CN110146075B - 一种增益补偿自适应滤波的sins/dvl组合定位方法 - Google Patents

一种增益补偿自适应滤波的sins/dvl组合定位方法 Download PDF

Info

Publication number
CN110146075B
CN110146075B CN201910488837.8A CN201910488837A CN110146075B CN 110146075 B CN110146075 B CN 110146075B CN 201910488837 A CN201910488837 A CN 201910488837A CN 110146075 B CN110146075 B CN 110146075B
Authority
CN
China
Prior art keywords
sins
error
state
dvl
value
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
CN201910488837.8A
Other languages
English (en)
Other versions
CN110146075A (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.)
Harbin Institute of Technology Weihai
Original Assignee
Harbin Institute of Technology Weihai
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 Harbin Institute of Technology Weihai filed Critical Harbin Institute of Technology Weihai
Priority to CN201910488837.8A priority Critical patent/CN110146075B/zh
Publication of CN110146075A publication Critical patent/CN110146075A/zh
Application granted granted Critical
Publication of CN110146075B publication Critical patent/CN110146075B/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
    • 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
    • G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
    • G01S15/00—Systems using the reflection or reradiation of acoustic waves, e.g. sonar systems
    • G01S15/02—Systems using the reflection or reradiation of acoustic waves, e.g. sonar systems using reflection of acoustic waves
    • G01S15/06—Systems determining the position data of a target
    • G01S15/42—Simultaneous measurement of distance and other co-ordinates
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
    • G01S15/00—Systems using the reflection or reradiation of acoustic waves, e.g. sonar systems
    • G01S15/86—Combinations of sonar systems with lidar systems; Combinations of sonar systems with systems not using wave reflection

Landscapes

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

Abstract

一种增益补偿自适应滤波的SINS/DVL组合定位方法，涉及高精度SINS/DVL组合定位。本发明是为了有效解决由于传统SINS/DVL组合导航的滤波算法灵活性不足的影响，导致定位精度较低的问题。本发明所述的一种增益补偿自适应滤波的SINS/DVL组合定位方法，首先基于捷联惯导系统和多普勒计程仪传感器的信息，获取相应状态初值和观测值；然后建立基于组合导航误差模型对应的系统方程和观测方程，用增益补偿改进自适应滤波算法对误差进行校正，并获取校正后目标的速度和位置误差信息；最后将获得的误差信息和捷联惯导与多普勒计程仪的观测信息进行融合，得出高精度的定位结果。

Description

