熊周兵，楊 威，丁 可，梁鋒華，鄭 玲，黎予生

(1.重慶長安汽車股份有限公司 汽車研究總院，重慶 401120；2.重慶大學(xué) 機(jī)械傳動國家重點(diǎn)實(shí)驗(yàn)室，重慶 400044)

基于預(yù)瞄模糊控制的自動泊車算法研究

熊周兵1，楊 威2，丁 可1，梁鋒華1，鄭 玲2，黎予生1

(1.重慶長安汽車股份有限公司 汽車研究總院，重慶 401120；2.重慶大學(xué) 機(jī)械傳動國家重點(diǎn)實(shí)驗(yàn)室，重慶 400044)

針對目前自動泊車系統(tǒng)轉(zhuǎn)向不連續(xù)問題，提出一種兩段式泊車路徑規(guī)劃算法，采用Sigmoid函數(shù)對路徑進(jìn)行函數(shù)擬合，確定泊車路徑。建立了橫向預(yù)瞄模型，對路徑追蹤誤差進(jìn)行估計(jì)，采用模糊控制方法，對擬合后的理想路徑進(jìn)行追蹤；搭建了PreScan虛擬仿真環(huán)境，通過超聲波傳感器獲取車位信息，對提出的基于預(yù)瞄模糊控制的自動泊車算法進(jìn)行了仿真驗(yàn)證。仿真結(jié)果表明：Sigmoid函數(shù)對兩段式泊車路徑的擬合決定系數(shù)可達(dá)0.99以上；采用該預(yù)瞄模糊控制泊車算法，汽車能很好地追蹤規(guī)劃的路徑，并成功泊入預(yù)定車位。

兩段式泊車；函數(shù)擬合；預(yù)瞄模糊控制；PreScan

隨著汽車數(shù)量的逐年攀升，城市停車場空間趨于飽和，經(jīng)驗(yàn)不足的駕駛員很難輕松地將汽車泊入擁擠的泊車位，因泊車造成的事故也屢見不鮮。自動泊車系統(tǒng)為解決泊車?yán)щy的問題提供了先進(jìn)的技術(shù)手段。

目前，國內(nèi)外主要采用兩種方法進(jìn)行自動泊車控制算法的研究[1]：① 路徑規(guī)劃：借助傳感器獲取泊車空間幾何形狀與障礙位置，根據(jù)車輛運(yùn)動學(xué)模型，預(yù)先規(guī)劃出一條理想的路徑，設(shè)計(jì)控制算法執(zhí)行路徑跟蹤，實(shí)現(xiàn)自動泊車；② 利用經(jīng)驗(yàn)知識模擬駕駛員操作控制，如采用模糊邏輯、神經(jīng)網(wǎng)絡(luò)算法等。但這些算法都過于復(fù)雜，不利于泊車實(shí)時性要求。Paromtchik等[2]利用回歸算法擬合正弦曲線得到理想路徑實(shí)現(xiàn)路徑規(guī)劃，但該算法需要很大的泊車空間。李紅等[3]采用多約束優(yōu)化的方法擬合反正切曲線設(shè)計(jì)理想路徑，但該算法計(jì)算效率較低，不利于實(shí)際應(yīng)用。ZHAO、CHANG等[4-5]應(yīng)用泊車經(jīng)驗(yàn)知識設(shè)計(jì)模糊控制器來控制泊車過程，但該算法對汽車初始位置敏感，對泊車空間要求較大。吳冰等[6]采用粒子群方法、結(jié)合RBF神經(jīng)網(wǎng)絡(luò)實(shí)現(xiàn)自動泊車路徑規(guī)劃，其仿真成功率高，但對初始位置要求較高，計(jì)算量大。

自動泊車系統(tǒng)的設(shè)計(jì)難度在于汽車能在盡可能小的空間內(nèi)快速決策出泊入過程中的方向盤轉(zhuǎn)角，實(shí)現(xiàn)多種空間狀態(tài)下的自動泊入?？紤]到工程適用性與算法魯棒性，本文采用路徑規(guī)劃的方法尋找理想路徑。以此為基礎(chǔ)，研究基于預(yù)瞄模糊控制的平行泊車控制算法，以實(shí)現(xiàn)汽車穩(wěn)定追蹤理想路徑，降低泊入過程中的計(jì)算積分誤差，提高不同環(huán)境下的泊車成功率。

