基于視覺傳感器的智能車控制算法設計

王 建 ，張曉煒 ，楊 錦 ，昝 鑫 ，劉小勇

（1.中國科學技術大學 自動化系，安徽 合肥230027；2.西安交通大學 人工智能與機器人研究所，陜西 西安710049；3.西安交通大學 金禾經濟研究中心，陜西 西安710049；4.西安交通大學 電子與信息工程學院，陜西 西安710049）

以飛思卡爾智能車大賽為背景，制作了一個能巡線快速行駛的小車。

1 巡線控制算法

1.1 運動模型

智能車的運動控制是通過控制驅動馬達和伺服舵機實現的。在理想情況下，可以認為4個輪胎繞著同一個圓心做純滾動，其離散運動模型公式為：

其中V是線速度，W為角速度，x(t)、y(t)為小車的坐標，angle(t)為智能車的方向角（以小車前進方向為起始線，逆時針為正），Δt為采樣周期。模型如圖1所示。

由于模型車的運動速度較高，在行駛過程中輪胎與賽道會發生一定側滑，因此還需要引進側滑修正。

1.2 舵機控制

方向控制是控制策略的難點。傳統的PID算法[1，2]不需要對控制對象做細致分析，只需根據控制效果對參數進行調整[3]。但由于控制器的輸入量均對輸出量產生影響，且高度耦合，很難根據控制效果準確地對參數進行修正，因此參數的整定工作十分繁重。此外，當車的狀態不同時（如低速和高速時），最佳參數也有很大不同，這給進一步調試帶來了困難。

本文采用“最優曲率法”。控制思想是：模型車在一個控制周期內的運動軌跡可以近似為一段圓弧，整個運動軌跡可以看做由小段圓弧連接成的曲線。在假設線速度恒定的前提下，給定路徑上的一個點就能確定出智能車到達該點所應具有的舵機角度[4]。在每個控制周期內選擇路徑上的一點（或若干點），根據預先建立的圓弧運動模型計算出模型車在接下來的一個（或若干個）運動周期內要到達選定點所需的理想狀態，此時該控制周期的輸出量就是在未來幾個控制周期內使模型車的實際狀態盡可能逼近理想狀態的控制量。

以車的后橋中點為原點建立坐標系，車的中軸為y軸，其正向為車的前方。設(c_x，c_y)為路徑上的點在該坐標系中的坐標。則控制量的計算公式為[5]：

其中r_b為后輪的轉彎半徑，L為車的前橋與后橋的距離，wheel_angle為車前輪的理想角度，rudder為舵機的PWM輸出值，m為將wheel_angle轉換為rudder的系數（由實驗測得）。

這種方法在車速較低時可以取得很好的效果，當車速較高時，有很多因素對控制效果的影響會顯著增大，需要作出適當的補償。其中主要因素是控制量的實際輸出時刻與理想輸出時刻的延時，這個延時是由圖像處理和控制量的計算都需要一定時間造成的。對此，本文采取的方法是根據車的當前狀態估算出控制量的輸出時刻的路徑信息，用這個路徑信息計算控制量。

設延時期間車的角度變化量為Δw，則橫縱坐標變化量分別為 Δx、Δy，則當前時刻的前瞻點（c_x，c_y）在經過延時 Δt后的坐標為（p_x，p_y），用 p_x、p_y 計算控制量。設車速為v，當前車前輪轉角為β，車長為L，則計算公式為：

證明公式(7)～公式(11)：

R=L/tanβ，得公式（7）；

在直角三角形 ABC 中，∠ABC=(π-Δw)/2，Δx=AB×cos∠ABC=AB×sin(Δw/2)，Δy=AB×sin∠ABC=AB×cos(Δw/2)，而 AB 即為 v×Δt，即得公式（8）和公式（9）。

p_y=GF=AE×sin(Δw)+GE×cos(Δw)；而 AE=c_x-Δx，GE=c_y-Δy，由此得公式（10）和公式（11），如圖 2 所示。

在對下弦混凝土進行澆筑施工前，需對梁體實施檢查與調整，再進行高強螺栓的安裝。對于高強度螺栓，其施工主要分為兩部分，即為初擰與終擰，要求在一日內完成。待初擰完成后，采用敲擊法進行檢查，確認合格后，使用白色油漆做好標記，以便對終擰時是否出現轉動現象進行檢查。終擰主要使用扭矩法，開始前先用扳手做好標定，要求誤差不能超過扭矩±5%。各班組在操作之前和之后都應對扳手做好校正，將校正的結果準確填寫到記錄表格當中，然后由專門的校正人員進行簽字確認。操作完成后的校正過程中，若發現實際誤差較大，則需要對扳手做特別檢查，根據檢查結果及早處理。待終擰完成后，使用紅色的油漆作出相關標記。

另一個重要因素是側滑。智能車在行駛過程中存在側滑，可以通過側滑角作補償。設車輪的角度為θ，加入側滑后的等效車輪角度為θ′，本文假設二者符合如下關系：

在計算出車輪應當具有的實際角度θ′后，用上式求出θ，進而求出PWM的值。這樣就可以補償側滑的影響。

還有一個重要因素是，當車速較高時，舵機的響應速度相對較慢。要克服這個問題，應使舵機提前做出反應。因此需要在控制器的輸出量中引入路徑的趨勢。本文用前瞻點處的斜率k表示路徑的趨勢，在原控制量θ的基礎上增加一個與路徑趨勢相關的分量 θ″=para×v×k，其中para通過實驗結果自動調整，如果需要，可以將para設為和其他變量相關的函數。

