上海理工大学光电信息与计算机工程学院，上海

收稿日期：2023年4月12日；录用日期：2023年6月21日；发布日期：2023年6月30日

摘要

文中提出一种将激光雷达和轮式里程计相融合的方法，用于提升SLAM (Simultaneous Localization and Mapping)算法前端点云配准效率和点云畸变矫正效果。在点云配准方面，对PL-ICP算法做出改进，首先对激光点云进行预处理去除无效点，再利用自适应体素滤波方法对激光点云进行下采样，在保留点云特征的同时将点云稀疏化，从而减少点云配准的计算量，利用轮式里程计的测量值为点云配准提供初值，提高点云配准的效率和定位效果。在点云畸变矫正方面，按照轮式里程计测量的机器人位姿的时间戳，利用拉格朗日线性插值法对点云配准配得到的机器人位姿进行线性插值，利用EKF算法融合轮式里程计测量的位姿和点云配准插值得到的位姿对轮式里程计的测量误差做出矫正，然后为激光点云提供运动补偿，从而去除点云畸变提升SLAM算法定位和建图效果。利用ROS搭建仿真环境验证了本文提出的算法的有效性。

关键词

SLAM，轮式里程计，PL-ICP，EKF，多传感器融合，点云畸变矫正

Simultaneous Localization and Map Construction Algorithm for Mobile Robots Based on 2D LiDAR

Yuchun Tang

School of Optical-Electrical and Computer Engineering, University of Shanghai for Science and Technology, Shanghai

Received: Apr. 12^th, 2023; accepted: Jun. 21^st, 2023; published: Jun. 30^th, 2023

ABSTRACT

This paper proposes a method for fusing data from a lidar and wheel odometry to improve the efficiency of front-end point cloud registration and distortion correction in SLAM (Simultaneous Localization and Mapping) algorithms. Specifically, in terms of point cloud registration, the paper improves the PL-ICP algorithm by first preprocessing the lidar point cloud to remove invalid points, and then using an adaptive voxel filtering method to down sample the lidar point cloud, which reduces the computational load of point cloud registration while preserving the point cloud features. In addition, the paper uses the measurement values from the wheel odometry to provide an initial value for point cloud registration, which improves the efficiency and accuracy of point cloud registration. In terms of point cloud distortion correction, the paper uses the timestamp of the robot pose measured by the wheel odometry, and applies Lagrangian linear interpolation to linearly interpolate the robot pose obtained from point cloud registration. The paper then uses the EKF algorithm to fuse the robot pose measured by the wheel odometry and the pose obtained from point cloud registration to correct for the measurement errors of the wheel odometry. Finally, the paper provides motion compensation for the lidar point cloud to remove point cloud distortion and improve the localization and mapping effects of the SLAM algorithm. The paper verifies the effectiveness of the proposed algorithm by using ROS to build a simulation environment.

Keywords:SLAM, Wheel Odometry, PL-ICP, EKF, Multi-Sensor Fusion, Point Cloud Distortion Correction

Copyright © 2023 by author(s) and Hans Publishers Inc.

This work is licensed under the Creative Commons Attribution International License (CC BY 4.0).

http://creativecommons.org/licenses/by/4.0/

1. 引言

SLAM的关键技术根据传感器在陌生环境中获取自身的位姿信息和环境信息，估计自身运动的位置进行实时定位并利用这些信息增量式的构建环境地图 [1] ，SLAM算法按照传感器不同可分为基于相机的视觉SLAM [2] [3] 基于激光雷达的激光SLAM [4] [5] 。相机使用便捷、能够获取丰富的环境信息、但容易受到外部环境的干扰。激光雷达抗干扰能力强、受光照影响弱、能直接获取深度信息。其中2D激光雷达因其售价便宜，对算力要求低，易于构建栅格地图等特点而被广泛应用于机器人领域。

仅使用激光雷达进行定位与建图时，在变速场景和特征较少的退化场景中点云配准效率低、定位效果差，在移动机器人快速运动时激光点云易发生畸变，最终导致建图效果差，因此多传感器的融合 [6] [7] 成了近几年的研究热点。Chen S等人 [8] 将激光雷达和相机结合，通过构建视觉相似性的词袋模型提升回环检测的效果，以提高SLAM算法的后端优化性能，但在纹理缺失的场景鲁棒性较差。章弘凯等人 [9] 提出将IMU和激光雷达相结合，用IMU的预积分结果为点云配准提供初值提升点云配准效率，同时将其积分结果作为约束条件加入到SLAM算法的后端优化中减小轨迹飘移的影响提高载体的定位精度，但IMU对线加速度进行二次积分的误差较大。Hess W等人 [10] 提出了基于图优化的Cartographer算法，经过不断的发展已经能和GPS、IMU、轮式里程计等多种传感器融合，但仅将辅助传感器的数据用于未来帧的位姿推，并未用于点云畸变矫正。

激光点云畸变会使环境地图发生偏移，进而影响到后端优化和回环的效果最终影响移动机器人定位精度和建图效果。点云畸变的矫正最直接的方法是利用高频率的激光雷达，在激光雷达频率较高情况下点云畸变可以忽略，但这会带来高昂的硬件成本。M. Roh等人 [11] 利用GPS/INS传感器对点云信息进行校正，在每次扫描的同时获取激光雷达载体的确切位置和方向，但是其测量方式依赖卫星，因此难以应用于室内或隧道等封闭环境。基于帧间配准的纯运动估计法因其不需要额外辅助的传感器而被广泛的应用于点云畸矫正 [12] [13] [14] 。如果激光雷达载体在一个扫描周期内的运动状态非匀速或则激光频率太低，使用该方法对点云矫正的效果并不理想。Yang W等人 [15] 提出一种激光雷达和相机融合方法，通过全速度估计来进行点云畸变矫正。激光雷达在径向方向上能精确测量距离信息，但只能使用稀疏的角度信息，而相机作为补充传感器可以提供密集的角度分辨率，此外，还利用概率卡尔曼滤波方法融合估计的速度提升点云矫正效果，但相机提供的信息易受环境干扰。Zhang B等人 [16] 提出利用UKF融合激光雷达和IMU数据的方式进行点云畸变矫正，UKF用于融合IMU的角速度、线加速度和激光雷达的位姿，得到IMU的角速度偏差、线加速度偏差和激光雷达的线速度，然后得到激光雷达在一个扫描周期内各时刻的位姿，从而消除点云的失真得到激光雷达在一个扫描周期内各时刻的位姿，从而消除点云的失真。虽然IMU能测量激光雷达载体的角速度和线加速，但线速度累计误差较大。

