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

摘要

本发明公开一种基于地形辅助的水下载体紧组合导航方法，主要步骤包括：根据水下载体的运动状态选取状态量，建立水下载体组合导航系统模型，初始化水下地形组合导航系统；初始化高斯和粒子滤波器，搭建高斯过程‑高斯和粒子滤波框架并进行水下载体紧组合导航。采用高斯过程‑高斯和粒子滤波紧组合导航框架，可充分利用传感器信息，方法可移植性强，适用于非线性非高斯的实际系统，并且具有免重采样的优点，相较于传统的粒子滤波组合导航，组合导航精度更高，有利于推进基于地形辅助的水下载体组合导航系统的实际应用。

主权项

1.一种基于地形辅助的水下载体紧组合导航方法，其特征在于，该方法包含以下步骤：步骤1)根据水下载体的运动状态选取系统状态量，系统状态量具体为：式中：ξ、η分别为水下载体所处位置的经度、纬度；为水下载体距海平面的下潜深度；H、P、R分别为水下载体的航向角、俯仰角、横滚角；ε_x、ε_y、ε_z分别为陀螺仪在载体坐标系x轴、y轴、z轴的角速率零偏；[·]^T表示矩阵转置；步骤2)建立水下载体组合导航系统模型，并初始化组合导航系统；其中，水下载体组合导航系统模型为：式中：x_k+1为k+1时刻的状态，为k时刻的状态估计值，e_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个高斯和项i＝1，2，...，G，为k时刻第i个高斯和项权值，为x_k服从均值为方差为的高斯分布，其中，k＝0时，和根据先验分布得到，在k时刻时，每个高斯和项中抽样M个粒子，第i个高斯和项中的第j个粒子记为粒子i＝1，2，...，G；j＝1，2，...，M，以粒子水平位置为中心，选取水下地形数据库中中τ个节点，以τ个节点的水平位置向量及对应的地形数据作为粒子处地形模型的训练数据；搭建高斯过程‑高斯和粒子滤波框架进行水下载体紧组合导航。