CN117606491B - 一种自主式水下航行器的组合定位导航方法及装置 - Google Patents

一种自主式水下航行器的组合定位导航方法及装置 Download PDF

Info

Publication number
CN117606491B
CN117606491B CN202410096634.5A CN202410096634A CN117606491B CN 117606491 B CN117606491 B CN 117606491B CN 202410096634 A CN202410096634 A CN 202410096634A CN 117606491 B CN117606491 B CN 117606491B
Authority
CN
China
Prior art keywords
sins
information
navigation
dvl
auv
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
CN202410096634.5A
Other languages
English (en)
Other versions
CN117606491A (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.)
Ocean University of China
Original Assignee
Ocean University of China
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 Ocean University of China filed Critical Ocean University of China
Priority to CN202410096634.5A priority Critical patent/CN117606491B/zh
Publication of CN117606491A publication Critical patent/CN117606491A/zh
Application granted granted Critical
Publication of CN117606491B publication Critical patent/CN117606491B/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/20—Instruments for performing navigational calculations
    • G01C21/203—Specially adapted for sailing ships
    • 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/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/18—Stabilised platforms, e.g. by gyroscope
    • 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
    • G01S15/00—Systems using the reflection or reradiation of acoustic waves, e.g. sonar systems
    • G01S15/02—Systems using the reflection or reradiation of acoustic waves, e.g. sonar systems using reflection of acoustic waves
    • G01S15/50—Systems of measurement, based on relative movement of the target
    • G01S15/58—Velocity or trajectory determination systems; Sense-of-movement determination systems
    • G01S15/588—Velocity or trajectory determination systems; Sense-of-movement determination systems measuring the velocity vector
    • 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
    • G01S15/00—Systems using the reflection or reradiation of acoustic waves, e.g. sonar systems
    • G01S15/86—Combinations of sonar systems with lidar systems; Combinations of sonar systems with systems not using wave reflection
    • 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)
  • Acoustics & Sound (AREA)
  • Navigation (AREA)

Abstract

本申请提供了一种自主式水下航行器的组合定位导航方法及装置，判断全球定位系统GPS是否能够获取定位信息；如果能够获取定位信息，则通过捷联惯性导航系统SINS和GPS组合导航；如果不能够获取定位信息，则通过捷联惯性导航系统SINS和多普勒计程仪DVL组合导航。本申请当AUV运行在水面时，对GPS原始位置进行基于MCKF自适应容错滤波后，再通过INS/GPS进行导航定位；当AUV下潜后或处于其它GPS无法正常定位的区域时，将DVL与INS使用卡尔曼滤波的方式进行组合，提供更为准确的预测结果。

Description

