基于改進純跟蹤的智能車路徑跟隨方法*

潘世舉，李永樂，李子先，何濱兵，朱 愿，徐友春

（陸軍軍事交通學院，天津 300161）

前言

近年來，智能車輛迅速發(fā)展，逐步在礦區(qū)、港口、園區(qū)等相對封閉場景中進行商業(yè)化應用，并以輔助駕駛的方式參與人們的日常交通，具有廣闊的前景。智能車系統(tǒng)涉及定位導航、環(huán)境感知、決策規(guī)劃、運動控制等關鍵技術［1-3］，其中路徑跟隨技術是通過控制轉(zhuǎn)向系統(tǒng)改變橫向運動狀態(tài)，使車輛按照預設路線行駛，是運動控制的重要組成部分。

針對路徑跟隨技術，國內(nèi)外學者進行了大量研究。根據(jù)是否需要建立車輛模型，可將現(xiàn)有研究分為無模型方法和有模型方法兩大類。無模型的控制方法主要包括PID 方法［4］、模糊方法［5］、神經(jīng)網(wǎng)絡方法［6］等，這類方法主要依靠人類經(jīng)驗或大量訓練樣本，能夠?qū)崿F(xiàn)路徑跟隨功能，但參數(shù)選定過程復雜，或需要采集大量的高質(zhì)量訓練樣本，訓練耗時長，泛化能力較弱。有模型的控制方法主要包括Stanley方 法［7］、LQR方法［8］、MPC方法［9］和純跟蹤（pure pursuit，PP）方法［10-15］等，這類方法具備參數(shù)選定簡單、跟隨精度高的特點，但隨著模型復雜度的提高，求解難度和運算成本升高，甚至無法滿足實時性要求；另外，復雜車輛模型參數(shù)較多，無法對其進行充分的測量或辨識，也是導致復雜車輛模型在實際車輛控制應用中難于部署和控制精度達不到預期要求的重要原因。

上述方法中，PP 方法是以阿克曼轉(zhuǎn)向模型為基礎的控制方法，考慮了車輛的系統(tǒng)特性，控制參數(shù)少，可移植性和泛化能力較強，但前視距離的選擇主要依靠人工經(jīng)驗，且跟隨曲率較大的路徑時精度較低，因此目前的研究主要集中在前視距離的選擇和偏差反饋的利用。文獻［10］中將車速作為選擇前視距離的依據(jù)，車速越高，前視距離越長，并提出基于橫向偏差的PI 轉(zhuǎn)角補償策略，提高了轉(zhuǎn)彎精度，但跟隨直線時的抗擾動能力較弱。文獻［11］中將目標點的確定分為兩步：第1 步利用速度和橫向偏差得到路徑上的目標點，以求快速消除偏差；第2 步利用橫向偏差和曲率確定路徑外的目標點，但存在直線超調(diào)問題。文獻［12］中以橫向偏差和航向偏差為基礎，通過粒子群優(yōu)化算法實時選定前視距離，提高直線行駛精度，但未考慮轉(zhuǎn)彎工況。文獻［13］中將PID 控制器和PP 控制器結合在一起，利用強化學習模型在兩者之間進行權衡，但路徑跟隨精度較低。文獻［14］中利用PP 方法得到基本轉(zhuǎn)向控制量、深度強化學習得到校正控制量，兩者結合提高了低速行駛條件下的路徑跟隨能力，但需采集訓練樣本和消耗大量訓練時間。文獻［15］中根據(jù)速度和路徑彎度動態(tài)調(diào)節(jié)前視距離，實現(xiàn)農(nóng)機的路徑跟隨控制，同時設置橫向偏差閾值，當偏差大于閾值時對控制量進行放大處理，提高直線行駛的精度，但無法解決消除初始偏差過程中的超調(diào)問題。

針對以上問題，本文中提出一種基于改進純跟蹤（improved pure pursuit，I-PP）的路徑跟隨方法。首先建立純跟蹤模型，并分析純跟蹤模型在初始偏差條件下的控制效果；然后將航向偏差作為反饋量，對前輪轉(zhuǎn)角控制量進行補償，采用類模糊方法動態(tài)調(diào)節(jié)補償權重，并根據(jù)期望轉(zhuǎn)彎半徑限制行駛速度；最后通過實車試驗選定控制參數(shù)，并驗證控制效果。結果表明，I-PP 方法能夠快速消除初始偏差，提高行駛平穩(wěn)性，并保持較低的運算耗時。

1 純跟蹤方法

1.1 純跟蹤模型

