基于障礙物衍生狀態(tài)格的智能車避障軌跡規(guī)劃*

胡延平，田 博，陳無畏，張銳陳

（1.合肥工業(yè)大學(xué)機(jī)械工程學(xué)院，合肥 230009； 2.合肥工業(yè)大學(xué)汽車與交通工程學(xué)院，合肥 230009）

前言

根據(jù)行車路徑規(guī)劃算法的原理，將目前的軌跡規(guī)劃算法歸納為兩種：傳統(tǒng)算法和智能算法。傳統(tǒng)的路徑規(guī)劃算法有人工勢場法［1-2］和啟發(fā)式搜索算法（如 A*算法［3］，Dijkstra算法［4］，RRT算法［5］）等。其中文獻(xiàn)［6］中對A*算法和Dijkstra算法從搜索速度和搜索效率進(jìn)行比較。針對RRT算法很強(qiáng)的隨機(jī)性，文獻(xiàn)［7］中將人工勢場法中引力的思想引入RRT算法中，提高了算法的實(shí)時(shí)性。智能算法主要有蟻群算法［8-9］、模糊邏輯算法［10］、神經(jīng)網(wǎng)絡(luò)算法和遺傳算法［11-12］等。另外，對于整體的半結(jié)構(gòu)環(huán)境中，文獻(xiàn)［13］中通過 Bézier曲線［14］提出了在靜態(tài)規(guī)劃和動(dòng)態(tài)障礙規(guī)劃之間切換的路徑規(guī)劃方法。

狀態(tài)格（state lattice）是由 Kelly和 Pivtoraiko［15］提出的一種將離散圖嵌入到連續(xù)機(jī)器人配置空間中的路徑規(guī)劃方法。Madas等［16］曾將此方法與其他方法進(jìn)行分析比較。Kushleyev等［17］在該算法的基礎(chǔ)上考慮障礙物的運(yùn)動(dòng)不確定性，將短期軌跡規(guī)劃和無時(shí)間量的路徑規(guī)劃結(jié)合。本文中針對動(dòng)態(tài)障礙物，以障礙物的狀態(tài)為出發(fā)點(diǎn)建立衍生狀態(tài)格，通過設(shè)定S格成本，結(jié)合搜索算法處理多障礙物條件下的最優(yōu)軌跡規(guī)劃問題，與常規(guī)的柵格法相比無須對整個(gè)環(huán)境進(jìn)行網(wǎng)格劃分。最后通過仿真試驗(yàn)和硬件在環(huán)試驗(yàn)對該算法的可行性和有效性進(jìn)行了驗(yàn)證。

1 衍生狀態(tài)格的概念

在實(shí)際換道過程中對障礙車輛和自車進(jìn)行矩形包裹。衍生狀態(tài)格（下面簡稱S格）是由障礙物和避障車輛衍生的矩形方格，其尺寸同自車尺寸，其位置和狀態(tài)參數(shù)由障礙物確定，如圖1所示。

圖中：C，Z和S分別表示避障車輛、障礙物和S格；S1和S2分別由障礙物Z1和Z2衍生所得。障礙物和對應(yīng)S格的速度相等，即vz＝vs，S格和自車寬度相等即Ws＝Wc。W1為S格與障礙物相鄰側(cè)的垂直距離，W1數(shù)值的設(shè)定如圖2所示。

圖中：等效前輪轉(zhuǎn)角為γ，兩前輪轉(zhuǎn)角為α和β；l和 lr分別為軸距和后懸值。若剛好達(dá)到不碰撞效果，則滿足如下關(guān)系式：

圖2 W1數(shù)值的確定

由式（1）可知，在避免碰撞的前提下，W1的最小尺寸由兩前輪最大轉(zhuǎn)角決定，在路徑規(guī)劃中將W1作為定值。障礙物與對應(yīng)S格距離Wzs表達(dá)式為

Wzs＝（Wz+Ws）／2+W1

S格創(chuàng)建完成后，因?yàn)樽攒嚺cS格位置重合時(shí)可保證避障車輛與障礙物間碰撞的可能性為零，故避障軌跡便是在自車和S格之間規(guī)劃。

2 基于S格的避障軌跡規(guī)劃算法

2.1 正反梯形約束的橫擺角加速度模型

