周 婷
(安徽三聯(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é)同避障算法。
通過多傳感器信息預(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)
建立移動(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)模型的建立。
通過融合的傳感器數(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ì)。
將此次設(shè)計(jì)方法,與基于S型曲線的機(jī)器人避障算法、基于改進(jìn)動(dòng)態(tài)窗口法的機(jī)器人避障算法,進(jìn)行對(duì)比實(shí)驗(yàn),比較多機(jī)器人避障成功率和避障路徑運(yùn)行時(shí)間。
以加工車間為例,生成多移動(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)。
設(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í)行效率。
此次研究設(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ī)器人軌跡跟蹤精度。