基于视觉辅助导航的小旋翼机群编队穿越障碍技术

王梓豪，朱 波,2，王 奇，厚秉新，秦开宇*

(1.电子科技大学航空航天学院 成都611731；2.中山大学航空航天学院 广州510006)

旋翼机集群编队穿越障碍技术是一个多学科交叉的领域，涉及到了编队飞行控制[1-2]、无人机集群自组织网络[3-4]、机载传感器数据融合[5-6]及空间环境智能感知[7-8]等。其中，旋翼机集群对目标的感知定位与避障能力是其完成复杂飞行任务的重要基础之一。考虑到旋翼机的载荷能力，轻便型视觉传感器是作为辅助定位与避障穿越任务的良好选择。针对此类问题，国内外已经展开了相关研究，文献[9-12]提供了目前常见的一些基于视觉辅助的旋翼机编队导航方案；文献[13-14]提供了基于主−从式(leader-follower)异构旋翼机编队的导航方案。此外，在近地面低空飞行的情况下，由于气压计不稳定带来的旋翼机与地面相对测高误差也是一个需要解决的难题。随着激光雷达的发展，小型LiDAR激光传感器可作为旋翼机低空测高的良好选择，研究者们也开始关注该领域的相关问题，如基于LiDAR的地形测量[15]、机载LiDAR辅助避障[16]、LiDAR与惯性导航器件融合定位[17]等。

旋翼机编队技术应用广泛，如空中侦测、编队作战打击、无人机物流等。这些应用中涉及穿越复杂障碍[18-20]，如峡谷、高楼缝隙、大型物流仓库的门窗等。如何为无人机提供有效的环境感知和穿越障碍技术是一个亟待解决的问题。基于以上思考，本文提出一种飞行通道约束的概念，将街道两边建筑或者具有直线特征的障碍抽象成类似矩形门框的两条竖边，并精准估计门框的中心点，以此作为期望航点实现对障碍的穿越。

该研究内容参加了2018年由空军装备部举办的首届“无人争锋”智能无人机集群系统挑战赛科目1(SC-1)：密集编队穿越竞速赛。主要研究贡献如下：

1)完成异构无人机编队穿越仿真验证，并构建了一套无人机集群编队飞行实机软硬件测试平台；

2)设计一种基于单目视觉辅助的目标相对定位方法，实现了无人机编队在线实时穿越与路径规划；

3)设计一套多传感器融合的安全飞行策略，并设计一种基于激光定高数据的时域滤波器，为无人机编队在低空复杂环境中的飞行定高提供保障。

1 无人机编队穿越技术总体框架

总体框架研究将小旋翼无人机编队穿越系统分为硬件、软件和算法3个层面，如图1所示。

图1 无人机编队穿越障碍技术框图

以此技术框架为基础，进一步研究基于视觉辅助的小旋翼无人机编队穿越障碍技术，以解决主−从式异构旋翼机编队飞行任务中的避障穿越、路径规划、安全飞行策略等技术问题。

2 无人机编队硬件平台构建

旋翼机分为主机和从机两个型号，主机采用550 mm轴距机架和14寸桨叶，质量为630 g，最大载重6 kg。从机采用360 mm轴距机架和9寸桨叶，机架总重量为267 g，最大载重3 kg。此外，主机将额外搭载机载英特尔迷你i7处理器、机载路由器、单目视觉传感器和云台，担负起地面站软件控制、WIFI广播、障碍感知与定位等重要任务。图2展示了试飞准备阶段的主机和从机实物图。

图2 试飞阶段实机展示图

主机搭载3自由度云台的单目视觉传感器为避障穿越提供辅助视觉输入信息。该摄像头具有轻便、低成本、安装便捷等特点，最大分辨率1 600×1 200，焦距2.8 mm，刷新率30 Hz，模块尺寸30 mm×30 mm。采用采样率500 Hz工业级激光传感器LiDAR Lite辅助定高，实现低空飞行条件下对地面的精确相对定高。根据光照条件不同，实测该激光雷达有效距离为16～40 m之间。

主机的机载型地面站可以通过WiFi路由器实现和从机的WiFi收发模块进行通信，实现位置信息、控制指令的交互。相比于传统的固定地面站，由于主机采用机载地面站和机载WiFi路由器的形式，较好地保障了与附近从机指令交互的实时性。此外，地面控制人员若发现异常，可以利用远距离数传模块实现编队一键返航功能，进一步提升了集群飞行的安全性。

3 编队飞行软件设计

