第十六届全国大学生智能汽车比赛—摄像头算法控制总结_智能车环岛补线

 第十六届全国大学生智能汽车比赛是我大学参加的第一个有意义的比赛，让我在学校实验室SCA打开了进入大学的大门，无数次的包夜和学习让我静下了心，很荣幸参加过这样一次的竞赛，收获颇丰。简单的做一个总结，算是画上大二一学年的句号，希望对你们有帮助。

本文仅自己看法，并不是权威结论。若有错误或不足之处，还请多多包涵。

摄像头算法控制我分为二类：图像处理和车体控制

目录

第一类：图像处理

1.1 摄像头如何使用

1.2 扫线

1.3 中线

1.4 图像的直道和弯道

1.5 元素处理

1.5.1 十字、环岛、三岔路口

1.5.2 坡道

1.5.3 车库

第二类：车体控制

2.1 转向环

2.2 速度环

---

第一类：图像处理

1.1 摄像头如何使用

我比赛用的总钻风130°摄像头，拿到摄像头后，使用例程可以在IPS屏幕上看到一个灰度图像（原始图像）。这个原始图像我们一般都要进行处理，也可以不处理。我来介绍一下常用的三种处理方法.（1）图像二值化。（2）图像灰度。（3）边缘检测算法。

我用的是简单好用的图像二值化，所以我拿二值化进行讲解。我先简单介绍一下二值化图像，在原始图像的基础上，把他分为黑和白（即赛道是白，其它是黑）。

灰度图像有256个像素点 即0-255。0-255是什么意思，个人理解即颜色的深浅。0是黑，255是白。0-255是介于黑至白之间。对于一个灰度图像中，图像中每一个点都是在0-255之间，每一个点都有一个值.受光照影响，不同位置，不同时间，不同光照，每一个点的值都会不同。所以我们要对这个值进行区分黑白，要进行处理。处理的方法就是：大津法（还有其他方法不唯一，大津法用的人比较多）

大津法就是对你的灰度图像进行处理后得到一个阈值。输入进去一个二位数组图像，然后会返回给你一个阈值，然后你就用这个阈值进行二值化（即大于阈值为白（黑），小于阈值为黑（白）。即你就可以得到一个二值化图像。

/*************************************************************** 
* 函数名称：GetOSTU(mt9v03x_csi_image); 大津法
* 函数输入：摄像头传感器图像数组（mt9v03x_csi_image）
* 函数输出：阈值大小 （Threshold）
* 功能说明：求阈值大小 
***************************************************************/ 
uint8 GetOSTU(uint8 tmImage[MT9V03X_CSI_H][MT9V03X_CSI_W])
{ 
  int16 i,j; 
  uint32 Amount = 0; 
  uint32 PixelBack = 0; 
  uint32 PixelIntegralBack = 0; 
  uint32 PixelIntegral = 0; 
  int32 PixelIntegralFore = 0; 
  int32 PixelFore = 0; 
  double OmegaBack, OmegaFore, MicroBack, MicroFore, SigmaB, Sigma; // 类间方差; 
  int16 MinValue, MaxValue; 
  uint8 Threshold = 0;
  uint8 HistoGram[256];              //  
 
  for (j = 0; j < 256; j++)  HistoGram[j] = 0; //初始化灰度直方图 
  
  for (j = 0; j < MT9V03X_CSI_H; j++) 
  { 
    for (i = 0; i < MT9V03X_CSI_W; i++) 
    { 
      HistoGram[tmImage[j][i]]++; //统计灰度级中每个像素在整幅图像中的个数
    } 
  } 
  
  for (MinValue = 0; MinValue < 256 && HistoGram[MinValue] == 0; MinValue++) ;        //获取最小灰度的值
  for (MaxValue = 255; MaxValue > MinValue && HistoGram[MinValue] == 0; MaxValue--) ; //获取最大灰度的值
      
  if (MaxValue == MinValue)     return MaxValue;         // 图像中只有一个颜色    
  if (MinValue + 1 == MaxValue)  return MinValue;        // 图像中只有二个颜色
    
  for (j = MinValue; j <= MaxValue; j++)    Amount += HistoGram[j];        //  像素总数
    
  PixelIntegral = 0;
  for (j = MinValue; j <= MaxValue; j++)
  {
    PixelIntegral += HistoGram[j] * j;//灰度值总数
  }
  SigmaB = -1;
  for (j = MinValue; j < MaxValue; j++)
  {
    PixelBack = PixelBack + HistoGram[j];   //前景像素点数
    PixelFore = Amount - PixelBack;         //背景像素点数
    OmegaBack = (double)PixelBack / Amount;//前景像素百分比
    OmegaFore = (double)PixelFore / Amount;//背景像素百分比
    PixelIntegralBack += HistoGram[j] * j;  //前景灰度值
    PixelIntegralFore = PixelIntegral - PixelIntegralBack;//背景灰度值
    MicroBack = (double)PixelIntegralBack / PixelBack;   //前景灰度百分比
    MicroFore = (double)PixelIntegralFore / PixelFore;   //背景灰度百分比
    Sigma = OmegaBack * OmegaFore * (MicroBack - MicroFore) * (MicroBack - MicroFore);//计算类间方差
    if (Sigma > SigmaB)                    //遍历最大的类间方差g //找出最大类间方差以及对应的阈值
    {
      SigmaB = Sigma;
      Threshold = j;
    }
  }
  return Threshold;                        //返回最佳阈值;
}

 得到阈值进行二值化处理