純跟蹤模型如圖1 所示。圖中：(x，y)為車輛后軸中心位置坐標；l為軸距；θ為航向角；v為行駛速度；Ld為前視距離；Pd為目標路徑點；Rd為車輛的期望轉(zhuǎn)彎半徑；δd為前輪轉(zhuǎn)角控制量；Od為車輛的轉(zhuǎn)向中心；α為車輛與目標路徑點的前視角度偏差。由阿克曼轉(zhuǎn)向幾何關系得前輪轉(zhuǎn)角、軸距和轉(zhuǎn)彎半徑的關系：

圖1 PP方法示意圖

由PP方法得前輪轉(zhuǎn)角控制量為

1.2 控制效果分析

如圖2（a）所示，智能車與期望路徑存在初始偏差：橫向偏差為le；航向偏差為θe；紅色圓弧為由PP方法確定的行駛路線，與期望路徑的夾角為θd。

隨著時間的推移，車輛與期望路徑的橫向偏差逐漸減小，車輛行駛路線與期望路徑之間始終存在夾角θd，由此導致在消除初始橫向偏差后，車輛以航向偏差θe向期望路徑另一側行駛，橫向偏差反向增大，從而導致車輛在期望路徑附近振蕩，如圖2（b）所示。

圖2 PP方法跟隨期望路徑

綜上所述，PP 方法具有約束條件少、計算量小和控制參數(shù)少等優(yōu)點，但無法滿足實際應用中對消除誤差速度和較小振蕩的要求。

2 改進純跟蹤方法

為實現(xiàn)快速消除偏差，同時保證振蕩較小的目標，將航向偏差作為反饋量，對前輪轉(zhuǎn)角控制量進行補償，采用類模糊方法對補償權重進行動態(tài)調(diào)節(jié)，并根據(jù)期望轉(zhuǎn)彎半徑限制行駛速度。

2.1 基于航向偏差的轉(zhuǎn)角補償

航向偏差反映車輛與期望路徑之間的相對方向關系，是衡量路徑跟隨效果的重要指標之一。參考斯坦福大學提出的Stanley 方法，將航向偏差作為動態(tài)反饋變量對前輪轉(zhuǎn)角進行補償。

式中：δc為補償后的前輪轉(zhuǎn)角控制量；k為補償權重。

2.2 類模糊的補償權重

為快速消除路徑跟隨過程中的偏差，同時在靠近期望路徑時減小振蕩，根據(jù)橫向偏差和航向偏差動態(tài)調(diào)節(jié)補償權重k。當橫向偏差le較大、航向偏差θe較小時，補償權重k應為負大；當橫向偏差le較小、航向偏差θe較小時，補償權重k應為零；當橫向偏差le較小、航向偏差θe較大時，補償權重k應為正大。

根據(jù)上述權重調(diào)節(jié)規(guī)則，可實現(xiàn)補償權重k隨偏差動態(tài)變化，各變量論域如表1 所示，其中l(wèi)max、θmax、m分別為橫向偏差極值、航向偏差極值、補償權重極值，均為定值。

表1 變量論域

為減少計算量，采用類模糊方法，省略模糊化處理、模糊規(guī)則、解模糊等步驟，根據(jù)空間中不同線3點確定唯一平面的方法，確定橫向偏差、航向偏差和補償權重之間的關系。空間中不同線的3 點為(lmax，0，-m)、(0，0，0)、(0，θmax，m)，則補償權重k為

2.3 基于轉(zhuǎn)彎半徑的速度限制

為保證車輛行駛的平穩(wěn)性，根據(jù)轉(zhuǎn)彎半徑對行駛速度進行限制：

式中：vc為速度控制量；vmax為目標行駛速度；Rc為期望轉(zhuǎn)彎半徑。

根據(jù)阿克曼轉(zhuǎn)向模型，由控制量δc確定的轉(zhuǎn)彎半徑為Rc，即式（6）。

3 實車試驗

3.1 試驗平臺

為驗證本文方法在實車環(huán)境中的有效性，利用“軍交木牛Ⅱ”平臺進行實車試驗，如圖3（a）所示。該平臺為課題組自行設計研制的無人載貨運輸智能車，搭載云臺相機、激光雷達、超聲波雷達、慣性導航等高性能傳感器，以及工控機、DSRC、遠距離電臺等設備，能夠?qū)崿F(xiàn)超視距操控、自主行駛、編隊運輸?shù)裙δ埽浖軜嬋鐖D3（b）所示。在校園內(nèi)部的平坦道路開展試驗，路面為干燥且較新的瀝青路面，附著系數(shù)在0.8～1.0 之間。試驗平臺參數(shù)和部分控制參數(shù)如表2所示。

表2 車輛參數(shù)和部分控制參數(shù)

圖3 “軍交木牛Ⅱ”試驗平臺

3.2 前視距離選定試驗

為選定合適的前視距離Ld，分別設定為2、3、4 m，初始橫向偏差為-1 m，初始航向偏差為零，vmax為2 km/h，期望路徑為直線，采用PP 方法進行路徑跟隨試驗，結果如圖4所示。

