顧家瑋，唐 煒，張 樂，陳峻山，周 魯，成志鋒

(1.上海無(wú)線電設(shè)備研究所·上?！?01109；2.西北工業(yè)大學(xué) 自動(dòng)化學(xué)院·西安·710129)

0 引 言

多智能體協(xié)同在民用、工業(yè)、國(guó)防、軍用等領(lǐng)域中有著重要的應(yīng)用價(jià)值。特別在軍事方面，多智能體協(xié)同作戰(zhàn)平臺(tái)正在成為各個(gè)國(guó)家武器裝備家族中的一個(gè)重要分支[1-2]。具體地，在未知環(huán)境中，可以利用多智能體協(xié)同完成偵察、搜索等任務(wù)[3]。就多機(jī)器人系統(tǒng)中的協(xié)同控制、路徑規(guī)劃、協(xié)同感知、信息決策等方面而言，目前，軍事任務(wù)中的多機(jī)器人協(xié)同模式尚不完善，如何對(duì)機(jī)器人群體進(jìn)行有效的協(xié)調(diào)和控制，一直是機(jī)器人軍事化應(yīng)用中的關(guān)鍵。室內(nèi)環(huán)境下多機(jī)器人的協(xié)同定位與路徑規(guī)劃，是考察多機(jī)器人系統(tǒng)協(xié)作機(jī)制的一種常用研究方法。

自主導(dǎo)航作為移動(dòng)機(jī)器人的關(guān)鍵技術(shù)，需要考慮定位、任務(wù)規(guī)劃、路徑規(guī)劃問題。針對(duì)定位問題，采用磁帶或電磁導(dǎo)航[4-5]的第一代導(dǎo)航方式，以及依靠環(huán)境中信標(biāo)或二維碼[6]定位的第二代定位導(dǎo)航方式，都需要預(yù)先布置標(biāo)記信息，而不能被應(yīng)用于軍事偵察任務(wù)中。SLAM技術(shù)[7]是目前機(jī)器人產(chǎn)業(yè)的主要研究方向，已產(chǎn)生了基于視覺、激光雷達(dá)、聲納等的SLAM算法[8-9]。Geppert將絕對(duì)位姿估計(jì)與視覺慣性里程計(jì)進(jìn)行了結(jié)合，提出了多攝像機(jī)視覺慣性定位算法，使得系統(tǒng)可以提供無(wú)漂移的姿態(tài)估計(jì)[10]。Kim依靠卷積神經(jīng)網(wǎng)絡(luò)，利用環(huán)境信息將位置識(shí)別問題轉(zhuǎn)化成了位置分類問題，通過學(xué)習(xí)點(diǎn)云描述符得到了一個(gè)定位器[11]。北京理工大學(xué)的馬家辰[12]等對(duì)傳統(tǒng)粒子濾波算法存在的粒子消耗問題進(jìn)行了改進(jìn)，通過引入遺傳算法優(yōu)化保持了粒子的多樣性，從而提高了定位的精度；浙江大學(xué)網(wǎng)絡(luò)系統(tǒng)與控制研究所利用以多狀態(tài)約束卡爾曼濾波為基礎(chǔ)的視覺慣性測(cè)量，依靠先驗(yàn)雷達(dá)地圖提供邊界三維導(dǎo)航，修正了視覺里程計(jì)的累計(jì)漂移[13]。為提高SLAM的定位精度，還可采用高精度傳感器和采用多傳感器融合等方法[14-16]。其中，多傳感器融合可將各類傳感器獲得的數(shù)據(jù)進(jìn)行分析處理，將不同的傳感器數(shù)據(jù)進(jìn)行融合，以實(shí)現(xiàn)互補(bǔ)，從而使系統(tǒng)在成本相對(duì)低廉時(shí)，魯棒性和精度更高。

