本發(fā)明涉及多機(jī)協(xié)作機(jī)器人領(lǐng)域，尤其涉及一種多機(jī)器人防碰撞方法及系統(tǒng)。

背景技術(shù)：

隨著機(jī)器人行業(yè)的崛起，越來越多的行業(yè)已經(jīng)引入了機(jī)器人，單個(gè)機(jī)器人的自動(dòng)化改造是用生產(chǎn)線代替生產(chǎn)線，機(jī)器人作為整個(gè)生產(chǎn)線中的組成部分，很難單獨(dú)拿出來，如果某個(gè)環(huán)節(jié)的機(jī)器人壞了，在沒有設(shè)計(jì)備份的情況下，整個(gè)產(chǎn)線可能要停工在一些領(lǐng)域中存在效率低下的問題，這時(shí)機(jī)器人運(yùn)動(dòng)且不能發(fā)生碰撞是一個(gè)很重要的問題。在自主移動(dòng)多機(jī)器人領(lǐng)域中，為了達(dá)到較大的工作效率，單個(gè)機(jī)器人移動(dòng)速度都比較大，碰撞可能導(dǎo)致機(jī)器人損壞或造成其他人身財(cái)產(chǎn)損失。對于以相當(dāng)大的速度移動(dòng)的現(xiàn)代工業(yè)機(jī)器人，碰撞通常既對機(jī)器人又對它們所處理的工件引起嚴(yán)重的損壞，這將產(chǎn)生較大的損失而導(dǎo)致生產(chǎn)停工，與自動(dòng)操作機(jī)器人進(jìn)行交互的人員的安全也被給予最高的優(yōu)先級(jí)。在具有移動(dòng)功能的現(xiàn)代工業(yè)機(jī)器人或其它機(jī)器中，移動(dòng)代表對于人員和其它對象的危險(xiǎn)，因此必須防止在移動(dòng)過程中，機(jī)器人與對象之間發(fā)生碰撞，為此，如何在發(fā)生碰撞之前控制機(jī)器人，使機(jī)器人停止移動(dòng)或改變路徑來防止碰撞的預(yù)測防碰撞方法是十分重要的。

在現(xiàn)有技術(shù)中，目前已有的防碰撞方法主要是在機(jī)器人的基座上安裝測距儀器(如超聲測距傳感器、紅外測距傳感器)，根據(jù)測距儀器測量碰撞距離并改變機(jī)器人的軌跡實(shí)現(xiàn)在短距離內(nèi)避免碰撞，但是在現(xiàn)有技術(shù)存在以下四個(gè)問題：

1、針對機(jī)器人與障礙物和機(jī)器人與機(jī)器人預(yù)測碰撞的判斷方法相同，產(chǎn)生計(jì)算響應(yīng)延遲，增加了計(jì)算時(shí)間；

2、機(jī)器人在復(fù)雜環(huán)境中的減速區(qū)間小，易堆積在一個(gè)地方；

3、不能夠有效地防止機(jī)器人發(fā)生碰撞。

因而，如何解決現(xiàn)有技術(shù)中的問題，是目前本領(lǐng)域技術(shù)人員亟待解決的技術(shù)問題。

技術(shù)實(shí)現(xiàn)要素：

本發(fā)明的目的是提供一種多機(jī)器人防碰撞方法及系統(tǒng)，實(shí)現(xiàn)解決現(xiàn)有防碰撞方法中不能很好預(yù)測碰撞，提高機(jī)器人在復(fù)雜環(huán)境中的活動(dòng)能力，增大機(jī)器人的活動(dòng)范圍的問題。

為了達(dá)到上述目的，本發(fā)明提供的技術(shù)方案如下：

一種多機(jī)器人防碰撞方法，包括步驟：

機(jī)器人獲取運(yùn)動(dòng)速度和運(yùn)動(dòng)軌跡，得到在預(yù)設(shè)時(shí)間內(nèi)進(jìn)行仿真移動(dòng)后的位置，創(chuàng)建環(huán)境地圖；依次得到每個(gè)機(jī)器人幾何外形頂點(diǎn)的坐標(biāo)；依次判斷每個(gè)機(jī)器人是否與障礙物和/或其他機(jī)器人發(fā)生碰撞，當(dāng)是時(shí)標(biāo)記；否則，不標(biāo)記；采集所有碰撞標(biāo)記信息、機(jī)器人位置相關(guān)信息，更新環(huán)境地圖，并調(diào)整所述機(jī)器人的移動(dòng)速度和移動(dòng)軌跡。

本發(fā)明能夠?qū)崿F(xiàn)多機(jī)器人在同一工作區(qū)域內(nèi)預(yù)測碰撞并防止碰撞的目的，通過仿真機(jī)器人一段時(shí)間的運(yùn)動(dòng)，并計(jì)算每個(gè)機(jī)器人的軌跡，以及在此過程中檢測機(jī)器人是否會(huì)與機(jī)器人或障礙物發(fā)生碰撞，對于預(yù)測發(fā)生碰撞的機(jī)器人標(biāo)記碰撞信息，將碰撞時(shí)間、地點(diǎn)、相關(guān)物體信息輸出預(yù)測結(jié)果，從而調(diào)整移動(dòng)速度和移動(dòng)方向，并調(diào)整即將發(fā)生碰撞的機(jī)器人的移動(dòng)速度和移動(dòng)方向，本發(fā)明對每個(gè)機(jī)器人的運(yùn)行做一段時(shí)間的預(yù)測，避免即將發(fā)生碰撞的時(shí)候才被機(jī)器人檢測到，可以做到提前預(yù)判，可以有效地防止機(jī)器人卡死或者搖擺不定的情況發(fā)生，從而避免發(fā)生碰撞，提高機(jī)器人在復(fù)雜環(huán)境中的活動(dòng)能力，增大機(jī)器人的活動(dòng)范圍。

進(jìn)一步，所述多機(jī)器人防碰撞方法包括：

獲取所有機(jī)器人幾何外形頂點(diǎn)在所述環(huán)境地圖上的坐標(biāo)信息；判斷當(dāng)前機(jī)器人是否有碰撞標(biāo)記信息；當(dāng)是時(shí)，切換下一機(jī)器人；否則判斷所述碰撞標(biāo)記信息是否為機(jī)器人；當(dāng)碰撞標(biāo)記信息是機(jī)器人時(shí)，預(yù)設(shè)每個(gè)機(jī)器人的中心點(diǎn)，按照標(biāo)簽先后順序，依次判斷所述當(dāng)前機(jī)器人與任意一個(gè)所述下一機(jī)器人中心點(diǎn)之間的距離是否小于等于碰撞最小距離；當(dāng)否時(shí)，輸出不碰撞到其他機(jī)器人；當(dāng)是時(shí)，判斷所述當(dāng)前機(jī)器人幾何外形的任意一線段是否與所述下一機(jī)器人幾何外形的任意一線段相交；當(dāng)是時(shí)，輸出碰撞到其他機(jī)器人，標(biāo)記所述碰撞機(jī)器人；否則輸出不碰撞到其他機(jī)器人；當(dāng)碰撞標(biāo)記信息不是機(jī)器人時(shí)，根據(jù)機(jī)器人幾何外形頂點(diǎn)進(jìn)行適應(yīng)地圖分辨率的插值運(yùn)算，按照標(biāo)簽先后順序，依次判斷所述當(dāng)前機(jī)器人幾何外形上任意一頂點(diǎn)是否在所述移動(dòng)環(huán)境地圖上的障礙物標(biāo)識(shí)區(qū)域內(nèi)；當(dāng)是時(shí)，輸出碰撞到障礙物，標(biāo)記所述碰撞機(jī)器人；否則，輸出不碰撞到障礙物。

本發(fā)明通過判斷當(dāng)前機(jī)器人是否有碰撞信息，能夠有效地減少判斷時(shí)間，只需要得知若有碰撞信息，則切換至下一個(gè)機(jī)器人進(jìn)行判斷，若沒有碰撞信息，再進(jìn)行下一步驟，大大地減少了判斷時(shí)間。本發(fā)明針對機(jī)器人與障礙物和機(jī)器人與機(jī)器人預(yù)測碰撞做不同的判斷方法，可以極大地減少計(jì)算時(shí)間，保證計(jì)算實(shí)時(shí)性。

進(jìn)一步，所述多機(jī)器人防碰撞方法包括：所述碰撞最小距離為兩個(gè)機(jī)器人之間的安全距離的和值，所述機(jī)器人的安全距離為所述機(jī)器人的中心點(diǎn)到所述機(jī)器人幾何外形頂點(diǎn)的最遠(yuǎn)距離。

進(jìn)一步，所述多機(jī)器人防碰撞方法應(yīng)用下列公式(1)進(jìn)行運(yùn)算：

C＝A+k₁·U＝B+k₂·V (3)

其中，C表示原點(diǎn)到碰撞點(diǎn)的向量，A和B分別表示原點(diǎn)到兩條線段起點(diǎn)的向量、U和V分別表示兩條線段起點(diǎn)到終點(diǎn)的向量、k₁表示C-A與U的長度比例、k₂表示C-B與V的長度比例。

進(jìn)一步，所述多機(jī)器人防碰撞方法應(yīng)用下列公式(2)進(jìn)行運(yùn)算：

k₂·(U×V)＝(B-A)×U (2)

如果U×V＝0并且(B-A)×U＝0，說明兩條直線共線；

如果U×V＝0并且(B-A)×U≠0，說明兩條直線平行；

如果U×V≠0、0≤k₁≤1且0≤k₂≤1，說明兩條線段相交；

其他情況，說明兩條線段不平行也不相交；

其中，A和B分別表示原點(diǎn)到兩條線段起點(diǎn)的向量、U和V分別表示兩條線段起點(diǎn)到終點(diǎn)的向量、k₁表示C-A與U的長度比例，k₂表示C-B與V的長度比例。

進(jìn)一步，所述多機(jī)器人防碰撞方法包括：

獲取所有機(jī)器人的位置相關(guān)信息和障礙物的位置相關(guān)信息；所述機(jī)器人進(jìn)行垂直投影得到所述機(jī)器人的幾何外形，并得到所述機(jī)器人幾何外形頂點(diǎn)的坐標(biāo)；根據(jù)所述位置相關(guān)信息和機(jī)器人的幾何外形，設(shè)置不同灰度值來標(biāo)定位置區(qū)域，第一灰度值表示寬敞區(qū)域，第二灰度值表示障礙物區(qū)域，第三灰度值表示機(jī)器人垂直投影幾何外形區(qū)域，第四灰度值表示未知區(qū)域；根據(jù)所述位置相關(guān)信息和所述灰度值的映射關(guān)系創(chuàng)建環(huán)境地圖。

本發(fā)明判斷機(jī)器人是否發(fā)生碰撞只需要判斷在地圖中該機(jī)器人幾何外形下是否只存在當(dāng)前機(jī)器人信息和地圖空閑信息，如果該機(jī)器人幾何外形下存在其他信息(如障礙物信息、其他機(jī)器人幾何外形信息)則說明機(jī)器人發(fā)生碰撞。

進(jìn)一步，所述多機(jī)器人防碰撞方法包括：

獲取所有機(jī)器人的初始信息，包括機(jī)器人位置相關(guān)信息和環(huán)境信息；得到每個(gè)機(jī)器人的運(yùn)動(dòng)速度，構(gòu)建每個(gè)機(jī)器人的運(yùn)動(dòng)軌跡。

進(jìn)一步，所述多機(jī)器人防碰撞方法包括：

采集所述碰撞標(biāo)記信息、機(jī)器人位置相關(guān)信息；儲(chǔ)存所述采集的信息，更新環(huán)境地圖；規(guī)劃所述機(jī)器人的防碰撞路徑，并廣播發(fā)送控制指令給各個(gè)機(jī)器人；判斷所述機(jī)器人是否在所述預(yù)設(shè)響應(yīng)時(shí)間內(nèi)接收到所述控制指令；若是則所述機(jī)器人根據(jù)所述控制指令調(diào)整所述移動(dòng)速度和移動(dòng)軌跡；否則，標(biāo)記所述機(jī)器人發(fā)生故障，獲取所述發(fā)生故障的機(jī)器人的位置相關(guān)信息，并返回重新開始。

進(jìn)一步，所述多機(jī)器人防碰撞方法包括：所述運(yùn)動(dòng)軌跡包括運(yùn)動(dòng)方向、運(yùn)動(dòng)角度和運(yùn)動(dòng)路徑；所述位置相關(guān)信息包括所述位置、速度、方位、形狀和高度。

本發(fā)明還提供一種多機(jī)器人防碰撞系統(tǒng)，包括：

碰撞預(yù)測模塊、主控模塊；

所述碰撞預(yù)測模塊與所述主控模塊通訊連接，獲取所述主控模塊發(fā)送的運(yùn)動(dòng)速度和運(yùn)動(dòng)軌跡；所述機(jī)器人在預(yù)設(shè)時(shí)間內(nèi)根據(jù)所述運(yùn)動(dòng)速度和運(yùn)動(dòng)軌跡仿真移動(dòng)；獲取仿真移動(dòng)后的位置，創(chuàng)建環(huán)境地圖；依次得到每個(gè)機(jī)器人幾何外形頂點(diǎn)的坐標(biāo)；依次判斷每個(gè)機(jī)器人是否與障礙物和/或其他機(jī)器人發(fā)生碰撞，當(dāng)是時(shí)標(biāo)記；否則，不標(biāo)記；

所述主控模塊采集所有碰撞標(biāo)記信息、機(jī)器人位置相關(guān)信息；根據(jù)所述采集的信息更新環(huán)境地圖，并發(fā)送移動(dòng)速度控制指令和移動(dòng)軌跡控制指令至所述機(jī)器人。

本發(fā)明能夠?qū)崿F(xiàn)多機(jī)器人在同一工作區(qū)域內(nèi)預(yù)測碰撞并防止碰撞的目的，通過仿真機(jī)器人一段時(shí)間的運(yùn)動(dòng)，并計(jì)算每個(gè)機(jī)器人的軌跡，以及在此過程中檢測機(jī)器人是否會(huì)與機(jī)器人或障礙物發(fā)生碰撞，對于預(yù)測發(fā)生碰撞的機(jī)器人標(biāo)記碰撞信息，將碰撞時(shí)間、地點(diǎn)、相關(guān)物體信息輸出預(yù)測結(jié)果，從而調(diào)整移動(dòng)速度和移動(dòng)方向，并調(diào)整即將發(fā)生碰撞的機(jī)器人的移動(dòng)速度和移動(dòng)方向，可以有效地防止機(jī)器人卡死或者搖擺不定的情況發(fā)生，從而避免發(fā)生碰撞，提高機(jī)器人在復(fù)雜環(huán)境中的活動(dòng)能力，增大機(jī)器人的活動(dòng)范圍。

進(jìn)一步，所述多機(jī)器人防碰撞系統(tǒng)的所述碰撞預(yù)測模塊還包括：

獲取創(chuàng)建子模塊和判斷標(biāo)記子模塊；

