[发明专利]一种大潜深AUV下潜定位方法

权利要求书

1.一种大潜深AUV下潜定位方法，其特征在于，包括：

步骤一：通过大潜深AUV搭载水下通信节点、捷联惯导系统以及温盐深传感器，得到大潜深AUV的基本信息；

步骤二：构建大潜深AUV的纯距离误差估计滤波模型；

1)设短时间内捷联惯导系统的位置误差和速度误差均为常值；

2)建立大潜深AUV的纯距离误差估计滤波模型的离散系统方程为：

X_k＝Φ_kX_k-1+w_k

其中，

上式中，分别为大潜深AUV在NED导航坐标系下惯导系统北向和东向的位置误差，分别为大潜深AUV在载体坐标系下纵向和横向测速误差，T为采样时间，w_k～N(0,Q)，Q为w_k的4×4协方差矩阵，ψ、θ、γ分别为大潜深AUV的艏向、纵倾角和横倾角；

3)将母船同大潜深AUV的测距信息r_k和大潜深AUV温盐深传感器输出的深度信息d_k作为量测信息，设观测噪声为加性白噪声，建立大潜深AUV纯距离误差估计滤波模型的观测方程为：

z_k＝h(X_k)+v_k

其中，

上式中，为NED导航坐标系下大潜深AUV捷联惯导系统北向和东向定位信息，为NED导航坐标系下水面母船北向和东向定位信息，v_k～N(0,R)，R为v_k的1×1协方差；

步骤三：根据同步水声通信协议，水面母船向大潜深AUV发送水面母船各时刻在NED坐标系下的水平位置信息以及相应的时间信息；

步骤四：根据大潜深AUV水声通信系统接收到的步骤三所述时间信息，大潜深AUV记录此时的捷联惯导系统定位信息以及此时的温盐深传感器输出的温度、导电率和深度信息；

步骤五：根据同步水声通信协议，通过传输时间和温盐深传感器信息，大潜深AUV计算得到自身到母船的测距信息；

步骤六：构建强跟踪UKF算法模型；

1)设强跟踪UKF的初始状态为：

上式中，0_4×1、0_4×4分别为4维零矢量和4维全零方阵；

2)时间更新：为状态的估计值，不考虑系统协方差矩阵Q，则滤波状态协方差矩阵的时间更新为：

3)根据水面母船水平定位信息以及大潜深AUV捷联惯导系统定位信息，计算观测方程参数和

4)根据观测方程参数和基于滤波状态的时间更新通过确定性观测模型z_k＝h(X_k)预测量测信息为：

上式中，为状态矢量的前两项；

5)根据母船和大潜深AUV的测距信息r和大潜深AUV温盐深传感器输出的深度信息d，得到强跟踪UKF的观测信息z_k为：

6)计算量测信息残差为：

7)计算残差协方差矩阵为：

上式中，ρ为遗忘因子，且0ρ≤1，ρ＝0.95；

8)基于UT变换计算系统协方差矩阵Q通过确定性观测模型h的传播Q_zz；根据和Q，求取Sigma点集{ζ_i}，(i＝1,…，L)；计算Sigma点{ζ_i}通过确定性观测模型h的传播ξ_i＝h(ζ_i)，最后计算Qzz为：

上式中，为求二阶统计特性时的权系数；

9)计算残差中非正交噪声矢量协方差为：

N_k＝V_k-R-Q_zz；

10)基于UT变换计算k-1时刻状态协方差矩阵P_k,k-1的时间更新通过确定性观测模型h的传播P_zk,k-1；根据和P_k,k-1求取Sigma点集{χ_i}，(i＝1,…，L)；计算Sigma点{χ_i}通过确定性观测模型h的传播δ_i＝h(χ_i)；根据计算状态协方差矩阵P_k,k-1的时间更新通过确定性观测模型h的传播P_zk,k-1；

上式中，为求二阶统计特性时的权系数；

11)计算渐消因子矩阵Λ_k；令M_k＝P_zk,k-1，计算基于计算渐消因子矩阵元素λ_i(i＝1,…，4)，构建渐消因子矩阵Λ_k＝diag(λ₁,λ₂,λ₃,λ₄)；

12)状态协方差矩阵的强跟踪时间更新为：

13)以和为观测方程参数，基于UT变换计算量测信息预测值；根据和P_k,k-1求取Sigma点集{ζ_i}，(i＝1,…，L)；计算Sigma点{ζ_i}通过确定性观测模型h的传播ξ_i＝h(ζ_i)；根据

计算和

上式中，和分别为求一、二阶统计特性时的权系数；

14)计算滤波增益为：

15)滤波状态量测更新为：

16)滤波状态协方差矩阵量测更新为：

步骤七：根据强跟踪UKF算法，融合水面母船水平定位信息、大潜深AUV同母船的测距信息和大潜深AUV捷联惯导系统定位信息、大潜深AUV深度信息，跟踪深潜过程中大潜深AUV在NED坐标系下的北向和东向水平定位误差以及载体坐标系下AUV的纵向和横向测速误差，对捷联惯导系统输出进行校正，得到精确的大潜深AUV下潜定位信息。