第18届全国大学生智能汽车竞赛四轮车开源讲解【6】--环岛

开源汇总写在下面

第18届全国大学生智能汽车竞赛四轮车开源讲解_Joshua.X的博客-CSDN博客

写在前面

环岛可以说是折磨广大车友的老元素了，本人也是深受其害。调车前期在找环岛特征点；中期在优化识别，减少误判；后期在调整参数，调整控制，优化路径。

不过很遗憾，最后赛场上我还是在环岛上出现了问题，导致未能完赛。下面我将我所有环岛经验，心得分享给大家，希望给大家带来一些灵感和启发。

连续环岛

注：以下方案有可能会用到一个或者多个下述变量，以下变量均在开源【3】边线提取一章有讲。

const uint8  Standard_Road_Wide[MT9V03X_H];//标准赛宽数组
volatile int Left_Line[MT9V03X_H]; //左边线数组
volatile int Right_Line[MT9V03X_H];//右边线数组
volatile int Mid_Line[MT9V03X_H];  //中线数组
volatile int Road_Wide[MT9V03X_H]; //实际赛宽数组
volatile int White_Column[MT9V03X_W];//每列白列长度
volatile int Search_Stop_Line;     //搜索截止行,只记录长度，想要坐标需要用视野高度减去该值
volatile int Boundry_Start_Left;   //左右边界起始点
volatile int Boundry_Start_Right;  //第一个非丢线点,常规边界起始点
volatile int Left_Lost_Time;       //边界丢线数
volatile int Right_Lost_Time;
volatile int Both_Lost_Time;//两边同时丢线数
int Longest_White_Column_Left[2]; //最长白列,[0]是最长白列的长度，也就是Search_Stop_Line搜索截止行，[1】是第某列
int Longest_White_Column_Right[2];//最长白列,[0]是最长白列的长度，也就是Search_Stop_Line搜索截止行，[1】是第某列
int Left_Lost_Flag[MT9V03X_H] ; //左丢线数组，丢线置1，没丢线置0
int Right_Lost_Flag[MT9V03X_H]; //右丢线数组，丢线置1，没丢线置0

一、环岛特征识别

环岛识别需要的点有以下几种。

1.角点

角点也叫拐点，和前文十字我们使用到的角点是一个东西。

判别方法也是一模一样，利用边线误差过大突然过大，我称之为“边线撕裂”。

典型图像如下：

右下角点

参考代码如下：

/*-------------------------------------------------------------------------------------------------------------------
  @brief     右下角点检测
  @param     起始点，终止点
  @return    返回角点所在的行数，找不到返回0
  Sample     Find_Right_Down_Point(int start,int end);
  @note      角点检测阈值可根据实际值更改
-------------------------------------------------------------------------------------------------------------------*/
int Find_Right_Down_Point(int start,int end)//找四个角点，返回值是角点所在的行数
{
    int i,t;
    int right_down_line=0;
    if(Right_Lost_Time>=0.9*MT9V03X_H)//大部分都丢线，没有拐点判断的意义
        return right_down_line;
    if(start<end)
    {
        t=start;
        start=end;
        end=t;
    }
    if(start>=MT9V03X_H-1-5)//下面5行数据不稳定，不能作为边界点来判断，舍弃
        start=MT9V03X_H-1-5;
    if(end<=MT9V03X_H-Search_Stop_Line)
        end=MT9V03X_H-Search_Stop_Line;
    if(end<=5)
       end=5;
    for(i=start;i>=end;i--)
    {
        if(right_down_line==0&&//只找第一个符合条件的点
           abs(Right_Line[i]-Right_Line[i+1])<=5&&//角点的阈值可以更改
           abs(Right_Line[i+1]-Right_Line[i+2])<=5&&  
           abs(Right_Line[i+2]-Right_Line[i+3])<=5&&
              (Right_Line[i]-Right_Line[i-2])<=-5&&
              (Right_Line[i]-Right_Line[i-3])<=-10&&
              (Right_Line[i]-Right_Line[i-4])<=-10)
        {
            right_down_line=i;//获取行数即可
            break;
        }
    }
    return right_down_line;
}

2.边界连续性

参考下图，左边界是近乎一条直线，相邻任意两行边界的差值都很小，我们称之为“连续”的。

右边线因为角点等多种原因，边线是“撕裂的”，我称之为“边界不连续”，也就是边线断掉了。

判断方法很简单，甚至比角点还简单，只要判断上下两行之间边界撕裂情况，当大于某个阈值时，就认为边界不连续。

（角点一定是不连续，不连续不一定是角点，角点的判断的条件深度更深，建立在不连续的基础上）

典型图形如下：

右边线被ABC三个点断开了

参考代码如下：

/*-------------------------------------------------------------------------------------------------------------------
  @brief     右赛道连续性检测
  @param     起始点，终止点
  @return    连续返回0，不连续返回断线出行数
  Sample     continuity_change_flag=Continuity_Change_Right(int start,int end)
  @note      连续性的阈值设置为5，可更改
-------------------------------------------------------------------------------------------------------------------*/
int Continuity_Change_Right(int start,int end)
{
    int i;
    int t;
    int continuity_change_flag=0;
    if(Right_Lost_Time>=0.9*MT9V03X_H)//大部分都丢线，没必要判断了
       return 1;
    if(start>=MT9V03X_H-5)//数组越界保护
        start=MT9V03X_H-5;
    if(end<=5)
       end=5;
    if(start<end)//都是从下往上计算的，反了就互换一下
    {
       t=start;
       start=end;
       end=t;
    }

    for(i=start;i>=end;i--)
    {
        if(abs(Right_Line[i]-Right_Line[i-1])>=5)//连续性阈值是5，可更改
       {
            continuity_change_flag=i;
            break;
       }
    }
    return continuity_change_flag;
}

