视觉和惯导信息融合小型无人机位姿估计研究

王继红，吴伯彪，张亚超，赵明冬

（郑州科技学院电气工程学院，河南 郑州 450064）

0 引 言

无人机执行艰巨复杂的任务时，对位姿的估计及其重要，卫星导航是常用的方法。但是卫星的信号在许多复杂的环境下会变得非常微弱甚至失锁。因此，可采用自主导航方式（主要包含惯性导航、视觉导航及雷达导航）代替。而较轻的单目视觉传感器以其小体积、低功耗闻名，将其用于无人机上，既可用来定位，也可用来进行无人值守和目标追踪与锁定。视觉同步定位与建图（visual simultaneous localization and mapping,VSLAM）技术[1]在近些年发展较快，这为无人机位姿估计提供了一种新的思路。

VSLAM技术指利用相机拍摄的图像获取自身位姿的同时完成对周围环境图像的构建。目前主要包含两种方法：一是通过两幅图像特征的提取获取相机此时的位姿，较为典型的为PTAM算法[2]以及ORB-SLAM算法[3]。另一种则是通过优化图像中的像素帧间获取位姿变化，称为直接法，较为典型的为SVO算法[4]和LSDSLAM算法[5]。由于直接法计算量较大，无法满足无人机位姿的实时解算。因此，本文利用ORB-SLAM算法进行位姿估计。

单目视觉的输出频率较低，无法满足无人机对高动态性能的需求，且利用纯视觉在复杂环境中获取的位姿不够精确。因此，将惯性测量单元（inertial measurement unit，IMU）和视觉进行组合用于无人机可获得高精度的位姿估计。

将视觉传感器和IMU进行融合可分为松紧耦合[6-7]两种。其中，松耦合通过扩展卡尔曼滤波[8]或无迹卡尔曼滤波（unscented Kalman filter，UKF）[9]将视觉传感器和IMU两个单独模块获得的位姿信息进行融合，文献[10]和[11]采用的信息融合属于松耦合。紧耦合则利用滤波器对视觉和IMU产生的中间数据进行处理后获得的位姿信息，其算法相对松耦合较为复杂，且扩展性不强，无法对多传感器信息进行融合，文献[12]和[13]采用的信息融合属于紧耦合。

综上所述，基于松耦合具有更低的复杂度，可以满足无人机对输出频率的高要求；且松耦合具备较强的扩展性，可以满足无人机上多传感器的信息融合。因此本文将通过松耦合完成无人机上的多信息融合，以此实现位姿信息的估计：首先通过无人机上的超声波传感器完成尺度的估计，然后利用最小二乘法完成尺度估计值的优化。最后把单目视觉传感器获得的位姿估计值当作观测量，通过扩展卡尔曼滤波完成对IMU位姿估计值的修正以提高位姿估计的精度。

1 系统的整体框架及初始化处理

1.1 系统的整体结构

系统的整体框架如图1所示，主要包括超声波传感器和IMU用来进行数据采集，单目相机用来进行图像采集。通过带尺度的ORB-SLAM算法对单目相机和超声波传感器采集的信息进行解算获得频率为25 Hz的六自由度位姿信息；通过惯导对IMU采集的数据进行解算得到频率为200 Hz的六自由度位姿信息。利用扩展卡尔曼滤波算法对25 Hz和200 Hz的六自由度位姿信息进行融合，最终获得频率为200 Hz的六自由度位姿估计。

图1 系统的整体结构

1.2 坐标系定义

图2 系统坐标系定义

1.3 视觉和IMU内外参标定

本文对固联后的单目相机和IMU进行安装，如图3所示，坐标系符合右手规则，以IMU初始时刻的姿态作为世界坐标系。采用二维码标定板和三轴转台对相机与IMU进行单独标定，以此获取相机的内参和IMU的零漂等参数。

图3 固联的视觉惯导系统

1.4 IMU的初始对准

众所周知，惯性器件使用之前必须进行初始对准以获取IMU的初始估计姿态，进而得到导航坐标系下IMU的初始姿态，也就是世界坐标系下的重力矢量。本文采用的方法是通过IMU自带的磁强计计算出偏航角，再结合重力矢量在三轴上的分量及IMU测量的加速度进行俯仰角和横滚角的解算，如式（1）和式（2）所示。

