基于信息融合的移動(dòng)機(jī)器人全局避障路徑規(guī)劃算法

周 婷

(安徽三聯(lián)學(xué)院 基礎(chǔ)實(shí)驗(yàn)教學(xué)中心，安徽 合肥 230071)

移動(dòng)機(jī)器人被廣泛應(yīng)用于多個(gè)領(lǐng)域，其工作環(huán)境也日益復(fù)雜化.在包含眾多障礙物的環(huán)境中，為了保證機(jī)器人正常工作，需要采用路徑規(guī)劃算法建立一條可行路線(xiàn)，避免與障礙物發(fā)生碰撞[1].根據(jù)規(guī)劃路徑控制機(jī)器人行駛方向，輔助移動(dòng)機(jī)器人順利完成工作任務(wù)[2].目前，為了實(shí)現(xiàn)全局避障路徑規(guī)劃，已經(jīng)涌現(xiàn)了多種規(guī)劃算法.

文獻(xiàn)[3]采用柵格法描述全局障礙物分布情況，按照機(jī)器人投影面積，設(shè)置柵格大小，并針對(duì)每一個(gè)柵格，建立多階障礙物矩陣.根據(jù)障礙物分布狀態(tài)，通過(guò)鄰域搜索方式生成一條機(jī)器人運(yùn)行路徑.同時(shí)，引用改進(jìn)代價(jià)函數(shù)，根據(jù)地形條件更新路徑規(guī)劃方案.但是，該算法規(guī)劃路徑避障效果較差.文獻(xiàn)[4]參考傳統(tǒng)規(guī)劃算法生成的路徑，提取出關(guān)鍵轉(zhuǎn)折點(diǎn)，以此為圓心采集周?chē)鷧^(qū)域環(huán)境信息.根據(jù)障礙物距離信息，通過(guò)變距離算法優(yōu)化關(guān)鍵點(diǎn)，重構(gòu)移動(dòng)機(jī)器人運(yùn)行路徑.但是，該算法規(guī)劃路徑總里程較長(zhǎng).文獻(xiàn)[5]構(gòu)建兩棵搜索樹(shù)，在全局范圍內(nèi)交替搜索避障關(guān)鍵點(diǎn)，并對(duì)每個(gè)節(jié)點(diǎn)進(jìn)行目標(biāo)偏置擴(kuò)展.針對(duì)選定的節(jié)點(diǎn)進(jìn)行布線(xiàn)，并運(yùn)用增強(qiáng)算法計(jì)算障礙物對(duì)節(jié)點(diǎn)的影響.最后，通過(guò)碰撞檢測(cè)和冗余節(jié)點(diǎn)修剪等處理方式，驗(yàn)證布線(xiàn)路徑的可行性.但是，該算法規(guī)劃路徑的平滑性較差.

文中為了解決傳統(tǒng)路徑規(guī)劃算法的不足之處，提出采用信息融合算法，融合不同傳感器采集的環(huán)境信息，精準(zhǔn)定位移動(dòng)機(jī)器人所處位置，以此為基礎(chǔ)建立全局避障路徑規(guī)劃算法.尤其面對(duì)密集障礙區(qū)，文中提出根據(jù)機(jī)器人動(dòng)態(tài)位置信息，從障礙物之中，建立一條安全可行的行駛路徑，達(dá)到縮短規(guī)劃路徑總里程的目的.

1 基于信息融合的移動(dòng)機(jī)器人全局避障路徑規(guī)劃算法設(shè)計(jì)

1.1 構(gòu)建全局環(huán)境柵格模型

為了實(shí)現(xiàn)全局避障路徑規(guī)劃，文中采用柵格法建立機(jī)器人運(yùn)行環(huán)境場(chǎng)景模型[6].柵格大小直接影響環(huán)境建模效果，本文以移動(dòng)機(jī)器人直徑為依據(jù)設(shè)置柵格長(zhǎng)度，全局環(huán)境地圖的總柵格數(shù)可以通過(guò)以下公式計(jì)算，

