[go: up one dir, main page]
More Web Proxy on the site http://driver.im/

WO2023045357A1 - 一种基于统计相似度量的组合导航鲁棒滤波方法 - Google Patents

一种基于统计相似度量的组合导航鲁棒滤波方法 Download PDF

Info

Publication number
WO2023045357A1
WO2023045357A1 PCT/CN2022/093182 CN2022093182W WO2023045357A1 WO 2023045357 A1 WO2023045357 A1 WO 2023045357A1 CN 2022093182 W CN2022093182 W CN 2022093182W WO 2023045357 A1 WO2023045357 A1 WO 2023045357A1
Authority
WO
WIPO (PCT)
Prior art keywords
error
measurement
dvl
sins
vector
Prior art date
Application number
PCT/CN2022/093182
Other languages
English (en)
French (fr)
Inventor
徐博
郭瑜
赵玉新
吴磊
胡俊苗
陈崇
王连钊
李盛新
王潇雨
王朝阳
Original Assignee
哈尔滨工程大学
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 哈尔滨工程大学 filed Critical 哈尔滨工程大学
Priority to US17/843,451 priority Critical patent/US12104908B2/en
Publication of WO2023045357A1 publication Critical patent/WO2023045357A1/zh

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/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/20—Instruments for performing navigational calculations
    • 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
    • Y02T90/00—Enabling technologies or technologies with a potential or indirect contribution to GHG emissions mitigation

Definitions

  • the carrier coordinate system is represented by b
  • the earth-centered earth-fixed coordinate system is represented by e
  • the "east-north-sky" geographic coordinate system is used as the navigation system by n
  • the inertial coordinate system is represented by i
  • the DVL frame coordinate system Expressed with d.
  • ⁇ g n is the projection of the gravity error vector in the n system.
  • ⁇ p represents a position error vector composed of latitude error ⁇ L, longitude error ⁇ , and altitude error ⁇ h.
  • R M and R N represent the radius of curvature of the meridian circle and the prime vertical circle, respectively.
  • the DVL beam measurement noise may be non-Gaussian noise caused by outliers, so the similarity function f z (t) is selected as ⁇ is a degree of freedom parameter, and m is a measurement dimension, which is 1 in the present invention.

