摘要：針對(duì)無(wú)人駕駛車輛在自主行駛過(guò)程中存在的避障失效、避障規(guī)劃軌跡不光滑、避障算法實(shí)時(shí)性差等問(wèn)題，提出一種基于Lidar的車輛動(dòng)態(tài)避障路徑規(guī)劃方法，采用A*算法進(jìn)行全局規(guī)劃，在已知環(huán)境圖上進(jìn)行尋優(yōu)，并對(duì)新出現(xiàn)的障礙物采用DWA算法進(jìn)行避障，同時(shí)采用全局局部?jī)?yōu)化的雙重避障算法實(shí)現(xiàn)無(wú)人車輛的安全抵達(dá)。

關(guān)鍵詞：多線激光雷達(dá)；無(wú)人駕駛車；局部軌跡規(guī)劃

中圖分類號(hào)：U467 收稿日期：2024-07-15

DOI：10.19999/j.cnki.1004-0226.2024.09.016

1 傳感器選取

激光雷達(dá)可以獲得高精度的數(shù)字高程，并產(chǎn)生高精度的3D定位信息，從而可以更好地識(shí)別出被測(cè)量對(duì)象的具體位置和尺寸［1］。激光雷達(dá)發(fā)射的高頻率激光可以在1 s內(nèi)對(duì)被測(cè)目標(biāo)進(jìn)行100個(gè)量級(jí)的激光掃描，得到目標(biāo)的位置信息，即激光點(diǎn)云，再由算法對(duì)目標(biāo)進(jìn)行3D模型化，在獲取目標(biāo)的位置信息的同時(shí)，還可以提取出目標(biāo)的大概輪廓。

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

2 控制算法

2.1 基于A*算法的全局路徑規(guī)劃

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

[fn=gn+hn] （1）

式中，[fn]為用來(lái)選取下一個(gè)遍歷節(jié)點(diǎn)的總成本函數(shù)；[gn]為從起始點(diǎn)到結(jié)點(diǎn)n的最短路值；[hn]為從目標(biāo)點(diǎn)到結(jié)點(diǎn)n的最短路徑的激勵(lì)值。

2.2 基于改進(jìn)DWA算法的局部路徑規(guī)劃

2.2.1 優(yōu)b39d772d194a05699411920f68af6f6e27cefa1adfa42967f2a1a59bc541c150化DWA預(yù)軌跡選取空間

本文擬采用離散小波變換（DWA）的思想，利用離散小波變換的思想，將避障問(wèn)題轉(zhuǎn)化為最優(yōu)解，從而實(shí)現(xiàn)基于離散小波分解的無(wú)人車輛路徑規(guī)劃。然而，當(dāng)遇到障礙物較多時(shí)，基于DWA的離散小波變換，其目標(biāo)函數(shù)是下一步的最優(yōu)路徑，從而導(dǎo)致規(guī)劃路徑并非最短，且規(guī)劃過(guò)程不光滑。

微分流形是一種具有微分結(jié)構(gòu)的拓?fù)淞餍?，它是一種拓?fù)渑c幾何的重要研究?jī)?nèi)容［4］。本文擬采用微分流形切空間的方法，在流形切空間中建立通用的無(wú)人飛行器軌跡模型，并在此基礎(chǔ)上，對(duì)已有障礙物的無(wú)人車輛進(jìn)行去除。無(wú)人飛行器的軌跡方程式可歸納為：

[B=T×N] （4）

[N=B×T] （5）

式中，[Snt]為t時(shí)間n下無(wú)人車輛的運(yùn)動(dòng)軌跡；T為無(wú)人機(jī)航跡的切線矢量；B為無(wú)人機(jī)的輔助法矢量；N為無(wú)人機(jī)航跡的主方向矢量。

將無(wú)人飛行器的軌跡抽象為一剛性運(yùn)動(dòng)，其中，將無(wú)人飛行器的移動(dòng)狀態(tài)設(shè)為｛T，B，N}，如圖1所示，S為無(wú)人飛行器的運(yùn)行軌跡，p為S軌道上的姿態(tài)點(diǎn)，在軌跡的各個(gè)姿態(tài)點(diǎn)p上所對(duì)應(yīng)的切空間如下：