3.单调性改变

常规赛道都是单调递增/单调递减的，

然而在环岛的某些状态，会看到弧，会看到其他的角，这会有单调性转转变的特点。

先看一下环岛需要判断的单调转折出现在哪些地方。

首先是刚入环岛的这个圆弧，常规来判断其实这是不太好处判断的。

弧_单调转折

我们将圆弧放大来看，可以看到右边界在向左，向上伸展，经过了圆弧处，反过来向右向上伸展。这就是单调性的转折，边界的数值在先变小，再变大。

弧_单调转折放大

这是即将要出环的状态，我们将那个尖角放大来看。

单调转折点

这里明显出现了一个尖角，但他还没有变成角点，因为撕裂不够大，相邻的两行边界差距不够大。

但是边界延伸方向却变了，从向右伸展，变成突然向左伸展，这也是单调性的改变。

单调转折点放大

我就根据单调性突变，抓住边界连续几个点先向左伸展，再向右伸展（先向右伸展，再向左伸展），这个特点写了一个单调转折点判断函数。原理很暴力，但是用来抓这几个点，效果很好。

/*-------------------------------------------------------------------------------------------------------------------
  @brief     单调性突变检测
  @param     起始点，终止行
  @return    点所在的行数，找不到返回0
  Sample     Find_Right_Up_Point(int start,int end);
  @note      前5后5它最大（最小），那他就是角点
-------------------------------------------------------------------------------------------------------------------*/
int Monotonicity_Change_Right(int start,int end)//单调性改变，返回值是单调性改变点所在的行数
{
    int i;
    int monotonicity_change_line=0;

    if(Right_Lost_Time>=0.9*MT9V03X_H)//大部分都丢线，没有单调性判断的意义
        return monotonicity_change_line;
    if(start>=MT9V03X_H-1-5)//数组越界保护
        start=MT9V03X_H-1-5;
     if(end<=5)
        end=5;
    if(start<=end)
        return monotonicity_change_line;
    for(i=start;i>=end;i--)//会读取前5后5数据，所以前面对输入范围有要求
    {
        if(Right_Line[i]==Right_Line[i+5]&&Right_Line[i]==Right_Line[i-5]&&
        Right_Line[i]==Right_Line[i+4]&&Right_Line[i]==Right_Line[i-4]&&
        Right_Line[i]==Right_Line[i+3]&&Right_Line[i]==Right_Line[i-3]&&
        Right_Line[i]==Right_Line[i+2]&&Right_Line[i]==Right_Line[i-2]&&
        Right_Line[i]==Right_Line[i+1]&&Right_Line[i]==Right_Line[i-1])
        {//一堆数据一样，显然不能作为单调转折点
            continue;
        }
        else if(Right_Line[i] <Right_Line[i+5]&&Right_Line[i] <Right_Line[i-5]&&
        Right_Line[i] <Right_Line[i+4]&&Right_Line[i] <Right_Line[i-4]&&
        Right_Line[i]<=Right_Line[i+3]&&Right_Line[i]<=Right_Line[i-3]&&
        Right_Line[i]<=Right_Line[i+2]&&Right_Line[i]<=Right_Line[i-2]&&
        Right_Line[i]<=Right_Line[i+1]&&Right_Line[i]<=Right_Line[i-1])
        {//就很暴力，这个数据是在前5，后5中最大的，那就是单调突变点
            monotonicity_change_line=i;
            break;
        }
    }
    return monotonicity_change_line;
}

当然也可以将条件写的更加具体一点，比如对于转折点上下都需要有单调性要求，这个就看各位的识别要求了。

4.斜率补边界线

这是环岛中的一种补线方法，有3个参数。补线斜率，开始补线行，终止补线行。

/*-------------------------------------------------------------------------------------------------------------------
  @brief     通过斜率，定点补线--
  @param     k       输入斜率
             startY  输入起始点纵坐标
             endY    结束点纵坐标
  @return    null
  Sample     K_Add_Boundry_Left(float k,int startY,int endY);
  @note      补得线直接贴在边线上
-------------------------------------------------------------------------------------------------------------------*/
void K_Add_Boundry_Left(float k,int startX,int startY,int endY)
{
    int i,t;
    if(startY>=MT9V03X_H-1)
        startY=MT9V03X_H-1;
    else if(startY<=0)
        startY=0;
    if(endY>=MT9V03X_H-1)
        endY=MT9V03X_H-1;
    else if(endY<=0)
        endY=0;
    if(startY<endY)//--操作，start需要大
    {
        t=startY;
        startY=endY;
        endY=t;
    }
//这里有bug，下方循环--循环，需要start要大，只进行y的互换，但是没有进行x的互换
//建议进行判断，如果start更小，那就进行++访问
//这里修改各位自行操作
    for(i=startY;i>=endY;i--)
    {
        Left_Line[i]=(int)((i-startY)/k+startX);//(y-y1)=k(x-x1)变形，x=(y-y1)/k+x1
        if(Left_Line[i]>=MT9V03X_W-1)
        {
            Left_Line[i]=MT9V03X_W-1;
        }
        else if(Left_Line[i]<=0)
        {
            Left_Line[i]=0;
        }
    }
}

5.斜率画线

这和上面的斜率补线一个意思，上面补的是边线，这里补的是实线，直接将像素点涂黑。

参数需要斜率，起始点横纵坐标，结束行。

