五次多項式優(yōu)化的平行泊車路徑規(guī)劃

胡勤明，王金剛，張小俊

河北工業(yè)大學 機械工程學院，天津 300132

近幾年，我國的汽車保有量快速增長，但城市的停車位已無法適應(yīng)汽車數(shù)量的快速增長，擁擠的停車場給駕駛員泊車入位帶來了很大的難度。因此，人們對于自動泊車系統(tǒng)的需求越來越強烈。目前，自動泊車技術(shù)主要分為三個方面：環(huán)境感知、路徑規(guī)劃和跟蹤控制。本文將主要從自動泊車路徑規(guī)劃的角度進行研究。

Sungwoo等[1]、張持等[2]采用兩段相切圓弧的方式規(guī)劃平行泊車的路徑；Zhdanov等[3]采用圓弧與直線相結(jié)合的方式規(guī)劃平行泊車路徑，上述兩種泊車路徑規(guī)劃的方簡單、計算效率高，但在切點處存在路徑曲率突變。Qin等[4]利用圓弧-直線進行泊車路徑初次規(guī)劃并使用貝塞爾曲線對初始路徑進行平滑處理。Vorobieva等[5]、張家旭等[6-7]采用回旋曲線對泊車路徑進行平滑處理，避免車輛出現(xiàn)前輪原地轉(zhuǎn)向的情況，并提出了車輛在狹小車位泊車的路徑規(guī)劃方法。李紅等[8]、吳中偉[9]基于車輛運動學模型對泊車碰撞進行了分析，并建立約束方程避免碰撞，然后通過B樣條曲線進行自動泊車路徑規(guī)劃，所得路徑曲線的曲率連續(xù)無突變。胡文等[10]提出了基于駕駛員經(jīng)驗的自動泊車規(guī)劃算法，按駕駛員的操作習慣將泊車路徑分為六段，根據(jù)控制需求對每段路徑進行不同的求解計算，解決原地轉(zhuǎn)向問題。Zhang等[11]、黃江等[12]基于五次多項式對泊車路徑進行優(yōu)化，整條路徑均為多項式曲線，保證了路徑曲率的連續(xù)性。五次多項式路徑曲線不僅光滑、具有連續(xù)的曲率，還能保證加加速度（Jerk）連續(xù)變化。Takahashi等[13]已經(jīng)證明：任何Jerk最優(yōu)化問題中的解都可以使用一個五次多項式來表示。Jerk直接關(guān)系到車內(nèi)人員的舒適度，Jerk連續(xù)平緩的變化可以確保乘客擁有舒適的乘車體驗。上述方法雖然對泊車路徑進行了優(yōu)化，不過還存在一些不足：如B樣條曲線求解復(fù)雜、計算量偏大；整條路徑均為五次多項式曲線的平行泊車路徑需要的泊車位尺寸較大；回旋曲線無法使Jerk連續(xù)平緩變化等。

借鑒以上研究的優(yōu)勢與不足，提出一種五次多項式優(yōu)化的平行泊車路徑規(guī)劃方法。考慮到多項式如果低于五次則無法獲得具有較優(yōu)Jerk的曲線，如果高于五次則會導(dǎo)致計算量偏大，因此選用五次多項式。在自動泊車的過程中，整條路徑均為五次多項式曲線的路徑需要的泊車位尺寸較大，車輛只有保持以較小的轉(zhuǎn)彎半徑不變進行轉(zhuǎn)向，所需的最小平行泊車位尺寸才能較小，所以在“圓弧-直線-圓弧”路徑的基礎(chǔ)上，只對曲率突變處進行平滑過渡，而其他部分仍為直線或圓弧。

1 車輛運動學模型

車輛運動學模型在阿克曼轉(zhuǎn)向幾何的基礎(chǔ)上建立，且將車輛視為剛體。車輛在泊車的過程中，車速一般小于10 km/h，無需考慮車輪的滑移，認為車輪只做滾動和轉(zhuǎn)向運動；如圖1所示，為簡化泊車路徑規(guī)劃時的避障分析，通常將車輛外形簡化成矩形，矩形的大小為車身的最大外形尺寸。

