狹小車位平行泊車路徑規(guī)劃方法研究*

胡 杰，朱令磊，陳瑞楠，鐘鑫凱，徐文才，張敏超

（1. 武漢理工大學(xué)，現(xiàn)代汽車零部件技術(shù)湖北省重點實驗室，武漢 430070；2. 武漢理工大學(xué)，汽車零部件技術(shù)湖北省協(xié)同創(chuàng)新中心，武漢 430070；3. 新能源與智能網(wǎng)聯(lián)車湖北工程技術(shù)研究中心，武漢 430070）

前言

隨著社會經(jīng)濟的發(fā)展，汽車保有量不斷增加，停車位逐漸緊張的同時，泊車位的空間愈加狹窄，對于駕駛經(jīng)驗不足的駕駛員將是一個巨大的挑戰(zhàn)。為解決這一難題，能夠在狹窄車位下完成泊車任務(wù)的自動泊車技術(shù)逐漸成為研究重點。

目前已有的自動泊車算法主要分為4 種：一是基于幾何的方法，主要包括曲線組合和插值擬合方法，如文獻［1］中設(shè)計“圓弧-線段-圓弧”的曲線組合路徑，通過雙步庫內(nèi)調(diào)整策略解決狹窄泊車問題，文獻［2］中綜合考慮汽車運動學(xué)約束、泊車碰撞約束等，提出基于回旋曲線的平行泊車路徑規(guī)劃，避免了原地轉(zhuǎn)向現(xiàn)象，該類方法運算耗時短，規(guī)劃路徑基本可控，便于工程的應(yīng)用，但泊車路徑存在突變點，曲率并不連續(xù)，且對起始位姿要求比較高；二是基于數(shù)值優(yōu)化的方法，通過求解泊車路徑約束優(yōu)化問題獲得最優(yōu)泊車路徑，如文獻［3］中建立以泊車時間最短作為優(yōu)化目標(biāo)的優(yōu)化框架，可在狹窄車位的情況規(guī)劃出泊車路徑，但未考慮路徑約束，文獻［6］中綜合考慮避障和車輛運動學(xué)約束，建立最優(yōu)化多段式泊車路徑規(guī)劃，解決了泊車過程中停車原地轉(zhuǎn)向的問題，該類算法求解能力強，但計算量過大、耗時長難以滿足工程的需求；三是基于采樣的方法，在構(gòu)型空間中生成樣本點，并尋找滿足任務(wù)需求的樣本點序列作為規(guī)劃結(jié)果，如文獻［8］中采用RRT*算法進行路徑搜索，找到可行路徑后，通過Dijkstra 算法對路徑的長度和前后換向的次數(shù)進行優(yōu)化，但并不能保證規(guī)劃路徑曲率的連續(xù)性，文獻［10］中結(jié)合RRT*算法與非線性優(yōu)化算法，實現(xiàn)曲率連續(xù)的泊車軌跡，該類算法對泊車場景的適應(yīng)性強；四是基于學(xué)習(xí)的方法，即設(shè)計從環(huán)境到控制的策略神經(jīng)網(wǎng)絡(luò)，利用行為克隆或強化學(xué)習(xí)對策略網(wǎng)絡(luò)進行訓(xùn)練，如文獻［13］中利用非線性規(guī)劃獲得演示數(shù)據(jù)，通過模仿學(xué)習(xí)初始化策略神經(jīng)網(wǎng)絡(luò)（PNN），經(jīng)過強化學(xué)習(xí)對策略網(wǎng)絡(luò)進行改進，優(yōu)點是通過融合消除各模塊間誤差、泛化性好，但學(xué)習(xí)算法的訓(xùn)練難以收斂，且應(yīng)用受限于仿真環(huán)境與實際的差異。

據(jù)此，本文中詳細分析了狹小車位自主泊車路徑規(guī)劃的問題，結(jié)合幾何法設(shè)計簡單耗時短和數(shù)值優(yōu)化計算能力強的優(yōu)點，提出計算能力強、適用范圍廣、曲率連續(xù)的平行泊車路徑規(guī)劃方法。首先建立了平行泊車路徑規(guī)劃框架，將平行泊車問題分解為調(diào)整規(guī)劃和入庫規(guī)劃問題，然后基于回旋-圓弧組合的約束最優(yōu)化問題設(shè)計調(diào)整模塊，求解最佳出庫點，隨后基于五次多項式和圓弧-直線-回旋組合的方式設(shè)計起始點到出庫點的入庫最優(yōu)化問題，求解最佳泊車點。最后，通過仿真和實車測試驗證算法的適應(yīng)性和有效性。