1 自動泊車路徑規(guī)劃算法

一個完整的自動泊車系統(tǒng)包括4個部分：環(huán)境感知、路徑規(guī)劃、路徑跟蹤、人機(jī)交互[7]。自動泊車工作流程如圖1所示。當(dāng)駕駛員啟動自動泊車系統(tǒng)后，在車輛運(yùn)動過程中，傳感器會自動探測周圍空間環(huán)境。當(dāng)有適合尺寸的泊車空間時[7]，人機(jī)交互系統(tǒng)會提示駕駛員找到車位，系統(tǒng)自動進(jìn)行路徑規(guī)劃。若可以規(guī)劃出一條泊入路徑則提示駕駛員可以進(jìn)行泊車，否則駕駛員只能放棄當(dāng)前泊車位置。之后按照系統(tǒng)提示，駕駛員掛入倒擋，路徑跟蹤控制開始執(zhí)行，最終完成泊車。

圖1 自動泊車工作流程

自動泊車系統(tǒng)的路徑規(guī)劃是根據(jù)車載超聲波傳感器探測到泊車需求方向上的車位空間信息，再利用這些信息規(guī)劃出一條理想的路徑。相比智能算法，路徑規(guī)劃可大大降低計(jì)算的復(fù)雜程度，并預(yù)先確定汽車能否在此泊車位完成自動泊車。

1.1 路徑規(guī)劃運(yùn)動學(xué)模型

圖2為基于阿克曼轉(zhuǎn)向機(jī)構(gòu)的車輛運(yùn)動學(xué)模型[8]。 由于正常情況下泊車速度很低(約≤5 km/h)，故后輪可視為無滑移，即后輪軌跡的y方向速度為0，根據(jù)圖2可知車輛后軸中心點(diǎn)坐標(biāo)：

(1)

前后軸中心點(diǎn)坐標(biāo)之間存在如下關(guān)系：

xr=xf-lcosθ

yr=yf-lsinθ

(2)

對式(2)進(jìn)行求導(dǎo)得：

(3)

將式(3)代入式(1)得

(4)

圖2 車輛運(yùn)動學(xué)模型

設(shè)δ為阿克曼轉(zhuǎn)向機(jī)構(gòu)中前軸中心點(diǎn)等效轉(zhuǎn)向角，則前軸中心點(diǎn)的速度為：

(5)

將式(5)代入式(4)得

(6)

再將式(5)與(6)代入式(3)，可獲得后輪中心點(diǎn)速度:

(7)

將式(7)對時間積分，可推導(dǎo)出后輪中心點(diǎn)的運(yùn)動軌跡：

(8)

左后輪的運(yùn)動軌跡為：

(9)

右后輪的運(yùn)動軌跡為：

(10)

其中：θ為汽車方位角；l為軸距；w為輪距。

由式(9)與(10)可以看出：當(dāng)汽車低速泊車時，后輪的運(yùn)動軌跡與車速無關(guān)，只與汽車的軸距、輪距與轉(zhuǎn)向輪轉(zhuǎn)角有關(guān)，并且運(yùn)行軌跡是一段圓弧。上述軌跡規(guī)劃的運(yùn)動學(xué)方程為兩段式泊車提供了理論依據(jù)。

1.2 兩段式泊車路徑擬合方法

依據(jù)汽車低速泊車后輪軌跡為圓弧的特點(diǎn)，根據(jù)泊車空間的尺寸大小確定兩段相切圓弧，汽車后輪可以沿著圓弧運(yùn)動順利泊入車位，這就是兩段式泊車。通過控制汽車的最小轉(zhuǎn)彎半徑，兩段式泊車不僅能夠減少泊車空間，降低泊車換擋次數(shù)，還能使汽車平行泊入車庫內(nèi)走過的路徑最短。兩段式泊車幾何路徑規(guī)劃如圖3所示。

圖3 兩段式泊車幾何路徑規(guī)劃

根據(jù)阿克曼轉(zhuǎn)向原理，車輛運(yùn)行過程中轉(zhuǎn)彎半徑與前輪轉(zhuǎn)角之間的關(guān)系為

