摘 要：針對(duì)水面無人艇USV全局路徑規(guī)劃中存在的生成路徑冗余、無法滿足實(shí)時(shí)性及局部路徑規(guī)劃易陷入局部最優(yōu)等問題，提出了融合改進(jìn)動(dòng)態(tài)避障策略和可變步長(zhǎng)A-Star算法的路徑規(guī)劃算法DAVSA。首先，通過可變步長(zhǎng)搜索策略對(duì)柵格地圖進(jìn)行預(yù)處理，篩選出關(guān)鍵跳轉(zhuǎn)點(diǎn)作為A-Star算法待考慮的擴(kuò)展節(jié)點(diǎn)，快速生成全局最優(yōu)路徑。其次，提出路徑二次優(yōu)化策略，縮短全局路徑長(zhǎng)度，解決全局路徑的轉(zhuǎn)折點(diǎn)多、路徑冗余的問題。最后，針對(duì)傳統(tǒng)A-Star算法無法滿足實(shí)時(shí)路徑規(guī)劃的問題，提出改進(jìn)局部路徑動(dòng)態(tài)避障策略，構(gòu)建全局路徑距離評(píng)價(jià)因子，達(dá)到實(shí)時(shí)最優(yōu)路徑規(guī)劃的目的。仿真結(jié)果表明，相較于IDWA算法，DAVSA算法的平均全局尋路時(shí)間減少了59.46%，綜合算法運(yùn)行時(shí)間減少了5.08%，綜合路徑長(zhǎng)度縮短了9.08%，能夠滿足動(dòng)態(tài)環(huán)境中實(shí)時(shí)避障的要求。

關(guān)鍵詞：跳點(diǎn)搜索；A-Star算法；可變步長(zhǎng)；地圖預(yù)處理；路徑二次優(yōu)化；動(dòng)態(tài)避障；路徑規(guī)劃

中圖分類號(hào)：TP242 文獻(xiàn)標(biāo)識(shí)碼：A 文章編號(hào)：2095-1302（2025）03-00-10

0 引 言

隨著海洋資源保護(hù)的重要性不斷提升以及大國(guó)競(jìng)爭(zhēng)日趨激烈，發(fā)展水面無人艦艇變得至關(guān)重要。新一代智能水面無人艦艇（Unmanned Surface Vessel， USV）具有自主導(dǎo)航、智能避障和自主探測(cè)目標(biāo)區(qū)域環(huán)境信息的能力[1]，在海洋資源勘探、環(huán)境監(jiān)測(cè)和海洋安全等領(lǐng)域得到廣泛應(yīng)用。高效的路徑規(guī)劃技術(shù)對(duì)提高USV的作業(yè)效率和資源利用率至關(guān)重要[2]。USV的路徑規(guī)劃算法主要分為基于已知環(huán)境信息的全局路徑規(guī)劃和未知環(huán)境的局部路徑規(guī)劃。其中，全局路徑規(guī)劃包括A-Star算法、RRT算法、遺傳算法、蟻群算法、粒子群算法、神經(jīng)網(wǎng)絡(luò)算法、強(qiáng)化學(xué)習(xí)算法等。局部路徑規(guī)劃包括動(dòng)態(tài)窗口算法（Dynamic Window Algorithm， DWA）、人工勢(shì)場(chǎng)法、時(shí)間彈性帶算法、模型預(yù)測(cè)控制算法、基于采樣的算法等[3-14]。在靜態(tài)已知地圖環(huán)境中，全局路徑規(guī)劃算法有著更好的性能，可以很快地找到一條最優(yōu)路徑，但不適用于動(dòng)態(tài)多變的地圖環(huán)境。局部路徑規(guī)劃算法能夠根據(jù)實(shí)時(shí)環(huán)境信息進(jìn)行路徑規(guī)劃，快速響應(yīng)環(huán)境變化，并及時(shí)修正路徑，但是存在容易陷入局部最優(yōu)的問題。因此，找到一種既能快速生成全局最優(yōu)路徑，又能根據(jù)實(shí)時(shí)信息進(jìn)行動(dòng)態(tài)避障的路徑規(guī)劃算法成為USV安全、高效行駛的關(guān)鍵。

A-Star算法由于具有易實(shí)現(xiàn)、低復(fù)雜度等優(yōu)點(diǎn)，被廣泛應(yīng)用在全局路徑規(guī)劃中，但是該算法存在生成路徑冗余和無法滿足實(shí)時(shí)性等問題。針對(duì)這些問題，許多國(guó)內(nèi)外學(xué)者做了大量的研究工作。文獻(xiàn)[15]提出了一種基于跳點(diǎn)搜索的分層?xùn)鸥竦貓D的JA*算法，將環(huán)境信息分為結(jié)構(gòu)層與非結(jié)構(gòu)層，并建立了搜索策略切換機(jī)制，從而有效減少了計(jì)算量，但是該算法只適用于靜態(tài)環(huán)境地圖。文獻(xiàn)[16]提出了改進(jìn)的A-Star算法與貪婪算法相結(jié)合的多目標(biāo)路徑規(guī)劃算法，改進(jìn)后的算法收斂速度更快，同時(shí)剔除了A-Star算法中不必要的節(jié)點(diǎn)，生成的路徑更加平滑，路徑長(zhǎng)度減少了5%，但是不能應(yīng)對(duì)地圖中存在移動(dòng)障礙物的情況。文獻(xiàn)[17]提出了一種名為FAPF的模糊決策方法，旨在消除USV在路徑上遇到障礙物無法避障的問題。但是，該算法沒有將障礙物的碰撞類型作更細(xì)致的劃分。

