基于弧長(zhǎng)預(yù)瞄的車輛側(cè)向跟蹤控制研究*

李 爽，徐延海，陳 靜，朱鵬興

（西華大學(xué)，汽車測(cè)控與安全四川省重點(diǎn)實(shí)驗(yàn)室，成都 610039）

前言

20世紀(jì)80年代早期，郭孔輝等在進(jìn)行駕駛員行為模型及駕駛員-車輛閉環(huán)操作系統(tǒng)的仿真研究過程中提出了預(yù)測(cè)-跟隨系統(tǒng)理論，并在此理論基礎(chǔ)上，以Ackermann幾何關(guān)系為支撐，從控制理論角度出發(fā)建立了預(yù)瞄最優(yōu)曲率模型。該模型比補(bǔ)償矯正模型更符合人的駕駛行為，因此被廣泛應(yīng)用到路徑跟隨控制中［1-2］。

預(yù)瞄最優(yōu)曲率模型是在駕駛員-車輛閉環(huán)操作系統(tǒng)的仿真研究中提出的［3］。期望路徑在仿真研究時(shí)容易被描述為關(guān)于時(shí)間的函數(shù)［4］，但在無人駕駛控制領(lǐng)域中，車輛實(shí)際行駛的期望路徑很難用時(shí)間函數(shù)描述，相較之下它更容易以地理位置坐標(biāo)點(diǎn)集的形式描述，本文中據(jù)此建立新的算法來獲取預(yù)瞄點(diǎn)和預(yù)瞄點(diǎn)側(cè)向位移［5］。

文獻(xiàn)［6］中提出預(yù)瞄點(diǎn)搜索算法，通過坐標(biāo)變換將大地坐標(biāo)系下的期望路徑點(diǎn)與車輛坐標(biāo)系下的期望路徑點(diǎn)進(jìn)行換算，然后在描述預(yù)期道路的數(shù)表中找出最近點(diǎn)的兩個(gè)相鄰點(diǎn)，再使用線性插值法計(jì)算預(yù)瞄點(diǎn)坐標(biāo)，求得預(yù)瞄點(diǎn)側(cè)向位移。該方法對(duì)于道路變化具有較高的適應(yīng)性，但是大量的坐標(biāo)變換使得算法的整體計(jì)算量較大。文獻(xiàn)［7］中提出了圓弧式預(yù)瞄位置確定法，該方法無需坐標(biāo)變換也無需求解復(fù)雜方程組，提高了計(jì)算效率，但是當(dāng)期望路徑發(fā)生變化，而固定預(yù)瞄距離選取不當(dāng)時(shí)，獲取預(yù)瞄點(diǎn)就需要重新進(jìn)行路徑規(guī)劃或通過更復(fù)雜的算法重新確定預(yù)瞄距離。

對(duì)于人-車-路閉環(huán)系統(tǒng)而言，預(yù)瞄時(shí)間與預(yù)瞄距離對(duì)于系統(tǒng)的穩(wěn)定性和適應(yīng)性具有較大影響［8-9］，如何使得系統(tǒng)在預(yù)瞄參數(shù)不變的情況下，適應(yīng)更多路況，對(duì)于降低算法的整體計(jì)算量和提高系統(tǒng)的整體穩(wěn)定性具有重要意義。為此，本文中基于預(yù)瞄最優(yōu)曲率模型，探究了一種依據(jù)車輛實(shí)際行駛路程獲取預(yù)瞄點(diǎn)和預(yù)瞄點(diǎn)側(cè)向位移的弧長(zhǎng)預(yù)瞄算法。并且，根據(jù)2自由度車輛模型確定了此種方法下的預(yù)瞄點(diǎn)側(cè)向位移與前輪轉(zhuǎn)角之間的關(guān)系，然后利用新建立的系統(tǒng)方框圖［10］，在CarSim與Simulink聯(lián)合仿真環(huán)境下驗(yàn)證了該方法在預(yù)瞄參數(shù)不變的情況下適應(yīng)不同道路工況的有效性與可靠性［11］，最后，對(duì)該方法下的人-車-路閉環(huán)系統(tǒng)轉(zhuǎn)向盤穩(wěn)定性影響因素進(jìn)行了仿真分析。

