CN109813316A - 一种基于地形辅助的水下载体紧组合导航方法 - Google Patents
一种基于地形辅助的水下载体紧组合导航方法 Download PDFInfo
- Publication number
- CN109813316A CN109813316A CN201910032320.8A CN201910032320A CN109813316A CN 109813316 A CN109813316 A CN 109813316A CN 201910032320 A CN201910032320 A CN 201910032320A CN 109813316 A CN109813316 A CN 109813316A
- Authority
- CN
- China
- Prior art keywords
- moment
- underwater
- gaussian
- carrier
- underwater carrier
- 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.)
- Granted
Links
Landscapes
- Navigation (AREA)
- Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)
Abstract
本发明公开一种基于地形辅助的水下载体紧组合导航方法,主要步骤包括:根据水下载体的运动状态选取状态量,建立水下载体组合导航***模型,初始化水下地形组合导航***;初始化高斯和粒子滤波器,搭建高斯过程‑高斯和粒子滤波框架并进行水下载体紧组合导航。采用高斯过程‑高斯和粒子滤波紧组合导航框架,可充分利用传感器信息,方法可移植性强,适用于非线性非高斯的实际***,并且具有免重采样的优点,相较于传统的粒子滤波组合导航,组合导航精度更高,有利于推进基于地形辅助的水下载体组合导航***的实际应用。
Description
技术领域
本发明涉及一种水下载体组合导航方法,具体涉及一种基于地形辅助的水下载体紧组合导航方法。
背景技术
水下载体在水中工作时,无法实时接收全球卫星导航***信号,基本惯性导航***的误差随时间积累,不能满足水下长航时的定位要求。基于地形辅助的组合导航是一种通过地球物理量匹配修正惯性导航误差,从而提高导航精度的方式。
目前,基于地形辅助的水下载体组合导航主要存在两大关键技术难点:第一,水下载体组合导航***依赖水下地形数据库的建立,然而对于水下导航实际应用来说,高精度水下地形数据库较难获取,采集和存储高精度水下地形数据库是难点,目前普遍采用的多为双线性插值法构建水下地形数据库,而这种方法所得到的水下地形数据库不能完全反应水下地形特征。第二,基于地形辅助的水下载体组合导航***实际为非线性非高斯***,目前的多数研究是将水下载体组合导航***近似为线性化高斯***或非线性化高斯***,在此基础上进行研究的。上述两点限制了基于地形辅助的水下载体组合导航***从理论研究向实际应用发展。
基于上述背景技术,本发明提出一种基于地形辅助的水下载体紧组合导航方法,该方法提出一种高斯过程-高斯和粒子滤波紧组合导航框架,这种导航方法可有效提升水下载体组合导航精度。
发明内容
技术问题:本发明的目的是为了采用低精度水下地形数据库时,有效提升水下载体组合导航***的导航精度,提供一种基于地形辅助的水下载体紧组合导航方法。
技术方案:本发明的一种基于地形辅助的水下载体紧组合导航方法,包括以下步骤:
步骤1)根据水下载体的运动状态选取***状态量,***状态量具体为:
式中:ξ、η分别为水下载体所处位置的经度、纬度;为水下载体距海平面的下潜深度;H、P、R分别为水下载体的航向角、俯仰角、横滚角;εx、εy、εz分别为陀螺仪在载体坐标系x轴、y轴、z轴的角速率零偏;[·]T表示矩阵转置;
步骤2)建立水下载体组合导航***模型,并初始化组合导航***;其中,水下载体组合导航***模型为:
式中:xk+1为k+1时刻的状态,为k时刻的状态估计值,ek为过程噪声,Δxk为从k时刻到k+1时刻得状态转移矩阵,且
式中,分别为k时刻水下载体的航向角、俯仰角、横滚角估计值;为从载体坐标系到导航坐标系变换的姿态矩阵;vk为k时刻的载体速度;Δt为高度值采样间隔;分别为水下载体在k时刻沿载体坐标系x轴、y轴、z轴三个方向的角速率计算值,且 其中,ωx,k、ωy,k、ωz,k分别为k时刻陀螺沿载体坐标系x轴、y轴、z轴三个方向的输出值;为xk位置处的水深值,且h(·)为水下地形模型;为k时刻压力传感器测的的水下载体距水平面的下潜深度;prek为k时刻便携式测深仪测得的水下载体距离海底的高度值;ηk为量测噪声;zzk为k时刻的观测量;
步骤3)初始化G个高斯和项为k时刻第i个高斯和项权值,为xk服从均值为方差为的高斯分布,其中,k=0时,和根据先验分布得到,在k时刻时,每个高斯和项中抽样M个粒子,第i个高斯和项中的第j个粒子记为粒子以粒子水平位置为中心,选取水下地形数据库中中τ个节点,以τ个节点的水平位置向量及对应的地形数据作为粒子处地形模型的训练数据;搭建高斯过程-高斯和粒子滤波框架进行水下载体紧组合导航;
进一步,搭建高斯过程-高斯和粒子滤波框架并进行水下载体紧组合导航,具体步骤为:
3.1)当k时刻时,建立基于高斯过程的水下地形模型:选取协方差函数为
式中,pou和pov为训练样本水平位置向量的任意元素;kin(·)是协方差函数;σh是地形数据标准差;σv是样本噪声标准差;lhori是地形模型水平尺度;lverti是地形模型垂直尺度;Θ={σh,σv,lhori,lverti}是协方差函数的超参数;当u=v时,δuv=1;当u≠v时,δuv=0;
训练样本的条件概率对数似然函数为:
式中,K(po,po)={kin(pou,pov)|u,v=1,2,...,τ}为τ×τ阶协方差矩阵;Iτ为τ维单位矩阵;p(·)为概率;
超参数估计值为:
3.2)在k时刻时,粒子的水平位置向量对应的水深值可计算得
式中,为k时刻的水平位置向量和之间的1×τ阶协方差矩阵;
3.3)在k时刻时,计算重要性函数根据高斯和项的重要性函数p(xk|zz0:k)得到抽样样本点集的权重i=1,2,...,G,j=1,2,...,M;
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为过程噪声,Δxk为从k时刻到k+1时刻得状态转移矩阵,且
式中,分别为k时刻水下载体的航向角、俯仰角、横滚角估计值;为从载体坐标系到导航坐标系变换的姿态矩阵;vk为k时刻的载体速度;Δt为高度值采样间隔;分别为水下载体在k时刻沿载体坐标系x轴、y轴、z轴三个方向的角速率计算值,且 ,其中,ωx,k、ωy,k、ωz,k分别为k时刻陀螺沿载体坐标系x轴、y轴、z轴三个方向的输出值;为xk位置处的水深值,且h(·)为水下地形模型;为k时刻压力传感器测的的水下载体距水平面的下潜深度;prek为k时刻便携式测深仪测得的水下载体距离海底的高度值;ηk为量测噪声;zzk为k时刻的观测量;
步骤3)初始化G个高斯和项为k时刻第i个高斯和项权值,为xk服从均值为方差为的高斯分布,其中,k=0时,和根据先验分布得到,在k时刻时,每个高斯和项中抽样M个粒子,第i个高斯和项中的第j个粒子记为粒子以粒子水平位置为中心,选取水下地形数据库中中τ个节点,以τ个节点的水平位置向量及对应的地形数据作为粒子处地形模型的训练数据;搭建高斯过程-高斯和粒子滤波框架进行水下载体紧组合导航;
进一步,搭建高斯过程-高斯和粒子滤波框架并进行水下载体紧组合导航,具体步骤为:
3.1)当k时刻时,建立基于高斯过程的水下地形模型:选取协方差函数为
式中,pou和pov为训练样本水平位置向量的任意元素;kin(·)是协方差函数;σh是地形数据标准差;σv是样本噪声标准差;lhori是地形模型水平尺度;lverti是地形模型垂直尺度;Θ={σh,σv,lhori,lverti}是协方差函数的超参数;当u=v时,δuv=1;当u≠v时,δuv=0;
训练样本的条件概率对数似然函数为:
式中,K(po,po)={kin(pou,pov)|u,v=1,2,...,τ}为τ×τ阶协方差矩阵;Iτ为τ维单位矩阵;p(·)为概率;
超参数估计值为:
3.2)在k时刻时,粒子的水平位置向量对应的水深值可计算得
式中,为k时刻的水平位置向量和之间的1×τ阶协方差矩阵;
3.3)在k时刻时,计算重要性函数根据高斯和项的重要性函数p(xk|zz0:k)得到抽样样本点集的权重i=1,2,...,G,j=1,2,...,M;
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)建立水下载体组合导航***模型,并初始化组合导航***;其中,水下载体组合导航***模型为:
式中:xk+1为k+1时刻的状态,为k时刻的状态估计值,ek为过程噪声,为从k时刻到k+1时刻得状态转移矩阵,且
式中,分别为k时刻水下载体的航向角、俯仰角、横滚角估计值;为从载体坐标系到导航坐标系变换的姿态矩阵;vk为k时刻的载体速度;Δt为高度值采样间隔;分别为水下载体在k时刻沿载体坐标系x轴、y轴、z轴三个方向的角速率计算值,且 其中,ωx,k、ωy,k、ωz,k分别为k时刻陀螺沿载体坐标系x轴、y轴、z轴三个方向的输出值;为xk位置处的水深值,且h(·)为水下地形模型;为k时刻压力传感器测的的水下载体距水平面的下潜深度;prek为k时刻便携式测深仪测得的水下载体距离海底的高度值;ηk为量测噪声;zzk为k时刻的观测量;
步骤3)初始化G个高斯和项i=1,2,...,G,为k时刻第i个高斯和项权值,为xk服从均值为方差为的高斯分布,其中,k=0时,和根据先验分布得到,在k时刻时,每个高斯和项中抽样M个粒子,第i个高斯和项中的第j个粒子记为粒子i=1,2,...,G;j=1,2,...,M,以粒子水平位置为中心,选取水下地形数据库中中τ个节点,以τ个节点的水平位置向量及对应的地形数据作为粒子处地形模型的训练数据;搭建高斯过程-高斯和粒子滤波框架进行水下载体紧组合导航。
2.根据权利要求1所述的一种基于地形辅助的水下载体组合导航方法,其特征在于步骤3),所述搭建高斯过程-高斯和粒子滤波框架并进行水下载体紧组合导航,具体步骤为:
3.1)当k时刻时,建立基于高斯过程的水下地形模型:选取协方差函数为
式中,pou和pov为训练样本水平位置向量的任意元素;kin(·)是协方差函数;σh是地形数据标准差;σv是样本噪声标准差;lhori是地形模型水平尺度;lverti是地形模型垂直尺度;Θ={σh,σv,lhori,lverti}是协方差函数的超参数;当u=v时,δuv=1;当u≠v时,δuv=0;
训练样本的条件概率对数似然函数为:
式中,K(po,po)={kin(pou,pov)|u,v=1,2,...,τ}为τ×τ阶协方差矩阵;Iτ为τ维单位矩阵;p(·)为概率;
超参数估计值为:
3.2)在k时刻时,粒子的水平位置向量对应的水深值可计算得
式中,为k时刻的水平位置向量和之间的1×τ阶协方差矩阵;
3.3)在k时刻时,计算重要性函数根据高斯和项的重要性函数p(xk|zz0:k)得到抽样样本点集的权重i=1,2,...,G,j=1,2,...,M;
3.4)计算均值方差更新高斯和项权值并进行高斯和项权值归一化
3.5)计算输出均值
计算输出方差
3.6)通过式(2)进行从时刻k到时刻k+1的状态转移,继续重复步骤2.1)-2.5),直至航行结束,停止运算。
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN201910032320.8A CN109813316B (zh) | 2019-01-14 | 2019-01-14 | 一种基于地形辅助的水下载体紧组合导航方法 |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN201910032320.8A CN109813316B (zh) | 2019-01-14 | 2019-01-14 | 一种基于地形辅助的水下载体紧组合导航方法 |
Publications (2)
Publication Number | Publication Date |
---|---|
CN109813316A true CN109813316A (zh) | 2019-05-28 |
CN109813316B CN109813316B (zh) | 2022-07-29 |
Family
ID=66603741
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
CN201910032320.8A Active CN109813316B (zh) | 2019-01-14 | 2019-01-14 | 一种基于地形辅助的水下载体紧组合导航方法 |
Country Status (1)
Country | Link |
---|---|
CN (1) | CN109813316B (zh) |
Cited By (3)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN111220146A (zh) * | 2019-12-10 | 2020-06-02 | 哈尔滨工程大学 | 一种基于高斯过程回归学习的水下地形匹配定位方法 |
CN111238483A (zh) * | 2020-02-12 | 2020-06-05 | 上海海事大学 | 基于sir粒子滤波方法的低功耗地形辅助导航***及方法 |
CN111458050A (zh) * | 2020-04-15 | 2020-07-28 | 国家海洋技术中心 | 一种用于海气界面水边界层的温度剖面精细化测量传感器 |
Citations (8)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
FR2959810B1 (fr) * | 2010-05-05 | 2012-06-08 | Thales Sa | Procede de definition d'un systeme de navigation. |
CN102818567A (zh) * | 2012-08-08 | 2012-12-12 | 浙江大学 | 集合卡尔曼滤波-粒子滤波相结合的auv组合导航方法 |
CN105205313A (zh) * | 2015-09-07 | 2015-12-30 | 深圳大学 | 模糊高斯和粒子滤波方法、装置及目标跟踪方法、装置 |
CN105424036A (zh) * | 2015-11-09 | 2016-03-23 | 东南大学 | 一种低成本水下潜器地形辅助惯性组合导航定位方法 |
CN106017460A (zh) * | 2016-05-20 | 2016-10-12 | 东南大学 | 一种地形辅助惯导紧组合的水下潜器导航定位方法 |
CN106683122A (zh) * | 2016-12-16 | 2017-05-17 | 华南理工大学 | 一种基于高斯混合模型和变分贝叶斯的粒子滤波方法 |
CN107314768A (zh) * | 2017-07-06 | 2017-11-03 | 上海海洋大学 | 水下地形匹配辅助惯性导航定位方法及其定位*** |
WO2018119912A1 (zh) * | 2016-12-29 | 2018-07-05 | 深圳大学 | 基于并行模糊高斯和粒子滤波的目标跟踪方法及装置 |
-
2019
- 2019-01-14 CN CN201910032320.8A patent/CN109813316B/zh active Active
Patent Citations (8)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
FR2959810B1 (fr) * | 2010-05-05 | 2012-06-08 | Thales Sa | Procede de definition d'un systeme de navigation. |
CN102818567A (zh) * | 2012-08-08 | 2012-12-12 | 浙江大学 | 集合卡尔曼滤波-粒子滤波相结合的auv组合导航方法 |
CN105205313A (zh) * | 2015-09-07 | 2015-12-30 | 深圳大学 | 模糊高斯和粒子滤波方法、装置及目标跟踪方法、装置 |
CN105424036A (zh) * | 2015-11-09 | 2016-03-23 | 东南大学 | 一种低成本水下潜器地形辅助惯性组合导航定位方法 |
CN106017460A (zh) * | 2016-05-20 | 2016-10-12 | 东南大学 | 一种地形辅助惯导紧组合的水下潜器导航定位方法 |
CN106683122A (zh) * | 2016-12-16 | 2017-05-17 | 华南理工大学 | 一种基于高斯混合模型和变分贝叶斯的粒子滤波方法 |
WO2018119912A1 (zh) * | 2016-12-29 | 2018-07-05 | 深圳大学 | 基于并行模糊高斯和粒子滤波的目标跟踪方法及装置 |
CN107314768A (zh) * | 2017-07-06 | 2017-11-03 | 上海海洋大学 | 水下地形匹配辅助惯性导航定位方法及其定位*** |
Non-Patent Citations (2)
Title |
---|
HONGMEI CHEN等: "Stochastic stability of Gaussian filters for nonlinear integrated navigation system with intermittent measurements", 《2016 IEEE TRANSPORTATION ELECTRIFICATION CONFERENCE AND EXPO, ASIA - PACIFIC (ITEC)》 * |
孙立国等: "基于高斯混合无迹粒子滤波的地形辅助导航算法", 《中国惯性技术学报》 * |
Cited By (5)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN111220146A (zh) * | 2019-12-10 | 2020-06-02 | 哈尔滨工程大学 | 一种基于高斯过程回归学习的水下地形匹配定位方法 |
CN111220146B (zh) * | 2019-12-10 | 2022-10-14 | 哈尔滨工程大学 | 一种基于高斯过程回归学习的水下地形匹配定位方法 |
CN111238483A (zh) * | 2020-02-12 | 2020-06-05 | 上海海事大学 | 基于sir粒子滤波方法的低功耗地形辅助导航***及方法 |
CN111458050A (zh) * | 2020-04-15 | 2020-07-28 | 国家海洋技术中心 | 一种用于海气界面水边界层的温度剖面精细化测量传感器 |
CN111458050B (zh) * | 2020-04-15 | 2021-11-09 | 国家海洋技术中心 | 一种用于海气界面水边界层的温度剖面精细化测量传感器 |
Also Published As
Publication number | Publication date |
---|---|
CN109813316B (zh) | 2022-07-29 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
CN109443379B (zh) | 一种深海潜航器的sins/dvl水下抗晃动对准方法 | |
CN104316045B (zh) | 一种基于sins/lbl的auv水下交互辅助定位***及定位方法 | |
CN104075715B (zh) | 一种结合地形和环境特征的水下导航定位方法 | |
CN109141436A (zh) | 改进的无迹卡尔曼滤波算法在水下组合导航中的应用方法 | |
CN109324330A (zh) | 基于混合无导数扩展卡尔曼滤波的usbl/sins紧组合导航定位方法 | |
CN105424036B (zh) | 一种低成本水下潜器地形辅助惯性组合导航定位方法 | |
CN110006433A (zh) | 海底油气管检测机器人的组合导航定位***及方法 | |
CN104880191B (zh) | 一种基于太阳矢量的偏振辅助导航方法 | |
CN107314768A (zh) | 水下地形匹配辅助惯性导航定位方法及其定位*** | |
CN106643709B (zh) | 一种海上运载体的组合导航方法及装置 | |
CN109737956A (zh) | 一种基于双应答器的sins/usbl相位差紧组合导航定位方法 | |
CN107797125B (zh) | 一种减小深海探测型auv导航定位误差的方法 | |
CN111596333B (zh) | 一种水下定位导航方法及*** | |
CN111829512B (zh) | 一种基于多传感器数据融合的auv导航定位方法及*** | |
CN103744098A (zh) | 基于sins/dvl/gps的auv组合导航*** | |
CN101949703A (zh) | 一种捷联惯性/卫星组合导航滤波方法 | |
CN110514203A (zh) | 一种基于isr-ukf的水下组合导航方法 | |
CN103017755A (zh) | 一种水下导航姿态测量方法 | |
CN106017460B (zh) | 一种地形辅助惯导紧组合的水下潜器导航定位方法 | |
CN106840211A (zh) | 一种基于kf和stupf组合滤波的sins大方位失准角初始对准方法 | |
CN109813316A (zh) | 一种基于地形辅助的水下载体紧组合导航方法 | |
CN110567454A (zh) | 一种复杂环境下sins/dvl紧组合导航方法 | |
CN107063245A (zh) | 一种基于5阶ssrckf的sins/dvl组合导航滤波方法 | |
CN109443356A (zh) | 一种含测量噪声的无人船位置与速度估计结构及设计方法 | |
CN104931994A (zh) | 一种基于软件接收机的分布式深组合导航方法及*** |
Legal Events
Date | Code | Title | Description |
---|---|---|---|
PB01 | Publication | ||
PB01 | Publication | ||
SE01 | Entry into force of request for substantive examination | ||
SE01 | Entry into force of request for substantive examination | ||
GR01 | Patent grant | ||
GR01 | Patent grant |