一种基于视觉惯性里程计的微型无人机控制系统设计

孙艺东，戴天虹，罗飞扬

（1.东北林业大学 机电工程学院，黑龙江哈尔滨，150000；2.中国民航大学 航空工程学院，天津，300000）

0 引言

近年来，微型无人机系统的应用范围越来越广，适用途径也从传统的航拍，巡逻等应用，逐渐向室内外建图、农业播撒、特种侦察等专业的行业应用转变[1]。针对无人机本身设计难度适中及愈发丰富的行业应用背景，越来越多的高校将无人机控制系统设计纳入大学本科和研究生阶段重要的创新环节。各类关于无人机的学科竞赛也层出不穷，其中全国大学生电子设计竞赛是教育部和工业和信息化部共同发起的大学生学科竞赛之一，组委会于2021年比赛中，提出了面向行业应用的“植保无人机”赛题。

赛题要求设计一基于四旋翼飞行器的模拟植保飞行器，完成自主飞行、循迹和控制激光笔的闪烁功能，以此来模拟对指定田块完成“撒药”作业。这类赛题要求无人机能够在室内无GPS场景下进行位置和姿态控制[2]，并完成指定的路径的规划和目标搜索和识别功能。对无人机姿态控制系统、机载传感器和机载图像识别模块都有了更复杂的需求。本文针对上述赛题需求，提出了一种基于视觉惯性里程计的微型无人机控制系统设计方案，解决了无人机在室内无GPS有效定位条件下的无人机位姿控制和路径规划问题，结合对各类视觉识别传感器的分析，最终提出了一套针对“植保无人机”赛题的解决方案。

目前针对无人机等微小型机器人，常用的室内定位方法包括，光流法、短距基站定位法、并发建图与定位法和视觉惯性里程计法[3]。其中，视觉惯性里程计法因其鲁棒性高、响应灵敏、有高度集成的商业解决方案等优点，在无人机室内应用的研发中被广泛使用。

1 视觉惯性里程计原理

视觉惯性里程计(VIO,Visual-Inertial Odometry)也称视觉惯性系统(VINS，Visual-Inertial System)，是融合相机和IMU数据实现定位的方法。该算法由计算机视觉SLAM体系中的视觉里程计算法(VO，Visual Odometry)演化产生[4]。

在传统的定位方法中，利用视觉元件和惯性元件可以分别对目标进行定位。利用双目相机，于像素平面进行目标特征点识别和匹配、确定相差从而计算出目标物体的三维坐标，该方法对于低速运动的特征点丰富的目标能够进行较为准确的定位。然而对于高速或运动状态剧烈改变的物体，由于其相机帧率和快门时间等内参因素，这种定位方式难以运行。利用搭载在目标物体上的捷联惯性部件，如加速度计和陀螺仪，能够高速、实时地测量物体的姿态和加速度。相对于视觉方法，惯性部件可以估计快速运动状态或运动状态剧烈变化等情况，具有良好的普适性。数据和惯性数据融合有很好的互补性。利用这种互补性，将视觉数据和惯性部件数据进行融合，将能够大幅度提升定位的准确性和稳定性。

表1 相机和IMU的缺点及互补性

视觉惯性里程计算法将视觉数据和惯性数据分别进行预处理：将惯性数据进行积分得出惯性估计的位姿序列，对图像特征点进行追踪得出图像估计的位姿序列。将上述两种位置估计数据对齐并进行融合，通过计算并最大化物体位置和姿态坐标在视觉、惯性测量数据下的后验概率给出位姿坐标的最优解[5]，从而实时跟踪目标位置，可以估计出相机轨迹的真实尺度。

随着MEMS器件的快速发展，利用智能手机、深度传感器等设备可以便捷地获取IMU数据和摄像头拍摄数据，融合IMU 和视觉信息的VIO 算法可以很大程度地提高视觉SLAM 算法性能[6]，是一种低成本高性能的导航方案。

2 系统总体设计

2.1 系统设计要求

设计一种基于四旋翼飞行器的模拟植保飞行器，能够对指定田块完成“撒药” 作业。如图1所示作业区中，灰色部分是非播撒区域，绿色部分是待“播撒农药” 的区域，分成多个 50cm× 50cm 虚线格区块，用 1~28 数字标识，以全覆盖飞行方式完成播撒作业。

图1 作业区示意图

图1中，黑底白字的“十”字是飞行器起降点标识，“A”标识是播撒作业起点区块，飞行器用启闭可控的激光笔的闪烁光点表示播撒动作，在作业区中放置一只高度为150cm杆塔，杆塔上套有圆环形条形码。作业中或返航途中，飞行器识别条形码，并用 LED 闪烁次数显示数字。

图2 系统模块整体设计图

2.2 主控系统设计

系统以STM32F405RG作为飞行控制器的主控芯片，飞行控制器集成MPU6050陀螺仪和加速度计传感器，IST8310磁罗盘传感器，SPL06气压计。同时预留了PMW9301光流传感器和VL53Lx激光测距传感器的安装插座。系统模块的整体设计如图3所示。

系统以Raspberry Pi 4B作为飞行计算机，用以处理来自Intel Realsense T265视觉惯性里程计的数据。Raspberry Pi 4B采用博通BCM2711型1.5GHz四核架构CPU，并最高配备8GB内存，体积小重量轻，并配备USB3.0接口。

