基于改進的Pure Pursuit智能客車軌跡跟蹤算法研究

彭之川， 朱 田， 易慧斌

(中車時代電動汽車股份有限公司湘江新區(qū)分公司， 長沙 412007)

智能駕駛的出現(xiàn)不僅能夠極大緩解駕駛員疲勞，也能一定程度上緩解交通擁堵。在不同的道路狀況和車輛工況下，路徑跟蹤系統(tǒng)是智能駕駛保證行車安全的重中之重?，F(xiàn)廣泛使用的Pure Pursuit路徑跟蹤算法基于單車轉(zhuǎn)向模型，根據(jù)路徑和預(yù)瞄距離計算前輪轉(zhuǎn)角，其效果依賴于預(yù)瞄距離的選取。在大曲率半徑下，預(yù)瞄距離選擇不合適會導致車輛走蛇形、超調(diào)或者無法跟蹤上預(yù)定行駛路徑。因此，本文嘗試采用Stanely控制方法，作為Pure Pursuit方法的補償，優(yōu)化車輛在不同路徑下的軌跡跟蹤效果。

1 智能駕駛客車軌跡跟蹤算法

智能駕駛客車的智能駕駛系統(tǒng)包括感知系統(tǒng)、決策控制系統(tǒng)、定位系統(tǒng)等。智能駕駛客車從定位系統(tǒng)獲取到車輛GPS位置后，與特征地圖進行匹配，得到車輛前方的一條全局路徑，由決策控制系統(tǒng)局部規(guī)劃模塊計算出一條局部路徑，再通過軌跡跟蹤模塊實現(xiàn)車輛按預(yù)定線路行駛。在智能駕駛軌跡跟蹤算法中，傳統(tǒng)的PID算法能夠保證車輛的橫向和縱向精度，但需要大量試驗來標定參數(shù);3次曲線軌跡擬合算法能夠很好地控制車輛以平滑的軌跡行駛到預(yù)定道路上，但在直線到轉(zhuǎn)向的切換過程中，擬合效果不好，會導致控制精度較低。

本文設(shè)計軌跡跟蹤控制器參考人工駕駛，在駕駛員行車過程中，其眼睛會對前方路徑進行觀測，控制方向盤和車速按照自己的意愿行駛。同時根據(jù)期望路徑選取預(yù)瞄點，并根據(jù)路徑橫向偏差和航向角偏差進行方向盤轉(zhuǎn)角補償，確保車輛行駛在預(yù)定軌跡上。

基于Pure Pursuit的軌跡跟蹤算法是基于幾何關(guān)系和阿克曼轉(zhuǎn)向定理來計算車輛前輪轉(zhuǎn)向角[1-3]。如圖1所示，建立大地坐標系，(x，y)坐標點表示坐標平面的位置，x、y軸單位為m，車輛簡化為二自由度的模型，車輛后輪中心點為(x0,y0)，其中(x，y)為期望路徑上的預(yù)瞄點，應(yīng)用正弦定理可以得到：

(1)

計算可得：

(2)

式中：L為智能駕駛客車前后軸距；δ1為前輪轉(zhuǎn)角;La為車輛后輪中心與預(yù)瞄點距離。

由此可以看出，Pure Pursuit軌跡跟蹤算法的效果很大程度上取決于預(yù)瞄點的選取。

基于Stanely的軌跡跟蹤[4]算法是根據(jù)車輛航向角偏差和車輛與期望路徑的橫向偏差來計算車輛前輪轉(zhuǎn)向角。如圖1所示，在和Pure Pursuit相同的坐標系下，根據(jù)幾何關(guān)系可得前輪航向角偏差θe：

θe=δ-θ1

(3)

式中:θ1為期望航向角。

根據(jù)橫向偏差efa和航向偏角，得到Stanely補償前輪偏角δ2：

δ2=θe+arctan(kefa/v)

(4)

其中，k為增益系數(shù)，根據(jù)實際路況所決定，一般直線路段上取0.5，曲線路段上取1。

