基于激光雷達檢測的無人駕駛車輛路徑規劃算法仿真研究

摘要：針對無人駕駛車輛在自主行駛過程中存在的避障失效、避障規劃軌跡不光滑、避障算法實時性差等問題，提出一種基于Lidar的車輛動態避障路徑規劃方法，采用A*算法進行全局規劃，在已知環境圖上進行尋優，并對新出現的障礙物采用DWA算法進行避障，同時采用全局局部優化的雙重避障算法實現無人車輛的安全抵達。

關鍵詞：多線激光雷達；無人駕駛車；局部軌跡規劃

中圖分類號：U467 收稿日期：2024-07-15

DOI：10.19999/j.cnki.1004-0226.2024.09.016

1 傳感器選取

激光雷達可以獲得高精度的數字高程，并產生高精度的3D定位信息，從而可以更好地識別出被測量對象的具體位置和尺寸［1］。激光雷達發射的高頻率激光可以在1 s內對被測目標進行100個量級的激光掃描，得到目標的位置信息，即激光點云，再由算法對目標進行3D模型化，在獲取目標的位置信息的同時，還可以提取出目標的大概輪廓。

Velodyne公司出品的“冰球”16列雙返型激光雷達具有世界上最好的精確度和標定能力，并且具有優良的能效和溫度感應范圍，具有消除輻射和干擾的功能，降低了噪音。Velodyne“冰球”16列激光雷達的技術參數在表1中有詳細說明。

2 控制算法

2.1 基于A*算法的全局路徑規劃

A*法是當前廣泛使用的一種全局規劃方法，最早在1968年公布，作為迪杰斯特拉算法的推廣［2］。Dijkstra算法具有全局搜索能力，但它需要將從起點到終點的全部結點都遍歷一遍，效率低下且存儲空間較大［3］。為了解決上述問題，提出了一種基于A*的啟發式方法：

[fn=gn+hn] （1）

式中，[fn]為用來選取下一個遍歷節點的總成本函數；[gn]為從起始點到結點n的最短路值；[hn]為從目標點到結點n的最短路徑的激勵值。

2.2 基于改進DWA算法的局部路徑規劃

2.2.1 優b39d772d194a05699411920f68af6f6e27cefa1adfa42967f2a1a59bc541c150化DWA預軌跡選取空間

本文擬采用離散小波變換（DWA）的思想，利用離散小波變換的思想，將避障問題轉化為最優解，從而實現基于離散小波分解的無人車輛路徑規劃。然而，當遇到障礙物較多時，基于DWA的離散小波變換，其目標函數是下一步的最優路徑，從而導致規劃路徑并非最短，且規劃過程不光滑。

微分流形是一種具有微分結構的拓撲流形，它是一種拓撲與幾何的重要研究內容［4］。本文擬采用微分流形切空間的方法，在流形切空間中建立通用的無人飛行器軌跡模型，并在此基礎上，對已有障礙物的無人車輛進行去除。無人飛行器的軌跡方程式可歸納為：

[B=T×N] （4）

[N=B×T] （5）

式中，[Snt]為t時間n下無人車輛的運動軌跡；T為無人機航跡的切線矢量；B為無人機的輔助法矢量；N為無人機航跡的主方向矢量。

將無人飛行器的軌跡抽象為一剛性運動，其中，將無人飛行器的移動狀態設為｛T，B，N}，如圖1所示，S為無人飛行器的運行軌跡，p為S軌道上的姿態點，在軌跡的各個姿態點p上所對應的切空間如下：

切空間的本質就是曲面M在點p處的無窮小領域進行無窮大的線性擴張所產生的空間。圖1中[ψ1，ψ2，…，ψi]分別對應著流形切空間中的不同切向量，即表示的是無人車運動朝向。從[ψ1，ψ2，…，ψi]中選取機器人下一時刻的軌跡，去除掉與障礙物有交集區域的軌跡從而優化了允許速度空間，一定程度上減少了軌跡評價數目，使機器人能夠準確躲避障礙物運動。

圖2是傳統DWA仿真的軌跡與流行空間切矢量仿真的軌跡對照圖，可以看出，通過選擇下一時刻的預航跡，在密集障礙區，通過切矢量選擇下一時刻的預航跡，可以使下一時刻的軌跡更平滑地銜接，從而減少規劃時間，確保無人車輛的穩定行駛，提升運營效率。

2.2.2 改進評價函數

傳統DWA算法中，評估函數評估指標相互獨立，缺乏對運動軌跡選擇的綜合考量，且不同環境下的權值比例差別較大，使得機器人在復雜環境下極易陷入局部極值［5-6］。針對傳統DWA算法在障礙物較多的情況下，選擇的路徑并非最優的問題［7］，提出了一種新的基于小波分解的多目標優化方法，公示如下：

