CN115790669B - 基于最大熵平滑变结构滤波的dvl误差自适应标定方法 - Google Patents

基于最大熵平滑变结构滤波的dvl误差自适应标定方法 Download PDF

Info

Publication number
CN115790669B
CN115790669B CN202310079740.8A CN202310079740A CN115790669B CN 115790669 B CN115790669 B CN 115790669B CN 202310079740 A CN202310079740 A CN 202310079740A CN 115790669 B CN115790669 B CN 115790669B
Authority
CN
China
Prior art keywords
representing
dvl
error
matrix
kernel function
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
CN202310079740.8A
Other languages
English (en)
Other versions
CN115790669A (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.)
Hohai University HHU
Original Assignee
Hohai University HHU
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 Hohai University HHU filed Critical Hohai University HHU
Priority to CN202310079740.8A priority Critical patent/CN115790669B/zh
Publication of CN115790669A publication Critical patent/CN115790669A/zh
Application granted granted Critical
Publication of CN115790669B publication Critical patent/CN115790669B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

Classifications

    • 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

Landscapes

  • Navigation (AREA)

Abstract

本发明公开了一种基于最大熵平滑变结构滤波的DVL误差自适应标定方法，属于水下机器人导航定位技术领域，其主要目的在于复杂水下环境下估计出DVL的误差信息，进一步提高SINS/DVL系统的组合导航精度。本发明的主要步骤包括：DVL误差标定空间模型构建、滤波初始化及状态更新、基于改进最大熵原理的量测噪声估计、基于平滑变结构原理的滤波增益计算以及状态估计及均方误差计算。本发明可以解决复杂水下环境下DVL误差估计精度差的问题。相比基于卡尔曼滤波原理的DVL误差标定方法。本方法能够准确地估计出DVL各项误差信息，进一步提高SINS/DVL的组合导航精度。

Description

