張明海 丁寧 曹立波 顏京才 李旭升

（1.湖南大學(xué)，汽車(chē)車(chē)身先進(jìn)設(shè)計(jì)制造國(guó)家重點(diǎn)實(shí)驗(yàn)室，長(zhǎng)沙 410082；2.毫末智行科技有限公司保定分公司，保定 071000）

主題詞：自動(dòng)泊車(chē) 路徑規(guī)劃 直線—圓弧—回旋曲線

1 前言

隨著汽車(chē)保有量的不斷增加，城市交通嚴(yán)重?fù)矶?，泊?chē)環(huán)境越來(lái)越擁擠、復(fù)雜，技術(shù)不足或缺乏經(jīng)驗(yàn)的駕駛員難以將車(chē)輛安全、準(zhǔn)確、快速地泊入車(chē)位。因此，開(kāi)發(fā)安全舒適的自動(dòng)泊車(chē)系統(tǒng)對(duì)泊車(chē)安全具有重要意義[1]。

作為自動(dòng)泊車(chē)系統(tǒng)的組成部分，路徑規(guī)劃模塊是確保泊車(chē)過(guò)程安全舒適的關(guān)鍵[2]。曲線插值是常用的泊車(chē)路徑規(guī)劃方法，能夠?qū)⒙窂接煤瘮?shù)表達(dá)，具有運(yùn)算耗時(shí)短、規(guī)劃路徑基本平滑可控的優(yōu)點(diǎn)，缺點(diǎn)是缺乏靈活性。直線和圓弧組合是一種經(jīng)典的方法，杜賓斯（Dubins）曲線[3]和Reeds-Shepp 曲線[4]可以生成兩點(diǎn)間的最短路徑，包括直線段和圓弧段，前者對(duì)應(yīng)無(wú)倒車(chē)的情況，后者對(duì)應(yīng)有倒車(chē)的情況。一些學(xué)者在此基礎(chǔ)上進(jìn)行了拓展或改進(jìn)：Kim等[5]提出通過(guò)組合不同的圓弧和直線在狹窄環(huán)境中搜索所有可行路徑，該方法較實(shí)用，且停車(chē)路徑可以通過(guò)最小化成本函數(shù)進(jìn)行優(yōu)化；Sung?woo 等[6]將圓弧定位法應(yīng)用于平行停車(chē)，提出了一種完全基于幾何的方法，該方法最大的缺點(diǎn)是在直線和圓弧的連接點(diǎn)處曲率不連續(xù)，為了精確地跟蹤路徑，車(chē)輛必須在連接點(diǎn)停下，重新調(diào)整前輪方向，這將導(dǎo)致不必要的時(shí)間消耗和轉(zhuǎn)向系統(tǒng)磨損。為了避免這些問(wèn)題，學(xué)者們提出了一些解決方法，其中回旋曲線在處理路徑平滑性方面具有獨(dú)特優(yōu)勢(shì)。Vorobieva等[7-8]采用雙圓弧規(guī)劃平行泊車(chē)路徑，然后使用回旋曲線進(jìn)行平滑處理，解決了泊車(chē)過(guò)程中原地轉(zhuǎn)向的問(wèn)題。張家旭等[9]基于直線-圓弧組合方式設(shè)計(jì)平行泊車(chē)路徑，并通過(guò)對(duì)固定回旋曲線的平移和翻轉(zhuǎn)變換完成曲率連續(xù)的平行泊車(chē)路徑規(guī)劃，但使用回旋線時(shí)移動(dòng)了圓弧端點(diǎn)。

本文采用幾何方法，結(jié)合曲線路徑最短和回旋曲線曲率平滑且符合車(chē)輛轉(zhuǎn)彎特性的優(yōu)點(diǎn)，對(duì)垂直車(chē)位的自動(dòng)泊車(chē)路徑規(guī)劃開(kāi)展研究?？紤]開(kāi)始泊車(chē)時(shí)航向角不為零的情況，首先使用直線圓弧組合的方式設(shè)計(jì)基礎(chǔ)的一步和多步泊車(chē)路徑，然后使用回旋線對(duì)圓弧段進(jìn)行優(yōu)化，可以有效消除路徑中曲率突變的不平滑點(diǎn)，使整個(gè)泊車(chē)路徑曲率連續(xù)，避免原地轉(zhuǎn)向，以實(shí)現(xiàn)不同初始位置和姿態(tài)角條件下的垂直泊車(chē)路徑規(guī)劃。