一种增益补偿自适应滤波的SINS/DVL组合定位方法
技术领域
本发明涉及高精度的水下定位技术
背景技术
在实际的水下定位过程中，由于水下环境的复杂多变，使得捷联惯导与多普勒计程仪组合定位(SINS/DVL)系统的系统噪声和量测噪声的统计特性往往具有一定的时变性。为了对卡尔曼滤波算法进行合理的改进使其对噪声统计特性的变化具有一定的自适应能力，从而来进一步提高滤波精度，达到高精度的定位。本发明采用滤波增益补偿方法对改进自适应滤波算法进行优化，采用优化后的滤波算法对定位系统误差发散的情况进行抑制，从而实现提高定位精度的目的。
发明内容
本发明的目的是为了解决由于复杂水下环境下SINS/DVL组合定位方法系统噪声时变特性，导致定位精度较低的问题，提出一种增益补偿自适应滤波的SINS/DVL组合定位方法。
本发明所述的一种增益补偿自适应滤波的SINS/DVL组合定位方法包括以下步骤：
步骤一、系统建立SINS/DVL组合定位误差模型状态变量X＝[δvE δvN α β γ δL δλ εE εN εU δvd δΔ δC]T，其中δvE和δvN为东、北向速度误差，α、β、γ为平台失准角，δL和δλ为经度误差和纬度误差，εE、εN、εU为东、北、天方向陀螺漂移，δvd为多普勒测量速度偏移误差，δΔ为偏流角误差，δC为刻度系数误差；
步骤二、系统通过惯性导航组件中的陀螺仪敏感载体的三轴角速度信息和加速度计测得三轴(东、北、天)加速度信息aE、aN、aH，由多普勒计程仪获得速度vd、偏流角Δ和姿态等导航信息；
步骤三、系统将这三个方向的加速度分量aE、aN、aH带入公式(1)分别进行积分，即可得到载体沿这三个方向的速度分量vE、vN、vH。再将三个速度分量带入公式(2)进行积分得到载体的经度L、纬度λ和深度d，其中R为地球半径，t0为运动初始时刻，tk为运动过程中某时刻；
Figure BDA0002086270680000021
Figure BDA0002086270680000022
系统将这些信息带入公式(3)～(15)中求出误差模型对应的各状态变量X＝[δvEδvN α β γ δL δλ εE εN εU δvd δΔ δC]T：
其中，Ω为陀螺振动频率为常量，g为重力加速度为常量，ΔaN，ΔaE为加速度误差；
东向和北向速度误差公式：
Figure BDA0002086270680000023
Figure BDA0002086270680000032
平台失准角：
Figure BDA0002086270680000033
Figure BDA0002086270680000034
Figure BDA0002086270680000035
位置误差：
Figure BDA0002086270680000036
Figure BDA0002086270680000037
陀螺漂移：
εE＝-βEεE+wE (10)
εN＝-βNεN+wN (11)
εU＝-βUεU+wU (12)
Figure BDA0002086270680000038
为陀螺仪在东、北、天方向的误差相关时间，wE、
wN、wU为高斯白噪声；
多普勒计程仪的速度、偏流角和刻度误差：
δvd＝-βdδvd+wd (13)
δΔ＝-βΔδΔ+wΔ (14)
δC＝0 (15)
其中
Figure BDA0002086270680000039
为速度偏移误差和偏流角误差的相关时间，wd、wΔ为高斯白噪声；
步骤四、系统建立系统状态方程和系统量测方程，如公式(16)和式(21)所示：
状态方程描述为：
Figure BDA0002086270680000041
式中：
WSINS＝[0 0 aE aN 0 0 0 wE wN wU wd wΔ 0]T (17)
根据式(3)～(15)建立状态传递阵FSINS/DVL
Figure BDA0002086270680000042
其中：
Figure BDA0002086270680000043
有：
Figure BDA0002086270680000044
Figure BDA0002086270680000045
Figure BDA0002086270680000046
Figure BDA0002086270680000047
Figure BDA0002086270680000051
Figure BDA0002086270680000052
对于F6×6有：
Figure BDA0002086270680000053
系统量测方程为：
Figure BDA0002086270680000054
对于HSINS/DVL和VSINS/DVL有：
Figure BDA0002086270680000055
VSINS/DVL＝[vE vN]T (23)
这里系统噪声方差阵：
Figure BDA0002086270680000056
量测噪声方差阵：
Figure BDA0002086270680000057
步骤五、系统建立滤波增益改进自适应滤波算法，并对滤波增益改进自适应卡尔曼滤波算法系统的状态方程和量测方程进行描述，如公式(26)、(27)：
状态方程描述为：
Xk＝FkXk-1+GWk (26)
系统量测方程为：
Figure BDA0002086270680000061
其中，Xk为k时刻的状态变量值；Xk-1为k-1时刻的状态变量；F为作用在Xk-1上的状态变换系数；Wk为k时刻的状态噪声值；G为作用在上Wk的系数；Hk为观测模型系数，把真实状态空间映射成观测空间；Zk为k时刻的观测值，由捷联惯导的东向、北向速度误差和多普勒计程仪东向北向速度误差的差构成；Vk为k时刻的观测噪声值；
步骤六、将Fk，Gk，Wk，Hk，Vk，以及初始的状态变量X带入系统的状态方程和量测方程进行一步预测，求出k时刻预测的量测值
Figure BDA0002086270680000062
和
Figure BDA0002086270680000063
Figure BDA0002086270680000064
为k时刻量测值的误差；
步骤七、系统根据稳定性判据，判断
Figure BDA0002086270680000065
若成立则系统发散，执行步骤八，采用增益补偿自适应滤波；若不成立则系统收敛，执行步骤九，采用增益补偿强跟踪卡尔曼滤波；
步骤八、系统将对应参数及量代入公式(28)～(37)中进行计算，
Figure BDA0002086270680000066
Figure BDA0002086270680000067
Figure BDA0002086270680000068
Figure BDA0002086270680000069
Figure BDA00020862706800000610
Figure BDA0002086270680000071
其中，
Figure BDA0002086270680000072
为k时刻的状态变量值；
Figure BDA0002086270680000073
为由k-1时刻的状态变量所得到的k时刻的预测值；Fk,k-1为作用在
Figure BDA0002086270680000074
上的状态变换系数；Kk为作用在
Figure BDA0002086270680000075
上的滤波增益补偿卡尔曼系数；Hk为观测模型系数，把真实状态空间映射成观测空间；Zk为k时刻的观测值，k时刻预测的量测值为
Figure BDA0002086270680000076
Pk,k-1为先验估计误差协方差值；Pk为后验估计误差协方差值；在(28)～(33)式子中
Figure BDA0002086270680000077
分别由时变噪声估值方程所计算得式(34)～(37)：
Figure BDA0002086270680000078
Figure BDA0002086270680000079
Figure BDA00020862706800000710
Figure BDA00020862706800000711
实现增益补偿自适应滤波，得到下一时刻状态变量估计值
Figure BDA00020862706800000712
执行步骤十。
步骤九、系统将对应参数及参量代入公式(38)～(46)中进行计算：
Figure BDA00020862706800000713
Figure BDA00020862706800000714
Figure BDA00020862706800000715
λk+1＝diag[λ1(k+1),λ2(k+1),…,λm(k+1)] (41)
Figure BDA0002086270680000081
Figure BDA0002086270680000082
Figure BDA0002086270680000083
Figure BDA0002086270680000084
Figure BDA0002086270680000085
实现增益补偿强跟踪卡尔曼滤波，得到下一时刻状态变量估计值
Figure BDA0002086270680000086
执行步骤十；
步骤十、系统更新变换k＝k+1，将得到的状态变量估计值
Figure BDA0002086270680000087
记为新的状态变量值
Figure BDA0002086270680000088
步骤十一、系统判断k是否等于n，如果是，则执行步骤十二，否则执行步骤六；
步骤十二、系统得到生成的系统状态变量序列
Figure BDA0002086270680000089
Figure BDA00020862706800000810
输出的最终结果
Figure BDA00020862706800000811
即为当前时刻经过滤波校正后的状态结果。包含
Figure BDA00020862706800000812
是误差量的校正结果，再结合当前时刻SINS/DVL组合定位系统的观测值Yn＝[vEn vNn Lnλn vdn]T，得到校正后的航行器东向、北向速度
Figure BDA00020862706800000813
和经纬度信息
Figure BDA00020862706800000814
以及多普勒计程仪的速度信息
Figure BDA00020862706800000815
其中，
Figure BDA00020862706800000816
步骤十三、系统判断SINS/DVL组合定位任务是否完成，如果是，执行步骤十四，否则执行步骤二；
步骤十四、系统结束增益补偿自适应滤波的SINS/DVL组合定位任务。
附图说明
图1为一种增益补偿自适应滤波的SINS/DVL组合定位方法的流程图。
具体实施方式
具体实施方式一
步骤一、系统建立SINS/DVL组合定位误差模型状态变量X＝[δvE δvN α β γ δL δλ εE εN εU δvd δΔ δC]T，其中δvE和δvN为东、北向速度误差，α、β、γ为平台失准角，δL和δλ为经度误差和纬度误差，εE、εN、εU为东、北、天方向陀螺漂移，δvd为多普勒测量速度偏移误差，δΔ为偏流角误差，δC为刻度系数误差；
步骤二、系统通过惯性导航组件中的陀螺仪敏感载体的三轴角速度信息和加速度计测得三轴(东、北、天)加速度信息aE、aN、aH，由多普勒计程仪获得速度vd、偏流角Δ和姿态等导航信息；
步骤三、系统将这三个方向的加速度分量aE、aN、aH带入公式(1)分别进行积分，即可得到载体沿这三个方向的速度分量vE、vN、vH。再将三个速度分量带入公式(2)进行积分得到载体的经度L、纬度λ和深度d，其中R为地球半径，t0为运动初始时刻，tk为运动过程中某时刻；
Figure BDA0002086270680000091
Figure BDA0002086270680000101
系统将这些信息带入公式(3)～(15)中求出误差模型对应的各状态变量X＝[δvEδvN α β γ δL δλ εE εN εU δvd δΔ δC]T：
其中，Ω为陀螺振动频率为常量，g为重力加速度为常量，ΔaN，ΔaE为加速度误差；
东向和北向速度误差公式：
Figure BDA0002086270680000102
Figure BDA0002086270680000103
平台失准角：
Figure BDA0002086270680000104
Figure BDA0002086270680000105
Figure BDA0002086270680000106
位置误差：
Figure BDA0002086270680000107
Figure BDA0002086270680000108
陀螺漂移：
εE＝-βEεE+wE (10)
εN＝-βNεN+wN (11)
εU＝-βUεU+wU (12)
Figure BDA0002086270680000111
为陀螺仪在东、北、天方向的误差相关时间，wE、
wN、wU为高斯白噪声；
多普勒计程仪的速度、偏流角和刻度误差：
δvd＝-βdδvd+wd (13)
δΔ＝-βΔδΔ+wΔ (14)
δC＝0 (15)
其中
Figure BDA0002086270680000112
为速度偏移误差和偏流角误差的相关时间，wd、wΔ为高斯白噪声；
步骤四、系统建立系统状态方程和系统量测方程，如公式(16)和式(21)所示：
状态方程描述为：
Figure BDA0002086270680000113
式中：
WSINS＝[0 0 aE aN 0 0 0 wE wN wU wd wΔ 0]T (17)
根据式(3)～(15)建立状态传递阵FSINS/DVL
Figure BDA0002086270680000114
其中：
Figure BDA0002086270680000121
有：
Figure BDA0002086270680000122
Figure BDA0002086270680000123
Figure BDA0002086270680000124
Figure BDA0002086270680000125
Figure BDA0002086270680000126
Figure BDA0002086270680000127
对于F6×6有：
Figure BDA0002086270680000128
系统量测方程为：
Figure BDA0002086270680000129
对于HSINS/DVL和VSINS/DVL有：
Figure BDA0002086270680000131
VSINS/DVL＝[vE vN]T (23)
这里系统噪声方差阵：
Figure BDA0002086270680000132
量测噪声方差阵：
Figure BDA0002086270680000133
步骤五、系统建立滤波增益改进自适应滤波算法，并对滤波增益改进自适应卡尔曼滤波算法系统的状态方程和量测方程进行描述，如公式(26)、(27)：
状态方程描述为：
Xk＝FkXk-1+GWk (26)
系统量测方程为：
Figure BDA0002086270680000134
其中，Xk为k时刻的状态变量值；Xk-1为k-1时刻的状态变量；F为作用在Xk-1上的状态变换系数；Wk为k时刻的状态噪声值；G为作用在上Wk的系数；Hk为观测模型系数，把真实状态空间映射成观测空间；Zk为k时刻的观测值，由捷联惯导的东向、北向速度误差和多普勒计程仪东向北向速度误差的差构成；Vk为k时刻的观测噪声值；
步骤六、将Fk，Gk，Wk，Hk，Vk，以及初始的状态变量X带入系统的状态方程和量测方程进行一步预测，求出k时刻预测的量测值
Figure BDA0002086270680000141
和
Figure BDA0002086270680000142
Figure BDA0002086270680000143
为k时刻量测值的误差；
步骤七、系统根据稳定性判据，判断
Figure BDA0002086270680000144
若成立则系统发散，执行步骤八，采用增益补偿自适应滤波；若不成立则系统收敛，执行步骤九，采用增益补偿强跟踪卡尔曼滤波；
步骤八、系统将对应参数及量代入公式(28)～(37)中进行计算，
Figure BDA0002086270680000145
Figure BDA0002086270680000146
Figure BDA0002086270680000147
Figure BDA0002086270680000148
Figure BDA0002086270680000149
Figure BDA00020862706800001410
其中，
Figure BDA00020862706800001411
为k时刻的状态变量值；
Figure BDA00020862706800001412
为由k-1时刻的状态变量所得到的k时刻的预测值；Fk,k-1为作用在
Figure BDA00020862706800001413
上的状态变换系数；Kk为作用在
Figure BDA00020862706800001414
上的滤波增益补偿卡尔曼系数；Hk为观测模型系数，把真实状态空间映射成观测空间；Zk为k时刻的观测值，k时刻预测的量测值为
Figure BDA00020862706800001415
Pk,k-1为先验估计误差协方差值；Pk为后验估计误差协方差值；在(28)～(33)式子中
Figure BDA00020862706800001416
分别由时变噪声估值方程所计算得式(34)～(37)：
Figure BDA00020862706800001417
Figure BDA00020862706800001418
Figure BDA00020862706800001419
Figure BDA0002086270680000151
实现增益补偿自适应滤波，得到下一时刻状态变量估计值
Figure BDA0002086270680000152
执行步骤十。
步骤九、系统将对应参数及参量代入公式(38)～(46)中进行计算：
Figure BDA0002086270680000153
Figure BDA0002086270680000154
Figure BDA0002086270680000155
λk+1＝diag[λ1(k+1),λ2(k+1),…,λm(k+1)] (41)
Figure BDA0002086270680000156
Figure BDA0002086270680000157
Figure BDA0002086270680000158
Figure BDA0002086270680000159
Figure BDA00020862706800001510
实现增益补偿强跟踪卡尔曼滤波，得到下一时刻状态变量估计值
Figure BDA00020862706800001511
执行步骤十；
步骤十、系统更新变换k＝k+1，将得到的状态变量估计值
Figure BDA00020862706800001512
记为新的状态变量值
Figure BDA00020862706800001513
步骤十一、系统判断k是否等于n，如果是，则执行步骤十二，否则执行步骤六；
步骤十二、系统得到生成的系统状态变量序列
Figure BDA0002086270680000161
Figure BDA0002086270680000162
输出的最终结果
Figure BDA0002086270680000163
即为当前时刻经过滤波校正后的状态结果。包含
Figure BDA0002086270680000164
是误差量的校正结果，再结合当前时刻SINS/DVL组合定位系统的观测值Yn＝[vEn vNn Lnλn vdn]T，得到校正后的航行器东向、北向速度
Figure BDA0002086270680000165
和经纬度信息
Figure BDA0002086270680000166
以及多普勒计程仪的速度信息
Figure BDA0002086270680000167
其中，
Figure BDA0002086270680000168
步骤十三、系统判断SINS/DVL组合定位任务是否完成，如果是，执行步骤十四，否则执行步骤二；
步骤十四、系统结束增益补偿自适应滤波的SINS/DVL组合定位任务。
具体实施方案二，本实施方式是对具体实施方式一所述的一种增益补偿自适应滤波的SINS/DVL组合定位方法的步骤七进一步说明，其特征在于判断滤波是否发散的判据，根据滤波估计的误差与期望误差的大小关系来判断。
具体实施方案三，本实施方式是对具体实施方式一所述的一种增益补偿自适应滤波的SINS/DVL组合定位方法的做出进一步说明，其特征在于采用的滤波增益补偿算法，针对水下海流的外部干扰或者水下航行器的航速航向发生突变，会使得滤波过程得估计值在跟踪水下航行器得真实状态时有轻微的延迟，从而使得导航精度和质量下降的问题，可以实现快速对系统状态进行实时跟踪并减小误差的积累速度。
具体实施方案四，本实施方式是对具体实施方式一所述的一种增益补偿自适应滤波的SINS/DVL组合定位方法做出进一步说明，其特征在于采用改进自适应卡尔曼滤波，既可以获得既能保证滤波精度，又可以有效阻止滤波过程发散。
具体实施方案五，本实施方式是对具体实施方式一所述的一种增益补偿自适应滤波的SINS/DVL组合定位方法做出进一步说明，其特征在于采用增益补偿改进自适应滤波算法，将增益补偿算法和改进自适应卡尔曼算法融合，融合算法的精确度相比于融合之前的算法要高出一个数量级。

