基于噪聲縮放的自適應(yīng)UKF-SLAM算法

王祖麟,秦 菘,梁毓明

(江西理工大學(xué)電氣工程與自動(dòng)化學(xué)院,江西贛州341000)

基于噪聲縮放的自適應(yīng)UKF-SLAM算法

王祖麟,秦 菘,梁毓明

(江西理工大學(xué)電氣工程與自動(dòng)化學(xué)院,江西贛州341000)

針對(duì)擴(kuò)展卡爾曼濾波(EKF)算法在移動(dòng)機(jī)器人同時(shí)定位和環(huán)境建模(SLAM)中的缺點(diǎn),即非線性系統(tǒng)簡(jiǎn)單線性化所導(dǎo)致的系統(tǒng)狀態(tài)方程的不準(zhǔn)確性、雅克比矩陣的計(jì)算所導(dǎo)致的計(jì)算復(fù)雜化以及噪聲模型不確定性所導(dǎo)致的濾波穩(wěn)定性降低等問(wèn)題,提出一種對(duì)噪聲自適應(yīng)的UKF-SLAM算法。該算法通過(guò)對(duì)噪聲縮放進(jìn)而改變?cè)肼暷Ｐ?利用觀測(cè)殘差序列準(zhǔn)確估計(jì)觀測(cè)噪聲模型協(xié)方差,運(yùn)用預(yù)測(cè)的新息協(xié)方差和IAE開(kāi)窗法求其系統(tǒng)狀態(tài)噪聲縮放因子,從而準(zhǔn)確估計(jì)系統(tǒng)狀態(tài)噪聲模型協(xié)方差,實(shí)現(xiàn)對(duì)不確定的噪聲模型能夠自適應(yīng)UKF-SLAM算法。UKF的Sigma點(diǎn)采樣策略是比例對(duì)稱采樣。實(shí)驗(yàn)結(jié)果證明,該方法相對(duì)EKF算法和UKF算法具有較高的定位精度和自適應(yīng)能力。

同時(shí)定位和環(huán)境建模;無(wú)跡卡爾曼濾波;噪聲縮放;在線自適應(yīng);比例對(duì)稱采樣;開(kāi)窗法

1 概述

實(shí)現(xiàn)智能移動(dòng)機(jī)器人的關(guān)鍵技術(shù)之一就是移動(dòng)機(jī)器人同時(shí)定位和環(huán)境建模(Simultaneous Location and Mapping,SLAM),即一方面機(jī)器人通過(guò)自身傳感器實(shí)現(xiàn)對(duì)周圍環(huán)境模型的建立,另一方面機(jī)器人依靠所建立的環(huán)境模型估計(jì)自身所在環(huán)境中的位置[1]。SLAM可以理解為機(jī)器人同時(shí)估計(jì)其位置和環(huán)境模型特征的估計(jì)問(wèn)題。

目前最經(jīng)典的算法就是擴(kuò)展卡爾曼濾波(Extend Kalman Filtering,EKF)[2],該算法是通過(guò)把機(jī)器人的系統(tǒng)模型進(jìn)行泰勒展開(kāi),將其高階忽略,把非線性系統(tǒng)變?yōu)榫€性系統(tǒng),但是當(dāng)其高階項(xiàng)無(wú)法忽略時(shí),該系統(tǒng)就會(huì)出現(xiàn)較大誤差,并且濾波器很難穩(wěn)定,最終導(dǎo)致濾波器無(wú)法收斂,使得所建立的環(huán)境模型精度較低[3]。此外,EKF算法必須獲取系統(tǒng)噪聲和觀測(cè)噪聲的先驗(yàn)估計(jì)特性,在實(shí)際中是很難實(shí)現(xiàn)的,況且如果這些先驗(yàn)數(shù)據(jù)不準(zhǔn)確或者信息量不足,則會(huì)造成濾波器估計(jì)精度降低。

