[发明专利]一种基于改进强跟踪滤波状态观测器的船舶动力定位方法

摘要

本发明公开了一种基于改进强跟踪滤波状态观测器的船舶动力定位方法，包括以下步骤采集船舶的位置和艏向；利用改进强跟踪滤波状态观测器滤除掉船舶位置和艏向中噪声部分，得到满足精度要求的船舶位置和艏向估计值发送给PID控制器；PID控制器根据船舶位置和艏向期望值和接收到的信息，输出控制力和力矩，控制船舶运动。本发明在外界干扰发生突变的情况下跟踪滤波效果更好，本发明能够提高船舶的稳定性。

主权项

一种基于改进强跟踪滤波状态观测器的船舶动力定位方法，其特征在于，包括以下几个步骤：步骤一：采集船舶的位置和艏向；步骤二：利用改进强跟踪滤波状态观测器滤除掉船舶位置和艏向中噪声部分，得到满足精度要求的船舶位置和艏向估计值发送给PID控制器；步骤三：PID控制器根据船舶位置和艏向期望值和接收到的信息，输出控制力和力矩，控制船舶运动；所述的利用改进强跟踪滤波状态观测器滤除掉船舶位置和艏向中噪声部分的方法为：步骤一：建立船舶运动数学模型的状态空间方程：x·=f(x,u,w)]]>z＝h(x,v)其中，f是船舶系统的非线性系统状态函数矩阵，h是非线性系统量测函数，x＝[ηT ν'T]T是选定的状态向量，η＝[ηx ηy ηψ]T表示在北东坐标系下的船舶的位置和姿态角，ν'＝[U V R]T表示在船体坐标系下的船舶的线速度和角速度，u＝[X Y N]T是输入向量表示在船体坐标系下的船舶所受的力和力矩，z为输出向量表示在北东坐标系下的包含噪声部分的船舶位置和艏向，w＝wv和v＝wz为互不相关的零均值高斯白噪声，将状态空间方程的离散化表示为：xk=fk-1(xk-1,uk-1,wk-1)zk=hk(xk,vk);]]>步骤二：将包含噪声部分的船舶位置和艏向zk作为改进强跟踪滤波状态观测器的输入，得到满足精度要求的船舶位置和艏向估计值所述的改进强跟踪滤波状态观测器通过以下设计步骤实现：a、确定状态初始值误差协方差初值p0、遗忘因子ρ、弱化因子β和常数αi(i),i＝1,2,…,n；b、计算残差：Sk=zk-Hkx^k|k-1]]>其中，Hk为函数h进行泰勒展开所得到的雅格比矩阵，x^k|k-1=f(xk-1,uk-1)]]>Hk=∂hk(xk,vk)∂xk|xk=x^k|k-1;]]>c、计算多重次优渐消因子矩阵和矩阵确定残差协方差阵Vk：Vk=ϵ1ϵ1T,k=1ρVk-1+ϵkϵkT1+ρ,k≥2]]>其中，ρ为预先设定的遗忘因子；求得多重次优渐消因子矩阵λk：λki=αick,αick≥11,αick<1,]]>其中αi为常数，系数A‾=diag[α1,α2,...,αn],]]>Nk=Vk-HkΓk-1Qk-1Γk-1THkT-βRk]]>Mk=A‾Φk,k-1Pk-1Φk,k-1THkTHk]]>其中，Qk和Rk分别为环境干扰高频噪声和测量噪声的方差，β为预先设定的弱化因子，pk为误差估计协方差，Φk和Γk为函数f进行泰勒展开所得的雅格比矩阵，Φk-1=∂fk-1(xk-1,uk-1,wk-1)∂x^k-1|xk-1=x^k-1]]>Γk-1=∂fk-1(xk-1,uk-1,wk-1)∂wk-1|xk-1=x^k-1,]]>根据多重次优渐消因子矩阵求得矩阵Λ‾k=diag[λk1,λk2,...,λkn];]]>d、根据矩阵计算状态预报误差协方差pkk‑1：Pk|k-1=Λ‾kΦk,k-1Pk-1Φk,k-1TΛ‾kT+Γk-1Qk-1Γk-1T;]]>e、根据状态预报误差协方差pkk‑1计算滤波增益kkKk=Pk|k-1HkT(HkPk|k-1HkT+Rk)-1;]]>f、更新状态估计x^k=x^k|k-1+Kk(zk-Hkx^k|k-1);]]>g、更新误差估计协方差pkPk=(I-KkHk)Pk|k-1(I-KkHk)T+KkRkKkT;]]>h、保存滤波数据结果将中的作为满足要求的船舶位置和艏向估计值传送给PID控制器，重复步骤b到步骤h，直到数据处理结束。