動態障礙物狀態空間重構的無人車路徑規劃

雷芳華，袁小芳

（1.湖南大學電氣與信息工程學院，湖南 長沙 410082；2.郴州職業技術學院現代裝備制造學院，湖南 郴州 423000）

1 引言

局部路徑規劃作為無人駕駛汽車研究的關鍵技術之一，其根據車載傳感器采集的障礙物信息和全局期望路徑信息重新規劃出避開障礙物的局部最優路徑，側重于依賴車載傳感器（激光雷達，超聲波等）的采集信息，具有很強的實時性。目前局部路徑規劃方法歸納為兩類：傳統局部路徑規劃方法和智能局部路徑規劃方法。傳統局部路徑規劃方法主要有人工勢場法、柵格法和向量場直方圖法等，對于障礙物信息已知的情況下處理避障問題尚可，但是遇到障礙物信息未知或動態障礙物，則不能很好處理［1］。針對動態環境規劃效果不佳問題，以A*算法為代表的基于圖搜索的路徑規劃算法，分別提出了改進A*［2］、MH A*［3］、DMH A*［4］算法等，以快速擴展樹（RRT）為代表的基于采樣的路徑規劃算法，文獻中分別提出了EG-RRT［5］、RRT*［6］、CL-RRT*［7］算法等，提高了路徑規劃的質量。智能局部路徑規劃方法主要有模糊控制法、神經網絡法、混合智能法、協同控制法等，對于障礙物信息未知或部分未知的情況下處理避障問題需要的硬件設備要求苛刻，且每種規劃算法都有其優點及局限性，難以采用單一的算法實現動態環境精準、安全可靠的路徑規劃［8］。針對動態環境精準和安全可靠規劃問題，文獻中分別提出了改進A*與蟻群結合算法［9］、改進RRT與APF結合算法［10］等，提高了路徑規劃的精準和可靠。這些方法在局部路徑規劃領域中發揮了重要作用。

為了路徑規劃實際可行，一般需要結合車身尺寸對障礙物進行膨脹處理，障礙物尺寸較大，易出現車輛從障礙物中間穿越的現象，然而通過加大避障功能函數的權重系數可以解決，但是權重系數增大會使規劃結果出現保守現象［11］。文獻［12］曾將3D虛擬危險勢場構造為道路的三角函數與障礙物的指數函數相疊加，當車輛可能發生與障礙物的碰撞時，生成避免碰撞的理想軌跡。文獻［13］提出了障礙物衍生狀態格的概念，規劃的軌跡更短，曲率更小，能實現安全避障。這里針對動態障礙物，以車載傳感器采集的障礙物狀態信息為出發點，重構建立帶安全區域的膨脹構型，將動態障礙物狀態空間轉換為瞬態靜態障礙物狀態空間，以規劃路徑與全局參考路徑的最小化偏差為目標，對于障礙物避障將以懲罰函數的方式實現，并將控制增量進行約束作為目標函數的狀態量。最后，在特定動態行駛工況中，采用CarSim與Simulink聯合仿真，對該避障路徑規劃方法的可行性和穩定性進行了驗證。

2 障礙物狀態空間重構的概念

在避障過程中的障礙物是由靜態障礙物（靜態車、樹木、建筑物等）和動態障礙物（動態車、行人、動物等）組成。然而，在車載傳感器采集的障礙物信息中，由于遮擋等原因，采集的障礙物信息并不能完全構造障礙物的外形尺寸，本論文假設車載傳感器感知的部分障礙物信息是已知，能采集的信息主要包括各障礙物的左邊緣輪廓、最近點、右邊緣輪廓、最近點相對距離、相對速度、障礙物寬度等信息。

2.1 障礙物信息采集及求基準點

無人駕駛車在靜態過程中對障礙物信息進行采集，主要包括兩種采集情況，如圖1所示。（1）車載傳感器檢測到①④⑤（圖左邊）；（2）車載傳感器檢測到①②③（圖右邊），即為左邊緣點、最近點和右邊緣點三點共線。

圖1 無人駕駛車靜態采集障礙物信息示意圖Fig.1 Self-Driving Car Static Collection of Obstacle Information Diagram

無人駕駛車在靜態過程中對障礙物信息進行處理，確定重構的基準點，可用數學表達式表示如下：

式中：在無人駕駛車坐標系下（如圖1左邊圖），（x1，y1）—障礙物最近點①坐標；（x2，y2）、（x3，y3）—障礙物檢測寬度的左邊緣點②、右邊緣點③坐標；（xg，yg）—②③點的中心點，即為重構的基準點；ζ—防止分母為0的較小正數。

2.2 障礙物信息點坐標系轉換

