devbox
小蓝xlanll
这个屌丝很懒，什么也没留下！
关注作者
热门标签
热门文章
当前位置:   article > 正文

17届智能车图像处理部分讲解_总钻风摄像头采集图像过程

作者：小蓝xlanll | 2024-04-24 00:46:53

赞

踩

总钻风摄像头采集图像过程

目录

须知

寻边线

寻拐点

补线

最后

须知

讲解代码使用的摄像头为总钻风摄像头，图像像素为188*120，图像进行了二值化，这里建议如果需要对光线有要求的同学使用灰度处理。没有使用过上位机，展示的图片都是直接拍摄2寸IPS的显示。

寻边线

当我们采集到一帧图像并做完二值化后，我们就开始寻边线，我使用了简单方便的左右扫线，因为在某些情况顶部图像会是黑的，所以使用从图像靠近车头的一端扫线。此时我做了些许处理，如果第一次扫线就从中间开始扫，以后会从上一次计算的中线开始扫，这也需要每次扫完一行就得计算一下中线。为了更加符合赛道变化，下一行的扫描起始点是上一行的中线，当然某些特殊情况也会出问题，比如遇到斑马线，所以又必须对斑马线做处理。左右扫线简单方便，但是执行起来需要遍历的数据量比较多。

  1. //-------------------------------------------------------------------------------------------------------------------
  2. //  @brief      左右巡边线
  3. //  @param      BoundaryRight   右丢线行数
  4. //  @param      BoundaryLeft    左丢线行数 
  5. //  @param
  6. //  @param
  7. //  @return     void
  8. //  @note       采用左右扫线的方法，从图像最低端开始，每行扫完会计算一次中线，下一行扫描从上一行中线开始
  9. //-------------------------------------------------------------------------------------------------------------------
  10. static uint8 star = 93;                             //寻线初始点
  11. //需优化：有时其他赛道会干扰
  12. //如果当左边线寻到有边界则使以后边线都处于边界
  13. void Get_Line(void)
  14. {
  15. //    float k=0;  //斜率
  16. //    float b=0;  //截距
  17.     //uint8 left=0;
  18.     //int kuan=0;
  19. //    k=((float)xtwo - (float)xone)/((float)ytwo - (float)yone);
  20. //    b=(float)yone - ((float)xone*k);
  21.     BoundaryRight = 0;
  22.     BoundaryLeft = 0;
  23.     if(Middle_Black_Point[119] == 0 || Middle_Black_Point[119] == 187)  //判断起始点是否在图像中间
  24.     {
  25.         star = 93;
  26.     }
  27.     else
  28.     {
  29.         star = Middle_Black_Point[119];
  30.     }
  31.     for(uint8 y=119;y>=0;y--)
  32.     {
  33.         for(uint8 x=star;x<188;x++)
  34.         {
  35.             if(Emo_imag[y][x]==EmoBlack)  //黑黑黑即判断为边线
  36.             {
  37.                 if(Emo_imag[y][x+1]==EmoBlack && Emo_imag[y][x+2]==EmoBlack)
  38.                 {
  39.                     if(y < Endline )
  40.                         Right_Black_Point[y] = 187;
  41.                     else
  42.                         Right_Black_Point[y]=x;
  43.                     //left=star;
  44.                     break;
  45.                 }
  46.             }
  47.             if(x==186)                      //有点问题，返校复查
  48.             {
  49.                 Right_Black_Point[y]=187;
  50.                 //left=star;
  51.                 if(y > Endline && y < 108)
  52.                     BoundaryRight++;
  53.                 break;
  54.             }
  55.         }
  56.         for(uint8 x=star;x>=0;x--)
  57.         {
  58.             if(Emo_imag[y][x]==EmoBlack)
  59.             {
  60.                 if(Emo_imag[y][x-1]==EmoBlack && Emo_imag[y][x-2]==EmoBlack)
  61.                 {
  62.                     if(y < Endline)
  63.                         Left_Black_Point[y] = 0;
  64.                     else
  65.                         Left_Black_Point[y]=x;
  66.                     break;
  67.                 }
  68.             }
  69.             if(x==0)
  70.             {
  71.                 Left_Black_Point[y]=0;
  72.                 if(y > Endline && y < 108)
  73.                     BoundaryLeft++;
  74.                 break;
  75.             }
  76.         }
  77.         if(Right_Black_Point[y]==187&&Left_Black_Point[y]==0)        //两边都没找到线
  78.         {
  79.             Middle_Black_Point[y]=Middle_Black_Point[y+1];
  80.             //star=Middle_Black_Point[y];
  81.             star = 93;
  82.         }
  83.         else if(Right_Black_Point[y]==187&&Left_Black_Point[y]!=0)    //左边找到线
  84.         {
  85.             Middle_Black_Point[y]=Left_Black_Point[y]+Straight[y];
  86.             star=Middle_Black_Point[y];
  87. //            if(CurvatureRight < -0.0045)
  88. //            {
  89. //                kuan = ((y-120)*(y-120)/(20)+93);
  90. //                kuan = kuan > (187-Right_Black_Point[y]) ? (187-Right_Black_Point[y]) : kuan;
  91. //                Middle_Black_Point[y]=Left_Black_Point[y]+kuan;
  92. //            }
  93.         }
  94.         else if(Left_Black_Point[y]==0&&Right_Black_Point[y]!=187)    //右边找到线
  95.         {
  96.             Middle_Black_Point[y]=Right_Black_Point[y]-Straight[y];
  97.             star=Middle_Black_Point[y];
  98. //            if(CurvatureRight < -0.0045)
  99. //            {
  100. //                kuan = ((y-120)*(y-120)/(20)+93);
  101. //                kuan = kuan > Right_Black_Point[y] ? Right_Black_Point[y] : kuan;
  102. //                Middle_Black_Point[y]=Right_Black_Point[y]-kuan;
  103. //            }
  104.         }
  105.         else             //两边都找到线
  106.         {
  107.             Middle_Black_Point[y]=(uint8)(((int)Right_Black_Point[y]+(int)Left_Black_Point[y])/2);
  108.             star=Middle_Black_Point[y];
  109.         }
  110. //        Middle_Black_Point[y] = Middle_Black_Point[y] < 1 ? 1 : Middle_Black_Point[y];
  111. //        Middle_Black_Point[y] = Middle_Black_Point[y] > 186 ? 186 : Middle_Black_Point[y];
  112.         Middle_Black_Point[y] = Middle_Black_Point[y] < 1 ? 0 : Middle_Black_Point[y];
  113.         Middle_Black_Point[y] = Middle_Black_Point[y] >186 ? 187 :Middle_Black_Point[y];
  114.         if(y==0)
  115.             break;
  116.     }
  117.     //star = Middle_Black_Point[119];
  118. }

