CN111024064B - 一种改进Sage-Husa自适应滤波的SINS/DVL组合导航方法 - Google Patents

一种改进Sage-Husa自适应滤波的SINS/DVL组合导航方法 Download PDF

Info

Publication number
CN111024064B
CN111024064B CN201911163364.0A CN201911163364A CN111024064B CN 111024064 B CN111024064 B CN 111024064B CN 201911163364 A CN201911163364 A CN 201911163364A CN 111024064 B CN111024064 B CN 111024064B
Authority
CN
China
Prior art keywords
representing
dvl
value
measurement
information
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.)
Active
Application number
CN201911163364.0A
Other languages
English (en)
Other versions
CN111024064A (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.)
Southeast University
Original Assignee
Southeast University
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 Southeast University filed Critical Southeast University
Priority to CN201911163364.0A priority Critical patent/CN111024064B/zh
Publication of CN111024064A publication Critical patent/CN111024064A/zh
Application granted granted Critical
Publication of CN111024064B publication Critical patent/CN111024064B/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
    • 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
    • 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
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06F—ELECTRIC DIGITAL DATA PROCESSING
    • G06F2218/00—Aspects of pattern recognition specially adapted for signal processing
    • G06F2218/02—Preprocessing

Abstract

本发明公开了一种改进Sage‑Husa自适应滤波的SINS/DVL组合导航方法，其主要目的在于解决外部量测噪声未知情况下基于SINS/DVL的组合导航问题。本发明的主要步骤包括：空间模型建立、滤波状态更新、滤波增益计算及量测更新、量测噪声方差阵计算和可变滑动窗处理等。本发明可以解决外部量测噪声未知情况下的SINS/DVL组合导航精度差和稳定性差的问题。相比目前普遍采用的基于Sage‑Husa的SINS/DVL的组合导航方法，本方法能够准确估计外界量测噪声，进一步提高组合导航精度和系统稳定性。

Description

