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

高 宇，趙嵩郢

應(yīng)用研究

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

高 宇1，趙嵩郢2

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

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

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

0 引言

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

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

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

1 水面無人艇運(yùn)動學(xué)建模

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

式中，為無人艇艇體總質(zhì)量。xG和zG分別是無人艇重心在軸和軸上的坐標(biāo)，Ixx和Izz分別是其相對軸和軸的慣性矩，Ixx是其在平面的平面慣性積。和分別是無人艇相對軸和軸的運(yùn)動動量，和是其在軸和軸上的動量矩。

2 水面無人艇速度空間

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

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

圖2 水面無人艇速度空間

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

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

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

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

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

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

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

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

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

上式中，為距離最近障礙物與所得軌跡的距離，為無人艇與障礙物需保持的最小距離，若所得軌跡上不經(jīng)過障礙物，可將設(shè)置為一個(gè)較大的常數(shù)，從而避免其在評價(jià)中權(quán)重過大，影響評價(jià)標(biāo)準(zhǔn)。

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

其具體實(shí)現(xiàn)步驟如下：

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

3）根據(jù)公式（7）對速度空間里的所有軌跡進(jìn)行評價(jià)；

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

5）輸出軌跡，重復(fù)迭代，直至到達(dá)目標(biāo)點(diǎn)。

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

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

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

4 仿真實(shí)驗(yàn)對比與分析

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

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

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

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

5 結(jié)果分析

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

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

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

[3] 劉建. 水面無人艇路徑規(guī)劃技術(shù)的研究[D]. 江蘇科技大學(xué), 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]. 計(jì)算機(jī)科學(xué), 2011, 38(09): 211-214+219.

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

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

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

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

[14] 張洋洋, 瞿棟, 柯俊等. 基于速度障礙法和動態(tài)窗口法的無人水面艇動態(tài)避障[J]. 上海大學(xué)學(xué)報(bào)(自然科學(xué)版), 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