黃江 魏德奎 秦良艷 李攀 杜軍

（1.重慶理工大學(xué)，重慶 400054；2.重慶郵電大學(xué)，重慶 400065）

主題詞：自動(dòng)泊車 滑?？刂?路徑規(guī)劃 五次多項(xiàng)式 路徑跟蹤

1 前言

目前，自動(dòng)泊車系統(tǒng)的研究?jī)?nèi)容主要包括路徑規(guī)劃、路徑跟蹤、車位探測(cè)與定位3個(gè)方向。針對(duì)泊車路徑規(guī)劃問題：文獻(xiàn)[1]、文獻(xiàn)[2]利用2段相切的圓弧進(jìn)行路徑規(guī)劃，但規(guī)劃路徑存在曲率不連續(xù)的問題；文獻(xiàn)[3]指出，車速和轉(zhuǎn)向盤轉(zhuǎn)角角速度恒定時(shí)，車輛行駛軌跡將會(huì)固定，并在此基礎(chǔ)上對(duì)雙圓弧軌跡進(jìn)行改進(jìn)，提出了基于雙勻速軌跡的平行泊車路徑規(guī)劃方法，解決了傳統(tǒng)的雙圓弧泊車路徑曲率不連續(xù)的問題；文獻(xiàn)[4]～文獻(xiàn)[6]對(duì)泊車避障環(huán)境進(jìn)行分析，建立了約束方程，并采用B樣條曲線進(jìn)行路徑規(guī)劃設(shè)計(jì)，所得路徑滿足曲率連續(xù)要求，但是約束條件和路徑曲線參數(shù)較多，求解過程較為復(fù)雜；文獻(xiàn)[7]為避免自動(dòng)泊車過程中進(jìn)入泊車死區(qū)，設(shè)計(jì)了滿足安全駛?cè)胲囄坏牟窜嚽斑M(jìn)水平線，并以此作為起始區(qū)域進(jìn)行泊車規(guī)劃，保證了泊車的安全性，但沒有考慮曲率連續(xù)性要求的問題。針對(duì)泊車路徑跟蹤問題：文獻(xiàn)[8]～文獻(xiàn)[10]采用模糊控制的方法進(jìn)行泊車控制；文獻(xiàn)[11]基于非時(shí)間參考系路徑跟蹤控制設(shè)計(jì)了非時(shí)間參考的定點(diǎn)跟蹤控制方法，提高了泊車的精確度；文獻(xiàn)[12]利用二次型最優(yōu)控制方法進(jìn)行了泊車控制器設(shè)計(jì)，完成了對(duì)泊車軌跡的有效跟蹤。

自動(dòng)泊車過程要求泊車軌跡曲率連續(xù)、泊車控制器控制精度高，以實(shí)現(xiàn)泊車過程的連貫與安全。本文以逆向行駛的方法進(jìn)行泊車路徑的初步規(guī)劃，并利用五次多項(xiàng)式對(duì)路徑進(jìn)行平滑處理，同時(shí)，為了提高平行泊車控制器的控制精度，以滑?？刂茷榛A(chǔ)，結(jié)合非時(shí)間參考控制方法，提出一種基于非時(shí)間參考的滑模控制路徑跟蹤方法，以實(shí)現(xiàn)泊車系統(tǒng)高精度的路徑跟蹤效果。

2 車輛運(yùn)動(dòng)學(xué)模型

汽車的外形尺寸直接影響泊車過程中避障的成功率，為了在泊車避障路徑規(guī)劃過程中留下一定裕度的同時(shí)簡(jiǎn)化汽車運(yùn)動(dòng)學(xué)分析，以矩形代替車輛外形，其長(zhǎng)度和寬度分別為汽車外形尺寸的最大長(zhǎng)度和最大寬度，如圖1所示。通常，泊車過程中汽車的速度維持在較低水平，車輛不會(huì)出現(xiàn)明顯的側(cè)傾或俯仰運(yùn)動(dòng)，因此簡(jiǎn)化模型不會(huì)對(duì)分析結(jié)果產(chǎn)生較大影響。

