基于線性擴(kuò)張狀態(tài)觀測(cè)器的平行泊車路徑跟蹤*

王健 趙又群? 季學(xué)武 劉亞輝 臧利國(guó)

(1.南京航空航天大學(xué) 能源與動(dòng)力學(xué)院，江蘇 南京 210016;2.清華大學(xué) 汽車安全與節(jié)能國(guó)家重點(diǎn)實(shí)驗(yàn)室，北京 100084)

隨著世界汽車工業(yè)的飛速發(fā)展，汽車產(chǎn)銷量的快速增長(zhǎng)與可利用泊車空間的日益減少之間的矛盾變得日益突出.在擁擠的城市街道，泊車變得越來越困難，特別是對(duì)于沒有經(jīng)驗(yàn)的駕駛員來說，泊車更加困難［1］.為了提高駕駛舒適性和安全性，開發(fā)智能泊車輔助系統(tǒng)迫在眉睫.智能泊車輔助系統(tǒng)能夠利用超聲波傳感器和攝像頭感知、識(shí)別車輛路徑信息，通過控制電動(dòng)助力轉(zhuǎn)向系統(tǒng)進(jìn)行轉(zhuǎn)向操作來實(shí)現(xiàn)有效泊車.采用該系統(tǒng)，泊車時(shí)駕駛員只需控制好油門和剎車即可，大大減輕了駕駛員的負(fù)擔(dān).

路徑規(guī)劃和路徑跟蹤是平行泊車?yán)碚撗芯恐械膬蓚€(gè)關(guān)鍵部分.在路徑規(guī)劃方面，國(guó)內(nèi)外學(xué)者進(jìn)行了大量研究，研究中多采用兩段圓弧相切的方法，設(shè)計(jì)的圓弧需要滿足避障約束條件［2-5］.為了解決路徑曲率不連續(xù)問題，有研究者采用了B 樣條曲線、Bezier曲線擬合、多項(xiàng)式曲線擬合、回旋曲線擬合和β 樣條曲線擬合等方法［6-8］.路徑規(guī)劃好之后，路徑跟蹤的準(zhǔn)確與否直接決定了平行泊車是否成功.Zhao 和Ollero等［9-10］采用模糊控制方法設(shè)計(jì)了路徑跟蹤控制器，通過超聲波傳感器實(shí)時(shí)檢測(cè)車輛與泊車位之間的位置關(guān)系，據(jù)此決定方向盤的轉(zhuǎn)角大小.Demirli 等［11］設(shè)計(jì)了自適應(yīng)模糊控制路徑跟蹤控制器來控制車輛的跟蹤參考路徑.在Lian 等［12］采用模糊滑模控制方法設(shè)計(jì)的控制器中，將駕駛員經(jīng)驗(yàn)考慮進(jìn)了模糊控制模塊，控制力的大小通過模糊控制進(jìn)行調(diào)節(jié)，其方向則由滑?？刂茮Q定.上述方法均基于現(xiàn)代控制理論來設(shè)計(jì)控制器，但存在以下不足:難以制定準(zhǔn)確的模糊控制規(guī)則，模糊控制和滑?？刂频墓こ虘?yīng)用困難.為解決上述問題，文中基于線性擴(kuò)張狀態(tài)觀測(cè)器(LESO)設(shè)計(jì)路徑跟蹤控制器.該觀測(cè)器不需建立被控對(duì)象的精確數(shù)學(xué)模型，結(jié)合了經(jīng)典控制理論和現(xiàn)代控制理論的優(yōu)點(diǎn)［13-14］.LESO 可以將系統(tǒng)存在的干擾和模型不確定性觀測(cè)出來并補(bǔ)償?shù)?，因此可有效提高系統(tǒng)的魯棒性.

1 平行泊車系統(tǒng)描述

