CN107024226B - 一种基于惯导/dvl/usbl组合的惯导误差估计方法 - Google Patents

一种基于惯导/dvl/usbl组合的惯导误差估计方法 Download PDF

Info

Publication number
CN107024226B
CN107024226B CN201610068960.0A CN201610068960A CN107024226B CN 107024226 B CN107024226 B CN 107024226B CN 201610068960 A CN201610068960 A CN 201610068960A CN 107024226 B CN107024226 B CN 107024226B
Authority
CN
China
Prior art keywords
inertial navigation
filter
sub
usbl
dvl
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
CN201610068960.0A
Other languages
English (en)
Other versions
CN107024226A (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.)
Beijing Automation Control Equipment Institute BACEI
Original Assignee
Beijing Automation Control Equipment Institute BACEI
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 Beijing Automation Control Equipment Institute BACEI filed Critical Beijing Automation Control Equipment Institute BACEI
Priority to CN201610068960.0A priority Critical patent/CN107024226B/zh
Publication of CN107024226A publication Critical patent/CN107024226A/zh
Application granted granted Critical
Publication of CN107024226B publication Critical patent/CN107024226B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Classifications

    • 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
    • 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

Abstract

本发明属于惯导技术，具体公开一种基于惯导/DVL/USBL组合的惯导误差估计方法，包括同步采集惯导的位置和速度、DVL的速度、USBL的位置等测试数据，并设计三个通过经典Kalman滤波实现的滤波器的配合：采用子滤波器1实现惯导/DVL组合、采用子滤波器2实现惯导/USBL组合，采用主滤波器，获得惯导误差的高精度估计；从而实现了高环境适应性，低位置累计误差的惯导误差估计方法。

Description

