基于STM32四旋翼飞行器设计与算法

杨洪亚　裴文卉

摘 要：近些年微机电系统（MEMS）的快速发展，现代控制的理论算法在四旋翼飞行器控制得到广范运用。文章从四个部分对四旋翼飞行器的硬件设计与控制算法进行研究，首先分析飞行器原理与结构，建立动力学方程模型，然后对主控电路STM32F411CE和发射、接收电路NRF51822+RFX2401C的设计进行介绍，之后利用卡尔曼滤波算法将获取MPU9250（加速度计和陀螺仪）的姿态数据进行融合，设计出一款超高性能PID控制器，最后利用数学工具MATLAB/Simulink对滤波前后的信号波形进行比较，突显卡尔曼滤波算法先进性。

关键词：动力学方程；NRF51822+RFX2401C；卡尔曼滤波算法； MATLAB/Simulink

中图分类号：V249.1 文献标志码：A 文章编号：2095-2945（2018）17-0034-05

Abstract： In recent years， with the rapid development of Micro Electromechanical System （MEMS）， modern control theory and algorithm have been widely used in the control of four-rotor aircraft. In this paper， the hardware design and control algorithm of the four-rotor aircraft is studied in four parts. Firstly， the principle and structure of the vehicle are analyzed， and the dynamic equation model is established. Then the design of main control circuit STM32F411CE and transmitting and receiving circuit NRF51822 + RFX2401C is introduced. Then the attitude data obtained from MPU9250 （accelerometer and gyroscope） are fused by Kalman filter algorithm， and a super-high performance PID controller is designed. Finally， MATLAB/Simulink is used to compare the signal waveforms before and after filtering， which highlights the advancement of Kalman filtering algorithm.

Keywords： kinetic equation； NRF51822+RFX2401C； Kalman filter algorithm； MATLAB/Simulink

四旋翼飞行器因具备体积小，质量轻，结构简单，可悬停和垂直起降等优点，可用于国防军事，海洋执法监测和民用生活航拍领域。本文从工程设计角度建立卡尔曼状态方程和测量方程，将姿态角度进行精密解算来对电机精确控制，实现空中翻转、悬停等。

1 四旋翼飞行器的结构与原理

四旋翼飞行器模型采用十字交叉式，由四个直流电机驱动风叶组成的六自由度，欠驱动系统。因存在旋转力矩与平衡系统的耦合问题，传统的直升机通过借助尾桨调节，来平衡反扭矩， 因此四旋翼飞行器在设计时需要考虑多变量，非线性系统的问题。工作原理如下：

1.1 垂直上升、下降运动：飞行器自稳后，电机1，2，3，4号保持同样速度，同时加速或减速，飞行器会发生垂直上升或下降运动。如图1所示。

1.2 俯仰、横滚（前后、左右）运动：飞行器自稳后，1号电机增速（减速），3号电机减速（加速），同时保持其他两个电机转速不变，这样机体会存在拉力差，使机身前后倾斜，拉力产生水平分量，使机体向前运动。如图2所示，同理，同样的方法作用到2号、4号电机会发发生横滚（左右）运动，如图3所示

1.3 偏航运动：增大（减小）1号、3号电机，减小（增大）2号、4号电机转速，机体会发生顺时针偏航运动，反之为逆时针偏航运动。如图4所示。

2 运动方程模型

为获得四旋翼飞行器数学模型，我们可以假定a.四旋翼飞行器模型是均匀对称的刚体；b.飞行器各个方向的拉力与推进器转速的平方成正比。设立两个基坐标参考系：惯性坐标系E（OXYZ）和飞行器坐标系B（OXYZ），如图5所示。首先设定欧拉角？啄=[？准 ？兹 ？鬃]T，将飞行器经过三次简单转动得到模型的三個姿态角依次为？鬃→？兹→？准。这三次旋转分别得到从机体坐标系到地面坐标系XYZ轴的转换矩阵如下：

3 硬件电路设计

3.1 飞行器硬件系统主要组成

系统电路主要包括气压计定高模块MS5611-01BA，无线收发模块，陀螺加速度计模块MPU9250，核心MCU STM32F411电路，空心杯直流电机等几个重要模块组成。

3.2 主控电路最小系统组成

本设计中采用ST公司的STM32F411RE最为主控芯片，该芯片是Cortex-M4内核，带FPU（Float Point Unit，源点运算单元）和DSP（Digital Signal Processing，数字信号处理）指令集，主频可达到168MHz（可获得210DMIPS的处理能力，并且拥有128KB的SRAM（Static Random Access Memory，静态随机存取存储器，带摄像头DCMI接口。还拥有ART（Adaptive Real-Time Accelerator，自适应实施加速器），可以达到相当于Flash零等待周期的性能。电路如图6所示。

芯片采用双晶振设计，8M晶振经过锁相环可到180MHz，RTC实时时钟采用32.768MHz晶振。

3.3 通讯电路设计

本文飞行器通讯电路采用NORDIC公司的低功耗蓝牙芯片Nrf51822，该款芯片是32位ARM Cortex-M0，有256KB FLASH存储，16KB RAM存储，睡眠模式功耗电流仅为 0.4uA，正常工作电压范围为1.8-3.6V，最高输出功率为4dbm，灵敏度为92.5dBm与RFX2401C结合可将发射距离扩大到100m。如图7所示。

