四轴PID控制算法详解(单环PID、串级PID)

正文开始：这篇文章分为三个部分：

• PID原理普及
• 常用四轴的两种PID算法讲解(单环PID、串级PID)
• 如何做到垂直起飞、四轴飞行时为何会飘、如何做到脱控？
  
  

PID原理普及

1、  对自动控制系统的基本要求：

        稳、准、快：

        稳定性（P和I降低系统稳定性，D提高系统稳定性）：在平衡状态下，系统受到某个干扰后，经过一段时间其被控量可以达到某一稳定状态；

        准确性（P和I提高稳态精度，D无作用）：系统处于稳态时，其稳态误差；

        快速性（P和D提高响应速度，I降低响应速度）：系统对动态响应的要求。一般由过渡时间的长短来衡量。

2、  稳定性：当系统处于平衡状态时，受到某一干扰作用后，如果系统输出能够恢复到原来的稳态值，那么系统就是稳定的；否则，系统不稳定。

3、  动态特性（暂态特性，由于系统惯性引起）：系统突加给定量（或者负载突然变化）时，其系统输出的动态响应曲线。延迟时间、上升时间、峰值时间、调节时间、超调量和振荡次数。

通常：    上升时间和峰值时间用来评价系统的响应速度；

                超调量用来评价系统的阻尼程度；

                调节时间同时反应响应速度和阻尼程度；

4、  稳态特性：在参考信号输出下，经过无穷时间，其系统输出与参考信号的误差。影响因素：系统结构、参数和输入量的形式等

5、  比例（P）控制规律：具有P控制的系统，其稳态误差可通过P控制器的增益Kp来调整：Kp越大，稳态误差越小；反之，稳态误差越大。但是Kp越大，其系统的稳定性会降低。

由上式可知，控制器的输出m(t)与输入误差信号e(t)成比例关系，偏差减小的速度取决于比例系数Kp：Kp越大，偏差减小的越快，但是很容易引起振荡（尤其是在前向通道中存在较大的时滞环节时）；Kp减小，发生振荡的可能性小，但是调节速度变慢。单纯的P控制无法消除稳态误差，所以必须要引入积分I控制。原因：（R为参考输入信号，Kv为开环增益）

当参考输入信号R不为0时，其稳态误差只能趋近于0，不能等于0。因为开环增益Kv不为0。

6、 比例微分（PD）控制规律：可以反应输入信号的变化趋势，具有某种预见性，可为系统引进一个有效的早期修正信号，以增加系统的阻尼程度，而从提高系统的稳定性。（tao为微分时间常数）

如果系统中存在较大时滞的环节，则输出变化总是落后于当前误差的变化，解决的方法就是使抑制误差的作用变化“超前”，增强系统的稳定性。

7、  积分（I）控制规律：由于采用了积分环节，若当前误差e(t)为0，则其输出信号m(t)有可能是一个不为0的常量。需要注意的是，引入积分环节，可以提到系统型别，使得系统可以跟踪更高阶次的输入信号，以消除稳态误差。

8、 比例积分（PI）控制规律：在保证系统稳定的前提下，引入PI控制器可以提高它的稳态控制质量，消除其稳态误差。（TI为积分时间常数）

        积分调节可以消除静差，但有滞后现象，比例调节没有滞后现象，但存在静差。

        PI调节就是综合P、I两种调节的优点，利用P调节快速抵消干扰的影响，同时利用I调节消除残差。

9、 比例积分微分（PID）控制规律：除了积分环节提高了系统型别，微分环节提高了系统的动态性能。

观察PID的公式可以发现：Kp乘以误差e(t)，用以消除当前误差；积分项系数Ki乘以误差e(t)的积分，用于消除历史误差积累，可以达到无差调节；微分项系数Kd乘以误差e(t)的微分，用于消除误差变化，也就是保证误差恒定不变。由此可见，P控制是一个调节系统中的核心，用于消除系统的当前误差，然后，I控制为了消除P控制余留的静态误差而辅助存在，对于D控制，所占的权重最少，只是为了增强系统稳定性，增加系统阻尼程度，修改PI曲线使得超调更少而辅助存在。

10、P控制对系统性能的影响：

• 开环增益越大，稳态误差减小（无法消除，属于有差调节）
• 过渡时间缩短
• 稳定程度变差
  
  

11、I控制对系统性能的影响：

• 消除系统稳态误差（能够消除静态误差，属于无差调节）
• 稳定程度变差
  
  

12、D控制对系统性能的影响：

• 减小超调量
• 减小调节时间（与P控制相比较而言）
• 增强系统稳定性
• 增加系统阻尼程度
  
  

13、PD控制对系统性能的影响：

• 减小调节时间
• 减小超调量
• 增大系统阻尼，增强系统稳定性
• 增加高频干扰
  
  

