一種基于軌跡預(yù)判的垂直泊車路徑規(guī)劃算法研究

熊斌　鐘益林　彭邦煌　白軍明

【摘? 要】為解決目前量產(chǎn)自動(dòng)泊車系統(tǒng)泊車成功率低、泊車完成后車輛姿態(tài)偏斜等問(wèn)題，研究搭建基于阿克曼轉(zhuǎn)向幾何學(xué)和車輛運(yùn)動(dòng)學(xué)的車輛運(yùn)動(dòng)模型，構(gòu)建基于可行駛區(qū)域的柵格電子地圖，在幾何路徑規(guī)劃方法中融入基于軌跡預(yù)判的碰撞約束方法和路徑居中算法，采用車輛膨脹輪廓模型，最大程度利用電子地圖可行駛區(qū)域和保證路徑安全性，設(shè)計(jì)出一種基于軌跡預(yù)判的垂直泊車路徑規(guī)劃算法，并通過(guò)仿真測(cè)試和實(shí)車測(cè)試驗(yàn)證算法的可行性。該算法可大大提高泊車成功率，能幫助解決泊車完成后車輛偏斜不居中的問(wèn)題。

【關(guān)鍵詞】自動(dòng)泊車；路徑規(guī)劃；幾何法；軌跡預(yù)判

中圖分類號(hào)：U469.72? ? 文獻(xiàn)標(biāo)志碼：A? ? 文章編號(hào)：1003-8639（ 2023 ）09-0019-07

Research on Vertical Parking Path Planning Algorithm Based on Trajectory Prediction

XIONG Bin，ZHONG Yilin，PENG Banghuang，BAI Junming

（BYD Automotive Engineering Research Institute，Shenzhen 518118，China）

【Abstract】In order to solve the problems of the current mass-produced automatic parking system，such as the low parking success rate and the vehicle attitude skew after parking，a vehicle motion model based on Ackermann steering geometry and vehicle kinematics was built，and a network based on the drivable area was constructed. The grid electronic map integrates the collision constraint method based on trajectory prediction and the path centering algorithm into the geometric path planning method，and adopts the vehicle expansion contour model to maximize the use of the drivable area of the electronic map and ensure the safety of the path. Predictive vertical parking path planning algorithm. Simulation test and real vehicle test verify the feasibility of the algorithm，the algorithm greatly improves the success rate of parking，and solves the problem of vehicle deflection and non-centering after parking is completed.

【Key words】automatic parking；path planning；geometric method；trajectory prediction

作者簡(jiǎn)介

熊斌（1989—），主任控制算法工程師，主要從事新能源汽車整車智能控制算法開(kāi)發(fā)工作；彭邦煌，男，軟件測(cè)試開(kāi)發(fā)部經(jīng)理，研究方向?yàn)槠囁惴ㄑ邪l(fā)。

隨著電子元器件和現(xiàn)代控制理論方法快速發(fā)展，汽車自動(dòng)化和智能化水平不斷提升，汽車不僅僅是代步工具，用戶對(duì)車輛安全性、自動(dòng)化和科技感等要求也越來(lái)越高，各車企為提升市場(chǎng)競(jìng)爭(zhēng)力，不斷增加投入來(lái)提升汽車的智能自動(dòng)化技術(shù)。

在國(guó)內(nèi)，隨著汽車數(shù)量不斷增加，分配到每輛汽車的泊車空間越來(lái)越小，狹小的泊車位十分考驗(yàn)駕駛員的駕駛技術(shù)，泊車入庫(kù)造成的交通事故在所有交通事故中的占比越來(lái)越大，因此自動(dòng)泊車技術(shù)成為國(guó)內(nèi)外學(xué)者重點(diǎn)研究對(duì)象。大眾汽車在1992年首次在其IRVW概念車上搭載了自動(dòng)泊車技術(shù)，但由于成本過(guò)高未量產(chǎn)；隨著硬件成本降低，豐田汽車在2003年在普瑞斯車型上基于攝像頭感知車位實(shí)現(xiàn)自動(dòng)泊車，并實(shí)現(xiàn)量產(chǎn)；雪鐵龍汽車在2007年畢加索車型上，通過(guò)6個(gè)測(cè)距傳感器來(lái)感知車位實(shí)現(xiàn)自動(dòng)泊車；2013年，奧迪汽車在CES上展示了代客泊車系統(tǒng)，即通過(guò)手機(jī)一鍵操作，車輛可以自主尋找車位并完成自動(dòng)泊車；近十幾年，一些國(guó)內(nèi)外供應(yīng)商如法雷奧、德賽西威、縱目科技、百度等紛紛推出自己的自動(dòng)泊車系統(tǒng)，一些自主汽車品牌如長(zhǎng)安、小鵬、吉利等都研制出自動(dòng)泊車系統(tǒng)。