(11)

其中αout為前外輪轉(zhuǎn)角。從圖3可知:汽車以最小轉(zhuǎn)彎半徑轉(zhuǎn)向，在兩段圓弧相切點(diǎn)處車輛方位角達(dá)到最大值，其值為

(12)

其中:Δ為安全緩沖距離;p2為側(cè)邊庫底距車軸對稱線的縱向距離;rmin為最小轉(zhuǎn)彎半徑;B為車寬。

(13)

此函數(shù)具有4個擬合參數(shù)，分別為a1，a2，a3，a4，其2階導(dǎo)數(shù)為

(14)

該擬合函數(shù)的2階導(dǎo)數(shù)連續(xù)，表明函數(shù)曲率連續(xù)變化，即車輛沿這條路徑運(yùn)動時轉(zhuǎn)向角無突變。同時，-a2a3的取值可以保證該函數(shù)的曲率在x=0時趨近于0，即車輛在泊車起始位置處前輪轉(zhuǎn)向角為0。為驗(yàn)證兩段式泊車路徑規(guī)劃函數(shù)擬合效果，設(shè)計(jì)3種仿真工況參數(shù)，如表1所示。表2是根據(jù)3種工況得到的擬合參數(shù)。路徑擬合結(jié)果見圖4。

表2 函數(shù)擬合參數(shù)

圖4 路徑擬合結(jié)果

為檢驗(yàn)函數(shù)擬合優(yōu)度，可用決定系數(shù)來定量評價，其計(jì)算公式為

(15)

由式(15)計(jì)算得到仿真試驗(yàn)中3種工況下的擬合決定系數(shù)分別為0.999 7，0.999 4，0.999 5，說明采用4個擬合參數(shù)的Sigmoid函數(shù)擬合效果較好，對兩段式泊車路徑的近似程度較高。

2 路徑追蹤控制

路徑追蹤控制是無人駕駛智能汽車技術(shù)中的核心技術(shù)之一，目前常用的追蹤控制方法主要有PID算法、模糊推理、模糊PID、神經(jīng)網(wǎng)絡(luò)或多種算法相結(jié)合的綜合控制[9]。無論采用何種控制方法，首先需要建立路徑追蹤誤差模型。

2.1 橫向預(yù)瞄模型

預(yù)瞄的引入，相當(dāng)于建立駕駛員模型，實(shí)現(xiàn)路徑追蹤的閉環(huán)控制。斯坦福大學(xué)Stanley智能車研究所提出了距離-角度偏差預(yù)瞄模型[10]；CLA提出了環(huán)形預(yù)瞄模型[11]；北京理工大學(xué)采用了Follow the Carrot模型[12]；Pure Persuit模型[13]設(shè)計(jì)了一條圓弧，車輛可沿這條圓弧到達(dá)目標(biāo)點(diǎn)，由此圓弧計(jì)算車輛的前輪偏角控制量。以上模型主要用于智能汽車行駛過程中的路徑追蹤。自動泊車有別于智能汽車的行駛控制，其運(yùn)動速度低、運(yùn)動空間小，且速度方向?yàn)樨?fù)，采用以上預(yù)瞄模型難以快速準(zhǔn)確地實(shí)現(xiàn)誤差計(jì)算。本文在距離-角度偏差預(yù)瞄模型基礎(chǔ)上提出橫向預(yù)瞄模型[14]，如圖5所示。

當(dāng)車輛探測到可行的泊車空間時停止，進(jìn)行路徑規(guī)劃，以當(dāng)前車輛停止的位置與方位角建立大地坐標(biāo)系，如圖5所示。圖5中的坐標(biāo)系一旦確定，整個自動泊車過程都將在此坐標(biāo)系下完成。車輛運(yùn)動前方一定距離的預(yù)瞄點(diǎn)K與相應(yīng)橫向位置K′的偏差Δy稱之為橫向距離誤差，預(yù)瞄點(diǎn)的方位角θ與相應(yīng)橫向位置K′方位角的偏差Δφ稱之為橫向角度誤差，橫向距離誤差與橫向角度誤差統(tǒng)稱為橫向預(yù)瞄誤差。在實(shí)際計(jì)算過程中，設(shè)定一定權(quán)值來分配距離誤差和角度誤差的影響權(quán)重，其關(guān)系式為：

