四旋翼无人机路径跟踪控制系统软硬件设计与实验研究

阚亚雄

（镇江高等专科学校现代装备制造学院，江苏镇江 212028）

0 引言

四旋翼无人机在近年不断成为人们日常生活中的关注热点[1]。目前国内相关四旋翼无人机的研究已初步展开，潘春荣[2]研究了无人机姿态测量系统的设计，以ARM微处理器作为处理单元，采用互补滤波进行姿态解算，基本满足了无人机姿态测量的要求。刘辉邦等[3]和WUY等[4]的四轴无人机系统采用STM32单片机为主控制器单元，接受系统多传感器数据，并计算飞行器航姿。文中的飞行控制系统通过滤波算法提高了传感器数据的精确性，但对飞行器的轨迹跟踪等实验较少涉及。张新英等[5]和SCHIRRER A[6]讨论的四旋翼无人机自主循迹系统设计，提出了四旋翼利用机载摄像头进行自主循迹的方案，通过单目或双目相机视觉测距系统上经过图像采集、双目矫正、双目立体匹配等步骤得到视觉结果，实现了四旋翼飞行器的自主飞行功能。以上的研究为本文的四旋翼无人机轨迹研究提供了有益的参考，但是文献中自主轨迹循迹系统的智能度不高，依赖实验场地中的黑色标线才可以完成稳定流畅的循迹任务。同时，以上关于上位机PC软件界面尤其是四旋翼无人机的实时状态直观显示的设计较少，缺少进行姿态控制、路径跟踪的仿真实验。因此，一种新型的四旋翼无人机路径跟踪系统软硬件研究显得尤为必要。

本文以某四旋翼无人机实物为研究对象，提出一种基于STM32微处理器的四旋翼无人机路径跟踪控制系统的设计方案，阐述了控制系统的总体结构、硬件电路的设计、软件算法的设计与实现，在软件仿真和路上实验中实现四旋翼无人机的基本位姿控制、路径跟踪、自主避障、机上各传感器的数据采集与通信并能实现上位控制系统对无人机直观的实时状态显示功能。

1 控制系统总体框架

1.1 某小型四旋翼无人机控制系统结构

四旋翼无人机控制系统主要由上位机地面控制系统和下位机机载控制系统组成。其中下位机机载控制系统以STM32F103RCT6嵌入式芯片为系统的核心芯片，主要负责与上位机控制芯片的无线通信、传感器数据读取、数据集成、控制算法拟合、电机控制和电池管理。机载传感器包括采用MPU9250微惯导运动处理组件、BMP280高度气压传感器、无线通信模块和12 V锂电池，本系统的总体结构如图1所示。四旋翼无人机控制系统主控芯片与超声波传感器和红外传感器之间采用USART串口通讯，主控芯片与微惯导运动组件和高度气压计之间采用I2C总线通讯协议实现姿态角和速度数据；上位机地面控制系统与下位机机载控制系统间通过无线通讯模块实现数据指令的互传。

图1 四旋翼无人机控制系统硬件分布

1.2 四旋翼主控芯片最小系统设计

如图2所示，最小系统电路图给出了控制电路的其他引脚定义，如PA0、PA1、PA2、PA3是四旋翼无人机4路电机的PWM控制信号输出引脚，IMU_SDA:PB6、IMU_SDL:PB7为MPU9250的I2C数据接口。系统通过NRF_RX:PA4、NRF_TX:PA5和NRF_FLOW:PA6引脚与无线通讯模块连接等。

图2 四旋翼主控芯片STM32F103RCT6最小系统

1.3 四旋翼无人机动力学建模及控制算法流程设计

根据相关文献资料[6]，四旋翼无人机4个对称安装的旋翼正反转后产生的升力、下降力和不同旋翼之间的速度差可以实现四旋无人机的在三维空间里的前进、后退、上升、下降、纵摇和转向6个自由度运动。根据拉格朗日方程[7]，四旋翼无人机的六自由度动力学模型可表示为：

式中：x、y、z分别为四旋翼无人机在惯性坐标系中X、Y、Z方向上的位置坐标；、φ、θ、ψ分别为四旋翼无人机的滚转角、俯仰角和偏航角；l为四旋翼无人机旋翼末端到质心的直线距离；m为四旋翼无人机的负载总重量；Ii为转动惯量[6]；U1、U2、U3和U4为计算得到的中间控制输入，其定义为：

