[发明专利]一种基于地形辅助的水下载体紧组合导航方法

权利要求书

1.一种基于地形辅助的水下载体紧组合导航方法，其特征在于，该方法包含以下步骤：

步骤1)根据水下载体的运动状态选取系统状态量，系统状态量具体为：

式中：_ξ、_η分别为水下载体所处位置的经度、纬度；为水下载体距海平面的下潜深度；H、P、R分别为水下载体的航向角、俯仰角、横滚角；ε_x、ε_y、ε_z分别为陀螺仪在载体坐标系x轴、y轴、z轴的角速率零偏；[·]^T表示矩阵转置；

步骤2)建立水下载体组合导航系统模型，并初始化组合导航系统；其中，水下载体组合导航系统模型为：

式中：x_k+1为k+1时刻的状态，为k时刻的状态估计值，e_k为过程噪声，Δx_k为从k时刻到k+1时刻得状态转移矩阵，且

式中，分别为k时刻水下载体的航向角、俯仰角、横滚角估计值；为从载体坐标系到导航坐标系变换的姿态矩阵；v_k为k时刻的载体速度；Δt为高度值采样间隔；分别为水下载体在k时刻沿载体坐标系x轴、y轴、z轴三个方向的角速率计算值，且其中，ω_x,k、ω_y,k、ω_z,k分别为k时刻陀螺沿载体坐标系x轴、y轴、z轴三个方向的输出值；为x_k位置处的水深值，且h(·)为水下地形模型；为k时刻压力传感器测得的水下载体距水平面的下潜深度；pre_k为k时刻便携式测深仪测得的水下载体距离海底的高度值；η_k为量测噪声；zz_k为k时刻的观测量；

步骤3)初始化G个高斯和项为k时刻第i个高斯和项权值，为x_k服从均值为方差为的高斯分布，其中，k＝0时，和根据先验分布得到，在k时刻时，每个高斯和项中抽样M个粒子，第i个高斯和项中的第j个粒子记为粒子以粒子水平位置为中心，选取水下地形数据库中中τ个节点，以τ个节点的水平位置向量及对应的地形数据作为粒子处地形模型的训练数据，搭建高斯过程-高斯和粒子滤波框架进行水下载体紧组合导航。

2.根据权利要求1所述的一种基于地形辅助的水下载体紧组合导航方法，其特征在于步骤3)，所述搭建高斯过程-高斯和粒子滤波框架并进行水下载体紧组合导航，具体步骤为：

3.1)当k时刻时，建立基于高斯过程的水下地形模型：选取协方差函数为

式中，po_u和po_v为训练样本水平位置向量的任意元素；kin(·)是协方差函数；σ_h是地形数据标准差；σ_v是样本噪声标准差；l_hori是地形模型水平尺度；l_verti是地形模型垂直尺度；Θ＝{σ_h,σ_v,l_hori,l_verti}是协方差函数的超参数；当u＝v时，δ_uv＝1；当u≠v时，δ_uv＝0；

训练样本的条件概率对数似然函数为：

式中，为粒子的超参数；K(po,po)＝{kin(po_u,po_v)|u,v＝1,2,...,τ}为τ×τ阶协方差矩阵；I_τ为τ维单位矩阵；p(·)为概率；

超参数估计值为：

3.2)在k时刻时，粒子的水平位置向量对应的水深值可计算得

式中，为k时刻的水平位置向量和阶协方差矩阵；

3.3)在k时刻时，计算重要性函数根据高斯和项的重要性函数p(x_k|zz_0:k)得到抽样样本点集的权重

3.4)计算均值方差更新高斯和项权值并进行高斯和项权值归一化

3.5)计算输出均值

计算输出方差

3.6)通过式(2)进行从时刻k到时刻k+1的状态转移，继续重复步骤2.1)-2.5)，直至航行结束，停止运算。