基于“Pure Pursuit”自動(dòng)駕駛汽車的路徑跟蹤控制

王亮，陳齊平，羅玉峰，陳立偉，謝加超

(1.華東交通大學(xué)載運(yùn)工具與裝備教育部重點(diǎn)實(shí)驗(yàn)室，江西南昌 330013;2.江西江鈴集團(tuán)江鈴新能源汽車有限公司智能電子開發(fā)部，江西南昌 330013)

0 引言

近年來，隨著汽車保有量的持續(xù)大幅增加，智能汽車和自動(dòng)駕駛技術(shù)有效地減緩交通擁堵、降低車輛事故發(fā)生率，其已成為研究熱點(diǎn)[1]。路徑跟蹤作為自動(dòng)駕駛技術(shù)的核心任務(wù)之一，其主要任務(wù)是控制車輛沿著參考軌跡行駛，路徑跟蹤過程中產(chǎn)生誤差，交通事故率與交通通行效率等均由跟蹤誤差大小決定。而跟蹤誤差主要取決于車輛當(dāng)前時(shí)刻前輪轉(zhuǎn)角，文中直接通過當(dāng)前車速確定預(yù)瞄距離，進(jìn)一步確定預(yù)瞄點(diǎn)橫向誤差，利用Pure Pursuit算法輸出前輪轉(zhuǎn)角，保證了自動(dòng)駕駛汽車沿著參考路徑行駛且降低了行駛過程中交通事故率。因此，研究基于Pure Pursuit算法對(duì)路徑跟蹤控制可行性和穩(wěn)定性有著重要意義。

自動(dòng)駕駛汽車循跡控制方法主要有LQR、MPC和純跟蹤算法[2]，一般分為預(yù)瞄和非預(yù)瞄兩種[3]，非預(yù)瞄控制方法借助于MPC算法[4-6]，在此基礎(chǔ)上運(yùn)用模糊控制算法，實(shí)現(xiàn)橫縱向耦合路徑跟蹤控制[7]。建立車輛運(yùn)動(dòng)學(xué)模型并進(jìn)行線性化處理，得到跟蹤誤差模型,利用未來時(shí)刻誤差確定車輛方向盤轉(zhuǎn)角[8]，但算法過程較復(fù)雜，不易實(shí)現(xiàn)。預(yù)瞄控制即純跟蹤算法[9-10]，純跟蹤控制方法更貼近真實(shí)駕駛風(fēng)格，控制精度較高，且算法簡(jiǎn)單易實(shí)現(xiàn)，因此被廣泛采用到路徑跟蹤控制中[11]，涉及農(nóng)業(yè)機(jī)械、航空航天等各個(gè)領(lǐng)域。利用模糊控制確定插秧機(jī)預(yù)瞄距離，考慮到其轉(zhuǎn)向系統(tǒng)為一階慣性環(huán)節(jié)，因此可建立運(yùn)動(dòng)學(xué)模型，穩(wěn)定地實(shí)現(xiàn)插秧機(jī)路徑跟蹤控制[12]。改進(jìn)的智能飛機(jī)牽引車路徑導(dǎo)航用偏差統(tǒng)計(jì)的方法實(shí)現(xiàn)不同預(yù)測(cè)距離下最優(yōu)融合系數(shù)的選擇，提高控制精度，該算法的路徑軌跡誤差控制在0.5 m范圍內(nèi)，滿足飛機(jī)自動(dòng)牽引滑行的精度要求[13]。但這些均缺乏了與農(nóng)業(yè)機(jī)械、航空航天等領(lǐng)域相關(guān)的仿真軟件去驗(yàn)證方法的準(zhǔn)確性。起初，有學(xué)者將預(yù)瞄距離考慮定值去研究純跟蹤算法[14]，能夠利用機(jī)器視覺保持車道。三輛車以10～60 km/h速度行駛，但較少考慮到車輛速度對(duì)循跡過程影響。OLLERO和HEREDIA等[15]給出了純追蹤模型曲線跟蹤模型在極坐標(biāo)系下的模型公式，對(duì)于曲線跟蹤情況，采用極坐標(biāo)更便于推導(dǎo)車輛運(yùn)動(dòng)學(xué)模型與曲線跟蹤的控制量。但在實(shí)際應(yīng)用中，大多數(shù)儀器給出的實(shí)時(shí)數(shù)值都是基于直角坐標(biāo)系下的數(shù)據(jù)。

