第18届全国大学生智能汽车竞赛四轮车开源讲解【5】--直道、弯道、十字

开源汇总写在下面

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

一、元素识别

智能车花费时间最多的就是元素识别这一环节，经过我们前几章摄像头矫正，边线提取，中线计算，速度/方向控制。这几个环节都做好的话，车子是可以在简单的赛道中间进行基本的寻迹。沿着直道，弯道走。

但是想要完成比赛要求，需要对元素进行处理，包括但不限于：弯道，直道，十字，环岛，坡道，横断，断路，车库，三叉，T字等。

我们从本章开始进行图像元素识别，元素识别主要是依靠特征点的排列组合，以及防止误判。

而且元素识别最重要的是思路，我会详细讲解我的图像识别的思路，讲解特征点的提取。代码并不重要，我在下面提供的代码各位仅供参考。

还是那句话：不同算法之间没有优劣之分，多少国赛选手仍跑着最简单的算法，这并不影响什么。

二、直道

先看几张标准直道图像。

直道

直道

直道

这几张图像都是在跑车时候实际的图片，各位可以看一看，长直道有什么特点。

我在判断的时候用了以下几个特征点：

1. 前瞻很远，也就是Search_Stop_Line计数很大
2. 边界起始点很靠下
3. 赛道没有丢线，或者丢线很少
4. 摄像头获取到的误差很小

代码如下

/*-------------------------------------------------------------------------------------------------------------------
  @brief     直道检测
  @param     null
  @return    null
  Sample     Straight_Detect()；
  @note      利用最长白列，边界起始点，中线起始点，
-------------------------------------------------------------------------------------------------------------------*/
void Straight_Detect(void)
{
    Straight_Flag=0;
    if(Search_Stop_Line>=65)//截止行很远
    {
        if(Boundry_Start_Left>=68&&Boundry_Start_Right>=65)//起始点靠下
        {
            if(-5<=Err&&Err<=5)//误差很小
            {
                Straight_Flag=1;//认为是直道
            }
        }
    }
}

我的直道判断写的其实不好，下面是我在比赛后看了一些技术报告，包括自己的一些想法

1. 中线分布方差
2. 视野远处到近处的斜率

由于直道的情况下中线近乎是一条直线，所以中线数组的方差一定不会太大。

视野远处到近处的斜率可以作为前方道路是否“直”的一个判断标准。

三、弯道

缓弯

急弯

弯道我没有进行处理，直接用的摄像头误差丢给pid进行计算的，但是想要跑的好，其实可以区分大小弯，这里可以提供一些建议。

1. 视野长度
2. 最长白列偏左还是偏右
3. 左/右边丢线数
4. 边界起始点位置
5. 中线位置
6. 曲率计算

这些都可以组合起来，分辨出不同的弯，然后对应不同的参数，得到比较好的控制效果。

s弯也是非常考验控制技术的一环。他不专门作为元素，但是比赛中可能会出现。处理的好可以做到如履平地，直线飞过。

小s弯

s弯

实物图和图像图放在上面，大家可自行分析出特点，进行对应处理。

四、十字

说起十字，大家是不是想到这样的一张图

标准十字

正入十字环

这是一张标准的十字，正常处理可以很简单。

中间区域丢线多，找到四个拐点，连线即可即可。

但是，你常常看到的是这样的十字：

超远十字

十字中途

近处变形十字

斜入十字

斜出十字

这种斜入，斜出，弯道入，十字中途，都是十字。

处理元素就要处理各种情况下的元素，不然没有实际意义。

首先要观察这几张十字都有什么特征。

1. 存在双边丢线，且双边丢线基本位于视野中间。
2. 存在起码2个角点或者更多。
3. 最长白列较长。

当发现以上情况可以启动十字判断流程。

1.正入十字

注：以下文章中：角点==拐点，两者是一个东西，没有区别。

我们关注一下正入十字时的边线情况。

标准十字边线情况

我们以左下角点为例，将图像放大来看。

左下角点放大情况

画圆处是理想的角点，我们可以找一找他有什么规律。

1. 角点向下几个边线横坐标差距不大。
2. 角点向上几个边线横坐标差距较大，越向上横差距越大。
3. 角点向上可能会有丢线。

当同时满足这些条件的时候，我认为我找到了一个角点。

