孙希君 王秋滢 王水根 吴应为

摘  要：精确鲁棒的定位系统是保证室内巡检机器人正常工作的重要基础。文章基于机器人操作系统（ROS），针对现有公开视觉算法ORB-SLAM2在低性能计算平台上因计算能力不足导致的特征跟踪丢失问题，提出一种将ORB-SLAM2与惯性导航系统（Inertial Navigation System， INS）解算误差进行卡尔曼滤波融合的方法。经公开数据集验证表明，该方法能够完整地估计出视觉失效时丢失的位姿信息，与ORB-SLAM2相比，定位系统的精度与鲁棒性有效提高。

关键词：ROS;定位;卡尔曼滤波;ORB-SLAM2

中图分类号：TP242    文献标识码：A文章编号：2096-4706（2021）13-0139-06

Visual Inertial Fusion Positioning Method Based on ROS Indoor Inspection Robot

SUN Xijun1， WANG Qiuying2，3， WANG Shuigen4， WU Yingwei1

（1.College of Information and Communication Engineering， Harbin Engineering University， Harbin  150001， China; 2.College of Underwater Acoustic Engineering， Harbin Engineering University， Harbin  150001， China; 3.Key Laboratory of Underwater Acoustic Technology， Harbin Engineering University， Harbin  150001， China; 4.Yantai Iray Technology Co.， Ltd.， Yantai  264006， China）

Abstract： Accurate and robust positioning system is an important basis to ensure the normal operation of indoor inspection robot. Based on the robot operating system （ROS）， aiming at the loss of feature tracking caused by the insufficient computing power of the existing public vision algorithm ORB-SLAM2 on the low-performance computing platform， this paper proposes a method for Kalman filter fusion in the solution errors of ORB-SLAM2 and inertial navigation system （INS）. The verification of public data sets shows that this method can completely estimate the pose information lost when visual failure. Compared with ORB-SLAM2， the accuracy and robustness of the positioning system are effectively improved.

Keywords： ROS; positioning; Kalman filter; ORB-SLAM2

0  引  言

在化學工业工厂、电力设备厂房、电力隧道、物流仓储库房等室内高危作业环境下，配备24小时持续巡检机器人可有效提升作业人员的工作效率，并在极大程度上保证员工安全问题。确保巡检机器人正常工作的关键前提是机器人自身可以实现高精度、高可靠性的实时自主定位。

常见的用于单一定位系统的传感器主要有卫星接收机、激光雷达、视觉相机、惯性测量单元（Inertial Measurement Unit， IMU）由于建筑物遮挡等影响室内机器人无法通过卫星信号进行精准定位，激光雷达的价格相对昂贵，而视觉相机与IMU的成本相对低廉、且无须外界提供任何信息便可进行独立自主定位，因此利用视觉相机与IMU进行运载体的定位逐渐引起学术研究者的广泛关注[1]。

现有的纯视觉算法都存在着易受光照、相机快速运动等问题的影响，基于直接法的DSO（Direct Sparse Odometry）算法[2]在计算速度上相对较快，但是对光照尤其敏感，在实际场景中很难保持稳定的曝光时间以至于系统经常无法正常工作;基于特征点法的Orb-slam算法[3]在相机平稳运动时定位精度较高，但在相机快移动或光照突然变化等复杂动态场景下会出现跟踪丢失[4]无法检测特征点的问题。本文基于ROS（Robot Operating System）开发平台，验证分析了ORB-SLAM2算法的实际效果，并针对算法出现的问题提出一种与IMU误差状态进行卡尔曼滤波融合的方法来对视觉信息进行补偿修正，解决了复杂场景下纯视觉算法跟踪丢失的问题，提高了系统的定位精度与鲁棒性。

1  ORB-SLAM2视觉算法

ORB-SLAM2算法是目前非常完善的主流视觉SLAM算法之一，它通过提取并匹配图像中的ORB特征来逐帧估计相机的位置与姿态。整个算法由三个线程分别进行，跟踪线程负责ORB特征的提取与相机位姿估计并确定是否需要引入关键帧，局部建图线程负责处理新的关键帧并完成局部地图的创建，采用局部束调整的方法来筛选去除多余的关键帧，对局部空间内的特征点与相机位姿进行求解，闭环检测部分则对全局的地图与关键帧进行检测与校正以消除累积误差。其最大的优点是在于它在原有的视觉里程计基础上增加了闭环检测线程，保证了相机轨迹与地图的全局一致性，一定程度上避免了累积误差对系统带来的影响。ORB-SLAM2算法的整体流程图如图1所示。

