多路徑下航向預(yù)估的無(wú)人車路徑跟蹤算法研究

魯棒棒，皮大偉，謝伯元，王顯會(huì)

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

1 引言

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

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

圖1 控制架構(gòu)

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

2.1 道路模型

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

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

2.2 二自由度車輛模型

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

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

(1)

(2)

其中：

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

Iz為車輛繞z軸的轉(zhuǎn)動(dòng)慣量(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)向時(shí)轉(zhuǎn)向機(jī)構(gòu)具有一定的遲滯性，視為一階慣性環(huán)節(jié)，則有:

(3)

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

(4)

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

(5)

2.3 附加航向模型

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

(6)

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

φ=ψd-ψ

(7)

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

(8)

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

(9)

圖3 附加航向模型

2.3 航向預(yù)估模型

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

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

設(shè)控制器的采樣周期為T，無(wú)人駕駛車輛的前輪車速近似等于縱向車速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è)計(jì)

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

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

3.1 多路徑控制器

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

變量signal為衡量車輛處于直道或者彎道的標(biāo)志，若處于直道，signal≥0；處于彎道時(shí)，signal<0。

各代價(jià)變量為

(13)

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

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

(14)

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

3.2 模糊PID控制器

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

圖7 模糊PID控制框圖

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

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

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

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

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

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

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

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

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

在這種工況下，車輛當(dāng)前實(shí)際航向相比于希望跟隨的期望航向小一些。為避免車輛在跟蹤過(guò)程中產(chǎn)生航向超調(diào)，應(yīng)取較小的Kp，同時(shí)，為減少車輛進(jìn)入穩(wěn)定狀態(tài)的調(diào)節(jié)時(shí)間，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 仿真與實(shí)車實(shí)驗(yàn)

為驗(yàn)證本文提出的算法的有效性，本文進(jìn)行了SIMULINK仿真和實(shí)車實(shí)驗(yàn)，為表現(xiàn)所提出的多路徑下基于航向預(yù)估的模糊PID控制器(以下簡(jiǎn)稱航向預(yù)估算法)的跟蹤性能，與只以實(shí)際航向和附加航向?yàn)榉答伭康脑隽渴絇ID算法控制器進(jìn)行比較。

4.1 仿真研究

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

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

表3 車輛參數(shù)

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

圖8 路徑跟蹤曲線

圖9 航向跟蹤曲線

圖10 橫向位置誤差曲線

圖11 縱向車速曲線

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

4.2 實(shí)車實(shí)驗(yàn)

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

圖12 路徑跟蹤曲線

圖13 橫向位置偏差曲線

圖14 縱向車速曲線

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

5 結(jié)論

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

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