目前量產(chǎn)自動(dòng)泊車系統(tǒng)存在許多問(wèn)題，比如車位識(shí)別率較低、泊車成功率不高、揉庫(kù)次數(shù)過(guò)多、泊車時(shí)間過(guò)長(zhǎng)、泊車過(guò)程發(fā)生剮蹭、泊車軌跡不連續(xù)、泊車完成后車輛偏斜不居中等。路徑規(guī)劃模塊是自動(dòng)泊車系統(tǒng)的一部分，良好的路徑規(guī)劃算法不但能保證泊車路徑軌跡安全性、可行性和連續(xù)性，還可以縮短泊車時(shí)間和實(shí)現(xiàn)泊車完成后車輛居中，因此研究路徑規(guī)劃算法具有重要意義。

1? 自動(dòng)泊車系統(tǒng)組成及工作原理

1.1? 自動(dòng)泊車系統(tǒng)介紹

自動(dòng)泊車系統(tǒng)構(gòu)成示意如圖1所示，其系統(tǒng)主要由軟件系統(tǒng)和硬件系統(tǒng)組成。其中，硬件系統(tǒng)包括環(huán)境感知傳感器、自動(dòng)泊車控制器、轉(zhuǎn)向制動(dòng)擋位執(zhí)行器等；軟件系統(tǒng)包括底層操作系統(tǒng)、環(huán)境感知模塊、路徑規(guī)劃模塊、車輛控制模塊、泊車主程序、人機(jī)交互界面等。

自動(dòng)泊車控制器是感知算法、路徑規(guī)劃算法和車輛控制算法等軟件的運(yùn)行載體，并將一些自動(dòng)泊車信息傳遞給人機(jī)交互界面進(jìn)行顯示；環(huán)境感知傳感器包括超聲波雷達(dá)、毫米波雷達(dá)、攝像頭、IMU等傳感器，負(fù)責(zé)采集周圍環(huán)境信息以構(gòu)建自動(dòng)泊車電子地圖；執(zhí)行器由電動(dòng)轉(zhuǎn)向系統(tǒng)、電液制動(dòng)系統(tǒng)和擋位系統(tǒng)等組成，負(fù)責(zé)執(zhí)行車輛控制模塊的指令信息來(lái)進(jìn)行轉(zhuǎn)向、制動(dòng)或擋位切換等操作。

環(huán)境感知模塊通過(guò)感知定位算法可以獲取車輛位姿信息和識(shí)別車位、障礙物等目標(biāo)；路徑規(guī)劃模塊根據(jù)感知模塊提供的環(huán)境信息，規(guī)劃出一條車輛可行駛、無(wú)碰撞的路徑軌跡；車輛控制模塊根據(jù)路徑規(guī)劃模塊給出的路徑軌跡，控制轉(zhuǎn)向系統(tǒng)、制動(dòng)系統(tǒng)和擋位系統(tǒng)等執(zhí)行機(jī)構(gòu)，控制車輛沿著規(guī)劃的路徑軌跡運(yùn)動(dòng)；泊車主程序用于控制自動(dòng)泊車過(guò)程狀態(tài)切換等，泊車過(guò)程狀態(tài)包括待機(jī)、搜車位、泊車準(zhǔn)備就緒、泊車中、泊車暫停、泊車完成等；人機(jī)交互界面主要用于駕駛員操作和信息顯示作用，駕駛員操作包括自動(dòng)泊車開(kāi)啟關(guān)閉、泊入車位選擇、車頭車尾泊入選擇等，信息顯示包括車輛位姿、車位信息、障礙物信息、路徑軌跡信息等顯示。

1.2? 自動(dòng)泊車工作原理

自動(dòng)泊車類型（圖2）按照車輛停放位置主要分為水平泊車、垂直泊車和斜列泊車3種方式。水平泊車方式車輛停放位置與道路基準(zhǔn)線平行，垂直泊車方式車輛停放位置與道路基準(zhǔn)線垂直，斜列泊車方式車輛停放位置與道路基準(zhǔn)線呈非水平垂直的一定角度。