1 2自由度穩(wěn)態(tài)車輛模型

1.1 2自由度車輛模型

2自由度車輛動(dòng)力學(xué)模型的運(yùn)動(dòng)微分方程［12］為

式中：m為車輛質(zhì)量；u為穩(wěn)態(tài)條件下的縱向車速；a為質(zhì)心到前軸的距離；b為質(zhì)心到后軸的距離；Iz為車輛繞OZ軸的轉(zhuǎn)動(dòng)慣量；k1為前輪側(cè)偏剛度；k2為后輪側(cè)偏剛度為質(zhì)心側(cè)向加速度；ωr為橫擺角速度為橫擺角加速度；β為質(zhì)心側(cè)偏角；δ為前輪轉(zhuǎn)角。

1.2 穩(wěn)態(tài)假設(shè)

根據(jù)Ackermann幾何關(guān)系，將車輛在行駛時(shí)的某一時(shí)刻等效為沿半徑R的曲線期望路徑行駛的穩(wěn)態(tài)條件［13］，如圖1所示。在該穩(wěn)態(tài)條件下，車輛動(dòng)力學(xué)模型的運(yùn)動(dòng)微分方程為

式中：vss為穩(wěn)態(tài)條件下的側(cè)向速度；ωrss為穩(wěn)態(tài)條件下的橫擺角速度；δss為穩(wěn)態(tài)條件下的前輪轉(zhuǎn)角。

圖1 車輛穩(wěn)態(tài)行駛相對(duì)坐標(biāo)系

2 弧長(zhǎng)預(yù)瞄及路徑跟蹤

2.1 弧長(zhǎng)預(yù)瞄方法

如圖1所示，基于預(yù)瞄最優(yōu)曲率模型，以車輛等效質(zhì)心為原點(diǎn)O，以車輛行駛方向?yàn)檎齾^(qū)間，以車輛縱向速度方向?yàn)閄軸，車輛側(cè)向速度方向?yàn)閅軸，建立車輛行駛相對(duì)坐標(biāo)系［5］。將車輛行駛中的某一時(shí)刻凍結(jié)，此時(shí)車輛狀態(tài)可等效為駕駛員沿著一個(gè)軌跡曲率為轉(zhuǎn)向中心為G點(diǎn)的期望路徑L行駛。然后，設(shè)駕駛員沿著X軸方向有一個(gè)預(yù)瞄參考點(diǎn)C；C*為L(zhǎng)上距離C最近的一點(diǎn)；d為C點(diǎn)到O點(diǎn)的距離；oss為C點(diǎn)到C*點(diǎn)的距離；設(shè)預(yù)瞄點(diǎn)P為沿車輛行駛方向的期望軌跡L上，到車輛質(zhì)心O的弧長(zhǎng)恰好等于d的一點(diǎn)為預(yù)瞄點(diǎn)P到X軸的距離（即預(yù)瞄點(diǎn)側(cè)向位移）。

式中：dp為固定預(yù)瞄距離；tp為預(yù)瞄時(shí)間。

2.2 路徑跟蹤側(cè)向控制

根據(jù)式（2）和車輛做穩(wěn)態(tài)圓周運(yùn)動(dòng)的運(yùn)動(dòng)規(guī)律，可知

由式（2）展開消去vss得到 δss與 ωrss的關(guān)系式，再代入式（4）可得

于是期望的前輪轉(zhuǎn)角δss與oss之間的關(guān)系為

式中Vss為車輛實(shí)際行駛方向上的綜合車速。此時(shí)，C點(diǎn)的坐標(biāo)為（d，0），G點(diǎn)的坐標(biāo)為可得

根據(jù)式（7）與式（12）可知期望的前輪轉(zhuǎn)角δss與新定義的預(yù)瞄點(diǎn)側(cè)向位移之間的關(guān)系式為

2.3 大地慣性坐標(biāo)系下的預(yù)瞄點(diǎn)側(cè)向位移

