混合新生地图信息的随机有限集同步定位与地图创建方法

史剑鸣,章 飞,曾庆军,孙陶莹

(江苏科技大学 电子信息学院,镇江 212003)

由于水下环境复杂,通过传感器获得观测信息的方式单一,且无法获得先验环境的准确信息,因此许多陆上经典导航方法运用于水下机器人导航定位问题时存在许多制约与不足[1]．定位与地图创建(simultaneous localization and mapping, SLAM)方法是一种重要且常用的自主导航方法[2-4]．传统SLAM方法在密集杂波和特征数目多的水下环境中存在数据关联精度低、计算复杂的问题,且在SLAM实现过程中,由于机器人运动过程中的误差、传感器的观测误差、以及传感器不确定性造成地图特征漏检等问题,都将影响数据关联过程中的准确性,因此,传统SLAM方法的准确性对地图特征信息的数据关联非常敏感[4-7]．

近年来,基于随机有限集合(random finite sets, RFS)理论的概率假设密度SLAM(PHD-SLAM)方法获得了广泛的关注[8-11],这是一种在估计地图特征时不依赖数据关联,并将传统SLAM方法中所忽略的由于传感器不确定性造成的地图特征漏检概率带入地图特征滤波的SLAM新方法,适用于数据关联模糊、目标特征多且变化剧烈的环境中[1,4-6]．与传统的SLAM方法相比，在地图特征数目估计,地图特征位置精度估计,机器人位姿的估计,以及滤波算法的运行速度上都有较大改进[4,12-14]．但由于PHD算法的特性,需要提供一定先验信息才能递归进行滤波运算,而SLAM过程是在未知环境中从未知位置进行运动,无法提供先验信息,文献[15]提出一种新特征策略,将移动机器人上一时刻的观测集合作为当前时刻的先验信息集合,带入PHD滤波当中，从而估计出新出现地图特征的位置．然而,由于新生目标集合仅仅采用上一时刻的观测集合,因此在地图特征位置及数目的估计精度上存在不足．

为此,文中提出一种混合新目标信息的随机有限集SLAM方法(mixed birth map information RFS-SLAM, MBMI-PHD-SLAM)．该方法将上一时刻之前PHD-SLAM已探测过的地图信息中靠近机器人的地图信息加入上一时刻的观测集合中,作为PHD-SLAM预测过程中新生地图特征集合．当机器人重新回到已探测过的环境中或已探测过的地图特征重新出现在机器人视域内时,通过增加先验信息,改进PHD-SLAM方法对地图特征数目估计及地图特征位置精度估计．

1 PHD-SLAM问题描述

1.1 地图信息的RFS模型

(1) 地图特征的RFS模型

不同于传统SLAM方法,PHD-SLAM方法的观测信息以及地图特征不再采用向量序列的形式表示,而是使用随机有限集合形式来表示．地图特征的数目从零到任意大小随意变化,随着在机器人不断出现在机器人视域内的地图特征而单调增加,每个地图特征之间是相互独立的[15]．

(1)

(2) 地图观测的RFS模型

在k时刻,位姿为Xk的机器人获得的特征观测集合表示为Zk,其RFS模型为

(2)

Dk(m,Xk)建模为一个Bernoulli型RFS,Dk(m,Xk)=φ的概率为1-PD(m|Xk)[12],表示由于传感器的不确定性造成的地图特征漏检概率；Dk(m,Xk)={z}的概率密度为PD(m|Xk)gk(z|m,Xk),表示能够检测到地图特征的概率，其中PD(m|Xk)表示当机器人位姿为Xk时,能够探测到位于m处的地图特征概率，gk(z|m,Xk)表示机器人携带的传感器模型．

1.2 PHD的地图创建

PHD-SLAM的实现过程与FastSLAM相似,采用Rao-Blackwellised(RB)粒子滤波器实现机器人位姿后验估计．地图信息不再采用FastSLAM中的向量序列的方式进行预测和更新,而是使用PHD滤波描述地图特征的状态,并根据每一时刻的观测更新地图特征．