所述獲取創(chuàng)建子模塊與所述判斷標(biāo)記子模塊通訊連接，獲取所述在預(yù)設(shè)時(shí)間內(nèi)進(jìn)行仿真移動(dòng)后的位置，創(chuàng)建環(huán)境地圖，依次得到每個(gè)機(jī)器人幾何外形頂點(diǎn)的坐標(biāo)；所述獲取創(chuàng)建子模塊還獲取所有機(jī)器人的位置相關(guān)信息和障礙物的位置相關(guān)信息，進(jìn)行垂直投影得到所述機(jī)器人的幾何外形，并得到所述機(jī)器人幾何外形頂點(diǎn)的坐標(biāo)，根據(jù)所述位置相關(guān)信息和機(jī)器人的幾何外形，設(shè)置不同灰度值來標(biāo)定位置區(qū)域，第一灰度值表示寬敞區(qū)域，第二灰度值表示障礙物區(qū)域，第三灰度值表示機(jī)器人垂直投影幾何外形區(qū)域，第四灰度值表示未知區(qū)域；根據(jù)所述位置相關(guān)信息和所述灰度值的映射關(guān)系創(chuàng)建環(huán)境地圖，并將所述位置相關(guān)信息和所述環(huán)境地圖發(fā)送至所述判斷標(biāo)記子模塊；

所述判斷標(biāo)記子模塊與所述主控模塊通訊連接，根據(jù)所述機(jī)器人幾何外形頂點(diǎn)在所述環(huán)境地圖上的坐標(biāo)信息，依次判斷每個(gè)機(jī)器人是否有碰撞標(biāo)記信息，當(dāng)是時(shí)切換下一機(jī)器人進(jìn)行判斷；否則判斷所述碰撞標(biāo)記信息是否為機(jī)器人；當(dāng)碰撞標(biāo)記信息是機(jī)器人時(shí)，預(yù)設(shè)每個(gè)機(jī)器人的中心點(diǎn)，按照標(biāo)簽先后順序，依次判斷所述當(dāng)前機(jī)器人與任意一個(gè)所述下一機(jī)器人中心點(diǎn)之間的距離是否小于等于碰撞最小距離；當(dāng)中心點(diǎn)之間的距離大于碰撞最小距離時(shí)，輸出不碰撞到其他機(jī)器人；當(dāng)中心點(diǎn)之間的距離小于等于碰撞最小距離時(shí)，判斷所述當(dāng)前機(jī)器人幾何外形的任意一線段是否與所述下一機(jī)器人幾何外形的任意一線段相交；當(dāng)是時(shí)，輸出碰撞到其他機(jī)器人，標(biāo)記所述碰撞機(jī)器人；否則，輸出不碰撞到其他機(jī)器人；當(dāng)碰撞標(biāo)記信息不是機(jī)器人時(shí)，按照標(biāo)簽先后順序，依次判斷所述當(dāng)前機(jī)器人幾何外形上任意一頂點(diǎn)是否在所述移動(dòng)環(huán)境地圖上的障礙物標(biāo)識(shí)區(qū)域內(nèi)；當(dāng)是時(shí)，輸出碰撞到障礙物，標(biāo)記所述碰撞機(jī)器人；否則，輸出不碰撞到障礙物；

其中，所述運(yùn)動(dòng)軌跡包括運(yùn)動(dòng)方向、運(yùn)動(dòng)角度和運(yùn)動(dòng)路徑，所述位置相關(guān)信息包括所述位置、速度、方位、形狀和高度，所述碰撞最小距離為兩個(gè)機(jī)器人之間的安全距離的和值，所述機(jī)器人的安全距離為所述機(jī)器人的中心點(diǎn)到所述機(jī)器人幾何外形頂點(diǎn)的最遠(yuǎn)距離。

本發(fā)明通過判斷當(dāng)前機(jī)器人是否有碰撞信息，能夠有效地減少判斷時(shí)間，只需要得知若有碰撞信息，則切換至下一個(gè)機(jī)器人進(jìn)行判斷，若沒有碰撞信息，再進(jìn)行下一步驟，大大地減少了判斷時(shí)間。本發(fā)明針對機(jī)器人與障礙物和機(jī)器人與機(jī)器人預(yù)測碰撞做不同的判斷方法，可以極大地減少計(jì)算時(shí)間，保證計(jì)算實(shí)時(shí)性。

進(jìn)一步，所述多機(jī)器人防碰撞系統(tǒng)包括，

所述判斷標(biāo)記子模塊根據(jù)下列公式(1)進(jìn)行運(yùn)算：

C＝A+k₁·U＝B+k₂·V (4)

其中，C表示原點(diǎn)到碰撞點(diǎn)的向量，A和B分別表示原點(diǎn)到兩條線段起點(diǎn)的向量、U和V分別表示兩條線段起點(diǎn)到終點(diǎn)的向量、k₁表示C-A與U的長度比例、k₂表示C-B與V的長度比例。

進(jìn)一步，所述多機(jī)器人防碰撞系統(tǒng)包括，

所述判斷標(biāo)記子模塊根據(jù)下列公式(2)進(jìn)行判斷：

k₂·(U×V)＝(B-A)×U (2)

如果U×V＝0并且(B-A)×U＝0，說明兩條直線共線；

如果U×V＝0并且(B-A)×U≠0，說明兩條直線平行；

如果U×V≠0、0≤k₁≤1且0≤k₂≤1，說明兩條線段相交；

其他情況，說明兩條線段不平行也不相交；

其中，A和B分別表示原點(diǎn)到兩條線段起點(diǎn)的向量、U和V分別表示兩條線段起點(diǎn)到終點(diǎn)的向量、k₁表示C-A與U的長度比例，k₂表示C-B與V的長度比例。

進(jìn)一步，所述多機(jī)器人防碰撞系統(tǒng)還包括：所述主控模塊還獲取所有機(jī)器人的初始信息，包括機(jī)器人位置相關(guān)信息和環(huán)境信息，得到每個(gè)機(jī)器人的所述運(yùn)動(dòng)速度，構(gòu)建每個(gè)機(jī)器人的所述運(yùn)動(dòng)軌跡，并發(fā)送所述運(yùn)動(dòng)速度和運(yùn)動(dòng)軌跡至所述碰撞預(yù)測模塊，所述主控模塊儲(chǔ)存所述采集的碰撞標(biāo)記信息和機(jī)器人位置相關(guān)信息，根據(jù)所述儲(chǔ)存的信息更新環(huán)境地圖，規(guī)劃所述機(jī)器人的防碰撞路徑，并廣播發(fā)送控制指令給各個(gè)機(jī)器人；判斷所述機(jī)器人是否在所述預(yù)設(shè)響應(yīng)時(shí)間內(nèi)接收到所述控制指令；若是則根據(jù)所述控制指令調(diào)整所述移動(dòng)速度和移動(dòng)軌跡；否則，標(biāo)記所述機(jī)器人發(fā)生故障，獲取所述發(fā)生故障的機(jī)器人的位置相關(guān)信息，并發(fā)送至所述碰撞預(yù)測模塊。

本發(fā)明中對于預(yù)測發(fā)生碰撞的機(jī)器人標(biāo)記碰撞信息，將碰撞時(shí)間、地點(diǎn)、相關(guān)物體信息輸出預(yù)測結(jié)果供主控模塊做調(diào)整。能極大地避免機(jī)器人與障礙物或其他機(jī)器人發(fā)生碰撞，提高機(jī)器人在復(fù)雜環(huán)境中的活動(dòng)能力，增大機(jī)器人的活動(dòng)范圍。

與現(xiàn)有技術(shù)相比，本發(fā)明提供一種多機(jī)器人防碰撞方法及系統(tǒng)，至少帶來以下一種技術(shù)效果：

1、本發(fā)明能極大地避免機(jī)器人與障礙物或其他機(jī)器人發(fā)生碰撞，可以有效地防止機(jī)器人卡死或者搖擺不定的情況發(fā)生，提高機(jī)器人在復(fù)雜環(huán)境中的活動(dòng)能力，增大機(jī)器人的活動(dòng)范圍。

