CN111829512A - 一种基于多传感器数据融合的auv导航定位方法及系统 - Google Patents

Abstract

本发明涉及一种基于多传感器数据融合的AUV导航定位方法及系统。该方法中，采用不依赖外部信息、不受环境影响的惯性导航系统作为主惯导系统，GPS/USBL作为辅助定位系统，DVL作为辅助测速系统，旨在应用多种较低精度传感器设计一种高精度组合导航系统。步骤一：建立惯性导航运动学模型；步骤二：定义AUV的3维空间、6个自由度的运动参数；所述AUV是指无人水下潜航器；步骤三：确定组合导航系统模型的基本形式；步骤四：进行惯性导航、声学导航和卫星导航相结合的组合导航。该组合导航方式应用在AUV的水面、水下运动测试中，均取得较好的定位效果，定位误差在10m左右，是系统具有更高的稳定性和实用性。

Description

一种基于多传感器数据融合的AUV导航定位方法及系统

技术领域

本发明发明涉及一种基于多传感器数据融合的AUV导航定位方法及系统， 属于水下组合导航技术领域。

背景技术

水下导航问题仍然是水下移动平台所面临的主要技术挑战之一，由于水下环 境的复杂性和特殊性，AUV(无人水下潜航器)水下定位极其困难。学者们关 注的重点在于如何克服水下环境对水下定位的影响，提高水下定位的精度，也是 近年来的研究热点。水下环境对于AUV定位的影响主要表现为以下几个方面:

(1)干扰多，影响大。在海洋中，不仅存在海面风、潮汐、海流、海面锋、 温度跃层、密度跃层等干扰，还存在温度、盐度、密度、压强等对水下定位仪器 的影响。

(2)能见度低。由于光在海水中衰减和散射比在大气中强烈得多，海水中的 水平视程仅为大气中的千分之一。通常情况下海中能见度为20m，具体数值会受 到水质及水色的影响。

(3)电磁波在水中无法传播，声信号传播效率低，可靠率低。这使得陆地上 一些精准的定位设备(如GPS)的应用受到了很大的限制。

(4)成本高、体积大、重量大的设备不便于AUV携带。由于AUV自身体积 和重量的限制，对水下定位设备的体积和重量会形成一定的制约。同时，高精度 的水下定位设备一般价格比较昂贵，在一些非军用的航行器上的应用也受到了限 制。

(5)隐蔽性问题。对于军事用途的AUV而言，隐蔽性是执行任务的先决条 件，有些声学或视觉的水下定位方法，会将自身暴露。

实际应用中，需根据AUV的实际作业需求，结合上述水下环境的特点选择 合适的水下定位方法。不仅要考虑AUV工作的深度范围，也要考虑AUV执行 任务的航程时间。不同的水下定位方法有不同的适用范围，多种水下定位方法组 合可以提高定位精度。

目前，学者们正努力寻求适合水下环境的导航定位方法。常用的水下导航方 法分为以下几类，如图1所示。但由于单一导航方式总是存在缺陷，在精度、可 靠性上还无法满足水下长时间、长航程的高精度导航需求，部分导航手段只能作 为辅助，通过组合导航手段对不同导航方式进行优势互补，可以获得更高的导航 性能。

发明内容

本发明发明的目的是：为了解决水下单一导航缺陷的问题，选取最佳统计理 论为方法，采用信息融合技术构建组合导航系统，根据各个单一导航方法进行取 长补短，并且可以在不影响整体导航精度的前提下降低某单一导航系统的精度要 求，降低导航程度和技术难点，具有高性价比和高精度的特点。

本发明采用如下技术方案实现：

一种基于多传感器数据融合的AUV组合导航方法，包括以下几个步骤：

步骤一：建立惯性导航运动学模型；

步骤二：定义AUV的3维空间、6个自由度的运动参数；所述AUV是指 无人水下潜航器；

步骤三：确定组合导航系统模型的基本形式；

步骤四：进行惯性导航、声学导航和卫星导航相结合的组合导航。

进一步的，惯性导航运动学模型具体为：

惯性导航姿态更新为：

其中，Δθ＝[Δθ_x Δθ_y Δθ_z]^T表示载体坐标系下陀螺仪输出的三轴角度变化信息；ε＝[ε_x ε_y ε_z]^T表示载体坐标系下陀螺三轴常值漂移信息；[q₀ q₁ q₂ q₃]^T表 示四元数信息，姿态更新时采用旋转矢量法更新四元数；Δq＝[Δq₀ Δq₁ Δq₂ Δq₃]^T为四元数变化量；

惯性导航速度更新为：

其中，V＝[V_N V_E V_D]^T表示n系下三维速度信息，

为捷联矩阵， ΔV＝[ΔV_x ΔV_yΔV_z]^T表示b系下加速度计输出的三轴速度变化信息，g表示当地 重力加速度，Δt表示采样时间间隔；ΔA＝[ΔA_x ΔA_y ΔA_z]^T表示b系下加速度计的 常值零偏信息；

