王麗娟,關(guān)龍新,,張明華,,時樂泉,王愛春,吳曉建,

(1. 南昌大學(xué) 機(jī)電工程學(xué)院,南昌 330031;2. 江鈴汽車股份有限公司,南昌 330001)

隨著5G通信和人工智能技術(shù)的迅速發(fā)展,自動駕駛成為汽車行業(yè)研究熱點[1-2],發(fā)展自動駕駛技術(shù)在許多國家已提升到戰(zhàn)略層面。考慮到當(dāng)前高級別自動駕駛技術(shù)在復(fù)雜城市道路下全面實施仍存在較大挑戰(zhàn),園區(qū)低速場景自動駕駛技術(shù)具備落地可行性。因而,開展低速情況下路徑跟蹤控制[3-4]具有重要應(yīng)用價值。

近年來,國內(nèi)外學(xué)者對車輛的側(cè)向跟蹤控制開展了大量的理論研究和驗證。日本東京大學(xué)的Ando等[5]通過設(shè)計RTK-GNSS/INS和激光雷達(dá)的組合定位方式以及采用線性LQR控制器精確實現(xiàn)橫向控制。密歇根大學(xué)的Xu和Peng[6]基于自行車模型設(shè)計了單點預(yù)瞄的線性二次型控制器,并將道路曲率增廣為狀態(tài)向量,構(gòu)建了LQR控制系統(tǒng)以提高側(cè)向跟蹤精度。Yu等[7]考慮了輪胎特性,設(shè)計了前饋加反饋的側(cè)向跟蹤控制器。陳亮等[8]基于最優(yōu)前輪側(cè)偏力,設(shè)計了LQR橫向控制器。以上文獻(xiàn)側(cè)向控制器均考慮了動力學(xué)特性,且在較高速度工況下能夠?qū)崿F(xiàn)較好的跟蹤效果,為發(fā)展適用于基于動力學(xué)模型的線性二次型算法作了積極探索。但在低速場景下,輪胎受力影響較小,若采用以上基于迭代求解的方法反而會增加車載工控機(jī)的計算負(fù)荷。

針對低速場景,Morales等[9]設(shè)計了一種基于二維激光掃描儀以反應(yīng)性地獲取目標(biāo)點的純跟蹤控制方法。新加坡國立大學(xué)的Andersen等[10]通過考慮追蹤點的航向方向問題,提出了一種改進(jìn)的純追蹤算法來解決切角和超調(diào)問題。Shan等[11]提出一種基于駕駛經(jīng)驗獲取車速,道路前方曲率和前瞻距離的三者關(guān)系設(shè)計模糊控制器動態(tài)調(diào)整前瞻距離的純跟蹤算法。李爽等[12]提出一種依據(jù)車輛實際行駛路程、以固定預(yù)瞄時間及自車車速間接獲取預(yù)瞄點側(cè)向位移的弧長預(yù)瞄方法,推導(dǎo)了預(yù)瞄點側(cè)向位移與車輛前輪轉(zhuǎn)角之間的關(guān)系,并建立了側(cè)向跟蹤控制模型。以上文獻(xiàn)對基于幾何運(yùn)動學(xué)的純跟蹤算法均作了改進(jìn)和創(chuàng)新,但預(yù)瞄距離是基于車輛當(dāng)前車速而設(shè)計的,尚未考慮跟蹤前方軌跡所規(guī)劃的速度可能存在變化,不能更好地體現(xiàn)智駕車輛在未來一段時域內(nèi)對規(guī)劃路徑的預(yù)測。

本文提出一種結(jié)合規(guī)劃路徑和規(guī)劃速度信息的動態(tài)預(yù)瞄距離跟蹤控制算法,提升自車對運(yùn)動規(guī)劃路徑在未來一段時域內(nèi)的預(yù)測能力,使預(yù)瞄距離具備基于實時規(guī)劃速度的適應(yīng)性,更好地達(dá)到實時更新路徑預(yù)覽曲線,實現(xiàn)動態(tài)調(diào)整車輛跟蹤點的目的,同時滿足低速場景下更低算力及更高精度的要求。為此,本文基于該設(shè)計方法,最終獲取具有速度自適應(yīng)性的前視距離-車輛前輪轉(zhuǎn)角關(guān)系。在CarSim和MATLAB/Simulink聯(lián)合仿真環(huán)境下驗證算法的有效性和準(zhǔn)確性,最后在實車平臺進(jìn)行進(jìn)一步地驗證,并對實驗結(jié)果作深入分析。