在真實(shí)海洋環(huán)境中，USV隨時(shí)會(huì)遇到突發(fā)或未知的風(fēng)險(xiǎn)，因此需要采取局部路徑規(guī)劃的動(dòng)態(tài)避障策略，以確保航行的安全。其中最經(jīng)典的動(dòng)態(tài)避障算法是動(dòng)態(tài)窗口法，DWA算法具有良好的避障能力，能夠滿足實(shí)時(shí)路徑規(guī)劃等要求，其針對(duì)動(dòng)態(tài)障礙物也有顯著的避障效果，并且生成的路徑更為平滑，缺點(diǎn)是容易陷入局部最優(yōu)。文獻(xiàn)[18]提出了一種改進(jìn)蟻群算法與DWA算法融合的路徑規(guī)劃算法，解決了蟻群算法的鎖死問題，但是該算法在尋找全局路徑時(shí)所消耗的時(shí)間成本較高。文獻(xiàn)[19]提出了改進(jìn)A*算法與DWA算法相融合的路徑規(guī)劃方法，在保證全局路徑最優(yōu)的前提下，增強(qiáng)了其動(dòng)態(tài)避障的能力，但是移動(dòng)障礙物運(yùn)動(dòng)方式過于單一，避障性能無法充分體現(xiàn)。文獻(xiàn)[20]提出了一種結(jié)合DWA算法的改進(jìn)型IWSO-DWA算法，并在USV的路徑規(guī)劃中引入國(guó)際海上避碰規(guī)則公約（COLREGs），提出的方法不僅能在復(fù)雜的海洋環(huán)境中規(guī)劃出最優(yōu)的全局路徑，還能實(shí)時(shí)動(dòng)態(tài)地避開其他船只，確保USV的安全航行。文獻(xiàn)[21]提出了一種滿足COLREGs的IDWA路徑規(guī)劃方法，解決了USV無法到達(dá)目的地、路徑不合理等問題，使得USV能夠避開靜態(tài)障礙物到達(dá)目的地，但是在遇到左側(cè)存在交叉移動(dòng)障礙物的情況時(shí)，采取此策略可能會(huì)發(fā)生碰撞風(fēng)險(xiǎn)。

綜上所述，針對(duì)USV在全局路徑規(guī)劃中存在的生成路徑冗余、實(shí)時(shí)性低及局部路徑規(guī)劃易陷入局部最優(yōu)等問題，本文提出了融合改進(jìn)動(dòng)態(tài)避障策略和可變步長(zhǎng)A-Star算法的路徑規(guī)劃算法（Dynamic Avoidance and Variable Step A-star， DAVSA）。DAVSA分為全局路徑規(guī)劃和局部路徑規(guī)劃2個(gè)部分。在全局路徑規(guī)劃階段，通過可變步長(zhǎng)搜索策略對(duì)柵格地圖進(jìn)行預(yù)處理，篩選出關(guān)鍵跳轉(zhuǎn)點(diǎn)作為A-Star算法待考慮的擴(kuò)展節(jié)點(diǎn)，利用啟發(fā)式搜索快速生成全局最優(yōu)路徑，減少尋路過程中節(jié)點(diǎn)冗余的問題。接著，提出路徑二次優(yōu)化策略，進(jìn)一步縮短全局路徑長(zhǎng)度。在局部路徑規(guī)劃階段，針對(duì)傳統(tǒng)A-Star算法實(shí)時(shí)性低的問題，提出局部路徑動(dòng)態(tài)避障策略，構(gòu)建全局路徑距離評(píng)價(jià)因子，達(dá)到實(shí)時(shí)最優(yōu)路徑規(guī)劃的目的。

1 環(huán)境建模

常見的水面無人艦艇路徑規(guī)劃地圖建模方法有路標(biāo)導(dǎo)航法、SLAM技術(shù)建模、柵格地圖法等。其中柵格地圖操作簡(jiǎn)單，易于實(shí)現(xiàn)，適用范圍廣，因此本文采用柵格法構(gòu)建環(huán)境模型。將海域環(huán)境離散為一個(gè)個(gè)柵格，如圖1所示。每個(gè)柵格表示一個(gè)狀態(tài)，白色柵格表示自由柵格，代表可行區(qū)域；黑色柵格表示障礙物節(jié)點(diǎn)，不可通行和穿越。在地圖上自左向右、自下而上由0開始為柵格添加編號(hào)，依次是0， 1， 2， 3， ...， 399。

柵格編號(hào)與柵格之間的轉(zhuǎn)換計(jì)算方法如式（1）和式（2）所示：

（1）

i=（x-1）+（y-1）*n （2）

式中：（x， y）表示柵格對(duì)應(yīng)的坐標(biāo)位置；i表示柵格編號(hào)；n表示柵格總列數(shù)；mod（）為取余函數(shù)；floor（）為向下取整函數(shù)。

2 DAVSA算法設(shè)計(jì)

DAVSA算法分為全局路徑規(guī)劃和局部路徑規(guī)劃2部分。首先，利用可變步長(zhǎng)搜索策略篩選出地圖中所有的跳點(diǎn)，再根據(jù)啟發(fā)式搜索對(duì)其進(jìn)行遍歷，找到一條全局最優(yōu)路徑；然后，對(duì)生成的全局路徑進(jìn)行二次優(yōu)化，并提取路徑上的關(guān)鍵點(diǎn)信息，作為下一步局部路徑規(guī)劃的引導(dǎo)點(diǎn)；最后，由DWA算法依次導(dǎo)航到每個(gè)局部引導(dǎo)點(diǎn)，并改進(jìn)路徑評(píng)價(jià)函數(shù)，引入全局路徑評(píng)價(jià)因子，使其盡量沿著已經(jīng)規(guī)劃好的全局路徑行駛。這樣，全局與局部的融合不僅可以使無人艇快速到達(dá)目的地，同時(shí)也可以避免陷入局部最優(yōu)并提高算法實(shí)時(shí)性，保證了系統(tǒng)的時(shí)效性和安全性。通過不斷地更新和優(yōu)化局部路徑，可以使無人艇始終處于最佳狀態(tài)，保證任務(wù)的順利完成。全局路徑規(guī)劃的目的是快速找到一條全局最優(yōu)路徑，因此需要引入可變步長(zhǎng)搜索策略和路徑二次優(yōu)化策略。

算法流程如圖2所示。

2.1 變步長(zhǎng)搜索策略