2、本發(fā)明盡量保證大多數(shù)機(jī)器人能正常運(yùn)行。

3、本發(fā)明中對于預(yù)測發(fā)生碰撞的機(jī)器人標(biāo)記碰撞信息，將碰撞時(shí)間、地點(diǎn)、相關(guān)物體信息輸出預(yù)測結(jié)果供主控模塊做調(diào)整。

4、本發(fā)明針對機(jī)器人與障礙物和機(jī)器人與機(jī)器人預(yù)測碰撞做不同的判斷方法，可以極大地減少計(jì)算時(shí)間，保證計(jì)算實(shí)時(shí)性。

5、本發(fā)明對每個(gè)機(jī)器人的運(yùn)行做一段時(shí)間的預(yù)測，避免即將發(fā)生碰撞的時(shí)候才被機(jī)器人檢測到，可以做到提前預(yù)判。

附圖說明

下面將以明確易懂的方式，結(jié)合附圖說明優(yōu)選實(shí)施方式，對一種多機(jī)器人防碰撞方法及系統(tǒng)特性、技術(shù)特征、優(yōu)點(diǎn)及其實(shí)現(xiàn)方式予以進(jìn)一步說明。

圖1是本發(fā)明一種多機(jī)器人防碰撞方法一個(gè)實(shí)施例的流程圖；

圖2是本發(fā)明一種多機(jī)器人防碰撞方法另一個(gè)實(shí)施例的流程圖；

圖3是本發(fā)明一種多機(jī)器人防碰撞方法又一實(shí)施例的流程圖；

圖4是本發(fā)明一種多機(jī)器人防碰撞系統(tǒng)一個(gè)實(shí)施例的結(jié)構(gòu)示意圖；

圖5是本發(fā)明一種多機(jī)器人防碰撞系統(tǒng)另一個(gè)實(shí)施例的結(jié)構(gòu)示意圖；

圖6是本發(fā)明一種多機(jī)器人防碰撞系統(tǒng)再一實(shí)施例的環(huán)境地圖的示意圖；

圖7是本發(fā)明機(jī)器人與機(jī)器人碰撞的示意圖；

圖8是本發(fā)明機(jī)器人與障礙物碰撞的示意圖；

圖9是本發(fā)明機(jī)器人與機(jī)器人之間發(fā)生碰撞的碰撞最小距離示意圖；

圖10是本發(fā)明機(jī)器人與機(jī)器人之間發(fā)生碰撞的判斷線段相交的示意圖。

附圖標(biāo)號(hào)說明：

1000機(jī)器人；

1001萬向輪；

1002基座；

1003滾輪；

1100碰撞預(yù)測模塊；

1110獲取創(chuàng)建子模塊；

1120判斷標(biāo)記子模塊；

1200主控模塊；

2000障礙物；

1寬敞區(qū)域；

2障礙物區(qū)域；

3機(jī)器人垂直投影幾何外形區(qū)域；

4未知區(qū)域。

具體實(shí)施方式

為了更清楚地說明本發(fā)明實(shí)施例或現(xiàn)有技術(shù)中的技術(shù)方案，下面將對照附圖說明本發(fā)明的具體實(shí)施方式。顯而易見地，下面描述中的附圖僅僅是本發(fā)明的一些實(shí)施例，對于本領(lǐng)域普通技術(shù)人員來講，在不付出創(chuàng)造性勞動(dòng)的前提下，還可以根據(jù)這些附圖獲得其他的附圖，并獲得其他的實(shí)施方式。

為使圖面簡潔，各圖中只示意性地表示出了與本發(fā)明相關(guān)的部分，它們并不代表其作為產(chǎn)品的實(shí)際結(jié)構(gòu)。另外，以使圖面簡潔便于理解，在有些圖中具有相同結(jié)構(gòu)或功能的部件，僅示意性地繪示了其中的一個(gè)，或僅標(biāo)出了其中的一個(gè)。在本文中，“一個(gè)”不僅表示“僅此一個(gè)”，也可以表示“多于一個(gè)”的情形。

本發(fā)明提供一種多機(jī)器人防碰撞方法的一個(gè)實(shí)施例，參照圖1，包括：

S300機(jī)器人獲取運(yùn)動(dòng)速度和運(yùn)動(dòng)軌跡，得到在預(yù)設(shè)時(shí)間內(nèi)進(jìn)行仿真移動(dòng)后的位置，創(chuàng)建環(huán)境地圖；S400依次得到每個(gè)機(jī)器人幾何外形頂點(diǎn)的坐標(biāo)；S500依次判斷每個(gè)機(jī)器人是否與障礙物和/或其他機(jī)器人發(fā)生碰撞，當(dāng)是時(shí)標(biāo)記；否則，不標(biāo)記；S600采集所有碰撞標(biāo)記信息、機(jī)器人位置相關(guān)信息，更新環(huán)境地圖，并調(diào)整所述機(jī)器人的移動(dòng)速度和移動(dòng)軌跡。

本發(fā)明實(shí)施例中，機(jī)器人1000在預(yù)設(shè)時(shí)間內(nèi)進(jìn)行仿真移動(dòng)，本發(fā)明通過仿真機(jī)器人1000一段時(shí)間的運(yùn)動(dòng)，并計(jì)算每個(gè)機(jī)器人1000的軌跡，判斷機(jī)器人1000是否與障礙物2000和/或其他機(jī)器人1000發(fā)生碰撞，是否發(fā)生碰撞只需要判斷在環(huán)境地圖中該機(jī)器人1000幾何外形下是否只存在當(dāng)前機(jī)器人1000信息和地圖空閑信息，如果該機(jī)器人1000幾何外形下存在其他信息(如障礙物2000信息、其他機(jī)器人1000幾何外形信息)則說明機(jī)器人1000與障礙物2000和/或其他機(jī)器人1000發(fā)生碰撞，對于預(yù)測發(fā)生碰撞的機(jī)器人1000標(biāo)記碰撞信息，將碰撞時(shí)間、地點(diǎn)、相關(guān)物體信息輸出預(yù)測結(jié)果，從而調(diào)整移動(dòng)速度和移動(dòng)軌跡，避免即將發(fā)生碰撞的時(shí)候才被機(jī)器人1000檢測到，可以做到提前預(yù)判，能極大地避免機(jī)器人1000與障礙物2000或其他機(jī)器人1000發(fā)生碰撞，可以有效地防止機(jī)器人1000卡死或者搖擺不定的情況發(fā)生，提高機(jī)器人1000在復(fù)雜環(huán)境中的活動(dòng)能力，增大機(jī)器人1000的活動(dòng)范圍。

