基于視覺的重型車輛車道保持控制方法

付 行， 任文峰， 王 凱

（陜西重型汽車有限公司， 陜西 西安 710200）

車道保持研究方法較多，基于位置偏差和橫擺角誤差模型［1］，能很好地控制車輛進行車道保持。基于單目視覺的車道保持［2］對如何識別車道線進行了詳細說明。通過檢測車輛位置相對于車道中心線的偏移量，根據設計的控制規律計算出轉向盤轉角大小和方向［3］。基于位置偏差和航向角偏差的聯合控制模型［4］，仿真驗證效果較好。本文主要突出介紹車道保持控制方法，本文介紹橫向控制理論在車道保持中的應用原理以及PD控制算法，應用Simulink和TruckSim聯合仿真和實車驗證控制算法的合理性和有效性。

1 車道保持系統方案設計

視覺傳感器采集車道信息，通過CAN線傳入上位機系統，以車輛質心實際位置和車道中心的橫向偏移為輸入，通過預瞄理論結合PD控制算法計算輸出轉角，通過CAN線輸出給底層下位機控制系統控制車輛在車道內行駛。

1.1 硬件

車道保持系統所用到的硬件設備有Mobileye視覺傳感器，用于感知道路信息，主要是車道曲率、斜率、車道寬度等信息。為了便于快速開發系統，車道保持的計算平臺選擇的是工控機，該工控機集成CAN卡便于接收視覺傳感器信息和向底層發送控制命令，電機控制器收到控制指令。車道保持測試平臺是我廠生產的第3代重型越野車，配備了電動助力轉向電機。系統硬件架構如圖1所示。

圖1 硬件架構圖

1.2 軟件系統

該軟件系統基于C#開發，應用多線程技術將傳感器數據接收解析和橫向控制算法分離執行，使得數據接收和控制算法互不影響，提高了系統可靠性。車道保持的軟件系統如圖2所示。視覺傳感器感知獲取道路數據，按協議解析后會得到車道線曲率ρ為二次項數，在直道時ρ接近于0。還能獲知一次項系數k、距離左邊車道的距離dl，距離右邊車道的距離dr等信息。由此可得道路中心線方程為：

圖2 車道保持軟件系統

式 （1）中以攝像頭為原點，x為車輛行駛方向，y為垂直于x軸向右，（dl+dr）/2為視覺傳感器和車道中心的橫向偏移量。

2 控制策略

車道保持系統主要研究的是通過橫向控制策略控制車輛在車道內穩定快速行駛，本文研究將橫向位置偏差作為控制量，通過PD算法調節系統響應和超調，輸出控制車輛轉向的方向盤轉角。同時加入車輛橫向約束，限制道路曲率變化較大時帶來的不穩定性問題。

2.1 橫向控制算法

預瞄理論是參照駕駛員開車時眼睛注視道路前方，控制方向盤使車輛按照駕駛員的意愿向前行駛。向前預瞄的距離與車輛速度、道路曲率等因素有關［5］。本文在研究時縱向控制約束限定速度會隨著道路曲率的增大而減小，所以這里不加入道路曲率對預瞄距離的約束，只考慮速度對預瞄距離的影響。當速度較快時，預瞄距離質心應遠一些，當速度較慢時預瞄距離車輛質心近一些。如圖3所示，假設車輛位于車道線左邊 （右邊與左邊情形類似），矩形框代表車輛，質心為O。從車輛質心O沿著車輛航向方向向前預瞄一定距離OB為d，B就是預瞄點。點B向車道中心線做垂線相交于點C，BC是車輛預瞄點的橫向偏差ds。這里設定在車道線左邊的橫向偏差為負，右邊的橫向偏差為正。

圖3 橫向偏差示意圖

設預瞄距離OB為λv，λ是速度v的倍數，所以以攝像頭為原點，點B的坐標為 （0，λv）。點B的坐標已知，求點B到道路中心線的距離ds，由式 （1）可知道路中心線的方程已知，則點B到道路中心線任一點的距離ds平方為：