文章針對(duì)垂直泊車路徑規(guī)劃算法進(jìn)行研究，以垂直泊車方式為例，闡述自動(dòng)泊車工作原理，如圖3、圖4所示。泊車系統(tǒng)激活后，開(kāi)始搜車位，感知模塊檢測(cè)到車位有效后，語(yǔ)音提示“找到車位，請(qǐng)踩剎車”，駕駛員踩下制動(dòng)踏板后在人機(jī)交互界面選擇車位，感知模塊將車位信息、障礙物信息和車輛位姿信息等環(huán)境信息發(fā)送給路徑規(guī)劃模塊，路徑規(guī)劃模塊根據(jù)接收到的環(huán)境信息規(guī)劃出泊車的路徑軌跡（圖3中的垂直泊車路徑軌跡由直線M0M1、圓弧M1M2、圓弧M2M3、圓弧M3M4和直線M4M5組成）發(fā)送給車輛控制模塊，車輛控制模塊根據(jù)路徑軌跡計(jì)算出方向盤轉(zhuǎn)角、速度和擋位等決策信息來(lái)控制車輛執(zhí)行機(jī)構(gòu)，駕駛員松開(kāi)制動(dòng)踏板后車輛按照路徑軌跡行走到達(dá)泊車終點(diǎn)后完成泊車。

2? 垂直泊車路徑規(guī)劃算法設(shè)計(jì)

2.1? 車輛運(yùn)動(dòng)模型

要規(guī)劃泊車路徑軌跡，首先要建立車輛運(yùn)動(dòng)模型，得到車輪轉(zhuǎn)向角和車輛路徑軌跡點(diǎn)坐標(biāo)、航向角的數(shù)學(xué)關(guān)系，故本文在阿克曼轉(zhuǎn)向幾何學(xué)基礎(chǔ)上建立了車輛二自由度模型。

2.1.1? 阿克曼轉(zhuǎn)向模型

2.1.2? 車輛運(yùn)動(dòng)學(xué)模型

本文研究對(duì)象為前輪轉(zhuǎn)向汽車，泊車過(guò)程車速較低（一般在3km/h以下），因此車輛可以近似為一個(gè)剛體，假設(shè)車輛只做純滾動(dòng)運(yùn)動(dòng)，不發(fā)生側(cè)滑，由此可確定如圖6所示的車輛二自由度模型，即車輛運(yùn)動(dòng)學(xué)模型。

2.2? 電子地圖

進(jìn)行自動(dòng)泊車路徑規(guī)劃的前提是需要知道待泊入車位信息、車輛位姿信息和障礙物信息等環(huán)境信息，環(huán)境感知模塊將這些環(huán)境信息需要按一定的格式輸出至路徑規(guī)劃模塊，車輛位姿信息和車位信息通過(guò)建立電子地圖坐標(biāo)系可以得到，障礙物信息（可行駛區(qū)域）通過(guò)網(wǎng)格劃分和網(wǎng)格是否被占用來(lái)判定來(lái)得到。

2.2.1? 坐標(biāo)系建立

要構(gòu)建電子地圖，首先要建立參考坐標(biāo)系，如圖7所示，以車輛泊車開(kāi)始時(shí)的后軸中心點(diǎn)O為坐標(biāo)原點(diǎn)，車輛直線前進(jìn)方向?yàn)閄軸正方向，采用右手法則沿著X軸正方向逆時(shí)針旋轉(zhuǎn)90°得到Y(jié)軸正方向，建立了電子地圖坐標(biāo)系，從而可以確定待泊入車位4個(gè)角點(diǎn)P0、P1、P2和P3坐標(biāo)，車位4個(gè)角點(diǎn)坐標(biāo)由攝像頭感知得到，圖中點(diǎn)S0為泊車過(guò)程中任意位置的車輛后軸中心點(diǎn)，點(diǎn)S0的坐標(biāo)和航向角θ由IMU感知得到，航向角θ的定義為電子地圖坐標(biāo)系X軸正方向沿著逆時(shí)針?lè)较蛐D(zhuǎn)到車輛直線前進(jìn)方向的角度，范圍為0～360°。

2.2.2? 網(wǎng)格劃分

環(huán)境感知模塊輸出自動(dòng)泊車電子地圖可行駛區(qū)域格式一般通過(guò)劃分網(wǎng)格占用情況進(jìn)行輸出，考慮到控制器算力、路徑規(guī)劃算法需求響應(yīng)時(shí)間、環(huán)境感知模塊輸出范圍及精度，電子地圖選取25m×25m范圍進(jìn)行輸出，網(wǎng)格大小為10cm×10cm。

