基于多傳感數據的多移動機器人協同避障算法

周 婷

(安徽三聯學院，安徽 合肥 230071)

移動機器人具備導航定位、路徑規劃、感知信息等功能，涉及生活、監控、科學、救災、軍事、工業等領域，可以進行危險系數較高的工作，降低工作人員的重復性工作，解放勞動力，探測人們無法到達的區域[1]。多移動機器人是一個復雜系統，在復雜環境下，若賦予機器人的任務較為繁重，單個移動機器人無法完成任務，則需要多個移動機器人組成一個系統，通過協作計算和信息共享，突破單個機器人技術局限，獲得更合理的工作方式，完成繁復任務[2-3]。協同避障是多移動機器人系統的熱點問題，因此，研究多移動機器人協同避障算法，使各個機器人跟隨期望路徑，通力合作完成任務，具有重要意義[4]。

現階段，多移動機器人協同避障算法相關研究已取得較大進展，葉文濤等人[5]提出基于S型曲線的機器人避障算法，對移動機器人進行運動學分析，分析機器人碰撞障礙物的情況，仿真模擬不同的運動情形和運動方式，規劃機器人的S型曲線路徑，但該算法導航路徑存在沖擊振蕩，機器人避障成功率較低。常新新等人[6]提出基于改進動態窗口法的機器人避障算法，通過激光雷達等傳感器，采集機器人前進路徑的障礙物方位信息，計算機器人前進路徑的方位角范圍，利用動態窗口，選取局部路徑的最優方位角，導航機器人避障軌跡，但該算法避障路徑運行時間較長。針對以上問題，結合現有的研究理論，提出基于多傳感數據的多移動機器人協同避障算法。

1 多移動機器人協同避障算法

1.1 融合多移動機器人的多傳感數據

通過多傳感器信息預處理和坐標統一，融合多移動機器人的多傳感數據。在移動機器人上安裝傳感器，包括激光雷達和里程計等，測量周圍環境信息，設置各個傳感器的初始采樣時間和采樣頻率，保持采樣時刻和頻率相同，得到多傳感器的同步量測數據[7]。建立傳感器和機器人坐標系的姿態變換矩陣A如式(1)所示。

(1)

式(1)中姿態變換矩陣A為正交矩陣，a、b、c分別為傳感器相對機器人的側傾角度、俯仰角度、偏轉角度。當移動機器人執行任務時，多個機器人移動到工作空間，標定機器人位姿，統計機器人不同位姿時末端執行器的空間位置，設執行器在傳感器和機器人坐標系下的姿態角分別為B1、B2，兩個坐標系的轉換關系如式(2)所示。

B1=AB2

(2)

通過以上操作，完成多傳感信息的時間配準和空間配準。當多傳感器的量測數據轉換至機器人坐標系后，融合配準數據，設m個傳感器的量測數據分別為X1,X2,…,Xm，引入m個加權因子，X1,X2,…,Xm加權融合后的值X如式(3)所示。

(3)

式(3)中Ci、Xi分別為第i個傳感器的權值和量測數據，i∈(1,m)[8]。求取最優加權因子，歷史量測數據均值Xi(n)如式(4)所示。

(4)

式(4)中n為歷史數據個數，xc為歷史序列的第c個量測數據，c∈(1,n)[9]。將多傳感器看作一個復雜系統，測量對象的真實值為D，把X1,X2,…,Xm視為真值的無偏估計，真實值D包含測量數據和高斯白噪聲，傳感器系統總方差σ2如式(5)所示。

(5)

(6)

(7)

1.2 建立移動機器人運動模型

建立移動機器人運動模型，獲得機器人運動控制量。忽略噪聲和滑動對機器人的影響，機器人機體速度E如式(8)所示。

E=dcosF+esinF

(8)

其中F為機器人坐標系下的位姿方位角[10]。假定機器人運動不發生側移，僅在機體軸方向進行轉動和直線，把機體速度E看作線速度，設機器人運動位置為(d,e)，d和e分別為橫向和豎向的軸心線方向，機器人坐標系下的速度如式(9)所示。

(9)

(10)

將線速度E和角速度f作為機器人受到的控制量。設機器人質量為H，轉動慣量為I，驅動輪半徑為K，驅動輪之間的距離為k，機器人向心力矩陣h如式(11)所示。

(11)

機器人約束矩陣L如式(12)所示。

(12)

機器人輸入矩陣J如式(13)所示。

(13)

根據牛頓力學原理，可得機器人輸入力矩J應滿足以下條件，如式(14)所示。

O+h+j=JM-LTN

(14)

式(14)中O為對稱且正定的矩陣，j為機器人重力分量，N表示機器人受到約束，M為機器人的控制量，包括線速度E和角速度f[12]。改變輸入力矩J，調節機器人速度和方向，通過連續控制，使機器人到達任意空間位置。至此完成移動機器人運動模型的建立。

1.3 規劃多移動機器人協同避障軌跡

通過融合的傳感器數據，掌握移動機器人所在的空間信息，采用蟻群算法，規劃多個機器人協同避障路徑，分別控制各個機器人運動控制量，為單機器人導航最優全局路徑。由傳感器數據可知機器人工作空間的障礙物信息，利用柵格法，將工作環境抽象為一個柵格地圖，將最小化路徑長度，作為機器人運動目標，目標函數P如式(15)所示。

