基于ROS的无人扫地车高精度地图定位研究

基于 ROS的无人扫地车高精度地图定位研究

徐智豪，贺刘劝，陈智彪，王朱霖

（南通大学，江苏 南通 226000）

摘 要：为进一步研究并解决无人扫地车在校园道路内的自主定位和路径规划问题，本文提出利用激光雷达进行构建实时点云高精度地图并利用点云ICP算法进行刚体定位估算从而实现对无人车的动态定位。同时针对ICP算法的局限性提出了使用NDT算法对其进行改进。实验结果表明，该方法可以提高小车建图的精度以及灵敏度，提升无人车工作时的稳定性和安全性，减少了交通事故的发生。

关键词：无人驾驶；ROS；动态定位；点云建图算法

1 引 言

根据无人扫地车的独特用途和驾驶特性，在构建无人扫地车辆的控制算法时，首要的任务是高精度地图的构建设计，以确保无人基地车辆运行并符合运行要求。本文车辆使用velodyne16线激光雷达传感器，用以获取道路环境信息，使用了NDT算法进行了改进的ICP点云匹配算法，生成点云建图所需的点云数据。

2 点云建图算法设计

点云匹配是三维激光点云研究的关键，平台在相近的位置扫描得到的点云，可能会因为其视角不同导致数量不一致，但是会有重叠，而点云匹配的任务就是在给定一个粗略的位置后,能够得到正确的刚体变换使点云之间重叠程度最大。

2.1 ICP算法

ICP（Iterative Closest Point迭代最近点）算法是一种点集对点集配准方法。如图2.1所示，PR（红色点云）和RB（蓝色点云）是两个点集，该算法就是计算怎么把PB平移旋转，使PB和PR尽量重叠。

图2.1 三维坐标系下两片点云的示意图

ICP算法基本思想：

三维点云匹配问题的目的是找到P 和Q变化的矩阵R和T，对于

(1)

利用最小二乘法求解最优解使

(2)

最小时的R和T。

先对平移向量T进行初始的估算，具体方法是分别得到点集P和Q的中心：

(3)

(4)

在计算转换之前，从两个点集中的每个点减去相应的质心。

(5)

(6)

则上述最优化目标函数可以转化为：

(7)

最优化问题分解为：1.求使E最小的 2.求

T= (8)

ICP算法的优点在于它在较好的初值情况下可以得到很好的算法收敛性，不必对处理的点集进行分割和特征提取，但是作为一种基于点的方法，它最大的问题在于搜索对应点的过程占据了很高的计算成本。

2.2 NDT算法

3D NDT方法以一系列局部概率密度函数（probability density functions）将点云转化为一种平滑的曲面表达方式。3D NDT算法的第一步是将三维空间划分为规则的体素，并假设单元中的每个点都对单元的分布特性作贡献，并为每一个体素单元计算。

NDT 则使用局部 PDF(probability density function) 来描述点云的局部分布，该方法的优点是：1. 正态分布局部是平滑的，具有连续的导数；2. 每个 PDF 可以认为是局部平面的近似，描述了平面的位置（均值 μ\muμ）、朝向、形状和平滑性（协方差 Σ\SigmaΣ 的特征向量和特征值，特征向量描述的点云分布的主成分（principal components））等特征。

在3D 场景中，如果方差比较近似，局部点云形状为 point/sphere；如果一个方差远大于另外两个方差，局部点云形状为 line；如果一个方差远小于另外两个方差，局部点云形状为 plane。

相比ICP方法，NDT方法使用概率分布对点云的形状进行描述，充分考虑了点云的几何特性，有效提高了匹配的效率。但是同样基于非线性优化的NDT 算法在遇到误差较大的初值时，也会很容易落入局部极值中。

基本步骤如下：

将激光雷达扫描的点云数据分割为多个部分，并且计算各个部分中的点云数据的均值和方差，并根据均值和方差将各部分中的点云分布拟合为正态分布，如图3.3所示。均值和方差的计算方法为：

(9)

(10)

概率密度函数为：

(11)

通过以上方法计算激光雷达扫描的数据中点的响应概率，并以此作为目标函数，求解出使该目标函数取最大值的位姿变换关系。待求的优化目标函数为：

(12)

其中：

(13)

式中， ——点云中的点，R——旋转矩阵，t——平移向量。

经过上述目标函数的优化，NDT点云配准算法的匹配精度相较于ICP算法得到了提升，但仍会受到点云数据分割的部分参数的大小的影响。

