基于改進動態窗口法的無人船目標跟蹤運動規劃方法

馬金帥，劉蘭軍，金久才，陳明志

（1.中國海洋大學工程學院，山東 青島 266100；2.自然資源部第一海洋研究所，山東 青島 266061；3.自然資源部海洋遙測技術創新中心，山東 青島 266061）

無人船（Unmanned Surface Vehicle，USV） 是一種新型海上智能平臺，具有集成度高、運營成本低等優勢，同時具備體積小、隱蔽性好、機動性強等特點，可搭載不同載荷完成環境監測、海上維權等多種任務[1-4]。利用無人船對可疑船只進行快速跟蹤、監視，同時回傳目標船只信息，可以有效減少人員安全問題。在多淺灘、礁石和漁船的復雜環境下執行運動目標跟蹤任務，如何兼顧避障和目標跟蹤任務，是一個現實問題。針對目標跟蹤任務進行運動規劃，合理選擇有效的航向航速控制指令，是多障礙物環境下完成目標運動跟蹤任務的關鍵。

針對運動目標跟蹤問題，國內外學者開展了大量的研究，研究方向主要包括跟蹤制導律研究、路徑規劃算法研究和人工智能目標跟蹤方法研究。在跟蹤制導律研究方面，針對無障礙物環境下單目標跟蹤問題，挪威科技大學FOSSEN T I[5]系統性研究了3 種常用的無人船目標跟蹤制導律，包括純跟蹤法（Pure Pursuit，PP）、方位不變法（Constant Bearing，CB）和視線制導法（Line of Sight，LOS）。針對高速直線行駛目標船只跟蹤問題，挪威科技大學MORTEN B 等[6]采用CB 制導律，設計了新型速度控制器，實現了對目標船只的跟蹤。韓國首爾大學JONGHOEK K[7]提出了一種新穎的目標跟蹤制導律和環繞監視制導律，完成了目標跟蹤和環繞監視任務，并從理論上證明了目標跟蹤距離的收斂性。哈爾濱工程大學周鴻偉[8]提出了一種模仿跟蹤制導方法，無人船實時模擬被跟蹤目標的速度和航向，但存在著無人船和目標之間的距離無法縮短的問題。在目標跟蹤路徑規劃算法方面，?VEC P 等[9]使用蒙特卡洛采樣方法估計目標未來姿態，結合A*算法采用軌跡跟蹤方式，完成了目標跟蹤任務。ERIKSEN B[10]提出了基于航海雷達的動態窗口法，將LOS 制導律和執行器的特性融合到動態窗口法來跟蹤船只目標。自然資源部第一海洋研究所汪棟[11]針對多礁石、漁船等近海復雜環境下靜態目標點的跟蹤問題，提出了一種基于有限狀態機模型的無人船局部路徑規劃算法。在人工智能無人船目標跟蹤研究方面，海軍工程大學曹世杰等[12]提出了一種基于強化學習（Q-learning）的無人船目標跟蹤規劃方法，通過改進學習策略，為靜態場景下動靜態目標跟蹤提供了無模型解。大連海事大學鄭帥[13]采用DDPG（Deep Deterministic Policy Gradient） 深度強化學習算法訓練優化無人船駕駛決策模型，利用Gazebo 物理仿真器生成模型訓練的學習樣本，實現了無人船自主追蹤漂移遇險目標。

本文針對多障礙物環境下運動目標跟蹤問題，提出了一種適用于無人船平臺的目標跟蹤運動規劃方法，將自適應制導律、危險度評估因子引入傳統動態窗口法（Dynamic Window Algorithm，DWA）的目標函數中，同時優化了速度采樣空間，可快速完成無人船運動目標跟蹤任務，并實時規避動靜態障礙物。

1 無人船跟蹤規劃模型

1.1 無人船運動模型構建

本文運動規劃研究主要集中于目標跟蹤任務中無人船航速和航向的規劃，只考慮水平面內三自由度的運動，而忽略無人船的橫搖、縱搖和垂蕩3 個自由度。坐標系采用北東地（North East Down，NED）坐標系。無人船的運動模型表示如下。

式中，X（t）=[x（t），y（t），v（t），θ（t）]為無人船的狀態變量；[x（t），y（t）]為無人船的位置坐標；v（t）為無人船的航速；θ（t）為無人船的航向角。

1.2 障礙物膨化模型