圖1 車輛外形簡(jiǎn)化模型

圖1中，A、B、C、D為汽車外形頂點(diǎn)，車輛參數(shù)如表1所示。

表1 車輛外形參數(shù)

由于后輪為非轉(zhuǎn)向輪，故可選取汽車后軸中心點(diǎn)為參考點(diǎn)，以其運(yùn)動(dòng)軌跡表示汽車的行駛軌跡，以車位左后方頂點(diǎn)d為原點(diǎn)，以平行車位方向?yàn)閄軸，以車位寬度方向?yàn)閅軸建立坐標(biāo)系，建立運(yùn)動(dòng)學(xué)模型：

式中，x(t)、y(t)分別為汽車后軸中心點(diǎn)橫、縱坐標(biāo)；φ(t)為汽車航向角；δ(t)為前輪等效轉(zhuǎn)角；v(t)為車速。

3 平行泊車路徑規(guī)劃算法

3.1 平行泊車初步路徑規(guī)劃

汽車從泊車起始點(diǎn)O1泊車至終點(diǎn)O4的過程如圖2所示，可分為3個(gè)階段：首先假定車輛初始姿態(tài)平行于車位方向，目標(biāo)車輛從初始位置O1點(diǎn)處以R1為半徑、M1為圓心行駛θ角度后到達(dá)O2點(diǎn)；接著沿直線段O2O3行駛至O3點(diǎn)；最后從O3點(diǎn)處以R2為半徑、M2點(diǎn)為圓心行駛θ角度后到達(dá)O4點(diǎn)，泊車完成。

圖2 泊車過程

為方便計(jì)算，并減小可泊車車位長(zhǎng)度，本文將R2和R1設(shè)定為車輛后軸中點(diǎn)的最小轉(zhuǎn)彎半徑Rmin，Rmin由轉(zhuǎn)向運(yùn)動(dòng)學(xué)關(guān)系得到：

利用逆向行駛的方法推算泊車起始點(diǎn)的范圍，根據(jù)避障條件確定O4、O3坐標(biāo)，只需調(diào)整O2O3段的長(zhǎng)度，便可求得O1的坐標(biāo)。

可得O4點(diǎn)坐標(biāo)為：

式中，Lsafe1為汽車泊車終止?fàn)顟B(tài)與后方障礙物之間的安全距離，根據(jù)參考文獻(xiàn)[3]，本文取Lsafe1=0.2 m。

如果車輛能夠從O4沿軌跡O3O4安全駛出車位，則車輛端點(diǎn)B與車位端點(diǎn)a不能發(fā)生碰撞，即當(dāng)B點(diǎn)繞M2轉(zhuǎn)動(dòng)至B1點(diǎn)時(shí)，B1與a點(diǎn)間的距離應(yīng)大于安全距離Lsafe2，本文取Lsafe2=0.2 m，如圖3所示。

圖3 B點(diǎn)避障分析

根據(jù)幾何關(guān)系可推算得到最小車位長(zhǎng)度Lmin為：

當(dāng)B點(diǎn)繞M2轉(zhuǎn)動(dòng)θ角度至B1點(diǎn)時(shí)，后軸中心同樣從O4轉(zhuǎn)動(dòng)θ角度至O3：

Lmin和θ可根據(jù)車輛參數(shù)求出，從而得出O3點(diǎn)的坐標(biāo)：

分析可知，O3點(diǎn)處車輛航向角φ=θ，O2O3段軌跡斜率為tanθ。O1點(diǎn)的選取范圍線段與O2O3平行，故其斜率同樣為tanθ。

