CN114236173A - 基于sins与gnss组合及dvl的洋流速度估计方法 - Google Patents

Abstract

本发明提供了一种基于SINS与GNSS组合及DVL的洋流速度估计方法，由SINS求解出运载体的速度和位置信息；设置SINS与GNSS组合导航的状态变量为12维误差状态量，并构建***误差状态方程；将GNSS提供的速度和位置信息与SINS解算的速度和位置信息作差，作为卡尔曼滤波的观测量，并建立量测方程；利用卡尔曼滤波校正后的最终时刻姿态信息，将DVL测速结果投影至导航坐标系；将卡尔曼滤波输出校正后的导航速度与相同时刻DVL投影在导航坐标系下的速度作差，得到洋流估计速度。本发明根据卡尔曼滤波校正后的最终时刻高精度姿态信息，计算得到高精度的姿态矩阵，利于将DVL测量的速度投影至导航系，采用DVL测速辅助，求解出洋流速度，精度可提高至洋流速度的0.1％。

Description

基于SINS与GNSS组合及DVL的洋流速度估计方法

技术领域

本发明涉及信息测量技术领域，具体涉及一种洋流速度估计方法，更具体是一种基于SINS与GNSS组合及DVL的洋流速度估计方法。

背景技术

捷联惯导***/全球卫星导航***(SINS/GNSS)组合导航中，通常用多普勒测速仪(DVL)提供运载体的外参考速度。目前船用DVL多为相对计程仪，当计程仪与海底距离大于计程仪的量程时，计程仪的输出为载体相对于水流层的速度，计程仪的测速精度受到洋流影响，必须想办法抑制洋流对导航***造成的影响。

对洋流速度估算，然后进行洋流补偿能有效抑制洋流对导航***带来的影响，而且洋流在气象中主要影响气温和降水，在实际生活中可以为渔业和航运提供参考，所以洋流速度的估算方法具有重要的研究价值。

申请号为CN201310122146.9的中国专利公开了一种组合导航洋流速度测量方法，该方法需要提前在电子海图或洋流数据库中查找航行海域内洋流模型参数，具有一定局限性。其在卡尔曼滤波中，将惯导***求出的速度信息与DVL测速信息作为滤波观测量，会由于洋流速度的存在而导致导航结果发散。

综上所述，急需一种基于SINS与GNSS组合及DVL的洋流速度估计方法以解决现有技术中存在的问题。

发明内容

本发明目的在于提供一种基于SINS与GNSS组合及DVL的洋流速度估计方法，旨在解决现有洋流速度测量方法需要先获得航行海域内洋流模型参数，具有一定局限性，以及采用纯惯导解算误差较大的问题，具体技术方案如下：

一种基于SINS与GNSS组合及DVL的洋流速度估计方法，包括以下步骤：

步骤S1：由SINS求解出运载体的速度和位置信息；

步骤S2：设置SINS与GNSS组合导航的状态变量为12维误差状态量，并构建卡尔曼滤波的***误差状态方程；

步骤S3：将GNSS提供的速度和位置信息与SINS解算的速度和位置信息作差，作为卡尔曼滤波的观测量，并建立量测方程；

步骤S4：利用卡尔曼滤波校正后的最终时刻姿态信息，将DVL测速结果投影至导航坐标系；

步骤S5：将卡尔曼滤波输出校正后的导航速度与相同时刻DVL投影在导航坐标系下的速度作差，得到洋流估计速度。

以上技术方案中优选的，所述步骤S1中解算的速度信息包括东向速度v_e和北向速度v_n，解算的位置信息包括纬度L和经度λ。

以上技术方案中优选的，所述步骤S2中，12维误差状态量如式1)：

其中，φ_x、φ_y和φ_z分别为地理坐标系东北天三个方向的姿态失准角，δv_e和δv_n分别为导航坐标系下东向和北向速度误差，δL和δλ分别为纬度误差和经度误差，ε_x、ε_y和ε_z分别为东北天三个方向的陀螺漂移，

和

分别为东北两个方向的加速度计零偏。

以上技术方案中优选的，所述步骤S2中，***误差状态方程如式2)：

其中，F为状态矩阵，W为***噪声。

以上技术方案中优选的，W为零均值的高斯白噪声，如式3)：

W＝[ω_gx ω_gy ω_gz ω_ax ω_ay 0 0 0 0 0 0 0]^T 式3)，

