智能车整理日记【七】————三岔

---

前言

这一篇记录的是三岔路口的处理方式，给大家提供一种思路。我的正入三岔和弯接三岔用的一套代码，弯接的条件稍微放宽一点。欢迎大家提出意见和修改！ 一些数组和参数定义在前几篇文章有说明，如果觉得这篇文章有帮助就点个赞支持一下吧！

---

一、入三岔

先看三岔的图像，其中最明显的特征就是红色框出来的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导致进错三岔的话我觉得应该要改改控制或者摄像头的角度。如果你觉得文章有所帮助还希望点赞鼓励一下！欢迎大家在评论区提出改进意见！

最后的最后如果想看看我这一年智能车的经历和建议可以到第一篇文章最下面浏览一下 智能车学习日记【一】