高 強(qiáng)，陸 洲，段晨東，徐 婷

（1.長安大學(xué)汽車學(xué)院，西安 710064；2.長安大學(xué)電子與控制工程學(xué)院，西安 710064）

前言

高級(jí)駕駛輔助系統(tǒng)（advanced driving assistant system，ADAS）是當(dāng)前汽車領(lǐng)域 熱門的研究方向。ADAS系統(tǒng)通過車載傳感器感知周圍環(huán)境和汽車信息，進(jìn)行分析和處理后，實(shí)現(xiàn)汽車定速巡航、車道保持和碰撞預(yù)警等功能［1］。自動(dòng)泊車系統(tǒng)（automatic parking system，APS）是ADAS的組成部分，是智能汽車重要的研究?jī)?nèi)容之一。APS能夠自動(dòng)尋找停車位并泊入，有效減小駕駛員在泊車過程中的緊張程度，降低新手對(duì)狹小車位的泊車難度，減少剮蹭事故的發(fā)生［2］。國內(nèi)外學(xué)者對(duì)自動(dòng)泊車技術(shù)進(jìn)行了多方面的研究［3-9］。

城市中垂直車位比較常見。當(dāng)前垂直自動(dòng)泊車技術(shù)的研究大多基于汽車初始位置航向角為零的情況（理想狀況），沒有考慮初始位置航向角誤差對(duì)泊車的影響。在實(shí)際自動(dòng)泊車過程中，初始位置航向角通常不會(huì)為零（即相對(duì)理想狀態(tài)存在誤差），此時(shí)如按零初始位置航向角進(jìn)行路徑規(guī)劃和泊車，會(huì)造成汽車實(shí)際路徑與規(guī)劃路徑存在較大偏差，從而在泊車中造成剮蹭或泊車失敗等問題。針對(duì)上述不足，本文中研究了汽車初始位置航向角不為零時(shí)，垂直泊車路徑規(guī)劃與路徑跟蹤問題。首先對(duì)汽車初始位置航向角不為零時(shí)的泊車路徑進(jìn)行規(guī)劃，使用四次樣條函數(shù)改進(jìn)規(guī)劃路徑；然后針對(duì)常用的預(yù)瞄誤差前饋方法在路徑跟蹤中有時(shí)存在較大誤差的問題，設(shè)計(jì)了預(yù)瞄誤差前饋和航向角反饋相結(jié)合的控制方法，對(duì)期望路徑進(jìn)行跟蹤；通過Simulink/CarSim聯(lián)合仿真，研究了所設(shè)計(jì)垂直泊車算法的有效性；最后通過實(shí)車試驗(yàn)對(duì)該算法進(jìn)行了驗(yàn)證。

1 研究方法

1.1 自動(dòng)泊車系統(tǒng)工作流程

自動(dòng)泊車系統(tǒng)工作流程如圖1所示。駕駛員開啟自動(dòng)泊車功能后，APS系統(tǒng)對(duì)汽車進(jìn)行控制，汽車向前行駛，同時(shí)車載環(huán)視攝像頭對(duì)汽車周圍環(huán)境進(jìn)行感知。當(dāng)檢測(cè)到停車位時(shí)，APS對(duì)當(dāng)前車位進(jìn)行路徑規(guī)劃，然后控制汽車進(jìn)行路徑跟蹤，直到汽車成功泊入停車位。泊車完成后轉(zhuǎn)向盤回正并掛駐車擋，自動(dòng)泊車系統(tǒng)關(guān)閉。

圖1 自動(dòng)泊車系統(tǒng)工作流程圖

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

汽車輪廓形狀直接影響泊車成功率。由于實(shí)際汽車的輪廓形狀較為復(fù)雜且難以準(zhǔn)確建模，這里在給予一定裕度的情況下使用矩形替代汽車模型，其長度和寬度分別為汽車的最大長度和最大寬度，簡(jiǎn)化后的汽車模型如圖2所示。在本文計(jì)算、仿真和實(shí)車試驗(yàn)中，汽車主要參數(shù)如表1所示。

圖2 簡(jiǎn)化汽車模型

表1 汽車主要參數(shù)表