障礙物檢測是無人船運行過程中實時避障的關鍵技術，本文假設無人船傳感器系統可以提供無人船和周圍環境中障礙物的位置和航向航速信息，控制器性能足夠好，不考慮海洋環境對無人船運動造成的影響。

為了簡化計算量，將跟蹤區域內的動靜態障礙物做外接圓處理，將無人船的半徑和傳感器不確定度作為膨化因子對障礙物進行膨化處理。對于“狹長”的障礙物，使用多個外接圓進行處理。此時忽略無人船尺寸，將無人船視為質點進行處理。

圖1 等效障礙物示意圖

2 改進動態窗口法

2.1 傳統動態窗口法

傳統動態窗口法是一種基于采樣的反應式局部路徑規劃方法，其借鑒了預測控制中滾動優化的思想，通過在可行速度、角速度空間中進行采樣，模擬機器人在一定時間內的多組運行軌跡，根據目標函數選取最優軌跡對應的速度、角速度來驅動機器人運動。

2.1.1 速度采樣空間

無人船在二維空間水平面內運動，由于無人船自身動力學限制、動態障礙物和靜態障礙物影響，需要對無人船的實際速度和角速度的選取進行約束。

約束1：無人船最高航速限制、最大角速度限制，決定了速度采樣空間動態窗口的上下限。

式中，Vs為約束1 下的無人船速度采樣空間；vmin和vmax為無人船的最小航行速度和最大航行速度；ωmin和ωmax為無人船的最小航行角速度和最大航行角速度。速度、角速度參數由無人船的自身性能決定。無人船的航速和角速度的采樣必須要在該區間內選取才是合理的采樣速度。

約束2：無人船的線加速度和角加速度約束。

式中，Vd為約束2 下的無人船速度采樣空間；va和ωa為無人船當前的速度和角速度；v˙和ω˙分別是無人船的最大線加速度和最大角加速度。

約束3：安全避障距離約束，即保證無人船在當前速度情況下進行最大加速度制動，可避免和最近的障礙物碰撞。

式中，Va為約束3 下的無人船速度采樣空間；v˙為無人船最大線加速度。

綜上，取三者的交集作為動態窗口的速度采樣空間。

2.1.2 預測軌跡和目標函數

動態窗口法通過選擇合適的向前預測時間，基于速度采樣空間中可行速度和可行角速度，生成多條預測軌跡（圖2），選取最優軌跡對應的速度和角速度作為控制量，發送給控制層。

圖2 無人船軌跡預測圖

預測軌跡的評估利用評價函數完成，通常評價函數設計如下。

式中，heading（v，ω）為制導評價函數；dist（v，ω）為避碰評價函數；velocity（v，ω）為速度評價函數；α、β和γ分別是制導函數權重、避障函數權重和速度函數權重。heading（v，ω）制導評價函數表征當前航向角與期望航向角之間的偏差。dist（v，ω）避碰評價函數表征無人船和最近的障礙物之間的距離，當無人船與障礙物之間的距離超過一定值時，該函數值設置為常數。velocity（v，ω）速度評價函數表征無人船的運行速度情況，在滿足約束條件的情況下，無人船航速越快越好。heading（v，ω），dist（v，ω）和velocity（v，ω）函數均為歸一化之后的函數。

2.2 改進動態窗口法

2.2.1 自適應目標跟蹤制導律

為解決動態窗口法在避障時兼顧目標跟蹤任務，設計了一種自適應切換制導律，實現純跟蹤（Pure Pursuit，PP） 制導和方位不變法（Constant Bearing，CB） 制導自適應切換。純跟蹤制導律簡單、直接，適用于無障礙下快速跟蹤動態目標；方位不變法制導律在避障方面更有優勢。根據障礙物環境和目標運動狀態，自適應切換跟蹤制導律，可保障避障的同時，改善跟蹤效果。自適應制導律示意圖如圖3 所示。

純跟蹤制導律屬于二點制導方式，在目標跟蹤時只考慮無人船和目標的位置信息。此時無人船的期望航向由無人船指向船只目標（圖3），期望航向設置如下。

圖3 自適應切換制導律示意圖

式中，PT為目標位置；PU為無人船的位置；kpp為系數。

將預測軌跡的航向與期望航向的差值作為評價函數的一部分，純跟蹤制導律下，headingpp（v，ω）函數設置如下。CB 制導也被稱為平行制導，船員廣泛使用CB制導規則來避免船舶之間的碰撞。CB 制導律適合在障礙環境下對目標船只進行避碰跟蹤。CB 制導律下的無人船期望航向vnd設置如下。