其中ω_gx、ω_gy和ω_gz分别为陀螺仪东北天三个方向的噪声，ω_ax和ω_ay为加速度计东向和北向的噪声。

以上技术方案中优选的，其中，状态矩阵F如式4)：

具体地，式4)中：

其中，ω_ie为地球自转角速度，v_e、v_n分别为SINS求解的东向速度和北向速度，L、λ分别为SINS求解的纬度和经度，R_M、R_N分别为运载体所在点子午圈和卯酉圈曲率半径，f_e、f_n、f_u分别为运载体沿东北天三个方向的加速度，

为姿态矩阵。

以上技术方案中优选的，所述步骤S3中，量测方程如式5)：

其中，L、λ、h分别为SINS解算的纬度、经度及高度，v_e、v_n、v_u分别为SINS解算的东向、北向和天向速度；L_GPS、λ_GPS、h_GPS分别为GNSS提供的纬度、经度和高度信息，v_ge、v_gn、v_gu分别为GNSS提供的东向速度、北向速度和天向速度；V为测量噪声向量，H为观测矩阵。

以上技术方案中优选的，式5)中观测矩阵H为：

其中，

以上技术方案中优选的，步骤S4根据式6)将DVL测速结果投影至导航坐标系：

其中，

分别为DVL测量的速度在载体系和导航系的投影。

以上技术方案中优选的，步骤S5中根据式7)计算洋流估计速度：

其中，

分别为估计的东向和北向洋流速度，v'_e、v'_n分别为SINS与GNSS组合导航经卡尔曼滤波后解算的导航东向速度、导航北向速度，

分别为DVL测量的东向速度和北向速度在导航系下的投影，δv_se为估算东向洋流速度与真实东向洋流速度误差值，δv_sn为估算北向洋流速度与真实北向洋流速度误差值，θ为洋流的方向，v_s为估计的洋流速度大小。

应用本发明的技术方案，具有以下有益效果：

本发明的估计方法根据卡尔曼滤波校正后的最终时刻的高精度姿态信息，计算得到高精度的姿态矩阵，利于将DVL测量的速度投影至导航系，采用DVL测速辅助，通过引入外速度信息，求解出洋流速度。相对于现有的组合导航洋流速度测量方法，本发明的估计方法精度可提高至洋流速度的0.1％，有效地提高洋流速度估计精度。

除了上面所描述的目的、特征和优点之外，本发明还有其它的目的、特征和优点。下面将参照图，对本发明作进一步详细的说明。

附图说明

构成本申请的一部分的附图用来提供对本发明的进一步理解，本发明的示意性实施例及其说明用于解释本发明，并不构成对本发明的不当限定。在附图中：

图1是本发明估计方法的流程图；

图2是试验案例中东向洋流速度估计误差示意图；

图3是试验案例中北向洋流速度估计误差示意图。

具体实施方式

为了便于理解本发明，下面将对本发明进行更全面的描述，并给出了本发明的较佳实施例。但是，本发明可以以许多不同的形式来实现，并不限于本文所描述的实施例。相反地，提供这些实施例的目的是使对本发明的公开内容的理解更加透彻全面。

除非另有定义，本文所使用的所有的技术和科学术语与属于本发明的技术领域的技术人员通常理解的含义相同。本文中在本发明的说明书中所使用的术语只是为了描述具体的实施例的目的，不是旨在于限制本发明。

实施例1：

参见图1，一种基于SINS(捷联惯导***)与GNSS(全球卫星导航***)组合及DVL(多普勒测速仪)的洋流速度估计方法，针对对水工作模式下DVL测速误差主要来自洋流速度误差，利用SINS/GNSS组合导航和卡尔曼滤波技术得到高精度姿态信息(即更新姿态矩阵)，将DVL速度投影至导航坐标系，再与组合导航得到的速度做差，可以估算出这片海域洋流速度的大小和方向，具体包括以下步骤：

步骤S1：由SINS求解出运载体(即舰船)的速度和位置信息；

具体地，所述步骤S1中解算的速度信息包括东向速度v_e和北向速度v_n，解算的位置信息包括纬度L和经度λ。

步骤S2：设置SINS与GNSS组合导航的状态变量为12维误差状态量，并构建卡尔曼滤波的***误差状态方程；

具体地，所述步骤S2中，12维误差状态量如式1)：

其中，φ_x、φ_y和φ_z分别为地理坐标系东北天三个方向的姿态失准角，δv_e和δv_n分别为导航坐标系下东向和北向速度误差，δL和δλ分别为纬度误差和经度误差，ε_x、ε_y和ε_z分别为东北天三个方向的陀螺漂移，

