基于反饋純跟蹤的智能車路徑跟隨方法*

潘世舉，李建市，李 華，婁靜濤，徐友春

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

前言

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

針對路徑跟隨技術，國內外學者進行了大量研究。PID 控制［4］是根據車輛與期望路徑之間的狀態偏差，利用比例、積分、微分的方式計算控制量，具有無需建立系統模型的優點，但參數整定難度大。模糊控制［5］是以模糊數學為基礎的控制方法，對復雜的車輛系統具有較好的控制效果，但隸屬度函數和模糊規則的設計主要依靠人的經驗。神經網絡控制［6］是應用神經網絡技術，依靠大量樣本對難以精確建模的復雜非線性車輛系統進行模型識別，或直接作為控制器，實現路徑跟隨功能，但存在過程不可控、泛化能力較弱的缺點。Stanley 方法［7］是設計基于前軸中心與期望路徑間橫向偏差的非線性反饋函數，實現橫向誤差指數收斂于0，跟蹤精度較高，但對期望路徑的平滑度要求高，抗干擾能力較弱。LQR 方法［8］通過全狀態線性反饋控制律構成閉環最優控制系統，設計兼顧多項性能指標的目標函數得到最優解，但對復雜非線性車輛系統的線性化處理會導致模型不準確，對路徑連續性的要求較高。MPC 方法［9-11］經過模型預測、滾動優化、反饋校正等過程，具有系統地考慮預測信息和處理多約束優化問題的能力，但隨著模型復雜度的提高和約束維度的增加，求解難度和運算成本升高，模型參數不準確或存在外部干擾時效果較差。自適應控制［11-12］包含模型參考自適應控制和自校正控制，前者是設計自適應結構克服車輛模型參數不準確、執行器精度低等問題；后者是在線估計系統模型參數，動態調整控制器參數，使系統適應環境的變化，存在設計難度大、傳遞函數構造依賴經驗等問題。滑模控制［13-14］的不連續性可以迫使系統按照預定“滑動模態”的狀態軌跡運動，對參數不確定性和外部擾動具有強魯棒性，但會在有延遲或未建模的高頻動態特性時產生控制量振蕩現象，影響車輛的穩定性。魯棒控制［14-16］的主要理論有Kharitonov區間理論、H∞控制理論和結構奇異值理論（μ理論）等，當車輛行駛工況變化、外部干擾以及建模誤差等不確定性存在時，具有優越的魯棒穩定性和魯棒性能，但魯棒控制器的階數較高，運算量大。

純跟蹤方法是基于阿克曼轉向模型的控制方法，考慮了車輛的運動學特性，控制參數少且整定過程簡單，對路徑平滑度要求低，求解難度和運算量較小，滿足智能車輛的實時性要求。文獻［17］中將車速作為選擇前視距離的依據，車速越高，前視距離越長，并提出基于橫向偏差的PI 轉角補償策略，提高了轉彎精度，但跟隨直線時的抗擾動能力較弱。文獻［18］中將目標點的確定分為兩步：第一步利用速度和橫向偏差得到路徑上的目標點，以求快速消除偏差；第二步利用橫向偏差和曲率確定路徑外的目標點，但仍存在轉彎偏內、出彎偏外的問題，跟隨精度較低。文獻［19］中以橫向偏差和航向偏差為基礎，通過粒子群優化算法實時選定前視距離，提高直線行駛精度，但未考慮轉彎工況。文獻［20］中將PID 控制器和純跟蹤控制器結合在一起，利用強化學習模型在兩者之間進行權衡，路徑跟隨精度較低。文獻［21］中利用純跟蹤方法得到基本轉向控制量、深度強化學習得到校正控制量，兩者結合提高了低速行駛條件下的路徑跟隨能力，但需采集訓練樣本和消耗大量訓練時間。文獻［22］中依據速度和路徑彎度調整前視距離，實現農機的路徑跟隨控制，同時設置橫向偏差閾值，當偏差大于閾值時對控制量進行放大處理，提高直線行駛的精度，但跟隨曲線路徑的精度較低。

