增量式与位置式PID算法（C语言实现与电机控制项目）

1、PID 算法C 语言原代码

先贴上一种常见的比较通用的C语言增量式PID算法吧

1. typedef struct PID
   
2. {
   
3. intSetPoint; //设定目标 DesiredValue
   
4. longSumError; //误差累计
   
5. doubleProportion; //比例常数Proportional Const
   
6. doubleIntegral; //积分常数 IntegralConst
   
7. doubleDerivative; //微分常数Derivative Const
   
8. intLastError; //Error[-1]
   
9. intPrevError; //Error[-2]
   
10. } PID;
    
11. static PID sPID;
    
12. static PID *sptr = &sPID;
    
13. /
    
14. *=============================================================
    
15. =======
    
16. InitializePID Structure PID 参数初始化
    
17. =============================================================
    
18. ======*/
    
19. void IncPIDInit(void)
    
20. {
    
21. sptr->SumError= 0;
    
22. sptr->LastError= 0; //Error[-1]
    
23. sptr->PrevError= 0; //Error[-2]
    
24. sptr->Proportion= 0; //比例常数Proportional Const
    
25. sptr->Integral= 0; //积分常数IntegralConst
    
26. sptr->Derivative= 0; //微分常数Derivative Const
    
27. sptr->SetPoint= 0;
    
28. }
    
29. 
    
30. /
    
31. *=============================================================
    
32. ======= 增量式PID 计算部分
    
33. =============================================================
    
34. =======*/
    
35. int IncPIDCalc(int NextPoint)
    
36. {
    
37. registerint iError, iIncpid; //当前误差
    
38. iError= sptr->SetPoint - NextPoint; //增量计算
    
39. iIncpid= sptr->Proportion * iError //E[k]项
    
40.         -sptr->Integral * sptr->LastError //E[k-1]项                     
    
41.         +sptr->Derivative * sptr->PrevError; //E[k-2]项
    
42. 
    
43. //存储误差，用于下次计算
    
44. sptr->PrevError= sptr->LastError;
    
45. sptr->LastError= iError;
    
46. //返回增量值
    
47. return(iIncpid);
    
48. }

复制代码

2、PID 整定口诀
                    参数整定找最佳， 从小到大顺序查。
                    先是比例后积分， 最后再把微分加。
                    曲线振荡很频繁， 比例度盘要放大。
                    曲线漂浮绕大弯， 比例度盘往小扳。
                    曲线偏离回复慢， 积分时间往下降。
                    曲线波动周期长， 积分时间再加长。
                    曲线振荡频率快， 先把微分降下来。
                    动差大来波动慢， 微分时间应加长。
                    理想曲线两个波， 前高后低四比一。
                    一看二调多分析， 调节质量不会低。
说实话整定口诀对于初学者来说，其实根本就看不懂，只有从实际整定过程中才能慢慢发觉其中的奥秘。


3、项目原理
学到这里对PID算法的理解只是停留在理论，那么我们来结合实际看看，这里我参考的是平衡小车之家的资料，不得不说确实浅显易懂，先从电机的简单控制开始理解吧！


3.1直流电机
简单来说就是，把+和-分别接到电池的正极和负极，电机即可转动；
如果是把的+和-分别接到电池的负极和正极，则电机会反方向转动。电机的转速可以理解为和外接的电压是正相关的（实际是由电枢电流决定）。


3.2.减速器
一般直流电机的转速都是一分钟几千上万转的，所以一般需要安装减速器。减速器是一种相对精密的机械零件，使用它的目的是降低转速，增加转矩。减速后的直流电机力矩增大、可控性更强。按照传动级数不同可分为单级和多级减速器；按照传动类型可分为齿轮减速器、蜗杆减速器和行星齿轮减速器。