视觉惯性里程计模块被安装在机身上层前方无遮挡区域，保证传感器收集的图像准确无干扰。飞行计算机安装在机身上层后方区域，在机身纵向方向与前方传感器进行配平，保证无人机的重心不偏移。飞行控制器被安装在机身下层，使用斜拉橡胶球减震板与机身进行软连接，使集成在飞行控制器上的光流模块和激光测距模块能无遮挡的对地照射，同时保证无人机飞行时产生的高频震动不易传至下方飞行控制器。这种下置式飞行控制器有区别于传统的飞控安装方式，可以极大的减小重量及系统布线压力。无人机实物图如图3所示，飞行控制器硬件电路实物图如图4所示。

图3 无人机实物图

图4 飞行控制器硬件电路实物图

3 系统程序设计

3.1 视觉惯性里程计程序设计

本系统使用Intel RealSenseT265作为视觉惯性里程计传感器，该传感器通过USB3.0协议与飞行计算机，即树莓派连接。利用传感器供应商提供的SDK程序。我们可以便捷的获取到T265所输出的三轴位置信息、速度信息和四元数。

其中，位置信息及速度信息均为地面坐标系下的三维向量(Xv,Yv,Zv)及(Vvx,Vvy,Vzv)。而在无人机位置控制系统中，我们需要使用到地面坐标系下的三维位置坐标(Xv,Yv,Zv)作为位置外环输入，使用机体坐标系下的三轴速度(Vbx,Vby,Vbv)作为速度内环输入。因此需使用坐标变换方法，将地面坐标系下(Vvx,Vvy,Vzv)转换为机体坐标系下(Vbx,Vby,Vbv)，即求飞行速度矢量在集体轴系各轴投影。其公式如下：

其中(φ,θ,)ψ为机体三轴欧拉角，即俯仰，横滚及偏航。对应的旋转公式如下：

其中欧拉角由四元数进行求取，公式如下：

经过上述旋转过程后，将飞行控制器所需数据通过串口以数据包的形式进行发送。飞行控制器接收数据并执行位置控制逻辑，如图5所示。

图5 视觉惯性里程计的位置控制

3.2 任务控制逻辑

无人机在执行前后两次任务的过程中，根据视觉惯性里程计提供的当前坐标进行航线飞行，在航线飞行过程中通过K210图像传感器反馈的下方颜色信息，控制激光笔进行打点。通过前方的Openmv图像传感器进行，杆体和条形码的识别，从而判断杆体是否在飞行路径上，防止任务中撞杆。任务控制逻辑如图6所示。

图6 任务控制逻辑图

3.3 视觉惯性里程计的校准与在线矫正

视觉惯性里程计在初始化过程中，使用图像和加速度信息进行内参估计。这种估计依赖在初始化过程中图像和惯性数据的质量，因此视觉惯性里程计在每次使用时，需提供一个较好的外部环境以帮助里程计更好的初始化，从而获得更好的重复精度。同时，在里程计工作过程中，将通过词袋法进行闭环修正。修正过程将产生速度及位置数据的跳变，对无人机飞行产生影响。因此在初始化过程中，通过携带飞机绕场平稳运动，可以有效减少里程计因闭环修正产生的跳变。绕场运动示意图如图7所示。

图7 绕场运动示意图

视觉惯性模块因其制造工艺、出场前内参校准、工作前校准等不确定因素，很难保证其尺度的一致性。经如图8所示的方法进行多次测量，确定这种由尺度不确定性产生的误差满足一定规律。如图8所示，多次使无人机按折线飞行产生的尺度误差成比例规律分布。

图8 误差测试过程示意图

针对上述视觉惯性模块存在的普遍尺度误差，提出一种利用飞控机载激光测距模块进行的在线校正算法，进一步提高位姿控制精度。即在无人机起飞过程中，使无人机在里程计数据1M处悬停，多次使用激光测距对地进行测距，测定的平均实际距离X2即为实际高度（米），即可求得比例系数K=X2/100。将比例系数K乘入里程计数据，即可得校正后的数据。经验证，这种方式可以有效的补偿T265的定位误差。

4 测试方案与测试结果

4.1 测试方案

根据设计需求进行分步测试，测试内容包括：撒播点数测试，条形码识别测试和降落精度测试。测试在自主搭建安全网空间内进行，使用的机型为带防撞圈的250mm轴距无人机。

4.2 测试结果及分析

表2 基础部分成功撒播点数记录表（部分）

表3 基础部分机降落位置距“十”字中心点距离（部分）

表4 发挥部分成功撒播点数记录表（部分）

表5 openMV识别成功次数记录表（部分）

表6 发挥部分机降落位置距“十”字中心点距离（部分）

模拟撒播过程中，漏洒点数，降落位置以及openMV识别条形码的稳定性与飞行器的稳定性密切相关，飞行器越稳定，漏洒点数越少，降落位置越符合规范，识别成功率也更大。实验数据表明，随着对飞行器参数的调整，飞行器逐渐趋于稳定状态，基本达到预期效果。

5 总结

本文以基于视觉惯性里程计的无人机控制系统为设计对象，以2021年全国大学生电子设计大赛G题为设计目标，从系统设计和软件控制逻辑层面搭建出一个较为完整的运行体系。并以视觉惯性里程计为研究对象，通过分析与测试，归纳阐明了里程计误差产生的原因及误差对系统控制的影响。通过改良系统初始化流程，并提出一种结合激光测距进行在线校正的算法逻辑，使里程计数据更加稳定鲁棒。并通过实验获得的实际数据，对无人机进行微调，最终使其实现稳定悬停，路径规划及避障功能，达到了设计目标。