申请/专利权人：河北汉光重工有限责任公司

申请日：2021-09-22

公开（公告）日：2024-06-14

公开（公告）号：CN113959434B

主分类号：G01C21/16

分类号：G01C21/16;G01C21/20

优先权：

专利状态码：有效-授权

法律状态：2024.06.14#授权;2022.02.15#实质审查的生效;2022.01.21#公开

摘要：本发明提出了一种可调节的SINS、DVL、USBL组合导航方法，能够根据各种实际情况调整运行模式，实现优势互补，提高整个系统的工作适应性，完成各种情况下的组合导航。本发明在SINS、DVL、USBL组合导航的实际使用中，通过对不同的情况采用可调节的灵活组合导航方式，获得惯导误差的准确估计。该组合导航可以实现优势互补，提高整个系统的工作适应性，完成各种情况下的组合导航，可调节的灵活组合导航方式有效减小了惯导误差，提高了组合导航系统的精度。

主权项：1.一种可调节的SINS、DVL、USBL组合导航方法，其特征在于，通过采集SINS的信息和全球定位系统的信息实现初始对准；对准完成后同步采集SINS的位置和速度、DVL的速度、USBL的位置数据进行组合导航；SINS、DVL以及USBL实时判断各自输出信息是否有效及输出信息有效时精度是否高于设定值，根据具体情况选择不同的滤波方式，具体如下：DVL和USBL量测数据均有效，且精度均高于设定值时，采用集中滤波形式进行SINS、DVL、USBL组合导航；其中，量测量为DVL的速度和SINS的速度之差以及USBL的位置和SINS的位置之差，采用经典的Kalman滤波方程解算获得状态估计值，反馈姿态、速度和位置并把估计状态值置零；集中滤波进行数据融合时对数据进行统一计算，实现对状态的最优估计；当DVL和USBL量测数据均有效，且至少一个量测数据精度不高于设定值时，转变成联邦滤波形式进行SINS、DVL和SINS、USBL组合导航；联邦滤波采用主滤波器和子滤波器形式，在主滤波器中将两个子滤波器的输入信息和主惯导的量测进行信息融合得到状态估计值，反馈姿态、速度和位置并把估计状态值置零；当DVL量测数据有效，USBL量测数据无效时，转变成SINS、DVL组合导航，量测量为DVL的速度和SINS的速度之差，采用经典的Kalman滤波方程解算获得状态估计值，反馈姿态、速度和位置并把估计状态值置零；当DVL量测数据无效，USBL量测数据有效时，转变成SINS、USBL组合导航，量测量为USBL的位置和SINS的位置之差，采用经典的Kalman滤波方程解算获得状态估计值，反馈姿态、速度和位置并把估计状态值置零；当DVL和USBL量测数据均无效时，不进行组合导航，转变成SINS导航，等待DVL或USBL量测再次有效时进入组合导航状态；所述组合导航方法中，组合导航的状态变量包括姿态误差、速度误差、位置误差、陀螺漂移和加速度计零偏；不同条件下的滤波器的状态变量均为15维，如下所示； 其中，分别代表惯性导航失准角误差中的东向失准角、北向失准角、天向失准角误差，δVE、δVN、δVU分别代表惯性导航速度误差中东向速度误差、北向速度误差和天向速度误差；δL、δλ、δh分别代表惯性导航位置误差中的纬度误差、经度误差和高度误差；εE、εN、εU分别代表x轴陀螺、y轴陀螺和z轴陀螺的随机常值漂移；分别代表x轴加速度计、y轴加速度计和z轴加速度计的随机常值零偏；Kalman滤波系统的系统方程为： 其中，X为系统误差状态变量，W为系统噪声变量，F为系统状态转移矩阵，G为系统噪声转换矩阵；W＝[wgxwgywgzwaxwaywaz]T其中，wgx、wgy、wgz分别为x轴陀螺、y轴陀螺和z轴陀螺的噪声；wax、way、waz和分别为x轴加速度计，y轴加速度计和z轴加速度计的噪声； 其中，是载体系到导航系的姿态变换矩阵； 系统状态转移矩阵中各子矩阵表示如下： 其中，RMh为地球的子午圈主曲率半径，RNh为地球的卯酉圈主曲率半径，ωie为地球自转角速度，L为当地的纬度，分别为惯导的东速、北速和天速，分别为惯导地理坐标系下三个方向的比力；量测方程为：Z＝HX+V；系统状态方程及量测方程的离散化为：Xk＝φk,k-1Xk-1+Γk-1Wk-1Zk＝HkXk+Vk式中，Xk为k时刻的状态向量，也就是被估计矢量，Xk-1为k-1时刻的状态向量，φk,k-1为k-1时刻到k时刻的一步状态转移矩阵，Γk-1是系统噪声输入矩阵，Wk-1为k-1时刻的系统噪声；Zk为k时刻的测量序列，Hk为k时刻的测量矩阵，Vk为k时刻的测量噪声序列；状态转移矩阵：φk,k-1＝I+Fk-1Δt+…其中，I是信息矩阵，为P-1矩阵；利用标准卡尔曼滤波方程计算状态的最优估计如下：状态一步预测向量：Xkk-1＝φk,k-1Xk-1；滤波增益：状态估值计算：Xk＝Xkk-1+KkZk-HkXkk-1；一步预测均方误差矩阵：估计均方误差方程：其中，Rk是量测噪声序列的方差阵，Qk-1是量测噪声序列的方差阵；采用集中滤波时，Z和H如下所示：

全文数据：

权利要求：

百度查询： 河北汉光重工有限责任公司 一种可调节的SINS、DVL、USBL组合导航方法

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

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