视觉与惯性融合的多旋翼飞行机器人室内定位技术

张怀捷，马静雅，刘浩源，郭品，邓慧超，*，徐坤，丁希仑

1．北京航空航天大学 机器人研究所，北京 100191

2．北京空间飞行器总体设计部，北京 100094

多旋翼飞行机器人具有结构简单、功能性强、动态特性好的特点，被广泛应用于环境监测、灾难救援、边境巡检等公共安全领域［1-2］。在面向城市或者森林的搜索与救援任务时，多旋翼飞行机器人机动性高，可以观测到大范围的视野，但由于森林和建筑物的遮挡，依赖于全球定位系统（Global Position System， GPS）的定位方法失效，对目标的搜索与救援任务也变得困难。因此，实现多旋翼飞行机器人无GPS 环境下的自主跟踪及降落任务，具有较强的应用价值和现实意义。

对于自主跟踪及降落问题，传统导航系统采用惯性导航系统（Inertial Navigation System， INS）与GPS 来实现［3］。但是，INS 会因为积分导致累积误差，GPS 也会由于卫星信号被遮挡而失效，因此，基于视觉定位［4-6］的方式来实现自主跟踪及降落可以让飞行机器人适应大多数复杂环境下的情况。

而对于自主定位问题，室外开阔环境下，主要通过GPS 全球定位系统来进行定位，在森林、室内等环境下，卫星信号被遮挡会导致机器人定位失效，从而影响飞行机器人自主功能的实现。目前，国内外提出了一些室内定位的方法［7-8］，例如基于无线保真（Wireless Fidelity， WiFi）、射频识 别 技 术（Radio Frequency Identification， RFID）［9］、超 宽 带 技 术（Ultra Wide Band， UWB）、红外等，但这些方法需要依赖于外部设备使得飞行机器人的应用场景受限，不具备在未知环境下完全自主的功能。因此，采用同步定位与建图（Simultaneous Localization and Mapping， SLAM）技术拥有更大的优势。

视觉SLAM 中的视觉前端也叫做视觉里程计［10］，主要包括特征点提取和匹配，数据关联，位姿估计几部分。按其数据关联方式不同，通常将其分为特征点法和直接法。特征点法是在关键点提取的基础上，通过描述子匹配建立数据关联［11-12］；直接法是在关键点提取的基础上，基于灰度不变假设的光流跟踪实现数据关联［13-14］。此外，有学者将卷积神经网络的深度估计用于直接法中，与单目SLAM 中的深度估计融合，为位姿估计提供绝对的尺度信息［15］。

由于飞行机器人运动变化较快，仅依靠视觉定位方式会使得定位精度低及稳定性差。目前，研究人员普遍采用多传感器融合定位的方式来实现位姿估计以减小飞行机器人运动过程中的定 位 偏 差，例 如MSCKF［16］、VINS-Mono［17］、ORB-SLAM3［18］等。

为实现室内场景下的自主跟踪及降落任务，设计了基于视觉定位的地面平台自主跟踪及降落系统，在Gazebo 中仿真验证其有效性。面向室内 定 位 的 场 景 需 求，借 鉴 了VINS-Mono［17］中 的融合方法，针对室内场景图像对比度较低的问题，在视觉前端加入图像增强算法处理从而减少光流跟踪中的误匹配点数以提高定位精度。为解决飞行机器人单目视觉惯性定位中的漂移问题，本文提出了一种基于图像信息的特征点提取和图像帧发布策略，以提高飞行机器人的定位精度及稳定性。将改进后的视觉前端数据与惯性数据融合，在EuRoC 数据集中对比评估算法结果，设计飞行机器人平台，完成了真实场景下的室内定位实验，基于运动捕捉系统获取的定位真值评估定位结果，完成了室内自主跟踪及降落实验，验证了系统有效性。

1 系统框架

