2024电赛H题相关代码及算法——自动行驶小车

目录

前言

一、MPU6050零漂处理

二、MPU6050的Yaw（±180）误差处理

三、PID算法（增量式+位置式）

四、灰度传感器（以8路为例）

1、获取黑线偏差

2、判断ABCD点（有无黑线交点）

五、总结

---

前言

        小编分享了2024电赛H题参考方案（+视频演示＋核心控制代码）——自动行驶小车文章后，于是根据其中问题写了一些可能用到的参考代码，希望能帮助大家，有问题欢迎大家指出。

一、MPU6050零漂处理

        在进行角度、转向控制中，大多采用MPU6050来进行姿态检测，但由于MPU6050的特性，上电运行时三个角度（Pitch、roll、yaw）存在一定的零漂，尤其时yaw角，有时上电就漂了20多度。

        为此提出了一种解决方法就是：在进行MPU6050初始化之后，马上进行初始值确定，通过多次测量求平均值的算法求得新的yaw，后续就以这个值为yaw的初始值，然后再进行运动控制。此方法不一定有效，推荐尝试。

        参考代码如下：

int i = 0;float pitch,roll,yaw;float start_yaw[50];float base_yaw = 0;float process_data(float data[],uint8_t NUM_SAMPLES);while ( mpu_dmp_init()){     delay_ms(20);}//等待初始化完成while(1){     mpu_dmp_get_data(&pitch, &roll, &yaw);    start_yaw[i++] = yaw;    if(i == 50) //采样50次    {         base_yaw = process_data(start_yaw,50);        break;    }    delay_ms(5);}float process_data(float data[],uint8_t NUM_SAMPLES) //均值滤波算法{     // 找到最大值和最小值的索引    int max_index = 0;    int min_index = 0;    for (int i = 1; i < NUM_SAMPLES; i++) {         if (data[i] > data[max_index]) {             max_index = i;        }        if (data[i] < data[min_index]) {             min_index = i;        }    }    // 去除最大值和最小值    float sum = 0;    for (int i = 0; i < NUM_SAMPLES; i++) {         if (i != max_index && i != min_index) {             sum += data[i];        }    }    // 计算剩余数据的平均值（除去最大值和最小值后剩余NUM_SAMPLES-2个）    float average = sum / (NUM_SAMPLES - 2);    return average;}

二、MPU6050的Yaw（-180~+180）运用处理

        用过MPU6050的朋友都知道，经过DMP处理得到的三个角度的范围（如下代码），他们并不是连续的，在控制转向环等时，需要进行特殊处理后才可得到准确的角度误差。

//得到dmp处理后的数据(注意,本函数需要比较多堆栈,局部变量有点多)//pitch:俯仰角 精度:0.1°   范围:-90.0° <---> +90.0°//roll:横滚角  精度:0.1°   范围:-180.0°<---> +180.0°//yaw:航向角   精度:0.1°   范围:-180.0°<---> +180.0°//返回值:0,正常//    其他,失败u8 mpu_dmp_get_data(float *pitch,float *roll,float *yaw);

         处理方案一：根据特定的角度进行加减360°，但是需要判断什么时候该减，什么时候该加，有点麻烦。比如，小车当前角度为150°，如果要转到180°，这个时候肯定需要有大于180的值，但他会变成负值，所以这是需要将得到的负角度加上360°。

#define limit_180_Z(n) ((n>0)?(n-360):n)#define limit_180_F(n) ((n<0)?(n+360):n)

        处理方案二：通过算法处理，无论陀螺仪测得的数据为多少，直接将其转化为与目标角度的偏差，直接得到误差，加入闭环控制算法即可。

#define My_abs(n) (n>0?n:(-n))//输入角度目标值，和当前读出yaw角度即可，自动算出最小偏差。float Yaw_error(float Target, float Now) {     float error;    if (Target >= 0) 	{         if (Now <= 0) 		{             if (My_abs(Now) < (180 - Target)) 			{                 error = My_abs(Now) + Target;            } 			else 			{                 error = -(180 - Target) - (180 - My_abs(Now));            }        } 		else 		{             if (Now > 0) 			{                 error = Target - Now;            }        }    } 	else if (Target < 0)	 {         if (Now > 0) 		{             if (Now > Target + 180)			{                 error = (180 - Now) + (180 - My_abs(Target));            }			 else if (Now < Target + 180) 			{                 error = -(My_abs(Target) + Now);            }        } 		else if (Now < 0) 		{             error = -(My_abs(Target) - My_abs(Now));        }    }         return error;}