惯性导航位置更新为：

其中：P＝[P_N P_E P_D]^T表示n系下三维位置信息。

进一步的，所述n系即为导航坐标系：定为N-E-D的地理坐标系，原点位于 载体当前所在位置，x_n,y_n在地理水平面内，x_n轴沿当地经线的切线向北，y_n轴沿 当地纬线的切线向东，z_n轴垂直向下构成右手坐标系；

所述b系是指载体坐标系，载体坐标系即AUV本体坐标系，原点位于载体 质心处，x_b沿载体轴向向前，y_b沿载体向右，z_b垂直于载体平面，三轴满足右手 法则，随着载体的转动而转动。

进一步的，定义AUV的3维空间、6个自由度的运动参数，具体为：

(P_N,P_E,P_D)：表示在导航坐标系下，AUV在三个坐标轴下的位置分量；

(V_N,V_E,V_D)：表示在导航坐标系下，AUV在三个坐标轴下的速度分量；

(γ,θ,ψ)：γ为横滚角，选取逆时针方向为正方向，其取值范围是(-180°,180°)；θ为俯仰角，正方向与横滚角相同，取值范围是(-90°,90°)；ψ为偏航角，左偏航 为正，取值范围是(-180°,180°)。

进一步的，组合导航系统模型的基本形式为

其中，X为状态量，Φ_k-1为状态转移矩阵，Z为量测量，H_k为量测矩阵，w_k-1与v_k代表过程噪声与量测噪声，服从正态分布w_k-1～(0,Q_k)，v_k～(0,R_k)，Q_k是过程 噪声方差且非负定；R_k是量测噪声方差且正定。

进一步的，进行惯性导航、声学导航和卫星导航相结合的组合导航，具体为：(1)确定状态方程，具体为：

状态量X＝[q₀ q₁ q₂ q₃ V_N V_E V_D P_N P_E P_D ε_x ε_y ε_z ΔA_x ΔA_y ΔA_z]^T； 状态转移矩阵Φ_k-1如下：

其中：

t₂₁＝2q₁(ΔV_x-ΔA_x),t₂₂＝2q₀(ΔV_y-ΔA_y),t₂₃＝2q₁(ΔV_z-ΔA_z)

t₂₄＝2q₀(ΔV_x-ΔA_x)+2q₂(ΔV_y-ΔA_y)+2q₃(ΔV_z-ΔA_z)

t₂₅＝2q₀(ΔV_x-ΔA_x),t₂₆＝2q₂(ΔV_z-ΔA_z),t₃₇＝2q₃(ΔV_y-ΔA_y)

t₂₇＝2q₀(ΔV_x-ΔA_x)+2q₂(ΔV_z-ΔA_z)+2q₃(ΔV_y-ΔA_y)

t₂₈＝2q₀q₃,t₂₉＝q₀ ²,t₃₀＝q₁ ²,t₃₁＝q₂ ²,t₃₂＝q₃ ²,t₃₃＝2q₂(ΔV_x-ΔA_x)

t₃₄＝2q₃(ΔV_x-ΔA_x),t₃₅＝2q₀(ΔV_y-ΔA_y),t₃₈＝2q₀q₁

t₃₆＝2q₃(ΔV_x-ΔA_x)+2q₁(ΔV_z-ΔA_z)+2q₀(ΔV_y-ΔA_y)

状态噪声w_k的方差Q_k具体为：

其中：

Q_q＝BB^T,Q_v＝AA^T

(2)确定量测方程：

USBL/GPS量测矩阵为：

DVL量测矩阵为：

磁力计量测矩阵为：

H_mag＝[-2l₈l₁₄(q₃l₃-q₃l₄+q₃l₅+q₃l₆+2q₀q₁q₂)；-2l₈l₁₄(-q₂l₃+q₂l₄+q₂l₅+q₂l₆+2q₀q₁q₃)；

2l₈l₁₄(q₁l₃+q₁l₄+q₁l₅-q₁l₆+2q₀q₂q₂)；2l₈l₁₄(q₀l₃+q₀l₄-q₀l₅+q₀l₆+2q₁q₂q₃)；o_1×12]_1×16

其中：

量测噪声v_k的方差R_k根据实际的USBL/GPS、DVL和磁力计系统型号选定；

(3)通过状态方程和量测方程估算状态量X；

(4)通过[q₀ q₁ q₂ q₃]^T计算γ,θ,ψ；

(5)根据步骤(3)得到的状态量X中的(P_N,P_E,P_D)和(V_N,V_E,V_D)与步骤(4) 得到的γ,θ,ψ，进行组合导航对AUV进行实时定位。

进一步的，声学导航是指通过USBL提供位置信息，通过DVL提供速度信 息的导航方式，USBL是指超短基线定位系统，DVL是指多普勒测速系统；卫星 导航通过GPS提供位置信息；磁力计集成在惯性导航系统内部，用于提供航向 信息。