3.4 PCB设计

PCB设计采用四层板设计，材料选用FR-4，在RF 射频电路设计时采用50欧阻抗匹配，通过PCB参数计算工具Saturn PCB Design Toolkit 如图8所示。

差分电路阻抗匹配为100Ω，PCB层间距如图9所示。

用Altium Designer軟件设计出的PCB如图10所示。

4 算法部分-卡尔曼滤波器的PID控制

扩展卡尔曼滤波算法是一种高效的递推算法，它被广泛应用在工业和科研领域中，特别是飞行器控制中，具有很高的姿态解算精度。建立状态方程，通过对系统状态进行估算得到下一时刻的姿态角。卡尔曼滤波器的设计要求系统模型为离散模型。定义的线性状态方程状态空间模型为：

X（k）=AX（k-1）+BU（k）+W（k）

式中：X（k）为当前系统状态的估算值，A为前一状态X（k）和后一状态X（k+1）的状态转移矩阵，U（k）为k时刻控制输入；B为输入控制，W（k）为k时刻系统的过程噪声及系统误差，当W（k）无限逼近于0，意味着过程噪声W（k）为高斯白噪声N（0，Qk），Qk为过程噪声的协方差矩阵，反之，W（k）为有色噪声。

观测方程即系统输出方程为 z（k）=HX（k）+V（k）

式中：H为测量相关状态矩阵；z（k）为k时刻的观测值，当加速度计的观测噪声V（k）逼近于0，意味着高斯白噪声N（0，Rk），Rk为观测噪声协方差矩阵，反之，V（k）为有色噪声。

基于卡尔曼滤波的PID控制结构图如图11所示。

建立状态空间模型为：

xk=f（xk-1'+uk+wk）Zk=h（xk，Vk）

其中W（k）和V（k）均为随机信号服从正态分布：

p（w）～N（0，Qk）p（v）～N（0，Rk）

协方差系数Rk与Qk是影响滤波效果和响应速度的重要参数，Rk取值越小，滤波响应和收敛越快；Qk取值越小，抑制、滤除噪声的能力就越强。

卡尔曼滤波的基本公式为：

（1）状态预测方程：由系统状态变量k-1时刻的最优值X（k-1|k-1）和系统输入U（k）可以求出k时刻系统预测值X（k|k-1）为

X（k|k-1）=AX（k-1|k-1）+BU（k）

（2）协方差预测方程：根据k-1时刻系统协方差矩阵 预测k时刻系统协方差矩阵P（k|k-1）为

P（k|k-1）=AP（k-1|k-1）AT+Q

（3）卡尔曼增益计算方程：根据状态变量预测值和协方差矩阵预测值计算卡尔曼增益Kg（k）为

Kg（k）=P（k|k-1）HT/（HP（k|k-1）HT+R）

（4）最优值更新方程：由状态变量预测值和系统测量值计算k时刻状态变量最优值X（k|k）为

X（k|k）=X（k|k-1）+Kg（k）（Z（k）-HX（k|k-1））

（5）协方差更新方程：更新k时刻协方差矩阵P（k|k）为

P（k|k）=（1-Kg（k）H）P（k|k-1）

最优值和协方差更新方程都计算结束后，开始进行下一轮循环，往复（1）-（5）步。递推算法的本质体现在估计过程，每次只需根据以前的测量值递归计算，就能得到当前时刻的状态估计。卡尔曼滤波器的工作流程如图12所示。

用MATLAB/Simulink仿真结果，如图13所示。

5 结束语

本文通过对四旋翼飞行器硬件电路设计以及姿态传感器获取姿态角的数据解算，建立飞行器的数学模型，利用估计后的姿态角设计PID控制器实现对飞行器角度扰动的纠正。利用MATLAB仿真和分析，验证卡尔曼滤波算法在飞行器控制应用中可以极大减小误差。

参考文献：

[1]唐建杰，王鑫.基于PI-PD控制器的四旋翼姿态控制[J].电子技术与软件工程，2014.

[2]张栋，焦嵩鸣，刘延泉.互补滤波和卡尔曼滤波的融合姿态解算方法[J].传感器与微系统2017，36（3）：62-66.

[3]冯新宇，范红刚，辛亮.四旋翼无人飞行器设计[M].北京：清华大学出版社，2017：20.

[4]张帅.四旋翼飞行器稳定性的非线性控制[D].上海：上海电力学院，2017.

[5]王伟.四旋翼飞行器姿态控制系统设计[J].科学技术与工程，2013，13（19）：5514-5519.

[6]历小伟，郭玉英.四旋翼飞行器的动力学建模与飞行控制[J].自动化与仪器仪表，2017，01：130-132.

[7]杨广杰，白宝明，李凯.四旋翼无人飞行器飞行控制系统设计[D].西安：西安电子科技大学，2013.

[8]张丽娟，孙戈.四旋翼飞行器的姿态角估计与控制[D].西安：西安科技大学，2016.

[9]胡琦逸，王建中，邹洪波.四旋翼飞行器的姿态估计与优化控制研究[D].杭州：杭州电子科技大学，2013.