袁 帥，劉同健，欒方軍，張 鳳，吳 健

（1.沈陽(yáng)建筑大學(xué) 電氣與控制工程學(xué)院，沈陽(yáng) 110168；2.沈陽(yáng)建筑大學(xué) 計(jì)算機(jī)科學(xué)與工程學(xué)院，沈陽(yáng) 110168）

自主導(dǎo)航是移動(dòng)機(jī)器人的關(guān)鍵技術(shù)，同時(shí)定位與建圖(Simultaneous Localization and Mapping,SLAM）用于有效建立環(huán)境地圖與描述機(jī)器人位姿狀態(tài)，是移動(dòng)機(jī)器人自主導(dǎo)航的前提[1]。粒子濾波（Particle Filter,PF）SLAM 算法解決在完全未知環(huán)境下移動(dòng)機(jī)器人SLAM 問(wèn)題。由于PF-SLAM 算法迭代會(huì)出現(xiàn)粒子權(quán)重衰退現(xiàn)象，采用重采樣的優(yōu)勝劣汰方法改善該問(wèn)題后，又導(dǎo)致了粒子多樣性喪失問(wèn)題[2]。此外傳統(tǒng)SLAM方法都是基于觀測(cè)噪聲已知的前提，若此前提不成立，即觀測(cè)噪聲未知或突變時(shí)，傳統(tǒng)SLAM 算法性能會(huì)受到極大影響。

針對(duì)PF-SLAM 算法中粒子多樣性喪失的問(wèn)題，文獻(xiàn)[3]將粒子群方法引入到無(wú)跡粒子濾波SLAM 中，提高了粒子多樣性及系統(tǒng)狀態(tài)估計(jì)精度。文獻(xiàn)[4]采用遺傳算法中交叉變異方法，解決PF 粒子多樣性喪失問(wèn)題。文獻(xiàn)[5]采用布谷鳥(niǎo)算法優(yōu)化重采樣方法，將粒子引導(dǎo)至高似然區(qū)域，提高了算法精度。文獻(xiàn)[6]模擬螢火蟲(chóng)群體的生存機(jī)制優(yōu)化重采樣過(guò)程，解決粒子多樣性喪失問(wèn)題。文獻(xiàn)[7]采用排擠機(jī)制和啟發(fā)式蝙蝠算法，通過(guò)引入慣性權(quán)重提高了粒子多樣性。針對(duì)SLAM問(wèn)題中的時(shí)變觀測(cè)噪聲，文獻(xiàn)[8]采用殘差和信息序列的正交原則對(duì)觀測(cè)噪聲進(jìn)行實(shí)時(shí)調(diào)整，但計(jì)算復(fù)雜度高，無(wú)法滿(mǎn)足系統(tǒng)實(shí)時(shí)性。文獻(xiàn)[9]提出多個(gè)并行濾波器近似真實(shí)噪聲的多模型自適應(yīng)算法，實(shí)際運(yùn)行中需要知道傳感器的精確誤差特性，難度較大且不易實(shí)現(xiàn)。近年來(lái)，變分貝葉斯的機(jī)器學(xué)習(xí)方法[10]因其計(jì)算量低、時(shí)效好的優(yōu)勢(shì)，被廣泛應(yīng)用于SLAM 算法中。文獻(xiàn)[11]采用線性變分貝葉斯方法對(duì)SLAM問(wèn)題中地圖特征和噪聲方差進(jìn)行迭代更新。文獻(xiàn)[12]利用變分貝葉斯方法對(duì)移動(dòng)機(jī)器人狀態(tài)和未知觀測(cè)噪聲參數(shù)的聯(lián)合后驗(yàn)概率進(jìn)行估計(jì)，解決觀測(cè)噪聲參數(shù)未知或變化時(shí)，傳統(tǒng)濾波算法出現(xiàn)的濾波發(fā)散問(wèn)題。

在上述研究工作基礎(chǔ)上，本論文提出基于變分貝葉斯的近鄰采樣PF-SLAM 算法（Variational Bayesian Adjacent Sampling-Particle Filter-Simultaneous Localization and Mapping,VBAS-PF-SLAM），采用高斯混合模型對(duì)時(shí)變觀測(cè)噪聲建模，利用變分貝葉斯循環(huán)迭代方式估算未知參數(shù)分布，處理時(shí)變觀測(cè)噪聲；參考高權(quán)重保留粒子，通過(guò)近鄰位置關(guān)系優(yōu)化低權(quán)重調(diào)整粒子的位置分布，避免重采樣過(guò)程中粒子退化現(xiàn)象，同時(shí)提高粒子多樣性，使得粒子更好地表示機(jī)器人位置概率分布。仿真實(shí)驗(yàn)結(jié)果表明本文提出的算法保證了實(shí)時(shí)性且有效提高移動(dòng)機(jī)器人同時(shí)定位與建圖的精度。

1 移動(dòng)機(jī)器人PF-SLAM 算法

移動(dòng)機(jī)器人驅(qū)動(dòng)模型與觀測(cè)模型如下：

式中Xk=（xk,yk,gk）T為k時(shí)刻機(jī)器人狀態(tài)向量，xk、yk、gk為移動(dòng)機(jī)器人在二維正交坐標(biāo)系下的位置和朝向信息，xi、yi為環(huán)境路標(biāo)的位置信息，μk為零均值、Qk為高斯白噪聲的協(xié)方差，為速度噪聲，為轉(zhuǎn)向角噪聲，mk,αk為機(jī)器人控制變量，其中mk為機(jī)器人移動(dòng)速度，αk為機(jī)器人前輪相對(duì)于機(jī)器人底座偏轉(zhuǎn)角度。Δt為傳感器采樣時(shí)間間隔，WB為前后輪間軸距。觀測(cè)模型中l(wèi)為機(jī)器人與環(huán)境路標(biāo)之間激光傳感器測(cè)量距離，φ為機(jī)器人朝向與環(huán)境路標(biāo)夾角，機(jī)器人朝向由慣性傳感器解算后獲得，vk為觀測(cè)噪聲（包括時(shí)變與時(shí)不變兩種情況的觀測(cè)噪聲）。

假設(shè)已知1:k時(shí)刻機(jī)器人狀態(tài)信息、環(huán)境觀測(cè)信息、機(jī)器人控制信息與環(huán)境路標(biāo)信息，則機(jī)器人SLAM問(wèn)題就可看成求解后驗(yàn)概率分布問(wèn)題。

傳統(tǒng)粒子濾波SLAM算法使用里程計(jì)數(shù)據(jù)和傳感器觀測(cè)信息作為重要性建議分布，從中采樣得到粒子，對(duì)機(jī)器人位姿狀態(tài)和路標(biāo)進(jìn)行估計(jì)。算法步驟如下：

Step 1：重要性采樣：建議分布中采樣得到N個(gè)初始粒子；

Step 2：粒子權(quán)重計(jì)算：根據(jù)當(dāng)前時(shí)刻機(jī)器人觀測(cè)信息更新粒子權(quán)值；

Step 3：粒子重采樣：淘汰小權(quán)重粒子、復(fù)制大權(quán)重粒子并歸一化新粒子；

Step 4：定位與建圖：根據(jù)粒子分布信息與傳感器觀測(cè)數(shù)據(jù)估計(jì)機(jī)器人位姿狀態(tài)并構(gòu)建環(huán)境地圖；

Step 5：機(jī)器人運(yùn)行至下一時(shí)刻，根據(jù)驅(qū)動(dòng)模型獲得新的粒子分布，返回至Step 2 循環(huán)運(yùn)行。

2 基于變分貝葉斯優(yōu)化的時(shí)變觀測(cè)噪聲估計(jì)

2.1 時(shí)變觀測(cè)噪聲建模

傳統(tǒng)SLAM方法采用單高斯分布擬合時(shí)不變觀測(cè)噪聲，理論上能夠取得精確估計(jì)結(jié)果。時(shí)不變觀測(cè)噪聲的單高斯多維隨機(jī)變量X服從分布描述為：

式中X表示維度為d的隨機(jī)列向量，μ為均值，Σ 為協(xié)方差。

然而在實(shí)際SLAM場(chǎng)景中，移動(dòng)機(jī)器人平臺(tái)抖動(dòng)、周邊環(huán)境干擾及傳感器存在噪聲等問(wèn)題會(huì)造成機(jī)器人移動(dòng)模型觀測(cè)噪聲參數(shù)變化，這種情況下傳統(tǒng)濾波方法不能調(diào)節(jié)觀測(cè)噪聲參數(shù)會(huì)導(dǎo)致SLAM精度下降或發(fā)散?？紤]到實(shí)際環(huán)境存在多個(gè)噪聲來(lái)源，單高斯分布無(wú)法描述這種情況，所以本文采用高斯混合模型表示時(shí)變觀測(cè)噪聲[13]。

