CN113790724A - 一种基于速度阻尼的惯性/多普勒组合导航方法及系统 - Google Patents

Abstract

本发明提供一种基于速度阻尼的惯性/多普勒组合导航方法及系统，该方法包括：初始对准结束后，惯导系统进入速度阻尼状态；建立外速度阻尼回路，并在外速度阻尼回路基础上建立全阻尼回路；当多普勒测速对底速度有效时，则建立基于速度阻尼的卡尔曼滤波状态方程，通过实时速度组合导航，输出高精度位置、速度及航姿信息；当多普勒测速对底速度无效时，进入全阻尼回路，输出阻尼后的速度、航姿及位置信息。通过该方案可以提高水下组合导航精度，并在多普勒测速对水有效时，有效阻尼纯惯性导航的舒拉周期和地球周期，满足航行器在不同海底深度环境下对高性能导航信息的需求。

Description

一种基于速度阻尼的惯性/多普勒组合导航方法及系统

技术领域

本发明涉及导航技术领域，尤其涉及一种基于速度阻尼的惯性/多普勒 组合导航方法及系统。

背景技术

多普勒具有对底测速精度高、性能稳定的特点，为满足水下无人航行器 对高精度导航信息的需求，一般采用惯性导航设备与多普勒进行速度组合导 航方式，实时输出高精度导航信息。随着水下无人航行器航程的增加，航行 器航行海域和范围变大，很多海域由于海底较深(大于1500米)，多普勒测 速仪不能测量航行器的对底速度，只能测量航行器的对水速度，在洋流较大 时(大于2Kn)，严重影响航行器在水底的自主航行。

为解决此问题，目前，采用的方案主要有两种，一种是多普勒速度对底 时，直接用多普勒对水速度与惯导组合，这样相当于把洋流速度带入组合导 航系统，提供的速度精度与多普勒对水速度精度基本相同，严重影响航行器 的航行安全，并且不能保证航行器到达预定位置，容易导致任务失败；另一 种方法是，航行器浮出水面，采用惯导与卫星导航的信息进行组合导航，输 出高精度的导航信息，可以保证导航信息的精度要求，失去航行器水下航行 条件，不能满足航行器的使用要求。

发明内容

有鉴于此，本发明实施例提供了一种基于速度阻尼的惯性/多普勒组合 导航方法及系统，以解决多普勒对水测速精度不能满足水下无人航行器使用 要求的问题。

在本发明实施例的第一方面，提供了一种基于速度阻尼的惯性/多普勒 组合导航方法，包括：

初始对准结束后，惯导系统进入速度阻尼状态；

建立外速度阻尼回路，并在外速度阻尼回路基础上建立全阻尼回路；

当多普勒测速对底速度有效时，则建立基于速度阻尼的卡尔曼滤波状态 方程，通过实时速度组合导航，输出高精度位置、速度及航姿信息；

当多普勒测速对底速度无效时，进入全阻尼回路，输出阻尼后的速度、 航姿及位置信息。

在本发明实施例的第二方面，提供了一种基于速度阻尼的惯性/多普勒 组合导航系统，包括：

初始对准模块，用于初始对准结束后，惯导系统进入速度阻尼状态，建 立外速度阻尼回路，并在外速度阻尼回路基础上建立全阻尼回路；

组合导航模块，用于当多普勒测速对底速度有效时，则建立基于速度阻 尼的卡尔曼滤波状态方程，通过实时速度组合导航，输出高精度位置、速度 及航姿信息；

速度阻尼导航模块，用于当多普勒测速对底速度无效时，进入全阻尼回 路，输出阻尼后的速度、航姿及位置信息。

本发明实施例中，在现有的惯性/卫星组合导航、惯性/多普勒组合导航 等模式的基础上，根据速度阻尼技术原理，提出一种基于速度阻尼的惯性/ 多普勒速度组合导航方法。该方法充分利用多普勒对底和对水测速信息的特 点，增加速度阻尼回路，解决了现有多普勒对水测速精度不能满足水下无人 航行器使用问题。为保证在对底有效时的惯性/多普勒组合导航精度、满足 水下航行，在对底或对水有效时，均采用速度阻尼方式取代传统的纯惯性导 航方式，建立基于速度阻尼的卡尔曼滤波状态方程，从而保证组合导航的导航性能；在多普勒测速有效时，惯导实时工作于速度阻尼状态，在多普勒对 底有效时，采用基于速度阻尼的卡尔曼滤波状态方程，实时惯性/多普勒组 合导航，当对底无效对水有效时，实时输出基于对水阻尼的速度和航姿参数， 供航行器安全航行，从而在根本上解决洋流较大时，实时提供可靠精度的速 度信息供无人水下航行器安全航行。

