吳 勇, 關(guān)勝曉

?

基于無(wú)跡卡爾曼濾波器的改進(jìn)SLAM問(wèn)題求解方法①

吳 勇, 關(guān)勝曉

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

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

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

1　引言

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

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

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

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

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

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

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

2 基于UKF的SLAM

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

2.1 SLAM簡(jiǎn)述

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

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

2.2 基于UKF的解決方案

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

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

3 局部取樣的一致性

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

3.1 一致性

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

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

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

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

3.2 重構(gòu)UKF公式

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

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

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

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

4 基于S-UKF的SLAM

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

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

圖2 局部和鄰居地圖

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

① 預(yù)測(cè)階段

② 局部更行階段

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

④ 全局更新階段

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

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

5.1 仿真

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

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

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

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

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

表1 仿真結(jié)果

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

為了進(jìn)一步驗(yàn)證該方法的性能, 我們使用標(biāo)準(zhǔn)數(shù)據(jù)集-維多利亞公園數(shù)據(jù)集來(lái)進(jìn)行實(shí)驗(yàn). 該數(shù)據(jù)集是由一輛帶有范圍測(cè)量傳感器和動(dòng)態(tài)GPS天線的移動(dòng)車輛收集的. 因?yàn)闃淠镜恼趽跏沟肎PS測(cè)量的數(shù)據(jù)不連續(xù), 所以該實(shí)驗(yàn)并未使用GPS數(shù)據(jù). 車輛的運(yùn)動(dòng)是由速度和角度編碼器來(lái)進(jìn)行測(cè)量的. 因?yàn)槿雍途植咳硬呗缘牡葍r(jià)性已經(jīng)在上一節(jié)驗(yàn)證過(guò), 所以這里只討論不同方法的效率差異.

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

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

圖6 不同方法的累計(jì)運(yùn)行時(shí)間

6 結(jié)語(yǔ)

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

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]