同理，設置方位不變法制導律headingcb（v，ω）函數如下。

綜上，構造heading（v，ω）函數如下。

式中，λ為制導律切換因子。當無人船需要進行避碰時，λ為0，否則λ設置為1。

2.2.2 采樣空間優化

動態窗口法只模擬一段時間內的軌跡，而無人船搭載的障礙物檢測傳感器包括航海雷達等可以在較遠的距離檢測到障礙物。為保證無人船的航行安全，本文采用最近會遇點（Closest Point of Approach，CPA）方法對會船時可能發生的碰撞進行檢測，剔除掉可能會發生碰撞的軌跡。最近會遇點方法主要包括DCPA（Distance to Closest Point of Approach）和TCPA（Time of Closest Point of Approach）。其中，DCPA 是指船舶會船時相互之間的最近距離，TCPA 是指駛抵最近會遇點的時間。DCPA和TCPA計算公式如下。

式中，x0和y0為無人船的位置；xt和yt為障礙物船舶的位置。若TCPA小于0，則表明無人船和障礙物在相互遠離，無碰撞風險。若TCPA大于0，則表明無人船和障礙物正在相互靠近，DCPA為兩船保持當前速度會遇時最近的距離。通過計算預測軌跡的最近會遇點，將可能產生碰撞的軌跡提前剔除。優化采樣空間后的軌跡如圖4 所示。

圖4 優化后的采樣軌跡

2.2.3 考慮碰撞危險度的目標函數構建

無人船在執行目標跟蹤任務時，需要完成避障任務。原始動態窗口法只考慮動態障礙物的位置而不考慮動態障礙物的速度，容易出現距離動態障礙 物過近的現象，對動態障礙物避障效果差。為防止出現距離動態障礙物過近的現象，不僅要考慮無人船距離障礙物的當前距離，還需要對兩船會遇時可能發生碰撞的危險進行評估。根據式（13）、式（14）計算TCPA和DCPA，可以對動態船舶障礙物碰撞危險度進行評估。

針對“久航490”無人船，設計危險度函數risk（v，ω）如下。

式中，boundary 為最大會遇距離閾值。當無人船與障礙物船只最大會遇距離超過boundary 時，risk（v，ω）函數取值為常數。當TCPA小于0 時，表明無人船和運動的障礙物船只完成了會船，兩船處于相互遠離的狀態。

根據以上，構建新的目標函數如下。

式中，heading（v，ω）為自適應制導律制導評價函數；risk（v，ω）為動態船舶碰撞危險度評估函數；δ為動態船舶碰撞危險度評估函數的權重。

3 仿真結果及分析

3.1 無人船參數和目標跟蹤流程

3.1.1 無人船參數

為驗證算法的有效性，針對自然資源部第一海洋研究所研發的“久航490”無人船[14]（圖5），利用MATLAB 2019b 平臺進行仿真實驗。根據“久航490”無人船實際動力學性能，設置無人船仿真參數見表1。

圖5 “久航490”無人船

表1 “久航490”無人船仿真參數表

3.1.2 目標跟蹤算法流程

（1）初始化無人船和障礙物狀態：無人船的狀態通常由定位系統、慣導等提供，包括無人船的位置、航向、航速等信息。障礙物的信息通常由航海雷達、AIS、激光雷達等障礙物檢測傳感器提供，本文仿真中假設動靜態障礙物都在無人船的探測范圍內，為已知信息。

（2）自適應制導律選取：根據無人船和障礙物的狀態信息，自適應選擇跟蹤制導律，從而計算期望航向。

（3）優化速度空間：根據無人船性能和當前狀態，確定動態窗口，利用最近會遇點方法優化動態窗口。

（4） 利用改進的目標函數對預測軌跡進行評價，選取最優軌跡對應的控制量作為控制指令，更新無人船和障礙物的狀態。如果無人船距離目標船只距離在50 m 以內，則視為完成目標跟蹤任務。否則，繼續執行以上步驟。

3.2 仿真驗證及結果分析

針對本文提出的改進動態窗口法進行驗證，采用原始動態窗口法與改進算法進行對比。采用靜態障礙物模擬航海雷達檢測到的礁石等障礙物，動態障礙物模擬航行的船舶。圖6 中黑色船只為運動的目標船只，紅色線代表目標船只的航行軌跡。白色雙體船代表無人船，綠色線代表無人船航行軌跡。黑色圈代表膨化處理后的靜止障礙物，藍色圈代表膨化處理后的動態船只障礙物。

