PID_OUT = （Kp × 误差）+ （Ki × 累计误差）+（Kd × 误差间的差值）

常用PID算法：
①PI算法：
特点：

从时域上看，只要存在偏差，积分就会不停对偏差积累，因此稳态时误差一定为零
不足：比例与积分动作都是对过去控制误差进行操作， 不对未来控制误差进行预测，限制了控制性能。
PI调节将比列调节的快速反应与积分调节消除静差的特点结合，实现好的调节效果。
PI调节适用于控制通道滞后较小、负荷变化不大、 工艺参数不允许有静差的系统。
②PD算法：
适用于舵机快速响应

对于惯性较大的对象，常常希望能加快控制速度， 此时可增加微分作用。

特点：

比例控制对于惯性较大对象，控制过程缓慢，控制品质不佳。比例微分控制可提高控制速度，对惯性较大对象，可改善控制质量，减小偏差，缩短控制时间。
理想微分作用持续时间太短， 执行器来不及响应。实际使用中，一般加以惯性延迟，称为实际微分。
PD 调节以比例调节为主，微分调节为辅，PD调节是有差调节。
PD 调节具有提高系统稳定性、抑制过渡过程最大动态偏差的作用。
PD 调节有利于提高系统响应速度。
PD 调节抗干扰能力差，一般只能应用于被调参数 变化平稳的生产过程。
微分作用太强时，容易造成系统振荡。
③PID算法：
将比例、积分、微分三种调节规律结合在一起， 只要三项作用的强度配合适当，既能快速调节，又能消除余差，可得到满意的控制效果。

特点：

PID 控制作用中，比例作用是基础控制；微分作用是 用于加快系统控制速度；积分作用是用于消除静差。

只要比例、积分、微分三种控制规律强度配合适当， 既能快速调节，又能消除余差，可得到满意控制效果。

Kp 较小时，系统对微分和积分环节的引入较为敏感，积分会引起超调，微分可能会引起振荡，而振荡剧烈的时候超调也会增加。

Kp 增大时，积分环节由于滞后产生的超调逐渐减小，此时如果想要继续减少超调可以适当引入微分环节。继续增大 Kp 系统可能会不太稳定，因此在增加 Kp 的同时引入 Kd 减小超调，可以保证在 Kp 不是很大的情况下也能取得较好的稳态特性和动态性能。

Kp 较小时，积分环节不宜过大，Kp 较大时积分环节也不宜过小（否则调节时间会非常地长），当使用分段PID ，在恰当的条件下分离积分，可以取得更好的控制效果。原因在于在稳态误差即将满足要求时，消除了系统的滞后。因此系统超调会明显减少。

增量式PID的优缺点：
位置型PID和增量型PID的比较：

位置式需引入初始阀位值，增量式不需要
位置式需防积分饱和，增量式不会产生积分饱和
只有存在偏差时，增量式才会有输出
增量式容易实现从手动到自动的切换
位置式PID控制的输出与整个过去的状态有关，用到了误差的累加值;而增量式PID的输出只与当前拍和前两拍的误差有关，因此位置式PID控制的累积误差相对更大;
优：

①算式中不需要累加。控制增量Δu(k)的确定仅与最近3次的采样值有关，容易通过加权处理获得比较好的控制效果；
②计算机每次只输出控制增量，即对应执行机构位置的变化量，故机器发生故障时影响范围小、不会严重影响生产过程；
③手动—自动切换时冲击小。当控制从手动向自动切换时，可以作到无扰动切换。

缺：

①需要对控制量进行记忆。

②存在稳态误差

PID优化方法：
①抗积分饱和：
如果系统总是存在统一的方向偏差，就可能无限累加而进行饱和，极大影响系统性能

所谓积分饱和就是指系统存在一个方向的偏差，PID控制器的输出由于积分作用的不断累加而扩大，从而导致控制器输出不断增大超出正常范围进入饱和区。当系统出现反响的偏差时，需要首先从饱和区退出，而不能对反向的偏差进行快速的响应。

为了解决积分饱和的问题，人们引入了抗积分饱和的PID算法。所谓抗积分饱和算法，其思路是在计算U(k)的时候，先判断上一时刻的控制量U(k-1)是否已经超出了限制范围。若U(k-1)>Umax，则只累加负偏差；若U(k-1)

