基于STM32的多传感器四旋翼姿态控制系统设计*

任剑秋，钟小勇，张小红

(江西理工大学 理学院，江西 赣州 341000)

0 引言

四旋翼是一种典型的多输入输出、非线性、强耦合的欠驱动系统[1]，控制系统复杂，但其结构紧凑、灵活性和机动性好[2]，在军事和民用领域都有广泛的应用前景。在四旋翼控制系统中，飞行的姿态控制尤为重要，直接影响无人机的控制精度和飞行效果[3]。四旋翼机载姿态传感器受到环境如固有的振动、噪声、磁场等客观因素影响以及传感器自身工艺的限制，可能导致传感器测量的姿态信息与实际姿态有较大偏差[4-5]，要实现稳定飞行，并非易事。为此，本文结合模块化设计的思想，采用多传感器数据融合，设计一种基于STM32的多传感器四旋翼姿态控制系统。

1 四旋翼结构及飞行原理

1.1 四旋翼无人机结构

根据飞行器定义的机头方向不同，无人机可分为十字型和X型两种模式[6]。十字型布于钢丝绳周围的无人机的机头方向指向某个旋翼，而X型无人机的机头方向指向两个旋翼中线。考虑到今后加装摄像头等传感器，为了不遮挡视线，本文采用X型无人机模式。X型无人机4个电机独立分布于机臂四个顶点，如图1所示，安装在电机上的四个螺旋桨为无人机提供升力。四个旋翼按照旋转方向不同，有正反桨之分，同一对角线上的旋翼旋转方向相同，其中电机1、3逆时针旋转为反桨，电机2、4顺时针旋转为正桨。正反桨的设计使飞行器的陀螺效应和空气扭矩效应相互抵消，从而保证四旋翼的平稳飞行[7]。

图1 四旋翼结构模型

1.2 四旋翼无人机运动姿态

四旋翼无人机通过四个电机转速的改变来控制四个旋翼产生的升力，进而控制无人机的运动。无人机运动可以分解为俯仰、横滚、偏航、垂直四种基本的运动。通过对无人机这四个基本运动的控制，可以实现无人机在地理坐标系中升降、前后、左右、偏航等运动。同时提高(降低)电机2、3的转速，降低(提高)电机1、4的转速，使旋翼产生的转矩不平衡，可以实现飞行器的俯(仰)姿态，进而控制无人机前后运动。同理，同时提高(降低)电机1、2的转速，降低(提高)电机3、4的转速，可以实现飞行器的横滚姿态，进而控制无人机左右运动。同时提高(降低)电机1、3的转速，降低(提高)电机2、4的转速，使旋翼产生的反扭矩不平衡，可以实现飞行器的偏航姿态，进而控制无人机顺(逆)时针的偏航运动。同时提高(降低)电机1、2、3、4的转速，可以控制无人机上升(下降)运动，当旋翼产生的总升力刚好抵消无人机重力时，无人机完成悬停动作。

2 系统硬件设计

四旋翼无人机控制系统包括主控制器，姿态及位置多种传感器，驱动、遥控、通信、电源等模块。主控制器通过采集传感器的实时数据信息，进行姿态解算，得到无人机的姿态及位置信息，并根据遥控器指令，结合相关算法控制无人机的姿态。控制系统硬件如图2所示。

图2 控制系统硬件示意图

2.1 主控芯片

本设计采用的ARM Cortex M4内核的32位STM32-F407ZET6芯片作为主控芯片。该系列芯片具有168 MHz最大频率、PWM高速定时器、全双工I2C、高速USART、高速SPI等性能特点，能够满足四旋翼无人机数据处理及控制要求。

2.2 驱动模块

四旋翼无人机对电机的转速要求较高，控制系统采用无刷直流电机。主控制器经过电子调速器驱动直流电机，通过控制PWM波占空比调节电机转速。电机选用朗宇X2212-13，980 kV无刷直流电机，其响应速度快、调速范围大，可靠性高。电调选用好盈天行者30 A直流电调，能够持续稳定提供30 A输出电流，可对无刷电机进行快速调节。针对以上选用的电机和电调，配套使用直径为10 mm、螺距为4.5 mm的正反桨为无人机提供升力。

