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

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

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

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

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

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

1 移動機器人PF-SLAM 算法

移動機器人驅(qū)動模型與觀測模型如下：

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

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

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

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

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

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

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

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

2 基于變分貝葉斯優(yōu)化的時變觀測噪聲估計

2.1 時變觀測噪聲建模

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

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

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

高斯混合模型作為高斯分量的簡單線性疊加能夠提供比單高斯分布更豐富的時變噪聲擾動，進而可以更準(zhǔn)確描述多個來源的噪聲概率分布，提高移動機器人SLAM 估計精度。圖1為采用單高斯分布曲線近似表示混合高斯分布時變噪聲的示意圖，圖中紅色曲線近似表示時變觀測噪聲分布，藍色曲線為多個單高斯分布擬合的高斯混合模型。

圖1 時變噪聲分布示意圖Fig.1 Time-varying noise distribution diagram

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

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

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

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

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

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

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

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

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

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

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

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

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

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

式中γs,j為式(11)隱變量Z后驗分布歸一化常數(shù)：

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

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

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

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

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

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

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

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

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

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

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

3.1.3 粒子采樣過程

Step 1：傳統(tǒng)的重采樣方法對粒子集合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最遠粒子Ps1，對該粒子位置進行優(yōu)化（詳見采樣粒子位置優(yōu)化部分）得到P's1，將其加入到采樣組SGS；同時搜尋Ps1的2*dim鄰近粒子，將鄰近粒子與Ps1從調(diào)整組中刪除；

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

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

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

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

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

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

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

將變分貝葉斯粒子濾波方法與近鄰采樣粒子濾波算法結(jié)合，提出基于變分貝葉斯優(yōu)化的近鄰采樣PF-SLAM 算法（VBAS-PF-SLAM），處理時變觀測噪聲的同時提高了粒子多樣性和系統(tǒng)估計精度。圖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)，使用高斯混合模型近似時變觀測噪聲，產(chǎn)生初始粒子；初始化高斯混合模型未知參數(shù)的超參數(shù)。

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

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

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

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

采樣算法改進重采樣，步驟如下：

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

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

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

6) 判斷機器人是否到達終點，對2)～5)步驟進行迭代操作。

5 仿真實驗與分析

5.1 移動機器人SLAM 仿真實驗環(huán)境

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

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

5.2 基于近鄰采樣的 PF-SLAM 算法（AS-PF-SLAM）驗證

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

6 結(jié) 論

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

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