階段約束下Gauss配點離散化平行車位自動泊車軌跡規劃*

劉 平，陳 卓，劉明杰，，樸昌浩，Jang Soohyun，萬凱林

（1.重慶郵電大學自動化學院，重慶 400065；2.韓國電子技術研究所，Korea Seongnam 13230；3.重慶長安汽車股份有限公司，重慶 400023）

前言

汽車作為現代社會重要的交通工具，在日常出行和生活中扮演著越來越重要的角色。然而，日益復雜的城市車位環境對泊車技術提出了更高要求。自動泊車作為解放駕駛員雙手、提高泊車效率和安全性的重要技術在近些年得到了廣泛關注和研究［1-2］。

自主泊車技術中，泊車軌跡規劃是實現自動泊車的重要基礎。當前，泊車軌跡規劃算法主要包括：幾何方法、圖搜索算法、隨機采樣方法、智能學習算法和數值優化方法等。其中，幾何方法主要通過曲線組合和插值擬合獲取軌跡，比如文獻［3］中采用兩段圓弧添加一段直線的方式來規劃泊車路徑，車輛可從水平初始位姿行駛一段S 形曲線路徑后進入泊車位并擺正車身姿態；文獻［4］中則基于回旋曲線的平行泊車路徑規劃，避免了原地轉向現象，實現規劃路徑基本可控。而圖搜索算法則首先將二維環境地圖或者三維環境地圖離散化成柵格或者晶格，然后從起點出發向目標點搜索出一條滿足最優準則的路徑，比如文獻［5］中構建柵格地圖搜索空間，并采用混合A*算法進行了自主代客泊車路徑規劃；文獻［6］中以停車場信息為基礎，設計了多分辨率路線圖，可以在較小的在線計算成本下獲得自動泊車軌跡規劃。隨機采樣方法具有概率完備性，在構型空間中通過生成樣本點，并尋找滿足任務的規劃結果，如文獻［7］中結合RRT*（rapidly-exploring random tree）算法與非線性優化算法，實現曲率連續的泊車軌跡并適應多泊車場景。智能學習算法主要通過設計策略神經網絡，利用行為克隆或強化學習對策略網絡進行訓練從而獲得軌跡［8］，如文獻［9］中利用非線性規劃獲得演示數據，進而搭建策略神經網絡模型獲取泊車軌跡。數值優化方法則是將自動泊車軌跡規劃整理為車輛在環境約束和車輛自身特性約束下從起始位置行駛至目標位置滿足某些評價指標的約束動態優化問題（dynamic optimization problem，DOP），采用數值計算方法求解并得到車輛最優運動軌跡［10］。

相較于其他幾種方法，數值優化方法采用車輛運動學方程、約束條件和評價車輛行駛性能的指標來描述軌跡規劃問題，得到的軌跡具有直觀準確的特點［11］，因而備受關注。其中，偽譜法作為泊車軌跡數值優化方法中的一種，具有求解精度高、計算量少、結構簡單、求解效率高等優點，得到眾多學者的青睞。比如，文獻［10］和文獻［12］中基于最優化和動態優化理論，分別采用傳統高斯偽譜法和hp 自適應偽譜法對自主泊車路徑規劃問題進行了求解。偽譜方法的思想是通過正交配點將軌跡規劃DOP 問題離散化，并通過全局插值多項式逼近狀態變量和控制變量，從而轉化為非線性規劃（nonlinear programming，NLP）問題再進行數值優化方法求解［13-14］。當前，Gauss 偽譜法在狀態變量、控制變量和協調變量的近似精度更高、收斂速度更快，在處理初始和終端約束問題上具有優勢，得到了廣泛的應用［15-16］。然而，文獻［17］中指出，全局Gauss 配點具有兩端密集中間稀疏的特點，此種離散化配點方式對于泊車軌跡求解是不利的，特別是在稀疏配點的地方約束變化劇烈而導致無法求解。為了解決此問題，文獻［18］中提出了一種在不同泊車空間采用分段高斯偽譜法的自主泊車路徑規劃方法，在不同階段進行單獨求解；文獻［14］中則提出了兩階段配點重構來提升高斯偽譜法的求解能力；文獻［19］中研究了偽譜拼接法對多階段泊車進行求解并獲得泊車路徑規劃。以上方法在分段多次求解的方式下可以獲取不同車位大小的泊車軌跡。

