CN115979253A - 一种基于抗差滤波的水下机器人多传感器紧组合导航方法 - Google Patents

一种基于抗差滤波的水下机器人多传感器紧组合导航方法 Download PDF

Info

Publication number
CN115979253A
CN115979253A CN202211516059.7A CN202211516059A CN115979253A CN 115979253 A CN115979253 A CN 115979253A CN 202211516059 A CN202211516059 A CN 202211516059A CN 115979253 A CN115979253 A CN 115979253A
Authority
CN
China
Prior art keywords
representing
measurement
tight
matrix
follows
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.)
Pending
Application number
CN202211516059.7A
Other languages
English (en)
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 CN202211516059.7A priority Critical patent/CN115979253A/zh
Publication of CN115979253A publication Critical patent/CN115979253A/zh
Pending legal-status Critical Current

Links

Images

Abstract

本发明公开了一种基于抗差滤波的水下机器人多传感器紧组合导航方法，首先，构建SINS‑DVL‑USBL紧组合模型，获取紧组合模型的状态方程和量测方程；其次，滤波状态更新，在状态方程的基础上计算一步预测向量和协方差矩阵，为量测更新提供协方差矩阵；然后，基于马氏距离的量测噪声估计，为量测更新提供量测噪声矩阵；最后，滤波量测更新，用DVL和USBL的信息修正SINS信息，获得导航结果。本发明能够有效消除野值，在减低运算量的情况下提高机器人导航的精度；获得复杂环境下水下机器人的高精度位置信息，进一步提高水下机器人的作业效率。

Description

