﻿ UKF 和 PF 融合算法在动力定位船舶状态估计中的应用研究
 舰船科学技术  2017, Vol. 39 Issue (3): 49-53 PDF
UKF 和 PF 融合算法在动力定位船舶状态估计中的应用研究

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
Abstract: 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.
Key words: dynamic position     state estimation     unscented kalman filter     particle filter
0 引　言

 图 1 融合算法整体框图 Fig. 1 Whole frame diagram of fusion algorithm
1 船舶系统的数学模型

1.1 船舶的运动学模型

 图 2 地球坐标系与随船坐标系 Fig. 2 Earth coordinate system and ship coordinate system

 $\dot \eta = R(\psi )v\text{，}$ (1)

 ${ R}(\psi ) = \left[ {\begin{array}{*{20}{c}}{\cos \psi } & { - \sin \psi } & 0\\{\sin \psi } & {\cos \psi } & 0\\0 & 0 & 1\end{array}} \right]\text{。}$ (2)
1.2 船舶的动力学模型 1.2.1 船舶的低频模型

 ${ M}\dot v + { D}v = \tau \text{。}$ (3)

1.2.2 船舶的高频模型[5]

 $\left\{ \!\!\!{\begin{array}{*{20}{l}}{{{\dot \xi }_h} = {{ A}_h}{\xi _h} + {{ E}_h}{\omega _h}}\text{，}\\{{\eta _h} = {{ C}_h}{\xi _h}}\text{，}\end{array}} \right.$ (4)

 \begin{aligned}{A_{21}} = & - {\rm diag}\{ \omega _{01}^2,\omega _{02}^2,\omega _{03}^2\} \text{，}\\[3pt]{A_{22}} = & - {\rm diag}\{ 2{\xi _1}{\omega _{01}},2{\xi _2}{\omega _{02}},2{\xi _3}{\omega _{03}}\} \text{，}\begin{array}{*{20}{c}}{}\end{array}\\[3pt]\Sigma = & {\rm diag}\{ {k_1},{k_2},{k_3}\} \text{。}\end{aligned} (5)

 $\left\{ {\begin{array}{*{20}{l}}{{{\dot \xi }_h} = {{ A}_h}{\xi _h} + {E_h}{\omega _h}}\text{，}\\[6pt]{{\eta _h} = {{ C}_h}{\xi _h}}\text{，}\\[6pt]\!\!\!\! \begin{array}{l}{{\dot \xi }_l} = {{ A}_l}{\xi _l} + {E_l}{\omega _l}\text{，}\\[6pt]\!\! {y_l} = {{ C}_l}{\xi _l}\text{，}\\[6pt]\!\! {{ A}_l} = \left[ {\begin{array}{*{20}{c}}0 & {R(\psi )}\\[6pt]\!\! 0 & { - {M^{ - 1}}D}\end{array}} \right]\text{，}\\[6pt]{{ C}_l} = [0\begin{array}{*{20}{l}}{}\end{array}I]\text{，}\end{array}\\[6pt]{y = {y_l} + {\eta _h} + \nu }\text{。}\end{array}} \right.$ (6)
2 UKF 和 PF 的船舶状态估计融合算法 2.1 无迹卡尔曼滤波（UKF）

2.2 粒子滤波

2.3 融合算法设计

K 时刻的第i 粒子相应的每个粒子的权值为ωi k），将K 时刻的该粒子带入到船舶当前时刻的状态方程中，pi k）得到K + 1 时刻该粒子状态的估计为：

 $\begin{array}{l}{p_i}(k + 1|k) = {A_l}{p_i}(k) + {E_l}{\omega _l}\text{，}\\[3pt]{y_{{p_i}}}(k + 1) = {C_l} \cdot {p_i}(k + 1|k) + {y_h} + v\text{，}\end{array}$ (7)

 ${P}_k^i = [{p_i}(k) {p_i}(k) + \sqrt {(n + \lambda ){P_{k - 1}}} {p_i}(k) - \sqrt {(n + \lambda ){P_{k - 1}}} ] \text{，}$ (8)

 $W_k^i = [\lambda /(n + \lambda ) 1/2(n + \lambda ) 1/2(n + \lambda )]\text{，}$ (9)

σ 采样点的一步预测：

 ${P}_{k + 1|k}^i = {A_l}{P}_k^i + {E_l}{\omega _l}$\text{，} (10)

 ${\bar p_i}(k + 1|k) = \sum\limits_{j = 1}^{2n + 1} {{W_{j,}}_k^i{{P}_{j,}}_k^i} \text{，}$ (11)

 ${P_p} \!\!= \!\!\!{\sum\limits_{j = 1}^{2n + 1} {{W_{j,}}_k^i[{{P}_{j,}}_k^i \!-\! {{\bar p}_i}(k \!+\! 1|k)][{{P}_{j,}}_k^i \!-\! {{\bar p}_i}(k \!+\! 1|k)]} ^{\rm T}} \!\!+\! Q\text{，}$ (12)

σ 采样点测量的一步预测：

 ${Y_{{\rm P}_k^i}} = {C_l} \cdot {P}_k^i(k + 1|k) + {y_h} + v \text{，}$ (13)

 ${\bar y_{{p_i}}}(k + 1|k) = \sum\limits_{j = 1}^{2n + 1} {{W_{j,}}_k^i} {Y_{j,{P}_k^i}}\text{，}$ (14)

 ${P_y} \!\!=\!\!\! {\sum\limits_{j = 1}^{2n + 1} {{W_{j,}}_k^i[{Y_{j,}}_k^i \!- \!{{\bar y}_{{p_i}}}(k \!+\! 1|k)][{Y_{j,}}_k^i \!-\! {{\bar y}_{{p_i}}}(k \!+\! 1|k)]} ^{\rm T}} \!\!\!+\! R\text{，}$ (15)

 ${P_{p,y}} = {\sum\limits_{j = 1}^{2n + 1} {{W_{j,}}_k^i[{Y_{j,}}_k^i - {{\bar y}_{{p_i}}}(k + 1|k)][{{P}_{j,}}_k^i - {{\bar p}_i}(k + 1|k)]} ^{\rm T}}\text{，}$ (16)

 $K_k^i = {P_{p,y}}{P_y}^{ - 1}\text{，}$ (17)

i 个粒子的状态和协方差的更新为：

 ${p_i}(k + 1) = {\bar p_i}(k + 1|k) + K_k^i({y_{{p_i}}}(k + 1) - {\bar y_{{p_i}}}(k + 1|k))\text{，}$ (18)
 ${P_{k + 1}} = {P_p} - K_k^i{P_y}K_k^{iT}\text{。}$ (19)

 ${\omega _i}\left( {k + 1} \right),\left( {i = 1,2,3, \ldots ,N} \right)\text{，}$ (20)

 $x(k + 1) = \sum\limits_{i = 1}^N {{p_i}} (k + 1){\omega _i}(k + 1)\text{。}$ (21)

 图 3 融合算法流程 Fig. 3 Fusion algorithm flow
3 仿真结果

 $\begin{array}{l}{ M} = \left[ {\begin{array}{*{20}{c}}{1.1274} & 0 & 0 \\0 & {1.8902} & { - 0.0744}\\0 & { - 0.0744} & {0.1278}\end{array}} \right],\\[20pt]{ D} = \left[ {\begin{array}{*{20}{c}}{0.0358} & 0 & 0\\0 & {0.1183} & { - 0.0124}\\0 & { - 0.0041} & {0.0308}\end{array}} \right]\text{。}\end{array}$ (22)

 $\left\{ \!\!\!\!{\begin{array}{*{20}{l}}{\xi = \left[ {\begin{array}{*{20}{c}}{0.06} & {0.1} & {0.15}\end{array}} \right]}\text{，}\\{{\omega _o} = 2\pi \left[ {\begin{array}{*{20}{c}}1 & {1/2} & {1/3}\end{array}} \right]}\text{，}\\{k = [\begin{array}{*{20}{c}}1 & {1.2} & {0.8}\end{array}]}\text{。}\end{array}} \right.$ (23)

 图 4 船舶X 方向的位置 Fig. 4 The position of ship in X axis

 图 5 船舶X 方向位置滤波值与真实值的误差 Fig. 5 The error between estimated value and true value in X axis

 图 6 船舶Y 方向的位置 Fig. 6 The position of ship inY axis

 图 7 船舶Y 方向位置滤波值与真实值的误差 Fig. 7 The error between estimated value and true value inY axis

 图 8 船舶X 方向的位置 Fig. 8 The position of ship inX axis

 图 9 船舶X 方向位置滤波值与真实值的误差 Fig. 9 The error between estimated value and true value in X axis

 图 10 船舶Y 方向的位置 Fig. 10 The position of ship inY axis

 图 11 船舶Y 方向位置滤波值与真实值的误差 Fig. 11 The error between estimated value and true value inY axis

2 种算法的滤波效果对比如表 2 所示。对比 2 算法的滤波效果可知，融合算法在开始时由于粒子生成的随机性以及状态参数较多，在第 1 步估计时误差较大，但是随后就能立即减小误差，紧跟船舶的低频状态。而通过 UKF 得到的误差的在一个相对比较大的范围内震荡。从误差的均值和方差可以看出，在高频干扰和噪声同时存在以及 UKF 参数设置相同的条件下，融合算法的状态估计相比于只使用 UKF 精度更高，滤波的效果也比较稳定。

4 结　语

 [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] 王元慧. 模型预测控制在动力定位系统中的应用[D]. 哈尔滨: 哈尔滨工程大学, 2006.