Thrun等人利用SEIF(Spare Extended Information Filter)方法實(shí)現(xiàn)了多機(jī)器人的同步定位與制圖。該算法用高斯馬爾可夫隨機(jī)場(chǎng)模型表示機(jī)器人的位姿和地圖信息，通過搜索局部地圖路標(biāo)和全局地圖路標(biāo)的匹配關(guān)系，將局部地圖映射到全局地圖。Howard提出了一種基于粒子濾波的多機(jī)器人同步定位和制圖算法，每個(gè)機(jī)器人可用粒子集來近似表征機(jī)器人位姿的概率密度函數(shù)。該算法需要提前獲得各個(gè)機(jī)器人之間的相對(duì)位置關(guān)系，通過坐標(biāo)系變換得到完整的地圖。Ryad Chellali等提出了一種基于環(huán)境特征的多機(jī)器人SLAM算法。每個(gè)機(jī)器人可利用激光測(cè)距儀和相機(jī)采集環(huán)境中垂直和水平的線特征，利用EKF結(jié)合里程計(jì)信息估計(jì)自身的姿態(tài)，同時(shí)利用線特征構(gòu)建環(huán)境特征空間。在多機(jī)器人共有的環(huán)境特征匹配成功之后，就可以求出多個(gè)機(jī)器人之間的坐標(biāo)變換關(guān)系。

路徑規(guī)劃作為整個(gè)系統(tǒng)的關(guān)鍵技術(shù)之一，是多機(jī)器人協(xié)作系統(tǒng)順利運(yùn)行和多機(jī)協(xié)同效能的重要保證。針對(duì)共享工作空間的多機(jī)器人系統(tǒng)的路徑規(guī)劃，需要解決以下兩方面的問題：(1)機(jī)器人相對(duì)于靜態(tài)障礙物的無(wú)碰撞路徑的規(guī)劃；(2)多機(jī)器人相互之間的無(wú)碰撞協(xié)調(diào)路徑規(guī)劃。Pan等針對(duì)以上兩個(gè)問題提出了基于編隊(duì)控制的虛擬彈簧方法，各機(jī)器人根據(jù)控制律和虛擬目標(biāo)搜索方法，在預(yù)定的編隊(duì)中計(jì)算到達(dá)目標(biāo)的最優(yōu)路徑[17]。針對(duì)機(jī)器個(gè)體難以識(shí)別周圍機(jī)器人屬性的問題，Bae等將深度Q學(xué)習(xí)與傳統(tǒng)的卷積神經(jīng)網(wǎng)絡(luò)(Convolution Neural Network，CNN)進(jìn)行了結(jié)合。其中，CNN利用其環(huán)境中的圖像信息分析精確的情況，機(jī)器人則根據(jù)這種情況進(jìn)行導(dǎo)航[18]。Bader與Binder在假設(shè)各機(jī)器人定位信息完備的情況下，將時(shí)間空間優(yōu)先思想用于多機(jī)路徑規(guī)劃[19-20]，將多機(jī)器人規(guī)劃問題分解成了多個(gè)單機(jī)規(guī)劃問題，從而降低了問題的復(fù)雜度。個(gè)體按照優(yōu)先級(jí)的順序移動(dòng)，由單機(jī)路徑規(guī)劃器檢查潛在碰撞，然而并未被部署在實(shí)際的機(jī)器人系統(tǒng)中。在實(shí)際的多機(jī)器人路徑規(guī)劃中，路徑?jīng)_突的種類繁雜且需要考慮全局效率優(yōu)化的問題，而定位問題常被作為已知假設(shè)來進(jìn)行路徑規(guī)劃研究。針對(duì)以上兩點(diǎn)，目前國(guó)內(nèi)多數(shù)的研究成果依然處于理論階段，并沒有將其結(jié)合部署在實(shí)際的多機(jī)器人系統(tǒng)中，并進(jìn)行實(shí)驗(yàn)驗(yàn)證。

本文針對(duì)一般復(fù)雜室內(nèi)環(huán)境，結(jié)合SLAM技術(shù)，研究并建立了多機(jī)器人協(xié)同SLAM系統(tǒng)。系統(tǒng)采用多傳感器融合的方式，以提高機(jī)器人的定位精度?？紤]到環(huán)境信息未知，在由多個(gè)機(jī)器人構(gòu)建得到子圖的基礎(chǔ)上，系統(tǒng)基于坐標(biāo)變換進(jìn)行地圖融合。在全局地圖信息已知的情況下，對(duì)文獻(xiàn)[20]所提出的多機(jī)路徑規(guī)劃方法，本文采用SLAM技術(shù)解決其中的定位假設(shè)問題。最后，在實(shí)際環(huán)境中，利用三臺(tái)機(jī)器人給出了多機(jī)協(xié)同地圖融合的結(jié)果，并驗(yàn)證了多機(jī)路徑規(guī)劃方案的有效性。本文的重點(diǎn)在于多機(jī)器人SLAM系統(tǒng)和平臺(tái)的實(shí)驗(yàn)驗(yàn)證。

