基于分段高斯偽譜法的平行自主泊車路徑規(guī)劃*

錢立軍，吳 冰，仇多洋，胡偉龍

（合肥工業(yè)大學(xué)汽車與交通工程學(xué)院，合肥 230009）

前言

隨著輔助駕駛和無人駕駛技術(shù)的發(fā)展，汽車自動化、智能化程度在逐漸提高，其中自主泊車技術(shù)可以減小駕駛員在面對狹小空間泊車時的難度，同時也提高了駕駛員舒適性和泊車安全性。自主泊車技術(shù)研究包括3個方面：環(huán)境感知、路徑規(guī)劃和路徑跟蹤控制。路徑規(guī)劃作為其中一項(xiàng)重要技術(shù)，對此國內(nèi)外學(xué)者已做大量研究，文獻(xiàn)［1］和文獻(xiàn)［2］中基于幾何方法構(gòu)造泊車路徑，通過汽車從泊車位逆向駛出和起始泊車位置結(jié)合，以最小轉(zhuǎn)彎半徑的圓弧與其他半徑的圓弧相切進(jìn)行連接得到泊車路徑，該方法較為簡單實(shí)用，但得到的路徑曲率不連續(xù)。文獻(xiàn)［3］～文獻(xiàn)［5］中分別采用回旋曲線、B樣條曲線和反正切多項(xiàng)式曲線進(jìn)行泊車路徑規(guī)劃，解決了幾何路徑曲率不連續(xù)的問題，但需要較大的車位空間，在面對狹小空間時無法完成路徑規(guī)劃。文獻(xiàn)［6］和文獻(xiàn)［7］中基于智能算法進(jìn)行泊車路徑規(guī)劃和優(yōu)化，將平行自主泊車過程分為3段，采用模糊控制方法設(shè)計(jì)泊車控制算法；以最短路徑為目標(biāo)，采用遺傳算法對垂直泊車路徑進(jìn)行優(yōu)化，可擴(kuò)大泊車初始區(qū)域。文獻(xiàn)［8］和文獻(xiàn)［9］中基于最優(yōu)化和動態(tài)優(yōu)化理論，分別采用傳統(tǒng)高斯偽譜法和聯(lián)立動態(tài)優(yōu)化法對自主泊車路徑規(guī)劃問題進(jìn)行求解。

針對以上問題，本文中提出一種在不同泊車區(qū)域內(nèi)采用分段高斯偽譜法求解的方法，在快速求解前提下以最短泊車完成時間為最優(yōu)目標(biāo)函數(shù)，建立車輛運(yùn)動學(xué)模型、動力學(xué)約束、端點(diǎn)約束和路徑約束，用最優(yōu)控制問題描述自主泊車路徑規(guī)劃問題。通過分段高斯偽譜法先將最優(yōu)控制問題進(jìn)行離散化處理，根據(jù)泊車區(qū)域不同建立相應(yīng)的約束模型，再采用內(nèi)點(diǎn)法對非線性問題進(jìn)行求解。對5種不同平行泊車工況路徑規(guī)劃進(jìn)行仿真分析，并與傳統(tǒng)偽譜法進(jìn)行對比，最后通過實(shí)車試驗(yàn)驗(yàn)證了該方法得到的自主泊車路徑的有效性。

1 車輛泊車運(yùn)動過程分析

1.1 車輛運(yùn)動學(xué)模型建立

車輛在自主泊車過程中車速較低，可忽略輪胎發(fā)生側(cè)滑，根據(jù)阿克曼前輪轉(zhuǎn)向原理，建立車輛自主泊車運(yùn)動學(xué)模型，如圖1所示。

圖中，R（x，y）為車輛后軸中心點(diǎn)，v為后軸中心點(diǎn)速度且方向沿車輛軸線，O為車輛瞬時轉(zhuǎn)彎圓心，L為車輛軸距，f和r分別為車輛的前軸到車頭距離和后軸到車尾距離，d為車輛寬度，φ為前輪擺角，θ為車身姿態(tài)角，A，B，C，D分別為車身的4個頂點(diǎn)。

根據(jù)車輛運(yùn)動學(xué)模型可得運(yùn)動學(xué)微分方程如下：

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

式中：a為車輛加速度；ω為前輪擺角角速度。為了提高駕乘人員的舒適性，對車輛加速度變化率j進(jìn)行了約束。

根據(jù)幾何關(guān)系，由后軸中心點(diǎn)R（x，y）可得A，B，C，D的坐標(biāo)值如下：