目前自動(dòng)駕駛汽車路徑跟蹤控制算法運(yùn)用到實(shí)踐過程中較為復(fù)雜且缺乏與Carsim專業(yè)軟件進(jìn)行仿真驗(yàn)證，文中提出一種基于Pure Pursuit自動(dòng)駕駛路徑跟蹤控制算法，該算法根據(jù)當(dāng)前車速計(jì)算預(yù)瞄距離，進(jìn)而通過自動(dòng)駕駛平臺(tái)獲得預(yù)瞄點(diǎn)橫向偏差確定前輪轉(zhuǎn)角。取得了以下兩個(gè)方面工程貢獻(xiàn)：(1)提出的基于Pure Pursuit自動(dòng)駕駛路徑跟蹤控制算法，算法穩(wěn)定可行，控制計(jì)算過程更簡(jiǎn)單；(2)該控制方法誤差小，精度高，滿足路徑跟蹤控制要求，有助于提高新能源汽車量產(chǎn)。

基于以上文獻(xiàn)針對(duì)自動(dòng)駕駛汽車路徑跟蹤控制研究方法存在的不足，為了提高自動(dòng)駕駛汽車路徑跟蹤控制中的可行性和穩(wěn)定性，本文作者設(shè)計(jì)出路徑跟蹤控制技術(shù)路線圖，如圖1所示。

圖1 路徑跟蹤控制技術(shù)圖

1 參考軌跡設(shè)計(jì)

1.1 數(shù)據(jù)采集與轉(zhuǎn)化

借助某集團(tuán)新能源汽車自動(dòng)駕駛平臺(tái)可實(shí)現(xiàn)車輛高精度定位，選中園區(qū)智能電子開發(fā)部附近一條線路作為參考軌跡。為了獲得參考軌跡上各點(diǎn)高精度經(jīng)緯度值，因此車輛在滿足勻低速行駛工況下，對(duì)參考軌跡各點(diǎn)經(jīng)緯度值進(jìn)行采集。

進(jìn)一步研究Pure Pursuit算法在路徑跟蹤的應(yīng)用，利用高精度定位獲取各點(diǎn)經(jīng)緯度值，涉及不同坐標(biāo)系，存在坐標(biāo)轉(zhuǎn)換的問題[16]。將其轉(zhuǎn)化為大地坐標(biāo)下坐標(biāo)值，地球長(zhǎng)半軸為6 378 136 m，短半軸約6 356 755 m。針對(duì)單點(diǎn)轉(zhuǎn)化大地坐標(biāo)過程，x、y分別表示參考軌跡在大地坐標(biāo)系橫、縱坐標(biāo)值，I、B分別記為經(jīng)度系數(shù)和緯度系數(shù)，計(jì)算公式為

I=Lon·π/180

(1)

B=Lat·π/180

(2)

式中:Lat表示緯度值，Lon表示經(jīng)度值。

(3)

式中:B0、Tan為常數(shù)。

(4)

(5)

式中:e1表示第一偏心率，e2表示第二偏心率。

常數(shù)K滿足公式(6)：

(6)

則由以上表達(dá)式可知，大地坐標(biāo)x、y可表示為公式(7)：

(7)

由公式(7)可知，自動(dòng)駕駛平臺(tái)對(duì)目標(biāo)路徑的經(jīng)緯度數(shù)值進(jìn)行采集，通過進(jìn)一步轉(zhuǎn)化到大地坐標(biāo)系下，即可獲得大地坐標(biāo)系下坐標(biāo)值。

1.2 數(shù)據(jù)處理

將已轉(zhuǎn)化橫、縱坐標(biāo)數(shù)據(jù)導(dǎo)入到MATLAB新建變量中。橫、縱坐標(biāo)共兩個(gè)變量，因此需建立兩個(gè)表格，并分別命名為x1、y1。接著在命名窗口中輸入cftool，選用“Sum of Sine”中選項(xiàng)作為參考軌跡的擬合曲線[17]，該多項(xiàng)式可詳細(xì)描述出直線、連續(xù)彎道等復(fù)雜路徑，便實(shí)現(xiàn)了對(duì)轉(zhuǎn)化后的數(shù)據(jù)擬合。參考軌跡擬合結(jié)果，如公式(8)所示：

y1=a1sin(b1x+c1)+a2sin(b2x+c2)+a3sin(b3x+c3)

(8)

式中:a1，b1，c1，a2，b2，c2，a3，b3和c3均為相關(guān)系數(shù)，根據(jù)參考軌跡經(jīng)緯度值及擬合過程，可得：

(9)

2 路徑跟蹤控制

2.1 運(yùn)動(dòng)學(xué)模型

