基于安全屏障監(jiān)控的車道自動(dòng)保持控制*

陳亞偉，邵毅明，束海波，郝西祥

（1.重慶交通大學(xué)機(jī)電與車輛工程學(xué)院，重慶 400074； 2.重慶交通大學(xué)交通運(yùn)輸學(xué)院，重慶 400074）

前言

車道保持控制作為一種駕駛輔助系統(tǒng)，可以使車輛保持在規(guī)定車道內(nèi)行駛，提高駕駛舒適性和安全性。它是車輛運(yùn)動(dòng)控制的關(guān)鍵技術(shù)之一，直接影響車輛的行駛安全性［1-3］。

近年來(lái)，針對(duì)車道保持控制的研究日益增多。文獻(xiàn)［4］中通過(guò)仿真比較了H∞、自適應(yīng)PID和模糊控制在車道保持方面的性能。文獻(xiàn)［5］中提出了PID自動(dòng)轉(zhuǎn)向控制，并通過(guò)軌跡仿真驗(yàn)證了有效性。文獻(xiàn)［6］中同時(shí)設(shè)計(jì)了多個(gè)穩(wěn)定控制器，以防止傳感器故障，實(shí)現(xiàn)容錯(cuò)車道保持。文獻(xiàn)［7］中為提高跟蹤精度和平順性，提出了一種頻率整形預(yù)覽控制算法。綜上所述，車道保持控制研究日益得到重視，但如何基于車輛的動(dòng)力學(xué)特性，在保證平穩(wěn)轉(zhuǎn)向的同時(shí)，確保行車安全，實(shí)現(xiàn)安全與車道保持的協(xié)同控制，還有待進(jìn)一步討論。

雖然通過(guò)濾波和降低增益等方法可實(shí)現(xiàn)平穩(wěn)運(yùn)行，但是此類方法可能會(huì)降低跟蹤精度和安全性，尤其是在彎道行駛時(shí)，這種現(xiàn)象更加明顯。車道保持的安全性涉及到車道偏離、周圍車輛干擾和車道檢測(cè)失敗導(dǎo)致的不穩(wěn)定性等多方面因素。在本文中，設(shè)定安全行駛的關(guān)鍵指標(biāo)是保持小的橫向偏移和航向角誤差，以避免車道偏離。

如圖1所示，不同于對(duì)平滑性考慮，安全性是高度非線性的。如果跟蹤誤差很小，顯然車輛的轉(zhuǎn)向平滑度更重要，但隨著誤差的增加，安全則變成首要考慮因素。

圖1 平滑性與安全性分析

基于上述分析，本文中以車道保持控制為研究對(duì)象，在分析其結(jié)構(gòu)與特性的基礎(chǔ)上，提出了一種安全監(jiān)控的預(yù)覽車道保持控制算法。具體來(lái)說(shuō)，主要完成以下工作。

（1）為實(shí)現(xiàn)平滑的車道跟蹤，提出了一種離散時(shí)間預(yù)覽控制算法，該算法與Mobileye模塊協(xié)同工作。

（2）為保證安全，設(shè)計(jì)了一種基于屏障功能的控制方法，與預(yù)覽控制并行工作。其主要特點(diǎn)是可防止車輛離開(kāi)安全區(qū)，但如果汽車是安全的，則保持休眠狀態(tài)。

（3）在實(shí)車上加載控制算法，進(jìn)行仿真和道路測(cè)試，以驗(yàn)證該控制算法的有效性。

1 車道保持系統(tǒng)模型

在本節(jié)中，建立了車道模型和車輛橫向動(dòng)力學(xué)模型，提出了最優(yōu)車道保持問(wèn)題的代價(jià)函數(shù)。

1.1 車道模型

本文中使用一個(gè)Mobileye 660模塊來(lái)感知車道標(biāo)記，該視覺(jué)系統(tǒng)輸出涉及到的每個(gè)車道標(biāo)線的詳細(xì)信息，包括橫向偏移量、航向角誤差、車道曲率及其導(dǎo)數(shù)、車道檢測(cè)質(zhì)量、最大感知范圍和車道標(biāo)線類型。車輛局部坐標(biāo)系下的車道輪廓線由3階多項(xiàng)式描述，即