電子地圖網(wǎng)格劃分示意如圖8所示，以泊車開(kāi)始時(shí)的車輛后軸中心點(diǎn)作為坐標(biāo)原點(diǎn)構(gòu)建坐標(biāo)系，建立25m×25m區(qū)域范圍的電子地圖，電子地圖X坐標(biāo)和Y坐標(biāo)的范圍均為-12.5～12.5m，將地圖均等劃分成62500個(gè)10cm×10cm的網(wǎng)格，按從上到下、從右往左順序?qū)W(wǎng)格進(jìn)行編號(hào)，環(huán)境感知模塊通過(guò)網(wǎng)格變量來(lái)標(biāo)記和記憶每個(gè)網(wǎng)格是否被障礙物占用來(lái)輸出車輛可行駛區(qū)域，網(wǎng)格變量定義為當(dāng)網(wǎng)格被障礙物占用時(shí)值為0，當(dāng)網(wǎng)格未被占用時(shí)值為1，當(dāng)網(wǎng)格不能判定是否被占用時(shí)值為2。

2.3? 路徑規(guī)劃設(shè)計(jì)

常用的路徑規(guī)劃算法包括圖搜法、幾何法和人工智能算法等。圖搜法包括Dijkstra、RRT、A*等算法，缺點(diǎn)是算法搜索速度慢、未考慮車輛運(yùn)動(dòng)約束、規(guī)劃路徑可能不可行駛等；幾何法包括圓弧直線、微分平坦、B樣條曲線、多項(xiàng)式曲線等算法，運(yùn)算速度很快，缺點(diǎn)是規(guī)劃路徑曲率不一定連續(xù)；人工智能算法包括模糊控制、神經(jīng)網(wǎng)絡(luò)、遺傳算法等算法，非常依賴專家經(jīng)驗(yàn)系統(tǒng)建立和大量樣本訓(xùn)練。

2.3.1? 幾何路徑規(guī)劃方法

考慮到算法的復(fù)雜度、計(jì)算效率和控制器算力，本文選用基于圓弧直線拼接的幾何路徑規(guī)劃方法來(lái)對(duì)自動(dòng)泊車路徑進(jìn)行規(guī)劃，采用基于軌跡預(yù)判的碰撞約束方法來(lái)保證規(guī)劃路徑軌跡不與障礙物發(fā)生碰撞，利用基于軌跡預(yù)判的路徑居中方法來(lái)保證泊車完成后車輛在車位居中無(wú)偏斜，幾何法得到的路徑軌跡按照一定的格式輸出給車輛控制模塊，車輛控制模塊根據(jù)路徑信息來(lái)進(jìn)行路徑跟蹤控制。

2.3.2? 碰撞約束方法

進(jìn)行碰撞約束前，先得對(duì)車輛輪廓進(jìn)行建模，由于車輛輪廓由不規(guī)則曲線組成，計(jì)算輪廓點(diǎn)坐標(biāo)比較繁雜，因此常用規(guī)則形狀的模型代替原輪廓形狀，如圖10所示的圓模型和圖11所示的長(zhǎng)方形模型，圓模型和長(zhǎng)方形模型會(huì)占用部分可行駛區(qū)域，從而增加泊車所需空間，可能不滿足狹小空間泊車要求，因此本文采用圖12所示的基于膨脹法的輪廓模型。該模型保留了車輛原有輪廓形狀，為減小計(jì)算量每間隔10cm取一個(gè)輪廓點(diǎn)，以后軸中心點(diǎn)為原點(diǎn)，點(diǎn)Ai為選取的任意輪廓點(diǎn)，點(diǎn)Ai極坐標(biāo)為（Ri，θi），考慮到感知模塊測(cè)量誤差，采用膨脹法來(lái)保證車輛不發(fā)生碰撞，即車輛輪廓往徑向擴(kuò)張距離L，用膨脹輪廓點(diǎn)Bi（Ri+L，θi）來(lái)代替點(diǎn)Ai。

