基于粒子群圆检测的无人机目标定位算法

李文强　王嘉栋　隋国荣

摘  要：无人机在室内等复杂环境中飞行时，存在GPS信号较弱、惯性传感器累计误差较大等问题，导致无法实现室内的精准定位。本文提出一种基于粒子群圆检测算法的无人机目标定位方法，该方法通过OpenCV视觉模组进行图像预处理，并通过增量式PID（Proportion Integration Differentiation）与图像滤波相结合的粒子群圆检测算法获得目标标靶的核心坐标与半径等参数，通过对运动的实时控制、增加判断条件等提高无人机的标定效率与准确率。实验结果表明，该方法能够将无人机的位置调整时间控制在4 s以内，且将标定准确率提高到了90%，极大地缩短了无人机的标定时间，提升了无人机的位置标定准确率。

关键词：粒子群圆检测;增量式PID算法;图像处理;目标定位;运动控制

中图分类号：TP272     文献标识码：A

UAV Target Positioning Algorithm based on Particle Swarm Circle Detection

LI Wenqiang， WANG Jiadong， SUI Guorong

（College of Opto-electronic Information and Computer Engineering， University of Shanghai for Science and Technology， Shanghai 200093， China）

1935021414@st.usst.edu.cn; wangjd_0618@163.com; suigr@usst.edu.cn

Abstract： When flying indoors or in other complicated environments， UAVs （Unmanned Aerial Vehicle） usually have problems such as weak GPS signals and large cumulative errors of inertial sensors， which make it impossible to achieve accurate indoor positioning. This paper proposes a UAV target positioning algorithm based on particle swarm circle detection algorithm， which preprocesses the image through OpenCV visual module， and obtains parameters such as the core coordinates and radius of the target through particle swarm circle detection algorithm combined with incremental PID （Proportion Integration Differentiation） and filtering.The calibration efficiency and accuracy of UAV are improved by real-time control of motion and the increase of judgment conditions. Experimental results show that the proposed algorithm can control the position adjustment time of the UAV within 4 seconds， and increase the calibration accuracy to 90%， which greatly shortens the UAV calibration time and improves the UAV position calibration accuracy.

Keywords： particle swarm circle detection; incremental PID algorithm; image processing; target positioning; motion control

1   引言（Introduction）

隨着人工智能和集成电路等前沿技术的快速发展，智能系统广泛应用于各个领域，智能产品也更加受到人们的青睐。四旋翼飞行器作为集传感器、计算机、自动控制等多学科领域为一体的智能化交叉产品，可适用于各种飞行场景，具有灵活度高、安全性好的特点[1]。

一般来说，当前四旋翼无人机的自主定位方法分为四种：基于惯性传感器自主定位方法、基于GPS自主定位方法、基于惯性传感器与GPS组合自主定位方法、基于机器视觉自主定位方法[2]。由于机器视觉具备定位精度高、成本低、逻辑简单等特点，近年来国内外对无人机在机器视觉自主定位方向的研究也逐渐深入。2014 年，QUINTERO等人利用单目摄像头结合博弈论、随机最优控制等理论实现了无人机对随机地面目标的识别跟踪[3];同年，中国科学技术大学的郑伟研究了基于特定双圆目标、单目视觉的飞行器定点悬停，以及基于光束平差法的飞行器导航算法[4]。

本文针对基于机器视觉定位的室内智能无人机，提出了一种新的飞行控制算法。实验测试结果显示，该飞行控制方式能使无人机在4 s内完成自主定位，并能够将定位精度提升到90%左右;使无人机系统可以实现实时高精度的目标识别，为无人机在未来无人配送、目标识别以及工业机器人等应用方向的研究打下了理论和实践基础。

2   无人机系统硬件设计（System hardware design of UAV）

2.1   系统控制器模块

本系统采用ARM公司生产的STM32F407VET6为飞行控制器的主控芯片，控制无人机系统四路PWM信号输出，以维持整个机身飞行的平顺性。如图1所示为本系统采用的飞行控制器设计模块。采用STM32F407VET6为主控芯片，控制物料投放装置应用DMA技术获取二维激光雷达实时返回数据，并能通过上位机实时监测飞行时的加速度、角速度、磁通量等数据。该型号的芯片成本低，外设资源丰富，功耗低，运算速率快。

2.2   系统供电电路设计

本系统采用4S航模锂电池供电，标称电压为14.8 V。鉴于电调工作时电流较大，故而采用并联形式进行分流。为了保证树莓派、飞行控制器等正常工作，另外使用DC/DC降压模块完成5 V电压的输出。图2为无人机的电能流向图。

3   飞行控制算法（Flight control algorithm）

无人机在飞行时会受到返回气流等外界因素的影响，且这些外部影响往往是不可抗拒的[5]，故而需要光流、对地激光等传感器实时返回数据至飞行控制器。后者再通过算法解算，向四个电调输出PWM信号，动态调整电机转速，实现无人机定点悬停功能。因此，在OpenCV模块精准定位圆靶位置后，仍需通过算法调整机身位置，直至稳定降落到投放区域为止。

