一種改進(jìn)的UKF-SLAM算法

呂太之， 周 武， 趙春霞

(1. 江蘇海事學(xué)院 信息工程學(xué)院， 南京 210017； 2. 南京理工大學(xué) 計(jì)算機(jī)科學(xué)與技術(shù)學(xué)院， 南京 210094；3. 浙江師范大學(xué)工學(xué)院， 浙江 金華 321019)

0 引 言

自21世紀(jì)初以來(lái)， 國(guó)內(nèi)外對(duì)機(jī)器人技術(shù)的發(fā)展越來(lái)越重視， 使其成為當(dāng)今前沿高新技術(shù)研究最活躍的領(lǐng)域之一[1]. 同時(shí)定位與地圖創(chuàng)建(simultaneous localization and mapping, SLAM)具有重要的理論和應(yīng)用價(jià)值， 是實(shí)現(xiàn)移動(dòng)機(jī)器人自主導(dǎo)航的關(guān)鍵. SLAM已經(jīng)在移動(dòng)機(jī)器人、 無(wú)人車(chē)輛、 無(wú)人水下航行器、 無(wú)人機(jī)、 增強(qiáng)現(xiàn)實(shí)等領(lǐng)域得到廣泛應(yīng)用[2-3]. 然而， SLAM技術(shù)仍然面臨很多挑戰(zhàn)， 如復(fù)雜的大環(huán)境、 可靠的數(shù)據(jù)融合、 非線(xiàn)性和未知的先驗(yàn)知識(shí)等[4-6].

SLAM問(wèn)題的常用概率解決方案有兩類(lèi)， 基于卡爾曼濾波(Kalman Filter, KF)和基于粒子濾波(Particle Filter, PF)的SLAM算法[6]. 擴(kuò)展卡爾曼濾波(Extended Kalman Filter, EKF)將非線(xiàn)性化的系統(tǒng)線(xiàn)性化， 來(lái)應(yīng)用于SLAM問(wèn)題的解決. EKF-SLAM在機(jī)器人導(dǎo)航領(lǐng)域被廣泛應(yīng)用， 相繼在室內(nèi)、 結(jié)構(gòu)化道路、 水下等場(chǎng)合取得了成功. Bailey等人對(duì)EKF-SLAM算法進(jìn)行了詳細(xì)的論述和實(shí)驗(yàn)驗(yàn)證， 同時(shí)對(duì)EKF-SLAM算法進(jìn)行了優(yōu)化， 提高了狀態(tài)向量維數(shù)很高時(shí)算法的效率[7]. 當(dāng)路標(biāo)數(shù)量較多時(shí)， EKF-SLAM計(jì)算量很大， 同時(shí)由于EKF在線(xiàn)性化的過(guò)程中， 不可避免的引入了線(xiàn)性化誤差， 這樣狀態(tài)向量的估計(jì)就難以達(dá)到最優(yōu). 為了降低計(jì)算復(fù)雜度和減少線(xiàn)性化誤差， 研究人員提出一系列改進(jìn)方法. 降低計(jì)算復(fù)雜度的方法有不變擴(kuò)展卡爾曼濾波(Invariant Extended Kalman Filter, IEKF)[8]、 平均擴(kuò)展卡爾曼濾波(Mean Extended Kalman Filter, MEKF)[9]等. 對(duì)于提高算法精確度方面主要從運(yùn)動(dòng)模型的改進(jìn)和減少由線(xiàn)性化帶來(lái)的誤差著手. 無(wú)跡卡爾曼濾波(Unscented Kalman Filter, UKF)使用UT(Unscented Transform)變換通過(guò)Sigma點(diǎn)重構(gòu)出狀態(tài)向量的統(tǒng)計(jì)特性(均值和誤差)， 為非線(xiàn)性化的SLAM問(wèn)題提供了一種解決思路. Martinez-Cantin等[10]提出將UKF應(yīng)用于機(jī)器人位姿和地圖的估計(jì). 曲麗萍[11]等提出基于平方根的無(wú)跡卡爾曼濾波， 在具有與UKF濾波相同精度的情況下， 確保了計(jì)算的穩(wěn)定性和協(xié)方差矩陣的半正定性. 在UKF-SLAM的基礎(chǔ)上， 學(xué)者們提出迭代平方根的UKF-SLAM[12]、 AUKF-SLAM(Augmented UKF-SLAM)[13]等算法.