1 場(chǎng)景和問題描述

基本的室內(nèi)環(huán)境表述為如圖1所示的場(chǎng)景：(1)藍(lán)色柵格表示環(huán)境中的障礙物，紅色方塊表示機(jī)器人r={robot_0,robot_1,…,robot_n}。其中，機(jī)器人的總數(shù)量為n+1個(gè)，g0、g1、g2分別對(duì)應(yīng)robot_0、robot_1與robot_2在某次任務(wù)中需要到達(dá)的目標(biāo)位置。機(jī)器人執(zhí)行第i次任務(wù)的目標(biāo)點(diǎn)集合為G={g1i,g2i,…,gni}；(2)場(chǎng)景中的可行路徑如圖1所示。障礙物間的路徑為單行道路，即同一時(shí)刻只允許單個(gè)機(jī)器人通過。在多機(jī)器人系統(tǒng)對(duì)該室內(nèi)環(huán)境信息未知的情況下，本文中機(jī)器人協(xié)同的目的為，利用多個(gè)機(jī)器人協(xié)作完成該未知區(qū)域地圖信息的獲?。辉诘玫降貓D信息后，考慮多機(jī)器人路徑規(guī)劃，控制多機(jī)器人到達(dá)各自的指定目的地，并完成各自的任務(wù)。

圖1 室內(nèi)環(huán)境模型Fig.1 Model of indoor environment

2 多機(jī)器人位姿估計(jì)和地圖融合

多機(jī)器人系統(tǒng)需要機(jī)器人在完成自身定位的基礎(chǔ)上，分享自身的位姿和傳感器信息。對(duì)各機(jī)器人構(gòu)建的環(huán)境地圖進(jìn)行拼接，可完成全局地圖的構(gòu)建，并以此提高構(gòu)圖效率。

2.1 機(jī)器人位姿估計(jì)

機(jī)器人位姿估計(jì)作為導(dǎo)航的重要部分，是進(jìn)行后續(xù)構(gòu)圖以及規(guī)劃的基礎(chǔ)，而編碼器的自身誤差等問題會(huì)降低機(jī)器人的定位精度。本文通過多傳感器融合提高了機(jī)器人的定位精度，機(jī)器人個(gè)體主要通過慣性測(cè)量單元(Inertial Measurement Unit，IMU)來糾正里程計(jì)的誤差，其原理圖如圖2所示。對(duì)加速度計(jì)和陀螺儀進(jìn)行零漂校準(zhǔn)后，陀螺儀根據(jù)加速度計(jì)和磁力計(jì)的漂移補(bǔ)償進(jìn)行互補(bǔ)濾波，并將其作為濾波的觀測(cè)更新方程數(shù)據(jù)，將陀螺儀作為狀態(tài)更新方程數(shù)據(jù)，隨后輸出姿態(tài)角信息。當(dāng)室內(nèi)機(jī)器人啟動(dòng)時(shí)，利用IMU 初始值替代里程計(jì)數(shù)據(jù)，獲取較為準(zhǔn)確的初始位置；在機(jī)器人移動(dòng)后，里程計(jì)每間隔一段時(shí)間訪問經(jīng)加速度計(jì)修正后的二次積分位移值，并將其與里程計(jì)自身的位移進(jìn)行比較。若積分位移很小，里程計(jì)換算位移較大，則說明發(fā)生了車輪打滑或懸空現(xiàn)象。此時(shí)，里程計(jì)位移應(yīng)采用IMU數(shù)據(jù)，否則仍應(yīng)使用編碼器位移數(shù)據(jù)。

圖2 里程計(jì)校正框圖Fig.2 Principle of odometer correction