/*-------------------------------------------------------------------------------------------------------------------
  @brief     根据斜率划线
  @param     输入斜率，定点，画一条黑线
  @return    null
  Sample     K_Draw_Line(k, 20,MT9V03X_H-1 ,0)
  @note      补的就是一条线，需要重新扫线
-------------------------------------------------------------------------------------------------------------------*/
void K_Draw_Line(float k, int startX, int startY,int endY)
{
    int endX=0;

    if(startX>=MT9V03X_W-1)//限幅处理
        startX=MT9V03X_W-1;
    else if(startX<=0)
        startX=0;
    if(startY>=MT9V03X_H-1)
        startY=MT9V03X_H-1;
    else if(startY<=0)
        startY=0;
    if(endY>=MT9V03X_H-1)
        endY=MT9V03X_H-1;
    else if(endY<=0)
        endY=0;
    endX=(int)((endY-startY)/k+startX);//(y-y1)=k(x-x1)变形，x=(y-y1)/k+x1
    Draw_Line(startX,startY,endX,endY);
}

/*-------------------------------------------------------------------------------------------------------------------
  @brief     画线
  @param     输入起始点，终点坐标，补一条宽度为2的黑线
  @return    null
  Sample     Draw_Line(0, 0,MT9V03X_W-1,MT9V03X_H-1);
             Draw_Line(MT9V03X_W-1, 0,0,MT9V03X_H-1);
                                    画一个大×
  @note     补的就是一条线，需要重新扫线
-------------------------------------------------------------------------------------------------------------------*/
void Draw_Line(int startX, int startY, int endX, int endY)
{
    int i,x,y;
    int start=0,end=0;
    if(startX>=MT9V03X_W-1)//限幅处理
        startX=MT9V03X_W-1;
    else if(startX<=0)
        startX=0;
    if(startY>=MT9V03X_H-1)
        startY=MT9V03X_H-1;
    else if(startY<=0)
        startY=0;
    if(endX>=MT9V03X_W-1)
        endX=MT9V03X_W-1;
    else if(endX<=0)
        endX=0;
    if(endY>=MT9V03X_H-1)
        endY=MT9V03X_H-1;
    else if(endY<=0)
        endY=0;
    if(startX==endX)//一条竖线
    {
        if (startY > endY)//互换
        {
            start=endY;
            end=startY;
        }
        for (i = start; i <= end; i++)
        {
            if(i<=1)
                i=1;
            image_two_value[i][startX]=IMG_BLACK;
            image_two_value[i-1][startX]=IMG_BLACK;
        }
    }
    else if(startY == endY)//补一条横线
    {
        if (startX > endX)//互换
        {
            start=endX;
            end=startX;
        }
        for (i = start; i <= end; i++)
        {
            if(startY<=1)
                startY=1;
            image_two_value[startY][i]=IMG_BLACK;
            image_two_value[startY-1][i]=IMG_BLACK;
        }
    }
    else //上面两个是水平，竖直特殊情况，下面是常见情况
    {
        if(startY>endY)//起始点矫正
        {
            start=endY;
            end=startY;
        }
        else
        {
            start=startY;
            end=endY;
        }
        for (i = start; i <= end; i++)//纵向补线，保证每一行都有黑点
        {
            x =(int)(startX+(endX-startX)*(i-startY)/(endY-startY));//两点式变形
            if(x>=MT9V03X_W-1)
                x=MT9V03X_W-1;
            else if (x<=1)
                x=1;
            image_two_value[i][x] = IMG_BLACK;
            image_two_value[i][x-1] = IMG_BLACK;
        }
        if(startX>endX)
        {
            start=endX;
            end=startX;
        }
        else
        {
            start=startX;
            end=endX;
        }
        for (i = start; i <= end; i++)//横向补线，保证每一列都有黑点
        {

            y =(int)(startY+(endY-startY)*(i-startX)/(endX-startX));//两点式变形
            if(y>=MT9V03X_H-1)
                y=MT9V03X_H-1;
            else if (y<=0)
                y=0;
            image_two_value[y][i] = IMG_BLACK;
        }
    }
}

6.辅助传感器

理论上来说，依靠纯摄像头完全可以进行环岛元素的判断，我也实现过。但是现场环境千变万化，没人知道你的车子会看到什么，会判断什么。为了提高车模的稳定性，我建议在比赛规则允许的情况下使用上一些辅助的传感器，如陀螺仪，电磁来进行辅助。可以显著提高车子的稳定性。

6.1陀螺仪

因为环岛是个圆，从车环岛入环开据积分角度始积分，这样就即使只看陀螺仪数据也大概确定车模处于环岛什么状态。因为陀螺仪和图像是独立的。图像会受一些影响，进入到错误的环岛状态。这时候就可以利用陀螺仪，当角度到达某个范围时，再开启下一个阶段的判断，这样可以有效防止误判。

陀螺仪处理有很多办法，四元数，卡尔曼滤波，一节线性滤波都可以得到想要的数据。这里就不展开介绍。

6.2电磁

环岛电磁线走线示意图

电磁线在环岛处有一个独一无二的特点，就是会在圆和直线的切线处会电磁线会重合。这样会造成这个点的电感值是赛道其他地方的两倍，可以抓住这一特点，作为入环的判断，或者入环的确定。

二、环岛阶段划分

我是将环岛划分到了9个阶段，其实现在想一想划分的有点太多了，有些划分的不是很合理。

我先介绍一下我当前代码中的写法，然后再介绍一下个人感觉比较好的做法。

以右环为例，左环是一样的道理。

1状态

环岛1状态

1.判断条件

1. 左边连续，右边不连续
2. 左边单调，右边不单调
3. 左边丢线少，右边丢线多，双边丢线少
4. 搜索截止行很远
5. 边界起始点很靠下
6. 右下角存在角点
7. 角点向上存在单调突变点