我将这种判断办法我称为边界撕裂法。

右下角点情况

右上角点情况

左上角点情况

其他几个点的情况也差不多，都是在上角点上侧边线差距不大，下面边线差距很大，还有可能出现丢线，下角点同理。

左下角点判断函数参考如下：

/*-------------------------------------------------------------------------------------------------------------------
  @brief     左下角点检测
  @param     起始行，终止行
  @return    返回角点所在的行数，找不到返回0
  Sample     left_down_guai[0]=Find_Left_Down_Point(MT9V03X_H-1,20);
  @note      角点检测阈值可根据实际值更改
-------------------------------------------------------------------------------------------------------------------*/
int Find_Left_Down_Point(int start,int end)//找左下角点，返回值是角点所在的行数
{
    int i,t;
    int left_down_line=0;
    if(Left_Lost_Time>=0.9*MT9V03X_H)//大部分都丢线，没有拐点判断的意义
       return left_down_line;
    if(start<end)//--访问，要保证start>end
    {
        t=start;
        start=end;
        end=t;
    }
    if(start>=MT9V03X_H-1-5)//下面5行上面5行数据不稳定，不能作为边界点来判断，舍弃
        start=MT9V03X_H-1-5;//另一方面，当判断第i行时，会访问到i+3和i-4行，防止越界
    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(left_down_line==0&&//只找第一个符合条件的点
           abs(Left_Line[i]-Left_Line[i+1])<=5&&//角点的阈值可以更改
           abs(Left_Line[i+1]-Left_Line[i+2])<=5&&  
           abs(Left_Line[i+2]-Left_Line[i+3])<=5&&
              (Left_Line[i]-Left_Line[i-2])>=5&&
              (Left_Line[i]-Left_Line[i-3])>=10&&
              (Left_Line[i]-Left_Line[i-4])>=10)
        {
            left_down_line=i;//获取行数即可
            break;
        }
    }
    return left_down_line;
}

相类似的角点判断这里不在重复阐述，核心原理是找到边界撕裂，记录边界撕裂起始处的行数。

当找到四个角点时，对应连线即可。

左右对应点连上即可

这里提供一下左边线连线函数，右边线同理，不再赘述。

/*-------------------------------------------------------------------------------------------------------------------
  @brief     左补线
  @param     补线的起点，终点
  @return    null
  Sample     Left_Add_Line(int x1,int y1,int x2,int y2);
  @note      补的直接是边界，点最好是可信度高的,不要乱补
-------------------------------------------------------------------------------------------------------------------*/
void Left_Add_Line(int x1,int y1,int x2,int y2)//左补线,补的是边界
{
    int i,max,a1,a2;
    int hx;
    if(x1>=MT9V03X_W-1)//起始点位置校正，排除数组越界的可能
       x1=MT9V03X_W-1;
    else if(x1<=0)
        x1=0;
     if(y1>=MT9V03X_H-1)
        y1=MT9V03X_H-1;
     else if(y1<=0)
        y1=0;
     if(x2>=MT9V03X_W-1)
        x2=MT9V03X_W-1;
     else if(x2<=0)
             x2=0;
     if(y2>=MT9V03X_H-1)
        y2=MT9V03X_H-1;
     else if(y2<=0)
             y2=0;
    a1=y1;
    a2=y2;
    if(a1>a2)//坐标互换
    {
        max=a1;
        a1=a2;
        a2=max;
    }
    for(i=a1;i<=a2;i++)//根据斜率补线即可
    {
        hx=(i-y1)*(x2-x1)/(y2-y1)+x1;
        if(hx>=MT9V03X_W)
            hx=MT9V03X_W;
        else if(hx<=0)
            hx=0;
        Left_Line[i]=hx;
    }
}

核心也就是两点确定一条直线。根据数学中的直线函数的两点式，填入对应数据，补线就行。

当然，这是最理想的情况。

2.斜入十字

但凡车入十字时歪了，或者是急弯接着十字下面的两个拐点就看不见了。

大家看一看上面我列举的几张斜入十字的图，但凡十字出现，下面的角点会有可能看不见，但是上面一定存在两个角点。

所以我将十字判断的核心放到了上面两个角点上。

只要上面两个拐点同时出现，且坐标位置合理，我就认为当前元素是十字，下面两个角点存在与否不重要，下面有点，那就连上，没有点就用其他办法。