O1和O2點(diǎn)下限位置點(diǎn)的選取應(yīng)保證車輛沿O1O2行駛時(shí)，車身右側(cè)不與車位頂點(diǎn)a發(fā)生碰撞。如圖4所示，當(dāng)汽車沿直線段O2O3行駛至汽車后軸延長(zhǎng)線與車身右側(cè)的交點(diǎn)I點(diǎn)到達(dá)圖3中B1點(diǎn)位置時(shí)再轉(zhuǎn)彎行駛即可。這種處理方法求得的O2點(diǎn)雖然不是O2點(diǎn)縱坐標(biāo)最小值的位置，但是簡(jiǎn)化了運(yùn)算分析量，故以該位置作為O2點(diǎn)的縱坐標(biāo)下限值位置點(diǎn)，則此狀態(tài)下的O1點(diǎn)即為泊車起始區(qū)域的下限坐標(biāo)值所在點(diǎn)。

圖4 I點(diǎn)避障分析

可推算O1點(diǎn)下限坐標(biāo)為：

式中，O1minX和O1minY分別為O1點(diǎn)下限位置點(diǎn)的橫、縱坐標(biāo)。

如圖5所示，汽車沿軌跡O1O2行駛時(shí)，需要保證車身左側(cè)端點(diǎn)A不與道路左側(cè)發(fā)生碰撞，即軌跡AA1的縱坐標(biāo)的最大值應(yīng)小于道路寬度Lroad。

O1點(diǎn)的縱坐標(biāo)O1y需滿足：

由此可得O1點(diǎn)縱坐標(biāo)的上限值O1maxY為：

圖5 A點(diǎn)避障分析

根據(jù)O1點(diǎn)選取范圍段的斜率和式（7）推算得到O1點(diǎn)上限值橫坐標(biāo)O1maxX為：

則O1點(diǎn)的選取范圍為：

式中，O1X、O1Y分別為O1點(diǎn)橫、縱坐標(biāo)，且O1X∈[O1minX,O1maxX]。

分析可知，若車輛的初始姿態(tài)平行于X軸，則車輛在O1點(diǎn)的選取范圍線段右方時(shí)，均可倒車行駛至曲線O1maxO1min上的點(diǎn)后再進(jìn)入泊車軌跡。所以實(shí)際泊車起始點(diǎn)的選取范圍應(yīng)為3條曲線所包含的區(qū)域：

實(shí)際泊車起始點(diǎn)的選取范圍如圖6所示。

圖6 泊車起始點(diǎn)可選區(qū)域

3.2 路徑平滑處理

采用逆向行駛方法得到的泊車路徑存在曲率不連續(xù)的問題，故需進(jìn)行平滑處理。傳統(tǒng)的路徑平滑處理方式是采用反正切函數(shù)和Sigmoid函數(shù)，不能滿足軌跡起點(diǎn)零曲率或小曲率的要求，而B樣條曲線處理方式的求解參數(shù)數(shù)量較多，計(jì)算量較大，本文采用的五次多項(xiàng)式路徑平滑處理方法在滿足軌跡起點(diǎn)零曲率或小曲率要求的同時(shí)，表達(dá)式更簡(jiǎn)潔、求解參數(shù)數(shù)量更少、計(jì)算量更小。為提高路徑曲線的平滑度，需要適量增加最小車位長(zhǎng)度，同時(shí)泊車起始點(diǎn)行駛范圍需向右平移一定距離得到新的起始點(diǎn)坐標(biāo)O′1(X1,Y1)。本文設(shè)定最小車位長(zhǎng)度為6.8 m，泊車起始點(diǎn)向右移動(dòng)1 m。假設(shè)泊車路徑表達(dá)式為：

式中，K1～K6為五次多項(xiàng)式各項(xiàng)系數(shù)。

泊車起始狀態(tài)與泊車終止?fàn)顟B(tài)的車輛姿態(tài)應(yīng)與X軸方向平行，所以泊車軌跡方程在泊車起始點(diǎn)與終點(diǎn)處的導(dǎo)數(shù)為0，可建立如下約束方程：

