基于無跡卡爾曼濾波器的改進SLAM問題求解方法①

吳 勇, 關(guān)勝曉

?

基于無跡卡爾曼濾波器的改進SLAM問題求解方法①

吳 勇, 關(guān)勝曉

(中國科學(xué)技術(shù)大學(xué)信息科學(xué)技術(shù)學(xué)院, 合肥 230027)

目前在即時定位與地圖構(gòu)建(Simultaneous Localization And Mapping, SLAM)的研究中已經(jīng)使用局部取樣策略來降低無跡卡爾曼濾波(Unscented Kalman Filter, UKF)的計算復(fù)雜度至狀態(tài)向量維數(shù)的平方等級. 但是在大規(guī)模的SLAM中平方復(fù)雜度仍然難以滿足實時性需求. 為了解決這個問題, 提出了一種收縮無跡卡爾曼濾波器(Shrink Unscented Kalman Filter, S-UKF), 并應(yīng)用于SLAM問題中. 首先證明了解耦非線性系統(tǒng)中的部分取樣策略和全取樣策略的等價性. 然后提出了一個通過重構(gòu)公式中相關(guān)項的收縮方式來降低計算復(fù)雜度. 最后, 仿真實驗的結(jié)果和基于真實環(huán)境數(shù)據(jù)集的實驗結(jié)果證明了該方法的有效性.

SLAM; 無跡卡爾曼濾波(UKF); 局部取樣; 計算復(fù)雜度

1　引言

同步定位與地圖構(gòu)建(SLAM)是指自主車輛或移動機器人在未知的環(huán)境中構(gòu)建地圖并使用構(gòu)建的地圖同步定位自己的位置[1]. 相比于常規(guī)的自主導(dǎo)航系統(tǒng)(例如GPS和地圖匹配), SLAM不需要任何外部設(shè)備或者關(guān)于環(huán)境的任何先驗信息, 這使得SLAM尤其適用于無GPS環(huán)境的機器人導(dǎo)航中. 正是由于SLAM的這些特點, 使得它在過去的20年間受到極大的關(guān)注[2-5].

基于前人的理論工作[6,7], 已經(jīng)發(fā)展出種類繁多的基于濾波的方法來解決SLAM問題, 例如擴展卡爾曼濾波器(Extended Kalman Filter, EKF)[8]、無跡卡爾曼濾波器(UKF)[9]、以FastSLAM而聞名的粒子濾波器(Particle Filter, PF)[10]以及稀疏擴展信息濾波器(Sparse Extended Information Filter, SEIF)[11]. 其中EKF有著相對較低的計算成本, 這使得它在實際中已經(jīng)被廣泛應(yīng)用[3,7]. 但是, 在基于EKF的SLAM中需要將系統(tǒng)線性化以及計算雅可比矩陣, 這會導(dǎo)致性能的降低甚至使濾波發(fā)散. 同樣的問題也會出現(xiàn)在SEIF中. 與此相反, 在基于取樣的濾波算法中使用一組樣本和權(quán)重來估計狀態(tài)向量, 不需要線性化和計算雅可比矩陣. 其缺點則是增加了一個附加的計算復(fù)雜度. 與FastSLAM相比, 基于UKF的SLAM使用確定性取樣和權(quán)重, 不會有退化問題. 因而, 如果UKF的計算成本可以顯著降低那么它就是一個切實可行的選擇.

前人在基于UKF的SLAM中研究了很多方法. Holmes等人[12]提出了一種基于UKF平方根(Square-Root Unscented Kalman Filter, SRUKF)的方法用來解決monoSLAM(Monocular SLAM)問題, 其中使用協(xié)方差矩陣的平方根參與遞推運算, 避免了在每次迭代步驟中O(N3)復(fù)雜度的分解. 尤其是通過重排序狀態(tài)向量的方法這種計算代價可以降低到O(N2), 其中N是狀態(tài)向量的維度. 在靜態(tài)環(huán)境下的SLAM過程中, Andrade-Cetto等人[13]提出局部無跡變換(Unscented Transformation, UT)的策略, 其中使用UKF來估計運動過程中車輛的位姿, 使用常規(guī)EKF來更新全局的狀態(tài)向量. 但是, 在迭代步驟中仍然需要線性化來預(yù)測相關(guān)性. 為了避免線性化, Huang等人[14]提出了基于線性回歸模型的局部取樣的策略. 這項研究表明, 雅可比矩陣可以從σ點推斷出來. 然后, UKF就可以被表示為一個基于線性回歸模型, 復(fù)雜度為O(N2)的濾波形式. 然而這項工作卻沒有明確分析局部取樣策略相對于全取樣策略的效果.