1.2 泊車避障約束分析

自主泊車過程可看作是車輛在道路和障礙車輛組成空間內(nèi)的避障運(yùn)動，泊車路徑必須滿足在泊車過程中無碰撞發(fā)生和自身約束條件。平行自主泊車區(qū)域如圖2所示。圖中Ls1＋Ls2和Lp分別為道路和泊車位的長度，Ws和Wp分別為道路和泊車位的寬度。

圖2 平行自主泊車區(qū)域示意圖

根據(jù)文獻(xiàn)［10］對上述自主泊車空間劃分為兩個區(qū)域，分別為自由運(yùn)動區(qū)域Zf和臨近泊車位區(qū)域Zp，通過分別對兩個空間建立不同邊界約束，減少約束條件規(guī)模，提高自主泊車最優(yōu)化控制問題的求解速度，兩區(qū)域范圍表示為

區(qū)域Zf和Zp構(gòu)成了泊車區(qū)域，因此車輛4個頂點(diǎn)A，B，C，D在泊車過程中需要落在區(qū)域Zf和Zp中。

雖然上述約束可避免車輛頂點(diǎn)不與障礙物發(fā)生碰撞，但不能保證車身與障礙物之間不發(fā)生碰撞，車輛從區(qū)域Zf進(jìn)入Zp時，如圖2所示，車身BC邊可能會與P點(diǎn)發(fā)生碰撞，若無碰撞發(fā)生，在車輛進(jìn)入?yún)^(qū)域Zp之前，BC邊需保持處于P點(diǎn)的左側(cè)。由幾何關(guān)系可得

2 泊車最優(yōu)控制問題建立

2.1 最優(yōu)控制問題

不失一般性，非線性最優(yōu)控制Bolza問題可以描述為

式中的系統(tǒng)狀態(tài)變量和控制變量分別為x和μ，且x（t）∈Rn，μ（t）∈Rm，t0和tf分別為系統(tǒng)初始時刻和終止時刻。min J為最優(yōu)控制問題的目標(biāo)函數(shù)，式（9）中第1式為系統(tǒng)狀態(tài)微分方程，第2式為路徑約束不等式方程，第3式為端點(diǎn)約束等式方程。

2.2 車輛動力學(xué)條件約束

車輛在運(yùn)動過程中受到車輛自身物理?xiàng)l件約束，其對系統(tǒng)的部分狀態(tài)變量和控制變量的約束為

式中：vmax為車輛運(yùn)動的最大車速；amax為最大加速度；jmax為最大加速度變化率；φmax為最大前輪擺角；ωmax為最大前輪擺角角速度。

2.3 端點(diǎn)約束

基于上述的運(yùn)動學(xué)模型和約束條件，車輛可以完成從起始時刻t0到終止時刻tf的無碰撞運(yùn)動，本文中僅考慮在t0和tf時刻車輛為靜止情況，起始時刻t0車輛后軸中心點(diǎn)處于指定位置（x0，y0），終止時刻tf車輛4個頂點(diǎn)均在泊車位空間內(nèi)，在t0和tf時刻都要求車身水平，轉(zhuǎn)向盤處于中間位置，即車身姿態(tài)角和轉(zhuǎn)向盤轉(zhuǎn)角均為0°，端點(diǎn)約束條件為

2.4 最優(yōu)目標(biāo)函數(shù)

自主泊車技術(shù)解決交通問題其中之一就是避免造成交通阻塞，幫助駕駛員快速完成車輛泊車入位，減少對車道的占用時間，因此本文中選用最短泊車完成時間min tf為最優(yōu)控制問題的目標(biāo)函數(shù)。

3 分段高斯偽譜法算法設(shè)計(jì)

高斯偽譜法（GPM）是一種直接求解最優(yōu)化方法，該方法首先對連續(xù)的最優(yōu)化問題進(jìn)行離散化處理，再進(jìn)行非線性規(guī)劃問題求解［11-12］，但該方法為全局優(yōu)化方法，隨著問題約束數(shù)量增多和求解精度要求提高，需要增加大量配置點(diǎn)而導(dǎo)致計(jì)算時間急劇增加，甚至出現(xiàn)求解不收斂。本文中提出一種在不同泊車區(qū)域內(nèi)采用分段高斯偽譜法求解的方法，并對區(qū)域Zf和區(qū)域Zp連接處的時刻條件進(jìn)行松弛處理，提高最優(yōu)控制問題求解收斂速率。

