具体实施方式

下面结合附图对本发明做进一步描述。

本发明提供了一种模型参数不确定下动力定位船非线性状态估计方法，对于模型不确定连续可微非线性系统，E-SVSF是一种有效的滤波算法，但仍存在一定局限性：一是要求系统方程是适度非线性且可微的，并且需要计算系统的Jacobi矩阵；二是受计算机字长限制，误差积累可能使协方差失去非负定性，导致滤波发散。

本发明针对现有技术的不足，提供了一种基于容积规则计算状态预测值，状态协方差预测，测量预测值和测量协方差预测值的容积平滑变结构滤波(C-SVSF)算法，新的算法无需计算系统Jacobi矩阵，既提高了E-SVSF算法估计精度，又扩展了E-SVSF算法应用范围；同时，通过在状态传递过程中使用协方差的平方根因子，来保证协方差正定性和对称性，进而得到平方根容积平滑变结构滤波(SRC-SVSF)，SRC-SVSF是具有数值稳定性的滤波算法。

一种模型参数不确定下动力定位(DP)船非线性状态估计方法，按以下步骤进行：

一、关键参数的分析与计算，包括构建DP船数学模型，基于容积规则计算状态预测值，状态协方差预测，测量预测值和测量协方差预测值。

二、基于参数设计容积平滑变结构滤波(C-SVSF)算法。新的算法无需计算系统Jacobi矩阵，既提高了E-SVSF算法估计精度，又扩展了E-SVSF算法应用范围；

三、计算在状态传递过程中使用协方差的平方根因子，来保证协方差正定性和对称性，进而得到平方根容积平滑变结构滤波(SRC-SVSF)，SRC-SVSF是具有数值稳定性的滤波算法。

首先给出DP船的模型的回顾，其次给出DP船离散时间状态模型，再次对测量方程的建立，之后容积卡尔曼滤波器的非线性系统状态估计的滤波步骤和公式，最后在状态传递过程中使用协方差的平方根因子，来保证协方差正定性和对称性，进而得到平方根容积平滑变结构滤波，下面详细介绍本发明的实施过程。

1.DP动力船模型

DP船三自由度运动在两种坐标系下的关系如下图1所示，建立北东地坐标系{n}＝(x_n,y_n,z_n)，o_n表示坐标原点，x_n，y_n和z_n分别指向北，东和地心，x_N，y_N和ψ表示北向位置，东向位置和艏向角。同时建立船体固定坐标系{b}＝(x_b,y_b,z_b)，o_b表示船舶质心或中心，x_b，y_b和z_b分别指向艏向，横向和船底，u，v和r表示纵荡速度，横荡速度和回转率。

DP船的北向位置、东向位置和艏向角可记为η＝[x_N,y_E,ψ]^T，DP船纵荡速度、横荡速度和回转率记为υ＝[u_N,v_E,r]^T。其中艏向角是低频分量与高频分量的叠加，即ψ＝ψ_l+ψ_h，ψ_l是低频分量，ψ_h是高频分量。其中符号[·]^T表示矩阵[·]的转置。

在低海况下，对于DP船的纵荡方向、横荡方向和艏摇三个自由度的运动，可建立对应的DP船三自由度运动学模型为：

其中，符号是矢量η的一阶导数，状态转换矩阵R(ψ_l+ψ_h)为：

同时，由于DP船舶不论工作于哪种模式，一般都是低速运行的，此时速度变化量与实际相比变化较小，因此，速度变化可由以下连续噪声加速模型表示：

其中，符号表示矢量η的二阶导数，ρ表示高斯随机噪声。

若用I表示单位矩阵，则无干扰下的DP船连续时间三自由度运动学模型如下所示：

DP船总运动可以看作由波频运动分量和低频运动分量组成。因而，当不考虑系统噪声干扰且在低海况下，DP船的纵荡方向、横荡方向和艏摇三个自由度的低频运动模型和波频运动模型分别如下所示。

理想情况下，当不考虑加性相关噪声，乘性噪声以及参数不确定时，DP船连续时间三自由度低频运动模型可以用式(1)以及式(5)～(7)表示：

τ＝B_uu (6)

其中，为输入量，表示n维状态空间，表示驱动器常值矩阵，τ为推进器输出的力和力矩。是风浪流等慢变环境干扰力。M是包含刚体质量和水动力附加质量的DP船舶惯性矩阵，即M＝M_RB+M_A，M_RB为广义惯性矩阵，M_A为广义附加惯性矩阵，D为线性阻尼矩阵，其表达形式可以分别定义如下所示：

其中，m为船舶质量；X_u、Y_v、Y_r、N_r为每个坐标轴方向的线性粘性阻尼；I_z为关于z_b轴的惯性矩阵；为每个坐标轴方向的附加质量；x_G为刚体重心在x轴方向的坐标。