进一步的，本发明还提出一种AUV组合导航系统，包括：

模型建立模块：建立惯性导航运动学模型；

运动参数定义模块：定义AUV的3维空间、6个自由度的运动参数；所述 AUV是指无人水下潜航器；

组合导航系统模型确定模块：确定组合导航系统模型的基本形式；

组合导航模块：进行惯性导航、声学导航和卫星导航相结合的组合导航。

本发明相比现有技术的优点在于：

(1)本发明最终在水面选取惯导系统、GPS组合方式，水下选取惯导、多 普勒测速系统(Doppler Velocity Log，DVL)、超短基线定位系统(Ultra-Short Baseline System,USBL)的组合导航方式，见图2。

(1)水面上GPS信号较好，定位精度较高，采取惯导+GPS的组合的方式， 降低计算量，提高计算速度。

(2)由于USBL的速率较低，首先选取惯导+DVL速度的组合方式获取定 位信息，但长时间会出现误差积累导致发散，因此将USBL的位置信息作为校正 辅助手段，定时对系统的定位信息进行修正。

附图说明

图1.水下导航方式分类示意图

图2.AUV组合导航方式示意图

图3.惯导系统解算方框图

图4.超短基线定位系统的定位原理图

图5.USBL系统坐标框图

图6.四射束空间几何定位图

图7.GPS轨迹与组合后的轨迹对比示意图

图8.基准轨迹与组合后的轨迹对比示意图

具体实施方式

下面将结合附图和实施例对本发明作进一步的详细说明。

本发明涉及一种基于多传感器数据融合的AUV导航定位方法及系统。该方 法中，采用不依赖外部信息、不受环境影响的惯性导航系统作为主惯导系统， GPS/USBL作为辅助定位系统，DVL作为辅助测速系统，旨在应用多种较低精度 传感器设计一种高精度组合导航系统。针对组合导航中各个传感器的更新频率不 一致问题，本发明采用序贯卡尔曼滤波技术解决该问题，外部设备未提供量测情 况下，惯导系统仅进行时间更新，完成状态预测过程，待获取外部设备信息后， 进行量测更新。

惯导系统由于其惯性器件的固有特性，产生的误差会随时间积累，DVL是一 种常用测速导航系统，将它与惯导系统组合在一起组合，能抑制纯惯导误差随时 间发散的问题。因此，可用GPS/USBL提供位置量测，DVL提供速度量测，构 成组合导航系统，对惯导系统进行修正。

能接收到GPS信号时优先使用惯导系统+GPS+DVL的组合状态，无法获得 GPS信号时采用惯导系统+USBL+DVL的组合方式，从而对系统进行高精度导 航。将该组合导航方式应用在AUV的水面、水下运动测试中，均取得较好的定 位效果，定位误差在10m左右，是系统具有更高的稳定性和实用性。

无人机导航控制系统中多采用惯导、光流传感器，GPS定位系统、超声波等 组合导航的方试进行实时导航控制，而水下的特殊环境使得一些定位方法的应用 受到了极大的限制，无人水下潜航器在水下时，GPS无信号，光流传感器也无法 获得速度，因此欲寻求代替GPS和光流的传感器进行水下导航定位，具体步骤 如下：

步骤一：根据惯性导航、声学导航、卫星导航等导航方式的定位、测速、定 姿原理，建立运动学模型：

1.惯导系统运动学模型

图3的方框图为惯导系统的解算过程，运动学公式表示如下：

(1)惯性导航姿态更新，求解姿态捷联矩阵的过程是SINS得以进行导航的 基础和核心，

式(1)中，Δθ＝[Δθ_x Δθ_y Δθ_z]^T表示载体坐标系下陀螺仪输出的三轴角度变化信息；ε＝[ε_x ε_y ε_z]^T表示载体坐标系下陀螺三轴常值漂移信息；[q₀ q₁ q₂ q₃]^T表示四元数信息，姿态更新时采用旋转矢量法更新四元数； Δq＝[Δq₀ Δq₁ Δq₂ Δq₃]^T为四元数变化量。

(2)惯性导航速度更新

速度更新方程将三轴的加速度计信息，通过姿态矩阵转换至n系下，获得速 度矢量，则速度更新环节中的矢量微分方程为：

式(2)中，V＝[V_N V_E V_D]^T表示n系下三维速度信息，

为捷联矩阵， ΔV＝[ΔV_xΔV_y ΔV_z]^T表示b系下加速度计输出的三轴速度变化信息，g表示当地 重力加速度，Δt表示采样时间间隔；ΔA＝[ΔA_x ΔA_y ΔA_z]^T表示b系下加速度计的 常值零偏信息。

将式(2)展开为三轴形式：

其中