式中 x為縱向距離，x∈ [0 ，x＾max]。將上述多項(xiàng)式記為

前方道路曲率為

由于傳感器安裝在前風(fēng)窗玻璃上，在車輛質(zhì)心之前ds處，因此測(cè)得的e＾y被修正為

由于ds的影響較小（大約0.4 m），在其它觀測(cè)結(jié)果中便不再進(jìn)行修正。

基于車道標(biāo)記生成參考路徑Ω，實(shí)現(xiàn)車輛的實(shí)時(shí)導(dǎo)航。如果只檢測(cè)到車道標(biāo)線的一側(cè)，則參考路徑Ω為

式中d0為車道標(biāo)線到車輛質(zhì)心的期望偏移量。

如果檢測(cè)到兩邊車道標(biāo)線，則設(shè)定參考路徑Ω為

1.2 車輛橫向動(dòng)力學(xué)模型

采用車輛單軌動(dòng)力學(xué)模型進(jìn)行控制設(shè)計(jì)，圖2為車輛橫向動(dòng)力學(xué)示意圖，表1為圖2中符號(hào)及其含義。本文中假設(shè)車輛不失穩(wěn)定性，即側(cè)向加速度小于0.3g，因此，可認(rèn)為輪胎的側(cè)向力與其滑移角成正比［8］。

圖2 車道保持控制的車輛動(dòng)力學(xué)模型

表1 動(dòng)力學(xué)模型的符號(hào)及定義

其中：

式中σi為集總系數(shù)，定義為

控制輸入為前輪轉(zhuǎn)角δ，δ∈R R，車道曲率cR為關(guān)鍵擾動(dòng)，cR∈R R。為便于控制器的設(shè)計(jì)和實(shí)現(xiàn)，將連續(xù)時(shí)間系統(tǒng)式（6）轉(zhuǎn)換為具有固定采樣周期Δτ的線性離散時(shí)間系統(tǒng)，表示如下：

式中：A∈R R4×4；B∈R R4；D∈R R4；k為階躍序列。

1.3 最優(yōu)車道保持

將車道保持任務(wù)轉(zhuǎn)換為最優(yōu)控制問(wèn)題（OCP），該問(wèn)題要求具有最小化的平滑性和面向精度的代價(jià)函數(shù)，該函數(shù)為

式中Q∈R R4×4和R∈R R分別為半正定陣和正定矩陣，即 Q≥0，R＞0。

式（9）需在無(wú)界范圍內(nèi)獲得 cR的數(shù)值，由于Mobileye的最大范圍約為100 m，因此在時(shí)刻k，cR僅在窗口［k，k＋N］中可用，其中N是預(yù)覽步數(shù)。假設(shè)預(yù)覽窗口之外的道路是直的，即

雖然通過(guò)減小跟蹤誤差x（k）可對(duì)代價(jià)函數(shù)進(jìn)行調(diào)整，但隨著誤差的增加，安全準(zhǔn)則變得更加重要，因此，需要對(duì) ey和 eφ施加一個(gè)硬約束Φ，即

2 預(yù)覽車道保持控制算法的設(shè)計(jì)

本節(jié)中首先設(shè)計(jì)預(yù)覽控制算法而不考慮安全約束式（11），然后在第3節(jié)中設(shè)計(jì)屏障控制算法。當(dāng)式（8）中的擾動(dòng)D·cR（k）為0時(shí)，車道保持系統(tǒng)式（8）～式（9）成為一個(gè)線性二次調(diào)節(jié)器（LQR），從而可以解析求解，但D·cR不為0。針對(duì)這種時(shí)變擾動(dòng)，一種直接的方法是在線數(shù)值求解優(yōu)化問(wèn)題，但在線計(jì)算時(shí)間長(zhǎng)，實(shí)時(shí)性較差［10］。與耗時(shí)的數(shù)值方法不同，本文中提出的預(yù)覽控制方法通過(guò)在狀態(tài)向量中加入未來(lái)擾動(dòng)來(lái)尋求解析解，然后將其作為一個(gè)增廣的LQR問(wèn)題進(jìn)行求解［11］。