设计的多旋翼飞行机器人平台系统总体框架如图1 所示，分为4 个子模块：飞控模块、室内定位模块、自主导航模块及地面站模块。飞控模块搭载气压计和惯性测量单元（Inertial Measurement Unit， IMU）姿态传感器，运行实时性较高的姿态解算和位置控制，实现稳定飞行；定位模块包括机载处理器、相机、IMU，获取图像信息和姿态数据，在机载处理器中运行定位算法，为飞行机器人提供实时的位姿估计结果；自主导航模块包括机载处理器和俯视相机两部分，运行自主跟踪及降落算法，为飞控提供自主导航指令；地面站实时监控系统运行结果。

图1 系统总体框架Fig.1 System overall framework

定位系统框架如图2 所示。为实现室内场景下飞行机器人定位飞行任务，在前端视觉里程计中加入图像增强处理以提高图像灰度对比。为解决后端视觉惯性融合优化中的漂移问题，提出了一种基于图像信息的特征点提取和图像帧发布策略以提高定位精度及稳定性。之后，基于Perspective-n-Point（PnP）实现了连续帧帧间位姿估计。最后，对IMU 数据进行预积分，将改进后的视觉前端数据与惯性数据初始化对齐后，利用非线性优化方法求解飞行机器人位姿。

图2 视觉惯性融合定位系统框架Fig.2 Visual-inertial fusion positioning system framework

针对飞行机器人自主跟踪及降落任务中任务场景复杂、手动降落不稳定及现有GPS 定位精度不高的情况，设计基于视觉定位的自主跟踪及降落系统，总体流程如图3 所示：用户通过人机交互模块（上位机或遥控器）输入指令；飞行机器人自主起飞，搜索地面目标；通过悬停校准、水平接近、自主降落三种状态机的切换自主执行跟踪及降落任务。

图3 自主跟踪及降落流程图Fig.3 Flowchart of autonomous tracking and landing

2 基于直接法的单目视觉里程计

2.1 图像增强

由于应用场景为室内场景，对于相机获取的图像信息，需进行图像增强处理以提高灰度对比用于减少光流跟踪中的误匹配点。区别于OpenCV 中的全局直方图均衡化思想，采用图像块分块插值的思想来实现。首先，计算灰度直方图，对图像以块为单位，统计并存储每个块中不同灰度像素的数量计算灰度直方图；其次，裁剪直方图，对灰度直方图纵轴设置一个标准阈值，统计其像素灰度平均值，对超出阈值，超出平均值或者低于平均值的情况分别裁剪，计算累计直方图并对其归一化；最后，对图像块间进行分块插值以减少块状效应。图像块角点像素保持原灰度，角点以外的部分采用双线性插值方法，通过循环遍历图像中所有像素点计算其图像增强处理后的灰度值。如图4 所示，插值节点选择待求像素点相邻4 个图像块的中心点像素坐标Q11(x1，y1)、Q12(x1，y2)、Q21(x2，y1)、Q22(x2，y2)，P点为插值点。

图4 双线性插值Fig.4 Bilinear interpolation

双线性插值的公式为

式中：f(Q)为该点像素灰度值；f(x，y)为经双线性插值处理后待求点的像素灰度值。

2.2 图像角点提取

面向SLAM 领域的局部特征提取问题，特征点以其计算量较小，对于场景中光照、遮挡、旋转的鲁棒性较高等优势被广泛应用。角点作为特征点中重要的一种，主要应用于目标跟踪和视觉定位等场合中。表1综合比较Shi-Tomasi［19］、Fast［20］、SIFT［21］3种角点，结合多旋翼飞行机器人室内定位的实时性和准确性要求，决定采用提取速度较快，且同时具备光照鲁棒和旋转不变性的Shi-Tomasi角点，Shi-Tomasi角点提取示意图如图5 所示。

表1 角点性能综合比较分析Table 1 Comprehensive comparative analysis of corner performance

图5 Shi-Tomasi 角点提取示意图Fig.5 Schematic diagram of Shi-Tomasi corner extraction

