本發(fā)明涉及機器人運動控制的，具體涉及用于非圓形清潔機器人的碰撞預測方法。

背景技術：

1、目前清潔場景中大多掃地機的機身造型都是圓形結構，圓形結構的運動規(guī)劃只需考慮機體前側障礙物的避障處理，對于清潔場景上存在的非圓形掃地機，比如，一種d型清潔機器人，作為前圓后方造型的掃地機不同于圓形掃地機，清潔運動時運動幅度較大，在后退和旋轉動作中容易碰撞上至少一個方位上的障礙物，導致清潔機器人不能有效地兼顧到多個方位的障礙物帶來的碰撞問題。

技術實現(xiàn)思路

1、本技術公開用于非圓形清潔機器人的碰撞預測方法，具體的技術方案如下：

2、用于非圓形清潔機器人的碰撞預測方法，其特征在于，所述碰撞預測方法包括：

3、步驟a、根據(jù)非圓形清潔機器人的左驅動輪速度與右驅動輪速度的和值、非圓形清潔機器人的左驅動輪速度與右驅動輪速度的差值絕對值、以及非圓形清潔機器人在第一時刻的位姿，計算非圓形清潔機器人在第二時刻的位姿；步驟b、在確定非圓形清潔機器人在第一時刻的位姿和其在第二時刻的位姿的基礎上，根據(jù)非圓形清潔機器人在預測位置處的對齊點與非圓形清潔機器人的一個方位上的障礙物位置點之間的平面投影距離在第一時刻和第二時刻之間的變化，預測非圓形清潔機器人與該方位上的障礙物相碰撞的情況；步驟c、將預先獲得的非圓形清潔機器人的新方位上的障礙物位置點更新為步驟b所述的非圓形清潔機器人的一個方位上的障礙物位置點，然后重復執(zhí)行步驟b，直至預先獲得的所有方位上的障礙物位置點都執(zhí)行過步驟b，以預測出非圓形清潔機器人與各個方位上的障礙物相碰撞的情況。

4、綜上，非圓形清潔機器人還根據(jù)左右驅動輪速度來預測非圓形清潔機器人的運動趨勢及其在相應時刻所能移動到的位置，在非圓形清潔機器人實際移動之前，當計算確定第二時刻離對應方位上的障礙物距離比第一時刻離對應方位上的障礙物距離更近時，確認非圓形清潔機器人即將撞到對應方位上的障礙物，完成非圓形清潔機器人與對應方位上的障礙物相碰撞的情況的預測，充分考慮機體結構及其運動過程中與對應方位處的障礙物位置點的位置關系，在相互孤立的多個方位的障礙物分布的環(huán)境下，通過重復執(zhí)行步驟b和步驟c來確定多個方位上的障礙物位置，并依次判斷非圓形清潔機器人從當前位置移動至目標導航位置的過程中是否存在與各個方位上的障礙物相碰撞的趨勢，兼顧到機體邊緣的多個方位與障礙物碰觸問題，也有效地兼顧到各個方位上的障礙物帶來的碰撞問題，實現(xiàn)非圓形清潔機器人可以執(zhí)行各個方位上的障礙物碰撞預測，避免非圓形清潔機器人直接碰撞上對應方位上的障礙物，或者在后退和旋轉動作中避開一個特定方位上的障礙物卻又碰撞上另一個特定方位上的障礙物，使得非圓形清潔機器人在多個方位分布的障礙物場景下避障動作簡單靈活。

5、進一步地，預先獲得的所有方位上的障礙物位置點是位于以非圓形清潔機器人的機體中心為圓心的一定半徑的目標圓域內(nèi)，非圓形清潔機器人的對齊點與其機體中心之間的最大平面投影距離小于所述一定半徑，所述一定半徑小于非圓形清潔機器人安裝的測距傳感器的最大掃描半徑，測距傳感器支持旋轉掃描以獲得360度方向上的各個障礙物；步驟c中，所述非圓形清潔機器人的新方位上的障礙物位置點是預先獲得的所有方位上的障礙物位置點當中，未執(zhí)行過步驟b的障礙物位置點。從而通過執(zhí)行步驟c來依次預測非圓形清潔機器人的各個方位上的障礙物位置點是否被移動過程中的非圓形清潔機器人碰撞上，兼顧到全方位的碰撞預測問題，為尚未移動的非圓形清潔機器人提供更加全面有效的障礙物分布信息和避障路徑規(guī)劃策略。

