四轴PID控制算法

四轴控制原理

四轴飞行器的螺旋桨与空气发生相对运动，产生了向上的升力，当升力大于四轴的重力时四轴就可以起飞了。

四轴飞行器飞行过程中如何保持水平呢？

我们先假设一种理想状况，四个电机的转速是完全相同的，是不是我们控制四轴飞行器的四个电机保持同样的转速，当转速超过一个临界点时（升力刚好抵消重力）四轴就可以平稳的飞起来了呢？

答案是否定的，由于四个电机转向相同，四轴会发生旋转。我们控制四轴电机1和电机3同向（逆时针旋转），电机2电机4反向（顺时针旋转），刚好正反扭矩抵消，巧妙的实现了平衡，如下图所示。

实际上由于电机和螺旋桨本身制造的差异我们无法做到四个电机转速完全相同，很有可能飞行器起飞之后就侧翻。这时候大家可能会想到要用遥控器来控制电机，我们来尝试一下下面向右侧翻的情况。

由于电机的不平衡，在人眼的观察下发现飞机向右侧翻，我们控制右侧电机1电机2提高转速增加升力，飞机归于平衡。由于 飞机是一个动态系统，在接下来我们会一直重复：观察->大脑计算->控制->观察->大脑计算->控制。

但事实上这是不可能的，因为人无法长时间精确的同时控制四个电机。我们需要一个自动反馈系统替代人操作来完成飞机的自稳定，我们人只需要控制飞机的方向和高度就可以了。这个系统中反馈由姿态传感器替代眼睛，而大脑则由单片机来替代。这时候该PID控制系统出场。

PID控制理论

PID控制是最常见，应用最为广泛的自动反馈系统。PID控制器由偏差的比例（P，Proportional）、积分（I，Integral）和微分（D，Derivative）来对被控对象进行控制。这里的积分或微分，都是偏差对时间的积分或微分。

对于一个自动反馈控制系统来说，有几个基本的指标。

• 稳定性（P和I降低系统稳定性，D提高系统稳定性）：在平衡状态下，系统受到某个干扰后，经过一段时间其被控量可以达到某一稳定状态；
• 准确性（P和I提高稳态精度，D无作用）：系统处于稳态时，其稳态误差（Steady-state error）；
• 快速性（P和D提高响应速度，I降低响应速度）：系统对动态响应的要求。一般由过渡时间的长短来衡量。

比例（P）控制

比例控制是一种最简单的控制方式。其控制器的输出与输入误差信号成比例关系。当仅有比例控制时系统输出存在稳态误差。比例项输出：

积分（I）控制

在积分控制中，控制器的输出与输入误差信号的积分成正比关系。对于只有比例控制的系统存在稳态误差，为了消除稳态误差，在控制器中必须引入“积分项”。积分项是误差对时间的积分，随着时间的增加，积分项会增大。这样，即便误差很小，积分项也会随着时间的增加而加大，它推动控制器的输出增大使稳态误差进一步减小，直到等于零。因此，比例积分(PI)控制器，可以使系统在进入稳态后无稳态误差。 积分项输出：

微分（D）控制

在微分控制中，控制器的输出与输入误差信号的微分成正比关系。微分调节就是偏差值的变化率。使用微分调节能够实现系统的超前控制。如果输入偏差值线性变化，则在调节器输出侧叠加一个恒定的调节量。大部分控制系统不需要调节微分时间。因为只有时间滞后的系统才需要附加这个参数。微分项输出：

综上所述得到一条公式，这个就是PID控制数学表达式：

更多关于PID控制理论知识，可以参考维基百科PID控制。

四轴PID控制-单环

在Crazepony四轴飞行器5.0版本及以前，我们使用的是单环增量式PD控制，下面是角度单环PID控制框图。这里对图中几个数据进行说明，期望角度就是遥控器控制飞行器的角度值，反馈当前角度就是传感器测得的飞行器角度，这里的角度指的是Roll/Pitch/Yaw三个角度，而且在PID控制计算的时候，是相互独立的。

以ROLL方向角度控制为例：

• 测得ROLL轴向偏差：

偏差=目标期望角度-传感器实测角度 
DIF_ANGLE.X = EXP_ANGLE.X - Q_ANGLE.Roll;

• 比例项的计算：

比例项输出 = 比例系数P * 偏差
Proportion =  PID_Motor.P * DIF_ANGLE.X;

• 微分项计算： 由于陀螺仪测得的是ROLL轴向旋转角速率，角速率积分就是角度，那么角度微分即角速率，所以微分量刚好是陀螺仪测得的值。

微分输出=微分系数D*角速率
DifferentialCoefficient = PID_Motor.D * DMP_DATA.GYROx;

• 整合结果总输出为：

ROLL方向总控制量=比例项输出+微分量输出

ROLL和PITCH轴按照以上公式计算PID输出，但YAW轴比较特殊，因为偏航角法线方向刚好和地球重力平行，这个方向的角度无法由加速度计直接测得，需要增加一个电子罗盘来替代加速度计。如果不使用罗盘的话，我们可以单纯的通过角速度积分来测得偏航角，缺点是由于积分环节中存在积分漂移，偏航角随着时间的推移会偏差越来越大，就会出现航向角漂移的问题。我们不使用罗盘就没有比例项，只仅使用微分环节来控制。