傳統(tǒng)的A-Star算法采用八鄰域搜索策略，如圖3所示。在擴(kuò)展節(jié)點(diǎn)時(shí)需要將當(dāng)前節(jié)點(diǎn)周圍所有非障礙物節(jié)點(diǎn)和不在開放列表中的節(jié)點(diǎn)都進(jìn)行遍歷，圖中淺灰色節(jié)點(diǎn)表示需要擴(kuò)展的節(jié)點(diǎn)。選擇一個(gè)函數(shù)啟發(fā)值最小的節(jié)點(diǎn)作為父節(jié)點(diǎn)，將該節(jié)點(diǎn)加入到關(guān)閉列表中。然后繼續(xù)下一輪的遍歷，直到遍歷到終點(diǎn)為止。最后，從終點(diǎn)開始向前回溯，順著每個(gè)節(jié)點(diǎn)的父節(jié)點(diǎn)一直往前走，直到回溯到起點(diǎn)。當(dāng)回溯到起點(diǎn)后，就得到了一條從起點(diǎn)到終點(diǎn)的全局最優(yōu)路徑。這樣雖然計(jì)算簡(jiǎn)單，但是存在大量非必要節(jié)點(diǎn)的遍歷操作，以及對(duì)開放列表和關(guān)閉列表的反復(fù)讀寫操作，使得算法在應(yīng)對(duì)大場(chǎng)景地圖時(shí)整體效率不高。

為了解決傳統(tǒng)A-Star算法在路徑規(guī)劃中存在的擴(kuò)展節(jié)點(diǎn)多的問題，本文提出采用可變步長(zhǎng)搜索策略對(duì)其進(jìn)行改進(jìn)。可變步長(zhǎng)搜索策略在原有算法的基礎(chǔ)上修改節(jié)點(diǎn)擴(kuò)展規(guī)則，不再對(duì)所有鄰居節(jié)點(diǎn)進(jìn)行遍歷、計(jì)算，而是只遍歷地圖中的關(guān)鍵跳轉(zhuǎn)點(diǎn)，簡(jiǎn)稱跳點(diǎn)。跳點(diǎn)就是可以在搜索時(shí)可以直接跳躍的節(jié)點(diǎn)，2個(gè)跳點(diǎn)之間的節(jié)點(diǎn)不會(huì)被遍歷，這樣就大大提高了搜索效率。不同于Jump-A*算法，可變步長(zhǎng)A*算法通過動(dòng)態(tài)調(diào)整搜索步長(zhǎng)以適應(yīng)地圖的不同區(qū)域，而Jump-A*算法通過跳過對(duì)稱的、無障礙的路徑段，來減少搜索空間。

關(guān)鍵跳轉(zhuǎn)點(diǎn)的選取規(guī)則如下：節(jié)點(diǎn)N是起點(diǎn)或終點(diǎn)；節(jié)點(diǎn)N至少有一個(gè)強(qiáng)迫鄰居；節(jié)點(diǎn)N的水平或垂直方向有滿足上述2個(gè)條件的節(jié)點(diǎn)。如果節(jié)點(diǎn)N滿足任意條件，則稱節(jié)點(diǎn)N為關(guān)鍵跳轉(zhuǎn)點(diǎn)。

強(qiáng)迫鄰居是指算法在遍歷過程中必須要訪問的節(jié)點(diǎn)。在搜索過程中，會(huì)把當(dāng)前節(jié)點(diǎn)的強(qiáng)迫鄰居設(shè)定為下一步的目標(biāo)點(diǎn)。圖4表示當(dāng)前節(jié)點(diǎn)N周圍沒有障礙物存在的情況，此時(shí)當(dāng)前節(jié)點(diǎn)N周圍的灰色區(qū)域代表一般鄰居，一般鄰居節(jié)點(diǎn)不進(jìn)行遍歷操作。

假設(shè)節(jié)點(diǎn)N的父節(jié)點(diǎn)為P，如果節(jié)點(diǎn)P經(jīng)過N到達(dá)節(jié)點(diǎn)Q的路徑長(zhǎng)度比不經(jīng)過N到達(dá)Q的任意路徑的長(zhǎng)度短，則稱Q是N的強(qiáng)迫鄰居。如圖5所示，節(jié)點(diǎn)P經(jīng)過節(jié)點(diǎn)N到達(dá)節(jié)點(diǎn)Q的路徑長(zhǎng)度要比不經(jīng)過節(jié)點(diǎn)N到達(dá)節(jié)點(diǎn)Q的任意路徑的長(zhǎng)度要小，因此將節(jié)點(diǎn)Q定義為節(jié)點(diǎn)N的強(qiáng)迫鄰居。圖5（a）表示在水平方向搜索時(shí)，障礙物節(jié)點(diǎn)位于當(dāng)前節(jié)點(diǎn)下方的情況，此時(shí)節(jié)點(diǎn)P經(jīng)過當(dāng)前節(jié)點(diǎn)N到達(dá)Q的距離最短，因此Q為N的強(qiáng)迫鄰居節(jié)點(diǎn)；同理，圖5（b）是在對(duì)角方向搜索時(shí)，障礙物節(jié)點(diǎn)位于當(dāng)前節(jié)點(diǎn)N左側(cè)的情況，Q為N的強(qiáng)迫鄰居節(jié)點(diǎn)。

加入可變步長(zhǎng)搜索策略可以減少傳統(tǒng)算法在搜索中的擴(kuò)展節(jié)點(diǎn)個(gè)數(shù)，從而大大提高搜索效率，其算法工作原理如下：

（1）首先判斷當(dāng)前節(jié)點(diǎn)是否為跳點(diǎn)；

（2）如果當(dāng)前節(jié)點(diǎn)是跳點(diǎn)，則向當(dāng)前節(jié)點(diǎn)的強(qiáng)迫鄰居節(jié)點(diǎn)擴(kuò)展，如果強(qiáng)迫鄰居節(jié)點(diǎn)也是跳點(diǎn)，則繼續(xù)向其他強(qiáng)迫鄰居擴(kuò)展；

（3）如果當(dāng)前節(jié)點(diǎn)不是跳點(diǎn)，則搜索算法會(huì)把當(dāng)前節(jié)點(diǎn)從待擴(kuò)展節(jié)點(diǎn)中刪除；

（4）每個(gè)節(jié)點(diǎn)的評(píng)估值是根據(jù)當(dāng)前節(jié)點(diǎn)到起點(diǎn)和終點(diǎn)的歐氏距離計(jì)算得出的。