受到文獻[8]中研究的啟發(fā), 我們指出了基于UKF的SLAM可以被收縮從而顯著降低其計算成本. 本文的主要貢獻如下:

① 證明了局部取樣策略提供了完全相同的車輛姿態(tài)估計, 和路標(biāo)的數(shù)目無關(guān);

② 提出了一個基于UKF的SLAM的收縮實現(xiàn)(S-UKF), 復(fù)雜度為O(NL2), 其中NL是局部地圖中的路標(biāo)數(shù);

③ 通過仿真和真實環(huán)境的數(shù)據(jù)集來展示基于S-UKF的SLAM的效率.

2 基于UKF的SLAM

在本節(jié)中, 我們通過公式給出了基于濾波方式解決SLAM問題的簡要概述, 然后是一個基于UKF解決方案的介紹.

2.1 SLAM簡述

由于車輛的位置和方向之間的耦合, 所以SLAM是一個非線性估計問題. 在基于濾波的SLAM中, 問題通常表示為狀態(tài)空間模型, 其中狀態(tài)向量由機器人當(dāng)前位姿和環(huán)境中所有觀察到的路標(biāo)位置組成. 對于二維的平面SLAM, 令表示時刻機器人的水平位置和方位角, 令表示所有被觀測到的路標(biāo), 其中表示路標(biāo)的數(shù)量. 離散非線性運動方程一般可表示為:

在SLAM系統(tǒng)中并沒有絕對的觀測結(jié)果, 所以通過使用外感受性的傳感器例如激光雷達或者攝像頭來獲得機器人和環(huán)境之間的相對測量值, 并以此值來校正狀態(tài)的估計. 觀測模型如下:

2.2 基于UKF的解決方案

在SLAM中, 為了進行有效的近似非線性變換, UKF首先計算點以及歸一化權(quán)重以得到狀態(tài)向量的分布. 然后變換這些點來估計非線性變換. 在預(yù)測階段, 一組點及權(quán)重能由如下式(5)計算:

在觀測值更新階段, 觀測值的預(yù)測分布也可以類似的如下計算:

3 局部取樣的一致性

在SLAM問題中, 每個階段的非線性變換中只會出現(xiàn)部分狀態(tài), 因此局部取樣策略是用來降低復(fù)雜度的一個很有吸引力的選擇. 在文獻[13]中, 基于標(biāo)準(zhǔn)的UKF的SLAM中, 僅僅基于車輛位姿的UT變換的實驗結(jié)果表明其低估了協(xié)方差矩陣隨著路標(biāo)數(shù)的增長而增長的速度. 那么, 局部取樣策略的效果如何呢?

3.1 一致性

引理1. 考慮一個非線性變換可分成兩個完全分離的非線性部分和線性部分, 如下所示:

那么不論是對全取樣策略還是對局部取樣策略UKF和常數(shù)縮放因子就能提供一個完全相同的關(guān)于的估計值. 即該估計值獨立于的大小.

圖1 使用基于完全取樣和局部取樣策略的UKF針對不同的路標(biāo)進行坐標(biāo)轉(zhuǎn)換的估計實驗. 虛線圓和實線圓分別表示蒙特卡羅和UKF在3σ范圍內(nèi)的橢圓.

為了驗證非線性估計算法的性能, 一個典型的方法就是極坐標(biāo)系到笛卡爾坐標(biāo)系的轉(zhuǎn)換[13,15]. 設(shè)定觀測量的均值和協(xié)方差矩陣分別為, 圖1展示了基于不同取樣策略的UKF算法的結(jié)果, 其中不同的路標(biāo)分別被增廣到狀態(tài)向量. 盡管在1000個樣本下, UKF和蒙特卡羅方法的結(jié)果存在不同之處, 但是無論取樣策略是什么, 路標(biāo)的數(shù)目是多少, UKF的結(jié)果全部相同.

