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(x_1:t, m|z_1:t, u_1:t-1), 即由观测值z_1:t=z₁, z₂, …, z_t和控制量u_1:t-1=u₁, u₂, …, u_t-1估计机器人的运行轨迹x_1:t=x₁, x₂, …, x_t和环境地图m。

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

$ \begin{array}{*{20}{l}} {p({\mathit{\boldsymbol{x}}_{1:t}},m|{\mathit{\boldsymbol{z}}_{1:t}},{\mathit{\boldsymbol{u}}_{1:t - 1}}) = }\\ {p(m|{\mathit{\boldsymbol{x}}_{1:t}},{\mathit{\boldsymbol{z}}_{1:t}})p({\mathit{\boldsymbol{x}}_{1:t}}|{\mathit{\boldsymbol{z}}_{1:t}},{\mathit{\boldsymbol{u}}_{1:t - 1}})} \end{array} $

(1)

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

步骤1  采样。在t时刻, 基于t-1时刻的粒子{x_t-1ⁱ}在提议分布q(x_1:t⁽ⁱ⁾|z_1:t, u_1:t-1)中进行采样。

步骤2  权值计算。粒子的权值w_t⁽ⁱ⁾计算公式为:

$ \begin{array}{*{20}{l}} {w_t^{(i)} = \frac{{p(\mathit{\boldsymbol{x}}_{1:t}^{(i)}|{\mathit{\boldsymbol{z}}_{1:t}},{\mathit{\boldsymbol{u}}_{1:t - 1}})}}{{q(\mathit{\boldsymbol{x}}_{1:t}^{(i)}|{\mathit{\boldsymbol{z}}_{1:t}},{\mathit{\boldsymbol{u}}_{1:t - 1}})}} = }\\ {{\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} \frac{{p({\mathit{\boldsymbol{z}}_t}|\mathit{\boldsymbol{x}}_t^{(i)},m_{t - 1}^{(i)})p(\mathit{\boldsymbol{x}}_t^{(i)}|\mathit{\boldsymbol{x}}_{t - 1}^{(i)},{\mathit{\boldsymbol{u}}_{t - 1}})}}{{q(\mathit{\boldsymbol{x}}_t^{(i)}|\mathit{\boldsymbol{x}}_{1:t - 1}^{(i)},{\mathit{\boldsymbol{z}}_{1:t}},{\mathit{\boldsymbol{u}}_{1:t - 1}})}}w_{t - 1}^{(i)}} \end{array} $

(2)

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

步骤4  地图估计。计算p(m⁽ⁱ⁾|x_1:t⁽ⁱ⁾, z_1:t), 通过第i个粒子的轨迹x_1:t⁽ⁱ⁾和观测值z_1:t估计该粒子的地图m⁽ⁱ⁾。

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

$ {N_{{\rm{eff}}}} = \frac{1}{{\sum\limits_{i = 1}^N {{{({w^{(i)}})}^2}} }} $

(3)

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

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

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

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

$ {\rm{SV}} = \frac{1}{M}\sum\limits_{m = 1}^M {{{(N_t^{(m)} - Nw_t^{(m)})}^2}} $

(4)

重采样前有M个粒子, 重采样后有N个粒子, w_t^(m)表示t时刻的粒子权重, N_t^(m)表示粒子实际复制的次数。SV作为一个代价函数, 计算M个粒子复制次数与其期望复制次数的差的平方和, SV越小, 粒子分布在重采样前后差别越小, 粒子多样性保留越多。

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

步骤1  计算floor(Nw_t^(m))作为粒子第1次复制次数(floor函数表示向下取整), 得到剩余权值:

$ \hat w_t^{(m)} = w_t^{(m)} - {\rm{floor}}(Nw_t^{(m)})/N $

(5)

步骤2  计算$L = \sum\limits_{m = 1}^M {{\rm{floor}}\left( {Nw_t^{\left( m \right)}} \right)} $。

步骤3  比较剩余权值$\hat w_t^{\left( m \right)}$, 最大的N-L个粒子再多复制一次, 避免一些权值较小但有可能是真实值的粒子被删除。

2.2 仿真验证

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

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

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

3 激光SLAM算法测量模型优化 3.1 基于RBPF的激光SLAM算法

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

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