選取APS系統(tǒng)開始工作時(shí)刻后輪中心點(diǎn)為汽車參考點(diǎn)。在低速泊車工況下，根據(jù)阿克曼轉(zhuǎn)向原理，將汽車視為平面剛體，即只具有x方向、y方向和橫擺3個(gè)自由度。汽車在坐標(biāo)系的坐標(biāo)為(x，y)，航向角為ψ，汽車運(yùn)動(dòng)學(xué)方程為

式中：v為車速；δf為等效前輪轉(zhuǎn)角。

1.3 垂直泊車基礎(chǔ)路徑規(guī)劃策略

假設(shè)汽車垂直泊車時(shí)初始位置航向角為θ，以汽車前進(jìn)方向?yàn)閄軸，自動(dòng)泊車系統(tǒng)開始工作時(shí)刻汽車后軸中心為原點(diǎn)O，原點(diǎn)左側(cè)垂直方向?yàn)閅軸建立坐標(biāo)系，如圖3所示。通過攝像頭模塊探測(cè)得到垂直車位的兩個(gè)頂點(diǎn)坐標(biāo)為(xa，ya)和(xb，yb)，可得汽車相對(duì)于車位的初始位置航向角θ為

圖3 垂直泊車第1次停車點(diǎn)示意圖

如圖3定義初始位置航向角向上為正，向下為負(fù)。進(jìn)行泊車路徑規(guī)劃時(shí)要確定以下幾個(gè)關(guān)鍵點(diǎn)的坐標(biāo)。

（1）泊車起始點(diǎn)S1坐標(biāo)。假設(shè)汽車行駛到S1點(diǎn)時(shí)，以最小轉(zhuǎn)向半徑R倒車，考慮車位右頂點(diǎn)(xb，yb)的約束，汽車在S2點(diǎn)第1次停車，駛過的圓弧路徑為

根據(jù)汽車運(yùn)動(dòng)幾何關(guān)系，易證車身M點(diǎn)在倒車過程中與車位右頂點(diǎn)(xb，yb)距離最近。設(shè)安全閾值LS1=0.1 m，可得點(diǎn)O1(xO1，yO1)的坐標(biāo)為

從而求得起始點(diǎn)S1的坐標(biāo)為

圖4 旋轉(zhuǎn)坐標(biāo)系后的垂直泊車工況

式中LS2=0.1 m，為設(shè)置的S2點(diǎn)安全閾值。同時(shí)為保證泊車終止點(diǎn)位姿良好，應(yīng)使汽車最終停車點(diǎn)的航向角在OX1Y1坐標(biāo)系下接近90°，如圖5所示。若汽車沿規(guī)劃路徑行駛中滿足上述條件，則可實(shí)現(xiàn)一步泊車入庫。

圖5 垂直泊車一步泊車入庫

（2）停車點(diǎn)S2坐標(biāo)。若汽車行駛到S2點(diǎn)時(shí)車身左后頂點(diǎn)Nd不滿足式（5），則須多步入庫，此時(shí)須進(jìn)行下一步路徑規(guī)劃。汽車基于OX1Y1坐標(biāo)系下航向角變化量如圖6所示。

圖6 多步泊車路徑航向角變化

由圖6幾何關(guān)系可以求出：

汽車從路徑點(diǎn)S1到路徑點(diǎn)S2駛過的航向角ψ1為

根據(jù)求得的航向角ψ1，計(jì)算S2點(diǎn)的坐標(biāo)為

（3）前進(jìn)點(diǎn)S3坐標(biāo)。接下來汽車應(yīng)調(diào)整轉(zhuǎn)向盤轉(zhuǎn)角為零，沿著直線路徑S2S3朝前行駛到S3點(diǎn)，最后再以最小轉(zhuǎn)向半徑R倒車入庫，路徑曲線為規(guī)劃后多步泊車路徑如圖7所示。

圖7 垂直泊車多步泊車入庫

設(shè)直線段路徑S2S3的長度為Lb，則根據(jù)幾何關(guān)系可以求得

則S3點(diǎn)坐標(biāo)為

汽車從路徑點(diǎn)S3到路徑點(diǎn)S4駛過的航向角ψ2為

根據(jù)ψ2的值可得路徑的圓心坐標(biāo)：

最后求得S4點(diǎn)坐標(biāo)：

