張 航 姚敏茹 曹 凱 高 嵩

(西安工業(yè)大學電子信息工程學院 陜西 西安 710021)

0 引 言

隨著工業(yè)發(fā)展與人們生活水平的提高，機器人的工作環(huán)境與任務也變得更加復雜和多樣。盡管單個機器人的性能和工作效率隨著技術的發(fā)展得到很大提高，但仍存在很多單個機器人無法解決的問題，故多機器人控制技術逐漸得到廣大學者的重視。與單個機器人相比，多機器人系統(tǒng)具有以下優(yōu)勢：多機器人系統(tǒng)中各個機器人并行工作，能夠在更短的時間完成任務；在某些機器人出現(xiàn)故障無法正常工作時，則由其余的機器人代替，提高了系統(tǒng)的魯棒性；多機器人系統(tǒng)可以攜帶更多的傳感器，采集更加豐富的環(huán)境信息，提高環(huán)境檢測結(jié)果的準確率。目前，利用機器人進行資源探測，災區(qū)搜救等越來越多的實際應用都需要使用環(huán)境的地圖模型，針對單個機器人構(gòu)建的環(huán)境地圖范圍小、效率低、難以滿足大范圍環(huán)境地圖構(gòu)建的問題，利用多機器人系統(tǒng)高效地構(gòu)建地圖[1-3]逐漸成為新的研究熱點。多機器人構(gòu)建環(huán)境地圖是在單個機器人分別繪制不同區(qū)域局部地圖的基礎上，綜合各個機器人建立的局部地圖，將局部地圖融合成環(huán)境的全局地圖的方法。該方法的核心步驟是局部地圖間的地圖配準，用來進行局部地圖之間的數(shù)據(jù)關聯(lián)，根據(jù)地圖配準得到的局部地圖間的轉(zhuǎn)換矩陣實現(xiàn)地圖融合，而地圖配準[4]又分為直接法和間接法兩類。

直接法是利用機器人之間位姿的相對觀測數(shù)據(jù)，來求解局部地圖坐標系之間的轉(zhuǎn)換關系。文獻[5]詳細介紹了直接地圖配準的具體過程，即各個不同的坐標系之間的幾何約束關系，機器人利用自身攜帶的傳感器進行相互測量，采用一定的數(shù)據(jù)處理手段實現(xiàn)地圖配準。文獻[6]對上述方法進行了改進，提高了局部地圖的轉(zhuǎn)換精度。直接法的優(yōu)點在于簡單易行，計算復雜度相對較低，但是地圖配準精度過于依賴傳感器測量精度，且要求機器人之間至少有一次相遇，在實際應用過程中可行性不高。

間接法是指利用數(shù)據(jù)關聯(lián)算法識別出局部地圖之間的重疊區(qū)域的公共特征點或是地圖頻譜特征，進一步求解局部地圖之間的對應關系，從而完成地圖配準。文獻[7]研究一種迭代最近鄰點算法進行地圖融合，并利用相對熵濾波器來整合地圖間的差異。文獻[8]提出一種成對一致性最大化(PCM)算法，能夠找到局部地圖間最大匹配一致集，該方法在真實數(shù)據(jù)集上的性能優(yōu)越，但需要采集的數(shù)據(jù)信息過多，不易于實施。文獻[9]主要分析了基于柵格地圖表征方式的特征匹配方法，提出了基于自適應隨機漫步規(guī)劃算法[12]，以一種隨機搜索算法，搜索出局部地圖之間最大的重合部分，實現(xiàn)了精確的柵格地圖匹配。文獻[10]提出了一種新型的計算柵格地圖轉(zhuǎn)換矩陣的方法，主要包括圖像分割，數(shù)據(jù)關聯(lián)以及轉(zhuǎn)換矩陣的近似等步驟。文獻[11]在文獻[10]的基礎上加入了一種基于神經(jīng)網(wǎng)絡和自組織地圖的地圖學習步驟，對地圖的重疊區(qū)域進行聚類分析，再對聚類項進行匹配，從而得到地圖間的轉(zhuǎn)換矩陣。文獻[13]設計了一套MAP-API框架，保證了地圖融合時地圖數(shù)據(jù)的一致性，增強了系統(tǒng)的魯棒性。文獻[14]提出了一種基于正弦圖的PSO算法，能夠有效解決由于缺少初始相對位姿引起的算法陷入局部最小問題，實現(xiàn)更為準確的地圖融合。間接進行地圖配準的方法依賴于可靠的數(shù)據(jù)關聯(lián)方法，一般精度較高，但計算復雜度會隨著搜索空間的增大而迅速上升。

