呂太之， 周 武， 趙春霞

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

0 引 言

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

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

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

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

1) 繼承抗外部干擾的EKF-SLAM算法， 將其應用于UKF-SLAM算法， 通過臨近觀測分析來檢測外部干擾， 不會受累計誤差的影響， 提高了外部干擾檢測的精度.

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

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

1 SLAM問題描述

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

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

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

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

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

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

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

基于運動和觀測模型， SLAM問題的后驗概率可以使用貝葉斯濾波器迭代更新， 式(1)改寫成

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

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

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

2 UKF-SLAM算法

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

UKF-SLAM初始化過程包括均值、 協(xié)方差矩陣和Sigma點權值的設置.

x0=xinitP0=Pinit,(5)

通過Sigma點計算、 預測和觀測更新三個步驟完成狀態(tài)向量和協(xié)方差矩陣的迭代更新.

1) Sigma點計算

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

(8)

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

(10)

尺度常量η計算如下

(11)

2) 預測

根據(jù)上一時刻的Sigma點和運動模型預測t時刻的Sigma點.

(12)

3) 更新

(15)

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

3 改進的UFK-SLAM算法

3.1 算法設計思路

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

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

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

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

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

提出算法通過對比t-1時刻和t時刻的觀測來檢測移動機器人是否受到外部干擾. 假定t-1時刻移動機器人處于原點位置， 前進的方向為X軸.t-1時刻機器人觀測到的第j個路標信息為zt-1(mj)= [lt-1(mj),βt-1(mj)]. 以t-1時刻移動機器人位置為原點， 第j個坐標的位置為

(23)

(24)

式中： ΔT為采樣周期；vt為車輛后軸中心處運動速度；αt為車輛的轉向角；L為兩軸的軸距. 通過平移和旋轉操作， 可以獲得第j個路標以t時刻機器人為原點下的預測位置

(25)

最后通過臨近兩次觀測不一致性來確定移動機器人或者路標是否受到外部干擾.

1) 移動機器人受到外部干擾

(27)

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

(28)

Dβt(mj)～N(0,1)服從正態(tài)分布. 當對多個路標觀測Dβt(mj)超過指定閥值并且角度偏移方向趨于一致的時候就認定移動機器人受到外部干擾.

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

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

2) 路標受到外部干擾

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

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

3.3 算法流程

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

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

輸入：

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

步驟：

∥如果臨近觀測相同路標數(shù)量小于3， 不進行外部干擾檢測

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

return;

end if

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

disturanceType=checkDisturance(zt-1,zt);

if disturanceType==ROBOTDISTURANCE

else if disturanceType==LANDMARKDISTRUANCE

end if

輸出：

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

圖 1 改進UKF-SLAM算法流程圖Fig.1 The flowchart of the improved UKF-SLAM algorithm

4 仿真實驗結果及分析

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

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

觀測數(shù)據(jù)由路標的距離lt(mj)和方向角βt(mj)組成.t時刻移動機器人觀測方程為

(32)

4.1 移動機器人受到外部干擾

設計一個大小為250 m×200 m的包含75個路標的仿真環(huán)境， 如圖 2(a)所示， 移動機器人的初始位置為(0,0).

圖 2 三種算法在機器人受到外部干擾下的實驗結果Fig.2 Comparison between the three algorithms with outlier disturbances on robots

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

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

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

表 1 三種算法的50次仿真實驗對比

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

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

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

圖 4 所示為路標位置的均方根誤差曲線圖. 從圖中可以看出， 改進UKF-SLAM算法比其他兩種算法對于路標位置的估計誤差要小.

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

4.2 路標受到外部干擾

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

圖 5 三種算法在路標受到外部干擾下的實驗結果Fig.5 Comparison between the three algorithms with outlier disturbances on landmarks

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

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

表 2 三種算法的50次仿真實驗對比

5 結束語

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