基于RBPF的激光SLAM算法优化设计

吴正越,张 超,林 岩

(北京航空航天大学 自动化科学与电气工程学院,北京 100191)

0 概述

同时定位与建图(Simultaneous Location and Mapping,SLAM)是机器人自主导航的基础,也是实现机器人智能化的关键技术。激光SLAM是目前较稳定和主流的机器人定位导航方法,相比于视觉SLAM,激光SLAM的观测精度更高,其在简单的室内环境中具有明显优势[1]。目前,激光SLAM算法大都基于粒子滤波器(Particle Filter,PF),这种SLAM算法被称为PF-SLAM算法。由于环境中可能存在大量路标,使得PF-SLAM算法状态向量维数非常高,计算较复杂。为解决该问题,文献[2]将Rao-Blackwellised粒子滤波器(Rao-Blackwellised Particle Filter,RBPF)应用于SLAM算法中,提出一种RBPF-SLAM算法。该算法将机器人定位和建图进行分解[3-4],用粒子滤波器对机器人实现定位,用卡尔曼滤波器进行建图。RBPF-SLAM使基于粒子滤波器的SLAM算法计算量大幅减小,促进了SLAM理论的发展[5]。文献[6]基于RBPF-SLAM提出一种激光SLAM算法Gmapping,该算法将高精度的激光测量数据加入到提议分布求取中,使提议分布更接近实际后验分布,从而提高了算法的效率。

RBPF-SLAM算法在粒子滤波过程中的简单重采样会导致样本贫化问题,致使SLAM定位和建图误差过大。为此,多种改进的重采样方法相继被提出,其中,自适应重采样方法成为研究热点之一。文献[7]对自适应重采样方法进行详细分析,文献[8]提出利用自适应方法改进重采样的SLAM算法。虽然这些算法能够缓解样本贫化问题,提高估计准确性,但计算量大幅增加,且只能达到有限精度。文献[9]提出最小采样方差(MSV)重采样方法,该方法可使采样方差最小化,保证重采样前后粒子分布的一致性,使得权值较小但更接近真实值的粒子不被删除。将最小方差重采样方法引入到RBPF-SLAM算法中,可以提高SLAM的位姿估计和建图精度。

基于RBPF的激光SLAM算法在计算提议分布的过程中需要建立激光测量模型[10]。波束模型作为测距仪的近似物理模型,存在光滑性差、计算复杂度高等问题。Gmapping采用似然域模型作为激光测量模型,似然域模型将传感器扫描的终点映射到地图的全局坐标空间,利用映射点与地图中障碍物的距离计算概率[11]。似然域模型相比波束模型光滑性更好,计算更高效,但由于其假定环境是静态的,因此不能对动态物体进行清晰建模。本文利用最小方差重采样方法改进Gmapping算法的重采样过程,以缓解样本贫化问题。在Gmapping算法原有激光测量模型的基础上加入意外物体的观测概率,使算法在动态环境中建立准确的激光测量模型。

1 RBPF-SLAM算法

SLAM算法的关键是求解后验概率p(x1:t,m|z1:t,u1:t-1),即由观测值z1:t=z1,z2,…,zt和控制量u1:t-1=u1,u2,…,ut-1估计机器人的运行轨迹x1:t=x1,x2,…,xt和环境地图m。

基于粒子滤波的PF-SLAM算法在求解后验概率p(x1:t,m|z1:t,u1:t-1)的过程中,由于状态空间维数扩张,计算复杂度非常高。RBPF-SLAM算法将SLAM问题分解为定位和建图两部分,降低了采样时的状态空间维数,从而达到简化计算的目的[12-13]。RBPF-SLAM算法后验概率计算如下:

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

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

(1)

RBPF-SLAM算法具体步骤如下:

(2)

步骤3重采样。按照粒子权值在整体粒子权值中的占比复制粒子,重采样后粒子的权值都设置为1/N。

在RBPF-SLAM算法的粒子迭代更新过程中,部分粒子会出现粒子退化问题,这些粒子的权值减小到可以忽略的程度,但仍会占用计算资源[14]。粒子退化程度表示如下:

(3)

当Neff小于给定的一个阈值时,粒子将发生严重退化。为克服粒子退化以减少计算量,RBPF-SLAM算法在重采样过程中丢弃权值小的粒子,只保留权值大的粒子。

2 RBPF-SLAM算法重采样策略改进

2.1 MSV重采样算法

传统RBPF-SLAM算法重采样策略是去除权值小的粒子,多复制权值大的粒子。这种简单粗糙的重采样方法会使得粒子的多样性丢失,造成样本贫化问题,导致定位和建图失真[15-16]。

采样方差(Sampling Variance,SV)可以度量粒子分布在重采样前后的差别,进而衡量重采样方法对粒子多样性的破坏程度[17]。SV的计算公式如下:

(4)

本文引入MSV方法以改进原RBPF-SLAM算法中的重采样策略。MSV方法可使SV最小化,使重采样前后粒子分布保持一致,缓解样本贫化现象。MSV方法具体步骤如下:

(5)

2.2 仿真验证

本文在Matlab平台上利用悉尼大学户外机器人中心(ACFR)发布的SLAM算法仿真器进行实验。原RBPF-SLAM算法与本文改进的RBPF-SLAM算法都用60个粒子实现仿真,结果如图1所示。其中,五角星为真实路标,加号为SLAM算法估计的路标,曲线为机器人运行轨迹。从图1可以看出,在相同粒子数目情况下,原RBPF-SLAM算法估计路标的位置与路标真实位置相差较大,改进的RBPF-SLAM算法估计路标的位置与真实路标位置则相对一致,即该算法建图效果更好。