一种基于惯导/DVL/USBL组合的惯导误差估计方法
技术领域
本发明属于惯导技术，具体涉及一种基于惯导/DVL/USBL组合的惯导误差估计方法。
背景技术
惯导/DVL组合可以实现远距离的组合导航，但位置误差累积。惯导/USBL组合可以实现位置误差不累积的组合导航，然而受USBL作用范围限制，不易实现远距离的组合导航，一般只能在特定水域工作。惯导/DVL/USBL组合可以实现优势互补，提高整个系统的工作适应性。现有技术中并没有一种基于惯导/DVL/USBL组合的惯导误差估计方法。
发明内容
本发明的目的在于提供一种基于惯导/DVL/USBL组合的惯导误差估计方法。
实现本发明目的的技术方案：
一种基于惯导/DVL/USBL组合的惯导误差估计方法，它包括下列步骤：
步骤1：同步采集惯导的位置和速度、DVL的速度、USBL的位置等测试数据；
步骤2：采用子滤波器1实现惯导/DVL组合；
子滤波器1的输入信息为惯导的速度与DVL的速度；子滤波器1的状态变量为17维向量：
Figure BDA0000919295570000011
其中，φN、φU、φE分别为惯导的北向、天向、东向轴失准角；δvN、δvU、δvE分别为惯导的北向、天向、东向轴速度误差；
Figure BDA0000919295570000012
δh、δλ分别为惯导的纬度、高度和经度误差；εN、εU、εE和▽N、▽U、▽E分别为惯导的等效北向、天向、东向轴陀螺漂移和加速度计零偏；δθs为DVL的安装误差角，δks为DVL的刻度系数误差；
经过经典的Kalman滤波方程解算，获得子滤波器1的状态估计值和估计均方误差阵；取子滤波器1的状态估计值的前15个变量，记作
Figure BDA0000919295570000029
估计均方误差阵的前15行前15列矩阵，记作P1；
步骤3：采用子滤波器2实现惯导/USBL组合；
子滤波器2的输入信息为惯导的位置与USBL的位置；子滤波器2的状态变量为17维向量：
Figure BDA0000919295570000021
其中，φN、φU、φE分别为惯导的北向、天向、东向轴失准角；δvN、δvU、δvE分别为惯导的北向、天向、东向轴速度误差；
Figure BDA0000919295570000022
δh、δλ分别为惯导的纬度、高度和经度误差；εN、εU、εE和▽N、▽U、▽E分别为惯导的等效北向、天向、东向轴陀螺漂移和加速度计零偏；δθa为USBL的安装误差角，δka为USBL的刻度系数误差；
经过经典的Kalman滤波方程解算，获得子滤波器2的状态估计值和估计均方误差阵；取子滤波器2的状态估计值的前15个变量，记作
Figure BDA0000919295570000023
估计均方误差阵的前15行前15列矩阵，记作P2；
步骤4：采用主滤波器，获得惯导误差的高精度估计；
主滤波器的输入信息为状态估计值
Figure BDA0000919295570000024
估计均方误差阵P1、状态估计值
Figure BDA0000919295570000025
和估计均方误差阵P2，
计算融合估计误差方差阵，公式为：
Figure BDA0000919295570000026
其中，P1 -1为P1的逆矩阵，
Figure BDA0000919295570000027
为P2的逆矩阵；
计算主滤波器的状态估计值，公式为：
Figure BDA0000919295570000028
Figure BDA0000919295570000031
为主滤波器估计出的惯导误差。
所述的
Figure BDA0000919295570000038
包括φNφUφEδvNδvUδvE
Figure BDA0000919295570000032
δhδλεNεUεE▽N▽U▽U等15个变量。
本发明的有益技术效果在于：
本发明提供的一种基于惯导/DVL/USBL组合的惯导误差估计方法通过同步采集惯导的位置和速度、DVL的速度、USBL的位置等测试数据，并设计三个滤波器的配合：采用子滤波器1实现惯导/DVL组合、采用子滤波器2实现惯导/USBL组合，采用主滤波器，获得惯导误差的高精度估计；从而实现了高环境适应性，低位置累计误差的惯导误差估计方法。
具体实施方式
下面结合实施例对本发明作进一步详细说明。
本发明提供的一种惯导/DVL/USBL组合导航结构包括惯导、DVL、USBL、子滤波器1、子滤波器2和主滤波器。
惯导：由陀螺、加速度计、导航计算机等组成的可实现自主导航的惯导导航系统。通过其导航计算机的导航解算，可给出惯导的位置、速度、姿态等导航信息。
DVL：Doppler Velocity Log，多普勒计程仪。通过多普勒效应测量DVL相对于水底的速度。
USBL：Ultra Short Baseline，超短基线定位系统。由USBL本体和应答器构成了基线，其中应答器的位置精确已知，则USBL本体通过向应答器发射声波解算出基线长度，即可获得USBL的位置信息。
子滤波器1：输入惯导的速度和DVL的速度，输出状态估计值
Figure BDA0000919295570000033
和估计均方误差阵P1。
子滤波器2：输入惯导的位置和USBL的位置，输出状态估计值
Figure BDA0000919295570000034
和估计均方误差阵P2。
主滤波器：子滤波器1和子滤波器2的输出端与主滤波器的输入端连接。主滤波器输入状态估计值
Figure BDA0000919295570000035
估计均方误差阵P1、状态估计值
Figure BDA0000919295570000036
和估计均方误差阵P2，输出状态估计值
Figure BDA0000919295570000037
本发明提供的一种基于惯导/DVL/USBL组合的惯导误差估计方法包括下列步骤：
步骤1：同步采集惯导的位置和速度、DVL的速度、USBL的位置等测试数据。
步骤2：采用子滤波器1实现惯导/DVL组合。
子滤波器1的输入信息为惯导的速度与DVL的速度。子滤波器1的状态变量为17维向量：
Figure BDA0000919295570000041
其中，φN、φU、φE分别为惯导的北向、天向、东向轴失准角；δvN、δvU、δvE分别为惯导的北向、天向、东向轴速度误差；
Figure BDA0000919295570000042
δh、δλ分别为惯导的纬度、高度和经度误差；εN、εU、εE和▽N、▽U、▽E分别为惯导的等效北向、天向、东向轴陀螺漂移和加速度计零偏；δθs为DVL的安装误差角，δks为DVL的刻度系数误差。
经过经典的Kalman滤波方程解算，获得子滤波器1的状态估计值和估计均方误差阵；取子滤波器1的状态估计值的前15个变量，记作
Figure BDA0000919295570000045
估计均方误差阵的前15行前15列矩阵，记作P1。
步骤3：采用子滤波器2实现惯导/USBL组合。
子滤波器2的输入信息为惯导的位置与USBL的位置。子滤波器2的状态变量为17维向量：
Figure BDA0000919295570000043
其中，φN、φU、φE分别为惯导的北向、天向、东向轴失准角；δvN、δvU、δvE分别为惯导的北向、天向、东向轴速度误差；
Figure BDA0000919295570000044
δh、δλ分别为惯导的纬度、高度和经度误差；εN、εU、εE和▽N、▽U、▽E分别为惯导的等效北向、天向、东向轴陀螺漂移和加速度计零偏；δθa为USBL的安装误差角，δka为USBL的刻度系数误差。
经过经典的Kalman滤波方程解算，获得子滤波器2的状态估计值和估计均方误差阵；取子滤波器2的状态估计值的前15个变量，记作
Figure BDA0000919295570000051
估计均方误差阵的前15行前15列矩阵，记作P2。
步骤4：采用主滤波器，获得惯导误差的高精度估计。
主滤波器的输入信息为状态估计值
Figure BDA0000919295570000052
估计均方误差阵P1、状态估计值
Figure BDA0000919295570000053
和估计均方误差阵P2，
计算融合估计误差方差阵，公式为：
Figure BDA0000919295570000054
其中，P1 -1为P1的逆矩阵，
Figure BDA0000919295570000055
为P2的逆矩阵；
计算主滤波器的状态估计值，公式为：
Figure BDA0000919295570000056
Figure BDA0000919295570000057
为主滤波器估计出的惯导误差，
Figure BDA0000919295570000058
包括φNφUφEδvNδvUδvE
Figure BDA0000919295570000059
δhδλεNεUεE▽N▽U▽U等15个变量。
至此，给出了一种基于惯导/DVL/USBL组合的惯导误差估计方法。根据此发明，可以获得惯导误差量的高精度估计，用以修正惯导的位置、速度、姿态角等信息后，提高惯导系统的导航精度。
上面结合实施例对本发明作了详细说明，但是本发明并不限于上述实施例，在本领域普通技术人员所具备的知识范围内，还可以在不脱离本发明宗旨的前提下作出各种变化。本发明中未作详细描述的内容均可以采用现有技术。

