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

十六届智能车全向组算法开源（四）-圆环处理_lcd_showint

作者：花生_TL007 | 2024-04-07 22:16:31

赞

踩

lcd_showint




系列文章目录

之前一段时间在处理手头的一些事情，将算法开源系列搁置了一段时间（主要是懒了）。之前介绍了预处理，边线，拐点等，这一章主要介绍环岛。

算法开源系列（三）

算法开源系列（二）

算法开源系列（一）

硬件开源系列（二）

硬件开源系列（一）




前言

         在我比赛的算法，通常在处理元素之前，需要具备良好的图像边界，拐点，边界起始行，边界截止行等主要条件。之前系列文章介绍过了边界，和拐点。有人留言提到过拐点的程序，其实最简单的方法，就是连续几行的边界做差，差值满足条件就判断为拐点。

        圆环！！这个在我做车三年一直都是噩梦存在的元素，虽然16届增加了新元素三岔，我依然在圆环花费了最多的时间。因为今年选用的是摄像头入环，所以在处理圆环的时候，不仅仅要考虑如何识别，还需要在入环，环内进行补线处理。于是我在处理圆环大致分为识别，入环切线，入环 ，环内，出环，出环切线，完全出环这几个阶段。

  1. void R_uppoint( unsigned char Row_START,unsigned char Row_END ,unsigned char Col_START,unsigned char Col_END,unsigned char Edge_ROW)
  2. {
  3.   int Col;
  4.   //uint8 N_P=0;
  5.   int C1=0;
  6.   int Row=Edge_ROW;
  7.  
  8.     for(Row=Edge_ROW;Row>Edge_ROW-7 && Row>ROW_START+1;Row--)//从本行开始向上扫10行
  9.     {
  10.       for(Col=Col_END-2;Col>15;Col--)//初始值固定
  11.       {
  12.         if(difference_sum(image[Row][Col-COL_SPACE],image[Row][Col])>=g_threshold_value)//检测到跳变点
  13.         {
  14.           if( abs(Col-COL_SPACE-C1)<2&& Col-COL_SPACE<=C1)//如果两列差值小于3，说明C为拐点
  15.           {
  16.             Inflection_point.R_up_point.x=Row;
  17.             Inflection_point.R_up_point.y=C1;
  18.             return;
  19.            }
  20.           C1=Col-COL_SPACE;
  21.           break;
  22.  
  23.       }
  24.     }
  25.  }
  26. }




一、起始行和截止行

顾名思义吧，起始行就是第一次扫到边界的那一行，截止行是最后一次找到边界的行。这两个特征代表了边界的可用范围。

二、圆环识别：

      圆环的特征还是相当明显，一边边线，连续，一边边线，缺失。这里我采用的方式是在识别到一边拐点存在，一边拐点不存在，并且存在的拐点距离最远端很近，这里我以当拐点距离最远行小于5行为标准。当存在这些条件后，再进行拐点上面几行边线缺失判断。以左环为例，当左边存在拐点，并且拐点上方有三行不存在边线。就行下面的右边线计数，当右边先满足远小近大的条件，并且连续存在13行边线就满足圆环的识别。这种识别是很稳定的，但由于车库的存在通常会与车库误识别，所以对于两者区别还需要想办法解决。

  1.              round->Entrance_count=0;//圆环入口计数清零
  2.             if(point->L_down_point.x+5>ROW_END-1)
  3.                 Round_row=ROW_END-1;
  4.              else
  5.                 Round_row=point->L_down_point.x+5;
  6.              for(;Round_row>point->L_down_point.x-10&&Round_row>=ROW_START;Round_row--)//从左拐点下面5行到上面10行进行搜索
  7.              {
  8.                 if(str->RightEdge[Round_row+1]!=0&&str->RightEdge[Round_row]!=0)
  9.                 {
  10.                    //如果右边正常递减，且不丢线     斜率符合要求
  11.                    if(str->RightEdge[Round_row+1]-str->RightEdge[Round_row]>=0 &&str->RightEdge[Round_row+1]-str->RightEdge[Round_row]<=2)
  12.                    {
  13.                       round->Entrance_count++; //圆环入口计数增加
  14.                    }
  15.                 }
  16.              }
  17.              if(round->Entrance_count>=13)//min修改，其中有13行满足条件即可判定为圆环
  18.              {
  19.                 round->Round_flag=1;
  20.                 RoundType=Left_Round;
  21.                 RoundProcess=Find_Round;
  22.  
  23.                  RoadType= Round_Road;
  24.              //    BlueRing;
  25.                 //RoundSize=Large_Round;   //待定
  26.             }

进环前处理：