所述n系即为导航坐标系：定为N-E-D的地理坐标系，原点位于载体当前所 在位置，x_n,y_n在地理水平面内，x_n轴沿当地经线的切线向北，y_n轴沿当地纬线的 切线向东，z_n轴垂直向下构成右手坐标系；

所述b系是指载体坐标系，载体坐标系即AUV本体坐标系，原点位于载体 质心处，x_b沿载体轴向向前，y_b沿载体向右，z_b垂直于载体平面，三轴满足右手 法则，随着载体的转动而转动。

(3)惯性导航位置更新

位置更新主即将速度积分获得载体相对于地球表面的运动，则位置更新方程 为

其中：P＝[P_N P_E P_D]^T表示n系下三维位置信息。

2.超短基线定位系统(USBL)模型

超短基线定位系统通过测得由发射器在水下发送水声信号到目标海域海底 安置已知应答器的往返时间计算得到斜距。根据水声信号传送到声头每个水听器 的相位差计算得到运载体的方位角。

图4为USBL的定位原理示意图。令基阵的中心O作为坐标原点，天向反方 向为z轴正向，根据右手定则，建立基阵坐标系oxyz。四个水听器坐落在坐标系x轴和y轴上，同轴水听器间间距为d，则各点坐标为(±d/2，±d/2)。应答器具 体位置为T处，其坐标为(x_a,y_a,z_a)，由图4易得：

x_a＝l cosθ_x

y_a＝l cosθ_y

USBL定位原理图中l为斜距，m为斜距l在水平面oxy上的投影，θ为水平方 位角即m与x轴的夹角，θ_x和θ_y分别为l与x轴、y轴的夹角。

设定声波的波长为λ，

表示同轴上水听器接收水声信号的相位差：

将式(7)代入到式(5)中，可得

斜距l＝c·T_ri/2，T_ri为问答机发送声学信号到应答器返回声学信号的往返时间，声波在水下传输速度为c。

可由超短基线测量得到，由此能够计算出目 标相对基阵坐标系的水平位置(x_a,y_a)、深度z_a和声波入射角(θ_x,θ_y)等。

USBL导航解算方程的坐标转换，把USBL中水听器的几何中心定为坐标原 点，东北天坐标系用ox_ty_tz_t表示。当不考虑水听器安装误差时，超短基线系统坐 标系(u系)与载体坐标系(b系)的三轴空间内呈相互平行关系。u系(b系)、t系和e 系三个坐标系在空间中的分布情况如图5所示。

从图5中可以看出，s与p分别为地球坐标系下应答器与载体的位置矢量， 其中由于应答器是事先布置在海底的，所以s为已知位置矢量。r是应答器在载 体坐标系下的位置矢量，载体坐标系到地球坐标系的变换矩阵用

表示，由 向量定理可得应答器的位置矢量为，获得USBL定位模型：

r＝R^T(s-p) (9)

3.多普勒测速系统(DVL)模型

DVL是现阶段海上航行载体测速重要导航手段之一，基于多普勒效应原理它 能够长航时的提供载体精确的速度测量信息，和捷联惯导系统能够形成良好的互 补性，进而被广泛应用在各个领域。

多普勒效应是一种关于微波技术的现象。1842年，著名物理学家多普勒无意 中在研究声学问题时发现一种现象，即在发射声波与反射声波物体间存在相对位 移的时候，发射频率f₁和反射频率f₂并不一致。发射与反射频率的差值恰好与两 者之间的相对速度呈现线性关系，这个频率差值即为多普勒频移。科学家们基于 这种效应研制出了多普勒计程仪、多普勒雷达等系统，下面主要介绍基于四射束 的多普勒系统测速方法。

四射束的多普勒系统的波束空间和反射点位置如图6所示，

设载体的速度为v，四射束具有相同的倾角α和偏角β，声波沿每个方向的 多普勒频移都是三个竖直方向速度分量的多普勒频移之和，四个射束的多普勒频 移分别是

式中，A_i,B_i,C_i,D_i(i＝x,y,z)分别是四个波束的方向余弦，满足

将式(11)代入到式(10)，可得

联立求解式(12)，得到三轴方向速度，获得DVL测速模型：

式中

针对上述导航方式的原理，分析其局限性。为满足长航程、高精度的导航需 求，提供一种多传感器信息融合的组合导航技术：

惯性导航系统(Inertial navigation system,INS)长时间导航的误差积累严重，需 定时校正；超短基线定位系统系统定位范围较小，自主性差，且需要地面端支持； GPS定位系统受天气等外界环境因素影响较大，且在水中长时间没有信号；多普 勒测速仪测速精度束缚于有效射程，一旦DVL的波束超越了既定里程，对其测 速准确度影响较为明显，并且会受安装误差、水温、水速、水深、水中盐浓度等 因素影响较大。

