算法分析与设计PID算法实例.docx

1、 PID算法实例分析与设计一， 摘要在对机械器件（如电机，电热水器等）进行电子控制的实现时，经常会由于被控制器件的被控制量的变化以及反馈存在一定的机械延迟导致控制的效果不理想，精度不高。尤其是在控制对于实时系统的控制要求极高的设备，如小车，四轴飞行器，3D打印机等对电机的控制要求非常高，就要求有合适的算法对这些器件进行控制。PID算法应运而生。在过程控制中，按偏差的比例（P）、积分（I）和微分（D）进行控制的PID控制器（亦称PID调节器）是应用最为广泛的一种自动控制器。它具有原理简单，易于实现，适用面广，控制参数相互独立，参数的选定比较简单等优点；而且在理论上可以证明，对于过程控制的典型对象

2、“一阶滞后+纯滞后”与“二阶滞后+纯滞后”的控制对象，PID控制器是一种最优控制。PID调节规律是连续系统动态品质校正的一种有效方法，它的参数整定方式简便，结构改变灵活（PI、PD、）。控制点包含三种比较简单的PID控制算法，分别是：增量式算法，位置式算法，微分先行。 这三种PID算法虽然简单，但各有特点，基本上能满足一般控制的大多数要求。本文将以一个具体的用于控制两轮平衡小车的PID算法c语言实例对PID算法进行分析与设计。本实验中使用的处理器是ST公司的STM32f103C8T6芯片，硬件方面，使用市场上广泛使用的陀螺仪mpu6050作为加速度传感器和方位传感器，使用tb6612芯片制作点

3、击驱动部分，直流减速电机作为小车的电机。编译器是keil公司的mdk for arm（keil5）。关键词：PID算法，stm32，平衡小车，mpu6050二， 实验设计1. PID算法的初步设计准备在进行实际算法设计之前，首先要进行一些准备工作，确保后续的PID算法设计能够顺利进行。在此之前首先深入了解PID算法设计思路。第一要明确设计目的，设计目标是制作一个能够保持平衡的两轮平衡小车，那么首先要保持平衡，其次要能够正常直线行走，再次能实现转向，一个小车就完成了。所以为了完成这些功能的设计，首先要有一个功能足够的PID体系。 比例（P）控制比例控制是一种最简单的控制方式。其控制器的输出与输入

4、误差信号成比例关系。当仅有比例控制时系统输出存在稳态误差（Steady-state error）。积分（I）控制在积分控制中，控制器的输出与输入误差信号的积分成正比关系。对一个自动控制系统，如果在进入稳态后存在稳态误差，则称这个控制系统是有稳态误差的或简称有差系统（System with Steady-state Error）。为了消除稳态误差，在控制器中必须引入“积分项”。积分项对误差取决于时间的积分，随着时间的增加，积分项会增大。这样，即便误差很小，积分项也会随着时间的增加而加大，它推动控制器的输出增大使稳态误差进一步减小，直到等于零。因此，比例+积分(PI)控制器，可以使系统在进入稳态后

5、无稳态误差。微分（D）控制微分调节就是偏差值的变化率。例如，如果输入偏差值线性变化，则在调节器输出侧叠加一个恒定的调节量。大部分控制系统不需要调节微分时间。因为只有时间滞后的系统才需要附加这个参数。如果画蛇添足加上这个参数反而会使系统的控制受到影响。首先看PID的增量型公式：PID=Uk+KP*【E(k)-E(k-1)】+KI*E(k)+KD*【E(k)-2E(k-1)+E(k-2)】由公式可以看出，要实现PID算法的编程，所需的PID变量至少有，KP,KI,KD三个系数。实验过程中，这三个变量的值就是许多工程师在实际调试过程中要调整的值。了解这点之后就开始进行实际编码。2. 直立PID的设计

