CN106017460A - 一种地形辅助惯导紧组合的水下潜器导航定位方法 - Google Patents

Abstract

本发明公开了一种地形辅助惯导紧组合的水下潜器导航定位方法，主要步骤包括：初始化组合导航系统；利用非线性离散状态方程进行状态量一步预测；利用非线性离散量测方程计算粒子滤波器的重要性权值；归一化重要性权值并估计潜器的导航参数；进行重要性重采样；完成非线性粒子滤波算法。本发明方法能有效、充分地利用各子系统的信息，相比地形辅助惯导松组合方式，定位的精度有明显提高，保证了水下潜器的长航时导航定位精度。

Description

一种地形辅助惯导紧组合的水下潜器导航定位方法

技术领域

本发明涉及一种水下潜器组合导航定位方法，具体涉及一种地形辅助惯导紧组合的水下潜器导航定位方法。

背景技术

水下潜器经过长时间航行后，惯性导航系统输出的位置已经积累较大误差。考虑到安全性、隐蔽性和长时间水下作业的实际情况，采用地球物理场辅助惯性导航的无源导航技术，主要包括惯性/地形、惯性/重力和惯性/地磁组合导航。其基本原理都是通过潜器装备的传感器测量航行轨迹上的地球物理场数据，再与已有的海图数据库进行匹配，确定水下潜器的最佳位置。

地形导航最早应用在航空领域，作为战机和巡航导弹的一种辅助导航方法，其中最著名的有TERCOM和SITAN。目前，水下地形辅助惯性组合导航的研究多用于装备有高精度惯性系统、多波束测深仪和高分辨率高精度海图数据库的高成本水下潜器。G.T.Donovan采用高成本水下潜器，比较了多波束测深仪、四波束DVL测深仪和单波束测深仪的导航定位效果。D.K.Meduna采用低成本水下潜器，研究了基于四波束多普勒测深仪的地形辅助惯导紧组合导航定位。单波束测深仪具有成本低、使用方便的优点，但其波束数量太少，采用单波束测深仪的低成本水下潜器的导航定位往往不能满足导航需求，基于单波束测深仪的低成本水下潜器地形辅助惯导紧组合研究还尚未见。

发明内容

技术问题：本发明的目的是为了解决当采用单波束测深仪时，地形辅助惯导松组合方式不能满足低成本水下潜器的导航定位要求，导致组合导航定位精度下降的问题，提供一种地形辅助惯导紧组合的水下潜器导航定位方法。由于单波束测深仪每次观测的波束数仅为单个，在粒子滤波的重要性权值的计算中不仅考虑水深误差，而且引入水平距离误差，从而可以提高低成本水下潜器组合导航的定位精度和收敛速度。

技术方案：本发明的一种地形辅助惯导紧组合的水下潜器导航定位方法，包括以下步骤：

步骤1)初始化地形辅助惯导组合导航定位系统，为系统的导航参数更新提供初始值：通过全球导航卫星系统获取潜器的初始位置参数；根据惯性测量单元采集的潜器角速率和比力信息，以及多普勒测速仪测得的载体系速度，进行捷联惯性导航系统的初始对准，获取潜器的初始姿态矩阵和导航系初始速度信息；初始时刻k＝0，初始化M个粒子，根据先验概率分布进行粒子初始化；

步骤2)令k:＝k+1，利用以潜器位置、姿态角和陀螺漂移为状态量的非线性离散状态方程进行一步预测，得到k时刻的一步预测状态量x_k；

步骤3)利用非线性离散量测方程处理水深数据并提取水平距离，计算波束投影点水深误差、潜器的东向距离误差和北向距离误差，然后计算重要性权值；

步骤4)归一化重要性权值，得到k时刻状态量的最小均方估计为将其作为组合后的潜器导航参数输出；

步骤5)进行重要性重采样：得到新的粒子集合

步骤6)判断潜器航行时间是否结束，若是则结束本方法，否则返回步骤2)。

进一步的，本发明方法中，步骤2)中的非线性离散状态方程为：