圖1 車輛運動學模型Fig.1 Vehicle kinematics model

由于車輛后輪的運動方向始終與車身的運動方向相同，因此車輛的位置坐標選在車輛后軸中心點(xr,yr)。車輛參數(shù)符號如表1所示。

建立車輛的運動學模型：

式中，xr和yr分別為車輛后軸中心點處的橫、縱坐標；θ為車身航向角；vr為車輛后軸中心點處的速度；ω為車身轉(zhuǎn)動角速度。

為方便描述車輛在某一點處的位置和航向角，定義車輛的位姿狀態(tài)為Sn=(Xn,Yn,θn)。

表1 車輛參數(shù)符號Table 1 Symbols for vehicle parameters

2 多項式過渡曲線

在自動泊車的過程中，若車輛的泊車路徑存在曲率突變點，當車輛勻速經(jīng)過該位置時，理論上需要轉(zhuǎn)向盤在瞬時由當前角度轉(zhuǎn)到另一個角度，但這是不可能實現(xiàn)的，所以路徑的曲率突變點最終會導(dǎo)致車輛的實際行駛路徑與規(guī)劃路徑相比產(chǎn)生較大偏差。

“多項式過渡曲線”的核心是以多項式曲線作為過渡曲線，來保證路徑曲率平滑過渡。如圖2所示，設(shè)過渡曲線的起點O為坐標系原點，則車輛的初始狀態(tài)為S0=(X0,Y0,θ0)=(0,0,0)。如果車輛直接以最小轉(zhuǎn)彎半徑Rmin向左轉(zhuǎn)動，則車輛的轉(zhuǎn)動瞬心為C01=(0,Rmin)。圓C00由圓C01平移得到，其圓心坐標為(XC00,YC00)=(Lccosμ,Lcsinμ+Rmin)?！岸囗検竭^渡曲線”的終點P在圓C00的某一位置，選取合適大小的角λ，并定義為過渡角，過渡角便決定了點P的坐標。另外，設(shè)偏角為ψ，直線OC00的長度為R0。其中R0可以由余弦定理計算得出：

圖2 多項式過渡曲線Fig.2 Polynomial transition curve

偏角ψ可由正弦定理計算得出：

進而可計算出過渡曲線終點P的坐標：

過渡曲線終點P處的切線與X軸之間的夾角為：

則車輛在過渡曲線終點P處的位姿狀態(tài)為SP=(xP,yP,θP)。

設(shè)過渡曲線的表達式為：

式中，k1～k6為過渡曲線表達式的系數(shù)[14]。

設(shè)車輛在過渡曲線起點和終點的坐標分別為(xO,yO)和(xP,yP)，由此可得：

將過渡曲線的表達式（6）對x求導(dǎo)可得：

因為車輛在過渡曲線的起點和終點處的航向角分別為θO和θP，所以可以得到如下約束方程：

過渡曲線還需要保證曲率的連續(xù)性，其中曲率的計算公式為：

根據(jù)曲率計算公式（12）可計算出過渡曲線的曲率方程為：

因為過渡曲線為五次多項式，過渡曲線的曲率必定連續(xù)，但過渡曲線的起點和終點在與其他曲線相切時，曲率也不能存在跳變。圖2中O點可以認為是過渡曲線與水平線相切，P點可以認為是過渡曲線與半徑為Rmin的圓弧相切，兩點處的曲率應(yīng)分別為KO=0和KP=1/Rmin，因此可得出以下約束方程：

將式（7）、（8）、（10）、（11）、（14）和式（15）聯(lián)立便可節(jié)出系數(shù)k1～k6，從而可以得到車輛的過渡曲線。