附图说明

为了更清楚地说明本发明实施例中的技术方案，下面将对实施例或现有 技术描述中所需要使用的附图作简单介绍，显而易见地，下面描述的附图仅 仅是本发明的一些实施例，对于本领域普通技术人员来讲，在不付出创造性 劳动的前提下，还可以根据这些附图获取其他附图。

图1为本发明的一个实施例提供的一种基于速度阻尼的惯性/多普勒组 合导航方法的流程示意图；

图2为本发明的一个实施例提供的航程定位误差曲线示意图；

图3为本发明的一个实施例提供的一种基于速度阻尼的惯性/多普勒组 合导航方法的另一流程示意图；

图4为本发明的一个实施例提供的一种基于速度阻尼的惯性/多普勒组 合导航系统的结构示意图。

具体实施方式

为使得本发明的发明目的、特征、优点能够更加的明显和易懂，下面将 结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整 地描述，显然，下面所描述的实施例仅仅是本发明一部分实施例，而非全部 的实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性 劳动前提下所获得的所有其它实施例，都属于本发明保护的范围以下结合附 图对本发明的原理和特征进行描述，所举实例只用于解释本发明，并非用于 限定本发明的范围。

本发明的说明书或权利要求书及上述附图中的术语“包括”以及其他相 近意思表述，意指覆盖不排他的包含，如包含一系列步骤或单元的过程、方 法或系统、设备没有限定于已列出的步骤或单元。

本发明的说明书或权利要求书及上述附图中的术语“包括”以及其他 相近意思表述，意指覆盖不排他的包含，如包含一系列步骤或单元的过程、 方法或系统、设备没有限定于已列出的步骤或单元。此外，“第一”“第二” 用于区分不同对象，并非用于描述特定顺序。

需要说明的是，水面舰船一般可以实时接收卫星导航信息，采用卫星导 航校准主惯导方式。主惯导一般都是选择最好性能体制的惯性导航设备，其 纯惯性导航精度比水下无人航行器高一个数量级以上，一般工作在纯惯性导 航状态或速度阻尼状态，速度阻尼采用的速度一般为电磁测速仪测量的对水 速度，可以有效阻尼纯惯性导航的舒拉周期和地球周期，输出的速度和航姿 基本无周期误差信号，大幅提高导航信息的精度。多普勒对水速度性能优于 电磁对水测量性能，在对水与对底测速两种条件下，始终采用速度阻尼方式， 重新建立惯导/多普勒速度组合的阻尼误差方程，在多普勒速度对底有效时， 输出惯导/多普勒速度组合导航信息，在对底无效对水有效时，不进行组合 导航，只工作于速度阻尼，输出高性能的速度和航姿信息，速度和航姿误差 主要为常值误差，其中速度常值误差一般优于0.2Kn,有效保证水下航行的安 全航行。

请参阅图1，图1为本发明一个实施例提供的一种基于速度阻尼的惯性/ 多普勒组合导航方法的流程示意图，包括：

S101、初始对准结束后，惯导系统进入速度阻尼状态；

所述初始对准用于在惯性导航开始前，进行坐标对准、初始参数的测量 和标定的过程。所述速度阻尼状态为基于速度信息进行阻尼导航的状态。

在本发明实施例中，采用的坐标系定义如下：

导航参考坐标系(t系)：坐标系原点在经纬度位置，x轴指东，y轴指 北，z轴指天；

惯性平台坐标系(p系)：惯导基于姿态解算跟踪、复现导航参考坐标 系所实际得到的坐标系；

惯导计算坐标系(c系)：基于惯导解算经纬度所确定的东北天坐标系， 原点在计算经纬度位置，x轴指东，y轴指北，z轴指天；

载体坐标系(b系)：固联在运载体上的参考坐标系，x轴沿运载体右 侧，y轴指向运载体前向，z轴指向运载体顶部；

载体初始惯性坐标系(b0系)：初始时刻与载体坐标系重合，并与惯性 坐标系相对静止；