$x_k={\hat{x}}_{k-1}+\left\{\begin{array}{c}\left[\begin{array}{ccc}1& 0& 0\\ 0& 1& 0\end{array}\right]R\left({\hat{q}}_{k-1}\right){\mathrm{\υ}}_{k-1}\mathrm{\Δ}t\\ 0\\ \left[\begin{array}{cc}\cos {\widehat{\mathrm{\γ}}}_{k-1}& \sin {\widehat{\mathrm{\γ}}}_{k-1}\end{array}\right]\left\{\begin{array}{c}{\mathrm{\ω}}_{x,k}-{\mathrm{\ϵ}}_{x,k-1}\\ {\mathrm{\ω}}_{z,k-1}-{\mathrm{\ϵ}}_{z,k-1}\end{array}\right\}\mathrm{\Δ}t\\ \left[\begin{array}{ccc}\tan {\widehat{\mathrm{\θ}}}_{k-1}\sin {\widehat{\mathrm{\γ}}}_{k-1}& 1& -\tan {\widehat{\mathrm{\θ}}}_{k-1}\cos {\widehat{\mathrm{\γ}}}_{k-1}\end{array}\right]\left\{\begin{array}{c}{\mathrm{\ω}}_{x,k-1}-{\mathrm{\ϵ}}_{x,k-1}\\ {\mathrm{\ω}}_{y,k-1}-{\mathrm{\ϵ}}_{y,k-1}\\ {\mathrm{\ω}}_{z,k-1}-{\mathrm{\ϵ}}_{z,k-1}\end{array}\right\}\mathrm{\Δ}t\\ \left[\begin{array}{cc}-\frac{\sin {\widehat{\mathrm{\γ}}}_{k-1}}{\cos {\widehat{\mathrm{\θ}}}_{k-1}}& \frac{\cos {\widehat{\mathrm{\γ}}}_{k-1}}{{\mathrm{cos\θ}}_{k-1}}\end{array}\right]\left\{\begin{array}{c}{\mathrm{\ω}}_{x,k-1}-{\mathrm{\ϵ}}_{x,k-1}\\ {\mathrm{\ω}}_{z,k-1}-{\mathrm{\ϵ}}_{z,k-1}\end{array}\right\}\mathrm{\Δ}t\\ 0_3\end{array}\right\}+e_{k-1}---\left(1\right)$

式中：为k-1时刻的状态量估计值，当k＝1时，为初始化的状态量，x_k为k时刻的一步预测状态量，状态量的通用表达式为x＝[p,q,ε]^T；

p为n系下潜器的位置向量，p＝[x_E,x_N,z]^T，x_E和x_N分别为潜器的东向位置和北向位置分量，z是采用压力传感器测得的潜器的下潜深度，n系为导航坐标系，是东北天地理坐标系；

q为潜器的姿态角向量，q＝[θ,γ,ψ]^T，θ、γ和ψ分别为俯仰角、横滚角和航向角；

ε为b系下陀螺的常值漂移向量，ε＝[ε_x,ε_y,ε_z]^T，ε_x、ε_y和ε_z分别为对应于x轴、y轴和z轴的角速率常值漂移，b系为载体坐标系，是以潜器右前上方向矢量右手定则构成的坐标系；

为k-1时刻估计的潜器的姿态角，分别为估计的俯仰角、横滚角和航向角，为捷联惯性导航系统计算的k-1时刻潜器由b系到n系的姿态矩阵；

υ_k-1是k-1时刻多波束测深仪测得的潜器在b系的速度；

ω_x,k-1、ω_y,k-1和ω_z,k-1分别为k-1时刻三个轴向陀螺输出的角速率，ε_x,k-1,ε_y,k-1,ε_z,k-1分别为对应轴向角速率的常值漂移；

Δt为水深测量的采样时间；e_k-1是系统噪声，Σ_e为系统噪声方差；[·]^T表示矩阵转置。

进一步的，本发明方法中，所述步骤3)中的非线性离散量测方程为：

$\left[\begin{array}{c}{y}_k\\ d_k\end{array}\right]=\left[\begin{array}{c}\hat{h}(x_k,r_k,a)-z_k\\ D(x_k^i,{\hat{x}}_{k-1})\end{array}\right]+{\mathrm{\η}}_k---\left(2\right)$

式中：y_k为k时刻波束投影点到潜器的天向距离，y_k＝[0 0 1]R(q_k)ar_k；