基于最大熵平滑变结构滤波的DVL误差自适应标定方法
技术领域
本发明属于水下机器人导航定位技术，具体涉及一种基于最大熵平滑变结构滤波的DVL误差自适应标定方法。
背景技术
DVL误差标定是实现SINS/DVL高精度组合导航的基础。DVL误差标定方法能够估计出IMU（IMU是实现SINS的设备）与DVL之间的安装误差角、DVL刻度因子误差以及IMU与DVL之间杆臂误差。当SINS/DVL进行组合导航过程时，将上述标定的误差信息补偿到SINS/DVL组合导航系统中能够进一步提高系统导航精度。因此，DVL误差标定是实现SINS/DVL组合导航的前提和基础。
传统的DVL误差标定方法一般采用GNSS/SINS组合的高精度速度信息和DVL输出的速度信息的差值作为卡尔曼量测输入信息，通过建立误差模型实现对DVL误差的估计。同时，近年来随着最优估计算法的发展，如梯度下降法、牛顿法等，最优估计方法也应用到DVL误差标定领域。
然而，目前的方法存在以下两个问题：一方面，由于水下环境复杂，DVL声波在传播中会遇到噪声或者各种干扰，导致DVL输出的速度信息存在较大干扰，这对DVL误差标定带来了一定的难度；另一方面，目前的方法在对DVL误差标定的时候只考虑了IMU与DVL之间的安装误差角和DVL刻度因子误差，未考虑杆臂误差的影响。
针对上述问题，本发明提出一种基于最大熵平滑变结构滤波的DVL误差自适应标定方法，一方面，同时考虑安装误差角、DVL刻度因子误差以及杆臂误差，建立基于滤波原理的空间模型；另一方面，为提高算法的鲁棒性，在传统卡尔曼滤波算法的基础上，分别引入改进的最大熵准则以及平滑变结构滤波，实现对外界干扰测量信息的鲁棒处理，进而提高DVL误差标定精度。
发明内容
发明目的：本发明的目的在于解决水下DVL误差标定的问题，提供一种基于最大熵平滑变结构滤波的DVL误差自适应标定方法。
本发明的基于最大熵平滑变结构滤波的DVL误差自适应标定方法，该方法首先已知：多普勒计程仪，缩写为DVL，测量得到的设备系下的三维速度信息
Figure SMS_1
，其中
Figure SMS_2
分别表示设备坐标系下x方向的速度信息，设备坐标系下y方向的速度信息以及设备坐标系下z方向的速度信息，上标T表示矩阵的转置；
惯导/卫星组合输出的导航坐标系下的三维速度信息：
Figure SMS_3
，其中
Figure SMS_4
分别表示导航坐标系下x方向的速度信息，导航坐标系下y方向的速度信息以及导航坐标系下z方向的速度信息；
惯导/卫星组合输出的导航坐标系下的三维姿态信息：
Figure SMS_5
，其中
Figure SMS_6
表示俯仰角；
Figure SMS_7
表示横滚角；
Figure SMS_8
表示航向角；
所述标定方法包括如下步骤：
步骤1. DVL误差标定空间模型构建，空间模型包括状态方程构建和量测方程构建；
步骤2. 进行滤波初始化，即状态更新；
步骤3. 基于改进最大熵原理进行量测噪声估计，所述改进最大熵原理是指把高斯核函数和多项式核函数取长补短结合起来构成混合核函数计算权重；
步骤4. 引入平滑变结构原理，实现对卡尔曼滤波增益的计算；
步骤5. 状态估计及均方误差计算。
进一步地，步骤1的具体方法为：
首先建立状态方程：
Figure SMS_9
其中，
Figure SMS_10
表示标定模型状态转移矩阵；
Figure SMS_11
表示标定模型系统噪声；
Figure SMS_12
表示标定模型状态向量，
Figure SMS_13
表示对
Figure SMS_14
求一次导数，选取惯性测量单元和DVL安装误差角、DVL刻度因子误差以及惯性测量单元和DVL的杆臂误差作为标定模型状态向量，即：
Figure SMS_15
其中，
Figure SMS_16
表示惯性测量单元和DVL在x方向的安装误差角；
Figure SMS_17
表示惯性测量单元和DVL在y方向的安装误差角；
Figure SMS_18
表示惯性测量单元和DVL在z方向的安装误差角；
Figure SMS_19
表示DVL刻度因子误差；
Figure SMS_20
表示惯性测量单元和DVL在x方向的杆臂误差；
Figure SMS_21
表示惯性测量单元和DVL在y方向的杆臂误差；
Figure SMS_22
表示惯性测量单元和DVL在z方向的杆臂误差；其中：
Figure SMS_23
由于惯性测量单元和DVL的安装误差角，DVL刻度因子误差以及惯性测量单元和DVL的杆臂误差都为常值，所以其微分方程均为零，因此标定模型状态转移矩阵可表示如下：
Figure SMS_24
其中，
Figure SMS_25
表示一个7行7列的矩阵，其中每个元素都为0；
接着，建立量测方程，其量测方程表示如下：
Figure SMS_26
其中，
Figure SMS_27
表示标定模型量测转移矩阵；
Figure SMS_28
表示量测模型量测噪声矩阵；
Figure SMS_29
表示标定模型量测矩阵，可表示如下：
Figure SMS_30
其中，
Figure SMS_31
表示DVL测量得到的设备系下的三维速度信息；
Figure SMS_32
表示载体坐标系下SINS/GNSS融合得到的速度信息，其中转换关系如下：
Figure SMS_33
其中，
Figure SMS_34
表示导航坐标系下SINS/GNSS输出的速度信息；
Figure SMS_35
表示导航系到载体系的姿态转移矩阵，可表示如下：
Figure SMS_36
其中，
Figure SMS_37
表示SINS/GNSS融合得到的俯仰角；
Figure SMS_38
表示SINS/GNSS融合得到的横滚角；
Figure SMS_39
表示SINS/GNSS融合得到的航向角；
根据上述过程，可得标定模型量测转移矩阵：
Figure SMS_40
其中，
Figure SMS_41
表示惯导输出的x方向的角速度信息；
Figure SMS_42
表示惯导输出的y方向的角速度信息；
Figure SMS_43
表示惯导输出的z方向的角速度信息；
Figure SMS_44
表示载体坐标系下SINS/GNSS融合得到的x方向的速度信息；
Figure SMS_45
表示载体坐标系下SINS/GNSS融合得到的y方向的速度信息；
Figure SMS_46
表示载体坐标系下SINS/GNSS融合得到的z方向的速度信息，即：
Figure SMS_47
。
进一步地，步骤2的具体方法为：
设置滤波初值
Figure SMS_48
和
Figure SMS_49
，
状态估计
Figure SMS_50
其中，
Figure SMS_51
表示状态一步转移矩阵；
Figure SMS_52
表示k-1时刻的状态向量估计值，
Figure SMS_53
表示相对于k-1时刻中k时刻的状态估计值；
状态估计均方误差阵：
Figure SMS_54
其中，
Figure SMS_55
表示k-1时刻的状态估计的均方误差阵；
Figure SMS_56
表示系统噪声方差阵，
Figure SMS_57
表示一步预测均方误差阵。
进一步地，步骤3的具体方法为：
计算等效量测残差
Figure SMS_58
：
Figure SMS_59
其中，
Figure SMS_60
表示量测噪声方差阵，
Figure SMS_61
表示k时刻的量测转移矩阵，
Figure SMS_62
表示k时刻的量测向量；
把高斯核函数和多项式核函数取长补短结合起来构成混合核函数
Figure SMS_63
：
Figure SMS_64
其中，
Figure SMS_65
表示高斯核函数，
Figure SMS_66
表示多项式核函数；m表示权重系数，当m= 0时，混合核函数退化为多项式核函数，当m= 1时，混合核函数退化为高斯核函数；两个核函数具体表示如下：
Figure SMS_67
其中，
Figure SMS_68
表示核函数带宽，
Figure SMS_69
为常值；d表示次方；
利用上述混合核函数计算权重矩阵
Figure SMS_70
可得：
Figure SMS_71
其中
Figure SMS_72
表示k时刻误差的混合核函数，
计算修正后的量测噪声矩阵为：
Figure SMS_73
其中，
Figure SMS_74
表示修正后的量测噪声矩阵。
进一步地，步骤4的具体方法为：
首先通过引入平滑变结构原理，实现对卡尔曼滤波增益的重新设计，根据平滑变结构原理和标定空间模型计算时变平滑边界层
Figure SMS_75
，如下式所示：
Figure SMS_76
其中
Figure SMS_77
和
Figure SMS_78
仅为中间自定义变量，没有物理含义，分别表示如下
Figure SMS_79
其中，
Figure SMS_80
表示收敛率取值为0到1之间；
Figure SMS_81
表示求绝对值符号，
Figure SMS_82
表示一步预测协方差矩阵，
Figure SMS_83
表示k+1时刻的状态估计量；
Figure SMS_84
其中，
Figure SMS_85
表示滤波增益矩阵，
Figure SMS_86
表示k+1时刻的量测向量，
Figure SMS_87
表示求矩阵的对角矩阵；
Figure SMS_88
表示饱和函数，定义如下：
Figure SMS_89
进一步地，步骤5的具体方法为：
量测更新中状态估计为：
Figure SMS_90
均方误差阵更新为：
Figure SMS_91
其中，
Figure SMS_92
表示单位矩阵。
与现有技术相比，本发明具有如下优点：
(1)本发明针对DVL误差标定模型构建问题，同时考虑IMU与DVL之间的安装误差角、DVL刻度因子误差以及IMU与DVL之间的杆臂误差，构建了基于滤波原理的DVL误差标定空间模型，进一步完善了DVL误差标定信息的全面性。
(2)本发明在传统最大熵原理的基础上，考虑不同核函数的优缺点，提出一种基于混合核函数的最大熵估计方法，进一步提高了对异常值的处理能力。
(3) 本发明在滤波算法的基础上，引入平滑变结构原理，通过设计滤波增益矩阵，进一步提高了滤波算法的鲁棒性。
附图说明
图1为本发明所描述的基于最大熵平滑变结构滤波的DVL误差自适应标定方法原理图；
图2为本发明方法的在x方向、y方向、z方向的安装误差角估计曲线；
图3为本发明方法的DVL刻度因子误差估计曲线；
图4为本发明方法的杆臂在x方向、y方向、z方向的误差估计曲线。
具体实施方式
下面对本发明技术方案进行详细说明，但是本发明的保护范围不局限于所述实施例。
本发明的基于最大熵平滑变结构滤波的DVL误差自适应标定方法，该方法首先已知：多普勒计程仪，缩写为DVL，测量得到的设备系下的三维速度信息
Figure SMS_93
，其中
Figure SMS_94
分别表示设备坐标系下x方向的速度信息，设备坐标系下y方向的速度信息以及设备坐标系下z方向的速度信息，上标T表示矩阵的转置；
惯导/卫星组合输出的导航坐标系下的三维速度信息：
Figure SMS_95
，其中
Figure SMS_96
分别表示导航坐标系下x方向的速度信息，导航坐标系下y方向的速度信息以及导航坐标系下z方向的速度信息；
惯导/卫星组合输出的导航坐标系下的三维姿态信息：
Figure SMS_97
，其中
Figure SMS_98
表示俯仰角；
Figure SMS_99
表示横滚角；
Figure SMS_100
表示航向角；
如图1所示，本发明所述标定方法包括如下步骤：
步骤1. DVL误差标定空间模型构建，空间模型包括状态方程构建和量测方程构建；
首先建立状态方程：
Figure SMS_101
其中，
Figure SMS_102
表示标定模型状态转移矩阵；
Figure SMS_103
表示标定模型系统噪声；
Figure SMS_104
表示标定模型状态向量，
Figure SMS_105
表示对
Figure SMS_106
求一次导数，选取惯性测量单元和DVL安装误差角、DVL刻度因子误差以及惯性测量单元和DVL的杆臂误差作为标定模型状态向量，即：
Figure SMS_107
其中，
Figure SMS_108
表示惯性测量单元和DVL在x方向的安装误差角；
Figure SMS_109
表示惯性测量单元和DVL在y方向的安装误差角；
Figure SMS_110
表示惯性测量单元和DVL在z方向的安装误差角；
Figure SMS_111
表示DVL刻度因子误差；
Figure SMS_112
表示惯性测量单元和DVL在x方向的杆臂误差；
Figure SMS_113
表示惯性测量单元和DVL在y方向的杆臂误差；
Figure SMS_114
表示惯性测量单元和DVL在z方向的杆臂误差；其中：
Figure SMS_115
由于惯性测量单元和DVL的安装误差角，DVL刻度因子误差以及惯性测量单元和DVL的杆臂误差都为常值，所以其微分方程均为零，因此标定模型状态转移矩阵可表示如下：
Figure SMS_116
其中，
Figure SMS_117
表示一个7行7列的矩阵，其中每个元素都为0；
接着，建立量测方程，其量测方程表示如下：
Figure SMS_118
其中，
Figure SMS_119
表示标定模型量测转移矩阵；
Figure SMS_120
表示量测模型量测噪声矩阵；
Figure SMS_121
表示标定模型量测矩阵，可表示如下：
Figure SMS_122
其中，
Figure SMS_123
表示DVL测量得到的设备系下的三维速度信息；
Figure SMS_124
表示载体坐标系下SINS/GNSS融合得到的速度信息，其中转换关系如下：
Figure SMS_125
其中，
Figure SMS_126
表示导航坐标系下SINS/GNSS输出的速度信息；
Figure SMS_127
表示导航系到载体系的姿态转移矩阵，可表示如下：
Figure SMS_128
其中，
Figure SMS_129
表示SINS/GNSS融合得到的俯仰角；
Figure SMS_130
表示SINS/GNSS融合得到的横滚角；
Figure SMS_131
表示SINS/GNSS融合得到的航向角；
根据上述过程，可得标定模型量测转移矩阵：
Figure SMS_132
其中，
Figure SMS_133
表示惯导输出的x方向的角速度信息；
Figure SMS_134
表示惯导输出的y方向的角速度信息；
Figure SMS_135
表示惯导输出的z方向的角速度信息；
Figure SMS_136
表示载体坐标系下SINS/GNSS融合得到的x方向的速度信息；
Figure SMS_137
表示载体坐标系下SINS/GNSS融合得到的y方向的速度信息；
Figure SMS_138
表示载体坐标系下SINS/GNSS融合得到的z方向的速度信息，即：
Figure SMS_139
。
以上即为本发明提出的标定空间模型。
步骤2. 进行滤波初始化，即状态更新；
设置滤波初值
Figure SMS_140
和
Figure SMS_141
，
状态估计
Figure SMS_142
其中，
Figure SMS_143
表示状态一步转移矩阵；
Figure SMS_144
表示k-1时刻的状态向量估计值，
Figure SMS_145
表示相对于k-1时刻中k时刻的状态估计值；
状态估计均方误差阵：
Figure SMS_146
其中，
Figure SMS_147
表示k-1时刻的状态估计的均方误差阵；
Figure SMS_148
表示系统噪声方差阵，
Figure SMS_149
表示一步预测均方误差阵。
步骤3. 基于改进最大熵原理进行量测噪声估计，所述改进最大熵原理是指把高斯核函数和多项式核函数取长补短结合起来构成混合核函数计算权重；
计算等效量测残差
Figure SMS_150
：
Figure SMS_151
其中，
Figure SMS_152
表示量测噪声方差阵，
Figure SMS_153
表示k时刻的量测转移矩阵，
Figure SMS_154
表示k时刻的量测向量；
传统的最大熵原理中，采用高斯核函数计算权重。高斯核函数作为局部性核函数具有学习能力强、泛化性能较弱的特点。而多项式核函数作为全局性核函数具有泛化性能强、学习能力较弱的特点。因此本发明把高斯核函数和多项式核函数取长补短结合起来构成混合核函数
Figure SMS_155
：
Figure SMS_156
其中，
Figure SMS_157
表示高斯核函数，
Figure SMS_158
表示多项式核函数；m表示权重系数，当m= 0时，混合核函数退化为多项式核函数，当m= 1时，混合核函数退化为高斯核函数；两个核函数具体表示如下：
Figure SMS_159
其中，
Figure SMS_160
表示核函数带宽，
Figure SMS_161
为常值；d表示次方；
利用上述混合核函数计算权重矩阵
Figure SMS_162
可得：
Figure SMS_163
其中
Figure SMS_164
表示k时刻误差的混合核函数，
计算修正后的量测噪声矩阵为：
Figure SMS_165
其中，
Figure SMS_166
表示修正后的量测噪声矩阵。
步骤4. 引入平滑变结构原理，实现对卡尔曼滤波增益的计算；
为提高算法的鲁棒性，本发明对传统卡尔曼滤波增益计算进行修改。通过引入平滑变结构原理，实现对卡尔曼滤波增益的重新设计，进一步提高算法的鲁棒性。根据平滑变结构原理和标定空间模型计算时变平滑边界层
Figure SMS_167
，如下式所示：
Figure SMS_168
其中
Figure SMS_169
和
Figure SMS_170
仅为中间自定义变量，没有物理含义，分别表示如下
Figure SMS_171
其中，
Figure SMS_172
表示收敛率取值为0到1之间；
Figure SMS_173
表示求绝对值符号，
Figure SMS_174
表示一步预测协方差矩阵，
Figure SMS_175
表示k+1时刻的状态估计量；
Figure SMS_176
其中，
Figure SMS_177
表示滤波增益矩阵，
Figure SMS_178
表示k+1时刻的量测向量，
Figure SMS_179
表示求矩阵的对角矩阵；
Figure SMS_180
表示饱和函数，定义如下：
Figure SMS_181
步骤5. 状态估计及均方误差计算。
量测更新中状态估计为：
Figure SMS_182
均方误差阵更新为：
Figure SMS_183
其中，
Figure SMS_184
表示单位矩阵。
仿真参数设置如下：
DVL输出频率2Hz，DVL刻度因子误差为0.002；IMU与DVL之间的安装误差设置为2°，-1°，1°；IMU与DVL的杆臂误差设置为1m，-1m，0.5m。其中图2为仿真结果，图标显示了卡尔曼滤波方法和本发明方法的安装误差角估计结果，从图2中可以看出，本发明方法更接近真值。图3给出了两种方法对应的DVL安装误差估计结果，从图中可以看出本发明方法精度优于卡尔曼滤波方法。图4给出了两种方法对应的杆臂误差估计曲线，从图中可以看出，本发明方法在杆臂估计精度方面优于卡尔曼滤波方法。
应理解这些实施例仅用于说明本发明而不用于限制本发明的范围，在阅读了本发明之后，本领域普通技术人员对本发明的各种等价形式的修改均落于本申请所附权利要求所限定的范围。