3.3电机驱动
要实现电机调试和换向功能，我们可以使用单片机实现的，但是单片机IO 的带负载能力较弱，而直流电机是大电流感性负载，所以我们需要功率放大器件，在这里，我们选择了 TB6612FNG。
TB6612FNG 是东芝半导体公司生产的一款直流电机驱动器件，它具有大电流MOSFET-H桥结构，双通道电路输出，可同时驱动 2 个电机。也许大家更熟悉被用烂的L298N，其实这两者的使用基本一致的。而且，相比 L298N 的热耗性和外围二极管续流电路，它无需外加散热片，外围电路简单，只需外接电源滤波电容就可以直接驱动电机，利于减小系统尺寸。对于 PWM 信号输入频率范围，高达 100 kHz 的频率更是足以满足我们大部分的需求了。


3.4编码器

编码器是一种将角位移或者角速度转换成一连串电数字脉冲的旋转式传感器，我们可以通过编码器测量到底位移或者速度信息。编码器从输出数据类型上分，可以分为增量式编码器和绝对式编码器。
从编码器检测原理上来分，还可以分为光学式、磁式、感应式、电容式。常见的是光电编码器（光学式）和霍尔编码器（磁式）。
这里使用增量式输出的霍尔编码器。编码器有 AB 相输出，所以不仅可以测速，还可以辨别转向。根据上图的接线说明可以看到，我们只需给编码器电源5V 供电，在电机转动的时候即可通过 AB 相输出方波信号。编码器自带了上拉电阻，所以无需外部上拉，可以直接连接到单片机IO读取。
那么单片机如何采集编码器数据？
因为编码器输出的是标准的方波，所以我们可以使用单片机（STM32 STM8 51等）直接读取。在软件中的处理方法是分两种，自带编码器接口的单片机如STM32，可以直接使用硬件计数。而没有编码器接口的单片机如 51 单片机，可以通过外部中断读取，比如把编码器 A 相输出接到单片机的外部中断输入口，这样就可通过跳变沿触发中断，然后在对应的外部中断服务函数里面，通过 B 相的电平来确定正反转。如当 A 相来一个跳变沿的时候，如果 B 相是高电平就认为是正转，低电平就认为是反转。

4、电机速度闭环控制
4.1原理
速度闭环控制就是根据单位时间获取的脉冲数测量电机的速度信息，并与目标值进行比较，得到控制偏差，然后通过对偏差的比例、积分、微分进行控制，使偏差趋向于零的过程。
需要说明的是，这里速度控制 20ms 一次，一般建议 10ms 或者 5ms，因为在这里电机是使用 USB 供电，速度比较慢，20ms 可以延长获取速度的单位时间，提高编码器的采值。
首先由于需要知道速度，所以一般都需要带编码器的电机，编码器输出有ab相，可以通过单片机定时器的捕获模式来得到速度，之后在单片机内部进行PID算法的运算，得到输出所需要的速度，通过控制占空比来输出PWM波，控制电机的速度，这里用的主控是STM32c8t6。

4.2核心代码

1. /**************************************************************************
   
2. 函数功能：增量PI控制器
   
3. 入口参数：编码器测量值，目标速度
   
4. 返回  值：电机PWM
   
5. 根据增量式离散PID公式
   
6. pwm+=Kp[e（k）-e(k-1)]+Ki*e(k)+Kd[e(k)-2e(k-1)+e(k-2)]
   
7. e(k)代表本次偏差
   
8. e(k-1)代表上一次的偏差  以此类推
   
9. pwm代表增量输出
   
10. 在我们的速度控制闭环系统里面，只使用PI控制
    
11. pwm+=Kp[e（k）-e(k-1)]+Ki*e(k)
    
12. **************************************************************************/
    
13. int Incremental_PI (int Encoder,int Target)
    
14. {   
    
15.    float Kp=20,Ki=30;   
    
16.      static int Bias,Pwm,Last_bias;         //相关内部变量的定义。
    
17.      Bias=Encoder-Target;                //求出速度偏差，由测量值减去目标值。
    
18.      Pwm+=Kp*(Bias-Last_bias)+Ki*Bias;   //使用增量 PI 控制器求出电机 PWM。
    
19.      Last_bias=Bias;                       //保存上一次偏差
    
20.      return Pwm;                         //增量输出
    
21. }

