CN112556697A - 一种基于联邦结构的浅耦合数据融合导航方法 - Google Patents

一种基于联邦结构的浅耦合数据融合导航方法 Download PDF

Info

Publication number
CN112556697A
CN112556697A CN202011442969.6A CN202011442969A CN112556697A CN 112556697 A CN112556697 A CN 112556697A CN 202011442969 A CN202011442969 A CN 202011442969A CN 112556697 A CN112556697 A CN 112556697A
Authority
CN
China
Prior art keywords
sins
usbl
navigation
dvl
weight
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.)
Pending
Application number
CN202011442969.6A
Other languages
English (en)
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.)
Jiangsu University of Science and Technology
Original Assignee
Jiangsu University of Science and Technology
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 Jiangsu University of Science and Technology filed Critical Jiangsu University of Science and Technology
Priority to CN202011442969.6A priority Critical patent/CN112556697A/zh
Publication of CN112556697A publication Critical patent/CN112556697A/zh
Pending legal-status Critical Current

Links

Images

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
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06F—ELECTRIC DIGITAL DATA PROCESSING
    • G06F17/00—Digital computing or data processing equipment or methods, specially adapted for specific functions
    • G06F17/10—Complex mathematical operations
    • G06F17/11—Complex mathematical operations for solving equations, e.g. nonlinear equations, general mathematical optimization problems
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06F—ELECTRIC DIGITAL DATA PROCESSING
    • G06F18/00—Pattern recognition
    • G06F18/20—Analysing
    • G06F18/25—Fusion techniques
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06F—ELECTRIC DIGITAL DATA PROCESSING
    • G06F30/00—Computer-aided design [CAD]
    • G06F30/20—Design optimisation, verification or simulation
    • G06F30/25—Design optimisation, verification or simulation using particle-based methods

Landscapes

  • Engineering & Computer Science (AREA)
  • Physics & Mathematics (AREA)
  • Remote Sensing (AREA)
  • Radar, Positioning & Navigation (AREA)
  • General Physics & Mathematics (AREA)
  • Theoretical Computer Science (AREA)
  • Data Mining & Analysis (AREA)
  • Mathematical Physics (AREA)
  • General Engineering & Computer Science (AREA)
  • Pure & Applied Mathematics (AREA)
  • Automation & Control Theory (AREA)
  • Computational Mathematics (AREA)
  • Evolutionary Computation (AREA)
  • Mathematical Analysis (AREA)
  • Mathematical Optimization (AREA)
  • Navigation (AREA)
  • Computer Hardware Design (AREA)
  • Computer Vision & Pattern Recognition (AREA)
  • Evolutionary Biology (AREA)
  • Artificial Intelligence (AREA)
  • Bioinformatics & Cheminformatics (AREA)
  • Life Sciences & Earth Sciences (AREA)
  • Operations Research (AREA)
  • Bioinformatics & Computational Biology (AREA)
  • Algebra (AREA)
  • Databases & Information Systems (AREA)
  • Software Systems (AREA)
  • Geometry (AREA)

Abstract

本发明公开了一种基于联邦结构的浅耦合数据融合导航方法，即针对SINS/USBL/DVL组合导航方法在AUV对接回收中的应用，提供了一种将联邦结构与浅耦合模式相结合的数据融合导航方法。利用联邦滤波结构与浅耦合数据融合模式结合，提高数据处理效率，并用基于权值优化的UPF代替传统的子滤波器中的Kalman滤波，通过优化权值，提高粒子多样性，解决粒子退化问题。本发明通过联邦滤波算法与浅耦合数据融合模式结合，提高数据处理效率，并且引入基于权值优化的UPF算法，解决AUV对接回收系统模型建立粗糙或者系统模型非高斯、非线性的问题，通过优化权值，使得粒子多样性增加，有效提高AUV对接回收中导航的精度和稳定性。

Description