低速通過彎道時(shí)，轉(zhuǎn)向輪基本無側(cè)向力作用，因此，無人車輪胎滾動(dòng)時(shí)可忽略輪胎側(cè)偏角。此時(shí)車輛轉(zhuǎn)向必須如下所示，外側(cè)輪胎轉(zhuǎn)彎半徑大于內(nèi)側(cè)輪胎[18]，即Ackermann轉(zhuǎn)向模型，如圖2所示。

圖2 Ackermann轉(zhuǎn)向模型

由圖2可知，假定小轉(zhuǎn)向角時(shí)，根據(jù)幾何關(guān)系，可以得出轉(zhuǎn)向角為

(10)

式中:d表示輪距，L表示軸距，R表示轉(zhuǎn)彎半徑，δ1表示左前輪轉(zhuǎn)角，δ2表示右前輪轉(zhuǎn)角，P表示轉(zhuǎn)向中心。

為簡(jiǎn)化計(jì)算，將四輪車輛模型轉(zhuǎn)換為車輛單軌模型，并假設(shè)車輛在平面行駛[19]，簡(jiǎn)化車輛單軌模型，如圖3所示。

圖3 車輛單軌模型

車輛單軌模型將4輪車輛簡(jiǎn)化成2輪的模型，并且假定車輪只在平面上行駛，采用該模型的優(yōu)點(diǎn)能夠在較低速度的場(chǎng)景下預(yù)測(cè)車輛運(yùn)動(dòng)，簡(jiǎn)化了前輪轉(zhuǎn)向角與后軸遵循的曲率之間的幾何關(guān)系，由圖3及式(10)可知：

(11)

式中:δ表示前輪轉(zhuǎn)角，R表示在給定轉(zhuǎn)向角下后軸遵循的圓的半徑。

2.2 Pure Pursuit算法

從車輛單軌模型出發(fā)，Pure Pursuit算法以車后軸為切點(diǎn),車輛縱向車身為切線,通過控制前輪轉(zhuǎn)角，使車輛可以沿著一條經(jīng)過目標(biāo)路點(diǎn)的圓弧行駛[20]。其中，XOY表示大地坐標(biāo)系，xoy表示車輛坐標(biāo)系，(goal_X,goal_Y)表示預(yù)瞄點(diǎn)在大地坐標(biāo)系位置，(Veh_X,Veh_Y)表示車輛在大地坐標(biāo)系XOY位置，跟蹤示意圖，如圖4所示。

圖4 跟蹤示意

結(jié)合正弦定理，由圖4可得：

(12)

式中：Ld表示車輛后軸位置到預(yù)瞄點(diǎn)距離，α表示目前車身姿態(tài)和預(yù)瞄點(diǎn)的夾角。

道路曲率k為

(13)

由式(11)—(13)可求出前輪轉(zhuǎn)角為

(14)

設(shè)預(yù)瞄點(diǎn)在車輛坐標(biāo)系xoy位置為(goal_x,goal_y)，在圖4中，與點(diǎn)(goal_X,goal_Y)重合，通過大地坐標(biāo)系與車輛坐標(biāo)系之間轉(zhuǎn)化[21]，如公式(15)所示：

(15)

式中：θ表示車輛航向角。

則由式(15)可得：

(16)

式中：goal_x表示車輛距離參考軌跡橫向偏差，為了簡(jiǎn)化計(jì)算，可重新定義一個(gè)參數(shù)量e，表示車輛當(dāng)前姿態(tài)和目標(biāo)路點(diǎn)在橫向上的誤差，參考軌跡橫向偏差goal_x即為橫向上的誤差e；goal_y表示車輛預(yù)瞄距離，可設(shè)為s。

則前輪轉(zhuǎn)角為

(17)

預(yù)瞄距離s的選取與車速、跟蹤誤差有關(guān)[12]，預(yù)瞄距離過大會(huì)導(dǎo)致路徑跟蹤存在延遲，預(yù)瞄距離過小則會(huì)導(dǎo)致路徑跟蹤發(fā)生震蕩，在實(shí)際運(yùn)用過程中需要考慮到參考軌跡曲率[22]。因此，預(yù)瞄距離的選取對(duì)自動(dòng)駕駛路徑跟蹤控制有著重要意義。文中自動(dòng)駕駛路徑跟蹤預(yù)瞄距離選取考慮到車速與速度成正比，文中比例系數(shù)為1.9。

橫向偏差e=goal_y成立條件[23]：

[(goal_x)x(i)-s]·[(goal_x)x(i+1)-s]≤0

(18)

2.3 聯(lián)合仿真模型搭建