设当前窗口中心像素坐标为P(x，y)，其像素灰度值为I(x，y)，对窗口沿X、Y轴正向移动微小位移(u，v)，中心点像素灰度值为I(x+u，y+v)，则窗口沿各个方向移动所造成的像素灰度值的变化为

式中：

对当前图像所有窗口设定一个标准阈值，以M矩阵中最小的特征值作为当前窗口得分，当得分超过设定阈值时，即认为当前像素点是一个角点。

2.3 基于图像信息的特征点提取和图像帧发布策略

飞行机器人在视觉与惯性融合定位过程中常会出现朝一个方向持续漂移的问题。经过实验分析，其主要原因是飞行机器人的速度变化过快，在不同运动场景下采用相同的图像帧发布频率和特征点发布数量会导致在机器人速度较慢、场景变化不大的情况下有相似的图像帧冗余，而在机器人速度较快，或机身抖动较为严重的情况下又会因为发布的图像帧不足使得用于优化的图像数据不足，导致视觉定位失效。此时，融合过程更多依赖于IMU 的数据，而仅依靠IMU 实现定位又会受到IMU 噪声的影响产生累计漂移，导致定位失效。因此，本文提出了一种应用于飞行机器人的基于图像信息的特征点提取和图像帧发布策略，以提高飞行机器人定位性能，如图6所示。

图6 基于图像信息的特征点提取和图像帧发布策略Fig.6 Feature point extraction and image frame publishing strategy based on image information

对相邻两帧图像建立图像金字塔，采用计时器获取相邻两帧图像的时间间隔，基于金字塔光流跟踪［22-23］得到对应特征点的匹配关系，做差得到像素速度，对滑动窗口内所有像素点求和平均，根据窗口内所有像素点的平均速度来控制视觉前端发布给后端用于优化的特征点数量和图像帧频率，考虑到飞行机器人调姿和飞行过程中抖动的影响，对数据做了延时和移动平均处理，通过平滑数据来达到防抖的效果。

之后，基于随机抽样一致算法（Random Sample Consensus， RANSAC）实现误匹配点剔除，基于PnP 实现连续帧的位姿估计，将位姿估计结果与惯性数据融合，优化求解飞行机器人位姿估计。

3 视觉惯性融合定位

融合思路借鉴了VINS-Mono［17］中的融合思想，将改进后前端视觉里程计中的相机初始位姿估计与IMU 预积分结果初始化对齐，利用相机旋转约束标定陀螺仪偏置，同时初始化IMU 速度、重力向量以及相机位姿估计时的尺度因子。

在原有优化方程的基础上，将改进后的视觉前端连续帧估计结果与IMU 状态数据融合加入方程，计算噪声项的协方差矩阵，归一化两者量纲，基于滑动窗口法优化求解飞行机器人位姿，优化方程表示为

式中：rp为先验残差，通过被边缘化的某帧对窗口内其他帧的约束关系来构建；B、C为IMU 测量值和视觉测量值的集合；rB为IMU 预积分残差，通过IMU 预积分结果变式求得；rC为视觉重投影残差；ρ为柯西核函数；优化变量χ为滑动窗口中IMU状态信息，包含位置、速度、旋转角度、相机外参、加速度计和陀螺仪偏置ba、bg及三角化恢复出的特征点逆深度λ信息；可以表示为

除原有的图像数据外，将改进后的视觉前端增加的特征点和图像帧信息投影至参考帧下，与参考帧中的测量值构建重投影残差。Σp、ΣB、ΣC为3 种误差项的协方差矩阵。采用列文伯格-马夸尔特方法，调用Ceres 库完成求解［17］。

4 自主跟踪及降落策略