Claims (4)

1.一种基于最大熵平滑变结构滤波的DVL误差自适应标定方法，该方法首先已知：
多普勒计程仪，缩写为DVL，测量得到的设备系下的三维速度信息
Figure QLYQS_1
其中
Figure QLYQS_2
分别表示设备坐标系下x方向的速度信息，设备坐标系下y方向的速度信息以及设备坐标系下z方向的速度信息，上标T表示矩阵的转置；
惯导/卫星组合输出的导航坐标系下的三维速度信息：
Figure QLYQS_3
其中
Figure QLYQS_4
分别表示导航坐标系下x方向的速度信息，导航坐标系下y方向的速度信息以及导航坐标系下z方向的速度信息；
惯导/卫星组合输出的导航坐标系下的三维姿态信息：[θ γ ψ]T，其中θ表示俯仰角；γ表示横滚角；ψ表示航向角；
其特征在于，所述标定方法包括如下步骤：
步骤1.DVL误差标定空间模型构建，空间模型包括状态方程构建和量测方程构建；
步骤2.进行滤波初始化，即状态更新；
步骤3.基于改进最大熵原理进行量测噪声估计，所述改进最大熵原理是指把高斯核函数和多项式核函数取长补短结合起来构成混合核函数计算权重，具体方法为：
计算等效量测残差ek：
Figure QLYQS_5
其中，Rk表示量测噪声方差阵，Hk表示k时刻的量测转移矩阵，Zk表示k时刻的量测向量；
把高斯核函数和多项式核函数取长补短结合起来构成混合核函数Gmix(x)：
Gmix(x)＝mGgauss(x)+(1-m)Gpoly(x)，0≤m≤1
其中，Ggauss(x)表示高斯核函数，Gpoly(x)表示多项式核函数；m表示权重系数，当m＝0时，混合核函数退化为多项式核函数，当m＝1时，混合核函数退化为高斯核函数；两个核函数具体表示如下：
Figure QLYQS_6
其中，σ表示核函数带宽，λ＞0为常值；d表示次方；
利用上述混合核函数计算权重矩阵Ωk可得：
Ωk＝diag[Gmix(ek)]
其中Gmix(ek)表示k时刻误差的混合核函数，
计算修正后的量测噪声矩阵为：
Figure QLYQS_7
其中，Rk表示修正后的量测噪声矩阵；
步骤4.引入平滑变结构原理，实现对卡尔曼滤波增益的计算，具体方法为：
首先通过引入平滑变结构原理，实现对卡尔曼滤波增益的重新设计，根据平滑变结构原理和标定空间模型计算时变平滑边界层Ψk+1，如下式所示：
Figure QLYQS_8
其中，H表示标定模型量测转移矩阵，A和Sk+1仅为中间自定义变量，没有物理含义，分别表示如下
Figure QLYQS_9
sk+1＝HPk+1|kHT+Rk+1
其中，ζ表示收敛率取值为0到1之间；|.|abs表示求绝对值符号，Pk+1|k表示一步预测协方差矩阵，
Figure QLYQS_10
表示k+1时刻的状态估计量；
Figure QLYQS_11
其中，Kk表示滤波增益矩阵，Zk+1表示k+1时刻的量测向量，diag()表示求矩阵的对角矩阵；sat()表示饱和函数，定义如下：
Figure QLYQS_12
步骤5.状态估计及均方误差计算。
2.根据权利要求1所述的基于最大熵平滑变结构滤波的DVL误差自适应标定方法，其特征在于，步骤1的具体方法为：
首先建立状态方程：
Figure QLYQS_13
其中，F表示标定模型状态转移矩阵；W表示标定模型系统噪声；X表示标定模型状态向量，
Figure QLYQS_14
表示对X求一次导数，选取惯性测量单元和DVL安装误差角、DVL刻度因子误差以及惯性测量单元和DVL的杆臂误差作为标定模型状态向量，即：
X＝[φxφyφz δK LxLyLz]T
其中，φx表示惯性测量单元和DVL在x方向的安装误差角；φy表示惯性测量单元和DVL在y方向的安装误差角；φz表示惯性测量单元和DVL在z方向的安装误差角；δK表示DVL刻度因子误差；Lx表示惯性测量单元和DVL在x方向的杆臂误差；Ly表示惯性测量单元和DVL在y方向的杆臂误差；Lz表示惯性测量单元和DVL在z方向的杆臂误差；其中：
Figure QLYQS_15
由于惯性测量单元和DVL的安装误差角，DVL刻度因子误差以及惯性测量单元和DVL的杆臂误差都为常值，所以其微分方程均为零，因此标定模型状态转移矩阵可表示如下：
F＝07×7
其中，07×7表示一个7行7列的矩阵，其中每个元素都为0；
接着，建立量测方程，其量测方程表示如下：
Z＝HX+V
其中，V表示量测模型量测噪声矩阵；Z表示标定模型量测矩阵，表示如下：
Figure QLYQS_16
其中，
Figure QLYQS_17
表示DVL测量得到的设备系下的三维速度信息；
Figure QLYQS_18
表示载体坐标系下SINS/GNSS融合得到的速度信息，其中转换关系如下：
Figure QLYQS_19
其中，
Figure QLYQS_20
表示导航坐标系下SINS/GNSS输出的速度信息；
Figure QLYQS_21
表示导航系到载体系的姿态转移矩阵，可表示如下：
Figure QLYQS_22
其中，θ表示SINS/GNSS融合得到的俯仰角；γ表示SINS/GNSS融合得到的横滚角；ψ表示SINS/GNSS融合得到的航向角；
根据上述过程，可得标定模型量测转移矩阵：
Figure QLYQS_23
其中，
Figure QLYQS_24
表示惯导输出的x方向的角速度信息；
Figure QLYQS_25
表示惯导输出的y方向的角速度信息；
Figure QLYQS_26
表示惯导输出的z方向的角速度信息；
Figure QLYQS_27
表示载体坐标系下SINS/GNSS融合得到的x方向的速度信息；
Figure QLYQS_28
表示载体坐标系下SINS/GNSS融合得到的y方向的速度信息；
Figure QLYQS_29
表示载体坐标系下SINS/GNSS融合得到的z方向的速度信息，即：
Figure QLYQS_30
3.根据权利要求1或2所述的基于最大熵平滑变结构滤波的DVL误差自适应标定方法，其特征在于，步骤2的具体方法为：
设置滤波初值
Figure QLYQS_31
和P0＝[5，5，5，0.5，3，3，3]T，
状态估计
Figure QLYQS_32
其中，Fk|k-1表示状态一步转移矩阵；
Figure QLYQS_33
表示k-1时刻的状态向量估计值，
Figure QLYQS_34
表示相对于k-1时刻中k时刻的状态估计值；
状态估计均方误差阵：
Figure QLYQS_35
其中，Pk-1表示k-1时刻的状态估计的均方误差阵；Qk-1表示系统噪声方差阵，Pk|k-1表示一步预测均方误差阵。
4.根据权利要求1所述的基于最大熵平滑变结构滤波的DVL误差自适应标定方法，其特征在于，步骤5的具体方法为：
量测更新中状态估计为：
Figure QLYQS_36
均方误差阵更新为：
Pk＝(I-KkHk)Pk|k-1
其中，I表示单位矩阵。
CN202310079740.8A 2023-02-08 2023-02-08 基于最大熵平滑变结构滤波的dvl误差自适应标定方法 Active CN115790669B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202310079740.8A CN115790669B (zh) 2023-02-08 2023-02-08 基于最大熵平滑变结构滤波的dvl误差自适应标定方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202310079740.8A CN115790669B (zh) 2023-02-08 2023-02-08 基于最大熵平滑变结构滤波的dvl误差自适应标定方法

