[发明专利]一种基于反演的水下导航系统动力学模型中扰动估计方法

说明书

本发明公开了一种基于反演的水下导航系统动力学模型中扰动估计方法，包括以下步骤：根据水下航行器的动力学矢量方程，建立含有未知扰动的非线性系统状态方程和量测方程；采用容积卡尔曼滤波对水下航行器进行状态估计，得到其新息序列；采用递推最小二乘估计方法，由新息序列估计总力大小；采用迭代算法对所求总力进行修正；由总力求扰动力。本发明无需建立扰动力模型，动力学模型简单；对扰动力没有限制，适用性强；不需要准备大量样本数据，对扰动力进行在线估计，是一种实用性强的扰动估计方法。

技术领域

本发明属于水下航行器导航技术领域，具体涉及一种基于反演的导航系统动力学模型中扰动估计方法。

背景技术

高精度、强鲁棒性的导航算法是水下航行器完成复杂导航任务的保障。水下航行器利用水下推进装置对航行器进行航速控制，通过舵装置或矢量推进装置改变航向，其在军事或是民用都有广泛的应用。动力学模型直接地描述了水下航行器的运动参数与输入力/力矩之间的关系，能够有效避免因速度传感器失效或故障产生较大导航误差的问题，从而提高导航系统的鲁棒性和容错性。然而，导航系统动力学模型具有高度非线性、时变、强耦合等特点，根据刚体运动建立精确的动力学模型非常困难，而且航行时受到风、波、浪等外界环境的随机扰动，因此，为了减小扰动力对导航系统的影响，对扰动力进行实时估计有着相当重要的意义。

目前，通常有两类方法扰动估计方法，一是采用基于扰动力模型的系统参数辨识方法，但该方法对外界环境扰动有一定的局限性，且会增加动力学模型的复杂性；二是利用神经网络进行动力学模型自适应调节，但该算法需要有一定数量的样本数据，且样本数据要尽可能完备与准确，但精确的训练数据通常难以获得或代价昂贵。

发明内容

本发明目的是提供一种水下航行器动力学模型中扰动估计方法，能够有效的估计扰动力，以解决传统方法中建立扰动力模型复杂、对扰动力估计有一定的局限性，神经网络模型中大量的训练数据难以实现等缺点。

为实现上述目的，本发明采用的技术方案为：

一种基于反演的水下导航系统动力学模型中扰动估计方法，包括以下步骤：

步骤一，根据水下航行器的动力学矢量方程，建立含有未知扰动的非线性系统状态方程和量测方程；

步骤二，采用容积卡尔曼滤波对水下航行器进行状态估计，得到其新息序列；

步骤三，采用递推最小二乘估计方法，由新息序列估计总力大小；

步骤四，采用迭代算法对所求总力进行修正；

步骤五，由总力求扰动力。

步骤一的具体方法如下：

水下航行器有六个自由度运动，为描述航行器的运动情况，引入两种参考坐标系，分别为大地坐标系O-x_Gy_Gz_G和以航行器重心为原点的载体坐标系O-x_By_Bz_B；

航行器刚体动力学矢量表示式为

式中，向量υ为载体坐标系下分解的速度矢量υ＝[u,v,w,p,q,r]^T，其中，u是纵荡方向速度，v是横荡方向速度，w是垂荡方向速度，p是纵摇角速度，q是横摇角速度，r是艏摇角速度；M为系统惯性矩阵，其中包含附加质量；C(υ)为科里奥利向心力矩阵，其中包含附加质量；D(υ)为阻尼系数矩阵；τ为推进系统的主动力和力矩矢量；τ_d为外部环境扰动力矢量；

在大地坐标系下水下航行器的位置向量为η＝[x,y,z,φ,θ,ψ]^T，x、y、z为航行器位置，φ、θ、ψ为航行器的姿态角；在大地坐标系下位置对时间的导数与在载体坐标系下速度的转换关系为