为方便描述，这里采用了包含二阶海浪运动的海浪近似模型，即海浪在纵荡横荡艏摇三个自由度上的波频近似模型可表示为：

其中，σ_i(i＝1～3)与海浪强度有关；ζ_i(i＝1～3)为相对阻尼系数，取值为0.05～0.2之间；ω_0i(i＝1～3)是海浪谱主导频率，反映了海浪的有义波高。

则，海浪在每个坐标轴方向的连续时间状态空间描述为：

其中，是北向位置，东向位置和艏向角的波频运动分量，表示波频运动干扰，状态转移矩阵分量分别为：

幅值矩阵E_h2为：

令则动力定位船连续时间三自由度波频运动模型可重写为以下形式：

DP船的北向位置、东向位置、艏向角、纵荡速度、横荡速度以及回转率信息分别由DGPS、电罗经或IMU等传感器单元获得。

传感器测量值可以看作由波频运动分量η_h与低频运动分量η_l叠加而成，因而，当不考虑加性测量噪声干扰时，传感器观测模型可表示为以下统一形式：

y＝h_l(η_l)+h_h(η_h) (18)

η_h＝Λ_hξ₁₂ (19)

其中，y表示测量值；h_h(·)和h_l(·)分别是波频测量矩阵和低频测量矩阵；Λ_h是具有合适维数的波频运动矩阵。

2.DP动力船状态空间模型和测量方程建立

DP船三自由度离散时间状态空间模型，可以考虑以下模型参数不确定的非线性系统模型；

x_k＝f_k-1(x_k-1)+ω_k (20)

z_k＝h_k(x_k)+υ_k (21)

其中，f_k-1(·)和h_k(·)表示状态转移矩阵和测量转移矩阵，且具有不确定性；x_k表示状态矢量；z_k表示观测值；ω_k和υ_k是系统噪声，满足且q和r表示均值，Q和R表示方差。对于以上模型不确定动态系统，记系统初始状态为初始估计误差协方差为P_0|0。

基于容积规则计算状态预测值，状态协方差预测，测量预测值和测量协方差预测值的关键参数的分析与计算。具体实施步骤如下所述：

由初始值和P_0|0，则得初始容积点为：

其中，S_k-1|k-1＝chol(P_k-1|k-1)表示由Cholesky分解法得到的平方根因子，即

3.DP船非线性系统状态估计容积平滑变结构滤波

基于Cubature规则设计C-SVSF，C-SVSF算法计算过程如下：

步骤1：计算基于容积规则的状态预测和协方差预测值P_k|k-1

其中，表示状态传递容积点。

步骤2：计算上一时刻的测量估计误差e_z,k-1|k-1

首先计算观测方程的传递容积点：

那么得到测量估计值为：

则测量估计误差为：

步骤3：计算测量预测新息误差协方差和测量新息e_z,k|k-1

步骤4分别计算滤波增益K_k，估计值更新协方差更新P_k|k以及平滑边界层ψ_k+1。

基于新息和测量误差，计算增益K_k+1：

式中，参数0＜γ＜1；符号‘+’表示伪逆；diag(·)是·的对角化形式；‘|·|’是·的绝对值；符号表示Schur乘积(元素对元素相乘)；表示边界层的对角化形式。

由增益可得状态估计更新值为：

计算估计误差协方差更新值P_k+1|k+1：

基于公式(31)(32)(33)可以计算出滤波增益K_k，估计值更新协方差更新P_k|k，再计算最优边界层ψ_k+1：

最优边界层ψ_k+1可通过对协方差P_k+1|k+1求偏导数得到，即令：

可得，

其中，

则以上步骤1～步骤4即是基于Cubature规则的C-SVSF算法。

与CKF相似，C-SVSF在最优边界层计算中要求计算协方差的逆矩阵，因而协方差矩阵需要满足非负定性的要求。然而由于计算机字长限制造成的误差累计可能会使协方差变负定或出现奇异，从而使滤波算法发散。因而，为了保证C-SVSF的稳定性，通过由下三角分解的方法给出了基于平方根方法的SRC-SVSF算法。

由于协方差的非负定性，协方差可分解成以下形式 在滤波器的迭代过程中，使用状态协方差平方根因子来替代协方差，即可得SRC-SVSF算法。

4.DP船非线性系统状态估计平方根容积平滑变结构滤波

对SRC-SVSF算法具体说明，假设初始状态协方差平方根因子S_0|0和初始估计值已知，那么具体SRC-SVSF算法如下所示：

步骤1：由式(23)得状态预测Tria(·)表示矩阵“·”的下三角分解，则可得预测误差协方差的平方根因子S_k|k-1为：

其中，

则预测误差互协方差

步骤2：由式(28)～(30)计算测量估计以及测量估计误差e_z,k-1|k-1；由式(30)计算新息e_z,k|k-1；由下式(39)计算测量预测值

步骤3：计算新息协方差的平方根因子

其中，