一种基于联邦结构的浅耦合数据融合导航方法
技术领域
本发明涉及一种基于联邦结构的浅耦合数据融合导航方法，尤其涉及应用于AUV回收对接组合导航系统的数据融合方法及滤波算法的改进，属于水下机器人技术领域。
背景技术
随着人们对陆地资源的不断地开发利用，陆地资源的匮乏问题愈发引起广泛关注。进入21世纪，越来越多的国家把战略眼光转向了海洋资源的开发利用。水下机器人作为对海洋资源探索发开的重要工具，在水下资源探索开发的工作中发挥着至关重要的作用。按照不同的作业方式，水下机器人可分为二种：第一种有缆遥控水下机器人(RemoteOperated Vehicle，即ROV)，第二种是无缆自主水下机器人(Autonomous UnderwaterVehicle，即AUV)。
AUV的回收对接技术，涉及到控制、导航、信号、流体力学等众多领域。对AUV回收对接过程进行深入研究，能够推动水下机器人技术走向成熟，更好地开发海洋经济。在回收过程中，导航问题仍然是面临的主要技术挑战之一。导航技术是水下机器人的“眼睛”，持续为其提供机器人的姿态、位置、速度、距离等重要信息。精准的导航技术是实现AUV合理高效地回收与对接的前提与关键。但是AUV载体一般搭载有限的测量传感器，质量轻，易受水下噪声影响，实现AUV的精确导航是一项具有挑战性的工作。单传感器导航方式已经不能满足AUV回收对接的高精度要求，多传感器组合导航数据和定位方法信息融合，已经成为了目前解决单传感器精度低下问题的主要技术方法和重要发展方向。
SINS(Strapdown Inertial Navigation Systems，捷联惯性导航系统)利用陀螺仪和加速度计测量载体相对于惯性空间的角速度和加速度信息，通过解算可求出载体的三维速度、位置和姿态信息。SINS不需要任何外来信息，也不向外辐射信息，仅依靠系统自身就能在全球范围内实现连续的定位和定向。该系统已被广泛地应用于航海、航空、航天、交通等各个领域。但是，随着系统工作时间的延长，SINS系统的导航误差会随之积累增长，此时就需要利用外部传感器的观测信息通过滤波算法来修正补偿SINS系统，以抑制其随时间积累的误差。
USBL(Ultra-Short Baseline，超短基线定位系统)利用声学定位技术实现对水下目标的定位和跟踪。该导航系统定位精度较高，机动范围较大，与其他基于声学定位的基线系统相比，USBL基线尺寸较小(一般在几厘米至十几厘米的量级)、灵活性高、操作简单、成本较低，因此在水下作业和海洋开发等工程领域得到了广泛应用。但是USBL由于易受海洋环境的影响，经常会出现数据跳动或数据丢失现象，连续性差，在某些需要高精度导航定位的场合受到较大的限制。
DVL(Doppler Velocity Log，多普勒计程仪)是广泛应用于水下及水面航行器组合系统的速度测量装置。该装置利用安装在载体上的超声换能器向海底发射超声波，并根据多普勒效应原理测量载体速度，是具有实时输出载体三维速度与航程能力的声学导航定位系统。有抗干扰能力强、反应快、隐蔽性好、速度测量精确和稳定性高等优点。但是当DVL的波束超出了有效射程范围，测速精度会严重下降。
由于SINS、USBL和DVL都存在各自的优缺点，可以采用先进的信息融合技术，将USBL输出的位置信息与SINS位置进行数据信息融合，DVL输出的速度信息与SINS速度进行数据信息融合，最后对SINS的数据进行反馈校正，达到高精度导航的目的。该组合导航系统既能克服纯惯性导航系统误差积累问题，又能适用于水下导航定位的特殊环境，为水下载体和目标提供高精度的导航信息，是一种精度高、可靠性好、容错性强的水下导航系统。因此，对水下运载器SINS/USBL/DVL组合导航系统的研究具有重要的理论意义和工程应用价值。需要结合AUV实际应用情况，改进多传感器组合导航的数据融合模式与方法，提高AUV回收对接的精度与安全性。
目前AUV的导航系统采取集中式滤波，及利用SINS与USBL/DVL两两组合提高导航精度为主。申请号为“201710038292.1”的专利文献公开了“一种基于SINS/USBL紧组合的AUV水下导航定位方法”，并通过卡尔曼滤波对两传感器位置差数据进行滤波融合，通过反馈校正SINS的位置信息，此方法容错性不高，速度误差会随时间积累。并且只能建立近似的数学模型并且很难获得精确的噪声统计特性，若系统模型精确性差或者系统噪声为非高斯白噪声时，容易出现发散问题。申请号为“201911163364.0”的专利文献公开了“一种改进Sage-Husa自适应滤波的SINS/DVL组合导航方法”，通过改进的滤波算法融合SINS和DVL数据信息，修正导航数据(姿态、位置、速度等)，但是缺少USBL对位置信息的精确测量，精度不高。这样的集中式滤波，都存在滤波发散的问题，以及系统容错性能差，有一导航传感器故障，就会污染整个导航系统，使系统导航出现偏差。
发明内容
本发明主要提供了一种基于联邦结构的浅耦合数据融合导航方法，针对AUV回收对接SINS/USBL/DVL的组合导航系统，采用基于分级滤波的模式设计滤波结构，增强了系统的容错性能。在联邦结构的基础上，对子滤波器采用浅耦合数据融合模式，简化了系统结构，缩减了系统计算量。并且将优化权值的UPF，即优化权值的无迹卡尔曼粒子滤波算法引入子滤波器中，更好地处理非高斯、非线性模型系统，提高局部滤波器的适用性以及稳定性。通过优化权值，更加能够从一定程度上避免因为粒子权值过大，而导致的滤波过程中粒子退化的现象，即权值大的粒子增多，权值小的粒子减少，提高滤波过程中粒子的多样性，更加逼近真实状态。最后利用联邦滤波技术，在主滤波器中把子滤波器中的信息有机结合起来，通过二级滤波对数据融合处理，得更好的滤波效果，并对SINS进行数据反馈校正，提高导航系统精度。
为了达到上述系统设计目的，本发明采用以下技术方案予以实现：
一种基于联邦结构的浅耦合数据融合导航方法包含下列步骤：
1、AUV首先通过传感器获取得到初始位置、姿态、速度等导航数据；
2、在基于联邦结构的浅耦合数据融合导航方法中，将联邦滤波结构与浅耦合数据融合模式相结合设计滤波系统，并建立SINS/USBL和SINS/DVL导航子滤波器的数学模型；
3、在子滤波器中引入基于权值优化的UPF算法即基于权值优化的无迹卡尔曼粒子滤波算法，经过计算获得组合导航系统状态的两组局部最优估计值
Figure BDA0002823079250000031
和局部最优误差协方差阵Pi，其中i＝1，2；
4、利用主滤波器的全局最优估计，反馈校正SINS的数据信息，循环迭代进行AUV对接回收自主导航。
前述的基于联邦结构的浅耦合数据融合导航方法，所述步骤2中，将联邦滤波结构与浅耦合数据融合模式相结合设计滤波系统，具体内容为：
本组合导航系统将联邦滤波结构与浅耦合数据融合导航方法结合，系统采集SINS的导航信息作为参考系统的信息来源，将USBL系统的位置数据与DVL的速度数据作为辅助信息，通过将SINS采集的位置、速度数据分别与USBL、DVL量测信息作差，得到的差值传入至子滤波器当中，利用SINS/USBL子滤波器对导航位置作出状态估计，利用SINS/DVL子滤波器对导航速度作出最优估计，而USBL与DVL两个子系统之间互不影响，不进行数据融合，也不直接对子滤波器进行数据反馈校正。
前述的基于联邦结构的浅耦合数据融合导航方法，步骤2中，SINS/USBL和SINS/DVL导航子滤波器的数学模型为：
⑴状态方程
Figure BDA0002823079250000032
式中，f(x，t)为系统的状态矩阵；w(t)＝[wgx，wgy，wgz，wax，way，waz]T表示系统噪声；[wgx，wgy，wgz]为陀螺的白噪声，[wax，way，waz]为加速度计的白噪声；G(t)为系统的噪声驱动矩阵。
⑵量测方程
①SINS/USBL子滤波器量测方程：捷联惯性导航系统SINS通过对陀螺仪和加速度输出的角运动信息和线运动信息进行解算，可以获得载体的位置信息，在SINS/USBL的子系统中，需要把SINS的位置量测信息与USBL的位置量测信息作差得到滤波器的输入量，得到SINS/USBL组合导航子系统的量测量：
Figure BDA0002823079250000041
式中，δLSINS、δλSINS、δλUSBL为捷联惯导的经度、纬度、高度位值误差；δLUSBL、δλUSBL、δhUSBL的输出转换到地理坐标系g的经度、纬度和高度误差，设为白噪声。因此SINS/USBL导航系统量测方程为：
ZP＝H(t)X(t)+VUSBL(t)
其中，VUSBL(t)为USBL的高斯白噪声，方差为R，量测矩阵H为：
H(t)＝[03×7 03×3 03×6]
②SINS/DVL子滤波器量测方程：在SINS/DVL的子系统中，需要把SINS的速度量测信息与DVL的速度量测信息作差得到滤波器的输入量。则SINS/DVL组合导航子系统的量测量为：
Figure BDA0002823079250000042
式中,δVESINS、δVNSINS、δVUSINS代表SINS的速度误差，δVEDVL、δVNDVL、δVUDVL表示DVL的速度误差，假设其为白噪声，则SINS/DVL子滤波器的量测方程为：
ZV＝H(t)X(t)+VDVL(t)
其中,VDVL(t)为DVL的高斯白噪声，反差为R，量测矩阵H(t)为:H(t)＝[03×4 03×303×9]
前述的基于联邦结构的浅耦合数据融合导航方法，步骤(3)UPF滤波算法的步骤，具体流程为：
⑴初始化：k＝0参数设定，粒子数i＝1，2，…，N，仿真时间k＝1，2，…，K。根据先验分布中产生样本
Figure BDA0002823079250000043
Figure BDA0002823079250000044
Figure BDA0002823079250000045
Figure BDA0002823079250000051
Figure BDA0002823079250000052
⑵重要性采样：i＝1，2，…，N，使用UKF算法更新粒子
①选取粒子：
Figure BDA0002823079250000053
式(17)中，na＝nx+nw+nv且
Figure BDA0002823079250000054
即
Figure BDA0002823079250000055
为扩展后的状态向量。
②时间更新
Figure BDA0002823079250000056
Figure BDA0002823079250000057
Figure BDA0002823079250000058
Figure BDA0002823079250000059
Figure BDA00028230792500000510
式中，
Figure BDA00028230792500000511
为所有粒子点的一步预测加权和。
③量测更新：
Figure BDA00028230792500000512
Figure BDA00028230792500000513
Figure BDA00028230792500000514
Figure BDA00028230792500000515
Figure BDA00028230792500000516
④采样粒子：
Figure BDA00028230792500000517
式(28)中，N(·)表示高斯函数。
令：
Figure BDA00028230792500000518
⑶计算权重：
Figure BDA0002823079250000061
⑷归一化权值：
Figure BDA0002823079250000062
⑸计算有效粒子数：
Figure BDA0002823079250000063
⑹进行重采样：如果Nthr≤Neff≤N(Nthr为给定阈值参数)，采用优化权值后的UPF算法进行估计以提高精度；如果Neff<Nthr，则说明粒子退化比较严重，需对粒子进行重采样，从而得到新的样本集
Figure BDA0002823079250000064
重新分配权值
Figure BDA0002823079250000065
⑺将局部最优估计状态
Figure BDA0002823079250000066
和误差协方差Pi输出至主滤波器中，按照下式进行全局最优估计，得到全局最优估计。
Figure BDA0002823079250000067
⑻返回步骤2。
前述的基于联邦结构的浅耦合数据融合导航方法，步骤⑷中对归一化权值进行优化，具体包括：
采用的权值优化方法是基于每次重采样过程中对归一化之后的权值，通过引入一个影响因子λ2(-1<λ<1)来优化粒子的重要性权重，粒子的权重变为
Figure BDA0002823079250000068
然后归一化权值，得到第k时刻第i个粒子的权重为：
Figure BDA0002823079250000069
由上述公式可知
Figure BDA00028230792500000610
所以
Figure BDA00028230792500000611
随着i增加逐渐减小，则有
Figure BDA0002823079250000071
得到优化后的权值得到增大，能够避免粒子退化的现象。
与现有技术相比，本发明的有益效果是：
1、本发明方法，以联邦滤波结构为系统框架，结合浅耦合融合模式设计子滤波器，提高系统的系统运算效率与容错性能，通过二级滤波提升了导航系统的精度；
2、所建立的子滤波器数学模型采用差值法，传入子滤波器的估计量是SINS系统和USBL系统及DVL系统的差值，因为估计量较小，可以提高滤波器精度；
3、在子滤波器中，用UPF算法代替卡尔曼滤波算法进行滤波融合，能够更好的处理非高斯、非线性模型系统，增强系统的适用性和稳定性；
4、所采用的基于权值优化的UPF算法，通过引入优化因子λ2，对粒子的权重进行调整，提高权值小的粒子权值，降低权值大的粒子权值，加强了重采样后粒子的有效性和多样性，解决粒子退化问题，有效提高了UPF算法的性能。
附图说明
图1为本发明方法的组合导航系统的结构示意图；
图2为本发明方法的组合导航系统的SINS/USBL子滤波器结构示意图；
图3为本发明方法的组合导航系统的SINS/DVL子滤波器结构示意图；
图4为本发明方法的组合导航系统的子滤波器中基于权值优化的UPF算法流程图；
图5为一种基于联邦结构的浅耦合数据融合方法的流程图。
具体实施方式
如图1，针对AUV回收对接SINS/USBL/DVL的组合导航系统，采用基于分级滤波的模式设计滤波结构，增强了系统的容错性能。在联邦结构的基础上，对子系统采用浅耦合数据融合模式，简化了系统结构，缩减了系统计算量。并且将权值优化的无迹卡尔曼粒子滤波算法(UPF)引入子滤波器中，更好地处理非高斯、非线性模型系统，提高局部滤波器的适用性以及稳定性。通过优化权值，更加能够从一定程度上避免因为粒子权值过大，而导致的滤波过程中粒子退化的现象，即权值大的粒子增多，权值小的粒子减少，提高滤波过程中粒子的多样性，更加逼近真实状态。最后利用联邦滤波技术，在主滤波器中把子滤波器中的信息有机结合起来，通过两级滤波对数据融合处理，得更好的滤波效果，并对SINS进行数据反馈校正，提高导航系统精度。
为了达到上述系统设计目的，本发明采用以下技术方案予以实现：
一种基于联邦结构的浅耦合数据融合导航方法，包括以下步骤：
Step1 AUV首先通过传感器获取得到初始导航(位置、姿态、速度)数据；
具体内容：初始位姿信息主要从捷联惯性导航系统SINS、超短基线定位系统USBL、多普勒计程仪DVL这些传感器获得；其中捷联惯性导航系统SINS采集到AUV的三维速度、位置信息和姿态角信息，其中位置信息包括纬度、经度和高度信息，姿态角信息包括航向角、俯仰角和横滚角信息；超短基线定位系统USBL采集到回收装置在AUV的基阵坐标系内相对于三轴x轴、y轴和z轴的角度信息和分别投影到三轴x轴、y轴和z轴上的距离信息，根据实际情况采集其中信息；多普勒计程仪DVL采集到的AUV速度信息主要包括前进速度信息、横向速度信息、垂直速度信息中的任一种或任几种。
Step2在SINS/USBL/DVL组合导航系统设计中，由于捷联惯性导航系统SINS可以仅依靠自身系统全天候、全天时的提供AUV三维姿态、位置和速度信息，并且抗干扰能力强，因此，将SINS作为主导航系统，超短基线定位系统USBL/普勒计程仪DVL作为辅助导航系统，构成SINS/USBL/DVL组合导航系统,。如图2、3，首先设计SINS/USBL和SINS/DVL滤波器，再建立导航子滤波器的数学模型，其步骤为：
1、状态方程
SINS/USBL/DVL组合导航系统中，由于USBL的定位精度，DVL的速度精度都高于SINS，误差远小于SINS的位置和速度误差，且不随时间积累。因此，为了减少系统维度，在子系统模型中将只考虑SINS的误差作为状态变量，USBL和DVL的误差假设为理想的高斯白噪声。
选取东北天(E-N-U)地理坐标系g作为组合导航的导航坐标系n，根据惯导系统的误差方程，SINS/USBL/DVL组合导航系统的状态X(t)为
Figure BDA0002823079250000081
式中，q＝[q0、q1、q2、q3]T为SINS的姿态误差四元数；δV＝[δVE、δVN、δVU]T为SINS东、北、天方向的速度误差；δP＝[δL、δλ、δh]T为SINS的经度、纬度和高度误差；ε＝[εx、εy、εz]T表示陀螺的随机漂移；
Figure BDA0002823079250000082
是加速度计的随机常值误差。
根据(1)式可得SINS/USBL/DVL组合导航系统的状态方程为：
Figure BDA0002823079250000083
式中，f(x，t)为系统的状态矩阵；w(t)＝[wgx，wgy，wgz，wax，way，waz]T表示系统噪声；[wgx，wgy，wgz]为陀螺的白噪声，[wax，way，waz]为加速度计的白噪声；G(t)为系统的噪声驱动矩阵，且系统状态阵和噪声驱动阵分别为
Figure BDA0002823079250000091
Figure BDA0002823079250000092
2、量测方程
①SINS/USBL子滤波器量测方程：捷联惯性导航系统SINS通过对陀螺仪和加速度输出的角运动信息和线运动信息进行解算，可以获得载体的姿态、速度和位置数据信息。在SINS/USBL的子系统中，需要把SINS的位置量测信息与USBL的位置量测信息作差得到滤波器的输入量。则SINS/USBL组合导航子系统的量测量为：
ZP＝PSINS-PUSBL＝[LSINS-LUSBL，λSINS-λUSBL，hSINS-hUSBL]…(5)
进一步表示为：
Figure BDA0002823079250000093
式(6)中，δLSINS、δλSINS、δλUSBL为捷联惯导的经度、纬度、高度位值误差；δLUSBL、δλUSBL、δhUSBL的输出转换到地理坐标系g的经度、纬度和高度误差，设为白噪声。因此SINS/USBL导航系统量测方程为：
ZP＝H(t)X(t)+VUSBL(t)…(7)
式(7)中，VUSBL(t)为USBL的高斯白噪声，方差为R，量测矩阵H为：
H(t)＝[03×7 03×3 03×6]…(8)
②SINS/DVL子滤波器量测方程：在SINS/DVL的子系统中，需要把SINS的速度量测信息与DVL的速度量测信息作差得到滤波器的输入量。则SINS/DVL组合导航子系统的量测量为：
ZV＝[VESINS-VEDVL，VNSINS-VEDVL，VUSINS-VUDVL]T…(9)
在式(9)中VESINS、VNSINS、VUSINS是SINS输出的地理坐标系g下的东向、北向、天向速度信息，VEDVL、VEDVL、VUDVL是DVL输出的变换到地理坐标系g下的东向、北向、天向速度信息。量测信息ZV可以表示为：
Figure BDA0002823079250000101
在式(10)中,δVESINS、δVNSINS、δVUSINS代表SINS的速度误差，δVEDVL、δVNDVL、δVUDVL表示DVL的速度误差，假设其为白噪声，则SINS/DVL子滤波器的量测方程为：
ZV＝H(t)X(t)+VDVL(t)…(11)
在式(11)中,VDVL(t)为DVL的高斯白噪声，反差为R，量测矩阵H(t)为:
H(t)＝[03×4 03×3 03×9]…(12)
Step3在子滤波器中采用基于权值优化的无迹卡尔曼粒子滤波算法(UPF)，经过滤波融合获得组合导航系统状态的两组局部最优估计值
Figure BDA0002823079250000102
和局部最优误差协方差阵Pi(其中i＝1，2)。如图4所示，为基于权值优化的无迹卡尔曼粒子滤波(UPF)算法的算法流程，其具体步骤为：
1、初始化：k＝0参数设定，粒子数i＝1，2，…，N，仿真时间k＝1，2，…，K。根据先验分布中产生样本
Figure BDA0002823079250000103
Figure BDA0002823079250000104
Figure BDA0002823079250000105
Figure BDA0002823079250000106
Figure BDA0002823079250000107
2、状态预测，即重要性采样：i＝1，2，…，N，使用UKF算法更新粒子
①选取粒子：
Figure BDA0002823079250000108
式(17)中，na＝nx+nw+nv且
Figure BDA0002823079250000109
即
Figure BDA00028230792500001010
为扩展后的状态向量。
②时间更新
Figure BDA00028230792500001011
Figure BDA00028230792500001012
Figure BDA0002823079250000111
Figure BDA0002823079250000112
Figure BDA0002823079250000113
式中，
Figure BDA0002823079250000114
为所有粒子点的一步预测加权和。
③量测更新：
Figure BDA0002823079250000115
Figure BDA0002823079250000116
Figure BDA0002823079250000117
Figure BDA0002823079250000118
Figure BDA0002823079250000119
④采样粒子：
Figure BDA00028230792500001110
式(28)中，N(·)表示高斯函数。
令：
Figure BDA00028230792500001111
3、计算权重：
Figure BDA00028230792500001112
4、权值优化：针对UPF算法重采样导致的粒子贫化问题，利用了一种重要性权值优化重采样思想，通过引入一个优化因子λ2来优化粒子的重要性权重，调整每个粒子的权值，提高权值小的粒子权值，降低权值大的粒子权值，加强了重采样后粒子的有效性和多样性，有效地提高了UPF算法的性能。采用的权值优化方法是基于每次重采样过程中对归一化之后的权值，通过引入一个优化因子λ2(-1<λ<1)来优化粒子的重要性权重，粒子的权重变为
Figure BDA00028230792500001113
然后归一化权值，得到第k时刻第i个粒子的权重为：
Figure BDA00028230792500001114
5、计算有效粒子数：
Figure BDA0002823079250000121
6、进行重采样：如果Nthr≤Neff≤N(Nthr为给定阈值参数)，采用优化权值后的UPF算法进行估计以提高精度；如果Neff<Nthr，则说明粒子退化比较严重，需对粒子进行重采样，从而得到新的样本集
Figure BDA0002823079250000122
重新分配权值
Figure BDA0002823079250000123
7、将局部最优估计状态
Figure BDA0002823079250000124
和误差协方差Pi输出至主滤波器中，按照(33)式进行全局滤波，得到全局最优估计
Figure BDA0002823079250000125
8、返回步骤2
Step5利用主滤波器的全局最优估计，反馈校正参考系统(SINS)的数据信息，循环迭代进行AUV对接回收自主导航。
除上述实施例外，本发明还可以有其他实施方式，凡采用等同替换或等效变换形成的技术方案，均落在本发明要求的保护范围内。