式（14）是在相對(duì)坐標(biāo)系下得出的，但式中并未包含需要進(jìn)行坐標(biāo)轉(zhuǎn)換的參數(shù)，所以無論是相對(duì)坐標(biāo)系還是絕對(duì)坐標(biāo)系下，只要能夠求得新定義的預(yù)瞄點(diǎn)側(cè)向位移則該關(guān)系式就可適用。根據(jù)之前的定義，預(yù)瞄點(diǎn)P為沿車輛行駛方向的期望軌跡L上，到車輛質(zhì)心O的弧長(zhǎng)恰好等于d的一點(diǎn)為預(yù)瞄點(diǎn)P到X軸的距離。為了求得P，需要將前期規(guī)劃出的曲線轉(zhuǎn)化為一系列點(diǎn)集，點(diǎn)集的定義為（xi，yi，si）［16］。

圖2 曲線點(diǎn)集化描述

如圖2所示，xi，yi，si分別為各采集點(diǎn)在大地慣性坐標(biāo)系下的橫坐標(biāo)、縱坐標(biāo)和到規(guī)劃起始點(diǎn)（xO，yO，sO）的路程，Δx=xi-xi-1為定值，各點(diǎn)之間作線性插值處理。每一路徑規(guī)劃周期內(nèi)，（xO，yO，sO）即為規(guī)劃開始時(shí)的車輛質(zhì)心位置的世界坐標(biāo)，則此時(shí)預(yù)瞄點(diǎn)P到（xO，yO，sO）的弧長(zhǎng)即為預(yù)瞄距離d，根據(jù)積分中值定理，點(diǎn)集中一定存在且唯一存在兩個(gè)相鄰點(diǎn)，使得si-1≤d≤si，所以不會(huì)出現(xiàn)其它算法容易出現(xiàn)的多參考點(diǎn)問題，而此時(shí)預(yù)瞄點(diǎn)P的位置坐標(biāo)為

當(dāng)車輛駛離規(guī)劃起始點(diǎn)后，假設(shè)車輛始終沿著規(guī)劃路徑行駛，設(shè)其行駛路程為sr，則此時(shí)預(yù)瞄點(diǎn)P的坐標(biāo)為

式中：si-1≤sr＋d≤si；（xi-1，yi-1，si-1）和（xi，yi，si）為點(diǎn)集中的兩相鄰點(diǎn)。

在大地慣性坐標(biāo)系下獲得預(yù)瞄點(diǎn)P的坐標(biāo)后，假設(shè)車輛的航向角為φ，預(yù)瞄距離為d，如圖3所示。

圖3 大地慣性坐標(biāo)下新定義的預(yù)瞄示意圖

圖中，（xt，yt）為參考點(diǎn)C大地坐標(biāo)，（xp，yp）為預(yù)瞄點(diǎn)P大地坐標(biāo)，（xr，yr）為車輛質(zhì)心大地坐標(biāo)，可得

3 側(cè)向跟蹤控制模型

如圖4所示，在預(yù)瞄最優(yōu)曲率模型中，期望路徑L的軌跡中心線側(cè)向距離被描述為時(shí)間t的函數(shù)f（t）。在所考察的瞬時(shí)t，車輛即時(shí)狀態(tài)為y=y（t），當(dāng)駕駛員預(yù)瞄距離為d時(shí)，有預(yù)瞄時(shí)間為當(dāng)前時(shí)刻車速，預(yù)瞄點(diǎn)的側(cè)向距離為f（t＋T）。當(dāng)駕駛員以最優(yōu)的期望轉(zhuǎn)向盤轉(zhuǎn)角沿著期望最優(yōu)的軌跡曲率行駛時(shí)間T后，車輛到達(dá)預(yù)瞄點(diǎn)。此時(shí)，其側(cè)向位移y（t＋T）與t時(shí)刻的預(yù)期側(cè)向距離f（t＋T）相一致。

圖4 預(yù)瞄最優(yōu)曲率模型的預(yù)瞄示意圖

其人-車-路閉環(huán)系統(tǒng)方框圖如圖5所示，圖中，i為轉(zhuǎn)向系統(tǒng)傳動(dòng)比，l為軸距［17］，τ為延遲時(shí)間，h（s）=e-τs為延遲滯后環(huán)節(jié)為積分環(huán)節(jié)。

圖5 人-車-路閉環(huán)系統(tǒng)方框圖