本發(fā)明提供一種多機(jī)器人防碰撞方法的另一個(gè)實(shí)施例，參照圖2。相對于第一個(gè)實(shí)施例，相同的步驟在此不再贅述，步驟S500進(jìn)一步包括：S510獲取所有機(jī)器人幾何外形頂點(diǎn)在所述環(huán)境地圖上的坐標(biāo)信息；S520判斷當(dāng)前機(jī)器人是否有碰撞標(biāo)記信息；當(dāng)是時(shí)，執(zhí)行步驟S590；否則，執(zhí)行步驟S530；S530判斷所述碰撞標(biāo)記信息是否為機(jī)器人；當(dāng)是時(shí)，執(zhí)行步驟S560；否則，執(zhí)行步驟S540；S540根據(jù)機(jī)器人幾何外形頂點(diǎn)進(jìn)行適應(yīng)地圖分辨率的插值運(yùn)算；S550按照標(biāo)簽先后順序，依次判斷所述當(dāng)前機(jī)器人幾何外形上任意一頂點(diǎn)是否在所述移動(dòng)環(huán)境地圖上的障礙物標(biāo)識(shí)區(qū)域內(nèi)；當(dāng)是時(shí)，執(zhí)行步驟S551；否則，執(zhí)行步驟S552；S551輸出碰撞到障礙物，標(biāo)記所述碰撞機(jī)器人；S552輸出不碰撞到障礙物；S560預(yù)設(shè)每個(gè)機(jī)器人的中心點(diǎn)；S570按照標(biāo)簽先后順序，依次判斷所述當(dāng)前機(jī)器人與任意一個(gè)所述下一機(jī)器人中心點(diǎn)之間的距離是否小于等于碰撞最小距離；當(dāng)是時(shí)，執(zhí)行步驟S580；否則，執(zhí)行步驟S582；S580判斷所述當(dāng)前機(jī)器人幾何外形的任意一線段是否與所述下一機(jī)器人幾何外形的任意一線段相交；當(dāng)是時(shí)，執(zhí)行步驟S581；否則，執(zhí)行步驟S582；S581輸出碰撞到其他機(jī)器人，標(biāo)記所述碰撞機(jī)器人；S582輸出不碰撞到其他機(jī)器人；S590切換下一機(jī)器人，繼續(xù)執(zhí)行步驟S520。

本發(fā)明實(shí)施例中，上述步驟效果不再贅述，步驟S510到S590中，步驟S510通過判斷當(dāng)前機(jī)器人1000是否有碰撞信息，能夠有效地減少判斷時(shí)間，只需要得知若有碰撞信息，則切換至下一個(gè)機(jī)器人1000進(jìn)行判斷，若沒有碰撞信息，再進(jìn)行下一步驟，大大地減少了判斷時(shí)間；步驟S520到步驟590通過判斷碰撞信息是否為機(jī)器人1000，當(dāng)碰撞標(biāo)記信息為機(jī)器人1000時(shí)，預(yù)設(shè)每個(gè)機(jī)器人1000的中心點(diǎn)；如圖7，是機(jī)器人1000與機(jī)器人1000發(fā)生碰撞的示意圖，若所述機(jī)器人1000為輪式機(jī)器人1000，以萬向輪1001所在的位置作為輪式機(jī)器人1000的頭部，則所述輪式機(jī)器人1000的車體1002和車輪1003的垂直投影幾何外形區(qū)域與其余的輪式機(jī)器人1000的車體1002和車輪1003的垂直投影幾何外形區(qū)域在所述環(huán)境地圖上相交則表示所述兩個(gè)輪式機(jī)器人1000之間發(fā)生碰撞；如圖9是機(jī)器人1000與機(jī)器人1000之間發(fā)生碰撞的碰撞最小距離示意圖，依次判斷所述當(dāng)前機(jī)器人1000與任意一個(gè)所述下一機(jī)器人1000中心點(diǎn)之間的距離是否小于等于碰撞最小距離；如果當(dāng)前機(jī)器人1000與任意一個(gè)所述下一機(jī)器人1000中心點(diǎn)之間的距離大于碰撞最小距離，輸出當(dāng)前機(jī)器人1000不碰撞到任一其他機(jī)器人1000；如果當(dāng)前機(jī)器人1000與任意一個(gè)所述下一機(jī)器人1000中心點(diǎn)之間的距離小于等于碰撞最小距離，如圖10是機(jī)器人1000與機(jī)器人1000之間發(fā)生碰撞的判斷線段相交的示意圖，再繼續(xù)判斷所述當(dāng)前機(jī)器人1000幾何外形的任意一線段是否與所述下一機(jī)器人1000幾何外形的任意一線段相交；如果所述當(dāng)前機(jī)器人1000幾何外形的任意一線段與所述下一機(jī)器人1000幾何外形的任意一線段相交時(shí)，輸出當(dāng)前機(jī)器人1000碰撞到至少一個(gè)機(jī)器人1000的碰撞信息，標(biāo)記所述相互之間發(fā)生碰撞的兩個(gè)機(jī)器人1000；否則，如果所述當(dāng)前機(jī)器人1000幾何外形的任意一線段與所述下一機(jī)器人1000幾何外形的任意一線段不相交時(shí)，輸出當(dāng)前機(jī)器人1000不碰撞到任何機(jī)器人1000，切換下一機(jī)器人1000，繼續(xù)判斷當(dāng)前機(jī)器人1000是否有碰撞標(biāo)記信息。

其中所述碰撞最小距離為兩個(gè)機(jī)器人1000之間的安全距離的和值，所述機(jī)器人1000的安全距離為所述機(jī)器人1000的中心點(diǎn)到所述機(jī)器人1000幾何外形頂點(diǎn)的最遠(yuǎn)距離。

所述步驟S580應(yīng)用下列公式(1)進(jìn)行運(yùn)算：

C＝A+k₁·U＝B+k₂·V (5)

其中，C表示原點(diǎn)到碰撞點(diǎn)的向量，A和B分別表示原點(diǎn)到兩條線段起點(diǎn)的向量、U和V分別表示兩條線段起點(diǎn)到終點(diǎn)的向量、k₁表示C-A與U的長度比例、k₂表示C-B與V的長度比例。

對于式(1)左右兩邊分別叉乘向量V，即

(A+k₁·U)×V＝(B+k₂·V)×V (6)

又因?yàn)閂×V＝0，可以求出k₁：

k₁·(U×V)＝(B-A)×V (7)

同理對于式(4)左右兩邊分別叉乘向量U，即因?yàn)閁×U＝0，可以求出k₂：

(A+k₁·U)×U＝(B+k₂·V)×U (8)

k₂·(V×U)＝(A-B)×U (9)

其中U×V＝-V×U，則能夠得到式(6)

k₂·(U×V)＝(B-A)×U (10)

如果U×V＝0并且(B-A)×U＝0，說明兩條直線共線；

如果U×V＝0并且(B-A)×U≠0，說明兩條直線平行；

如果U×V≠0、0≤k₁≤1且0≤k₂≤1，說明兩條線段相交；

其他情況，說明兩條線段不平行也不相交。

其中，A和B分別表示原點(diǎn)到兩條線段起點(diǎn)的向量、U和V分別表示兩條線段起點(diǎn)到終點(diǎn)的向量、k₁表示C-A與U的長度比例，k₂表示C-B與V的長度比例。

本發(fā)明是為了解決多機(jī)器人1000在同一工作區(qū)域內(nèi)預(yù)測碰撞的方法，該方法通過仿真機(jī)器人1000一段時(shí)間的運(yùn)動(dòng)，并計(jì)算每個(gè)機(jī)器人1000的軌跡，以及在此過程中檢測機(jī)器人1000是否會(huì)與障礙物2000和機(jī)器人1000發(fā)生碰撞，將得到的預(yù)測結(jié)果輸出。該方法可以有效地指出哪個(gè)機(jī)器人1000在什么時(shí)候、什么位置、與什么物體發(fā)生碰撞，可以有效地防止機(jī)器人1000卡死或者搖擺不定的情況發(fā)生，提高機(jī)器人1000在復(fù)雜環(huán)境中的活動(dòng)能力，增大機(jī)器人1000的活動(dòng)范圍。每次對機(jī)器人1000預(yù)測很小的時(shí)間的動(dòng)作，因此機(jī)器人1000移動(dòng)的距離很短，判斷機(jī)器人1000是否發(fā)生碰撞只需要判斷在地圖中該機(jī)器人1000幾何外形下是否只存在當(dāng)前機(jī)器人1000信息和地圖空閑信息，如果該機(jī)器人1000幾何外形下存在其他信息(如障礙物2000信息、其他機(jī)器人1000幾何外形信息)則說明機(jī)器人1000發(fā)生碰撞。首先判斷兩個(gè)機(jī)器人1000的距離是否滿足兩個(gè)機(jī)器人1000碰撞的最小距離，若不滿足這個(gè)最小距離則說明兩個(gè)機(jī)器人1000還足夠遠(yuǎn)以至于不會(huì)發(fā)生碰撞。最小距離為兩個(gè)機(jī)器人1000的安全距離之和，機(jī)器人1000的安全距離為機(jī)器人1000中心點(diǎn)到機(jī)器人1000幾何外形的最遠(yuǎn)距離，考慮到機(jī)器人1000的幾何外形可以看作一個(gè)多邊形，機(jī)器人1000的安全距離為機(jī)器人1000中心點(diǎn)到機(jī)器人1000幾何外形頂點(diǎn)的最遠(yuǎn)距離。其次機(jī)器人1000的幾何外形可以看作一個(gè)多邊形，機(jī)器人1000的碰撞與機(jī)器人1000的碰撞可以看作多邊形與多邊形的重疊，進(jìn)一步可以看作是線段與線段的相交，即如果機(jī)器人1000的幾何外形的一條線段與另一個(gè)機(jī)器人1000的幾何外形的一條線段相交，說明兩個(gè)機(jī)器人1000發(fā)生碰撞。

