CN113218421A - 北斗拒止条件下捷联惯导系统鲁棒自适应动态对准方法 - Google Patents

北斗拒止条件下捷联惯导系统鲁棒自适应动态对准方法 Download PDF

Info

Publication number
CN113218421A
CN113218421A CN202110513665.2A CN202110513665A CN113218421A CN 113218421 A CN113218421 A CN 113218421A CN 202110513665 A CN202110513665 A CN 202110513665A CN 113218421 A CN113218421 A CN 113218421A
Authority
CN
China
Prior art keywords
array
robust
adaptive
beidou
innovation
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
Application number
CN202110513665.2A
Other languages
English (en)
Other versions
CN113218421B (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.)
63921 Troops of PLA
Original Assignee
63921 Troops of PLA
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 63921 Troops of PLA filed Critical 63921 Troops of PLA
Priority to CN202110513665.2A priority Critical patent/CN113218421B/zh
Publication of CN113218421A publication Critical patent/CN113218421A/zh
Application granted granted Critical
Publication of CN113218421B publication Critical patent/CN113218421B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

Classifications

    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C25/00—Manufacturing, calibrating, cleaning, or repairing instruments or devices referred to in the other groups of this subclass
    • G01C25/005—Manufacturing, calibrating, cleaning, or repairing instruments or devices referred to in the other groups of this subclass initial alignment, calibration or starting-up of inertial devices

Abstract

本发明属于自适应滤波技术领域，公开了一种北斗拒止条件下捷联惯导系统鲁棒自适应动态对准方法，初始对准滤波模型的构建；进行卡尔曼滤波时间更新；计算观测量与观测新息之间的马氏距离，进行观测新息权值计算；基于Sage‑Husa滤波方法重新定义R阵自适应估计方程使其具有鲁棒性，并对R阵进行鲁棒自适应估计；通过引入膨胀因子，修正估计出的R阵；利用修正后的R阵进行KF的量测更新。本发明既能够保证R阵的鲁棒自适应性，也能保证整个滤波器的鲁棒性，即能在对R阵进行自适应估计的同时，实现鲁棒滤波。本发明的鲁棒自适应KF算法能够在复杂环境下实现对R阵的实时估计，有效克服观测异常值的不良影响。

Description