為了進一步提升Gauss 偽譜法在自動泊車軌跡規劃上的求解能力和軌跡規劃精度，從本質上實現Gauss偽譜法階段配點與整體求解，本文提出基于階段約束下Gauss 配點偽譜法，用于平行車位自動泊車軌跡規劃的整體求解。首先，在全面分析平行車位泊車過程的基礎上，將泊車過程分為了泊車起動、接近車位、進入車位、姿態調整和泊車落位5 個階段，并結合范圍邊界和圓弧理論建立相應的約束條件；以此為基礎，結合車輛運動學方程建立了泊車軌跡DOP 數學模型；然后，將泊車軌跡在時間區間上劃分為5段，并采用非均勻多Gauss配點對每個階段進行離散，在易發生碰撞的區域增加配點數，在安全區域減少配點數，實現多階段分區域配點，并進行整體求解，以此降低優化時間，進而提升軌跡優化精度；最后，在算法實現的基礎上，進行仿真實驗，在不同車位長度下泊車進行仿真測試，并與分區域高斯偽譜法全局優化方法對比以驗證所提方法的有效性。

1 車輛自動泊車數學模型

1.1 車輛運動學模型

基于阿克曼轉向原理［20］，結合圖1 所示的車輛運動學示意圖，建立車輛泊車動力學模型。首先，作如下假設：

圖1 車輛運動學示意圖

（1）車輛在行駛過程中車輪沒有產生側向力；

（2）車輛的側向速度為0，車輪的側偏角為0°。

圖1 中：R(x，y)為車輛后軸中心點，m；L為車輛軸距，m；f和r分別為車輛前軸到車頭距離和后軸到車位的距離，m；d為車輛寬度，m；φ(t)為前輪擺角，（°）；θ(t)為車身姿態角，（°）；A、B、C、D為車身4 個頂點，m。根據上述模型，可得車輛微分方程模型如下：

式中：v(t)為后軸中心點速度，m/s；a(t)為車輛加速度，m/s2；ω(t)為前輪轉角角速度。同時，得到的A、B、C、D4點x坐標關系為

A、B、C、D4點y坐標關系為

1.2 自動泊車階段約束

泊車運動本質上是一個避障運動，泊車路徑要滿足無碰撞發生且滿足自身約束條件，結合車輛泊車過程，在文獻［18］中泊車區域的基礎上，本文將自動泊車過程劃分為5個階段。

（1）泊車起動階段（N1段）

根據日常生活中的泊車經驗，車輛一般在前車車輛附近開始泊車［21］。本文設定泊車起動階段為車輛從起動泊車開始，車輛到達泊車車位右前端設定位置的過程如圖2 所示。本文劃定泊車起動階段區域為Z1，即車輛從起始位置開始，短暫行駛一段距離后，車輛A、B、C、D4 點均處于Z1區域內。在此階段中，包含車輛從較遠起始位置到達規劃的合適起始位置以及車輛在泊車起始位置開始起動兩種情況。

圖2 泊車起動階段

圖2 中：L1+L2和L3分別為泊車道路長度和泊車車位的長度，m；W1和W2分別為道路和泊車位的寬度，m。以車位左頂點為坐標原點，建立坐標系，并分析泊車起動階段約束條件。在此階段，主要是讓車輛從起始的位置到達距離車位較近的位置，則A、B、C、D4 點均需要在車位上方區域Z1內運動，根據道路長度與車位長度，區域Z1表示范圍為

式中：Z1(x)表示Z1區域內x坐標的取值；Z1(y)表示Z1區域內y坐標的取值。因此，對于泊車起動階段的約束條件為