6、两轮平衡小车不同于四轮的小车，开机之后就必须时刻保持直立，否则就会摔倒而无法进行任何操作，所以小车要完成的第一步就是保持直立。首先解释一下小车保持直立的原理。小车是一个直立的个体，那么当它要倒下之前，要首先倾斜，但这时如果小车提前预知了这个趋势，并且控制小车的轮子向倾斜的方向运动小段距离，就能保持平衡。小车只要时刻都保持着这个运动，就可以实现一直保持直立。考虑到这个过程对于高响应速度的要求以及其依赖于对小车状态趋势的分析，这个过程使用P（比例）D（微分）驱动而不使用I（积分）。代码如下：int balance(float Angle,float Gyro) float Bias,kp=575,

7、kd=2.7; int balance; Bias=Angle-ZHONGZHI; /求出平衡的角度中值，和机械相关 balance=kp*Bias+Gyro*kd; /计算平衡控制的电机PWM return balance;这个函数有两个形参，这两个形参分别是平衡倾角和平衡角速度，由单片机控制的陀螺仪mpu6050实时传回的数据经过卡尔曼滤波计算得出。关于陀螺仪和卡尔曼滤波的算法与此处的PID算法无关不再叙述。上面的balance函数是计算PWM值的一个函数，在另一中断函数中，使用该函数为电机PWM赋值。Balance_Pwm =balance(Angle_Balance,Gyro_Bala

8、nce);简要说明，PWM在此处指的是单片机通过IO口传出的频率，占空比都可控的方波信号，单片机通过这些方波信号来控制电机的转动与转速，根据电机种类的不同而控制方式也不同。此处可简要理解为pwm的变量值越大，电机的转速越快。那么我们来看balance函数的具体内容，函数中的kp为比例系数，kd为微分系数，该值为已经调试好的取值。输出一个值balance=kp*Bias+kd*Gyro，Angle为平衡倾角，而Bias= Angle-ZHONGZHI的意思就是求出平衡方向的倾角与实际要控制的值之间的差值，既是被控制量与被控制的值的差值，这个差值与kp的乘积就是比例系数的变量。然后再看另一个，另一

9、个是平衡倾角角加速度，由加速度传感器直接经过计算而得，也就是现成的平衡倾角（被控制量）的微分变化，与kd的乘积就是微分系数的变量。两者相加就完成了直立PID的计算。3. 速度PID的设计解决了直立的问题，下一步就要让小车能够自由前行。既是实现速度PID控制。int velocity(int encoder_left,int encoder_right) static float Velocity,Encoder_Least,Encoder,Movement; static float Encoder_Integral,Target_Velocity; float kp=210,ki=1.05;

10、 if(1=Flag_Qian) Movement=Target_Velocity/Flag_sudu;else if(1=Flag_Hou)Movement=-Target_Velocity/Flag_sudu; else Movement=0; Encoder_Least=(encoder_left+encoder_right)-0; Encoder *= 0.8; Encoder += Encoder_Least*0.2; Encoder_Integral +=Encoder; Encoder_Integral=Encoder_Integral-Movement; if(Encoder_

11、Integral10000) Encoder_Integral=10000; if(Encoder_Integral10000) Encoder_Integral=10000; if(Encoder_Integral-10000)Encoder_Integral=-10000; 则是为了防止积分变量过大，当系统处于特殊情况时积分变量不断增大可能会对系统的调整造成很大影响，于是在每次积分变量计算结束之后都对积分变量进行一个上限判断（正值上限与负值上限），如果积分变量超过10000则把它的值限定在10000，这样做增加了系统的稳定性，同时在开发过程中这样的语句也有很大的意义。最后把积分变量和比例变

12、量相加赋给电机pwm：Velocity=Encoder*kp+Encoder_Integral*ki;这样就能把每一次的计算结果传递给电机，让电机按照指定的计算结果转动，保证电机已给定的控制量方式转动。4. 转向环PID的设计两个电机都赋予了速度之后，小车就能被控制直走了，但是还需要一个转向的指令，这样小车才能够实现正常的功能。如下为转向PID函数。int turn(int encoder_left,int encoder_right,float gyro) Static float Turn_Target,Turn,Encoder_temp,Turn_Convert=0.9,Turn_Cou