針對(duì)以上EKF問(wèn)題,人們提出了很多算法解決這個(gè)問(wèn)題。文獻(xiàn)[4]提出無(wú)跡卡爾曼濾波器(UKF)濾波器,該算法是一種用采樣策略將非線性逼近線性的辦法。UKF[5]是以UT(Unscented Transformation)變換為基礎(chǔ),采用KF線性濾波框架,其采樣形式為確定性采樣,不存在PF的粒子退化問(wèn)題,對(duì)非線性分布統(tǒng)計(jì)量的計(jì)算精度至少為2階。但是UT變換采樣點(diǎn)到中心點(diǎn)的距離會(huì)隨著系統(tǒng)位數(shù)的增加而越來(lái)越遠(yuǎn),進(jìn)而出現(xiàn)非局部效應(yīng)。文獻(xiàn)[6]提出了比例對(duì)稱采樣可以解決采樣非局部效應(yīng)。文獻(xiàn)[7]提出了基于平方根容積卡爾曼濾波的SLAM算法。文獻(xiàn)[8]提出了基于線段特征匹配的EKF-SLAM算法。

在實(shí)際過(guò)程中,UKF也會(huì)存在噪聲模型不準(zhǔn)確對(duì)濾波器精度和穩(wěn)定性的影響,并且主要是濾波增益和噪聲模型不能在線調(diào)節(jié),不具備在線自適應(yīng)能力。考慮了以上UKF算法的缺點(diǎn),文獻(xiàn)[9-10]提出了將UKF和粒子濾波器相結(jié)合的算法,文獻(xiàn)[11]提出了通過(guò)調(diào)節(jié)UKF參數(shù)的改進(jìn)辦法。本文提出基于噪聲縮放的UKF自適應(yīng)SLAM算法,該算法利用機(jī)器人觀測(cè)殘差序列的均值和協(xié)方差改變觀測(cè)噪聲協(xié)方差值;并運(yùn)用IAE開(kāi)窗法和新息預(yù)測(cè)值得到系統(tǒng)狀態(tài)噪聲協(xié)方差縮放因子,進(jìn)而求取系統(tǒng)噪聲方差。

2 問(wèn)題描述

SLAM的基本含義是機(jī)器人定位和環(huán)境建模同時(shí)進(jìn)行,一方面機(jī)器人利用已經(jīng)創(chuàng)建好的環(huán)境模型矯正機(jī)器人的位置狀態(tài),從而提高機(jī)器人的定位精度;另一方面機(jī)器人依靠可靠的機(jī)器人位置,建立出精度更高的環(huán)境模型。其問(wèn)題可以描述為:當(dāng)機(jī)器人進(jìn)入一個(gè)未知環(huán)境中,k時(shí)刻的機(jī)器人位置狀態(tài)為s(k),k時(shí)刻的地圖特征為m(k),故機(jī)器人的狀態(tài)向量表示為X(k)=(s(k),m(k))T,u(k)表示時(shí)刻k-1到k時(shí)刻的運(yùn)動(dòng)控制輸入,Z(k)表示為k時(shí)刻的系統(tǒng)觀測(cè)向量。SLAM其本質(zhì)可以理解為在貝葉斯框架下的機(jī)器人定位和環(huán)境模型的預(yù)估問(wèn)題, SLAM用概率描述可以表示為:

根據(jù)貝葉斯定理可知:

其中,η為歸一化常數(shù);p(Z(k)|X(k))為機(jī)器人觀測(cè)模型;p(X(k)|u(k),Z(k))為機(jī)器人運(yùn)動(dòng)模型,也就是根據(jù)觀測(cè)模型的概率和運(yùn)動(dòng)模型的概率分布遞歸的求其機(jī)器人的位置狀態(tài)和環(huán)境地圖模型。