(16)

其中：Xr為后軸中心x坐標(biāo)；Yr為后軸中心y坐標(biāo)；XK為預(yù)瞄點(diǎn)x坐標(biāo)；P為預(yù)瞄距離；Y(X)為擬合曲線函數(shù)表達(dá)式；i為權(quán)重分配系數(shù)，考慮到泊車過程中位置誤差與角度誤差對追蹤都具有直接影響，因此取權(quán)重分配系數(shù)i=0.5。

圖5 橫向預(yù)瞄模型

2.2 路徑追蹤模糊控制

汽車運(yùn)動學(xué)系統(tǒng)具有強(qiáng)非線性與耦合性，采用模糊控制既可實(shí)現(xiàn)路徑規(guī)劃與路徑追蹤的實(shí)時性、魯棒性，又能發(fā)揮經(jīng)驗(yàn)知識對泊車精度提升的優(yōu)勢。本文以橫向預(yù)瞄誤差與誤差變化率為模糊控制輸入、轉(zhuǎn)向外輪轉(zhuǎn)角為模糊控制輸出設(shè)計(jì)模糊控制器，其控制系統(tǒng)如圖6所示。

分別劃分7個誤差輸入、誤差變化率輸入與轉(zhuǎn)角輸出的模糊集合，模糊量化后論域均為[-1,1]，其隸屬度函數(shù)如圖7所示。

圖6 預(yù)瞄閉環(huán)控制系統(tǒng)

圖7 隸屬度函數(shù)

圖7中，NB，NM，NS，Z，PS，PM，PB分別代表“負(fù)大”、“負(fù)中”、“負(fù)小”、“零”、“正小”、“正中”、“正大”，模糊規(guī)則采用ifEandECthenU，共產(chǎn)生49條規(guī)則，如表3所示。由規(guī)則生成的模糊控制面如圖8所示。

表3 模糊規(guī)則

圖8 模糊控制面

3 測試場景設(shè)計(jì)與仿真

某乘用車的外形參數(shù)如表4所示。

表4 某乘用車外形參數(shù)Table 4 Shape parameters of a passenger car

3.1 基于Matlab的仿真驗(yàn)證

針對以上的兩段式泊車路徑規(guī)劃以及基于橫向預(yù)瞄的模糊控制路徑追蹤控制方法，采用Matlab/Simulink，構(gòu)建控制系統(tǒng)仿真框圖，對本文提出的自動泊車控制方法進(jìn)行驗(yàn)證。圖9是3種工況下的仿真結(jié)果。

從圖9可知:預(yù)瞄模糊控制能有效控制車輛沿著理想軌跡運(yùn)動，其誤差在可接受范圍內(nèi)，并且方向盤轉(zhuǎn)角也在初始位置由0逐漸增大，變化過程中并無突變，滿足泊車要求。車輛泊入車庫后，相對車庫位置比較靠后，還需要前進(jìn)一段距離駛到車庫中間位置。仿真結(jié)果表明:基于預(yù)瞄模糊控制的平行泊車算法計(jì)算實(shí)時性好、魯棒性強(qiáng)，具有較高的工程實(shí)用價值。

3.2 基于PreScan的仿真驗(yàn)證

PreScan是專業(yè)的主動安全設(shè)計(jì)與環(huán)境、傳感器建模仿真軟件，可以很大程度上還原真實(shí)場景，提高仿真的準(zhǔn)確度與真實(shí)性。本文以PreScan場景為基礎(chǔ)，結(jié)合軟件中的超聲波傳感器，建立自動泊車仿真環(huán)境，實(shí)時驗(yàn)證控制算法的正確性。圖10是構(gòu)建的PreScan場景，圖11是工況2的泊車過程實(shí)時仿真結(jié)果。

圖10 PreScan場景

圖11 PreScan VisViewer與GUI仿真過程