除線(xiàn)性化誤差和計(jì)算復(fù)雜度， 基于卡爾曼濾波的SLAM算法中一個(gè)不容忽視的問(wèn)題就是外部干擾. 在真實(shí)環(huán)境中， 移動(dòng)機(jī)器人輪胎打滑、 遇到障礙物導(dǎo)致前進(jìn)受阻、 突發(fā)外力導(dǎo)致位置發(fā)生大的變化等外部干擾都會(huì)對(duì)SLAM產(chǎn)生較大的影響， 因此降低其魯棒性， 甚至導(dǎo)致定位和地圖創(chuàng)建的失敗. 傳統(tǒng)的卡爾曼濾波算法在受到外部干擾的情況下會(huì)導(dǎo)致誤差急劇增加， 但隨著不斷的觀測(cè)來(lái)更新?tīng)顟B(tài)， 誤差會(huì)逐漸減少， 所以算法不需要考慮外部干擾的問(wèn)題. 但卡爾曼濾波算法應(yīng)用于SLAM問(wèn)題上， 外部干擾在影響移動(dòng)機(jī)器人定位的同時(shí)也會(huì)影響到環(huán)境地圖. 環(huán)境地圖和移動(dòng)機(jī)器人定位相互影響， 不會(huì)隨著觀測(cè)來(lái)減少誤差， 而是進(jìn)一步累計(jì)誤差. 傳統(tǒng)UKF-SLAM算法并沒(méi)有考慮外部干擾的影響， 因此， 當(dāng)發(fā)生外部干擾情況的時(shí)候， UKF-SLAM算法會(huì)導(dǎo)致估計(jì)的不確定性遠(yuǎn)小于真實(shí)情況， 從而降低了SLAM的精度和魯棒性. 為了減少外部干擾對(duì)SLAM算法的影響， 近幾年各種不同的算法被提出. Ting[14]等通過(guò)對(duì)觀測(cè)數(shù)據(jù)采用增量期望值最大的算法來(lái)檢測(cè)外部干擾. Havangi[15]提出改進(jìn)的模糊神經(jīng)網(wǎng)絡(luò)算法來(lái)動(dòng)態(tài)調(diào)整先驗(yàn)知識(shí)的協(xié)方差. 然而該算法只調(diào)整觀測(cè)誤差的協(xié)方差矩陣R， 同時(shí)也容易受到累計(jì)誤差的影響. Choi[16]等提出基于協(xié)方差膨脹的移動(dòng)均值(Shifted Mean Based Covariance Inflation Technique, SMCI)方法來(lái)降低外部干擾的影響. SMCI-SLAM算法通過(guò)方差膨脹來(lái)減少外部干擾的影響， 但效果在下一個(gè)周期才能體現(xiàn)， 同時(shí)沒(méi)有區(qū)別對(duì)待不同類(lèi)型的外部干擾， 也容易受累計(jì)誤差的影響.

基于前期研究成果[17]， 針對(duì)外部干擾會(huì)導(dǎo)致基于UKF-SLAM算法精度降低甚至發(fā)散的問(wèn)題， 提出了一種改進(jìn)的UKF-SLAM算法. 算法主要特點(diǎn)在于:

1) 繼承抗外部干擾的EKF-SLAM算法， 將其應(yīng)用于UKF-SLAM算法， 通過(guò)臨近觀測(cè)分析來(lái)檢測(cè)外部干擾， 不會(huì)受累計(jì)誤差的影響， 提高了外部干擾檢測(cè)的精度.

2)不僅處理干擾對(duì)控制輸入的影響， 也處理干擾對(duì)觀測(cè)過(guò)程的影響. 將干擾分類(lèi)處理， 根據(jù)不同類(lèi)型的外部干擾使用不同方式來(lái)膨脹系統(tǒng)狀態(tài)方差， 擴(kuò)大其不確定性， 使系統(tǒng)狀態(tài)迅速收斂到真值.

3)改進(jìn)干擾檢測(cè)算法， 根據(jù)臨近觀測(cè)對(duì)比以及控制噪聲和觀測(cè)噪聲協(xié)方差矩陣來(lái)判斷是否發(fā)生小概率事件， 以此確定SLAM過(guò)程是否存在外部干擾.

1 SLAM問(wèn)題描述