惯性导航系统通过陀螺仪和加速度计测量角速度和加速度，经过解算得到位 置、速度与姿态等导航参数。不依赖外部信息可自主进行导航，成为现阶段主流 的导航定位系统。但由于其惯性器件的固有特性，产生的误差会随时间积累，DVL 是一种常用测速导航系统，将它与INS组合在一起形成INS/DVL组合导航子系 统，能抑制纯惯导误差随时间发散的问题。因此，可用GPS/USBL提供位置量测， DVL提供速度量测，构成组合导航系统，对惯导系统进行修正。能接收到GPS 信号时优先使用INS+GPS+DVL的组合状态，无法获得GPS信号时采用 INS+USBL+DVL的组合方式，从而对系统进行高精度导航，如图2所示。

步骤二：定义AUV无人水下潜航器3维空间，6个自由度的运动特性参数：

AUV在水下执行任务时，需要具体参数来描述其运动特征。下面，定义AUV 在导航坐标系下的运动参数：

导航坐标系：通常定为(N-E-D)的地理坐标系，原点位于载体当前所在位 置，x_n,y_n在地理水平面内，x_n轴沿当地经线的切线向北，y_n轴沿当地纬线的切线 向东，z_n轴垂直向下构成右手坐标系；

(P_N,P_E,P_D)：表示在导航坐标系下，AUV在三个坐标轴下的位置分量；

(V_N,V_E,V_D)：表示在导航坐标系下，AUV在三个坐标轴下的速度分量；

(γ,θ,ψ)：γ为横滚角，选取逆时针方向为正方向，其取值范围是(-180°,180°)；θ为俯仰角，正方向与横滚角相同，取值范围是(-90°,90°)；ψ为偏航角，左偏航 为正，取值范围是(-180°,180°)。

(p,ω_θ,ω)：表示在导航坐标系下，AUV沿着三个轴的角速度，规定逆时针方 向为正。

步骤三：确定组合导航系统模型的基本形式；

组合导航系统模型的基本形式为

其中，X为状态量，Φ_k-1为状态转移矩阵，Z为量测量，H_k为量测矩阵，w_k-1与v_k代表过程噪声与量测噪声，服从正态分布w_k-1～(0,Q_k)，v_k～(0,R_k)，Q_k是过程 噪声方差且非负定；R_k是量测噪声方差且正定。

步骤四：进行惯性导航、声学导航和卫星导航相结合的组合导航。

将设计的组合导航算法应用在实体AUV中，该导航系统硬件设备组成见表 1：

表1导航硬件设备组成

其中，惯导系统SBG内部集成磁力计，可依靠该设备作为航向参考。

包括步骤如下：

(1)选取AUV在导航系中的三轴姿态、三轴速度、三轴位置以及三轴常值 陀螺漂移、三轴常值加速度计零偏作为状态量，确定状态方程

状态量X＝[q₀ q₁ q₂ q₃ V_N V_E V_D P_N P_E P_D ε_x ε_y ε_z ΔA_x ΔA_y ΔA_z]^T；

状态转移矩阵Φ_k-1如下：

其中：

t₂₁＝2q₁(ΔV_x-ΔA_x),t₂₂＝2q₀(ΔV_y-ΔA_y),t₂₃＝2q₁(ΔV_z-ΔA_z)

t₂₄＝2q₀(ΔV_x-ΔA_x)+2q₂(ΔV_y-ΔA_y)+2q₃(ΔV_z-ΔA_z)

t₂₅＝2q₀(ΔV_x-ΔA_x),t₂₆＝2q₂(ΔV_z-ΔA_z),t₃₇＝2q₃(ΔV_y-ΔA_y)

t₂₇＝2q₀(ΔV_x-ΔA_x)+2q₂(ΔV_z-ΔA_z)+2q₃(ΔV_y-ΔA_y)

t₂₈＝2q₀q₃,t₂₉＝q₀ ²,t₃₀＝q₁ ²,t₃₁＝q₂ ²,t₃₂＝q₃ ²,t₃₃＝2q₂(ΔV_x-ΔA_x)

t₃₄＝2q₃(ΔV_x-ΔA_x),t₃₅＝2q₀(ΔV_y-ΔA_y),t₃₈＝2q₀q₁

t₃₆＝2q₃(ΔV_x-ΔA_x)+2q₁(ΔV_z-ΔA_z)+2q₀(ΔV_y-ΔA_y)

Δθ_x,Δθ_y,Δθ_z为三轴陀螺仪输出，ΔV_x,ΔV_y,ΔV_z为三轴加速度计输出。

状态噪声w_k的方差Q_k具体为：

其中：

Q_q＝BB^T,Q_v＝AA^T

(2)确定量测方程。根据步骤二定义量测矩阵H，USBL的位置、DVL的 速度、磁力计的磁航向作为量测量。其中DVL提供的是载体系下AUV的速度， 与状态量之间存在坐标转换；磁力计提供的磁航向与状态量无直接关系，也需进 行变换，变换方法如下：

1)融合USBL的位置(与融合GPS的位置相同)：