3.1 時域變換

式（8）中最優(yōu)控制問題的時間取值范圍為t∈［t0，tf］，將時間分成p段，即0＝t0＜t1…tq-1＜tq＜…tp＝tf，由此每段時間域的步長為

由于GPM中選取離散點(diǎn)的區(qū)間為τ∈［-1，1］，因此通過時域變換將第q段上時間變量轉(zhuǎn)換為變量τ，變換如下：

由式（13）可得每段時間區(qū)域內(nèi)的最優(yōu)控制問題，其描述如下：

3.2 變量離散化

GPM是將控制變量和狀態(tài)變量均需要在Legendre-Gauss（LG）配置點(diǎn)上進(jìn)行離散化，再通過構(gòu)建拉格朗日插值函數(shù)逼近控制變量和狀態(tài)變量，在區(qū)間［τ0，τf］上，N階的Legendre多項(xiàng)式為

以式（15）的零點(diǎn)和τ0組成N＋1個配置點(diǎn)，在每個t（q）時間域中，狀態(tài)變量、控制變量由多項(xiàng)式近似表示為

式（16）中，Lagrange插值基函數(shù)為

由式（16）第1式求導(dǎo)，可將插值函數(shù)微分逼近狀態(tài)變量的微分形式，得到在τk點(diǎn)的狀態(tài)變量導(dǎo)數(shù)，可近似表示為

結(jié)合式（14）和式（18）可得微分代數(shù)等式約束：

離散后的路徑約束和端點(diǎn)約束分別為

在自主泊車過程中需要保證路徑曲率連續(xù)性，因此狀態(tài)變量也必須為連續(xù)變化，即每段終端時刻狀態(tài)變量作為下一段初始時刻狀態(tài)變量，控制變量可以不連續(xù)，其連接處約束等式為

由于LG配置點(diǎn)中不包含終端時刻狀態(tài)值，由式（22），根據(jù)Legendre-Gauss型求積公式計(jì)算終端時刻狀態(tài)變量：

式中Ak和f（τk）分別為高斯積分權(quán)重系數(shù)和求積函數(shù)。

經(jīng)過上述離散化過程，可將自主泊車路徑規(guī)劃的最優(yōu)控制問題轉(zhuǎn)化為非線性規(guī)劃問題進(jìn)行求解，可采用SQP、內(nèi)點(diǎn)法等方法，本文中使用IPOPT［13］內(nèi)點(diǎn)法工具箱進(jìn)行求解。

3.3 改進(jìn)的分段高斯偽譜法

在最優(yōu)控制問題的約束數(shù)量較多時，傳統(tǒng)偽譜法為了滿足求解精度會增加大量配置點(diǎn)，這導(dǎo)致求解速度和收斂速度都會降低，因此，一方面可通過選取合適的配置點(diǎn)數(shù)和段數(shù)提高求解效率［14-15］，另一方面針對研究對象將最優(yōu)控制問題分解為若干求解難度較低的子問題后再進(jìn)行求解［16-17］。根據(jù)上文中的自主泊車避障約束分析，將泊車空間劃分為區(qū)域Zf和區(qū)域Zp，由于兩個區(qū)域連接點(diǎn)位置不確定，因此將此連接點(diǎn)處的狀態(tài)參數(shù)和控制參數(shù)作為待優(yōu)化的參數(shù)。

本文中提出一種改進(jìn)的分段高斯偽譜法，首先對控制問題進(jìn)行無最優(yōu)目標(biāo)全局求解，將求得最優(yōu)解作為后續(xù)優(yōu)化的初值，再分別在區(qū)域Zf和區(qū)域Zp內(nèi)采用高斯偽譜法對自主泊車路徑最優(yōu)控制問題進(jìn)行求解，減小求解過程中約束規(guī)模，提高算法收斂性，其算法描述見表1。

表1 算法描述

4 仿真分析

根據(jù)車輛自主泊車運(yùn)動學(xué)模型，通過Matlab／Simulink搭建自主泊車仿真平臺，以某SUV為仿真對象，其車輛參數(shù)和物理約束如表2所示。

本文中以平行自主泊車工況為例進(jìn)行路徑規(guī)劃算法驗(yàn)證和仿真分析。為了驗(yàn)證算法統(tǒng)一有效性，針對5種不同長度車位選取同一泊車起始位置，選取后軸中心點(diǎn)位置為x0＝9 m；y0＝1.5 m，車位長度Lp分別為4.8～7.8 m，即分別為車身長度的1.1～1.8倍，車位寬度Wp為2.5 m，道路寬度Ws為3.5 m，車輛泊入車位的起始條件和終止條件如式（11）所示，仿真結(jié)果如圖3～圖12所示。

