基于預(yù)瞄的模糊自適應(yīng)路徑跟蹤算法研究

劉長(zhǎng)偉，湯慶濤，王金橋，高 潔，白彥梅，王新樹(shù)

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

0 引 言

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

本文基于預(yù)瞄理論提出一種基于模糊控制的自適應(yīng)前視距離純追蹤模型的路徑跟蹤方法。純追蹤模型利用幾何方法模擬駕駛時(shí)情況，所用參數(shù)較少，具有一定預(yù)見(jiàn)性，但確定前視距離較困難，可通過(guò)模糊控制自適應(yīng)調(diào)整前視距離來(lái)解決，從而確定智能汽車(chē)路徑跟蹤時(shí)期望的轉(zhuǎn)向角度。

1 數(shù)學(xué)建模

設(shè)計(jì)導(dǎo)航算法時(shí)，簡(jiǎn)化車(chē)輛數(shù)學(xué)模型，得到車(chē)輛橫向運(yùn)動(dòng)學(xué)二輪簡(jiǎn)化模型[5]，如圖1 所示，將車(chē)輛左右車(chē)輪重疊為僅有前后車(chē)輪，其中車(chē)輛前輪坐標(biāo)為（xq，yq），后輪坐標(biāo)為（x，y），θ為車(chē)輛航向角，δ為車(chē)輛前輪轉(zhuǎn)角，L為車(chē)輛軸距，v為車(chē)輛行駛速度。

圖1 車(chē)輛運(yùn)動(dòng)學(xué)二輪模型

由圖1可得如下各式。

式中：x′為后輪橫坐標(biāo)對(duì)時(shí)間的導(dǎo)數(shù)；y′為后輪縱坐標(biāo)對(duì)時(shí)間的導(dǎo)數(shù)；x′q為前輪橫坐標(biāo)對(duì)時(shí)間的導(dǎo)數(shù)；y′q為前輪縱坐標(biāo)對(duì)時(shí)間的導(dǎo)數(shù)。

根據(jù)式（1）～（3）可得

即

式中：θ′為車(chē)輛航向角對(duì)時(shí)間的導(dǎo)數(shù)；t為時(shí)間。

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

由式（7）～（9）得

綜上，車(chē)輛橫向運(yùn)動(dòng)學(xué)模型為

式（11）適用于車(chē)輛低速行駛工況，并且不考慮輪胎側(cè)滑等影響，采用此模型解決車(chē)輛自動(dòng)導(dǎo)航時(shí)的路徑跟蹤問(wèn)題。

2 模糊自適應(yīng)純追蹤算法

2.1 純追蹤算法

根據(jù)幾何關(guān)系建立轉(zhuǎn)向角度與轉(zhuǎn)彎半徑間關(guān)系，在當(dāng)前位置點(diǎn)和預(yù)定路徑上的預(yù)瞄點(diǎn)間規(guī)劃一條圓弧，計(jì)算車(chē)輪的轉(zhuǎn)向角度，如圖2所示。

圖2 純追蹤模型原理

將車(chē)輛行駛簡(jiǎn)化為二維運(yùn)動(dòng)，圖2中預(yù)定路徑上的預(yù)瞄點(diǎn)坐標(biāo)為（a，b），Lq為車(chē)輛當(dāng)前位置與預(yù)瞄點(diǎn)間弧的弦長(zhǎng)，即前視距離，R為圓弧的曲率半徑，Pe為車(chē)輛與預(yù)定路徑的橫向偏差，θe為車(chē)輛當(dāng)前航向與預(yù)定路徑航向之間的角度偏差，即航向偏差。

由圖2可得如下各式。

聯(lián)立式（12）～（14）得

根據(jù)文獻(xiàn)[7]得到a的計(jì)算式為

同時(shí)由阿克曼轉(zhuǎn)向原理[8]得

式中：L為軸距，δ為前輪轉(zhuǎn)角。

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

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

2.2 模糊控制算法

目前模糊控制應(yīng)用越來(lái)越廣泛，將實(shí)踐經(jīng)驗(yàn)轉(zhuǎn)化為模糊控制規(guī)則，在模糊控制器中對(duì)控制參數(shù)進(jìn)行模糊化處理，并將處理結(jié)果輸入執(zhí)行機(jī)構(gòu)[9-10]。