Claims (5)

1.一种增益补偿自适应滤波的SINS/DVL组合定位方法，其特征在于所述方法包括以下步骤：
步骤一、系统建立SINS/DVL组合定位误差模型状态变量X＝[δvE δvN α β γ δL δλ εEεN εU δvd δΔ δC]T，其中δvE和δvN为东、北向速度误差，α、β、γ为平台失准角，δL和δλ为经度误差和纬度误差，εE、εN、εU为东、北、天方向陀螺漂移，δvd为多普勒测量速度偏移误差，δΔ为偏流角误差，δC为刻度系数误差；
步骤二、系统通过惯性导航组件中的陀螺仪敏感载体的三轴角速度信息和加速度计测得三轴(东、北、天)加速度信息aE、aN、aH，由多普勒计程仪获得速度vd、偏流角Δ和姿态等导航信息；
步骤三、系统将这三个方向的加速度分量aE、aN、aH带入公式(1)分别进行积分，即可得到载体沿这三个方向的速度分量vE、vN、vH；再将三个速度分量带入公式(2)进行积分得到载体的经度L、纬度λ和深度d，其中R为地球半径，t0为运动初始时刻，tk为运动过程中某时刻；
Figure FDA0002086270670000011
Figure FDA0002086270670000012
系统将这些信息带入公式(3)～(15)中求出误差模型对应的各状态变量X＝[δvE δvN αβ γ δL δλ εE εN εU δvd δΔ δC]T：
其中，Ω为陀螺振动频率为常量，g为重力加速度为常量，ΔaN，ΔaE为加速度误差；
东向和北向速度误差公式：
Figure FDA0002086270670000021
Figure FDA0002086270670000022
平台失准角：
Figure FDA0002086270670000023
Figure FDA0002086270670000024
Figure FDA0002086270670000025
位置误差：
Figure FDA0002086270670000026
Figure FDA0002086270670000027
陀螺漂移：
εE＝-βEεE+wE (10)
εN＝-βNεN+wN (11)
εU＝-βUεU+wU (12)
Figure FDA0002086270670000028
为陀螺仪在东、北、天方向的误差相关时间，wE、wN、wU为高斯白噪声；
多普勒计程仪的速度、偏流角和刻度误差：
δvd＝-βdδvd+wd (13)
δΔ＝-βΔδΔ+wΔ (14)
δC＝0 (15)
其中
Figure FDA0002086270670000031
为速度偏移误差和偏流角误差的相关时间，wd、wΔ为高斯白噪声；
步骤四、系统建立系统状态方程和系统量测方程，如公式(16)和式(21)所示：
状态方程描述为：
Figure FDA0002086270670000032
式中：
WSINS＝[0 0 aE aN 0 0 0 wE wN wU wd wΔ 0]T (17)
根据式(3)～(15)建立状态传递阵FSINS/DVL
Figure FDA0002086270670000033
其中：
Figure FDA0002086270670000034
有：
Figure FDA0002086270670000041
Figure FDA0002086270670000042
Figure FDA0002086270670000043
Figure FDA0002086270670000044
Figure FDA0002086270670000045
Figure FDA0002086270670000046
对于F6×6有：
Figure FDA0002086270670000047
系统量测方程为：
Figure FDA0002086270670000048
对于HSINS/DVL和VSINS/DVL有：
Figure FDA0002086270670000049
VSINS/DVL＝[vE vN]T (23)
这里系统噪声方差阵：
Figure FDA00020862706700000410
量测噪声方差阵：
Figure FDA0002086270670000051
步骤五、系统建立滤波增益改进自适应滤波算法，并对滤波增益改进自适应卡尔曼滤波算法系统的状态方程和量测方程进行描述，如公式(26)、(27)：
状态方程描述为：
Xk＝FkXk-1+GWk (26)
系统量测方程为：
Figure FDA0002086270670000052
其中，Xk为k时刻的状态变量值；Xk-1为k-1时刻的状态变量；F为作用在Xk-1上的状态变换系数；Wk为k时刻的状态噪声值；G为作用在上Wk的系数；Hk为观测模型系数，把真实状态空间映射成观测空间；Zk为k时刻的观测值，由捷联惯导的东向、北向速度误差和多普勒计程仪东向北向速度误差的差构成；Vk为k时刻的观测噪声值；
步骤六、将Fk，Gk，Wk，Hk，Vk，以及初始的状态变量X带入系统的状态方程和量测方程进行一步预测，求出k时刻预测的量测值
Figure FDA0002086270670000053
和
Figure FDA0002086270670000054
Figure FDA0002086270670000055
Figure FDA0002086270670000056
为k时刻量测值的误差；
步骤七、系统根据稳定性判据，判断
Figure FDA0002086270670000057
若成立则系统发散，执行步骤八，采用增益补偿自适应滤波；若不成立则系统收敛，执行步骤九，采用增益补偿强跟踪卡尔曼滤波；
步骤八、系统将对应参数及量代入公式(28)～(37)中进行计算，
Figure FDA0002086270670000061
Figure FDA0002086270670000062
Figure FDA0002086270670000063
Figure FDA0002086270670000064
Figure FDA0002086270670000065
Figure FDA0002086270670000066
其中，
Figure FDA0002086270670000067
为k时刻的状态变量值；
Figure FDA0002086270670000068
为由k-1时刻的状态变量所得到的k时刻的预测值；Fk,k-1为作用在
Figure FDA0002086270670000069
上的状态变换系数；Kk为作用在
Figure FDA00020862706700000610
上的滤波增益补偿卡尔曼系数；Hk为观测模型系数，把真实状态空间映射成观测空间；Zk为k时刻的观测值，k时刻预测的量测值为
Figure FDA00020862706700000611
Pk,k-1为先验估计误差协方差值；Pk为后验估计误差协方差值；在(28)～(33)式子中
Figure FDA00020862706700000612
分别由时变噪声估值方程所计算得式(34)～(37)：
Figure FDA00020862706700000613
Figure FDA00020862706700000614
Figure FDA00020862706700000615
Figure FDA00020862706700000616
实现增益补偿自适应滤波，得到下一时刻状态变量估计值
Figure FDA00020862706700000617
执行步骤十；
步骤九、系统将对应参数及参量代入公式(38)～(46)中进行计算：
Figure FDA0002086270670000071
Figure FDA0002086270670000072
Figure FDA0002086270670000073
λk+1＝diag[λ1(k+1),λ2(k+1),…,λm(k+1)] (41)
Figure FDA0002086270670000074
Figure FDA0002086270670000075
Figure FDA0002086270670000076
Figure FDA0002086270670000077
Figure FDA0002086270670000078
实现增益补偿强跟踪卡尔曼滤波，得到下一时刻状态变量估计值
Figure FDA0002086270670000079
执行步骤十；
步骤十、系统更新变换k＝k+1，将得到的状态变量估计值
Figure FDA00020862706700000710
记为新的状态变量值
Figure FDA00020862706700000711
步骤十一、系统判断k是否等于n，如果是，则执行步骤十二，否则执行步骤六；
步骤十二、系统得到生成的系统状态变量序列
Figure FDA00020862706700000712
Figure FDA00020862706700000713
输出的最终结果
Figure FDA00020862706700000714
即为当前时刻经过滤波校正后的状态结果；包含
Figure FDA00020862706700000715
是误差量的校正结果，再结合当前时刻SINS/DVL组合定位系统的观测值Yn＝[vEn vNn Ln λn vdn]T，得到校正后的航行器东向、北向速度
Figure FDA0002086270670000081
和经纬度信息
Figure FDA0002086270670000082
以及多普勒计程仪的速度信息
Figure FDA0002086270670000083
其中，
Figure FDA0002086270670000084
步骤十三、系统判断SINS/DVL组合定位任务是否完成，如果是，执行步骤十四，否则执行步骤二；
步骤十四、系统结束增益补偿自适应滤波的SINS/DVL组合定位任务。
2.根据权利要求1所述的一种增益补偿自适应滤波的SINS/DVL组合定位方法的步骤七进一步说明，其特征在于判断滤波是否发散的判据，根据滤波估计的误差与期望误差的大小关系来判断。
3.根据权利要求1所述的一种增益补偿自适应滤波的SINS/DVL组合定位方法的做出进一步说明，其特征在于采用的滤波增益补偿算法，针对水下海流的外部干扰或者水下航行器的航速航向发生突变，会使得滤波过程得估计值在跟踪水下航行器得真实状态时有轻微的延迟，从而使得导航精度和质量下降的问题，可以实现快速对系统状态进行实时跟踪并减小误差的积累速度。
4.根据权利要求1所述的一种增益补偿自适应滤波的SINS/DVL组合定位方法做出进一步说明，其特征在于采用改进自适应卡尔曼滤波，既可以获得既能保证滤波精度，又可以有效阻止滤波过程发散。
5.根据权利要求1所述的一种增益补偿自适应滤波的SINS/DVL组合定位方法做出进一步说明，其特征在于采用增益补偿改进自适应滤波算法，将增益补偿算法和改进自适应卡尔曼算法融合，融合算法的精确度相比于融合之前的算法要高出一个数量级。
CN201910488837.8A 2019-06-06 2019-06-06 一种增益补偿自适应滤波的sins/dvl组合定位方法 Active CN110146075B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN201910488837.8A CN110146075B (zh) 2019-06-06 2019-06-06 一种增益补偿自适应滤波的sins/dvl组合定位方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN201910488837.8A CN110146075B (zh) 2019-06-06 2019-06-06 一种增益补偿自适应滤波的sins/dvl组合定位方法

