魯棒棒，皮大偉，謝伯元，王顯會

(南京理工大學(xué)，江蘇 南京 210094)

1 引言

作為當今汽車行業(yè)的熱門，無人駕駛車輛已廣泛應(yīng)用于安保巡邏、物流配送等方面。對此，無人駕駛車輛在較低車速下的路徑跟蹤研究顯得尤為關(guān)鍵。在低速下的路徑跟蹤研究中最為關(guān)鍵的是設(shè)計精度高、穩(wěn)定、適應(yīng)性好的路徑跟蹤控制器[1-2]。目前，研究路徑跟蹤問題的控制算法有很多，主要為純點跟蹤算法、斯坦利算法、模型預(yù)測算法[3-6]等。國內(nèi)外學(xué)者也對此做了大量研究[7-9]，這方面面臨的主要問題是縱向車速和方向的解耦降低了算法的精度和穩(wěn)定性，目前的路徑跟蹤控制算法在設(shè)計控制器的時候大多忽略縱向車速的影響，以某一恒定的經(jīng)驗來確定控制器的參數(shù)，導(dǎo)致縱向車速變化后控制精度和穩(wěn)定性都有所降低。為解決這一問題，以一輛無人物流車為研究對象，以GPS采集的離散數(shù)表形式的數(shù)據(jù)建立多路徑下的預(yù)期道路模型，通過引入GPS獲取的車輛位置、慣性導(dǎo)航系統(tǒng)、前輪轉(zhuǎn)角、車速等位姿反饋量，建立二自由度車輛模型、航向預(yù)估模型及附加航向模型，計算橫向位置偏差、航向預(yù)估量及附加航向，以航向預(yù)估量、附加航向、車輛當前航向、前輪轉(zhuǎn)角為航向反饋量，設(shè)計多路徑下基于航向預(yù)估的模糊PID控制器，并通過SIMULINK仿真和對應(yīng)的實車實對算法進行驗證。

控制架構(gòu)如圖1。本文旨在設(shè)計多路徑下適應(yīng)縱向車速的基于航向預(yù)估與模糊控制的PID控制器，安排如下：第二節(jié)著重于預(yù)期道路模型、航向預(yù)估模型、附加航向模型的建立。在第三節(jié)，設(shè)計了多路徑下適應(yīng)縱向車速的基于航向預(yù)估的模糊PID控制器。第四節(jié)通過SINULINK仿真和對應(yīng)的實車實驗對提出的路徑跟蹤算法進行驗證。最后，在第五節(jié)中給出簡短的結(jié)論。

圖1 控制架構(gòu)

2 動力學(xué)與數(shù)學(xué)模型

2.1 道路模型

本文設(shè)計的控制器建立在預(yù)先獲取的理想道路模型的前提下。無人車希望跟隨的“道路”千變?nèi)f化，錯綜復(fù)雜[10]?？紤]到本文的道路模型通過GPS采集的數(shù)據(jù)獲取，采用離散的數(shù)表形式描述道路，如圖2。根據(jù)道路曲率的大小確定該段道路離散點的數(shù)量，二者成正比，道路曲率越大，數(shù)量越多，點越密集。

圖2 預(yù)期道路的描述

2.2 二自由度車輛模型

無人駕駛車輛在進行路徑跟蹤的過程中，橫向位置偏差y隨縱向車速、橫擺角速度等車輛狀態(tài)參數(shù)改變而變化。無人駕駛車輛作為高度非線性的復(fù)雜系統(tǒng)，橫向車速、橫擺角速度等參數(shù)無法通過測量獲取。為更精確地計算由車輛當前位置與預(yù)期道路產(chǎn)生的橫向位置偏差y，引入只有側(cè)向和橫擺兩個自由度的車輛二自由度模型。

車輛二自由度模型微分方程如下：

(1)

(2)

其中：

m為無人駕駛車輛整車質(zhì)量(kg)；

Iz為車輛繞z軸的轉(zhuǎn)動慣量(kg.m2)；

kf、kr分別為前后輪胎側(cè)偏系數(shù)(N/rad)；

u、ν分別為汽車縱向、橫向速度(m/s)；

a、b分別為質(zhì)心到前后軸距離(m)；

ω為汽車橫擺角速度(rad/s)；

β為質(zhì)心側(cè)偏角(rad)；

δ為前輪轉(zhuǎn)角(rad)；