式中，[xs，ys]為機器人起始點坐標；[xt，yt]為機器人目標點坐標；M為模擬地圖的面積；[xi，yi]為機器人在i時刻的位置坐標；數字[v，w]為障礙數目評價子函數。這個函數的設定是，隨著機器人周圍的障礙物的數目越來越多，這個函數就會變得越來越小，[δ]的值會越大，路徑的危險性越小。此時評價函數為：

[Gv，w=α×headingv，w+β×distv，w+]

[γ×velocityv，w+δ×numberv，w] （9）

DWA算法利用無人車輛與目標點之間的夾角很小，使得無人車輛能夠更快地朝著目的地移動，而當接近目的地時，評價函數（heading，w）會發生顯著改變，從而使得車輛在接近目的地時失去穩定性。因此，當無人飛行器接近目標點時，其方位估計值應該被降低。具體公式如下：

式中，heading為在跟蹤結束時，無人機離目標點的位置偏移角度q；路徑為全局路徑規劃的長度；Edist為從目標點到目標點的距離。在無人機的初始運行階段，[ε]較大，heading評估作用較大；無人車靠近目標點時，[ε]較小，在這個時候，Edist的權重系數會增加，而標題的評價作用會越來越小。在此基礎上，提出了一種基于視覺信息的無人駕駛車輛跟蹤方法。最后的評估功能得到了改善：

[Gv，w=α×iheadingv，w+β×distv，w+]

[γ×velocityv，w+δ×numberv，w] （12）

2.2.3 效果對比試驗

搭建無人駕駛車輛在復雜障礙環境下的自主規劃能力，并對其運動軌跡、方向、起點到目的地所需的時間等參數進行實驗驗證。在同一場景下，比較不同算法在不同方案下的運行效率，并對其結果進行分析。

設定初始狀態時，無人飛行器的位置坐標是（0，0），移動狀態是靜態，方向角是0，無人機的最大線性速度設定為8 m/s，最小線性速度是0 m/s，最大角速度是30 °/s2，最小線加速度是-30 °/s2，最大線加速度是3 m/s2，最大角加速度是30 °/s2，無人飛行器的擴張半徑確定在障礙物擴張半徑之內，DWA算法的主要參數設定如表2所示。

為了進一步提高DWA算法在密集區障礙環境中的預航跡選擇空間，本文擬通過不同尺寸的障礙圖和同樣尺寸的狹窄可行區域圖兩種模擬場景，并在環境1下，進行算法優化前后路徑規劃效果的比較。在圖3中，點坐標（0，0）是機器人的移動起點，點坐標（20，20）是目標坐標，在試驗中，設定了不同尺寸的障礙物（紫環的尺寸），藍線是無人駕駛車輛的最后一條路線。

如圖3a所示，在遇到稠密障礙區域時，原有DWA算法會選擇繞過障礙區域行進，而增加障礙數的DWA則減少了其危險性；在狹窄區域，不繞過障礙物，直接穿越障礙區抵達目標點，從而減少了路徑規劃時間。

2.3 雙層避障算法的流程設計

圖4給出了用A*算法和改進DWA算法進行無人駕駛車輛的軌跡規劃流程。首先，地圖服務器會將一張靜態地圖發送到A*算法中，同時輸入無人車的起點，在全局地圖上，A*算法根據地圖搜索出整個區域的路線覆蓋目標點，再在全局路線上設定節點作為本地路線規劃的目的地，當無人駕駛車輛行駛時，若在無障礙物的情況下，就會逐漸靠近本地目標，若發現有障礙物，就會用改進的DWA方法，將其轉移到下一個地方，這樣不斷重復，直至抵達目的地為止。

3 仿真與分析

為了檢驗A*以及改進的DWA雙層障礙規避算法的有效性，采用turtle 3、waffle_pi等無人車輛模型，在 ROS環境中進行試驗，在此基礎上，給出了模型的有關參數，如表3所示。

在ROS中，建立了三個模擬環境，如圖5所示。在圖5a中設定了一個“C”形的障礙環境，通過實驗驗證了融合A*和改進DWA可以解決DWA易陷入局部極小的缺點；圖5b中環境2設定為靜止、稠密的障礙環境，該圖中的障礙以正方形為主；圖5c是在環境3中設定的一種動態稠密的障礙環境，其中以柱狀障礙為主，將柱狀障礙與附圖中豎向壁的間距設定成比無人駕駛車輛加上擴張半徑的間距大，到橫向墻壁的間距設定成比無人駕駛車輛加上擴張半徑的間距小。圖中所示的白弧的障礙物，為規劃完成后才加到環境圖上的，其距墻壁的距離都比無人駕駛車輛加擴張半徑的距離要大。