其中，俯仰角和横滚角用θx、θy表示，g表示当地的重力加速度，IMU在x轴、y轴和z轴上的加速度用gx、gy、gz表示。通过式（3）、式（4）和式（5）可计算得到偏航角θx：

其中，磁强计在三个轴上的输出分别用Mx、My、Mz表示。但是这种方法只是粗对准，后续还需采用卡尔曼滤波进行一个较为精确的对准，以获取精确的结果。

2 基于最小二乘的单目视觉尺度估计

由于在位姿估计时单目VSLAM算法无法获得绝对的尺度信息，这就需要其他信息进行辅助，以获取可在无人机上使用的尺度估计。目前最好的单目VSLAM算法是ORB-SLAM算法，其在VSLAM算法的基础上加入了闭环检测，提高了整个程序的运行效率。但是本文研究的无人机位姿估计是在大场景下进行的。对算法的实时解算要求非常高。因此本文在ORB-SLAM算法的基础上关闭了闭环检测线程，且限制了关键帧和地图点的数量，这样可大大提高算法的实时解算速度。

由于以单目相机为传感器的ORB-SLAM算法无法获取深度/尺度信息。因此本文将通过超声波传感器获取无人机的飞行高度，然后利用最小二乘法估计出其绝对尺度。

在一个固定的时间间隔内，单目ORB-SLAM可获取无人机在起飞阶段上升的高度差估计值，记为zi，这个时间段内无人机上升的实际高度差通过超声波传感器测得，记为hi，也称为绝对高度，因此尺度λ近似满足λ≈h/z。

但是这种方法误差较大，很难得到最优解。因此，本文将通过最小二乘估计完成尺度参数的估计。结合n组测量的观测值及尺度高度两者的关系可得：

式中：λ——待估计的参数；

ε1，ε2，···，εi——相互独立的误差值。

但是单目视觉ORB-SLAM在实际应用中会发生漂移，需要对不同时间段的测量值分配不同的权重ω，然后通过加权最小二乘法完成最终尺度的估计，如下式所示：

经过多次实验发现此方法估计的尺度不仅计算简便，而且估计的精度较高，在发挥充分稳定性的同时能实时完成尺度的估计。

3 基于EKF的信息融合

虽然无人机的位姿信息通过VSLAM和惯导均可获取，但是惯导解算时的输出频率相对VSLAM较快，惯导随着时间的增长会出现累积误差，而VSLAM姿态误差较大。因此，利用EKF进行信息融合时，本文将VSLAM的输出当作观测值，对惯导的状态进行预测，实时修正惯导解算得到的位姿。

3.1 惯导解算过程

利用IMU对惯导进行解算时需考虑噪声对其产生的影响，模型如下：

式中：am——加速度计的测量值；

a——IMU相对于惯性系的全加速度；

ωm——陀螺的测量值；

ω——IMU相对于惯性系的角速度；

na、ng——服从正态分布的高斯白噪声；

ba、——惯导内加速度计和陀螺的零漂。

可将零漂当作噪声引起的随机游走过程。

IMU的运动方程可用下式表示：

σ——通过角速度积分求得的角度旋转矢量。

通过圆锥算法可消除旋转矢量带来的常值漂移[14]。

通过下式可完成对速度的解算：

对速度进行积分可解算出IMU当前的位置信息，如下式所示：

其中T为IMU的采样周期。

3.2 EKF预测过程

当旋转角较小时，可通过小角度近似法求得四元数误差和角度误差之间的关系，分别用δq和δθ表示，如下式所示：

3.3 EKF更新过程

作为滤波可调参数的协方差矩阵R可通过观测噪声给定。通过观测矩阵和R可求得EKF的更新过程，如式（21）～（23）所示。

系统的增益矩阵K为

随后通过式（22）和（23）更新系统的状态向量和状态协方差矩阵。

4 结果与讨论

实验选用的无人机为大疆的M100小型多旋翼无人机，无人机自身配备了GPS接收器以及机载IMU，通过内部算法可以直接得到无人机的运行轨迹。整个视觉惯导位姿估计系统搭载于无人机的下方，如图4所示，相机为普通的USB摄像头，可获得1080P的彩色图像，图像输出最高频率为60 Hz，IMU的型号为MTI300。室内实验和室外实验均选择开阔的场地进行，实验当天天气晴朗，可见度较高，非常适合实验，其中室外无人机飞行的环境如图4所示。系统中将超声波传感器也垂直安装于相机正下方，三者形成固联。