q_k为一步预测姿态角，R(q_k)为捷联惯性导航系统计算的潜器由b系到n系的一步预测姿态矩阵；

r_k为k时刻单波束测深仪测量的波束距离，在b系下为负值，a为b系下波束方位向量；

为波束投影点在海图上的水深值，通过双线性插值获取该水深值；

x_E,k和x_N,k分别表示k时刻一步预测状态量x_k中的东向位置和北向位置分量；

z_k是k时刻压力传感器测得的潜器的下潜深度；

d_k为多普勒测速仪推算的潜器在k和k-1相邻水深测量时刻的水平距离，d＝[d_E,d_N]^T，d_E和d_N分别为相邻水深测量时刻潜器的东向距离和北向距离分量；

为第i个粒子一步预测的水平位置和k-1时刻潜器的水平位置估计值之间的距离，包括东向距离和北向距离分量；

为第i个粒子的一步预测状态量，这里具体指其中的东向位置和北向位置分量；

为k-1时刻潜器的状态量估计值，这里具体指其中的东向位置和北向位置估计值；

η_k是距离测量噪声，Σ_η为距离噪声方差。

进一步的，本发明方法中，所述步骤3)中根据下式计算重要性权值：

$w_k^i=p(y_k,d_k|x_k^i,r_k,a)---\left(3\right)$

式中：为第i个粒子在k时刻的重要性权值；表示在和r_k数据上的似然函数；

k＝1时，

$p\left(y_k\right|x_k^i,r_k,a)=\frac{1}{\sqrt{2{\mathrm{\π\Σ}}_{\mathrm{\η}}}}\exp (-\frac{1}{2{\mathrm{\Σ}}_{\mathrm{\η}}}{\left(y_k-\hat{h}\left(x_k^i,r_k,a\right)+z_k\right)}^2)---\left(4\right)$

k>1时，有

$\begin{array}{c}p(y_k,d_k|x_k^i,r_k,a)\\ =\frac{1}{\sqrt{2\mathrm{\π}{\mathrm{\Σ}}_{\mathrm{\η}}}}\exp (-\frac{1}{2{\mathrm{\Σ}}_{\mathrm{\η}}}({\left(\mathrm{\Δ}{d}_{E,k}^i\right)}^2+{\left(\mathrm{\Δ}{d}_{N,k}^i\right)}^2+{(y_k-\hat{h}(x_k^i,r_k,a)+z_k)}^2))\end{array}---\left(5\right)$

式中：y_k为k时刻波束投影点到潜器的天向距离，y_k＝[0 0 1]R(q_k)ar_k；

d_k为多普勒测速仪推算的潜器在k和k-1相邻水深测量时刻的水平距离；

为第i个粒子的一步预测状态量，这里具体指其中的东向位置和北向位置分量；

r_k为k时刻单波束测深仪测量的波束距离，在b系下为负值，a为b系下波束方位向量；

是采用双线性插值法计算粒子的波束投影点在海图上的水深值；

z_k是k时刻压力传感器测得的潜器的下潜深度；

d_E,k和d_N,k分别表示多普勒测速仪推算的潜器在k和k-1相邻水深测量时刻的东向距离和北向距离分量；

和分别表示第i个粒子一步预测的水平位置和k-1时刻潜器的水平位置估计值之间的东向距离和北向距离分量；

和分别表示k时刻第i个粒子的东向距离差和北向距离差。

本发明在D.K.Meduna紧组合研究的基础上，提出基于单波束测深仪的地形辅助惯导紧组合方式水下潜器导航定位方法。该方法能有效、充分地利用各子系统的信息，相比地形辅助惯导松组合，定位精度有明显提高，保证了水下潜器的长航时导航定位精度。

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

(1)本发明中的地形辅助惯导紧组合导航系统直接采用水深误差作为外观测量之一，将潜器的位置、姿态估计与水深值紧密结合，能有效、充分地利用各子系统的信息，相比采用位置误差作为外观测量的地形辅助惯导松组合方式，定位的精度有明显提高。

(2)粒子滤波中重要性权值的计算不仅考虑水深误差，而且引入潜器的水平距离误差，在不增加任何外围设备的前提下，增加外观测量有助于提高了水下潜器组合导航的定位精度和收敛速度。