當然，無人駕駛車在復雜障礙物環境中要實現避障功能，有必要在駕駛（動態）過程中對障礙物信息進行采集，還要將在無人駕駛車坐標系下的障礙物信息點轉換到全局坐標系，才能進行障礙物的避障路徑規劃，如圖2所示。

圖2 無人駕駛車動態采集障礙物信息示意圖Fig.2 Self-Driving Car Dynamic Collection of Obstacle Information

圖2中，由于受車載傳感器檢測信息的遮擋等因素制約，檢測到的障礙物狀態空間具有不確定性，例如以圖2中②③為障礙物的檢測寬度，其障礙物車的實際構型可以是①④⑤、①⑥⑦等。為障礙物狀態空間重構提供了可變的基礎。

根據圖2示意，將如下動態的坐標系轉換數學表達式代入式（1）中，可求得在全局坐標系下的基準點（xobs，g，yobs，g）：

式中：Brobs，j—第j個障礙物信息點在全局坐標系下的夾角集；（xobs，j，yobs，j）—第j個障礙物信息點在全局坐標系下的坐標集；BrXj—第j個障礙物信息點在無人駕駛車坐標系的夾角集，如BrCj、BrLj、BrRj等；（xj，yj）—第j個障礙物點在無人駕駛車坐標系的坐標集，如（x1，y1）、（x2，y2）、（x3，y3）等；在全局坐標系下，Yɑw—無人駕駛車的航向角；（X0，Y0）—無人駕駛車的坐標。此公式的關系式將實現靜態狀態空間重構轉為動態狀態空間重構。

2.3 障礙物信息點重構

在全局坐標系下，提出以式（2）求得的基準點為中心，以檢測輪廓寬度為尺寸，將重構障礙物信息點總數設置為M，采用以下構型函數對障礙物信息點數據進行重構：

其中雙八構型數學表達式如下：

其中圓形構型數學表達式如下：

式中：在全局坐標系下，（xm，ym）—重構后障礙物構型的第m個信息點的坐標；（xobs，g，yobs，g）—全局坐標系下障礙物基準點的坐標；Wid—在最近點處檢測到障礙物的寬度；m—設置的第m個點；pi—圓周率π。

3 基于障礙物狀態空間重構的避障路徑規劃

3.1 數學模型

這里模型預測控制是建立在假設無人駕駛車點質量模型的基礎上，由路徑重規劃模塊和跟蹤控制模塊組成，其中路徑重規劃模塊中引入了懲罰函數，并將控制增量進行約束作為目標函數的狀態量。在路徑重規劃層的模型預測控制器中不再進行線性化，而是直接運用非線性化模型求解，具備高求解精度，結合非線性二次規劃算法處理動態障礙物條件下的最優路徑規劃問題。為實現規劃層與控制層的順利對接，采用最小二乘法多項式曲線擬合的方式對離散點線性化。具體控制系統的數據傳遞，如圖3所示。

圖3 帶避障功能的模型預測控制系統Fig.3 Model Predictive Control System with Obstacle Avoidance Function

圖3中，帶避障功能的模型預測控制系統主要由路徑重規劃模塊接收來自重構過的障礙物信息和全局的參考路徑信息，再由跟蹤控制模塊接收規劃模塊輸出的局部路徑規劃，輸出前輪偏角為控制量。

懲罰函數的基本思路是通過重構后的障礙物狀態坐標點與局部路徑參考點的距離偏差來調整懲罰函數值的大小，且兩點距離越近，其值越大，其避障功能數學表達式如下［11］：

式中：Jobs，i，m—采樣時刻i的第m個障礙物信息點的避障函數；Sobs—權重系數；（xm，ym）—第m個障礙物點在全局坐標系下的坐標；（X0，Y0）—無人駕駛車的坐標；ζ—防止分母為0的較小正數。

然而，路徑重規劃模塊的目標函數則是最小化路徑規劃與全局參考路徑的偏差，且實現避障功能。具體目標函數數學表達式如下［11］：

式中：η（t+i|t）—時刻t的狀態變量；Ut—控制量前輪偏角的物理約束條件；Ui—控制增量作為約束的狀態量；Q—反映控制器對全局參考路徑跟隨能力的權重矩陣；R—對控制量平穩變化的權重矩陣；Np—預測時域；M—重構障礙物信息點總數。

當局部路徑規劃不理想時，盡管可以加大權重系數Sobs來調整，但是也會加大跟蹤偏差等問題。因此，采用動態障礙物狀況空間重構對于避障功能的實現顯得尤為重要。

3.2 避障路徑重規劃算法流程

