申请/专利权人：中国电子科技集团公司第五十四研究所

申请日：2024-01-09

公开（公告）日：2024-03-08

公开（公告）号：CN117664115A

主分类号：G01C21/16

分类号：G01C21/16;G01C21/20

优先权：

专利状态码：在审-实质审查的生效

法律状态：2024.03.26#实质审查的生效;2024.03.08#公开

摘要：本发明提出一种基于距离模式的SINSDVL协同导航方法，属于多水下航行器的协同导航技术。其包括：1从AUV获取当前时刻的SINS、DVL的传感器数据；2距离测量模块获取当前时刻与主AUV的距离量测信息和主AUV的位置信息；3根据因子图模型建立协同导航的因子，包括SINS因子、DVL四波束因子、距离因子；4利用卡方检测对历史时刻的因子进行删选；5建立因子图最优估计模型，对当前时刻从AUV的状态进行全局估计，输出多源信息融合之后的优化结果。本发明在自身组合导航系统搭载的SINSDVL组合导航系统的前提下，提出一种利用主AUV位置信息和与主AUV距离信息完成导航的一种方法，能够利用因子图“即插即用”的特性，实现多源信息的全局估计。

主权项：1.一种基于距离模式的SINSDVL协同导航方法，其特征在于：包括：步骤1：从AUV获取当前时刻的SINS、DVL的传感器数据；从AUV自身搭载SINS、DVL导航传感器，当前时刻获取SINS的加速度计和陀螺仪的值，完成惯导更新，获取DVL四波束速度信息，建立DVL紧组合模型：1根据SINS当前时刻速度计算DVL坐标系下的速度： 其中，表示SINS表示导航坐标系下的速度信息，表示SINS的速度误差，φn表示SINS的姿态误差，表示导航坐标系与载体坐标系的姿态转移矩阵，表示载体系与DVL坐标系因安装误差导致的转移矩阵，表示DVL坐标系与四波束坐标系之间的转移矩阵，通常计算为： 其中，α表示DVL的波束夹角。2DVL的四波束速度模型可表示为： 其中，δK表示刻度因子，wDVL表示随机误差。3建立SINSDVL紧组合模型： 步骤2：从AUV获取主AUV的位置信息以及距离主AUV的距离信息，建立距离模型：1假设是由SINS计算的从AUV的相对位置和主AUV的位置信息，相对位置矢量计算如下： 忽略小量的二阶误差，测距误差为： 其中表示相对位置误差。2根据主AUV的位置信息和从AUV的位置信息建立距离模型： 其中，表示主AUV的位置信息，表示从AUV的位置信息，RNh＝RN+h，RMh＝RM+j： Re表示地球长半轴半径，e表示地球偏心率。将转到测距坐标系： 其中，表示载体坐标系与测距坐标系的旋转矩阵，表示SINS与测距传感器之间的杆臂误差，表示地理坐标系与导航坐标系的旋转矩阵，与当前位置有关： 由位置误差引起的旋转矩阵计算误差为： 3计算 其中， 考虑安装误差和杆臂误差： 忽略高阶小量误差： 其中，简化为： 则为： 4根据SINS和距离模型，其距离误差模型为： 其中，步骤3：根据因子图模型建立协同导航的因子：1建立SINS因子：rSINS,k＝rxk|k-1,Xk＝Fk|k-1Xk-1-Xk其中，Fk|k-1表示状态转移矩阵，φn,δVn,δP,ε,分别是姿态误差、速度误差、位置误差、陀螺常数漂移和加速度计偏置。2建立DVL紧组合因子： 其中，3建立距离因子： 其中，Hdis＝[H103×3H203×6]步骤4：根据步骤3的因子建立因子图全局优化函数： 其中，Pm|m-1is表示m时刻的先验协方差,RDVL,misDVL在m时刻的噪声模型,Rdis,m是m时刻的噪声模型。步骤5：对步骤4的代价函数进行最优估计，输出导航结果。

全文数据：

权利要求：

百度查询： 中国电子科技集团公司第五十四研究所 一种基于距离模式的SINS/DVL协同导航方法

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

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