一种改进Sage-Husa自适应滤波的SINS/DVL组合导航方法
技术领域
本发明属于水下载体组合导航技术，具体涉及一种SINS/DVL组合导航方法。
背景技术
随着人类对海洋的不断探索，资源开发、海洋环境勘察、作业等需求也随之增加。水下航行器可以在深水中代替人类完成对海洋资源的探测和开发，为生产节省了大量的成本。水下航行器已经成为人类海洋开发的重要工具之一。随着人类对于海洋探索领域的逐渐深入，水下航行器所要完成的任务也越来越复杂，工作的时间也越来越长。现在水下航行器的发展面临着许多问题，其中高精度的自主导航定位是其面临的主要问题之一。自主导航系统可以长时间为水下航行器提供导航信息，保证水下航行器的正确行驶。
在组合导航系统中，卡尔曼滤波是受到广泛应用的信息融合算法。卡尔曼滤波的广泛应用是从上世纪六十年代开始的，该滤波器的本质是一个递推过程，是对数据的最优最小方差估计，该滤波器可把不同传感器之间的信息进行融合，实现最优估计。传统卡尔曼滤波为线性的，但实际中很多系统为非线性系统，扩展卡尔曼滤波随即被提出，该滤波不再要求系统为线性，在一定程度上弥补了传统卡尔曼滤波的不足。之后同样基于非线性系统的无迹卡尔曼滤波被提出，该算法通过设定采样点来实现对系统状态量的递推，对采样点进行U变换代替原有值。为了获得较好的滤波估计，以上方法均要求系统模型准确并且外部干扰信号为统计特性已知的不相关白噪声，否则不能保证滤波精度，甚至可能导致滤波发散。然而在实际的SINS/DVL(Strapdown Inertial Navigation System/Doppler VelocityLogger)组合导航系统中，由于导航器件的误差漂移、运载体机动产生的动态误差以及水中复杂环境的影响，系统模型很难完全准确，噪声的统计特性也具有不确定性。为解决这一问题，有学者提出了Sage-Husa自适应滤波算法，对噪声的统计特性进行实时估计修正。但是，在使用Sage-Husa过程中，外界量测噪声估计误差较大，从而导致系统精度下降或滤波器发散。
发明内容
发明目的：本发明的目的在于解决外界噪声未知情况下基于Sage-Husa自适应滤波的SINS/DVL组合导航精度低及滤波异常问题，提供一种改进Sage-Husa自适应滤波的SINS/DVL组合导航方法。
技术方案：本发明所述的一种改进Sage-Husa自适应滤波的SINS/DVL组合导航方法，包括以下步骤：
S1、建立SINS/DVL组合导航状态空间模型；
S2、根据IMU获得的载体角速度信息和加速度信息进行捷联惯性导航解算，获得导航系下载体的姿态、速度和位置估计值；
S3、根据S1的状态空间模型进行卡尔曼滤波更新，得到状态估计值，其中滤波量测更新时根据量测值和滤波器估计之间值的误差来自适应调整当前量测信息的权重，并利用序贯滤波方法对卡尔曼量测噪声方差阵限制大小，并对其进行可变滑动窗处理；
S4、将滤波器估计得到的状态估计值中的姿态误差、速度误差和位置误差反馈到捷联惯性导航系统输出的姿态、速度和位置估计值中进行修正处理，得到最终姿态、速度和位置信息。
进一步地，所述步骤S1包括：
S1-1、建立状态方程：
Figure BDA0002286751800000021
其中，F表示状态转移矩阵；X表示状态向量；G表示系统噪声矩阵；W表示系统噪声矢量；状态向量为17维，表示如下：
Figure BDA0002286751800000022
其中，[φx φy φz]T表示俯仰、横滚、航向姿态角误差；
Figure BDA0002286751800000023
表示东向、北向、天向速度误差；[δλ δL δh]T表示经度、纬度、高度误差；
Figure BDA0002286751800000024
表示加速度计零偏；[εx εy εz]T表示陀螺零偏；KD表示DVL刻度因子；bPS表示深度计零偏；
S1-2、建立量测方程：
Z＝HX+V
其中，Z为量测信息；H为量测方程转移矩阵，V为量测噪声，其中Z为：
Figure BDA0002286751800000025
其中，
Figure BDA0002286751800000034
表示DVL波束通道1的速度测量信息，
Figure BDA0002286751800000035
表示DVL波束通道2的速度测量信息，
Figure BDA0002286751800000036
表示DVL波束通道3的速度测量信息，
Figure BDA0002286751800000037
表示DVL波束通道4的速度测量信息；
Figure BDA0002286751800000038
表示DVL坐标系下捷联惯性导航系统输出的对应DVL通道1的速度信息，
Figure BDA0002286751800000039
表示DVL坐标系下捷联惯性导航系统输出的对应DVL通道2的速度信息，
Figure BDA00022867518000000310
表示DVL坐标系下捷联惯性导航系统输出的对应DVL通道3的速度信息，
Figure BDA00022867518000000311
表示DVL坐标系下捷联惯性导航系统输出的对应DVL通道4的速度信息，
Figure BDA00022867518000000312
表示压力传感器输出的深度信息；HPS表示捷联惯性导航系统输出的深度信息。
进一步地，所述步骤S3中滤波量测更新方程如下：
Figure BDA0002286751800000031
Figure BDA0002286751800000032
Pk＝(I-KkHk)Pk,k-1
其中，k表示k时刻；Kk表示卡尔曼滤波增益值，Pk,k-1表示一步预测均方误差阵；Xk,k-1表示状态一步预测；Rk表示量测噪声协方差阵，rk表示量测噪声均值；λ表示遗忘因子，由量测值和滤波器估计之间值的误差决定。
进一步地，所述遗忘因子采用自适应调整函数来计算，形式如下：
Figure BDA0002286751800000033
其中，λmin表示预设的遗忘因子最小值；E[]表示取平均值；εk表示量测值和估计值误差，其计算公式为：
Figure BDA00022867518000000313
进一步地，所述步骤S3中采用序贯滤波方法卡尔曼量测噪声方差阵进行大小限制的处理方法如下：
假设卡尔曼量测噪声方差阵
Figure BDA00022867518000000314
为对角线矩阵，采用序贯滤波在进行第i个标量序贯量测更新时，标量量测方程为：
Figure BDA0002286751800000041
简记
Figure BDA0002286751800000042
再设置如下限制条件：
Figure BDA0002286751800000043
其中βk表示重力扁率；
Figure BDA0002286751800000046
和
Figure BDA0002286751800000047
分别为预设的最小值和最大值；
通过上述处理计算，可以将量测噪声方差阵
Figure BDA0002286751800000048
限制在
Figure BDA0002286751800000049
和
Figure BDA00022867518000000410
之间。
进一步地，所述步骤S3中按下式对量测噪声方差阵
Figure BDA00022867518000000411
进行可变滑动窗处理：
Figure BDA0002286751800000044
其中，k表示k时刻；εk-j表示量测值和估计值误差，j表示滑动窗中第j个变量；m是可变的滑动窗口值；m的值范围满足以下条件：
Figure BDA0002286751800000045
其中，N为预设的窗口最大值。
有益效果：
(1)本发明将遗忘因子引入滤波算法中，提出一种遗忘因子自适应调节函数。提高了自适应滤波的精度。
(2)本发明将序贯滤波引入Sage-Husa算法中，同时通过设置条件限制测量噪声阵的变化，进一步提高了滤波的稳定性。
(3)本发明提出一种可变滑动窗方法，并应用到量测噪声方差阵的数据处理中，进一步提高了Sage-Husa自适应滤波对外界噪声的估计精度。
附图说明
图1为本发明所描述的改进Sage-Husa自适应滤波的SINS/DVL组合导航方法原理图；
图2为本发明所提出的可变滑动窗调节过程(m<N)；
图3为本发明所提出的可变滑动窗调节过程(m>N)；
图4为两种方法的位置误差对比曲线(三个方向的位置误差曲线)；
图5为两种方法的水平位置误差对比曲线。
具体实施方式
下面结合附图对本发明的技术方案作进一步说明。
针为提高外界噪声未知情况下组合导航的精度和稳定性，本发明从改进算法稳定性和噪声估计精度出发，提出一种改进Sage-Husa自适应滤波的SINS/DVL组合导航方法。该方法基于SINS/DVL组合导航模型，通过引入遗忘因子调节函数和可变滑动窗方法来提高系统的稳定性和精度。该方法能够在外界噪声未知情况下提高组合导航精度和稳定性。
参照图1，在一个实施例中，改进Sage-Husa自适应滤波的SINS/DVL组合导航方法包括以下步骤：
已知量：
IMU采集的光纤陀螺数据；
Figure BDA0002286751800000051
其中
Figure BDA0002286751800000052
表示x轴角速度信息；
Figure BDA0002286751800000053
表示y轴角速度信息；
Figure BDA0002286751800000054
表示z轴角速度信息。
IMU采集的加速度计数据
Figure BDA0002286751800000055
Figure BDA0002286751800000056
表示x轴加速度计数据；
Figure BDA0002286751800000057
表示y轴加速度计数据；
Figure BDA0002286751800000058
表示z轴加速度计数据。
DVL采集的四个波束通道的速度信息：
Figure BDA0002286751800000059
其中
Figure BDA00022867518000000510
表示DVL通道1的速度信息；
Figure BDA00022867518000000511
表示DVL通道2的速度信息；
Figure BDA00022867518000000512
表示DVL通道3的速度信息；
Figure BDA00022867518000000513
表示DVL通道4的速度信息。
输出量：
速度信息：
Figure BDA00022867518000000514
其中，
Figure BDA00022867518000000515
表示导航系下x方向的速度信息；
Figure BDA00022867518000000516
表示导航系下y方向的速度信息；
Figure BDA00022867518000000517
表示导航系下z方向的速度信息。
位置信息：λ,L,h。其中，λ表示地理经度值；L表示地理纬度值；h表示高度值。
姿态信息：θ,γ,ψ。其中，θ表示俯仰角信息；γ表示横滚角信息；ψ表示航向角信息。
步骤1：建立空间模型。
建立的状态方程如下：
Figure BDA0002286751800000061
其中，F表示状态转移矩阵；X表示状态向量；G表示系统噪声矩阵；W表示系统噪声矢量。状态向量为17维，可表示如下：
Figure BDA0002286751800000062
其中，[φx φy φz]T表示俯仰、横滚、航向姿态角误差；
Figure BDA0002286751800000066
表示东向、北向、天向速度误差；[δλ δL δh]T表示经度、纬度、高度误差；
Figure BDA0002286751800000067
表示加速度计零偏；[εx εy εz]T表示陀螺零偏；KD表示DVL刻度因子；bPS表示深度计零偏。
状态转移矩阵F可表示为：
Figure BDA0002286751800000063
其中，
Figure BDA0002286751800000064
Figure BDA0002286751800000065
Figure BDA0002286751800000071
Figure BDA0002286751800000072
Figure BDA0002286751800000073
Figure BDA0002286751800000074
其中：b表示载体坐标系；n表示导航坐标系；
Figure BDA0002286751800000076
表示b系到n系的姿态转移矩阵；ge表示地球坐标系下的重力值；β表示重力扁率；
Figure BDA0002286751800000077
表示加速度计噪声；
Figure BDA0002286751800000078
表示陀螺噪声；ωie表示地球自转角速率；VE表示东向速度；VN表示北向速度；VU表示天向速度；
Figure BDA0002286751800000079
表示导航系下天向加速度；
Figure BDA00022867518000000710
表示导航系下东向加速度；
Figure BDA00022867518000000711
表示导航系下北向加速度；L表示地理纬度；h表示高度；β1＝(1/8)(2βf+f2)，β2＝3.08×10-6s-2；f表示椭球(地球)扁率；RM、RN可表示如下：
Figure BDA0002286751800000075
步骤2：建立量测方程。
建立的量测方程如下：
Z＝HX+V
其中，Z为量测信息；H为量测方程转移矩阵，V为量测噪声。其中Z为：
Figure BDA0002286751800000081
其中，
Figure BDA0002286751800000084
表示DVL波束通道1的速度测量信息；
Figure BDA0002286751800000085
表示DVL波束通道2的速度测量信息；，
Figure BDA0002286751800000086
表示DVL波束通道3的速度测量信息；，
Figure BDA0002286751800000087
表示DVL波束通道4的速度测量信息；
Figure BDA0002286751800000088
表示DVL坐标系下捷联惯性导航系统输出的对应DVL通道1的速度信息，
Figure BDA0002286751800000089
表示DVL坐标系下捷联惯性导航系统输出的对应DVL通道2的速度信息，
Figure BDA00022867518000000810
表示DVL坐标系下捷联惯性导航系统输出的对应DVL通道3的速度信息，
Figure BDA00022867518000000811
表示DVL坐标系下捷联惯性导航系统输出的对应DVL通道4的速度信息，
Figure BDA00022867518000000812
表示压力传感器输出的深度信息；HPS表示捷联惯性导航系统输出的深度信息。量测方程中H阵如下：
Figure BDA0002286751800000082
其中，
Figure BDA0002286751800000083
其中，
Figure BDA00022867518000000813
表示载体系到DVL坐标系的姿态转移矩阵；I3×3表示3行3列单位阵；
Figure BDA00022867518000000814
表示失准角；
Figure BDA00022867518000000815
表示导航系到载体系的姿态转移矩阵；
Figure BDA00022867518000000816
表示导航系下捷联惯性导航的速度值。
步骤3：根据IMU获得的载体角速度信息和加速度信息进行捷联惯性导航解算，获得导航系下载体的姿态、速度和位置信息。其中姿态信息包括：俯仰角θ，横滚角γ和航向角ψ；速度信息包括：
Figure BDA00022867518000000817
表示导航系下x方向的速度信息，
Figure BDA00022867518000000818
表示导航系下y方向的速度信息，
Figure BDA00022867518000000819
表示导航系下z方向的速度信息；位置信息包括：λ表示地理经度值；L表示地理纬度值；h表示高度值。
步骤4：滤波状态更新。
将步骤1和步骤2中的状态方程和量测方程写成如下形式：
Figure BDA0002286751800000091
其中，k表示k时刻；系统噪声矩阵Wk-1和量测噪声矩阵Vk满足如下公式：
Figure BDA0002286751800000092
其中，qk表示系统噪声均值；rk表示量测噪声均值；Qk表示系统噪声协方差阵；Rk表示量测噪声协方差阵，Cov[]表示协方差；δkj为常值，可认为是1。
滤波状态更新如下：
Figure BDA0002286751800000093
Figure BDA0002286751800000094
其中，Pk,k-1表示一步预测均方误差阵；Xk,k-1表示状态一步预测；Pk-1表示前一时刻的均方误差阵。根据步骤1的状态方程，滤波状态更新以后可得到17维的状态估计值。
步骤5：计算滤波增益及更新量测。
传统的卡尔曼滤波增益计算公式如下：
Figure BDA0002286751800000095
其中，Kk表示卡尔曼滤波增益值；在上述公式基础上，引入遗忘因子λ，遗忘因子自适应调节函数如下：
Figure BDA0002286751800000096
Figure BDA0002286751800000097
其中，λmin表示λ的最小值，本发明中取0.75；εk表示新息值即量测值和估计值误差。由公式可知，当误差较大时，λ应该减小，进而减少当前量测信息的权重。当误差较小时，λ应该增大，进而增大当前量测信息的权重。因为遗忘因子的大小可以由外界量测值和滤波器估计值的误差决定。我们以误差均值E[e(n)]作为误差点，当小于这个值时，λ的值趋近于1，此时认为外界量测信息值比较准确，当大于这个值时，λ的值趋近于λmin，此时认为外界量测信息精度较差，并给其分配较小的权重。从自适应调节函数可以看出，遗忘因子λ由误差控制，可以保证误差在E[e(n)]附近摆动，变化缓慢，可以提高sage-husa自适应滤波的稳定性。滤波量测更新如下：
Figure BDA0002286751800000101
Figure BDA0002286751800000102
Pk＝(I-KkHk)Pk,k-1
其中Zk的计算可根据步骤2得到；量测噪声方差阵Rk对滤波的精度影响很大，下面针对该值的计算本发明提出一种新的方法。
步骤6：计算量测噪声方差阵。
传统的Sage-Husa自适应滤波中量测噪声方差阵
Figure BDA0002286751800000105
的计算如下：
Figure BDA0002286751800000103
上述公式可以看出，如果实际系统的量测噪声与理论建模值相比偏小，则
Figure BDA0002286751800000106
会比较小；如果系统噪声设置偏大，则
Figure BDA0002286751800000107
会比较大。以上两种情况都可能导致
Figure BDA0002286751800000108
从而容易使
Figure BDA0002286751800000109
失去正定性，引起滤波异常。本发明采用序贯滤波方法并对
Figure BDA00022867518000001010
对角线元素进行大小限制处理。
假设
Figure BDA00022867518000001011
为对角线矩阵，采用序贯滤波在进行第i个标量序贯量测更新时，标量量测方程为：
Figure BDA0002286751800000104
简记
Figure BDA0002286751800000111
再设置如下限制条件：
Figure BDA0002286751800000112
通过上述处理计算，可以将量测噪声方差阵
Figure BDA0002286751800000115
限制在
Figure BDA0002286751800000116
和
Figure BDA0002286751800000117
之间，从而具有更好的自适应能力和可靠性。
Figure BDA0002286751800000118
和
Figure BDA0002286751800000119
两个极值一般设置如下：最小值一般为初始R值的0.01倍，最大值一般为初始R值的100倍。
可变滑动窗处理。根据以上得到的
Figure BDA00022867518000001110
对其进行可变滑动窗处理，进一步提高该值的精度。
通过在Sage-Husa自适应滤波器中进行测量噪声均值处理，可以进一步提高测量噪声的估计精度。可变滑动窗处理公式如下：
Figure BDA0002286751800000113
其中m是可变的滑动窗口值。m的值范围满足以下条件：
Figure BDA0002286751800000114
其中，N为预设的窗口最大值。当检测到R值发生较大变化时m＝0，此时滑动窗口从0到N递增。当m的值大于N时，窗口m＝N。该方法能够很好地自适应R值的变化。窗口调节的两个过程如图2和3所示。图2是当m<N时的窗口调节过程。从图中可以看出，随着观测数据的增加窗口m也逐渐增大。图3是当m>N时的窗口滑动调节过程。从图中可以看出，随着观测数据的增加，移动窗口大小一直保持为N。
得到比较准确的
Figure BDA00022867518000001111
后，通过卡尔曼量测更新，便能进一步提高状态估计的精度。
步骤7：滤波器反馈处理。
将卡尔曼滤波器估计得到的17维状态估计值中的姿态误差、速度误差和位置误差反馈到捷联惯性导航系统输出的姿态、速度和位置信息中进行修正处理，最终得到相对准确的姿态、速度和位置信息。
下面通过一个实例验证本发明的效果。仿真参数如下：
IMU采样频率：200Hz；仿真时间：1400s；陀螺常值漂移：0.02°/h；加速度计零偏：100ug；初始偏移角度设置为0.02°，-0.02°和0.5°。DVL比例因子设置为0.002。DVL的输出频率设置为1Hz。车辆运动的起始位置设置为纬度32.056°N和经度118.794°E。
载体的运动模型为：设载体运动的俯仰角为θ，横滚角为γ，航向角为ψ。
Figure BDA0002286751800000121
其中，摇摆幅度依次是：θm＝3°，γm＝4°，ψm＝3°；初始相位依次是：θ0＝0°，γ0＝0°，ψ0＝0°。
从图4到图5可以看出，本发明的位置精度要优于传统的Sage-Husa自适应滤波方法。