平行泊車系統(tǒng)主要由以下幾部分組成:位于前保險(xiǎn)桿兩側(cè)的兩個(gè)超聲波傳感器、1 個(gè)電子控制單元、1個(gè)方向盤角度傳感器、1 套電動(dòng)助力轉(zhuǎn)向系統(tǒng)、4 個(gè)輪速傳感器和1 個(gè)人機(jī)交互界面.基于鈴木北斗星汽車(樣機(jī))開發(fā)的平行泊車系統(tǒng)結(jié)構(gòu)如圖1 所示.

圖1 平行泊車系統(tǒng)的結(jié)構(gòu)Fig.1 Structure of parallel parking system

2 路徑規(guī)劃

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

車輛運(yùn)動(dòng)學(xué)模型越精確，越能夠真實(shí)反映車輛的運(yùn)動(dòng)情況.在泊車速度較低、車輛后輪與地面之間滿足純滾動(dòng)約束、后軸中心速度v 垂直于后軸的條件下，以車輛后軸中心為參考位置，建立圖2 所示的車輛運(yùn)動(dòng)學(xué)模型.

圖2 車輛運(yùn)動(dòng)學(xué)模型Fig.2 Kinematics model of vehicles

考慮外界干擾和建模不確定性，建立以后軸中心為參考點(diǎn)的車輛運(yùn)動(dòng)學(xué)方程如下:

式中，v 為車輛后軸中心速度，f(v，t)為由于路面顛簸引起的外界擾動(dòng)，ws(v，t)為由轉(zhuǎn)向機(jī)械機(jī)構(gòu)復(fù)雜性引起的轉(zhuǎn)向運(yùn)動(dòng)學(xué)建模不確定性，δ為前軸中心轉(zhuǎn)角，Ts為轉(zhuǎn)向系統(tǒng)延遲時(shí)間，u 為前軸中心轉(zhuǎn)角控制量，其他變量的定義如圖2 所示.

2.2 參考路徑

圖3 所示為平行泊車路徑，由一段直線(S0)和兩段圓弧(S1、S2)組成.建立平行泊車過程的方程如下:

式中，R1、R2、S0、θ 和h0等參數(shù)的定義如圖3 所示.泊車終止點(diǎn)目標(biāo)位置由Drl、Dlo和Drr三個(gè)安全距離參數(shù)限定，如圖4 所示.

圖3 平行泊車路徑Fig.3 Parallel parking path

圖4 目標(biāo)位置限定Fig.4 Goal position constraint

車輛駛離車位的過程中，車身的右前方要躲避前方障礙物，設(shè)安全距離為Dfr，根據(jù)圖4，可以計(jì)算出轉(zhuǎn)彎半徑R1的最大值，如下所示:

式中，Lp為超聲波傳感器檢測(cè)到的有效泊車位長(zhǎng)度，Lr為后懸長(zhǎng)度，La為車身長(zhǎng)度，Lrr為車輛后輪中心平面到車身外沿的長(zhǎng)度，Wa為車寬，Wb為后輪軸距.受車輛本身最小轉(zhuǎn)彎半徑Rmin的限制，R1的最小值為

式中，Lb為車輛軸距.R1的取值范圍如下:

圖5 和6 為泊車過程中可能發(fā)生碰撞的位置.由圖5 和6 可以計(jì)算出轉(zhuǎn)彎半徑R2 的最大值和最小值.具體計(jì)算過程可由式(5)和(6)描述:

式中，(m1，n1)和(m2，n2)分別為圓心O1和圓心O2的坐標(biāo)，Δy 表示側(cè)向超聲波傳感器到馬路沿的距離，(Cx，Cy)表示前方障礙物左后端位置坐標(biāo).

圖5 第二碰撞位置Fig.5 The second collision position

圖6 第三碰撞位置Fig.6 The third collision position

式中，H 為左側(cè)障礙物與馬路沿之間的距離，d 為側(cè)面超聲波傳感器與右側(cè)障礙物之間的距離，Lob為車頭左前方距左側(cè)障礙物的安全距離.

R2的取值范圍如下:

最近泊車起始位置和最遠(yuǎn)泊車起始位置可以表示為

