[发明专利]基于虚拟速度量测的水下无人潜航器测流DVL/SINS组合导航方法

权利要求书

1.基于虚拟速度量测的中层水域SINS/测流DVL组合导航方法，其特征在于，具体方法如下：

(1)获取安装在水下无人潜航器上的传感器实时数据，具体包括多普勒测速仪的速度量测数据以及惯性测量元件所输出的陀螺仪和加速度计的输出数据，并利用传感器数据执行姿态解算算法，获得SINS系统以及DVL系统的导航系下速度，具体步骤如下：

S1.1：选取当地地理坐标系作为导航坐标系，分别指向东、北、天，记导航坐标系为n；载体坐标系原点位于载体重心，三个轴分别指向载体的右、前、上，记载体坐标系为b；记惯性坐标系为i、地球坐标系为e，则导航坐标系下导航解算的微分方程如式(1)～(4)：

其中，q表示载体坐标系到导航坐标系的旋转四元数；ωnbb表示载体系b和导航系n之间的相对角速度在载体系b下的投影；ωien表示地球系e与惯性系i的相对角速度在导航系n下的投影；ωenn表示导航系n与地球系e之间的相对角速度在导航系n下的投影；Vn＝vEvNvUT是对地速度在导航系n下的投影；fb表示加速度计输出的比力信息；gn表示重力加速度在导航系n下的投影；L、λ和h分别表示纬度、经度和高度；R_n、R_e分别表示地球子午圈、卯酉圈曲率半径；

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

S1.3：DVL传感器安装在载体的底部，通过向下发送超声波以完成对载体速度的测量。高精度的DVL传感器分别在载体的前后左右四个方向发送超声波束，可获得AUV在载体坐标系下的三维速度Vdvlb＝[vdvlrightvdvlfrontvdvlup]。然后，利用S1.2中SINS系统解算获得的AUV姿态，可获取到导航坐标系下的三维速度：

(2)利用步骤(1)中获得的IMU输出、SINS系统以及DVL系统的导航系下速度，构建一组AUV的虚拟对地速度量测；

步骤(2)中t时刻虚拟速度量测的构建方程具体为：

式中，代表构建的t时刻不含洋流干扰的由虚拟东向速度量测和虚拟北向速度量测组成的虚拟速度量测向量，为由SINS在t-1时刻经过捷联解算算法获得的东向速度和北向速度组成的向量，为从t-1时刻到t时刻构建的速度变化量，其计算方式如下：

式中，为DVL测量误差协方差矩阵，而为SINS系统的速度估计误差协方差矩阵；

(3)将步骤(2)中构建的水下无人潜航器虚拟对地速度量测与步骤(1)中获得SINS系统、DVL系统速度输出一同输入至卡尔曼滤波器中，进行信息融合，完成对水下无人潜航器的定位与导航，具体步骤如下；

S3.1：选取SINS系统姿态失准角、SINS系统导航系速度误差、SINS系统位置误差、陀螺常值误差和、速度计常值误差以及水流对地速度作为17维状态变量，选取DVL系统与SINS系统导航系速度误差、SINS系统与高度计高度测量误差以及SINS系统与虚拟速度量测导航系速度误差作为5维量测变量，获得离散化的kalman滤波状态方程和量测方程；

S3.2：给定状态估计和估计误差方差的初始值X0和P0，根据k时刻的观测值Z(k)，实时递推计算得到k时刻的状态估计Xkk；

S3.3：利用步骤3.2中得到的状态估计对SINS输出的导航参数进行修正。

2.根据权利要求1所述的基于虚拟速度量测的水下无人潜航器测流DVL/SINS组合导航方法，其特征在于，

ΔV_est的计算方式如下：

式中，f^b(t)为加速度计在t时刻的比力输出值，为SINS系统t时刻解算获得的姿态矩阵，φ为SINS系统计算导航系与实际的地理导航系之间的失准角，b_a为SINS中加速度计的常值零偏估计值。