弧長(zhǎng)預(yù)瞄方法如圖3所示，將期望路徑L的軌跡中心線以點(diǎn)集（xi，yi，si）的形式描述出來，預(yù)瞄點(diǎn)的側(cè)向距離被描述為關(guān)于行駛路程s的函數(shù)f（s）。設(shè)在t時(shí)刻f（t）=f（s），在（t＋T）時(shí)刻f（t＋T）=f（s＋d），預(yù)瞄時(shí)間T為車輛以當(dāng)前車速沿著期望路徑行駛預(yù)瞄距離d所需的時(shí)間。弧長(zhǎng)預(yù)瞄方法下的側(cè)向跟蹤閉環(huán)控制系統(tǒng)方框圖如圖6所示，圖中，P0為式（15）代表插值環(huán)節(jié)，P1為式（19）代表預(yù)瞄環(huán)節(jié)，P2為式（14）代表跟隨環(huán)節(jié)，δ*ss為最優(yōu)的期望前輪轉(zhuǎn)角，V（s）代表被控車輛［18］。

圖6 側(cè)向跟蹤閉環(huán)控制系統(tǒng)方框圖

4 模型的有效性和可靠性仿真分析

CarSim是廣泛運(yùn)用于車輛動(dòng)力學(xué)仿真及智能駕駛仿真環(huán)境搭建的商業(yè)軟件。本文中根據(jù)推導(dǎo)的側(cè)向跟蹤閉環(huán)控制系統(tǒng)方框圖，在Simulink中建立路徑跟蹤側(cè)向控制器，并利用該控制器在CarSim／Simulink聯(lián)合仿真環(huán)境下建立“人-車-路”閉環(huán)仿真系統(tǒng)，以進(jìn)行下列工況的仿真分析［19］。其中仿真參數(shù)如表1所示。

表1 仿真參數(shù)

4.1 圓周工況

為了衡量弧長(zhǎng)預(yù)瞄方法下的側(cè)向控制算法運(yùn)算速度，建立文獻(xiàn)［6］和文獻(xiàn)［7］中所采用的半徑為80 m的環(huán)形路徑，并用40個(gè)有序路徑點(diǎn)描述該路徑。設(shè)置仿真時(shí)間為35 s，車速為60 km／h，仿真步長(zhǎng)取1 ms，其余參數(shù)如表1所示，仿真結(jié)果如圖7所示。

圖7 圓周工況車輛行駛軌跡

如圖7所示，車輛能夠獲得良好的跟隨效果，表明所述弧長(zhǎng)預(yù)瞄方法下的路徑跟蹤側(cè)向控制模型對(duì)于簡(jiǎn)單路況有效。在該工況下，文獻(xiàn)［6］中所述的預(yù)瞄閉環(huán)系統(tǒng)仿真平均用時(shí)7.57 s，文獻(xiàn)［7］中的則用時(shí)6.36 s。為了更客觀地評(píng)價(jià)本算法的運(yùn)算速度，本文中在兩臺(tái)計(jì)算機(jī)上分別進(jìn)行了20次相同工況的仿真，得出文中所述弧長(zhǎng)預(yù)瞄方法下的閉環(huán)仿真系統(tǒng)平均運(yùn)算時(shí)長(zhǎng)為3.75和4.56 s（計(jì)算機(jī)配置為：神舟T6-X5，CPU i5-7300HQ，主頻2.5 GHz和Thinkpad T540P，CPU i7-4710MQ，主頻2.5 GHz）。結(jié)果表明該模型具有較快的運(yùn)算速度。

4.2 復(fù)雜工況

4.2.1 “7S”道路

為檢驗(yàn)所述基于弧長(zhǎng)預(yù)瞄方法的路徑跟蹤側(cè)向控制模型在復(fù)雜道路上的跟蹤精度，建立文獻(xiàn)［6］中所述的“7S”道路，該道路由7個(gè)彎道組成，總長(zhǎng)560 m，設(shè)置仿真時(shí)間為50 s，車速為40 km／h，仿真步長(zhǎng)取1 ms，其余參數(shù)如表1所示，仿真結(jié)果如圖8和圖9所示，全路段平均跟蹤誤差約為0.08 m，其中最大瞬時(shí)跟蹤誤差約為0.53 m。因此，該模型在預(yù)瞄時(shí)間和預(yù)瞄距離不變的情況下依然能夠在復(fù)雜工況下保持較高的跟蹤精度。