3.1   目标视觉处理

相比于简单的HSV直方图、高斯模糊等图像处理方法，本设计采用一种能够对图像进行检测，同时结合高斯滤波器的导向滤波算法对标靶图像的噪声进行滤波[6]。由此算法处理后的图形如图3（a）所示，再经过去噪、自适应阈值处理后，可得到二值化处理后如图3（b）所示的标靶图像，由图3可见处理效果明显。

在得到帧率较高的二值化图像后，无人机需要在5—6 s实现“稳、精、准”地飞行至圆心正上方。因此，快速定位圆心点、实时返回像素坐标值至飞行控制器便显得尤为重要。由于霍夫圆检测方法的计算量大、耗时较长，故而使用一种粒子群圆检测的算法。该算法通过设置最大迭代次数与阈值等参数，解决粒子陷入局部最优等问题，以此检测出最佳圆的位置与半径[7]。通过该算法检测最小的圆，将其用红色边缘线框出，并将圆心用红点标出，效果如图4所示。通过多次实验测试可知，使用该算法能在得到同等结果的条件下耗时更短，进而加快无人机执行任务的速度。

3.2   相对坐标调整

无人机定位过程中，视觉模组将对拍摄获得的每一帧图像进行处理。一般地，以图像左上顶点作为原点O建立以像素为单位的u-v坐标系，像素点在此坐标系下的u坐标值、v坐标值分别代表像素点所在的行数与列数。

同时，要精确标定无人机的目标点，还需要引入以物理单位（例如毫米）表示的图像坐标系x-y。一般地，以相机光轴与图像平面的交点（一般位于图像平面的中心，也称作图像主点）作为原点，并且x轴、y轴分别与u轴、v轴平行。设、分别为x轴、y轴的物理尺寸，假设原点在u-v坐标系下的坐标为，则图像中每个像素点在u-v坐标系与x-y坐标系之间有如下关系：

图5显示了在某定点投递目的的识别中的图像坐标系，通过公式（1）便可得到目标像素点在x-y坐标系下的坐标值;再通过实际测量中心圆环的半径，由小孔成像原理便可获得无人机定点时x方向、y方向上的可接受误差距离、;最后通过公式（2），便可获得u-v坐标系下无人机定点时可接受的误差像素点数、。

3.3   增量式PID及数据滤波

在OpenCV将目标坐标点返回至飞行控制器后，后者会根据坐标点进行姿态调整，进而移动至目标点完成投放。此时要借助增量式PID作为系统自调整方法，完成无人机姿态的实时变换。增量式PID相较于普通的PID方法，其控制增量的改变量只与三次采样值相关，通过简单的加权处理就能够得到较好的效果，对系统的算力要求更低[8]。此外，增量式PID规避了系统累计误差可能造成系统崩溃的问题，更加稳定、可靠。

公式（3）中，为第次所得到的误差，为常系数，、为周期，为控制增量。

然而，在实际物料运输中，仅依靠增量式PID算法会导致物料投放位置在一定范围内出现随机性，可重复性较差。因此，综合考虑机身机械结构、环境风力等因素的影响，本设计提出一种median-average滤波方法。此滤波方法能够在一定时间内迅速移动至目标点并且减小环境以及自身难以完全自稳的影响——使用“中位值滤波”+“算数平均滤波”的滤波方法对增量式PID计算得到的姿态解算速度进行滤波[9]，即对每n 个数据进行排序后去除其中的最大值与最小值，再對剩下的数据求取平均值，返回飞行控制器实时的速度值并传递给飞控。进而，无人机会以该速度进行位置调整。在下一次读取数据进行计算时，所得到的数据会作为n 个数据中的最后一个，同时为了保证数据个数的一致性，系统会自动将上次所得数据的第一个去除。

其中，第k 次的n 个数据为：;、分别代表第k 次得到的n 个数据中的最大值、最小值。

该种滤波方式可以消除系统存在的偶然误差及噪声，对于无人机的运动控制而言是较为合理的。

下面以u-v坐标系中u轴方向，并假设目标u=50、数据滤波n=5进行Simulink仿真，可得如图6所示的定位中坐标变化仿真图。从图6可以得出：系统在6 s的时间内即可完成动态调整，保持在目标量附近微小振荡。

3.4   完成判断条件

在保证精准性的前提下，也应考虑到无人机在执行任务中的高效性。因此，需要增加一个判断条件，即：累计坐标值保持在可接受的误差范围内的计数值，反复更新每次计数值的增加量，若两者同时满足条件则无人机开启降落模式。