附图说明

图1为一种地形辅助惯导紧组合的水下潜器导航定位结构框图；

图2为用Matlab仿真得到的潜器分别在地形粗糙区域(A区)和地形平坦区域(B区)的真实航行轨迹平面投影以及海图等深线分布图。

图3为用Matlab仿真得到的潜器在A区分别采用地形辅助惯导松/紧组合的导航定位结果图；

图4为用Matlab仿真得到的潜器在B区分别采用地形辅助惯导松/紧组合的导航定位结果图。

具体实施方式

下面结合实施例和说明书附图对本发明作进一步的说明。

如图1所示，本发明的一种地形辅助惯导紧组合的水下潜器导航定位方法，具体步骤如下：

步骤1)初始化地形辅助惯导组合导航定位系统，为系统的导航参数更新提供初始值：通过全球导航卫星系统获取潜器的初始位置参数；根据惯性测量单元采集的潜器角速率和比力信息，以及多普勒测速仪测得的载体系速度，进行捷联惯性导航系统的初始对准，获取潜器的初始姿态矩阵和导航系初始速度信息；初始时刻k＝0，初始化M个粒子，根据先验概率分布进行粒子初始化；

步骤2)令k:＝k+1，利用以潜器位置、姿态角和陀螺漂移为状态量的非线性离散状态方程(1)进行一步预测，得到k时刻的一步预测状态量x_k；

$x_k={\hat{x}}_{k-1}+\left\{\begin{array}{c}\left[\begin{array}{ccc}1& 0& 0\\ 0& 1& 0\end{array}\right]R\left({\hat{q}}_{k-1}\right){\mathrm{\υ}}_{k-1}\mathrm{\Δ}t\\ 0\\ \left[\begin{array}{cc}\cos {\widehat{\mathrm{\γ}}}_{k-1}& \sin {\widehat{\mathrm{\γ}}}_{k-1}\end{array}\right]\left\{\begin{array}{c}{\mathrm{\ω}}_{x,k}-{\mathrm{\ϵ}}_{x,k-1}\\ {\mathrm{\ω}}_{z,k-1}-{\mathrm{\ϵ}}_{z,k-1}\end{array}\right\}\mathrm{\Δ}t\\ \left[\begin{array}{ccc}\tan {\widehat{\mathrm{\θ}}}_{k-1}\sin {\widehat{\mathrm{\γ}}}_{k-1}& 1& -\tan {\widehat{\mathrm{\θ}}}_{k-1}\cos {\widehat{\mathrm{\γ}}}_{k-1}\end{array}\right]\left\{\begin{array}{c}{\mathrm{\ω}}_{x,k-1}-{\mathrm{\ϵ}}_{x,k-1}\\ {\mathrm{\ω}}_{y,k-1}-{\mathrm{\ϵ}}_{y,k-1}\\ {\mathrm{\ω}}_{z,k-1}-{\mathrm{\ϵ}}_{z,k-1}\end{array}\right\}\mathrm{\Δ}t\\ \left[\begin{array}{cc}-\frac{\sin {\widehat{\mathrm{\γ}}}_{k-1}}{\cos {\widehat{\mathrm{\θ}}}_{k-1}}& \frac{\cos {\widehat{\mathrm{\γ}}}_{k-1}}{{\mathrm{cos\θ}}_{k-1}}\end{array}\right]\left\{\begin{array}{c}{\mathrm{\ω}}_{x,k-1}-{\mathrm{\ϵ}}_{x,k-1}\\ {\mathrm{\ω}}_{z,k-1}-{\mathrm{\ϵ}}_{z,k-1}\end{array}\right\}\mathrm{\Δ}t\\ 0_3\end{array}\right\}+e_{k-1}---\left(1\right)$

式中：为k-1时刻的状态量估计值，当k＝1时，为初始化的状态量，x_k为k时刻的一步预测状态量，状态量的通用表达式为x＝[p,q,ε]^T；

p为n系下潜器的位置向量，p＝[x_E,x_N,z]^T，x_E和x_N分别为潜器的东向位置和北向位置分量，z是采用压力传感器测得的潜器下潜深度，n系为导航坐标系，是东北天地理坐标系；

