基于MPC的四旋翼无人机航迹跟踪控制系统

冯 强，齐 伟

(1.西安明德理工学院，西安 710124; 2.西安航天动力机械有限公司，西安 710025)

0 引言

近年来无人机的应用越来越广泛，因其独特的体积小、隐蔽性强、飞行稳定等优点，在军事及民用领域都发挥着极其重要的作用，尤其在抢险救灾及影视行业中都扮演了十分重要的角色[1-2]。四旋翼无人机系统不同于普通无人机，采用非线性系统，另因其特殊的驱动也对无人机的航迹跟踪控制带来了困难[3-4]。

无人机的飞行环境复杂，这些问题都对四旋翼无人机的控制提出了很高的要求，控制系统会直接影响四旋翼无人机的飞行状态和任务执行的优势性。为了更好的实现四旋翼无人机的航迹跟踪和控制，本文基于MPC航迹规划算法和航迹跟踪算法对硬件和软件部分进行设计，通过数据通讯实现地面控制与空中飞行控制的数据交换，提出了一种基于MPC控制的四旋翼无人机跟踪控制系统，并通过实验验证了该控制系统的有效性。

1 四旋翼无人机航迹跟踪控制系统硬件结构设计

根据四旋翼无人机的飞行荷载有限的特点，通过空中和地面两个部分实现控制系统的设计，结构如图1所示。

图1 基于MPC的四旋翼无人机航迹跟踪控制系统硬件结构

根据图1可知，根据不同的任务需求，无人机航迹跟踪控制系统分为 3 个部分：空中飞行自主控制模块、地面航迹规划系统、紧急情况下的人工干预模块，空中控制器主要负责稳态飞行和航迹跟踪控制，地面控制器主要负责飞行的动态航线规划，并通过无线通信技术将两部分结果进行数据连接，该控制系统中，地面控制决定了无人机的航迹规划，要求系统能够对周围环境进行甄别并有效规避障碍物[5-6]。空中控制则要求能够准确执行地面控制系统所规划的航线。

1.1 四旋翼无人机平台设计

四旋翼无人机平台主要硬件包括：飞行器、电机、螺旋桨。为避免在调试过程中因摔机造成机架损坏，选用主体采模块化设计、便于拆卸和更换的F450机架。为了要最大限度的提升无人机的系统性能，要合理搭配电机和螺旋桨，电机在选择上主要考虑无人机的最大飞行荷载，电机自重应小于电机最大动力的2/5。螺旋桨的选择主要考虑电机的电压值，一般电压值较大时优先选择小型螺旋桨；当电压值较小时，优先选用大型螺旋桨。本文设计使用4 s电池，电压为22 V，搭配APC1540螺旋桨[7-8]。

1.2 飞行控制器设计

飞行控制器的主控计算机是整个系统的核心，要负责传感器数据的采集与分析、自主飞行控制算法的计算、输出指令和其他模块间无线数据传输，因此，主控计算机应具有强大的计算能力，多个通信接口，较低的功耗，合适的重量和体积。因此本文采用法国 ST 公司 STM32F4 系列芯片主板，如图2所示。

图2 STM32F4 系列芯片主板

该主板的芯片内核处理器为 Cortex-M4，辅以运算单元FPU，主要频率为169 MHz[9-10]。根据图2可知，该芯片连接PWM输出接口，电源接口及调试接口，保证STM32F4系列芯片主板的稳定运行，通过遥控数据接口实现飞控数据的接收机传输；通过导航数据接口及无线数据接口实现飞控数据的处理，通过姿态数据接口实现无人机姿态控制数据的传输，并在芯片连接的液晶显示屏上展示。

1.3 导航控制器设计

导航控制器主流的GPC定位系统精度不够，存在很大的误差，无法满足四旋翼无人机的导航要求，所以本文基于借助 Ublox 6H GPS定位技术，采用MS5803数字式高度计来实现导航定位。

本文设计的导航控制器如图3所示。

图3 导航控制器

图3为导航控制器结构图，接收机接收四旋翼无人机航迹数据，通过数字气压计及主控板实现外界大气压力，温度，湿度和海拔高度的测量，通过Ublox 6H GPS定位技术实现导航定位。

