十八届智能车负压电磁组（三）：后轮PID控制及差速控制篇
本文介绍小车的主要控制模块，即后轮PID控制以及差速控制。
·
文章目录
前言
本文介绍小车的主要控制模块，即后轮PID控制以及差速控制。
一、PID控制
这里不在介绍PID的由来，大家可以自行搜索。这里主要介绍代码，对于速度PID来说有位置式PID和增量式PID，使用最多的是增量式PID，但具体使用哪一种，需要根据自己调速度环时，判断哪一种调起来效果更好。
（1）代码
下面代码为 pid.h文件
typedef struct
{
float kp; //P
float ki; //I
float kd; //D
float imax; //积分限幅
float out_p; //KP输出
float out_i; //KI输出
float out_d; //KD输出
float out; //pid输出
float last_out;//pid上一次输出
float out_diff;//pid当前输出与上一次输出的差值
float integrator; //< 积分值
float last_error; //< 上次误差
float last_derivative;//< 上次误差与上上次误差之差
float target; //< 设置的期望值
}pid_param_t;
void Pid_Init(pid_param_t * pid);//PID初始化
float PidLocCtrl(pid_param_t * pid, float actual_val);//位置式PID
float PidLocCtrl_I_Div(pid_param_t * pid, float actual_val, float error_limt);//位置式 积分分离法 PID
float PidLocCtrl_Lim_Weaken(pid_param_t * pid, float actual_val);//位置式 遇限削弱法 PID
float PidIncCtrl(pid_param_t * pid, float actual_val);//pid增量式控制器输出
void PidSet(pid_param_t * pid, float p, float i, float d, float imax);//pid参数设置
void pidSetTarget(pid_param_t * pid, float target);//pid设置期望值
void pidClear(pid_param_t * pid);//pid清零
float constrain_float(float dat, float low, float high);//限幅函数下面代码为 pid.c文件
#include "pid.h"
/*************************************************************************
* 函数名称：void Pid_Init(pid_param_t * pid)
* 功能说明：PID初始化函数
* 参数说明：
* @param pid ： 参数
* 函数返回：无
* 备 注：
*************************************************************************/
void Pid_Init(pid_param_t * pid)
{
pid->kp = 0;
pid->ki = 0;
pid->kd = 0;
pid->imax = 0;
pid->out_p = 0;
pid->out_i = 0;
pid->out_d = 0;
pid->out = 0;
pid->last_out = 0;
pid->out_diff = 0;
pid->integrator= 0;
pid->last_error= 0;
pid->last_derivative = 0;
pid->target = 0;
}
/*************************************************************************
* 函数名称：float PidLocCtrl(pid_param_t * pid, float actual_val)
* 功能说明：位置式PID
* 参数说明：
* @param pid ： 参数
* @param actual_val ： 当前值
* 函数返回：pid
* 备 注：对积分进行限幅
*************************************************************************/
float PidLocCtrl(pid_param_t * pid, float actual_val)
{
/* 累积误差 */
float error = pid->target - actual_val;
pid->integrator += error;
/* 误差限幅 */
pid->integrator = constrain_float(pid->integrator, -pid->imax, pid->imax);
pid->out_p = pid->kp * error ;
pid->out_i = pid->ki * pid->integrator;
pid->out_d = pid->kd * (error - pid->last_error);
pid->last_error = error;
pid->out = pid->out_p + pid->out_i + pid->out_d;
//printf("pid->out=%F\n",pid->out);
return pid->out;
}
/*************************************************************************
* 函数名称：float PidLocCtrl_I_Div(pid_param_t * pid, float actual_val, float error_limt)
* 功能说明：位置式 积分分离法 PID
* 参数说明：
* @param pid ： 参数
* @param actual_val ： 当前值
* @param erroe_limt ： 误差门限值，该值后期实验整定
* 函数返回：pid
* 备 注：采用积分分离法，抗积分饱和 P=0.06 I = 0.04 D = 0
*************************************************************************/
float PidLocCtrl_I_Div(pid_param_t * pid, float actual_val, float error_limt)
{
/* 累积误差 */
float error = pid->target - actual_val;//期望值-当前值
pid->integrator += error;//偏差累计
/* 积分分离 */
if(error >= error_limt || error <= -error_limt )//error的绝对值大于error_limt
{
pid->integrator = 0;//将积分项置0
}
pid->out_p = pid->kp * error;
pid->out_i = pid->ki * pid->integrator;
pid->out_d = pid->kd * (error - pid->last_error);
pid->last_error = error;
pid->out = pid->out_p + pid->out_i + pid->out_d;
return pid->out;
}
/*************************************************************************
* 函数名称：float PidLocCtrl_Lim_Weaken(pid_param_t * pid, float actual_val)
* 功能说明：位置式 遇限削弱法 PID
* 参数说明：
* @param pid ： 参数
* @param actual_val ： 当前值
* 函数返回：pid
* 备 注：采用遇限削弱法，抗积分饱和
*************************************************************************/
float PidLocCtrl_Lim_Weaken(pid_param_t * pid, float actual_val)
{
/* 累积误差 */
float error = pid->target - actual_val;//期望值-当前值
/* 遇限削弱 */
if((pid->out >= PID_out_max && error < 0) || //上一次的pid_out大于上限值，并且偏差小于0
(pid->out <= -PID_out_max && error > 0)) //上一次的pid_out小于下限值，并且偏差大于0
{
//才对偏差进行累计
pid->integrator += error;//偏差累计
}
pid->out_p = pid->kp * error;
pid->out_i = pid->ki * pid->integrator;
pid->out_d = pid->kd * (error - pid->last_error);
pid->last_error = error;
pid->out = pid->out_p + pid->out_i + pid->out_d;
return pid->out;
}
/*************************************************************************
* 函数名称：float PidIncCtrl(pid_param_t * pid, float actual_val)
* 功能说明：pid增量式控制器输出
* 参数说明：
* @param pid pid参数
* @param actual_val 当前值
* 函数返回：PID输出结果 注意输出结果已经包涵了上次结果
* 备 注：
*************************************************************************/
float PidIncCtrl(pid_param_t * pid, float actual_val)
{
float error = pid->target - actual_val;//计算当前无误差
// //消除抖动误差
// if(error < 5 && error > -5)
// error = 0;
pid->out_p = pid->kp * (error - pid->last_error);
pid->out_i = pid->ki * error;
pid->out_d = pid->kd * ((error - pid->last_error) - pid->last_derivative);
pid->last_derivative = error - pid->last_error;
pid->last_error = error;
pid->out = pid->out_p + pid->out_i + pid->out_d;
return pid->out;
}
/*************************************************************************
* 函数名称：void PidSet(pid_param_t * pid, float p, float i, float d, float imax)
* 功能说明：pid参数设置
* 参数说明：
* @param pid pid参数
* @param p 比例项
* @param i 积分项
* @param d 微分项
* @param imax 积分上限
* 函数返回：无
* 备 注：
*************************************************************************/
void PidSet(pid_param_t * pid, float p, float i, float d, float imax)
{
pid->kp = p;
pid->ki = i;
pid->kd = d;
pid->imax = imax;
}
/*************************************************************************
* 函数名称：void pidSetTarget(pid_param_t * pid, float t)
* 功能说明：pid设置期望值
* 参数说明：
* @param pid pid参数
* @param t pid期望值
* 函数返回：无
* 备 注：
*************************************************************************/
void pidSetTarget(pid_param_t * pid, float target)
{
pid->target = target;
}
/*************************************************************************
* 函数名称：void pidClear(pid_param_t * pid)
* 功能说明：pid清零
* 参数说明：
* @param pid pid参数
* 函数返回：无
* 备 注：
*************************************************************************/
void pidClear(pid_param_t * pid)
{
pid->integrator= 0;
pid->last_error= 0;
pid->last_derivative = 0;
pid->out = 0;
}
/*************************************************************************
* 函数名称：float constrain_float(float amt, float low, float high)
* 功能说明：限幅函数
* 参数说明：
* @param amt ： 参数
* @param low ： 最低值
* @param high ： 最高值
* 函数返回：无
* 备 注：
*************************************************************************/
float constrain_float(float dat, float low, float high)
{
if(dat<=low)
return low;
else if(dat>=high)
return high;
else
return dat;
}（2）PID测试
这里我简单使用一个测试函数，来展示pid函数怎么调用。
#inlcude "pid.h"
void main(void)
{
//左轮
float Left_motor_kp = 1;//kp
float Left_motor_ki = 2;//ki
float Left_motor_kd = 0;//kd
float Pid_Motor_Left_imax = 1000;//误差积分项阈值
float Left_pluse= 100;//左轮实际速度，该变量为编码器输出数值，这里我直接设置的100，大家使用时，一定要将该变量设置为编码器实际输出数值
float left_speed_loop_out = 0;//左轮速度环速度输出
//右轮
float Right_motor_kp = 1;
float Right_motor_ki = 2;
float Right_motor_kd = 0;
float Pid_Motor_Right_imax = 1000;//误差积分项阈值
float Right_pluse= 100;//右轮实际速度，该变量为编码器输出数值，这里我直接设置的100，大家使用时，一定要将该变量设置为编码器实际输出数值
float right_speed_loop_out = 0;//右轮速度环速度输出
//(1)定义左右两个车轮pid结构体
pid_param_t Pid_Left_Motor;//左电机PID结构体
pid_param_t Pid_Right_Motor;//右电机PID结构体
//(2)初始化pid结构体
//左电机pid设置
Pid_Init(&Pid_Left_Motor);//PID初始化
PidSet(&Pid_Left_Motor, Left_motor_kp, Left_motor_ki, Left_motor_kd, Pid_Motor_Left_imax);//pid参数设置
pidSetTarget(&Pid_Left_Motor,0);//pid参数设置
//右电机pid设置
Pid_Init(&Pid_Right_Motor);//PID初始化
PidSet(&Pid_Right_Motor, Right_motor_kp, Right_motor_ki, Right_motor_kd, Pid_Motor_Right_imax);//pid参数设置
pidSetTarget(&Pid_Right_Motor,0);//pid参数设置
while(1)
{
#if 1
//(3.1) 增量式PID
left_speed_loop_out += PidIncCtrl(&Pid_Left_Motor,Left_pluse);
right_speed_loop_out += PidIncCtrl(&Pid_Right_Motor,Right_pluse);
#else
//(3.2) 位置式PID
left_speed_loop_out = PidLocCtrl(&Pid_Left_Motor,Left_pluse);//位置式
right_speed_loop_out = PidLocCtrl(&Pid_Right_Motor,Right_pluse);//位置式
#endif
//(4)pid输出限幅
left_speed_loop_out = constrain_float(left_speed_loop_out，-100*100,100*100);
right_speed_loop_out = constrain_float(right_speed_loop_out，-100*100,100*100);
//(5)将输出传递给pwm
//使用自己的pwm设置函数
}
}
二、差速控制
对于四轮车模来说，控制转向只依靠舵机是不够的，还需要后轮差速来辅助转向。对于后轮差速可以简单理解成两个轮的速度环目标速度不一样，就可实现差速控制。例如，向左转时，就可以把左轮速度环的目标速度Pid_Left_Motor.target设置小一点，至于要设置成多少，有两种方法。
（1）使用舵机方向环控制
可以根据上一篇讲的舵机的方向环输出结果来实现实时控制，即出现大弯时，方向环输出值变大，那么内测轮速度就会很小；出现小弯时，两轮的速度差不会太大，以实现快速过弯。
pid_param_t Pid_Servo;//定义舵机方向环结构体
ServoPidLocCtrl(&Pid_Servo,error);//调用舵机方向环PID
float StraightSpeed = 200;//直道速度，这里是以编码器实际输出即脉冲数为速度，也可以自己根据车轮的齿数来换算成实际速度m/s
float ratio = 0.2;//差速比例
//左转弯：减小左轮目标速度，右轮不变
Pid_Left_Motor.target = StraightSpeed - ratio * Pid_Servo.out;
Pid_Right_Motor.target = StraightSpeed;
//右转弯：减小右轮目标速度，左轮不变
Pid_Right_Motor.target = StraightSpeed - ratio * Pid_Servo.out;
Pid_Left_Motor.target = StraightSpeed;
（2）使用角度环来控制
对于角度环，主要是使用陀螺仪的角度来控制。
pid_param_t Pid_Servo;//定义舵机方向环结构体
ServoPidLocCtrl(&Pid_Servo,error);//舵机方向环PID
pid_param_t Pid_angle_motor;//电机角速度环结构体
Pid_angle_motor.target = Pid_Servo.out;//使用方向环的输出作为角度环的目标值
//yaw_angle 为陀螺仪的偏航角
PidLocCtrl(&Pid_angle_motor,yaw_angle);//角度环PID
float StraightSpeed = 200;//直道速度，这里是以编码器实际输出即脉冲数为速度，也可以自己根据车轮的齿数来换算成实际速度m/s
//左转弯：减小左轮目标速度，右轮不变
Pid_Left_Motor.target = StraightSpeed - Pid_angle_motor.out;
Pid_Right_Motor.target = StraightSpeed;
//右转弯：减小右轮目标速度，左轮不变
Pid_Right_Motor.target = StraightSpeed - Pid_angle_motor.out;
Pid_Left_Motor.target = StraightSpeed;
至此，可以实现小车在电磁线的引导下，通过弯道和直道。
下一篇将讲述一下对于特殊元素的处理。
十八届智能车负压电磁组（四）：赛道特殊元素处理
有“AI”的1024 = 2048，欢迎大家加入2048 AI社区
更多推荐
42
0- 0
已为社区贡献5条内容
所有评论(0)