線控四輪轉(zhuǎn)向汽車平行泊車路徑規(guī)劃與跟蹤控制

張家旭，卜純研，王晨，趙健?

（1.吉林大學(xué) 汽車仿真與控制國(guó)家重點(diǎn)實(shí)驗(yàn)室，吉林 長(zhǎng)春 130022；2.中國(guó)第一汽車集團(tuán)有限公司智能網(wǎng)聯(lián)研發(fā)院，吉林長(zhǎng)春 130011）

汽車運(yùn)動(dòng)學(xué)非完整約束使得經(jīng)驗(yàn)不足的駕駛員在狹窄環(huán)境內(nèi)難以完成泊車任務(wù)，而自動(dòng)泊車系統(tǒng)是解決此問題的有效手段之一.在精確感知外界環(huán)境的前提下，泊車路徑規(guī)劃與跟蹤控制方法是決定自動(dòng)泊車系統(tǒng)性能的核心要素[1-2].因此，深入研究泊車路徑規(guī)劃與跟蹤控制方法對(duì)于解決汽車運(yùn)動(dòng)學(xué)非完整約束導(dǎo)致的泊車難問題具有重要的意義.

泊車路徑規(guī)劃方法主要有幾何規(guī)劃方法[3-5]、智能規(guī)劃方法[6-7]和優(yōu)化規(guī)劃方法[8-9].文獻(xiàn)[3]通過圓弧、直線和回旋曲線的平移與旋轉(zhuǎn)變換規(guī)劃出曲率連續(xù)的泊車路徑.文獻(xiàn)[4]通過參數(shù)化Gompertz 曲線獲得可執(zhí)行的泊車路徑.文獻(xiàn)[5]采用圓弧-直線-β 樣條曲線組合的方式規(guī)劃出曲率連續(xù)的泊車路徑.采用幾何規(guī)劃方法得到的泊車路徑適用場(chǎng)景受限于初始泊車位姿、泊車位和不規(guī)則障礙物信息等，可行的泊車起始區(qū)域較小.文獻(xiàn)[6]利用熟練駕駛員的泊車數(shù)據(jù)訓(xùn)練神經(jīng)網(wǎng)絡(luò)，使其可以模擬熟練駕駛員完成泊車任務(wù).文獻(xiàn)[7]利用自適應(yīng)神經(jīng)模糊推理系統(tǒng)學(xué)習(xí)熟練駕駛員的泊車操作，進(jìn)而自動(dòng)完成泊車任務(wù).采用智能規(guī)劃方法得到的泊車路徑適用場(chǎng)景受限于訓(xùn)練樣本的規(guī)模和多樣性，泛化能力較弱.文獻(xiàn)[8]將泊車路徑規(guī)劃問題轉(zhuǎn)化為約束最優(yōu)化問題，并利用高斯偽譜法求解得到最優(yōu)泊車路徑.為了兼顧計(jì)算效率和求解精度，文獻(xiàn)[9]采用自適應(yīng)偽譜法求解泊車路徑規(guī)劃問題來(lái)獲得最優(yōu)泊車路徑.采用優(yōu)化規(guī)劃方法得到的泊車路徑適用場(chǎng)景最廣，但優(yōu)化求解方法的較大計(jì)算量限制了其工程應(yīng)用.

泊車路徑跟蹤控制方法主要有模型預(yù)測(cè)跟蹤控制方法[10]、非線性魯棒跟蹤控制方法[11]和滑模跟蹤控制方法[5，12-13].文獻(xiàn)[10]將受控自回歸積分滑動(dòng)平均模型作為預(yù)測(cè)模型，采用模型預(yù)測(cè)控制方法設(shè)計(jì)泊車路徑跟蹤控制策略.為了抑制外界干擾和名義模型不確定性對(duì)泊車路徑跟蹤精度的影響，文獻(xiàn)[11]采用非線性魯棒控制方法設(shè)計(jì)泊車路徑跟蹤控制策略.文獻(xiàn)[5]建立包含外界干擾的汽車運(yùn)動(dòng)學(xué)名義模型，并采用滑模自抗擾控制方法設(shè)計(jì)泊車路徑跟蹤控制策略.文獻(xiàn)[12]建立非時(shí)間參考泊車路徑跟蹤控制模型，并基于此設(shè)計(jì)了泊車路徑滑模跟蹤控制策略.文獻(xiàn)[13]采用反正切函數(shù)代替泊車路徑滑模跟蹤控制策略中符號(hào)函數(shù)來(lái)抑制傳統(tǒng)滑模固有的抖振問題.上述泊車路徑跟蹤控制方法的被控對(duì)象均是前輪轉(zhuǎn)向汽車，無(wú)法直接應(yīng)用于線控四輪轉(zhuǎn)向汽車.