針對以上研究中存在的問題，提出一種反饋純跟蹤的控制方法，提高路徑跟隨精度和行駛穩定性。首先分析純跟蹤方法中影響控制效果的因素；然后根據車輛速度和路徑曲率動態調整前視距離，并在轉彎半徑較小時，以橫向偏差為反饋變量，對前輪轉角控制量進行動態補償；接著通過仿真試驗選定控制參數，并以傳統純跟蹤方法為對照組，驗證所提出方法的有效性；最后利用“軍交木牛Ⅱ”平臺在不同速度和載荷工況下進行實車試驗驗證。

1 純跟蹤方法

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

圖1 純跟蹤方法示意圖

由純跟蹤方法得前輪轉角控制量：

2 反饋純跟蹤方法

為提高路徑跟隨精度和穩定性，首先對傳統純跟蹤模型進行變換處理，分析影響路徑跟隨效果的因素，然后提出基于車速和路徑曲率的前視距離動態調整方法，并以橫向偏差為反饋對前輪轉角進行補償。

2.1 路徑跟隨效果影響因素分析

為便于分析前視距離和前視橫向偏差對路徑跟隨效果的影響，對純跟蹤模型進行變換處理。由圖1可得前視角度偏差α與前視橫向偏差ed的關系：

結合式（2）和式（3），將前視橫向偏差ed引入前輪轉角控制量的計算：

由式（4）可知，當前視橫向偏差ed一定時，僅存在一個未知參數Ld。當Ld較大時，存在2 階系統的過阻尼階躍響應，前輪轉角較小，車輛表現為以較大轉彎半徑緩慢靠近目標路徑點，到達期望路徑點的耗時較長；當Ld較小時，存在2階系統的欠阻尼階躍響應，前輪轉角較大，車輛表現為以較小轉彎半徑快速靠近目標路徑點，到達期望路徑點的耗時較短。因此前視距離的大小決定車輛向目標路徑點收斂的速度。

當前視距離Ld已確定時，純跟蹤方法是以前視橫向偏差為輸入的前饋控制方法。如圖2（a）所示，當車輛在彎道內行駛時，利用前視距離Ld確定的目標點在車輛的右側，由式（4）計算得到向右的前輪轉角量，控制車輛沿虛線所示的路徑接近目標點。在此過程中，車輛行駛路徑與期望路徑之間始終存在實時橫向偏差，如陰影所示，且無法通過式（4）主動消除。

圖2 前視距離的選定

綜上所述，固定的前視距離和只依靠前視橫向偏差無法得到滿意的路徑跟隨效果。

2.2 動態前視距離

有關研究結果表明車輛行駛速度影響前視距離的選定［17］。當車速較高時，為保證駕駛安全性和車輛平穩性，需要對遠處道路情況提前做出反應，要求前視距離較大；當車速較低時，可適當降低前視距離。

此外，路徑彎曲程度同樣會影響前視距離的選擇［22］。如圖2（b）所示，采用前視距離Ld跟隨更彎曲的期望路徑時，虛線所示的車輛行駛路徑與期望路徑之間的橫向偏差變大，路徑跟隨精度降低。為保持跟隨精度，應采用較小的前視距離L1，減小虛線所示的車輛行駛路徑與期望路徑之間的橫向偏差。

綜上，車速越高、路徑彎曲程度越低，前視距離應越大；車速越低、路徑彎曲程度越高，前視距離應越小。假設前視距離與車速、路徑彎曲程度之間存在線性關系，則前視距離Ld由式（5）確定。

式中：L0為前視距離基值，為定值；k1為車速系數，k1＞0；k2為曲率系數，k2＜0；c為距離車輛后軸中心最近的路徑點曲率。

2.3 反饋補償轉角

橫向偏差是衡量路徑跟隨精度的重要指標，質量變化、道路傾斜、輪胎側滑等均會導致純跟蹤方法的橫向偏差增大。為提高路徑跟隨精度，克服純跟蹤方法無法主動消除橫向偏差的問題，提出以橫向偏差為反饋變量的補償轉角計算方法，如圖3 所示。圖中el為橫向偏差，k3為橫向偏差系數，參照式（4）計算反饋轉角補償控制量：

