改進粒子群優化算法的多機器人地圖構建

張 航 姚敏茹 曹 凱 高 嵩

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

0 引 言

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

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

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

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

1 地圖配準

1.1 問題描述

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

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

(1)

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

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

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

(2)

(3)

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

(4)

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

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

(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 粒子群優化算法

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

(6)

(7)

粒子群優化算法的具體流程如圖1所示。

圖1 粒子群優化算法流程圖

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

1.3 適應度函數

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

(8)

(9)

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

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

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

(10)

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

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

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

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

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

圖3 Gazebo仿真場景

(a) TB0構建的地圖 (b) TB1構建的地圖

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

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

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

表1 地圖融合結果及誤差

續表1

圖5 地圖m1和m2的融合結果

圖6 地圖m2和m3的融合結果

為了驗證實驗的可靠性，又分別對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在不同區間出現的次數，實驗結果如表2所示。雖然在Qs∈[10%,30%]時，SA-PSO算法重新進行地圖配準的概率較大，但Qs∈[10%,30%]通常是由于構建的局部地圖質量較差導致，重新進行地圖配準也很難改善Qi。在Qs∈[70%,90%]時，算法重新進行地圖配準的概率較小，通常得到的Qs=Qi，最終SA-PSO算法在Qs∈[30%,70%]適用度較好。

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

2.2 機器人實驗

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

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

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

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

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

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

3 結 語

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