基于交互式多模型滤波的船体变形角估计方法与流程

本发明涉及船舶船体变形领域，设计了一种基于交互式多模型滤波的船体变形角估计方法。

背景技术：

随着科学技术的进步，现代船舶和船载设备都在朝着高精度、高自动化的方向快速发展，现代船舶都会安装有各种导航设备和攻防设备，包括各种雷达、光学观瞄设备、跟踪测量设备、导弹发射架等。为保证设备的正常运行，需要建立一个统一的空间姿态基准。但是，船舶并不是一个绝对刚体，在海上航行时，由于船体变形角，空间姿态基准的精度受到严重制约。因此精确估计船体变形角对于船舶以及船载设备都有着重要意义，具有广阔的应用前景。

估计船体变形角时，对于变形角模型的建立提出了较高要求，但由于在实际航行中，变形角模型参数的不确定最终会影响估计精度。因此在模型不确定的情况下，精确估计船体变形角是值得深入研究的课题。针对结构和参数不确定或者变化，多模型估计是一种非常有效的自适应估计方法。

技术实现要素：

本发明的目的在于提供一种能够在变形角参数不确定的情况下仍能准确估计船体变形角的基于交互式多模型滤波估计船体变形的惯性测量方法。

本发明的目的是这样实现的，一种基于交互式多模型滤波的船体变形角估计方法，包括如下步骤：

步骤一：在船艏、船艉分别安装一套船体导航装置，即主惯导与子惯导，对两套惯导建立考虑挠曲变形的基本滤波模型；

所述基本滤波模型的系统状态向量为

式中，δv表示主惯导与子惯导系统速度误差，表示主惯导与子惯导系统姿态误差，▽和ε分别表示加速度计零偏和陀螺漂移，ξ表示主惯导与子惯导之间的安装误差角，θ表示挠曲变形角，表示挠曲变形角速率。

滤波模型为

式中，w为系统噪声矩阵，w～n(0,q)，q为w的方差；v为量测方差矩阵，v～n(0,rv)，rv为v的方差。其中，f和g分别为：

其中，

i表示单位阵。

其中，vx，vy分别表示主惯导东向、北向速度，ωie表示地球自转角速率，表示当地纬度，r表示地球半径，表示子惯导姿态矩阵，上标n表示导航坐标系，下标s子惯导载体坐标系，fx，fy，fz分别表示沿东北天方向的比力，β＝[βxβyβz]，且βx，βy，βz分别表示东北天方向的挠曲变形参数。

步骤二：，按照挠曲变形参数的不确定性，利用基本滤波模型构建交互模型；

所述的交互模型为：其中，分别由将中的β替代为β1,β2,...,βn得到。

其中，

其中，β1,β2,...,βn为挠曲变形参数向量；c1,c2,...,cn为交互模型常值参数；n为所设置交互模型个数。

步骤三：主惯导与子惯导系统完成初始对准并进行惯导解算，实时采集两套惯导系输出的惯性器件测量数据、导航解算数据；对交互模型滤波器进行初始化；

步骤四：进行交互模型滤波器的解算，得到船体变形角估计结果。

(1)首先，输入交互：

其中：为第j个交互模型滤波器的输入，用于k时刻解算；为第i个交互模型滤波器在k-1时刻的状态估计值；为第j个交互模型滤波器的协方差输入，用于k时刻解算；为第i个交互模型滤波器在k-1时刻的协方差。j＝1,2,…,n；i＝1,2,…,n。

为k-1时刻第i个交互模型滤波器估计结果到k时刻第j个交互模型滤波器输入的转移概率；其中为第j个交互模型预测概率，为k-1时刻第i个模型的概率；pij为概率转移矩阵p的元素，n为所设置交互模型个数。

(2)各个交互模型滤波器进行滤波解算，再经交互输出，得到总的估计结果；且保存各个交互模型滤波器的滤波解算结果，用于下一时刻输入交互；

所述交互输出的表达式为

总的状态估计计算:

总的协方差估计pk|k计算：

其中，为k时刻第j个交互模型的概率。