几种方法抗饱和积分：

积分分离或者称为去积分算法，变速积分（积分项和误差相映射）；

增量式PID分步积分与变速积分的融合实现_control_zj的博客-CSDN博客

在饱和的时候将积分器的累计值初始化到一个比较理想的值；

若积分饱和因为目标值突然变化而产生，将目标值以适当斜率的斜坡变化可避免此情形；

将积分累计量限制上下限，避免积分累计量超过限制值；

如果PID输出已经饱和，重新计算积分累计量，使输出恰好为合理的范围；

PID的TRICK(一)简述五种PID积分抗饱和（ANTI-Windup）方法 - 知乎 (zhihu.com)

系统对于积分项的要求是，系统偏差大时，积分作用应该减弱甚至是全无，而在偏差小时，则应该加强。积分系数取大了会产生超调，甚至积分饱和，取小了又不能短时间内消除静差。因此，根据系统的偏差大小改变积分速度是有必要的。 变积分PID的基本思想是设法改变积分项的累加速度，使其与偏差大小相对应：偏差越大，积分越慢; 偏差越小，积分越快。

②不完全微分：
不完全微分_百度百科 (baidu.com)

不完全的微分它使得在偏差作阶跃式变化时出现的输出瞬时跳变得到一定程度的缓和，因而在实际的PID控制算法中得到广泛采用。在PID 控制中，微分信号的引入可以改善系统的动态特性，但也易引入高频干扰，在误差扰动突变的时候尤其显出微分项的不足。要想解决这个问题，可以在控制算法中加入低通滤波器

标准微分会产生短时间跳变，实际系统不能完全响应，效果不好，会震荡。

③微分先行：
某些给定值频繁且大幅变化的场合，微分项常常会引起系统的振荡。为了适应这种给定值频繁变化的场合，人们设计了微分先行算法。

类似于不完全微分（不完全微分是通过削弱变化量来尽可能的实现完全响应，而微分先行PID控制是只对输出量进行微分，而对给定指令不起微分作用），微分部分只与测量值有关，而且与连续的几个测量值都有关。而与设定值没有关系，设定值的阶跃变化不会造成高频的干扰。

④带死区PID控制：
在偏差小到一点程度时PID不起作用

PID控制器开发笔记之八：带死区的PID控制器的实现_木南创智-CSDN博客

在计算机控制系统中，由于系统特性和计算精度等问题，致使系统偏差总是存在，系统总是频繁动作不能稳定。为了解决这种情况，我们可以引入带死区的PID算法。

带死区的PID控制算法就是检测偏差值，若是偏差值达到一定程度，就进行调节。若是偏差值较小，就认为没有偏差。用公式表示如下：

 

其中的死区值得选择需要根据具体对象认真考虑，因为该值太小就起不到作用，该值选取过大则可能造成大滞后。

带死区的PID算法，对无论位置型还是增量型的表达式没有影响，不过它是一个非线性系统。

除以上描述之外还有一个问题，在零点附近时，若偏差很小，进入死去后，偏差置0会造成积分消失，如是系统存在静差将不能消除，所以需要人为处理这一点。

引入死区的主要目的是消除稳定点附近的波动，由于测量值的测量精度和干扰的影响，实际系统中测量值不会真正稳定在某一个具体的值，而与设定值之间总会存在偏差，而这一偏差并不是系统真实控制过程的反应，所以引入死区就能较好的消除这一点。

当然，死区的大小对系统的影响是不同的。太小可能达不到预期的效果，而太大则可能对系统的正常变化造成严重滞后，需要根据具体的系统对象来设定。

⑤串级PID
类比于小车上坡，因为单环PID输出的速度 V 不一定是真实的速度 V，所以内环再加上速度环PID，构成串级PID（外环为位置环，输出的值是小车的理论速度，内环为速度环，输入是小车的理论速度，希望尽可能的使输出为理论速度），外环的输出是内环的输入

同样类比于四轴，外环为角度环，输出的是期望达到该角度所需要的PWM（也就是角速度，也可以理解为角速度和PWM的映射），内环为角速度环，输入是期望的角速度和自身真实的角速度，输出为最终的PWM（角速度）