高斯混合模型作為高斯分量的簡(jiǎn)單線性疊加能夠提供比單高斯分布更豐富的時(shí)變?cè)肼晹_動(dòng)，進(jìn)而可以更準(zhǔn)確描述多個(gè)來(lái)源的噪聲概率分布，提高移動(dòng)機(jī)器人SLAM 估計(jì)精度。圖1為采用單高斯分布曲線近似表示混合高斯分布時(shí)變?cè)肼暤氖疽鈭D，圖中紅色曲線近似表示時(shí)變觀測(cè)噪聲分布，藍(lán)色曲線為多個(gè)單高斯分布擬合的高斯混合模型。

圖1 時(shí)變?cè)肼暦植际疽鈭DFig.1 Time-varying noise distribution diagram

在采用高斯混合模型對(duì)時(shí)變觀測(cè)噪聲建模后，通過(guò)變分貝葉斯學(xué)習(xí)方法估算混合模型的未知參數(shù)分布，解決未知或突變觀測(cè)噪聲產(chǎn)生的狀態(tài)發(fā)散問(wèn)題。

使用多個(gè)單高斯分布加權(quán)和近似表示時(shí)變觀測(cè)噪聲vk，得到高斯混合模型：

式中L為單高斯分布個(gè)數(shù)，αk,l為k時(shí)刻高斯項(xiàng)l的系數(shù)，μk,l為高斯分布均值，為協(xié)方差。設(shè)有S個(gè)觀測(cè)噪聲樣本，隱變量Z表示為S×L的矩陣，將上述參數(shù)與隱變量Z統(tǒng)一為噪聲參數(shù)集合ψk=［αk,μk,Λk,Z］。

