UKF 和 PF 融合算法在動力定位船舶狀態(tài)估計中的應用研究

曹園山，孫 強

(中國船舶科學研究中心，江蘇 無錫 214082)

UKF 和 PF 融合算法在動力定位船舶狀態(tài)估計中的應用研究

曹園山，孫 強

(中國船舶科學研究中心，江蘇 無錫 214082)

針對船舶動力定位狀態(tài)估計時使用擴展卡爾曼濾波導致模型失配而產(chǎn)生濾波精度不高甚至濾波發(fā)散的問題，設計一種融合無跡卡爾曼濾波和粒子濾波的動力定位船舶狀態(tài)估計算法。該算法以粒子濾波作為整體框架，運用無跡卡爾曼濾波對粒子狀態(tài)的每次更新進行最優(yōu)化估計，從而最優(yōu)化了每個粒子的狀態(tài)，再根據(jù)每個粒子的重要性分布，得出船舶復合運動中的低頻狀態(tài)。Matlab 仿真結(jié)果表明，該方法能夠從含有高頻和噪聲干擾的測量信息中估計出的船舶低頻運動狀態(tài)，相比于直接使用 UKF，該方法的濾波精度更高，濾波性能也比較穩(wěn)定。

動力定位；狀態(tài)估計；無跡卡爾曼濾波；粒子濾波

0 引 言

船舶動力定位是船舶依靠自身動力，在控制系統(tǒng)的作用下，抵抗外界干擾，使得船舶以某一首向到達設定的位置或者按照預定航跡運行。船舶在海面的運動是低頻運動和高頻運動的疊加，船舶的高頻運動是一種往復運動，動力定位系統(tǒng)無需對其進行控制，因此，在進行動力定位時，需要將測量信息中的高頻信息濾除，以獲得船舶的低頻信息。獲得船舶準確的低頻位置信息是船舶動力定位中的關鍵。動力定位船舶的狀態(tài)觀測器作為動力定位系統(tǒng)中分離船舶測量信息中的高頻運動和低頻運動的重要工具是動力定位系統(tǒng)研究的一大熱點。

如今的動力定位船舶上大多用卡爾曼濾波的方法獲得船舶低頻信息，由于船舶是一個非線性系統(tǒng)，一般使用擴展卡爾曼濾波 EKF[1]和無跡卡爾曼濾波UKF[2]來進行船舶低頻狀態(tài)預測，但是使用 EKF 在將非線性模型線性化的過程中忽略高階項，很容易產(chǎn)生模型失配。使用 UKF 時，需要隨著不同的噪聲狀況即時修改噪聲協(xié)方差矩陣，否則就會出現(xiàn)濾波精度不佳的狀況。由于粒子濾波[3–4]的濾波過程對于噪聲項不敏感，于是本文通過融合無跡卡爾曼濾波和粒子濾波設計了一種船舶狀態(tài)估計算法，以提高同樣參數(shù)設置下單獨使用 UKF 算法的濾波精度。該融合算法的整體框圖如圖 1 所示。

1 船舶系統(tǒng)的數(shù)學模型

對于通過算法從船舶運動的測量信息中分離出低頻信息，首先就是要對船舶這個非線性系統(tǒng)以及其高頻低頻運動進行建模。

1.1 船舶的運動學模型

動力定位控制的是船舶橫蕩、縱蕩和首搖 3 個自由度的運動，為了描述船舶的運動一般建立地球坐標系 O-XEYEZE和隨船坐標系 O-XYZ，地球坐標系用來描述船舶的位置，隨船坐標系用來描述船舶所受的水動力以及船舶的運動。由于隨船坐標系不是一個慣性系，所以在運算的時候涉及到狀態(tài)信息在這 2 個坐標系之間轉(zhuǎn)換。地球坐標系和隨船坐標系如圖 2 所示。

一般用 η =[x,y,ψ]T表示船舶在地球坐標系下的位置和首向， v=[u,v,w]T表示船體坐標系下船舶的縱蕩、橫蕩和首搖的速度，它們的轉(zhuǎn)換關系為：

其中，

1.2 船舶的動力學模型

1.2.1 船舶的低頻模型

動力定位系統(tǒng)控制的船舶一般都是處于低速運動的情況下的，因此根據(jù)文獻[5]可以得到簡化的船舶低頻運動模型：