2 垂直泊車(chē)基礎(chǔ)路徑規(guī)劃

2.1 汽車(chē)運(yùn)動(dòng)學(xué)模型

為保證泊車(chē)過(guò)程的準(zhǔn)確性，需要對(duì)車(chē)身輪廓運(yùn)動(dòng)軌跡進(jìn)行計(jì)算，在給予一定裕度的情況下，將車(chē)身輪廓等效為矩形，如圖1所示，圖中L為軸距，Lf、Lr分別為前、后懸，W為車(chē)寬。

圖1 汽車(chē)簡(jiǎn)化模型

由于泊車(chē)過(guò)程車(chē)速較低，可以忽略車(chē)輛側(cè)傾和輪胎滑移的情況。本文采用基于阿克曼轉(zhuǎn)向原理的運(yùn)動(dòng)學(xué)模型，將車(chē)視為平面剛體，選取后軸中心作為參考點(diǎn)，具有x、y、φ3個(gè)自由度，其中(x,y)為車(chē)輛坐標(biāo)，φ為航向角，是車(chē)身與x軸正方向的夾角，逆時(shí)針為正，如圖2所示。

圖2 阿克曼轉(zhuǎn)向模型

車(chē)輛的狀態(tài)方程為：

式中，v為車(chē)速；δ為等效前輪轉(zhuǎn)角。

根據(jù)最大等效前輪轉(zhuǎn)角δmax和軸距L，可求得后軸中心對(duì)應(yīng)的最小轉(zhuǎn)彎半徑Rmin：

2.2 單步泊車(chē)路徑規(guī)劃

車(chē)輛開(kāi)始泊車(chē)時(shí)，首先通過(guò)全景環(huán)視系統(tǒng)檢測(cè)車(chē)位角點(diǎn)P1、P4，在車(chē)位左角點(diǎn)P1處建立坐標(biāo)系。此時(shí)后軸中心位于S1點(diǎn)，以S1為規(guī)劃起點(diǎn)，坐標(biāo)可根據(jù)P1、P4點(diǎn)得到，車(chē)身與車(chē)位的夾角φ即為初始航向角，垂直停車(chē)位尺寸為l×w，如圖3 所示。車(chē)輛先從S1點(diǎn)沿X軸前進(jìn)或后退至S2點(diǎn)，再按最小轉(zhuǎn)彎半徑Rmin行駛至S3點(diǎn)，切換倒擋按Rmin后退至S4點(diǎn)，最后沿直線后退至S5點(diǎn)泊入車(chē)位。圖3中曲線為車(chē)輛后軸中心的軌跡，O0為的圓心，對(duì)應(yīng)的圓心角為θ0，O1為的圓心，對(duì)應(yīng)的圓心角為θ1。

圖3 單步垂直泊車(chē)示意

圖4 單步泊車(chē)碰撞風(fēng)險(xiǎn)點(diǎn)示意

由圖4中幾何關(guān)系可得O1的坐標(biāo)為：

圖3中O0的Y坐標(biāo)為：

在圖3中，對(duì)于ΔO0O1N，有：

由此可得O0的X坐標(biāo)為：

S2、S3、S4的坐標(biāo)可以通過(guò)式（4）～（7）求得：

實(shí)際泊車(chē)時(shí)，起始點(diǎn)可能相對(duì)于S2點(diǎn)偏前，須先后退一段距離，如圖5所示。此外，初始航向角φ一般不為0°，泊車(chē)過(guò)程需先通過(guò)一段圓弧，將車(chē)輛調(diào)整到與X軸平行的位置，再按照以上方法進(jìn)行泊車(chē)，圖6 所示為初始航向角不為零的泊車(chē)路徑。

圖5 起始點(diǎn)偏右時(shí)泊車(chē)路徑示意

圖6 初始角度φ≠0°的泊車(chē)路徑示意

2.3 多步泊車(chē)路徑規(guī)劃