2.3 通信模块

通信模块选用ATK-WLDBG无线调试器，调试器分为地面端与飞控端，两者间采用2.4 G无线通信。在地面端以921 600波特率向上位机发送数据，实现无线仿真调试及数据传递；在飞控端通过UART与主控制器相连。另外，在飞控端备用了一个4P的SWD接口，支持MDK编辑器，无线程序烧录。

2.4 遥控模块

本设计选用天地飞7通道遥控器WFT07。遥控器发送用户指令，通过2.4 G无线技术发射各通道PWM信号。机载接收器接收到信号后，发送到主控制器定时器通道。主控系统利用定时器的输入捕获功能得到遥控器的期望指令数据。

2.5 姿态传感器

姿态传感器采用MPU6050采集无人机的角速度与加速度原始信息。MPU6050集成3轴陀螺仪，3轴MEMS加速度计，其测量范围可根据器件寄存器的数据位进行设置。在本控制系统中，加速度量程设为±16 g，角速度量程设为±2 000 °/s。为了确定偏航角及误差修正、补偿，MPU6050通过I2C接口(数据线SDA_ES、时钟线SCL_ES)与磁传感器HMC5883L相连，组成9轴传感器，具体电路如图3所示。主控芯片同样通过I2C接口(数据线SDA、时钟线SCL)与MPU6050相连接，并以100 Hz频率采集姿态传感器数据。

图3 姿态传感器应用电路

2.6 位置传感器

选用压力传感器SPL06-001来测量四旋翼无人机的飞行高度，测压范围为300～1 200 hPa，工作温度范围为-40 ℃～85 ℃，最高采样率可达300 Hz，分辨力为5 cm。相对传统的传感器而言，其气压精度和灵敏度更高。SPL06-001芯片通过SPI总线与主控芯片相连，具体电路如图4所示。

图4 压力传感器应用电路

水平位置传感器采用SE-100 GPS传感器采集无人机室外的经纬度信息，其定位精度最高可以达到0.5 m，测速精度为0.1 m/s，更新频率为18 Hz，主控芯片通过UART以9 600波特率采集传感器数据。

2.7 电源模块

电源选用格式4 000 mA、25C、3S锂电池为无人机供电。锂电池除了为电调供电，同时作为输入电压经MP2359芯片稳压至5 V，再经AMS1117降压至3.3 V，供给主控芯片、各个传感器及模块电源。相对于由电调供应5 V电压方式，此供电稳压效果更好，具体电路如图5所示。为了调试、下载方便，5 V电源也可采用USB供电。另外，为了避免电量引起事故，控制系统还设计了电压检测及报警电路。当锂电池电压低于某个数值时，控制系统报警示意并启动相关的安全措施。

图5 电源及报警电路

3 系统软件设计

本文采用C语言编程，确保通电后各个功能模块可以正常工作，并可以根据遥控指令实现无人机的稳定飞行控制。系统上电后完成各模块的初始化，定时器设置5 ms的控制周期，每次中断任务请求读取传感器模块的数据，经过处理得到无人机的实时姿态数据，同时读取遥控器的指令，将其作为期望数据传入PID控制器，输出4路PWM信号控制四旋翼4个电机的转速，从而实现对无人机的飞行控制。软件控制流程如图6所示。

图6 软件控制流程图

3.1 姿态解算

3.1.1 欧拉角与四元数

根据姿态传感器测量的角速度值、加速度值解算出无人机的实际姿态角，将其作为反馈量用于控制无人机的飞行。本文将传感器得到的原始数据通过四元数方程解算姿态角度，避免使用欧拉角可能出现的万向节死锁，同时还能避免大量的三角函数运算，这样可减少计算量。

