基于多點序列預(yù)瞄的自動駕駛汽車路徑跟蹤算法研究＊

趙凱 朱愿 馮明月 蘇致遠 郭宏達

（1.陸軍軍事交通學(xué)院，天津 300161；2.軍事交通運輸研究所，天津 300161）

主題詞：自動駕駛汽車 預(yù)瞄信息 路徑彎曲度 路徑跟蹤

1 前言

移動機器人是復(fù)雜的非線性系統(tǒng)，且運行于不同的復(fù)雜工況下，其路徑跟蹤問題一直是研究的重點和難點[1-4]。移動機器人所跟蹤的路徑大致可分為顯式（Explicit）和隱式（Implicit）兩種，相對于隱式路徑，顯式路徑的基本思想是只需從傳感器數(shù)據(jù)中提取特定路徑的基本信息，可大幅降低處理的復(fù)雜性，因此多采用顯式路徑作為目標(biāo)跟蹤路徑[5]。為提高移動機器人的路徑跟蹤能力，Maier D等[6]提出了一種改進的GPS傳感器模型來生成城市區(qū)域的GPS軌跡地圖，并使用全地形機器人檢驗了模型的合理性。Takai R等[7]將實時動態(tài)差分GPS（Real-time Kinematic GPS，RTK-GPS）與慣性測量單元（Inertial Measurement Unit，IMU）結(jié)合，用于無人履帶式拖拉機的巡線跟蹤。

在控制策略的選擇上，除傳統(tǒng)的PID控制外，考慮當(dāng)前車輛姿態(tài)和跟隨路徑之間的幾何關(guān)系亦可提供更簡單的跟蹤策略[8]。文獻[4]中，謝德勝等人采用標(biāo)準(zhǔn)卡爾曼濾波器對受干擾時的RTK-GPS數(shù)據(jù)進行濾波，用最優(yōu)估計值作為機器人的真實位置，使用角度偏差PD控制器輸出轉(zhuǎn)向，將橫向偏差控制在0.67 m以內(nèi)，但其移動速度僅為2 m/s。Zhang Wenyu等[8]人采用粒度支持向量回歸（Granular Support Vector Machine，GSVR）的方法提高了Pure Pursuit算法的適應(yīng)性和動態(tài)性能，使得自主機器人以1.2 m/s的速度跟蹤路徑時，直線追蹤最大橫向偏差小于0.088 m。

自動駕駛汽車作為輪式移動機器人的一種，同樣存在著路徑跟蹤這一難題。對此，本文提出了一種基于預(yù)瞄信息的路徑跟蹤算法。車輛接收實時差分GPS數(shù)據(jù)，確定其在全局坐標(biāo)系中的位置，根據(jù)當(dāng)前位姿及其與路徑間的相對運動關(guān)系建立預(yù)瞄點序列搜索模型，使用多點預(yù)瞄的策略預(yù)測路徑的曲率變化，并按一定的規(guī)則控制車速。另外，將預(yù)瞄偏差角作為控制器的輸入，通過Pure Pursuit算法計算出前輪轉(zhuǎn)角控制量，用以控制車輛的轉(zhuǎn)向。最后，通過試驗平臺驗證了跟蹤算法的有效性。

2 多點序列預(yù)瞄信息的獲取

路徑預(yù)瞄信息包括橫向位置偏差、預(yù)瞄偏差角、路徑彎曲度等。對這些信息加以利用能夠使車輛更準(zhǔn)確地跟蹤路徑[9-10]。為了獲取路徑的預(yù)瞄信息，首先需建立車輛和路徑的相對運動關(guān)系模型。

2.1 車輛—路徑相對運動關(guān)系模型

經(jīng)簡化和抽象，建立的車輛—路徑相對運動關(guān)系模型如圖1所示。其中：EON為全局（大地）坐標(biāo)系，E軸正方向為正東方向，N軸正方向為正北方向；以車輛后軸中點為坐標(biāo)原點建立局部（車身）坐標(biāo)系XO′Y，規(guī)定X軸的正方向為車輛前進方向；δ為前輪轉(zhuǎn)角；θ為航向角，即車輛前進方向與E軸的夾角；v為前進速度；L為軸距。

圖1 車輛—路徑相對運動關(guān)系模型