單步泊車(chē)所需空間較大，垂直泊車(chē)場(chǎng)景常遇到泊車(chē)空間不足的情況，比如道路較窄或?qū)ο蜍?chē)位停有車(chē)輛。如圖7 所示，車(chē)輛輪廓左前點(diǎn)A1與上邊界會(huì)發(fā)生碰撞。此時(shí)單步泊車(chē)難以實(shí)現(xiàn)，需采用多步泊車(chē)的方法完成。從圖7中可以看出，引起碰撞的原因是一步泊車(chē)路徑中S3點(diǎn)的Y坐標(biāo)過(guò)大，因此，通過(guò)約束YS3即可避免碰撞。

圖7 車(chē)輛與上邊界碰撞示意

圖8 為車(chē)輛在S3點(diǎn)以及行駛至A點(diǎn)的泊車(chē)示意，為避免碰撞，車(chē)輛左前點(diǎn)A1與上邊界需保留安全距離，將J與A1的距離定為安全距離Lsafe2。

圖8 多步垂直泊車(chē)空間約束

由圖8中幾何關(guān)系可得：

由圖9和圖10中幾何關(guān)系有：

圖9 多步泊車(chē)路徑示意

圖10 S4點(diǎn)計(jì)算示意

設(shè)直線EF的解析式為y1=kx+b，則：

由式（19）～（22）可得到θ2、θ3的值以及O2、O3、S5、S6的坐標(biāo)。

3 垂直泊車(chē)路徑規(guī)劃算法優(yōu)化

前文設(shè)計(jì)的路徑規(guī)劃方法由直線和圓弧組成，可以完成泊車(chē)任務(wù)。但由于曲率不連續(xù)，要使車(chē)輛沿規(guī)劃的路徑行駛，必須在直線和圓弧連接處停車(chē)原地轉(zhuǎn)向，這會(huì)造成轉(zhuǎn)向機(jī)構(gòu)及輪胎的磨損，還會(huì)延長(zhǎng)泊車(chē)時(shí)間。因此，需要對(duì)第2節(jié)中的路徑進(jìn)行優(yōu)化?；匦€可以很好地描述車(chē)輛行駛時(shí)轉(zhuǎn)向的行為，如果車(chē)輛以恒定的速度運(yùn)動(dòng)，并以恒定的角速度轉(zhuǎn)向，則后軸中心駛過(guò)的軌跡即為回旋線?；匦€具有曲率連續(xù)變化的特點(diǎn)[10]，滿足泊車(chē)路徑對(duì)曲率平滑的要求。本文使用回旋曲線對(duì)基礎(chǔ)路徑進(jìn)行優(yōu)化。

3.1 回旋曲線

回旋曲線是曲率K隨其弧長(zhǎng)s線性變化的曲線，可表示為：

式中，σ為曲率變化率。

車(chē)輛從起點(diǎn)O開(kāi)始，初始狀態(tài)為q0=(x0，y0，φ0，δ0)=(0，0，0，0)，x0、y0、φ0、δ0分別為車(chē)輛的初始x坐標(biāo)、y坐標(biāo)、車(chē)身航向角、曲率，以恒定的縱向速度和恒定的轉(zhuǎn)向速度向左行駛，直至點(diǎn)E，轉(zhuǎn)向盤(pán)到達(dá)最大角度，此時(shí)轉(zhuǎn)彎半徑為Rmin，車(chē)輛后軸中心駛過(guò)的軌跡如圖11所示。距起始點(diǎn)s處車(chē)輛的姿態(tài)信息為：

圖11 回旋曲線

式中，A為回旋曲線參數(shù)，A2=RL=1/σ；Cf、Sf為菲涅爾積分：

圓心C的坐標(biāo)為：

由圖11可得：

為充分利用車(chē)輛的最大轉(zhuǎn)向角并縮小泊車(chē)空間，回旋曲線長(zhǎng)度S應(yīng)盡可能小?；匦€長(zhǎng)度與車(chē)速及轉(zhuǎn)向速度有關(guān)：

式中，tmin為轉(zhuǎn)向盤(pán)從0°轉(zhuǎn)到最大角度所需時(shí)間；vδ為最大轉(zhuǎn)向速度；vmax為泊車(chē)過(guò)程中的最大車(chē)速。

所用回旋線參數(shù)定為A2min=RminSmin，R1由式（25）～式（28）計(jì)算，αc為回旋線對(duì)應(yīng)的角度。

3.2 連續(xù)曲率路徑優(yōu)化