Claims (3)

1.一种改进Sage-Husa自适应滤波的SINS/DVL组合导航方法，其特征在于，包括以下步骤：
S1、建立SINS/DVL组合导航状态空间模型，包括：
S1-1、建立状态方程：
Figure FDA0003201793710000011
其中，F表示状态转移矩阵；X表示状态向量；G表示系统噪声矩阵；W表示系统噪声矢量；状态向量为17维，表示如下：
Figure FDA0003201793710000012
其中，[φx φy φz]T表示俯仰、横滚、航向姿态角误差；
Figure FDA0003201793710000013
表示东向、北向、天向速度误差；[δλ δL δh]T表示经度、纬度、高度误差；
Figure FDA0003201793710000014
表示加速度计零偏；[εx εy εz]T表示陀螺零偏；KD表示DVL刻度因子；bPS表示深度计零偏；
S1-2、建立量测方程：
Z＝HX+V
其中，Z为量测信息；H为量测方程转移矩阵，V为量测噪声，其中Z为：
Figure FDA0003201793710000015
其中，
Figure FDA0003201793710000016
表示DVL波束通道1的速度测量信息，
Figure FDA0003201793710000017
表示DVL波束通道2的速度测量信息，
Figure FDA0003201793710000018
表示DVL波束通道3的速度测量信息，
Figure FDA0003201793710000019
表示DVL波束通道4的速度测量信息；
Figure FDA00032017937100000110
表示DVL坐标系下捷联惯性导航系统输出的对应DVL通道1的速度信息，
Figure FDA00032017937100000111
表示DVL坐标系下捷联惯性导航系统输出的对应DVL通道2的速度信息，
Figure FDA00032017937100000112
表示DVL坐标系下捷联惯性导航系统输出的对应DVL通道3的速度信息，
Figure FDA00032017937100000113
表示DVL坐标系下捷联惯性导航系统输出的对应DVL通道4的速度信息，
Figure FDA00032017937100000114
表示压力传感器输出的深度信息；HPS表示捷联惯性导航系统输出的深度信息；
S2、根据IMU获得的载体角速度信息和加速度信息进行捷联惯性导航解算，获得导航系下载体的姿态、速度和位置估计值；
S3、根据S1的状态空间模型进行卡尔曼滤波更新，得到状态估计值，其中滤波量测更新时根据量测值和滤波器估计值之间的误差来自适应调整当前量测信息的权重，并利用序贯滤波方法对卡尔曼量测噪声协方差阵限制大小，并对其进行可变滑动窗处理，其中滤波量测更新方程如下：
Figure FDA0003201793710000021
Figure FDA0003201793710000022
Pk＝(I-KkHk)Pk,k-1
其中，k表示k时刻；Kk表示卡尔曼滤波增益值，Pk,k-1表示一步预测均方误差阵；Rk表示量测噪声协方差阵，rk表示量测噪声均值；λ表示遗忘因子，由量测值和滤波器估计之间值的误差决定，形式如下：
Figure FDA0003201793710000023
其中，λmin表示预设的遗忘因子最小值；E[]表示取平均值；εk表示量测值和估计值误差，其计算公式为：
Figure FDA0003201793710000024
S4、将滤波器估计得到的状态估计值中的姿态误差、速度误差和位置误差反馈到捷联惯性导航系统输出的姿态、速度和位置估计值中进行修正处理，得到最终姿态、速度和位置信息。
2.根据权利要求1所述的SINS/DVL组合导航方法，其特征在于，所述步骤S3中采用序贯滤波方法对卡尔曼量测噪声协方差阵进行大小限制的处理方法如下：
假设卡尔曼量测噪声协方差阵
Figure FDA0003201793710000025
为对角线矩阵，采用序贯滤波在进行第i个标量序贯量测更新时，标量量测方程为：
Figure FDA0003201793710000026
简记
Figure FDA0003201793710000031
再设置如下限制条件：
Figure FDA0003201793710000032
其中βk表示重力扁率；
Figure FDA0003201793710000033
和
Figure FDA0003201793710000034
分别为预设的最小值和最大值；
通过上述处理计算，将量测噪声协方差阵
Figure FDA0003201793710000035
限制在
Figure FDA0003201793710000036
和
Figure FDA0003201793710000037
之间。
3.根据权利要求1所述的SINS/DVL组合导航方法，其特征在于，所述步骤S3中按下式对量测噪声协方差阵
Figure FDA0003201793710000038
进行可变滑动窗处理：
Figure FDA0003201793710000039
其中，k表示k时刻；εk-j表示量测值和估计值误差，j表示滑动窗中第j个变量；m是可变的滑动窗口值；m的值范围满足以下条件：
Figure FDA00032017937100000310
其中，N为预设的窗口最大值。
CN201911163364.0A 2019-11-25 2019-11-25 一种改进Sage-Husa自适应滤波的SINS/DVL组合导航方法 Active CN111024064B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN201911163364.0A CN111024064B (zh) 2019-11-25 2019-11-25 一种改进Sage-Husa自适应滤波的SINS/DVL组合导航方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN201911163364.0A CN111024064B (zh) 2019-11-25 2019-11-25 一种改进Sage-Husa自适应滤波的SINS/DVL组合导航方法

