基于三维激光雷达和反光柱的机器人定位*

陈新宇，雷 斌,1b，蒋 林,1b，张文俊

(1.武汉科技大学a.冶金装备及其控制教育部重点实验室；b.机器人与智能系统研究院，武汉 430081；2.立得空间信息技术股份有限公司，武汉 430073)

0 引言

随着响应国家“中国制造2025”战略规划，移动机器人技术在实际生活中和产品应用等领域也获到了高速的进展，如物流机器人[1-2]对货物的搬运、服务机器人[3]在家庭、酒店和商场中的使用、巡检机器人[4-5]在电力维修巡检方面的运用和自动驾驶技术[6-7]的逐渐崛起。

其中移动机器人的定位就是要通过不同的传感器获取其在当前环境下的位姿信息。常用的传感器如轮式里程计、GPS、IMU(惯性测量单元)、相机和激光测距仪等。基于轮式里程计的定位[8]和基于IMU(惯性测量单元)的定位方法在定位过程中会产生累计误差，需要进行修正。基于相机的定位方法[9]主要是通过获取环境中的视觉特征如ORB[10]特征来进行匹配定位，但是会受到光照和测距有限影响，定位鲁棒性不高。基于激光雷达的定位方法是通过当前帧和上一帧数据进行搜索匹配，常用的匹配方法有：ICP[11](迭代最近点)、NDT[12](正态分布转换)和KD树等点云配准方法。而激光雷达不受环境光照等影响，具有精度高，测量距离远和鲁棒性更高等优点。

激光雷达也可以通过获取环境中人工设定的特殊标签进行定位，RONZONI等[12]通过布置的标签进行机器人定位，但是这种利用线段进行特征匹配的方法效率不高，定位精度和定位鲁棒性较差。吴波[13]提出了一种利用二维激光雷达和设置的反光板来进行移动机器人定位的方法，并结合里程计使用扩展卡尔曼滤波算法融合定位，但是该方法的使用场景有限。周凯月等[14]提出了一种联合二维激光雷达和反光柱的定位系统，将反光柱作为位姿图优化的一个节点，采用联合优化激光点误差和反光柱匹配误差的方式定位机器人的位姿。丁松等[15]提出了一种基于特征分割与特征匹配的智能叉车定位算法，该方法采用单线激光雷达，基于反光柱反射点特征分割算法和以反光柱之间距离为先验条件的特征匹配来求解智能叉车的位姿。

但是上述方法在更加空旷的环境和缺少特征信息的环境中的定位效果较差。因此，本文针对上述问题设计了一种基于三维激光雷达和反光柱的移动机器人定位算法，该算法利用激光雷达的反射强度信息对反光柱进行点云分割。采用ALOAM算法构建点云地图。构建KD树最近邻域搜索进行匹配求解机器人的位姿。本文分别借助仿真环境和真实环境实验平台进行相关方法阐述及验证。

1 反光柱识别和分割

1.1 激光雷达反射强度原理

三维激光雷达的测距原理为飞行时间测量法。主要是通过激光的飞行时间计算物体与激光传感器的距离。其计算方法如式(1)所示。

(1)

式中，d为距离；c为光速；t为激光束的飞行时间。激光雷达的转子上固定安装多对激光发射和接收装置，通过内部电机旋转实现水平方向360°扫描。物体在传感器坐标中的位置由发射角度和距离(即到达时间)确定。强度值由从反射信号接收的能量与发射的激光功率之比确定。接收功率Pr的物理公式为：

(2)

式中，Pe是发射激光束的功率；Dr是接收器直径；ηsys是系统发射因子；ηatm是大气发射因子；α是物体表面与激光束之间的入射角；ρ是物体的材料反射率；测量距离R和入射角α为外部参数。而ηsys和ηatm是常数参数。因此反射强度I的物理公式为：

(3)

式中，ηall是一个常数。表面反射率ρ仅与入射角α和测量距离R有关：

(4)

式中，R为激光雷达的测量距离，物体表面和激光束之间的入射角α采用分析局部法线获得。对于每个点P∈P3，寻找最近的两点P1和P2，局部表面法线n可以表示为：

(5)

式中，×表示叉积，入射角为：

(6)

校准强度扫描I～可以从具有几何读数P和强度读数I的原始扫描中推导出，三维激光雷达使用距离测量部分来校准入射角度。同时低强度值往往会导致较低的信噪比(SNR)，这也降低了在低值下进行强度测量的测距精度。因此在相同的测量距离和入射角下，采用不同的反射材料，强度值会随着物体的材料反射率发生改变，引入高反射率的材料可改变点云反射强度信息。