圖13為垂直泊車過(guò)程幾何碰撞約束示意圖，點(diǎn)M0為車輛泊車起始位置，以點(diǎn)M0為原點(diǎn)建立了電子地圖，泊車過(guò)程中后軸中心點(diǎn)坐標(biāo)可由感知模塊獲取，從而可以求得車輛所有的膨脹輪廓點(diǎn)Bi笛卡爾坐標(biāo)，圖中空白網(wǎng)格為車輛可行駛區(qū)域，帶陰影網(wǎng)格為車輛不可行駛區(qū)域，泊車開(kāi)始后車輛沿著直線M0M1運(yùn)動(dòng)到達(dá)關(guān)鍵點(diǎn)M1，點(diǎn)M1坐標(biāo)計(jì)算見(jiàn)2.3.1章節(jié)，然后車輛在點(diǎn)M1向順時(shí)針?lè)较虼驖M方向盤以最小轉(zhuǎn)彎半徑沿著圓弧M1M2向后運(yùn)動(dòng)到關(guān)鍵點(diǎn)M2，車輛在點(diǎn)M2向逆時(shí)針?lè)较虼驖M方向盤以最小轉(zhuǎn)彎半徑沿著圓弧M2M3向前運(yùn)動(dòng)到關(guān)鍵點(diǎn)M3，關(guān)鍵點(diǎn)M2和M3的坐標(biāo)分別由泊車過(guò)程中車輛膨脹輪廓點(diǎn)Bi與不可行駛區(qū)域發(fā)生碰撞（相交）條件得到，車輛在點(diǎn)M3向順時(shí)針?lè)较虼驖M方向盤以最小轉(zhuǎn)彎半徑沿著圓弧M3M4向前運(yùn)動(dòng)到關(guān)鍵點(diǎn)M4，點(diǎn)M4坐標(biāo)通過(guò)幾何相切條件（車輛在點(diǎn)M4的航向角為90°）計(jì)算得到，此時(shí)車身擺正，然后車輛沿著直線M4M5到達(dá)關(guān)鍵點(diǎn)M5，然后泊車完成。假如，圖13中泊入車位對(duì)面無(wú)障礙物，則車輛達(dá)到點(diǎn)M3后會(huì)繼續(xù)向前轉(zhuǎn)向運(yùn)動(dòng)，直到車身擺正，然后車輛往后直線運(yùn)動(dòng)將會(huì)碰撞到泊入車位右側(cè)障礙物，因此需要增加車輛不碰撞車位右側(cè)障礙物的約束條件，本文采用基于軌跡預(yù)判的碰撞約束方法來(lái)解決這個(gè)問(wèn)題。

2.3.3? 軌跡居中方法

根據(jù)2.3.2章節(jié)講述的碰撞約束條件可以保證車輛能泊入車位，但是不能保證泊車完成后車輛在車位中左右居中，本文設(shè)計(jì)了一種基于軌跡預(yù)判的自動(dòng)泊車路徑居中方法來(lái)解決這個(gè)問(wèn)題。

圖15為基于軌跡預(yù)判的車輛自動(dòng)泊車路徑規(guī)劃左側(cè)居中算法示意圖，其中點(diǎn)P0、P1、P2和P3為車位的4個(gè)角點(diǎn)，后軸中心點(diǎn)沿著圓弧A0A3以最小轉(zhuǎn)彎半徑向后運(yùn)動(dòng)來(lái)泊入車位，車輛達(dá)到點(diǎn)A3位置后停車，然后沿著圓弧A3B3以最小轉(zhuǎn)彎半徑向前運(yùn)動(dòng)，初始位置點(diǎn)A0坐標(biāo)為已知的，點(diǎn)A3的位置需要求解，通過(guò)在點(diǎn)A3出預(yù)判下一段路徑軌跡，通過(guò)居中條件來(lái)求解點(diǎn)A3坐標(biāo)。

該算法具體求解過(guò)程是：泊車開(kāi)始時(shí)控制車輛在點(diǎn)A0原地方向盤沿順時(shí)針?lè)较虼驖M，后軸中心點(diǎn)以車輛最小轉(zhuǎn)彎半徑沿著圓弧A0A3向后運(yùn)動(dòng)，依次經(jīng)過(guò)點(diǎn)A1、A2和A3。假設(shè)后軸中心點(diǎn)行進(jìn)到點(diǎn)A1時(shí)，圓弧A1B1為下一段預(yù)判的路徑軌跡，即假設(shè)車輛在點(diǎn)A1停車后，車輛將在點(diǎn)A1原地方向盤逆時(shí)針打滿，沿著圓弧A1B1前進(jìn)直到車身前進(jìn)方向與車位中心線（或者直線P0P1）平行，此時(shí)B1在車位中心線右側(cè)，未達(dá)到居中條件，因此車輛達(dá)到點(diǎn)A1位置不停車，繼續(xù)向后運(yùn)動(dòng)；假設(shè)后軸中心點(diǎn)行進(jìn)到點(diǎn)A2時(shí)，圓弧A2B2為下一段預(yù)判的路徑軌跡，即假設(shè)車輛在點(diǎn)A2停車后，車輛將在點(diǎn)A2原地方向盤逆時(shí)針打滿，沿著圓弧A2B2前進(jìn)直到車身前進(jìn)方向與車位中心線（或者直線P0P1）相切，此時(shí)B2正好在車位中心線上，達(dá)到居中條件，因此車輛達(dá)到點(diǎn)A2位置后停車，然后將沿著下一段路徑圓弧A2B2前進(jìn)運(yùn)動(dòng)；假設(shè)后軸中心點(diǎn)行進(jìn)到點(diǎn)A3時(shí)，圓弧A3B3為下一段預(yù)判的路徑軌跡，即假設(shè)車輛在點(diǎn)A3停車后，車輛將在點(diǎn)A3原地方向盤逆時(shí)針打滿，沿著圓弧A3B3前進(jìn)直到車身前進(jìn)方向與車位中心線（或者直線P0P1）平行，此時(shí)B3在車位中心線左側(cè)，未達(dá)到居中條件，因此車輛不會(huì)運(yùn)動(dòng)到點(diǎn)A3位置。故，基于軌跡預(yù)判的車輛泊車居中算法可以計(jì)算出居中關(guān)鍵點(diǎn)A2，車輛沿著圓弧A0A3運(yùn)動(dòng)，在達(dá)到點(diǎn)A2將停車并規(guī)劃下一段路徑圓弧A2B2，可以求解出點(diǎn)A2和B2的位置，假設(shè)沿著圓弧A0A2運(yùn)動(dòng)過(guò)程遇到障礙物未能達(dá)到點(diǎn)A2，則需要通過(guò)基于軌跡預(yù)判的路徑規(guī)劃右側(cè)居中算法來(lái)對(duì)路徑軌跡進(jìn)行居中調(diào)整。