第2節(jié)規(guī)劃的路徑中，存在的問(wèn)題是直線與圓弧處曲率突變（從0 突變到1/R），且圓弧段起始曲率為1/R，這會(huì)給泊車(chē)過(guò)程帶來(lái)困難。希望車(chē)輛每次前進(jìn)/后退的路徑曲率都是從0開(kāi)始，并以0結(jié)束，且每一段路徑曲率都是連續(xù)的。優(yōu)化的思路是結(jié)合回旋線，將每一段圓弧都調(diào)整為“回旋線-圓弧-回旋線”（CAC）組合的曲線。

定義CAC 組合曲線包括：參數(shù)為A的回旋線，長(zhǎng)度S，初始曲率為0，末端曲率為1/R；半徑為R，角度為θ的圓?。粎?shù)為A的回旋線，長(zhǎng)度L，初始曲率為1/R，末端曲率為0。

針對(duì)第2節(jié)的結(jié)果，將最小半徑Rmin替換為R1，使用回旋曲線將半徑為R1的圓弧過(guò)渡到半徑為Rmin的圓弧。此外，需將圓弧以S2為中心旋轉(zhuǎn)角度μ，從而使優(yōu)化后的航向角連續(xù)，如圖12所示。

圖12 回旋曲線優(yōu)化的泊車(chē)路徑示意

對(duì)于不同的圓心角度α，轉(zhuǎn)化為回旋曲線-圓弧-回旋曲線（CAC 序列），考慮以下2 種情況：當(dāng)α≥2αc時(shí)，使用完整的CAC 序列，如圖13a 所示；當(dāng)α<2αc時(shí)，使用2段回旋線（CC 序列）進(jìn)行優(yōu)化，如圖13b 所示?；匦€對(duì)應(yīng)的參數(shù)Anew、Lnew將有所變化：

圖13 回旋曲線優(yōu)化圓弧的2種情況

式中，Δφ=(φend-φstart)為汽車(chē)轉(zhuǎn)過(guò)的角度。

4 仿真驗(yàn)證

本文針對(duì)某品牌SUV 汽車(chē)展開(kāi)試驗(yàn)，汽車(chē)相關(guān)參數(shù)如表1 所示，采用尺寸為2.5 m×6 m 的垂直車(chē)位。通過(guò)MATLAB對(duì)垂直泊車(chē)進(jìn)行仿真。

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

圖14 所示為汽車(chē)單步泊車(chē)規(guī)劃的路徑，圖14a中，車(chē)輛的初始坐標(biāo)為(-2,2)，初始航向角為0°；圖14b中，初始坐標(biāo)為(-3,2)，初始航向角為-5°。對(duì)于不同的初始位置和角度，本文的方法可以規(guī)劃出直線-圓弧-回旋曲線組合的安全平滑泊車(chē)路徑，但所需泊車(chē)空間較大。

圖14 單步泊車(chē)仿真結(jié)果

為確保泊車(chē)過(guò)程的安全性并提高路徑規(guī)劃的適用性，根據(jù)實(shí)際泊車(chē)場(chǎng)景，將橫向泊車(chē)距離限制為6 m。圖15 為在不同的初始坐標(biāo)和初始航向角下，規(guī)劃的多步泊車(chē)路徑均能避免碰撞且平滑。

圖15 多步泊車(chē)仿真結(jié)果

5 結(jié)束語(yǔ)

本文對(duì)垂直車(chē)位自動(dòng)泊車(chē)場(chǎng)景，規(guī)劃了具有不同初始航向角的平滑泊車(chē)路徑。首先，根據(jù)車(chē)輛初始姿態(tài)以及泊車(chē)空間，考慮運(yùn)動(dòng)學(xué)約束和碰撞約束，設(shè)計(jì)基于直線-圓弧組合的單步和多步垂直泊車(chē)基礎(chǔ)路徑規(guī)劃方法。然后，對(duì)于連接點(diǎn)曲率不平滑的問(wèn)題，結(jié)合回旋曲線對(duì)基礎(chǔ)路徑規(guī)劃結(jié)果進(jìn)行優(yōu)化，實(shí)現(xiàn)了泊車(chē)路徑的曲率平滑，有效避免了泊車(chē)過(guò)程中的原地轉(zhuǎn)向。基于MATLAB 的仿真結(jié)果表明，對(duì)于不同的初始航向角，該方法能規(guī)劃出安全無(wú)碰撞的平滑軌跡，驗(yàn)證了安全性和可行性。