自主降落系统包含5 个坐标系，如图7 所示，分别为世界坐标系Ow、机器人本体坐标系Ob、相机坐标系Oc、Marker 坐标系Om、相机像素平面坐标系Op。其中，飞行机器人初始坐标系原点与世界坐标系Ow原点重合，Xw、Yw、Zw的正方向采用飞行机器人坐标表示的通用方法，分别指向北、东、地心3 个方向。飞行机器人本体坐标系Ob以其质心为原点，Xb、Yb、Zb分别指向机头、机身右侧、垂直机身向下3 个方向。相机坐标系Oc以其光心为原点，Xc、Yc、Zc分别指向相机右侧、下方、垂直相机平面向外侧3 个方向。Marker 坐标系Om以Aruco Marker 几何中心为原点，Xm、Ym、Zm分别指向北、东、地心3 个方向。其中，Ow到Ob的位姿变换关系Tw b可以由定位算法得到。由于选用了RealSenseD435i 相机固连在机身本体上，Ob到Oc的位姿变换关系Tb c由相机安装位置决定。相机坐标系下的目标位置Pc由Aruco Marker 的位姿估计结果得到。由此，可以通过坐标转换关系不断更新目标Marker 在世界坐标系下的位姿信息Pw用于位姿校正：

图7 坐标转换关系Fig.7 Coordinate transformation relationship

根据任务需求，为飞行机器人自主任务建立状态机，包含初始化、起飞、目标搜索、悬停校准、水平接近、自主降落共6 个状态机标志位。其中，目标识别部分采用了Aruco Marker 模块提供地面目标相对于机身本体的三维位置信息。由于俯视相机固连于机身本体，通过目标搜索、悬停校准、水平接近3 个状态机之间的状态切换实现对于地面目标的跟踪，满足许可水平误差阈值要求后执行自主降落，算法框架如图8 所示。

图8 自主跟踪及降落Fig.8 Autonomous tracking and landing

算法主要需要解决以下4 个关键点：

1） 俯视相机固连在飞行机器人本体上，飞行机器人运动或者调姿过程会影响标志物在视野中的位置，同时考虑到通信延时的影响，采用悬停校准的方式来不断更新标志物在世界坐标系下的位置。

2） 飞行机器人相比于地面移动机器人具有高动态、移动范围大的特性，飞行过程会影响目标检测的稳定性，所以需要设立目标初检测、稳定检测及检测丢失几个标志位来处理检测丢失的情况。其中，目标初检测以识别模块的回调函数触发作为判断标准，持续触发回调函数一定时间即视为稳定检测。对于检测丢失标志位，考虑到飞行过程中抖动对检测稳定性造成的影响，以距离上次触发识别模块的回调函数间隔超过1 s作为检测丢失标志位的判断依据。

3） 飞行机器人在执行自主任务时，由机载处理器执行控制指令输入，需要在外环开发遥控失控保护功能以保证安全性。

4） 飞行机器人对目标的自主降落需要建立在对自身准确定位的基础上。但是，无论在室外采用GPS 全球定位系统还是室内采用SLAM 算法，定位结果都会存在一定程度的漂移。所以，需要以当前漂移位置作为基准，可以有效减弱飞行机器人自身定位精度对于自主降落系统本身的影响。此外，对机器人到达目标位置的判断要给予一定的容错估计，当实测位置误差小于某一阈值且持续一定时间，认为机器人已经到达其目标位置。

对于初始化状态机（initialization），需要执行传感器校准，传感器状态监控，通信连接状态确认。在所有状态就绪后，使能模式切换，由遥控指令输入切换回机载传感器指令输入，完成手动模式向自主模式的切换，使能解锁服务，一切就绪后，进入起飞状态机。

对于起飞状态机（take_off），考虑到定位数据的漂移影响，不以[0，0，0]作为初始位姿，而记录当前实测位姿数据，以该数据作为初始点。同时，开启俯视相机，开启目标检测功能，不断更新标志物在相机视野中的位姿，到达初始起飞位置。

对于目标搜索状态机（target_search），结合飞行高度和RealSense 相机视场角范围，给定初始目标所在矩形区域大小，按一定策略遍历区域内目标位置，识别到目标后，切换悬停校准状态机。倘若遍历区域内所有位置之后未识别到目标，返回初始点降落。

