智能车比赛常用代码开源_智能车代码

关于智能车比赛里常用的几个算法

本文不探讨算法如何实现的，要学习原理网上资料很多大家可以自己找。只会讲如何使用代码，引用注明出处，禁止商用

1.灰度图像处理类

1.1二值化算法

1.1.1 均值二值化

//-------------------------------------------------------------------------------------------------------------------
// @brief		均值二值化
// @param		*p				图像指针
// @param		row				图像高度或者行数
// @param		col				图像宽度或者列数
// @return	均值阈值
// Sample usage:				mean_binaryzation(image[0],120,120)
//-------------------------------------------------------------------------------------------------------------------
uint8 mean_binaryzation(uint8 *p,uint8 height,uint8 width)
{
	uint32 gray_sum=0;
	uint16 temp=0;
	for(int i=0;i<height;i++)
	{
		for(int j=0;j<width;j++)
		{
			temp = *(p+i*width+j);
			gray_sum += temp;
		}
	}
	return gray_sum/(height*width);
}
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22

*p:就是图像数组的名字。（数组名可以当指针相信大家还是知道的），返回的就是二值化阈值，然后遍历每个图像的像素，该值比较就行。

1.1.2 大津二值化

//-------------------------------------------------------------------------------------------------------------------
// @brief		大津法二值化
// @param		*p				图像指针
// @param		height				图像高度或者行数
// @param		col				图像宽度或者列数
// @return	大津法阈值
// Sample usage:				ostu_binaryzation(image[0],120,120)
//-------------------------------------------------------------------------------------------------------------------
uint8 back_mean_gray[GrayScale] = {0}; //GrayScale就是图像的量化精度，一般就是256
uint8 front_mean_gray[GrayScale] = {0};
uint16 back_count_array[GrayScale]={0};
float back_proportion_array[GrayScale]={0};
uint16 pixel_num = image_height*image_width;//像素数量图像长乘以宽
uint8 ostu_binaryzation(uint8 *p, uint8 height, uint8 width)
{
	uint32 gray_sum = 0;
	uint32 back_graysum = 0;
	uint16 back_pixelsum = 0;
	float class_variance=0;
	float tmp_class_variance=0;
	uint8 threshold=0;
	uint16 tmp_count=0;
	
	for(int i = 0;i<GrayScale;i++)
	{
		back_proportion_array[i] = 0;
		back_count_array[i] = 0;
	}
//	memset(back_proportion_array, 0, sizeof(back_proportion_array));
//	memset(back_proportion_count, 0, sizeof(back_proportion_count)); //C语言内置的将数组置0的程序，但试了一下好像还没上面的方法快
	
	for(int j=0;j<pixel_num;j++)
	{
		gray_sum += *(p+j);
		back_count_array[*(p+j)] += 1;
	}
	
	for(int i=0;i<GrayScale;i++)
	{
		back_graysum += back_count_array[i]*i;
		back_pixelsum += back_count_array[i];
		back_mean_gray[i] = back_graysum/back_pixelsum;
		front_mean_gray[i] = (gray_sum - back_graysum)/(pixel_num - back_pixelsum);
		back_proportion_array[i] = (float)back_pixelsum/pixel_num;
		tmp_class_variance =  back_proportion_array[i]*(1-back_proportion_array[i])*(back_mean_gray[i]-front_mean_gray[i])*(back_mean_gray[i]-front_mean_gray[i]);
		if(tmp_class_variance>class_variance)
		{
			threshold = i;
			class_variance = tmp_class_variance;
		}
	}
	return threshold;
}
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53

用法跟均值是一样的。里面一些常量的含义看注释就行。

1.1.3 sobel算子

//-------------------------------------------------------------------------------------------------------------------
// @brief		sobel算子
// @param		*p									图像指针
// @param		row									像素行数
// @param		col									像素列数
// @return	像素梯度
// Sample usage:			
//-------------------------------------------------------------------------------------------------------------------
int16 sobel(uint8 *p,uint8 row,uint8 col)
{
	int16 Gx,Gy,G;
	if(row==0||col==0||row==image_height||col==image_width)
	{
		return false;

	}
	else
	{
		Gx = myabs(2*((*(p+row*(image_width)+(col-1)))-(*(p+row*(image_width)+(col+1))))+((*(p+(row+1)*(image_width)+(col-1)))-(*(p+(row+1)*(image_width)+(col+1))))+((*(p+(row-1)*(image_width)+(col-1)))-(*(p+(row-1)*(image_width)+(col+1)))));
		Gy = myabs(2*((*(p+(row-1)*(image_width)+col))-(*(p+(row+1)*(image_width)+col)))+((*(p+(row-1)*(image_width)+(col+1)))-(*(p+(row+1)*(image_width)+(col+1))))+((*(p+(row-1)*(image_width)+(col-1)))-(*(p+(row+1)*(image_width)+(col-1)))));
		G = Gx+Gy;
		return G;
	}
}
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24