圖16為基于軌跡預(yù)判的車輛自動(dòng)泊車路徑規(guī)劃右側(cè)居中算法示意圖。泊車開(kāi)始時(shí)控制車輛在點(diǎn)C0原地方向盤沿逆時(shí)針?lè)较虼驖M，后軸中心點(diǎn)以車輛最小轉(zhuǎn)彎半徑沿著圓弧C0C3向前運(yùn)動(dòng)，依次經(jīng)過(guò)點(diǎn)C1、C2和C3。假設(shè)后軸中心點(diǎn)行進(jìn)到點(diǎn)C1時(shí)，圓弧C1D1為下一段預(yù)判的路徑軌跡，即假設(shè)車輛在點(diǎn)C1停車后，車輛將在點(diǎn)C1原地方向盤順時(shí)針打滿，沿著圓弧C1D1后退直到車身前進(jìn)方向與車位中心線（或者直線P0P1）平行，此時(shí)D1在車位中心線左側(cè)，未達(dá)到居中條件，因此車輛達(dá)到點(diǎn)C1位置不停車，繼續(xù)向前運(yùn)動(dòng)；假設(shè)后軸中心點(diǎn)行進(jìn)到點(diǎn)C2時(shí)，圓弧C2D2為下一段預(yù)判的路徑軌跡，即假設(shè)車輛在點(diǎn)C2停車后，車輛將在點(diǎn)C2原地方向盤順時(shí)針打滿，沿著圓弧C2D2后退直到車身前進(jìn)方向與車位中心線（或者直線P0P1）相切，此時(shí)D2正好在車位中心線上，達(dá)到居中條件，因此車輛達(dá)到點(diǎn)C2位置后停車，然后將沿著下一段路徑圓弧C2D2向后運(yùn)動(dòng)；假設(shè)后軸中心點(diǎn)行進(jìn)到點(diǎn)C3時(shí)，圓弧C3D3為下一段預(yù)判的路徑軌跡，即假設(shè)車輛在點(diǎn)C3停車后，車輛將在點(diǎn)C3原地方向盤順時(shí)針打滿，沿著圓弧C3D3后退直到車身前進(jìn)方向與車位中心線（或者直線P0P1）平行，此時(shí)D3在車位中心線左側(cè)，未達(dá)到居中條件，因此車輛不會(huì)運(yùn)動(dòng)到點(diǎn)C3位置。因此，基于軌跡預(yù)判的車輛泊車居中算法可以計(jì)算出居中關(guān)鍵點(diǎn)C2坐標(biāo)，車輛沿著圓弧C0C3運(yùn)動(dòng)，在達(dá)到點(diǎn)C2將停車并規(guī)劃下一段路徑圓弧C2D2，可以求解出點(diǎn)C2和D2的位置，假設(shè)沿著圓弧C0C2運(yùn)動(dòng)過(guò)程遇到障礙物未能達(dá)到點(diǎn)C2，則需要通過(guò)圖15所示的基于軌跡預(yù)判的路徑規(guī)劃左側(cè)居中算法來(lái)對(duì)路徑軌跡進(jìn)行居中調(diào)整。