北斗拒止条件下捷联惯导系统鲁棒自适应动态对准方法
技术领域
本发明属于自适应滤波技术领域，尤其涉及一种北斗拒止条件下捷联惯导系统鲁棒自适应动态对准方法(SHRAKF)、系统、介质、水下航行器。
背景技术
目前，自主式水下航行器(Autonomous Underwater Vehicle，AUV)是海洋资源勘探、开发，海洋工程以及科学研究的主要运载体，其自主导航系统主要依赖惯性导航系统(Inertial navigation system，INS)。捷联式惯导系统(Strapdown inertial navigationsystem，SINS)凭借其自主性强、隐蔽性高、结构简单、体积小及成本低等优点，在水下定位导航授时(Position,Navigation and Timing,PNT)领域受到广泛关注，并成为AUV自主导航定位的重要工具。然而，SINS的本质是基于牛顿第二定律的积分推算系统，误差会随着时间而积累。由此可知，任何初始误差如初始姿态误差、惯性测量单元(InertialMeasurement Unit，IMU)器件误差等都会进入到积分系统造成积分运算误差，进而影响SINS长航时、长航程的导航精度。因此，准确地确定积分推算系统的积分初始值对实现SINS精确导航有着至关重要的作用。初始对准是水下导航的前置步骤，是确保高精度导航的重要前提。而水下环境拒止全球导航卫星系统(Global Navigation Satellite System，GNSS)，如北斗卫星导航系统(简称“北斗”)信号，所以无法利用GNSS进行初始对准，研究不依赖北斗的水下初始对准技术是水下PNT领域中的一大技术重、难点。在无北斗辅助条件下，通常会用多普勒计程仪(Doppler velocity log，DVL)辅助SINS进行水下动态初始对准。
卡尔曼滤波(Kalman Filter,KF)方法是初始对准系统中广泛使用的一种信息融合方式，该算法采用预建噪声模型，认为噪声为统计特性已知的不相关高斯白噪声。利用KF处理线性、无偏系统问题时，系统的干扰与量测噪声的统计特性先验已知，则可以通过滤波获得系统状态的最优估计。但是，在水下初始对准中，由于深海环境中情况复杂，如洋流干扰、海底地质环境、海水温度、盐度变化、鱼群影响等均会影响DVL对速度的测量，速度辅助信息容易受到野值等非高斯噪声的污染，这些不确定因素将导致KF中的量测噪声阵不够准确，从而导致滤波性能降低，甚至滤波发散。因此，亟需一种北斗拒止条件下捷联惯导系统鲁棒自适应动态对准方法。
通过上述分析，现有技术存在的问题及缺陷为：在水下动态初始对准中，由于深海环境中情况复杂，会影响DVL对速度的测量，外部辅助信息容易受到野值等非高斯噪声的污染，这些不确定因素也将导致KF中的量测噪声阵R阵不够准确，从而使滤波性能降低，甚至发散。
解决以上问题及缺陷的难度为：对于长航时、长航程的AUV而言，其工作的外部环境复杂、多变，并且存在很多无法避免的不可控因素，高精度姿态测量、确定是保证其正常航行的重要前提。不同于陆上或水面上，由于电磁波信号在水下衰减极快，导致水下SINS的动态启动以及长航时、长航程导航无法获取来自北斗的速度、位置信息的辅助。在水面以上，运载体可以通过卫星定位测姿或卫星/INS组合测姿获取准确的姿态信息。而在水下无GNSS信号的环境中，SINS初始姿态的确定尤其是动态条件下初始对准尤为困难。
解决以上问题及缺陷的意义为：对无北斗信号条件下SINS动态初始对准方法的研究，对提高AUV机动性、隐蔽性、长航时长航程和高精度的导航能力具有重要的现实意义。
发明内容
针对现有技术存在的问题，本发明提供了一种北斗拒止条件下捷联惯导系统鲁棒自适应动态对准方法(SHRAKF)、系统、介质、水下航行器。
本发明是这样实现的，一种北斗拒止条件下捷联惯导系统鲁棒自适应动态对准方法(SHRAKF)，包括以下步骤：
步骤一，初始对准滤波模型的构建；
步骤二，进行KF算法的时间更新，即进行SHRAKF时间更新；
步骤三，进行观测量与观测新息之间的马氏距离计算，计算观测新息对应的权值；
步骤四，基于Sage Husa方法设计量测噪声阵(R阵)鲁棒自适应估计策略，重新定义R阵鲁棒自适应估计表达式，进行R阵自适应估计；
步骤五，基于马氏距离算法，引入膨胀因子，对估计出的R阵进行修正；
步骤六，利用修正后的R阵进行KF算法量测更新，即进行SHRAKF的量测更新。
进一步，步骤一中，所述考虑初始对准滤波模型的构建，包括：
考虑初始对准滤波模型如式(1)和式(2)所示：
xk＝Fk-1xk-1+wk-1 (1)
zk＝Hkxk+υk (2)
式中，xk、wk-1分别为状态向量和过程噪声向量；wk～N(0,Qk)，Qk为过程噪声协方差阵；zk、υk分别为观测量和量测噪声向量，且υk通常假定服从正态分布，即υk～N(0,Rk)，Rk为量测噪声协方差阵。
进一步，步骤二中，所述进行KF的时间更新，即进行SHRAKF时间更新，包括：
k时刻SHRAKF时间更新方程如下：
Figure BDA0003061002020000031
Figure BDA0003061002020000032
式中，Fk-1为k-1时刻状态转移矩阵，
Figure BDA0003061002020000033
为状态量的先验估计，Pkk-1为状态误差协方差阵的先验估计。
进一步，步骤三中，所述进行观测量与观测新息之间的马氏距离计算，观测新息对应的权值计算，包括：
Figure BDA0003061002020000041
式中，μk为观测新息向量，zk为观测向量，
Figure BDA0003061002020000042
为观测量的先验估计，Hk为量测阵。为辨识异常新息，考虑到k时刻观测量zk与观测新息μk之间的马氏距离表达式如下：
Figure BDA0003061002020000043
式中，
Figure BDA0003061002020000044
Figure BDA0003061002020000045
为观测量的真实值。选取统计门限η满足
Figure BDA0003061002020000046
服从卡方分布，η为辨识门限，取
Figure BDA0003061002020000047
当Mk＞η时，观测量
Figure BDA0003061002020000048
将会被标记为异常观测量/新息；反之，
Figure BDA0003061002020000049
将会被标记为正常观测量/新息。新息向量μk对应的权值表达式如下：
Figure BDA00030610020200000410
若k时刻观测信息异常，则权值
Figure BDA00030610020200000411
反之，ωk＝1。
进一步，步骤四中，所述基于Sage Husa方法设计量测噪声阵(R阵)鲁棒自适应估计策略，重新定义R阵鲁棒自适应估计表达式，包括：
Figure BDA00030610020200000412
Figure BDA00030610020200000413
Pk＝(I-KkHk)Pk|k-1 (10)
在高斯条件下，基于利用Sage Husa方法，k时刻R阵自适应估计的一般表达式如下：
Figure BDA00030610020200000414
式中，
Figure BDA00030610020200000415
0.9≤b＜1为衰减因子，取经验值；式(11)对异常观测量是没有鲁棒性的，若k时刻观测量异常，即μk异常，估计出来的
Figure BDA00030610020200000416
也是不准确的。结合式(7)和式(11)，重新定义非高斯条件下k时刻R阵自适应估计表达式如下：
Figure BDA0003061002020000051
用
Figure BDA0003061002020000052
替代式(8)中Rk并进行标准KF的量测更新，即为基于Sage Husa算法的自适应KF方法(SHAKF)的量测更新。
进一步，步骤五中，所述引入膨胀因子κk，修正估计出的量测噪声协方差阵
Figure BDA0003061002020000053
包括：
若
Figure BDA0003061002020000054
满足
Figure BDA0003061002020000055
则
Figure BDA0003061002020000056
将被标记为异常观测量，此时通过引入膨胀因子κk用以修正估计出的量测噪声协方差阵
Figure BDA0003061002020000057
即：
Figure BDA0003061002020000058
将式(13)带入式(6)可得：
Figure BDA0003061002020000059
式(14)可以转化为求解κk的非线性问题，如式(15)所示：
Figure BDA00030610020200000510
式中，κk可以通过牛顿迭代法求解；κk(i+1)与κk(i)的关系可表示为：
Figure BDA00030610020200000511
式中，
Figure BDA00030610020200000512
且κk(i)初始值为κk(0)＝1；当满足
Figure BDA00030610020200000513
时，迭代终止。
进一步，步骤六中，所述利用修正后的R阵进行KF量测更新，即进行SHRAKF量测更新，包括：
SHRAKF量测更新过程如下：
Figure BDA00030610020200000514
Figure BDA00030610020200000515
根据式(13)-(16)计算修正后的量测噪声阵
Figure BDA00030610020200000516
Figure BDA00030610020200000517
Figure BDA0003061002020000061
Pk＝(I-KkHk)Pk|k-1 (21)。
本发明的另一目的在于提供一种应用所述的北斗拒止条件下捷联惯导系统鲁棒自适应动态对准方法的北斗拒止条件下捷联惯导系统鲁棒自适应动态对准系统，所述北斗拒止条件下捷联惯导系统鲁棒自适应动态对准系统包括：
对准滤波模型构建模块，用于考虑初始对准滤波模型的构建；
时间更新模块，用于进行KF的时间更新，即进行北斗拒止条件下捷联惯导系统鲁棒自适应动态对准方法(SHRAKF)时间更新；
计算观测新息权值模块，用于进行观测量与观测新息之间马氏距离计算以及进行观测新息权值计算；
量测噪声阵鲁棒自适应估计模块，用于在非高斯条件下进行量测噪声阵鲁棒自适应估计；
修正模块，用于通过马氏距离算法引入膨胀因子，修正估计出的量测噪声协方差阵；
量测更新模块，用于进行SHRAKF的量测更新。
本发明的另一目的在于提供一种自主式水下航行器，所述自主式水下航行器执行所述的北斗拒止条件下捷联惯导系统鲁棒自适应动态对准方法。
本发明的另一目的在于提供一种计算机可读存储介质，储存有指令，当所述指令在计算机上运行时，使得计算机执行所述的北斗拒止条件下捷联惯导系统鲁棒自适应动态对准方法。
结合上述的所有技术方案，本发明所具备的优点及积极效果为：
本发明通过动态初始对准滤波模型的构建；进行卡尔曼滤波(KF)时间更新；计算观测量与观测新息之间的马氏距离，进行观测新息权值计算；基于Sage-Husa滤波方法重新定义R阵自适应估计方程使其具有鲁棒性，并对R阵进行鲁棒自适应估计；通过引入膨胀因子，修正估计出的R阵；利用修正后的R阵进行KF的量测更新，即为SHRAKF的量测更新过程。本发明既能够保证R阵的鲁棒自适应性，也能保证整个滤波器的鲁棒性，即SHRAKF能在对R阵进行自适应估计的同时，实现鲁棒滤波。本发明的鲁棒自适应KF算法能够在复杂环境下实现对R阵的实时估计，有效克服观测异常值的不良影响。
相比于现有技术，本发明的优点进一步包括：
本发明提供的北斗拒止条件下捷联惯导系统鲁棒自适应动态对准方法(SHRAKF)，既能够在北斗拒止的水下复杂环境下保证量测噪声阵的鲁棒自适应性，也保证了整个滤波器的鲁棒性。也就是说，SHRAKF能在对量测噪声阵进行自适应估计的同时，实现鲁棒滤波。本发明的SHRAKF具有以下特点：1.能够在复杂环境下实现对R阵的实时估计(实时性)；2.能够有效克服观测异常值的不良影响(鲁棒性)。
选取900s船载实测数据进行初始对准试验。其中，惯性测量单元(InertialMeasurement Unit，IMU)和多普勒计程仪(DVL)性能指标分别如表1，表2所示。DVL输出速度如图3所示。
表1本实施例中IMU性能指标
主要技术指标 陀螺仪 加速度计
测量范围 ±200°/s ±15g
更新频率 200Hz 200Hz
精度等级 <0.02°/h(1σ) <50ug(1σ)
标度因数重复性 ≤30ppm ≤50ppm
表2本实施例中DVL性能指标
主要技术指标
测速精度 0.5％V±0.5cm/s
测速范围 -10knot～20knot
更新频率 1Hz
发射频率 300kHz
底跟踪深度 300m
试验中，设置SINS初始失准角为[1°；1°；1°]，式(7)中统计门限η2设置为
Figure BDA0003061002020000081
在实际应用中，量测噪声统计特性先验信息通常是不确定或未知的，因此假设初始量测噪声阵R0是不准确的。试验中，设置R0＝diag([0.012,0.012])m2/s2。
对于水下SINS动态初始对准，外部复杂环境是不可避免的，也就是说，DVL输出速度不可避免地会受到非高斯噪声的污染。本发明提供的北斗拒止条件下捷联惯导系统鲁棒自适应动态对准方法简记为SHRAKF，传统的卡尔曼滤波方法简记为KF，基于Sage Husa算法的自适应KF方法简记为SHAKF。为进一步验证本发明提供的SHRAKF在非高斯条件下的有效性、可行性和优势，分别利用KF、SHAKF、SHRAKF进行初始对准试验，姿态、速度和位置对准结果如图4-图6所示。
图4-图6中，黑色虚线、浅灰虚线和实线分别表示由KF、SHAKF和SHRAKF得到的初始对准误差曲线。由图4-图6可以明显看出，在观测量受到野值污染时刻，KF得到的初始对准误差曲线出现跳变。由图4和图5可以看出，SHAKF得到的水平姿态角和速度对准误差曲线相比于KF具备较好的收敛性。然而，SHAKF得到的航向角及位置对准误差曲线在受野值污染时刻发生跳变。SHRAKF得到的初始对准误差曲线在整个动态对准过程中是稳定、收敛的。相比于KF，SHAKF，SHRAKF具备更强鲁棒性和更高对准精度。
图7表示分别利用SHAKF和SHRAKF得到的R阵分量自适应估计结果。可以明显看出，相比于SHAKF，利用SHRAKF得到的R阵分量自适应估计曲线更加稳定。实际的R阵约为diag(0.01，0.01)m2/s2。经计算，SHRAKF方法对R(1,1)估计值的时间平均(0～900s)为0.0074m2/s2，对R(2,2)估计值的时间平均(0～900s)为0.0100m2/s2；SHAKF方法对R(1,1)估计值的时间平均(0～900s)为1.5611m2/s2，对R(2,2)估计值的时间平均(0～900s)为11.1005m2/s2。因此，在非高斯条件下，相比于SHAKF，SHRAKF对R阵具有更高的自适应估计精度及稳定性。
根据试验结果，不同方法得到的航向角和位置对准误差标准差如表3所示。
表3不同方法得到初始对准误差的标准差
航向角(°) 位置(m)
KF 3.4970 130.0000
SHAKF 0.5721 16.7800
SHRAKF 0.3602 6.8680
由表3可知，SHRAKF得到的对准误差标准差小于KF和SHAKF。也就是说，相比于KF和SHAKF，SHRAKF得到的初始对准误差更加稳定。初始对准试验结果验证了SHRAKF的鲁棒性和自适应性。
附图说明
为了更清楚地说明本发明实施例的技术方案，下面将对本发明实施例中所需要使用的附图做简单的介绍，显而易见地，下面所描述的附图仅仅是本发明的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下还可以根据这些附图获得其他的附图。
图1是本发明实施例提供的北斗拒止条件下捷联惯导系统鲁棒自适应动态对准方法流程图。
图2是本发明实施例提供的北斗拒止条件下捷联惯导系统鲁棒自适应动态对准系统结构框图；
图中：1、对准滤波模型构建模块；2、时间更新模块；3、计算观测新息权值模块；4、量测噪声阵(R阵)鲁棒自适应估计模块；5、修正模块；6、量测更新模块。
图3是本发明实施例提供的选取900s船载实测数据进行初始对准试验中DVL输出速度图。
图4是本发明实施例提供的不同方法姿态角对准误差示意图。其中，图4(a)俯仰角对准误差图；图4(b)横滚角对准误差图；图4(c)横滚角对准误差图。
图5是本发明实施例提供的不同方法速度对准误差示意图。其中，图5(a)东向速度对准误差图；图5(b)北向速度对准误差图。
图6是本发明实施例提供的不同方法位置对准误差图。
图7是本发明实施例提供的不同方法量测噪声协方差分量自适应估计示意图。
具体实施方式
为了使本发明的目的、技术方案及优点更加清楚明白，以下结合实施例，对本发明进行进一步详细说明。应当理解，此处所描述的具体实施例仅仅用以解释本发明，并不用于限定本发明。
针对现有技术存在的问题，本发明提供了一种北斗拒止条件下捷联惯导系统鲁棒自适应动态对准方法(SHRAKF)及系统，下面结合附图对本发明作详细的描述。
如图1所示，本发明实施例提供的北斗拒止条件下捷联惯导系统鲁棒自适应动态对准方法及系统包括以下步骤：
S101，考虑初始对准滤波模型的构建；
S102，进行KF算法的时间更新，即进行SHRAKF时间更新；
S103，进行观测量与观测新息之间的马氏距离计算，计算观测新息对应的权值；
S104，基于Sage Husa方法设计量测噪声阵(R阵)鲁棒自适应估计策略，重新定义R阵鲁棒自适应估计表达式，进行R阵自适应估计；
S105，基于马氏距离算法，引入膨胀因子，对估计出的R阵进行修正；
S106，利用修正后的R阵进行KF算法量测更新，即进行基于Sage Husa方法的鲁棒自适应KF算法SHRAKF的量测更新。
如图2所示，本发明实施例提供的北斗拒止条件下捷联惯导系统鲁棒自适应动态对准系统，包括：
对准滤波模型构建模块1，用于考虑初始对准滤波模型的构建；
时间更新模块2，用于进行KF的时间更新，即进行SHRAKF的时间更新；
计算观测新息权值模块3，用于进行观测量与观测新息之间马氏距离计算以及进行观测新息权值计算；
量测噪声阵(R阵)鲁棒自适应估计模块4，用于在非高斯条件下进行R阵鲁棒自适应估计；
修正模块5，用于通过马氏距离算法引入膨胀因子，修正估计出的量测噪声协方差阵；
量测更新模块6，用于进行SHRAKF的量测更新。
下面结合实施例对本发明作进一步描述。
实施例：实验验证与结果分析
1、数据来源
试验数据是从一套船载实验系统中采集得到的，其中，惯性测量单元(InertialMeasurement Unit，IMU)和多普勒计程仪(DVL)性能指标分别如表1，表2所示。DVL输出速度如图3所示。
表1本实施例中IMU性能指标
主要技术指标 陀螺仪 加速度计
测量范围 ±200°/s ±15g
更新频率 200Hz 200Hz
精度等级 <0.02°/h(1σ) <50ug(1σ)
标度因数重复性 ≤30ppm ≤50ppm
表2本实施例中DVL性能指标
Figure BDA0003061002020000111
Figure BDA0003061002020000121
2、实验验证与结果分析
选取900s船载实测数据进行初始对准试验。试验中，设置SINS初始失准角为[1°；1°；1°]，式(7)中统计门限η2设置为
Figure BDA0003061002020000122
在实际应用中，量测噪声统计特性先验信息通常是不确定或未知的，因此假设初始量测噪声阵是不准确的，即R0＝diag([0.012,0.012])m2/s2。
对于水下SINS动态初始对准，外部复杂环境是不可避免的，也就是说，DVL输出速度不可避免地会受到非高斯噪声的污染。本发明提供的新的鲁棒自适应KF方法简记为SHRAKF，传统的卡尔曼滤波方法简记为KF，基于Sage Husa算法的自适应卡尔曼滤波方法简记为SHAKF。为进一步验证本发明提供的SHRAKF在非高斯条件下的有效性、可行性和优势，分别利用KF、SHAKF、SHRAKF进行初始对准试验，姿态、速度和位置对准结果如图4-图6所示。
图4-图6中，黑色虚线、浅灰虚线和红色实线分别表示由KF、SHAKF和SHRAKF得到的初始对准误差曲线。由图4-图6可以明显看出，在观测量受到野值污染时刻，KF得到的初始对准误差曲线出现跳变。由图4和图5可以看出，SHAKF得到的水平姿态角和速度对准误差曲线相比于KF具备较好的收敛性。然而，SHAKF得到的航向角及位置对准误差曲线在受野值污染时刻发生跳变。SHRAKF得到的初始对准误差曲线在整个动态对准过程中是稳定、收敛的。相比于KF，SHAKF，SHRAKF具备更强鲁棒性和更高对准精度。
图7表示分别利用SHAKF和SHRAKF得到的R阵分量自适应估计结果。可以明显看出，相比于SHAKF，利用SHRAKF得到的R阵分量自适应估计曲线更加稳定。实际的R阵约为diag(0.01，0.01)m2/s2。经计算，SHRAKF方法对R(1,1)估计值的时间平均(0～900s)为0.0074m2/s2，对R(2,2)估计值的时间平均(0～900s)为0.0100m2/s2；SHAKF方法对R(1,1)估计值的时间平均(0～900s)为1.5611m2/s2，对R(2,2)估计值的时间平均(0～900s)为11.1005m2/s2。因此，在非高斯条件下，相比于SHAKF，SHRAKF对R阵具有更高的自适应估计精度及稳定性。
根据试验结果，不同方法得到的航向角和位置对准误差标准差如表3所示。
表3不同方法得到初始对准误差的标准差
航向角(°) 位置(m)
KF 3.4970 130.0000
SHAKF 0.5721 16.7800
SHRAKF 0.3602 6.8680
由表3可知，SHRAKF得到的对准误差标准差小于KF和SHAKF。也就是说，相比于KF和SHAKF，SHRAKF得到的初始对准误差更加稳定。初始对准试验结果验证了SHRAKF的鲁棒性和自适应性。
在上述实施例中，可以全部或部分地通过软件、硬件、固件或者其任意组合来实现。当使用全部或部分地以计算机程序产品的形式实现，所述计算机程序产品包括一个或多个计算机指令。在计算机上加载或执行所述计算机程序指令时，全部或部分地产生按照本发明实施例所述的流程或功能。所述计算机可以是通用计算机、专用计算机、计算机网络、或者其他可编程装置。所述计算机指令可以存储在计算机可读存储介质中，或者从一个计算机可读存储介质向另一个计算机可读存储介质传输，例如，所述计算机指令可以从一个网站站点、计算机、服务器或数据中心通过有线(例如同轴电缆、光纤、数字用户线(DSL)或无线(例如红外、无线、微波等)方式向另一个网站站点、计算机、服务器或数据中心进行传输)。所述计算机可读取存储介质可以是计算机能够存取的任何可用介质或者是包含一个或多个可用介质集成的服务器、数据中心等数据存储设备。所述可用介质可以是磁性介质，(例如，软盘、硬盘、磁带)、光介质(例如，DVD)、或者半导体介质(例如固态硬盘SolidState Disk(SSD))等。
以上所述，仅为本发明的具体实施方式，但本发明的保护范围并不局限于此，任何熟悉本技术领域的技术人员在本发明揭露的技术范围内，凡在本发明的精神和原则之内所作的任何修改、等同替换和改进等，都应涵盖在本发明的保护范围之内。