Claims (5)

1.一种基于联邦结构的浅耦合数据融合导航方法，其特征在于，该方法包含下列步骤：
(1)AUV首先通过传感器获取得到初始位置、姿态、速度等导航数据；
(2)在基于联邦结构的浅耦合数据融合导航方法中，将联邦滤波结构与浅耦合数据融合模式相结合设计滤波系统，并建立SINS/USBL和SINS/DVL导航子滤波器的数学模型；
(3)在子滤波器中引入基于权值优化的UPF算法即基于权值优化的无迹卡尔曼粒子滤波算法，经过计算获得组合导航系统状态的两组局部最优估计值
Figure FDA0002823079240000012
和局部最优误差协方差阵Pi，其中i＝1，2；
(4)利用主滤波器的全局最优估计，反馈校正SINS的数据信息，循环迭代进行AUV对接回收自主导航。
2.如权利要求1所述的基于联邦结构的浅耦合数据融合导航方法，其特征在于，所述步骤(2)中，将联邦滤波结构与浅耦合数据融合模式相结合设计滤波系统，具体内容为：
本组合导航系统将联邦滤波结构与浅耦合数据融合导航方法结合，系统采集SINS的导航信息作为参考系统的信息来源，将USBL系统的位置数据与DVL的速度数据作为辅助信息，通过将SINS采集的位置、速度数据分别与USBL、DVL量测信息作差，得到的差值传入至子滤波器当中，利用SINS/USBL子滤波器对导航位置作出状态估计，利用SINS/DVL子滤波器对导航速度作出最优估计，而USBL与DVL两个子系统之间互不影响，不进行数据融合，也不直接对子滤波器进行数据反馈校正。
3.如权利要求1所述的基于联邦结构的浅耦合数据融合导航方法，其特征在于，所述步骤(2)中，SINS/USBL和SINS/DVL导航子滤波器的数学模型为：
(1)状态方程
Figure FDA0002823079240000011
式中，f(x，t)为系统的状态矩阵；w(t)＝[wgx，wgy，wgz，wax，way，waz]T表示系统噪声；[wgx，wgy，wgz]为陀螺的白噪声，[wax，way，waz]为加速度计的白噪声；G(t)为系统的噪声驱动矩阵。
(2)量测方程
①SINS/USBL子滤波器量测方程：捷联惯性导航系统SINS通过对陀螺仪和加速度输出的角运动信息和线运动信息进行解算，可以获得载体的位置信息，在SINS/USBL的子系统中，需要把SINS的位置量测信息与USBL的位置量测信息作差得到滤波器的输入量，得到SINS/USBL组合导航子系统的量测量：
Figure FDA0002823079240000021
式中，δLSINS、δλSINS、δλUSBL为捷联惯导的经度、纬度、高度位值误差；δLUSBL、δλUSBL、δhUSBL的输出转换到地理坐标系g的经度、纬度和高度误差，设为白噪声。因此SINS/USBL导航系统量测方程为：
ZP＝H(t)X(t)+VUSBL(t)
其中，VUSBL(t)为USBL的高斯白噪声，方差为R，量测矩阵H为：
H(t)＝[03×7 03×3 03×6]
②SINS/DVL子滤波器量测方程：在SINS/DVL的子系统中，需要把SINS的速度量测信息与DVL的速度量测信息作差得到滤波器的输入量。则SINS/DVL组合导航子系统的量测量为：
Figure FDA0002823079240000022
式中，δVESINS、δVNSINS、δVUSINS代表SINS的速度误差，δVEDVL、δVNDVL、δVUDVL表示DVL的速度误差，假设其为白噪声，则SINS/DVL子滤波器的量测方程为：
ZV＝H(t)X(t)+VDVL(t)
其中，VDVL(t)为DVL的高斯白噪声，反差为R，量测矩阵H(t)为：H(t)＝[03×4 03×3 03×9]。
4.如权利要求1所述的基于联邦结构的浅耦合数据融合导航方法，其特征在于，所述步骤(3)UPF滤波算法的具体流程为：
(1)初始化：k＝0参数设定，粒子数i＝1，2，…，N，仿真时间k＝1，2，…，K。根据先验分布中产生样本
Figure FDA0002823079240000023
Figure FDA0002823079240000024
Figure FDA0002823079240000025
Figure FDA0002823079240000026
Figure FDA0002823079240000027
(2)重要性采样：i＝1，2，…，N，使用UKF算法更新粒子
①选取粒子：
Figure FDA0002823079240000031
式(17)中，na＝nx+nw+nv且
Figure FDA0002823079240000032
即
Figure FDA0002823079240000033
为扩展后的状态向量。
②时间更新
Figure FDA0002823079240000034
Figure FDA0002823079240000035
Figure FDA0002823079240000036
Figure FDA0002823079240000037
Figure FDA0002823079240000038
式中，
Figure FDA0002823079240000039
为所有粒子点的一步预测加权和。
③量测更新：
Figure FDA00028230792400000310
Figure FDA00028230792400000311
Figure FDA00028230792400000312
Figure FDA00028230792400000313
Figure FDA00028230792400000314
④采样粒子：
Figure FDA00028230792400000315
式(28)中，N(·)表示高斯函数。
令：
Figure FDA00028230792400000316
(3)计算权重：
Figure FDA00028230792400000317
(4)归一化权值：
Figure FDA0002823079240000041
(5)计算有效粒子数：
Figure FDA0002823079240000042
(6)进行重采样：如果Nthr≤Neff≤N(Nthr为给定阈值参数)，采用优化权值后的UPF算法进行估计以提高精度；如果Neff＜Nthr，则说明粒子退化比较严重，需对粒子进行重采样，从而得到新的样本集
Figure FDA0002823079240000043
重新分配权值
Figure FDA0002823079240000044
(7)将局部最优估计状态
Figure FDA0002823079240000045
和误差协方差Pi输出至主滤波器中，按照下式进行全局最优估计，得到全局最优估计。
Figure FDA0002823079240000046
(8)返回步骤2。
5.如权利要求4所述的基于联邦结构的浅耦合数据融合导航方法，其特征在于，所述步骤(4)中对归一化权值进行优化，具体包括：
采用的权值优化方法是基于每次重采样过程中对归一化之后的权值，通过引入一个影响因子λ2(-1＜λ＜1)来优化粒子的重要性权重，粒子的权重变为
Figure FDA0002823079240000047
然后归一化权值，得到第k时刻第i个粒子的权重为：
Figure FDA0002823079240000048
由上述公式可知
Figure FDA0002823079240000049
所以
Figure FDA00028230792400000410
随着i增加逐渐减小，则有
Figure FDA00028230792400000411
得到优化后的权值得到增大，能够避免粒子退化的现象。
CN202011442969.6A 2020-12-08 2020-12-08 一种基于联邦结构的浅耦合数据融合导航方法 Pending CN112556697A (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202011442969.6A CN112556697A (zh) 2020-12-08 2020-12-08 一种基于联邦结构的浅耦合数据融合导航方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202011442969.6A CN112556697A (zh) 2020-12-08 2020-12-08 一种基于联邦结构的浅耦合数据融合导航方法

Publications (1)

Publication Number Publication Date
CN112556697A true CN112556697A (zh) 2021-03-26

Family

ID=75062670

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202011442969.6A Pending CN112556697A (zh) 2020-12-08 2020-12-08 一种基于联邦结构的浅耦合数据融合导航方法

Country Status (1)

Country Link
CN (1) CN112556697A (zh)

Cited By (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN113124863A (zh) * 2021-04-15 2021-07-16 南京航空航天大学 一种基于kld采样的混合粒子联邦滤波数据处理方法
CN113630164A (zh) * 2021-08-09 2021-11-09 南京航空航天大学 面向无人机毫米波通信平台的三维upf波束跟踪方法
CN113959434A (zh) * 2021-09-22 2022-01-21 河北汉光重工有限责任公司 一种可调节的sins、dvl、usbl组合导航方法
CN114492030A (zh) * 2022-01-25 2022-05-13 哈尔滨工业大学 基于实测数据回放的水下无人机导航算法调试系统
CN115855049A (zh) * 2023-02-07 2023-03-28 河海大学 基于粒子群优化鲁棒滤波的sins/dvl导航方法

Citations (6)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN101975585A (zh) * 2010-09-08 2011-02-16 北京航空航天大学 一种基于mrupf的捷联惯导系统大方位失准角初始对准方法
CN103278163A (zh) * 2013-05-24 2013-09-04 哈尔滨工程大学 一种基于非线性模型的sins/dvl组合导航方法
CN103684352A (zh) * 2013-12-18 2014-03-26 中国电子科技集团公司第五十四研究所 基于差分演化的粒子滤波方法
CN107589748A (zh) * 2017-08-21 2018-01-16 江苏科技大学 基于UnscentedFastSLAM算法的AUV自主导航方法
CN109062230A (zh) * 2018-08-06 2018-12-21 江苏科技大学 水下辅助采油机器人控制系统及动力定位方法
CN110455287A (zh) * 2019-07-24 2019-11-15 南京理工大学 自适应无迹卡尔曼粒子滤波方法

Patent Citations (6)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN101975585A (zh) * 2010-09-08 2011-02-16 北京航空航天大学 一种基于mrupf的捷联惯导系统大方位失准角初始对准方法
CN103278163A (zh) * 2013-05-24 2013-09-04 哈尔滨工程大学 一种基于非线性模型的sins/dvl组合导航方法
CN103684352A (zh) * 2013-12-18 2014-03-26 中国电子科技集团公司第五十四研究所 基于差分演化的粒子滤波方法
CN107589748A (zh) * 2017-08-21 2018-01-16 江苏科技大学 基于UnscentedFastSLAM算法的AUV自主导航方法
CN109062230A (zh) * 2018-08-06 2018-12-21 江苏科技大学 水下辅助采油机器人控制系统及动力定位方法
CN110455287A (zh) * 2019-07-24 2019-11-15 南京理工大学 自适应无迹卡尔曼粒子滤波方法

Non-Patent Citations (3)

* Cited by examiner, † Cited by third party
Title
LINGHUI BAO,ET AL: "AUV Docking Recovery Based on USBL Integrated Navigation Method", 《2019 IEEE》, pages 5804 - 5809 *
冉星浩: "基于无迹卡尔曼滤波和权值优化的改进粒子滤波算法", 《探 测 与 控 制 学报 》, pages 74 - 79 *
张国良: "基于大深度的SINS/DVL /USBL组合导航技术研究", 《中国优秀硕士学位论文全文数据库》, pages 30 - 66 *

Cited By (6)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN113124863A (zh) * 2021-04-15 2021-07-16 南京航空航天大学 一种基于kld采样的混合粒子联邦滤波数据处理方法
CN113630164A (zh) * 2021-08-09 2021-11-09 南京航空航天大学 面向无人机毫米波通信平台的三维upf波束跟踪方法
CN113630164B (zh) * 2021-08-09 2023-02-14 南京航空航天大学 一种面向无人机毫米波通信平台的三维upf波束跟踪方法
CN113959434A (zh) * 2021-09-22 2022-01-21 河北汉光重工有限责任公司 一种可调节的sins、dvl、usbl组合导航方法
CN114492030A (zh) * 2022-01-25 2022-05-13 哈尔滨工业大学 基于实测数据回放的水下无人机导航算法调试系统
CN115855049A (zh) * 2023-02-07 2023-03-28 河海大学 基于粒子群优化鲁棒滤波的sins/dvl导航方法

Similar Documents

Publication Publication Date Title
WO2020062791A1 (zh) 一种深海潜航器的sins/dvl水下抗晃动对准方法
CN112556697A (zh) 一种基于联邦结构的浅耦合数据融合导航方法
CN107314768B (zh) 水下地形匹配辅助惯性导航定位方法及其定位系统
CN109324330B (zh) 基于混合无导数扩展卡尔曼滤波的usbl/sins紧组合导航定位方法
Zhang et al. Autonomous underwater vehicle navigation: a review
CN111829512B (zh) 一种基于多传感器数据融合的auv导航定位方法及系统
CN102829777B (zh) 自主式水下机器人组合导航系统及方法
CN110567454B (zh) 一种复杂环境下sins/dvl紧组合导航方法
CN111596333B (zh) 一种水下定位导航方法及系统
CN113834483B (zh) 一种基于可观测度的惯性/偏振/地磁容错导航方法
CN111273298B (zh) 基于波浪滑翔器组网技术的水下声学目标定位与跟踪方法
CN112697138B (zh) 一种基于因子图优化的仿生偏振同步定位与构图的方法
CN109579850B (zh) 基于对水速度辅助惯导的深水智能导航方法
CN110186461A (zh) 一种基于重力梯度信息测距的协同导航方法
CN112747748A (zh) 一种基于逆向解算的领航auv导航数据后处理方法
CN111735455A (zh) 基于改进的高斯距离迭代算法对接回收组合导航方法
CN104061930A (zh) 基于捷联惯性制导和多普勒计程仪的导航方法
CN115031726A (zh) 一种数据融合导航定位方法
Cahyadi et al. Loosely Coupled GNSS and IMU Integration for Accurate i-Boat Horizontal Navigation.
CN111307136B (zh) 一种双智能水下机器人水下航行地形匹配导航方法
CN110954097A (zh) 一种用于机器人组合的导航定位方法
CN111829511A (zh) 一种基于m估计的auv组合导航方法及系统
CN103697887B (zh) 一种基于捷联惯导系统和多普勒计程仪的优化导航方法
CN113959434B (zh) 一种可调节的sins、dvl、usbl组合导航方法
CN113670303B (zh) 一种基于rbf神经网络的sins/dvl组合导航流速补偿方法

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