經過上述改進之后，最優曲率法在車速較高的時候依然能夠取得很好的控制效果。此部分程序流程圖如圖3所示。

圖3 舵機控制流程圖

該算法的優點是參變量較少，且相互間的耦合很小，對控制效果的影響較為獨立，可以根據實驗結果快速確定調整量。只要對參數進行適當調整，就能取得較好的控制效果。

1.3 速度控制

在速度決策方面，首先嘗試了基于有效前瞻的速度控制[6]，即v=基準值+有效前瞻量×系數。這個策略的好處是簡單有效。但事實上智能車通過路線的最佳速度和前瞻量并不呈簡單的線性關系。如果處于彎道中，尤其是“十”字彎中時，由于車的阻力增大，車速也大大下降，同時由于車體運行較穩定，車輪不會發生大幅度轉向，側滑的風險很小，這時可以提速；當出彎時，如果檢測或預測到前方較平緩，可以放心加速。由于局部快不一定整體快，因此還要做好銜接，使得整個跑的過程中各速度間過渡圓滑。基于這些想法，本文在前面的速度決策上做了改進，在保證安全性的前提下提高了整體速度。

1.4 仿真調試

為了便于分析控制效果，本文搭建了一個Matlab仿真環境。在這個仿真環境中，車模的參數和動態響應特性都是參照實驗結果設計的，并引入了側滑、延時、舵角死區等很多干擾因素，可以在很大程度上反映算法的真實情況。仿真界面如圖4所示。

在實際的調試過程中，本文首先在仿真環境下設計算法，并分析模型車通過各種跑道時所需要的理想的舵機角度，測試通過后將算法移植到模型車上。如果實際控制效果與仿真結果差別較大，則將控制算法的執行情況與理想情況下的期望值進行比較，針對差別有目的地作出改進。這在很大程度上提高了調試效率。

2 貪婪路徑規劃

2.1 優化問題

優化問題中有兩個約束條件：

(1)車不出邊界。

(2)小車行駛路徑最短。主要是彎道切彎行駛，大小S彎走最優路徑。

對于一般問題，全局最優解是最好的結果，但是智能小車行駛的跑道是隨機的，并且小車傳感器獲得的道路信息有限，要得到全局最優解很困難，所以根據貪婪算法思想，在局部的每一點獲得最優解來逼近全局最優解。貪婪路徑規劃就是給出一些小車行駛路徑點，保證小車行駛時最大程度地滿足以上兩個優化條件。

2.2 最優解分析求解

比賽用的路徑只有兩種：直道和圓弧道。關鍵在圓弧道，分單圓弧道和多圓弧道，單圓弧道指銜接兩個直道的一個彎道，多圓弧道指S形彎道。

首先，分析單圓弧。單圓弧示意圖如圖5所示，已知路徑寬為60 cm，T點為小車傳感器識別出的最遠前瞻路徑點。

如果車向著T點行駛去，車有可能超出跑道。所以需要計算出最遠前瞻點和車頭連線（OT）與路徑圓弧線（弧 OT）之間的最遠距離 D，如果D超出 20 cm（車模寬20 cm，當車模恰好壓跑道邊時，車模中心與路徑線的距離是 20 cm），就不滿足約束條件(1)。

當D超出20 cm時，可以將前瞻點T后移，計算出使D=20 cm的T′點，如圖5所示。在給出的直線上根據小車的速度可以選取一點作為貪婪規劃的解路徑點。速度快時，選點前移，反之后移。

其次，對于多圓弧S形道，需要找到兩個圓弧的連接點，稱為轉折點。圖6中T點為轉折點。S形道路與一般彎道路徑不同，不能用單圓弧的方法求出最優路徑點，因為最遠前瞻點可能跨兩個圓弧，求取距離D困難，最重要的是用單圓弧的方法利用最遠前瞻點求出的解不是最優的解。可以發現在圓弧轉折點前面的圓弧（弧OT）是一個單圓弧，用這段單圓弧來求解才是合理的。利用最小二乘法可以對每一個點求出一個斜率，其中轉折點的特點是轉折點是斜率曲線的極值點。可以發現，對于 90°小S彎，D等于14.65 cm，小車直線通過，而120°中等 S彎，D等于 25 cm。

需要注意，由于傳感器的視野和圖像處理的限制，有時最遠前瞻點只能取到N點，如圖5所示。這時可以使用廣角鏡頭（實驗中選取焦距f=2.8 mm）擴大視野，提高圖像處理算法的精度來使前瞻點盡量靠前，到達T附近。

本文根據轉向模型，提出了“最優曲率”算法及“貪婪路徑規劃”算法。實驗表明，這兩種算法結合起來能使小車更安全、快速地行駛，可作為除PID算法和模糊控制算法以外的第三種選擇。

[1]卓晴，黃開勝，邵貝貝，等.學做智能車[M].北京：北京航空航天大學出版社，2007.

[2]馬艷，徐淑華，周建春.基于攝像頭尋跡的智能電動車的設計[J].機械與電子，2009，27(2)：21-24.

[3]艾寧，瞿少成，劉冬，等.基于 CCD攝像頭的智能車路徑識別及跟蹤研究[J].電子測量技術，2009，32（8）：77-80.

[4]尹念東，余群.基于橫向預瞄偏差的駕駛員前視軌跡控制模型[J].汽車工程，2002，24(4)：287-289.

[5]魏玉虎，石琛宇，姜建釗，等.基于視覺的智能車轉向控制策略[J].電子技術應用，2009，35(1)：130-134.

[6]蔡慶楠，蔡興旺，潘錦洲.上海大學S.U.L.挑戰者隊技術報告[R]，2009.