(15)

式(15)中Ut、Ut-1為第t步、第t-1步機器人的橫坐標值，Vt、Vt-1為第t步、第t-1步的縱坐標值[13]。標識柵格地圖的障礙物區域，統計障礙柵格，設障礙柵格位置坐標為(u,v)，避障約束條件如式(16)所示。

當機器人到達目標位置后，終止運動，停止路徑搜索，設置終止運動約束條件如式(17)所示。

max(|Ut-1-Ut-2|,|Vt-1-Vt-2|)≥max(|Ut-Ut-1|,|Vt-Vt-1|)

(17)

式(17)中Ut-2、Vt-2分別為t-2步機器人的橫、縱坐標。當max(|Ut-1-Ut-2|,|Vt-1-Vt-2|)取值為1時，max(|Ut-Ut-1|,|Vt-Vt-1|)取0或1；max(|Ut-1-Ut-2|,|Vt-1-Vt-2|)取值為0時，max(|Ut-Ut-1|,|Vt-Vt-1|)取0。機器人t-1時刻到達目標點后，通過終止運動約束條件，使機器人不再繼續運動。將多個機器人視為標準蟻群，單個機器人視為一只螞蟻，機器人選擇下一步行走節點時，根據公式(16)和(17)兩個約束條件，統計下一步所有可選節點，可選節點應滿足避障和終止運動約束，提取機器人起點和終點構成的向量，以及當前節點和下一步可選節點構成的向量，引入最優路徑指引函數Q如式(18)所示。

Q=pcos(o1-o2)-P

(18)

式(18)中p為常數，o1、o2為兩個向量與橫軸的夾角[14]。迭代更新機器人下一步可選節點，計算每個可選節點對應的信息素強度Q，選取信息素強度最大的柵格maxQ，將該柵格作為機器人下一步最優節點[15]。搜索所有機器人的最優可選節點，得到局部最優路徑，下達輸入力矩的控制指令，直至多機器人到達終點，獲得全局最優避障路徑。至此完成多移動機器人協同避障軌跡的規劃，實現多移動機器人協同避障算法設計。

2 實驗分析

將此次設計方法，與基于S型曲線的機器人避障算法、基于改進動態窗口法的機器人避障算法，進行對比實驗，比較多機器人避障成功率和避障路徑運行時間。

2.1 多移動機器人實驗平臺

以加工車間為例，生成多移動機器人運行空間的地形圖，環境整體面積為10m×10m，空間內部有貨架和桌椅等障礙物，整個空間覆蓋無線網絡，如圖1所示。

(a)復雜環境移動空間

(b)簡單環境移動空間圖1 多移動機器人工作環境柵格地圖

選擇多臺移動機器人，機器人型號為BNRT-MOB-44，利用無線路由器，連接機器人無線終端和局域網，將機器人作為下位機，通過Socket通信，在主控PC和機器人之間傳輸數據，由上位機運行多機器人協同避障算法，將控制信息發送給機器人，機器人接收PC端命令后，執行相應的動作，各個機器人到達指定目標位置。多個移動機器人編隊隊形和周邊環境如圖2所示。

圖2 多移動機器人編隊隊形和周邊環境

機器人安裝的傳感器包括激光雷達和里程計，為保證機器人運動狀態穩定性，共進行50次實驗。

2.2 實驗結果分析

設置移動機器人避障安全距離為0.3m，保持距離為0.4m，改變機器人行駛速度，若單機器人距離障礙物小于0.3m，或未到達目標點，視為避障失敗，否則為避障成功，統計成功次數，計算成功次數和總次數的比值，三種算法成功率測試結果如圖3所示。

(a)復雜環境避障成功率

(b)簡單環境避障成功率圖3 多移動機器人避障成功率實驗對比結果

由圖3(a)和圖3(b)可知，避障成功率隨著機器人移動速度的增加而減小，針對復雜環境和簡單環境，設計算法多機器人協同避障成功率，明顯高于另外兩種算法，機器人可以穿過障礙物到達目標位置，滿足多機器人協同控制要求。

在第一組實驗的基礎上，讓多機器人按照三種算法規劃好的路徑運行，統計多機器人運行時間，實驗對比結果如圖4所示。

(a)復雜環境運行時間

(b)簡單環境運行時間圖4 避障路徑運行時間實驗對比結果

由圖4(a)和圖4(b)可知，機器人行駛速度增加時，規劃路徑運行時間隨之減少，在兩種移動環境中，設計算法規劃的避障路徑運行時間最短，有效保證了機器人執行效率。

3 結論

此次研究設計了一種多移動機器人協同避障算法，融合多個傳感器量測數據，獲得周圍信息，提高了機器人避障成功率，縮短了避障路徑運行時間。但此次設計方法仍存在一定不足，忽略了道路不平坦對機器人造成的干擾，在今后的研究中，會將實際情況的干擾因素考慮在內，實時切換多移動機器人的編隊隊形，解決機器人編隊的運動沖突，進一步提高機器人軌跡跟蹤精度。