由于USBL设备本身提供的是水面端与水下端换能器的相对位置，水下端安 装在AUV上，水面端有GPS的支持，因此可以计算出水下AUV的经纬度， USBL/GPS量测矩阵为：

2)融合DVL的速度：

将载体系(b系)下的速度转换到导航系(n系)下，获得DVL量测矩阵

3)融合磁航向信息：

磁航向通过磁力计(mag)提供的三轴信息可计算出磁航向，将磁航向作用 到状态量四元数中，

其中，QuatToEual为四元数转化欧拉角函数，QuatToTbn为四元数转化姿态矩 阵函数，EulToQuat为欧拉角转化四元数函数。式(19)表示，通过当前时刻的四元 数计算航向角作为一步预测量，令航向角为0，计算航向角为0时的姿态矩阵， 将b系下的磁力计信息通过航向角为0时的姿态矩阵转换到n系下，计算当前时 刻的航向作为量测，此时两坐标系之间只有航向角的偏移。

对航向的量测公式求导获得mag量测矩阵：

其中：

量测噪声v_k的方差R_k根据实际的USBL/GPS、DVL和磁力计系统型号选定。

惯导系统的更新频率设置为100Hz，磁力计集成在惯导系统内部，频率与惯 导系统相同，DVL的更新频率大致为5Hz，GPS的位置更新频率为2Hz，速度更 新频率为10Hz，USBL的更新频率为0.5Hz，由于频率的不一致性，本系统采取 序贯卡尔曼滤波的融合方式进行最优估计。

(3)通过状态方程和量测方程估算状态量X；

(4)通过[q₀ q₁ q₂ q₃]^T计算γ,θ,ψ；

(5)根据步骤(3)得到的状态量X中的(P_N,P_E,P_D)和(V_N,V_E,V_D)与步骤(4) 得到的γ,θ,ψ，进行组合导航对AUV进行实时定位。

声学导航是指通过USBL提供位置信息，通过DVL提供速度信息的导航方 式，USBL是指超短基线定位系统，DVL是指多普勒测速系统；卫星导航通过 GPS提供位置信息；磁力计集成在惯性导航系统内部，用于提供航向信息。

实施过程：

实施例一：

对AUV进行水面测试，在经纬度为40.3457756°N,115.7364502°E的官厅水库进 行组合导航算法的测试验证。同时采集SBG、DVL、GPS的数据，运动轨迹大 致为三角形。将导航设备的误差均等效为零均值高斯白噪声，陀螺漂移噪声为 0.2rad/s，加速度计零偏噪声为0.35m/s2，磁航向噪声为0.1745rad，GPS位置误 差噪声为0.3m，DVL速度误差噪声0.3m/s，由于在水面测试，GPS信号良好， 在该位置可搜到10颗以上卫星，因此，对采集的数据采取步骤四中的组合导航 算法，及IMU+GPS+DVL的组合方式，组合结果与GPS的轨迹对比，如图7所 示。

实施例二：

对AUV进行水下测试，在经纬度为40.3434715°N,115.7399673°E的官厅水库进 行组合导航算法的测试验证。同时采集SBG、DVL、USBL的数据，导航参数设 置与实施例1一致。此时，GPS搜索不到卫星，采用IMU+USBL+DVL的组合定 位方式，对实验数据进行分析处理，组合结果与AUV本体携带的高精度惯导进 行对比，如图8所示

实施效果：

GPS在卫星星数较多时的定位精度较高，但其更新频率较低，为惯导系统更 新频率的百分之一，且GPS受环境能够因素影响较大，该处空旷无干扰，因此 将其作为基准。由实施例一的对比结果可知，采用IMU+GPS+DVL的组合方式 的精度在10m以内。

通过实施例二的结果，并计算基准轨迹与组合后的轨迹距离分别为450.7m 何438.1m，精度在10m左右。

AUV水面运动时，GPS具有漂移，静止时达到最大(0-8m)，且更新频率 较低，从以上实施例不难看出，与惯导系统、多普勒测速系统组合后位置误差可 减小到10m以内。若水下运动时，惯导系统仅与超短基线定位系统组合，超短基 线定位系统的更新频率更低，该AUV中采用的惯导系统精度较低，在超短基线 定位系统更新的间隙会发散，因此采取惯导系统、多普勒测速系统、超短基线定 位系统三种设备的组合导航，组合后位置误差可减小到10m左右，本方法能够实 现长时间高精度的水下导航定位，明显改善了单一系统的性能，具有可行性。

Claims (10)

1.一种基于多传感器数据融合的AUV组合导航方法，其特征在于，包括以下几个步骤：

步骤一：建立惯性导航运动学模型；

步骤二：定义AUV的3维空间、6个自由度的运动参数；所述AUV是指无人水下潜航器；

步骤三：确定组合导航系统模型的基本形式；

步骤四：进行惯性导航、声学导航和卫星导航相结合的组合导航。