1.4 姿态指引仪

姿态指引仪包括陀螺仪、加速度计和磁力计。加速度计的主要功能是通过测量重力加速度[11-12]来校正陀螺仪的俯仰和横滚角，输出三轴加速度和角速度，然后将其反馈到控制系统作为对照参考。磁力计用于测量地磁场的漂移以校正偏航角[13]。本位采用 MTI 姿态参考系统设计姿态指引仪，参考仪内部分别集成三轴加速度计、轴磁力计、角速率陀螺和温度传感器。采用卡尔曼滤波算法得到姿态估计参数，分辨率为 0.1，数据更新频率为256 Hz，俯仰角输出范围为±90°，角速度输出角度±300°，通信波特率921 700 bps。

1.5 数据通信器

数据通信器为地面控制系统和空中控制系统间的数据交换纽带，本文采用 MaxStream 公司的9XTend OEMRF模块，通信距离最远可大70 km，采用5 V的串行接口，天线接口采用RPSMA，植入抗干扰技术，避免无线电干扰，灵敏度110 dBm,工作频率为900 MHz，数据处理量为240 Kbps。

1.6 人工干预器

地面控制模块的主要任务是规划航线，本文设计采用高配置终端计算机。当无人机发生突发意外情况时，可以切换到人工遥控模式，人工干预主要通过 FUTABA T8FG 遥控器和RS5803SB 接收器实现，FUTABA 遥控器和接收器采用SBus传输协议，该协议遵循串口数据流传输模式的基本数据流协议，以100 bps 的频率对数据单元进行发送。根据Sbus 协议的数据单元进行有效传输，每次有效传输的数据包括25个字节，且以低速高速交替形式进行周期性传输。Futaba T8FG 遥控器每个指令占用11 位的空间传输空间，总共包含了 16 通道的控制指令。

2 基于MPC的四旋翼无人机航迹跟踪控制系统软件设计

本文软件本设计系统中，地面控制中心的航线规划系统是基于 windows 系统的 PC 计算机开发的， 自主航线跟踪控制系统是基于 STM32 微处理器进行裸机开发。提供了完善的 C++语言，C ++语言库函数进行应用层软件的开发和调试，极大地降低了程序的开发难度。航迹跟踪控制中心采用基于芯片STM32F417 的裸机开发，无操作系统，在IAR Embedded Workbench for ARMv6.4 集成开发环境下进行裸机开发，在该环境下 ST 公司开发了不同类型芯片的操作库文件，这样极大地降低了应用程序开发的难度[14-15]。

MPC是一种进阶过程控制方法，这是一种多变量控制策略，以最优动态轨迹为控制目标，设定航线规划软件流程如图4所示。

图4 航线规划软件流程

观察图4可知，采用空间质点模型来实现飞行控制系统的航线规划，根据三维方向上的速度变化率得出其动态性能约束。通过网络云端数据获取障碍物数据信息，建立航线规划系统环境模型，利用鼠标在地图上标记目标地点，从地面网站获取目标地点位置，规划到系统目录下生成文本文档。在规划航线时，通过读取地面共享信息得出具体障碍物数据信息，对障碍物空间进行规划，利用无线数据传输获取无人机的实时飞行状况，实行航线动态规划[16-18]。

对航线规划问题建模，采用MPC控制方法，以AMPL标准模型语言对多目标优化问题描述，然后调用CPLEX软件对目标函数求解，同时忽略滚动时域，把第一个点作为飞行目标点，求出N个时域的最优解航线，具体流程如图5所示。

图5 最优解航线流程

接口数据接收程序接收的数据主要有：MTI姿态系统数据、GPS导航系统数据、无线链路数据、遥控器控制数据。其中MTI数据，GPS数据和无线链路数据是通过硬件接口开进行数据交互的，遥控数据为Futaba配套的特殊SBUs通信协议，通过接口改进，可以转换为一般的通信协议。在数据传输中，MTI姿态数据和遥控器数据格式大体相同，每次传输的数据包中包含一前端数字，N个字节的传输数据集和末端的数据验证。