1.2 反光柱点云识别和分割

反光柱是一种外表贴附高反射强度材料的圆柱体，激光照射在其表面上会有超高的反射强度。因此，本文利用反光柱的反射强度进行特征提取，进而对反光柱点云进行分割，进而实现机器人的全局定位。图1所示为反光柱的实物图，由激光雷达扫描获得的含有反光柱的激光帧中的反射强度值较大，因此设置阈值，进行点云分割。在本文中选择的反射强度阈值是200。对于激光雷达扫描到的每一帧点云保存XYZI信息，设置一个反射强度阈值，认为小于反射强度阈值200的都为离散点，将其滤除。

图1 反光柱实物图

1.3 特征角点提取

本文使用十六线激光雷达获取激光数据进行特征角点的提取。采用从一帧激光帧中计算曲率的方式进行特征点的提取。具体的特征提取方法为，先对每一根激光扫描线进行遍历，从第六个点开始对每一个点分别提取其前后各五个点，利用这十个点计算该点的距离偏差平方，作为其曲率。曲率公式为：

(7)

1.4 特征匹配

(8)

对于求解得到的点到直线的距离采用列文伯格-马夸特法非线性优化进行求解。LM方法的框架为下：

步骤1：设置先验值x0和先验优化半径μ；

步骤2：对于第k次迭代，在高斯牛顿法的基础上加上信赖区域，求解：

(9)

(10)

式中，f(xk)是目标函数；J(xk)T是f(xk)的一阶导数也称为雅可比矩阵；μ是信赖区域的半径；D是系数矩阵。

步骤3：计算ρ；

(11)

式中，ρ是近似程度的评价指标。

步骤6：如果ρ过大，则令xk+1=xk+Δxk；

步骤7：判断结果是否达到极小值。如果没有达到返回步骤2。

在该非线性优化中引入拉格朗日乘子来对目标函数进行约束，其公式为：

(12)

(13)

(14)

式中，λ是系数因子，对上式求导得到：

J(Δxk)f(x)+J(Δxk)(Δxk)+λDTDΔx=0

(15)

化简后求解增量的线性方程：

(H+λDTD)Δxk=g

(16)

对上式进行求解直到达到收敛为止。结合反光柱的定位环境因此本文选用列文伯格-马夸尔特优化的方法来对机器人的位姿进行优化，得到更加精确的定位信息。

2 反光柱匹配定位系统

2.1 反光柱的布置

为了实现机器人定位的精度，在反光柱环境中对反光柱的摆放主要有以下要求：机器人在行驶轨迹上每个位置至少能观测到两个反光柱；反光柱的高度和机器人上的激光传感器高度相同。

2.2 反光柱定位系统

反光柱定位系统的框架如图2所示。按照反光柱的摆放规则布置反光柱环境，基于三维激光雷达禾赛PandarXT16激光雷达进行激光数据的采集，对采集的激光雷达数据进行点云预处理，提取点云中所有点的反射强度信息，设置一个反射强度阈值，对反光柱点云进行分割。采用ALOAM算法利用反光柱点云构建反光柱点云地图。对当前帧进行预处理，计算点云中每个点的曲率，利用曲率进行特征提取根据前一帧的位姿将当前帧的角点坐标转换到世界坐标系下。利用反光柱点云地图构建KD树，在世界坐标系下在KD树中通过最近邻域搜索最近的5个角点，由5个角点的坐标均值，构建协方差矩阵。对协方差矩阵进行特征值分解。利用特征值和特征向量计算点到直线的距离，利用列文伯格-马夸尔特对特征向量进行非线性优化，求解输出机器人的当前位姿。

图2 反光柱定位系统框架图

3 实验结果和分析

3.1 仿真环境实验

本文所采用的移动机器人仿真实验平台是基于Ubuntu16.04操作系统和ROS机器人开发平台的GAZEBO仿真平台自主开发和搭建。所搭建的仿真机器人MRobot的底盘为差分运动底盘，所使用的激光传感器为仿真的VLP-16三维激光雷达，其技术参数如表1所示。最大测量距离为100 m，最大扫描范围为0°～360°，最高分辨率可达0.1°，最高扫描频率可至20 Hz。所使用的反光柱的直径为0.1 m，高度为1 m。在GAZEBO中搭建一个长度为10 m，宽度为2.5 m的仿真走廊环境，反光柱在走廊两侧以2 m为间距，对称摆放。为验证本文反光柱定位算法在仿真环境中机器人静态定位的精度，分别进行了静态定位实验和动态定位实验。将机器人在仿真走廊反光柱系统中进行静态定位测试，将其与GAZEBO仿真环境中的真实机器人坐标进行比较。动态定位实验是将记录的机器人定位轨迹和预定的真实机器人运动轨迹进行比较。定位的误差计算方式为：