Claims (10)

1.一种北斗拒止条件下捷联惯导系统鲁棒自适应动态对准方法，其特征在于，所述北斗拒止条件下捷联惯导系统鲁棒自适应动态对准方法包括以下步骤：
步骤一，初始对准滤波模型的构建；
步骤二，进行KF算法的时间更新；
步骤三，进行观测量与观测新息之间的马氏距离计算，计算观测新息对应的权值；
步骤四，基于Sage Husa方法设计量测噪声阵(R阵)鲁棒自适应估计策略，重新定义R阵鲁棒自适应估计表达式，进行R阵自适应估计；
步骤五，基于马氏距离算法，引入膨胀因子，对估计出的R阵进行修正；
步骤六，利用修正后的R阵进行KF算法量测更新，即进行基于Sage Husa方法的鲁棒自适应KF算法SHRAKF的量测更新。
2.如权利要求1所述的北斗拒止条件下捷联惯导系统鲁棒自适应动态对准方法，其特征在于，所述步骤一初始对准滤波模型的构建，包括：
考虑初始对准滤波模型如式(1)和式(2)所示：
xk＝Fk-1xk-1+wk-1 (1)
zk＝Hkxk+υk (2)
式中，xk、wk-1分别为状态向量和过程噪声向量；wk～N(0,Qk)，Qk为过程噪声协方差阵；zk、υk分别为观测量和量测噪声向量，且υk通常假定服从正态分布，即υk～N(0,Rk)，Rk为量测噪声协方差阵。
3.如权利要求1所述的北斗拒止条件下捷联惯导系统鲁棒自适应动态对准方法，其特征在于，所述步骤二进行KF的时间更新，即进行SHRAKF时间更新，包括：
k时刻SHRAKF时间更新方程如下：
Figure FDA0003061002010000011
Figure FDA0003061002010000012
式中，Fk-1为k-1时刻状态转移矩阵，
Figure FDA0003061002010000021
为状态量的先验估计，Pk|k-1为状态误差协方差阵的先验估计。
4.如权利要求1所述的北斗拒止条件下捷联惯导系统鲁棒自适应动态对准方法，其特征在于，所述步骤三进行观测量与观测新息之间的马氏距离计算，观测新息对应的权值计算，包括：
Figure FDA0003061002010000022
式中，μk为观测新息向量，zk为观测向量，
Figure FDA0003061002010000023
为观测量的先验估计，Hk为量测阵；为辨识异常新息，k时刻观测量zk与观测新息μk之间的马氏距离表达式如下：
Figure FDA0003061002010000024
式中，
Figure FDA0003061002010000025
Figure FDA0003061002010000026
为观测量的真实值；选取统计门限η满足
Figure FDA0003061002010000027
服从卡方分布，η为辨识门限，取
Figure FDA0003061002010000028
当Mk＞η时，观测量
Figure FDA0003061002010000029
将被标记为异常观测量/新息；反之，
Figure FDA00030610020100000210
将被标记为正常观测量/新息；新息向量μk对应的权值表达式如式(7)。
Figure FDA00030610020100000211
若k时刻观测信息μk异常，则其对应的权值
Figure FDA00030610020100000212
反之，ωk＝1。
5.如权利要求1所述的北斗拒止条件下捷联惯导系统鲁棒自适应动态对准方法，其特征在于，所述步骤四基于Sage Husa方法设计量测噪声阵(R阵)鲁棒自适应估计策略，重新定义R阵鲁棒自适应估计表达式，包括：
Figure FDA00030610020100000213
Figure FDA00030610020100000214
Pk＝(I-KkHk)Pk|k-1 (10)
在高斯条件下，基于利用Sage Husa方法，k时刻R阵自适应估计的一般表达式如下：
Figure FDA0003061002010000031
式中，
Figure FDA0003061002010000032
0.9≤b＜1为衰减因子，取经验值；式(11)对异常的观测量没有鲁棒性，若k时刻的观测量异常，即μk异常，估计出来的
Figure FDA0003061002010000033
不准确；结合式(7)和式(11)，重新定义非高斯条件下k时刻R阵自适应估计表达式如下：
Figure FDA0003061002010000034
用
Figure FDA0003061002010000035
替代式(8)中Rk并进行标准KF的量测更新，即为基于Sage Husa方法鲁棒自适应KF算法。
6.如权利要求1所述的北斗拒止条件下捷联惯导系统鲁棒自适应动态对准方法，其特征在于，所述步骤五通过引入膨胀因子κk，修正估计出的量测噪声协方差阵
Figure FDA0003061002010000036
包括：
若
Figure FDA0003061002010000037
满足
Figure FDA0003061002010000038
则
Figure FDA0003061002010000039
将被标记为异常观测量，此时通过引入膨胀因子κk用以修正估计出的量测噪声协方差阵
Figure FDA00030610020100000310
即：
Figure FDA00030610020100000311
将式(13)带入式(6)得：
Figure FDA00030610020100000312
式(14)转化为求解κk的非线性问题，如式(15)所示：
Figure FDA00030610020100000313
式中，κk通过牛顿迭代法求解；κk(i+1)与κk(i)的关系表示为：
Figure FDA00030610020100000314
式中，
Figure FDA00030610020100000315
且κk(i)初始值为κk(0)＝1；当满足
Figure FDA00030610020100000316
时，迭代终止。
7.如权利要求1所述的北斗拒止条件下捷联惯导系统鲁棒自适应动态对准方法，其特征在于，所述步骤六利用修正后的R阵进行KF量测更新，即进行SHRAKF的量测更新，包括：
SHRAKF量测更新过程如下：
Figure FDA0003061002010000041
Figure FDA0003061002010000042
根据式(13)-(16)计算修正后的量测噪声阵
Figure FDA0003061002010000043
Figure FDA0003061002010000044
Figure FDA0003061002010000045
Pk＝(I-KkHk)Pk|k-1 (21)。
8.一种北斗拒止条件下捷联惯导系统鲁棒自适应动态对准系统，其特征在于，所述北斗拒止条件下捷联惯导系统鲁棒自适应动态对准系统包括：
对准滤波模型构建模块，用于考虑初始对准滤波模型的构建；
时间更新模块，用于进行KF的时间更新，即进行SHRAKF时间更新；
计算观测新息权值模块，用于进行观测量与观测新息之间马氏距离计算以及进行观测新息权值计算；
量测噪声阵鲁棒自适应估计模块，用于在非高斯条件下进行量测噪声阵鲁棒自适应估计；
修正模块，用于通过马氏距离算法引入膨胀因子，修正估计出的量测噪声协方差阵；
量测更新模块，用于进行SHRAKF的量测更新。
9.一种计算机可读存储介质，储存有指令，当所述指令在计算机上运行时，使得计算机执行如权利要求1～7任意一项所述的北斗拒止条件下捷联惯导系统鲁棒自适应动态对准方法。
10.一种自主式水下航行器，其特征在于，所述自主式水下航行器执行如权利要求1～7任意一项所述的北斗拒止条件下捷联惯导系统鲁棒自适应动态对准方法。
CN202110513665.2A 2021-05-11 2021-05-11 北斗拒止条件下捷联惯导系统鲁棒自适应动态对准方法 Active CN113218421B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202110513665.2A CN113218421B (zh) 2021-05-11 2021-05-11 北斗拒止条件下捷联惯导系统鲁棒自适应动态对准方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202110513665.2A CN113218421B (zh) 2021-05-11 2021-05-11 北斗拒止条件下捷联惯导系统鲁棒自适应动态对准方法