為了使車輛初始狀態(tài)前輪等效轉(zhuǎn)角為0，車輛在起點(diǎn)處泊車軌跡的二階導(dǎo)數(shù)需為0，則：

為避免泊車過程中車身與車位頂點(diǎn)a發(fā)生碰撞，車輛行駛軌跡需要經(jīng)過O3點(diǎn)。而為了使得路徑更加平滑，泊車軌跡曲率K應(yīng)滿足：

為滿足軌跡的曲率約束要求，需要使O3點(diǎn)縱坐標(biāo)減小LO3。根據(jù)多次仿真測(cè)試，當(dāng)LO3=0.05 m時(shí)，可得到滿足曲率約束條件和避障條件的泊車軌跡方程，調(diào)整后的點(diǎn)坐標(biāo)為(X3,Y3)。由此可得到約束方程：

4 路徑跟蹤控制器設(shè)計(jì)

4.1 非時(shí)間參考系路徑跟蹤系統(tǒng)誤差分析

非時(shí)間參考路徑跟蹤控制方法最早應(yīng)用于機(jī)器人路徑跟蹤系統(tǒng)[13]，通過引入非時(shí)間參考量代替時(shí)間參考量，解決傳統(tǒng)路徑跟蹤方法中將期望軌跡視為時(shí)間函數(shù)的問題。

非時(shí)間參考量需要滿足隨時(shí)間單調(diào)遞增的要求，故本文選取非時(shí)間參考量f=-x(t)，求導(dǎo)可得：

因?yàn)椴窜嚂r(shí)的速度為負(fù)，泊車過程中車身航向角φ∈[-0.5π,0.5π]，所以f?＞0 ，滿足非時(shí)間參考量隨時(shí)間變化單調(diào)遞增的要求。非時(shí)間參考的車輛運(yùn)動(dòng)學(xué)方程可表示為：

設(shè)目標(biāo)路徑函數(shù)為yr=F(xr)，若汽車能夠無誤差地跟蹤，當(dāng)車輛后軸中心點(diǎn)橫坐標(biāo)為xr時(shí)，相應(yīng)的縱坐標(biāo)為yr、航向角為φr、前輪等效轉(zhuǎn)角為δr。根據(jù)軌跡函數(shù)的一次導(dǎo)數(shù)和二次導(dǎo)數(shù)，結(jié)合式（22）可得：

在已知目標(biāo)路徑函數(shù)的前提下，可通過式（23）和式（24）計(jì)算得到橫坐標(biāo)為xr下所對(duì)應(yīng)的車輛理想狀態(tài)參數(shù)。

選取車輛路徑跟蹤誤差為：

式中，y、φ為汽車后軸中心點(diǎn)橫坐標(biāo)為x時(shí)所對(duì)應(yīng)的汽車實(shí)際狀態(tài)參數(shù)，xe、ye和φe分別為縱向誤差、橫向誤差和航向角誤差。

則以非時(shí)間參考量f=-x(t)參考的路徑跟蹤偏差為：

4.2 滑模變結(jié)構(gòu)路徑跟蹤控制器設(shè)計(jì)

滑模變結(jié)構(gòu)控制是自動(dòng)控制系統(tǒng)設(shè)計(jì)方法中重要的研究分支，因算法簡(jiǎn)單、魯棒性好、適應(yīng)性強(qiáng)、實(shí)用性廣而被廣泛運(yùn)用?；Ｗ兘Y(jié)構(gòu)控制的核心是設(shè)計(jì)出合適的切換函數(shù)和變結(jié)構(gòu)控制律，使系統(tǒng)狀態(tài)軌跡線能快速到達(dá)設(shè)計(jì)的切換面進(jìn)行滑動(dòng)模態(tài)運(yùn)動(dòng)[14]。

設(shè)定系統(tǒng)的參考狀態(tài)量為：

對(duì)上式求導(dǎo)：

設(shè)計(jì)滑模切換函數(shù)s為：

其中k1＞0，對(duì)上式求導(dǎo)可得：

