自主泊車路徑規(guī)劃一致性方法*

余卓平 夏浪 熊璐

（同濟(jì)大學(xué)，上海 201804）

主題詞：自主泊車 一致性路徑規(guī)劃 凸優(yōu)化

1 前言

自主泊車路徑規(guī)劃作為智能汽車技術(shù)的典型代表，要求在極其狹窄的環(huán)境中找尋汽車從當(dāng)前區(qū)域到指定停車位之間的可行駛路徑。輪式機(jī)器人所具有的不完整約束性質(zhì)，以及車輛在復(fù)雜、狹窄環(huán)境里的碰撞檢測(cè)，都使自主泊車路徑規(guī)劃更具挑戰(zhàn)性。

在對(duì)當(dāng)前泊車環(huán)境完全理解的基礎(chǔ)上，幾何法以其簡(jiǎn)單、高效的特點(diǎn)，在路徑規(guī)劃中得到大量應(yīng)用，它通過獲取當(dāng)前車輛、環(huán)境障礙物、目標(biāo)泊車位三者的幾何關(guān)系，采用直線或曲線求解車輛可行駛泊車路徑。具有開創(chuàng)性的幾何法當(dāng)屬Dubins曲線[1]和可倒車Dubins曲線[2]。為彌補(bǔ)Dubins曲線曲率不連續(xù)的缺點(diǎn)，更多學(xué)者采用樣條曲線[3]、多項(xiàng)式曲線[4]以及回旋曲線[5]路徑規(guī)劃。然而，基于幾何的方法對(duì)環(huán)境要求高，通常每一套算法僅對(duì)應(yīng)一種或一類環(huán)境，在停車環(huán)境日益復(fù)雜化的今天，幾何法已很難滿足自主泊車的“自主”需求，以快速隨機(jī)擴(kuò)展樹（Rapidly Random Tree，RRT）或A*為基礎(chǔ)的各種變形搜索算法普遍用于輪式機(jī)器人的路徑規(guī)劃。然而，這些算法并未廣泛應(yīng)用于自主泊車路徑規(guī)劃研究。Schwesinger U等人[6]在對(duì)歐洲代客充電（Valet Charge，V-Charge）項(xiàng)目的調(diào)研中提出將Hybrid A*算法應(yīng)用于第3階段自主泊車路徑規(guī)劃，但該文僅限于思路，并未提及相關(guān)仿真及實(shí)車試驗(yàn)。Kwon H等人[7]提出KPP（Korea University Path Planner）算法，對(duì)標(biāo)基礎(chǔ)RRT算法，仿真結(jié)果表明，在泊車工況下，KPP算法在提供可選路徑寬度、曲率連續(xù)性等方面較基礎(chǔ)RRT算法有更佳表現(xiàn)。此外，一種基于數(shù)值優(yōu)化求解的方法引起了關(guān)注。浙江大學(xué)的Li Bai[8]采用拉格朗日差值點(diǎn)的方法，將動(dòng)態(tài)連續(xù)規(guī)劃問題離散為非線性優(yōu)化問題，以數(shù)值求解的方式迭代求解出自主泊車可行路徑。然而仿真結(jié)果表明，該方法存在實(shí)時(shí)性差、優(yōu)化過程初值敏感等缺點(diǎn)，實(shí)用效果并不理想。Zips P[9]與Gao H[10]等人分別針對(duì)泊車過程道路狹窄以及保障泊車過程安全性開展研究，核心部分同樣以數(shù)值求解的方式離散車輛狀態(tài)點(diǎn)，求解車輛路徑。

針對(duì)自主泊車工況，現(xiàn)有研究大多針對(duì)平行庫(kù)位與垂直庫(kù)位，鮮見關(guān)于斜庫(kù)位的泊車研究。根據(jù)我國(guó)公安部1988年頒布的《停車場(chǎng)規(guī)劃設(shè)計(jì)規(guī)則（試行）》，標(biāo)準(zhǔn)庫(kù)位包含不同角度的斜庫(kù)位。本文基于數(shù)值優(yōu)化求解的特點(diǎn)，提出一種自主泊車路徑規(guī)劃的一致性方法，針對(duì)不同角度的庫(kù)位均可規(guī)劃出滿足需求的路徑，環(huán)境適應(yīng)性強(qiáng)。

2 問題描述

2.1 問題背景

路徑規(guī)劃本質(zhì)是兩點(diǎn)邊值問題的求解。針對(duì)泊車規(guī)劃的場(chǎng)景，統(tǒng)一約束方程，構(gòu)造最優(yōu)目標(biāo)函數(shù)：

式中，gi(x)為等式約束；hj(x)為不等式約束；f(x)為優(yōu)化目標(biāo)。

針對(duì)一般的軌跡優(yōu)化問題，優(yōu)化對(duì)象包含T·K個(gè)變量，其中T為整個(gè)問題離散的狀態(tài)數(shù)目，K為優(yōu)化對(duì)象狀態(tài)數(shù)。本文依次分析泊車問題中的等式約束、不等式約束以及優(yōu)化目標(biāo)，對(duì)整個(gè)泊車問題進(jìn)行全面的數(shù)學(xué)表述。

2.2 等式約束

現(xiàn)實(shí)生活中，泊車過程車速較慢，本文忽略懸架作用，避免復(fù)雜的動(dòng)力學(xué)模型，采用經(jīng)典線性2自由度汽車運(yùn)動(dòng)學(xué)模型如圖1所示。

圖1 汽車運(yùn)動(dòng)學(xué)模型

其運(yùn)動(dòng)學(xué)方程為：

式中，(x(t),y(t))為車輛當(dāng)前位置參考點(diǎn)（車輛后軸中心）P的坐標(biāo)；φ(t)為車輛航向角；v(t)為車輛后軸中心線速度；δ(t)為前輪轉(zhuǎn)向角；a(t)為車輛切向加速度；L為軸距。

本文將整個(gè)泊車過程離散為(N+1)個(gè)狀態(tài)，假設(shè)任意兩相鄰狀態(tài)時(shí)間間隔top相等，離散后車輛的前后時(shí)刻狀態(tài)變換滿足：

式中，zt∈Rnz為系統(tǒng)在（tt∈{t0,t1,…,tN}）時(shí)刻的所有狀態(tài)變量；zs為系統(tǒng)初始狀態(tài)；zf表示系統(tǒng)目標(biāo)狀態(tài)；nz為狀態(tài)變量個(gè)數(shù)，包括x(t)、y(t)、φ(t)、v(t)；ut∈Rnu為系統(tǒng)在t時(shí)刻的輸入變量；nu為輸入變量個(gè)數(shù)，包括δ(t)、a(t)；Rnu→Rnz表示系統(tǒng)的狀態(tài)變換；TF為整個(gè)泊車過程所需時(shí)間。

2.3 不等式約束

假設(shè)障礙物OM與車輛E均為定義在R2上的真錐（Proper Cone），即含有非空內(nèi)部且有界[11]。當(dāng)前環(huán)境t時(shí)刻車輛占據(jù)的空間表示為Et?R2，障礙物占據(jù)的空間為OM?R2，OM=O1∪O2∪…∪Om。t時(shí)刻車輛與障礙物不發(fā)生沖突可表示為Et∩OM=?。通常上述問題是非凸的，因此需要對(duì)該問題進(jìn)行進(jìn)一步描述。

假設(shè)車輛與障礙物均為定義在R2上的多胞形（Poly?tope），那么，障礙物與車輛所占據(jù)的空間可表示為：

式中，A∈Rl·n；b∈Rl；Cm∈Rkm·n；dm∈Rkm；n為空間維度；l、k為組成凸集的超平面?zhèn)€數(shù)。

定義在R2上兩凸集Et、Om間的距離可表示為dist(Et,Om)=inf{||x-y||2|x∈Et,y∈Om}，如圖2所示，其中連接兩凸集的虛線為歐幾里得2范數(shù)（Euclidean Norm 2）最小值，即車輛與障礙物的距離。對(duì)于泊車過程的避障約束，通常設(shè)定車輛與障礙物之間的安全距離dmin，即dist(Et,Om)>dmin,?t∈{1,2,…,N}。進(jìn)一步描述為：

圖2 車輛與障礙物間距離示意

由于OM、Et的非空凸集性質(zhì)，原問題滿足Slater條件，即?x∈Et，y∈Om，Ax=b，Cmy=dm，滿足強(qiáng)對(duì)偶性要求[11-12]，且對(duì)偶問題的最優(yōu)解即原問題的最優(yōu)解。上述不等式的對(duì)偶問題可以表示為 maxλ,μ{-bTλ-dTmμ}>dmin，ATλ+CTmμ=0，||ATλ||*≤1，λ?0，μ?0[13]，即：

式中，λ、μ均為拉格朗日變量；||?||*為對(duì)偶范數(shù)。

針對(duì)障礙物，可通過獲得障礙物角點(diǎn)坐標(biāo)求取矩陣Cm、dm。針對(duì)車輛本身，當(dāng)車輛在P(xt,yt)點(diǎn)以航向角φ行駛時(shí)，車輛4個(gè)角點(diǎn)坐標(biāo)分別為：

式中，e1、e3分別為車輛后軸到前、后端的距離；e2為車輛后軸中心至左、右端的距離。

將式（7）代入式（4），通過變換可得：

2.4 優(yōu)化目標(biāo)

