高宏巖，亓志同

（山東科技大學電氣與自動化工程學院，山東青島 266590）

0 引言

自動泊車仿真技術是開發(fā)自動泊車系統(tǒng)的核心技術之一，是提高自動泊車系統(tǒng)可靠性、安全性和降低開發(fā)成本的重要環(huán)節(jié)［1］。泊車路徑規(guī)劃和跟蹤控制是自動泊車系統(tǒng)中的兩個重要組成環(huán)節(jié)。本文針對狹窄車位情況，提出庫外泊車與庫內(nèi)調(diào)整結合的平行泊車路徑規(guī)劃方法，設計路徑跟蹤模型預測控制算法。采用Matlab構建路徑規(guī)劃仿真模型，通過GUI界面實現(xiàn)泊車過程的動態(tài)仿真。采用Carsim 軟件構建車輛模型，利用Matlab 編寫路徑跟蹤控制算法程序，通過設計二者接口搭建自動泊車仿真平臺。學生可在此平臺開展仿真實驗研究，在GUI 界面設置車輛和車位參數(shù)，對泊車路徑規(guī)劃算法在不同參數(shù)下的泊車效果進行分析，亦可自行設計跟蹤控制算法，利用仿真平臺開展分析研究。

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

在滿足泊車起始位置約束條件、車輛最小轉彎半徑、車輛自身轉向約束和泊車空間避障約束條件下［2］，采用庫外泊車與庫內(nèi)調(diào)整結合的方法進行平行泊車路徑規(guī)劃。

1.1 泊車起始位置約束條件

（1）車身傾斜角θ 的確定。若車位長度足夠長，經(jīng)過一次倒車入庫后車身調(diào)正或車身傾斜角度θ在允許角度范圍內(nèi)（本文設為3°），則泊車過程結束。若車位長度狹窄，經(jīng)過一次倒車入庫后車身傾斜角度θ ＞3°，則需要進行庫內(nèi)調(diào)整。在滿足避障約束條件前提下，利用迭代搜索方法尋找盡可能小的θ值，以減少庫內(nèi)調(diào)整次數(shù)。采用出庫倒推方法分析避障約束條件，如圖1 所示。車輛在庫內(nèi)時，后軸中心點M（x，y）的坐標為

圖1 泊車起始位置分析

在出庫過程中，為避免車輛B點與車位A1點發(fā)生碰撞，則避障約束條件為

式中，Rrf為車輛右前角轉彎半徑，由車輛自身參數(shù)決定?？紤]到車輛轉向受自身轉向角約束限制，令傾斜角θ在0°～30°之間依次迭代搜索，直至搜索到滿足式（1）的θ最小值。

（2）Mj和My的確定。由圖1 可知，Mj和My的縱坐標均為

車身側邊的轉彎半徑為

則Mj橫坐標為

My橫坐標與直線段Q2K2方程有關，設直線方程為

利用Matlab中Solve函數(shù)求解k和b的值，由此求得最遠起始點My橫坐標為

1.2 庫外泊車路徑規(guī)劃

車輛從泊車起始位置可行區(qū)域M0點進行庫外泊車路徑規(guī)劃，如圖2 所示。

圖2 庫外泊車路徑規(guī)劃

1.3 庫內(nèi)調(diào)整過程分析

庫內(nèi)調(diào)整過程如圖3 所示。

圖3 庫內(nèi)調(diào)整過程

庫內(nèi)調(diào)整是指經(jīng)一次倒車入庫后車身傾斜角度θ不在允許車身傾斜的角度范圍內(nèi)，車輛需在車位內(nèi)進行多次短距離的前進、后退往復式調(diào)整，直至滿足要求。因車位空間狹窄，為盡量減少換擋次數(shù)，車輛以最小轉彎半徑R在橫軸方向上移動最大安全距離，以保證每次調(diào)整的傾斜角度達到最大。

（1）前進調(diào)整車身過程分析。前進調(diào)整過程如圖3（a）所示，掛前進擋并向右轉動方向盤直至車輛B點與車位邊緣A1B1距離達安全距離df時停車，車輛狀態(tài)由（x，y，θ）調(diào)整為，在保證安全性條件下，求解車身最大調(diào)整角度θ1_max＝θ -θ1。由圖3（a）分析求解：

式中：a＝Vw／2 -R；b＝Vl-Er；c＝Sl-dr-df-sin θ·（Vw／2 +R）-Er·cos θ。當c2＞a2+b2時，方程無解，表明車身傾斜角度在未到達安全距離時已經(jīng)滿足要求，這種情況下車身最大調(diào)整角度θ1_max＝θ。以O3為轉向中心，R為轉彎半徑，轉過圓心角度為θ1_max的圓弧路徑即可。

（2）后退調(diào)整車身過程分析。后退調(diào)整過程如圖3（b）所示，掛倒擋并向左轉動方向盤直至車輛D點與車位邊緣D1C1距離達安全距離dr時停車。后退過程中，由前一階段的車輛狀態(tài)調(diào)整至，求解后退過程最大調(diào)整角度θ2_max＝θ1-θ2。由圖3（b）分析求解：

