一种基于纯惯性测量的水下长航程导航定位方法及装置

1.本发明涉及导航系统技术领域，具体涉及一种基于纯惯性测量的水下长航程导航定位方法及装置。


背景技术：

2.惯性测量单元（imu）是惯性导航系统(ins)的重要组成部分，惯性导航系统是依靠陀螺仪和加速度计等敏感器件，在惯性坐标系下记录载体的运动，借助基本牛顿力学知识提供全天候导航信息的独立自主导航系统。
3.由于惯性导航系统具备连续输出载体位置速度和姿态信息、短时导航精度高、完全独立自主等突出优点，在车、船、飞机等导航等领域应用广泛。然而，对于长航时、长航程导航任务需求，惯性测量系统的缺陷也十分明显。从惯性导航系统的基本可知，通过对加速度的一次积分获得载体速度量，二次积分获得位置相关量。二次积分过程势必将测量数据中的噪声量不断累积放大，最终导致解算出的导航信息发散，直至无法使用。
4.目前常用的解决手段是，将惯性导航系统与其他定位技术（全球定位系统、水声定位）结合构成组合导航。例如，使用卫星定位修正惯性导航，可避免其误差随时间的迅速积累。然而，在接收卫星信号的同时也暴露了航行器的当前位置，使航行器丧失隐蔽性。


技术实现要素：