采集到的GPS軌跡實則是一系列有序的經(jīng)緯度坐標(biāo)，將其由WGS-84坐標(biāo)系轉(zhuǎn)換到平面直角坐標(biāo)系，再將軌跡點序列的起點作為坐標(biāo)原點（i=1）建立全局（大地）坐標(biāo)系，GPS軌跡點在全局坐標(biāo)系下的坐標(biāo)表示為(E(i),N(i))(i=1,2,…,n-1,n)，i為GPS軌跡點序號。自動駕駛汽車所跟蹤的目標(biāo)路徑以上述點序列形式給出。設(shè)車輛的后軸中點在全局坐標(biāo)系下的位置坐標(biāo)為(ge,gn)，則車輛的運動學(xué)模型可表示為：

2.2 預(yù)瞄偏差角與路徑彎曲度

在人工駕駛車輛過程中，駕駛員的眼睛對前方路徑不斷進行預(yù)瞄，根據(jù)前方路徑的相關(guān)信息決定車輛的轉(zhuǎn)彎方向、轉(zhuǎn)角及車速，使車輛盡量逼近前方的路徑。對于自動駕駛汽車的路徑跟蹤控制問題，參考人工駕駛行為，引入路徑彎曲度和預(yù)瞄偏差角[11]的概念。

假設(shè)自動駕駛汽車運動方向與連接預(yù)瞄跟蹤點和當(dāng)前位置點的連線的夾角為α，參考文獻[9]，定義該夾角為預(yù)瞄偏差角（Preview-deviation-yaw）。根據(jù)文獻[9]的分析可知，路徑跟蹤的橫向控制問題可以轉(zhuǎn)換成預(yù)瞄偏差角的跟蹤問題。

對于目標(biāo)路徑預(yù)瞄跟蹤點處曲率變化的描述，本文提出多點預(yù)瞄的預(yù)瞄策略。除在目標(biāo)路徑上選取一個預(yù)瞄點作為跟蹤點外，其余預(yù)瞄點僅用來描述路徑的曲率變化，獲取前方路徑的彎曲度。首先建立預(yù)瞄點搜索算法模型如圖2所示，以自動駕駛汽車為參考對象，將GPS軌跡點序列轉(zhuǎn)化到局部坐標(biāo)系XO′Y中，則軌跡點序列在局部（車身）坐標(biāo)系下的坐標(biāo)表示為(X(i),Y(i))(i=1,2,…,n-1,n)：

圖2 預(yù)瞄點搜索算法模型

圖2 中，ρ為預(yù)瞄距離；ed為橫向位置偏差，即當(dāng)前位置與GPS軌跡之間的距離偏差；Pj(j=1,2,…,n-1,n)為在目標(biāo)路徑即GPS軌跡點序列上獲取的預(yù)瞄點序列。

搜索預(yù)瞄跟蹤點P1的步驟為：

a. 由式（2）將GPS軌跡點轉(zhuǎn)化到局部坐標(biāo)系下；

b.在描述目標(biāo)路徑的GPS軌跡點序列中找出與車輛當(dāng)前位置距離最近的點，作為本次搜索的起始點；

c. 從起始點開始，沿車身前進方向，在GPS點序列中依次尋找一個點滿足：式中，p1為滿足式（3）的點在GPS軌跡點序列中的序號。

該點即為預(yù)瞄跟蹤點P1，由此可完成一次預(yù)瞄跟蹤點的搜索。當(dāng)車輛運動至新位置時，重復(fù)上述步驟，完成一次新的搜索。P1確定后，α隨即確定。

人工駕駛行為研究發(fā)現(xiàn)，駕駛員主要根據(jù)道路曲率的變化控制車速[9]。因此，為了對自動駕駛汽車的縱向速度進行控制，P1確定后，需要尋找其余多個預(yù)瞄點Pj(j=2,…,n-1,n)來確定路徑的彎曲程度。在局部標(biāo)系XO′Y中，使用折線表達目標(biāo)路徑的曲率變化比較方便，如圖3所示，通過定義路徑彎曲度C來描述預(yù)瞄點序列處路徑的彎曲程度：

式中，λj為預(yù)瞄點Pj(j=1,2,…,n-1,n)處的切線與車輛前進方向的夾角。

|λj+1-λj|的含義是切線角度的相對變化量，用以描述路徑曲率的變化，進而表示道路的彎曲程度。路徑方向單方向改變或左右擺動變化，路徑的彎曲度都在增加。

圖3 目標(biāo)路徑彎曲度計算示意