3.2 重構(gòu)UKF公式

基于引理1, 我們就能夠重寫點集和權(quán)重如下:

將式(22)和式(27)代入式(26), 得到

在更新階段, 我們也能類似地重寫式(12)如下:

我們注意到如果在預(yù)測階段將控制輸入向量也考慮進來的話, 那么可以將該向量添加到非線性部分從而得到同樣的結(jié)果.

4 基于S-UKF的SLAM

正如文獻[17]中描述的那樣, UKF是一個線性回歸卡爾曼濾波器. 而且該線性回歸模型已經(jīng)成功的被應(yīng)用到UKF中, 并且在文獻[14]中用來做可觀測性分析. 因此根據(jù)上一節(jié)的結(jié)果我們就能將UKF表達為一個近似線性形式, 然后S-UKF就像壓縮擴展卡爾曼濾波器(Compressed Extended Kalman Filter, CEKF)[8]一樣被應(yīng)用.

根據(jù)SLAM中傳感器的測量范圍, 全局地圖能夠被分為兩個單獨的部分如圖2所示. 相應(yīng)地, 狀態(tài)向量和協(xié)方差矩陣能被表示如下:

圖2 局部和鄰居地圖

類似地, 在路標(biāo)初始化階段, 我們能夠通過式(30)得到局部線性因子, 則全局線性因子為:

① 預(yù)測階段

② 局部更行階段

③ 路標(biāo)初始化階段

④ 全局更新階段

5 實驗結(jié)果與分析

在本節(jié)中, 我們進行了仿真和真實環(huán)境數(shù)據(jù)集實驗, 以評估該算法的性能. 所有的實驗都是在一臺主頻2.20GHz的AMD處理器, 2GB內(nèi)存, 裝有Matlab2012a的計算機上實現(xiàn)的. 在這些實驗中, 范圍測量傳感器用來提供測量數(shù)據(jù). 為了更加全面的比較, 我們也分別實現(xiàn)了基于EKF和CEKF的SLAM.

5.1 仿真

在仿真中, 我們采用一種流行的創(chuàng)建方案[18]用來隨機生成仿真環(huán)境, 如圖3所示. 有35個路標(biāo)放置在規(guī)劃好的路徑周圍, 使用一個最大測量距離為30m, 頻率為10Hz的180°范圍測量傳感器來測量這些路標(biāo). 測量噪聲被設(shè)為均值為0, 標(biāo)準(zhǔn)差分別為0.1m和1°. 車輛以v=3m/s的恒定速度運行, 且最大轉(zhuǎn)向角速度為w=30°/s,取樣頻率為40Hz. 測量的標(biāo)準(zhǔn)差分別為0.3m/s和3°. 運動和觀測模型可以在文獻[18]中找到詳細描述.

車輛從位置[0,0]出發(fā), 并沿著這條路徑循環(huán)5次. 使用均方根誤差(Root Mean Square Error, RMSE)和平均歸一化估計的誤差(Normalized Estimation Squared, NEES)來驗證全取樣和局部取樣策略的等價性. 因此, 對每個算法進行50次蒙特卡羅測試. 公平起見, 對每個不同的算法使用相同的隨機數(shù)種子.

因為標(biāo)準(zhǔn)濾波器和收縮濾波器具有相同的準(zhǔn)確度和一致性, 因此基于CEKF和S-UKF的估計在圖3中并未給出. 圖3顯示了關(guān)于EKF、UKF和局部取樣UKF(Partial Unscented Kalman Filter, PUKF)的實驗結(jié)果. 圖4呈現(xiàn)了從50次蒙特卡羅測試中得到的車輛位姿誤差的平均NEES. 從以上兩幅圖中我們可以發(fā)現(xiàn)UKF和PUKF具有完全相同的準(zhǔn)確性和一致性, 這可以通過分析表1種的RMES來進一步驗證. 根據(jù)表中的結(jié)果, 與EKF相比UKF和PUKF更具優(yōu)勢. 從不同算法的運行時間來看, 我們能夠發(fā)現(xiàn)相對于標(biāo)準(zhǔn)的UKF, 局部取樣策略能夠降低38.98%的復(fù)雜度. 此外, S-UKF的復(fù)雜度降低了64.33%, 這已經(jīng)接近CKF的復(fù)雜度, 完全能滿足SLAM的實時性要求.

