智能车F车模控制思路分享--十八届摄像头三轮组国一

C18607

已于 2023-09-20 12:45:05 修改

阅读量2.3k

点赞数 14

分类专栏：智能车文章标签：单片机

于 2023-09-20 12:32:47 首次发布

本文链接：https://blog.csdn.net/C18607/article/details/133071225

版权

智能车专栏收录该内容

1 篇文章 0 订阅

订阅专栏

机械结构

在保持原有车模电机的底盘不变的基础上，将电池后置并尽可能贴近地面，使车模的重心后置，使得在弯道处能有更好的转向性能。

速度控制思路

数据处理

调用开源库中的编码器读取函数获取速度信息，将获取到的速度信息进行一阶互补滤波

    Measure_Speedr_least=-encoder_get_count(TIM6_ENCOEDER);

    encoder_clear_count(TIM6_ENCOEDER);

    Measure_Speedz_least=encoder_get_count(TIM2_ENCOEDER);

    encoder_clear_count(TIM2_ENCOEDER);

    //编码器一阶滤波
    Measure_Speedz *= 0.2;
    Measure_Speedz += 0.8*Measure_Speedz_least;

    Measure_Speedr *= 0.2;
    Measure_Speedr += 0.8*Measure_Speedr_least;

速度闭环

在速度控制使用了单闭环的思路，即将两轮的速度和作为控制量，从而能更好的控制车模的整体速度，在速度环上采用LADC自抗扰控制。

float r_LADRC   = 100,//快速跟踪因子
      h_LADRC   = 0.002;//滤波因子,系统调用步长
float b0_LADRC  = 250,//系统系数
      wc_LADRC  = 50,//delta为fal（e，alpha，delta）函数的线性区间宽度
      w0_LADRC  = 200;//扩张状态观测器反馈增益1

 /**
  * 函数说明 LADRC初始参考值
  * 		WangShun于2022-07-03创建
  */	
const float LADRC_Unit[5][5]=
{
	{0.005,20,100,400,0.5},
	{0.001,20,33,133,8},
	{0.005,100,20,80,0.5},
	{0.005,100,14,57,0.5},
	{0.005,100,50,10,1}
};
/**
  * 函数说明：LADRC初始化
  * 		WangShun于2022-07-03创建
  */	
void LADRC_Init(LADRC_NUM *LADRC_TYPE1)
{
	LADRC_TYPE1->h= h_LADRC; //定时时间及时间步长
    LADRC_TYPE1->r =r_LADRC; //跟踪速度参数
    LADRC_TYPE1->wc=wc_LADRC; //观测器带宽
    LADRC_TYPE1->w0=w0_LADRC; //状态误差反馈率带宽
	LADRC_TYPE1->b0=b0_LADRC; //系统参数
}
/**
  * 函数说明：LADRC缺省
  * 		WangShun于2022-07-03创建
  */
void LADRC_REST(LADRC_NUM *LADRC_TYPE1)
{
	LADRC_TYPE1->z1= 0; //定时时间及时间步长
    LADRC_TYPE1->z2 =0; //跟踪速度参数
    LADRC_TYPE1->z3=0; //观测器带宽

}
/**
  * 函数名：void ADRC_TD(LADRC_NUM *LADRC_TYPE1,float Expect)
  * 函数说明：LADRC跟踪微分部分
  * @param[in]	入口参数，期望值Expect(v0)输出值v1,v2
  * @par 修改日志
  * 		WangShun于2022-05-28创建
  */
void LADRC_TD(LADRC_NUM *LADRC_TYPE1,float Expect)
{
    float fh= -LADRC_TYPE1->r*LADRC_TYPE1->r*(LADRC_TYPE1->v1-Expect)-2*LADRC_TYPE1->r*LADRC_TYPE1->v2;
    LADRC_TYPE1->v1+=LADRC_TYPE1->v2*LADRC_TYPE1->h;
    LADRC_TYPE1->v2+=fh*LADRC_TYPE1->h;
}
/**
  * 函数名：LADRC_ESO(LADRC_NUM *LADRC_TYPE1,float FeedBack)
  * 函数说明：LADRC线性状态观测器
  * @param[in]
  * @par 修改日志
  * 		WangShun于2022-07-03创建
  */