和

分别为东北两个方向的加速度计零偏。

优选的，所述步骤S2中，***误差状态方程如式2)：

其中，F为状态矩阵，W为***噪声。

具体地，W为零均值的高斯白噪声，如式3)：

W＝[ω_gx ω_gy ω_gz ω_ax ω_ay 0 0 0 0 0 0 0]^T 式3)，

其中ω_gx、ω_gy和ω_gz分别为陀螺仪东北天三个方向的噪声，ω_ax和ω_ay为加速度计东向和北向的噪声。

优选的，状态矩阵F如式4)：

具体地，式4)中：

其中，ω_ie为地球自转角速度，v_e、v_n分别为SINS求解的东向速度和北向速度，L、λ分别为SINS求解的纬度和经度，R_M、R_N分别为运载体所在点子午圈和卯酉圈曲率半径，f_e、f_n、f_u分别为运载体沿东北天三个方向的加速度，

为姿态矩阵，此处

为当前时刻根据卡尔曼滤波校正后的姿态信息获得的姿态矩阵；其中卡尔曼滤波为本领域人员的公知常识，其具体计算内容在此不再进一步说明。

步骤S3：将GNSS提供的速度和位置信息与SINS解算的速度和位置信息作差，作为卡尔曼滤波的观测量，并建立量测方程；

具体地，所述步骤S3中，量测方程如式5)：

其中，L、λ、h分别为SINS解算的纬度、经度及高度，v_e、v_n、v_u分别为SINS解算的东向、北向和天向速度；L_GPS、λ_GPS、h_GPS分别为GNSS提供的纬度、经度和高度信息，v_ge、v_gn、v_gu分别为GNSS提供的东向速度、北向速度和天向速度；V为测量噪声向量，H为观测矩阵。

优选的，观测矩阵H为：

进一步优选的，

步骤S4：利用卡尔曼滤波校正后的最终时刻姿态信息，将DVL测速结果投影至导航坐标系；

步骤S4根据式6)将DVL测速结果投影至导航坐标系：

其中，

分别为DVL测量的速度在载体系和导航系的投影，式6)中

为姿态矩阵，此处

为最终时刻根据卡尔曼滤波校正后的姿态信息获得的姿态矩阵。

步骤S5：将卡尔曼滤波输出校正后的导航速度与相同时刻DVL投影在导航坐标系下的速度作差，得到洋流估计速度(导航速度具体是：将SINS的位置和速度信息与GNSS的位置和速度信息经卡尔曼滤波校正后输出导航速度)。

步骤S5中根据式7)计算洋流估计速度：

其中，

分别为估计的东向和北向洋流速度，v'_e、v'_n分别为SINS与GNSS组合导航经卡尔曼滤波后解算的导航东向速度、导航北向速度，

分别为DVL测量的东向速度和北向速度在导航系下的投影，δv_se为估算东向洋流速度与真实东向洋流速度误差值，δv_sn为估算北向洋流速度与真实北向洋流速度误差值，θ为洋流的方向，v_s为估计的洋流速度大小。

试验案例：

对本实施例的方法进行仿真验证，仿真试验条件设置为：

舰船以5m/s的速度匀速向北运动，洋流速度设置为2m/s，方向为地理坐标系北偏东60°，仿真时长1h，惯性器件采样频率为20Hz，DVL和GNSS采样频率为1Hz；

陀螺常值误差ε_x＝ε_y＝ε_z＝0.003°/h，陀螺仪随机误差为

加速度常值误差

加速度计随机误差为

GNSS定位误差为0.1m，速度误差为0.01m/s；DVL测速精度为0.2％±0.2cm/s，有效量程为425m～500m。

本实施例方法估计的洋流速度与设置的洋流速度误差仿真结果如图2、图3所示。仿真时间设置为1h，且DVL和GNSS的采样周期为1s，所以求出了3600个洋流速度，可以看出0.2h后洋流速度估计值基本稳定，对0.2h后的东向和北向的洋流速度求和取平均值得到，

则洋流的流向可以估算为地理坐标系北偏东60.29°。东向和北向洋流速度误差分别为0.0015m/s和0.0095m/s，洋流速度误差不到真实洋流速度的0.1％。

以上所述仅为本发明的优选实施例而已，并不用于限制本发明，对于本领域的技术人员来说，本发明可以有各种更改和变化。凡在本发明的精神和原则之内，所作的任何修改、等同替换、改进等，均应包含在本发明的保护范围之内。