4.2.2 上海國(guó)際賽道

圖8 7S道路上的軌跡跟蹤

圖9 7S道路上的跟蹤誤差

為更好地驗(yàn)證該模型在速度隨路程變化而預(yù)瞄距離與預(yù)瞄時(shí)間不變的條件下對(duì)于復(fù)雜道路的適應(yīng)性與跟隨性，本文中將總長(zhǎng)度約為5.5 km的上海國(guó)際賽道用1 386個(gè)有序路徑點(diǎn)描述出來，并運(yùn)用縱向速度跟蹤控制器建立如圖10所示的速度隨路程變化的仿真模型，仿真時(shí)間修改為335 s，仿真步長(zhǎng)1 ms，其它仿真參數(shù)不變。

圖10 上海賽道速度隨路程變化曲線

仿真結(jié)果如圖11和圖12所示，車輛行駛軌跡與期望路徑幾乎完全重合，全路段平均跟蹤誤差約為0.02 m，最大瞬時(shí)跟蹤誤差約為0.60 m。由此可知，該模型在預(yù)瞄時(shí)間和固定預(yù)瞄距離不變的情況下不僅能夠適應(yīng)復(fù)雜工況及縱向速度變化，還能夠保持較高的跟蹤精度。

圖11 上海賽道的軌跡跟蹤

圖12 上海賽道的跟蹤誤差

5 人-車-路閉環(huán)系統(tǒng)轉(zhuǎn)向盤轉(zhuǎn)角穩(wěn)定影響因素仿真分析

在進(jìn)行CarSim／Simulink聯(lián)合仿真時(shí)發(fā)現(xiàn)，當(dāng)預(yù)瞄距離在某些范圍內(nèi)取值時(shí)，轉(zhuǎn)向盤轉(zhuǎn)角會(huì)發(fā)生劇烈的振蕩，而這會(huì)嚴(yán)重影響人-車-路閉環(huán)仿真系統(tǒng)的穩(wěn)定性與路徑跟蹤控制算法的控制精度，為探明影響轉(zhuǎn)向盤轉(zhuǎn)角穩(wěn)定性的具體原因，進(jìn)行如下仿真分析。

5.1 臨界前視距影響仿真分析

對(duì)于人-車閉環(huán)系統(tǒng)，其系統(tǒng)穩(wěn)定性受駕駛?cè)嘶蛘哕囕v響應(yīng)延遲τ和駕駛?cè)祟A(yù)瞄距離d的影響。當(dāng)預(yù)瞄距離d大于臨界前視距Lp（即車輛以當(dāng)前車速V在延遲時(shí)間τ內(nèi)行駛的距離）時(shí)系統(tǒng)穩(wěn)定，否則系統(tǒng)不穩(wěn)定［8］。為了研究人-車-路閉環(huán)系統(tǒng)的轉(zhuǎn)向盤轉(zhuǎn)角穩(wěn)定性是否仍然符合該特性，進(jìn)行如下仿真實(shí)驗(yàn)：設(shè)響應(yīng)延遲時(shí)間 τ為0.1 s，目標(biāo)車速V為45 km／h（故Lp=τV=1.25 m），仿真時(shí)長(zhǎng)35 s，行駛路徑為4.1節(jié)中所述的圓周工況，插值方式采用線性插值法，預(yù)瞄距離d分別取1和3 m，其余參數(shù)與表1相同，結(jié)果如圖13所示。

由圖13可見，在預(yù)瞄距離d小于臨界前視距Lp時(shí)，轉(zhuǎn)向盤轉(zhuǎn)角發(fā)生劇烈振蕩；在預(yù)瞄距離d大于臨界前視距Lp時(shí)，轉(zhuǎn)向盤轉(zhuǎn)角振蕩幅度明顯減小，但是劇烈振蕩仍然存在。可見，人-車-路閉環(huán)系統(tǒng)下的轉(zhuǎn)向盤轉(zhuǎn)角穩(wěn)定條件與人-車閉環(huán)系統(tǒng)的穩(wěn)定條件并不完全一致。

5.2 路徑連續(xù)性影響仿真分析