轮式里程计能够直接测量移动机器人的位移和角度，具有较高的局部位置测量精度和局部角度测量精度，并且有较高的位姿更新频率，通常能达到100~200 Hz左右，本文将轮式里程计和激光雷达相融合用于点云畸变矫正，和提升在不同场景下的点云配准效率和定位效果。

2. 移动机器人模型与点云配准

2.1. 移动机器人运动模型

轮式里程计通过测量移动机器人两主动轮的转速得到移动移动机器人的线速度v和角速度w，从而推算出移动机器人在世界坐标系下的位姿变化信息。在世界坐标系下移动机器人两轮差分底盘运动学模型如图1所示。

图1. 两轮差分底盘运动学模型

设左轮转速为 $w_l$ ，半径为 $r_l$ ，右轮的转速为 $w_r$ 半径与左轮相同，两轮间的距离为2d，移动机器人的角速度为和线速度分别为w和v，计算如式(1)。

$\left[\begin{array}{l}v\\ w\end{array}\right]=\left[\begin{array}{cc}\frac{r_l}{2}& \frac{r_r}{2}\\ -\frac{r_l}{2d}& \frac{r_r}{2d}\end{array}\right]\left[\begin{array}{l}{w}_l\\ w_r\end{array}\right]=J\left[\begin{array}{l}{w}_l\\ w_r\end{array}\right]$ (1)

在间隔时间 $\Delta t_o$ 极短时，假设其线速度角速度恒定，移动机器人运动变化的圆心角 $\theta$ 的计算如式(2)。