本發(fā)明插值運(yùn)算的方法有Lagrange插值(拉格朗日插值)、Newton插值(牛頓插值)、Hermite插值(艾米插值)、分段線性插值、樣條插值等插值方法。通過判斷當(dāng)前機(jī)器人1000是否有碰撞信息，能夠有效地減少判斷時(shí)間，只需要得知若有碰撞信息，則切換至下一個(gè)機(jī)器人1000進(jìn)行判斷，若沒有碰撞信息，再進(jìn)行下一步驟，大大地減少了判斷時(shí)間。本發(fā)明針對機(jī)器人1000與障礙物2000和機(jī)器人1000與機(jī)器人1000預(yù)測碰撞做不同的判斷方法，可以極大地減少計(jì)算時(shí)間，保證計(jì)算實(shí)時(shí)性。

本發(fā)明提供一種多機(jī)器人防碰撞方法的另一個(gè)實(shí)施例，參照圖3。相對于第一個(gè)實(shí)施例，相同的步驟在此不再贅述，進(jìn)一步包括：

S100獲取所有機(jī)器人的初始信息，包括機(jī)器人位置相關(guān)信息和環(huán)境信息；

S200得到每個(gè)機(jī)器人的所述運(yùn)動(dòng)速度，構(gòu)建每個(gè)機(jī)器人的所述運(yùn)動(dòng)軌跡；

S300機(jī)器人獲取運(yùn)動(dòng)速度和運(yùn)動(dòng)軌跡，得到在預(yù)設(shè)時(shí)間內(nèi)進(jìn)行仿真移動(dòng)后的位置，創(chuàng)建環(huán)境地圖；

S410獲取所有機(jī)器人的位置相關(guān)信息和障礙物的位置相關(guān)信息；

S420所述機(jī)器人進(jìn)行垂直投影得到所述機(jī)器人的幾何外形，并得到所述機(jī)器人幾何外形頂點(diǎn)的坐標(biāo)；

S430根據(jù)所述位置相關(guān)信息和機(jī)器人的幾何外形，設(shè)置不同灰度值來標(biāo)定位置區(qū)域，第一灰度值表示寬敞區(qū)域，第二灰度值表示障礙物區(qū)域，第三灰度值表示機(jī)器人垂直投影幾何外形區(qū)域，第四灰度值表示未知區(qū)域；

S440根據(jù)所述位置相關(guān)信息和所述灰度值的映射關(guān)系創(chuàng)建環(huán)境地圖；

S500依次判斷每個(gè)機(jī)器人是否與障礙物和/或其他機(jī)器人發(fā)生碰撞，當(dāng)是時(shí)標(biāo)記；否則，不標(biāo)記；

S610采集所述碰撞標(biāo)記信息、機(jī)器人位置相關(guān)信息；

S620儲(chǔ)存所述采集的信息，更新環(huán)境地圖；

S630規(guī)劃所述機(jī)器人的防碰撞路徑，并廣播發(fā)送控制指令給各個(gè)機(jī)器人；

S640判斷所述機(jī)器人是否在所述預(yù)設(shè)響應(yīng)時(shí)間內(nèi)接收到所述控制指令；若是則執(zhí)行步驟S650；否則，執(zhí)行步驟S660；

S650所述機(jī)器人根據(jù)所述控制指令調(diào)整所述移動(dòng)速度和移動(dòng)軌跡；

S660標(biāo)記所述機(jī)器人發(fā)生故障，獲取所述發(fā)生故障的機(jī)器人的位置相關(guān)信息，并返回所述步驟S100。

本發(fā)明實(shí)施例中，上述步驟效果不再贅述，步驟S100到S200中，獲取所有機(jī)器人1000的初始信息，包括機(jī)器人1000位置相關(guān)信息和環(huán)境信息；根據(jù)所述位置相關(guān)信息和環(huán)境信息可以計(jì)算得到每個(gè)機(jī)器人1000的在短時(shí)間內(nèi)進(jìn)行仿真移動(dòng)的運(yùn)動(dòng)速度和運(yùn)動(dòng)軌跡，如圖7本發(fā)明通過不同的灰度值來標(biāo)定該位置區(qū)域的內(nèi)所包含的信息，第一灰度值表示寬敞區(qū)域1，第二灰度值表示障礙物2000區(qū)域2，第三灰度值表示機(jī)器人垂直投影幾何外形區(qū)域3，第四灰度值表示未知區(qū)域4，根據(jù)所述位置相關(guān)信息和所述灰度值的映射關(guān)系創(chuàng)建環(huán)境地圖；本發(fā)明判斷機(jī)器人1000是否發(fā)生碰撞只需要判斷在地圖中該機(jī)器人1000幾何外形下是否只存在當(dāng)前機(jī)器人1000信息和地圖空閑信息，如果該機(jī)器人1000幾何外形下存在其他信息(如障礙物2000信息、其他機(jī)器人1000幾何外形信息)則說明機(jī)器人1000發(fā)生碰撞。根據(jù)所述環(huán)境地圖和判斷方法依次判斷每個(gè)機(jī)器人1000是否發(fā)生碰撞，當(dāng)發(fā)生碰撞時(shí)進(jìn)行標(biāo)記；否則，不進(jìn)行標(biāo)記；標(biāo)記結(jié)束后，采集所述碰撞標(biāo)記信息、機(jī)器人1000位置相關(guān)信息，將所述采集的信息進(jìn)行儲(chǔ)存，當(dāng)根據(jù)標(biāo)簽順序依次儲(chǔ)存所有機(jī)器人1000的位置相關(guān)信息和碰撞標(biāo)記信息后，更新環(huán)境地圖，根據(jù)更新后的環(huán)境地圖，規(guī)劃所述機(jī)器人1000的防碰撞路徑，并廣播發(fā)送控制指令給各個(gè)機(jī)器人1000；判斷所述機(jī)器人1000是否在所述預(yù)設(shè)響應(yīng)時(shí)間內(nèi)接收到所述控制指令；如果在預(yù)設(shè)時(shí)間內(nèi)所述機(jī)器人1000接收到控制指令，則機(jī)器人1000根據(jù)所述控制指令調(diào)整移動(dòng)速度和移動(dòng)軌跡，如果在預(yù)設(shè)時(shí)間內(nèi)所述機(jī)器人1000沒有接收到控制指令，則標(biāo)記所述機(jī)器人1000發(fā)生故障，獲取所述發(fā)生故障的機(jī)器人1000的位置相關(guān)信息，則所述發(fā)生故障的機(jī)器人1000成為環(huán)境障礙物2000，變成環(huán)境信息的一部分，因此需要重新獲取所有機(jī)器人1000的初始信息，包括機(jī)器人1000位置相關(guān)信息和環(huán)境信息，這樣能夠避免機(jī)器人1000因?yàn)檠舆t導(dǎo)致在預(yù)設(shè)時(shí)間內(nèi)所述機(jī)器人1000沒有接收到控制指令，或者機(jī)器人1000因?yàn)榘l(fā)生突發(fā)性的故障導(dǎo)致在預(yù)設(shè)時(shí)間內(nèi)所述機(jī)器人1000沒有接收到控制指令，從而能極大地避免機(jī)器人1000與障礙物2000或其他機(jī)器人1000發(fā)生碰撞，可以有效地防止機(jī)器人1000卡死或者搖擺不定的情況發(fā)生，提高機(jī)器人1000在復(fù)雜環(huán)境中的活動(dòng)能力，增大機(jī)器人1000的活動(dòng)范圍。