我们在识别到环的时候，是在环外的，所以需要保持一段距离直行到环的切点A。我的方法是通过 B点之前的斜率进行补线。    

  1.                    for(Round_row=ROW_END;Round_row>=ROW_START;Round_row--)
  2.                    {
  3.                       if(str->RightEdge[Round_row])
  4.                       {
  5.                           char temp_r;
  6.                           char temp_l;
  7.                           char track;
  8.                           temp_r=str->RightEdge[Round_row];
  9.                           track=Boundary.track_Width[Round_row];
  10.                           temp_l=temp_r-track-1;
  11.                           if(temp_l>=79)
  12.                           {
  13.                               temp_l=78;
  14.  
  15.                           }
  16.                           if(temp_l<=1)
  17.                           {
  18.                               temp_l=1;
  19.  
  20.                           }
  21.                           str->LeftEdge[Round_row]=temp_l;
  22.                         // str->LeftEdge[Round_row]=str->RightEdge[Round_row]-str->track_Width[Round_row];//左边界等于右边界减半宽
  23.                       }
  24.                       else
  25.                       {
  26.                          str->LeftEdge[Round_row]=0;
  27.                       }
  28.                    }

 进环处理：

对于进环，需要将右边线通过补线方式补成一个类似与弯道的情况。然后通过陀螺仪积分判断旋转的角度，当角度满足车身已经进入到环里的条件，就可以正常的巡线走了。

 

  1. case Into_Round://进环    关键 分两步
  2.                 {
  3.            // lcd_showint8(110,6, 2);
  4.                    /*进环的补线处理*/
  5.                  if(!Inflection_point.L_down_point.x&&!Inflection_point.L_up_point.x&&str->LeftEdge[ROW_END-5])//&&str->LeftEdge[ROW-4])
  6.                  // if(Inflection_point.L_up_point.x==0&&Inflection_point.L_down_point.x&&str->LeftEdge[ROW_END-3])
  7. //                  if(Inflection_point.L_up_point.x&&(left_ad>=220||right_ad>200)&&!str->LeftEdge[ROW_END-1])
  8.                  {
  9.  
  10.                       RoundProcess=Into_Round2;
  11.                     }
  12.  
  13.                   else
  14.                    {      //获取补线点信息
  15.                       if(point->L_down_point.x>5) //记录上一次拐点存在的信息
  16.                       {
  17.                          Inflection_Temp_x=point->L_down_point.x;
  18.                          Inflection_Temp_y=point->L_down_point.y;
  19.                       }
  20.                       //得到信息后，开始补线
  21.                       for(uint8 i=Inflection_Temp_x;i>2;i--)
  22.                       {
  23.                          if(image[i][Inflection_Temp_y]>=Threshold&&image[i-1][Inflection_Temp_y]<Threshold)
  24.                          {
  25.                             LeftLoopTemp_R=i-1;
  26.                             str->EndLine=i-1;
  27.                             break;
  28.                          }
  29.                       }
  30.                       for(uint8 i=Inflection_Temp_y;i<75;i++)
  31.                       {
  32.                          if((image[LeftLoopTemp_R-1][i+1]>=Threshold)&&(image[LeftLoopTemp_R-1][i]<Threshold))
  33.                          {
  34.                             LeftLoopTemp_C=i;
  35.  
  36.                             break;
  37.                          }
  38.                       }
  39.  
  40.                       /*进环斜率*/
  41.                       LeftLoopSlope=1.3;//*(str->RightEdge[aa1]-aa2)/(aa1-LeftLoopTemp_R);
  42.  
  43.                       for(uint8 i=0;i<=28;i++)
  44.                       {
  45.                          str->RightEdge[LeftLoopTemp_R+i]=(int)(i*LeftLoopSlope+LeftLoopTemp_C);
  46.  
  47.                        str->RightEdge[LeftLoopTemp_R+i]=int8_range_protect(str->RightEdge[LeftLoopTemp_R+i],1,79) ;
  48.  
  49.                          if(str->RightEdge[LeftLoopTemp_R+i]>78||LeftLoopTemp_R+i>ROW_END)
  50.                             break;
  51.                       }
  52.                    }
  53.  
  54.                    for(uint8 i=str->EndLine;i<ROW_END;i++)
  55.                    {
  56.                       for(uint8 j=1;j<=79;j++)
  57.                       {
  58.                          if(image[i][j]>=Threshold&&image[i][j+1]<Threshold)
  59.                          {
  60.                             if(str->RightEdge[i]>j+1)
  61.                                    str->RightEdge[i]=j+1;
  62.                             break;
  63.                          }
  64.                       }
  65.                    }
  66.                    break;
  67.                 }
  68.               case Into_Round2://进环    关键 4
  69.                 {
  70.             //  lcd_showint8(110,6, 3);
  71.                    /*判断是否进入环岛*/
  72.                    if(angle_z<=-40)//&&str->L_StartLine==0&& RoundSize==Small_Round)      //定距离一段
  73.                    {
  74.                       RoundProcess=In_Round;
  75.                       Inflection_Temp_x=0;
  76.                       Inflection_Temp_y=0;
  77.                       //lcd_showint8(80,5, 4);
  78.                    }
  79.  
  80. #if 1                                 //补线
  81.                    for(uint8 i=str->EndLine;i<=ROW_END;i++)
  82.                    {
  83.                       for(uint8 j=1;j<=75;j++)
  84.                       {
  85.                          if(image[i][j]>=Threshold&&image[i][j+1]<Threshold&&image[i][j+2]<Threshold&&image[i][j+3]<Threshold)
  86.                         //if (difference_sum(image[i][j],image[i][j-COL_SPACE])>=g_threshold_value)
  87.                          {
  88.                             if(str->RightEdge[i]>j+1)
  89.                                    str->RightEdge[i]=j+1;
  90.  
  91.                             break;
  92.                          }
  93.                       }
  94.                    }
  95.  
  96.  
  97.                    /*进环斜率*/
  98.                    LeftLoopSlope=2.0;
  99.                    for(uint8 i=ROW_END;i>ROW_START;i--)
  100.                    {
  101.                      //  lcd_showint8(80,5, 5);
  102.                       if(str->RightEdge[i]!=0)
  103.                          str->RightEdge[i]=str->RightEdge[i]<(int)(LeftLoopSlope*i+5)?str->RightEdge[i]:(int)(LeftLoopSlope*i+5);
  104.                       else
  105.                          str->RightEdge[i]=(int)(LeftLoopSlope*i+5);
  106.                       if(str->RightEdge[i]>79)
  107.                          str->RightEdge[i]=0;
  108.                    }
  109.                    for(uint8 i=29;i>str->EndLine&&i>ROW_START+2;i--)
  110.                    {
  111.                        //lcd_showint8(80,5, 6);
  112.                       if(image[i][10]>=Threshold&&image[i-1][10]<Threshold&&image[i-2][10]<Threshold)
  113.                       //if (difference_sum(image[i-2][10],image[i][10])>=g_threshold_value)
  114.                       {
  115.                          for(uint8 j=i;j>str->EndLine&&j>ROW_START;j--)
  116.                          {
  117.                             str->LeftEdge[j]=0;
  118.                          }
  119.                          str->EndLine=i-1;        //改变endline
  120.                          break;
  121.                       }
  122.                       if(str->LeftEdge[i]>30)
  123.                       {
  124.                          str->LeftEdge[i]=0;   //限幅
  125.                       }
  126.                    }
  127. #endif
  128.                    break;
  129.                 }

 出环处理：

