基于海鷗優化改進采樣過程的RBPF-SLAM算法

施振穩，張志安，黃學功，華 洪，陳冠星

(南京理工大學 機械工程學院， 南京 210094)

1 引言

隨著科技水平的發展，移動機器人的智能化水平和自適應能力大大提升，如今在軍事領域、工業領域、航天飛行領域以及日常生活和社會的各個層面都有著廣泛的應用[1]。同時定位與建圖(simultaneous location and mapping，SLAM)是移動機器人研究的熱點問題，是機器人實現自主導航的關鍵環節。SLAM概念由文獻[2]于1991年正式提出，指載體在未知環境中通過自身裝備的傳感器不斷獲取周圍環境信息，構建周圍環境的增量式地圖，同時根據所構建的地圖估計其位姿。早期，研究人員主要使用擴展卡爾曼濾波器(extended kalman filter，EKF)和粒子濾波器(particle filtering，PF)解決SLAM問題。然而EKF和PF算法都因效率低、計算復雜度大，無法很好滿足SLAM實時性需求。為此，文獻[3]提出基于Rao-Blackwellize粒子濾波器(rao-blackwellized particle filter，RBPF)的FastSLAM算法，將SLAM問題分解成機器機器人定位問題和基于已知機器人位姿的建圖問題，解決了狀態空間維數過高的問題，從而大大減少了計算量。然而傳統的RBPF-SLAM方法必須用大量粒子才能很好地描述后驗概率密度分布，但是粒子數越多，算法復雜度越高，導致效率低下，而且多數粒子在經過多次迭代后，其權值將不斷減小，就會出現粒子退化問題。

針對傳統基于RBPF粒子濾波SLAM方法的不足，國內外不少學者進行過研究。文獻[4]基于FastSLAM提出一種名為Gmapping激光SLAM算法，該算法將高精度的激光測量數據加入到提議分布求取中，使提議分布更接近實際后驗分布，從而提高算法的效率。文獻[5-7]均嘗試利用群智能算法優化RBP-SLAM算法，群智能算法與RBPF-SLAM算法結合的研究方興未艾。海鷗優化算法(seagull optimization Algorithm，SOA)是文獻[8]于2019提出的一種新穎的群智能優化算法，它通過模擬海鷗的遷徙和攻擊行為來建立數學模型，可獲得比粒子群優化等目前其他常見同類型算法具有更優的性能。本文將其引入PBRF-SLAM中，在采樣過程中，每個粒子根據傳播模型得到多個輔助粒子，再利用SOA的全局探測能力得到最優值作為此次的真實傳播，從而避免優秀粒子因陷入局部極值而退化為噪聲很大的粒子的情況。

對于RBPF-SLAM中出現的粒子退化問題，傳統方法使用重要性重采樣方法(sampling importance resampling，SIR)重新生成采樣粒子，該方法盡管緩解了粒子退化問題，然而會破壞樣本多樣性。采樣方差(sampling variance，SV)可以度量粒子分布在重采樣前后的差異，從而可以評判重采樣算法對粒子多樣性的影響程度[9]?；诓蓸臃讲?，文獻[10]提出最小方差重采樣方法(minimum sampling variance，MSV)，該方法可使重采樣前后的采樣方差最小化，并保護權值較小但更接近真實值的粒子不被刪除，從而保證了重采樣前后粒子的多樣性。本文算法使用最小方差重采樣方法替換原先的重采樣方法并充分使用輔助粒子，進一步提高采樣后粒子的多樣性。

2 RBPF-SLAM算法

SLAM問題本質上是利用機器人獲取的傳感器數據去估計環境地圖m和機器人軌跡x1∶t的后驗概率p(x1∶t，m|z1∶t，u1∶t-1)的問題。在SLAM問題中有2個重要的數學模型：運動模型p(xt|zt-1，ut-1)與觀測模型p(zt|xt，m)。運動模型表示給定上一時刻機器人的位姿xt-1和控制命令ut-1，機器人獲得新位姿xt的概率密度；觀測模型表示給定機器人位姿xt和地圖m，傳感器獲得觀測值zt的概率密度。

RBPF-SLAM關鍵思想就是將p(x1∶t，m|z1∶t，u1∶t-1)分解成如式(1)所示的軌跡估計和地圖估計2個后驗概率的乘積[11]：

p(x1∶t，m|z1∶t，u1∶t-1)=p(x1∶t|z1∶t，u1∶t-1)·

p(m|x1∶t，z1∶t)

(1)

其中:z1∶t代表機器人從1時刻到t時刻傳感器獲得的觀測信息，u1∶t-1代表從1時刻到t-1時刻機器人的控制信息。