其中，Pj(j=2,…,n-1,n)的選取可采用等間隔數(shù)選取，即每間隔一定數(shù)量的路徑序列點選取一個預(yù)瞄點。預(yù)瞄點之間的間隔數(shù)量根據(jù)描述目標(biāo)路徑的序列點的“稀疏”程度來選取適當(dāng)?shù)闹?，否則不足以表征路徑的彎曲程度。可將間隔點之間的歐氏距離之和作為間隔數(shù)量的選取指標(biāo)，該指標(biāo)應(yīng)取(1/10～1/8)ρ。

3 基于多點序列預(yù)瞄的路徑跟蹤算法

對于本文研究的自動駕駛汽車系統(tǒng)，控制量主要是前輪轉(zhuǎn)角和縱向速度。自動駕駛汽車的控制系統(tǒng)是一個典型的時延、非線性不穩(wěn)定系統(tǒng)，而預(yù)瞄控制動作具有明顯的預(yù)見性，從而明顯優(yōu)于傳統(tǒng)的依靠信息反饋產(chǎn)生控制動作的控制算法。本文提出的路徑跟蹤算法框架如圖4所示。

3.1 預(yù)瞄距離的確定

預(yù)瞄距離直接影響路徑跟蹤的精確性，它的選取至關(guān)重要[10,12]。較小的預(yù)瞄距離可使車輛更準(zhǔn)確地跟蹤路徑，并能夠跟蹤更大曲率的路徑；較大的預(yù)瞄距離可減少跟蹤過程中車輛的超調(diào)，提高跟蹤的平穩(wěn)性

圖4 路徑跟蹤算法框架

為了簡化預(yù)瞄距離的調(diào)優(yōu)，可按車輛的縱向速度確定預(yù)瞄距離[9-10]。此外，預(yù)瞄距離通常在最小值和最大值處飽和，因此其與車輛縱向速度之間的關(guān)系可用經(jīng)驗公式表示：

式中，lmin、lmax分別為最小、最大預(yù)瞄距離；a為常數(shù)。

3.2 基于路徑彎曲度的縱向控制算法

在局部坐標(biāo)系下，λj可表示為：

式中，pj為第j個預(yù)瞄點Pj在給定的GPS軌跡點中的序號；(Xr，Yr)為預(yù)瞄點在局部坐標(biāo)系下的坐標(biāo)。

由式（4）、式（6）計算得到路徑彎曲度C后，只考慮路徑曲率變化對于車速的影響時，基本原則為：C越大，車速v越小；反之，車速v越大。在特定的工況下可能需要車速不超過某個值vmax，因此，為了保證速度v隨著路徑彎曲度C的增大而顯著減小，車速的計算為：

式中，kc(kc＞C)為常數(shù)。

在路徑給定的情況下，可離線計算出該路徑各點處的路徑彎曲度C以及整段路徑的最大、最小路徑彎曲度Cmax、Cmin，則kc的選取范圍即為Cmin＜kc＜Cmax。

3.3 基于Pure Pursuit方法的橫向控制算法

基于Pure Pursuit的路徑跟蹤方法是一種幾何方法，被廣泛應(yīng)用于機器人的路徑跟蹤控制[12]。該算法以車輛后軸中點為切點，車輛縱向?qū)ΨQ軸為切線，通過預(yù)瞄偏差角的幾何關(guān)系計算前輪偏角δ，使車輛可沿經(jīng)過預(yù)瞄點的圓弧行駛，在這一過程中預(yù)瞄偏差角也隨之趨于零，如圖5所示。

應(yīng)用正弦定理可得：

式（9）也可以表示為：

式中，ld為當(dāng)前位置與預(yù)瞄點P1間的距離；κ為圓弧曲率。

圖5 Pure Pursuit算法

根據(jù)簡化阿克曼車輛模型，前輪轉(zhuǎn)角δ可表示為：

根據(jù)式（9）、式（10）可以得到基于Pure Pursuit算法的前輪轉(zhuǎn)角控制量為：

ld=ρcosα，帶入式（12）后，便只有1個可調(diào)參數(shù)，即預(yù)瞄距離ρ，這使得算法易于實現(xiàn)和調(diào)整。

4 試驗驗證

4.1 試驗平臺

采用如圖6所示的1∶5縮微自主電動車。該車采用前輪轉(zhuǎn)向、后輪驅(qū)動，質(zhì)量為9.5 kg，軸距58 cm，輪距40 cm，最小離地間隙為6 cm。主控計算機采用NVIDIA的Jetson TX2，最高主頻2 GHz，搭載Ubuntu系統(tǒng)，采用Python作為編程語言。自行設(shè)計了接口和控制電路，產(chǎn)生脈沖寬度調(diào)制（Pulse Width Modulation，PWM）信號控制舵機和電子調(diào)速器，實現(xiàn)差分GPS數(shù)據(jù)轉(zhuǎn)串口輸出、主控計算機和底層的通信，以及為差分GPS板卡和主控計算機供電。