1 車輛運動學(xué)模型

由于自動泊車過程中，車輛速度較低且無側(cè)向偏移，可假定車輛的轉(zhuǎn)彎過程作圓周運動，將四輪阿克曼轉(zhuǎn)向模型簡化為兩輪自行車模型，同時汽車外輪廓直接影響了泊車的成功率。為了避免與環(huán)境發(fā)生碰撞，同時給予富裕的泊車空間，以車輛最大長度和最大寬度包裹的矩形框來描述汽車外輪廓。建立的車輛運動學(xué)模型如圖1所示。

圖1 汽車運動學(xué)模型

本文中仿真和實車試驗參數(shù)如表1 所示，選擇車輛后軸中心作為車輛參考點，根據(jù)圖1 所示的幾何關(guān)系，可計算得出汽車后軸中心點最小轉(zhuǎn)彎半徑為6.5 m。

表1 汽車主要參數(shù)

此外，車輛外輪廓的各坐標(biāo)點可表示為

式中、為參考點，即后軸中心的坐標(biāo)，其余參數(shù)見圖1和表1。

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

本文中所建立的平行泊車路徑規(guī)劃框架包括入庫規(guī)劃模塊和調(diào)整規(guī)劃模塊。調(diào)整規(guī)劃模塊根據(jù)車位大小自行計算調(diào)整次數(shù)，入庫規(guī)劃模塊根據(jù)出庫關(guān)鍵點和自車位置計算最佳泊車點，具體規(guī)劃流程如圖2所示。首先，建立以為原點車位坐標(biāo)系，將泊車過程逆向化，以泊車終點作為起點，以最小轉(zhuǎn)彎半徑轉(zhuǎn)彎出庫直至車輛右頂點到達車位右邊界，計算此時的車輛狀態(tài)和車輛右頂點坐標(biāo)，如圖3 所示。若點縱坐標(biāo)小于車位寬度，則表示車位長度無法滿足一次泊入，執(zhí)行調(diào)整規(guī)劃模塊，計算點新坐標(biāo)，直至滿足出庫要求或調(diào)整次數(shù)達到限值，則結(jié)束調(diào)整規(guī)劃模塊。然后，以出庫點作為新的泊車目標(biāo)點，將泊車問題簡化為一次入庫問題，執(zhí)行入庫規(guī)劃模塊，尋找最佳泊車點，通過該泊車點若能生成一條起點到終點的無碰撞的滿足曲率要求的路徑，則入庫規(guī)劃結(jié)束。最后，將入庫路徑和調(diào)整路徑結(jié)合得到一條完整的平行泊車路徑。

圖2 泊車規(guī)劃流程圖

圖3 最小轉(zhuǎn)彎半徑出庫示意圖

2.1 調(diào)整規(guī)劃

現(xiàn)有研究普遍采用圓弧-直線組合通過讓汽車產(chǎn)生側(cè)向位移來不斷逼近目標(biāo)點，但是該幾何法曲率不連續(xù)，車輪須頻繁原地轉(zhuǎn)向，對轉(zhuǎn)向機構(gòu)不利，其次通過雙圓弧來實現(xiàn)，想要獲得較大的側(cè)向位移且每次車姿都要平行車位，對縱向空間和最小轉(zhuǎn)彎半徑的要求較高。故本文中設(shè)計了一種通過庫內(nèi)調(diào)整車姿以快速達到目標(biāo)位置的調(diào)整規(guī)劃方法，如圖4 所示。該調(diào)整規(guī)劃路段由后退路徑和前進路徑組成，后退路徑為了調(diào)整車輛位姿以創(chuàng)造調(diào)整空間使前進路徑更容易滿足出庫要求。前進路徑是為了達到車位右邊界時盡可能滿足出庫要求。

圖4 庫內(nèi)調(diào)整規(guī)劃