參照圖4，本發(fā)明一種多機(jī)器人防碰撞系統(tǒng)的一個(gè)實(shí)施例，包括：機(jī)器人1000，所述機(jī)器人1000包括碰撞預(yù)測模塊1200、主控模塊1100；

所述碰撞預(yù)測模塊與所述主控模塊通訊連接，獲取所述主控模塊發(fā)送的運(yùn)動(dòng)速度和運(yùn)動(dòng)軌跡；所述機(jī)器人在預(yù)設(shè)時(shí)間內(nèi)根據(jù)所述運(yùn)動(dòng)速度和運(yùn)動(dòng)軌跡仿真移動(dòng)；獲取仿真移動(dòng)后的位置，創(chuàng)建環(huán)境地圖；依次得到每個(gè)機(jī)器人幾何外形頂點(diǎn)的坐標(biāo)；依次判斷每個(gè)機(jī)器人是否與障礙物和/或其他機(jī)器人發(fā)生碰撞，當(dāng)是時(shí)標(biāo)記；否則，不標(biāo)記；

所述主控模塊與所述碰撞預(yù)測模塊通訊連接，采集所有碰撞標(biāo)記信息、機(jī)器人位置相關(guān)信息；根據(jù)所述采集的信息更新環(huán)境地圖，并發(fā)送移動(dòng)速度控制指令和移動(dòng)軌跡控制指令至所述機(jī)器人。

本發(fā)明實(shí)施例中，所述機(jī)器人1000獲取所述主控模塊1200發(fā)送的運(yùn)動(dòng)速度和運(yùn)動(dòng)軌跡，根據(jù)所述獲取的運(yùn)動(dòng)速度和運(yùn)動(dòng)軌跡，在預(yù)設(shè)時(shí)間內(nèi)移動(dòng)；通過仿真機(jī)器人1000一段時(shí)間的運(yùn)動(dòng)，所述主控模塊1200獲取所述在預(yù)設(shè)時(shí)間內(nèi)進(jìn)行仿真移動(dòng)后的位置，創(chuàng)建環(huán)境地圖；依次得到每個(gè)機(jī)器人1000幾何外形頂點(diǎn)的坐標(biāo)；碰撞預(yù)測模塊1100依次判斷每個(gè)機(jī)器人1000是否發(fā)生碰撞，當(dāng)是時(shí)標(biāo)記碰撞；否則，不標(biāo)記碰撞；采集所有碰撞標(biāo)記信息、機(jī)器人1000位置相關(guān)信息；根據(jù)所述采集的信息更新環(huán)境地圖，并發(fā)送移動(dòng)速度控制指令和移動(dòng)軌跡控制指令至所述機(jī)器人1000；并計(jì)算每個(gè)機(jī)器人1000的軌跡，以及在此過程中檢測機(jī)器人1000是否會(huì)與機(jī)器人1000或障礙物2000發(fā)生碰撞，將得到的預(yù)測結(jié)果輸出采集作參考，對于預(yù)測發(fā)生碰撞的機(jī)器人1000標(biāo)記碰撞信息，將碰撞時(shí)間、地點(diǎn)、相關(guān)物體信息輸出預(yù)測結(jié)果，從而調(diào)整移動(dòng)速度和移動(dòng)軌跡，避免即將發(fā)生碰撞的時(shí)候才被機(jī)器人1000檢測到，可以做到提前預(yù)判，能極大地避免機(jī)器人1000與障礙物2000或其他機(jī)器人1000發(fā)生碰撞，并調(diào)整即將發(fā)生碰撞的機(jī)器人1000的移動(dòng)速度和移動(dòng)軌跡，可以有效地防止機(jī)器人1000卡死或者搖擺不定的情況發(fā)生，從而避免發(fā)生碰撞，提高機(jī)器人1000在復(fù)雜環(huán)境中的活動(dòng)能力，增大機(jī)器人1000的活動(dòng)范圍。

本發(fā)明提供一種多機(jī)器人防碰撞系統(tǒng)的另一個(gè)實(shí)施例，參照圖5。相對于上一個(gè)實(shí)施例，相同的部分在此不再贅述，所述碰撞預(yù)測模塊1100還包括：獲取創(chuàng)建子模塊1110和判斷標(biāo)記子模塊1120；

所述獲取創(chuàng)建子模塊1110與所述主控模塊1200通訊連接，獲取所述在預(yù)設(shè)時(shí)間內(nèi)進(jìn)行仿真移動(dòng)后的位置，創(chuàng)建環(huán)境地圖，依次得到每個(gè)機(jī)器人1000幾何外形頂點(diǎn)的坐標(biāo)；所述獲取創(chuàng)建子模塊1110還獲取所有機(jī)器人的位置相關(guān)信息和障礙物2000的位置相關(guān)信息，進(jìn)行垂直投影得到所述機(jī)器人1000的幾何外形，并得到所述機(jī)器人1000幾何外形頂點(diǎn)的坐標(biāo)，根據(jù)所述位置相關(guān)信息和機(jī)器人1000的幾何外形，設(shè)置不同灰度值來標(biāo)定位置區(qū)域，第一灰度值表示寬敞區(qū)域1，第二灰度值表示障礙物區(qū)域2，第三灰度值表示機(jī)器人垂直投影幾何外形區(qū)域3，第四灰度值表示未知區(qū)域4；根據(jù)所述位置相關(guān)信息和所述灰度值的映射關(guān)系創(chuàng)建環(huán)境地圖，并將所述位置相關(guān)信息和所述環(huán)境地圖發(fā)送至所述判斷標(biāo)記子模塊1120；

所述判斷標(biāo)記子模塊1120與所述判斷標(biāo)記子模塊1120通訊連接，根據(jù)所述機(jī)器人幾何外形頂點(diǎn)在所述環(huán)境地圖上的坐標(biāo)信息，依次判斷每個(gè)機(jī)器人1000是否有碰撞標(biāo)記信息，當(dāng)是時(shí)切換下一機(jī)器人1000進(jìn)行判斷；否則判斷所述碰撞標(biāo)記信息是否為機(jī)器人1000；當(dāng)碰撞標(biāo)記信息是機(jī)器人1000時(shí)，預(yù)設(shè)每個(gè)機(jī)器人1000的中心點(diǎn)，按照標(biāo)簽先后順序，依次判斷所述當(dāng)前機(jī)器人1000與任意一個(gè)所述下一機(jī)器人1000中心點(diǎn)之間的距離是否小于等于碰撞最小距離；當(dāng)中心點(diǎn)之間的距離大于碰撞最小距離時(shí)，輸出不碰撞到其他機(jī)器人1000；當(dāng)中心點(diǎn)之間的距離小于等于碰撞最小距離時(shí)，判斷所述當(dāng)前機(jī)器人1000幾何外形的任意一線段是否與所述下一機(jī)器人1000幾何外形的任意一線段相交；當(dāng)是時(shí)，輸出碰撞到其他機(jī)器人1000，標(biāo)記所述碰撞機(jī)器人1000；否則，輸出不碰撞到其他機(jī)器人1000；當(dāng)碰撞標(biāo)記信息不是機(jī)器人1000時(shí)，按照標(biāo)簽先后順序，依次判斷所述當(dāng)前機(jī)器人1000幾何外形上任意一頂點(diǎn)是否在所述移動(dòng)環(huán)境地圖上的障礙物標(biāo)識(shí)區(qū)域2內(nèi)；當(dāng)是時(shí)，輸出碰撞到障礙物2000，標(biāo)記所述碰撞機(jī)器人1000；否則，輸出不碰撞到障礙物2000；