工況2的自動泊車過程可分為5個階段：(a) 汽車低速前進(jìn)，啟動自動泊車程序，超聲波傳感器啟動，開始搜索車位；(b) 發(fā)現(xiàn)車位，減速停車，根據(jù)超聲波傳感器獲取的泊車環(huán)境信息重構(gòu)空間坐標(biāo)，規(guī)劃兩段式泊車路徑并擬合，完成后掛入倒擋開始泊車；(c) 預(yù)瞄模糊控制算法實(shí)時計(jì)算汽車前輪轉(zhuǎn)角，利用轉(zhuǎn)向比計(jì)算方向盤轉(zhuǎn)角，實(shí)現(xiàn)轉(zhuǎn)向控制；(d) 順利泊入車位，為使車輛停在車庫中間，還需要前進(jìn)修正一次；(e) 前進(jìn)修正完成泊車；(f) GUI仿真過程展示了超聲波傳感器獲取的環(huán)境信息，其中虛線是車輛運(yùn)動軌跡，實(shí)線是障礙輪廓。

4 結(jié)束語

本文針對目前自動泊車系統(tǒng)轉(zhuǎn)向不連續(xù)問題，提出了基于兩段式泊車的路徑規(guī)劃方法，建立了橫向預(yù)瞄模型，采用模糊控制方法對理想路徑進(jìn)行了追蹤控制，并通過仿真驗(yàn)證了控制算法的有效性。

采用兩段式泊車，設(shè)計(jì)了一條路程最短的路徑，利用Sigmoid函數(shù)，對兩段式泊車路徑進(jìn)行了參數(shù)擬合。結(jié)果表明：其擬合決定系數(shù)達(dá)到0.99以上，函數(shù)曲線滿足泊車過程中曲率要求。

研究了基于預(yù)瞄模糊控制的閉環(huán)控制平行泊車算法，通過控制汽車轉(zhuǎn)向外輪轉(zhuǎn)角，實(shí)現(xiàn)了對理想路徑的追蹤控制，其計(jì)算精度能滿足要求，在不同的車庫尺寸及初始位置條件下，均能順利泊入車庫；基于PreScan搭建了自動泊車的場景模型，聯(lián)合Matlab/Simulink控制仿真，驗(yàn)證了自動泊車控制算法的有效性和正確性，通過真實(shí)泊車場景的構(gòu)建，直觀再現(xiàn)了控制算法的效果。

[1] 郭孔輝，姜輝，張建偉，等.基于模糊邏輯的自動平行泊車轉(zhuǎn)向控制器[J].吉林大學(xué)學(xué)報(工學(xué)版)，2009，39 (S2):236-240.

GUO Konghui,JIANG hui,ZHANG Jianwei,et al.Automatic parallel parking steering controller based on fuzzy logic control theory[J].Journal of Jilin University(Engineering and Technology Edition),2009，39 (S2):236-240.

[2] PAROMTCHIK I E,LAUGIER C.Autonomous Parallel Parking and Returning to Traffic Maneuvers[C]//Proceedings of the 1997 IEEE/RSJ International Conference on Intelligent Robot and Systems.Grenoble,France:[s.n.],1997:21-23.

[3] 李紅，郭孔輝，宋曉琳，等.基于Matlab的多約束自動平行泊車軌跡規(guī)劃[J].中南大學(xué)學(xué)報(自然科學(xué)版)， 2013，44(1):101-107.

LI Hong,GUO Konghui,SONG Xiaolin，et al.Trajectory planning of automatic parallel parking with multi-constraints based on Matlab[J].Journal of Central South University(Science and Technology),2013，44 (1):101-107.

[4] ZHAO Y N,COLLINS E G.Robust automatic parallel parking in tightspaces via fuzzy logic[J].Robotics and Autonomous Systems,2005,51 (5):111-127.

[5] CHANG S J,LI J.Design and implementation of fuzzy parallel parking control for a car-type mobile robot[J].Journal of Intelligent & Robotic System,2004,34(2):175-180.

[6] 吳冰，錢立軍，虞明，等.基于RBF神經(jīng)網(wǎng)絡(luò)的自動泊車路徑規(guī)劃[J].合肥工業(yè)大學(xué)學(xué)報(自然科學(xué)版)，2014，35 (4):459-540.

WU Bing,QIAN Lijun,YU Ming，et al.Path planning of automatic parallel parking based don RBF neural network[J].Journal of Hefei University of Technology(Natural Science),2014，35 (4):459-540.