回旋線大多作為過渡曲線，其曲率半徑隨曲線上某一點至該曲線起點距離成反比，或說，其曲率與距離成正比，即=/，其中為曲率對距離的變化率。由于泊車環(huán)境比較狹窄，往往需要快速轉(zhuǎn)動轉(zhuǎn)向盤，所以本文采用轉(zhuǎn)向盤所需要的轉(zhuǎn)動時間確定回旋曲線的長度，轉(zhuǎn)向盤轉(zhuǎn)動時間和回旋曲線長度分別為

式中：、為始末曲率；為轉(zhuǎn)向盤的轉(zhuǎn)動角速度；為轉(zhuǎn)向傳動比。

根據(jù)曲率變化和曲線長度計算曲率變化率，得到初始方位角為零的回旋曲線段終點曲率的點的坐標(biāo)和夾角為

式中：為總弧長；為曲率變化率；為終點曲率；為冪級數(shù)展開個數(shù)，展開數(shù)越高，求解精度越高，但相應(yīng)計算量也越大，本文選取=15。當(dāng)、和數(shù)值選取不同正負(fù)號時，可得到在不同象限的回旋曲線。

（1）回旋曲線的構(gòu)建方法

作為通用的方法，假設(shè)該回旋曲線段要連接的兩段圓弧，且已知第1 個圓弧終點的位置、方位角和半徑以及第2 個圓弧的半徑。建立初始曲率和方位角為0的回旋曲線坐標(biāo)系，如圖5所示。

圖5 回旋曲線坐標(biāo)系xoy與車位坐標(biāo)系XOY

圖中和分別為點在泊車位坐標(biāo)系和回旋曲線坐標(biāo)系下的車輛航向角，坐標(biāo)系和的轉(zhuǎn)換關(guān)系為

構(gòu)建步驟：

①計算回旋曲線坐標(biāo)系原點在泊車位坐標(biāo)系下的位姿；根據(jù)曲率變化率和點的曲率計算弧長S= k，由式（8）～式（10）可計算點在回旋曲線坐標(biāo)系下的位姿（，，），隨后將點的坐標(biāo)（泊車位坐標(biāo)系）代入式（11）～式（13）反求點在泊車位坐標(biāo)系的位姿（X，Y，）。

②計算點車位坐標(biāo)系下的位姿；根據(jù)車輛運動方向處的曲率，由式（6）和式（7）計算的弧長S，計算弧長S= S+ S由式（8）～式（10）計算點在回旋曲線坐標(biāo)系下的位姿（，，），將它們代入式（11）～式（13），求得點的在泊車位坐標(biāo)系的位姿。

當(dāng)回旋曲線段一端的曲率為0 時，則只須進行步驟①即可，此時即為要求的回旋曲線段。需要說明的是本節(jié)和后面的調(diào)整規(guī)劃和入庫歸劃關(guān)于回旋曲線的構(gòu)建，只須確定連接點的位姿即達到目的。只當(dāng)通過調(diào)整規(guī)劃和入庫規(guī)劃，得到最佳泊車路徑后，要通過實車試驗驗證時，才需要各回旋曲線段的具體軌跡，即離散點的坐標(biāo)，屆時由式（8）～式（10）計算即可。

（2）圓弧的構(gòu)建方法

圓弧線是主要路徑曲線，由車輛以某一旋轉(zhuǎn)中心、固定轉(zhuǎn)向半徑轉(zhuǎn)過一定角度所生成的路徑。設(shè)車輛參考點走過一段圓弧，不論前進或后退，若定義從泊車位坐標(biāo)系橫坐標(biāo)向量旋轉(zhuǎn)至與參考點的速度向量方向一致時所轉(zhuǎn)過的角度為方位角（逆時針為正，順時針為負(fù)），則圓弧上任一點，包括兩個端點的坐標(biāo)、圓心坐標(biāo)和方位角存在如下關(guān)系：