在出环时，由于左边线不存在，因此也需要进行补线操作。目的就是为了出环时车转弯方向是正确的，

 

  1.  case Leave_Round://离开圆环
  2.                 {
  3.                  //  lcd_showint8(110,4, 6);
  4.                    Round_num=0;
  5.                    for(Round_row=str->R_StartLine;Round_row>str->EndLine+1&&Round_row>ROW_START+10;Round_row--)
  6.                    {
  7.                       if(str->RightEdge[Round_row]==0)
  8.                       {
  9.                         Round_num++;
  10.                       }
  11.                    }
  12.                    if(Round_num<3&&((str->R_StartLine-str->EndLine)>22||str->RightEdge[str->EndLine]>18))  //30需要调  huo 判断斜率
  13.                    {
  14.                       RoundProcess=Leave_Round2;//暂定
  15.                    }
  16.                    else
  17.                    {
  18.                       LeftLoopSlope=2.5;
  19.                       for(uint8 i=29;i>5;i--)
  20.                       {
  21.                          if(str->RightEdge[i]!=0)
  22.                             str->RightEdge[i]=str->RightEdge[i]<(unsigned char)(LeftLoopSlope*i+5)?str->RightEdge[i]:(unsigned char)(LeftLoopSlope*i+5);
  23.                          else
  24.                             str->RightEdge[i]=(unsigned char)(LeftLoopSlope*i+5);
  25.                          if(str->RightEdge[i]>79)
  26.                             str->RightEdge[i]=0;
  27.                       }
  28.  
  29.                       for(uint8 i=29;i>str->EndLine;i--)    //消除左线误搜索
  30.                       {
  31.                          if(str->LeftEdge[i]>40)
  32.                          {
  33.                             str->LeftEdge[i]=0;
  34.                          }
  35.                          if(i<29&&i>ROW_START)
  36.                          {
  37.                             if(str->LeftEdge[i]!=0&&str->LeftEdge[i-1]==0&&str->LeftEdge[i+1]==0)
  38.                                    str->LeftEdge[i]=0;
  39.                          }
  40.                       }
  41.                    }
  42.                    break;
  43.                 }

出环后处理：