上述方法規(guī)劃的路徑曲率不連續(xù)，為了滿足曲率連續(xù)這一約束條件，并且使規(guī)劃的路徑能夠滿足轉(zhuǎn)向執(zhí)行機(jī)構(gòu)的要求，采用三次B 樣條曲線對(duì)規(guī)劃的泊車路徑進(jìn)行曲線擬合.限于篇幅，文中對(duì)曲線擬合部分不展開描述，最終規(guī)劃的泊車路徑曲線如圖7 所示.

圖7 泊車路徑Fig.7 Parking paths

3 線性擴(kuò)張狀態(tài)觀測(cè)器

3.1 線性擴(kuò)張狀態(tài)觀測(cè)器的相關(guān)理論

線性擴(kuò)張狀態(tài)觀測(cè)器可以將系統(tǒng)所受到的外界干擾和建模不確定性部分觀測(cè)出來并補(bǔ)償?shù)?，保證系統(tǒng)的魯棒性.它將系統(tǒng)受到的外界干擾和未建模部分看作一個(gè)新的狀態(tài)——擴(kuò)張狀態(tài)，通過輸出反饋的方式觀測(cè)這個(gè)狀態(tài)［15］.對(duì)于單輸入單輸出非線性時(shí)變系統(tǒng)，

式中:y1和u0分別表示控制輸出和控制輸入;F(t)，...，y1(t)，w(t))表示非線性時(shí)變未知?jiǎng)討B(tài)，包含系統(tǒng)的內(nèi)部動(dòng)態(tài)和外部干擾w(t)，為方便描述，把它簡(jiǎn)記為F，w(t)可以是時(shí)變的，也可以是定常的;b 為未知的控制增益系數(shù)，通?？梢愿鶕?jù)具體系統(tǒng)求解其范圍，亦可以粗略估計(jì)，取b0≈b.方程(8)可重新定義為

通過上述轉(zhuǎn)化，將非線性系統(tǒng)轉(zhuǎn)化成線性系統(tǒng)的狀態(tài)方程形式，從而構(gòu)造線性擴(kuò)張狀態(tài)觀測(cè)器，實(shí)時(shí)估計(jì)出被擴(kuò)張的狀態(tài)量.其中，擴(kuò)張狀態(tài)觀測(cè)器的一般形式為

［z1z2… zn+1］T是擴(kuò)張狀態(tài)［x1x2… xn+1］T的估計(jì)值，［l1l2… ln+1］為設(shè)計(jì)參數(shù).

3.2 線性擴(kuò)張狀態(tài)觀測(cè)器的設(shè)計(jì)

平行泊車系統(tǒng)可以看作二階系統(tǒng)，采用三階擴(kuò)張狀態(tài)觀測(cè)器進(jìn)行未知建模和外界干擾的觀測(cè)估計(jì).考慮車輛運(yùn)動(dòng)學(xué)方程

平行泊車路徑跟蹤模型可以認(rèn)為是包含yr和φ 兩個(gè)參數(shù)的二階系統(tǒng)，令x1=yr，則系統(tǒng)性擴(kuò)張狀態(tài)方程如下:

文中將預(yù)瞄跟隨理論應(yīng)用于平行泊車路徑跟蹤過程［16］，如圖8 所示.控制器的控制目的是讓車輛跟隨理想?yún)⒖架壽E，即控制側(cè)向位移誤差Δy 趨近于0.圖8 中，L 表示預(yù)瞄距離，T 表示預(yù)瞄時(shí)間，泊車過程中車速較低，預(yù)瞄時(shí)間可以取固定值，仿真中T 取0.5 s.其中，側(cè)向位移誤差Δy 可以表示成

式中，(x0，y0，φ0)為車輛當(dāng)前時(shí)刻的位置和偏航角度，vx和vy表示縱向速度在水平和垂直方向的分量，f(x)表示參考路徑.

圖8 預(yù)瞄跟隨Fig.8 Preview follower

