CN116702479A - 一种水下航行器未知输入与位置估计方法及系统 - Google Patents
一种水下航行器未知输入与位置估计方法及系统 Download PDFInfo
- Publication number
- CN116702479A CN116702479A CN202310690334.5A CN202310690334A CN116702479A CN 116702479 A CN116702479 A CN 116702479A CN 202310690334 A CN202310690334 A CN 202310690334A CN 116702479 A CN116702479 A CN 116702479A
- Authority
- CN
- China
- Prior art keywords
- estimation
- expression
- unknown
- underwater vehicle
- delay
- 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.)
- Granted
Links
- 238000000034 method Methods 0.000 title claims abstract description 36
- 238000005259 measurement Methods 0.000 claims abstract description 37
- 238000005457 optimization Methods 0.000 claims abstract description 20
- 239000011159 matrix material Substances 0.000 claims description 27
- 238000010276 construction Methods 0.000 claims description 18
- 238000012417 linear regression Methods 0.000 claims description 7
- 238000005070 sampling Methods 0.000 claims description 6
- 230000008569 process Effects 0.000 claims description 5
- 238000006243 chemical reaction Methods 0.000 claims description 3
- 238000000354 decomposition reaction Methods 0.000 claims description 3
- 230000007704 transition Effects 0.000 claims description 3
- 238000010586 diagram Methods 0.000 description 4
- 238000012986 modification Methods 0.000 description 2
- 230000004048 modification Effects 0.000 description 2
- XLYOFNOQVPJJNP-UHFFFAOYSA-N water Substances O XLYOFNOQVPJJNP-UHFFFAOYSA-N 0.000 description 2
- 238000010521 absorption reaction Methods 0.000 description 1
- 230000002411 adverse Effects 0.000 description 1
- 230000003190 augmentative effect Effects 0.000 description 1
- 230000004888 barrier function Effects 0.000 description 1
- 230000009286 beneficial effect Effects 0.000 description 1
- 230000005540 biological transmission Effects 0.000 description 1
- 230000015572 biosynthetic process Effects 0.000 description 1
- 238000004891 communication Methods 0.000 description 1
- 230000007547 defect Effects 0.000 description 1
- 230000001934 delay Effects 0.000 description 1
- 230000007613 environmental effect Effects 0.000 description 1
- 238000001914 filtration Methods 0.000 description 1
- 230000010365 information processing Effects 0.000 description 1
Classifications
-
- 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
-
- 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
- G06F17/00—Digital computing or data processing equipment or methods, specially adapted for specific functions
- G06F17/10—Complex mathematical operations
- G06F17/16—Matrix or vector computation, e.g. matrix-matrix or matrix-vector multiplication, matrix factorization
-
- 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/18—Complex mathematical operations for evaluating statistical data, e.g. average values, frequency distributions, probability functions, regression analysis
-
- G—PHYSICS
- G06—COMPUTING; CALCULATING OR COUNTING
- G06F—ELECTRIC DIGITAL DATA PROCESSING
- G06F2119/00—Details relating to the type or aim of the analysis or the optimisation
- G06F2119/10—Noise analysis or noise optimisation
-
- G—PHYSICS
- G06—COMPUTING; CALCULATING OR COUNTING
- G06F—ELECTRIC DIGITAL DATA PROCESSING
- G06F2119/00—Details relating to the type or aim of the analysis or the optimisation
- G06F2119/12—Timing analysis or timing optimisation
-
- Y—GENERAL TAGGING OF NEW TECHNOLOGICAL DEVELOPMENTS; GENERAL TAGGING OF CROSS-SECTIONAL TECHNOLOGIES SPANNING OVER SEVERAL SECTIONS OF THE IPC; TECHNICAL SUBJECTS COVERED BY FORMER USPC CROSS-REFERENCE ART COLLECTIONS [XRACs] AND DIGESTS
- Y02—TECHNOLOGIES OR APPLICATIONS FOR MITIGATION OR ADAPTATION AGAINST CLIMATE CHANGE
- Y02T—CLIMATE CHANGE MITIGATION TECHNOLOGIES RELATED TO TRANSPORTATION
- Y02T10/00—Road transport of goods or passengers
- Y02T10/10—Internal combustion engine [ICE] based vehicles
- Y02T10/40—Engine management systems
Landscapes
- Engineering & Computer Science (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Mathematical Physics (AREA)
- Theoretical Computer Science (AREA)
- Data Mining & Analysis (AREA)
- Computational Mathematics (AREA)
- Pure & Applied Mathematics (AREA)
- Mathematical Optimization (AREA)
- Mathematical Analysis (AREA)
- General Engineering & Computer Science (AREA)
- Databases & Information Systems (AREA)
- Software Systems (AREA)
- Algebra (AREA)
- Operations Research (AREA)
- Life Sciences & Earth Sciences (AREA)
- Bioinformatics & Cheminformatics (AREA)
- Probability & Statistics with Applications (AREA)
- Evolutionary Computation (AREA)
- Bioinformatics & Computational Biology (AREA)
- Computer Hardware Design (AREA)
- Geometry (AREA)
- Evolutionary Biology (AREA)
- Computing Systems (AREA)
- Measurement Of Velocity Or Position Using Acoustic Or Ultrasonic Waves (AREA)
- Navigation (AREA)
Abstract
本申请公开了一种水下航行器未知输入与位置估计方法及系统,方法包括:获取水下航行器的航行数据,并基于航行数据构建系统状态空间模型;对系统状态空间模型中的量测方程进行第一次优化,得到优化后量测方程,并基于优化后量测方程构建延时递推框架;基于延时递推框架,搭建未知非线性控制输入和延时到达观测信息之间的递推关联;基于递推关联得到未知非线性控制输入估计值;基于未知非线性控制输入估计值,得到位置状态后验估计表达式和第一估计误差协方差表达式;对后验估计表达式进行第二次优化,得到最终位置状态表达式和第二估计误差协方差表达式。本申请缓解观测值中的延时偏差对未知输入估计及位置估计的影响,提高水下航行器的定位精度。
Description
技术领域
本申请属于水下航行器协同定位技术领域,具体涉及一种水下航行器未知输入与位置估计方法及系统。
背景技术
受全球定位系统信号在水下的快速衰减以及水下恶劣的声学信道条件的影响,自主水下航行器自身位置估计一直是一个艰巨的任务。与传统的惯性系统和声基线系统相比,协同定位系统具有精度高、航程远、成本低、鲁棒性好的优点,可以大大增强多水下航行器系统的协同作业能力。然而,信息处理与传输所消耗的时间导致协同定位所需观测信息存在时变时间延时,此外,协同定位系统传感器如罗经、多普勒计程仪等受自身结构、工作原理及水下环境因素如海洋生物障碍、海底强吸声地质、深海沟等影响可能出现故障或失效。以上恶劣条件的出现均可能导致定位精度降低甚至估计结果无效。
罗经和多普勒计程仪故障都会导致无法获得有效的控制输入,从而导致先验信息的不准确,这必然会影响定位精度。目前,现有算法可以在实时外部观测的辅助下完成对控制输入的估计,如经典的增广卡尔曼滤波器、经过状态解耦优化的滤波器等。然而,这些方法依赖于相对准确的观测信息,而水下通信延时的存在导致观测数据存在延时偏差,此偏差的存在使得非线性控制输入估计的准确性无法得到保证,进而对位置估计产生不利影响。
发明内容
本申请旨在解决现有技术的不足,提出一种水下航行器未知输入与位置估计方法及系统,能够有效缓解观测值中的延时偏差对未知输入估计及位置估计的影响,提高水下航行器在非理想作业环境中的定位精度。
为实现上述目的,本申请提供了如下方案:
一种水下航行器未知输入与位置估计方法,包括以下步骤:
获取水下航行器的航行数据,并基于所述航行数据构建系统状态空间模型;
对所述系统状态空间模型中的量测方程进行第一次优化,得到优化后量测方程,并基于所述优化后量测方程构建延时递推框架;
基于所述延时递推框架,搭建未知非线性控制输入和延时到达观测信息之间的递推关联;
基于所述递推关联得到未知非线性控制输入估计值;
基于所述未知非线性控制输入估计值,得到位置状态后验估计表达式和第一估计误差协方差表达式;
对所述后验估计表达式进行第二次优化,得到最终位置状态表达式和第二估计误差协方差表达式。
优选的,所述航行数据包括:位置数据、速度数据和航向数据。
优选的,所述系统状态空间模型的构建方法包括:
基于所述位置数据、所述速度数据和所述航向数据构建离散时间模型:
其中,和/>分别为前向和右向速度,/>为航向,χm,[k]=[am,[k],bm,[k]]为领航者在k时刻的位置坐标,χ[k]=[a[k],b[k]]T为跟随者在k时刻的位置坐标,γ[k]为领航者与跟随者之间的相对距离,(·)T为转置操作,Δt为采样周期,q[k-l]=[qx,[k-l],qy,[k-l]]T为过程噪声,r[k]为量测噪声;
基于所述离散时间模型构建所述系统状态空间模型:
其中,状态转移矩阵Φ[k-1]为二维单位矩阵,Γ[k-1]=diag(Δt,Δt)和u[k-1]=[ux,uy]T分别为控制输入,且有和为量测方程。
优选的,所述优化后量测方程为:
其中,τ为延时采样时刻,C[k-τ]=(Pxy,[k-τ,k-τ-1])T(Pyy,[k-τ,k-τ-1])-1为观测矩阵,γ[k-τ]为k-τ时刻相对距离量测值,为k-τ时刻相对距离预测值,/>为k-τ时刻跟随者位置的一步预测估计值;
η[k-τ]=r[k-τ]+l[k-τ]包含量测噪声与线性化误差两部分,l[k-τ]为k-τ时刻的统计线性化误差,线性化误差的协方差表示为:
Θ[k-τ]=E(l[k-τ]l[k-τ] T)
=Pyy,[k-τ,k-τ-1]-(Pxy,[k-τ,k-τ-1])T(Pxx,[k-τ,k-τ-1])-1Pxy,[k-τ,k-τ-1]-R[k-τ]
其中,Pyy,[k-τ,k-τ-1]和Pxy,[k-τ,k-τ-1]分别为自协方差与互协方差矩阵,R[k-τ]=E(r[k-τ]r[k-τ] T)为k-τ时刻量测噪声协方差矩阵。
优选的,所述延时递推框架为:
其中,
为延时转化为的偏差:
Υ[k]中的延时带来的附加项
优选的,所述递推关联包括:
其中,
进一步得到:
其中,Pxx,[k,k-1]为状态的一步预测误差协方差,
κ[k]=E(η[k-τ]η[k-τ] T)=R[k-τ]+Θ[k-τ],
L[k]为T[k]经过Cholesky分解后的下三角矩阵。
优选的,所述未知非线性控制输入估计值的计算方法包括:
基于所述递推关联构建递推关联的线性回归表达式:
基于所述线性回归表达式计算所述未知非线性控制输入估计值:
其中,
优选的,所述后验估计表达式为:
其中,K[k]为增益矩阵;
所述第一估计误差协方差表达式为:
优选的,所述第二次优化的方法包括:舍去所述后验估计表达式中与未知输入相关项,得到所述最终位置状态表达式和所述第二估计误差协方差表达式;
所述最终位置状态表达式为:
所述第二估计误差协方差表达式为:
其中,
本申请还提供了一种水下航行器未知输入与位置估计系统,包括:模型构建模块、框架构建模块、关联构建模块、估计模块、后验状态模块和位置估计模块;
所述模型构建模块用于获取水下航行器的航行数据,并基于所述航行数据构建系统状态空间模型;
所述框架构建模块用于对所述系统状态空间模型中的量测方程进行第一次优化,得到优化后量测方程,并基于所述优化后量测方程构建延时递推框架;
所述关联构建模块用于基于所述延时递推框架,搭建未知非线性控制输入和延时到达观测信息之间的递推关联;
所述估计模块用于基于所述递推关联得到未知非线性控制输入估计值;
所述后验状态模块用于基于所述未知非线性控制输入估计值,得到位置状态后验估计表达式和第一估计误差协方差表达式;
所述位置估计模块用于对所述后验估计表达式进行第二次优化,得到最终位置状态表达式和第二估计误差协方差表达式。
与现有技术相比,本申请的有益效果为:
(1)本申请将航向角、前向速度和右向速度建模为非线性单元,降低了估计模型的复杂性;
(2)本申请通过重构观测矩阵和测量噪声方差矩阵对测量方程进行修正,建立了基于线性回归的延迟框架,从模型角度削弱了非实时观测对未知输入估计的影响;
(3)本申请构建了延迟到达量测与非线性控制输入估计之间的递推关联,并进一步推导了不受非线性输入干扰的后验估计表达形式,有效削弱未知输入对位置估计的影响,提高了系统的鲁棒性。
附图说明
为了更清楚地说明本申请的技术方案,下面对实施例中所需要使用的附图作简单地介绍,显而易见地,下面描述中的附图仅是本申请的一些实施例,对于本领域普通技术人员来讲,在不付出创造性劳动性的前提下,还可以根据这些附图获得其他的附图。
图1为本申请实施例的方法流程示意图;
图2为本申请实施例的系统结构示意图;
图3为本申请实施例的协同定位系统多潜航器实际航行轨迹图;
图4为本申请实施例的定位误差比较图;
图5为本申请实施例的未知输入估计误差比较图。
具体实施方式
下面将结合本申请实施例中的附图,对本申请实施例中的技术方案进行清楚、完整地描述,显然,所描述的实施例仅仅是本申请一部分实施例,而不是全部的实施例。基于本申请中的实施例,本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例,都属于本申请保护的范围。
为使本申请的上述目的、特征和优点能够更加明显易懂,下面结合附图和具体实施方式对本申请作进一步详细的说明。
实施例一
在本实施例中,如图1所示,一种水下航行器未知输入与位置估计方法,其包括以下步骤:
S1.获取水下航行器的航行数据,并基于航行数据构建系统状态空间模型。
其中,航行数据包括:位置数据、速度数据和航向数据。
系统状态空间模型的构建方法包括:
基于位置数据、速度数据和航向数据构建离散时间模型:
其中,为速度向量,/>和/>分别为前向和右向速度,/>为航向,χm,[k]=[am,[k],bm,[k]]为领航者在k时刻的位置坐标,χ[k]=[a[k],b[k]]T为跟随者在k时刻的位置坐标,γ[k]为领航者与跟随者之间的相对距离,(·)T为转置操作,Δt为采样周期,q[k-l]=[qx,[k-l],qy,[k-l]]T为过程噪声,r[k]为量测噪声;
基于离散时间模型构建系统状态空间模型:
其中,状态转移矩阵Φ[k-1]为二维单位矩阵,Γ[k-1]=diag(Δt,Δt)和u[k-1]=[ux,uy]T分别为控制输入,且有和为量测方程,且E(q[k]r[l] T))=0。
S2.对系统状态空间模型中的量测方程进行第一次优化,得到优化后量测方程,并基于优化后量测方程构建延时递推框架。
优化后量测方程为:
其中,τ为延时采样时刻,C[k-τ]=(Pxy,[k-τ,k-τ-1])T(Pyy,[k-τ,k-τ-1])-1为观测矩阵,Υ[k-τ]为k-τ时刻相对距离量测值,为k-τ时刻相对距离预测值,/>为k-τ时刻跟随者位置的一步预测估计值;
η[k-τ]=r[k-τ]+l[k-τ]包含量测噪声与线性化误差两部分,l[k-τ]为k-τ时刻的统计线性化误差,线性化误差的协方差表示为:
Θ[k-τ]=E(l[k-τ]l[k-τ] T)
=Pyy,[k-τ,k-τ-1]-(Pxy,[k-τ,k-τ-1])T(Pxx,[k-τ,k-τ-1])-1Pxy,[k-τ,k-τ-1]-R[k-τ]
其中,Pyy,[k-τ,k-τ-1]和Pxy,[k-τ,k-τ-1]分别为自协方差与互协方差矩阵,其表达形式与容积卡尔曼滤波中相同,R[k-τ]=E(r[k-τ]r[k-τ] T)为k-τ时刻量测噪声协方差矩阵。
延时递推框架为:
其中,为修正后的量测噪声协方差矩阵,其中包含了/>
为延时转化为的偏差:
γ[k]中的延时带来的附加项
S3.基于延时递推框架,搭建未知非线性控制输入和延时到达观测信息之间的递推关联。
递推关联包括:
其中,
进一步得到:
其中,Pxx,[k,k-1]为状态的一步预测误差协方差,
κ[k]=E(η[k-τ]η[k-τ] T)=R[k-τ]+Θ[k-τ],
L[k]为T[k]经过Cholesky分解后的下三角矩阵。
S4.基于递推关联得到未知非线性控制输入估计值。
未知非线性控制输入估计值的计算方法包括:
基于递推关联构建递推关联的线性回归表达式:
基于线性回归表达式计算未知非线性控制输入的加权最小二乘估计值,即未知非线性控制输入估计值:
其中,
S5.基于未知非线性控制输入估计值,得到位置状态后验估计表达式和第一估计误差协方差表达式。
后验估计表达式为:
其中,K[k]为增益矩阵;
第一估计误差协方差表达式为:
S6.对后验估计表达式进行第二次优化,得到最终位置状态表达式和第二估计误差协方差表达式。
第二次优化的方法包括:舍去后验估计表达式中与未知输入相关项,得到最终位置状态表达式和第二估计误差协方差表达式;
最终位置状态表达式为:
第二估计误差协方差表达式为:
其中,
实施例二
在本实施例中,如图2所示,一种水下航行器未知输入与位置估计系统,包括:模型构建模块、框架构建模块、关联构建模块、估计模块、后验状态模块和位置估计模块;
模型构建模块用于获取水下航行器的航行数据,并基于航行数据构建系统状态空间模型;
框架构建模块用于对系统状态空间模型中的量测方程进行第一次优化,得到优化后量测方程,并基于优化后量测方程构建延时递推框架;
关联构建模块用于基于延时递推框架,搭建未知非线性控制输入和延时到达观测信息之间的递推关联;
估计模块用于基于递推关联得到未知非线性控制输入估计值;
后验状态模块用于基于未知非线性控制输入估计值,得到位置状态后验估计表达式和第一估计误差协方差表达式;
位置估计模块用于对后验估计表达式进行第二次优化,得到最终位置状态表达式和第二估计误差协方差表达式。
实施例三
在本实施例中,如图3所示,为协同定位系统多潜航器实际航行轨迹图,此处考虑2个领航者1个跟随者的经典场景,在协同定位过程中,通过水声设备每隔一段时间测量领航者与跟随者之间的相对距离。两个领航者分别位于随从者两侧,这种队形可以提高系统的可观测性。图4和图5分别为协同定位误差比较图和未知输入估计误差比较图,可以看出,当量测信息存在延时偏差时,RCKF、RCKF、MBCDCKF定位精度不佳,这是由于三者在估计过程中需要相对准确的观测值,而DUICKF在搭建起非实时观测值与未知输入之间的递推关联后,有效的削弱了未知输入估计对观测信息实时性的依赖程度,提高了算法在观测值存在延时偏差下的未知输入估计精度,使得位置估计精度得以提高。
以上所述的实施例仅是对本申请优选方式进行的描述,并非对本申请的范围进行限定,在不脱离本申请设计精神的前提下,本领域普通技术人员对本申请的技术方案做出的各种变形和改进,均应落入本申请权利要求书确定的保护范围内。
Claims (10)
1.一种水下航行器未知输入与位置估计方法,其特征在于,包括以下步骤:
获取水下航行器的航行数据,并基于所述航行数据构建系统状态空间模型;
对所述系统状态空间模型中的量测方程进行第一次优化,得到优化后量测方程,并基于所述优化后量测方程构建延时递推框架;
基于所述延时递推框架,搭建未知非线性控制输入和延时到达观测信息之间的递推关联;
基于所述递推关联得到未知非线性控制输入估计值;
基于所述未知非线性控制输入估计值,得到位置状态后验估计表达式和第一估计误差协方差表达式;
对所述后验估计表达式进行第二次优化,得到最终位置状态表达式和第二估计误差协方差表达式。
2.根据权利要求1所述一种水下航行器未知输入与位置估计方法,其特征在于,所述航行数据包括:位置数据、速度数据和航向数据。
3.根据权利要求2所述一种水下航行器未知输入与位置估计方法,其特征在于,所述系统状态空间模型的构建方法包括:
基于所述位置数据、所述速度数据和所述航向数据构建离散时间模型:
其中,和/>分别为前向和右向速度,/>为航向,χm,[k]=[am,[k],bm,[k]]为领航者在k时刻的位置坐标,χ[k]=[a[k],b[k]]T为跟随者在k时刻的位置坐标,γ[k]为领航者与跟随者之间的相对距离,(·)T为转置操作,Δt为采样周期,q[k-1]=[qx,[k-1],qy,[k-1]]T为过程噪声,r[k]为量测噪声;
基于所述离散时间模型构建所述系统状态空间模型:
其中,状态转移矩阵Φ[k-1]为二维单位矩阵,Γ[k-1]=diag(Δt,Δt)和u[k-1]=[ux,uy]T分别为控制输入,且有和为量测方程。
4.根据权利要求3所述一种水下航行器未知输入与位置估计方法,其特征在于,所述优化后量测方程为:
其中,τ为延时采样时刻,C[k-τ]=(Pxy,[k-τ,k-τ-1])T(Pyy,[k-τ,k-τ-1])-1为观测矩阵,Υ[k-τ]为k-τ时刻相对距离量测值,为k-τ时刻相对距离预测值,/>为k-τ时刻跟随者位置的一步预测估计值;
η[k-τ]=r[k-τ]+l[k-τ]包含量测噪声与线性化误差两部分,l[k-τ]为k-τ时刻的统计线性化误差,线性化误差的协方差表示为:
Θ[k-τ]=E(l[k-τ]l[k-τ] T)
=Pyy,[k-τ,k-τ-1]-(Pxy,[k-τ,k-τ-1])T(Pxx,[k-τ,k-τ-1])-1Pxy,[k-τ,k-τ-1]-R[k-τ]
其中,Pyy,[k-τ,k-τ-1]和Pxy,[k-τ,k-τ-1]分别为自协方差与互协方差矩阵,R[k-τ]=E(r[k-τ]r[k-τ] T)为k-τ时刻量测噪声协方差矩阵。
5.根据权利要求4所述一种水下航行器未知输入与位置估计方法,其特征在于,所述延时递推框架为:
其中,
为延时转化为的偏差:
Υ[k]中的延时带来的附加项
6.根据权利要求5所述一种水下航行器未知输入与位置估计方法,其特征在于,所述递推关联包括:
其中,
进一步得到:
其中,Pxx,[k,k-1]为状态的一步预测误差协方差,
κ[k]=E(η[k-τ]η[k-τ] T)=R[k-τ]+Θ[k-τ],
L[k]为T[k]经过Cholesky分解后的下三角矩阵。
7.根据权利要求6所述一种水下航行器未知输入与位置估计方法,其特征在于,所述未知非线性控制输入估计值的计算方法包括:
基于所述递推关联构建递推关联的线性回归表达式:
基于所述线性回归表达式计算所述未知非线性控制输入估计值:
其中,
8.根据权利要求7所述一种水下航行器未知输入与位置估计方法,其特征在于,所述后验估计表达式为:
其中,K[k]为增益矩阵;
所述第一估计误差协方差表达式为:
9.根据权利要求8所述一种水下航行器未知输入与位置估计方法,其特征在于,所述第二次优化的方法包括:舍去所述后验估计表达式中与未知输入相关项,得到所述最终位置状态表达式和所述第二估计误差协方差表达式;
所述最终位置状态表达式为:
所述第二估计误差协方差表达式为:
其中,
10.一种水下航行器未知输入与位置估计系统,其特征在于,包括:模型构建模块、框架构建模块、关联构建模块、估计模块、后验状态模块和位置估计模块;
所述模型构建模块用于获取水下航行器的航行数据,并基于所述航行数据构建系统状态空间模型;
所述框架构建模块用于对所述系统状态空间模型中的量测方程进行第一次优化,得到优化后量测方程,并基于所述优化后量测方程构建延时递推框架;
所述关联构建模块用于基于所述延时递推框架,搭建未知非线性控制输入和延时到达观测信息之间的递推关联;
所述估计模块用于基于所述递推关联得到未知非线性控制输入估计值;
所述后验状态模块用于基于所述未知非线性控制输入估计值,得到位置状态后验估计表达式和第一估计误差协方差表达式;
所述位置估计模块用于对所述后验估计表达式进行第二次优化,得到最终位置状态表达式和第二估计误差协方差表达式。
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202310690334.5A CN116702479B (zh) | 2023-06-12 | 2023-06-12 | 一种水下航行器未知输入与位置估计方法及系统 |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202310690334.5A CN116702479B (zh) | 2023-06-12 | 2023-06-12 | 一种水下航行器未知输入与位置估计方法及系统 |
Publications (2)
Publication Number | Publication Date |
---|---|
CN116702479A true CN116702479A (zh) | 2023-09-05 |
CN116702479B CN116702479B (zh) | 2024-02-06 |
Family
ID=87832126
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
CN202310690334.5A Active CN116702479B (zh) | 2023-06-12 | 2023-06-12 | 一种水下航行器未知输入与位置估计方法及系统 |
Country Status (1)
Country | Link |
---|---|
CN (1) | CN116702479B (zh) |
Citations (9)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN107063300A (zh) * | 2016-12-21 | 2017-08-18 | 东南大学 | 一种基于反演的水下导航系统动力学模型中扰动估计方法 |
CN108871365A (zh) * | 2018-07-06 | 2018-11-23 | 哈尔滨工业大学 | 一种航向约束下的状态估计方法及系统 |
CN109782289A (zh) * | 2018-12-26 | 2019-05-21 | 中国电子科技集团公司第二十研究所 | 一种基于基线几何结构约束的水下航行器定位方法 |
CN110779519A (zh) * | 2019-11-18 | 2020-02-11 | 哈尔滨工程大学 | 一种具有全局收敛性的水下航行器单信标定位方法 |
CN110794409A (zh) * | 2019-10-21 | 2020-02-14 | 哈尔滨工程大学 | 一种可估计未知有效声速的水下单信标定位方法 |
CN111595348A (zh) * | 2020-06-23 | 2020-08-28 | 南京信息工程大学 | 一种自主水下航行器组合导航系统的主从式协同定位方法 |
CN113324546A (zh) * | 2021-05-24 | 2021-08-31 | 哈尔滨工程大学 | 罗经失效下的多潜航器协同定位自适应调节鲁棒滤波方法 |
CN113791432A (zh) * | 2021-08-18 | 2021-12-14 | 哈尔滨工程大学 | 一种提高ais定位精度的组合导航定位方法 |
CN114370878A (zh) * | 2022-01-04 | 2022-04-19 | 江苏科技大学 | 一种基于stackf的多auv协同定位方法 |
-
2023
- 2023-06-12 CN CN202310690334.5A patent/CN116702479B/zh active Active
Patent Citations (9)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN107063300A (zh) * | 2016-12-21 | 2017-08-18 | 东南大学 | 一种基于反演的水下导航系统动力学模型中扰动估计方法 |
CN108871365A (zh) * | 2018-07-06 | 2018-11-23 | 哈尔滨工业大学 | 一种航向约束下的状态估计方法及系统 |
CN109782289A (zh) * | 2018-12-26 | 2019-05-21 | 中国电子科技集团公司第二十研究所 | 一种基于基线几何结构约束的水下航行器定位方法 |
CN110794409A (zh) * | 2019-10-21 | 2020-02-14 | 哈尔滨工程大学 | 一种可估计未知有效声速的水下单信标定位方法 |
CN110779519A (zh) * | 2019-11-18 | 2020-02-11 | 哈尔滨工程大学 | 一种具有全局收敛性的水下航行器单信标定位方法 |
CN111595348A (zh) * | 2020-06-23 | 2020-08-28 | 南京信息工程大学 | 一种自主水下航行器组合导航系统的主从式协同定位方法 |
CN113324546A (zh) * | 2021-05-24 | 2021-08-31 | 哈尔滨工程大学 | 罗经失效下的多潜航器协同定位自适应调节鲁棒滤波方法 |
CN113791432A (zh) * | 2021-08-18 | 2021-12-14 | 哈尔滨工程大学 | 一种提高ais定位精度的组合导航定位方法 |
CN114370878A (zh) * | 2022-01-04 | 2022-04-19 | 江苏科技大学 | 一种基于stackf的多auv协同定位方法 |
Non-Patent Citations (2)
Title |
---|
BO XU ET AL: "Maximum correntropy delay Kalman filter for SINS/USBL integrated navigation", 《ISA TRANSACTIONS》 * |
卢健 等: "利用FastSLAM框架的多自治水下航行器同时定位与跟踪算法", 《控制理论与应用》 * |
Also Published As
Publication number | Publication date |
---|---|
CN116702479B (zh) | 2024-02-06 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
CN109459040B (zh) | 基于rbf神经网络辅助容积卡尔曼滤波的多auv协同定位方法 | |
CN109974706B (zh) | 一种基于双运动模型的主从式多auv协同导航方法 | |
CN110823217A (zh) | 一种基于自适应联邦强跟踪滤波的组合导航容错方法 | |
CN110794409B (zh) | 一种可估计未知有效声速的水下单信标定位方法 | |
CN111337020A (zh) | 引入抗差估计的因子图融合定位方法 | |
CN110132308B (zh) | 一种基于姿态确定的usbl安装误差角标定方法 | |
CN105783940A (zh) | 基于信息预评判及补偿修正的sins/dvl/es组合导航方法 | |
CN109579850B (zh) | 基于对水速度辅助惯导的深水智能导航方法 | |
CN103697910A (zh) | 自主水下航行器多普勒计程仪安装误差的校正方法 | |
CN110779519B (zh) | 一种具有全局收敛性的水下航行器单信标定位方法 | |
CN110555398B (zh) | 一种基于滤波最优平滑确定故障首达时刻的故障诊断方法 | |
CN111982105B (zh) | 一种基于sins/lbl紧组合的水下导航定位方法及系统 | |
CN111076728A (zh) | 基于dr/usbl的深潜载人潜水器组合导航方法 | |
CN103605886A (zh) | 一种船舶动力定位系统多模型自适应融合滤波方法 | |
CN115307643A (zh) | 一种双应答器辅助的sins/usbl组合导航方法 | |
CN112710304A (zh) | 一种基于自适应滤波的水下自主航行器导航方法 | |
CN114265047B (zh) | 一种大潜深auv的定位基阵联合标校方法 | |
CN117606491B (zh) | 一种自主式水下航行器的组合定位导航方法及装置 | |
CN116680500B (zh) | 水下航行器在非高斯噪声干扰下的位置估计方法及系统 | |
CN111999747B (zh) | 一种惯导-卫星组合导航系统的鲁棒故障检测方法 | |
CN116702479B (zh) | 一种水下航行器未知输入与位置估计方法及系统 | |
CN110873813B (zh) | 一种水流速度估算方法、组合导航方法及装置 | |
Hou et al. | Improved exponential weighted moving average based measurement noise estimation for strapdown inertial navigation system/Doppler velocity log integrated system | |
CN116358554A (zh) | 具有异常观测处理能力的水下航行器因子图融合导航方法 | |
CN115047505B (zh) | 基于载波相位差分辅助的gnss定位方法及导航方法 |
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 |