基于传感器融合的同步定位与建图系统

邓 张，王吉芳，黄荣锐，马 飞

(北京信息科技大学 机电工程学院，北京 100192)

0 引言

随着同步定位与地图构建系统(simultaneous localization and mapping，SLAM)研究的不断深入，如何提高定位精度，获取更为准确的机器人位姿信息成为近几年来的热门研究问题。在单一传感器的同步定位与地图构建方法中，按传感器的种类不同可分为激光SLAM与视觉SLAM[1]。

其中激光SLAM最具代表性的是Gmapping方法[2]，该方法采用粒子滤波(RBPF)的方式完成二维栅格地图的构建，在室内环境下能获取较高精度的地图。但该方法在大场景中，构建地图所需的内存和计算量大，将产生粒子耗散的问题，所以不适合大场景构图。

视觉SLAM所使用的相机可分为单目、双目、深度相机(RGB-D)。使用深度相机的RGBD-SLAM[3]是视觉SLAM中具有代表性的一种。该方法从每一帧的颜色图像提取特征，计算描述符，利用随机抽样一致性算法与迭代最近点算法计算两帧之间的运动状态估计，并提出一个环境测量模型来判断运动状态估计是否可接受。在后端回环检测中，采用基于一般图形优化方法对位姿图进行优化，得到优化后的轨迹，用于建立地图。但该方法仅考虑了相机的运动情况，并没有考虑机器人在运动过程中产生的漂移与误差，导致机器人实际路径偏离理论值的现象。

对此，一些学者提出基于传感器融合的同步定位与地图构建方法。如Miguel-angel等[4]提出一种基于随机抽样一致性(RANSAC)的非线性光照测量方法，利用视觉里程计融合轮式里程计来估计移动平台的位姿。但该方法着眼于姿态的融合，利用轮式里程计对移动平台的运动状态进行预估，当移动平台发生漂移或者运动状态变化较大时，系统就会产生严重的误差。

为改善上述问题，本文提出一种基于传感器融合的同步定位与地图构建系统，选用深度相机Kinect2，利用SLAM算法，建立视觉里程计；加入IMU传感器，对移动平台的运动状态进行预估，建立惯性里程计；结合驱动电机编码器，建立轮式里程计，使用扩展卡尔曼滤波方法，进行信息融合。

1 三轮全向移动平台系统构建

1.1 系统构成

本文实验验证平台为自主搭建的三轮全向移动平台[5]，其模型如图1所示。

平台具有3层结构：底层、中间层和顶层。顶层安装有Kinect2传感器和平台显示器，Kinect2可获取深度、彩色和红外图像，最高可达33帧/s，可输出为二进制文件。中间层安装有移动平台的电源、上位机、下位机、开关和降压模块等设备。底层安装有IMU惯性传感器MPU6050[6]、移动平台的全向轮、电机和伺服电机驱动器。底层框架采用等三角形结构，能够有效减少搭建材料的使用，避免圆形的笨重结构，确保移动平台的运动性能。其系统结构如图2所示。

图2 移动平台系统结构

1.2 平台运动学模型

三轮全向移动平台的运动学模型如图3所示，XOY为世界坐标系，xoy为移动平台坐标系。

图3 移动平台运动学模型

本文实验在室内环境下进行，移动平台以前后移动为主。为在移动过程中尽量减少电机工作数量，设计移动平台坐标系，以轮1轴线与轮2轴线的中心线为x轴，与x轴垂直向左为y轴，其中L为移动平台的直径，则移动平台中心到3个全向轮与地面接触点之间的距离为L/2，R为驱动轮的直径，3个驱动轮的线速度分别为v1、v2、v3，移动平台相对于移动平台坐标系的线速度为vx、vy，自转角速度为ω，对轮1进行运动学分析如下：

1)分解vx：设v1与vx的夹角为β，将vx分解到v1上得到

v1x=vx×cosβ

(1)

2)分解vy：将vy分解到v1上得到

v1y=vy×sinβ

(2)

3)移动平台自身的角速度为ω，则自转引起的线速度作用在轮1上的速度为

v1ω=L/2×ω

(3)

4)最后将分解得到的速度v1x、v1y、v1ω进行合成，得到轮1的线速度为

v1=v1x+v1y+v1ω=

vx×cosβ+vy×sinβ+L/2×ω

(4)

对轮2、轮3进行与轮1同样的运动学分析后，得到三轮全向移动平台的运动学模型方程组：

(5)

2 传感器融合定位算法

