四旋翼飞行器的PID反馈控制——从水桶模型讲起，由浅入深

PID反馈控制

$$U(t)=K_p(err(t)+{\frac{1}{T}}_I\int err(t)dt+\frac{T_{D}derr(t)}{dt})$$

PID(proportion integration differentiation)是一种数学思想，P for 比例，I for 积分，D for 微分。常用于工程控制类领域中，调控某被控制量，这个被控制量可以是：温度，水位，速度等等。这些被控制量都有一个特点，当前状态到达目标状态需要一个过程，无法“一蹴而就”，这个过程可快可慢，可能会超过预设的目标值（超调），还可能一直在目标值附近徘徊（震荡）。而为了使这个过程尽可能符合人的预期，引入PID控制算法。

换言之，PID是给机器看的，机器是不会思考的，但PID可以帮助其“思考”，让它的行为更加“合理”。

水池模型

引入$K_p$

为了更好地理解PID的概念，我将从一个经典的“水池模型”讲起。

假设现在有一个水池，池子里现在没有水，现在向池子里注水，希望达到1m的高度。注意：这里的注水速率（m/s）不可突变，也就是说，不能前一秒还是10m/s的速率注水，下一秒速率马上就减为0了，这显然是不符合实际的。

那么此时有一个很自然的想法，我根据误差（err=1m-当前高度）去设定我当下的速率，当err=0（到达1m时），速率自然降为0。
$$v(t)=K_p\ast err(t)$$
$t$指$t$时刻，通过上面的公式，令err简单乘以一个比例系数（$k_p$，常数），得到当前速率$v(t)$。

具象一点，令Kp=0.5，采样周期为1s，那么t=0时，$v(0)=0.5\ast (1-0)=0.5$，也就是说，此时的进水速率是0.5m/s。那么第1s时（一次采样周期后），水位来到0.5m，此时$v(1)=0.5\ast (1-0.5)=0.25$，那么第2s时水位来到0.75m。。。于是经过若干次采样周期，水位必然会来到1m。

那么是否到这里问题就解决了呢？看起来我们仅仅需要$K_p$就可以将系统控制得很好。

然而在现实中，常常会出现稳态误差的问题。

还是刚才这个例子，假设这个水池存在“漏水”的问题：每次加水的过程中，池子都要漏掉0.1m高度的水。那么这样的设定会产生什么问题呢？假设经过了若干次加水，现在水位来到了0.8m，那么很容易算出下一次的进水速率时0.1m/s，而这个速率恰好=漏水速率。也就是说，下1s池子的水位将不会变化，因为进去的水不多不少都被漏出去了。

系统将一直维持在0.8m的水位，永远无法到达预设的1m水位。而这，便是所谓的稳态误差。

稳态误差实则是一个系统误差，是可预见的，是由于模型的缺陷导致，因为像例子中“漏水”的情况在现实中是几乎无可避免的。对于稳态误差的出现，笔者是这样理解的：
$$\begin{gathered} 模型由err给与系统一个增长的“动力”，这个“动力”随着err的变化而变化，最终必然与那个“下降”的趋势（系统原先就有的）相等， \\ 达成一个巧妙的平衡，但此时的平衡是err仍然存在的前提下的，因此这样的“稳定”是我们不想看到的。 \end{gathered}$$

引入$K_i$

为解决稳态误差的问题，我们需要去打破那个“平衡”。而当平衡产生时，err是静止的，因此随之带来的系统增长趋势也是静止的，如果没有外界的作用，这样的平衡会一直持续下去。

那么如何才能让机器“知道”这样的平衡已经维持太久了，有点不太对劲了呢？

让err和时间做一个积分，产生一个“补偿”，加在原有的增益上。于是便引入了$K_i$。

放在刚才的水池模型中，
$$v(t)=K_p\ast err(t)+K_i\ast {\int }_0^{t}err(i)\ast di$$

也就是说，构成v(t)的不光是刚才的比例项，还有一个关于err积分的积分项，那么当达到0.8m的稳态误差时，由于err始终无法消除，积分项会随时间不断累加，v增大，打破平衡，水位会继续上升，直到到达我们的预设值1m。

引入$K_d$

问题似乎已经完美地解决了，那么为什么还要引入$K_d$呢？

想下这样一个问题：当水位到达1m的预设值时，水位会马上停止上升吗?

显然不会，因为虽然此时err=0，比例项为0，但由于积分项是过去多次err的累加，因此，此时的积分项并不为0，根据公式，此时的v不会为0，水位还会继续上升。而像这样超过设定值的情况，工程上的学名叫**“超调”**。

不过不必担心，超调之后，err会变为负数，v（t）的比例项会变为负数，而积分项随着时间的累积也必然会变为负数。也就是说，一段时间后，v（t）会变为一个负数。表现在水池模型中，水位会下降。

但是当水位下降到1m时，系统同样会面临一个相同的问题——也就是“刹不住车”，水位会继续下降，低于目标值。这样的现象在工程中叫欠调。