多傳感器系統(tǒng)模型如式(1)所示。在該模型中，每個(gè)傳感器對(duì)應(yīng)的觀測(cè)量及其觀測(cè)方程均可不同。前一個(gè)傳感器更新之后的系統(tǒng)狀態(tài)量數(shù)據(jù)，可以作為下一個(gè)傳感器的預(yù)測(cè)狀態(tài)量，以實(shí)現(xiàn)融合，最后一個(gè)傳感器輸出的數(shù)據(jù)可以用來進(jìn)行下一個(gè)時(shí)刻的迭代。

x(t+1)=Ax(t)+Bu(t)+ε
y1(t)=C1x(t)+δ1
?
yn(t)=Cnx(t)+δn

(1)

式中，x(t)代表t時(shí)刻的系統(tǒng)狀態(tài)量，u(t)代表t時(shí)刻對(duì)系統(tǒng)的控制量，A、B為系統(tǒng)參數(shù)，yi(k)(i=1，…，n)代表某個(gè)測(cè)量值，Ci(i=1，…，n)代表某個(gè)測(cè)量值的觀測(cè)矩陣，ε和δi(i=1，…，n)分別為過程中的噪聲。

系統(tǒng)狀態(tài)量可定義如下

[x,y,z,θ,φ,ψ]T

(2)

向量中，x,y,z指移動(dòng)機(jī)器人在空間坐標(biāo)中的三維坐標(biāo)，θ為移動(dòng)機(jī)器人繞X軸旋轉(zhuǎn)的角度，也稱俯仰角，φ為移動(dòng)機(jī)器人繞Z軸旋轉(zhuǎn)的角度，也稱翻滾角，ψ為移動(dòng)機(jī)器人繞Y軸旋轉(zhuǎn)的角度，也稱偏航角。

系統(tǒng)輸入為

[u1,u2]T

(3)

系統(tǒng)方程為

x(t)=x(t-1)+u1(t)cos(ψ(t-1))
y(t)=y(t-1)+u1(t)sin(ψ(t-1))
ψ(t)=ψ(t-1)+u2(t)

(4)

通過數(shù)據(jù)信息，分別得到輪式里程計(jì)、IMU的觀測(cè)值和觀測(cè)協(xié)方差矩陣，可運(yùn)用多傳感器信息融合的流程對(duì)系統(tǒng)狀態(tài)進(jìn)行更新。

里程計(jì)的觀測(cè)量為[x,y,z,θ,φ,ψ]T，對(duì)應(yīng)的物理參數(shù)為移動(dòng)機(jī)器人在空間中的坐標(biāo)值、俯仰角、翻滾角和偏航角，對(duì)應(yīng)的觀測(cè)矩陣為

(5)

IMU的觀測(cè)量為[θ,φ,ψ]T，物理參數(shù)為俯仰角、翻滾角和偏航角，其對(duì)應(yīng)的觀測(cè)矩陣為

(6)

數(shù)據(jù)融合的程序流程為

(1)當(dāng)傳感器數(shù)據(jù)開始發(fā)布時(shí)，開始監(jiān)聽里程計(jì)和IMU話題。

(3)控制量u1和u2分別指的是在一個(gè)固定的時(shí)間內(nèi)，由機(jī)器人左右輪的速度數(shù)據(jù)進(jìn)行計(jì)算而得到的移動(dòng)距離和偏航角度，同里程計(jì)發(fā)布的話題數(shù)據(jù)中的坐標(biāo)系偏移程度來計(jì)算得出，然后進(jìn)行狀態(tài)預(yù)測(cè)。

(4)獲取里程計(jì)信息，進(jìn)行數(shù)據(jù)對(duì)比。如果符合偏差條件，則更新系統(tǒng)狀態(tài)，否則轉(zhuǎn)至(3)。

2.2 多機(jī)器人地圖融合

系統(tǒng)中的每個(gè)機(jī)器人都有自身的坐標(biāo)系，基于單個(gè)機(jī)器人SLAM得到的子地圖均以各自的起點(diǎn)為地圖原點(diǎn)來進(jìn)行構(gòu)建?？紤]到機(jī)器人在三維空間中的運(yùn)動(dòng)主要以平移和旋轉(zhuǎn)為主，本文采用了基于坐標(biāo)變換的地圖對(duì)齊方法。該方法的本質(zhì)是尋找各個(gè)機(jī)器人坐標(biāo)系之間的相對(duì)關(guān)系，利用相對(duì)變換關(guān)系對(duì)各自的子地圖做相應(yīng)變換，從而將其合并到同一個(gè)坐標(biāo)系下，以建立全局地圖。