綜上所述，若機器人的初始相對位置已知，則在地圖融合前即可獲得各子地圖間的轉(zhuǎn)換矩陣；若各個子地圖間存在共同地標，則利用地標即可實現(xiàn)地圖間的轉(zhuǎn)換；若機器人間能夠?qū)崿F(xiàn)相互觀測，則可以通過計算機器人間的相對距離和方向求解轉(zhuǎn)換矩陣。但在大多數(shù)實際場景下，上述信息通常是未知的，獲取轉(zhuǎn)換矩陣的唯一方法為匹配局部地圖的重疊區(qū)域或外觀[15]。文獻[14，16]利用PSO算法計算精確的轉(zhuǎn)換矩陣，但通常由于重疊區(qū)域較小或算法過早收斂導致地圖拼接錯位。為了解決這一問題，本文研究了一種基于SA-PSO算法的多機器人構(gòu)建地圖方法，該方法通過準確地找到局部地圖之間的最優(yōu)轉(zhuǎn)換矩陣來進行地圖融合。首先，每個機器人負責建立不同區(qū)域的局部地圖，根據(jù)局部地圖之間重疊區(qū)域的相似度設計適應度函數(shù)Z(x,y)；其次，利用粒子群優(yōu)化算法搜索最優(yōu)的旋轉(zhuǎn)和平移矩陣，使得局部地圖間能夠獲得最大重疊區(qū)域；最后，設計自適應概率函數(shù)Q(x,y)對局部地圖進行重新配準。

1 地圖配準

1.1 問題描述

對于非結(jié)構(gòu)環(huán)境或是一些難以提取特征的復雜環(huán)境，通常用柵格地圖來表示。當機器人構(gòu)建柵格地圖時，環(huán)境被劃分為相同分辨率的二維柵格，計算機把地圖中的柵格以矩陣的形式儲存，每個矩陣元素對應一個柵格，柵格中的參數(shù)表示其存在障礙物的可能性。將柵格地圖離散化為N行、M列的矩陣。用函數(shù)表示為：

m:[0,N]×[0,M]→R

(1)

式中：函數(shù)m表示該地圖的置信度模型，m(x,y)表示地圖(x,y)處的柵格被障礙物占據(jù)的可能性。例如m(x,y)=1表示柵格(x,y)處有障礙物，m(x,y)=0則剛好相反。

在地圖融合的過程中，需要利用適當?shù)霓D(zhuǎn)換函數(shù)對局部地圖進行平移和旋轉(zhuǎn)。假設tx、ty和θ三個參數(shù)分別代表向x軸方向平移，y軸方向平移和旋轉(zhuǎn)角度，可將該轉(zhuǎn)換矩陣定義為：

TRtx,ty,θ(x,y):R2→R2

(2)

(3)

以兩個機器人為例，將協(xié)作建圖問題描述為：機器人R1和R2從不同的位置出發(fā)，分別對環(huán)境進行探索并建立兩個局部柵格地圖m1和m2。讓m1保持靜止，m2根據(jù)不同的轉(zhuǎn)換矩陣Ttx,ty,q(x,y)進行平移和旋轉(zhuǎn)，直到兩個局部地圖的重疊區(qū)域達到最大。兩個局部地圖的重疊區(qū)域被定義為：

(4)

式中：m1[i,j]=m2[i,j]時，Eq(m1[i,j],m2[i,j])=1，否則Eq(m1[i,j],m2[i,j])=0，函數(shù)Z(x,y)表示兩幅地圖重疊區(qū)域的相似程度。最終地圖融合問題被定義為：給定局部地圖m1∈IN,M、m2∈IN,M，確定Ttx,ty,q(x,y)中的參數(shù){tx,ty,θ}使重疊函數(shù)Z(m1,Ttx,ty,θ(m2))達到最大值。

