小車(chē)位環(huán)境下的自動(dòng)泊車(chē)路徑規(guī)劃與跟蹤控制

馮欣陽(yáng)，李春書(shū)，唐風(fēng)敏

河北工業(yè)大學(xué) 機(jī)械工程學(xué)院，天津 300401

隨著我國(guó)汽車(chē)保有量的增加，一些問(wèn)題也隨之顯現(xiàn)，泊車(chē)可用空間的減小便是其中之一。狹小車(chē)位環(huán)境下的泊車(chē)需要豐富的駕駛經(jīng)驗(yàn)及嫻熟的駕駛技術(shù)，無(wú)形之中增加了駕駛?cè)藛T的操作難度和操作體驗(yàn)。目前，智能車(chē)輛的自動(dòng)泊車(chē)研究是許多研究者們關(guān)注的問(wèn)題之一[1-3]。自動(dòng)泊車(chē)包括環(huán)境感知、泊車(chē)控制、人機(jī)交互等技術(shù)。

泊車(chē)控制主要有兩種方法：人工智能方法和運(yùn)動(dòng)狀態(tài)規(guī)劃及軌跡跟蹤。人工智能方法通過(guò)智能算法使控制器在泊車(chē)過(guò)程中模擬駕駛員的操作行為[3]。文獻(xiàn)[4-9]通過(guò)蟻群算法、神經(jīng)網(wǎng)絡(luò)、模糊控制等智能算法，實(shí)現(xiàn)泊車(chē)操作；由于實(shí)際泊車(chē)環(huán)境的復(fù)雜性，駕駛員需要面對(duì)不同的泊車(chē)環(huán)境做出不同反應(yīng)，增加了人工智能方法的實(shí)現(xiàn)難度。一些研究者提出基于路徑規(guī)劃及路徑跟蹤的泊車(chē)控制方法[10-18]。研究者們對(duì)于泊車(chē)路徑規(guī)劃的研究大多采用曲線組合或多項(xiàng)式插值的方式[11-14]。此外，有些研究者也采用貝塞爾曲線、B樣條曲線設(shè)計(jì)泊車(chē)路徑[15-16]。路徑跟蹤采用滑模控制、自適應(yīng)控制、模型預(yù)測(cè)控制等多種跟蹤控制方法[12-18]。

上述泊車(chē)路徑規(guī)劃的研究均是針對(duì)一次泊入到位路徑進(jìn)行設(shè)計(jì)實(shí)現(xiàn)泊車(chē)的；若車(chē)位長(zhǎng)度較小，不能實(shí)現(xiàn)一次泊入到位，車(chē)輛需要在車(chē)位內(nèi)進(jìn)行位姿調(diào)整。為方便描述，將需要在車(chē)位內(nèi)進(jìn)行位姿調(diào)整的車(chē)位稱(chēng)為小車(chē)位。目前，一些研究者已針對(duì)小車(chē)位環(huán)境自動(dòng)泊車(chē)的路徑規(guī)劃進(jìn)行研究。文獻(xiàn)[19]通過(guò)建立預(yù)定義模態(tài)集，采用遍歷策略，計(jì)算最優(yōu)參數(shù)，對(duì)泊入段、調(diào)整段路徑進(jìn)行規(guī)劃；但模態(tài)集的建立需要考慮多種情況，計(jì)算量較大。文獻(xiàn)[20]通過(guò)B 樣條曲線建立泊入路徑及調(diào)整路徑；該調(diào)整路徑曲率連續(xù)，但是B樣條曲線復(fù)雜程度高，求解過(guò)程較為困難。文獻(xiàn)[21]根據(jù)車(chē)位長(zhǎng)度及車(chē)輛當(dāng)前航向角，通過(guò)多次揉庫(kù)，實(shí)現(xiàn)狹小車(chē)位內(nèi)的姿態(tài)調(diào)整；該方法耗時(shí)長(zhǎng)、計(jì)算成本高。文獻(xiàn)[22]通過(guò)三段式路徑規(guī)劃實(shí)現(xiàn)平行車(chē)位內(nèi)的位姿調(diào)整；調(diào)整路徑通過(guò)前進(jìn)圓弧路段實(shí)現(xiàn)位姿調(diào)整，但該方法在換向時(shí)，曲率不連續(xù)且所需車(chē)位較大。文獻(xiàn)[23]利用曲線組合設(shè)計(jì)了泊入路徑和調(diào)整路徑，采用相切圓弧實(shí)現(xiàn)車(chē)位內(nèi)的倒車(chē)位姿調(diào)整，其調(diào)整路徑存在曲率突變的不足。

