IT小白

这个屌丝很懒，什么也没留下！

热门标签

第18届全国大学生智能汽车竞赛四轮车开源讲解【6】--环岛_不利用陀螺仪识别环岛

作者：IT小白 | 2024-03-01 13:44:21

踩

不利用陀螺仪识别环岛

开源汇总写在下面

第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.边界连续性

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

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

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

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

典型图形如下：

参考代码如下：


/*-------------------------------------------------------------------------------------------------------------------
  @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.判断条件

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

2.连线情况

将单调突变点向下连线

此时我们已经得到了上单调突变点坐标，计算他距离右边界的距离为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状态

1.判断条件

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

2.连线情况

将单调突变点向下连线

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

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

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

3.跳出条件

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

在2状态下，电磁信号激增，进入3状态。
在2状态下，单调突变点纵坐标靠下，或者横坐标靠左，进入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状态

1.判断条件

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

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

2.连线情况

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

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

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

这是环岛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状态时陀螺仪候开始积分，积满60度，自动跳到4。
进入1状态时编码器开始积分，积分一定距离（注意大小环区别），自动跳到4。
当右上角点比较靠下，或者比较靠左，进入状态4。

4状态

1.判断条件

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

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

2.跳出条件

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

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

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

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

3.连线情况

最后跳出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状态

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.跳出条件

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

6状态

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.跳出条件

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

7状态

1.判断条件

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

2.连线情况

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

3.跳出条件

寻找右上角点，角点位置合理进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状态

1.判断条件

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

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

1.判断条件

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

2.连线情况

无

3.跳出条件

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

三、注意事项

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

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

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

1.优化

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

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

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

四、实用技巧

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

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


/*-------------------------------------------------------------------------------------------------------------------
  @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);//屏幕上打出环岛状

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

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

希望能够帮助到一些人。

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

声明：本文内容由网友自发贡献，转载请注明出处：【wpsshop】

第18届全国大学生智能汽车竞赛四轮车开源讲解【6】--环岛_不利用陀螺仪识别环岛

开源汇总写在下面

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

一、环岛特征识别

1.角点

2.边界连续性

3.单调性改变

4.斜率补边界线

5.斜率画线

6.辅助传感器

6.1陀螺仪

6.2电磁

二、环岛阶段划分

1状态

1.判断条件

2.连线情况

3.跳出条件

2状态

1.判断条件

2.连线情况

3.跳出条件

3状态

1.判断条件

2.连线情况

3.跳出条件

4状态

1.判断条件

2.跳出条件

3.连线情况

5状态

1.判断条件

2.连线情况

3.跳出条件

6状态

1.判断条件

2.连线情况

3.跳出条件

7状态

1.判断条件

2.连线情况

3.跳出条件

8状态

1.判断条件

2.连线情况

3.跳出条件

9状态

1.判断条件

2.连线情况

3.跳出条件

三、注意事项

1.优化

四、实用技巧

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