式 中Ax、Bx、Cx、Dx和Ay、By、Cy、Dy分別表示車輛A、B、C、D頂點的x坐標和y坐標。

（2）接近車位階段（N2段）

接近車位階段是指車輛完成泊車起動階段后，車身進入泊車車位平行區域的過程，如圖3 所示。此階段中，車輛4 頂點A、B、C、D從Z1區域開始逐漸進入區域Z3。此外，在接近車位過程中需要保證車輛與車庫右頂點P不發生碰撞。基于倒推的最小半徑規劃方法［22］，本文提出了結合點到直線距離以及圓弧理論的約束構建方法。

圖3 接近車位階段

在車輛泊車過程中，車輛以圓心O做逆時針圓弧運動，在圓弧運動中，車輛右側車位C點不能與P點發生碰撞。接近車位階段，車輛A、B、C、D4 點在自由大區域（定義為Z2）內運動，區域Z2由Z1和Z3兩個區域組成，其中Z3表示范圍為

然而，為了使泊車時車輛運動路徑最短，減少所需要的泊車空間，在此區域內，本文根據泊車設定點P0(x0，y0)，取圓心坐標點為O(x0，-1)構建圓弧約束條件，如圖4 所示。首先，車輛航向角即為BC邊的斜角，根據截距式直線方程可得BC邊表達式：

圖4 接近車位約束條件

圖5 進入車位階段

式中Cx、Cy為C點的x、y坐標。

取O到BC邊的垂直交點為E，則根據幾何關系構建得到泊車路徑約束：

①泊車軌跡滿足|OE| ＞|OP|，即

式中：|OE|由點到直線距離公式構建；(Px，Py)為車庫左頂點P點的x，y坐標。

②泊車軌跡滿足車輛左側頂點A的y坐標始終小于W1/2，即

此階段結束后C、D點在Z3區域內。

（3）進入車位階段（N3段）

進入車位階段為車輛車身從泊車車位平行區域進入泊車車位的過程，如圖 5 所示。進入車位階段主要實現車輛從Z1到Z3的完全過渡，最終A、B、C、D4 點完全進入了Z3區域，在此階段車輛仍在Z2大區域內運動。此階段，同樣需要防止車輛與車位右頂點碰撞，由于車輛做圓弧運動，因此進入車位階段與接近車位階段聯合采用式（8）和式（9）約束條件。

（4）姿態調整階段（N4段）

姿態調整階段為車輛車身從泊車車位平行區域逐步調整姿態全部已進入泊車車位的過程，如圖6所示。車輛從泊車區域Z3逐漸進入車位最終到達區域Z4內，因此只須確保車輛4 個頂點落在區域Z3內才不會發生碰撞。

圖6 姿態調整階段

因此，在此區域內，構建車輛A、B、C、D4頂點的約束條件為

（5）泊車落位階段（N5段）

泊車落位階段為車輛在泊車車位內通過姿態調整到達泊車目標點的過程，如圖7 所示。在此階段中，車輛A、B、C、D4 點需要在泊車目標區域Z4內，因而需要滿足的約束條件為

圖7 泊車落位階段

此外，在上述5 階段劃分的基礎上，車輛通常需要從預先設定的初始時刻狀態到達指定的終端時刻狀態，因而自動泊車軌跡規劃過程還須滿足初始和終端約束條件，同時還須滿足自身物理約束，分別表示為

式中：vmax表示泊車時最大速度，m/s；amax表示泊車運動最大加速度，m/s2；φmax表示最大前輪擺角，（°）；ωmax表示最大前輪擺角角速度，（°）/s；t0表示泊車起始時刻，s；tf表示泊車終止時刻，s。

1.3 自動泊車軌跡規劃DOP模型

結合模型分析后，定義車輛泊車過程狀態向量x(t)=[x(t)，y(t)，θ(t)，v(t)，φ(t)]T，控制向量u(t)=[a(t)，ω(t)]T，則式（1）車輛自動泊車運動微分方程式可以簡化為

式中f：R×Rn×Rm是關 于時 間t∈R、狀 態向 量x(t) ∈Rn和控制向量u(t) ∈Rm的非線性函數，n和m分別表示狀態向量和控制向量的維度。結合式（13），系統的初始狀態值和終端狀態值可以表示為

同時，根據5 階段劃分，得到階段約束條件，將式（5）、式（8）～式（11）路徑約束寫成通用路徑約束形式：

式（12）控制邊界條件則表示為

此外，在車輛泊車過程中，通常存在各種各樣的優化目標，如能耗最低，泊車時間最短等。本文主要對泊車時間最短進行研究，用終值項表示為

最終，自動泊車軌跡優化問題可以描述為如下動態優化問題。

DOP 問題（P1）：給定式（14）連續動態方程和式（15）狀態初始條件，通過優化控制量u(t)使其在整個控制時域t內滿足式（17）路徑約束條件和式（18）邊界約束，在到達式（16）指定終端約束的同時，式（19）目標函數為最優。

2 多階段Gauss 配點參數化軌跡優化算法

通常，DOP問題的參數化優化求解思路為：將狀態/控制變量進行離散，然后采用插值多項式逼近，進而將最優控制問題轉化為離散點處狀態、控制變量為待優化參量的NLP問題。

2.1 Legendre-Gauss配點插值逼近

本文采用Lagrange插值對泊車DOP的狀態變量和控制變量進行離散逼近。首先，結合泊車的5 階段劃分，將整體泊車時間區間劃分成5 段，初始時刻為t0、終端時刻為t5=tf，各階段內部時間節點為t1，t2，t3，t4，整個區間劃分為5 段，每一段的長度分別記 為n1，n2，…，n4+1，第m段對應的時間區間為[tm-1，tm] (m=1，2，...，5)，每段長度分別記為θ1，θ2，...，θ5，引入新的時間變量τ將控制時域進行時間尺度變換：

經過式（20）轉換后，每個子區間的時間域轉換為[-1，1]區間，而t∈[tm-1，tm]對應的時刻則可以利用以下公式進行反變換：

此時，問題P1 變換為基于時間域[-1，1]的多段最優控制問題（optimal control problem，OCP），其中式（14）微分方程轉換為如下形式：

進一步，在時間尺度轉換的基礎上，本文采用逼近效果優良的Legendre 多項式在各段[-1，1]區間進行離散配點選擇進而完成狀態和控制變量的離散化。根據Legendre 正交多項式的性質可知，該多項式的零點是位于開區間(-1，1)內的單重實根，零點的計算可以由定理1 保證，證明過程請參閱文獻［14］。

定理1.已知N次Legendre多項式如下：

該多項式的根可由以下Q矩陣的特征值求取：

此時，對于任意分段區間[tm-1，tm]可以得到Nm個位于開區間(-1，1) 內的離散點，通常稱為Legendre Gauss（LG）配點。在獲取離散配點的基礎上，狀態變量和控制變量采用Lagrange 插值方法進行逼近，記第m時間段[tm-1，tm]的LG 配點的數目為Km，則該段的狀態向量xm(τ)和控制向量um(τ)的Lagrange插值逼近可以表示為

式中：Xm，i為狀態向量在第m段初始時刻處或在LG配置點處的值；Um，i為控制向量在LG 配點處的值；Lm，i(τ)和為Lagrange插值多項式的基函數。

式中τm，i(i=1，…，Km)為第m段的第i個配置點，τm，0=-1。

2.2 DOP離散化

經Lagrange 插值離散逼近后，則可以對DOP問題進行離散化。對式（23）中狀態變量對τ求導可得：

進而，聯立式（25）和式（22）可得：

進一步定義：

并令Zm=[Hm，1...Hm，m]，代入式（26）后可以得到經LG配置點離散化后的非線性等式方程組：

式中：Xm=[Xm，1，…，Xm，Km]T∈R 是狀態向量第m階段Km個配置點處的值；Um=[Um，1，…，Um，Km]T∈R 是對應的離散控制參數；F(Xm，Um，τm，1，…，τm，Km)由式（30）求取。

最終，經過多階段狀態變量和控制變量LG配點離散多項式插值后，自動泊車軌跡規劃DOP 問題被轉化為如下NLP問題（P2）：

3 算法實現步驟

基于上述多階段Gauss 配點離散參數化軌跡優化理論過程，給出本文提出算法的實現步驟，其優化策略流程如圖8所示，具體實現步驟如下。

圖8 基于階段劃分的多Gauss配點參數化優化算法流程圖

（1）輸入泊車車輛模型及車位環境信息；

（2）得到自動泊車軌跡規劃DOP問題P1；

（3）設置分段階段數m，選擇每階段LG 配點數Km；

（4）采用定理1 計算各階段LG 配點，計算微分矩陣Hm；

（5）將各階段狀態量和控制量進行Lagrange 插值離散，得到式（29）離散化動力學方程非線性等式方程組；

（6）使用階段劃分多Gauss 配點參數化將問題（P1）離散化為NLP問題（P2）；

（7）設定優化參數初始值和求解精度，優化NLP求解器求解問題（P2）；

（8）得到狀態變量和控制變量優化求解結果，采用時間尺度轉換優化結果；

（9）輸出優化結果。

4 仿真測試

以文獻［18］中采用的某SUV 車輛為測試對象，選擇泊車時間最短作為優化目標函數進行測試。車輛參數和車輛控制約束取值如表1 所示。取車輛后軸中心點位置為x0=9 m，y0=1.5 m；設定車位寬度W2為2.5 m，泊車道路長度L1長為6 m，L2長為14 m，泊車道路總長為 20 m，道路寬度中線寬度為3.5 m；道路邊界寬度即W1為7 m；泊車車位長度L3設置為7.8、6.8、5.8 m 3 種不同長度進行測試。結合泊車過程5 階段采用等間隔劃分，自動泊車DOP問題現為多段動態優化問題，各階段分別表示為N1，N2，…，N5。同時，測試初始值和終端約束值如表2 所示。本文仿真測試在配置為2.1 GHz AMD Ryzen 5 4600U with Radeon Graphics處理器和16 GB、3 200 MHz 內存的個人電腦MATLAB 2016a 平臺上進行。

表1 車輛參數及控制約束

表2 車輛自動泊車軌跡優化邊界值約束

由于國家道路標準平衡式車位長度為長6 m，寬2.4 m［23］，故而本文將7.8 和6.8 m 車位歸類為長車位，而5.8 m 歸類為窄車位。在長車位測試條件下，本文采用相同階段配點進行測試，而在窄車位條件下，因為初始位置（9.0 m，1.5 m）對于窄車位來說較遠，故采用將泊車起動階段起始泊車點較遠時規劃至合適的設定位置（7.0 m，1.5 m）后再進行滿足約束條件方式進行泊車，長、窄車位測試條件下的多階段配點選擇，如表3所示。

表3 長窄車位下各階段配點數配置

此外，為了驗證本文提出方法求解的有效性，將本文提出的階段劃分多Gauss 配點參數化優化算法與文獻［18］中等提出的全局分區域高斯偽譜在同種車輛模型、同等車位約束下進行對比測試。表4 列出了本文所提出的方法與文獻方法的優化結果對比。分析可知：在7.8 和6.8 m 長車位條件下，本文方法得到的泊車時間分別為8.879 和10.16 s，相較于分區域偽譜法平均減少2.07 s，泊車時間減少比例達17.9%；在5.8 m 窄車位條件下，本文方法得到的泊車時間為11.547 s，相較于分區域偽譜法減少4.79 s，泊車時間減少比例達29.3%?？傮w來講，在3 種車位測試條件下，本文方法泊車時間平均減少2.976 s，泊車時間性能平均提升21.7%，展示出本文研究方法對于平行車位自動泊車軌跡規劃的有效性。

表4 不同方法泊車軌跡優化結果對比

為了直觀展示自動泊車軌跡規劃效果，圖9 給出了長車位下本文方法得到的泊車軌跡規劃圖。由圖可知，在7.8 和6.8 m 長車位條件下，本文方法均得到了滿足車位約束和道路約束的平滑泊車軌跡，并在較短時間內到達指定的泊車位置。同時，分析圖9 可知，泊車車輛的車身均在車道中線以內，顯示出本文方法泊車軌跡規劃的效能。相應地，2 種長車位下得到的各階段自動泊車狀態和控制變量曲線如圖10 所示。圖中結果顯示在不同車位下各狀態和控制曲線趨勢一致，無明顯振動，各個子段均滿足約束條件，在不同車位條件下能做出對應的優化調整，確保規劃軌跡滿足要求。此外，在7.8 m 車位條件下，本文方法規劃得到前輪轉向角在約束范圍內且速度曲線均未超過零，表明此時不需要進行前后移動車輛即可滿足泊車要求，同時速度曲線顯示泊車過程速度調節較少；而在6.8 m 車位條件下，車輛僅在第4 階段，即姿態調整階段以微小的正速度對車輛進行調整。圖9 和圖10 結果顯示出本文提出算法在長車位泊車條件下泊車規劃的正確性。

圖9 長車位下本文方法泊車軌跡規劃圖

圖10 長車位下自動泊車狀態和控制變量曲線

在窄車位（5.8 m 車位）條件下，考慮到車輛起始位置與車輛位置較遠（與車位距離達3.2 m），在測試中將泊車起動階段設置為（7.0 m，1.5 m），利用本文提出方法進行規劃得到的軌跡、狀態、控制曲線如圖11所示。

圖11 窄車位下本文方法泊車軌跡規劃圖

由圖可知，在窄車位下本文方法依舊規劃得到了平滑的泊車軌跡，車輛先進行平行移動，然后進行泊車入位，在泊車過程中均未與泊車邊界發生碰撞并準確到達設定的終端位置，顯示出本文方法對于窄車位泊車軌跡規劃的適應性。

圖12 給出了窄車位條件下得到的各階自動泊車狀態和控制變量曲線。圖中航向角曲線與長車位條件下曲線軌跡趨勢一致；在速度曲線方面，由于車位變窄，車輛可調整的空間變小，因而泊車過程中速度調整增加，在接近車位階段，為了防止與邊界發生碰撞，規劃速度有明顯的減速過程，這也是泊車時間相較于長車位增加的原因所在，但在遠離邊界后速度回升，實現快速泊車。圖11和圖12測試結果顯示在窄車位條件下本文方法能夠很好地規劃得到泊車軌跡，并優化得到規劃速度和轉角曲線，進一步驗證了本文提出方法的正確性和適應性。

圖12 窄車位泊車軌跡以及狀態變量曲線

5 結論

本文提出一種階段約束下多Gauss 配點參數化自動泊車軌跡規劃算法。在分析泊車動作過程的基礎上將自動泊車劃分成了5 個階段并建立了對應的約束條件。根據泊車階段劃分，在高斯配點離散化逼近基礎上，提出了多階段局部高斯配點策略，以此提高泊車軌跡規劃的精準度；在譜方法算法框架下，提出了多階段Gauss 配點參數化軌跡優化算法，可以有效改善車輛泊車過程的軌跡規劃精度，提升泊車效率。通過對通用小型汽車在長窄車位軌跡優化仿真測試發現，與等分區域全局優化偽譜法算法相比，改進方法得到的軌跡更平滑，泊車時間更短，泊車時間平均減少2.976 s，泊車時間性能平均提升達到21.7%，顯示了所提出方法對車輛自動泊車的有效性和實用價值。下一步，將針對泊車過程時間自適應分配，開展實車驗證工作，并在時間分配和配點配置中尋求最佳組合，不斷提升泊車算法的效率。