為使泊車(chē)路徑曲率連續(xù)無(wú)突變且計(jì)算方便，利用回旋曲線曲率連續(xù)的特點(diǎn)，將回旋曲線、直線、圓弧曲線組合設(shè)計(jì)形成平滑的泊入段路徑和調(diào)整段路徑；在非時(shí)間參考變量的前提下結(jié)合滑模控制原理，設(shè)計(jì)路徑跟蹤控制器，實(shí)現(xiàn)泊車(chē)路徑的跟蹤控制。

1 平行車(chē)位泊車(chē)路徑規(guī)劃

1.1 小車(chē)位環(huán)境及路徑分析

當(dāng)車(chē)輛側(cè)向泊入平行車(chē)位時(shí)，停泊時(shí)的標(biāo)準(zhǔn)位姿通常如參考文獻(xiàn)[24]所描述的狀態(tài)，如圖1所示。ab和dc分別代表車(chē)輛前端和后端；Lf、L、Lr、Wa分別代表前懸、軸距、后懸、車(chē)寬；Lp、Wp分別代表車(chē)位的長(zhǎng)度與寬度；Db、Dr分別代表車(chē)輛后端、右端與車(chē)位障礙物邊界之間的安全距離。

圖1 車(chē)輛泊車(chē)位姿示意圖Fig.1 Diagram of vehicle standard position and pose

如圖2 所示，建立大地坐標(biāo)系OXY，O為固定原點(diǎn)，障礙物邊界點(diǎn)A與原點(diǎn)O重合，泊車(chē)初始位姿如圖2 中I 位置所示，車(chē)輛的初始位置、初始航向角分別為C1、θ0，選擇車(chē)輛后軸的中心點(diǎn)作為路徑跟蹤的參考點(diǎn)。

圖2 小車(chē)位下的泊入路徑Fig.2 Parking path under small parking space

泊入路徑由回旋曲線C1-C2、圓弧曲線C2-C3、回旋曲線C3-C4、直線C4-C5、回旋曲線C5-C6、圓弧曲線C6-P組成；車(chē)輛的極限位置為圖2中III位置所示。

調(diào)整路徑由前進(jìn)路段和后退路段組成，前進(jìn)路段由回旋曲線P-P1、回旋曲線P1-P2、圓弧曲線P2-P3、回旋曲線P3-P4、回旋曲線P4-P5、圓弧曲線P5-P6、回旋曲線P6-P7組成；P-P1、P1-P2為兩段反向回旋曲線，P3-P4、P4-P5為兩段反向回旋曲線；后退路段由直線P7-P8組成；調(diào)整路徑如圖3所示。

圖3 小車(chē)位內(nèi)調(diào)整路徑Fig.3 Adjust path in small parking space

設(shè)Rmin為車(chē)輛最小轉(zhuǎn)彎半徑，泊車(chē)路徑的最大曲率不超過(guò)ρmax=1Rmin；在泊車(chē)過(guò)程中為避免車(chē)身與障礙物發(fā)生碰撞，需要滿(mǎn)足以下條件：（1）泊入階段，車(chē)輛右前頂點(diǎn)b與A點(diǎn)不發(fā)生碰撞（記為Cons G）；車(chē)輛右側(cè)bc與A點(diǎn)不發(fā)生碰撞。（2）調(diào)整階段，車(chē)輛右前頂點(diǎn)b與車(chē)位BC邊界不發(fā)生碰撞；車(chē)輛前端ab與車(chē)位AB邊界的安全距離為Ds。

1.2 泊入路徑的設(shè)計(jì)

設(shè)回旋曲線的曲率變化率為k，初始點(diǎn)為固定原點(diǎn)O，初始方位角為零，回旋曲線上曲率為ρ的i點(diǎn)坐標(biāo)及方位角αi為：

設(shè)圓弧曲線的半徑為R，圓心角為θ，起始點(diǎn)為固定原點(diǎn)O，初始方位角為零，圓弧曲線上對(duì)應(yīng)圓心角為θ的j點(diǎn)坐標(biāo)為：

xj=Rsinθ（4）

yj=R(1-cosθ) （5）

回旋曲線C1-C2曲率變化率為kC12，C1-C2曲線構(gòu)造原理如圖4 所示，由式（1）、（2）得到初始回旋曲線O-C2*，C2*處的方位角為αC2*由式（3）求得，初始曲線如I所示。將曲線O-C2*逆時(shí)針旋轉(zhuǎn)π+θ0，如位置II所示，沿向量OC1平移至O和C1重合，得到回旋曲線C1-C2，如位置III所示。

圖4 C1-C2 曲線構(gòu)造原理Fig.4 Construction principle of C1-C2 curve