為估計(jì)觀測(cè)噪聲參數(shù)，假設(shè)高斯混合模型的先驗(yàn)分布為Gauss-Wishart 分布：

采用變分貝葉斯方法[14]迭代估計(jì)高斯混合模型中未知噪聲參數(shù)的超參數(shù) Σ,ν,m,β,λ，進(jìn)而獲得時(shí)變觀測(cè)噪聲參數(shù)估計(jì)值。

2.2 高斯混合模型噪聲參數(shù)估計(jì)

通過(guò)驅(qū)動(dòng)模型與觀測(cè)數(shù)據(jù)推斷系統(tǒng)狀態(tài)及未知噪聲參數(shù)。假設(shè)系統(tǒng)狀態(tài)與未知噪聲參數(shù)后驗(yàn)分布為：

由于ψk的表示形式復(fù)雜，難以直接估計(jì)噪聲參數(shù)的真實(shí)后驗(yàn)分布p(ψk|xk,y1:k)，根據(jù)變分近似思想，引入易處理的分布q(ψk)近似真實(shí)后驗(yàn)分布。假設(shè)基于變分貝葉斯學(xué)習(xí)模型的對(duì)數(shù)邊緣似然函數(shù)為：

式中KL(q(ψk)||p(xk,ψk|y1:k))為KL散度，用來(lái)描述q(ψk)與p(ψk|xk,y1:k)真實(shí)分布的差距，F(xiàn)(q(ψk))為變分自由能?？紤]到式(7)左邊值恒定，當(dāng)q(ψk)與真實(shí)分布p(ψk|xk,y1:k)充分接近時(shí)，KL散度最小，F(xiàn)(q(ψk))最大。變分貝葉斯方法通過(guò)對(duì)q(ψk)進(jìn)行迭代，使得變分自由能F(q(ψk))最大化，即獲得最小KL散度。