/*************************************************************** 
* 函数名称：Get_Pixle(void)
* 函数输入：无
* 函数输出：无
* 功能说明：二值化处理图像像素点
***************************************************************/
void Get_Pixle(void)
{  
  uint8 Gate;
  Gate = GetOSTU(mt9v03x_csi_image);
  for(uint8 hang=0;hang<Row;hang++)
    for(uint8 lie=0;lie<Col;lie++)
    {
      if(mt9v03x_csi_image[hang][lie]>=Gate)
        Pixle[hang][lie]=white;
      else
        Pixle[hang][lie]=black;
    }
}

 

1.2 扫线

  关于摄像头循迹，一般都是按照获取赛道中线，根据赛道中线来进行循迹的。如何获得赛道中线，我来简单介绍一下。在2.1中我们知道中线是按照左右边线获得的，而左右边线我们则需要在图像中进行处理获得，这个过程就是扫线。

  在二值化图像的基础上进行扫线处理，以图像60行120列为例，从上到下为0-60行，我们取第60行中点（图像下面相比上面较稳定，所以我们从下面开始），也就第60行第60列为起始点往两边进行扫线。

#define Row    60               //图像行数
#define Col    120              //图像列数

一般来说，我们的车是放在赛道中间，即起始点为白点，然后我们往两边判断一直到黑点，我们记录下黑点坐标放进左右边线的数组里，若没有扫到黑点则记录图像数组边界值放进左右边线数组里。这样每一行都这样重复就可以获取左右边线数组。简单的扫线就这么结束了。（后面还会补充扫线进阶篇，你也可以根据简单的扫线，自己想一想代码如何升级）

横向扫中线demo

/************************************************************
        
【函数名称】Horizontal_line
【功    能】横向巡线函数
【参    数】无
【返 回 值】无
【实    例】Horizontal_line();
【注意事项】无
***********************************************************/
void Horizontal_line(void)
{
  uint8 i,j;
  if(Pixle[Row-1][Col/2]==0)
  {
    if(Pixle[Row-1][5]==white)
      midline[Row]=5;
    else if(Pixle[Row-1][Col-5]==white)
      midline[Row]=Col-5;
    else
      midline[Row]=Col/2;
  }
  else
	{
    midline[Row]=Col/2;		
	}
	
  for(i=Row-1;i>0;i--)
  {
    for(j=midline[i+1];j>=0;j--)
    {
      if(Pixle[i][j]==0||j==0)
      {
        leftline[i]=j;
        break;
      }
    }
    for(j=midline[i+1];j<=Col-1;j++)
    {
      if(Pixle[i][j]==0||j==Col-1)
      {
        rightline[i]=j;
        break;
      }
    }
    midline[i]=(leftline[i]+rightline[i])/2;
   if(Pixle[i-1][midline[i]]==0||i==0)
    {
   for(j=i;j>0;j--)
   {
    midline[j]=midline[i];
    leftline[j]=midline[i];
    rightline[j]=midline[i];
   }
      break;
    }
  }
}