Publications (2)

Publication Number Publication Date
CN113218421A true CN113218421A (zh) 2021-08-06
CN113218421B CN113218421B (zh) 2023-07-04

Family

ID=77095055

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202110513665.2A Active CN113218421B (zh) 2021-05-11 2021-05-11 北斗拒止条件下捷联惯导系统鲁棒自适应动态对准方法

Country Status (1)

Country Link
CN (1) CN113218421B (zh)

Cited By (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN114777812A (zh) * 2022-04-17 2022-07-22 中国人民解放军国防科技大学 一种水下组合导航系统行进间对准与姿态估计方法

Citations (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN108759870A (zh) * 2018-07-03 2018-11-06 哈尔滨工业大学 一种基于新型鲁棒广义高阶容积卡尔曼滤波的传递对准方法
CN109443379A (zh) * 2018-09-28 2019-03-08 东南大学 一种深海潜航器的sins/dvl水下抗晃动对准方法
CN109974695A (zh) * 2019-04-09 2019-07-05 东南大学 基于Krein空间的水面舰艇导航系统的鲁棒自适应滤波方法
US20210080287A1 (en) * 2019-09-18 2021-03-18 Harbin Engineering University Method for initial alignment of radar assisted airborne strapdown inertial navigation system

Patent Citations (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN108759870A (zh) * 2018-07-03 2018-11-06 哈尔滨工业大学 一种基于新型鲁棒广义高阶容积卡尔曼滤波的传递对准方法
CN109443379A (zh) * 2018-09-28 2019-03-08 东南大学 一种深海潜航器的sins/dvl水下抗晃动对准方法
CN109974695A (zh) * 2019-04-09 2019-07-05 东南大学 基于Krein空间的水面舰艇导航系统的鲁棒自适应滤波方法
US20210080287A1 (en) * 2019-09-18 2021-03-18 Harbin Engineering University Method for initial alignment of radar assisted airborne strapdown inertial navigation system

Cited By (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN114777812A (zh) * 2022-04-17 2022-07-22 中国人民解放军国防科技大学 一种水下组合导航系统行进间对准与姿态估计方法
CN114777812B (zh) * 2022-04-17 2024-04-05 中国人民解放军国防科技大学 一种水下组合导航系统行进间对准与姿态估计方法

Also Published As

Publication number Publication date
CN113218421B (zh) 2023-07-04

Similar Documents

Publication Publication Date Title
WO2022088797A1 (zh) 考虑量测异常的集群式多深海潜航器的协同定位方法
CN109283562B (zh) 一种车联网中车辆三维定位方法及装置
Li et al. A multi-model EKF integrated navigation algorithm for deep water AUV
CN110207695B (zh) 一种适用于深海auv的无速度辅助单信标定位方法
CN112146655B (zh) 一种BeiDou/SINS紧组合导航系统弹性模型设计方法
CN110132281B (zh) 一种基于询问应答模式的水下高速目标高精度自主声学导航方法
CN111174774A (zh) 一种某深度水层面模式下的导航信息融合方法及系统
CN110703205B (zh) 基于自适应无迹卡尔曼滤波的超短基线定位方法
Wang et al. System noise variance matrix adaptive Kalman filter method for AUV INS/DVL navigation system
Das et al. An experimental study on relative and absolute pose graph fusion for vehicle localization
CN113218421A (zh) 北斗拒止条件下捷联惯导系统鲁棒自适应动态对准方法
CN114370878B (zh) 一种基于stackf的多auv协同定位方法
Wang et al. A novel adaptive sliding observation-based cooperative positioning algorithm under factor graph framework for multiple UUVs
CN111708008B (zh) 一种基于imu和tof的水下机器人单信标导航方法
CN110763234B (zh) 一种水下机器人海底地形匹配导航路径规划方法
CN116026324B (zh) 用于水-空跨介质航行器的跨域导航系统和方法
CN115096302A (zh) 捷联惯性基导航系统信息滤波鲁棒对准方法、系统及终端
CN116358544A (zh) 基于声学特征匹配定位修正惯性导航误差的方法和系统
CN113503891B (zh) 一种sinsdvl对准校正方法、系统、介质及设备
CN113821766B (zh) 一种海洋声学导航系统误差改正方法
CN116222541A (zh) 利用因子图的智能多源组合导航方法及装置
CN113008235B (zh) 基于矩阵k-l散度的多源导航信息融合方法
Koshaev AUV relative position and attitude determination using acoustic beacons
US20100289699A1 (en) Method and Apparatus for Determining a Position of a Mobile Station in a Wireless Communication System
Wu et al. Improved adaptive iterated extended Kalman filter for GNSS/INS/UWB-integrated fixed-point positioning

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