2.根据权利要求1所述的基于多传感器数据融合的AUV组合导航方法，其特征在于：惯性导航运动学模型具体为：

惯性导航姿态更新为：

其中，Δθ＝[Δθ_x Δθ_y Δθ_z]^T表示载体坐标系下陀螺仪输出的三轴角度变化信息；ε＝[ε_x ε_y ε_z]^T表示载体坐标系下陀螺三轴常值漂移信息；[q₀ q₁ q₂ q₃]^T表示四元数信息，姿态更新时采用旋转矢量法更新四元数；Δq＝[Δq₀ Δq₁ Δq₂ Δq₃]^T为四元数变化量；

惯性导航速度更新为：

其中，V＝[V_N V_E V_D]^T表示n系下三维速度信息，

为捷联矩阵，ΔV＝[ΔV_x ΔV_y ΔV_z]^T表示b系下加速度计输出的三轴速度变化信息，g表示当地重力加速度，Δt表示采样时间间隔；ΔA＝[ΔA_x ΔA_y ΔA_z]^T表示b系下加速度计的常值零偏信息；

惯性导航位置更新为：

其中：P＝[P_N P_E P_D]^T表示n系下三维位置信息。

3.根据权利要求2所述的基于多传感器数据融合的AUV组合导航方法，其特征在于：所述n系即为导航坐标系：定为N-E-D的地理坐标系，原点位于载体当前所在位置，x_n,y_n在地理水平面内，x_n轴沿当地经线的切线向北，y_n轴沿当地纬线的切线向东，z_n轴垂直向下构成右手坐标系；

所述b系是指载体坐标系，载体坐标系即AUV本体坐标系，原点位于载体质心处，x_b沿载体轴向向前，y_b沿载体向右，z_b垂直于载体平面，三轴满足右手法则，随着载体的转动而转动。

4.根据权利要求3所述的基于多传感器数据融合的AUV组合导航方法，其特征在于：定义AUV的3维空间、6个自由度的运动参数，具体为：

(P_N,P_E,P_D)：表示在导航坐标系下，AUV在三个坐标轴下的位置分量；

(V_N,V_E,V_D)：表示在导航坐标系下，AUV在三个坐标轴下的速度分量；

(γ,θ,ψ)：γ为横滚角，选取逆时针方向为正方向，其取值范围是(-180°,180°)；θ为俯仰角，正方向与横滚角相同，取值范围是(-90°,90°)；ψ为偏航角，左偏航为正，取值范围是(-180°,180°)。

5.根据权利要求4所述的基于多传感器数据融合的AUV组合导航方法，其特征在于：组合导航系统模型的基本形式为

其中，X为状态量，Φ_k-1为状态转移矩阵，Z为量测量，H_k为量测矩阵，w_k-1与v_k代表过程噪声与量测噪声，服从正态分布w_k-1～(0,Q_k)，v_k～(0,R_k)，Q_k是过程噪声方差且非负定；R_k是量测噪声方差且正定。

6.根据权利要求5所述的基于多传感器数据融合的AUV组合导航方法，其特征在于：进行惯性导航、声学导航和卫星导航相结合的组合导航，具体为：

(1)确定状态方程，具体为：

状态量X＝[q₀ q₁ q₂ q₃ V_N V_E V_D P_N P_E P_D ε_x ε_y ε_z ΔA_x ΔA_y ΔA_z]^T；

状态转移矩阵Φ_k-1如下：

其中：

t₁₅＝ΔV_x-ΔA_x,t₁₆＝ΔV_y-ΔA_y,t₁₇＝ΔV_z-ΔA_z

t₂₁＝2q₁(ΔV_x-ΔA_x),t₂₂＝2q₀(ΔV_y-ΔA_y),t₂₃＝2q₁(ΔV_z-ΔA_z)

t₂₄＝2q₀(ΔV_x-ΔA_x)+2q₂(ΔV_y-ΔA_y)+2q₃(ΔV_z-ΔA_z)

t₂₅＝2q₀(ΔV_x-ΔA_x),t₂₆＝2q₂(ΔV_z-ΔA_z),t₃₇＝2q₃(ΔV_y-ΔA_y)

t₂₇＝2q₀(ΔV_x-ΔA_x)+2q₂(ΔV_z-ΔA_z)+2q₃(ΔV_y-ΔA_y)

t₂₈＝2q₀q₃,t₂₉＝q₀ ²,t₃₀＝q₁ ²,t₃₁＝q₂ ²,t₃₂＝q₃ ²,t₃₃＝2q₂(ΔV_x-ΔA_x)

t₃₄＝2q₃(ΔV_x-ΔA_x),t₃₅＝2q₀(ΔV_y-ΔA_y),t₃₈＝2q₀q₁

t₃₆＝2q₃(ΔV_x-ΔA_x)+2q₁(ΔV_z-ΔA_z)+2q₀(ΔV_y-ΔA_y)