圓弧曲線C2-C3的曲率為ρC23，圓心角為θC23，初始圓弧曲線O-C3*由式（4）和（5）求得，將其逆時(shí)針旋轉(zhuǎn)αC2*+θ0+π，沿向量OC2平移至O與C2重合，得到圓弧曲線C2-C3，C3點(diǎn)處的方位角為αC3=θ0+αC2*+θC23。

同理，其余回旋曲線和圓弧曲線也類(lèi)似求得。回旋曲線C3-C4的曲率變化率為kC34，初始回旋曲線為OC3′，C3′處的方位角為αC3′。設(shè)P點(diǎn)的方位角為θ1。圓弧曲線C6-P的曲率為ρC6P，圓心角為θC6P，初始圓弧曲線為O-C6*；回旋曲線C5-C6的曲率變化率為kC56，初始回旋曲線為O-C6′，C6′處的方位角為αC6′；連接C4、C5得到直線C4-C5。

上述圓弧曲線的半徑均取Rmin，滿(mǎn)足θ1+θC6P+αC6′=αC3+αC3′。

泊入運(yùn)動(dòng)中允許的最大車(chē)速vmax1=3 km/h，曲率變化率的最大值kmax1=wmax2，wmax代表前輪最大轉(zhuǎn)向角速度。

泊入路徑中直線C4-C5長(zhǎng)度LC45最小為優(yōu)化目標(biāo)，泊入路徑規(guī)劃轉(zhuǎn)換為非線性規(guī)劃：式中，第一個(gè)不等式表示車(chē)輛右前頂點(diǎn)b與A點(diǎn)不發(fā)生碰撞；第二個(gè)不等式為直線C4-C5存在性條件；第三個(gè)不等式表示車(chē)輛bc側(cè)與A點(diǎn)不發(fā)生碰撞；第四個(gè)等式表示直線C4-C5分別與回旋曲線C5-C6、回旋曲線C3-C4曲線相切。

1.3 調(diào)整段路徑設(shè)計(jì)

為了方便設(shè)計(jì)，將圓弧曲線P2-P3分為P2-Q和Q-P3兩段圓弧，Q點(diǎn)切線水平，如圖3（b）所示。

回旋曲線P6-P7的曲率變化率為kP67，初始回旋曲線為O-P6*，P6*處的方位角為αP6*；圓弧曲線P5-P6的曲率為ρP56，圓心角為θP56，初始圓弧曲線為O-P5*；回旋曲線P4-P5的曲率變化率為kP45，初始回旋曲線為O-P5′，P5′處的方位角為αP5′；回旋曲線P3-P4的曲率變化率為kp34，初始回旋曲線為O-P3*，P3*處的方位角為αP3*；圓弧曲線Q-P3的圓心角為θ33，曲率為ρQP3，初始圓弧曲線為O-P3′；回旋曲線P-P1的曲率變化率為kPP1，初始回旋曲線為O-P*，P*處的方位角為αP*；回旋曲線P1-P2的曲率變化率為kP12，初始回旋曲線為O-P2*，P2*處的方位角為αP2*；圓弧曲線P2-Q的圓心角為θ32，曲率為ρP2Q，初始圓弧曲線為O-P2′。

連接P7-P8，P8點(diǎn)為圖1所示參考點(diǎn)的位置。

圓弧曲線P5-P6的半徑取為Rmin。在調(diào)整路徑中，為保證車(chē)輛能夠跟隨路徑，允許的最大車(chē)度vmax2=0.75 km/h，P6-P7選擇曲率變化率的最大值kmax2=2wmax。將P與P7點(diǎn)的橫向距離LXPP7最小作為優(yōu)化目標(biāo)，調(diào)整路徑規(guī)劃轉(zhuǎn)換為非線性規(guī)劃：

式中，第一個(gè)等式限定P點(diǎn)的位置關(guān)系；第二個(gè)不等式表示圓弧路徑P2-P3存在性條件；第三個(gè)不等式表示車(chē)輛b點(diǎn)與障礙物BC側(cè)不發(fā)生碰撞；第四個(gè)不等式表示車(chē)輛ab側(cè)與障礙物AB側(cè)不發(fā)生碰撞。

2 小車(chē)位下的泊車(chē)路徑的跟蹤控制

在泊車(chē)過(guò)程中車(chē)輛以低速行駛，根據(jù)阿克曼轉(zhuǎn)向原理，車(chē)輛的運(yùn)動(dòng)學(xué)模型為：

式中，(x,y)代表參考點(diǎn)坐標(biāo)，v代表車(chē)速，φ代表車(chē)輛航向角，δ代表車(chē)輛的前輪轉(zhuǎn)向角。

