基于STM32F4的四轴航拍飞行器

通过以上算法我成功得出了飞行器的姿态角，在开启电机的情况下，角度误差在+/-2°以内，满足了控制要求。姿态计算效果如下图所示，其中红色和蓝色是直接由加速度计算出的俯仰角和横滚角，青色和黄色为姿态结算后的俯仰角和横滚角。由图中数据可看出，解算的姿态角不仅能即时的反应角度变化切曲线平滑，说明姿态解算算法有效。

4.2.控制设计

由于四轴飞行器独特的机械结构，即结构上的对称设计，使得四轴在俯仰角的控制欲横滚角的控制上有这近乎相同的控制特性，且两者相对独立。四轴飞行器的俯仰，横滚，偏航，升降可以通过四个输入量来控制。通过设定一个期望角度，调整电机转速，使得测得的姿态角稳定在期望角。控制律的设计主要采用是闭环控制。以姿态角做为被控制量，采用经典的PID控制算法。

四轴飞行器系统是一个时变且非线性的系统，采用传统PID算法的单一的反馈控制会使系统存在不同程度的超调和振荡现象，无法得到理想的控制效果。本文将前馈控制引入到了四轴飞行器系统的控制中，有效地改善了系统的实时性，提高了系统的反应速度；并且根据四轴飞行器系统的特点，对数字PID算法进行了改进，引入了微分先行环节，改善了系统的动态特性；使得控制器能够更好地适应四轴飞行器系统的实际情况。

在姿态角的控制中，本设计将控制器捕获到的遥控器信号转换为一个角度，作为期望角，与解算出来的测量角作差，得到偏差error。将error乘以一个比例系数kp。在只有kp作用下，系统会有静差所以考虑利用积分ki控制消除稳态误差。但积分控制会降低系统的动态性能，甚至造成闭环系统不稳定，因此要对积分进行限幅，防止积分过大。

对于微分，如果采用传统的D方法，在人为操纵四轴时会产生输入的设定值变化频繁且幅度较大，从而造成系统的振荡。对人为控制十分不利，为了解决设定值的频繁变化给系统带来的不良影响，本文在姿态角控制上引入了微分先行PID算法，其特点是只对输出量进行微分，即只对陀螺仪角速度测量值进行微分，而不对姿态角的设定值进行微分。这样，在设定值发生变化时，输出量并不会改变，而被控量的变化相对是比较缓和的，这就很好地避免了设定值的频繁变化给系统造成的振荡，明显地改善了系统的动态性能。控制周期定为4ms，姿态控制系统示意图如下：

图5-1姿态控制系统示意图

通过前面一章的介绍我们已经的达到了俯仰，横滚，航向三个控制量，然后将它们分别输入三个独立的如上图所示的PID控制器，我们可以得到三个PID输出：pid_roll，pid_pitch，pid_yaw将这三个输出量做简单的线性运算输出给电机。部分代码如下：

#define PIDMIX(X,Y,Z) Motor_Thr + pid_pitch* Y+ pid_roll*X + pid_yaw*Z

MOTOR1=MOTORLimit(PIDMIX(+1,+1,-1)); //REAR_R 后右电机

MOTOR2=MOTORLimit(PIDMIX(-1,-1,-1)); //FRONT_R 前右电机

MOTOR3=MOTORLimit(PIDMIX(-1,+1,+1)); //REAR_L 后左电机

MOTOR4=MOTORLimit(PIDMIX(+1,-1,+1)); //FRONT_L 前左电机

4.3. PID参数调节

PID参数的整定是PID控制的关键环节，直接影响到控制的效果。故一个PID设计的好不好往往要看其参数能否调节好，本设计的PID参数调节采用凑试法。凑试法是通过实际的闭环系统，通过观察系统的响应曲线，在本设计中通过观察被调量，PID输出，设定值三条曲线，判断出kp,ki,kd对系统响应的影响，反复尝试，最终达到满意响应，从而达到确定控制参数的kp,ki,kd的目的。在参数调节过程总遵循以下原则[17]:

（1）在输出不振荡时，增大比例增益P。

（2）在输出不振荡时，减小积分时间常数Ti。

（3）在输出不振荡时，增大微分时间常数Td。

（4）一般步骤：

a.确定比例增益P

确定比例增益P时，首先去掉PID的积分项和微分项，一般是令Ti=0、Td=0（具体见PID的参数设定说明），使PID为纯比例调节。输入设定为系统允许的最大值的60%~70%，由0逐渐加大比例增益P，直至系统出现振荡；再反过来，从此时的比例增益P逐渐减小，直至系统振荡消失，记录此时的比例增益P，设定PID的比例增益P为当前值的60%~70%。比例增益P调试完成。

b.确定积分时间常数Ti

比例增益P确定后，设定一个较大的积分时间常数Ti的初值，然后逐渐减小Ti，直至系统出现振荡，之后在反过来，逐渐加大Ti，直至系统振荡消失。记录此时的 Ti，设定PID的积分时间常数Ti为当前值的150%~180%。积分时间常数Ti调试完成。

c.确定积分时间常数Td

积分时间常数Td一般不用设定，为0即可。若要设定，与确定 P和Ti的方法相同，取不振荡时的30%。

d.系统空载、带载联调，再对PID参数进行微调