本发明公开一种低成本水下潜器地形辅助惯性组合导航定位方法，包括五个步骤：步骤一，初始化组合导航定位系统；步骤二，建立以潜器位置误差为状态量的线性离散状态方程；步骤三，采用高斯过程回归建立以水深值为观测量的非线性离散量测方程；步骤四，构建非线性组合导航系统的粒子滤波算法；步骤五，计算位置误差并修正捷联惯性导航系统位置，完成组合导航位置参数更新，实现水下潜器组合导航的准确定位。本发明具有算法简单、建模准确、定位精度高等优点，为装备低分辨率海图和单波束测深仪的低成本水下潜器提供一种以捷联惯性导航系统为主，地形导航为辅的组合导航定位新方案。
1.一种低成本水下潜器地形辅助惯性组合导航定位方法，其特征在于：具体步骤如下：步骤1)初始化组合导航定位系统，为系统的导航参数更新提供初始值：使用位置传感器获取潜器的初始位置参数；根据惯性测量单元采集的潜器角速率和比力信息，以及多普勒测速仪测得的载体系速度，进行捷联惯性导航系统的初始对准，获取潜器的初始姿态矩阵和导航系初始速度信息；
步骤2)根据潜器的运动模型，建立以潜器位置误差为状态量的3维线性离散状态方程；
步骤3)采用高斯过程回归建立以水深值为观测量的1维非线性离散量测方程；
步骤4)由步骤2)和步骤3)组成非线性组合导航系统模型，构建粒子滤波算法；
步骤5)计算位置误差并修正捷联惯性导航系统位置，完成组合导航位置参数更新；
所述步骤2)中，根据潜器的运动模型，建立以潜器位置误差为状态量的3维线性离散状态方程，具体过程为：以东北天地理坐标系作为导航坐标系，以潜器右前上方向构成的坐标系作为载体坐标系；
xt和xt+1分别为t时刻和t+1时刻潜器的水平位置向量，xt＝[xE,t,xN,t]T，xE,t和xN,t分别为t时刻潜器的东向和北向位置分量；[·]T表示矩阵转置；ut为惯性导航系统计算的从t时刻到t+1时刻潜器的水平位置变化量；vt是t时刻的系统噪声；
为惯性导航系统计算的t时刻潜器的水平位置向量；δxt和δxt+1分别为t时刻和t+1时刻潜器的水平位置误差向量；
由式(2)扩展的以潜器位置误差为状态量的3维线性离散状态方程为ξt+1＝ξt+vt (3)
ξt和ξt+1分别为t时刻和t+1时刻潜器的系统状态向量，ξt＝[δxE,t,δxN,t,bt]T，δxE,t,δxN,t,bt分别为t时刻潜器的东向、北向和天向位置误差分量；
所述步骤3)中，采用高斯过程回归建立以水深值为观测量的1维非线性离散量测方程，具体过程为：
3.1)粒子滤波器选取M个粒子，以第m个粒子，m＝1,2,…,M，的导航系水平位置向量为中心，以海图上3×3邻域内n个节点的3维位置向量作为该粒子海图水深建模的训练样本；
3.2)高斯过程回归模型初始化：设定协方差函数和超参数，采用神经网络协方差函数，变量xp与xq的神经网络协方差函数为式中：
xp和xq为训练样本水平位置向量中的任意元素； 为信号方差； 为尺度矩阵；lE和lN分别为水平东向和北向的特征尺度；超参数θ＝{lE,lN,σf,σn}，其中σn为样本噪声标准差；
3.3)高斯过程回归模型训练：分别以第m个粒子训练样本中水平位置向量集 和含噪声的垂直水深向量集 作为该粒子高斯过程回归的输入数据和输出数据，建立训练样本条件概率p(hm|Xm,θm)的对数似然函数为式中：
K(Xm,Xm)＝{k(xp,m,xq,m)|p,q＝1,2,…,n}为式(4)确定的n×n阶无噪声对称正定的协方差矩阵；θm为第m个粒子的超参数；In为n维单位矩阵；
通过最大化似然函数设置超参数最优解，似然函数对超参数求偏导为式中：
3.4)组合导航过程中，将粒子滤波器的第m个粒子在t时刻的水平位置向量 输入t时刻该粒子建立的高斯过程回归模型，则可得到海图上t时刻对应位置的水深值 及其方差分别为式中：
Xm,t和hm,t分别为t时刻第m个粒子高斯过程回归的输入数据和输出数据；
为t时刻第m个粒子的 和Xm,t之间的1×n阶协方差矩阵， 为 和xp,m,t的神经网络协方差函数，其表达式取式(4)；K(Xm,t,Xm,t)＝{k(xp,m,t,xq,m,t)|p,q＝1,2,…,n}为t时刻第m个粒子的Xm,t和Xm,t之间的n×n阶协方差矩阵，k(xp,m,t,xq,m,t)为xp,m,t和xq,m,t的神经网络协方差函数； 为 自身的神经网络协方差函数，表达式取式(4)中的神经网络协方差函数表达式；
zt为t时刻潜器测量的量测量，具体为单波束测深仪测量的潜器距离海底的高度和压力传感器测量的潜器距离海面的深度之和；海图模型 表示潜器在海图上的水深值， 和 分别为惯性导航系统计算的t时刻潜器的东向和北向位置分量，f(·)采用高斯过程回归建立；ηt为t时刻的量测噪声；
所述步骤4)中，构建非线性组合导航系统的粒子滤波算法，具体过程为：
4.1)由步骤2)和步骤3)组成的非线性组合导航系统模型为式中：
ξt和ξt+1分别为t和t+1时刻的系统状态向量；zt为t时刻的量测量； 为海图模型；
vt和ηt分别为t时刻的系统噪 声向量和量测噪声向量，且vt和ηt满足δtj为δ函数；
4.2)粒子滤波初始化：由先验概率p(ξ0)产生粒子群 所有粒子权值为1/M；
4.3)更新：在t时刻更新粒子权值，重要性权值为式中：
和 分别为第m个粒子在t时刻和t-1时刻的权值， 为服从均值为 方差为的高斯分布；
一种低成本水下潜器地形辅助惯性组合导航定位方法
技术领域
[0001] 本发明涉及一种水下潜器组合导航定位方法，具体涉及一种低成本水下潜器地形辅助惯性组合导航定位方法。
背景技术
[0002] 水下潜器经过长时间航行后，惯性导航系统输出的位置已经积累较大误差。考虑到安全性、隐蔽性和长时间水下作业的实际情况，采用地球物理场辅助惯性导航的无源导航技术，主要包括惯性/地形、惯性/重力和惯性/地磁组合导航。其基本原理都是通过潜器装备的传感器测量航行轨迹上的地球物理场数据，再与已有的海图数据库进行匹配，确定水下潜器的最佳位置。
[0003] 地形导航最早应用在航空领域，作为战机和巡航导弹的一种辅助导航方法，其中最著名的有TERCOM和SITAN。目前，水下地形辅助惯性组合导航的研究多用于装备有导航级惯性系统、多波束测深仪和高分辨率高精度海图数据库的高成本水下潜器。由于水下环境的特殊性，高分辨率的海图数据库往往很难获取，因此，针对装备低分辨率海图及单波束测深仪的低成本水下潜器地形辅助惯性组合导航定位的研究具有实际意义。
[0004] 海底地形的变化通过水深的分布情况反映，在地形辅助惯性组合导航的研究中，海图水深模型建立的准确性是影响导航精度的主要因素之一。在海图分辨率较高情况下(优于10m)，海图水深建模多采用最邻近点插值法和双线性插值法。但在海图分辨率较低情况下(大于10m小于30m)，以上方法构建的水深模型准确度差，导致组合导航定位精度急剧下降。因此，建立一种适于低分辨率海图的水深模型，有助于提高组合导航的定位精度，同时可扩展地形辅助惯性组合导航定位在低成本水下潜器中的应用。
发明内容
[0005] 本发明的目的是为了解决高分辨率海图采用的海图水深模型不再适用于低分辨率海图水深建模，导致组合导航定位精度下降的问题，提供一种低成本水下潜器地形辅助惯性组合导航定位方法，该方法采用高斯过程回归建立海图水深模型，利用高斯过程回归在海图采样点学习和预测的特点，可准确构建海图水深模型，进而提高低成本水下潜器的组合导航定位精度。
[0006] 本发明采用的技术方案为：一种低成本水下潜器地形辅助惯性组合导航定位方法，具体步骤如下：
[0007] 步骤1)初始化组合导航定位系统，为系统的导航参数更新提供初始值：使用位置传感器获取潜器的初始位置参数；根据惯性测量单元采集的潜器角速率和比力信息，以及多普勒测速仪测得的载体系速度，进行捷联惯性导航系统的初始对准，获取潜器的初始姿态矩阵和导航系初始速度信息；
[0008] 步骤2)根据潜器的运动模型，建立以潜器位置误差为状态量的3维线性离散状态方程；
[0009] 步骤3)采用高斯过程回归建立以水深值为观测量的1维非线性离散量测方程；
[0010] 步骤4)由步骤2)和步骤3)组成非线性组合导航系统模型，构建粒子滤波算法；
[0011] 步骤5)计算位置误差并修正捷联惯性导航系统位置，完成组合导航位置参数更新。
[0012] 作为优选，所述步骤2)中，根据潜器的运动模型，建立以潜器位置误差为状态量的3维线性离散状态方程，具体过程为：
[0013] 以东北天地理坐标系作为导航坐标系，以潜器右前上方向构成的坐标系作为载体坐标系；
[0014] 潜器的运动模型为
[0015] xt+1＝xt+ut+vt (1)
[0016] 式中：
[0017] xt和xt+1分别为t时刻和t+1时刻潜器的水平位置向量，xt＝[xE,t,xN,t]T，xE,t和xN,t分别为t时刻潜器的东向和北向位置分量；[·]T表示矩阵转置；ut为惯性导航系统计算的从t时刻到t+1时刻潜器的水平位置变化量；vt是t时刻的系统噪声；
[0018] 定义 则
[0019] δxt+1＝δxt+vt (2)
[0020] 式中：
[0021] 为惯性导航系统计算的t时刻潜器的水平位置向量；δxt和δxt+1分别为t时刻和t+1时刻潜器的水平位置误差向量；
[0022] 由式(2)扩展的以潜器位置误差为状态量的3维线性离散状态方程为
[0023] ξt+1＝ξt+vt (3)
[0024] 式中：
[0025] ξt和ξt+1分别为t时刻和t+1时刻潜器的位置误差向量，ξt＝[δxE,t,δxN,t,bt]T，δxE,t,δxN,t,bt分别为t时刻潜器的东向、北向和天向位置误差分量。
[0026] 作为优选，所述步骤3)中，采用高斯过程回归建立以水深值为观测量的1维非线性离散量测方程，具体过程为：
[0027] 3.1)粒子滤波器选取M个粒子，以第m个粒子，m＝1,2,…,M，的导航系水平位置向量为中心，以海图上3×3邻域内n个节点的3维位置向量作为该粒子海图水深建模的训练样本；
[0028] 3.2)高斯过程回归模型初始化：设定协方差函数和超参数，采用神经网络协方差函数，变量xp与xq的神经网络协方差函数为
[0029]
[0030] 式中：
[0031] xp和xq为训练样本水平位置向量中的任意元素； 为信号方差； 为尺度矩阵；lE和lN分别为水平东向和北向的特征尺度；超参数θ＝{lE,lN,σf,σn}，其中σn为样本噪声标准差；
[0032] 3.3)高斯过程回归模型训练：分别以第m个粒子训练样本中水平位置向量集和含噪声的垂直水深向量集 作为该粒子高斯过程回归的输入数据和输出数据，建立训练样本条件概率p(hm|Xm,θm)的对数似然函数为
[0033]
[0034] 式中：
[0035] K(Xm,Xm)＝{k(xp,m,xq,m)|p,q＝1,2,…,n}为式(4)确定的n×n阶无噪声对称正定的协方差矩阵；θm为第m个粒子的超参数；In为n维单位矩阵；
[0036] 通过最大化似然函数设置超参数最优解，似然函数对超参数求偏导为[0037]
[0038] 式中：
[0039] tr(·)表示矩阵的迹；
[0040] 3.4)组合导航过程中，将粒子滤波器的第m个粒子在t时刻的水平位置向量 输入t时刻该粒子建立的高斯过程回归模型，则可得到海图上t时刻对应位置的水深值 及其方差 分别为
[0041]
[0042]
[0043] 式中：
[0044] Xm,t和hm,t分别为t时刻第m个粒子高斯过程回归的输入数据和输出数据；
[0045] 为t时刻第m个粒子的 和Xm,t之间的1×n阶协方差矩阵， 为 和xp,m,t的协方差函数，其表达式取式(4)；K(Xm,t,Xm,t)＝{k(xp,m,t,xq,m,t)|p,q＝1,2,…,n}为t时刻第m个粒子的Xm,t和Xm,t之间的n×n阶协方差矩阵，k(xp,m,t,xq,m,t)为xp,m,t和xq,m,t的神经网络协方差函数； 为 自身的协方差函数，表达式取式(4)中的神经网络协方差函数表达式；
[0046] 3.5)非线性离散量测方程为
[0047]
[0048] 式中：
[0049] zt为t时刻潜器测量的水深值，具体为单波束测深仪测量的潜器距离海底的高度和压力传感器测量的潜器距离海面的深度之和；海图模型表示潜器在海图上的水深值， 和 分别为惯性导航系统计算的t时刻潜器的东向和北向位置分量，f(·)采用高斯过程回归建立；ηt为t时刻的量测噪声。
[0050] 作为优选，所述步骤4)中，构建非线性组合导航系统的粒子滤波算法，具体过程为：
[0051] 4.1)由步骤2)和步骤3)组成的非线性组合导航系统模型为
[0052]
[0053] 式中：
[0054] ξt和ξt+1分别为t和t+1时刻的系统状态向量；zt为t时刻的量测量； 为t时刻的非线性观测函数；vt和ηt分别为t时刻的系统噪声向量和量测噪声向量，且vt和ηt满足δtj为δ函数；
[0055] 4.2)粒子滤波初始化：由先验概率p(ξ0)产生粒子群 所有粒子权值为1/M；
[0056] 4.3)更新：在t时刻更新粒子权值，重要性权值为
[0057]
[0058] 式中：
[0059] 和 分别为第m个粒子在t时刻和t-1时刻的权值， 为服从均值为 方差为 的高斯分布；
[0060] 并且归一化： 则得到t时刻状态量的最小均方估计为
[0061] 4.4)重采样：得到新的粒子集合
[0062] 4.5)预测：利用状态方程式(3)预测
[0063] 4.6)时刻t:＝t+1，转到4.3)。
[0064] 有益效果：
[0065] (1)本发明采用高斯过程回归构建海图水深模型，不仅能对系统观测量进行预测，还能给出预测值的方差，从而可以对预测值的不确定性进行定量分析。
[0066] (2)本发明采用地形辅助惯性导航松组合的定位方式对惯性导航计算的水下潜器位置进行修正，这种模式下的地形导航模块可移植性强，整个组合导航定位系统具有更强的鲁棒性。
[0067] (3)本发明与最邻近点插值和双线性插值方法相比，在低分辨率海图下，水深模型构建更准确，组合导航定位精度更高。
附图说明
[0068] 图1为一种低成本水下潜器地形辅助惯性组合导航定位流程框图；
[0069] 图2为水下潜器实际航行轨迹和捷联惯性导航系统(SINS)计算的潜器航行轨迹以及海图等深线分布图。
具体实施方式
[0070] 下面结合附图和具体实施方式对本发明作进一步的说明。
[0071] 如图1所示，本发明的一种低成本水下潜器地形辅助惯性组合导航定位方法，具体步骤如下：
[0072] 步骤1)初始化组合导航定位系统，为系统的导航参数更新提供初始值：使用位置传感器获取潜器的初始位置参数；根据惯性测量单元采集的潜器角速率和比力信息，以及多普勒测速仪测得的载体系速度，进行捷联惯性导航系统的初始对准，获取潜器的初始姿态矩阵和导航系初始速度信息；
[0073] 步骤2)根据潜器的运动模型，建立以潜器位置误差为状态量的3维线性离散状态方程，具体过程为：
[0074] 以东北天地理坐标系作为导航坐标系，以潜器右前上方向构成的坐标系作为载体坐标系；
[0075] 潜器的运动模型为
[0076] xt+1＝xt+ut+vt (1)
[0077] 式中：
[0078] xt和xt+1分别为t时刻和t+1时刻潜器的水平位置向量，xt＝[xE,t,xN,t]T，xE,t和xN,t分别为t时刻潜器的东向和北向位置分量；[·]T表示矩阵转置；ut为惯性导航系统计算的从t时刻到t+1时刻潜器的水平位置变化量；vt是t时刻的系统噪声；
[0079] 定义 则
[0080] δxt+1＝δxt+vt (2)
[0081] 式中：
[0082] 为惯性导航系统计算的t时刻潜器的水平位置向量；δxt和δxt+1分别为t时刻和t+1时刻潜器的水平位置误差向量；
[0083] 由式(2)扩展的以潜器位置误差为状态量的3维线性离散状态方程为
[0084] ξt+1＝ξt+vt (3)
[0085] 式中：
[0086] ξt和ξt+1分别为t时刻和t+1时刻潜器的位置误差向量，ξt＝[δxE,t,δxN,t,bt]T，δxE,t,δxN,t,bt分别为t时刻潜器的东向、北向和天向位置误差分量。
[0087] 步骤3)采用高斯过程回归建立以水深值为观测量的1维非线性离散量测方程，具体过程为：
[0088] 3.1)粒子滤波器选取M个粒子，以第m个粒子，m＝1,2,…,M，的导航系水平位置向量为中心，以海图上3×3邻域内n个节点的3维位置向量作为该粒子海图水深建模的训练样本；
[0089] 3.2)高斯过程回归模型初始化：设定协方差函数和超参数，采用神经网络协方差函数，变量xp与xq的神经网络协方差函数为
[0090]
[0091] 式中：
[0092] xp和xq为训练样本水平位置向量中的任意元素； 为信号方差； 为尺度矩阵；lE和lN分别为水平东向和北向的特征尺度；超参数θ＝{lE,lN,σf,σn}，其中σn为样本噪声标准差；
[0093] 3.3)高斯过程回归模型训练：分别以第m个粒子训练样本中水平位置向量集和含噪声的垂直水深向量集 作为该粒子高斯过程回归的输入数据和输出数据，建立训练样本条件概率p(hm|Xm,θm)的对数似然函数为
[0094]
[0095] 式中：
[0096] K(Xm,Xm)＝{k(xp,m,xq,m)|p,q＝1,2,…,n}为式(4)确定的n×n阶无噪声对称正定的协方差矩阵；θm为第m个粒子的超参数；In为n维单位矩阵；
[0097] 通过最大化似然函数设置超参数最优解，似然函数对超参数求偏导为[0098]
[0099] 式中：
[0100] tr(·)表示矩阵的迹；
[0101] 3.4)组合导航过程中，将粒子滤波器的第m个粒子在t时刻的水平位置向量 输入t时刻该粒子建立的高斯过程回归模型，则可得到海图上t时刻对应位置的水深值 及其方差 分别为
[0102]
[0103]
[0104] 式中：
[0105] Xm,t和hm,t分别为t时刻第m个粒子高斯过程回归的输入数据和输出数据；
[0106] 为t时刻第m个粒子的 和Xm,t之间的1×n阶协方差矩阵， 为 和xp,m,t的协方差函数，其表达式取式(4)；K(Xm,t,Xm,t)＝{k(xp,m,t,xq,m,t)|p,q＝1,2,…,n}为t时刻第m个粒子的Xm,t和Xm,t之间的n×n阶协方差矩阵，k(xp,m,t,xq,m,t)为xp,m,t和xq,m,t的神经网络协方差函数； 为 自身的协方差函数，表达式取式(4)中的神经网络协方差函数表达式；
[0107] 3.5)非线性离散量测方程为
[0108]
[0109] 式中：
[0110] zt为t时刻潜器测量的水深值，具体为单波束测深仪测量的潜器距离海底的高度和压力传感器测量的潜器距离海面的深度之和；海图模型表示潜器在海图上的水深值， 和 分别为惯性导航系统计算的t时刻潜器的东向和北向位置分量，f(·)采用高斯过程回归建立；ηt为t时刻的量测噪声。
[0111] 步骤4)由步骤2)和步骤3)组成非线性组合导航系统模型，构建粒子滤波算法，具体过程为：
[0112] 4.1)由步骤2)和步骤3)组成的非线性组合导航系统模型为
[0113]
[0114] 式中：
[0115] ξt和ξt+1分别为t和t+1时刻的系统状态向量；zt为t时刻的量测量； 为t时刻的非线性观测函数；vt和ηt分别为t时刻的系统噪声向量和量测噪声向量，且vt和ηt满足δtj为δ函数；
[0116] 4.2)粒子滤波初始化：由先验概率p(ξ0)产生粒子群 所有粒子权值为1/M；
[0117] 4.3)更新：在t时刻更新粒子权值，重要性权值为
[0118]
[0119] 式中：
[0120] 和 分别为第m个粒子在t时刻和t-1时刻的权值， 为服从均值为 方差为 的高斯分布；
[0121] 并且归一化： 则得到t时刻状态量的最小均方估计为
[0122] 4.4)重采样：得到新的粒子集合
[0123] 4.5)预测：利用状态方程式(3)预测
[0124] 4.6)时刻t:＝t+1，转到4.3)。
[0125] 步骤5)计算位置误差并修正捷联惯性导航系统位置，完成组合导航位置参数更新。
[0126] 本发明的可行性通过如下仿真加以验证：
[0127] (1)地形辅助导航(TAN)和捷联惯性导航系统(SINS)，构成以SINS为主、TAN为辅的SINS/TAN组合导航定位系统，水下潜器以4kn速度匀速直航，初始位置分别为东向1134m，北向911.7m，天向-112.9m，初始航向为北偏东145°；
[0128] (2)根据选用的惯性测量单元精度，设置陀螺仪随机常值漂移0.05°/h，随机白噪声0.05°/ 加速度计随机常值偏置0.2mg，随机白噪声0.1mg/ 多普勒测速仪测量误差0.05m/s，初始水平位置误差100m，初始航向角误差0.5°；
[0129] (3)海图范围：3000m×1200m，海图分辨率分为10m、20m和30m，水深量程范围：-241.7m～0m，海图水深误差为20cm，单波束测深仪量程范围0.3m～75m，测量精度1cm，压力传感器测深范围0.1m～300m，测量精度为深度的0.01％，水深测量误差±1m；
[0130] (4)惯性传感器数据更新周期为5ms，滤波周期为10s，航行时间7mins；
[0131] (5)粒子滤波器选用1000个粒子，高斯过程回归采用神经网络协方差函数，超参数初始值θ0＝{1,1,3,0.01}。
[0132] 水下潜器实际航行轨迹和SINS计算的潜器航行轨迹以及海图等深线分布如图2所示。在不同海图分辨率下，分别采用最邻近点插值、双线性插值法和高斯过程回归构建海图水深模型，实现水下潜器地形辅助惯性组合导航定位。通过50次蒙特卡洛实验，取50次实验的平均值，得到组合导航定位结果如表1所示。由如下表1可以看出，当海图分辨率为10m时，采用双线性插值法和高斯过程回归构建水深模型的组合导航定位精度均小于一个海图分辨率；当海图分辨率为30m时，本发明的定位精度小于一个海图分辨率，定位效果均优于以上两种方法。
[0133] 表1
[0134]
[0135] 以上结合附图对本发明的实施方式做出详细说明，但本发明不局限于所描述的实施方式。对本领域的普通技术人员而言，在本发明的原理和技术思想的范围内，对这些实施方式进行多种变化、修改、替换和变形仍落入本发明的保护范围内。
