基于磁力计、IMU和单目视觉的自主定位方法

樊建彪，孙 剑，樊欢欢,陈 伟

(1.西安交通大学航天航空学院，西安 710049；2.西安交通大学机械结构强度与振动国家重点实验室，西安 710049)

0 引言

近年来，随着社会发展和科技技术的进步，移动机器人越来越多地应用于日常生活中。导航技术不仅是移动机器人的核心技术，同时也是移动机器人自主完成任务的基础。目前移动机器人的导航系统主要依靠GPS(全球定位系统)与 IMU进行融合解算位姿信息，而对于复杂环境此方案无法保证GPS信号的实时性，同时IMU模块的零偏会造成解算的位姿漂移，因此应用场景有限。

SLAM(simultaneous localization and mapping)技术依靠移动机器人自身携带的传感器对周围环境进行观测，同时通过融合算法对各传感器的数据进行处理，可以实时得到移动机器人在空间中的位姿，不依赖于外部传感器信息，是实现机器人在复杂环境下自主定位导航的关键技术。SLAM技术于20世纪80年代被提出，经过几十年的发展，已经产生多种成熟的SLAM算法。

目前主要的SLAM算法包括单目视觉SLAM[1-5]、双目视觉SLAM[6-8]、RGB-D视觉SLAM[9]和激光雷达SLAM[10]。单目视觉SLAM系统仅需一台摄像头和一套低成本IMU，其体积小、成本低、功耗小等优点引起了学术界和工业界的极大兴趣。自从基于扩展卡尔曼滤波的Mono-SLAM[11]算法被提出以后，单目视觉逐渐流行起来。目前有众多的单目视觉解决方案，包括PTAM[12]、SVO[13]、ORB-SLAM2[14]、MSCKF[15]和VINS-SLAM[16]等。这些方案主要是基于滤波器或者优化器来融合IMU和视觉传感器数据对位姿进行解算。而目前的单目视觉SLAM算法主要是依靠IMU数据来解决单目视觉解算过程中的尺度问题，但IMU存在漂移，其尺度解算精度不高。同时单目视觉SLAM算法主要依靠单目视觉数据进行优化得到位姿信息，但是当运动为纯旋转时，则无法通过求解对极几何来求解机器人的姿态角。该算法在实际应用中存在位姿解算漂移的问题，同时解算精度不高。

通过将三维磁阻传感器按照载体坐标系安装，磁力计可测量载体空间磁场的三维磁感应强度，据此可以计算出载体的姿态角。因此本文首次提出了基于磁力计、IMU和单目视觉传感器融合的位姿解算方法，用来对视觉尺度因子、重力、速度、IMU参数和载体姿态进行估计，解决单目视觉在解算位姿时由于纯旋转情况而出现的不稳定情况，同时本方案具有高精确和强鲁棒的特性。

1 算法设计

本文通过融合磁力计、IMU和单目视觉传感器数据，设计能够在三维空间中实时估计移动机器人位姿的算法。所设计的扩展卡尔曼滤波器将磁力计和IMU数据进行融合，解算出机器人的姿态并对陀螺仪零偏进行了估计，之后用陀螺仪的零偏估计量修正陀螺仪数据，同时将估计的姿态量用于加速度计的积分，从而得到机器人的位置量。通过单目视觉传感器解算对极几何和PnP计算位置信息。通过图优化算法融合IMU计算的位置残差和单目视觉计算的残差，得到移动机器人的位置状态和加速度计的零偏量。用计算得到的加速度计的零偏修正加速度计数据。至此就可以估计出移动机器人的位置和姿态状态。算法流程如图1所示。

图1 算法流程图

图1中，bw和ba分别表示通过算法估计出来的陀螺仪零偏量和加速度计零偏量。通过对IMU中各传感器的零偏进行修正，可以得到更加精确的位姿信息并提高系统的鲁棒性。

1.1 磁力计与IMU卡尔曼姿态估计器

1.1.1 IMU传感器特性

IMU传感器包括三轴陀螺仪和三轴加速度计，可以测量高频的角速度和线加速度数据。但是，IMU的测量数据中包含两种测量误差，即测量噪声和零偏。测量噪声是高斯白噪声，零偏服从高斯随机游走。因此IMU的测量模型如式(1)：

(1)

1.1.2 基于卡尔曼滤波器的姿态解算算法

建立基于非线性卡尔曼滤波器的姿态解算算法，其状态方程和量测方程[17]如下

Z=h(X)+V

(2)

式中，状态向量X=[q0，q1，q2，q3，bwx，bwy，bwz]，分别表示四元数和陀螺仪的偏差，W为系统噪声，服从高斯分布；V为测量噪声，服从高斯分布，四元数微分方程为

对上式进行离散化可以得到如下状态转移方程

XK+1=AKXK

(3)

式中

AK=(I+TF)=I+

XK(i)=qi,i=0,1,2,3。T为IMU传感器数据采集时间间隔。

对于加速度的测量数据可以通过方向余弦矩阵进行估计，其中假设重力加速度为常数|g|，可以根据式(4)得到加速度计在载体坐标下的测量方程。

(4)

式中

(5)