通過對(duì)地圖中跳點(diǎn)的篩選，可以有效簡(jiǎn)化啟發(fā)式搜索流程，在傳統(tǒng)A-Star算法基礎(chǔ)上增加跳點(diǎn)搜索策略，可以減少尋路過程中的計(jì)算次數(shù)，快速地找到一條全局最優(yōu)路徑，從而提高算法搜索效率。加入可變步長(zhǎng)搜索策略之后的搜索路徑如圖6所示。

圖6（a）中淺灰色節(jié)點(diǎn)表示傳統(tǒng)A-Star算法在搜索時(shí)需要擴(kuò)展的節(jié)點(diǎn)，圖6（b）表示加入了可變步長(zhǎng)搜索策略后的擴(kuò)展節(jié)點(diǎn)。由此可見，加入可變步長(zhǎng)搜索策略之后，擴(kuò)展節(jié)點(diǎn)明顯減少，算法效率大大提高。

2.2 路徑二次優(yōu)化策略

加入可變步長(zhǎng)搜索策略的A-Star雖然能夠快速找到一條全局最優(yōu)路徑，但是受限于八鄰域方向搜索，生成的全局路徑上仍然存在冗余節(jié)點(diǎn)過多、路徑的轉(zhuǎn)折點(diǎn)過多、路徑長(zhǎng)度較長(zhǎng)、直線不可達(dá)等問題，因此提出路徑二次優(yōu)化策略。目的是進(jìn)一步縮短全局路徑，減少路徑的轉(zhuǎn)折點(diǎn)，為下一步的局部探索打下良好的基礎(chǔ)，并最小化無人艦艇的動(dòng)能損失。路徑經(jīng)過二次優(yōu)化后的效果如圖7（c）所示。

路徑二次優(yōu)化是在加入可變步長(zhǎng)策略之后進(jìn)行的優(yōu)化操作。具體操作流程如下：假設(shè)全局路徑由lt;1， 2， 3， ...， ngt;共n個(gè)節(jié)點(diǎn)依次連接組成，首先從1號(hào)節(jié)點(diǎn)開始，判斷l(xiāng)t;1， 3gt;是否可以直接連接且不穿過地圖中的障礙物節(jié)點(diǎn)，如果可以，則2號(hào)節(jié)點(diǎn)為冗余節(jié)點(diǎn)，應(yīng)直接刪除；此時(shí)該路徑還剩余l(xiāng)t;1， 3， 4， ...， ngt;共n-1個(gè)節(jié)點(diǎn)，再判斷l(xiāng)t;1， 4gt;節(jié)點(diǎn)是否可以直接連接；若此時(shí)穿越地圖中的障礙物節(jié)點(diǎn)，則3號(hào)節(jié)點(diǎn)就不為冗余節(jié)點(diǎn)，應(yīng)保留。接下來判斷4號(hào)節(jié)點(diǎn)是否為冗余節(jié)點(diǎn)，判斷條件是lt;3， 5gt;是否可以直接連接，依次類推直到節(jié)點(diǎn)n結(jié)束。此時(shí)，一條包含了n個(gè)路徑節(jié)點(diǎn)的無碰撞路徑二次優(yōu)化完成，可形成由節(jié)點(diǎn)lt;1， ...， k， ...ngt;（k≥0）組成的新的全局無碰撞路徑。

如圖7所示，傳統(tǒng)算法規(guī)劃出的路徑上有l(wèi)t;1， 2， 3， ...， 12gt;共12個(gè)節(jié)點(diǎn)，經(jīng)過可變步長(zhǎng)策略優(yōu)化之后的路徑可以將起點(diǎn)至3號(hào)節(jié)點(diǎn)、3號(hào)節(jié)點(diǎn)至9號(hào)節(jié)點(diǎn)、11號(hào)節(jié)點(diǎn)至終點(diǎn)之間的冗余節(jié)點(diǎn)剔除，此時(shí)路徑上有l(wèi)t;3， 9， 10， 11gt;共4個(gè)節(jié)點(diǎn)。根據(jù)路徑二次優(yōu)化策略的思想，首先判斷起點(diǎn)至9號(hào)節(jié)點(diǎn)是否可以直接聯(lián)通，發(fā)現(xiàn)連接的路徑不經(jīng)過障礙物節(jié)點(diǎn)，所以3號(hào)節(jié)點(diǎn)為冗余節(jié)點(diǎn)，應(yīng)刪除；然后連接起點(diǎn)與10號(hào)節(jié)點(diǎn)，中間經(jīng)過障礙物，則9號(hào)不是冗余節(jié)點(diǎn)，應(yīng)該保留；再連接9號(hào)與11號(hào)節(jié)點(diǎn)，同樣經(jīng)過障礙物，故保留10號(hào)節(jié)點(diǎn)；然后連接10號(hào)節(jié)點(diǎn)與終點(diǎn)，路徑經(jīng)過障礙物，故11號(hào)節(jié)點(diǎn)保留，經(jīng)過處理后的路徑上還有l(wèi)t;9， 10， 11gt;共3個(gè)節(jié)點(diǎn)。

通過路徑二次優(yōu)化策略，可以有效地減少路徑中的冗余節(jié)點(diǎn)數(shù)量，簡(jiǎn)化路徑，從而提高全局路徑質(zhì)量。

3 改進(jìn)動(dòng)態(tài)窗口算法

傳統(tǒng)的DWA算法雖然能夠滿足局部動(dòng)態(tài)避障的要求，但是容易陷入局部最優(yōu)，因此需要引入全局路徑信息進(jìn)行引導(dǎo)。若采用未經(jīng)過二次優(yōu)化的全局路徑，可能會(huì)出現(xiàn)路徑長(zhǎng)度較長(zhǎng)的問題。因此，選擇在二次優(yōu)化后的路徑上選取局部引導(dǎo)點(diǎn)，以提升局部路徑規(guī)劃的效果。對(duì)比路徑如圖8所示。其中，局部引導(dǎo)點(diǎn)取自經(jīng)過二次路徑優(yōu)化的全局路徑的轉(zhuǎn)折點(diǎn)。

為了更好地展現(xiàn)局部路徑規(guī)劃避障策略的效果，還需要對(duì)無人艇進(jìn)行運(yùn)動(dòng)學(xué)建模和速度采樣建模。運(yùn)動(dòng)學(xué)模型反映了無人艇在運(yùn)動(dòng)過程中的行為特征，如線速度和角速度等。速度采樣模型則反映了在一定時(shí)間窗口內(nèi)的速度變化情況，并生成若干采樣曲線預(yù)估無人艇到達(dá)的目標(biāo)位置等。