void LADRC_ESO(LADRC_NUM *LADRC_TYPE1,float FeedBack)
{
    float Beita_01=3*LADRC_TYPE1->w0;
    float Beita_02=3*LADRC_TYPE1->w0*LADRC_TYPE1->w0;
    float Beita_03=LADRC_TYPE1->w0*LADRC_TYPE1->w0*LADRC_TYPE1->w0;

    float e= LADRC_TYPE1->z1-FeedBack;
    LADRC_TYPE1->z1+= (LADRC_TYPE1->z2 - Beita_01*e)*LADRC_TYPE1->h;
    LADRC_TYPE1->z2+= (LADRC_TYPE1->z3 - Beita_02*e + LADRC_TYPE1->b0*LADRC_TYPE1->u)*LADRC_TYPE1->h;
    LADRC_TYPE1->z3+=-Beita_03*e*LADRC_TYPE1->h;
}
int decelerate_max = 8000;
int increase_max = 4000;
/**
   *@Brief  LADRC_LSEF
   *@Date   线性控制率
			WangShun于2022-07-03创建
   */
void LADRC_LF(LADRC_NUM *LADRC_TYPE1)
{
    float Kp=LADRC_TYPE1->wc*LADRC_TYPE1->wc;
    float Kd=2*LADRC_TYPE1->wc;
	/**
       *@Brief  按自抗扰入门书上kd = 2wc
       *@Before Kd=3*LADRC_TYPE1->wc;
       *@Now    Kd=2*LADRC_TYPE1->wc;
       *@WangShun  2022-04-27  注释
       */
    float e1=LADRC_TYPE1->v1-LADRC_TYPE1->z1;
    float e2=LADRC_TYPE1->v2-LADRC_TYPE1->z2;
    float u0=Kp*e1+Kd*e2;
    LADRC_TYPE1->u=(u0-LADRC_TYPE1->z3)/LADRC_TYPE1->b0;
	if(LADRC_TYPE1->u>increase_max)
		LADRC_TYPE1->u=increase_max;
	else if(LADRC_TYPE1->u<-decelerate_max)
		LADRC_TYPE1->u=-decelerate_max;
}
/**
  * LADRC控制函数 .
  * 将其置于任务循环中即可
  * @par 其它
  * @par 修改日志
  * @WangShun  2022-07-03  注释
  */
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 Power_control()
{
    if(Gearshift>=3)
    {
      increase_max = Speed_Control*100;
    }
    else
    {
      increase_max = 4000;
    }
}

转向控制

数据采集和处理

陀螺仪使用icm20602六轴陀螺仪

通过SPI通信协议获取车模的X,Y,Z轴的角速度以及加速度信息

    Get_Angle();
    get_gyro_z = (int32)(icm20602_gyro_z/16.4384+0.5f);

转向闭环

经过多次调试后，我们发现传统pd闭环的稳定性较差，在面对不同的弯道的适应性不好。最后我们对PD参数进行了一维模糊，同时加入陀螺仪的z轴角速度作为抑制参数，进一步的提高了车模运行的稳定性

//模糊PID控制实现函数
int FuzzyPIDcontroller(float e_max, float e_min, float ec_max, float ec_min, float kp_max, float kp_min, float error, float error_c,float ki_max,float ki_min,float kd_max,float kd_min,float error_pre,float error_ppre)
{
    qerror = Quantization(e_max, e_min, error);    //将 误差 error 映射到论域中
    //qerror_c = Quantization(ec_max, ec_min, error_c);     //将误差变化 error_c 映射到论域中
    Get_grad_membership(qerror);  //计算误差 error 和误差变化 error_c 的隶属度
    GetSumGrad();   //计算输出增量 △kp、△ki、△kd 的总隶属度
    GetOUT();       // 计算输出增量 △kp、△ki、△kd 对应论域值
    detail_kp = Inverse_quantization(kp_max, kp_min, qdetail_kp);    //去模糊化得到增量 △kp
    detail_ki = Inverse_quantization(ki_max, ki_min, qdetail_ki);    //去模糊化得到增量 △ki
    detail_kd = Inverse_quantization(kd_max, kd_min, qdetail_kd);    //去模糊化得到增量 △kd
    qdetail_kd = 0;
    qdetail_ki = 0;
    qdetail_kp = 0;

    kp = kp + detail_kp;    //得到最终的 kp 值
    ki = ki + detail_ki;    //得到最终的 ki 值
    kd = kd + detail_kd;    //得到最终的 kd 值
    if (kp < 0)
        kp = 0;
    if (ki < 0)
        ki = 0;
    if (kd < 0)
        kd = 0;
    detail_kp = 0;
    detail_ki = 0;
        detail_kd = 0;
        int output = kp*(error) + kd * (error - error_pre );    //计算最终的输出
    return output;
}