(1)

上式中，N表示環(huán)境地圖總柵格數(shù)，W表示地圖長(zhǎng)度，A表示地圖寬度，f表示柵格長(zhǎng)度，也是移動(dòng)機(jī)器人直徑.全局環(huán)境地圖的總柵格數(shù)N的計(jì)算是根據(jù)全局地圖的長(zhǎng)度、寬度與柵格長(zhǎng)度之間的比值相乘計(jì)算，獲得了總柵格數(shù).

通常情況下，全局環(huán)境模型的單元柵格包含兩種狀態(tài)，分別是包含障礙物的柵格以及空白的自由柵格.為了將不規(guī)則形狀的障礙物在柵格中描述出來(lái)，文中提出采用膨脹處理的方式，通過(guò)元素“探針”進(jìn)行平移探索，生成膨脹后的障礙物柵格，障礙物柵格膨脹處理模式如圖1所示.

(a) 目標(biāo)圖像 (b) 結(jié)構(gòu)元素 (c) 膨脹結(jié)果

按照?qǐng)D1所示的膨脹處理模式，將障礙物所處位置直觀(guān)表示出來(lái)，考慮到膨脹法的運(yùn)用，會(huì)形成部分未清掃區(qū)域，影響路徑規(guī)劃結(jié)果[7].因此，提出在全局環(huán)境柵格模型構(gòu)建過(guò)程中，柵格主要包括四種狀態(tài)，分別是大型障礙物完全占據(jù)柵格、大型障礙物邊界占據(jù)柵格、小型障礙物柵格以及不含障礙物柵格.其中，全局避障路徑規(guī)劃過(guò)程中，障礙物邊界占據(jù)柵格與小型障礙物占據(jù)柵格，可以當(dāng)作自由柵格來(lái)看，便于避障路徑的規(guī)劃.

1.2 設(shè)計(jì)信息融合定位方法

為了避免環(huán)境特征信息片面性現(xiàn)象，引發(fā)的移動(dòng)機(jī)器人定位不精確問(wèn)題，提出信息融合定位方法，融合多個(gè)傳感器的定位信息，精準(zhǔn)定位機(jī)器人所處位置，面對(duì)障礙物較為集中的區(qū)域[8]，不再?gòu)耐鈧?cè)繞行，而是從障礙物群中間安全穿行，實(shí)現(xiàn)規(guī)劃路徑總里程的縮短.文中提出在移動(dòng)機(jī)器人上布置里程計(jì)和麥克風(fēng)陣列，移動(dòng)機(jī)器人運(yùn)動(dòng)后，得到圖2所示的示意圖.

圖2 移動(dòng)機(jī)器人運(yùn)動(dòng)示意圖

為了結(jié)合里程計(jì)和聲源信息，需要先分別計(jì)算二者所描述的機(jī)器人定位值.如圖2所示，機(jī)器人的位姿變化，主要是沿著圓弧運(yùn)動(dòng)[9]，依托于上一時(shí)刻的里程計(jì)定位值，得到當(dāng)前時(shí)刻里程計(jì)定位值.

(2)

公式(2)中，k表示時(shí)刻，(c,d,θ)表示機(jī)器人位姿，(ck,dk,θk)表示k時(shí)刻機(jī)器人位姿，(ck-1,dk -1,θk-1)表示k-1時(shí)刻機(jī)器人位姿，D表示機(jī)器人移動(dòng)距離，β表示機(jī)器人方向角變化值，cos表示余弦函數(shù)，sin表示正弦函數(shù).其中

Pk=Pk-1+Sk,

(3)

公式(3)中，P表示里程計(jì)顯示的機(jī)器人位姿，S表示位姿增量.

采用聲源定位技術(shù)，根據(jù)麥克風(fēng)陣列的平面投影和距陣列中心距離等參數(shù)信息，得出聲源在二維平面上的坐標(biāo)信息.