返回的是每个像素的梯度值，然后每幅图像的边缘一圈像素因为八领域我这里直接返回了0，sobel一般配合Canny算法使用提取边缘，但算法复杂度有点高，我当时的单片机带不动就没有写，大家单片机算力好的话或者能优化优化的话可以试试。

1.1.4 图像腐蚀

//-------------------------------------------------------------------------------------------------------------------
// @brief		腐蚀
// @param		*p									图像指针
// @param		row									像素行数
// @param		col									像素列数
// @return	ture 白点 false 黑点
// Sample usage:			
//-------------------------------------------------------------------------------------------------------------------
bool etch_kernel(uint8 *p,uint8 row,uint8 col,uint8 threshold)
{
	uint8 sum;
	static uint8 max_col = image_width-1;
	static uint8 max_row = image_height-1;
	if(row==0&&col==0)
	{
		sum = kernel(p,row,col,threshold)+kernel(p,row+1,col,threshold)+ kernel(p,row,col+1,threshold)+ kernel(p,row+1,col+1,threshold);
		if(sum == 4)return true;
		else return false;
	}
	else if(row==0&&col==max_col)
	{
		sum = kernel(p,row,col,threshold)+kernel(p,row+1,col,threshold)+ kernel(p,row,col-1,threshold)+ kernel(p,row+1,col-1,threshold);
		if(sum == 4)return true;
		else return false;
	}
	else if(row==max_row&&col==0)
	{
		sum = kernel(p,row,col,threshold)+kernel(p,row-1,col,threshold)+ kernel(p,row,col+1,threshold)+ kernel(p,row-1,col+1,threshold);
		if(sum == 4)return true;
		else return false;
	}
	else if(row==max_row&&col==max_col)
	{
		sum = kernel(p,row,col,threshold)+kernel(p,row-1,col,threshold)+ kernel(p,row,col-1,threshold)+ kernel(p,row-1,col-1,threshold);
		if(sum == 4)return true;
		else return false;
	}
	else if(row==0)
	{
		sum = kernel(p,row,col,threshold)+kernel(p,row,col-1,threshold)+ kernel(p,row,col+1,threshold)+ kernel(p,row+1,col-1,threshold)+ kernel(p,row+1,col+1,threshold)+ kernel(p,row+1,col,threshold);
		if(sum == 6)return true;
		else return false;
	}
	else if(row==max_row)
	{
		sum = kernel(p,row,col,threshold)+kernel(p,row,col-1,threshold)+ kernel(p,row,col+1,threshold)+ kernel(p,row-1,col-1,threshold)+ kernel(p,row-1,col+1,threshold)+ kernel(p,row-1,col,threshold);
		if(sum == 6)return true;
		else return false;
	}
	else if(col==0)
	{
		sum = kernel(p,row+1,col,threshold)+kernel(p,row,col,threshold)+ kernel(p,row-1,col,threshold)+ kernel(p,row-1,col+1,threshold)+ kernel(p,row,col+1,threshold)+ kernel(p,row+1,col+1,threshold);
		if(sum == 6)return true;
		else return false;
	}
	else if(col==max_col)
	{
		sum = kernel(p,row+1,col,threshold)+kernel(p,row,col,threshold)+ kernel(p,row-1,col,threshold)+ kernel(p,row-1,col-1,threshold)+ kernel(p,row,col-1,threshold)+ kernel(p,row+1,col-1,threshold);
		if(sum == 6)return true;
		else return false;
	}
	else
	{
		sum = kernel(p,row+1,col,threshold)+kernel(p,row,col,threshold)+ kernel(p,row-1,col,threshold)+ kernel(p,row-1,col-1,threshold)+ kernel(p,row,col-1,threshold)+ kernel(p,row+1,col-1,threshold)+ kernel(p,row+1,col+1,threshold)+ kernel(p,row,col+1,threshold)+ kernel(p,row-1,col+1,threshold);
		if(sum == 9)return true;
		else return false;
	}
}
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68

这里的代码不全，返回的bool值，然后这里的kernel是一个对比像素与阈值大小的函数，参考如下：

//-------------------------------------------------------------------------------------------------------------------
// @brief		二值判断
// @param		*p					图像指针
// @param		row					像素行数
// @param		col					像素列数
// @param		threshold		阈值
// @return	ture 白点 false 黑点
// Sample usage:				bin_judge(image[0],0,0,threshold)
//-------------------------------------------------------------------------------------------------------------------
bool bin_judge(uint8 *p,uint8 row,uint8 col,uint8 threshold)
{
	if(*(p+row*image_width+col)>threshold)
	{
		return true;
	}
	else return false;
}

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18

就是把 kernel换成 bin_judge就行，或者加一句 #define kernel bin_judge