$\{\begin{array}{l}w=J_{21}{w}_l+J_{22}{w}_r\\ \theta =w\Delta t\end{array}$ (2)

$\{\begin{array}{l}v=J_{11}{w}_l+J_{12}{w}_r\\ x_c=x_{t-1}-\frac{v}{w}\sin \left({\theta }_{t-1}\right)\\ y_c=y_{t-1}+\frac{v}{w}\cos \left({\theta }_{t-1}\right)\end{array}$ (3)

$\left[\begin{array}{l}{x}_t\\ y_t\\ {\theta }_t\end{array}\right]=\left[\begin{array}{l}{x}_{t-1}\\ y_{t-1}\\ {\theta }_{t-1}\end{array}\right]+\left[\begin{array}{c}-\frac{v}{w}\sin {\theta }_{t-1}+\frac{v}{w}\sin \left({\theta }_{t-1}+w\Delta t\right)\\ \frac{v}{w}\cos {\theta }_{t-1}-\frac{v}{w}\cos \left({\theta }_{t-1}+w\Delta t\right)\\ w\Delta t\end{array}\right]$ (4)

设 $p_{t-1}={\left(x_{t-1},y_{t-1},{\theta }_{t-1}\right)}^{\text{T}}$ 表示移动机器人上一时刻在世界坐标系中的位姿，由几何关系可得角 $\theta$ 、 $\gamma$ 和 $\lambda$ 相等， $\left(x_c,y_c\right)$ 表示其运动半径的圆心所在的位置，其计算如式(3)，则当前时刻移动机器人在世界坐标系的位姿 $p_t={\left(x_t,y_t,{\theta }_t\right)}^{\text{T}}$ 计算如式(4)。

2.2. 激光点云配准原理

激光SLAM建图主要分为前端点云配准，后端优化，和回环检测等三个部分。点云配准即寻求一组旋转和平移参数使得对齐后的上一帧点云和参考点云在同一参考坐标系下达到最大的重叠，通过求解的旋转和平移参数恢复出两帧点云数据扫描期间的移动机器人的位姿变化信息。常用的二维激光点云配准算法有ICP [17] 算法和PL-ICP [18] 算法，ICP算法是基于点对点的距离扫描匹配算法，构造点到点的距离误差函数。 $C^j=\left\{c_1^j,c_2^j,c_3^j,\cdots ,c_i^j\right\}$ 表示当前帧的扫描点云， $C^{j-1}=\left\{c_1^{j-1},c_2^{j-1},c_3^{j-1},\cdots ,c_i^{j-1}\right\}$ 表示与之对应的上一帧点云，N表示一帧点云中激光点的个数。构建误差方程如式(5)寻求一组解 $\left(R\left(\theta \right),t\right)$ 使得误差方程的值最小。

$E\left(R\left(\theta \right),t\right)=\frac{1}{N}{\displaystyle \underset{i=1}{\overset{N_p}{\sum }}{\Vert c_i^{j-1}-R\left(\theta \right)c_i^j-t\Vert }^2}$ (5)

PL-ICP则是在ICP的基础上利用点到线的距离作为误差取代了点到点的距离作为误差。激光点实际上是对真实环境中曲面的离散采样，最好的误差尺度是激光点到实际曲面的距离，而PL-ICP采用分段线性的方法对实际曲面进行近似，用当前帧点云中的激光点到上一帧点云中对应最近两激光点连线的距离来近似实际激光点到曲面的距离。PL-ICP的误差方程更贴近实际情况，因为比起直接计算点到点的距离，其考虑了点云的局部结构，因此精度更高不容易陷入局部最优，对场景的适应性和收敛精度都高于ICP。

2.3. 点云配准初值获取

无论使用哪种点云配准方法，若没有为点云配准提供准确的初值，点云配准的定位性能会快速劣化甚至完全失效。文中将轮式里程计和激光雷达相融合对PL-ICP算法做出改进，提高点云匹配的效率和对不同场景的适应性，具体流程如图2所示。

在长廊和特征物极少的退化场景中，由于没有参照物激光雷达的观测数据在某一维度高度相似，如图3所示。随着机器人的移动雷达坐标系由 $O_{C^{j-1}}$ 移动到 $O_{C^j}$ 处，但在两个坐标系下的激光点 $c_i^j$ 和 $c_i^{j-1}$ 的

图2. 改进的PL-ICP算法流程图

图3. 激光雷达坐标变化

距离信息完全一样，在这种情况下通过点云配准得到的机器人位姿变化信息接近为零。在有初值的情况下点云 $C^j$ 变换到点云 $C^{j_1}$ ，参考坐标系由 $O_{C^j}$ 变换到 $O_{C^{j_1}}$ ，再由点云 $C^{j_1}$ 继续和上一帧点云 $C^{j-1}$ 进行配准，配准误差可以明显减小，且初值越准确点云配准效果越好。获取点云配准初值常用的方法是利用匀速预测模型，由上一次激光点云配准的结果基于匀速假设计算激光雷达载体的运动速度，并将计算的结果用于推测未来帧的激光雷达载体在世界坐标系下的位姿，计算出激光雷达的相对位姿变化信息，然后将其用于点云配准的初值。但在变速场景和雷达频率较低的场景中匀速预测模型提供的初值误差较大，不能准确反应出激光雷达载体的位姿变换情况，最终导致点云配准的结果无法快速收敛和定位失败。

轮式里程计的频率远高于激光雷达，在一帧激光扫描时期内可以多次测量机器人位姿，因此将轮式里程计的测量值用于点云配准的初值能准确的反应出在一帧扫描时间内机器人的位姿变化。点云配准初值 $b_k=\left(t_k,{\theta }_k\right)=\left(t_{t_s,t_e},{\theta }_{t_s,t_e}\right)$ ，由轮式里程计测量的移动机器人在世界坐标系下的位姿计算相对位姿变换矩阵得到。已知一帧激光点云扫描的起止时间为 $t_s$ 和 $t_e$ 。轮式里程计测量机器人位姿的频率远高于激光雷达，在 $t_s$ 和 $t_e$ 之间存在多帧机器人位姿信息，两个传感器的测量值存在不同的时间戳，如图4所示，因此需要进行时间同步。

图4. 不同传感器时间轴

用 $t_l=\left\{t_s,t_e,t_{s+1},t_{e+1},\cdots ,t_{s+n},t_{e+n}\right\}$ 表示激光点云的时间轴， $t_o=\left\{t_{{}_{o1}},t_{{}_{o2}},t_{{}_{o3}},t_{{}_{o4}},\cdots ,t_{{}_{o(j-1)}},t_{{}_{oj}}\right\}$ 表示里程计的时间轴。利用两个双端容器 $de{q}_l$ ， $de{q}_o$ 分别存储点云数据和里程计信息进行时间同步，每拿到一帧点云数据则在 $de{q}_o$ 容器中搜索对应时间的机器人位姿，若 $t_s$ 、 $t_e$ 时刻没有对应的机器人位姿则分别搜索最接近 $t_s$ 、 $t_e$ 时刻的位姿，然后利用线性插值得到对应的位姿，为了增加搜索效率每次对 $de{q}_o$ 中小于 $t_e$ 时刻的位姿信息进行删除。假设 $t_s$ 时刻对应的前后两帧里程计的时间戳为 $t_{o1}$ 、 $t_{o2}$ ，对应机器人位姿为 $p_{t_{o1}}$ 、 $p_{t_{o2}}$ ，则 $t_s$ 时刻的机器人位姿 $p_{t_s}$ 计算如式(6)同理可计算得到 $p_{t_e}$ 。轮式里程计测量得到的移动机器人位姿为 $1\times 7$ 的矩阵组成，由世界坐标系下的距离信息和四元数组成如式(7)。将 $p_{t_s}$ 和 $p_{t_e}$ 转换成 $4\times 4$ 矩阵形式用 $T_{t_s}$ 和 $T_{t_e}$ 表示，其相对位姿变换矩阵用 ${}_{t_s}{}^{t_e}T$ 表示，计算如式(8)，计算 $t_{t_s,t_e}$ 和 ${\theta }_{t_s,t_e}$ 的值如式(9)。

$p_{t_s}=p_{t_{o2}}\frac{t-t_{o1}}{t_{o2}-t_{o1}}+p_{t_{o1}}\frac{t_{o2}-t}{t_{o2}-t_{o1}}$ (6)

$p=\left[X,Y,Z,x,y,z,w\right]$ (7)

${}_{t_s}{}^{t_e}T\text{=}{T}_{t_s}{}^{-1}{T}_{t_e}$ (8)

$\{\begin{array}{l}{t}_{t_s,t_e}\text{=}\left[\begin{array}{l}{}_{t_s}{}^{t_e}T{}_{\left(0,3\right)}\\ {}_{t_s}{}^{t_e}T{}_{\left(1,3\right)}\end{array}\right]\\ {\theta }_{t_s,t_e}\text{=}a\tan 2\left({}_{t_s}{}^{t_e}T{}_{\left(1,0\right)},{}_{t_s}{}^{t_e}T{}_{\left(0,0\right)}\right)\end{array}$ (9)

2.4. 自适应体素滤波

一帧点云激光点较多，如果将所有激光点都用于点云匹配计算量较大，并非所有激光点数据都有效，有的导致配准结果无法收敛，如激光打在桌子边缘导致返回的激光强度低、小于或大于有效距离而产生的inf点和nan点，这些噪声点可能会导致两帧点云无法通过旋转平移重叠。因此需对点云数据进行预处理去除噪声点以提高点云配准效率。设定最大测距阀值 $d_{\max }$ 和最近离阀值 $d_{\min }$ ，初始扫描点 $c_i^j$ 与 $d_{\max }$ 和 $d_{\min }$ 进行距离比较去除无效点。距离激光雷达近的物体可以扫描得到更多的激光点，这就导致点云比较密集，同时也会出现大量的重叠点，而远处的点则相对稀疏。为了降低计算量基于体素滤波对点云进行下采样，在保存点云特征的同时降低点云的稀疏性，同时能够在一定程度上去除离群点。已知点云数据在X、Y、Z轴上分布的最大最小值记为 $x_{\max }$ 、 $x_{\min }$ 、 $y_{\max }$ 、 $y_{\min }$ 、 $z_{\max }$ 、 $z_{\min }$ ，计算体素栅格的边长 $l_x$ 、 $l_y$ 、 $l_Z$ 如式(10)。

$\{\begin{array}{l}{l}_x=x_{\max }-x_{\min }\\ l_y=y_{\max }-y_{\min }\\ l_Z=z_{\max }-z_{\min }\end{array}$ (10)

设置体素小栅格的边长为 $l_v$ ，将X、Y、Z三轴均分为l、w、h份，则可以划分为 $l\ast w\ast h$ 个体素小栅格，其中 $sum$ 表示体素小栅格的个数 $sum=l\ast w\ast h$ ， $\lfloor .\rfloor$ 表示向下取正，其中l、w、h的计算如式(11)。对每个激光点所属的体素小栅格进行编号确定每个激光点所属的体素小栅格，设编号为 $\left(f,j,k\right)$ ，其中f、j、k的计算如式(12)。

$l=\lfloor \frac{l_x}{l_v}\rfloor w=\lfloor \frac{l_y}{l_v}\rfloor h=\lfloor \frac{l_z}{l_v}\rfloor$ (11)

$f=\lfloor \frac{x_i-x_{\min }}{l_v}\rfloor j=\lfloor \frac{y_i-y_{\min }}{l_v}\rfloor k=\lfloor \frac{z_i-z_{\min }}{l_v}\rfloor$ (12)

n表示每个小栅格内的激光点数，计算每个体素小栅格的质心 $g_i$ 替代栅格内的其他激光点，其计算如式(13)。

$g_i^j=\frac{1}{n}{\displaystyle \underset{i}{\overset{n}{\sum }}{C}_i^j}$ (13)

得到滤波后的点云 $C^j=\left\{g_1^j,g_2^j,g_3^j,\cdots ,g_i^j\right\}$ ，若质心点不存在则用距离质心最近的点替代其他点。滤波阈值 $l_v$ 的选取对点云的配准效果至关重要， $l_v$ 选取过大会使点云过于稀疏导致有效特征点太少而使得点云配准失败。 $l_v$ 选取太小，虽然能保留足够的激光点，但并不能达到减少点云配准计算量的目的。机器人在移动过程中通常会遇到空间环境变化明显的情况，如从走廊进入室内，如果使用固定的 $l_v$ 值进行体素滤波，在不同的场景下滤波后的激光点会相差数倍，采用固定滤波阀值会影响到点云配准的效率。因此本文使用自适应体素滤对滤波阀值的选取做出改进，利用固定的激光点数取代固定的滤波阀值。设置最大滤波阀值 $l_{\max }$ 和最小滤波阀值 $l_{\min }$ 进行体素滤波，如果滤波后的激光点数远小于或大于目标点数则利用二分法继续滤波，如果体素滤波后激光点数接近目标点数，则输出滤波后的点云。滤波后的平均激光点数与原始激光点数的比值用γ表示。在点云配准搜索对应点的时候，找到其中一个点后，另一个点通常在其附近，但滤波后的激光点为无序状态，为了提升搜索对应点的效率，将滤波后的激光数据按照角度递增的关系进行排序。

2.5. 点云配准迭代

计算当前帧的激光点在上一帧激光点云所在激光雷达坐标系下的位置 $g_i^w$ 如式(14)，其中 $b_k=\left(t_k,{\theta }_k\right)$ 。对激光点 $g_i^w$ 在上一帧点云 $C^{j-1}$ 中寻找距离它最近的两个激光点，其下标记为i和 $i+1$ ，设 $n_i$ 为 $g_i^{j-1}$ 和 $g_{i+1}^{j-1}$ 连线的法向量，构建误差方程如(15)求解 $b_{k+1}$ 使得误差方程的值最小。

$g_i^w\triangleq g_i\oplus b_k=R\left({\theta }_k\right)g_i+t_k$ (14)

$E\left(b_{k+1}\right)={\displaystyle \underset{i}{\sum }{\left(n_i^{\text{T}}\left[R\left({\theta }_{k+1}\right)g_i^w+t_{k+1}-g_{i+1}^{j-1}\right]\right)}^2}$ (15)

将 $b_{k+1}$ 作为下一次迭代的初值计算得到新的点云 $C^{j_1}$ 继续和上一帧点云 $C^{j-1}$ 进行配准，求解结果用 $b_{k+2}$ 表示，循环迭代n次以后直到n等于配准次数的阀值或则 $b_{k+n}$ 小于配准误差阀值则停止迭代。将每次求解的结果用矩阵 $T_{b_{k+i}}$ 表示，则最终两帧点云的配准结果 ${}_{c^{j-1}}^{c^j}T=T_{b_{k+n}}{T}_{b_{k+n-1}}\cdots T_{b_k}$ ，迭代过程如图5所示。机器人初始时刻在世界坐标系下的位姿矩阵为 $T_0$ ，经过j帧点云扫描配准后定位得到的移动机器人在世界坐标系中的位姿矩阵 $T_j=T_0{}_{c_1}{}^{c_2}T{}_{c_2}{}^{c_3}T\cdots {}_{c_{j-1}}{}^{c_j}T$ 。改进PL-ICP算法的伪代码如表1。

3. 激光点云畸变矫正

3.1. 点云畸变原理分析

激光雷达通过旋转能够覆盖360˚的视野，在一次测量中，每束激光产生一个包含距离和角度信息的单点，一圈扫描结束所有激光点的集合构成一帧点云信息。常用的二维激光雷达的频率在5~20 Hz，这就意味着激光雷达旋转一周获取周围环境的信息需要0.2 s~0.05 s，如果激光雷达在一个扫描周期内是静止的则一帧点云中每个激光点对应的移动机器人位姿是相同的，都为激光雷达扫描开始时的第一个激光

图5. 点云配准迭代过程

表1. 改进PL-ICP算法伪代码

点所对应的移动机器人位姿。激光雷达测量到的物体的位置信息由激光雷达坐标系转换到机器人坐标系再转换到世界坐标系从而得到被测物体在整个地图中的位置。激光雷达在进行360度扫描时会伴随着移动机器人的移动，这导致单个激光点在空间中有不同的参考位置，但激光雷达的点云数据在进行封装时会默认所有激光点的信息是在同一机器人位姿下瞬时获取的，因此点云数据会失真。如图6所示假设激光雷达的频率是10 Hz，逆时针方向旋转，搭载激光雷达的移动机器人沿x方向运动，线速度为1 m/s，机器人由位置A在激光雷达的四分之一个扫描周期内经过位置B运动到了位置C，AC的距离为2.5 cm。激光雷达在位置A处扫描得到激光点s，在位置B处扫描得到激光点p，在位置C处扫描得到激光点t，但所有激光点都会以A的位置为参考坐标系将其转换到世界坐标系下，因为忽略了点C所在的位置，最终呈现在地图中激光点t的位置会在点f处，以此类推当前帧的整个点云都会发生偏移如图中的点云 $C^2$ ，而点云 $C^1$ 才是激光雷达扫描得到的真实点云，点云 $C^2$ 为由于机器人移动引起的畸变的点云。点云矫正的目的就是将点云 $C^2$ 还原成点云 $C^1$ 。

图6. 点云畸变原理

3.2. 点云矫正流程

消除点云畸变的关键是还原出激光雷达载体在一帧点云扫描周期内准确的位姿信息。基于匀速预测模型的纯估计法对激光点云进行畸变矫正的主要步骤：1) 对于一帧激光点云中的每一激光点都赋予对应的时间戳，假设激光雷达载体匀速运动，通过帧间匹配的位移除以时间求出激光雷达载体的移动速度；2) 根据每一个激光点对应的时间戳进行运动补偿。由于运动控制器噪声的存在移动机器人运动并非匀速的，且在实际应用中移动机器人的使用不局限于匀速场景。轮式里程计能够直接测量移动机器人的位移和角度，具有较高的局部角度测量精度和局部位置测量精度，并且更新频率远高于激光雷达，相比于匀速预测模型轮式里程计的测量值能够更加准确的反应机器人的实际运动情况。设当前帧激光雷达扫描的起始时间为 $t_s$ 终止时间为 $t_e$ ， $\Delta t_l=t_s-t_e$ ，激光雷达的频率为 $f_l$ ，一帧里程计的时间为 $\Delta t_o$ ，频率为 $f_o$ ， $k=f_o÷f_l$ 。算法的整体流程如下：

1) 通过改进的PL-ICP算法配准得到 $\Delta t_l$ 时间内移动机器人的相对位姿变化信息，通过线性插值得到 $\Delta t_l$ 时间内k个机器人位姿 $T_{l_t}$ ，计算如式(16)。

2) 使用EKF算法融合改进PL-ICP算法配准后插值得到的机器人位姿和轮式里程计测量的位姿，得到矫正后的机器人位姿信息，对矫正后的位姿进行线性插值，使每个激光点对应一个移动机器人位姿。