Publications (2)

Publication Number Publication Date
CN110146075A CN110146075A (zh) 2019-08-20
CN110146075B true CN110146075B (zh) 2022-06-21

Family

ID=67590666

Family Applications (1)

Application Number Title Priority Date Filing Date
CN201910488837.8A Active CN110146075B (zh) 2019-06-06 2019-06-06 一种增益补偿自适应滤波的sins/dvl组合定位方法

Country Status (1)

Country Link
CN (1) CN110146075B (zh)

Families Citing this family (11)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN110763872A (zh) * 2019-11-21 2020-02-07 中国船舶重工集团公司第七0七研究所 一种多普勒测速仪多参数在线标定方法
CN111024064B (zh) * 2019-11-25 2021-10-19 东南大学 一种改进Sage-Husa自适应滤波的SINS/DVL组合导航方法
CN111024076B (zh) * 2019-12-30 2022-04-22 北京航空航天大学 一种基于仿生偏振的水下组合导航系统
CN113311463A (zh) * 2020-02-26 2021-08-27 北京三快在线科技有限公司 Gps延迟时间在线补偿方法、装置、电子设备和存储介质
CN111504324B (zh) * 2020-04-27 2022-07-26 西北工业大学 一种噪声自适应滤波的水下组合导航方法
CN112710304B (zh) * 2020-12-17 2022-12-13 西北工业大学 一种基于自适应滤波的水下自主航行器导航方法
CN112683271B (zh) * 2020-12-17 2023-10-27 东南大学 一种考虑可观测性的水域观测平台的组合定位方法
CN112729291B (zh) * 2020-12-29 2022-03-04 东南大学 一种深潜长航潜水器sins/dvl洋流速度估计方法
CN115112154B (zh) * 2022-08-30 2022-11-18 南开大学 一种水下自主导航定位系统的标定方法
CN116165885B (zh) * 2022-11-29 2023-11-14 华东交通大学 一种高速列车的无模型自适应鲁棒控制方法及系统
CN116068540B (zh) * 2023-02-17 2023-09-12 哈尔滨工程大学 一种声学多普勒测速径向波束角校正方法