2.1 惯性里程计构成

本文采用的惯性传感器主要由三轴加速度传感器与三轴陀螺仪组成，可以同时测量移动平台的加速度与角速度。但由于受重力加速度偏移量的影响[7]，三轴加速度传感器不能直接测得移动平台的运动加速度，故在测量三轴加速度时，需要计算重力加速度偏移量。

如图4所示，OXYZ为世界坐标系，OiXiYiZi为传感器参考坐标系，从世界坐标系到传感器参考坐标系可分解为3个旋转过程。

图4 MPU6050坐标转换

绕X轴旋转角度α的旋转矩阵Cx可以表示为

(6)

绕Y轴旋转角度γ的旋转矩阵Cy可以表示为

(7)

绕Z轴旋转角度θ的旋转矩阵Cz可以表示为

(8)

则惯性传感器从世界坐标系OXYZ到传感器坐标系OiXiYiZi的转换关系可以表示为

(9)

(10)

对式(10)进行逆向求解后得

(11)

(12)

在构建惯性里程计时，需要将传感器采集的三轴线性加速度减去同方向的重力加速度偏移量，再通过积分的方法求移动平台的运动轨迹。设移动平台在t时刻的加速度为a(t)，线速度为v(t)，位移为s(t)，其物理关系为

(13)

(14)

由于系统采样属于离散模型，设时间Δt为采样点n-1到采样点n的时间间隔，则对式(13)与式(14)进行离散化处理得到

(15)

(16)

设Δa(n)=a(n)-a(n-1)，联合式(15)与式(16)进行化简可得

s(n)=s(n-1)+

(17)

2.2 视觉里程计构成

本文采用Mathieu Labbé等[8]提出的一种基于实时外观的映射系统(real time appearance-based mapping，RTAB-MAP)进行视觉里程计的构建。该系统利用一个与时间和尺度无关的基于外观的定位与构图方案，通过优化闭环检测中定位点的“取回”与“转移”，从而减少闭环检测的运算量，加快相机获取的前一帧与后一帧的图像之间的匹配，增强系统的快速性。该系统的基本流程可以分为前端、后端、回环检测3个部分，其总体框架如图5所示。

图5 RTAB-MAP系统框图

RTAB-MAP系统中前端首先利用尺度不变的特征转换方法(scale-invariant feature transform，SIFT)，在空间尺度中寻找极值点，并提取出其位置、尺度、旋转不变量作为特征点；其次将获取的特征点利用随机抽样一致性算法(RANSAC)对Kinect2获取的图像信息进行相邻两帧间的匹配，以系统获取的第一帧为参考帧，并以参考帧为坐标系完成视觉里程计的构建；然后以RANSAC计算结果为初值，再调用一般图形优化方法(G2O)进行优化，以改善相机当前位姿估计算法(PNP)的结构，即为相机的位姿估计选取合适的特征点；最后将匹配到的特征点放到地图中匹配计算相机位姿，并通过当前采集的图像与地图中的特征点进行匹配，完成局部地图的构建。

后端主要通过前端传递的相机位姿估计信息进行处理，计算姿态变化与相机的运动状态的估计，得到相机的运动轨迹。

回环检测部分主要利用连续图像间特征点的匹配，判断移动机器人在不同帧的参考下，其位置变化是否一致，从而弱化里程计随时间变化的累计误差，使相机运动轨迹具有全局一致性。首先，通过前端的特征匹配，利用词袋法提取从相机取得的图像特征的视觉词汇向量，也就是图像中的特征点的名称；然后，将所有提取到的特征点向量集合到一块，利用均值聚类算法(K-Means)合并词义相近的视觉词汇，构造一个包含K种词汇的单词表；最后，统计单词表中每种单词在图像中出现的次数并进行排序，从而将图像表示成为一个K维的有序向量，并利用该向量以在线增量的形式创建定位点，即在系统中实时增加新的定位点并进行保存。

完成定位点的创建后，将新的定位点与存储在工作内存(WM)中的旧的定位点进行匹配，利用离散贝叶斯过滤器(Discrete-Bayes)来判断是否能够形成闭环。当检测到新、旧定位点之间有一定的概率形成闭环时，新、旧定位点就被匹配到一块。当地图中闭环检测的数目超过某个阈值时，就将工作内存中不太可能形成闭环的定位点从短期记忆模块(STM)中转移到长期记忆模块(LTM)中，以待下一次回环检测使用。最后利用构成闭环的定位点进行从局部到全局的图形优化，即将从前端获取的局部地图按构成闭环的定位点匹配，拼装成全局地图，其原理如图6所示。

