高 宇，趙嵩郢

應用研究

基于動態(tài)窗口法的無人艇局部路徑規(guī)劃

高 宇1，趙嵩郢2

(1. 渤海造船廠集團有限公司，葫蘆島 125000；2. 武漢船用電力推進裝置研究所，武漢 430064)

本文設計了一種基于動態(tài)窗口法的無人艇局部路徑規(guī)劃方法。首先建立無人艇運動學模型，結合艇體機動性能給出無人艇運動方程。然后根據無人艇在短時間間隔內能達到的速度設置搜索空間，進而組成動態(tài)窗口。在動態(tài)窗口中確定速度產生的安全軌跡，設置動態(tài)窗口法軌跡評價函數選出下一時刻無人艇航行的最優(yōu)軌跡，并在Matlab中進行仿真驗證。結果顯示，本文所設計基于動態(tài)窗口法的無人艇局部路徑方法可以有效結合水面無人艇運動學參數，并充分考慮避碰規(guī)則以及障礙物因子，在其趨向目標點的同時也有效躲避障礙物，提高水面無人艇的自主避障能力。

水面無人艇 局部路徑 動態(tài)窗口法

0 引言

無人水面艇(unmanned surface vessel，USV)作為一種智能化小型水面運動平臺，在海事監(jiān)管、軍事作戰(zhàn)及環(huán)境檢測等領域充當了重要角色[1]。水面無人艇具有反應快速、機動性強、自動化水平較高的特點，可以快速有效地完成指定任務[2]。路徑規(guī)劃是無人艇智能航行模塊的重要組成部分，也是無人艇繞開海面障礙物安全抵達目標點的先決條件[3]。

海洋環(huán)境復雜，根據對環(huán)境地圖的理解差異，水面無人艇的路徑規(guī)劃主要可分為兩種：基于電子海圖的全局路徑規(guī)劃，以及基于傳感器信息的局部路徑規(guī)劃[4-7]。全局路徑規(guī)劃側重于在已知全部地圖后的無人艇全局移動路徑搜索，其路徑精度與地圖的分辨率直接相關[8～10]；局部路徑規(guī)劃側重于對周圍環(huán)境的感知及障礙物避讓，重點在于培養(yǎng)水面無人艇自主避障能力[11-12]。

水面無人艇實際運行過程中，由于電子海圖范圍較大，且無人艇體型較小，所形成適航環(huán)境模型不精確，所以其航行過程中需設置局部路徑規(guī)劃來進一步修正航線，從而保證水面無人艇能夠正確避開障礙物，安全抵達目標點。水面無人艇局部路徑規(guī)劃中，主要是通過艇載傳感器探測周圍環(huán)境信息，建立局部地圖，并結合局部地圖和原來規(guī)劃的全局路徑方向進行局部的路徑規(guī)劃[13]。水面無人艇能否在行進過程正確避開動態(tài)障礙物，首先基于其運動學分析，再采用動態(tài)規(guī)劃的方式進行路徑規(guī)劃，及時有效地調整速度或方向，保證安全航行。

1 水面無人艇運動學建模

水面無人艇的運動學模型與動力學模型的建立是其運動規(guī)劃與控制的基礎。由于海況復雜，水面無人艇在航行過程中受風、浪、流影響，導致無人艇的垂蕩、橫搖和縱搖。其中，橫搖對無人艇航行安全的影響最大。由于風、浪、流等環(huán)境干擾因素在建立無人艇運動學及動力學模型過程中產生過多不確定因素，特別在對水面無人艇進行閉環(huán)控制時，無人艇本體及周圍環(huán)境信息存在測量誤差，且測量信號受隨機噪聲的影響。因此，在本文方法中，為保證所建立數學模型的準確性和控制系統(tǒng)的有效性，將水面無人艇的航行過程，近似看作一種服從于二維平面的平面運動，忽略垂蕩和縱搖等三維不定因素的影響和各運動狀態(tài)間的概合，僅考慮橫搖的影響[10]。

式中，為無人艇艇體總質量。xG和zG分別是無人艇重心在軸和軸上的坐標，Ixx和Izz分別是其相對軸和軸的慣性矩，Ixx是其在平面的平面慣性積。和分別是無人艇相對軸和軸的運動動量，和是其在軸和軸上的動量矩。

