0 引言

    四旋翼飞行器对地面移动目标的追踪在军用、民用及商业化应用中发挥着越来越重要的作用^[1]，对敌军车辆、坦克实时跟踪并侦查，以获取敌军行踪；对灾后的人员搜救，以获取实时灾情；对快速移动的物体进行拍摄，以获取可靠的影视资料等，均涉及飞行器对地面移动目标的追踪。李永健等人给出了采用OpenCV处理图像获取反馈信号的方案^[2]，但OpenCV运行需机载计算机或采用上位机，不适合于小型嵌入式系统；文献[3]给出了采用光流法对运动目标进行检测的方案，但由于光流法更多地是检测移动目标的速度，无法检测地面移动目标相对于飞行器的位置，追踪效果不理想；文献[4]给出了采用ADAMS与MATLAB仿真的多回路PID位置跟踪控制的模型，但该模型的实现较为困难，需运行速度较快的处理器。文献[5]给出了双闭环轨迹跟踪与控制的方案，提高了飞行器追踪地面移动目标的鲁棒性；文献[6]采用OpenMV制作了全自动模拟目标搜救系统，为飞行器追踪地面移动目标提供了新方案。

    要完成飞行器对地面移动目标的快速追踪，需满足以下要求：首先，飞行器本身运动控制须非常灵活，响应迅速，滞后少；接着，要为控制系统提供必要的反馈，主要是运动目标与飞行器的位置偏差以及运动目标的移动速度；最后，需根据反馈对飞行器做出可靠的控制。

1 系统响应速度提高

    为了实现对地面快速移动目标的实时追踪，飞行器的响应速度必须较快，当地面目标移动时，飞行器应当能够根据地面目标的位置变化实时追踪。如果飞行器的响应速度较慢，当地面目标移动速度较快时，飞行器未能实时追踪，地面目标可能已经偏离摄像头视野，失去反馈信号，继而失控。主要从以下两方面提高飞行器打舵响应速度。

1.1 姿态PID控制的改进

    通常，飞行器姿态控制为串级PID控制^[7]，控制流如图1所示。

    飞行器在悬停状态下，PITCH、ROLL的姿态角都应当为0，当飞行器做出追踪地面移动目标的动作时，PITCH、ROLL姿态角应当转到一定的角度，才可完成追踪，判断飞行器响应速度的标准也就为当飞行器从0°姿态角转到一定角度姿态角的时间，如果这个过程时间越短，则飞行器的响应速度就越快，反之越慢。为了减少该过度过程的时间，提高响应速度，在外环角度PID控制器上做出了改进，外环角度PID控制器的表达式为^[8](1):

其中，u(t)为输出的角速度，K_p为比例系数，K_i为积分系数，K_d为微分系数，e(t)为期望的姿态角和实际的姿态角的偏差。式(1)中，微分项代表姿态角偏差变化的速率，当期望的姿态角为零时，姿态角偏差变化的速率也就是姿态角变化的速率，姿态角变化的速率为角速度，而陀螺仪输出的原始数据即为角速度。因此，可用陀螺仪的原始数据代替外环角度PID控制器的微分项，如式(2)所示：

其中，Gyro为陀螺仪的原始数据，Gyro_x代表着ROLL方向的角速度，Gyro_y代表PITCH方向的角速度。陀螺仪输出的原始数据动态响应性能好，能实时地反映飞行器的角速度。相比采用角度偏差的微分作为微分项，陀螺仪输出的角速度作为微分项更能体现飞行器姿态角的变化趋势，形成有效的震荡阻尼，进而抑制飞行器的震荡。