一种基于抗差滤波的水下机器人多传感器紧组合导航方法
技术领域
本发明属于水下机器人导航定位技术，具体涉及一种基于抗差滤波的水下机器人多传感器紧组合导航方法。
背景技术
为了使得水下机器人在水下能够顺利作业，需要为水下机器人提供精确的导航信息。目前最常用的导航方法是捷联惯性导航系统(Strap-down Inertial NavigationSystem，SINS)，优点是在短时间内能够提供精确的位姿信息，自主性好且具有较强的隐蔽性，但SINS也存在一定的缺点，比如定位误差会随着时间积累、成本高等。为此，必须采用其他导航方式来修正SINS的累积误差，实现高精度组合导航。多普勒计程仪(Doppler logVelocity，DVL)是利用多普勒原理的测速传感器。基于SINS/DVL的组合导航方法能够有效修正SINS误差发散，大幅度提高导航性能。然而，针对长航时的SINS/DVL组合导航系统来说存在两个问题：一方面，由于DVL测量带有误差，因此长时间SINS/DVL组合仍然存在误差发散问题；另一方面，由于复杂的水下环境，DVL在测量过程中容易出现信号缺失，在信号缺失期间主要依赖惯性导航设备，会使系统精度快速下降。因此，需要其他能够提供绝对位置信息的导航设备。基于声学的导航系统是水下常用的导航方式。通常水声定位系统依据基线的长度可分为长基线、短基线和超短基线。由于超短基线(Ultra Short Base Line，USBL)的声基阵之间的距离小，使用方便易于安装，目前使用范围在逐渐加大，而长、短基线因为在安装和回收的过程比较繁琐限制其使用比例。综合以上分析可知，基于SINS-DVL-USBL的多传感器融合方法是水下机器人的主要导航方式。
本发明结合SINS、DVL、USBL提出一种基于紧组合模型的异步序贯抗差滤波方法。首先，基于DVL及USBL原始输出信息提出一种基于SINS/DVL/USBL的紧组合导航模型。建立紧组合模型对应的状态方程和量测方程，将状态量在传统15维的基础上增加DVL及USBL相关误差量进而扩充到20维；其次，为减少量测矩阵维度提高运算效率，引入异步序贯滤波技术提出一种基于异步序贯抗差滤波方法。该方法利用马氏距离构造抗差卡尔曼滤波算法，通过统计学特性利用假设检验探测出野值。
发明内容
发明目的：本发明提供一种基于抗差滤波的水下机器人多传感器紧组合导航方法，针对量测信息野值问题，在异步序贯滤波的基础上引入马氏距离，提高了滤波的鲁棒性。
技术方案：本发明提供一种基于抗差滤波的水下机器人多传感器紧组合导航方法，包括以下步骤：
(1)构建SINS-DVL-USBL紧组合模型，获取紧组合模型的状态方程和量测方程；
(2)滤波状态更新：在状态方程的基础上计算一步预测向量和协方差矩阵，为量测更新提供协方差矩阵；
(3)基于马氏距离的量测噪声估计，为量测更新提供量测噪声矩阵；
(4)滤波量测更新：用DVL和USBL的信息修正SINS信息，获得导航结果。
进一步地，所述步骤(1)实现过程如下：
构建紧组合模型的状态方程：
Figure BDA0003971967150000021
其中，Xtight表示状态向量；Ftight表示紧组合模型状态转移矩阵；Gtight表示系统噪声输入矩阵；Wtight表示紧组合模型噪声向量；状态向量Xtight表示如下：
Figure BDA0003971967150000022
其中，
Figure BDA0003971967150000023
分别表示东向、北向和天向速度误差；φE、φN、φU表示俯仰角、横滚角和航向角误差；δL、δλ、δh分别表示纬度、经度和高度误差；
Figure BDA0003971967150000024
分别表示三个方向的陀螺常值零偏；
Figure BDA0003971967150000025
分别表示三个方向的加表常值零偏；δθx，δθy，δθz分别表示USBL三个方向的安装误差角；δD表示USBL的斜距常值误差；δK表示DVL刻度因子误差；
紧组合模型状态转移矩阵Ftight表示如下：
Figure BDA0003971967150000031
在量测方程构建方面，以方位角、斜距、深度、速度作为量测信息，其量测方程为：
Figure BDA0003971967150000032
其中，Ztight表示量测矩阵；Htight表示紧组合模型量测转移矩阵；Vtight表示量测噪声向量；
Figure BDA0003971967150000033
表示USBL方位角；
Figure BDA0003971967150000034
表示斜距；haU表示高度；
Figure BDA0003971967150000035
Figure BDA0003971967150000036
分别表示DVL输出的四个通道方向的速度信息；
基于SINS-DVL-USBL的紧组合模型的量测转移矩阵Htight由涉及USBL量测转移矩阵和涉及DVL量测转移矩阵两部分组成，具体如下：
Figure BDA0003971967150000037
其中涉及USBL部分的量测转移矩阵HUSBL为：
Figure BDA0003971967150000038
其中，Hh和Hn表示如下：
Figure BDA0003971967150000039
其中，Hr＝[0 0 -1]T；
Ha、Hp和Hs表示如下：
Figure BDA00039719671500000310
其中，
Figure BDA00039719671500000311
表示载体系到声基阵坐标系的方向余弦矩阵；
Figure BDA00039719671500000312
表示地球系到导航系得方向余弦矩阵；
Figure BDA00039719671500000313
表示声基阵坐标系到载体坐标系的杆臂值在载体系的投影；
Figure BDA0003971967150000041
表示载体系到导航坐标系的方向余弦矩阵
Figure BDA0003971967150000042
表示SINS解算得到的载体位置和应答器的坐标在声基阵坐标系a系下的相对位置；
Figure BDA0003971967150000043
表示声基阵坐标系到载体坐标系的杆臂值在声基阵坐标系的投影；
DVL相关的量测转移矩阵HDVL：
Figure BDA0003971967150000044
其中，Hu和Hv表示如下：
Figure BDA0003971967150000045
其中，
Figure BDA0003971967150000046
表示载体系到DVL设备系的方向余弦矩阵；δθ表示安装误差角；
Figure BDA0003971967150000047
表示导航系到载体系的方向余弦矩阵；
Figure BDA0003971967150000048
表示SINS解算得到的导航系下的速度信息；
根据序贯滤波思想，在进行滤波前将k时刻的量测方程分解成如下8组：
Figure BDA0003971967150000049
其中，
Figure BDA00039719671500000410
表示k时刻子量测转移矩阵；
Figure BDA00039719671500000411
表示k时刻子量测向量；
Figure BDA00039719671500000412
表示k时刻子量测噪声向量。
进一步地，所述步骤(2)实现过程如下：
状态一步预测：
Figure BDA00039719671500000413
状态一步预测误差协方差矩阵：
Figure BDA00039719671500000414
其中，Fk-1,tight表示紧组合模型状态一步转移矩阵；
Figure BDA00039719671500000415
表示状态向量估计值；Pkk-1表示状态一步预测误差协方差矩阵；Qk-1表示系统噪声协方差矩阵。
进一步地，所述步骤(3)实现过程如下：
通过k时刻的量测向量和量测方程得到信息向量表示如下：
Figure BDA0003971967150000051
其中N＝1,…,8；
量测信息的概率密度函数为：
Figure BDA0003971967150000052
其中，m＝8表示量测信息的维数；判断量测信息是否含有野值的公式如下：
Figure BDA0003971967150000053
其中，Mk表示马氏距离；如果量测信息不存在野值，Mk参数应该服从自由度为m的χ2分布；根据给定的显著水平α对Mk进行χ2检验，则事件发生概率为：
Figure BDA0003971967150000054
其中，
Figure BDA0003971967150000055
表示显著性水平α对应的上分位点；P发生的概率很小，如果发生，即认为量测信息受到野值干扰，由此实现对野值的检测；当量测信息出现野值时，引入抗差调节因子λk，调节量测噪声协方差矩阵
Figure BDA0003971967150000056
Figure BDA0003971967150000057
其中抗差因子定义如下：
Figure BDA0003971967150000058
当量测信息存在野值时通过抗差因子调节量测噪声协方差矩阵以提高系统的抗差特性；当量测信息正常时，抗差因子为1进行正常的异步序贯卡尔曼滤波更新。
进一步地，所述步骤(4)实现过程如下：
计算k时刻子量测更新的滤波增益：
Figure BDA0003971967150000061
其中，
Figure BDA0003971967150000062
表示k时刻的子量测更新滤波增益；
Figure BDA0003971967150000063
表示k时刻误差协方差矩阵；
Figure BDA0003971967150000064
表示量测噪声协方差矩阵；
Figure BDA0003971967150000065
其中，
Figure BDA0003971967150000066
表示k时刻子状态更新状态向量；
Figure BDA0003971967150000067
表示k时刻子量测向量；k时刻的误差协方差矩阵更新如下：
Figure BDA0003971967150000068
有益效果：与现有技术相比，本发明的有益效果：本发明通过对DVL及USBL原始信息处理，设计了基于USBL方位角、斜距以及DVL四通道速度信息的SINS-DVL-USBL紧组合导航模型；为减少量测矩阵维度提高运算效率，引入异步序贯滤波技术提出一种基于异步序贯抗差滤波方法；本发明针对量测信息野值问题，在异步序贯滤波的基础上引入马氏距离，进一步提高了滤波的鲁棒性。
附图说明
图1为本发明的流程图；
图2SINS、DVL以及USBL数据异步量测更新示意图；
图3为载体仿真轨迹图；
图4为东、北和天向三个方向位置误差曲线图；
图5为水平位置误差曲线图。
具体实施方式
下面结合附图对本发明作进一步详细说明。
本发明提供一种基于抗差滤波的水下机器人多传感器紧组合导航方法，如图1所示，具体实现过程如下：
步骤1：构建SINS-DVL-USBL紧组合模型。
坐标系定义如下：a系：声基阵坐标系；b系：载体坐标系；n系：导航坐标系；d系：DVL设备坐标系。
捷联惯性导航系统(Strap-down Inertial Navigation System，SINS)输出信息包括速度
Figure BDA0003971967150000069
位置：纬度L、经度λ和高度h。
超短基线(Ultra Short Base Line，USBL)设备输出信息包括两个方位角：
Figure BDA0003971967150000071
Figure BDA0003971967150000072
斜距信息：
Figure BDA0003971967150000073
高度信息：haU。
多普勒计程仪(Doppler log Velocity，DVL)设备输出信息包括四个通道方向的速度信息：
Figure BDA0003971967150000074
构建紧组合模型的状态方程：
Figure BDA0003971967150000075
其中，Xtight表示状态向量；Ftight表示紧组合模型状态转移矩阵；Gtight表示系统噪声输入矩阵；Wtight表示系统噪声向量。
状态向量Xtight表示如下：
Figure BDA0003971967150000076
其中，
Figure BDA0003971967150000077
分别表示东向、北向和天向速度误差；φE、φN、φU表示俯仰角、横滚角和航向角误差；δL、δλ、δh分别表示纬度、经度和高度误差；
Figure BDA0003971967150000078
分别表示三个方向的陀螺常值零偏；
Figure BDA0003971967150000079
分别表示三个方向的加表常值零偏；δθx，δθy，δθz分别表示USBL三个方向的安装误差角；δD表示USBL的斜距常值误差；δK表示DVL刻度因子误差。
紧组合的状态向量是在传统15维松组合状态向量的基础上增加了USBL的安装误差、斜距常值误差和DVL刻度因子误差。考虑到增加的五个状态量都为小量且可认为是常值。因此，其微分为0。紧组合模型状态转移矩阵Ftight可表示如下：
Figure BDA00039719671500000710
在量测方程构建方面，以方位角、斜距、深度、速度作为量测信息。其量测方程可表示如下：
Figure BDA0003971967150000081
其中，Ztight表示量测矩阵；Htight表示紧组合模型量测转移矩阵；Vtight表示量测噪声向量；
Figure BDA0003971967150000082
表示USBL方位角；
Figure BDA0003971967150000083
表示斜距；haU表示高度；
Figure BDA0003971967150000084
Figure BDA0003971967150000085
分别表示DVL输出的四个通道方向的速度信息。
基于SINS-DVL-USBL的紧组合模型的量测转移矩阵Htight包括涉及USBL量测转移矩阵和涉及DVL量测转移矩阵两部分组成，具体如下：
Figure BDA0003971967150000086
其中涉及USBL部分的量测转移矩阵HUSBL可表示为：
Figure BDA0003971967150000087
其中，Hh和Hn表示如下：
Figure BDA0003971967150000088
其中，Hr＝[0 0 -1]T。
Ha、Hp和Hs可表示如下：
Figure BDA0003971967150000089
其中，
Figure BDA00039719671500000810
表示载体系到声基阵坐标系的方向余弦矩阵；
Figure BDA00039719671500000811
表示地球系到导航系得方向余弦矩阵。
Figure BDA00039719671500000812
表示声基阵坐标系到载体坐标系的杆臂值在载体系的投影。
Figure BDA00039719671500000813
表示载体系到导航坐标系的方向余弦矩阵。
Figure BDA00039719671500000814
表示SINS解算得到的载体位置和应答器的坐标在声基阵坐标系a系下的相对位置；
Figure BDA00039719671500000815
表示声基阵坐标系到载体坐标系的杆臂值在声基阵坐标系的投影。
根据上述公式以及状态量可以得到DVL相关的量测转移矩阵HDVL：
Figure BDA0003971967150000091
其中，Hu和Hv表示如下：
Figure BDA0003971967150000092
其中，
Figure BDA0003971967150000093
表示载体系到DVL设备系的方向余弦矩阵；δθ表示安装误差角；
Figure BDA0003971967150000094
表示导航系到载体系的方向余弦矩阵；
Figure BDA0003971967150000095
表示SINS解算得到的导航系下的速度信息。
根据序贯滤波思想，在进行滤波前将k时刻的量测方程可以分解成如下8组：
Figure BDA0003971967150000096
其中，
Figure BDA0003971967150000097
表示k时刻子量测转移矩阵；
Figure BDA0003971967150000098
表示k时刻子量测向量。
Figure BDA0003971967150000099
表示k时刻子量测噪声向量。
步骤2：滤波状态更新：在状态方程的基础上计算一步预测向量和协方差矩阵，为量测更新提供协方差矩阵。
状态一步预测：
Figure BDA00039719671500000910
状态一步预测误差协方差矩阵：
Figure BDA00039719671500000911
其中，Fk-1,tight表示紧组合模型状态一步转移矩阵；
Figure BDA00039719671500000912
表示状态向量估计值。Pkk-1表示状态一步预测误差协方差矩阵；Qk-1表示系统噪声协方差矩阵。
步骤3：基于马氏距离的量测噪声估计。
通过上述计算新息向量可表示如下：
Figure BDA0003971967150000101
其中N＝1,…,8。
量测信息的概率密度函数可表示为：
Figure BDA0003971967150000102
其中，m＝8表示量测信息的维数；根据上式定义判断量测信息是否含有野值的公式如下：
Figure BDA0003971967150000103
其中，Mk表示马氏距离；如果量测信息不存在野值，Mk参数应该服从自由度为m的χ2分布。根据给定的显著水平α对Mk进行χ2检验，则事件发生概率为：
Figure BDA0003971967150000104
其中
Figure BDA0003971967150000105
表示显著性水平α对应的上分位点；P发生的概率很小，如果发生，即认为量测信息受到野值干扰，由此实现对野值的检测。当量测信息出现野值时，引入抗差调节因子λk，调节量测噪声协方差矩阵
Figure BDA0003971967150000106
Figure BDA0003971967150000107
其中抗差因子定义如下：
Figure BDA0003971967150000108
由以上分析可知，当量测信息存在野值时可通过抗差因子调节量测噪声协方差矩阵以提高系统的抗差特性；当量测信息正常时，抗差因子为1进行正常的异步序贯卡尔曼滤波更新。
步骤4：滤波量测更新：用DVL和USBL的信息修正SINS信息，获得导航结果。
计算k时刻子量测更新的滤波增益：
Figure BDA0003971967150000111
其中，
Figure BDA0003971967150000112
表示k时刻的子量测更新滤波增益。
Figure BDA0003971967150000113
表示k时刻误差协方差矩阵。
Figure BDA0003971967150000114
表示量测噪声协方差矩阵。
Figure BDA0003971967150000115
其中，
Figure BDA0003971967150000116
表示k时刻子状态更新状态向量；
Figure BDA0003971967150000117
表示k时刻子量测向量。k时刻误差协方差矩阵更新如下：
Figure BDA0003971967150000118
按照上述计算流程SINS、DVL以及USBL数据异步量测更新步骤如下，具体流程如图2所示：
1)首先接收到IMU信息(200Hz)，此时进行滤波状态更新(状态更新1)；
2)接收到DVL信息(2Hz)，此时进行序贯量测更新(序贯量测更新1)；
3)将步骤2序贯量测更新后的状态量同步到新的状态更新中(状态更新2)；
4)收到USBL信息(周期6s)，此时进行序贯量测更新(序贯量测更新2)；
5)将步骤4)序贯量测更新后的状态量同步到新的状态更新中(状态更新3)；
6)执行状态更新3等新的DVL数据到来。
基于SINS-DVL-USBL组合导航仿真参数如下：陀螺零偏：0.01°/h；加表零偏：50ug；SINS更新频率：200Hz。USBL安装误差：θab＝[0.2°0.2°0.5°]T；USBL侧向误差：0.5°；测时误差：0.5ms；数据更新周期：3s；深度计误差：0.5m。DVL安装误差：[0.1°0.1°0.2°]T；DVL刻度因子系数：0.003；输出频率：2Hz。按照上述仿真条件可得到载体仿真轨迹如图3所示。在图3的基础上分别使用传统的序贯卡卡尔曼滤波算法与本发明的方法开展SINS-DVL-USBL组合导航仿真验证，结果如图4和图5所示。图4是东、北和天向三个方向位置误差曲线，与传统的序贯卡卡尔曼滤波算法相比，本发明的方法位置误差更小；图5是在图4的基础上计算的水平位置误差曲线，更能够说明本发明方法的位置误差小。