3.1 運(yùn)動(dòng)模型

假設(shè)無人艇非全向移動(dòng)，有左右2個(gè)推進(jìn)裝置，可以前進(jìn)和圍繞原點(diǎn)O左右旋轉(zhuǎn)，具有X、Y軸方向上的線速度ν和繞原點(diǎn)O旋轉(zhuǎn)的角速度ω。在相鄰時(shí)刻Δt內(nèi)，運(yùn)動(dòng)距離短，可將相鄰兩點(diǎn)之間的運(yùn)動(dòng)軌跡看成直線，如圖9所示。

無人艇的行駛距離可表示為：

ΔS=v*Δt （3）

式中：ν表示無人艇的線速度；Δt是相鄰時(shí)刻的間隔；ΔS為無人艇在Δt時(shí)刻內(nèi)的移動(dòng)距離。轉(zhuǎn)換成無人艇的坐標(biāo)表示為：

Δx=vΔtcos（θt） （4）

Δy=vΔtsin（θt） （5）

式中：θ是無人艇的朝向和水平軸的夾角；Δx是無人艇在Δt時(shí)間內(nèi)水平方向上的移動(dòng)距離；Δy是無人艇在Δt時(shí)間內(nèi)垂直方向上的移動(dòng)距離。無人艇在下一個(gè)時(shí)刻的位置表示為：

x=x0+vΔtcos（θt） （6）

y=y0+vΔtsin（θt） （7）

θt=θt0+ωΔt （8）

式中：（x0， y0）表示無人艇的初始位置；（x， y）表示經(jīng)過Δt時(shí)間無人艇移動(dòng)的位置。

根據(jù)以上無人艇的運(yùn)動(dòng)模型，用速度和時(shí)間信息推算出無人艇的下一個(gè)位置的狀態(tài)信息。

3.2 速度采樣模型

DWA算法主要通過在無人艇周圍建立動(dòng)態(tài)窗口來搜索最佳路徑，在動(dòng)態(tài)窗口內(nèi)對(duì)一系列線速度和角速度的候選值進(jìn)行采樣，評(píng)估無人艇當(dāng)前位置與目標(biāo)點(diǎn)之間的距離、路徑上是否有障礙物等因素，計(jì)算出無人艇在未來一段時(shí)間內(nèi)可能通過的窗口，推導(dǎo)出無人艇的最佳線速度和角速度，并選擇最優(yōu)解作為無人艇下一步的運(yùn)動(dòng)速度。速度采樣模型如圖10所示。其中，ν表示線速度；ω表示角速度；νa表示線加速度；ωa表示角加速度；Δt表示單位時(shí)間間隔。

3.3 DWA算法改進(jìn)

在速度空間（ν， ω）中采樣，根據(jù)運(yùn)動(dòng)學(xué)模型推測(cè)對(duì)應(yīng)的軌跡，引入路徑評(píng)價(jià)函數(shù)，對(duì)軌跡進(jìn)行打分，選取最優(yōu)的軌跡，一般來說，評(píng)價(jià)函數(shù)如下：

G（v， ω）=σ（α*heading（v， ω）+β*dist（v， ω）+γ*velocity（v， ω））" "（9）

式中：heading（ν， ω）為方位角評(píng)價(jià)函數(shù)，用于評(píng)價(jià)無人艇在當(dāng)前的設(shè)定速度下，軌跡末端朝向與目標(biāo)點(diǎn)之間的角度差距；dist（ν， ω）為無人艇處于預(yù)測(cè)軌跡末端點(diǎn)位置時(shí)與地圖上最近障礙物的距離，根據(jù)該距離對(duì)于靠近障礙物的采樣點(diǎn)進(jìn)行懲罰，確保無人艇的避障能力，降低無人艇與障礙物發(fā)生碰撞的概率；velocity（ν， ω）為當(dāng)前無人艇的線速度，物理意義是使無人艇避開障礙物，朝著目標(biāo)以較快的速度行駛。

上述計(jì)算并非簡(jiǎn)單地累加，而需進(jìn)行歸一化處理。通過歸一化避免不同類別得分基數(shù)相差太大，從而影響最終結(jié)果。在歸一化以后，將每個(gè)部分的權(quán)重分別定義為σ、α、β、γ，并賦予初始的權(quán)值，不同的權(quán)重系數(shù)會(huì)直接影響無人艇行駛路徑的選取。因此，引入模糊控制系統(tǒng)函數(shù)，通過輸入無人艇當(dāng)前的方位角、與最近障礙物的距離、線速度等參數(shù)，輸出一個(gè)用于調(diào)整權(quán)重系數(shù)的集合，實(shí)現(xiàn)根據(jù)當(dāng)前環(huán)境的特征自適應(yīng)地調(diào)整權(quán)重系數(shù)。

為了減少能量流失，設(shè)計(jì)了全局節(jié)能路徑偏離評(píng)價(jià)子函數(shù)distEvalu（ν， ω），權(quán)重為λ，目的是使得融合算法規(guī)劃的路徑更接近全局最優(yōu)路徑。評(píng)價(jià)子函數(shù)distEvalu（ν， ω）結(jié)合全局節(jié)能路徑信息，根據(jù)歐氏距離公式計(jì)算無人艇當(dāng)前位置與全局路徑之間的最短距離，使無人艇在局部路徑規(guī)劃時(shí)更好地遵循全局路徑，修改后的綜合評(píng)價(jià)函數(shù)如下：

G（v， ω）=σ（α*heading（v， ω）+β*dist（v， ω）+γ*velocity（v， ω）+

λ*distEvalu（v， ω）） （10）

3.4 碰撞類型及避障策略

本文按照碰撞方式的不同，將碰撞類型分為4類：正面相對(duì)行駛的障礙物、左側(cè)交叉行駛的障礙物、右側(cè)交叉行駛的障礙物和同向行駛的障礙物。如圖11所示，在遇到移動(dòng)障礙物在正面、左側(cè)交叉、右側(cè)交叉和追隨的情況下，無人艇會(huì)分別做出不同的避障策略。

