基于视惯融合的无人飞行器导航数据仿真方法*

张逸舟，曾庆化,2,3，许 睿，王景琪，罗怡雪

（1.南京航空航天大学 自动化学院 导航研究中心·南京·211106；2.南京航空航天大学 先进飞行器导航、控制与健康管理工业和信息化部重点实验室·南京·211106；3.南京航空航天大学 江苏省物联网与控制技术重点实验室·南京·211106）

0 引 言

针对当前无人飞行器在使用过程中可能面临的卫星信号拒止等条件，依靠载体自身所搭载的传感设备完成智能导航定位与准确的路径规划将变得更具挑战性。当飞行器执行隐蔽工作任务或是飞行环境相对复杂时，使用自主式的传感系统如惯性器件和对外式的传感系统如相机器件、激光雷达、气压高度计等，将更有利于实现同步定位与建图（Simultaneous Localization and Mapping，SLAM）功能，因而被广泛研究。

单独的传感器导航系统具有一定局限性，例如惯性器件在短时间内可以得到较准确的位姿信息，但会因噪声的积累使得误差随时间发散。视觉导航中,单目图像序列难以提供有效的尺度信息，在运动模糊、特征丢失的情况下将无法正常工作。因此，惯性导航通常被认为是与视觉导航互补的导航系统：通过运动恢复结构的约束可限制惯性测量积分结果的发散，且与惯性测量融合后系统的尺度、俯仰角和滚转角均具备能观性[1]。现如今，视觉惯性里程计（Visual-Inertial Odometry，VIO）已成为发展较成熟的组合导航算法，该技术被广泛应用在机器人[2]、陆地车辆[3]等自主导航设备中。

在视惯融合导航方法中，ORB（Oriented FAST and Rotated BRIEF）特征点[4]提取方法较为经典，但该方法容易错误匹配特征且运算效率有待提升；光流法[5]则是跟踪特征的另一种方式，光流可用于表示载体的运动，在连续帧间跟踪特征时能兼顾鲁棒性与速度。在后端中有基于滤波和基于优化两种组合策略，基于李群的视惯融合算法[6]利用无迹卡尔曼滤波对算法进行优化，较典型的是利用光流的双目多状态约束卡尔曼滤波（Multi-State Constraint Kalman Filter， MSCKF）[7]的视觉惯性里程计，也有基于Census变换改进光流的单目视觉导航算法[8]。采用图优化方法能够有效提高视惯融合的鲁棒性，基于该方法的尺度不敏感多传感器融合框架[9]已在多种平台下有所应用。

香港科技大学的开源视觉惯性里程计算法VINS-Mono[10]是基于图优化的单目视觉惯性里程计算法，在启动后需要首先完成初始化过程，在载体做匀速直线运动的过程中启动将难以完成求解。针对此问题，本文设计了一种该场景下视觉特征与惯性数据的仿真方案，研究并解决了初始化、航迹设置和特征点仿真等系列问题。仿真实验表明，在使用本文算法辅助完成初始化过程后，VINS-Mono可以很好地应用于无人飞行器自主导航作业，在不加入噪声时，无人机500m平直飞行过程的定位误差不超过真值的3.5%，为该场景下的无人飞行器视觉惯性组合导航问题提供了重要的算法参考。

1 VINS-Mono算法模型研究

VINS-Mono方案是一个基于紧耦合与图优化的滑动窗口单目视觉惯性里程计，使用紧耦合算法将具备更高的精度与准确性[11]。算法使用图优化进行组合，该优化模型的三部分残差组成为：基于光束平差法（Bundle Adjustment，BA）的视觉残差、惯性测量残差以及边缘化先验残差。本节将对与仿真过程直接相关的视觉残差和惯性残差模型进行简要分析。

1.1 视觉残差模型分析

VINS-Mono中使用光束平差法构造误差函数，得到重投影误差之和最小的状态估计。视觉重投影误差被定义为一个特征点在归一化相机坐标系下的估计值与观测值的差。假设第i、j帧为前后相邻帧，前帧的某个归一化特征点坐标可用（ui,vi）表示，对应逆深度λ在算法中已求，后帧中依据光流法跟踪观测到的该特征点坐标可用（uj,vj）表示，相机与载体系外参数阵为Tbc，载体在世界坐标系中的位姿为Twbi、Twbj，则后帧中对于该特征点的估计值计算如下