Δx=|xt-X|

(17)

Δy=|yt-Y|

(18)

Δθ=|θt-θ|

(19)

Δs2=(xt-X)2+(yt-Y)2

(20)

式中，Δx为横坐标定位误差；Δy为纵坐标定位误差；Δθ为航向角定位误差；Δs2为距离偏差平方；xt为实际测量的横坐标；yt为实际测量的纵坐标；θt为实际测量的航向角；X为真实的横坐标；Y为真实的纵坐标；θ为真实的航向角。

表1 VLP-16激光雷达性能参数

图3 仿真反光柱环境 图4 仿真机器人实验平台

3.1.1 仿真环境静态实验

实验步骤如下：订阅激光雷达激光话题，利用ALOAM算法构建反光柱地图。输入反光柱点云地图和当前帧激光点云。启动仿真机器人，进行静态定位实验。静态的定位误差如图5所示，分别为X误差、Y误差、yaw误差和距离平方偏差。表2为静态定位数据的误差分析，由表2可知，由本文算法图5仿真环境静态定位误差得到的横坐标、纵坐标、航向角和距离偏差的平均值分别为0.017 38 m、0.034 17 m、0.002 61 m和0.005 04 m。标准差分别为0.025 19 m、0.023 05 m、0.005 42 m和0.002 37 m。从实验数据可知，机器人定位位姿平均值在横坐标和纵坐标上的差值在0.05 m之内，航向角上差值在0.01之内。它们的标准差在横坐标和纵坐标方向上都是0.05 m量级或以下，航向角在0.01以下。因此可以证明本文算法在仿真环境下机器人定位精度较高，稳定性较好，基本没有出现定位失败的情况。

图5 仿真环境静态定位误差

表2 仿真环境静态定位误差 (m)

3.1.2 仿真环境动态实验

为进一步验证本文算法的定位效果，进行机器人进行动态定位测试。实验步骤如下：机器人以0点为起点沿着Y轴方向分别进行了匀速为0.1 m/s、0.2 m/s和0.3 m/s的动态定位轨迹的测试，行走的距离为10 m。机器人的真实运动轨迹为Y轴。机器人输出定位轨迹和机器人真实运动轨迹的轨迹以及动态定位误差如表3所示。

表3 仿真环境动态定位实验分析

由表3的实验结果可知机器人的输出定位轨迹和机器人真实运动轨迹重合度较高，方向相同，轨迹曲线基本保持一致。机器人仿真动态定位误差数值分析如表4所示，其中X、Y和S分别为X轴误差、Y轴误差和距离平方差。机器人运动速度分别为0.1 m/s、0.2 m/s和0.3 m/s时定位距离偏差的平均值分别为0.001 837 m、0.002 3 m和0.002 38 m。标准差分别为0.001 743 m、0.001 65 m和0.001 692 m。从实验数据可知，机器人定位位姿平均值在横坐标和纵坐标上的差值在0.06 m之内。它们的标准差在横坐标和纵坐标方向上都是0.05 m量级或以下。经过实验数据表明本文定位算法可以满足在仿真环境动中实现较高的定位精度。

表4 仿真环境动态定位误差

3.2 真实环境定位实验

本文所使用的实验平台硬件部分主要包括工控机处理器为Core i7-860S、移动机器人底盘是ROBOTM500、激光传感器为禾赛pandarXT16线激光雷达。禾赛pandarXT16线激光雷达数据参数如表5所示。搭建的实验平台如图6所示。

表5 PandarXT16激光雷达性能参数

本文所使用的软件平台为ubuntu16.04操作系统。在ubuntu系统中采用ROS(机器人开发平台)进行通信。在本文实验中，禾赛pandarXT16多线激光雷达数据通过多倍通电台传入工控机。反光柱环境如图7所示，在走廊两侧依次排列。反光柱的直径为0.09 m，高度为0.5 m，在走廊两侧以2 m为间距，对称摆放。

图6 机器人实验平台 图7 实际反光柱环境

3.2.1 反光柱点云分割实验

