一种基于OpenMV的飞行器激光引导算法

黄迅 崔敏 黄隆群 邱敏卿

摘  要：一套能够在短距离内获得飞行器相对于目标物的位置，并能引导飞行器飞向目标的引导系统，对于当下的无人机定位应用开发来说是十分必要的。大多数固定翼飞行器目前都采用GPS的方式进行引导，但是在实际应用中，通常GPS精度只能够达到数米级，而更加精确的定位，就必须依靠诸如激光等主动导引方案。文章提出了一种基于OpenMV的固定翼飞行器近距离激光引导算法，在朝目标物照射激光后，固定翼飞行器能根据OpenMV求出的位置偏差量，结合PID自动调整自身姿态，稳定地朝激光方向飞行，为飞行器主动引导导航提供了一种可实现的廉价思路。

关键词：固定翼;定位算法;激光引导;OpenMV;PID

中图分类号：TP242         文献标志码：A         文章编号：2095-2945（2019）11-0019-04

Abstract： A guiding system that can obtain the position of the aircraft relative to the target in a short distance and can guide the aircraft to the target is very necessary for the development of the current UAV positioning application. At present， most fixed-wings are currently guided by GPS， but generally， GPS accuracy can only reach several meters. In order to locate more accurately， active guidance schemes such as laser must be relied on. In this paper， a OpenMV-based near-range laser guidance algorithm for fixed-wing aircraft is presented. After the laser is irradiated toward the target， according to this scheme， the fixed-wing aircraft will automatically adjust its posture according to the obtained deviation amount combined with the PID algorithm， and then stabilize the flight towards the target. It proposes an achievable cheap solution for fixed-wing active navigation.

Keywords： fixed wing; positioning algorithm; laser guidance; OpenMV; PID

1 概述

隨着无人机技术的逐步发展以及无人机市场的逐步扩大，无人机定位技术的应用场景也越来越多。以常见的四旋翼无人机为例，现在实用化的四旋翼无人机一般都要有光流定点作为室内定位，GPS与光流辅助作为室外定位[1]。目前无人机行业的领军者大疆甚至开发出了视觉里程计技术，实现了极为精确的室内/外定位技术。可以说，不能进行定点定位的无人机是残缺的，不会被市场所接纳。

而固定翼无人机的导航以及定位技术，目前却仍然比较单一，大部分还是以GPS导航定位为主，少部分会结合a的导航方式都属于卫星导航，卫星导航受环境的影响非常大，室内或者室外电磁干扰大的时候，卫星导航就没用了，局限性大。因此也需要一种能够脱离对卫星的依赖，但是又能实现导航的引导系统出现。

针对以上问题，笔者提出了一种可以在短距离生效的激光引导方案，该方案基于现在流行的OpenMV，在飞行器飞行过程中，只需给定一束特定的激光束照向目标，OpenMV就能发现激光束的位置并生成对应的控制信号引导飞行器朝目标航向飞行。这种方案能够摆脱固定翼飞机导航对卫星的依赖，在短距离导航场合有一定的应用价值。

2 激光引导系统的总体架构

在本例中，激光引导系统主要由红色激光照射器、固定翼飞行器、APM开源飞控、姿态调整执行器和OpenMV开源图像识别模块等五部分组成。

2.1 红色激光照射器

激光照射器采用市场上常见能买到的530nm、100mw 的红色激光手电，这种激光手电可以照射出稳定的激光并且焦点可调。

2.2 固定翼飞行器

笔者采用了飞翼构型的固定翼飞行器来作为这套制导系统的搭载平台。如图1所示。飞翼布局的飞行器相对于正常式布局的飞行器来说具有诸多优点。飞翼式的飞机没有机身，只有一张巨大的机翼，只需要左、右两片副翼就可以完成包括俯仰、滚转、偏航三个自由度的控制，虽然其本身相对于正常式布局的飞行器来说并不是一种特别稳定的结构，但是配合飞控之后其稳定性能和正常式布局的固定翼相差无几。而其简单的机械结构，方便快速维修的特性，非常适合用于进行可能会造成损毁的固定翼导航飞行试验。

2.3 APM开源飞控

APM开源飞控来源于著名无人机技术论坛DIY Drones，是一个历史悠久，非常成熟的飞控，而其经过多年的发展，目前也发展出了多种飞控形式。其中，最小最轻，最适合于狭窄空间应用的当属Mini APM[2]。

APM飞控“开源”意味着其软件和硬件都是开源的，而其软件已经有成熟的ArduPilot作为基础，可供二次开发，扩展性强。更重要的是，APM原生支持飞翼构型的飞行器，为开发省下了很多功夫。

2.4 姿态调整执行器

姿态调整执行器本质上即为一个摆动矢量发动机。它能够为飞行器的姿态调整提供一个瞬态、衡定和线性的力。而传统飞翼飞行器调整姿态用的副翼在调整姿态时容易受当前空速影响，不同空速下产生的力也不尽相同，而副翼在偏转时又会反过来影响当前飞行器的空速。因此，它对于飞行器位置的影响是一个非线性耦合的问题，不利于用在要求瞬态反应很快的飞行器激光导引上面。

