曹園山，孫 強(qiáng)

(中國(guó)船舶科學(xué)研究中心，江蘇 無(wú)錫 214082)

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

曹園山，孫 強(qiáng)

(中國(guó)船舶科學(xué)研究中心，江蘇 無(wú)錫 214082)

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

動(dòng)力定位；狀態(tài)估計(jì)；無(wú)跡卡爾曼濾波；粒子濾波

0 引 言

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

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

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

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

1.1 船舶的運(yùn)動(dòng)學(xué)模型

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

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

其中，

1.2 船舶的動(dòng)力學(xué)模型

1.2.1 船舶的低頻模型

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

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

1.2.2 船舶的高頻模型[5]

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

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

由于本次的仿真主要來(lái)驗(yàn)證該算法跟蹤效果，因此未加設(shè)定船舶所受外力為0，實(shí)際使用的船舶模型為：

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

2.1 無(wú)跡卡爾曼濾波（UKF）

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

2.2 粒子濾波

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

2.3 融合算法設(shè)計(jì)

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

設(shè) K 時(shí)刻的第 i 粒子相應(yīng)的每個(gè)粒子的權(quán)值為ωi（k），將 K 時(shí)刻的該粒子帶入到船舶當(dāng)前時(shí)刻的狀態(tài)方程中，pi（k）得到 K + 1 時(shí)刻該粒子狀態(tài)的估計(jì)為：

再對(duì)該粒子進(jìn)行不敏變換，得到其 2n + 1 個(gè) σ 采樣點(diǎn)：

對(duì)應(yīng)的權(quán)值為：

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

對(duì)粒子 pi（k）進(jìn)行 UKF 遞推。

σ 采樣點(diǎn)的一步預(yù)測(cè)：

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

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

σ 采樣點(diǎn)測(cè)量的一步預(yù)測(cè)：

粒子測(cè)量值的估計(jì)：

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

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

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

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

根據(jù)更新后的粒子狀態(tài)以及當(dāng)前時(shí)刻的實(shí)際的量測(cè)值來(lái)更新每個(gè)粒子的權(quán)值：

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

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

整個(gè)融合算法的框圖如圖 3 所示。

3 仿真結(jié)果

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

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

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

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

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

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

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

表 2 濾波效果對(duì)比Tab. 2 Filtering effect comparison

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

4 結(jié) 語(yǔ)

船舶的狀態(tài)估計(jì)在動(dòng)力定位中有著極為重要的地位，對(duì)于船舶低頻運(yùn)動(dòng)的估計(jì)的精度直接關(guān)系著動(dòng)力定位系統(tǒng)的精度和穩(wěn)定性。本文提出將粒子濾波與在船舶狀態(tài)估計(jì)中使用廣泛的 UKF 進(jìn)行融合，得到一種融合算法。為了驗(yàn)證算法效果，本文將融合算法和僅用 UKF 算法進(jìn)行了仿真對(duì)比試驗(yàn)。Matlab 仿真結(jié)果顯示，該融合算法在濾波效果上總體優(yōu)于單獨(dú)使用 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]王元慧. 模型預(yù)測(cè)控制在動(dòng)力定位系統(tǒng)中的應(yīng)用[D]. 哈爾濱:哈爾濱工程大學(xué), 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

國(guó)家科技支撐計(jì)劃資助項(xiàng)目（2014BAB13B01）

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