基于感知信息的USV 目標環繞跟蹤方法

宋吉廣 ,李德隆 * ,馮 亮 ,劉 陽 ,林 揚 ,孫鐵銘

(1.中國科學院沈陽自動化研究所 機器人學國家重點實驗室,遼寧 沈陽,110016;2.中國科學院 機器人與智能制造創新研究院,遼寧 沈陽,110169)

0 引言

目前,無人系統已經成為未來軍事和民用領域發展的重點。近20 年來,無人水面艇(unmanned surface vehicle,USV)技術成為國內外研究熱點,在反水雷、偵查、監控、電子戰以及反偵察作戰中具有很大的優勢,保證了無人員傷亡前提下完成作戰任務,同時在民用領域可執行探測資源、搜救、氣象監測和預報、信號中繼站等任務,因此深入研究USV 技術,對于提升我國國防實力具有重要的意義[1-2]。

目標跟蹤是USV 研究領域的一項關鍵技術,USV 目標跟蹤方法的研究,對于海底礦物探測、港口防御和目標打擊等領域有著廣泛的應用前景。USV 作為探知海洋豐富資源的一種智能多功能探測載體,需能夠在無人狀態下自主完成規劃任務。目前,在USV 目標跟蹤技術領域,可實現對鎖定目標進行基于感知信息下的目標跟蹤任務,Qiao 等[3]使用基于平方根Sage-Husa 自適應魯棒卡爾曼濾波方法進行USV 雷達目標跟蹤,在對測量和觀測噪聲進行聯合估計后,從仿真和試驗中驗證了該算法的有效性;饒六中等[4]使用基于單目視覺對水面艇目標進行跟蹤,具體方式是通過比較運動過程中標志物圖像與期望圖像的位置關系,實時計算出偏航角和偏航距離作為控制器輸入量,并控制跟蹤船航向和航速,從而實現對被跟蹤船的跟蹤;秦世洋等[5]使用模型預測控制(model predictive control,MPC)的方法,將環境干擾等問題轉換為二次規劃問題,可實現穩定的目標點跟蹤。但在實際工程發展中,存在一些對目標信息采集更加全面具體的任務,如對于突發事故的船只,人工無法靠近或抵達,不能獲得其最新發展動態并及時處理,當前技術狀態下USV 處于相對目標的單一方位而無法滿足該需求。因此,文中提出了一種基于USV 平臺、搭載雷達載荷、通過實時感知目標信息、基于虛擬目標點跟蹤算法的目標環繞跟蹤方法,對目標進行環繞跟蹤,完成對目標的360°全方位監視,通過此種方法能夠快速、高效且安全地完成任務,提高工作效率和安全性。

使用“勇士號”USV 作為算法驗證載體,如圖1 所示。“勇士號”USV 是由中國科學院沈陽自動化研究所聯合有關合作單位研制的新型多模式操控高速USV。

圖1 “勇士號”USV 實航效果圖Fig.1 Actual navigation effect of the Warrior USV

通過建立的仿真平臺進行理論驗證,使其具備良好的自主環繞跟蹤能力,從而完成設定的任務,實航試驗驗證了文中所提方法的有效性。

1 基于無跡卡爾曼濾波器的目標運動狀態估計方法

采用無跡卡爾曼濾波(unscented Kalman filter,UKF)對描述系統狀態的均值和方差的采樣點進行非線性變換,得到的點分布近似真實值和方差值于2 階以上,同時UKF 不需要采用雅克比矩陣對狀態方程和量測方程線性化,對于非線性系統,UKF 的濾波效果優于擴展卡爾曼濾波(extended kalman filter,EKF)。同時,由于濾波對象為感知信息獲得,其數據分布具有非線性特點,故選用UKF 方法對目標運動狀態進行估計[6-7]。

無跡變換(unscented transform,UT)方法中,假設非線性變換y=f(x),向量x是n維隨機變量,設其均值為、方差為Dx,y的特性統計計算使用UT 得到的 2N+1個 χi點和對應的權值Wi。將向量x的取值范圍近似為高斯分布,并假設變化點yi=f(χi)近似服從高斯分布,再通過取權值和求加權方差獲得期望所求[8-9]。

點集相對均值為對稱分布,同時對稱點加權值相同,故其樣本均值和方差與原始均值和方差相等。使用UKF 的目標運動狀態估計,其計算方法如下。

1) 隨機變量均值及方差計算

2) 時間更新(預測)

構建 χi點權值,共有2N+1個,過程如下:

式中:W為χ權值,且權值之和為1;為均值使用權值;為方差使用權值;η為微調參數,為(N+η)Dx平方根的第i行或第j列向量,則預測點 χi、均值和方差計算如下:

3) 量測更新

新測量值z(k)獲取后,狀態均值和方差更新如下:

2 目標環繞跟蹤方法原理

基于艇雷達感知目標信息,包括目標的中心位置、航向、運動方向和目標標記ID 號,目標環繞跟蹤模塊對指定ID 號目標進行環繞跟蹤。

依托于水平面航向控制器[10],采用虛擬目標點跟蹤算法實現目標環繞跟蹤。根據運動目標航向、速度信息,自適應調整USV 環繞半徑及運動速度,充分考慮USV 本身運動特性,確定組成環繞一圈所需的虛擬目標點數量,以目標位置為原點,設定運動目標航向為Y軸正方向,根據右手定則建立運動目標坐標系,計算各點在運動坐標系中的位置。航行器在運動過程中實時計算到達虛擬目標點所需的航向角,通過水平面航向控制器完成航向閉環,航行器依次跟蹤每個虛擬目標點,以重規劃策略保障,進而實現目標環繞跟蹤[11-12]。算法原理如圖2 所示,算法總體框圖如圖3 所示。

圖2 虛擬目標點跟蹤算法原理Fig.2 Principle of virtual target point tracking algorithm

圖3 算法總體框架圖Fig.3 Overall framework of algorithm

圖2 中:XEOYE為大地坐標系;Xs1OYs1和XsO1Ys為不同時刻下USV 運動坐標系;XA1PAYA1和XA2PAYA2為不同時刻的目標運動坐標系。

1) 虛擬目標點劃分

首先根據經驗擬合設計USV 的環繞速度,并自主解算環繞半徑,即

式中: ε為比例系數;va為量測目標速度;v為航行器速度值;floor(·)為取數字整數函數;η1為3 階速度參數;η2為2 階速度參數;η3為速度變化率參數;η4為偏移參數;R為環繞半徑。

根據輸入速度及半徑計算虛擬目標點數量

式中:λ1為比例系數;λ2為速度參數;λ3為偏移參數。然后將圓等分形成虛擬目標點集P。

2) 首虛擬目標點選取及環繞方向選擇

獲得虛擬目標點點數后,根據當前位置PVe對首虛擬目標點進行選取,并確定環繞方向,當PVe在小于設定半徑處時,如圖4 所示,以中心點為原點建立運動坐標系,設航行器在運動坐標系下坐標點為(x,y),當前目標航向角為θa,當前無人航行器航向角為θ,令H為環繞方向決斷參數,有

圖4 首虛擬目標點求取示意圖Fig.4 Solution of the first virtual target point

式中:deal(·)為對括號內的取值規范化(在 [0,2π]區間);H≥0時,令環繞方向為順時針,否則為逆時針;當PVe在圓外時,以航行器位置為原點,若航行器的航向與Y軸的正方向夾角小于90°,令環繞方向為順時針,否則為逆時針。

當PVe在圓內時,如圖4(a)所示,設首虛擬目標點對應點集合序號為a,即對應點為P[a],則

式中: ψad為由PVe、量測目標位置P0和虛擬目標點位置Pa所形成的 ?P0PVePa中 ∠PaP0PVe值。PVe在圓外時,如圖4(b)所示,依據環繞方向確定切點位置PA1,在虛擬目標點序列中距離PA1最近的點則為首虛擬目標點,然后將首虛擬目標點向外延長PA1PFi至PFi點,以保證USV 跟蹤距離的穩定收斂至設定距離。

3) 坐標變換

將所有虛擬目標點在運動坐標系下的坐標轉換為在大地坐標系下的坐標,即設轉換矩陣為R,虛擬目標點序列在大地坐標系下的虛擬目標點序列為Pan、航向為φ,則有

式中: θa為目標航向角;θ為大地坐標系下USV 航向角。

4) 虛擬目標點切換判據

虛擬目標點切換判據是是否改變USV 當前所跟蹤的虛擬目標點的判別依據,其切換原理如圖5 所示。

圖5 動態虛擬目標點切換判據原理圖Fig.5 Principle of dynamic virtual target point swiching criterion

其切換條件為

式中:x和y分別為在虛擬目標坐標系下航行器位置的橫、縱坐標;x0為X軸偏移參數;y0為Y軸偏移參數。

5) 重規劃策略