平台采用的地面站软件是由北京飞行魔方科技有限公司利用Mission Planner二次开发的无人机编队控制地面站软件。该地面站主要由飞行器管理、任务设计、实时控制、实时数据查看等部分组成。本文利用Visual Studio 2015开发了基于视觉避障算法和数传通信的专用开发包，该开发包基于编队控制软件提供的服务使用命名管道实现双向通讯，保证了两个进程通讯的可靠性和时效性，通过管道发送命令字符控制编队。利用开发的视觉避障模块，可以向该地面站发布最新航点数据，实现无人机集群的在线路径规划与避障穿越功能；利用远距离数传模块实现紧急返航等功能。利用开发包和无人机地面站软件，可以实时监控无人机集群的基本飞行状态、定位数据、航迹规划轨迹等信息。

4 编队避障穿越算法设计

4.1 视觉识别与辅助定位算法

当旋翼机编队发现并接近待穿越的障碍区域时，主机利用视觉传感器获取目标障碍的图像信息后求解其几何穿越点的相对3D空间位置。利用低精度的GPS获取了多帧图像间的基线，采用三角定位初步估计目标穿越点并模拟出穿越路径。由于GPS模块往往存在1～5 m的定位估计误差，需要在飞行过程中利用多帧2D图像进行连续跟踪，修正穿越点的空间位置坐标使得其保持在2D图像的中心区域，算法流程如图3所示。

图3 视觉识别与避障算法流程图

假设两张图中同一特征角点空间坐标为P(X,Y,Z)，其投影在2D图像的坐标为p2(x2,y2,z2)和p1(x1,y1,z1)。两帧间相机运动为[R,t]，其中R为旋转矩阵，t为平行向量。由于相机安装了云台，可知旋转矩阵R为单位矩阵，因此两帧图像的空间变化由位移向量t表征，可得为基线长度即为t。

根据对极几何理论可以知，设x1,x2为p1,p2特征点归一化坐标，它们满足：

上式左侧为0，可以看做是未知数为z2的方程进行求解，同理可以求得z1。已知目标深度后，可以利用单目针孔模型获得穿越点空间位置，并将穿越航点下发到地面站。利用多帧实时解算连续修正穿越点定位误差，以提高成功穿越障碍的概率。当发现穿越点计算偏离原值大于0.1 m时，进行实时路径规划修正。

4.2 编队飞行与控制策略

4.2.1多传感器定高保护算法

一方面，考虑到气压计和GPS信号融合获得的高度信息通常存在0.3～0.8 m的误差，因此采用激光雷达测高作为辅助手段。另一方面，为了防止激光定高数据抖动或者故障时旋翼机出现冲高等危险动作，采用激光、气压计、GPS信号同时对高度进行估计：

1)在0～16 m：正常飞行避障条件下，激光作为主要参考，利用滤波后的激光数据作为无人机相对地面的测高值；

2)在12 m以上：加入Pixhawk内部融合的气压计和GPS信息作为保护，如果检测到激光定高数据相对Pixhawk内部相对高度数据值偏离超过2 m即判定激光数据异常，则自动切换到Pixhawk内部气压计和GPS信号融合的定高数据。

4.2.2基于激光数据的时域滤波方法

考虑到在低空区域更多关注无人机集群相对地面的高度，针对激光定高数据面对地面变化时抖动大容易引起飞行控制环路产生过调节现象或者震荡反馈等问题，给出了一种时域滤波方法：

式中， HFn为n时刻待求的无人机期望高度； HFn−1为n−1时 刻已知的无人机期望高度； HLn−1为n−1时刻的激光雷达数据。S定义为滤波补偿项：

4.2.3编队飞行安全距离策略

考虑到异构无人机编队采用的是主−从结构，且视觉传感器搭载在主机上。因此，编队采用了一字型方案，主机在前，从机跟随排列，从而提供更可靠和宽阔的视觉输入信息。考虑到GPS误差等因素，设定单个飞机水平安全防碰撞区域为2.5 m，机间水平方向间隔保持5 m。垂直地面方向考虑有激光传感器，定位精度相对较高，设定单个飞机垂直安全防碰撞区域为2 m，考虑到气流气压等因素，机间垂直方向安全距离为4 m。

4.3 无人机集群编队逻辑

结合“无人争锋”比赛的实验试飞场地，编队穿越飞行测试流程如下：1)无人机编队利用机载地面站发布起飞指令后，一字型编队飞行到各个门框障碍附近并搜索门框目标；2)利用主机的单目相机和LiDAR激光等传感器实现障碍目标定位，解算穿越点的空间坐标和穿越航向角，并分配各个无人机的穿越路线；3)利用地面站软件将最终航点下发到其他从机，无人机编队进行穿越障碍并向下一个障碍区域编队飞行；4)依次完成4个门框的穿越任务，返回起飞点完成安全降落。

此外，利用4.2节中的保护策略和pixhawk中的故障保护设计，一旦发生主机丢失、坠毁等严重事故，从机将自动切换一键返航，达到减小机群损失的目的。