(4)

公式(4)中，(x,y)表示聲源平面坐標(biāo)，v表示聲速，δ1、δ2、δ3、δ4表示聲源到達(dá)時(shí)間延遲，η表示麥克風(fēng)到陣列中心的距離.

圖2中，(a,b)表示麥克風(fēng)陣列的位置，Q0、Q1表示兩個(gè)聲源，XIYIOI表示聲源定位坐標(biāo)系.兩個(gè)聲源呈現(xiàn)出對(duì)稱(chēng)分布特點(diǎn)，在全局坐標(biāo)系中，機(jī)器人位姿表示為

(5)

公式(5)中，(xQ0,yQ0)表示聲源Q0的坐標(biāo)，(xQ1,yQ1)表示聲源Q1的坐標(biāo)，arctan表示反正切函數(shù).

里程計(jì)定位信息與聲源定位信息的融合本質(zhì)上是當(dāng)某一項(xiàng)定位技術(shù)所得信息不可靠時(shí)，采用另一項(xiàng)技術(shù)所得的定位結(jié)果為參考，確保最終定位結(jié)果誤差范圍較小.依托于機(jī)器人歷史定位信息，和里程計(jì)顯示數(shù)據(jù)，得出當(dāng)前位姿估計(jì)結(jié)果，再結(jié)合麥克風(fēng)陣列測(cè)量的定位值，將機(jī)器人位姿表示為

ψ=φ″+t×μt,

(6)

公式(6)中，ψ表示當(dāng)前機(jī)器人位姿，φ″表示麥克風(fēng)陣列測(cè)得的機(jī)器人位姿，t表示最后獲取聲源定位信息的時(shí)刻，μ表示里程計(jì)測(cè)得的位姿增量.

1.3 建立移動(dòng)機(jī)器人避障方案

根據(jù)機(jī)器人當(dāng)前位置信息，文中采用人工勢(shì)場(chǎng)法，提出引力勢(shì)函數(shù)和斥力勢(shì)函數(shù)[10]，建立移動(dòng)機(jī)器人避障方案.其中，引力勢(shì)函數(shù)可以表示為

(7)

公式(7)中，H表示引力勢(shì)函數(shù)，e表示大于零的引力勢(shì)場(chǎng)常量，ω0表示機(jī)器人位置向量，ω1表示目標(biāo)位置.

斥力勢(shì)函數(shù)可以表示為

(8)

公式(8)中，G表示斥力勢(shì)函數(shù)，λ表示常量系數(shù)，z表示機(jī)器人與障礙物的最短距離，z0表示障礙物在市場(chǎng)中影響范圍.

通過(guò)引力和斥力的共同作用，得到一條安全的運(yùn)動(dòng)路線(xiàn).為了避免移動(dòng)機(jī)器人避障路徑規(guī)劃結(jié)果出現(xiàn)局部最小點(diǎn)問(wèn)題，本文進(jìn)一步改進(jìn)了傳統(tǒng)的人工勢(shì)場(chǎng)算法，實(shí)現(xiàn)全局避障路徑規(guī)劃.根據(jù)機(jī)器人定位結(jié)果，以其所處位置為原點(diǎn)設(shè)計(jì)一個(gè)坐標(biāo)系，并引出一條與目標(biāo)點(diǎn)相切的直線(xiàn)，以機(jī)器人到切線(xiàn)的長(zhǎng)度為半徑，生成避障路徑規(guī)劃范圍扇形圖，并將其劃分為兩個(gè)區(qū)域，如圖3所示.

圖3 避障路徑范圍規(guī)劃