針對(duì)第1節(jié)中所提到的EKF-SALM算法的不足和缺點(diǎn),文獻(xiàn)[12]提出了UKF算法,其算法的核心是采用SUT變換和確定性采樣策略逼近非線性系統(tǒng)?；镜脑硎窃谙到y(tǒng)狀態(tài)分布中選取一定量的采樣點(diǎn),使這些采樣點(diǎn)的期望值和協(xié)方差等于系統(tǒng)狀態(tài)分布的期望值和協(xié)方差;然后將這些點(diǎn)帶入到非線性系統(tǒng)函數(shù)中,就得到了相應(yīng)的非線性函數(shù)點(diǎn)集,最后通過(guò)得到的點(diǎn)集求取變換后的期望值和協(xié)方差。

根據(jù)以上關(guān)于SLAM問(wèn)題的描述,設(shè)機(jī)器人的狀態(tài)方程和觀測(cè)方程分別為:

其中,系統(tǒng)狀態(tài)噪聲均值為w(k);協(xié)方差為Q(k);系統(tǒng)的觀測(cè)噪聲均值為v(k);協(xié)方差為R(k)。

在利用UT變換算法時(shí),首要任務(wù)是確定Sigma點(diǎn)的采樣策略。目前提出的采樣策略有單行采樣,對(duì)稱采樣等[13]。為了解決非局部效應(yīng),本文采用的是比例對(duì)稱采樣[6-7],利用UKF算法求其狀態(tài)向量的均值和協(xié)方差Px,假設(shè)X(k)為n維向量,那么選取采樣個(gè)數(shù)為L(zhǎng)=2n+1按照比例對(duì)稱策略進(jìn)行Sigma點(diǎn)近似,其遵循如下的條件函數(shù)來(lái)確保能夠準(zhǔn)確地抓取Xi的必要特征。

遵循上述條件函數(shù),求得比例修正的對(duì)稱采樣Sigma點(diǎn)χi′為:

那么X(k)對(duì)應(yīng)的均值和協(xié)方差矩陣可近似的表示為:

以上就是UKF算法對(duì)均值和協(xié)方差的非線性變換計(jì)算,根據(jù)算法可以發(fā)現(xiàn)相對(duì)于EKF算法有至少存在2階的估計(jì)精度,不需要計(jì)算雅克比矩陣,算法比EKF更容易實(shí)現(xiàn);而相對(duì)于粒子濾波器,采用的粒子點(diǎn)是較少的,計(jì)算量更小。

3 UKF-SLAM算法

系統(tǒng)狀態(tài)噪聲和觀測(cè)噪聲的協(xié)方差對(duì)于UKF算法在SLAM提高濾波精度,定位精度和地圖建模精度尤為重要。因?yàn)镼(k)和R(k)直接影響著當(dāng)前時(shí)刻狀態(tài)信息和上一時(shí)刻狀態(tài)信息之間的權(quán)值。而確定R(k)和Q(k)的一般是依靠系統(tǒng)狀態(tài)噪聲和觀測(cè)噪聲的先驗(yàn)知識(shí)。根據(jù)前面介紹的UKF算法,如果均值X(0)和協(xié)方差P(0)取值存在誤差,那么UKF算法的第2步Sigma點(diǎn)測(cè)采用就會(huì)受到影響,進(jìn)而影響算法的預(yù)測(cè)部分;而且經(jīng)系統(tǒng)的觀測(cè)方程,使得觀測(cè)量Z(k)會(huì)存在一定的誤差。

經(jīng)過(guò)以上分析可以看出如果R(k)和Q(k)的不確定性將對(duì)整個(gè)算法的濾波精度產(chǎn)生較大影響,進(jìn)而影響SLAM的定位精度和地圖建模精度。

針對(duì)在SLAM中UKF算法出現(xiàn)的系統(tǒng)狀態(tài)噪聲和觀測(cè)噪聲模型不確定性,所導(dǎo)致的UKF算法收斂速度變慢,估計(jì)精度降低,甚至出現(xiàn)濾波發(fā)散等問(wèn)題,本文提出了一種在線自適應(yīng)調(diào)整噪聲模型并同時(shí)控制系統(tǒng)噪聲模型異常干擾源對(duì)濾波效果產(chǎn)生影響的算法。