6、進一步地，所述根據(jù)非圓形清潔機器人在預測位置處的對齊點與非圓形清潔機器人的一個方位上的障礙物位置點之間的平面投影距離在相鄰兩個時刻之間的變化，預測非圓形清潔機器人移動過程中與該方位上的障礙物相碰撞的情況的方法包括：若非圓形清潔機器人在預測位置處的對齊點與非圓形清潔機器人的一個方位上障礙物位置點之間的平面投影距離，小于非圓形清潔機器人在當前位置處的對齊點與非圓形清潔機器人的同一方位上障礙物位置點之間的平面投影距離時，確定非圓形清潔機器人移動過程中存在與該方位上的障礙物相碰撞的趨勢；若非圓形清潔機器人在預測位置處的對齊點與非圓形清潔機器人的一個方位上障礙物位置點之間的平面投影距離，大于或等于非圓形清潔機器人在當前位置處的對齊點與非圓形清潔機器人的同一方位上障礙物位置點之間的平面投影距離時，確定非圓形清潔機器人移動過程中不存在與該方位上的障礙物相碰撞的趨勢。因此，本技術方案可以根據(jù)非圓形清潔機器人在預測位置處的對齊點與非圓形清潔機器人的一個方位上障礙物位置點之間的平面投影距離在相鄰兩個時刻之間的變化，預測非圓形清潔機器人移動過程中與該方位上障礙物相碰撞的情況，避免非圓形清潔機器人在導航移動過程中或因錯誤觸發(fā)避障操作過程中直接碰撞上障礙物。

7、進一步地，若非圓形清潔機器人的左驅動輪速度與非圓形清潔機器人的右驅動輪速度均不等于數(shù)值0，則非圓形清潔機器人將其從第一時刻開始，在第二時刻預期移動到的位置設置為預測位置；非圓形清潔機器人的當前位置是非圓形清潔機器人在第一時刻的機體中心所占據(jù)的二維坐標位置，預測位置是非圓形清潔機器人在第二時刻的機體中心所占據(jù)的二維坐標位置；第二時刻與第一時刻的時間間隔是預先設置好；其中，非圓形清潔機器人在當前位置的位姿包括非圓形清潔機器人的當前位置的坐標和非圓形清潔機器人在第一時刻的移動方向；其中，非圓形清潔機器人在預測位置處的位姿包括預測位置的坐標和非圓形清潔機器人在第二時刻的移動方向。本技術方案選擇在非圓形清潔機器人未開始從第一時刻移動之前，按照左驅動輪速度與右驅動輪速度所產(chǎn)生的預期運動軌跡或移動位置規(guī)劃出第二時刻所移動到的位置，即預測位置，可以等效于通過當前機體中心位置計算非圓形清潔機器人在第二時刻機體中心位置，便于判斷非圓形清潔機器人按照當前移動方向移動與障礙物的碰撞趨勢。

8、進一步地，所述根據(jù)非圓形清潔機器人的左驅動輪速度與右驅動輪速度的和值、非圓形清潔機器人的左驅動輪速度與右驅動輪速度的差值絕對值、以及非圓形清潔機器人在第一時刻的位姿，計算非圓形清潔機器人在第二時刻的位姿的方法包括：非圓形清潔機器人將其實時測得的左驅動輪速度設置為vl，非圓形清潔機器人將其實時測得的右驅動輪速度設置為vr；然后，非圓形清潔機器人將其移動速度vc設置為等于同時將非圓形清潔機器人移動產(chǎn)生的角速度ωc設置為等于其中，左驅動輪與右驅動輪通過輪軸連接，輪軸的長度是l；非圓形清潔機器人將當前位置的坐標設置為(x0,y0)，非圓形清潔機器人還將其在第一時刻的移動方向指示的角度設置為θ0；然后非圓形清潔機器人基于移動速度vc、角速度ωc以及移動規(guī)劃時間t，計算其在預測位置處的位姿，作為非圓形清潔機器人在第二時刻的位姿，其中，計算其在預測位置處的位姿的方法包括：

9、

10、

11、θ1＝θ0+ωct；

12、其中，移動規(guī)劃時間t是所述第二時刻與所述第一時刻之間的時間間隔；(x1,y1)是非圓形清潔機器人在預測位置處的坐標，θ1是非圓形清潔機器人在第二時刻的移動方向指示的角度。