式中：b为升力系数；Ωi为第i个旋翼的转速；d为反扭矩系数。

本文提出了一种将滑模法（SMC），反步法（backstepping）和自适应率控制（Adaptive Control）相结合的自适应反步滑模算法（ABSMC），其特点在于具有较好的鲁棒性、较好的跟踪精度和较强的抗干扰容错能力。根据ABSMC算法演算出四旋翼无人机滚转角自适应容错控制率U2为：

同理偏航角控制率U4为：

2 四旋翼控制系统软硬件设计与实现

2.1 机载传感器电路设计

如图3所示，姿态传感器采用MPU9250微惯导9轴运动处理组件。主要功能是感知无人机静态和动态过程中实时的姿态角变化（俯仰角、横滚角、航向角），静态角度误差为0.1°，动态角度误差为1°，基本满足要求。气压传感器BMP280通过实时监测无人机所处环境气压值间接得到飞行高度数据。MPU9250微惯导9轴运动处理组件和BMP280气压传感器固定安装在四旋翼无人机中心位置，均采用I2C通信方式与主控制芯片实现数据交换。

图3 四旋翼无人机控制系统位姿传感器和气压传感器电路设计

2.2 直流电机及舵机驱动电路设计

四旋翼无人机系统采用直流电机驱动，无人机依靠4个空心杯直流电机不同速度的正反转控制实现期望的飞行动作、避障动作和轨迹跟踪动作，因此驱动电路的设计是四旋翼的运动性能指标的关键。本系统采用4个微型716专用无刷直流电机，响应速度快、调速范围大及可靠性高。转速最快可达45 000 r/min。如图4所示，每个MOS管SI2302控制1个电机，实际使用中引脚与主控制器STM32的PWM输出信号端口PA0、PA1、PA2、PA3相连，当信号端口收到高电平状态时，其控制的直流电机处于运行状态，电机正传、反转、刹车等，反之处于停止状态。

图4 四旋翼无人机控制系统驱动电路

2.3 四旋翼无人机控制系统软件程序设计

图5 所示为四旋翼无人机控制系统软件流程。首先，四旋翼未工作时系统处于待机模式，STM32属于断电状态。当按键按下后机载STM32系统进行初始化，同时地面控制站进行界面初始化。然后，上位机地面控制站将启动RadiolinkTask进程等待机载STM32主控芯片的连接申请。连接确认后将获取下位机机载传感器中的姿态角、四旋翼无人机高度和气压等的实时数据，简单校对后使用Sendto（）函数转送地面控制站，同时使用rev（）函数将地面控制站发送来的控制命令以串口的形式发送给机载STM32系统，完成地面控制命令的传输。机载STM32控制器运行AtkpRxAnlTask进程接受处理地面控制站下发的数据，解算出目标位置坐标，再下发到StabilizerTask控制进程中去。StabilizerTask进程包含姿态解算互补滤波算法ImuUpdate（），更新周期为100Hz。STM32芯片读取MPU9250传感器和UM220传感器采集到的加速度计、微惯导陀螺仪的数据和GPS/北斗位置定位信息后利用ImuUpdate（）函数对数据进行低通滤波和偏置调整，数据融合后输出四旋翼实际的姿态数据和位置坐标信息。还包含自适应姿态控制函数AttitudeControl（）和自适应位置控制函数PositionControl（），其中自适应姿态控制函数，更新周期为300 Hz，期望姿态角度和目标位置信息由地面控制站下达后，与ImuUpdate（）函数输出的四旋翼实际角度和实际位置值做差值比较，利用偏差值作为姿态控制器和位置控制器的输入值，经过自适应容错算法的计算后，输出控制目标姿态和飞行到目标位置值所需要的电机油门参数，最后发送给运动控制函数MotorInit（），从而通过PWM控制电机使四旋翼到达目标位置并获得目标姿态角。控制软件程序集成烧录在STM32微处理器中，开发工具为Keil uVision5，开发时采用C语言编制。

图5 四旋翼无人机控制系统软件流程

2.4 上位机地面控制站界面设计及实现

