CN111854747B - 一种载体大机动情况下的dvl辅助sins粗对准方法 - Google Patents
一种载体大机动情况下的dvl辅助sins粗对准方法 Download PDFInfo
- Publication number
- CN111854747B CN111854747B CN202010860244.2A CN202010860244A CN111854747B CN 111854747 B CN111854747 B CN 111854747B CN 202010860244 A CN202010860244 A CN 202010860244A CN 111854747 B CN111854747 B CN 111854747B
- Authority
- CN
- China
- Prior art keywords
- dvl
- matrix
- sins
- information
- carrier
- 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
Links
- 238000000034 method Methods 0.000 title claims abstract description 36
- 239000011159 matrix material Substances 0.000 claims abstract description 89
- 239000013598 vector Substances 0.000 claims abstract description 27
- 238000005259 measurement Methods 0.000 claims abstract description 21
- 238000006243 chemical reaction Methods 0.000 claims abstract description 11
- 230000009466 transformation Effects 0.000 claims description 22
- 230000005540 biological transmission Effects 0.000 claims description 6
- 230000005484 gravity Effects 0.000 claims description 4
- 238000002604 ultrasonography Methods 0.000 claims description 4
- 230000009286 beneficial effect Effects 0.000 description 1
- 238000010586 diagram Methods 0.000 description 1
Images
Classifications
-
- 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
-
- 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
Landscapes
- Engineering & Computer Science (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Manufacturing & Machinery (AREA)
- Automation & Control Theory (AREA)
- Navigation (AREA)
Abstract
Description
技术领域
本发明涉及一种载体大机动情况下的DVL辅助SINS粗对准方法,属于组合导航对准技术,特别适用于水下组合导航领域。
背景技术
为利用丰富的海洋资源,海洋勘探成为人类发展的必修课题。水下导航***作为探测海洋的核心技术,决定着水下任务的有效进行。SINS(即:捷联惯性导航***)/DVL(即:多普勒测速仪)组合导航凭借其高自主性和抗干扰性,逐渐成为最常用的水下组合导航方式。SINS/DVL组合导航***中,粗对准通过计算粗略的姿态信息,为精对准和后续导航提供初始姿态信息,是组合导航的首要任务。
现有技术中对SINS/DVL组合导航***的粗对准方法主要集中于载体低动态情况,未考虑载体大机动情况下由姿态和速度变化引入的DVL测速误差。该误差会大幅降低粗对准速度与精度指标。为了提高SINS/DVL粗对准速度与精度,亟需提出一种适用于载体大机动情况的粗对准方法。
发明内容
为解决上述问题,本发明公开了一种载体大机动情况下的DVL辅助SINS粗对准方法,能有效地提高SINS/DVL组合导航***在大机动情况下粗对准的收敛速度与精度。
为达到上述目的,本发明的技术方案如下:
一种载体大机动情况下的DVL辅助SINS粗对准方法,包括如下步骤:
S1:初始化转换矩阵和姿态矩阵为单位阵;
S2:根据上一时刻对准输出的姿态矩阵更新速度微分方程;
S5:采用步骤S4中校正后的DVL测速信息与SINS输出信息构建参考与观测矢量,根据最优基四元数法计算初始姿态矩阵,更新当前姿态信息;
S6:重复步骤S2到S5直至对准过程结束。
优选的,所述步骤S1具体包括以下过程:
初始化转换矩阵和姿态矩阵为单位阵:
优选的,所述步骤S2具体包括以下过程:
根据上一时刻对准输出的姿态矩阵更新速度微分方程:
优选的,所述步骤S3具体包括以下过程:
优选的,所述步骤S4具体包括以下过程:
其中,Vb(t)为校正后的DVL测速信息,为DVL接收t2时刻b2系下的DVL速度,c为超声波波速,f0为DVL超声波信号发射频率。为DVL接收t2时刻SINS输出的载体速度信息,为DVL发射t1时刻SINS输出的载体速度信息,A为DVL转换矩阵,[Δf1 Δf2 Δf3Δf4]T为DVL四轴向频移信息。
优选的,所述步骤S5具体包括以下过程:
采用步骤S4校正后的DVL测速信息与SINS输出信息构建参考与观测矢量,根据最优基四元数法计算初始姿态矩阵,更新当前姿态信息:
S5.1采用步骤S4校正后的DVL测速信息与SINS输出信息构建参考矢量α和观测矢量β:
其中,α为参考矢量,β为观测矢量,Vb(0)为初始时刻DVL测速信息;
S5.2采用最优基四元数法构建K矩阵:
其中,K(t)为K矩阵;
S5.5计算姿态矩阵:
优选的,所述步骤S6具体包括以下过程:
重复步骤S2到S5直至对准过程结束。
本发明的有益效果是:
本发明利用SINS输出姿态与速度信息对DVL测速误差进行建模与补偿,提高了粗对准的速度与精度。
附图说明
图1为本发明的载体大机动情况下的DVL辅助SINS粗对准方法原理图;
图2为本发明的载体大机动情况下的DVL辅助SINS粗对准方法流程图。
具体实施方式
下面结合附图和具体实施方式,进一步阐明本发明,应理解下述具体实施方式仅用于说明本发明而不用于限制本发明的范围。
本发明所述的一种载体姿态动态情况下的惯性/多普勒动基座粗对准方法,实现原理如图1所示,实验流程如图2所示。其流程主要包括以下步骤:
步骤S1,初始化转换矩阵和姿态矩阵为单位阵。
在载体进入初始对准过程时,未定义转换矩阵及姿态矩阵,需对其进行初始化。
具体包括以下过程:
初始化转换矩阵和姿态矩阵为单位阵:
步骤S2,根据上一时刻对准输出的姿态矩阵更新速度微分方程。
在SINS更新过程中,采用比力方程更新速度。
具体包括以下过程:
根据上一时刻对准输出的姿态矩阵更新速度微分方程:
具体包括以下过程:
SINS/DVL组合导航***粗对准的关键问题是t时刻与0时刻机体速度精度问题。DVL通过发射和接收信号的频差来测量相对速度。Janus结构中,DVL速度与DVL波束速度的关系为:
载体小机动情况下,发射与接收时刻的DVL速度、DVL波束速度不变,可认为是常值。然而,在载体大机动情况下,发射与接收时刻的DVL速度、DVL波束速度变化,应对其进行区分。
定义发射t1时刻DVL波束速度为:
定义接收t2时刻DVL波束速度为:
用发射t1与接收t2时刻的DVL速度、DVL波束速度表示DVL四轴向频移信息:
其中,[Δf1 Δf2 Δf3 Δf]T为DVL四轴向频移信息,c为超声波波速,f0为DVL超声波信号发射频率。
载体小机动情况下,可直接用DVL输出速度计算载体速度,但载体大机动情况下,由于发射与接收时刻速度不同,不能直接计算,本发明创新性地通过SINS输出建立发射与接收时刻速度关系校正载体速度。
具体包括以下过程:
其中,Vb(t)为校正后的DVL测速信息,为DVL接收t2时刻b2系下的DVL速度,c为超声波波速,f0为DVL超声波信号发射频率。为DVL接收t2时刻SINS输出的载体速度信息,为DVL发射t1时刻SINS输出的载体速度信息,A为DVL转换矩阵,[Δf1 Δf2 Δf3Δf4]T为DVL四轴向频移信息。
步骤S5,采用步骤S4中校正后的DVL测速信息与SINS输出信息构建参考与观测矢量,根据最优基四元数法计算初始姿态矩阵,更新当前姿态信息。
考虑载体坐标系下比力方程:
将其转换到b0系下:
对左式进行积分:
同时:
又有:
得到:
根据该公式,本发明创造性地用校正后的DVL测速信息构造参考与观测矢量更新姿态信息。
具体包括以下过程:
采用步骤S4校正后的DVL测速信息与SINS输出信息构建参考与观测矢量,根据最优基四元数法计算初始姿态矩阵,更新当前姿态信息:
S5.1采用步骤S4校正后的DVL测速信息与SINS输出信息构建参考矢量α和观测矢量β:
其中,α为参考矢量,β为观测矢量,Vb(0)为初始时刻DVL测速信息;
S5.2采用最优基四元数法构建K矩阵:
其中,K(t)为K矩阵;
S5.5计算姿态矩阵:
步骤S6,重复步骤S2到S5直至对准过程结束。具体包括以下过程:
重复步骤S2到S5直至对准过程结束。
本发明方案所公开的技术手段不仅限于上述实施方式所公开的技术手段,还包括由以上技术特征任意组合所组成的技术方案。
Claims (1)
1.一种载体大机动情况下的DVL辅助SINS粗对准方法,其特征在于,包括以下步骤:
S1:初始化转换矩阵和姿态矩阵为单位阵;
S2:根据上一时刻对准输出的姿态矩阵更新速度微分方程;
其中,Vb(t)为校正后的DVL测速信息,为DVL接收t2时刻b2系下的DVL速度,c为超声波波速,f0为DVL超声波信号发射频率,为DVL接收t2时刻SINS输出的载体速度信息,为DVL发射t1时刻SINS输出的载体速度信息,A为DVL转换矩阵,[△f1△f2△f3△f4]T为DVL四轴向频移信息;
S5:采用步骤S4中校正后的DVL测速信息与SINS输出信息构建参考与观测矢量,根据最优基四元数法计算初始姿态矩阵,更新当前姿态信息;
S5.1采用步骤S4校正后的DVL测速信息与SINS输出信息构建参考矢量α和观测矢量β:
其中,α为参考矢量,β为观测矢量,Vb(0)为初始时刻DVL测速信息;
S5.2采用最优基四元数法构建K矩阵:
其中,K(t)为K矩阵;
S5.5计算姿态矩阵:
S6:重复步骤S2到S5直至对准过程结束。
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202010860244.2A CN111854747B (zh) | 2020-08-25 | 2020-08-25 | 一种载体大机动情况下的dvl辅助sins粗对准方法 |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202010860244.2A CN111854747B (zh) | 2020-08-25 | 2020-08-25 | 一种载体大机动情况下的dvl辅助sins粗对准方法 |
Publications (2)
Publication Number | Publication Date |
---|---|
CN111854747A CN111854747A (zh) | 2020-10-30 |
CN111854747B true CN111854747B (zh) | 2022-08-12 |
Family
ID=72966855
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
CN202010860244.2A Active CN111854747B (zh) | 2020-08-25 | 2020-08-25 | 一种载体大机动情况下的dvl辅助sins粗对准方法 |
Country Status (1)
Country | Link |
---|---|
CN (1) | CN111854747B (zh) |
Families Citing this family (3)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN112747770B (zh) * | 2020-12-16 | 2022-10-04 | 中国船舶重工集团有限公司第七一0研究所 | 一种基于速度量测的载体机动中初始对准方法 |
CN112798021B (zh) * | 2021-04-15 | 2021-07-13 | 中国人民解放军国防科技大学 | 基于激光多普勒测速仪的惯导***行进间初始对准方法 |
CN114001602A (zh) * | 2021-10-26 | 2022-02-01 | 东北大学秦皇岛分校 | 基于四元数卡尔曼滤波去噪融合的火箭炮扰动检测方法 |
Citations (6)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN108225375A (zh) * | 2018-01-08 | 2018-06-29 | 哈尔滨工程大学 | 一种基于中值滤波的抗外速度野值的优化粗对准方法 |
CN109141475A (zh) * | 2018-09-14 | 2019-01-04 | 苏州大学 | 一种dvl辅助sins鲁棒行进间初始对准方法 |
CN109443379A (zh) * | 2018-09-28 | 2019-03-08 | 东南大学 | 一种深海潜航器的sins/dvl水下抗晃动对准方法 |
CN110031882A (zh) * | 2018-08-02 | 2019-07-19 | 哈尔滨工程大学 | 一种基于sins/dvl组合导航***的外量测信息补偿方法 |
CN111323012A (zh) * | 2020-04-24 | 2020-06-23 | 东南大学 | 载体高动态环境下的ins辅助dvl测速误差补偿方法 |
CN111397603A (zh) * | 2020-04-24 | 2020-07-10 | 东南大学 | 载体姿态动态情况下的惯性/多普勒动基座粗对准方法 |
-
2020
- 2020-08-25 CN CN202010860244.2A patent/CN111854747B/zh active Active
Patent Citations (6)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN108225375A (zh) * | 2018-01-08 | 2018-06-29 | 哈尔滨工程大学 | 一种基于中值滤波的抗外速度野值的优化粗对准方法 |
CN110031882A (zh) * | 2018-08-02 | 2019-07-19 | 哈尔滨工程大学 | 一种基于sins/dvl组合导航***的外量测信息补偿方法 |
CN109141475A (zh) * | 2018-09-14 | 2019-01-04 | 苏州大学 | 一种dvl辅助sins鲁棒行进间初始对准方法 |
CN109443379A (zh) * | 2018-09-28 | 2019-03-08 | 东南大学 | 一种深海潜航器的sins/dvl水下抗晃动对准方法 |
CN111323012A (zh) * | 2020-04-24 | 2020-06-23 | 东南大学 | 载体高动态环境下的ins辅助dvl测速误差补偿方法 |
CN111397603A (zh) * | 2020-04-24 | 2020-07-10 | 东南大学 | 载体姿态动态情况下的惯性/多普勒动基座粗对准方法 |
Non-Patent Citations (1)
Title |
---|
徐晓苏等.《基于改进型CKF的SINS初始对准方法》.《华中科技大学学报(自然科学版)》.2016,第44卷(第1期), * |
Also Published As
Publication number | Publication date |
---|---|
CN111854747A (zh) | 2020-10-30 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
CN111854747B (zh) | 一种载体大机动情况下的dvl辅助sins粗对准方法 | |
CN109443379B (zh) | 一种深海潜航器的sins/dvl水下抗晃动对准方法 | |
CN111426318B (zh) | 基于四元数-扩展卡尔曼滤波的低成本ahrs航向角补偿方法 | |
CN112097763B (zh) | 一种基于mems imu/磁力计/dvl组合的水下运载体组合导航方法 | |
WO2022028286A1 (zh) | 一种运动约束辅助的基于改进Sage-Husa自适应滤波的水下组合导航方法 | |
US8224574B2 (en) | System for multiple navigation components | |
CN111323012B (zh) | 载体高动态环境下的ins辅助dvl测速误差补偿方法 | |
CN112284384B (zh) | 考虑量测异常的集群式多深海潜航器的协同定位方法 | |
CN104422948A (zh) | 一种嵌入式组合导航***及其方法 | |
CN111397603B (zh) | 载体姿态动态情况下的惯性/多普勒动基座粗对准方法 | |
CN112729291A (zh) | 一种深潜长航潜水器sins/dvl洋流速度估计方法 | |
CN104061930B (zh) | 一种基于捷联惯性制导和多普勒计程仪的导航方法 | |
CN112747748A (zh) | 一种基于逆向解算的领航auv导航数据后处理方法 | |
CN115307643A (zh) | 一种双应答器辅助的sins/usbl组合导航方法 | |
CN114459476B (zh) | 基于虚拟速度量测的水下无人潜航器测流dvl/sins组合导航方法 | |
CN113847915A (zh) | 一种捷联惯导/多普勒组合导航***的导航方法 | |
CN113670302B (zh) | 一种运动效应影响下的惯性/超短基线组合导航方法 | |
CN103697887B (zh) | 一种基于捷联惯导***和多普勒计程仪的优化导航方法 | |
CN116380067A (zh) | 一种适应于挑战环境下的无人艇旋转调制惯导***及方法 | |
CN110873813A (zh) | 一种水流速度估算方法、组合导航方法及装置 | |
CN111964684B (zh) | 一种基于sins/lbl紧组合的水下导航混合定位方法及*** | |
CN113776525B (zh) | 一种基于斜距差匹配的惯性/单声源被动组合导航方法 | |
CN116165690B (zh) | 一种基于gnss/ins的双自适应因子组合导航定位方法 | |
CN113280812B (zh) | 一种基于sins/lbl紧组合的高程约束水下自适应导航方法及装置 | |
CN114166203A (zh) | 一种基于改进的s-h自适应联邦滤波的智能水下机器人多源组合导航方法 |
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 |