2.连线情况

1. 将单调突变点向下连线

环岛1拉线情况

此时我们已经得到了上单调突变点坐标，计算他距离右边界的距离为L1。

我们令L2=L1*k，系数k我这里取得0.15。然后从在图像最下面一行取右边界向左移动L2距离，两点连线。

例如：

我的上图单调点坐标（100，10），摄像头图像尺寸180*70

那么L1=180-100=80，L2=80*0.15=12

L2点坐标就是（180-12，69）

那么我们将（100，10），（168，69）两点进行连线。得到了状态1的补线。

3.跳出条件

当右下角的那块角点消失，也就是当右边界起始点变高，就进入2状态。

2状态

环岛2状态

1.判断条件

1. 在1状态下边界起始点变高，也就是右下角点消失。

2.连线情况

1. 将单调突变点向下连线

连线原则和状态1一样，这样就做到了边线位置与角点无关，只与单调点位置有关，即使角点判断失误，仍可以流畅前进。

状态1之所以判断角点，一方面为了1状态环岛的确认，防止误判。

另一方面找到角点坐标，向上开始寻找单调突变点，防止将角点误判为单调突变点。

3.跳出条件

我用过好几个版本的跳出条件，各有利弊，这里只供参考。

1. 在2状态下，电磁信号激增，进入3状态。
2. 在2状态下，单调突变点纵坐标靠下，或者横坐标靠左，进入3状态。
3. 在2状态下，边界起始点靠下，说明大圆弧来到了视野下方，进入3状态。

        else if(Island_State==2)//2状态下方丢线，上方即将出现大弧线
        {
            monotonicity_change_line[0]=Monotonicity_Change_Right(70,5);//单调性改变
            monotonicity_change_line[1]=Right_Line[monotonicity_change_line[0]];
            Right_Add_Line((int)(MT9V03X_W-1-(monotonicity_change_line[1]*0.15)),MT9V03X_H-1,monotonicity_change_line[1],monotonicity_change_line[0]);
//            if(Island_State==2&&(Boundry_Start_Right>=MT9V03X_H-10))//右下角再不丢线进3
            if(Island_State==2&&(Boundry_Start_Right>=MT9V03X_H-5||monotonicity_change_line[0]>50))//右下角再不丢线进3
            {
                Island_State=3;//下方丢线，说明大弧线已经下来了
            }
        }

3状态

环岛3状态

1.判断条件

1. 在2状态下，电磁信号激增，进入3状态。
2. 在2状态下，单调突变点纵坐标靠下，或者横坐标靠左，进入3状态。
3. 在2状态下，边界起始点靠下，说明大圆弧来到了视野下方，进入3状态。

上个状态的跳出条件就是下个状态的进入条件。

2.连线情况

环岛3状态其实是个人认为比较麻烦的状态，他的稳定与否直接决定了你入环的情况，我就是因为这个状态没有处理好，赛场上环岛出现了问题。

3状态主要是找右上角点，可以一直找，找到了且在给定区域中，再连线。因为有时候环比较大，圆弧阶段比较长，角点会稍微等一下才会看到。

关于角点判断在环岛3状态还有个问题，是最长白列扫线自身的bug。

环岛3初期

这是环岛3的初期，此时最长白列在左侧，是可以扫到右侧的这个角点，可以连线角点和左下点。

当角点靠下时，（因为角点判断在图最上最下5行不进行判断）认为可以停止补线。

但是，出现了这样的问题。

角点靠下停止补线

当角点靠下后，停止补线。由于最长白列仍然位于左边，右边线与我们期望位置不符，他会沿着当前的边线得到的误差，向着直道方向跑去。

这种情况有一个解法，进入环岛3状态，强制将最长白列锁定在右侧，

这样的话，右上角点就会变成左上角点，找到角点后，进行连线操作即可。

锁定最长白列位置

待角点靠下或者靠左，停止补线，仍锁定最长白列，让他继续寻找边界，就可以了。

锁定最长白列位置

这种情况有一点风险，就是图像有可能会“跳帧”。就比如可能上一秒图像的角点还在上方，下一秒由于卡顿，或者其他情况，角点突然靠下，结果车子没有识别到这个角点，补线就会乱补线，产生问题。

下面我介绍一下我最后使用的办法，效果其实也不是很好。

补线，这条线既是“动”的，也是“定”的。

动：以每次角点出现的位置为准，每次的位置可能不一样。

定：在3状态时，线是不动的。

首先在环岛3状态仍然限制最长白列范围。主要是限制最长白列的搜索起始列。

我左下定点坐标（20,69），那么最长白列搜索起始行就从25或者30开始。不然的话边线还是会混乱。

然后还是正常的找到右上角点，当角点出现在划定的区域内，定点和角点间的斜率lk。

开始画线。

这次画的是一条实线，就是将图像上的像素点涂黑，不是将某点记录为边界。

线画完了之后，重新扫线，也就是说在环岛3状态需要扫两次边界。

画线的目的是围城一块区域，保证边线一定在这个范围内。

斜率画线

斜率画线

画的这条线就会位置固定起来，由于这是一条死线，（不是实时抓角点的原因也是怕跳帧）在车的运行中，不一定会完美贴合角点，但是一般情况下都会在角点附近，不太影响车模运行。

这样的话，车子就可以沿着这条线和角点围城的域得区到的边界，进入环岛。

当然需要注意的是这条线的密度要够高，不能出现这种情况

画线“漏了”

线画漏了，最长白列从中穿过去了，这样右边界就会找错。

画死线的目的是圈定范围，但这样就和没画线一样。

