舵机 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 最理想了，这样来看这个函数写的就很多余。
到目前为止，我的小车已经可以在不考虑环岛的情况下跑下一圈了。没有斜入十字的情况，十字不经处理直接冲过去就好。
原文：https://www.cnblogs.com/stephen0829/p/13358000.html