整套十字处理代码如下。

void Cross_Detect()
{
    int down_search_start=0;//下角点搜索开始行
    Cross_Flag=0;
    if(Island_State==0&&Ramp_Flag==0)//与环岛互斥开
    {
        Left_Up_Find=0;
        Right_Up_Find=0;
        if(Both_Lost_Time>=10)//十字必定有双边丢线，在有双边丢线的情况下再开始找角点
        {
            Find_Up_Point( MT9V03X_H-1, 0 );
            if(Left_Up_Find==0&&Right_Up_Find==0)//只要没有同时找到两个上点，直接结束
            {
                return;
            }
        }
        if(Left_Up_Find!=0&&Right_Up_Find!=0)//找到两个上点，就认为找到十字了
        {
            Cross_Flag=1;//确定对应标志位，便于各元素互斥掉
            down_search_start=Left_Up_Find>Right_Up_Find?Left_Up_Find:Right_Up_Find;//用两个上拐点坐标靠下者作为下点的搜索上限
            Find_Down_Point(MT9V03X_H-5,down_search_start+2);//在上拐点下2行作为下角点的截止行
            if(Left_Down_Find<=Left_Up_Find)
            {
                Left_Down_Find=0;//下点不可能比上点还靠上
            }
            if(Right_Down_Find<=Right_Up_Find)
            {
                Right_Down_Find=0;//下点不可能比上点还靠上
            }
            if(Left_Down_Find!=0&&Right_Down_Find!=0)
            {//四个点都在，无脑连线，这种情况显然很少
                Left_Add_Line (Left_Line [Left_Up_Find ],Left_Up_Find ,Left_Line [Left_Down_Find ] ,Left_Down_Find);
                Right_Add_Line(Right_Line[Right_Up_Find],Right_Up_Find,Right_Line[Right_Down_Find],Right_Down_Find);
            }
            else if(Left_Down_Find==0&&Right_Down_Find!=0)//11//这里使用的是斜率补线
            {//三个点                                     //01
                Lengthen_Left_Boundry(Left_Up_Find-1,MT9V03X_H-1);
                Right_Add_Line(Right_Line[Right_Up_Find],Right_Up_Find,Right_Line[Right_Down_Find],Right_Down_Find);
            }
            else if(Left_Down_Find!=0&&Right_Down_Find==0)//11
            {//三个点                                      //10
                Left_Add_Line (Left_Line [Left_Up_Find ],Left_Up_Find ,Left_Line [Left_Down_Find ] ,Left_Down_Find);
                Lengthen_Right_Boundry(Right_Up_Find-1,MT9V03X_H-1);
            }
            else if(Left_Down_Find==0&&Right_Down_Find==0)//11
            {//就俩上点                                    //00
                Lengthen_Left_Boundry (Left_Up_Find-1,MT9V03X_H-1);
                Lengthen_Right_Boundry(Right_Up_Find-1,MT9V03X_H-1);
            }
        }
        else
        {
            Cross_Flag=0;
        }
    }
    //角点相关变量，debug使用
    //ips200_showuint8(0,12,Cross_Flag);
//    ips200_showuint8(0,13,Island_State);
//    ips200_showuint8(50,12,Left_Up_Find);
//    ips200_showuint8(100,12,Right_Up_Find);
//    ips200_showuint8(50,13,Left_Down_Find);
//    ips200_showuint8(100,13,Right_Down_Find);
}

判断流程如下

1. 当前状态不是环岛，不是坡道，因为需要做到元素互斥，一次只有可能是一个元素。
2. 图像中累计双边丢线数量大于10，开始找上拐点。
3. 上拐点找到了，以上拐点纵坐标靠下者作为下拐点搜索的上限。
4. 搜索下拐点。
5. 下拐点位置合理性判断，下拐点不可能比上拐点还靠上。
6. 如果四个点都存在，直接连线；三个点存在，根据条件使用斜率补线和直接连线。
7. 只有两个上点存在，进行斜率补线。

十字判断流程图

这样处理十字有一个好处，不用进行十字的状态机处理。

传统十字需要区分如下状态：

1. 车即将入十字，存在四个角点；
2. 车已经进入十字，只有两个上角点，
3. 车即将出十字，上角点靠下；
4. 车出十字，角点消失。