• YAW轴输出：

微分输出=微分系数D*角速率

YAW方向控制量 = PID_YAW.D * DMP_DATA.GYROz;

四轴PID控制-串级

角度单环PID控制算法仅仅考虑了飞行器的角度信息，如果想增加飞行器的稳定性(增加阻尼)并提高它的控制品质，我们可以进一步的控制它的角速度，于是角度/角速度-串级PID控制算法应运而生。在这里，相信大多数朋友已经初步了解了角度单环PID的原理，但是依旧无法理解串级PID究竟有什么不同。其实很简单：它就是两个PID控制算法，只不过把他们串起来了(更精确的说是套起来)。那这么做有什么用？答案是，它增强了系统的抗干扰性(也就是增强稳定性)，因为有两个控制器控制飞行器，它会比单个控制器控制更多的变量，使得飞行器的适应能力更强。为了更为清晰的讲解串级PID，这里笔者依旧画出串级PID的原理框图，如图所示：

Crazepony在5.1版本及以后，就是采用角度/角速度串级PID控制。详见Control.c文件中的CtrlAttiAng(void)函数和CtrlAttiRate(void)函数。

如何整定串级PID时的经验，来自CSDN网友Nemo之家博客四轴PID讲解。原则是先整定内环PID，再整定外环P。

• 内环P：从小到大，拉动四轴越来越困难，越来越感觉到四轴在抵抗你的拉动；到比较大的数值时，四轴自己会高频震动，肉眼可见，此时拉扯它，它会快速的振荡几下，过几秒钟后稳定；继续增大，不用加人为干扰，自己发散翻机。特别注意：只有内环P的时候，四轴会缓慢的往一个方向下掉，这属于正常现象。这就是系统角速度静差。
• 内环I：前述PID原理可以看出，积分只是用来消除静差，因此积分项系数个人觉得没必要弄的很大，因为这样做会降低系统稳定性。从小到大，四轴会定在一个位置不动，不再往下掉；继续增加I的值，四轴会不稳定，拉扯一下会自己发散。特别注意：增加I的值，四轴的定角度能力很强，拉动他比较困难，似乎像是在钉钉子一样，但是一旦有强干扰，它就会发散。这是由于积分项太大，拉动一下积分速度快，给 的补偿非常大，因此很难拉动，给人一种很稳定的错觉。
• 内环D：这里的微分项D为标准的PID原理下的微分项，即本次误差-上次误差。在角速度环中的微分就是角加速度，原本四轴的震动就比较强烈，引起陀螺的值变化较大，此时做微分就更容易引入噪声。因此一般在这里可以适当做一些滑动滤波或者IIR滤波。从小到大，飞机的性能没有多大改变，只是回中的时候更加平稳；继续增加D的值，可以肉眼看到四轴在平衡位置高频震动(或者听到电机发出滋滋的声音)。前述已经说明D项属于辅助性项，因此如果机架的震动较大，D项可以忽略不加。
• 外环P：当内环PID全部整定完成后，飞机已经可以稳定在某一位置而不动了。此时内环P，从小到大，可以明显看到飞机从倾斜位置慢慢回中，用手拉扯它然后放手，它会慢速回中，达到平衡位置；继续增大P的值，用遥控器给不同的角度给定，可以看到飞机跟踪的速度和响应越来越快；继续增加P的值，飞机变得十分敏感，机动性能越来越强，有发散的趋势。

电机的输出整合

油门控制Throttle是电机输出的基准值，增加油门即可四轴升高，减小油门即下降。最后整合上面通过PID算得的ROLL/PITCH/YAW三轴输出量进行电机控制。整合如下，在Control.c文件的函数CtrlMotor()中实现。

Motor[2] = (int16_t)(Thr - Pitch - Roll - Yaw );    //M3
Motor[0] = (int16_t)(Thr + Pitch + Roll - Yaw );    //M1
Motor[3] = (int16_t)(Thr - Pitch + Roll + Yaw );    //M4
Motor[1] = (int16_t)(Thr + Pitch - Roll + Yaw );    //M2

Crazepony左下角为M1机臂，在代码中对应电机为Motor[0]，然后逆时针依次为M2，M3，M4，如图所示。如果四轴绕ROLL轴向右倾斜5度，那么电机1电机2应该提高升力，电机3电机0减小升力恢复平衡状态，所以有以下规则：

• Roll方向旋转，为了恢复平衡，则电机1电机2同侧出力，电机0电机3反向出力（Motor[1]和Motor[2]中的Roll为-，Motor[0]和Motor[3]中的Roll为+）
• Pitch方向旋转，为了恢复平衡，则电机2电机3同侧出力，电机0电机1反向出力（Motor[2]和Motor[3]中的Pitch为-，Motor[0]和Motor[1]中的Pitch为+）

• Yaw方向旋转，为了恢复平衡，则电机1电机3同侧出力，电机0电机2反向出力，（Motor[1]和Motor[3]中的Yaw为+，Motor[0]和Motor[2]中的Yaw为-）
• 转载http://www.crazepony.com/book/wiki/algorithm-pid.html
•