Publications (2)

Publication Number Publication Date
CN111024064A CN111024064A (zh) 2020-04-17
CN111024064B true CN111024064B (zh) 2021-10-19

Family

ID=70207237

Family Applications (1)

Application Number Title Priority Date Filing Date
CN201911163364.0A Active CN111024064B (zh) 2019-11-25 2019-11-25 一种改进Sage-Husa自适应滤波的SINS/DVL组合导航方法

Country Status (1)

Country Link
CN (1) CN111024064B (zh)

Families Citing this family (12)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN111504324B (zh) * 2020-04-27 2022-07-26 西北工业大学 一种噪声自适应滤波的水下组合导航方法
CN112254718B (zh) 2020-08-04 2024-04-09 东南大学 一种运动约束辅助的基于改进Sage-Husa自适应滤波的水下组合导航方法
CN112507281B (zh) * 2020-11-19 2024-01-02 东南大学 一种基于双状态多因子抗差估计sins/dvl紧组合系统的方法
CN112710304B (zh) * 2020-12-17 2022-12-13 西北工业大学 一种基于自适应滤波的水下自主航行器导航方法
CN112733284B (zh) * 2020-12-22 2022-12-27 长春工程学院 一种汽车线束波纹管波峰切割定位信息融合方法
CN113175926B (zh) * 2021-04-21 2022-06-21 哈尔滨工程大学 一种基于运动状态监测的自适应水平姿态测量方法
CN113916222B (zh) * 2021-09-15 2023-10-13 北京自动化控制设备研究所 基于卡尔曼滤波估计方差约束的组合导航方法
CN113984054A (zh) * 2021-09-17 2022-01-28 兰州交通大学 基于信息异常检测的改进Sage-Husa自适应融合滤波方法及多源信息融合设备
CN114111843B (zh) * 2021-11-24 2022-09-02 东南大学 一种捷联惯导系统最优动基座初始对准方法
CN115855049B (zh) * 2023-02-07 2023-05-12 河海大学 基于粒子群优化鲁棒滤波的sins/dvl导航方法
CN115790669B (zh) * 2023-02-08 2023-04-28 河海大学 基于最大熵平滑变结构滤波的dvl误差自适应标定方法
CN116165690B (zh) * 2023-04-21 2023-07-07 山西省娄烦县皇姑山矿业有限责任公司 一种基于gnss/ins的双自适应因子组合导航定位方法

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 水中航走体の自動動作確認方法及び装置
CN104764467A (zh) * 2015-04-08 2015-07-08 南京航空航天大学 空天飞行器惯性传感器误差在线自适应标定方法
CN107621264A (zh) * 2017-09-30 2018-01-23 华南理工大学 车载微惯性/卫星组合导航系统的自适应卡尔曼滤波方法
CN108844540A (zh) * 2018-09-11 2018-11-20 北京机械设备研究所 一种结合协方差和Sage-Husa滤波技术的自适应滤波方法
CN110146075A (zh) * 2019-06-06 2019-08-20 哈尔滨工业大学（威海） 一种增益补偿自适应滤波的sins/dvl组合定位方法

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 水中航走体の自動動作確認方法及び装置
CN104764467A (zh) * 2015-04-08 2015-07-08 南京航空航天大学 空天飞行器惯性传感器误差在线自适应标定方法
CN107621264A (zh) * 2017-09-30 2018-01-23 华南理工大学 车载微惯性/卫星组合导航系统的自适应卡尔曼滤波方法
CN108844540A (zh) * 2018-09-11 2018-11-20 北京机械设备研究所 一种结合协方差和Sage-Husa滤波技术的自适应滤波方法
CN110146075A (zh) * 2019-06-06 2019-08-20 哈尔滨工业大学（威海） 一种增益补偿自适应滤波的sins/dvl组合定位方法