以帶避障功能的模型預測控制系統為主框架，在路徑重規劃層，提出無人駕駛車避障路徑重規劃算法流程圖，如圖4 所示。協同環境建模和二次規劃問題優化的避障路徑重規劃算法主要按照以下三步實現。（1）采集實時障礙物信息，計算重構基準點，再結合無人駕駛車的初始狀態進行坐標系轉換，然后根據障礙物固有構型信息進行重構，從而構建障礙物環境地圖；（2）利用障礙物環境地圖、全局參考路徑以及無人駕駛車初始狀態，以目標函數和運動學約束為運算基礎，然后根據是否達到預測步長和目標函數值最小進行判斷，從而實現二次規劃問題滾動優化；（3）得到目標函數最優值的無人駕駛車離散狀態量，采用最小二乘法曲線擬合法進行線性化，最終得到期望路徑，以供跟蹤控制模塊。

圖4 無人駕駛車避障路徑重規劃算法流程圖Fig.4 Flow Chart of Replanning Algorithm for Obstacle Avoidance Path of Self-Driving Car

3.3 構型及工況設置

模型預測控制法的路徑規劃問題考慮到忽略了車身尺寸帶來的安全問題，需要對障礙物進行膨脹處理，為安全起見，原則上需要加設安全區域，構型必須使車輛與任何障礙物保持安全距離，設置安全區域過小，會帶來碰撞或刮擦的風險，設置安全區域過大，避障功能太過于保守，延長了規劃路徑。然而，考慮到圖2中的障礙物形狀的不確定性，提出了重構障礙物狀態空間的方法，如圖5所示。將以②③障礙物檢測寬度為輪廓寬度，④點為重構中心，重構成“圓形”和“雙八”構型，其中未進行重構的障礙物形狀基于正常車輛的外形尺寸，假設定為“長方形”構型。

圖5 障礙物重構構型及工況設置Fig.5 Obstacle Reconstruction Configuration and Working Condition Setting

為檢測構型的可行性和穩定性，特設置的“超車”工況，靜態障礙物停在60m處，動態障礙物停在120m處，當無人駕駛車到達70m時，動態障礙物以5m∕s的速度向前運動，如圖5所示。以“雙八”構型為例，當動態檢測到“雙八”構型障礙物時，其懲罰函數值變化情況，如圖6所示。

圖6 檢測到“雙八”構型障礙物的三維函數圖Fig.6 Three-Dimensional Function Diagram of Obstacles with "Double Eight" Configuration Detected

4 仿真研究

4.1 不同構型對避障的影響

4.1.1 障礙物重構的位置變化

為驗證基于障礙物狀態空間重構避障路徑規劃的可行性，采用Carsim和Simulink的聯合仿真進行研究，其仿真框架主要是參照的模型預測控制系統進行構建，根據仿真研究測試經驗將預測步長設置為28，控制步長設置為2，如圖3所示。左邊緣點重構位置與右邊緣點重構位置的距離等于障礙物重構寬度，設置實際障礙物的寬度為2m，分析無人駕駛車行駛中障礙物重構位置和重構寬度的變化情況。從表1可以分析出，無人駕駛車在動態過程中，其重構位置和重構輪廓寬度都在變化，當無人車軌跡到達（23.8，0.2）時，重構位置的變化主要是無人車運動接近障礙物，傳感器檢測到障礙物放大過程的誤差，誤差值保持在0.1左右，誤差可以忽略不計，并且重構寬度一直未變，無人車從行駛方向左邊超車，當無人車軌跡到達（35.5，0.9）時，重構位置、重構寬度的變化主要是檢測到的障礙物輪廓發生了變化，由檢測到原來的障礙物前輪廓變成了檢測到障礙物前輪廓和左邊輪廓的變化，當無人車軌跡到達（58.7，2.0）時，其障礙物重構寬度達到最大化3.2m，其后由于逐漸收緊右邊緣的檢測范圍，導致瞬間變小。由于以上原因導致的重構位置和重構寬度變化，對于避障來說，負面影響并不大，甚至有助于避障功能，障礙物重構寬度越大，安全區域設置越大，促使無人駕駛車接近障礙物時由于懲罰函數值變大而遠離障礙物，起到了微調作用。

表1 重構位置變化Tab.1 Reconstruction Position Change

4.1.2 不同構型對避障的影響

為驗證不同構型的避障效果，采用圖5的“超車”工況，進行靜動態避障仿真研究。根據文獻對權重系數的設置情況［11］，分別以權重系數100和1000兩種情況進行避障對比，分析避障效果和“穿越”現象兩項指標。不同構型以“圓形”、“雙八”構型和不重構為對比主因素，經過仿真對比研究，可得結果，如圖7所示。分析圖7（a）權重系數100的避障情況，其中圓構型和未重構都發生了“穿越”現象，即使圓構型成功避障靜態障礙物，但是其進入最佳路徑較慢，帶來避障風險，曲率也比“雙八”構型避障略差一點。再分析圖7（b）權重系數1000的避障情況，提高權重系數的確改善了避障效果，但是也存在問題，如圓形構型車輛避障軌跡過于接近障礙物，將帶來碰撞或刮擦的風險，未重構型還發生了“穿越”現象。綜合分析可得，經過重構后的“雙八”構型避障效果更優。