开发这套方案所采用的摆动矢量发动机是使用两个球副连杆和两组舵机分别控制摆盘的两个自由度，而摆盘上固定着一个发动机，它为整个系统提供动力，如图2所示。

2.5 OpenMV开源图像识别模块

OpenMV是一个以STM32F427芯片为核心的开源低成本机器视觉模块。其利用Python作为编程语言进行编程。而在其内部已经集成了很多常见的图像识别算法，比如颜色识别、核相关滤波、特征检测等，还支持在增加外置驱动板之后用于控制舵机。虽然其摄像头像素比较低，只有30W像素左右，但是对于识别任务来说已经足够，太多的像素点反而不利于移动芯片进行处理，甚至还要因此做下采样，使其分辨率降低，增加图像处理的计算速度。

3 设计与实验

3.1 激光信号识别

3.1.1 OpenMV程序的基本结构形式

在本应用中，OpenMV的作用就是要识别达到目标物上的激光。由于笔者使用的激光是一种红色的可见激光，因此可以通过OpenMV中自带的查找色块函数直接找出环境中的激光。

在OpenMV中，程序的执行方式是以每帧执行一次的方式实现的。换而言之，OpenMV处理视频流的方式，就是不断的拍照，然后对每一张拍到的照片进行图像识别处理，因此，OpenMV中的程序都遵循以下的基本结构形式。

即所有的程序都放在不断的循环之中，以循环的方式进行相应的处理。

3.1.2 激光点识别算法

要使飞行器能够朝激光光点所在的位置正确的飞行，首先就得让OpenMV能够识别出激光。在本应用中，OpenMV的激光光点识别主要采用颜色识别的方式进行相应的识别。

颜色识别是数字图像处理领域中最常用的应用之一，通過颜色色域的过滤，能够把环境中特定颜色的物品分离出来[3]。OpenMV识别到激光光点时其内部机器视觉所能看到的效果如图3所示，左图为测试环境，其墙体立柱上的光点即为激光光斑。右图为OpenMV视野，可以看到激光光斑已经被完全分离出来。

在程序实现上，笔者采用了OpenMV中自带的查找视野中对应LAB色域色块的函数：

其中，red_threshold中存放的是所要查找色块的色域值，在本应用中red_threshold存放的即为激光光斑的LAB色域值。

而采用OpenMV作颜色识别应用，还需考虑的一个影响识别率的重要问题就是环境的光照问题。实验表明，对相同的物品，在不同的光照环境下，摄像头视野中所显示的物品颜色完全不同，而其机器视觉中所识别到的LAB直方图成分也有很大差异。

然而，采用激光光斑则不需要关心这个在大多数OpenMV颜色识别应用中会出现的问题，因为激光本身具有色域固定的性质，而且激光光斑通常亮度会比环境光亮好几倍[4]，因此也更加容易被摄像头识别并分离出来。

但是，在实际应用中，OpenMV的find_blobs函数通常机制是会返回环境中所有包含这种颜色的点的位置以及大小。因此其函数输出量中通常会有很多用不到的色块，实际结果就会给后续的处理程序引入了很多“杂波”，图4（a）所示是find_blobs未经滤波时，输出的激光光点所在位置。

为了去除环境中的这些杂波，必须引入某种形式上的“滤波器”，去将这些杂波滤除。根据笔者的实际测试后发现，可以用查找环境中最大色块的办法来滤去大部分的杂波。因为很多杂波相对于激光的光斑来说其所占的像素值都非常的小，只有1～2个像素，与真正的光斑特征在大小上的相差最为明显。查找杂波的代码如下所示：

其中，blobs为find_blobs函数的输出量。在这里它作为输入量输入到函数中去，而return max_blob则输出经过查找之后最大色块所在的位置和大小。由此实现了基础滤波，滤波效果如图4（b）所示。

然而在实际试验时，发现这种滤波的方法并不足够好。因为有很多时候颜色的干扰是随机猝发的，甚至本来激光光斑被识别在了视野中央，而由于飞行器的突然抖动，光斑会闪到屏幕左上角或者其他位置。而也正是这些猝发性的干扰有时会对飞行器的导航带来灾难性的后果。

为了滤除这些干扰，笔者采取了“帧抛弃”的办法。即每帧都进行一次本帧光斑所识别到的位置以上一帧的位置进行比对，如果位置相差太大，超过所设定的阈值，就抛弃掉这一帧，直接识别下一帧，周而复始，不断循环。

根据实际测试，采用以上两种滤波办法，就已经能够滤除大部分的干扰。并且保证飞行器在进行激光识别和导航时不出差错。

3.2 飞行器导航算法

3.2.1 激光制导思路

在获得了激光光点的位置后，接下来要做的就是要引导飞行器飞向激光光点所在的位置。由于采用的是视觉导航方式，即此时飞行器上装载的OpenMV应该已经能够“看到”光斑的所在。在这种情况下，笔者认为飞行器此时已经是朝向激光所在的位置飞去的，只是需要在击中之前做一些微调。

