基于STM32的目标跟踪无人机整体及飞控系统控制算法设计与研究

陈斌 施伟 原鑫鑫 马原

摘要：本次设计依据四旋翼无人机实际运行测试中的需求，论述了一种基于STM32的目标跟踪无人机整体及飞控系统控制算法设计方案，包括系统软硬件设计、飞控系统控制算法设计、系统动态飞行实验测试等方面。测试结果表明系统可通过各个模块的配合实现对飞控及电机的精确控制，具有相应速度快、定位误差小、运行较为稳定等特点。

关键词：STM32;飞控系统;实时跟踪;软硬件设计

中圖分类号：TP212 文献标识码：A 文章编号：1007-9416（2019）12-0175-04

0 引言

在人工智能和中国制造2025的大背景下，智能无人机已经从概念化走向商业化，市场份额不断扩大，发展迅速，国内无人机技术已经取得了部分成果，但与发达国家相比较仍具有差距，未来无人机可为影视跟踪拍摄、足球赛事判罚和濒危动物生活习性监控等提供了一个新的解决方案，智能无人机的应用市场前景很广阔。在这些应用领域中，对无人机提出了小型化、长航时等新的性能要求，面对这些新需求，无人机的整体架构及飞控系统也需要进行相应的改进设计，以满足无人机飞行时的控制性能要求，因此，对于小型无人机整体及飞控系统的设计与研究将对无人机的进一步应用具有重要的意义。

1 系统硬件设计

为了实现完善的姿态控制，需要多传感器参与控制反馈，由主控制器收集信息并综合所有传感器信息产生控制量，将控制量传递给电机调速器，从而实现对无人机姿态、位置、高度的控制。本次设计根据四旋翼飞行器飞行原理，建立系统动力学模型，选用飞越650机架作为机体，通过好盈乐天40A电子调速器控制4个无刷电机转速来实现无人机的欠驱动系统飞行。系统以STMF407作为四旋翼自主无人机控制的核心，由锂电池模块、电机调速控制模块、传感器检测模块、飞控模块等构成。飞行控制模块包括角度传感器（陀螺仪和加速度计）、磁力计、气压计，传感器检测模块包括TOF线性激光传感器、光流模块、PMU电源管理模块。此外，选取Jetson TX2嵌入式开发板应用改进的KCF算法在该部件上进行实时地目标跟踪，如图1所示为无人机控制系统硬件总体方案。

2 系统软件设计

本次软件系统设计所采用的STM32F407ZGT6主控芯片软件开发环境在MDK-ARM 5.15开发环境下以C与C++混合编程，并配置相应的外设实现姿态解算、运动控制。

2.1 设计原则及流程

本次作品的软件设计主要分为两部分：飞控模块的软件设计和目标实时跟踪的软件设计。飞控模块是基于STM32F407ZGT6微处理器的软件程序设计，目标实时跟踪的软件设计是应用机器学习算法中改进的KCF算法。

首先对飞控模块进行初始化，单片机进入定时器中断循环，STM32F407ZGT6单片机采用icm20602传感器通过SPI协议读取icm20602中的陀螺仪、加速度计数据，每2ms进行一次姿态解算，得到欧拉角，每4ms进行一次单片机的控制运算，输出最终的PWM值，进而完成飞行姿态控制回路的设计，使无人机稳定飞行;通过单片机获取线性激光传感器和气压计的高度数据，将两者的数据进行融合后，每隔10ms进行一次高度环的计算，从而完成飞行高度控制回路的设计;单片机读取光流传感器和Jetson TX2通过串口发送的目标位置信息数据，每隔20ms将两者数据进行互补融合运算，从而完成飞行位置控制回路的设计。如图2所示为四旋翼无人机控制系统的总体方案。

无人机的飞控软件运行于STM32F407ZGT6硬件电路，基于ST标准外设库开发。

主要实现下列功能：

（1）读取传感器信息：通过串口读取姿态传感器数据信息;通过串口读取光流传感器与TOF线性激光传感器信息;通过SPI读取气压计信息。

（2）接收机信号收取：遥控机接收机采用高电平的时间长度来承载信息，根据接收机品牌不同，更新频率从50Hz到500Hz不等，高电平时间长度通常为1ms～2ms。利用外部中断对高电平时间进行测量，从而获取接收机信息。