結(jié)合國(guó)際海上避碰規(guī)則COLREGs，若無人艇與移動(dòng)障礙物發(fā)生正面碰撞，且位于無人艇的左側(cè)時(shí)，無人艇應(yīng)該采取右側(cè)繞行的避障策略，如圖11（a）所示；若移動(dòng)障礙物出現(xiàn)在無人艇左側(cè)并向右行駛時(shí)，此時(shí)無人艇應(yīng)該采取左側(cè)繞行的避障策略，如圖11（b）所示；同理，當(dāng)移動(dòng)障礙物自右向左行駛時(shí)，無人艇應(yīng)該采取右側(cè)繞行的避障策略，如圖11（c）所示；當(dāng)移動(dòng)障礙物位于無人艇的正前方，并同向行駛時(shí)，無人艇可以分別在其左右兩側(cè)繞行，如圖11（d）所示。

4 實(shí)驗(yàn)仿真

首先為了驗(yàn)證DAVSA算法在水面無人艦艇路徑規(guī)劃方面的性能，對(duì)二維水面環(huán)境進(jìn)行仿真建模，采用柵格地圖構(gòu)建仿真環(huán)境地圖，對(duì)傳統(tǒng)A-Star算法、文獻(xiàn)[21]的IDWA算法和DAVSA算法進(jìn)行了對(duì)比分析。其次為了驗(yàn)證動(dòng)態(tài)避障策略在真實(shí)海域的有效性，選取真實(shí)海域的電子海圖并將其作柵格化處理，分別在簡(jiǎn)單海域和復(fù)雜海域的柵格地圖中進(jìn)行4種避障策略的驗(yàn)證。

仿真實(shí)驗(yàn)的軟硬件配置見表1。

本文利用柵格地圖構(gòu)建二維水面仿真環(huán)境，USV的起點(diǎn)坐標(biāo)為（1，1），終點(diǎn)坐標(biāo)為（19，19）。USV的運(yùn)動(dòng)學(xué)模型參數(shù)見表2。

4.1 全局路徑規(guī)劃實(shí)驗(yàn)驗(yàn)證

在全局路徑規(guī)劃過程中開展算法對(duì)比實(shí)驗(yàn)。對(duì)比算法分別為傳統(tǒng)A-Star算法、IDWA算法、引入可變步長(zhǎng)的A-Star算法與DAVSA算法，對(duì)比指標(biāo)有尋路時(shí)間、擴(kuò)展節(jié)點(diǎn)個(gè)數(shù)和路徑長(zhǎng)度等，實(shí)驗(yàn)結(jié)果如圖12～圖15所示。

圖13中淺灰節(jié)點(diǎn)代表IDWA算法在尋路過程中的擴(kuò)展節(jié)點(diǎn)，對(duì)比圖12的傳統(tǒng)A-Star算法，二者都能找到一條全局最優(yōu)路徑。但是在時(shí)間方面，IDWA算法相對(duì)于傳統(tǒng)A-Star算法有所縮短，擴(kuò)展節(jié)點(diǎn)個(gè)數(shù)也相對(duì)減少。圖14所示為引入可變步長(zhǎng)之后的A-Star算法實(shí)驗(yàn)結(jié)果，其中淺灰色節(jié)點(diǎn)為路徑上的關(guān)鍵跳點(diǎn)，深灰色為備選跳點(diǎn)。相比于IDWA算法，該算法的尋路時(shí)間明顯縮短，擴(kuò)展節(jié)點(diǎn)個(gè)數(shù)也相對(duì)減少。圖15表示DAVSA算法的最終路徑，相對(duì)于引入可變步長(zhǎng)的A-Star算法和IDWA算法，路徑長(zhǎng)度進(jìn)一步縮短，擴(kuò)展節(jié)點(diǎn)個(gè)數(shù)也明顯減少，具體見表3。

通過表3可以看出，擴(kuò)展節(jié)點(diǎn)的減少使得算法尋路時(shí)間縮短，雖然相較于可變步長(zhǎng)的A-Star，DAVSA的節(jié)點(diǎn)進(jìn)一步減少，但由于路徑二次優(yōu)化消耗了一定的時(shí)間，因此總體尋路時(shí)間略有增加。

4.2 簡(jiǎn)單海域地圖下的動(dòng)態(tài)避障對(duì)比實(shí)驗(yàn)

加入動(dòng)態(tài)障礙物后，要求其實(shí)時(shí)規(guī)劃的路徑應(yīng)避開移動(dòng)的障礙物，因此需要采用DWA算法的思想來避免移動(dòng)障礙物對(duì)無人艇的影響。在規(guī)劃路徑時(shí)，首先考慮到無人艇當(dāng)前的狀態(tài)以及運(yùn)動(dòng)能力，選擇合適的線速度和角速度。接著，考慮移動(dòng)障礙物的位置和速度，并反向應(yīng)用到無人艇的速度和方向上。這樣就可以避免無人艇撞到障礙物，同時(shí)規(guī)劃出一條安全通暢的路徑。在實(shí)際應(yīng)用中，可以根據(jù)實(shí)時(shí)的障礙物信息進(jìn)行動(dòng)態(tài)調(diào)整，保證路徑的正確性和完整性。

在進(jìn)行局部路徑規(guī)劃時(shí)，選取全局路徑的關(guān)鍵點(diǎn)作為局部路徑規(guī)劃的局部引導(dǎo)點(diǎn)，以這些點(diǎn)為基礎(chǔ)，結(jié)合無人艇的當(dāng)前狀態(tài)信息，改進(jìn)局部路徑規(guī)劃算法，使其沿著全局最優(yōu)路徑向目標(biāo)點(diǎn)前進(jìn)。無人艇可以根據(jù)在當(dāng)前的局部環(huán)境中探測(cè)到的障礙物運(yùn)動(dòng)情況進(jìn)行實(shí)時(shí)避障；同時(shí)，依據(jù)全局最優(yōu)路徑的評(píng)價(jià)因子和全局路徑信息來實(shí)時(shí)規(guī)劃路徑，避免與障礙物發(fā)生碰撞。

為了驗(yàn)證無人艇動(dòng)態(tài)避障策略的有效性，本文將選取真實(shí)海域的電子海圖進(jìn)行柵格化處理，選取簡(jiǎn)單海域環(huán)境并將其柵格化為20×20的柵格地圖，圖16所示為簡(jiǎn)單海域的柵格地圖。