1.4 垂直泊車路徑改進(jìn)

當(dāng)汽車行駛到S3點(diǎn)時(shí)，需要重新對(duì)路徑進(jìn)行規(guī)劃。實(shí)際泊車過程中，由于存在傳感器、執(zhí)行器等多種誤差，可能會(huì)導(dǎo)致汽車不能準(zhǔn)確跟蹤期望路徑，那么基于最小轉(zhuǎn)向半徑的路徑規(guī)劃有可能導(dǎo)致路徑跟蹤失?。ㄒ蛐枰霓D(zhuǎn)向半徑小于最小半徑），進(jìn)而造成泊車失敗。為增加泊車算法的魯棒性，同時(shí)減小汽車原地轉(zhuǎn)向現(xiàn)象，本文中選用四次樣條函數(shù)改進(jìn)段路徑，使其曲率連續(xù)并滿足汽車轉(zhuǎn)向約束。四次樣條函數(shù)公式可表示為

根據(jù)規(guī)劃路徑起始點(diǎn)位置和航向角，可得

根據(jù)路徑終止點(diǎn)位置和航向角，可得

最后，確定障礙物約束，即在倒車過程中不能與障礙物發(fā)生刮蹭。選取一個(gè)約束點(diǎn)為中間點(diǎn)，可得

根據(jù)式（16）～式（20），可以得到一條唯一的四次樣條曲線，該曲線即為規(guī)劃的路徑。在垂直泊車過程中為使得生成路徑滿足汽車轉(zhuǎn)向半徑約束，將S3的位置沿當(dāng)前航向角方向延長一段距離Ld1，則延長后的樣條函數(shù)起始點(diǎn)坐標(biāo)為

相應(yīng)地將泊車終止點(diǎn)坐標(biāo)S4向下延長一段距離Ld2，則延長后樣條函數(shù)終止點(diǎn)坐標(biāo)為

選取不同的中間點(diǎn)，會(huì)生成不同曲率的曲線，這里以考慮車位左端點(diǎn)約束時(shí)的S2點(diǎn)為參考點(diǎn)，并對(duì)其進(jìn)行修正作為中間點(diǎn)：

式中Δs為修正值。根據(jù)式（21）～式（23），可以得到系數(shù)確定的四次樣條函數(shù)曲線。

1.5 垂直泊車橫向跟蹤控制模塊

自動(dòng)泊車系統(tǒng)控制轉(zhuǎn)向盤轉(zhuǎn)角，使汽車在前進(jìn)方向的垂向產(chǎn)生速度，從而使汽車沿已規(guī)劃路徑行駛稱為橫向控制，主要使用的算法有預(yù)瞄控制、PID控制、模糊控制、LQR和MPC控制等［3-7］。國外MacAdam提出了最優(yōu)預(yù)瞄控制模型［8］。

國內(nèi)郭孔輝院士提出了單點(diǎn)預(yù)瞄最優(yōu)曲率模型［9］，其原理如圖8所示。假設(shè)汽車以恒定速度v行駛，汽車跟蹤的路徑方程為f(t)，路徑上點(diǎn)即為理想預(yù)瞄點(diǎn)。駕駛員根據(jù)前方路徑點(diǎn)的信息，結(jié)合汽車自身運(yùn)動(dòng)狀態(tài)，對(duì)汽車到達(dá)期望點(diǎn)的誤差進(jìn)行估計(jì)，計(jì)算一個(gè)最優(yōu)圓弧路徑，并根據(jù)圓弧曲率半徑與轉(zhuǎn)向盤轉(zhuǎn)角的關(guān)系得到一個(gè)轉(zhuǎn)向盤轉(zhuǎn)角的期望輸入。最優(yōu)圓弧路徑在實(shí)踐中一般通過實(shí)車反復(fù)調(diào)試得到。若汽車后軸中心點(diǎn)的實(shí)際軌跡為y(t)，汽車側(cè)向加速度為y，在較低行駛速度下，根據(jù)阿克曼轉(zhuǎn)向原理，汽車軌跡曲率可視為與等效前輪轉(zhuǎn)角成正比：

圖8 最優(yōu)預(yù)瞄曲率原理