定義Lyapunov函數(shù)為：

采用指數(shù)趨近律來設(shè)計(jì)滑模變結(jié)構(gòu)控制器，以改善趨近運(yùn)動(dòng)的動(dòng)態(tài)品質(zhì)，?。?/p>

式中，k3＞0；k2＞0；sgn(s)為符號(hào)函數(shù)。

可得到：

當(dāng)且僅當(dāng)s=0時(shí)等號(hào)成立，根據(jù)連續(xù)系統(tǒng)滑動(dòng)模態(tài)的存在性和可達(dá)性[14]可知，系統(tǒng)的滑動(dòng)模態(tài)存在且可達(dá)，即系統(tǒng)s在趨近律式（32）作用下可到達(dá)平衡點(diǎn)s=0。

結(jié)合式（30）和式（32），可設(shè)計(jì)泊車系統(tǒng)轉(zhuǎn)向控制量，即前輪等效轉(zhuǎn)角為：

由于指數(shù)趨近律設(shè)計(jì)的滑?？刂破髦惺褂昧朔?hào)函數(shù)sgn(s)，系統(tǒng)會(huì)出現(xiàn)不連續(xù)的開關(guān)特性，使得滑模變結(jié)構(gòu)系統(tǒng)在滑動(dòng)模態(tài)下存在高頻的振動(dòng)現(xiàn)象，從而導(dǎo)致路徑跟蹤過程中控制器會(huì)出現(xiàn)較大波動(dòng)，繼而影響泊車控制的精確度。為抑制振動(dòng)，本文采取飽和函數(shù)sat(s/Δ)來代替符號(hào)函數(shù)sgn(s)，以保證系統(tǒng)的輸入在切換面附近光滑連續(xù)：

式中，Δ為邊界層厚度。

對(duì)控制器參數(shù)k1、k2、k3和Δ進(jìn)行調(diào)試修改，得到合理的參數(shù)取值，便能得到較好的控制效果。

5 仿真分析

5.1 路徑規(guī)劃仿真

在MATLAB中輸入車輛參數(shù)，且假定道路寬度為L(zhǎng)road=4 m，采用逆向行駛的方法得到初步規(guī)劃的泊車起始點(diǎn)選取范圍和主要路徑點(diǎn)坐標(biāo)。然后將泊車起始點(diǎn)向右平移1 m后得到新的起始點(diǎn)選擇區(qū)域?yàn)椋?/p>

式中，X∈[8.443 8,10.775 8]。

分別選擇泊車起始點(diǎn)上限值(O1maxX+1,O1maxY)、下限值(O1minX+1,O1minY)和中間點(diǎn)(0.5(O1minX+O1maxX)+1,0.5(O1maxY+O1minY))進(jìn)行求解計(jì)算，得到如圖7所示的3個(gè)起始點(diǎn)對(duì)應(yīng)行駛軌跡。分別對(duì)3條規(guī)劃軌跡進(jìn)行求導(dǎo)和求曲率計(jì)算，結(jié)果分別如圖8和圖9所示。

圖7 路徑規(guī)劃仿真結(jié)果

圖8 規(guī)劃路徑導(dǎo)數(shù)

圖9 規(guī)劃泊車路徑曲率

由圖7～圖9可知，求解出的3條規(guī)劃軌跡都能夠順利連接泊車起始點(diǎn)和終點(diǎn)，未與道路邊界和車位邊界發(fā)生碰撞，軌跡曲線的導(dǎo)數(shù)均為0，曲率均在-0.231 6～0.231 6 m-1范圍內(nèi)，泊車起始點(diǎn)處曲率均為0，滿足避障條件、泊車姿態(tài)條件、最大曲率約束條件和初始位置等效前輪轉(zhuǎn)角的要求。

5.2 路徑跟蹤控制器仿真