我最后的故障点就是这条线画了，边线数据更新了，但是车子不往环岛里面进，至今也想不清为什么。

   else if(Island_State==3)//下面已经出现大弧线，且上方出现角点
        {
            if(k!=0)//已经找到点了，画一条死线
            {
                K_Draw_Line(k,30,MT9V03X_H-1,0);
                Longest_White_Column();//刷新最长白列
            }
            else//还在找点
            {
                Right_Up_Guai[0]=Find_Right_Up_Point(40,10);//找右上拐点
                Right_Up_Guai[1]=Right_Line[Right_Up_Guai[0]];
                if(Right_Up_Guai[0]<10)//角点位置不对，退出环岛
                {
                    Island_State=0;
                    Right_Island_Flag=0;
                }
                if(k==0&&(15<=Right_Up_Guai[0]&&Right_Up_Guai[0]<50)&&(70<Right_Up_Guai[1]&&Right_Up_Guai[1]<150))
                {//当角点出现在给定范围内
                    island_state_3_up[0]= Right_Up_Guai[0];
                    island_state_3_up[1]= Right_Up_Guai[1];
                    k=(float)((float)(MT9V03X_H-island_state_3_up[0])/(float)(20-island_state_3_up[1]));
                    K_Draw_Line(k,30,MT9V03X_H-1,0);
                    Longest_White_Column();//刷新赛道数据
                }
            }
            if((Island_State==3)&&(abs(FJ_Angle)>=60))//只依靠陀螺仪积分
            {
                k=0;//斜率清零
                Island_State=4;
                Longest_White_Column();//最长白列
            }
        }

3.跳出条件

3状态也有很多跳出方法，各位看情况使用。

1. 进入1状态时陀螺仪候开始积分，积满60度，自动跳到4。
2. 进入1状态时编码器开始积分，积分一定距离（注意大小环区别），自动跳到4。
3. 当右上角点比较靠下，或者比较靠左，进入状态4。

4状态

环岛4状态

1.判断条件

3状态满足条件后，自动进入4状态。

在环岛中运行都是4状态。

2.跳出条件

1. 单调突变点出现，位置在规定的范围内。
2. 当点出现后，顺便进行补线。

图示单调突变点c出现在指定区域中

由于每次车模轨迹不同，环岛大小不同，角点出现位置不定，所以划定了一块区域，在这个区域中出现转折点都认为合理。

4跳5状态需要陀螺仪辅助，陀螺仪积分满220度以上才开启，不用陀螺仪也可以。只是怕担心如下情况出现。

环岛光干扰

放大图

由于光线的干扰，这里出现了一个单调转折点。依靠算法无法和标准转折点区分，只能在陀螺仪积分未到指定角度时暂时不开启判断。等到陀螺仪积分满足一定条件再打开判断，尽可能排除干扰。

3.连线情况

4状态补线

最后跳出4状态时才开始补线。

这个状态的连线情况比较特殊。他既是“死的”，又是“活的”，和环岛3状态一致，这补的是边界线。

当转折点第一次出现在规定区域内，记录转折点坐标，还有屏幕最下方边界坐标，计算出当前斜率k。然后使用下面的斜率补线函数，补一条线。

void K_Add_Boundry_Left(float k,int startX,int startY,int endY)

这个斜率k需要使用静态变量，保证只记录一次，在后续6状态，7状态都要使用这个斜率，不重新计算。

另外这一条线在4状态结束或者5状态开始补线都可以。

4状态代码如下

 else if(Island_State==4)//4状态完全进去环岛了
{
            if(FJ_Angle>200)//环岛积分200度后再打开单调转折判断
            {
                monotonicity_change_line[0]=Monotonicity_Change_Left(90,10);//单调性改变
                monotonicity_change_line[1]=Left_Line[monotonicity_change_line[0]];
                if((Island_State==4)&&(35<=monotonicity_change_line[0]&&monotonicity_change_line[0]<=55&&monotonicity_change_line[1]<MT9V03X_W-10))
                {//转折点在固定区域内补线，顺便进5
                   island_state_5_down[0]=MT9V03X_H-1;
                   island_state_5_down[1]=Left_Line[MT9V03X_H-1]-5;//抓住第一次出现的斜率，定死
                   k=(float)((float)(MT9V03X_H-monotonicity_change_line[0])/(float)(island_state_5_down[1]-monotonicity_change_line[1]));
                   K_Add_Boundry_Left(k,island_state_5_down[1],island_state_5_down[0],0);
                   Island_State=5;
                }
            }
}

5状态

环岛5状态

1.判断条件

1. 单调突变点出现，位置在规定的范围内。

2.连线情况

还是沿着4状态时的补线，不动。

  else if(Island_State==5)//准备出环岛
        {
            K_Add_Boundry_Left(k,island_state_5_down[1],island_state_5_down[0],0);
            if(Island_State==5&&Boundry_Start_Left<MT9V03X_H-20)//左边先丢线
            {
                Island_State=6;
            }
        }

3.跳出条件

1. 左下角黑块丢失，也就是边界起始点变高，进入6状态.

6状态

环岛6状态

1.判断条件

1. 左下角黑角丢失，也就是边界起始点变高，进入6状态.

2.连线情况

   else if(Island_State==6)//继续出
        {
            K_Add_Boundry_Left(k,island_state_5_down[1],island_state_5_down[0],0);
            if((Island_State==6)&&(Boundry_Start_Left>MT9V03X_H-10||abs(FJ_Angle)>=320))//左边先丢线
            {//
                k=0;
                Island_State=7;
            }
        }

和5状态一致，继续保持着4状态的补线情况。

3.跳出条件

1. 左边界起始点再低变低，进入7状态。
2. 或者环岛积分过多，强制进7状态。

7状态

环岛7状态

1.判断条件