q为潜器姿态角向量，q＝[θ,γ,ψ]^T，θ、γ和ψ分别为俯仰角、横滚角和航向角；

ε为b系下陀螺的常值漂移向量，ε＝[ε_x,ε_y,ε_z]^T，ε_x、ε_y和ε_z分别为对应于x轴、y轴和z轴的角速率常值漂移，b系为载体坐标系，是以潜器右前上方向矢量右手定则构成的坐标系；

为k-1时刻估计的潜器的姿态角，和分别为估计的俯仰角、横滚角和航向角，为捷联惯性导航系统计算的k-1时刻潜器由b系到n系的姿态矩阵；

υ_k-1是k-1时刻多波束测深仪测得的潜器在b系的速度；

ω_x,k-1、ω_y,k-1和ω_z,k-1分别为k-1时刻三个轴向陀螺输出的角速率，ε_x,k-1,ε_y,k-1,ε_z,k-1分别为对应轴向角速率的常值漂移；

Δt为水深测量的采样时间；e_k-1是系统噪声，Σ_e为系统噪声方差；[·]^T表示矩阵转置；

步骤3)利用非线性离散量测方程(2)处理水深数据并提取水平距离，计算波束投影点水深误差、潜器的东向距离误差和北向距离误差，然后计算重要性权值；

$\left[\begin{array}{c}{y}_k\\ d_k\end{array}\right]=\left[\begin{array}{c}\hat{h}(x_k,r_k,a)-z_k\\ D(x_k^i,{\hat{x}}_{k-1})\end{array}\right]+{\mathrm{\η}}_k---\left(2\right)$

式中：y_k为k时刻波束投影点到潜器的天向距离，y_k＝[0 0 1]R(q_k)ar_k；

q_k为一步预测姿态角，R(q_k)为捷联惯性导航系统计算的潜器由b系到n系的一步预测姿态矩阵；

r_k为k时刻单波束测深仪测量的波束距离，在b系下为负值，a为b系下波束方位向量；

为波束投影点在海图上的水深值，通过双线性插值获取该水深值；

x_E,k和x_N,k分别表示k时刻一步预测状态量x_k中的东向位置和北向位置分量；

z_k是k时刻压力传感器测得的潜器的下潜深度；

d_k为多普勒测速仪推算的潜器在k和k-1相邻水深测量时刻的水平距离，d＝[d_E,d_N]^T，d_E和d_N分别为相邻水深测量时刻潜器的东向距离和北向距离分量；

为第i个粒子一步预测的水平位置和k-1时刻潜器的水平位置估计值之间的距离，包括东向距离和北向距离分量；

为第i个粒子的一步预测状态量，这里具体指其中的东向位置和北向位置分量；

为k-1时刻载体的状态量估计值，这里具体指其中的东向位置和北向位置估计值；

η_k是距离测量噪声，Σ_η为距离噪声方差；

$w_k^i=p(y_k,d_k|x_k^i,r_k,a)---\left(3\right)$

式中：为第i个粒子在k时刻的重要性权值；表示在和r_k数据上的似然函数；

k＝1时，

$p\left(y_k\right|x_k^i,r_k,a)=\frac{1}{\sqrt{2{\mathrm{\π\Σ}}_{\mathrm{\η}}}}\exp (-\frac{1}{2{\mathrm{\Σ}}_{\mathrm{\η}}}{\left(y_k-\hat{h}\left(x_k^i,r_k,a\right)+z_k\right)}^2)---\left(4\right)$

k>1时，有

$\begin{array}{c}p(y_k,d_k|x_k^i,r_k,a)\\ =\frac{1}{\sqrt{2\mathrm{\π}{\mathrm{\Σ}}_{\mathrm{\η}}}}\exp (-\frac{1}{2{\mathrm{\Σ}}_{\mathrm{\η}}}({\left(\mathrm{\Δ}{d}_{E,k}^i\right)}^2+{\left(\mathrm{\Δ}{d}_{N,k}^i\right)}^2+{(y_k-\hat{h}(x_k^i,r_k,a)+z_k)}^2))\end{array}---\left(5\right)$

式中：y_k为k时刻波束投影点到潜器的天向距离，y_k＝[0 0 1]R(q_k)ar_k；

