pid算法（平衡车+风力摆）

前段时间做了一辆平衡车，移植学长的代码，虽然做出来了，但是对于其中的pid控制还是不熟，这几天又重新研究了一下，现在来做一个总结，也供大家学习。

1.为什么要用pid

1.PID即：Proportional（比例）、Integral（积分）、Differential（微分）的缩写。

顾名思义，PID控制算法是结合比例、积分和微分三种环节于一体的控制算法，它是连续系统中技术最为成熟、应用最为广泛的一种控制算法，该控制算法出现于20世纪30至40年代，适用于对被控对象模型了解不清楚的场合。PID控制的实质就是根据输入的偏差值，按照比例、积分、微分的函数关系进行运算，运算结果用以控制输出。Kp项代表系统的现在，Ki项可以计算系统的过去，Kd项预测系统的未来。

这里讲一下C语言实现积分功能：例如，求二次函数面积，将区域x轴分为10000份，每一份近似为一个矩形，然后利用公式sum+=(f0+f1)*h/2（上底加下底乘高除二）把10000份矩形累加出来，得到的结果和积分公式结果一样。

#include<stdio.h>
int main()
{float a=0,b=2,h,f0,f1,sum=0,n=10000;f0=a*a+a*2+1;h=(b-a)/n;for(int i=1;i<=n;i++){a=a+h;f1=a*a+a*2+1;sum+=(f0+f1)*h/2;f0=f1;}printf("%f",sum);}

8.666765
--------------------------------
Process exited after 0.04064 seconds with return value 0
请按任意键继续. . .

2.数学公式可能比较枯燥，通过以下例子，了解PID算法的应用。

例如，我想让一辆两轮平衡车立起来，通过mpu6050得到小车角度，再利用比例系数建立一次函数pwm=k*angle，当车子往左倾斜的时候电机向左走，往右倾斜的时候电机向右走，这样是不是可以让平衡车立起来了呢？其实是不行的，因为车子可能向左倾斜的时候可能往左走过头了，向右倾斜的时候往右也可能走过头了，这样得到的就不是一辆平衡车，而是一辆摇摆车。

此时就需要使用PID控制算法了。

接着咱再来详细了解PID控制算法的三个最基本的参数：Kp比例增益、Ki积分增益、Kd微分增益。

1、Kp比例增益

利用目标值减去当前值得到偏差，根据调节装置的“调节力度”，建立一个一次函数的关系，就可以实现最基本的“比例”控制了，例如：pwm=Kp*angle

Kp越大，调节作用越激进，Kp调小会让调节作用更保守。

若你正在制作一个平衡车，有了P的作用，你会发现，平衡车在平衡角度附近来回“狂抖”，比较难稳住。

2、Kd微分增益

有了P的作用，不难发现，只有P好像不能让平衡车站起来，好像整个系统不是特别稳定，总是在“抖动”，这时候就需要d的上场了。

设想有一个弹簧：现在我有一个弹簧上面挂了个物体，拉它一下，然后松手，这时它会震荡起来，因为阻力很小，它可能会震荡很长时间，才会重新停在平衡位置。

请想象一下：要是把上图所示的系统浸没在水里，同样拉它一下：这种情况下，重新停在平衡位置的时间就短得多。

Kd项就相当于水，让被控制的物理量的化速度”趋于0，即类似于“阻尼”的作用，只要什么时候，这个量具有了速度，D就向相反的方向用力，尽力刹住这个变化。

Kd参数越大，向速度相反方向刹车的力道就越强，如果是平衡小车，加上P和D两种控制作用，如果参数调节合适，它应该可以站起来了。

这就是平衡小车的直立PD控制：

int balance_UP(float Angle,float Mechanical_balance,float Gyro)
{  float Bias;int balance;Bias=Angle-Mechanical_balance;                               //===求出平衡的角度中值和机械相关balance=balance_UP_KP*Bias+balance_UP_KD*Gyro;  //===计算平衡控制的电机PWM  PD控制   kp是P系数 kd是D系数return balance;
}

3、Ki积分增益

以热水为例，假如有个人把加热装置带到了非常冷的地方，开始烧水了，需要烧到50℃。

在P的作用下，水温慢慢升高，直到升高到45℃时，他发现了一个不好的事情：天气太冷，水散热的速度，和P控制的加热的速度相等了。

这可怎么办？

P兄这样想：我和目标已经很近了，只需要轻轻加热就可以了。
D兄这样想：加热和散热相等，温度没有波动，我好像不用调整什么。
于是，水温永远地停留在45℃，永远到不了50℃。

根据常识，我们知道，应该进一步增加加热的功率，可是增加多少该如何计算呢？

前辈科学家们想到的方法是真的巧妙，设置一个积分量，只要偏差存在，就不断地对偏差进行积分（累加），并反应在调节力度上。

这样一来，即使45℃和50℃相差不是太大，但是随着时间的推移，只要没达到目标温度，这个积分量就不断增加，系统就会慢慢意识到：还没有到达目标温度，该增加功率啦！

到了目标温度后，假设温度没有波动，积分值就不会再变动，这时，加热功率仍然等于散热功率，但是，温度是稳稳的50℃。

Ki的值越大，积分时乘的系数就越大，积分效果越明显，所以，I的作用就是，减小静态情况下的误差，让受控物理量尽可能接近目标值。

