CN109813316B - 一种基于地形辅助的水下载体紧组合导航方法 - Google Patents

Abstract

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

Description

一种基于地形辅助的水下载体紧组合导航方法

技术领域

本发明涉及一种水下载体组合导航方法，具体涉及一种基于地形辅助的水下载体紧组合导航方法。

背景技术

水下载体在水中工作时，无法实时接收全球卫星导航系统信号，基本惯性导航系统的误差随时间积累，不能满足水下长航时的定位要求。基于地形辅助的组合导航是一种通过地球物理量匹配修正惯性导航误差，从而提高导航精度的方式。

目前，基于地形辅助的水下载体组合导航主要存在两大关键技术难点：第一，水下载体组合导航系统依赖水下地形数据库的建立，然而对于水下导航实际应用来说，高精度水下地形数据库较难获取，采集和存储高精度水下地形数据库是难点，目前普遍采用的多为双线性插值法构建水下地形数据库，而这种方法所得到的水下地形数据库不能完全反应水下地形特征。第二，基于地形辅助的水下载体组合导航系统实际为非线性非高斯系统，目前的多数研究是将水下载体组合导航系统近似为线性化高斯系统或非线性化高斯系统，在此基础上进行研究的。上述两点限制了基于地形辅助的水下载体组合导航系统从理论研究向实际应用发展。

基于上述背景技术，本发明提出一种基于地形辅助的水下载体紧组合导航方法，该方法提出一种高斯过程-高斯和粒子滤波紧组合导航框架，这种导航方法可有效提升水下载体组合导航精度。

发明内容

技术问题：本发明的目的是为了采用低精度水下地形数据库时，有效提升水下载体

组合导航系统的导航精度，提供一种基于地形辅助的水下载体紧组合导航方法。

技术方案：本发明的一种基于地形辅助的水下载体紧组合导航方法，包括以下步骤：

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

式中：ξ、η分别为水下载体所处位置的经度、纬度；

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

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

式中：_xk+1为k+1时刻的状态，

为k时刻的状态估计值，_ek为过程噪声，Δ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个高斯和项权值，

为_xk服从均值为

方差为

的高斯分布，其中，k＝0时，

和

根据先验分布得到，

在k时刻时，每个高斯和项中抽样M个粒子，第i个高斯和项中的第j个粒子记为粒子

以粒子

水平位置

为中心，选取水下地形数据库中

中τ个节点，以τ个节点的水平位置向量

及对应的地形数据

作为粒子

处地形模型的训练数据；搭建高斯过程-高斯和粒子滤波框架进行水下载体紧组合导航；

进一步，搭建高斯过程-高斯和粒子滤波框架并进行水下载体紧组合导航，具体步骤为：

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时刻

的水平位置向量

和

之间的1×τ阶协方差矩阵；

3.3)在k时刻时，计算重要性函数

根据高斯和项的重要性函数p(x_k|zz_0:k)得到抽样样本点集

的权重

3.4)计算均值

方差

更新高斯和项权值

并进行高斯和项权值归一化

3.5)计算输出均值

计算输出方差

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

有益效果：本发明与现有技术相比，具有以下优点：

采用高斯过程-高斯和粒子滤波紧组合导航框架，可充分利用传感器信息，方法可移植性强，适用于非线性非高斯的实际系统，并且具有免重采样的优点，相较于传统的粒子滤波组合导航，组合导航精度更高，有利于推进基于地形辅助的水下载体组合导航系统的实际应用。

附图说明

图1为一种基于地形辅助的水下载体紧组合导航方法框图，

图2为水下载体真实航行轨迹和组合导航计算的航行轨迹仿真图，

图3为本发明方法的定位误差结果曲线图。

具体实施方式

根据下述实施例，可以更好的理解本发明。如图1所示，本发明的一种基于地形辅助的水下载体紧组合导航方法，具体步骤如下：

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

式中：ξ、η分别为水下载体所处位置的经度、纬度；

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

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

式中：_xk+1为k+1时刻的状态，

为k时刻的状态估计值，_ek为过程噪声，Δ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个高斯和项权值，

为_xk服从均值为

方差为

的高斯分布，其中，k＝0时，

和

根据先验分布得到，

在k时刻时，每个高斯和项中抽样M个粒子，第i个高斯和项中的第j个粒子记为粒子

以粒子

水平位置

为中心，选取水下地形数据库中

中τ个节点，以τ个节点的水平位置向量

及对应的地形数据

作为粒子

处地形模型的训练数据；搭建高斯过程-高斯和粒子滤波框架进行水下载体紧组合导航；

进一步，搭建高斯过程-高斯和粒子滤波框架并进行水下载体紧组合导航，具体步骤为：

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时刻

的水平位置向量

和

之间的1×τ阶协方差矩阵；

3.3)在k时刻时，计算重要性函数

根据高斯和项的重要性函数p(x_k|zz_0:k)得到抽样样本点集

的权重

3.4)计算均值

方差

更新高斯和项权值

并进行高斯和项权值归一化

3.5)计算输出均值

计算输出方差

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

本发明的可行性通过如下仿真加以验证：

(1)陀螺仪随机常值漂移0.03°/h，角度随机游走0.01°/√h，加速度计随机常值偏置0.1mg，速度随机游走5μg/√Hz，初始水平位置误差100m，初始航向角误差0.5°；

(2)水下载体以4kn速度匀速航行，初始水平位置为(130.009°E,26.0035°N)，航行轨迹如图2所示；水下地形数据库范围：(130°E～130.027°E，26°N～26.011°N)，海图水深误差为20cm；便携式测深仪精度1cm，压力传感器精度为下潜深度的0.01％，水深测量误差±1m；

(3)高斯和粒子滤波器G＝3，M＝300；高斯过程超参数初始值Θ＝{3,0.01,1,1}

通过仿真，验证了本方法的可行性(如图2所示)，其导航精度较基于粒子滤波的组合导航有显著提升(如图3)。

以上所述仅是本发明的优选实施方式，应当指出，对于本技术领域的普通技术人员来说，在不脱离本发明原理的前提下，还可以做出若干改进和润饰，这些改进和润饰也应视为本发明的保护范围。

Claims (2)

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)，直至航行结束，停止运算。

Images