重規劃主要應對海面復雜環境及傳感器誤差帶來的干擾,是一個容錯過程。當實際航跡偏離理論計算時,結合USV 性能,進行當前USV 的最新未跟蹤以及接下來的虛擬目標點位置重規劃,即最近虛擬目標點距離目標進行最大距離的拉伸或縮短,然后隨著USV 運動位置變化逐漸收斂至初始規劃虛擬目標點位置,以最快的響應時間完成收斂進而修正航跡,實現良好的環繞跟蹤[13-14]。

3 驗證情況

基于USV 模型辨識方法[15]構建艇模型,并搭建USV 仿真平臺進行仿真實驗[16],設目標航速1.3 m/s,USV 自主規劃環繞半徑90 m,環繞速度18 kn,USV 依據輸入信息進行虛擬目標點劃分,并根據當前自身以及目標信息進行坐標轉換,將虛擬目標點轉換至大地坐標系下坐標,根據初始位置和環繞方向進行首虛擬目標跟蹤點進行選擇,再實時檢測是否到達虛擬目標點切換判據,實現對所有虛擬目標點的跟蹤,在此過程中,檢測是否達到重規劃判據,設定重規劃觸發條件為偏離設計虛擬目標點距離占比環繞半徑超10%,以保證軌跡的快速收斂。算法輸出結果通過航行控制器對執行機構進行實時控制,進而實現對目標的實時環繞跟蹤,其仿真結果如圖6 和圖7 所示。

圖6 目標環繞跟蹤仿真Fig.6 Target surround tracking simulation

圖7 目標環繞跟蹤姿態信息Fig.7 Target surround tracking attitude information

仿真結果表明,該過程中未觸發重規劃功能,此過程基于虛擬目標點跟蹤算法的目標環繞跟蹤方法能夠在虛擬目標情況下對目標進行穩定的環繞跟蹤,表明算法的理論正確性。

2019 年在大連開展的海上試驗中,基于“勇士號”USV 平臺實時感知信息,對動態目標進行目標環繞跟蹤。根據算法設計,自主規劃環繞半徑為150 m,環繞速度為18 kn,設定重規劃觸發條件為偏離設計虛擬目標點距離占比環繞半徑超10%,對目標進行實時的環繞跟蹤,針對感知設備量測目標航向、速度數據波動較大對目標環繞跟蹤效果有較大影響問題,使用基于UKF 的目標運動狀態估計對雷達感知信息輸入進行處理,如圖8 和圖9 所示,分別為對航向和速度進行濾波和狀態估計處理。目標環繞試驗結果如圖10 和圖11 所示。

圖8 航向濾波處理結果Fig.8 Course filtering processing result

圖9 速度濾波處理結果Fig.9 Velocity filtering processing result

圖10 目標環繞跟蹤實航軌跡Fig.10 Actual flight of target surround tracking

圖11 目標環繞跟蹤姿態信息Fig.11 Target surround tracking attitude information

實航試驗結果表明,在“勇士號”USV 平臺下,基于感知信息的目標環繞跟蹤方法,能夠實現對未知狀態的運動目標進行自主規劃的目標環繞跟蹤,并能夠適應3 級海況,試驗過程中,觸發了重規劃功能,如圖12 所示。經過重規劃功能調用,運動軌跡快速收斂至可接受范圍內并保持,說明了該算法具備良好的抗干擾能力和穩定性,證明了算法設計的合理性與實用性。

圖12 USV 和目標間距離與環繞半徑差值Fig.12 Distance and radius difference between USV and target

4 結束語

為增強USV 的自主跟蹤能力,提出了基于感知信息的目標環繞跟蹤方法,算法以“勇士號”USV 作為驗證平臺,根據設定的目標環繞跟蹤需求,結合自身航行性能,設計相關試驗。該算法可對未知運動狀態下的目標進行環繞跟蹤,在對感知信息進行運動狀態估計后,結合重規劃策略,使得USV 在復雜海況下具備良好的目標環繞跟蹤能力,在仿真平臺下及實航效果均達到了設計需求,充分驗證了算法設計的合理性、穩定性及實用性。該方法可有效持續地對鎖定目標進行環繞跟蹤,通過搭載其他探測載荷,可高效地對目標進行特征采集等任務,但通過試驗結果發現,使用雷達感知信息作為USV 目標環繞跟蹤算法輸入,對雷達輸入信息進行一系列處理誤差仍較大,對目標環繞跟蹤精度造成一定的影響,因此在未來研究中,將增加USV 載荷如光電、紅外、測距儀等與雷達數據進行融合處理,以獲得更為精確的信息輸入,進而提高目標環繞跟蹤的穩定性和精確性。