d_k为多普勒测速仪推算的潜器在k和k-1相邻水深测量时刻的水平距离；

为第i个粒子的一步预测状态量，这里具体指其中的东向位置和北向位置分量；

r_k为k时刻单波束测深仪测量的波束距离，在b系下为负值，a为b系下波束方位向量；

是采用双线性插值法计算粒子的波束投影点在海图上的水深值；

z_k是k时刻压力传感器测得的潜器的下潜深度；

d_E,k和d_N,k分别表示多普勒测速仪推算的潜器在k和k-1相邻水深测量时刻的东向距离和北向距离分量；

和分别表示第i个粒子一步预测的水平位置和k-1时刻潜器的水平位置估计值之间的东向距离和北向距离分量；

和分别表示k时刻第i个粒子的东向距离差和北向距离差；

步骤4)归一化重要性权值，得到k时刻状态量的最小均方估计为将其作为组合后的潜器导航参数输出；

步骤5)进行重要性重采样：得到新的粒子集合

步骤6)判断潜器航行时间是否结束，若是则结束本方法，否则返回步骤2)。

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

(1)地形辅助导航和捷联惯性导航系统，构成以捷联惯导为主、地形导航为辅的地形辅助惯导组合导航定位系统，水下潜器分别在地形粗糙区域(A区)和地形平坦区域(B区)两个区域以2节速度航行，潜器实际航迹平面投影如图2所示；

(2)根据选用的惯性测量单元精度，设置陀螺随机常值漂移0.05°/h，随机白噪声0.05°/√hr，加速度计随机常值偏置0.2mg，随机白噪声0.2m/sec/√hr，多普勒测速仪测量误差0.05m/s，潜器在A区的初始位置(667.1m,300.2m,0m)，潜器在B区的初始位置(2223.6m,66.7m,0m)，初始水平位置误差100m，初始航向角误差0.5°，初始纵摇角误差0.05°，初始横摇角误差0.02°；

(3)海图范围：3000m×1200m，海图分辨率分为10m，水深量程范围：-241.7m～0m，海图水深误差为20cm，单波束测深仪量程范围0.3m～300m，测量精度±1cm+0.1％(×水深值)，压力传感器测深范围0.1m～300m，测量精度为深度的0.01％，水深测量误差±1m；

(4)惯性传感器数据更新周期为50ms，水深测量的采样时间和滤波周期均为1s，航行时间3500s；

(5)粒子滤波器选用2000个粒子。

在不同地形区域(A区和B区)下，分别采用地形辅助惯导松/紧组合实现水下潜器导航定位。通过50次蒙特卡洛(MC)实验，取50次实验的平均值，得到组合导航定位结果如图3和图4所示，导航统计结果见表1。从结果中可以看出，本发明所提方法无论在地形粗糙区域还是地形平坦区域，导航的结果均优于传统松组合方法。

表1 50次MC仿真结果

上述实施例仅是本发明的优选实施方式，应当指出：对于本技术领域的普通技术人员来说，在不脱离本发明原理的前提下，还可以做出若干改进和等同替换，这些对本发明权利要求进行改进和等同替换后的技术方案，均落入本发明的保护范围。

Claims (4)

1.一种地形辅助惯导紧组合的水下潜器导航定位方法，其特征在于，该方法包括以下步骤：

步骤1)初始化地形辅助惯导组合导航定位系统，为系统的导航参数更新提供初始值：通过全球导航卫星系统获取潜器的初始位置参数；根据惯性测量单元采集的潜器角速率和比力信息，以及多普勒测速仪测得的载体系速度，进行捷联惯性导航系统的初始对准，获取潜器的初始姿态矩阵和导航系初始速度信息；初始时刻k＝0，初始化M个粒子，根据先验概率分布进行粒子初始化；

步骤2)令k:＝k+1，利用以潜器位置、姿态角和陀螺漂移为状态量的非线性离散状态方程进行一步预测，得到k时刻的一步预测状态量x_k；

步骤3)利用非线性离散量测方程处理水深数据并提取水平距离，计算波束投影点水深误差、潜器的东向距离误差和北向距离误差，然后计算重要性权值；

