基于RX23T的四旋翼飞行器控制系统设计＊

曹继华，梁 伟，刘 杰，姚慧雄，向晓燕

(吉首大学信息科学与工程学院，湖南 吉首 416000)

0 引言

近年来，随着微电子技术的发展和控制理论的进一步发展，四旋翼飞行器已发展成为集军用，商用，民用于一体的多功能无人驾驶工具，并不断有新的功能加载到它的上面[1]。

本设计使用了R5F523T5ADFM单片机作为四旋翼飞行器的控制核心，在对获取的图像数据进行分析后，结合PID控制算法，通过编程，仿真和反复的调试，最终实现了四旋翼飞行器对地面运动目标跟踪飞行的功能。

1 四旋翼飞行原理

四旋翼飞行器的四个旋翼都位于同一水平面，飞行器通过控制旋翼上四个电机的转速来改变旋翼升力的大小，从而实现四旋翼飞行器姿态稳定。相邻位置螺旋桨的旋转方向是相反的，正好抵消了四旋翼飞行器的正反力矩，因此可以保证四个电机同时转动而机身不转动。当某两个相邻的电机转速减小而另一对相邻电机转速增大，则会产生向某个方向运动的速度[2]。本设计通过测量器件感知飞行器姿态发生变化，将测量到的姿态信息发送到处理器处理，经过控制器解算后输出到执行机构，控制了电机的转速，使得飞行器按照期望的姿态稳定飞行。由于采用的是负反馈闭环控制，所以整个四旋翼飞行器系统会逐渐趋向于稳定的状态。

2 系统的硬件平台设计

硬件平台设计主要采用两块R5F523T5ADFM单片机和若干传感器，其中一块R5F523T5ADFM芯片与姿态传感器模块构成飞行控制板，主要对飞行器的飞行姿态进行控制。另一块R5F523T5ADFM芯片与测距传感器模块，图像传感器模块构成导航数据处理板，将传感器原始数据经处理后上传到飞行控制板，从而对飞行器实现导航。系统硬件组成如图1所示。

图1 系统组成框图

2.1 主控芯片的选择

本系统的硬件平台采用R5F523T5ADFM单片机作为控制核心，其具有一个32bit的CPU核心,4Gbyte寻址空间，内含16个通用寄存器，10个控制寄存器，2个72位累加寄存器，最高系统时钟可达40MHz。同时该芯片集成了128KbyteROM，12KbyteRAM，以及各种片上外设。具有丰富的接口以满足与各种传感器的连接通信，和较强的性能使得从传感器采集的数据得到实时处理。

2.2 姿态传感器模块的选择

采用MPU9250芯片内置的三轴加速度计，三轴陀螺仪以及三轴磁力计获取原始姿态数据，经解算后得到飞行器的姿态数据。其中陀螺仪可编程其量程为（±250，±500，±1000度/秒），可进行三轴（x，y，z）16位ADC角速度数字输出。加速度计可编程其量程为（±2g，±4g，±8g，±16g），可进行三轴16位ADC加速度数字输出。

2.3 测距传感器模块的选择

采用KS103超声波模块对飞行器飞行高度进行测量。该超声波模块包含实时温度补偿的距离探测，具有高探测精度。探测 频率可达500Hz，使用 I2C/串口串口接口与主机通信，自动响应主机的I2C/串口串行口控制指令，短距探测量程由10cm、20cm、……、至470cm，能满足快速近距离探测，从而实现室内实时处理高度数据。

2.4 图像传感器模块的选择

采用曝光芯片为OV7725，包含一块AL422B板上FIFO芯片的摄像头模块。OV7725感光芯片在低光照情况下也具有高灵敏度，标准的SCCB通信接口，支持RGB彩色模式数据输出，同时具有帧同步能力，采集的彩色图像数据通过8位并口传输给单片机，具有满足实时数据传输的速度，且保证图像信息采集的完整度。用来为飞行器采集地面图像信息，从而使飞行器得以在室内实现循迹飞行，跟踪飞行和定点悬停。

3 系统软件设计

3.1 循迹模块算法设计

为了保证飞行器能够实时有效的获取目标的位置信息，考虑采用机器视觉的方法对运动目标进行定位[3]。采用一块R5F523T5ADFM单片机进行导航控制。导航板通过视觉传感器获取地面的图像数据，经过一定的控制算法处理后得到小车和圆心位置信息[5]，并通过串口发送给飞控主控芯片从而达到循迹的效果。

3.1.1 图像获取

为了能够更准确的获取飞行器当前状态下的所扫描到的赛道信息，将摄像头传感器安装在飞行器下方的中间位置。配置ov7725摄像头的寄存器，将其输出窗口设置成60*80，并且能够自动调节边缘的增强、自动调节噪声的抑制。同时单片机配置相应的输入输出端口和中断触发源，根据摄像头传感器内部产生的场同步信号和行同步信号进行图像采集并保存到缓存区。

赛道是由黑色和白色两种背景颜色组成，故采集时可将图像二值化为0和1两种数值(0为黑色，1为白色)，以便于数据的保存和处理。数学表达式如公式⑴。

阀值(Threshold)用来把目标和背景区分开。由于赛道背景颜色单一，故可以通过采集几帧不同状态下的图像用来提取静态阈值。

3.1.2 目标识别算法

从采集的图像中计算出目标物体的位置信息是飞行器自主飞行过程中核心的部分，目标检测识别算法设计思路如图2所示，主要分为以下三步。