Non-Patent Citations (1)

* Cited by examiner, † Cited by third party
Title
基于自适应滤波的SINS/DVL组合导航系统;徐晓苏等;《华中科技大学学报（自然科学版）》;20150331;第43卷(第3期);第95-99页 *

Also Published As

Publication number Publication date
CN111024064A (zh) 2020-04-17

Similar Documents

Publication Publication Date Title
CN111024064B (zh) 一种改进Sage-Husa自适应滤波的SINS/DVL组合导航方法
CN109211276B (zh) 基于gpr与改进的srckf的sins初始对准方法
CN110398257B (zh) Gps辅助的sins系统快速动基座初始对准方法
CN111156987B (zh) 基于残差补偿多速率ckf的惯性/天文组合导航方法
CN107270893B (zh) 面向不动产测量的杆臂、时间不同步误差估计与补偿方法
CN112097763B (zh) 一种基于mems imu/磁力计/dvl组合的水下运载体组合导航方法
CN112505737B (zh) 一种gnss/ins组合导航方法
CN110567454B (zh) 一种复杂环境下sins/dvl紧组合导航方法
CN109425339B (zh) 一种基于惯性技术的考虑杆臂效应的舰船升沉误差补偿方法
CN109945859B (zh) 一种自适应h∞滤波的运动学约束捷联惯性导航方法
CN110954102B (zh) 用于机器人定位的磁力计辅助惯性导航系统及方法
CN113063429B (zh) 一种自适应车载组合导航定位方法
CN103389095A (zh) 一种用于捷联惯性/多普勒组合导航系统的自适应滤波方法
CN109612460B (zh) 一种基于静止修正的垂线偏差测量方法
CN111750865B (zh) 一种用于双功能深海无人潜器导航系统的自适应滤波导航方法
CN111024074B (zh) 一种基于递推最小二乘参数辨识的惯导速度误差确定方法
CN112857398B (zh) 一种系泊状态下舰船的快速初始对准方法和装置
CN110595434B (zh) 基于mems传感器的四元数融合姿态估计方法
CN114136310B (zh) 一种惯性导航系统误差自主抑制系统及方法
CN113175926B (zh) 一种基于运动状态监测的自适应水平姿态测量方法
CN109084755B (zh) 一种基于重力视速度与参数辨识的加速度计零偏估计方法
CN110375773B (zh) Mems惯导系统姿态初始化方法
CN111220151B (zh) 载体系下考虑温度模型的惯性和里程计组合导航方法
CN110388942B (zh) 一种基于角度和速度增量的车载姿态精对准系统
CN114111840B (zh) 一种基于组合导航的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
GR01 Patent grant
GR01 Patent grant