傳統(tǒng)路徑跟蹤的方法是將路徑視作時(shí)間參考函數(shù)，若車(chē)輛跟蹤過(guò)程中出現(xiàn)誤差，這種誤差會(huì)隨著時(shí)間逐漸增加，甚至?xí)苯訉?dǎo)致車(chē)輛失去跟蹤能力[25]。引入f=-x做為非時(shí)間參考量，車(chē)輛的運(yùn)動(dòng)學(xué)方程可表示為：

圖5 路徑跟蹤偏差示意圖Fig.5 Diagram of path tracking deviation

結(jié)合式（8）、（9）、（10），基于非時(shí)間參考變量的跟蹤偏差為：

3 仿真分析

車(chē)輛的基本參數(shù)如表1所示。

表1 車(chē)輛基本參數(shù)Table 1 Basic vehicle parameters

泊車(chē)路徑跟蹤效果如圖6所示。泊車(chē)過(guò)程中，汽車(chē)外輪廓與障礙物邊界之間的相對(duì)位置如圖7所示。

圖7 汽車(chē)外輪廓包絡(luò)圖Fig.7 Envelope diagram of automobile outline

由圖6、7 可知，車(chē)輛行駛路徑與規(guī)劃路徑基本重合，汽車(chē)外輪廓的包絡(luò)在行駛過(guò)程中未與周邊障礙物產(chǎn)生碰撞，滿(mǎn)足車(chē)輛避障約束及泊車(chē)要求等。

圖6 路徑跟蹤效果圖Fig.6 Diagram of path tracking

泊車(chē)過(guò)程中的縱向誤差如圖8所示。圖8（a）、（b）、（c）分別表示泊入路徑、前進(jìn)調(diào)整路段和后退路段的縱向誤差。

圖8 路徑跟蹤過(guò)程中的縱向誤差Fig.8 Longitudinal error in path tracking

由圖8可知，泊入路段中的最大縱向誤差為0.005 4 m；調(diào)整路徑中的前進(jìn)路段的最大縱向誤差為0.002 2 m，后退路段最大縱向誤差為0.002 1 m，泊車(chē)終止點(diǎn)的縱向誤差為0.002 1 m。

泊車(chē)過(guò)程中的航向誤差如圖9所示。圖9（a）、（b）、（c）分別代表泊入路段、前進(jìn)調(diào)整路段和后退調(diào)整路段的航向誤差。

由圖9可知，泊入路徑中的最大航向誤差為0.114 6°；調(diào)整路徑中的前進(jìn)路段中的最大航向誤差為0.401 1°，后退路段最大航向誤差為0.383 9°，泊車(chē)終止點(diǎn)的航向角誤差為0.383 9°。

圖9 路徑跟蹤過(guò)程中的航向誤差Fig.9 Course angle error in path tracking

仿真過(guò)程中車(chē)輛的實(shí)際前輪轉(zhuǎn)角如圖10所示。圖10（a）、（b）、（c）分別為泊入路段、前進(jìn)調(diào)整路段、后退調(diào)整路段在不同函數(shù)作用下的前輪轉(zhuǎn)角。

圖10 不同函數(shù)作用下的前輪轉(zhuǎn)角Fig.10 Front wheel rotation angle under different functions

由圖10知，車(chē)輛在路徑跟隨過(guò)程中，前輪轉(zhuǎn)角無(wú)突變，車(chē)輛行駛平滑；連續(xù)函數(shù)作用下的滑模控制器相比符號(hào)函數(shù)作用下的滑模控制器較大的抑制了高頻抖振。

由圖6～10可知，在滑模控制器的作用下，車(chē)輛與周?chē)系K物未發(fā)生碰撞，最大縱向誤差為0.005 4 m，最大航向誤差為0.401 1°，跟蹤誤差較小；全程前輪轉(zhuǎn)角變化平滑，抖振抑制效果良好。

4 結(jié)論

對(duì)于小車(chē)位環(huán)境泊車(chē)難的問(wèn)題，設(shè)計(jì)了泊入路徑和調(diào)整路徑，將回旋曲線、直線、圓弧曲線進(jìn)行組合規(guī)劃，實(shí)現(xiàn)了不同曲率路徑之間的平滑過(guò)渡，該規(guī)劃方法具有計(jì)算簡(jiǎn)便、曲率無(wú)突變的優(yōu)點(diǎn)。基于非時(shí)間參考誤差設(shè)計(jì)滑模控制器，將指數(shù)趨近率中的符號(hào)函數(shù)替換為連續(xù)函數(shù)，改善了泊車(chē)過(guò)程中的高頻抖振問(wèn)題。仿真結(jié)果表明，該路徑規(guī)劃方法滿(mǎn)足小車(chē)位泊車(chē)的要求，前輪轉(zhuǎn)角無(wú)突變、跟蹤誤差小，較好地解決了小車(chē)位泊車(chē)難、位姿調(diào)整難的問(wèn)題。