式中：r為道路曲率半徑；δf為等效前輪轉(zhuǎn)角；R1為汽車轉(zhuǎn)彎半徑；i為轉(zhuǎn)向系傳動(dòng)比。

此時(shí)汽車橫向位置為y(t1)，設(shè)駕駛員預(yù)瞄距離為d，預(yù)瞄時(shí)間為T=d/v。經(jīng)過一個(gè)預(yù)瞄時(shí)間T后，汽車橫向位置變成y(t1+T)，對(duì)該式進(jìn)行泰勒級(jí)數(shù)展開，忽略高階項(xiàng)，保留其2階項(xiàng)得

根據(jù)誤差最小原則，駕駛員期望得到一條最優(yōu)軌跡，使得汽車經(jīng)過一段時(shí)間T后可以到達(dá)預(yù)期位置f(t1+T)。由式（24）～式（26）可以得到基于最優(yōu)曲率的轉(zhuǎn)向盤轉(zhuǎn)角：

式中δsw為轉(zhuǎn)向盤轉(zhuǎn)角，且δsw=δfi。

根據(jù)控制需要，規(guī)劃出來的汽車路徑由一系列坐標(biāo)點(diǎn)組成。在實(shí)際跟蹤過程中，根據(jù)當(dāng)前汽車位置，選擇一個(gè)合理預(yù)瞄距離d，通過計(jì)算得到參考路徑距預(yù)瞄點(diǎn)最近的點(diǎn)(xj，yj)與預(yù)瞄點(diǎn)(xd，yd)的距離，這段距離稱之為預(yù)瞄誤差e(j)。預(yù)瞄誤差示意圖如圖9所示。由圖可知，為使汽車實(shí)際行駛路徑與期望路徑誤差最小，由式（27）可得預(yù)瞄誤差與此時(shí)轉(zhuǎn)向盤轉(zhuǎn)角δe之間的傳遞函數(shù)：

圖9 預(yù)瞄誤差

式中j為預(yù)瞄點(diǎn)序號(hào)。

當(dāng)前很多研究采用預(yù)瞄誤差前饋控制方法對(duì)規(guī)劃路徑進(jìn)行跟蹤。由于跟蹤中存在模型誤差和環(huán)境干擾等因素，僅依靠預(yù)瞄誤差前饋的控制方法有時(shí)難以保證精確的跟蹤效果和良好的跟蹤穩(wěn)定性?？紤]到實(shí)際泊車過程中，汽車航向角對(duì)路徑跟蹤效果影響較大，因此本文中增加汽車航向角反饋環(huán)節(jié)來同時(shí)對(duì)轉(zhuǎn)向盤轉(zhuǎn)角進(jìn)行調(diào)節(jié)，即采用預(yù)瞄誤差前饋加航向角反饋的控制方法對(duì)規(guī)劃路徑進(jìn)行跟蹤。

如圖10所示，在某一時(shí)刻汽車位置為(xrk，yrk，ψrk)，路 徑 上 距 離 汽 車 最 近 的 一 點(diǎn) 坐 標(biāo) 為(xk，yk，ψk)，則汽車此時(shí)的航向角誤差λerr定義為

圖10 汽車航向角誤差示意圖

式中k為路徑點(diǎn)序號(hào)。

采用PID算法減小航向角誤差，調(diào)節(jié)轉(zhuǎn)向盤轉(zhuǎn)角。轉(zhuǎn)向盤轉(zhuǎn)角的PID控制表達(dá)式為

式中：kp、ki和kd分別為比例、積分、微分系數(shù)?？紤]預(yù)瞄誤差前饋，結(jié)合航向角反饋，則轉(zhuǎn)向盤轉(zhuǎn)角δsw為

整個(gè)泊車路徑跟蹤模塊控制框架如圖11所示。

圖11 預(yù)瞄控制器框架圖

2 仿真驗(yàn)證

基于Simulink/CarSim對(duì)所提出的泊車路徑跟蹤算法進(jìn)行了聯(lián)合仿真。首先在CarSim中根據(jù)目標(biāo)參數(shù)建立了汽車模型和停車位場(chǎng)景，并導(dǎo)入Simulink。然后在Simulink中搭建了基于樣條函數(shù)的路徑規(guī)劃模塊，并建立了預(yù)瞄誤差前饋加航向角反饋的跟蹤控制模塊，最后進(jìn)行聯(lián)合仿真。