通過(guò)平均場(chǎng)理論，將聯(lián)合概率分布近似為多變量概率分布函數(shù)的乘積，q(ψk)可分解為：

式中隱變量Z的近似后驗(yàn)分布為q(Z)及噪聲參數(shù)的近似聯(lián)合后驗(yàn)分布q(αk,μk,Λk)通過(guò)坐標(biāo)上升迭代方法求解[15]，得到：

將式(9)轉(zhuǎn)化為：

式中超參數(shù)β,m,Σ,ν,λ更新公式如下：

對(duì)于式(12)的中間變量定義為：

式中γs,j為式(11)隱變量Z后驗(yàn)分布?xì)w一化常數(shù)：

通過(guò)式(12)(13)迭代更新超參數(shù)的值，每次迭代后計(jì)算變分自由能并計(jì)算：

當(dāng)ΔF小于閾值τ時(shí)即認(rèn)為變分自由能最大化，此時(shí)q(ψk)與真實(shí)分布接近，進(jìn)而獲得時(shí)變觀測(cè)噪聲的參數(shù)估計(jì)值。

3 基于近鄰采樣的PF-SLAM 算法

傳統(tǒng)PF-SLAM 算法引入重采樣方法解決迭代過(guò)程中權(quán)值退化問(wèn)題時(shí)會(huì)出現(xiàn)粒子多樣性喪失問(wèn)題，并且重采樣后的粒子位置分布也影響了機(jī)器人位姿估計(jì)精度。本文提出基于近鄰采樣的 PF-SLAM 算法(AS-PF-SLAM)過(guò)程分為以下三步：基于近鄰位置關(guān)系的粒子采樣、采樣粒子位置優(yōu)化、構(gòu)建新的粒子集。

3.1 基于近鄰位置關(guān)系的粒子采樣

將傳統(tǒng)重采樣后的粒子集分為高權(quán)重粒子保留組與低權(quán)重粒子調(diào)整組，根據(jù)近鄰位置關(guān)系對(duì)調(diào)整組粒子采樣，使用較少數(shù)量的粒子分布，覆蓋調(diào)整組粒子分布區(qū)域，優(yōu)化粒子位置分布，增強(qiáng)粒子多樣性。

3.1.1 重采樣粒子數(shù)量計(jì)算

根據(jù)采樣粒子的維度，計(jì)算其近鄰粒子數(shù)量，例如維度為1、2 維的情況下，其近鄰粒子數(shù)量分別2、4，進(jìn)一步推廣到dim維情況下的近鄰粒子數(shù)量為2*dim。在粒子采樣中，由采樣粒子代表其近鄰粒子的概率密度分布。調(diào)整組中粒子采樣數(shù)量為(dim為粒子維度)： 調(diào)整組粒子數(shù)/2*dim+1。

3.1.2 近鄰位置關(guān)系的計(jì)算

樣本點(diǎn)Xi與樣本空間中其它點(diǎn)Xj的近鄰關(guān)系由歐式距離來(lái)衡量。距離公式如下：

式中k為樣本點(diǎn)的維度分量，當(dāng)p=2時(shí)，表示歐式距離；歐式距離可以簡(jiǎn)單有效地反映兩點(diǎn)之間位置拓?fù)潢P(guān)系，計(jì)算復(fù)雜度小，更符合本文要求。

3.1.3 粒子采樣過(guò)程

Step 1：傳統(tǒng)的重采樣方法對(duì)粒子集合Sk重采樣，獲得重采樣集合SA，將SA集合中的高權(quán)重粒子構(gòu)成保留組SGH，未選入SA中的Sk其他粒子構(gòu)成調(diào)整組SGA，粒子數(shù)為NGA；

Step 2：在保留組SGH中尋找權(quán)重最高的粒子Pm，根據(jù)近鄰位置關(guān)系尋找調(diào)整組中距離Pm最遠(yuǎn)粒子Ps1，對(duì)該粒子位置進(jìn)行優(yōu)化（詳見(jiàn)采樣粒子位置優(yōu)化部分）得到P's1，將其加入到采樣組SGS；同時(shí)搜尋Ps1的2*dim鄰近粒子，將鄰近粒子與Ps1從調(diào)整組中刪除；

