龚学锐，闵华松

(武汉科技大学 信息科学与工程学院，武汉 430081)

0 引言

机器人的室内定位由于卫星导航失效，主要利用轮式里程计、惯性测量单元(inertial measurement unit,IMU)和视觉传感器进行定位。轮式里程计和惯性导航二者算法简单快速，但精度不高，容易受到环境的影响且存在严重的误差累积[1]。视觉里程计(visual odometry，V0)则通过安装在机器人上的摄像头采集的图像数据，利用连续的图像序列来估计机器人位姿。VO是视觉传感器定位的基础，如RGBD-SLAM-V2算法[2]和RTAB-MAP[3-4]。Raul M等提出比较完整的单目ORB-SLAM算法[5]之后又加入了对双目相机和RGB-D相机的支持[6]。ORB-SLAM相比于前两者在实时性与鲁棒性方面都兼备[7]，适用于移动机器人的室内高精度定位。然而当机器人运动速度较快时，视觉传感器数据采样率会低于移动速度，这会造成兴趣点数目减少而引起精度降低甚至跟踪失败[8]。针对这一问题，惯性传感器虽然存在严重的定位误差累计，但对于短期运动预测精度较好。两者特性形成互补，因此本文着重研究两者组合定位方法提高定位精度。

视觉和惯性的传感器融合根据融合方法分为紧耦合和松耦合两类[9]。紧耦合利用两者相关性将视觉与惯性直接融合。这种融合方法精度较高，但需要很高的计算量，在移动机器人需要进行很多任务规划的同时，严重影响了实时性。松耦合系统相对独立的处理视觉和惯性两个部分，在保持简单高效的同时Falquez J等人证明松耦合之后SLAM精度和速度相比于无IMU结构都更好[10]。在文献[11]中提出了一个基于扩展卡尔曼滤波器(Extended Kalman Filter，EKF)的松耦合算法, 将视觉里程计作为独立部分处理后和惯性测量的结果进行融合, 并成功与PTAM算法结合完成机器人定位。这种方法计算量较小，能应用于不同方法的视觉里程计算法，可移植性较强。

因此，本文将惯性导航与视觉SLAM两种定位方法在基于EKF的松耦合结构下相互融合，构建具有较高定位精度的定位系统。采用ORB-SLAM2算法，使用Kinect作为视觉传感器与IMU进行组合定位与制图研究。首先完成了SLAM系统与机器人坐标系的构建，通过ORB-SLAM2的视觉里程计得到Kinect的位姿，从而计算出移动机器人的运动轨迹。并将视觉定位结果与IMU数据通过扩展卡尔曼滤波器进行融合，提高室内移动机器人的定位精度。

1 基于Kinect的视觉里程计定位

1.1 ORB-SLAM2算法

ORB-SLAM2是基于单目，双目和RGB-D相机的一套完整的SLAM方案。它能够实现地图重用，回环检测和重新定位的功能。无论是在室内还是室外都能够在标准的CPU上进行实时工作。对于处理来自RGB-D传感器的图像，ORB-SLAM2后端采用光速法平差优化法来建立全局地图，能够长时间并且精确的完成全局定位。

ORB-SLAM2的视觉里程计主要包含跟踪和局部地图构建两个线程，其中相机位姿的确定主要由跟踪部分负责，这一部分任务是通过对新一帧图像提取ORB特征点进行粗略估计相机初始位姿并对局部空间中的特征点以及相机位姿进行优化，通过跟踪局部地图，利用投影的方法和当前帧关联做联合优化来得到较为准确的相机位姿。其系统流程如图1所示。

图1 ORB-SLAM2系统流程图

1.2 机器人定位

本文用6个参数来表示机器人的位移和姿态变化，Δx，Δy，Δz代表机器人位置的变化，Δα，Δβ，Δγ表示机器人姿态变化。当机器人从点A[x1,y1,z1]移动到点B[x2,y2,z2]时，通过姿态变化可以得到一个3×3的旋转矩阵R，通过位置变化得到平移向量t=[Δx,Δy,Δz]，通过式(1)可以计算得到点B坐标。

[x2,y2,z2]T=R[x1,y1,z1]T+t

(1)

ORB-SLAM2可以准确的估计相机的位移和姿态变化，由于在机器人移动过程中相机位置相对于机器人本体保持不变，因此相机的位姿变换就能转换成机器人的位姿变换。假如每两个相邻的机器人运动都能够精确地确定唯一的姿态变换，那么移动机器人的定位问题就解决了。