車輛換道采用非線性橫擺角參考模型，滿足正反梯形約束的橫擺角加速度模型［18］，采用車輛換道時(shí)期橫擺角加速度·θ·的參考模型如圖3所示。

圖3 期望橫擺角加速度參考模型

圖中 θdm為最大橫擺角加速度，令時(shí)間滿足：tbta＝td-tc＝te-td＝tg-tf＝ti-th＝tj-ti＝tl-tk＝T1，同時(shí)tctb＝tf-te＝th-tg＝tk-tj＝T2。為路徑規(guī)劃時(shí)計(jì)算方便，令T2＝5T1。車輛期望橫擺角加速度模型和期望橫擺角參考模型可用數(shù)學(xué)表達(dá)式表示如下：

設(shè)避障軌跡為Sb，同時(shí)Sbx和Sby分別為避障軌跡沿著車道方向和垂直車道方向上的投影長度，其中Sby即為自車和S格的橫向距離，滿足：

2.2 單障礙物避障軌跡規(guī)劃

2.2.1 直道路況

以避障車輛起始橫向避障控制時(shí)質(zhì)心的位置為坐標(biāo)原點(diǎn)，建立相對地面靜止的直角坐標(biāo)系。單障礙物下，根據(jù)S格定義，根據(jù)道路寬度可產(chǎn)生最多兩個(gè)可行S格，示意圖如圖4所示。

圖4 直道S格創(chuàng)建示意圖

圖4 中W2和Wzs分別表示障礙物和S格中點(diǎn)與橫坐標(biāo)距離值。根據(jù)期望橫擺角加速度模型：

式中：LS為避障過程中S格的運(yùn)動(dòng)距離；lC為自車縱向尺寸。當(dāng)避障車輛車頭與障礙物最近側(cè)縱向距離為L時(shí)開始跟蹤規(guī)劃的避障路徑，且L＞0。令（x（t），y（t））表示車輛避障軌跡質(zhì)心的坐標(biāo)，則避障軌跡為

其中 ta≤t≤tl

2.2.2 彎道路況

在彎道上利用極坐標(biāo)系進(jìn)行建模。彎道上所創(chuàng)建S格運(yùn)動(dòng)狀態(tài)以繞彎道圓心角速度表示，且等于障礙物繞彎道圓心角速度，同樣，W2和Wzs分別表示障礙物和S格中點(diǎn)相對避障車輛沿極徑距離值，示意圖如圖5所示。

圖5 彎道S格創(chuàng)建示意圖

圖5 中，R為極徑，λ為極角，θO為基于動(dòng)態(tài)直角坐標(biāo)系的橫擺角，即自車縱軸與動(dòng)態(tài)直角坐標(biāo)系橫軸的夾角。定義RC，RS和RZ分別為避障車輛、S格和障礙物的極徑。圖5中以靜態(tài)障礙物為例，vS／RS＝vZ／RZ＝0。SbR為避障軌跡Sb在極軸上的投影長度，Sbλ表示避障軌跡Sb的極角范圍。滿足關(guān)系式：

針對避障軌跡Sb的規(guī)劃如下：對于自車θO（t）同樣采用正反梯形約束的橫擺角加速度模型，軌跡規(guī)劃的時(shí)間段為［ta，tl］，與直道時(shí)區(qū)別在于式（4）中Sbλ由 SbR表示，滿足關(guān)系式：

同理，在直道避障過程中縱向距離滿足的關(guān)系式（6）在彎道極坐標(biāo)系中表示為

式中λS為［ta，tl］時(shí)間內(nèi)狀態(tài)格運(yùn)動(dòng)的極角范圍。令（RC（t），λC（t））表示自車在避障過程中質(zhì)心的實(shí)時(shí)極坐標(biāo)，RC（ta）表示起始階段自車的極徑大小，則避障軌跡為

2.3 多障礙物避障軌跡規(guī)劃

當(dāng)出現(xiàn)較多S格時(shí)，每相鄰兩S格之間軌跡規(guī)劃滿足上一節(jié)單障礙物避障軌跡規(guī)劃的策略。出現(xiàn)較多S格時(shí)涉及到較優(yōu)軌跡的求解，提出S格成本的概念，可理解為針對此規(guī)劃路徑進(jìn)行路徑跟蹤時(shí)的難易程度；同時(shí)參考Dijkstra算法進(jìn)行最優(yōu)路徑選擇。關(guān)于多障礙物軌跡規(guī)劃的流程圖如圖6所示。