1.3 中线

我们是根据赛道中线进行循迹，所以中线的处理也非常重要。

中线 = （左边线+右边线）/2 

中线 = 左边线+X

中线 = 右边线-X

 X的值，根据实际情况来定。

1.4 图像的直道和弯道

所有元素处理过后都是直道和弯道，这部分的处理也决定了你车体速度的上限。不仅是你PID的调参还有你图像的处理。

先说简单的，根据中线的值来确定直道和弯道，在不同情况下，调用不同的PID参数。

直道和弯道检测边线丢线状态函数，辅助元素识别。

 
/************************************************************
【函数名称】Lost_line_right
【功    能】右侧图像丢线检查函数
【参    数】无
【返 回 值】-1为未丢线 其他为丢线起始行
【实    例】Lost_line_right();
【注意事项】无
************************************************************/
 
int8 Lost_line_right(void)
{
  uint8 i;
  for(i=50;i>10;i--)
  if(rightline[i]==119)		return i;
  return -1;
}
 
/************************************************************
      
【函数名称】Lost_line_left
【功    能】左侧图像丢线检查函数
【参    数】无
【返 回 值】-1为未丢线 其他为丢线起始行
【实    例】Lost_line_left();
【注意事项】无
************************************************************/
 
int8 Lost_line_left(void)
{
  uint8 i;
  for(i=50;i>10;i--)
  if(leftline[i]==0) return i;
  return -1;
}

直线判断函数，辅助元素识别

/************************************************************
        
【函数名称】Straight_line_judgment
【功    能】直线判断函数
【参    数】arr为传入数组
【返 回 值】1为直线 0为非直线
【实    例】Straight_line_judgment(left_arr);
【注意事项】只判断20-55
************************************************************/
 
uint8 Straight_line_judgment(int arr[Row])
{
  short i,sum=0;
  float kk;
  kk=((float)arr[45]-(float)arr[10])/35.0;//ok
  sum = 0;
  for(i=10;i<=45;i++)
    if(((arr[10]+(float)(i-10)*kk)-arr[i])<=35) sum++;
    else break;
  if(sum>34&&kk>-1.1&&kk<1.1) return 1;
  else return 0;
}

1.5 元素处理

1.5.1 十字、环岛、三岔路口

  这三个元素我是同一个处理方法。三个问题：

（1）拐点问题（左右看看，上下看看）

所有拐点我都分为下拐点和上拐点。

下拐点：十字下面两个拐点，环岛入环和出环拐点，三叉下面两个拐点

下拐点都是在边线上，在一定范围内遍历数组寻找最大值或最小值。

上拐点：十字上面两个拐点，三叉路口上拐点。

上拐点（抽象讲解）图像数组 范围内 从左到右，从下往上扫点（白变黑的跳变点），扫到把行坐标放入一维数组内，然后范围内搜索较大（小）值（一次就好）。

拐点详细讲解

（2）补线问题

两点确定一条直线，确定斜率，就确定两点中的每一个点，然后让这条直线上所有白点变成黑点，在重新扫线，就可以了。

/************************************************************							
【函数名称】connect_line
【功    能】连线函数
【参    数】两点横纵坐标
【返 回 值】无
【实    例】line(0,0,20,30);
【注意事项】无
************************************************************/
void connect_line(uint8 x1,uint8 y1,uint8 x2,uint8 y2)
{
 
	short i,j,swap;
  float k;
  if(y1>y2)
  {
    swap = x1;
    x1 = x2;
    x2 = swap;
    swap = y1;
    y1 = y2;
    y2 = swap;
  }
  if(x1==x2)
  {
    for(i=y1;i<y2+1;i++)
    Pixle[i][x1]=0;
  }
  else if(y1==y2)
  {
    for(i=x1;i<x2+1;i++)
    Pixle[y1][i]=0;
  }
  else
  {
    k = ((float)x2-(float)x1)/((float)y2-(float)y1);
    for(i=y1;i<=y2;i++)
    Pixle[i][(short)(x1+(i-y1)*k)]=0;		
  }	
}

 