13、nt; float Turn_Amplitude=15/Flag_sudu,Kp=60,Kd=0; /=遥控左右部分=/ if(1=Flag_Left|1=Flag_Right) if(+Turn_Count=1)Encoder_temp=myabs(encoder_left+encoder_right);Turn_Convert=50/Encoder_temp;if(Turn_Convert3)Turn_Convert=3; elseTurn_Convert=0.9;Turn_Count=0;Encoder_temp=0;if(1=Flag_Left) Turn_Target-=Turn_C

14、onvert;else if(1=Flag_Right) Turn_Target+=Turn_Convert; else Turn_Target=0; if(Turn_TargetTurn_Amplitude) Turn_Target=Turn_Amplitude; if(Turn_Target-Turn_Amplitude) Turn_Target=-Turn_Amplitude;if(Flag_Qian=1|Flag_Hou=1) Kd=1; else Kd=0; Turn=-Turn_Target*Kp -gyro*Kd; return Turn;转向部分使用PD控制，有kp，kd两个变

15、量。函数的三个形参分别是左轮速度，右轮速度和当然角度。这个转向环PID使用的是带有陀螺仪获取方向的控制。那么分析函数本身，先判断flag_left或者flag_right是否为1，如果为1再进行转向PID的计算，如果不是，就把turn_convert置为初值0.9，turn_count置为初值0，encoder_temp也置为0（这几个变量在函数的开始就定义为静态变量可以一直存在并且保存其值大小）。若转向标志变量设置为1，则进行计算，若Turn_Count变量自加后为1，给encoder_temp变量赋值为当前两轮速度和。而Turn_Convert变量则赋值为50/encoder_temp，是

16、根据速度越快转向越慢的调速，这样比较符合正常的感觉，在不同速度时转向变量大小若相同则会产生快速的时候转向过快的问题。同时如下两个语句：if(Turn_Convert3)Turn_Convert=3;是给Turn_Convert变量进行限幅，避免出现过大的转速变量导致系统出现问题，或者变向过快或者过慢。经过上式计算得出的Turn_Convert是当前得出的转向变量的控制目标值，于是要根据转动方向的不同为两个方向上的转动目标值赋予不同的值。如果向左转动则左轮减去转动量，右轮加上转动量。如果向右则正好相反。if(1=Flag_Left) Turn_Target-=Turn_Convert;else

