基于預瞄的模糊自適應路徑跟蹤算法研究

劉長偉，湯慶濤，王金橋，高 潔，白彥梅，王新樹

（奇瑞新能源汽車股份有限公司，安徽 蕪湖 241000）

0 引 言

路徑跟蹤是智能汽車控制的關鍵技術，目前主要方法有PID （Proportional-Integral-Derivative，比例、積分和微分）控制、模糊控制、神經網絡控制、MPC（Model Predictive Control，模型預測控制）以及傳感器信息融合算法等。江浩斌[1]根據不同車速和道路曲率，利用遺傳算法優化得到一種所有典型彎道工況下時域參數自適應的MPC 路徑跟蹤控制器。楊琛[2]基于遺傳算法優化PID 控制器提出一種路徑跟蹤控制方案，建立狀態空間型無人艇運動數學模型以及隨機干擾數學模型，在傳統LOS （Line-of-Sight，視線制導）算法中引入積分項，實時補償航向漂角干擾，以減小橫向誤差。梁藝瀟[3]利用汽車動力學模型求解自適應率，通過車輛-路徑聯合模型，基于LQR（Linear Quadratic Regulator，線性二次型調節器）求解各頂點子模型處的反饋控制律，提出一種基于多模型自適應方法的智能汽車路徑跟蹤控制。張炳力[4]利用神經網絡自學習和自調整特性，結合模糊控制，設計一種軌跡跟蹤控制器。

本文基于預瞄理論提出一種基于模糊控制的自適應前視距離純追蹤模型的路徑跟蹤方法。純追蹤模型利用幾何方法模擬駕駛時情況，所用參數較少，具有一定預見性，但確定前視距離較困難，可通過模糊控制自適應調整前視距離來解決，從而確定智能汽車路徑跟蹤時期望的轉向角度。

1 數學建模

設計導航算法時，簡化車輛數學模型，得到車輛橫向運動學二輪簡化模型[5]，如圖1 所示，將車輛左右車輪重疊為僅有前后車輪，其中車輛前輪坐標為（xq，yq），后輪坐標為（x，y），θ為車輛航向角，δ為車輛前輪轉角，L為車輛軸距，v為車輛行駛速度。

圖1 車輛運動學二輪模型

由圖1可得如下各式。

式中：x′為后輪橫坐標對時間的導數；y′為后輪縱坐標對時間的導數；x′q為前輪橫坐標對時間的導數；y′q為前輪縱坐標對時間的導數。

根據式（1）～（3）可得

即

式中：θ′為車輛航向角對時間的導數；t為時間。

由于車輛后輪受到非完整性限制[6]，則式（4）滿足x′= cosθ，y′= sinθ或者成倍數關系，所以當車輛以速度v行駛時，x′和y′可表示為

由式（7）～（9）得

綜上，車輛橫向運動學模型為

式（11）適用于車輛低速行駛工況，并且不考慮輪胎側滑等影響，采用此模型解決車輛自動導航時的路徑跟蹤問題。

2 模糊自適應純追蹤算法

2.1 純追蹤算法

根據幾何關系建立轉向角度與轉彎半徑間關系，在當前位置點和預定路徑上的預瞄點間規劃一條圓弧，計算車輪的轉向角度，如圖2所示。

圖2 純追蹤模型原理

將車輛行駛簡化為二維運動，圖2中預定路徑上的預瞄點坐標為（a，b），Lq為車輛當前位置與預瞄點間弧的弦長，即前視距離，R為圓弧的曲率半徑，Pe為車輛與預定路徑的橫向偏差，θe為車輛當前航向與預定路徑航向之間的角度偏差，即航向偏差。

由圖2可得如下各式。

聯立式（12）～（14）得

根據文獻[7]得到a的計算式為

同時由阿克曼轉向原理[8]得

式中：L為軸距，δ為前輪轉角。

將式（16）代入式（15）、（17）得

由式（18）可知，為確定前輪轉角δ需確定前視距離Lq，Lq決定跟蹤精度。如果Lq較大，則跟蹤行駛的路徑曲率較小，跟蹤軌跡較平滑，但控制響應時間較長；如果Lq較小，則跟蹤行駛的路徑曲率較大，雖然此時控制響應時間很短，但會產生較大振蕩。有學者將Lq視為關于速度的函數，相關參數包括車速、最小轉彎半徑、最大制動加速度等，通過計算得到固定的Lq，但由此得到的不同路徑下的跟蹤效果差別很大。車輛行駛中，當橫向偏差和航向偏差較大時，前視距離Lq較小，此時可迅速減小誤差；相反，如果橫向偏差和航向偏差較小，則Lq較大。

2.2 模糊控制算法

目前模糊控制應用越來越廣泛，將實踐經驗轉化為模糊控制規則，在模糊控制器中對控制參數進行模糊化處理，并將處理結果輸入執行機構[9-10]。

模糊控制系統主要由模糊化、模糊推理、清晰化、規則庫、控制對象5部分組成，其中模糊化、模糊推理、清晰化、規則庫構成模糊控制器[11]，如圖3所示。

圖3 模糊控制原理

3 路徑跟蹤仿真

本文采用一種改進的基于模糊自適應前視距離的智能車路徑跟蹤方法，對純追蹤模型中的前視距離進行實時調整，根據實時不同航向偏差和橫向偏差確定前視距離和前輪轉角，如圖4所示。

圖4 模糊自適應路徑跟蹤原理

3.1 變量模糊化

