申请/专利权人：湖南航天机电设备与特种材料研究所

申请日：2020-09-14

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

公开（公告）号：CN112083425B

主分类号：G01S15/06

分类号：G01S15/06;G01S15/87;G01C21/16

优先权：

专利状态码：有效-授权

法律状态：2024.03.12#授权;2021.01.01#实质审查的生效;2020.12.15#公开

摘要：本发明公开了一种引入径向速度的SINSLBL紧组合导航方法，在LBL水声定位系统的应答器阵列中，每个应答器的应答信号频率不同，应答信号的理论频率和实际接收频率之差为多普勒频率，它反映了水下航行器与应答器在径向方向的相对运动关系，即径向速度；在传统的SINSLBL紧组合中引入SINS和LBL计算的径向速度作为扩展卡尔曼滤波器的观测量，与速度相关的直接观测量的引入增加了速度误差的可观测性，提高了速度误差的估计精度和组合导航精度。

主权项：1.一种引入径向速度的SINSLBL紧组合导航方法，其特征在于，包括：步骤1：在水下航行器上安装SINS，在海底布放LBL水声定位系统的应答器阵列，并精确标定每个应答器的位置；步骤2：在LBL水声定位系统中，根据声信号在水下航行器与应答器之间的传播时延，计算出水下航行器与应答器之间的传播距离、以及声信号到达两个不同应答器的传播距离之差；根据实际接收到的应答信号以及设置的参考信号，计算出水下航行器与应答器在径向方向的相对运动速度；步骤3：在SINS中，计算出水下航行器在导航坐标系下的姿态和速度以及水下航行器在地球坐标系下的位置，并将水下航行器在地球坐标系下的位置转换成地球直角坐标；根据所述步骤1中已标定的应答器的位置计算出水下航行器与应答器之间的传播距离、以及声信号到达两个不同应答器的传播距离之差；步骤4：根据所述步骤3中SINS计算的水下航行器在导航坐标系下的速度计算出水下航行器与应答器在径向方向的相对运动速度；步骤5：由SINS的姿态误差、速度误差、位置误差、陀螺漂移误差以及加速度计偏置误差构成扩展卡尔曼滤波器的状态变量；根据所述状态变量建立状态方程；由所述步骤2中LBL水声定位系统计算出的传播距离、传播距离之差和相对运动速度，步骤3中SINS计算出的传播距离和传播距离之差以及步骤4中计算出的相对运动速度构成扩展卡尔曼滤波器的观测量；根据所述状态变量和观测量建立观测方程；步骤6：根据所述步骤5中的状态方程和观测方程估计出SINS的姿态误差、速度误差、位置误差、陀螺漂移误差以及加速度计偏置误差；步骤7：根据所述步骤6中估计出的姿态误差、速度误差、位置误差、陀螺漂移误差以及加速度计偏置误差对所述SINS输出的姿态、速度、位置、陀螺漂移和加速度计偏置进行修正，得到修正后的姿态、速度、位置、陀螺漂移和加速度计偏置；步骤8：重置所述扩展卡尔曼滤波器的状态变量，循环执行步骤2～7，直到水下航行器作业结束；所述步骤5中，状态方程为：Xk+1＝Fk+1kXk+wk+1其中，Xk、Xk+1分别为k时刻、k+1时刻的状态变量，Fk+1k为状态一步转移矩阵，wk+1为系统过程噪声；状态变量φ为SINS的姿态误差，φ＝[φEφNφU]T，φE、φN和φU分别为SINS的俯仰角误差、横滚角误差和航向角误差，δν为SINS的速度误差，δν＝[δvEδvNδvU]T，δvE、δvN和δvU分别为SINS的东向速度误差、北向速度误差和天向速度误差，δp为SINS的位置误差，δp＝[δLδλδh]T，δL、δλ和δh分别为SINS的纬度误差、经度误差和高度误差，ε为SINS的陀螺漂移误差，ε＝[εxεyεz]T，εx、εy和εz分别为SINS的陀螺在载体坐标系x、y、z轴的漂移误差，为SINS的加速度计偏置误差，和分别为SINS的加速度计在载体坐标系x、y、z轴的偏置误差；观测方程为：Zk+1＝Hk+1Xk+1+vk+1其中，Zk+1为k+1时刻的观测量，Hk+1为k+1时刻的观测矩阵，Xk+1为k+1时刻的状态变量，vk+1为系统观测噪声，观测量为LBL水声定位系统计算的水下航行器与第i个应答器之间的传播距离，为SINS计算的水下航行器与第i个应答器之间的传播距离，为LBL水声定位系统计算的声信号到达第i个应答器和第j个应答器的传播距离之差，为SINS计算的声信号到达第i个应答器和第j个应答器的传播距离之差，为LBL水声定位系统计算的水下航行器与第i个应答器在径向方向的相对运动速度，为SINS计算的水下航行器与第i个应答器在径向方向的相对运动速度；k+1时刻的观测矩阵 CVVi＝[cosζxcosζycosζz]， 其中，RN为地球子午圈半径，Re为地球半径，e为地球偏心率，a和b分别为地球椭圆长轴和短轴半径，h为SINS计算的水下航行器在地球坐标系下的高度，L为SINS计算的水下航行器在地球坐标系下的纬度，λ为SINS计算的水下航行器在地球坐标系下的经度，0i×3为i×3维零向量，Xs＝[xsyszs]T，Xs为SINS计算的水下航行器的地球直角坐标，xs,ys,zs分别为SINS计算的水下航行器的地球直角坐标的分量，Xi＝[xiyizi]T，Xi为步骤1中第i个应答器的地球直角坐标，xi,yi,zi分别为第i个应答器的地球直角坐标的分量，vE为SINS计算的水下航行器在导航坐标系下的东向速度，vN为SINS计算的水下航行器在导航坐标系下的北向速度，vU为SINS计算的水下航行器在导航坐标系下的天向速度。

全文数据：

权利要求：

百度查询： 湖南航天机电设备与特种材料研究所 一种引入径向速度的SINS/LBL紧组合导航方法

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

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