基于船舶操控模型的UKF濾波算法研究＊

董大瓊 崔恒彬 康曉磊 高 立

（海軍蚌埠士官學校航海系 蚌埠 233012)

1 引言

船舶運動建模時基于舵角反饋信息的濾波模型結合EKF濾波算法能較好地得到艦船在直線航行或轉彎時的航向精度［1］，但經緯度的濾波效果不明顯。在濾波定位的算法中，EKF/KF濾波算法存在需要計算雅各比矩陣、濾波穩定性不足、計算復雜等缺點，相比之下UKF濾波算法更具優勢［2～3］。

本文以船舶操控模型為基礎，提出了北斗二代＋GPS聯合定位濾波，以羅蘭C＋ASF修正為備用定位的方案，將整個濾波器最優劃分為若干個子濾波器，提高了系統的健壯性；引入更高精度的UKF濾波算法，且對算法進行了改進，使船舶在機動轉彎時濾波器仍給出高精度的航向、航速和定位數據，通過仿真實驗證明了方案的可行性、有效性。

2 UKF濾波算法及運動模型現狀

2.1 UKF算法［4］

給定無跡卡爾曼濾波算法的濾波初值，計算系統初始sigma點以及權值，按照下述方程進行濾波運算：

2.2 舵角與航向、橫漂模型

舵角與航向關系模型即航向H與舵角β模型，舵角、航向、橫漂模型如下［5］：

H、β、υ分別為航向、舵角、橫漂，值系數TiKi為流體動力參數，計算方法參看相關文獻［1］。當操舵幅度不大時（β≤15°)，可以認為＝0。

2.3 艦船濾波模型

目前艦船運動建模時以緯度經度航向航向加速度航速 北向海流速度 東向海流速度為狀態向量，以經緯度，航向、航速為觀測量，上述狀態方程中－1/τVLN、－1/τVLE分別北向海流相關時間、東向海流相關時間，W1（t)、W2（t)、W3（t)、W4（t)為服從高斯分布的零均值白噪聲。

3 現狀分析及改進

3.1 UKF算法的改進［6］

UKF經過一段時間濾波后，增益陣K達到最小值，若系統狀態發生突變，則濾波結果惡化，針對本文中的模型對UKF進行以下改進。

定義檢測量［7］：E（k)＝｜Z（k)－（k)｜

根據不同量測向量定義臨界值：σ

當 E（k)＞σ時P（k)＝P（k)＊E（k)

以上次E（k)＞σ調整為基礎，當E（k)＜σ時P（k)＝P（k)/E（k)。

3.2 船舶控制濾波模型［8］

濾波模型如下所示：

狀態向量：緯度 經度 航向 航向加速度 航向二階加速度 航速 北向海流速度 東向海流速度

上述狀態方程中VPN、VPE、－1/τVLN、－1/τVLE分別為北向橫漂速度、東向橫漂速度、北向海流相關時間、東向海流相關時間，W1（t)、W2（t)、W3（t)、W4（t)為服從高斯分布的零均值白噪聲。

上述方程的狀態向量為八維，將該濾波模型整合劃分，示意圖如圖1。

圖1 濾波器結構圖

航向濾波器狀態方程：

緯度濾波器狀態方程：

經度濾波器狀態方程：

經緯度濾波器觀測向量分別為：經度、航速、緯度航速。

3.3 濾波方案設計

北斗二代無源導航系統已經提供相關服務且精度與GPS相當，羅蘭C導航系統的定位精度將隨著ASF修正數據庫的逐步建立而逐步提高，方案設計如圖2所示。

圖2 方案設計圖

系統檢測各信號源是否可信，由權值分配模塊決定各濾波器輸出數據的比重［9］，由于羅蘭C定位數據經ASF修正后，精度可進一步提高，這里作為備份使用［10］，同時該系統也可以作為ASF數據測量之用［11］。

4 濾波方案仿真實驗

實驗分直線航行和直線＋機動航行兩種情況，初始數據：Δφ＝20m、Δλ＝20m、ΔH ＝0.5°、ΔV ＝1Kn、VLN＝3.25Kn、VLE＝3.82Kn、φ0＝45°、λ0＝45°、H0＝10°、V0＝20Kn。

航向濾波器：Q＝diag（［0 0 1e－4］)、R＝0.3、P0＝diag（［0.25 1e－4 1e－4］)、x0＝diag（［10 0 0］)。

經緯度濾波器：τVLN＝τVLE＝3600s、Q＝diag（［0 1e－3 0.1］)、R＝diag（［400 1］)、P0＝diag（［400 0.5 0.8］)、x0＝diag（［45 20 3］)。

σ＝20m。UKF濾波算法采樣點采用比例修正采樣策略，權值分配模塊GPS、BD、羅蘭C的設置分別為：0.5、0.5、0。

4.1 直線航行

以數據的初始設置為基礎，船舶不操舵進行直線航行222s，航向、航速、經緯度濾波的結果如圖3～圖5所示。

圖3 航向誤差濾波結果

圖4 緯度誤差濾波結果

圖5 經度誤差濾波結果

濾波結果證明改進后的UKF濾波算法結合本文提出的船舶操控模型，在直線航行階段能顯著的提高航向、航速、經緯度的精度。

4.2 機動航行

總共航行666s。其中，在200s時，舵角打到8°，在300s時舵角恢復到0°，濾波結果如圖6～8所示。

圖6 航向誤差濾波結果

圖7 緯度誤差濾波結果

圖8 經度誤差濾波結果

濾波結果證明改進后的UKF濾波算法能及時有效的提高船舶機動轉彎時的定位精度，濾波器的整體性能較好。

5 結語

實驗證明，基于船舶操控模型結合UKF改進算法的濾波器，保證了艦船在直線航行與機動轉彎時航向、航速、經緯度的高精，證明了方案的可行性，對艦船濾波系統的設計有較高的參考價值和現實意義。

［1］董大瓊，陳永冰.基于舵角信息的航向修正和橫漂速度估計算法研究［J］.艦船電子工程，2009，29（8)：6568.

［2］劉星.UKF和EKF在衛星姿態確定中的應用研究［D］.中國科學院，2007：2831.

［3］Giannitrapani A，Ceccarelli N，Scortecci F.Comparison of EKF and UKF for Spacecraft Localization via Angle Measurements［J］.IEEE Transactions on Aerospace and Electronic Systems，2011，47（2)：7584.

［4］王康，劉莉，杜小菁，等.基于UKF的GPS定位算［J］.宇航學報，2011，32（4)：795801.

［5］賈欣樂，楊鹽生.船舶運動數學模型［M］.大連：大連海事大學出版社，1999：238250.

［6］齊立峰，陶建峰，馮新喜，等.一種改進的UKF非線性濾波器研究［J］.彈箭與制導學報，2011，31（2)：179188.

［7］胡迪，董云峰.基于自適應UKF的敏感器故障診斷算法［J］.北京航空航天大學學報，2011，37（6)：639643.

［8］李文魁，周永余.艦艇綜合導航系統［M］.武漢：海軍工程大學出版社，2004：162170.

［9］Huerta J M，Vidal J，Girmeus A.Joint Particle Filter and UKF Position Tracking in Severe NonLineofSight Situations［J］.Selected Topics in Signal Processing，IEEE Journal of，2009，5（3)：874888.

［10］周坤芳，徐志剛.建立ASF數據庫提高長二系統在臺灣海峽的定位精度［J］.導航，2001，4：106110.

［11］高立，李文魁，陳永冰.羅蘭C TOAASF測量系統設計與實現［J］.船電技術，2010，30（8)：2427.