Step 3：將上一步中優(yōu)化的P's1放在調(diào)整組中，類(lèi)似于Step 2 的處理過(guò)程，尋找當(dāng)前調(diào)整組中距離P's1最遠(yuǎn)的粒子Ps2，對(duì)該粒子位置優(yōu)化（詳見(jiàn)采樣粒子位置優(yōu)化部分）得到P's2，將其加入采樣組SGS；同時(shí)搜尋Ps2的2*dim鄰近粒子，將這些鄰近粒子與Ps2從調(diào)整組中刪除。對(duì)以后的粒子重復(fù)Step 2 的過(guò)程，直至調(diào)整組中粒子數(shù)量低于2*dim+1，獲得SGS中粒子個(gè)數(shù)為

3.2 采樣粒子位置優(yōu)化

傳統(tǒng)重采樣沒(méi)有考慮粒子位置分布對(duì)機(jī)器人估計(jì)精度的影響。本文根據(jù)粒子權(quán)重將粒子劃分為權(quán)重較小的調(diào)整粒子Psj與權(quán)重較大的保留粒子Pam。通過(guò)近鄰位置關(guān)系，找到距離調(diào)整點(diǎn)最近的2*dim個(gè)保留點(diǎn)Pam，比較近鄰保留點(diǎn)Pam的權(quán)重大小，選擇最高權(quán)重的近鄰保留點(diǎn)，通過(guò)式(16)計(jì)算調(diào)整點(diǎn)Psj的位置：

式中，randn是介于(0,1)區(qū)間的正態(tài)分布隨機(jī)數(shù)，（1-abs（randn）/step）為學(xué)習(xí)因子，其中step為randn隨機(jī)數(shù)向最優(yōu)粒子位置逼近程度，取值為3σ(σ為randn標(biāo)準(zhǔn)差)，Psj為調(diào)整點(diǎn)位置，Pam為調(diào)整粒子附近權(quán)重最大的保留粒子位置。

3.3 構(gòu)建重采樣粒子集

重構(gòu)SGS與SA為重采樣粒子集S'A，集合粒子個(gè)數(shù)為N，與重采樣前Sk的元素個(gè)數(shù)相同。SA是由傳統(tǒng)重采樣方法產(chǎn)生的高權(quán)重粒子集，缺乏粒子多樣性且未考慮粒子位置分布對(duì)機(jī)器人概率分布的影響。因此，本算法將SA去掉M個(gè)粒子后，再與SGS一起構(gòu)成新的粒子集S'A。為了降低計(jì)算量，可以采用確定性重采樣方法在SA中選擇去掉的粒子。

4 基于變分貝葉斯優(yōu)化近鄰采樣PF-SLAM 算法

將變分貝葉斯粒子濾波方法與近鄰采樣粒子濾波算法結(jié)合，提出基于變分貝葉斯優(yōu)化的近鄰采樣PF-SLAM 算法（VBAS-PF-SLAM），處理時(shí)變觀測(cè)噪聲的同時(shí)提高了粒子多樣性和系統(tǒng)估計(jì)精度。圖2為VBAS-PF-SLAM 算法流程圖，具體算法步驟如下：

圖2 基于變分貝葉斯優(yōu)化的近鄰采樣PF-SLAM 流程圖Fig.2 Flow chart of adjacent sampling PF-SLAM algorithm based on variational Bayesian optimization

1) 初始化概率密度函數(shù)p(x0)，使用高斯混合模型近似時(shí)變觀測(cè)噪聲，產(chǎn)生初始粒子；初始化高斯混合模型未知參數(shù)的超參數(shù)。

2) 在建議分布中采樣N個(gè)粒子，從狀態(tài)方程中得到先驗(yàn)概率：

3) 機(jī)器人狀態(tài)估計(jì)：傳感器獲取觀測(cè)數(shù)據(jù)并根據(jù)觀測(cè)值計(jì)算粒子權(quán)重ωki：

