CN115790669A - 基于最大熵平滑变结构滤波的dvl误差自适应标定方法 - Google Patents
基于最大熵平滑变结构滤波的dvl误差自适应标定方法 Download PDFInfo
- Publication number
- CN115790669A CN115790669A CN202310079740.8A CN202310079740A CN115790669A CN 115790669 A CN115790669 A CN 115790669A CN 202310079740 A CN202310079740 A CN 202310079740A CN 115790669 A CN115790669 A CN 115790669A
- Authority
- CN
- China
- Prior art keywords
- dvl
- representing
- error
- matrix
- measurement
- 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
Links
- 238000000034 method Methods 0.000 title claims abstract description 55
- 238000001914 filtration Methods 0.000 title claims abstract description 38
- 238000005259 measurement Methods 0.000 claims abstract description 76
- 238000004364 calculation method Methods 0.000 claims abstract description 12
- 238000010276 construction Methods 0.000 claims abstract description 8
- 239000011159 matrix material Substances 0.000 claims description 74
- 230000006870 function Effects 0.000 claims description 40
- 230000004927 fusion Effects 0.000 claims description 20
- 238000009434 installation Methods 0.000 claims description 20
- 230000007704 transition Effects 0.000 claims description 11
- 230000003044 adaptive effect Effects 0.000 claims description 8
- 238000012546 transfer Methods 0.000 claims description 7
- 230000008569 process Effects 0.000 claims description 4
- 238000006243 chemical reaction Methods 0.000 claims description 3
- 238000009499 grossing Methods 0.000 claims description 2
- 101100218330 Pseudomonas sp. (strain ADP) atzA gene Proteins 0.000 claims 1
- 238000004422 calculation algorithm Methods 0.000 description 7
- 238000012545 processing Methods 0.000 description 2
- 238000004088 simulation Methods 0.000 description 2
- 241000786798 Atya Species 0.000 description 1
- 230000002159 abnormal effect Effects 0.000 description 1
- 238000011161 development Methods 0.000 description 1
- 238000010586 diagram Methods 0.000 description 1
- 238000005516 engineering process Methods 0.000 description 1
- 238000011478 gradient descent method Methods 0.000 description 1
- 238000012986 modification Methods 0.000 description 1
- 230000004048 modification Effects 0.000 description 1
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误差标定是实现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,测量得到的设备系下的三维速度信息,其中分别表示设备坐标系下x方向的速度信息,设备坐标系下y方向的速度信息以及设备坐标系下z方向的速度信息,上标T表示矩阵的转置;
所述标定方法包括如下步骤:
步骤1. DVL误差标定空间模型构建,空间模型包括状态方程构建和量测方程构建;
步骤2. 进行滤波初始化,即状态更新;
步骤3. 基于改进最大熵原理进行量测噪声估计,所述改进最大熵原理是指把高斯核函数和多项式核函数取长补短结合起来构成混合核函数计算权重;
步骤4. 引入平滑变结构原理,实现对卡尔曼滤波增益的计算;
步骤5. 状态估计及均方误差计算。
进一步地,步骤1的具体方法为:
首先建立状态方程:
其中,表示标定模型状态转移矩阵;表示标定模型***噪声;表示标定模型状态向量,表示对求一次导数,选取惯性测量单元和DVL安装误差角、DVL刻度因子误差以及惯性测量单元和DVL的杆臂误差作为标定模型状态向量,即:
其中,表示惯性测量单元和DVL在x方向的安装误差角;表示惯性测量单元和DVL在y方向的安装误差角;表示惯性测量单元和DVL在z方向的安装误差角;表示DVL刻度因子误差;表示惯性测量单元和DVL在x方向的杆臂误差;表示惯性测量单元和DVL在y方向的杆臂误差;表示惯性测量单元和DVL在z方向的杆臂误差;其中:
由于惯性测量单元和DVL的安装误差角,DVL刻度因子误差以及惯性测量单元和DVL的杆臂误差都为常值,所以其微分方程均为零,因此标定模型状态转移矩阵可表示如下:
接着,建立量测方程,其量测方程表示如下:
根据上述过程,可得标定模型量测转移矩阵:
其中,表示惯导输出的x方向的角速度信息;表示惯导输出的y方向的角速度信息;表示惯导输出的z方向的角速度信息;表示载体坐标系下SINS/GNSS融合得到的x方向的速度信息;表示载体坐标系下SINS/GNSS融合得到的y方向的速度信息;表示载体坐标系下SINS/GNSS融合得到的z方向的速度信息,即:
进一步地,步骤2的具体方法为:
状态估计
状态估计均方误差阵:
进一步地,步骤3的具体方法为:
计算修正后的量测噪声矩阵为:
进一步地,步骤4的具体方法为:
进一步地,步骤5的具体方法为:
量测更新中状态估计为:
均方误差阵更新为:
与现有技术相比,本发明具有如下优点:
(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,测量得到的设备系下的三维速度信息,其中分别表示设备坐标系下x方向的速度信息,设备坐标系下y方向的速度信息以及设备坐标系下z方向的速度信息,上标T表示矩阵的转置;
如图1所示,本发明所述标定方法包括如下步骤:
步骤1. DVL误差标定空间模型构建,空间模型包括状态方程构建和量测方程构建;
首先建立状态方程:
其中,表示标定模型状态转移矩阵;表示标定模型***噪声;表示标定模型状态向量,表示对求一次导数,选取惯性测量单元和DVL安装误差角、DVL刻度因子误差以及惯性测量单元和DVL的杆臂误差作为标定模型状态向量,即:
其中,表示惯性测量单元和DVL在x方向的安装误差角;表示惯性测量单元和DVL在y方向的安装误差角;表示惯性测量单元和DVL在z方向的安装误差角;表示DVL刻度因子误差;表示惯性测量单元和DVL在x方向的杆臂误差;表示惯性测量单元和DVL在y方向的杆臂误差;表示惯性测量单元和DVL在z方向的杆臂误差;其中:
由于惯性测量单元和DVL的安装误差角,DVL刻度因子误差以及惯性测量单元和DVL的杆臂误差都为常值,所以其微分方程均为零,因此标定模型状态转移矩阵可表示如下:
接着,建立量测方程,其量测方程表示如下:
根据上述过程,可得标定模型量测转移矩阵:
其中,表示惯导输出的x方向的角速度信息;表示惯导输出的y方向的角速度信息;表示惯导输出的z方向的角速度信息;表示载体坐标系下SINS/GNSS融合得到的x方向的速度信息;表示载体坐标系下SINS/GNSS融合得到的y方向的速度信息;表示载体坐标系下SINS/GNSS融合得到的z方向的速度信息,即:
以上即为本发明提出的标定空间模型。
步骤2. 进行滤波初始化,即状态更新;
状态估计
状态估计均方误差阵:
步骤3. 基于改进最大熵原理进行量测噪声估计,所述改进最大熵原理是指把高斯核函数和多项式核函数取长补短结合起来构成混合核函数计算权重;
传统的最大熵原理中,采用高斯核函数计算权重。高斯核函数作为局部性核函数具有学习能力强、泛化性能较弱的特点。而多项式核函数作为全局性核函数具有泛化性能强、学习能力较弱的特点。因此本发明把高斯核函数和多项式核函数取长补短结合起来构成混合核函数:
计算修正后的量测噪声矩阵为:
步骤4. 引入平滑变结构原理,实现对卡尔曼滤波增益的计算;
为提高算法的鲁棒性,本发明对传统卡尔曼滤波增益计算进行修改。通过引入平滑变结构原理,实现对卡尔曼滤波增益的重新设计,进一步提高算法的鲁棒性。根据平滑变结构原理和标定空间模型计算时变平滑边界层,如下式所示:
步骤5. 状态估计及均方误差计算。
量测更新中状态估计为:
均方误差阵更新为:
仿真参数设置如下:
DVL输出频率2Hz,DVL刻度因子误差为0.002;IMU与DVL之间的安装误差设置为2°,-1°,1°;IMU与DVL的杆臂误差设置为1m,-1m,0.5m。其中图2为仿真结果,图标显示了卡尔曼滤波方法和本发明方法的安装误差角估计结果,从图2中可以看出,本发明方法更接近真值。图3给出了两种方法对应的DVL安装误差估计结果,从图中可以看出本发明方法精度优于卡尔曼滤波方法。图4给出了两种方法对应的杆臂误差估计曲线,从图中可以看出,本发明方法在杆臂估计精度方面优于卡尔曼滤波方法。
应理解这些实施例仅用于说明本发明而不用于限制本发明的范围,在阅读了本发明之后,本领域普通技术人员对本发明的各种等价形式的修改均落于本申请所附权利要求所限定的范围。
Claims (6)
1.一种基于最大熵平滑变结构滤波的DVL误差自适应标定方法,该方法首先已知:
其特征在于,所述标定方法包括如下步骤:
步骤1. DVL误差标定空间模型构建,空间模型包括状态方程构建和量测方程构建;
步骤2. 进行滤波初始化,即状态更新;
步骤3. 基于改进最大熵原理进行量测噪声估计,所述改进最大熵原理是指把高斯核函数和多项式核函数取长补短结合起来构成混合核函数计算权重;
步骤4. 引入平滑变结构原理,实现对卡尔曼滤波增益的计算;
步骤5. 状态估计及均方误差计算。
2.根据权利要求1所述的基于最大熵平滑变结构滤波的DVL误差自适应标定方法,其特征在于,步骤1的具体方法为:
首先建立状态方程:
其中,表示标定模型状态转移矩阵;表示标定模型***噪声;表示标定模型状态向量,表示对求一次导数,选取惯性测量单元和DVL安装误差角、DVL刻度因子误差以及惯性测量单元和DVL的杆臂误差作为标定模型状态向量,即:
其中,表示惯性测量单元和DVL在x方向的安装误差角;表示惯性测量单元和DVL在y方向的安装误差角;表示惯性测量单元和DVL在z方向的安装误差角;表示DVL刻度因子误差;表示惯性测量单元和DVL在x方向的杆臂误差;表示惯性测量单元和DVL在y方向的杆臂误差;表示惯性测量单元和DVL在z方向的杆臂误差;其中:
由于惯性测量单元和DVL的安装误差角,DVL刻度因子误差以及惯性测量单元和DVL的杆臂误差都为常值,所以其微分方程均为零,因此标定模型状态转移矩阵可表示如下:
接着,建立量测方程,其量测方程表示如下:
根据上述过程,可得标定模型量测转移矩阵:
其中,表示惯导输出的x方向的角速度信息;表示惯导输出的y方向的角速度信息;表示惯导输出的z方向的角速度信息;表示载体坐标系下SINS/GNSS融合得到的x方向的速度信息;表示载体坐标系下SINS/GNSS融合得到的y方向的速度信息;表示载体坐标系下SINS/GNSS融合得到的z方向的速度信息,即:
4.根据权利要求3所述的基于最大熵平滑变结构滤波的DVL误差自适应标定方法,其特征在于,步骤3的具体方法为:
计算修正后的量测噪声矩阵为:
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 true CN115790669A (zh) | 2023-03-14 |
CN115790669B 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 (8)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN107797106A (zh) * | 2017-05-08 | 2018-03-13 | 南京航空航天大学 | 一种加速em未知杂波估计的phd多目标跟踪平滑滤波方法 |
CN110375731A (zh) * | 2019-07-01 | 2019-10-25 | 东南大学 | 一种混合交互式多模型滤波方法 |
CN111024064A (zh) * | 2019-11-25 | 2020-04-17 | 东南大学 | 一种改进Sage-Husa自适应滤波的SINS/DVL组合导航方法 |
WO2020253854A1 (zh) * | 2019-06-21 | 2020-12-24 | 台州知通科技有限公司 | 移动机器人姿态角解算方法 |
CN112230195A (zh) * | 2020-09-02 | 2021-01-15 | 清华大学 | 基于非线性最优平滑层策略的平滑变结构滤波方法及*** |
CN113376672A (zh) * | 2021-06-07 | 2021-09-10 | 长春工业大学 | 基于模糊卡尔曼滤波的北斗伪距定位算法 |
CN114909608A (zh) * | 2022-05-27 | 2022-08-16 | 哈尔滨工程大学 | 一种基于mimu/里程轮/光电测速传感器组合的非开挖管道定位方法 |
CN115307643A (zh) * | 2022-08-24 | 2022-11-08 | 东南大学 | 一种双应答器辅助的sins/usbl组合导航方法 |
-
2023
- 2023-02-08 CN CN202310079740.8A patent/CN115790669B/zh active Active
Patent Citations (8)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN107797106A (zh) * | 2017-05-08 | 2018-03-13 | 南京航空航天大学 | 一种加速em未知杂波估计的phd多目标跟踪平滑滤波方法 |
WO2020253854A1 (zh) * | 2019-06-21 | 2020-12-24 | 台州知通科技有限公司 | 移动机器人姿态角解算方法 |
CN110375731A (zh) * | 2019-07-01 | 2019-10-25 | 东南大学 | 一种混合交互式多模型滤波方法 |
CN111024064A (zh) * | 2019-11-25 | 2020-04-17 | 东南大学 | 一种改进Sage-Husa自适应滤波的SINS/DVL组合导航方法 |
CN112230195A (zh) * | 2020-09-02 | 2021-01-15 | 清华大学 | 基于非线性最优平滑层策略的平滑变结构滤波方法及*** |
CN113376672A (zh) * | 2021-06-07 | 2021-09-10 | 长春工业大学 | 基于模糊卡尔曼滤波的北斗伪距定位算法 |
CN114909608A (zh) * | 2022-05-27 | 2022-08-16 | 哈尔滨工程大学 | 一种基于mimu/里程轮/光电测速传感器组合的非开挖管道定位方法 |
CN115307643A (zh) * | 2022-08-24 | 2022-11-08 | 东南大学 | 一种双应答器辅助的sins/usbl组合导航方法 |
Non-Patent Citations (2)
Title |
---|
任臻;李积英;吴昊;: "基于M估计的鲁棒后向平滑CKF单站跟踪算法" * |
李松;唐小妹;孙鹏跃;张可;王飞雪;: "GNSS/INS紧组合最大熵卡尔曼滤波算法" * |
Also Published As
Publication number | Publication date |
---|---|
CN115790669B (zh) | 2023-04-28 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
CN109916407B (zh) | 基于自适应卡尔曼滤波器的室内移动机器人组合定位方法 | |
CN104884902B (zh) | 用于三轴磁力计和三轴加速度计的数据融合的方法和装置 | |
CN107690567B (zh) | 利用扩展卡尔曼滤波器用于对移动载体设备的航行进行追踪的方法 | |
CN110823215B (zh) | 一种无人机相对导航信息融合方法 | |
CN113819906A (zh) | 一种基于统计相似度量的组合导航鲁棒滤波方法 | |
CN109086250B (zh) | 适用于带斜置光纤陀螺的mems惯组的数据融合方法 | |
CN104075711B (zh) | 一种基于CKF的IMU/Wi‑Fi信号紧组合室内导航方法 | |
CN109827571A (zh) | 一种无转台条件下的双加速度计标定方法 | |
CN116817896B (zh) | 一种基于扩展卡尔曼滤波的姿态解算方法 | |
CN113587926A (zh) | 一种航天器空间自主交会对接相对导航方法 | |
CN110632636B (zh) | 一种基于Elman神经网络的载体姿态估计方法 | |
CN110595434B (zh) | 基于mems传感器的四元数融合姿态估计方法 | |
CN115979253A (zh) | 一种基于抗差滤波的水下机器人多传感器紧组合导航方法 | |
Wang et al. | A robust backtracking CKF based on Krein space theory for in-motion alignment process | |
CN114739397A (zh) | 矿井环境运动惯性估计自适应卡尔曼滤波融合定位方法 | |
CN113434806B (zh) | 一种抗差自适应多模型滤波方法 | |
CN111982126B (zh) | 一种全源BeiDou/SINS弹性状态观测器模型设计方法 | |
JP2010096647A (ja) | 航法装置及び推定方法 | |
CN110375773B (zh) | Mems惯导***姿态初始化方法 | |
CN115790669A (zh) | 基于最大熵平滑变结构滤波的dvl误差自适应标定方法 | |
CN113447018B (zh) | 一种水下惯性导航***的姿态实时估计方法 | |
CN110470294B (zh) | 一种虚拟测量与卡尔曼滤波融合的载体姿态估计方法 | |
CN113267183A (zh) | 一种多加速度计惯导***的组合导航方法 | |
CN112683265A (zh) | 一种基于快速iss集员滤波的mimu/gps组合导航方法 | |
CN111442786A (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 |