1 運(yùn)用規(guī)劃路徑及速度的動態(tài)預(yù)瞄距離的路徑跟蹤控制設(shè)計

1.1 路徑跟蹤控制律

純跟蹤算法是卡內(nèi)基梅隆大學(xué)學(xué)者提出的路徑跟蹤控制算法[13]。圖1分別給出了純跟蹤控制算法在行車和倒車時路徑跟蹤的原理圖。

圖1 行車和倒車控制分析Fig. 1 Analysis of travel and reverse control for intelligent vehicle

1.1.1 行車跟蹤控制

如圖1a)所示,點A(xa,ya)為車輛后軸中心,參考路徑點C(xc,yc)為某時刻期望路徑點,C點也被稱為預(yù)瞄點,前視距離ld為預(yù)瞄點與車輛后軸中心點的歐氏距離。車輛航向方向為向量AB的方向,前輪轉(zhuǎn)角為δf,轉(zhuǎn)彎半徑為R,由此可得

ld=2Rsinα

(1)

由幾何關(guān)系可求得α和前視距離ld為:

(2)

(3)

式中:α為車輛后軸中心與預(yù)瞄點連線的航向與車輛行駛航向的差值;θyaw為車輛的行駛航向。

根據(jù)阿克曼轉(zhuǎn)向原理,可求得前輪轉(zhuǎn)角δf為

(4)

由式(1)和式(4)可得

(5)

聯(lián)立式(2)、式(3)和式(5),可求得最終的前輪轉(zhuǎn)角δf為

(6)

式中L為車輛軸距。

1.1.2 倒車跟蹤控制

(7)

行車和倒車過程中車輛航向角相差180°,故需要對其進(jìn)行弧度角歸一化處理,將倒車時的航向角范圍統(tǒng)一限定在(-π,π),防止方向盤轉(zhuǎn)角發(fā)生突變,同時有助于加快收斂,提升控制算法的效率。故有

(8)

(9)

(10)

(11)

(12)

1.2 路徑動態(tài)預(yù)覽模型

參照人工駕駛經(jīng)驗,路徑跟蹤實質(zhì)為預(yù)瞄-跟隨反饋閉環(huán)過程,且預(yù)瞄距離應(yīng)具有速度適應(yīng)性。目前,預(yù)瞄距離是基于車輛當(dāng)前車速而設(shè)計的,尚未考慮跟蹤前方軌跡所規(guī)劃的速度可能存在變化。本文提出結(jié)合規(guī)劃路徑和規(guī)劃速度信息的動態(tài)預(yù)瞄距離方法,即考慮未來的車速狀態(tài)對預(yù)瞄距離進(jìn)行動態(tài)調(diào)整,從而獲取具有速度自適應(yīng)性的前視距離-車輛前輪轉(zhuǎn)角關(guān)系。

幾何學(xué)跟蹤控制模型與參考路徑如圖2所示。依據(jù)規(guī)劃軌跡的屬性,設(shè)計路徑動態(tài)預(yù)覽模型,以車輛后軸中心坐標(biāo)A(xa,ya)作為車輛當(dāng)前點,遍歷參考路徑所有離散點尋找到歐式距離最短的車輛匹配點Pk(xk,yk),以車輛匹配點為起始點動態(tài)預(yù)覽路徑曲線長度為ΔS處的預(yù)瞄點npre(xn,yn,dθyaw),預(yù)瞄點npre即為車輛跟蹤的目標(biāo)路徑點。

圖2 幾何學(xué)跟蹤控制模型與參考路徑Fig. 2 Geometric tracking control model and its reference path

由幾何關(guān)系近似可求得ΔS,即