得出過渡曲線后，車輛可沿過渡曲線切入到半徑為Rmin的圓C00路徑上，當車輛沿圓C00路徑再轉(zhuǎn)過一定角度后，車輛的轉(zhuǎn)向盤還需要回正，這一個過程同樣可以用上文提出的“多項式過渡曲線”。

如圖3所示，為簡化后續(xù)路徑規(guī)劃的復(fù)雜度，以C00為圓心，RO為半徑作圓，此圓弧稱為虛圓路徑，其起點和終點分別為O點和N點。

圖3 虛圓路徑Fig.3 Virtual circle path

其中，在虛圓路徑的起點O和終點N處，車身航向與虛圓路徑切線的夾角為β，其中β=ψ。因此，引入虛圓路徑后，只需要知道車輛的初始狀態(tài)S0、圓心C00的坐標、虛圓半徑R0和虛圓路徑轉(zhuǎn)過的角度φ，就可以快速地由幾何關(guān)系計算出車輛的轉(zhuǎn)彎末狀態(tài)SN，進而根據(jù)上文確定的過渡角λ求出P點和Q點處的車輛位姿狀態(tài)SP和SQ，最終便可由上文的方法解出過渡曲線OP、QN和圓弧曲線PQ。虛圓路徑的引入為實際轉(zhuǎn)彎路徑以及后續(xù)的路徑規(guī)劃提供了便捷，大大提高了計算效率。

如圖4所示的路徑稱為“圓弧-直線-圓弧”平行泊車路徑，以下簡稱“C-L-C”路徑。由于O1O2和O3O4段的轉(zhuǎn)彎半徑固定不變，所以路徑曲率在起點、終點以及圓弧與直線的相切點上均存在突變。本文基于上述“多項式過渡曲線”對“C-L-C”路徑進行優(yōu)化。因為車輛在低速運動的情況下車輪不會產(chǎn)生橫向和縱向滑移，所以規(guī)劃平行泊車路徑時可使用“逆向工程”的方法，即規(guī)劃出一條車輛從平行泊車位駛出的路徑。

圖4“圓弧-直線-圓弧”路徑Fig.4“C-L-C”path

3 最小平行泊車位尺寸計算

3.1 最小平行泊車位長度分析

如圖5所示，設(shè)平行泊車位的4個頂點分別為a、b、c、d，其中d點設(shè)為坐標系的原點。為降低碰撞風險，在車輛四周取長度為Lsafe的安全距離。所以在終點O4處S4=(X4,Y4,θ4)=(Lsafe+Lr,W/2,0)。

圖5 最小平行泊車位長度分析Fig.5 Analysis of minimum parallel parking space length

按“逆向工程”的方法，車輛左轉(zhuǎn)向駛出平行泊車位，當車身的B點與車位的a、d點在同一條直線上時，車輛的轉(zhuǎn)向盤完成回正，車輛將沿直線前進繼續(xù)駛離平行泊車位。在該位置時，車身的B點與平行泊車位的a點的最小距離為Lsafe[12]。若求出B點的坐標(XB,YB)，則平行泊車位的最小長度為：

設(shè)虛圓路徑轉(zhuǎn)過的角度為φ，則O3處車輛的橫、縱坐標及車身航向角分別為：

根據(jù)車輛在O3點處的坐標及車身航向角可得出車身B點的坐標：

將式（16）～（21）聯(lián)立便可解出平行泊車位的最小長度LPmin以及車輛在O3點處的位姿S3=(X3,Y3,θ3)。

3.2 最小平行泊車位寬度分析

平行泊車位的最小寬度取決于車身C點的運動軌跡，即C點軌跡縱坐標的最小值YCmin。車輛從O4點駛出泊車位時，車輛會首先沿過渡曲線行駛，過渡曲線的曲率半徑是由∞逐漸過渡到Rmin，由幾何關(guān)系易知曲率半徑越小，則YCmin的數(shù)值越小。所以，如圖6所示，為了簡化計算，將車輛直接以最小轉(zhuǎn)彎半徑Rmin駛出平行泊車位，車身C點沿曲率半徑為RC的路徑CC′運動，其中曲率半徑RC的大小為：