圖4（a）為橫向偏差變化曲線，前視距離為2、3 m時的超調(diào)量分別為0.13、0.07 m，前視距離為4 m 時無超調(diào)，但行駛后期的路徑跟隨偏差較大，路徑跟隨精度較低。圖4（b）為航向偏差變化曲線，隨著前視距離的增大，航向偏差極值減小，但收斂速度基本一致。圖4（c）為轉(zhuǎn)角控制量變化曲線，為方便數(shù)據(jù)分析，將轉(zhuǎn)角控制量進行歸一化處理，由圖可知，前視距離越大，控制量變化越平穩(wěn)。

圖4 不同前視距離的試驗結果

綜上，為保持較高的路徑跟隨精度，同時保證控制量的平穩(wěn)變化，選定3 m為前視距離。

3.3 m值選定試驗

為選定I-PP 方法的權重極值m，分別設定為2、4、6，Ld=3，初始橫向偏差為-1 m，初始航向偏差為零，vmax為2 km/h，期望路徑為直線，以PP 方法為對照組進行試驗，結果如圖5所示。

圖5 不同m值的試驗結果

圖5（a）為橫向偏差變化曲線，PP 方法的超調(diào)量為0.07 m，m為2、4、6 時無超調(diào)現(xiàn)象，隨著m值增大，偏差消除速度加快。圖5（b）為航向偏差變化曲線，PP 方法的航向偏差極值最小，但存在超調(diào)現(xiàn)象；隨著m值的增大，航線偏差極值逐漸增大，且變化越劇烈。由圖5（c）可知，在消除初始偏差的過程中，PP方法的控制量變化平穩(wěn)，m值越大，控制量的變化越劇烈。

綜上，為快速消除偏差，保證轉(zhuǎn)角控制量平穩(wěn)變化，選定m值為4。

3.4 不同初始偏差試驗

為驗證所提方法在不同初始偏差條件下的路徑跟隨效果，控制參數(shù)不變，vmax為2 km/h，期望路徑為直線，以PP方法為對照組進行試驗。

3.4.1 不同初始橫向偏差

初始航向偏差為零，初始橫向偏差分別為-0.5、1.5 m，結果如圖6 所示。圖6（a）中，PP 方法在不同初始橫向偏差下的超調(diào)量分別為0.03、0.14 m，存在小幅振蕩；I-PP 方法的偏差收斂速度更快，且不存在超調(diào)振蕩現(xiàn)象。圖6（b）中，在初始階段，I-PP方法的控制量遠大于PP 方法，初始偏差的消除速度更快，當偏差較小時，減小控制量，避免超調(diào)振蕩。

圖6 不同初始橫向偏差的試驗結果

結果表明，I-PP 方法能夠適應不同初始橫向偏差的工況。

3.4.2 不同初始航向偏差

初始橫向偏差為-1.5 m，初始航向偏差分別為-45°、-10°、10°、45°，結果如圖7所示。

圖7（a）中，在上述初始航向偏差下，PP 方法的超調(diào)量為0.08、0.04、0.05、0.32 m，I-PP 方法無超調(diào)，初始橫向偏差消除速度更快。

圖7（b）中，當初始航向偏差為-45°、-10°、10°時，I-PP 方法下的偏差極值大于PP 方法；當初始航向偏差為45°時，PP 方法下的偏差極值大于I-PP 方法。I-PP 方法下的航向偏差向零收斂的速度更快，且無超調(diào)振蕩；PP 方法下的航向偏差變化更平穩(wěn)，但存在超調(diào)振蕩。

圖7（c）中，I-PP方法的控制量極值遠大于PP方法，在短時間內(nèi)迅速變化，加快初始偏差的消除速度，同時避免超調(diào)振蕩。

圖7 不同初始航向偏差的試驗結果

結果表明，所提I-PP 方法能夠在不同初始橫向偏差和不同航向偏差的工況下，具有較好的路徑跟隨效果。

3.5 不同速度試驗

為驗證所提方法在不同目標行駛速度條件下的路徑跟隨效果，控制參數(shù)保持不變，初始橫向偏差為1.5 m，初始航向偏差為30°，vmax分別為5、10 km/h，期望路徑為直線，以PP 方法為對照組進行試驗，結果如圖8所示。

由圖8（a）可得，5 和10 km/h 的工況下，PP 方法的超調(diào)量分別為0.14、0.09 m，偏差消除速度較慢；I-PP 方法的超調(diào)量分別為0.02、0.004 m，初始偏差消除速度較快，且無明顯超調(diào)振蕩。由圖8（b）可得，PP 方法下的航向變化更平穩(wěn)，且航向偏差極值小于I-PP 方法。由圖8（c）可得，為快速消除初始偏差，I-PP 方法下的控制量極值更大。圖8（d）可得，在初始階段，I-PP 方法的行駛速度小于PP 方法，主要原因是：I-PP 方法轉(zhuǎn)角控制量較大，為保證平穩(wěn)性，根據(jù)轉(zhuǎn)彎半徑對行駛速度進行了限制。由圖8（e）可得，PP 方法的橫擺角速度極值分別為94、100（°）/s，I-PP 方法為64、73（°）/s，在消除初始偏差的過程中，I-PP方法的行駛狀態(tài)更平穩(wěn)。