在移動(dòng)機(jī)器人運(yùn)行地圖構(gòu)建程序時(shí)，都會(huì)產(chǎn)生一個(gè)屬于自身的參考坐標(biāo)系和局部地圖。局部地圖和機(jī)器人的參考坐標(biāo)系之間為剛性連接，環(huán)境中的障礙物通過柵格地圖的形式保存在局部地圖內(nèi)。在機(jī)器人出發(fā)前，需確認(rèn)機(jī)器人之間相對(duì)的坐標(biāo)關(guān)系。在系統(tǒng)進(jìn)行地圖拼接時(shí)，機(jī)器人通過計(jì)算自身參考系與參考機(jī)器人的坐標(biāo)系的轉(zhuǎn)換矩陣，便可推算出機(jī)器人的局部地圖的變換關(guān)系。同各機(jī)器人運(yùn)動(dòng)的齊次坐標(biāo)變換，可以將局部地圖中的所有信息映射到參考機(jī)器人的地圖坐標(biāo)系中，并且將參考機(jī)器人的地圖作為全局地圖進(jìn)行保存和使用。

(7)

(8)

將中間矩陣定義為一個(gè)矩陣R，其由兩組基的內(nèi)積組成，用來描述旋轉(zhuǎn)前后同一個(gè)向量的坐標(biāo)變換關(guān)系。矩陣R描述了旋轉(zhuǎn)關(guān)系。在歐氏變換中，某個(gè)向量α經(jīng)過一次旋轉(zhuǎn)和一次平移t后，得到了α,，則有

α,=Rα+t

(9)

其中，t被稱為平移向量。由此，可得到歐氏空間內(nèi)不同機(jī)器人坐標(biāo)系的變換關(guān)系?；谶@個(gè)坐標(biāo)變換關(guān)系，可進(jìn)行地圖的匹配與拼接。

3 基于沖突規(guī)避的多機(jī)路徑規(guī)劃

本文所采用的多機(jī)器人路徑規(guī)劃方案如圖3所示。首先，以隨機(jī)優(yōu)先級(jí)排序方法和一個(gè)建議速度對(duì)系統(tǒng)進(jìn)行優(yōu)先級(jí)規(guī)劃和速度規(guī)劃。在方案中，單機(jī)路徑規(guī)劃采用A*算法。在A*算法的每次迭代中，檢查當(dāng)前擴(kuò)展的路徑點(diǎn)是否與已生成的路徑存在沖突。若路徑搜索出現(xiàn)沖突，則嘗試擴(kuò)展搜索圖，使其能夠找到可以規(guī)避當(dāng)前沖突的路徑點(diǎn)。在此過程中，若搜索到當(dāng)前機(jī)器人的目標(biāo)點(diǎn)，則初步規(guī)劃完成，得到候選路徑，并可檢查該路徑是否有效。如果路徑有效，則將其添加到路徑列表中，然后根據(jù)優(yōu)先級(jí)列表，為下一個(gè)機(jī)器人進(jìn)行單機(jī)器人路徑規(guī)劃。如果生成的路徑無(wú)效，則重新進(jìn)行速度規(guī)劃。如果通過重新選擇機(jī)器人行駛速度仍然無(wú)法解決問題，則需要重新規(guī)劃?rùn)C(jī)器人優(yōu)先級(jí)順序。如果成功為每一個(gè)機(jī)器人找到了各自合適的路徑，則路徑生成器會(huì)輸出對(duì)應(yīng)的路徑列表。

圖3 多機(jī)器人路徑規(guī)劃Fig.3 The process of Multi-robot path planning

由于以某一優(yōu)先級(jí)序列規(guī)劃得到個(gè)體路徑只是問題的可行解而并不是最優(yōu)解，故在此方案中需要重新分配優(yōu)先級(jí)，按照上述步驟多次進(jìn)行路徑規(guī)劃，對(duì)比所有優(yōu)先級(jí)生成路徑的代價(jià)，選擇代價(jià)最小的路徑作為最優(yōu)解。