RBPF-SLAM算法的具體步驟如下：

2) 采樣過程。根據提議分布采樣粒子。一般將機器人的里程計運動模型和傳感器的觀測模型相結合作為提議分布π，根據上一代粒子集χt-1生成下一代粒子集χt。在具體實現中，這個過程由兩步實現，首先根據里程計運動模型獲得粒子新的位姿，然后根據觀測模型修正粒子的位姿。預測下一時刻的位置過程可以用下式表示：

(2)

(3)

4) 重采樣過程。粒子濾波的重采樣步驟是重要的一步，但是過于頻繁地重采樣，將導致粒子快速貧化，為此在RBPF-SLAM中通過有效采樣尺度標準來評判粒子集的優劣，確定何時進行重采樣步驟，公式如下：

(4)

3 優化的RBPF-SLAM算法

3.1 海鷗優化算法

海鷗優化算法是一種仿生智能優化算法，其優化思想來源于海鷗個體間的遷徙和攻擊過程。

1) 遷徙。在遷移過程中，該算法模擬了海鷗向一個位置移動到另一個位置移動過程。這個過程由3個步驟組成：避免碰撞、搜索者向最佳鄰居的方向移動、保持與最佳搜索者的接近[12]。

步驟1避免碰撞：避免海鷗個體之間的相互碰撞。引入一個變量A用于計算新的搜索位置：

(5)

(6)

其中： 引入fc變量控制變量A的值，變量A由fc線性減少到0。

步驟2搜索者向最佳鄰居的方向移動：在避免了鄰居之間的碰撞后，搜索者根據式(6)向最佳鄰居的方向移動：

(7)

B=2×A2×rd

(8)

其中rd為[0，1]之間的隨機數。

步驟3保持與最佳搜索者的接近：最后搜索者需要根據最佳搜索者更新其位置：

(9)

2) 攻擊。海鷗在遷徙過程中會不斷地改變攻擊的角度和速度，當攻擊獵物時，將進行空中的螺旋運動，該運動可描述為：

x′=r×cos(k)

(10)

y′=r×cos(k)

(11)

z′=r×k

(12)

r=u×ekv

(13)

其中：r是每一圈螺旋半徑的大小，k的范圍為0≤k≤2π。u和v是定義螺旋形狀的常數，e是自然對數底。

(14)

在海鷗優化算法中需要一個適應度函數計算每只海鷗個體的適應度值。在激光SLAM中可以根據機器人在全局標系下的估計位姿，將當前掃描的激光雷達數據點逐一映射到全局地圖中得到測量似然域，而后根據測量似然域和全局地圖的匹配程度來評價估計位姿的好壞[12]。將評價似然域和全局地圖的匹配程度的計算函數作為海鷗優化算法的適應度函數：

(15)

其中，S為匹配得分，bm,i表示激光束i在全局坐標系下所對應的柵格，mp,i為全局地圖中距bm,i最近的占用柵格，Gs表示高斯系數，Dis函數表示求2個柵格的距離，N表示激光雷達的總激光束數。

將海鷗優化算法應用到RBPF-SLAM的采樣過程中。為避免優秀粒子因采樣的隨機性退化為噪聲很大的粒子的情況，每個粒子從運動模型采樣時均采樣B個粒子，本文中稱這些粒子為輔助粒子。然后將這些輔助粒子作為海鷗個體，進行迭代尋優，將其中的最優值作為本次采樣的結果，這個過程可以用下式表示：

(16)

其中n為輔助粒子的序號。

3.2 優化重采樣策略

在傳統RBPF-SLAM重采樣過程中，采用輪盤賭法來選擇被保留的粒子，從而使得大權重的粒子更有可能被復制，小權重的粒子更有可能被去除。然而，這種根據權重隨機地復制和去除粒子的方式，容易導致粒子多樣性喪失，在SLAM過程中產生定位失真、回環時無法保證建圖一致性等現象。針對這一問題，使用最小方差重采樣方法來替換RBPF-SLAM中簡單、粗糙的重采樣策略，并且為了進一步提高粒子集的多樣性，在選擇粒子時根據重采樣計數選擇輔助粒子代替重復復制的粒子。本文使用的重采樣方法步驟如下：

步驟1為每個粒子i設置一個重采樣計數C[i]，并初始化為0。

C[i]=floor(Mω[i])

(17)

其中，floor函數表示向下取整，M為粒子總數：

(18)

步驟3計算所有粒子的重采樣計數總數：

(19)

步驟5根據每個粒子的重采樣計數C[i]，選擇匹配得分最高的C[i]個輔助粒子進行復制。