14、PI控制对系统性能的影响：

• 提高系统型别，减少系统稳态误差
• 增强系统抗高频干扰能力
• 调节时间增大
  
  

15、P调节、I调节降低系统稳定性

        D调节增强系统稳定性

        所以PI调节器的P比P调节器的P要小一些，PD调节器的P比P调节器的P要大一些

16、位置式PID表达式（数字PID）：

P(n)为第n次输出，e(n)为第n次偏差值，Ts为系统采用周期，Ti为积分时间常数，Td为微分时间常数

17、消除随机干扰的措施：

• 几个采样时刻的采样值求平均后代替本次的采样值
• 微分项的四点中心差分(e(n)-e(n-3)+3e(n-1)-3e(n-2))*1/(6Ts)
• 矩形积分改为梯形积分  
  
  

18、PID调试一般原则

• 在输出不振荡时，增大比例增益P
• 在输出不振荡时（能消除静态误差就行），减小积分时间常数Ti
• 在输出不振荡时，增大微分时间常数Td
  
  

19、描述比例Kp的性能：比例带。比例带就是Kp的倒数：比例带越大，Kp越小，无超调，稳态误差大，调节时间长；比例带越小，Kp越大，系统会有超调，甚至发散，稳态 误差减小，调节时间缩短

20、描述积分Ki的性能：积分时间常数Ti。与积分系数Ki也是倒数关系：积分时间常数Ti越大，积分系数Ki越小，系统稳定性增加，但是调节速度变慢；积分时间常数Ti越小， 积分系数Ki越大，系统稳定性降低，甚至振荡发散。无论增大还是减小积分时间常数Ti，被调量最后都没有静差。

21、描述微分Kd的性能：微分时间常数Td。主要用于克服调节对象有较大的时滞。Td越大，微分作用越强，系统阻尼程度增加。

22、比例P调节作用：

系统一旦出现偏差，比例调节立即产生调节作用用以减少偏差

比例作用大，可以加快调节，减少调节时间，减少稳态误差

过大的比例作用，使系统的稳定性下降

23、积分I调节作用：

消除稳态误差

系统稳定性下降，动态响应变慢

24、微分D调节作用：

具有预见性，能预见偏差变化趋势，有超前的控制作用，增强系统动态性能

减少超调量，减少调节时间

25、比例积分PI调节器

• 引入积分消除了稳态误差，但是降低了原有系统的稳定性
• 超调趋势随着Kp增大、积分时间Ti减小而增大
  
  

26、积分分离的措施：（在系统启动，结束或者大幅度增减时，短时间系统输出会有很大偏差，造成PID运算的积分积累，致使控制量超过执行结构可能允许的最大动作范围， 引起系统较大的超调，甚至振荡）

• 当误差大于某个阈值时，采用PD控制，当误差在该阈值范围内，采用PID控制
  
  

注意：阈值的选取，过大，则达不到积分分离的目的；过小，则会导致无法进入积分区

27、抗积分饱和的措施：

• 限制PI调节器的输出（这样有可能在正常操作中不能消除系统的余差）
• 积分分离法：误差在某个范围内开启积分调节（既不会积分饱和又能在小偏差时利用积分作用消除偏差）
• 遇限削弱积分法：调节器输出大于某个值后，只累加负误差（可避免控制量长时间停留在饱和区）
  
  

28、比例微分PD调节器

• 仍然属于有差调节
• 提高了系统的稳定性，因为微分D的作用总是试图抑制被调量的振荡，所以也增加了系统的阻尼程度
• 因为提高了系统的稳定性，所以可以适当加大Kp
• D只是辅助作用，主要还是P控制
  
  

29、MATLAB仿真

纯P调节（Kp大，稳态误差小，响应快，但超调大）

PI调节（Ti小，响应速度加快，超调大，系统振荡加剧）

PI调节（在同样积分常数Ti下，减小比例增益Kp可减小

超调，增加系统的稳定性）

PD调节（引入微分项，提高了响应速度，增加了系统的稳定性但不能消除系统的余差）

PD调节（微分时间越大，微分作用越强，响应速度越快，系统越稳定）

PID调节（PD基础上I作用的引入消除了余差，达到了理想的多项性能指标要求：超调、上升时间、调节时间、余差等）

30、PID参数整定需要查看三种基本曲线，缺一不可：

• 设定值
• 被调量
• PID输出
  
  

如果是串级调节系统，还需要收集：

• 副调的被调量
• 副调PID输出
  
  

31、在整定PID参数时，PID三个参数的大小都不是绝对的，而是相对的。也就是说，如果发现一个参数比较合适，就把这个参数固定死，不管别的参数怎么变化，永远不动前 面固定的参数。这是要不得的。

