一种非线性船舶动力定位系统的自适应滤波方法

本发明属于船舶与海洋工程领域，涉及一种非线性船舶动力定位系统的自适应滤波方法。

背景技术：

1、船舶处于复杂的运动环境之中，同时受到低频和高频运动的影响，为了降低推进器的损耗，在设计动力定位系统时，要求控制器给出的推进指令尽可能的只抵消低频运动，而尽量不响应由一阶波浪力引起的高频运动。船舶动力定位系统中的滤波器的作用是滤去量测传感器中的噪声，并且将量测值中的高频和低频分开，获取系统所需要的低频运动量，另外还可以获得量测系统无法获得的速度，缓变环境力等参数。

2、卡尔曼滤波是应用较为广泛的滤波方法，但它的适用范围是线性系统，船舶动力定位系统是非线性系统。扩展卡尔曼滤波在非线性系统中应用较多，然而在对非线性系统线性化的过程中，不可避免地会引起线性化误差，滤波精度可能会大大降低，且在线性化过程中，需要计算系统方程的雅克比矩阵，该矩阵的计算有一定的难度。无迹卡尔曼滤波(unscented kalman filter,ukf)直接对系统的非线性函数的统计特性进行近似，而不是对非线性函数线性近似，且不需要计算雅克比矩阵，在一定程度上解决了扩展卡尔曼滤波存在的问题，该方法应用在很多领域。基于蒙特卡罗仿真的粒子滤波，也常用来处理非线性系统的状态估计问题，滤波精度较高但计算量较大，未能广泛应用。

3、虽然滤波方法可以估计出位置、速度、环境力等参数，但是在动力定位船舶系统中因海洋环境的多变性及不确定性，使得噪声统计特性具有不确定性。这种不确定性会给滤波精度产生极大的影响，甚至会引起滤波的发散现象。蒋帆等设计的基于指数加权的量测噪声协方差矩阵r自适应更新的ukf算法，并将其应用在船舶定位系统中，改善了由于传感器数据异常、未知环境干扰引起的估计精度下降的问题，但其只考虑了量测噪声，未考虑系统本身带来的影响，即未更新系统状态协方差矩阵从而导致误差增大(文献1)。丁浩晗等提出一种自适应ukf来应对复杂海况下船舶模型不准确引起过程噪声矩阵发生变化，进而导致估计精度下降的难题，记录特定时间段内的观测值，对其与后验估计进行偏差计算，由此设计自适应算法，实时估计过程噪声矩阵，但未考虑滤波出现发散的情况(文献2)。王帅洋等提出了基于误差序列协方差估计的自适应ukf，利用观测变量残差更新量测噪声协方差矩阵r，未考虑过程噪声的更新问题(文献3)。deng等基于残差协方差匹配法对量测噪声协方差进行自适应，进而采用自适应比例因子对过程噪声协方差进行调整，提出了一种rqaukf方法，并将其应用于船舶动力定位系统。在对过程噪声的更新中，采取的是单一的同比例调整的方法，未能考虑各个量的差异性且对初始值有很强的依赖性(文献4)。

4、[文献1]蒋帆,徐海祥,冯辉等.传感器数据异常下动力定位鲁棒状态估计方法[j].大连理工大学学报,2020,60(06):610-618.

5、[文献2]丁浩晗,冯辉,徐海祥.基于自适应无迹卡尔曼滤波的动力定位状态估计[j].大连海事大学学报,2016,42(4):7-12.

6、[文献3]王帅洋,杨宣访,胡致远等.基于改进无迹卡尔曼滤波的动力定位系统状态估计[j].船舶工程,2022,44(03):102-109.

7、[文献4]deng f,yang h l,wang lj.adaptive unscented kalman filter basedestimation and filtering for dynamic positioning with model uncertainties[j].international journal of control,automation and systems,2019,17:667-678.

8、因此，需要一种既考虑到量测噪声、过程噪声更新又有效限制滤波发散的适用于船舶定位系统的非线性自适应滤波方法。

技术实现思路

