CN105424036B - 一种低成本水下潜器地形辅助惯性组合导航定位方法 - Google Patents

一种低成本水下潜器地形辅助惯性组合导航定位方法 Download PDF

Info

Publication number
CN105424036B
CN105424036B CN201510758207.XA CN201510758207A CN105424036B CN 105424036 B CN105424036 B CN 105424036B CN 201510758207 A CN201510758207 A CN 201510758207A CN 105424036 B CN105424036 B CN 105424036B
Authority
CN
China
Prior art keywords
mrow
msub
msubsup
particle
formula
Prior art date
Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
Active
Application number
CN201510758207.XA
Other languages
English (en)
Other versions
CN105424036A (zh
Inventor
程向红
周玲
朱倚娴
戴晨曦
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Southeast University
Original Assignee
Southeast University
Priority date (The priority date is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the date listed.)
Filing date
Publication date
Application filed by Southeast University filed Critical Southeast University
Priority to CN201510758207.XA priority Critical patent/CN105424036B/zh
Publication of CN105424036A publication Critical patent/CN105424036A/zh
Application granted granted Critical
Publication of CN105424036B publication Critical patent/CN105424036B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Classifications

    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G01C21/10—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration
    • G01C21/12—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning
    • G01C21/16—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G01C21/20—Instruments for performing navigational calculations
    • G01C21/203—Specially adapted for sailing ships

Landscapes

  • Engineering & Computer Science (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (AREA)
  • Automation & Control Theory (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Navigation (AREA)

Abstract

本发明公开一种低成本水下潜器地形辅助惯性组合导航定位方法，包括五个步骤：步骤一，初始化组合导航定位系统；步骤二，建立以潜器位置误差为状态量的线性离散状态方程；步骤三，采用高斯过程回归建立以水深值为观测量的非线性离散量测方程；步骤四，构建非线性组合导航系统的粒子滤波算法；步骤五，计算位置误差并修正捷联惯性导航系统位置，完成组合导航位置参数更新，实现水下潜器组合导航的准确定位。本发明具有算法简单、建模准确、定位精度高等优点，为装备低分辨率海图和单波束测深仪的低成本水下潜器提供一种以捷联惯性导航系统为主，地形导航为辅的组合导航定位新方案。

Description

一种低成本水下潜器地形辅助惯性组合导航定位方法
技术领域
本发明涉及一种水下潜器组合导航定位方法，具体涉及一种低成本水下潜器地形辅助惯性组合导航定位方法。
背景技术
水下潜器经过长时间航行后，惯性导航系统输出的位置已经积累较大误差。考虑到安全性、隐蔽性和长时间水下作业的实际情况，采用地球物理场辅助惯性导航的无源导航技术，主要包括惯性/地形、惯性/重力和惯性/地磁组合导航。其基本原理都是通过潜器装备的传感器测量航行轨迹上的地球物理场数据，再与已有的海图数据库进行匹配，确定水下潜器的最佳位置。
地形导航最早应用在航空领域，作为战机和巡航导弹的一种辅助导航方法，其中最著名的有TERCOM和SITAN。目前，水下地形辅助惯性组合导航的研究多用于装备有导航级惯性系统、多波束测深仪和高分辨率高精度海图数据库的高成本水下潜器。由于水下环境的特殊性，高分辨率的海图数据库往往很难获取，因此，针对装备低分辨率海图及单波束测深仪的低成本水下潜器地形辅助惯性组合导航定位的研究具有实际意义。
海底地形的变化通过水深的分布情况反映，在地形辅助惯性组合导航的研究中，海图水深模型建立的准确性是影响导航精度的主要因素之一。在海图分辨率较高情况下(优于10m)，海图水深建模多采用最邻近点插值法和双线性插值法。但在海图分辨率较低情况下(大于10m小于30m)，以上方法构建的水深模型准确度差，导致组合导航定位精度急剧下降。因此，建立一种适于低分辨率海图的水深模型，有助于提高组合导航的定位精度，同时可扩展地形辅助惯性组合导航定位在低成本水下潜器中的应用。
发明内容
本发明的目的是为了解决高分辨率海图采用的海图水深模型不再适用于低分辨率海图水深建模，导致组合导航定位精度下降的问题，提供一种低成本水下潜器地形辅助惯性组合导航定位方法，该方法采用高斯过程回归建立海图水深模型，利用高斯过程回归在海图采样点学习和预测的特点，可准确构建海图水深模型，进而提高低成本水下潜器的组合导航定位精度。
本发明采用的技术方案为：一种低成本水下潜器地形辅助惯性组合导航定位方法，具体步骤如下：
步骤1)初始化组合导航定位系统，为系统的导航参数更新提供初始值：使用位置传感器获取潜器的初始位置参数；根据惯性测量单元采集的潜器角速率和比力信息，以及多普勒测速仪测得的载体系速度，进行捷联惯性导航系统的初始对准，获取潜器的初始姿态矩阵和导航系初始速度信息；
步骤2)根据潜器的运动模型，建立以潜器位置误差为状态量的3维线性离散状态方程；
步骤3)采用高斯过程回归建立以水深值为观测量的1维非线性离散量测方程；
步骤4)由步骤2)和步骤3)组成非线性组合导航系统模型，构建粒子滤波算法；
步骤5)计算位置误差并修正捷联惯性导航系统位置，完成组合导航位置参数更新。
作为优选，所述步骤2)中，根据潜器的运动模型，建立以潜器位置误差为状态量的3维线性离散状态方程，具体过程为：
以东北天地理坐标系作为导航坐标系，以潜器右前上方向构成的坐标系作为载体坐标系；
潜器的运动模型为
xt+1＝xt+ut+vt (1)
式中：
xt和xt+1分别为t时刻和t+1时刻潜器的水平位置向量，xt＝[xE,t,xN,t]T，xE,t和xN,t分别为t时刻潜器的东向和北向位置分量；[·]T表示矩阵转置；ut为惯性导航系统计算的从t时刻到t+1时刻潜器的水平位置变化量；vt是t时刻的系统噪声；
定义则
δxt+1＝δxt+vt (2)
式中：
为惯性导航系统计算的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维单位矩阵；
通过最大化似然函数设置超参数最优解，似然函数对超参数求偏导为
式中：
tr(·)表示矩阵的迹；
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)中的神经网络协方差函数表达式；
3.5)非线性离散量测方程为
式中：
zt为t时刻潜器测量的水深值，具体为单波束测深仪测量的潜器距离海底的高度和压力传感器测量的潜器距离海面的深度之和；海图模型表示潜器在海图上的水深值，和分别为惯性导航系统计算的t时刻潜器的东向和北向位置分量，f(·)采用高斯过程回归建立；ηt为t时刻的量测噪声。
作为优选，所述步骤4)中，构建非线性组合导航系统的粒子滤波算法，具体过程为：
4.1)由步骤2)和步骤3)组成的非线性组合导航系统模型为
式中：
ξt和ξt+1分别为t和t+1时刻的系统状态向量；zt为t时刻的量测量；为t时刻的非线性观测函数；vt和ηt分别为t时刻的系统噪声向量和量测噪声向量，且vt和ηt满足δtj为δ函数；
4.2)粒子滤波初始化：由先验概率p(ξ0)产生粒子群所有粒子权值为1/M；
4.3)更新：在t时刻更新粒子权值，重要性权值为
式中：
和分别为第m个粒子在t时刻和t-1时刻的权值，为服从均值为方差为的高斯分布；
并且归一化：则得到t时刻状态量的最小均方估计为
4.4)重采样：得到新的粒子集合
4.5)预测：利用状态方程式(3)预测
4.6)时刻t:＝t+1，转到4.3)。
有益效果：
(1)本发明采用高斯过程回归构建海图水深模型，不仅能对系统观测量进行预测，还能给出预测值的方差，从而可以对预测值的不确定性进行定量分析。
(2)本发明采用地形辅助惯性导航松组合的定位方式对惯性导航计算的水下潜器位置进行修正，这种模式下的地形导航模块可移植性强，整个组合导航定位系统具有更强的鲁棒性。
(3)本发明与最邻近点插值和双线性插值方法相比，在低分辨率海图下，水深模型构建更准确，组合导航定位精度更高。
附图说明
图1为一种低成本水下潜器地形辅助惯性组合导航定位流程框图；
图2为水下潜器实际航行轨迹和捷联惯性导航系统(SINS)计算的潜器航行轨迹以及海图等深线分布图。
具体实施方式
下面结合附图和具体实施方式对本发明作进一步的说明。
如图1所示，本发明的一种低成本水下潜器地形辅助惯性组合导航定位方法，具体步骤如下：
步骤1)初始化组合导航定位系统，为系统的导航参数更新提供初始值：使用位置传感器获取潜器的初始位置参数；根据惯性测量单元采集的潜器角速率和比力信息，以及多普勒测速仪测得的载体系速度，进行捷联惯性导航系统的初始对准，获取潜器的初始姿态矩阵和导航系初始速度信息；
步骤2)根据潜器的运动模型，建立以潜器位置误差为状态量的3维线性离散状态方程，具体过程为：
以东北天地理坐标系作为导航坐标系，以潜器右前上方向构成的坐标系作为载体坐标系；
潜器的运动模型为
xt+1＝xt+ut+vt (1)
式中：
xt和xt+1分别为t时刻和t+1时刻潜器的水平位置向量，xt＝[xE,t,xN,t]T，xE,t和xN,t分别为t时刻潜器的东向和北向位置分量；[·]T表示矩阵转置；ut为惯性导航系统计算的从t时刻到t+1时刻潜器的水平位置变化量；vt是t时刻的系统噪声；
定义则
δxt+1＝δxt+vt (2)
式中：
为惯性导航系统计算的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维单位矩阵；
通过最大化似然函数设置超参数最优解，似然函数对超参数求偏导为
式中：
tr(·)表示矩阵的迹；
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)中的神经网络协方差函数表达式；
3.5)非线性离散量测方程为
式中：
zt为t时刻潜器测量的水深值，具体为单波束测深仪测量的潜器距离海底的高度和压力传感器测量的潜器距离海面的深度之和；海图模型表示潜器在海图上的水深值，和分别为惯性导航系统计算的t时刻潜器的东向和北向位置分量，f(·)采用高斯过程回归建立；ηt为t时刻的量测噪声。
步骤4)由步骤2)和步骤3)组成非线性组合导航系统模型，构建粒子滤波算法，具体过程为：
4.1)由步骤2)和步骤3)组成的非线性组合导航系统模型为
式中：
ξt和ξt+1分别为t和t+1时刻的系统状态向量；zt为t时刻的量测量；为t时刻的非线性观测函数；vt和ηt分别为t时刻的系统噪声向量和量测噪声向量，且vt和ηt满足δtj为δ函数；
4.2)粒子滤波初始化：由先验概率p(ξ0)产生粒子群所有粒子权值为1/M；
4.3)更新：在t时刻更新粒子权值，重要性权值为
式中：
和分别为第m个粒子在t时刻和t-1时刻的权值，为服从均值为方差为的高斯分布；
并且归一化：则得到t时刻状态量的最小均方估计为
4.4)重采样：得到新的粒子集合
4.5)预测：利用状态方程式(3)预测
4.6)时刻t:＝t+1，转到4.3)。
步骤5)计算位置误差并修正捷联惯性导航系统位置，完成组合导航位置参数更新。
本发明的可行性通过如下仿真加以验证：
(1)地形辅助导航(TAN)和捷联惯性导航系统(SINS)，构成以SINS为主、TAN为辅的SINS/TAN组合导航定位系统，水下潜器以4kn速度匀速直航，初始位置分别为东向1134m，北向911.7m，天向-112.9m，初始航向为北偏东145°；
(2)根据选用的惯性测量单元精度，设置陀螺仪随机常值漂移0.05°/h，随机白噪声0.05°/加速度计随机常值偏置0.2mg，随机白噪声0.1mg/多普勒测速仪测量误差0.05m/s，初始水平位置误差100m，初始航向角误差0.5°；
(3)海图范围：3000m×1200m，海图分辨率分为10m、20m和30m，水深量程范围：-241.7m～0m，海图水深误差为20cm，单波束测深仪量程范围0.3m～75m，测量精度1cm，压力传感器测深范围0.1m～300m，测量精度为深度的0.01％，水深测量误差±1m；
(4)惯性传感器数据更新周期为5ms，滤波周期为10s，航行时间7mins；
(5)粒子滤波器选用1000个粒子，高斯过程回归采用神经网络协方差函数，超参数初始值θ0＝{1,1,3,0.01}。
水下潜器实际航行轨迹和SINS计算的潜器航行轨迹以及海图等深线分布如图2所示。在不同海图分辨率下，分别采用最邻近点插值、双线性插值法和高斯过程回归构建海图水深模型，实现水下潜器地形辅助惯性组合导航定位。通过50次蒙特卡洛实验，取50次实验的平均值，得到组合导航定位结果如表1所示。由如下表1可以看出，当海图分辨率为10m时，采用双线性插值法和高斯过程回归构建水深模型的组合导航定位精度均小于一个海图分辨率；当海图分辨率为30m时，本发明的定位精度小于一个海图分辨率，定位效果均优于以上两种方法。
表1
以上结合附图对本发明的实施方式做出详细说明，但本发明不局限于所描述的实施方式。对本领域的普通技术人员而言，在本发明的原理和技术思想的范围内，对这些实施方式进行多种变化、修改、替换和变形仍落入本发明的保护范围内。