3.1 噪聲自適應(yīng)的UKF算法

根據(jù)UKF算法和機(jī)器人的狀態(tài)方程和觀測(cè)方程,首先定義觀測(cè)新息值矩陣d(k)和觀測(cè)殘差值矩陣ε(k):

其中,d(k)是由k時(shí)刻的預(yù)測(cè)狀態(tài)值和輸入值所推算出來(lái)的;而ε(k)是由k時(shí)刻的更新?tīng)顟B(tài)值和輸入值推算出來(lái)的?？梢钥闯鰀(k)僅僅使用了預(yù)測(cè)值來(lái)求,所以帶有噪聲不確定性帶來(lái)的誤差,可以反映出系統(tǒng)所受到的擾動(dòng)。

根據(jù)觀測(cè)噪聲模型和UKF算法,對(duì)ε(k)進(jìn)行處理,得到其均值和協(xié)方差如下所示:

其中,P(k)為系統(tǒng)方程的協(xié)方差矩陣的更新值,根據(jù)UKF算法可以求出,進(jìn)而可以估計(jì)出觀測(cè)噪聲協(xié)方差矩陣R(k)。

基于觀測(cè)協(xié)方差的自適應(yīng)算法一般采用的是開(kāi)窗法,有IAE濾波和RAE濾波2種[14],IAE濾波使用m個(gè)歷史新息值來(lái)推算當(dāng)前新息的協(xié)方差矩陣估計(jì)值(k):

一個(gè)最優(yōu)的濾波器應(yīng)該滿足,通過(guò)上面開(kāi)窗法IAE預(yù)測(cè)的k時(shí)刻的協(xié)方差值(k)和UKF算法所預(yù)測(cè)的P(k|k-1)基本保持一致。即:

由于觀測(cè)模型噪聲矩陣R(k)不確定性帶來(lái)的誤差,將會(huì)導(dǎo)致(k)和預(yù)測(cè)的系統(tǒng)狀態(tài)協(xié)方差P(k|k-1)并不相等。即:

式(16)和式(17)可以用來(lái)判斷系統(tǒng)噪聲模型是否發(fā)生改變,分以下2種情況討論觀測(cè)模型是否發(fā)生變化。

(1)如果系統(tǒng)的狀態(tài)噪聲模型沒(méi)有發(fā)生改變,式(13)是成立的。

根據(jù)以上分析,只要知道UKF算法k時(shí)刻預(yù)測(cè)狀態(tài)協(xié)方差值P(k|k-1)和利用開(kāi)窗法LAE所預(yù)測(cè)的k時(shí)刻的協(xié)方差值(k),那么就可以根據(jù)式(13)和式(14)判斷系統(tǒng)狀態(tài)噪聲模型有沒(méi)有發(fā)生變化,進(jìn)而可以知道系統(tǒng)狀態(tài)噪聲模型的具體變化情況。

定義縮放因子β(k)為:

根據(jù)式(16),系統(tǒng)狀態(tài)噪聲的協(xié)方差Q(k)可以表示為:

縮放因子β(k)取平方根是為了噪聲更加平滑,使得整個(gè)系統(tǒng)噪聲模型更加平滑,其可以取大于1也可以取小于1的數(shù)值,使得更加充分的調(diào)節(jié)噪聲模型。

3.2 噪聲自適應(yīng)的UKF-SLAM算法

根據(jù)UKF算法原理和噪聲自適應(yīng)的UKF算法原理,從而實(shí)現(xiàn)噪聲自適應(yīng)的UKF-SLAM算法,具體步驟如下:

(1)系統(tǒng)初始化,初始化k=0時(shí)刻的均值、協(xié)方差。

(2)對(duì)于i=1,2,…,2n,進(jìn)行Sigma點(diǎn)采樣。采用策略是比例對(duì)稱采樣?？蓞⒖际?4)到式(8);