所設(shè)計(jì)的平行泊車路徑跟蹤控制器如圖9 所示.為了便于工程應(yīng)用，將線性擴(kuò)張狀態(tài)觀測(cè)器寫成離散形式.三階線性擴(kuò)張狀態(tài)觀測(cè)器的離散形式如下所示:

式中，ω0是由極點(diǎn)配置得到的線性擴(kuò)張狀態(tài)觀測(cè)器的帶寬.

圖9 路徑跟蹤控制器Fig.9 Path tracking controller

線性組合離散表達(dá)式為

式中，ωc為期望的閉環(huán)系統(tǒng)帶寬，一般情況下取ω0=(5～10)ωc.為了消除外界干擾和模型不確定性干擾，實(shí)際輸入到執(zhí)行機(jī)構(gòu)的控制量u 可以表示為

4 仿真與實(shí)車測(cè)試

4.1 仿真結(jié)果及分析

考慮泊車時(shí)的路面顛簸，外界干擾量對(duì)泊車速度的影響量f(v，t)可以表示成車輛縱向速度v和時(shí)間t 的函數(shù)，即f(v，t)=vsin(8t).考慮轉(zhuǎn)向系統(tǒng)運(yùn)動(dòng)學(xué)建模的不確定性，此時(shí)，ws(v，t)=0.02v·sin(5t).轉(zhuǎn)向系統(tǒng)延遲作為系統(tǒng)內(nèi)部干擾量進(jìn)行處理，延遲時(shí)間Ts=0.5 s.將仿真結(jié)果與采用傳統(tǒng)PID控制器進(jìn)行的路徑跟蹤效果進(jìn)行對(duì)比，以驗(yàn)證文中建立的控制器在解決平行泊車路徑跟蹤時(shí)的有效性和魯棒性.線性擴(kuò)張狀態(tài)觀測(cè)器的相關(guān)參數(shù)如下:b0=1.6，ω0=140，ωc=20.PID 控制器參數(shù)采用最優(yōu)方法優(yōu)化如下:Kp=2，Ki=0.5，Kd=1.泊車速度曲線如圖10 所示，其中車輛縱向速度v=1 m/s.基于線性擴(kuò)張狀態(tài)觀測(cè)器設(shè)計(jì)的路徑跟蹤控制器與PID 控制器的路徑跟蹤效果對(duì)比如圖11 所示.

圖10 仿真設(shè)定的泊車速度Fig.10 Simulation parking speed

圖11 仿真環(huán)境下的路徑跟蹤效果對(duì)比Fig.11 Path tracking comparison in simulation environment

從圖11 可以看出，基于線性擴(kuò)張狀態(tài)觀測(cè)器的路徑跟蹤效果優(yōu)于PID 控制效果.其中，線性擴(kuò)張狀態(tài)觀測(cè)器觀測(cè)出的系統(tǒng)總擾動(dòng)量如圖12 所示.在不改變控制器參數(shù)的情況下，試驗(yàn)測(cè)得的泊車速度如圖13 所示，路徑跟蹤效果對(duì)比如圖14 所示.泊車末端PID 控制器不能很好地控制車輛跟蹤的理想路徑.線性擴(kuò)張狀態(tài)觀測(cè)器的控制效果優(yōu)于PID 控制器的原因在于，前者能夠觀測(cè)出外界干擾和模型不確定性，如圖15 所示.

圖12 仿真環(huán)境下估計(jì)的總擾動(dòng)量Fig.12 Estimated total disturbance in simulation environment

圖13 試驗(yàn)測(cè)得的泊車速度Fig.13 Parking speed measured in experiment

圖14 試驗(yàn)環(huán)境下的路徑跟蹤效果對(duì)比Fig.14 Path tracking comparison in experiment environment

圖15 試驗(yàn)環(huán)境下估計(jì)的總擾動(dòng)量Fig.15 Estimated total disturbance in experiment environment

4.2 實(shí)車測(cè)試結(jié)果及分析