多普勒坐标系(d系)：固联在运载体上的参考坐标系，x轴指向右侧， y轴指向前向，z轴与x轴、y轴构成右手正交坐标系。

具体的，利用动态解析对准算法快速确定惯导粗略水平姿态及航向，获 得航行器初始航姿；

其中，粗对准结束后转入精对准，同步采集卫导水平位置信息，以惯导 位置误差作为观测量进行卡尔曼滤波完成精对准；精对准结束后，若多普勒 速度有效，转入速度阻尼状态。

可以理解的是，本实施例中，将惯导原来的纯惯性导航原理增加速度阻 尼通道，在对水和对底有效时，惯导始终工作于速度阻尼状态。

S102、建立外速度阻尼回路，并在外速度阻尼回路基础上建立全阻尼回 路；

所述外速度阻尼回路，即外阻尼导航状态，导航过程中利用外部速度信 息和惯导内部信息进行速度比较，根据差值进行阻尼导航。所述全阻尼回路， 是在水平阻尼基础上，加上方位阻尼的形成的导航回路。

具体的，多普勒测速包括对底(相对海底)工作状态和对水(相对水流) 工作状态，不管对水或对底状态，只要速度信息有效，惯导进入外速度阻尼 工作状态。速度阻尼包含东向速度和北向速度两个通道，以惯导东向速度修 正回路为例，介绍传统的三阶阻尼网络。

由以上东向速度阻尼回路可以看出，在对东向速度两个积分环节之间加 入校正网络H_y(s)，可以使由加速度计误差ΔA_y、等效陀螺漂移ε_x及初始误差角 φ_x0引起的周期振荡性误差角衰减下来，从而保证惯导输出的速度和航姿误 差信息不包含舒拉周期和地球周期，提供给航行器高精度的速度信息。

其中，校正网络表示为：

S103、当多普勒对底速度有效，则建立基于速度阻尼的卡尔曼滤波状态 方程，进行速度组合导航，输出高精度位置、速度及航姿信息；

具体的，利用阻尼后的导航信息，在多普勒对底有效时，进行惯性解算 获得导航参数，构造基于速度阻尼的卡尔曼滤波状态转移矩阵，并计算离 散状态转移矩阵，利用公式计算观测矩阵；同步采集多普勒速度信息，计 算惯导速度误差作为观测量，进行卡尔曼滤波预测和修正，实时输出高精度 的组合导航信息。

进一步的，在多普勒对底速度有效时，同步采集多普勒速度信息，计算 惯导速度误差作为观测量，基于惯性误差方程、多普勒误差方程建立系统状 态方程以及观测方程。

系统误差方程如下：

姿态误差模型：

速度误差方程：

位置误差方程：

多普勒误差方程如下：

基于惯性系统误差方程与多普勒误差方程，选取惯导姿态误差、速度误 差、位置误差、陀螺漂移、加表漂移、多普勒航向安装误差、纵横摇安装误 差以及三维标度因数误差等21个状态变量建立卡尔曼滤波器：

建立卡尔曼滤波方程如下：

其中，G(t)为滤波模型所对应系统噪声分布矩阵；W(t)为系统激励噪声； H(t)为根据惯导误差方程以及多普勒误差方程所建立的描述状态变量之间耦 合关系的状态矩阵；X(t)为系统观测变量；Z(t)为系统观测矩阵。

Z＝[δV_E,δV_N,δV_U]^T (6)

在多普勒对底测速有效时，利用多普勒速度信息，以惯导速度误差误差 作为观测量。系统观测方程切换为如下状态：

其中，观测方程为H，观测噪声为V，

对公式(5)所示连续系统状态方程进行离散化可得到(8)所示离 散模型：

其中

T_n为离散化周期。

S104、对底速度无效时，进入全阻尼回路，输出阻尼后的速度、航姿及 位置信息。

在多普勒对底无效时，多普勒对水速度有效，惯导只需进入S102中的 速度阻尼，输出阻尼后的速度、航姿及位置。

本实施例中，在多普勒测速对底有效时，在阻尼的基础上，惯性/多普 勒组合导航，输出高精度位置、速度及航姿信息，在多普勒对底无效对水有 效时，仅工作于速度阻尼方式，输出高性能的速度和航姿信息，同时，输出 的位置信息也有提高，最终可以满足航行器在不同海底深度环境下对高性能 导航信息的需求。