式中，cr＝Sl-dr-df-sin θ1·（Vw／2 +R）-（Vl-Er）·cos θ1。當＞a2+時，方程無解，表明車身傾斜角度在未到達安全距離時已經(jīng)滿足要求，這種情況下車身最大調(diào)整角度θ2_max＝θ1。以O4為轉向中心，R為轉彎半徑，轉過圓心角度為θ2_max的圓弧路徑即可。

若后退調(diào)整后，車身傾斜角未在允許范圍內(nèi)，則以后退車身狀態(tài)為參照，重復前進-后退往復式運動，直至傾斜角滿足要求，完成泊車操作。

2 路徑跟蹤控制

路徑跟蹤控制采用模型預測控制算法，它利用當前狀態(tài)變量和控制量預測未來輸出量［3］，并通過求解帶約束條件的目標優(yōu)化函數(shù)得到控制序列。

2.1 預測模型建立

設x和y分別是車輛后軸中點的橫、縱坐標，δ 為航向角，φ為等效前輪轉向角，v為后軸中心速度，則低速泊車時車輛運動學模型為［4］

在參考路徑點（xr，yr，δr）將車輛運動學模型進行線性化和離散化處理得到車輛線性誤差模型［5］：

式中：A為5 ×5 的矩陣；B 為5 ×2 的矩陣；C 為3 ×5的矩陣。設模型預測的控制時域為Nc，預測時域為Np，經(jīng)過遞推計算得到預測輸出［7］：

式中：Φ為3Np×5 的矩陣；Θ為3Np×2（Nc+1）的矩陣。

目標函數(shù)采用二次型形式［8］：

式中：Q和R為權重矩陣；σ 為松弛因子；ω 為σ 的權重系數(shù)。為使車輛平穩(wěn)行駛，需要對車速和前輪轉角及其增量設定約束條件［9］，在約束條件下利用Matlab 對目標函數(shù)進行求解得到控制時域內(nèi)的控制增量序列［10］，將此序列的第一個值與前一時刻控制量、當前時刻參考控制量相加得到當前時刻控制量作用于系統(tǒng)。

2.2 粒子群優(yōu)化時域參數(shù)

粒子群優(yōu)化時域參數(shù)步驟：

步驟1初始化種群規(guī)模、隨機位置、速度等參數(shù)。

步驟2隨機賦值給Np、Nc并取整。

步驟3計算適應度函數(shù)J，更新局部及全局最優(yōu)值。

步驟4更新各個粒子的位置和速度。

步驟5滿足終止條件，輸出最優(yōu)參數(shù)組合。

本設計將Np范圍限定在（0，60），Nc范圍限定在（0，40），迭代次數(shù)為20，經(jīng)粒子群優(yōu)化后輸出最優(yōu)時域參數(shù)組合為（38，7）。

3 仿真平臺設計

3.1 路徑規(guī)劃仿真平臺設計

在Matlab 中編寫程序實現(xiàn)上述泊車路徑規(guī)劃算法，程序編寫具體步驟：

步驟1令n＝0，已知車輛、車位、安全距離等環(huán)境參數(shù)的條件下，給定車輛起始位置的縱向距離Ld，由Ld＜l1+R-Rrf-Vw／2 -Sw判斷是否滿足條件，若否，則重新給定參數(shù)。

步驟2迭代車身傾斜角θ，直至搜索到滿足式（1）的θ值，否則算法結束，泊車失敗。

步驟3確定Ld、θ 值后，計算泊車最近、最遠起始位置Mj、My。

步驟4在（xMj，xMy）中給定起始點，由式（2）（3）確定庫外泊車路徑，若θ≤3°，則泊車結束；否則繼續(xù)下一步。

步驟5令n＝n+1，若n＜4，根據(jù)上一時刻車輛狀態(tài)及式（4），求解車身最大調(diào)整角度θ1_max，進行前進路徑調(diào)整。若調(diào)整后車身傾斜角≤3°，則泊車結束；否則繼續(xù)下一步。

步驟6令n＝n+1，若n＜4，根據(jù)上一時刻車輛狀態(tài)及公式（5），求解車身最大調(diào)整角度θ2_max，進行后退路徑調(diào)整。若調(diào)整后車身傾斜角≤3°，則泊車結束；否則返回步驟5。

步驟7若n＝4，庫內(nèi)調(diào)整次數(shù)超過最大限制，則泊車失敗。在Matlab中設計GUI可視化界面輸入各項參數(shù)，如圖4 所示。實驗時，可以根據(jù)具體車型和泊車環(huán)境輸入車輛參數(shù)和車位參數(shù)，本實驗采用馬自達CX-30為實驗車型。

圖4 GUI界面設置

設計圖5 所示場景進行路徑規(guī)劃仿真分析：固定車輛參數(shù)不變，假設車輛距離車位邊緣縱向距離Ld為0.6 m，車位寬度為2.5 m，依次改變車位長度為6.7、6、5.6、5.5 m，觀察路徑變化，如圖5 所示。