AC在平面構(gòu)造等深圖上不是氣水內(nèi)外邊界的寬度，而是代表了氣水內(nèi)外邊界深度差，因此可以用FB表示，相當(dāng)于在兩個(gè)坐標(biāo)系下進(jìn)行了轉(zhuǎn)換，這一點(diǎn)是該方法的關(guān)鍵。

在此方案中，單機(jī)路徑規(guī)劃的實(shí)現(xiàn)是整個(gè)多機(jī)全局路徑規(guī)劃的基礎(chǔ)，每個(gè)機(jī)器人都需要按照優(yōu)先級(jí)順序規(guī)劃生成自己的行駛路徑。傳統(tǒng)A*算法是針對(duì)單機(jī)器人建立的，因此在多機(jī)器人路徑規(guī)劃中需要考慮以下兩方面的問題并對(duì)其進(jìn)行完善：

(1)路徑占用：?jiǎn)螜C(jī)器人路徑規(guī)劃時(shí)，必須考慮已完成規(guī)劃的候選路徑，并以此約束新路徑生成；

(2)多機(jī)沖突規(guī)避：針對(duì)同一條路徑，還需考慮多機(jī)器人之間的等待、避碰問題。

4 實(shí)驗(yàn)驗(yàn)證

4.1 實(shí)驗(yàn)環(huán)境和平臺(tái)

實(shí)驗(yàn)選定了一個(gè)長(zhǎng)、寬分別為8.6m與7.3m的室內(nèi)房間，利用一套由多個(gè)自主移動(dòng)機(jī)器人構(gòu)成的多機(jī)器人系統(tǒng)進(jìn)行了多機(jī)器人協(xié)同地圖構(gòu)建與路徑規(guī)劃實(shí)驗(yàn)。該系統(tǒng)的網(wǎng)絡(luò)結(jié)構(gòu)如圖4所示，其中包括三臺(tái)機(jī)器人和一臺(tái)PC機(jī)。每個(gè)機(jī)器人以樹莓派3B作為上位機(jī)，其中安裝了ROS系統(tǒng)，下位機(jī)為基于STM32的運(yùn)動(dòng)控制器?；赥CP/IP網(wǎng)絡(luò)通信協(xié)議完成多機(jī)器人信息交互，構(gòu)成了一個(gè)多機(jī)器人實(shí)驗(yàn)系統(tǒng)。實(shí)驗(yàn)系統(tǒng)利用Pibot機(jī)器人作為機(jī)器人個(gè)體，均配備有IMU和光電編碼器來完成室內(nèi)的相對(duì)定位，并配備了激光雷達(dá)等避障傳感器。單個(gè)機(jī)器人可實(shí)現(xiàn)基于激光雷達(dá)的SLAM技術(shù)，完成地圖構(gòu)建、導(dǎo)航避障等功能。

圖4 實(shí)驗(yàn)系統(tǒng)網(wǎng)絡(luò)結(jié)構(gòu)Fig.4 Composition of the experiment system

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

(1)多機(jī)地圖融合

本節(jié)采用兩臺(tái)Pibot機(jī)器人進(jìn)行多機(jī)器人地圖拼接實(shí)驗(yàn)，實(shí)際拼接結(jié)果如圖5(a)所示。實(shí)驗(yàn)采用2.2節(jié)所述的變換關(guān)系完成地圖拼接。其中，圖5(b)為由機(jī)器人robot_0構(gòu)建得到的子地圖，機(jī)器人robot_1由另一點(diǎn)出發(fā)構(gòu)建地圖對(duì)應(yīng)時(shí)刻的實(shí)驗(yàn)結(jié)果如圖5(c)所示。對(duì)照真實(shí)室內(nèi)環(huán)境，兩臺(tái)機(jī)器人均可正確返回環(huán)境信息。在此基礎(chǔ)上，將同時(shí)刻兩臺(tái)機(jī)器人所構(gòu)建的子地圖進(jìn)行融合，得到的結(jié)果如圖5(d)所示。實(shí)驗(yàn)結(jié)果表明，兩臺(tái)機(jī)器人地圖拼接結(jié)果中的公共區(qū)域高度重合，且沒有尺度上的偏差。多機(jī)器人協(xié)同探測(cè)所獲得的地圖包含了全部的地圖信息。與單個(gè)移動(dòng)機(jī)器人構(gòu)建的地圖相比，多機(jī)器人系統(tǒng)能夠在保證地圖信息正確且完整的情況下，有效提高探測(cè)效率。