步骤4)归一化重要性权值，得到k时刻状态量的最小均方估计为将其作为组合后的潜器导航参数输出；

步骤5)进行重要性重采样：得到新的粒子集合

步骤6)判断潜器航行时间是否结束，若是则结束本方法，否则返回步骤2)。

2.根据权利要求1所述的一种地形辅助惯导紧组合的水下潜器导航定位方法，其特征在于，所述步骤2)中的非线性离散状态方程为：

$x_k={\hat{x}}_{k-1}+\left\{\begin{array}{c}\left[\begin{array}{ccc}1& 0& 0\\ 0& 1& 0\end{array}\right]R\left({\hat{q}}_{k-1}\right){\mathrm{\υ}}_{k-1}\mathrm{\Δ}t\\ 0\\ \[\begin{array}{cc}\cos {\widehat{\mathrm{\γ}}}_{k-1}& \sin {\widehat{\mathrm{\γ}}}_{k-1}\end{array}\]\left\{\begin{array}{c}{\mathrm{\ω}}_{x,k-1}-{\mathrm{\ϵ}}_{x,k-1}\\ {\mathrm{\ω}}_{z,k-1}-{\mathrm{\ϵ}}_{z,k-1}\end{array}\right\}\mathrm{\Δ}t\\ \[\begin{array}{ccc}\tan {\widehat{\mathrm{\θ}}}_{k-1}\sin {\widehat{\mathrm{\γ}}}_{k-1}& 1& -\tan {\widehat{\mathrm{\θ}}}_{k-1}\cos {\widehat{\mathrm{\γ}}}_{k-1}\end{array}\]\left\{\begin{array}{c}{\mathrm{\ω}}_{x,k-1}-{\mathrm{\ϵ}}_{x,k-1}\\ {\mathrm{\ω}}_{y,k-1}-{\mathrm{\ϵ}}_{y,k-1}\\ {\mathrm{\ω}}_{z,k-1}-{\mathrm{\ϵ}}_{z,k-1}\end{array}\right\}\mathrm{\Δ}t\\ \[\begin{array}{cc}-\frac{\sin {\widehat{\mathrm{\γ}}}_{k-1}}{\cos {\widehat{\mathrm{\θ}}}_{k-1}}& \frac{\cos {\widehat{\mathrm{\γ}}}_{k-1}}{\cos {\widehat{\mathrm{\θ}}}_{k-1}}\end{array}\]\left\{\begin{array}{c}{\mathrm{\ω}}_{x,k-1}-{\mathrm{\ϵ}}_{x,k-1}\\ {\mathrm{\ω}}_{z,k-1}-{\mathrm{\ϵ}}_{z,k-1}\end{array}\right\}\mathrm{\Δ}t\\ 0_3\end{array}\right\}+e_{k-1}---\left(1\right)$

式中：为k-1时刻的状态量估计值，当k＝1时，为初始化的状态量，x_k为k时刻的一步预测状态量，状态量的通用表达式为x＝[p,q,ε]^T；

p为n系下潜器的位置向量，p＝[x_E,x_N,z]^T，x_E和x_N分别为潜器的东向位置和北向位置分量，z是采用压力传感器测得的潜器的下潜深度，n系为导航坐标系，是东北天地理坐标系；

q为潜器的姿态角向量，q＝[θ,γ,ψ]^T，θ、γ和ψ分别为俯仰角、横滚角和航向角；

ε为b系下陀螺的常值漂移向量，ε＝[ε_x,ε_y,ε_z]^T，ε_x、ε_y和ε_z分别为对应于x轴、y轴和z轴的角速率常值漂移，b系为载体坐标系，是以潜器右前上方向矢量右手定则构成的坐标系；

为k-1时刻估计的潜器的姿态角， 和分别为估计的俯仰角、横滚角和航向角，为捷联惯性导航系统计算的k-1时刻潜器由b系到n系的姿态矩阵；

υ_k-1是k-1时刻多波束测深仪测得的潜器在b系的速度；

ω_x,k-1、ω_y,k-1和ω_z,k-1分别为k-1时刻三个轴向陀螺输出的角速率，ε_x,k-1,ε_y,k-1,ε_z,k-1分别为对应轴向角速率的常值漂移；