為了驗(yàn)證結(jié)合非時(shí)間參考系設(shè)計(jì)的滑模路徑跟蹤控制器在泊車系統(tǒng)路徑跟蹤控制應(yīng)用中的有效性，本文利用MATLAB/Simulink搭建了汽車運(yùn)動(dòng)學(xué)模型和路徑跟蹤控制器，選擇前文的軌跡3作為參考軌跡，設(shè)定車輛初始姿態(tài)平行于X軸，車速為-1 m/s，選定控制器參數(shù)k1=25、k2=1.1、k3=0.01、Δ=0.5進(jìn)行仿真驗(yàn)證，結(jié)果如圖10～圖12所示。

圖10 速度恒定時(shí)路徑跟蹤仿真結(jié)果

圖11 速度恒定時(shí)路徑跟蹤效果

由圖10可知，利用本文設(shè)計(jì)的控制器進(jìn)行泊車控制，當(dāng)泊車速度恒定時(shí)，車輛能夠順利進(jìn)入車位且未與四周障礙物發(fā)生碰撞，滿足安全性要求。

由圖11可知，控制器進(jìn)行平行泊車路徑跟蹤時(shí)，后軸中心點(diǎn)的運(yùn)動(dòng)軌跡和車輛的航向角與規(guī)劃軌跡基本重合，表明車輛能夠順利跟蹤目標(biāo)路徑和目標(biāo)姿態(tài)。

圖12 速度恒定時(shí)路徑跟蹤誤差

由圖12可知，泊車路徑跟蹤精度較高，泊車終點(diǎn)處的航向角為-0.004 6 rad，可近似認(rèn)為車身姿態(tài)平行于X軸，滿足泊車結(jié)束時(shí)姿態(tài)平行于車位方向的要求。

在實(shí)際泊車控制中，車輛速度難以維持在一固定值，但通常在一較低的數(shù)值附近波動(dòng)。為驗(yàn)證泊車控制器在速度波動(dòng)的情況下的有效性，本文選取速度v=(0.1sin(5t)-1) m/s進(jìn)行仿真驗(yàn)證，仍選擇軌跡3為參考軌跡。車輛初始姿態(tài)平行于X軸，仿真結(jié)果如圖13～圖15所示。

圖13 速度波動(dòng)時(shí)路徑跟蹤仿真結(jié)果

圖14 速度波動(dòng)時(shí)路徑跟蹤效果

圖15 速度波動(dòng)時(shí)路徑跟蹤誤差

由圖13～圖15可知，當(dāng)車輛泊車速度波動(dòng)時(shí)，控制器仍能夠使車輛安全地完成泊車車輛軌跡和姿態(tài)仍具有較好的跟蹤效果。由圖15可知，泊車終點(diǎn)處的車身航向角為-0.004 7 rad，滿足泊車終點(diǎn)姿態(tài)要求。雖然誤差曲線因?yàn)檐囁俚淖兓诟欉^程中出現(xiàn)了明顯波動(dòng)，但跟蹤過程中誤差均較小，可認(rèn)為跟蹤精度較高。

綜上所述，當(dāng)車速恒定和波動(dòng)時(shí)，控制器均能準(zhǔn)確地跟蹤目標(biāo)軌跡，具有較強(qiáng)的適應(yīng)性。

6 結(jié)束語

本文首先利用逆向行駛的方式，推算出泊車起始點(diǎn)的選擇范圍，并利用五次多項(xiàng)式對(duì)初步規(guī)劃路徑進(jìn)行平滑處理，以滿足泊車系統(tǒng)對(duì)軌跡曲率連續(xù)的要求，并結(jié)合非時(shí)間參考系路徑跟蹤方法和滑模變結(jié)構(gòu)控制設(shè)計(jì)了基于平行泊車的自動(dòng)泊車系統(tǒng)路徑跟蹤控制器，仿真結(jié)果表明，控制器具有較好的跟蹤效果，當(dāng)速度波動(dòng)時(shí)仍能較好地跟蹤規(guī)劃軌跡。