CN112556697A - 一种基于联邦结构的浅耦合数据融合导航方法 - Google Patents
一种基于联邦结构的浅耦合数据融合导航方法 Download PDFInfo
- 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
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导航子滤波器的数学模型;
4、利用主滤波器的全局最优估计,反馈校正SINS的数据信息,循环迭代进行AUV对接回收自主导航。
前述的基于联邦结构的浅耦合数据融合导航方法,所述步骤2中,将联邦滤波结构与浅耦合数据融合模式相结合设计滤波***,具体内容为:
本组合导航***将联邦滤波结构与浅耦合数据融合导航方法结合,***采集SINS的导航信息作为参考***的信息来源,将USBL***的位置数据与DVL的速度数据作为辅助信息,通过将SINS采集的位置、速度数据分别与USBL、DVL量测信息作差,得到的差值传入至子滤波器当中,利用SINS/USBL子滤波器对导航位置作出状态估计,利用SINS/DVL子滤波器对导航速度作出最优估计,而USBL与DVL两个子***之间互不影响,不进行数据融合,也不直接对子滤波器进行数据反馈校正。
前述的基于联邦结构的浅耦合数据融合导航方法,步骤2中,SINS/USBL和SINS/DVL导航子滤波器的数学模型为:
⑴状态方程
式中,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组合导航子***的量测量:
式中,δ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组合导航子***的量测量为:
式中,δ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滤波算法的步骤,具体流程为:
⑵重要性采样:i=1,2,…,N,使用UKF算法更新粒子
①选取粒子:
②时间更新
③量测更新:
④采样粒子:
式(28)中,N(·)表示高斯函数。
⑶计算权重:
⑷归一化权值:
⑸计算有效粒子数:
⑹进行重采样:如果Nthr≤Neff≤N(Nthr为给定阈值参数),采用优化权值后的UPF算法进行估计以提高精度;如果Neff<Nthr,则说明粒子退化比较严重,需对粒子进行重采样,从而得到新的样本集重新分配权值
⑻返回步骤2。
前述的基于联邦结构的浅耦合数据融合导航方法,步骤⑷中对归一化权值进行优化,具体包括:
得到优化后的权值得到增大,能够避免粒子退化的现象。
与现有技术相比,本发明的有益效果是:
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)为
式中,q=[q0、q1、q2、q3]T为SINS的姿态误差四元数;δV=[δVE、δVN、δVU]T为SINS东、北、天方向的速度误差;δP=[δL、δλ、δh]T为SINS的经度、纬度和高度误差;ε=[εx、εy、εz]T表示陀螺的随机漂移;是加速度计的随机常值误差。
根据(1)式可得SINS/USBL/DVL组合导航***的状态方程为:
式中,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组合导航子***的量测量为:
ZP=PSINS-PUSBL=[LSINS-LUSBL,λSINS-λUSBL,hSINS-hUSBL]…(5)
进一步表示为:
式(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可以表示为:
在式(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),经过滤波融合获得组合导航***状态的两组局部最优估计值和局部最优误差协方差阵Pi(其中i=1,2)。如图4所示,为基于权值优化的无迹卡尔曼粒子滤波(UPF)算法的算法流程,其具体步骤为:
2、状态预测,即重要性采样:i=1,2,…,N,使用UKF算法更新粒子
①选取粒子:
②时间更新
③量测更新:
④采样粒子:
式(28)中,N(·)表示高斯函数。
3、计算权重:
4、权值优化:针对UPF算法重采样导致的粒子贫化问题,利用了一种重要性权值优化重采样思想,通过引入一个优化因子λ2来优化粒子的重要性权重,调整每个粒子的权值,提高权值小的粒子权值,降低权值大的粒子权值,加强了重采样后粒子的有效性和多样性,有效地提高了UPF算法的性能。采用的权值优化方法是基于每次重采样过程中对归一化之后的权值,通过引入一个优化因子λ2(-1<λ<1)来优化粒子的重要性权重,粒子的权重变为然后归一化权值,得到第k时刻第i个粒子的权重为:
5、计算有效粒子数:
6、进行重采样:如果Nthr≤Neff≤N(Nthr为给定阈值参数),采用优化权值后的UPF算法进行估计以提高精度;如果Neff<Nthr,则说明粒子退化比较严重,需对粒子进行重采样,从而得到新的样本集重新分配权值
8、返回步骤2
Step5利用主滤波器的全局最优估计,反馈校正参考***(SINS)的数据信息,循环迭代进行AUV对接回收自主导航。
除上述实施例外,本发明还可以有其他实施方式,凡采用等同替换或等效变换形成的技术方案,均落在本发明要求的保护范围内。
Claims (5)
1.一种基于联邦结构的浅耦合数据融合导航方法,其特征在于,该方法包含下列步骤:
(1)AUV首先通过传感器获取得到初始位置、姿态、速度等导航数据;
(2)在基于联邦结构的浅耦合数据融合导航方法中,将联邦滤波结构与浅耦合数据融合模式相结合设计滤波***,并建立SINS/USBL和SINS/DVL导航子滤波器的数学模型;
(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)状态方程
式中,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组合导航子***的量测量:
式中,δ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组合导航子***的量测量为:
式中,δ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滤波算法的具体流程为:
(2)重要性采样:i=1,2,…,N,使用UKF算法更新粒子
①选取粒子:
②时间更新
③量测更新:
④采样粒子:
式(28)中,N(·)表示高斯函数。
(3)计算权重:
(4)归一化权值:
(5)计算有效粒子数:
(6)进行重采样:如果Nthr≤Neff≤N(Nthr为给定阈值参数),采用优化权值后的UPF算法进行估计以提高精度;如果Neff<Nthr,则说明粒子退化比较严重,需对粒子进行重采样,从而得到新的样本集重新分配权值
(8)返回步骤2。
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)
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)
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 | 南京理工大学 | 自适应无迹卡尔曼粒子滤波方法 |
-
2020
- 2020-12-08 CN CN202011442969.6A patent/CN112556697A/zh active Pending
Patent Citations (6)
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)
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)
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导航定位方法及*** | |
CN103744098B (zh) | 基于sins/dvl/gps的auv组合导航*** | |
CN102829777B (zh) | 自主式水下机器人组合导航***及方法 | |
CN110567454B (zh) | 一种复杂环境下sins/dvl紧组合导航方法 | |
CN111273298B (zh) | 基于波浪滑翔器组网技术的水下声学目标定位与跟踪方法 | |
CN111596333B (zh) | 一种水下定位导航方法及*** | |
CN113834483B (zh) | 一种基于可观测度的惯性/偏振/地磁容错导航方法 | |
CN107797125B (zh) | 一种减小深海探测型auv导航定位误差的方法 | |
CN109579850B (zh) | 基于对水速度辅助惯导的深水智能导航方法 | |
CN110186461A (zh) | 一种基于重力梯度信息测距的协同导航方法 | |
CN112747748A (zh) | 一种基于逆向解算的领航auv导航数据后处理方法 | |
CN111735455A (zh) | 基于改进的高斯距离迭代算法对接回收组合导航方法 | |
CN104061930A (zh) | 基于捷联惯性制导和多普勒计程仪的导航方法 | |
Cahyadi et al. | Loosely Coupled GNSS and IMU Integration for Accurate i-Boat Horizontal Navigation. | |
CN115031726A (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 |