激光SLAM移动机器人室内定位研究

严小意，郭 杭

(南昌大学信息工程学院，江西 南昌 330031)

移动机器人在未知室内环境导航中，稳定可靠的定位结果是规划路径的关键先决条件[1-2]。在室外环境，GNSS或GNSS/INS融合导航系统是一种有效的导航定位解决方法。然而，GNSS在室内环境却变得无效，而室内避障对于移动机器人定位显得尤为重要，必须根据环境场景、路径规划和指令这些信息来控制移动机器人沿着路径移动[3-5]。因此，有必要采用一些导航传感器来获得这些信息，移动机器人一般采用声呐、激光雷达、相机等传感器来检测环境。由于激光传感器具有测距实时性快、不受光线环境限制、测距精度高、误差相对恒定等特点，因此在构建地图、机器人的导航定位、环境建模等各方面得到普遍应用[6]。

LiDAR扫描匹配技术虽然能够提供高精度的位置、航向定位结果，但是遇到环境特征不明显的情况，LiDAR的定位精度便大幅度下降，容易受到外界环境的干扰，同时LiDAR采样频率低[7]。惯性导航技术相对于LiDAR定位技术而言，它不受外界环境的影响，具备自主导航的能力，可以提供髙频率的速度、位置、姿态全导航参数。可是，随着时间延长，IMU的导航定位误差会逐渐累积，因此单独的惯性导航仅仅能在很短的时间内提供高精度的导航定位结果。而LiDAR/IMU组合导航结合了LiDAR和IMU这两种导航定位技术的优势，取长补短，可实现在室内连续工作的高采样率、高精度的组合系统。

综上所述，本文采用基于LiDAR/IMU组合导航的信息融合技术，将LiDAR扫描匹配算法解算得到的位置、姿态观测量作为组合卡尔曼滤波器的输入量，与IMU得到的位置、姿态结果进行组合，以提高室内移动机器人的运动轨迹定位精度。

1 传感器介绍

1.1 惯性测量单元

IMU是用来测量物体三轴姿态角及加速度的传感器[8]。本文仅对加速度计和陀螺仪的数据进行处理，而物体在载体坐标系下的加速度通过三轴加速度计获取，载体相对于导航坐标系的角速度通过三轴陀螺仪获取，根据在三维空间中物体的角速度和加速度，进而解算出物体的姿态。由于不可避免的各种干扰因素，导致陀螺仪及加速度计从初始对准开始产生误差，其导航误差即随时间而增长，因此，应该考虑它的误差和噪声。用字母b代表误差，字母n代表噪声。噪声假设是零均值高斯白噪声，且进行建模为随机过程。实际值与角速度和线加速度测量值可以表达为

wt=wm-bw-nw

(1)

at=C(qt)(am-ba-na)+g

(2)

式中，wt和wm分别为角速度的实际值和测量值；g为重力矢量；qt为真实姿态的四元数；C(qt)为真实姿态qt的旋转矩阵；nw和na分别为陀螺仪和加速度计的噪声；bw和ba表示相关误差。由于这些误差，IMU测量会随着时间推移产生漂移，为了避免这种漂移，IMU通常与其他传感器融合，如相机或激光雷达。

1.2 激光雷达

LiDAR是一种测量从传感器到被测物的角度和距离的光学传感器。依据从发射激光脉冲到接收脉冲的时间计算传感器与被测物体间的距离[9-10]。这种技术常用来创建高分辨率地图，如SLAM是一个实质性的应用，其目的是构建一个未知环境的地图且同时追踪机器人的位姿。激光雷达径向扫描平面中的环境，为了获得每个扫描点的坐标，采用方程式

式中，d为从激光雷达到扫描点的距离；φ为光束角度。由于(x，y)是在激光雷达载体坐标系下扫描点的坐标，并非导航坐标，因此需要将它们转换为导航坐标。

2 组合导航方案

由于单一的激光导航测量会随着时间延长存在累积误差，GPS在室内丢失信号而无法定位，因此，有必要引入其他导航传感器来获取移动机器人精准的位姿信息以改善单一传统的导航系统带来的累积缺陷[11]。总之，基于LiDAR/IMU的卡尔曼滤波组合导航系统可用来减少LiDAR定位误差，从而提高移动机器人在室内自主导航中的定位精度和稳定。卡尔曼滤波[12]是一种最优的线性估计方法，它使用状态空间描述方法来描述系统，并连续估计各种传感器输出的估计误差，从而得到所有传感器的最优信息。