然后水位又重新开始上升。。。反复几次最终趋于稳定，达到目标值。被控制量在超调和欠调之间反复切换的过程叫做震荡。

对于一个好的系统，我们肯定是不希望它震荡的，我们希望被控制量能尽快地到达设定值，且是平稳地到达，“软着陆”。

就如同汽车来到一个红灯前一样，快到停止线时，肯定会给一脚刹车，然后稳稳地停在线上。

那么如何给系统一个“刹车”呢？有一个很自然的想法：当被控制量快到设定值时，假如err变化过快，我就减缓系统的增益。那么如何衡量err的变化快慢呢？这里显然是一个微分的思想：
$$\frac{d(err)}{dt}$$
由于水位上升过程中，err是逐渐减小的，因此上面这个微分是一个负数，我们令其乘上一个比例系数$K_d$，加在v上，自然便会有一个“刹车”的效果。同时，err变化得越快，这个“刹车”效果就越明显，而这显然是我们想要的。

至此，v（t）的表达式便升级为：
$$v(t)=K_p\ast err(t)+K_i\ast {\int }_0^{t}err(i)\ast di+K_d\ast \frac{d(err)}{dt}$$
那么现在再回头看开头的公式：
$$U(t)=K_p(err(t)+{\frac{1}{T}}_I\int err(t)dt+\frac{T_{D}derr(t)}{dt})$$
其实这两个公式就是完全等价的，前者是3个自由系数$K_p,K_i,K_d$，后者是$K_p,T_I,T_D$，本质上是一回事，自由度是不变的。为了使用方便，我们常常用前面那个公式。

而在实际工程场景中，很少有真正“连续”的情况出现，往往都是“离散”的，传感器会有一个采样周期。因此公式可以改写为离散的形式：
$$u(k)=K_p\ast err(k)+K_i\ast \sum _{n=0}^{k}err(n)\ast dt+K_d\ast \frac{(err(k)-err(k-1))}{dt}$$
由此我们便得到了一个PID反馈模型，可以使系统快速，精准，平稳地达到我们所期望的状态。

PID实现四轴飞行器的姿态修正

在四旋翼飞行器的飞控算法中，PID反馈控制运用十分广泛。飞行器在空中极易受到外界因素的干扰和影响，气流的扰动，空气密度的变化，甚至是电池电压的变化都会对飞行器的姿态造成影响。

如果没有飞控的介入，四旋翼飞行器是绝对无法飞行的。有些人觉得四旋翼飞行器有四个螺旋桨，非常稳定，四个螺旋桨一起转升力抵消重力飞机就升起来了。其实不然，如果没有飞控，就算在无比理想的情况下（无风，无气流）飞机也绝对无法起飞，必定会失控。因为首先，实际飞机的重心绝不在正中心；其次，电机之间的运行状况可能会有极小的差别，哪怕是一模一样的品牌和型号。多种因素的叠加导致螺旋桨的升力与重力不在一条直线上，这将导致飞行器受到一个被翻转的扭矩，而这样的翻转无疑是发散的，也就是说就越翻越大，最终翻机。后果无疑是灾难性的！

因此，我们希望，当飞行器偏离预设的姿态时，系统能自动产生一个纠正的“回力”，保持预设的姿态。那么PID算法用在这里就十分合适。

四轴的PID又分为单环PID和双环PID。

单环PID：

这里先介绍单环的PID，经过上面的介绍，很容易想到这里的err就是当前姿态与期望姿态的误差。那么具体分解，姿态分为三个欧拉角pitch，roll，yaw。为方便理解，我们先只介绍一个角，pitch角，另外两个角的处理是十分类似的，最终做一个简单的叠加就可以实现整机的控制。

单环PID原理并不复杂，只需搞清楚每一个成分，“对号入座”即可。

对于pitch这个维度来说，$err=pitc{h}_{exp}-pitc{h}_{cur}$。而$err$的微分，也就是角度关于时间的导数，即角速度，我们可以直接通过陀螺仪读取。

于是我们可以很容易实现这部分代码：

//pitch
	Out_p.exp=pitch_exp;
	Out_p.cur=Angle.pitch;
	Out_p.err_sum+=(Out_p.exp-Out_p.cur);
	Pitch_Out=-1*(Out_p.Kp*(Out_p.exp-Out_p.cur) + Out_p.Ki*Out_p.err_sum*dt + Out_p.Kd*Gyro.y*RtA);

$pitch\_exp$是通过遥控器输入的，它反映了操控者对飞机姿态的期望；$Angle.pitch$是由imu姿态解算得到。dt为迭代周期，也就是这部分代码的执行周期。

注意：最后一行代码的最后一项，$Out\_p.Kd\ast Gyro.y\ast RtA$，多乘了一个$RtA$（弧度制到角度制的倍数），是为了和前面的项相统一，角度均为角度制，这样可以令PID的三个参数在一个数量级，方便后续调参。