1、本发明的目的在于克服现有技术的不足，提供一种非线性船舶动力定位系统的自适应滤波方法，在无迹卡尔曼滤波的基础上，利用双遗忘因子同时估计量测噪声协方差阵和过程噪声协方差阵，在保证二者非负定的同时，有效地避免滤波发散。

2、为解决上述技术问题，本发明采用以下技术方案。

3、一种非线性船舶动力定位系统的自适应滤波方法，包括以下步骤：

4、步骤1、建立非线性船舶动力定位系统的状态空间模型；

5、步骤2、初始化：给定初始状态估计值后验协方差阵p0|0，量测噪声r0，过程噪声q0；

6、步骤3、更新量测噪声：利用预测值和量测值之间的误差计算出先验残差ek，将开窗法与指数形式相结合计算出遗忘因子b1进而计算贡献比例d1,k，使得遗忘因子的值稳定在0.95～0.99之间，保证算法的稳定性；忽略自适应sage-husa算法中更新量测噪声协方差阵中的负号项，并结合有偏估计对其进行更新，保证量测噪声协方差阵的正定性；根据协方差匹配原理，结合先验残差与理论残差的对比结果判断是否重新更新量测噪声协方差阵rk，在需要重新计算量测噪声协方差阵rk时，在原式前乘上先验残差与理论残差的比值，保证量测噪声协方差阵的值能够稳定在一定范围之间，且在一定程度上维持整个算法的稳定性；

7、步骤4、更新过程噪声：利用估计值和量测值计算出后验残差δk，与遗忘因子b1的取值方式相似，同样将开窗法与指数形式相结合计算出与后验残差有关的遗忘因子b2，进而计算出贡献比例d2,k，忽略sage-husa算法中更新过程噪声协方差阵的负号项；并结合有偏估计保证过程噪声协方差阵qk的非负性，考虑到整个系统的收敛性，添加判断条件，判断后验残差与理论残差之间的关系，根据判断结果重新更新过程噪声qk，防止因过程噪声协方差阵qk导致滤波发散。

8、所述的非线性船舶动力定位系统的空间状态模型如下：

9、

10、y＝hx+v

11、其中，x为系统状态向量，y为量测向量，τ为控制输入，ω为过程噪声，v为量测噪声；

12、式中，x＝[ξht,ηt,υt,bt]t，h＝[ch,i3×3,03×3,03×3]；其中，

13、ch＝[03*3i3*3]，

14、a22＝-diag{2ξ1ωo1,2ξ2ωo2,2ξ3ωo3}，ω01,ω02,ω03为波浪pm谱中的谱峰频率；ξ1,ξ2,ξ3为相对阻尼系数，ψ为低频运动中艏摇角，r(ψ)为坐标转换矩阵，xh,yh,ψh为高频运动中纵荡、横荡位置和艏摇角，η为低频运动中纵荡、横荡位置和艏摇角，υ表示低频速度矩阵，m表示质量矩阵，d表示阻尼矩阵，b表示未建模环境力，tb为包含时间常数的三维对角矩阵，τ表示合外力，ωh、ωυ、ωb分别为零均值高斯白噪声向量。

15、为满足计算的需要，需要将连续状态模型离散化，其形式可写为：

16、xk＝f(xk-1,uk-1)+ωk-1

17、yk＝h(xk)+vk

18、其中，k表示时刻，xk为系统状态向量，yk为量测向量，uk-1为控制输入，ωk-1为过程噪声，vk为量测噪声，f为系统转移函数，h为量测函数，过程噪声和量测噪声为互不相关的零均值的高斯白噪声，过程噪声的协方差矩阵为qk，量测噪声的协方差矩阵为rk，即：e(ωk)＝0，e(vk)＝0，cov(ωk)＝qk，cov(vk)＝rk。

19、具体地，在步骤3中，利用预测值和量测值之间的误差计算出先验残差ek，将开窗法与指数形式相结合计算出遗忘因子b1进而计算贡献比例d1,k，使得遗忘因子的值稳定在0.95～0.99之间，保证算法的稳定性，具体过程包括：

20、计算2n+1个sigma点和其对应的权值，i＝0,1,…,2n，即

21、

22、