i=k,k+1,…,n

(13)

根據(jù)式(2)和式(3)可求得前視距離ld_pre和γ_pre為:

(14)

(15)

式(13)～式(15)中:n為路徑動態(tài)預(yù)覽點的個數(shù);γ_pre為車輛后軸中心與預(yù)瞄點連線的航向與車輛行駛航向的差值;Ts為采樣時間;vi為規(guī)劃速度。

當(dāng)預(yù)覽路徑點的數(shù)量n大于剩余參考路徑點的數(shù)量時,則未來每一時刻車輛跟蹤的目標(biāo)路徑點均為參考路徑的最后一個點nend(xend,yend,dθend),同理可近似求得以車輛匹配點為起始點動態(tài)預(yù)覽路徑的曲線長度ΔS*為

i=k,k+1,…,nend

(16)

(17)

(18)

2 側(cè)向跟蹤控制

本文提出的基于路徑動態(tài)預(yù)覽模型的低速智能汽車側(cè)向跟蹤算法,如圖3所示?？紤]未來行駛軌跡中速度的變化,以規(guī)劃速度實時動態(tài)調(diào)整預(yù)瞄距離,從而獲取車輛在下一時刻的路徑跟蹤點,其預(yù)覽機(jī)制更具合理性。

圖3 基于路徑動態(tài)預(yù)覽模型的側(cè)向跟蹤算法Fig. 3 Lateral tracking algorithm based on the dynamic preview path model

在路徑動態(tài)預(yù)覽點的個數(shù)n小于剩余參考路徑點的總數(shù)量時,聯(lián)立式(5)、式(14)和式(15)可求得前輪轉(zhuǎn)角δfpre,計算車輛當(dāng)前點與預(yù)瞄點的側(cè)向跟蹤誤差ed和航向誤差eφ分別為:

(19)

ed=(xn-xa)sin(dθyaw)+(ya-yn)cos(dθyaw) (20)

eφ=θyaw-dθyaw

(21)

式中:δfpre為本文設(shè)計的純跟蹤算法的前輪轉(zhuǎn)角,dθyaw為預(yù)瞄點npre的參考航向。

(22)

(23)

(24)

3 實驗結(jié)果與分析

3.1 仿真實驗

3.1.1 仿真平臺介紹

CarSim和MATLAB/Simulink聯(lián)合仿真平臺由CarSim提供車輛動力學(xué)模塊,MATLAB語言編寫控制器代碼,同時在MATLAB/Simulink環(huán)境下導(dǎo)入?yún)⒖嫉母櫬窂?從而實現(xiàn)閉環(huán)仿真。其中仿真參數(shù)轉(zhuǎn)向系角傳動比為15.9,軸距L為2.865 m。

3.1.2 雙移線工況與復(fù)雜場景工況

為了驗證本文提出的跟蹤算法的可行性和高效性,建立如圖4a)所示的雙移線工況作為仿真實驗的測試場景一,仿真時間設(shè)置為100 s,車輛行駛車速為7 km/h。建立如圖4b)所示的連續(xù)彎道工況作為仿真實驗的測試場景二,進(jìn)一步驗證本文算法的高效性和準(zhǔn)確性,其仿真時間為44 s,車輛行駛車速為28 km/h。

圖4 雙移線與復(fù)雜場景工況Fig. 4 Double-lane change and complex scenario conditions

3.1.3 路徑跟蹤仿真實驗

在CarSim和MATLAB/Simulink聯(lián)合仿真平臺中,應(yīng)用場景一進(jìn)行行車和倒車工況下算法仿真驗證,應(yīng)用場景二進(jìn)行中低速工況下算法控制效果驗證,仿真結(jié)果如圖5～圖7所示。

圖5 測試場景一行車時的路徑跟蹤控制效果(7 km/h)Fig. 5 Path tracking control performance in test scenario one while traveling (7 km/h)

圖6 測試場景一倒車時的路徑跟蹤控制效果(7 km/h)Fig. 6 Path tracking control performance in test scenario one while reversing (7 km/h)