那么这个PID迭代公式的输出是什么呢？因为他是直接作用于电机（电调）的，因此不难想到他是PWM的一个补偿，而这个补偿是带符号的，因为姿态的偏差自然是有正有负的。不同位置的电机，使用这部分补偿的符号是不同的（详见后文双环PID）。

双环PID：

单环PID只做引入，实际上，要想飞行器能够真正地平稳飞行，并拥有良好的抗干扰性和鲁棒性，必须使用双环PID（可以参考文末的实拍视频进行对比）。

双环PID分为角度环（外环）和角速度环（内环）。角度又有三个维度：pitch、roll、yaw，为了方便解释和理解，我们先只考虑pitch一个维度。

首先由遥控器输入一个期望的姿态角度，如果不做操作，那默认就是期望pitch和roll和yaw都为0，即姿态水平。外环借由姿态解算imu得到当前实际的欧拉角。实际角度和期望角度共同作为外环的输入，经过PID处理，产生一个输出，也就是期望的角速度。

类似的，这个期望的角速度和实际的角速度（陀螺仪直接采集得）又共同作为内环的输入。内环经过PID处理生成一个输出，这个输出是直接作用于电调，作用电机。

//pitch外环
	Out_p.exp=pitch_exp;
	Out_p.cur=Angle.pitch;//
	Out_p.err_sum+=(Out_p.exp-Out_p.cur);
	w_pitch_out=-1*(Out_p.Kp*(Out_p.exp-Out_p.cur) + Out_p.Ki*Out_p.err_sum*dt + Out_p.Kd*Gyro.y*RtA);//方向适配

//pitch内环
	In_p.exp=w_pitch_out;//输入w期望值
	In_p.cur=Gyro.y*RtA;//从陀螺仪获取当前值
	In_p.err_sum+=(In_p.exp-In_p.cur);//累计误差err
	Pitch_Out=In_p.Kp*(In_p.exp-In_p.cur) + In_p.Ki*In_p.err_sum*dt + In_p.Kd*(In_p.exp-In_p.cur-In_p.LastErr);//pid表达式，给出输出
	In_p.LastErr=In_p.exp-In_p.cur;//更新上一次的误差

如代码所示，内环的代码必须执行在外环代码之后，因此这里涉及一个简单的同步问题，尤其是涉及操作系统时必须注意。Pitch_Out直接作用于电机，并根据不同的螺旋桨位置，前面带一个正号或负号。

代码写好后，PID的编写远没有真正完成，外环、内环的Kp、Ki、Kd还是未知数，也就是说还有6个未知数（仅对于pitch），剩下的工作就是上飞机调试。调试成功的一个重要指标是：内环的实际角速度对于期望角速度有一个良好的跟随性。

由于需要不断尝试参数，因此不可能每次改变参数都烧录一次代码，这就需要远程改变参数，这里我们使用了蓝牙模块，可以实现不下飞机修改参数。同时，为了监测角速度的跟随性，我们还使用了一个可视化的航模调试工具：匿名四轴上位机。

如下图所示，这是我们的飞机调试完成后，所达到的角速度跟随效果：

pitch（黄线是预期角速度，蓝线是实际角速度）:

roll（蓝线是预期角速度，紫线是实际角速度）：

可以看到，实际角速度对于期望的角速度有一个较好的跟随性，前者略微落后于后者是十分正常的。

Pitch和Roll可以分开独立调试，Yaw情况较为简单，宽容度高，给一个不太激进的参数，甚至肉眼去调试即可。

最后的三个输出加上一个油门的基础值（speed），作用于电机，不同位置的螺旋桨，3个维度PID输出值带的符号是不同的，详见如下代码：

//电机代码
	Power1=speed+1000-1+Pitch_Out-Roll_Out+Yaw_Out;//螺旋桨1
	Power2=speed+1000-1+Pitch_Out+Roll_Out-Yaw_Out;//螺旋桨2
	Power3=speed+1000-1-Pitch_Out-Roll_Out-Yaw_Out;//螺旋桨3
	Power4=speed+1000-1-Pitch_Out+Roll_Out+Yaw_Out;//螺旋桨4
	
	PWM_SetCCR_1(Power1);//作用占空比，驱动电机
	PWM_SetCCR_2(Power2);
	PWM_SetCCR_3(Power3);
	PWM_SetCCR_4(Power4);

到这里，就可以尝试飞行了。

祝你好运！

实拍视频：

双环PID：
https://www.bilibili.com/video/BV1Qb4y137Jw/?spm_id_from=333.999.0.0&vd_source=b943c4e86faa94cd849c14cc66f912f3

单环PID：
https://www.bilibili.com/video/BV1CN4y1s74W/?spm_id_from=333.999.0.0&vd_source=b943c4e86faa94cd849c14cc66f912f3

飞行测试：
https://www.bilibili.com/video/BV1tG411671j/?spm_id_from=333.999.0.0