Claims (1)

1.一种基于惯导/DVL/USBL组合的惯导误差估计方法，其特征在于：该方法包括如下步骤，
步骤1：同步采集惯导的位置和速度、DVL的速度、USBL的位置测试数据；
步骤2：采用子滤波器1实现惯导/DVL组合；
子滤波器1的输入信息为惯导的速度与DVL的速度；子滤波器1的状态变量为17维向量：
Figure FDA0002825029930000011
其中，φN、φU、φE分别为惯导的北向、天向、东向轴失准角；δvN、δvU、δvE分别为惯导的北向、天向、东向轴速度误差；
Figure FDA0002825029930000012
δh、δλ分别为惯导的纬度、高度和经度误差；εN、εU、εE和
Figure FDA0002825029930000013
分别为惯导的等效北向、天向、东向轴陀螺漂移和加速度计零偏；δθs为DVL的安装误差角，δks为DVL的刻度系数误差；
经过经典的Kalman滤波方程解算，获得子滤波器1的状态估计值和估计均方误差阵；取子滤波器1的状态估计值的前15个变量，记作
Figure FDA0002825029930000014
估计均方误差阵的前15行前15列矩阵，记作P1；
步骤3：采用子滤波器2实现惯导/USBL组合；
子滤波器2的输入信息为惯导的位置与USBL的位置；子滤波器2的状态变量为17维向量：
Figure FDA0002825029930000015
其中，φN、φU、φE分别为惯导的北向、天向、东向轴失准角；δvN、δvU、δvE分别为惯导的北向、天向、东向轴速度误差；
Figure FDA0002825029930000016
δh、δλ分别为惯导的纬度、高度和经度误差；εN、εU、εE和
Figure FDA0002825029930000017
分别为惯导的等效北向、天向、东向轴陀螺漂移和加速度计零偏；δθa为USBL的安装误差角，δksa为USBL的刻度系数误差；
经过经典的Kalman滤波方程解算，获得子滤波器2的状态估计值和估计均方误差阵；取子滤波器2的状态估计值的前15个变量，记作
Figure FDA0002825029930000018
估计均方误差阵的前15行前15列矩阵，记作P2；
步骤4：采用主滤波器，获得惯导误差的高精度估计；
主滤波器的输入信息为状态估计值
Figure FDA0002825029930000019
估计均方误差阵P1、状态估计值
Figure FDA00028250299300000110
和估计均方误差阵P2，
计算融合估计误差方差阵，公式为：
P1＝(P1 -1+P2 -1)-1
其中，P1 -1为P1的逆矩阵，P2 -1为P2的逆矩阵；
计算主滤波器的状态估计值，公式为：
Figure FDA00028250299300000111
Figure FDA00028250299300000112
为主滤波器估计出的惯导误差；
所述的
Figure FDA00028250299300000113
包括φN φU φE δvN δvU δνE
Figure FDA00028250299300000114
δh δλ εN εU εE
Figure FDA00028250299300000115
等15个变量。
CN201610068960.0A 2016-02-01 2016-02-01 一种基于惯导/dvl/usbl组合的惯导误差估计方法 Active CN107024226B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN201610068960.0A CN107024226B (zh) 2016-02-01 2016-02-01 一种基于惯导/dvl/usbl组合的惯导误差估计方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN201610068960.0A CN107024226B (zh) 2016-02-01 2016-02-01 一种基于惯导/dvl/usbl组合的惯导误差估计方法

