从0开始的智能车代码（5）

环岛处理方法存在一个严重的 bug，在入环或过环时会出现反向打角的现象。以左侧入环为例，小车首先会发现左侧存在一个 V 字跳变点，同时根据这个跳变点补右边线，这是正确的。然而，当小车向左打角到一定程度之后，原先的 V 字跳变点会被认为是右侧的跳变点，然后进行反向补线。

解决这个问题的方法是判断出环岛方向。具体的方法是在遇环时判断跳变点的方向或者在入环时判断第一次发现跳变点的方向。

另一个 bug 是当 V字跳变点位于图像正中时，在两侧都会判断出跳变点，根据之前的判断方法，两侧都出现跳变点会被判定成十字。我的解决方案是添加一段特殊情况处理代码。

由于出环补线的程序运行效果不太理想，我更改了出环的程序。在图像中判断存在出环跳变点时，停止刷新偏差值，保持原有的偏差值直到发现足够多的右侧边界点。

void imageBinary(void)
{
  uint16 grayCount[256]={0};
  uint32 graySum=0;
  uint16 pixelSum=0;
  float graypro[256];
  
  uint8 threshold;
  uint16 i,j;
  uint16 step=2;//间隔
  
  for(i=0;i<MT9V032_H;i+=step){
    for(j=0;j<MT9V032_W;j+=step){
      grayCount[image[i][j]]++;
      graySum+=image[i][j];
      pixelSum++;
    }
  }
  
  for(i=0;i<256;i++){
    graypro[i]=(float)grayCount[i]/pixelSum;
  }
  
  float w0,w1,u0Temp,u1Temp,u0,u1,u,deltaTmp,deltaMax=0;
  for(i=0;i<256;i++){
    w0=w1=u0Temp=u1Temp=u0=u1=u=deltaTmp=0;
    for(j=0;j<256;j++){
      if(j<=i){
        w0+=graypro[j];
        u0Temp+=j*graypro[j];
      }
      else{
        w1+=graypro[j];
        u1Temp+=j*graypro[j];
      }
    }
    u0=u0Temp/w0;
    u1=u1Temp/w1;
    u=u0Temp+u1Temp;
    deltaTmp=w0*(u0-u)*(u0-u)+w1*(u1-u)*(u1-u);
    if(deltaTmp>deltaMax){
      deltaMax=deltaTmp;
      threshold=i;
    }
  }
  
  for(i=0;i<MT9V032_H;i++){
    for(j=0;j<MT9V032_W;j++){
      binaryImage[i][j]=(image[i][j]>threshold)?255:0;
    }
  }
  if(threshold<168){//出界停车
    DisableInterrupts;
    ctimer_pwm_duty(TIMER1_PWMCH0_A18,0);
    ctimer_pwm_duty(TIMER2_PWMCH1_B4,0);
  }
}

