基于多傳感數(shù)據(jù)的多移動(dòng)機(jī)器人協(xié)同避障算法

周 婷

(安徽三聯(lián)學(xué)院，安徽 合肥 230071)

移動(dòng)機(jī)器人具備導(dǎo)航定位、路徑規(guī)劃、感知信息等功能，涉及生活、監(jiān)控、科學(xué)、救災(zāi)、軍事、工業(yè)等領(lǐng)域，可以進(jìn)行危險(xiǎn)系數(shù)較高的工作，降低工作人員的重復(fù)性工作，解放勞動(dòng)力，探測(cè)人們無法到達(dá)的區(qū)域[1]。多移動(dòng)機(jī)器人是一個(gè)復(fù)雜系統(tǒng)，在復(fù)雜環(huán)境下，若賦予機(jī)器人的任務(wù)較為繁重，單個(gè)移動(dòng)機(jī)器人無法完成任務(wù)，則需要多個(gè)移動(dòng)機(jī)器人組成一個(gè)系統(tǒng)，通過協(xié)作計(jì)算和信息共享，突破單個(gè)機(jī)器人技術(shù)局限，獲得更合理的工作方式，完成繁復(fù)任務(wù)[2-3]。協(xié)同避障是多移動(dòng)機(jī)器人系統(tǒng)的熱點(diǎn)問題，因此，研究多移動(dòng)機(jī)器人協(xié)同避障算法，使各個(gè)機(jī)器人跟隨期望路徑，通力合作完成任務(wù)，具有重要意義[4]。

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

1 多移動(dòng)機(jī)器人協(xié)同避障算法

1.1 融合多移動(dòng)機(jī)器人的多傳感數(shù)據(jù)

通過多傳感器信息預(yù)處理和坐標(biāo)統(tǒng)一，融合多移動(dòng)機(jī)器人的多傳感數(shù)據(jù)。在移動(dòng)機(jī)器人上安裝傳感器，包括激光雷達(dá)和里程計(jì)等，測(cè)量周圍環(huán)境信息，設(shè)置各個(gè)傳感器的初始采樣時(shí)間和采樣頻率，保持采樣時(shí)刻和頻率相同，得到多傳感器的同步量測(cè)數(shù)據(jù)[7]。建立傳感器和機(jī)器人坐標(biāo)系的姿態(tài)變換矩陣A如式(1)所示。

(1)

式(1)中姿態(tài)變換矩陣A為正交矩陣，a、b、c分別為傳感器相對(duì)機(jī)器人的側(cè)傾角度、俯仰角度、偏轉(zhuǎn)角度。當(dāng)移動(dòng)機(jī)器人執(zhí)行任務(wù)時(shí)，多個(gè)機(jī)器人移動(dòng)到工作空間，標(biāo)定機(jī)器人位姿，統(tǒng)計(jì)機(jī)器人不同位姿時(shí)末端執(zhí)行器的空間位置，設(shè)執(zhí)行器在傳感器和機(jī)器人坐標(biāo)系下的姿態(tài)角分別為B1、B2，兩個(gè)坐標(biāo)系的轉(zhuǎn)換關(guān)系如式(2)所示。

B1=AB2

(2)

通過以上操作，完成多傳感信息的時(shí)間配準(zhǔn)和空間配準(zhǔn)。當(dāng)多傳感器的量測(cè)數(shù)據(jù)轉(zhuǎn)換至機(jī)器人坐標(biāo)系后，融合配準(zhǔn)數(shù)據(jù)，設(shè)m個(gè)傳感器的量測(cè)數(shù)據(jù)分別為X1,X2,…,Xm，引入m個(gè)加權(quán)因子，X1,X2,…,Xm加權(quán)融合后的值X如式(3)所示。

(3)

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

(4)

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

(5)

(6)

(7)

1.2 建立移動(dòng)機(jī)器人運(yùn)動(dòng)模型

建立移動(dòng)機(jī)器人運(yùn)動(dòng)模型，獲得機(jī)器人運(yùn)動(dòng)控制量。忽略噪聲和滑動(dòng)對(duì)機(jī)器人的影響，機(jī)器人機(jī)體速度E如式(8)所示。

