1、第 44 卷第 4 期兵 器 装 备 工 程 学 报2023 年 4月收稿日期:2022 06 01;修回日期:2022 06 20作者简介:王瑞(1997)，男，硕士研究生，E-mail:wangrui957163 com。通信作者:戴劲松(1968)，男，博士研究生，研究员，E-mail:djs101163 com。doi:1011809/bqzbgcxb202304039基于改进视觉惯性里程计的无人机系统设计王瑞，王茂森，戴劲松，管红根(南京理工大学 机械工程学院，南京210094)摘要:对小型旋翼无人机在缺乏 GNSS 信号情况下的定位导航问题，提出了基于改进视觉惯性里程计的无人机软硬
2、件方案。首先设计带优先级的样本点采集策略和并行化计算方法，改进传统 ANSAC 算法，嵌入 VINS-Fusion 并用于无人机定位;其次，搭建无人机硬件平台，设计了飞控通信接口与控制系统。同时为高效地验证算法有效性，借助gazebo 实现了无人机系统仿真。实验结果表明:算法在基础矩阵计算中平均内点比例提高 1.8%，使用算法改进后的里程计在 MH_01 序列上的均方根误差降低 0 001 m，在 MH_02 序列上的均方根误差降低 0 019 m，满足无人机定位与导航时对精度与实时性的要求。关键词:视觉惯性里程计;改进 ANSAC;外点剔除;VINS-Fusion;无人机本文引用格式:王瑞，
3、王茂森，戴劲松，等 基于改进视觉惯性里程计的无人机系统设计J 兵器装备工程学报，2023，44(4):275 281Citation format:WANG ui，WANG Maosen，DAI Jinsong，et al UAV system design based on an improved visual inertialodometry J Journal of Ordnance Equipment Engineering，2023，44(4):275 281中图分类号:TP391文献标识码:A文章编号:2096 2304(2023)04 0275 07UAV system desi
4、gn based on an improved visual inertial odometryWANG ui，WANG Maosen，DAI Jinsong，GUAN Honggen(School of Mechanical Engineering，Nanjing University of Science and Technology，Nanjing 210094，China)Abstract:Aiming at the positioning and navigation of small rotary-wing UAVs in the absence of GNSSsignals，th
5、is paper proposes a UAV software and hardware solution based on an improved visual inertialodometry Firstly，the sampling point acquisition strategy and the parallel calculation method with priorityare designed，the traditional ANSAC algorithm is improved，and VINS-Fusion is embedded and used forUAV po
6、sitioning Secondly，the UAV hardware platform is built，and the flight control communicationinterface and control system is designed At the same time，in order to effectively verify the validity of thealgorithm，the simulation of the UAV system is realized with the help of gazebo The experimental result
7、sshow that the average interior point ratio of the algorithm in the basic matrix calculation increases by1 8%，the root mean square error of the odometry after using the improved algorithm on the MH_01sequence reduces by 0 001 m，and the root mean square error of the MH_02 sequence reduces by0 019 m，w
8、hich meets the accuracy and real-time requirements of UAV positioning and navigationKey words:visual inertial odometry;the improved ANSAC;outlier elimination;VINS-Fusion;UAV0引言以多旋翼无人机为主的小型空中机器人技术取得了长足发展，广泛应用于军事侦察、武装打击、人员搜救，场景重建等领域。上述应用场景中，无人机主要依靠工作人员遥控操作，只有少部分可以根据电脑终端设定的轨迹进行小范围、低复杂度的飞行。小型多旋翼无人机的定位是实
9、现其在复杂环境下自主移动的基本问题，在军事、民用等多种领域下有着广阔的应用前景。对于实现无人机的智能化与自主化而言，最重要的前提是无人机能够利用机载传感器完成对环境的感知和自身位姿的精确估计。常见机载传感器有GPS、北斗、惯性测量单元、视觉传感器和激光传感器等。GPS、北斗等卫星导航定位系统具有全天候工作、覆盖范围广等特点，将其与惯性测量单元融合，可以获得较高的位姿估计精度，在军用和民用无人机上有较多应用，但当无人机在城市或丛林等遮挡严重的环境中时，卫星导航系统的定位数据不可靠，甚至无法使用1。文献 2利用激光雷达产生的点云数据实现无人机在缺乏 GPS 信号情况下进行自身定位，但激光雷达体积大
10、、且价格昂贵，不适用于小型旋翼无人机。文献 3 使用单目相机估计无人机位姿并基于 Apriltag2 进行融合以获取深度信息，该方法可以弥补单目相机无法获取环境尺度的缺陷，尺度信息依赖于 Apriltag2，在室内场景下有较好效果，但不适用于室外环境。文献 4提出了使用TK 技术辅助定位的方案，在室外可以获得较高的定位精度，但无人机搭载的相机仅用于拍摄与后期数据处理，没有介绍在 TK 信号覆盖不到场景下的解决方案。文献 5则进一步将 TK 定位数据融合到基于视觉的定位方案中，通过扩展卡尔曼滤波算法将不同信息进行有效融合，有利于解决由于累积误差造成的漂移问题，但该方式需要事先在环境中架设 TK
11、基站，不具有普适性。文献 6使用视觉与高度传感器进行无人机在室内环境下的定位，但只适用于低速运动情况，且在室外光线变化大时效果不理想。基于优化的视觉惯性里程计(visual-inertial-odometry，VIO)采用视觉和惯性测量单元紧耦合的方式来估计本体运动状态7，充分结合了2 种传感器的特性，在 GPS 信号缺失的场景下具有定位精度高、成本低等优点，因此该方式逐渐成为当前主流发展趋势，受到了普遍关注。针对小型多旋翼无人机在无 GNSS 信号的复杂场景下对自身位姿的估计问题，在基于优化的视觉惯性里程计基础上，提出改进的里程计并构建一套无人机系统进行验证。通过改进后的 ANSAC 算法剔
12、除外点，设计飞控通信接口与控制模块并搭建了 gazebo 仿真环境。实验验证，改进后算法具有良好的定位精度与实时性，对无人机在复杂环境下的自主运动具有实际工程意义。1视觉惯性里程计1 1视觉 SLAM 数学模型经典的视觉 SLAM 数学模型如下:xk=f(xk1，uk)+wkzk，j=h(yj，xk)+vk，j(1)式(1)表示运动方程和观测方程，xk与 xk 1分别表示 k，k 1 时刻的位姿，uk表示 k 时刻的运动输入量，wk表示对应的噪声。zk，j表示 k 时刻，在 xk位姿下对 yj路标的观测值，vk，j表 示 k 时 刻 的 观 测 噪 声。在 视 觉 惯 性 里 程 计 中，f(
13、xk 1，uk)由惯性测量单元的数学模型确定，h(yj，xk)由相机投影模型确定。惯性测量单元通过预积分模型构造残差项，而视觉部分通过重投影误差构造残差项，最后利用非线性优化算法求解代价函数即可得到融合后的最优状态量。1 2对极约束与基础矩阵相机分别从视角 1 与视角 2 观察三维空间中同一点 P，在 P 点未知的情况下，求解视角 1 到视角 2 相机的运动，可以通过对极几何约束解决，如图 1 所示。图 1对极几何Fig 1 Epipolar geometry对极约束关系可表示为公式(2):pT2Fp1=0(2)式中:p1与 p2为真实物理世界中某点 P 分别在视角 1 与视角 2 下的投影，
14、F 为 3 3 的基础矩阵。p1与 p2使用齐次坐标，在相差一个常系数情况下式(2)仍然成立，因此 F 矩阵的自由度为 8，一般使用 8 点法8 计算。假设 p1=x1，y1，1T，p2=x2，y2，1T，代入式(2)，得到:x2，y2，1f1f2f3f4f5f6f7f8f9x1y11=0(3)将基础矩阵 F 各元素展开为一个向量处理，可得到 8 点法求解基础矩阵的一般式。文献 9 认为，原始图像坐标点的齐次值各分量的数量级相差太大，导致 8 点法求解基础矩阵的结果不稳定，提出一种预处理方式，先对像素坐标进行归一化处理，再使用 8点法求解基础矩阵，最后通过基础矩阵分解得到相机运动的672兵 器
15、 装 备 工 程 学 报http:/bzxb cqut edu cn/旋转矩阵和平移向量。在已知三维空间点和像素点的情况下，可以通过 n 点透视法(Perspective-n-Point，PnP)求解相机位姿。无论是通过对极约束还是 PnP 方法求解相机位姿，求解精度与特征点的匹配精度都有很大关系，因此，对前端算法进行改进，通过剔除错误匹配点，达到提高里程计精度与鲁棒性的目的。2改进 ANSAC 算法剔除外点2 1ANSAC 算法根据前端光流追踪或特征匹配算法，不同视角下两幅图像的匹配点通常远远大于 8 对，为充分利用更多的点计算基础矩阵，可以采用最小二乘算法。但在实践中考虑匹配存在误差，由于
16、噪声的存在，匹配点对之间并不能严格遵循式(2)，最小二乘算法中错误的匹配数据可能会导致结果偏离真实值。随机采样一致性算法(random sample consensus，ANSAC)是一种从包含错误值的数据中迭代估计模型参数的算法10。它具有异常检测的特性，在视觉惯性里程计中，将错误匹配点称为外点，正确的匹配点称为内点，借助该算法可以实现 2 个作用:在缺失 3D 点时计算基础矩阵，从而进一步分解出相机的运动姿态;向里程计后端发送匹配点对之前，通过对极约束计算基础矩阵的方式，尽可能剔除外点，为后端优化创造更好的条件。传统的 ANSAC 算法包含以下几个步骤:1)将所有匹配点集 S 中的特征点进行归一化处理。2)在匹配点集 S 中随机采样抽取8 对点，采用8 点法计算基础矩阵 F。3)在基础矩阵 F 下，计算所有匹配点的重投影误差 e，并与预设阈值 T 进行比较，当 eT 时，记该点为内点，否则将该点标记为外点，通过内点占总匹配点的比例计算该模型得分 score。4)重复步骤 2)、3)，直到达到最大迭代次数或满足内点比例超过预设值，获取得分最高的结果作为基础矩阵 F，并从匹配点集 S