1.2 参数的整定

    整定合适的PID参数，可提高飞行器的动态响应性，串级PID中，内环为角速度控制，在角速度控制环中，期望的角速度为外环输入的角速度，实际的角速度为陀螺仪输出的角速度数据，输出的是PWM脉宽直接控制电机。因此该控制环既控制着角速度又直接控制电机，是最基础、最重要的控制环，内环应当根据角速度偏差输出电机转速及时修正角速度，能否及时修正角速度偏差由该控制环决定，飞行器的动态响应性能也由该控制环决定。在整定该控制环参数过程中，可适当增大K_p值，当系统产生等幅震荡时，加入K_d；当加入K_d能抑制系统的等幅震荡时，继续加入K_p，直到加入K_d不能抑制系统的等幅震荡为止。采取此临界状态的K_p作为内环的比例系数，采取能抑制住震荡的K_d作为内环的微分系数。在抑制住系统的等幅震荡之后，如果系统仍然以一定的角速度转动,则适当加入积分项，以能消除系统的静态误差时的K_i作为微分系数。以绕陀螺仪y轴的角速度作为参考，在整定合适的参数K_p=1.5、K_i=0.5、K_d=1.6后，PITCH姿态角的角速度期望值为0时，绕陀螺仪y轴的角速度变化如图2所示，考虑到陀螺仪数据会有零点漂移^[9]，以及受电机震动的影响，角速度变化数据在0上下±50°/s的波动是可以接受的。

    当给内环一个期望的角速度时，内环角速度变化数据如图3所示。

    外环则为角度控制环，在角度控制环中，输入的期望值为视觉追踪所期望的姿态角，输入的实际值为姿态解算的实际姿态角，输出的量作为角速度输入给内环。在角度控制环中，外环角度的变化通过内环角速度的变化去改变，即外环负责根据姿态角的偏差计算出应当以多大的角速度去修正该姿态角偏差，内环负责根据角速度的偏差计算出应当以多大的电机转速去修正该角速度偏差。外环主要控制对象为姿态角，而姿态角改变是视觉追踪的基础，因此，外环决定了飞行器能够追踪移动速度多快的目标。在内环的基础上，外环参数整定做到动态响应即可，先增大K_p值，当飞行器能够迅速修正姿态角偏差时，增大K_d抑制震荡；当姿态角存在静态误差时，增大K_i修正静态误差^[10]。整定参数K_p=0.8、K_i=0.05、K_d=0.6后，打舵测试如图4所示。

    如图4所示，当期望的姿态角发生改变时，能以较大的角速度去修正姿态角偏差，迅速响应，过程过渡时间短，系统基本无震荡，达到了较为理想的状态。

2 移动目标位置获取

    拥有反馈是一个系统闭环控制的重要条件，在飞行器追踪小车控制系统中，飞行器要获取小车的位置最简单有效的方式为判断小车在摄像头视野范围的位置。期望的位置为(0，0)，当小车在摄像头视野范围中心时，小车在摄像头视野范围的位置为（0，0）；若小车不在中心位置，则期望位置与实际位置存在偏差，需通过PID控制修正该偏差，以此达到追踪小车的效果。

2.1 OpenMV简介

    Python作为脚本语言，无法进行底层的操作，而MicroPython是一个Python3的语言解析器，非常小巧，只需要256 KB的代码空间及16 KB RAM空间，并针对微处理器受限的环境进行了优化，借助MicroPython，32位的ARM处理器(比如STM32F405)就可以采用Python进行底层操作。OpenMV是一款基于STM32F765和OV7725摄像头且安装了MicroPyhon解析器的开源机器视觉模块，其底层驱动和图像处理库用C语言进行封装，并参考了部分OpenCV库，预留接口供MicroPython调用。采用OpenMV进行开发，可通过Python脚本借助MicroPython调用底层驱动与图像处理库，可节省开发周期，同时封装好的驱动层与图像处理库降低了嵌入式与图像处理开发的门槛^[11-12]。