圖7 測試場景二-中低速工況跟蹤控制效果(28 km/h)Fig. 7 Path tracking control performance for low-speed conditions in test scenario two (28 km/h)

仿真場景一中,車輛行車測試的初始狀態(tài)為:后軸中心坐標(biāo)(x0r,y0r)為(0,0),φ=0,δ=0,車輛以7 km/h的速度跟蹤期望路徑。由圖5可知:最大側(cè)向誤差為0.016 9 m,質(zhì)心處最大航向誤差為5.768°,基本穩(wěn)定在4°以內(nèi)。同時,仿真車輛倒車測試的初始狀態(tài)為:后軸中心坐標(biāo)(x0r,y0r)為(0,0),φ=180°,δ=0,車輛同樣以7 km/h的速度跟蹤期望路徑。由圖6可知:該控制器的最大側(cè)向誤差為0.028 6 m,質(zhì)心處最大航向誤差為5.015°,基本穩(wěn)定在4°以內(nèi)。在場景二大地坐標(biāo)系下,車輛以28 km/h的速度跟蹤大曲率彎道路況。由圖7可知:最大側(cè)向誤差為0.079 7 m,基本穩(wěn)定在0.05 m以內(nèi),質(zhì)心處最大航向誤差為7.165°,基本穩(wěn)定在5°以內(nèi)。

仿真實驗結(jié)果顯示,場景一行車測試中的側(cè)向跟蹤精度優(yōu)于倒車測試,但航向跟蹤精度劣于倒車測試,主要原因是行車和倒車時的預(yù)覽距離不一致。兩種工況雖存在一定差異,但總體上跟蹤精度保持在0.03 m以內(nèi)。在場景二中,以中低速工況跟蹤連續(xù)大曲率彎道,其跟蹤精度保持在0.08 m以內(nèi)。通過仿真實驗驗證,證實本文提出的基于路徑動態(tài)預(yù)覽模型的純跟蹤控制方法具有可行性和準(zhǔn)確性。

3.2 實車實驗

3.2.1 實驗平臺介紹

以某品牌車輛為實車實驗平臺,其參數(shù)如表1所示。

表1 實車測試車輛參數(shù)Tab. 1 Tested vehicle parameters

該測試車輛低速自主泊車系統(tǒng)架構(gòu)主要有車載工控機(jī)計算單元、環(huán)境感知模塊、高精地圖、RTK定位模塊、決策規(guī)劃模塊、控制模塊及汽油車智能底盤。圖8為低速自主泊車系統(tǒng)架構(gòu)。

圖8 低速自主泊車系統(tǒng)架構(gòu)Fig. 8 Low-speed autonomous parking system architecture

環(huán)境感知模塊結(jié)合高精度地圖通過攝像頭、超聲波雷達(dá)等傳感器獲取有效車位及障礙物信息實現(xiàn)車位尋找和有效避撞;RTK定位模塊獲取車輛實時定位信息以確保規(guī)劃和控制的精度達(dá)到厘米級;決策規(guī)劃模塊實時規(guī)劃穩(wěn)定連續(xù)的軌跡以確保跟蹤控制的穩(wěn)健性;控制模塊計算最優(yōu)控制量精準(zhǔn)控制智能汽車自主泊入車位;計算單元包含一臺高效算力的車載工控機(jī),各個模塊并行計算,通過機(jī)器人操作系統(tǒng)實現(xiàn)模塊間信息的高效交互。

3.2.2 定位方式GNSS/INS

GNSS/INS組合導(dǎo)航系統(tǒng)是無人駕駛汽車高精度導(dǎo)航定位的關(guān)鍵技術(shù)之一,具有自主性強(qiáng)、導(dǎo)航參數(shù)完備、短時精度高和隱蔽性好等優(yōu)點[14]。在本次實驗車輛中安裝了完備的組合導(dǎo)航(GNSS/INS)設(shè)備,如圖9所示,分別為車載天線和慣性測量單元IMU(加速度計和陀螺儀),由慣性測量單元獲取車輛線運(yùn)動和角運(yùn)動參數(shù)并基于航位實時獲取車輛的位姿信息、速度信息,為本文設(shè)計的側(cè)向跟蹤控制算法實現(xiàn)精準(zhǔn)跟蹤參考路徑提供了定位條件。