通过视觉和IMU优化模块中式(17)可以得到优化后加速度计的零偏估计并对加速度计进行修正，因此修正后的测量值为

磁力计的测量方程与加速度计测量方程相同。对磁场进行归一化，并假设其指向y轴。磁力计的测量方程见式(6)

(6)

根据式(2)～(6)所得到的状态方程和测量方程进行卡尔曼更新，求解后验状态估计和后验状态协方差矩阵见式(7)：

(7)

1.2 基于视觉传感器和IMU的非线性图优化

1.2.1 IMU测量残差

首先对IMU的测量数据进行积分，为IMU的残差计算提供约束关系。在给定的两帧连续图像bk和bk+1之间，可以通过加速度计、陀螺仪的测量数据和式(7)中解算的姿态角和陀螺仪偏差估计值计算bk+1帧时刻的位置，速度和姿态角信息，见式(8)[16]：

(8)

(9)

式中

(10)

对于式(10)采用欧拉法进行离散处理求解积分值，同时进行泰勒一阶近似。就可以计算得到bk和bk+1之间的位移、速度和姿态角的增量，如式(11)所示：

(11)

对于摄像机视频窗口内两个连续帧图像bk和bk+1之间的IMU测量数据，根据式(9)～式(11)，IMU积分过程的测量残差为[16]

(12)

1.2.2 视觉测量残差

(13)

式中，s为待求的尺度因子，表示与实际移动距离尺寸相关的缩放比例。

定义单目视觉在图像平面上的重投影误差，通过将第i帧图像中观测到的特征点n重投影到第j帧图像中，则视觉测量残差为

(14)

(15)

1.2.3 图优化状态方程

定义需要优化的状态向量如下

(16)

由式(12)和(14)定义代价函数为

(17)

式中，B为所有IMU测量数据的集合，C为解算关键帧内所有图像特征点的集合。通过Ceres Solver优化器，对式(17)求解最小二乘问题，就可以得到位置估计量、陀螺仪和加速度计零偏。至此，完成了由视觉和IMU传感器求解位置的过程。

2 实验结果

2.1 实验平台

实验平台采用optor惯性相机、Pixhawk飞控硬件和深圳玩智商科技有限公司生产的Dashgo D1移动机器人平台。optor惯性相机带有一个MPU-6050模块，集成了三轴陀螺仪和三轴加速度计，还带有两个30fps、分辨率为752×480的摄像头。用Pixhawk飞控中的HMC5883L三轴磁力计采集地磁场数据。Dashgo D1移动机器人的移动精度低于1%，作为移动平台验证算法的移动精度和旋转精度。所有传感器都通过USB连接到NUC机载电脑上，进行数据采集，并在机载电脑上运行算法进行位姿估计，如图1所示。

图2 实验平台

2.2 实验结果

通过Dashgo D1移动机器人搭载实验设备在室内分别沿着XY轴直线运动15m和绕z轴旋转360°，分别运行本文算法和VINS-SLAM算法，进行实验并记录实验数据。将本文算法解算结果和当前解算精度高的VINS-SLAM算法解算结果进行对比。

首先进行沿XY轴直线运动的实验，使移动机器人以0.05m/s的速度分别沿着XY轴直线匀速移动15m。记录并对比本文算法和VINS-SLAM算法解算结果的误差。实验结果如图3～图6所示，横轴表示时间，纵轴表示移动距离。

图3 X轴实验结果

图4 X轴实验误差

图5 Y轴实验结果

图6 Y轴实验误差

将Dashgo D1移动机器人绕z轴按2°/s匀速旋转360°进行Yaw姿态角解算实验。记录并对比本文算法和VINS-SLAM算法解算结果的误差，如图7、图8所示。

图7 Yaw角实验结果

图8 Yaw角实验误差

从图(8)可以看出，由于VINS-SLAM算法只用IMU和单目视觉进行位姿解算，在纯旋转的情况下，当旋转180°后由于此时视觉传感器无法解算对极几何而解算失效，此时只依赖陀螺仪积分进行姿态解算，就会出现姿态解算漂移。而本文算法主要依靠磁力计和IMU进行姿态解算，有效避免了单目视觉SLAM算法中存在的姿态解算漂移的问题。

表1 实验误差结果

通过表1对比本文算法与不使用磁力计的VINS-SLAM算法可以看出，本文算法通过使用磁力计使位置解算误差降低到0.4m以内。而解算的姿态角克服了VINS-SLAM算法中由于纯旋转情况而产生的漂移问题，并且解算精度在5°以内。

3 结论

本文首次将磁力计、单目视觉和 IMU传感器数据进行融合，解算移动机器人位姿状态，并通过实验对设计的算法进行验证，与VINS-SLAM单目视觉算法进行对比。实验结果表明，所设计的基于磁力计、IMU和单目视觉的自主定位算法，不但提高了移动机器人的位姿解算精度，同时也克服了现有的单目视觉SLAM算法在纯旋转情况下姿态角解算漂移的问题。本算法由于只使用磁力计、IMU和单目摄像头，具有质量小、成本低、消耗计算资源少等优点，因此可广泛应用于小型移动机器人的自主导航系统中。