三、PID算法（增量式+位置式）

        但凡涉及到控制，一般选用PID控制算法来实现，其通过模拟PID离散化处理而来。在进行小车的速度环、转向环、巡线等控制中普遍使用。

        下面就以C语言来实现其控制代码：

/** * @brief      增量式 PID and 位置式 PID *  */typedef enum{     POSITION_PID = 0,    DELTA_PID    = 1,}PID_mode;#define XIAN_FU 7000               //积分限幅#define LIMIT(x,min,max) (x)=(((x)<=(min))?(min):(((x)>=(max))?(max):(x)))  //限幅定义int pid_control(PID_mode mode,float get_speed, float set_Target,float P,float I,float D){ 	static int Integral,Last_error,LLast_Error;    int Error,pid_out;          Error = set_Target - get_speed;        if (mode == POSITION_PID) // position PID  {     Integral +=  Error;     LIMIT(Integral,-XIAN_FU,XIAN_FU);//积分限幅    pid_out = P*Error + I*Integral + D*(Error - Last_error);        Last_error = Error;     }  else if (mode == DELTA_PID) // delta PID  {     pid_out += P* (Error - Last_error) + I * Error + D * (Error - 2*Last_error + LLast_Error);          Last_error = Error;    LLast_Error = Last_error;       }    return pid_out;}

        如果这两种控制算法还不达标的话，可以对其进行控制算法改进：

1、积分分离；

2、变速积分；

3、微分先行；

4、。。。。。。 

四、灰度传感器（以8路为例）

1、获取黑线偏差

        在进行循迹或巡线的控制过程中，若我们需要采用闭环控制（PID控制），则需要获得黑线和小车中心线的偏差，然后以减小此偏差为目的去控制小车，从而实现稳定的循迹或巡线功能。而如果采取摄像头如（CCD、OpenMV等）获取次偏差的话，其获取的偏差数值相对灰度传感器而言，要连续得多，相同控制参数下，用摄像头要平稳得多，但是只要参数调得好，算法使用得精，用灰度传感器效果也是很不错得。

        用灰度传感器获取偏差，就需要我们自行处理数据，相当于对不同得情况进行人为赋值，以此来规定误差，类似于模糊控制。这里以8路灰度传感器为例，以其中间的4、5路为中心，即当4、5路同时识别到，而其他路没有识别到时，就认为其偏差为零。设向右偏为正的偏差，则向左偏为负偏差。当然，需要考虑黑线宽度，传感器各路间隔等因素，情况分得越细越好。

        以下代码只做参考（写得有点垃）：

