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

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

（1. 武漢理工大學，現代汽車零部件技術湖北省重點實驗室，武漢 430070；2. 武漢理工大學，汽車零部件技術湖北省協同創新中心，武漢 430070；3. 新能源與智能網聯車湖北工程技術研究中心，武漢 430070）

前言

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

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

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

1 車輛運動學模型

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

圖1 汽車運動學模型

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

表1 汽車主要參數

此外，車輛外輪廓的各坐標點可表示為

式中、為參考點，即后軸中心的坐標，其余參數見圖1和表1。

2 平行泊車路徑規劃

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

圖2 泊車規劃流程圖

圖3 最小轉彎半徑出庫示意圖

2.1 調整規劃

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

圖4 庫內調整規劃

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

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

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

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

（1）回旋曲線的構建方法

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

圖5 回旋曲線坐標系xoy與車位坐標系XOY

圖中和分別為點在泊車位坐標系和回旋曲線坐標系下的車輛航向角，坐標系和的轉換關系為

構建步驟：

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

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

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

（2）圓弧的構建方法

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

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

2.1.1 調整路徑設計

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

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

2.1.2 調整路徑優化設計

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

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

2.2 入庫規劃

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

圖6 平行泊車入庫規劃

2.2.1 第1段入庫路徑設計

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

2.2.2 第2段入庫路徑設計

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

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

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

2.2.3 最佳泊車起點選取

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

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

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

3 仿真分析

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

式中：X，Y和X，Y以及X，Y分別為車輛回轉中心和后軸中心以及點的坐標；X，Y為點初始坐標。

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

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

表2 工況詳細信息

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

圖7 仿真結果圖

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

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

最后從整體仿真結果來看，本方法可以以較快效率完成庫內調整，同時具備較強的適應性。

4 實車試驗

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

圖8 試驗平臺車

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

圖9 平臺構架圖

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

圖10 車位6 m泊車試驗

圖11 車位7 m泊車試驗

圖12 車位8 m泊車試驗

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

5 結論

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