Landscapes

  • Engineering & Computer Science (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (AREA)
  • Automation & Control Theory (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Navigation (AREA)

Abstract

一种基于统计相似度量的组合导航鲁棒滤波方法，考虑SINS/DVL紧组合导航系统中正常的DVL波束测量信息和大误差的波束测量信息同时出现的情形，针对目前现有的组合导航系统鲁棒滤波器对量测信息处理粗糙、导致正常量测信息易丢失的问题，基于多维量测方程分解和统计相似度量提出了新的鲁棒滤波方法。在将SINS/DVL紧组合导航系统的多维量测方程进行分解的同时，引入统计相似度量(SSM)理论，协助每个波束的测量噪声方差在大测量误差出现时完成各自的自适应更新，最终保证每个波束测量信息处理的独立性。方法可用于非理想条件下的水下航行器组合导航领域。

Description

一种基于统计相似度量的组合导航鲁棒滤波方法 技术领域
本发明属于非理想条件下的组合导航技术领域，具体涉及一种基于统计相似度量的组合导航鲁棒滤波方法。
背景技术
捷联惯导/多普勒测速计程仪(SINS/DVL)组合导航系统是目前水下航行器最常用的组合导航方法之一。其中紧组合方式可以利用DVL的原始波束测量信息，容错性好，逐渐被广泛使用。DVL采用多普勒频移原理，通过计算向水底发射的声波频率和接受的反射声波频率之差可以测量载体沿声波波束方向的速度。但是在一些复杂情况下，DVL会出现较大的测量误差，例如，DVL发射的声波波束受海洋生物遮挡无法到达海底，或者当水下航行器驶过海沟时，航行器与海床之间的距离超出DVL声波波束范围，亦或者海底存在强吸波材料(如淤泥等)，发出的声波波束不能反射回来。理论上，当DVL的某一个波束出现大的测量误差时，紧组合系统的滤波算法需要能够自动降低对该波束错误信息的利用率。但是目前组合导航系统所使用的鲁棒滤波器多是对所有量测信息的利用率进行统一的调节，对于SINS/DVL紧组合导航系统这种典型的多维量测系统来说，降低对单个大测量误差的波束信息的利用率可能会导致其他正常的波束测量信息在系统中没有被充分使用，从而丢失有用信息。
因此，需要发明在不影响导航系统对正常波束信息利用的情况下降低含有大测量误差的波束信息影响的鲁棒滤波方法。
发明内容
本发明的目的在于提供一种基于统计相似度量的组合导航鲁棒滤波方法。
本发明的目的通过如下技术方案来实现：包括以下步骤：
步骤1：定义坐标系；
定义载体坐标系采用b表示，地心地固坐标系用e表示，“东-北-天”地理坐标系作为导航系采用n表示，惯性坐标系用i表示，DVL框架坐标系用d表示；
定义不同坐标系下的速度：
Figure PCTCN2022093182-appb-000001
Figure PCTCN2022093182-appb-000002
Figure PCTCN2022093182-appb-000003
Figure PCTCN2022093182-appb-000004
其中，
Figure PCTCN2022093182-appb-000005
表示捷联惯导系统在b系中的速度矢量；
Figure PCTCN2022093182-appb-000006
表示n系内捷联惯导系统的速度矢量；
Figure PCTCN2022093182-appb-000007
表示捷联惯导系统在d系中的速度矢量；
Figure PCTCN2022093182-appb-000008
表示d系中DVL波束的速度矢量；
步骤2：建立SINS/DVL紧组合导航系统状态方程；
选取SINS的误差作为状态，包括失准角φ、速度误差δV n、位置误差δp、陀螺常值漂移ε b和加速度计常值零偏
Figure PCTCN2022093182-appb-000009
状态方程如下：
Figure PCTCN2022093182-appb-000010
Figure PCTCN2022093182-appb-000011
Figure PCTCN2022093182-appb-000012
Figure PCTCN2022093182-appb-000013
Figure PCTCN2022093182-appb-000014
Figure PCTCN2022093182-appb-000015
Figure PCTCN2022093182-appb-000016
Figure PCTCN2022093182-appb-000017
Figure PCTCN2022093182-appb-000018
Figure PCTCN2022093182-appb-000019
其中，ε b表示陀螺常值漂移矢量；
Figure PCTCN2022093182-appb-000020
是陀螺随机漂移矢量；
Figure PCTCN2022093182-appb-000021
是加速度计常值零偏矢量；
Figure PCTCN2022093182-appb-000022
是加速度计随机零偏矢量；φ表示由俯仰、横滚和偏航失准角组成的失准角矢量；
Figure PCTCN2022093182-appb-000023
是方向余弦矩阵，表示从b系到n系的变换；
Figure PCTCN2022093182-appb-000024
表示b系下b系相对于i系的角速率矢量，对应的计算误差为
Figure PCTCN2022093182-appb-000025
v n＝[v E v N v h] T是n系中的载体相对于地面的速度矢量，对应 的误差矢量是δv n；f b是加速度计的输出；δg n是重力误差向量在n系中的投影；δp表示由纬度误差δL、经度误差δλ和高度误差δh组成的位置误差矢量；R M和R N分别表示子午线圆和本初垂直圆的曲率半径；
步骤3：建立SINS/DVL紧组合导航系统量测方程，并分解；
量测方程：
Figure PCTCN2022093182-appb-000026
Z＝HX+v
Figure PCTCN2022093182-appb-000027
其中，
Figure PCTCN2022093182-appb-000028
表示测量噪声；I 3×3是三维单位矩阵；[·×]表示向量的叉乘运算；
Figure PCTCN2022093182-appb-000029
α是波束和载体之间的水平角；
Figure PCTCN2022093182-appb-000030
设置为0°或45°；
DVL各个波束的测量噪声通常情况下是不相关的，其量测噪声协方差阵R＝E[vv T]是一个对角阵，因此将量测方程进行等效分解：
Figure PCTCN2022093182-appb-000031
其中，H j表示量测矩阵的第j行；v j是相应的测量噪声，不同的DVL波束的测量噪声不相关，这里是将DVL的四个波束测量值假设为由不同的传感器分别进行测量获得；
步骤4：基于统计相似度量构建状态估计的代价函数；
Figure PCTCN2022093182-appb-000032
Figure PCTCN2022093182-appb-000033
Figure PCTCN2022093182-appb-000034
其中，
Figure PCTCN2022093182-appb-000035
和
Figure PCTCN2022093182-appb-000036
分别表示状态一步预测结果和一步预测协方差阵；N(X；X k+1,P k+1)表示关于X的高斯分布，均值是X k+1，方差是P k+1；相似度函数f x(t)选择为f x(t)＝-0.5t；相似度函数f z(t)选为
Figure PCTCN2022093182-appb-000037
ω是自由度参数，m是量测维数，m＝1；q＝4；
步骤5：应用高斯-牛顿迭代求取代价函数的最优值；
步骤5.1：初始化迭代次数t＝0，设定迭代初值为
Figure PCTCN2022093182-appb-000038
步骤5.2：计算滤波增益
Figure PCTCN2022093182-appb-000039
Figure PCTCN2022093182-appb-000040
其中，
Figure PCTCN2022093182-appb-000041
和
Figure PCTCN2022093182-appb-000042
相当于用来调节一步预测协方差阵和量测噪声方差的辅助参数；
步骤5.3：计算后验状态
Figure PCTCN2022093182-appb-000043
Figure PCTCN2022093182-appb-000044
步骤5.4：计算估计误差协方差阵
Figure PCTCN2022093182-appb-000045
Figure PCTCN2022093182-appb-000046
步骤5.5：判断是否满足
Figure PCTCN2022093182-appb-000047
若不满足，则令t＝t+1，返回步骤5.2；否则，令
Figure PCTCN2022093182-appb-000048
输出SINS的误差X；
步骤6：将估计的SINS的误差X反馈给SINS，进行修正；
根据估计的SINS的误差X，获取失准角φ、速度误差δV n、位置误差δp；对于SINS解算出的姿态阵
Figure PCTCN2022093182-appb-000049
速度
Figure PCTCN2022093182-appb-000050
位置
Figure PCTCN2022093182-appb-000051
姿态误差的反馈修正方法为：
Figure PCTCN2022093182-appb-000052
速度误差δV n和位置误差δp直接在SINS的输出中直接减去即可：
Figure PCTCN2022093182-appb-000053
本发明的有益效果在于：
本发明考虑SINS/DVL紧组合系统中正常的DVL波束测量信息和大误差的波束测量信息同时出现的情形，针对目前现有的组合导航系统鲁棒滤波器对量测信息处理粗糙，导致正常量测信息易丢失的问题，基于多维量测方程分解和统计相似度量提出了新的鲁棒滤波方法。本发明在将SINS/DVL紧组合导航系统的多维量测方程进行分解的同时，引入统计相似度量(SSM)理论，协助每个波束的测量噪声方差在大测量误差出现时完成各自的自适应更新，最终保证每个波束测量信息处理的独立性。本发明可用于非理想条件下的水下航行器组合导航领域。
附图说明
图1为SINS/DVL紧组合导航系统结构框图。
图2为不同坐标系下速度关系图。
图3为湖试实验轨迹图。
图4为速度误差和定位误差比较图。
图5为DVL波束测量值与辅助参数变化对比图。
图6为本发明的实施流程图。
具体实施方式
下面结合附图对本发明做进一步描述。
本发明的目的在于，针对SINS/DVL紧组合导航系统，提供一种可以对每个DVL波束的测量噪声方差进行单独调节的鲁棒滤波方法，即当某一量测出现较大误差时可以自动增大滤波器中所对应的测量噪声方差阵，而其他正常量测的测量噪声方差阵几乎不进行调节。
步骤一：建立SINS/DVL紧组合导航系统模型；
首先定义常用坐标系，载体坐标系采用b表示，地心地固坐标系用e表示，“东-北-天”地理坐标系作为导航系采用n表示，惯性坐标系用i表示，DVL框架坐标系用d表示。
图1描述了SINS/DVL紧组合导航系统的结构。SINS作为一种主要的导航设备，提供速度、姿态和位置信息。作为辅助导航设备，DVL提供四维测量矢量，即DVL在四个波束方向上相对于海底的速度。将SINS解转化为d坐标系下的速度与DVL波束速度的差值作为导 航滤波器的观测量。然后对状态进行估计并反馈给SINS，以减小SINS的导航误差。
在本发明中，假设SINS与DVL之间的安装角误差和DVL的标度因数在设备安装完成后已经进行了标定和补偿。残余的安装角误差和标度因数误差可以忽略不计。因此，本发明只选取SINS的误差作为状态，包括失准角φ、速度误差δV n、位置误差δp、陀螺常值漂移ε b和加速度计常值零偏
Figure PCTCN2022093182-appb-000054
状态方程如下：
Figure PCTCN2022093182-appb-000055
Figure PCTCN2022093182-appb-000056
Figure PCTCN2022093182-appb-000057
Figure PCTCN2022093182-appb-000058
Figure PCTCN2022093182-appb-000059
Figure PCTCN2022093182-appb-000060
Figure PCTCN2022093182-appb-000061
Figure PCTCN2022093182-appb-000062
Figure PCTCN2022093182-appb-000063
Figure PCTCN2022093182-appb-000064
式中ε b表示陀螺常值漂移矢量，
Figure PCTCN2022093182-appb-000065
是陀螺随机漂移矢量。是加速度计常值零偏矢量
Figure PCTCN2022093182-appb-000066
Figure PCTCN2022093182-appb-000067
是加速度计随机零偏矢量。φ表示由俯仰、横滚和偏航失准角组成的失准角矢量。
Figure PCTCN2022093182-appb-000068
是方向余弦矩阵，表示从b系到n系的变换。
Figure PCTCN2022093182-appb-000069
表示b系下b系相对于i系的角速率矢量，对应的计算误差为
Figure PCTCN2022093182-appb-000070
v n＝[v E v N v h] T是n系中的载体相对于地面的速度矢量，对应 的误差矢量是δv n。f b是加速度计的输出。δg n是重力误差向量在n系中的投影。δp表示由纬度误差δL、经度误差δλ和高度误差δh组成的位置误差矢量。R M和R N分别表示子午线圆和本初垂直圆的曲率半径。
本发明在引入测量方程之前，定义了不同坐标系下的速度，以四波束配置DVL为例，如图2所示。
Figure PCTCN2022093182-appb-000071
是捷联惯导系统在b系中的速度矢量。
Figure PCTCN2022093182-appb-000072
表示n系内捷联惯导系统的速度矢量。
Figure PCTCN2022093182-appb-000073
表示捷联惯导系统在d系中的速度矢量。
Figure PCTCN2022093182-appb-000074
表示d系中DVL波束的速度矢量。
Figure PCTCN2022093182-appb-000075
Figure PCTCN2022093182-appb-000076
Figure PCTCN2022093182-appb-000077
Figure PCTCN2022093182-appb-000078
捷联惯导系统在导航系下的速度与d系下的DVL波束速度之间的关系如下
Figure PCTCN2022093182-appb-000079
Figure PCTCN2022093182-appb-000080
Figure PCTCN2022093182-appb-000081
Figure PCTCN2022093182-appb-000082
其中α是波束和载体之间的水平角。通常，
Figure PCTCN2022093182-appb-000083
设置为0°或45°。
DVL波束测量值建模如下：
Figure PCTCN2022093182-appb-000084
其中
Figure PCTCN2022093182-appb-000085
表示DVL的测量噪声。将捷联惯导系统的速度输出转换到d系的结果为
Figure PCTCN2022093182-appb-000086
式中，I 3×3是三维单位矩阵。[·×]表示向量的叉乘运算。
联立(19)和(20)可以得到量测方程
Figure PCTCN2022093182-appb-000087
Z＝HX+v   (22)
Figure PCTCN2022093182-appb-000088
Figure PCTCN2022093182-appb-000089
表示测量噪声。
步骤二：分解SINS/DVL紧组合导航系统的多维量测方程；
DVL各个波束的测量噪声通常情况下是不相关的，其量测噪声协方差阵R＝E[vv T]是一个对角阵。因此在本发明中可以将量测方程(22)进行等效分解。
Figure PCTCN2022093182-appb-000090
其中H j表示量测矩阵的第j行。v j是相应的测量噪声，不同的DVL波束的测量噪声不相关。这里是将DVL的四个波束测量值假设为由不同的传感器分别进行测量获得。
步骤三：引入统计相似度量概念，并基于统计相似度量构建状态估计的代价函数；
步骤3.1：引入统计相似度量(SSM)
统计相似度度量s(a,b)可以用来表示两个随机向量a和b之间的相似度，定义如下：
s(a,b)＝E[f(||a-b|| 2)]＝∫∫f(||a-b|| 2)p(a,b)dadb    (25)
式中，||·||表示欧氏范数，p(a,b)表示随机向量a和b之间的联合概率密度。f(·)表示相似度函数，并且满足以下三个条件。
a)f(·)是在定义域[0，+∞)内的连续函数。
b)f(·)是单调递减函数，即
Figure PCTCN2022093182-appb-000091
c)f(·)的二阶导数是非负的，即
Figure PCTCN2022093182-appb-000092
统计相似度度量符合相似度度量的通常定义。随机向量之间的相似性越高，统计相似度度量就越大。选择f(t)为f(t)＝-t时，SSM表示不同随机向量之间的负均方误差。当选择f(t) 为
Figure PCTCN2022093182-appb-000093
时，SSM表示不同随机向量之间的相关熵。通过选择不同的相似函数，可以实现不同的SSM。
步骤3.2：建立鲁棒代价函数
首先假设系统模型为
X k+1＝f k(X k)+n k   (26)
Figure PCTCN2022093182-appb-000094
式中，k表示时刻，X k+1表示一个p维状态。
Figure PCTCN2022093182-appb-000095
表示第j个量测，f k(·)和
Figure PCTCN2022093182-appb-000096
表示状态转移函数和测量函数。n k和
Figure PCTCN2022093182-appb-000097
表示过程噪声和量测噪声。Q k和
Figure PCTCN2022093182-appb-000098
表示过程噪声标称协方差阵和第j个量测噪声标称协方差阵，对于本发明所介绍的SINS/DVL紧组合导航系统来说，q为4。
标准卡尔曼滤波(KF)本质上是状态预测和量测信息的加权，以获得状态的最优估计，它的加权最小二乘代价函数反映了状态和预测状态之间以及量测和预测量测之间的均方误差。而本发明则构造了反映状态与预测状态、量测与预测量测之间SSM的代价函数。
Figure PCTCN2022093182-appb-000099
式中，
Figure PCTCN2022093182-appb-000100
和
Figure PCTCN2022093182-appb-000101
表示状态一步预测结果和一步预测协方差阵，
Figure PCTCN2022093182-appb-000102
和
Figure PCTCN2022093182-appb-000103
表示量测噪声方差
Figure PCTCN2022093182-appb-000104
和一步预测协方差阵
Figure PCTCN2022093182-appb-000105
的均方根分解。
为了便于求解(28)所示的代价函数，将后验分布近似为高斯分布并利用詹森(Jensen)不等式求解上式中代价函数的下界，将其作为一个新的代价函数。
Figure PCTCN2022093182-appb-000106
Figure PCTCN2022093182-appb-000107
Figure PCTCN2022093182-appb-000108
式中，N(X；X k+1,P k+1)表示关于X的高斯分布，均值是X k+1，方差是P k+1。
当测量方程
Figure PCTCN2022093182-appb-000109
为非线性时，可以采用sigma点变换，如无迹变换规则或三次球面径向容积规则进行近似求解。当测量方程为线性时，(31)可写成
Figure PCTCN2022093182-appb-000110
步骤四：应用高斯-牛顿迭代求取代价函数的最优值；
求解代价函数极值，通常情况下可以采用对代价函数求导，令导数为零的方法去计算。
Figure PCTCN2022093182-appb-000111
由于式(33)为非线性方程，因此在本发明中使用高斯-牛顿迭代方法对(33)进行求解。
根据高斯-牛顿迭代法，对于非线性方程式(33)，解的数值更新过程为
Figure PCTCN2022093182-appb-000112
式中，上标t表示第t次迭代。迭代初值设为
Figure PCTCN2022093182-appb-000113
表示代价函数J(X k+1,P k+1)的近似海森(Hessian)矩阵。
Figure PCTCN2022093182-appb-000114
Figure PCTCN2022093182-appb-000115
其中，
Figure PCTCN2022093182-appb-000116
Figure PCTCN2022093182-appb-000117
Figure PCTCN2022093182-appb-000118
Figure PCTCN2022093182-appb-000119
整理得
Figure PCTCN2022093182-appb-000120
上式中，滤波增益为
Figure PCTCN2022093182-appb-000121
其中
Figure PCTCN2022093182-appb-000122
和
Figure PCTCN2022093182-appb-000123
相当于用来调节一步预测协方差阵和量测噪声方差的辅助参数。
估计误差为
Figure PCTCN2022093182-appb-000124
由此可以直接得到估计误差协方差阵
Figure PCTCN2022093182-appb-000125
通常情况下，对于SINS/DVL紧组合导航系统，陀螺和加速度计的随机漂移构成的过程噪声是具有精确协方差的高斯噪声。因此，相似度函数f x(t)选择为f x(t)＝-0.5t，即采用负均方误差来度量状态与预测状态之间的相似程度。而DVL波束测量噪声则可能是由野值引起的非高斯噪声，因此相似度函数f z(t)选为
Figure PCTCN2022093182-appb-000126
ω是自由度参数，m是量测维数，在本发明中为1。
步骤五：将估计的导航误差反馈给捷联惯导。
通过鲁棒卡尔曼滤波器，可以得到估计的失准角φ，速度误差δV n和位置误差δp，假设捷联惯导纯惯性解算出的姿态阵、速度、位置分别为
Figure PCTCN2022093182-appb-000127
由于采用的是反馈校正去修正捷联惯导，惯导的误差量时刻都保持为小量，因此姿态误差的反馈修正方法为
Figure PCTCN2022093182-appb-000128
速度误差δV n和位置误差δp直接在捷联惯导的输出中直接减去即可。
Figure PCTCN2022093182-appb-000129
步骤六：使用湖试实验数据对所提出算法进行验证。
图3表示一次湖试实验的航行轨迹。实验中以自研捷联惯导和DVL作为待测试设备，GPS接收机和法国进口PHINS惯导组合作为基准。
图4表示所提出算法和已有的鲁棒滤波算法应用在SINS/DVL紧组合系统中的速度误差和水平定位误差。其中，RSTKF表示已有的基于Student’s t分布建模的鲁棒卡尔曼滤波器。MCKF表示已有的基于最大相关熵准则的卡尔曼滤波器。SSMKF表示已有的基于统计相似度量的卡尔曼滤波器，SSMKF与本发明所提出的鲁棒卡尔曼滤波器最大的区别在于，SSMKF只使用一个辅助参数去对所有量测的利用率做统一调节，而本发明提出的鲁棒卡尔曼滤波器则是使用多个辅助参数分别调节不同量测分量的利用率。从图4中可以看出，所提出算法的定位误差和速度误差均最小，和基于相同统计相似度量准则的SSMKF相比，所提算法的估计精度明显更好。
图5则表示实验中一段DVL波束测量值和每个波束对应的辅助参数变化情况。从图中可以看出，当出现大离群点时，DVL波束所对应的辅助参数会明显减小，从而使得波束测量噪声方差增大，降低大离群点量测在估计结果中所占的比重。而正常的DVL波束对应的辅助参数不会发生明显变化，使得所提出算法可以在降低错误量测影响的同时保证正常测量的使用。
本发明考虑SINS/DVL紧组合系统中正常的DVL波束测量信息和大误差的波束测量信息同时出现的情形，针对目前现有的组合导航系统鲁棒滤波器对量测信息处理粗糙，导致正常量测信息易丢失的问题，基于多维量测方程分解和统计相似度量提出了新的鲁棒滤波方法。
本发明在将SINS/DVL紧组合导航系统的多维量测方程进行分解的同时，引入统计相似度量(SSM)理论，协助每个波束的测量噪声方差在大测量误差出现时完成各自的自适应更新，最终保证每个波束测量信息处理的独立性。本发明可用于非理想条件下的水下航行器组 合导航领域。
本发明的主要优点体现在：
(1)本发明将SINS/DVL紧组合导航系统的量测方程进行分解，并利用统计相似度量测概念建立代价函数，提高了组合导航系统鲁棒信息融合方法在非正常量测出现的情况下对正常量测信息的利用效率；
(2)本发明对非线性代价函数进行迭代求解，降低了数值计算误差；
(3)本发明在建立代价函数和对其进行求解时均将系统考虑为非线性系统，因此本发明所提出的鲁棒信息融合方法拥有着广阔的适用范围，既可用于线性系统，也可以用于非线性系统。
以上所述仅为本发明的优选实施例而已，并不用于限制本发明，对于本领域的技术人员来说，本发明可以有各种更改和变化。凡在本发明的精神和原则之内，所作的任何修改、等同替换、改进等，均应包含在本发明的保护范围之内。