Claims (1)

1.一种低成本水下潜器地形辅助惯性组合导航定位方法，其特征在于：具体步骤如下：
步骤1)初始化组合导航定位系统，为系统的导航参数更新提供初始值：使用位置传感器获取潜器的初始位置参数；根据惯性测量单元采集的潜器角速率和比力信息，以及多普勒测速仪测得的载体系速度，进行捷联惯性导航系统的初始对准，获取潜器的初始姿态矩阵和导航系初始速度信息；
步骤2)根据潜器的运动模型，建立以潜器位置误差为状态量的3维线性离散状态方程；
步骤3)采用高斯过程回归建立以水深值为观测量的1维非线性离散量测方程；
步骤4)由步骤2)和步骤3)组成非线性组合导航系统模型，构建粒子滤波算法；
步骤5)计算位置误差并修正捷联惯性导航系统位置，完成组合导航位置参数更新；
所述步骤2)中，根据潜器的运动模型，建立以潜器位置误差为状态量的3维线性离散状态方程，具体过程为：
以东北天地理坐标系作为导航坐标系，以潜器右前上方向构成的坐标系作为载体坐标系；
潜器的运动模型为
xt+1＝xt+ut+vt (1)
式中：
xt和xt+1分别为t时刻和t+1时刻潜器的水平位置向量，xt＝[xE,t,xN,t]T，xE,t和xN,t分别为t时刻潜器的东向和北向位置分量；[·]T表示矩阵转置；ut为惯性导航系统计算的从t时刻到t+1时刻潜器的水平位置变化量；vt是t时刻的系统噪声；
定义则
δxt+1＝δxt+vt (2)
式中：
为惯性导航系统计算的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的神经网络协方差函数为
<mrow> <mi>k</mi> <mrow> <mo>(</mo> <mrow> <msub> <mi>x</mi> <mi>p</mi> </msub> <mo>,</mo> <msub> <mi>x</mi> <mi>q</mi> </msub> </mrow> <mo>)</mo> </mrow> <mo>=</mo> <msubsup> <mi>&amp;sigma;</mi> <mi>f</mi> <mn>2</mn> </msubsup> <mi>arcsin</mi> <mrow> <mo>&amp;lsqb;</mo> <mfrac> <mrow> <mn>2</mn> <msubsup> <mi>x</mi> <mi>p</mi> <mi>T</mi> </msubsup> <msub> <mi>&amp;Lambda;x</mi> <mi>q</mi> </msub> </mrow> <msqrt> <mrow> <mrow> <mo>(</mo> <mrow> <mn>1</mn> <mo>+</mo> <mn>2</mn> <msubsup> <mi>x</mi> <mi>p</mi> <mi>T</mi> </msubsup> <msub> <mi>&amp;Lambda;x</mi> <mi>p</mi> </msub> </mrow> <mo>)</mo> </mrow> <mrow> <mo>(</mo> <mrow> <mn>1</mn> <mo>+</mo> <mn>2</mn> <msubsup> <mi>x</mi> <mi>q</mi> <mi>T</mi> </msubsup> <msub> <mi>&amp;Lambda;x</mi> <mi>q</mi> </msub> </mrow> <mo>)</mo> </mrow> </mrow> </msqrt> </mfrac> <mo>&amp;rsqb;</mo> </mrow> <mo>-</mo> <mo>-</mo> <mo>-</mo> <mrow> <mo>(</mo> <mn>4</mn> <mo>)</mo> </mrow> </mrow>
式中：
xp和xq为训练样本水平位置向量中的任意元素；为信号方差；为尺度矩阵；lE和lN分别为水平东向和北向的特征尺度；超参数θ＝{lE,lN,σf,σn}，其中σn为样本噪声标准差；
3.3)高斯过程回归模型训练：分别以第m个粒子训练样本中水平位置向量集和含噪声的垂直水深向量集作为该粒子高斯过程回归的输入数据和输出数据，建立训练样本条件概率p(hm|Xm,θm)的对数似然函数为
<mrow> <mi>log</mi> <mi> </mi> <mi>p</mi> <mrow> <mo>(</mo> <msub> <mi>h</mi> <mi>m</mi> </msub> <mo>|</mo> <msub> <mi>X</mi> <mi>m</mi> </msub> <mo>,</mo> <msub> <mi>&amp;theta;</mi> <mi>m</mi> </msub> <mo>)</mo> </mrow> <mo>=</mo> <mo>-</mo> <mfrac> <mn>1</mn> <mn>2</mn> </mfrac> <msubsup> <mi>h</mi> <mi>m</mi> <mi>T</mi> </msubsup> <msubsup> <mi>K</mi> <mrow> <mi>h</mi> <mo>,</mo> <mi>m</mi> </mrow> <mrow> <mo>-</mo> <mn>1</mn> </mrow> </msubsup> <msub> <mi>h</mi> <mi>m</mi> </msub> <mo>-</mo> <mfrac> <mn>1</mn> <mn>2</mn> </mfrac> <mi>l</mi> <mi>o</mi> <mi>g</mi> <mo>|</mo> <msub> <mi>K</mi> <mrow> <mi>h</mi> <mo>,</mo> <mi>m</mi> </mrow> </msub> <mo>|</mo> <mo>-</mo> <mfrac> <mi>n</mi> <mn>2</mn> </mfrac> <mi>l</mi> <mi>o</mi> <mi>g</mi> <mrow> <mo>(</mo> <mn>2</mn> <mi>&amp;pi;</mi> <mo>)</mo> </mrow> </mrow>
式中：
K(Xm,Xm)＝{k(xp,m,xq,m)|p,q＝1,2,…,n}为式(4)确定的n×n阶无噪声对称正定的协方差矩阵；θm为第m个粒子的超参数；In为n维单位矩阵；
通过最大化似然函数设置超参数最优解，似然函数对超参数求偏导为
<mrow> <mfrac> <mo>&amp;part;</mo> <mrow> <mo>&amp;part;</mo> <msub> <mi>&amp;theta;</mi> <mi>m</mi> </msub> </mrow> </mfrac> <mi>log</mi> <mi> </mi> <mi>p</mi> <mrow> <mo>(</mo> <msub> <mi>h</mi> <mi>m</mi> </msub> <mo>|</mo> <msub> <mi>X</mi> <mi>m</mi> </msub> <mo>,</mo> <msub> <mi>&amp;theta;</mi> <mi>m</mi> </msub> <mo>)</mo> </mrow> <mo>=</mo> <mfrac> <mn>1</mn> <mn>2</mn> </mfrac> <mi>t</mi> <mi>r</mi> <mrow> <mo>(</mo> <mo>(</mo> <mrow> <msup> <mi>&amp;alpha;&amp;alpha;</mi> <mi>T</mi> </msup> <mo>-</mo> <msubsup> <mi>K</mi> <mrow> <mi>h</mi> <mo>,</mo> <mi>m</mi> </mrow> <mrow> <mo>-</mo> <mn>1</mn> </mrow> </msubsup> </mrow> <mo>)</mo> <mfrac> <mrow> <mo>&amp;part;</mo> <msub> <mi>K</mi> <mrow> <mi>h</mi> <mo>,</mo> <mi>m</mi> </mrow> </msub> </mrow> <mrow> <mo>&amp;part;</mo> <msub> <mi>&amp;theta;</mi> <mi>m</mi> </msub> </mrow> </mfrac> <mo>)</mo> </mrow> </mrow>
式中：
tr(·)表示矩阵的迹；
3.4)组合导航过程中，将粒子滤波器的第m个粒子在t时刻的水平位置向量输入t时刻该粒子建立的高斯过程回归模型，则可得到海图上t时刻对应位置的水深值及其方差分别为
<mrow> <msubsup> <mi>h</mi> <mrow> <mi>m</mi> <mo>,</mo> <mi>t</mi> </mrow> <mo>*</mo> </msubsup> <mo>=</mo> <mi>K</mi> <mrow> <mo>(</mo> <msubsup> <mi>x</mi> <mrow> <mi>m</mi> <mo>,</mo> <mi>t</mi> </mrow> <mo>*</mo> </msubsup> <mo>,</mo> <msub> <mi>X</mi> <mrow> <mi>m</mi> <mo>,</mo> <mi>t</mi> </mrow> </msub> <mo>)</mo> </mrow> <msup> <mrow> <mo>&amp;lsqb;</mo> <mi>K</mi> <mrow> <mo>(</mo> <msub> <mi>X</mi> <mrow> <mi>m</mi> <mo>,</mo> <mi>t</mi> </mrow> </msub> <mo>,</mo> <msub> <mi>X</mi> <mrow> <mi>m</mi> <mo>,</mo> <mi>t</mi> </mrow> </msub> <mo>)</mo> </mrow> <mo>+</mo> <msubsup> <mi>&amp;sigma;</mi> <mi>n</mi> <mn>2</mn> </msubsup> <msub> <mi>I</mi> <mi>n</mi> </msub> <mo>&amp;rsqb;</mo> </mrow> <mrow> <mo>-</mo> <mn>1</mn> </mrow> </msup> <msub> <mi>h</mi> <mrow> <mi>m</mi> <mo>,</mo> <mi>t</mi> </mrow> </msub> </mrow>
<mrow> <mi>cov</mi> <mrow> <mo>(</mo> <msubsup> <mi>h</mi> <mrow> <mi>m</mi> <mo>,</mo> <mi>t</mi> </mrow> <mo>*</mo> </msubsup> <mo>)</mo> </mrow> <mo>=</mo> <mi>k</mi> <mrow> <mo>(</mo> <msubsup> <mi>x</mi> <mrow> <mi>m</mi> <mo>,</mo> <mi>t</mi> </mrow> <mo>*</mo> </msubsup> <mo>,</mo> <msubsup> <mi>x</mi> <mrow> <mi>m</mi> <mo>,</mo> <mi>t</mi> </mrow> <mo>*</mo> </msubsup> <mo>)</mo> </mrow> <mo>-</mo> <mi>K</mi> <mrow> <mo>(</mo> <msubsup> <mi>x</mi> <mrow> <mi>m</mi> <mo>,</mo> <mi>t</mi> </mrow> <mo>*</mo> </msubsup> <mo>,</mo> <msub> <mi>X</mi> <mrow> <mi>m</mi> <mo>,</mo> <mi>t</mi> </mrow> </msub> <mo>)</mo> </mrow> <msup> <mrow> <mo>&amp;lsqb;</mo> <mi>K</mi> <mrow> <mo>(</mo> <msub> <mi>X</mi> <mrow> <mi>m</mi> <mo>,</mo> <mi>t</mi> </mrow> </msub> <mo>,</mo> <msub> <mi>X</mi> <mrow> <mi>m</mi> <mo>,</mo> <mi>t</mi> </mrow> </msub> <mo>)</mo> </mrow> <mo>+</mo> <msubsup> <mi>&amp;sigma;</mi> <mi>n</mi> <mn>2</mn> </msubsup> <msub> <mi>I</mi> <mi>n</mi> </msub> <mo>&amp;rsqb;</mo> </mrow> <mrow> <mo>-</mo> <mn>1</mn> </mrow> </msup> <mi>K</mi> <mrow> <mo>(</mo> <msub> <mi>X</mi> <mrow> <mi>m</mi> <mo>,</mo> <mi>t</mi> </mrow> </msub> <mo>,</mo> <msubsup> <mi>x</mi> <mrow> <mi>m</mi> <mo>,</mo> <mi>t</mi> </mrow> <mo>*</mo> </msubsup> <mo>)</mo> </mrow> </mrow>
式中：
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)中的神经网络协方差函数表达式；
<mrow> <mi>K</mi> <mrow> <mo>(</mo> <msub> <mi>X</mi> <mrow> <mi>m</mi> <mo>,</mo> <mi>t</mi> </mrow> </msub> <mo>,</mo> <msubsup> <mi>x</mi> <mrow> <mi>m</mi> <mo>,</mo> <mi>t</mi> </mrow> <mo>*</mo> </msubsup> <mo>)</mo> </mrow> <mo>=</mo> <msup> <mrow> <mo>&amp;lsqb;</mo> <mi>K</mi> <mrow> <mo>(</mo> <msubsup> <mi>x</mi> <mrow> <mi>m</mi> <mo>,</mo> <mi>t</mi> </mrow> <mo>*</mo> </msubsup> <mo>,</mo> <msub> <mi>X</mi> <mrow> <mi>m</mi> <mo>,</mo> <mi>t</mi> </mrow> </msub> <mo>)</mo> </mrow> <mo>&amp;rsqb;</mo> </mrow> <mi>T</mi> </msup> <mo>;</mo> </mrow>
3.5)非线性离散量测方程为
<mrow> <msub> <mi>z</mi> <mi>t</mi> </msub> <mo>=</mo> <mover> <mi>f</mi> <mo>~</mo> </mover> <mrow> <mo>(</mo> <msub> <mi>&amp;xi;</mi> <mi>t</mi> </msub> <mo>;</mo> <msub> <mover> <mi>x</mi> <mo>~</mo> </mover> <mi>t</mi> </msub> <mo>)</mo> </mrow> <mo>+</mo> <msub> <mi>&amp;eta;</mi> <mi>t</mi> </msub> </mrow>
式中：
zt为t时刻潜器测量的量测量，具体为单波束测深仪测量的潜器距离海底的高度和压力传感器测量的潜器距离海面的深度之和；海图模型表示潜器在海图上的水深值，和分别为惯性导航系统计算的t时刻潜器的东向和北向位置分量，f(·)采用高斯过程回归建立；ηt为t时刻的量测噪声；
所述步骤4)中，构建非线性组合导航系统的粒子滤波算法，具体过程为：
4.1)由步骤2)和步骤3)组成的非线性组合导航系统模型为
<mfenced open = "{" close = ""> <mtable> <mtr> <mtd> <mrow> <msub> <mi>&amp;xi;</mi> <mrow> <mi>t</mi> <mo>+</mo> <mn>1</mn> </mrow> </msub> <mo>=</mo> <msub> <mi>&amp;xi;</mi> <mi>t</mi> </msub> <mo>+</mo> <msub> <mi>v</mi> <mi>t</mi> </msub> </mrow> </mtd> </mtr> <mtr> <mtd> <mrow> <msub> <mi>z</mi> <mi>t</mi> </msub> <mo>=</mo> <mover> <mi>f</mi> <mo>~</mo> </mover> <mrow> <mo>(</mo> <msub> <mi>&amp;xi;</mi> <mi>t</mi> </msub> <mo>;</mo> <msub> <mover> <mi>x</mi> <mo>~</mo> </mover> <mi>t</mi> </msub> <mo>)</mo> </mrow> <mo>+</mo> <msub> <mi>&amp;eta;</mi> <mi>t</mi> </msub> </mrow> </mtd> </mtr> </mtable> </mfenced>
式中：
ξ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时刻的权值，为服从均值为方差为的高斯分布；
并且归一化：则得到t时刻状态量的最小均方估计为
4.4)重采样：得到新的粒子集合
4.5)预测：利用状态方程式(3)预测
4.6)时刻t:＝t+1，转到4.3)。
CN201510758207.XA 2015-11-09 2015-11-09 一种低成本水下潜器地形辅助惯性组合导航定位方法 Active CN105424036B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN201510758207.XA CN105424036B (zh) 2015-11-09 2015-11-09 一种低成本水下潜器地形辅助惯性组合导航定位方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN201510758207.XA CN105424036B (zh) 2015-11-09 2015-11-09 一种低成本水下潜器地形辅助惯性组合导航定位方法