圖3 對EKF、UKF和PUKF測試的結(jié)果. 值得注意的是, UKF和PUKF的結(jié)果重合在一起. 星號和點線分別表示真實的路標(biāo)和路徑. 圓和實線分別表示基于UKF和PUKF的對路標(biāo)和路徑的估計. 方形和虛線則表示EKF的結(jié)果.

圖4 50次蒙特卡羅測試中得到的車輛位姿誤差的平均NEES. 實線(頂部)表示EKF. 虛線和點實線分別表示UKF和PUKF, 可以看出他們是完全相同且重疊在一起的.

表1 仿真結(jié)果

5.2 真實環(huán)境數(shù)據(jù)集實驗

為了進一步驗證該方法的性能, 我們使用標(biāo)準(zhǔn)數(shù)據(jù)集-維多利亞公園數(shù)據(jù)集來進行實驗. 該數(shù)據(jù)集是由一輛帶有范圍測量傳感器和動態(tài)GPS天線的移動車輛收集的. 因為樹木的遮擋使得GPS測量的數(shù)據(jù)不連續(xù), 所以該實驗并未使用GPS數(shù)據(jù). 車輛的運動是由速度和角度編碼器來進行測量的. 因為全取樣和局部取樣策略的等價性已經(jīng)在上一節(jié)驗證過, 所以這里只討論不同方法的效率差異.

圖5給出了CEKF和S-UKF在維多利亞公園數(shù)據(jù)集上的實驗結(jié)果. 圖6顯示了EKF、CEKF、UKF、PUKF和S-UKF的累計運行時間. 由圖6可以看出, UKF因為復(fù)雜度為故其運行時間快速增長至1910.23s. 通過局部取樣策略, PUKF的復(fù)雜度降低到, 這已經(jīng)接近EKF的復(fù)雜度了. 最后, S-UKF和CEKF在每個階段的計算成本是大致恒定的. S-UKF的總花費時間是176.32s, 略大于CEKF的157.27s. 此次試驗中, 運行了44min, 有204個路標(biāo)被檢測到. 從該實際數(shù)據(jù)集的實驗結(jié)果中我們可以知道, 基于S-UKF的SALM可以滿足實時性的要求.

圖5 使用CEKF和S-UKF的實驗結(jié)果. 因為GPS信號經(jīng)常被樹木阻擋, 很難提供可靠的地面實況. GPS結(jié)果用點表示. 實線和加號分別表示CEKF估計的軌跡和路標(biāo). 相應(yīng)地, 虛線和圓圈表示S-UKF的結(jié)果.

圖6 不同方法的累計運行時間

6 結(jié)語

在本文中, 我們研究了兩種降低UKF復(fù)雜度的方法, 分別是局部取樣策略和收縮方法. 證明了局部取樣策略和原有估計的一致性. 在高斯假設(shè)下, 相關(guān)性的傳播被改寫為一個類似線性的形式, 這使得收縮方法能適用于UKF中. 進一步的, 基于S-UKF的SLAM能夠降低至關(guān)于局部路標(biāo)數(shù)目平方的復(fù)雜度. 基于仿真和真實環(huán)境數(shù)據(jù)集的實驗結(jié)果, 證明了該方法可以滿足實際應(yīng)用中的實時性要求.

1 Durrant-Whyte H, Bailey T. Simultaneous localization and mapping: Part i. IEEE Robotics and Automation Magazine, 2006, 13(2): 99–110.

2 Dissanayake MWMG, Newman P, Clark S, Durrant-Whyte HF, Csorba M. A solution to the simultaneous localization and map building (SLAM) problem. IEEE Trans. on Robotics and Automation, 2001, 17(3): 229–241.