Claims (1)

  1. 一种基于统计相似度量的组合导航鲁棒滤波方法，其特征在于，包括以下步骤：
    步骤1：定义坐标系；
    定义载体坐标系采用b表示，地心地固坐标系用e表示，“东-北-天”地理坐标系作为导航系采用n表示，惯性坐标系用i表示，DVL框架坐标系用d表示；
    定义不同坐标系下的速度：
    Figure PCTCN2022093182-appb-100001
    Figure PCTCN2022093182-appb-100002
    Figure PCTCN2022093182-appb-100003
    Figure PCTCN2022093182-appb-100004
    其中，
    Figure PCTCN2022093182-appb-100005
    表示捷联惯导系统在b系中的速度矢量；
    Figure PCTCN2022093182-appb-100006
    表示n系内捷联惯导系统的速度矢量；
    Figure PCTCN2022093182-appb-100007
    表示捷联惯导系统在d系中的速度矢量；
    Figure PCTCN2022093182-appb-100008
    表示d系中DVL波束的速度矢量；
    步骤2：建立SINS/DVL紧组合导航系统状态方程；
    选取SINS的误差作为状态，包括失准角φ、速度误差δV n、位置误差δp、陀螺常值漂移ε b和加速度计常值零偏
    Figure PCTCN2022093182-appb-100009
    状态方程如下：
    Figure PCTCN2022093182-appb-100010
    Figure PCTCN2022093182-appb-100011
    Figure PCTCN2022093182-appb-100012
    Figure PCTCN2022093182-appb-100013
    Figure PCTCN2022093182-appb-100014
    Figure PCTCN2022093182-appb-100015
    Figure PCTCN2022093182-appb-100016
    Figure PCTCN2022093182-appb-100017
    Figure PCTCN2022093182-appb-100018
    Figure PCTCN2022093182-appb-100019
    其中，ε b表示陀螺常值漂移矢量；
    Figure PCTCN2022093182-appb-100020
    是陀螺随机漂移矢量；
    Figure PCTCN2022093182-appb-100021
    是加速度计常值零偏矢量；
    Figure PCTCN2022093182-appb-100022
    是加速度计随机零偏矢量；φ表示由俯仰、横滚和偏航失准角组成的失准角矢量；
    Figure PCTCN2022093182-appb-100023
    是方向余弦矩阵，表示从b系到n系的变换；
    Figure PCTCN2022093182-appb-100024
    表示b系下b系相对于i系的角速率矢量，对应的计算误差为
    Figure PCTCN2022093182-appb-100025
    v n＝[v E v N v h] T是n系中的载体相对于地面的速度矢量，对应的误差矢量是δv n；f b是加速度计的输出；δg n是重力误差向量在n系中的投影；δp表示由纬度误差δL、经度误差δλ和高度误差δh组成的位置误差矢量；R M和R N分别表示子午线圆和本初垂直圆的曲率半径；
    步骤3：建立SINS/DVL紧组合导航系统量测方程，并分解；
    量测方程：
    Figure PCTCN2022093182-appb-100026
    Z＝HX+v
    Figure PCTCN2022093182-appb-100027
    其中，
    Figure PCTCN2022093182-appb-100028
    表示测量噪声；I 3×3是三维单位矩阵；[·×]表示向量的叉乘运算；
    Figure PCTCN2022093182-appb-100029
    α是波束和载体之间的水平角；
    Figure PCTCN2022093182-appb-100030
    设置为0°或45°；
    DVL各个波束的测量噪声通常情况下是不相关的，其量测噪声协方差阵R＝E[vv T]是一个对角阵，因此将量测方程进行等效分解：
    Figure PCTCN2022093182-appb-100031
    其中，H j表示量测矩阵的第j行；v j是相应的测量噪声，不同的DVL波束的测量噪声不相关，这里是将DVL的四个波束测量值假设为由不同的传感器分别进行测量获得；
    步骤4：基于统计相似度量构建状态估计的代价函数；
    Figure PCTCN2022093182-appb-100032
    Figure PCTCN2022093182-appb-100033
    Figure PCTCN2022093182-appb-100034
    其中，
    Figure PCTCN2022093182-appb-100035
    和
    Figure PCTCN2022093182-appb-100036
    分别表示状态一步预测结果和一步预测协方差阵；N(X；X k+1,P k+1)表示关于X的高斯分布，均值是X k+1，方差是P k+1；相似度函数f x(t)选择为f x(t)＝-0.5t；相似度函数f z(t)选为
    Figure PCTCN2022093182-appb-100037
    ω是自由度参数，m是量测维数，m＝1；q＝4；
    步骤5：应用高斯-牛顿迭代求取代价函数的最优值；
    步骤5.1：初始化迭代次数t＝0，设定迭代初值为
    Figure PCTCN2022093182-appb-100038
    步骤5.2：计算滤波增益
    Figure PCTCN2022093182-appb-100039
    Figure PCTCN2022093182-appb-100040
    其中，
    Figure PCTCN2022093182-appb-100041
    和
    Figure PCTCN2022093182-appb-100042
    相当于用来调节一步预测协方差阵和量测噪声方差的辅助参数；
    步骤5.3：计算后验状态
    Figure PCTCN2022093182-appb-100043
    Figure PCTCN2022093182-appb-100044
    步骤5.4：计算估计误差协方差阵
    Figure PCTCN2022093182-appb-100045
    Figure PCTCN2022093182-appb-100046
    步骤5.5：判断是否满足
    Figure PCTCN2022093182-appb-100047
    若不满足，则令t＝t+1，返回步骤5.2；否则，令
    Figure PCTCN2022093182-appb-100048
    输出SINS的误差X；
    步骤6：将估计的SINS的误差X反馈给SINS，进行修正；
    根据估计的SINS的误差X，获取失准角φ、速度误差δV n、位置误差δp；对于SINS解算出的姿态阵
    Figure PCTCN2022093182-appb-100049
    速度
    Figure PCTCN2022093182-appb-100050
    位置
    Figure PCTCN2022093182-appb-100051
    姿态误差的反馈修正方法为：
    Figure PCTCN2022093182-appb-100052
    速度误差δV n和位置误差δp直接在SINS的输出中直接减去即可：
    Figure PCTCN2022093182-appb-100053
