作者：小柠檬 | 来源：3DCV

在公众号「3DCV」后台，回复「原论文」获取论文

+v：dddvision，备注：机器人导航，拉你入群。文末附行业细分群

导读

定位技术是机器人技术中的一个关键问题，包括导航控制和路径规划。传统的定位方法涉及使用全球定位系统（GPS），这在实现精确定位和导航方面提出了重大挑战。因此，非基于GPS的定位和导航方法已成为机器人领域的研究热点。该文提出一种基于激光雷达数据采集的点云信息帧匹配方法。它结合了一种改进的激光点云数据位姿估计算法，并将其与非线性优化技术相结合进行标定和优化，从而实现移动机器人在未知环境下的精确定位。利用ROS机器人操作系统构建仿真实验平台，对程序进行设计，并验证改进算法的验证。实验结果表明，改进的帧匹配算法在映射和定位效果方面表现出更好的鲁棒性和定位精度。同时，既能满足工程要求，又能基于光栅地图完成移动机器人的自主导航。

1、介绍

斯坦福研究所（SRI）的Nils Nilssen等人在1960年代初开发了第一个Shakey自主移动机器人。随着技术的进步，工业移动机器人现在被广泛应用于各种行业，包括航空航天、物流分拣、机械制造、动力传动和化工生产。移动机器人的导航技术是体现其自动化和智能化的关键环节。

哈尔滨工业大学和北京化工大学开发了一种轮式移动机器人的路径跟踪控制方法。微波信号发射点的错位会使基于GNSS的地面参考帧（TRF）产生偏差。杨波等人采用基于航向信息的MIMU/GPS组合导航方法解决了少于四颗卫星的高精度导航问题。特别是在未对准的条件下，在大角度下可以快速收敛滤波。通过GPS/GLONASS模糊度修复，Ali Mohammed Al-Shaery将定位精度提高到厘米级。

传统GPS的定位和导航精度仅为5-10米。已经提出了各种算法来提高GPS定位精度和完善GPS信号丢失场景，但GPS定位仍然存在问题且成本高昂，而惯性导航计算则存在初始化和累积误差。惯性导航计算会受到初始化和累积误差问题的影响。通过激光扫描技术和实时定位系统，可以进行实时姿态跟踪。同步定位和映射 （SLAM） 是在 1990 年代开发的，用于在不熟悉的环境中进行姿态估计和定位。DE-SLAM是由Xing、Zhiwei等人开发的可视化SLAM系统，用于处理短期和长期动态元素。最近，Liao、Ziwei等人将对象的概念引入SLAM中，并提出了一种新的单目语义对象SLAM（SO-SLAM）。

基于激光雷达成像原理和帧间匹配方法，利用基于ROS机器人操作系统构建的仿真环境，分析了激光雷达移动机器人的导航算法。

2、激光雷达的采集原理与畸变校正

2.1激光雷达原理

在激光传感器中，激光发射组件快速旋转并发射脉冲激光，脉冲激光是一种三维传感器，用于接收回波信号以获取目标上的所有目标点数据。这部分称为激光雷达的距离图像;另一部分是物体反射激光束后接收到的回波强度，其强度取决于目标在激光波长处的反射特性。Ladar原理图如图1所示。

激光雷达到目标物体的距离为，激光脉冲从发射到接收的时间间隔为，可得到：

在飞行时间法中，由于对时间测量精度要求较高，飞行时间的微小测量误差都会造成较大的测距误差:

是时间差的测量误差。

2.2激光雷达的畸变校正

由于物体或安装激光雷达的物体的绝对位置发生变化，点云数据会发生一定程度的扭曲，这可能会影响点云帧之间的姿态估计结果。

为了更好地理解机器人和地图，建立了统一的笛卡尔坐标系，如图2所示，表示现实世界、机器人和激光雷达传感器之间的相对位置关系。世界坐标系、机器人坐标系和激光雷达坐标系分为、和。时间之后的机器人和激光雷达坐标系分别定义为和 坐标系。和个被设置为世界坐标系中的观察位置，一个是计时开始时观察到的目标点，是在时间后观察到的目标点。

进行一系列坐标变换，将所有激光点的数据转换为第一束激光原点的坐标系，然后对数据进行打包，从而大大减少了激光雷达数据帧的失真。

2.3机器人现场运动学模型

差速轮机器人底盘具有结构简单、成本低廉、型号简单等特点。该仿真实验使用图3中的底盘模型。

差速器底盘运动模型如下。差速器底盘运动模型如图4所示。

车轮到底盘中心的距离为，底盘中心到旋转中心的距离为，底盘中心线速度为，底盘中心角速度为，左轮和右轮的线速度分别为和分别。

由于两个车轮的角速度等于底盘中心的角速度，因此得到以下公式。

接下来，可以计算底盘的中心线速度，

两轮差速器底盘运动学模型可由下式获得。

3、改进的激光器间ICP帧匹配算法

激光雷达测量目标点数据以点云形式输出。点云数据是激光雷达采集的一组三维空间点。假设第一组点云由激光雷达获得，激光雷达采用第二组点云机器人进行旋转和平移的姿态转换后。和的坐标分别对应运动前后的坐标系（坐标的原点始终是激光雷达的中心）。此外，我们基于相关算法对点云存储的顺序进行了筛选和调整，使和中的点排列有序。

假设机器人旋转并平移，则从坐标系变换到无误差的公式为：

由于噪声和假匹配，上述公式可能并不总是正确的，因此我们尝试最小化目标函数。例如，（,）不对应于空间中的同一点，但特征匹配算法错误地认为两者是同一点。

