從0開始的智能車代碼（4）

環島是處理起來相對比較困難的一種賽道元素。條條大路通環島，解決環島問題也有許多不同的方案。在我參考過環島的代碼時，復雜的代碼邏輯和龐大的代碼量使我望而卻步，見到巨人的肩膀魁梧卻布滿荊棘，我決定自己動手豐衣足食。我的方法是 MELP 四步過環島，即遇環（Meet）、入環（Enter）、出環（Leave）、過環（Pass）。

首先，在尋找左右邊線時留意邊線橫坐標（所在列數）突然變化的點，我將這種點稱為跳變點。我將跳變點分為兩類，賽道交匯形成的跳變點稱為 A 字跳變點，賽道分支形成的跳變點稱為 V 字跳變點。所有跳變點可以分成左側 A 字跳變點、左側 V 字跳變點、右側 A 字跳變點、右側 V 字跳變點這4種。

遇環：圖像中出現 A 字跳變點，說明小車遇到一個圓環。此時將遇環標志位置1，同時記錄下圓環的方向。

入環：在遇環標志位為1的情況下，圖像中出現 V 字跳變點，說明小車即將駛入圓環。此時將入環標志位置1。根據遇環時記錄下的圓環方向進行補線：若圓環為左側環，將跳變點與圖像右下角連線作為右邊線；若圓環為右側環，將跳變點與圖像左下角連線作為左邊線。

出環：在入環標志位為1的情況下，圖像中出現 A 字跳變點，說明小車即將駛出圓環。此時將出環標志位置1。根據遇環時記錄下的圓環方向進行補線：若圓環為左側環，將跳變點與圖像頂部中點相連作為右邊線；若圓環為右側環，將跳變點與圖像頂部中點連線作為左邊線。