13、從而根據(jù)非圓形清潔機器人的左驅動輪速度與右驅動輪速度、以及非圓形清潔機器人在第一時刻的位姿，在非圓形清潔機器人實際發(fā)生位移之前計算出非圓形清潔機器人在第二時刻的位姿，作為非圓形清潔機器人在預測位置的位姿。用于評估其與各個方位的障礙物的碰撞關系。

14、進一步地，所述非圓形清潔機器人包括機身、機頭和兩個對稱設置的輪子，兩個對稱設置的輪子通過輪軸連接，輪軸設置于所述機身和所述機頭之間的分界線處；機身和機頭組成的機體造型不是圓形，機頭的前方是指向非圓形清潔機器人的移動方向。從而在非圓形清潔機器人的移動方向是機器人坐標系的橫坐標軸正方向，非圓形清潔機器人的機體中軸線可以設置為機器人坐標系的橫坐標軸，非圓形清潔機器人的機體中心是機器人坐標系的原點的情況下，通過左驅動輪和右驅動輪來改變機器人坐標系的坐標軸朝向，以實時記錄下非圓形清潔機器人的移動方向信息。

15、進一步地，非圓形清潔機器人的機體中心與障礙物位置點的連線與非圓形清潔機器人的機體邊緣的交點標記為非圓形清潔機器人的對齊點，對齊點與障礙物位置點之間的平面投影距離是對齊點與障礙物位置點在非圓形清潔機器人的行走平面內(nèi)形成的直線距離。若非圓形清潔機器人的左驅動輪速度不等于非圓形清潔機器人的右驅動輪速度，則非圓形清潔機器人被配置為沿著預設時針方向按照角速度ωc轉動預設角度，對于同一障礙物位置點，非圓形清潔機器人的對齊點被配置為沿著預設時針方向的反方向轉動預設角度，以使：對于同一障礙物位置點，非圓形清潔機器人的位姿變化時，非圓形清潔機器人的對齊點變化。每當計算非圓形清潔機器人的對齊點與非圓形清潔機器人的一個方位上障礙物位置點之間的距離在相鄰兩個時刻之間會產(chǎn)生變化，可以用于判斷非圓形清潔機器人從第一時刻移動至第二時刻的過程中是否存在與相應方位上障礙物相碰撞的趨勢。

16、進一步地，預先獲取非圓形清潔機器人的一個方位上障礙物位置點的坐標，記為障礙物的位置坐標(xp,yp)；在非圓形清潔機器人確定當前位置的坐標為(x0,y0)，且確定其在當前位置處移動方向指示的角度為θ0后，基于相對坐標轉換公式將障礙物的位置坐標(xp,yp)換算為第一機器人坐標系下的待規(guī)避障礙物第一時刻坐標(relaxp0,relayp0)；然后利用待規(guī)避障礙物第一時刻坐標(relaxp0,relayp0)計算待規(guī)避障礙物第一時刻對齊角度relaθ0；其中，第一機器人坐標系的橫坐標軸正方向是非圓形清潔機器人在當前位置處移動方向，第一機器人坐標系的原點設置在當前位置處；在非圓形清潔機器人確定預測位置處的坐標為(x1,y1)，且確定其在預測位置處移動方向指示的角度為θ1后，基于相對坐標轉換公式將障礙物的位置坐標(xp,yp)換算為第二機器人坐標系下的待規(guī)避障礙物第二時刻坐標(relaxp1,relayp1)；然后利用待規(guī)避障礙物第二時刻坐標(relaxp1,relayp1)計算待規(guī)避障礙物第二時刻對齊角度relaθ1；其中，第二機器人坐標系的橫坐標軸正方向是非圓形清潔機器人在預測位置處移動方向，第二機器人坐標系的原點設置在預測位置處；其中，(xp,yp)、(x0,y0)以及(x1,y1)均是位于世界坐標系下。從而通過相對坐標轉換公式獲得待規(guī)避障礙物在第二時刻的位姿信息，用于對比非圓形清潔機器人的邊緣與障礙物的位置坐標(用于表示該障礙物位置的激光點)之間的距離在相鄰兩個時刻之間的變化，便于根據(jù)該距離的變化來判斷非圓形清潔機器人從第一時刻移動至第二時刻的過程中是否存在與障礙物相碰撞的趨勢。

17、進一步地，基于相對坐標轉換公式將障礙物的位置坐標(xp,yp)換算為待規(guī)避障礙物第一時刻坐標的方法包括：relaxp0＝(xp-x0)cosθ0+(yp-y0)sinθ0；relayp0＝(yp-y0)cosθ0+(xp-x0)sinθ0；