則路徑最低點的縱坐標為：

所以，最平行小泊車位的寬度為：

圖6 最小平行泊車位寬度分析Fig.6 Analysis of minimum parallel parking space width

4 泊車可行起始區(qū)域分析

4.1 道路上邊界碰撞分析

如圖7所示，車身AD邊易與道路上邊界mn發(fā)生碰撞，因此需要確定泊車起始點O1縱坐標的上極限Y1max，這與分析最小平行泊車位寬度相同，Y1max的大小取決于車身A點軌跡縱坐標的最大值YAmax。同理，為了簡化計算，將車輛直接以最小轉(zhuǎn)彎半徑Rmin行駛，車身A點沿曲率半徑為RA的路徑AA′運動，其中曲率半徑RA的大小為：

則車身A點運動軌跡最高點的縱坐標為：

圖7 道路上邊界碰撞分析Fig.7 Collision analysis of upper road boundary

式中，WR為道路的寬度。

將式（25）與式（26）聯(lián)立可解出車輛泊車起始點O1縱坐標的上極限Y1max。

4.2 道路下邊界碰撞分析

如圖8所示，避免車身BC邊與道路下邊界af發(fā)生碰撞，關(guān)鍵在于O2的坐標。車輛后軸線與車身BC邊的交點記為K，因為K點運動軌跡的曲率半徑最小，所以車輛在從O2點向O1點行駛時，只需要考慮避免車身K點與道路下邊界的a點發(fā)生碰撞。當車輛在O3點時，記此時車身A點的位置為M，車輛沿直線行駛，當車身的K點到達M點位置時，記此時車輛坐標為O2min，即O2點的下極限位置。

圖8 道路下邊界碰撞分析Fig.8 Collision analysis of under road boundary

設(shè)O2min點與O3點間的距離為L32min，車輛在O2min點處的位姿狀態(tài)為S2min=(X2min,Y2min,θ2)，其中θ2=θ3，另外：

M點的縱坐標為：

將式（27）、（28）、（29）聯(lián)立可求解出O2min的坐標。

4.3 確定泊車可行起始區(qū)域

泊車可行起始區(qū)域是由泊車起始極限位置O1min和O1max決定的。在上文分析車身BC邊與道路下邊界af的碰撞情況時，已經(jīng)求解出O2min點的坐標，所以可求出O1下極限O1min點的坐標：

如圖9所示，O2O3段直線路徑與X軸之間的夾角為θ3，而且由幾何關(guān)系易知泊車可行起始區(qū)域的邊界O1maxO2max與X軸之間的夾角也為θ3，所以O(shè)1max點的橫坐標為：

所以，泊車可行起始區(qū)域為：

圖9 泊車可行起始區(qū)域分析Fig.9 Analysis of parking availability starting area

5 仿真驗證

5.1 路徑規(guī)劃仿真

基于以上所提出的路徑規(guī)劃算法以及車輛的運動學約束和碰撞約束，建立平行泊車仿真模型。根據(jù)某車型的尺寸參數(shù)，仿真模型的車輛參數(shù)設(shè)置如表2所示。

表2 車輛參數(shù)Table 2 Vehicle parameters

根據(jù)城市道路工程設(shè)計規(guī)范[15]，平行泊車位的尺寸設(shè)置為7 m×2.4 m，道路寬度設(shè)置為3.5 m。同時，車輛四周的安全距離設(shè)為Lsafe=200 mm。為避免偶然性，選取兩個不同坐標點（10，1.5）和（10，1.7）作為泊車的起始點，進行兩組平行泊車仿真，兩組仿真實驗分別記為A組和B組。