串级PID可增加系统的稳定性，抗干扰

为什么外环的输入是内环的期望呢？因为你在设计控制器的时候，内环的目的就是实现用执行器控制直接能改变的变化量，外环的目的就是找到合适的曲线，来引导目标的微分变化，从而达到间接控制的效果。

为什么要用串级PID呢
看上去PID不是已经够完美了吗？只要我能给出反馈，我就能控制所以的物理量不是吗？

为什么还要双闭环PID呢？那为什么说外环输入是内环期望呢？为什么位置误差经过PID就变成速度期望了呢？

那我们就来设计一个位置控制器吧！

感觉好像没什么问题，但是跟之前有些许不一样，之前我们的**「执行器能直接改变反馈值」**，但是在这个控制器里，执行器依然只能改变升力，只不过，因为升力改变，会有加速度，从而导致速度变化，最终导致位置变化。

如果你按照图中的思路设计控制器，那就说明**「你只对位置有期望」**，这意味着你对升力，速度没有期望，也就意味着加速度，速度是什么都无所谓。

所以实际效果变成什么呢？「你会以一个你无所谓的加速度，和速度，到达你的期望位置。」

这时候奇怪的事情发生了，到达你的期望位置时有加速度和速度，那这个就不是**「到达期望位置」了，这叫「经过期望位置」**。你会发现你的对象在期望位置附近“反复横跳”，超过后，又退回去，退过了，又超过去。

这时候你才想起来，原来**「你期望的是停在期望位置上」。「什么叫停在期望位置上？就是到达期望位置的同时加速度与速度都为0。」**

这个问题是如何产生的，为什么之前没有这个问题。

之前在单级PID时我们举的例子中，期望被控制的物理量，都是可以直接被执行器改变的。

但是在这个例子中，「期望被控制的物理量，不能被执行器直接改变，但是可以被间接改变，并且你知道间接改变的过程。」

之前我们想控制位置，类似于控制机器人，给出命令，它直接走到对应的位置。

现在我们想控制位置，类似于控制车，你只能踩油门加速，或者刹车进行减速，你只能通过速度控制间接控制位置。

但是你肯定知道，车我们也是可以控制位置的，还记得驾校里的倒车入库吗？不就是让你精准的控制位置吗？

开车的时候是怎么控制位置的呢？你想停在指定位置的时候，你先以一定的速度前往指定位置，快到的时候，通过刹车不断减速，直到速度为0，停到期望的位置。

这个过程告诉了我们什么？我们只通过控制速度就可以间接控制位置，也就是说**「我们可以通过控制某个物理量的变化快慢，从而间接控制这个物理量。」**

所以我们只需要选择**「合适的速度变化曲线」**就可以停在我们想要的位置。那我们就可以把控制器设置成下面的结构。

什么是**「合适的速度变化曲线」**呢，

首先当到达期望位置的时候，速度应该为0。

其次当没有到达期望位置的时候，期望速度是可以减小位置误差的。

总结一下：「期望速度必须能减小位置误差，且位置误差为0时，期望速度为0。」

什么意思呢？如果前方为正方向，当期望位置在我们前方时，即target-current>0,那我们就希望能有个向前的正速度，去缩小这个误差，当误差为0时，我们希望速度为0。满足这样的过程的速度变化曲线，就是**「合适的速度变化曲线」**。

想想开车的时候，快到终点的时候会出现什么操作？

新手在没达位置的时候，希望有一个比较快的速度，快到了直接一脚刹车踩到底。

高手可以提前预判，匀速降低速度。

更高阶的可以变化的减速让整个速度变化如丝般顺滑。

就有了下面的曲线。

（第一个曲线是实际中是做不到的，因为速度减小需要过程，不能突变）

因为我们对期望速度的要求**「期望速度必须能减小位置误差，且位置误差为0时，期望速度为0。「所以我们发现这些期望速度都是」与位置误差相关的且经过0点的曲线」**。

位置误差与期望速度在这时候才建立起的关系，注意，强调一遍，是我们主动把位置误差与期望速度联系起来的，起因是我们希望控制速度达到控制位置的效果。

而这种控制能够被实现的本质是，「速度与位置存在我们已知的简单变化关系，即速度的积分是位置」，所以改变期望速度才能改变位置，才能减小误差。