2.3基于 NDT与ICP的快速点云匹配与建图

本文使二步法进行相邻帧之间的点云配准，首先使用预处理之后的点云数据利用 NDT 算法进行点云配准，对无人车的位姿进行粗估计，然后使用经 ICP 算法对 NDT 算法进行校正点云配准的误差，使得帧间误差函数最优，并完成对无人车位姿的精确估计。设经过点云预处理之后 t 时刻与 t + 1 时刻周围环境点云分别为

和 ，然后用 的点云与 的点云进行配准。

基本流程如下：

输入相邻帧点云 和

输入最优变换矩阵T

用NDT算法进行点云粗配

用ICP算法进行点云精配

图 2.2 基于NDT和ICP算法的改进算法流程图

其中使用NDT和ICP算法分别进行点云粗配、点云精配的基本程序流程如下：

1、使用NDT算法进行点云粗配：

for all do

q

end for

find

for all do

PDF P( )

S(P) PDF

end for

2、使用ICP算法进行点云精配：

begin：

（ ） ( , )对应点

d , 平均距离

minE(R,T)

End

3 实验测试

3.1不同算法实验结果对比

本实验无人车以 2.0 m/s 恒定速度行驶，为保证数据有效性，选取相邻帧之间的点云配准后得到平均变换矩阵。无人车虽然没有依靠惯性导航（IMU）进行定位，但是本文算法、ICP算法及NDT算法与车载惯性导航数据进行对比分析，下面分别给出相邻帧之间变换的平均矩阵T：

(14)

其中R为3*3的旋转矩阵，t为3*1的平移矩阵。

以IMU变换矩阵为参考进行数据分析：

IMU平均变换矩阵=

ICP平均变换矩阵=

NDT平均变换矩阵=

NDT-ICP平均变换矩阵=

以上这几组数据是ICP、NDT、NDT-ICP三种算法进行帧间点云配准时得到的平均变换矩阵，包括旋转矩阵R和平移矩阵T。本文以IMU提供的有效数据为基准，并IMU矩阵的值进行对比，本文所给出的改进算法无论是旋转矩阵R还是平移矩阵T都更接近于IMU的值。

表1 变换误差分析

如表1所示，经过改进后的算法在各个维度上的精确性均为三类算法中最高的。

3.2 实车测试结果

在Linux终端里面打开地图录制命令，遥控控制无人车在实验区域行走一遍，获取地图数据，生成地图包，在rviz图形界面里面运行Cartographer_ros来运行该算法，生成图形地图显示在rivz图形界面里面，获取的地图如下图3.1所示

图3.1 无人扫地车建图

4 结 语

本文主要构建了一种基于NDT与ICP的快速点云匹配算法以此解决了传统ICP算法耗时长效率低的缺点也保留了ICP算法的高收敛性的优点。第一步采用NDT算法进行点云配准，实现对无车位姿的粗估计，第二步采用ICP算法进行点云配准后的点云位姿校正，使帧间点云平移距离与旋转角度达到最优的配准参数，实现对无人车位姿的精确估计；最后点云配准随时间累积，点云信息不断更新，完成对点云地图的构建，为无人车的自主导航提供了先验地图，最终实现无人车的精确定位。后续这种改进算法不仅可应用于无人汽车的建图也可以应用于各种其它智能设备的建图环节。

参考文献

[1]刘东洋. 基于多线激光雷达的移动机器人SLAM技术研究[D]. 哈尔滨工业大学, 2020.

[2] Yan M, Wang J, Li J, et al. Loose coupling visual-lidar odometry by combining VISo2 and LOAM[C]. In: 36th Chinese Control Conference, 2017, 6841-6846.

[3] Zhang J, Singh S. Visual-lidar Odometry and Mapping: Low-drift, Robust, and Fast[C]. In: IEEE Conference on Robotics and Automation (ICRA), 2015.

[4]黄鑫涛. 工厂无人驾驶扫地车的设计与实现[D]. 北京林业大学, 2020.

[5]倪志康. 基于三维激光的移动机器人SLAM算法研究[D]. 苏州大学, 2020.

基金项目：创新训练项目 基于ROS的自主扫地车控制系统设计 202010304179H

作者简介：徐智豪（2000-），男，安徽池州，南通大学，本科

贺刘劝（2001-），女，河南永城，南通大学，本科

陈智彪（2001-），男，江苏南京，南通大学，本科

王朱霖（2001-），男，江苏苏州，南通大学，本科

3