PID循迹控制算法
前言
让小车进行循迹最重要的是获取车相对于循迹线的位置，关于循迹模块的原理我已经在之前的一篇博客上讲过，如果大家没看过的话可以点此链接：PID循迹模块原理。
PID循迹原理简介
PID循迹的原理我们大致可以用常识性思维去思考下，如图1所示为循迹车寻线行走图示，现在小车要按照中间那条黑线循迹行走，如果车头往左偏则需要控制方向盘让车往右转，车头往又偏则需要让车往左转。但是仅仅只有左转和右转的话，车一到左边就向右转，一到右边就向左转，则车会来回在线左右晃动，速度一快就会偏离航向。这种循迹方法是最简单的方法，市场上常用的循迹传感器控制方法也就是这种，因为这种循迹传感器只能输出0或1，如图2所示。
所以这种传感器控制效果很差，速度慢了还好，速度稍微一块就会很容易偏离循迹线。如果我们需要实现更为精确的控制就需要引入比例P控制，也就是我们需要通过考验获取到线性输出的循迹传感器，获取到小车偏离黑线的程度，偏离的越远就向黑线方向转的程度就越大，偏离的越近就向黑线方向转的程度就越小，这就是引入比例P控制。但是转的时候又不能使小车转的过快，这就需要引入微分d控制，也就是PI转向控制器即可控制小车的循迹转向。小车在循迹的的时候有时也需要控制小车的循迹速度，控制小车的循迹速度需要PI速度控制器，前提当然我们也需要一个获取小车速度的传感器，也就是编码器，下面开始介绍下PID控制器原理。
PID控制器原理
PID控制器是一种线性控制方法，控制原理如图3-1所示，它根据给定值r(t)与实际输出值y(t)构成控制偏差e(t)，即e(t)=r(t)-y(t)。对偏差进行比例、积分、微分运算，将三种运算结果相加，即可得到PID控制器的控制输出u(t)。在连续时间域中，PID控制器算法表达式如下：
式中：kp为比例系数，Ti为积分时间常数；Td为微分时间常数。
在循迹控制中，只需要PD控制器即可，公式中输入偏差e(t)为循迹传感器输出值，经过PD控制器后输出u(t)直接控制小车转向。在循迹控制中我们也需要控制循迹车的前进的速度，控制循迹车前进速度需要通过PI控制器来实现。循迹小车的整个控制原理系统框图如图所示。
PID控制器代码实现
PID控制器实现代码如下：
//定义pid结构体
struct PID
{
float kp;
float ki;
float kd;
};
typedef struct PID _PID;
//定义转向pid参数结构体
_PID TurnPdate=
{
.kp=100, //赋值比例值60
.ki=0, //赋值积分值
.kd=-34 //赋值微分值-45
};
//定义速度pid参数结构体
_PID SpdPdate=
{
.kp=-50, //赋值比例值-80
.ki=-6, //赋值积分值-10
.kd=0 //赋值微分值
};
/*@brief: 位置式PID控制器
* @param:
* [in] float deviation: 和目标值得偏差
* [in] _PID pid: 位置式pid的参数
* @return: 调节占空比的一个整形数
*/
int PositionPID(float deviation,_PID pid)
{
float Position_KP=pid.kp,Position_KI=pid.ki,Position_KD=pid.kd;
static float Bias,Pwm,Integral_bias,Last_Bias;
Bias=deviation; //计算偏差
Integral_bias+=Bias; //求出偏差的积分
Pwm=Position_KP*Bias+Position_KI*Integral_bias+Position_KD*(Bias-Last_Bias); //位置式PID控制器
Last_Bias=Bias; //保存上一次偏差
return Pwm;
}
/*@brief: 为速度pid使用位置式PID控制器，对积分值进行限幅
* @param:
* [in] float deviation: 和目标值得偏差
* [in] _PID pid: 位置式pid的参数
* @return: 调节占空比的一个整形数
*/
int PositionPIDToSpd(float deviation,_PID pid)
{
float Position_KP=pid.kp,Position_KI=pid.ki,Position_KD=pid.kd;
static float Bias,Pwm,Integral_bias,Last_Bias,pwmKI=0;
Bias=deviation; //计算偏差
Integral_bias+=Bias; //求出偏差的积分
pwmKI=Position_KI*Integral_bias;
if(pwmKI>MAX_MOTOR_PWM) Integral_bias=MAX_MOTOR_PWM/Position_KI;
Pwm=Position_KP*Bias+pwmKI+Position_KD*(Bias-Last_Bias); //位置式PID控制器
Last_Bias=Bias; //保存上一次偏差
return Pwm;
}
代码中PositionPID(float deviation,_PID pid)中函数deviation偏差参数输入循迹传感器输出值，pid参数需要根据自己车型和循迹场地调试设置。
输出转向控制
通过PD转向控制器获取到输出数据后，需要控制舵机转向，一般循迹车有两种车型，分别为舵机转向型和差速转向型。
如图5所示为差速转向车型，这种车转向方法是通过控制车轮的差速。我们可以想到如果让小车往右转，则需左轮往前转，然后右轮往后转，且差速越大转的越快。同理，让小车往左转的话需要右轮往前转，左轮往后转。实现小车向前走的话就是两个轮子同时往前转即可，控制小车循迹速度的话要映入PI速度控制器，同时需要传感器测量出小车的速度，这是就需要大家使用编码器电机了。
代码实现如下：
/*@brief:根据循迹传感器pid调节小车转向使小车处于黑线中间
* @param:
* [in]int TraceDate: 循迹传感器输出的值
* [in]float TarSpdL:右边电机目标速度,最大速度越1.27m/s
* @return: 返回调节电机速度的转向pwm
*/
int ChangeTraceTurn(int TraceDate)
{
int pwm=0;
int bias;
bias=TraceDate;
pwm=PositionPID(bias,TraceTurnPdate);
if(pwm>5000) pwm=5000;//限幅
else if(pwm<-5000) pwm=-5000;
return pwm;
}
/*@brief:根据pid调节左边电机到目标速度
* @param:
* [in]int EncodeSpdL: 当前左电机编码器测速值
* [in]float TarSpdL:左边电机目标速度,最大速度越1.19m/s
* @return: 返回左边电机计算后的pwm占空比
*/
int ChangeSpeedMotorL(int NowEncodeSpdL,float TarSpdL)
{
int pwm=0;
int bias;
int TarEncodeSpdL;
TarEncodeSpdL=(int)((TarSpdL*ACircleEncoder)/(WheelOneCircleDis*100)+0.5f);//根据目标速度求出目标编码器速度
bias=NowEncodeSpdL-TarEncodeSpdL;
pwm=PositionPIDToSpd(bias,SpdPdate);
if(pwm>5000) pwm=5000;//限幅
else if(pwm<-5000) pwm=-5000;
return pwm;
}
/*@brief:根据pid调节右边电机到目标速度
* @param:
* [in]int EncodeSpdL: 当前右电机编码器测速值
* [in]float TarSpdL:右边电机目标速度,最大速度越1.27m/s
* @return: 返回右边电机计算后的pwm占空比
*/
int ChangeSpeedMotorR(int NowEncodeSpdR,float TarSpdR)
{
int pwm=0;
int bias;
int TarEncodeSpdR;
TarEncodeSpdR=(int)((TarSpdR*ACircleEncoder)/(WheelOneCircleDis*100)+0.5f);//根据目标速度求出目标编码器速度
bias=NowEncodeSpdR-TarEncodeSpdR;
pwm=PositionPIDToSpd(bias,SpdPdate);
if(pwm>5000) pwm=5000;//限幅
else if(pwm<-5000) pwm=-5000;
return pwm;
}
/*@brief: 让小车根据循迹黑线走
*@param:
* [in]TraceDate: 循迹传感器输出的值
* [in]TarSpeed:循迹的目标速度
*@return: 到达目标点返回1，否则返回0
*/
void TraceMove(int TraceDate,float TarSpeed)
{
int turnpwm=0;
int spdpwml=0,spdpwmr=0;
int pwml=0,pwmr=0;
turnpwm=ChangeTraceTurn(TraceDate);
spdpwml=ChangeSpeedMotorL(Encode_Left,TarSpeed);
spdpwmr=ChangeSpeedMotorR(Encode_Right,TarSpeed);
pwmr=turnpwm+spdpwmr;
if(pwmr>5000) pwmr=5000;//限幅
else if(pwmr<-5000) pwmr=-5000;
pwml=-turnpwm+spdpwml;
if(pwml>5000) pwml=5000;//限幅
else if(pwml<-5000) pwml=-5000;
Set_Pwm_Motor1(pwmr); //设置电机1占空比
Set_Pwm_Motor2(pwml); //设置电机2占空比
}
舵机转向车结构如图6所示，控制舵机转向原理和差速转向相似，不同点是控制转向的输出最后作用于舵机上，将循迹传感器通过PD控制后的输出控制舵机即可，大家可以参考差速转向写舵机转向的代码，同样舵机转向也需要控制车的行驶速度。
总结
关于小车的PID循迹控制，最重要的点是需要一个能够获取线性输出的循迹传感器，然后通过PD控制器控制舵机循迹转向,其次我们在循迹的同时如果需要控制小车的行驶速度需要引入PI速度控制器，两者共同作用即可实现小车循迹的PID控制。