圖6 縮微自主電動車

4.2 試驗結(jié)果及分析

首先利用試驗平臺的遙控功能，操控縮微自主電動車在校園開闊地帶采集了具有代表性的直線和“S”形差分GPS軌跡點序列作為目標(biāo)路徑。結(jié)合3.1節(jié)對于預(yù)瞄距離選取的分析，設(shè)定最小預(yù)瞄距離lmin=2 m，最大預(yù)瞄距離lmax=7 m，常數(shù)a=1.2。差分GPS的更新頻率為20 Hz，故設(shè)控制周期T=0.05 s，最大車速vmax=5 m/s，常數(shù)kc=4。車輛通過$GPRMC的報文數(shù)據(jù)確定自身在全局坐標(biāo)系中的位置、航向和速度。

4.2.1 路徑跟蹤效果分析

目標(biāo)路徑序列點以GPS軌跡點的形式給出。在GPS軌跡采集過程中，人工操控不穩(wěn)定以及GPS信號受到干擾等因素使得所采集的軌跡點不夠平滑，增加了控制的難度。圖7所示為所采集的目標(biāo)路徑和縮微電動車以5 m/s的速度跟隨目標(biāo)路徑的結(jié)果。車輛在初始位置時，相對于目標(biāo)路徑有較大橫向位置偏差，在跟蹤過程中，車輛迅速跟蹤上目標(biāo)路徑，同時也能克服目標(biāo)路徑不平滑帶來的不良影響，跟蹤軌跡非常平滑。

圖7 路徑跟蹤結(jié)果

4.2.2 橫向控制偏差分析

圖8所示為路徑跟蹤過程中的橫向位置偏差曲線。試驗結(jié)果表明，車輛在直線形路段的路徑跟蹤橫向位置誤差可以控制在0.05 m以內(nèi)，在曲率較大的“S”形路段，橫向位置偏差較大，但也可以控制在0.29 m以內(nèi)。不過，從圖7可以看出，目標(biāo)路徑本身不夠平滑，使得彎道段的橫向位置偏差計算結(jié)果偏大。

圖8 橫向位置偏差

4.2.3 跟蹤平穩(wěn)性分析

圖9所示為通過Pure Pursuit算法得到的前輪轉(zhuǎn)角控制量，可以看出，本文的橫向控制方法得出的前輪轉(zhuǎn)角控制量平滑，這使得車輛在跟隨過程中，舵機不會出現(xiàn)較大抖動，跟蹤更加平穩(wěn)。

圖10所示為車速v和路徑彎曲度C的試驗結(jié)果，兩者的關(guān)系滿足式（7）的控制規(guī)則。車輛直線段基本保持勻速運動，在進入彎道時提前減速，并能隨路徑彎曲度的增大而減小，既保證了車輛在轉(zhuǎn)彎時減低速度又能保證所有時刻的速度都不超過最大限定速度。由此可見，本文提出的方法對于車輛在縱向速度的控制是有效的。

圖9 前輪轉(zhuǎn)角控制量

圖10 速度與路徑彎曲度

5 結(jié)束語

本文針對自動駕駛汽車的路徑跟蹤問題，通過在目標(biāo)路徑上提取多個預(yù)瞄點來獲取路徑的預(yù)瞄信息，按C-v關(guān)系模型進行自動駕駛汽車的縱向控制，基于Pure Pursuit方法建立了橫向控制模型，從而實現(xiàn)了車輛的縱、橫向控制。試驗結(jié)果表明，車輛在跟蹤具有代表性的直線和“S”形路徑時，可將橫向位置偏差控制在0.29 m以內(nèi)，其中直線段的橫向偏差可以控制在0.05 m以內(nèi)，驗證了所提方法的有效性以及合理性。

采用上述方法的自動駕駛汽車縱、橫向綜合控制，在實現(xiàn)準(zhǔn)確路徑跟蹤的同時，還保證了車輛行駛的平穩(wěn)性。目前，該方法在研究和試驗過程中忽略了轉(zhuǎn)向滯后的問題，下一步將考慮引入預(yù)測步驟加以解決。對于更高速度下的適應(yīng)性也需要進一步研究和驗證。