圖3 反饋補償轉角計算方法示意圖

由車輛運動學模型［18］可知，前輪轉角一定時，車速越高，橫擺角速度越大；車速越低，橫擺角速度越小。當車速較低時，應增大前輪轉角控制量，保持較高的橫擺角速度，從而快速消除橫向偏差。因此，車速越低，k3應越大。假設車速和k3之間存在如下關系。

式中：r為反饋補償的轉彎半徑閾值，當轉彎半徑大于r時k3為零，依據經驗確定；n為系數，與車輛速度共同確定k3的大小；k3max為k3的飽和值，依據經驗確定。

綜合式（4）和式（6），反饋純跟蹤方法的前輪轉角控制量δr由純跟蹤轉角δd和反饋補償轉角δc兩部分組成：

3 仿真試驗

3.1 仿真環境

采用軍交智能車團隊研發的智能駕駛軟件系統進行仿真試驗，該系統由用戶界面（user interface，UI）、定位（localization）、宏觀規劃（routing）、環境感知（perception）、決策規劃（planning）、運動控制（control）、底層驅動（chassis）等模塊構成，模塊間采用ROS（robot operating system）的消息發布和訂閱機制進行數據交互。其中UI 模塊內嵌仿真功能，采用運動學模型［18］推算車輛運動狀態。

為保證仿真的真實性，在仿真過程中使用的車輛模型參數與實車一致，即軸距l為1.2 m，前輪轉角范圍為[-0.593 4 rad，0.593 4 rad]，并將前輪轉角的執行延時設置為0.1 s。為排除縱向控制對路徑跟隨的影響，車速與所設定的速度一致。

仿真試驗時，啟用UI、Routing、Planning 和Control 模塊。其中UI 提供仿真和交互功能，數據發布周期為0.02 s；Planning 以車輛位置為起點，結合高精度地圖生成期望路徑，數據發布周期為0.1 s；Control 根據車輛運動狀態和期望路徑，計算前輪轉角控制量，數據發布周期為0.02 s，各模塊間的數據交互如圖4（a）所示。

圖4 數據交互和行駛路線

控制技術測試的常用工況包括雙移線、“S”型線、“8”型線等。其中雙移線的轉彎半徑較大，車輛行駛速度較快，重點觀測車輛側向加速度和質心側偏角等狀態，評估軌跡跟蹤精度和車輛穩定性；“S”型線和“8”型線的轉彎半徑較小且存在連續彎道，重點評估車輛在連續轉彎工況的軌跡跟蹤精度。本文研究主要集中在提高低速工況下駛入和駛出彎道的路徑跟隨精度和行駛穩定性，上述常用工況不能良好地滿足試驗要求，因此選擇基于校園高精度地圖的路線。所選路線包含直線、右轉、左轉工況，最小轉彎半徑為5.2 m，如圖4（b）所示。

采用離散、差別大的數值進行控制參數選定試驗，重點在于分析參數取值對控制效果的影響，有利于本文方法在其他智能車輛平臺的移植應用；最終選定的參數并非最優參數。

3.2 前視距離選定試驗

為選定合適的前視距離Ld和前視距離基值L0，進行仿真試驗。車速v設定為2 m/s，假設Ld=L0，并設定為1.5、3.0、5.0 m，采用純跟蹤方法進行路徑跟隨試驗，結果如圖5所示。

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

圖5（a）為橫向偏差變化曲線，圖5（b）為轉角控制量變化曲線，為方便數據分析，將轉角控制量進行歸一化處理。前視距離為1.5、3.0、5.0 m 時的橫向偏差極值分別為0.124、0.139、0.399 m，控制量極值分別為40.1、37.1、32.4。隨著前視距離的增大，跟隨偏差增大，控制量極值降低。前視距離為1.5 m時，跟隨精度較高，2 階系統的欠阻尼階躍響應明顯（圖5（b）），轉角控制量變化較快，產生較大的振蕩；前視距離為5.0 m 時，2 階系統呈現過阻尼階躍響應，路徑跟隨精度較低；前視距離為3.0 m 時，2階系統的欠阻尼階躍響應存在但不明顯，路徑跟隨精度較高。綜上所述，為保證路徑跟隨精度，同時兼顧車輛行駛的平穩性，選定3 m 為前視距離基值和傳統純跟蹤方法的固定前視距離。