當機器人的個數(shù)n>2時，需要對以上的函數(shù)進行拓展：

(5)

同理，當m1[i,j]=m2[i,j]=…=mk[i,j]時，Eq(m1[i,j],m2[i,j],…,mk[i,j])=1, 否則Eq(m1[i,j],m2[i,j], …,mk[i,j])=0。

1.2 粒子群優(yōu)化算法

粒子群優(yōu)化算法[17-19]是基于種群的一種演化算法，根據(jù)個體對環(huán)境的適應度來決定個體的移動方向。與其他演化算法不同的是，粒子群優(yōu)化算法不需要對每個個體使用演化算子，而是把個體當作沒有體積和質(zhì)量的粒子，并給每個粒子賦予初始位置和速度。每個粒子會在搜索空間以一定的速度飛行，假設第i個粒子表示為Xi(xi1,xi2,…，xin)，其中n為未知變量的個數(shù)。它所經(jīng)歷的最優(yōu)位置(即適應度最高的位置)記為Pi(pi1,pi2,…，pin)，也稱為pbest，另一個是整個粒子群體經(jīng)歷過的最好位置gbest。第i個粒子的速度用Vi(vi1,vi2,…，vin)來表示，則n維變量根據(jù)以下的公式進行迭代：

(6)

(7)

粒子群優(yōu)化算法的具體流程如圖1所示。

圖1 粒子群優(yōu)化算法流程圖

粒子按照迭代公式與算法流程圖描述的方式從不同的位置開始搜索，由各個粒子最佳位置的參數(shù)來確定Ttx,ty,θ(x,y)中參數(shù){tx,ty,θ}的最佳賦值。慣性權(quán)重ωt的引入大大提高了粒子群優(yōu)化算法的搜索效率和質(zhì)量，增強了算法跳出局部最優(yōu)進行全局尋優(yōu)的能力。

1.3 適應度函數(shù)

適應度函數(shù)通常選取待優(yōu)化的目標函數(shù)，本文采用地圖重疊區(qū)域的相似度Z(x,y)作為尋優(yōu)函數(shù)。假設兩幅局部地圖的地圖矩陣為m1和m2，m1和m2之間的相似度函數(shù)Z(m1,m2)表示為：

(8)

(9)

式中：c表示地圖m1和m2中柵格設定的參數(shù)值，m1[p]表示地圖m1中位置p=(x,y)處的參數(shù)值為c。例如m(x,y)=1表示柵格(x,y)處有障礙物，m(x,y)=0則剛好相反，m(x,y)=-1表示(x,y)處為未知區(qū)域。md(p1,p2)表示點p1和點p2之間的曼哈頓距離[19]。numc(m1)表示地圖m1中參數(shù)值為c的柵格個數(shù)。

計算地圖的相似度函數(shù)Z(x,y)核心在于計算d-mapc，d-mapc就是地圖中的點p1=(x1,y1)到最近的參數(shù)值為c的點p2=(x2,y2)曼哈頓距離[20-21]組成的矩陣：

d-mapc[x1][y1]=min{md(p1,p2)|m2[p2]=c}

(10)

圖2(a)為Gazebo仿真平臺搭建的場景地圖。圖2(b)表示機器人建立的柵格地圖，柵格參數(shù)由矩陣T1表示。圖2(c)表示柵格參數(shù)c為1的d-mapc地圖，柵格參數(shù)由矩陣Tdmap表示。

(a) 場景地圖 (b) 柵格地圖 (c) d-mapc圖2 構(gòu)建d-mapc

假設a1、a2分別是地圖m1和m2中相似的重疊區(qū)域，由于機器人在采集數(shù)據(jù)的過程中受到噪聲的干擾，導致單個機器人建立局部地圖的效果也各有差異，因此當PSO算法得到最優(yōu)參數(shù){tx,ty,θ}時，a1、a2之間的圖像距離通常是近似為0或大于0。為了解決由于算法過早收斂引起的誤差，設計函數(shù)逐一對比a1、a2中相同位置p=(x,y)處的參數(shù)值：

