CN103090884B - 基于捷联惯导系统的多普勒计程仪测速误差抑制方法 - Google Patents

Abstract

本发明提供的是一种基于捷联惯导系统的多普勒计程仪测速误差抑制方法。启动多普勒计程仪与捷联惯导系统，将多普勒计程仪(DVL)输出速度与捷联惯导系统(INS)输出速度进行综合处理，并经过进行高通数字滤波，可输出优化后的速度。本发明能在不滤除运动高频信息的同时滤除多普勒计程仪高频噪声，为其它惯性系统初始对准、导航提供速度和加速度参考。

Description

基于捷联惯导系统的多普勒计程仪测速误差抑制方法

技术领域

本发明涉及的是一种测速误差抑制方法，特别是涉及一种多普勒计程仪的测速误差抑制方法。

背景技术

多普勒计程仪是一种常用船用计程仪，它是利用声波或超声波在水中的多普勒效应制成的计程仪，由于它可以直接测量船舶对地的速度，并且稳定可靠，所以近年来受到广泛应用。在船舶导航系统中，多普勒计程仪（DVL）测量速度通常用作辅助捷联罗经系统，DVL测量速度及其测量速度的差分值（加速度）提供给捷联罗经系统，这样可以去除船舶速度和加速度对于捷联罗经系统导航的影响。但是多普勒计程仪输出的速度中含有大量的噪声高频分量，这样会影响到对于捷联罗经系统的导航解算。

目前针对多普勒计程仪测速问题，国内已有人开始研究，并且提出了一些不错的方法，傅菊英在《海洋技术》发表的《声学多普勒计程仪测速误差分析与补偿》。这篇文章主要对多普勒计程仪的安装偏差、波束方向误差和电路设计误差等固定误差、通过标定试验给出多普勒计程仪的补偿系数并验证其有效性。但是他的标定实在实验室的环境下进行标定，属于事先标定，不能满足实时性的要求，并且精度较差。由于是针对固定误差的标定，所以灵活性也较差。

发明内容

本发明的目的在于提供一种既能很好的滤去多普勒中的高频误差分量，又能避免滤去船舶真实运动信息中的高频部分的基于捷联惯导系统的多普勒计程仪测速误差抑制方法。

本发明的目的是这样实现的：

步骤1：向船舶的多普勒计程仪和船舶捷联惯导系统中输入初始位置信息，并且采集陀螺和加速度计的输出信息；

步骤2：通过步骤1中的初始位置信息和采集的陀螺、加速度计的输出信息，获得初始的捷联惯导系统捷联矩阵

步骤3：捷联惯导系统进入导航工作阶段，实时解算k时刻的捷联姿态矩阵与导航坐标系下的速度Vⁿ(k)；

步骤4：多普勒计程仪实时输出第k时刻的多普勒速度V_dvl(k)；

步骤5：由捷联惯导系统测量得到导航坐标系下的速度Vⁿ(k)，并通过Vⁿ(k)测量得到k时刻的船舶前进速度

步骤6：由步骤4中与步骤5中测量得到ΔV(k)，

$\mathrm{\ΔV}\left(k\right)=V_{\mathrm{dvl}}\left(k\right)-V_y^b\left(k\right);$

步骤7：对ΔV(k)进行高通数字滤波得到

$\tilde{V}\left(k\right)=W\left(k\right)\mathrm{\ΔV}\left(k\right);$

步骤8：将补偿到多普勒速度中，得到优化后的多普勒速度U_dvl(k)

$U_{\mathrm{dvl}}\left(k\right)=V_{\mathrm{dvl}}\left(k\right)-\tilde{V}\left(k\right).$

本发明还可以包括：

1、所述通过Vⁿ(k)测量得到k时刻的船舶前进速度的具体方法为：由航坐标系下的速度Vⁿ(k)与捷联姿态矩阵测量得到k时刻载体坐标系下的速度

$V^b\left(k\right)={\left[C_b^n\left(k\right)\right]}^T{V}^n\left(k\right),$

其中[]^T表示转置，V^b(k)的矢量形式为

$V^b\left(k\right)=\left[\begin{array}{c}{V}_x^b\left(k\right)\\ V_y^b\left(k\right)\\ V_z^b\left(k\right)\end{array}\right]$

其中为船体右舷方向速度、为垂直船舶甲板平面方向速度、为船舶前进速度。

2、所述高通数字滤波中选用高通数字FIR滤波器，选取凯塞窗为

$W\left[k\right]=\frac{I\left[14\sqrt{1-{(\frac{2k}{N-1}-1)}^2}\right]}{87}$

其中函数I[x]定义为：

$I\left[x\right]=1+\underset{j=1}{\overset{13}{\mathrm{\Σ}}}{\left[\frac{{(x/2)}^j}{j!}\right]}^2$

x为函数I[x]的自变量，取并且有

N＝10.4/f_s

其中f_s为采样频率。 