对于悬停校准状态机（hold_calibration），首先记录初始悬停位置，根据位姿估计结果进行悬停校准。如果在悬停校准状态机下目标丢失，切换目标搜索状态机。如果目标相对机身水平位置精度在一定时间内稳定满足误差要求且目标满足稳定检测许可判断，切换自主降落状态机。若未达到精度要求但目标满足稳定检测许可判断，切换水平接近状态机。

对于水平接近状态机（horizon_approach），要以当前实测水平位置作为基准，若到达目标位置且满足稳定检测许可判断，切换悬停校准状态机，若到达目标位置但检测丢失，切换目标搜索状态机。

对于自主降落状态机（auto_landing），本算法采用的定高设备为气压计，考虑到飞行机器人的近地效应会对气压计的估计精度产生较大影响，自主降落任务完成的判断标准由IMU 数据得出。当Z轴速度稳定小于某一阈值且持续0.5 s 以上，判断飞行机器人已降落，执行电机上锁，退出自主模式，由遥控接管指令输入。

5 仿真验证分析

5.1 自主跟踪及降落Gazebo 仿真

为验证所编写的基于视觉定位的地面平台自主跟踪及降落算法可靠性，在Gazebo 中建立飞行机器人仿真模型进行硬件在环仿真，如图9所示。

图9 自主跟踪及降落仿真Fig.9 Simulation of autonomous tracking and landing

终端为程序输出，左上角为俯视相机机载视野，右下角为地面站状态监测。使用笔记本电脑替代机载处理器发布任务指令，地面站负责监控当前机器人状态，对算法进行优化设计验证其准确有效性。

5.2 公开数据集验证

在EuRoC 数据集［24］上测试定位算法结果，该数据集基于微型飞行器的室内飞行实验，包含了相机和IMU 的信息，采用的数据集序列为MH_01～MH_05、V1_01～V1_03 共8 个 数 据集，每个数据集基于2 种改进思路的不同组合进行了5 次对比实验，得到40 组实验结果。其中，MH_01_easy 数据集场景偏暗，V1_02_medium数据集飞行速度变化较快，适合本文所改进的定位算法应用场景，故着重分析这2 个数据集的实验结果。采用里程计评测（Evaluation of Odometry， EVO）工具评估改进前后的算法估计值与真实轨迹的差异，计算每对位姿之间的绝对轨迹误差，通过计算均方根误差（Root Mean Square Error， RMSE）、平 均 误 差（Mean Absolute Error， MAE）和 方 差（Sum of Squares for Error， SSE）3 项指标来评估定位精度。MH_01_easy 数据集评估结果如表2 所示，V1_02_medium 数据集评估结果如表3 所示。

表2 MH_01_easy 数据集评估结果Table 2 MH_01_easy dataset evaluation results

表3 V1_02_medium 数据集评估结果Table 3 V1_02_medium dataset evaluation results

改进方案相比于原有方案，策略1 中仅加入了图像增强算法，策略2 中仅加入了本文提出的一种基于图像信息的特征点提取和图像帧发布策略，改进后的方法同时加入了以上2 种改进思路，用以提高飞行机器人飞行过程中的定位精度。实验结果表明，平均误差和均方根误差在改进后均减小，其中，MH_01_easy 数据集场景偏暗，飞行速度变化较慢，仅引入图像增强算法处理条件下，平均误差减少了7%，均方根误差减少了5%，方差减少了10%；仅引入一种基于图像信息的特征点提取和图像帧发布策略后，平均误差减少了2%，均方根误差减少了1%，方差减少了5%；同时引入两种改进思路后，平均误差减小了10%，均方根误差减少了7%，方差减小了13%。V1_02_medium 数据集场景图像对比度较高，飞行速度变化较快，仅引入图像增强算法处理条件下，平均误差减少了3%，均方根误差减少了2%，方差减少了4%；仅引入一种基于图像信息的特征点提取和图像帧发布策略后，平均误差减少了12%，均方根误差减少了9%，方差减少了18%；同时引入两种改进思路后，平均误差减小了14%，均方根误差减少了10%，方差减小了20%。测试结果表明，在场景整体较暗使得图像对比度较低时引入图像增强算法效果明显，而在飞行机器人运动速度较快使得相机视野中图像特征变化较快时引入本文提出的一种基于图像信息的特征点提取和图像帧发布策略效果明显。同时加入2 种改进思路后，改进后的算法相比于原有方案，定位精度均有较大的提升。

