施振穩(wěn)，張志安，黃學(xué)功，華 洪，陳冠星

(南京理工大學(xué) 機(jī)械工程學(xué)院， 南京 210094)

1 引言

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

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

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

2 RBPF-SLAM算法

SLAM問(wèn)題本質(zhì)上是利用機(jī)器人獲取的傳感器數(shù)據(jù)去估計(jì)環(huán)境地圖m和機(jī)器人軌跡x1∶t的后驗(yàn)概率p(x1∶t，m|z1∶t，u1∶t-1)的問(wèn)題。在SLAM問(wèn)題中有2個(gè)重要的數(shù)學(xué)模型：運(yùn)動(dòng)模型p(xt|zt-1，ut-1)與觀測(cè)模型p(zt|xt，m)。運(yùn)動(dòng)模型表示給定上一時(shí)刻機(jī)器人的位姿xt-1和控制命令ut-1，機(jī)器人獲得新位姿xt的概率密度；觀測(cè)模型表示給定機(jī)器人位姿xt和地圖m，傳感器獲得觀測(cè)值z(mì)t的概率密度。

RBPF-SLAM關(guān)鍵思想就是將p(x1∶t，m|z1∶t，u1∶t-1)分解成如式(1)所示的軌跡估計(jì)和地圖估計(jì)2個(gè)后驗(yàn)概率的乘積[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代表機(jī)器人從1時(shí)刻到t時(shí)刻傳感器獲得的觀測(cè)信息，u1∶t-1代表從1時(shí)刻到t-1時(shí)刻機(jī)器人的控制信息。

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

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

(2)

(3)

4) 重采樣過(guò)程。粒子濾波的重采樣步驟是重要的一步，但是過(guò)于頻繁地重采樣，將導(dǎo)致粒子快速貧化，為此在RBPF-SLAM中通過(guò)有效采樣尺度標(biāo)準(zhǔn)來(lái)評(píng)判粒子集的優(yōu)劣，確定何時(shí)進(jìn)行重采樣步驟，公式如下：

(4)

3 優(yōu)化的RBPF-SLAM算法

3.1 海鷗優(yōu)化算法

海鷗優(yōu)化算法是一種仿生智能優(yōu)化算法，其優(yōu)化思想來(lái)源于海鷗個(gè)體間的遷徙和攻擊過(guò)程。

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

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

(5)

(6)

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

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

(7)

B=2×A2×rd

(8)

其中rd為[0，1]之間的隨機(jī)數(shù)。

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

(9)

2) 攻擊。海鷗在遷徙過(guò)程中會(huì)不斷地改變攻擊的角度和速度，當(dāng)攻擊獵物時(shí)，將進(jìn)行空中的螺旋運(yùn)動(dòng)，該運(yùn)動(dòng)可描述為：

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是定義螺旋形狀的常數(shù)，e是自然對(duì)數(shù)底。

(14)

在海鷗優(yōu)化算法中需要一個(gè)適應(yīng)度函數(shù)計(jì)算每只海鷗個(gè)體的適應(yīng)度值。在激光SLAM中可以根據(jù)機(jī)器人在全局標(biāo)系下的估計(jì)位姿，將當(dāng)前掃描的激光雷達(dá)數(shù)據(jù)點(diǎn)逐一映射到全局地圖中得到測(cè)量似然域，而后根據(jù)測(cè)量似然域和全局地圖的匹配程度來(lái)評(píng)價(jià)估計(jì)位姿的好壞[12]。將評(píng)價(jià)似然域和全局地圖的匹配程度的計(jì)算函數(shù)作為海鷗優(yōu)化算法的適應(yīng)度函數(shù)：

(15)

其中，S為匹配得分，bm,i表示激光束i在全局坐標(biāo)系下所對(duì)應(yīng)的柵格，mp,i為全局地圖中距bm,i最近的占用柵格，Gs表示高斯系數(shù)，Dis函數(shù)表示求2個(gè)柵格的距離，N表示激光雷達(dá)的總激光束數(shù)。

將海鷗優(yōu)化算法應(yīng)用到RBPF-SLAM的采樣過(guò)程中。為避免優(yōu)秀粒子因采樣的隨機(jī)性退化為噪聲很大的粒子的情況，每個(gè)粒子從運(yùn)動(dòng)模型采樣時(shí)均采樣B個(gè)粒子，本文中稱這些粒子為輔助粒子。然后將這些輔助粒子作為海鷗個(gè)體，進(jìn)行迭代尋優(yōu)，將其中的最優(yōu)值作為本次采樣的結(jié)果，這個(gè)過(guò)程可以用下式表示：

(16)

其中n為輔助粒子的序號(hào)。

3.2 優(yōu)化重采樣策略

