本发明属于水下航行器导航技术领域，具体涉及一种基于反演的导航系统动力学模型中扰动估计方法。
背景技术：
高精度、强鲁棒性的导航算法是水下航行器完成复杂导航任务的保障。水下航行器利用水下推进装置对航行器进行航速控制，通过舵装置或矢量推进装置改变航向，其在军事或是民用都有广泛的应用。动力学模型直接地描述了水下航行器的运动参数与输入力/力矩之间的关系，能够有效避免因速度传感器失效或故障产生较大导航误差的问题，从而提高导航系统的鲁棒性和容错性。然而，导航系统动力学模型具有高度非线性、时变、强耦合等特点，根据刚体运动建立精确的动力学模型非常困难，而且航行时受到风、波、浪等外界环境的随机扰动，因此，为了减小扰动力对导航系统的影响，对扰动力进行实时估计有着相当重要的意义。
目前，通常有两类方法扰动估计方法，一是采用基于扰动力模型的系统参数辨识方法，但该方法对外界环境扰动有一定的局限性，且会增加动力学模型的复杂性；二是利用神经网络进行动力学模型自适应调节，但该算法需要有一定数量的样本数据，且样本数据要尽可能完备与准确，但精确的训练数据通常难以获得或代价昂贵。
技术实现要素：
本发明目的是提供一种水下航行器动力学模型中扰动估计方法，能够有效的估计扰动力，以解决传统方法中建立扰动力模型复杂、对扰动力估计有一定的局限性，神经网络模型中大量的训练数据难以实现等缺点。
为实现上述目的，本发明采用的技术方案为：
一种基于反演的水下导航系统动力学模型中扰动估计方法，包括以下步骤：
步骤一，根据水下航行器的动力学矢量方程，建立含有未知扰动的非线性系统状态方程和量测方程；
步骤二，采用容积卡尔曼滤波对水下航行器进行状态估计，得到其新息序列；
步骤三，采用递推最小二乘估计方法，由新息序列估计总力大小；
步骤四，采用迭代算法对所求总力进行修正；
步骤五，由总力求扰动力。
步骤一的具体方法如下：
水下航行器有六个自由度运动，为描述航行器的运动情况，引入两种参考坐标系，分别为大地坐标系o-xgygzg和以航行器重心为原点的载体坐标系o-xbybzb；
航行器刚体动力学矢量表示式为
式中，向量υ为载体坐标系下分解的速度矢量υ＝[u,v,w,p,q,r]t，其中，u是纵荡方向速度，v是横荡方向速度，w是垂荡方向速度，p是纵摇角速度，q是横摇角速度，r是艏摇角速度；m为系统惯性矩阵，其中包含附加质量；c(υ)为科里奥利向心力矩阵，其中包含附加质量；d(υ)为阻尼系数矩阵；τ为推进系统的主动力和力矩矢量；τd为外部环境扰动力矢量；
在大地坐标系下水下航行器的位置向量为η＝[x,y,z,φ,θ,ψ]t，x、y、z为航行器位置，φ、θ、ψ为航行器的姿态角；在大地坐标系下位置对时间的导数与在载体坐标系下速度的转换关系为
其中，j(η)为转换矩阵；
将式(1)和式(2)的所表示非线性数学模型转化成状态空间模型，即可得到状态方程和量测方程
其中，
式中，x＝[ηtυt]t为状态变量；w为系统状态方程的高斯白噪声；z为观测向量；v为量测方程的高斯白噪声；a为系统状态转移矩阵；b为输入量控制矩阵；h为系统量测矩阵；06×6为6×6的零矩阵；i6×6为6×6的单位矩阵；
将式(3)离散化，得到k时刻的离散化系统模型为
其中，φ为离散化的系统状态转移矩阵，φ＝exp(a×△t)，△t为采样间隔；γ为离散化的输入量控制矩阵，
步骤二的具体方法如下：
(2.1)设定初始参数
设定初始时刻水下航行器的系统状态值x0，状态协方差p0/0，系统噪声协方差q，量测噪声协方差r，灵敏度矩阵ms，力协方差矩阵pb；
(2.2)时间更新
分解状态估计误差协方差阵
pk/k＝sk/ksk/kt
其中，pk/k为k时刻状态估计误差协方差阵，sk/k是k时刻状态估计误差协方差阵分解的下三角矩阵；
构造容积点并经状态方程传播
其中，
计算状态预测值
(2.3)测量更新
构造容积点并经量测方程传播
zi,k+1/k＝hk+1(xi,k+1/k),i＝1,2,…,m
其中xi,k+1/k，zi,k+1/k是相应的容积点；
计算观测量预测值
估计新息自协方差矩阵
估计互协方差矩阵的一步预测值
卡尔曼增益矩阵
新息序列
估计当前时刻的状态向量
误差协方差矩阵
步骤三的具体方法如下：
(3.1)求k+1时刻灵敏度矩阵
bs(k+1)＝h[φms(k)+i]γ
ms(k+1)＝[i-kk+1h][φms(k)+i]
(3.2)k+1时刻估计力的增益矩阵
kb(k+1)＝γ-1pb(k)bts(k+1)[bs(k+1)γ-1pb(k)bts(k+1)+pzz]-1
其中，γ为增益矩阵调节因子；pb为k时刻估计力的误差协方差阵。
(3.3)估计k+1时刻的总力
(3.4)更新估计力的误差协方差矩阵
pb(k+1)＝[i-kb(k+1)bs(k+1)]γ-1pb(k)
步骤四的具体方法如下：
(4.1)
(4.2)
(4.3)如果|△f|>σ，则循环步骤(4.1)和(4.2)；否则终止循环；其中σ为误差允许范围。
步骤五中，通过下式求扰动力：
有益效果：本发明提出了一种基于反演的扰动估计方法，运用扰动力引起的状态量变化反演推算出扰动力。通过建立动力学模型，应用容积卡尔曼滤波对系统状态进行估计，利用新息序列经递推最小二乘法估计总力，并采用迭代算法对总力进行修正，最后求得扰动力。该方法无需建立扰动力模型，动力学模型简单；对扰动力没有限制，适用性强，是一种实用性强的扰动估计方法。
本发明方法与传统的扰动估计方法相比，是利用运动状态直接对扰动力进行反演运算，无需对扰动力建模分析，可靠性高；采用容积卡尔曼滤波与递推最小二乘算法估计力，实时性强；采用迭代算法对总力进行估计，提高了估计精度。
附图说明
图1是本发明的基于反演的导航系统动力学模型中扰动估计方法的流程示意框图。
具体实施方式
下面结合附图对本发明作更进一步的说明。
如图1所示，一种基于反演的水下导航系统动力学模型扰动估计方法，包括以下步骤：
(1)根据水下航行器的动力学矢量方程，建立含有未知扰动的非线性系统状态方程和量测方程
在海洋环境扰动下，航行器一般有六个自由度运动。为描述航行器的运动情况，引入两种参考坐标系，大地坐标系o-xgygzg和以航行器重心为原点的载体坐标系o-xbybzb。航行器刚体动力学矢量表示式为
式中，向量υ为载体坐标系下分解的速度矢量υ＝[u,v,w,p,q,r]t，其中，u是纵荡方向速度，v是横荡方向速度，w是垂荡方向速度，p是纵摇角速度，q是横摇角速度，r是艏摇角速度；m为系统惯性矩阵，其中包含附加质量；c(υ)为科里奥利向心力矩阵，其中包含附加质量；d(υ)为阻尼系数矩阵；τ为推进系统的力和力矩矢量；τd为风、浪、流等环境和外部作用力矢量。
在大地坐标系下航行器的位置向量为η＝[x,y,z,φ,θ,ψ]t，x、y、z为航行器位置，φ、θ、ψ为航行器的姿态角。在大地坐标系下位置对时间的导数与在载体坐标系下速度的转换关系为
其中，j(η)为转换矩阵。
将式(1)和式(2)的所表示非线性数学模型转化成状态空间模型，即可得到状态方程和量测方程
其中，
式中，x＝[ηtυt]t为状态变量；w为系统状态方程的高斯白噪声；z为观测向量；v为量测方程的高斯白噪声；a为系统状态转移矩阵；b为输入量控制矩阵；h为系统量测矩阵；06×6为6×6的零矩阵；i6×6为6×6的单位矩阵。
将公式(3)离散化，得到k时刻的离散化系统模型为
其中，φ为离散化的系统状态转移矩阵，φ＝exp(a×△t)，△t为采样间隔；γ为离散化的输入量控制矩阵，
(2)采用容积卡尔曼滤波对水下航行器进行状态估计，得到其新息序列
2.1)设定初始参数
设定初始时刻水下航行器的系统状态值x0，状态协方差p0/0，系统噪声协方差q，量测噪声协方差r，灵敏度矩阵ms，力协方差矩阵pb。
2.2)时间更新
分解状态估计误差协方差阵
pk/k＝sk/ksk/kt
其中，pk/k为k时刻状态估计误差协方差阵，sk/k是k时刻状态估计误差协方差阵分解的下三角矩阵。
构造容积点并经状态方程传播
其中，
计算状态预测值
2.3)测量更新
构造容积点并经量测方程传播
zi,k+1/k＝hk+1(xi,k+1/k),i＝1,2,…,m
其中，xi,k+1/k，zi,k+1/k是相应的容积点。
计算观测量预测值
估计新息自协方差矩阵
估计互协方差矩阵的一步预测值
卡尔曼增益矩阵
新息序列
估计当前时刻的状态向量
误差协方差矩阵
(3)采用最小二乘估计器由新息序列估计总力大小
3.1)求灵敏度矩阵
bs(k+1)＝φ[fms(k)+i]γ
ms(k+1)＝[i-k(k+1)h][φms(k)+i]
3.2)估计力的增益矩阵
kb(k+1)＝γ-1pb(k)bts(k+1)[bs(k+1)γ-1pb(k)bts(k+1)+pzz]-1
其中，γ为增益矩阵调节因子；pb为k时刻估计力的误差协方差阵。
3.3)估计当前时刻的总力
3.4)更新估计力的误差协方差矩阵
pb(k+1)＝[i-kb(k+1)bs(k+1)]γ-1pb(k)
(4)采用迭代算法对所求总力进行修正
4.1)
4.2)
4.3)如果|△f|>σ，则循环4.1)和4.2)；否则终止循环；其中σ为误差允许范围。
(5)通过下式由总力求扰动力
以上所述仅是本发明的优选实施方式，应当指出：对于本技术领域的普通技术人员来说，在不脱离本发明原理的前提下，还可以做出若干改进和润饰，这些改进和润饰也应视为本发明的保护范围。