圖1 軌跡跟蹤控制算法模型圖

由式(3)和式(4)得到最終期望前輪偏角

δ=pδ1+qδ2

(5)

其中，δ、δ1、δ2都是時間t的函數(shù)，p、q為增益系數(shù)，根據(jù)實際路況所決定，一般p取0.8，q取0.2。本文選取的預(yù)瞄點La同樣根據(jù)當前實際車速和道路類型來決定,單位為m。

(6)

式中：v為車輛速度，m/s；t為時間，一般取1 s。

2 智能駕駛客車軌跡跟蹤仿真及試驗

2.1 智能駕駛客車車輛模型

將智能駕駛客車假設(shè)為一個剛體，忽略車輛的俯仰和翻滾運動，假設(shè)質(zhì)心位置不隨車輛載重改變而改變，地面附著系數(shù)為恒定值，忽略客車轉(zhuǎn)向執(zhí)行機構(gòu)的時延，將智能駕駛客車簡化為兩輪模型[5-7]，如圖1所示。

由力矩平衡方程和牛頓第二定律可知[8-9]：

(7)

(8)

式中：m為整車質(zhì)量；V為車輛速度；I為轉(zhuǎn)動慣量；ω為車輛橫擺角速度；G為方向盤與輪胎傳動比；Cf、Cr分別為前、后輪輪胎側(cè)偏剛度；a為質(zhì)心與前軸的距離；b為質(zhì)心與后軸的距離；δ為前輪偏角。

由圖1可以計算得到車輛在x，y方向的速度：

(9)

(10)

式中：θ為車輛的橫擺角。

實際運行過程中，車輛橫擺角較小，因此可以將式(9)和式(10)簡化為式(11)和式(12)。

(11)

(12)

根據(jù)以上公式，可以得到簡化的智能駕駛客車動力學模型[10-11]：

(13)

其中，

根據(jù)式(11)、(12)、(13)，在MATLAB/Simulink中建立車輛動力學模型進行仿真測試。

2.2 仿真結(jié)果分析

根據(jù)以上建立的智能駕駛客車動力學模型，對Pure Pursuit軌跡跟蹤和改進的Pure Pursuit軌跡跟蹤控制策略進行對比，其軌跡跟蹤結(jié)果如下。

2.2.1 直線換道行駛工況

如圖2所示，仿真車速為30 km/h，坐標系為大地坐標系，坐標點表示所處大地坐標下的實際位置。車輛的起始位置為(0，1)處，初始朝向為x軸正方向，在x軸100 m處開始換道，兩車道相鄰2 m。

圖2 改進前、后換道工況軌跡跟蹤仿真結(jié)果

如圖2中虛線軌跡所示，Pure Pursuit控制算法在換道工況時，在90 m處開始介入，車輛在換道過程中，118 m處會出現(xiàn)軌跡跟蹤超調(diào)的現(xiàn)象，之后在150 m處完成換道(誤差為2 cm左右)，換道距離為60 m。

如圖2中點軌跡所示，改進后的Pure Pursuit控制算法在換道工況時，同樣在90 m處開始介入，在120 m即可完成換道(誤差為2 cm左右)，換道距離為30 m，期間并沒有出現(xiàn)軌跡跟蹤超調(diào)的現(xiàn)象。

如圖3所示，縱坐標為方向盤角度，正值為方向盤左轉(zhuǎn)，負值為方向盤右轉(zhuǎn)，橫坐標為仿真步長，每個仿真步長的時間為0.01 s。

圖3 改進前、后換道工況轉(zhuǎn)角仿真結(jié)果

改進前的Pure Pursuit控制算法在進入彎道時，會根據(jù)預(yù)瞄距離得到一個較大的方向盤轉(zhuǎn)角，之后再緩慢反向轉(zhuǎn)動方向盤，但是方向盤往回轉(zhuǎn)動的速度過慢，導致車輛軌跡跟蹤超調(diào)，所以造成整體換道距離變長。