(4)首先按式(11)確定系統(tǒng)觀測(cè)噪聲協(xié)方差R(k),然后根據(jù)觀測(cè)模型對(duì)第(3)步的預(yù)測(cè)部分進(jìn)行更新,設(shè)Pzz為觀測(cè)模型的協(xié)方差,Pxz為系統(tǒng)狀態(tài)模型和觀測(cè)模型之間的協(xié)方差,K(k)為濾波增益。

其更新方程如下:

4 系統(tǒng)建模與實(shí)驗(yàn)分析

4.1 系統(tǒng)建模

系統(tǒng)的模型主要包括機(jī)器人坐標(biāo)中的模型、機(jī)器人運(yùn)動(dòng)模型、機(jī)器人控制模型、傳感器觀測(cè)模型、系統(tǒng)噪聲模型,本實(shí)驗(yàn)中采用的模型來(lái)自于悉尼大學(xué)的經(jīng)典數(shù)據(jù)庫(kù)[15],其中機(jī)器人坐標(biāo)中的模型如圖1所示。

圖1 機(jī)器人坐標(biāo)中的模型

機(jī)器人運(yùn)動(dòng)模型如下:

機(jī)器人觀測(cè)模型如下:

那么k時(shí)刻m個(gè)特征觀測(cè)量為機(jī)器人的觀測(cè)向量Z(k),如下所示:

4.2 實(shí)驗(yàn)環(huán)境

實(shí)驗(yàn)是在PC機(jī)上用MATLAB編寫軟件仿真進(jìn)行的,實(shí)驗(yàn)所構(gòu)造的環(huán)境區(qū)域?yàn)?0×40的地圖。機(jī)器人從起始狀態(tài)S(0)=(0.005,0.005,0.001)開(kāi)始步進(jìn),其仿真參數(shù)如表1所示。

表1 仿真參數(shù)

記NAUKF-SLAM為噪聲自適應(yīng)UKF-SLAM算法。對(duì)于UKF-SLAM算法和NAUKF-SLAM算法,用到的參數(shù)設(shè)定為:α取1,β取2,κ取3-n。

4.3 實(shí)驗(yàn)結(jié)果分析

在以上的實(shí)驗(yàn)環(huán)境,設(shè)定系統(tǒng)狀態(tài)噪聲δd(k)為2 cm,δθ(k)為0.5°,觀測(cè)噪聲δd(k)為0.1 m,δθ(k)為 1°。對(duì) EKF-SLAM、UKF-SLAM 和 NAUKFSLAM算法分別進(jìn)行仿真實(shí)驗(yàn),并對(duì)以上實(shí)驗(yàn)結(jié)果進(jìn)行分析和比較。

3種算法分別在系統(tǒng)狀態(tài)噪聲和觀測(cè)噪聲在服從上述噪聲分布的情況下,機(jī)器人步進(jìn)2 000次建立的路徑估計(jì)曲線和地圖特征估計(jì)點(diǎn),其仿真效果分別如圖2～圖4所示。該仿真圖是在同一地圖中進(jìn)行的,機(jī)器人真實(shí)路徑和地圖路標(biāo)是相同的,其中實(shí)線為機(jī)器人實(shí)際行走的軌跡;虛線為算法估計(jì)的路徑;+表示算法估計(jì)的路標(biāo);◇表示實(shí)際路標(biāo)的位置。

圖2 EKF-SLAM算法仿真結(jié)果

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

圖4 NAUKF-SLAM算法仿真結(jié)果

從效果圖可以看出UKF-SLAM算法對(duì)機(jī)器人位姿的估計(jì)和對(duì)地圖的建模比EKF更加準(zhǔn)確,因?yàn)閁KF-SLAM算法至少可以達(dá)到2階的定位精度和環(huán)境建模精度;而NAUKF-SLAM算法對(duì)機(jī)器人路徑估計(jì)基本和實(shí)際的運(yùn)動(dòng)路線相吻合,并且從地圖特征的可信區(qū)域可以看出NAUKF-SLAM對(duì)地圖特征的估計(jì)和實(shí)際的地圖特征基本重合,實(shí)現(xiàn)了對(duì)機(jī)器人位姿的精確估計(jì)和環(huán)境模型的精確建模,因?yàn)樵贜AUKF-SLAM算法中所建立的噪聲自適應(yīng)模型可以實(shí)時(shí)的調(diào)節(jié)其噪聲值。此外,由于提供的噪聲是服從高斯分布的白噪聲,所以3種算法最后都是收斂的,精度都逐漸提高。

