比例最小偏度單行采樣的平方根UKF-SLAM算法

汪貴冬，陳躍東，陳孟元

安徽工程大學(xué)安徽省電氣傳動與控制重點實驗室，安徽蕪湖 241000

比例最小偏度單行采樣的平方根UKF-SLAM算法

汪貴冬，陳躍東，陳孟元

安徽工程大學(xué)安徽省電氣傳動與控制重點實驗室，安徽蕪湖 241000

對于UKF-SLAM算法所存在的濾波增益矩陣計算失真，采用對稱采樣計算復(fù)雜度相對較高且易產(chǎn)生非局部效應(yīng)等問題，提出基于比例最小偏度單行采樣的平方根UKF-SLAM算法。改進(jìn)后的算法采用協(xié)方差陣的平方根代替協(xié)方差陣帶入迭代運算，并以比例最小偏度單行采樣的方式優(yōu)化采樣策略。仿真結(jié)果表明，該算法能夠有效地提高機(jī)器人位姿以及特征地圖的估計精度，并降低了計算復(fù)雜度，提高算法的穩(wěn)定性。

濾波增益；采樣策略；平方根；計算復(fù)雜度

未知環(huán)境下，利用自身所攜帶的傳感器，機(jī)器人遞增式地創(chuàng)建環(huán)境的特征地圖，與此同時在創(chuàng)建的地圖中修正自身的位置，即移動機(jī)器人的同步定位與地圖構(gòu)建問題（Simultaneous Localization and Mapping，SLAM）[1]。應(yīng)用于SLAM的經(jīng)典算法是將機(jī)器人的運動模型和觀測模型進(jìn)行一階泰勒級數(shù)展開，然后利用擴(kuò)展卡爾曼濾波（Extended Kalman Filter，EKF）對機(jī)器人的位姿以及特征圖進(jìn)行同時最優(yōu)估計。此模型最早是由Sm ith和Cheeseman提出[2-3]。諸多國內(nèi)外學(xué)者基于EKF-SLAM框架，進(jìn)行優(yōu)化與改進(jìn)。例如，呂太之提出的采用極坐標(biāo)對比臨近兩次的觀測值來檢測與減小外部干擾，提高算法的估計精度與魯棒性[4]；張海強(qiáng)等提出改進(jìn)的壓縮型EKF-SLAM算法來降低計算復(fù)雜度[5]；周武等則提出全局觀測地圖模型來改進(jìn)地圖的表征方式[6]。但線性化過程中產(chǎn)生的截斷誤差，且雅克比矩陣計算量大等固有缺陷使得EKF-SLAM算法并不適合在大規(guī)模的環(huán)境地圖中使用[7]。

因此，在研究如何改進(jìn)EKF-SLAM的同時，Julier等人提出了一種可以直接應(yīng)用于非線性系統(tǒng)的新型濾波器[8]，即無跡卡爾曼濾波[9]（UKF）。UKF是一種基于最小方差估計準(zhǔn)則的非線性高斯?fàn)顟B(tài)估計器。類似于擴(kuò)展卡爾曼濾波，均使用高斯隨機(jī)變量來近似狀態(tài)分布，不同的是，UKF使用一些確定性采樣得到的加權(quán)采樣點（Sigma點）來逼近。利用這些采樣點，可以較好地描述高斯隨機(jī)變量真實的均值及方差，其精度逼近泰勒二階，而同等條件下的EKF只能達(dá)到一階精度。在計算復(fù)雜度上，UKF與EKF屬于同階次的，且無需進(jìn)行雅克比矩陣的計算，所以UKF更易于實現(xiàn)[10]。進(jìn)一步研究發(fā)現(xiàn)，由于計算機(jī)的舍入誤差，UKF算法的狀態(tài)協(xié)方差陣在傳遞的過程中，易失去非負(fù)定性和對稱性，從而造成濾波器的發(fā)散。平方根濾波[11]指出傳遞協(xié)方差陣的平方根代替?zhèn)鬟f協(xié)方差陣本身很好地解決了這一問題，并成功應(yīng)用于GPS/DR組合定位[12]與室內(nèi)定位[13]中。