圖7 不同構型避障軌跡對比圖Fig.7 Comparison Diagram of Obstacle Avoidance Trajectories of Different Configurations

4.2 動態避障的穩定性

為驗證基于障礙物狀態空間重構避障路徑規劃應用的穩定性，聯合Carsim和Simulink進行仿真研究。以“雙八”構型為重構構型，仿真時無人駕駛車分別以低速10m∕s、中速20m∕s 和高速30m∕s行駛，行駛工況采用圖5的“超車”工況，仿真的無人駕駛車運動學特性結果，如圖8所示。無人駕駛車三種行駛速度避障過程中的橫擺角速度和側向加速度的變化情況，如圖8（a）、圖8（b）所示。無人駕駛車三種行駛速度避障過程中的路徑軌跡，如圖8（c）所示。由圖可得，在三種行駛速度動態避障過程中無人駕駛車橫擺角速度峰值都能低于0.25rad∕s，側向加速度峰值都能低于4.41m∕s2（即0.45g）從三種速度行駛在該工況下，本方法規劃出的軌跡能夠滿足汽車穩定行駛。

圖8 無人駕駛車的運動學特性Fig.8 Kinematics of Driverless Cars

4.3 與傳統人工勢場法仿真對比研究

傳統人工勢場法路徑規劃的基本思想是將無人駕駛車的行駛運動，設計成一種抽象的人造引力場的運動，目標路徑對車產生“引力”，障礙物對車產生“斥力”，以通過求合力的方式來控制無人駕駛車的運動，應用勢場法規劃出來的路徑一般是比較平滑并且安全。

為驗證提出的基于動態障礙物狀態空間重構的避障路徑規劃的可靠性，將與傳統人工勢場法進行規劃路徑的仿真對比研究，根據仿真研究測試經驗將傳統人工勢場法的系統參數設置為較優：引力的增益系數k=15，斥力增益系數m=5，障礙影響距離P=30，障礙點個數n=10（未膨脹處理，靜態障礙物4個，動態障礙物6個），步長l=0.5，循環迭代次數J=800。比較結果，如圖9所示。從避障軌跡對比可得重構障礙物信息的模型預測控制法相比于傳統人工勢場法，在控制避障方面變道更早，曲率更優，有利于消除動態障礙物緊急制動帶來的碰撞風險，在橫向軌跡峰值方面偏離更小，規劃軌跡更短，有利于避障控制的靈活性和可靠性。

圖9 基于障礙物狀態空間重構的MPC法與傳統人工勢場法的仿真對比Fig.9 Simulation Comparison Between MPC Method Based on Obstacle State Space Reconstruction and Traditional Artificial Potential Field Method

5 結論

（1）提出動態障礙物狀態空間重構的概念，將采集的動態障礙物狀態空間信息轉化為可實際操作的障礙物重構狀態空間信息，為避障路徑規劃提供可靠障礙物信息，構建障礙物環境地圖，保證了避障規劃的可行性。

（2）采用帶避障功能的模型預測控制系統，在路徑重規劃模塊中引入了懲罰函數，通過帶安全區域的障礙物重構信息，以改良權重系數對避障的影響，設置能實現靜態障礙物和動態障礙物避障的行駛“超車”工況，對重構位置信息變化進行了分析，驗證了該重構方法的可靠性。

（3）將重構法應用于動態避障路徑規劃仿真研究中，在特定行駛“超車”工況場景下以三種車速分別進行了實現安全避障的仿真研究。結果表明：根據權重系數100和1000情況下分別對不同構型進行避障效果和“穿越”現象兩項指標的分析，最終認為“雙八”構型避障效果更優，且比未重構的模型預測控制法避障性能強；以三種行駛速度為因素，分析橫擺角速度、側向加速度和橫向軌跡的運動學特性，其峰值和軌跡都能夠滿足汽車的穩定行駛；與傳統人工勢場法進行對比，從避障軌跡對比可得重構法相比于傳統人工勢場法，在控制避障方面變道更早，曲率更優，有利于消除動態障礙物緊急制動帶來的碰撞風險，在橫向軌跡峰值方面偏離更小，規劃軌跡更短，有利于避障控制的靈活性和可靠性。

（4）只針對障礙物的狀態空間進行了重構，實現了避障路徑規劃，沒有考慮無人駕駛車自身的尺寸因素，也沒有考慮行駛多目標環境等因素，在后期的無人駕駛車避障研究中將考慮無人駕駛車自身尺寸和行駛環境等約束條件。