[发明专利]SINS/DVL/USBL水下紧组合导航方法及装置、水下载体控制设备

说明书

本发明公开了一种SINS/DVL/USBL水下紧组合导航方法及装置、水下载体控制设备，包括：步骤1，将SINS解算的水下载体的速度信息转化为DVL坐标系下的原始波束速度、及将SINS解算的水下载体的位置信息转化为USBL声学基阵坐标系下的斜距和方位角；步骤2，将SINS速度转换的原始波束速度和DVL量测的原始波束速度输入SINS/DVL子滤波器中，输出第一状态估计和第一协方差阵；步骤3，将SINS位置转换的斜距和方位角和USBL量测的原始斜距、方位角输入SINS/USBL子滤波器中，输出第二状态估计和第二协方差阵；步骤4，将状态估计和协方差阵与输入SINS/DVL/USBL主滤波器，生成的全局最优估计值及其相应的协方差阵，按照预设分配规则重置子滤波器的状态估计值，本发明能够实现对SINS导航解算信息的修正，保证精确导航信息输出，解决DVL或USBL信息缺失的问题。

技术领域

本发明涉及一种水下导航技术领域，尤其是涉及一种SINS/DVL/USBL水下紧组合导航方法及装置、水下载体控制设备。

背景技术

相对陆空导航，水下导航因具有环境复杂、信息源少、隐蔽性要求高等特点而格外困难。目前国际常用的水下导航定位技术手段有惯性导航技术、多普勒测速仪(DVL，Doppler Velocity Log)、水声定位技术以及组合导航技术等。单一的导航装置由于自身存在的不足，已无法满足现代导航装置高精度高可靠性的要求。例如，捷联惯导系统(SINS，strap-down inertial navigation system)能够为水下运载体提供完备导航信息，但其误差随时间积累。多普勒测速仪虽能够测得高精度的速度信息，但位置误差仍随航程积累。超短基线(USBL)水声定位装置虽能够量测应答器相对于基阵的相对位置信息，但其作用范围有限，只能在特定范围内使用。

组合导航技术利用两种或两种以上的非相似导航装置对同一导航信息做量测并解算以形成量测量。从这些量测量中计算出各惯导的误差并校正，能够提高水下载体的导航精度。现有的组合导航方式有SINS/DVL组合导航和SINS/USBL组合导航。

SINS/DVL组合导航是一种速度组合的模式，虽然能够获得水下载体实时高精度的速度信息，但位置误差仍随时间积累。且如果遇见一些特殊情况，如DVL受载体姿态角动态影响的情况和DVL只有受限数量的波束量测可用的情况，SINS/DVL松组合导航无法使用。SINS/USBL组合导航能够有效抑制导航装置位置误差的发散，但又受水声换能器作用距离的限制，常用于区域范围内的高精度定位。且当USBL部分观测信息缺失时，SINS/USBL松组合导航不足以满足定位任务的要求。

发明内容

本发明的目的是提供一种SINS/DVL/USBL水下紧组合导航方法及装置，能够集合SINS/DVL组合和SINS/USBL组合的优点，且能够在缺失传感器信息的情况下实现水下载体长续航高精度导航。

为实现上述目的，本发明提供一种SINS/DVL/USBL水下紧组合导航方法，其包括：

步骤1，通过SINS公共参考系统将解算的水下载体的速度信息V_n＝[V_E V_N V_U]^T转化为DVL坐标系下的原始波束速度以及将所述解算的水下载体的位置信息[L λ h]^T转化为USBL声学基阵坐标系下的斜距和方位角[α_s β_s r_s]；

步骤2，将步骤1获得的原始波束速度和DVL量测得到的原始波束速度两者输入到SINS/DVL子滤波器中进行组合，输出第一状态估计和第一协方差阵；