付 行， 任文峰， 王 凱

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

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

1 車道保持系統(tǒng)方案設(shè)計(jì)

視覺(jué)傳感器采集車道信息，通過(guò)CAN線傳入上位機(jī)系統(tǒng)，以車輛質(zhì)心實(shí)際位置和車道中心的橫向偏移為輸入，通過(guò)預(yù)瞄理論結(jié)合PD控制算法計(jì)算輸出轉(zhuǎn)角，通過(guò)CAN線輸出給底層下位機(jī)控制系統(tǒng)控制車輛在車道內(nèi)行駛。

1.1 硬件

車道保持系統(tǒng)所用到的硬件設(shè)備有Mobileye視覺(jué)傳感器，用于感知道路信息，主要是車道曲率、斜率、車道寬度等信息。為了便于快速開(kāi)發(fā)系統(tǒng)，車道保持的計(jì)算平臺(tái)選擇的是工控機(jī)，該工控機(jī)集成CAN卡便于接收視覺(jué)傳感器信息和向底層發(fā)送控制命令，電機(jī)控制器收到控制指令。車道保持測(cè)試平臺(tái)是我廠生產(chǎn)的第3代重型越野車，配備了電動(dòng)助力轉(zhuǎn)向電機(jī)。系統(tǒng)硬件架構(gòu)如圖1所示。

圖1 硬件架構(gòu)圖

1.2 軟件系統(tǒng)

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

圖2 車道保持軟件系統(tǒng)

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

2 控制策略

車道保持系統(tǒng)主要研究的是通過(guò)橫向控制策略控制車輛在車道內(nèi)穩(wěn)定快速行駛，本文研究將橫向位置偏差作為控制量，通過(guò)PD算法調(diào)節(jié)系統(tǒng)響應(yīng)和超調(diào)，輸出控制車輛轉(zhuǎn)向的方向盤(pán)轉(zhuǎn)角。同時(shí)加入車輛橫向約束，限制道路曲率變化較大時(shí)帶來(lái)的不穩(wěn)定性問(wèn)題。

2.1 橫向控制算法

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

圖3 橫向偏差示意圖

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

為了簡(jiǎn)便，令f=ds2。要求點(diǎn)B到中心線的距離就是求點(diǎn)B到道路中心線的最近距離，因此式 （2）對(duì)x求導(dǎo)得：

這是條件極值問(wèn)題，求函數(shù)的極小值可得點(diǎn)C（x，y）就是道路中心線離預(yù)瞄點(diǎn)最近的點(diǎn)，進(jìn)而可求得預(yù)瞄橫向偏差ds。

考慮到重型卡車自身質(zhì)量、體積較大、慣性較大等特征，單獨(dú)使用預(yù)瞄前向控制會(huì)引起調(diào)節(jié)響應(yīng)過(guò)慢，車輛進(jìn)入穩(wěn)態(tài)時(shí)間較長(zhǎng)。本文引入車輛質(zhì)心橫向偏差作用修正車輛和車道線的位置偏差。點(diǎn)O到車道中心線的距離OA為dv。由式（1）可知，視覺(jué)傳感器和道路中心線的距離為：

根據(jù)相似三角形對(duì)應(yīng)邊成比例，可知dv：

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

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

2.2 PD控制

如圖4所示，車道保持控制的目標(biāo)是通過(guò)控制方向盤(pán)角度減小與車道中心線的位置偏差。以2.1位置偏差為控制輸入，通過(guò)PD參數(shù)控制環(huán)節(jié)，調(diào)整響應(yīng)速度和減小超調(diào)。

圖4 基于位置偏差的控制

PD控制量比例增益P主要調(diào)節(jié)方向盤(pán)轉(zhuǎn)角大小，車速低時(shí)P值大，高速時(shí)P值小?？刂屏緿 （微分系數(shù)）影響自動(dòng)駕駛系統(tǒng)的響應(yīng)速度，可消除或減弱系統(tǒng)慣性以及滯后帶來(lái)的振動(dòng)現(xiàn)象。通過(guò)穩(wěn)態(tài)轉(zhuǎn)向增益、增益調(diào)度的調(diào)節(jié)之后，得到最優(yōu)期望前輪轉(zhuǎn)角，這里不考慮轉(zhuǎn)向系統(tǒng)的非線性，認(rèn)為前輪轉(zhuǎn)角與方向盤(pán)成比例關(guān)系，前輪轉(zhuǎn)角乘以轉(zhuǎn)向傳動(dòng)比就得到方向盤(pán)轉(zhuǎn)角。飽和特性是指前輪轉(zhuǎn)角設(shè)定在一定范圍內(nèi)，根據(jù)車輛不同設(shè)定范圍不同，本文設(shè)定為-0.785～0.785弧度。這里要注意的是在乘以傳動(dòng)比系數(shù)時(shí)首先要將弧度轉(zhuǎn)換為度。

2.3 橫向約束

車輛在行駛道路上存在曲率變化較大，如果不考慮橫向約束執(zhí)行機(jī)構(gòu)轉(zhuǎn)動(dòng)速率的限制，可能造成車輛轉(zhuǎn)角突然變大，導(dǎo)致車輛失穩(wěn)，甚至側(cè)翻。為了保證車輛的橫向穩(wěn)定性需要對(duì)橫向加以約束。

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