3) 已知机器人位姿通过坐标变换把所有激光点转换到以第一个激光点对应雷达位姿为基准的坐标系下，得到矫正后的点云数据。

$T_{l_t}=\left(\frac{t-t_s}{t_e-t_s}\right)T_{t_s}{}_{c^{j-1}}^{c^j}T$ (16)

3.3. EKF数据融合

在SLAM算法中卡尔曼滤波算法 [19] 常用来做多传感器数据的融合，以达到传感器载体位姿矫正的目的，其原理是利用上一时刻矫正后的载体位姿通过状态转移方程得到当前时刻的预测位姿，同时利用当前时刻的观测值修正当前时刻的预测值，得到当前时刻矫正后的位姿。但卡尔曼滤波不适用于非线性系统，为了解决非线性问题扩展卡尔曼滤波算法 [20] 在卡尔曼滤波算法的基础上对状态转移方程和观测方程进行一阶泰勒展开达到线性化的目的。移动机器人的运动往往是非线性的，因此本文利用扩展卡尔曼滤波算法进行数据融合。输入为轮式里程计测量位姿 $p_t$ 和激光雷达点云配准插值的位姿 $l_t$ ，输出为矫正后的机器人位姿 ${\omega }_t$ ，其中 $l_t$ 由 $T_{l_t}$ 转换为1 × 3的矩阵得到。将移动机器人的运动模型分解为一个无噪声分量和一个随机噪声分量如式(17)，式中 $u_t=\left(v_t,w_t\right)$ 表示控制量，对式(17)进行一阶泰勒展开线性化得到式(18)。 $G_{p,t}$ 和 $G_{u,t}$ 表示方程 $g(.)$ 关于p和u求导的雅克比矩阵。 $G_{p,t}$ 和 $G_{u,t}$ 的计算如式(19)和(20)。