圖9 車載天線和IMUFig. 9 Vehicle antenna and IMU

3.2.3 路徑跟蹤實車實驗

為確保本次實車實驗的安全性,實驗場地選擇在某園區(qū)露天停車場,測試車輛車速為7 km/h。測試共分為兩個場景:第一個為大曲率垂直彎道,目的是驗證本文設(shè)計的算法對大彎道場景下的魯棒適應(yīng)性,同時與文獻(xiàn)[15-16]中基于動力學(xué)模型的線性LQR算法作控制效果和計算消耗做對比;第二個場景為自主泊車場景,用于檢測本文設(shè)計的算法在泊車場景下的控制效果。

如圖10和圖11所示,在大曲率垂直彎道場景測試中,本文設(shè)計的算法最大側(cè)向跟蹤誤差和航向誤差分別為0.069 2 m和0.109 8 rad,基于動力學(xué)模型的線性LQR算法在左右彎道場景測試的最大側(cè)向跟蹤誤差和航向誤差分別為-0.124 1 m、-0.127 6 m和0.125 5 rad、0.089 8 rad。

圖10 本文算法與線性LQR算法側(cè)向跟蹤誤差效果比對Fig. 10 Comparison of the effect of lateral tracking error between proposed algorithm and linear LQR algorithm

圖12為本文算法與線性LQR算法基于某型號車載工控機(jī)單次求解時的計算時間比對。本文算法平均求解一次耗時0.056 3 ms,線性LQR算法平均求解一次耗時1.356 9 ms。圖13為本文算法在自主泊車場景下的控制效果,其最大的側(cè)向跟蹤誤差和航向誤差分別為0.153 0 m和0.151 5 rad。實車測試結(jié)果中,在大曲率垂直彎道測試場景下,本文設(shè)計的算法跟蹤精度總體上優(yōu)于基于動力學(xué)的線性LQR算法,同時,計算效率也優(yōu)于LQR算法。主要原因是基于動力學(xué)模型的LQR算法迭代次數(shù)更多,計算消耗更大,而本文設(shè)計的側(cè)向跟蹤算法,考慮跟蹤前方軌跡所規(guī)劃的速度可能存在變化,達(dá)到實時動態(tài)調(diào)整預(yù)瞄距離的作用,具備更低計算消耗和更高跟蹤精度的優(yōu)勢。

圖12 本文算法與線性LQR算法計算時間對比Fig. 12 Comparison of computation time between proposed algorithm and linear LQR algorithm

圖13 本文算法在自主泊車場景的應(yīng)用Fig. 13 Application of proposed algorithm to autonomous parking scenarios

4 結(jié)論

本文提出了一種結(jié)合規(guī)劃路徑和規(guī)劃速度信息的動態(tài)預(yù)瞄距離跟蹤控制算法,能夠提高智能汽車在低速側(cè)向跟蹤控制的精確度。仿真實驗結(jié)果中,低速場景一中最大側(cè)向誤差保持在0.03 m以內(nèi),質(zhì)心處最大航向誤差保持在6°以內(nèi),中低速場景二中跟蹤精度保持在0.08 m以內(nèi),質(zhì)心處最大航向誤差保持在7°左右,表明本文提出的控制策略具有可行性和準(zhǔn)確性。

實車實驗結(jié)果中,在大曲率垂直彎道場景下,本文算法最大跟蹤誤差和航向誤差分別為0.069 2 m和0.109 8 rad,其平均求解一次耗時0.056 3 ms,線性LQR算法平均求解一次耗時1.356 9 ms。結(jié)果表明:低速場景下本文算法跟蹤精度和計算速率均優(yōu)于基于動力學(xué)模型的LQR算法,滿足低速環(huán)境下智能汽車路徑跟蹤的要求。同時,彌補(bǔ)了智駕系統(tǒng)感知模塊模型訓(xùn)練時算力不足的缺陷。