E=dcosF+esinF

(8)

其中F為機(jī)器人坐標(biāo)系下的位姿方位角[10]。假定機(jī)器人運(yùn)動(dòng)不發(fā)生側(cè)移，僅在機(jī)體軸方向進(jìn)行轉(zhuǎn)動(dòng)和直線，把機(jī)體速度E看作線速度，設(shè)機(jī)器人運(yùn)動(dòng)位置為(d,e)，d和e分別為橫向和豎向的軸心線方向，機(jī)器人坐標(biāo)系下的速度如式(9)所示。

(9)

(10)

將線速度E和角速度f作為機(jī)器人受到的控制量。設(shè)機(jī)器人質(zhì)量為H，轉(zhuǎn)動(dòng)慣量為I，驅(qū)動(dòng)輪半徑為K，驅(qū)動(dòng)輪之間的距離為k，機(jī)器人向心力矩陣h如式(11)所示。

(11)

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

(12)

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

(13)

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

O+h+j=JM-LTN

(14)

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

1.3 規(guī)劃多移動(dòng)機(jī)器人協(xié)同避障軌跡

通過融合的傳感器數(shù)據(jù)，掌握移動(dòng)機(jī)器人所在的空間信息，采用蟻群算法，規(guī)劃多個(gè)機(jī)器人協(xié)同避障路徑，分別控制各個(gè)機(jī)器人運(yùn)動(dòng)控制量，為單機(jī)器人導(dǎo)航最優(yōu)全局路徑。由傳感器數(shù)據(jù)可知機(jī)器人工作空間的障礙物信息，利用柵格法，將工作環(huán)境抽象為一個(gè)柵格地圖，將最小化路徑長(zhǎng)度，作為機(jī)器人運(yùn)動(dòng)目標(biāo)，目標(biāo)函數(shù)P如式(15)所示。

(15)

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