5.本发明的主要目的是提供一种基于纯惯性测量的水下长航程导航定位方法及装置，以提高长航时、长航程导航精度。该方法利用惯性测量单元的纯惯性测量数据，经数据预处理后，提取数据中隐含的载体运动特征，关联载体导航参数，通过神经网络模型训练，来构建惯性数据特征与导航参数的相关量之间的映射模型，进而可将数据输入到训练得到的惯性数据特征与导航参数的相关量之间的映射模型中得到神经网络输出值，即神经网络模型预测得到航向变化量和位移变化量，即可推算载体的导航参数，进而获得载体位置。
6.基于上述目的，本发明提供一种基于纯惯性测量的水下长航程导航定位方法，包括以下步骤：s1.惯性测量数据预处理，对惯性测量单元的惯性测量数据进行数据预处理；s2. 运动特征提取, 提取惯性测量数据中隐含的载体运动特征，关联载体导航参数；s3. 神经网络模型训练，通过神经网络模型训练，来构建惯性数据特征与导航参数的相关量之间的映射模型；s4. 神经网络模型预测，将惯性测量数据输入到训练得到的惯性数据特征与导航参数的相关量之间的映射模型中得到神经网络输出值，即可推算载体导航参数。
7.进一步，步骤s1中，数据预处理包括通过运动加速度误差模型从加速度计测得的表观加速度中分离重力分量和地球自转分量，所述运动加速度误差模型的方程为：
其中，为载体在导航坐标系下的运动加速度，为载体坐标系下加速度计测得的表观加速度，是和姿态有关的载体坐标系与导航坐标系之间的旋转矩阵，为导航坐标系下重力加速度（其余部分不加坐标系说明均是指在导航坐标系中），为载体绕地球转动以及地球自转产生的干扰加速度，表示导航坐标系相对于惯性坐标系的旋转，表示惯性系统在地球表面附近因地球表面弯曲引起的导航坐标系旋转，是与载体的速度和深度有关量；随着载体航行位置不同，重力加速度计算公式为：其中，是万有引力系数，是地球质量， 代表纬度，代表载体距地心的距离，计算公式为：式中，a代表地球长半轴，b代表地球短半轴；由于载体并非一直保持以水平姿态航行，造成重力加速度在载体坐标系x轴、y轴上有重力分量；对重力加速度在载体x轴、y轴的重力分量进行计算公式为：、y轴的重力分量进行计算公式为：为载体纵倾角，为横滚角，为重力加速度在载体轴上的分量，为重力加速度在载体轴上的分量；地球自转对运动加速度的影响计算公式为：地球自转对运动加速度的影响计算公式为：其中，为地球自转在纬度处的投影，是载体航行深度，是东向速度，是北向速度。
8.进一步，步骤s1中，数据预处理还包括数据降噪，采用零相位数字滤波器对加速度数据和角速度数据做降噪处理，并对加速度数据和角速度数据进行归一化处理；其中，加速度数据包括运动加速度、表观加速度和重力加速度，角速度数据包括航向角速度、俯仰角速度、横滚角速度。
9.进一步，步骤s2中，选取均值、最大值、最小值、标准差、频域均值、熵、四分位值、频域信号方差、运动强度、峰值、偏度、变异系数、和/或窗口数据之和作为载体导航参数的特征描述量；
利用gps数据计算出时间窗口内航向变化量和位移变化量作为监督学习的训练标签。
10.进一步，步骤s3中，通过长短期记忆网络来构建惯性数据特征与导航参数的相关量之间的映射模型；定义网络的损失函数为：定义网络的损失函数为：为t时刻网络的输出预测值，为真实标签值，mean()为对括号中的变量取均值；设置神经网络输入节点数，输出节点数，学习率lr，隐藏层数h_num，每次训练样本数batch_size，时间步长timestep，迭代次数iter，隐藏层中神经元的数量；在神经网络参数设置完成后，开始训练，若损失函数收敛到预设的阈值以内，则代表训练完成，终止对神经网络模型的训练。
11.进一步，步骤s4中，将采集到的数据中的p%作为训练集，q%作为测试集，其中p + q = 100，将测试集中的数据输入到训练得到的惯性数据特征与导航参数的相关量之间的映射模型中得到神经网络输出值，神经网络输出值包括神经网络模型预测得到航向变化量和位移变化量；并利用下面公式解算出载体位置；并利用下面公式解算出载体位置；式中代表东向位移，代表北向位移，代表航向。
12.进一步，所述载体为自主式水下航行器。
13.另一方面，本发明提供一种基于纯惯性测量的水下长航程导航定位装置，所述装置用于实现所述基于纯惯性测量的水下长航程导航定位方法，所述装置包括：惯性测量数据输入接口、惯性测量数据预处理模块、运动特征提取模块、历史特征存储模块、神经网络模型训练模块、模型更新判决模块、神经网络模型预测模块和输出或显示接口。
14.进一步，所述惯性测量数据输入接口用于实时接收和解析惯性测量单元发送的惯性测量数据；惯性测量数据预处理模块用于实现步骤s1中惯性测量数据预处理；运动特征提取模块用于实现步骤s2中运动特征提取的算法；历史特征存储模块用于在装置与惯性测量单元配套安装后，持续记录累积惯性测量单元的惯性测量数据，以逐渐增加模型训练的数据来源；神经网络模型训练模块用于实现步骤s3中神经网络模型训练的算法；模型更新判决模块用于神经网络模型训练模块训练得到新的神经网络模型后，判决是否提供给神经网络模型预测模块进行更新；神经网络模型预测模块用于实现步骤s4中神经网络模型预测的算法；输出或显示接口，用于将解算得到的载体导航参数以指定格式传输给其他装置，或发送至显示装置用于目视查看；所述导航参数包括载体位置、航向以及载体位置变化量和航向变化量。
15.进一步，惯性测量数据预处理模块包括运动加速度误差模型子模块和数据降噪子模块。
16.本发明具有的有益效果：本发明从惯性测量单元原始记录数据与载体的运动特征之间的关系出发，得到载体在时间窗口内的航行状态变化量与惯性数据变化特征的非线性映射关系，通过对时间窗口内的惯性测量数据进行特征提取和对位置、航向变化量解算，引入神经模型网络构建特征量与变化量之间的映射关系，可实现从惯性数据特征的角度来估计载体的导航参数，进而避免了直接利用惯性测量数据进行传统导航解算带来的误差累积效应。
附图说明
17.图1 为本发明实施例基于纯惯性测量的水下长航程导航定位方法的流程框架示意图；图2为本发明实施例中运动加速度误差模型中重力分量侧视图；图3为本发明实施例中运动加速度误差模型中重力分量后视图；图4为本发明实施例中航向变化量测试结果示意图；图5为本发明实施例中位移变化量测试结果示意图；图6为本发明实施例中基于神经网络的惯性测量数据预测航迹与真实航迹示意图；图7为本发明实施例中基于纯惯性测量的水下长航程导航定位装置架构示意图。
具体实施方式
18.下面将结合附图，对本发明的技术方案进行清楚、完整地描述，显然，所描述的实施例是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。
19.在本发明的描述中，需要说明的是，术语“中心”、“上”、“下”、“左”、“右”、“竖直”、“水平”、“内”、“外”等指示的方位或位置关系为基于附图所示的方位或位置关系，仅是为了便于描述本发明和简化描述，而不是指示或暗示所指的装置或元件必须具有特定的方位、以特定的方位构造和操作，因此不能理解为对本发明的限制。此外，术语“第一”、“第二”、“第三”仅用于描述目的，而不能理解为指示或暗示相对重要性。
20.在本发明的描述中，需要说明的是，除非另有明确的规定和限定，术语“安装”、“相连”、“连接”应做广义理解，例如，可以是固定连接，也可以是可拆卸连接，或一体地连接；可以是机械连接，也可以是电连接；可以是直接相连，也可以通过中间媒介间接相连，可以是两个元件内部的连通。对于本领域的普通技术人员而言，可以根据具体情况理解上述术语在本发明中的具体含义。
21.以下结合附图对本发明的具体实施方式进行详细说明。应当理解的是，此处所描述的具体实施方式仅用于说明和解释本发明，并不用于限制本发明。
22.如图1-7所示，本发明一种基于纯惯性测量的水下长航程导航定位方法及装置的优选实施例，介绍如下：
一种基于纯惯性测量的水下长航程导航定位方法（通常航程大于100km称之为长航程）,如图1所示，该方法包括以下步骤：步骤s1、惯性测量数据预处理，对惯性测量单元（imu）的纯惯性测量数据进行数据预处理；惯性测量数据预处理模块包含两个子模块，即运动加速度误差模型子模块和数据降噪子模块。
23.运动加速度误差模型子模块是用于从加速度计测得的表观加速度中分离重力分量和地球自转分量。
24.运动加速度误差模型方程为：其中，为载体在导航坐标系下的运动加速度，为载体坐标系下加速度计测量到的表观加速度，是和姿态有关的载体坐标系与导航坐标系之间的旋转矩阵，为导航坐标系下重力加速度，为载体绕地球转动以及地球自转产生的干扰加速度，表示导航坐标系相对于惯性坐标系的旋转，表示惯性系统在地球表面附近因地球表面弯曲引起的导航坐标系旋转，是与载体的速度和深度有关量；其中，所述载体为自主式水下航行器（简称auv）。
25.可选的，随着载体航行位置不同，重力加速度计算公式为：其中，是万有引力系数，，是地球质量，，代表纬度，r代表载体距地心的距离，计算公式为：式中a代表地球长半轴a = 6378km，b代表地球短半轴b = 6357km。
26.由于auv并非一直保持以水平姿态航行，这就造成重力加速度在载体坐标系x轴、y轴上有重力分量，如图2所示。
27.进一步地，对重力加速度在载体x轴、y轴的重力分量进行计算公式为：进一步地，对重力加速度在载体x轴、y轴的重力分量进行计算公式为：为载体纵倾角，为横滚角，为重力加速度在载体轴上的分量，为重力加速度在载体轴上的分量。
28.可选的，地球自转对运动加速度的影响计算公式为：可选的，地球自转对运动加速度的影响计算公式为：
其中，为地球自转在纬度处的投影，是载体航行深度，是东向速度，是北向速度。
29.进一步地，数据降噪子模块主要实现滤波降噪和数据归一化功能。
30.优选的，数据降噪子模块包括零相位数字滤波器，采用零相位数字滤波器对加速度和角速度数据做降噪处理。
31.设置滤波器参数：“低通”，“滤波器阶数=5”，“halfpowerfrequency=0.055”，“designmethod=butter”。
32.进一步地，对加速度和角速度数据进行归一化处理，归一化计算公式为：式中，代表原始惯性测量数据，代表归一化后的加速度和角速度数据，min()代表对括号中的变量取最小值，max()代表对括号中的变量取最大值。
33.步骤s2、运动特征提取，提取数据中隐含的载体运动特征，关联载体导航参数；载体运动特征是对载体运动的宏观描述，例如匀速、匀速直线，加速、减速、匀加速、匀减速、回转等。
34.优选的，选择均值和窗口数据之和作为航向变化量的特征描述量，均值计算公式为：窗口数据之和计算公式为：式中，中i为窗口内航向角速度数据时间序列，i取值为1-n，n是窗口数据量大小，sum()代表对括号中的变量求和。
35.优选的，本发明中时间窗口取为5秒，惯性测量单元数据采样频率为100hz，故n=500。
36.进一步地，利用gps数据计算出时间窗口内航向变化量作为监督学习的训练标签。
37.优选的，选取均值、最大值、最小值、标准差、频域均值、熵、四分位值、频域信号方差、运动强度、峰值、偏度、变异系数、窗口数据之和作为位移变化量的特征描述量。其中标准差计算公式为：频域均值计算公式为：；熵计算公式为：；
四分位值计算公式为：；频域信号方差计算公式为：；运动强度计算公式为：；峰值计算公式为：；偏度计算公式为：；变异系数计算公式为：窗口数据之和计算公式为：；其中，fft()为对括号中的变量做快速傅里叶变换，mean()为对括号中的变量取均值，sum()为对括号中的变量求和， 0.25n代表总数据量n的四分之一，0.75n代表总数据量n的四分之三，var()代表对括号中的变量求方差，std()代表对括号中的变量求标准差。
38.进一步地，利用gps记录的数据计算出时间窗口内位移变化量作为监督学习的训练标签。通过运动特征提取模块实现上述方法中运动特征提取的算法。
39.历史特征存储模块，用于持续记录和存储累积惯性测量单元的惯性测量数据，以逐渐增加模型训练的数据来源。
40.步骤s3、神经网络模型训练，通过神经网络模型训练，来构建惯性数据特征与导航参数的相关量之间的映射模型。
41.惯性数据特征是对惯性数据变化的描述，当载体匀速直线运动时，惯性测量单元测量到的加速度、角速度是以0为中心上下震荡的，这种以0为中心，上下震荡即是惯性数据特征。当载体从匀速变为加速时，惯性测量单元测量到的加速度应从以0为中心上下震荡变为逐渐增大。这种以0为中心上下震荡变为逐渐增大也是惯性数据特征。再如，当载体突然转向时，惯性测量单元测量到的航向角速度会有一个瞬间峰值信号，这种瞬间峰值信号也是惯性数据特征。
42.惯性测量单元所记录的惯性测量数据有严格的时间序列，虽然对记录的数据划分了窗口，但窗口之间依然存在时间依赖性。本发明选择对时间序列有较好预测能力的长短期记忆网络（lstm）来构建惯性数据特征与导航参数的相关量之间的映射模型。
43.进一步地，定义本发明网络的损失函数。本发明定义网络的损失函数为：
为t时刻网络的输出预测值，为真实标签值，mean()为对括号中的变量取均值。
44.进一步地，设置神经网络输入节点数，输出节点数，学习率lr，隐藏层数h_num，每次训练样本数batch_size，时间步长timestep，迭代次数iter，隐藏层中神经元的数量。
45.进一步地，设置神经网络中航向变化量预测中输入节点数为2，位移变化量预测中输入节点数为70，航向变化量与位移变化量预测输出节点数均为1，学习率lr=0.001，隐藏层数h_num=2, 每次训练样本数batch_size=30，时间步长timestep=2，迭代次数iter=2000，隐藏层中神经元的数量，优选的，根据公式计算隐藏层神经元数为2和48（四舍五入取整数）：在神经网络参数设置完成后，开始训练，若损失函数收敛到预设的阈值以内，则代表训练完成，终止对神经网络模型的训练。
46.步骤s4、神经网络模型预测，将数据输入到训练得到的惯性数据特征与导航参数的相关量之间的映射模型中得到神经网络输出值，即可推算载体位置。
47.本发明中，将惯性测量单元所记录的惯性测量数据的70%作为训练集，30%作为测试集，将测试集中的数据输入到训练得到的惯性数据特征与导航参数的相关量之间的映射模型中得到神经网络输出值，航向变化量和位移变化量，如图4，图5所示。其中图4中，横轴代表计算时间点，本方法中时间窗口为5秒，横轴中一个时间点代表5秒。纵轴代表每5秒载体的航向变化量。上子图显示了用本方法得到的预测值，下子图显示了载体的真实航向变化量。图5中横轴代表计算时间点，本方法中时间窗口为5秒，横轴中一个时间点代表5秒。纵轴代表每5秒载体的位移变化量。上子图显示了用本方法得到的预测值，下子图显示了载体的真实位移变化量。
48.根据图4、图5得到的位移变化量和航向变化量，利用以下公式解算出载体位置，最终结果如图6所示。其中设置载体初始坐标 (l
x
, ly)为（0，0），初始航向=0，=0，式中代表东向位移，代表北向位移，代表航向。
49.最后，将经训练与测试后的惯性数据特征与导航参数的相关量之间的映射模型嵌入用于纯惯性测量的水下长航程导航的开发板中，即可用于实际使用。
50.根据本发明的第2方面，一种基于纯惯性测量的水下长航程导航定位装置，包括惯性测量数据输入接口、惯性测量数据预处理模块、运动特征提取模块、历史特征存储模块、神经网络模型训练模块、模型更新判决模块、神经网络模型预测模块和载体位置、航向等导航参数输出或显示接口，惯性测量数据输入接口、历史特征存储模块、模型更新判决模块、载体导航参数输出或显示接口均为使得上述所提出的一种基于纯惯性测量的水下长航程
导航定位方法在硬件装置中在线实现。其中，惯性测量数据输入接口用于实时接收和解析惯性测量单元发送的惯性测量数据；惯性测量数据预处理模块包括但不限于两个子模块，即运动加速度误差模型子模块和数据降噪子模块，实现上述方法中步骤s1、惯性测量数据预处理的算法；运动特征提取模块实现上述方法中步骤s2、运动特征提取的算法；历史特征存储模块用于在装置与某惯性测量单元配套安装后，持续记录累积该惯性测量单元的惯性测量数据，以逐渐增加模型训练的数据来源；神经网络模型训练模块实现上述方法中步骤s3、神经网络模型训练的算法；模型更新判决模块用于神经网络模型训练模块训练得到新的神经网络模型后，是否提供给神经网络模型预测模块进行更新。模型更新判决模块可采用定时更新、定启动次数更新等多种判决机制；神经网络模型预测模块实现上述方法步骤s4中神经网络模型预测的算法；载体位置、航向等导航参数的导航参数输出或显示接口，将解算得到的载体位置、航向等导航参数以指定格式数据给其他装置，或发送至显示装置用于目视查看。
51.在本说明书的描述中，参考术语“实施例”、“示例”等的描述意指结合该实施例或示例描述的具体特征、结构、材料或者特点包含于本发明的至少一个实施例或示例中。在本说明书中，对上述术语的示意性表述不必须针对的是相同的实施例或示例。此外，本领域的技术人员可以在不产生矛盾的情况下，将本说明书中描述的不同实施例或示例以及其中的特征进行结合或组合。
52.上述内容虽然已经示出和描述了本发明的实施例，可以理解的是，上述实施例是示例性的，不能理解为对本发明的限制，本领域的普通技术人员在本发明的范围内可以对上述实施例进行变化、修改、替换和变型等更新操作。