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

开源 0

开源汇总写在下面

第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,没丢线置0int 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_FlaFlag_Show_102(MT9V03X_W,image_two_value,Electromagnet_Flag);//屏幕上打出环岛状Flag_Show_202(MT9V03X_W,image_two_value,Barricade_Flag);//屏幕上打出环岛状

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

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

希望能够帮助到一些人。

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

也许您对下面的内容还感兴趣: