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

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

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

前言

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

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

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

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

1 車輛自動泊車數(shù)學(xué)模型

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

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

圖1 車輛運動學(xué)示意圖

（1）車輛在行駛過程中車輪沒有產(chǎn)生側(cè)向力；

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

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

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

A、B、C、D4點y坐標(biāo)關(guān)系為

1.2 自動泊車階段約束

泊車運動本質(zhì)上是一個避障運動，泊車路徑要滿足無碰撞發(fā)生且滿足自身約束條件，結(jié)合車輛泊車過程，在文獻(xiàn)［18］中泊車區(qū)域的基礎(chǔ)上，本文將自動泊車過程劃分為5個階段。

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

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

圖2 泊車起動階段

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

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

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

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

接近車位階段是指車輛完成泊車起動階段后，車身進(jìn)入泊車車位平行區(qū)域的過程，如圖3 所示。此階段中，車輛4 頂點A、B、C、D從Z1區(qū)域開始逐漸進(jìn)入?yún)^(qū)域Z3。此外，在接近車位過程中需要保證車輛與車庫右頂點P不發(fā)生碰撞?；诘雇频淖钚“霃揭?guī)劃方法［22］，本文提出了結(jié)合點到直線距離以及圓弧理論的約束構(gòu)建方法。

圖3 接近車位階段

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

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

圖4 接近車位約束條件

圖5 進(jìn)入車位階段

式中Cx、Cy為C點的x、y坐標(biāo)。

取O到BC邊的垂直交點為E，則根據(jù)幾何關(guān)系構(gòu)建得到泊車路徑約束：

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

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

②泊車軌跡滿足車輛左側(cè)頂點A的y坐標(biāo)始終小于W1/2，即

此階段結(jié)束后C、D點在Z3區(qū)域內(nèi)。

（3）進(jìn)入車位階段（N3段）

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

（4）姿態(tài)調(diào)整階段（N4段）

姿態(tài)調(diào)整階段為車輛車身從泊車車位平行區(qū)域逐步調(diào)整姿態(tài)全部已進(jìn)入泊車車位的過程，如圖6所示。車輛從泊車區(qū)域Z3逐漸進(jìn)入車位最終到達(dá)區(qū)域Z4內(nèi)，因此只須確保車輛4 個頂點落在區(qū)域Z3內(nèi)才不會發(fā)生碰撞。

圖6 姿態(tài)調(diào)整階段

因此，在此區(qū)域內(nèi)，構(gòu)建車輛A、B、C、D4頂點的約束條件為

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

泊車落位階段為車輛在泊車車位內(nèi)通過姿態(tài)調(diào)整到達(dá)泊車目標(biāo)點的過程，如圖7 所示。在此階段中，車輛A、B、C、D4 點需要在泊車目標(biāo)區(qū)域Z4內(nèi)，因而需要滿足的約束條件為

圖7 泊車落位階段

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

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

1.3 自動泊車軌跡規(guī)劃DOP模型

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

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

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

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

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

最終，自動泊車軌跡優(yōu)化問題可以描述為如下動態(tài)優(yōu)化問題。

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

2 多階段Gauss 配點參數(shù)化軌跡優(yōu)化算法

通常，DOP問題的參數(shù)化優(yōu)化求解思路為：將狀態(tài)/控制變量進(jìn)行離散，然后采用插值多項式逼近，進(jìn)而將最優(yōu)控制問題轉(zhuǎn)化為離散點處狀態(tài)、控制變量為待優(yōu)化參量的NLP問題。

2.1 Legendre-Gauss配點插值逼近

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

經(jīng)過式（20）轉(zhuǎn)換后，每個子區(qū)間的時間域轉(zhuǎn)換為[-1，1]區(qū)間，而t∈[tm-1，tm]對應(yīng)的時刻則可以利用以下公式進(jìn)行反變換：

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

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

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

該多項式的根可由以下Q矩陣的特征值求?。?/p>

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

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

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

2.2 DOP離散化

經(jīng)Lagrange 插值離散逼近后，則可以對DOP問題進(jìn)行離散化。對式（23）中狀態(tài)變量對τ求導(dǎo)可得：

進(jìn)而，聯(lián)立式（25）和式（22）可得：

進(jìn)一步定義：

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

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

最終，經(jīng)過多階段狀態(tài)變量和控制變量LG配點離散多項式插值后，自動泊車軌跡規(guī)劃DOP 問題被轉(zhuǎn)化為如下NLP問題（P2）：

3 算法實現(xiàn)步驟

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

圖8 基于階段劃分的多Gauss配點參數(shù)化優(yōu)化算法流程圖

（1）輸入泊車車輛模型及車位環(huán)境信息；

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

（3）設(shè)置分段階段數(shù)m，選擇每階段LG 配點數(shù)Km；

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

（5）將各階段狀態(tài)量和控制量進(jìn)行Lagrange 插值離散，得到式（29）離散化動力學(xué)方程非線性等式方程組；

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

（7）設(shè)定優(yōu)化參數(shù)初始值和求解精度，優(yōu)化NLP求解器求解問題（P2）；

（8）得到狀態(tài)變量和控制變量優(yōu)化求解結(jié)果，采用時間尺度轉(zhuǎn)換優(yōu)化結(jié)果；

（9）輸出優(yōu)化結(jié)果。

4 仿真測試

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

表1 車輛參數(shù)及控制約束

表2 車輛自動泊車軌跡優(yōu)化邊界值約束

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

表3 長窄車位下各階段配點數(shù)配置

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

表4 不同方法泊車軌跡優(yōu)化結(jié)果對比

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

圖9 長車位下本文方法泊車軌跡規(guī)劃圖

圖10 長車位下自動泊車狀態(tài)和控制變量曲線

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

圖11 窄車位下本文方法泊車軌跡規(guī)劃圖

由圖可知，在窄車位下本文方法依舊規(guī)劃得到了平滑的泊車軌跡，車輛先進(jìn)行平行移動，然后進(jìn)行泊車入位，在泊車過程中均未與泊車邊界發(fā)生碰撞并準(zhǔn)確到達(dá)設(shè)定的終端位置，顯示出本文方法對于窄車位泊車軌跡規(guī)劃的適應(yīng)性。

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

圖12 窄車位泊車軌跡以及狀態(tài)變量曲線

5 結(jié)論

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