文獻(xiàn)[8]針對(duì)自主泊車工況，將總時(shí)長(zhǎng)TF設(shè)為優(yōu)化目標(biāo)，即整個(gè)過程用時(shí)最少、路徑最短。然而最短路徑在實(shí)際工況中可能并非最佳路徑。泊車過程希望在獲得最短路徑以及最小時(shí)長(zhǎng)的前提下，車輛控制輸入應(yīng)盡量小，在實(shí)際工況中表現(xiàn)為轉(zhuǎn)向角度盡量小、加速度盡量小，因此，本文設(shè)定目標(biāo)函數(shù)為：

式中，ut表示每一個(gè)迭代步驟的控制量（前輪轉(zhuǎn)角δ(t)、切向加速度a(t)）；p∈R、q∈R分別為時(shí)間和控制量的權(quán)重，在實(shí)際仿真過程作為經(jīng)驗(yàn)值進(jìn)行適當(dāng)選取。

2.5 內(nèi)點(diǎn)法

內(nèi)點(diǎn)法（Interior-Point Methods）是一類解決線性或非線性凸優(yōu)化問題的算法，通過引入懲罰函數(shù)，將約束問題轉(zhuǎn)換成無約束問題，隨后在迭代過程中不斷更新懲罰函數(shù)，將可行解“固定”在一定區(qū)域內(nèi)，最終得到最優(yōu)解。代表性的內(nèi)點(diǎn)法有障礙法（Barrier Methods）和原始對(duì)偶法（Primal-Dual Methods）。無論面對(duì)線性規(guī)劃（Linear Programming，LP）問題還是二次規(guī)劃（Quadratic Programming，QP）問題，內(nèi)點(diǎn)法在算法復(fù)雜度上都展示了極好的性能。

對(duì)泊車問題的統(tǒng)一優(yōu)化框架包含上述等式約束、不等式約束以及優(yōu)化目標(biāo)，依據(jù)該優(yōu)化框架，以及對(duì)當(dāng)前環(huán)境的完整描述，運(yùn)用內(nèi)點(diǎn)法即可進(jìn)行泊車路徑的求解。

3 仿真驗(yàn)證

3.1 仿真描述

本文基于Ubuntu 16.04.4 LTS環(huán)境Atom JunoLab Plug-in:Julia-client/uber-juno對(duì)提出的方法進(jìn)行仿真驗(yàn)證，程序語言為Julia，處理器為Intel Core i5-7200U，最高睿頻3.1 GHz，內(nèi)存8 GB，DDR4，2 133 MHz。數(shù)值優(yōu)化求解器為開源優(yōu)化庫(kù)IPOPT v3.12.9。仿真車輛模型采用某小型純電動(dòng)轎車的參數(shù)，庫(kù)位大小參照國(guó)家停車場(chǎng)規(guī)劃設(shè)計(jì)規(guī)則，其余具體參數(shù)見表1。

表1 泊車場(chǎng)景相關(guān)參數(shù)

3.2 仿真結(jié)果與討論

仿真泊車場(chǎng)景模擬實(shí)際生活中各種角度的庫(kù)位，與道路夾角分別為0°、30°、45°、60°、90°，仿真結(jié)果如圖3～圖7所示。其中，陰影矩形表示障礙物，在實(shí)際工況中可能表示非目標(biāo)庫(kù)位、對(duì)向車道等。從圖3～圖7可以看出，針對(duì)各種角度的庫(kù)位，本文提出的一致性算法均可成功規(guī)劃出車輛起點(diǎn)至目標(biāo)庫(kù)位的可行路徑，各車輛狀態(tài)與運(yùn)用學(xué)參數(shù)均滿足車輛本身物理要求，滿足了自主泊車“自主”的要求。

圖3 0°庫(kù)位角仿真場(chǎng)景及相應(yīng)運(yùn)動(dòng)學(xué)參數(shù)

圖4 30°庫(kù)位角仿真場(chǎng)景及相應(yīng)運(yùn)動(dòng)學(xué)參數(shù)

3.2.1 實(shí)時(shí)性

由于泊車工況車輛自身速度較低，且周圍通常無其他行駛車輛，與無人車路徑規(guī)劃相比，自主泊車路徑規(guī)劃實(shí)時(shí)性不足通常不會(huì)帶來安全隱患。然而，如果當(dāng)前環(huán)境突然變化等情況發(fā)生，車輛不得不停止并等待計(jì)算出新的路徑，等待時(shí)間過長(zhǎng)勢(shì)必極大影響用車體驗(yàn)。表2給出了5個(gè)場(chǎng)景的計(jì)算總用時(shí)。

由表2可知，針對(duì)不同庫(kù)位角度，規(guī)劃時(shí)間有些許差異，總體上，實(shí)時(shí)性在可接受范圍，但并不能令人滿意。本次模型的一致性，使得問題所需要的求解器強(qiáng)大而可行，由于工具有限，本文采取數(shù)值求解的方法。該方法將連續(xù)變量進(jìn)行離散化處理，變量個(gè)數(shù)為離散點(diǎn)個(gè)數(shù)與系統(tǒng)變量個(gè)數(shù)之積。龐大的變量數(shù)量給提升算法實(shí)時(shí)性帶來不小的挑戰(zhàn)。

圖5 45°庫(kù)位角仿真場(chǎng)景及相應(yīng)運(yùn)動(dòng)學(xué)參數(shù)

圖7 90°庫(kù)位角仿真場(chǎng)景及相應(yīng)運(yùn)動(dòng)學(xué)參數(shù)

表2 不同場(chǎng)景規(guī)劃用時(shí)

眾所周知，對(duì)每一個(gè)優(yōu)化問題來說，初值的選取極為重要。求解的過程是否收斂、求解所需的時(shí)間、求得全局解還是局部解，都與初值的選取息息相關(guān)。理想狀況下，所選取的初值應(yīng)滿足上述所有約束，包括等式約束和不等式約束，然而由于車輛航向角的存在，適當(dāng)?shù)某踔颠x取并不容易。本文首先假設(shè)泊車過程無任何障礙物，將線性離散起始點(diǎn)至目標(biāo)點(diǎn)坐標(biāo)值作為x(t)、y(t)的初值，其余變量φ(t)、v(t)、δ(t)、a(t)各值均置為零，求取該泊車場(chǎng)景的可行解，隨后將該解所對(duì)應(yīng)的所有變量作為初值，求取障礙物存在時(shí)的泊車軌跡。

然而仿真表明，隨意的初值選取，使得算法的實(shí)時(shí)性并不理想。針對(duì)此問題，本文進(jìn)行了進(jìn)一步仿真，將上一個(gè)場(chǎng)景得出的解作為下一個(gè)場(chǎng)景的初值進(jìn)行計(jì)算，如90°庫(kù)位角度得出的解作為60°的初值，60°庫(kù)位角度的解作為45°的初值，依次類推，90°采取最新60°庫(kù)位角求得的解。通過這樣初值選取，仿真時(shí)間如表3所示。

表3 改變優(yōu)化初值后不同場(chǎng)景規(guī)劃用時(shí)

從表3可以看出，重新選取初值后，計(jì)算速度得到了顯著提高。這給實(shí)際泊車提供了一個(gè)新思路：為了提升泊車規(guī)劃實(shí)時(shí)性，可預(yù)先存取一定數(shù)量的解在規(guī)劃器中，針對(duì)實(shí)際具體泊車場(chǎng)景，提取與當(dāng)前場(chǎng)景相近的離線場(chǎng)景所對(duì)應(yīng)的解作為初值，在提高算法實(shí)時(shí)性的同時(shí)也極大地保障了問題的可收斂性。

3.2.2 車輛行為

由圖4～圖6可以看出，車輛泊車過程中，相比于調(diào)整自身位置，車輛希望更快向目標(biāo)庫(kù)位靠近，遇到障礙物后再進(jìn)行轉(zhuǎn)向操作。這與優(yōu)化過程的初值密切相關(guān)，特別是車輛坐標(biāo)初值。依上文所述，泊車過程初值選取的是不存在障礙物情況下的可行解，由于該解無視環(huán)境中障礙物，在當(dāng)前迭代優(yōu)化目標(biāo)的驅(qū)動(dòng)下，軌跡希望更快靠近目標(biāo)庫(kù)位。由此，該初值所得出的最終解通常會(huì)有一段路徑離障礙物很近。實(shí)際工況中，駕駛員可能希望車輛距離障礙物較遠(yuǎn)，然而從數(shù)值優(yōu)化本身的角度看，所求出的路徑滿足不等式約束，且滿足優(yōu)化目標(biāo)的最小值，便是可行解。

4 結(jié)束語

本文驗(yàn)證了自主泊車路徑規(guī)劃一致性方法在仿真環(huán)境中的可行性，尚未進(jìn)行實(shí)車試驗(yàn)。本算法要求車輛當(dāng)前信息與環(huán)境信息準(zhǔn)確可靠，然而實(shí)際情況下，傳感器輸入信息極有可能前、后時(shí)刻不一致。此時(shí)須在當(dāng)前基礎(chǔ)上進(jìn)一步提升算法實(shí)時(shí)性，根據(jù)更新后的輸入信息實(shí)時(shí)求取路徑。

此外，非線性數(shù)值優(yōu)化求解器數(shù)量多達(dá)數(shù)十種[14]，本文僅討論Ipopt使用情況。針對(duì)自主泊車路徑規(guī)劃問題，是否有其他求解器在求解成功率、實(shí)時(shí)性等方面有更佳表現(xiàn)，值得研究。