其中，所述運(yùn)動(dòng)軌跡包括運(yùn)動(dòng)方向、運(yùn)動(dòng)角度和運(yùn)動(dòng)路徑，所述位置相關(guān)信息包括所述位置、速度、方位、形狀和高度，所述碰撞最小距離為兩個(gè)機(jī)器人1000之間的安全距離的和值，所述機(jī)器人1000的安全距離為所述機(jī)器人1000的中心點(diǎn)到所述機(jī)器人1000)幾何外形頂點(diǎn)的最遠(yuǎn)距離。

所述判斷標(biāo)記子模塊1120根據(jù)下列公式(1)進(jìn)行運(yùn)算：

C＝A+k₁·U＝B+k₂·V (11)

其中，C表示原點(diǎn)到碰撞點(diǎn)的向量，A和B分別表示原點(diǎn)到兩條線段起點(diǎn)的向量、U和V分別表示兩條線段起點(diǎn)到終點(diǎn)的向量、k₁表示C-A與U的長度比例、k₂表示C-B與V的長度比例。

所述判斷標(biāo)記子模塊1120對于式(1)左右兩邊分別叉乘向量V進(jìn)行判斷，即

(A+k₁·U)×V＝(B+k₂·V)×V (12)

又因?yàn)閂×V＝0，可以求出k₁：

k₁·(U×V)＝(B-A)×V (13)

同理對于式(4)左右兩邊分別叉乘向量U，即因?yàn)閁×U＝0，可以求出k₂：

(A+k₁·U)×U＝(B+k₂·V)×U (14)

k₂·(V×U)＝(A-B)×U (15)

其中U×V＝-V×U，則能夠得到式(6)

k₂·(U×V)＝(B-A)×U (16)

如果U×V＝0并且(B-A)×U＝0，說明兩條直線共線；

如果U×V＝0并且(B-A)×U≠0，說明兩條直線平行；

如果U×V≠0、0≤k₁≤1且0≤k₂≤1，說明兩條線段相交；

其他情況，說明兩條線段不平行也不相交。

其中，A和B分別表示原點(diǎn)到兩條線段起點(diǎn)的向量、U和V分別表示兩條線段起點(diǎn)到終點(diǎn)的向量、k₁表示C-A與U的長度比例，k₂表示C-B與V的長度比例。

本發(fā)明實(shí)施例中，上述相同模塊的效果不再贅述，本發(fā)明能夠?qū)崿F(xiàn)多機(jī)器人1000在同一工作區(qū)域內(nèi)預(yù)測碰撞并防止碰撞的目的，本發(fā)明通過仿真機(jī)器人1000一段時(shí)間的運(yùn)動(dòng)，并計(jì)算每個(gè)機(jī)器人1000的軌跡，以及在此過程中檢測機(jī)器人1000是否會(huì)與其余的機(jī)器人1000發(fā)生碰撞，通過判斷當(dāng)前機(jī)器人1000是否有碰撞信息，能夠有效地減少判斷時(shí)間，只需要得知若有碰撞信息，則切換至下一個(gè)機(jī)器人1000進(jìn)行判斷，若沒有碰撞信息，再進(jìn)行下一步驟，大大地減少了判斷時(shí)間；當(dāng)有碰撞信息時(shí)，判斷當(dāng)前機(jī)器人1000與其余機(jī)器人1000是否發(fā)生碰撞的方法與判斷當(dāng)前機(jī)器人1000與障礙物2000是否發(fā)生碰撞的方法是不同的，本發(fā)明針對機(jī)器人1000與障礙物2000和機(jī)器人1000與機(jī)器人1000預(yù)測碰撞做不同的判斷方法，可以極大地減少計(jì)算時(shí)間，保證計(jì)算實(shí)時(shí)性。根據(jù)所述環(huán)境地圖和判斷方法依次判斷每個(gè)機(jī)器人1000是否發(fā)生碰撞，當(dāng)發(fā)生碰撞時(shí)進(jìn)行標(biāo)記；否則，不進(jìn)行標(biāo)記；標(biāo)記結(jié)束后，采集所述碰撞標(biāo)記信息、機(jī)器人1000位置相關(guān)信息，將所述采集的信息進(jìn)行儲(chǔ)存，當(dāng)根據(jù)標(biāo)簽順序依次儲(chǔ)存所有機(jī)器人1000的位置相關(guān)信息和碰撞標(biāo)記信息后，更新環(huán)境地圖，根據(jù)更新后的環(huán)境地圖，規(guī)劃所述機(jī)器人1000的防碰撞路徑，并廣播發(fā)送控制指令給各個(gè)機(jī)器人1000；判斷所述機(jī)器人1000是否在所述預(yù)設(shè)響應(yīng)時(shí)間內(nèi)接收到所述控制指令；如果在預(yù)設(shè)時(shí)間內(nèi)所述機(jī)器人1000接收到控制指令，則機(jī)器人1000根據(jù)所述控制指令調(diào)整移動(dòng)速度和移動(dòng)軌跡，如果在預(yù)設(shè)時(shí)間內(nèi)所述機(jī)器人1000沒有接收到控制指令，則標(biāo)記所述機(jī)器人1000發(fā)生故障，獲取所述發(fā)生故障的機(jī)器人1000的位置相關(guān)信息，則所述發(fā)生故障的機(jī)器人1000成為環(huán)境障礙物2000，變成環(huán)境信息的一部分，因此需要重新獲取所有機(jī)器人1000的初始信息，包括機(jī)器人1000位置相關(guān)信息和環(huán)境信息，這樣能夠避免機(jī)器人1000因?yàn)檠舆t導(dǎo)致在預(yù)設(shè)時(shí)間內(nèi)所述機(jī)器人1000沒有接收到控制指令，或者機(jī)器人1000因?yàn)榘l(fā)生突發(fā)性的故障導(dǎo)致在預(yù)設(shè)時(shí)間內(nèi)所述機(jī)器人1000沒有接收到控制指令，從而能極大地避免機(jī)器人1000與障礙物2000或其他機(jī)器人1000發(fā)生碰撞，可以有效地防止機(jī)器人1000卡死或者搖擺不定的情況發(fā)生，提高機(jī)器人1000在復(fù)雜環(huán)境中的活動(dòng)能力，增大機(jī)器人1000的活動(dòng)范圍。

應(yīng)當(dāng)說明的是，上述實(shí)施例均可根據(jù)需要自由組合。以上所述僅是本發(fā)明的優(yōu)選實(shí)施方式，應(yīng)當(dāng)指出，對于本技術(shù)領(lǐng)域的普通技術(shù)人員來說，在不脫離本發(fā)明原理的前提下，還可以做出若干改進(jìn)和潤飾，這些改進(jìn)和潤飾也應(yīng)視為本發(fā)明的保護(hù)范圍。