$p_t=g\left(u_t,p_{t-1}\right)+N\left(0,R_t\right)$ (17)

$g\left(u_t,p_{t-1}\right)\approx g\left(u_t,{\omega }_{t-1}\right)+G_{p,t}\left(p_{t-1}-{\omega }_{t-1}\right)$ (18)

$\begin{array}{c}{G}_{p,t}=\frac{g\left(u_t,{\omega }_{t-1}\right)}{\partial p_{t-1}}=\left[\begin{array}{ccc}\frac{\partial x^{\prime }}{\partial {\omega }_{t-1,x}}& \frac{\partial x^{\prime }}{\partial {\omega }_{t-1,y}}& \frac{\partial x^{\prime }}{\partial {\omega }_{t-1,\theta }}\\ \frac{\partial y^{\prime }}{\partial {\omega }_{t-1,x}}& \frac{\partial y^{\prime }}{\partial {\omega }_{t-1,y}}& \frac{\partial y^{\prime }}{\partial {\omega }_{t-1,\theta }}\\ \frac{\partial {\theta }^{\prime }}{\partial {\omega }_{t-1,x}}& \frac{\partial {\theta }^{\prime }}{\partial {\omega }_{t-1,y}}& \frac{\partial {\theta }^{\prime }}{\partial {\omega }_{t-1,\theta }}\end{array}\right]\\ =\left[\begin{array}{ccc}1& 0& -\frac{v_t}{w_t}\cos {\omega }_{t-1,\theta }+\frac{v_t}{w_t}\cos \left({\omega }_{t-1,\theta }+w_t\Delta t\right)\\ 0& 1& -\frac{v_t}{w_t}\sin {\omega }_{t-1,\theta }+\frac{v_t}{w_t}\sin \left({\omega }_{t-1,\theta }+w_t\Delta t\right)\\ 0& 0& 1\end{array}\right]\end{array}$ (19)

$\begin{array}{c}{G}_{u,t}=\frac{g\left(u_t,{\omega }_{t-1}\right)}{\partial u_t}=\left[\begin{array}{cc}\frac{\partial x^{\prime }}{\partial v_t}& \frac{\partial x^{\prime }}{\partial w_t}\\ \frac{\partial y^{\prime }}{\partial v_t}& \frac{\partial y^{\prime }}{\partial w_t}\\ \frac{\partial {\theta }^{\prime }}{\partial v_t}& \frac{\partial {\theta }^{\prime }}{\partial w_t}\end{array}\right]\\ =\left[\begin{array}{cc}\frac{-\sin \theta +\sin \left(\theta +w_t\Delta t\right)}{w_t}& \frac{v_t\left(\sin \theta -\sin \left(\theta +w_t\Delta t\right)+w_t\cos \left(\theta +w_t\Delta t\right)\Delta t\right)}{w_t^2}\\ \frac{\cos \theta -\cos \left(\theta +w_t\Delta t\right)}{w_t}& \frac{-v_t\left(\cos \theta -\cos \left(\theta +w_t\Delta t\right)+w_t\sin \left(\theta +w_t\Delta t\right)\Delta t\right)}{w_t^2}\\ 0& \Delta t\end{array}\right]\end{array}$ (20)

扩展卡尔曼滤波的预测方程如式(21) (22)，当前时刻移动机器人预测位姿期望值 ${\bar{p}}_t$ 由上一时刻校正后的位姿期望值和无误差项的移动机器人运动模型得到， ${\Sigma }_t$ 表示位姿的协方差矩阵。 $G_{u,t}{M}_t{G}_{u,t}^{\text{T}}$ 表示控制空间运动噪声向状态空间运动噪声的近似映射。

${\bar{p}}_t={\omega }_{t-1}+\left[\begin{array}{c}-\frac{v}{w}\sin {\theta }_{t-1}+\frac{v}{w}\sin \left({\theta }_{t-1}+w\Delta t\right)\\ \frac{v}{w}\cos {\theta }_{t-1}-\frac{v}{w}\cos \left({\theta }_{t-1}+w\Delta t\right)\\ w\Delta t\end{array}\right]$ (21)

${\bar{\Sigma }}_t=G_{p,t}{\Sigma }_{t-1}{G}_{p,t}^{\text{T}}+G_{u,t}{M}_t{G}_{u,t}^{\text{T}}$ (22)