步骤4：由式(31)～(32)计算滤波增益K_k以及估计值更新根据公式计算平滑边界层由公式计算新息协方差。

步骤5：计算协方差的平方根因子S_k|k更新：

其中，

且估计误差协方差更新为

本发明包括以下有益效果：

1、本发明C-SVSF克服了非线性系统必须连续可微的条件，且不需要计算系统的Jacobi矩阵，即保持了E-SVSF的鲁棒性，又由于容积规则的引入使得状态预测和协方差预测的精度得到了进一步提高。

2、本发明SRC-SVSF算法由于在递推过程中使用了协方差的平方根因子，因而保障了SRC-SVSF算法的数值稳定性。

3、本发明所述的C-SVSF和SRC-SVSF算法对于DP船的有效性以及存在噪声干扰时算法的鲁棒性，在DP船测量噪声协方差未知时，C-SVSF和SRC-SVSF具有相似估计精度，且高于E-SVSF，同时由于E-SVSF对噪声干扰的鲁棒性，因而精度均高于CKF。

本发明的效果可以通过以下仿真实验结果进一步说明。

在本仿真实验中，设DP船状态的初始值为x₀＝[1,2,0.1,1,1.5,1]'，状态噪声协方差为Q＝diag([10,10,10,10,10,10])，测量噪声协方差为R＝diag([20,20,20,20,20,20])。测量矩阵h_k＝I_6×6。假设状态初始估计值状态估计误差协方差初始值P_0|0＝diag([1,1,1,1,1,1])。

对DP船非线性估计系统来说，由以上分析可知，与EKF相比，CKF估计精度更高，因而这里以CKF作为一种对比算法。然后分别与E-SVSF，C-SVSF和SRC-SVSF算法进行仿真对比。

这里仍然用RMSE作为算法性能指标函数，并进行了200次蒙特卡罗状态估计仿真实验。为了验证算法对于模型参数不确定性的估计效果，这里分别通过观测噪声协方差已知和观测噪声协方差未知两种情况下的仿真实验来验证算法，仿真结果分别如下(a)和(b)所示：

(a)测量噪声协方差精确已知

仿真实验采样周期为1s，采样时间为1800s。这里假设已知测量噪声协方差的准确值，即令R＝diag([20,20,20,20,20,20])。那么，在200次蒙特卡罗状态估计仿真实验后，4种算法下的DP船舶北向位置，东向位置、艏向角、纵荡速度、横荡速度和回转率的RMSE值分别如图2和图3所示。其中，CKF算法RMSE曲线用破折线表示，E-SVSF算法RMSE值用点划线表示，C-SVSF算法RMSE值用实线表示，SRC-SVSF算法RMSE值用虚线来表示。

表1已知噪声下算法的平均RMSE

在噪声协方差已知时，CKF是最优状态估计方法，故CKF具有最小RMSE。由于状态和协方差预测引入了Jacobi矩阵，因而E-SVSF误差最大。同时容积规则的引入使C-SVSF算法精度高于E-SVSF。而SRC-SVSF由于在估计过程中使用协方差的平方根因子进行迭代，因而与C-SVSF具有相同的估计精度。

表1是状态在不同算法下平均RMSE。可知C-SVSF及其平方根形式SRC-SVSF具有相同估计误差，且都高于E-SVSF，说明了算法的有效性。

(b)测量噪声协方差未知

假设测量噪声协方差的精确值未知，即令R＝diag([500,500,1000,1000,1000,1000])。那么200次蒙特卡罗仿真实验后，DP船状态RMSE值分别如图4和图5所示。

由于在无法获得精确的测量噪声协方差时，CKF将失去最优性，因而CKF估计误差最大。而基于E-SVSF滤波方法，可以克服模型参数的不确定性，因而估计精度高于CKF算法。同时由于容积规则的引入，使得C-SVSF和SRC-SVSF的状态估计效果均优于E-SVSF。

同时表2给出了测量噪声协方差未知时，DP船状态变量在4种算法下的状态估计平均RMSE值。

表2未知噪声时算法的平均RMSE

综上可知：对于具有模型参数不确定的DP船非线性状态估计仿真实验，当测量噪声协方差已知时，C-SVSF、SRC-SVSF与CKF具有相近估计精度，且都高于E-SVSF的估计精度；当测量噪声协方差未知时，C-SVSF和SRC-SVSF具有相似估计精度，且高于E-SVSF，同时由于E-SVSF对噪声干扰的鲁棒性，因而精度均高于CKF。进一步说明本发明C-SVSF和SRC-SVSF算法的有效性以及存在噪声干扰时算法的鲁棒性。

以上所述仅为本发明的优选实施例而已，并不用于限制本发明，对于本领域的技术人员来说，本发明可以有各种更改和变化。凡在本发明的精神和原则之内，所作的任何修改、等同替换、改进等，均应包含在本发明的保护范围之内。