為了驗(yàn)證文中路徑跟蹤控制算法的有效性，進(jìn)行了實(shí)車驗(yàn)證.將所開發(fā)的平行泊車系統(tǒng)安裝在鈴木北斗星汽車上，進(jìn)行路徑跟蹤測(cè)試.采用上海司南導(dǎo)航公司開發(fā)的差分GPS 進(jìn)行車輛實(shí)時(shí)位置數(shù)據(jù)采集.其中，基站和移動(dòng)站型號(hào)分別為M300 和M600 系列，該差分GPS 同時(shí)支持GPS 和北斗系統(tǒng)，位置定位精度為1 cm，偏航角精度為0.1°，數(shù)據(jù)更新速率為10 Hz.試驗(yàn)中對(duì)車輛實(shí)際運(yùn)行軌跡進(jìn)行數(shù)據(jù)采集，并與規(guī)劃的理想泊車路徑進(jìn)行對(duì)比，結(jié)果見圖16.

圖16 實(shí)車試驗(yàn)數(shù)據(jù)Fig.16 Real vehicle experiment data

實(shí)車試驗(yàn)中，車位大小由側(cè)方超聲波傳感器測(cè)得，車位長(zhǎng)度為6 m，寬度為2.5 m.為了計(jì)算整個(gè)泊車過程中側(cè)向位移誤差的平均值，在理想路徑和實(shí)際路徑上隨機(jī)取10 個(gè)路徑點(diǎn)計(jì)算其側(cè)向位移誤差，結(jié)果見表1.通過計(jì)算相同位置X 處的理想泊車軌跡縱向位移Yideal和實(shí)車測(cè)試得到的泊車軌跡Yactual，可以計(jì)算出最大側(cè)向位移誤差為0.111 m，平均誤差為0.0677 m，說明利用文中的路徑跟蹤算法可以控制車輛無碰撞地實(shí)現(xiàn)平行泊車.

表1 側(cè)向位移誤差Table 1 Lateral displacement error

5 結(jié)語

文中考慮路面顛簸、轉(zhuǎn)向系統(tǒng)運(yùn)動(dòng)學(xué)模型的不確定性以及轉(zhuǎn)向系統(tǒng)的延遲，建立了平行泊車運(yùn)動(dòng)學(xué)模型，并設(shè)計(jì)了基于線性擴(kuò)張狀態(tài)觀測(cè)器的平行泊車路徑跟蹤控制器.該跟蹤控制器能夠?qū)⒛Ｐ筒淮_定性以及外界干擾作為一個(gè)總的擾動(dòng)量，通過線性擴(kuò)張狀態(tài)觀測(cè)器將其觀測(cè)出來并補(bǔ)償?shù)?，保證了系統(tǒng)的魯棒性.仿真結(jié)果表明，基于線性擴(kuò)張狀態(tài)觀測(cè)器設(shè)計(jì)的路徑跟蹤控制器的控制效果優(yōu)于傳統(tǒng)PID 控制器，且其工程實(shí)現(xiàn)簡(jiǎn)單，觀測(cè)器參數(shù)具有魯棒性;而PID 參數(shù)需要根據(jù)不同工況不斷調(diào)整，適應(yīng)性較差.實(shí)車試驗(yàn)結(jié)果表明，基于線性擴(kuò)張狀態(tài)觀測(cè)器設(shè)計(jì)的路徑跟蹤控制器能夠精確控制車輛完成平行泊車任務(wù)，最大誤差僅為0.111 m.文中提出的路徑跟蹤方法可以應(yīng)用到智能駕駛領(lǐng)域，能夠提高車輛高速行駛并且受到外界干擾時(shí)的路徑跟蹤魯棒性，為實(shí)現(xiàn)車輛真正的無人駕駛打下了基礎(chǔ).

［1］Huang S J，Lin G Y.Parallel auto-parking of a model vehicle using a self-organizing fuzzy controller［J］.Proceedings of the Institution of Mechanical Engineers(Part D):Journal of Automobile Engineering，2010，224(8):997-1012.