示例性的，利用本发明的方法在青岛某海域进行了航行试验，采用陀螺 精度为0.05度/小时的50型激光惯导。航行启动对准，匀速直航120秒， 动态对准8分钟后转入导航状态，惯导转入速度阻尼状态，组合导航5小时， 航程近55Km，形成百分比优于0.1％D，航程定位误差曲线如图2，相应的试 验结果如表1所示：

多普勒组合水平位置误差	50(最大值/米)
		总路程	55(公里)
水平位置误差	0.1％D(误差/总路程)
		航行轨迹	青岛某海域

表1组合导航试验结果统计

在另一实施例中，如图3所示，基于速度阻尼的惯性/多普勒组合导航 开始后，惯导系统先进行2分钟的动态对准，再进行8分钟的精对准，完成 初始对准后，采集多普勒速度信息，进行速度阻尼导航解算，并计算卡尔曼 滤波状态转移矩阵。

判断多普勒速度信息对底速度有效还是对水速度有效，若对底速度有效， 则计算惯性/多普勒观测矩阵与观测量，若对水速度有效，则进入速度阻尼 回路，经过卡尔曼滤波后，算输出阻尼后的速度、航姿及位置等导航信息。

为保证组合导航精度，对底有效时，实时基于速度阻尼的惯性/多普勒 组合导航，对水有效时，输出速度阻尼后的高精度速度及航姿信息。通过速 度阻尼和速度阻尼误差状态方程，保证组合导航精度与传统组合导航精度相 同，其次，在对水有效时，有效阻尼纯惯性导航的舒拉周期和地球周期，输 出的速度误差主要为常值速度误差(一般小于0.2Kn),为无人水下航行器的 航行提供有效保障。

应理解，上述实施例中各步骤的序号大小并不意味着执行顺序的先后， 各过程的执行顺序应以其功能和内在逻辑确定，而不应对本发明实施例的 实施过程构成任何限定。

图4为本发明实施例提供的一种基于速度阻尼的惯性/多普勒组合导航 系统的结构示意图，该系统包括：

初始对准模块410，用于初始对准结束后，惯导系统进入速度阻尼状态， 建立外速度阻尼回路，并在外速度阻尼回路基础上建立全阻尼回路；

具体的，利用动态解析对准算法快速确定惯导粗略水平姿态及航向，获 得航行器初始航姿；

其中，惯导系统粗对准结束后转入精对准，同步采集卫星导航水平位置 信息，以惯导位置误差作为观测量进行卡尔曼滤波完成精对准，精对准结束 后，若判定多普勒速度有效，则进入速度阻尼状态。

其中，所述外速度阻尼回路包括东向速度阻尼回路和北向速度阻尼回路。

在所述外速度阻尼回路的积分过程中增加校正网络，使由加速度计误差、 等效陀螺漂移、初始误差角引起的周期震荡性误差角衰减。从而保证惯导输 出的速度和航姿误差信息不包含舒拉周期和地球周期，提供给航行器高精度 的速度信息。

组合导航模块420，用于当多普勒测速对底速度有效时，则建立基于速 度阻尼的卡尔曼滤波状态方程，通过实时速度组合导航，输出高精度位置、 速度及航姿信息；

具体的，利用阻尼后的导航信息，在多普勒测速对底有效时，进行惯性 解算获得导航参数，构造基于速度阻尼的卡尔曼滤波状态转移矩阵，并计算 离散状态转移矩阵，根据公式计算观测矩阵；同步采集多普勒速度信息，计 算惯导速度误差作为观测量，进行卡尔曼滤波预测和修正，实时输出高精度 的组合导航信息。

速度阻尼导航模块430，用于当多普勒测速对底速度无效时，进入全阻 尼回路，输出阻尼后的速度、航姿及位置信息。

在上述实施例中，对各个实施例的描述都各有侧重，某个实施例中没有详述 或记载的部分，可以参见其它实施例的相关描述。

本领域普通技术人员可以理解实现上述实施例方法中的全部或部分步 骤是可以通过程序或指令来控制相关的硬件完成，所述的程序可以存储于一 计算机可读取存储介质中，该程序在执行时，实现基于速度阻尼的惯性/多 普勒组合导航，所述的存储介质包括如ROM/RAM等。