為了驗(yàn)證NAUKF-SLAM算法對(duì)噪聲的自適應(yīng)性,3種算法在同一個(gè)地圖中進(jìn)行實(shí)驗(yàn):地圖大小為40 m×40 m,機(jī)器人運(yùn)行時(shí)間為500 s,特征點(diǎn)數(shù)為40個(gè)。為了更好的比較3種算法性能,求取算法的估計(jì)值和真實(shí)值之間的誤差。算法的估計(jì)值包括對(duì)機(jī)器人位置的估計(jì)、機(jī)器人姿態(tài)的估計(jì)以及對(duì)環(huán)境特征點(diǎn)的估計(jì)。每種算法都是在同一地圖上進(jìn)行N次實(shí)驗(yàn),采樣歸一化誤差來(lái)表示其估計(jì)值和真實(shí)值之間的關(guān)系,如對(duì)機(jī)器人位置的估計(jì)誤差關(guān)系為:E(S(k),S*(k)),S(k)表示機(jī)器人實(shí)際的路徑,S*(k)表示算法估計(jì)的機(jī)器人路徑,其公式如下:

其中,N=50。

首先在上述的噪聲不變的情況下,添加(0,1)正態(tài)分布的環(huán)境噪聲,對(duì)3種算法進(jìn)行實(shí)驗(yàn),其3種誤差分別對(duì)應(yīng)圖5～圖7。雖然添加了高斯噪聲,造成了對(duì)系統(tǒng)噪聲模型和觀測(cè)模型的影響,但是NAUKF-SLAM的定位精度和環(huán)境特征點(diǎn)估計(jì)精度都較好,體現(xiàn)出其對(duì)噪聲的自適應(yīng)性。

圖5 高斯分布噪聲下機(jī)器人位置平均估計(jì)誤差

圖6 高斯分布噪聲下機(jī)器人姿態(tài)平均估計(jì)誤差

圖7 高斯分布噪聲下特征點(diǎn)平均估計(jì)誤差

為了進(jìn)一步驗(yàn)證NAUKF-SLAM對(duì)噪聲的自適應(yīng)性,取2種不同的噪聲對(duì)其實(shí)驗(yàn)。

添加(0,1)均勻分布的環(huán)境噪聲,然后對(duì)3種算法進(jìn)行實(shí)驗(yàn),其3種誤差如圖8～圖10所示。從圖中可以看出3種算法的誤差都有所增加,但是NAUKF-SLAM算法誤差曲線基本在其他2種算法曲線下,其定位精度和地圖建模精度更高。

圖8 均勻分布噪聲下機(jī)器人位置平均估計(jì)誤差

圖9 均勻分布噪聲下機(jī)器人姿態(tài)平均估計(jì)誤差

添加參數(shù)為0.01的指數(shù)分布的環(huán)境噪聲,其3種誤差分別對(duì)應(yīng)圖11～圖13。雖然加入了指數(shù)噪聲,3種算法的誤差也都有所增加,但是NAUKFSLAM誤差曲線基本在其他2種算法誤差曲線以下,其定位精度和地圖建模精度上更高。

圖10 均勻分布噪聲下特征點(diǎn)平均估計(jì)誤差

圖11 指數(shù)分布噪聲下機(jī)器人位置平均估計(jì)誤差

圖12 指數(shù)分布噪聲下機(jī)器人位姿平均估計(jì)誤差

圖13 指數(shù)分布噪聲下特征點(diǎn)平均估計(jì)誤差