如圖3所示，明確避障路徑規(guī)劃范圍后，采用人工勢(shì)場(chǎng)法編寫(xiě)避障方案.首先判斷兩個(gè)避障路徑規(guī)劃區(qū)域內(nèi)是否包含其他障礙物，若沒(méi)有則可以直接計(jì)算機(jī)器人與目標(biāo)點(diǎn)直接的距離，以及機(jī)器人與障礙物之間的間距，搜索到最優(yōu)避障方向.若區(qū)域內(nèi)包含其他障礙物，需要求解障礙物坐標(biāo)向量，計(jì)算出該障礙物產(chǎn)生的斥力，重新得出合力方向.按照上述方案，完成移動(dòng)機(jī)器人避障路徑規(guī)劃.

1.4 生成全局路徑規(guī)劃結(jié)果

依托于機(jī)器人避障方案生成初始全局避障路徑后，針對(duì)轉(zhuǎn)彎問(wèn)題設(shè)計(jì)相應(yīng)的防碰撞策略，生成最終路徑規(guī)劃結(jié)果根據(jù)機(jī)器人轉(zhuǎn)彎半徑和圓心，得到圓心轉(zhuǎn)動(dòng)方向，以及位置.

(9)

公式(9)中，J表示移動(dòng)機(jī)器人轉(zhuǎn)彎圓心，R表示轉(zhuǎn)彎半徑，(xJ，yJ)表示圓心坐標(biāo)，M0表示圓心方向，M1表示起始點(diǎn)方向，ξ表示圓心轉(zhuǎn)動(dòng)方向.

機(jī)器人順時(shí)針?lè)较蜣D(zhuǎn)彎過(guò)程中，可以根據(jù)圓心方向和轉(zhuǎn)彎點(diǎn)坐標(biāo)，求解轉(zhuǎn)彎起始點(diǎn)與轉(zhuǎn)彎目標(biāo)點(diǎn)之間的間距，以及轉(zhuǎn)彎斜率和轉(zhuǎn)彎角度.

(10)

公式(10)中，τ1表示轉(zhuǎn)彎起始點(diǎn)，τ2表示轉(zhuǎn)彎目標(biāo)點(diǎn)，E表示兩點(diǎn)間距的長(zhǎng)度，F(xiàn)L表示圓外切線(xiàn)，?表示斜率，κ1表示離開(kāi)第一段弧的角度，κ2表示到達(dá)第二段弧的角度.

按照上述轉(zhuǎn)彎策略，將機(jī)器人轉(zhuǎn)彎問(wèn)題拆分為兩個(gè)分段，確保轉(zhuǎn)彎角度平緩，避免移動(dòng)機(jī)器人運(yùn)行出現(xiàn)碰撞問(wèn)題.至此，完成移動(dòng)機(jī)器人全局避障路徑規(guī)劃算法的整體設(shè)計(jì).

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

2.1 實(shí)驗(yàn)準(zhǔn)備

為了驗(yàn)證本文設(shè)計(jì)算法的可行性，以?huà)叩貦C(jī)器人為實(shí)驗(yàn)對(duì)象，進(jìn)行全局避障路徑規(guī)劃.本次實(shí)驗(yàn)選定的機(jī)器人直徑和高度分別為30 cm和12 cm，在該機(jī)器人頂部位置安裝雷達(dá)，并在下方電路板上連接編碼器、麥克風(fēng)陣列等傳感設(shè)備.在空曠的場(chǎng)所內(nèi)，應(yīng)用多種物品充當(dāng)障礙物，設(shè)置實(shí)驗(yàn)場(chǎng)景.將掃地機(jī)器人放置在實(shí)驗(yàn)環(huán)境中，進(jìn)行避障路徑規(guī)劃，部分實(shí)驗(yàn)環(huán)境實(shí)景圖如圖4所示.

圖4 部分實(shí)驗(yàn)場(chǎng)景圖

根據(jù)本文研究?jī)?nèi)容，在實(shí)驗(yàn)開(kāi)始之前，啟動(dòng)掃地機(jī)器人在實(shí)驗(yàn)場(chǎng)景內(nèi)行走一周，獲取環(huán)境柵格地圖.再將機(jī)器人放置在起點(diǎn)位置，明確機(jī)器人運(yùn)行終點(diǎn)生成全局避障路徑.