2 视觉惯性组合定位

单纯的视觉导航在长时间运行过程中会产生累计的误差，并且在机器人运动速度过快时效果不佳，所以需要引入其他导航系统进行修正。由于GPS信号在室内失效无法定位，因此本文采用视觉惯性组合的方法来减少视觉传感器产生的误差，使移动机器人在室内的定位精度得到提高。

2.1 机器人坐标系构建

ORB-SLAM2的视觉里程计的直接测量值是针对自身坐标系map，与机器人坐标系并不产生任何关联。本文希望将视觉里程计的map坐标系融入机器人坐标系中，因此在系统建模过程中需要通过坐标变换来实现。

ORB-SLAM2输出的相机位姿是在自身map坐标系下的相机位姿，即ORB-SLAM2输出map-camera_rgb_optical_frame转换关系，而机器人坐标系的转换关系依据自身模型建立，输出形式为odom-base_link-others(camera_rgb_optical_frame等)，为使得SLAM系统与机器人坐标系建立关系，根据以上两个关系的融合，建立map-odom转换关系。我们可以得到已建立连接的两个坐标系之间的旋转和平移关系，因此设R为坐标系间的旋转矩阵，T为平移矩阵，P为相机在该坐标系下的坐标。

Rmap_camera=Rmap_odom*Rodom_camera

(2)

Rmap—odom=Rmap—camera*Rodom—camera- 1

(3)

Pmap=Rmap_odom*Podom+Tmap_odom

(4)

Pmap=Tmap_camera

(5)

Podom=Todom_camera

(6)

联立上式得:

Tmap_odom=Tmap_camera-Rmap_odom*Todom_camera

(7)

根据上面的推导过程可以得出map-odom的旋转关系和平移关系，建立ORB-SLAM2与机器人的坐标系关联，便于查询包括imu、kinect、机器人等在整个框架下的坐标。

2.2 组合定位导航方案

本文将ORB-SLAM2的视觉里程计与IMU数据在扩展卡尔曼滤波器中进行融合，形成松耦合的VIO框架，模块化的松耦合框架能够使整个视觉惯性系统在机器人运行中无缝的处理新增或丢失的传感器消息而不会导致整个系统的崩溃，使得机器人长期的任务中不会由于短暂的跟踪失败或IMU数据丢失导致对自身定位出现较大的飘移。

扩展卡尔曼滤波器是一种高效率的递归滤波器，它能够从一系列的不完全包含噪声的测量中，估计动态系统的状态。由于其算法众所周知，在这里只详细的介绍本系统的实施步骤。整个EKF框架大体分为预测和更新两大步骤。使用IMU数据通过运动模型进行预测步骤然后再由SLAM输出的传感器位姿进行更新步骤。我们的目标是估计一个移动机器人的全三维姿态和速度。随着时间的推移，这个过程可以被描述为一个非线性的动态系统。

xk=f(xk-1)+wk-1

(8)

其中，x为机器人的系统状态，它包括机器人的3D位姿以及各自的速度，k表示所处时刻，f是一个非线性状态转移函数，w为假设为正态分布的过程噪声。另外测量数据我们表示为：

zk=h(xk)+vk

(9)

其中，z为测量量，h是一种将状态映射到测量空间的非线性传感器模型。v是正态分布的测量噪声。

算法中的第一阶段，如等式(10)和式(11)所示，是执行预测当前时间状态估计和误差协方差时间的预测步骤，它主要是基于系统模型和IMU读数完成:

(10)

(11)

其中，f是从牛顿力学导出的标准三维运动模型，u为IMU测量数据，P为估计协方差误差，F为f的雅可比矩阵，Q为扰动变量，是过程噪声协方差。

之后通过式(12)～式(14)执行更新步骤，利用观测矩阵H和测量协方差R和估计协方差误差P来计算卡尔曼增益用于更新状态和协方差矩阵。

(12)

(13)

(14)

考虑到室内移动机器人的特性，由于机器人本体在地面只会在x-y平面内进行运动，Z方向的位姿估计只会对定位精度造成干扰，因此在融合过程中忽略各部分在Z轴方向的估计。在机器人平台上，将视觉里程计作为一盒黑匣子，独立的计算位姿并输出。扩展卡尔曼滤波器接受视觉里程计与惯性传感器的数据后，完成融合输出新的位姿。算法流程如图2所示。

图2 算法流程图