通过公式（2）推导得到无人机在一次实验条件下u-v坐标系中可接受的误差像素=5，假设无人机总体计数值超过140、单次计数值增长量超过50，则满足判断条件直接降落。如图7所示，设右方参考轴数值代表计数值，（i=1，2，3，4）段表示OpenCV获取的坐标在误差范围内，在无人机识别到目标图像、动态调整约4 s后，坐标值已经能完全保持在误差范围内波动。考虑到风速、机体自身机械结构等影响，在实际情况下无人机难以如图7所示将误差维持在更小的范围内，因此加入判断条件不仅能提升无人机的定点效率，也能在一定程度上提高无人机定点的精确性。

3.5   降落投放物块

无人机飞行到指定位置附近并调整位置使其满足判断条件后，会触发一键降落指令。在降落完毕后，以STM32F407VET6为核心的飞行控制器会向主控芯片发送“OK”指令。主控芯片接收到信号后，会输出一路PWM波信号，驱动投放装置中的9g舵机旋转90°，使物块平稳落在标定点。

4   测试结果（Test result）

4.1   测试场地图设计

为测试本文提出的室内无人机运动控制方法的精确性与高效性，特别设置了如图8所示的测试场地图。场地中包含边长为500 mm的正方形起降区“H”，最大圆环直径为500 mm、最小圆环直径为100 mm的靶环区（共5 环）。每次测试中，圆靶中心点A将在边长为600 mm的虚形边框内随机取点，无人机将从起降区“H”处起飞，向前飞行直至识别到红色的靶环，然后通过本文提出的运动控制方法标定圆靶中心，最后降落投放测试物块，记录物块落点所在的环数为一次有效测试成绩（落点在最小圆环内记5 环，向外依次递减）。

4.2   测试无人机飞行控制流程图

图9为飞行控制流程图，通过一系列的姿态解算与位置调整，该无人机能够实现迅速识别、精确定位，并稳定降落至目标点处投放物块。

4.3   测试结果

用于测试的无人机轴距为420 mm，且搭配价格适中的电机、电调进行物块投放实验，以验证本文所提出的飞行控制方法对无人机位置标定的准确率。为避免偶然误差，以5 次投放的环数加和作为一次实验的考核标准，记录在表1中。

由表1数据可见，通过上述的飞行运动控制方法可以使无人机90%以上都能将测试物块投放在圆环中心（即视作5 环），仅有少数几次投在了4 环。从总体标定准确率来看，证明通过本文提出的室内无人机飞行控制算法，可以将定位精度维持在90%左右。

5   结论（Conclusion）

本文针对基于机器视觉的室内智能无人机，提出了一种新的飞行控制算法。首先利用导向滤波算法对标靶图像进行滤波处理，然后利用优化的粒子群圆检测算法获得标靶中心位置。为了更好地实现无人机的空中自主定位，通过相对坐标调整，获得无人机可接受的定位误差像素点，利用增量式PID和数据滤波算法，实现在短时间内完成动态调整获得定位坐标，保证定位坐标的精确性，最终实现无人机的准确定位和物资投递。实验测试结果显示，该飞行控制方式能使得无人机在4 s内完成自主定位，并能够将定位精度提升到90%左右;使无人机系统可以实现实时高精度的目标识别，为无人机在未来无人配送、目标识别以及工业机器人等应用方向的研究打下了理论和实践基础。

参考文献（References）

[1] 巩梦岩，孟青，程思强，等.基于STM32的四旋翼自主投靶飞行器[J].软件，2018，39（11）：116-119.

[2] 周克旻.基于Kinect室内四旋翼无人机的定位跟踪与姿态估计[D].北京：华北电力大学，2019.

[3] QUINTERO S A P， HESPANHA J P. Vision-based target tracking with a small UAV： Optimization-based control strategies[J]. Control Engineering Practice， 2014（32）：28-42.

[4] 郑伟.基于视觉的微小型四旋翼飞行机器人位姿估计与导航研究[D].合肥：中国科学技术大学，2014.

[5] 吴晓阳.基于OpenCV的运动目标检測与跟踪[D].杭州：浙江大学，2008.

[6] 刘志强，孙怀远，宋晓康.基于机器视觉的药品包装滤波算法实现[J].软件，2018，39（09）：93-96.

[7] 李福庆，苏湛.粒子群圆检测算法[J].软件导刊，2018，17（01）： 60-63.

[8] 王祎晨.增量式PID和位置式PID算法的整定比较与研究[J].工业控制计算机，2018，31（05）：123-124.

[9] 张龙青，陶娟娟，张羊鸿，等.基于中位值平均滤波算法和OLED屏的PM2.5空气质量检测系统设计[J].信息通信，2016（12）：79-80.

作者简介：

李文强（2000-），男，本科生.研究领域：嵌入式开发.

王嘉栋（2001-），男，本科生.研究领域：嵌入式开发.

隋国荣（1974-），男，博士，副教授.研究领域：光电检测，机器视觉.本文通讯作者.