我实测四个角点同时出现的情况太少了，几乎看不到，所以没有使用传统方法。

用我们现在这种方法可以省去状态机，有几个角点就按照对应着连线，不再区分十字状态。

3.补线

我的十字补线函数有两种：

Lengthen_Left_Boundry (Left_Up_Find-1,MT9V03X_H-1);
Lengthen_Right_Boundry(Right_Up_Find-1,MT9V03X_H-1);
Left_Add_Line (Left_Line [Left_Up_Find ],Left_Up_Find ,Left_Line [Left_Down_Find ] ,Left_Down_Find);
Right_Add_Line(Right_Line[Right_Up_Find],Right_Up_Find,Right_Line[Right_Down_Find],Right_Down_Find);

一个是Add_Line，一个是Lengthen_Left，两者有很大区别。

Add_Line的用途：两点确定一条直线。

Lengthen_Left用途：只有一个点，在这个点向上找点确定斜率，画出一根线。因为十字角点向上一定是短直道，那么我就沿着直道向下做一个斜率补线。

Lengthen_Left详情如下。

1. 函数有两个参数，一个是补线起始行，一个是终止补线行。
2. 角点处开始，向上移动三行，顺便访问该行的边线的横坐标。
3. 这样我们就得到了两个点（补线起始行，补线起始行上面第3行，确定了行数再去找一下对应行数的边界的横坐标，就有了两个点的坐标），就可以算出这一条短线的斜率。
4. 有一个点（起始点），有斜率（起始点，起始点向上数3行的点，这两点之间的斜率），就可以向下补线，补到终止行即可。

斜率补线示意图

这样补的线和下面的角点就没有关系，无论有没有下角点，都可以补出一条边界线。

代码如下

/*-------------------------------------------------------------------------------------------------------------------
  @brief     右左边界延长
  @param     延长起始行数，延长到某行
  @return    null
  Sample     Lengthen_Right_Boundry(int start,int end)；
  @note      从起始点向上找3个点，算出斜率，向下延长，直至结束点
-------------------------------------------------------------------------------------------------------------------*/
void Lengthen_Right_Boundry(int start,int end)
{
    int i,t;
    float k=0;
    if(start>=MT9V03X_H-1)//起始点位置校正，排除数组越界的可能
        start=MT9V03X_H-1;
    else if(start<=0)
        start=0;
    if(end>=MT9V03X_H-1)
        end=MT9V03X_H-1;
    else if(end<=0)
        end=0;
    if(end<start)//++访问，坐标互换
    {
        t=end;
        end=start;
        start=t;
    }

    if(start<=5)//因为需要在开始点向上找3个点，对于起始点过于靠上，不能做延长，只能直接连线
    {
        Right_Add_Line(Right_Line[start],start,Right_Line[end],end);
    }
    else
    {
        k=(float)(Right_Line[start]-Right_Line[start-4])/5.0;//这里的k是1/斜率
        for(i=start;i<=end;i++)
        {
            Right_Line[i]=(int)(i-start)*k+Right_Line[start];//(x=(y-y1)*k+x1),点斜式变形
            if(Right_Line[i]>=MT9V03X_W-1)
            {
                Right_Line[i]=MT9V03X_W-1;
            }
            else if(Right_Line[i]<=0)
            {
                Right_Line[i]=0;
            }
        }
    }
}

但斜率补线也有bug，当图像的边界撕裂不是那么明显的时候，会找错角点，然后根据斜率补出一条有问题的线。

角点处撕裂不明显，斜率补线出现bug

十字出现bug

上面一张图出现2个bug。

1. 左上角点附近撕裂不明显，角点附近过斜率缓，导致角点向上的边线斜率与正常直道斜率不一致，补线出现异常。
2. 右下角角点没判，是由于角向上几行撕裂程度小于阈值，就不认为他是角点。就没有使用上下角点之间直接拉线，用的斜率补线。

所以需要我们将十字附近黑胶贴的稳定，笔直，牢固，防止反光之类的情况发生。

另外角点的阈值也需要根据实际情况进行修改，做到稳定不误判。

下面是我利用上面的十字判断方法实际跑出来的小s接十字环，效果很好。

小s入十字环

希望能够帮助到一些人。

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

qq：2296449414