复制代码

这里可以看到使用的是增量式比例积分控制器，Kp和Ki的值在函数中临时设置，完全按照公式编写，简单易懂。
4.3定时控制

1. int Target_velocity=50;  //设定速度控制的目标速度为50个脉冲每10ms
   
2. int TIM3_IRQHandler(void)  
   
3. {   
   
4.     if(TIM3->SR&0X0001)//10ms定时中断
   
5.     {   
   
6.           TIM3->SR&=~(1<<0);                                       //===清除定时器1中断标志位         
   
7.           Encoder=Read_Encoder(2);                                 //===读取编码器的值，M法测速，输出为每10ms的脉冲数
   
8.         Led_Flash(100);                                          //===LED闪烁;指示单片机正常运行   
   
9.           Moto1=Incremental_PI(Encoder,Target_velocity);           //===速度PI控制器
   
10.         Xianfu_Pwm();                                            //===PWM限幅
    
11.         Set_Pwm(Moto1);                                          //===赋值给PWM寄存器  
    
12.     }           
    
13.      return 0;   
    
14. }

复制代码

这里控制周期设定的是每10ms控制一次，设置在10ms的中断中进行，得到控制量后，在经过简单的赋值和去绝对值来输出给驱动的PWM控制器。
4.4其他代码

1. 
   
2. /**************************************************************************
   
3. 函数功能：赋值给PWM寄存器
   
4. 入口参数：PWM
   
5. 返回  值：无
   
6. **************************************************************************/
   
7. void Set_Pwm(int moto1)
   
8. {
   
9.             if(moto1>0)         AIN2=1,         AIN1=0;
   
10.             else              AIN2=0,           AIN1=1;
    
11.             PWMA=myabs(moto1);
    
12. }
    
13. 
    
14. /**************************************************************************
    
15. 函数功能：限制PWM赋值
    
16. 入口参数：无
    
17. 返回  值：无
    
18. **************************************************************************/
    
19. void Xianfu_Pwm(void)
    
20. {   
    
21.       int Amplitude=7100;    //===PWM满幅是7200 限制在7100
    
22.     if(Moto1<-Amplitude) Moto1=-Amplitude;  
    
23.         if(Moto1>Amplitude)  Moto1=Amplitude;   
    
24. }
    
25. 
    
26. /**************************************************************************
    
27. 函数功能：绝对值函数
    
28. 入口参数：int
    
29. 返回  值：unsigned int
    
30. **************************************************************************/
    
31. int myabs(int a)
    
32. {         
    
33.       int temp;
    
34.         if(a<0)  temp=-a;  
    
35.       else temp=a;
    
36.       return temp;
    
37. }

复制代码

主函数

1. int main(void)
   
2. {
   
3.     Stm32_Clock_Init(9);            //系统时钟设置
   
4.     ...
   
5.     MiniBalance_PWM_Init(7199,0);   //=====初始化PWM 10KHZ 高频可以防止电机低频时的尖叫声
   
6.     Encoder_Init_TIM2();            //初始化编码器
   
7.   Timer3_Init(99,7199);           //=====10MS进一次中断服务函数，中断服务函数在control.c
   
8.     while(1);
   
9. 
   
10. }

复制代码

5、电机位置闭环控制位置闭环控制就是根据编码器的脉冲累加测量电机的位置信息，并与目标值进行比较，得到控制偏差，然后通过对偏差的比例、积分、微分进行控制，使偏差趋向于零的过程。
5.1核心代码

1. /**************************************************************************
   
2. 函数功能：位置式PID控制器
   
3. 入口参数：编码器测量位置信息，目标位置
   
4. 返回  值：电机PWM
   
5. 根据位置式离散PID公式
   
6. pwm=Kp*e(k)+Ki*∑e(k)+Kd[e（k）-e(k-1)]
   
7. e(k)代表本次偏差
   
8. e(k-1)代表上一次的偏差  
   
9. ∑e(k)代表e(k)以及之前的偏差的累积和;其中k为1,2,,k;
   
10. pwm代表输出
    
11. **************************************************************************/
    