圖6 流程圖

2.3.1 確定可行S格

根據(jù)S格的概念制定針對自車位置時(shí)所選S格是否可行的判斷規(guī)則如下。

（1）由圖7自車C與目標(biāo)S格位置創(chuàng)建圖中陰影區(qū)域，若該區(qū)域檢測到障礙物，則判斷此時(shí)S格不可行。

圖7 判斷規(guī)則

（2）已知C車與目標(biāo)S格的橫縱距離后，根據(jù)式（6）判斷是否滿足避障要求的最短縱向距離。

（3）當(dāng)C車位于某S格位置后，保持直行若無碰撞障礙物的風(fēng)險(xiǎn)，即稱此S格為終點(diǎn)S格，視為避障結(jié)束。

2.3.2 S格成本計(jì)算

對于多條可行S格路徑即涉及到最優(yōu)選擇問題，針對所創(chuàng)建S格的狀態(tài)信息構(gòu)造計(jì)算公式，表示S格路徑跟蹤的難易程度，即計(jì)算S格成本數(shù)值越低，該路徑越優(yōu)。根據(jù)式（2）～式（4）可知避障軌跡由障礙物狀態(tài)決定的因素為vs和W。取Wmas和vsmax為軌跡規(guī)劃中能接受的極值。令σw＝W／Wmax，σvs＝vs／vsmax，同時(shí)定義符號ξ為C車到某一S格的成本值，其計(jì)算公式為

式中 ξw和 ξvs為權(quán)重系數(shù)。當(dāng)

2.3.3 最優(yōu)路徑搜索算法

對于確定的可行S格路徑，以C車避障起始點(diǎn)作為根節(jié)點(diǎn)，同一障礙物衍生的狀態(tài)格屬于兄弟節(jié)點(diǎn)，終點(diǎn)S格稱為終節(jié)點(diǎn)。借鑒Dijkstra算法得出一條從根節(jié)點(diǎn)到終節(jié)點(diǎn)的最優(yōu)路徑，此算法中的權(quán)值即為S格成本，定義 q（C，S i）為C到 S i格的權(quán)值，q（C，S i，S j）＝q（C，S i）+q（S i，S j）。以搭建的搜索樹模型為基礎(chǔ)，以圖8場景為例，具體執(zhí)行步驟如表1所示（簡化說明，不包括障礙物Z4）。

表1 算法執(zhí)行步驟

最終輸出C車從起始位置到任一節(jié)點(diǎn)權(quán)值最小的路徑，再從C車到終節(jié)點(diǎn)的路徑中選出權(quán)值最小的路徑做為最終結(jié)果。

3 算法的仿真試驗(yàn)

為驗(yàn)證本文中基于衍生狀態(tài)格的智能車避障軌跡規(guī)劃算法在路徑規(guī)劃應(yīng)用中的有效性，通過模擬障礙物環(huán)境，運(yùn)用該算法完成路徑規(guī)劃。

3.1 直道避障仿真試驗(yàn)

為說明基于S格的避障軌跡規(guī)劃算法應(yīng)用的可行性，聯(lián)合 CarSim和 Matlab／Simulink進(jìn)行仿真試驗(yàn)。為驗(yàn)證算法對速度的魯棒性，仿真時(shí)無人駕駛汽車分別以中速20和高速30 m／s行駛。障礙物場景及所創(chuàng)建對應(yīng)S格如圖8所示，其中vz1＝10 m／s，vz2＝5 m／s，vz3＝7 m／s，vz4＝8 m／s。

圖8 創(chuàng)建S格

取 Wmax＝11 m，vsmax＝30 m／s，ξw＝0.3，ξvs＝0.2；通過2.3.3最優(yōu)路徑搜索算法得相應(yīng)可行S格路徑的權(quán)值計(jì)算結(jié)果，如表2所示。

表2 權(quán)值計(jì)算結(jié)果

得最優(yōu)軌跡為C-S1-S4。若汽車車速為20 m／s，解得 L1＝32 m，L2＝37 m，規(guī)劃的路徑如圖9所示。