3.3 動態前視距離試驗

為選定合適的動態前視距離車速系數k1，設定車速v為2 m/s，前視距離基值L0為3.0 m，k1分別為0.05、0.1、0.3，k2=0，采用純跟蹤方法進行路徑跟隨試驗，結果如圖6 所示，橫向偏差極值分別為0.129、0.133、0.165 m，控制量極值分別為37.1、36.7、35.8。隨著k1增大，路徑跟隨精度降低，但欠阻尼階躍響應減弱，車輛穩定性增強。綜上，選定k1的值為0.1。

圖6 不同k1的試驗結果

為選定合適的動態前視距離曲率系數k2，設定車速v為2 m/s，前視距離基值L0為3.0 m，k1為0.1，k2分別為-5、-10、-15，采用純跟蹤方法進行路徑跟隨試驗，結果如圖7 所示，橫向偏差極值分別為0.128、0.116、0.163 m，隨著k2減小，路徑跟隨精度有所下降。k2=-15 時，轉彎時的前視距離驟降至1 m 以下（圖7（c）），導致轉角控制量過大，超出允許范圍（圖7（b）），且在短時間內劇烈振蕩，車輛穩定性變差。綜上，選定k2的值為-10。

圖7 不同k2的試驗結果

由上述結果可知，適當減小前視距離能提高路徑跟隨的精度，但隨著前視距離的逐漸縮短，精度的提升空間有限，同時會導致轉角控制量的劇烈變化，影響車輛行駛的平穩性。

3.4 反饋補償轉角試驗

為選定合適的系數n，設定車速v為2 m/s，L0為3 m，k1=0.1，k2=-10，轉彎半徑閾值r和飽和值k3max依據經驗確定，分別為300 m、10，n分別為1、2、3，采用反饋純跟蹤方法進行路徑跟隨試驗，結果如圖8所示。橫向偏差極值分別為0.078、0.074、0.076 m，反饋補償轉角極值分別為5.3、6.4、13.6。隨著n逐漸增大，跟隨精度會有所提高，當n=3時反饋轉角控制量變化劇烈，影響車輛穩定性。綜上，選定n的值為2。

圖8 不同n的試驗結果

3.5 不同速度工況試驗

3.5.1 固定速度試驗

為驗證反饋純跟蹤方法在不同速度工況下的路徑跟隨效果，參數設置與3.4 節保持一致，以3 m 前視距離的純跟蹤方法為對照組，設定車速為0.8、1.5、3.0 m/s，試驗結果如圖9 所示。純跟蹤方法的橫向偏差極值分別為0.125、0.118、0.112 m，反饋純跟蹤方法分別為0.077、0.080、0.078 m。

圖9 固定速度的試驗結果

3.5.2 變化速度試驗

為驗證反饋純跟蹤方法在變化速度工況下的路徑跟隨效果，設定車輛的最高速度為5 m/s，其他條件與3.5.1節保持一致，結果如圖10所示。圖10（a）為車輛速度變化曲線，在進入彎道前減速，駛出彎道后加速。圖10（b）為橫向偏差變化曲線，純跟蹤和本文方法的橫向偏差極值分別為0.147、0.079 m。

圖10 變化速度的試驗結果

綜上，在2 m/s 速度工況下選定的控制參數，能夠在其他速度工況下保持較小的橫向偏差，表明兩種方法的魯棒性較好；在3 種速度工況下，純跟蹤方法的橫向偏差極值大于0.10 m，反饋純跟蹤方法的橫向偏差極值小于0.08 m，后者的跟隨精度和適應性較好。

4 實車試驗

4.1 試驗平臺

為驗證本文方法在實車環境中的有效性和仿真試驗選定參數的控制效果，利用“軍交木牛Ⅱ”平臺進行實車試驗，如圖11（a）所示。該平臺為課題組自行設計研制的無人載貨運輸智能車，搭載云臺相機、激光雷達、超聲波雷達、慣性導航等高性能傳感器，以及工控機、DSRC、遠距離電臺等設備，能夠實現超視距操控、自主行駛、編隊運輸等功能。