在本发明中，应保证捷联惯导系统与多普勒计程仪采样同步，并具有相同的采样频率。

在船用运行过程中，多普勒计算地理系上的测速误差方程如下（由于惯性系统计算中地理系未知，平台系即为计算地理系）：

$\mathrm{\δ}{V}^p=C_b^p\mathrm{\ΔC}\·V^b+u^p---\left(1\right)$

其中，δV^p为测速误差在平台系上的投影，u为广义噪声在平台系上的投影，u的振荡幅度很大，且组成形式也比较复杂。组成形主要有两部分，一部分为自身测量噪声，另一部分是复杂的海浪运动对多普勒测量的影响。为常系数误差，为捷联矩阵，V^b为速度在载体系上的投影，由于部分实质上也受到运动变化的影响，因此可以将δV^p的误差重新划分。

$\mathrm{\δ}{V}^p=\mathrm{\δ}{V}_{\mathrm{\ρ}}^p+\mathrm{\δ}{V}_r^p+\mathrm{\δ}{V}_{\mathrm{\Δc}}^p---\left(2\right)$

其中，表示多普勒本身的噪声，表示波浪所引起的测速误差噪声，表示多普勒刻度系数误差所引起的测速误差。

观察式（2），其中和直接可认定为高频误差；对于部分，由于船舶在航行中多数情况速度稳定，机动的情况比较少，因此可以认为该误差存在低频和高频两部分误差。令表示δV^p的高频部分，表示低频部分，则式（2）可以改写为：

$\mathrm{\δ}{V}^p=\mathrm{\δ}{V}_h^p+\mathrm{\δ}{V}_l^p---\left(3\right)$

由于式（3）中的高频误差的存在，利用多普勒速度差分计算出的加速度误差大幅增加，制约了捷联罗经系统中加速度的补偿计算。所以在实际运行过程中需要滤去高频部分。

但是在船舶运行过程中受舵角水动力、风浪干扰、以及机体震动的影响，所以不仅误差项δV^p包含高频项，船舶真实的运动速度V^p也包含了大量的高频部分，所以不能够直接对多普勒所测速度进行低通滤波。

但是惯性导航系统的速度误差与多普勒速度不同，为固有的长周期振荡形式，并且由于导航系统和多普勒共享一个惯性测量元件，所以提供的速度中包含的真实速度与多普勒中包含的真实运动速度V^p一致。两个速度相减就能消去真实速度V^p。并且导航速的速度误差频 率主要在84.4分钟附近，与多普勒高频部分的频段相差悬殊。

本发明利用这种双系统工作状态下的特性，用导航系统提供的船舶速度来优化多普勒测速误差，很好的滤去了多普勒中的高频误差分量，同时又避免了滤去船舶真实运动信息中的高频部分。

本发明提供了一种基于捷联惯导系统的多普勒计程仪（DVL）测速误差抑制方法，该方法可以在保留真实运动信息的基础上，对DVL噪声中的高频分量进行滤除，利用处理后的速度进行速度差分，测量得出的船舶运动加速度精度较高。本发明利用捷联惯导系统所提供的速度来优化多普勒速度测量值，去除多普勒速度测量值中的高频误差，使得加速度的影响可以得到补偿，从而提高导航系统的精度。

附图说明

图1a-图1b为本发明多普勒速度误差曲线图，其中图1a为东向速度误差；图1b为北向速度误差。

图2a-图2b为本发明优化后的多普勒速度误差图，其中图2a为东向优化速度误差；图2b为北向优化速度误差。

具体实施方式

下面举例对本发明作更详细的描述。

本发明涉及的坐标系有：p-数学平台坐标系；b-载体坐标系。两个坐标系之间的转换用方向余弦矩阵表示。

1、船舶启动，预热多普勒计程仪和船舶捷联惯导系统，输入初始位置信息，并且采集陀螺和加速度计的输出信息；

2、通过步骤1中的初始位置信息和陀螺、加速度计采集信息，获得初始的捷联惯导系统捷联矩阵

3、捷联惯导系统和多普勒计程仪进入导航工作阶段，实时输出信息；

4、多普勒计程仪DVL实时输出第k时刻的多普勒速度V_dvl(k)；

5、捷联惯导系统实时测量得到k时刻导航坐标系下的速度Vⁿ(k)；

$V^n\left(k\right)=\left[\begin{array}{c}{V}_E^n\left(k\right)\\ V_N^n\left(k\right)\\ V_U^n\left(k\right)\end{array}\right]$

其中上角标表示导航坐标系n系，下角标E、N、U分别表示导航坐标系的东轴、北轴和天轴，并且有k＝1,2,…,M,

6、捷联惯导系统进行实时更新测量，输出k时刻的捷联矩阵然后根据该捷联矩阵把导航坐标系上的速度转换为机体坐标系上的速度

$V^b\left(k\right)={\left[C_b^n\left(k\right)\right]}^T{V}^n\left(k\right)$

其中[]^T表示转置，V^b(k)的矢量形式为