對(duì)汽車初始位置航向角分別為5°、0°和-5°時(shí)自動(dòng)垂直泊車系統(tǒng)路徑規(guī)劃及路徑跟蹤結(jié)果進(jìn)行仿真，結(jié)果如圖12～圖14所示。

圖12 初始位置航向角為5°的泊車仿真

圖14 初始位置航向角為-5°的泊車仿真

從仿真結(jié)果可以看出，在汽車初始位置航向角分別為5°、0°和-5°時(shí)，路徑規(guī)劃模塊都規(guī)劃出了泊車路徑。圖12（a）和圖13（a）泊車過程中，僅使用預(yù)瞄誤差前饋控制方法時(shí)，對(duì)曲率不變的路徑和跟蹤效果良好。在對(duì)四次樣條函數(shù)規(guī)劃的路徑進(jìn)行跟蹤時(shí)，由于曲率是變化的，航向角也隨之改變，采用單一的預(yù)瞄誤差前饋控制跟蹤存在較大的滯后現(xiàn)象，跟蹤效果較差，最大橫向誤差分別達(dá)到了25和20 cm，最終泊車姿態(tài)不太理想，偏角分別為7°和5°。而在加入了航向角反饋后（圖12（b）、圖13（b）），在曲率不變路徑跟蹤效果同樣較好，而在曲率變化的路徑，最大橫向誤差分別減小到6和5 cm，跟蹤精度更高，跟蹤響應(yīng)速度快，且最終泊車姿態(tài)良好，偏角均小于5°。由圖14（a）可以看出，在初始位置航向角為-5°時(shí)，由于曲率變化引起汽車航向角變化量較大，僅使用預(yù)瞄誤差前饋的控制模塊不能及時(shí)對(duì)汽車轉(zhuǎn)向盤轉(zhuǎn)角進(jìn)行調(diào)整，導(dǎo)致汽車在跟蹤時(shí)側(cè)向偏差最大達(dá)到了40 cm（點(diǎn)P），最終泊車位置姿態(tài)角達(dá)到了35°，汽車輪廓線與車位輪廓線發(fā)生了剮蹭，泊車失敗。由圖14（b）可以看出，按本文方法加入了航向角反饋控制后，汽車轉(zhuǎn)向盤調(diào)整及時(shí)，最大側(cè)向誤差點(diǎn)P的誤差小于5 cm，汽車能夠準(zhǔn)確地跟蹤規(guī)劃路徑，順利泊入車位，且最終泊車姿態(tài)良好，偏角小于5°。

圖13 初始位置航向角為0°的泊車仿真

因此，在泊車路徑跟蹤過程中，對(duì)于曲率不變路徑，預(yù)瞄誤差前饋控制就能較好地對(duì)路徑進(jìn)行跟蹤，但對(duì)于曲率變化的路徑，預(yù)瞄誤差前饋控制跟蹤誤差變大且具有較大滯后現(xiàn)象。在預(yù)瞄誤差前饋的基礎(chǔ)上加入了航向角反饋控制后，對(duì)曲率不變和曲率變化路徑都具有較好的跟蹤效果，且最終泊車位姿良好。

3 垂直泊車路徑規(guī)劃與跟蹤實(shí)車驗(yàn)證

3.1 試驗(yàn)平臺(tái)

為了對(duì)本文中提出的垂直泊車路徑規(guī)劃和路徑跟蹤策略進(jìn)行工程驗(yàn)證，利用一輛試驗(yàn)車進(jìn)行了多組實(shí)車試驗(yàn)。試驗(yàn)車如圖15所示，該車具有ETRS電子排擋和EPS電動(dòng)助力轉(zhuǎn)向系統(tǒng)，能夠?qū)崿F(xiàn)對(duì)轉(zhuǎn)向盤轉(zhuǎn)角、加速、制動(dòng)踏板和擋位的自動(dòng)控制。

圖15 試驗(yàn)汽車示意圖