寻拐点

寻拐点呢我采用了类似于八邻域的原理，首先是下拐点，我们从底端向上寻找，因为拐点对应的一定是丢线的，所以向上一直寻找到丢线行，以丢线行再向下对一个个点进行判断。

以右下拐点为例，我们放大像素块，我们从红点像素块的左下开始以顺时针旋转，当扫描的点周边的八个像素块出现四个白色像素块时即满足拐点。但是需要注意，拐点是不会距离丢线行数太多的，为了防止拐点误判的出现，我们应限制以丢线行数进行扫描的次数。前边说过左右扫线会因为斑马线造成干扰，所以扫寻完下拐点后，上拐点选择从图像上端向下扫，同样是向下扫到丢线后向再向上寻点，当然，对于扫描点附近八个像素块的旋转就应该换个方向。

  1. //-------------------------------------------------------------------------------------------------------------------
  2. //  @brief      拐点寻找
  3. //  @param      findlcount   拐点距离丢线的行数，用于判断大小圆环，和区分P字和圆环
  4. //  @param
  5. //  @param
  6. //  @param
  7. //  @return     void
  8. //  @note       采用5邻域的原理寻找拐点，下拐点从图像低端往上扫，上拐点从图像上方向下扫，左右扫线会在斑马线出现问题，
  9. //-------------------------------------------------------------------------------------------------------------------
  10. void Identify(void)
  11. {
  12.     uint8 findr_x = 0;    //右点
  13.     uint8 findr_y = 0;
  14.     uint8 examr_x = 0;
  15.     uint8 examr_y = 0;
  16.     uint8 findl_x = 0;    //左点
  17.     uint8 findl_y = 0;
  18.     uint8 examl_x = 0;
  19.     uint8 examl_y = 0;
  20.     uint8 star = 0;
  21.     uint8 end = 0;
  22.     uint8 examcount = 0;
  23.     //uint8 count;
  24.     //uint8 examerror;
  25. //    uint8 dircount;
  26.     int directionrd[5][2] =  {{-1,1}, {-1,0}, {-1,-1}, {0,1}, {1,1}};  //顺时针下方向数组先x再y
  27.     int directionld[5][2] =  {{1,1}, {1,0}, {1,-1}, {0,-1}, {-1,-1}};  //逆时针下方向数组
  28.     int directionru[5][2] =  {{1,1}, {0,1}, {-1,1}, {-1,0}, {-1,-1}};  //逆时针上方向数组
  29.     int directionlu[5][2] =  {{-1,1}, {0,1}, {1,1}, {1,0}, {1,-1}};  //逆时针上方向数组
  30.  
  31.     //每次采集后都对拐点标志位清零
  32.     if(Right_Up_Point_finish_flag == 1)
  33.         Right_Up_Point_finish_flag = 0;
  34.     if(Left_Up_Point_finish_flag == 1)
  35.         Left_Up_Point_finish_flag = 0;
  36.     if(Right_Down_Point_finish_flag == 1)
  37.         Right_Down_Point_finish_flag = 0;
  38.     if(Left_Down_Point_finish_flag == 1)
  39.         Left_Down_Point_finish_flag = 0;
  40.     for(uint8 y = 105 ; y >= 30 ; y--)
  41.     {
  42.         if(Right_Down_Point_finish_flag == 0)
  43.         {
  44.             if(y > Endline && Right_Black_Point[y-1]==187 && Right_Black_Point[y-2]==187 && Emo_imag[y][Right_Black_Point[y]-6] == EmoWhite
  45.                && y > Endline && Emo_imag[y-2][Right_Black_Point[y]] == EmoWhite && Emo_imag[y-5][Right_Black_Point[y]] == EmoWhite)    //右下拐点
  46.             {
  47.                 star=y;
  48.                 for(uint8 y=star;y<=(star+18);y++)
  49.                 {
  50.                     if(Right_Black_Point[y]<184 && Emo_abs(Right_Black_Point[y+1]-Right_Black_Point[y])<3
  51.                     && Emo_abs(Right_Black_Point[y+2]-Right_Black_Point[y])<4)
  52.                     {
  53.                         findr_x=Right_Black_Point[y];
  54.                         findr_y=y;
  55.                         for(uint8 dircount = 0;dircount<5;dircount++)
  56.                         {
  57.                             examr_x=findr_x+directionrd[dircount][0];
  58.                             examr_y=findr_y+directionrd[dircount][1];
  59.                             if(Emo_imag[examr_y][examr_x]==255)
  60.                             {
  61.                                 examcount++;
  62.                             }
  63.                         }
  64.                         if(examcount >= 4)
  65.                         {
  66.                             examcount=0;
  67.                             Right_Down_Point[0]=findr_x;
  68.                             Right_Down_Point[1]=findr_y;
  69. //                            if(Last_Right_Point[0] == 0)
  70. //                            {
  71. //                                Last_Right_Point[0] = Right_Down_Point[1];
  72. //                            }
  73. //                            if(Emo_Uint8_dec(Right_Down_Point[1],Last_Right_Point[0]) < -15)
  74. //                            {
  75. //                                Right_Down_Point_finish_flag = 0;
  76. //                            }
  77. //                            else
  78. //                            {
  79.                                 Right_Down_Point_finish_flag = 1;
  80. //                                Last_Right_Point[0] = Right_Down_Point[1];
  81. //                            }
  82.     //                        findrcount = (int)y-(int)star;
  83.                             break;
  84.                         }
  85.                         else
  86.                         {
  87.                             Right_Down_Point_finish_flag = 0;
  88.                             examcount=0;
  89.                         }
  90.                     }
  91.                     if(y==100)
  92.                     {
  93.                         Right_Down_Point_finish_flag=0;
  94.                     }
  95.                 }
  96.             }
  97.         }
  98.         if(Left_Down_Point_finish_flag == 0)
  99.         {
  100.             if(y > Endline && Left_Black_Point[y-1]==0 && Left_Black_Point[y-2]==0 && Emo_imag[y][Left_Black_Point[y]+6] == EmoWhite
  101.                && y > Endline && Emo_imag[y-2][Left_Black_Point[y]] == EmoWhite && Emo_imag[y-5][Left_Black_Point[y]] == EmoWhite)     //左下拐点
  102.             {
  103.                 star=y;
  104.                 for(uint8 y=star;y<=(star+18);y++)
  105.                 {
  106.                     if(Left_Black_Point[y]>4 && Emo_abs(Left_Black_Point[y+1]-Left_Black_Point[y])<3
  107.                        && Emo_abs(Left_Black_Point[y+2]-Left_Black_Point[y])<4)
  108.                     {
  109.                         findl_x=Left_Black_Point[y];
  110.                         findl_y=y;
  111.                         for(uint8 dircount = 0;dircount<5;dircount++)
  112.                         {
  113.                             examl_x=findl_x+directionld[dircount][0];
  114.                             examl_y=findl_y+directionld[dircount][1];
  115.                             if(Emo_imag[examl_y][examl_x]==255)
  116.                             {
  117.                                 examcount++;
  118.                             }
  119.                         }
  120.                         if(examcount>=4 )
  121.                         {
  122.                             examcount=0;
  123.                             Left_Down_Point[0]=findl_x;
  124.                             Left_Down_Point[1]=findl_y;
  125. //                            if(Last_Left_Point[0] == 0)
  126. //                            {
  127. //                                Last_Left_Point[0] = Left_Down_Point[1];
  128. //                            }
  129. //                            if(Emo_Uint8_dec(Left_Down_Point[1],Last_Left_Point[0]) < -15)
  130. //                            {
  131. //                                Left_Down_Point_finish_flag = 0;
  132. //                            }
  133. //                            else
  134. //                            {
  135.                                 Left_Down_Point_finish_flag = 1;
  136. //                                Last_Left_Point[0] = Left_Down_Point[1];
  137. //                            }
  138.     //                        findlcount = y-star;
  139.                             break;
  140.                         }
  141.                         else
  142.                         {
  143.                             Left_Down_Point_finish_flag = 0;
  144.                             examcount=0;
  145.                         }
  146.                     }
  147.                     if(y==100)
  148.                     {
  149.                         Left_Down_Point_finish_flag=0;
  150.                     }
  151.                 }
  152.             }
  153.         }
  154. //        if(CrossLeft_Down_Point_finish_flag==1 && CrossRight_Down_Point_finish_flag==1)
  155. //        {
  156. //            break;
  157. //        }
  158.     }
  159.     if(Left_Down_Point_finish_flag==1 && Right_Down_Point_finish_flag==1)
  160.         end=Right_Down_Point[1];
  161.     else if(Left_Down_Point_finish_flag==1)
  162.         end=Left_Down_Point[1];
  163.     else if(Right_Down_Point_finish_flag==1)
  164.         end=Right_Down_Point[1];
  165.     else
  166.         end=94;
  167.     for(uint8 y=20;y<=end;y++)
  168.     {
  169.         if(Right_Up_Point_finish_flag == 0)
  170.         {
  171.             if(y > Endline && Right_Black_Point[y+1]==187 && Right_Black_Point[y+2]==187 && Right_Black_Point[y+3]==187)   //右上拐点
  172.             {
  173.                star=y;
  174.                for(uint8 y=star;y>=(star-22);y--)
  175.                {
  176.                    if(Right_Black_Point[y]<180 && Emo_abs(Right_Black_Point[y-1]-Right_Black_Point[y])<4
  177.                    && Emo_abs(Right_Black_Point[y-2]-Right_Black_Point[y])<4 && Emo_imag[y][Right_Black_Point[y]-6] == EmoWhite
  178.                    && Emo_imag[y-1][Right_Black_Point[y-1]-5] == EmoWhite && Emo_imag[y-2][Right_Black_Point[y]-5] == EmoWhite
  179.                    && Emo_imag[y-3][Right_Black_Point[y]-5] == EmoWhite && Right_Black_Point[y] > Middle_Black_Point[y] && Emo_imag[y+3][Right_Black_Point[y]] == EmoWhite
  180.                    && Emo_imag[y+5][Right_Black_Point[y]] == EmoWhite && Emo_imag[y+7][Right_Black_Point[y]] == EmoWhite)
  181.                    {
  182.                        findr_x=Right_Black_Point[y];
  183.                        findr_y=y;
  184.                        for(uint8 dircount = 0;dircount<5;dircount++)
  185.                        {
  186.                            examr_x=findr_x+directionru[dircount][0];
  187.                            examr_y=findr_y+directionru[dircount][1];
  188.                            if(Emo_imag[examr_y][examr_x]==255)
  189.                            {
  190.                                examcount++;
  191.                            }
  192.                        }
  193.                        if(examcount>=4 && findr_y >Endline)
  194.                        {
  195.                            examcount=0;
  196.                            Right_Up_Point[0]=findr_x;
  197.                            Right_Up_Point[1]=findr_y;
  198.                            if(Last_Right_Point[1] == 0)
  199.                            {
  200.                                Last_Right_Point[1] = Right_Up_Point[1];
  201.                            }
  202.                            if(Right_Up_Point[1] < 16)
  203.                            {
  204.                                Right_Up_Point[1] = Last_Right_Point[1];
  205.                                Right_Up_Point_finish_flag = 0;
  206.                            }
  207.                            else
  208.                            {
  209.                                Right_Up_Point_finish_flag = 1;
  210.                                Last_Right_Point[1] = Right_Up_Point[1];
  211.                            }
  212.                            findrcount=(int)star-(int)findr_y;
  213.                            break;
  214.                        }
  215.                        else
  216.                        {
  217.                            Right_Up_Point_finish_flag = 0;
  218.                            examcount=0;
  219.                        }
  220.                    }
  221.                    if(y==16)
  222.                    {
  223.                        Right_Up_Point_finish_flag=0;
  224.                        break;
  225.                    }
  226.                    if(y == 1)
  227.                    {
  228.                        break;
  229.                    }
  230.                }
  231.             }
  232.         }
  233.         if(Left_Up_Point_finish_flag == 0)
  234.         {
  235.             if(y > Endline && Left_Black_Point[y+1]==0 && Left_Black_Point[y+2]==0 && Left_Black_Point[y+3]==0)     //左上拐点
  236.             {
  237.                 star=y;
  238.                 for(uint8 y=star;y>=(star-22);y--)
  239.                 {
  240.                     if(Left_Black_Point[y]>8 && Emo_abs(Left_Black_Point[y-1]-Left_Black_Point[y])<4
  241.                    && Emo_abs(Left_Black_Point[y-2]-Left_Black_Point[y])<4 && Emo_imag[y][Left_Black_Point[y]+6] == EmoWhite
  242.                    && Emo_imag[y-1][Left_Black_Point[y-1]+5] == EmoWhite && Emo_imag[y-2][Left_Black_Point[y]+5] == EmoWhite
  243.                    && Emo_imag[y-3][Left_Black_Point[y]+5] == EmoWhite && Left_Black_Point[y] < Middle_Black_Point[y] && Emo_imag[y+3][Left_Black_Point[y]] == EmoWhite
  244.                    && Emo_imag[y+5][Left_Black_Point[y]] == EmoWhite && Emo_imag[y+7][Left_Black_Point[y]] == EmoWhite)
  245.                     {
  246.                         findl_x=Left_Black_Point[y];
  247.                         findl_y=y;
  248.                         for(uint8 dircount = 0;dircount<5;dircount++)
  249.                         {
  250.                             examl_x=findl_x+directionlu[dircount][0];
  251.                             examl_y=findl_y+directionlu[dircount][1];
  252.                             if(Emo_imag[examl_y][examl_x]==255)
  253.                             {
  254.                                 examcount++;
  255.                             }
  256.                         }
  257.                         if(examcount>=4 && findl_y > Endline)
  258.                         {
  259.                             examcount=0;
  260.                             Left_Up_Point[0]=findl_x;
  261.                             Left_Up_Point[1]=findl_y;
  262.                             if(Last_Left_Point[1] == 0)
  263.                             {
  264.                                 Last_Left_Point[1] = Left_Up_Point[1];
  265.                             }
  266.                             if(Left_Up_Point[1] < 16)
  267.                             {
  268.                                 Left_Up_Point[1] = Last_Left_Point[1];
  269.                                 Left_Up_Point_finish_flag = 0;
  270.                             }
  271.                             else
  272.                             {
  273.                                 Left_Up_Point_finish_flag = 1;
  274.                                 Last_Left_Point[1] = Left_Up_Point[1];
  275.                             }
  276.                             findlcount=(int)star-(int)y;
  277.                             break;
  278.                         }
  279.                         else
  280.                         {
  281.                             Left_Up_Point_finish_flag = 0;
  282.                             examcount=0;
  283.                         }
  284.                     }
  285.                     if(y==16)
  286.                     {
  287.                         Left_Up_Point_finish_flag=0;
  288.                         break;
  289.                     }
  290.                     if(y == 1)
  291.                     {
  292.                         break;
  293.                     }
  294.                 }
  295.             }
  296.         }
  297.     }
  298.  
  299. }