式中：X、Y和X、Y分別為圓弧上某點和圓心在泊車位坐標(biāo)系中的坐標(biāo)；為該點的方位角；為圓弧半徑，當(dāng)右手4 個手指順著圓弧運動方向，若拇指由紙面指向外（或方位角增加，即Δ0）時取正值，若拇指由紙面指向內(nèi)（或方位角減小，即Δ0）時取負(fù)值。因此，構(gòu)建圓弧段的具體步驟為：先將圓弧起點的坐標(biāo)作為X、Y連同方位角代入式（14），求得圓心坐標(biāo)X、Y，再將它們和圓弧終點的方位角（注意此時已有增量Δ）同樣代入式（14），求得的X、Y即為圓弧終點的坐標(biāo)，而其方位角自然是Δ

2.1.1 調(diào)整路徑設(shè)計

調(diào)整路徑如圖4 所示。首先調(diào)整后退路徑，以車輛達到車位右邊界的極限位置作為后退路徑起點。和為回旋曲線段，和為圓弧段。給定已知條件為起點的位姿與曲率、點的曲率、圓弧的圓心角Δ、點的曲率和圓弧的圓心角Δ。遵循曲率連續(xù)的原則，即可按照前面“（1）回旋曲線的構(gòu)建方法”和“（2）圓弧的構(gòu)建方法”完成后退路徑的構(gòu)建。

然后調(diào)整前進路徑，為回旋曲線段，為圓弧段。由于前進段目的是盡可能駛出車位，故以最小轉(zhuǎn)彎半徑作為圓弧段半徑行駛最佳。這意味著原須給定的點的曲率已確定為與車輛最小轉(zhuǎn)彎半徑相對應(yīng)的最大曲率。因此，可采用同樣方法構(gòu)建前進路徑的曲線。至于圓弧段的終點，由于在出庫過程中，車輛外輪廓點首先與車位邊界發(fā)生碰撞，故建立以圓心、||為半徑的圓弧方程（X-X）+（Y-Y）=，計算當(dāng)X到車位右邊界時的Y，即為終點的坐標(biāo)。

2.1.2 調(diào)整路徑優(yōu)化設(shè)計

為使車輛通過后退和前進調(diào)整更快滿足出庫要求，則每次調(diào)整后的Y不斷接近車位寬，故本文以、Δ、、Δ作為待定參數(shù)，將最大化Y作為優(yōu)化目標(biāo)，則上述平行泊車進入段路徑規(guī)劃問題可轉(zhuǎn)化為如下約束優(yōu)化問題：

式中約束、、、為對車輛輪廓的約束。庫內(nèi)調(diào)整過程中，車輛的障礙物為車位邊界，故車輛輪廓須滿足的約束為：、點不與停車位右邊界碰撞，即X，X≤X-；、點不與停車位左邊界碰撞，即X，X≥X+；、點不與停車位下邊界碰撞，即Y，Y≥，其中，為車輛與泊車位壁面的最小距離,取0.1 m。而曲率和弧度的約束為：、小于最大曲率，圓弧圓心角小于π/2。

2.2 入庫規(guī)劃

現(xiàn)有研究的平行泊車入庫規(guī)劃普遍沒有考慮當(dāng)車輛位于車位后方時路徑是否有可行解的情況，而且通過單曲線組合方式所確定的可泊車區(qū)域往往把航向角約束為零，并不能找到一個最佳的泊車點，因此本文設(shè)計了一個兩段式入庫規(guī)劃，如圖6 所示，第1 段為前進路徑，第2 段為后退路徑，通過建立非線性規(guī)劃模型確定合適的泊車點生成入庫規(guī)劃路徑。

圖6 平行泊車入庫規(guī)劃

2.2.1 第1段入庫路徑設(shè)計

第1 段入庫路徑是生成泊車最佳點到調(diào)整規(guī)劃出庫點的曲線，本節(jié)采用結(jié)合回旋-圓弧-直線組合的方式，具體以出庫點作為起點逆向推導(dǎo)至泊車點。考慮到為使車輛盡可能接近出庫點，調(diào)整規(guī)劃的前進路徑的圓弧段選用最小轉(zhuǎn)彎半徑，該路徑未必是最優(yōu)路徑，故入庫規(guī)劃以調(diào)整規(guī)劃的退后路徑與前進路徑的銜接點作為起點，并重新編號為。如圖6 所示，第1 段路徑中，、、和為回旋曲線段，和為圓弧段，而為直線段。給定已知條件為：起點的位姿與曲率、點的曲率、圓弧段的圓心角?、直線長度為、點的曲率、圓弧段的圓心角?和點的曲率為0。遵循曲率連續(xù)的原則，即可按照同樣的方法，構(gòu)建第1段入庫路徑。