状态噪声w_k的方差Q_k具体为：

其中：

Q_q＝BB^T,Q_v＝AA^T

(2)确定量测方程：

USBL/GPS量测矩阵为：

DVL量测矩阵为：

磁力计量测矩阵为：

H_mag＝[-2l₈l₁₄(q₃l₃-q₃l₄+q₃l₅+q₃l₆+2q₀q₁q₂)；-2l₈l₁₄(-q₂l₃+q₂l₄+q₂l₅+q₂l₆+2q₀q₁q₃)；2l₈l₁₄(q₁l₃+q₁l₄+q₁l₅-q₁l₆+2q₀q₂q₂)；2l₈l₁₄(q₀l₃+q₀l₄-q₀l₅+q₀l₆+2q₁q₂q₃)；o_1×12]_1×16其中：

l₉＝q₀q₃ l₁₀＝q₁q₂ l₂＝l₉+l₁₀

l₇＝l₃+l₄-l₅-l₆

t₁₂＝4l₈l₁₁ l₁₃＝1/(l₁₂+1.0)

量测噪声v_k的方差R_k根据实际的USBL/GPS、DVL和磁力计系统型号选定；

(3)通过状态方程和量测方程估算状态量X；

(4)通过[q₀ q₁ q₂ q₃]^T计算γ,θ,ψ；

(5)根据步骤(3)得到的状态量X中的(P_N,P_E,P_D)和(V_N,V_E,V_D)与步骤(4)得到的γ,θ,ψ，实现对AUV的组合导航实时定位。

7.根据权利要求6所述的基于多传感器数据融合的AUV组合导航方法，其特征在于：声学导航是指通过USBL提供位置信息，通过DVL提供速度信息的导航方式，USBL是指超短基线定位系统，DVL是指多普勒测速系统；卫星导航通过GPS提供位置信息；磁力计集成在惯性导航系统内部，用于提供航向信息。

8.根据权利要求6所述的基于多传感器数据融合的AUV组合导航方法，其特征在于：AUV在水面时，采用惯性导航、GPS、DVL导航组合的方式，AUV在水下时，选取惯性导航、DVL、USBL的组合的导航方式。

9.一种根据权利要求1所述的基于多传感器数据融合的AUV组合导航方法实现的AUV组合导航系统，其特征在于包括：

模型建立模块：建立惯性导航运动学模型；

运动参数定义模块：定义AUV的3维空间、6个自由度的运动参数；所述AUV是指无人水下潜航器；

组合导航系统模型确定模块：确定组合导航系统模型的基本形式；

组合导航模块：进行惯性导航、声学导航和卫星导航相结合的组合导航。

10.根据权利要求9所述的AUV组合导航系统，其特征在于：惯性导航运动学模型具体为：

惯性导航姿态更新为：

其中，Δθ＝[Δθ_x Δθ_y Δθ_z]^T表示载体坐标系下陀螺仪输出的三轴角度变化信息；ε＝[ε_x ε_y ε_z]^T表示载体坐标系下陀螺三轴常值漂移信息；[q₀ q₁ q₂ q₃]^T表示四元数信息，姿态更新时采用旋转矢量法更新四元数；Δq＝[Δq₀ Δq₁ Δq₂ Δq₃]^T为四元数变化量；

惯性导航速度更新为：

其中，V＝[V_N V_E V_D]^T表示n系下三维速度信息，

为捷联矩阵，ΔV＝[ΔV_x ΔV_y ΔV_z]^T表示b系下加速度计输出的三轴速度变化信息，g表示当地重力加速度，Δt表示采样时间间隔；ΔA＝[ΔA_x ΔA_y ΔA_z]^T表示b系下加速度计的常值零偏信息；

惯性导航位置更新为：

其中：P＝[P_N P_E P_D]^T表示n系下三维位置信息；

定为N-E-D的地理坐标系，原点位于载体当前所在位置，x_n,y_n在地理水平面内，x_n轴沿当地经线的切线向北，y_n轴沿当地纬线的切线向东，z_n轴垂直向下构成右手坐标系；

所述b系是指载体坐标系，载体坐标系即AUV本体坐标系，原点位于载体质心处，x_b沿载体轴向向前，y_b沿载体向右，z_b垂直于载体平面，三轴满足右手法则，随着载体的转动而转动；

定义AUV的3维空间、6个自由度的运动参数，具体为：

(P_N,P_E,P_D)：表示在导航坐标系下，AUV在三个坐标轴下的位置分量；

(V_N,V_E,V_D)：表示在导航坐标系下，AUV在三个坐标轴下的速度分量；

(γ,θ,ψ)：γ为横滚角，选取逆时针方向为正方向，其取值范围是(-180°,180°)；θ为俯仰角，正方向与横滚角相同，取值范围是(-90°,90°)；ψ为偏航角，左偏航为正，取值范围是(-180°,180°)。

Images