文獻(xiàn)［6］中指出期望路徑的方向不連續(xù)性會(huì)導(dǎo)致轉(zhuǎn)向盤轉(zhuǎn)角振蕩，并通過改變插值方式以增強(qiáng)期望路徑點(diǎn)方向的連續(xù)性的方法，有效緩解了該問題。因此，為研究人-車-路閉環(huán)系統(tǒng)轉(zhuǎn)向盤轉(zhuǎn)角的穩(wěn)定性是否只與路徑方向的不連續(xù)性有關(guān)，在5.1節(jié)中的仿真條件下，將線性插值法改為Cubic Spline插值法以增強(qiáng)期望路徑點(diǎn)方向連續(xù)性，結(jié)果如圖14所示。

圖14 Cubic Spline插值下的轉(zhuǎn)向盤轉(zhuǎn)角

由圖14可見，采用Cubic Spline插值法后，當(dāng)預(yù)瞄距離d大于臨界前視距Lp時(shí)，轉(zhuǎn)向盤轉(zhuǎn)角隨著d增大振蕩逐漸收斂，轉(zhuǎn)向盤轉(zhuǎn)角穩(wěn)定性明顯增強(qiáng)；當(dāng)預(yù)瞄距離d小于臨界前視距Lp時(shí)，轉(zhuǎn)向盤轉(zhuǎn)角仍然存在較大振蕩。

由上可知，人-車-路閉環(huán)系統(tǒng)的轉(zhuǎn)向盤穩(wěn)定性不僅受駕駛?cè)耍ɑ蛘哕囕v）響應(yīng)延遲時(shí)間τ和駕駛?cè)祟A(yù)瞄距離d的影響，也受期望路徑點(diǎn)方向連續(xù)性的影響；當(dāng)期望路徑點(diǎn)方向具有較高連續(xù)性，且系統(tǒng)預(yù)瞄距離d大于臨界前視距Lp時(shí)，轉(zhuǎn)向盤趨于穩(wěn)定。

6 結(jié)論

基于預(yù)瞄最優(yōu)曲率模型，本文中提出了一種依據(jù)弧長(zhǎng)預(yù)瞄方法的路徑跟蹤側(cè)向控制模型，通過建模、仿真和分析，可得如下結(jié)論。

（1）采用弧長(zhǎng)預(yù)瞄方法的路徑跟蹤側(cè)向控制模型，無需相對(duì)坐標(biāo)與大地坐標(biāo)相互變換和求解高階方程組，明顯提高了人-車-路閉環(huán)系統(tǒng)的仿真速度，若將其運(yùn)用到無人駕駛控制領(lǐng)域，可彌補(bǔ)環(huán)境感知、數(shù)據(jù)融合與路徑規(guī)劃計(jì)算量較大的不足。

（2）采用弧長(zhǎng)預(yù)瞄方法的路徑跟蹤側(cè)向控制模型，在速度發(fā)生變化而預(yù)瞄距離和預(yù)瞄時(shí)間不變的條件下，對(duì)于不同復(fù)雜程度期望路徑都能保持較高的跟蹤精度與較高的適應(yīng)性。

（3）采用弧長(zhǎng)預(yù)瞄方法下的人-車-路閉環(huán)系統(tǒng)，其轉(zhuǎn)向盤穩(wěn)定性同時(shí)受駕駛?cè)耍ɑ蛘哕囕v）響應(yīng)延遲時(shí)間τ、預(yù)瞄距離d和路徑點(diǎn)方向連續(xù)性的影響，當(dāng)期望路徑點(diǎn)方向具有較高連續(xù)性，且系統(tǒng)預(yù)瞄距離d大于臨界前視距Lp時(shí)，轉(zhuǎn)向盤趨于穩(wěn)定。

（4）本文中的弧長(zhǎng)預(yù)瞄方法路徑跟蹤側(cè)向控制模型在仿真環(huán)境下，不僅跟蹤精度較高，適應(yīng)性較高，還具有計(jì)算速度快等特點(diǎn)，對(duì)路徑跟蹤控制器、駕駛員模擬器及無人駕駛系統(tǒng)仿真軟件開發(fā)等方面有較高的參考價(jià)值。