//以STM32为例,默认扫到黑线为高电平#define Get_Value1 GPIO_ReadInputDataBit(GPIOF,GPIO_Pin_7)#define Get_Value2 GPIO_ReadInputDataBit(GPIOF,GPIO_Pin_6)#define Get_Value3 GPIO_ReadInputDataBit(GPIOF,GPIO_Pin_5)#define Get_Value4 GPIO_ReadInputDataBit(GPIOF,GPIO_Pin_4)#define Get_Value5 GPIO_ReadInputDataBit(GPIOF,GPIO_Pin_3)#define Get_Value6 GPIO_ReadInputDataBit(GPIOF,GPIO_Pin_2)#define Get_Value7 GPIO_ReadInputDataBit(GPIOF,GPIO_Pin_1)#define Get_Value8 GPIO_ReadInputDataBit(GPIOF,GPIO_Pin_0)int Get_Err(void){     int err = 0;    //中间    if(!Get_Value1 && !Get_Value2 && !Get_Value3 && Get_Value4 && Get_Value5 && !Get_Value6 && !Get_Value7 && !Get_Value8) err = 0;    //右边    else if(!Get_Value1 && !Get_Value2 && !Get_Value3 && !Get_Value4 && Get_Value5 && !Get_Value6 && !Get_Value7 && !Get_Value8) err = 1;    else if(!Get_Value1 && !Get_Value2 && !Get_Value3 && !Get_Value4 && Get_Value5 && Get_Value6 && !Get_Value7 && !Get_Value8) err = 2;    else if(!Get_Value1 && !Get_Value2 && !Get_Value3 && !Get_Value4 && !Get_Value5 && Get_Value6 && !Get_Value7 && !Get_Value8) err = 3;    else if(!Get_Value1 && !Get_Value2 && !Get_Value3 && !Get_Value4 && !Get_Value5 && Get_Value6 && Get_Value7 && !Get_Value8) err = 4;    else if(!Get_Value1 && !Get_Value2 && !Get_Value3 && !Get_Value4 && !Get_Value5 && !Get_Value6 && Get_Value7 && !Get_Value8) err = 5;    else if(!Get_Value1 && !Get_Value2 && !Get_Value3 && !Get_Value4 && !Get_Value5 && !Get_Value6 && Get_Value7 && Get_Value8) err = 6;    else if(!Get_Value1 && !Get_Value2 && !Get_Value3 && !Get_Value4 && !Get_Value5 && !Get_Value6 && !Get_Value7 && Get_Value8) err = 7;    //左边    else if(!Get_Value1 && !Get_Value2 && !Get_Value3 && Get_Value4 && !Get_Value5 && !Get_Value6 && !Get_Value7 && !Get_Value8) err = -1;    else if(!Get_Value1 && !Get_Value2 && Get_Value3 && Get_Value4 && !Get_Value5 && !Get_Value6 && !Get_Value7 && !Get_Value8) err = -2;    else if(!Get_Value1 && !Get_Value2 && Get_Value3 && !Get_Value4 && !Get_Value5 && !Get_Value6 && !Get_Value7 && !Get_Value8) err = -3;    else if(!Get_Value1 && Get_Value2 && Get_Value3 && !Get_Value4 && !Get_Value5 && !Get_Value6 && !Get_Value7 && !Get_Value8) err = -4;    else if(!Get_Value1 && Get_Value2 && !Get_Value3 && !Get_Value4 && !Get_Value5 && !Get_Value6 && !Get_Value7 && !Get_Value8) err = -5;    else if(Get_Value1 && Get_Value2 && !Get_Value3 && !Get_Value4 && !Get_Value5 && !Get_Value6 && !Get_Value7 && !Get_Value8) err = -6;    else if(Get_Value1 && !Get_Value2 && !Get_Value3 && !Get_Value4 && !Get_Value5 && !Get_Value6 && !Get_Value7 && !Get_Value8) err = -7;         return err;  }

2、判断ABCD点（有无黑线的交点）

        本次黑线宽度为1.5cm，其宽度较大，所以可以只用灰度传感器便可以判断是否到达该点。

        只要在一定时间内（不能只判断一次，与按键消抖差不多一个理），一次都没有检测到黑线，便可认为丢失黑线了，到了ABCD点处。

//N 自定#define N  3int i,j;//参数类型尽量大一些，防止溢出uint8_t line_flag = 0;//判断是否在线上 0不在，1在void get_ABCD(void) //每隔一定时间调用一次函数，进行判断是否在线上，需要结合N考虑{     if(!Get_Value1 && !Get_Value2 && !Get_Value3 && !Get_Value4 && !Get_Value5 && !Get_Value6 && !Get_Value7 && !Get_Value8)    {         j = 0;        if(++i >= N)        {             line_flag = 0;        }    }    else    {         i = 0;        if(++j >= N)        {             line_flag = 1;        }            }   }

        根据以上代码，可以判断小车经过ABCD这样的点几次了，从而可以判断小车到达ABCD的具体点位。

uint8_t last_line_flag = 0;uint8_t ABCD_count = 0if(last_line_flag != line_flag ) ABCD_count++;last_line_flag = line_flag;//每换一题，ABCD_count需要清零

五、总结

       代码仅作参考，有误之处多担待。有些代码可能写得累赘多余，如果有更好的方法还请不吝赐教！！！

        控制参考方案请查看：2024电赛H题参考方案（+视频演示＋核心控制代码）——自动行驶小车

        今天是电赛第二天，希望能帮到大家，有误之处望指正！！！