若汽車以30 m／s的速度恒速行駛時(shí)，根據(jù)判斷規(guī)則得出唯一可行軌跡為C-S2-S5。根據(jù)策略解得L1＝48.6 m，L2＝62 m，規(guī)劃的路徑如圖10所示。

圖9 軌跡規(guī)劃圖

圖10 軌跡規(guī)劃圖

為進(jìn)一步研究汽車在跟隨路徑過程中的運(yùn)動(dòng)學(xué)特性，基于CarSim平臺(tái)搭建了環(huán)境模型，并將所規(guī)劃的路徑數(shù)據(jù)用CarSim的動(dòng)力學(xué)模型進(jìn)行仿真。圖11（a）為汽車在此車速下避障時(shí)的期望軌跡和跟蹤軌跡。圖11（b）和圖11（c）分別為汽車運(yùn)動(dòng)過程中的側(cè)向加速度和橫擺角速度的變化情況。由圖可見，在動(dòng)態(tài)避障過程中汽車側(cè)向加速度的峰值均低于 0.25 m／s2，橫擺角速度峰值均低于 0.15 rad／s。在此路況下，本算法所規(guī)劃出的軌跡能夠滿足汽車穩(wěn)定行駛。

3.2 彎道避障仿真試驗(yàn)

為體現(xiàn)本文避障策略在彎道路況時(shí)的有效性，進(jìn)行彎道避障仿真試驗(yàn)。仿真時(shí)所取彎道障礙物場景如圖 12所示。其中 vc＝5 m／s，vz＝vs＝0。

確定W1尺寸后創(chuàng)建對應(yīng)S格，如圖12所示。其中以S1格為目標(biāo)，SbR＝3.5 m。采用 MATLAB／Simulink進(jìn)行仿真，根據(jù)2.2彎道單障礙物避障策略，求得λ0＝55°，并得到規(guī)劃的避障軌跡，仿真結(jié)果如圖13所示。

圖13（a）為自車極徑RC在避障時(shí)的仿真曲線，圖13（b）為彎道車輛避障過程中期望軌跡，滿足行駛過程中曲率連續(xù)變化的要求。

3.3 算法的仿真對比試驗(yàn)

圖11 汽車動(dòng)力學(xué)特性

柵格法是利用尺寸相同的柵格對二維工作空間進(jìn)行劃分，柵格的大小以自車的尺寸為準(zhǔn)。針對動(dòng)態(tài)障礙物，有學(xué)者提出時(shí)間柵格法［19］。利用本文中建立的模擬場景（圖8），運(yùn)用柵格法完成避障軌跡規(guī)劃，并與S格法進(jìn)行對比分析。建立的二維時(shí)間柵格環(huán)境模型見圖14，利用Dijkstra算法對時(shí)間柵格進(jìn)行搜索。軟件平臺(tái)是 MATLAB R2014，得到的兩種算法的規(guī)劃路徑對比見圖15。

圖12 路況圖

圖13 彎道避障仿真結(jié)果

圖14 時(shí)間柵格環(huán)境模型

圖15 規(guī)劃路徑仿真對比

由圖15可見，從所規(guī)劃軌跡長度和曲率方面來看，利用本文算法所規(guī)劃的軌跡更優(yōu)。因?yàn)闁鸥穹ㄔ谝?guī)劃下一個(gè)目的地時(shí)，是在已經(jīng)建立的柵格體系中尋找路徑較短的無障礙格。而本文算法以障礙物為出發(fā)點(diǎn)建立的S格能很好地控制與障礙物間的距離。圖16反映了兩種策略分別在搜尋目標(biāo)柵格和創(chuàng)建S格的區(qū)別。

圖16 兩種策略的對比

圖16 中灰色陰影表示目標(biāo)柵格或者S格。其中（1）和（3）表示同一柵格體系下具有相同目標(biāo)柵格時(shí)障礙物的兩種可能位置，（2）和（4）分別表示與（1）和（3）障礙物位置相同時(shí)所創(chuàng)建S格的位置。由圖可見，本文算法能控制目標(biāo)S格和障礙物的距離，而柵格法則只能保證目標(biāo)柵格和障礙物之間的距離在一定的范圍內(nèi)，可能導(dǎo)致避障過程中橫向運(yùn)動(dòng)較長，軌跡曲率較大。從而進(jìn)一步說明圖15中的仿真結(jié)論。