本文首先對UKF算法與平方根UKF算法（SR-UKF）應(yīng)用于SLAM過程進(jìn)行MATLAB對比仿真實驗，得出SR-UKF算法可以明顯提高機(jī)器人位姿估計的精度。但對于SLAM這種對實時性要求較高的系統(tǒng)，降低計算復(fù)雜度、提高計算效率也是至關(guān)重要的[14]。再對SR-UKF算法進(jìn)行深入研究，考慮通過改變采樣策略，使用比例最小偏度單行采樣代替對稱采樣，減少Sigma點數(shù)目來降低算法的計算復(fù)雜度，從而提高實時性。因此，提出一種基于比例最小偏度單行采樣的平方根UKF（SPSR-UKF）算法。

1 SLAM問題的概率描述

其中，Z1:k和u1:k為已知條件。采用貝葉斯濾波原理計算式（1）的后驗概率分布，獲得狀態(tài)的最優(yōu)估計。整個過程分為兩個步驟，第一步進(jìn)行預(yù)測，由機(jī)器人的位姿計算模型以及k-1時刻的后驗概率分布得到k時刻的先驗概率分布：

第二步進(jìn)行觀測更新。由系統(tǒng)的觀測模型得到k時刻的觀測值Zk，實現(xiàn)由先驗概率分布求解后驗概率分布：

其中，η為歸一化系數(shù)。

2 SR-UKF-SLAM算法

2.1 UKF-SLAM算法

UKF以UT變換為核心，以卡爾曼濾波為基本框架，使用確定性采樣的方式得到狀態(tài)估計的后驗狀態(tài)分布。在進(jìn)行濾波之前，將原來的狀態(tài)向量增廣為高斯噪聲變量：

圖1 UKF-SLAM算法框圖

2.2 SR-UKF-SLAM算法

在上述的濾波模型中，計算機(jī)的舍入誤差隨著濾波的深入逐漸累積，濾波誤差協(xié)方差矩陣Pk和預(yù)測誤差協(xié)方差陣Pk|k-1失去非負(fù)定性和對稱性的可能性也隨之增大，使得濾波增益Kk計算失真，造成濾波器發(fā)散。因此，在UKF-SLAM算法模型的基礎(chǔ)上，用協(xié)方差平方根Sk更新代替協(xié)方差Pk更新(Pk=Sk)，提出基于平方根的UKF-SLAM算法（SR-UKF-SLAM）。具體算法過程中，需要用到QR分解以及Cholesky因子更新兩種運算方式來降低計算復(fù)雜度。區(qū)別于UKF-SLAM算法，其優(yōu)化過程如下：

為了避免矩陣逆運算，在對濾波增益Kk的處理上，考慮采用回代法求解，如式（10）所示。最終用式（11）、（13）對狀態(tài)向量進(jìn)行迭代更新。

3 SPSR-UKF-SLAM算法

雖然SR-UKF-SLAM算法在一定程度提高了機(jī)器人位姿和特征地圖的估計精度，但對于SLAM這種對實時性要求較高的系統(tǒng)，希望通過減少Sigma點的數(shù)目來進(jìn)一步降低計算復(fù)雜度。且在對稱采樣中，Sigma點到中心的距離隨的狀態(tài)維數(shù)增加而增大，這樣易產(chǎn)生非局部效應(yīng)，影響了濾波的精度與穩(wěn)定性。

最小偏度單行采樣是一種采樣點數(shù)最少的采樣方式，由于采樣點個數(shù)對UKF的計算量有著直接影響，因此，該策略更適合應(yīng)用于實時性較高的系統(tǒng)中。根據(jù)Julier等人在文獻(xiàn)[16]中的分析，對于n維狀態(tài)分布向量，至少需要n+1個采樣點來確定系統(tǒng)的驗后分布。研究發(fā)現(xiàn)，比例修正可以很好地解決非局部效應(yīng)的問題。因此，本文結(jié)合上述采樣策略的優(yōu)點，應(yīng)用到SLAM過程中，提出一種基于比例最小偏度的平方根UKF-SLAM算法，即SPSR-UKF-SLAM算法。算法修正了SR-UKF-SLAM算法的采樣策略，其具體采樣過程如下：

（3）初始向量(L=1)為：

對于輸入的狀態(tài)維數(shù)L=2，3，…，n時，向量的迭代公式為：

4 算法仿真實驗及其仿真結(jié)果分析