圆环的类似拐点距离丢线行比较远，于是我选择放开上拐点的距离限制，转而记录下扫描的拐点距离丢线的行数，正好发现可以用来区分17届新加的元素P字 。

 当然因为如果距离圆环太远的话，圆环的特征是不太明显的，所以我会在识别到上下拐点时进入元素前摇的函数，在到达百分比区别P字和圆环前，将由此函数接管。

  1. //元素前摇
  2. //-------------------------------------------------------------------------------------------------------------------
  3. //  @brief      元素前摇
  4. //  @param
  5. //  @param
  6. //  @param
  7. //  @param
  8. //  @return     void
  9. //  @note       在扫描到两个符合的上下两个拐点后，针对先形拐点进行判断是什么元素，判断包括圆环、十字、P字
  10. //-------------------------------------------------------------------------------------------------------------------
  11. void Windup(void)
  12. {
  13.     if(WindupL_flag == 1)
  14.     {
  15.         //Beepindex = Beepon;
  16.         Link_Left_One_Point[0] = Left_Down_Point[0];
  17.         Link_Left_One_Point[1] = Left_Down_Point[1];
  18.         Link_Left_Two_Point[0] = Left_Up_Point[0];
  19.         Link_Left_Two_Point[1] = Left_Up_Point[1];
  20.         PaddingR = 0;
  21.         PaddingL = 1;
  22.         if(Right_Up_Point_finish_flag == 1 && Right_Down_Point_finish_flag == 1 && Left_Down_Point[1] > 42 && findlcount < 6 && Left_Up_Point[0] > 30)
  23.         {
  24.             Cross_flag = 1;
  25.             WindupL_flag = 0;
  26.             Beepindex = 0;
  27.         }
  28.         else if(Left_Up_Point_finish_flag == 1 && Left_Down_Point_finish_flag == 1 &&
  29.                 Right_Up_Point_finish_flag == 0 && Right_Down_Point_finish_flag == 0
  30.                 && Left_Down_Point[1] >= 75 && Left_Down_Point[1] <= 105 && findlcount < 4 && Left_Up_Point[0] > 30
  31.                 && (Left_Up_Point[0] - Left_Down_Point[0]) <= 42 && (Left_Down_Point[1] - Left_Up_Point[1]) >= 45
  32.                 && Right_Black_Point[Left_Up_Point[1]] < Right_Black_Point[(Left_Up_Point[1]+Left_Down_Point[1])/2]
  33.                 && Right_Black_Point[(Left_Up_Point[1]+Left_Down_Point[1])/2] <  Right_Black_Point[Left_Down_Point[1]]
  34.                 && Right_Black_Point[Left_Up_Point[1]] < Right_Black_Point[Emo_one_third(Left_Down_Point[1],Left_Up_Point[1])]
  35.                 && Right_Black_Point[Emo_one_third(Left_Down_Point[1],Left_Up_Point[1])] <  Right_Black_Point[Left_Down_Point[1]]
  36.                 && Right_Black_Point[Left_Up_Point[1]] < Right_Black_Point[Emo_two_third(Left_Down_Point[1],Left_Up_Point[1])]
  37.                 && Right_Black_Point[Emo_two_third(Left_Down_Point[1],Left_Up_Point[1])] <  Right_Black_Point[Left_Down_Point[1]]
  38.                 && Right_Black_Point[Left_Up_Point[1]] < Right_Black_Point[Left_Down_Point[1]])
  39.         {
  40.             SlalomLeft_flag = 1;
  41.             Slalomcount = 1;
  42.             WindupL_flag = 0;
  43.         }
  44.         else if(Left_Up_Point_finish_flag == 1 && Left_Down_Point_finish_flag == 1 &&
  45.                 Right_Up_Point_finish_flag == 0 && Right_Down_Point_finish_flag == 0
  46.                 && Left_Down_Point[1] >= 75 && findlcount >= 8 && Left_Up_Point[0] > 30 && (Left_Down_Point[1] - Left_Up_Point[1]) >= 45
  47.                 && (Left_Down_Point[1] - Left_Up_Point[1]) <= 78 && Emo_imag[Left_Up_Point[1]][Left_Up_Point[0] - 5] != EmoWhite
  48.                 && Right_Black_Point[Left_Up_Point[1]] < Right_Black_Point[(Left_Up_Point[1]+Left_Down_Point[1])/2]
  49.                 && Right_Black_Point[(Left_Up_Point[1]+Left_Down_Point[1])/2] <  Right_Black_Point[Left_Down_Point[1]]
  50.                 && Right_Black_Point[Left_Up_Point[1]] < Right_Black_Point[Emo_one_third(Left_Down_Point[1],Left_Up_Point[1])]
  51.                 && Right_Black_Point[Emo_one_third(Left_Down_Point[1],Left_Up_Point[1])] <  Right_Black_Point[Left_Down_Point[1]]
  52.                 && Right_Black_Point[Left_Up_Point[1]] < Right_Black_Point[Emo_two_third(Left_Down_Point[1],Left_Up_Point[1])]
  53.                 && Right_Black_Point[Emo_two_third(Left_Down_Point[1],Left_Up_Point[1])] <  Right_Black_Point[Left_Down_Point[1]]
  54.                 && Right_Black_Point[Left_Up_Point[1]] < Right_Black_Point[Left_Down_Point[1]])
  55.         {
  56.             if(findlcount >= 16)
  57.             {
  58.                 CircleBig = 1;
  59.             }
  60.             CircleLeft_flag = 1;
  61.             Circlecount = 1;
  62.             WindupL_flag = 0;
  63.         }
  64.         else if(Left_Up_Point_finish_flag == 1 && Left_Down_Point_finish_flag == 1 && Right_Up_Point_finish_flag == 0 && Right_Down_Point_finish_flag == 0 && Left_Down_Point[1] <= 110
  65.                 && findlcount <= 17)
  66.         {
  67.             GarageL_Find();
  68.             if(GarageL_Findfinish_flag == 1)
  69.             {
  70.                 GarageL_Findfinish_flag = 0;
  71.                 GarageL_flag = 1;
  72.                 WindupL_flag = 0;
  73.             }
  74.         }
  75.         else if(Left_Down_Point_finish_flag == 0)
  76.         {
  77.             WindupL_flag = 0;
  78.             PaddingR = 0;
  79.             PaddingL = 0;
  80.             Beepindex = 0;
  81.         }
  82.     }
  83.     else if(WindupR_flag == 1)
  84.     {
  85.         //Beepindex = Beepon;
  86.         Link_Right_One_Point[0] = Right_Down_Point[0];
  87.         Link_Right_One_Point[1] = Right_Down_Point[1];
  88.         Link_Right_Two_Point[0] = Right_Up_Point[0];
  89.         Link_Right_Two_Point[1] = Right_Up_Point[1];
  90.         PaddingR = 1;
  91.         PaddingL = 0;
  92.         if(Left_Up_Point_finish_flag == 1 && Left_Down_Point_finish_flag == 1 && Right_Down_Point[1] > 42 && findrcount < 6 && Right_Up_Point[0] < 158)
  93.         {
  94.             Cross_flag = 1;
  95.             WindupR_flag = 0;
  96.             Beepindex = 0;
  97.         }
  98.         else if(Right_Up_Point_finish_flag == 1 && Right_Down_Point_finish_flag == 1 &&
  99.                 Left_Up_Point_finish_flag == 0 && Left_Down_Point_finish_flag == 0
  100.             && Right_Down_Point[1] >= 75 && Right_Down_Point[1] <= 105 && findrcount < 4 && Right_Up_Point[0] < 158
  101.             && (Right_Down_Point[0] - Right_Up_Point[0]) <= 42 && Right_Down_Point[1] - Right_Up_Point[1] >= 45
  102.             && Right_Down_Point[1] - Right_Up_Point[1] <= 78 && Emo_imag[Right_Up_Point[1]][Right_Up_Point[0] + 5] != EmoWhite
  103.             && Left_Black_Point[Right_Up_Point[1]] > Left_Black_Point[(Right_Up_Point[1]+Right_Down_Point[1])/2]
  104.             && Left_Black_Point[(Right_Up_Point[1]+Right_Down_Point[1])/2] >  Left_Black_Point[Right_Down_Point[1]]
  105.             && Left_Black_Point[Right_Up_Point[1]] > Left_Black_Point[Emo_one_third(Right_Down_Point[1],Right_Up_Point[1])]
  106.             && Left_Black_Point[Emo_one_third(Right_Down_Point[1],Right_Up_Point[1])] >  Left_Black_Point[Right_Down_Point[1]]
  107.             && Left_Black_Point[Right_Up_Point[1]] > Left_Black_Point[Emo_two_third(Right_Down_Point[1],Right_Up_Point[1])]
  108.             && Left_Black_Point[Emo_two_third(Right_Down_Point[1],Right_Up_Point[1])] >  Left_Black_Point[Right_Down_Point[1]]
  109.             && Left_Black_Point[Right_Up_Point[1]] > Left_Black_Point[Right_Down_Point[1]])
  110.         {
  111.             SlalomRight_flag = 1;
  112.             Slalomcount = 1;
  113.             WindupR_flag = 0;
  114.         }
  115.         else if(Right_Up_Point_finish_flag == 1 && Right_Down_Point_finish_flag == 1 &&
  116.                 Left_Up_Point_finish_flag == 0 && Left_Down_Point_finish_flag == 0
  117.             && Right_Down_Point[1] >= 75 && findrcount >= 9 && Right_Up_Point[0] < 158 && Right_Down_Point[1] - Right_Up_Point[1] >= 45
  118.             && Left_Black_Point[Right_Up_Point[1]] > Left_Black_Point[(Right_Up_Point[1]+Right_Down_Point[1])/2]
  119.             && Left_Black_Point[(Right_Up_Point[1]+Right_Down_Point[1])/2] >  Left_Black_Point[Right_Down_Point[1]]
  120.             && Left_Black_Point[Right_Up_Point[1]] > Left_Black_Point[Emo_one_third(Right_Down_Point[1],Right_Up_Point[1])]
  121.             && Left_Black_Point[Emo_one_third(Right_Down_Point[1],Right_Up_Point[1])] >  Left_Black_Point[Right_Down_Point[1]]
  122.             && Left_Black_Point[Right_Up_Point[1]] > Left_Black_Point[Emo_two_third(Right_Down_Point[1],Right_Up_Point[1])]
  123.             && Left_Black_Point[Emo_two_third(Right_Down_Point[1],Right_Up_Point[1])] >  Left_Black_Point[Right_Down_Point[1]]
  124.             && Left_Black_Point[Right_Up_Point[1]] > Left_Black_Point[Right_Down_Point[1]])
  125.         {
  126.             if(findrcount >= 16)
  127.             {
  128.                 CircleBig = 1;
  129.             }
  130.             CircleRight_flag = 1;
  131.             Circlecount = 1;
  132.             WindupR_flag = 0;
  133.         }
  134.         else if(Right_Up_Point_finish_flag == 1 && Right_Down_Point_finish_flag == 1 &&
  135.                 Left_Up_Point_finish_flag == 0 && Left_Down_Point_finish_flag == 0 && Right_Down_Point[1] <= 110 && findrcount <= 17)
  136.         {
  137.             GarageR_Find();
  138.             if(GarageR_Findfinish_flag == 1)
  139.             {
  140.                 GarageR_Findfinish_flag = 0;
  141.                 GarageR_flag = 1;
  142.                 WindupR_flag = 0;
  143.             }
  144.         }
  145.         else if(Right_Down_Point_finish_flag == 0)
  146.         {
  147.             WindupR_flag = 0;
  148.             PaddingR = 0;
  149.             PaddingL = 0;
  150.             Beepindex = 0;
  151.         }
  152.     }
  153. }

