智能车四轮控制思路分享

甩葱雪樱

已于 2024-05-07 22:54:21 修改

阅读量961

点赞数 9

文章标签： c语言开源算法

于 2024-05-07 20:23:20 首次发布

本文链接：https://blog.csdn.net/zzlwhile/article/details/138541950

版权

机械结构

四轮车的重心尽量靠近中心位置，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);
}