32、如果是串级调节系统，在整定参数时，一般把主、副调隔离开来，先整定一个回路，再全面考虑。一般而言，先整定内回路。把PID参数隔离开来，先去掉积分、微分作 用，让系统变为纯比例调节方式，再考虑积分，最后考虑微分

33、整定比例带：

方法：逐渐加大比例作用，一直到系统发生等幅振荡，记录下此时的比例增益，乘以0.6~0.8即可

注意1：比例作用很强时的振荡周期很有规律，基本上呈正弦波；而在极弱比例作用参数下的系统有时也会呈现出有规律的振荡，但是往往参杂了几个小波峰。

现象：最终整定的系统，其调节效果应该是被调量波动小而平缓。在一个扰动过来之后，被调量的波动应该呈现“一大一小两个波”（波形高度差4:1）

注意2：如果看不到这种被调量的周期特征，那说明参数整定的很好。即满足快速性，也不会超调

整定积分时间：

方法：主调的作用是为了消除静态偏差，当比例作用整定好的时候，就需要逐渐加强积分作用（调小积分时间Ti或者增大积分项系数Ki），直到消除静差为止。也就是说， 积分作用只是辅助比例作用进行调节，它仅仅是为了消除静态偏差。

整定微分作用：

方法：逐渐加强微分作用（增加微分时间Td或者增加微分项系数Kd），直到PID输出毛刺过多

34、串级调节系统，一般而言，主调的比例弱，积分强，以消除静差；副调的比例强，积分弱，以消除干扰。但是不绝对！

35、不完全微分PID控制：微分控制对高频干扰非常敏感，为了避免在误差扰动突变时的微分控制的不足，在微分项通道前加一个低通滤波器可以使得系统性能得到改善

36、微分先行PID控制：只对被调量进行微分，而不对给定值进行微分。这样的处理在改变给定值时，输出不会改变，被控量的变化通常比较缓和，适用于给定值频繁升降的场合。

常用四轴的两种PID算法讲解(单环PID、串级PID)

        这里主要讲解的PID算法属于一种线性控制器，这种控制器被广泛应用于四轴上。要控制四轴，显而易见的是控制它的角度，那么最简单，同时也是最容易想到的一种控制策略就是角度单环PID控制器，系统框图如图所示：

或许有些朋友看得懂框图，但是编程实现有一定困难，在这里笔者给出了伪代码：

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

同样，为了帮助一些朋友编程实现，给出串级PID伪代码：

关于如何整定单环PID与串级PID的问题，请原谅笔者的能力有限，无法给出标准而可靠的整定流程，这里我给出三个链接，第一个为阿莫论坛的一位同学的单环PID整定现象与思考，个人觉得参考价值很大；第二、三两个分别为APM与PX4的串级PID整定现象说明，大家可以参考他们的网页说明。

www.amobbs.com/thread-5554367-1-1.html

copter.ardupilot.com/wiki/configuration/tuning/

pixhawk.org/users/multirotor_pid_tuning

而笔者在整定串级PID时的经验则是：先整定内环PID，再整定外环P。

内环P：从小到大，拉动四轴越来越困难，越来越感觉到四轴在抵抗你的拉动；到比较大的数值时，四轴自己会高频震动，肉眼可见，此时拉扯它，它会快速的振荡几下，过几秒钟后稳定；继续增大，不用加人为干扰，自己发散翻机。

特别注意：只有内环P的时候，四轴会缓慢的往一个方向下掉，这属于正常现象。这就是系统角速度静差。

内环I：前述PID原理可以看出，积分只是用来消除静差，因此积分项系数个人觉得没必要弄的很大，因为这样做会降低系统稳定性。从小到大，四轴会定在一个位置不动，不再往下掉；继续增加I的值，四轴会不稳定，拉扯一下会自己发散。

特别注意：增加I的值，四轴的定角度能力很强，拉动他比较困难，似乎像是在钉钉子一样，但是一旦有强干扰，它就会发散。这是由于积分项太大，拉动一下积分速度快，给  的补偿非常大，因此很难拉动，给人一种很稳定的错觉。

内环D：这里的微分项D为标准的PID原理下的微分项，即本次误差-上次误差。在角速度环中的微分就是角加速度，原本四轴的震动就比较强烈，引起陀螺的值变化较大，此时做微分就更容易引入噪声。因此一般在这里可以适当做一些滑动滤波或者IIR滤波。从小到大，飞机的性能没有多大改变，只是回中的时候更加平稳；继续增加D的值，可以肉眼看到四轴在平衡位置高频震动(或者听到电机发出滋滋的声音)。前述已经说明D项属于辅助性项，因此如果机架的震动较大，D项可以忽略不加。