sam(m1,m2)=num{p=(x,y)|m1[p]=m2[p]∈C}

(11)

dif(m1,m2)=num{p=(x,y)|m1[p]≠m2[p]∈C}

(12)

2 實驗與仿真

2.1 算法仿真實驗

本文的所有仿真實驗是在Linux操作系統(tǒng)下的物理仿真平臺Gazebo上完成的。仿真過程采用的機器人模型是Turtlebot3 Waffle，Turtlebot3是一個低成本，可編程的基于ROS的移動機器人。每一臺Turtlebot3可獨自通過ROS提供的gmapping功能包來建立自己所在周圍環(huán)境的局部地圖，該功能包提供包括對激光雷達的SLAM，根據(jù)激光雷達的輸入及姿態(tài)數(shù)據(jù)建立一個基于柵格的2D地圖。假設有兩臺Turtlebot3分別建立了局部地圖m1和m2，以Gazebo中的世界中心作為世界坐標系的坐標原點，確定了m1和m2初始位置與方向。將m2相對于世界坐標系的初始位姿按每逆時針3°旋轉(zhuǎn)進行配準，以及局部地圖原點的位置按照(+50,0,-50)進行平移，共有120×23=960種可能，利用粒子群優(yōu)化算法從中找出相似度高的重疊區(qū)域，進行地圖合并。所有的地圖都由397×397的矩陣來表示，每個柵格都有對應的參數(shù)值，如果這個柵格未被機器人檢索，則參數(shù)值被設置為-1，使用相同大小的矩陣來表示地圖大幅縮短了算法運行的時間。

如圖3所示，該場景是在Gazebo室內(nèi)場景，共有3個房間，每個房間都有一臺Turtlebot3機器人在單獨進行SLAM(機器人由上往下依次為：TB0,TB1,TB2)。Turtlebot3建立的局部地圖的初始角度各不相同，通過SA-PSO算法對圖中待拼接的柵格地圖進行實驗，得到的實驗結(jié)果如圖4所示。

圖3 Gazebo仿真場景

(a) TB0構(gòu)建的地圖 (b) TB1構(gòu)建的地圖

(c) TB2構(gòu)建的地圖 (d) PSO算法過早收斂

(e) SA-PSO算法進行地圖配準圖4 多機器人構(gòu)建地圖仿真結(jié)果

在仿真實驗中，為了驗證加入概率函數(shù)對地圖融合算法的影響，對實驗中2組地圖分別進行實驗，考慮到機器人建立局部地圖的誤差，設定Qs(x,y)達到90%屬于地圖融合成功案例。仿真結(jié)果如表1所示，當Qs(m1,m2)為64.09%時，SA-PSO算法重新進行地圖配準，使得Qi(m1,m2)達到91.35%。同理，Qs(m2,m3)為44.73%時，Qi(m2,m3)達到89.53%；相反，Qs(x,y)在75%至90%之間時，SA-PSO算法跳出局部最優(yōu)解的能力減弱，很難改善地圖融合效果。例如，當Qs(m1,m2)為83.64%以及Qs(m2,m3)為81.16%時，由于此時SA-PSO算法重新進行地圖配準的概率較小，得到的Qi(m1,m2)和Qi(m2,m3)不發(fā)生改變。此外，當各地圖之間沒有足夠的重疊區(qū)域時，Qs(x,y)很難達到設定的成功率。例如Qs(m2,m3)為68.34%時，由于重疊區(qū)域較小，SA-PSO算法重新進行地圖配準后的Qi(m2,m3)僅為69.82%。圖5和圖6為地圖融合結(jié)果分析圖。(表1中Qi(x,y)為SA-PSO算法搜索重疊區(qū)域的柵格匹配成功率，e與ei分別是Qs(x,y)與設定成功率90%間的誤差比較以及Qi(x,y)與設定成功率90%間的誤差比較)。