在傳統(tǒng)RBPF-SLAM重采樣過(guò)程中，采用輪盤賭法來(lái)選擇被保留的粒子，從而使得大權(quán)重的粒子更有可能被復(fù)制，小權(quán)重的粒子更有可能被去除。然而，這種根據(jù)權(quán)重隨機(jī)地復(fù)制和去除粒子的方式，容易導(dǎo)致粒子多樣性喪失，在SLAM過(guò)程中產(chǎn)生定位失真、回環(huán)時(shí)無(wú)法保證建圖一致性等現(xiàn)象。針對(duì)這一問(wèn)題，使用最小方差重采樣方法來(lái)替換RBPF-SLAM中簡(jiǎn)單、粗糙的重采樣策略，并且為了進(jìn)一步提高粒子集的多樣性，在選擇粒子時(shí)根據(jù)重采樣計(jì)數(shù)選擇輔助粒子代替重復(fù)復(fù)制的粒子。本文使用的重采樣方法步驟如下：

步驟1為每個(gè)粒子i設(shè)置一個(gè)重采樣計(jì)數(shù)C[i]，并初始化為0。

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

(17)

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

(18)

步驟3計(jì)算所有粒子的重采樣計(jì)數(shù)總數(shù)：

(19)

步驟5根據(jù)每個(gè)粒子的重采樣計(jì)數(shù)C[i]，選擇匹配得分最高的C[i]個(gè)輔助粒子進(jìn)行復(fù)制。

整體的算法流程如下：

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 仿真實(shí)驗(yàn)

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

(20)

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

圖1 Intel Research Lab 數(shù)據(jù)集仿真實(shí)驗(yàn)結(jié)果圖Fig.1 Simulation experiment results of Intel Research Lab

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

圖2 ACES Building數(shù)據(jù)集仿真實(shí)驗(yàn)結(jié)果圖Fig.2 Simulation experiment resultsofACES Building

表1記錄了用metricEvaluator評(píng)估工具計(jì)算的2次仿真結(jié)果的定位誤差。定位誤差由平移誤差和旋轉(zhuǎn)誤差兩部分組成，計(jì)算工具的計(jì)算結(jié)果包含了誤差的平均值和標(biāo)準(zhǔn)差。從計(jì)算結(jié)果可以看出對(duì)于2個(gè)數(shù)據(jù)集本文算法的定位效果均有明顯提升。其中Intel Research Lab數(shù)據(jù)集總體平移誤差的平均值減少了36.36%，總體旋轉(zhuǎn)誤差的平均值減少了33.33%。ACES Building數(shù)據(jù)集總體平移誤差的平均值減少了41.67%，總體旋轉(zhuǎn)誤差的平均值減少了40%。

表1 定位誤差

5 實(shí)體測(cè)試

圖3是本文算法的實(shí)驗(yàn)平臺(tái)，該平臺(tái)采用四輪差分驅(qū)動(dòng)，并搭載有4個(gè)光電編碼器和思嵐A1激光雷達(dá)。實(shí)驗(yàn)環(huán)境為某科技園辦公樓的日字形的走廊，部分實(shí)驗(yàn)環(huán)境如圖4所示。

圖3 實(shí)驗(yàn)平臺(tái)圖Fig.3 The experimental platform

圖4 部分實(shí)驗(yàn)環(huán)境圖Fig.4 Part of the experimental environment

對(duì)于實(shí)際的機(jī)器人實(shí)驗(yàn)，由于難以采集到機(jī)器人真實(shí)軌跡信息，因此不能通過(guò)定量計(jì)算誤差來(lái)衡量地圖準(zhǔn)確度。雖然單單通過(guò)直觀地對(duì)比構(gòu)建地圖的方法具有一定的局限性，但是通過(guò)對(duì)比用相同粒子數(shù)目生成的環(huán)境地圖與真實(shí)環(huán)境的相似程度，還是可以定性地評(píng)判算法的優(yōu)劣。如圖5所示，同為30個(gè)粒子的情況下，本文算法所構(gòu)建的地圖于實(shí)際的建筑的一致性更好，并且沒(méi)有因?yàn)榛丨h(huán)時(shí)粒子多樣性不足而出現(xiàn)明顯的重影的現(xiàn)象。

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

6 結(jié)論

1) 在傳統(tǒng)RBPF-SLAM算法基礎(chǔ)上，利用海鷗優(yōu)化算法改善RBPF的粒子采樣過(guò)程，從避免了優(yōu)秀粒子退化為噪聲很大的粒子的情況。

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

3) 室內(nèi)結(jié)果表明優(yōu)化后的算法能夠完成構(gòu)建地圖的任務(wù)且能獲得與實(shí)際環(huán)境一致性更好的地圖。