根據(jù)以上分析，建立Carsim車輛模型與Simulink算法模型，聯(lián)合仿真模型如圖5所示。通過對(duì)參考軌跡數(shù)據(jù)的采集及轉(zhuǎn)化數(shù)據(jù)處理，獲得參考軌跡在大地坐標(biāo)系下橫、縱坐標(biāo)值分別存儲(chǔ)到From Workspace1和From Workspace2兩個(gè)工作空間中；利用MATLAB Function 模塊，結(jié)合存儲(chǔ)到工作空間中的數(shù)值、車輛實(shí)際坐標(biāo)以及實(shí)際航向角，通過坐標(biāo)轉(zhuǎn)化，可計(jì)算出預(yù)瞄點(diǎn)在車輛坐標(biāo)系縱坐標(biāo)值goal_y；將預(yù)瞄點(diǎn)縱坐標(biāo)值作為PP algorithm(Pure Pursuit算法)模塊輸入，另一個(gè)輸入為車輛縱向速度，便可通過算法求出當(dāng)前時(shí)刻方向盤轉(zhuǎn)向角。

圖5 聯(lián)合仿真模型

由圖5可知，Carsim模塊輸出當(dāng)前時(shí)刻車輛實(shí)際位置信息、車輛航向角和縱向速度，為了實(shí)現(xiàn)自動(dòng)駕駛汽車完成參考軌跡跟蹤，根據(jù)車輛當(dāng)前時(shí)刻縱向速度確定預(yù)瞄距離，尋找到當(dāng)前時(shí)刻預(yù)瞄點(diǎn)，利用Pure Pursuit算法求出當(dāng)前時(shí)刻方向盤轉(zhuǎn)向角并輸入到Carsim模塊，從而建立閉環(huán)控制的自動(dòng)駕駛汽車軌跡跟蹤仿真模型。

3 實(shí)驗(yàn)與分析

3.1 試驗(yàn)平臺(tái)搭建

某集團(tuán)新能源易至EV3微型車經(jīng)過改裝后，包括GPS天線、4G天線和慣性導(dǎo)航等定位設(shè)備、攝像頭和傳感器等感知設(shè)備、車體和工控臺(tái)。文中搭建了自動(dòng)駕駛汽車實(shí)車試驗(yàn)平臺(tái)，如圖6所示。

圖6 實(shí)車試驗(yàn)平臺(tái)

DTU模塊支持移動(dòng)、聯(lián)通2G/3G/4G，電信4G網(wǎng)絡(luò)，網(wǎng)絡(luò)支持最大下行速率150 Mb/s和最大上行速率50 Mb/s，在缺乏3G和4G網(wǎng)絡(luò)覆蓋的偏遠(yuǎn)地區(qū)也能正常工作，接收4G天線信號(hào)并輸出差分?jǐn)?shù)據(jù)等相關(guān)報(bào)文信息。982-RG具有實(shí)現(xiàn)差分定位和GPS定位功能，接收DTU模塊差分?jǐn)?shù)據(jù)和無線GPS天線信號(hào)，輸出車輛高精度定位，包括經(jīng)度、緯度值等，可以用于自動(dòng)駕駛汽車參考軌跡獲取。RT由牛津大學(xué)提供的一種集成技術(shù)設(shè)備，由慣性導(dǎo)航(實(shí)時(shí)進(jìn)行精確運(yùn)動(dòng)測(cè)量的儀器，內(nèi)部具有六軸陀螺儀和加速度計(jì)等主要零件組成)、基站(移動(dòng)設(shè)備接入互聯(lián)網(wǎng)的接口設(shè)備)和設(shè)備固定件組成，該設(shè)備用于接收982-RG輸出的GPS信號(hào)和差分?jǐn)?shù)據(jù)，信息輸入到自動(dòng)駕駛汽車工控臺(tái)。

3.2 路徑跟蹤實(shí)驗(yàn)分析

為了驗(yàn)證聯(lián)合仿真結(jié)果的正確性，進(jìn)行了實(shí)車試驗(yàn)，3種實(shí)驗(yàn)工況車速分別為10、20和30 km/h。預(yù)瞄距離s分別為5、10、15 m，車輛整備質(zhì)量為1 115 kg，仿真和試驗(yàn)橫向距離為60 m，路面附著系數(shù)為0.85?？紤]到試驗(yàn)車為純電動(dòng)汽車，Carsim中傳動(dòng)系設(shè)置為No Dataset，即電動(dòng)汽車模式。分別對(duì)仿真軌跡與參考軌跡、實(shí)車軌跡與參考軌跡結(jié)果進(jìn)行了分析與對(duì)比。v=10 km/h的軌跡曲線如圖7所示。

圖7 v=10 km/h的軌跡曲線