切空間的本質(zhì)就是曲面M在點(diǎn)p處的無(wú)窮小領(lǐng)域進(jìn)行無(wú)窮大的線性擴(kuò)張所產(chǎn)生的空間。圖1中[ψ1，ψ2，…，ψi]分別對(duì)應(yīng)著流形切空間中的不同切向量，即表示的是無(wú)人車運(yùn)動(dòng)朝向。從[ψ1，ψ2，…，ψi]中選取機(jī)器人下一時(shí)刻的軌跡，去除掉與障礙物有交集區(qū)域的軌跡從而優(yōu)化了允許速度空間，一定程度上減少了軌跡評(píng)價(jià)數(shù)目，使機(jī)器人能夠準(zhǔn)確躲避障礙物運(yùn)動(dòng)。

圖2是傳統(tǒng)DWA仿真的軌跡與流行空間切矢量仿真的軌跡對(duì)照?qǐng)D，可以看出，通過(guò)選擇下一時(shí)刻的預(yù)航跡，在密集障礙區(qū)，通過(guò)切矢量選擇下一時(shí)刻的預(yù)航跡，可以使下一時(shí)刻的軌跡更平滑地銜接，從而減少規(guī)劃時(shí)間，確保無(wú)人車輛的穩(wěn)定行駛，提升運(yùn)營(yíng)效率。

2.2.2 改進(jìn)評(píng)價(jià)函數(shù)

傳統(tǒng)DWA算法中，評(píng)估函數(shù)評(píng)估指標(biāo)相互獨(dú)立，缺乏對(duì)運(yùn)動(dòng)軌跡選擇的綜合考量，且不同環(huán)境下的權(quán)值比例差別較大，使得機(jī)器人在復(fù)雜環(huán)境下極易陷入局部極值［5-6］。針對(duì)傳統(tǒng)DWA算法在障礙物較多的情況下，選擇的路徑并非最優(yōu)的問(wèn)題［7］，提出了一種新的基于小波分解的多目標(biāo)優(yōu)化方法，公示如下：

式中，[xs，ys]為機(jī)器人起始點(diǎn)坐標(biāo)；[xt，yt]為機(jī)器人目標(biāo)點(diǎn)坐標(biāo)；M為模擬地圖的面積；[xi，yi]為機(jī)器人在i時(shí)刻的位置坐標(biāo)；數(shù)字[v，w]為障礙數(shù)目評(píng)價(jià)子函數(shù)。這個(gè)函數(shù)的設(shè)定是，隨著機(jī)器人周圍的障礙物的數(shù)目越來(lái)越多，這個(gè)函數(shù)就會(huì)變得越來(lái)越小，[δ]的值會(huì)越大，路徑的危險(xiǎn)性越小。此時(shí)評(píng)價(jià)函數(shù)為：

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

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

DWA算法利用無(wú)人車輛與目標(biāo)點(diǎn)之間的夾角很小，使得無(wú)人車輛能夠更快地朝著目的地移動(dòng)，而當(dāng)接近目的地時(shí)，評(píng)價(jià)函數(shù)（heading，w）會(huì)發(fā)生顯著改變，從而使得車輛在接近目的地時(shí)失去穩(wěn)定性。因此，當(dāng)無(wú)人飛行器接近目標(biāo)點(diǎn)時(shí)，其方位估計(jì)值應(yīng)該被降低。具體公式如下：

式中，heading為在跟蹤結(jié)束時(shí)，無(wú)人機(jī)離目標(biāo)點(diǎn)的位置偏移角度q；路徑為全局路徑規(guī)劃的長(zhǎng)度；Edist為從目標(biāo)點(diǎn)到目標(biāo)點(diǎn)的距離。在無(wú)人機(jī)的初始運(yùn)行階段，[ε]較大，heading評(píng)估作用較大；無(wú)人車靠近目標(biāo)點(diǎn)時(shí)，[ε]較小，在這個(gè)時(shí)候，Edist的權(quán)重系數(shù)會(huì)增加，而標(biāo)題的評(píng)價(jià)作用會(huì)越來(lái)越小。在此基礎(chǔ)上，提出了一種基于視覺(jué)信息的無(wú)人駕駛車輛跟蹤方法。最后的評(píng)估功能得到了改善：

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

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

2.2.3 效果對(duì)比試驗(yàn)

