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

董大瓊 崔恒彬 康曉磊 高 立

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

1 引言

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

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

2 UKF濾波算法及運動模型現(xiàn)狀

2.1 UKF算法［4］

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

2.2 舵角與航向、橫漂模型

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

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

2.3 艦船濾波模型

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

3 現(xiàn)狀分析及改進

3.1 UKF算法的改進［6］

UKF經(jīng)過一段時間濾波后，增益陣K達到最小值，若系統(tǒng)狀態(tài)發(fā)生突變，則濾波結(jié)果惡化，針對本文中的模型對UKF進行以下改進。

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

根據(jù)不同量測向量定義臨界值：σ

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

以上次E（k)＞σ調(diào)整為基礎(chǔ)，當E（k)＜σ時P（k)＝P（k)/E（k)。

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

濾波模型如下所示：

狀態(tài)向量：緯度 經(jīng)度 航向 航向加速度 航向二階加速度 航速 北向海流速度 東向海流速度

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

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

圖1 濾波器結(jié)構(gòu)圖

航向濾波器狀態(tài)方程：

緯度濾波器狀態(tài)方程：

經(jīng)度濾波器狀態(tài)方程：

經(jīng)緯度濾波器觀測向量分別為：經(jīng)度、航速、緯度航速。

3.3 濾波方案設(shè)計

北斗二代無源導(dǎo)航系統(tǒng)已經(jīng)提供相關(guān)服務(wù)且精度與GPS相當，羅蘭C導(dǎo)航系統(tǒng)的定位精度將隨著ASF修正數(shù)據(jù)庫的逐步建立而逐步提高，方案設(shè)計如圖2所示。

圖2 方案設(shè)計圖

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

4 濾波方案仿真實驗

實驗分直線航行和直線＋機動航行兩種情況，初始數(shù)據(jù)：Δφ＝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］)。

經(jīng)緯度濾波器：τ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濾波算法采樣點采用比例修正采樣策略，權(quán)值分配模塊GPS、BD、羅蘭C的設(shè)置分別為：0.5、0.5、0。

4.1 直線航行

以數(shù)據(jù)的初始設(shè)置為基礎(chǔ)，船舶不操舵進行直線航行222s，航向、航速、經(jīng)緯度濾波的結(jié)果如圖3～圖5所示。

圖3 航向誤差濾波結(jié)果

圖4 緯度誤差濾波結(jié)果

圖5 經(jīng)度誤差濾波結(jié)果

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

4.2 機動航行

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

圖6 航向誤差濾波結(jié)果

圖7 緯度誤差濾波結(jié)果

圖8 經(jīng)度誤差濾波結(jié)果

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

5 結(jié)語

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

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

［2］劉星.UKF和EKF在衛(wèi)星姿態(tài)確定中的應(yīng)用研究［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］賈欣樂，楊鹽生.船舶運動數(shù)學模型［M］.大連：大連海事大學出版社，1999：238250.

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

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

［8］李文魁，周永余.艦艇綜合導(dǎo)航系統(tǒng)［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數(shù)據(jù)庫提高長二系統(tǒng)在臺灣海峽的定位精度［J］.導(dǎo)航，2001，4：106110.

［11］高立，李文魁，陳永冰.羅蘭C TOAASF測量系統(tǒng)設(shè)計與實現(xiàn)［J］.船電技術(shù)，2010，30（8)：2427.