線控四輪轉(zhuǎn)向汽車具有更好的機(jī)動(dòng)靈活性，在狹窄環(huán)境內(nèi)更容易實(shí)現(xiàn)泊車操作[14].本文針對(duì)線控四輪轉(zhuǎn)向汽車平行泊車場(chǎng)景的路徑規(guī)劃與跟蹤控制問題，提出一種基于改進(jìn)的粒子群優(yōu)化算法的平行泊車路徑規(guī)劃方法，以及一種基于前饋控制和反饋控制的平行泊車路徑跟蹤控制策略.首先，綜合考慮線控四輪轉(zhuǎn)向汽車運(yùn)動(dòng)學(xué)非完整約束、動(dòng)力總成子系統(tǒng)和轉(zhuǎn)向子系統(tǒng)的過程約束和邊界約束、直線交點(diǎn)法描述的避障約束、泊車的初始位姿和目標(biāo)位姿約束，建立以最小化平行泊車過程總時(shí)長(zhǎng)為目標(biāo)的平行泊車路徑規(guī)劃約束最優(yōu)化問題，并采用可以有效處理等式約束和不等式約束的粒子群優(yōu)化算法對(duì)其進(jìn)行求解，進(jìn)而得到最優(yōu)平行泊車路徑.隨后，利用平行泊車路徑規(guī)劃過程得到的前輪轉(zhuǎn)向角和后輪轉(zhuǎn)向角作為前饋控制量，并利用汽車實(shí)際位姿與期望位姿的偏差構(gòu)建PI 反饋控制量，實(shí)現(xiàn)對(duì)規(guī)劃的平行泊車路徑快速、精確和穩(wěn)定的跟蹤控制.最后，利用車輛動(dòng)力學(xué)仿真軟件構(gòu)建模型在環(huán)仿真系統(tǒng)，驗(yàn)證所提出的線控四輪轉(zhuǎn)向汽車平行泊車路徑規(guī)劃方法和路徑跟蹤控制策略的可行性和有效性.

1 平行泊車路徑規(guī)劃

1.1 平行泊車路徑規(guī)劃問題描述

線控四輪轉(zhuǎn)向汽車平行泊車過程中處于低速大轉(zhuǎn)角行駛狀態(tài)，可認(rèn)為四個(gè)車輪無(wú)側(cè)偏的繞同一瞬時(shí)圓心做圓周運(yùn)動(dòng).如圖1 所示，將線控四輪轉(zhuǎn)向汽車后軸中點(diǎn)作為參考點(diǎn)，建立其平行泊車過程中的運(yùn)動(dòng)學(xué)方程為：

圖1 線控四輪轉(zhuǎn)向汽車運(yùn)動(dòng)學(xué)模型Fig.1 The wire-four-wheel steering vehicle kinematic model

式中：xr、yr、vr和ar分別為線控四輪轉(zhuǎn)向汽車后軸中點(diǎn)橫坐標(biāo)、縱坐標(biāo)、速度和加速度；δf和δr分別為線控四輪轉(zhuǎn)向汽車前輪和后輪轉(zhuǎn)向角；ωf和ωr分別為線控四輪轉(zhuǎn)向汽車前輪和后輪轉(zhuǎn)向角速度；φ 和L分別為線控四輪轉(zhuǎn)向汽車方位角和軸距.

定義系統(tǒng)狀態(tài)向量和控制向量為：