汽車實(shí)車試驗(yàn)平臺(tái)如圖16所示，主要由以下部分組成：（1）感知系統(tǒng)：主要包括環(huán)視攝像頭（檢測(cè)停車位信息）和12個(gè)超聲波雷達(dá)（實(shí)現(xiàn)避障）；（2）攝像頭控制器：分析環(huán)視攝像頭采集到的數(shù)據(jù)，以及汽車自身數(shù)據(jù)，得到停車位角點(diǎn)坐標(biāo)；（3）泊車控制器（infineon TC233）：運(yùn)行基于四次樣條函數(shù)的路徑規(guī)劃算法，以及預(yù)瞄誤差前饋加航向角反饋路徑跟蹤算法，輸出轉(zhuǎn)向盤轉(zhuǎn)角信號(hào)、擋位、加速踏板和制動(dòng)踏板信號(hào)；（4）CAN盒（VPN1630）：負(fù)責(zé)整車的CAN通信，在其配套軟件CANoe上對(duì)整車報(bào)文進(jìn)行解碼和編制收發(fā)目標(biāo)報(bào)文；（5）UDE聯(lián)合調(diào)試器：負(fù)責(zé)將Hightec生成的可執(zhí)行程序燒入控制器以及全局變量的觀測(cè)和代碼調(diào)試；（6）PC端（ThinkPad T470P）：進(jìn)行數(shù)據(jù)分析、代碼修改和變量監(jiān)控；（7）供電模塊：為控制器提供12 V穩(wěn)定電壓。

圖16 汽車試驗(yàn)平臺(tái)組成

3.2 試驗(yàn)算法和結(jié)果分析

航向角是實(shí)車試驗(yàn)中重要的參數(shù)，但它難以借助一般的車載傳感器直接獲得，本試驗(yàn)通過以下算法計(jì)算得到車輛航向角：

其中：

式中：（x，y)為汽車后軸中心坐標(biāo)；Δb為等效后軸位移增量；Δθ為航向角增量；Δbcl、Δbcr分別為左后輪和右后輪位移增量，可以通過輪速脈沖和車輪半徑求得。

分別對(duì)初始位置航向角為10°、5°、0°和-5°的情況進(jìn)行了試驗(yàn)。期望后軸中心坐標(biāo)通過UDE聯(lián)合調(diào)試器從泊車控制器中讀出，實(shí)際后軸中心坐標(biāo)從汽車慣導(dǎo)數(shù)據(jù)獲得，并通過CAN總線由CANoe處理讀出，最后在MATLAB中繪圖。試驗(yàn)結(jié)果如圖17～圖20所示。

圖17 初始位置航向角10°的泊車試驗(yàn)

圖18 初始位置航向角5°的泊車試驗(yàn)

圖19 初始位置航向角0°的泊車試驗(yàn)

圖20 初始位置航向角-5°的泊車試驗(yàn)

由試驗(yàn)結(jié)果可以看出，在初始位置航向角為10°、5°、0°和-5°的情況下，汽車均能順利規(guī)劃并跟蹤泊車路徑，泊入車位，跟蹤誤差小于5 cm，最終停車點(diǎn)位姿角小5°，跟蹤精度較高，控制效果較好。說明本文中提出的垂直泊車路徑規(guī)劃方法和控制策略能夠較好地實(shí)現(xiàn)實(shí)際交通環(huán)境中的垂直泊車問題，具有較好的應(yīng)用前景。

4 結(jié)論

對(duì)具有初始位置航向角的泊車工況進(jìn)行了路徑規(guī)劃，并結(jié)合四次樣條函數(shù)對(duì)路徑進(jìn)行了改進(jìn)。設(shè)計(jì)了一種預(yù)瞄誤差前饋加航向角反饋的跟蹤控制算法，在規(guī)劃路徑曲率不變和變化的情況下都能夠較好地跟蹤規(guī)劃路徑，跟蹤精度高，魯棒性強(qiáng)。基于Simulink/CarSim進(jìn)行了聯(lián)合仿真，仿真結(jié)果表明在具有一定初始航向角的情況下，路徑規(guī)劃模塊均能規(guī)劃出一條可行路徑，路徑跟蹤控制模塊可以對(duì)其進(jìn)行精準(zhǔn)跟蹤。實(shí)車試驗(yàn)也驗(yàn)證了本文提出垂直泊車路徑規(guī)劃和路徑跟蹤方法的有效性。