為了驗(yàn)證動(dòng)態(tài)避障策略的有效性，在簡(jiǎn)單海域柵格地圖中，針對(duì)航行中經(jīng)常遇到的情況，增加4種不同移動(dòng)方向的動(dòng)態(tài)障礙物，分別是正面相對(duì)行駛的障礙物、同向行駛的移動(dòng)障礙物、左側(cè)交叉行駛的障礙物和右側(cè)交叉行駛的障礙物。在20×20簡(jiǎn)單柵格環(huán)境下，將DAVSA算法與IDWA算法[21]進(jìn)行對(duì)比實(shí)驗(yàn)，測(cè)試4種USV的動(dòng)態(tài)避障策略針對(duì)不同移動(dòng)方向障礙物的效果，結(jié)果如圖17～圖20所示。

由圖17～圖20可明顯看出，DAVSA算法下的路徑均優(yōu)于IDWA算法下的路徑，尤其是在遇到自左向右和自右向左移動(dòng)的障礙物的情況下。這是因?yàn)橥ㄟ^引入動(dòng)態(tài)避障策略，可以減少無人艇與移動(dòng)障礙物的會(huì)遇時(shí)間，在縮短全局尋路綜合時(shí)間的同時(shí)也能縮短路徑長(zhǎng)度。

當(dāng)無人艇遇到正面相對(duì)行駛的移動(dòng)障礙物時(shí)，無人艇應(yīng)該向右舷改變路線，以免發(fā)生碰撞。圖17（a）為采用IDWA算法的無人艇避障路線，圖17（b）為采用DAVSA算法的無人艇避障路線。當(dāng)無人艇遇到同向行駛的移動(dòng)障礙物，且位于無人艇的右側(cè)方向時(shí)，無人艇應(yīng)向左舷改變航向。圖18（a）為采用IDWA算法的無人艇避障路線，圖18（b）

為采用DAVSA算法的無人艇避障路線。當(dāng)無人艇檢測(cè)到移動(dòng)障礙物來自無人艇的左舷，遇到左舷交叉的情況，無人艇應(yīng)向右舷改變路線，以免發(fā)生碰撞。圖19（a）為采用IDWA算法的無人艇避障路線，圖19（b）為采用DAVSA算法的無人艇避障路線。當(dāng)無人艇檢測(cè)到移動(dòng)障礙物來自無人艇的右舷，遇到右舷交叉的情況，無人艇應(yīng)向左舷改變路線，以免發(fā)生碰撞。圖20（a）表示采用IDWA算法的無人艇避障路線，圖20（b）為采用DAVSA算法的無人艇避障路線。

通過算法仿真得到表4的數(shù)據(jù)。仿真結(jié)果表明，對(duì)障礙物在4種不同移動(dòng)方向上的算法性能提升值取平均，DAVSA算法相較于IDWA算法，平均全局尋路時(shí)間降低了59.46%，綜合算法運(yùn)行時(shí)間降低了5.08%，綜合路徑長(zhǎng)度縮短了9.08%，能夠滿足動(dòng)態(tài)環(huán)境中實(shí)時(shí)避障的要求。具體計(jì)算公式如式（11）～式（13）所示：

（11）

式中：Tglobal表示全局尋路時(shí)間提升率；TglobalIDWAi表示第i個(gè)IDWA算法的全局尋路時(shí)間；TglobalDAVSAi表示第i個(gè)DAVSA算法的全局尋路時(shí)間。

（12）

式中：Tcomplete表示綜合尋路時(shí)間提升率；TcompleteIDWAi表示第i個(gè)IDWA算法的綜合尋路時(shí)間；TcompleteDAVSAi表示第i個(gè)DAVSA算法的綜合尋路時(shí)間。

（13）

式中：Lpath表示路徑長(zhǎng)度縮短率；LpathIDWAi表示第i個(gè)IDWA算法的路徑長(zhǎng)度；LpathDAVSAi表示第i個(gè)DAVSA算法的路徑長(zhǎng)度。

DAVSA算法之所以優(yōu)于IDWA算法，是因?yàn)镈AVSA針對(duì)不同來向的移動(dòng)障礙物分別對(duì)應(yīng)不同的避障策略，綜合考慮無人艇與移動(dòng)障礙物之間的相對(duì)位置、速度和障礙物距離等因素，從而保證無人艇的安全導(dǎo)航，并最大程度上接近在全局最優(yōu)路徑上行駛。由表4可知，在遇到不同來向的移動(dòng)障礙物時(shí)，DAVSA算法在全局尋路時(shí)間、綜合尋路時(shí)間以及路徑長(zhǎng)度這3個(gè)性能指標(biāo)上均優(yōu)于IDWA算法。

4.3 復(fù)雜環(huán)境下的動(dòng)態(tài)避障策略驗(yàn)證

為了進(jìn)一步驗(yàn)證無人艇在復(fù)雜海域的避障能力，本文選取復(fù)雜海域環(huán)境進(jìn)行仿真實(shí)驗(yàn)。同樣地，對(duì)復(fù)雜海域電子地圖進(jìn)行柵格化處理。圖21所示為復(fù)雜海域的柵格地圖。

在復(fù)雜海域柵格地圖中，同樣增加4種不同移動(dòng)方向的動(dòng)態(tài)障礙物，分別是正面相對(duì)行駛、同向行駛、左側(cè)交叉行駛和右側(cè)交叉行駛的移動(dòng)障礙物。在20×20的復(fù)雜柵格環(huán)境下，測(cè)試USV的動(dòng)態(tài)避障策略對(duì)于4種不同移動(dòng)方向障礙物的效果，結(jié)果如圖22～圖25所示。

由圖22～圖25可知，DAVSA算法能夠在遵守COLREGs的前提下，針對(duì)不同來向的移動(dòng)障礙物采取對(duì)應(yīng)的避障策略，自動(dòng)計(jì)算出最佳的避障路徑，避免與障礙物發(fā)生碰撞，從而有效地確保無人艇在簡(jiǎn)單和復(fù)雜的海域環(huán)境中安全航行。

