基于多信息融合的轮式机器人定位算法研究

沈念伟，于大泳

(上海理工大学机械工程学院，上海 200093)

17853589237@163.com;825298801@qq.com

1 引言(Introduction)

机器人在室内定位的过程中，位姿是极为重要的，精确地获取位姿能够大大减小导航的误差。机器人位姿主要依靠传感器去获取，但是单一传感器存在一定的缺陷，而多种传感器的信息融合却能够产生更加准确、稳定以及可靠的定位精度。目前用于定位的传感器的种类繁多，例如人们所熟知的GPS导航定位系统、激光雷达传感器、IMU传感器和里程计，等等。这些传感器都可以用于不同场景的导航，GPS依据天上的卫星系统来进行定位导航，在室外的定位效果最佳，但是在室内却差强人意，一般用于汽车、手机等产品上；激光雷达传感器是通过发射激光束获取点云数据来进行定位的，但是在一些较为窄长的通道地图中定位效果却不太理想；IMU是通过加速度计和陀螺仪来获取机器人的线速度和角速度进行定位的，其最大的优势是在短时内能够产生极佳的定位效果，但是随着时间的推移，IMU也会产生累积误差，之后的定位效果也不尽人意；里程计主要通过安装在车轮上的光电编码器来获取机器人的位姿信息，但是在机器人的运动过程中难免会出现车轮打滑的情况，其定位效果也会大打折扣。

综上所述，使用单一的传感器进行定位，肯定无法适应不同的场景，其定位效果会有欠缺，很难达到理想的状态，因此考虑使用多传感器的信息进行融合来提高机器人的定位精度。

2 传感器定位原理(Sensor positioning principle)

2.1 里程计定位原理

图1 里程计模型Fig.1 Odometer model

里程计传感器在车轮打滑的情况下会产生一定的定位误差，这样的误差会在很大程度上降低里程计的精度。因此，使用里程计时一般都会配备IMU传感器来进行校正，这样就可以很好地克服这个缺点。

2.2 IMU定位原理

IMU传感器主要是依靠陀螺仪和加速度计来进行定位的，首先陀螺仪传感器可以获得机器人的角速度数据，加速度计传感器可以获得机器人的加速度数据，对加速度数据进行两次积分后得到机器人的位移数据。

对于IMU传感器的姿态解算，目前常用且较为优秀的算法为方向余弦矩阵算法(Direction Cosine Matrix，DCM)。这种算法本质上是一种互补滤波算法，是一个用低通滤波器滤除高频噪声，用高通滤波器滤除低频噪声，分别把相对应测量中的噪声去除，然后融合两种信号的过程，如图2所示。

图2 互补滤波原理Fig.2 Principle of complementary filtering

其具体流程主要分三步进行：

(1)构建方向余弦矩阵方程。机器人在运动过程中，其在空间中的位姿是绕着、、轴运动所形成的，绕轴旋转的顺序不相同，所使用的分解方式也不相同。最常用的分解方法是欧拉角法，其原理是把机器人的位姿变换通过在、、轴的旋转顺序来表示，即：

图3 欧拉角示意图Fig.3 Euler angle diagram

(2)方向余弦矩阵对时间进行求导。在得到机器人的方向余弦矩阵之后，对其进行求导，可得到欧拉角的微分方程，即：

可以采用四阶龙格库塔法对欧拉角的微分方程进行求解，结果即为机器人的实时位姿。

2.3 激光雷达定位原理

对于激光雷达传感器的位姿解算算法而言，目前比较常用就是最近点搜索法(Iterative Closest Point，ICP)。ICP的基本原理及推导过程如下：

图4 ICP算法的匹配原理Fig.4 Matching principle of ICP algorithm

3 多传感器信息融合算法的设计(Design of multisensor information fusion algorithm)

通过第2部分的传感器定位原理可知，依靠单一的传感器进行定位时，机器人可能会存在比较大的定位误差，这些误差会导致机器人“迷失方向”。因此，本文提出一种多传感器信息融合的定位算法，其流程图如图5所示。