18、利用待規(guī)避障礙物第一時刻坐標(relaxp0,relayp0)計算待規(guī)避障礙物第一時刻對齊角度relaθ0是

19、

20、基于相對坐標轉換公式將障礙物的位置坐標(xp,yp)換算為待規(guī)避障礙物第二時刻坐標的方法包括：relaxp1＝(xp-x1)cosθ1+(yp-y1)sinθ1；relayp1＝(yp-y1)cosθ1+(xp-x1)sinθ1；

21、利用待規(guī)避障礙物第二時刻坐標(relaxp1,relayp1)計算出的待規(guī)避障礙物第二時刻對齊角度relaθ1是

22、

23、從而依據(jù)非圓形清潔機器人的機體中心的位置坐標和障礙物的位置坐標，利用前述相對坐標轉換公式進行旋轉變換和平移變換，實現(xiàn)將障礙物的位置坐標轉換到機器人坐標系下。

24、進一步地，非圓形清潔機器人在當前位置處的對齊點與非圓形清潔機器人在當前位置處的機體中心之間的平面投影距離與待規(guī)避障礙物第一時刻對齊角度存在預先設置的映射關系；其中，非圓形清潔機器人在當前位置處的對齊點與非圓形清潔機器人在當前位置處的機體中心之間的平面投影距離使用robot_rela_dist(relaθ0)表示；非圓形清潔機器人在預測位置處的對齊點與非圓形清潔機器人在預測位置處的機體中心之間的平面投影距離與待規(guī)避障礙物第二時刻對齊角度存在預先設置的映射關系；其中，非圓形清潔機器人在預測位置處的對齊點與非圓形清潔機器人在預測位置處的機體中心之間的平面投影距離使用robot_rela_dist(relaθ1)表示；預先設置的映射關系用于表示非圓形清潔機器人在一個對齊角度的指向上的對齊點與機體中心之間的平面投影距離與同一對齊角度之間的函數(shù)關系；對齊點與機體中心之間的平面投影距離是對齊點與機體中心在非圓形清潔機器人的行走平面內(nèi)形成的距離，用于表示機體中心沿著一個對齊角度指示的方向到機體邊緣的距離。從而實現(xiàn)使用映射關系枚舉出非圓形清潔機器人以機體中心為圓心的360個角度指向上，對齊點到機體中心的距離信息，便于非圓形清潔機器人移動過的不同時刻或不同位置處，計算非圓形清潔機器人的對齊點與用于表示一個障礙物位置的激光點之間的距離時通過預先設置的映射關系進行相應對齊角度上的距離信息調用。

25、進一步地，非圓形清潔機器人在當前位置處的對齊點與待規(guī)避障礙物第一時刻坐標之間的距離d0的計算方式為：非圓形清潔機器人在預測位置處的對齊點與待規(guī)避障礙物第二時刻坐標之間的距離d1的計算方式為：當d1<d0時，確定非圓形清潔機器人從當前位置開始，按照角速度ωc轉動的過程中，機體中心與一個方位上障礙物位置點之間的距離減小，則確定非圓形清潔機器人移動過程中存在與所述一個方位上的障礙物位置點處的障礙物相碰撞的趨勢；當d1>d0時，確定非圓形清潔機器人從當前位置開始，按照角速度ωc轉動的過程中，機體中心與一個方位上障礙物位置點之間的距離增大，則確定非圓形清潔機器人移動過程中不存在與所述一個方位上的障礙物位置點處的障礙物相碰撞的趨勢。

26、綜上，前述技術方案通過考慮機體結構和提前計算出的位姿狀態(tài)，實現(xiàn)有效的碰撞預測，能夠在確定非圓形清潔機器人在當前位置處的對齊點與待規(guī)避障礙物第一時刻坐標之間的距離的情況下，未發(fā)生移動前就計算出非圓形清潔機器人在預測位置處的對齊點與待規(guī)避障礙物第二時刻坐標之間的距離并判斷非圓形清潔機器人的對齊點與用于表示一個障礙物位置的激光點之間的距離在相鄰兩個時刻之間是變小或變大，提高碰撞預測的準確性；從而在相對于對應方位的障礙物預留出所述非圓形清潔機器人在當前位置處的對齊點與待規(guī)避障礙物第一時刻坐標之間的距離(用于防止機器人碰觸障礙物的安全間距)的情況下，確定觸發(fā)機器人避障的準確時機，避免非圓形清潔機器人按照原來的驅動輪速度直接碰撞上障礙物。