對(duì)比表4、表5可知，在復(fù)雜環(huán)境下DAVSA算法的綜合尋路時(shí)間和路徑長(zhǎng)度都有所增加。這是由執(zhí)行動(dòng)態(tài)避障策略所導(dǎo)致的，無人艇需要更多的時(shí)間來調(diào)整路徑，路徑長(zhǎng)度也可能因?yàn)楸苷隙兊酶L(zhǎng)。

5 結(jié) 語(yǔ)

針對(duì)USV在全局路徑規(guī)劃中存在的生成路徑冗余且無法滿足實(shí)時(shí)性的要求，以及局部路徑規(guī)劃過程中容易陷入局部最優(yōu)等問題，提出了DAVSA的路徑規(guī)劃算法。改進(jìn)后的算法能夠快速生成一條全局最優(yōu)路徑，經(jīng)過二次優(yōu)化后擁有更短的路徑，同時(shí)具有較好的動(dòng)態(tài)避障能力，能夠有效地響應(yīng)各種場(chǎng)景下的移動(dòng)障礙物，并產(chǎn)生相應(yīng)的狀態(tài)變化。文中通過實(shí)驗(yàn)驗(yàn)證了算法的有效性。雖然該算法已經(jīng)取得了一定的成果，但還存在一些不足之處。當(dāng)前的實(shí)驗(yàn)都是在已知全局地圖信息的前提下進(jìn)行的，未來的研究方向?qū)?huì)聚焦于未知環(huán)境下的路徑規(guī)劃和實(shí)時(shí)預(yù)測(cè)，并研究適用于復(fù)雜海洋環(huán)境下的水面無人艦艇路徑規(guī)劃算法。

注：本文通訊作者為劉琦。

參考文獻(xiàn)

[1] LIU C， XIANG X， HUANG J， et al. Development of USV autonomy： architecture， implementation and sea trials [J]. Brodogradnja， 2022， 73（1）： 89-107.

[2] ZHAO L， BAI Y， WANG F， et al. Path planning for autonomous surface vessels based on improved artificial fish swarm algorithm： a further study [J]. Ships and offshore structures， 2023， 18（9）： 1325-1337.

[3] TANG X， ZHU Y， JIANG X. Improved A-star algorithm for robot path planning in static environment [J]. Journal of physics： conference series， 2021， 1792（1）： 012067.

[4] MAO S， YANG P， GAO D， et al. A motion planning method for unmanned surface vehicle based on improved RRT algorithm [J]. Journal of marine science and engineering， 2023， 11（4）： 687.

[5] LIU C， LIU A， WANG R， et al. Path planning algorithm for multi-locomotion robot based on multi-objective genetic algorithm with elitist strategy[J]. Micromachines， 2022， 13（4）： 616.

[6]郝琨，張慧杰，李志圣，等.基于改進(jìn)避障策略和雙優(yōu)化蟻群算法的機(jī)器人路徑規(guī)劃[J].農(nóng)業(yè)機(jī)械學(xué)報(bào)，2022，53（8）：303-312.

[7] PHUNG M， HA Q. Safety-enhanced UAV path planning with spherical vector-based particle swarm optimization [J]. Applied soft computing， 2021， 107： 107376.

[8] SUNG I， CHOI B， NIELSEN P. On the training of a neural network for online path planning with offline path planning algorithms [J]. International journal of information management， 2021， 57： 102142.

[9] CHEN L， WANG Y， MIAO Z， et al. Transformer-based imitative reinforcement learning for multi-robot path planning [J]. IEEE transactions on industrial informatics， 2023， 19（10）： 10233-10243.

[10] YAO M， DENG H， FENG X， et al. Improved dynamic windows approach based on energy consumption management and fuzzy logic control for local path planning of mobile robots [J]. Computers amp; industrial engineering， 2024， 187： 109767.

[11] HE Z， CHU X， LIU C， et al. A novel model predictive artificial potential field based ship motion planning method considering COLREGs for complex encounter scenarios [J]. ISA transactions， 2023， 134： 58-73.

[12] DAI W， MA X. Improvement of collision detection using time elastic band algorithm [C]//2021 The 9th International Conference on Information Technology： IoT and Smart City. [S.l.]： [s.n.]， 2021： 93-97.

[13] HANG P， HUANG S， CHEN X， et al. Path planning of collision avoidance for unmanned ground vehicles： a nonlinear model predictive control approach [J]. Proceedings of the institution of mechanical engineers， Part I： journal of systems and control engineering， 2021， 235（2）： 222-236.

[14] ZHOU C， HUANG B， FRANTI P. A review of motion planning algorithms for intelligent robots [J]. Journal of intelligent manufacturing， 2022， 33（2）： 387-424.

[15]周熙棟，張輝，陳波.非結(jié)構(gòu)化場(chǎng)景下基于改進(jìn)JPS算法的移動(dòng)機(jī)器人路徑規(guī)劃[J].控制與決策，2024，39（2）：474-482.

[16] XIANG D， LIN H， OUYANG J， et al. Combined improved A* and greedy algorithm for path planning of multi-objective mobile robot [J]. Scientific reports， 2022， 12（1）： 1-12.

[17] WANG N， XU H， LI C， et al. Hierarchical path planning of unmanned surface vehicles： a fuzzy artificial potential field approach [J]. International journal of fuzzy systems， 2021， 23（6）： 1797-1808.

[18]魏立新，張鈺錕，孫浩，等. 基于改進(jìn)蟻群和 DWA 算法的機(jī)器人動(dòng)態(tài)路徑規(guī)劃[J]. 控制與決策，2022，37（9）：2211-2216.

[19]劉鈺銘，黃海松，范青松，等.基于改進(jìn)A*-DWA算法的移動(dòng)機(jī)器人路徑規(guī)劃[J].計(jì)算機(jī)集成制造系統(tǒng)，2024，30（1）：158-171.

[20] LIANG J， LIU L. Optimal path planning method for unmanned surface vehicles based on improved shark-inspired algorithm[J]. Journal of marine science and engineering， 2023， 11（7）： 1386.

[21] GUAN W， WANG K. Autonomous collision avoidance of unmanned surface vehicles based on improved a-star and dynamic window approach algorithms[J]. IEEE intelligent transportation systems magazine， 2023， 15（3）： 36-50.