4) 使用變分貝葉斯方法[14]循環(huán)迭代地求出混合模型中未知參數(shù)分布。

5) 針對(duì)權(quán)重退化及粒子多樣性喪失問(wèn)題，采用近鄰

采樣算法改進(jìn)重采樣，步驟如下：

① 按式(15)計(jì)算粒子間近鄰位置關(guān)系并采樣。

② 按式(16)對(duì)采樣后的粒子位置分布進(jìn)行優(yōu)化。

③ 位置優(yōu)化后的粒子與原重采樣粒子重構(gòu)成新的粒子集。

6) 判斷機(jī)器人是否到達(dá)終點(diǎn)，對(duì)2)～5)步驟進(jìn)行迭代操作。

5 仿真實(shí)驗(yàn)與分析

5.1 移動(dòng)機(jī)器人SLAM 仿真實(shí)驗(yàn)環(huán)境

圖3為仿真實(shí)驗(yàn)環(huán)境，圖中綠色十字、米字符和紅色米字符分別代表135 個(gè)隨機(jī)設(shè)定的特征點(diǎn)位置、機(jī)器人運(yùn)動(dòng)的17 個(gè)路標(biāo)點(diǎn)和機(jī)器人估計(jì)的特征點(diǎn)位置，用于描述機(jī)器人同時(shí)定位與地圖構(gòu)建中傳感器掃描的環(huán)境特征，折線為機(jī)器人預(yù)定前進(jìn)路線。移動(dòng)機(jī)器人SLAM 仿真參數(shù)設(shè)定：機(jī)器人移動(dòng)速度為3 m/s，激光雷達(dá)傳感器探測(cè)距離為0～30 m，探測(cè)范圍為360°。機(jī)器人控制信號(hào)時(shí)間間隔為0.025 s，噪聲參數(shù)維度為二維，高斯混合模型設(shè)定由5個(gè)單高斯分布構(gòu)成L=5，取迭代閾值為 ΔF＜10-8時(shí)迭代停止，機(jī)器人系統(tǒng)噪聲為μk=diag[（0.1m）2,（π/180）2]，時(shí)不變觀測(cè)噪聲協(xié)方差為vk=diag[（0.1m）2,0.0552]，參考該值設(shè)置時(shí)變觀測(cè)噪聲協(xié)方差，使得二者數(shù)量級(jí)相同，粒子集中粒子個(gè)數(shù)為100，圖中黑色橢圓為機(jī)器人位姿估計(jì)時(shí)的粒子群放大圖，由100 個(gè)藍(lán)色圓點(diǎn)組成。

圖3 機(jī)器人定位與建圖仿真結(jié)果Fig.3 Simulation results of robot localization and mapping

5.2 基于近鄰采樣的 PF-SLAM 算法（AS-PF-SLAM）驗(yàn)證

5.2.1 本算法對(duì)傳統(tǒng)重采樣算法改進(jìn)效果對(duì)比

將近鄰采樣算法（AS）與15 種傳統(tǒng)重采樣方法應(yīng)用于零均值單高斯分布模擬觀測(cè)噪聲的機(jī)器人定位與建圖中，分析近鄰采樣算法對(duì)15 種傳統(tǒng)重采樣方法改進(jìn)前后的移動(dòng)機(jī)器人位置估計(jì)均方根誤差(Root Mean Square Error,RMSE)。

式中（xm_k,ym_k）為機(jī)器人真實(shí)位置，（xk,yk）為機(jī)器人估計(jì)位置，n為機(jī)器人迭代次數(shù)。

如圖4所示，藍(lán)色條形統(tǒng)計(jì)為15 種傳統(tǒng)重采樣算法的機(jī)器人位置估計(jì)均方根誤差，橙色條形統(tǒng)計(jì)為近鄰采樣算法的機(jī)器人位置估計(jì)均方根誤差。仿真數(shù)據(jù)表明，通過(guò)近鄰采樣算法改進(jìn)后的重采樣算法將機(jī)器人位置估計(jì)均方根誤差平均降低了36.14%，證明本算法能夠有效提高PF-SLAM 精度，且具有通用性。

