深潜载人潜水器ADCP辅助SINS导航方法与流程

本发明涉及高精度水下导航定位技术领域，尤其涉及一种深潜载人潜水器adcp辅助sins导航方法，特别适用于水下载体的中层水域导航。

背景技术：

载人潜水器是进入深海进行科学研究和调查作业的重要运载工具，具有可以使人亲临现场、充分发挥人的主观能动性等优点，被称为“海洋学研究领域的重要基石”。高精度导航定位是载人潜水器在深海复杂环境中开展作业必不可少的条件，但高可靠性、高稳定性、高精度的水下导航也是一项重大的挑战。在水面上，可以利用全球导航卫星系统(globalnavigationsatellitesystem，gnss)提供速度位置信息；在近海底，可以利用多普勒测速仪器(dopplervelocitylog，dvl)辅助捷联惯性导航系统(strapdowninertialnavigationsystem，sins)构成sins/dvl组合导航。然而，在远离海面或海底的这段水域，gnss和dvl均不可用，使得中层水域导航仍然是当前水下导航面前的严峻挑战之一。

目前，声学定位系统和惯性测量单元(inertialmeasurementunit，imu)是中层水域xy导航唯一可用的传感器。然而，声学定位系统不仅需要水面支持母船或预先布设的专用设备，而且易受多径效应影响，且定位精度与作用距离相关。此外，尽管sins具有自主性、数据更新率高和导航信息完备等优点，然而它是通过对imu提供的包含固有偏置的角运动和线运动信息积分计算姿态、速度和位置，使得sins定位误差随时间累计。

声学多普勒流速剖面仪(acousticdopplercurrentprofiler，adcp)已经成为现代深潜潜水器的标准配置，在现有水下导航传感器的技术条件下，利用声学多普勒流速剖面仪(adcp)的测速信息来辅助捷联惯性导航系统(sins)，实现一种经济可行的中层水域导航方案用于深潜载人潜水器下潜/上浮阶段至关重要。

技术实现要素：

发明目的：针对中层水域gps和dvl均不可用以及sins定位误差随时间累计的问题，本发明提出一种深潜载人潜水器adcp辅助sins导航方法，实现深潜载人潜水器下潜/上浮过程中完全自主的高精度导航定位。

技术方案：为实现本发明的目的，本发明所采用的技术方案是：

一种深潜载人潜水器adcp辅助sins导航方法，包括以下步骤：

(1)将安装在载人潜水器上的惯性测量单元提供的角运动和线运动测量作为sins的输入，执行导航解算算法，输出sins的导航解，即姿态、速度和位置；

(2)安装在载人潜水器底部的adcp测量载人潜水器相对水流的速度利用重叠的adcp测量推导出载人潜水器的对地速度v^b和水流速度

(3)利用kalman滤波将步骤1中的sins导航解与步骤2中获取的载人潜水器对地速度进行融合，构成sins/adcp组合导航以抑制sins误差发散。

进一步地，所述步骤1中导航解算算法如下：

s1.1：选取当地地理坐标系作为导航坐标系，分别指向北、东、地，而载体坐标系则分别指向载体的前、右、下；记导航坐标系为n，载体坐标系为b，惯性坐标系为i以及地球坐标系为e，则导航坐标系下导航解算的微分方程为：

其中，表示载体坐标系到导航坐标系的姿态矩阵；表示载体坐标系相对于导航坐标系的角速率在导航坐标系下的投影；表示地球坐标系相对于惯性坐标系的角速率在导航坐标系下的投影；表示导航坐标系相对于地球坐标系的角速率在导航坐标系下的投影；vⁿ＝[vnvevd]^t是对地速度在导航坐标系下的投影，vn、ve、vd分别表示对地速度在导航坐标系中北向、东向和地向的投影；表示加速度计输出的比力信息；gⁿ表示重力加速度在导航坐标系下的投影；l、λ和h分别表示纬度、经度和高度；rn、re分别表示地球子午圈、卯酉圈曲率半径；

上述表达式如下：

其中，表示陀螺仪输出的角运动信息，ω是地球自转角速率；

s1.2：已知初始时刻的姿态、速度和位置，通过对导航解算的微分方程(1)-(3)进行积分，实时递归计算得到当前时刻的姿态、速度和位置。

进一步地，所述步骤2中利用adcp测量载人潜水器相对水流的速度方法如下：以固定频率发射声波脉冲，接收反射回来有多普勒频移的声波，通过频移计算波速向的相对速度，利用波束间夹角将波束向的相对速度转换为载体坐标系下的速度

进一步地，所述步骤2中利用重叠的adcp测量推导载人潜水器的速度和水流速度方法如下：

s2.1：根据设定的单元深度将adcp扫描范围划分为多个深度单元，设单元深度为d米，adcp扫描范围为d米，则adcp每次可以测量n＝d/d个深度单元的速度；

s2.2：初始化第一个深度单元：获取载人潜水器下潜起始时刻的姿态、速度和位置，得到第一个深度单元载人潜水器对地速度v^b(bin1)，同时利用adcp一次测量1～n深度单元载人潜水器相对水流速度根据载体坐标系下载人潜水器对地速度表示式计算出1～n深度单元水流速度

s2.3：载人潜水器下潜至第2个深度单元，adcp测量获得2～(n+1)深度单元载人潜水器相对水流速度adcp重复观测水流深度单元2～n；

s2.4：递推计算第2个深度单元载人潜水器的对地速度v^b(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表示如下：

其中，φ表示欧拉失准角，δvⁿ表示速度误差，δ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系下导航解算的微分方程为：

其中，表示载体坐标系到导航坐标系的姿态矩阵；表示载体坐标系相对于导航坐标系的角速率在导航坐标系下的投影；表示地球坐标系相对于惯性坐标系的角速率在导航坐标系下的投影；表示导航坐标系相对于地球坐标系的角速率在导航坐标系下的投影；vⁿ＝[vnvevd]^t是对地速度在导航坐标系的投影；vn、ve、vd分别表示对地速度在导航坐标系中北向、东向和地向的投影；表示加速度计输出的比力信息；gⁿ表示重力加速度在导航系下的投影；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测量推导出载人潜水器的对地速度v^b和水流速度

实施例中，设置深度单元为1m，adcp扫描范围为20m，则adcp每次可以测量20个深度单元的速度。应当理解，这里的数值仅是举例说明的作用，在其他实施例中可以根据实际情况进行相应的设置。

具体算法流程可以表示为：

步骤2.2.1：初始化第一个深度单元，获取v^b(bin1)、和

假定载人潜水器在海面上停留几分钟，此时可以利用全球定位系统(gps)对捷联惯性导航系统进行(sins)初始对准，即提供下潜起始时刻的姿态、速度和位置。由此可以获得第一个深度单元载人潜水器对地速度v^b(bin1)，同时可以利用adcp一次测量1～20深度单元载人潜水器相对水流速度由此可以计算出1～20深度单元水流速度计算公式为：

步骤2.2.2：载人潜水器下潜至第2个深度单元，adcp测量获得2～21深度单元载人潜水器相对水流速度与步骤2.2.1相比，adcp重复观测水流深度单元2-20。

步骤2.2.3：递推计算第2个深度单元载人潜水器的对地速度v^b(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表示如下：

其中，φ表示欧拉失准角，δvⁿ表示速度误差，δ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输出的导航参数的误差发散；另一方面，可以利用修正的姿态信息实现速度转换，不需要额外配备姿态传感器。