图5 多传感器信息融合定位流程图Fig.5 Flow chart of multi-sensor information fusion positioning

3.1 里程计融合器的研究设计

里程计融合器是通过扩展卡尔曼滤波算法融合里程计传感器和IMU传感器的信息。在运用扩展卡尔曼滤波过程中，用里程计的运动模型作为预测量，用IMU的数据作为观测量。根据公式(4)可得扩展卡尔曼滤波的系统状态方程：

对扩展卡尔曼滤波的系统状态方程进行泰勒展开的一阶线性化处理，其结果即为雅可比矩阵(Jacobian Matrix)：

根据公式(13)和公式(14)可以求出系统的观测方程：

3.2 激光雷达融合器的研究设计

由于激光雷达求解机器人位姿会因为没有精准的初始位姿导致定位失败，因此本文设计了激光雷达融合器，其最主要的作用是融合激光雷达的信息以及里程计融合器输出的位姿信息来对机器人进行位姿解算，信息融合的算法依旧是运用扩展卡尔曼滤波算法。

在激光雷达融合器的设计中，激光雷达的数据作为观测量，激光雷达的观测模型如公式(16)所示：

在第2部分中介绍了使用ICP算法进行激光雷达的定位，使用这种方法的优点就是定位精度高，但是该算法的使用比较依赖位姿的初始值，一旦初始值效果不理想，就容易导致算法陷入局部最优的情况，这样就会导致定位精度降低。激光雷达融合器的设计流程如下所示：

(1)里程计融合器的系统状态方程为：

(2)根据ICP算法，其算法的初始值是里程计融合器输出的位姿，其激光雷达的测量值为：

(4)跳转到第一步进行迭代，可以不断地输出激光雷达融合器的值。

3.3 加权融合器的研究设计

4 实验分析(Experimental analysis)

本课题为了验证多信息融合定位算法的优势，在上海理工大学综合楼进行了实验验证。实验环境如图6所示，机器人位姿解算的参考值是ROS系统中move_base功能包所发布的话题数据，通过Cartographer算法建立地图，所建地图如图7所示。

图6 实验的真实环境Fig.6 Real environment of experiment

图7 Cartographer建图Fig.7 Cartographer mapping

首先，在实验场景中标记一个初始点，记为坐标(0，0)，初始点位于机器人底盘的中心位置。以机器人正前方为轴，垂直轴方向为轴建立直角坐标系，在复杂的实验场景中随机设置四个路标，并且测量和记录机器人在坐标系中的位置。然后，在终端输入rosrun turtlesim turtle_teleop_key，操作机器人移动，机器人运动到路标位置时，计算机终端会输出机器人的位姿数据。在教室中随机设置的四个路标如图8所示。

图8 机器人在地图中的路标位置Fig.8 Robot＇s landmark location on the map

每次机器人从起始点移动到路标处后就记录下机器人的位姿信息，然后继续朝向下一个路标移动，直到返回初始点，更换路标坐标，重复三次实验。单独进行单一里程计定位、单一激光雷达定位及多传感器信息融合定位，误差结果如表1所示，根据误差结果的绝对值绘制误差折线图如图9所示。通过对比可知，多信息融合的定位误差最小，且相对于单一传感器，其平均定位精度提高了约60%。

表1 在复杂环境中机器人的定位误差Tab.1 Positioning error of robot in complex environment

图9 定位误差折线图Fig.9 Line chart of positioning error

5 结论(Conclusion)

机器人的定位问题一直是机器人导航研究的难点，仅靠单一的传感器无法达到较为理想的定位状态。合理利用里程计、IMU和激光雷达，通过对多传感器的信息进行融合，能够极大提高定位精度。通过实验结果可知，相对于单一传感器而言，多信息融合的定位精度提升了60%，从而使得后期机器人的导航精度也会有所提升。