///区间映射函数
float Quantization(float maximum,float minimum,float x)
{
    float qvalues= 6.0 *(abs(x)-minimum)/(maximum - minimum)-3;
    return qvalues;
}

//输入e与de/dt隶属度计算函数
void Get_grad_membership(float error)
{
    if (error > e_membership_values[0] && error < e_membership_values[6])
    {
        for (int i = 0; i < num_area - 2; i++)
        {
            if (error >= e_membership_values[i] && error <= e_membership_values[i + 1])
            {
                e_gradmembership[0] = -(error - e_membership_values[i + 1]) / (e_membership_values[i + 1] - e_membership_values[i]);
                e_gradmembership[1] = 1+(error - e_membership_values[i + 1]) / (e_membership_values[i + 1] - e_membership_values[i]);
                e_grad_index[0] = i;
                e_grad_index[1] = i + 1;
                break;
            }
        }
    }
    else
    {
        if (error <= e_membership_values[0])
        {
            e_gradmembership[0] = 1;
            e_gradmembership[1] = 0;
            e_grad_index[0] = 0;
            e_grad_index[1] = -1;
        }
        else if (error >= e_membership_values[6])
        {
            e_gradmembership[0] = 1;
            e_gradmembership[1] = 0;
            e_grad_index[0] = 6;
            e_grad_index[1] = -1;
        }
    }
}

// 获取输出增量kp,ki,kd的总隶属度
void GetSumGrad()
{
    int  Kp_rule_list[7] =  {-3,NM,NS,ZO,PS,PM,PB};        //kp规则表


    int  Ki_rule_list[7] =  {PB,PM,PS,ZO,NS,NM,-3};    //ki规则表


    int  Kd_rule_list[7] =  {PB,PM,PS,ZO,NS,NM,-3};    //kd规则表


    // 初始化 Kp、Ki、Kd 总的隶属度值为 0
    for (int i = 0; i <= num_area - 1; i++)
    {
        KpgradSums[i] = 0;
        KigradSums[i] = 0;
        KdgradSums[i] = 0;
    }

        for (int i = 0; i < 2; i++)
     {
                if (e_grad_index[i] == -1)
                {
                        continue;
            }
                            int indexKp = Kp_rule_list[e_grad_index[i]] + 3;
                            int indexKi = Ki_rule_list[e_grad_index[i]] + 3;
                            int indexKd = Kd_rule_list[e_grad_index[i]] + 3;
                            KpgradSums[indexKp] = e_gradmembership[i];
                            KigradSums[indexKi] = e_gradmembership[i];
                            KdgradSums[indexKd] = e_gradmembership[i];
                }
}

// 计算输出增量kp,kd,ki对应论域值
void GetOUT()
{
    for (int i = 0; i < num_area - 1; i++)
    {
        qdetail_kp += kp_menbership_values[i] * KpgradSums[i];
        qdetail_ki += ki_menbership_values[i] * KigradSums[i];
        qdetail_kd += kd_menbership_values[i] * KdgradSums[i];
    }
}

//反区间映射函数
float Inverse_quantization(float maximum, float minimum, float qvalues)
{
    float x = (maximum - minimum) *(qvalues + 3)/6 + minimum;
    return x;
}


int Rotary_compensate;
int Garge_compensate;
int Garge_compensate_ki;
extern int trackStage;
int Fuzzy_Turning_PID(float actual, float set)
{
   error_Turning = set - actual;
   error_c_Turning = error_Turning - error_pre_Turning;
   FuzzyPID();    //初始化PID参数
   err_Turning = FuzzyPID_Gyro_controller(e_max_Turning,  e_min_Turning,  ec_max_Turning,  ec_min_Turning,  kp_max_Turning*10,  kp_min_Turning*10,  error_Turning,  error_c_Turning,  ki_max_Turning*10,  ki_min_Turning*10, kd_max_Turning*10, kd_min_Turning*10,error_pre_Turning  , error_ppre_Turning);  //模糊PID控制实现函数
   error_pre_Turning = error_Turning;
   error_ppre_Turning = error_pre_Turning;
   Fuzzy_Turning_Pwm = err_Turning;
   return  Fuzzy_Turning_Pwm;

    }