3? 自動(dòng)泊車試驗(yàn)設(shè)計(jì)

3.1? 垂直泊車仿真測(cè)試

3.1.1? 仿真測(cè)試工裝

如圖17和圖18所示，垂直泊車仿真測(cè)試工裝包括感知模擬模塊、路徑規(guī)劃模塊和路徑仿真顯示模塊，其中感知模擬模塊和路徑規(guī)劃模塊分別在虛擬機(jī)上命令窗口獨(dú)立并行運(yùn)行，感知模擬模塊用于模擬攝像頭、雷達(dá)等感知傳感器發(fā)送垂直車位4個(gè)角點(diǎn)坐標(biāo)和車輛初始坐標(biāo)、航向角信息，路經(jīng)規(guī)劃模塊收到車位及車輛位姿信息后進(jìn)行路徑規(guī)劃，并按一定格式輸出泊車路徑信息并保存為.m文件，路徑仿真顯示模塊是用MATLAB搭建的GUI界面，將路徑.m文件導(dǎo)入GUI界面后可以顯示泊車過(guò)程車輛輪廓包絡(luò)軌跡，可以直觀看出泊車過(guò)程車輛是否發(fā)生碰撞，并且可以輸出泊車完成后車輛在車位中的偏移距離及偏斜角度數(shù)值。

3.1.2? 仿真測(cè)試

通過(guò)仿真測(cè)試工裝，分別模擬如圖19所示垂直泊車場(chǎng)景，圖中待泊入車位寬度W分別為2.6m、2.8m和3.0m的車位，車位長(zhǎng)度L為6.0m，車輛行駛方向相對(duì)車位線P0P3的初始航向角為0°，車輛右側(cè)到車位線P0P3的初始偏距H分別為0.5m、1m、1.5m和2.0m，組合總計(jì)12種垂直場(chǎng)景，保存虛擬機(jī)上得到的路徑軌跡.m文件，將.m文件導(dǎo)入MATLAB仿真界面中顯示泊車軌跡，記錄泊車完成后在車位中的偏移距離和偏斜角度。

圖20為虛擬機(jī)輸出路徑信息，模擬的是車位寬度為2.8m、初始偏距為2.0m的垂直泊車場(chǎng)景的路徑規(guī)劃，規(guī)劃出的路徑軌跡信息包括各軌跡點(diǎn)X坐標(biāo)、Y坐標(biāo)、曲率、航向角、擋位等信息。圖21為接收?qǐng)D20的路徑信息后MATLAB仿真界面顯示結(jié)果，界面根據(jù)虛擬機(jī)輸出的后軸中心點(diǎn)軌跡信息，計(jì)算出車輛各輪廓點(diǎn)坐標(biāo)，并繪制出后軸中心點(diǎn)和車輛輪廓點(diǎn)曲線（即車輛運(yùn)動(dòng)包絡(luò)線），界面還可以輸出各垂直泊車場(chǎng)景泊車完成后車輛在車位中的偏移距離和偏斜角度，如表1所示。仿真結(jié)果表明，當(dāng)車位寬度大于等于2.6m，車輛初始偏距離為0.5～2.0m范圍時(shí)，設(shè)計(jì)的垂直泊車路徑規(guī)劃算法可以成功規(guī)劃出泊車路徑軌跡，路徑軌跡與環(huán)境障礙物沒(méi)有發(fā)生碰撞，滿足自動(dòng)泊車安全性要求，泊車完成后車輛在車位基本居中和車身基本擺正，車輛偏移距離小于等于5cm，車輛偏斜角度小于等于0.5°，滿足泊車完成后車輛姿態(tài)要求。

3.2? 垂直泊車實(shí)車測(cè)試

3.2.1? 實(shí)車測(cè)試工裝

垂直泊車測(cè)試工裝由4個(gè)攝像頭及12個(gè)超聲波雷達(dá)組成的感知模塊、主控制器、上位機(jī)等組成，搭載車輛為比亞迪某車型，其中感知模塊負(fù)責(zé)獲取待泊入車位4個(gè)角點(diǎn)坐標(biāo)和車輛位姿信息，主控制器上運(yùn)行泊車過(guò)程狀態(tài)切換、路徑規(guī)劃及路徑跟蹤控制等軟件算法，上位機(jī)用于軟件編譯下載及泊車信息顯示打印等。

3.2.2? 實(shí)車測(cè)試

