结合传感器融合与退火参数改进RBPF-SLAM算法

李 旦,王冠凌,丰宇航

(安徽工程大学 a.电气工程学院;b.高端装备先进感知与智能控制教育部重点实验室,安徽 芜湖 241000)

0 引言

为确定移动机器人在未知环境中的位置,需要对移动机器人进行定位,然而定位又建立在环境地图的基础上。最早Smith等[1]提出同时定位与地图构建(simultaneous localization and mapping,SLAM)的概念,主要内容为移动机器人根据位姿估计与传感器获取的未知环境地图信息,实现环境地图的构建。目前SLAM仍然是机器人等研究领域的热点[2-3]。

早期主要是由扩展卡尔曼滤波(extended kalman filter,EKF)和粒子滤波(particle filter,PF)解决SLAM问题[4-5]。在非线性高斯系统中,EKF的精度并不高,尤其是在环境特征量较大时,计算的复杂度高,并且EKF难以处理数据关联的问题[6]。粒子滤波是用粒子集近似后验概率密度分布,能够有效处理非线性非高斯分布的问题,因而粒子滤波在SLAM等研究领域应用广泛。

Doucet等[7]利用RBPF算法处理SLAM问题,Montemerlo等[8]在此基础上提出Fast-SLAM算法(RBPF-SLAM)。该算法将机器人定位和地图更新分开进行,先对位姿进行估计,再根据位姿构建环境地图,同时每个粒子都包含一种运动轨迹以及一幅环境地图。因此RBPF-SLAM算法相较于EKF和PF算法,实时性和效率更高,可以有效解决SLAM问题。但该算法中所用的粒子数目较多,计算量大,此外频繁地执行重采样可能会剔除有效粒子,造成粒子退化[9]。

Griseti等为改进RBPF-SLAM算法提出了Gmapping算法[10]34。此算法在计算提议分布时不仅使用移动机器人的运动模型,同时还考虑激光雷达传感器的观测信息,这样使得提议分布更加精准,提高了算法性能。为减少重采样的次数引入自适应重采样,从而重采样只有在粒子数下降至阈值以下时才执行,由权重高的粒子来代替权重低的粒子。Gmapping算法在闭环较多的环境地图中仍需要较多粒子数目,并且构建的栅格地图效果不理想,其稳定性有待提高。潘杨杰等[11-12]利用轮式里程计信息与IMU进行融合,准确地估计轮式机器人的位姿。孙弋等[13-14]利用退火参数优化混合提议分布,使得优化后的提议分布更加接近目标分布。王依人等[15]引入自适应重采样,减轻频繁执行重采样造成粒子退化的问题。

目前RBPF-SLAM算法中利用里程计估计移动机器人位姿,且仅用运动模型作为提议分布,从而影响构建环境地图精度。因此本研究提出融合轮式里程计信息和IMU信息提高位姿精度以及利用退火参数优化混合提议分布,有效减少采样粒子数目和计算量。

1 RBPF-SLAM算法

RBPF-SLAM算法主要是利用雷达传感器观测信息z1∶t=z1,z2,…，zt和轮式里程计信息u1∶t-1=u1,u2,…，ut-1来估计移动机器人的运动轨迹x1∶t=x1,x2,…，xt和环境地图m的后验概率p(x1∶t,m|z1∶t,u1∶t-1)。RBPF-SLAM算法将联合后验概率分解为如下形式:

p(x1∶t,m|z1∶t,u1∶t-1)=p(x1∶t|z1∶t,u1∶t-1)p(m|x1∶t,z1∶t)

。

(1)

公式(1)中将p(x1∶t,m|z1∶t,u1∶t-1)分解为两个独立的后验概率乘积,先估计移动机器人的运动轨迹,然后由运动轨迹结合观测数据更新环境地图。p(m|x1∶t,z1∶t,u1∶t-1)为地图的后验概率,p(x1∶t|z1∶t,u1∶t-1)为运动轨迹的后验概率,需要粒子滤波器对潜在的运动轨迹进行估算,同时每个粒子都具有一种运动轨迹。由这些粒子的运动轨迹和系统的观察构建最终的环境地图。

RBPF-SLAM算法采用序列重要性重采样滤波器(SIR)对移动机器人进行位姿估计和环境地图的更新,其具体流程可以分为以下4个步骤:

(2)

由以上四个步骤得出,权重的计算需要根据上一时刻的数据,导致计算量会变得越来越大。Doucet等提出限制提议分布q,得到计算重要性权重的递归公式[16]:

(3)

根据文献[10]得到计算展开公式[10]34为

(4)

由公式(2)、(3)、(4)可求得如下权重计算公式：

(5)

2 改进RBPF-SLAM算法

2.1 里程计和IMU信息融合

应用EKF算法主要估计移动机器人的位姿信息,利用轮式里程计和IMU建立位姿融合的状态模型。由于里程计和IMU数据中都有移动机器人的转向角度和速度的信息,故位姿状态向量为

(6)

(7)

里程计的数据可以直接获取,则得到其预测方程为

(8)

其中:Hodom为轮式里程计的预测矩阵It;eodom,t(d)为轮式里程计的误差,且它为服从高斯分布N(0,Rodom(d))的协方差矩阵。

由于只是研究二维空间的环境地图,因此对于IMU仅需要三轴中Z的轴数据,则IMU的预测方程可表示为

(9)

其中:HIMU为IMU的预测矩阵;eIMU,t为IMU数据的误差,且它为服从高斯分布N(0,RIMU)的协方差矩阵。

将轮式里程计和IMU的预测方程进行联合,从而得到系统的预测方程为

(10)

其中:系统的预测矩阵H为