后面有时间会做一个关于巡线思路的分享，该思路抛弃了算法对阈值的依赖，但我自己尝试了一段时间，虽然适应性还可以，但最后还是有一些问题没有解决，后面分享后也欢迎大家探讨。

2021.12.27 今天先更新到这，后续会继续分享关于控制，姿态结算方面的代码

智能车里的滤波和简单的状态估计算法

一阶互补滤波

//-------------------------------------------------------------------------------------------------------------------
// @brief		一阶互补滤波
//-------------------------------------------------------------------------------------------------------------------
void first_order_com_filter()
{
	//执行前记得更新相关数据
	const static float com_coeffcient =0.01; //对加速度计取值的权重
	const static float com_d_time=0.001;	//注意：dt的取值为滤波器采样时间0.001s,使用互补滤波时，一般取1ms作为中断
   float euler_angle=(atan((float)icm_acc_x/(float)icm_acc_z))*180/3.14;  //角度不对检查此处选择的轴是否正确，与陀螺仪的安装方式相关。
	upright_state.pitch_angle = com_coeffcient * euler_angle+ (1-com_coeffcient) * (upright_state.pitch_angle + 6*icm_gyro_y * com_d_time);//filtering
	//互补滤波，这里角速度选择y轴角速度也与陀螺仪的安装方式有关，以及公式中的加减号也与陀螺仪的安装方式有关，具体了解互补滤波器的原理后自行分析。
}
1
2
3
4
5
6
7
8
9
10
11
12

这里主要将几个轴和数据输入选择对了就可以正常输出角度了。对于直立车我们只需要测量一个俯仰角，那输入的就只有一个轴的欧拉角（对应代码里的euler_angle）和角速度 (对应代码里的icm_gyro_y);
对于几个参数说明一下：
com_coeffcient:对加速度计取值的权重,取值范围0到1。就看你对自己传感器的信任程度，比如计算角度时你对加速度信任程度高就调大一点，对角速度计（陀螺仪）信任度高就调小点，一般市面上的六轴都是角速度精度高，加速度精度不行，所以该参数取值都很小。
com_d_time:程序执行的周期，单位为秒，该段程序应该放进定时器中断中，这个参数就是你执行该程序的时间间隔。

卡尔曼滤波

void kalman_filter()
{
	//卡尔曼参数
	static float Q_angle = 0.012;//如果角度跟随比较慢调大该值
	static float Q_gyro  = 0.05;//如果角度波动比较大调大该值
	static float R_angle = 5;//跟随慢调小，波动大调大。其实五差不多  可以通过调节上面两个参数让波形达到较好的效果
	static float dt      = 0.002;//dt为kalman滤波器采样时间,根据单片机性能取，一般是1-2ms，对应上面的com_d_time
	//下面的参数不要动
	static float Q_bias=0;
	static float K_0, K_1;
	static float PP[2][2] = { { 1, 0 },{ 0, 1 } };

	static float Angle_Final=0; //这个可以动，就是车摸启动的时候角度。其实不给也行，但是这样程序需要一段时间计算，让输出角度收敛于真实角度。
	
	//执行该程序前记得更新相关输入get_data();
	Angle_Final = Angle_Final - Q_bias*dt + 5.8*(icm_gyro_y)*dt;//先验估计，这里的加减号跟传感器安装方式有关怎么都调不好的话试试换号
	
	PP[0][0] = PP[0][0] + Q_angle -(PP[0][1]-PP[1][0])*dt;
	PP[0][1] = PP[0][1] - PP[1][1]*dt;
	PP[1][0] = PP[1][0] - PP[1][1]*dt;
	PP[1][1] = PP[1][0] + Q_gyro;      //计算预测协方差矩阵
	
	float euler_angle=(atan((float)icm_acc_x/(float)icm_acc_z))*180/3.14;  //角度不对检查此处选择的轴是否正确，与陀螺仪的安装方式相关。
	//测量模型方程
	
	K_0 = PP[0][0]/(PP[0][0]+R_angle);
	K_1 = PP[1][0]/(PP[0][0]+R_angle);  //计算卡尔曼增益
	
	Angle_Final = Angle_Final + K_0*(euler_angle-Angle_Final);
	Q_bias = Q_bias + K_1*(euler_angle-Angle_Final); //计算最优估计值
	
	PP[0][0] = PP[0][0] - K_0*PP[0][0];
	PP[0][1] = PP[0][1] - K_0*PP[0][1];
	PP[1][0] = PP[1][0] - K_1*PP[0][0];
	PP[1][0] = PP[1][0] - K_0*PP[0][1];//更新协方差矩阵
	
	upright_state.pitch_angle = Angle_Final;
	upright_state.pitch_angle_gyro = icm_gyro_y;//数据输出	
}

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40

看注释就行了。

2021.12.29 今天先更新到这，后续会继续分享关于控制方面的代码