圖4 本算法對(duì)傳統(tǒng)重采樣算法改進(jìn)效果對(duì)比Fig.4 This algorithm compares the improvement effect of traditional resampling algorithm

5.2.2 本算法與基于蝙蝠算法、粒子群算法的PF-SLAM 仿真對(duì)比

圖5為 AS-PF-SLAM 算法與基于粒子群PF-SLAM 算法（PSO-PF-SLAM）、基于蝙蝠算法的PF-SLAM（BA-PF-SLAM）的PF-SLAM 粒子位置分布及粒子多樣性對(duì)比圖。圖5(a)為移動(dòng)機(jī)器人初始粒子分布圖，即機(jī)器人在初始時(shí)刻確定自身位置，但沒(méi)有確定自身朝向角。圖5(b)為通過(guò)外部傳感器觀測(cè)后的粒子分布圖，即機(jī)器人運(yùn)動(dòng)較短距離后，考慮自身朝向角不確定性，粒子向周身360°范圍內(nèi)擴(kuò)散，再根據(jù)傳感器觀測(cè)值對(duì)粒子群進(jìn)行粒子權(quán)重系數(shù)評(píng)估（高、低權(quán)重粒子）的位置分布。

針對(duì)傳統(tǒng)重采樣方法未考慮粒子位置分布對(duì)濾波精度的影響以及粒子多樣性喪失現(xiàn)象。AS 算法通過(guò)粒子間近鄰?fù)負(fù)湮恢藐P(guān)系將低權(quán)重粒子保留并趨近于近鄰高權(quán)重粒子，如圖5(c)所示，使得粒子位置分布更接近機(jī)器人概率分布。圖5(d)為AS-PF-SLAM 算法優(yōu)化后的新粒子集，其具有更好的粒子位置分布及粒子多樣性，進(jìn)而提高移動(dòng)機(jī)器人SLAM 精度。

粒子群優(yōu)化算法(PSO)效仿鳥(niǎo)群交流特征進(jìn)行粒子采樣尋找全局最優(yōu)解。圖5(e)為PSO-PF-SLAM 算法粒子位置優(yōu)化圖，粒子在搜索空間中單獨(dú)搜尋局部最優(yōu)粒子和全局最優(yōu)粒子，使其位置向高權(quán)重粒子靠近。與AS 算法位置優(yōu)化方式相比，PSO 算法中無(wú)法尋到最優(yōu)位置的低權(quán)重粒子直接淘汰，圖5(f)為PSO算法粒子集。

蝙蝠算法（BA）效仿蝙蝠覓食的回聲定位方式進(jìn)行粒子采樣，在個(gè)體速度與位置基礎(chǔ)上增加頻率和聲波，利于粒子跳出局部最優(yōu)，提高運(yùn)算效率并增加粒子多樣性。圖5(g)為BA-PF-SLAM 算法粒子位置優(yōu)化圖，低權(quán)重粒子向最優(yōu)粒子移動(dòng)、高權(quán)重粒子進(jìn)行方向不定的隨機(jī)移動(dòng)。相比于PSO-PF-SLAM 算法，BA-PF-SLAM 算法的低權(quán)重粒子隨著高權(quán)重粒子的隨機(jī)移動(dòng)易于擺脫局部極值吸引，使趨近全局最優(yōu)的粒子增多，但距離高權(quán)重粒子較遠(yuǎn)的其他粒子仍會(huì)被淘汰。圖5(h)為BA-PF-SLAM 算法的粒子集。

圖5 多種算法優(yōu)化的粒子位置分布及多樣性對(duì)比圖Fig.5 Particle distribution and diversity comparison of multiple algorithms optimization

綜上所述，AS-PF-SLAM 算法通過(guò)粒子間近鄰?fù)負(fù)湮恢藐P(guān)系，將低權(quán)重粒子趨近高權(quán)重的粒子位置，穩(wěn)定且保留了更多采樣粒子，優(yōu)化粒子位置分布的同時(shí)進(jìn)一步提高了粒子多樣性，使粒子更均勻的分布在可能區(qū)域內(nèi)，在真實(shí)位置附近的權(quán)重較大且復(fù)制數(shù)量較多，保證了最優(yōu)位置的估計(jì)精度。