因为转向控制中没用使用I这一项参数，所以我们将i参数替换为陀螺仪z轴角速度的抑制系数，同时对I参量也进行一维模糊。

在模糊规则中模糊表PI参数的模糊表的选择尤为重要，我们将PI参数和摄像头获取的偏差联系起来，P参数的大小与偏差成正比关系，I参数的大小与偏差成反比关系，即摄像头偏差越大，P参数越大，I参数越小，即在转弯时削弱陀螺仪的抑制作用，以获得更快速的弯道响应速度。

转向环和速度环

得到转向环和速度环的数据后，我们将两个环的参数相加作为电机的PWM输出。经过调试，并级控制的响应速度快，同时降低了调参的难度。

但是并级控制由于直接将转向环输出直接用于电机控制，这就使电池电压的稳定性对小车运行的稳定性影响极大。在不同的电池电压下，车模的运行状态会截然不同，为了解决这一问题，我们在电路中加入了电池电压检测电路，根据电池电压对电机输出的PWM参数进行归一化处理，从而保证小车能够在不同电池电压下稳定运行。

    LADRC_Loop(&Motor_LADRC,Target_Speed,Measure_speed);
    Normalization_Left_Pwm = Battery_normalization(Motor_LADRC.u-Turn_pwm);
    Normalization_Right_Pwm = Battery_normalization(Motor_LADRC.u+Turn_pwm);
    Speed_Set(Normalization_Left_Pwm,Normalization_Right_Pwm);

void Voltage_Init()
{
    adc_init(VOLTAGE_PIN,ADC_12BIT);
    int i,j;
       //连续采多次,填满缓冲区
       for(i=0;i<=VOLTAGE_BUFFER_SIZE-1;i++)
       {
           Voltage_Buffer[i]=adc_mean_filter_convert(VOLTAGE_PIN, 4);
       }

       unsigned short temp=0;
       //先对电感数据缓存做一个排序
       for(i=0;i<=VOLTAGE_BUFFER_SIZE-1-1;i++)
       {
           for(j=i+1;j<VOLTAGE_BUFFER_SIZE;j++)
           {
               if(Voltage_Buffer[i]>Voltage_Buffer[j])
               {
                   temp=Voltage_Buffer[i];
                   Voltage_Buffer[i]=Voltage_Buffer[j];
                   Voltage_Buffer[j]=temp;
               }
           }
       }
       //取中位数
       temp=VOLTAGE_BUFFER_SIZE/2;
       Voltage_ADC=Voltage_Buffer[temp];

       Voltage = (float)((Voltage_ADC*3.3)/4096)*6;

       if(Voltage<8.4)
       {
           Low_Battery = 1;
       }
       else
       {
           Low_Battery = 0;
       }
       SPEED_MAX = 10000*(10.4/Voltage);
       if(SPEED_MAX>10000)
       {
           SPEED_MAX = 10000;
       }
}
int Battery_normalization(int PWM)
{
    int normalization_pwm;
    normalization_pwm = PWM * 12.0f/Voltage;
    return normalization_pwm;
}

C18607

关注

14
点赞
踩
110

收藏

觉得还不错? 一键收藏
8
评论
智能车F车模控制思路分享--十八届摄像头三轮组国一

在不同的电池电压下，车模的运行状态会截然不同，为了解决这一问题，我们在电路中加入了电池电压检测电路，根据电池电压对电机输出的PWM参数进行归一化处理，从而保证小车能够在不同电池电压下稳定运行。在模糊规则中模糊表PI参数的模糊表的选择尤为重要，我们将PI参数和摄像头获取的偏差联系起来，P参数的大小与偏差成正比关系，I参数的大小与偏差成反比关系，即摄像头偏差越大，P参数越大，I参数越小，即在转弯时削弱陀螺仪的抑制作用，以获得更快速的弯道响应速度。经过调试，并级控制的响应速度快，同时降低了调参的难度。
复制链接

扫一扫