为了能够精确的击中激光光斑所在的位置，就必须采取某种调整控制方法，参考文献[5]，笔者设定飞行器的调节目标，只要把激光光斑的位置调节到视界的中心之上，即与飞行器的中轴线重合，这样就能够保证飞行器始终朝激光所在位置飞行，并最终击中激光所在位置。

因此，要调整飞行器的飞行轨迹，首先要做的就是要计算出激光光斑与飞行器中轴线之间的差值。根据下面的代码可以求出这个差值。

其中max_blob.cx（）、max_blob.cy（）为识别出的激光色块的中心點坐标，img.height（）和img.width（）为摄像头视野的高度和宽度。在求出了俯仰（Pitch）、滚转（Roll）方向的差值之后，需要把这个差值转化为相应的控制信号传递给控制飞行器矢量电机的两个舵机，以促使飞行器调整自身的姿态。意即需要将位置的偏差量转化为舵机的偏转角度数值。

而为了更好的控制飞行器姿态调整的过程，防止过调，就必须要考虑偏差的变化速度以及系统静态误差的积累。为了做到这一点，笔者引入了PID调节算法用于控制飞行器的位置偏差量。在本应用中，笔者采用增量式的PID控制方法，整个系统为一个单级PID，其控制回路框图如图5所示。

其中，r（t）为偏差量，e（t）为反馈值c（t）与控制信号加和后的量，u（t）为输出给飞行器矢量电机的动作信号。

在最初的偏差量进入了PID系统，并得到最终的调定输出量之后，其输出量还需要转化为OpenMV控制舵机所能接受的角度数值，如下面的代码所示：

其中，Pitch_servo.angle（）和Roll_servo.angle（）分别设定矢量电机控制俯仰和滚转舵机的角度，Pitch _output和Roll _output分别为单级PID计算出的俯仰和滚转动作信号。

3.2.2 滚转方向的灵敏度

在实际开发过程中，还发现滚转方向姿态控制具有左滚转和右滚转灵敏度不同的问题，通常，飞行器左滚的趋势会比右滚更加明显。经过仔细分析之后发现，之所以灵敏度会不同，是因为驱动飞行器飞行的螺旋桨是顺时针，也就是往飞行器右滚的方向旋转的。根据物理学角动量守恒定律，左右滚转时必然会存在一个方向灵敏，一个方向不灵敏的情况。为了克服这种情况对姿态调整所造成的误差，我们对PID算法进行了调定，令飞行器在往左滚和往右滚之时具有不一样的PID系数，右滚的时候PID三环的系数设置比左滚的时候大，这样就完美地解决了滚转方向导致的灵敏度差异问题。

3.2.3 飞行器自身的增稳

除了导航外，飞行器在飞行过程中的整体稳定性也是需要考虑的。为了增加整套飞行系统的鲁棒性，笔者在其APM飞控中增加了飞行器在Pitch（俯仰）、Roll（滚转）和Yaw（偏航）方向的角度限制[6]，当飞行器超过这些角度限制时，会驱动副翼，使飞行器自动回正，以达到增加鲁棒性的目的。同时，飞行器本身由于APM内自带的位置式PID，因此能保证了自身的飞行稳定。APM与我们的激光导航系统融合计算，共同控制飞行器的导航制导飞行，保证飞行器在飞行上的稳定和航向上的精度。

4 效果测试

激光光斑识别和飞行器飞行时的实际击中效果如图6所示，其中大的十字线是飞 中 轴线所在位置，小的十字线所在的位置是激光光斑所在的位置。经过测试，这套方案能够保证固定翼飞行器在30米的范围内能够准确的击中激光光斑所照射的位置。其精度半径约达。

5 结论

本文探究了一种基于OpenMV的固定翼激光引导算法。经过试验证明，这套方案能够在30m的范围内引导固定翼飞行器精确击中目标。为OpenMV在固定翼位置定位上的应用以及视觉导航上面提供了实际应用范例，探索了不依赖卫星进行导航的一套可行的固定翼制导方案。

参考文献：

[1]Zhen W， Zeng S， Soberer S. Robust localization and localizability estimation with a rotating laser scanner[C]//2017 IEEE International Conference on Robotics and Automation（ICRA）. IEEE， 2017：6240-6245.

[2]张兆龙.基于自抗扰控制的飞机飞行姿态控制研究[D].南京航空航天大学，2016.

[3]邹浩，郭雨婷，李佳盈，等.基于OPENMV的色彩引导机器人系统研究[J].科技资讯，2018，16（25）：85-86.

[4]梅妍玭，傅荣.基于OpenMV的3D定位识别系统[J].新技术新工艺，2018（02）：50-52.

[5]韩鹏，耿增显，刘宏，等.无人机视觉识别与移动追踪技术综述[J].飞航导弹，2018（10）：24-28.

[6]秦宁，胡立夫，耿家乐.基于可变数字目标识别的四旋翼火灾监测系统[J].中国科技信息，2019（01）：93-95