(a)機(jī)器人出發(fā)前的相對(duì)位置

(b)機(jī)器人0的子地圖

(c)機(jī)器人1的子地圖

(d)融合之后生成的全局地圖

(2)多機(jī)器人路徑規(guī)劃

本節(jié)首先基于stage平臺(tái)搭建了仿真環(huán)境，選取四臺(tái)虛擬機(jī)器人R1、R2、R3、R4，起點(diǎn)對(duì)應(yīng)A、B、F、G。對(duì)應(yīng)的目標(biāo)點(diǎn)為G1、G2、G3、G4。采用第三節(jié)所述的路徑多機(jī)規(guī)劃方法進(jìn)行仿真實(shí)驗(yàn)，規(guī)劃路徑如圖6(a)所示，分別為R1：A-B-C-E(紅色)；R2：B-C-D-C-G2(綠色)；R3：F-H(紫色)；R4：G-I(黃色)。其中，R1和R2具有相同的路徑段，即發(fā)生了路徑占用。各個(gè)機(jī)器人依據(jù)所規(guī)劃路徑向目標(biāo)行進(jìn)的過程如圖6(b)～圖6(d)所示。由于R3、R4的路徑無(wú)任何沖突占用問題，所規(guī)劃的路徑是駛向目標(biāo)點(diǎn)的最短路徑，兩者在77s時(shí)到達(dá)目標(biāo)點(diǎn)，即R3、R4的子系統(tǒng)用時(shí)為77s。R1與R2存在路徑?jīng)_突，此時(shí)R2到達(dá)C點(diǎn)。若此時(shí)R2直接到達(dá)目標(biāo)，則會(huì)導(dǎo)致C-G2段路徑堵塞，從而使得R1目標(biāo)不可到達(dá)，故使得R2機(jī)器人在周圍節(jié)點(diǎn)等待R1先行通過。如圖6(c)所示，第82s時(shí)R2行進(jìn)至D點(diǎn)等待，當(dāng)R1通過該路徑段后，R2沿D-C-G2段到達(dá)目標(biāo)點(diǎn)。在第188s時(shí)，機(jī)器人全部到達(dá)目標(biāo)點(diǎn)。系統(tǒng)用時(shí)為188s。仿真結(jié)果表明，該多機(jī)路徑規(guī)劃算法可以成功地為多個(gè)機(jī)器人規(guī)劃出可行駛的路徑，方法可以有效解決路徑?jīng)_突及堵塞問題。同時(shí)，由于需要解決沖突規(guī)避問題，算法效率受到了機(jī)器人數(shù)量、具體路徑占用種類等因素的影響。

(a)第0s

(b)第77s

(c)第82s

(d)第188s

為驗(yàn)證本文多機(jī)器人系統(tǒng)的實(shí)際路徑規(guī)劃能力，基于第三章多機(jī)路徑規(guī)劃流程進(jìn)行了實(shí)驗(yàn)，實(shí)驗(yàn)環(huán)境與相應(yīng)路徑搜索圖如圖7所示。本節(jié)使用三臺(tái)Pibot機(jī)器人進(jìn)行了實(shí)驗(yàn)，三臺(tái)機(jī)器人在圖7(b)中的白色字符自下至上可分別表示為robot_0、robot_1、robot_2。其中，藍(lán)色線段為路徑搜索圖中的可行駛路徑。

(a)實(shí)驗(yàn)場(chǎng)景

(b)對(duì)實(shí)驗(yàn)場(chǎng)景構(gòu)建的全局地圖和機(jī)器人的初始坐標(biāo)圖7 實(shí)驗(yàn)場(chǎng)景與路徑搜索圖 Fig.7 The experimental scene