SLAM問(wèn)題可以描述為機(jī)器人在未知環(huán)境中從一個(gè)未知的位置開(kāi)始移動(dòng)， 在不斷的移動(dòng)中根據(jù)控制信息和傳感器觀測(cè)進(jìn)行自身定位， 同時(shí)構(gòu)建增量式地圖. SLAM的主要目標(biāo)就是基于一組從開(kāi)始到t時(shí)刻帶有誤差的控制數(shù)據(jù)u1:t和觀測(cè)數(shù)據(jù)z1:t中獲得對(duì)移動(dòng)機(jī)器人位姿和地圖信息最好的估計(jì). SLAM問(wèn)題可以描述為如下的后驗(yàn)概率.

p(xi,Θ|z1:t,u1:t),(1)

式中：xt是當(dāng)前機(jī)器人的位姿；Θ是環(huán)境地圖.xt可以通過(guò)上一時(shí)刻位姿xt-1和當(dāng)前時(shí)刻控制數(shù)據(jù)ut的概率函數(shù)來(lái)表示， 稱(chēng)之為運(yùn)動(dòng)模型.

xt=g(xt-1,ut)+εt,(2)

式中：εt為服從高斯分布的運(yùn)動(dòng)模型噪聲. 通過(guò)傳感器獲得的觀測(cè)數(shù)據(jù)同樣可以被概率函數(shù)表示， 稱(chēng)之為觀測(cè)模型.

zt=h(xt,Θ)+δt,(3)

式中：δt為服從高斯分布的觀測(cè)模型噪聲.

基于運(yùn)動(dòng)和觀測(cè)模型， SLAM問(wèn)題的后驗(yàn)概率可以使用貝葉斯濾波器迭代更新， 式(1)改寫(xiě)成

p(xt,Θ|z1:t,u1:t)=ηp(zt|xt,Θ,z1:t,u1:t),

p(xt,Θ|z1:t-1,u1:t,x0).(4)

卡爾曼濾波和粒子濾波都是典型的貝葉斯濾波. 卡爾曼濾波是將SLAM后驗(yàn)概率描述為均值為xt， 協(xié)方差為Pt的多維高斯分布. 粒子濾波則是用有限的樣本集合來(lái)代表SLAM的后驗(yàn)分布.

2 UKF-SLAM算法

EKF在迭代更新移動(dòng)機(jī)器人狀態(tài)時(shí)采用泰勒級(jí)數(shù)線(xiàn)性化系統(tǒng)模型， 不可避免地引入了線(xiàn)性化模型誤差， 同時(shí)雅可比矩陣的更新也增大了EKF的計(jì)算量. 無(wú)跡卡爾曼濾波(Unscented Kalman Filter, UKF)是用2L+1(L為系統(tǒng)狀態(tài)的維數(shù))個(gè)能表征系統(tǒng)狀態(tài)向量的Sigma點(diǎn)重構(gòu)出系統(tǒng)統(tǒng)計(jì)特性. 通過(guò)非線(xiàn)性系統(tǒng)模型來(lái)實(shí)現(xiàn)機(jī)器人系統(tǒng)狀態(tài)的更新， 提高了估計(jì)的精度， 同時(shí)該算法不需要推導(dǎo)系統(tǒng)的雅可比矩陣， 節(jié)省了計(jì)算時(shí)間. 基于此， UKF被廣泛應(yīng)用于SLAM問(wèn)題中[10, 18].

UKF-SLAM初始化過(guò)程包括均值、 協(xié)方差矩陣和Sigma點(diǎn)權(quán)值的設(shè)置.

x0=xinitP0=Pinit,(5)

通過(guò)Sigma點(diǎn)計(jì)算、 預(yù)測(cè)和觀測(cè)更新三個(gè)步驟完成狀態(tài)向量和協(xié)方差矩陣的迭代更新.

1) Sigma點(diǎn)計(jì)算

若t-1時(shí)刻系統(tǒng)狀態(tài)均值和協(xié)方差為xt-1和Pt-1， 首先增廣系統(tǒng)狀態(tài)向量得到如下均值和方差.

(8)

式中：Q和R分別代表控制噪聲和觀測(cè)噪聲的協(xié)方差矩陣. 根據(jù)增廣矩陣計(jì)算Sigma點(diǎn).

(10)

尺度常量η計(jì)算如下

(11)

2) 預(yù)測(cè)

根據(jù)上一時(shí)刻的Sigma點(diǎn)和運(yùn)動(dòng)模型預(yù)測(cè)t時(shí)刻的Sigma點(diǎn).

