基于“Pure Pursuit”自動駕駛汽車的路徑跟蹤控制

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

(1.華東交通大學載運工具與裝備教育部重點實驗室，江西南昌 330013;2.江西江鈴集團江鈴新能源汽車有限公司智能電子開發部，江西南昌 330013)

0 引言

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

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

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

基于以上文獻針對自動駕駛汽車路徑跟蹤控制研究方法存在的不足，為了提高自動駕駛汽車路徑跟蹤控制中的可行性和穩定性，本文作者設計出路徑跟蹤控制技術路線圖，如圖1所示。

圖1 路徑跟蹤控制技術圖

1 參考軌跡設計

1.1 數據采集與轉化

借助某集團新能源汽車自動駕駛平臺可實現車輛高精度定位，選中園區智能電子開發部附近一條線路作為參考軌跡。為了獲得參考軌跡上各點高精度經緯度值，因此車輛在滿足勻低速行駛工況下，對參考軌跡各點經緯度值進行采集。

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

I=Lon·π/180

(1)

B=Lat·π/180

(2)

式中:Lat表示緯度值，Lon表示經度值。

(3)

式中:B0、Tan為常數。

(4)

(5)

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

常數K滿足公式(6)：

(6)

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

(7)

由公式(7)可知，自動駕駛平臺對目標路徑的經緯度數值進行采集，通過進一步轉化到大地坐標系下，即可獲得大地坐標系下坐標值。

1.2 數據處理

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

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

(8)

式中:a1，b1，c1，a2，b2，c2，a3，b3和c3均為相關系數，根據參考軌跡經緯度值及擬合過程，可得：

(9)

2 路徑跟蹤控制

2.1 運動學模型

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

圖2 Ackermann轉向模型

由圖2可知，假定小轉向角時，根據幾何關系，可以得出轉向角為

(10)

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

為簡化計算，將四輪車輛模型轉換為車輛單軌模型，并假設車輛在平面行駛[19]，簡化車輛單軌模型，如圖3所示。

圖3 車輛單軌模型

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

(11)

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

2.2 Pure Pursuit算法

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

圖4 跟蹤示意

結合正弦定理，由圖4可得：

(12)

式中：Ld表示車輛后軸位置到預瞄點距離，α表示目前車身姿態和預瞄點的夾角。

道路曲率k為

(13)

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

(14)

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

(15)

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

則由式(15)可得：

(16)

式中：goal_x表示車輛距離參考軌跡橫向偏差，為了簡化計算，可重新定義一個參數量e，表示車輛當前姿態和目標路點在橫向上的誤差，參考軌跡橫向偏差goal_x即為橫向上的誤差e；goal_y表示車輛預瞄距離，可設為s。

則前輪轉角為

(17)

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

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

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

(18)

2.3 聯合仿真模型搭建

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

圖5 聯合仿真模型

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

3 實驗與分析

3.1 試驗平臺搭建

某集團新能源易至EV3微型車經過改裝后，包括GPS天線、4G天線和慣性導航等定位設備、攝像頭和傳感器等感知設備、車體和工控臺。文中搭建了自動駕駛汽車實車試驗平臺，如圖6所示。

圖6 實車試驗平臺

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

3.2 路徑跟蹤實驗分析

為了驗證聯合仿真結果的正確性，進行了實車試驗，3種實驗工況車速分別為10、20和30 km/h。預瞄距離s分別為5、10、15 m，車輛整備質量為1 115 kg，仿真和試驗橫向距離為60 m，路面附著系數為0.85。考慮到試驗車為純電動汽車，Carsim中傳動系設置為No Dataset，即電動汽車模式。分別對仿真軌跡與參考軌跡、實車軌跡與參考軌跡結果進行了分析與對比。v=10 km/h的軌跡曲線如圖7所示。

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

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

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

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

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

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

表1 路徑跟蹤誤差對比

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

4 結論

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