為了研究改進(jìn)后的算法對估計精度的影響，在MATLAB仿真環(huán)境下，利用Tim Bailey開發(fā)的開源仿真平臺，分別對UKF-SLAM、SR-UKF-SLAM以及SPSRUKF-SLAM進(jìn)行仿真實驗。計算機(jī)采用Inter Core-i5的雙核處理器，主頻2.6 GHz，內(nèi)存為4 GB。在實驗中，模擬250 m×200 m的室外環(huán)境，機(jī)器人從坐標(biāo)點（0，0）沿17個路徑點（“?”）巡游一圈，機(jī)器人的行使速度為2 m/s，環(huán)境中隨機(jī)分布了35個靜止的特征點（“*”），兩種算法的仿真結(jié)果如圖2～圖4所示，細(xì)線表示機(jī)器人的理想運動軌跡。虛線表示機(jī)器人的估計路徑，“+”為SLAM估計的特征點位置，橢圓表示特征點估計的誤差范圍。圖2至圖4表明，在初始階段，UKF-SLAM與SRUKF-SLAM計算誤差相似，但隨著時間的推移，由于新的特征不斷加入，UKF-SLAM的機(jī)器人估計軌跡偏離理想運動軌跡的程度越來越大，且特征地圖的誤差橢圓也明顯大于SR-UKF-SLAM算法。與SR-UKF-SLAM算法相比，而SPSR-UKF-SLAM算法過程中，機(jī)器人估計軌跡進(jìn)一步逼近理想的運動軌跡，且誤差橢圓進(jìn)一步縮小。

圖2 UKF-SLAM仿真結(jié)果

圖3 SR-UKF-SLAM仿真結(jié)果

圖4 SPSR-UKF-SLAM仿真結(jié)果

機(jī)器人探索一圈歷時330 s左右，進(jìn)行多次實驗取均值。本文取其前300 s的仿真結(jié)果進(jìn)行數(shù)據(jù)分析。圖5（a）、（b）、（c）分別為UKF-SLAM算法、SR-UKF-SLAM算法以及SR-UKF-SLAM算法在機(jī)器人位姿的x方向、y方向以及航向角的誤差對比圖。從圖中可以看出，三種算法對機(jī)器人位姿估計的誤差均呈上升趨勢，但對比于前兩種算法，SR-UKF-SLAM算法的估計誤差相對平穩(wěn)，并保持較低的水平。以上的實驗結(jié)果均表明，SPSR-UKF-SLAM算法在降低計算復(fù)雜度的同時，也提高了機(jī)器人位姿的估計精度。

圖5 三種算法的估計誤差對比

為了更直觀地對比三種SLAM算法的性能，分別計算三種算法的平均誤差，如表1所示。從表1中的數(shù)據(jù)得知，三種算法中，SPSR-UKF-SLAM算法的機(jī)器人位姿估計精度最高。與SR-UKF-SLAM算法相比，SPSRUKF-SLAM算法在機(jī)器人位姿的x方向、y方向以及航向角的誤差分別降低了19.08%、29.31%、50.47%。

表1 算法誤差統(tǒng)計比較

5 結(jié)論

根據(jù)UKF-SLAM算法存在的計算機(jī)的舍入誤差帶來的濾波器發(fā)散，使用協(xié)方差平方根更新替代協(xié)方差更新；并針對對稱采樣的采樣點較多，實時性差且易產(chǎn)生非局部效應(yīng)等問題，變換采樣策略為比例最小偏度單行采樣，從而提出了更為優(yōu)化的SR-UFK-SLAM算法。經(jīng)過MATLAB仿真實驗表明，SPSR-UKF-SLAM算法與UKF-SLAM算法相比，可以有效地提高機(jī)器人位姿的估計精度，以及特征地圖的估計準(zhǔn)確度，且計算復(fù)雜度相對較低，并具有更好的穩(wěn)定性。

[1]王璐，蔡自興.未知環(huán)境中移動機(jī)器人并發(fā)建圖與定位（CML）的研究進(jìn)展[J].機(jī)器人，2004，26（4）：380-384.

[2]Sm ith R C，Cheeseman P.On the representation and estimation of spatial uncertainty[J].Robotics Research，1986，5（4）：231-238.

[3]Sm ith R，Self M，Cheesseman P.Estimating uncertain spatial relationships in robotics[C]//Proc of Conf Uncertainty in Artificial Intelligence，1988：435-461.