6 飞行实验验证

6.1 多旋翼飞行机器人平台

基于任务需求设计并搭建多旋翼飞行机器人平台，硬件系统包含F550 机架及动力系统，Pixhawk 飞控及配套飞行设备一套。传感设备为RealSenseD455 与RealSenseD435i 相 机，板 载 处理设备选用Jetson Xavier NX，飞行机器人平台如图10 所示。

图10 多旋翼飞行机器人平台Fig.10 Multi rotor flying robot platform

6.2 图像增强及角点提取实验

采用飞行机器人机载相机获取当前环境中图像信息，应用本文图像增强算法实验处理后的结果如图11 所示，左侧为输入RGB 图像，中间为灰度图，右侧为引入图像增强算法处理后的结果。

图11 图像增强算法处理结果Fig.11 Image enhancement algorithm processing results

在室内实验室环境下，旋转飞行机器人机载相机每隔15°拍摄一组图像，如图12 所示。测试结果表明，Shi-Tomasi 具有良好的旋转不变性，对L 型角点检测效率较高，提取速度较快，4 幅图像提取角点数量的平均值为158，平均耗时0.076 ms，总耗时12.008 ms，RealSense 相机彩色图像输出频率为30 Hz，可以满足实时处理的要求。

图12 角点提取实验Fig.12 Corner extraction experiment

6.3 室内定位实验

在机载处理器中运行室内定位算法，完成室内悬停实验，如图13 所示。基于Motion Analysis运动捕捉系统提供的定位真值评估定位结果，三轴定位数据比较如图14 所示，横轴表示时间，纵轴表示定位误差。实验结果表明，在悬停过程中，定位误差保持在10 cm 以内，可以在室内环境下实现稳定悬停，满足要求。

图13 飞行机器人室内悬停Fig.13 Indoor hovering of flying robot

图14 三轴定位数据对比Fig.14 Comparison of three-axis positioning data

完成室内悬停实验后，为进一步验证本文引入的图像增强算法及一种基于图像信息的特征点提取和图像帧发布策略的改进效果，在暗光环境及明亮环境下完成对比实验。实验场景如图15所示，图15中A为暗光环境，B为明亮环境。

图15 飞行实验场景Fig.15 Flight experiment scene

采用遥控器输入位置指令，在室内场景下完成飞行机器人封闭轨迹实验，飞行实验选择在以实验室中心位置为区域中心，高度为0.7 m，水平区域为4 m×2.5 m 的范围内进行，定位结果见图16 和图17。其中，图16 为暗光环境下改进前方法与本文方法定位数据对比，图17 为明亮环境及暗光环境两种不同光照条件下本文方法定位数据与动捕系统的真值数据对比。

图16 暗光环境下水平定位数据对比Fig.16 Comparison of horizontal positioning data in dark environment

图17 不同光照条件下水平定位数据对比Fig.17 Comparison of horizontal positioning data under different lighting conditions

封闭轨迹实验结果表明，在靠近窗口的位置即X轴位置超过3.5 m 沿Y轴正方向飞行时，相机视野中图像特征更新较快，参与优化的图像帧较多，同时，窗帘的抖动被视为动态目标，对算法精度造成了比较大的影响。通过计算平均误差，在室内实验室较暗环境下，改进前算法定位平均误差为0.26 m，本文方法定位平均误差为0.17 m，有效提高了定位精度。此外，在室内实验室明亮环境下重复实验，本文方法定位平均误差为0.14 m，满足室内自主跟踪及降落实验的需求。