图1 2种算法SLAM仿真结果对比

为比较2种重采样方法的定位效果,记录SLAM过程中机器人估计位置与实际位置的间距,结果如图2所示。从图2可以看出,MSV重采样方法具有更高的定位精度。

图2 2种方法在SLAM过程中的定位误差对比

通过以上实验结果对比可知,相对原RBPF-SLAM算法,改进的RBPF-SLAM算法定位和建图精度更高,在重采样过程中样本贫化问题得到有效缓解。

3 激光SLAM算法测量模型优化

3.1 基于RBPF的激光SLAM算法

激光SLAM是一类以激光数据作为传感器输入的SLAM算法[18]。经典的激光SLAM算法Gmapping以RBPF-SLAM算法作为基础[19]。

RBPF-SLAM算法的提议分布选取非常重要,提议分布越接近目标分布,采样所需的粒子数目越少[20]。Gmapping将激光测量数据加入到提议分布求取中,可以获得更精确的提议分布,融合激光测量的提议分布表示如下:

(6)

将式(6)代入RBPF-SLAM算法的权值计算中,得到新的权值更新公式为:

(7)

(8)

其中,归一化因子η(i)为:

(9)

3.2 似然域模型改进

1)测量噪声。真实环境和单束激光测量如图3所示。

图3 真实环境和单束激光测量

测量到的点与地图m上第k(k=1,2,…,N)个物体之间的欧式距离为dk,传感器测量到物体的概率phit(z|x,mk)用一个0均值的高斯函数表示:

phit(z|x,mk)=εσhitdk

(10)

整体测量概率为所有N个物体概率的叠加,图3的整体测量概率如图4所示。

图4 由环境引起的测量概率

2)测量失败。如果观测值z为激光测量最大量程z=zmax,这在物理世界中表示测量失败,似然域模型直接将观测值丢弃。

3)随机测量。用一个均匀分布prand对测量中的随机噪声进行建模,如式(11)所示:

(11)

似然域模型假定环境为静态环境,但现实中的环境大多为动态环境。为使激光测量模型更真实地反映动态环境,本文在似然域模型中加入意外对象观测的概率。假设意外对象在环境中随机出现,则距离传感器越近的对象被探测到的概率越大,本文用一个负指数函数作为意外物体观测概率pshort(z|x,m),如图5所示,pshort(z|x,m)的有效域为(0,z*),z*为地图中激光测量光束方向上最近障碍间的距离。

图5 动态物体测量概率模型

改进后的似然域模型p(z|m,x)如式(12)所示:

p(z|m,x)=λhitphit+λrandprand+λshortpshort

(12)

其中,λ是各部分概率的权重。

4 机器人平台激光SLAM实验

图6所示为真实机器人平台,该平台采用四轮差速驱动的方式,机器人底盘部分向上位机反馈里程计信息,激光测量装置采用国产RPLIDAR-A2激光雷达。上位机运行Linux(Ubuntu 16.04)的ROS系统,利用ROS分布式通信的特性实现机器人底盘控制、传感器数据获取以及SLAM算法。SLAM算法包括基于RBPF的激光SLAM算法(Gmapping)和改进后的激光SLAM算法,改进算法利用MSV方法并将改进的似然域模型作为激光测量模型。

图6 机器人平台

手柄控制机器人平台在多人走动的办公室进行建图实验,构建的栅格地图如图7所示,其中,图7(a)、图7(b)分别为原激光SLAM算法和改进SLAM算法构建的办公室地图。从图7可以看出,原激光SLAM算法构建的地图存在偏移歪斜的问题,而改进算法构建的地图更整齐。

图7 2种算法的办公室环境建图结果

在建图过程中,手柄控制机器人走到指定的位置,记录算法估计的机器人位姿与实际机器人位姿的误差,结果如表1所示。从表1可以看出,改进的激光SLAM算法定位误差更小,定位效果更好。

表1 2种算法的办公室环境定位误差对比

在杂物较多的仓库中构建的栅格地图如图8所示,其中,图8(a)、图8(b)分别为原激光SLAM算法和改进激光SLAM算法构建的仓库地图。从图8可以看出,原激光SLAM算法构建的地图比较混乱,部分墙体出现重影,而改进算法构建的地图更清晰。

图8 2种算法的仓库环境建图结果

在仓库环境中,2种算法的机器人定位误差如表2所示,从表2可以看出,改进的激光SLAM算法定位精度更高。

表2 2种算法的仓库环境定位误差对比

5 结束语

本文对传统RBPF-SLAM算法进行研究,针对RBPF-SLAM算法中简单重采样造成的样本贫化问题,采用MSV重采样方法进行改进。在基于RBPF的激光SLAM算法中,传统的激光测量似然域模型没有考虑动态物体的影响,为此,本文在测量模型中加入意外物体的观测概率,使激光测量模型能更好地反映真实环境。通过以上2点对基于RBPF的激光SLAM算法进行改进,并在真实机器人平台上实现室内激光SLAM实验,结果表明,改进的激光SLAM算法在动态环境中取得了较好的建图和定位效果。意外物体观测概率密度的加入使得激光SLAM算法计算量有所增加,下一步将采用更简单的一次函数来近似模拟动态物体测量概率模型,以降低算法的计算复杂度。