一种自主式水下航行器的组合定位导航方法及装置
技术领域
本申请涉及组合导航技术领域，尤其涉及一种自主式水下航行器的组合定位导航方法及装置。
背景技术
海洋中生物资源和矿产资源丰富，水下机器人被广泛应用于海洋的探索。自主式水下航行器(Autonomous underwater vehicle,AUV)在水下通过组合导航想要得到精确的导航结果，除了拥有合适的导航系统之外，还需选择恰当的滤波算法来对各个导航系统的数据进行处理。对于由水声信号多路径等问题产生脉冲噪声，导致水下组合导航实际应用中可能遇到量测噪声出现厚尾分布，或者由于AUV剧烈运动会导致传感器失效问题，因此缺乏可靠的导航技术仍然是限制水下机器人应用的重要因素。
由于AUV在远海作业时，环境条件相对苛刻，而惯性导航系统(InertialNavigation System，INS)在多数环境下可以自行对准，且可以不依赖于外部信息独立计算AUV的导航信息，因此适用于常规型AUV导航系统。然而由于传感器本身通过积分计算出位置、速度和姿态的特点，存在测量误差，I计算过程会导致误差的累积，系统的导航精度会随着时间不断发散。因此需要辅助GPS、DVL(Doppler Velocity Log，多普勒速度计)等传感器限制其误差的增长。
然而，AUV在水面航行时受到风浪的影响，无线信号容易产生“多径效应”，从而导致GPS给出的位置时常会有不确定性的跳变，导致水下组合导航实际应用中可能遇到量测噪声出现厚尾分布，GPS位置跳变会严重影响松耦合模式下的组合导航效果。
捷联惯性导航系统SINS结合多普勒计程仪DVL被广泛应用于水下机器人的自主导航，其能提供稳定的实时速度信息，且不会随时间积累误差，但是DVL能否提供连续的速度测量对组合导航精度至关重要，例如，在DVL发射的声波无法到达海底的情况下，DVL会以水跟踪的方式工作，提供水参考速度，或者无法提供速度。考虑到在任务期间DVL可能会长时间失效，因此DVL速度预测模型面临输入时序片段更长、预测精度要求更高的需求。
发明内容
本申请一种自主式水下航行器的组合定位导航方法及装置，本申请的技术方案如下：
根据本申请实施例的第一方面，提供一种自主式水下航行器的组合定位导航方法，包括：
S1:判断全球定位系统GPS是否能够获取自主式水下航行器AUV的定位信息；
S2:如果能够获取定位信息，则通过捷联惯性导航系统SINS和全球定位系统GPS组合导航，确定所述AUV的目标位置；
S3:如果不能够获取定位信息，则通过捷联惯性导航系统SINS和多普勒计程仪DVL组合导航，确定所述AUV的目标位置。
可选地，通过捷联惯性导航系统SINS和全球定位系统GPS组合导航确定所述AUV的目标位置，包括：
S21:获取SINS检测到的处于惯性坐标系的纯惯位置信息转换为导航坐标系中的速度、位置和姿态信息；
S22:将GPS获得的位置信息与SINS获取的纯惯位置信息进行融合，对SINS的纯惯位置信息进行误差补偿，使用滤波算法估计最优AUV状态以确定所述AUV的目标位置。
可选地，所述通过捷联惯性导航系统SINS和全球定位系统GPS组合导航确定所述AUV的目标位置，包括：
S221:判断GPS获取的位置信息是否有跳变野值；
S222:如果有跳变野值，则使用基于最大熵准则的卡尔曼滤波算法模型对所述GPS获取的AUV轨迹进行平滑后得到的后验估计值与所述SINS的解算信息进行数据融合；
S223:如果没有跳变野值，则使用基于最大熵准则的卡尔曼滤波算法的先验估计值与捷联惯导系统SINS的解算信息进行数据融合。
可选地，所述基于最大熵准则的卡尔曼滤波算法，具体包括：
S2221：构建基于SINS和GPS组合导航系统的线性离散时间模型：
xk＝φk|(k-1)xk-1+ωk-1Rk
zk＝Hkxk+vk
其中，xk和zk分别为k时刻的状态向量和量测向量；φk|(k-1)和Hk分别为系统转移矩阵和量测矩阵；ωk-1和vk分别为相互独立且均值为零的过程噪声和量测噪声，其协方差矩阵分别为Qk-1和Rk；
构建基于最大熵准则的卡尔曼滤波算法模型为：
其中，表示状态向量xk的先验估计；Pk|(k-1)表示先验误差协方差矩阵；上标T表示转置；Gσ表示高斯函数，ek,i为ek的第i个分量,i＝(1....n)，σ为高斯核函数的核宽；
S2222:通过设置状态向量初值和协方差矩阵初值P0进行滤波初始化；
S2223:通过求解状态向量的先验估计和先验误差协方差矩阵Pk|(k-1)进行时间更新：
S2224:根据时间更新后所得的量测信息计算ek，然后基于ek计算权矩阵ψk，并用权矩阵修改量测噪声协方差矩阵以进行量测更新：
利用修改后的量测噪声协方差矩阵计算卡尔曼增益Kk，得到后验估计值和后验误差协方差矩阵Pk：
Pk＝(I-KkHk)Pk|(k-1)
其中，I表示单位矩阵。
可选地，通过捷联惯性导航系统SINS和多普勒计程仪DVL组合导航确定所述AUV的目标位置，包括：
S31:获取SINS检测到的处于惯性坐标系的纯惯位置信息转换为导航坐标系中的速度、位置和姿态信息；
S32:将DVL获得的速度信息与SINS获取的纯惯速度信息进行融合，对SINS的纯惯位置信息进行误差补偿，使用滤波算法估计最优的AUV状态以确定所述AUV的目标位置。
可选地，所述通过捷联惯性导航系统SINS和多普勒计程仪DVL组合导航确定所述AUV的目标位置，具体包括：
当DVL正常工作时，将DVL的速度测量信息和SINS的解算信息进行数据融合，以及，用SINS的解算信息与DVL的速度测量信息进行Informer模型训练，其中，将SINS解算信息作为Informer模型的训练输入，训练目标输出为预设时间内DVL的速度测量信息；
如果DVL失效，则通过Informer模型预测结果代替失效的DVL信息与所述SINS解算信息进行数据融合。
可选地，所述Informer模型训练包括：
S321:构建Informer模型；
S322:读入SINS的解算信息和DVL的速度测量信息并进行数据预处理，所述SINS的解算信息和DVL的速度测量信息为时序数据，将所述时序数据按照6:3:1的比例划分为训练集、验证集、测试集后进行标准化，得到预处理后的时序数据；
S323:对预处理后的训练集ζinput进行正余弦位置编码，得到模型输入数据；
S324:从模型输入数据中选取一个Batch_size大小的数据，输入到构建的Informer模型中，对模型进行训练，其中，所述Batch_size大小设置为20；
S325:计算当前Batch_size大小的数据的预测值与真实值的Loss损失值，使用均方误差MSE损失来训练网络；
S326:根据所述Loss损失值，通过自适应运动估计算法ADAM进行误差的反向传播，更新模型网络权重参数；其中所述自适应运动估计算法ADAM中β1＝0.9，β2＝0.98，ò＝10-9，lr＝0.001，其中β1为控制一阶动量，β2为控制二阶动量，ò为模糊因子，lr为学习率；
S327:若所述训练集中的数据已经全部被训练完成，则将所述验证集SINS的解算信息输入到模型中，计算所述验证集中DVL预测值与真实值之间的Loss损失值；
S328:根据所述Loss损失值判断是否收敛且达到最高迭代次数，则将所述测试集的数据输入到训练完成的Informer模型中，得到预测数据；
S329:将预测数据进行反标准化处理，得到第一DVL速度增量预测值；并且将所述第一DVL速度增量预测值输出,通过速度累加得到k时刻DVL速度相对应的单位时间进行预测：
式中：(Vx0,Vy0,Vz0)表示当前任务的DVL前右下初始速度，(Vxk,Vyk,Vzk)分别表示k时刻DVL的在载体坐标系下的前右下速度。
可选地，所述Informer模型由编码器和解码器体系结构组成，所述编码器由第一输入层、位置编码层和编码器层的堆叠组成，所述解码器由第二输入层、解码器层和输出层堆叠组成；其中，
所述第一输入层，用于根据SINS和DVL采集的AUV姿态及速度等时序信息数据集ζinput通过全连接神经网络将输入的时间序列数据映射为维度为dmodel的向量；
所述位置编码层，用于对维度为dmodel的向量使用正余弦位置编码方式进行位置编码，得到位置编码向量模型输入数据；所述正余弦位置编码公式为：
其中，PE表示局部时间戳，pos表示向量位置，i是向量中的第i个维度，dmodel是指模型的维数，10000用于控制sin和cos函数的频率，指数2i/dmodel控制频率在维度上的增速；
所述编码器层，用于将所述位置编码向量作为多头稀疏概率自注意力机制层的输入，其中，所述注意力机制包括查询向量Q，相关性向量K、被查询信息的向量V；以及使用KL散度度量查询向量Q的均匀分布和注意力概率分布之间的相似性；以及当相似性的值达到预设阈值时，查询向量Q在注意力分布中起主导作用，选取查询向量中超过阈值的向量组成新的查询向量根据该新的查询向量重新计算稀疏概率自注意力机制。
可选地，所述解码器层由两个多头自注意层组成；所述第二输入层的输入由第一输入层的输入序列从后截取的预测长度的序列与和所述预测长度等长的0填充序列组成。
根据本申请实施例的第二方面，一种自主式水下航行器的组合定位导航方法的装置包括：
判断模块，用于执行判断全球定位系统GPS是否能够获取自主式水下航行器AUV的定位信息；
水上组合导航模块，用于执行如果能够获取定位信息，则通过捷联惯性导航系统SINS和全球定位系统GPS组合导航确定所述AUV的目标位置；
水下组合导航模块，用于执行如果不能够获取定位信息，则通过捷联惯性导航系统SINS和多普勒计程仪DVL组合导航确定所述AUV的目标位置。
有益效果：
本申请提供了一种自主式水下航行器的组合定位导航方法及装置，本申请通过一种自主式水下航行器的组合定位导航方法解决不同场景下的导航需求，判断全球定位系统GPS是否能够获取自主式水下航行器AUV的定位信息；如果能够获取定位信息，则通过捷联惯性导航系统SINS和全球定位系统GPS组合导航，确定所述AUV的目标位置；如果不能够获取定位信息，则通过捷联惯性导航系统SINS和多普勒计程仪DVL组合导航，确定所述AUV的目标位置。本申请通过一种自主式水下航行器的组合定位导航方法解决不同场景下的导航需求，为AUV在水下或者水面动态运行提供更为准确的预测结果。
应当理解的是，以上的一般描述和后文的细节描述仅是示例性和解释性的，并不能限制本申请。
附图说明
此处的附图被并入说明书中并构成本说明书的一部分，示出了符合本申请的实施例，并与说明书一起用于解释本申请的原理，并不构成对本申请的不当限定。
图1是根据一示例性实施例示出的一种自主式水下航行器的组合定位导航方法的流程示意图；
图2是根据一示例性实施例示出的捷联惯性导航系统SINS和全球定位系统GPS组合导航的流程示意图；
图3根据另一示例性实施例示出的捷联惯性导航系统SINS和全球定位系统GPS组合导航流程示意图；
图4是根据一示例性实施例示出的捷联惯性导航系统SINS和多普勒计程仪DVL组合导航流程示意图；
图5是根据一示例性实施例示出的捷联惯性导航系统SINS和多普勒计程仪DVL组合导航流程示意图；
图6是根据一示例性实施例示出的一种自主式水下航行器的组合定位导航装置结构示意图。
具体实施方式
为了使本领域普通人员更好地理解本申请的技术方案，下面将结合附图，对本申请实施例中的技术方案进行清楚、完整地描述。
本发明基于INS的组合导航系统，通过GPS、DVL传感器辅助，结合AUV的实际工作状态和传感器的误差特性，提出了一种自主式水下航行器的组合定位导航方法。
图1为本申请一示例性实施例提供的一种自主式水下航行器的组合定位导航方法的流程示意图。如图1所示，自主式水下航行器的组合定位导航方法具体包括：
S1:判断全球定位系统GPS是否能够获取自主式水下航行器AUV的定位信息。
S2:如果能够获取定位信息，则通过捷联惯性导航系统SINS和全球定位系统GPS组合导航确定所述AUV的目标位置。
S3:如果不能够获取定位信息，则通过捷联惯性导航系统SINS和多普勒计程仪DVL组合导航确定所述AUV的目标位置。
本申请涉及的多种检测器包括INS(Inertial Navigation System，惯性导航系统)、GPS(Global Positioning System，全球定位系统)以及DVL(Doppler Velocity Log，多普勒速度计)。INS作为一种利用惯性传感器(陀螺仪和加速度计)测量载体的加速度和角速度信息的装置，通过积分计算出位置、速度和姿态的自主导航系统。GPS一种卫星导航系统，通过接收卫星发射的无线电信号来确定地球上任意位置的经纬度和高程信息。DVL(Doppler Velocity Log，多普勒速度计)：一种基于声纳技术的水下速度计，通过测量水声信号多普勒效应来确定水下载体的速度。
在一些实施例中，本申请针对以上多种检测器的特点，当AUV运行在水面时，对GPS原始位置进行基于MCKF自适应容错滤波后，再通过INS/GPS进行组合导航定位；当AUV下潜后或处于其它GPS无法正常定位的区域时，将DVL与INS使用卡尔曼滤波的方式进行组合导航。
一些实施例中，如图2所示，SINS由陀螺仪101和加速度计102组成，能够提供SINS机体框架中的角速度和比力。通过这些测量，可以获得包括位置、姿态和速度在内的导航输出，将GPS103获得的位置信息与SINS获取的纯惯位置信息进行融合，对SINS的纯惯位置信息进行误差补偿，使用滤波算法估计最优AUV状态以确定所述AUV的目标位置。
具体的，捷联惯性导航系统(SINS)和全球定位系统(GPS)组合导航的流程具体包括：
S21:获取SINS检测到的处于惯性坐标系的纯惯位置信息转换为导航坐标系中的速度、位置和姿态信息。
AUV在水面运动时时，惯性传感器SINS和GPS接收机的数据直接进行融合来实现组合导航。SINS中的惯性传感器陀螺仪和加速度计测量出自主式水下航行器(AUV)的姿态和加速度信息，将陀螺仪和加速度计的数据转换为导航坐标系中的速度、位置和姿态信息。
S22:将GPS获得的位置信息与SINS获取的纯惯位置信息进行融合，对SINS的纯惯位置信息进行误差补偿，使用滤波算法估计最优AUV状态以确定所述AUV的目标位置。
在一些实施例中，如图3所示，通过捷联惯性导航系统SINS和全球定位系统GPS组合导航确定所述AUV的目标位置GPS原始位置进行基于MCKF自适应容错滤波的具体流程如下：
S221:判断GPS获取的位置信息是否有跳变野值；
S222:如果有跳变野值，则使用基于最大熵准则的卡尔曼滤波算法模型对所述GPS获取的AUV轨迹进行平滑后得到的后验估计值与所述SINS的解算信息进行数据融合；
S223:如果没有跳变野值，则使用基于最大熵准则的卡尔曼滤波算法的先验估计值与捷联惯导系统SINS的解算信息进行数据融合。
在一些实施例中，基于最大熵准则的卡尔曼滤波(MCKF)算法包括：
S2221：构建基于SINS和GPS组合导航系统的线性离散时间模型：
xk＝φk|(k-1)xk-1+ωk-1Rk
zk＝Hkxk+vk
其中，xk和zk分别为k时刻的状态向量和量测向量；φk|(k-1)和Hk分别为系统转移矩阵和量测矩阵；ωk-1和vk分别为相互独立且均值为零的过程噪声和量测噪声，其协方差矩阵分别为Qk-1和Rk；
构建基于最大熵准则的卡尔曼滤波算法模型为：
其中，表示状态向量xk的先验估计；Pk|(k-1)表示先验误差协方差矩阵；上标T表示转置；Gσ表示高斯函数，ek,i为ek的第i个分量,i＝(1....n)，σ为高斯核函数的核宽。
在一些实施例中，滤波器的系统状态如下：
XGps＝[xGps yGps vE VN aE aN]
上式中的xGPS和yGPS分别表示在通用横墨卡托网格系统(UTM)坐标系下东和北向的位置；vE和vN分别代表东和北向的运动速度；aE和aN分别为相应的加速度。
系统状态更新方程如下：
系统的观测模型为：
ZGPS＝[xGPS yGPS]
在最小均方误差(MMSE)准则下，最小化代价函数得到卡尔曼滤波(KF)的递推表达式：
其中，表示状态向量xk的先验估计；Pk|(k-1)表示先验误差协方差矩阵；其中上标T表示转置。
高斯核函数为：
其中，σ为高斯核函数的核宽；
为了增强滤波算法在非高斯量测噪声下的鲁棒性，将相关熵的概念引入代价函数的量测部分,定义ek为：
代价函数可修改为
其中，ek,i为ek的第i个分量,i＝(1....n)，σ为高斯核函数的核宽。
S2222:通过设置状态向量初值和协方差矩阵初值P0进行滤波初始化；
S2223:通过求解状态向量的先验估计和先验误差协方差矩阵Pk|(k-1)进行时间更新：
S2224:根据时间更新后所得的量测信息计算ek，然后基于ek计算权矩阵ψk，并用权矩阵修改量测噪声协方差矩阵以量测更新：
利用修改后的量测噪声协方差矩阵计算卡尔曼增益Kk，得到后验估计值和后验误差协方差矩阵Pk：
Pk＝(I-KkHk)Pk|(k-1)
其中，I表示单位矩阵。
本申请的AUV当在水面行驶时候，通过SINS和GPS组合导航提供完整位置、速度和姿态信息，但是AUV在水面航行，容易收到风浪的影响，无线信号容易产生“多径效应”，从而导致GPS产生不确定性的跳变。本申请在检测到GPS产生跳变的时候，使用MCKF对GPS轨迹进行平滑处理后，在与GPS进行组合导航，有效的减少了GPS的跳变额测量误差造成的影响，使得定位数据更加准确。
在本申请的另一些实施例中，当检测到无法获取GPS定位时候，确定AUV由水面运行至水下，则通过SINS和DVL组合导航确定所述AUV的目标位置。
如图4所示，SINS由陀螺仪101和加速度计102组成，能够提供SINS机体框架中的角速度和比力。通过这些测量，可以获得包括位置、姿态和速度在内的导航输出。DVL在多普勒仪器框架中提供速度。将DVL获得的速度信息与SINS获取的纯惯速度信息进行融合，对SINS的纯惯位置信息进行误差补偿，使用滤波算法以估计最优的AUV状态，以确定所述AUV的目标位置。
为了匹配SINS的速度，首先需要将其转化为带有对准标定参数的机体框架，DVL速度可以转化为带有SINS姿态的导航框架,作为速度匹配积分方案，选择DVL和SINS在导航坐标系下的速度作为卡尔曼滤波器的输入，通过卡尔曼滤波器的估计，导航输出可以重置。SINS和DVL组合的结构如图5所示:
S31:获取SINS检测到的处于惯性坐标系的纯惯位置信息转换为导航坐标系中的速度、位置和姿态信息；
S32:将DVL获得的速度信息与SINS获取的纯惯速度信息进行融合，对SINS的纯惯位置信息进行误差补偿，使用滤波算法以估计最优的AUV状态，以确定所述AUV的目标位置，其中，包括位置、速度中的至少一个导航信息。
DVL在某些情况下可能长时间无法提供速度，针对这一问题，本申请在DVL发生故障时，通过神经网络来预测速度测量值，后续的组合导航可以通过预测进行。
具体的，当DVL正常工作时，将DVL的速度测量信息和SINS的解算信息进行数据融合，以及，用SINS的解算信息与DVL的速度测量信息进行Informer模型训练，其中，将SINS解算信息作为Informer模型的训练输入，训练目标输出为预设时间内DVL的速度测量信息；
如果DVL失效，则通过Informer模型预测结果代替失效的DVL信息与所述SINS解算信息进行数据融合。
一些实施例中，用SINS解算信息与DVL输出信息进行Informer模型训练,当DVL正常工作时，将SINS解算信息作为Informer模型的训练输入，训练目标输出为未来一段时间内的DVL的速度测量值，当故障检测模块检测DVL是否失效时，如果DVL失效，通过Informer模型预测结果代替失效DVL信息进行组合导航，当DVL检测没有失效时，则通过SINS和DVL正常导航。通过滤波结果校正SINS输出，保持水下组合导航系统定位精度。
所述Informe模型架构如图5所示，可根据SINS和DVL采集的AUV姿态及速度等时序信息数据集ζinput，通过Informer架构计算DVL测量的前右下速度增量的时序信息数据集ζoutput。所述Informer模型由编码器和解码器体系结构组成，所述编码器由第一输入层501、位置编码层502和编码器层503的堆叠组成，所述解码器由第二输入层504、解码器层505和输出层506堆叠组成。
具体的，在所述第一输入层501中，根据SINS和DVL采集的AUV姿态及速度等时序信息数据集ζinput通过全连接神经网络将输入的时间序列数据映射为维度为dmodel的向量；
在位置编码层502中，对维度为dmodel的向量使用正余弦位置编码方式进行位置编码，得到位置编码向量模型输入数据；正余弦位置编码公式如下所示：
其中，PE表示局部时间戳，pos表示向量位置，i是向量中的第i个维度，dmodel是指模型的维数，10000用于控制sin和cos函数的频率，指数2i/dmodel控制频率在维度上的增速；
在编码器层503中，通过将输入向量与位置编码向量逐元素相加来对时间序列ζinput的数据中顺序信息进行编码，通过位置编码层将ζinput的顺序信息进入到模型中，最后生成位置编码向量被送入编码器层的多头稀疏概率自注意力机制层5031的输入，注意力机制包括查询向量Q，一般还包括被查询信息与其他信息的相关性向量K、被查询信息的向量V；在编码器层503计算查询向量Q的均匀分布和注意力概率分布；使用KL散度(Kullback-Leibler Divergence)度量查询向量Q的均匀分布和注意力概率分布之间的相似性；当相似性的值达到预设阈值时，查询向量Q在注意力分布中起主导作用，选取查询向量中超过阈值的向量组成新的查询向量Q，根据该新的查询向量重新计算稀疏概率自注意力机制。
更进一步的，经过正余弦编码的数据作为Informer模型输入，L表示输入序列的长度，表示输入序列中每一个元素经过位置编码之后的向量表示，令 WQ，WK，WV为权重矩阵，随机初始化，模型训练中根据loss进行梯度更新。Q，K，V作为自注意力机制的输入，自注意力机制的计算过程可被表示为：
其中，Q、K、V分别是查询向量(query)、键向量(key)和值向量(value)组成的矩阵d是防止梯度消失而增加的比例因子，softmax()是激活函数。
第i个查询向量qi的注意力系数的概率形式是:
其中，k(qi,kj)为非对称的指数核函数表示注意力概率分布。
使用q(kj|qi)＝1/LK表示均匀分布，LK表示Q向量的长度，即输入长度；若查询向量qi的注意力概率分布p(kj|qi)远离均匀分布q(kj|qi)，则说明该查询向量qi对于注意力权重占主要作用，应当保留该查询向量qi，使用KL散度公式度量注意力概率分布p(kj|qi)和均匀分布q(kj|qi)之间的相似性，可表示为：
其中，上式的第一项是qi对于所有的key的一个Log－Sum－Exp(另一种求取最大值的方法即LSE)，第二项则是它们的算数平均值。
KL(qi||p)表示查询向量qi的注意力概率分布p(kj|qi)与均匀分布q(kj|qi)之间的差异性度量值，当其结果较大时，则表示查询向量qi在注意力分布中起主导作用，选择这些主要的查询向量qi组成向量取代注意力公式中的Q矩阵，新的注意力公式可被表示为：
进一步的，在编码器层之间引入一条通道，连接多头注意力模块，也即多头注意力模块的输出不经过残差机制、归一化层、全连接层，而是通过直达注意力通道传达到下一层编码器作为多头注意力模块的附加输入，可表示为：
其中，Prev表示上一层编码器的多头注意力模块输出，初始值设为0。
由于稀疏注意力层的输出特征映射存在值V的冗余组合，因此通过蒸馏操作层5032将更高的权重赋予给具有主导特征的优势特征，并在生成下一个编码器层多头注意力层5031的特征映射.从j到j+1层的蒸馏操作的过程如下：
式中，主要描述第t个时间序列从j层至j+1层的蒸馏过程；为第一目标数据，[]表示包含了多头注意机制的Attention Block，即注意力块；Conv1d(·)表示在时间维度上的一维卷积滤波器，滤波后的数据通过ELU(·)激活函数层完成映射，最终由最大值池化层MaxPool(·)降低采样频率；由此完成对第一目标数据的蒸馏操作，形成第二目标数据。
通过Informer提出的时序问题使用自注意力蒸馏机制，使得每层的解码器都将输入序列的长度缩短一半，从而极大地节约了编码器Encoder的内存开销和计算时间。
在一些实施例中，解码器(Decoder)是由第二输入层504、解码器层505及输出层506组成。解码器输入从编码器输入的最后一个数据点开始，输入层将解码器通过输入层的全连接神经网络映射为dmodel维向量，与位置编码向量逐元素相加来对时间序列的数据中顺序信息进行编码，生成位置编码向量被送入解码器层。该解码层由两个一样的多头自注意层5051组成，用来缓解长期生成推理预测的速度下降，概率稀疏的自注意力(probsparseself-attention)在计算中应用了掩码多头注意力(masked multi-head attention)。
与编码器中多头概率稀疏自注意力不同，解码器中的自注意力机制为了防止每个位置关注未来的信息，避免自回归，将掩码多头概率稀疏自注意力机制(Masked Multi-head Probsparse Self-attention)的点积设置为-∞，最后通过输出层一个全连接层获得了最终的DVL的速度输出。
在一些实施例中，编码器输入一段历史数据，而解码器输入是由编码器输入序列从后开始截取的一段和与预测长度等长的“0”填充序列组成，也即所述第二输入层的输入由第一输入层的输入序列从后截取的预测长度的序列与和所述预测长度等长的0填充序列组成。
在一些实施例中，将SINS解算信息与DVL输出信息进行Informer模型训练，最终获取DVL的速度增量，所述Informer训练包括：
在S321中，构建所述Informer模型。
在S322中，读入SINS的解算信息和DVL的速度测量信息并进行数据预处理，所述SINS的解算信息和DVL的速度测量信息为时序数据，将所述时序数据按照6:3:1的比例划分为训练集、验证集、测试集后进行标准化，得到预处理后的时序数据。
在一些实施例中，描述AUV的运动状态需要相应的参考系，且传感器有自己独立的坐标系统，在实际应用之前通常需要对坐标系进行转化。本申请以“北—东—地”导航坐标系和“前—右—下”载体坐标系为参考，载体的航向角定义为以北向为零度，AUV以顺时针方向指向的角度；载体的俯仰角定义为水平面下，AUV头部上下变化的角度，以向上变化为正；横滚角为以AUV前轴进行的旋转角，顺时针转动为正。
载体坐标系与导航坐标系之间的旋转关系可以分解为三个角独立的角度变化，以载体系向导航坐标系转换为例，其中沿着航向的旋转矩阵：
沿着俯仰的旋转矩阵表示为：
沿着横滚的旋转矩阵表示为：
载体系至导航系的旋转矩阵可表示为载体先绕航向旋转，其次绕俯仰旋转，最后绕横滚旋转，因此旋转矩阵表示为三个矩阵相乘：
由于旋转矩阵为单位矩阵，单位矩阵的逆矩阵为矩阵本身的转置，根据载体系至导航系的旋转矩阵易求得导航系至载体系的旋转矩阵：
通常载体系至导航系的旋转矩阵用于导航传感器原始数据的坐标转换，从而应用于航位推算中；导航系至载体系的旋转矩阵用于INS与其它传感器的组合。
数据预处理将DVL的速度预测视为时间序列回归任务，将SINS及DVL观测的数据被用作Informer框架的输入数据来回归DVL的速度，由于DVL给出的是基于载体坐标下的速度，因此需要将SINS获取的速度做预处理，将其旋转至在载体坐标系下。
上式中的为导航坐标系至载体坐标系的旋转矩阵，vn为SINS获取的速度。
第i时刻的输入向量可以定义为：
ζi＝,(vxi,vyi,vzi,βi,αi,θi,axi,ayi,azi,ωxi,ωyi,ωzi)
式中：(vxi,vyi,vzi)为i时刻AUV机体框架内SINS测得的前进速度、右舷速度和向下速度。(βi,αi,θi)表示SINS单元观测到的AUV偏航、纵摇和横摇。(axi,ayi,azi)及(ωxi,ωyi,ωzi)分别为SINS在时刻i的三轴加速度和角速度。第i时刻到第i+m时刻的输入向量构成输入序列，输入序列可以表示为：ζinput＝{ζi,ζi+1,ζi+2,...,ζi+m}，i时刻的输入序列共包含周期为m的时间步的数据。
为了描述预测DVL的速度，将单位时间内的载体坐标系下的前右下速度增量作为框架的输出，输出序列为未来n个时刻后单位时间内的前右下速度增量的预测序列，描述为：
同时，训练标签为单位时间内的DVL测量的前右下速度增量，本发明定义一个单位时间为与DVL配置采样频率一致的时间间隔，训练标签序列描述为：
ζ_label＝{ζj,ζj+1,ζj+2,...,ζj+n}
ζj＝(δvxj,δvyj,δvzj)
其中，δvxj，δvyj及δvzj分别为j时刻载体坐标系下DVL测量的前右下速度增量。
为了生成标记数据集，我们使用了固定长度的滑动时间窗方法来构建用于模型训练和评估的ψinput、ψoutput对。在应用滑动窗口获取特征和标签之前，为了提高模型预测精度，归一化处理数据.利用StandardScaler原理去均值和方差归一化，标准差标准化(StandardScale)使得经过处理的数据符合标准正态分布，即均值为0，标准差为1，其转化函数为：
其中，μ为所有样本数据的均值，σ为所有样本数据的标准差。
然后，我们在缩放后的训练集上运行一个滑动窗口，以获得具有特征和标签的训练样本，它们分别是前M个和后N个观察值。
在S323中，对预处理后的训练集ζinput进行正余弦位置编码，得到模型输入数据。
对数据集ζinput进行正余位置编码，并通过输入层的全连接神经网络映射为dmodel维向量。编码层的公式如下所示：
在S324中，从模型输入数据中选取一个Batch_size大小的数据，输入到构建的Informer模型中，对模型进行训练，其中，所述Batch_size大小设置为20。
在S325中，计算当前Batch_size大小的数据的预测值与真实值的Loss损失值，使用均方误差(MSE)损失来训练网络。
一些实施例中，损失值为：
在S326中，根据所述Loss损失值，通过自适应运动估计算法(ADAM)进行误差的反向传播，更新模型网络权重参数；其中本发明自适应运动估计算法ADAM中β1＝0.9，β2＝0.98，ò＝10-9，lr＝0.001。其中β1为控制一阶动量，β2为控制二阶动量，ò为模糊因子，lr为学习率；
在S327中，若所述训练集中的数据已经全部被训练完成，则将所述验证集SINS的解算信息输入到模型中，计算所述验证集中DVL预测值与真实值之间的Loss损失值；
在S328中，根据所述Loss损失值判断是否收敛且达到最高迭代次数，则将所述测试集的数据输入到训练完成的Informer模型中，得到预测数据；
在S329中，将预测数据进行反标准化处理，得到第一DVL速度增量预测值，以及将所述第一DVL速度增量预测值输出,通过速度累加得到k时刻DVL速度对相应的单位时间进行预测：
式中：(Vx0,Vy0,Vz0)表示当前任务的DVL前后下初始速度，(Vxk,Vyk,Vzk)分别表示k时刻DVL的在载体坐标系下的前右下速度。
本申请所涉及的在AUV水下导航定位中，SINS为主导航系统，DVL测量的速度信息为外部辅助信息。采用间接滤波法，将SINS解算得到的导航坐标系与DVL测量的速度作为量测量进行导航滤波，实现对SINS解算的导航信息实时修正，保证AUV的长航时、高精度导航。
在一些实施例中，SINS/DVL组合通常采用，采用间接法滤波，间接滤波同样选择惯性仪器零偏作为状态变量，间接滤波适用姿态角误差、速度误差、位置误差作为状态变量，惯导更新信息作为输入，滤波结果需要反馈给惯导更新信息，补偿后的惯导更新信息作为导航信息输出，同时作为下一时刻的输入重新参与惯导更新，由于选择导航信息的误差和惯性器件零偏作为状态变量都属于小量，因此可以直接使用线性卡尔曼滤波。
本申请通过一种自主式水下航行器的组合定位导航方法解决不同场景下的导航需求，当自主式水下航行器AUV运行在水面时，对GPS原始位置进行基于MCKF自适应容错滤波后，再通过INS/GPS进行导航定位；当AUV下潜后或处于其它GPS无法正常定位的区域时，将DVL与INS使用卡尔曼滤波的方式进行组合，并建立基于Informer架构的DVL速度时序预测模型，相比于以往的时间序列预测模型，该模型能够在DVL故障期间，支持更长的时间序列片段输入和输出，并提供更为准确的预测结果。
图6为本申请一示例性实施例提供的一种自主式水下航行器的组合定位导航装置的结构示意图。如图6所示，本申请提供的一种自主式水下航行器的组合定位导航装置包括：
判断模块601，用于执行判断全球定位系统GPS是否能够获取自主式水下航行器AUV的定位信息；
水上组合导航处理模块602，用于执行如果能够获取定位信息，则通过捷联惯性导航系统SINS和全球定位系统GPS组合导航确定所述AUV的目标位置；
水下组合导航处理模块603，用于执行如果不能够获取定位信息，则通过捷联惯性导航系统SINS和多普勒计程仪DVL组合导航确定所述AUV的目标位置。
本申请实施例提供的装置可以具体用于执行上述图1对应方法实施例所提供的方案，具体功能和所能实现的技术效果此处不再赘述。
本发明实施例还提供一种非易失性存储设备包括：处理器，以及与处理器通信连接的存储器；
存储器存储计算机执行指令；
处理器执行存储器存储的计算机执行指令，以实现上述任一方法实施例所提供的方案，具体功能和所能实现的技术效果此处不再赘述。该电子设备可以为上述提及的服务器。
本申请实施例还提供一种计算机可读存储介质，计算机可读存储介质中存储有计算机执行指令，计算机执行指令被处理器执行时用于实现上述任一方法实施例所提供的方案，具体功能和所能实现的技术效果此处不再赘述。
本申请实施例还提供了一种计算机程序产品，程序产品包括：计算机程序，计算机程序存储在可读存储介质中，电子设备的至少一个处理器可以从可读存储介质读取计算机程序，至少一个处理器执行计算机程序使得电子设备执行上述任一方法实施例所提供的方案，具体功能和所能实现的技术效果此处不再赘述。
本申请实施例描述的应用场景是为了更加清楚的说明本申请实施例的技术方案，并不构成对于本申请实施例提供的技术方案的限定，本领域普通技术人员可知，随着新应用场景的出现，本申请实施例提供的技术方案对于类似的技术问题，同样适用。
所属技术领域的技术人员能够理解，本申请的各个方面可以实现为系统、方法或程序产品。因此，本申请的各个方面可以具体实现为以下形式，即：完全的硬件实施方式、完全的软件实施方式(包括固件、微代码等)，或硬件和软件方面结合的实施方式，这里可以统称为“电路”、“模块”或“系统”。
在一些可能的实施方式中，根据本申请的电子设备可以包括至少一个处理器、以及至少一个存储器。其中，存储器存储有程序代码，当程序代码被处理器执行时，使得处理器执行本说明书上述描述的根据本申请各种示例性实施方式的运营数据管理方法。例如，处理器可以执行如运营数据管理方法中的步骤。
应当注意，尽管在上文详细描述中提及了装置的若干单元或子单元，但是这种划分仅仅是示例性的并非强制性的。实际上，根据本申请的实施方式，上文描述的两个或更多单元的特征和功能可以在一个单元中具体化。反之，上文描述的一个单元的特征和功能可以进一步划分为由多个单元来具体化。
此外，尽管在附图中以特定顺序描述了本申请方法的操作，但是，这并非要求或者暗示必须按照该特定顺序来执行这些操作，或是必须执行全部所示的操作才能实现期望的结果。附加地或备选地，可以省略某些步骤，将多个步骤合并为一个步骤执行，和/或将一个步骤分解为多个步骤执行。
本领域内的技术人员应明白，本申请的实施例可提供为方法、系统、或计算机程序产品。因此，本申请可采用完全硬件实施例、完全软件实施例、或结合软件和硬件方面的实施例的形式。而且，本申请可采用在一个或多个其中包含有计算机可用程序代码的计算机可用存储介质(包括但不限于磁盘存储器、CD-ROM、光学存储器等)上实施的计算机程序产品的形式。
本申请是参照根据本申请实施例的方法、设备(系统)、和计算机程序产品的流程图和/或方框图来描述的。应理解可由计算机程序指令实现流程图和/或方框图中的每一流程和/或方框、以及流程图和/或方框图中的流程和/或方框的结合。可提供这些计算机程序指令到通用计算机、专用计算机、嵌入式处理机或其他可编程图像缩放设备的处理器以产生一个机器，使得通过计算机或其他可编程图像缩放设备的处理器执行的指令产生用于实现在流程图一个流程或多个流程和/或方框图一个方框或多个方框中指定的功能的装置。
这些计算机程序指令也可存储在能引导计算机或其他可编程图像缩放设备以特定方式工作的计算机可读存储器中，使得存储在该计算机可读存储器中的指令产生包括指令装置的制造品，该指令装置实现在流程图一个流程或多个流程和/或方框图一个方框或多个方框中指定的功能。
这些计算机程序指令也可装载到计算机或其他可编程图像缩放设备上，使得在计算机或其他可编程设备上执行一系列操作步骤以产生计算机实现的处理，从而在计算机或其他可编程设备上执行的指令提供用于实现在流程图一个流程或多个流程和/或方框图一个方框或多个方框中指定的功能的步骤。
显然，本领域的技术人员可以对本申请进行各种改动和变型而不脱离本申请的精神和范围。这样，倘若本申请的这些修改和变型属于本申请权利要求及其等同技术的范围之内，则本申请也意图包含这些改动和变型在内。

