四轴飞行器的PID算法

四轴飞行器采用的是双闭环PID控制

当四轴飞行器正常飞行时，突遇外力(风等)或磁场干扰，使加速度传感器或磁力传感器采集数据失真，造成姿态解算出来的欧拉角错误，只用角度单环情况下，使系统很难稳定运行，因此可以加入角速度作为内环，角速度由陀螺仪采集数据输出，采集值一般不存在受外界影响情况，抗干扰能力强，并且角速度变化灵敏，当受外界干扰时，回复迅速；同理，高度环中气压传感器同样也会受到外界干扰，引入z轴加速度环可有效避免外界干扰造成的影响，增强了系统的鲁棒性。

四轴飞行器双闭环PID控制，如图3、图4所示。角度作为外环，角速度作为内环，进行姿态PID控制；当需要定高时，高度作为外环，z轴加速度作为内环，进行高度PID控制。其中，PID输出为油门值，油门给定电子调速器值，电子调速器控制电机使空间三轴欧拉角和高度变化。

图3姿态PID控制总体流程图

图4高度PID控制总体流程图

PID控制算法采用位置式数字PID控制：

式(7)中u(t)为PID输出值，e(t)为期望值与实际值之差，

为积分量，

为微分量，kp，、ki、kd。为比例、积分、微分系数。

在将积分量，微分量离散化得到PID计算公式

式(8)中T为更新时间。

基于公式(8)，姿态PID控制算法

式(9)为角度环PID计算公式，(10)为角速度环PID计算公式。AngelPIDOut(t)为角度环PID输出，AngelRatePIDOut(t)为角速度环PID输出，e(t)=期望角度一实际角度，e＇(t)=AngelPIDOut(t) - 实际角速度。

同理高度PID控制算法：

式(11)为高度环PID计算公式，公式(12)为加速度环PID计算公式，AltitudePIDOut(t)为高度环PID输出，AcceleratePIDOut(t)为加速度环PID输出。e(t)=期望高度一实际高度，e＇(t)=AltitudePIDOut(t) - (z轴加速度 - 重力加速度值)。