1. 左边界起始点再低变低，进入7状态。

2.连线情况

7状态停止连线，由常规巡线寻找边界，自动调整。

3.跳出条件

1. 寻找右上角点，角点位置合理进8。

找到角点后可以在这里补线，也可以在8状态开始进行补线。

  else if(Island_State==7)//基本出环岛，找角点
        {
            Right_Up_Guai[0]=Find_Right_Up_Point(MT9V03X_H-10,0);//获取左上点坐标，找到了去8
            Right_Up_Guai[1]=Right_Line[Right_Up_Guai[0]];
            if((Island_State==7)&&((Right_Up_Guai[1]>=MT9V03X_W-88&&(5<=Right_Up_Guai[0]&&Right_Up_Guai[0]<=MT9V03X_H-20))))//注意这里，对横纵坐标都有要求，因为赛道不一样，会意外出现拐点
            {//当角点位置合理时，进8
                Island_State=8;
            }
        }

8状态

环岛8状态

1.判断条件

1. 7状态找角点，角点位置在规定的范围内自动进8。

2.连线情况

右上角点延长补线，和十字用的补线方式一样。

抓角点，角点上面几个点计算斜率，向下延长即可。

3.跳出条件

1. 角点位置靠右
2. 角点位置靠下
3. 右边线起始点变很低，下方不在丢线

以上三点满足任意一点，都可自动进入9状态，基本认为环岛结束。顺便将相关变量清零，等待下一次使用。

else if(Island_State==8)//环岛8
        {
            Right_Up_Guai[0]=Find_Right_Up_Point(MT9V03X_H-1,10);//获取右上点坐标
            Right_Up_Guai[1]=Right_Line[Right_Up_Guai[0]];
            Lengthen_Right_Boundry(Right_Up_Guai[0]-1,MT9V03X_H-1);
            if((Island_State==8)&&(Right_Up_Guai[0]>=MT9V03X_H-20||(Right_Up_Guai[0]<10&&Boundry_Start_Left>=MT9V03X_H-10)))//当拐点靠下时候，认为出环了，环岛结束
            {//角点靠下，或者下端不丢线，认为出环了
                FJ_Angle=0;
                Island_State=9;
                Right_Island_Flag=0;
            }
        }

9状态

环岛9状态

9状态时严格意义讲车模就已经离开环岛了，只是为了环岛判断之间有一定的间隔，防止连续判环。

1.判断条件

1. 8状态角点靠下或者消失自动进入9状态。

2.连线情况

无

3.跳出条件

计时器计时1s后环岛状态自动归零，环岛彻底结束。

三、注意事项

环岛每一个阶段都是动态的。想要调好他，最好是让车自由跑起来， 想办法获取到他的图像，可以利用图传，或者将图像存在sd卡里，后期分析。用手推和车自己跑的话还是有不少区别的。

我在前文摄像头说的图像四个角出现“暗角”，这会非常影响我对元素的识别。

因为我好多处理用到了边界起始点，如果出现“暗角”那我边界起始点就永远是最下方，导致元素判断无法进行。

1.优化

大家可以看到，环岛的1，环岛2，环岛7，环岛8，左边线都是正常的，那么我们完全可以用左边界+（标准赛道宽度/2），直接得到中线。不依靠右边补线，然后（左+右）/2得到中线。

直接半边补线这样更加稳定。因为只要找角点，他都会有一定发的风险找不到，或者找错，不如直接用现有可靠的边界线平移获得。

我曾经试了一下，效果很好。但是因为距离比赛时间太近了，不敢改了

环岛1

环岛2

环岛7

环岛8

四、实用技巧

大家可以看到我图像左上角都有数字，这个数字基本代表着环岛的状态，这样我就可以只看图像就知道环岛的情况。

同理，还可以将路障，坡道，断路，等多种元素的状态写进去，只看图像就知道，当前判断了什么元素，在什么状态。非常实用。具体操作如下。

/*-------------------------------------------------------------------------------------------------------------------
  @brief     元素标志位显示
  @param     二值化图片数组
  @return    null
  Sample     Image_Flag_Show(MT9V03X_W,image_two_value,Island_State);
  @note      要在图片显示前使用，传入标志位可更改
-------------------------------------------------------------------------------------------------------------------*/
void Image_Flag_Show(uint8 MT9V03XW,uint8(*InImg)[MT9V03XW],uint8 image_flag)
{
    for(uint8 H=1;H<8;H++)
    {
        for(uint8 W=1;W<7;W++)
        {
            switch (image_flag)
            {
               // case 0:     *(*(InImg+H)+W)=Image_Flags[0][H][W];    break;//环岛0~8
                case 1:     *(*(InImg+H)+W)=Image_Flags[1][H][W];    break;
                case 2:     *(*(InImg+H)+W)=Image_Flags[2][H][W];    break;
                case 3:     *(*(InImg+H)+W)=Image_Flags[3][H][W];    break;
                case 4:     *(*(InImg+H)+W)=Image_Flags[4][H][W];    break;
                case 5:     *(*(InImg+H)+W)=Image_Flags[5][H][W];    break;
                case 6:     *(*(InImg+H)+W)=Image_Flags[6][H][W];    break;
                case 7:     *(*(InImg+H)+W)=Image_Flags[7][H][W];    break;
                case 8:     *(*(InImg+H)+W)=Image_Flags[8][H][W];    break;
                case 9:     *(*(InImg+H)+W)=Image_Flags[9][H][W];    break;
                default:    break;
            }
        }
    }
}

通过修改坐标偏移量，可以在不同位置显示。