為消除擾動(dòng)，將［k，k＋N］中的 cR（k）和原始系統(tǒng)狀態(tài) x（k）合并為增廣狀態(tài)矩陣 χ（k）：

代價(jià)函數(shù)變?yōu)槿缦滤荆?/p>

式中：O和 I為0和單位矩陣；L為預(yù)覽曲率的映射。

對(duì)于該增廣時(shí)不變的LQR問(wèn)題，可以通過(guò)Pontryagin的最大（或最?。┰瓌t來(lái)求解：

式中 β＝βT＝（I＋B R-1BTP）-1為集總矩陣。P由黎卡提方程解出：

為避免求解高維方程式（16）和對(duì) x（k）與CR（k）解耦，將 P分為4個(gè)子矩陣并簡(jiǎn)化式（16）的運(yùn)算：

其中 ζ＝AT（I＋PB R-1BT）-1

由式（17），求出矩陣 P和Pc：

式（18）的第1個(gè)方程實(shí)際上是沒(méi)有預(yù)覽模塊的原始系統(tǒng)的黎卡提方程。由于式（14）中D 和L的特殊結(jié)構(gòu)，因此，求解式（18）的第2個(gè)方程：

式中pi為Pc的第i列向量。

將式（18）和式（19）代入式（15）得到最優(yōu)控制：

式中Kb∈ RR和Kf∈ RRN＋1是反饋增益向量。

將δ*代入式（13），可得

最優(yōu)控制式（20）由兩部分組成：第一部分Kbx（k）是反饋?lái)憫?yīng)，通過(guò)Q和R可減少增益Kb以獲得更平穩(wěn)的轉(zhuǎn)向；第二部分KfCR處理預(yù)覽的車道曲率，并基于未來(lái)車道曲率和車輛動(dòng)力學(xué)生成主動(dòng)轉(zhuǎn)向。前饋控制是一種較好的前饋控制方式，在擾動(dòng)到達(dá)車輛之前進(jìn)行動(dòng)作，是實(shí)現(xiàn)更平穩(wěn)、更準(zhǔn)確控制的關(guān)鍵。

由車道標(biāo)線模型式（1）和式（2）可知，車道曲率為線性變化，預(yù)覽控制可簡(jiǎn)化為

如果N足夠高，則Kc和Kcd收斂于

由式（22）可知，控制器僅包含 x（k），cR（k）和）的反饋操作。也就是說(shuō)，控制器只需6個(gè)增益，并且定時(shí)性較好。

如果去掉預(yù)覽部分，控制器則成為簡(jiǎn)單的比例微分（PD）控制：

3 安全屏障控制設(shè)計(jì)

為滿足安全約束式（11），設(shè)計(jì)了一個(gè)安全屏障，并將其施加到所提出的預(yù)覽控制上［12］。

為限制車輛使其處于安全區(qū)，引入以下不等式約束：

式（25）將安全區(qū)定義為一個(gè)橢圓區(qū)域，用ψ＝{ x Φ＜0}表示，邊界為 ψ＝{ x Φ＝0}。

利用屏障控制使x∈ψ，安全屏障函數(shù)設(shè)計(jì)為

上式具有特殊的性質(zhì)，即當(dāng)x→ψ時(shí)，h→0，表示邊界上的能量為0；當(dāng)x→0，h→1，表示能量最高和安全行駛。如果x趨近于ψ，可以阻止h的減小，那么系統(tǒng)就會(huì)保持在h＞0的ψ內(nèi)部。該過(guò)程可通過(guò)限制·h的大小來(lái)實(shí)現(xiàn)。

式中：γ＞0；ε為松弛因子，用于穩(wěn)定系統(tǒng)、補(bǔ)償系統(tǒng)延遲和模型不匹配。

由式（27）的約束，當(dāng)x遠(yuǎn)離ψ時(shí)，h可以自由改變，當(dāng) xp→ψ，·h→0時(shí)，h將停止增加。

根據(jù)采樣時(shí)間Δτ，式（27）可以變換為

