CN110274591B - 深潜载人潜水器adcp辅助sins导航方法 - Google Patents
深潜载人潜水器adcp辅助sins导航方法 Download PDFInfo
- Publication number
- CN110274591B CN110274591B CN201910535185.9A CN201910535185A CN110274591B CN 110274591 B CN110274591 B CN 110274591B CN 201910535185 A CN201910535185 A CN 201910535185A CN 110274591 B CN110274591 B CN 110274591B
- Authority
- CN
- China
- Prior art keywords
- navigation
- coordinate system
- adcp
- speed
- sins
- 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.)
- Expired - Fee Related
Links
- 238000000034 method Methods 0.000 title claims abstract description 40
- 230000005888 antibody-dependent cellular phagocytosis Effects 0.000 title claims abstract 25
- XLYOFNOQVPJJNP-UHFFFAOYSA-N water Substances O XLYOFNOQVPJJNP-UHFFFAOYSA-N 0.000 claims abstract description 58
- 238000005259 measurement Methods 0.000 claims abstract description 44
- 238000001914 filtration Methods 0.000 claims abstract description 15
- 230000008569 process Effects 0.000 claims abstract description 13
- 238000004422 calculation algorithm Methods 0.000 claims abstract description 10
- 230000009189 diving Effects 0.000 claims abstract description 5
- 239000011159 matrix material Substances 0.000 claims description 35
- 238000004364 calculation method Methods 0.000 claims description 7
- 230000009897 systematic effect Effects 0.000 claims description 6
- 230000026676 system process Effects 0.000 claims description 5
- 230000009466 transformation Effects 0.000 claims description 5
- 230000001133 acceleration Effects 0.000 claims description 3
- 230000005484 gravity Effects 0.000 claims description 3
- 230000007704 transition Effects 0.000 claims description 3
- 238000000691 measurement method Methods 0.000 claims 1
- 238000012937 correction Methods 0.000 description 4
- 238000010586 diagram Methods 0.000 description 3
- 239000006185 dispersion Substances 0.000 description 2
- 230000000694 effects Effects 0.000 description 2
- 238000005516 engineering process Methods 0.000 description 2
- 238000011160 research Methods 0.000 description 2
- 238000004458 analytical method Methods 0.000 description 1
- 230000009286 beneficial effect Effects 0.000 description 1
- 238000006243 chemical reaction Methods 0.000 description 1
- 230000004927 fusion Effects 0.000 description 1
- 230000002401 inhibitory effect Effects 0.000 description 1
- 230000010354 integration Effects 0.000 description 1
- 238000011835 investigation Methods 0.000 description 1
- 230000004899 motility Effects 0.000 description 1
- 239000004575 stone Substances 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
- 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/18—Stabilised platforms, e.g. by gyroscope
Landscapes
- Engineering & Computer Science (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- Automation & Control Theory (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Navigation (AREA)
Abstract
本发明公开了一种深潜载人潜水器ADCP辅助SINS导航方法，包括以下步骤：1)、将安装在载人潜水器上的惯性测量单元提供的角运动和线运动测量作为SINS的输入，执行导航解算算法，输出SINS的导航解，即姿态、速度和位置；2)、安装在载人潜水器底部的ADCP测量载人潜水器相对水流的速度，利用重叠的ADCP测量推导出载人潜水器的对地速度和水流速度；3)、利用kalman滤波将步骤1中的SINS导航解与步骤2中获取的载人潜水器对地速度进行融合，构成SINS/ADCP组合导航以抑制SINS误差发散。本发明解决了中层水域GPS和DVL均不可用以及SINS定位误差随时间累计的问题，能够实现深潜载人潜水器下潜/上浮过程中完全自主的高精度导航定位。
Description
技术领域
本发明涉及高精度水下导航定位技术领域，尤其涉及一种深潜载人潜水器ADCP辅助SINS导航方法，特别适用于水下载体的中层水域导航。
背景技术
载人潜水器是进入深海进行科学研究和调查作业的重要运载工具，具有可以使人亲临现场、充分发挥人的主观能动性等优点，被称为“海洋学研究领域的重要基石”。高精度导航定位是载人潜水器在深海复杂环境中开展作业必不可少的条件，但高可靠性、高稳定性、高精度的水下导航也是一项重大的挑战。在水面上，可以利用全球导航卫星系统(Global Navigation Satellite System，GNSS)提供速度位置信息；在近海底，可以利用多普勒测速仪器(Doppler Velocity Log，DVL)辅助捷联惯性导航系统(Strapdown InertialNavigation System，SINS)构成SINS/DVL组合导航。然而，在远离海面或海底的这段水域，GNSS和DVL均不可用，使得中层水域导航仍然是当前水下导航面前的严峻挑战之一。
目前，声学定位系统和惯性测量单元(Inertial Measurement Unit，IMU)是中层水域XY导航唯一可用的传感器。然而，声学定位系统不仅需要水面支持母船或预先布设的专用设备，而且易受多径效应影响，且定位精度与作用距离相关。此外，尽管SINS具有自主性、数据更新率高和导航信息完备等优点，然而它是通过对IMU提供的包含固有偏置的角运动和线运动信息积分计算姿态、速度和位置，使得SINS定位误差随时间累计。
声学多普勒流速剖面仪(Acoustic Doppler Current Profiler，ADCP)已经成为现代深潜潜水器的标准配置，在现有水下导航传感器的技术条件下，利用声学多普勒流速剖面仪(ADCP)的测速信息来辅助捷联惯性导航系统(SINS)，实现一种经济可行的中层水域导航方案用于深潜载人潜水器下潜/上浮阶段至关重要。
发明内容
发明目的：针对中层水域GPS和DVL均不可用以及SINS定位误差随时间累计的问题，本发明提出一种深潜载人潜水器ADCP辅助SINS导航方法，实现深潜载人潜水器下潜/上浮过程中完全自主的高精度导航定位。
技术方案：为实现本发明的目的，本发明所采用的技术方案是：
一种深潜载人潜水器ADCP辅助SINS导航方法，包括以下步骤：
(1)将安装在载人潜水器上的惯性测量单元提供的角运动和线运动测量作为SINS的输入，执行导航解算算法，输出SINS的导航解，即姿态、速度和位置；
(2)安装在载人潜水器底部的ADCP测量载人潜水器相对水流的速度
利用重叠的ADCP测量推导出载人潜水器的对地速度vb和水流速度
(3)利用kalman滤波将步骤1中的SINS导航解与步骤2中获取的载人潜水器对地速度进行融合，构成SINS/ADCP组合导航以抑制SINS误差发散。
进一步地，所述步骤1中导航解算算法如下：
S1.1：选取当地地理坐标系作为导航坐标系，分别指向北、东、地，而载体坐标系则分别指向载体的前、右、下；记导航坐标系为n，载体坐标系为b，惯性坐标系为i以及地球坐标系为e，则导航坐标系下导航解算的微分方程为：
其中，
表示载体坐标系到导航坐标系的姿态矩阵；
表示载体坐标系相对于导航坐标系的角速率在导航坐标系下的投影；
表示地球坐标系相对于惯性坐标系的角速率在导航坐标系下的投影；
表示导航坐标系相对于地球坐标系的角速率在导航坐标系下的投影；Vn＝[VN VE VD]T是对地速度在导航坐标系下的投影，VN、VE、VD分别表示对地速度在导航坐标系中北向、东向和地向的投影；
表示加速度计输出的比力信息；gn表示重力加速度在导航坐标系下的投影；L、λ和h分别表示纬度、经度和高度；Rn、Re分别表示地球子午圈、卯酉圈曲率半径；
上述
表达式如下：
其中，
表示陀螺仪输出的角运动信息，Ω是地球自转角速率；
S1.2：已知初始时刻的姿态、速度和位置，通过对导航解算的微分方程(1)-(3)进行积分，实时递归计算得到当前时刻的姿态、速度和位置。
进一步地，所述步骤2中利用ADCP测量载人潜水器相对水流的速度方法如下：以固定频率发射声波脉冲，接收反射回来有多普勒频移的声波，通过频移计算波速向的相对速度，利用波束间夹角将波束向的相对速度转换为载体坐标系下的速度
进一步地，所述步骤2中利用重叠的ADCP测量推导载人潜水器的速度和水流速度方法如下：
S2.1：根据设定的单元深度将ADCP扫描范围划分为多个深度单元，设单元深度为d米，ADCP扫描范围为D米，则ADCP每次可以测量N＝D/d个深度单元的速度；
S2.2：初始化第一个深度单元：获取载人潜水器下潜起始时刻的姿态、速度和位置，得到第一个深度单元载人潜水器对地速度vb(bin1)，同时利用ADCP一次测量1～N深度单元载人潜水器相对水流速度
根据载体坐标系下载人潜水器对地速度表示式
计算出1～N深度单元水流速度
S2.3：载人潜水器下潜至第2个深度单元，ADCP测量获得2～(N+1)深度单元载人潜水器相对水流速度
ADCP重复观测水流深度单元2～N；
S2.4：递推计算第2个深度单元载人潜水器的对地速度vb(bin2)和第N+1个深度单元水流速度
假定从第1个深度单元下潜至第2个深度单元过程中，水流速度
保持不变，则有
于是有：
其中，
和
分别表示重叠深度单元的第二次测量和第一次测量，继而可以求出
S2.5：依次递推下去，重复执行步骤S2.3和步骤S2.4，得到所有深度单元载人潜水器的对地速度
和水流速度
进一步地，所述步骤3中利用kalman滤波技术实现SINS/ADCP组合导航的方法如下：
S3.1：选取姿态误差、速度误差、位置误差、陀螺常值误差和加速度计常值误差作为状态变量，选取速度误差作为量测变量，获得离散化的kalman滤波状态方程和量测方程；
S3.2：给定状态估计和估计误差方差的初始值
和P0，根据k时刻的观测值Zk，实时递推计算得到k时刻的状态估计
S3.3：利用步骤3.2中得到的状态估计对SINS输出的导航参数进行修正。
进一步地，所述步骤S3.1包括：
S3.1.1：选取姿态误差、速度误差、位置误差、陀螺常值误差和加速度计常值误差作为状态变量，状态变量X表示如下：
其中，φ表示欧拉失准角，δVn表示速度误差，δp＝[δL δλ δh]T表示位置误差，εb表示陀螺常值误差，
表示加速度计常值误差；
S3.1.2：选取速度误差作为量测变量，表示如下：
其中，
表示导航解算获得的导航坐标系下的对地速度，
表示导航解算获得的姿态矩阵，
表示利用ADCP获取的载体坐标系下的对地速度，上标～表示其包含误差；
S3.1.3：根据SINS误差传播方程和获取的速度信息，得到离散化的卡尔曼滤波状态方程和量测方程：
其中，Xk为k时刻的状态估计，Xk-1为k-1时刻的状态估计，Zk为k时刻的观测值，φk,k-1为非奇异状态一步转移矩阵，Γk,k-1为系统过程噪声输入矩阵，Hk为观测矩阵，Wk-1是系统随机过程噪声序列，Vk是系统随机量测噪声序列；
S3.1.4：系统过程噪声和观测噪声的统计特性，设定如下：
其中，Qk是系统过程噪声Wk的方差矩阵，Rk是系统量测噪声Vk的方差矩阵，k和j表示时间序列中的两个不同时刻，δkj是Kronecker-δ函数。
所述步骤S3.2具体求解过程如下：
状态一步预测：
状态估计：
滤波增益矩阵：
一步预测误差方差矩阵：
估计误差方差矩阵：Pk＝[I-KkHk]Pk,k-1；
其中，I表示单位矩阵。
进一步地，所述方法还包括步骤(4)：利用步骤3中获取的姿态矩阵对步骤2中获取的水流速度
进行坐标变换，获得导航坐标系下的水流速度
有益效果：与现有技术相比，本发明的技术方案具有以下有益的技术效果：
1、本发明提出了一种ADCP辅助SINS的导航方法，该方法仅仅只需利用安装在载人潜水器上的多普勒流速剖面仪(ADCP)和惯性测量单元(IMU)的测量数据进行运算，且ADCP和IMU是现代水下潜水器的标准配置，因此本发明不需要额外的导航传感器就能实现下潜/上浮过程中的组合导航，方法简便快捷。
2、本发明提出的导航算法能够有效地抑制SINS定位误差发散，使得深潜载人潜水器下潜至海底时获得高精度的导航定位结果。
3、本发明利用ADCP重叠的测量来推导载人潜水器的对地速度和水流速度，使得不仅可以获取载体下潜/上浮过程中的对地速度，而且还能获得不同深度的水流速度剖面。提出的导航算法能够利用修正后的姿态信息将获得的载体坐标系下的水流速度剖面投影到导航坐标系下，可无需姿态传感器实现ADCP的测速功能。
附图说明
图1为根据本发明实施例的ADCP辅助SINS导航框架图；
图2为根据本发明实施例的ADCP测速示意图。
具体实施方式
下面结合附图对本发明的技术方案作进一步说明。
本发明在不考虑声学定位系统的条件下，利用安装在载人潜水器底部的ADCP测量载人潜水器相对水流的速度，进一步利用重叠的ADCP测量推导出对地速度和不同深度的水流速度剖面；通过kalman滤波技术，将获取的对地速度与SINS输出的导航参数进行信息融合，构成速度匹配组合导航以抑制SINS定位误差发散，从而实现深潜载人潜水器下潜/上浮过程中完全自主的高精度导航定位。如图1所示，本发明所述的一种深潜载人潜水器ADCP辅助SINS导航方法，包括以下步骤：
步骤1：将安装在载人潜水器上的惯性测量单元(IMU)提供的角运动和线运动测量作为捷联惯性导航系统(SINS)的输入，执行导航解算算法，输出捷联惯性导航系统(SINS)的导航参数，即姿态、速度和位置。
惯性测量单元(IMU)包括三个两两正交的陀螺仪和三个两两正交的加速度计，陀螺仪输出载体的角运动信息
加速度计输出载体的线运动信息
步骤1所述导航解算算法如下：
步骤1.1：选取当地地理坐标系作为导航坐标系，分别指向北、东、地，而载体坐标系则分别指向载体的前、右、下；记导航坐标系为n，载体坐标系为b，惯性坐标系为i以及地球坐标系为e，则n系下导航解算的微分方程为：
其中，
表示载体坐标系到导航坐标系的姿态矩阵；
表示载体坐标系相对于导航坐标系的角速率在导航坐标系下的投影；
表示地球坐标系相对于惯性坐标系的角速率在导航坐标系下的投影；
表示导航坐标系相对于地球坐标系的角速率在导航坐标系下的投影；Vn＝[VN VE VD]T是对地速度在导航坐标系的投影；VN、VE、VD分别表示对地速度在导航坐标系中北向、东向和地向的投影；
表示加速度计输出的比力信息；gn表示重力加速度在导航系下的投影；L、λ和h分别表示纬度、经度和高度；Rn、Re分别表示地球子午圈、卯酉圈曲率半径；
所述
表达式如下：
其中，
表示陀螺仪输出的角运动信息，Ω是地球自转角速率；
步骤1.2：已知初始时刻的姿态、速度和位置，通过对导航解算微分方程进行积分可以实时递归计算得到当前时刻的姿态、速度和位置。
由于陀螺仪输出载体的角运动信息
和加速度计输出载体的线运动信息
包含固有偏置，从而使得基于积分运算获得的捷联惯性导航系统(SINS)的导航参数误差随时间累积。
步骤2：安装在载人潜水器底部的声学多普勒测速仪(ADCP)可以测量载人潜水器相对水流的速度，利用重叠的ADCP测量可以推导出载人潜水器的对地速度和水流速度。
在载体坐标系下，载人潜水器对地速度可以表示为：
其中，
表示载人潜水器相对水流速度，
表示水流速度。
步骤2.1：利用ADCP测量载人潜水器相对水流速度
图2为ADCP测速示意图。如图2所示，ADCP安装在载人潜水器底部，装有4个声换能器，声束轴线(即换能器轴线)与ADCP轴线的夹角称为声束角，通常为20°、25°或30°。实施例中每个换能器的发射声线与剖面仪轴线成投射角30°。测速工作流程为：以固定频率发射声波脉冲，接收反射回来有“多普勒”频移的声波，通过频移计算波速向的相对速度，利用声束角将波束向(波束坐标)的相对速度转换为载体坐标系下的速度
步骤2.2：利用重叠的ADCP测量推导出载人潜水器的对地速度vb和水流速度
实施例中，设置深度单元为1m，ADCP扫描范围为20m，则ADCP每次可以测量20个深度单元的速度。应当理解，这里的数值仅是举例说明的作用，在其他实施例中可以根据实际情况进行相应的设置。
具体算法流程可以表示为：
步骤2.2.1：初始化第一个深度单元，获取vb(bin1)、
和
假定载人潜水器在海面上停留几分钟，此时可以利用全球定位系统(GPS)对捷联惯性导航系统进行(SINS)初始对准，即提供下潜起始时刻的姿态、速度和位置。由此可以获得第一个深度单元载人潜水器对地速度vb(bin1)，同时可以利用ADCP一次测量1～20深度单元载人潜水器相对水流速度
由此可以计算出1～20深度单元水流速度
计算公式为：
步骤2.2.2：载人潜水器下潜至第2个深度单元，ADCP测量获得2～21深度单元载人潜水器相对水流速度
与步骤2.2.1相比，ADCP重复观测水流深度单元2-20。
步骤2.2.3：递推计算第2个深度单元载人潜水器的对地速度vb(bin2)和第21个深度单元水流速度
假定从第1个深度单元下潜至第2个深度单元过程中，水流速度
保持不变。根据方程(7)可以推导得到：
于是有
其中，
和
分别表示重叠深度单元的第二次测量和第一次测量。继而可以求出
步骤2.2.4：依次递推下去，重复步骤2.2.2和步骤2.2.3里面的操作，可以得到所有深度单元载人潜水器的对地速度
和水流速度
步骤3：利用kalman滤波技术，将步骤1中的捷联惯性导航系统(SINS)与步骤2中获取的载人潜水器对地速度有机融合，构成SINS/ADCP组合导航以抑制SINS误差发散。具体方法如下：
步骤3.1：根据选取的状态变量和量测变量，获得离散化的kalman滤波状态方程和量测方程；
选取姿态误差、速度误差、位置误差、陀螺常值误差和加速度计常值误差作为状态变量，状态变量X表示如下：
其中，φ表示欧拉失准角，δVn表示速度误差，δp＝[δL δλ δh]T表示位置误差，εb表示陀螺常值误差，
表示加速度计常值误差；
选取速度误差作为量测变量，表示如下：
其中，
表示导航解算获得的导航坐标系下的对地速度，
表示导航解算获得的姿态矩阵，
表示利用ADCP获取的载体坐标系下的对地速度，在此上标～表示其包含误差。
根据SINS误差传播方程和获取的速度信息，得到离散化的卡尔曼滤波状态方程和量测方程：
其中，Xk为k时刻的状态估计，Xk-1为k-1时刻的状态估计，Zk为k时刻的观测值，φk,k-1为非奇异状态一步转移矩阵，Γk,k-1为系统过程噪声输入矩阵，Hk为观测矩阵，Wk-1是系统随机过程噪声序列，Vk是系统随机量测噪声序列；
系统过程噪声和观测噪声的统计特性，设定如下：
其中，Qk是系统过程噪声Wk的方差矩阵，Rk是系统量测噪声Vk的方差矩阵，k和j表示时间序列中的两个不同时刻，δkj是Kronecker-δ函数。
步骤3.2：给定状态估计和估计误差方差的初始值
和P0，根据k时刻的观测值Zk，实时递推计算得到k时刻的状态估计
具体求解过程如下：
状态一步预测：
状态估计：
滤波增益矩阵：
一步预测误差方差矩阵：
估计误差方差矩阵：Pk＝[I-KkHk]Pk,k-1；
其中，I表示单位矩阵；
步骤3.3：利用步骤3.2中获得的状态估计去校正SINS中输出的导航参数，以抑制定位误差的发散。
姿态、速度、位置校正公式如下：
公式左边是校正后的导航参数，右边是SINS输出的导航参数且相关校正项由滤波估计得到，每次滤波估计出状态变量后用于校正。当然，本领域技术人员应当了解，并不是所有状态变量都一定会用于校正，还涉及到可观测性/可观测度分析。
步骤4：利用步骤3.3中修正过后的姿态矩阵对步骤2中获得的水流速度
进行坐标变换，可以获得导航坐标系下的水流速度
其中，
表示载体坐标系相对于导航坐标系的变换关系，即载体坐标系到导航坐标系的姿态矩阵。
获得水流速度可实现ADCP自身的功能。ADCP单独使用时，就是用来测量不同深度的水流速度，为了将测量得到的载体坐标系下的速度转换到导航坐标系下，还需要配备姿态传感器。在本发明中，一方面，可以利用ADCP的速度信息去抑制SINS输出的导航参数的误差发散；另一方面，可以利用修正的姿态信息实现速度转换，不需要额外配备姿态传感器。
Claims (8)
1.一种深潜载人潜水器ADCP辅助SINS导航方法，其特征在于，所述方法包括以下步骤：
(1)将安装在载人潜水器上的惯性测量单元提供的角运动和线运动测量作为SINS的输入，执行导航解算算法，输出SINS的导航解，即姿态、速度和位置；
(2)安装在载人潜水器底部的ADCP测量载人潜水器相对水流的速度
利用重叠的ADCP测量推导出载人潜水器的对地速度vb和水流速度
具体包括如下步骤：
S2.1：根据设定的单元深度将ADCP扫描范围划分为多个深度单元，设单元深度为d米，ADCP扫描范围为D米，则ADCP每次测量N＝D/d个深度单元的速度；
S2.2：初始化第一个深度单元：获取载人潜水器下潜起始时刻的姿态、速度和位置，得到第一个深度单元载人潜水器对地速度vb(bin1)，同时利用ADCP一次测量1～N深度单元载人潜水器相对水流速度
根据载体坐标系下载人潜水器对地速度表示式
计算出1～N深度单元水流速度
S2.3：载人潜水器下潜至第2个深度单元，ADCP测量获得2～(N+1)深度单元载人潜水器相对水流速度
ADCP重复观测水流深度单元2～N；
S2.4：递推计算第2个深度单元载人潜水器的对地速度vb(bin2)和第N+1个深度单元水流速度
假定从第1个深度单元下潜至第2个深度单元过程中，水流速度
保持不变，则有
于是有：
其中，
和
分别表示重叠深度单元的第二次测量和第一次测量，继而求出：
S2.5：依次递推下去，重复执行步骤S2.3和步骤S2.4，得到所有深度单元载人潜水器的对地速度
和水流速度
(3)利用kalman滤波将步骤(1)中的SINS导航解与步骤(2)中获取的载人潜水器对地速度进行融合，构成SINS/ADCP组合导航模型，求解得到组合导航的状态估计，对SINS输出的导航参数进行修正。
2.根据权利要求1所述的一种深潜载人潜水器ADCP辅助SINS导航方法，其特征在于，所述步骤(1)中导航解算算法如下：
S1.1：选取当地地理坐标系作为导航坐标系，分别指向北、东、地，而载体坐标系则分别指向载体的前、右、下；记导航坐标系为n，载体坐标系为b，惯性坐标系为i以及地球坐标系为e，则导航坐标系下导航解算的微分方程为：
其中，
表示载体坐标系到导航坐标系的姿态矩阵；
表示载体坐标系相对于导航坐标系的角速率在导航坐标系下的投影；
表示地球坐标系相对于惯性坐标系的角速率在导航坐标系下的投影；
表示导航坐标系相对于地球坐标系的角速率在导航坐标系下的投影；Vn＝[VN VE VD]T是对地速度在导航坐标系下的投影，VN、VE、VD分别表示对地速度在导航坐标系中北向、东向和地向的投影；
表示加速度计输出的比力信息；gn表示重力加速度在导航坐标系下的投影；L、λ和h分别表示纬度、经度和高度；Rn、Re分别表示地球子午圈、卯酉圈曲率半径；
上述
表达式如下：
其中，
表示陀螺仪输出的角运动信息，Ω是地球自转角速率；
S1.2：已知初始时刻的姿态、速度和位置，通过对导航解算的微分方程(1)-(3)进行积分，实时递归计算得到当前时刻的姿态、速度和位置。
3.根据权利要求2所述的一种深潜载人潜水器ADCP辅助SINS导航方法，其特征在于，所述步骤(2)中利用ADCP测量载人潜水器相对水流的速度方法如下：以固定频率发射声波脉冲，接收反射回来有多普勒频移的声波，通过频移计算波速向的相对速度，利用波束角将波束向的相对速度转换为载体坐标系下的速度
4.根据权利要求1所述的一种深潜载人潜水器ADCP辅助SINS导航方法，其特征在于，所述步骤(3)中利用kalman滤波技术实现SINS/ADCP组合导航的方法如下：
S3.1：选取姿态误差、速度误差、位置误差、陀螺常值误差和加速度计常值误差作为状态变量，选取速度误差作为量测变量，获得离散化的kalman滤波状态方程和量测方程；
S3.2：给定状态估计和估计误差方差的初始值
和P0，根据k时刻的观测值Zk，实时递推计算得到k时刻的状态估计
S3.3：利用步骤3.2中得到的状态估计对SINS输出的导航参数进行修正。
5.根据权利要求4所述的一种深潜载人潜水器ADCP辅助SINS导航方法，其特征在于，所述步骤S3.1包括：
S3.1.1：选取姿态误差、速度误差、位置误差、陀螺常值误差和加速度计常值误差作为状态变量，状态变量X表示如下：
X＝[(φ)T (δVn)T (δp)T (εb)T (▽b)T]T (11)
其中，φ表示欧拉失准角，δVn表示速度误差，δp＝[δL δλ δh]T表示位置误差，εb表示陀螺常值误差，▽b表示加速度计常值误差；
S3.1.2：选取速度误差作为量测变量，表示如下：
其中，
表示导航解算获得的导航坐标系下的对地速度，
表示导航解算获得的姿态矩阵，
表示利用ADCP获取的载体坐标系下的对地速度，上标～表示其包含误差；
S3.1.3：根据SINS误差传播方程和获取的速度信息，得到离散化的卡尔曼滤波状态方程和量测方程：
其中，Xk为k时刻的状态估计，Xk-1为k-1时刻的状态估计，Zk为k时刻的观测值，φk,k-1为非奇异状态一步转移矩阵，Γk,k-1为系统过程噪声输入矩阵，Hk为观测矩阵，Wk-1是系统随机过程噪声序列，Vk是系统随机量测噪声序列；
S3.1.4：系统过程噪声和观测噪声的统计特性，设定如下：
其中，Qk是系统过程噪声Wk的方差矩阵，Rk是系统量测噪声Vk的方差矩阵，k和j表示时间序列中的两个不同时刻，δkj是Kronecker-δ函数。
6.根据权利要求5所述的一种深潜载人潜水器ADCP辅助SINS导航方法，其特征在于，所述步骤S3.2具体求解过程如下：
状态一步预测：
状态估计：
滤波增益矩阵：
一步预测误差方差矩阵：
估计误差方差矩阵：Pk＝[I-KkHk]Pk,k-1；
其中，I表示单位矩阵。
7.根据权利要求1所述的一种深潜载人潜水器ADCP辅助SINS导航方法，其特征在于，所述方法还包括步骤(4)：利用步骤(3)中获取的姿态矩阵对步骤(2)中获取的水流速度
进行坐标变换，获得导航坐标系下的水流速度
8.根据权利要求7所述的一种深潜载人潜水器ADCP辅助SINS导航方法，其特征在于，所述坐标变换表达式为：
其中，
表示载体坐标系到导航坐标系的姿态矩阵。
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201910535185.9A CN110274591B (zh) | 2019-06-20 | 2019-06-20 | 深潜载人潜水器adcp辅助sins导航方法 |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201910535185.9A CN110274591B (zh) | 2019-06-20 | 2019-06-20 | 深潜载人潜水器adcp辅助sins导航方法 |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN110274591A CN110274591A (zh) | 2019-09-24 |
| CN110274591B true CN110274591B (zh) | 2020-11-20 |
Family
ID=67961168
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN201910535185.9A Expired - Fee Related CN110274591B (zh) | 2019-06-20 | 2019-06-20 | 深潜载人潜水器adcp辅助sins导航方法 |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN110274591B (zh) |
Families Citing this family (6)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN110873813B (zh) * | 2019-12-02 | 2021-09-28 | 中国人民解放军战略支援部队信息工程大学 | 一种水流速度估算方法、组合导航方法及装置 |
| CN111288988A (zh) * | 2020-02-20 | 2020-06-16 | 云南电网有限责任公司电力科学研究院 | 一种架空机器人组合定位方法 |
| CN111366156A (zh) * | 2020-04-17 | 2020-07-03 | 云南电网有限责任公司电力科学研究院 | 基于神经网络辅助的变电站巡检机器人导航方法及系统 |
| CN112684207B (zh) * | 2020-12-17 | 2022-03-11 | 东南大学 | 一种深潜载人潜水器adcp速度估计与修正算法 |
| CN112729291B (zh) * | 2020-12-29 | 2022-03-04 | 东南大学 | 一种深潜长航潜水器sins/dvl洋流速度估计方法 |
| CN114295136B (zh) * | 2021-12-22 | 2023-12-01 | 北京理工大学 | 一种基于视觉辅助的海空协同抗洋流干扰导航方法 |
Family Cites Families (6)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN102519450B (zh) * | 2011-12-12 | 2014-07-02 | 东南大学 | 一种用于水下滑翔器的组合导航装置及方法 |
| CN104870940A (zh) * | 2012-10-29 | 2015-08-26 | 德立文亚迪仪器公司 | 用于水柱辅助导航的系统和方法 |
| CN102980579B (zh) * | 2012-11-15 | 2015-04-08 | 哈尔滨工程大学 | 一种自主水下航行器自主导航定位方法 |
| CN104502633B (zh) * | 2014-12-29 | 2018-04-13 | 南京世海声学科技有限公司 | 一种用于声学多普勒流速剖面仪的流场数据校正方法 |
| CN105021843A (zh) * | 2015-07-28 | 2015-11-04 | 江苏中海达海洋信息技术有限公司 | 600kHZ宽带声学多普勒流速剖面仪及其实现方法 |
| CN107063245B (zh) * | 2017-04-19 | 2020-12-25 | 东南大学 | 一种基于5阶ssrckf的sins/dvl组合导航滤波方法 |
-
2019
- 2019-06-20 CN CN201910535185.9A patent/CN110274591B/zh not_active Expired - Fee Related
Also Published As
| Publication number | Publication date |
|---|---|
| CN110274591A (zh) | 2019-09-24 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN110274591B (zh) | 深潜载人潜水器adcp辅助sins导航方法 | |
| CN109443379B (zh) | 一种深海潜航器的sins/dvl水下抗晃动对准方法 | |
| CN109737956B (zh) | 一种基于双应答器的sins/usbl相位差紧组合导航定位方法 | |
| CN104457754B (zh) | 一种基于sins/lbl紧组合的auv水下导航定位方法 | |
| Kepper et al. | A navigation solution using a MEMS IMU, model-based dead-reckoning, and one-way-travel-time acoustic range measurements for autonomous underwater vehicles | |
| CN103744098B (zh) | 基于sins/dvl/gps的auv组合导航系统 | |
| Chang et al. | Initial alignment for a Doppler velocity log-aided strapdown inertial navigation system with limited information | |
| Li et al. | A novel backtracking navigation scheme for autonomous underwater vehicles | |
| CN106767793A (zh) | 一种基于sins/usbl紧组合的auv水下导航定位方法 | |
| CN107797125B (zh) | 一种减小深海探测型auv导航定位误差的方法 | |
| CN110345941B (zh) | 深潜载人潜水器sins自辅助导航方法 | |
| CN104316045A (zh) | 一种基于sins/lbl的auv水下交互辅助定位系统及定位方法 | |
| CN111076728A (zh) | 基于dr/usbl的深潜载人潜水器组合导航方法 | |
| CN105091907A (zh) | Sins/dvl组合中dvl方位安装误差估计方法 | |
| CN112684207B (zh) | 一种深潜载人潜水器adcp速度估计与修正算法 | |
| CN105806363A (zh) | 基于srqkf的sins/dvl水下大失准角对准方法 | |
| CN104061930B (zh) | 一种基于捷联惯性制导和多普勒计程仪的导航方法 | |
| CN111982105B (zh) | 一种基于sins/lbl紧组合的水下导航定位方法及系统 | |
| Énonsen et al. | Recent developments in the HUGIN AUV terrain navigation system | |
| Ji et al. | Navigation and Control of Underwater Tracked Vehicle Using Ultrashort Baseline and Ring Laser Gyro Sensors. | |
| Stanway | Dead reckoning through the water column with an acoustic Doppler current profiler: Field experiences | |
| Allotta et al. | Localization algorithm for a fleet of three AUVs by INS, DVL and range measurements | |
| CN114459476A (zh) | 基于虚拟速度量测的水下无人潜航器测流dvl/sins组合导航方法 | |
| Wang et al. | AUV navigation based on inertial navigation and acoustic positioning systems | |
| CN110873813B (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 | ||
| CF01 | Termination of patent right due to non-payment of annual fee | ||
| CF01 | Termination of patent right due to non-payment of annual fee |
Granted publication date: 20201120 |