1.多车规则
对于17届智能车的多车编队组来说，想要使得三车有序不相撞，可以通过超声波模块来完成速度闭环保持距离，或是开环控制速度使得三车匀速跑完全程。
超声波水太深，我比较菜，把持不住。受前摄像头四轮队员方案启发，以及再看过紫丁香车队的强大的找灯效果后，我们开始使用找灯方案。
我们的队名：寻光，正好对应了这一种方案。
本文将介绍一种使用灯光来测距完成测算准确距离的方案。
2.方案环境
1.在前车尾部安装一个光源，如图：
光源应当尽量靠近地面，因为在透视图中，我们将图像看做一个二维平面。
2.使用透视变换 ，方便对距离进行真实换算（图为前车挡住三叉入口）
3.尽可能的降低图像增益，使灯灰度值接近255，而白色赛道灰度值尽可能与之差距大。
3.方案思路
1.在原图中划定一个矩形边框，在矩形边框内寻找灯在原图中的坐标
2.根据坐标与逆透视矩阵，算出原图中的坐标在透视图中对应的坐标
3.用简单的两点距离公式，计算车头坐标与灯坐标的距离。
4.代码实现
一、找灯
/***************找灯范围*************************/
light_flag.start_x=188/2-90;
light_flag.end_x=188/2+90;
light_flag.end_y=100-1;
/*************找灯代码**************************/
for (i = 0; i < 120; i++) {
for (j = 0; j < 188; j++) {
if (light_flag.judge) {
if (i >= light_flag.start_y && i <= light_flag.end_y
&& j >= light_flag.start_x && j <= light_flag.end_x) {
if (light_flag.max_gray < CamImage[120 - 1 - i][j]
&& CamImage[120 - 1 - i][j] >= 230) {
light_flag.max_gray = CamImage[120 - 1 - i][j];
light_flag.x = j;
light_flag.y = i;
light_flag.find = 1;
light_flag.judge = 0;
}
}
}
}
}
//CamImage为摄像头灰度图像数组代码以及思路非常简单，就是在一个已定的矩形范围内，从近处开始，寻找较亮的点。
虽然思路简单，但是效果很好，有人可能会考虑到反光问题，经过测试，在实际环境中，远处更容易出现反光，而我们只要遵循以上思路，从近处开始寻找，就不会被反光所影响。
需要注意的是，的遍历可能会耗费时间，所以推荐将这段代码放在大津法或者其他图像处理的循环中。
第一行：灯的灰度值 light_flag.max_gray
第二行：距离 light_flag.Distance
第三行：车头在透视图中x坐标 light_flag.middle_x (坐标原点图像左上角，意味着y越大越近)
第四行：车头在透视图中y坐标 light_flag.middle_y (坐标原点图像左上角，意味着y越大越近)
第五行：灯在透视图中x坐标 light_flag.local_x (坐标原点图像左上角，意味着y越大越近)
第六行：灯在透视图中y坐标 light_flag.local_y (坐标原点图像左上角，意味着y越大越近)
红色方框就是设定的找灯区域 ，红点就是找到的灯。（由于屏幕刷新，手机拍摄效果不好）
找到灯之后，就意味着知道了灯在原图中的x，y坐标，下一步就是只对坐标进行一次浮点运算，计算出其在透视图中的坐标，用于测距。
二、对坐标进行逆透视
计算灯的坐标
先提供一个计算函数
void solve_point(float *mat, float *x, float *y) {
float a1 = mat[0];
float b1 = mat[1];
float c1 = -mat[2];
float a2 = mat[3];
float b2 = mat[4];
float c2 = -mat[5];
*x = (b1 * c2 - b2 * c1) / (b1 * a2 - b2 * a1);
*y = (c1 * a2 - c2 * a1) / (b1 * a2 - b2 * a1);
}
再利用先前博客提到的透视变换矩阵进行坐标计算 ，local_x,local_y就是在透视图中的坐标
float mat[6], solve_x, solve_y;
light_flag.y = 120 - 1 - light_flag.y;
mat[0] = change_un_Mat[2][0] * light_flag.y - change_un_Mat[1][0];
mat[1] = change_un_Mat[2][1] * light_flag.y - change_un_Mat[1][1];
mat[2] = change_un_Mat[2][2] * light_flag.y - change_un_Mat[1][2];
mat[3] = change_un_Mat[2][0] * light_flag.x - change_un_Mat[0][0];
mat[4] = change_un_Mat[2][1] * light_flag.x - change_un_Mat[0][1];
mat[5] = change_un_Mat[2][2] * light_flag.x - change_un_Mat[0][2];
solve_point(mat, &solve_x, &solve_y);
light_flag.local_x = solve_x;
light_flag.local_y = solve_y;注意：第二行中，Y是反过来的，所以要用120（原图高）-1-坐标！！！
计算车头坐标
light_flag.start_y=40;
light_flag.middle_x=((float)(light_flag.start_x+light_flag.end_x)/2);
light_flag.middle_y=120-1-((float)(light_flag.start_y));
float mat[6],solve_x,solve_y;
mat[0]=change_un_Mat[2][0]* light_flag.middle_y -change_un_Mat[1][0];
mat[1]=change_un_Mat[2][1]* light_flag.middle_y -change_un_Mat[1][1];
mat[2]=change_un_Mat[2][2]* light_flag.middle_y -change_un_Mat[1][2];
mat[3]=change_un_Mat[2][0]* light_flag.middle_x -change_un_Mat[0][0];
mat[4]=change_un_Mat[2][1]* light_flag.middle_x -change_un_Mat[0][1];
mat[5]=change_un_Mat[2][2]* light_flag.middle_x -change_un_Mat[0][2];
solve_point(mat,&solve_x,&solve_y);
light_flag.middle_x=solve_x;
light_flag.middle_y=solve_y;第二、三行是在设定原图中车头的坐标。
注意：Y是反过来的，所以要用120（原图高）-1-坐标！！！
还需要注意的是：
这里最终计算的middle_x，y为车头在透视图中坐标，坐标原点在左上角！！！！左上角！！！
这里最终计算的local_x，y为车头在透视图中坐标，坐标原点在左上角！！！！左上角！！！
也就是说，用透视后的图的高减去 y的值越大，距离越远。
我们是100*114（高*长）的透视图，（100-y）后，尤其是车头,y可能或出现负数，那是因为在原图中的坐标透视后不在透视图内，不影响距离计算。
三、距离计算
这里提供了两种距离计算方案
直线距离
直接使用两点距离公式就OK了
light_flag.Distance =sqrt((light_flag.local_x - light_flag.middle_x)* (light_flag.local_x - light_flag.middle_x)+ (light_flag.local_y - light_flag.middle_y)* (light_flag.local_y - light_flag.middle_y));
行距
由于多车编队，前车可能会影响后车的摄像头图像，所以可以采用控制行距的方法。
light_flag.Distance = light_flag.middle_y -light_flag.local_y ;
4.总结
一、在使用时一定要注意坐标轴原点。
二、要调整图像增益，使得灯不会影响大津法二值化图像。
三、在逆透视基础上可以实现距离的精准测算，完全代替超声波。
四、找到灯之后可以进行适量抠图将车从图像中抠去（紫丁香车队方案）。
五、可以在车头也加一个灯，降低扣车难度。