利用式（2）（3）將式（1）重寫為：

利用四階龍格庫(kù)塔積分法將式（4）離散化為[15]：

式中：計(jì)算步長(zhǎng)h 與系數(shù)K1、K2、K3和K4可分別表示為

式中：tf和N 分別為平行泊車過程總時(shí)長(zhǎng)和離散點(diǎn)數(shù)量（k）=（u（k）+u（k+1））/2.

考慮線控四輪轉(zhuǎn)向汽車動(dòng)力總成子系統(tǒng)和線控四輪轉(zhuǎn)向子系統(tǒng)的過程約束和邊界約束，可得：

式中：vrmax和armax分別為線控四輪轉(zhuǎn)向汽車后軸中點(diǎn)速度和加速度最大值；δfmax和δrmax分別為線控四輪轉(zhuǎn)向汽車前輪和后輪轉(zhuǎn)向角最大值；ωfmax和ωrmax分別為線控四輪轉(zhuǎn)向汽車前輪和后輪轉(zhuǎn)向角速度最大值.

基于汽車后軸中點(diǎn)坐標(biāo)和方位角可得如圖1 所示的汽車輪廓四個(gè)頂點(diǎn)A、B、C 和D 的坐標(biāo)為：

式中：Lf、Lr和W 分別為汽車前懸、后懸和寬度.

汽車平行泊車過程中其輪廓邊緣AB 與如圖2 所示的泊車位前方障礙物無(wú)碰撞的約束條件可表示為：

汽車平行泊車過程中其輪廓邊緣BC 與如圖2 所示的泊車位前方障礙物無(wú)碰撞的約束條件可表示為：

汽車平行泊車過程中其輪廓邊緣BC 與如圖2 所示的泊車位側(cè)方路基無(wú)碰撞的約束條件可表示為：

式中：Wp為平行泊車位寬度.

汽車平行泊車過程中其輪廓邊緣CD 與如圖2 所示的泊車位后方障礙物無(wú)碰撞的約束條件可表示為：

圖2 平行泊車位Fig.2 Parallel parking slot

式中：Lp為平行泊車位長(zhǎng)度.

已知平行泊車的初始位姿和目標(biāo)位姿分別為（x0，y0，φ0）和（xf，yf，φf(shuō)），則有

將式（5）、式（8）和式（14）描述的等式約束條件記為c（x，u）=0，將式（7）、式（10）～式（13）描述的不等式約束條件記為g（x，u）≤0，則平行泊車路徑規(guī)劃問題轉(zhuǎn)化為以平行泊車過程總時(shí)長(zhǎng)為目標(biāo)的約束最優(yōu)化問題，使線控四輪轉(zhuǎn)向汽車在最短時(shí)間內(nèi)完成平行泊車任務(wù).

由式（6）和式（15）可知，給定離散點(diǎn)數(shù)量N，最小化平行泊車過程總時(shí)長(zhǎng)tf約束最優(yōu)化問題可等價(jià)為最小化計(jì)算步長(zhǎng)h 約束最優(yōu)化問題.

1.2 平行泊車路徑規(guī)劃問題求解

粒子群優(yōu)化算法是一種基于啟發(fā)式信息的隨機(jī)搜索算法，具有收斂速度快和設(shè)置參數(shù)少等優(yōu)點(diǎn)，特別適用于無(wú)約束最優(yōu)化問題求解[16].本節(jié)利用最優(yōu)化問題的約束條件改進(jìn)粒子群優(yōu)化算法的適應(yīng)度函數(shù)，使改進(jìn)的粒子群優(yōu)化算法可以求解式（15）描述的平行泊車路徑規(guī)劃約束最優(yōu)化問題.已知系統(tǒng)控制向量u（k），1≤k≤N 包含三個(gè)分量，則表征系統(tǒng)控制向量的粒子位置向量和速度向量包含3N 個(gè)分量.若按照第一個(gè)系統(tǒng)控制向量三個(gè)分量至最后一個(gè)系統(tǒng)控制向量三個(gè)分量先后方式排序，則第i 個(gè)粒子的位置向量和速度向量可分別表示為：