观测模型 $z=h\left(p_t,l_t,a\right)$ 描述的是 $p_t$ 和 $l_t$ 的相对位置信息，如式(23)，a是传感器的误差 $\left(a_r,a_{\beta }\right)~N\left(0,A\right)$ 。其中 $z={\left(r,\beta \right)}^{\text{T}}$ ，r是距离， $\beta$ 是方位角。

$Z=\left[\begin{array}{l}\sqrt{{\left(p_{t,x}-l_{t,x}\right)}^2+{\left(p_{t,y}-l_{t,y}\right)}^2}\\ {\tan }^{-1}\left(p_{t,y}-l_{t,y}\right)/\left(p_{t,x}-l_{t,x}\right)-\theta \end{array}\right]+\left[\begin{array}{l}{a}_r\\ a_{\beta }\end{array}\right]$ (23)

$A=\left[\begin{array}{cc}{\sigma }_r^2& \\ & {\sigma }_{\beta }^2\end{array}\right]$ (24)

观测模型的泰勒展开线性近似如式(26)，式(27)为h关于机器人位姿的雅克比矩阵。

$q={\left(l_{t,x}-{\omega }_{t,x}\right)}^2+{\left(l_{t,y}-{\omega }_{t,y}\right)}^2$ (25)

$h\left(p_t,l_t\right)\approx h\left({\omega }_t,l_t\right)+H_t\left(p_t-{\bar{p}}_t\right)$ (26)

$H_t=\frac{\partial h\left({\bar{p}}_t,l_t\right)}{\partial p_t}=\left[\begin{array}{ccc}-\frac{l_{t,x}-{\bar{p}}_{t,x}}{\sqrt{q}}& -\frac{l_{t,y}-{\bar{p}}_{t,y}}{\sqrt{q}}& 0\\ \frac{l_{t,y}-{\bar{p}}_{t,y}}{q}& -\frac{l_{t,x}-{\bar{p}}_{t,x}}{q}& -1\end{array}\right]$ (27)

$H_w=\frac{\partial h\left({\bar{p}}_t,l_t\right)}{\partial z}=\left[\begin{array}{cc}1& 0\\ 0& 1\end{array}\right]$ (28)

计算卡尔曼增益矩阵K如式(29)

$\{\begin{array}{l}{S}_t=H_t\bar{\sum }{H}_t^{\text{T}}+H_{w}A{H}_w^{\text{T}}\\ K_t=\bar{\sum }{H}_t^{\text{T}}{S}_t^{-1}\end{array}$ (29)

$z=\left[\begin{array}{l}\sqrt{{\left({\bar{p}}_{t,x}-l_{t,x}\right)}^2+{\left({\bar{p}}_{t,y}-l_{t,y}\right)}^2}\\ {\tan }^{-1}\left({\bar{p}}_{t,y}-l_{t,y}\right)/\left({\bar{p}}_{t,x}-l_{t,x}\right)-\theta \end{array}\right]$ (30)

状态更新得到矫正后的机器人位姿的期望值和协方差如式(31)。

$\{\begin{array}{l}{\omega }_t={\bar{p}}_t+K_t\left(z_t-{\bar{z}}_t\right)\\ {\sum }_t=\left(I-K_t{H}_t\right){\bar{\sum }}_t\end{array}$ (31)

3.4. 点云数据矫正

从 $t_s$ 到 $t_e$ 时间段一共有k个机器人位姿 $\left\{{\omega }_{t_s},{\omega }_{t_{s+1}},{\omega }_{t_{s+2}},\cdots ,{\omega }_{t_{s+k-2}},{\omega }_{t_e}\right\}$ ，位姿由EKF融合轮式里程测量位姿和帧间配准插值的位姿得到。在k个机器人位姿之间进行线性插值，设 ${\omega }_{t_s}$ 和 ${\omega }_{t_{s+1}}$ 之间有 $n-2$ 个位姿 $\left\{{\omega }_{t_s},{\omega }_{t_{s1}},{\omega }_{t_{s2}},\cdots ,{\omega }_{t_{s(n-2)}},{\omega }_{t_{s+1}}\right\}$ ，插值计算如式(32)。

${\omega }_t={\omega }_{t_{s+1}}\frac{t-t_s}{t_{s+1}-t_s}+{\omega }_{t_s}\frac{t_{s+1}-t}{t_{s+1}-t_s}$ (32)

在获取每个激光点对应的位姿信息后需要将所有激光点转换到以第一个激光点为基准的坐标系下。

如图7所示， $O_w$ 表示世界坐标系， $O_r$ 表示机器人坐标系， $O_l$ 表示雷达坐标系。设 $p_i$ $\left(x_i^w,y_i^w,\beta \right)$为某一帧激光进行信息采集时第一束激光采集信息时机器人所在的位置，该位置所对应的激光雷达坐标系也作为基坐标系。在一帧点云扫描期间激光束扫描到障碍物时产生激光点 ${}^{si}p{}^{`}$ 的距离和角度信息，此时机器人所在位置为 $p_{i+1}$ $\left(x_{i+1}^w,y_{i+1}^w,\theta \right)$ ，此时的机器人坐标系用 $O_{r_2}$ 表示，雷达坐标系用 $O_{l_2}$ 表示， $O_{l_2}$ 坐标系下的激光点 ${}^{si}p{}^{`}$ 的数据信息存储一帧点云中，存储数据时参考的雷达坐标系为 $O_l$ ，点 ${}^{si}p{}^{`}$ 的位置经过坐标的旋转和平移变换后相对于世界坐标系 $O_w$ 的位置在 ${}^{si}p$ 处。在已知 $p_{i+1}$ 的情况下将 ${}^{si}p{}^{`}$ 的位置转换到 $O_l$ 坐标

图7. 点云坐标变换

系下即可得到校正后的激光点，其中 $p_{i+1}={\omega }_t$ 。设 ${}_{si}{}^{ri}t$ 和 ${}_{si}{}^{ri}R$ 分别表示从激光雷达坐标系到机器人坐标系的平移向量和旋转矩阵， ${}_{ri}{}^{wi}t$ 和 ${}_{ri}{}^{wi}R$ 分别表示从机器人坐标系到世界坐标系的平移向量和旋转向量。 ${}^{ri}p$ 和 ${}^{wi}p$ 分别表示激光点在机器人坐标系和世界坐标系下的位置，其计算如式(33) (34)。

${}^{ri}p={}_{si}{}^{ri}R{}^{si}p+{}_{si}{}^{ri}t$ (33)

${}^{wi}p={}_{ri}{}^{wi}R{}^{ri}p+{}_{ri}{}^{wi}t={}_{ri}{}^{wi}R\left({}_{si}{}^{ri}R{}^{si}p+{}_{si}{}^{ri}t\right)+{}_{ri}{}^{wi}t$ (34)

将其转换为齐次形式如式(35) (36)

${}^{wi}\tilde{p}={}_{ri}{}^{wi}T{}_{si}{}^{ri}T{}^{si}\tilde{p}$ (35)

${}_{ri}{}^{wi}T=\left(\begin{array}{cc}{}_{ri}{}^{wi}R& {}_{ri}{}^{wi}t\\ 0^T& 1\end{array}\right){}_{ri}{}^{wi}T=\left(\begin{array}{cc}{}_{ri}{}^{wi}R& {}_{ri}{}^{wi}t\\ 0^T& 1\end{array}\right){}^{si}\tilde{p}=\left(\begin{array}{l}{}^{si}p\\ 1\end{array}\right)$ (36)

再将激光点由世界坐标系转换到换到以 $O_l$ 为原点的坐标系下用 ${}^{si}{p}_s$ 表示，计算如式(37)，最终得到矫正后的点云新信息。

${}^{si}{p}_s={}_{ri}{}^{wi}T{}^{-1}\left({}^{wi}\tilde{p}-{\tilde{p}}_0\right)$ (37)

4. 实验与分析

4.1. 实验说明

基于ROS (Robot Operating System)操作系统进行仿真实验，验证本文提出的算法。利用Gazebo平台构建3D仿真室内环境和仿真移动机器人如图8所示，整体为矩形长40 m，宽20 m，在室内和过道均布置有不同的障碍物构成结构化场景如图8，世界坐标系原点在图中蓝色直线处，垂直向左为x轴正方向，垂直向下为y轴正方向。所用平台为Ubuntu20.04，ROSkinetic，搭载处理器为i7-6500U运行内存为4G的计算机。仿真机器人底盘为四轮模型，底盘为左右两个主动轮和前后方各一个万向轮，搭载有单线激光雷达和里程计。轮式里程计频率为100 Hz，测量误差为±0.1%，激光雷达频率为10 Hz，测量最远距离8 m，最小0.05 m，水平视角360˚，角度分辨率为0.36˚，测量误差±0.02 m。

图8. 实验场景

实验分为三个部分：1) 在室内结构化场景中和退化场景中，机器人处于变速运动的情况下，对比本文提出的改进的点云配准算法和其他点云配准算法在点云配准效率和定位效果上的差异。2) 讨论点云稀疏性对点云配准效率和定位效果的影响。3) 将本文提出的融合轮式里程计进行点云畸变矫正的方法移植到Cartographer算法中分别对比无点云畸变矫正、基于匀速预测模型进行点云畸变矫正的Cartographer算法在回环误差和场景还原度方便的差异。