/*-------------------------------------------------------------------------------------------------------------------
  @brief     元素标志位显示
  @param     二值化图片数组
  @return    null
  Sample     Flag_Show_202(MT9V03X_W,image_two_value,Island_State);
  @note      第二行倒数第二个数字
-------------------------------------------------------------------------------------------------------------------*/
void Flag_Show_202(uint8 MT9V03XW,uint8(*InImg)[MT9V03XW],uint8 image_flag)
{
    for(uint8 H=10+1;H<10+8;H++)
    {
        for(uint8 W=MT9V03XW-10+1;W<MT9V03XW-10+8;W++)
        {
            switch (image_flag)
            {
                //case 0:     *(*(InImg+H)+W)=Image_Flags[0][H-10][W-(MT9V03XW-10)];    break;
                case 1:     *(*(InImg+H)+W)=Image_Flags[1][H-10][W-(MT9V03XW-10)];    break;
                case 2:     *(*(InImg+H)+W)=Image_Flags[2][H-10][W-(MT9V03XW-10)];    break;
                case 3:     *(*(InImg+H)+W)=Image_Flags[3][H-10][W-(MT9V03XW-10)];    break;
                case 4:     *(*(InImg+H)+W)=Image_Flags[4][H-10][W-(MT9V03XW-10)];    break;
                case 5:     *(*(InImg+H)+W)=Image_Flags[5][H-10][W-(MT9V03XW-10)];    break;
                case 6:     *(*(InImg+H)+W)=Image_Flags[6][H-10][W-(MT9V03XW-10)];    break;
                case 7:     *(*(InImg+H)+W)=Image_Flags[7][H-10][W-(MT9V03XW-10)];    break;
                case 8:     *(*(InImg+H)+W)=Image_Flags[8][H-10][W-(MT9V03XW-10)];    break;
                case 9:     *(*(InImg+H)+W)=Image_Flags[9][H-10][W-(MT9V03XW-10)];    break;
                default:    break;
            }
        }
    }
}