本文不考慮車輛縱向控制，設(shè)定車輛在一定速度下，車道曲率由車輛傳感器給出，所以由式 （7）可知當(dāng)?shù)缆非首兇髸r(shí)，車輛加速度會(huì)變小。由動(dòng)力學(xué)關(guān)系求得：

式中：w——車輛轉(zhuǎn)角；L——車輛軸距。

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

3 車道保持系統(tǒng)仿真與測(cè)試

通過(guò)仿真驗(yàn)證橫向偏差控制算法的有效性，同時(shí)調(diào)節(jié)PD參數(shù)，形成經(jīng)驗(yàn)積累，作為實(shí)車測(cè)試調(diào)試的指導(dǎo)依據(jù)。

3.1 Simulink與TruckSim聯(lián)合仿真

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

圖5 整車模型

圖6 Simulink模型仿真流程

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

圖7 車道線和車輛運(yùn)動(dòng)軌跡對(duì)比

圖8 橫向偏差

3.2 實(shí)車測(cè)試

以我廠生產(chǎn)的越野車為平臺(tái)，在我公司試驗(yàn)場(chǎng)對(duì)車道保持系統(tǒng)進(jìn)行了測(cè)試。如圖9藍(lán)色線標(biāo)記，該試驗(yàn)場(chǎng)是由東西兩側(cè)直道和南北兩側(cè)彎道組成的環(huán)形道路，全長(zhǎng)4.6km。在測(cè)試之前，首先對(duì)視覺(jué)傳感器作了標(biāo)定，對(duì)控制參數(shù)進(jìn)行了設(shè)置?？刂浦芷谠O(shè)置為50ms。速度是由駕駛員控制，彎道時(shí)速度小于40km/h，直道時(shí)平均速度小于60km/h。預(yù)瞄距離設(shè)置為0.6v，位置偏差參數(shù)m為0.7，n為0.3。比例系數(shù)P為0.3，微分系數(shù)為0.05。并用CANoe對(duì)傳感器信息和工控機(jī)經(jīng)CAN卡發(fā)往底層的控制信息進(jìn)行了檢測(cè)，保證數(shù)據(jù)通暢。實(shí)車測(cè)試過(guò)程如圖10所示。

圖9 試驗(yàn)場(chǎng)地

圖10 實(shí)車測(cè)試

在測(cè)試過(guò)程中發(fā)現(xiàn)車輛在彎道時(shí)容易出現(xiàn)抖動(dòng)、滯后、偏差較大等問(wèn)題，結(jié)合調(diào)試軟件和仿真經(jīng)驗(yàn)，得出車輛在不同速度下響應(yīng)速度和滯后性不同。故將車道保持系統(tǒng)按速度分為5個(gè)等級(jí)，小于25km/h為低速，25～35km/h為中低速，35～45km/h為中速，45～55km/h為中高速，大于55km/h為高速。對(duì)不同速度下分別設(shè)置了和速度v成線性關(guān)系的P參數(shù)和不同的D參數(shù)。

這樣比例增益P是關(guān)于速度v的線性關(guān)系，使得P隨著v增大而逐漸減小，D逐漸增大。經(jīng)過(guò)對(duì)PD參數(shù)的調(diào)節(jié)，截取彎道時(shí)車輛質(zhì)心和車道中心線橫向偏差如圖11所示，車輛距離車道線中心的位置偏差基本穩(wěn)定在0.2m范圍內(nèi)，偶爾出現(xiàn)抖動(dòng)偏差。分析其原因在測(cè)試過(guò)程中發(fā)現(xiàn)由于路面不是絕對(duì)的平整，道路檢測(cè)以及逆透視變換定位存在誤差，造成偏差，會(huì)出現(xiàn)一定幅度的隨機(jī)波動(dòng)，進(jìn)而造成轉(zhuǎn)角的計(jì)算值出現(xiàn)波動(dòng)，方向盤(pán)偶爾會(huì)出現(xiàn)小幅度的抖動(dòng)。為解決這個(gè)問(wèn)題，我們?cè)谧罱K發(fā)轉(zhuǎn)向命令給車輛執(zhí)行機(jī)構(gòu)前，將轉(zhuǎn)角計(jì)算結(jié)果應(yīng)用限幅平均濾波法進(jìn)行轉(zhuǎn)角濾波。限幅平均濾波法的過(guò)程，首先將每次計(jì)算得到的值先進(jìn)行限幅處理，再送入隊(duì)列進(jìn)行遞推平均濾波處理。處理過(guò)程如下。

1）根據(jù)經(jīng)驗(yàn)確定相鄰兩次轉(zhuǎn)角差的最大值Maxdif。

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

3）用最新的轉(zhuǎn)角值組成一個(gè)先進(jìn)先出的隊(duì)列，隊(duì)列長(zhǎng)度為5，將這5個(gè)數(shù)取均值，得到最終的輸出轉(zhuǎn)角。

經(jīng)過(guò)濾波后，在同一路段采集車輛質(zhì)心和車道中心下橫向偏差如圖12所示，與圖11相比，雖然車輛整體抖動(dòng)趨勢(shì)相同，但偏差明顯變小，沒(méi)有較大的尖峰出現(xiàn)。

圖11 實(shí)車橫向偏差

圖12 濾波后實(shí)車橫向偏差

4 結(jié)論

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