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

熊斌　鐘益林　彭邦煌　白軍明

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

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

中圖分類(lèi)號(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—），主任控制算法工程師，主要從事新能源汽車(chē)整車(chē)智能控制算法開(kāi)發(fā)工作；彭邦煌，男，軟件測(cè)試開(kāi)發(fā)部經(jīng)理，研究方向?yàn)槠?chē)算法研發(fā)。

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

2.2? 電子地圖

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

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

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

2.2.2? 網(wǎng)格劃分

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

電子地圖網(wǎng)格劃分示意如圖8所示，以泊車(chē)開(kāi)始時(shí)的車(chē)輛后軸中心點(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)輸出車(chē)輛可行駛區(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)是算法搜索速度慢、未考慮車(chē)輛運(yùn)動(dòng)約束、規(guī)劃路徑可能不可行駛等；幾何法包括圓弧直線、微分平坦、B樣條曲線、多項(xiàng)式曲線等算法，運(yùn)算速度很快，缺點(diǎn)是規(guī)劃路徑曲率不一定連續(xù)；人工智能算法包括模糊控制、神經(jīng)網(wǎng)絡(luò)、遺傳算法等算法，非常依賴(lài)專(zhuān)家經(jīng)驗(yàn)系統(tǒng)建立和大量樣本訓(xùn)練。

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

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

2.3.2? 碰撞約束方法

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

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

2.3.3? 軌跡居中方法

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

圖15為基于軌跡預(yù)判的車(chē)輛自動(dòng)泊車(chē)路徑規(guī)劃左側(cè)居中算法示意圖，其中點(diǎn)P0、P1、P2和P3為車(chē)位的4個(gè)角點(diǎn)，后軸中心點(diǎn)沿著圓弧A0A3以最小轉(zhuǎn)彎半徑向后運(yùn)動(dòng)來(lái)泊入車(chē)位，車(chē)輛達(dá)到點(diǎn)A3位置后停車(chē)，然后沿著圓弧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ò)程是：泊車(chē)開(kāi)始時(shí)控制車(chē)輛在點(diǎn)A0原地方向盤(pán)沿順時(shí)針?lè)较虼驖M(mǎn)，后軸中心點(diǎn)以車(chē)輛最小轉(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è)車(chē)輛在點(diǎn)A1停車(chē)后，車(chē)輛將在點(diǎn)A1原地方向盤(pán)逆時(shí)針打滿(mǎn)，沿著圓弧A1B1前進(jìn)直到車(chē)身前進(jìn)方向與車(chē)位中心線（或者直線P0P1）平行，此時(shí)B1在車(chē)位中心線右側(cè)，未達(dá)到居中條件，因此車(chē)輛達(dá)到點(diǎn)A1位置不停車(chē)，繼續(xù)向后運(yùn)動(dòng)；假設(shè)后軸中心點(diǎn)行進(jìn)到點(diǎn)A2時(shí)，圓弧A2B2為下一段預(yù)判的路徑軌跡，即假設(shè)車(chē)輛在點(diǎn)A2停車(chē)后，車(chē)輛將在點(diǎn)A2原地方向盤(pán)逆時(shí)針打滿(mǎn)，沿著圓弧A2B2前進(jìn)直到車(chē)身前進(jìn)方向與車(chē)位中心線（或者直線P0P1）相切，此時(shí)B2正好在車(chē)位中心線上，達(dá)到居中條件，因此車(chē)輛達(dá)到點(diǎn)A2位置后停車(chē)，然后將沿著下一段路徑圓弧A2B2前進(jìn)運(yùn)動(dòng)；假設(shè)后軸中心點(diǎn)行進(jìn)到點(diǎn)A3時(shí)，圓弧A3B3為下一段預(yù)判的路徑軌跡，即假設(shè)車(chē)輛在點(diǎn)A3停車(chē)后，車(chē)輛將在點(diǎn)A3原地方向盤(pán)逆時(shí)針打滿(mǎn)，沿著圓弧A3B3前進(jìn)直到車(chē)身前進(jìn)方向與車(chē)位中心線（或者直線P0P1）平行，此時(shí)B3在車(chē)位中心線左側(cè)，未達(dá)到居中條件，因此車(chē)輛不會(huì)運(yùn)動(dòng)到點(diǎn)A3位置。故，基于軌跡預(yù)判的車(chē)輛泊車(chē)居中算法可以計(jì)算出居中關(guān)鍵點(diǎn)A2，車(chē)輛沿著圓弧A0A3運(yùn)動(dòng)，在達(dá)到點(diǎn)A2將停車(chē)并規(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ù)判的車(chē)輛自動(dòng)泊車(chē)路徑規(guī)劃右側(cè)居中算法示意圖。泊車(chē)開(kāi)始時(shí)控制車(chē)輛在點(diǎn)C0原地方向盤(pán)沿逆時(shí)針?lè)较虼驖M(mǎn)，后軸中心點(diǎn)以車(chē)輛最小轉(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è)車(chē)輛在點(diǎn)C1停車(chē)后，車(chē)輛將在點(diǎn)C1原地方向盤(pán)順時(shí)針打滿(mǎn)，沿著圓弧C1D1后退直到車(chē)身前進(jìn)方向與車(chē)位中心線（或者直線P0P1）平行，此時(shí)D1在車(chē)位中心線左側(cè)，未達(dá)到居中條件，因此車(chē)輛達(dá)到點(diǎn)C1位置不停車(chē)，繼續(xù)向前運(yùn)動(dòng)；假設(shè)后軸中心點(diǎn)行進(jìn)到點(diǎn)C2時(shí)，圓弧C2D2為下一段預(yù)判的路徑軌跡，即假設(shè)車(chē)輛在點(diǎn)C2停車(chē)后，車(chē)輛將在點(diǎn)C2原地方向盤(pán)順時(shí)針打滿(mǎn)，沿著圓弧C2D2后退直到車(chē)身前進(jìn)方向與車(chē)位中心線（或者直線P0P1）相切，此時(shí)D2正好在車(chē)位中心線上，達(dá)到居中條件，因此車(chē)輛達(dá)到點(diǎn)C2位置后停車(chē)，然后將沿著下一段路徑圓弧C2D2向后運(yùn)動(dòng)；假設(shè)后軸中心點(diǎn)行進(jìn)到點(diǎn)C3時(shí)，圓弧C3D3為下一段預(yù)判的路徑軌跡，即假設(shè)車(chē)輛在點(diǎn)C3停車(chē)后，車(chē)輛將在點(diǎn)C3原地方向盤(pán)順時(shí)針打滿(mǎn)，沿著圓弧C3D3后退直到車(chē)身前進(jìn)方向與車(chē)位中心線（或者直線P0P1）平行，此時(shí)D3在車(chē)位中心線左側(cè)，未達(dá)到居中條件，因此車(chē)輛不會(huì)運(yùn)動(dòng)到點(diǎn)C3位置。因此，基于軌跡預(yù)判的車(chē)輛泊車(chē)居中算法可以計(jì)算出居中關(guān)鍵點(diǎn)C2坐標(biāo)，車(chē)輛沿著圓弧C0C3運(yùn)動(dòng)，在達(dá)到點(diǎn)C2將停車(chē)并規(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)泊車(chē)試驗(yàn)設(shè)計(jì)

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

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

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

3.1.2? 仿真測(cè)試

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

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

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

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

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

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

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

4? 結(jié)論

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

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

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

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

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

（編輯? 凌? 波）