h（k＋1）由以下泰勒級(jí)數(shù)近似得到：

對(duì)h求導(dǎo)得到如下結(jié)果：

為簡(jiǎn)化以下表示，定義一個(gè)新的矩陣ωe為

狀態(tài)變量x及其導(dǎo)數(shù)滿足以下的關(guān)系：

將式（6）、式（29）和式（32）代入式（28），得到保證x∈ψ的輸入δ為

在線應(yīng)用中，可以忽略泰勒展開(kāi)的高階近似。式（33）生成了面向安全δ的可行集合，當(dāng)車輛接近屏障ψ時(shí)，該可行集合主動(dòng)干涉預(yù)覽控制過(guò)程。

在式（33）中δ的邊界為

通過(guò)安全屏障控制δ，預(yù)覽控制δ*監(jiān)督生成最終的轉(zhuǎn)向指令δ*為

其中，如果xTωB0→0，δ→∞，則δ*＝δ*。

4 實(shí)車實(shí)驗(yàn)

將所提出的安全監(jiān)控預(yù)覽車道自動(dòng)保持算法載入到實(shí)際車輛，然后在實(shí)際道路進(jìn)行實(shí)驗(yàn)，驗(yàn)證所提算法的有效性。

4.1 車輛平臺(tái)及算法實(shí)現(xiàn)

如圖3所示，用于道路實(shí)驗(yàn)的車輛為吉利博瑞，配置的Mobileye 660和IMU模塊可測(cè)量橫向跟蹤誤差、橫擺運(yùn)動(dòng)和未來(lái)道路曲率。

圖3 實(shí)驗(yàn)測(cè)試車輛

采用C＋＋語(yǔ)言實(shí)現(xiàn)由安全屏障功能監(jiān)控的預(yù)覽車道保持控制，車輛動(dòng)力學(xué)參數(shù)如表2所示。

表2 車輛參數(shù)

4.2 仿真分析

為減少模型不匹配，使用單軌模型式（6）來(lái)估計(jì)車輛動(dòng)力學(xué)。如圖4（a）所示，道路由直線部分和隨后半徑為200 m的曲線組成，車速設(shè)定為20 m／s。具有安全屏障功能的預(yù)覽控制和PD控制結(jié)果如圖4所示。

（1）預(yù)覽控制的性能

將預(yù)覽控制與PD控制進(jìn)行比較，預(yù)覽控制在車輛進(jìn)入曲線之前就開(kāi)始發(fā)揮作用，PD控制器僅在進(jìn)入車輛曲線后才發(fā)揮作用，并且在轉(zhuǎn)向和橫向位移中出現(xiàn)超調(diào)現(xiàn)象。

圖4 安全屏障監(jiān)控的預(yù)覽控制仿真結(jié)果

如圖4（c）所示，預(yù)覽控制的橫向偏移ey進(jìn)入穩(wěn)態(tài)為30 cm，PD控制下的橫向偏移ey最大值為60 cm?？煽闯鲱A(yù)覽控制可以減小控制誤差，提高車輛的安全性。

（2）安全屏障控制的性能

設(shè)置 γ＝4，eym＝30 cm，eφm＝15°以激活屏障控制。由于預(yù)覽控件的誤差較小，在這種情況下其屏障控制不會(huì)調(diào)整其動(dòng)作。在PD控制中橫向偏移ey的最大值為60 cm，這已經(jīng)超出了式（25）給定的安全約束。

在圖4（h）中，PD控制下的屏障控制函數(shù)h（x）在t＝7 s時(shí)已經(jīng)變?yōu)樨?fù)數(shù)；而屏障控制器使前輪轉(zhuǎn)角向左轉(zhuǎn)2°，這可以防止h（x）下降到0。由PD控制和屏障控制計(jì)算的δ的數(shù)值如圖4（g）所示。

由上述仿真結(jié)果可知，屏障控制的主要優(yōu)點(diǎn)：在車道保持控制是安全的情況下不發(fā)生動(dòng)作，一旦車輛接近安全邊界便立即啟動(dòng)進(jìn)行轉(zhuǎn)向控制干預(yù)。