本文采用的地理坐标系(n系)为北西天；载体坐标系(b系)为前左上，即机体x轴指向前方，y轴指向左方，z轴指向上方，参见图1。在研究物体转动和位置的变化中，常用俯仰角θ、横滚角φ、航向角Ψ 来表示确定向量的转动位置。无人机各轴的角度变化用欧拉角表示，需要依次绕三个轴进行旋转才能求出复合后的旋转矩阵，本文采用z、y、x的旋转顺序。用欧拉角来表示无人机从载体坐标系b旋转到地理坐标系n的旋转矩阵公式为[8]：

式中c、s分别表示cos和sin函数。

3.1.2 互补滤波

对于MPU6050来说，陀螺仪在b系中积分得到的角度不受加速度的影响，但是随着时间的增加，积分漂移和温度漂移带来的误差会比较大。加速度计对无人机的加速度比较敏感，但取瞬时值计算倾角误差比较大，应合理应用补偿算法校正误差[10]。本文使用加速度计数据，通过互补滤波法对陀螺仪数据进行处理，以获得较高精度的校正结果。

互补滤波主要以陀螺仪测得数据为主，通过加速度计测量值来弥补误差。互补滤波法姿态解算的具体步骤如下：

(1)静止状态下加速度计测得的地理系加速度值应为[0 0 g]T，归一化处理后，转换到载体系，理想重力分量应为：

(2)将加速度计测量的加速度数据归一化。

(3)加速度计归一化后的值与重力向量叉乘，得到三轴的误差向量。

(4)得到三轴的误差向量后，分别使用PI控制器，对陀螺仪测量的角速度进行补偿。

其中，eInt为积分项，ts为传感器采样周期，kp为比例系数，ki为积分系数，ωx、ωy、ωz为陀螺仪测量的三轴角速度，为补偿后的三轴角速度。

(5)用补偿后的角速度值，使用一阶毕卡法更新四元数，四元数更新表达式如下：

在初始时刻给定一个四元数初值，即可通过陀螺仪测得的角速度不断更新四元数。

(6)将更新后的四元数进行归一化处理，按照式(3)计算实际的姿态角度。

3.2 串级PID姿态控制

PID算法在控制工程实践中应用广泛，并且不需要精确的物理模型[11-12]。本文选用串级PID控制策略，以提高无人机飞行过程中的干扰能力和适应性。串级PID分为内、外两环，外环为角度PID环，内环为角速度PID环。外环输入为遥控器给定的期望角度与姿态解算的实际角度偏差，通过外环PID后，将输出的期望角速度与传感器反馈的实际角速度偏差作为内环输入，最终转换为PWM波信号控制电机转速。姿态控制系统流程图如图7所示。

图7 姿态串级PID控制流程图

4 实验结果与分析

本文设计的X型四旋翼无人机实物图如图8所示。无人机姿态控制中串级PID参数的合适与否直接决定了无人机是否能够稳定飞行。对无人机的俯仰、横滚以及航向三个姿态通道分别进行PID参数的整定，三个通道整定的PID参数如表1所示。

图8 四旋翼无人机实物图

表1 三个姿态通道的PID参数

为了检验控制系统的效果，遥控器分别给定横滚和俯仰两个通道的期望角度值，主控芯片通过通信模块将姿态数据发送到上位机，并在上位机上进行波形显示。图9和图10分别是横滚角和俯仰角实际角度跟随期望角度的变化情况。从图9和图10可以看出，无人机实际角度可以快速、稳定地追踪到期望角度，并且在没有给定期望角度即默认为0°时，无人机姿态的实际角度可以稳定在0°。实验验证了四旋翼无人机姿态控制系统的有效性以及无人机飞行的稳定性。

图9 横滚角度变化

图10 俯仰角度变化

5 结论

四旋翼无人机在飞行过程中姿态传感器容易受到复杂气流、磁场干扰、桨叶震动等因素影响，姿态控制的不精准会造成无人机机体抖动、漂移甚至失控等问题，为此本文设计了一种基于STM32的多传感器四旋翼无人机控制系统。实际飞行试验表明，该系统可快速灵活地控制四旋翼姿态，并实现四旋翼稳定飞行。同时，也为集成其他传感器模块，实现导航、循迹、跟踪等功能奠定了基础。