Claims (7)

1.一种自主式水下航行器的组合定位导航方法，其特征在于，所述组合定位导航方法包括：
S1:判断全球定位系统GPS是否能够获取自主式水下航行器AUV的定位信息；
S2:如果能够获取定位信息，则通过捷联惯性导航系统SINS和全球定位系统GPS组合导航，确定所述AUV的目标位置；
S3:如果不能够获取定位信息，则通过捷联惯性导航系统SINS和多普勒计程仪DVL组合导航，确定所述AUV的目标位置；
通过捷联惯性导航系统SINS和全球定位系统GPS组合导航确定所述AUV的目标位置，包括：
S21:获取SINS检测到的处于惯性坐标系的纯惯位置信息，并转换为导航坐标系中的速度、位置和姿态信息；
S22:将GPS获得的位置信息与SINS获取的纯惯位置信息进行融合，对所述SINS的纯惯位置信息进行误差补偿，以及使用滤波算法估计最优AUV状态以确定所述AUV的目标位置；
所述通过捷联惯性导航系统SINS和全球定位系统GPS组合导航确定所述AUV的目标位置，包括：
S221:判断GPS获取的位置信息是否有跳变野值；
S222:如果有跳变野值，则使用基于最大熵准则的卡尔曼滤波算法模型对所述GPS获取的AUV轨迹进行平滑后得到的后验估计值与所述SINS的解算信息进行数据融合；
S223:如果没有跳变野值，则使用基于最大熵准则的卡尔曼滤波算法的先验估计值与捷联惯导系统SINS的解算信息进行数据融合；
所述基于最大熵准则的卡尔曼滤波算法，具体包括：
S2221：构建基于SINS和GPS组合导航系统的线性离散时间模型：
xk＝φk|(k-1)xk-1+ωk-1Rk
zk＝Hkxk+vk
其中，xk和zk分别为k时刻的状态向量和量测向量；φk|(k-1)和Hk分别为系统转移矩阵和量测矩阵；ωk-1和vk分别为相互独立且均值为零的过程噪声和量测噪声，其协方差矩阵分别为Qk-1和Rk；
构建基于最大熵准则的卡尔曼滤波算法模型为：
其中，表示状态向量xk的先验估计；Pk|(k-1)表示先验误差协方差矩阵；上标T表示转置；Gσ表示高斯函数，ek，i为ek的第i个分量，i＝(1....n)，σ为高斯核函数的核宽；
S2222：通过设置状态向量初值和协方差矩阵初值P0进行滤波初始化；
S2223：通过求解状态向量的先验估计和先验误差协方差矩阵Pk(k-1)进行时间更新：
S2224：根据时间更新后所得的量测信息计算ek，然后基于ek计算权矩阵ψk，并用权矩阵修改量测噪声协方差矩阵以进行量测更新：
利用修改后的量测噪声协方差矩阵计算卡尔曼增益Kk，得到后验估计值和后验误差协方差矩阵Pk：
Pk＝(I-KkHk)Pk|(k-1)
其中，I表示单位矩阵。
2.如权利要求1所述的组合定位导航方法，其特征在于，通过捷联惯性导航系统SINS和多普勒计程仪DVL组合导航确定所述AUV的目标位置，包括：
S31：获取SINS检测到的处于惯性坐标系的纯惯位置信息转换为导航坐标系中的速度、位置和姿态信息；
S32：将DVL获得的速度信息与SINS获取的纯惯速度信息进行融合，对SINS的纯惯位置信息进行误差补偿，使用滤波算法估计最优的AUV状态以确定所述AUV的目标位置。
3.如权利要求2所述的组合定位导航方法，其特征在于，所述通过捷联惯性导航系统SINS和多普勒计程仪DVL组合导航确定所述AUV的目标位置，具体包括：
当DVL正常工作时，将DVL的速度测量信息和SINS的解算信息进行数据融合，以及，用SINS的解算信息与DVL的速度测量信息进行Informer模型训练，其中，将SINS解算信息作为Informer模型的训练输入，训练目标输出为预设时间内DVL的速度测量信息；
如果DVL失效，则通过Informer模型预测结果代替失效的DVL信息与所述SINS解算信息进行数据融合。
4.如权利要求3所述的组合定位导航方法，其特征在于，所述Informer模型训练包括：
S321:构建Informer模型；
S322:读入SINS的解算信息和DVL的速度测量信息并进行数据预处理，所述SINS的解算信息和DVL的速度测量信息为时序数据，将所述时序数据按照6:3:1的比例划分为训练集、验证集、测试集后进行标准化，得到预处理后的时序数据；
S323:对预处理后的训练集ζinput进行正余弦位置编码，得到模型输入数据；
S324:从模型输入数据中选取一个Batch_size大小的数据，输入到构建的Informer模型中，对模型进行训练，其中，所述Batch_size大小设置为20；
S325:计算当前Batch_size大小的数据的预测值与真实值的Loss损失值，使用均方误差MSE损失来训练网络；
S326:根据所述Loss损失值，通过自适应运动估计算法ADAM进行误差的反向传播，更新模型网络权重参数；其中所述自适应运动估计算法ADAM中β1＝0.9，β2＝0.98，ò＝10-9，lr＝0.001，其中β1为控制一阶动量，β2为控制二阶动量，ò为模糊因子，lr为学习率；
S327:若所述训练集中的数据已经全部被训练完成，则将所述验证集SINS的解算信息输入到模型中，计算所述验证集中DVL预测值与真实值之间的Loss损失值；
S328:根据所述Loss损失值判断是否收敛且达到最高迭代次数，则将所述测试集的数据输入到训练完成的Informer模型中，得到预测数据；
S329:将预测数据进行反标准化处理，得到第一DVL速度增量预测值；并且将所述第一DVL速度增量预测值输出,通过速度累加得到k时刻DVL速度相对应的单位时间进行预测：
式中：(Vx0,Vy0,Vz0)表示当前任务的DVL前右下初始速度，(Vxk,Vyk,Vzk)分别表示k时刻DVL的在载体坐标系下的前右下速度。
5.如权利要求4所述的组合定位导航方法，其特征在于：所述Informer模型由编码器和解码器体系结构组成，所述编码器由第一输入层、位置编码层和编码器层的堆叠组成，所述解码器由第二输入层、解码器层和输出层堆叠组成；其中，
所述第一输入层，用于根据SINS和DVL采集的AUV姿态及速度等时序信息数据集ζinput通过全连接神经网络将输入的时间序列数据映射为维度为dmodel的向量；
所述位置编码层，用于对维度为dmodel的向量使用正余弦位置编码方式进行位置编码，得到位置编码向量模型输入数据；所述正余弦位置编码公式为：
其中，PE表示局部时间戳，pos表示向量位置，i是向量中的第i个维度，dmodel是指模型的维数，10000用于控制sin和cos函数的频率，指数2i/dmodel控制频率在维度上的增速；
所述编码器层，用于将所述位置编码向量作为多头稀疏概率自注意力机制层的输入，其中，所述注意力机制包括查询向量Q，相关性向量K、被查询信息的向量V；以及使用KL散度度量查询向量Q的均匀分布和注意力概率分布之间的相似性；以及当相似性的值达到预设阈值时，查询向量Q在注意力分布中起主导作用，选取查询向量中超过阈值的向量组成新的查询向量根据该新的查询向量重新计算稀疏概率自注意力机制。
6.如权利要求5所述的组合定位导航方法，其特征在于：所述解码器层由两个多头自注意层组成；所述第二输入层的输入由第一输入层的输入序列从后截取的预测长度的序列与和所述预测长度等长的0填充序列组成。
7.一种自主式水下航行器的组合定位导航装置，其特征在于，包括：
判断模块，用于执行判断全球定位系统GPS是否能够获取自主式水下航行器AUV的定位信息；
水上组合导航模块，用于执行如果能够获取定位信息，则通过捷联惯性导航系统SINS和全球定位系统GPS组合导航确定所述AUV的目标位置；其中，通过捷联惯性导航系统SINS和全球定位系统GPS组合导航确定所述AUV的目标位置，包括：
获取SINS检测到的处于惯性坐标系的纯惯位置信息，并转换为导航坐标系中的速度、位置和姿态信息；
将GPS获得的位置信息与SINS获取的纯惯位置信息进行融合，对所述SINS的纯惯位置信息进行误差补偿，以及使用滤波算法估计最优AUV状态以确定所述AUV的目标位置；
所述通过捷联惯性导航系统SINS和全球定位系统GPS组合导航确定所述AUV的目标位置，包括：
判断GPS获取的位置信息是否有跳变野值；
如果有跳变野值，则使用基于最大熵准则的卡尔曼滤波算法模型对所述GPS获取的AUV轨迹进行平滑后得到的后验估计值与所述SINS的解算信息进行数据融合；
如果没有跳变野值，则使用基于最大熵准则的卡尔曼滤波算法的先验估计值与捷联惯导系统SINS的解算信息进行数据融合；
所述基于最大熵准则的卡尔曼滤波算法，具体包括：
构建基于SINS和GPS组合导航系统的线性离散时间模型：
xk＝φk(k-1)xk-1+ωk-1Rk
zk＝Hkxk+vk
其中，xk和zk分别为k时刻的状态向量和量测向量；φk|(k-1)和Hk分别为系统转移矩阵和量测矩阵；ωk-1和vk分别为相互独立且均值为零的过程噪声和量测噪声，其协方差矩阵分别为Qk-1和Rk；
构建基于最大熵准则的卡尔曼滤波算法模型为：
其中，表示状态向量xk的先验估计；Pk|(k-1)表示先验误差协方差矩阵；上标T表示转置；Gσ表示高斯函数，ek，i为ek的第i个分量，i＝(1....n)，σ为高斯核函数的核宽；
通过设置状态向量初值和协方差矩阵初值P0进行滤波初始化；
通过求解状态向量的先验估计和先验误差协方差矩阵Pk|(k-1)进行时间更新：
根据时间更新后所得的量测信息计算ek，然后基于ek计算权矩阵ψk，并用权矩阵修改量测噪声协方差矩阵以进行量测更新：
利用修改后的量测噪声协方差矩阵计算卡尔曼增益Kk，得到后验估计值和后验误差协方差矩阵Pk：
Pk＝(I-KkHk)Pk|(k-1)
其中，I表示单位矩阵；
水下组合导航模块，用于执行如果不能够获取定位信息，则通过捷联惯性导航系统SINS和多普勒计程仪DVL组合导航确定所述AUV的目标位置。
CN202410096634.5A 2024-01-24 2024-01-24 一种自主式水下航行器的组合定位导航方法及装置 Active CN117606491B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202410096634.5A CN117606491B (zh) 2024-01-24 2024-01-24 一种自主式水下航行器的组合定位导航方法及装置

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202410096634.5A CN117606491B (zh) 2024-01-24 2024-01-24 一种自主式水下航行器的组合定位导航方法及装置

