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

舵機 PID 控制首先要得到一個偏差值，得到偏差值的途徑是識別賽道中間的電磁線或者使用攝像頭進行圖像識別。目前的方案是攝像頭采集圖像，使用大津法求動態閾值，圖像二值化，然后從圖像底端向上搜索左右邊線，根據左右邊線計算中線。根據我的理解屏幕中間的位置也就是車的正前方是當前值，根據圖像提取的中線是預期值。在中部靠下的位置取一點與同行屏幕中點相減計算偏差值的一部分，使用線性回歸方程計算中線的斜率作為偏差的另一部分。這兩部分乘上合適的系數相加得到總偏差。在實踐的過程中，我發現斜率的系數是 0 比較合適。

int16 regression(uint16 *line,uint16 startLine,uint16 endLine){//線性回歸方程計算斜率
  uint16 num=0;
  uint16 sumX=0;
  uint16 sumY=0;
  uint16 aveX=0;
  uint16 aveY=0;
  
  uint16 i;
  for(i=startLine;i<endLine;i++){
    num++;
    sumX+=i;
    sumY+=line[i];
  }
  aveX=sumX/num;
  aveY=sumY/num;
  
  int16 slopeUp=0;
  int16 slopeDown=0;
  
  for(i=startLine;i<endLine;i++){
    slopeUp+=(line[i]-aveY)*(i-aveX);
    slopeDown+=(i-aveX)*(i-aveX);
  }
  
  return slopeUp/slopeDown;
}

線性回歸方程求斜率的函數我目前沒有用到。

int16 imageProcess(uint8 *image,uint16 col,uint16 row){
  uint16 i,j;
  uint16 edgeL[MT9V03X_CSI_H];
  uint16 edgeR[MT9V03X_CSI_H];
  uint16 midline[MT9V03X_CSI_H];
  
  midline[MT9V03X_CSI_H-1]=col/2;
  
  for(i=row-2;;i--){
    edgeL[i]=0;
    edgeR[i]=col-1;
    
    for(j=midline[i+1];j>0;j--){
      if(*(image+i*col+j)==0){
        edgeL[i]=j;
        break;
      }
    }
    
    for(j=midline[i+1];j<col-1;j++){
      if(*(image+i*col+j)==0){
        edgeR[i]=j;
        break;
      }
    }
    midline[i]=(edgeL[i]+edgeR[i])/2;
    *(image+i*col+midline[i])=0;
    if(i==0){
      break;
    }
  }
  
  int16 offset=（MT9V03X_CSI_W/2-midline[105])*2;//regression(midline,60,120);
  
  return offset;
}

 只要圖像合適，這個偏差值還是相當靠譜的。

void servoCtrl(int16 offset){
  uint16 P=6,D=0;
  
  static int16 offset1;
  static int16 offset2;
  static int16 offset3;
  static int16 offset4;
  
  offset4=offset3;
  offset3=offset2;
  offset2=offset1;
  offset1=offset;
  
  int16 delta=P*offset+D*(offset-offset4);
  delta=(((delta>-360)?delta:-360)<360)?delta:360;
  
  pwm_duty(PWM4_MODULE2_CHA_C30,3600+delta);
}

至於這個舵機控制函數，仍然是把 D 調成 0 最理想了，這樣來看這個函數寫的就很多余。

到目前為止，我的小車已經可以在不考慮環島的情況下跑下一圈了。沒有斜入十字的情況，十字不經處理直接沖過去就好。