(1) 地图预测

Vk|k-1(m|Xk)=Vk-1|k-1(m|Xk-1)+b(m|Xk)

(4)

式中：Vk-1|k-1(m|Xk-1)为k-1时刻根据机器人位姿Xk-1得到的后验地图特征信息RFS的PHD；Vk|k-1(m|Xk)为k时刻预测可能出现的地图特征RFS的PHD；b(m|Xk)为随着机器人运动可能进入机器人视域内新生地图特征信息RFS的PHD．

(2) 地图的更新

Vk|k(m|Xk)=Vk|k-1(m|Xk)(1-PD(m|Xk))+

(5)

式中：Vk|k(m|Xk)为k时刻后验地图特征信息RFS的PHD；Λ(m|Xk)=PD(m|Xk)gk(z|m,Xk),PD(m|Xk)表示位姿为Xk的机器人能够探测到位于m处地图特征的概率；ck(z)为k时刻由杂波引起的杂波RFS的PHD．

上述RB-PHD-SLAM方法中,b(m|Xk)提供地图特征信息滤波器先验信息,用以确定新的地图特征在机器人视域内可能出现的位置.而SLAM问题是在未知的环境中从未知地点出发,缺乏先验信息,传统的PHD-SLAM方法在k时刻使用k-1观测集合作为k时刻的b(m|Xk)，因此对机器人视域内新出现的地图特征估计存在滞后.尤其当机器人重新回到已探测过的环境中或已探测过的地图特征重新出现在机器人视域内时，未能利用已经探测过的地图信息来改善对当前时刻地图特征的估计精度．针对这一问题,文中提出一种混合新生地图信息策略作为b(m|Xk),将上一时刻之前探测到的地图信息中靠近机器人位置的地图特征加入到观测信息中,作为新生地图信息集合,通过增加先验信息用以改善当前时刻地图特征的估计精度．

2 新生地图特征的RFS建模

在估计地图特征目标数目以及地图特征精度中,预测阶段起重要作用,文中使用的混合新生目标信息的方法,增加预测阶段中的新生地图特征RFS的PHD,当机器人从初始时刻出发时由于缺乏先验信息,使用k-1观测集合作为k时刻可能出现新地图特征信息b(m|Xk)．当机器人经过PHD-SLAM更新出已探测区域地图信息后,将已探测地图信息中靠近机器人位姿的地图特征作为先验信息,加入上一时刻观测集合中一起作为当前时刻的b(m|Xk)带入当前时刻的PHD预测阶段．

b(m|Xk)=Zk-1+Mf

(6)

(7)

式中：Mf为已探测地图中靠近机器人位姿的地图特征的RFS的PHD；ν为机器人的速度；R为机器人所携带传感器扫描半径；m为经过PHD-SLAM方法得出的已探测地图特征信息．根据机器人速度ν和传感器扫描半径R,确定出一个阈值T,当已探索过的地图特征集合内有地图特征位置与机器人位置之间距离小于这一阈值时,将该地图特征作为可能出现在当前时刻机器人视域内的新地图特征,γ(m|Xk-1)表示已探测地图特征中小于阈值的地图特征形成的RFS的PHD．上述方法实现的伪代码如下:

1 ifM=φ

2b(m|Xk)=Zk-1

3 else ifM={m1,m2…mn}

4 fori=1 ton

5 T=R+ν

6 if (mi-Xk-1)

7Mf=Mf+mi

8 end for

9b(m|Xk)=Zk-1+Mf

10 endif

混合新生地图信息策略中的b(m|Xk)不仅包括k-1时刻的观测集合还加入了已探测区域内地图信息中靠近机器人位姿的地图特征集合,当机器人视域内已探测过的地图特征重新出现时,加入b(m|Xk)中作为先验信息的这部分地图特征,经过PHD-SLAM地图滤波后,重新出现的地图特征的权重会增加,从而提高了对地图特征点位置和数目的估计精度．

3 MBMI-PHD-SLAM方法及实现