搭建無(wú)人駕駛車輛在復(fù)雜障礙環(huán)境下的自主規(guī)劃能力，并對(duì)其運(yùn)動(dòng)軌跡、方向、起點(diǎn)到目的地所需的時(shí)間等參數(shù)進(jìn)行實(shí)驗(yàn)驗(yàn)證。在同一場(chǎng)景下，比較不同算法在不同方案下的運(yùn)行效率，并對(duì)其結(jié)果進(jìn)行分析。

設(shè)定初始狀態(tài)時(shí)，無(wú)人飛行器的位置坐標(biāo)是（0，0），移動(dòng)狀態(tài)是靜態(tài)，方向角是0，無(wú)人機(jī)的最大線性速度設(shè)定為8 m/s，最小線性速度是0 m/s，最大角速度是30 °/s2，最小線加速度是-30 °/s2，最大線加速度是3 m/s2，最大角加速度是30 °/s2，無(wú)人飛行器的擴(kuò)張半徑確定在障礙物擴(kuò)張半徑之內(nèi)，DWA算法的主要參數(shù)設(shè)定如表2所示。

為了進(jìn)一步提高DWA算法在密集區(qū)障礙環(huán)境中的預(yù)航跡選擇空間，本文擬通過(guò)不同尺寸的障礙圖和同樣尺寸的狹窄可行區(qū)域圖兩種模擬場(chǎng)景，并在環(huán)境1下，進(jìn)行算法優(yōu)化前后路徑規(guī)劃效果的比較。在圖3中，點(diǎn)坐標(biāo)（0，0）是機(jī)器人的移動(dòng)起點(diǎn)，點(diǎn)坐標(biāo)（20，20）是目標(biāo)坐標(biāo)，在試驗(yàn)中，設(shè)定了不同尺寸的障礙物（紫環(huán)的尺寸），藍(lán)線是無(wú)人駕駛車輛的最后一條路線。

如圖3a所示，在遇到稠密障礙區(qū)域時(shí)，原有DWA算法會(huì)選擇繞過(guò)障礙區(qū)域行進(jìn)，而增加障礙數(shù)的DWA則減少了其危險(xiǎn)性；在狹窄區(qū)域，不繞過(guò)障礙物，直接穿越障礙區(qū)抵達(dá)目標(biāo)點(diǎn)，從而減少了路徑規(guī)劃時(shí)間。

2.3 雙層避障算法的流程設(shè)計(jì)

圖4給出了用A*算法和改進(jìn)DWA算法進(jìn)行無(wú)人駕駛車輛的軌跡規(guī)劃流程。首先，地圖服務(wù)器會(huì)將一張靜態(tài)地圖發(fā)送到A*算法中，同時(shí)輸入無(wú)人車的起點(diǎn)，在全局地圖上，A*算法根據(jù)地圖搜索出整個(gè)區(qū)域的路線覆蓋目標(biāo)點(diǎn)，再在全局路線上設(shè)定節(jié)點(diǎn)作為本地路線規(guī)劃的目的地，當(dāng)無(wú)人駕駛車輛行駛時(shí)，若在無(wú)障礙物的情況下，就會(huì)逐漸靠近本地目標(biāo)，若發(fā)現(xiàn)有障礙物，就會(huì)用改進(jìn)的DWA方法，將其轉(zhuǎn)移到下一個(gè)地方，這樣不斷重復(fù)，直至抵達(dá)目的地為止。

3 仿真與分析

為了檢驗(yàn)A*以及改進(jìn)的DWA雙層障礙規(guī)避算法的有效性，采用turtle 3、waffle_pi等無(wú)人車輛模型，在 ROS環(huán)境中進(jìn)行試驗(yàn)，在此基礎(chǔ)上，給出了模型的有關(guān)參數(shù)，如表3所示。

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

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

由圖6建立的結(jié)果可知，gmapping算法能夠建立一幅已知的障礙環(huán)境圖，并且在所建立的環(huán)境中，障礙物相對(duì)位置與所構(gòu)建環(huán)境中障礙物的位置是完全吻合的。

試驗(yàn)中DWA算法評(píng)價(jià)功能的權(quán)重設(shè)定見(jiàn)表4。DWA在改進(jìn)前僅包含了方位、距離、速度三個(gè)子函數(shù)，所以將障礙個(gè)數(shù)的權(quán)重設(shè)定為0，并對(duì)障礙個(gè)數(shù)進(jìn)行相應(yīng)的加權(quán)。

