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

一种基于多传感器数据融合的auv导航定位方法及*** Download PDF

Info

Publication number
CN111829512B
CN111829512B CN202010514731.3A CN202010514731A CN111829512B CN 111829512 B CN111829512 B CN 111829512B CN 202010514731 A CN202010514731 A CN 202010514731A CN 111829512 B CN111829512 B CN 111829512B
Authority
CN
China
Prior art keywords
navigation
auv
carrier
representing
degrees
Prior art date
Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
Active
Application number
CN202010514731.3A
Other languages
English (en)
Other versions
CN111829512A (zh
Inventor
卢曼曼
郭眀禹
李宇航
张南南
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
China Academy of Aerospace Aerodynamics CAAA
Original Assignee
China Academy of Aerospace Aerodynamics CAAA
Priority date (The priority date is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the date listed.)
Filing date
Publication date
Application filed by China Academy of Aerospace Aerodynamics CAAA filed Critical China Academy of Aerospace Aerodynamics CAAA
Priority to CN202010514731.3A priority Critical patent/CN111829512B/zh
Publication of CN111829512A publication Critical patent/CN111829512A/zh
Application granted granted Critical
Publication of CN111829512B publication Critical patent/CN111829512B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Classifications

    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G01C21/10—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration
    • G01C21/12—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning
    • G01C21/16—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation
    • G01C21/165—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation combined with non-inertial navigation instruments
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G01C21/005—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 with correlation of navigation data from several sources, e.g. map or contour matching
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G01C21/20—Instruments for performing navigational calculations
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
    • G01S19/00—Satellite radio beacon positioning systems; Determining position, velocity or attitude using signals transmitted by such systems
    • G01S19/38—Determining a navigation solution using signals transmitted by a satellite radio beacon positioning system
    • G01S19/39—Determining a navigation solution using signals transmitted by a satellite radio beacon positioning system the satellite radio beacon positioning system transmitting time-stamped messages, e.g. GPS [Global Positioning System], GLONASS [Global Orbiting Navigation Satellite System] or GALILEO
    • G01S19/42—Determining position
    • G01S19/45—Determining position by combining measurements of signals from the satellite radio beacon positioning system with a supplementary measurement
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
    • G01S19/00—Satellite radio beacon positioning systems; Determining position, velocity or attitude using signals transmitted by such systems
    • G01S19/38—Determining a navigation solution using signals transmitted by a satellite radio beacon positioning system
    • G01S19/39—Determining a navigation solution using signals transmitted by a satellite radio beacon positioning system the satellite radio beacon positioning system transmitting time-stamped messages, e.g. GPS [Global Positioning System], GLONASS [Global Orbiting Navigation Satellite System] or GALILEO
    • G01S19/42—Determining position
    • G01S19/45—Determining position by combining measurements of signals from the satellite radio beacon positioning system with a supplementary measurement
    • G01S19/47—Determining position by combining measurements of signals from the satellite radio beacon positioning system with a supplementary measurement the supplementary measurement being an inertial measurement, e.g. tightly coupled inertial

Landscapes

  • Engineering & Computer Science (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Automation & Control Theory (AREA)
  • Computer Networks & Wireless Communication (AREA)
  • Navigation (AREA)
  • Position Fixing By Use Of Radio Waves (AREA)

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表示载体坐标系下陀螺三轴常值漂移信息；[q0 q1 q2 q3]T表示四元数信息，姿态更新时采用旋转矢量法更新四元数；Δq＝[Δq0 Δq1 Δq2 Δq3]T为四元数变化量；
惯性导航速度更新为：
其中，V＝[VN VE VD]T表示n系下三维速度信息，为捷联矩阵，ΔV＝[ΔVx ΔVyΔVz]T表示b系下加速度计输出的三轴速度变化信息，g表示当地重力加速度，Δt表示采样时间间隔；ΔA＝[ΔAx ΔAy ΔAz]T表示b系下加速度计的常值零偏信息；
惯性导航位置更新为：
其中：P＝[PN PE PD]T表示n系下三维位置信息。
进一步的，所述n系即为导航坐标系：定为N-E-D的地理坐标系，原点位于载体当前所在位置，xn,yn在地理水平面内，xn轴沿当地经线的切线向北，yn轴沿当地纬线的切线向东，zn轴垂直向下构成右手坐标系；
所述b系是指载体坐标系，载体坐标系即AUV本体坐标系，原点位于载体质心处，xb沿载体轴向向前，yb沿载体向右，zb垂直于载体平面，三轴满足右手法则，随着载体的转动而转动。
进一步的，定义AUV的3维空间、6个自由度的运动参数，具体为：
(PN,PE,PD)：表示在导航坐标系下，AUV在三个坐标轴下的位置分量；
(VN,VE,VD)：表示在导航坐标系下，AUV在三个坐标轴下的速度分量；
(γ,θ,ψ)：γ为横滚角，选取逆时针方向为正方向，其取值范围是(-180°,180°)；θ为俯仰角，正方向与横滚角相同，取值范围是(-90°,90°)；ψ为偏航角，左偏航为正，取值范围是(-180°,180°)。
进一步的，组合导航***模型的基本形式为
其中，X为状态量，Φk-1为状态转移矩阵，Z为量测量，Hk为量测矩阵，wk-1与vk代表过程噪声与量测噪声，服从正态分布wk-1～(0,Qk)，vk～(0,Rk)，Qk是过程噪声方差且非负定；Rk是量测噪声方差且正定。
进一步的，进行惯性导航、声学导航和卫星导航相结合的组合导航，具体为：
(1)确定状态方程，具体为：
状态量X＝[q0 q1 q2 q3 VN VE VD PN PE PD εx εy εz ΔAx ΔAy ΔAz]T；
状态转移矩阵Φk-1如下：
其中：
t15＝ΔVx-ΔAx,t16＝ΔVy-ΔAy,t17＝ΔVz-ΔAz
t19＝2q1(ΔVy-ΔAy),t20＝2q0(ΔVz-ΔAz)
t21＝2q1(ΔVx-ΔAx),t22＝2q0(ΔVy-ΔAy),t23＝2q1(ΔVz-ΔAz)
t24＝2q0(ΔVx-ΔAx)+2q2(ΔVy-ΔAy)+2q3(ΔVz-ΔAz)
t25＝2q0(ΔVx-ΔAx),t26＝2q2(ΔVz-ΔAz),t37＝2q3(ΔVy-ΔAy)
t27＝2q0(ΔVx-ΔAx)+2q2(ΔVz-ΔAz)+2q3(ΔVy-ΔAy)
t28＝2q0q3,t29＝q0 2,t30＝q1 2,t31＝q2 2,t32＝q3 2,t33＝2q2(ΔVx-ΔAx)
t34＝2q3(ΔVx-ΔAx),t35＝2q0(ΔVy-ΔAy),t38＝2q0q1
t36＝2q3(ΔVx-ΔAx)+2q1(ΔVz-ΔAz)+2q0(ΔVy-ΔAy)
状态噪声wk的方差Qk具体为：
其中：
Qq＝BBT,Qv＝AAT
(2)确定量测方程：
USBL/GPS量测矩阵为：
DVL量测矩阵为：
磁力计量测矩阵为：
Hmag＝[-2l8l14(q3l3-q3l4+q3l5+q3l6+2q0q1q2)；-2l8l14(-q2l3+q2l4+q2l5+q2l6+2q0q1q3)；
2l8l14(q1l3+q1l4+q1l5-q1l6+2q0q2q2)；2l8l14(q0l3+q0l4-q0l5+q0l6+2q1q2q3)；o1×12]1×16
其中：
l9＝q0q3 l10＝q1q2 l2＝l9+l10
l7＝l3+l4-l5-l6
t12＝4l8l11 l13＝1/(l12+1.0)
量测噪声vk的方差Rk根据实际的USBL/GPS、DVL和磁力计***型号选定；
(3)通过状态方程和量测方程估算状态量X；
(4)通过[q0 q1 q2 q3]T计算γ,θ,ψ；
(5)根据步骤(3)得到的状态量X中的(PN,PE,PD)和(VN,VE,VD)与步骤(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表示载体坐标系下陀螺三轴常值漂移信息；[q0 q1 q2 q3]T表示四元数信息，姿态更新时采用旋转矢量法更新四元数；Δq＝[Δq0 Δq1 Δq2 Δq3]T为四元数变化量。
(2)惯性导航速度更新
速度更新方程将三轴的加速度计信息，通过姿态矩阵转换至n系下，获得速度矢量，则速度更新环节中的矢量微分方程为：
式(2)中，V＝[VN VE VD]T表示n系下三维速度信息，为捷联矩阵，ΔV＝[ΔVx ΔVy ΔVz]T表示b系下加速度计输出的三轴速度变化信息，g表示当地重力加速度，Δt表示采样时间间隔；ΔA＝[ΔAx ΔAy ΔAz]T表示b系下加速度计的常值零偏信息。
将式(2)展开为三轴形式：
其中
所述n系即为导航坐标系：定为N-E-D的地理坐标系，原点位于载体当前所在位置，xn,yn在地理水平面内，xn轴沿当地经线的切线向北，yn轴沿当地纬线的切线向东，zn轴垂直向下构成右手坐标系；
所述b系是指载体坐标系，载体坐标系即AUV本体坐标系，原点位于载体质心处，xb沿载体轴向向前，yb沿载体向右，zb垂直于载体平面，三轴满足右手法则，随着载体的转动而转动。
(3)惯性导航位置更新
位置更新主即将速度积分获得载体相对于地球表面的运动，则位置更新方程为
其中：P＝[PN PE PD]T表示n系下三维位置信息。
2.超短基线定位***(USBL)模型
超短基线定位***通过测得由发射器在水下发送水声信号到目标海域海底安置已知应答器的往返时间计算得到斜距。根据水声信号传送到声头每个水听器的相位差计算得到运载体的方位角。
图4为USBL的定位原理示意图。令基阵的中心O作为坐标原点，天向反方向为z轴正向，根据右手定则，建立基阵坐标系oxyz。四个水听器坐落在坐标系x轴和y轴上，同轴水听器间间距为d，则各点坐标为(±d/2，±d/2)。应答器具***置为T处，其坐标为(xa,ya,za)，由图4易得：
xa＝lcosθx
ya＝lcosθy
USBL定位原理图中l为斜距，m为斜距l在水平面oxy上的投影，θ为水平方位角即m与x轴的夹角，θx和θy分别为l与x轴、y轴的夹角。
设定声波的波长为λ，表示同轴上水听器接收水声信号的相位差：
将式(7)代入到式(5)中，可得
斜距l＝c·Tri/2，Tri为问答机发送声学信号到应答器返回声学信号的往返时间，声波在水下传输速度为c。可由超短基线测量得到，由此能够计算出目标相对基阵坐标系的水平位置(xa,ya)、深度za和声波入射角(θx,θy)等。
USBL导航解算方程的坐标转换，把USBL中水听器的几何中心定为坐标原点，东北天坐标系用oxtytzt表示。当不考虑水听器安装误差时，超短基线***坐标系(u系)与载体坐标系(b系)的三轴空间内呈相互平行关系。u系(b系)、t系和e系三个坐标系在空间中的分布情况如图5所示。
从图5中可以看出，s与p分别为地球坐标系下应答器与载体的位置矢量，其中由于应答器是事先布置在海底的，所以s为已知位置矢量。r是应答器在载体坐标系下的位置矢量，载体坐标系到地球坐标系的变换矩阵用表示，由向量定理可得应答器的位置矢量为，获得USBL定位模型：
r＝RT(s-p) (9)
3.多普勒测速***(DVL)模型
DVL是现阶段海上航行载体测速重要导航手段之一，基于多普勒效应原理它能够长航时的提供载体精确的速度测量信息，和捷联惯导***能够形成良好的互补性，进而被广泛应用在各个领域。
多普勒效应是一种关于微波技术的现象。1842年，著名物理学家多普勒无意中在研究声学问题时发现一种现象，即在发射声波与反射声波物体间存在相对位移的时候，发射频率f1和反射频率f2并不一致。发射与反射频率的差值恰好与两者之间的相对速度呈现线性关系，这个频率差值即为多普勒频移。科学家们基于这种效应研制出了多普勒计程仪、多普勒雷达等***，下面主要介绍基于四射束的多普勒***测速方法。
四射束的多普勒***的波束空间和反射点位置如图6所示，
设载体的速度为v，四射束具有相同的倾角α和偏角β，声波沿每个方向的多普勒频移都是三个竖直方向速度分量的多普勒频移之和，四个射束的多普勒频移分别是
式中，Ai,Bi,Ci,Di(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)的地理坐标系，原点位于载体当前所在位置，xn,yn在地理水平面内，xn轴沿当地经线的切线向北，yn轴沿当地纬线的切线向东，zn轴垂直向下构成右手坐标系；
(PN,PE,PD)：表示在导航坐标系下，AUV在三个坐标轴下的位置分量；
(VN,VE,VD)：表示在导航坐标系下，AUV在三个坐标轴下的速度分量；
(γ,θ,ψ)：γ为横滚角，选取逆时针方向为正方向，其取值范围是(-180°,180°)；θ为俯仰角，正方向与横滚角相同，取值范围是(-90°,90°)；ψ为偏航角，左偏航为正，取值范围是(-180°,180°)。
(p,ωθ,ω)：表示在导航坐标系下，AUV沿着三个轴的角速度，规定逆时针方向为正。
步骤三：确定组合导航***模型的基本形式；
组合导航***模型的基本形式为
其中，X为状态量，Φk-1为状态转移矩阵，Z为量测量，Hk为量测矩阵，wk-1与vk代表过程噪声与量测噪声，服从正态分布wk-1～(0,Qk)，vk～(0,Rk)，Qk是过程噪声方差且非负定；Rk是量测噪声方差且正定。
步骤四：进行惯性导航、声学导航和卫星导航相结合的组合导航。
将设计的组合导航算法应用在实体AUV中，该导航***硬件设备组成见表1：
表1导航硬件设备组成
其中，惯导***SBG内部集成磁力计，可依靠该设备作为航向参考。
包括步骤如下：
(1)选取AUV在导航系中的三轴姿态、三轴速度、三轴位置以及三轴常值陀螺漂移、三轴常值加速度计零偏作为状态量，确定状态方程
状态量X＝[q0 q1 q2 q3 VN VE VD PN PE PD εx εy εz ΔAx ΔAy ΔAz]T；
状态转移矩阵Φk-1如下：
其中：
t15＝ΔVx-ΔAx,t16＝ΔVy-ΔAy,t17＝ΔVz-ΔAz
t19＝2q1(ΔVy-ΔAy),t20＝2q0(ΔVz-ΔAz)
t21＝2q1(ΔVx-ΔAx),t22＝2q0(ΔVy-ΔAy),t23＝2q1(ΔVz-ΔAz)
t24＝2q0(ΔVx-ΔAx)+2q2(ΔVy-ΔAy)+2q3(ΔVz-ΔAz)
t25＝2q0(ΔVx-ΔAx),t26＝2q2(ΔVz-ΔAz),t37＝2q3(ΔVy-ΔAy)
t27＝2q0(ΔVx-ΔAx)+2q2(ΔVz-ΔAz)+2q3(ΔVy-ΔAy)
t28＝2q0q3,t29＝q0 2,t30＝q1 2,t31＝q2 2,t32＝q3 2,t33＝2q2(ΔVx-ΔAx)
t34＝2q3(ΔVx-ΔAx),t35＝2q0(ΔVy-ΔAy),t38＝2q0q1
t36＝2q3(ΔVx-ΔAx)+2q1(ΔVz-ΔAz)+2q0(ΔVy-ΔAy)
Δθx,Δθy,Δθz为三轴陀螺仪输出，ΔVx,ΔVy,ΔVz为三轴加速度计输出。
状态噪声wk的方差Qk具体为：
其中：
Qq＝BBT,Qv＝AAT
(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量测矩阵：
其中：
量测噪声vk的方差Rk根据实际的USBL/GPS、DVL和磁力计***型号选定。
惯导***的更新频率设置为100Hz，磁力计集成在惯导***内部，频率与惯导***相同，DVL的更新频率大致为5Hz，GPS的位置更新频率为2Hz，速度更新频率为10Hz，USBL的更新频率为0.5Hz，由于频率的不一致性，本***采取序贯卡尔曼滤波的融合方式进行最优估计。
(3)通过状态方程和量测方程估算状态量X；
(4)通过[q0 q1 q2 q3]T计算γ,θ,ψ；
(5)根据步骤(3)得到的状态量X中的(PN,PE,PD)和(VN,VE,VD)与步骤(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 (6)

1.一种基于多传感器数据融合的AUV组合导航方法，其特征在于，包括以下几个步骤：
步骤一：建立惯性导航运动学模型；
步骤二：定义AUV的3维空间、6个自由度的运动参数；所述AUV是指无人水下潜航器；
步骤三：确定组合导航***模型的基本形式；
步骤四：进行惯性导航、声学导航和卫星导航相结合的组合导航；
惯性导航运动学模型具体为：
惯性导航姿态更新为：
其中，Δθ＝[Δθx Δθy Δθz]T表示载体坐标系下陀螺仪输出的三轴角度变化信息；ε＝[εx εy εz]T表示载体坐标系下陀螺三轴常值漂移信息；[q0 q1 q2 q3]T表示四元数信息，姿态更新时采用旋转矢量法更新四元数；Δq＝[Δq0 Δq1 Δq2 Δq3]T为四元数变化量；
惯性导航速度更新为：
其中，V＝[VN VE VD]T表示n系下三维速度信息，为捷联矩阵，ΔV＝[ΔVx ΔVy ΔVz]T表示b系下加速度计输出的三轴速度变化信息，g表示当地重力加速度，Δt表示采样时间间隔；ΔA＝[ΔAx ΔAy ΔAz]T表示b系下加速度计的常值零偏信息；
惯性导航位置更新为：
其中：P＝[PN PE PD]T表示n系下三维位置信息；
定义AUV的3维空间、6个自由度的运动参数，具体为：
(PN,PE,PD)：表示在导航坐标系下，AUV在三个坐标轴下的位置分量；
(VN,VE,VD)：表示在导航坐标系下，AUV在三个坐标轴下的速度分量；
(γ,θ,ψ)：γ为横滚角，选取逆时针方向为正方向，其取值范围是(-180°,180°)；θ为俯仰角，正方向与横滚角相同，取值范围是(-90°,90°)；ψ为偏航角，左偏航为正，取值范围是(-180°,180°)；
组合导航***模型的基本形式为
其中，X为状态量，Φk-1为状态转移矩阵，Z为量测量，Hk为量测矩阵，wk-1与vk代表过程噪声与量测噪声，服从正态分布wk-1～(0,Qk)，vk～(0,Rk)，Qk是过程噪声方差且非负定；Rk是量测噪声方差且正定。
2.根据权利要求1所述的基于多传感器数据融合的AUV组合导航方法，其特征在于：所述n系即为导航坐标系：定为N-E-D的地理坐标系，原点位于载体当前所在位置，xn,yn在地理水平面内，xn轴沿当地经线的切线向北，yn轴沿当地纬线的切线向东，zn轴垂直向下构成右手坐标系；
所述b系是指载体坐标系，载体坐标系即AUV本体坐标系，原点位于载体质心处，xb沿载体轴向向前，yb沿载体向右，zb垂直于载体平面，三轴满足右手法则，随着载体的转动而转动。
3.根据权利要求1所述的基于多传感器数据融合的AUV组合导航方法，其特征在于：进行惯性导航、声学导航和卫星导航相结合的组合导航，具体为：
(1)确定状态方程，具体为：
状态量X＝[q0 q1 q2 q3 VN VE VD PN PE PD εx εy εz ΔAx ΔAy ΔAz]T；
状态转移矩阵Φk-1如下：
其中：
t15＝ΔVx-ΔAx,t16＝ΔVy-ΔAy,t17＝ΔVz-ΔAz
t19＝2q1(ΔVy-ΔAy),t20＝2q0(ΔVz-ΔAz)
t21＝2q1(ΔVx-ΔAx),t22＝2q0(ΔVy-ΔAy),t23＝2q1(ΔVz-ΔAz)
t24＝2q0(ΔVx-ΔAx)+2q2(ΔVy-ΔAy)+2q3(ΔVz-ΔAz)
t25＝2q0(ΔVx-ΔAx),t26＝2q2(ΔVz-ΔAz),t37＝2q3(ΔVy-ΔAy)
t27＝2q0(ΔVx-ΔAx)+2q2(ΔVz-ΔAz)+2q3(ΔVy-ΔAy)
t28＝2q0q3,t29＝q0 2,t30＝q1 2,t31＝q2 2,t32＝q3 2,t33＝2q2(ΔVx-ΔAx)
t34＝2q3(ΔVx-ΔAx),t35＝2q0(ΔVy-ΔAy),t38＝2q0q1
t36＝2q3(ΔVx-ΔAx)+2q1(ΔVz-ΔAz)+2q0(ΔVy-ΔAy)
状态噪声wk的方差Qk具体为：
其中：
Qq＝BBT,Qv＝AAT
(2)确定量测方程：
USBL/GPS量测矩阵为：
DVL量测矩阵为：
磁力计量测矩阵为：
Hmag＝[-2l8l14(q3l3-q3l4+q3l5+q3l6+2q0q1q2)；-2l8l14(-q2l3+q2l4+q2l5+q2l6+2q0q1q3)；
2l8l14(q1l3+q1l4+q1l5-q1l6+2q0q2q2)；2l8l14(q0l3+q0l4-q0l5+q0l6+2q1q2q3)；o1×12]1×16其中：
l9＝q0q3 l10＝q1q2 l2＝l9+l10
l7＝l3+l4-l5-l6
t12＝4l8l11 l13＝1/(l12+1.0)
量测噪声vk的方差Rk根据实际的USBL/GPS、DVL和磁力计***型号选定；
(3)通过状态方程和量测方程估算状态量X；
(4)通过[q0 q1 q2 q3]T计算γ,θ,ψ；
(5)根据步骤(3)得到的状态量X中的(PN,PE,PD)和(VN,VE,VD)与步骤(4)得到的γ,θ,ψ，实现对AUV的组合导航实时定位。
4.根据权利要求3所述的基于多传感器数据融合的AUV组合导航方法，其特征在于：声学导航是指通过USBL提供位置信息，通过DVL提供速度信息的导航方式，USBL是指超短基线定位***，DVL是指多普勒测速***；卫星导航通过GPS提供位置信息；磁力计集成在惯性导航***内部，用于提供航向信息。
5.根据权利要求4所述的基于多传感器数据融合的AUV组合导航方法，其特征在于：AUV在水面时，采用惯性导航、GPS、DVL导航组合的方式，AUV在水下时，选取惯性导航、DVL、USBL的组合的导航方式。
6.一种根据权利要求1所述的基于多传感器数据融合的AUV组合导航方法实现的AUV组合导航***，其特征在于包括：
模型建立模块：建立惯性导航运动学模型；
运动参数定义模块：定义AUV的3维空间、6个自由度的运动参数；所述AUV是指无人水下潜航器；
组合导航***模型确定模块：确定组合导航***模型的基本形式；
组合导航模块：进行惯性导航、声学导航和卫星导航相结合的组合导航；
惯性导航运动学模型具体为：
惯性导航姿态更新为：
其中，Δθ＝[Δθx Δθy Δθz]T表示载体坐标系下陀螺仪输出的三轴角度变化信息；ε＝[εx εy εz]T表示载体坐标系下陀螺三轴常值漂移信息；[q0 q1 q2 q3]T表示四元数信息，姿态更新时采用旋转矢量法更新四元数；Δq＝[Δq0 Δq1 Δq2 Δq3]T为四元数变化量；
惯性导航速度更新为：
其中，V＝[VN VE VD]T表示n系下三维速度信息，为捷联矩阵，ΔV＝[ΔVx ΔVy ΔVz]T表示b系下加速度计输出的三轴速度变化信息，g表示当地重力加速度，Δt表示采样时间间隔；ΔA＝[ΔAx ΔAy ΔAz]T表示b系下加速度计的常值零偏信息；
惯性导航位置更新为：
其中：P＝[PN PE PD]T表示n系下三维位置信息；
定为N-E-D的地理坐标系，原点位于载体当前所在位置，xn,yn在地理水平面内，xn轴沿当地经线的切线向北，yn轴沿当地纬线的切线向东，zn轴垂直向下构成右手坐标系；
所述b系是指载体坐标系，载体坐标系即AUV本体坐标系，原点位于载体质心处，xb沿载体轴向向前，yb沿载体向右，zb垂直于载体平面，三轴满足右手法则，随着载体的转动而转动；
定义AUV的3维空间、6个自由度的运动参数，具体为：
(PN,PE,PD)：表示在导航坐标系下，AUV在三个坐标轴下的位置分量；
(VN,VE,VD)：表示在导航坐标系下，AUV在三个坐标轴下的速度分量；
(γ,θ,ψ)：γ为横滚角，选取逆时针方向为正方向，其取值范围是(-180°,180°)；θ为俯仰角，正方向与横滚角相同，取值范围是(-90°,90°)；ψ为偏航角，左偏航为正，取值范围是(-180°,180°)。
CN202010514731.3A 2020-06-08 2020-06-08 一种基于多传感器数据融合的auv导航定位方法及*** Active CN111829512B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202010514731.3A CN111829512B (zh) 2020-06-08 2020-06-08 一种基于多传感器数据融合的auv导航定位方法及***

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202010514731.3A CN111829512B (zh) 2020-06-08 2020-06-08 一种基于多传感器数据融合的auv导航定位方法及***

Publications (2)

Publication Number Publication Date
CN111829512A CN111829512A (zh) 2020-10-27
CN111829512B true CN111829512B (zh) 2024-04-09

Family

ID=72898524

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202010514731.3A Active CN111829512B (zh) 2020-06-08 2020-06-08 一种基于多传感器数据融合的auv导航定位方法及***

Country Status (1)

Country Link
CN (1) CN111829512B (zh)

Families Citing this family (7)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN112630769B (zh) * 2020-11-30 2023-12-12 东南大学 一种以无人机为载体的雷达流速测量***及方法
CN112987067B (zh) * 2021-02-24 2023-06-27 博雅工道（北京）机器人科技有限公司 组合导航的洋流速度估计方法、装置和一种处理器
CN113608169B (zh) * 2021-05-20 2023-08-25 济南大学 一种基于序贯融合算法的多auv协同定位方法
CN113776514A (zh) * 2021-08-10 2021-12-10 深之蓝海洋科技股份有限公司 液体下检测竖直表面的机器人的相对导航方法和装置
CN113959434B (zh) * 2021-09-22 2024-06-14 河北汉光重工有限责任公司 一种可调节的sins、dvl、usbl组合导航方法
CN114689039A (zh) * 2022-06-01 2022-07-01 青岛海舟科技有限公司 一种波浪滑翔器短时定位缺失的导航方法及***
CN115060274B (zh) * 2022-08-17 2022-11-18 南开大学 一种水下一体式自主导航装置及其初始对准方法

Citations (7)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN106643709A (zh) * 2016-10-10 2017-05-10 东南大学 一种海上运载体的组合导航方法及装置
CN109324330A (zh) * 2018-09-18 2019-02-12 东南大学 基于混合无导数扩展卡尔曼滤波的usbl/sins紧组合导航定位方法
CN109579850A (zh) * 2019-01-16 2019-04-05 中国海洋大学 基于对水速度辅助惯导的深水智能导航方法
CN109765523A (zh) * 2018-12-21 2019-05-17 山东省科学院海洋仪器仪表研究所 基于自适应akf的单应答器斜距水声定位方法及***
CN110006433A (zh) * 2019-04-22 2019-07-12 哈尔滨工程大学 海底油气管检测机器人的组合导航定位***及方法
CN110057365A (zh) * 2019-05-05 2019-07-26 哈尔滨工程大学 一种大潜深auv下潜定位方法
CN110207694A (zh) * 2019-05-27 2019-09-06 哈尔滨工程大学 一种基于相对位置信息的极区格网惯导/超短基线组合导航方法

Family Cites Families (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
WO2018080921A1 (en) * 2016-10-24 2018-05-03 Seabed Geosolutions B.V. Catenary modeling for a plurality of deployment lines for an offshore seismic system
US11442191B2 (en) * 2017-05-02 2022-09-13 Seabed Geosolutions B.V. System and method for deploying ocean bottom seismic nodes using a plurality of underwater vehicles

Patent Citations (7)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN106643709A (zh) * 2016-10-10 2017-05-10 东南大学 一种海上运载体的组合导航方法及装置
CN109324330A (zh) * 2018-09-18 2019-02-12 东南大学 基于混合无导数扩展卡尔曼滤波的usbl/sins紧组合导航定位方法
CN109765523A (zh) * 2018-12-21 2019-05-17 山东省科学院海洋仪器仪表研究所 基于自适应akf的单应答器斜距水声定位方法及***
CN109579850A (zh) * 2019-01-16 2019-04-05 中国海洋大学 基于对水速度辅助惯导的深水智能导航方法
CN110006433A (zh) * 2019-04-22 2019-07-12 哈尔滨工程大学 海底油气管检测机器人的组合导航定位***及方法
CN110057365A (zh) * 2019-05-05 2019-07-26 哈尔滨工程大学 一种大潜深auv下潜定位方法
CN110207694A (zh) * 2019-05-27 2019-09-06 哈尔滨工程大学 一种基于相对位置信息的极区格网惯导/超短基线组合导航方法

Non-Patent Citations (1)

* Cited by examiner, † Cited by third party
Title
光纤捷联惯导***的阻尼及校正技术研究;卢曼曼;《中国优秀硕士论文电子期刊》;正文全文 *

Also Published As

Publication number Publication date
CN111829512A (zh) 2020-10-27

Similar Documents

Publication Publication Date Title
CN111829512B (zh) 一种基于多传感器数据融合的auv导航定位方法及***
CN104316045B (zh) 一种基于sins/lbl的auv水下交互辅助定位***及定位方法
CN107314768B (zh) 水下地形匹配辅助惯性导航定位方法及其定位***
Zhang et al. Autonomous underwater vehicle navigation: a review
Bao et al. Integrated navigation for autonomous underwater vehicles in aquaculture: A review
Gade The seven ways to find heading
US8004930B2 (en) Methods and systems for determining coordinates of an underwater seismic component in a reference frame
CN106643709B (zh) 一种海上运载体的组合导航方法及装置
CN103744098B (zh) 基于sins/dvl/gps的auv组合导航***
CN111947651B (zh) 水下组合导航信息融合方法、***及自主式水下航行器
CN113311436B (zh) 一种移动平台上激光测风雷达运动姿态测风订正方法
CN102829777A (zh) 自主式水下机器人组合导航***及方法
CN111380518B (zh) 一种引入径向速度的sins/usbl紧组合导航定位方法
CN210719199U (zh) 一种水下机器人多设备组合导航***
CN106017460B (zh) 一种地形辅助惯导紧组合的水下潜器导航定位方法
Xu et al. A novel self-adapting filter based navigation algorithm for autonomous underwater vehicles
CN110806209A (zh) 一种水下机器人多设备组合导航***及方法
CN112556697A (zh) 一种基于联邦结构的浅耦合数据融合导航方法
CN110186461A (zh) 一种基于重力梯度信息测距的协同导航方法
CN105547289A (zh) 一种水下航行器组合导航***及导航信息融合方法
CN111735455A (zh) 基于改进的高斯距离迭代算法对接回收组合导航方法
CN112747748A (zh) 一种基于逆向解算的领航auv导航数据后处理方法
Armstrong et al. Field measurement of surface ship magnetic signature using multiple AUVs
CN112083425A (zh) 一种引入径向速度的sins/lbl紧组合导航方法
CN115930951A (zh) 一种基于多传感器组合的短期水下auv隐蔽导航方法

Legal Events

Date Code Title Description
PB01 Publication
PB01 Publication
SE01 Entry into force of request for substantive examination
SE01 Entry into force of request for substantive examination
GR01 Patent grant
GR01 Patent grant