Publications (2)

Publication Number Publication Date
CN107024226A CN107024226A (zh) 2017-08-08
CN107024226B true CN107024226B (zh) 2021-03-16

Family

ID=59524897

Family Applications (1)

Application Number Title Priority Date Filing Date
CN201610068960.0A Active CN107024226B (zh) 2016-02-01 2016-02-01 一种基于惯导/dvl/usbl组合的惯导误差估计方法

Country Status (1)

Country Link
CN (1) CN107024226B (zh)

Families Citing this family (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN108303079B (zh) * 2017-12-21 2021-01-26 中国船舶重工集团公司第七0七研究所 一种水下usbl反向应用的数据平滑方法
CN108444476B (zh) * 2018-02-05 2021-04-23 哈尔滨工程大学 一种考虑水声通信延迟的多水下无人航行器极区协同导航方法
CN110207694A (zh) * 2019-05-27 2019-09-06 哈尔滨工程大学 一种基于相对位置信息的极区格网惯导/超短基线组合导航方法
CN113959434A (zh) * 2021-09-22 2022-01-21 河北汉光重工有限责任公司 一种可调节的sins、dvl、usbl组合导航方法
CN115790642A (zh) * 2022-10-13 2023-03-14 北京自动化控制设备研究所 一种mems惯导系统参数误差置信度评测方法

Citations (6)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US4783744A (en) * 1986-12-08 1988-11-08 General Dynamics, Pomona Division Self-adaptive IRU correction loop design interfacing with the target state estimator for multi-mode terminal handoff
CN101665150A (zh) * 2008-09-05 2010-03-10 康弗蒂姆技术有限公司 动态定位架构
CN103217157A (zh) * 2012-01-18 2013-07-24 北京自动化控制设备研究所 一种惯导/里程计自主组合导航方法
CN103278163A (zh) * 2013-05-24 2013-09-04 哈尔滨工程大学 一种基于非线性模型的sins/dvl组合导航方法
CN104280025A (zh) * 2013-07-08 2015-01-14 中国科学院沈阳自动化研究所 基于无色卡尔曼滤波的深海机器人超短基线组合导航方法
CN105091907A (zh) * 2015-07-28 2015-11-25 东南大学 Sins/dvl组合中dvl方位安装误差估计方法

Patent Citations (6)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US4783744A (en) * 1986-12-08 1988-11-08 General Dynamics, Pomona Division Self-adaptive IRU correction loop design interfacing with the target state estimator for multi-mode terminal handoff
CN101665150A (zh) * 2008-09-05 2010-03-10 康弗蒂姆技术有限公司 动态定位架构
CN103217157A (zh) * 2012-01-18 2013-07-24 北京自动化控制设备研究所 一种惯导/里程计自主组合导航方法
CN103278163A (zh) * 2013-05-24 2013-09-04 哈尔滨工程大学 一种基于非线性模型的sins/dvl组合导航方法
CN104280025A (zh) * 2013-07-08 2015-01-14 中国科学院沈阳自动化研究所 基于无色卡尔曼滤波的深海机器人超短基线组合导航方法
CN105091907A (zh) * 2015-07-28 2015-11-25 东南大学 Sins/dvl组合中dvl方位安装误差估计方法

Non-Patent Citations (1)

* Cited by examiner, † Cited by third party
Title
深水自主水下航行器导航与运动控制系统的设计与研究;向晔;《中国优秀硕士学位论文全文数据库（电子期刊）》;20150815(第08期);第二章 导航定位系统的设计研究 第13-29页 *

Also Published As

Publication number Publication date
CN107024226A (zh) 2017-08-08

Similar Documents

Publication Publication Date Title
CN107024226B (zh) 一种基于惯导/dvl/usbl组合的惯导误差估计方法
CN107655476B (zh) 基于多信息融合补偿的行人高精度足部导航方法
CN102538792B (zh) 一种位置姿态系统的滤波方法
Wu et al. Velocity/position integration formula part I: Application to in-flight coarse alignment
Eustice et al. Synchronous‐clock, one‐way‐travel‐time acoustic navigation for underwater vehicles
JP4819802B2 (ja) 複数のナビゲーションコンポーネントの整合システム及び方法
CN108594272B (zh) 一种基于鲁棒卡尔曼滤波的抗欺骗干扰组合导航方法
CN105910602B (zh) 一种组合导航方法
CN107015259B (zh) 采用多普勒测速仪计算伪距/伪距率的紧组合方法
CN103217699B (zh) 一种基于偏振信息的组合导航系统递推优化初始对准方法
CN108444476B (zh) 一种考虑水声通信延迟的多水下无人航行器极区协同导航方法
CN111323050A (zh) 一种捷联惯导和多普勒组合系统标定方法
CN103900565A (zh) 一种基于差分gps的惯导系统姿态获取方法
CN110849360B (zh) 面向多机协同编队飞行的分布式相对导航方法
CN110793518B (zh) 一种海上平台的定位定姿方法及系统
CN108303079A (zh) 一种水下usbl反向应用的数据平滑方法
CN110207698B (zh) 一种极区格网惯导/超短基线紧组合导航方法
CN111722295A (zh) 一种水下捷联式重力测量数据处理方法
JP4818260B2 (ja) インターフェロメトリック・センシングシステム
CN104949687A (zh) 一种长时间导航系统全参数精度评估方法
CN111220151B (zh) 载体系下考虑温度模型的惯性和里程计组合导航方法
CN108692727B (zh) 一种带有非线性补偿滤波器的捷联惯导系统
CN115950419A (zh) 超小型无人机用组合导航方法、装置及系统
CN112229401B (zh) 适用于ins-gps伪距融合的量测信息同步外推方法及系统
CN115371706A (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