在無人駕駛車輛的路徑規劃中，首先要根據全局圖進行全局尋優，因此先要構建一個全局圖。在ROS中，常見的SLAM定位建模技術有gmapping和優化Hectorslam（gmapping）兩種。gmapping是一種最常用的二維SLAM算法，它使用的是一種粒子濾波技術，當場景擴大時，粒子數量會隨之增多。Hectorslam采用高斯牛頓法，對觀測設備要求較高，不需要長程計，適用于非平坦地區的無人機和地面移動機器人。通過比較，本文采用基于gmapping的方法進行地圖構建，該方法對激光雷達的頻率要求較低，魯棒性強，但在快速轉彎過程中，傳感器極易出現誤匹配，從而影響車輛避障模擬試驗。利用ROS中已有的地圖和定位算法gmapping構建上述環境的效果，其結果見圖6，其中黑色部分為障礙占據范圍，白色部分為可行駛范圍，灰色部分為未知范圍。

由圖6建立的結果可知，gmapping算法能夠建立一幅已知的障礙環境圖，并且在所建立的環境中，障礙物相對位置與所構建環境中障礙物的位置是完全吻合的。

試驗中DWA算法評價功能的權重設定見表4。DWA在改進前僅包含了方位、距離、速度三個子函數，所以將障礙個數的權重設定為0，并對障礙個數進行相應的加權。

通過對兩層式避障算法的模擬試驗，得到了如圖7、圖8、圖9所示的效果。其中，以無人駕駛車輛的位置作為起點（無人車輛計劃完成之后，將其恢復到初始狀態），紅色的箭頭表示目標地點，白色的表示可以行駛的范圍，綠色的表示未知的范圍，黑色的表示障礙物的占有范圍。灰曲線是根據全局規劃的A*算法生成的，而紅線則是兩層式的避障算法。

在環境1中，由圖7a可知，將A*和DWA兩種方法相結合，能夠在“C”字形障礙區內進行無人駕駛車輛的路徑規劃，但由于距離障礙物太遠，并未選擇較好的駕駛區域。由圖7b可知，將A*算法與改進 DWA方法相結合，能有效地解決DWA易陷入局部極小的問題，而且所得到的方案具有較好的合理性。

圖8是在環境2下的模擬結果，由8a可以看到，當無人駕駛車輛通過密集的障礙物時，由于離障礙物太近，有安全隱患。如圖8b所示，改進后的DWA算法能夠在流形上選擇與障礙物無關的預航跡，從而實現對障礙物的平穩繞行，既能保障無人車輛的安全，又能保持路徑的光滑性。

圖9是在環境3下的模擬結果，其中新增加的障礙占據面積由一條圓曲線表示，黑部表示靜止障礙占據范圍，灰色網格表示可行駛區域，綠色網格表示待探索區域。

如圖9所示，與傳rYlGbC+mpIrxtHQab3BqYw==統DWA算法相比，采用A*算法進行全局規劃時，車輛在障礙物稠密區域時會繞過障礙物，達到目標點，證明了DWA算法易陷入局部極值；在DWA中增加障礙數目評價子函數，減少DWA對密集區內障礙物危險程度的判定，使得無人車輛能夠穿越稠密區域抵達目的地，從而減少無人車輛的行程，符合模擬結論。從圖9b可以看到，當無人駕駛車輛接近目標點時，新加入的障礙物會阻礙車輛的行進，使得該方法能夠避免新增加的障礙，同時避免了障礙物的產生，使得車輛的避障軌跡更加流暢，所選擇的路徑也比原有的DWA方法好。

在此基礎上，對三種不同的情況進行了優化，結果對比如表5所示。

從表5可以看出，與A*、改進DWA相比，兩種新的DWA方法可以使規劃路線長度降低20.05%，規劃時間降低21.17%，進一步驗證了對DWA算法改進的有效性，說明在復雜環境下改進的DWA算法能夠提高無人車的運行效率，減短路徑長度和規劃時間。

4 結語

本文在已有虛擬車道存在的情況下，利用Lidar主動探測模式和局部路徑規劃方法，實現無人車輛避障。基于A*、DWA等算法，對動態規劃方法進行二次優化，實現對機器人行進方向及速度的實時修正，從而獲得流暢、高效的避障路徑，提高機器人的避障效率。通過試驗，證明了本文所提出的算法的實時性和穩定性。

參考文獻：

[1]朱茂飛，胡方亞，李娜可，等.無人駕駛汽車路徑規劃算法綜述[J].農業裝備與車輛工程，2023，61（11）：18-22.

[2]潘紹飛.無人駕駛汽車路徑規劃算法研究綜述[J].汽車實用技術，2022，47（4）：162-165.

[3]楊時川，胡曉曉，胡漢橋.淺析幾種無人駕駛汽車路徑規劃算法[J].時代汽車，2021（1）：171-173.

[4]袁師召，李軍.無人駕駛汽車路徑規劃研究綜述[J].汽車實用技術，2022（5）：21-23.

[5]蔣紅梅，黃鵬，廖愛文.基于GPS與激光雷達的無人駕駛組合導航研究[J].科技創新與應用，2023，13（16）：31-34.

[6]劉博，羅霞，朱健.無人駕駛車輛自動避障路徑規劃仿真研究[J].計算機仿真，2018，35（2）：105-110.

[7]曾爽.多因素影響下的無人駕駛汽車最短路徑規劃[J].區域治理，2020（45）：1-2.