但ORB-SLAM2算法采用的三线程结构也对计算机的CPU负荷能力有更高的要求，当相机快速运动引起图像模糊或者光照强度突变时，在性能较低的计算机或嵌入式系统上将无法正常工作，会出现特征点跟踪丢失的问题。因此，本文在原有的ORB-SLAM2算法基础上做了改进，将其与惯性导航误差状态做卡尔曼滤波融合，用惯性导航解算的误差修正ORB-SLAM2算法在特征跟踪丢失时的位姿误差，并将误差反馈到子系统中进行迭代修正，使其可以在低性能计算平台上更稳定地自主工作。

2  滤波融合定位算法

多传感器融合主要包括紧耦合和松耦合2种基本框架。松耦合是指各子系统分别解算后再融合，紧耦合则是在系统前端将各子系统的测量参数均加入模型中，状态量维度的提高使得紧耦合算法的计算量也更大，为不再额外增加平台的计算负担且便于后续研究中视觉算法的轻量化改进，本文采用松耦合框架来构建模型实现视觉惯性的数据融合。

2.1  构建IMU误差状态模型

本文利用IMU自身的传感器误差及解算得到的导航信息的误差作为状态量进行建模，微小量误差相对于直接将导航信息作为状态量的方法在线性化过程[5]中得到的结果更为精确。

IMU测量获取到的原始数据的计算公式为：

ai=Riw（i）（aw-gw）+ab+an                            （1）

ωi=ωw+ωb+ωn                                 （2）

其中，ai、aw、ab、an分别表示加速度计在传感器坐标系下测得的带误差的实际加速度、世界坐标系下不带噪声的真实加速度、加速度计自身零偏、测量过程中加速度计产生的随机噪声，gw表示重力向量，Riw（i）表示在惯性导航模块中定义的世界坐标系到IMU坐标系的旋转矩阵，本文中直接将IMU坐标系作为世界坐标系，ωi、ωw、ωb、ωn分别表示陀螺仪在传感器坐标系下测得的带误差的实际角速度、世界坐标系下不带误差的真实角速度、陀螺仪自身零偏以及测量过程中陀螺仪产生的随机噪声。

惯性导航解算中对加速度进行一次积分获取速度信息，再进行二次积分获取位置信息，本文为避免欧拉角万向锁的影响，采用汉密尔顿表示法的四元数来描述姿态信息[6]。实际场景中自主运动机器人的运动学模型会受随机误差及IMU内部噪声的影响，本文假设两种噪声均为高斯白噪声。结合上述IMU测量的计算公式可推导出真实状态下的运动学微分方程为：

Pw（i）=vw（i）

vw（i）=aw=Riw（i）T（ai-ab-an）+gw

qw（i）=qw（i）ωw=qw（i）（ωi-ωb-ωn）

ab=aε

ωb=ωε                                                                  （3）

其中pw（i）、vw（i）、aw、ωw分别表示机器人在世界坐标系下的位置、速度、加速度与角速度信息，qw（i）表示机器人在三维空间中的方向信息，ab、ωb分别表示惯性测量单元加速度计与陀螺仪的零偏误差，aε、ωε分别表示加速度计与陀螺仪的随机游走噪声，该世界坐标系在本文中即为IMU坐标系。

理想状态下的运动学模型是在假设不存在外界随机误差及传感器内部偏差的情况下建立的，忽略掉噪声影响，理想的标称运动学微分方程为：

Ps=vs

vs=as=Riw（i）T（ai-ab）+gs

qs=qsωs=qs（ωi-ωb）

abs=0

ωbs=0                                                           （4）

其中ps、vs、qs分別表示机器人在世界坐标系下的理想位置、速度、四元数姿态信息，as、ωs分别表示不考虑零偏误差的加速度、角速度信息，gs表示理想重力加速度，abs、ωbs分别表示理想情况下的加速度计与陀螺仪零偏误差。

将真实状态运动学方程与理想状态运动学方程联立作差，便可推算出IMU解算信息误差以及传感器内部随机游走误差的运动学模型：

δp=δv