图4 机载视觉惯导系统和实验现场

4.1 尺度估计结果分析

实验前需对小型无人机进行初始化，让无人机进行多次的垂直起飞以获取多组高度差数据，以保证尺度优化的精度和稳定性。本文进行了包含闭环检测的近距离尺度估计实验和不包含闭环检测的远距离尺度估计实验。近距离实验无人机位姿信息的真值通过VICON运动捕捉系统提供，远距离实验无人机位姿信息通过高精度组合导航系统提供。

实验结果如图5、图6和表1所示，其中图5为近距离实验，在室内进行；图6为远距离实验，在室外进行。起始位置和终止位置分别由圆点和三角形表示，星号代表闭环检测的位置。图中红线代表带尺度估计的ORB-SLAM估计位置，绿线代表无尺度估计的ORB-SLAM估计位置，蓝线是无人机的真实位置。

表1 尺度估计值

图5 ORB-SLAM尺度估计室内实验

图6 ORB-SLAM尺度估计室外实验

从实验结果可得，无人机运行的前半段时间，带尺度的ORB-SLAM轨迹与无人机真实的轨迹几乎重合，体现了尺度估计效果的明显。但是随着时间的增长，尺度发生了一定的漂移，导致通过ORBSLAM得到的轨迹相比真实轨迹产生了误差。此时通过无人机产生的高度变化对尺度估计进行重启可对漂移进行一定程度上的修正。另随着时间的增长，ORB-SLAM自身也产生了一定的累积误差，当闭环检测成功后会对累积误差进行一定程度的修正，对尺度估计的精度有较为明显的提高。

4.2 位姿估计结果分析

本节将进行通过纯惯导对位姿信息进行解算实验，然后通过信息融合解算出最终的位姿估计信息。

4.2.1 惯导解算结果分析

本文选择的IMU输出频率为200 Hz，通过静态实验对IMU的初始姿态和零漂进行获取；接着进行IMU在动态环境下的实验，利用IMU围绕半径为2 m的圆旋转一周形成一个圆形轨迹。从实验结果图7中可得，IMU解算的姿态信息与真值基本重合，体现了姿态解算具有较高的准确度。但是位置结果却在后几十秒出现了较大的漂移，这是因为无人机上使用的IMU精度较低，解算的位置自然会出现较大的漂移，因此需加入其他传感器进行信息融合以完成对位置的修正，从而解算出更准确的位姿。

4.2.2 EKF融合结果分析

本文将ORB-SLAM的位姿估计结果当作观测值以修正IMU解算结果。本文在数据融合时利用ROS发布节点的时间进行视觉和惯导的数据对齐。信息融合实验选择在室外的大场景下进行，飞行轨迹如图6所示，误差见表2，实验分析结果见图8和图9。

由图8可得，融合后的位置信息与真值基本一致，相比图7精度得到了一定的提升，且随着时间的增长也不再发生漂移。由图9可得，融合后的位置误差和姿态误差都有一定的降低，尤其姿态误差下降地较为明显。表2中，无人机在200 m的动态飞行中，融合后的位置误差和姿态误差相对纯惯导或者单目ORB-SLAM算法均有很大程度的减小，位置误差的均方根低于0.995 m，水平姿态角的均方根误差低于1.915°，偏航角的均方根误差达到了2.235°。上述实验表明，通过信息的融合不但提高了位姿的估计精度，而且解决了纯视觉输出频率低的问题，满足了无人机对高动态特性的需求，解决了无人机在卫星信号失锁时无法精确定位的问题。

图7 IMU动态实验解算结果

图8 EKF融合位姿真值对比图

图9 EKF融合位姿误差对比图

表2 融合实验误差分析值（距离约200 m）

5 结束语

本文以无人机在卫星信号失锁时无法实现精确定位为研究背景，设计了一个包含单目视觉、IMU及超声波传感器的组合系统，并且提出利用最小二乘法完成尺度估计值的优化。通过扩展卡尔曼滤波实现单目视觉与惯导的信息融合，可提高位姿的估计精度，并解决纯视觉输出频率低的问题，满足无人机对高动态特性的需求，为无人机在卫星信号失锁时无法精确定位的问题提供一种解决办法。