因为上下拐点之间是有丢线的，所以我们需要做补线处理。

补线

对于十字或者刚开始的P字和圆环，我们只要补直线就好。

  1. /*******************补线**********************/
  2. //先找到要补充的两条线，通过两点计算斜率得到两点组成的一次方程，得到剩余x的位置，将线换做左右边线
  3. //-------------------------------------------------------------------------------------------------------------------
  4. //  @brief      直线补线函数
  5. //  @param      xone  第一个补线点x坐标
  6. //  @param      yone  第一个补线点y坐标
  7. //  @param      xtwo  第二个补线点x坐标
  8. //  @param      ytwo  第二个补线点y坐标
  9. //  @return     void
  10. //  @note  Left_Black_Point[y]为我的左边线数组 Right_Black_Point[y]为我的右边线数组
  11. //-------------------------------------------------------------------------------------------------------------------
  12. void Padding_LineR(uint8 xone,uint8 yone,uint8 xtwo,uint8 ytwo)
  13. {
  14.     float k=0;  //斜率
  15.     float b=0;  //截距
  16.     //uint8 xstar=0;
  17.     //uint8 xend=0;
  18.     uint8 ystar=0;
  19.     uint8 yend=0;
  20.  
  21.     k=((float)ytwo - (float)yone)/((float)xtwo - (float)xone);
  22.     //k=((float)xtwo - (float)xone)/((float)ytwo - (float)yone);
  23.     b=(float)yone - ((float)xone*k);
  24.  
  25.     if(yone>ytwo)
  26.     {
  27.         ystar=ytwo;
  28.         yend=yone;
  29.     }
  30.     else
  31.     {
  32.         ystar=yone;
  33.         yend=ytwo;
  34.     }
  35.     for(uint8 y=ystar;y<=yend;y++)
  36.     {
  37.         Right_Black_Point[y]=(uint8)(((float)y-b)/k);  //两点之间补线
  38.     }
  39. }