表1 地圖融合結(jié)果及誤差

續(xù)表1

圖5 地圖m1和m2的融合結(jié)果

圖6 地圖m2和m3的融合結(jié)果

為了驗證實驗的可靠性，又分別對3組地圖進行100次實驗。同樣假定Qs(x,y)達到90%屬于地圖融合成功案例，將地圖融合失敗案例分為Qs∈[10%,30%]、Qs∈[30%,50%]、Qs∈[50%,70%]、Qs∈[70%,90%]、Qs∈[90%,100%]幾種情況，分析PSO算法和SA-PSO算法在300次實驗中得到的Qs在不同區(qū)間出現(xiàn)的次數(shù)，實驗結(jié)果如表2所示。雖然在Qs∈[10%,30%]時，SA-PSO算法重新進行地圖配準的概率較大，但Qs∈[10%,30%]通常是由于構(gòu)建的局部地圖質(zhì)量較差導致，重新進行地圖配準也很難改善Qi。在Qs∈[70%,90%]時，算法重新進行地圖配準的概率較小，通常得到的Qs=Qi，最終SA-PSO算法在Qs∈[30%,70%]適用度較好。

表2 地圖融合成功率分析表

2.2 機器人實驗

在實驗室場景下使用Turtlebot3機器人，機器人之間利用ROS中的節(jié)點進行通信，所有機器人完成對應區(qū)域的地圖構(gòu)建后，將局部地圖數(shù)據(jù)發(fā)送給被設置為節(jié)點管理器的PC，統(tǒng)一進行地圖融合。場景1為地面移動機器人控制研究實驗室，實驗室中間為障礙物區(qū)域；場景2為水下機器人控制研究實驗室，中間的長方形水池為水下機器人運動控制平臺；場景3為學術交流會議室和走廊，場景1，場景2和場景3分別如圖7所示。

(a) 實際場景1與機器人構(gòu)建的地圖

(b) 實際場景2與機器人構(gòu)建的地圖

(c) 實際場景3與機器人構(gòu)建的地圖

(d) PSO算法融合后的地圖 (e) SA-PSO算法融合后的地圖圖7 實際場景下的實驗結(jié)果

場景1和場景2中的Turtlebot3機器人分別作為從機器人，構(gòu)建當前場景的局部地圖，并利用ROS話題通信機制將局部地圖數(shù)據(jù)回傳給場景3的主機器人。在主機器人的控制PC上分別執(zhí)行兩種算法進行地圖融合，PSO算法的地圖融合結(jié)果如圖7(d)所示，Qs為69.37%。在此基礎上，SA-PSO算法跳出局部最優(yōu)重新進行地圖融合，結(jié)果如圖7(e)所示，Qi為91.04%。需要注意的是，在實驗過程中機器人的移動速度不能過快，否則會導致構(gòu)建柵格地圖精度不高。為了防止走廊的場景過于單一導致的局部地圖建立產(chǎn)生誤差，在走廊及實驗室的角落人為放置一些圓柱體，以提高SLAM建圖中特征匹配的精度。

3 結(jié) 語

本文提出了一種基于SA-PSO算法的多機器人構(gòu)建地圖方法，利用地圖重疊區(qū)域匹配問題建立數(shù)學模型，求解出適應度函數(shù)最大時的局部地圖間轉(zhuǎn)換矩陣，進行局部地圖的配準。同時還設計了自適應概率函數(shù)，用來解決優(yōu)化算法得到的解為局部最優(yōu)解的問題，提高了地圖融合的成功率。本文分別在Gazebo平臺和實際物理環(huán)境中使用Turtlebot3機器人進行實驗。實驗結(jié)果表明，該方法可以用于大規(guī)模環(huán)境柵格地圖建立，將多幅局部柵格地圖融合成全局柵格地圖，提高了構(gòu)建地圖的實時性與可靠性。本文主要研究了多機器人的二維柵格地圖融合問題，未來主要研究如何在融合后地圖上實現(xiàn)多機器人的編隊，以及在完善本文方法的基礎上著重研究三維點云地圖的拼接問題。