Publications (2)

Publication Number Publication Date
CN105424036A CN105424036A (zh) 2016-03-23
CN105424036B true CN105424036B (zh) 2018-02-13

Family

ID=55502410

Family Applications (1)

Application Number Title Priority Date Filing Date
CN201510758207.XA Active CN105424036B (zh) 2015-11-09 2015-11-09 一种低成本水下潜器地形辅助惯性组合导航定位方法

Country Status (1)

Country Link
CN (1) CN105424036B (zh)

Families Citing this family (15)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN106017460B (zh) * 2016-05-20 2018-08-14 东南大学 一种地形辅助惯导紧组合的水下潜器导航定位方法
CN106052688B (zh) * 2016-08-08 2019-01-08 西安电子科技大学 基于地形轮廓匹配的惯性导航系统速度累积误差修正方法
CN106125004A (zh) * 2016-08-29 2016-11-16 哈尔滨理工大学 基于神经网络核函数gpr的锂电池健康状态预测方法
CN106405427A (zh) * 2016-08-29 2017-02-15 哈尔滨理工大学 基于神经网络与Maternard核函数GPR的锂电池健康状态预测方法
CN108171316B (zh) * 2017-12-27 2020-06-30 东南大学 一种面向不动产测量的改进型惯性定位方法
CN108562287A (zh) * 2018-01-08 2018-09-21 哈尔滨工程大学 一种基于自适应采样粒子滤波的水下地形辅助导航方法
CN108663051A (zh) * 2018-04-28 2018-10-16 南京信息工程大学 一种水下无源组合导航系统建模及信息融合方法
CN109298436A (zh) * 2018-05-15 2019-02-01 重庆邮电大学 一种多信息融合的室内定位与导航方法
CN109211276B (zh) * 2018-10-30 2022-06-03 东南大学 基于gpr与改进的srckf的sins初始对准方法
CN109813316B (zh) * 2019-01-14 2022-07-29 东南大学 一种基于地形辅助的水下载体紧组合导航方法
CN110471096A (zh) * 2019-09-11 2019-11-19 哈尔滨工程大学 一种分布式海底飞行节点群体定位方法
CN110763234B (zh) * 2019-10-15 2022-10-28 哈尔滨工程大学 一种水下机器人海底地形匹配导航路径规划方法
CN111220146B (zh) * 2019-12-10 2022-10-14 哈尔滨工程大学 一种基于高斯过程回归学习的水下地形匹配定位方法
CN112097763B (zh) * 2020-08-28 2022-07-05 西北工业大学 一种基于mems imu/磁力计/dvl组合的水下运载体组合导航方法
CN116222584B (zh) * 2023-05-10 2023-07-25 北京白水科技有限公司 群组导航定位中的分组信息的确定方法、装置及设备