5 仿真实验与实机编队飞行验证

5.1 定高时域滤波器仿真

针对4.2.2节的滤波器，仿真中对两种地形进行模拟与分析：1)具有连续坑洼和树木的地形，在该类地形条件下，通常希望滤波器能过滤激烈的数据抖动，保持平稳飞行。2)具有台阶特征的地形，在该类地形条件下，通常希望无人机能保持和地面的相对高度，并在台阶处作平滑上升动作，减少激烈的陡然攀升动作。

模拟飞行高度相对于地面为5 m，设定气压计噪声为标准差0.20、方差1、均值0的高斯噪声；激光噪声为标准差0.01、方差1、均值0的高斯噪声；在不同阻力因子R下的仿真激光数据的滤波输出结果如图4所示。

图4 定高时域滤波器仿真

由图4a可得到，随着R的减小，过滤效果越来越明显，且相比于缓慢变化的气压计定高，利用激光相对定高数据更具稳定性。由图4b可以得，随着R的减小，通过陡变平台的平滑效果越来越明显，平滑过渡防止了无人机控制环路过高的调节量，既节约了能源，其飞行过程也更加安全平稳。

5.2 视觉定位仿真

考虑到无人机飞行测试成本，首先在室内设计按比例缩小的门框障碍，利用机载单目相机离线测试视觉定位算法可靠性。相较于室内，室外环境更多地受到光照和成像质量的影响。本文在户外实验中还原了比赛场地障碍设置并进行了实际测量，该门框障碍高2.4 m，宽7.2 m。室内和室外的测试效果如图5和图6所示。

图5 室内视觉定位仿真示意

图6 室外视觉定位仿真示意

因研究背景是无人机集群穿越障碍，将问题转换为寻找合适的障碍穿越角度，其中定义a ngle角为门框法线方向与门框中点和镜头连线的夹角，即该时刻无人机应该选择的穿越航向角。分别进行的20次室内、室外仿真测试中，穿越角度平均误差分别为0.85°和1.87°，满足无人机集群的穿越条件。

5.3 ROS无人机编队穿越仿真

利用ROS(robot operating system)系统的Rviz仿真工具进行了无人机编队飞行逻辑验证。其可视化的特点方便分析飞行逻辑，图7和图8给出了编队飞行逻辑示意图和Rviz下的仿真轨迹，红点为随机出现的门框障碍区域，黄点为主机，黑点为从机。

图7 无人机集群飞行穿越障碍示意图

图8 Rviz仿真无人机集群飞行穿越障碍

5.4 室外实机编队与穿越实验

在完成无人机的LiDAR激光传感器调试、单目相机与云台测试、集群离线仿真编队验证后，旋翼机编队在正式的测试场地进行了完整的穿越障碍飞行测试。表1为测试飞行场地中4个障碍门框区域(障碍门将在该区域附近随机出现)的经纬度数据以及门框法线相对于正北顺时针朝向的角度数据。

表1 障碍穿越区域中心坐标信息

旋翼机需要完成对目标障碍进行搜索、识别、编队穿越。图9和图10展示了编号不同区域，主机获取的视觉第一视角。图11和图12展示了不同位置观测到无人机编队穿越门框障碍的瞬间。

图9 2号障碍主机第一视角

图10 1号附近主机第一视角

图11 1号障碍平视穿越瞬间

图12 3号障碍仰视穿越瞬间

可以看出，2号门附近地面平坦整洁，1号门附近树木坑洼较多。在不同的光照条件、地形条件下，无人机集群均能稳定地识别、估计定位并穿越障碍目标。

在条件允许的情况下尽可能多的在4个区域进行了编队穿越障碍测试。通过表2可以看出，由于各个穿越区地形特点与测试难度不同，飞行成功率也存在差异。其中，2号门区域地面平坦、环境简单的特点提高了其穿越成功率。1、4号门地面坑洼，树木繁多，且该区域飞行时测试时出现磁罗盘误差较大，因此穿越成功率有所下降。总体来说，在实机编队穿越测验中穿越总体成功率达到77%左右，说明该技术具备可行性和进一步研究的价值。

表2 给出了比赛场地测试的穿越次数、穿越航向角与成功率的相关实验数据。

6 结束语

本文完整地构建了该小旋翼无人机编队系统的软硬件一体化平台。利用视觉、LiDAR等传感器实现了对无人机集群的辅助测高与目标障碍的定位与穿越，提出了一种小旋翼无人机编队穿越障碍技术。针对复杂室外环境穿越障碍这一技术目标，提出基于单目视觉的连续跟踪定位算法和基于激光雷达的时域滤波方法，提高了对待穿越障碍目标的定位精度和可靠性，并通过大量仿真测试和实机编队飞行实验验证了该技术方案的可行性。