基于迭代擴展Kalman濾波建議分布和線性優化重采樣的快速同步定位與構圖

王宏健 王 晶 劉振業

?

基于迭代擴展Kalman濾波建議分布和線性優化重采樣的快速同步定位與構圖

王宏健*①王 晶①②劉振業②

①(哈爾濱工程大學自動化學院 哈爾濱 150001)②(中國航天科工集團第8357研究所 天津 300308)

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

同步定位與構圖；迭代擴展Kalman濾波建議分布；線性優化重采樣；特征提取

1　引言

機器人在部分或完全未知的環境中，自身位置不確定的情況下，利用自身攜帶的傳感器來感知周圍信息，通過對環境中路標的反復觀察完成自身及路標位置的校正，這一過程可以同時實現機器人自身的定位和環境地圖的創建，這一技術被稱為同步定位與構圖(Simultaneous Localization And Mapping, SLAM)。這一能力被很多人認為是機器人真正實現自主的先決條件[1,2]。近年來，眾多學者提出各種SLAM技術和方法用于解決室內[3,4]、室外[5]、空中[6]、甚至水下[7,8]無人機器人的自主導航定位問題。SLAM作為一種地形輔助導航方法，對于自主水下航行器(AUV)的自主導航定位具有重要的意義。

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

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

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

2.1 IEKF建議分布

對于非線性系統，EKF通過泰勒級數展開并忽略高階項對非線性系統模型進行線性化，這無法避免地會引入截斷誤差，導致狀態估計的性能較差。然而，IEKF能夠得到狀態的最大后驗概率估計，其性能優于傳統的EKF[13]。所以，用IEKF產生建議分布，將更符合狀態變量的實際后驗概率分布。

對于非線性系統

預測過程：

2.2 線性優化重采樣

線性優化重采樣的基本思想為：在需要復制某個樣本時，將該樣本和符合一定條件的被拋棄的樣本進行適當的線性組合以產生一個新樣本，并用這些新樣本來代替簡單重采樣中復制的樣本，從而避免樣本的簡單復制。通過這種組合，可以使粒子的分布更加接近真實的后驗概率分布。

具體的線性組合方式為

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

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

IEKF-LOR-FastSLAM中AUV路徑和路標位置所組成的聯合概率分布可以表示為

采用粒子濾波器完成AUV的位置估計，可以有效地從樣本空間中采樣；而路標的位置估計則采用Kalman濾波器來實現，不同的路標采用獨立的濾波器。

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

(2)根據運動模型預測AUV的位置；

(3)獲得新的環境測量值，并執行數據關聯。采用獨立兼容最近鄰數據關聯方法(ICNN)，更新AUV位置及地圖。判斷新測量的特征是否是地圖中已經存在的特征：

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

將權值歸一化，同時采用EKF更新地圖中特征的位置估計；

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

(4)線性優化重采樣。將簡單重采樣中復制的粒子與偽拋棄組中的粒子進行線性組合，并自適應選出權值增大的粒子；

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

3 AUV同步定位與構圖系統建模

3.1 AUV運動學模型

圖2 全局坐標系G和載體坐標系V之間的關系圖

3.2 特征模型

本文使用的特征數據是結構化港口環境的測量數據，因而選用線特征來構建環境特征地圖。環境中線特征是靜止的，所以其特征模型為

3.3 傳感器測量模型

AUV配置多普勒測速儀(DVL)、羅經(compass)和壓力計等傳感器實時測量狀態向量中的速度、艏向和深度，建立線性測量模型為

采用機械掃描成像聲吶感知環境，其返回值直接表示在檢測到線特征時刻聲吶坐標系中，通過與聲吶在船體坐標系中的位置進行復合運算即可得到返回值在船體坐標系中的坐標。由于測量到新特征后需要進行數據關聯，則需要將地圖中已經存在的特征轉換到當前的船體坐標系中。

3.4 特征提取

目前國內SLAM問題研究中大多針對濾波方法進行改進，常常忽略了環境特征提取這一關鍵性步驟，由于SLAM方法相對于普通定位方法的優勢即通過對環境特征的感知來校正AUV自身的位置，一旦特征提取錯誤，勢必導致特征以及AUV定位精度的降低，因而只有準確提取到環境中的特征，才能真正實現對AUV位置的校正。本文提出一種基于Hough變換的特征提取方法，根據獲得的聲吶數據，通過投票的方法提取到表示特征的參數。

將聲吶發射的每個波束所返回的一組距離、強度值稱作bin。由于環境中的目標在聲學圖像中表現為高強度回波信號，而非目標區域表現為低強度回波信號，可見只有部分聲吶數據對于特征提取是有用的。因此，需要對聲吶數據進行預處理：

(1)設置適當的閾值，將聲學圖像中強度值小于該閾值的bins剔除；

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

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

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

霍夫空間中具體的投票過程如下：

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

4 基于IEKF-LOR-FastSLAM的AUV同步定位與構圖仿真驗證

4.1 基于海試數據的仿真試驗

為了體現文中提出的IEKF-LOR-FastSLAM算法的優越性，通過與標準FastSLAM算法及文獻[15]中提出的指數等級選擇重采樣FastSLAM算法進行定位效果比對。根據式(16)所示的噪聲，單次仿真結果如圖4~圖6所示。

經研究發現，指數等級選擇FastSLAM僅通過指數等級選擇算法產生兩個粒子的索引，然后將這兩個粒子以隨機值為系數進行線性組合。由于該算法并未充分重視小權值粒子的作用，根據指數等級算法所選擇的粒子大多仍是具有大權值的粒子；此外，該算法并未考慮產生的新粒子是否優于原粒子，而直接將產生的粒子放入到重采樣后的粒子集中。綜上兩種原因，使得指數等級選擇重采樣FastSLAM在長期的同步定位與構圖過程中穩定性不及IEKF-LOR-FastSLAM。

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

4.2 一致性分析

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

下面分析IEKF-LOR-FastSLAM算法的一致性，通過歸一化估計協方差(Normalised Estimation Error Squared, NEES)[17]來定義濾波器的這一特性。濾波的一致性準則為：狀態誤差均值為零，實際的協方差應與濾波器得到的協方差的幅值相同。即

NEES定義為

圖5 北向估計誤差比較

圖6 定位效果比較

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

5 結論

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

感謝西班牙Girona大學的David博士等提供的利用Ictineu AUV獲得的港口試驗數據集。

圖7 基于IEKF-LOR-FastSLAM的同步定位與構圖效果

圖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]歐陽成, 姬紅兵, 郭志強. 改進的多模型粒子PHD和CPHD濾波算法[J]. 自動化學報, 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]左軍毅, 張怡哲, 梁彥.自適應不完全重采樣粒子濾波器[J]. 自動化學報, 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迭代擴展卡爾曼粒子濾波跟蹤算法[J].電子與信息學報, 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]張亮, 蔣榮欣, 陳耀武. 移動機器人在未知環境下的同步定位與地圖重建方法[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年生，教授，博士生導師，研究方向為船舶與水下航行器自主控制與仿真.

王晶： 女，1984年生，博士生，研究方向為自主水下航行器導航技術、海洋環境特征提取技術.

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改回

國家自然科學基金(E091002/50979017)，教育部高等學校博士學科點專項科研基金(20092304110008)，中央高校基本科研業務費專項資金(HEUCFZ 1026)和哈爾濱市科技創新人才研究專項資金(2012RFXXG083)資助課題