1) 点云配准效率：以点云配准的平均耗时和平均迭代次数为准。

2) 点云配准定位误差：以激光雷达数据集停止录制时移动机器人在世界坐标系中的真实位置与点云配准定位位置的欧式距离作为定位误差评判标准。回环误差计算原理与定位误差计算方式相同，不同的是回环的起点与终点真值相同，均为世界坐标系坐标原点。

3) 场景还原度：场景还原度使用SSIM (Structural Similarity Index Measure)算法 [21] 进行计算，用Python对原始场景地图和SLAM算法生成的地图分别计算SSIM值，SSIM的值越大说明算法对场景的还原度越高，点云畸变矫正效果越好。

4.2. 点云配准实验

4.2.1. 结构化场景中的点云配准实验

机器人从世界坐标系坐标原点出发进入室2后从另外一个门出，录制数据集共482帧点云信息，机器人最大移动速度为1.5 m/s，最大加速度为2 m/s²，最大角速度为1 rad/s，整体运动状态为变速行驶。针对该数据集分别运行文本提出的改进的PL-ICP算法(在实验中记为ours)，ICP算法，PL-ICP无初值提供的算法，PL-ICP以匀速运动模型预测初值的算法(在本文中记为PL-ICP+)，点云配准迭代的误差阀值均为0.000001 m，经过参数调试最终γ值选取为0.72，即滤波后的激光点数与原激光点数的平均比值。点云配准平均迭代次数的小梯图如图9所示，在同样没有提供初值的情况下，ICP算法的平均迭代次数为19次，PL-ICP算法的平均迭代次数为33次，这说明在点云配准的过程中相对于ICP算法PL-ICP算法对初值更加敏感，但从图10中可以看出ICP算法的平均配准耗时远远大于PL-ICP算法的平均耗时，ICP算法的平均耗时为0.1298 s，而一次点云扫描的时间0.1 s，这最终会导致在点云配准的过程中出现掉帧的情况。PL-ICP算法的平均耗时为0.0341 s，这是因为PL-ICP算法本身是二次收敛的，所以尽管收敛到相同的误差精度PL-ICP算法需要更多的迭代次数但其耗时仍远小于ICP算法。PL-ICP+算法点云配准的平均耗时为0.0263 s，平均迭代次数为20次，本文的算法点云平均配准耗时为0.0209 s，平均迭代次数为14次。从图9、图10中可以看出本文的算法具有更少的离群值且大多数值都集中在均值以下，这说明本文的算法对变化的场景具有更强的适应性和稳定应。离群点的出现是因为在结构化场景中机器人移动速度较快或短时间内转角较大的情况下环境特征变化明显，导致点云配准的过程中寻找对应点的效率降低，所以点云配准时间和迭代次数会增加。本文提出的改进的点云配准算法利用轮式里程计的测量值为点云配准提供的初值比基于匀速预测模型提供的初值更加准确，且能适用于移动机器人变速运动的场景，同时利用自适应体素滤波法能够将点云稀疏化且能去除点云中的部分离群点，所以点云配准效率得到了显著提升。

图9. 点云配准迭代次数

图10. 点云配准耗时

在数据集停止录制时机器人真实位姿为(10.0355 m, −01739 m, 1.3997 rad)。点云配准定位位姿和误差如表2所示，其中ICP算法的定位误差最大为0.8612 m远大于PL-ICP算法的0.4386 m，但其角度误差小于PL-ICP算法。相比于PL-ICP算法PL-ICP+算法的定位误差和角度误差得到减小，但x轴误差却更大了。本文的算法无论在x轴和Y轴上的误差，还是定位误差和角度误差上均为最小，相比于PL-ICP算法和PL-ICP+算法角度误差分别减少了78.6%和53.2%，定位误差分别减少了64.5%和51.3%。

表2. 在结构化场景中的点云配准误差

4.2.2. 退化场景中的点云配准实验