［2］Mukherjee R，Anderson A.A surface integral approach to the motion planning of nonholonomic systems［J］.Journal of Dynamic Systems，Measurement and Control，1994，116(3):315-325.

［3］Laumont J P，Jacobs P E，Taix M，et al.A motion planner for nonholonomic mobile robots ［J］.IEEE Transactions on Robotics and Automation，1994，10(5):577-593.

［4］Jiang K，Seneviratne L D.A sensor guided autonomousparking system for nonholonomic mobile robots ［C］∥Proceedings of 1999 IEEE International Conference on Robotics and Automation.Detroit:IEEE，1999:311-316.

［5］吳冰，錢立軍，虞明，等.基于RBF 神經(jīng)網(wǎng)絡(luò)的自動(dòng)泊車路徑規(guī)劃［J］.合肥工業(yè)大學(xué)學(xué)報(bào):自然科學(xué)版，2012，35(4):459-463.Wu Bing，Qian Li-jun，Yu Ming，et al.Path planning of automatic parallel parking based on RBF neural network［J］.Journal of Hefei University of Technology:Natural Science Edition，2012，35(4):459-463.

［6］Fraichard T，Scheuer A.From reeds and shepp’s to continuous-curvature paths ［J］.IEEE Transactions on Robotics，2004，20(6):1025-1035.

［7］Wang Y J，Cartmell M P.Autonomous vehicle parallel parking design using function fitting approaches［J］.Robotica，16(2):159-170.

［8］Gómez-Bravo F，Cuesta F，Ollero A，et al.Continuous curvature path generation based on β-spline curves for parking manoeuvres ［J］.Robotics and Autonomous Systems，2008，56(4):360-372.

［9］Zhao Y N，Emmanuel G，Collins Jr.Robust automatic parallel parking in tight spaces via fuzzy logic［J］.Robotics and Autonomous Systems，2005，51(2/3):111-127.

［10］Ollero A，García-Cerezo A，Martínez J L，et al.Fuzzy tracking methods for mobile robots［M］∥Jamshidi M，ed.Applications of fuzzy logic:towards high machine intelligence quotient systems.Upper Saddle River:Prentice-Hall，1997:347-364.

［11］Demirli K，Khoshnejad M.Autonomous parallel parking of a car-like mobile robot by a neuro fuzzy sensor based controller［J］.Fuzzy Sets and Systems，2009，160(19):2876-2891.

［12］Lian K Y，Chiu C S，Chiang T S.Parallel parking a carlike robot using fuzzy gain scheduling ［C］∥Proceedings of 1999 IEEE International Conference on Control Applications.Hawai:IEEE，1999:1686-1691.

［13］Gao Z Q.Scaling and bandwidth-parameterization based controller tuning ［C］∥Proceedings of the American Control Conference.Denver:IEEE，2003:4989-4996.

［14］韓京清.從PID 技術(shù)到自抗擾控制技術(shù)［J］.控制工程，2002，9(3):13-18.Han Jing-qing.From PID technique to active disturbances rejection control technique［J］.Control Engineering of China，2002，9(3):13-18.

［15］陳增強(qiáng)，孫明瑋，楊瑞光.線性自抗擾控制器的穩(wěn)定性研究［J］.自動(dòng)化學(xué)報(bào)，2013，39(5):574-580.Chen Zeng-qiang，Sun Ming-wei，Yang Rui-guang.Research on the stability of linear active disturbance rejection control［J］.Acta Automatica Sinica，2013，39(5):574-580.

［16］楊志強(qiáng)，譚彧.車道保持系統(tǒng)的車輛狀態(tài)預(yù)測(cè)模型［J］.農(nóng)業(yè)機(jī)械學(xué)報(bào)，2008，39(10):25-29.Yang Zhi-qiang，Tan Yu.Model based on states preview for lane keeping system［J］.Transactions of the Chinese Society for Agricultural Machinery，2008，39(10):25-29.