一种基于交互式多模型滤波的船体变形角估计方法，还包括：采用速度加角速率匹配模式；

量测方程中的量测量即为

z＝[δvxδvyδωxδωyδωz]^t

式中，δvx，δvy表示主惯导与子惯导速度差，δωx,δωy,δωz表示主惯导与子惯导角速度差。

量测矩阵h为：

其中

ω1x、ω1y、ω1z分别表示主惯导在东北天方向方向输出的角速度。

本发明方法具有如下有益效果：

一、在建立船体变形角的数学模型时，考虑了参数不确定对滤波结果的影响，更符合实际航行中的真实情况；

二、利用交互模型滤波器进行状态估计，经过输入交互和输出交互，使变形角估计精度比常规卡尔曼滤波估计精度更高。

附图说明

图1是本发明方法的船体变形估计流程图。

图2是本发明方法在3个交互模型情况下的解算结构图。

图3是本发明方法的动态变形角估计误差示意图。

图4是本发明方法的静态变形角估计误差示意图。

具体实施方式

下面结合附图对本发明做更详细的描述：

步骤一：在船艏、船艉分别安装一套船体导航装置，即主惯导与子惯导，对两套惯导建立基本滤波模型；

所述基本滤波模型的系统状态向量为

式中，δv表示主惯导与子惯导系统速度误差，表示主惯导与子惯导系统姿态误差，▽和ε分别表示加速度计零偏和陀螺漂移，ξ表示主惯导与子惯导之间的安装误差角，θ表示挠曲变形角，表示挠曲变形角速率。

基于速度加角速率匹配法建立船体变形模型，选取系统状态向量为

式中，δv表示主惯导与子惯导系统速度误差，表示主惯导与子惯导系统姿态误差，▽和ε分别表示加速度计零偏和陀螺漂移，ξ表示主惯导与子惯导之间的安装误差角，θ表示挠曲变形角，表示挠曲变形角速率。选取东北天地理坐标系为导航坐标系，由于是在船体上测量，忽略天向速度和天向加速度计分量，所以状态向量19维。

由惯导基本误差方程，可得

式中上标n表示导航坐标系，下标i表示惯性坐标系，下标e表示地球坐标系，下标b表示载体坐标系，选取东北天地理坐标系为导航坐标系。f表示比力，表示地球自转角速率在导航坐标系的投影，表示船体相对地球运动角速率在导航系下的投影，和则为主惯导与子惯导两个相应角速率的差值。为导航系相对惯性系的角速率，表示由惯导解算的和真实的之间的误差。表示捷联矩阵。

加速度计零偏▽和陀螺漂移ε是惯性导航系统的主要误差来源，根据陀螺漂移的特性，通常包括陀螺的随机常值漂移ε0、慢变漂移εr和快变漂移εg。陀螺漂移可表示为：

与陀螺漂移误差模型分析类似，加速度计误差模型通常也分为三种分量，但在导航设计过程中，通常只考虑随机常值误差，即加速度计偏置误差

主子惯导之间的安装误差角视为常值，即：

由于舰船在海面上受不同海况的影响，其动态挠曲变形与随机噪声驱动下的随机过程近似，这一特性与白噪声干扰下的马尔科夫过程很类似，因此，采用二阶马尔科夫过程进行船体变形的模拟。可得三轴向挠曲变形角方程

其中β是挠曲变形参数，其大小与相关时间有关

βi＝2.146/τi(i＝x,y,z)(7)

τi是相应轴上的变形相关时间。ηi是白噪声，其方差大小由参数qηi决定

σi则是变形角θi的方差。

所建立的滤波模型为

式中，w为系统噪声矩阵，w～n(0,q)，q为w的方差；v为量测方差矩阵，v～n(0,rv)，rv为v的方差。其中，f和g分别为：

其中，

i表示单位阵。

其中，vx，vy分别表示主惯导东向、北向速度，ωie表示地球自转角速率，表示当地纬度，r表示地球半径，表示子惯导姿态矩阵，上标n表示导航坐标系，下标s子惯导载体坐标系，fx，fy，fz分别表示沿东北天方向的比力，β＝[βxβyβz]，且βx，βy，βz分别表示东北天方向的挠曲变形参数。