(12)

3) 更新

(15)

Kt=Pxtxt(Pxtzt)-1,(20)

3 改進(jìn)的UFK-SLAM算法

3.1 算法設(shè)計(jì)思路

在未知環(huán)境中探索的時(shí)候， 外部干擾會(huì)導(dǎo)致誤差突然增大， 從而降低SLAM估計(jì)精度和魯棒性. 在SLAM過(guò)程中， 移動(dòng)機(jī)器人會(huì)根據(jù)上一時(shí)刻的狀態(tài)和控制輸入來(lái)預(yù)測(cè)當(dāng)前位姿和路標(biāo)位置. 如果觀測(cè)傳感器觀測(cè)的數(shù)據(jù)與預(yù)測(cè)的數(shù)據(jù)一致， 系統(tǒng)狀態(tài)的更新是可靠的； 反之則是不可靠的， 有很大可能性發(fā)生了外部干擾. 外部干擾主要分為以下兩類(lèi)情況：

一類(lèi)是移動(dòng)機(jī)器人自身受到外部干擾， 導(dǎo)致此類(lèi)情況的主要原因有： ① 外力的作用而導(dǎo)致移動(dòng)機(jī)器人位姿發(fā)生較大的改變； ② 移動(dòng)機(jī)器人遇到障礙物或者地面潮濕等原因而導(dǎo)致車(chē)輪打滑使得從里程計(jì)讀取的數(shù)據(jù)不能真實(shí)反映其運(yùn)動(dòng)狀態(tài)； ③ 慣性測(cè)量單元發(fā)送故障， 讀取的是錯(cuò)誤數(shù)據(jù).

另一類(lèi)情況是路標(biāo)受到外部干擾， 導(dǎo)致此類(lèi)情況的主要原因有： ① 由于外力的作用導(dǎo)致路標(biāo)的自身位置發(fā)生了改變； ② 觀測(cè)傳感器發(fā)生故障， 讀取的是錯(cuò)誤數(shù)據(jù).

針對(duì)外部干擾影響SLAM估計(jì)精度問(wèn)題， 本文提出一種改進(jìn)的UKF-SLAM算法. 本文算法在UKF-SLAM算法基礎(chǔ)上增加干擾檢測(cè)和系統(tǒng)狀態(tài)方差膨脹的過(guò)程. 臨近觀測(cè)的不一致性能判斷出預(yù)測(cè)和觀測(cè)數(shù)據(jù)是否一致， 被用于外部干擾的檢測(cè). 當(dāng)檢測(cè)到存在外部干擾的時(shí)候， 針對(duì)不同類(lèi)型的外部干擾， 通過(guò)膨脹系統(tǒng)狀態(tài)協(xié)方差矩陣， 擴(kuò)大其不確定性， 使系統(tǒng)狀態(tài)迅速收斂到真值， 提高算法的估計(jì)精度和魯棒性.

3.2 干擾檢測(cè)和狀態(tài)方差膨脹

提出算法通過(guò)對(duì)比t-1時(shí)刻和t時(shí)刻的觀測(cè)來(lái)檢測(cè)移動(dòng)機(jī)器人是否受到外部干擾. 假定t-1時(shí)刻移動(dòng)機(jī)器人處于原點(diǎn)位置， 前進(jìn)的方向?yàn)閄軸.t-1時(shí)刻機(jī)器人觀測(cè)到的第j個(gè)路標(biāo)信息為zt-1(mj)= [lt-1(mj),βt-1(mj)]. 以t-1時(shí)刻移動(dòng)機(jī)器人位置為原點(diǎn)， 第j個(gè)坐標(biāo)的位置為

(23)

(24)

式中： ΔT為采樣周期；vt為車(chē)輛后軸中心處運(yùn)動(dòng)速度；αt為車(chē)輛的轉(zhuǎn)向角；L為兩軸的軸距. 通過(guò)平移和旋轉(zhuǎn)操作， 可以獲得第j個(gè)路標(biāo)以t時(shí)刻機(jī)器人為原點(diǎn)下的預(yù)測(cè)位置

(25)

最后通過(guò)臨近兩次觀測(cè)不一致性來(lái)確定移動(dòng)機(jī)器人或者路標(biāo)是否受到外部干擾.

1) 移動(dòng)機(jī)器人受到外部干擾

(27)