$ \begin{array}{*{20}{l}} {q(\mathit{\boldsymbol{x}}_t^{(i)}|\mathit{\boldsymbol{x}}_{1:t - 1}^{(i)},{\mathit{\boldsymbol{z}}_{1:t}},{\mathit{\boldsymbol{u}}_{1:t - 1}}) = }\\ {p(\mathit{\boldsymbol{x}}_t^{(i)}|\mathit{\boldsymbol{x}}_{1:t - 1}^{(i)},{\mathit{\boldsymbol{z}}_t},{\mathit{\boldsymbol{u}}_{1:t - 1}},m_{t - 1}^{(i)})} \end{array} $

(6)

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

$ w_t^{(i)} = w_{t - 1}^{(i)}\int p ({\mathit{\boldsymbol{z}}_t}|m_{t - 1}^{(i)},{\mathit{\boldsymbol{x}}^\prime })p({\mathit{\boldsymbol{x}}^\prime }|\mathit{\boldsymbol{x}}_{t - 1}^{(i)},{\mathit{\boldsymbol{u}}_{t - 1}}){\rm{d}}{\mathit{\boldsymbol{x}}^\prime } $

(7)

由于机器人状态分布p(x_t⁽ⁱ⁾|x_1:t-1⁽ⁱ⁾, z_t, u_1:t-1, m_t-1⁽ⁱ⁾)无法得到解析式, 只能用高斯分布模拟其分布。对于第i个粒子, 利用激光点云与地图匹配找到最佳机器人位姿估计, 在最佳机器人位姿估计附近采样K个点{x_j}, 用K个点的加权均值与方差作为高斯分布的均值μ和方差δ, 如下:

$ \left\{ {\begin{array}{*{20}{l}} {\mu _t^{(i)} = \frac{1}{{{\eta ^{(i)}}}}\sum\limits_{j = 1}^K {{\mathit{\boldsymbol{x}}_j}} \cdot p({\mathit{\boldsymbol{z}}_t}|m_{t - 1}^{(i)},{\mathit{\boldsymbol{x}}_j}) \cdot }\\ {{\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} p({\mathit{\boldsymbol{x}}_j}|\mathit{\boldsymbol{x}}_{t - 1}^{(i)},{\mathit{\boldsymbol{u}}_{t - 1}})}\\ {\delta _t^{(i)} = \frac{1}{{{\eta ^{(i)}}}}\sum\limits_{j = 1}^K p ({\mathit{\boldsymbol{z}}_t}|m_{t - 1}^{(i)},{\mathit{\boldsymbol{x}}_j})p({\mathit{\boldsymbol{x}}_j}|\mathit{\boldsymbol{x}}_{t - 1}^{(i)},{\mathit{\boldsymbol{u}}_{t - 1}}) \cdot }\\ {{\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} ({\mathit{\boldsymbol{x}}_j} - \mathit{\boldsymbol{u}}_t^{(i)}){{({\mathit{\boldsymbol{x}}_j} - \mathit{\boldsymbol{u}}_t^{(i)})}^{\rm{T}}}} \end{array}} \right. $

(8)

其中, 归一化因子η⁽ⁱ⁾为:

$ {\eta ^{(i)}} = \sum\limits_{j = 1}^K p ({\mathit{\boldsymbol{z}}_t}|m_{t - 1}^{(i)},{\mathit{\boldsymbol{x}}_j})p({\mathit{\boldsymbol{x}}_j}|\mathit{\boldsymbol{x}}_{t - 1}^{(i)},{\mathit{\boldsymbol{u}}_{t - 1}}) $

(9)

3.2 似然域模型改进

在RBPF-SLAM算法加入激光测量数据求取提议分布的过程中, 需要计算激光测量模型p(z_t|m_t-1⁽ⁱ⁾, x)。传统的激光测量模型采用似然域模型, 似然域模型假定了如下3种噪声和不确定性来源:

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

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

$ {p_{{\rm{hit}}}}(\mathit{\boldsymbol{z}}|\mathit{\boldsymbol{x}},{m_k}) = {\varepsilon _{{\sigma _{{\rm{hit}}}}}}{d_k} $

(10)

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

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

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

$ {p_{{\rm{ rand }}}} = \frac{1}{{{\mathit{\boldsymbol{z}}_{{\rm{ max }}}}}} $

(11)

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

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

$ p(\mathit{\boldsymbol{z}}|m,\mathit{\boldsymbol{x}}) = {\lambda _{{\rm{hit}}}}{p_{{\rm{hit}}}} + {\lambda _{{\rm{rand}}}}{p_{{\rm{rand}}}} + {\lambda _{{\rm{short}}}}{p_{{\rm{short}}}} $

(12)

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

4 机器人平台激光SLAM实验

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

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

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

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

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

5 结束语

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