申请/专利权人：东南大学

申请日：2020-11-19

公开（公告）日：2024-01-02

公开（公告）号：CN112507281B

主分类号：G06F17/16

分类号：G06F17/16;G01C21/16

优先权：

专利状态码：有效-授权

法律状态：2024.01.02#授权;2021.04.02#实质审查的生效;2021.03.16#公开

摘要：一种基于双状态多因子抗差估计SINSDVL紧组合系统的方法。通过下列步骤实现：步骤1：获取载体当前时刻SINS和DVL的采集数据；步骤2：根据步骤1的采集数据建立状态方程和量测方程；步骤3：对状态量和量测量进行卡方检测；步骤4：计算模型误差和量测误差的自适应因子；步骤5：根据步骤4的多因子调节卡尔曼滤波的增益矩阵；步骤6：根据步骤2的系统方程和步骤5的增益矩阵，利用卡尔曼进行最优估计，将递推结果反馈到SINS的解算结果，得到最终速度和位置信息。该方法利用DVL四波束的多普勒频移测量信息，在四波束测量值缺失非完整情况下依旧保证连续的测量信息。该方法能够有效抑制导航状态中的不确定噪声，提高导航精度。

主权项：1.一种基于双状态多因子抗差估计SINSDVL紧组合系统的方法，包括，其特征在于：步骤1：获取载体当前时刻SINS和DVL的采集数据；步骤2：根据步骤1的采集数据建立状态方程和量测方程；步骤2建立系统状态方程，具体步骤如下： 其中，F表示系统状态转移矩阵，X表示状态向量，G表示系统噪声矩阵，W表示系统噪声矢量，其中，状态向量表示为： 其中，[φxφyφz]T表示载体俯仰角、横滚角、航向角误差；表示载体东向、北向、天向速度误差；[δλδLδh]T表示载体地理经度、纬度、高度误差；表示SINS的加速度零偏；[εxεyεz]T表示SINS的陀螺零偏；[ρ1ρ2ρ3ρ4]T表示DVL四通道多普勒频移常值偏移；kD表示DVL刻度因子误差；bps表示深度计零偏；系统状态转移矩阵F、系统噪声矩阵G、系统噪声矢量W表示为： 其中， 其中，表示b系到n系的姿态转移矩阵，表示加速度计噪声，表示陀螺噪声，ωie表示地球自转角速率，VE表示东向速度，VN表示北向速度，VU表示天向速度，Re表示地球长半轴半径，e表示地球偏心率，ωie表示地球自转角速率，ge表示赤道重力，β表示地球重力扁率，β2＝3.08×10-6S-2，β1＝182βf+f2，f表示地球扁率；步骤2建立系统量测方程，具体步骤如下：Z＝HX+V其中，Z表示系统量测值，H表示量测方程转移矩阵，V表示量测噪声；定义DVL所在坐标系为d系，四通道所在坐标系为beam系，波束与水下航行器的夹角为a，DVL在d系下速度为则坐标系转移矩阵可表示为： 定义DVL发射声波信号时四通道的速度为接收脉冲时四通道的速度为声波信号在水中的速度为c，声波信号的发射频率为f0，则四波束的多普勒频移值为： 若忽略发射接收时DVL的速度变化量以及水声速度和DVL速度之间的较小量，则多普勒频移可简化为： 导航坐标系下SINS的速度为载体坐标系下SINS的速度为标定后载体坐标系与DVLd系的坐标系转移矩阵为标定后的安装偏差角其中，表示绕载体横轴的安装偏角，表示绕载体纵轴的安装偏角，表示绕载体竖轴的安装偏角，则利用导航坐标系的SINS速度计算四通道多普勒频移值为： DVL的多普勒频移测量误差模型为： 定义深度计的测量误差模型为： 则多普勒频移误差方程为： 则系统量测值Z、量测方程转移矩阵H、量测噪声V分别为： V＝[WDWps]T；步骤3：对状态量和量测量进行卡方检测；步骤4：计算模型误差和量测误差的自适应因子；根据步骤4对模型误差和量测误差的自适应因子，步骤如下：1根据最小二乘原则构造损失函数Jk： 其中，为k时刻观测自适应因子，βk为k时刻动力学模型信息的自适应因子，Pk为状态估计向量的权矩阵，为状态预测向量的权矩阵，为待估计值：对Jk求导并令导数为0，则待估计值为： 2结合步骤3卡方判定结果，自适应因子的选取准则为：①观测自适应因子观测自适应选取：定义误差判别统计量 则其中，c为常量，最优值为1.0，范围取1.0～1.5.②βk动力学模型信息的自适应因子选取：定义误差判别统计量 则其中，c为常量，最优值为1.0，范围取1.0～1.5；步骤5：根据步骤4的多因子调节卡尔曼滤波的增益矩阵；步骤6：根据步骤2的系统方程和步骤5的增益矩阵，利用卡尔曼进行最优估计，将递推结果反馈到SINS的解算结果，得到最终速度和位置信息。

全文数据：

权利要求：

百度查询： 东南大学 一种基于双状态多因子抗差估计SINS/DVL紧组合系统的方法

免责声明
1、本报告根据公开、合法渠道获得相关数据和信息，力求客观、公正，但并不保证数据的最终完整性和准确性。
2、报告中的分析和结论仅反映本公司于发布本报告当日的职业理解，仅供参考使用，不能作为本公司承担任何法律责任的依据或者凭证。

阅读全文 双屏查看 官方信息 专利公告 收藏专利 下载PDF 下载WORD