王宏健 王 晶 刘振业



基于迭代扩展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)资助课题