2 水面無人艇速度空間

在完成了艇體運動學建模后，水面無人艇的局部運動軌跡主要通過速度空間(,)確立。動態(tài)窗口法的搜索策略為無障礙最優(yōu)方向，通過速度空間可以快速得出下一時刻規(guī)劃軌跡的最優(yōu)解，從而將縱向與橫向兩個維度的優(yōu)化簡化成一個維度的優(yōu)化，使其在趨向目標節(jié)點的同時也能躲避障礙物。由于在速度(,)的二維空間中存在無數種速度組合，從而對應無數種運動軌跡。為控制采樣速度范圍，減少冗余計算量，通常速度空間(,)需受最大最小速度約束、動力學約束以及安全約束。

如圖2所示構建水面無人艇速度空間，橫坐標為前行角速度，縱坐標為前行線速度，故水面無人艇的最大最小速度空間V為圖示整個長方形區(qū)域。V表示在控制周期內考慮動力學性能無人艇可達的速度空間。圖中淺色區(qū)域為無人艇可安全行駛的空間V，深色區(qū)域則表示無人艇在該速度下可能與障礙物產生碰撞，白色矩形區(qū)域則為上述集合的交集V，即最終確定的采樣空間窗口，也稱為無人艇在該時刻的動態(tài)窗口。

圖2 水面無人艇速度空間

式中，min、max為無人艇前行線速度的最大值及最小值，min、max為前行叫速度的最大值及最小值。

2）動力學性能約束：由于艇體動力限制以及環(huán)境因素影響，無人艇速度空間受艇體最大加速度影響。V為水面無人艇動力學可達的速度空間：

3）碰撞安全約束：為保證無人艇在與障礙物發(fā)生碰撞前速度需降為零。V為探測器安全的速度空間：

滿足上述約束條件，則V為水面無人艇的安全速度空間，即有效動態(tài)窗口：

因間隔時間短，為便于簡化計算避免冗余，設定無人艇在每個時間周期內速度恒定。在連續(xù)的速度空間Vr中，對速度采樣點進行離散處理，以此得到無人艇在單個時間周期內每個采樣點的運動軌跡。

如圖3所示，矩形方框表征水面無人艇，途中曲線為無人艇前行可選擇的軌跡。其中，軌跡b和軌跡a為最大線加、減速度和最大角加、減速度下的極限速度軌跡，軌跡v為無人艇當前速度軌跡。

3 動態(tài)窗口法軌跡評價優(yōu)化

基于所得無人艇可行軌跡，下一步需選擇最優(yōu)軌跡并給定無人艇與其對應的速度空間()。動態(tài)窗口法的軌跡評價函數主要考慮三個方面，即所得軌跡與參考路徑的切合程度、所得軌跡與參考路徑終點的距離以及所得軌跡上是否途徑障礙物。

由此可設定動態(tài)窗口法軌跡函數為：

上式中，為距離最近障礙物與所得軌跡的距離，為無人艇與障礙物需保持的最小距離，若所得軌跡上不經過障礙物，可將設置為一個較大的常數，從而避免其在評價中權重過大，影響評價標準。

從而，所得的動態(tài)窗口法軌跡評價函數可以滿足無人艇局部路徑規(guī)劃需求，即保持較高航速下完成對于障礙物的及時規(guī)避，其算法流程如圖5所示：

其具體實現步驟如下：

1）根據艇載傳感器探測的局部環(huán)境信息構建局部柵格地圖；

3）根據公式（7）對速度空間里的所有軌跡進行評價；

4）選擇無人艇局部路徑規(guī)劃的最優(yōu)軌跡；

5）輸出軌跡，重復迭代，直至到達目標點。

圖5 動態(tài)窗口法算法流程

圖6 動態(tài)窗口法尋路過程示意

如圖6所示，在局部柵格地圖中無人艇依據動態(tài)窗口法進行局部路徑規(guī)劃。其中，黑色柵格表示不可通行障礙物；白色柵格代表無人艇可通行區(qū)域。曲線w為無人艇運行的過往軌跡曲線，黑色直線為無人艇當前航向，曲線u集合為無人艇下一時間間隔中的軌跡空間，所設置評價函數將從中選出最優(yōu)軌跡并輸出執(zhí)行。