由圖7可知，速度v=10 km/h時(shí)，仿真軌跡與參考軌跡、實(shí)車軌跡與參考軌跡變化趨勢(shì)接近一致。仿真軌跡起點(diǎn)坐標(biāo)為(0，-3)，因此初始階段存在振蕩現(xiàn)象。與參考軌跡相比，仿真試驗(yàn)過程中，整個(gè)仿真過程中，最大誤差不超過0.1 m。而實(shí)車驗(yàn)證過程中，橫向位移在區(qū)間[0,20 m]和[30 m,40 m]誤差約為0.2 m，區(qū)間[20 m,30 m]和[40 m,60 m]誤差達(dá)到0.3 m。仿真軌跡和實(shí)車軌跡最大誤差均出現(xiàn)在拐彎處，v=20 km/h的軌跡曲線如圖8所示。

圖8 v=20 km/h的軌跡曲線

由圖8可知，速度v=20 km/h時(shí)，仿真軌跡與參考軌跡、實(shí)車軌跡與參考軌跡變化趨勢(shì)也基本相同。與速度10 km/h跟蹤過程相比，仿真軌跡最大誤差約為0.2 m，實(shí)車軌跡誤差在橫向位移區(qū)間[20 m,30 m] 和[40 m,60 m]時(shí)達(dá)到了1.2 m，在區(qū)間[20 m,30 m]和[40 m,60 m]實(shí)車軌跡誤差約為0.4 m。v=30 km/h的軌跡曲線如圖9所示。

圖9 v=30 km/h的軌跡曲線

由圖9可知，當(dāng)速度v=30 km/h時(shí)，仿真軌跡最大誤差約為0.3 m，實(shí)車軌跡誤差超過了1.5 m，不滿足路徑跟蹤要求。為了直觀地描述不同速度對(duì)應(yīng)仿真實(shí)驗(yàn)和實(shí)車試驗(yàn)的跟蹤效果，其路徑跟蹤誤差對(duì)比如表1所示。

表1 路徑跟蹤誤差對(duì)比

由圖7—9及表1可知，仿真軌跡基本上與參考軌跡一致，對(duì)應(yīng)誤差均小于0.3 m，可以實(shí)時(shí)輸出車輛方向盤轉(zhuǎn)角，完成路徑跟蹤控制。但實(shí)車軌跡存在顯著差別，自動(dòng)駕駛車輛速度低于20 km/h，誤差均小于0.4 m，符合自動(dòng)駕駛路徑跟蹤要求。Pure Pursuit算法僅考慮到車輛運(yùn)動(dòng)學(xué)模型，未考慮到車輛動(dòng)力學(xué)，因此進(jìn)一步驗(yàn)證了該算法適用于低速且路況良好的自動(dòng)駕駛車輛路徑跟蹤控制。自動(dòng)駕駛車輛執(zhí)行機(jī)構(gòu)轉(zhuǎn)向電機(jī)存在延遲現(xiàn)象，導(dǎo)致試驗(yàn)車輛未能及時(shí)跟蹤參考軌跡。因此橫向位移在區(qū)間[0,25 m]內(nèi)，實(shí)車軌跡處于參考軌跡內(nèi)側(cè)，在區(qū)間[25 m,50 m]，實(shí)車軌跡處于參考軌跡外側(cè)。

4 結(jié)論

文中借助某集團(tuán)新能源自動(dòng)駕駛平臺(tái)進(jìn)行了目標(biāo)路徑設(shè)計(jì)。針對(duì)參考軌跡跟蹤控制問題，利用Carsim搭建車輛模型和Simulink搭建Pure Pursuit算法模型，計(jì)算出當(dāng)前時(shí)刻前輪轉(zhuǎn)角，進(jìn)行了路徑跟蹤控制仿真分析。最后，以江鈴新能源汽車為試驗(yàn)平臺(tái)對(duì)所提出的方法進(jìn)行了實(shí)車試驗(yàn)。仿真實(shí)驗(yàn)工況中，最大誤差均小于0.3 m，實(shí)車試驗(yàn)工況中車速低于20 km/h，實(shí)車軌跡誤差小于0.4 m，符合自動(dòng)駕駛汽車路徑跟蹤控制要求，考慮到實(shí)車試驗(yàn)執(zhí)行機(jī)構(gòu)存在延遲，跟蹤誤差較大，但低速工況下均能有效地實(shí)現(xiàn)自動(dòng)駕駛汽車沿著參考軌跡行駛。文中預(yù)瞄距離僅取決于縱向車速，未來將進(jìn)一步考慮到車速變化、循跡誤差大小以及道路曲率等工況對(duì)路徑跟蹤控制影響。