Claims (5)

1.一种基于抗差滤波的水下机器人多传感器紧组合导航方法，其特征在于，包括以下步骤：
(1)构建SINS-DVL-USBL紧组合模型，获取紧组合模型的状态方程和量测方程；
(2)滤波状态更新：在状态方程的基础上计算一步预测向量和协方差矩阵，为量测更新提供协方差矩阵；
(3)基于马氏距离的量测噪声估计，为量测更新提供量测噪声矩阵；
(4)滤波量测更新：用DVL和USBL的信息修正SINS信息，获得导航结果。
2.根据权利要求1所述的一种基于抗差滤波的水下机器人多传感器紧组合导航方法，其特征在于，所述步骤(1)实现过程如下：
构建紧组合模型的状态方程：
Figure FDA0003971967140000011
其中，Xtight表示状态向量；Ftight表示紧组合模型状态转移矩阵；Gtight表示系统噪声输入矩阵；Wtight表示紧组合模型噪声向量；状态向量Xtight表示如下：
Figure FDA0003971967140000012
其中，
Figure FDA0003971967140000013
分别表示东向、北向和天向速度误差；φE、φN、φU表示俯仰角、横滚角和航向角误差；δL、δλ、δh分别表示纬度、经度和高度误差；
Figure FDA0003971967140000014
分别表示三个方向的陀螺常值零偏；
Figure FDA0003971967140000015
分别表示三个方向的加表常值零偏；δθx，δθy，δθz分别表示USBL三个方向的安装误差角；δD表示USBL的斜距常值误差；δK表示DVL刻度因子误差；
紧组合模型状态转移矩阵Ftight表示如下：
Figure FDA0003971967140000016
在量测方程构建方面，以方位角、斜距、深度、速度作为量测信息，其量测方程为：
Figure FDA0003971967140000021
其中，Ztight表示量测矩阵；Htight表示紧组合模型量测转移矩阵；Vtight表示量测噪声向量；
Figure FDA0003971967140000022
表示USBL方位角；
Figure FDA0003971967140000023
表示斜距；haU表示高度；
Figure FDA0003971967140000024
Figure FDA0003971967140000025
分别表示DVL输出的四个通道方向的速度信息；
基于SINS-DVL-USBL的紧组合模型的量测转移矩阵Htight由涉及USBL量测转移矩阵和涉及DVL量测转移矩阵两部分组成，具体如下：
Figure FDA0003971967140000026
其中涉及USBL部分的量测转移矩阵HUSBL为：
Figure FDA0003971967140000027
其中，Hh和Hn表示如下：
Figure FDA0003971967140000028
其中，Hr＝[0 0 -1]T；
Ha、Hp和Hs表示如下：
Figure FDA0003971967140000029
其中，
Figure FDA00039719671400000210
表示载体系到声基阵坐标系的方向余弦矩阵；
Figure FDA00039719671400000211
表示地球系到导航系得方向余弦矩阵；
Figure FDA00039719671400000212
表示声基阵坐标系到载体坐标系的杆臂值在载体系的投影；
Figure FDA00039719671400000213
表示载体系到导航坐标系的方向余弦矩阵
Figure FDA00039719671400000214
表示SINS解算得到的载体位置和应答器的坐标在声基阵坐标系a系下的相对位置；
Figure FDA00039719671400000215
表示声基阵坐标系到载体坐标系的杆臂值在声基阵坐标系的投影；
DVL相关的量测转移矩阵HDVL：
Figure FDA0003971967140000031
其中，Hu和Hv表示如下：
Figure FDA0003971967140000032
其中，
Figure FDA0003971967140000033
表示载体系到DVL设备系的方向余弦矩阵；δθ表示安装误差角；
Figure FDA0003971967140000034
表示导航系到载体系的方向余弦矩阵；
Figure FDA0003971967140000035
表示SINS解算得到的导航系下的速度信息；
根据序贯滤波思想，在进行滤波前将k时刻的量测方程分解成如下8组：
Figure FDA0003971967140000036
其中，
Figure FDA0003971967140000037
表示k时刻子量测转移矩阵；
Figure FDA0003971967140000038
表示k时刻子量测向量；
Figure FDA0003971967140000039
表示k时刻子量测噪声向量。
3.根据权利要求1所述的一种基于抗差滤波的水下机器人多传感器紧组合导航方法，其特征在于，所述步骤(2)实现过程如下：
状态一步预测：
Figure FDA00039719671400000310
状态一步预测误差协方差矩阵：
Figure FDA00039719671400000311
其中，Fk-1,tight表示紧组合模型状态一步转移矩阵；
Figure FDA00039719671400000312
表示状态向量估计值；Pkk-1表示状态一步预测误差协方差矩阵；Qk-1表示系统噪声协方差矩阵。
4.根据权利要求1所述的一种基于抗差滤波的水下机器人多传感器紧组合导航方法，其特征在于，所述步骤(3)实现过程如下：
通过k时刻的量测向量和量测方程得到信息向量表示如下：
Figure FDA0003971967140000041
其中N＝1,…,8；
量测信息的概率密度函数为：
Figure FDA0003971967140000042
其中，m＝8表示量测信息的维数；判断量测信息是否含有野值的公式如下：
Figure FDA0003971967140000043
其中，Mk表示马氏距离；如果量测信息不存在野值，Mk参数应该服从自由度为m的χ2分布；根据给定的显著水平α对Mk进行χ2检验，则事件发生概率为：
Figure FDA0003971967140000044
其中，
Figure FDA0003971967140000045
表示显著性水平α对应的上分位点；P发生的概率很小，如果发生，即认为量测信息受到野值干扰，由此实现对野值的检测；当量测信息出现野值时，引入抗差调节因子λk，调节量测噪声协方差矩阵
Figure FDA0003971967140000046
Figure FDA0003971967140000047
其中抗差因子定义如下：
Figure FDA0003971967140000048
当量测信息存在野值时通过抗差因子调节量测噪声协方差矩阵以提高系统的抗差特性；当量测信息正常时，抗差因子为1进行正常的异步序贯卡尔曼滤波更新。
5.根据权利要求1所述的一种基于抗差滤波的水下机器人多传感器紧组合导航方法，其特征在于，所述步骤(4)实现过程如下：
计算k时刻子量测更新的滤波增益：
Figure FDA0003971967140000049
其中，
Figure FDA00039719671400000410
表示k时刻的子量测更新滤波增益；
Figure FDA00039719671400000411
表示k时刻误差协方差矩阵；
Figure FDA00039719671400000412
表示量测噪声协方差矩阵；
Figure FDA0003971967140000051
其中，
Figure FDA0003971967140000052
表示k时刻子状态更新状态向量；
Figure FDA0003971967140000053
表示k时刻子量测向量；k时刻误差协方差矩阵更新如下：
Figure FDA0003971967140000054
CN202211516059.7A 2022-11-30 2022-11-30 一种基于抗差滤波的水下机器人多传感器紧组合导航方法 Pending CN115979253A (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202211516059.7A CN115979253A (zh) 2022-11-30 2022-11-30 一种基于抗差滤波的水下机器人多传感器紧组合导航方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202211516059.7A CN115979253A (zh) 2022-11-30 2022-11-30 一种基于抗差滤波的水下机器人多传感器紧组合导航方法

Publications (1)

Publication Number Publication Date
CN115979253A true CN115979253A (zh) 2023-04-18

Family

ID=85958323

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202211516059.7A Pending CN115979253A (zh) 2022-11-30 2022-11-30 一种基于抗差滤波的水下机器人多传感器紧组合导航方法

Country Status (1)

Country Link
CN (1) CN115979253A (zh)

Cited By (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN116222578A (zh) * 2023-05-04 2023-06-06 山东大学 基于自适应滤波和最优平滑的水下组合导航方法及系统
CN116625360A (zh) * 2023-07-19 2023-08-22 苏州精源创智能科技有限公司 一种基于强跟踪模型的扫地机打滑识别和修正方法

Cited By (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN116222578A (zh) * 2023-05-04 2023-06-06 山东大学 基于自适应滤波和最优平滑的水下组合导航方法及系统
CN116222578B (zh) * 2023-05-04 2023-08-29 山东大学 基于自适应滤波和最优平滑的水下组合导航方法及系统
CN116625360A (zh) * 2023-07-19 2023-08-22 苏州精源创智能科技有限公司 一种基于强跟踪模型的扫地机打滑识别和修正方法
CN116625360B (zh) * 2023-07-19 2023-09-29 苏州精源创智能科技有限公司 一种基于强跟踪模型的扫地机打滑识别和修正方法

Similar Documents

Publication Publication Date Title
CN109324330B (zh) 基于混合无导数扩展卡尔曼滤波的usbl/sins紧组合导航定位方法
CN109724599B (zh) 一种抗野值的鲁棒卡尔曼滤波sins/dvl组合导航方法
CN115979253A (zh) 一种基于抗差滤波的水下机器人多传感器紧组合导航方法
CN110567454B (zh) 一种复杂环境下sins/dvl紧组合导航方法
CN110542438B (zh) 一种基于sins/dvl组合导航误差标定的方法
CN111024064A (zh) 一种改进Sage-Husa自适应滤波的SINS/DVL组合导航方法
CN101561496A (zh) 一种伪卫星和惯性组合导航系统的非线性补偿方法
CN112798021B (zh) 基于激光多普勒测速仪的惯导系统行进间初始对准方法
CN112504298A (zh) 一种gnss辅助的dvl误差标定方法
CN115096303B (zh) 一种gnss多天线与ins紧组合定位定姿方法和设备
CN110763872A (zh) 一种多普勒测速仪多参数在线标定方法
CN115307643A (zh) 一种双应答器辅助的sins/usbl组合导航方法
CN111323012A (zh) 载体高动态环境下的ins辅助dvl测速误差补偿方法
CN110207698B (zh) 一种极区格网惯导/超短基线紧组合导航方法
CN115388884A (zh) 一种智能体位姿估计器联合初始化方法
CN117053782A (zh) 一种水陆两栖机器人组合导航方法
CN111397603A (zh) 载体姿态动态情况下的惯性/多普勒动基座粗对准方法
CN114265047A (zh) 一种大潜深auv的定位基阵联合标校方法
CN116358566B (zh) 一种基于抗差自适应因子的粗差探测组合导航方法
CN112083425A (zh) 一种引入径向速度的sins/lbl紧组合导航方法
CN116576849A (zh) 一种基于gmm辅助的车辆融合定位方法及系统
CN110873813A (zh) 一种水流速度估算方法、组合导航方法及装置
CN114111840B (zh) 一种基于组合导航的dvl误差参数在线标定方法
CN112987054B (zh) 标定sins/dvl组合导航系统误差的方法和装置
CN113821766B (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