智能车整理日记【七】————三岔
文章目录
- 前言
- 一、入三岔
- 二、在三岔中
- 三、出三岔
- 总结
前言
这一篇记录的是三岔路口的处理方式，给大家提供一种思路。我的正入三岔和弯接三岔用的一套代码，弯接的条件稍微放宽一点。欢迎大家提出意见和修改！ 一些数组和参数定义在前几篇文章有说明，如果觉得这篇文章有帮助就点个赞支持一下吧！
一、入三岔
先看三岔的图像，其中最明显的特征就是红色框出来的v形部分。有些友友可能会觉得左右两边的变化也是特征，但在我看来左右变化很不稳定，要是弯接三岔可能看不到这些特征。所以我的三岔判断主要就是找上边的v型标志。
我找v型的方法是从下往上扫线。先选定一个范围，下图我会选最下边的左边线和右边线之间，从右边往左边遍历，依次往上扫线，当扫到边界时存入数组，这样我就能得到图中绿色的边界数组，接下来就是找v的最低点，即有个点的左右两边都大于它。找到v型最低的点后根据需求拉线即可。
思路：
找到V型标志的最低点
1.正常的直道入三岔，v型的最低点是比较好找的，是一个比较标准的v型。
2.如果是弯接三岔，这个v型会有不同，v的一边会比较陡，一边会比较缓，有点类似“_/”和“\ _”。这个时候可以适当更改一边的条件，先判断是否为弯接，再判断v是否一边陡，一边比较缓。
三岔分为3状态：状态一：入三岔。状态二：在三岔中。 状态三：出三岔。
sancha_y[ ]: v型每个点的y值的数组
sancha_x[ ]: v型每个点的x值的数组
sancha_flag: 三岔标志位
sancha_enter_flag： 入三岔标志位
sancha_top_y： v型最低点的y值
sancha_top_x： v型最低点的x值
//找v型的最低点
if (lefty[0] > 0 && righty[0] > 0)
{
for (x = rightx[0]; x < leftx[0]; x++)
{
for (y = 0; y < 65; y++)
{
if (Pixels[y][x] != 0 && Pixels[y + 1][x] != 0 && Pixels[y + 2][x] == 0)
{
sancha_y[cout] = (byte)(y + 1); //找v型边界
sancha_x[cout] = (byte)(x);
cout++;
break;
}
}
}
if (cout > 30)
{
for (x = 20; x < (byte)(cout - 20); x++)
{
if (sancha_y[x - 1] < down_y && sancha_y[x - 1] <= sancha_y[x] && sancha_y[x + 4] >= sancha_y[x - 1] && abs(sancha_y[x + 2] - sancha_y[x - 1]) < 3 && sancha_y[x - 6] >= sancha_y[x - 1] && abs(sancha_y[x + 4] - sancha_y[x - 1]) < 3 && abs(sancha_y[x - 6] - sancha_y[x - 1]) < 3 && sancha_y[x - 1] <= sancha_y[x + 10] && sancha_y[x] <= sancha_y[x + 13])
{
byte first_flag = 0;
if (sancha_enter_flag == 0 && x > 21 && x < (byte)(cout - 21)) //第一次判三岔的点时要严格点，点周围20范围内必须都比中点大
{
for (int k =(byte)(x - 21); k < x + 20; k++)
{
if (sancha_y[k] < sancha_y[x - 1])
{
first_flag = 1;
break;
}
}
}
if (first_flag == 0)
{
down_y = sancha_y[x - 1];
if (sancha_x[x - 1] >= 125)
{
if (x + 28 < cout && x - 41 > 0 && sancha_y[x + 28] >= sancha_y[x - 1] && sancha_y[x - 1] <= sancha_y[x + 9] && sancha_y[x - 30] > sancha_y[x - 1] && abs(sancha_y[x + 28] - sancha_y[x - 1]) < 7 && abs(sancha_y[x - 30] - sancha_y[x - 1]) < 9 && sancha_y[x - 41] > sancha_y[x - 1] && sancha_y[x - 1] <= 50)
{
sancha_top_y = sancha_y[x - 1];
sancha_top_x = sancha_x[x - 1];
down_y = sancha_y[x - 1];
}
}
else if (sancha_x[x - 1] < 125 && sancha_x[x - 1] > 60)
{
if (x - 25 > 0 && x + 29 < cout && sancha_y[x + 29] > sancha_y[x - 1] && sancha_y[x - 25] > sancha_y[x - 1] && sancha_y[x - 1] <= sancha_y[x + 9] && abs(sancha_y[x + 29] - sancha_y[x - 1]) < 7 && abs(sancha_y[x - 25] - sancha_y[x - 1]) < 7 && sancha_y[x - 1] <= 50)
{
sancha_top_y = sancha_y[x - 1];
sancha_top_x = sancha_x[x - 1];
down_y = sancha_y[x - 1];
}
if (x <= 25)
{
if (x - 15 > 0 && x + 28 < cout && sancha_y[x + 28] >= sancha_y[x - 1] && sancha_y[x - 15] > sancha_y[x - 1] && sancha_y[x - 1] <= sancha_y[x + 9] && abs(sancha_y[x + 28] - sancha_y[x - 1]) < 7 && abs(sancha_y[x - 15] - sancha_y[x - 1]) < 7 && sancha_y[x - 1] <= 50)
{
sancha_top_y = sancha_y[x - 1];
sancha_top_x = sancha_x[x - 1];
down_y = sancha_y[x - 1];
}
}
}
else if (sancha_x[x - 1] <= 60)
{
if (x + 35 < cout && x - 25 > 0 && sancha_y[x + 35] > sancha_y[x - 1] && sancha_y[x - 1] <= sancha_y[x + 18] && sancha_y[x - 1] <= sancha_y[x + 9] && sancha_y[x - 1] < sancha_y[x + 24] && sancha_y[x - 11] > sancha_y[x - 1] && sancha_y[x - 25] > sancha_y[x - 1] && abs(sancha_y[x + 35] - sancha_y[x - 1]) <= 7 && abs(sancha_y[x - 11] - sancha_y[x - 1]) < 7 && abs(sancha_y[x - 1] - sancha_y[x - 25]) < 7 && sancha_y[x - 1] <= 50)
{
sancha_top_y = sancha_y[x - 1];
sancha_top_x = sancha_x[x - 1];
down_y = sancha_y[x - 1];
}
}
}
}
}
if (sancha_top_y > 10)
{
sancha_enter_flag = 1;
sancha_flag = 1;
}
// SetText("—> 三岔上拐点为y = " + sancha_top_y + " x = " + sancha_top_x);
}
}
二、在三岔中
思路：这个状态其实比较好理解，前一个入三岔时我们为了拉线是要一直找三岔的v型的最低点的，当这个点比较低。或者是找不到消失了，那就可以进入到“在三岔中”的状态。这个状态只需要正常扫线正常跑即可，没有需要的图像处理，仅记录一个标志位。
if (sancha_inside_flag == 1)
{
if ((righty[0] > 0 && rightx[0] < 160) || (leftx[1] > 100 && leftx[1] < 180) || b >= 40) //右出三岔
{
for (y = 2; y < righty[0]; y++)
{
if (R_black[y] - R_black[y - 1] >= 0 && R_black[y + 1] - R_black[y] < 0 && R_black[y + 1] - R_black[y - 2] < 0)
{
j = R_black[y];
// SetText("从第j=" + j + "开始向左扫");
break;
}
}
for (x = j; x < L_black[0]; x++)
{
for (y = 0; y < 65; y++)
{
if (cout == 191)
break;
if (Pixels[y][x] != 0 && Pixels[y + 1][x] != 0 && Pixels[y + 2][x] == 0)
{
sancha_y[cout] = (byte)(y + 1);
sancha_x[cout] = (byte)(x);
cout++;
break;
}
}
}
if (cout > 20)
{
for (x = cout; x > 1; x--)
{
if (sancha_y[x] - sancha_y[x - 1] < 0 && sancha_y[x] != 0 && sancha_y[x - 1] != 0)
up_num++;
if (sancha_y[x] - sancha_y[x - 1] > 0 && sancha_y[x] != 0 && sancha_y[x - 1] != 0)
dowm_num++;
}
// SetText("向下的连续数dowmnum=" + dowm_num + "上连续数upnum=" + up_num);
for (x = (byte)(cout - 20); x > 26; x--)
{
if (sancha_y[x - 1] <= down_y && sancha_y[x - 1] <= sancha_y[x] && sancha_y[x + 4] >= sancha_y[x - 1] && sancha_y[x - 8] >= sancha_y[x - 1] && abs(sancha_y[x + 4] - sancha_y[x - 1]) < 3 && abs(sancha_y[x - 8] - sancha_y[x - 1]) < 3)
{
byte first_flag = 0;
if (sancha_out_flag == 0 && x > 21 && x < (byte)(cout - 20))
{
for (int k = (byte)(x - 20); k < x + 20; k++)
{
if (sancha_y[k] < sancha_y[x - 1])
{
first_flag = 1;
break;
}
}
}
if (first_flag == 0)
{
down_y = sancha_y[x - 1];
if (sancha_x[x - 1] >= 125)
{
if (x - 30 > 0 && sancha_y[x + 9] >= sancha_y[x - 1] && sancha_y[x - 30] >= sancha_y[x - 1] && abs(sancha_y[x + 9] - sancha_y[x - 1]) < 7 && abs(sancha_y[x - 30] - sancha_y[x - 1]) <= 9 && sancha_y[x - 41] > sancha_y[x - 1] && x - 41 > 0)
{
sancha_top_y = sancha_y[x - 1];
sancha_top_x = sancha_x[x - 1];
down_y = sancha_y[x - 1];
}
}
}
}
}
SetText("—> 三岔上拐点为y = " + sancha_top_y + " x = " + sancha_top_x);
}
}
if (sancha_top_y >= 15 && sancha_top_y <= 48) //找到最低点
{
sancha_out_flag = 1;
sancha_inside_flag = 0;
}
}
三、出三岔
思路：出三岔和入三岔的代码很相似。在三岔中的状态我们是找到v型的最低点的，等到了出口的位置，又看到了熟悉的v型，我们就故技重施再去找v型点进行拉线。同样的等这个v型点低到一定程度，或者消失了就可以退出三岔。
//找点出三岔
if (sancha_out_flag == 1)
{
if (sancha_top_y == 0)
{
for (x = R_black[0]; x < L_black[0]; x++)
{
for (y = 0; y < 65; y++)
{
if (Pixels[y][x] != 0 && Pixels[y + 1][x] != 0 && Pixels[y + 2][x] == 0)
{
sancha_y[cout] = (byte)(y + 1);
sancha_x[cout] = (byte)(x);
cout++;
break;
}
}
}
if (cout > 20)
{
for (x = cout; x > 1; x--)
{
if (sancha_y[x] - sancha_y[x - 1] < 0 && sancha_y[x] != 0 && sancha_y[x - 1] != 0)
up_num++;
if (sancha_y[x] - sancha_y[x - 1] > 0 && sancha_y[x] != 0 && sancha_y[x - 1] != 0)
dowm_num++;
}
SetText("向下的连续数dowmnum=" + dowm_num + "上连续数upnum=" + up_num);
for (x = (byte)(cout - 20); x > 26; x--)
{
if (sancha_y[x - 1] <= down_y && sancha_y[x - 1] <= sancha_y[x] && sancha_y[x + 4] >= sancha_y[x - 1] && sancha_y[x - 8] >= sancha_y[x - 1] && abs(sancha_y[x + 4] - sancha_y[x - 1]) < 3 && abs(sancha_y[x - 8] - sancha_y[x - 1]) < 3)
{
if (sancha_x[x - 1] >= 125)
{
if (x - 30 > 0 && sancha_y[x + 9] >= sancha_y[x - 1] && sancha_y[x - 30] > sancha_y[x - 1] && abs(sancha_y[x + 9] - sancha_y[x - 1]) < 7 && abs(sancha_y[x - 30] - sancha_y[x - 1]) < 9 && sancha_y[x - 41] > sancha_y[x - 1] && x - 41 > 0)
{
sancha_top_y = sancha_y[x - 1];
sancha_top_x = sancha_x[x - 1];
down_y = sancha_y[x - 1];
}
}
else if (sancha_x[x - 1] < 125 && sancha_x[x - 1] > 60)
{
if (sancha_y[x + 21] >= sancha_y[x - 1] && sancha_y[x - 20] > sancha_y[x - 1] && abs(sancha_y[x + 21] - sancha_y[x - 1]) < 7 && abs(sancha_y[x - 20] - sancha_y[x - 1]) < 7 && x - 20 > 0 && x + 21 < cout)
{
sancha_top_y = sancha_y[x - 1];
sancha_top_x = sancha_x[x - 1];
down_y = sancha_y[x - 1];
}
}
else if (sancha_x[x - 1] <= 60)
{
if (sancha_y[x + 39] > sancha_y[x - 1] && sancha_y[x - 21] > sancha_y[x - 1] && abs(sancha_y[x + 39] - sancha_y[x - 1]) < 7 && abs(sancha_y[x - 21] - sancha_y[x - 1]) < 7 && x + 39 < cout && x - 21 > 0)
{
sancha_top_y = sancha_y[x - 1];
sancha_top_x = sancha_x[x - 1];
down_y = sancha_y[x - 1];
}
}
}
if (sancha_top_y != 0)
{
break;
}
}
SetText("—> 三岔上拐点为y = " + sancha_top_y + " x = " + sancha_top_x);
}
}
if (abs(up_num - dowm_num) >= 30 || (sancha_lasty < 20 && sancha_lasty > 1 && sancha_top_y == 0 && L_black[10] == 185) || sancha_top_y < 5)
{
sancha_flag = 0;
sancha_out_flag = 0;
byte num = 0;
for (y = 0; y < 70; y++)
{
if (R_black[y] == 0 && L_black[y] == 185)
{
num++;
}
}
if (num > 65)
{
scanline(); //说明已经退出，开始正常扫线
}
SetText("-> 退出三岔状态 <-");
}
if (sancha_top_y != 0)
{
sancha_lasty = sancha_top_y;
sancha_lastx = sancha_top_x;
}
}
总结
本文提供的一个纯图像处理方案，我认为三岔是比较好写的图像之一，特征是比较明显的。之前也有些友友来问我弯接三岔的问题，我本身是没有特意把三岔进行分类，只要能看见v字就能进行补线。如果你是弯接三岔一直看不到v导致进错三岔的话我觉得应该要改改控制或者摄像头的角度。如果你觉得文章有所帮助还希望点赞鼓励一下！欢迎大家在评论区提出改进意见！
最后的最后如果想看看我这一年智能车的经历和建议可以到第一篇文章最下面浏览一下 智能车学习日记【一】