圖5 泊車路徑規(guī)劃示意圖

當車位長度為6.7 m時，無需庫內(nèi)調(diào)整即可安全泊車；縮小車位長度為6 m，庫外泊車后車身傾斜角為12.7°，經(jīng)過一次前進庫內(nèi)調(diào)整便可調(diào)正車身；繼續(xù)縮短車位長度到5.6 m，庫外泊車后車身傾斜角增大到19.4°，前進調(diào)整后傾斜角為8°，還需要經(jīng)過一次后退調(diào)整才能調(diào)正車身；將車位長度縮短到5.5 m時，入庫傾斜角增大到21.8°，需要經(jīng)過3 次庫內(nèi)調(diào)整?？梢?，隨著車位長度縮短，入庫后傾斜角會逐漸增大，庫內(nèi)調(diào)整次數(shù)增加。

3.2 路徑跟蹤控制聯(lián)合仿真平臺設計

路徑跟蹤控制聯(lián)合仿真平臺采用Simulink 與Carsim進行搭建，采用Carsim 構建車輛模型［11］，采用Matlab并封裝成為Simulink中F_Dmpc模塊。

（1）聯(lián)合仿真平臺搭建。仿真車輛及工況設置：構建車輛模型時需要在Carsim 中根據(jù)不同型號車輛的技術手冊設置相關參數(shù)。本實驗根據(jù)馬自達CX-30的技術手冊在Carsim 中配置車輛參數(shù)?？紤]泊車是低速倒車過程，因此將目標車速設為5 km／h，檔位選擇為倒擋模式，利用命令行語句OPT_INIT_PATH 0、SV_XO、SV_YO、SV_YAW 設置以車輛前軸中心為參考點的起始狀態(tài)。

接口設計：為將Carsim中設置好的車輛模型導入到Matlab 軟件中，需要先在Carsim 中設置相應的輸入、輸出變量。輸入變量為質(zhì)心車速、左前輪轉角、右前輪轉角、左后輪轉角和右后輪轉角。輸出變量為左后輪x坐標值、右后輪x坐標值、左后輪y坐標值、右后輪y坐標值、車輛航向角、車輛質(zhì)心縱向車速和方向盤轉角。設置聯(lián)合仿真步長為0.001 s，點擊“send to simulink”將車輛模型導入到Simulink 模塊庫中［12］，形成可以直接調(diào)用的vs-sf 模塊。在Simulink 調(diào)用車輛模型vs-sf模塊和跟蹤控制算法F_Dmpc 模塊，將二者對接并在F_Dmpc模塊中導入外部的泊車規(guī)劃路徑信息，由此搭建完成如圖6 所示的聯(lián)合仿真平臺。

圖6 Simulink／Carsim聯(lián)合仿真平臺

（2）路徑跟蹤仿真實驗結果。將上述規(guī)劃好的路徑點生成mat文件導入給控制算法模塊F_Dmpc，模型預測跟蹤控制算法根據(jù)期望位置信息和車輛模型反饋位置信息計算質(zhì)心車速和前輪轉角并輸出給Carsim的車輛模型，通過前輪轉角和質(zhì)心速度來控制車輛運動，循環(huán)更新直至完成泊車路徑跟蹤控制。針對狹窄車位情況開展仿真，設置車位長為6 m、寬為2.5 m，Ld為0.6 m，泊車速度為5 km／h，速度控制增量范圍為（-0.4 m／s，0.4 m／s），設置前輪轉角轉向范圍為（-35°，35°），轉角增量范圍為（-23°，23°），泊車路徑跟蹤控制仿真結果如圖7（a）～（d）所示。

圖7 泊車路徑跟蹤控制示意

由仿真結果分析可知：庫外泊車行駛過程中最大橫向誤差為4 cm，最大航向角誤差為2°，經(jīng)過庫外泊車后車身實際傾斜角為13.5°，接近路徑規(guī)劃中的參考傾斜角；庫內(nèi)調(diào)整過程中最大橫向誤差為3.1 cm，當車輛接近目標停車位置時橫向誤差縮小到1 cm，最終車身傾斜角為1.1°，在允許范圍之內(nèi)，可成功泊車。仿真結果表明，Simulink／Carsim 聯(lián)合仿真平臺能夠驗證模型預測跟蹤控制的效果。

4 結語

庫外泊車與庫內(nèi)調(diào)整結合的平行泊車路徑規(guī)劃方法為狹窄車位情況下泊車提供了新的思路。采用粒子群優(yōu)化模型預測控制的時域參數(shù)，避免人為設定參數(shù)的局限性，提高路徑跟蹤控制準確性。本文設計的自動泊車仿真平臺，可為自動化、車輛工程等相關專業(yè)實踐教學提供教學服務。自動泊車仿真平臺可模擬實車在不同工況下的泊車情況，具有較強的靈活性和實用性，學生可借助此仿真平臺研究不同環(huán)境參數(shù)下的泊車路徑規(guī)劃算法和路徑跟蹤控制算法。