而对于入环和出环这类的控制，我们希望车子运行能够更加顺滑一些，于是我们选择补曲线。当然还需注意开始和结束补线的时机。

  1. //补曲线，利用弯道进行补
  2. //Cx曲线上点x，Cy曲线上点y
  3. //-------------------------------------------------------------------------------------------------------------------
  4. //  @brief      曲线补线函数
  5. //  @param      Ux  上补线点x坐标
  6. //  @param      Uy  上补线点y坐标
  7. //  @param      Dx  下补线点x坐标
  8. //  @param      Dx  下补线点y坐标
  9. //  @return     void
  10. //  @note       利用拉个朗日插值法，上下两个点由图像决定，中间点为固定点，如果补线效果不好，需要重新校准中间点
  11. //  Left_Black_Point[y]为我的左边线数组 Right_Black_Point[y]为我的右边线数组
  12. //-------------------------------------------------------------------------------------------------------------------
  13. void Padding_CurveL(uint8 Ux,uint8 Uy,uint8 Dx,uint8 Dy)
  14. {
  15.     int x0 = 0,x1 = 0,x2 = 0;
  16.     int y0 = 0,y1 = 0,y2 = 0;
  17.  
  18.  
  19.     //0.0096   -2.1047  138.5182
  20.     //0.0103   -2.2049  141.8985
  21.     //x=x0(y-y1)(y-y2)/(y0-y1)(y0-y2)+x1(y-y0)(y-y2)/(y1-y0)(y1-y2)+x2(y-y0)(y-y1)/(y2-y0)(y2-y1)
  22.     x0 = (int)Dx;
  23.     x1 = (int)(Dx + (float)3*(Ux-Dx)/5);
  24.     x2 = (int)Ux;
  25.     y0 = (int)Dy;
  26.     y1 = (int)(Dy - (float)2*(Dy-Uy)/3);
  27.     y2 = (int)Uy;
  28.     if(Ux == 0 && Ux == 0 && Dx == 0 && Dy == 0)
  29.     {
  30.  
  31.     }
  32.     else
  33.     {
  34.         if(Dy > 110)
  35.         {
  36.             Dy = 110;
  37.         }
  38.         //Re = Left_Black_Point[Dy];
  39. //        Left_Black_Point[Dy] = (uint8)(a*(float)(Dy*Dy)+b*(float)Dy+c);
  40.         //error = (int)Re - (int)Left_Black_Point[Dy];
  41.         for(uint8 y = Uy ;y <= Dy;y++)
  42.         {
  43. //            Re = y - Uy + 25;
  44. //            Left_Black_Point[y]=(uint8)(a*(float)(Re*Re)+b*(float)Re+c);
  45.             Left_Black_Point[y]=(uint8)((x0*(y-y1)*(y-y2))/((y0-y1)*(y0-y2)))+((x1*(y-y0)*(y-y2))/((y1-y0)*(y1-y2)))+((x2*(y-y0)*(y-y1))/((y2-y0)*(y2-y1)));
  46.         }
  47.     }
  48.  
  49. }
  50.  
  51. void Padding_CurveR(uint8 Ux,uint8 Uy,uint8 Dx,uint8 Dy)
  52. {
  53.     int x0 = 0,x1 = 0,x2 = 0;
  54.     int y0 = 0,y1 = 0,y2 = 0;
  55.     //0.0096   -2.1047  138.5182
  56.     //0.0103   -2.2049  141.8985
  57.     //x=x0(y-y1)(y-y2)/(y0-y1)(y0-y2)+x1(y-y0)(y-y2)/(y1-y0)(y1-y2)+x2(y-y0)(y-y1)/(y2-y0)(y2-y1)
  58.     x0 = (int)Dx;
  59.     x1 = (int)(Dx - (float)3*(Dx-Ux)/5);
  60.     x2 = (int)Ux;
  61.     y0 = (int)Dy;
  62.     y1 = (int)(Dy - (float)2*(Dy-Uy)/3);
  63.     y2 = (int)Uy;
  64.     if(Ux == 0 && Ux == 0 && Dx == 0 && Dy == 0)
  65.     {
  66.  
  67.     }
  68.     else
  69.     {
  70.         if(Dy > 110)
  71.         {
  72.             Dy = 110;
  73.         }
  74.         for(uint8 y = Uy ;y <= Dy;y++)
  75.         {
  76.             Right_Black_Point[y]=(uint8)((x0*(y-y1)*(y-y2))/((y0-y1)*(y0-y2)))+((x1*(y-y0)*(y-y2))/((y1-y0)*(y1-y2)))+((x2*(y-y0)*(y-y1))/((y2-y0)*(y2-y1)));
  77.         }
  78.     }
  79.  
  80. }

 

 补线的点的确定就需要同学自己去定了，也可以参考我的源码上边的点。为了不让元素的代码很乱，所以我选择在需要补线的地方选定补线的点，并打开补线标志，这样当独列出一个函数用来补线，就不会造成元素的代码里边充斥着补线的代码。不然更改补线方式后，需要对每个补线的程序进行更改。

  1. //-------------------------------------------------------------------------------------------------------------------
  2. //  @brief      补线函数
  3. //  @param      PaddingR  右补线标志  0：不补线 1：要补线
  4. //  @param      PaddingL  左补线标志
  5. //  @param      Padding_CurveR  右曲线补线标志 0：补直线 1：补曲线
  6. //  @param      Padding_CurveL  左曲线补线标志
  7. //  @return     void
  8. //  @note       补线的方式都通过此函数进行，外部只需要做到决定补线点的位置，和是否左右补线
  9. //  Left_Black_Point[y]为我的左边线数组 Right_Black_Point[y]为我的右边线数组
  10. //-------------------------------------------------------------------------------------------------------------------
  11. void Padding_Line(void)
  12. {
  13.     if(PaddingL == 1 && PaddingR == 1 && Paddingcurve == 0)
  14.     {
  15.         Padding_LineR(Link_Right_One_Point[0],Link_Right_One_Point[1],Link_Right_Two_Point[0],Link_Right_Two_Point[1]);
  16.         Padding_LineL(Link_Left_One_Point[0],Link_Left_One_Point[1],Link_Left_Two_Point[0],Link_Left_Two_Point[1]);
  17.     }
  18.     else if(PaddingL == 1 && PaddingR == 0 && Paddingcurve == 0)
  19.     {
  20.         Padding_LineL(Link_Left_One_Point[0],Link_Left_One_Point[1],Link_Left_Two_Point[0],Link_Left_Two_Point[1]);
  21.         Padding_LineR(0,0,0,0);
  22.     }
  23.     else if(PaddingL == 0 && PaddingR == 1 && Paddingcurve == 0)
  24.     {
  25.         Padding_LineR(Link_Right_One_Point[0],Link_Right_One_Point[1],Link_Right_Two_Point[0],Link_Right_Two_Point[1]);
  26.         Padding_LineL(0,0,0,0);
  27.     }
  28.     else if(PaddingL == 0 && PaddingR == 1 && Paddingcurve == 1)
  29.     {
  30.         Padding_CurveR(Link_Right_One_Point[0],Link_Right_One_Point[1],Link_Right_Two_Point[0],Link_Right_Two_Point[1]);
  31.         Padding_CurveL(0,0,0,0);
  32.     }
  33.     else if(PaddingL == 1 && PaddingR == 0 && Paddingcurve == 1)
  34.     {
  35.         Padding_CurveL(Link_Left_One_Point[0],Link_Left_One_Point[1],Link_Left_Two_Point[0],Link_Left_Two_Point[1]);
  36.         Padding_CurveR(0,0,0,0);
  37.     }
  38.     else
  39.     {
  40.         Padding_LineL(0,0,0,0);
  41.         Padding_LineR(0,0,0,0);
  42.     }
  43. }

 最后

如果有对本文章有建议或者看法的话，可以评论或者私信我，希望可以和各位多多交流。本文章中的代码已经开源，开源地址：GitHub - StingeMoZGD/17th-SmartCar: 17届智能车竞赛，无线充电组

声明：本文内容由网友自发贡献，不代表【wpsshop博客】立场，版权归原作者所有，本站不承担相应法律责任。如您发现有侵权的内容，请联系我们。转载请注明出处：https://www.wpsshop.cn/w/小蓝xlanll/article/detail/476810
推荐阅读
相关标签

Copyright © 2003-2013 www.wpsshop.cn 版权所有，并保留所有权利。

  

闽ICP备14008679号