模糊控制中輸入和輸出變量的個數可以是一個也可以是多個，通常輸出變量越多則控制越復雜，控制精度也越高。本文采用二維模糊控制器，即有兩個輸入參數：車輛橫向偏差Pe和航向偏差θe，前視距離Lq為輸出變量。

1）橫向偏差Pe模糊化

在MATLAB 模糊控制模塊中設定Pe基本論域為[-100，100]，單位為cm；量化等級為{-6，-5，-4，-3，-2，-1，0，1，2，3，4，5，6}，量化因子為6/100=3/50，如圖5所示。

圖5 橫向偏差Pe隸度函數

2）航向偏差θe模糊化

在MATLAB 模糊控制模塊中設定θe基本論域為[-90，90]，單位為 °；量化等級為{-6，-5，-4，-3，-2，-1，0，1，2，3，4，5，6}，量化因子為6/90=1/15，如圖6所示。

圖6 航向偏差θe隸度函數

3）前視距離Lq模糊化

在MATLAB 模糊控制模塊中設定Lq基本論域為[0，800]，單位為cm；量化等級為{-6，-5，-4，-3，-2，-1，0，1，2，3，4，5，6}，量化因子為6/800=3/400，如圖7所示。

圖7 前視距離Lq隸度函數

3.2 模糊推理規則

對于輸入、輸出變量采用定性詞語“大、中、小”進行描述，并且由于每個變量均會出現正、負和零，則采用負大、負中、負小、零、正小、正中、正大對應描述，即模糊集合為{NB，NM，NS，ZO，PS，PM，PB}。模糊控制的關鍵是建立模糊規則，主要考慮在保證響應的基礎上能夠穩定，避免振蕩，處理好穩定與振蕩的關系。建立模糊規則的基本原則是：遇到橫向偏差較大情況，以減小偏差為主；遇到橫向偏差較小情況，注意防止超調，保持系統穩定。建立模糊規則前對橫向偏差Pe和航向偏差θe的正負作出約定：Pe在車輛前進方向偏向右側則為正，偏向左側則為負；當前航向與預定路徑的θe逆時針則為正，順時針則為負。車輛的不同位置如圖8所示，其中虛線為預定路徑。

圖8 車體與預定路徑位置關系

建立的車輛模糊控制規則共有49 條，組合后見表1，表1 中Lq的取值由Pe、θe共同確定。模糊控制曲面如圖9 所示，當遇到誤差較大情況時，輸出的前視距離較小，能夠盡快減小誤差，當遇到誤差較小情況時，輸出的前視距離較大，能夠保持系統穩定。

表1 模糊規則控制表

圖9 模糊控制曲面

3.3 模糊推理與清晰化

模糊推理是根據模糊控制規則進行模糊語言解釋的過程，將Pe、θe輸入模糊控制器，得到合適的Lq模糊語言。清晰化是指將輸出變量的模糊語言轉化為清晰值，常用方法包括重心法、最大隸度法、系數加權平均法、中位數法等。考慮重心法可以包含Pe、θe的所有信息，所以本文采用重心法進行清晰化處理。

3.4 路徑跟蹤仿真

基于MATLAB/Simulink 搭建仿真模型，包括簡化的運動學模型、純追蹤模型、模糊控制模型、轉向控制模型。

將得到的Lq作為輸入條件，得到模糊自適應純追蹤模型，如圖10所示。

圖10 模糊自適應純追蹤模型

3.5 仿真結果分析

針對圖10 仿真模型設定初始條件，θe1=10 °、Pe1=15 cm、θe2=-10 °、Pe2=-15 cm、v=1 m/s、L=0.6 m，仿真結果如圖11、12所示。

圖11 仿真結果（θe=10 °，Pe=15 cm）

由圖11 可知，當θe、Pe為正值且偏差較大時，模糊控制器發揮作用使偏差快速縮小。進行路徑跟蹤時，在0～1 s過程中，θe先增大后減小，Pe由正值減小為負值，從1 s 左右開始，θe、Pe二者的絕對值均不斷減小直至趨于0，達到車輛路徑跟蹤目標。

由圖12 可知，當θe、Pe為負值且偏差較大時，模糊控制器發揮作用使偏差快速縮小。進行路徑跟蹤時，在0～1 s過程中，θe先減小后增大至正值，Pe的絕對值先增大后減小，從1 s 左右開始，θe、Pe二者的絕對值均不斷減小直至趨于0，達到車輛路徑跟蹤目標。

圖12 仿真結果（θe=-10 °，Pe=15 cm）

設置仿真模型的初始條件：v=1 m/s、初始位置為（6，0）、θe=45 °、Pe=1 m，預定路徑為直線X= 5 m，仿真結果如圖13 所示，初始位置和預定路徑的橫向偏差為1 m，仿真結果中路徑跟蹤的最大橫向偏差為0.055 m，誤差為5.5%，根據經驗判斷，此誤差可以接受，車輛最終沿著指定路徑行走，驗證了算法的可行性。

圖13 路徑跟蹤仿真

4 結束語

本文對智能汽車的路徑跟蹤控制建立運動學模型，基于純跟蹤算法和模糊控制相結合方法，通過自適應調節前視距離實現路徑跟蹤。通過MATLAB/Simulink 搭建仿真模型，以不同的橫向偏差、航向偏差作為初始條件進行仿真，結果表明所建立的仿真模型能夠實現車輛路徑跟蹤，理論上驗證了該算法的可行性。