外环P：当内环PID全部整定完成后，飞机已经可以稳定在某一位置而不动了。此时内环P，从小到大，可以明显看到飞机从倾斜位置慢慢回中，用手拉扯它然后放手，它会慢速回中，达到平衡位置；继续增大P的值，用遥控器给不同的角度给定，可以看到飞机跟踪的速度和响应越来越快；继续增加P的值，飞机变得十分敏感，机动性能越来越强，有发散的趋势。

如何做到垂直起飞、四轴飞行时为何会飘、如何做到脱控？

        眼看这三个问题是三个不同的问题，其实就原理上讲，他们的原因在绝大多数情况下都是由于加速度计引起的。如果飞机可以垂直起飞，说明你的加速度计放置地很水平，同时也说明你的PID控制算法参数找的不错，既然可以垂直起飞，那么飞行过程中，只要无风，四轴几乎就不会飘，自然而然就可以脱控飞行。由此可见，加速度计是个十分重要的器件。

        在姿态解算中，或者说在惯性导航中，依靠的一个重要器件就是惯性器件，包括了加速度计和陀螺仪。陀螺仪的特性就是高频特性好，可以测量高速的旋转运动；而加速度计的低频特性好，可以测量低速的静态加速度。无论是何种算法(互补滤波、梯度下降、甚至是Kalman滤波器)，都离不开对当地重力加速度g的测量和分析。惯性导航利用的就是静态性能好的加速度计去补偿动态性能好的陀螺仪漂移特性，得到不飘并且高速的姿态跟踪算法，因此基于惯性器件的姿态解算，加速度计是老大，它说了算。

        下面，我给大伙推理一下四轴如何平稳飞信的思路，欢迎各位批评指出：:-)

        首先，为了让四轴平稳的悬停或飞行在半空中，四个电机必须提供准确的力矩->假设力矩与电机PWM输出呈线性关系，也就是必须提供准确的4路PWM->4路PWM由遥控器输入(期望角度)、PID算法及其参数和姿态解算输出(当前角度)组成，假设遥控器输入不变(类似脱控)、PID算法及其参数也较为准确(PID参数无需十分精确，但只要在某个合理的范围内，控制品质差不了多少)，也就是姿态解算的输出必须是十分准确的，可以真实反应飞行器的实际角度->姿态解算的结果由加速度计和陀螺仪给出，根据前述惯性导航的描述，加速度计补偿陀螺仪，因此要得到精确的姿态解算结果，务必要求加速度输出精确的重力加速度g->这里仅讨论悬停飞行，因此忽略掉额外的线性加速度(事实证明，在四轴强机动飞行过程中，线性加速度必须要考虑并消除)，假设加速度计输出重力加速度g，这个重力加速度g必须十分“精确”。

        我先写到这里，总结一下：精准力矩->精准PWM->精准姿态->加速度计输出“精确”重力加速度g。这里的“精确”打了引号，意思不是说加速度的性能十分好，要输出精确的当地加速度g，而是说它能够准确反应机架的角度。为了达到悬停、平稳的飞行效果，控制算法输出的PWM会让加速度计输出的重力加速度g在XOY平面内的分量就可能少，也就是说：PID控制算法控制的不是机架水平，而是加速度计水平，PID不知道机架是什么东西，它只认加速度计，它的使命就是让加速度计水平。我现在假设加速度计与机架存在某个角度，比如右倾30°，四轴主视图如图所示。

上图中，加速度计(红线)与四轴机架的水平面(虚线)呈30°。起飞后，PID控制算法会尝试将加速度计调整至水平位置，因此四轴就会往图中左边飘，倾斜角度也为30°。这就是为什么飞机无法垂直起飞，或者飞行过程中往一个方向飘的原因：加速度计和机架没有水平。因此在加速度计的机械安装时，尽量保证加速度计与机架水平。如果有些朋友已经将加速度计固定在飞控板上，可以通过遥控器的通道微调功能设置悬停时的期望角度，软件上校正这种机械不水平。

        除了上述讨论的加速度计安装水平问题，也需要对加速度计进行零偏置校正，具体的方法叫做6位置标定法：即将加速度计沿着6个方向放置，分别记录重力加速度计g在6个方向上的最大输出值，然后取平均，得到圆球的中心点(这里假设g投影为球，实际上为椭球，需要进行最小二乘法拟合求三轴标定系数)。

        最后提一点，如果加速度计和遥控器均做了调整，飞机可以做到垂直起飞，并且飞行效果还行，但是飞机的回中速度较慢，感觉就像是在抬轿子一样。具体描述：悬停时，猛往一个方向打摇杆后立即放手，飞机会往摇杆方向走很远才停下。这是因为飞机过于稳定，也就是内环的作用过强或者外环作用过弱导致，解决方法是降低内环P或者加大外环P。

以上内容，均为笔者学习过程中的体会和想法，难免有错误之处，还请诸位批评指出，共同学习进步。