Family Cites Families (8)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN102818567A (zh) * 2012-08-08 2012-12-12 浙江大学 集合卡尔曼滤波－粒子滤波相结合的auv组合导航方法
CN103542851B (zh) * 2013-11-04 2016-03-23 东南大学 一种基于水下地形高程数据库的水下航行器辅助导航定位方法
WO2015073424A1 (en) * 2013-11-12 2015-05-21 Conocophillips Company Ice alert system and method of evacuating an arctic floating platform from a hazardous ice condition
CN104048675B (zh) * 2014-06-26 2017-01-11 东南大学 一种基于高斯过程回归的组合导航系统故障诊断方法
CN104075715B (zh) * 2014-07-07 2017-09-01 东南大学 一种结合地形和环境特征的水下导航定位方法
CN104215259B (zh) * 2014-08-22 2018-04-24 哈尔滨工程大学 一种基于地磁模量梯度和粒子滤波的惯导误差校正方法
CN104316045B (zh) * 2014-11-06 2017-06-16 东南大学 一种基于sins/lbl的auv水下交互辅助定位系统及定位方法
CN104330084B (zh) * 2014-11-13 2017-06-16 东南大学 一种水下航行器用神经网络辅助组合导航方法

Also Published As

Publication number Publication date
CN105424036A (zh) 2016-03-23

