4. 系统软件设计
软件设计上由控制核心STM32F4读取传感器信息，解算姿态角，以姿态角为被控制量融合遥控信息后，输出到四个电机及两个舵机以完成四轴飞行控制和云台的稳定补偿。下图是软件流程：
系统初始化4ms定时等待否 是否到达定时是读取传感器数据姿态角解算伺服舵机PID计算电机PID计算输出舵机PWM捕获接收机信号遥控指令与平衡融合输出电机PWM
4.1.四元素计算姿态角的实现
根据前面给出的姿态解算方程与四元数，即可得到姿态计算系统的计算原理如下图（4-1） 图4-1 姿态算法流程图
本设计基于互补滤波的思想上完成的四元素算法，其核心思路为利用加速度测得的重力向量与估计姿态得到重力向量的误差来矫正陀螺仪积分误差，然后利用矫正后的陀螺仪积分得到姿态角。
首先不妨设处理后的加速度数据为：ax,ay,az，单位m/s^2。加速度计的向量为a（ax,ay,az）陀螺仪数据为：gx,gy,gz，单位rad/s。陀螺仪向量g（gx,gy,gz），由式（4-5）可得由载体到导航坐标系的四元数形式转换矩阵为：
g。 qb 根据余弦矩阵和欧拉角的定义，地理坐标系的重力向量，转到机体坐标系，
Tg是qb中的第三列的三个元素，即qbg??001?。所以加速的向量与估计重力向
量叉积：
e?aa?qbg??001?
T （4-7）
然后利用向量的叉积，e可视为误差向量，这个叉积向量仍旧是位于机体坐
标系上的，而陀螺积分误差也是在机体坐标系，而且叉积的大小与陀螺积分误差成正比，正好拿来纠正陀螺。由于陀螺是对机体直接积分，所以对陀螺的纠正量会直接体现在对机体坐标系的纠正。用上面得到的结果校正陀螺仪：
??g?t?k?e （4-8）
此处k为一个常量系数。
再利用二阶毕卡法解四元数微分方程（4-6），更新四元数为下一次计算做准备。毕卡二阶算法为： 2?1 （4-9） 其q(n?1)?{(1?0)??????}q(n) 82中??为角增量
???x???y???z??0
????0????? xzy?[??]?????y???z 0??x??? ???????0??yx?z?
2??02=??x2???y???z2
最后将四元数转变为欧拉角：
Q_ANGLE.Pitch=asin(-2 * q1 * q3 + 2 * q0* q2)
Q_ANGLE.Rool=atan2(2*q2*q3+2*q0*q1，-2*q1*q1-2*q2*q2+1) Q_ANGLE.Yaw=atan2(2*q1*q2+2*q0*q3,-2*q2*q2-2*q3*q3+1) 数据流程图如下： z?1 qg gbn?1 bn 加速度计 e ?? 微分方程运算 T 陀螺仪 其他运算
图4-5 数据流程图
通过以上算法我成功得出了飞行器的姿态角，在开启电机的情况下，角度误差在+/-2°以内，满足了控制要求。姿态计算效果如下图所示，其中红色和蓝色是直接由加速度计算出的俯仰角和横滚角，青色和黄色为姿态结算后的俯仰角和横滚角。由图中数据可看出，解算的姿态角不仅能即时的反应角度变化切曲线平滑，说明姿态解算算法有效。
?q????
4.2.控制设计
由于四轴飞行器独特的机械结构，即结构上的对称设计，使得四轴在俯仰角的控制欲横滚角的控制上有这近乎相同的控制特性，且两者相对独立。四轴飞行器的俯仰，横滚，偏航，升降可以通过四个输入量来控制。通过设定一个期望角度，调整电机转速，使得测得的姿态角稳定在期望角。控制律的设计主要采用是闭环控制。以姿态角做为被控制量，采用经典的PID控制算法。 四轴飞行器系统是一个时变且非线性的系统，采用传统PID算法的单一的反馈控制会使系统存在不同程度的超调和振荡现象，无法得到理想的控制效果。本文将前馈控制引入到了四轴飞行器系统的控制中，有效地改善了系统的实时性，提高了系统的反应速度；并且根据四轴飞行器系统的特点，对数字PID算法进行了改进，引入了微分先行环节，改善了系统的动态特性；使得控制器能够更好地适应四轴飞行器系统的实际情况。
在姿态角的控制中，本设计将控制器捕获到的遥控器信号转换为一个角度，作为期望角，与解算出来的测量角作差，得到偏差error。将error乘以一个比例系数kp。在只有kp作用下，系统会有静差所以考虑利用积分ki控制消除稳态误差。但积分控制会降低系统的动态性能，甚至造成闭环系统不稳定，因此要对积分进行限幅，防止积分过大。
对于微分，如果采用传统的D方法，在人为操纵四轴时会产生输入的设定值变化频繁且幅度较大，从而造成系统的振荡。对人为控制十分不利，为了解决设定值的频繁变化给系统带来的不良影响，本文在姿态角控制上引入了微分先行PID算法，其特点是只对输出量进行微分，即只对陀螺仪角速度测量值进行微分，而不对姿态角的设定值进行微分。这样，在设定值发生变化时，输出量并不会改变，而被控量的变化相对是比较缓和的，这就很好地避免了设定值的频繁变化给系统造成的振荡，明显地改善了系统的动态性能。控制周期定为4ms，姿态控制系统示意图如下：
图5-1姿态控制系统示意图
通过前面一章的介绍我们已经的达到了俯仰，横滚，航向三个控制量，然后将它们分别输入三个独立的如上图所示的PID控制器，我们可以得到三个PID
相关推荐：
loading