在出环后，由于环岛元素存在的缺口，仍需要补线进行引导。也是通过斜率来进行补线。

 

  1.   Round_num=0;
  2.                    for(Round_row=str->R_StartLine;Round_row>str->EndLine+1&&Round_row>ROW_START+10;Round_row--)
  3.                    {
  4.                       if(str->RightEdge[Round_row]==0)
  5.                       {
  6.                         Round_num++;
  7.                       }
  8.                    }
  9.                    if(Round_num<3&&((str->R_StartLine-str->EndLine)>22||str->RightEdge[str->EndLine]>18))  //30需要调  huo 判断斜率
  10.                    {
  11.                       RoundProcess=Leave_Round2;//暂定
  12.                    }
  13.                    else
  14.                    {
  15.                       LeftLoopSlope=2.5;
  16.                       for(uint8 i=29;i>5;i--)
  17.                       {
  18.                          if(str->RightEdge[i]!=0)
  19.                             str->RightEdge[i]=str->RightEdge[i]<(unsigned char)(LeftLoopSlope*i+5)?str->RightEdge[i]:(unsigned char)(LeftLoopSlope*i+5);
  20.                          else
  21.                             str->RightEdge[i]=(unsigned char)(LeftLoopSlope*i+5);
  22.                          if(str->RightEdge[i]>79)
  23.                             str->RightEdge[i]=0;
  24.                       }
  25.  
  26.                       for(uint8 i=29;i>str->EndLine;i--)    //消除左线误搜索
  27.                       {
  28.                          if(str->LeftEdge[i]>40)
  29.                          {
  30.                             str->LeftEdge[i]=0;
  31.                          }
  32.                          if(i<29&&i>ROW_START)
  33.                          {
  34.                             if(str->LeftEdge[i]!=0&&str->LeftEdge[i-1]==0&&str->LeftEdge[i+1]==0)
  35.                                    str->LeftEdge[i]=0;
  36.                          }
  37.                       }
  38.                    }

之后当车行过缺口就可以恢复正常巡线，这里可以积路程，简单一点，也可以采用起始行是否在图像最底端来判断。

最后

底下附上完整过程代码，以便大家推导理解，对于代码，不建议直接拿去用，因为图像与摄像头参数都不一样。可以根据上面提到的思路，自己写一遍。分阶段来实现进环，至少这套方法我在圆环这里基本没出现过问题，除了速度快了，麦轮没办法很及时的转弯进去。