23、其中，n为系统状态向量xk的维数，是权系数，其中i＝0,1,…,2nλ＝α2(n+κ)-n，α决定sigma点的散布程度，κ＝0；β描述x的分布信息，当高斯分布时，β的最优值是2；

24、计算2n+1个sigma点集的一步预测从而得到一步提前预测值及预测误差的协方差pk|k-1

25、

26、

27、

28、计算先验残差ek，并更新量测噪声协方差矩阵

29、

30、将开窗法与指数形式相结合计算出与先验残差有关的遗忘因子b1，其中λmin为遗忘因子的最小值，l1是利用开窗法计算窗宽内的先验残差，d1,k是与遗忘因子b1有关的贡献比例，其公式为：自适应的遗忘因子采用上述公式计算，从而保证遗忘因子自适应变化的同时，其值稳定在0.95～0.99之间。

31、进一步地，在步骤3中，忽略自适应sage-husa算法中更新量测噪声协方差阵中的负号项，并结合有偏估计对其进行更新，保证量测噪声协方差阵的正定性，其具体过程包括：

32、采用有偏估计的方式更新量测噪声协方差矩阵，保证该协方差矩阵的正定性并将遗忘因子带入量测噪声协方差矩阵中进行计算；

33、设置判断条件一：

34、若判据生效，则无需重新更新量测噪声；若不满足，则重新更新量测噪声，在上述更新量测噪声的基础上，添加系数，其更新公式为：

35、根据一步预测值再次使用ut变换，计算出2n+1个sigma点集

36、

37、将上述预测的sigma点集带入量测方程，得到对应的预测量测值并通过加权求和得到量测预测值和协方差py、pxy

38、

39、

40、

41、

42、具体地，在步骤4中，利用估计值和量测值计算出后验残差δk，与遗忘因子b1的取值方式相同计算出与后验残差有关的遗忘因子b2，结合开窗法和指数形式计算出贡献比例d2,k，忽略sage-husa算法中更新过程噪声协方差阵的负号项，其过程包括：

43、计算卡尔曼增益kk、系统的状态估计值和后验协方差阵pk|k

44、

45、

46、

47、计算后验残差δk，并更新过程噪声协方差矩阵

48、

49、与更新量测噪声协方差矩阵相类似，将开窗法与指数形式相结合计算出与后验残差有关的遗忘因子b2，进而计算出贡献比例d2,k，λmin为遗忘因子的最小值，通常取0.95，l2是利用开窗法计算后验残差，d2,k是与遗忘因子b2有关的贡献比例，其公式为：

50、进一步地，在步骤4中，结合有偏估计保证过程噪声协方差阵qk的非负性，考虑到整个系统的收敛性，添加判断条件，判断后验残差与理论残差之间的关系，根据判断结果重新更新过程噪声qk，防止因过程噪声协方差阵qk导致滤波发散，其过程包括：

51、设置判断条件二：

52、判断条件二考虑到的是后验残差与理论残差之间的关系，若后验残差的值大于理论残差，则重新更新过程噪声，其更新公式为：

53、

54、若判据生效，则无需重新更新过程噪声；

55、若k≤n，则继续按步骤3-4顺序依次进行计算，反之，则结束计算，式中n为设定的最大采样次数。

56、与现有技术相比，本发明具有以下优点和有益效果：

57、1.本发明为了自适应地更新噪声统计特性，设置两个不同的自适应遗忘因子分别更新量测噪声协方差和过程噪声协方差矩阵，使用与先验残差有关的遗忘因子更新量测噪声协方差，使用与后验残差有关的遗忘因子更新过程噪声协方差，且遗忘因子采用指数形式，可以保证在自适应变化的同时，使其值稳定在0.95～0.99左右；在更新过程噪声统计特性时，去掉了传统sage-husa算法中过程噪声协方差矩阵中的减号项，且采用了有偏估计，有效地保证了过程噪声的半正定性和量测噪声的正定性；为了避免滤波发散，在滤波过程中，不断检验实际的先验残差、后验残差，从而判断与理论特性是否一致，并以此作为判据重新更新噪声统计特性。

58、2.本发明适用于非线性系统，在保证算法可行性的前提下，可进一步提高算法的精度，降低了滤波发散的可能性。