2.2.2 第2段入庫路徑設(shè)計

第2 段入庫路徑，即后退段主要是為了指引車輛行駛到泊車點，為了路徑滿足連續(xù)變化，同時增加算法的適應(yīng)性，本文采用五次多項式設(shè)計第2 段入庫路徑。五次多項式的公式可以表示為

假設(shè)起始點和泊車點的位姿分別為（，，）和（，，），可得：

根據(jù)式（20）～式（25）建立6 個等式方程，可求解一條唯一的五次多項式曲線，該曲線即為規(guī)劃的路徑。

2.2.3 最佳泊車起點選取

同時連接兩段路徑，該點的選擇直接影響了整段入庫規(guī)劃的效率，本文中將、、、、Δ作為待優(yōu)化參數(shù)，將最小化兩段路徑總長度作為優(yōu)化目標(biāo)，則上述平行泊車入庫路徑規(guī)劃問題可轉(zhuǎn)化為如下約束最優(yōu)化問題：

實際上，車輛的泊車區(qū)除停車位之外，還有一個上邊界，如圖6 所示。其與停車位下邊界的距離，稱為泊車區(qū)總寬度，以‘’表示，8.5 m。

式（26）中的約束、、、為對路徑中車輛輪廓與泊車區(qū)邊界的碰撞約束，即車輛輪廓與泊車區(qū)邊界應(yīng)保持最小距離=0.1 m。約束須滿足不與泊車區(qū)上邊界碰撞，即Y≤-；約束須滿足不與停車位右邊界碰撞，即X≤X-；約束須滿足不與停車位下邊界碰撞，即Y≥；約束須滿足不與停車位左邊界和泊車區(qū)上邊界碰撞，即X≥和Y≤-；約束須滿足第1 段幾何入庫規(guī)劃的可能性，即X+(+)cos+sin/2 ≥X

3 仿真分析

本文在Matlab環(huán)境下對路徑規(guī)劃算法進行仿真驗證，首先根據(jù)最小轉(zhuǎn)彎半徑計算該車型一次泊入所需要的最小車位長度，建立以下等式：

式中：X，Y和X，Y以及X，Y分別為車輛回轉(zhuǎn)中心和后軸中心以及點的坐標(biāo)；X，Y為點初始坐標(biāo)。

先由第1 和第2 式分別求出回轉(zhuǎn)中心的坐標(biāo)X，Y和點的初始坐標(biāo)X，Y；再將其作為X，Y代入第3 式求出R；最后再利用第3 式，求當(dāng)Y等于泊車位寬度W時的X，即為車輛可一次泊入的泊車位最小長度L。W=2.5 m 時，實際求得L=7.225 m。

以最小車位逐漸縮小長度并以不同初始位姿建立9種仿真場景，詳細信息如表2所示。

表2 工況詳細信息

采用Matlab 工具箱中fmincon 求解器對上述規(guī)劃進行求解，并與“圓弧-直線”庫內(nèi)調(diào)整規(guī)劃進行對比，對本文調(diào)整規(guī)劃方法的調(diào)整次數(shù)進行分析，同時采用不同初始角和泊車位，對算法適應(yīng)性進行分 析，結(jié)果如圖7所示。

圖7 仿真結(jié)果圖

由Case2 可得，車位長度大于L時，調(diào)整路段均不規(guī)劃路徑，由入庫規(guī)劃模塊規(guī)劃出1 次入庫路徑；對于Case5，車位長度略小于L時，基于圓弧-直線規(guī)劃方法的調(diào)整次數(shù)為5 次，遠大于本文方法的1 次；而對于長度再次縮小1 m 的Case8，圓弧-直線方法已無法規(guī)劃出滿足最大調(diào)整次數(shù)的路徑，而本文方法調(diào)整次數(shù)僅增加了1 次，能夠充分利用庫內(nèi)空間完成規(guī)劃任務(wù)。

