在最前面推荐一个大佬的讲解，真的很浅显易懂，建议先看了解大概PID：链接
out_speed = p*err+d*now_speed;//pid最核心算法
运动员参加100米跑步，假设这个人可以瞬间提速，但是无法瞬间减速到0
起点：0米
终点：100米
/*************************************************/
p=0.5，i=0，d=0
起跑时：
运动员在d=0，终点在100，此时100-d=100
out_speed =100×0.5=50，所以起跑时运动员速度最大，每秒50米
一秒后：
运动员到d=50米位置，此时距离终点err=100-d=50
out_speed =50×0.5=25
两秒后：
运动员跑到50+25=75米的位置，距离终点err=100-75=25
out_speed =25×0.5=12.5
…
可以预测，运动员越接近终点，速度越小，开始时候速度最大，快速到终点，相比全程匀速10米每秒的速度跑，可以比较一下的值pid算法在相应速度和效率上的优势
/*************************************************/
p=0.5，i=0，d=0.1
起跑时：
运动员在d=0，终点在100，此时100-d=100
out_speed =100×0.5+0=50
所以起跑时运动员速度最大，每秒50米
一秒后：
运动员到d=50米位置，此时距离终点err=100-d=50
out_speed =50×0.5+0.1×（-50）=20
两秒后：
运动员跑到d=50+20=70米的位置，距离终点err=100-d=30
out_speed =20×0.5+0.1×（-20）=10.5
…
/*************************************************/
可以推测，d的存在会让运动员刹车能力更强！也就是在牺牲速度的情况下，增加了稳定性，到了终点能马上刹住，不超出终点，就好比在运动员身后栓了根绳子，跑得越快，绳子往后拉的力气越大，辅助运动员刹车。
仔细观察可以发现一秒后和两秒后的now_speed分别为和（-50）和（-20），
（-50） = err-lasterr=50-100
（-20）= err -lasterr=30-50=-20
因此你会发现，now_speed也就是网上大多数文章讲pid时提到的微分，再通俗点说就是err-lasterr和now_speed成比例，可以等效替换，得到如下式子
out_speed = p*err+d*（err-lasterr）;//pid最核心算法
注意：这里的系统输出的是速度，如果输出的是角度同样能控制，也一样都适用，变量名字换一下，pd重新调一下就好了，上面两个式子将会依次对照下面2个式子，可以结合着理解
out_Angle = p*err+d*now_Angle_speed;//now_Angle_speed是角速度，单位为rad/s
out_Angle = p*err+d*（err-lasterr）;//pid最核心算法
一般pd控制的系统就足够百分之80的系统用了
快速（P）、准确（I）、稳定（D）
P：Proportion（比例），就是输入偏差乘以一个常数。
I ：Integral（积分），就是对输入偏差进行积分运算。
D：Derivative（微分），对输入偏差进行微分运算。
（输入偏差=读出的被控制对象的值-设定值。比如说我要把温度控制在26度，但是现在我从温度传感器上读出温度为28度。则这个26度就是”设定值“，28度就是“读出的被控制对象的值”。）
1稳定性（P和I降低系统稳定性，D提高系统稳定性）：在平衡状态下，系统受到某个干扰后，经过一段时间其被控量可以达到某一稳定状态；
2 准确性（P和I提高稳态精度，D无作用）：系统处于稳态时，其稳态误差；
3快速性（P和D提高响应速度，I降低响应速度）：系统对动态响应的要求。一般由过渡时间的长短来衡量。
.随着比例系数Kp的增加，超调量增大（震荡变严重），系统响应速度加快，
积分控制部分的作用主要是用来消除静差。那么积分是怎样来消除静差的呢？
比例控制只能尽量将Err调节到0，
而微分的作用是将曲线的斜率控制到0则停止对其作用，
但斜率为0的时候Err并不一定为0。
这个时候我们就需要积分来起作用了。
我们知道曲线的积分相当于曲线与x轴围出来的面积。如下图，
积分作用的目的是使红色部分的面积和蓝色部分的面积的和为0，
那么即使系统在比例控制和微分控制部分已经趋于稳定，
只要Err不为0就会存在静差，只要存在静差那么积分就会对系统产生影响，
直到系统的Err值为0 。
那么这样我们的PID控制在理论上就可以达到一个非常精确的控制效果。
作用机理：
微分代表error变化斜率，下降时候其为负数；
Err随时间是一条斜率小于0的曲线，那么在周期时间内，Err越大，微分的绝对值越大，那么也就对Err的减小速度是起到抑制的作用的，直到最后斜率为0微分才会停止作用。
比例控制：能迅速反映误差，从而减小误差，但比例控制不能消除稳态误
差，KP的加大会引起系统的不稳定；
积分控制的作用是：只要系统存在误差，积分控制作用就不断地积累，
输出控制量以消除误差。因此只要有足够的时间，积分控制将能完全消
除误差，但是积分作用太强会使系统超调加大，甚至使系统出现振荡；
微分控制：可以减小超调量，克服振荡，使系统的稳定性提高，同时加快
系统的动态响应速度，减小调整时间，从而改善系统的动态性能。
位置闭环控制就是根据编码器的脉冲累加测量电机的位置信息，并与目标值进行比较，得到控制偏差，然后通过对偏差的比例、积分、微分进行控制，使偏差趋向于零的过程。
代码：
int Position_PID (int Encoder,int Target)
{
static float Bias,Pwm,Integral_bias,Last_Bias;
Bias=Encoder-Target; //计算偏差
Integral_bias+=Bias; //求出偏差的积分
Pwm=Position_KP*Bias+Position_KI*Integral_bias+Position_KD*(Bias-Last_Bias);
Last_Bias=Bias; //保存上一次偏差
return Pwm; //输出
}
速度闭环控制就是根据单位时间获取的脉冲数（这里使用了M法测速）测量电机的速度信息，并与目标值进行比较，得到控制偏差，然后通过对偏差的比例、积分、微分进行控制，使偏差趋向于零的过程。
Pwm+=Kp[e(k)-e(k-1)]+Kie(k)+Kd[e(k)-2e(k-1)+e(k-2)]
e(k)：本次偏差
e(k-1)：上一次的偏差
e(k-2)：上上次的偏差
Pwm代表增量输出
在我们的速度控制闭环系统里面只使用PI控制，因此对PID控制器可简化为以下公式：
Pwm+=Kp[e(k)-e(k-1)]+Kie(k)
代码实现
int Incremental_PI (int Encoder,int Target)
{
static float Bias,Pwm,Last_bias;
Bias=Encoder-Target; //计算偏差
Pwm+=Velocity_KP*(Bias-Last_bias)+Velocity_KI*Bias; //增量式PI控制器
Last_bias=Bias; //保存上一次偏差
return Pwm; //增量输出
}
增量式控制算法的优点
(1) 增量算法不需要做累加，控制量增量的确定仅与最近几次误差采
样值有关，计算误差或计算精度问题，对控制量的计算影响较小。而位
置算法要用到过去的误差的累加值，容易产生大的累加误差。
(2)增量式算法得出的是控制量的增量，例如阀门控制中、只输出阀
门开度的变化部分，误动作影响小，必要时通过逻辑判断限制或禁止本
次输出，不会严重影响系统的工作。而位置算法的输出是控制量的全量
输出，误动作影响大。
(3) 采用增量算法，易于实现手动到自动的无冲击切换。
(4) 利用增量算法，也很容易得出位置算法u(k)= u(k-1)+△u(k)
PID算法具有三大特性：
① 稳定性
② 快速性
③ 准确性
其实有时候并不一定都得用上
比如自动停车系统或者自动蓄水系统只需要用到1和3
刹车时用到123等等
接下来会举例说明
概念须知：
最大超调量：是响应曲线的最大峰值与稳态值的差，是评估系统稳定性的一个重要指标；
调节时间ts：是指响应曲线从原始工作状态出发，第一次到达输出稳态值所需的时间，是评估系统快速性的一个重要指标；
稳态误差ess：是被控量的稳定值与给定值之差，一般用于衡量系统的准确性，
接下来我们需要调整KP,KI,KD的数值以达到最高效率
特点：
比例控制较大，出现了震荡
需要加入微分d控制抑制
积分控制为零，但是没有静差，因为比例控制较强
（注意：经测试比例系数过大之后可减少静差出现）
特点：
比例控制kp减小，无震荡，响应变慢了
无积分控制且比例控制较弱时，会出现静差（积分减少静差）
特点：
1.在比例控制较强的情况下，加入比较大的微分控制，震动次数较小。
2.微分控制较大，响应变慢
特点：
目标：控制电机转90°，需要严格控制超调量、和静差。但是对响应速度无要求。
1.因为响应速度无要求，一般比例控制应该给小一点。
2.加大系统的阻尼防止超调，也就是微分参数尽量大。
3.另外因为比例参数较小，应该加入积分控制减小静差。
网上很多调参口诀，可以借鉴，我这里简单根据自己的经验理一遍过程
float X_PI (int Target_X){
static int err=0,last_err = 0;
float out_put;
err=x_mid-Target_X; //计算偏差 物体在右边 640-700 = -
out_put=sjx_p/1000*err+sjx_d/1000*(err - last_err); //Pd控制器
last_err = err;
return out_put; //输出
}
pid算法总结成一句代码就是第五句代码，
注意，代码中/1000指的是吧kp扩大1000倍，因为这里kp太小了，放大好调一点
经过2年的项目积累总结pid调参步骤如下：
1.将p和i和d设置为全局变量，然后调试时候用debug会事半功倍
2.一般先把i和d设为0
3.慢慢调p，如果发现怎么调都调不好可以尝试p给个负值试试
4.首先，先给个很小的p值，然后给个稍微大一点的p值，例如1和200（根据情况而定），看看控制的对象有没有明显的效果，表现为p小的时候，力气很小可以用手轻易掰动他，p大的时候能感受到明显的阻力
5.如果第四部能感受到，那么第五步就是从小到大给p，你会发现控制对象力气越来越大，当到达一定的值时，会发现控制对象开始抖动，这个很正常，因为p太大，pid震荡幅度越来越大，超调了
6，记录产生震荡的p的值为p1，然后往回调，直到停止震荡，记录此时震荡的p值位p2
7.最后根据实际情况，p的值应该在80%p2到90%p2之间
8.p调稳定之后，开始加d，一般情况下在第7步时，i和d为0，假如控制对象是电机，空载情况下只要一个p就足够了，如果这个电机需要带动一定重量的物体旋转，会发现只有p会发生如下情况：用力拨动整个结构，会感受到电机给的力越来越大，继续拨动，直到电机旋转90度，这个时候力已经很大了，此时放开，让他从90度归位到原来位置，会发现他并不会马上停在0度，而是会跑到-10度左右然后弹回来，一直震荡，慢慢靠近0
9.如果出现8的情况，这时候慢慢加d，先检验d有没有作用，和4的p一样，先给个小的d再给个稍微大的d，d太大会震荡，一般1和20差不多，如果发现用8的方法拨动机械，发现机械不会到-10，到-5就往回弹了（大概一个数字，可能直接就到0度也可能），说明d有效果
10.发现9有效果的话，和p一样，慢慢调大，重复8的步骤一直去拨动电机，看他反弹的情况，如果发现d太大已经导致电机抖动了了，就不要再加大了，看看这个效果你满不满意，用在这个项目够不够用，一般到这里是够了
11.如果10的效果还不满意，用户需要的是迅速从90度反应到0度，那么可以考虑适当减少p，p减少了，d可以稍微往上抬，就不会像10那样电机抖动了
12：一般只有少数类似飞控那一类的控制才需要用到积分i，笔者还未用到i的控制，一直以来基本pd控制就可以很稳定
做了如下优化：
优化前：
out_put=sjx_p/1000*err+sjx_d/1000*(err - last_err); //Pd控制器
优化后：
out_put=sjx_p/1000*err+sjx_d1/1000*(speed); //Pd控制器
说明：
位置差进行微分其实就是速度值，其中，速度值由电机foc算法反馈得到。