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

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

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

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

1 前言

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

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

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

2 多點序列預瞄信息的獲取

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

2.1 車輛—路徑相對運動關系模型

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

圖1 車輛—路徑相對運動關系模型

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

2.2 預瞄偏差角與路徑彎曲度

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

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

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

圖2 預瞄點搜索算法模型

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

搜索預瞄跟蹤點P1的步驟為：

a. 由式（2）將GPS軌跡點轉化到局部坐標系下；

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

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

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

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

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

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

圖3 目標路徑彎曲度計算示意

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

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

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

3.1 預瞄距離的確定

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

圖4 路徑跟蹤算法框架

為了簡化預瞄距離的調優，可按車輛的縱向速度確定預瞄距離[9-10]。此外，預瞄距離通常在最小值和最大值處飽和，因此其與車輛縱向速度之間的關系可用經驗公式表示：

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

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

在局部坐標系下，λj可表示為：

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

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

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

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

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

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

應用正弦定理可得：

式（9）也可以表示為：

式中，ld為當前位置與預瞄點P1間的距離；κ為圓弧曲率。

圖5 Pure Pursuit算法

根據簡化阿克曼車輛模型，前輪轉角δ可表示為：

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

ld=ρcosα，帶入式（12）后，便只有1個可調參數，即預瞄距離ρ，這使得算法易于實現和調整。

4 試驗驗證

4.1 試驗平臺

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

圖6 縮微自主電動車

4.2 試驗結果及分析

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

4.2.1 路徑跟蹤效果分析

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

圖7 路徑跟蹤結果

4.2.2 橫向控制偏差分析

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

圖8 橫向位置偏差

4.2.3 跟蹤平穩性分析

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

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

圖9 前輪轉角控制量

圖10 速度與路徑彎曲度

5 結束語

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

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