可以看到改進后的Pure Pursuit控制算法在進入彎道時，同樣會計算得到一個較大的方向盤轉(zhuǎn)角，但是其能夠很快根據(jù)車輛位置和期望軌跡來快速調(diào)整方向盤的角度，因此，車輛能夠快速跟上期望軌跡。

2.2.2 彎道行駛工況

如圖4所示，仿真采用的車速為10 km/h，坐標系為大地坐標系，坐標點表示所處大地坐標下的實際位置。車輛的起始位置為(5，3)處，初始朝向為與x軸正方向夾角45°。

圖4 改進前、后轉(zhuǎn)彎工況仿真結(jié)果

如圖4中虛線軌跡所示，Pure Pursuit控制算法會根據(jù)預(yù)瞄距離提前讓車輛開始轉(zhuǎn)向，在整個轉(zhuǎn)向過程中，車輛始終不在期望軌跡上，與其有1～2 m左右的偏差。在(10，15)坐標處，期望軌跡右轉(zhuǎn)彎結(jié)束，進入直行，車輛跟蹤軌跡開始超調(diào)，慢慢和期望軌跡有2 m左右的偏差。

如圖4中點軌跡所示，改進后的Pure Pursuit控制算法能夠使車輛軌跡很快行駛在期望軌跡上，與期望路徑基本重合。在(10，15)坐標處，該算法也存在一定的超調(diào)，但是距離偏差不到1 m，并且很快再次行駛在了期望軌跡上。

如圖5所示，縱坐標為方向盤角度，正值為方向盤左轉(zhuǎn)，負值為方向盤右轉(zhuǎn)，橫坐標為仿真步長，每個仿真步長的時間為0.01 s。

圖5 改進前、后轉(zhuǎn)彎工況仿真結(jié)果

改進前的Pure Pursuit控制算法在轉(zhuǎn)向工況計算轉(zhuǎn)角時，方向盤變化平滑，但是其向右轉(zhuǎn)動的最大角度達到了800°，超調(diào)之后，再慢慢向左回正，導致客車的跟蹤軌跡一直處于調(diào)整之中，是軌跡跟蹤過程中沒有對實際航向角和橫向偏差閉環(huán)控制所致。

改進后的Pure Pursuit控制算法，方向盤向右的最大角度雖然也達到了800°，但是從兩曲線的斜率可以看出，所耗費時間更短，即方向盤轉(zhuǎn)速更快。到了出彎點附近的時候，方向盤也能快速回正，這正是采用了Stanely控制算法作為Pure Pursuit控制算法的結(jié)果補償，使客車能夠最大限度行駛在期望軌跡上，并且變化也較平緩，車輛平順性得到保證。

綜上所述，改進的Pure Pursuit控制算法能夠使智能駕駛客車更好實現(xiàn)軌跡跟蹤。

2.3 智能駕駛客車軌跡跟蹤試驗

以C/C++代碼開發(fā)改進的Pure Pursuit控制算法后，放入12 m智能駕駛客車決策控制器中進行實車軌跡跟蹤效果測試。智能駕駛客車的期望軌跡是繞廠房一圈，其中包含有左轉(zhuǎn)、右轉(zhuǎn)、進出站以及換道等工況，以轉(zhuǎn)向和換道的試驗結(jié)果來驗證改進的Pure Pursuit控制算法的可行性，如圖6所示，坐標系為大地坐標系，坐標點表示所處大地坐標下的實際位置，以車輛的起始點為原點。

(a) 換道工況

(b) 轉(zhuǎn)向工況

從圖6所示的是換道工況、轉(zhuǎn)向工況的試驗結(jié)果可以看到，實線的車輛實際軌跡線和虛線的期望軌跡基本重合，軌跡偏差能夠控制在0.5 m左右。

3 結(jié) 論

根據(jù)實車試驗結(jié)果，能夠證明本文所提出的改進的Pure Pursuit控制算法能夠很好地實現(xiàn)軌跡跟蹤，使智能駕駛客車行駛在期望軌跡上。