4 仿真實驗對比與分析

為評價動態(tài)窗口法應用于無人艇局部路徑規(guī)劃場景中的應用效果，本文基于Matlab2018b搭建仿真場景，對動態(tài)窗口法應用效果進行驗證，設置的算法初始參數如下：

表1 動態(tài)窗口法參數設置

局部柵格地圖尺寸大小為10 m×10 m，水面無人艇的出發(fā)點為(0, 0)，目標點為(10, 10)。

圖7 動態(tài)窗口局部尋路仿真結果

5 結果分析

本文所設計基于動態(tài)窗口法的無人艇局部路徑方法可以有效結合水面無人艇運動學參數，并充分考慮避碰規(guī)則以及障礙物因子，在其趨向目標點的同時也有效躲避障礙物，能有效提高水面無人艇的自主避障能力。本文研究也存在不足之處，即對于動態(tài)障礙的考慮較少，后續(xù)將結合多通道的傳感器信息實現局部柵格地圖的實時更新，并基于真實環(huán)境對本文方法進行進一步的驗證及優(yōu)化。

[1] 楊文韜.世界無人水面艇發(fā)展綜述[J]. 現代軍事, 2014(10): 58-60.

[2] 胥鳳馳, 王偉, 李哲等. 無人艇系統(tǒng)的設計實現與未來展望[J]. 艦船科學技術，2019, 41(23): 39-43.

[3] 劉建. 水面無人艇路徑規(guī)劃技術的研究[D]. 江蘇科技大學, 2014.

[4] Mousazadeh H, Jafarbiglu H, Abdolmaleki H, et al. Developing a navigation,guidance and obstacle avoidance algorithm for an Unmanned Surface Vehicle (USV) by algorithms fusion[J]. Ocean Engineering, 2018, 159: 56-65. Aided Desigh, 1993,25(11): 801-811.

[5] 曲毅, 潘民子. 無人艇路徑自主規(guī)劃方法及策略研究綜述[J]. 信息通信, 2019(08): 278-280.

[6] Wang H, Wei Z. Stereovision based obstacle detection system for unmanned surface vehicle[C]. IEEE International Conference on Robotics and Biomimetics. IEEE, 2014: 917-921.

[7] Bibuli M, Singh Y, Sharma S, et al. A two layered optimal approach towards cooperative motion planning of unmanned surface vehicles in a constrained maritime Environment[J]. IFAC-Papers On Line, 2018, 51(29): 378-383.

[8] 范云生, 趙永生, 石林龍, 等. 基于電子海圖柵格化的無人水面艇全局路徑規(guī)劃[J]. 中國航海, 2017, 40(01): 47-52+113.

[9] 莊佳園, 萬磊, 廖煜雷, 孫寒冰. 基于電子海圖的水面無人艇全局路徑規(guī)劃研究[J]. 計算機科學, 2011, 38(09): 211-214+219.

[10] 饒森. 水面無人艇的全局路徑規(guī)劃技術研究[D]. 哈爾濱工程大學, 2007.

[11] 張鳳, 談大龍. 動態(tài)未知環(huán)境下基于相對坐標系的移動機器人實時運動規(guī)劃[J]. 機器人, 2004, 26(5): 434-438.

[12] 戴光明. 避障路徑規(guī)劃的算法研究[D]. 華中科技大學, 2004.

[13] 杜開君, 茅云生, 向祖權, 周永清, 宋利飛, 劉彬. 基于海事規(guī)則的水面無人艇動態(tài)障礙規(guī)避方法[J]. 船海工程, 2015, 44(03): 119-124.

[14] 張洋洋, 瞿棟, 柯俊等. 基于速度障礙法和動態(tài)窗口法的無人水面艇動態(tài)避障[J]. 上海大學學報(自然科學版), 2017, 23(01): 1-16.

Local path planning of unmanned vehicle based on dynamic window method

Gao Yu1, Zhao Songying2

(1. Bohai Shipyard Group Limited Company, Huludao 125000, Liaoning, China; 2. Wuhan Institute of Marine Electric Propulsion, Wuhan 430064, China)

TP332

A

1003-4862（2022）07-0050-05

2022-02-11

高宇（1995-），男，助理工程師。研究方向：自動化。E-mail: 15872427429@163.com