式中：Sm是觀測(cè)函數(shù)關(guān)于路標(biāo)位置的雅可比矩陣.Dlt(mj)～N(0,1)服從正態(tài)分布. 當(dāng)該值大于2.3即小概率事件發(fā)生的時(shí)候， 表示由于外部干擾的影響導(dǎo)致對(duì)路標(biāo)的觀測(cè)存在較大誤差的可能性較大. 根據(jù)觀測(cè)路標(biāo)數(shù)量、 控制和觀測(cè)誤差協(xié)方差矩陣設(shè)定閥值， 當(dāng)存在較大觀測(cè)誤差的路標(biāo)數(shù)量超過(guò)閥值， 就認(rèn)定移動(dòng)機(jī)器人受到外部干擾.

(28)

Dβt(mj)～N(0,1)服從正態(tài)分布. 當(dāng)對(duì)多個(gè)路標(biāo)觀測(cè)Dβt(mj)超過(guò)指定閥值并且角度偏移方向趨于一致的時(shí)候就認(rèn)定移動(dòng)機(jī)器人受到外部干擾.

當(dāng)判斷移動(dòng)機(jī)器人受到外部干擾的時(shí)候， 下一步的工作就是修復(fù)此類(lèi)干擾. 本文算法對(duì)移動(dòng)機(jī)器人位姿狀態(tài)的協(xié)方差矩陣進(jìn)行膨脹， 以此來(lái)加大機(jī)器人的不確定性， 讓系統(tǒng)狀態(tài)更傾向于觀測(cè)的結(jié)果， 從而在更新過(guò)程中使機(jī)器人更接近于真實(shí)的位置.

式中： avg(diffl)和avg(diffβ)代表路標(biāo)預(yù)測(cè)值和觀測(cè)值在距離和角度上的平均偏差值；Gut是運(yùn)動(dòng)模型關(guān)于控制輸入的雅可比矩陣.

2) 路標(biāo)受到外部干擾

當(dāng)只有單一路標(biāo)預(yù)測(cè)值與觀測(cè)值之間的距離差值超過(guò)閥值， 就認(rèn)定該路標(biāo)由于受到外部干擾而導(dǎo)致位置發(fā)生了改變. 假定第j個(gè)路標(biāo)受到外部干擾， 通過(guò)對(duì)該路標(biāo)的協(xié)方差矩陣Pm,j進(jìn)行膨脹,以此來(lái)加大該路標(biāo)估計(jì)的不確定性， 讓系統(tǒng)狀態(tài)更傾向于移動(dòng)機(jī)器人的位姿和其他路標(biāo)觀測(cè)的結(jié)果， 從而使得對(duì)系統(tǒng)狀態(tài)的更新更接近于真實(shí)值.

式中：diffl,j和diffβ,j代表第j個(gè)路標(biāo)預(yù)測(cè)值和觀測(cè)值在距離和角度上的偏差；Ht是觀測(cè)模型關(guān)于狀態(tài)向量的雅可比矩陣.

3.3 算法流程

本文算法改進(jìn)UKF-SLAM， 增加了干擾檢測(cè)的過(guò)程， 當(dāng)檢測(cè)到系統(tǒng)中存在外部干擾時(shí)， 通過(guò)對(duì)系統(tǒng)狀態(tài)方差的膨脹， 加大移動(dòng)機(jī)器人或路標(biāo)估計(jì)的不確定性， 提高了SLAM估計(jì)的精度. 算法1描述t時(shí)刻干擾檢測(cè)和狀態(tài)方差膨脹的流程.

算法1 干擾檢測(cè)和狀態(tài)方差膨脹

輸入：

t-1時(shí)刻協(xié)方差矩陣Pt-1，t-1時(shí)刻觀測(cè)zt-1，t時(shí)刻觀測(cè)zt

步驟：

∥如果臨近觀測(cè)相同路標(biāo)數(shù)量小于3， 不進(jìn)行外部干擾檢測(cè)

if (sameObservedLandmark(zt-1,zt)<3)

return;

end if

∥根據(jù)臨近兩次觀測(cè)檢測(cè)是否存在外部干擾

disturanceType=checkDisturance(zt-1,zt);

if disturanceType==ROBOTDISTURANCE

else if disturanceType==LANDMARKDISTRUANCE

end if

輸出：