為了簡便，令f=ds2。要求點B到中心線的距離就是求點B到道路中心線的最近距離，因此式 （2）對x求導得：

這是條件極值問題，求函數的極小值可得點C（x，y）就是道路中心線離預瞄點最近的點，進而可求得預瞄橫向偏差ds。

考慮到重型卡車自身質量、體積較大、慣性較大等特征，單獨使用預瞄前向控制會引起調節響應過慢，車輛進入穩態時間較長。本文引入車輛質心橫向偏差作用修正車輛和車道線的位置偏差。點O到車道中心線的距離OA為dv。由式（1）可知，視覺傳感器和道路中心線的距離為：

根據相似三角形對應邊成比例，可知dv：

以車輛前向預瞄的橫向偏差與質心的橫向偏差共同作用作為PD控制的輸入量進行控制，這里約定位置偏差為 ：

式中：m，n——比例系數，m和n之和為1。

2.2 PD控制

如圖4所示，車道保持控制的目標是通過控制方向盤角度減小與車道中心線的位置偏差。以2.1位置偏差為控制輸入，通過PD參數控制環節，調整響應速度和減小超調。

圖4 基于位置偏差的控制

PD控制量比例增益P主要調節方向盤轉角大小，車速低時P值大，高速時P值小。控制量D （微分系數）影響自動駕駛系統的響應速度，可消除或減弱系統慣性以及滯后帶來的振動現象。通過穩態轉向增益、增益調度的調節之后，得到最優期望前輪轉角，這里不考慮轉向系統的非線性，認為前輪轉角與方向盤成比例關系，前輪轉角乘以轉向傳動比就得到方向盤轉角。飽和特性是指前輪轉角設定在一定范圍內，根據車輛不同設定范圍不同，本文設定為-0.785～0.785弧度。這里要注意的是在乘以傳動比系數時首先要將弧度轉換為度。

2.3 橫向約束

車輛在行駛道路上存在曲率變化較大，如果不考慮橫向約束執行機構轉動速率的限制，可能造成車輛轉角突然變大，導致車輛失穩，甚至側翻。為了保證車輛的橫向穩定性需要對橫向加以約束。

式中：a——車輛橫向加速度；v——車輛速度；r——轉彎半徑，r與道路曲率ρ成倒數。

本文不考慮車輛縱向控制，設定車輛在一定速度下，車道曲率由車輛傳感器給出，所以由式 （7）可知當道路曲率變大時，車輛加速度會變小。由動力學關系求得：

式中：w——車輛轉角；L——車輛軸距。

由式 （7）和 （8）可知車輛的轉角可以隨道路曲率變化，將w作為車輛在道路曲率ρ，速度v給定條件下的最大轉角作為橫向約束。

3 車道保持系統仿真與測試

通過仿真驗證橫向偏差控制算法的有效性，同時調節PD參數，形成經驗積累，作為實車測試調試的指導依據。

3.1 Simulink與TruckSim聯合仿真

基于TruckSim搭建我廠重型車輛整車模型如圖5所示，結合Simulink控制策略進行聯合仿真，驗證視覺感知車道線控制車輛實施車道保持。本文所使用TruckSim版本并沒有視覺傳感器，所以無法對車道進行感知，應用預先設置道路坐標的方法替代視覺傳感器。搭建S型道路，實時輸出車輛位置信息，再應用全局搜索找出和車輛位置最小的中心車道線坐標信息，將二者之差作為車輛和道路中心線的橫向偏差。中心車道線是S型曲線。仿真時車速設定為固定值10m/s，利用預瞄點搜索算法［8］，以車輛和軌跡最近點開始向前搜索一定數量的軌跡坐標和預瞄點進行比較，取最近的點和預瞄點之間的距離作為預瞄點橫向偏差。將位置偏差置于圖5所示位置偏差控制模型，計算方向盤轉角，控制整車模沿著車道中心線行駛。模型仿真流程如圖6所示。

圖5 整車模型

圖6 Simulink模型仿真流程