[7] POHL J,SETHSSON M,DEGERMAN P,et al.A semi-automated parallel parking system forpassenger cars[J].J Automobile Engineering,2006,220:53-65.

[8] COLYER R E,ECONOMOU J T.Comparison of steering geometries for multi-wheeled vehicles by modelling and simulation[C]// Proceedings of the 37th IEEE Conference on Decision and Control.USA:IEEE,1998:3131-3133.

[9] 喬維高，徐學(xué).無人駕駛汽車的發(fā)展現(xiàn)狀及方向[J].上海汽車,2007(7):40-44.

QIAO Weigao,XU Xue.Development status and direction of driverless cars[J].Shanghai automobile,2007(7):40-44.

[10]SEBASTIAN T,MIKE M,HENDRIK D,et al.Stanley The Robot that Won the DARPA Grand Challenge[J].Journal of Field Robotics,2006,23(9):661-692.

[11]CHEN Q,OZUGUNER U,REDILL K.Ohio State University at the 2004 DARPA Grand Challenge:Developing a Completely Autonomous Vehicle[J].IEEE Intelligent.Systems,2004,19(5):8-11.

[12]CEM H,OZGUNER U,KEITH A.Redmill.Automated Lane Change Controller Design[J].IEEE Transactions on Intelligent Transportation System,2003,4(1):13-22.

[13]KRISTIJIAN M,ROLAND P,ROLAND S.Path Following for Autonomous Vehicle Navigation with Inherent Safety and Dynamic Margin[C]//IEEE Intelligent Vehicles Symposium[C].USA:IEEE,2008:108-113.

[14]趙玲.平行泊車方法研究與仿真[D].西安：長安大學(xué)，2009.

ZHAO Ling.Research and Simulation of parallel parking method[D].Xi’an:Chang’an University,2009.

(責(zé)任編輯 劉 舸)

Research on the Auto Parking Algorithm Based on the Preview Fuzzy Control

XIONG Zhou-bing1, YANG Wei2, DING Ke1, LIANG Feng-hua1, ZHENG Ling2, LI Yu-sheng1

(1.Chang’an Auto Global R&D Center, Chang’an Automobile Co., Ltd., Chongqing 401120, China; 2.The State Key Laboratory of Mechanical Transmissions, Chongqing University, Chongqing 400044, China)

The Sigmoid function was adopted to fit the two section of parking path planning, with the aim to solve the discontinuous problems of automatic parking system. The lateral preview model was established and the path tracking error was estimated. Then the preview fuzzy control algorithm was adopted to track the ideal path. Finally, PreScan virtual simulation environment was established and the information of the parking slots was obtained by ultrasonic sensors.Simulation results demonstrate that the fit coefficient of determination can be over 0.99 between the Sigmoid function and the two-section parking path. Based on the preview fuzzy control, the car can track the planning path successfully and park into parking spaces.

two-section parking; function fitting; preview fuzzy control; PreScan

2016-11-28 基金項(xiàng)目：重慶市自然科學(xué)基金重點(diǎn)資助項(xiàng)目(cstc2015jcyjBX0097)；重慶市重點(diǎn)產(chǎn)業(yè)共性關(guān)鍵技術(shù)創(chuàng)新專項(xiàng)項(xiàng)目(cstc2015zdcy-ztzx30001)

熊周兵， 男，博士，主要從事車輛工程研究；通訊作者：鄭玲，女，教授，博士生導(dǎo)師，主要從事汽車系統(tǒng)動力學(xué)與控制、智能材料與結(jié)構(gòu)系統(tǒng)、振動與噪聲控制等方面研究，E-mail:zling@cqu.edu.cn。

熊周兵，楊威，丁可，等.基于預(yù)瞄模糊控制的自動泊車算法研究[J].重慶理工大學(xué)學(xué)報(自然科學(xué))，2017(2):14-22.

format：XIONG Zhou-bing, YANG Wei, DING Ke,et al.Research on the Auto Parking Algorithm Based on the Preview Fuzzy Control[J].Journal of Chongqing University of Technology(Natural Science)，2017(2):14-22.

10.3969/j.issn.1674-8425(z).2017.02.003

U491.7

A

1674-8425(2017)02-0014-09