本文算法在UFK-SLAM數(shù)據(jù)關(guān)聯(lián)和系統(tǒng)狀態(tài)更新流程之間增加了干擾檢測(cè)和狀態(tài)方差膨脹的過(guò)程. 由于是在系統(tǒng)狀態(tài)更新之前引入外部干擾檢測(cè)和狀態(tài)方差膨脹， 所以本文算法能在干擾發(fā)生的周期內(nèi)做出快速響應(yīng). 圖 1 描述了t時(shí)刻改進(jìn)UKF-SLAM算法更新系統(tǒng)狀態(tài)的流程.

圖 1 改進(jìn)UKF-SLAM算法流程圖Fig.1 The flowchart of the improved UKF-SLAM algorithm

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

為了測(cè)試算法性能， 設(shè)計(jì)了UKF-SLAM[10]、 SMCI-SLAM[16]和改進(jìn)UKF-SLAM算法的仿真對(duì)比實(shí)驗(yàn). 移動(dòng)機(jī)器人t時(shí)刻的運(yùn)動(dòng)和觀測(cè)模型采用式(31)和(32). 給定控制輸入ut=[vt,αt]，vt是車(chē)輛后軸中心處運(yùn)動(dòng)速度，αt是車(chē)輛的轉(zhuǎn)向角. 移動(dòng)機(jī)器人運(yùn)動(dòng)模型可表示為

式中： ΔT為采樣周期；ε為控制噪聲；L為兩軸的軸距. SLAM問(wèn)題假定控制噪聲服從高斯分布.

觀測(cè)數(shù)據(jù)由路標(biāo)的距離lt(mj)和方向角βt(mj)組成.t時(shí)刻移動(dòng)機(jī)器人觀測(cè)方程為

(32)

4.1 移動(dòng)機(jī)器人受到外部干擾

設(shè)計(jì)一個(gè)大小為250 m×200 m的包含75個(gè)路標(biāo)的仿真環(huán)境， 如圖 2(a)所示， 移動(dòng)機(jī)器人的初始位置為(0,0).

圖 2 三種算法在機(jī)器人受到外部干擾下的實(shí)驗(yàn)結(jié)果Fig.2 Comparison between the three algorithms with outlier disturbances on robots

移動(dòng)機(jī)器人在此環(huán)境運(yùn)動(dòng)過(guò)程中， 每100個(gè)控制周期會(huì)隨機(jī)產(chǎn)生3～5次外部干擾. 由于外部干擾的存在導(dǎo)致移動(dòng)機(jī)器人實(shí)際的位姿改變與控制輸入相差甚遠(yuǎn)， 從而影響機(jī)器人定位和地圖創(chuàng)建的精度. 在此環(huán)境中采用UKF-SLAM、 SMCI-SLAM和改進(jìn)UKF-SLAM算法對(duì)移動(dòng)機(jī)器人的路徑和環(huán)境中的路標(biāo)進(jìn)行估計(jì)， 三種算法均假定數(shù)據(jù)關(guān)聯(lián)已知. 仿真實(shí)驗(yàn)結(jié)果如圖2(b)～圖2(d)， 圖中實(shí)線(xiàn)表示移動(dòng)機(jī)器人的運(yùn)行軌跡， 虛線(xiàn)表示各個(gè)算法預(yù)估的路徑. 加號(hào)表示路標(biāo)的真實(shí)位置， 圓圈表示各個(gè)算法估計(jì)的路標(biāo)位置.

從圖 2 中可以看出， 雖然三種算法與機(jī)器人真實(shí)的路徑都存在一定的誤差， 但是改進(jìn)UKF-SLAM算法無(wú)論在機(jī)器人路徑估計(jì)還是路標(biāo)位置估計(jì)上的誤差都要小于UKF-SLAM和SMCI-SLAM算法， 即改進(jìn)UKF-SLAM算法的估計(jì)更接近真實(shí)的狀態(tài).

由于在仿真環(huán)境中， 運(yùn)動(dòng)和觀測(cè)噪聲是隨機(jī)的， 每次實(shí)驗(yàn)的結(jié)果都是不一樣的， 所以不能僅用某一次的實(shí)驗(yàn)結(jié)果來(lái)判斷算法的性能. 均方根誤差(root-mean squared error, RMSE)用來(lái)衡量觀測(cè)值和真實(shí)值之間的偏差， 能很好地反映出算法的精確度. 以RMSE為標(biāo)準(zhǔn)， 對(duì)UKF-SLAM、 SMCI-SLAM和改進(jìn)UKF-SLAM算法進(jìn)行比較. 使用圖2(a)所示仿真環(huán)境， 在機(jī)器人運(yùn)動(dòng)過(guò)程中， 隨機(jī)產(chǎn)生外部干擾. 為了更詳細(xì)地評(píng)估三種算法的性能， 實(shí)驗(yàn)執(zhí)行了50次， 結(jié)果如表 1 所示. RMSE_P表示機(jī)器人位置估計(jì)的均方根誤差， RMSE_L表示路標(biāo)位置估計(jì)的均方根誤差.