(11)

系统的噪声协方差矩阵R(d)为

(12)

将移动机器人位姿信息的状态转移方程、预测方程和误差模型代入如下扩展卡尔曼滤波的公式(13)和公式(14)中就可实现移动机器人的位姿估计。

扩展卡尔曼滤波预测模型为

(13)

扩展卡尔曼滤波更新模型为

(14)

2.2 混合提议分布

如图1所示:实线为p(z|x)的概率分布,它是由激光雷达感知环境信息获得的高斯分布;虚线为p(x|x′,u)的概率分布,它是由轮式里程计所获得的高斯分布。

图1 观测模型和运动模型似然函数分布

由图1可知观测模型提供的信息准确度相比运动模型的准确度要高很多。RBPF-SLAM算法中运动模型相对于观测模型的噪声较大,仅使用运动模型会导致提议分布的精度低。为了解决上述问题,将轮式里程计与IMU数据融合得到精准的运动模型与激光雷达的联合模型,作为混合提议分布,如式(15)所示,

(15)

加入观测模型的粒子权重公式变成：

(16)

(17)

(18)

高斯分布构建混合提议分布之后就能对下一时刻移动机器人的位姿信息进行采样。此时粒子权重的计算公式为

(19)

加入精确的激光雷达传感器观测数据,使得重要性权重方差小,但是积分计算相对比较复杂,当观测模型中出现峰值分布,会对采样效率造成影响,致使滤波器的发散[17]。故采用退火参数α对混合提议分布中两种模型的比重进行调节,得到结果如式(20)所示:

(20)

对应的粒子权重计算公式为

(21)

其中退火参数范围为0≤α≤1。调整退火参数的关键在于重要性采样之后,对运动模型数据和观测模型数据进行比较,当运动模型数据比观测模型数据大时,需减小重要性权重的占比,即将α数值减小。反之,则增大α数值。通过实验以及观测数据与目标分布之间的关系对比得出,当提议分布中运动模型起主导作用时,取α为0.6;当传感器的观测模型接近目标分布时,取α为0.02,提高观测模型的比例。

2.3 自适应重采样

重采样步骤对粒子滤波具有重要影响,在重采样期间,将权重高的粒子代替权重低的粒子。然而频繁地执行重采样可能将有效粒子剔除,使得粒子退化[18]。故由Neff来表示粒子退化程度,其公式为

(22)

2.4 RBPF-SLAM算法改进流程

2) 由式(20)求得混合提议分布。

3) 在混合提议分布中对粒子进行采样。

4) 根据式(21)计算并更新每个粒子权重。

5) 进行自适应重采样操作。

3 实验结果和分析

3.1 改进RBPF-SLAM算法的实现

为测试改进RBPF-SLAM算法的性能,开展验证实验。仿真实验在一台操作系统为Ubuntu18.04的计算机(处理器:Intel Core i5-9400;RAM:8.00 GB)上运行,机器人操作系统(robot operating system,ROS)使用melodic版本运行RBPF-SLAM算法和改进的RBPF-SLAM算法。通过ROS平台的Gazebo软件里搭建仿真环境,移动机器人底盘使用两轮差速移动,装载轮式里程计、IMU和二维激光雷达。本实验搭建了两组仿真环境,大小分别为15 m×13 m和22 m×21 m。

ROS还自带一个三维可视化工具RVIZ,本研究中涉及的RVIZ用于显示激光雷达点云以及实时绘制环境地图。图2中两幅图是由传统RBPF-SLAM算法使用30个采样粒子构建出的二维栅格地图,由于仅用轮式里程计作为提议分布,导致该算法的误差较大,构建的环境地图部分出现了偏差。图3中两幅图是由改进的RBPF-SLAM算法使用15个采样粒子构建二维栅格地图,从地图的效果来看比传统算法构建的环境地图更加清晰准确。图2和图3中白色区域为激光雷达已扫描到的区域,黑色线条代表物体的轮廓,灰色区域则是激光雷达未能扫描到的地图区域。

图2 RBPF-SLAM算法构建环境地图

图3 改进RBPF-SLAM算法构建环境地图

从上述两组对比实验来看,改进的RBPF-SLAM算法通过使用较少的粒子数目可以构建出更加精确的环境地图。

3.2 实验结果分析

为了更加直观地分析改进的算法的位姿精度,在150 s内,将传统RBPF-SLAM算法与改进的算法在的环境地图中对位姿状态估计的精确性进行比较。由图4位姿状态估计的仿真曲线可知,改进的算法其均方根误差要小于传统算法。因此,改进的RBPF-SLAM通过EKF融合轮式里程计信息和IMU信息的位姿估计的精度优于RBPF-SLAM算法。

图4 位姿状态估计误差对比曲线

表1中将两种算法构建相同环境地图所用的粒子数目和时间做出比较。改进后的算法使用的粒子数目要少于传统算法使用的粒子数目,同时改进后的算法缩短了构建环境地图所用的时间。改进后的算法，即利用退火参数优化混合提议分布的算法,减少采样粒子数目,提高了系统的运行效率。

表1 RBPF-SLAM算法与改进的RBPF-SLAM算法构建环境地图结果比较

4 结论

本研究提出一种改进的RBPF-SLAM算法,使用扩展卡尔曼滤波融合传感器信息提高移动机器人的位姿,并利用退火参数优化提议分布减少粒子数目,得到更精确的环境地图。通过室内环境的仿真实验,验证了改进算法的位姿估计精确性和地图构建的高效性。虽然改进能够减少重采样次数,但增大了计算代价,在后续工作中将对重要性重采样进行优化,降低粒子退化以及提高算法性能。