$V^b\left(k\right)=\left[\begin{array}{c}{V}_x^b\left(k\right)\\ V_y^b\left(k\right)\\ V_z^b\left(k\right)\end{array}\right]$

其中为船体右舷方向速度，为垂直船舶甲板平面方向速度，为船舶前进速度，而电磁计程仪所记录速度为我们通常意义下船舶向正前方的前进速度，所以既为k时刻船舶纵向速度，既通常意义下的船舶速度，记录下

7、使导航速度和多普勒速度相减，得到 

$\mathrm{\ΔV}\left(k\right)=V_{\mathrm{dvl}}\left(k\right)-V_y^b\left(k\right)$

其中ΔV(k)为处理后的误差速度。

8、然后对误差速度ΔV(k)进行高通滤波处理，得到：

$\tilde{V}\left(k\right)=W\left(k\right)\mathrm{\ΔV}\left(k\right)$

$W\left[k\right]=\frac{I\left[14\sqrt{1-{(\frac{2k}{N-1}-1)}^2}\right]}{87}$

其中函数定义为：

$I_0\left[x\right]=1+\underset{j=1}{\overset{20}{\mathrm{\Σ}}}{\left[\frac{{(x/2)}^j}{j!}\right]}^2$

并且有

N＝5.2f_s/f

其中f_s为采样频率，f为通带边缘频率，在本发明中，应保证捷联惯导系统与多普勒计程仪采样同步，并具有相同的采样频率。

9、将补偿到DVL速度中：

$U_{\mathrm{dvl}}\left(k\right)=V_{\mathrm{dvl}}\left(k\right)-\tilde{V}\left(k\right)$

U_dvl(k)为优化后的DVL速度。

10、优化完成，利用U_dvl(k)去进行导航系统其他部分的计算。

根据图1a-图1b，图2a-图2b仿真结果图可以看出，优化后的多普勒速度误差去除了高频震荡误差，但是本身的运动信息完整的保留了下来，达到了本发明所需要的效果。

Claims (2)

1.一种基于捷联惯导系统的多普勒计程仪测速误差抑制方法，其特征是：

步骤1：向船舶的多普勒计程仪和船舶捷联惯导系统中输入初始位置信息，并且采集陀螺和加速度计的输出信息；

步骤2：通过步骤1中的初始位置信息和采集的陀螺、加速度计的输出信息，获得初始的捷联惯导系统捷联矩阵

步骤3：捷联惯导系统进入导航工作阶段，实时解算k时刻的捷联姿态矩阵与导航坐标系下的速度Vⁿ(k)；

步骤4：多普勒计程仪实时输出第k时刻的多普勒速度V_dvl(k)；

步骤5：由捷联惯导系统测量得到导航坐标系下的速度Vⁿ(k)，并通过Vⁿ(k)测量得到k时刻的船舶前进速度

步骤6：由步骤4中与步骤5中测量得到ΔV(k)，

$\mathrm{\ΔV}\left(k\right)=V_{\mathrm{dvl}}\left(k\right)-V_y^b\left(k\right);$

步骤7：对ΔV(k)进行高通数字滤波得到

$\tilde{V}\left(k\right)=W\left(k\right)\mathrm{\ΔV}\left(k\right);$

所述高通数字滤波中选用高通数字FIR滤波器，选取凯塞窗为

$W\left[k\right]=\frac{I[14\sqrt{1-{(\frac{2k}{N-1}-1)}^2]}}{87},$

其中函数I[x]定义为：

$I\left[x\right]=1+\underset{j=1}{\overset{13}{\mathrm{\Σ}}}{\left[\frac{{(x/2)}^j}{j!}\right]}^2,$

x为函数I[x]的自变量，取 $x=14\sqrt{1-{(\frac{2k}{N-1}-1)}^2},$ 并且有

N＝5.2f_s/f

其中f_s为采样频率，f为通带边缘频率；

步骤8：将补偿到多普勒速度中，得到优化后的多普勒速度U_dvl(k)

$U_{\mathrm{dvl}}\left(k\right)=V_{\mathrm{dvl}}\left(k\right)-\tilde{V}\left(k\right).$

2.根据权利要求1所述的基于捷联惯导系统的多普勒计程仪测速误差抑制方法，其特征是：所述通过Vⁿ(k)测量得到k时刻的船舶前进速度的具体方法为：由航坐标系下的速度Vⁿ(k)与捷联姿态矩阵测量得到k时刻载体坐标系下的速度

$V^b\left(k\right)={\left[C_b^n\left(k\right)\right]}^T{V}^n\left(k\right),$

其中[]^T表示转置，V^b(k)的矢量形式为

$V^b\left(k\right)=\left[\begin{array}{c}{V}_x^b\left(k\right)\\ V_y^b\left(k\right)\\ V_z^b\left(k\right)\end{array}\right],$

其中为船体右舷方向速度、为垂直船舶甲板平面方向速度、为船舶前进速度。