[发明专利]一种基于惯导/DVL/USBL组合的惯导误差估计方法

说明书

本发明属于惯导技术，具体公开一种基于惯导/DVL/USBL组合的惯导误差估计方法，包括同步采集惯导的位置和速度、DVL的速度、USBL的位置等测试数据，并设计三个通过经典Kalman滤波实现的滤波器的配合：采用子滤波器1实现惯导/DVL组合、采用子滤波器2实现惯导/USBL组合，采用主滤波器，获得惯导误差的高精度估计；从而实现了高环境适应性，低位置累计误差的惯导误差估计方法。

技术领域

本发明属于惯导技术，具体涉及一种基于惯导/DVL/USBL组合的惯导误差估计方法。

背景技术

惯导/DVL组合可以实现远距离的组合导航，但位置误差累积。惯导/USBL组合可以实现位置误差不累积的组合导航，然而受USBL作用范围限制，不易实现远距离的组合导航，一般只能在特定水域工作。惯导/DVL/USBL组合可以实现优势互补，提高整个系统的工作适应性。现有技术中并没有一种基于惯导/DVL/USBL组合的惯导误差估计方法。

发明内容

本发明的目的在于提供一种基于惯导/DVL/USBL组合的惯导误差估计方法。

实现本发明目的的技术方案：

一种基于惯导/DVL/USBL组合的惯导误差估计方法，它包括下列步骤：

步骤1：同步采集惯导的位置和速度、DVL的速度、USBL的位置等测试数据；

步骤2：采用子滤波器1实现惯导/DVL组合；

子滤波器1的输入信息为惯导的速度与DVL的速度；子滤波器1的状态变量为17维向量：

其中，φ_N、φ_U、φ_E分别为惯导的北向、天向、东向轴失准角；δv_N、δv_U、δv_E分别为惯导的北向、天向、东向轴速度误差；δh、δλ分别为惯导的纬度、高度和经度误差；ε_N、ε_U、ε_E和▽_N、▽_U、▽_E分别为惯导的等效北向、天向、东向轴陀螺漂移和加速度计零偏；δθ_s为DVL的安装误差角，δk_s为DVL的刻度系数误差；

经过经典的Kalman滤波方程解算，获得子滤波器1的状态估计值和估计均方误差阵；取子滤波器1的状态估计值的前15个变量，记作估计均方误差阵的前15行前15列矩阵，记作P₁；

步骤3：采用子滤波器2实现惯导/USBL组合；

子滤波器2的输入信息为惯导的位置与USBL的位置；子滤波器2的状态变量为17维向量：

其中，φ_N、φ_U、φ_E分别为惯导的北向、天向、东向轴失准角；δv_N、δv_U、δv_E分别为惯导的北向、天向、东向轴速度误差；δh、δλ分别为惯导的纬度、高度和经度误差；ε_N、ε_U、ε_E和▽_N、▽_U、▽_E分别为惯导的等效北向、天向、东向轴陀螺漂移和加速度计零偏；δθ_a为USBL的安装误差角，δk_a为USBL的刻度系数误差；

经过经典的Kalman滤波方程解算，获得子滤波器2的状态估计值和估计均方误差阵；取子滤波器2的状态估计值的前15个变量，记作估计均方误差阵的前15行前15列矩阵，记作P₂；