基于上述真实反光柱环境，进行反光柱点云分割实验。实验步骤如下：构建反光柱环境的原始点云地图。设置反光柱反射强度阈值，对反光柱原始点云地图进行分割，滤除多余的点云，保留反光柱点云。图8为机器人通过环境信息构建的原始反光柱点云地图。图9为分割后的反光柱点云地图，图中的柱状点云即为分割得到的反光柱。分割前后的反光柱点云地图点云数量以及点云所占比例如表6所示。反光柱点云地图分割出的反光柱点云不包含其他冗余点云，只保留反光柱点云，进行匹配定位。

图8 原始反光柱点云地图 图9 分割后的反光柱点云地图

表6 反光柱点云地图点云分割点云数量统计

3.2.2 静态定位实验

为验证本文反光柱定位算法在真实环境中机器人静态定位的精度，分别测试机器人在不用位置下的位姿，与机器人真实位姿进行对比，计算定位误差。本文算法在真实环境下的静态定位误差如图10所示。真实环境下的静态定位误差分析如表7所示。

图10 真实环境静态定位误差

表7 真实环境静态定位误差 (m)

由表7可知，X、Y、S分别为横坐标误差、纵坐标误差和距离偏差。横坐标、纵坐标和距离偏差的标准差分别为0.027 33 m、0.033 04 m和0.006 37 m，它们的平均值分别为0.010 38 m、0.094 17 m和0.012 61 m。从实验数据可知，机器人定位位姿平均值在横坐标和纵坐标上的差值在 0.1 m之内。其标准差在横坐标和纵坐标方向上都是0.04 m量级或以下。由此可以证明本文算法在真实环境下机器人的静态定位精度较高，稳定性较好，基本没有出现定位失败的情况，可以实现在真实环境中的静态定位。

3.2.3 动态定位实验

为验证本文反光柱定位算法在真实环境中机器人动态定位的精度，分别测试机器人在不同线速度下的运动轨迹。在真实环境中，本文以机器人的起点为原点，以机器人运动方向为Y轴，构建世界坐标系。分别以0.3 m/s、0.5 m/s和0.7 m/s的线速度匀速沿Y轴进行动态定位轨迹测试。机器人的真实运动轨迹为沿Y轴直线行驶9 m的直线。机器人的定位轨迹如图11所示，机器人在不同速度下的定位误差如图12所示。机器人在真实环境下的动态定位误差如表8所示。

图11 机器人运动轨迹

图12 不同速度下动态定位误差

表8 真实环境动态定位误差

由表8的实验结果可知机器人的输出定位轨迹和机器人真实运动轨迹重合度较高，方向相同，轨迹曲线基本保持一致。其中X、Y和S分别为X轴误差、Y轴误差和距离平方差。机器人运动速度分别为0.3 m/s、0.5 m/s和0.7 m/s时定位距离偏差的平均值分别为0.002 64 m、0.005 43 m和0.009 99 m。标准差分别为0.001 96 m、0.001 59 m和0.006 41 m。从实验数据可知，机器人定位位姿平均值在横坐标和纵坐标上的差值在0.06 m之内。它们的标准差在横坐标和纵坐标方向上都是0.03 m量级或以下。经过实验数据表明本文定位算法精度较高，稳定性好，可以满足在真实环境中使用。

3.2.4 对比实验

为了验证本文算法增加反光柱后对机器人定位精度的提升效果。本文进行了真实环境中有反光柱和无反光柱定位对比实验，分别记录了机器人在有反光柱的环境和无反光柱的空旷环境下机器人的定位轨迹，如图13所示。可以很清楚的看出空旷环境下的机器人定位轨迹在X轴负方向产生了很大的漂移，而增加反光住后，机器人定位轨迹和真实运动轨迹重回度较高，基本保持一致。因此可以证明在空旷环境等特殊环境下，相比于空旷环境增加反光柱后机器人定位精度更高，定位稳定性更好，整个定位系统的鲁棒性更高。

图13 定位轨迹对比实验

4 结束语

本文针对缺少特征信息的空旷环境下机器人定位稳定性和精度不高的问题，提出了一种三维激光雷达和反光柱耦合定位算法。该算法利用激光雷达的反射强度信息对反光柱进行点云分割。对当前帧激光进行特征提取，构建KD树最近邻域搜索和反光柱点云地图实现帧到地图的匹配，求解机器人的位姿。实验结果表明，本文所提出的方法具有更高的精度和较好的鲁棒性。可以有效满足机器人在定位中的需求。