δv=-Riw（i）T[ai-ab]×δθ-Riw（i）Tδab+δg-Riw（i）Tan

δθ=-[ωi-ωb]×δθ-δωb-ωn

δab=aε

δωb=ωε                                                              （5）

其中，δp=pw（i）-ps表示位置误差，δv=vw（i）-vs表示速度误差，δθ表示旋转的角度矢量，由qw（i）=qseδθ/2计算得到，δab=ab-abs表示加速度计误差，δωb=ωb-ωbs表示陀螺仪误差，δg=gw-gs表示重力加速度误差，本文将其忽略不计，即取δg=0，[·]×表示反对称矩阵。

2.2  创建观测模型

ORB-SLAM2利用采集到的图像数据可逐帧估计出相机的位姿信息，在ORB-SLAM2中位姿信息由跟踪线程中的函数返回，在此基础上，本文用4×4的刚体变换矩阵Tcw（c）来描述ORB-SLAM2算法返回的相机姿态，其公式为：

（6）

其中3×3矩阵Rcw（c）表示相机在世界坐标系中（相机的初始帧坐标系记为相机当前对应的世界坐标系）由初始帧到当前帧姿态的旋转矩阵，三维向量tcw（c）是用来描述相机在该世界坐标系下的位置的平移向量，该矩阵描述的是相机坐标系到世界坐标系的变换关系。对该矩阵求逆即可得到相机的外参矩阵，由于旋转矩阵为正交阵，故可推导出转换公式为：

（7）

由旋转矩阵和平移向量可以推导出相机的三维位置和姿态四元数信息，位置计算公式为：

pcx=tcw（c）（1，1）

pcy=tcw（c）（2，1）                                      （8）

pcz=tcw（c）（3，1）

其中pc=[pcx，pcy，pcz]T表示相机测量到的相机坐标系下的位置信息。

测量误差为观测值与理想预测值的残差，本文中预测值通过IMU解算得出，因此需要将相机坐标系下的位姿信息转换到IMU坐标系下进行空间同步，相机坐标系到IMU坐标系的转换关系可通过标定得出，记旋转关系为Ric，平移关系为pic，可推导出相机在IMU坐标系下的位置：

（9）

其中pi=[pix，piy，piz]T表示相机测量到的在IMU坐标系下的位置信息，三个坐标系的空间变换过程为：

Riw（c）=RicRcw（c）                               （10）

其中Riw（c）表示相机从相机的世界坐标系到IMU坐标系的旋转矩阵，有qi=[qiw，qix，qiy，qiz]T，qiw为实部，则相机由世界坐标系到IMU坐标系下的姿态四元数信息转换公式为：

（11）

由上述两组公式可以构建出3维位置观测误差模型：

δpi=pi-ps                                        （12）

通过以下公式可以推导出4维旋转矢量角模型：

（13）

2.3  濾波融合定位算法

在数据融合过程中将所有需要估计的信息列入状态量中，可用来进行辅助修正的信息列为观测量，本文中将IMU的解算误差及传感器内部随机游走误差作为状态量，记X=[δp  δv δθ  δab  δωb]T，观测量为已与IMU进行空间同步的视觉模块观测到的位姿误差，记Z=[δpi  δθi]T。融合定位算法的总体流程如图2所示。

以惯导系统的预测误差作为状态量，视觉解算的误差作为观测量，进行卡尔曼滤波融合过程，当ORB-SLAM2算法可以正常工作时相信观测模型，在视觉特征跟踪丢失时相信预测模型，为使IMU的预测精度更高，在每一次迭代后将更新得到的估计误差控制量反馈到惯导系统内部对状态量进行校正，记更新后的估计误差为δx=[δp    δv     δθ     δab        δωb] T，通过以下公式可计算得到经过校正的信息：

（14）

其中p为定位系统估计的新位置信息，q为新姿态四元数信息。

3  验证及分析

该融合算法完全基于kinetic版本的ROS开发测试，实验环境为Ubuntu16.04系统，CPU为英特尔i5-8300H。为验证分析本文融合定位算法的有效性，本文采用苏黎世联邦理工学院ETH公开的室内视觉惯性数据集对ORB-SLAM2算法和改进后的融合算法性能进行对比测试。该数据集的信息由搭载了双目VIO相机和运动捕捉系统的微型飞行器来采集，数据集中包含双目图像、已进行时间同步的IMU测量值以及精确的地面真值，其传感器的装配坐标系如图3所示。

