基于迭代擴(kuò)展Kalman濾波建議分布和線性優(yōu)化重采樣的快速同步定位與構(gòu)圖

王宏健 王 晶 劉振業(yè)

?

基于迭代擴(kuò)展Kalman濾波建議分布和線性優(yōu)化重采樣的快速同步定位與構(gòu)圖

王宏健*①王 晶①②劉振業(yè)②

①(哈爾濱工程大學(xué)自動(dòng)化學(xué)院 哈爾濱 150001)②(中國(guó)航天科工集團(tuán)第8357研究所 天津 300308)

針對(duì)標(biāo)準(zhǔn)快速同步定位與構(gòu)圖(FastSLAM)方法中由于樣本退化及貧化導(dǎo)致自主水下航行器(Autonomous Underwater Vehicle, AUV)及路標(biāo)位置估計(jì)精度嚴(yán)重下降的問(wèn)題，該文提出一種基于迭代擴(kuò)展Kalman濾波(Iterative Extended Kalman Filter, IEKF)建議分布和線性優(yōu)化重采樣的FastSLAM方法，通過(guò)IEKF融入最新觀測(cè)值從而降低樣本退化，為了降低樣本的貧化，將重采樣過(guò)程中復(fù)制的樣本與部分被拋棄的樣本通過(guò)線性組合產(chǎn)生新樣本。建立AUV的運(yùn)動(dòng)學(xué)模型、特征模型及傳感器的測(cè)量模型，通過(guò)Hough變換提取特征構(gòu)建全局地圖，采用改進(jìn)的FastSLAM方法基于海試數(shù)據(jù)進(jìn)行了AUV同步定位與構(gòu)圖試驗(yàn)，結(jié)果表明該文所設(shè)計(jì)的方法能夠有效避免樣本的退化及貧化，提高了AUV及路標(biāo)的位置估計(jì)精度；此外，一致性分析結(jié)果表明所設(shè)計(jì)算法具有長(zhǎng)期一致性。

同步定位與構(gòu)圖；迭代擴(kuò)展Kalman濾波建議分布；線性優(yōu)化重采樣；特征提取

1　引言

機(jī)器人在部分或完全未知的環(huán)境中，自身位置不確定的情況下，利用自身攜帶的傳感器來(lái)感知周?chē)畔ⅲㄟ^(guò)對(duì)環(huán)境中路標(biāo)的反復(fù)觀察完成自身及路標(biāo)位置的校正，這一過(guò)程可以同時(shí)實(shí)現(xiàn)機(jī)器人自身的定位和環(huán)境地圖的創(chuàng)建，這一技術(shù)被稱(chēng)為同步定位與構(gòu)圖(Simultaneous Localization And Mapping, SLAM)。這一能力被很多人認(rèn)為是機(jī)器人真正實(shí)現(xiàn)自主的先決條件[1,2]。近年來(lái)，眾多學(xué)者提出各種SLAM技術(shù)和方法用于解決室內(nèi)[3,4]、室外[5]、空中[6]、甚至水下[7,8]無(wú)人機(jī)器人的自主導(dǎo)航定位問(wèn)題。SLAM作為一種地形輔助導(dǎo)航方法，對(duì)于自主水下航行器(AUV)的自主導(dǎo)航定位具有重要的意義。

解決SLAM問(wèn)題的方法眾多，其中，粒子濾波(Particle Filtering, PF)是一種新的處理非線性、非高斯系統(tǒng)的有效方法[9,10]。Montemerlo等人[11]提出了基于粒子濾波器的SLAM方法，也稱(chēng)FastSLAM。雖然FastSLAM在模型非線性、計(jì)算復(fù)雜度、數(shù)據(jù)關(guān)聯(lián)等方面具有優(yōu)勢(shì)[12]，但是標(biāo)準(zhǔn)FastSLAM重采樣過(guò)程中由于大權(quán)值粒子的復(fù)制和小權(quán)值粒子的拋棄極易造成粒子貧化現(xiàn)象，即粒子集中只剩下少數(shù)大權(quán)值的粒子，從而導(dǎo)致被拋棄粒子中所攜帶的AUV路徑及地圖歷史信息一并被刪除，這一現(xiàn)象是FastSLAM定位及路標(biāo)位置估計(jì)精度下降的主要原因。

