算法设计论文PID算法实例Word格式文档下载.docx
- 文档编号:1562072
- 上传时间:2023-05-01
- 格式:DOCX
- 页数:14
- 大小:39.15KB
算法设计论文PID算法实例Word格式文档下载.docx
《算法设计论文PID算法实例Word格式文档下载.docx》由会员分享,可在线阅读,更多相关《算法设计论文PID算法实例Word格式文档下载.docx(14页珍藏版)》请在冰点文库上搜索。
在此之前首先深入了解PID算法设计思路。
第一要明确设计目的,设计目标是制作一个能够保持平衡的两轮平衡小车,那么首先要保持平衡,其次要能够正常直线行走,再次能实现转向,一个小车就完成了。
所以为了完成这些功能的设计,首先要有一个功能足够的PID体系。
比例(P)控制
比例控制是一种最简单的控制方式。
其控制器的输出与输入误差信号成比例关系。
当仅有比例控制时系统输出存在稳态误差(Steady-stateerror)。
积分(I)控制
在积分控制中,控制器的输出与输入误差信号的积分成正比关系。
对一个自动控制系统,如果在进入稳态后存在稳态误差,则称这个控制系统是有稳态误差的或简称有差系统(SystemwithSteady-stateError)。
为了消除稳态误差,在控制器中必须引入“积分项”。
积分项对误差取决于时间的积分,随着时间的增加,积分项会增大。
这样,即便误差很小,积分项也会随着时间的增加而加大,它推动控制器的输出增大使稳态误差进一步减小,直到等于零。
因此,比例+积分(PI)控制器,可以使系统在进入稳态后无稳态误差。
微分(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的设计
两轮平衡小车不同于四轮的小车,开机之后就必须时刻保持直立,否则就会摔倒而无法进行任何操作,所以小车要完成的第一步就是保持直立。
首先解释一下小车保持直立的原理。
小车是一个直立的个体,那么当它要倒下之前,要首先倾斜,但这时如果小车提前预知了这个趋势,并且控制小车的轮子向倾斜的方向运动小段距离,就能保持平衡。
小车只要时刻都保持着这个运动,就可以实现一直保持直立。
考虑到这个过程对于高响应速度的要求以及其依赖于对小车状态趋势的分析,这个过程使用P(比例)D(微分)驱动而不使用I(积分)。
代码如下:
intbalance(floatAngle,floatGyro)
{
floatBias,kp=575,kd=2.7;
intbalance;
Bias=Angle-ZHONGZHI;
//求出平衡的角度中值,和机械相关
balance=kp*Bias+Gyro*kd;
//计算平衡控制的电机PWM
returnbalance;
}
这个函数有两个形参,这两个形参分别是平衡倾角和平衡角速度,由单片机控制的陀螺仪mpu6050实时传回的数据经过卡尔曼滤波计算得出。
关于陀螺仪和卡尔曼滤波的算法与此处的PID算法无关不再叙述。
上面的balance函数是计算PWM值的一个函数,在另一中断函数中,使用该函数为电机PWM赋值。
Balance_Pwm=balance(Angle_Balance,Gyro_Balance);
简要说明,PWM在此处指的是单片机通过IO口传出的频率,占空比都可控的方波信号,单片机通过这些方波信号来控制电机的转动与转速,根据电机种类的不同而控制方式也不同。
此处可简要理解为pwm的变量值越大,电机的转速越快。
那么我们来看balance函数的具体内容,函数中的kp为比例系数,kd为微分系数,该值为已经调试好的取值。
输出一个值balance=kp*Bias+kd*Gyro,Angle为平衡倾角,而Bias=Angle-ZHONGZHI的意思就是求出平衡方向的倾角与实际要控制的值之间的差值,既是被控制量与被控制的值的差值,这个差值与kp的乘积就是比例系数的变量。
然后再看另一个,另一个是平衡倾角角加速度,由加速度传感器直接经过计算而得,也就是现成的平衡倾角(被控制量)的微分变化,与kd的乘积就是微分系数的变量。
两者相加就完成了直立PID的计算。
3.速度PID的设计
解决了直立的问题,下一步就要让小车能够自由前行。
既是实现速度PID控制。
intvelocity(intencoder_left,intencoder_right)
staticfloatVelocity,Encoder_Least,Encoder,Movement;
staticfloatEncoder_Integral,Target_Velocity;
floatkp=210,ki=1.05;
if(1==Flag_Qian)Movement=Target_Velocity/Flag_sudu;
elseif(1==Flag_Hou)Movement=-Target_Velocity/Flag_sudu;
elseMovement=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_Integral>
10000)Encoder_Integral=10000;
if(Encoder_Integral<
-10000)Encoder_Integral=-10000;
Velocity=Encoder*kp+Encoder_Integral*ki;
returnVelocity;
这个函数就是速度PID控制的函数,由于小车本身速度不快(使用直流减速电机),不需要对于很快的动作有过于明显的反应所以舍弃微分D而只使用P和I两个变量进行控制。
Flag_Qian和Flag_Hou两个变量是标志位,若变量为0表示不行进,Flag_Hou=1表示向后走,Flag_Qian=1表示往前走。
函数的两个形参分别是编码器返回值的左轮速度和右轮速度(编码器是用来检测转动圈数的,返回值是单位时间内的转动圈数,也就是速度)。
此处需要说明的是如下语句:
Encoder_Least=(encoder_left+encoder_right)-0;
Encoder_Least是当前速度值(两轮速度和),得到当前速度之后,并没有直接把它作为比例量与比例系数相乘,也没有把它的值作为一个时刻的积分量累加,而是让Encoder量自乘0.8,再与Encoder_Least乘0.2的结果相加。
这个处理过程其实是一个低通滤波过程。
由于编码器传回的数据也是直接从机械装置之中检测到的值,偶然事件对该值的影响可能会比较明显,但是这样就会偏离真实的转动量,所以对转动量(速度)这个值进行低通滤波之后再作为变量使用,这样做是为了让小车更加平稳,速度的PID效果更稳定高效。
Encoder_Integral这个变量则是转动量(速度)的积分变量。
积分量每次都加上一个当前变量值,减去该次的速度的目标值,表示速度与目标速度的差值的积分。
其中如下语句:
if(Encoder_Integral>
则是为了防止积分变量过大,当系统处于特殊情况时积分变量不断增大可能会对系统的调整造成很大影响,于是在每次积分变量计算结束之后都对积分变量进行一个上限判断(正值上限与负值上限),如果积分变量超过10000则把它的值限定在10000,这样做增加了系统的稳定性,同时在开发过程中这样的语句也有很大的意义。
最后把积分变量和比例变量相加赋给电机pwm:
Velocity=Encoder*kp+Encoder_Integral*ki;
这样就能把每一次的计算结果传递给电机,让电机按照指定的计算结果转动,保证电机已给定的控制量方式转动。
4.转向环PID的设计
两个电机都赋予了速度之后,小车就能被控制直走了,但是还需要一个转向的指令,这样小车才能够实现正常的功能。
如下为转向PID函数。
intturn(intencoder_left,intencoder_right,floatgyro)
{
StaticfloatTurn_Target,Turn,Encoder_temp,Turn_Convert=0.9,Turn_Count;
floatTurn_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_Convert<
0.6)Turn_Convert=0.6;
if(Turn_Convert>
3)Turn_Convert=3;
}
else
Turn_Convert=0.9;
Turn_Count=0;
Encoder_temp=0;
}
if(1==Flag_Left)Turn_Target-=Turn_Convert;
elseif(1==Flag_Right)Turn_Target+=Turn_Convert;
elseTurn_Target=0;
if(Turn_Target>
Turn_Amplitude)Turn_Target=Turn_Amplitude;
if(Turn_Target<
-Turn_Amplitude)Turn_Target=-Turn_Amplitude;
if(Flag_Qian==1||Flag_Hou==1)Kd=1;
elseKd=0;
Turn=-Turn_Target*Kp-gyro*Kd;
returnTurn;
转向部分使用PD控制,有kp,kd两个变量。
函数的三个形参分别是左轮速度,右轮速度和当然角度。
这个转向环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,是根据速度越快转向越慢的调速,这样比较符合正常的感觉,在不同速度时转向变量大小若相同则会产生快速的时候转向过快的问题。
同时如下两个语句:
if(Turn_Convert<
是给Turn_Convert变量进行限幅,避免出现过大的转速变量导致系统出现问题,或者变向过快或者过慢。
经过上式计算得出的Turn_Convert是当前得出的转向变量的控制目标值,于是要根据转动方向的不同为两个方向上的转动目标值赋予不同的值。
如果向左转动则左轮减去转动量,右轮加上转动量。
如果向右则正好相反。
if(1==Flag_Left)Turn_Target-=Turn_Convert;
然后再对增加转动变量后的转向速度进行限幅
最后如果正在向前或者向后行进,则增加一个kd=1多一个陀螺仪的纠正变量,if(Flag_Qian==1||Flag_Hou==1)Kd=1;
最后计算出转向PID的电机PWM值,并返回这个值。
Turn=-Turn_Target*Kp-gyro*Kd;
至此转动变量的PID计算也结束了。
三,实验的程序全貌
在这里给出操作这三个函数的控制函数:
intEXTI9_5_IRQHandler(void)
if(PBin(5)==0)
{
EXTI->
PR=1<
<
5;
//清楚line5上的中断标志位
Flag_Target=!
Flag_Target;
if(delay_flag==1)
{
if(++delay_50==10)delay_50=0,delay_flag=0;
//给主函数提供100ms精确延时
}
if(Flag_Target==1)//5ms读取一次陀螺仪和加速度计的值,更高的采样频率可以改善滤波效果
Get_Angle(Way_Angle);
//更新姿态
return0;
}//10ms控制一次,为保证测速时间的精准,首先读取编码器的值
Encoder_Left=-Read_Encoder
(2);
//读取编码器的值,因为两个电机的反向,所以对其中一个取反,保证输出极性一致
Encoder_Right=Read_Encoder(4);
//读取编码器的值
Get_Angle(Way_Angle);
//更新姿态
Balance_Pwm=balance(Angle_Balance,Gyro_Balance);
//;
平衡PID控制
Velocity_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);
//赋值给PWM寄存器
}
return0;
该函数为单片机的中断函数,当PB5这个引脚的电平为低电平时,触发该中断。
该终端由mpu6050的INT引脚触发,为5ms定时采样的定时中断,严格保证采样和数据处理的同步。
该函数展示了系统把直立环,速度环和转向环三者结合的整个算法和思想,实现了在保持平衡小车直立的前提下对于小车的速度以及转向的控制。
编者在每句后都加了注释在这里不再赘述。
以下函数为PWM的配置函数,经过整个中断函数计算得出的电机PWM值经由这个函数配置给电机。
voidSet_Pwm(intmoto1,intmoto2)
if(moto1<
0)AIN2=1,AIN1=0;
elseAIN2=0,AIN1=1;
PWMA=myabs(moto1);
if(moto2<
0)BIN1=0,BIN2=1;
elseBIN1=1,BIN2=0;
PWMB=myabs(moto2);
其中,PWMA和PWMB都是宏定义的变量:
#definePWMATIM1->
CCR1
#definePWMBTIM1->
CCR4
将TIM1->
CCR1这个寄存器宏定义给了PWMA,TIM->
CCR2这个寄存器宏定义给了PWMB,这样定义增强了代码的可读性。
上述函数中的mayabs函数是一个数学函数,取绝对值。
以下函数为PWM限幅函数。
其中PWM的最大值为7200是硬件限制。
经过对定时器的计算得出的。
具体计算过程在此不予说明。
voidXianfu_Pwm(void)
intAmplitude=6900;
//===PWM的最大值为7200限制在6900
if(Moto1<
-Amplitude)Moto1=-Amplitude;
if(Moto1>
Amplitude)Moto1=Amplitude;
if(Moto2<
-Amplitude)Moto2=-Amplitude;
if(Moto2>
Amplitude)Moto2=Amplitude;
最后把主函数展示出来
intmain(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_Init();
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精确延时。
下面再放一个简易的卡尔曼滤波的函数,不做解释,使用卡尔曼滤波或者互补滤波等可以使系统得到的加速度传感器传回值更稳定。
voidKalman_Filter(floatAccel,floatGyro)
angle+=(Gyro-Q_bias)*dt;
//Ï
È
Ñ
é
¹
À
¼
Æ
Pdot[0]=Q_angle-PP[0][1]-PP[1][0];
Pdot[1]=-PP[1][1];
Pdot[2]=-PP[1][1];
Pdot[3]=Q_gyro;
PP[0][0]+=Pdot[0]*dt;
PP[0][1]+=Pdot[1]*dt;
PP[1][0]+=Pdot[2]*dt;
PP[1][1]+=Pdot[3]*dt;
Angle_err=Accel-angle;
PCt_0=C_0*PP[0][0];
PCt_1=C_0*PP[1][0];
E=R_angle+C_0*PCt_0;
K_0=PCt_0/E;
K_1=PCt_1/E;
t_0=PCt_0;
t_1=C_0*PP[0][1];
PP[0][0]-=K_0*t_0;
PP[0][1]-=K_0*t_1;
PP[1][0]-=K_1*t_0;
PP[1][1]-=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方式
- 配套讲稿:
如PPT文件的首页显示word图标,表示该PPT已包含配套word讲稿。双击word图标可打开word文档。
- 特殊限制:
部分文档作品中含有的国旗、国徽等图片,仅作为作品整体效果示例展示,禁止商用。设计者仅对作品中独创性部分享有著作权。
- 关 键 词:
- 算法 设计 论文 PID 实例