本文基于ROS操作系统建立各模块之间的通讯，节点框架如图3所示。在整个算法运行过程中，各部分通过TF建立坐标系之间的转换关系。EKF节点订阅视觉里程计与惯性传感器发布的话题，在处理后进行位姿融合。

图3 ROS节点框架图

3 室内定位实验

本实验所用电脑配置为：CPU为i5-3210m，主频为2.5GHz，内存8G，不使用图形处理器(GPU)加速，系统为Ubuntu14.04，ROS版本为indigo。使用TUM数据集将本文算法与ORB-SLAM2及RGBD-SLAM-V2进行对比。在实际测试中将KInect搭载在移动机器人上进行定位实验，在ROS仿真平台gazebo中完成仿真环境测试。

图4 实验所用机器人平台

表1为在本文所用电脑上TUM数据集中测试结果，ORB-SLAM2的测试结果各方面均优于RGBD-SLAM-V2。在TUM数据集中由于没有惯性数据，但本文算法也优于RGBD-SLAM-V2，与ORB-SLAM2相差不大。

表1 fr1/desk数据集测试结果(单位：m)

通过gazebo搭建机器人的仿真环境，在移动机器人平台上搭载Kinect对本系统进行仿真测试。

图5 仿真环境运行界面

表2～表4分别为机器人直线前进1m、2m、3m时本文算法与ORB-SLAM2和轮式里程计与IMU融合的机器人坐标与误差。为消除单次实验的偶然性，本文共做3次实验取平均值，其中实验一与实验二机器人速度设定为0.5m/s，实验三中机器人速度设定为1m/s。在直线定位中可以看出单纯的视觉里程计定位由于Z方向上带来的误差，随着距离的增加与目标点偏差越来越大，传统的轮式里程计与惯性定位表现较为稳定但误差仍然较大。而本文改进的视觉惯性系统定位精度高于ORB-SLAM2。证明了视觉惯性的松耦合系统的可行性，展现了两者传感器之间互补的良好特性。

表2 直线行走1m实验结果 (单位：m)

表3 直线行走2m实验结果 (单位：m)

图6与图7分别为机器人行走3m×3m正方形的轨迹测试。由于在水平地面移动，因此只考虑X-Y平面坐标，使用轮式里程计与IMU时，机器人最后位置为(0.0515,0.1374)，与初始点相差0.1467m，在单独运行ORB-SLAM2时机器人最后位置为(0.0214,0.1051)，与初始点相差0.1072m。运行视觉惯性时，最后机器人位置为(0.0177,0.0674)，与初始点相差0.0696m。

图6 传统室内定位轨迹比较(单位：m)

图7 VIORB-SLAM2和视觉里程计定位轨迹比较(单位：m)

图8、图9与表5、表6为机器人行走一段开环路线与闭环路线的轨迹输出及误差分析。上述两个实验表明无论是在开环路径或闭环路径下，在姿态变化很大时(连续转弯)融合之后的定位精度及稳定性均明显优于轮式里程计和ORB-SLAM2，与理论轨迹相差不大。

(a) ORB-SLAM2 (b) VIORB-SLAM2

(c) 轮式里程计

ORB-SLAM2VIORB-SLAM2轮式里程计rmse0.0162890.0099880.021186mean0.0094680.0057450.018629median0.0049370.0021630.017312std0.0132550.0081710.010089min0.0001490.0003770.036750max0.1815120.0794060.522340

(a) ORB-SLAM2 (b) VIORB-SLAM2

(c) 轮式里程计

ORB-SLAM2VIORB-SLAM2轮式里程计rmse0.0804380.0124620.855962mean0.0626030.0072730.762932median0.0517110.0032290.820976std0.0505080.0101190.388078min0.0009680.0002870.188909max0.1663500.0774371.671109

4 结论

本文针对视觉里程计与传统室内定位存在的精度不高和累计误差较大的问题，提出了一种基于Kinect和IMU的机器人室内定位算法，由于Kinect兼备了RGB摄像头与深度传感器的优点，因此本文方法性价比高，可移植性较强。ORB-SLAM2作为视觉里程计与IMU进行扩展卡尔曼滤波组合定位通过更新修正机器人轨迹降低了视觉传感器的累积误差从而提高了室内机器人定位精度。并设计实验对所提出算法进行了验证。实验结果表明在定位精度与累计误差方面均有所改进。

本文目前提出的定位算法在视觉里程计或惯性传感器失效的情况下仍有较大误差，且在转动速度过快时产生的误差也比较大。因此下一步将开展深度组合算法研究，进一步提高精度与鲁棒性，并生成地图应用于实际导航中。