圖11 試驗平臺和路徑曲率

在瀝青鋪裝的校園道路中選取長度為250 m 的行駛路線，包含左轉、右轉、“S”型彎、直線等，曲率隨路線長度的變化如圖11（b）所示，曲率大致連續，存在短距離的小幅波動。

4.2 不同速度工況試驗

為驗證本文方法在不同速度工況下的路徑跟隨效果，采用仿真試驗選定的控制參數，以3 m 前視距離的純跟蹤方法、基于動力學模型的MPC 方法為對照組，設定車速為0.8、2.0和3.0 m/s，試驗結果如圖12 所示。純跟蹤方法的橫向偏差極值分別為0.42、0.18、0.17 m，MPC 方法分別為0.07、0.08、0.08 m，本文方法分別為0.15、0.12、0.09 m。橫向偏差極值由大到小依次為純跟蹤方法、本文方法、MPC方法。

圖12 不同速度的試驗結果

圖12（b）～圖12（d）為不同速度下的轉角控制量。由圖可知：純跟蹤方法的轉角變化平穩連續；MPC方法在曲率不連續處，轉角控制量出現振蕩，振蕩程度隨車速升高而增大，收斂速度與曲率平滑度相關；本文方法為保證在彎道內的跟隨精度，對前輪轉角的調節較為頻繁，轉角幅值較大，隨車速升高，調節頻率和幅值有所下降，在低速轉彎工況下，對車輛穩定性影響不大。

綜上，純跟蹤方法對路徑平滑程度要求較低，控制量變化平穩，跟隨精度較低；MPC 方法的精度較高，控制量變化依賴路徑平滑度，易發生振蕩；反饋純跟蹤方法對路徑依賴程度較低，具備較高的跟隨精度和穩定性。

4.3 不同載荷工況試驗

“軍交木牛Ⅱ”為無人載貨運輸智能車，要求在不同載荷工況下具備良好的路徑跟隨精度，如圖13（a）所示。以純跟蹤方法、MPC 方法為對照組，載荷為115、192 kg，車速為0.8、2.0 m/s 進行試驗，橫向偏差統計結果如圖13（b）～圖13（c）所示。115 kg載荷下，純跟蹤方法的橫向偏差極值分別為0.33、0.31 m，MPC方法為0.26、0.24 m，本文方法為0.16、0.11 m；192 kg 載荷下，純跟蹤方法的橫向偏差極值分別為0.47、0.32 m，MPC 方法0.33、0.29 m，本文方法為0.18、0.16 m；本文方法的偏差極值較小，且偏差分布較集中。

圖13 不同載荷的試驗結果

3 種方法在實車工況下的橫向偏差極值如圖13（d）所示。純跟蹤方法的精度較低，速度和載荷變化對精度的影響較大；基于動力學模型的MPC 方法在無負載工況下精度較高，速度變化對精度影響不大，載荷變化導致的車輛模型不準確對精度影響較大；反饋純跟蹤方法精度較高，速度和載荷變化對精度影響較小。

在實際應用中，載荷變化是常見工況。若采用MPC 方法，需要準確觀測車輛模型參數才能得到較高的路徑跟隨精度，而模型參數的準確測量存在較大難度。本文方法的控制參數和模型參數較少，能夠克服速度變化和載荷變化的影響，跟隨精度和穩定性較好。

5 結論

本文中提出了一種智能車路徑跟隨方法，以純跟蹤方法為基礎，根據車速和路徑曲率動態調節前視距離，同時在轉彎過程中根據橫向偏差和車速進行轉角補償，保證不同工況下的路徑跟隨精度。試驗結果表明：動態調整前視距離和反饋補償轉角能夠克服純跟蹤方法存在的曲線路徑跟隨精度較低的問題，改善路徑跟隨效果；同時，該方法在不同車速和不同載荷工況下，保持較好的跟隨精度和行駛穩定性，具有較強魯棒性。