基于stm32单片机的自动避障小车（基于STM32单片机四旋翼飞行器建模分析与设计）
基于stm32单片机的自动避障小车（基于STM32单片机四旋翼飞行器建模分析与设计）中文引用格式：曾宪阳，杨红莉，郁汉琪. 基于STM32单片机四旋翼飞行器建模分析与设计[J].电子技术应用，2016，42(12)：65-68.DOI：10.16157/j.issn.0258-7998.2016.12.017四旋翼飞行器；STM32单片机；欧拉方程；数学模型；卡尔曼滤波；PID调节中图分类号：V212.4A
曾宪阳1，2，杨红莉2，3，郁汉琪1
(1.南京工程学院 工业中心，江苏 南京211167；2.南京大学 数学系，江苏 南京210093；
3.南京工程学院 数理部，江苏 南京211167)
针对四旋翼飞行器稳定性差、控制难的问题，使用牛顿-欧拉方程建立了数学模型，提出了姿态解算的实现方法。设计了以STM32单片机为控制核心，加速度计、陀螺仪及磁力计等组成的硬件控制电路。提出将加速度计解算出的角度数据与陀螺仪解算出的角度数据进行融合，通过卡尔曼滤波滤去干扰信号，保证了角度数据的准确性。设计了三路串级PID控制器，通过对横滚角、俯仰角、偏航角3种姿态角进行控制，实现了对飞行器的悬停、前进、后退、左转、右转等控制。经室内外飞行测试表明，飞行器可以平稳飞行。
四旋翼飞行器；STM32单片机；欧拉方程；数学模型；卡尔曼滤波；PID调节
中图分类号：V212.4
A
DOI：10.16157/j.issn.0258-7998.2016.12.017
中文引用格式：曾宪阳，杨红莉，郁汉琪. 基于STM32单片机四旋翼飞行器建模分析与设计[J].电子技术应用，2016，42(12)：65-68.
英文引用格式：Zeng Xianyang，Yang Hongli，Yu Hanqi. Modeling analysis and design for four rotor aircraft based on the STM32 MCU[J].Application of Electronic Technique，2016，42(12)：65-68.
0 引言
四旋翼飞行器体积小、重量轻，与载人飞机相比成本很低，可广泛应用在交通检测、园林维护、灾区搜救等场合，目前在国内外正处于热门研究中[1-3]。但四旋翼飞行器是一个非线性、多变量、高度耦合、欠驱动系统（6个自由度，4个输入量），控制系统较为复杂，系统抗干扰能力差。只有采用了合适的控制方案才能够实现平稳的飞行，这就需要对飞行器进行力学和运动学上的分析并建立相对应的数学模型，设计单片机控制电路及软件控制系统等，实现对飞行器的稳定控制。
1 四旋翼飞行器数学模型建立
为简化分析，假设以下条件成立：
（1）机体坐标系的原点为飞行器的质心，并且与飞行器几何中心重合；
（2）只考虑螺旋桨产生的气流，而空气流动速度及空气阻力忽略不计；
（3）飞行器机体与螺旋桨均为刚体结构，机体的几何构造及质量对称；
（4）螺旋桨旋转时产生的反扭矩与转速的平方成正比，螺旋桨产生的升力与转速的平方成正比。
根据以上条件，设F为飞行器受到的外力和，m为飞行器质量，V为飞行速度，M为飞行器所受力矩之和，H是飞行器相对于导航坐标系的相对动量矩，受力分析如图1所示。根据牛顿-欧拉方程对四旋翼飞行器平移运动与旋转运动建立模型[4-7]。
根据欧拉方程则有三轴力矩平衡方程式：
由于姿态稳定控制不需对位置和高度进行控制，只需考虑角度控制即可，稳定控制时姿态变化较小，可忽略空气阻力的影响，这样得到简化后的动力学模型为：
从以上公式可以看出，在四旋翼飞行器的姿态控制模型中，对任一个角度的控制均可采用PID控制器对其进行控制。
2 硬件电路设计
2.1 硬件系统主要组成
系统硬件主要包括飞行架构、微控制器最小系统、无线通信系统、动力系统、惯性测量单元、电源等几个部分。其中飞行架构是其他几部分的载体，微控制器最小系统用于数据融合、姿态解算与调节，控制协调各模块的工作。无线通信系统用于遥控飞行器和接收数据；动力系统由驱动器、电机、螺旋桨组成，为飞行器提供升力和推动力；惯性测量及系统为飞行器提供姿态和高度等信息，是飞行器系统的重要组成部分。
2.2 主要电路原理图
系统主要电路原理图如图2所示。采用STM32F103C8T6高速单片机作为主控制单元，MPU6050和HMC5883L组成惯性测量及姿态检测单元，实现对三轴的加速度、三轴陀螺仪、三轴磁场方向的检测，输出原始数据给微控制器，进行融合姿态解算后得到横滚角、俯仰角、偏航角。采用空心杯直流电机，使用PWM进行调速。驱动电路由N沟道的场效应管形成的开关电路实现，选择漏源电阻较小的场效应管，防止电机输入电压变小造成电机转速的范围变窄，便于对电机转速调节的控制。
3 软件系统设计
3.1 姿态解算与卡尔曼滤波[8-12]
飞行器在运行过程中，CPU通过传感器MPU6050内部的三轴加速度传感器、三轴陀螺仪来实时采集当前各方向姿态数据，通过解算后得到三个方向的角度、角速度值，然后调节各电机转速，使其达到期望姿态。加速度传感器用来测量x、y、z三个方向的加速度值，然后与重力加速度比较即可算出三个方向的角度值，但是存在一定误差。三轴陀螺仪传感器用来测量x、y、z三个方向角速度，乘以时间即为角度。
通过陀螺仪计算出来的角度存在一定的误差，多次累加以后累积误差越来越大，最终导致计算出的角度与实际角度相差很大。本文的解决方法是把加速度计读出的角度与陀螺仪计算出的角度相结合，通过卡尔曼滤波计算出更为准确的角度值。
3.2 四旋翼飞行器PID控制器设计[13-15]
通过传感器准确获取角度、角速度数据后，再与期望姿态角进行对比，实现对飞行器姿态的调节控制。控制系统采用位置式PID控制器进行调节，四旋翼飞行器主要包括横滚角、俯仰角、偏航角3种姿态，只有对3个姿态角可控，才能实现对四旋翼飞行器的悬停、前进、后退、左转、右转等控制，因此需设计三路串级PID控制器，其框图如图3所示。
在设计中，外环采用角度环进行PI控制，内环采用角速度环进行D控制。设置这样的串级PID可以使飞行器在大角度偏差时修正速度快，小角度偏差时修正速度慢。外环期望角度为设定值，如悬停的期望值为0°，而实际角度值由卡尔曼滤波后的角度数据提供。外环PI控制器的输出值作为内环D控制器的期望值，内环角速度实际值由传感器MPU6050内部的陀螺仪提供。三路PID控制器的输出分别为roll_out、pitch_out、yaw_out，分别以PWM的形式输出给4个电机驱动电路，每个电机PWM线性组合如下：
Y轴前端电机：Throttle-roll_out-yaw_out；
Y轴后端电机：Throttle pitch_out-yaw_out；
X轴右端电机：Throttle-roll_out yaw_out；
X轴左端电机：Throttle roll_out yaw_out。
其中Throttle为电机输入固定值，也称油门。为保证飞行器稳定，内环控制器的采样频率为200 Hz，外环控制器采样频率50 Hz即可。为保证采样时间间隔均匀，PID控制器采样周期由高优先级定时器中断提供。串级PID调节、卡尔曼滤波轴波形图如图4所示。由图中可以看出飞行器在受到环境干扰时，通过串级PID调节可迅速让机身恢复至平衡位置。恢复时间很短，基本无振荡现象，说明飞行器在串级PID调节下具有较强的抗干扰能力，验证了PID调节系统设计的可靠性。
4 系统测试
4.1 姿态角测试
四旋翼飞行器采集到加速度计和陀螺仪的数据后，经过数据融合、卡尔曼滤波将两种数据融合在一起计算出较为精确的姿态角数据，再将数据通过串口发送到由匿名科创提供的上位机软件，将三轴姿态角用3D效果图直观显示出来。其中横滚角姿态解算3D效果如图5所示，由图中可以看出系统响应稳定、姿态平稳，说明系统能很好地实现这些姿态控制。
4.2 平衡飞行测试
将飞行器半固定在水平杆上调试内外环PID各项系数，在调试俯仰这一路的PID控制器时，先将Ki和Kp设为零，Kp增大到控制回路的输出出现临界振荡较为合适，当选取到12左右时，飞行器在上升过程中出现上下摆动的不稳定现象，通过设置Kd参数抵消比例作用调节时的过度反应和过冲干扰，使飞行器系统在上升时能够趋于稳定飞行，Kd取0.05左右就可以满足要求，为消除静态误差，Ki参数可以选取0.02左右。经反复调试达到稳定状态，在室内可使飞行器平稳悬停在空中，通过调节俯仰角的大小可让飞行器在室外水平稳定向前飞行。
5 结论
本文分析了四旋翼飞行器的控制原理，使用牛顿第二定律和欧拉方程建立了系统的数学模型，设计了以STM32高速单片机为核心的控制系统电路，通过卡尔曼滤波滤去了可能的干扰信号，保证了角度数据的精确性。设计三路串级PID调节控制器，实现了对横滚角、俯仰角、偏航角3种姿态角控制。室内外飞行测试结果表明，系统可以稳定平衡飞行。
参考文献
[1] 王贞琪，马洁.四旋翼几种控制方法比较[J].北京信息科技大学学报，2016，31(2)：35-39.
[2] 赵世荣，孙运强，石喜玲.基于信息融合四旋翼飞行器稳定性改进设计[J].电子器件，2015，38(6)：1417-1420.
[3] 郝伟，鲜斌.四旋翼无人机姿态系统的非线性容错控制设计[J].控制理论与应用，2015，32(11)：1457-1463.
[4] SCHREIER M，DARMSTADT T.Modeling and adaptive control of a quadrotor[C].Proceedings of 2012 IEEE International Conference on Mechatronics and Automation，IEEE，2012：383-390.
[5] 王伟，邱启明.六旋翼飞行器建模及位置跟踪控制[J].电子器件，2014，37(3)：507-512.
[6] 廖卫中，宗群，马亚丽.小型四旋翼无人机建模与有限时间控制[J].控制理论与应用，2015，32(10)：1343-1350.
[7] 陈航科，张东升，盛晓超.四旋翼飞行器悬停状态姿态控制建模与仿真[J].计算机仿真，2013，30(11)：41-45.
[8] 黄建，胡越黎，杨文荣，等.组合导航系统在四旋翼无人机上的实现[J].电子技术应用，2015，41(5)：167-170.
[9] 叶锃锋，冯恩信.基于四元数和卡尔曼滤波的两轮车姿态稳定方法[J].传感技术学报，2012，25(4)：524-528.
[10] 吴涛，白茹，朱礼尧，等.基于卡尔曼滤波的航姿参考系统设计[J].传感技术学报，2016，29(4)：531-535.
[11] 何川，李智，王勇军.基于STM32的四旋翼飞行器的姿态最优估计研究[J].电子技术应用，2015，41(12)：61-64.
[12] 蔡伟平，胡越黎，杨文荣.四旋翼双环滑模姿态控制系统设计与仿真[J].电子技术应用，2015，41(7)：150-153.
[13] SADEGHZADEH I，MEHTA A，ZHANG Y M，et al.Faulttolerant trajectory tracking control of a quadrotor helicopter using gainscheduled PID and model reference adaptive control[C].Annual Conference of the Prognostics and Health Management Society.Montreal，2011，9：1-10.
[14] 彭程，白越，乔冠宇.四旋翼无人机的偏航抗饱和与多模式PID控制[J].机器人，2015，37(4)：415-423.
[15] 甄红涛，齐晓慧，夏明旗，等.四旋翼无人机鲁棒自适应姿态控制[J].控制工程，2013，20(5)：915-919.