考慮車輛在轉(zhuǎn)向時轉(zhuǎn)向機構(gòu)具有一定的遲滯性，視為一階慣性環(huán)節(jié)，則有:

(3)

假設(shè)車輛在行駛過程中橫向車速v遠遠小于縱向車速u，即u?v。如圖3，有

(4)

聯(lián)立上式(1)(2)(3)(4)可獲得以橫擺角速度、橫向車速、橫向位置偏差、前輪轉(zhuǎn)角為狀態(tài)量的系統(tǒng)狀態(tài)空間表達式

(5)

2.3 附加航向模型

附加航向模型如圖3。在大地直角坐標系下，(X0，Y0)為GPS獲取的車輛在當前時刻質(zhì)心位置的坐標，(Xk+1，Yk+1)為預(yù)瞄點rk+1的坐標，ψ為大地坐標系下通過慣性導(dǎo)航系統(tǒng)獲得的車輛的當前航向，ψd為預(yù)瞄點rk+1在大地直角坐標系下無人駕駛車輛希望跟隨的期望航向。任何對無人駕駛車輛的控制需要轉(zhuǎn)換至車輛坐標系下進行，對預(yù)瞄點rk+1進行坐標轉(zhuǎn)換

(6)

無人駕駛車輛當前時刻的實際航向與預(yù)期道路下期望航向的航向差值φ為

φ=ψd-ψ

(7)

車輛質(zhì)心至預(yù)瞄點rk+1的距離l為

(8)

由于橫向位置偏差y而產(chǎn)生的附加航向φΔ為

(9)

圖3 附加航向模型

2.3 航向預(yù)估模型

航向預(yù)估模型就是基于縱向車速、前輪轉(zhuǎn)角預(yù)測出車輛的航向變化趨勢，以此為航向反饋并計入控制偏差來影響控制器輸出的前輪轉(zhuǎn)角，減小因航向偏差滯后帶來的控制效果變差的影響[11]。本文提出的航向與估算法基于簡化的車輛二自由度模型，預(yù)估模型如圖4。O為無人駕駛車輛轉(zhuǎn)向時的瞬時轉(zhuǎn)向中心，r為無人駕駛車輛轉(zhuǎn)向時繞O的轉(zhuǎn)向半徑，G為無人駕駛車輛的質(zhì)心，δ為前輪轉(zhuǎn)過的角度，a、b分別為G到車輛前軸與后軸的距離。

圖4 航向預(yù)估模型

設(shè)控制器的采樣周期為T，無人駕駛車輛的前輪車速近似等于縱向車速u，車輛在T內(nèi)的航向變化量即航向預(yù)估量Δθ近似為

Δθ=uT/r

(10)

前輪繞轉(zhuǎn)向中心O的轉(zhuǎn)向半徑r近似為

r=(a+b)/sinδ

(11)

聯(lián)立上面兩式可得

Δθ=uTsinδ/(a+b)

(12)

3 控制器設(shè)計

本文的控制策略建立在航向跟蹤的基礎(chǔ)上，在無人駕駛車輛的控制過程中，產(chǎn)生的橫向位置偏差和航向偏差全部轉(zhuǎn)化為航向偏差進行控制。為解決直道下車輛縱向車速變化時控制算法精度和安全性下降、彎道的航向偏差大、駛?cè)霃澋罆r轉(zhuǎn)向不足、駛出彎道時過度轉(zhuǎn)向等問題，考慮車速和前輪轉(zhuǎn)角和橫向位置對航向變化的影響，提出多路徑下基于航向預(yù)估的模糊PID控制算法。控制器分為兩層，上層為多路徑控制器，計算直道和彎道下對應(yīng)的航向偏差量，確保車輛在多路徑、縱向車速變化下行駛時具有良好的跟蹤性能，下層為模糊PID控制器，計算下發(fā)至車輛的期望前輪轉(zhuǎn)角，控制器架構(gòu)如圖5。

圖5 控制器架構(gòu)

3.1 多路徑控制器

設(shè)計多路徑控制器的目的是解決因路徑(直道和彎道)不同和縱向車速變化而導(dǎo)致的跟蹤算法精度和穩(wěn)定性下降的問題。為此引入路徑標志變量signal、0-1損失函數(shù)和代價變量kψ、kψd、kΔθ、kφΔ、kδ，控制器結(jié)構(gòu)如圖6。

