基于障礙物衍生狀態格的智能車避障軌跡規劃*

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

（1.合肥工業大學機械工程學院，合肥 230009； 2.合肥工業大學汽車與交通工程學院，合肥 230009）

前言

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

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

1 衍生狀態格的概念

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

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

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

圖2 W1數值的確定

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

Wzs＝（Wz+Ws）／2+W1

S格創建完成后，因為自車與S格位置重合時可保證避障車輛與障礙物間碰撞的可能性為零，故避障軌跡便是在自車和S格之間規劃。

2 基于S格的避障軌跡規劃算法

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

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

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

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

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

2.2 單障礙物避障軌跡規劃

2.2.1 直道路況

以避障車輛起始橫向避障控制時質心的位置為坐標原點，建立相對地面靜止的直角坐標系。單障礙物下，根據S格定義，根據道路寬度可產生最多兩個可行S格，示意圖如圖4所示。

圖4 直道S格創建示意圖

圖4 中W2和Wzs分別表示障礙物和S格中點與橫坐標距離值。根據期望橫擺角加速度模型：

式中：LS為避障過程中S格的運動距離；lC為自車縱向尺寸。當避障車輛車頭與障礙物最近側縱向距離為L時開始跟蹤規劃的避障路徑，且L＞0。令（x（t），y（t））表示車輛避障軌跡質心的坐標，則避障軌跡為

其中 ta≤t≤tl

2.2.2 彎道路況

在彎道上利用極坐標系進行建模。彎道上所創建S格運動狀態以繞彎道圓心角速度表示，且等于障礙物繞彎道圓心角速度，同樣，W2和Wzs分別表示障礙物和S格中點相對避障車輛沿極徑距離值，示意圖如圖5所示。

圖5 彎道S格創建示意圖

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

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

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

式中λS為［ta，tl］時間內狀態格運動的極角范圍。令（RC（t），λC（t））表示自車在避障過程中質心的實時極坐標，RC（ta）表示起始階段自車的極徑大小，則避障軌跡為

2.3 多障礙物避障軌跡規劃

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

圖6 流程圖

2.3.1 確定可行S格

根據S格的概念制定針對自車位置時所選S格是否可行的判斷規則如下。

（1）由圖7自車C與目標S格位置創建圖中陰影區域，若該區域檢測到障礙物，則判斷此時S格不可行。

圖7 判斷規則

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

（3）當C車位于某S格位置后，保持直行若無碰撞障礙物的風險，即稱此S格為終點S格，視為避障結束。

2.3.2 S格成本計算

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

式中 ξw和 ξvs為權重系數。當

2.3.3 最優路徑搜索算法

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

表1 算法執行步驟

最終輸出C車從起始位置到任一節點權值最小的路徑，再從C車到終節點的路徑中選出權值最小的路徑做為最終結果。

3 算法的仿真試驗

為驗證本文中基于衍生狀態格的智能車避障軌跡規劃算法在路徑規劃應用中的有效性，通過模擬障礙物環境，運用該算法完成路徑規劃。

3.1 直道避障仿真試驗

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

圖8 創建S格

取 Wmax＝11 m，vsmax＝30 m／s，ξw＝0.3，ξvs＝0.2；通過2.3.3最優路徑搜索算法得相應可行S格路徑的權值計算結果，如表2所示。

表2 權值計算結果

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

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

圖9 軌跡規劃圖

圖10 軌跡規劃圖

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

3.2 彎道避障仿真試驗

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

確定W1尺寸后創建對應S格，如圖12所示。其中以S1格為目標，SbR＝3.5 m。采用 MATLAB／Simulink進行仿真，根據2.2彎道單障礙物避障策略，求得λ0＝55°，并得到規劃的避障軌跡，仿真結果如圖13所示。

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

3.3 算法的仿真對比試驗

圖11 汽車動力學特性

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

圖12 路況圖

圖13 彎道避障仿真結果

圖14 時間柵格環境模型

圖15 規劃路徑仿真對比

由圖15可見，從所規劃軌跡長度和曲率方面來看，利用本文算法所規劃的軌跡更優。因為柵格法在規劃下一個目的地時，是在已經建立的柵格體系中尋找路徑較短的無障礙格。而本文算法以障礙物為出發點建立的S格能很好地控制與障礙物間的距離。圖16反映了兩種策略分別在搜尋目標柵格和創建S格的區別。

圖16 兩種策略的對比

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

4 硬件在環試驗

對本文避障軌跡規劃仿真結果通過硬件在環仿真試驗進行驗證，進一步說明所提出的軌跡規劃策略的可行性。裝備框圖如圖17所示。

圖17 硬件在環裝備框圖

該試驗臺主要由6部分組成，分別是上位機、下位機、接口系統、轉向器、控制器和用于模擬轉向阻力的伺服電機系統。通過上位機在CarSim中建立整車動力學模型，在Labview中輸入規劃的避障軌跡，通過CarSim／LabVIEW RT聯合，編寫軌跡跟蹤控制程序，并把命令發送給下位機。下位機為NI的PXI系統，實時運行上位機所建立的仿真程序。接口系統負責將傳感器采集到的前輪轉角信號輸入到PXI實時系統，同時將控制信號輸出給執行機構的控制器。

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

圖18 硬件在環試驗結果

由圖18可見，實際運行軌跡相比規劃的目標軌跡存在誤差和時滯，這是因為真實轉向系統存在載荷且電動機響應相對滯后。對試驗所得側向加速度、橫擺加速度和質心側偏角的峰值進行審核，確定未超過閾值，符合仿真時變化規律，從而保證所規劃軌跡能滿足車身穩定性。

5 結論

（1）提出衍生狀態格的概念，將特定環境下抽象的尋求一條合適的避障軌跡問題轉化為具體的根據設定規則的有限解擇優問題，在轉化過程中全面考慮自車與障礙物的尺寸和運動狀態，保證避障軌跡的有效性。

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

（3）將本文算法用于避障路徑規劃中，在特定路況場景兩種車速下分別進行了仿真和硬件在環試驗。結果表明：汽車按照規劃出的路徑行駛能夠實現安全避障，并且在運動過程中其側向加速度和橫擺角速度值均符合穩定性要求，驗證了算法的正確性和有效性。與柵格法進行對比，柵格法需要對整個路況環境搭建柵格再根據障礙物位置進行篩選，而本文算法直接根據障礙物狀態創建S格，通過仿真試驗證明本文算法所規劃的軌跡更短，曲率更小。

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