整體的算法流程如下：

Algorithm Optimal RBPF(χt-1, ut,zt)

fori=1 toMdo

//Generate samples from the transtion model

forn=1 toBdo

end for

end for

//calculate the importance weight

ωt← calWeight(xt,zt,mt-1)

//update particle set

ifNeff<1/2 then

χt← MSV resample(χt)

end if

//Update map

mt← registerScan(mt-1,xt,zt)

4 仿真實驗

為了驗證優化算法的有效性，采用Gmapping算法作為對照進行仿真實驗，算法來源于https//svn.open—slam.org[13]。仿真實驗所用的計算機，處理器為英特爾酷睿i7-10750H，6核，主頻為2.6 GHz，內存為16 GB，使用Ubuntu16.04操作系統。公開數據選用集Intel Research Lab和ACES Building數據集，這兩者均以Carmen格式保存了機器人運行一定時間內的激光雷達和里程的全部測量數據以及相應的時間戳。為了使得實驗結果具有可對比性，使用同一個數據集時，算法的所有參數的設置都相同。通過比較兩種算法的定位效果和所建地圖與基準地圖的差異程度來評價兩種算法的優劣。對于定位效果的定量對比，本文使用文獻[14]提供評估工具metricEvaluator，該工具可通過輸入SLAM算法得到機器人的軌跡信息機器人真實軌跡信息(Ground Truth)來計算總體的定位誤差，定誤差的計算公式如下：

(20)

圖1采用的是Intel Research Lab數據集，該數據集為經典的室內環境，環境大小約為28 m×28 m，內部有較多的特征。由仿真結果可知，在5個粒子的情況下，2種算法均能構建出較好的地圖，但可以看到Gmapping算法在黑框處出現明顯的重影現象，且整體的定位誤差較大[15]。

圖1 Intel Research Lab 數據集仿真實驗結果圖Fig.1 Simulation experiment results of Intel Research Lab

圖2采用的是ACES Building數據集，該數據集為走廊環境，環境大小約為50 m×50 m，環境規律整潔但是特征較少，且機器人運動的軌跡中構成較多的回環，通過多次實驗，不斷逐漸增加粒子數，由仿真結果可知，在20個粒子的情況下，本文算法已經可以構建出一致性較好地圖，Gmapping算法所構建的地圖仍有多處出現地圖不一致的情況。

圖2 ACES Building數據集仿真實驗結果圖Fig.2 Simulation experiment resultsofACES Building

表1記錄了用metricEvaluator評估工具計算的2次仿真結果的定位誤差。定位誤差由平移誤差和旋轉誤差兩部分組成，計算工具的計算結果包含了誤差的平均值和標準差。從計算結果可以看出對于2個數據集本文算法的定位效果均有明顯提升。其中Intel Research Lab數據集總體平移誤差的平均值減少了36.36%，總體旋轉誤差的平均值減少了33.33%。ACES Building數據集總體平移誤差的平均值減少了41.67%，總體旋轉誤差的平均值減少了40%。

表1 定位誤差

5 實體測試

圖3是本文算法的實驗平臺，該平臺采用四輪差分驅動，并搭載有4個光電編碼器和思嵐A1激光雷達。實驗環境為某科技園辦公樓的日字形的走廊，部分實驗環境如圖4所示。

圖3 實驗平臺圖Fig.3 The experimental platform

圖4 部分實驗環境圖Fig.4 Part of the experimental environment

對于實際的機器人實驗，由于難以采集到機器人真實軌跡信息，因此不能通過定量計算誤差來衡量地圖準確度。雖然單單通過直觀地對比構建地圖的方法具有一定的局限性，但是通過對比用相同粒子數目生成的環境地圖與真實環境的相似程度，還是可以定性地評判算法的優劣。如圖5所示，同為30個粒子的情況下，本文算法所構建的地圖于實際的建筑的一致性更好，并且沒有因為回環時粒子多樣性不足而出現明顯的重影的現象。

圖5 生成的柵格地圖Fig.5 TheComparison of generated grid map

6 結論

1) 在傳統RBPF-SLAM算法基礎上，利用海鷗優化算法改善RBPF的粒子采樣過程，從避免了優秀粒子退化為噪聲很大的粒子的情況。

2) 優化后的RBPF-SLAM取得了更好定位和地圖構建的效果，相比于Gmapping算法，總體的平移誤差的平均值分別降低了36.36%和41.67%，總體的旋轉誤差的平均值分別降低了33.33%和40%。

3) 室內結果表明優化后的算法能夠完成構建地圖的任務且能獲得與實際環境一致性更好的地圖。