線控四輪轉向汽車平行泊車路徑規劃與跟蹤控制

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

（1.吉林大學 汽車仿真與控制國家重點實驗室，吉林 長春 130022；2.中國第一汽車集團有限公司智能網聯研發院，吉林長春 130011）

汽車運動學非完整約束使得經驗不足的駕駛員在狹窄環境內難以完成泊車任務，而自動泊車系統是解決此問題的有效手段之一.在精確感知外界環境的前提下，泊車路徑規劃與跟蹤控制方法是決定自動泊車系統性能的核心要素[1-2].因此，深入研究泊車路徑規劃與跟蹤控制方法對于解決汽車運動學非完整約束導致的泊車難問題具有重要的意義.

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

泊車路徑跟蹤控制方法主要有模型預測跟蹤控制方法[10]、非線性魯棒跟蹤控制方法[11]和滑模跟蹤控制方法[5，12-13].文獻[10]將受控自回歸積分滑動平均模型作為預測模型，采用模型預測控制方法設計泊車路徑跟蹤控制策略.為了抑制外界干擾和名義模型不確定性對泊車路徑跟蹤精度的影響，文獻[11]采用非線性魯棒控制方法設計泊車路徑跟蹤控制策略.文獻[5]建立包含外界干擾的汽車運動學名義模型，并采用滑模自抗擾控制方法設計泊車路徑跟蹤控制策略.文獻[12]建立非時間參考泊車路徑跟蹤控制模型，并基于此設計了泊車路徑滑模跟蹤控制策略.文獻[13]采用反正切函數代替泊車路徑滑模跟蹤控制策略中符號函數來抑制傳統滑模固有的抖振問題.上述泊車路徑跟蹤控制方法的被控對象均是前輪轉向汽車，無法直接應用于線控四輪轉向汽車.

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

1 平行泊車路徑規劃

1.1 平行泊車路徑規劃問題描述

線控四輪轉向汽車平行泊車過程中處于低速大轉角行駛狀態，可認為四個車輪無側偏的繞同一瞬時圓心做圓周運動.如圖1 所示，將線控四輪轉向汽車后軸中點作為參考點，建立其平行泊車過程中的運動學方程為：

圖1 線控四輪轉向汽車運動學模型Fig.1 The wire-four-wheel steering vehicle kinematic model

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

定義系統狀態向量和控制向量為：

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

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

式中：計算步長h 與系數K1、K2、K3和K4可分別表示為

式中：tf和N 分別為平行泊車過程總時長和離散點數量（k）=（u（k）+u（k+1））/2.

考慮線控四輪轉向汽車動力總成子系統和線控四輪轉向子系統的過程約束和邊界約束，可得：

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

基于汽車后軸中點坐標和方位角可得如圖1 所示的汽車輪廓四個頂點A、B、C 和D 的坐標為：

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

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

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

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

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

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

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

式中：Lp為平行泊車位長度.

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

將式（5）、式（8）和式（14）描述的等式約束條件記為c（x，u）=0，將式（7）、式（10）～式（13）描述的不等式約束條件記為g（x，u）≤0，則平行泊車路徑規劃問題轉化為以平行泊車過程總時長為目標的約束最優化問題，使線控四輪轉向汽車在最短時間內完成平行泊車任務.

由式（6）和式（15）可知，給定離散點數量N，最小化平行泊車過程總時長tf約束最優化問題可等價為最小化計算步長h 約束最優化問題.

1.2 平行泊車路徑規劃問題求解

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

式中：s 為粒子群優化算法的當前迭代次數.

利用第i 個粒子的歷史最優解pbest

i（s）和群體的歷史最優解pbest（s）更新第i 個粒子的速度為：

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

式中：qmax和qmin分別為慣性部分權重的最大值和最小值；smax為總迭代次數.通過自適應調整慣性部分權重系數，使粒子群優化算法在前期有較高的全局搜索能力，在后期有較高的局部搜索能力.

利用第i 個粒子在迭代計算次數s+1 的速度更新其位置為：

利用最優化問題的約束條件計算第i 個粒子的適應度函數為：

式中：Ji（s）為第i 個粒子在迭代計算次數s 的優化目標值；J*（s）為第i 個粒子的歷史最差優化目標值；Vi（s）表示為

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

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

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

圖3 改進的粒子群優化算法Fig.3 Improved particle swarm optimization algorithm

2 平行泊車路徑跟蹤控制

本節基于前饋控制和反饋控制相結合的方式設計平行泊車路徑跟蹤控制策略.其中，前饋控制環節用于提高平行泊車路徑跟蹤控制策略的響應速度和精度，而反饋控制用于抑制外界擾動對平行泊車路徑跟蹤控制性能的影響.

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

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

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

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

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

由汽車前輪轉向角最大值與后輪轉向角最大值的比例關系，可得汽車后輪轉向角控制量為：

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

3 仿真分析

本節利用車輛動力學仿真軟件建立線控四輪轉向汽車平行泊車路徑規劃與跟蹤控制方法的模型在環仿真系統，對其可行性和有效性進行驗證.以天津一汽夏利N5 整車參數為基礎設計線控四輪轉向汽車的仿真參數為L=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；依據工程經驗，仿真驗證過程中粒子群優化算法的基礎參數分別設置為d1=1.2，d2=1.5，qmin=0.3，qmax=1.5，smax=1 000 和εj=ξj=10-3，種群規模設置為30、粒子速度向量邊界區域設置為[-1，1]、粒子位置向量中表征汽車后軸中點加速度信息的分量的邊界區域設置為[-armax，armax]，粒子位置向量中表征汽車前輪轉向角速度信息的分量的邊界區域設置為[-ωfmax，ωfmax]，粒子位置向量中表征汽車后輪轉向角速度信息的分量的邊界區域設置為[-ωrmax，ωrmax]，算法迭代收斂判斷容差設置為10-4；仿真結果如圖5所示.

如圖5（a）（b）所示，采用基于改進粒子群優化算法的平行泊車路徑規劃方法得到的平行泊車路徑可以安全、舒適地引導線控四輪轉向汽車進入平行泊車位，并且目標平行泊車路徑曲率的連續性有效避免了線控四輪轉向汽車原地轉向現象.

圖5 仿真結果Fig.5 Simulation results

如圖5（c）～（f）所示，采用基于前饋控制和反饋控制相結合方式設計的平行泊車路徑跟蹤控制策略可以使線控四輪轉向汽車快速、精確和穩定地跟蹤目標泊車路徑，進而安全無碰撞地完成平行泊車任務.

4 結論

本文提出了一種線控四輪轉向汽車平行泊車路徑規劃與跟蹤控制方法.綜合考慮了線控四輪轉向汽車運動學非完整約束、動力和轉向子系統約束、避障約束和泊車位姿約束，建立了平行泊車路徑規劃約束最優化問題，并采用可以改進的粒子群優化算法對其進行求解來獲得最優平行泊車路徑.采用前饋控制和PI 反饋控制相結合的方式設計了平行泊車路徑跟蹤控制策略，實現對規劃的平行泊車路徑快速、精確及穩定的跟蹤控制.利用車輛動力學仿真軟件構建模型在環系統，驗證所提出的線控四輪轉向汽車平行泊車路徑規劃與跟蹤控制方法的可行性和有效性，結果表明：所提出的方法可以快速、精確、穩定地引導線控四輪轉向汽車自動完成平行泊車任務.