⑴ 构造一个虚拟的初始为3*3的方形矩阵模板，矩阵的大小可以根据扫描的层数N(初始值为1)扩展，定义一个数组分别记录矩阵四个角位置相对于矩阵中心的偏移量。

⑵ 对输入的二值图像进行遍历，当扫描到黑点（数值信息为0）时，则以该像素点的位置为基点，匹配步骤⑴中的矩阵模板，判断四个角位置的像素点是否为黑点。若都为黑点，则增加层数(N+1)，扩展虚拟矩阵并更新数组的值继续判断，直到遇到白点(数值信息为1)结束，同时记录当前扫描的层数和基点的位置。

⑶ 对每次得到的层数的大小进行比较，取最大层数所对应的基点位置则是圆心。

图2 目标检测算法流程图

3.2 姿态解算模块算法设计

姿态解算是飞控系统的基础，系统采用的姿态结算，采用了四元数法，设计思路如图3所示。姿态解算得出的姿态数据会发布给姿态控制器，控制四旋翼飞行平稳。姿态解算的速度和精度，会直接影响系统的实时性和稳定性。姿态是用来描述一个刚体的固连坐标系和参考坐标系之间的角位置关系，常用的姿态表示方法有：欧拉角、方向余弦矩阵、四元数表示[4]。相比其他的表示方法，四元数法计算量小，算法简单，易于操作，避免了欧拉角的奇异性问题。使用四元数来更新姿态，最后转化为欧拉角发布给姿态控制器。

图3 姿态解算模块算法

四元数q=q0+q1i+q2j+q3k，欧拉角、方向余弦矩阵、四元数的关系如公式⑵。

陀螺仪测量角速度，具有高动态性，受温度和电压影响产生零点漂移现象，离散采样产生累积误差。加速度计的低频特性好，可以测量低速状态下的加速度。结合两者的优点，姿态解算的算法为：根据加速度计数据，转换到地理坐标系后，与对应参考的重力向量进行求误差，这个误差用来校正陀螺仪的输出，然后用陀螺仪数据进行四元数更新，再转换到欧拉角。

四元数姿态更新采用一阶毕卡算法进行，如（公式3）所示。其中Δt代表采样时间间隔，ωxb、ωyb、ωzb为载体坐标系下的角速度。

3.3 控制器设计

本系统采用串级PID控制，包括位置控制环和姿态控制环，姿态控制环内部又采用了双环PID，内回路为姿态角速度PID控制，外环为姿态角度P控制。遥控器或者上位机设定期望位置后，位置PID回路计算出所需的姿态角，送给姿态控制环，姿态控制环计算电机控制量，并转化为相应的PWM波输入给电机。系统控制器结构如图4所示。

图4 系统控制器结构图

4 飞行试验结果

⑴ 起飞捕获能力测试

测试方式：飞行器平稳放置水平面，小车静止于飞行器正前方一米处，飞行器一键起飞向前飞行，当捕获到小车位置时，悬停于小车正上方。飞行器起飞速度与小车位置捕获之间的关系如表1所示。

表1 起飞捕获能力测试

分析：飞行器需要以一个合适的速度飞向小车，若速度太快不容易悬停于小车上，若太慢则容易偏离小车所在方向。

⑵ 跟踪飞行能力测试

测试方式：在起飞捕获到小车位置并悬停于小车上方之后，小车开始运动，观察飞行器跟踪小车的飞行情况。飞行器跟踪能力与小车运动速度之间的关系如表2所示。

表2 跟踪飞行测试

分析：跟踪飞行时，小车的运动速度不宜太快，否则飞行器难以及时调整飞行方向，会失去对小车的跟踪。最终测试效果如图5所示。测试结果表明，所设计的飞行器及控制系统对地面运动目标可实现稳定、实时及精确的跟踪飞行。

图5 系统测试效果图

5 结束语

该四旋翼飞行器探测跟踪系统是基于两块R5F523T5ADFM单片机，一块进行姿态解算及飞行控制，另一块进行图像数据处理并计算出运动目标位移信息，通过UART接口实现双机通信。再结合相应的控制算法进行解算，输出到执行机构，改变电机转速，完成四旋翼飞行器跟踪地面运动小车的运动控制。无人机自动跟踪系统结合了传感器、控制理论、空气动力学、电机学、图像处理等多门学科的最新理论和实践技术，其应用前景相当广阔。将无人机运用到地面移动目标的跟踪中，能有效克服传统视觉跟踪监控范围限制、目标易丢失等问题，是视觉跟踪理论当前的研究热点。为提高系统的实用性，未来将在扰动作用下对飞行器稳定性及跟踪速度、精度等方面进一步研究。

参考文献(References):

[1]魏丽娜,宁会峰,陆旺等.多旋翼飞行器的关键技术及应用前景[J].自动化技术与应用,2016.9:1-4

[2]古春海.基于APM平台的无人机目标追踪系统研究[D].上海师范大学硕士学位论文,2016.

[3]樊珑.多旋翼无人机视觉引导降落研究[D].哈尔滨工业大学硕士学位论文,2016.

[4]雷家勇,达飞鹏,孟广猛.图像处理中圆心算法研究[J].计算机与现代化,2005.3.

[5]温全,成天乐,苏泽亚.基于STM32和OV2640的自主循迹四旋翼飞行器设计[J].微型机与应用,2016.35(22):105-107