以上所述，以上实施例仅用以说明本发明的技术方案，而非对其限制； 尽管参照前述实施例对本发明进行了详细的说明，本领域的普通技术人员应 当理解：其依然可以对前述各实施例所记载的技术方案进行修改，或者对其 中部分技术特征进行等同替换；而这些修改或者替换，并不使相应技术方案 的本质脱离本发明各实施例技术方案的精神和范围。

Claims (10)

1.一种基于速度阻尼的惯性/多普勒组合导航方法，其特征在于，包括：

初始对准结束后，惯导系统进入速度阻尼状态；

建立外速度阻尼回路，并在外速度阻尼回路基础上建立全阻尼回路；

当多普勒测速对底速度有效时，则建立基于速度阻尼的卡尔曼滤波状态方程，通过实时速度组合导航，输出高精度位置、速度及航姿信息；

当多普勒测速对底速度无效时，进入全阻尼回路，输出阻尼后的速度、航姿及位置信息。

2.根据权利要求1所述的方法，其特征在于，所述初始对准结束后，惯导系统进入速度阻尼状态包括：

利用动态解析对准算法快速确定惯导粗略水平姿态及航向，获得航行器初始航姿；

其中，惯导系统粗对准结束后转入精对准，同步采集卫星导航水平位置信息，以惯导位置误差作为观测量进行卡尔曼滤波完成精对准，精对准结束后，若判定多普勒速度有效，则进入速度阻尼状态。

3.根据权利要求1所述的方法，其特征在于，所述外速度阻尼回路包括东向速度阻尼回路和北向速度阻尼回路。

4.根据权利要求1所述的方法，其特征在于，在所述外速度阻尼回路的积分过程中增加校正网络，使由加速度计误差、等效陀螺漂移、初始误差角引起的周期震荡性误差角衰减。

5.根据权利要求1所述的方法，其特征在于，所述建立基于速度阻尼的卡尔曼滤波状态方程，通过实时速度组合导航，输出高精度位置、速度及航姿信息包括：

利用阻尼后的导航信息，在多普勒测速对底有效时，进行惯性解算获得导航参数，构造基于速度阻尼的卡尔曼滤波状态转移矩阵，并计算离散状态转移矩阵，基于公式计算观测矩阵；

同步采集多普勒速度信息，计算惯导速度误差作为观测量，进行卡尔曼滤波预测和修正，实时输出高精度的组合导航信息。

6.一种基于速度阻尼的惯性/多普勒组合导航系统，其特征在于，包括：

初始对准模块，用于初始对准结束后，惯导系统进入速度阻尼状态，建立外速度阻尼回路，并在外速度阻尼回路基础上建立全阻尼回路；

组合导航模块，用于当多普勒测速对底速度有效，则建立基于速度阻尼的卡尔曼滤波状态方程，通过实时速度组合导航，输出高精度位置、速度及航姿信息；

速度阻尼导航模块，用于当多普勒测速对底速度无效时，进入全阻尼回路，输出阻尼后的速度、航姿及位置信息。

7.根据权利要求6所述的系统，其特征在于，所述初始对准结束后，惯导系统进入速度阻尼状态包括：

利用动态解析对准算法快速确定惯导粗略水平姿态及航向，获得航行器初始航姿；

其中，惯导系统粗对准结束后转入精对准，同步采集卫星导航水平位置信息，以惯导位置误差作为观测量进行卡尔曼滤波完成精对准，精对准结束后，若判定多普勒速度有效，则进入速度阻尼状态。

8.根据权利要求6所述的系统，其特征在于，所述外速度阻尼回路包括东向速度阻尼回路和北向速度阻尼回路。

9.根据权利要求6所述的系统，其特征在于，在所述外速度阻尼回路的积分过程中增加校正网络，使由加速度计误差、等效陀螺漂移、初始误差角引起的周期震荡性误差角衰减。

10.根据权利要求6所述的系统，其特征在于，所述建立基于速度阻尼的卡尔曼滤波状态方程，通过实时速度组合导航，输出高精度位置、速度及航姿信息具体为：

利用阻尼后的导航信息，在多普勒测速对底有效时，进行惯性解算获得导航参数，构造基于速度阻尼的卡尔曼滤波状态转移矩阵，并计算离散状态转移矩阵，基于公式计算观测矩阵；

同步采集多普勒速度信息，计算惯导速度误差作为观测量，进行卡尔曼滤波预测和修正，实时输出高精度的组合导航信息。

Images