6.4 室内自主跟踪及降落实验

在完成飞行机器人的室内定位实验后，在6 m×6 m 的区域内随意放置靶标，在机载处理器上同时运行本文所编写的基于视觉定位的地面平台自主跟踪及降落算法，进行飞行机器人室内自主跟踪及降落实验，实验分为以下6 个部分：系统初始化、自主起飞、区域搜索、悬停校准、水平接近、自主降落，图18 为实验结果。

图18 中左下角为自主定位算法输出的位置估计值，右上角为机载俯视相机视野。A 为系统初始化：运行自主定位算法和自主导航算法，等待遥控器给予模式切换指令；B 为自主起飞：遥控器给予模式切换指令，使能自主导航任务中的失控保护；C、D 为区域搜索：在6 m×6 m 的区域内以一定策略遍历区域内所有位置，搜索地面靶标位置；E 为悬停校准：识别到靶标位置后，飞行机器人切换悬停模式，解算地面目标相对于机身本体的位姿并且持续更新；F 为水平接近：根据悬停校准的结果来执行水平迫近；G 为自主降落：当靶标位置相对于机身本体的水平位置误差达到允许阈值，执行自主降落；H 为上锁保护：由于近地效应的影响，气压计的高度估计会存在比较大的误差，因此，通过读取IMU 的Z轴速度来执行上锁保护，当Z轴速度小于0.2 m/s 且持续0.5 s 以上时，飞控及电机上锁，实验结束。

图18 室内自主跟踪及降落实验Fig.18 Indoor autonomous tracking and landing experiment

实验结果表明，本文设计的室内条件下飞行机器人自主跟踪及降落算法存在一定的降落误差，造成误差的原因有以下3 个方面：识别模块的精度，算法中自主降落状态机中允许降落时设定的水平位置误差阈值，以及最后执行自主降落过程时的水平定位误差。经过多次迭代实验并优化算法中各项指标参数，最终确定在降落过程进行两次悬停校准，同时设置降落状态机初始许可水平误差阈值为7 cm，实验相关数据如表4所示。

表4 自主跟踪及降落实验结果Table 4 Results of autonomous tracking and landing experiments

重复实验表明，所设计的飞行机器人室内自主降落系统误差维持在10 cm 以内，有效地减弱了室内定位实验中飞行机器人自身定位精度误差对于室内自主降落系统本身的影响。

7 结 论

1）为实现自主跟踪及降落任务，编写基于视觉定位的飞行机器人自主跟踪及降落算法，在Gazebo 中搭建飞行机器人模型完成仿真验证。

2）针对飞行机器人室内定位需求，在视觉前端加入图像增强处理，提出了一种基于图像信息的特征点提取和图像帧发布策略以提高定位精度，在EuRoC 数据集下采用EVO 工具评估定位效果。结果表明，改进的算法可以有效提高机器人定位精度。其中，MH_01_easy 数据集场景下平均误差减小了10%，均方根误差减少了7%，方差减小了13%，V1_02_medium 数据集场景下平均误差减小了14%，均方根误差减少了10%，方差减小了20%。

3）真实场景下搭建了一套多旋翼飞行机器人平台，完成室内定位实验，基于运动捕捉系统评估真实场景下飞行机器人定位精度。根据任务需求，完成室内场景下对地面平台的自主跟踪及降落实验。实验结果表明，室内实验室场景下，封闭轨迹实验的定位平均误差不超过0.17 m，所设计的飞行机器人室内自主降落系统误差保持在0.1 m 以内，有效地减弱了室内定位实验中飞行机器人自身定位精度误差对于室内自主降落系统本身的影响，满足室内场景下的自主跟踪及降落任务需求。

未来将基于本文的研究工作组合地面异构机器人目标实现空地协同任务，拓展应用场景，提升机器人在复杂场景下的适应能力。