表2 車輛參數(shù)和物理約束

圖3 L p＝7.8 m自主泊車示意圖

通過上文的自主泊車示意圖可知，在泊車的過程中，車輛與道路邊界和前后車輛未發(fā)生碰撞，泊車結(jié)束時，車輛處于泊車位空間內(nèi)且車身水平滿足自主泊車路徑的避障約束和端點(diǎn)約束條件，結(jié)果表明分段高斯偽譜法可對自主泊車路徑規(guī)劃最優(yōu)控制問題進(jìn)行求解并得到滿足條件的泊車路徑。

由圖3和圖5可知，在泊車空間較大工況下，車輛經(jīng)過一次倒退和一次車位內(nèi)向前微調(diào)完成泊車過程，由圖4和圖6可知，泊車路徑較平滑且完成泊車用時較短，分別為11.363和11.816 s，狀態(tài)變量、控制變量都滿足約束條件。由圖7、圖9和圖11可知，隨著泊車空間逐漸減小，車輛在區(qū)域Zp內(nèi)前后運(yùn)動次數(shù)和泊車完成時間也逐漸增加，泊車用時分別為16.337，24.078和55.057 s，這也與駕駛員日常泊車過程類似，但從區(qū)域Zf到區(qū)域Zp的過程中路徑依然較為平滑。由圖8、圖10和圖12可知，車輛通過左右轉(zhuǎn)動轉(zhuǎn)向盤分別在泊車位內(nèi)經(jīng)過2，5和19次前后移動微調(diào)直至泊車完成，對比發(fā)現(xiàn)在泊車位長度小于車身長度1.5倍時，車輛在泊車位內(nèi)的微調(diào)次數(shù)急劇增加。在圖12中，輪胎擺角角速度最大絕對值為2.904 2 rad／s，其數(shù)值超過上文設(shè)定約束范圍，表明車輛在狹小泊車位內(nèi)進(jìn)行微調(diào)時出現(xiàn)原地轉(zhuǎn)向現(xiàn)象。

圖4 L p＝7.8 m自主泊車仿真結(jié)果

圖5 L p＝6.8 m自主泊車示意圖

對上述5種不同長度泊車位平行泊車工況采用傳統(tǒng)GPM方法進(jìn)行求解，并與本文方法求解結(jié)果進(jìn)行對比分析，兩種不同高斯偽譜法的泊車完成用時最優(yōu)目標(biāo)tf和問題求解CPU計(jì)算耗時結(jié)果如圖13所示，在泊車位長度為4.8和5.3 m時，采用傳統(tǒng)GPM求解最優(yōu)控制問題時未收斂，在泊車位較大時，兩種方法都能夠收斂，泊車完成用時tf較為接近，但分段GPM在計(jì)算耗時方面占優(yōu)，分別為傳統(tǒng)GPM的15.85%，17.69%和27.44%，這是由于本文算法選用合理初值，在兩個泊車區(qū)域內(nèi)分別建立約束條件，減小了約束條件數(shù)量規(guī)模和運(yùn)算復(fù)雜度，提高了算法收斂性。

圖6 L p＝6.8 m自主泊車仿真結(jié)果

圖7 L p＝5.8 m自主泊車示意圖

5 實(shí)車試驗(yàn)

試驗(yàn)車采用超聲波傳感器作為環(huán)境感知傳感器，為自主泊車控制系統(tǒng)提供車位探測信息和車輛與周圍障礙物距離信息。電動助力轉(zhuǎn)向系統(tǒng)的角度傳感器提供轉(zhuǎn)向盤角度信號，制動系統(tǒng)的輪速傳感器和加速度傳感器分別提供輪速脈沖信號、縱向加速度信號和橫擺角速度信號，自主泊車控制器對上述信號處理得到泊車目標(biāo)路徑信息，包括目標(biāo)轉(zhuǎn)向盤轉(zhuǎn)角、目標(biāo)車速和目標(biāo)擋位等，通過轉(zhuǎn)向系統(tǒng)、制動系統(tǒng)、動力系統(tǒng)和擋位系統(tǒng)分別實(shí)現(xiàn)對試驗(yàn)車的轉(zhuǎn)向控制、車速控制和擋位控制，各系統(tǒng)之間信號采用CAN總線進(jìn)行通信，如圖14所示。