4 硬件在環(huán)試驗(yàn)

對本文避障軌跡規(guī)劃仿真結(jié)果通過硬件在環(huán)仿真試驗(yàn)進(jìn)行驗(yàn)證，進(jìn)一步說明所提出的軌跡規(guī)劃策略的可行性。裝備框圖如圖17所示。

圖17 硬件在環(huán)裝備框圖

該試驗(yàn)臺(tái)主要由6部分組成，分別是上位機(jī)、下位機(jī)、接口系統(tǒng)、轉(zhuǎn)向器、控制器和用于模擬轉(zhuǎn)向阻力的伺服電機(jī)系統(tǒng)。通過上位機(jī)在CarSim中建立整車動(dòng)力學(xué)模型，在Labview中輸入規(guī)劃的避障軌跡，通過CarSim／LabVIEW RT聯(lián)合，編寫軌跡跟蹤控制程序，并把命令發(fā)送給下位機(jī)。下位機(jī)為NI的PXI系統(tǒng)，實(shí)時(shí)運(yùn)行上位機(jī)所建立的仿真程序。接口系統(tǒng)負(fù)責(zé)將傳感器采集到的前輪轉(zhuǎn)角信號輸入到PXI實(shí)時(shí)系統(tǒng)，同時(shí)將控制信號輸出給執(zhí)行機(jī)構(gòu)的控制器。

以3.1設(shè)定路況場景為例，車速為20和30 m／s時(shí)的避障軌跡如圖9和圖10所示，其硬件在環(huán)試驗(yàn)結(jié)果如圖18所示，起始加入30 m直行距離。

圖18 硬件在環(huán)試驗(yàn)結(jié)果

由圖18可見，實(shí)際運(yùn)行軌跡相比規(guī)劃的目標(biāo)軌跡存在誤差和時(shí)滯，這是因?yàn)檎鎸?shí)轉(zhuǎn)向系統(tǒng)存在載荷且電動(dòng)機(jī)響應(yīng)相對滯后。對試驗(yàn)所得側(cè)向加速度、橫擺加速度和質(zhì)心側(cè)偏角的峰值進(jìn)行審核，確定未超過閾值，符合仿真時(shí)變化規(guī)律，從而保證所規(guī)劃軌跡能滿足車身穩(wěn)定性。

5 結(jié)論

（1）提出衍生狀態(tài)格的概念，將特定環(huán)境下抽象的尋求一條合適的避障軌跡問題轉(zhuǎn)化為具體的根據(jù)設(shè)定規(guī)則的有限解擇優(yōu)問題，在轉(zhuǎn)化過程中全面考慮自車與障礙物的尺寸和運(yùn)動(dòng)狀態(tài)，保證避障軌跡的有效性。

（2）采用正反梯形約束的橫擺角加速度模型求出針對S格的換道軌跡，提出S格成本的概念，利用Dijkstra算法對多個(gè)可行S格軌跡進(jìn)行擇優(yōu)。該策略全面考慮單障礙物和多障礙物場景的避障條件，并且在逐漸復(fù)雜的環(huán)境中避障策略循序漸進(jìn)。

（3）將本文算法用于避障路徑規(guī)劃中，在特定路況場景兩種車速下分別進(jìn)行了仿真和硬件在環(huán)試驗(yàn)。結(jié)果表明：汽車按照規(guī)劃出的路徑行駛能夠?qū)崿F(xiàn)安全避障，并且在運(yùn)動(dòng)過程中其側(cè)向加速度和橫擺角速度值均符合穩(wěn)定性要求，驗(yàn)證了算法的正確性和有效性。與柵格法進(jìn)行對比，柵格法需要對整個(gè)路況環(huán)境搭建柵格再根據(jù)障礙物位置進(jìn)行篩選，而本文算法直接根據(jù)障礙物狀態(tài)創(chuàng)建S格，通過仿真試驗(yàn)證明本文算法所規(guī)劃的軌跡更短，曲率更小。

（4）本文中主要通過障礙物的狀態(tài)創(chuàng)建S格，沒有考慮車道線、指示牌等標(biāo)識(shí)，也沒有考慮各類交通規(guī)則，在后期的智能車避障研究中可考慮車道上的各個(gè)因素從而創(chuàng)建不僅有針對性還有全面性的S格。