圖6 原始動態窗口目標跟蹤仿真軌跡

由圖6（a）和圖6（b）分析可知，原始DWA 無人船躲避靜態障礙物Os1 和Os2 向目標船只抵近。由圖6（c）分析可知，原始DWA 在T為50 s 開始躲避動態船只Od1 時。由于原始DWA 只考慮了無人船和船只障礙物之間的距離，而沒有考慮動態船只的速度，在距離船只較近時才開始避障，距離動態船只障礙最近距離只有20 m，安全風險極大。由圖6（d）分析可知，面對動態障礙物Od2，無人船并未采取避讓行為，保持直線航行，對動態障礙物避障效果較差。由圖6（e）和圖6（f）分析可知，在T為150 s時，無人船期望航向指向目標船只，導致原始方法選擇從Os3 和Os4 障礙物之間的狹小縫隙中穿過，無人船在躲避Os4 障礙物航向變化較大，且和動態障礙物之間的距離過近。穿越狹窄空間后，無人船航向和目標船只的航向差值較大，無人船需要一定的調整時間，錯過了跟蹤可疑機動目標的時機。

由圖7（a）、圖7（b）和圖7（c）可知，改進DWA由于考慮了將來碰撞的風險，在T為30 s 時對動態船只障礙物Od1 采取避讓行為，轉向時機早，與動態障礙物Od1 保持了足夠的安全距離。由圖7（d）可知，面對動態障礙物Od2 時，無人船適當向左轉向，保持足夠的安全距離。由圖7（e）可知，面對較為密集的障礙物Os3 和Os4，無人船自適應選擇跟蹤制導律，從Os4 的右側駛過，既與兩個靜態障礙物保持了足夠的安全距離，也充分考慮了目標船只的速度和航向，未錯過目標跟蹤時機。由圖7（f）可知，無人船檢測到右側動態船只障礙物時，采取了向左轉向保持安全距離的行為，同時跟蹤目標船只，逐漸縮短和目標船只之間的距離。

圖7 改進動態窗口法目標跟蹤仿真軌跡

由圖8 分析可知，原始DWA 距離最近障礙物的平均距離為50.8 m，改進DWA 距離最近障礙物的平均距離為63.8 m，安全距離增加了25.6%。原始動態窗口法完成目標跟蹤任務用時276 s，改進方法用時184 s，運動目標跟蹤時間縮短了33.3%。改進方法在目標跟蹤快速性和安全性方面都得到了提高。

圖8 目標距離和障礙物距離對比圖

由圖9 分析可知，采用改進DWA 方法的無人船在目標跟蹤過程，航向由0 m/s 逐步增加到允許的最大值5 m/s 后，可保持最高航速完成目標跟蹤與避障任務。而原始DWA 方法由于躲避動態障礙物不及時，出現了兩次降速現象，在140 s 附近由最高速度5 m/s 降速到1 m/s，易錯過跟蹤機動船只目標的時機。

圖9 跟蹤過程航速對比圖

綜合對比復雜場景下無人船目標跟蹤的過程可知，原始DWA 未優化采樣空間，一些當前不會產生碰撞但是將來可能產生碰撞的軌跡也被考慮，導致了傳統DWA 較為“短視”，容易出現距離障礙物較近時才采取避障行為的情況。改進方法在處理動態障礙物時，通過考慮未來碰撞危險度，采用提前避障行為，不會出現航向大幅度改變的情況，自適應跟蹤律的加入讓改進方法在跟蹤過程中期望航向的選取更加合理，減小了無人船因為避障導致跟丟機動目標的可能性。

4 結 論

針對多淺灘、礁石和漁船的復雜環境下可疑船只目標運動跟蹤的問題，本文提出了一種改進無人船目標跟蹤運動規劃算法。改進DWA 充分考慮了無人船和被跟蹤目標的狀態，引入了自適應目標跟蹤制導律，優化了采樣空間和目標函數。通過對比原始DWA 與改進DWA，證明優化后的采樣空間可以使無人船提前轉向，目標跟蹤路徑更加平滑。改進方法考慮障礙物的速度信息，可有效躲避動態障礙物，在目標跟蹤安全性和快速性方面具有明顯的優勢，下一步將該算法應用于自研發的無人船系統中，以驗證算法的實用性和效果。