圖8 不同速度的試驗結果

結果表明，所提I-PP 方法能夠在不同目標行駛速度條件下，根據(jù)期望轉(zhuǎn)彎半徑調(diào)節(jié)行駛速度，在快速消除初始偏差的同時，提高行駛平穩(wěn)性，路徑跟隨效果較好。

3.6 不同期望路徑試驗

上述試驗中的期望路徑均為直線路徑，期望航向為定值。為驗證所提方法對轉(zhuǎn)彎路徑的跟隨效果，控制參數(shù)不變，初始橫向偏差為1.5 m，航向偏差分別為-30°、30°，以PP 方法為對照組進行試驗，結果如圖9所示。

圖9 期望路徑與行駛路徑

由圖9 可得，當初始航向偏差為-30°時，兩種方法下的行駛路徑向期望路徑靠攏，但PP方法的超調(diào)量大于I-PP 方法。當初始航向偏差為30°時，PP 方法向期望路徑靠攏的更平滑，I-PP 方法向期望路徑靠攏的速度更快。在消除初始偏差后，與PP方法相比，I-PP 方法下的行駛路徑與期望路徑的偏差較小，路徑跟隨效果較好。

在干燥且較新的平坦瀝青路面選定I-PP 方法的控制參數(shù)，與PP 方法相比，I-PP 方法的初始偏差消除速度和行駛穩(wěn)定性有所提高。路面附著系數(shù)發(fā)生變化時，路徑跟隨精度降低，橫向偏差和航向偏差增大。I-PP 方法采用基于航向偏差的轉(zhuǎn)角補償策略，補償權重與橫向偏差、航向偏差相關，因此橫向偏差和航向偏差變化時，轉(zhuǎn)角控制量也隨之變化，保持較高的路徑跟隨精度。

當路面的側傾角度、俯仰角度和附著系數(shù)等狀態(tài)發(fā)生變化時，車輛的質(zhì)心和轉(zhuǎn)向特性等隨之發(fā)生變化，阿克曼轉(zhuǎn)向模型與車輛實際運動狀態(tài)的匹配程度降低，為保持較高的路徑跟隨精度，應適當減小前視距離、增大m值。隨著前視距離的減小和m值的增大，I-PP 方法對偏差的響應更加敏感，轉(zhuǎn)角控制量變化更劇烈，行駛穩(wěn)定性下降。

3.7 不同控制方法試驗

為驗證所提路徑跟隨方法的優(yōu)勢，控制參數(shù)不變，初始橫向偏差為1.5 m，初始航向偏差為30°，vmax為5 km/h，期望路徑為直線，以LQR 方法和MPC 方法為對照組進行試驗，結果如圖10所示。

圖10 不同控制方法的試驗結果

由圖10（a）的橫向偏差變化曲線可得，與PP 方法相比，LQR 方法和MPC 方法能夠消除初始偏差，橫向偏差變化更平穩(wěn)且無超調(diào)現(xiàn)象，但偏差消除速度較慢。由圖10（b）可知，I-PP 方法的航向偏差極值最大，LQR 方法的最小；與其他3 種方向相比，IPP 方法的航向偏差收斂速度更快，且無超調(diào)現(xiàn)象。由圖10（c）可得，MPC、LQR、PP、I-PP 方法的平均耗時分別為0.015、0.008、0.007、0.007 ms，MPC 方法和LQR 方法的部分耗時過長，在PP 方法的基礎上，引入轉(zhuǎn)角動態(tài)補償方法對運算耗時影響不大。

結果表明，I-PP 方法的初始偏差消除速度更快，與MPC 方法和LQR 方法相比具有運算耗時少的優(yōu)勢。

4 結論

本文中提出一種基于改進純跟蹤的路徑跟隨方法。以純跟蹤方法為基礎，以航向偏差為反饋變量對前輪轉(zhuǎn)角進行補償；以橫向偏差和航向偏差為輸入，采用類模糊的方法動態(tài)調(diào)節(jié)補償權重；根據(jù)期望轉(zhuǎn)彎半徑限制行駛速度，提高行駛平穩(wěn)性。試驗結果表明，在初始偏差、目標行駛速度和期望路徑不同的工況下，所提方法能夠?qū)崿F(xiàn)偏差的快速收斂，改善路徑跟隨效果，提高行駛平穩(wěn)性，同時保持較低運算耗時。