式中：s 為粒子群優(yōu)化算法的當(dāng)前迭代次數(shù).

利用第i 個(gè)粒子的歷史最優(yōu)解pbest

i（s）和群體的歷史最優(yōu)解pbest（s）更新第i 個(gè)粒子的速度為：

式中：r1，r2∈[0，1]為均勻分布的隨機(jī)數(shù)；d1和d2分別為粒子速度自身部分和社會(huì)部分權(quán)重系數(shù)，二者的有機(jī)組合可以使粒子群優(yōu)化算法保持收斂速度和搜索效果的均衡；q（s）為控制粒子歷史速度對(duì)當(dāng)前速度影響的慣性部分權(quán)重系數(shù)，可表示為：

式中：qmax和qmin分別為慣性部分權(quán)重的最大值和最小值；smax為總迭代次數(shù).通過自適應(yīng)調(diào)整慣性部分權(quán)重系數(shù)，使粒子群優(yōu)化算法在前期有較高的全局搜索能力，在后期有較高的局部搜索能力.

利用第i 個(gè)粒子在迭代計(jì)算次數(shù)s+1 的速度更新其位置為：

利用最優(yōu)化問題的約束條件計(jì)算第i 個(gè)粒子的適應(yīng)度函數(shù)為：

式中：Ji（s）為第i 個(gè)粒子在迭代計(jì)算次數(shù)s 的優(yōu)化目標(biāo)值；J*（s）為第i 個(gè)粒子的歷史最差優(yōu)化目標(biāo)值；Vi（s）表示為

式中：Nc和Ng分別為最優(yōu)化問題等式約束條件數(shù)量和不等式約束條件數(shù)量；μij（s）和ηij（s）表示為

式中：εj和ξj分別為最優(yōu)化問題等式約束條件允許的容差和不等式約束條件允許的容差.

綜上所述，可得如圖3 所示的基于改進(jìn)的粒子群優(yōu)化算法求解平行泊車路徑規(guī)劃約束最優(yōu)化問題的具體流程.

圖3 改進(jìn)的粒子群優(yōu)化算法Fig.3 Improved particle swarm optimization algorithm

2 平行泊車路徑跟蹤控制

本節(jié)基于前饋控制和反饋控制相結(jié)合的方式設(shè)計(jì)平行泊車路徑跟蹤控制策略.其中，前饋控制環(huán)節(jié)用于提高平行泊車路徑跟蹤控制策略的響應(yīng)速度和精度，而反饋控制用于抑制外界擾動(dòng)對(duì)平行泊車路徑跟蹤控制性能的影響.

如圖4 所示，首先利用汽車實(shí)際位置與期望位置的偏差eD（k）、汽車實(shí)際方位角與期望方位角的偏差eφ（k）計(jì)算前輪轉(zhuǎn)向角反饋控制量為：

圖4 平行泊車路徑跟蹤控制Fig.4 Parallel parking path tracking control

式中：KPD和KID分別為汽車實(shí)際位置與期望位置偏差的比例系數(shù)和積分系數(shù)；KPφ和KIφ分別為汽車實(shí)際方位角與期望方位角偏差的比例系數(shù)和積分系數(shù)；eD（k）和eφ（k）可表示為：

式中：（xr（k），yr（k），φ（k））和（xrd（k），yrd（k），φd（k））分別為汽車實(shí)際位姿和期望位姿.

若平行泊車路徑規(guī)劃過程得到的汽車前輪轉(zhuǎn)向角前饋控制量為δfd（k+1），則汽車前輪轉(zhuǎn)向角控制量可表示為：

由汽車前輪轉(zhuǎn)向角最大值與后輪轉(zhuǎn)向角最大值的比例關(guān)系，可得汽車后輪轉(zhuǎn)向角控制量為：

式中：δrd（k+1）為汽車后輪轉(zhuǎn)向角前饋控制量；Ks=δrmax/δfmax為比例系數(shù).

3 仿真分析