（3）状态问题

   1）十字

这是即将进入十字的布线状态，在看到两边丢线且中间大量空白时，进入十字状态，然后开始算出上下四个拐点，上拐点连接下拐点就可以如图所示了。不管你是即将进入十字还是十字中，还是即将出十字，都需要这么一个补线状态，所以，我们用简单的if来控制十字状态。

当下拐点消失时，我们固定下拐点值，直接连接上拐点。我简单来说一下我的代码怎么写的。

if( 左边线丢线&右边线丢线）   （条件判断的有点粗糙，后面详细说一下）     
{    
 if(中间大量白点）     
  {    
   进入十字标志   
  }       
}
 
 
 
if( 进入十字标志）                                                                       
{   
   搜索左上拐点（没搜到给定值）       搜索左下拐点  （没搜到给定值）   连接左上和左下两拐点    
 
   搜索右上拐点（没搜到给定值）       搜索右下拐点  （没搜到给定值）   连接右上和右下两拐点     
 
   if(两边不丢线，结束十字标志)
   {  
 
   }         
}      

                                                                                                                                                 

2）环岛 

                                    

   环岛的补线差不多就4个状态（还有一个出环切内的补线）      但是我分为7个状态。用的switch语句来控制个个状态，一但进入环岛第一个状态后面，在环岛结束前都不会误判，非常好用。

if(左边丢线 || 右边丢线)  //  进入元素识别状态
{
 
 
  左环岛判断
  if(左边丢线||右边不丢线)  // 可能是环岛或者弯道或者车库
  {
    if(右边边线是条直线)  // 排除弯道
    {
 
      给予左环岛标志；
      进入左环岛状态1；
    }
  }
 
roadabout_dispose();  //环岛处理函数
}
 
 
roadabout_dispose()
{
 if(左环岛标志位)
 {
  switch(左环岛状态)
   {
     case 1:
        if(左边即将不丢线)       
         {           
           进入左环岛状态2；
         }
     break;
 
     case 2:
        if(左下边为圆环)
         {
           进入左环岛状态3；
         }
     break;
 
     case 3:
        if(左边第二次丢线，找拐点补线进入环岛)
         {
           进入左环岛状态4；
         }
     break;
 
     case 4:
        if(进入环岛，环岛内)
         {
           进入左环岛状态5；
         }
     break;
 
     case 5:
        if(即将出环岛，找拐点补线出环岛)
         {
           进入左环岛状态6；
         }
     break;
 
     case 6:
        if(右边即将不丢线，车身摆正)
         {
           进入左环岛状态7；
         }
     break;
 
     case 7:
        if(左边即将不丢线)
         {
           进入左环岛状态8；
         }
     break;
 
     case 8:
        if(完全出环岛)
         {
           清除左环岛状态；
           清除左环岛标志位；
         }
     break;
 
   }
    
  if(左环岛状态1||左环岛状态2)
  {
   找到左下拐点；
   找到左上拐点；
   补线；
  }
 
 
  if(左环岛状态4)
  {
   找到左上拐点；
   定点补线；
  }
 
  if(左环岛状态6)
  {
   找到右下拐点；
   定点补线；
  }
 
  if(左环岛状态7)
  {
   定点补线切内环；
  }
 
  if(左环岛状态8)
  {
   左边定点补线，防抖；
  }
 
 }
}
 
 
 

   3)三岔路口

 作为十六届的新元素，起初觉得很简单，拉一条线就能过，后来错了，其实很有讲究和难度，并未有想象出来那么简单，衍生出来很多难搞的问题，要仔细对待。三岔路口最好的特征点，就是两个Y拐点和一个V拐点。

在你看到三岔路口时，并没有丢线状态，所以你在直道上就要判断Y拐点是否存在。有时三岔接弯道时，车体无法“完全”正 ，肉眼可能觉得车体正的（但是我们称这种为斜入）只能看到一个Y拐点。所以三岔路口要分为正入，左斜，右斜，三种判断。简单说一下怎么写的三岔，因为三岔路口我写的不是很好，2.8以上会误判不是很好，

if(中间上面出现黑点）
{
 判断Y拐点是否存在；
 2个Y拐点为正入；
 左边存在为左斜；
 右边存在为右斜；
 判断V拐点是否存在；
 V拐点存在或Y拐点存在进入三岔路口标志位1；
 进入三岔路口状态1；
}
然后根据3种情况和状态 进入下一阶段和拐点补线操作。
可以模仿环岛伪代码来写。
仅供参考。

借鉴一下思路即可。

1.5.2 坡道

坡道个人建议用红外测距，还是比较准的。即简单也准确。

1.5.3 车库

车库识别和处理详解

第二类：车体控制

车体控制主要是舵机和电机以及配合图像处理进行循迹。俗话说的好：图像处理的好，开环也能跑。说到控制那不得不说几个环的事，简单说一下最基本的两个环（1）转向环 （2）速度环

2.1 转向环

我用的是C车模，S3010舵机。转向要能进行左转和右转，所以我们要在图像上区分左和右。

例如：我的图像是 60×120（ 60行，120列）我利用（左边线+右边线)/2=中线 这时候你就取一行或多行 赛道中线-图像中线=error。得到error来区分左右，正常来说你把车体放在赛道中间，error会是0。车体往左边偏为正，车体往右边偏为负。（反之也可以）这样我们就区分了左和右，车体就可以进行转弯了。

但我们发现转弯可能过大或者过小，没法理想的转弯，这时候我们要用PID算法进行调节，新手建议位置式PID（非常够用）。个人建议起步用位置式PID中的纯P调节。调到2米时用PD调节，跳到2米8时换模糊PID。

这时候你把error放入位置式PID中，Ki和Kd给0，调节Kp，输出Steer。然后用舵机中值加减Steer得到的控制值来控制舵机灵敏的转向。这样舵机就可以根据在赛道的不同位置来进行灵活的转向了，这就是一个简单的转向环。

2.2 速度环

如果需要调整车体速度，则需要编码器来帮忙测量电机转动的脉冲，脉冲可以认为是距离，计算的距离和速度准不准其实都没有什么太大关系，你需要的只是能控制他速度的快慢就可以了。来达到他闭环的效果。（我记得我一开始只能跑1米多，学长都说摄像头跑1米/秒太慢了，结果我把速度环调号，直接2.1左右了）

增量PID代码

//PID结构体
typedef struct{
	int16 Current_Speed;//—————————————————————————————————————————————————当前速度
	int16 Target_Speed;//——————————————————————————————————————————————————目标速度
	int16 Encoder;//———————————————————————————————————————————————————————编码器值
	int16 E;//—————————————————————————————————————————————————————————————本次的偏差
	int16 E_L;//———————————————————————————————————————————————————————————上一次的偏差
	int16 E_L_L;//—————————————————————————————————————————————————————————上上次的偏差
	int16 KP;//————————————————————————————————————————————————————————————比例系数
	int16 KI;//————————————————————————————————————————————————————————————积分系数
	int16 KD;//————————————————————————————————————————————————————————————微分系数
	int16 PIDOUT;//————————————————————————————————————————————————————————PID输出
	int16 Target_Speed_old;
  int16 Current_Speed_last;	
  int16 Current_Speed_llast;		
}PID_struct;
 
PID_struct Motor1;
PID_struct Motor2;
 
/************************************************************
        
【函数名称】Motor_Parameters_Init(PID_struct* Motor)
【功    能】速度控制PID参数初始化
【参    数】无
【返 回 值】无
【实    例】Motor_Parameters_Init(&Motor);
【注意事项】无
************************************************************/
 
void Motor_Parameters_Init(PID_struct *Motor)
{
	
	Motor->Current_Speed   = 0;
	Motor->Target_Speed   = 0;
	Motor->Encoder        = 0;
	Motor->E            = 0;
	Motor->E_L          = 0;
	Motor->E_L_L        = 0;
	Motor->KP           = 400;      //350   300
	Motor->KI           = 80;       //60    45
	Motor->KD           = 60;        //60    65
	Motor->PIDOUT       = 0;
	Motor->Target_Speed_old = 0;
  Motor->Current_Speed_last = 0;	
  Motor->Current_Speed_llast = 0;	
}
 
void Motor_Parameters_Init2(PID_struct *Motor)
{
	
	Motor->Current_Speed   = 0;
	Motor->Target_Speed   = 0;
	Motor->Encoder        = 0;
	Motor->E            = 0;
	Motor->E_L          = 0;
	Motor->E_L_L        = 0;
	Motor->KP           = 350;      //350   300
	Motor->KI           = 60;       //60    45
	Motor->KD           = 60;        //60    65
	Motor->PIDOUT       = 0;
	Motor->Target_Speed_old = 0;
  Motor->Current_Speed_last = 0;	
  Motor->Current_Speed_llast = 0;	
}
 
 
 
/************************************************************
        
【函数名称】Motor_Parameters_Init(PID_struct* Motor)
【功    能】增量式PID控制
【参    数】PID结构体
【返 回 值】输出占空比
【实    例】Motro_PID_Control(&Motor)
【注意事项】无
************************************************************/
short Motro_PID_Control(PID_struct* Motor)
{
  int duty;
  //占空比计算
  duty = Motor->KP*(Motor->E-Motor->E_L)+Motor->KI*Motor->E+Motor->KD*(Motor->E+Motor->E_L_L-2*Motor->E_L);
  //更新偏差
  Motor->E_L_L = Motor->E_L;
  Motor->E_L = Motor->E;
  Motor->PIDOUT+=duty;
  //限幅
 
  if(Motor->PIDOUT>10000)
    Motor->PIDOUT=10000;
  else if(Motor->PIDOUT<-10000)
    Motor->PIDOUT=-10000;
  return Motor->PIDOUT;
}
 
 
 
/*-----------------------------------------------------------
                
                                 
【函数名称】：Motor_Ctrl
【功    能】：动力PWM输出
【传入参数】：num 电机编号 1-2    duty占空比  0-10000对应0%-100%
【返回参数】：无
【实    例】：Motor_Ctrl(1, 2000);
【注意事项】：无
-----------------------------------------------------------*/
void Motor_Ctrl(uint8 num, int16 duty)
{
 if(num==1)
 {
  if(duty>0)
  {
   pwm_duty(PWM1_MODULE3_CHA_D0, 0);
   pwm_duty(PWM1_MODULE3_CHB_D1, duty*5);
  }
  else
  {
   pwm_duty(PWM1_MODULE3_CHA_D0, -duty*5);
   pwm_duty(PWM1_MODULE3_CHB_D1, 0);
  }
 }
 if(num==2)
 {
  if(duty>0)
  {
   pwm_duty(PWM2_MODULE3_CHB_D3, 0);
   pwm_duty(PWM2_MODULE3_CHA_D2, duty*5);
  }
  else
  {
   pwm_duty(PWM2_MODULE3_CHB_D3, -duty*5);
   pwm_duty(PWM2_MODULE3_CHA_D2, 0);
  }
 }
}
 

增量PID一般都放在中断里面，中断代码如下

			Motor1.Encoder=qtimer_quad_get(QTIMER_1, QTIMER1_TIMER0_C0)/4; 
			Motor2.Encoder=-qtimer_quad_get(QTIMER_1, QTIMER1_TIMER2_C2)/4;
			qtimer_quad_clear(QTIMER_1, QTIMER1_TIMER0_C0);
			qtimer_quad_clear(QTIMER_1, QTIMER1_TIMER2_C2);
			
			Motor1.Current_Speed=Motor1.Encoder;
			Motor2.Current_Speed=Motor2.Encoder;
			Motor1.E = Motor1.Target_Speed-Motor1.Current_Speed;
			Motor2.E = Motor2.Target_Speed-Motor2.Current_Speed;
			Motor_Ctrl(1, Motro_PID_Control(&Motor1));
			Motor_Ctrl(2, Motro_PID_Control(&Motor2));	

 我的大二下学期献给了智能车实验室，我不曾后悔，结实了很多朋友，老师，三生有幸，一起包夜调车，一起吹牛，一起睡实验室地板。

回过头来，真正有意义的不是那一张国奖的奖状，而是那青春不曾忘记，努力奋斗的经历。

十六届全国大学生智能汽车代码总结