通過仿真，由圖7可以看出重型車輛的運動軌跡和車道中心線基本重合，圖8是車輛質心和車道中心線橫向偏差，在直道時車輛的橫向偏差基本控制在0.05m范圍內，在彎道時偏差增大，最大值超過0.1m，綜合情況滿足了車道保持的要求。

圖7 車道線和車輛運動軌跡對比

圖8 橫向偏差

3.2 實車測試

以我廠生產的越野車為平臺，在我公司試驗場對車道保持系統進行了測試。如圖9藍色線標記，該試驗場是由東西兩側直道和南北兩側彎道組成的環形道路，全長4.6km。在測試之前，首先對視覺傳感器作了標定，對控制參數進行了設置。控制周期設置為50ms。速度是由駕駛員控制，彎道時速度小于40km/h，直道時平均速度小于60km/h。預瞄距離設置為0.6v，位置偏差參數m為0.7，n為0.3。比例系數P為0.3，微分系數為0.05。并用CANoe對傳感器信息和工控機經CAN卡發往底層的控制信息進行了檢測，保證數據通暢。實車測試過程如圖10所示。

圖9 試驗場地

圖10 實車測試

在測試過程中發現車輛在彎道時容易出現抖動、滯后、偏差較大等問題，結合調試軟件和仿真經驗，得出車輛在不同速度下響應速度和滯后性不同。故將車道保持系統按速度分為5個等級，小于25km/h為低速，25～35km/h為中低速，35～45km/h為中速，45～55km/h為中高速，大于55km/h為高速。對不同速度下分別設置了和速度v成線性關系的P參數和不同的D參數。

這樣比例增益P是關于速度v的線性關系，使得P隨著v增大而逐漸減小，D逐漸增大。經過對PD參數的調節，截取彎道時車輛質心和車道中心線橫向偏差如圖11所示，車輛距離車道線中心的位置偏差基本穩定在0.2m范圍內，偶爾出現抖動偏差。分析其原因在測試過程中發現由于路面不是絕對的平整，道路檢測以及逆透視變換定位存在誤差，造成偏差，會出現一定幅度的隨機波動，進而造成轉角的計算值出現波動，方向盤偶爾會出現小幅度的抖動。為解決這個問題，我們在最終發轉向命令給車輛執行機構前，將轉角計算結果應用限幅平均濾波法進行轉角濾波。限幅平均濾波法的過程，首先將每次計算得到的值先進行限幅處理，再送入隊列進行遞推平均濾波處理。處理過程如下。

1）根據經驗確定相鄰兩次轉角差的最大值Maxdif。

2）得到的新的轉角與上一次轉角比較，若大于Maxdif，則本次值無效，用上一次的轉角值作為新的轉角；若小于Maxdif，則本次值有效。

3）用最新的轉角值組成一個先進先出的隊列，隊列長度為5，將這5個數取均值，得到最終的輸出轉角。

經過濾波后，在同一路段采集車輛質心和車道中心下橫向偏差如圖12所示，與圖11相比，雖然車輛整體抖動趨勢相同，但偏差明顯變小，沒有較大的尖峰出現。

圖11 實車橫向偏差

圖12 濾波后實車橫向偏差

4 結論

本文沒有進行獲取車道信息的研究。在視覺感知產品的基礎上對控制算法進行研究與驗證，將車輛相對車道的橫向偏差和車輛前方一定距離的點和車道橫向偏差作為位置偏差進行控制，不僅能修正車輛和車道的位置偏差，還能提前預測車道前方橫向偏差。同時提出與速度成線性變換的PD控制參數，使得車輛在不同速度時控制參量不同，保證了控制的精準性。將該系統在我廠生產的重型軍用卡車上進行測試，取得了很好的效果。本文只對車輛的橫向控制進行了研究，對于與縱向控制結合還有待研究。同時由于測試環境的限制，該系統并沒有進行高速試驗，只是在小于60km/h的路面上進行了測試，提高速度也是日后需要研究的重點工作。