本節(jié)利用車輛動(dòng)力學(xué)仿真軟件建立線控四輪轉(zhuǎn)向汽車平行泊車路徑規(guī)劃與跟蹤控制方法的模型在環(huán)仿真系統(tǒng)，對(duì)其可行性和有效性進(jìn)行驗(yàn)證.以天津一汽夏利N5 整車參數(shù)為基礎(chǔ)設(shè)計(jì)線控四輪轉(zhuǎn)向汽車的仿真參數(shù)為L(zhǎng)=4.155 m，W=1.645 m，Lf=0.8 m，Lr=0.95 m，vrmax=1 m·s-1，armax=2 m·s-2，δfmax=0.524 rad，δrmax=0.087 3 rad，ωfmax=ωrmax=0.524 rad·s-1；依據(jù)工程經(jīng)驗(yàn)，仿真驗(yàn)證過程中粒子群優(yōu)化算法的基礎(chǔ)參數(shù)分別設(shè)置為d1=1.2，d2=1.5，qmin=0.3，qmax=1.5，smax=1 000 和εj=ξj=10-3，種群規(guī)模設(shè)置為30、粒子速度向量邊界區(qū)域設(shè)置為[-1，1]、粒子位置向量中表征汽車后軸中點(diǎn)加速度信息的分量的邊界區(qū)域設(shè)置為[-armax，armax]，粒子位置向量中表征汽車前輪轉(zhuǎn)向角速度信息的分量的邊界區(qū)域設(shè)置為[-ωfmax，ωfmax]，粒子位置向量中表征汽車后輪轉(zhuǎn)向角速度信息的分量的邊界區(qū)域設(shè)置為[-ωrmax，ωrmax]，算法迭代收斂判斷容差設(shè)置為10-4；仿真結(jié)果如圖5所示.

如圖5（a）（b）所示，采用基于改進(jìn)粒子群優(yōu)化算法的平行泊車路徑規(guī)劃方法得到的平行泊車路徑可以安全、舒適地引導(dǎo)線控四輪轉(zhuǎn)向汽車進(jìn)入平行泊車位，并且目標(biāo)平行泊車路徑曲率的連續(xù)性有效避免了線控四輪轉(zhuǎn)向汽車原地轉(zhuǎn)向現(xiàn)象.

圖5 仿真結(jié)果Fig.5 Simulation results

如圖5（c）～（f）所示，采用基于前饋控制和反饋控制相結(jié)合方式設(shè)計(jì)的平行泊車路徑跟蹤控制策略可以使線控四輪轉(zhuǎn)向汽車快速、精確和穩(wěn)定地跟蹤目標(biāo)泊車路徑，進(jìn)而安全無(wú)碰撞地完成平行泊車任務(wù).

4 結(jié)論

本文提出了一種線控四輪轉(zhuǎn)向汽車平行泊車路徑規(guī)劃與跟蹤控制方法.綜合考慮了線控四輪轉(zhuǎn)向汽車運(yùn)動(dòng)學(xué)非完整約束、動(dòng)力和轉(zhuǎn)向子系統(tǒng)約束、避障約束和泊車位姿約束，建立了平行泊車路徑規(guī)劃約束最優(yōu)化問題，并采用可以改進(jìn)的粒子群優(yōu)化算法對(duì)其進(jìn)行求解來(lái)獲得最優(yōu)平行泊車路徑.采用前饋控制和PI 反饋控制相結(jié)合的方式設(shè)計(jì)了平行泊車路徑跟蹤控制策略，實(shí)現(xiàn)對(duì)規(guī)劃的平行泊車路徑快速、精確及穩(wěn)定的跟蹤控制.利用車輛動(dòng)力學(xué)仿真軟件構(gòu)建模型在環(huán)系統(tǒng)，驗(yàn)證所提出的線控四輪轉(zhuǎn)向汽車平行泊車路徑規(guī)劃與跟蹤控制方法的可行性和有效性，結(jié)果表明：所提出的方法可以快速、精確、穩(wěn)定地引導(dǎo)線控四輪轉(zhuǎn)向汽車自動(dòng)完成平行泊車任務(wù).