图6 所示为本四旋翼无人机控制系统的地面控制站人机交互界面。地面控制站软件整体分为机载传感器数据采集区、数据显示区、手动控制区和自主路径跟踪区。当地面控制接收到数据后，通过无线通信模块实现机载传感器的数据接收，将数据上传到机载传感器采集区，Labview不断通过VISA配置数据、读取数据并将读取后的数据存储到地面控制站芯片的内存中，再使用仪表盘或数字形式进行实现姿态角、航速、高度和路径跟踪的图像显示。人机交互界面仍设计有遥控和自动飞行的切换按钮。当按钮按下时，系统切换到地面控制人员的遥控飞行模式，通过人机交互界面上的飞行控制界面可以任意实时改变飞行器的姿态角、加减速度和急停返航动作。地面控制站的人机交互界面软件可以帮助地面控制人员更好、更直观地观察显示的参数并实时给四旋翼系统发送控制指令。

图6 四旋翼无人机地面控制站人机交互界面

3 仿真实验结果与分析

3.1 运动控制仿真实验

本文研究的对象是某型四旋翼无人机，重量1.5 kg，Ix＝Iy=1.745×10-2kg·m2，Iz=3.175×10-2kg·m2，l＝0.225 m，b＝4 N·m，d＝0.223 N/(m/s)2，动力方式为4电机推动，输出功率10 W，最大航速8 m/s。仿真实验平台为Matlab2019b，在四旋翼无人机模型的基础上进行仿真研究。假设四旋翼的初始姿态角均为0，初始位置为原点，即（x＝0，y＝0，z＝0），其所需控制的期望位置拟合为三维轨迹为水平面内和垂直面内的三角函数曲线，仿真系统分别向四旋翼无人机下达控制指令：

位置轨迹跟踪结果如图7所示。由图可以看出基于自适应容错算法的四旋翼位置控制器对三维轨迹有着良好的控制效果。跟踪过程中，距离误差不超过±0.7 m，x方向的速度误差不超过1.8 m/s。在0时刻范围内存在较大偏差是由于期望的位置轨迹所需的速度方向与实际速度方向偏差较大所导致的。仿真实验中控制系统在6 s时同时受到X、Y、Z轴的外力干扰，X、Y、Z轴方向各受到3 N的干扰力。观察受干扰后的跟踪曲线，在受到外界干扰后能够快速恢复原控制目标的要求。基于自适应反步滑模算法的路径跟踪系统可以实现四旋翼无人机的三维路径跟踪，具有一定的抗干扰能力，实现了设计目标。

图7 四旋翼无人机路径跟踪仿真曲线

3.2 实物实验

四旋翼无人机控制系统在人工遥控和自主路径跟踪模式下分别进行了实验，首先让四旋翼无人机进行一段路径轨迹跟踪飞行，设定在飞行过程中，无人机的飞行高度z＝0.7 m，无人机的偏航角始终为0，无人机跟踪一段双扭线轨迹。四旋翼无人机实物实验结果如图8所示。

如图9所示，四旋翼无人机在跟踪路径轨迹路线时，跟踪快速性较好，第5 s时即进行期望路径，跟踪时轨迹误差在z方向较小，稳定在0.7 m。而在x、y方向上存在着一定的误差，误差范围在0.1 m以内，同时姿态角误差小于±3°具有良好的位置跟踪控制效果。基本实现了系统的运动控制、路径跟踪和机上多传感器的数据采集收发与集成处理显示。测试结果显示系统软硬件基本满足设计要求。

图8 四旋翼无人机实物实验

图9 四旋翼无人机路径跟踪实验曲线

4 结束语

本文针对四旋翼无人机样机开发了一套具备三维路径跟踪功能的控制系统，研究并介绍了该系统的软硬件分布，主控芯片、MPU9250微惯导传感器、BMP280气压传感器和716直流电机的软硬件设计。该系统具备了包括四旋翼无人机的直线运动、调整姿态角、三维路径跟踪在内的飞行路径跟踪能力，同时可以实现机载传感器数据的实时采集和系统融合，使得地面控制站操作人员可以远程调整四旋翼无人机的飞行状态，观察机上数据。在今后的研究中将此基础上重点研究四旋翼无人机的导航规划功能，提高该系统的实用性和稳定性。