机械结构
四轮车的重心尽量靠近中心位置,PCB和电池的摆放位置尽可能低一些,保证小车在过弯时既能轻松的转向又不会甩尾,在直线上的加减速也会有更好的效果。
舵机调教
对于C车模,四轮车来说,转向最重要的一环就是舵机,如果舵机是歪的,那怎么跑都跑不好,拿到车模的第一步就是将舵机进行归中处理,并对舵机的左右极限角度进行限幅。
编码器的读值与滤波处理
我比赛时候用的是逐飞的正交编码器,读取编码器数值,我们只需要调用逐飞的开源库即可,这一个步骤是相对来说简单的,我们就不展开来说。
编码器读值
encoder_data_1 = encoder_get_count(ENCODER_1);
Left_Speed.Encoder = -(int)encoder_data_1;
encoder_clear_count(ENCODER_1);
encoder_data_2 = encoder_get_count(ENCODER_2);
Right_Speed.Encoder = (int)encoder_data_2;
encoder_clear_count(ENCODER_2);
编码器滤波
第二步就是编码器的滤波处理,由于小车在高速运行的过程中,即使编码器可能安装的比较稳定,但是还是会有车辆抖动的情况,这种情况在过弯的时候尤为明显,为了防止这种不必要的读值,我使用了FIR滤波。
void FIR_Init(FIR *filter, float sample_freq, float cutoff_freq)
{
filter->sample_freq = sample_freq;
filter->cutoff_freq = cutoff_freq;
float fr = sample_freq/cutoff_freq;
float ohm = tanf(PI/fr);
float c = 1.0f+2.0f*cosf(PI/4.0f)*ohm + ohm*ohm;
filter->b0 = ohm*ohm/c;
filter->b1 = 2.0f*filter->b0;
filter->b2 = filter->b0;
}
/*需要滤波的信号sample 上次的输出信号_output*/
float FIR_apply(FIR *filter, float sample)
{
filter->oupt = sample * filter->b0 + filter->temp1 * filter->b1 + filter->temp2 * filter->b2;
filter->temp2 = filter->temp1;
filter->temp1 = sample;
return filter->oupt;
}
FIR_Init(&FIR_encoder, 300, 30);
encoder_left=FIR_apply(&FIR_encoder, encoder1);
encoder_right=FIR_apply(&FIR_encoder, encoder2);
速度闭环
在处理了编码器的读值之后,就应该来调试我们的速度闭环了,在电机的速度闭环上,我这里给出两个方案:1.传统的增量式PI速度闭环 2.ADRC速度闭环
两个方法的优劣
对与新手来说,我更加推荐PID增量式,理由是好调参,资料更为全面,而且花不了很多时间就能得到一个不错的闭环效果。
而ADRC闭环需要调整的参数更多,但理论上效果是更加好的。
增量式PID
这里我给出增量式pid的代码,并使用前馈控制使电机响应更快
/*
* @brief:前馈控制
* 控制周期:0.002
* 设置转动惯量和摩擦系数均为理想状态
*/
float T = 0.002;//ms
float PreFeedback(int Input,int Last_Input)
{
float Output;
Output = (Input - Last_Input)/T + Input;
return Output;
}
void PID_Parameter_Init(PID *sptr)
{
sptr->SumError = 0;
sptr->LastError = 0;
sptr->PrevError = 0;
sptr->LastData = 0;
}
int32 PID_Increase(PID *sptr, float *PID, int32 NowData, int32 Point)
{
//当前误差,定义为寄存器变量,只能用于整型和字符型变量,提高运算速度
int32 iError, //当前误差
Increase/*,Last_Increase*/; //最后得出的实际增量
iError = Point - NowData; // 计算当前误差
Increase = PID[KP] * (iError - sptr->LastError)
+ PID[KI] * iError
+ PID[KD] * (iError - 2 * sptr->LastError + sptr->PrevError);
sptr->PrevError = sptr->LastError; // 更新前次误差
sptr->LastError = iError; // 更新上次误差
sptr->LastData = NowData; // 更新上次数据
Last_Increase = Increase;
Increase = PreFeedback(Increase,Last_Increase);//使用前馈调节加快响应
return Increase; // 返回增量
}
ADRC
这里是学习ADRC代码和视频的地方,我对于ADRC的理解基本也是来自这里
以下是ADRC的调用部分
void LADRC_Loop(LADRC_NUM *LADRC_TYPE1,float* Expect,float* RealTimeOut)
{
float Expect_Value = *Expect;
float Measure = *RealTimeOut;
LADRC_TD(LADRC_TYPE1,Expect_Value);
LADRC_ESO(LADRC_TYPE1,Measure);
LADRC_LF(LADRC_TYPE1);
}
陀螺仪的读值与滤波
零飘处理
由于陀螺仪在静态时会有零点漂移,故在初始化的时候我们需要对陀螺仪进行零飘处理
void offset_init() /陀螺仪零飘初始化,建议放在各外设初始化之前,因为太耗时
{
for (uint16_t i = 0; i < LOOP_NUM; i++)
{
imu660ra_get_gyro();
Gyro_offset_z+=icm20602_gyro_z;
DELAY_MS(2);
}
Gyro_offset_z/=LOOP_NUM;
}
我使用的陀螺仪是IMU660RA,这里读值也是直接调用逐飞的库,由于我们只用陀螺仪辅助转向,所以这里只取z轴(偏航角)的读数
imu660ra_get_gyro();
Gyro.z = (imu660ra_gyro_z-Gyro_offset_z)/16.40f;
IIR滤波
接下来便是对陀螺仪进行滤波处理,这里我使用的是IIR滤波。
void IIR_Init(filter2p_t *filter, float sample_freq, float cutoff_freq)
{
filter->sample_freq = sample_freq;
filter->cutoff_freq = cutoff_freq;
float fr = sample_freq/cutoff_freq;
float ohm = tanf(PI/fr);
float c = 1.0f+2.0f*cosf(PI/4.0f)*ohm + ohm*ohm;
filter->b0 = ohm*ohm/c;
filter->b1 = 2.0f*filter->b0;
filter->b2 = filter->b0;
filter->a1 = 2.0f*(ohm*ohm-1.0f)/c;
filter->a2 = (1.0f-2.0f*cosf(PI/4.0f)*ohm+ohm*ohm)/c;
}
/*需要滤波的信号sample 上次的输出信号_output*/
float IIR_apply(filter2p_t *filter, float sample)
{
float temp0 = sample - filter->temp1 * filter->a1 - filter->temp2 * filter->a2;
filter->oupt = temp0 * filter->b0 + filter->temp1 * filter->b1 + filter->temp2 * filter->b2;
filter->temp2 = filter->temp1;
filter->temp1 = temp0;
return filter->oupt;
}
IIR_Init(&IIR_gyro, 500, 30);
Gyro.z=IIR_apply(&IIR_gyro, Gyro.z);
转向环
转向环这里可以直接使用模糊pid,模糊pid对不同曲率的弯道适应能力相比较于普通的位置式pid强,但对于c车来说,我还是建议2.8m以上再使用模糊pid。
普通位置式
int32 PID_Realize(PID *sptr, float *PID, int32 NowData, int32 Point)//传统位置式
{
//当前误差,定义为寄存器变量,只能用于整型和字符型变量,提高运算速度
int32 iError, // 当前误差
Realize; // 最后得出的实际增量
iError = Point - NowData; // 计算当前误差
sptr->SumError += PID[KI] * iError; // 误差积分
if (sptr->SumError >= PID[KT])
{
sptr->SumError = PID[KT];
}
else if (sptr->SumError <= -PID[KT])
{
sptr->SumError = -PID[KT];
}
Realize = PID[KP] * iError
+ sptr->SumError
+ PID[KD] * (iError - sptr->LastError);
sptr->PrevError = sptr->LastError; // 更新前次误差
sptr->LastError = iError; // 更新上次误差
sptr->LastData = NowData; // 更新上次数据
return Realize; // 返回实际值
}
模糊pid
int32 PID_Fuzzy(PID *sptr, float *PID, int32 NowData, int32 Point)//模糊pid
{
//当前误差,定义为寄存器变量,只能用于整型和字符型变量,提高运算速度
int32 iError, // 当前误差
Fuzzy_U; // 最后得出的实际模糊增益
iError = Point - NowData; // 计算当前误差
sptr->SumError += PID[KI] * iError; // 误差积分
Fuzzy_U = (PID[KP] * Fuzzy( (float)iError,(float)(iError - sptr->LastError) ) ) * iError
+ PID[KD] * Fuzzy( (float)iError,(float)(iError - sptr->LastError) )*(iError - sptr->LastError);
sptr->PrevError = sptr->LastError; // 更新前次误差
sptr->LastError = iError; // 更新上次误差
sptr->LastData = NowData; // 更新上次数据
return Fuzzy_U; // 返回实际值
}
const int rule[7][7]=
{
{ 6 , 5 , 4 , 3 , 2 , 1 , 0},//0
{ 5 , 4 , 3 , 2 , 1 , 0 , 1},//1
{ 4 , 3 , 2 , 1 , 0 , 1 , 2},//2
{ 3 , 2 , 1 , 0 , 1 , 2 , 3},//3
{ 2 , 1 , 0 , 1 , 2 , 3 , 4},//4
{ 1 , 0 , 1 , 2 , 3 , 4 , 5},//5
{ 0 , 1 , 2 , 3 , 4 , 5 , 6},//6
};//模糊规则表,不改
float Fuzzy(float E,float EC)
{
/*输入量P语言值特征点*/
float EFF[7]={-75,-50,-25,0,25,50,75};//要改,最大偏差值分三段
/*输入量D语言值特征点*/
float DFF[7]={-15,-10,-5,0,5,10,15};
/*输出量U语言值特征点(根据赛道类型选择不同的输出值)*/
float UFF[7]={0,1,2,3,4,5,6};
float U=0; /*偏差,偏差微分以及输出值的精确量*/
float PF[2]={0},DF[2]={0},UF[4]={0};
/*偏差,偏差微分以及输出值的隶属度*/
int Pn=0,Dn=0,Un[4]={0};
float t1=0,t2=0,t3=0,t4=0,temp1=0,temp2=0;
/*隶属度的确定*/
/*根据PD的指定语言值获得有效隶属度*/
if(E>EFF[0] && E<EFF[6])
{
if(E<=EFF[1])
{
Pn=-2;
PF[0]=(EFF[1]-E)/(EFF[1]-EFF[0]);
}
else if(E<=EFF[2])
{
Pn=-1;
PF[0]=(EFF[2]-E)/(EFF[2]-EFF[1]);
}
else if(E<=EFF[3])
{
Pn=0;
PF[0]=(EFF[3]-E)/(EFF[3]-EFF[2]);
}
else if(E<=EFF[4])
{
Pn=1;
PF[0]=(EFF[4]-E)/(EFF[4]-EFF[3]);
}
else if(E<=EFF[5])
{
Pn=2;
PF[0]=(EFF[5]-E)/(EFF[5]-EFF[4]);
}
else if(E<=EFF[6])
{
Pn=3;
PF[0]=(EFF[6]-E)/(EFF[6]-EFF[5]);
}
}
else if(E<=EFF[0])
{
Pn=-2;
PF[0]=1;
}
else if(E>=EFF[6])
{
Pn=3;
PF[0]=0;
}
PF[1]=1-PF[0];
//判断D的隶属度
if(EC>DFF[0]&&EC<DFF[6])
{
if(EC<=DFF[1])
{
Dn=-2;
DF[0]=(DFF[1]-EC)/(DFF[1]-DFF[0]);
}
else if(EC<=DFF[2])
{
Dn=-1;
DF[0]=(DFF[2]-EC)/(DFF[2]-DFF[1]);
}
else if(EC<=DFF[3])
{
Dn=0;
DF[0]=(DFF[3]-EC)/(DFF[3]-DFF[2]);
}
else if(EC<=DFF[4])
{
Dn=1;
DF[0]=(DFF[4]-EC)/(DFF[4]-DFF[3]);
}
else if(EC<=DFF[5])
{
Dn=2;
DF[0]=(DFF[5]-EC)/(DFF[5]-DFF[4]);
}
else if(EC<=DFF[6])
{
Dn=3;
DF[0]=(DFF[6]-EC)/(DFF[6]-DFF[5]);
}
}
//不在给定的区间内
else if (EC<=DFF[0])
{
Dn=-2;
DF[0]=1;
}
else if(EC>=DFF[6])
{
Dn=3;
DF[0]=0;
}
DF[1]=1-DF[0];
/*使用误差范围优化后的规则表rule[7][7]*/
/*输出值使用13个隶属函数,中心值由UFF[7]指定*/
/*一般都是四个规则有效*/
Un[0]=rule[Pn+2][Dn+2];
Un[1]=rule[Pn+3][Dn+2];
Un[2]=rule[Pn+2][Dn+3];
Un[3]=rule[Pn+3][Dn+3];
if(PF[0]<=DF[0]) //求小
UF[0]=PF[0];
else
UF[0]=DF[0];
if(PF[1]<=DF[0])
UF[1]=PF[1];
else
UF[1]=DF[0];
if(PF[0]<=DF[1])
UF[2]=PF[0];
else
UF[2]=DF[1];
if(PF[1]<=DF[1])
UF[3]=PF[1];
else
UF[3]=DF[1];
/*同隶属函数输出语言值求大*/
if(Un[0]==Un[1])
{
if(UF[0]>UF[1])
UF[1]=0;
else
UF[0]=0;
}
if(Un[0]==Un[2])
{
if(UF[0]>UF[2])
UF[2]=0;
else
UF[0]=0;
}
if(Un[0]==Un[3])
{
if(UF[0]>UF[3])
UF[3]=0;
else
UF[0]=0;
}
if(Un[1]==Un[2])
{
if(UF[1]>UF[2])
UF[2]=0;
else
UF[1]=0;
}
if(Un[1]==Un[3])
{
if(UF[1]>UF[3])
UF[3]=0;
else
UF[1]=0;
}
if(Un[2]==Un[3])
{
if(UF[2]>UF[3])
UF[3]=0;
else
UF[2]=0;
}
t1=UF[0]*UFF[Un[0]];
t2=UF[1]*UFF[Un[1]];
t3=UF[2]*UFF[Un[2]];
t4=UF[3]*UFF[Un[3]];
temp1=t1+t2+t3+t4;
temp2=UF[0]+UF[1]+UF[2]+UF[3];//模糊量输出
U=temp1/temp2;
return U;
}
阿克曼转向模型
在小车后轮差速的策略上,我选择了阿克曼转向结构,如此转向方案可以让小车在弯道更丝滑
以下是阿克曼转向参考代码
void AKM_Analysis(float velocity , int32 angle)
{
//C车模数据
float Car_L_ = 205; //长(mm)
float Car_T_ = 153; //宽(mm)
float K_Turn_;
float Rad;
angle = LIMIT(angle, -70, 70);
Rad = DEGTORAD(angle);
K_Turn_ = FastTan(Rad) * Car_T_/2/Car_L_;
K_Turn_ = LIMIT(K_Turn_,-5,5);
Left_Speed.Target = velocity * (1+K_Turn_);
Right_Speed.Target = velocity * (1-K_Turn_);
}
void Servo_Analysis(int32 angle)
{
angle = LIMIT(angle, -70, 70);
Servo_Attribute.Servo_Target = Servo_Attribute.Servo_Mid - angle * 2.33;//这里角度系数是由舵机最大范围除以最大转向角度算出来的
Servo_Attribute.Servo_Target = LIMIT(Servo_Attribute.Servo_Target, Servo_Attribute.Servo_Min, Servo_Attribute.Servo_Max);
Servo_set(Servo_Attribute.Servo_Target);
}
控制总章程
以下代码便是我车辆控制转向环的总体代码,由于速度闭环我设置为更高的频率,就不放在转向环里。
void Cascade_Control(int mode)
{
//1.先由外环解析偏差,由于用到两种传感器,先选择偏差来源
if(Control_flag.Direct_flag)
{
switch(mode)
{
case Camera_Tracking:Speed_Control.Direct_Speed = PID_Fuzzy(&Direct_CAMERA_PID, Direct_Camera,img_attribute.Center_Deviation , SteerAttributeExceptMid); break;//摄像头获取偏差
case Induct_Tracking:Speed_Control.Direct_Speed = -PID_Realize(&Direct_Induct_PID, Direct_Induct, Induct_Attribute.Center_Deviation , SteerAttributeExceptMid);break;//电磁获取偏差
default:break;
}
}
//2.根据陀螺仪解析姿态
if(Control_flag.Turn_flag)
{
Speed_Control.Turn_Speed = -PID_Realize(&Turn_CAMERA_PID, Turn, Gyro.z , Speed_Control.Direct_Speed);
}
//3.由小车运动学模型解析左右轮差速
Servo_Analysis(Speed_Control.Turn_Speed);
Speed_Change(img_attribute.Gearshift);
AKM_Analysis(Speed_Control.Target_Speed, Speed_Control.Turn_Speed);
}