綜合以上的實(shí)驗(yàn)分析,可以看出隨著噪聲的增大或者減小,UKF-SLAM算法其誤差比EKF-SLAM算法較小,這是因?yàn)镋KF-SLAM算法僅僅保證了1階的精度,而UKF-SLAM至少有2階精度的存在。NAUKF-SLAM算法通過(guò)實(shí)時(shí)計(jì)算觀測(cè)殘差值的均值和協(xié)方差改變觀測(cè)噪聲協(xié)方差R(k),利用開(kāi)窗法和新息預(yù)測(cè)值實(shí)時(shí)計(jì)算出狀態(tài)噪聲縮放因子β(k)進(jìn)而改變系統(tǒng)狀態(tài)噪聲協(xié)方差Q(k),以實(shí)現(xiàn)更加精確的機(jī)器人定位和地圖建模。

5 結(jié)束語(yǔ)

針對(duì)EKF算法在SLAM中由于噪聲模型的改變所導(dǎo)致的精度降低甚至發(fā)散的問(wèn)題,本文提出了一種通過(guò)改變?cè)肼暷Ｐ偷淖赃m應(yīng)的UKF-SLAM算法(NAUKF-SLAM),通過(guò)實(shí)時(shí)觀測(cè)新息的均值和協(xié)方差來(lái)自適應(yīng)觀測(cè)噪聲模型R(k),以及通過(guò)IAE開(kāi)窗法和新息協(xié)方差預(yù)測(cè)值計(jì)算出縮放因子β(k),進(jìn)而自適應(yīng)系統(tǒng)狀態(tài)噪聲模型Q(k),以達(dá)到對(duì)系統(tǒng)噪聲模型的改變,或者有其他環(huán)境噪聲加入時(shí),能夠自適應(yīng)地調(diào)節(jié)其噪聲模型,進(jìn)而提高機(jī)器人SLAM精度。實(shí)驗(yàn)結(jié)果表明,本文算法SLAM精度優(yōu)于EKFSLAM和UKF-SLAM算法,一定程度上滿足了SLAM的精度要求。但是該算法對(duì)系統(tǒng)噪聲Q(k)是全局縮放的,其縮放因子β(k)沒(méi)有分配到每個(gè)單獨(dú)的狀態(tài)分量,可能對(duì)其精度有一定的影響,因此,下一步將在本文算法基礎(chǔ)上,對(duì)UKF和粒子濾波器結(jié)合同時(shí)定位和環(huán)境建模進(jìn)行研究。

[1] Durrant-Whyte H,Bailey T.Simultaneous Localization and Mapping:Part I[J].IEEE Robotics and Automation Magazine,2006,13(2):99-108.

[2] Dissanayake M W M G,Newman P,Clark S,et al.A Solution to the Simultaneous Localization and Mapbuilding(SLAM)Problem[J].Transactions of Robotics and Automation,2001,17(3):229-241.

[3] Singh S.Noise Impact on Time-series Forecasting Using an Intelligent Pattern Matching Technique[J].Pattern Recognition,1999,32(8):1389-1398.

[4] Andrade-Cetto J,Vidal-Calleja T,Sanfeliu A.Unscented Transfor mation ofVehicleStatesin Slam[C]// Proceedings of IEEE International Conference on Robotics and Automation.Barcelona,Spain:[s.n.],2005:324-329.

[5] Juliter S J.The SphericalSimplex Unscented Transformation[C]//Proceedings of American Control Conference.Denver,USA:[s.n.],2003:2430-2434.

[6] Wan E A,vander Merwe R.The Unscented Kalman Filter, in Kalman Filtering and Neural Networks[EB/OL].(2004-03-10).http://www.cse.ogi.edu/PacSoft/projects/sec/ wan01b.ps.

[7] 康軼非.平方根容積卡爾曼濾波在移動(dòng)機(jī)器人SLAM中的應(yīng)用[J].機(jī)器人,2013,35(2):186-193.

