Δu(k)=u(k)-u(k-1)=KP[err(k)-err(k-1)]+KIerr(k)+KD[err(k)-2err(k-1)+err(k-2)]

式中,KP為比例系數，KI為積分系數，KD為微分系數。Δu為舵機給定的增量[7-11]。

2 轉向模型

獲得道路的位置信息，即獲得了此刻小車的轉向角。參考文獻[11]中的舵機轉向模型，在理想狀態下，此刻控制舵機使前輪偏轉相應角度即可實現小車對道路的跟隨。舵機的轉向相當于給車一個向心力，在轉角不變時，小車做圓周運動；如果控制驅動電機的PWM脈寬不變，則小車做勻速圓周運動，驅動電機的 PWM脈寬對應的小車速度相當于勻速圓周運動的線速度。如圖5所示，設車的側輪距為b，舵機轉角為θ，O為車做圓周運動的圓心，R為外輪所對應半徑,則每一個轉角對應圓周運動的半徑[2]如下：

設控制周期為T,小車當前速度為V，α為一個控制周期T內小車沿圓周行駛的弧度數，則有：

由以上模型可以看出，不同的速度和轉角會在一個控制周期T內得到不同的弧度，對上述公式離散化后可以得到:

小車在轉彎的過程中，舵機轉角受多種因素限制(如：小車轉向機構造成的轉向的延滯、舵機的時間常數等），是一個漸變的過程，而利用公式(8)進行計算的前提是舵機的方向能隨控制量的改變而立即改變。為了簡化分析，將舵機模型視為僅具有延滯時間TD的延滯環節而忽略其時間常數。如圖6所示，由于TD的存在，速度越大，在滯后時間TD內，小車行走的距離越遠。

3 轉向策略設計

如圖7所示，設小車前方距離D處為彎道，小車此刻速度為V，結合圖6小車舵機轉向理想模型響應曲線，如果讓小車在A點入彎，速度必須滿足:

D=V×TD (9)

在圖4中，對于基準位置1和基準位置2，由道路信息提取的結果，為了保證A點入彎，基準位置2比基準位置1更早地發出轉向命令，需要更大的速度才可以在A點入彎，在保證兩位置的速度可以安全過彎的前提下，基準位置2得到了更高的入彎速度，所以更早地得到彎道的信息可以提高入彎速度。同時小車要想快速地入彎，必須將基準位置提到一個與此刻速度匹配的位置，才可以抵消舵機延時帶來的附加行駛路徑。但并不是說只要將基準位置選得足夠遠就可以得到很大的入彎速度，答案是否定的。小車的轉向為一個近似的圓周運動，不同的速度顯然對應不同的行駛半徑，即不同的路徑。速度快時，半徑大；速度慢時，半徑小。過快的速度會使小車沖出賽道。競賽賽道的彎道主要由360°、180°、90°、S型賽道組成。在圖8所示的180°彎道中，中間的粗實線為引導線，最外圍的實線為賽道邊界，曲線1、2、3、4為小車行駛路線。設所有行駛路線有同樣的入彎角度,曲線1、2、3為B點入彎，速度不同；曲線4為E點入彎，速度與曲線3速度一致。

顯然，曲線1、2、3中曲線3速度為最快，但路徑卻最長，同時還有沖出賽道的危險；曲線2雖“抄近道”，但速度稍慢；曲線1由于速度太慢而無法過彎。不難看出，其實曲線2、3、4的過彎時間相差并不是很大。但是由于曲線2中，由直道AB入彎時需要減速，過彎后需加速，這段時間卻比曲線3、4來得長。經綜合考慮，曲線3、4過彎時間最短。曲線3、4中曲線3很顯然有沖出賽道的危險，因此曲線4為過180°彎的最優路徑。可以看出，高速時提前入彎可以使小車以一個比較高的速度駛過彎道，同時保證了安全而又不會沖出賽道。

對于90°彎道，與180°類似，最優的策略是為保持直道高速行駛，通過上述的提前入彎策略保證安全過彎。

對于S道的進入，可以類推。而對于S道的行駛則應適當降低車速并將基準位置適當降低。如圖9所示，最短路徑也為最快速度，兩者是統一的。真實過程中很難做到讓車走CD路線，但是如果能使舵機保持在一個小角度左右調節的過程中則可逼近CD路線。實踐證明，高速通過S道時避免劇烈的角度變化給定為最優策略。

綜合以上分析，可以得到以下兩點結論：

(1) 基準位置距小車的距離與速度呈正比關系。想要不減速入彎，必須改變基準位置，使之可以滿足公式(9)。

(2) 速度與轉彎半徑呈正比關系。保持高速入彎，同時不沖出賽道，提前入彎策略可以有效解決這個問題。

4 策略的實現

由以上分析可知，場景中的基準位置可以近似看成入彎點，并且不同的速度會導致不同的入彎點，也會導致不同的過彎路徑。因此，需完成的控制任務是：高速過彎，在入彎前不減速，為了防止沖出賽道還需要提前入彎。可見，需要在與速度適應的入彎點的基礎上進一步提前基準位置，即基準位置離小車的前端距離為：

L=V×TD+V×τ (10)

式中，L為基準位置離小車的距離，V為此刻小車的速度，V×TD為抵消延遲時間帶來的附加行駛距離，V×τ為提前入彎點的距離。

可以看出，基準位置與小車的距離是一個與小車速度相關的變量，不同的速度需要不同的取值。對于不同的速度將基準位置按照公式(10)設置，這樣即可以實現高速入彎。為了得到較佳的入彎點，本策略對圖像平面(64×106)做了進一步的處理，針對已經得到的圖像平面(64×106)，將圖像平面平分為10個區域，每個區域求取引導線的平均位置，對于沒有引導線的區域使用一個不可能出現的值代替，這樣便將引導線信息簡化為10行信息表示，記為：average[i],i=1，2，…，10。其中，average[i]記錄每行信息中引導線的平均位置的列坐標值，i標表示行值。圖10為最終的圖像平面。