針對(duì)這一問(wèn)題，本文將從兩方面對(duì)標(biāo)準(zhǔn)的FastSLAM加以改進(jìn)，首先，選擇迭代擴(kuò)展Kalman濾波(IEKF)產(chǎn)生建議分布，使建議分布能夠融入最新的測(cè)量值，從而產(chǎn)生更加符合粒子真實(shí)分布的后驗(yàn)概率分布，得到更優(yōu)良的粒子集合，以緩解粒子的退化現(xiàn)象。其次，重采樣過(guò)程中通過(guò)將復(fù)制的粒子與部分符合條件的被拋棄的粒子進(jìn)行線性組合從而產(chǎn)生新的粒子，以保證粒子的多樣性。最終達(dá)到提高AUV和特征位置估計(jì)精度的目的。

2 基于IEKF建議分布和線性優(yōu)化重采樣的FastSLAM方法

2.1 IEKF建議分布

對(duì)于非線性系統(tǒng)，EKF通過(guò)泰勒級(jí)數(shù)展開(kāi)并忽略高階項(xiàng)對(duì)非線性系統(tǒng)模型進(jìn)行線性化，這無(wú)法避免地會(huì)引入截?cái)嗾`差，導(dǎo)致?tīng)顟B(tài)估計(jì)的性能較差。然而，IEKF能夠得到狀態(tài)的最大后驗(yàn)概率估計(jì)，其性能優(yōu)于傳統(tǒng)的EKF[13]。所以，用IEKF產(chǎn)生建議分布，將更符合狀態(tài)變量的實(shí)際后驗(yàn)概率分布。

對(duì)于非線性系統(tǒng)

預(yù)測(cè)過(guò)程：

2.2 線性優(yōu)化重采樣

線性優(yōu)化重采樣的基本思想為：在需要復(fù)制某個(gè)樣本時(shí)，將該樣本和符合一定條件的被拋棄的樣本進(jìn)行適當(dāng)?shù)木€性組合以產(chǎn)生一個(gè)新樣本，并用這些新樣本來(lái)代替簡(jiǎn)單重采樣中復(fù)制的樣本，從而避免樣本的簡(jiǎn)單復(fù)制。通過(guò)這種組合，可以使粒子的分布更加接近真實(shí)的后驗(yàn)概率分布。

具體的線性組合方式為

從簡(jiǎn)單重采樣被拋棄的樣本中選出滿足式(9)的樣本，將其放入偽拋棄組，以備在將來(lái)的線性優(yōu)化中使用。

2.3 基于IEKF建議分布和線性優(yōu)化重采樣的FastSLAM(IEKF-LOR-FastSLAM)

IEKF-LOR-FastSLAM中AUV路徑和路標(biāo)位置所組成的聯(lián)合概率分布可以表示為

采用粒子濾波器完成AUV的位置估計(jì)，可以有效地從樣本空間中采樣；而路標(biāo)的位置估計(jì)則采用Kalman濾波器來(lái)實(shí)現(xiàn)，不同的路標(biāo)采用獨(dú)立的濾波器。

IEKF-LOR-FastSLAM算法操作步驟如下：

(2)根據(jù)運(yùn)動(dòng)模型預(yù)測(cè)AUV的位置；

(3)獲得新的環(huán)境測(cè)量值，并執(zhí)行數(shù)據(jù)關(guān)聯(lián)。采用獨(dú)立兼容最近鄰數(shù)據(jù)關(guān)聯(lián)方法(ICNN)，更新AUV位置及地圖。判斷新測(cè)量的特征是否是地圖中已經(jīng)存在的特征：

(a)若是則更新粒子權(quán)值以便更新AUV的位置估計(jì)，有

將權(quán)值歸一化，同時(shí)采用EKF更新地圖中特征的位置估計(jì)；

(b)否則，將新測(cè)量的特征加入到地圖中，完成地圖的擴(kuò)張過(guò)程；

(4)線性優(yōu)化重采樣。將簡(jiǎn)單重采樣中復(fù)制的粒子與偽拋棄組中的粒子進(jìn)行線性組合，并自適應(yīng)選出權(quán)值增大的粒子；

(5)輸出AUV的位置、地圖中特征的位置。

3 AUV同步定位與構(gòu)圖系統(tǒng)建模

3.1 AUV運(yùn)動(dòng)學(xué)模型

圖2 全局坐標(biāo)系G和載體坐標(biāo)系V之間的關(guān)系圖

3.2 特征模型

本文使用的特征數(shù)據(jù)是結(jié)構(gòu)化港口環(huán)境的測(cè)量數(shù)據(jù)，因而選用線特征來(lái)構(gòu)建環(huán)境特征地圖。環(huán)境中線特征是靜止的，所以其特征模型為

3.3 傳感器測(cè)量模型

AUV配置多普勒測(cè)速儀(DVL)、羅經(jīng)(compass)和壓力計(jì)等傳感器實(shí)時(shí)測(cè)量狀態(tài)向量中的速度、艏向和深度，建立線性測(cè)量模型為

采用機(jī)械掃描成像聲吶感知環(huán)境，其返回值直接表示在檢測(cè)到線特征時(shí)刻聲吶坐標(biāo)系中，通過(guò)與聲吶在船體坐標(biāo)系中的位置進(jìn)行復(fù)合運(yùn)算即可得到返回值在船體坐標(biāo)系中的坐標(biāo)。由于測(cè)量到新特征后需要進(jìn)行數(shù)據(jù)關(guān)聯(lián)，則需要將地圖中已經(jīng)存在的特征轉(zhuǎn)換到當(dāng)前的船體坐標(biāo)系中。

3.4 特征提取

目前國(guó)內(nèi)SLAM問(wèn)題研究中大多針對(duì)濾波方法進(jìn)行改進(jìn)，常常忽略了環(huán)境特征提取這一關(guān)鍵性步驟，由于SLAM方法相對(duì)于普通定位方法的優(yōu)勢(shì)即通過(guò)對(duì)環(huán)境特征的感知來(lái)校正AUV自身的位置，一旦特征提取錯(cuò)誤，勢(shì)必導(dǎo)致特征以及AUV定位精度的降低，因而只有準(zhǔn)確提取到環(huán)境中的特征，才能真正實(shí)現(xiàn)對(duì)AUV位置的校正。本文提出一種基于Hough變換的特征提取方法，根據(jù)獲得的聲吶數(shù)據(jù)，通過(guò)投票的方法提取到表示特征的參數(shù)。

將聲吶發(fā)射的每個(gè)波束所返回的一組距離、強(qiáng)度值稱(chēng)作bin。由于環(huán)境中的目標(biāo)在聲學(xué)圖像中表現(xiàn)為高強(qiáng)度回波信號(hào)，而非目標(biāo)區(qū)域表現(xiàn)為低強(qiáng)度回波信號(hào)，可見(jiàn)只有部分聲吶數(shù)據(jù)對(duì)于特征提取是有用的。因此，需要對(duì)聲吶數(shù)據(jù)進(jìn)行預(yù)處理：

(1)設(shè)置適當(dāng)?shù)拈撝?，將聲學(xué)圖像中強(qiáng)度值小于該閾值的bins剔除；

(2)從余下的bins中挑選出局部最大者并保留；

(3)采用“二者之間最小距離準(zhǔn)則”，若兩個(gè)bins之間的距離小于某一預(yù)定值，則認(rèn)為二者重合，刪除其中一個(gè)。

圖3 線特征提取的聲吶模型

霍夫空間中具體的投票過(guò)程如下：

(1)根據(jù)聲吶的分辨率將Hough空間量化；

4 基于IEKF-LOR-FastSLAM的AUV同步定位與構(gòu)圖仿真驗(yàn)證

4.1 基于海試數(shù)據(jù)的仿真試驗(yàn)

為了體現(xiàn)文中提出的IEKF-LOR-FastSLAM算法的優(yōu)越性，通過(guò)與標(biāo)準(zhǔn)FastSLAM算法及文獻(xiàn)[15]中提出的指數(shù)等級(jí)選擇重采樣FastSLAM算法進(jìn)行定位效果比對(duì)。根據(jù)式(16)所示的噪聲，單次仿真結(jié)果如圖4~圖6所示。

經(jīng)研究發(fā)現(xiàn)，指數(shù)等級(jí)選擇FastSLAM僅通過(guò)指數(shù)等級(jí)選擇算法產(chǎn)生兩個(gè)粒子的索引，然后將這兩個(gè)粒子以隨機(jī)值為系數(shù)進(jìn)行線性組合。由于該算法并未充分重視小權(quán)值粒子的作用，根據(jù)指數(shù)等級(jí)算法所選擇的粒子大多仍是具有大權(quán)值的粒子；此外，該算法并未考慮產(chǎn)生的新粒子是否優(yōu)于原粒子，而直接將產(chǎn)生的粒子放入到重采樣后的粒子集中。綜上兩種原因，使得指數(shù)等級(jí)選擇重采樣FastSLAM在長(zhǎng)期的同步定位與構(gòu)圖過(guò)程中穩(wěn)定性不及IEKF-LOR-FastSLAM。

圖7為采用改進(jìn)的FastSLAM得到的AUV同步定位與構(gòu)圖效果圖。黑色加粗實(shí)線為通過(guò)Hough變換提取的線特征，通過(guò)對(duì)所提取的線特征進(jìn)行反復(fù)觀察，AUV不斷校正自身的位置及環(huán)境中線特征的位置，這一舉措有效避免了航位推算過(guò)程中由于誤差積累導(dǎo)致AUV位置估計(jì)嚴(yán)重偏離的情況；同時(shí)AUV無(wú)需周期性上浮到水面進(jìn)行GPS校正。這是AUV完成長(zhǎng)航程、長(zhǎng)時(shí)間隱蔽任務(wù)的重要保證之一。

4.2 一致性分析

4.1節(jié)中進(jìn)行的基于海試數(shù)據(jù)的仿真試驗(yàn)表明，所設(shè)計(jì)的IEKF-LOR-FastSLAM算法能夠提高定位精度，但該過(guò)程具有動(dòng)態(tài)性，單次試驗(yàn)難以說(shuō)明其統(tǒng)計(jì)特性，需多次試驗(yàn)后，進(jìn)行一致性分析[16](通常，將參數(shù)估計(jì)看作靜態(tài)估計(jì)問(wèn)題，而將狀態(tài)估計(jì)看作動(dòng)態(tài)估計(jì)問(wèn)題。將靜態(tài)估計(jì)的一致性定義為收斂到真值。而針對(duì)文中研究的動(dòng)態(tài)估計(jì)問(wèn)題通常是無(wú)收斂性可言的，只能利用當(dāng)前的狀態(tài)估計(jì)值以及相關(guān)的協(xié)方差矩陣進(jìn)行一致性分析)。

下面分析IEKF-LOR-FastSLAM算法的一致性，通過(guò)歸一化估計(jì)協(xié)方差(Normalised Estimation Error Squared, NEES)[17]來(lái)定義濾波器的這一特性。濾波的一致性準(zhǔn)則為：狀態(tài)誤差均值為零，實(shí)際的協(xié)方差應(yīng)與濾波器得到的協(xié)方差的幅值相同。即

NEES定義為

圖5 北向估計(jì)誤差比較

圖6 定位效果比較

一致性分析結(jié)果如圖8所示。標(biāo)準(zhǔn)的FastSLAM算法只在短期內(nèi)是一致的，究其根本原因?yàn)橹夭蓸訉?dǎo)致的粒子多樣性丟失，即部分?jǐn)y帶AUV及路標(biāo)位置信息的粒子的丟失；基于指數(shù)等級(jí)選擇重采樣的FastSLAM算法一度改善了粒子的這一貧化現(xiàn)象，然而由于其并未充分重視大量小權(quán)值粒子的作用，在文獻(xiàn)[15]中的短期仿真試驗(yàn)中可以取得較好的效果，然而基于實(shí)際海試數(shù)據(jù)仿真時(shí)，效果并不是十分理想；基于IEKF-LOR-FastSLAM和指數(shù)等級(jí)選擇重采樣FastSLAM本質(zhì)上均為將粒子進(jìn)行線性組合以避免粒子的簡(jiǎn)單復(fù)制，由于其將拋棄的小權(quán)值粒子存入偽拋棄組并適當(dāng)加以利用從而使得估計(jì)的一致性得到保證，IEKF-LOR-FastSLAM一致性數(shù)據(jù)大部分落在雙邊置信區(qū)域附近。

5 結(jié)論

文中提出的IEKF-LOR-FastSLAM有效地避免了粒子的退化及貧化現(xiàn)象。通過(guò)融入最新觀測(cè)值在降低樣本退化的同時(shí)將復(fù)制的粒子與偽拋棄組中的粒子進(jìn)行線性組合，從而確保了粒子的多樣性，保存了更多粒子中攜帶的AUV及地圖的歷史信息。同時(shí)針對(duì)SLAM研究中對(duì)真實(shí)海洋環(huán)境中特征提取這一難點(diǎn)，根據(jù)聲吶模型設(shè)計(jì)了基于Hough變換的特征提取方法，并比較準(zhǔn)確地提取到了環(huán)境中的特征，通過(guò)對(duì)特征的重復(fù)觀察實(shí)現(xiàn)對(duì)AUV位置估計(jì)的校正。而這也正是SLAM優(yōu)越于普通定位方法的精髓所在。海試數(shù)據(jù)仿真試驗(yàn)表明，所提方法一定程度上提高了AUV以及環(huán)境中特征的位置估計(jì)精度，而且提高了估計(jì)的長(zhǎng)期一致性，對(duì)于AUV執(zhí)行長(zhǎng)航時(shí)海洋監(jiān)測(cè)與水下作業(yè)具有重要的理論價(jià)值及實(shí)際應(yīng)用意義。

感謝西班牙Girona大學(xué)的David博士等提供的利用Ictineu AUV獲得的港口試驗(yàn)數(shù)據(jù)集。

圖7 基于IEKF-LOR-FastSLAM的同步定位與構(gòu)圖效果

圖8 AUV位姿一致性分析

[1]Dissanayake G, Newman P, Clark S,.. A solution to the simultaneous localization and map building (SLAM) problem[J]., 2001, 17(3): 229-241.

[2]Kortenkamp D, Bonasso R P, and Murphy R. AI Based Mobile Robots: Case Studies of Successful Robot Systems[M]. Cambridge, Massachusetts, MIT Press, 1998: 23-45.

[3]Bosse M, Newman P, Leonard J,.. An Atlas framework for scalable mapping[C]Proceedings of the IEEE International Conference on Robotics and Automation, Piscataway, NJ, USA, 2003: 1899-1906.

[4]Tardós J D, Neira J, Newman P M,.. Robust mapping and localization in indoor environments using sonar data[J]., 2002, 21(4): 311-330.

[5]Guivant J E and Nebot E M. Optimization of the simultaneous localization and map-building algorithm for real-time implementation[J]., 2001, 17(3): 242-257.

[6]Kim J H and Sukkarieh S. Airborne simultaneous localisation and map building[C]. Proceedings of the IEEE International Conference on Robotics and Automation, Piscataway, NJ, USA, 2003: 406-411.

[7]He B, Zhang H J, Li C,.. Autonomous navigation for autonomous underwater vehicles based on information filters and active sensing[J]., 2011, 11(11): 10958-10980.

[8]He B, Liang Y, Feng X,.. AUV SLAM and experiments using a mechanical scanning forward-looking sonar[J]., 2012, 12(7): 9386-9410.

[9]歐陽(yáng)成, 姬紅兵, 郭志強(qiáng). 改進(jìn)的多模型粒子PHD和CPHD濾波算法[J]. 自動(dòng)化學(xué)報(bào), 2012, 38(3): 341-348.

Ouyang C, Ji H B, and Guo Z Q. Improved multiple model particle PHD and CPHD filters[J]., 2012, 38(3): 341-348.

[10]左軍毅, 張怡哲, 梁彥.自適應(yīng)不完全重采樣粒子濾波器[J]. 自動(dòng)化學(xué)報(bào), 2012, 38(4): 647-652.

Zuo J Y, Zhang Y Z, and Liang Y. Particle filter based on adaptive part resampling[J]., 2012, 38(4): 647-652.

[11]Montemerlo M, Thrun S, Koller D,.. FastSLAM: a factored solution to the simultaneous localization and mapping problem[C]. AAAI National Conference on Artificial Intelligence, Piscataway, NJ, USA, 2002: 593-598.

[12]Durrant-Whyte H, and Bailey T. Simultaneous localization and mapping (SLAM): Part I, the essential algorithms[J]., 2006, 13(2): 99-108.

[13]張俊根, 姬紅兵. IMM迭代擴(kuò)展卡爾曼粒子濾波跟蹤算法[J].電子與信息學(xué)報(bào), 2010, 32(5): 1116-1120.

Zhang J G and Ji H B. IMM iterated extended Kalman particle filter based target tracking[J].&, 2010, 32(5): 1116-1120.

[14]Ribas D. Underwater SLAM for structured environments using an imaging sonar[D].[Ph.D. dissertation], Girona University, 2008.

[15]張亮, 蔣榮欣, 陳耀武. 移動(dòng)機(jī)器人在未知環(huán)境下的同步定位與地圖重建方法[J]. 控制與決策, 2010, 25(4): 515-520.

Zhang L, Jiang R X, and Chen Y W. Algorithm for mobile robots’ simultaneous localization and mapping in unknown environments[J]., 2010, 25(4): 515-520.

[16]Bar-Shalom Y, Li X R, and Kirubarajan T. Estimation with Applications to Tracking and Navigation[M]. London, John Wiley and Sons, 2001: 33-72.

[17]Bailey T, Nieto J, and Nebot E. Consistency of the FastSLAM algorithm[C]. Proceedings 2006 IEEE International Conference on Robotics and Automation, Piscataway, NJ, USA, 2006: 424-429.

王宏?。?女，1971年生，教授，博士生導(dǎo)師，研究方向?yàn)榇芭c水下航行器自主控制與仿真.

王晶： 女，1984年生，博士生，研究方向?yàn)樽灾魉潞叫衅鲗?dǎo)航技術(shù)、海洋環(huán)境特征提取技術(shù).

Fast Simultaneous Localization and Mapping Based on Iterative Extended Kalman Filter Proposal Distribution and Linear Optimization Resampling

Wang Hong-jian①Wang Jing①②Liu Zhen-ye②

①(,,150001,)②(8357,300308,)

The location estimated accuracy of Autonomous Underwater Vehicle (AUV) and landmarks decrease because of the degeneracy and impoverishment of samples in standard Fast Simultaneous Localization And Mapping (FastSLAM) algorithm. A improved FastSLAM algorithm based on Iterative Extended Kalman Filter (IEKF) proposal distribution and linear optimization resampling is presented in order to solve this issue. The latest observation is integrated with IEKF in order to decrease the sample degeneracy while the new samples are produced by the linear combination of copied samples and some abandoned ones in order to reduce the sample impoverishment. The kinematic model of AUV, feature model and the measurement models of sensors are all established. And then features are extracted with Hough transform to build the global map. The experiment of the improved FastSLAM algorithm with trial data shows that it can avoid the degeneracy and impoverishment of samples effectively and enhance the location estimation accuracy of AUV and landmarks. Moreover, the consistency analysis showed that the method possesses the consistency of long term.

Simultaneous Localization And Mapping (SLAM); Iterative Extended Kalman Filter (IEKF) proposal distribution; Linear optimization resampling; Feature extraction

TP391; TP242.6

A

1009-5896(2014)02-0318-07

10.3724/SP.J.1146.2012.01373

王宏健 cctime99@163.com

2012-10-26收到，2013-10-31改回

國(guó)家自然科學(xué)基金(E091002/50979017)，教育部高等學(xué)校博士學(xué)科點(diǎn)專(zhuān)項(xiàng)科研基金(20092304110008)，中央高?；究蒲袠I(yè)務(wù)費(fèi)專(zhuān)項(xiàng)資金(HEUCFZ 1026)和哈爾濱市科技創(chuàng)新人才研究專(zhuān)項(xiàng)資金(2012RFXXG083)資助課題