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.总结
一、在使用时一定要注意坐标轴原点。
二、要调整图像增益,使得灯不会影响大津法二值化图像。
三、在逆透视基础上可以实现距离的精准测算,完全代替超声波。
四、找到灯之后可以进行适量抠图将车从图像中抠去(紫丁香车队方案)。
五、可以在车头也加一个灯,降低扣车难度。