文中提出的MBMI-PHD-SLAM方法递归的过程中,机器人位姿估计分布使用Rao-Blackwellised粒子滤波器,每一个粒子的位姿代表机器人可能出现的位姿,粒子的权重代表可能性的大小,在机器人位姿已知情况下,采用PHD滤波器描述地图特征的概率分布,并利用观测集合进行更新．在k-1时刻,粒子集合的PHD-SLAM的强度可以表示为:

每个粒子所对应的地图后验分布采用高斯混合项的方式实现,即PHD的分布由一系列的加权高斯项表示,每一个高斯项的峰值表示路标可能存在的位置,可能性的大小由权重体现．

3.1 每个粒子的GM-PHD地图估计

(1) 预测阶段

根据每个粒子采样位置,可以得到k时刻新出现特征的高斯混合项:

(8)

(9)

根据式(4)可知,地图预测的高斯混合PHD为:

(10)

k时刻地图特征预测的高斯混合项包括k时刻新出现的地图特征高斯混合项,以及k-1时刻PHD滤波后得到的高斯混合项,即k-1时刻PHD滤波得到的老地图特征依然存在的高斯混合项．

为比较不同MCMC抽样算法的计算效率，选择陕北地区刘家河、神木等5个水文测站的年最大洪峰流量系列资料进行应用，资料概况如表1所示。

(2) 更新阶段

每个粒子的观测似然函数也采用高斯混合的形式,因此更新后地图的后验PHD同样具有高斯混合的形式

(11)

(12)

(13)

(14)

式中：μk|k,Pk|k,Sk可以通过基于EKF和UKF的PHD滤波器更新获得,由于EKF-PHD的结构简单,运算速度快,因此文中采用EKF-PHD的方式实现后验地图的估计更新．

(3) PHD的剪切、合并与地图特征的提取

PHD剪切、合并完成后,对高斯分量权重大于地图特征提取门限Tf的高斯分量进行地图特征提取,其高斯项的均值表示地图特征的位置,满足门限Tf高斯分量的个数表示地图特征的数目．

3.2 机器人位姿估计

机器人位姿估计采用RB粒子滤波器实现,实现过程包括两部分:粒子重要性采样以及粒子重采样．

(15)

(16)

(2) 粒子重采样

4 仿真结果及分析

4.1 仿真环境参数

文中的仿真环境为Matlab7,地图大小为50 m×60 m,地图特征数为30个,声呐传感器扫描角度为360°,扫描半径为10 m．机器人运动速度为3 m/s．运动噪声为均值为0,方差为0.5 m的高斯噪声,粒子个数为20个,根据经验值并参考文献[8]选取Tp=10-5．为验证改进的MBMI-PHD-SLAM方法的性能,将文中方法和文献[6]中RB-PHD-SLAM方法分别在杂波密度较低和较高的情况下进行仿真比较,对机器人定位误差,地图特征数目估计及地图特征位置估计误差进行对比分析．

4.2 仿真结果及分析

(1) 低杂波密度环境下仿真结果及分析

图1～4为轻度噪声观测环境下MBMI-PHD-SLAM和RB-PHD-SLAM运行50次地图仿真结果,轻度噪声环境杂波强度为3,杂波服从Poisson分布,观测距离噪声为均值为0,方差为0.3 m的高斯噪声．传感器检测地图特征目标概率PD=0.95．

图1 低杂波密度下MBMI-PHD-SLAM结果Fig.1 Results of MBMI-PHD-SLAM with low clutter density

图2是两种方法对每一步机器人位姿误差对比,图3是地图特征数目误差对比,图4是地图特征位置估计误差对比,文中采用最优子模型分配距离(optimal subpattern assignment, OSPA) 进行评价集合之间差异程度的误差距离．

图2 低杂波密度下机器人位姿误差对比Fig.2 Comparison of vehicle position with low clutter density

图3 低杂波密度下地图特征数目估计误差对比Fig.3 Comparison of map feature number estimation error with low clutter density