表 1 三種算法的50次仿真實(shí)驗(yàn)對(duì)比

改進(jìn)UKF-SLAM算法比UKF-SLAM在移動(dòng)機(jī)器人位置和路標(biāo)估計(jì)上的RMSE分別降低了53.80%和57.16%， 比SMCI-SLAM算法在移動(dòng)機(jī)器人位置和路標(biāo)位置估計(jì)的RMSE分別下降了31%和35%. 實(shí)驗(yàn)證明， 在移動(dòng)機(jī)器人受到外部干擾的環(huán)境中， 通過(guò)干擾檢測(cè)和狀態(tài)方差膨脹可以改進(jìn)SLAM估計(jì)的精度. 由于干擾檢測(cè)只對(duì)比了相鄰兩次觀測(cè)的路標(biāo)， 所耗費(fèi)的時(shí)間有限， 故改進(jìn)UKF-SLAM算法與UKF-SLAM和SMCI-SLAM算法執(zhí)行時(shí)間上接近.

在50次仿真實(shí)驗(yàn)中， 移動(dòng)機(jī)器人每次運(yùn)動(dòng)大約190 s. 圖 3 所示為UKF-SLAM、 SMCI-SLAM和改進(jìn)UKF-SLAM算法基于時(shí)間的移動(dòng)機(jī)器人位姿估計(jì)均方根誤差曲線(xiàn)圖. 圖中包含了四幅子圖， 分別是基于X軸、Y軸、 方向角和位置的均方根誤差. 從圖3中可以看出， 改進(jìn)UKF-SLAM比其他兩種算法在整個(gè)實(shí)驗(yàn)過(guò)程中都更接近移動(dòng)機(jī)器人的真實(shí)位置， 證明干擾檢測(cè)和狀態(tài)方差膨脹可以有效提高移動(dòng)機(jī)器人位姿估計(jì)精度.

圖 3 三種算法在機(jī)器人位姿上的均方根誤差Fig.3 The RMSE comparison in position among the three algorithms

圖 4 所示為路標(biāo)位置的均方根誤差曲線(xiàn)圖. 從圖中可以看出， 改進(jìn)UKF-SLAM算法比其他兩種算法對(duì)于路標(biāo)位置的估計(jì)誤差要小.

圖 4 三種算法在路標(biāo)位置上的均方根誤差Fig.4 The RMSE comparison in landmark position between the three algorithms

4.2 路標(biāo)受到外部干擾

采用與前面實(shí)驗(yàn)相同的仿真環(huán)境， 在機(jī)器人移動(dòng)過(guò)程中坐標(biāo)為(12,-3), (125,105)和(-20,140)的3個(gè)路標(biāo)由于受到外部干擾的影響， 位置發(fā)生了改變. 在路標(biāo)受到外部干擾的環(huán)境下使用UKF-SLAM、 SMCI-SLAM和改進(jìn)UKF-SLAM算法對(duì)移動(dòng)機(jī)器人的路徑和環(huán)境中的路標(biāo)進(jìn)行估計(jì). 三種算法均假定數(shù)據(jù)關(guān)聯(lián)已知， 仿真實(shí)驗(yàn)結(jié)果如圖 5(b)～圖5(d)所示.

圖 5 三種算法在路標(biāo)受到外部干擾下的實(shí)驗(yàn)結(jié)果Fig.5 Comparison between the three algorithms with outlier disturbances on landmarks