雖然仿真中預(yù)覽控制實(shí)現(xiàn)了精確的車道保持，但是由于模型的不確定性、外界干擾和感知誤差，仍然需要屏障函數(shù)。

4.3 實(shí)驗(yàn)結(jié)果

如圖5所示，在實(shí)際道路上，對(duì)提出的基于安全屏障監(jiān)控的預(yù)覽車道保持控制算法進(jìn)行了測(cè)試，實(shí)驗(yàn)道路存在高度彎曲的路段，最小半徑約為75 m。其中，圖5（a）為實(shí)驗(yàn)路線衛(wèi)星圖，圖5（b）和圖5（c）為道路測(cè)試場(chǎng)景，圖5（d）為所開(kāi)發(fā)的軟件界面。

圖5 路測(cè)場(chǎng)景

（1）預(yù)覽控制的性能

道路測(cè)試實(shí)驗(yàn)結(jié)果如圖6所示。從圖6（a）可知，道路曲率及其導(dǎo)數(shù)與真實(shí)道路軌跡大致吻合。在最初的105 s內(nèi)，車輛以32 km／h的速度行駛，經(jīng)過(guò)十字路口后，以50 km／h的速度巡航行駛。實(shí)驗(yàn)過(guò)程中，車輛的速度會(huì)自動(dòng)適應(yīng)道路曲率，以避免高橫向加速度，如圖6（c）所示，在140 s時(shí)，速度會(huì)下降到30 km／h。

如圖6（b）所示，轉(zhuǎn)向盤(pán)轉(zhuǎn)角在±10°范圍內(nèi)波動(dòng)，但在最劇烈的轉(zhuǎn)彎時(shí)，轉(zhuǎn)向盤(pán)轉(zhuǎn)角κsδ高達(dá)30°，圖中的陰影區(qū)域代表與道路曲率相關(guān)的預(yù)覽控制，它對(duì)轉(zhuǎn)向角的控制超過(guò)70%，其余部分來(lái)自與跟蹤誤差相關(guān)的反饋控制。

圖6 道路車道保持實(shí)驗(yàn)結(jié)果

如圖 6（d）所示，ey／eφ的最大值為 40 cm／3.5°，發(fā)生在道路曲線最尖銳的地方。

（2）安全屏障控制的性能

安全屏障控制的實(shí)驗(yàn)結(jié)果如圖7所示，實(shí)驗(yàn)最大車速約為60 km／h，最小路面半徑約為10 m。在轉(zhuǎn)彎段，車道標(biāo)記不可用，使用實(shí)時(shí)動(dòng)態(tài)（RTK）定位系統(tǒng)來(lái)提供虛擬車道。

在PD控制下，橫向偏移ey的峰值為42 cm。采用屏障控制來(lái)干預(yù)PD控制并設(shè)置安全誤差界限為eym＝30 cm。如圖7所示，誤差限制在30 cm以下，并且安全屏障函數(shù)h（x）＞0。從圖7中可以看到，橫向誤差在邊界內(nèi)被限制，從而在一定范圍內(nèi)輸出。

圖7 安全屏障控制實(shí)驗(yàn)結(jié)果

5 結(jié)論

本文中提出了一種安全監(jiān)控的車道自動(dòng)保持控制算法，實(shí)現(xiàn)了車輛的安全和車道保持的協(xié)同控制。通過(guò)仿真和道路實(shí)驗(yàn)，驗(yàn)證了控制系統(tǒng)的性能，得出如下結(jié)論。

（1）基于最優(yōu)控制理論設(shè)計(jì)的預(yù)覽車道保持控制算法，可使車道形狀變化的響應(yīng)更加平滑，而且在彎道上具有較高的跟蹤精度。

（2）為保證車道保持的安全性，提出了安全屏障算法來(lái)監(jiān)控預(yù)覽車道保持控制器。該安全屏障控制僅在車輛即將離開(kāi)安全區(qū)時(shí)啟動(dòng)工作，其余時(shí)間保持休眠狀態(tài)。仿真和實(shí)驗(yàn)結(jié)果表明，車道保持精度高，跟蹤誤差有界，并且安全可靠。