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

权利要求书

1.一种基于改进强跟踪滤波状态观测器的船舶动力定位方法，其特征在于，包括以下几个步骤：

步骤一：采集船舶的位置和艏向；

步骤二：利用改进强跟踪滤波状态观测器滤除掉船舶位置和艏向中噪声部分，得到满足精度要求的船舶位置和艏向估计值发送给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＝w_v和v＝w_z为互不相关的零均值高斯白噪声，

将状态空间方程的离散化表示为：

xk=fk-1(xk-1,uk-1,wk-1)zk=hk(xk,vk);]]>

步骤二：将包含噪声部分的船舶位置和艏向z_k作为改进强跟踪滤波状态观测器的输入，得到满足精度要求的船舶位置和艏向估计值

所述的改进强跟踪滤波状态观测器通过以下设计步骤实现：

a、确定状态初始值误差协方差初值p₀、遗忘因子ρ、弱化因子β和常数α_i(i),i＝1,2,…,n；

b、计算残差：

Sk=zk-Hkx^k|k-1]]>

其中，H_k为函数h进行泰勒展开所得到的雅格比矩阵，

x^k|k-1=f(xk-1,uk-1)]]>

Hk=∂hk(xk,vk)∂xk|xk=x^k|k-1;]]>

c、计算多重次优渐消因子矩阵和矩阵

确定残差协方差阵V_k：

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]]>

其中，Q_k和R_k分别为环境干扰高频噪声和测量噪声的方差，β为预先设定的弱化因子，p_k为误差估计协方差，Φ_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、根据矩阵计算状态预报误差协方差p_kk-1：

Pk|k-1=Λ‾kΦk,k-1Pk-1Φk,k-1TΛ‾kT+Γk-1Qk-1Γk-1T;]]>

e、根据状态预报误差协方差p_kk-1计算滤波增益k_k

Kk=Pk|k-1HkT(HkPk|k-1HkT+Rk)-1;]]>

f、更新状态估计

x^k=x^k|k-1+Kk(zk-Hkx^k|k-1);]]>

g、更新误差估计协方差p_k

Pk=(I-KkHk)Pk|k-1(I-KkHk)T+KkRkKkT;]]>

h、保存滤波数据结果将中的作为满足要求的船舶位置和艏向估计值传送给PID控制器，重复步骤b到步骤h，直到数据处理结束。

2.根据权利要求1所述的一种基于改进强跟踪滤波状态观测器的船舶动力定位方法，其特征在于：所述的PID控制器为：

τ=KPηe+KI∫ηedt+KDdηedt]]>

τ＝[τ_X τ_Y τ_N]^T表示船舶推进器输出推力和力矩，K_P，K_I和K_D为PID算法中的比例系数；η_e为船舶期望目标位置姿态η_d和实际位置姿态η之间的偏差，即η_e＝η_d-η，η＝[η_X,η_Y,η_ψ]^T分别对应东北坐标系下的船舶的位置和姿态角；η_d＝[η_Xd,η_Yd,η_ψd]^T分别对应船舶期望北东位置和姿态角。