（3）控制电机调速器与云台控制器：电机调速器与云台控制器均接受与接收机相类似的数字信号，用高电平时间长短来判断控制量的大小。采用内部定时器产生4路PWM波形用于控制六个电机转速，通过2路PWM波形实现对于云台俯仰、横滚的角度控制。

（4）与地面站通讯：通过数传模块进行双向通讯。打包机上的飞行信息发送给地面站，接收地面站传来的指令并正确执行。

（5）数据滤波：运用数字滤波方法对传感器测得的数据进行处理，主要目的是降低信号中的噪声水平，排除震动等因素对于传感器造成的影响。

（6）控制量解算：根据反馈与给定之间的偏差，计算位置、速度、高度、爬升速度、姿态角、姿态角速度等控制量。

2.2 基于EKF姿态解算算法设计

在无人机控制系统中，姿态信息是最基本的导航参数，大多的控制算法都是以姿态信息的准确性为前提的。本次设计采用扩展卡尔曼滤波器（EKF）来对速率陀螺、加速度计和磁力计的数据融合得到优化的姿态信息。

在导航坐标系的原点选在无人机重心处，取轴指向北，轴指向东，轴沿垂线方向指向天，即东北天坐标系。在载体坐标系的原点位于无人机重心处，沿机体横轴指向右，沿机体纵轴指向前，沿机体的竖轴指向上，满足右手定则。、、分别为无人机的航向角，俯仰角和姿态角。

由载体坐标系到导航坐标系转换的姿态矩阵为：

（1）

由四元数来表示的姿态矩阵为：

（2）

根据姿态矩阵的元素，可以确定（、、）的值，其中，和需要根据公式（3）对主值进行判断。

（3）

（4）

2.3 实时控制算法设计

在动力学模型的基础上，将四旋翼无人机实时控制算法分为三个PID控制回路，即姿态控制回路、高度控制回路和位置控制回路。

2.3.1 姿态控制回路设计

当四轴无人机正常飞行时，突遇外力（风等）或磁场干扰，使加速度传感器或磁力传感器采集数据失真，造成姿态解算出来的欧拉角错误，只用角度单环情况下，使系统很难稳定运行，因此可以加入角速度作为内环，角速度由陀螺仪采集数据输出，采集值一般不存在受外界影响情况，抗干扰能力强，并且角速度变化灵敏，当受外界干扰时，回复迅速，姿态控制回路如图3所示。

2.3.2 高度控制回路设计

高度控制是指通过改变四旋翼无人机的总升力，使得无人机的飞行高度达到并维持在给定高度的过程。高度控制的难点在于准确地获取高度信息。高度控制通过TOF线性激光传感器、加速度计、气压计数据进行数据融合得到数据，从而获取高度反馈。其中，TOF线性激光传感器具有极佳的测距精度，且不受气流的影响，但是只能进行短距离测距;气压计测距范围不受限制，且可以提供极佳的相对高度信息，但是气压计存在漂移现象，随着温度或阵风，其输出均会受到一定的影响;加速度计由于机体震动的影响，输出量中的噪声部分比较高，但是加速度计具有高动态与高灵敏的特点，可以对低频数据（如10Hz的超声波数据）进行插值计算，使低频数据也可以获取高动态的输出特性，高度控制回路设计如图4所示。

由于TOF线性激光传感器与气压计均有测量高度的功能，而加速度计可以测出机体竖直方向的加速度，因此，可以融合TOF线性激光传感器、气压计与加速度计的输出结果得到高度估计值。气压计用于高空高度数据的测量，TOF线性激光用于低空高度数据的测量。无人机在高空和低空飞行时高度数据的测量可以智能切换。

首先利用所测得高度数据与加速度计测出机体竖直方向的加速度进行一阶互补融合得到一个对地速度的估计值，然后利用该对地速度的估计值与气压计原始数据进行一阶互补融合得到最终的高度估计值，一阶互补融合根据如下式子计算：

（5）

式中：

：当前时刻融合得到的值

：上一时刻融合得到的值

：当前时刻高度测量值

：变化量

：计算权重

：计算时间间隔

在实际计算中可以根据加速度计和气压计的可信赖程度调整计算权重，从而使估计值更快的收敛。通过两次互补融合，可以得到一个比较可靠的对地速度的估计值和高度估计值。

2.3.3 位置控制回路設计