I在使用时还有个问题：需要设定积分限制，防止在刚开始加热时，就把积分量积得太大，难以控制。

B站有一个视频对于pid的概念讲的非常好，可以去看看
【硬核科普】PID算法从理论到实践

2.平衡车的pid应用

1.直立环

/**************************************************************************
函数功能：直立PD控制
入口参数：角度、机械平衡角度（机械中值）、角速度
返回  值：直立控制PWM
**************************************************************************/
int balance_UP(float Angle,float Mechanical_balance,float Gyro)
{  float Bias;int balance;Bias=Angle-Mechanical_balance;                               //===求出平衡的角度中值和机械相关balance=balance_UP_KP*Bias+balance_UP_KD*Gyro;  //===计算平衡控制的电机PWM  PD控制   kp是P系数 kd是D系数return balance;
}

Bias就是我们通过mpu6050所得到的角度和目标角度的差，Gyro是mpu6050陀螺仪原始数据gyroy，就是相当于此次误差和上一次误差的差值，这里直立环并没有用到Ki项，可以把Ki当作0处理。

pwm=Kpe(k)+Ki∑e(k)+Kd*[e（k）-e(k-1)]
e(k)代表本次偏差
e(k-1)代表上一次的偏差
∑e(k)代表e(k)以及之前的偏差的累积和;
pwm代表输出

我们通过PID算法对传感器采集回来的数据进行运算，最后会输出一个值，而这个值就是用来控制PWM的占空比，从而来控制电机的转速，这样就可以使控制达到准确、平滑的特点，在这里返回的balance就是我们想要的pwm值。

2.速度环

/**************************************************************************
函数功能：速度PI控制
入口参数：电机编码器的值
返回  值：速度控制PWM
**************************************************************************/
int velocity(int encoder_left,int encoder_right)
{  static float Velocity,Encoder_Least,Encoder,Movement;static float Encoder_Integral;//=============速度PI控制器=======================//Encoder_Least =(Encoder_Left+Encoder_Right)-0;  //上次速度差值  //===获取最新速度偏差==测量速度（左右编码器之和）-目标速度（此处为零）Encoder *= 0.8;                         //这次速度差值                            //===一阶低通滤波器Encoder += Encoder_Least*0.2;      //对速度偏差进行低通滤波                        //===一阶低通滤波器Encoder_Integral +=Encoder;//===积分出位移 积分时间：10msEncoder_Integral=Encoder_Integral-Movement;                       //===接收遥控器数据，控制前进后退if(Encoder_Integral>10000)    Encoder_Integral=10000;             //===积分限幅if(Encoder_Integral<-10000)      Encoder_Integral=-10000;            //===积分限幅Velocity=Encoder*velocity_KP+Encoder_Integral*velocity_KI;        //===速度控制if(pitch<-40||pitch>40)           Encoder_Integral=0;                            //===电机关闭后清除积分return Velocity;
}

首先要得到编码器的值encoder_left和encoder_right，得到之后对速度偏差进行低通滤波，防止速度突变，得到一个平滑的速度，再将速度差值进行累加（积分），但是积分值要限定一个最大值，最后根据PID公式计算出PWM值（Kd相当于0）。

平衡车

平衡车工程代码（实现的功能：直立，蓝牙控制行走，超声波避障）
平衡车工程

3.风力摆的pid应用

//函数功能:电机1位置式PID计算
int PID_M1_PosLocCalc(float NextPoint)
{register float  iError,dError;iError = M1PID.SetPoint - NextPoint;// 偏差M1PID.SumError += iError;                 // 积分if(M1PID.SumError > 3000.0)                    //积分限幅3000M1PID.SumError = 3000.0;else if(M1PID.SumError < -3000.0)M1PID.SumError = -3000.0; dError = iError - M1PID.LastError;  // 当前微分M1PID.LastError = iError;                     //更新return(int)(M1PID.Proportion*iError            // 比例项+M1PID.Integral*M1PID.SumError         // 积分项+M1PID.Derivative*dError);                  // 微分项
}

我们同样可以看出，Kp项是乘以偏差，Ki项乘以偏差和，Kd项乘以这次偏差和上次偏差的差值，积分值任然要进行限幅。

pwm=Kpe(k)+Ki∑e(k)+Kd*[e（k）-e(k-1)]
e(k)代表本次偏差
e(k-1)代表上一次的偏差
∑e(k)代表e(k)以及之前的偏差的累积和;
pwm代表输出

风力摆工程代码（老师发的例程代码，没试过）
风力摆工程

4.位置式pid和增量式pid

上面所讲的都是位置式pid，现在我们来看看增量式pid

比例P : e(k)-e(k-1) 这次误差-上次误差

积分I : e(i) 误差

微分D : e(k) - 2e(k-1)+e(k-2) 这次误差-2*上次误差+上上次误差

增量式PID根据公式可以很好地看出，只要使用前后三次测量值的偏差，即可由公式求出输出值，得到的结果是增量，即在上一次的控制量的基础上需要增加（负值意味减少）控制量，且增量式不会严重影响系统的工作。

增量式与位置式区别：

1.增量式算法得到的是控制量的增量，算式中不需要累加，只与最近三次测量值有关，不会严重影响系统的工作；
2.位置式算法得到的是过去偏差的累加值，数值很大，决定着系统的工作状态。

对于位置式pid和增量式pid，这篇文章讲的很清晰：
原文链接：位置式pid和增量式pid