變量signal為衡量車輛處于直道或者彎道的標志，若處于直道，signal≥0；處于彎道時，signal<0。

各代價變量為

(13)

控制器輸出量航向偏差E為

E=kψdψd-kψψ-kΔθΔθ-kφΔφΔ-kδδ

(14)

圖6 多路徑控制器結(jié)構(gòu)

3.2 模糊PID控制器

考慮到車輛縱向和橫擺運動之間的耦合關(guān)系，下層控制器器是基于模糊和PID控制理論設(shè)計的，控制框圖如圖7所示。模糊PID控制器可以計算下發(fā)至車輛的前輪期望轉(zhuǎn)角。

圖7 模糊PID控制框圖

模糊PID控制器由模糊控制和PID控制兩部分組成。模糊控制器的輸出量為PID控制器的三個參數(shù)。然后PID控制器計算下發(fā)至車輛的期望轉(zhuǎn)角。通過模糊控制對PID控制器的Kp、Ki、Kd三個參數(shù)自整定，可提高系統(tǒng)的魯棒性、穩(wěn)健性和適應(yīng)性。

上層多路徑控制器的輸出量為航向偏差E，因此模糊控制器的輸入量為航向偏差E及航向偏差變化率EC，模糊控制器的輸出為PID控制器的三個可調(diào)參數(shù)調(diào)整量ΔKp、ΔKi、ΔKd。

在模糊化過程中，輸入變量E、EC輸出模糊集合分別為NB(負大)，NM(負中)，NS(負小)，ZO(零)，PS(正小)，PM(正中)，PB(正大)，E及EC的模糊論域為[-3，3]。隸屬度函數(shù)采用較為簡單的三角函數(shù)，量化因子為1.1和2.0。ΔKp、ΔKi、ΔKd的輸出論域分別為[-3，3]，[-1，1]，[-0.2，0.2]。

將車輛行駛工況分成下述三個典型工況進行分析，以確定Kp，Ki，Kd的選取規(guī)則。

規(guī)則1：if E is PB and EC is PB，then ΔKpis NB，ΔKiis PB，ΔKdis PS

在這種工況下，車輛當前實際航向遠遠小于希望跟隨的期望航向，車輛處于嚴重轉(zhuǎn)向不足狀態(tài)。為了使車輛能夠迅速跟隨上期望航向，應(yīng)取較大的Kp，增大前輪轉(zhuǎn)角，同時為避免轉(zhuǎn)向機構(gòu)超過極限位置，應(yīng)取較小的Kd，Ki應(yīng)取為0，防止車輛實際航向超過期望航向。

規(guī)則2：if E is NS and EC is NB，then ΔKpis PM，ΔKiis NB，ΔKdis Z0

在這種工況下，車輛當前實際航向略大于希望跟隨的期望航向。為提高跟蹤精度，減小車輛在穩(wěn)定后與期望航向的差值，應(yīng)取較大的Kp，Ki，同時，為減少車輛進入穩(wěn)定狀態(tài)的調(diào)節(jié)時間，應(yīng)取較小的Kd。

規(guī)則3：if E is PM and EC is NM，then ΔKpis Z0，ΔKiis Z0，ΔKdis NS

在這種工況下，車輛當前實際航向相比于希望跟隨的期望航向小一些。為避免車輛在跟蹤過程中產(chǎn)生航向超調(diào)，應(yīng)取較小的Kp，同時，為減少車輛進入穩(wěn)定狀態(tài)的調(diào)節(jié)時間，Ki應(yīng)取適中的值，應(yīng)取較小的Kd并保持不變。由此構(gòu)成的模糊規(guī)則如表2所示。

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

PID控制器的輸入變量為航向偏差值E，輸出變量為期望轉(zhuǎn)角δd，δd為

(15)

模糊PID控制器的參數(shù)如表2。

表2 模糊PID參數(shù)

4 仿真與實車實驗

為驗證本文提出的算法的有效性，本文進行了SIMULINK仿真和實車實驗，為表現(xiàn)所提出的多路徑下基于航向預(yù)估的模糊PID控制器(以下簡稱航向預(yù)估算法)的跟蹤性能，與只以實際航向和附加航向為反饋量的增量式PID算法控制器進行比較。

4.1 仿真研究