（1）

该点的视觉重投影误差rc可用平移、旋转量（包含于Twbi、Twbj中）的映射函数表示，见式（2）。对于滑动窗口中的m帧图像,使用式（3）构建误差函数，由此得到视觉残差量。

（2）

（3）

其中，pci为第i帧时相机在空间中的位置；qci为第i帧时相机在空间中的旋转四元数。

1.2 惯性残差模型分析

惯性测量数学模型可用式（4）表示，bgyro、ngyro表示陀螺仪的偏置和白噪声，bacc、nacc表示加速度计的偏置和白噪声，偏置满足随机游走模型,即对应一阶导数为白噪声形式。

（4）

VIO系统中惯性测量频率通常高于图像，为避免每次更新相机姿态后对数据重复积分，VINS-Mono中引入了预积分概念。此处同样假设第i、j帧为前后相邻帧，预积分中的积分项是第j帧相对于第i帧图像坐标的姿态，而非相对世界坐标系的姿态，因此，预积分的值只与惯性测量单元（Inertial Measurement Unit，IMU）短期内测量值相关。定义位移、速度和角度的预积分增量如式（5）所示

（5）

式中，abt表示在机体系下t时刻的加速度真值；ωbt表示在机体系下t时刻的角速度真值；αbibj、βbibj、qbibj分别表示在关键帧之间的位置、速度、角度预积分增量项。由此可以得到式（6）中定义在i、j时刻IMU测量的位移、姿态、速度与偏置的残差，姿态通过四元数乘法计算得到，位移、速度与偏置的残差均直接相减获得。

（6）

2 无人机飞行视觉惯性仿真方案设计

由视觉惯性测量模型可知，若需对特定场景下的无人机飞行进行视觉惯性数据仿真，其难点在于构造无人机观测到的视觉特征点，以及陀螺仪和加速度计的三轴输出。本文使用贺一家团队[12]的开源视觉惯性仿真工具imusim进行方案设计与实验验证。

2.1 系统初始化仿真方法

VINS-Mono为单目VIO系统，此类算法的一大难题是退化运动将导致尺度的不确定性，诸如匀速直线运动、悬停、静止均被视作退化运动，但对于无人机而言，此类是常见的飞行状态[13]。初始化通常是VIO系统中非常重要的步骤，其目的是在未知情况下估计系统的重力方向、速度以及尺度的初始状态。

为了呈现可观测的尺度，系统在初始化过程中至少需要有两个轴上的非零加速度，故需要额外对初始化状态的视觉惯性数据进行仿真。仿真无人机在平飞作业前的初始化过程，其航迹应满足如下两点要求：1）初始化轨迹需要有对三轴的加速度激励，但为确保轨迹的真实性，三轴速度不可以发生跳变；2）起飞点与初始化轨迹结束点位置要确保一致，在初始化结束至平飞阶段三轴的速度不应发生跳变。

无人机起飞过程中势必会产生晃动，因此使用正弦、余弦函数进行起飞前的初始化模拟是合理的。导航坐标系为东北天（ENU）坐标系，初始化参数设计为：起点（0，0，82），东向、北向分别做幅值20m的余弦、正弦运动，天向做幅值2m的余弦运动且角速率为另两轴一定整数倍，运动时长40s，控制角速度使得初始化过程为整圈运动并回到起点。依据确定的轨迹参数可以推导仿真出相应的IMU数据[14]，具体的轨迹位置和运动公式如式（7）所示。轨迹如图1（b）所示。

（7）

式中，P（t）、V（t）、a（t）分别为初始化模拟过程中载体的位置、速度和加速度的预设表达式。

（a） 初始化过程视觉特征点跟踪

（b） 初始化航迹（红色标记处为起/终点）图1 初始化过程示意图Fig.1 Diagrams of the initialization process