12. int Position_PID (int Encoder,int Target)
    
13. {   
    
14.      float Position_KP=80,Position_KI=0.1,Position_KD=500;
    
15.      static float Bias,Pwm,Integral_bias,Last_Bias;
    
16.      Bias=Encoder-Target;                                  //求出速度偏差，由测量值减去目标值。
    
17.      Integral_bias+=Bias;                                    //求出偏差的积分
    
18.      Pwm=Position_KP*Bias+Position_KI*Integral_bias+Position_KD*(Bias-Last_Bias);       //位置式PID控制器
    
19.      Last_Bias=Bias;                                       //保存上一次偏差
    
20.      return Pwm;                                           //增量输出
    
21. }
    

复制代码

这里采用稍微复杂一点的PID控制，相比之前的速度控制多了个微分环节，但是由于是位置式控制，所以代码较为简单，容易理解。
5.2控制中断函数

1. int Target_position=11000;    //初始值是10000，目标值是11000
   
2. int TIM3_IRQHandler(void)  
   
3. {   
   
4.     if(TIM3->SR&0X0001)//10ms定时中断
   
5.     {   
   
6.           TIM3->SR&=~(1<<0);                                         //===清除定时器1中断标志位      
   
7.           Encoder=Read_Encoder(2);                                  //===读取编码器的位置数据 初始值是10000
   
8.         Led_Flash(100);                                           //===LED闪烁;指示单片机正常运行  
   
9.           Moto1=Position_PID(Encoder,Target_position);              //===位置PID控制器
   
10.           Xianfu_Pwm();                                             //===PWM限幅
    
11.         Set_Pwm(Moto1);                                          //===赋值给PWM寄存器  
    
12.     }           
    
13.      return 0;   
    
14. }
    

复制代码

其他代码和上面类似

6.参数整定
首先我们需要明确我们的控制目标，也就是满足控制系统的 3 个要求：


稳定性
快速性
准确性
具体的评估指标有最大超调量、上升时间、静差等。
最大超调量是响应曲线的最大峰值与稳态值的差，是评估系统稳定性的一个重要指标；上升时间是指响应曲线从原始工作状态出发，第一次到达输出稳态值所需的时间，是评估系统快速性的一个重要指标；静差是被控量的稳定值与给定值之差，一般用于衡量系统的准确性，具体可以参考前文的讲解。
在实践生产工程中，不同的控制系统对控制器效果的要求不一样。比如平衡车、倒立摆对系统的快速性要求很高，响应太慢会导致系统失控。智能家居里面的门窗自动开合系统，对快速性要求就不高，但是对稳定性和准确性的要求就很高，所以需要严格控制系统的超调量和静差。所以 PID 参数在不同的控制系统中是不一样的。只要我们理解了每个 PID 参数的作用，我们就可以应对工程中的各种项目的 PID 参数整定了。
一般而言，一个控制系统的控制难度，一般取决于系统的转动惯量和对响应速度的要求等。转动惯量越小、对响应速度要求越低，PID 参数就越不敏感。比如现在我们控制电机转 90°，需要严格控制超调量、和静差。但是对响应速度无要求。因为电机处于轻载的情况下，转动惯量很小，这是一个很容易完成的工作。根据上面的理论分析和实践，因为响应速度无要求，一般 P 应该给小一点，然后加大系统的阻尼防止超调，也就是 D 参数尽量大，另外因为 P 值较小，应该加入I 控制减小静差。


7.总结
调试装置的过程中会遇到各种各样的问题，硬件随外界环境会不断的出现变化，会干扰我们的调试以及运行结果。整定出能适应各种环境的参数，必须对每个环境都加以测试，综合得出最合适的参数。所以在图形化的调试整定过程能够快速直观的得出结论。


转自网络_zero.

学习借鉴一下

很实用的资料，谢谢！

好东西值得学习

很实用的资料，谢谢！，有整理好的文件下载更好

感谢分享！好好学习一下