模糊控制系統(tǒng)主要由模糊化、模糊推理、清晰化、規(guī)則庫(kù)、控制對(duì)象5部分組成，其中模糊化、模糊推理、清晰化、規(guī)則庫(kù)構(gòu)成模糊控制器[11]，如圖3所示。

圖3 模糊控制原理

3 路徑跟蹤仿真

本文采用一種改進(jìn)的基于模糊自適應(yīng)前視距離的智能車(chē)路徑跟蹤方法，對(duì)純追蹤模型中的前視距離進(jìn)行實(shí)時(shí)調(diào)整，根據(jù)實(shí)時(shí)不同航向偏差和橫向偏差確定前視距離和前輪轉(zhuǎn)角，如圖4所示。

圖4 模糊自適應(yīng)路徑跟蹤原理

3.1 變量模糊化

模糊控制中輸入和輸出變量的個(gè)數(shù)可以是一個(gè)也可以是多個(gè)，通常輸出變量越多則控制越復(fù)雜，控制精度也越高。本文采用二維模糊控制器，即有兩個(gè)輸入?yún)?shù)：車(chē)輛橫向偏差Pe和航向偏差θe，前視距離Lq為輸出變量。

1）橫向偏差Pe模糊化

在MATLAB 模糊控制模塊中設(shè)定Pe基本論域?yàn)閇-100，100]，單位為cm；量化等級(jí)為{-6，-5，-4，-3，-2，-1，0，1，2，3，4，5，6}，量化因子為6/100=3/50，如圖5所示。

圖5 橫向偏差Pe隸度函數(shù)

2）航向偏差θe模糊化

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

圖6 航向偏差θe隸度函數(shù)

3）前視距離Lq模糊化

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

圖7 前視距離Lq隸度函數(shù)

3.2 模糊推理規(guī)則

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

圖8 車(chē)體與預(yù)定路徑位置關(guān)系

建立的車(chē)輛模糊控制規(guī)則共有49 條，組合后見(jiàn)表1，表1 中Lq的取值由Pe、θe共同確定。模糊控制曲面如圖9 所示，當(dāng)遇到誤差較大情況時(shí)，輸出的前視距離較小，能夠盡快減小誤差，當(dāng)遇到誤差較小情況時(shí)，輸出的前視距離較大，能夠保持系統(tǒng)穩(wěn)定。

表1 模糊規(guī)則控制表

圖9 模糊控制曲面

3.3 模糊推理與清晰化

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

3.4 路徑跟蹤仿真

基于MATLAB/Simulink 搭建仿真模型，包括簡(jiǎn)化的運(yùn)動(dòng)學(xué)模型、純追蹤模型、模糊控制模型、轉(zhuǎn)向控制模型。

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

圖10 模糊自適應(yīng)純追蹤模型

3.5 仿真結(jié)果分析

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

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

由圖11 可知，當(dāng)θe、Pe為正值且偏差較大時(shí)，模糊控制器發(fā)揮作用使偏差快速縮小。進(jìn)行路徑跟蹤時(shí)，在0～1 s過(guò)程中，θe先增大后減小，Pe由正值減小為負(fù)值，從1 s 左右開(kāi)始，θe、Pe二者的絕對(duì)值均不斷減小直至趨于0，達(dá)到車(chē)輛路徑跟蹤目標(biāo)。

由圖12 可知，當(dāng)θe、Pe為負(fù)值且偏差較大時(shí)，模糊控制器發(fā)揮作用使偏差快速縮小。進(jìn)行路徑跟蹤時(shí)，在0～1 s過(guò)程中，θe先減小后增大至正值，Pe的絕對(duì)值先增大后減小，從1 s 左右開(kāi)始，θe、Pe二者的絕對(duì)值均不斷減小直至趨于0，達(dá)到車(chē)輛路徑跟蹤目標(biāo)。

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

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

圖13 路徑跟蹤仿真

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

本文對(duì)智能汽車(chē)的路徑跟蹤控制建立運(yùn)動(dòng)學(xué)模型，基于純跟蹤算法和模糊控制相結(jié)合方法，通過(guò)自適應(yīng)調(diào)節(jié)前視距離實(shí)現(xiàn)路徑跟蹤。通過(guò)MATLAB/Simulink 搭建仿真模型，以不同的橫向偏差、航向偏差作為初始條件進(jìn)行仿真，結(jié)果表明所建立的仿真模型能夠?qū)崿F(xiàn)車(chē)輛路徑跟蹤，理論上驗(yàn)證了該算法的可行性。