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

汪貴冬，陳躍東，陳孟元

安徽工程大學(xué)安徽省電氣傳動(dòng)與控制重點(diǎn)實(shí)驗(yàn)室，安徽蕪湖 241000

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

汪貴冬，陳躍東，陳孟元

安徽工程大學(xué)安徽省電氣傳動(dòng)與控制重點(diǎn)實(shí)驗(yàn)室，安徽蕪湖 241000

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

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

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

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

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

1 SLAM問(wèn)題的概率描述

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

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

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

2 SR-UKF-SLAM算法

2.1 UKF-SLAM算法

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

圖1 UKF-SLAM算法框圖

2.2 SR-UKF-SLAM算法

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

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

3 SPSR-UKF-SLAM算法

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

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

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

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

4 算法仿真實(shí)驗(yàn)及其仿真結(jié)果分析

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

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

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

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

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

圖5 三種算法的估計(jì)誤差對(duì)比

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

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

5 結(jié)論

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

[1]王璐，蔡自興.未知環(huán)境中移動(dòng)機(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ì)算機(jī)工程，2012，38（21）：1-4.

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

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

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

[8]張恒，樊曉平，劉艷麗.移動(dòng)機(jī)器人同時(shí)定位與地圖構(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的移動(dòng)機(jī)器人SLAM框架算法[J].電子學(xué)報(bào)，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é)報(bào)，2009，21（3）：721-722.

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

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

[15]石杏喜，趙春霞.基于概率的移動(dòng)機(jī)器人SLAM算法框架[J].計(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）；安徽高校省級(jí)自然科學(xué)研究項(xiàng)目（No.KJ2013A 041）。

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

2014-04-09

2014-05-30

1002-8331（2014）17-0063-05