Δt为水深测量的采样时间；e_k-1是系统噪声，Σ_e为系统噪声方差；[·]^T表示矩阵转置。

3.根据权利要求1所述的一种地形辅助惯导紧组合的水下潜器导航定位方法，其特征在于，所述步骤3)中的非线性离散量测方程为：

$\left[\begin{array}{c}{y}_k\\ d_k\end{array}\right]=\left[\begin{array}{c}\hat{h}(x_k,r_k,a)-z_k\\ D(x_k^i,{\hat{x}}_{k-1})\end{array}\right]+{\mathrm{\η}}_k---\left(2\right)$

式中：y_k为k时刻波束投影点到潜器的天向距离，y_k＝[0 0 1]R(q_k)ar_k；

q_k为一步预测姿态角，R(q_k)为捷联惯性导航系统计算的潜器由b系到n系的一步预测姿态矩阵；

r_k为k时刻单波束测深仪测量的波束距离，在b系下为负值，a为b系下波束方位向量；

为波束投影点在海图上的水深值，通过双线性插值获取该水深值；

x_E,k和x_N,k分别表示k时刻一步预测状态量x_k中的东向位置和北向位置分量；

z_k是k时刻压力传感器测得的潜器的下潜深度；

d_k为多普勒测速仪推算的潜器在k和k-1相邻水深测量时刻的水平距离，d＝[d_E,d_N]^T，d_E和d_N分别为相邻水深测量时刻潜器的东向距离和北向距离分量；

为第i个粒子一步预测的水平位置和k-1时刻潜器的水平位置估计值之间的距离，包括东向距离和北向距离分量；

为第i个粒子的一步预测状态量，这里具体指其中的东向位置和北向位置分量；

为k-1时刻潜器的状态量估计值，这里具体指其中的东向位置和北向位置估计值；

η_k是距离测量噪声，Σ_η为距离噪声方差。

4.根据权利要求1、2或3所述的一种地形辅助惯导紧组合的水下潜器导航定位方法，其特征在于，所述步骤3)中根据下式计算重要性权值：

$w_k^i=p(y_k,d_k|x_k^i,r_k,a)---\left(3\right)$

式中：为第i个粒子在k时刻的重要性权值；表示在和r_k数据上的似然函数；

k＝1时，

$p\left(y_k\right|x_k^i,r_k,a)=\frac{1}{\sqrt{2{\mathrm{\π\Σ}}_{\mathrm{\η}}}}\exp (-\frac{1}{2{\mathrm{\Σ}}_{\mathrm{\η}}}{\left(y_k-\hat{h}\left(x_k^i,r_k,a\right)+z_k\right)}^2)---\left(4\right)$

k>1时，有

$\begin{array}{c}p(y_k,d_k|x_k^i,r_k,a)\\ =\frac{1}{\sqrt{2{\mathrm{\π\Σ}}_{\mathrm{\η}}}}\exp (-\frac{1}{2{\mathrm{\Σ}}_{\mathrm{\η}}}({\left({\mathrm{\Δd}}_{E,k}^i\right)}^2+{\left({\mathrm{\Δd}}_{N,k}^i\right)}^2+{\left(y_k-\hat{h}\left(x_k^i,r_k,a\right)+z_k\right)}^2\left)\right)\end{array}---\left(5\right)$

式中：y_k为k时刻波束投影点到潜器的天向距离，y_k＝[0 0 1]R(q_k)ar_k；

d_k为多普勒测速仪推算的潜器在k和k-1相邻水深测量时刻的水平距离；

为第i个粒子的一步预测状态量，这里具体指其中的东向位置和北向位置分量；

r_k为k时刻单波束测深仪测量的波束距离，在b系下为负值，a为b系下波束方位向量；

是采用双线性插值法计算粒子的波束投影点在海图上的水深值；

z_k是k时刻压力传感器测得的潜器的下潜深度；

d_E,k和d_N,k分别表示多普勒测速仪推算的潜器在k和k-1相邻水深测量时刻的东向距离和北向距离分量；

和分别表示第i个粒子一步预测的水平位置和k-1时刻潜器的水平位置估计值之间的东向距离和北向距离分量；

和分别表示k时刻第i个粒子的东向距离差和北向距离差。