本发明采用速度加角速率匹配模式，量测方程中的量测量即为

z＝[δvxδvyδωxδωyδωz]^t(10)

式中，δvx，δvy表示主惯导与子惯导速度差，δωx,δωy,δωz表示主惯导与子惯导角速度差。

量测矩阵h为：

其中

ω1x、ω1y、ω1z分别表示主惯导在东北天方向方向输出的角速度。

步骤二：，按照挠曲变形参数的不确定性，利用基本滤波模型构建交互模型；

所述的交互模型为：其中，分别由将中的β替代为β1,β2,...,βn得到。

其中，

其中，β1,β2,...,βn为挠曲变形参数向量；c1,c2,...,cn为交互模型常值参数；n为所设置交互模型个数。

按照挠曲变形参数的不确定性划分为3个并行的交互模型滤波器，如图2所示。

在上述变形测量模型中，挠曲变形参数难以预先准确获得，这就导致所建立的模型存在误差，利用多模型滤波方法就是采用多个滤波器并行工作，而这些滤波器则涵盖模型误差的变化范围。通过将挠曲变形参数均匀地分成3个子区间，划分的个数即为模型的个数，即n＝3。优化参数后得到：

步骤三：主惯导与子惯导系统完成初始对准并进行惯导解算，实时采集两套惯导系输出的惯性器件测量数据、导航解算数据；对交互模型滤波器进行初始化；

步骤四：进行交互模型滤波器的解算；

(1)首先，输入交互：

其中：为第j个交互模型滤波器的输入，用于k时刻解算；为第i个交互模型滤波器在k-1时刻的状态估计值；为第j个交互模型滤波器的协方差输入，用于k时刻解算；为第i个交互模型滤波器在k-1时刻的协方差。j＝1,2,…,n；i＝1,2,…,n。

为k-1时刻第i个交互模型滤波器估计结果到k时刻第j个交互模型滤波器输入的转移概率；其中为第j个交互模型预测概率，为k-1时刻第i个模型的概率；pij为概率转移矩阵p的元素，n为所设置交互模型个数。

(2)各个交互模型滤波器进行滤波解算，再经交互输出，得到总的估计结果；且保存各个交互模型滤波器的滤波解算结果，用于下一时刻输入交互；

以和进行卡尔曼滤波：

一步预测

预测协方差

卡尔曼滤波增益

滤波估计

滤波协方差估计

交互后各模型状态和协方差经过卡尔曼滤波后得到的估计值和将分别作为下一时刻的交互前的状态和协方差估计。

模型概率更新：

采用似然函数来更新交互后的模型概率交互后模型j的概率似然函数为:

上式中，为量测残差

为相应的量测残差的协方差

则交互后模型j的概率为

上式中，c为归一化常数，且

其中为交互后第j个模型预测概率。

所述交互输出的表达式为总的状态估计计算:

总的协方差估计pk|k计算：

其中，为k时刻第j个交互模型的概率。

为了进一步说明本发明的有益效果，在以下条件下根据新的滤波算法进行仿真验证。

假设主惯导无误差，仿真参数选取：陀螺漂移：εx＝εy＝εz＝0.01°/h加速度计的随机零偏:▽x＝▽y＝100μg，舰船以东向10m/s的速度匀速直线航行，绕横摇轴、纵摇轴、航向轴的摇摆幅度分别为:θpm＝3°，θrm＝4°，θym＝5°，摇摆初始角和摇摆初始相位均假定为零。由假定摇摆周期分别为tp＝10s，tr＝8s，ty＝6s。本次仿真每种算法结果均在同等条件下重复进行50次，并统计50次仿真结果数据，结果如表1、表2和图3、图4所示。由表1、表2和图3、图4可知，较之传统的卡尔曼滤波方法，本发明方法的变形角估计误差更小，精度更高。

表1静态变形角估计误差的50次仿真统计均值

表2动态变形角估计误差的50次仿真统计均值。