//数字图像数组
const uint8 Image_Flags[][9][8]={
        {                                //0
        {0X00,0X00,0X00,0X00,0X00,0X00,0X00,0X00},
        {0X00,0X00,0XFF,0XFF,0XFF,0XFF,0X00,0X00},
        {0X00,0XFF,0X00,0X00,0X00,0X00,0XFF,0X00},
        {0X00,0XFF,0X00,0X00,0X00,0X00,0XFF,0X00},
        {0X00,0XFF,0X00,0X00,0X00,0X00,0XFF,0X00},
        {0X00,0XFF,0X00,0X00,0X00,0X00,0XFF,0X00},
        {0X00,0XFF,0X00,0X00,0X00,0X00,0XFF,0X00},
        {0X00,0X00,0XFF,0XFF,0XFF,0XFF,0X00,0X00},
        {0X00,0X00,0X00,0X00,0X00,0X00,0X00,0X00},
},
{                                        //1
        {0X00,0X00,0X00,0X00,0X00,0X00,0X00,0X00},
        {0X00,0X00,0XFF,0XFF,0X00,0X00,0X00,0X00},
        {0X00,0X00,0X00,0XFF,0X00,0X00,0X00,0X00},
        {0X00,0X00,0X00,0XFF,0X00,0X00,0X00,0X00},
        {0X00,0X00,0X00,0XFF,0X00,0X00,0X00,0X00},
        {0X00,0X00,0X00,0XFF,0X00,0X00,0X00,0X00},
        {0X00,0X00,0X00,0XFF,0X00,0X00,0X00,0X00},
        {0X00,0X00,0XFF,0XFF,0XFF,0X00,0X00,0X00},
        {0X00,0X00,0X00,0X00,0X00,0X00,0X00,0X00},
},
{                                        //2
        {0X00,0X00,0X00,0X00,0X00,0X00,0X00,0X00},
        {0X00,0X00,0XFF,0XFF,0XFF,0XFF,0X00,0X00},
        {0X00,0XFF,0X00,0X00,0X00,0X00,0XFF,0X00},
        {0X00,0X00,0X00,0X00,0X00,0X00,0XFF,0X00},
        {0X00,0X00,0X00,0X00,0XFF,0XFF,0X00,0X00},
        {0X00,0X00,0X00,0XFF,0X00,0X00,0X00,0X00},
        {0X00,0X00,0XFF,0X00,0X00,0X00,0X00,0X00},
        {0X00,0XFF,0XFF,0XFF,0XFF,0XFF,0XFF,0X00},
        {0X00,0X00,0X00,0X00,0X00,0X00,0X00,0X00},
},
{                                        //3
        {0X00,0X00,0X00,0X00,0X00,0X00,0X00,0X00},
        {0X00,0X00,0XFF,0XFF,0XFF,0XFF,0X00,0X00},
        {0X00,0XFF,0X00,0X00,0X00,0XFF,0XFF,0X00},
        {0X00,0X00,0X00,0X00,0X00,0XFF,0X00,0X00},
        {0X00,0X00,0X00,0X00,0XFF,0XFF,0X00,0X00},
        {0X00,0X00,0X00,0X00,0X00,0X00,0XFF,0X00},
        {0X00,0XFF,0X00,0X00,0X00,0X00,0XFF,0X00},
        {0X00,0X00,0XFF,0XFF,0XFF,0XFF,0X00,0X00},
        {0X00,0X00,0X00,0X00,0X00,0X00,0X00,0X00},
},
{                                        //4
        {0X00,0X00,0X00,0X00,0X00,0X00,0X00,0X00},
        {0X00,0X00,0X00,0X00,0XFF,0X00,0X00,0X00},
        {0X00,0X00,0X00,0XFF,0XFF,0X00,0X00,0X00},
        {0X00,0X00,0XFF,0X00,0XFF,0X00,0X00,0X00},
        {0X00,0XFF,0X00,0X00,0XFF,0X00,0X00,0X00},
        {0X00,0XFF,0X00,0X00,0XFF,0X00,0X00,0X00},
        {0X00,0XFF,0XFF,0XFF,0XFF,0X00,0X00,0X00},
        {0X00,0X00,0X00,0X00,0XFF,0X00,0X00,0X00},
        {0X00,0X00,0X00,0XFF,0XFF,0X00,0X00,0X00},
},
{                                        //5
        {0X00,0X00,0X00,0X00,0X00,0X00,0X00,0X00},
        {0X00,0XFF,0XFF,0XFF,0XFF,0XFF,0X00,0X00},
        {0X00,0XFF,0X00,0X00,0X00,0X00,0X00,0X00},
        {0X00,0XFF,0X00,0X00,0X00,0X00,0X00,0X00},
        {0X00,0XFF,0XFF,0XFF,0XFF,0XFF,0X00,0X00},
        {0X00,0X00,0X00,0X00,0X00,0XFF,0X00,0X00},
        {0X00,0XFF,0X00,0X00,0X00,0XFF,0X00,0X00},
        {0X00,0X00,0XFF,0XFF,0XFF,0X00,0X00,0X00},
        {0X00,0X00,0X00,0X00,0X00,0X00,0X00,0X00},
},
{                                        //6
        {0X00,0X00,0X00,0X00,0X00,0X00,0X00,0X00},
        {0X00,0XFF,0XFF,0XFF,0XFF,0XFF,0X00,0X00},
        {0X00,0XFF,0X00,0X00,0X00,0X00,0X00,0X00},
        {0X00,0XFF,0X00,0X00,0X00,0X00,0X00,0X00},
        {0X00,0XFF,0XFF,0XFF,0XFF,0XFF,0X00,0X00},
        {0X00,0XFF,0X00,0X00,0X00,0XFF,0X00,0X00},
        {0X00,0XFF,0X00,0X00,0X00,0XFF,0X00,0X00},
        {0X00,0XFF,0XFF,0XFF,0XFF,0XFF,0X00,0X00},
        {0X00,0X00,0X00,0X00,0X00,0X00,0X00,0X00},
},
{                                        //7
        {0X00,0X00,0X00,0X00,0X00,0X00,0X00,0X00},
        {0X00,0XFF,0XFF,0XFF,0XFF,0XFF,0X00,0X00},
        {0X00,0XFF,0X00,0X00,0X00,0XFF,0X00,0X00},
        {0X00,0X00,0X00,0X00,0X00,0XFF,0X00,0X00},
        {0X00,0X00,0X00,0X00,0XFF,0X00,0X00,0X00},
        {0X00,0X00,0X00,0XFF,0X00,0X00,0X00,0X00},
        {0X00,0X00,0X00,0XFF,0X00,0X00,0X00,0X00},
        {0X00,0X00,0X00,0XFF,0X00,0X00,0X00,0X00},
        {0X00,0X00,0X00,0X00,0X00,0X00,0X00,0X00},
},
{                                        //8
        {0X00,0X00,0X00,0X00,0X00,0X00,0X00,0X00},
        {0X00,0X00,0XFF,0XFF,0XFF,0XFF,0X00,0X00},
        {0X00,0XFF,0X00,0X00,0X00,0X00,0XFF,0X00},
        {0X00,0XFF,0X00,0X00,0X00,0X00,0XFF,0X00},
        {0X00,0X00,0XFF,0XFF,0XFF,0XFF,0X00,0X00},
        {0X00,0XFF,0X00,0X00,0X00,0X00,0XFF,0X00},
        {0X00,0XFF,0X00,0X00,0X00,0X00,0XFF,0X00},
        {0X00,0X00,0XFF,0XFF,0XFF,0XFF,0X00,0X00},
        {0X00,0X00,0X00,0X00,0X00,0X00,0X00,0X00},
},
{                                        //9
        {0X00,0X00,0X00,0X00,0X00,0X00,0X00,0X00},
        {0X00,0X00,0XFF,0XFF,0XFF,0XFF,0X00,0X00},
        {0X00,0XFF,0X00,0X00,0X00,0X00,0XFF,0X00},
        {0X00,0XFF,0X00,0X00,0X00,0X00,0XFF,0X00},
        {0X00,0X00,0XFF,0XFF,0XFF,0XFF,0XFF,0X00},
        {0X00,0X00,0X00,0X00,0X00,0X00,0XFF,0X00},
        {0X00,0X00,0X00,0X00,0X00,0XFF,0X00,0X00},
        {0X00,0XFF,0XFF,0XFF,0XFF,0X00,0X00,0X00},
        {0X00,0X00,0X00,0X00,0X00,0X00,0X00,0X00},
},
};

使用起来非常便捷，写入图像宽度，图像数组，显示的变量

Image_Flag_Show(MT9V03X_W,image_two_value,Island_State);//屏幕上打出环岛状
Flag_Show_101(MT9V03X_W,image_two_value,Ramp_Flag);//屏幕上打出环岛状态Ramp_Fla
Flag_Show_102(MT9V03X_W,image_two_value,Electromagnet_Flag);//屏幕上打出环岛状
Flag_Show_202(MT9V03X_W,image_two_value,Barricade_Flag);//屏幕上打出环岛状

当然，这里也是直接修改原图数组，大家注意不要影响原始图像的判断。

例如，放在左右下角会影响便捷起始点的判断。大家尽量放在图像上面，高处一般很少用到图像。

希望能够帮助到一些人。

本人菜鸡一只，各位大佬发现问题欢迎留言指出

qq：2296449414