高度计采集板上的导航定位信息是处理过的，可以实现输出数据的格式调节，因此所有的数据接收程序都接口中断用于触发MDA数据接收，以完成中断形式。 当缓冲区中有要访问的数据时，触发USART接收，检测到数据包前端数位后，关闭 USART 数据接收通道，触发MDA 直接接收 N 个字节的数据，DMA 接收完成触发中断打开 USART 数据接收通道[19-20]。

通信数据链设计采用 XTend OEMRF模块，考虑到无人机的飞行环境复杂多变，所以该通信模块设计为拓扑结构，地面Base随时掌握飞机的状态信息，包括经纬位置，速度和飞行姿态等等。地面控制站能够将规划航线控制指令上传到无人机，Remote根据根据接受的指令信息作出反应，实现Base和Remote间的数据交互，无人机根据指令执行飞行航线。考虑到信息传输过程中会出现数据错误和丢失的情况，在通行模式中设置base和Remote的应答机制，准确可靠地实现地面站与空中多飞行器的数据间通信。

3 实验结果与分析

为了验证本文提出的基于MPC的四旋翼无人机航迹跟踪控制系统的有效性，设计对比实验，选用本文提出的跟踪控制系统与传统的基于云台相机的四旋翼无人机航迹跟踪控制系统，基于数据挖掘的的四旋翼无人机航迹跟踪控制系统进行实验对比。设计实验参数如表1所示。

表1 实验参数

根据上述实验参数，选用本文基于MPC的四旋翼无人机航迹跟踪控制系统和基于云台相机的四旋翼无人机航迹跟踪控制系统、基于数据挖掘的四旋翼无人机航迹跟踪控制系统进行实验。

预计的跟踪控制结果如图6所示。

图6 预计的跟踪控制曲线

利用下式求解不同无人机航迹跟踪控制系统的跟踪结果：

(1)

式中，M为四旋翼无人机航迹跟踪坐标，X、Y、Z分别为横纵坐标及垂直坐标的预计位移值，通过上式求解航迹跟踪控制结果。

基于云台相机的四旋翼无人机航迹跟踪控制系统得到的跟踪控制结果如图7所示。

图7 基于云台相机的四旋翼无人机航迹跟踪控制结果

基于数据挖掘的四旋翼无人机航迹跟踪控制系统得到的跟踪控制结果如图8所示。

图8 基于数据挖掘的四旋翼无人机航迹跟踪控制结果

本文提出的基于MPC的四旋翼无人机航迹跟踪控制系统得到的跟踪控制结果如图9所示。

图9 基于MPC的四旋翼无人机航迹跟踪控制系统得到的跟踪控制结果

根据上述实验结果可知，本文基于MPC的四旋翼无人机航迹跟踪控制系统得到的跟踪控制结果与预期的跟踪控制曲线重合度更高，平均误差控制在1 cm以内。本文提出的控制系统具有颜色特征识别功能，MPC在跟踪控制过程中，会随着目标的移动而移动，对于无人机旋转角度有很好的跟踪能力，视线范围之内始终跟着跟踪目标。

综上所述，基于MPC的四旋翼无人机航迹跟踪控制系统具有很好的控制能力，可以在地面和空中实现对无人机精确稳定的跟踪控制。

4 结束语

本文根据综合控制系统的需求出发，设计了四旋翼无人机航线跟踪控制的硬件平台，然后基于MPC航迹规划算法和航迹跟踪算法对软件部分进行设计，最后通过数据通讯模块实现了无人机和地面控制系统间的数据通信，完成整个基于MPC的四旋翼航线跟踪控制系统。本文虽然基于MPC针对无人机航迹跟踪和控制提出了系统的方法，但还存在很大的改善空间，特别是对于姿态控制的分析中，弱化了各通道间的耦合，姿态动力学模型简化为线性化小扰动模型。在实际情况中，小型四旋翼无人机的飞行路径控制效果尚不理想。所以对航迹的控制研究将是今后的工作重点，对于小型旋翼飞行器来说能够自主避障的控制系统是未来发展的必然趋势，希望在未来的研究中能更好的实现对四旋翼无人机的航迹跟踪和控制。