所以合理的控制过程应该就可以设计成这样。

只要找个合适的函数，根据误差生成合适的期望速度曲线就可以了。

我们最常使用的是什么样的曲线呢？

是不是非常熟悉。

因为这两个曲线满足**「合适的期望速度」**的要求，同时也非常的简单。

只要是满足**「合适的期望速度」**的曲线都可以作为期望速度，显然如果在外环使用PID也是可以的，但是为什么实际中不这么用呢？因为添加积分(I)后，导致响应变慢，添加微分(D)容易引入噪声，只用P又简单又有效，何乐而不为呢？

到这里外环的输出终于理所应当的变成了内环的期望，但是这并不是因为这件事本就应该成立，而是通过你的设计，和真实存在的物理关系，才设计出来的合情合理的双闭环控制器。

我们再回忆一下这个控制器的设计过程

「1.执行器输出无法直接控制目标物理量。」

「2.发现控制器能控制物理量的变化量(微分)。」

「3.你想了个办法，可以通过控制物理量的微分，按照某种曲线变化，从而达到间接控制目标物理量。」

「4.这个曲线必须满足两点，存在目标物理量误差的时候，应该产生因一个能减小这个误差的变化，且当没有误差的时候，变化量也应该为0。」

「5.选择最简单实用的曲线即可，所以外环选择KP。」

「6.间接控制能成功的原因是因为，执行器能改变的量，与目标物理量之间有简单的物理关系。」

所以为什么外环的输入是内环的期望呢？因为你在设计控制器的时候，内环的目的就是实现用执行器控制直接能改变的变化量，外环的目的就是找到合适的曲线，来引导目标的微分变化，从而达到间接控制的效果。

位置型PID算法实现

/*定义结构体和公用体*/
typedef struct
{
  float setpoint;               /*设定值*/
  float kp;                     /*比例系数*/
  float ki;                     /*积分系数*/
  float kd;                     /*微分系数*/
  float lasterror;              /*前一拍偏差*/
  float preerror;               /*前两拍偏差*/
  float deadband;               /*死区*/
  float result;                 /*PID控制器计算结果*/
  float output;                 /*输出值0-100%*/
  float maximum;                /*输出值上限*/
  float minimum;                /*输出值下限*/
  float errorabsmax;            /*偏差绝对值最大值*/
  float errorabsmin;            /*偏差绝对值最小值*/
  float alpha;                  /*不完全微分系数*/
  float derivative;              /*微分项*/
  float integralValue;          /*积分累计量*/
}CLASSICPID;

接下来我们实现带死区、抗积分饱和、梯形积分、变积分算法以及不完全微分算法的增量型PID控制器。

 

