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

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

Info

Publication number
CN107024226A
CN107024226A CN201610068960.0A CN201610068960A CN107024226A CN 107024226 A CN107024226 A CN 107024226A CN 201610068960 A CN201610068960 A CN 201610068960A CN 107024226 A CN107024226 A CN 107024226A
Authority
CN
China
Prior art keywords
inertial navigation
error
estimation
subfilter
usbl
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
Application number
CN201610068960.0A
Other languages
English (en)
Other versions
CN107024226B (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维向量：
其中，φN、φU、φE分别为惯导的北向、天向、东向轴失准角；δvN、δvU、δvE分别为惯导的北向、天向、东向轴速度误差；δh、δλ分别为惯导的纬度、高度和经度误差；εN、εU、εE和▽N、▽U、▽E分别为惯导的等效北向、天向、东向轴陀螺漂移和加速度计零偏；δθs为DVL的安装误差角，δks为DVL的刻度系数误差；
经过经典的Kalman滤波方程解算，获得子滤波器1的状态估计值和估计均方误差阵；取子滤波器1的状态估计值的前15个变量，记作估计均方误差阵的前15行前15列矩阵，记作P1；
步骤3：采用子滤波器2实现惯导/USBL组合；
子滤波器2的输入信息为惯导的位置与USBL的位置；子滤波器2的状态变量为17维向量：
其中，φN、φU、φE分别为惯导的北向、天向、东向轴失准角；δvN、δvU、δvE分别为惯导的北向、天向、东向轴速度误差；δh、δλ分别为惯导的纬度、高度和经度误差；εN、εU、εE和▽N、▽U、▽E分别为惯导的等效北向、天向、东向轴陀螺漂移和加速度计零偏；δθa为USBL的安装误差角，δka为USBL的刻度系数误差；
经过经典的Kalman滤波方程解算，获得子滤波器2的状态估计值和估计均方误差阵；取子滤波器2的状态估计值的前15个变量，记作估计均方误差阵的前15行前15列矩阵，记作P2；
步骤4：采用主滤波器，获得惯导误差的高精度估计；
主滤波器的输入信息为状态估计值估计均方误差阵P1、状态估计值和估计均方误差阵P2，
计算融合估计误差方差阵，公式为：
其中，P1 -1为P1的逆矩阵，为P2的逆矩阵；
计算主滤波器的状态估计值，公式为：
为主滤波器估计出的惯导误差。
所述的
包括φNφUφEδvNδvUδvE δ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的速度，输出状态估计值和估计均方误差阵P1。
子滤波器2：输入惯导的位置和USBL的位置，输出状态估计值和估计均方误差阵P2。
主滤波器：子滤波器1和子滤波器2的输出端与主滤波器的输入端连接。主滤波器输入状态估计值估计均方误差阵P1、状态估计值和估计均方误差阵P2，输出状态估计值
本发明提供的一种基于惯导/DVL/USBL组合的惯导误差估计方法包括下列步骤：
步骤1：同步采集惯导的位置和速度、DVL的速度、USBL的位置等测试数据。
步骤2：采用子滤波器1实现惯导/DVL组合。
子滤波器1的输入信息为惯导的速度与DVL的速度。子滤波器1的状态变量为17维向量：
其中，φN、φU、φE分别为惯导的北向、天向、东向轴失准角；δvN、δvU、δvE分别为惯导的北向、天向、东向轴速度误差；δh、δλ分别为惯导的纬度、高度和经度误差；εN、εU、εE和▽N、▽U、▽E分别为惯导的等效北向、天向、东向轴陀螺漂移和加速度计零偏；δθs为DVL的安装误差角，δks为DVL的刻度系数误差。
经过经典的Kalman滤波方程解算，获得子滤波器1的状态估计值和估计均方误差阵；取子滤波器1的状态估计值的前15个变量，记作估计均方误差阵的前15行前15列矩阵，记作P1。
步骤3：采用子滤波器2实现惯导/USBL组合。
子滤波器2的输入信息为惯导的位置与USBL的位置。子滤波器2的状态变量为17维向量：
其中，φN、φU、φE分别为惯导的北向、天向、东向轴失准角；δvN、δvU、δvE分别为惯导的北向、天向、东向轴速度误差；δh、δλ分别为惯导的纬度、高度和经度误差；εN、εU、εE和▽N、▽U、▽E分别为惯导的等效北向、天向、东向轴陀螺漂移和加速度计零偏；δθa为USBL的安装误差角，δka为USBL的刻度系数误差。
经过经典的Kalman滤波方程解算，获得子滤波器2的状态估计值和估计均方误差阵；取子滤波器2的状态估计值的前15个变量，记作估计均方误差阵的前15行前15列矩阵，记作P2。
步骤4：采用主滤波器，获得惯导误差的高精度估计。
主滤波器的输入信息为状态估计值估计均方误差阵P1、状态估计值和估计均方误差阵P2，
计算融合估计误差方差阵，公式为：
其中，P1 -1为P1的逆矩阵，为P2的逆矩阵；
计算主滤波器的状态估计值，公式为：
为主滤波器估计出的惯导误差，
包括φNφUφEδvNδvUδvE δhδλεNεUεE▽N▽U▽U等15个变量。
至此，给出了一种基于惯导/DVL/USBL组合的惯导误差估计方法。根据此发明，可以获得惯导误差量的高精度估计，用以修正惯导的位置、速度、姿态角等信息后，提高惯导系统的导航精度。
上面结合实施例对本发明作了详细说明，但是本发明并不限于上述实施例，在本领域普通技术人员所具备的知识范围内，还可以在不脱离本发明宗旨的前提下作出各种变化。本发明中未作详细描述的内容均可以采用现有技术。

Claims (2)

1.一种基于惯导/DVL/USBL组合的惯导误差估计方法，其特征在于：它包括下列步骤：
步骤1：同步采集惯导的位置和速度、DVL的速度、USBL的位置等测试数据；
步骤2：采用子滤波器1实现惯导/DVL组合；
子滤波器1的输入信息为惯导的速度与DVL的速度；子滤波器1的状态变量为17维向量：
其中，φN、φU、φE分别为惯导的北向、天向、东向轴失准角；δvN、δvU、δvE分别为惯导的北向、天向、东向轴速度误差；分别为惯导的纬度、高度和经度误差；εN、εU、εE和▽N、▽U、▽E分别为惯导的等效北向、天向、东向轴陀螺漂移和加速度计零偏；δθs为DVL的安装误差角，δks为DVL的刻度系数误差；
经过经典的Kalman滤波方程解算，获得子滤波器1的状态估计值和估计均方误差阵；取子滤波器1的状态估计值的前15个变量，记作估计均方误差阵的前15行前15列矩阵，记作P1；
步骤3：采用子滤波器2实现惯导/USBL组合；
子滤波器2的输入信息为惯导的位置与USBL的位置；子滤波器2的状态变量为17维向量：
其中，φN、φU、φE分别为惯导的北向、天向、东向轴失准角；δvN、δvU、δvE分别为惯导的北向、天向、东向轴速度误差；分别为惯导的纬度、高度和经度误差；εN、εU、εE和▽N、▽U、▽E分别为惯导的等效北向、天向、东向轴陀螺漂移和加速度计零偏；δθa为USBL的安装误差角，δka为USBL的刻度系数误差；
经过经典的Kalman滤波方程解算，获得子滤波器2的状态估计值和估计均方误差阵；取子滤波器2的状态估计值的前15个变量，记作估计均方误差阵的前15行前15列矩阵，记作P2；
步骤4：采用主滤波器，获得惯导误差的高精度估计；
主滤波器的输入信息为状态估计值估计均方误差阵P1、状态估计值和估计均方误差阵P2，
计算融合估计误差方差阵，公式为：
其中，为P1的逆矩阵，为P2的逆矩阵；
计算主滤波器的状态估计值，公式为：
为主滤波器估计出的惯导误差。
2.根据权利要求1所述的一种基于惯导/DVL/USBL组合的惯导误差估计方法，其特征在于：所述的
包括等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 true CN107024226A (zh) 2017-08-08
CN107024226B 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)