多嘴一句，这里的方法也不是最好的，但之于我是很有效的。可能也有更快，更好，更丝滑的进环算法。我就充当一个抛砖引玉的角色，希望能帮助没有思路的车友有一种简单有效的方法去实现进环。这一章也是自己用手画的图，在我看来在调试元素的时候，趴赛道才是最好的方法。需要各位仔细琢磨，不断完善，才可以有适合自己的算法。

  1. void Round_Process(Str_Boundary *str,Str_Round *round,Str_Inflection_point *point)
  2. {
  3.     float  LeftLoopSlope;
  4.     float  RightLoopSlope;
  5.     uint8 break_point_num;
  6.     uint8 many_bp_row;
  7.     uint8 white_point_num;
  8.  
  9.     uint8  Round_num=0;
  10.     break_point_num=0;
  11.        switch(RoundType)
  12.        {
  13.         case Left_Round:  //左环
  14.           {
  15.              switch(RoundProcess)
  16.              {
  17.               case Find_Round://发现圆环
  18.                 {
  19.                 //    mark_garage();
  20.           //        lcd_showint8(110,4, 0);
  21.                     for(uint8 i=29;i>=0&&i>Boundary.EndLine;i--)
  22.                        {
  23.                             if( Boundary.LeftEdge[i]==0 && Boundary.RightEdge[i] )
  24.                              {
  25.                                for(uint8 j=Boundary.RightEdge[i];j-1>5;j--)
  26.                                {
  27.                                    if(mark_middle_num(image[i][j],image[i][j-1],Threshold)
  28.                                     && abs( difference_sum(image[i][j],image[i][j-1]) )>=g_threshold_value-10  )//跳变
  29.                                  {
  30.                                       break_point_num++;
  31.                                    if(break_point_num>7 )
  32.                                    {
  33.                                        many_bp_row++;
  34.                                      break;
  35.                                    }
  36.                                  }
  37.                                }
  38.  
  39.                              }
  40.  
  41.                            if(many_bp_row>3)//黑白跳变点数 90
  42.                            {
  43.                                round->Round_flag=0;
  44.                                RoadType= else_road;
  45.                                   return;
  46.                             }
  47.                        }
  48.   //                  lcd_showint8(120,5,many_bp_row);
  49.                    //*进入下个阶段的判定*/
  50.                    if(str->L_StartLine<25)   //da待改进
  51.                    {
  52.                       Round_num=0;
  53.                       for(Round_row=str->L_StartLine;Round_row>=str->L_StartLine-4&&Round_row>=ROW_START+1;Round_row--)   //防止误判     检测换中心岛
  54.                       {
  55.                          if(str->LeftEdge[Round_row]!=0)
  56.                          {
  57.                             Round_num+=str->LeftEdge[Round_row]-str->LeftEdge[Round_row+1] ;
  58.                          }
  59.                          else
  60.                          {
  61.                             Round_num=0;
  62.                          }
  63.                       }
  64.                       if(Round_num>10)
  65.                       {
  66.                          Round_num=0;
  67.                          RoundProcess=Find_Gap;//就认为有缺口
  68.                      //   lcd_showint8(80,1, 1);
  69.                       }
  70.                    }
  71.                    //*入环的补线*/
  72.                    for(Round_row=ROW_END;Round_row>=ROW_START;Round_row--)
  73.                    {
  74.                       if(str->RightEdge[Round_row])
  75.                       {
  76.                           char temp_r;
  77.                           char temp_l;
  78.                           char track;
  79.                           temp_r=str->RightEdge[Round_row];
  80.                           track=Boundary.track_Width[Round_row];
  81.                           temp_l=temp_r-track-1;
  82.                           if(temp_l>=79)
  83.                           {
  84.                               temp_l=78;
  85.  
  86.                           }
  87.                           if(temp_l<=1)
  88.                           {
  89.                               temp_l=1;
  90.  
  91.                           }
  92.                           str->LeftEdge[Round_row]=temp_l;
  93.                         // str->LeftEdge[Round_row]=str->RightEdge[Round_row]-str->track_Width[Round_row];//左边界等于右边界减半宽
  94.                       }
  95.                       else
  96.                       {
  97.                          str->LeftEdge[Round_row]=0;
  98.                       }
  99.                    }
  100.                    break;
  101.                 }
  102.               case Find_Gap://发现圆环中心
  103.                 {
  104.              //     lcd_showint8(110,6, 1);
  105.                    /*下一阶段的判定*/
  106.                    //底部三行左边界都不丢线
  107.                 //  if(str->LeftEdge[ROW_END-2]&&str->LeftEdge[ROW_END-1]&&str->LeftEdge[ROW_END-3])//&&str->LeftEdge[ROW_END-2]
  108.                //  if(Inflection_point.L_up_point.x&&Inflection_point.L_down_point.x&&(Inflection_point.L_down_point.x-Inflection_point.L_up_point.x)<15)
  109.                   // if(str->LeftEdge[ROW_END-2]&&str->LeftEdge[ROW_END-1]&&str->LeftEdge[ROW_END-3]&&Inflection_point.L_up_point.x)
  110.                  //   mark_garage();
  111.                     //   if(str->LeftEdge[ROW_END-1]-str->LeftEdge[ROW_END-2]>0&&str->LeftEdge[ROW_END-2]-str->LeftEdge[ROW_END-3]>0)
  112. //                    for(uint8 i=29;i>=0&&i>Boundary.EndLine;i--)
  113. //                       {
  114. //
  115. //
  116.                             if( Boundary.LeftEdge[i]==0 && Boundary.RightEdge[i] )
  117.                              {
  118. //                               for(uint8 j=Boundary.RightEdge[i];j-1>COL_END-10;j++)
  119. //                               {
  120. //                                   if(mark_middle_num(image[i][j],image[i][j-1],Threshold)
  121. //                                    && abs( difference_sum(image[i][j],image[i][j-1]) )>=g_threshold_value-10  )//跳变
  122. //                                 {
  123. //                                       break_point_num++;
  124. //                                   if(break_point_num>7 )
  125. //                                   {
  126. //                                       many_bp_row++;
  127. //                                     break;
  128. //                                   }
  129. //                                 }
  130. //                               }
  131.  
  132.                              }
  133. //
  134. //                           if(many_bp_row>2)//黑白跳变点数 90
  135. //                           {
  136. //                               round->Round_flag=0;
  137. //                               RoadType= else_road;
  138. //                                  return;
  139. //                            }
  140. //                       }
  141.  
  142.                     if(e_distance>1000)
  143.                      {
  144.                       if(Inflection_point.L_up_point.x)
  145.                       {
  146.  
  147.                           RoundProcess=Into_Round;
  148.                       }
  149.                      }
  150.  
  151.                    /*左补线*/
  152.                    for(Round_row=ROW_END;Round_row>=ROW_START;Round_row--)
  153.                    {
  154.                       if(str->RightEdge[Round_row])
  155.                       {
  156.  //                       str->LeftEdge[Round_row]=str->RightEdge[Round_row]-str->track_Width[Round_row];//左边界等于右边界减半宽
  157.                           char temp_r;
  158.                           char temp_l;
  159.                           char track;
  160.                           temp_r=str->RightEdge[Round_row];
  161.                           track=Boundary.track_Width[Round_row];
  162.                           temp_l=temp_r-track;
  163.                           if(temp_l>=79)
  164.                           {
  165.                               temp_l=78;
  166.  
  167.                           }
  168.                           if(temp_l<=1)
  169.                           {
  170.                               temp_l=1;
  171.  
  172.                           }
  173.                           str->LeftEdge[Round_row]=temp_l;
  174.                       }
  175.                       else
  176.                       {
  177.                          str->LeftEdge[Round_row]=0;
  178.                       }
  179.                    }
  180.                    break;
  181.                 }
  182.               case Into_Round://进环    关键 分两步
  183.                 {
  184.            // lcd_showint8(110,6, 2);
  185.                    /*进环的补线处理*/
  186.                  if(!Inflection_point.L_down_point.x&&!Inflection_point.L_up_point.x&&str->LeftEdge[ROW_END-5])//&&str->LeftEdge[ROW-4])
  187.                  // if(Inflection_point.L_up_point.x==0&&Inflection_point.L_down_point.x&&str->LeftEdge[ROW_END-3])
  188. //                  if(Inflection_point.L_up_point.x&&(left_ad>=220||right_ad>200)&&!str->LeftEdge[ROW_END-1])
  189.                  {
  190.  
  191.                       RoundProcess=Into_Round2;
  192.                     }
  193.  
  194.                   else
  195.                    {      //获取补线点信息
  196.                       if(point->L_down_point.x>5) //记录上一次拐点存在的信息
  197.                       {
  198.                          Inflection_Temp_x=point->L_down_point.x;
  199.                          Inflection_Temp_y=point->L_down_point.y;
  200.                       }
  201.                       //得到信息后，开始补线
  202.                       for(uint8 i=Inflection_Temp_x;i>2;i--)
  203.                       {
  204.                          if(image[i][Inflection_Temp_y]>=Threshold&&image[i-1][Inflection_Temp_y]<Threshold)
  205.                          {
  206.                             LeftLoopTemp_R=i-1;
  207.                             str->EndLine=i-1;
  208.                             break;
  209.                          }
  210.                       }
  211.                       for(uint8 i=Inflection_Temp_y;i<75;i++)
  212.                       {
  213.                          if((image[LeftLoopTemp_R-1][i+1]>=Threshold)&&(image[LeftLoopTemp_R-1][i]<Threshold))
  214.                          {
  215.                             LeftLoopTemp_C=i;
  216.  
  217.                             break;
  218.                          }
  219.                       }
  220.  
  221.                       /*进环斜率*/
  222.                       LeftLoopSlope=1.3;//*(str->RightEdge[aa1]-aa2)/(aa1-LeftLoopTemp_R);
  223.  
  224.                       for(uint8 i=0;i<=28;i++)
  225.                       {
  226.                          str->RightEdge[LeftLoopTemp_R+i]=(int)(i*LeftLoopSlope+LeftLoopTemp_C);
  227.  
  228.                        str->RightEdge[LeftLoopTemp_R+i]=int8_range_protect(str->RightEdge[LeftLoopTemp_R+i],1,79) ;
  229.  
  230.                          if(str->RightEdge[LeftLoopTemp_R+i]>78||LeftLoopTemp_R+i>ROW_END)
  231.                             break;
  232.                       }
  233.                    }
  234.  
  235.                    for(uint8 i=str->EndLine;i<ROW_END;i++)
  236.                    {
  237.                       for(uint8 j=1;j<=79;j++)
  238.                       {
  239.                          if(image[i][j]>=Threshold&&image[i][j+1]<Threshold)
  240.                          {
  241.                             if(str->RightEdge[i]>j+1)
  242.                                    str->RightEdge[i]=j+1;
  243.                             break;
  244.                          }
  245.                       }
  246.                    }
  247.                    break;
  248.                 }
  249.               case Into_Round2://进环    关键 4
  250.                 {
  251.             //  lcd_showint8(110,6, 3);
  252.                    /*判断是否进入环岛*/
  253.                    if(angle_z<=-40)//&&str->L_StartLine==0&& RoundSize==Small_Round)      //定距离一段
  254.                    {
  255.                       RoundProcess=In_Round;
  256.                       Inflection_Temp_x=0;
  257.                       Inflection_Temp_y=0;
  258.                       //lcd_showint8(80,5, 4);
  259.                    }
  260.  
  261. #if 1                                 //补线
  262.                    for(uint8 i=str->EndLine;i<=ROW_END;i++)
  263.                    {
  264.                       for(uint8 j=1;j<=75;j++)
  265.                       {
  266.                          if(image[i][j]>=Threshold&&image[i][j+1]<Threshold&&image[i][j+2]<Threshold&&image[i][j+3]<Threshold)
  267.                         //if (difference_sum(image[i][j],image[i][j-COL_SPACE])>=g_threshold_value)
  268.                          {
  269.                             if(str->RightEdge[i]>j+1)
  270.                                    str->RightEdge[i]=j+1;
  271.  
  272.                             break;
  273.                          }
  274.                       }
  275.                    }
  276.  
  277.  
  278.                    /*进环斜率*/
  279.                    LeftLoopSlope=2.0;
  280.                    for(uint8 i=ROW_END;i>ROW_START;i--)
  281.                    {
  282.                      //  lcd_showint8(80,5, 5);
  283.                       if(str->RightEdge[i]!=0)
  284.                          str->RightEdge[i]=str->RightEdge[i]<(int)(LeftLoopSlope*i+5)?str->RightEdge[i]:(int)(LeftLoopSlope*i+5);
  285.                       else
  286.                          str->RightEdge[i]=(int)(LeftLoopSlope*i+5);
  287.                       if(str->RightEdge[i]>79)
  288.                          str->RightEdge[i]=0;
  289.                    }
  290.                    for(uint8 i=29;i>str->EndLine&&i>ROW_START+2;i--)
  291.                    {
  292.                        //lcd_showint8(80,5, 6);
  293.                       if(image[i][10]>=Threshold&&image[i-1][10]<Threshold&&image[i-2][10]<Threshold)
  294.                       //if (difference_sum(image[i-2][10],image[i][10])>=g_threshold_value)
  295.                       {
  296.                          for(uint8 j=i;j>str->EndLine&&j>ROW_START;j--)
  297.                          {
  298.                             str->LeftEdge[j]=0;
  299.                          }
  300.                          str->EndLine=i-1;        //改变endline
  301.                          break;
  302.                       }
  303.                       if(str->LeftEdge[i]>30)
  304.                       {
  305.                          str->LeftEdge[i]=0;   //限幅
  306.                       }
  307.                    }
  308. #endif
  309.                    break;
  310.                 }
  311.               case In_Round://在环内    5
  312.                 {
  313.                // lcd_showint8(110,4,4);
  314.                    //记录上一次拐点存在的信息
  315.                    if(point->R_down_point.x&&image[point->R_down_point.x][point->R_down_point.y-2]>=Threshold&&image[point->R_down_point.x][point->R_down_point.y-3]>=Threshold&&image[point->R_down_point.x-1][point->R_down_point.y]>=Threshold&&image[point->R_down_point.x-2][point->R_down_point.y]>=Threshold)
  316.                    {
  317.                       Inflection_Temp_x=point->R_down_point.x;
  318.                       Inflection_Temp_y =point->R_down_point.y;
  319.                       //判断下一阶段
  320.                       if(point->R_down_point.x>5)
  321.                       {
  322.                          uint8 hang_num=0;            //清除白块计数值 8
  323.                          for(Round_row=point->R_down_point.x-1; Round_row>=point->R_down_point.x-6&&Round_row>ROW_START ; Round_row-- )     //在搜索到的黑白跳变点后，向上搜索白点，判断是否为干扰点
  324.                          {
  325.                             if(str->RowLose[Round_row]==4)
  326.                             {
  327.                                    hang_num++;
  328.                             }
  329.                          }
  330.                          if( hang_num>2|| point->R_down_point.x>20||Inflection_point.R_down_point.y>30)
  331.                          {
  332.                             RoundProcess=Out_Round;
  333.                            // lcd_showint8(110,4, 4);
  334.                          }
  335.                       }
  336.                    }
  337.  
  338.                    if(Inflection_Temp_x>0)
  339.                    {
  340.                       //补线
  341.                       LeftLoopSlope=2.0;
  342.                       for(uint8 i=Inflection_Temp_x+1;i>ROW_START;i--)
  343.                       {
  344.  
  345.                          str->RightEdge[i]=(int)((i-Inflection_Temp_x)*LeftLoopSlope+Inflection_Temp_y);    //fuhao
  346.                          if (str->RightEdge[i]<COL_START)
  347.                          {
  348.                             str->EndLine =i;
  349.                             break;
  350.                          }
  351.                       }
  352.                       //str->StartLine=ROW_END;
  353.                       //memset(str->LeftEdge, 0,sizeof(str->LeftEdge) );   //z左线清空
  354.                    }
  355.                    for(uint8 i=29;i>str->EndLine;i--)    //消除左线误搜索
  356.                    {
  357.                       if(str->LeftEdge[i]>30)
  358.                       {
  359.                          str->LeftEdge[i]=0;
  360.                       }
  361.                    }
  362.                    break;
  363.                 }
  364.  
  365.               case Out_Round://出环   6
  366.                 {
  367.                //     BlueRing;
  368.                //   lcd_showint8(110,4, 5);
  369.                    LeftLoopSlope=2.1;
  370.                    for(uint8 i=29;i>5;i--)//取小值
  371.                    {
  372.                       if(str->RightEdge[i]!=0)
  373.                          str->RightEdge[i]=str->RightEdge[i]<(unsigned char)(LeftLoopSlope*i+7)?str->RightEdge[i]:(unsigned char)(LeftLoopSlope*i+7);
  374.                       else
  375.                          str->RightEdge[i]=(unsigned char)(LeftLoopSlope*i+7);
  376.                       if(str->RightEdge[i]>79)
  377.                          str->RightEdge[i]=0;
  378.                    }
  379. //                 str->StartLine=ROW_END;
  380. //                 memset(str->LeftEdge, 0,sizeof(str->LeftEdge) );
  381. //
  382.                    if(angle_z<-85)   //下一阶段标志
  383.                    {
  384.                       RoundProcess=Leave_Round;
  385.                    }
  386.                    for(uint8 i=29;i>str->EndLine;i--)    //消除左线误搜索
  387.                    {
  388.                       if(str->LeftEdge[i]>35)
  389.                       {
  390.                          str->LeftEdge[i]=0;
  391.                       }
  392.                       if(i<29&&i>ROW_START)
  393.                       {
  394.                          if(str->LeftEdge[i]!=0&&str->LeftEdge[i-1]==0&&str->LeftEdge[i+1]==0)
  395.                             str->LeftEdge[i]=0;
  396.                       }
  397.                    }
  398.                    /*下一阶段的判定*/
  399.  
  400.                    break;
  401.                 }
  402.               case Leave_Round://离开圆环
  403.                 {
  404.                  //  lcd_showint8(110,4, 6);
  405.                    Round_num=0;
  406.                    for(Round_row=str->R_StartLine;Round_row>str->EndLine+1&&Round_row>ROW_START+10;Round_row--)
  407.                    {
  408.                       if(str->RightEdge[Round_row]==0)
  409.                       {
  410.                         Round_num++;
  411.                       }
  412.                    }
  413.                    if(Round_num<3&&((str->R_StartLine-str->EndLine)>22||str->RightEdge[str->EndLine]>18))  //30需要调  huo 判断斜率
  414.                    {
  415.                       RoundProcess=Leave_Round2;//暂定
  416.                    }
  417.                    else
  418.                    {
  419.                       LeftLoopSlope=2.5;
  420.                       for(uint8 i=29;i>5;i--)
  421.                       {
  422.                          if(str->RightEdge[i]!=0)
  423.                             str->RightEdge[i]=str->RightEdge[i]<(unsigned char)(LeftLoopSlope*i+5)?str->RightEdge[i]:(unsigned char)(LeftLoopSlope*i+5);
  424.                          else
  425.                             str->RightEdge[i]=(unsigned char)(LeftLoopSlope*i+5);
  426.                          if(str->RightEdge[i]>79)
  427.                             str->RightEdge[i]=0;
  428.                       }
  429.  
  430.                       for(uint8 i=29;i>str->EndLine;i--)    //消除左线误搜索
  431.                       {
  432.                          if(str->LeftEdge[i]>40)
  433.                          {
  434.                             str->LeftEdge[i]=0;
  435.                          }
  436.                          if(i<29&&i>ROW_START)
  437.                          {
  438.                             if(str->LeftEdge[i]!=0&&str->LeftEdge[i-1]==0&&str->LeftEdge[i+1]==0)
  439.                                    str->LeftEdge[i]=0;
  440.                          }
  441.                       }
  442.                    }
  443.                    break;
  444.                 }
  445.               case Leave_Round2://离开圆环 7
  446.                 {
  447.                    //可以拉长视野
  448.                //   lcd_showint8(110,4, 7);
  449.                    for(Round_row=ROW_END;Round_row>=ROW_START;Round_row--)
  450.                    {
  451.                       if(str->RightEdge[Round_row])
  452.                       {
  453.                        //  str->LeftEdge[Round_row]=str->RightEdge[Round_row]-str->track_Width[Round_row];//左边界等于右边界减半宽
  454.                           //if(str->RightEdge[Round_row])
  455.                          char temp_r;
  456.                          char temp_l;
  457.                          char track;
  458.                          temp_r=str->RightEdge[Round_row];
  459.                          track=str->track_Width[Round_row]+2.8;
  460.                          temp_l=temp_r-track;
  461.                          if(temp_l>=79)
  462.                          {
  463.                              temp_l=78;
  464.  
  465.                          }
  466.                          if(temp_l<=1)
  467.                          {
  468.                              temp_l=1;
  469.  
  470.                          }
  471.                          str->LeftEdge[Round_row]=temp_l;
  472.                       }
  473.                       else
  474.                       {
  475.                          str->LeftEdge[Round_row]=0;
  476.                       }
  477.                    }
  478.  
  479.                if (Lost_num<2)
  480.                  {
  481.  
  482.                   if(str->L_StartLine&&str->R_StartLine&&str->L_StartLine==29&&str->R_StartLine==29&&Inflection_point.L_down_point.x==0&&Inflection_point.L_up_point.x==0)
  483.                      {
  484.              //         lcd_showint8(110,4, 7);
  485.                       if(str->LeftEdge[ROW_END-2]-str->LeftEdge[ROW_END-1]>0&&str->LeftEdge[ROW_END-4]-str->LeftEdge[ROW_END-3]>0)
  486.                         {
  487.                            RoundProcess=Round_Not_Found;//暂定
  488.                            round->Round_flag=0;
  489.                            RoadType= else_road;
  490. //                           BlueQuiet;
  491.                         }
  492.                      }
  493.  
  494.                    }
  495.                    break;
  496.                 }
  497.               default:
  498.                 {
  499.                    break;
  500.                 }
  501.              }
  502.              break;
  503.           }
  504. }

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

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

  

闽ICP备14008679号