svd方法用于求解和。首先，应定义两组点云的质心：

目标函数可以看作是：

经过计算，如果,，目标函数可以简化为:

设，为最优解：

上面的等式可以扩展为:

让，然后分解，当为满秩时，可以得到：

对应独特的、组合，即可得到：

该文提出了一种改进的帧间匹配算法PL-ICP，利用激光点到最近两点线的距离来模拟激光点到表面的距离，提高了算法的精度和速度，减少了算法中的误差，如图5所示。

第一步，对坐标系进行转换，利用最近邻法则将当前数据帧投影到参考系坐标系中，去除误差过大的点，直至根据最小化误差函数对旋转矩阵R和平移矩阵T进行优化。

PL-ICP的目标函数表达式为：其中是法向量是时间的参考系，是时间在和之后的时间处更改为当前帧。PL-ICP示意图如图5所示。

利用上述最小二乘误差目标函数，利用G2O优化库完成了姿态估计解。

4、实验与分析

4.1、 机器人平台建设

本文提出了一种基于ROS机器人操作系统的LINUX环境下的实验平台设计和算法设计，并在ROS机器人操作系统中集成了Gazebo物理仿真平台和Raviz数据可视化平台。首先，根据硬件机器人底盘平台的设计，完成移动机器人通用底盘的URDF模型。设计方案以万向轮为支撑，以差分动力轮模型为支撑，完成环境感知的二维激光雷达检测设计。此外，还设计了底盘的形状、颜色、尺寸、碰撞参数和惯性矩阵。下图6为相应移动机器人通用底盘的结构连接图，下图7为RVIZ中机器人通用底盘的统一描述平台。

本实验设计了机器人底盘的凉亭仿真平台。在URDF机器人模型中添加凉亭参数标签，包括惯性、摩擦系数等。同样，在移动机器人底盘中加入控制器模型插件，实现传统的PID控制算法。在运动控制的基础上，设计了Python键盘控制。修改后的程序可以发出CMD_VEL运动控制命令。机器人底盘控制节点根据预期值和反馈值，对移动机器人底盘进行误差控制。

基于凉亭仿真的移动机器人底盘如图8和图9所示。在这种环境下增加了一些障碍物，并利用键盘运动控制节点来移动机器人底盘。

4.2、实验环境的构建

机器人的移动底盘已经加载到凉亭环境中，但其环境感知系统尚未添加。增加了单线激光雷达传感器的凉亭模型，并利用标定参数对激光运动畸变进行了补偿。该传感器可以定期扫描环境信息并设置分辨率参数。激光参数为扫描角度360°，单圈360检测点云数据，采样频率5.5Hz，检测距离0-10m。如图10所示，RVIZ显示移动机器人智能底盘采集的传感器数据：

上述物理仿真模型是在仿真环境中构建的，然后加载到凉亭环境中。构建激光SLAM，完成导航仿真实验，如图11所示。

4.3、在仿真环境中实现SLAM地图构建算法

针对本项目中的姿态求解，采用改进的ICP姿态估计SLAM算法，然后利用SLAM构建网格，完成机器人的导航功能。地图导航流程如下：

如图12所示，移动机器人底盘的SLAM算法分为使用前搭建的地图，改进后保存的网格图如图13所示，集成度更高。

如下图14所示，导航和最优路径规划功能基于设定的目标点和作为已知信息构建的地图SLAM完成。机器人通用底盘的路径规划和避障实验在RVIZ可视化平台中从当前位置渲染到目标点。

下图15显示了基于改进的ICP算法完成了机器人的自主定位和路径规划实验。

该传感器实时感知环境，修改网格图的障碍物信息，控制机器人底盘移动到设定的目标点，如图16所示。连接到激光雷达的机器人图片如图16所示。

在图17中，将机器人算法改进前后的轨迹分析与传统帧间匹配算法构建的网格图进行了对比。改进后的轨迹误差从-0.134m下降到-0.097m，平均误差从-0.076m下降到-0.050m。结果，均方根误差增加了69.86%，平均误差增加了34.21%，整体定位误差增加了约81%。定位精度对于机器人环境探索、任务规划和避障至关重要。姿态定位优化前后的误差对比见表1。

表1 姿态定位优化前后的误差比较

5、结论

本文利用ROS系统的Gazebo对差分移动机器人进行仿真，利用二维激光雷达传感器对周围环境进行检测，采集障碍物信息。将改进的LiDAR前端帧间匹配算法与最小二乘误差目标函数的构造相结合，并根据后端非线性优化函数程序进行优化。改进的帧间匹配算法求解的机器人姿态接近地面实况值，构建地图的精度也有所提高。稳定性较好，整体定位误差在81%左右。未来发展将实现传感器模型的标准化，将前端匹配和后端优化模块化，结合多种传感器融合，提升硬件计算平台的能力，并将该系统应用于精准、鲁棒的户外机器人定位导航系统。

专业！3D视觉视频课程

3D视觉工坊精品课程官网：3dcver.com

我们的3D视觉学习平台包含：相位偏折术、面结构光高阶、结构光、机械臂抓取、工业点云实战、Open3D、深度学习缺陷检测、3D缺陷检测、BEV&Occ、Transformer、模型部署、3D目标检测、单目深度估计、车载标定、规划与控制、无人机、三维视觉C++、三维视觉python、dTOF、相机标定、ROS2、机器人控制规划、LeGo-LAOM、激光-视觉-IMU-GPS融合SLAM、LOAM-SLAM、室内室外SLAM、VINS-Fusion、ORB-SLAM3、MVSNet三维重建、colmap、线面结构光、硬件结构光扫描仪。