17、if(1=Flag_Right) Turn_Target+=Turn_Convert; else Turn_Target=0;然后再对增加转动变量后的转向速度进行限幅if(Turn_TargetTurn_Amplitude) Turn_Target=Turn_Amplitude; if(Turn_TargetPR=15; /清楚line5上的中断标志位 Flag_Target=!Flag_Target; if(delay_flag=1) if(+delay_50=10) delay_50=0,delay_flag=0; /给主函数提供100ms精确延时 if(Flag_Target=1) /5

18、ms读取一次陀螺仪和加速度计的值，更高的采样频率可以改善滤波效果Get_Angle(Way_Angle); /更新姿态return 0; /10ms控制一次，为保证测速时间的精准，首先读取编码器的值Encoder_Left=-Read_Encoder(2); /读取编码器的值，因为两个电机的反向，所以对其中一个取反，保证输出极性一致Encoder_Right=Read_Encoder(4); /读取编码器的值 Get_Angle(Way_Angle); /更新姿态 Balance_Pwm =balance(Angle_Balance,Gyro_Balance); /；平衡PID控制 Veloc

19、ity_Pwm=velocity(Encoder_Left,Encoder_Right); /速度环PID控制，此处速度环是正反馈，就是小车快的时候要慢下来就需要再跑快一点 Turn_Pwm =turn(Encoder_Left,Encoder_Right,Gyro_Turn); /转向环PID控制 Moto1=Balance_Pwm-Velocity_Pwm+Turn_Pwm; /计算左轮电机最终PWM Moto2=Balance_Pwm-Velocity_Pwm-Turn_Pwm; /计算右轮电机最终PWM Xianfu_Pwm(); /pwm限幅Set_Pwm(Moto1,Moto2);

20、 /赋值给PWM寄存器 return 0; 该函数为单片机的中断函数，当PB5这个引脚的电平为低电平时，触发该中断。该终端由mpu6050的INT引脚触发，为5ms定时采样的定时中断，严格保证采样和数据处理的同步。该函数展示了系统把直立环，速度环和转向环三者结合的整个算法和思想，实现了在保持平衡小车直立的前提下对于小车的速度以及转向的控制。编者在每句后都加了注释在这里不再赘述。以下函数为PWM的配置函数，经过整个中断函数计算得出的电机PWM值经由这个函数配置给电机。void Set_Pwm(int moto1,int moto2) if(moto10)AIN2=1,AIN1=0;else AI

21、N2=0,AIN1=1;PWMA=myabs(moto1); if(moto2CCR1#define PWMB TIM1-CCR4将TIM1-CCR1这个寄存器宏定义给了PWMA，TIM-CCR2这个寄存器宏定义给了PWMB，这样定义增强了代码的可读性。上述函数中的mayabs函数是一个数学函数，取绝对值。以下函数为PWM限幅函数。其中PWM的最大值为7200是硬件限制。经过对定时器的计算得出的。具体计算过程在此不予说明。void Xianfu_Pwm(void) int Amplitude=6900; /=PWM的最大值为7200 限制在6900 if(Moto1Amplitude) Mot

22、o1=Amplitude; if(Moto2Amplitude) Moto2=Amplitude;最后把主函数展示出来int main(void) Stm32_Clock_Init(9); delay_init(72); LED_Init(); KEY_Init(); OLED_Init(); uart_init(72,128000); uart3_init(36,9600); MiniBalance_PWM_Init(7199,0); MiniBalance_PWM_Init(9999,35) ;Encoder_Init_TIM2(); Encoder_Init_TIM4(); Adc_In

23、it(); IIC_Init(); MPU6050_initialize(); DMP_Init(); /TIM3_Cap_Init(0XFFFF,72-1); EXTI_Init(); while(1) delay_flag=1;delay_50=0;while(delay_flag); While（1）之前的为各种外部设备和系统的初始化函数，while（1）内部为通过MPU6050的INT中断实现的50ms精确延时。下面再放一个简易的卡尔曼滤波的函数，不做解释，使用卡尔曼滤波或者互补滤波等可以使系统得到的加速度传感器传回值更稳定。void Kalman_Filter(float Accel

24、,float Gyro)angle+=(Gyro - Q_bias) * dt; /Pdot0=Q_angle - PP01 - PP10; Pdot1=-PP11;Pdot2=-PP11;Pdot3=Q_gyro;PP00 += Pdot0 * dt; PP01 += Pdot1 * dt; PP10 += Pdot2 * dt;PP11 += Pdot3 * dt;Angle_err = Accel - angle;PCt_0 = C_0 * PP00;PCt_1 = C_0 * PP10;E = R_angle + C_0 * PCt_0;K_0 = PCt_0 / E;K_1 = PC

25、t_1 / E;t_0 = PCt_0;t_1 = C_0 * PP01;PP00 -= K_0 * t_0; PP01 -= K_0 * t_1;PP10 -= K_1 * t_0;PP11 -= K_1 * t_1;angle+= K_0 * Angle_err; Q_bias+= K_1 * Angle_err; angle_dot = Gyro - Q_bias; 四， 实验总结和经验实验程序经调试可以在小车上达到非常良好的行驶效果，实验程序上写的参数都是经过实际调试PID参数之后试验认为比较合适的参数，因个人的硬件设施不同，PID系数一定会产生差异，本文中的参数只能作为参考，具体如何调PID的参数本身也十分考究。实验过程中，笔者曾在陀螺仪数据处理的方式中选择使用dmp方式，最后可能是偶然原因导致dmp方式计算总是使系统的反应较慢，达不到应有的效果。后来改用了卡尔曼滤波方式，系统的性能，小车的平稳性以及行进得到很好的改善。在其他程序中常见PID参数以另一种方式出现在程序中，即设定一个PID结构体，这种方式使用也非常方便，本文中为了讲解简便易懂，将所有的结构体变量设置为普通的变量，同时因为本文中的PID参数已经为设置好的，三个PID环中的kp，ki，kd参数都没有给外部接口，在调试过程中使用蓝牙串口直接调整这三个变量的值比较方便调试。不建议直接这样写。