Cited By (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN108303079A (zh) * 2017-12-21 2018-07-20 中国船舶重工集团公司第七0七研究所 一种水下usbl反向应用的数据平滑方法
CN108444476A (zh) * 2018-02-05 2018-08-24 哈尔滨工程大学 一种考虑水声通信延迟的多水下无人航行器极区协同导航方法
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
向晔: "深水自主水下航行器导航与运动控制系统的设计与研究", 《中国优秀硕士学位论文全文数据库（电子期刊）》 *

Cited By (6)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN108303079A (zh) * 2017-12-21 2018-07-20 中国船舶重工集团公司第七0七研究所 一种水下usbl反向应用的数据平滑方法
CN108444476A (zh) * 2018-02-05 2018-08-24 哈尔滨工程大学 一种考虑水声通信延迟的多水下无人航行器极区协同导航方法
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惯导系统参数误差置信度评测方法

Also Published As

Publication number Publication date
CN107024226B (zh) 2021-03-16

Similar Documents

Publication Publication Date Title
CN104457754B (zh) 一种基于sins/lbl紧组合的auv水下导航定位方法
Wu et al. Velocity/position integration formula part I: Application to in-flight coarse alignment
CN107024226A (zh) 一种基于惯导/dvl/usbl组合的惯导误差估计方法
CN106767752B (zh) 一种基于偏振信息的组合导航方法
CN101949703B (zh) 一种捷联惯性/卫星组合导航滤波方法
CN102538792B (zh) 一种位置姿态系统的滤波方法
CN105091907B (zh) Sins/dvl组合中dvl方位安装误差估计方法
CN103900565B (zh) 一种基于差分gps的惯导系统姿态获取方法
CN106500693B (zh) 一种基于自适应扩展卡尔曼滤波的ahrs算法
CN106767793A (zh) 一种基于sins/usbl紧组合的auv水下导航定位方法
CN105180968A (zh) 一种imu/磁强计安装失准角在线滤波标定方法
CN103727938B (zh) 一种管道测绘用惯导里程计组合导航方法
CN102519485B (zh) 一种引入陀螺信息的二位置捷联惯性导航系统初始对准方法
CN104236586B (zh) 基于量测失准角的动基座传递对准方法
CN102116634B (zh) 一种着陆深空天体探测器的降维自主导航方法
CN105371844A (zh) 一种基于惯性/天文互助的惯性导航系统初始化方法
CN105157724B (zh) 一种基于速度加姿态匹配的传递对准时间延迟估计与补偿方法
CN103674064B (zh) 捷联惯性导航系统的初始标定方法
CN106482746A (zh) 一种用于混合式惯导系统的加速度计内杆臂标定与补偿方法
CN103017787A (zh) 适用于摇摆晃动基座的初始对准方法
CN105547289A (zh) 一种水下航行器组合导航系统及导航信息融合方法
CN104316058B (zh) 一种采用交互多模型的移动机器人wsn/ins组合导航方法
CN109764870A (zh) 基于变换估计量建模方案的载体初始航向估算方法
CN106767928A (zh) 一种自适应快速传递对准方法
CN105988129A (zh) 一种基于标量估计算法的ins/gnss组合导航方法

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