内含代码PID平衡控制在智能巡逻机器人上的应用
Posted 三明治开发社区
tags:
篇首语:本文由小常识网(cha138.com)小编为大家整理,主要介绍了内含代码PID平衡控制在智能巡逻机器人上的应用相关的知识,希望对你有一定的参考价值。
姿态平衡控制
在直立控制里面加入速度负反馈无法达到速度闭环的目的,而且还会破坏直立控制系统,因此在保证直立控制的优先级条件下,开发者们要把速度控制放在直立控制的前面,也就是速度控制调节的结果仅仅是改变直立控制的目标值。
根据经验可知,小车的运行速度和小车的倾角是相关的。比如要提高小车向前行驶的速度,就需要增加小车向前倾斜的角度,倾斜角度加大之后,车轮在直立控制的作用下需要向前运动保持小车平衡,速度增大;如果要降低小车向前行驶的速度,就需要减小小车向前倾斜的角度,倾斜角度减小之后,车轮在直立控制的作用下向后运动保持小车平衡,速度减小。
开发者们把速度和直立两个控制器串联起来工作,其中速度控制的输出作为直立控制的输入,而直立控制的输出作为系统的输出,这其实就是一个串级控制系统。其中直立控制使用PD控制。因为编码器可能存在的噪声,为防止噪声被放大并消除系统的静差,这里速度控制使用PI控制。
直立PD控制
int balance(float Angle,float Gyro)
{
float Bias;
int balancePID;
Bias=Angle-Angle_OFFSET; //===求出平衡的角度中值 和机械相关
balancePID=Balance_Kp*Bias+Gyro*Balance_Kd; //===计算平衡控制的电机PWM PD控制 kp是P系数 kd是D系数
return balancePID;
}
速度PI控制
int velocity(int encoder_left,int encoder_right)
{
static float Velocity,Encoder_Least,Encoder,Movement;
static float Encoder_Integral,Target_Velocity;
//=============遥控前进后退部分=======================//
Target_Velocity=40;
if(Direction.Current==GO_STRAIGHT) Movement=-Target_Velocity/Flag_speed; //===前进标志位置1
else if(Direction.Current==GO_BACK) Movement=Target_Velocity/Flag_speed; //===后退标志位置1
else Movement=0;
//=============速度PI控制器=======================//
Encoder_Least =(encoder_left+encoder_right)-0;
Encoder *= 0.8; //===一阶低通滤波器
Encoder += Encoder_Least*0.2; //===一阶低通滤波器
Encoder_Integral +=Encoder; //===积分出位移 积分时间:10ms
Encoder_Integral=Encoder_Integral-Movement; //===接收遥控器数据,控制前进后退
if(Encoder_Integral>8000) Encoder_Integral=8000; //===积分限幅
if(Encoder_Integral<-8000) Encoder_Integral=-8000; //===积分限幅
Velocity=Encoder*Velocity_Kp+Encoder_Integral*Velocity_Ki; //===速度控制
if(Turn_Off(Angle_Balance,BAT_VOL)==1||Direction.Current==TURN_OFF)
Encoder_Integral=0;
return Velocity;
}
转向控制
除了保持平衡之外,小车也涉及到左右转动,因此还需要加入转向的控制,可参考如下
int turn(int encoder_left,int encoder_right,float gyro)//转向控制
{
static float Turn_Target,Turn,Encoder_temp,Turn_Convert=0.9,Turn_Count;
float Turn_Amplitude=30/Flag_speed,Kp=32,Kd=0;
if(Direction.Current==TURN_LEFT||Direction.Current==TURN_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(Direction.Current==TURN_LEFT){
Turn_Target+=Turn_Convert;
}
else if(Direction.Current==TURN_RIGHT){
Turn_Target-=Turn_Convert;
}
else Turn_Target=0;
if(Turn_Target>Turn_Amplitude) Turn_Target=Turn_Amplitude; //===转向速度限幅
if(Turn_Target<-Turn_Amplitude) Turn_Target=-Turn_Amplitude;
if(Direction.Current==GO_STRAIGHT||Direction.Current==GO_BACK||Direction.Current==KEEP_STOP)
Kd=-1 ;
else
Kd=0;
//=============转向PD控制器=======================//
Turn=-Turn_Target*Kp-gyro*Kd; //===结合Z轴陀螺仪进行PD控制
return Turn;
}
电机PWM控制
通过上述一系列控制计算后,开发者就得到了使小车平衡的PWM值(即下面程序中的Moto1和Moto2)。
Balance_Pwm = balance(Angle_Balance,Gyro_Balance);//===平衡PID控制
Velocity_Pwm= velocity(Enconder_left,Enconder_right);//===速度环PID控制
Turn_Pwm = turn(Enconder_left,Enconder_right,Gyro_Turn);//===转向环PID控制
Moto1=Balance_Pwm+Velocity_Pwm-Turn_Pwm;//===计算左轮电机最终PWM
Moto2=Balance_Pwm+Velocity_Pwm+Turn_Pwm;//===计算右轮电机最终PWM
Limit_Pwm(); //===PWM限幅
Set_Pwm(Moto1,Moto2); //===赋值给PWM寄存器
将该PWM幅值给对应寄存器,就可以查看小车实际运动状态了。
void Set_Pwm(int moto1,int moto2)
{
if(moto2>0) {AIN2_RESET;AIN1_SET;}
else {AIN2_SET;AIN1_RESET;}
TIM16_PWM_Set(myabs(moto2));
if(moto1>0) {BIN1_RESET;BIN2_SET;}
else {BIN1_SET;BIN2_RESET;}
TIM17_PWM_Set(myabs(moto1));
}
以上是关于内含代码PID平衡控制在智能巡逻机器人上的应用的主要内容,如果未能解决你的问题,请参考以下文章