[4]呂太之.一種新的抗外部干擾EKF-SLAM算法[J].計算機(jī)工程，2012，38（21）：1-4.

[5]張海強(qiáng)，竇麗華，方浩，等.基于壓縮型EKF的SLAM改進(jìn)算法[J].系統(tǒng)仿真學(xué)報，2009，21（18）：5668-5680.

[6]周武，趙春霞，沈亞強(qiáng)，等.基于全局觀測地圖模型的SLAM研究[J].機(jī)器人，2010，32（5）：647-654.

[7]李秀才，鄭志強(qiáng)，張輝.SLAM問題中機(jī)器人定位誤差分析與控制[J].自動化學(xué)報，2008，34（3）：323-330.

[8]張恒，樊曉平，劉艷麗.移動機(jī)器人同時定位與地圖構(gòu)建研究進(jìn)展[J].數(shù)據(jù)采集與處理，2005，20（4）：458-465.

[9]Julier S J，Uhlmann J K，Durrant-Whyte H.A new method for the nonlinear transformation of means and covariances in filters and estimation[J].IEEE Transactions on Automatic Control，2000，45（3）：477-482.

[10]石杏喜，趙春霞，郭劍輝.基于PF/CUKF/EKF的移動機(jī)器人SLAM框架算法[J].電子學(xué)報，2009（8）：1865-1868.

[11]Merwe R V D，Wan E A.The squre-root unscented Kalman filter for state and parameter-estimation[C]//International Conference on Acoustics，Speech，and Signal Processing，Utah，2001.

[12]楊靜，鄭南寧.一種基于SR-UKF的GPS/DR組合定位算法[J].系統(tǒng)仿真學(xué)報，2009，21（3）：721-722.

[13]安雷，張國良，湯文俊.基于RSSI的機(jī)器人室內(nèi)卡爾曼濾波定位算法[J].計算機(jī)工程與應(yīng)用，2012，48（8）：230-232.

[14]李樹榮，倪鵬飛.基于平方根UKF的SLAM算法[J].計算機(jī)工程與應(yīng)用，2011，47（22）：209-212.

[15]石杏喜，趙春霞.基于概率的移動機(jī)器人SLAM算法框架[J].計算機(jī)工程，2010，36（2）：31-32.

[16]Julier S J，Uhlmann J K.Reduced sigma point filters for propagation of means and covariances through nonlinear transformation[C]//Proc of American Control Conf，Jefferson City，2002：887-892.

WANG Guidong, CHEN Yuedong, CHEN Mengyuan

Anhui Key Laboratory of Electric Drive and Control, Anhui Polytechnic University, Wuhu, Anhui 241000, China

Due to the distortion of filter gain matrix and high calculation complexity associating with nonlocal effect if symmetric sampling is adopted in the UKF-SLAM algorithm, the UKF-SLAM algorithm adopting square root and single row sampling method based on the proportion and mini-skewness is proposed. The advanced algorithm puts the square root of covariance matrix into iterative computation instead of covariance matrix and adopts the optimized single row sampling strategy based on the proportion and mini-skewness. The experimental results show the algorithm can improve the estimation accuracy of feature map and the pose of robot, besides, that can reduce the complexity and increase stability of the algorithm.

filter gain; sampling strategy; square root; computational complexity

WANG Guidong, CHEN Yuedong, CHEN Mengyuan. UKF-SLAM algorithm adopting square root and single row sampling method based on proportion and mini-skewness. Computer Engineering and Applications, 2014, 50（17）：63-67.

A

TP24

10.3778/j.issn.1002-8331.1404-0132

安徽省自然科學(xué)基金（No.11040606M 153）；安徽高校省級自然科學(xué)研究項目（No.KJ2013A 041）。

汪貴冬（1990—），男，碩士在讀，研究領(lǐng)域為無線傳感器網(wǎng)絡(luò)；陳躍東（1956—），通訊作者，男，教授，碩導(dǎo)，研究領(lǐng)域為運動控制系統(tǒng)與檢測技術(shù)；陳孟元（1984—），男，講師，研究領(lǐng)域為無線傳感器網(wǎng)絡(luò)。E-mail：76920395@qq.com

2014-04-09

2014-05-30

1002-8331（2014）17-0063-05