圖8 L p＝5.8 m自主泊車仿真結(jié)果

圖9 L p＝5.3 m自主泊車示意圖

考慮到超聲波傳感器探測存在盲區(qū)，為了避免車輛發(fā)生碰撞，車輛在泊車位內(nèi)有效運(yùn)動范圍比實(shí)際泊車位小，因此泊車路徑規(guī)劃時采用有效運(yùn)動范圍作為算法輸入?yún)?shù)。本文中試驗(yàn)車上裝配的短距超聲波傳感器盲區(qū)范圍為0～0.15 m，所以規(guī)定有效車位長度比實(shí)測車位長度小0.3 m，試驗(yàn)中實(shí)際泊車位長度為在前述仿真中5種不同車位長度上分別增加0.3 m，泊車起始橫向位置為9.15 m，其他參數(shù)設(shè)置與仿真工況相同。車輛在駛過車位時，通過側(cè)向長距超聲波傳感器進(jìn)行車位探測，行駛至起始泊車位置并保持車輛靜止，自主泊車系統(tǒng)接管車輛，采用仿真得到的路徑數(shù)據(jù)指導(dǎo)車輛進(jìn)行泊車，實(shí)車試驗(yàn)如圖15所示。

圖10 L p＝5.3 m自主泊車仿真結(jié)果

圖11 L p＝4.8 m自主泊車示意圖

通過記錄實(shí)測車位長度、泊車過程中車輛是否與障礙物發(fā)生碰撞，以及測量泊車完成后車輛外側(cè)頂點(diǎn)A與D縱向位置和車身姿態(tài)角來判斷泊車完成情況，試驗(yàn)結(jié)果如表3所示。

由試驗(yàn)結(jié)果可知，由于超聲波傳感器波束角的物理特性導(dǎo)致對車位的測量長度比實(shí)際長度小，將測量長度作為路徑規(guī)劃算法的輸入?yún)?shù)也保證了泊車過程中的安全性。在泊車過程中車輛與障礙物未發(fā)生碰撞現(xiàn)象，泊車完成后，從頂點(diǎn)位置數(shù)據(jù)對比可知，隨著車位長度逐漸減小，車輛在縱向上也逐漸靠近車位外側(cè)，由于車位長度較小時車輛在泊車位內(nèi)前后移動微調(diào)次數(shù)增多，在微調(diào)過程中車輛逐漸偏向車位外側(cè)，但試驗(yàn)中完成泊車后，車輛外側(cè)的頂點(diǎn)均落在車位內(nèi)，車身姿態(tài)角在±1.5°范圍以內(nèi)，因此采用本文方法得到的路徑信息可有效指導(dǎo)自主泊車完成。

圖12 L p＝4.8 m自主泊車仿真結(jié)果

圖13 兩種方法求解結(jié)果對比圖

圖14 試驗(yàn)車自主泊車系統(tǒng)架構(gòu)圖

圖15 自主泊車平行工況實(shí)車試驗(yàn)

表3 實(shí)車試驗(yàn)結(jié)果

6 結(jié)論

（1）本文中提出一種在不同泊車區(qū)域采用分段高斯偽譜法的自主泊車路徑規(guī)劃方法，通過建立車輛運(yùn)動學(xué)模型、動力學(xué)約束、端點(diǎn)約束和路徑避障約束，將泊車路徑規(guī)劃問題轉(zhuǎn)換為最優(yōu)控制問題，并以車輛泊車完成時間最短為最優(yōu)目標(biāo)函數(shù)，通過無最優(yōu)目標(biāo)求解得到問題合理初值，再根據(jù)不同泊車區(qū)域設(shè)置約束條件并進(jìn)行求解。

（2）選取5種不同長度泊車位的平行自主泊車工況進(jìn)行路徑規(guī)劃仿真，結(jié)果表明，所得路徑滿足自主泊車過程中約束條件，并與傳統(tǒng)偽譜法進(jìn)行對比分析表明，本文方法具有更快收斂速度。

（3）在實(shí)車試驗(yàn)中采用本文算法求解得到的路徑信息指導(dǎo)車輛進(jìn)行自主泊車，在泊車過程中無碰撞發(fā)生，車輛最終停在車位內(nèi)。實(shí)踐表明，本文的研究結(jié)果可為自主泊車提供有效參考路徑。