[8] 張國(guó)良,湯文俊.基于線段特征匹配的EKF-SLAM算法[J].控制工程,2012,10(6):119-124.

[9] Wang X,Zhang H.A UPF-UKF Framework for SLAM [C]//Proceedings of IEEE International Conference on Roboticsand Automation.Piscataway,USA:IEEE, 2007:1664-1669.

[10] Kim C,SakthivelR,ChungW K.Unscented Fast SLAM:A Robust and Efficient Solution to the SLAM Problem[J].IEEE Transactions on Robotics,2008,24 (4):808-820.

[11] Shojaie K,ShahriA M.Iterated Unscented SLAM Algorithm for Nacigation of an Autonomous Mobile Robot[C]//Proceedings of IEEE/RSJ Internation Conference on Intelligent Robots and Systems.Piscataway, USA:IEEE Press,2008:1582-1587.

[12] Julier S J,Uhlmann J K.Unscented Filtering and Nonlinear Estimation[J].Proceedings of the IEEE, 2004,92(3):401-422.

[13] Julier S J,Uhlmann J K.A Consistant,Debiased Method for Converting Between Polar and Cartesian Coordinate Systems[C]//Proceedings of the 11th International Symposium on Aerospace Defense Sensing,Simulation and Controls.Orlando,USA:Press,1997:110-121.

[14] Mohamed A H,Schwarz K P.Adaptive Kalman Filtering for INS/GPS[J].Journal of Geodesy,1999,73(4): 193-203.

[15] Australian Centre for Field Robotics.Source Code [EB/OL].(2008-06-10).http://www-personal.acfr. usyd.edu.au/tbailey/.

編輯 索書(shū)志

Adaptive UKF-SLAM Algorithm Based on Noise Scaling

WANG Zu-lin,QIN Song,LIANG Yu-ming
(Faculty of Electronic Engineering and Automation,Jiangxi University of Science and Technology,Ganzhou 341000,China)

For Extend Kalman Filtering(EKF)algorithm disadvantage on the Simultaneous Location and Mapping

(SLAM),that is simple linearization of nonlinear systems resulting from the inaccuracy of the system state equation,Jacobi matrix calculation resulting from computational complexity,and noise uncertainty caused by the filtering reduced stability and other issues,this paper proposes a noise adaptive UKF-SLAM algorithm.In order to achieve adaptive UKF-SLAM algorithm,the paper scales the noise to change the noise model.Using the observed innovation sequence to accurately estimate the covariance of the measurement noise model.And using a new message convariance and IAE fenestration to find the system noise scaling factor,and thus accurately setimates the convariance of the system state noise model,it achieves a adaptive UKF-SLAM algorithm.The sampling strategy of UKF Sigma points is scaling symmetric sampling.Experimental results show that the algorithm has a high accuracy on SLAM compared with the EKF-SLAM and UKF-SLAM.

Simultaneous Location and Mapping(SLAM);Unscented Kalman Filtering(UKF);noise scaling;online adaptive;scale symmetric sampling;windowing method

1000-3428(2014)10-0143-07

A

TP18

10.3969/j.issn.1000-3428.2014.10.028

國(guó)家自然科學(xué)基金資助項(xiàng)目(61262013);江西省教育廳科技計(jì)劃基金資助項(xiàng)目(GJ11133)。

王祖麟(1954-),男,教授,主研方向:智能機(jī)器人,嵌入式系統(tǒng);秦 菘(通訊作者),碩士;梁毓明,講師、博士。

2013-10-16

2013-12-03E-mail:chinaqinsong@sina.cn

中文引用格式:王祖麟,秦 菘,梁毓明.基于噪聲縮放的自適應(yīng)UKF-SLAM算法[J].計(jì)算機(jī)工程,2014,40(10): 143-149,154.

英文引用格式:Wang Zulin,Qin Song,Liang Yuming.Adaptive UKF-SLAM Algorithm Based on Noise Scaling[J]. Computer Engineering,2014,40(10):143-149,154.