从图中对比中可知,当机器人在开始阶段两种方法并无太大差距,而当25步后地图信息构建完成,机器人重新回到已探测过的环境中,已探测地图中地图特征可能重新出现在机器人视域内时,MBMI-PHD-SLAM方法的地图特征数目误差及OSPA误差开始减小且变化平缓波动小．

图4 低杂波密度下OSPA误差对比Fig.4 Comparison of map OSPA error with low clutter density

将表1和表2两种方法进行对比可知在低密度杂波下,机器人在25步之前,处在未探测过的环境中时,地图特征数目误差及地图特征位置误差基本相同．但当25步之后MBMI-PHD-SLAM方法对于地图特征数目估计精度提高了72.48%,地图特征位置估计精度提高了49.53%,机器人位姿估计精度提高了37.01%．通过以上数据的对比分析可以看出,当机器人重新回到已探测过的环境时,与RB-PHD-SLAM方法相比,文中所提方法在对地图特征数目和位置以及机器人位置的估计精度方面均有大幅度的提高．

表1 未探测过的环境(25步之前)Table 1 Unexplored environment(25 steps before) m

表2 重新回到探测过的环境(25步之后)Table 2 Back to the detected environment(25 steps before) m

(2) 高密度杂波仿真结果及分析

图5～8为高密度杂波下MBMI-PHD-SLAM方法运行50次仿真结果,重度噪声环境杂波强度为10,杂波服从Poisson分布,观测距离噪声为均值为0,方差为0.3 m的高斯噪声．传感器检测地图特征目标概率PD=0.95．

图5 高杂波密度下MBMI-PHD-SLAMFig.5 Results of MBMI-PHD-SLAM with high clutter density

图6是两种算法对机器人位姿估计误差对比,图7,图8为每一步地图特征数目误差及地图特征位置误差对比．

图6 高杂波密度下机器人位姿估计误差对比Fig.6 Comparison of vehicle position with high clutter density

图7 高杂波密度下地图特征数目估计误差对比Fig.7 Comparison of map feature number estimation error with high clutter density

图8 高杂波密度下OSPA误差对比Fig.8 Comparison of map OSPA error with high clutter density

25步后地图信息构建完成,机器人重新回到已探测过的环境中,已探测地图中地图特征可能重新出现在机器人视域内时MBMI-PHD-SLAM方法相较于传统RB-PHD-SLAM在地图特征数目估计以及地图特征位置估计上仍有显著提高．

与轻噪声环境结果相比,重噪声环境下两种方法的性能有所下降,但25步后RB-PHD-SLAM地图特征数目变化程度以及OSPA误差变化程度仍大于MBMI—PHD-SLAM．

由表3，4可知,当25步之后MBMI-PHD-SLAM对于地图特征数目估计精度提高了75.78%,地图特征位置估计精度提高了33.95%,机器人位姿估计精度提高了25.07%．通过表中数据对比分析可知,当机器人重新回到已探测过的环境中时,文中所提方法相比RB-PHD-SLAM方法,在对地图特征数目和位置以及机器人位置的估计精度方面有大幅度的提高．

表3 未探测过的环境(25步之前)Table 3 Unexplored environment(25 steps before) m

表4 重新回到探测过的环境(25步之后)Table 4 Back to the detected environment (25 steps before) m

5 结论

(1)针对在密集杂波和地图特征数目多且变化剧烈的水下环境中,水下机器人对地图特征点位置和数目以及机器人自身位置估计精度较低的问题,提出了MBMI-PHD-SLAM方法,该方法使用随机有限集合的方式更加准确的描述地图特征信息,地图特征观测信息,以及传感器不确定性造成的漏检概率等真实环境因素,并可在使用少量粒子数目的情况下依旧能够得到较高的地图估计精度及机器人位姿估计精度．

(2) 使用混合新生地图特征策略改进了传统RB-PHD-SLAM,通过增加先验信息,明显提高了对地图特征位置和数目估计精度及机器人位姿的估计精度．

(3) 通过两种不同杂波密度环境下的仿真实验,验证了该方法在提高位姿估计精度以及地图估计精度方面的有效性．