Publications (2)

Publication Number Publication Date
CN115790669A CN115790669A (zh) 2023-03-14
CN115790669B true CN115790669B (zh) 2023-04-28

Family

ID=85430467

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202310079740.8A Active CN115790669B (zh) 2023-02-08 2023-02-08 基于最大熵平滑变结构滤波的dvl误差自适应标定方法

Country Status (1)

Country Link
CN (1) CN115790669B (zh)

Citations (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN107797106A (zh) * 2017-05-08 2018-03-13 南京航空航天大学 一种加速em未知杂波估计的phd多目标跟踪平滑滤波方法
CN114909608A (zh) * 2022-05-27 2022-08-16 哈尔滨工程大学 一种基于mimu/里程轮/光电测速传感器组合的非开挖管道定位方法

Family Cites Families (6)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN110146077A (zh) * 2019-06-21 2019-08-20 台州知通科技有限公司 移动机器人姿态角解算方法
CN110375731A (zh) * 2019-07-01 2019-10-25 东南大学 一种混合交互式多模型滤波方法
CN111024064B (zh) * 2019-11-25 2021-10-19 东南大学 一种改进Sage-Husa自适应滤波的SINS/DVL组合导航方法
CN112230195B (zh) * 2020-09-02 2024-07-30 清华大学 基于非线性最优平滑层策略的平滑变结构滤波方法及系统
CN113376672A (zh) * 2021-06-07 2021-09-10 长春工业大学 基于模糊卡尔曼滤波的北斗伪距定位算法
CN115307643A (zh) * 2022-08-24 2022-11-08 东南大学 一种双应答器辅助的sins/usbl组合导航方法

Patent Citations (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN107797106A (zh) * 2017-05-08 2018-03-13 南京航空航天大学 一种加速em未知杂波估计的phd多目标跟踪平滑滤波方法
CN114909608A (zh) * 2022-05-27 2022-08-16 哈尔滨工程大学 一种基于mimu/里程轮/光电测速传感器组合的非开挖管道定位方法

Also Published As

Publication number Publication date
CN115790669A (zh) 2023-03-14

Similar Documents

Publication Publication Date Title
CN111024064B (zh) 一种改进Sage-Husa自适应滤波的SINS/DVL组合导航方法
CN103630137B (zh) 一种用于导航系统的姿态及航向角的校正方法
CN103389506B (zh) 一种用于捷联惯性/北斗卫星组合导航系统的自适应滤波方法
CN110823215B (zh) 一种无人机相对导航信息融合方法
CN113819906A (zh) 一种基于统计相似度量的组合导航鲁棒滤波方法
CN112284384B (zh) 考虑量测异常的集群式多深海潜航器的协同定位方法
CN105136145A (zh) 一种基于卡尔曼滤波的四旋翼无人机姿态数据融合的方法
CN112798021B (zh) 基于激光多普勒测速仪的惯导系统行进间初始对准方法
CN115855049B (zh) 基于粒子群优化鲁棒滤波的sins/dvl导航方法
CN108592943B (zh) 一种基于opreq方法的惯性系粗对准计算方法
CN104483973A (zh) 基于滑模观测器的低轨挠性卫星姿态跟踪控制方法
CN112504298A (zh) 一种gnss辅助的dvl误差标定方法
CN112197789B (zh) 一种基于quest的ins/dvl安装误差标定方法
CN112857398A (zh) 一种系泊状态下舰船的快速初始对准方法和装置
CN115307643A (zh) 一种双应答器辅助的sins/usbl组合导航方法
CA2699137A1 (en) Hybrid inertial system with non-linear behaviour and associated method of hybridization by multi-hypothesis filtering
CN113587926A (zh) 一种航天器空间自主交会对接相对导航方法
CN115979253A (zh) 一种基于抗差滤波的水下机器人多传感器紧组合导航方法
CN110595434B (zh) 基于mems传感器的四元数融合姿态估计方法
CN113434806A (zh) 一种抗差自适应多模型滤波方法
CN115790669B (zh) 基于最大熵平滑变结构滤波的dvl误差自适应标定方法
CN117419723A (zh) 一种基于因子图的交互模型自适应组合导航方法
CN110375773B (zh) Mems惯导系统姿态初始化方法
CN113008229A (zh) 一种基于低成本车载传感器的分布式自主组合导航方法
CN116907503A (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