[发明专利]深潜载人潜水器ADCP辅助SINS导航方法

权利要求书

1.一种深潜载人潜水器ADCP辅助SINS导航方法，其特征在于，所述方法包括以下步骤：

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

(2)安装在载人潜水器底部的ADCP测量载人潜水器相对水流的速度利用重叠的ADCP测量推导出载人潜水器的对地速度v^b和水流速度具体包括如下步骤：

S2.1：根据设定的单元深度将ADCP扫描范围划分为多个深度单元，设单元深度为d米，ADCP扫描范围为D米，则ADCP每次测量N＝D/d个深度单元的速度；

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

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

S2.4：递推计算第2个深度单元载人潜水器的对地速度v^b(bin₂)和第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，则导航坐标系下导航解算的微分方程为：

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

上述表达式如下：

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

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

3.根据权利要求2所述的一种深潜载人潜水器ADCP辅助SINS导航方法，其特征在于，所述步骤(2)中利用ADCP测量载人潜水器相对水流的速度方法如下：以固定频率发射声波脉冲，接收反射回来有多普勒频移的声波，通过频移计算波速向的相对速度，利用波束角将波束向的相对速度转换为载体坐标系下的速度