3 Thrun S, et al. Simultaneous localization and mapping with sparse extended information filters. The International Journal of Robotics Research, 2004, 23(7-8): 693–716.

4 Davison AJ, Reid ID, Molton ND, Stasse O. MonoSLAM: Real-time single camera SLAM. IEEE Trans. on Pattern Analysis and Machine Intelligence, 2007, 29(6): 1052–1067.

5 Ribas D, Ridao P, Tardos JD, Neira J. Underwater SLAM in manmade structured environments. Journal of Field Robotics, 2008, 25(1112): 898–921.

6 Thrun S, Burgard W, Fox D. A probabilistic approach to concurrent mapping and localization for mobile robots. Autonomous Robots, 1998, 5(3-4): 253–271.

7 Dissanayake MG, Newman P, Clark S, Durrant-Whyte HF, Csorba M. A solution to the simultaneous localization and map building (slam) problem. IEEE Trans. on Robotics and Automation, 2001,17(3): 229–241.

8 Guivant JE, Nebot EM. Optimization of the simultaneous localization and map-building algorithm for real-time implementation. IEEE Trans. on Robotics and Automation, 2001, 17(3): 242–257.

9 Julier SJ, Uhlmann JK, Durrant-Whyte HF. A new approach for filtering nonlinear systems. Proc. of the 1995 American Control Conference. Seattle, WA. 1995. 1628–1632.

10 Montemerlo M. Fastslam: A factored solution to the simultaneous localization and mapping problem. Proc. the AAAI Intrenational Conference on Artificial Intelligence. Edmonton, Canada. 2002. 593–598.

11 Eustice R, Walter M, Leonard J. Sparse extended information filters: Insights into sparsification. 2005 IEEE/RSJ International Conference on Intelligent Robots and Systems. 2005. 3281–3288.

12 Holmes S, Klein G, Murray DW. A square root unscented Kalman filter for visual monoslam. IEEE International Conference on Robotics and Automation (ICRA). Pasadena, CA. 2008. 3710–3716.

13 Andrade-Cetto J, Vidal-Calleja T, Sanfeliu A. Unscented transformation of vehicle states in slam. IEEE International Conference on Robotics and Automation. Barcelona, Spain. 2005. 323–328.

14 Huang GP, Mourikis AI, Roumeliotis SI. A quadratic complexity observability-constrained unscented Kalman filter for slam. IEEE Trans. on Robotics, 2013, 29(5): 1226–1243.

15 Julier SJ, Uhlmann JK. Unscented filtering and nonlinear estimation. Proc. of the IEEE, 2004, 92(3): 401–422.

16 Carlevaris-Bianco N, Eustice RM. Generic factor-based node marginalization and edge sparsification for pose-graph slam. IEEE International Conference on Robotics and Automation (ICRA). Karlsruhe, Germany. 2013. 5748–5755.

17 Lefebvre T, Bruyninckx H, De Schuller J. Comment on “a new method for the nonlinear transformation of means and covariances in filters and estimators”. IEEE Trans. Automatic Control, 2002, 47(8): 1406–1409.

18 Bailey T, Nieto J, Guivant J St. Consistency of the ekf-slam algorithm. IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS). Beijing, China. 2006. 3562–3568.

Improved Solution Based on Unscented Kalman Filter in the SLAM

WU Yong, GUAN Sheng-Xiao

(School of Information Science and Technology, University of Science and Technology of China, Hefei 230027, China)

A partial sampling strategy was recently proposed to make the computational complexity of the unscented Kalman filter (UKF) quadratic with the state-vector dimension. However, the quadratic complexity remains a thorny issue in the large SLAM. To solve this problem, this paper presents a filtering solution for the SLAM problem called shrink unscented Kalman filter (S-UKF). It firstly proves that equivalence of the whole and partial sampling strategy for the decoupled nonlinear systems. Then a shrink form is presented by reconstruction the cross-correlation items to reduce the computational complexity. Finally, the simulation results and experimental results based on real environmental data sets validate the effectiveness of this method.

SLAM; unscented Kalman filter (UKF); partial sampling; computational complexity

2016-06-18;

2016-08-08

[10.15888/j.cnki.csa.005674]