式中：M 為船舶系統(tǒng)的慣量矩陣；D 為船舶的線性水動力矩陣；τ 為船舶所受的推進器推力和水動力的疊加；v 表示船舶的縱向速度、橫向速度和首搖速度。

1.2.2 船舶的高頻模型[5]

仿真試驗中一般使用二階振蕩器來模擬船舶 3 個自由度上的高頻運動，轉(zhuǎn)換成狀態(tài)空間形為：

式中：ω0i為諧振頻率；ξi為相對阻尼系數(shù)；k1，k2，k3都為大于0的常數(shù)，表示白噪聲的協(xié)方差。

由于本次的仿真主要來驗證該算法跟蹤效果，因此未加設定船舶所受外力為0，實際使用的船舶模型為：

2 UKF 和 PF 的船舶狀態(tài)估計融合算法

2.1 無跡卡爾曼濾波（UKF）

無跡卡爾曼濾波是 UT 變換和傳統(tǒng)卡爾曼濾波的結(jié)合，其也是一種遞推貝葉斯估計方法。無跡卡爾曼濾波是通過 UT 變換用一組確定的采樣點來逼近系統(tǒng)的后驗概率密度。對于非線性系統(tǒng)來說，通過這種方法可以略過系統(tǒng)線性化的環(huán)節(jié)，從而減少了線性化過程中的可能出現(xiàn)的模型失配問題。同時，對于高斯型的狀態(tài)向量，采用無跡卡爾曼濾波無需計算雅各比矩陣和漢森矩陣，可以直接通過采樣點得到狀態(tài)向量的概率密度均值和協(xié)方差，相比于擴展卡爾曼濾波（EKF）有著很大的優(yōu)越性。

2.2 粒子濾波

粒子濾波是基于蒙特卡羅思想，其是通過在狀態(tài)向量可能出現(xiàn)區(qū)域生成的若干隨機粒子對系統(tǒng)的概率密度函數(shù)進行近似，以實際測量值和每個粒子的狀態(tài)計算當前每個粒子的置信度，通過求均值的方法得到系統(tǒng)狀態(tài)向量的估計值，從而得到狀態(tài)向量的最小方差分布。由于其非參數(shù)化的特點，其對變量參數(shù)的非線性特性有著很強的建模能力，相比于 EKF 和 UKF，PF 的適用范圍更廣，精度也更高。

2.3 融合算法設計

算法開始，首先在設定區(qū)域內(nèi)對船舶所有可能出現(xiàn)的狀態(tài)進行隨機抽樣，得到 N 個粒子 pi（i = 1，2，3…，N）；根據(jù)各個粒子距離系統(tǒng)初始狀態(tài)的遠近計算每個粒子的初始的權值 ωi（i = 1，2，3…，N）。

設 K 時刻的第 i 粒子相應的每個粒子的權值為ωi（k），將 K 時刻的該粒子帶入到船舶當前時刻的狀態(tài)方程中，pi（k）得到 K + 1 時刻該粒子狀態(tài)的估計為：

再對該粒子進行不敏變換，得到其 2n + 1 個 σ 采樣點：

對應的權值為：

其中，n 和 λ 都為常數(shù)。

對粒子 pi（k）進行 UKF 遞推。

σ 采樣點的一步預測：

粒子狀態(tài)的估計：

粒子狀態(tài)誤差協(xié)方差的一步預測：

σ 采樣點測量的一步預測：

粒子測量值的估計：

粒子測量誤差協(xié)方差的一步預測：

粒子的量測和狀態(tài)的互協(xié)方差為：

粒子狀態(tài)的更新增益為：

第 i 個粒子的狀態(tài)和協(xié)方差的更新為：

根據(jù)更新后的粒子狀態(tài)以及當前時刻的實際的量測值來更新每個粒子的權值：

那么該時刻船舶的狀態(tài)估計值為

為了防止粒子退化，需要對粒子根據(jù)其歸一化后的權值進行重采樣，來淘汰權值比較小的粒子，復制權值大的粒子得到 N 個新的粒子。

整個融合算法的框圖如圖 3 所示。

3 仿真結(jié)果

為驗證算法的實用性，使用文獻[5]中的船舶數(shù)據(jù)，其實船模型的主要參數(shù)如表 1 所示。

