趙凱 朱愿 馮明月 蘇致遠(yuǎn) 郭宏達(dá)

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

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

1 前言

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

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

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

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

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

2.1 車輛—路徑相對(duì)運(yùn)動(dòng)關(guān)系模型

經(jīng)簡(jiǎn)化和抽象，建立的車輛—路徑相對(duì)運(yùn)動(dòng)關(guān)系模型如圖1所示。其中：EON為全局（大地）坐標(biāo)系，E軸正方向?yàn)檎龞|方向，N軸正方向?yàn)檎狈较颍灰攒囕v后軸中點(diǎn)為坐標(biāo)原點(diǎn)建立局部（車身）坐標(biāo)系XO′Y，規(guī)定X軸的正方向?yàn)檐囕v前進(jìn)方向；δ為前輪轉(zhuǎn)角；θ為航向角，即車輛前進(jìn)方向與E軸的夾角；v為前進(jìn)速度；L為軸距。

圖1 車輛—路徑相對(duì)運(yùn)動(dòng)關(guān)系模型

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

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

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

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

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

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

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

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

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

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

c. 從起始點(diǎn)開始，沿車身前進(jìn)方向，在GPS點(diǎn)序列中依次尋找一個(gè)點(diǎn)滿足：式中，p1為滿足式（3）的點(diǎn)在GPS軌跡點(diǎn)序列中的序號(hào)。

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

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

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

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

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

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

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

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

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

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

圖4 路徑跟蹤算法框架

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

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

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

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

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

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

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

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

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

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

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

式（9）也可以表示為：

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

圖5 Pure Pursuit算法

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

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

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

4 試驗(yàn)驗(yàn)證

4.1 試驗(yàn)平臺(tái)

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

圖6 縮微自主電動(dòng)車

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

首先利用試驗(yàn)平臺(tái)的遙控功能，操控縮微自主電動(dòng)車在校園開闊地帶采集了具有代表性的直線和“S”形差分GPS軌跡點(diǎn)序列作為目標(biāo)路徑。結(jié)合3.1節(jié)對(duì)于預(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。車輛通過(guò)$GPRMC的報(bào)文數(shù)據(jù)確定自身在全局坐標(biāo)系中的位置、航向和速度。

4.2.1 路徑跟蹤效果分析

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

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

4.2.2 橫向控制偏差分析

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

圖8 橫向位置偏差

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

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

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

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

圖10 速度與路徑彎曲度

5 結(jié)束語(yǔ)

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

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