仿真在MATLAB/SIMULINK環(huán)境下進行，實驗車輛前輪轉(zhuǎn)角的最大值為45°，為與其一致，將仿真中控制器輸出中加入最大值為0.785rad。

考慮到GPS等傳感器的采樣周期，將控制器采樣周期設(shè)為100ms，實驗車速為2-4m/s，仿真時系統(tǒng)參數(shù)如表4所示。

表3 車輛參數(shù)

以實驗場地中的一段直道加彎道的實際路徑作為仿真路徑，仿真初始位置偏差為1.1m，航向偏差為1°，直道時車速為4m/s，進入彎道時減速為2m/s，過彎后再加速至4m/s，仿真結(jié)果見圖8、圖9、圖10。

圖8 路徑跟蹤曲線

圖9 航向跟蹤曲線

圖10 橫向位置誤差曲線

圖11 縱向車速曲線

圖8表明，航向預(yù)估算法的跟蹤效果比常規(guī)PID控制算更好法。從圖9可以看出，在直道時，縱向車速由0.5增加至4m/s時，航向預(yù)估算法對應(yīng)的航向偏差變化較為平緩，在第16.5s時航向偏差趨于0，車輛進入穩(wěn)定狀態(tài)，常規(guī)PID控制算法航向變化較為劇烈，在第23.3s時航向偏差趨于0，調(diào)整時間較長；在彎道時，航向預(yù)估算法航向跟蹤效果比較理想，始終與理想航向保持一定的偏差，而常規(guī)控制算法在第47-52s內(nèi)時航向超調(diào)，在第62s時，駛出彎道重新駛?cè)胫钡?，車輛出現(xiàn)航向超調(diào)現(xiàn)象。圖10表明，當縱向車速變化時，航向預(yù)估算法的橫向位置誤差較小，保持在0.1m以內(nèi)，常規(guī)控制算法出現(xiàn)較為劇烈的震蕩現(xiàn)象；綜合比較圖8、圖9、圖10和圖11，航向預(yù)估算法的控制效果、穩(wěn)定性以及對車速的適應(yīng)性優(yōu)于常規(guī)算法。

4.2 實車實驗

實驗平臺為無人駕駛車輛，車輛位置由差分GPS系統(tǒng)給出，車輛航向由慣性導(dǎo)航系統(tǒng)給出，實驗路徑由一段直道加彎道和過彎后的一段直道組成，路徑初始位置偏差為0.5m，航向偏差為0°，直道時車速為4m/s，進入彎道時減速為2m/s，過彎后再加速至4m/s，實驗結(jié)果見圖12、圖13、圖14。

圖12 路徑跟蹤曲線

圖13 橫向位置偏差曲線

圖14 縱向車速曲線

從圖12可以看出，航向預(yù)估算法能夠較好地跟隨上路徑，而常規(guī)控制算法出現(xiàn)明顯的震蕩，控制的路徑沿理想路徑左右搖擺；圖13表明，航向預(yù)估算法的橫向位置偏差超調(diào)量較小，最大為0.1m，車輛進入穩(wěn)定狀態(tài)的調(diào)節(jié)時間為13s，在駛?cè)牒婉偝鰪澋罆r橫向位置偏差保持在0.1m以內(nèi)，而常規(guī)PID控制算法的橫向位置偏差最大達到了0.2m，車輛進入穩(wěn)定狀態(tài)的調(diào)節(jié)時間為18s，且出現(xiàn)明顯的震蕩，相比于常規(guī)控制算法，橫向位置偏差減少了0.1m，調(diào)節(jié)時間縮短了27.7%。

5 結(jié)論

針對低速下無人車的路徑跟蹤問題，設(shè)計了多路徑下適應(yīng)縱向車速的航向預(yù)估模糊PID控制器。通過引入航向預(yù)估量、前輪轉(zhuǎn)角、附加航向、實際航向等反饋量，解決多路徑下、縱向車速變化等因素下引起的跟蹤精度與穩(wěn)定性下降的問題。在未增加計算復(fù)雜性的前提下，提出的算法與常規(guī)算法相比，提高了路徑跟蹤的精度(橫向位置偏差減少至0.1m以內(nèi))，算法的穩(wěn)定有所提升(調(diào)節(jié)時間減少27.7%)。

提出的算法只在低速下對縱向車速等變化因素只具有一定的自適應(yīng)能力，應(yīng)用于全車速下的自適應(yīng)路徑跟蹤算法仍待研究。