当回环检测完成时，系统将对当前地图进行一次判断。如果相机视野范围内不再出现新的图像，则可以判断系统已经完成地图的构建，输出所建地图，并停止建图；如果相机的视野范围内出现新的图像，则可以判断系统还未完成地图的构建，并返回前端提取新的特征点，继续完成地图的构建，直到系统判断地图的构建完成为止。

2.3 算法框架

本文在RTAB-MAP算法的基础上，在后端的运动状态估计部分进行改进，利用机器人姿态扩展卡尔曼滤波功能包(robot_pose_ekf)，将视觉里程计、惯性里程计与轮式里程计的数据进行融合，来估计移动平台的真实位姿，包含如下几个步骤：

2)构建扩展卡尔曼滤波器：

(18)

3)通过扩展卡尔曼滤波，利用不同的里程计作为观测值，对系统进行滤波处理。预测过程如式(19)：

(19)

更新过程如式(20)：

(20)

式中：P为协方差；K为卡尔曼增益；Q为过程噪声；R为测量噪声；G为观测矩阵；I为单位矩阵。

图7 融合过程

5)发布更新后的状态量及协方差矩阵作为融合后的信息odom_combined。其算法框架如图8所示。

图8 算法框架

3 实验结果与分析

3.1 软件设计

本文软件的设计是基于ROS系统。通过在RTAB-MAP算法的基础上添加轮式里程计与惯性里程计的信息进行位姿的融合，ROS下对应的功能包和节点信息如表1所示。

表1 ROS下功能包

表1中，rosserial功能包主要负责上位机与下位机的通讯，交换运动控制话题cmd_vel与轮式里程计话题wheel_odom、惯性里程计话题imu_odom；rtab_map功能包负责将Kinect2的颜色信息kinect_rgb与深度信息kinect_depth经过前端处理后，发布视觉里程计信息vision_odom；robot_ekf功能包则将订阅的里程计信息进行融合并发布新的里程计信息odom_combined，完成传感器的信息融合。

3.2 实验结果和分析

本文通过控制移动平台做直线运动与圆周运动，对原算法与融合算法进行了比较和分析，其结果如图9所示，直线运动和圆周运动误差统计结果如表2所示。设定移动平台直线运动速度为0.7 m/s，圆周运动速度为0.4 m/s，通过分别计算视觉里程计、融合里程计与真实值的直线距离来分析误差。

图9 运动误差实验(样本数量=300)

表2 运动误差统计数据

实验结果表明，在移动平台直线运动与圆周运动实验过程中，融合算法较原算法所得运动误差明显减小，但是在圆周运动过程中，由于视觉里程计采集到的特征点数不够，导致位姿估计误差较大，融合效果也不如直线运动明显。

本文实验环境为5 m×6 m的实验室玄关，设定移动平台的运动轨迹，通过分析原算法与融合算法所得位姿估计结果，来判断改进算法的效果，实验结果如图10所示。

图10 实验结果

如图10(a)所示为利用RTAB-SLAM算法建立的点云地图，其中蓝色线为原算法中基于RGBD的位姿估计得到的运动轨迹，红色线为将原算法中RGBD信息提取出来后，与惯性里程计、轮式里程计融合得到的路径估计。从图中可以看出两种算法下，直线运动部分，视觉里程计得到的路径更趋近于直线，但由于移动平台运动过程中存在颠簸的情况，无法保证直线行进，故融合后的路径受其他传感器的影响，使其更接近于实际运动情况；在移动平台做旋转运动时，如图中黄色圈出部分，融合算法与原算法相比，漂移量明显减少。

如图10(b)所示为移动平台返回起始位置时的定位结果，其中标志odom为起始点参考坐标，标志kinect2_base_link为RGBD相机估计的移动平台位姿，标志base_footprint为融合后估计的移动平台位姿。从图中可以看出，融合后的定位结果更接近初始位置，该结果是移动平台位姿在做旋转运动时产生的累计误差导致的。

4 结束语

本文提出一种基于传感器融合的同步定位与地图构建方法，结合惯性传感器MPU6050、电机编码器进行位姿计算，对Kinect2获取的视觉里程计进行扩展卡尔曼滤波，实现运动估计优化。通过实验对改进前后的算法进行比较分析，相比于传统的基于RGBD的定位与建图算法，本文方法能够提高定位精度，更准确地追踪移动平台的位姿。