PCT/CN2022/093182 2021-09-23 2022-05-17 一种基于统计相似度量的组合导航鲁棒滤波方法 WO2023045357A1 (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
US17/843,451 US12104908B2 (en) 2021-09-23 2022-06-17 Robust filtering method for integrated navigation based on statistical similarity measure

Applications Claiming Priority (2)

Application Number Priority Date Filing Date Title
CN202111115235.1A CN113819906B (zh) 2021-09-23 2021-09-23 一种基于统计相似度量的组合导航鲁棒滤波方法
CN202111115235.1 2021-09-23

Related Child Applications (1)

Application Number Title Priority Date Filing Date
US17/843,451 Continuation US12104908B2 (en) 2021-09-23 2022-06-17 Robust filtering method for integrated navigation based on statistical similarity measure

Publications (1)

Publication Number Publication Date
WO2023045357A1 true WO2023045357A1 (zh) 2023-03-30

Family

ID=78915247

Family Applications (1)

Application Number Title Priority Date Filing Date
PCT/CN2022/093182 WO2023045357A1 (zh) 2021-09-23 2022-05-17 一种基于统计相似度量的组合导航鲁棒滤波方法

Country Status (2)

Country Link
CN (1) CN113819906B (zh)
WO (1) WO2023045357A1 (zh)

Cited By (13)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN116105731A (zh) * 2023-04-07 2023-05-12 中国人民解放军国防科技大学 稀疏测距条件下的导航方法、装置、计算机设备及介质
CN116255988A (zh) * 2023-05-11 2023-06-13 北京航空航天大学 一种基于舰船动力学组合导航复合抗干扰自适应滤波方法
CN116358566A (zh) * 2023-06-01 2023-06-30 山东科技大学 一种基于抗差自适应因子的粗差探测组合导航方法
CN116592880A (zh) * 2023-07-06 2023-08-15 中国科学院空天信息创新研究院 一种uwb_ins组合定位系统自主完好性检测方法
CN116678404A (zh) * 2023-06-08 2023-09-01 哈尔滨工程大学 一种基于多重傅里叶线性组合器的舰船升沉测量方法
CN116680500A (zh) * 2023-06-12 2023-09-01 哈尔滨工程大学 水下航行器在非高斯噪声干扰下的位置估计方法及系统
CN116793384A (zh) * 2023-05-17 2023-09-22 哈尔滨工程大学 一种基于剪枝最小二乘的多普勒计程仪标定方法及装置
CN116793346A (zh) * 2023-05-17 2023-09-22 哈尔滨工程大学 一种基于改进最小二乘的捷联惯性导航多普勒计程仪标定方法、计算机设备及介质
CN116817927A (zh) * 2023-08-24 2023-09-29 北京李龚导航科技有限公司 双滤波器组合导航定位与测姿方法、电子设备及介质
CN117146830A (zh) * 2023-10-31 2023-12-01 山东科技大学 一种自适应多信标航位推算和长基线的紧组合导航方法
CN117367410A (zh) * 2023-12-04 2024-01-09 中南大学 状态估计方法、无人水下航行器及计算机可读存储介质
CN117606491A (zh) * 2024-01-24 2024-02-27 中国海洋大学 一种自主式水下航行器的组合定位导航方法及装置
CN118377992A (zh) * 2024-06-24 2024-07-23 北京大学 基于内蕴混合有限元法求解线弹性力学问题的快速方法

Families Citing this family (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US12104908B2 (en) 2021-09-23 2024-10-01 Harbin Engineering University Robust filtering method for integrated navigation based on statistical similarity measure
CN113819906B (zh) * 2021-09-23 2023-07-21 哈尔滨工程大学 一种基于统计相似度量的组合导航鲁棒滤波方法
CN115031724A (zh) * 2022-03-21 2022-09-09 哈尔滨工程大学 Sins/dvl紧组合系统dvl波束故障处理方法
CN117848351B (zh) * 2024-03-05 2024-05-28 河海大学 基于可信度和统计相似度量测的水下自主导航系统及方法

Citations (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
JP2012180024A (ja) * 2011-03-02 2012-09-20 Ihi Corp 水中航走体の自動動作確認方法及び装置
CN109724599A (zh) * 2019-03-12 2019-05-07 哈尔滨工程大学 一种抗野值的鲁棒卡尔曼滤波sins/dvl组合导航方法
CN112507281A (zh) * 2020-11-19 2021-03-16 东南大学 一种基于双状态多因子抗差估计的sins/dvl紧组合系统
CN112525218A (zh) * 2020-11-23 2021-03-19 哈尔滨工程大学 一种ins/dvl组合导航系统鲁棒智能协同校准方法
CN113819906A (zh) * 2021-09-23 2021-12-21 哈尔滨工程大学 一种基于统计相似度量的组合导航鲁棒滤波方法

Family Cites Families (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN110057382B (zh) * 2019-04-23 2021-07-09 西北工业大学 一种基于发射坐标系的捷联惯导数值更新方法
CN111947651B (zh) * 2020-07-17 2022-06-21 中国人民解放军海军工程大学 水下组合导航信息融合方法、系统及自主式水下航行器

Patent Citations (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
JP2012180024A (ja) * 2011-03-02 2012-09-20 Ihi Corp 水中航走体の自動動作確認方法及び装置
CN109724599A (zh) * 2019-03-12 2019-05-07 哈尔滨工程大学 一种抗野值的鲁棒卡尔曼滤波sins/dvl组合导航方法
CN112507281A (zh) * 2020-11-19 2021-03-16 东南大学 一种基于双状态多因子抗差估计的sins/dvl紧组合系统
CN112525218A (zh) * 2020-11-23 2021-03-19 哈尔滨工程大学 一种ins/dvl组合导航系统鲁棒智能协同校准方法
CN113819906A (zh) * 2021-09-23 2021-12-21 哈尔滨工程大学 一种基于统计相似度量的组合导航鲁棒滤波方法

Non-Patent Citations (2)

* Cited by examiner, † Cited by third party
Title
HUANG, YULONG ET AL.: "A Novel Outlier-Robust Kalman Filtering Framework Based on Statistical Similarity Measure", IEEE TRANSACTIONS ON AUTOMATIC CONTROL, vol. 66, no. 6, 30 June 2021 (2021-06-30), XP011856465, ISSN: 0018-9286, DOI: 10.1109/TAC.2020.3011443 *
XU BO; GUO YU; HU JUNMIAO: "An Improved Robust Kalman Filter for SINS/DVL Tightly Integrated Navigation System", IEEE TRANSACTIONS ON INSTRUMENTATION AND MEASUREMENT, IEEE, USA, vol. 70, 12 May 2021 (2021-05-12), USA, pages 1 - 15, XP011856906, ISSN: 0018-9456, DOI: 10.1109/TIM.2021.3079556 *

Cited By (20)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN116105731A (zh) * 2023-04-07 2023-05-12 中国人民解放军国防科技大学 稀疏测距条件下的导航方法、装置、计算机设备及介质
CN116255988A (zh) * 2023-05-11 2023-06-13 北京航空航天大学 一种基于舰船动力学组合导航复合抗干扰自适应滤波方法
CN116793384A (zh) * 2023-05-17 2023-09-22 哈尔滨工程大学 一种基于剪枝最小二乘的多普勒计程仪标定方法及装置
CN116793346A (zh) * 2023-05-17 2023-09-22 哈尔滨工程大学 一种基于改进最小二乘的捷联惯性导航多普勒计程仪标定方法、计算机设备及介质
CN116358566A (zh) * 2023-06-01 2023-06-30 山东科技大学 一种基于抗差自适应因子的粗差探测组合导航方法
CN116358566B (zh) * 2023-06-01 2023-08-11 山东科技大学 一种基于抗差自适应因子的粗差探测组合导航方法
CN116678404A (zh) * 2023-06-08 2023-09-01 哈尔滨工程大学 一种基于多重傅里叶线性组合器的舰船升沉测量方法
CN116680500B (zh) * 2023-06-12 2024-03-22 哈尔滨工程大学 水下航行器在非高斯噪声干扰下的位置估计方法及系统
CN116680500A (zh) * 2023-06-12 2023-09-01 哈尔滨工程大学 水下航行器在非高斯噪声干扰下的位置估计方法及系统
CN116592880B (zh) * 2023-07-06 2023-11-17 中国科学院空天信息创新研究院 一种uwb_ins组合定位系统自主完好性检测方法
CN116592880A (zh) * 2023-07-06 2023-08-15 中国科学院空天信息创新研究院 一种uwb_ins组合定位系统自主完好性检测方法
CN116817927A (zh) * 2023-08-24 2023-09-29 北京李龚导航科技有限公司 双滤波器组合导航定位与测姿方法、电子设备及介质
CN116817927B (zh) * 2023-08-24 2023-12-22 北京李龚导航科技有限公司 双滤波器组合导航定位与测姿方法、电子设备及介质
CN117146830A (zh) * 2023-10-31 2023-12-01 山东科技大学 一种自适应多信标航位推算和长基线的紧组合导航方法
CN117146830B (zh) * 2023-10-31 2024-01-26 山东科技大学 一种自适应多信标航位推算和长基线的紧组合导航方法
CN117367410A (zh) * 2023-12-04 2024-01-09 中南大学 状态估计方法、无人水下航行器及计算机可读存储介质
CN117367410B (zh) * 2023-12-04 2024-03-19 中南大学 状态估计方法、无人水下航行器及计算机可读存储介质
CN117606491A (zh) * 2024-01-24 2024-02-27 中国海洋大学 一种自主式水下航行器的组合定位导航方法及装置
CN117606491B (zh) * 2024-01-24 2024-04-26 中国海洋大学 一种自主式水下航行器的组合定位导航方法及装置
CN118377992A (zh) * 2024-06-24 2024-07-23 北京大学 基于内蕴混合有限元法求解线弹性力学问题的快速方法

Also Published As

Publication number Publication date
CN113819906A (zh) 2021-12-21
CN113819906B (zh) 2023-07-21

Similar Documents

Publication Publication Date Title
WO2023045357A1 (zh) 一种基于统计相似度量的组合导航鲁棒滤波方法
CN109324330B (zh) 基于混合无导数扩展卡尔曼滤波的usbl/sins紧组合导航定位方法
Wang et al. Student’s t-based robust Kalman filter for a SINS/USBL integration navigation strategy
US20240142637A1 (en) System and method for gaussian process enhanced gnss corrections generation
Xu et al. An improved robust Kalman filter for SINS/DVL tightly integrated navigation system
CN112525218B (zh) 一种ins/dvl组合导航系统鲁棒智能协同校准方法
WO2022088797A1 (zh) 考虑量测异常的集群式多深海潜航器的协同定位方法
CN111947651B (zh) 水下组合导航信息融合方法、系统及自主式水下航行器
CN110794409B (zh) 一种可估计未知有效声速的水下单信标定位方法
CN109375646A (zh) 基于fmsrupf算法的auv对接回收自主导航方法
CN110749891A (zh) 一种可估计未知有效声速的自适应水下单信标定位方法
CN111380518A (zh) 一种引入径向速度的sins/usbl紧组合导航定位方法
CN112556697A (zh) 一种基于联邦结构的浅耦合数据融合导航方法
CN115307643A (zh) 一种双应答器辅助的sins/usbl组合导航方法
CN113324546B (zh) 罗经失效下的多潜航器协同定位自适应调节鲁棒滤波方法
Liu et al. A SINS aided correct method for USBL range based on maximum correntropy criterion adaptive filter
Guo et al. A robust SINS/USBL integrated navigation algorithm based on earth frame and right group error definition
CN116222578A (zh) 基于自适应滤波和最优平滑的水下组合导航方法及系统
Zhang et al. A calibration method of ultra-short baseline installation error with large misalignment based on variational Bayesian unscented Kalman filter
CN110207721B (zh) 一种考虑残差分布的无效地形匹配结果识别方法
US12104908B2 (en) Robust filtering method for integrated navigation based on statistical similarity measure
CN118533168A (zh) 一种基于集员无迹滤波的sins/dvl紧组合抗差导航方法及装置
CN113688562A (zh) 一种基于ga算法的浅水多波束褶皱误差自动改正方法
CN115371705A (zh) 一种基于特殊正交群和鲁棒不变扩展卡尔曼滤波的dvl标定方法
KR102469164B1 (ko) 무인 수상선의 지구물리적 내비게이션 장치 및 방법

Legal Events

Date Code Title Description
121 Ep: the epo has been informed by wipo that ep was designated in this application

Ref document number: 22871418

Country of ref document: EP

Kind code of ref document: A1

NENP Non-entry into the national phase

Ref country code: DE

122 Ep: pct application non-entry in european phase

Ref document number: 22871418

Country of ref document: EP

Kind code of ref document: A1