在完成对于初始化过程的补充后，VINS-Mono算法可以顺利恢复系统初值，并延续对于匀速平飞过程的里程计求解。该仿真方案亦可用于对接其他的非退化运动状态的轨迹解算。上述的初始化补充过程思路可简述为如下两部分：1）第一阶段：对系统状态值进行初始化运动补充，使算法完成初始化过程，获得系统状态初始值包括速度、重力加速度、偏差、白噪声和姿态初始值，同时获得以零位置为基准的位移量；2）第二阶段：继续完成整周的飞行过程，目的是使初始化完成后的航向角与所需对接的匀速平飞过程起点时刻的航向角相等或相近。整个过程的运动轨迹、特征点均为程序设置的虚拟轨迹、路标点，仿真设置的特征点经过光流跟踪后被转化为像素坐标点的形式存储在文本中。

2.2 平飞过程的地面特征设置

完成初始化后，进行无人机的正常作业阶段，出于算法验证的目的，此处仍使用imusim工具继续生成数据。平飞过程初速度设置与初始化结束瞬间相同且保持匀速直线运动，航向角保持为0°，过程中保持视野中始终存在多组地面特征点，且当无人机平行移动一段时间后,特征点群会周期性更新。VINS-Mono使用光流法进行跟踪，因此需要分段设置视野特征点，并保证每组特征点间均有重合部分，否则会导致特征跟踪中断。平飞过程对地面点的跟踪效果如图2所示。

图2 平飞500m地面特征点仿真示意图Fig.2 Simulation diagram of ground feature points in 500 meters flight mission

飞行过程中,假设惯性器件的坐标系与载体坐标系完全重合，相机与惯性器件坐标系垂直且有一定的位置偏移量。关于外参数矩阵以及相机的内参数矩阵[15]设置如表1所示。表1中,R、t为相机系至IMU系间的旋转矩阵和平移向量；fx、fy表示轴分辨率；u0、v0表示光心像素坐标零点偏置。

表1 相机与IMU仿真参数

3 高空平直飞行仿真实验与分析

用VINS-Mono算法解算无人机匀速平飞的俯拍视图与惯性数据，目的是验证该算法在此类场景下的可用性。结合imusim仿真工具，设置图像频率为30Hz，IMU输出频率为200Hz，以上述设计方案分别生成得到视觉关键帧点信息文件和陀螺仪与加速度计输出（数据为七维：时间戳、陀螺仪角速度输出、加速度计输出），结合时间戳对关键帧文件进行命名，实现数据的对齐。

（a） 无噪声仿真轨迹

飞行过程中的飞行距离与航向角统计如表2所示，因添加噪声后平飞轨迹不能正确跟踪，故加噪后的数据只统计到前80s作为参照。航距真值随平飞时间递增，航向角真值理论上始终为0°。由表2可知，在未加入IMU噪声时，无人机匀速平飞情况下可使用VINS-Mono对视觉惯性数据进行里程计求解。

表2 仿真数据解算值统计表格

VINS-Mono以光流法对视觉特征点进行跟踪，说明光流法跟踪的视觉导航精度尚可，平飞500m过程中航距解算误差不超过真值的3.5%，且航向角始终保持稳定。加入IMU噪声后，在误差积累到一定程度时系统需要二次初始化以实现故障恢复，由于本文中算法对地面特征点的仿真规律分布于北向且缺乏对三轴的激励，因此不能很好地进行二次初始化。

4 结 论

针对无人飞行器在可能的卫星拒止条件下的平直飞行作业场景，本文基于紧耦合的开源视觉惯性里程计算法VINS-Mono，设计了相应的数据仿真方案。针对单目VIO系统在退化运动下初始化困难的问题，进行了相应的初始化运动轨迹补充，具体为使用正余弦运动模拟无人机稳定飞行前的加速运动，并在后续对平飞过程的地面特征点进行设置和仿真。实验结果表明，该初始化补充方案可以正确恢复系统初值，并延续导航参数的解算，在理想条件下，500m平直飞行过程中航距解算误差比例不超过设置真值的3.5%，说明VINS-Mono算法具备在该场景下使用的可行性。但本文尚未对IMU噪声和故障恢复部分进行仿真，在加入IMU噪声后将无法进行正确解算，因此仍需更深层次的研究。