0


第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

标签: 汽车 c++ c语言

本文转载自: https://blog.csdn.net/qq_58114029/article/details/132839791
版权归原作者 Joshua.X 所有, 如有侵权,请联系我们删除。

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

还没有评论