如圖10所示，A、B兩組的優(yōu)化路徑均能夠避免邊界碰撞、完成平行泊車。由圖11（b）和圖12（b）中優(yōu)化路徑與C-L-C路徑的航向角變化對比可知，A、B兩組車輛在倒車泊車的過程中，優(yōu)化路徑的航向角均先發(fā)生變化，而且優(yōu)化路徑的最大航向角要小于C-L-C路徑的最大航向角，因此在圖11（a）和圖12（a）中表現(xiàn)為：優(yōu)化路徑所需的X向距離要略長但路徑更為平緩。圖11（c）和圖12（c）為兩種路徑的曲率隨X坐標的變化對比，從圖中的兩組仿真結(jié)果可以看出，C-L-C路徑的曲率在整個泊車過程中存在四個曲率突變點，而且完成泊車時路徑的曲率未歸零，即車輛前輪仍未回正；優(yōu)化路徑的曲率無突變點，曲率的最大值與最小值之間由一段曲線過渡，可避免因路徑曲率突變所導(dǎo)致路徑跟蹤誤差。

圖10 優(yōu)化路徑泊車仿真效果Fig.10 Parking simulation of optimized path

5.2 路徑跟蹤仿真

為驗證優(yōu)化路徑對路徑跟蹤效果的改善，基于MPC（模型預(yù)測控制）[16-18]算法在Simulink中搭建路徑跟蹤控制模型，車輛的運動速度設(shè)為5 km/h，將CarSim與Simulink進行聯(lián)合仿真，分別對優(yōu)化路徑和C-L-C路徑進行路徑跟蹤，對比兩組仿真實驗中路徑跟蹤的效果。

圖11 A組起點為（10，1.5）的路徑規(guī)劃仿真結(jié)果Fig.11 Path planning simulation results of group A starting point（10，1.5）

圖12 B組起點為（10，1.7）的路徑規(guī)劃仿真結(jié)果Fig.12 Path planning simulation results of group B starting point（10，1.7）

圖13 A組起點為（10，1.5）路徑跟蹤仿真實驗Fig.13 Path tracking simulation results of group A starting point（10，1.5）

觀察圖13（a）和圖14（a）可以發(fā)現(xiàn)，A、B兩組實驗中優(yōu)化路徑的實際路徑與理論路徑吻合度高；而圖13（b）和圖14（b）所示，C-L-C路徑的實際路徑與理論路徑之間產(chǎn)生了極為明顯的偏離。由圖13（c）和圖14（c）可知，A組中C-L-C路徑的跟蹤誤差最大值為208 mm，B組中C-L-C路徑的跟蹤誤差最大值為219 mm，兩組均超出了泊車時的安全距離Lsafe=200 mm，極易發(fā)生碰撞；A組優(yōu)化路徑的跟蹤誤差最大值為9 mm，B組優(yōu)化路徑的跟蹤誤差最大值為12 mm。因此，優(yōu)化路徑的跟蹤效果明顯優(yōu)于C-L-C路徑的跟蹤效果。

圖14 B組起點為（10，1.7）路徑跟蹤仿真實驗Fig.14 Path tracking simulation results of group B starting point（10，1.7）

6 結(jié)語

為解決自動泊車路徑規(guī)劃的曲率突變問題，提出了五次多項式優(yōu)化的平行泊車路徑規(guī)劃方法。使用五次多項式曲線對路徑的曲率突變處進行優(yōu)化，并以“虛圓半徑”作為最小轉(zhuǎn)彎半徑，按照“C-L-C”平行泊車路徑規(guī)劃的方法進行求解，從而得到優(yōu)化路徑。仿真證明：優(yōu)化的路徑規(guī)劃方法能夠規(guī)劃出滿足避障約束和車輛運動學約束的平行泊車路徑，解決了泊車路徑的曲率突變問題，從而提高路徑跟蹤效果，保證平行泊車過程的安全性、可靠性。

雖然優(yōu)化路徑的曲率不會發(fā)生突變，但存在小幅度的波動，下一步可對五次多項式的約束方程進行優(yōu)化，使路徑曲率變化更加平滑；后續(xù)還將針對狹小車位、垂直車位等場景，對本文提出的路徑規(guī)劃方法進行改進與驗證。