将长廊1中的障碍物去除，门设置为关闭状态，重新构造成退化场景，录制数据集共124帧点云信息，在数据集停止录制时移动机器人真实位姿为(12.1455 m, 0.0625 m, 0.0053 rad)机器人最大行驶速度为2 m/s，最大加速度2 m/s²，最大角速度为1 rad/s，整体运动状态为变速行驶。在退化场景激光雷达扫描得到的点云数据呈高度相似，所以各算法的点云配准迭代次数和耗时无明显差异，因此本小节仅讨论点云配准的定位误差。点云配准定位位姿和误差如表3所示，因为机器人是沿直线行驶所以y轴和角度的误差很小。主要参考x轴的误差，通过ICP算法和PL-ICP算法可以看出在无初值提供的情况下定位信息完全丢失。以匀速预测运动模型提供初值的PL-ICP算法的定位误差为5.6576 m远大于本文提出的算法的定位误差0.7107 m，说明匀速预测运动模型并不适用于变速的场景，而本文的算法在退化的场景中和变速场景具有更好的鲁棒性。综上所述，本文提出的改进的PL-ICP算法在点云配准效率和定位效果方面均有所提升且适用于变速场景和退化场景。

表3. 在退化场景中的点云配准误差

4.3. 讨论点云稀疏性对点云配准耗时和定位效果的影响

所用数据集和4.2.1所用数据集相同，验证γ (滤波前后一帧点云中激光点数的比值)的取值对点云配准的影响，γ取值越小点云越稀疏。当γ = 1时点云为滤波前的原始点云，通过rviz可视化点云数据如图11所示，对应场景为图8中的室2，通过图片可以看出在距离激光雷达较近的地方激光点比较密集出现大量重叠，而远处的点则相对稀疏。当γ = 0.60时无论距离远近点云的分布整体较为均匀，当γ = 0.21时点云已经非常稀疏只保留了部分几何特征。点云配准的效率和定位效果与点云的稀疏性有关，稀疏化的点云虽然能提升点云的配准速度，但点云过于稀疏会导致特征丢失且不利于寻找对应点，最终使得定位效果变差。如图12和图13所示，当γ值大于0.72时随着γ值的减小，点云配准耗时逐渐减少且定位误差也在不变小，但是当γ值在小于0.72以后点云配准的定位误差会逐渐增大。因此在实际应用中需要权衡时实性与定位精度选择合适的γ值。

图11. 不同γ值的点云图

图12. 不同γ值的点云配准定位误差

图13. 不同γ值的点云配准耗时

4.4. 点云畸变矫正实验

移动机器人从世界坐标系坐标原点出发遍历整个室内环境最后又回到起始位置，录制数据集共2223帧激光点云信息，机器人最大移动速度为1.5 m/s，最大加速度为2 m/s²，最大角速度为1 rad/s，整体运动状态为变速行驶。分别使用无点云畸变矫正的Cartographer算法、利用匀速模型进行点云畸变矫正的Cartographer算法、利用本文提出的融合轮式里程计进行点云畸变矫正的Cartographer算法分别运行该数据集。构建的二维地图如图14~图16所示，图中的蓝色线条为机器人的运动轨迹，轨迹上的点为机器人的位姿节点。

无点云畸变矫正时Cartographer算法构建的地图如图14，场景A对应的真实场景为图8中对应的室7和室8交界处，场景B对应的是室2和室3交界处，场景C对应的是室1和室4交界处。在真实场景所有墙面都是互相垂直的，因为点云畸变的存在图14的场景A中的墙面无法完全对齐，场景B中的墙面发生了严重倾斜，场景C中室1和室4之间的墙面已近完全丢失，这也就导致了机器人的轨迹出现了严重的飘移现象，对比图15和图16中的轨迹可以看出图14的场景C附近这部分的定位信息几乎完全丢失。基于匀速预测模型进行点云畸变矫正的Cartographer算法构建的地图如图15所示，可以看出同样的场景有了点云畸变矫正以后，建图效果得到了改善，但构建的地图与实际场景依然有较大的出入，图15 A中的墙面并未完全对齐，图15 B两墙面间距较大，图15 C中两墙面之间有大量的重影，这是因为当移动机器人处于变速运动时，匀速预测模型并不能准确的估计机器人的运动状态，所以点云畸变矫正的

图14. Cartographer (无点云畸变矫正)

图15. Cartographer (基于匀速预测模型的点云畸变矫正)

图16. Cartographer (融合轮式里程计的点云畸变矫正)

效果并不理想，这就导致构建的地图无法还原真实场景。对比图14~图16可以看出融合轮式里程计进行点云畸变矫正的Cartographer算法构建的地图整体无明显畸变。

回环误差效果如图14~图16所示。终点的真值与起点相同，从图14中可以看出实际定位位置和真值相差较远，这说明在没有点云畸变矫正的情况下Cartographer算法回环失败。从图15和图16可以看出点云畸变矫正后的Cartographer算法回环效果更好，从表4可以看出，融合轮式里程计进行点云畸变矫正的Cartographer算法的回环效果最好，误差在0.5 m以内，比基于匀速预测模型进行点云畸变矫正的Cartographer算法回环误差减小了81%。从图14~图16中可以看出除了重点圈出来的部分外，在长廊1，长廊3，长廊4，室2内也存在不用程度的地图偏移，特别在图14中长廊1发生了明显变形，室2内物体的特征也有部分丢失，图15、图16中虽然有所改变但也存在地图无法还原真实场景的情况。通过SSIM算法对比Cartographer算法结合不同点云畸变矫正方法后所建地图对真实场景的还原度如表5所示，从表5中可以看出文本提出的融合轮式里程计进行点云畸变矫正的方法对真实场景的还原度最高。

表4. 回环误差

表5. 场景还原度

5. 结论

本文对退化场景和变速场景中点云配准效率低，定位效果差，点云易发生畸变的原因进行了分析，将轮式里程计和激光雷达相融合用于改进上诉问题。将轮式里程计测量的移动机器人相对位姿变化信息作为PL-ICP算法配准的初值，同时利用自适应体素滤波法对原始点云进行下采样，在提升点云配准定位精度的同时降低计算量。实验结果表明本文提出的方法相比基于匀速预测模型的PL-ICP算法，在移动机器人变速运动时，在结构化场景中点云配准平均迭代次数减少了30%、平均耗时减少20.5%、定位误差减少了51.3%，在退化场景中的定位误差减少了87.4%。针对点云畸变问题本文提出基于EKF融合轮式里程计和点云配准插值的方法进行点云畸变矫正。实验结果表明融合轮式里程计进行点云畸变矫正的Cartographer算法在变速场景中回环误差减少了81%，对场景的还原度提升了24.2%，本文提出的点云畸变矫正方案除了移植到Cartographer算法中同样可以应用到其他算法中。

文章引用

汤玉春. 基于2D激光雷达的移动机器人同步定位与地图构建算法研究
Simultaneous Localization and Map Construction Algorithm for Mobile Robots Based on 2D LiDAR[J]. 建模与仿真, 2023, 12(04): 3299-3318. https://doi.org/10.12677/MOS.2023.124303

参考文献