垂直泊車實(shí)車測(cè)試時(shí)，用白膠條貼出車位寬度分別為2.6m、2.8m和3.0m的車位，車位長(zhǎng)度為6.0m，車輛初始航向角為0°，通過(guò)卷尺測(cè)量控制車輛初始偏距分別為0.5m、1m、1.5m和2.0m，組合總計(jì)12種垂直泊車場(chǎng)景，泊車完成后用卷尺測(cè)量車輛在車位中的偏移距離，并根據(jù)前后輪偏移距離計(jì)算出車輛偏移角度，圖22為垂直泊車完成后車輛姿態(tài)圖，圖23為總線采集的目標(biāo)軌跡點(diǎn)和實(shí)際運(yùn)動(dòng)軌跡點(diǎn)曲線圖，表2為泊車完成后車輛偏移距離和車輛偏斜角度數(shù)據(jù)統(tǒng)計(jì)表。實(shí)車測(cè)試結(jié)果表明，設(shè)計(jì)的路徑規(guī)劃算法在常見(jiàn)的垂直泊車場(chǎng)景都能在不發(fā)生碰撞前提下規(guī)劃出路徑并泊入車位中，泊車完成后車輛偏移距離小于等于10cm，車輛偏斜角度小于等于1.5°，由于車輛感知誤差和車輛跟蹤控制誤差，車輛偏移距離和偏斜角度均比仿真測(cè)試結(jié)果要大，但基本滿足泊車要求。

4? 結(jié)論

本文為解決目前量產(chǎn)自動(dòng)泊車系統(tǒng)泊車成功率不高、泊車完成后車輛姿態(tài)偏斜不居中等問(wèn)題，研究了一種基于軌跡預(yù)判的垂直泊車路徑規(guī)劃算法。研究搭建了基于阿克曼轉(zhuǎn)向幾何學(xué)和車輛運(yùn)動(dòng)學(xué)方程的車輛運(yùn)動(dòng)模型，構(gòu)建了基于可行駛區(qū)域的網(wǎng)格化電子地圖，在幾何法中融入基于軌跡預(yù)判的車位角點(diǎn)碰撞約束和路徑居中算法，通過(guò)車輛膨脹輪廓模型最大程度利用電子地圖可行駛區(qū)域和保證路徑安全性，設(shè)計(jì)出一種基于軌跡預(yù)判的垂直泊車幾何路徑規(guī)劃算法，通過(guò)仿真測(cè)試和實(shí)車測(cè)試驗(yàn)證了算法的可行性，該算法大大提高了泊車成功率，解決了泊車完成后車輛偏斜不居中問(wèn)題。

參考文獻(xiàn)：

[1] Gorinevsky D，Kapitanovsky A. Neural networkarchitecture for trajectory generation and control of automated car parking[J]. IEEE Transactions on Control Systems Technology，1996，4（1）：50-56.

[2] Daxwanger W A，Schmidt G K. Skill-based visual parking control using neural and fuzzy networks[J]. IEEE International Conference on Systems，1995（2）：1659-1664.

[3] Zhao Y，Collins Jr E G. Robust automatic parallel parking in tight spaces via fuzzylogic[J]. Robotics and AutonomousSystems，2005，51（2）：111-127.

[4] Chen Q Y，Chen T T，Yu H Y，et al. Lateral control for autonomous parking system with fractional order controller[J]. Journal of Software，2011（6）：1075-1078.

[5] Zheng G，Liang Z，Li J. Optimization of an intelligent controller for parallel autonomous parking[J]. Telkomnika Indonesian Journal of Electrical Engineering，2012，11（2）：1069-1075.

[6] 秦緒情. 自動(dòng)平行泊車系統(tǒng)定速模糊控制算法研究[D]. 長(zhǎng)春：吉林大學(xué)，2007.

[7] 宋金澤. 自主泊車系統(tǒng)關(guān)鍵技術(shù)研究[D]. 長(zhǎng)沙：國(guó)防科技大學(xué)，2009.

[8] 姜輝. 自動(dòng)平行泊車系統(tǒng)轉(zhuǎn)向控制策略的研究[D]. 長(zhǎng)春：吉林大學(xué)，2010.

[9] 郭孔輝，姜輝，張建偉，等. 基于路徑規(guī)劃的自動(dòng)平行泊車轉(zhuǎn)向控制器的研究[J].吉林大學(xué)學(xué)報(bào)（工學(xué)版），2011，41（2）：293-297.

[10] 李紅，郭孔輝，宋曉琳. 基于樣條理論的自動(dòng)垂直泊車軌跡規(guī)劃[J]. 湖南大學(xué)學(xué)報(bào)（自然科學(xué)版），2012，39（7）：25-30.

（編輯? 凌? 波）