當(dāng)機(jī)器人到達(dá)目標(biāo)位置后，終止運(yùn)動(dòng)，停止路徑搜索，設(shè)置終止運(yùn)動(dòng)約束條件如式(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步機(jī)器人的橫、縱坐標(biāo)。當(dāng)max(|Ut-1-Ut-2|,|Vt-1-Vt-2|)取值為1時(shí)，max(|Ut-Ut-1|,|Vt-Vt-1|)取0或1；max(|Ut-1-Ut-2|,|Vt-1-Vt-2|)取值為0時(shí)，max(|Ut-Ut-1|,|Vt-Vt-1|)取0。機(jī)器人t-1時(shí)刻到達(dá)目標(biāo)點(diǎn)后，通過終止運(yùn)動(dòng)約束條件，使機(jī)器人不再繼續(xù)運(yùn)動(dòng)。將多個(gè)機(jī)器人視為標(biāo)準(zhǔn)蟻群，單個(gè)機(jī)器人視為一只螞蟻，機(jī)器人選擇下一步行走節(jié)點(diǎn)時(shí)，根據(jù)公式(16)和(17)兩個(gè)約束條件，統(tǒng)計(jì)下一步所有可選節(jié)點(diǎn)，可選節(jié)點(diǎn)應(yīng)滿足避障和終止運(yùn)動(dòng)約束，提取機(jī)器人起點(diǎn)和終點(diǎn)構(gòu)成的向量，以及當(dāng)前節(jié)點(diǎn)和下一步可選節(jié)點(diǎn)構(gòu)成的向量，引入最優(yōu)路徑指引函數(shù)Q如式(18)所示。

Q=pcos(o1-o2)-P

(18)

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

2 實(shí)驗(yàn)分析

將此次設(shè)計(jì)方法，與基于S型曲線的機(jī)器人避障算法、基于改進(jìn)動(dòng)態(tài)窗口法的機(jī)器人避障算法，進(jìn)行對(duì)比實(shí)驗(yàn)，比較多機(jī)器人避障成功率和避障路徑運(yùn)行時(shí)間。

2.1 多移動(dòng)機(jī)器人實(shí)驗(yàn)平臺(tái)

以加工車間為例，生成多移動(dòng)機(jī)器人運(yùn)行空間的地形圖，環(huán)境整體面積為10m×10m，空間內(nèi)部有貨架和桌椅等障礙物，整個(gè)空間覆蓋無線網(wǎng)絡(luò)，如圖1所示。

(a)復(fù)雜環(huán)境移動(dòng)空間

(b)簡(jiǎn)單環(huán)境移動(dòng)空間圖1 多移動(dòng)機(jī)器人工作環(huán)境柵格地圖

選擇多臺(tái)移動(dòng)機(jī)器人，機(jī)器人型號(hào)為BNRT-MOB-44，利用無線路由器，連接機(jī)器人無線終端和局域網(wǎng)，將機(jī)器人作為下位機(jī)，通過Socket通信，在主控PC和機(jī)器人之間傳輸數(shù)據(jù)，由上位機(jī)運(yùn)行多機(jī)器人協(xié)同避障算法，將控制信息發(fā)送給機(jī)器人，機(jī)器人接收PC端命令后，執(zhí)行相應(yīng)的動(dòng)作，各個(gè)機(jī)器人到達(dá)指定目標(biāo)位置。多個(gè)移動(dòng)機(jī)器人編隊(duì)隊(duì)形和周邊環(huán)境如圖2所示。

圖2 多移動(dòng)機(jī)器人編隊(duì)隊(duì)形和周邊環(huán)境

機(jī)器人安裝的傳感器包括激光雷達(dá)和里程計(jì)，為保證機(jī)器人運(yùn)動(dòng)狀態(tài)穩(wěn)定性，共進(jìn)行50次實(shí)驗(yàn)。

2.2 實(shí)驗(yàn)結(jié)果分析

設(shè)置移動(dòng)機(jī)器人避障安全距離為0.3m，保持距離為0.4m，改變機(jī)器人行駛速度，若單機(jī)器人距離障礙物小于0.3m，或未到達(dá)目標(biāo)點(diǎn)，視為避障失敗，否則為避障成功，統(tǒng)計(jì)成功次數(shù)，計(jì)算成功次數(shù)和總次數(shù)的比值，三種算法成功率測(cè)試結(jié)果如圖3所示。

(a)復(fù)雜環(huán)境避障成功率

(b)簡(jiǎn)單環(huán)境避障成功率圖3 多移動(dòng)機(jī)器人避障成功率實(shí)驗(yàn)對(duì)比結(jié)果

由圖3(a)和圖3(b)可知，避障成功率隨著機(jī)器人移動(dòng)速度的增加而減小，針對(duì)復(fù)雜環(huán)境和簡(jiǎn)單環(huán)境，設(shè)計(jì)算法多機(jī)器人協(xié)同避障成功率，明顯高于另外兩種算法，機(jī)器人可以穿過障礙物到達(dá)目標(biāo)位置，滿足多機(jī)器人協(xié)同控制要求。

在第一組實(shí)驗(yàn)的基礎(chǔ)上，讓多機(jī)器人按照三種算法規(guī)劃好的路徑運(yùn)行，統(tǒng)計(jì)多機(jī)器人運(yùn)行時(shí)間，實(shí)驗(yàn)對(duì)比結(jié)果如圖4所示。

(a)復(fù)雜環(huán)境運(yùn)行時(shí)間

(b)簡(jiǎn)單環(huán)境運(yùn)行時(shí)間圖4 避障路徑運(yùn)行時(shí)間實(shí)驗(yàn)對(duì)比結(jié)果

由圖4(a)和圖4(b)可知，機(jī)器人行駛速度增加時(shí)，規(guī)劃路徑運(yùn)行時(shí)間隨之減少，在兩種移動(dòng)環(huán)境中，設(shè)計(jì)算法規(guī)劃的避障路徑運(yùn)行時(shí)間最短，有效保證了機(jī)器人執(zhí)行效率。

3 結(jié)論

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