Similar Documents

Publication Publication Date Title
CN105424036B (zh) 一种低成本水下潜器地形辅助惯性组合导航定位方法
CN104075715B (zh) 一种结合地形和环境特征的水下导航定位方法
CN104655131B (zh) 基于istssrckf的惯性导航初始对准方法
CN104197927B (zh) 水下结构检测机器人实时导航系统及方法
CN103759742B (zh) 基于模糊自适应控制技术的捷联惯导非线性对准方法
CN103744098B (zh) 基于sins/dvl/gps的auv组合导航系统
CN108871351B (zh) 一种auv海底地形匹配的动态路径规划方法
CN109000642A (zh) 一种改进的强跟踪容积卡尔曼滤波组合导航方法
CN109443379A (zh) 一种深海潜航器的sins/dvl水下抗晃动对准方法
CN111596333B (zh) 一种水下定位导航方法及系统
CN105806363B (zh) 基于srqkf的sins/dvl水下大失准角对准方法
CN106885569A (zh) 一种强机动条件下的弹载深组合arckf滤波方法
CN108318038A (zh) 一种四元数高斯粒子滤波移动机器人姿态解算方法
CN108562287A (zh) 一种基于自适应采样粒子滤波的水下地形辅助导航方法
CN106017460B (zh) 一种地形辅助惯导紧组合的水下潜器导航定位方法
CN106840211A (zh) 一种基于kf和stupf组合滤波的sins大方位失准角初始对准方法
CN107063245A (zh) 一种基于5阶ssrckf的sins/dvl组合导航滤波方法
CN104787260B (zh) 一种基于融合滤波器的水翼双体船纵向姿态估计方法
CN107576327A (zh) 基于可观测度分析的可变结构综合导航系统设计方法
CN107525502A (zh) 一种提高水下航行器惯性地形匹配导航平均精度的方法
CN104482942B (zh) 一种基于惯性系的最优两位置对准方法
CN108508463A (zh) 基于Fourier-Hermite正交多项式扩展椭球集员滤波方法
CN109813316A (zh) 一种基于地形辅助的水下载体紧组合导航方法
CN103616026B (zh) 一种基于h∞滤波的auv操纵模型辅助捷联惯导组合导航方法
CN106352880B (zh) 一种基于非线性网络的惯性导航算法架构的确定方法

Legal Events

Date Code Title Description
C06 Publication
PB01 Publication
C10 Entry into substantive examination
SE01 Entry into force of request for substantive examination
GR01 Patent grant
GR01 Patent grant
CP02 Change in the address of a patent holder

Address after: 210093 Nanjing University Science Park, 22 Hankou Road, Gulou District, Nanjing City, Jiangsu Province

Patentee after: Southeast University

Address before: 211189 No. 2 Southeast University Road, Jiangning District, Nanjing, Jiangsu

Patentee before: Southeast University

CP02 Change in the address of a patent holder