测量更新和时间更新是卡尔曼滤波的主要过程[13]。时间更新是指根据系统前一个时刻的状态和协方差矩阵，得到当前时刻状态先验估计和协方差先验估计，为两个相邻时刻系统的状态更新过程。测量更新是指将当前时刻获得的测量信息对时间更新过程中所得的状态先验估计和协方差先验估计进行更新校正的过程，以得到比先验估计要精确的状态后验估计和协方差后验估计的值[14-15]。依据当前时刻的状态后验估计和协方差后验估计又能知道下一时刻的状态先验估计和协方差先验估计，这就是卡尔曼滤波作用的优势。由于使用的状态方程是基于误差的，本文不涉及扩展卡尔曼滤波，这里使用构成反馈脉冲的滤波。

卡尔曼滤波初始化

(3)

(4)

(5)

式中，k为离散时间；Xk∈Rn为k时刻离散系统的状态向量；Ak∈Rn×n，为离散系统的状态转移矩阵；Bk∈Rn×l，为离散系统的输入控制矩阵；uk∈Rl，为离散系统的输入向量。

(2) 协方差更新可表示为

(6)

(3) 卡尔曼滤波增益可表示为

(7)

(4) 当前状态最优估计可表示为

(8)

(5) 更新当前时刻的协方差矩阵可表示为

(9)

式(7)—式(9)为卡尔曼滤波算法的测量更新过程。

3 室内定位试验分析

为了验证卡尔曼滤波融合后的定位精度，本文使用安装有Rfans16线激光雷达传感器和IMU传感器的WX-DP203移动机器人试验平台(由实验室设计和开发)，沿着实验室进行了室内试验。将激光雷达作为外部传感器，通过USB接口实现与PC的通信，利用ROS操作系统分布式的特点，PC机为主机，该产品数据采集频率为10 Hz，测距精度小于3 cm，在最远距离处测量误差小于8 mm，在测量范围为1.5 m内，误差小于0.5 mm。IMU用100 Hz频率采集数据。为了比较最后结果的定位精度，预先计划好相关路径，设置包括起始点的4个控制点，并利用全站仪获得局部坐标系下控制点的精准位置。原点设为起始点坐标，测量的控制点坐标见表1。

表1 控制点位置坐标 m

在试验过程中，机器人向前移动了8.8 m之后回到起点，每秒移动不超过20 cm，总共获得211个点。其中，机器人在ROS操作系统下形成的地图与真实定位轨迹和LiDAR/IMU组合定位轨迹的结果分别如图2、图3所示。表2为设置的4个控制点与LiDAR/IMU卡尔曼滤波融合后位置误差分析。真实轨迹与LiDAR/IMU组合定位轨迹的X方向、Y方向和欧拉距离的误差分析如图4所示。

表2 LiDAR/IMU卡尔曼滤波融合和控制点的位置比较 m

从表2可以看出，当机器人移动到左上拐角3.49 m处时，误差缓慢累积，定位精度为0.024/3.49×100%=0.57%，这属于比较高的定位精度，随着机器人平台移动，误差累积的更快，定位精度是0.056/3.79×100%=1.47%，另外，从图4中可以更加明显地看到误差累积变化。考虑到机器人本身可能在操作过程中侧滑以及LiDAR传感器容易受到噪声和其他因素的影响，在试验结果中存在一定程度的误差。但是从定位精度来看，LiDAR/IMU卡尔曼滤波融合在减小累积误差方面起着非常好的作用，这两种传感器组合可以实现更高的定位精度。机器人转弯前累积误差比较小，转弯后累积误差稍微变大，之后又慢慢变小。上述结果表明，短距离融合定位误差累积非常小，定位精度高。在最初的计算阶段IMU使轨迹从不稳定变为稳定，当位姿发生变化大时(如沿路线转弯)时，IMU传感器作用显得比较重要，能改善方向角的定位精度，并且还能提高方向角的稳定性。

4 结 论

本文将LiDAR/IMU两个传感器通过卡尔曼滤波器相融合进行综合定位系统研究，卡尔曼滤波器的综合定位结果利用IMU传感器减小LiDAR产生的漂移，进而减小其轨迹误差，提高了室内机器人的定位精度。Rfan16线LiDAR具有普通激光传感器的优点，且定位精度高，由于IMU的高采样率和相对较高的精度增加了数据采集数量，LiDAR和IMU集成系统相互受益，克服了各自的缺点，从而提高了系统的稳定性。然而，由于研究刚刚开始，该方法仍具有精度限制，还有许多工作等待后续处理。将来的研究工作是增加移动机器人的测量范围并提高计算运动参数的准确度，因为这种方法很容易受到噪音的影响。此外，还将尝试在机器人转动时减小摆动幅度，从而减少姿态误差。