Claims (10)

1.一种基于SINS与GNSS组合及DVL的洋流速度估计方法，其特征在于，包括以下步骤：

步骤S1：由SINS求解出运载体的速度和位置信息；

步骤S2：设置SINS与GNSS组合导航的状态变量为12维误差状态量，并构建卡尔曼滤波的***误差状态方程；

步骤S3：将GNSS提供的速度和位置信息与SINS解算的速度和位置信息作差，作为卡尔曼滤波的观测量，并建立量测方程；

步骤S4：利用卡尔曼滤波校正后的最终时刻姿态信息，将DVL测速结果投影至导航坐标系；

步骤S5：将卡尔曼滤波输出校正后的导航速度与相同时刻DVL投影在导航坐标系下的速度作差，得到洋流估计速度。

2.根据权利要求1所述的基于SINs与GNSS组合及DVL的洋流速度估计方法，其特征在于，所述步骤S1中解算的速度信息包括东向速度v_e和北向速度v_n，解算的位置信息包括纬度L和经度λ。

3.根据权利要求1所述的基于SINS与GNSS组合及DVL的洋流速度估计方法，其特征在于，所述步骤S2中，12维误差状态量如式1)：

其中，φ_x、φ_y和φ_z分别为地理坐标系东北天三个方向的姿态失准角，δv_e和δv_n分别为导航坐标系下东向和北向速度误差，δL和δλ分别为纬度误差和经度误差，ε_x、ε_y和ε_z分别为东北天三个方向的陀螺漂移，

和

分别为东北两个方向的加速度计零偏。

4.根据权利要求3所述的基于SINS与GNSS组合及DVL的洋流速度估计方法，其特征在于，所述步骤S2中，***误差状态方程如式2)：

其中，F为状态矩阵，W为***噪声。

5.根据权利要求4所述的基于sINs与GNSS组合及DVL的洋流速度估计方法，其特征在于，W为零均值的高斯白噪声，如式3)：

W＝[ω_gx ω_gy ω_gz ω_ax ω_ay 0 0 0 0 0 0 0]^T 式3)，

其中ω_gx、ω_gy和ω_gz分别为陀螺仪东北天三个方向的噪声，ω_ax和ω_ay为加速度计东向和北向的噪声。

6.根据权利要求5所述的基于SINS与GNSS组合及DVL的洋流速度估计方法，其特征在于，其中，状态矩阵F如式4)：

具体地，式4)中：

其中，ω_ie为地球自转角速度，v_e、v_n分别为SINS求解的东向速度和北向速度，L、λ分别为SINS求解的纬度和经度，R_M、R_N分别为运载体所在点子午圈和卯酉圈曲率半径，f_e、f_n、f_u分别为运载体沿东北天三个方向的加速度，

为姿态矩阵。

7.根据权利要求1所述的基于SINS与GNSS组合及DVL的洋流速度估计方法，其特征在于，所述步骤S3中，量测方程如式5)：

其中，L、λ、h分别为SINS解算的纬度、经度及高度，v_e、v_n、v_u分别为SINS解算的东向、北向和天向速度；L_GPS、λ_GPS、h_GPS分别为GNSS提供的纬度、经度和高度信息，v_ge、v_gn、v_gu分别为GNSS提供的东向速度、北向速度和天向速度；V为测量噪声向量，H为观测矩阵。

8.根据权利要求7所述的基于SINs与GNSS组合及DVL的洋流速度估计方法，其特征在于，式5)中观测矩阵H为：

其中，

9.根据权利要求6所述的基于SINS与GNSS组合及DVL的洋流速度估计方法，其特征在于，步骤S4根据式6)将DVL测速结果投影至导航坐标系：

其中，

分别为DVL测量的速度在载体系和导航系的投影。

10.根据权利要求1所述的基于SINS与GNSS组合及DVL的洋流速度估计方法，其特征在于，步骤S5中根据式7)计算洋流估计速度：

其中，

分别为估计的东向和北向洋流速度，v′_e、v′_n分别为SINS与GNSS组合导航经卡尔曼滤波后解算的导航东向速度、导航北向速度，

分别为DVL测量的东向速度和北向速度在导航系下的投影，δv_se为估算东向洋流速度与真实东向洋流速度误差值，δV_sn为估算北向洋流速度与真实北向洋流速度误差值，θ为洋流的方向，v_s为估计的洋流速度大小。

Images