采用零均值單高斯分布模擬觀測(cè)噪聲進(jìn)行仿真對(duì)比，結(jié)果如圖6所示，以機(jī)器人運(yùn)行迭代次數(shù)為橫坐標(biāo)、機(jī)器人位置估計(jì)誤差為縱坐標(biāo)，在相同仿真環(huán)境下，對(duì)比傳統(tǒng) PF-SLAM 算法（黑色線條）、PSO-PF-SLAM 算法（綠色線條）、BA-PF-SLAM 算法（藍(lán)色線條）與AS-PF-SLAM 算法（紅色線條）的移動(dòng)機(jī)器人位置估計(jì)誤差。

圖6 單高斯噪聲下多PF-SLAM 算法定位誤差對(duì)比Fig.6 Comparison of positioning errors of multiple PF-SLAM algorithms under single Gaussian noise

仿真數(shù)據(jù)表明，基于AS-PF-SLAM 算法的機(jī)器人位置估計(jì)精度較PSO-PF-SLAM、BA-PF-SLAM 算法的位置估計(jì)精度平均提升了27.36%。證明了本算法對(duì)于移動(dòng)機(jī)器人同時(shí)定位與地圖構(gòu)建的魯棒性增強(qiáng)有明顯效果。

5.3 基于變分貝葉斯優(yōu)化的近鄰采樣PF-SLAM 算法驗(yàn)證

采用高斯混合模型模擬時(shí)變觀測(cè)噪聲，進(jìn)行仿真對(duì)比，圖7為在相同仿真環(huán)境下對(duì)比傳統(tǒng)PF-SLAM、基于期望最大化算法（EM）的PF-SLAM 算法、VB-PF-SLAM 算法及VBAS-PF-SLAM 算法的移動(dòng)機(jī)器人位置估計(jì)誤差曲線。其中黑色線條代表PF-SLAM算法的位置估計(jì)誤差；綠色線條代表EM-PF-SLAM 算法的位置估計(jì)誤差；紅色線條代表VB-PF-SLAM 算法的位置估計(jì)誤差；藍(lán)色線條代表VBAS-PF-SLAM 算法的位置估計(jì)誤差。

圖7 混合高斯噪聲下多PF-SLAM 算法定位誤差對(duì)比Fig.7 Comparison of positioning errors of multiple PF-SLAM algorithms under mixed Gaussian noise

仿真數(shù)據(jù)表明，VBAS-PF-SLAM 算法的機(jī)器人位置估計(jì)精度較傳統(tǒng)PF-SLAM 算法精度提升了76%，較EM-PF-SLAM 算法提升了54%，較VB-PF-SLAM算法提升了33%，保證算法實(shí)時(shí)性的同時(shí)，顯著提升了機(jī)器人同時(shí)定位與建圖精度。

6 結(jié) 論

針對(duì)傳統(tǒng)PF-SLAM 算法中粒子多樣性易喪失及傳感器觀測(cè)噪聲時(shí)變問(wèn)題，本文提出基于變分貝葉斯優(yōu)化的近鄰采樣PF-SLAM算法，優(yōu)化粒子位置分布，并增強(qiáng)粒子多樣性，使得粒子更好地表示機(jī)器人概率分布。采用高斯混合模型對(duì)時(shí)變觀測(cè)噪聲建模，利用變分貝葉斯方法迭代計(jì)算混合模型中未知參數(shù)分布，處理時(shí)變觀測(cè)噪聲，提高移動(dòng)機(jī)器人SLAM估計(jì)精度。

實(shí)驗(yàn)結(jié)果表明，本算法與PSO 算法、BA 算法相比，具有更高精度與時(shí)效性，且對(duì)傳統(tǒng)重采樣算法改進(jìn)具有通用性。在觀測(cè)噪聲時(shí)變情況下，本算法顯著提高了機(jī)器人定位與建圖精度，對(duì)解決未知環(huán)境中的移動(dòng)機(jī)器人定位與地圖構(gòu)建問(wèn)題具有一定指導(dǎo)意義。