Publications (2)

Publication Number Publication Date
CN117606491A CN117606491A (zh) 2024-02-27
CN117606491B true CN117606491B (zh) 2024-04-26

Family

ID=89956533

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202410096634.5A Active CN117606491B (zh) 2024-01-24 2024-01-24 一种自主式水下航行器的组合定位导航方法及装置

Country Status (1)

Country Link
CN (1) CN117606491B (zh)

Families Citing this family (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN117968681B (zh) * 2024-03-29 2024-07-05 北京大学 定位方法、装置、跨介质航行器及存储介质
CN118408541A (zh) * 2024-04-08 2024-07-30 北京航空航天大学 基于广义最大相关熵的自适应鲁棒gnss/ins组合导航信息融合方法

Citations (11)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN102004447A (zh) * 2010-11-11 2011-04-06 西北工业大学 水下航行器组合导航与控制半实物仿真试验系统
CN102829777A (zh) * 2012-09-10 2012-12-19 江苏科技大学 自主式水下机器人组合导航系统及方法
CN103744098A (zh) * 2014-01-23 2014-04-23 东南大学 基于sins/dvl/gps的auv组合导航系统
CN109141436A (zh) * 2018-09-30 2019-01-04 东南大学 改进的无迹卡尔曼滤波算法在水下组合导航中的应用方法
CN110514203A (zh) * 2019-08-30 2019-11-29 东南大学 一种基于isr-ukf的水下组合导航方法
WO2019242336A1 (zh) * 2018-06-22 2019-12-26 东南大学 一种水下滑翔器导航定位系统及上浮精度校正方法
WO2022028286A1 (zh) * 2020-08-04 2022-02-10 东南大学 一种运动约束辅助的基于改进Sage-Husa自适应滤波的水下组合导航方法
CN114705188A (zh) * 2022-05-11 2022-07-05 东南大学 一种基于格网水流速度辅助的惯性/多普勒中水层导航方法
CN114777812A (zh) * 2022-04-17 2022-07-22 中国人民解放军国防科技大学 一种水下组合导航系统行进间对准与姿态估计方法
CN115727837A (zh) * 2022-11-02 2023-03-03 宜昌测试技术研究所 基于航姿和dvl的小型auv导航定位的方法及装置
WO2023045357A1 (zh) * 2021-09-23 2023-03-30 哈尔滨工程大学 一种基于统计相似度量的组合导航鲁棒滤波方法

Patent Citations (11)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN102004447A (zh) * 2010-11-11 2011-04-06 西北工业大学 水下航行器组合导航与控制半实物仿真试验系统
CN102829777A (zh) * 2012-09-10 2012-12-19 江苏科技大学 自主式水下机器人组合导航系统及方法
CN103744098A (zh) * 2014-01-23 2014-04-23 东南大学 基于sins/dvl/gps的auv组合导航系统
WO2019242336A1 (zh) * 2018-06-22 2019-12-26 东南大学 一种水下滑翔器导航定位系统及上浮精度校正方法
CN109141436A (zh) * 2018-09-30 2019-01-04 东南大学 改进的无迹卡尔曼滤波算法在水下组合导航中的应用方法
CN110514203A (zh) * 2019-08-30 2019-11-29 东南大学 一种基于isr-ukf的水下组合导航方法
WO2022028286A1 (zh) * 2020-08-04 2022-02-10 东南大学 一种运动约束辅助的基于改进Sage-Husa自适应滤波的水下组合导航方法
WO2023045357A1 (zh) * 2021-09-23 2023-03-30 哈尔滨工程大学 一种基于统计相似度量的组合导航鲁棒滤波方法
CN114777812A (zh) * 2022-04-17 2022-07-22 中国人民解放军国防科技大学 一种水下组合导航系统行进间对准与姿态估计方法
CN114705188A (zh) * 2022-05-11 2022-07-05 东南大学 一种基于格网水流速度辅助的惯性/多普勒中水层导航方法
CN115727837A (zh) * 2022-11-02 2023-03-03 宜昌测试技术研究所 基于航姿和dvl的小型auv导航定位的方法及装置

Non-Patent Citations (4)

* Cited by examiner, † Cited by third party
Title
A Method for Underwater Dynamic Gravimetry Combining Inertial Navigation System, Doppler Velocity Log, and Depth Gauge;Xiong, Zhiming；Cao, Juliang；Wu, Meiping；Cai, Shaokun；Yu, Ruihang；Wang, Minghao;IEEE-INST ELECTRICAL ELECTRONICS ENGINEERS INC;20200831;全文 *
SINS/DVL/AST水下组合导航中的鲁棒信息融合方法;朱兵;常国宾;何泓洋;许江宁;;国防科技大学学报;20201031(第05期);全文 *
基于SINS/DVL/GPS的AUV组合导航标定方法的研究及其误差分析;吕志刚;;舰船电子工程;20180630(第06期);全文 *
基于逆向解算的领航AUV导航数据后处理方法;董权威;岳才谦;王奥博;王亭亭;;水下无人系统学报;20200831(第04期);全文 *

Also Published As

Publication number Publication date
CN117606491A (zh) 2024-02-27

Similar Documents

Publication Publication Date Title
CN117606491B (zh) 一种自主式水下航行器的组合定位导航方法及装置
CN109724599B (zh) 一种抗野值的鲁棒卡尔曼滤波sins/dvl组合导航方法
CN109737956B (zh) 一种基于双应答器的sins/usbl相位差紧组合导航定位方法
CN111156987B (zh) 基于残差补偿多速率ckf的惯性/天文组合导航方法
CN109901205B (zh) 一种水下机器人多传感器融合与运动轨迹预测方法
CN111596333B (zh) 一种水下定位导航方法及系统
Zhang et al. NavNet: AUV navigation through deep sequential learning
CN110823217A (zh) 一种基于自适应联邦强跟踪滤波的组合导航容错方法
CN110057365A (zh) 一种大潜深auv下潜定位方法
CN111486845A (zh) 基于海底地形匹配的auv多策略导航方法
Huang et al. Variational Bayesian-based filter for inaccurate input in underwater navigation
CN106568442B (zh) 一种具有鲁棒特性的协同导航滤波方法
Cohen et al. BeamsNet: A data-driven approach enhancing Doppler velocity log measurements for autonomous underwater vehicle navigation
US20230243658A1 (en) Systems, Methods and Devices for Map-Based Object's Localization Deep Learning and Object's Motion Trajectories on Geospatial Maps Using Neural Network
Xu et al. A novel self-adapting filter based navigation algorithm for autonomous underwater vehicles
CN111854741B (zh) 一种gnss/ins紧组合滤波器及导航方法
CN111076728A (zh) 基于dr/usbl的深潜载人潜水器组合导航方法
CN110906933A (zh) 一种基于深度神经网络的auv辅助导航方法
CN110375731A (zh) 一种混合交互式多模型滤波方法
CN115307643A (zh) 一种双应答器辅助的sins/usbl组合导航方法
Fagogenis et al. Improving underwater vehicle navigation state estimation using locally weighted projection regression
CN113503872A (zh) 一种基于相机与消费级imu融合的低速无人车定位方法
Jin et al. A compensation algorithm with motion constraint in DVL/SINS tightly coupled positioning
Yan et al. Cooperative navigation in unmanned surface vehicles with observability and trilateral positioning method
CN117848339A (zh) 一种小型无人自主航行器的水面水下组合导航方法与装置

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