由于IMU安装在飞行器的正中心，IMU坐标系与飞行器的本体坐标系重合，因此，本文将视觉估计信息转换到IMU坐标系下进行解算得到的位姿即机器人的实际位姿。

对ORB-SLAM2算法进行验证，在飞行器运动过程中，相机在正常工作状态下ORB特征点跟踪情况如图4所示，当相机受到光照变化等环境因素影响时，跟踪线程无法正常工作，如图5所示部分出现特征点跟踪丢失的问题。

ORB-SLAM2算法的三线程使其对运行平台的计算能力要求较高，因此就出现了上述跟踪丢失的状况。将其解算得到的位姿信息、本文的融合位姿信息与运动捕捉系统采集到的基准值作对比，二维位置信息对比如图6所示，横滚、俯仰、偏航三个方向的姿态角信息对比如图7所示。

由图6与图7可以看出视觉算法定位过程中出现两次特征跟踪丢失的状况，ORB-SLAM2在本实验平台上的鲁棒性较弱，当CPU负担较大时，跟踪线程无法正常工作，就会出现如图中的丢失状况，定位精度大大降低。而本文的算法解决了视觉跟踪丢失的问题，在跟踪丢失时以IMU的估计值去校正结果，经过校正的估值更趋近于基准值。

图8与图9分别将ORB-SLAM2算法和本文融合算法估计的二维轨迹与真实轨迹进行对比，两图中灰色虚线均表示地面真实参考值，彩色线分别表示ORBSLAM2的轨迹与本文融合算法的轨迹，由蓝到红的颜色过渡条表示轨迹绝对误差的大小。由图可以更直观地看出跟踪丢失路段的估值已得到较好地修正，本文算法估计的整体轨迹更靠近真实轨迹。

采用计算绝对位姿误差的方式来验证轨迹的全局一致性，对两种算法的定位误差进行评定及分析，定位误差如图10、图11所示，ORB-SLAM2算法的定位误差在跟踪丢失时最大误差达3.22 m，整个运动过程中最小误差0.13 m，均值0.78 m，均方根误差1.0 m，利用本文的方法进行融合校正之后跟踪丢失问题得到有效解决，定位误差最大为0.71 m，最小误差0.003 m，均值0.08 m，均方根误差0.11 m，各项误差参数均表明，本文提出的算法定位精度有效提升，方法具有可行性。

4  结  论

本文提出了一种基于ROS开发的视觉惯性融合定位算法，解决了ORB-SLAM2算法在低性能计算平台上因计算能力不足出现的ORB特征跟踪丢失问题。本文首先建立了IMU误差模型与视觉观测误差模型，并将视觉信息与IMU信息进行空间同步，最后利用卡尔曼滤波估计出位姿误差，将误差反馈到系统中进行位姿更新。经数据集验证分析表明，本文提出的融合算法具有可行性，在定位精度和鲁棒性上均有效提高。未来的工作包括：轻量化视觉部分，减轻CPU计算负担，从根本上提高系统的可靠性。

参考文献：

[1] 杨观赐，王霄远，蒋亚汶，等.视觉与惯性传感器融合的SLAM技術综述 [J].贵州大学学报（自然科学版），2020，37（6）：1-12.

[2] ENGEL J，KOLTUN V，CREMERS D. Direct Sparse Odometry [J].IEEE Transactions on Pattern Analysis and Machine Intelligence，2018，40（3）：611-625.

[3] MUR-ARTAL R，TARDÓS J D. ORB-SLAM2：AnOpen-Source SLAM System for Monocular，Stereo，and RGB-D Cameras [J].IEEE Transactions on Robotics，2017，33（5）：1255-1262.

[4] 黄剑雄，刘小雄，章卫国，等.基于视觉/惯导的无人机组合导航算法研究 [J].计算机测量与控制，2021，29（2）：137-143+149.

[5] 杜小菁，陈洪，王欣，等.基于SINS解算的行人导航技术发展综述 [J].战术导弹技术，2020（1）：113-120.

[6] 孟春见，李开龙，张梦得.捷联惯性基组合导航滤波算法比较研究 [J].电光与控制，2020，27（2）：18-21.

作者简介：孙希君（1997—），女，汉族，山东武城人，硕士研究生在读，研究方向：传感器融合定位、组合导航。