Citations (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN101393025A (zh) * 2008-11-06 2009-03-25 哈尔滨工程大学 Auv组合导航系统无迹切换方法
CN102607330A (zh) * 2012-03-23 2012-07-25 东南大学 惯导系统传递对准中基准信息的优化处理方法
CN103389095A (zh) * 2013-07-24 2013-11-13 哈尔滨工程大学 一种用于捷联惯性/多普勒组合导航系统的自适应滤波方法
CN108828950A (zh) * 2018-07-23 2018-11-16 广东工业大学 一种自适应自抗扰控制方法、装置及设备
CN109737959A (zh) * 2019-03-20 2019-05-10 哈尔滨工程大学 一种基于联邦滤波的极区多源信息融合导航方法

Patent Citations (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN101393025A (zh) * 2008-11-06 2009-03-25 哈尔滨工程大学 Auv组合导航系统无迹切换方法
CN102607330A (zh) * 2012-03-23 2012-07-25 东南大学 惯导系统传递对准中基准信息的优化处理方法
CN103389095A (zh) * 2013-07-24 2013-11-13 哈尔滨工程大学 一种用于捷联惯性/多普勒组合导航系统的自适应滤波方法
CN108828950A (zh) * 2018-07-23 2018-11-16 广东工业大学 一种自适应自抗扰控制方法、装置及设备
CN109737959A (zh) * 2019-03-20 2019-05-10 哈尔滨工程大学 一种基于联邦滤波的极区多源信息融合导航方法

Non-Patent Citations (1)

* Cited by examiner, † Cited by third party
Title
High Performance Doppler-Inertial Navigation - Experimental Results;Mikael Bliksted Larsen;《: OCEANS 2000 MTS/IEEE Conference and Exhibition. Conference Proceedings 》;20001231;第1449-1456页 *

Also Published As

Publication number Publication date
CN110146075A (zh) 2019-08-20

Similar Documents

Publication Publication Date Title
CN110146075B (zh) 一种增益补偿自适应滤波的sins/dvl组合定位方法
CN110146076B (zh) 一种无逆矩阵自适应滤波的sins/dvl组合定位方法
CN110221332B (zh) 一种车载gnss/ins组合导航的动态杆臂误差估计和补偿方法
CN110398257B (zh) Gps辅助的sins系统快速动基座初始对准方法
CN111024064B (zh) 一种改进Sage-Husa自适应滤波的SINS/DVL组合导航方法
CN108226980B (zh) 基于惯性测量单元的差分gnss与ins自适应紧耦合导航方法
US7142983B2 (en) Method for the processing of non-continuous atom interferometer intertial instrument measurements and continuous wide bandwidth instrument measurements with a gravity database
CN110779521A (zh) 一种多源融合的高精度定位方法与装置
CN109724599A (zh) 一种抗野值的鲁棒卡尔曼滤波sins/dvl组合导航方法
CN110440830B (zh) 动基座下车载捷联惯导系统自对准方法
CN113063429B (zh) 一种自适应车载组合导航定位方法
CN111024074B (zh) 一种基于递推最小二乘参数辨识的惯导速度误差确定方法
CN110988926B (zh) 一种在松gnss/ins组合导航模式下实现位置精确定点欺骗偏移方法
CN108444476B (zh) 一种考虑水声通信延迟的多水下无人航行器极区协同导航方法
CN110285834B (zh) 基于一点位置信息的双惯导系统快速自主重调方法
CN109507706B (zh) 一种gps信号丢失的预测定位方法
CN109612460B (zh) 一种基于静止修正的垂线偏差测量方法
CN111750865B (zh) 一种用于双功能深海无人潜器导航系统的自适应滤波导航方法
CN113203418A (zh) 基于序贯卡尔曼滤波的gnssins视觉融合定位方法及系统
CN111121766A (zh) 一种基于星光矢量的天文与惯性组合导航方法
CN110849360B (zh) 面向多机协同编队飞行的分布式相对导航方法
CN110133695B (zh) 一种双天线gnss位置延迟时间动态估计系统及方法
CN112504275A (zh) 一种基于级联卡尔曼滤波算法的水面舰船水平姿态测量方法
CN113291493B (zh) 一种卫星多敏感器融合姿态确定方法和系统
CN111780753B (zh) 一种姿态误差反馈修正的提高惯性制导精度的方法

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