void PIDRegulator(CLASSICPID vPID,float pv)
{
  float thisError;
  float result;
  float factor;
 
  thisError=vPID->setpoint-pv; //得到偏差值
  result=vPID->result;
  if(fabs(thisError)>vPID->deadband)
  {
    vPID-> integralValue= vPID-> integralValue+ thisError;
    //变积分系数获取
    factor=VariableIntegralCoefficient(thisError,vPID->errorabsmax,vPID->errorabsmin);
   
    //计算微分项增量带不完全微分
    vPID-> derivative =kd*(1-vPID->alpha)*(thisError-vPID->lasterror +vPID->alpha*vPID-> derivative;
       
    result=vPID->kp*thisError+vPID->ki*vPID->integralValue +vPID-> derivative;
  }
  else
  {
    if((abs(vPID->setpoint-vPID->minimum)deadband)&&(abs(pv-vPID->minimum)deadband))
    {
       result=vPID->minimum;
    }
  }
 
  /*对输出限值，避免超调和积分饱和问题*/
 if(result>=vPID->maximum)
  {
    result=vPID->maximum;
  }
 if(result<=vPID->minimum)
  {
    result=vPID->minimum;
  } 
 
  vPID->preerror=vPID->lasterror; //存放偏差用于下次运算
  vPID->lasterror=thisError;
  vPID->result=result;
  vPID->output=((result-vPID->minimum)/(vPID->maximum-vPID->minimum))*100.0;
}
 

项目展示0：

 

智能车里面通过编码器获得车轮转速，具体原理如下。

其中dx是我们在dt时间内运动的距离，此处暂且理解为我们读取到的编码器的值（此处不再区分物理中的位移与路程，速度与速率）。其中dt越大，v就是平均速度，dt越小，v就趋近顺时速度。

我们将编码器放在单片机定时器中断中读取，比如我们每间隔10毫秒去读取一次编码器的值，读完了将它清空，那么我们就得到了在每10毫秒中编码器的旋转圈数。

（读取完要将寄存器清空，因为寄存器的值是累加的）

得到编码器旋转的圈数，再根据齿轮比算出轮胎转动圈数，再测量轮胎半径，理论上可以得到非常准确的车轮转速，但一般我们不这么做。

因为没有意义，理论上经过上述处理换算的转速是有单位的，但是我们做智能车控制非常多的控制量都是模糊量，他根本没有单位，比如摄像头误差，比如电磁差比和误差。

我们常规处理直接在定时器中读取编码器值就好,不必做特殊处理。

 

k是个比例系数。1/dt是定时器时间的倒数，b是齿轮比，以及车轮半径相关的系数，都是固定值，其中：

也就是说你又是测编码器之间的齿轮比，又是测车轮的半径，求得的速度也不过是在编码器反馈的数据前乘上一个固定系数而已。完全没有什么实际意义。何况车轮还有可能打滑，磨损造成测量不准。

大家如果想知道车速，直接用尺子测赛道长度，然后用手机掐秒表算一下平均速度即可。

车子弯道肯定要减速，直道加速，所以也无法用部分路程速度计算，大家常说的三米车就是平均速度，所以直接秒表尺子即可。

2.3电机闭环控制
上面我们知道了如何获取电机转速，下面我们讲一讲如何控制电机转速。

电机有两个接头，用来驱动电机。显然在电机两端施加电压越大，电机转速越快。

那么很简单，我只需要控制电机两端电压就可以控制电机转速。

比如相同的电机，我施加3V电压，他的转速肯定不如6v的电压转速快。

至于如何测量他的转速，我们使用上文提到的编码器外接齿轮啮合住电机输出端即可。

该电机推荐电压7.2V，空载电流约630mA。（其实你电压给大一点也可以，只是电机更容易烧）

单片机所使用的电源一般都是3.3V或5V，驱动电流更是毫安级别，不可能驱动这个大家伙。

2.3.1 电机驱动
想要驱动电机，起码需要一个比较高的电压。

这里就想到了我们车上有一块电池，用来给所有外设供电。

这样就有了7.4v的电压，把电机线接上电池，电机可以转，电池电压降低，电机转速变慢。

但是显然这样不符合我们的控制需求，不能自由控制电机转速。

这时候我们就需要一个电机驱动。

上面的电机驱动有四组对外接口。

一个输入电源，用来连接电池，两个输出接口，用来连接电机（一辆C车有两个电机），还有一组排针接口，作为输入控制信号。

通过输入信号，控制输出给电机的输出电压，这就是驱动。

用3.3v的电压，控制7.4v（此处电压取决于电池电压）的电机。//有那么一点点继电器，“弱电”控“强电”的感觉。

对于一个电机来说，驱动信号是两路信号。这两路信号可以控制电机的转速和方向。//具体原理是H桥，有兴趣的同学可以简单学习。基本原理就是通过控制四只MOS管导通顺序（每次导通一对），控制电流方向。

根据驱动器芯片的不同这两路信号略微有点区别。

无论是哪一种控制信号，想要控制速度，都需要控制PWM占空比。

这里简单介绍一下PWM，等后续有空再出一篇文章专门讲一讲PWM。

PWM(Pulse width modulation)也叫脉宽调制，可以用示波器测一下，他的样子就是一连串的矩形波。

他有两个参数，一个是频率，一个是占空比。

大家看驱动参数那张图可以在下面看到小字，不同型号的电机他的工作频率不一样，这个是电机的固有特性，大家按照要求设置即可。

我们主要看看占空比。

前面我们说，我们通过PWM可以控制电机转速。我们又知道，电机的电压变化会导致电机转速变化。我们能否得到结论，PWM就是控制的电机两端电压来控制的转速呢？

答案是可以的。