2.2 OpenMV获取小车位置的方法

    在OpenMV内置的图像处理库中，包含了寻找色块的库函数find_blobs()，该函数对于彩色图像而言，输入LAB参数值，可输出找到的各个彩色色块中心点在摄像头视野范围的位置；对于灰度图像而言，输入的是灰度阈值，可输出找到的各个灰色色块中心点在摄像头视野范围的位置。当处理彩色图像时，确定LAB参数值是关键，若参数选择不当，则可能无法识别目标色块，在实际的参数确定过程中，需多次试验，确定目标色块的LAB参数范围。图5为确定绿色LAB参数值的一次仿真，在次仿真中，L的范围为35～65，A的范围为-60～-25，B的范围为0～35。在该仿真中，并无噪声影响到对绿色色块的识别，是一个较为理想的参数，图6为在实验过程中，由该LAB参数对绿色色块的识别。

    当处理灰度图像时，由于灰度图像每一个像素点的颜色只需要用灰度值来区分，其中，每一个像素点的取值范围为0～255，当小于设定阈值时，判定为黑色；反之，判定为白色，调整该阈值即可区分黑色色块与周围环境。

    由于LAB色彩模型基于人对颜色的感觉，L代表亮度，A代表从洋红到绿色的范围，B代表从黄色至蓝色的范围[13]，因此，该色彩模型适合于人眼容易区分的色彩，不适合对黑色色块的捕捉。但实际环境是复杂的，当需要捕捉黑色色块时，采用灰度图像，可提高捕捉色块的鲁棒性。彩色图像存储维度比灰度图像大，因此处理速度也较慢，但实验测试发现，使用OpenMV3硬件平台，采用QQVGA图像，find_blobs()函数处理灰度图像可达到约60帧/s，处理彩色图像可达到约40帧/s，均能达到实时控制的需求。因此，可根据实际情况采用灰度图像或彩色图像，具体处理流程图如图7所示,计算色块坐标的方法如图8所示。

    如图8所示，由OpenMV捕捉到小车位于图像第n行、第m列个像素点之后，可计算出小车的坐标(x，y），这就是系统的反馈数据。得到该数据后，将该数据发送至飞控，在追踪小车过程中，期望的小车的位置始终是（0，0）。

3 追踪控制实现

    在获取到系统反馈的基础上，可根据反馈信号对系统进行控制，主要是修正期望位置与实际位置的偏差，即可实现对小车的跟踪。而PID控制是基于修正偏差的控制，能够有效修正偏差，但作为一个追踪的控制过程，还需要考虑到追踪的速度。如果小车速度较快，仅仅考虑到位置控制则可能追不上，因此，本文还设计了位置-速度的双环PID控制。

3.1 单环控制的实现

    若仅考虑到小车位置变化，飞行器及时调整姿态角，从而改变飞行器的位置，可达到追踪目的，具体控制流程图如图9所示。

    采用单环位置控制时，调整PID参数K_p=0.6、K_i=0.16、K_d=0.43时，飞行器移动坐标的X轴、Y轴如图10、图11所示，在小车静止或移动速度较慢的情况下，飞行器能够悬停在小车上方，可对移动速度较慢的小车进行追踪。

    在图10中小车坐标X轴数据在-15左右上下波动，在图11中坐标Y轴数据在15左右上下波动，而不是在理想的0值上下波动，存在静态误差。形成该静态误差的主要原因是加速计数据存在较大误差。加速计每次上电后的误差都有一定的变化，随时间偏移变化较小^[14]，而飞行器无法做到每次上电时刻都处于水平状态，加速计Z轴数据不一定等于重力加速度，因此无法上电校准，形成了误差，飞行器此后以该不准确的数据解算姿态，形成姿态的误差^[15]，造成飞行器位置飘动，位置PID控制器在克服由于姿态误差造成的飞行器的位置飘动时，形成了位置控制时的静态误差。

3.2 双环控制的实现

    为了解决在单环控制时小车速度较快、飞行器无法及时追踪的问题，设计了双环控制，控制流程图如图12所示。

    该控制将小车的位置与小车的移动速度作为反馈数据，可有效修正位置偏差的同时修正速度偏差。与单环PID控制相比，双环PID控制可追踪速度更快的小车，并且位置坐标浮动也比单环控制的小，X轴与Y轴的位置数据变化如图13、图14所示。

    将图10与图13进行比较：图10中纵坐标数据上下飘动范围为35左右，图13中纵坐标数据上下飘动范围为20左右，说明在X坐标轴上，串级PID控制使得飞行器飘动比单级PID控制小，串级PID控制比单级PID控制更加稳定。同理，将图11与图14进行比较：图11中纵坐标数据上下飘动范围为30左右，图14中纵坐标数据上下飘动范围为10左右，说明在Y坐标轴上，串级PID使得飞行器飘动比单级PID控制小，串级PID控制比单级PID控制更加稳定。在位置-速度PID控制中，位置PID控制器输出至速度PID控制器，当期望位置与当前位置偏差越大时，输出的期望速度越大，速度PID控制器输出的期望姿态角也越大，飞行器会以更快的速度飞至小车上方，以完成追踪速度较快的小车的任务。综上所述，位置-速度PID控制器不仅提升了飞行器悬停的稳定性，还使得飞行器能够追踪速度较快的小车。综上，在该飞行器控制系统中，从姿态的串级PID控制，到位置的串级PID控制，构成了一个4环PID控制系统。控制流程如图15所示。

4 实验验证

    本次实验采用S500机架，STM32F407处理器作为飞行控制板，整定响应迅速的PID参数，采用OpenMV获取飞行器相对于小车的位置数据，采用串级PID控制飞行器相对于小车的位置。测试飞行器在静止目标上的悬停以及在运动上的跟踪，并测试包含灰度与彩色两种类型的图像。测试步骤如图16所示。

    在图16所示的实验步骤中，先将小车放置在区域8，然后飞行器在区域10起飞，起飞之后，OpenMV采用灰度图像捕捉大黑点，并在大黑点上方悬停，悬停10 s之后，飞行器往区域8方向飞。同时OpenMV采用彩色图像捕捉绿色小车，若未能捕捉到小车，则一直往前飞，直到捕捉到绿色小车为止，飞行器成功悬停在绿色小车上方10 s之后，小车开始在区域1至区域9各个区域移动，飞行器追踪小车，并在区域1至区域9各个区域上方飞行。实验证明，飞行器可有效在大黑点与绿色小车上方悬停，并能成功追踪绿色小车。

5 结论

    本文设计了一种四旋翼飞行器追踪地面移动目标的控制策略，并采用该策略成功实现了飞行器追踪地面移动目标。改良了四旋翼飞行器的PID控制算法，整定了合适的PID参数，成功提高了四旋翼飞行器的打舵响应速度。采用OpenMV获取地面移动目标的位置，并采用串级PID控制器修正期望位置与当前位置的偏差，成功实现了四旋翼飞行器追踪地面移动目标。相比单环PID控制飞行器位置，串级PID控制不仅提高了悬停的鲁棒性，还使得飞行器可追踪移动速度较快的目标。

参考文献

[1] 刘添胜，陈益平，陈凯旋.四旋翼飞行器在台山输电巡维的应用[J].电工技术,2014(2)：65-66.

[2] 李永健.基于机器视觉的四旋冀无人机定点着陆系统设计与实现[D].广州：华南理工大学，2015.

[3] 王卉，李丽霞，刘慧洁，等.基于四旋翼飞行器的光流法动态目标检测[J].沈阳工程学院学报(自然科学版)，2017，13(1)：76-81.

[4] 姜香菊，曾幼涵，刘二林.四旋翼飞行器带速度补偿的多回路PID位置跟踪控制[J].测控技术，2017，36(4)：74-78，84.

[5] 许璟，蔡晨晓，李勇奇，等.小型四旋翼无人机双闭环轨迹跟踪与控制[J].控制理论与应用，2015，32(10)：1335-1342.

[6] 潘丽静，张虹波，周婷婷.全自动模拟目标搜救系统的设计与实现[J].电脑知识与技术，2016，12(28)：178-180.

[7] 贾峰，孙曼，秦磊.基于STM32的四旋翼飞行姿态串级控制[J].科学技术与工程，2015，15(6)：220-225.

[8] 李俊，李运堂.四旋翼飞行器的动力学建模及PID控制[J].辽宁工程技术大学学报(自然科学版)，2012，31(1)：114-117.

[9] 段栋栋.高精度MEMS陀螺仪的滤波算法研究[D].成都：电子科技大学，2014.

[10] 冯庆端，裴海龙.串级PID控制在无人机姿态控制的应用[J].微计算机信息，2009，25(22)：9-10，45.

[11] Pan Lijing，Zhang Hongbo，Zhou Tingting，et al.The design and implementation of automatic simulation target search and rescue system[J].Computer Knowledge & Technology，2016，12(10)：178-180.

[12] ISAKOV A.Using open MVL shell in research and education[C].Manufacturing Modelling，Management，and Control，2013：2191-2196.

[13] 张宏建.Lab色彩模式在图像处理中的应用[J].福建电脑，2011，27(1)：146-147.

[14] 郑海潮.四轴飞行器自主飞行控制及避障系统研究[D].成都：电子科技大学，2016.

[15] 冀亮,钱正洪,白茹.基于四元数的四轴无人机姿态的估计和控制[J].现代电子技术，2015(11)：112-116.

作者信息:

黄瑞敏1，2，李建奇1，2，张  斌1，杨民生1，王文虎1

（1.湖南文理学院 电气与信息工程学院，湖南 常德415000；

2.湖南文理学院 洞庭湖生态经济区建设与发展湖南省协同创新中心，湖南 常德415000）