由Case1～Case3 可以看出，車輛位于平行車位左側(cè)，但以-5°、0°、5°不同方位角作為起始狀態(tài)時，入庫規(guī)劃的最佳泊車點位置也發(fā)生變化，這是因為對五次多項式的曲率進行了約束，進而尋找滿足曲率約束、路徑最短的泊車點。由Case4～Case6 和Case7～Case9 可進一步看出，以不同初始角面對相同泊車位時，庫內(nèi)調(diào)整路徑相同，表明調(diào)整路段僅與車位大小有關(guān)，將起始位姿和泊車位對泊車效果的影響分開，能有效簡化入庫規(guī)劃算法。

最后從整體仿真結(jié)果來看，本方法可以以較快效率完成庫內(nèi)調(diào)整，同時具備較強的適應(yīng)性。

4 實車試驗

為了驗證規(guī)劃路徑的可行性，基于實車試驗平臺對其驗證。實車試驗平臺如圖8 所示，其主要參數(shù)如表1所示。

圖8 試驗平臺車

實車試驗平臺的軟件構(gòu)架如圖9 所示，采用分層結(jié)構(gòu)。底層驅(qū)動程序通過Labview 編寫，部署在NI 控制器中，通過CAN 報文控制車輛；上層路徑規(guī)劃和軌跡跟蹤模塊均在Simulink 中編寫，采用ROS平臺將自車信息和環(huán)境信息通過UDP 傳入Simulink處理，再通過UDP 將控制信號傳入到底層驅(qū)動程序進而控制車輛。為了保證實時接受信號和發(fā)送控制信號，Simulink 采用并行運算，以100 Hz 頻率接受UDP 的信息，以50 Hz 頻率運行路徑規(guī)劃和跟蹤模塊。其中本文所采用路徑跟蹤為曲率前饋和位置誤差反饋控制。

圖9 平臺構(gòu)架圖

因為本文規(guī)劃包含圓弧曲線，為了給控制模塊一定控制余量，將規(guī)劃所采用的最小轉(zhuǎn)彎半徑加大至7 m，根據(jù)式（27）算得所需L為7.79 m，故分別對車位大小8、7和6 m的情況進行試驗，其中起始位置位于車位同側(cè)，但位姿不固定。試驗結(jié)果如圖10～圖12 所示，其中藍色表示規(guī)劃路徑，紅色表示實車軌跡。

圖10 車位6 m泊車試驗

圖11 車位7 m泊車試驗

圖12 車位8 m泊車試驗

由試驗結(jié)果可以看出，在車位大小為8、7和6 m的情況下，均能規(guī)劃出滿足約束的路徑，而且能夠根據(jù)車位變化規(guī)劃出調(diào)整次數(shù)分別為0 次、1 次、2 次，由于控制累計誤差的存在和轉(zhuǎn)向角速度的限制導(dǎo)致跟蹤效果不是很好，但是均能完成泊車任務(wù)，說明規(guī)劃出的路徑具備有效性。

5 結(jié)論

本文中提出一種基于曲線組合和數(shù)值優(yōu)化相結(jié)合的方法研究平行泊車路徑規(guī)劃，有效解決泊車位愈加狹窄的泊車問題，首先建立平行泊車路徑規(guī)劃框架，將泊車過程逆向化并劃分為調(diào)整規(guī)劃路徑和入庫規(guī)劃路徑，以基于圓弧-回旋曲線的約束最優(yōu)化問題設(shè)計調(diào)整路徑，引導(dǎo)車輛庫內(nèi)調(diào)整位姿尋找出庫關(guān)鍵點，以基于五次多項式和圓弧-回旋曲線-直線的組合約束優(yōu)化設(shè)計入庫路徑，引導(dǎo)車輛尋找最佳泊車點；然后通過Matlab 仿真證明了算法的適應(yīng)性，可在不同車位大小和起始位姿下規(guī)劃出路徑；最后的實車試驗結(jié)果表明，算法所規(guī)劃出的路徑滿足車輛行駛要求，具備有效性。在實車試驗中發(fā)現(xiàn)，車輛跟蹤過程中的累計誤差對泊車效果有較大影響，因此后續(xù)將對泊車過程中的規(guī)劃路徑更新問題進行進一步研究。