其無因次質(zhì)量矩陣和阻尼矩陣分別為

表 1 實船模型參數(shù)Tab. 1 parameters of the ship

假設船舶的初始狀態(tài)為 [20 20 10 1 0.5 0.1]，船舶高頻運動的相對阻尼系數(shù) ξ 和諧振頻率 ωo以及白噪聲協(xié)方差分別為：

選擇粒子數(shù) N 為 100，采樣時間為 1 s，仿真時間為 500 s 進行算法的仿真。

融合算法仿真結(jié)果如圖4～圖7所示。

僅用 UKF 算法的仿真結(jié)果如圖 8～圖 11 所示。

表 2 濾波效果對比Tab. 2 Filtering effect comparison

2 種算法的濾波效果對比如表 2 所示。對比 2 算法的濾波效果可知，融合算法在開始時由于粒子生成的隨機性以及狀態(tài)參數(shù)較多，在第 1 步估計時誤差較大，但是隨后就能立即減小誤差，緊跟船舶的低頻狀態(tài)。而通過 UKF 得到的誤差的在一個相對比較大的范圍內(nèi)震蕩。從誤差的均值和方差可以看出，在高頻干擾和噪聲同時存在以及 UKF 參數(shù)設置相同的條件下，融合算法的狀態(tài)估計相比于只使用 UKF 精度更高，濾波的效果也比較穩(wěn)定。

4 結(jié) 語

船舶的狀態(tài)估計在動力定位中有著極為重要的地位，對于船舶低頻運動的估計的精度直接關系著動力定位系統(tǒng)的精度和穩(wěn)定性。本文提出將粒子濾波與在船舶狀態(tài)估計中使用廣泛的 UKF 進行融合，得到一種融合算法。為了驗證算法效果，本文將融合算法和僅用 UKF 算法進行了仿真對比試驗。Matlab 仿真結(jié)果顯示，該融合算法在濾波效果上總體優(yōu)于單獨使用 UKF算法，濾波性能更加穩(wěn)定。

[1]BALCHEN J G, JENSSEN N A, MATHISEN E, et al. A Dynamic positioning system based on Kalman filtering and optimal Control[J]. Modeling Identification & Control, 1980, 1(3): 135–163.

[2]SHI X, SUN X, FU M, et al. An unscented Kalman filter based wave filtering algorithm for dynamic ship positioning[C]// Automation and Logistics (ICAL), 2011 IEEE International Conference on. IEEE, 2011:399–404.

[3]DENG Xiao-long, JIN Jun, et al. A strong tracking particle filter for state estimation[C]// International Conference on Natural Computation, Icnc 2011, Shanghai, China, 26–28 July. 2011: 56–60.

[4]WEI Q, XIONG Z, LI C, et al. A robust approach for multiple vehicles tracking using layered particle filter[J]. AEU -International Journal of Electronics and Communications, 2011, 65(7): 609–618.

[5]王元慧. 模型預測控制在動力定位系統(tǒng)中的應用[D]. 哈爾濱:哈爾濱工程大學, 2006.

Application research of state estimation algorithm for dynamic positioning ship on fusion of unscented kalman filtering and particle filtering

CAO Yuan-shan, SUN Qiang
(China Ship Scientific Research Center, Wuxi 214082, China)

Considering the problem of low accuracy and instability when using extended kalman filter (EKF) in state estimation of dynamic positioning ship,a new fusion algorithm based on unscented kalman filter (UKF) and particle filter (PF) is designed.The algorithm takes PF as its overall frame,it uses UKF to find the optimal estimation of each particle state while updating the particle distribution.The state of ship in low frequency can be divided from its compound motion according to the importance factors of each particle. Simulation results show that the new algorithm can track the true state of ship in low frequency quickly from metrical information which contain high frequency signal and noise. Compared with the algorithm using UKF, this fusion algorithm has high precision and stable filtering effect.

dynamic position；state estimation；unscented kalman filter；particle filter

TP301.6

A

1672–7619(2017)03–0049–05

10.3404/j.issn.1672–7619.2017.03.010

2016–06–07；

2016–07–14

國家科技支撐計劃資助項目（2014BAB13B01）

曹園山(1992–)，男，碩士研究生，主要從事動力定位控制系統(tǒng)設計研究。