机器人底盘运动控制
运动控制使用增量式PID进行控制，控制周期20ms，当上位机发送速度控制命令时，下位机读取发送的速度值，获取的速度值需要转化成编码器脉冲，根据轮径、减速比确定速度与脉冲间的换算关系系数(单位m/s):
SpeedParm=CntPerCicle/(D*Pi*(50/100))
式中CntPerCicle为轮子每转一周所产生的脉冲数，D为轮子直径，根据测算，CntPerCircle取4100，D取10.5，故实际需要的脉冲为Speed*SpeedParm,据此找到速度与控制脉冲之间的换算关系。将计算的脉冲值传递给速度控制函数，在速度控制函数中首先对计算获取的脉冲值进行限幅处理，将目标控制的脉冲值与实际获取的脉冲值做差，根据计算的偏差进行分段，速度控制过程分为启动阶段、分段PID控制阶段、减速停止阶段。当偏差大于设定的启动脉冲值，并且实际运动的脉冲值小于设定脉冲判断为处于启动状态，当实际的脉冲大于等于设定启动脉冲时即可判断机器人已经启动，此时切换到PID控制阶段，PID控制阶段，使用增量式PID，根据脉冲偏差将整个PID控制过程分为三段，当偏差较大时给相对较大的PID系数，当实际的脉冲较为接近设定值时，切换到稍小的PID系数，当实际脉冲在设定值附近时，切换到较小的PID系数，当偏差较大时，系统能够较快的作出响应，当调到设定值附件时，较大的PID系数会导致系统不稳定，采用较小的PID系数,保证系统有很好的稳态控制效果。
速度、脉冲换算：
/**************************************************** Input : LeftSpeed RightSpeed OutPut : None Berif : SpeedControl Note : Add : Version: V 1.0 ****************************************************/ float SpeedParm=248.58;// (4100/(10.5*pi)) *2 short RightCntSet=0,LeftCntSet=0; void SpeedControl(int LeftSpeed,int RightSpeed) { if(LeftSpeed>SpeedMax)LeftSpeed=SpeedMax;//限幅 if(LeftSpeed<-SpeedMax)LeftSpeed=-SpeedMax; if(RightSpeed>SpeedMax)RightSpeed=SpeedMax; if(RightSpeed<-SpeedMax)RightSpeed=-SpeedMax; LeftCntSet=(short)(LeftSpeed*SpeedParm/100000);//上位机发送扩大10万倍、还原 RightCntSet=(short)(RightSpeed*SpeedParm/100000); RIGHT_ENCODE_CONTROL(RightCntSet); LEFT_ENCODE_CONTROL(LeftCntSet); }
电机脉冲闭环控制：
/***************************************************** Input : short EncodeCnt OutPut : None Berif : LEFT_ENCODE_CONTROL Add : Version: V 2.0 ******************************************************/ float ti=0.1,tp=0.19,td=0.015; float left_err=0,p_left_err=0,pp_left_err=0; float left_delta_duty=0;//0.044 0.079 0.05 float lki=0.6,lkp=0.79,lkd=0.25;//lki=0.045,lkp=0.079,lkd=0.05; char LeftMoveState=‘E‘;//S:start E:stop M:move float CorrectionParam=0.95; void LEFT_ENCODE_CONTROL(short EncodeCnt) { if(EncodeCnt==0) LEFT_PWM=0; if(EncodeCnt>EncodeCntMax)EncodeCnt=EncodeCntMax; if(EncodeCnt<EncodeCntMin)EncodeCnt=EncodeCntMin; if(EncodeCnt*EncodeLeftCnt<0)EncodeCnt=0; left_err=EncodeCnt-EncodeLeftCnt;//EncodeCnt 目标脉冲数;EncodeLeftCnt 实时编码器脉冲数 if((left_err<=10)&&(left_err>=-10)) lki=0.1,lkp=0.019,lkd=0.015; else if((left_err<=12)&&(left_err>=-12)) lki=0.2,lkp=0.19,lkd=0.025; else if((left_err<=18)&&(left_err>=-18)) lki=0.4,lkp=0.39,lkd=0.25; else lki=0.6,lkp=0.59,lkd=0.25; if((left_err>DeadZoneErrCnt)||(left_err<-DeadZoneErrCnt)) { if((EncodeLeftCnt<2)&&(EncodeLeftCnt>-2))//start up zone LeftMoveState=‘S‘; else { if((EncodeCnt<DeadZoneCnt)&&(EncodeCnt>-DeadZoneCnt)) LeftMoveState=‘E‘;//stop else LeftMoveState=‘M‘; } } if((left_err<=DeadZoneErrCnt)&&(left_err>=-DeadZoneErrCnt)) { if((EncodeCnt<DeadZoneCnt)&&(EncodeCnt>-DeadZoneCnt)) LeftMoveState=‘E‘;//End else LeftMoveState=‘M‘;//move normal } switch(LeftMoveState) { case ‘S‘: if(EncodeCnt>0) { if(LEFT_PWM<DeadZonePwm*CorrectionParam) LEFT_PWM=DeadZonePwm*CorrectionParam; left_delta_duty=2; } if(EncodeCnt<0) { if(LEFT_PWM>-DeadZonePwm*CorrectionParam) LEFT_PWM=-DeadZonePwm*CorrectionParam; left_delta_duty=-2; } break; case ‘M‘: left_err=(EncodeCnt-EncodeLeftCnt)/2.0; break; case ‘E‘: left_err=(EncodeCnt-EncodeLeftCnt)/1.0; break; default: break; } if(LeftMoveState!=‘S‘) { left_delta_duty=lkp*(left_err-p_left_err)+lki*left_err+lkd*(left_err-2*p_left_err+pp_left_err); pp_left_err=p_left_err; p_left_err=left_err; } if((left_delta_duty<=DeadZone)&&(left_delta_duty>0))//死区控制 { left_delta_duty=0; } if((left_delta_duty>=-DeadZone)&&(left_delta_duty<0)) { left_delta_duty=0; } LEFT_PWM+=left_delta_duty; if(EncodeCnt==0)LEFT_DIR_CONTROL(‘S‘);//方向控制 if(EncodeCnt>0)LEFT_DIR_CONTROL(‘D‘); if(EncodeCnt<0)LEFT_DIR_CONTROL(‘R‘); if(LEFT_PWM>PWM_Period) LEFT_PWM=PWM_Period;//限幅 if(LEFT_PWM<-PWM_Period)LEFT_PWM=-PWM_Period; if(LEFT_PWM<=0) { LEFT_MOTOR_PWM_OUT(-(short)LEFT_PWM); } else { LEFT_MOTOR_PWM_OUT((short)LEFT_PWM); } }
原文：https://www.cnblogs.com/xgth/p/11853728.html