在UKF-SLAM算法中， 某個(gè)路標(biāo)位置的改變不僅影響地圖估計(jì)， 也影響了移動(dòng)機(jī)器人定位的精度. 由于采用固定的噪聲模型， 使得對(duì)位置已改變路標(biāo)的過(guò)分信任， 導(dǎo)致移動(dòng)機(jī)器人位姿和其他路標(biāo)位置向已改變位置路標(biāo)偏移. 在UKF-SLAM算法中誤差是累計(jì)的， 最終會(huì)使得無(wú)論在地圖估計(jì)上的誤差還是機(jī)器人定位估計(jì)上的誤差都會(huì)變大. SMCI-SLAM算法和改進(jìn)UKF-SLAM算法通過(guò)對(duì)系統(tǒng)狀態(tài)的膨脹降低了對(duì)位置改變路標(biāo)的信任， 提高了SLAM的估計(jì)精度. 通過(guò)系統(tǒng)狀態(tài)膨脹， 雖然可能會(huì)降低被改變路標(biāo)位置估計(jì)的精度， 如圖5(d)改進(jìn)UKF-SLAM算法對(duì)于坐標(biāo)為(12,-3)的路標(biāo)估計(jì)精度不如UKF-SLAM， 但是如果不膨脹系統(tǒng)狀態(tài)， 會(huì)導(dǎo)致移動(dòng)機(jī)器人定位和地圖中其他路標(biāo)位置估計(jì)精度的降低， 如圖5(b)中， 由于該路標(biāo)位置的改變， 導(dǎo)致移動(dòng)機(jī)器人的位置和其他路標(biāo)位置都發(fā)生了偏移， 由于誤差是累計(jì)的， 從而導(dǎo)致后面對(duì)系統(tǒng)狀態(tài)估計(jì)的誤差越來(lái)越大. SMCI-SLAM算法是在路標(biāo)受到外部干擾影響的下一個(gè)周期才使用膨脹后的協(xié)方差更新地圖和機(jī)器人位置， 同時(shí)也受累計(jì)誤差的影響. 改進(jìn)UKF-SLAM算法在路標(biāo)受到外部影響的周期內(nèi)就使用膨脹后的協(xié)方差更新系統(tǒng)狀態(tài)， 能夠快速地對(duì)外部干擾做出反應(yīng)， 同時(shí)不受累計(jì)誤差的影響， 也對(duì)外部干擾做出了分類(lèi)， 所以更能反映真實(shí)的環(huán)境. 從圖中也可以看出， 改進(jìn)UKF-SLAM估計(jì)的精度要優(yōu)于SMCI-SLAM和UKF-SLAM算法.

由于運(yùn)動(dòng)和觀測(cè)噪聲的隨機(jī)性導(dǎo)致每次實(shí)驗(yàn)的結(jié)果不一樣， 這里仍然以RMSE為標(biāo)準(zhǔn)， 對(duì)UKF-SLAM、 SMCI-SLAM和改進(jìn)UKF-SLAM算法進(jìn)行比較. 環(huán)境地圖使用圖5(a)的仿真環(huán)境， 在移動(dòng)機(jī)器人運(yùn)動(dòng)過(guò)程中， 會(huì)隨機(jī)產(chǎn)生外部干擾導(dǎo)致3～5個(gè)路標(biāo)位置發(fā)生改變. 實(shí)驗(yàn)執(zhí)行了50次， 結(jié)果如表 2 所示， 結(jié)果表明在路標(biāo)受到外部干擾的環(huán)境中， 通過(guò)干擾檢測(cè)和狀態(tài)方差膨脹可以改進(jìn)SLAM估計(jì)的精度.

表 2 三種算法的50次仿真實(shí)驗(yàn)對(duì)比

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

對(duì)外部干擾影響SLAM估計(jì)精度的問(wèn)題， 提出了改進(jìn)的UKF-SLAM算法. 所提算法在系統(tǒng)狀態(tài)更新之前引入外部干擾檢測(cè)和狀態(tài)方差膨脹， 能在干擾發(fā)生的周期內(nèi)快速做出響應(yīng). 臨近觀測(cè)的不一致性用于檢測(cè)外部干擾不受累計(jì)誤差的影響， 提高了檢測(cè)的精度. 外部干擾分為移動(dòng)機(jī)器人受到的外部干擾和路標(biāo)受到的外部干擾， 對(duì)它們使用不同方式膨脹系統(tǒng)狀態(tài)協(xié)方差可以獲取更精確的系統(tǒng)狀態(tài)估計(jì). 仿真實(shí)驗(yàn)結(jié)果表明， 改進(jìn)UKF-SLAM算法比UKF-SLAM和SMCI-SLAM算法在系統(tǒng)發(fā)生外部干擾的情況下對(duì)系統(tǒng)狀態(tài)的估計(jì)更加精確， 同時(shí)增加的系統(tǒng)開(kāi)銷(xiāo)也很小.