多機(jī)路徑規(guī)劃過程如圖8所示。機(jī)器人的初始位置如圖8(a)所示。在上位機(jī)終端中，設(shè)定機(jī)器人各自目標(biāo)點(diǎn)為G0、G1、G2。robot_0、robot_1、robot_2所得路徑分別標(biāo)識(shí)為綠色(path0)、黃色(path1)、橙色(path2)。圖8(a)～圖8(d)是目標(biāo)點(diǎn)設(shè)定后，終端界面的實(shí)時(shí)位置顯示。圖8(e)～圖8(h)分別為機(jī)器人在某時(shí)刻的實(shí)際狀態(tài)圖。

(a)設(shè)置目標(biāo)點(diǎn)

(b)機(jī)器人1在等待

(c)機(jī)器人2在等待

(d)到達(dá)各自目的地

(e)機(jī)器人在出發(fā)點(diǎn)

(f)機(jī)器人1在等待

(g) 機(jī)器人2在等待

(h) 到達(dá)目的地

在圖8(a)中，path0與path1發(fā)生了部分重合，且robot_2目標(biāo)點(diǎn)G2處于path0上，這兩種情況均表明機(jī)器人路徑存在占用沖突。各機(jī)器人同時(shí)按照各自路徑行駛必然會(huì)導(dǎo)致機(jī)器人發(fā)生碰撞或堵塞，而從圖8(b)與圖8(f)可以看出，所采用的規(guī)劃方案使得robot_1在該位置等待robot_0優(yōu)先通行。在圖8(c)和圖8(g)中，robot_2等待robot_0優(yōu)先通行，從而使得機(jī)器人個(gè)體順利得到各個(gè)目標(biāo)位置，并且在行駛過程中不發(fā)生機(jī)器人與環(huán)境、機(jī)器人與機(jī)器人之間的碰撞。實(shí)驗(yàn)結(jié)果表明，所采用的多機(jī)器人路徑規(guī)劃方法可成功地為多個(gè)機(jī)器人規(guī)劃出路徑，達(dá)到了預(yù)期效果。

5 結(jié) 論

本文首先針對(duì)基于坐標(biāo)變換的地圖融合算法進(jìn)行了實(shí)驗(yàn)驗(yàn)證，有效提高了機(jī)器人系統(tǒng)針對(duì)未知環(huán)境地圖構(gòu)建的效率，在完成了全局地圖拼接的基礎(chǔ)上，對(duì)基于沖突規(guī)避的路徑規(guī)劃算法進(jìn)行了實(shí)驗(yàn)驗(yàn)證，主要研究了規(guī)劃流程和潛在沖突解決算法。在實(shí)驗(yàn)中，機(jī)器人系統(tǒng)順利完成了路徑規(guī)劃，解決了路徑規(guī)劃中潛在的路徑?jīng)_突和規(guī)劃死鎖的問題。在運(yùn)動(dòng)過程中，機(jī)器人規(guī)避了環(huán)境中的靜態(tài)障礙物，順利到達(dá)了各自的目的地?；谧鴺?biāo)變換的地圖融合算法可適用于倉(cāng)儲(chǔ)物流等應(yīng)用環(huán)境。

本文的研究還存在一些不足之處。首先，柵格地圖的特征信息相對(duì)匱乏。在后續(xù)的研究中，可以嘗試融合環(huán)境的圖像信息，利用柵格地圖表征障礙物分布和路徑信息，利用圖像特征進(jìn)行特征匹配和地圖擴(kuò)展；此外，本文使用的自主定位方式主要基于移動(dòng)機(jī)器人自身的傳感器來進(jìn)行匹配和定位，缺少相對(duì)于環(huán)境的絕對(duì)定位方式。在移動(dòng)機(jī)器人的動(dòng)態(tài)運(yùn)行過程中，往往會(huì)出現(xiàn)定位不及時(shí)或定位更新失敗的情況，進(jìn)而導(dǎo)致誤差的產(chǎn)生。在實(shí)際工程應(yīng)用中，可以嘗試加入無(wú)線電定位或關(guān)鍵點(diǎn)二維碼，以提高移動(dòng)機(jī)器人相對(duì)于環(huán)境的絕對(duì)定位精度。