2.2 實(shí)驗(yàn)環(huán)境柵格圖

為了保證路徑規(guī)劃結(jié)果的全局性，通過(guò)柵格化技術(shù)，模擬實(shí)驗(yàn)場(chǎng)景障礙物分布圖，得到圖5所示的實(shí)驗(yàn)環(huán)境柵格圖.

圖5 實(shí)驗(yàn)環(huán)境柵格圖

從圖5可以看出本次實(shí)驗(yàn)環(huán)境中障礙物大小不一，分布范圍廣泛，符合機(jī)器人全局避障路徑規(guī)劃實(shí)驗(yàn)要求.其中左下角圓形標(biāo)注單元格為機(jī)器人起始點(diǎn)，右上角五角星標(biāo)注的單元格，為移動(dòng)機(jī)器人運(yùn)行的目標(biāo)點(diǎn).

2.3 避障路徑規(guī)劃結(jié)果

在實(shí)驗(yàn)場(chǎng)景中，運(yùn)用文中提出的路徑規(guī)劃算法得出全局避障路徑.為了表現(xiàn)文中設(shè)計(jì)算法的優(yōu)越性，選用基于動(dòng)態(tài)窗口算法DWA和基于免疫機(jī)理的路徑規(guī)劃算法作為對(duì)照組，同樣進(jìn)行全局避障路徑規(guī)劃，得到圖6所示的規(guī)劃路徑對(duì)比圖.

圖6 不同算法規(guī)劃路徑對(duì)比圖

根據(jù)圖6所示的規(guī)劃路徑對(duì)比圖可以看出，三種算法所規(guī)劃的路徑，均避開(kāi)了障礙物，呈現(xiàn)出了良好的避障效果.其中，基于DWA算法和基于免疫機(jī)理算法由于無(wú)法精確定位機(jī)器人實(shí)時(shí)位置，面對(duì)密集的障礙物區(qū)域，均選擇向上或向下繞行，雖然達(dá)到了避障效果，但規(guī)劃出的路徑總里程較長(zhǎng).本文提出的算法由于引入了信息融合理念，結(jié)合了里程計(jì)和聲源信息，完成了機(jī)器人運(yùn)行位置的精確定位，再進(jìn)行全局避障規(guī)劃，從障礙物群中間穿行而過(guò)，達(dá)到避障效果的同時(shí)，縮短了規(guī)劃路徑的總里程.為了便于定量分析，匯總?cè)N算法規(guī)劃路徑總里程信息，得到表1所示的總里程對(duì)比表.

表1 不同算法規(guī)劃路徑總里程統(tǒng)計(jì)表

根據(jù)表1可知，文中設(shè)計(jì)算法規(guī)劃路徑的總里程為15.7 m，基于DWA算法、基于免疫機(jī)理算法規(guī)劃出的路徑，總里程分別達(dá)到了21.5 m和23.1 m.綜上所述，以信息融合技術(shù)為基礎(chǔ)，建立的避障路徑規(guī)劃算法，與其他算法相比總里程縮短了26.97%和32.03%.

3 結(jié)束語(yǔ)

移動(dòng)機(jī)器人的發(fā)展直接促進(jìn)了工業(yè)自動(dòng)化的進(jìn)步.隨著人們對(duì)機(jī)器人的要求越來(lái)越高，機(jī)器人全局避障路徑的規(guī)劃成為關(guān)鍵的研究問(wèn)題之一.本文以該問(wèn)題為研究重點(diǎn)，參考傳統(tǒng)路徑規(guī)劃算法，針對(duì)定位誤差引發(fā)的避障路徑總里程較長(zhǎng)的問(wèn)題，提出運(yùn)用信息融合技術(shù)，結(jié)合里程計(jì)定位信息和聲源定位信息，獲取實(shí)時(shí)精確定位信息，以此為基礎(chǔ)，得到一種新的全局避障路徑規(guī)劃算法.