void imageProcess(void){
  uint16 i,j;
  uint16 edgeL[MT9V032_H];
  uint16 edgeR[MT9V032_H];
  uint16 midline[MT9V032_H];
  
  edgeL[MT9V032_H-1]=0;
  edgeR[MT9V032_H-1]=MT9V032_W-1;
  midline[MT9V032_H-1]=MT9V032_W/2;
  
  for(i=MT9V032_H-2;;i--){
    edgeL[i]=0;
    edgeR[i]=MT9V032_W-1;
    
    for(j=midline[i+1];j>0;j--){
      if(binaryImage[i][j]==0){
        edgeL[i]=j;
        break;
      }
    }
    
    for(j=midline[i+1];j<MT9V032_W-1;j++){
      if(binaryImage[i][j]==0){
        edgeR[i]=j;
        break;
      }
    }
    
    midline[i]=(edgeL[i]+edgeR[i])/2;//考虑环岛补线后不能边判断边线边计算中线，这个中线只用来下一步寻找边界
    
    if(i==0){
      break;
    }
  }
  
  /*----环岛处理在这里开始----*/
  
  uint8 jumpFlagL=0;
  uint8 jumpFlagR=0;
  
  uint16 pointLX=0;//左边线跳变点X坐标
  uint16 pointLY=0;//左边线跳变点Y坐标
  uint16 pointRX=0;//右边线跳变点X坐标
  uint16 pointRY=0;//右边线跳变点Y坐标
  uint16 pointX=0;//跳变点X坐标
  uint16 pointY=0;//跳变点Y坐标
  
  uint8 pointSide=0;//跳变点方向，1表示左，2表示右
  uint8 pointType=0;//跳变点分类，1表示A字跳变点，2表示V字跳变点
  
  for(i=MT9V032_H*1/3;i<MT9V032_H-1;i++){//只判断下2/3部分图像的跳变点
    //左侧A字跳变点
    if(edgeL[i]-edgeL[i-1]>20){
      jumpFlagL=1;
      pointLX=edgeL[i];
      pointLY=i;
      pointSide=1;
      pointType=1;
    }
    //左侧V字跳变点
    else if(edgeL[i-1]-edgeL[i]>20){
      jumpFlagL=1;
      pointLX=edgeL[i-1];
      pointLY=i-1;
      pointSide=1;
      pointType=2;
    }
    //右侧A字跳变点
    if(edgeR[i-1]-edgeR[i]>20){
      jumpFlagR=1;
      pointRX=edgeR[i];
      pointRY=i;
      pointSide=2;
      pointType=1;
    }
    //右侧V字跳变点
    else if(edgeR[i]-edgeR[i-1]>20){
      jumpFlagR=1;
      pointRX=edgeR[i-1];
      pointRY=i-1;
      pointSide=2;
      pointType=2;
    }
  }
  
  uint8 jumpFlag=(jumpFlagL^jumpFlagR)&&((pointLY>MT9V032_H/2)||(pointRY>MT9V032_H/2));//防止十字误判
  if((pointLX==MT9V032_W/2)&&(pointRX==MT9V032_W/2)&&(pointLY>MT9V032_H/2)&&(pointRY>MT9V032_H/2)){
    jumpFlag=1;//入环V形特殊情况
  }
  
  if(jumpFlagL==1){
    pointX=pointLX;
    pointY=pointLY;
  }
  else if(jumpFlagR==1){
    pointX=pointRX;
    pointY=pointRY;
  }
  
  //四步过环，遇环、入环、出环、过环
  static uint8 meetRingFlag=0;//遇环标志位，第一个A字标志点——第一个V字标志点
  static uint8 enterRingFlag=0;//入环标志位，第一个V字标志点——第二个A字标志点
  static uint8 leaveRingFlag=0;//出环标志位，第二个A字标志点——第二个V字标志点
  static uint8 passRingFlag=1;//过环标志位，第二个V字标志点——下一个A字标志点
  static uint8 ringSide=0;//环岛类型，1表示左，2表示右
  
  uint16 pointLLCX=0;
  uint16 pointLLCY=MT9V032_H-1;
  uint16 pointLRCX=MT9V032_W-1;
  uint16 pointLRCY=MT9V032_H-1;
  
  float stepLength;//横坐标插值步长
  
  if(jumpFlag){//若有有效跳变点
    if(pointType==1){//若为A字跳变点，说明出环或遇环
      if(passRingFlag==1){//遇环//遇环仅判断方向用
        meetRingFlag=1;
        passRingFlag=0;
      }
      else if(enterRingFlag==1){//出环
        leaveRingFlag=1;
        enterRingFlag=0;
      }
    }
    else if(pointType==2){//若为V字跳变点，说明入环或过环
      if(meetRingFlag==1){//入环
        enterRingFlag=1;
        meetRingFlag=0;
      }
      else if(leaveRingFlag==1){//过环
        passRingFlag=1;
        leaveRingFlag=0;
      }
    }
  }
  
  if(meetRingFlag==1){
    if(jumpFlag==1){
      ringSide=pointSide;
    }
  }
  else if(enterRingFlag==1){
    if(jumpFlag==1){
      //入环补线
      //入环标志点（pointAX，pointAY），图像左下角（pointLLCX，pointLLCY），图像右下角（pointLRCX，pointLRCY）
      if(ringSide==1){//入环标志点在左边，从右下角点到入环标志点连线，补右边线
        stepLength=(float)(pointLRCX-pointX)/(float)(pointLRCY-pointY);
        for(i=0;i<pointLRCY-pointY;i++){
          edgeR[pointLRCY-i]=pointLRCX-(int)(i*stepLength);
          binaryImage[pointLRCY-i][edgeR[pointLRCY-i]]=0;
        }
      }
      else if(ringSide==2){//入环标志点在右边，从左下角点到入环标志点连线，补左边线
        stepLength=(float)(pointX-pointLLCX)/(float)(pointLLCY-pointY);
        for(i=0;i<pointLLCY-pointY;i++){
          edgeL[pointLLCY-i]=pointLLCX+(int)(i*stepLength);
          binaryImage[pointLLCY-i][edgeL[pointLLCY-i]]=0;
        }
      }
    }
  }
  else if(leaveRingFlag==1){
    if(ringSide==1){//若左圆环
      for(i=MT9V032_H*3/4;i<MT9V032_H-1;i++){//判断右边线下部分
        if(edgeR[i]==MT9V032_W-1){//若丢线
          return;//不改变偏差
        }
      }
    }
    if(ringSide==2){
      for(i=MT9V032_H*3/4;i<MT9V032_H-1;i++){
        if(edgeL[i]==0){
          return;
        }
      }
    }
  }
  
  else if(passRingFlag==1){
    if(jumpFlag==1){
      if(ringSide==1){
        stepLength=(float)(pointX-pointLLCX)/(float)(pointLLCY-pointY);
        for(i=0;i<pointLLCY-pointY;i++){
          edgeL[pointLLCY-i]=pointLLCX+(int)(i*stepLength);
          binaryImage[pointLLCY-i][edgeL[pointLLCY-i]]=0;
        }
      }
      else if(ringSide==2){
        stepLength=(float)(pointLRCX-pointX)/(float)(pointLRCY-pointY);
        for(i=0;i<pointLRCY-pointY;i++){
          edgeR[pointLRCY-i]=pointLRCX-(int)(i*stepLength);
          binaryImage[pointLRCY-i][edgeR[pointLRCY-i]]=0;
        }
      }
    }
  }
  
  /*----环岛处理在这里结束----*/
  
  for(i=0;i<MT9V032_H;i++){//重新计算中线
    midline[i]=(edgeL[i]+edgeR[i])/2;
    binaryImage[i][midline[i]]=binaryImage[i][midline[i]-1]=binaryImage[i][midline[i]+1]=0;//粗线
  }
  lcd_showint8(0,5,jumpFlag);
  lcd_showint16(0,6,meetRingFlag*1000+enterRingFlag*100+leaveRingFlag*10+passRingFlag);
  
  offset=MT9V032_W/2-midline[88];
}