過環：與入環的情況相反。在出環標志位為1的情況下，圖像中出現 V 字跳變點，說明小車即將通過圓環。此時將遇環標志位、入環標志位、出環標志位清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;
  
  uint8 jumpFlagL=0;
  uint8 jumpFlagR=0;
  
  uint16 pointX=0;//跳變點X坐標
  uint16 pointY=0;//跳變點Y坐標
  uint8 pointSide=0;//跳變點方向，1表示左，2表示右
  uint8 pointType=0;//跳變點分類，1表示A字跳變點，2表示V字跳變點
  
  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;
      }
    }
    
    //環島處理開始（見到巨人的肩膀魁梧卻布滿荊棘，我決定自己動手豐衣足食）
    //環島處理所有數都是非負數
    //bingo：和十字區分：掃描到一側的跳變點后（下半部分），對另一側的邊線進行判斷（下3/4部分）
    
    //尋找跳變點
    
    if(i>MT9V032_H*1/3){//只判斷下2/3部分圖像的跳變點（賽道突然變窄）
      //左側A字跳變點
      if(edgeL[i+1]-edgeL[i]>20){
        jumpFlagL=1;
        pointX=edgeL[i+1];
        pointY=i+1;
        pointSide=1;
        pointType=1;
      }
      //左側V字跳變點
      else if(edgeL[i]-edgeL[i+1]>20){
        jumpFlagL=1;
        pointX=edgeL[i];
        pointY=i;
        pointSide=1;
        pointType=2;
      }
      //右側A字跳變點
      if(edgeR[i]-edgeR[i+1]>20){
        jumpFlagR=1;
        pointX=edgeR[i+1];
        pointY=i+1;
        pointSide=2;
        pointType=1;
      }
      //右側V字跳變點
      else if(edgeR[i+1]-edgeR[i]>20){
        jumpFlagR=1;
        pointX=edgeR[i];
        pointY=i;
        pointSide=2;
        pointType=2;
      }
    }
    
    midline[i]=(edgeL[i]+edgeR[i])/2;//考慮環島補線后不能邊判斷邊線邊計算中線，這個中線只用來下一步尋找邊界
    
    if(i==0){
      break;
    }
  }
  
  //四步過環，遇環、入環、出環、過環
  static uint8 meetRingFlag=0;//遇環標志位
  static uint8 enterRingFlag=0;//入環標志位
  static uint8 leaveRingFlag=0;//出環標志位
  static uint8 ringSide=0;//環島類型，1表示左，2表示右

  uint16 pointLLCX=0;
  uint16 pointLLCY=MT9V032_H-1;
  uint16 pointLRCX=MT9V032_W-1;
  uint16 pointLRCY=MT9V032_H-1;
  
  uint16 pointTMX=MT9V032_W/2;
  uint16 pointTMY=0;
  
  float stepLength;//橫坐標插值步長
  
  if(jumpFlagL^jumpFlagR){//若有一側出現跳變點
    if(pointType==1){//若為A字跳變點，說明出環或遇環
      if(enterRingFlag==1){//出環
        leaveRingFlag=1;
        if(ringSide==1){//環島在左邊，從頂部中點到出環標志點連線，補右邊線
          stepLength=(float)(pointTMX-pointX)/(float)(pointTMY-pointY);
          for(i=0;i<pointTMY-pointY;i++){
            edgeR[pointTMY-i]=pointTMX-(int)(i*stepLength);
            binaryImage[pointTMY-i][edgeR[pointTMY-i]]=binaryImage[pointTMY-i][edgeR[pointTMY-i]-1]=binaryImage[pointTMY-i][edgeR[pointTMY-i]+1]=0;//粗線
          }
        }
        else if(ringSide==2){//環島在右邊，從頂部中點到出環標志點連線，補左邊線
          stepLength=(float)(pointX-pointTMX)/(float)(pointTMY-pointY);
          for(i=0;i<pointTMY-pointY;i++){
            edgeL[pointTMY-i]=pointTMX+(int)(i*stepLength);
            binaryImage[pointTMY-i][edgeL[pointTMY-i]]=binaryImage[pointTMY-i][edgeL[pointTMY-i]-1]=binaryImage[pointTMY-i][edgeL[pointTMY-i]+1]=0;//粗線
          }
        }
      }
      else if(meetRingFlag==0){//遇環
        meetRingFlag=1;
        ringSide=pointSide;
      }
      //目前A字跳變點可以自動處理，無需補線
    }
    else if(pointType==2){//若為V字跳變點，說明入環或過環
      if(leaveRingFlag==1){//過環//與入環相反//須先判斷過環
        meetRingFlag=0;
        enterRingFlag=0;
        leaveRingFlag=0;
        //過環補線
        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]]=binaryImage[pointLRCY-i][edgeR[pointLRCY-i]-1]=binaryImage[pointLRCY-i][edgeR[pointLRCY-i]+1]=0;//粗線
          }
        }
        else 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]]=binaryImage[pointLLCY-i][edgeL[pointLLCY-i]-1]=binaryImage[pointLLCY-i][edgeL[pointLLCY-i]+1]=0;//粗線
          }
        }
      }
      else if(meetRingFlag==1){//入環
        enterRingFlag=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]]=binaryImage[pointLRCY-i][edgeR[pointLRCY-i]-1]=binaryImage[pointLRCY-i][edgeR[pointLRCY-i]+1]=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]]=binaryImage[pointLLCY-i][edgeL[pointLLCY-i]-1]=binaryImage[pointLLCY-i][edgeL[pointLLCY-i]+1]=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,4,jumpFlagL^jumpFlagR);
  lcd_showint8(0,5,ringSide);
  lcd_showint8(0,6,meetRingFlag*100+enterRingFlag*10+leaveRingFlag);
  
  offset=MT9V032_W/2-midline[98];
}

添加環島處理后，斜入十字時圖像中會出現一個 A 字跳變點，將十字誤判成環島，我設想的解決方案在檢測到跳變點后在另一側的更大范圍內判斷丟線情況，若出現較大范圍丟線則可判斷為十字。