通過(guò)對(duì)兩層式避障算法的模擬試驗(yàn)，得到了如圖7、圖8、圖9所示的效果。其中，以無(wú)人駕駛車輛的位置作為起點(diǎn)（無(wú)人車輛計(jì)劃完成之后，將其恢復(fù)到初始狀態(tài)），紅色的箭頭表示目標(biāo)地點(diǎn)，白色的表示可以行駛的范圍，綠色的表示未知的范圍，黑色的表示障礙物的占有范圍?；仪€是根據(jù)全局規(guī)劃的A*算法生成的，而紅線則是兩層式的避障算法。

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

圖8是在環(huán)境2下的模擬結(jié)果，由8a可以看到，當(dāng)無(wú)人駕駛車輛通過(guò)密集的障礙物時(shí)，由于離障礙物太近，有安全隱患。如圖8b所示，改進(jìn)后的DWA算法能夠在流形上選擇與障礙物無(wú)關(guān)的預(yù)航跡，從而實(shí)現(xiàn)對(duì)障礙物的平穩(wěn)繞行，既能保障無(wú)人車輛的安全，又能保持路徑的光滑性。

圖9是在環(huán)境3下的模擬結(jié)果，其中新增加的障礙占據(jù)面積由一條圓曲線表示，黑部表示靜止障礙占據(jù)范圍，灰色網(wǎng)格表示可行駛區(qū)域，綠色網(wǎng)格表示待探索區(qū)域。

如圖9所示，與傳rYlGbC+mpIrxtHQab3BqYw==統(tǒng)DWA算法相比，采用A*算法進(jìn)行全局規(guī)劃時(shí)，車輛在障礙物稠密區(qū)域時(shí)會(huì)繞過(guò)障礙物，達(dá)到目標(biāo)點(diǎn)，證明了DWA算法易陷入局部極值；在DWA中增加障礙數(shù)目評(píng)價(jià)子函數(shù)，減少DWA對(duì)密集區(qū)內(nèi)障礙物危險(xiǎn)程度的判定，使得無(wú)人車輛能夠穿越稠密區(qū)域抵達(dá)目的地，從而減少無(wú)人車輛的行程，符合模擬結(jié)論。從圖9b可以看到，當(dāng)無(wú)人駕駛車輛接近目標(biāo)點(diǎn)時(shí)，新加入的障礙物會(huì)阻礙車輛的行進(jìn)，使得該方法能夠避免新增加的障礙，同時(shí)避免了障礙物的產(chǎn)生，使得車輛的避障軌跡更加流暢，所選擇的路徑也比原有的DWA方法好。

在此基礎(chǔ)上，對(duì)三種不同的情況進(jìn)行了優(yōu)化，結(jié)果對(duì)比如表5所示。

從表5可以看出，與A*、改進(jìn)DWA相比，兩種新的DWA方法可以使規(guī)劃路線長(zhǎng)度降低20.05%，規(guī)劃時(shí)間降低21.17%，進(jìn)一步驗(yàn)證了對(duì)DWA算法改進(jìn)的有效性，說(shuō)明在復(fù)雜環(huán)境下改進(jìn)的DWA算法能夠提高無(wú)人車的運(yùn)行效率，減短路徑長(zhǎng)度和規(guī)劃時(shí)間。

4 結(jié)語(yǔ)

本文在已有虛擬車道存在的情況下，利用Lidar主動(dòng)探測(cè)模式和局部路徑規(guī)劃方法，實(shí)現(xiàn)無(wú)人車輛避障?；贏*、DWA等算法，對(duì)動(dòng)態(tài)規(guī)劃方法進(jìn)行二次優(yōu)化，實(shí)現(xiàn)對(duì)機(jī)器人行進(jìn)方向及速度的實(shí)時(shí)修正，從而獲得流暢、高效的避障路徑，提高機(jī)器人的避障效率。通過(guò)試驗(yàn)，證明了本文所提出的算法的實(shí)時(shí)性和穩(wěn)定性。

參考文獻(xiàn)：

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

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

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

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

[5]蔣紅梅，黃鵬，廖愛(ài)文.基于GPS與激光雷達(dá)的無(wú)人駕駛組合導(dǎo)航研究[J].科技創(chuàng)新與應(yīng)用，2023，13（16）：31-34.

[6]劉博，羅霞，朱健.無(wú)人駕駛車輛自動(dòng)避障路徑規(guī)劃仿真研究[J].計(jì)算機(jī)仿真，2018，35（2）：105-110.

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