目标水平位置信息通过串口发送给飞控模块，通过飞控模块读取光流传感器对应的水平速率，对水平速率v（t）通过公式（6）得到水平位移s（t），将计算的水平位移s（t）和经过跟踪算法计算出的目标水平位置信息x在飞控模块中运用公式（7）进行互补融合，得到精确的位置信息y，将该结果作为位置环的外环控制回路，将通过飞控模块读取光流传感器对应的水平速率作为位置环的内环控制回路，进而完成位置控制回路的设计，如图5所示。

（6）

（7）

式中：

a：互补融合系数，取值区间[0，1]

3 系统动态飞行实验测试

在进行动态飞行测试的时候要对无人机的电源线，电调线焊锡处进行检查，检查电池容量及各接线情况，同时确保桨安装的正确。做好相应的准备工作。在进行电调飞行实验时，选择空旷无人且有草坪的地方。

3.1 飞行测试

将无人机置于水平地面，插电源插头，上电，操作者远离无人机一段距离，油门杆呈内八字（或外八字）形，看到LED灯慢闪，表示飞控已经解锁，之后慢慢提高油门，将无人机飞到适当高度，然后保持当前油门不动，理论上这时候无人机的升力和重力相等，无人机实现悬停。如图6无人机定点飞行现场测试图。

3.2 翻滚测试

推动油门，无人机爬升，保持合适高度，将无人机往左飞行一段，回中保存，再将无人机往右飞行一段，回中保存，对应的翻滚曲线如图7所示。

3.3 俯仰测试

推动油门，无人机将慢速上升，拨动遥控器方向杆，将无人机往前飞行一段然后回中保存，往后方飞行一段距离，对应的俯仰测试曲线如图8所示。

4 结语

本次设计依据四旋翼无人机实际运行测试中的需求，论述了基于STM32的目标跟踪无人机整体及飞控系统控制算法设计与研究，结合小型无人机的飞行特点和研发条件，对设计流程进行优化，并对EKF姿态解算和实时控制算法进行设计，在动力学模型的基础上，将无人机实时控制算法分为三个PID控制回路，即姿态控制回路、高度控制回路和位置控制回路。并完成系统动态飞行试验测试验证。测试结果表明系统可通过各个模块的配合实现对飞控及电机的精确控制，具有相应速度快、定位误差小、运行较为稳定等特点。

参考文献

[1] 刘焕晔.小型四旋翼飞行器飞行控制系统研究与设计[D].上海：上海交通大学，2009.

[2] 杨光祥.STM32单片机原理与工程实践[M].武汉：武汉理工大学出版社，2013.

[3] 李宁.基于MDK的STM32处理器开发应用[M].北京：北京航空航天大学出版社，2008.

[4] 单海燕.四旋翼无人直升机飞行控制技术研究[D].南京：南京航空航天大学，2008.

[5] 王永.反电动势无刷直流电机控制系统研究[D].南京：东南大学，2004.

[6] 姚元鹏.四旋翼直升机控制问题研究[D].哈尔滨：哈尔滨工业大学，2007.

[7] 芦勤桑.无人机遥测遥控地面站系统研究[D].南昌：南昌航空大学，2012.

[8] 李海泉.小型無人机飞行力学建模及虚拟训练平台的建立[D].南京：南京航空航天大学，2012.

[9] 尚何章.小型无人机飞行控制系统硬件设计与实现[D].南京：南京航空航天大学，2012.

[10] 胡占双.无人机飞行姿态检测及控制研究[D].沈阳：沈阳航空航天大学，2013.

Design and Research of  STM32 Target Tracking UAV Integrated and Flight Control System Control Algorithm

CHEN Bin1，SHI Wei2，YUAN Xin-xin1，MA Yuan1

（1.School of  Computer Science and Information Engineering， Shanghai University of  Applied Sciences， Shanghai  201400;

2.School of  Automotive Engineering， Jiangsu Automobile Technician College， Yangzhou  Jiangsu  225000）

Abstract：Based on the requirements in the actual test of the quadrotor drone， this design discussed a STM32-based target tracking drone overall and flight control system control algorithm design scheme， including system software and hardware design， flight control system control algorithm design， system dynamic flight test and other aspects. The test results showed that the system could achieve precise control of flight control and motor through the cooperation of various modules， which had the characteristics of fast speed， small positioning error， and stable operation.

Key words：STM32; Flight control system; real-time tracking; software and hardware design