目录
前言
小编分享了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 3
int 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 = 0
if(last_line_flag != line_flag ) ABCD_count++;
last_line_flag = line_flag;
//每换一题,ABCD_count需要清零
五、总结
代码仅作参考,有误之处多担待。有些代码可能写得累赘多余,如果有更好的方法还请不吝赐教!!!
控制参考方案请查看:2024电赛H题参考方案(+视频演示+核心控制代码)——自动行驶小车
今天是电赛第二天,希望能帮到大家,有误之处望指正!!!