基于車位長度的平行泊車仿真平臺設(shè)計

高宏巖，亓志同

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

0 引言

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

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

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

1.1 泊車起始位置約束條件

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

圖1 泊車起始位置分析

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

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

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

車身側(cè)邊的轉(zhuǎn)彎半徑為

則Mj橫坐標(biāo)為

My橫坐標(biāo)與直線段Q2K2方程有關(guān)，設(shè)直線方程為

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

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

車輛從泊車起始位置可行區(qū)域M0點進(jìn)行庫外泊車路徑規(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)進(jìn)行多次短距離的前進(jìn)、后退往復(fù)式調(diào)整，直至滿足要求。因車位空間狹窄，為盡量減少換擋次數(shù)，車輛以最小轉(zhuǎn)彎半徑R在橫軸方向上移動最大安全距離，以保證每次調(diào)整的傾斜角度達(dá)到最大。

（1）前進(jìn)調(diào)整車身過程分析。前進(jìn)調(diào)整過程如圖3（a）所示，掛前進(jìn)擋并向右轉(zhuǎn)動方向盤直至車輛B點與車位邊緣A1B1距離達(dá)安全距離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 θ。當(dāng)c2＞a2+b2時，方程無解，表明車身傾斜角度在未到達(dá)安全距離時已經(jīng)滿足要求，這種情況下車身最大調(diào)整角度θ1_max＝θ。以O(shè)3為轉(zhuǎn)向中心，R為轉(zhuǎn)彎半徑，轉(zhuǎn)過圓心角度為θ1_max的圓弧路徑即可。

（2）后退調(diào)整車身過程分析。后退調(diào)整過程如圖3（b）所示，掛倒擋并向左轉(zhuǎn)動方向盤直至車輛D點與車位邊緣D1C1距離達(dá)安全距離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。當(dāng)＞a2+時，方程無解，表明車身傾斜角度在未到達(dá)安全距離時已經(jīng)滿足要求，這種情況下車身最大調(diào)整角度θ2_max＝θ1。以O(shè)4為轉(zhuǎn)向中心，R為轉(zhuǎn)彎半徑，轉(zhuǎn)過圓心角度為θ2_max的圓弧路徑即可。

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

2 路徑跟蹤控制

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

2.1 預(yù)測模型建立

設(shè)x和y分別是車輛后軸中點的橫、縱坐標(biāo)，δ 為航向角，φ為等效前輪轉(zhuǎn)向角，v為后軸中心速度，則低速泊車時車輛運動學(xué)模型為［4］

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

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

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

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

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

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

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

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

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

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

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

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

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

3 仿真平臺設(shè)計

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

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

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

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

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

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

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

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

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

圖4 GUI界面設(shè)置

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

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

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

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

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

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

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

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

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

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

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

4 結(jié)語

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