CN108534775B - 基于捷联式惯性导航系统的空间轨迹重建方法及装置 - Google Patents

基于捷联式惯性导航系统的空间轨迹重建方法及装置 Download PDF

Info

Publication number
CN108534775B
CN108534775B CN201810658605.8A CN201810658605A CN108534775B CN 108534775 B CN108534775 B CN 108534775B CN 201810658605 A CN201810658605 A CN 201810658605A CN 108534775 B CN108534775 B CN 108534775B
Authority
CN
China
Prior art keywords
moment
acceleration
time
angular velocity
calculating
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
CN201810658605.8A
Other languages
English (en)
Other versions
CN108534775A (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.)
China Academy of Railway Sciences Corp Ltd CARS
Infrastructure Inspection Institute of CARS
Beijing IMAP Technology Co Ltd
Original Assignee
China Academy of Railway Sciences Corp Ltd CARS
Infrastructure Inspection Institute of CARS
Beijing IMAP Technology Co Ltd
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 China Academy of Railway Sciences Corp Ltd CARS, Infrastructure Inspection Institute of CARS, Beijing IMAP Technology Co Ltd filed Critical China Academy of Railway Sciences Corp Ltd CARS
Priority to CN201810658605.8A priority Critical patent/CN108534775B/zh
Publication of CN108534775A publication Critical patent/CN108534775A/zh
Application granted granted Critical
Publication of CN108534775B publication Critical patent/CN108534775B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

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

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

本发明提供一种基于捷联式惯性导航系统的空间轨迹重建方法及装置。该基于捷联式惯性导航系统的空间轨迹重建方法包括：获得t时刻b系的角速度和t时刻b系的加速度；根据t时刻b系的角速度和t时刻b系的加速度，得到t时刻n系的角速度和t时刻n系的加速度；根据t时刻n系的角速度和t时刻n系的加速度构建预测样本；将预测样本输入已构建的卷积神经网络模型中进行深度学习，得到t时刻增量量化值；根据t时刻n系的加速度和t时刻增量量化值，计算矫正后的t时刻n系的加速度；根据矫正后的t时刻n系的加速度构建空间轨迹，可以极大提升重建得到的空间轨迹的精度。

Description

基于捷联式惯性导航系统的空间轨迹重建方法及装置
技术领域
本发明涉及空间轨迹重建技术领域，具体地，涉及一种基于捷联式惯性导航系统的空间轨迹重建方法及装置。
背景技术
捷联式惯性导航技术(SINT，Strapdown Inertial Navigation Technology)广泛应用于轨道检测、制导控制、机器人运动跟踪、医疗康复等领域。对于安装有捷联式惯性导航系统(SINS，Strapdown Inertial Navigation System)的载体而言，其时空轨迹重建的要点在于如何通过惯性测量单元(IMU，Inertial Measurement Unit)在载体坐标系中输出的角速度与加速度，重建出载体在惯性导航坐标系中的空间轨迹。但现有技术中重建得到的空间轨迹精度不足，严重偏离实际的空间轨迹。
发明内容
本发明实施例的主要目的在于提供一种基于捷联式惯性导航系统的空间轨迹重建方法及装置，以极大提升重建得到的空间轨迹的精度。
为了实现上述目的，本发明实施例提供一种基于捷联式惯性导航系统的空间轨迹重建方法，包括：
获得t时刻b系的角速度和t时刻b系的加速度，其中，b系为载体坐标系，t≥4；
根据t时刻b系的角速度和t时刻b系的加速度，得到t时刻n系的角速度和t时刻n系的加速度，其中，n系为惯性导航坐标系；
根据t时刻n系的角速度和t时刻n系的加速度构建预测样本；
将预测样本输入已构建的卷积神经网络模型中进行深度学习，得到t时刻增量量化值；
根据t时刻n系的加速度和t时刻增量量化值，计算矫正后的t时刻n系的加速度；
根据矫正后的t时刻n系的加速度构建空间轨迹。
本发明实施例还提供一种基于捷联式惯性导航系统的空间轨迹重建装置，包括：
获得模块，用于获得t时刻b系的角速度和t时刻b系的加速度，其中，b系为载体坐标系，t≥4；
角速度和加速度计算模块，用于根据t时刻b系的角速度和t时刻b系的加速度，得到t时刻n系的角速度和t时刻n系的加速度，其中，n系为惯性导航坐标系；
预测样本构建模块，用于根据t时刻n系的角速度和t时刻n系的加速度构建预测样本；
增量量化值模块，用于将预测样本输入已构建的卷积神经网络模型中进行深度学习，得到t时刻增量量化值；
n系加速度矫正模块，用于根据t时刻n系的加速度和t时刻增量量化值，计算矫正后的t时刻n系的加速度；
空间轨迹构建模块，用于根据矫正后的t时刻n系的加速度构建空间轨迹。
本发明实施例还提供一种计算机设备，包括存储器、处理器及存储在存储器上并可在处理器上运行的计算机程序，处理器执行计算机程序时实现以下步骤：
获得t时刻b系的角速度和t时刻b系的加速度，其中，b系为载体坐标系，t≥4；
根据t时刻b系的角速度和t时刻b系的加速度，得到t时刻n系的角速度和t时刻n系的加速度，其中，n系为惯性导航坐标系；
根据t时刻n系的角速度和t时刻n系的加速度构建预测样本；
将预测样本输入已构建的卷积神经网络模型中进行深度学习，得到t时刻增量量化值；
根据t时刻n系的加速度和t时刻增量量化值，计算矫正后的t时刻n系的加速度；
根据矫正后的t时刻n系的加速度构建空间轨迹。
本发明实施例还提供一种计算机可读存储介质，其上存储有计算机程序，计算机程序被处理器执行时实现以下步骤：
获得t时刻b系的角速度和t时刻b系的加速度，其中，b系为载体坐标系，t≥4；
根据t时刻b系的角速度和t时刻b系的加速度，得到t时刻n系的角速度和t时刻n系的加速度，其中，n系为惯性导航坐标系；
根据t时刻n系的角速度和t时刻n系的加速度构建预测样本；
将预测样本输入已构建的卷积神经网络模型中进行深度学习，得到t时刻增量量化值；
根据t时刻n系的加速度和t时刻增量量化值，计算矫正后的t时刻n系的加速度；
根据矫正后的t时刻n系的加速度构建空间轨迹。
本发明实施例的基于捷联式惯性导航系统的空间轨迹重建方法及装置先获得t时刻b系的角速度和t时刻b系的加速度；再根据t时刻b系的角速度和t时刻b系的加速度得到t时刻n系的角速度和t时刻n系的加速度；然后根据t时刻n系的角速度和t时刻n系的加速度构建预测样本，将预测样本输入已构建的卷积神经网络模型中进行深度学习，得到t时刻增量量化值；接着根据t时刻n系的加速度和t时刻增量量化值，计算矫正后的t时刻n系的加速度；最后根据矫正后的t时刻n系的加速度构建空间轨迹，可以极大提升重建得到的空间轨迹的精度。
附图说明
为了更清楚地说明本发明实施例或现有技术中的技术方案，下面将对实施例描述中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图仅仅是本发明的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据这些附图获得其他的附图。
图1是本发明实施例中基于捷联式惯性导航系统的空间轨迹重建方法的流程图；
图2是本发明实施例中S102的具体流程图；
图3是本发明实施例卷积神经网络模型的构建流程图；
图4是本发明实施例中S106的具体流程图；
图5是本发明实施例中矫正b系加速度的具体流程图；
图6是本发明实施例中卷积神经网络的结构图；
图7是计算机视觉系统测得的空间轨迹、未经过深度学习矫正重建的空间轨迹与本发明实施例重建的空间轨迹在x轴方向上的对比示意图；
图8是计算机视觉系统测得的空间轨迹、未经过深度学习矫正重建的空间轨迹与本发明实施例重建的空间轨迹在y轴方向上的对比示意图；
图9是计算机视觉系统测得的空间轨迹、未经过深度学习矫正重建的空间轨迹与本发明实施例重建的空间轨迹在z轴方向上的对比示意图；
图10是计算机视觉系统测得的空间轨迹、未经过深度学习矫正重建的空间轨迹与本发明实施例重建的空间轨迹的三围对比示意图；
图11是本发明实施例中基于捷联式惯性导航系统的空间轨迹重建装置的结构框图。
具体实施方式
下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。
鉴于现有技术中重建得到的空间轨迹精度不足，严重偏离实际的空间轨迹，本发明实施例提供一种基于捷联式惯性导航系统的空间轨迹重建方法，可以极大提升重建得到的空间轨迹的精度。以下结合附图对本发明进行详细说明。
图1是本发明实施例中基于捷联式惯性导航系统的空间轨迹重建方法的流程图。如图1所示，基于捷联式惯性导航系统的空间轨迹重建方法包括：
S101：获得t时刻b系的角速度和t时刻b系的加速度，其中，b系为载体坐标系，t≥4。
S102：根据t时刻b系的角速度和t时刻b系的加速度，得到t时刻n系的角速度和t时刻n系的加速度，其中，n系为惯性导航坐标系。
S103：根据t时刻n系的角速度和t时刻n系的加速度构建预测样本。
S104：将预测样本输入已构建的卷积神经网络模型中进行深度学习(DL，DeepLearning)，得到t时刻增量量化值。
S105：根据t时刻n系的加速度和t时刻增量量化值，计算矫正后的t时刻n系的加速度。
S106：根据矫正后的t时刻n系的加速度构建空间轨迹。
图1所示的基于捷联式惯性导航系统的空间轨迹重建方法的执行主体可以为计算机，可以应用于需要SINS进行测量和导航的领域，如轨道检测设备、康复辅助训练设备等。由图1所示的流程可知，本发明实施例的基于捷联式惯性导航系统的空间轨迹重建方法先获得t时刻b系的角速度和t时刻b系的加速度；再根据t时刻b系的角速度和t时刻b系的加速度得到t时刻n系的角速度和t时刻n系的加速度；然后根据t时刻n系的角速度和t时刻n系的加速度构建预测样本，将预测样本输入已构建的卷积神经网络模型中进行深度学习，得到t时刻增量量化值；接着根据t时刻n系的加速度和t时刻增量量化值，计算矫正后的t时刻n系的加速度；最后根据矫正后的t时刻n系的加速度构建空间轨迹，可以极大提升重建得到的空间轨迹的精度。
图2是本发明实施例中S102的具体流程图。如图2所示，S102包括：
S201：根据t-1时刻从b系到n系的惯性旋转四元数和t时刻b系的角速度，计算t时刻从b系到n系的角速度旋转四元数。
S202：根据t-1时刻从b系到n系的惯性旋转四元数、t-1时刻标准步长和t-1时刻增量参数，计算t时刻从b系到n系的加速度旋转四元数。
S203：根据t时刻从b系到n系的角速度旋转四元数和t时刻从b系到n系的加速度旋转四元数，计算t时刻从b系到n系的惯性旋转四元数。
S204：根据t时刻从b系到n系的惯性旋转四元数，计算t时刻从b系到n系的姿态矩阵。
S205：根据t时刻b系的角速度和t时刻从b系到n系的姿态矩阵，计算t时刻n系的角速度。
S206：根据t时刻b系的加速度和t时刻从b系到n系的姿态矩阵，计算t时刻n系的加速度。
S201中，通过如下公式计算t时刻从b系到n系的角速度旋转四元数：
Figure BDA0001706124280000051
其中，
Figure BDA0001706124280000052
为t时刻从b系到n系的角速度旋转四元数，
Figure BDA0001706124280000053
为t-1时刻从b系到n系的惯性旋转四元数，bωt为t时刻b系的角速度，也是t时刻陀螺在b系下的测量值；
Figure BDA00017061242800000512
为四元数的Hamilton乘积，定义为：
Figure BDA0001706124280000054
S202中，通过如下公式计算t时刻从b系到n系的加速度旋转四元数：
Figure BDA0001706124280000055
其中，
Figure BDA0001706124280000056
为t时刻从b系到n系的加速度旋转四元数，
Figure BDA0001706124280000057
为t-1时刻从b系到n系的惯性旋转四元数，αt-1为t-1时刻标准步长，pt-1为t-1时刻增量参数。
一实施例中，t-1时刻增量参数按照如下方式确定：
根据t-2时刻从b系到n系的加速度旋转四元数和t-3时刻从b系到n系的加速度旋转四元数，计算t-1时刻对称正定矩阵；根据t-1时刻对称正定矩阵和t-1时刻从b系到n系的加速度旋转四元数，计算t-1时刻增量参数。
具体实施时，通过如下公式计算t-1时刻对称正定矩阵：
Figure BDA0001706124280000058
其中，
Figure BDA0001706124280000059
为t-1时刻对称正定矩阵，
Figure BDA00017061242800000510
为t-2时刻对称正定矩阵，
Figure BDA00017061242800000511
为yt-2的转置矩阵，
Figure BDA0001706124280000061
为st-2的转置矩阵，I为单位矩阵，
Figure BDA00017061242800000621
yt-2如下：
Figure BDA0001706124280000062
其中，
Figure BDA0001706124280000063
为t-2时刻从b系到n系的加速度旋转四元数，
Figure BDA0001706124280000064
为t-3时刻从b系到n系的加速度旋转四元数，
Figure BDA0001706124280000065
为t-2时刻从b系到n系的加速度旋转四元数的梯度，
Figure BDA0001706124280000066
为t-3时刻从b系到n系的加速度旋转四元数的梯度；
st-2如下：
Figure BDA0001706124280000067
以及，通过如下公式计算t-1时刻增量参数：
Figure BDA0001706124280000068
其中，pt-1为t-1时刻增量参数，
Figure BDA0001706124280000069
为t-1时刻从b系到n系的加速度旋转四元数，
Figure BDA00017061242800000610
为
Figure BDA00017061242800000611
的梯度。
一实施例中，t-1时刻标准步长按照如下方式确定：
执行迭代处理：根据t-1时刻从b系到n系的惯性旋转四元数和t-1时刻增量参数，判断t-2时刻标准步长是否满足预设条件；当t-2时刻标准步长满足预设条件时，将t-2时刻标准步长设置为t-1时刻标准步长；当t-2时刻标准步长不满足预设条件时，将t-2时刻标准步长乘以预设常数得到t-2时刻第j+1个标准步长，将t-2时刻标准步长设置为t-2时刻第j+1个标准步长，j为迭代次数。
具体实施时，预设条件如下：
Figure BDA00017061242800000612
其中，
Figure BDA00017061242800000613
为t-1时刻从b系到n系的惯性旋转四元数，αt-2为t-2时刻标准步长，pt-1为t-1时刻增量参数，ng为n系的重力场，ng＝[0,0,0,1]，bat-1为t-1时刻b系的加速度，c为预设参数，0＜c＜0.5，
Figure BDA00017061242800000614
为pt-1的转置矩阵，
Figure BDA00017061242800000615
为
Figure BDA00017061242800000616
的梯度；
Figure BDA00017061242800000617
如下：
Figure BDA00017061242800000618
Figure BDA00017061242800000619
如下：
Figure BDA00017061242800000620
S203中，通过如下公式计算t时刻从b系到n系的惯性旋转四元数：
Figure BDA0001706124280000071
其中，
Figure BDA0001706124280000072
为t时刻从b系到n系的惯性旋转四元数，
Figure BDA0001706124280000073
为t时刻从b系到n系的角速度旋转四元数，qa,t为t时刻从b系到n系的加速度旋转四元数；
λt如下：
Figure BDA0001706124280000074
其中，
Figure BDA0001706124280000075
为t时刻从b系到n系的加速度旋转四元数，
Figure BDA0001706124280000076
为t时刻从b系到n系的角速度旋转四元数。
S204中，通过如下公式计算t时刻从b系到n系的姿态矩阵：
Figure BDA0001706124280000077
Figure BDA0001706124280000078
其中，
Figure BDA0001706124280000079
为t时刻从b系到n系的姿态矩阵，
Figure BDA00017061242800000710
为t时刻从b系到n系的惯性旋转四元数。
S205中，通过如下公式计算t时刻n系的角速度：
Figure BDA00017061242800000711
其中，nωt为t时刻n系的角速度，
Figure BDA00017061242800000712
为t时刻从b系到n系的姿态矩阵，bωt为t时刻b系的角速度。
S206中，通过如下公式计算t时刻n系的加速度：
Figure BDA00017061242800000713
其中，nat为t时刻n系的加速度，
Figure BDA00017061242800000714
为t时刻从b系到n系的姿态矩阵，bat为t时刻b系的加速度。
图3是本发明实施例卷积神经网络模型的构建流程图。如图3所示，已构建的卷积神经网络模型按照如下方式构建：
S301：构建训练样本和验证样本，训练样本和验证样本均包括t’时刻n系的角速度、t’时刻n系的加速度和t’时刻增量量化值。
S302：根据训练样本对卷积神经网络模型进行训练，得到经过训练的卷积神经网络模型。
S303：根据验证样本对经过训练的卷积神经网络模型进行验证，若经过训练的卷积神经网络模型符合预设标准，则经过训练的卷积神经网络模型为已构建的卷积神经网络模型；若不符合，则利用训练样本对经过训练的卷积神经网络模型进行重新训练，直至经过训练的卷积神经网络模型符合预设标准为止。
在执行S301之前，需要构造训练集(X，Y)。其中，X包括t’时刻n系的角速度和t’时刻n系的加速度，Y包括t’时刻增量量化值。
一实施例中，t’时刻增量量化值按照如下方式确定：根据t’时刻n系的金标准加速度，t’时刻n系的加速度和量化精度，计算t’时刻增量量化值，公式如下：
Figure BDA0001706124280000081
Figure BDA0001706124280000082
其中，yt'为t’时刻增量量化值，
Figure BDA0001706124280000083
为t’时刻n系的金标准加速度，nat'为t’时刻n系的加速度，K为量化精度，κ为金标准与测量值的界限。大多数情况下，κ可以认为等于1，否则对应的yt'会被认为是异常数据而删除。上述公式中的
Figure BDA0001706124280000084
和nat'均为同一采样样本中的数据。
金标准加速度可以根据实际使用情况来生成。如果是轨道检测车等按既定路线运动的SINS，可以把检测线路的历史无故障检测样本经过惯性导航算法计算后得到的n系加速度进行统计，得到针对每一段检测线路的加速度平均值与标准差。这样，作为该段检测线路的训练集(X，Y)，当X为检测线路的历史无故障检测样本经过惯性导航算法计算后得到的n系加速度和角速度时，计算Y所需的金标准加速度为该段检测线路的加速度平均值与正负标准差之间的任意一个采样值。如果是可穿戴的SINS，可以利用其他传感器(如计算机视觉系统)测得的空间轨迹的二次差分得到金标准加速度，然后通过上述公式计算得到增量量化值作为Y。
训练集(X，Y)是由经过N次采样得到的N个样本构成，即X＝{x1,……,xk,……,xN}；Y＝{y1,……,yk,……,yN}。其中，xk包括第k次采样中得到的任意时刻n系的角速度和任意时刻n系的加速度，即
Figure BDA0001706124280000085
是一个6×T的时间序列，一共有T个采样时间；
Figure BDA0001706124280000086
为第k次采样中t’时刻n系的加速度，
Figure BDA0001706124280000087
为第k次采样中t’时刻n系的三轴角速度，t’为任意时刻。yk包括第k次采样中的任意时刻增量量化值，与
Figure BDA0001706124280000088
和
Figure BDA0001706124280000089
对应。将训练集(X，Y)中90％的样本作为训练样本，10％的样本作为训练样本验证样本。
图6是本发明实施例中卷积神经网络的结构图。如图6所示，卷积神经网络包括：多个第一卷积层、多个第二卷积层和全连接层。第一卷积层采用1×wl维卷积窗，可以避免输入端各通道互相影响。即，n系三个方向(x方向，y方向，z方向)的角速度(nωx，nωy，nωz)互不影响，n系三个方向(x方向，y方向，z方向)的加速度(nax，nay，naz)互不影响。第二卷积层采用3×wl维卷积窗，纵向步长为3，可以融合提取n系三个方向的角速度，融合提取n系三个方向的加速度，但角速度和加速度互不影响。全连接层有K+1个端口，可以对上一层的各数据进行融合。其中，wl为第一卷积层的卷积窗宽度，K为量化精度。卷积神经网络的其他部分，如池化层(pooling)等可以根据实际情况添加，各个卷积层与全连接层的实际层数也可以根据实际情况添加。
S105中，通过如下公式计算矫正后的t时刻n系的加速度：
Figure BDA0001706124280000091
其中，
Figure BDA0001706124280000092
为矫正后的t时刻n系的加速度，nat为t时刻n系的加速度，yt为t时刻增量量化值，K为量化精度，κ为金标准与测量值的界限。
图4是本发明实施例中S106的具体流程图。如图4所示，S106包括：
S401：根据矫正后的t时刻n系的加速度计算t时刻n系的速度。
S402：根据t时刻n系的速度计算t时刻n系的位移。
S403：根据t时刻n系的位移构建空间轨迹。
S401中，通过如下公式计算t时刻n系的速度：
Figure BDA0001706124280000093
其中，nvt为t时刻n系的速度，
Figure BDA0001706124280000094
为矫正后的t时刻n系的加速度。
S402中，通过如下公式计算t时刻n系的位移：
Figure BDA0001706124280000095
其中，nst为t时刻n系的位移，nvt为t时刻n系的速度，nvt-1为t-1时刻n系的速度。
图5是本发明实施例中矫正b系加速度的具体流程图。如图5所示，包括：
S501：根据t时刻从b系到n系的姿态矩阵计算t时刻从b系到n系的姿态逆矩阵。
S502：根据矫正后的t时刻n系的加速度和t时刻从b系到n系的姿态逆矩阵计算矫正后的t时刻b系的加速度。
S503：根据矫正后的t时刻b系的加速度和t时刻b系的加速度计算加速度预测偏差。
S504：根据t时刻b系的加速度和加速度预测偏差计算t+1时刻b系的加速度。
具体实施时，可以先将矫正后的t时刻b系的加速度与t时刻b系的加速度相减，得到加速度预测偏差。然后将t时刻b系的加速度与加速度预测偏差相减，得到t+1时刻b系的加速度，可以对b系下一时刻的加速度值进行反馈校正。
本发明的实施方案的流程如下：
1、根据某一历史时刻数据构建卷积神经网络模型：构建训练样本和验证样本，训练样本和验证样本均包括某一历史时刻n系的角速度、某一历史时刻n系的加速度和某一历史时刻增量量化值。根据训练样本对卷积神经网络模型进行训练，根据验证样本对经过训练的卷积神经网络模型进行验证，若经过训练的卷积神经网络模型符合预设标准，则卷积神经网络模型构建成功，否则利用训练样本对卷积神经网络模型进行重新训练，直至其符合预设标准为止。
2、根据t-1时刻从b系到n系的惯性旋转四元数和t时刻b系的角速度，计算t时刻从b系到n系的角速度旋转四元数。
3、根据t-2时刻从b系到n系的加速度旋转四元数和t-3时刻从b系到n系的加速度旋转四元数，计算t-1时刻对称正定矩阵；根据t-1时刻对称正定矩阵和t-1时刻从b系到n系的加速度旋转四元数，计算t-1时刻增量参数。
4、执行迭代处理：根据t-1时刻从b系到n系的惯性旋转四元数和t-1时刻增量参数，判断t-2时刻标准步长是否满足预设条件；当t-2时刻标准步长满足预设条件时，将t-2时刻标准步长设置为t-1时刻标准步长。
5、根据t-1时刻从b系到n系的惯性旋转四元数、t-1时刻标准步长和t-1时刻增量参数，计算t时刻从b系到n系的加速度旋转四元数。
6、根据t时刻从b系到n系的角速度旋转四元数和t时刻从b系到n系的加速度旋转四元数，计算t时刻从b系到n系的姿态矩阵。
7、根据t时刻b系的角速度和t时刻从b系到n系的姿态矩阵，计算t时刻n系的角速度。根据t时刻b系的加速度和t时刻从b系到n系的姿态矩阵，计算t时刻n系的加速度。
8、根据t时刻n系的角速度和t时刻n系的加速度构建预测样本，将预测样本输入已构建的卷积神经网络模型中进行深度学习，得到t时刻增量量化值。根据t时刻n系的加速度和t时刻增量量化值，计算矫正后的t时刻n系的加速度。
9、根据矫正后的t时刻n系的加速度构建空间轨迹。
10、根据t时刻从b系到n系的姿态矩阵计算t时刻从b系到n系的姿态逆矩阵，根据矫正后的t时刻n系的加速度和t时刻从b系到n系的姿态逆矩阵计算矫正后的t时刻b系的加速度。
11、根据矫正后的t时刻b系的加速度和t时刻b系的加速度计算加速度预测偏差，根据t时刻b系的加速度和加速度预测偏差计算t+1时刻b系的加速度。
图7是计算机视觉系统测得的空间轨迹、未经过深度学习矫正重建的空间轨迹与本发明实施例重建的空间轨迹在x轴方向上的对比示意图。图8是计算机视觉系统测得的空间轨迹、未经过深度学习矫正重建的空间轨迹与本发明实施例重建的空间轨迹在y轴方向上的对比示意图。图9是计算机视觉系统测得的空间轨迹、未经过深度学习矫正重建的空间轨迹与本发明实施例重建的空间轨迹在z轴方向上的对比示意图。图10是计算机视觉系统测得的空间轨迹、未经过深度学习矫正重建的空间轨迹与本发明实施例重建的空间轨迹的三围对比示意图。图7至图9中的横轴均为时间，单位为秒(s)，纵轴均为空间轨迹，单位为毫米(mm)，上图均为计算机视觉系统测得的空间轨迹，中图均为未经过深度学习矫正重建的空间轨迹，下图均为本发明实施例重建的空间轨迹。图10中的左图为计算机视觉系统测得的空间轨迹的三围示意图，中图为未经过深度学习矫正重建的空间轨迹的三围示意图，右图为本发明实施例重建的空间轨迹的三围示意图。图10中的坐标系的纵轴为z轴，符合右手坐标系，还可以转化为“北东下”(NED)即X-北，Y-东，Z-下，单位为毫米(mm)。如图7至图10所示，采用本发明的方法，把SINS(MPU9250)固定于手腕，对喝水动作进行轨迹重建，并分别与未经过深度学习矫正重建的空间轨迹和计算机视觉系统(CV)测得的空间轨迹进行比较，发现未经过深度学习矫正重建的空间轨迹与计算机视觉系统测得的空间轨迹有较大的偏差，而本发明实施例重建的空间轨迹与计算机视觉系统测得的空间轨迹较为相似，即采用本发明能较好地重建出喝水这种复杂的运动的空间轨迹。
综上，本发明实施例的基于捷联式惯性导航系统的空间轨迹重建方法先获得t时刻b系的角速度和t时刻b系的加速度；再根据t时刻b系的角速度和t时刻b系的加速度得到t时刻n系的角速度和t时刻n系的加速度；然后根据t时刻n系的角速度和t时刻n系的加速度构建预测样本，将预测样本输入已构建的卷积神经网络模型中进行深度学习，得到t时刻增量量化值；接着根据t时刻n系的加速度和t时刻增量量化值，计算矫正后的t时刻n系的加速度；最后根据矫正后的t时刻n系的加速度构建空间轨迹，可以极大提升重建得到的空间轨迹的精度。
基于同一发明构思，本发明实施例还提供了一种基于捷联式惯性导航系统的空间轨迹重建装置，由于该装置解决问题的原理与基于捷联式惯性导航系统的空间轨迹重建方法相似，因此该装置的实施可以参见方法的实施，重复之处不再赘述。
图11是本发明实施例中基于捷联式惯性导航系统的空间轨迹重建装置的结构框图。如图11所示，基于捷联式惯性导航系统的空间轨迹重建装置包括：
获得模块，用于获得t时刻b系的角速度和t时刻b系的加速度，其中，b系为载体坐标系，t≥4；
角速度和加速度计算模块，用于根据t时刻b系的角速度和t时刻b系的加速度，得到t时刻n系的角速度和t时刻n系的加速度，其中，n系为惯性导航坐标系；
预测样本构建模块，用于根据t时刻n系的角速度和t时刻n系的加速度构建预测样本；
增量量化值模块，用于将预测样本输入已构建的卷积神经网络模型中进行深度学习，得到t时刻增量量化值；
n系加速度矫正模块，用于根据t时刻n系的加速度和t时刻增量量化值，计算矫正后的t时刻n系的加速度；
空间轨迹构建模块，用于根据矫正后的t时刻n系的加速度构建空间轨迹。
在其中一种实施例中，角速度和加速度计算模块具体包括：
角速度旋转四元数单元，用于根据t-1时刻从b系到n系的惯性旋转四元数和t时刻b系的角速度，计算t时刻从b系到n系的角速度旋转四元数；
加速度旋转四元数单元，用于根据t-1时刻从b系到n系的惯性旋转四元数、t-1时刻标准步长和t-1时刻增量参数，计算t时刻从b系到n系的加速度旋转四元数；
惯性旋转四元数单元，用于根据t时刻从b系到n系的角速度旋转四元数和t时刻从b系到n系的加速度旋转四元数，计算t时刻从b系到n系的惯性旋转四元数；
姿态矩阵单元，用于根据t时刻从b系到n系的惯性旋转四元数，计算t时刻从b系到n系的姿态矩阵；
角速度单元，用于根据t时刻b系的角速度和t时刻从b系到n系的姿态矩阵，计算t时刻n系的角速度；
加速度单元，用于根据t时刻b系的加速度和t时刻从b系到n系的姿态矩阵，计算t时刻n系的加速度。
在其中一种实施例中，加速度旋转四元数单元具体用于：
根据t-2时刻从b系到n系的加速度旋转四元数和t-3时刻从b系到n系的加速度旋转四元数，计算t-1时刻对称正定矩阵；
根据t-1时刻对称正定矩阵和t-1时刻从b系到n系的加速度旋转四元数，计算t-1时刻增量参数。
在其中一种实施例中，加速度旋转四元数单元具体用于：
执行迭代处理：根据t-1时刻从b系到n系的惯性旋转四元数和t-1时刻增量参数，判断t-2时刻标准步长是否满足预设条件；当t-2时刻标准步长满足预设条件时，将t-2时刻标准步长设置为t-1时刻标准步长；当t-2时刻标准步长不满足预设条件时，将t-2时刻标准步长乘以预设常数得到t-2时刻第j+1个标准步长，将t-2时刻标准步长设置为t-2时刻第j+1个标准步长，j为迭代次数。
在其中一种实施例中，还包括：
训练样本和验证样本构建模块，用于构建训练样本和验证样本，训练样本和验证样本均包括t’时刻n系的角速度、t’时刻n系的加速度和t’时刻增量量化值；
训练和验证模块，用于根据训练样本对卷积神经网络模型进行训练，得到经过训练的卷积神经网络模型；根据验证样本对经过训练的卷积神经网络模型进行验证，若经过训练的卷积神经网络模型符合预设标准，则经过训练的卷积神经网络模型为已构建的卷积神经网络模型；若不符合，则利用训练样本对经过训练的卷积神经网络模型进行重新训练，直至经过训练的卷积神经网络模型符合预设标准为止。
在其中一种实施例中，还包括：
增量量化值计算模块，用于根据t’时刻n系的金标准加速度，t’时刻n系的加速度和量化精度，计算t’时刻增量量化值。
在其中一种实施例中，卷积神经网络包括：
多个第一卷积层、多个第二卷积层和全连接层；
第一卷积层采用1×wl维卷积窗，第二卷积层采用3×wl维卷积窗，全连接层有K+1个端口，其中，wl为第一卷积层的卷积窗宽度，K为量化精度。
在其中一种实施例中，空间轨迹构建模块具体包括：
速度单元，用于根据矫正后的t时刻n系的加速度计算t时刻n系的速度；
位移单元，用于根据t时刻n系的速度计算t时刻n系的位移；
空间轨迹单元，用于根据t时刻n系的位移构建空间轨迹。
在其中一种实施例中，还包括：
姿态逆矩阵模块，用于根据t时刻从b系到n系的姿态矩阵计算t时刻从b系到n系的姿态逆矩阵；
b系加速度矫正模块，用于根据矫正后的t时刻n系的加速度和t时刻从b系到n系的姿态逆矩阵计算矫正后的t时刻b系的加速度；
加速度预测偏差模块，用于根据矫正后的t时刻b系的加速度和t时刻b系的加速度计算加速度预测偏差；
加速度预测模块，用于根据t时刻b系的加速度和加速度预测偏差计算t+1时刻b系的加速度。
综上，本发明实施例的基于捷联式惯性导航系统的空间轨迹重建装置先获得t时刻b系的角速度和t时刻b系的加速度；再根据t时刻b系的角速度和t时刻b系的加速度得到t时刻n系的角速度和t时刻n系的加速度；然后根据t时刻n系的角速度和t时刻n系的加速度构建预测样本，将预测样本输入已构建的卷积神经网络模型中进行深度学习，得到t时刻增量量化值；接着根据t时刻n系的加速度和t时刻增量量化值，计算矫正后的t时刻n系的加速度；最后根据矫正后的t时刻n系的加速度构建空间轨迹，可以极大提升重建得到的空间轨迹的精度。
本发明实施例还提供了一种计算机设备，包括存储器、处理器及存储在存储器上并可在处理器上运行的计算机程序，处理器执行计算机程序时实现以下步骤：
获得t时刻b系的角速度和t时刻b系的加速度，其中，b系为载体坐标系，t≥4；
根据t时刻b系的角速度和t时刻b系的加速度，得到t时刻n系的角速度和t时刻n系的加速度，其中，n系为惯性导航坐标系；
根据t时刻n系的角速度和t时刻n系的加速度构建预测样本；
将预测样本输入已构建的卷积神经网络模型中进行深度学习，得到t时刻增量量化值；
根据t时刻n系的加速度和t时刻增量量化值，计算矫正后的t时刻n系的加速度；
根据矫正后的t时刻n系的加速度构建空间轨迹。
综上，本发明实施例的计算机设备先获得t时刻b系的角速度和t时刻b系的加速度；再根据t时刻b系的角速度和t时刻b系的加速度得到t时刻n系的角速度和t时刻n系的加速度；然后根据t时刻n系的角速度和t时刻n系的加速度构建预测样本，将预测样本输入已构建的卷积神经网络模型中进行深度学习，得到t时刻增量量化值；接着根据t时刻n系的加速度和t时刻增量量化值，计算矫正后的t时刻n系的加速度；最后根据矫正后的t时刻n系的加速度构建空间轨迹，可以极大提升重建得到的空间轨迹的精度。
本发明实施例还提供了一种计算机可读存储介质，其上存储有计算机程序，计算机程序被处理器执行时实现以下步骤：
获得t时刻b系的角速度和t时刻b系的加速度，其中，b系为载体坐标系，t≥4；
根据t时刻b系的角速度和t时刻b系的加速度，得到t时刻n系的角速度和t时刻n系的加速度，其中，n系为惯性导航坐标系；
根据t时刻n系的角速度和t时刻n系的加速度构建预测样本；
将预测样本输入已构建的卷积神经网络模型中进行深度学习，得到t时刻增量量化值；
根据t时刻n系的加速度和t时刻增量量化值，计算矫正后的t时刻n系的加速度；
根据矫正后的t时刻n系的加速度构建空间轨迹。
综上，本发明实施例的计算机可读存储介质先获得t时刻b系的角速度和t时刻b系的加速度；再根据t时刻b系的角速度和t时刻b系的加速度得到t时刻n系的角速度和t时刻n系的加速度；然后根据t时刻n系的角速度和t时刻n系的加速度构建预测样本，将预测样本输入已构建的卷积神经网络模型中进行深度学习，得到t时刻增量量化值；接着根据t时刻n系的加速度和t时刻增量量化值，计算矫正后的t时刻n系的加速度；最后根据矫正后的t时刻n系的加速度构建空间轨迹，可以极大提升重建得到的空间轨迹的精度。
以上所述的具体实施例，对本发明的目的、技术方案和有益效果进行了进一步详细说明，所应理解的是，以上所述仅为本发明的具体实施例而已，并不用于限定本发明的保护范围，凡在本发明的精神和原则之内，所做的任何修改、等同替换、改进等，均应包含在本发明的保护范围之内。

Claims (31)

1.一种基于捷联式惯性导航系统的空间轨迹重建方法，其特征在于，包括：
获得t时刻b系的角速度和t时刻b系的加速度，其中，b系为载体坐标系，t≥4；
根据所述t时刻b系的角速度和所述t时刻b系的加速度，得到t时刻n系的角速度和t时刻n系的加速度，其中，n系为惯性导航坐标系；
根据所述t时刻n系的角速度和所述t时刻n系的加速度构建预测样本；
将所述预测样本输入已构建的卷积神经网络模型中进行深度学习，得到t时刻增量量化值；
根据所述t时刻n系的加速度和所述t时刻增量量化值，计算矫正后的t时刻n系的加速度；
根据所述矫正后的t时刻n系的加速度构建空间轨迹；
所述根据所述t时刻b系的角速度和所述t时刻b系的加速度，得到t时刻n系的角速度和t时刻n系的加速度，包括：
根据t-1时刻从b系到n系的惯性旋转四元数和t时刻b系的角速度，计算t时刻从b系到n系的角速度旋转四元数；
根据所述t-1时刻从b系到n系的惯性旋转四元数、t-1时刻标准步长和t-1时刻增量参数，计算t时刻从b系到n系的加速度旋转四元数；
根据所述t时刻从b系到n系的角速度旋转四元数和所述t时刻从b系到n系的加速度旋转四元数，计算t时刻从b系到n系的惯性旋转四元数；
根据所述t时刻从b系到n系的惯性旋转四元数，计算t时刻从b系到n系的姿态矩阵；
根据所述t时刻b系的角速度和所述t时刻从b系到n系的姿态矩阵，计算t时刻n系的角速度；
根据所述t时刻b系的加速度和所述t时刻从b系到n系的姿态矩阵，计算t时刻n系的加速度。
2.根据权利要求1所述的基于捷联式惯性导航系统的空间轨迹重建方法，其特征在于，通过如下公式计算t时刻从b系到n系的角速度旋转四元数：
Figure FDA0002387035670000011
其中，
Figure FDA0002387035670000021
为t时刻从b系到n系的角速度旋转四元数，
Figure FDA0002387035670000022
为t-1时刻从b系到n系的惯性旋转四元数，bωt为t时刻b系的角速度，
Figure FDA0002387035670000023
为四元数的Hamilton乘积。
3.根据权利要求1所述的基于捷联式惯性导航系统的空间轨迹重建方法，其特征在于，通过如下公式计算t时刻从b系到n系的加速度旋转四元数：
Figure FDA0002387035670000024
其中，
Figure FDA0002387035670000025
为t时刻从b系到n系的加速度旋转四元数，
Figure FDA0002387035670000026
为t-1时刻从b系到n系的惯性旋转四元数，αt-1为t-1时刻标准步长，pt-1为t-1时刻增量参数。
4.根据权利要求1所述的基于捷联式惯性导航系统的空间轨迹重建方法，其特征在于，所述t-1时刻增量参数按照如下方式确定：
根据t-2时刻从b系到n系的加速度旋转四元数和t-3时刻从b系到n系的加速度旋转四元数，计算t-1时刻对称正定矩阵；
根据所述t-1时刻对称正定矩阵和t-1时刻从b系到n系的加速度旋转四元数，计算所述t-1时刻增量参数。
5.根据权利要求4所述的基于捷联式惯性导航系统的空间轨迹重建方法，其特征在于，所述t-1时刻标准步长按照如下方式确定：
执行迭代处理：根据所述t-1时刻从b系到n系的惯性旋转四元数和所述t-1时刻增量参数，判断t-2时刻标准步长是否满足预设条件；当所述t-2时刻标准步长满足所述预设条件时，将所述t-2时刻标准步长设置为所述t-1时刻标准步长；当所述t-2时刻标准步长不满足所述预设条件时，将所述t-2时刻标准步长乘以预设常数得到t-2时刻第j+1个标准步长，将所述t-2时刻标准步长设置为所述t-2时刻第j+1个标准步长，j为迭代次数。
6.根据权利要求5所述的基于捷联式惯性导航系统的空间轨迹重建方法，其特征在于，通过如下公式计算t-1时刻对称正定矩阵：
Figure FDA0002387035670000027
其中，
Figure FDA0002387035670000028
为t-1时刻对称正定矩阵，
Figure FDA0002387035670000029
为t-2时刻对称正定矩阵，
Figure FDA00023870356700000210
为yt-2的转置矩阵，
Figure FDA00023870356700000211
为st-2的转置矩阵，I为单位矩阵；
yt-2如下：
Figure FDA00023870356700000212
其中，
Figure FDA00023870356700000213
为t-2时刻从b系到n系的加速度旋转四元数，
Figure FDA00023870356700000214
为t-3时刻从b系到n系的加速度旋转四元数，
Figure FDA0002387035670000031
为t-2时刻从b系到n系的加速度旋转四元数的梯度，
Figure FDA0002387035670000032
为t-3时刻从b系到n系的加速度旋转四元数的梯度；
st-2如下：
Figure FDA0002387035670000033
7.根据权利要求6所述的基于捷联式惯性导航系统的空间轨迹重建方法，其特征在于，通过如下公式计算所述t-1时刻增量参数：
Figure FDA0002387035670000034
其中，pt-1为t-1时刻增量参数，
Figure FDA0002387035670000035
为t-1时刻从b系到n系的加速度旋转四元数，
Figure FDA0002387035670000036
为
Figure FDA0002387035670000037
的梯度。
8.根据权利要求7所述的基于捷联式惯性导航系统的空间轨迹重建方法，其特征在于，所述预设条件如下：
Figure FDA0002387035670000038
其中，
Figure FDA0002387035670000039
为t-1时刻从b系到n系的惯性旋转四元数，αt-2为t-2时刻标准步长，pt-1为t-1时刻增量参数，ng为n系的重力场，bat-1为t-1时刻b系的加速度，c为预设参数，0＜c＜0.5，
Figure FDA00023870356700000310
为pt-1的转置矩阵，
Figure FDA00023870356700000311
为
Figure FDA00023870356700000312
的梯度；
Figure FDA00023870356700000313
如下：
Figure FDA00023870356700000314
Figure FDA00023870356700000315
如下：
Figure FDA00023870356700000316
Figure FDA00023870356700000321
为四元数的Hamilton乘积。
9.根据权利要求1所述的基于捷联式惯性导航系统的空间轨迹重建方法，其特征在于，通过如下公式计算t时刻从b系到n系的惯性旋转四元数：
Figure FDA00023870356700000317
其中，
Figure FDA00023870356700000318
为t时刻从b系到n系的惯性旋转四元数，
Figure FDA00023870356700000319
为t时刻从b系到n系的角速度旋转四元数，
Figure FDA00023870356700000320
为t时刻从b系到n系的加速度旋转四元数；
λt如下：
Figure FDA0002387035670000041
其中，
Figure FDA0002387035670000042
为t-1时刻从b系到n系的加速度旋转四元数，
Figure FDA0002387035670000043
为t-1时刻从b系到n系的角速度旋转四元数。
10.根据权利要求1所述的基于捷联式惯性导航系统的空间轨迹重建方法，其特征在于，通过如下公式计算t时刻从b系到n系的姿态矩阵：
Figure FDA0002387035670000044
Figure FDA0002387035670000045
其中，
Figure FDA0002387035670000046
为t时刻从b系到n系的姿态矩阵，
Figure FDA0002387035670000047
为t时刻从b系到n系的惯性旋转四元数。
11.根据权利要求1所述的基于捷联式惯性导航系统的空间轨迹重建方法，其特征在于，通过如下公式计算t时刻n系的角速度：
Figure FDA0002387035670000048
其中，nωt为t时刻n系的角速度，
Figure FDA0002387035670000049
为t时刻从b系到n系的姿态矩阵，bωt为t时刻b系的角速度。
12.根据权利要求1所述的基于捷联式惯性导航系统的空间轨迹重建方法，其特征在于，通过如下公式计算t时刻n系的加速度：
Figure FDA00023870356700000410
其中，nat为t时刻n系的加速度，
Figure FDA00023870356700000411
为t时刻从b系到n系的姿态矩阵，bat为t时刻b系的加速度。
13.根据权利要求1所述的基于捷联式惯性导航系统的空间轨迹重建方法，其特征在于，所述已构建的卷积神经网络模型按照如下方式构建：
构建训练样本和验证样本，所述训练样本和验证样本均包括t’时刻n系的角速度、t’时刻n系的加速度和t’时刻增量量化值；
根据所述训练样本对卷积神经网络模型进行训练，得到经过训练的卷积神经网络模型；
根据所述验证样本对所述经过训练的卷积神经网络模型进行验证，若所述经过训练的卷积神经网络模型符合预设标准，则所述经过训练的卷积神经网络模型为已构建的卷积神经网络模型；若不符合，则利用所述训练样本对所述经过训练的卷积神经网络模型进行重新训练，直至所述经过训练的卷积神经网络模型符合预设标准为止。
14.根据权利要求13所述的基于捷联式惯性导航系统的空间轨迹重建方法，其特征在于，所述t’时刻增量量化值按照如下方式确定：
根据t’时刻n系的金标准加速度，t’时刻n系的加速度和量化精度，计算t’时刻增量量化值。
15.根据权利要求14所述的基于捷联式惯性导航系统的空间轨迹重建方法，其特征在于，所述t’时刻增量量化值按照如下公式确定：
Figure FDA0002387035670000051
Figure FDA0002387035670000052
其中，yt'为t’时刻增量量化值，
Figure FDA0002387035670000053
为t’时刻n系的金标准加速度，nat'为t’时刻n系的加速度，K为量化精度，κ为金标准与测量值的界限。
16.根据权利要求13所述的基于捷联式惯性导航系统的空间轨迹重建方法，其特征在于，所述卷积神经网络包括：
多个第一卷积层、多个第二卷积层和全连接层；
所述第一卷积层采用1×wl维卷积窗，所述第二卷积层采用3×wl维卷积窗，所述全连接层有K+1个端口，其中，wl为第一卷积层的卷积窗宽度，K为量化精度。
17.根据权利要求1所述的基于捷联式惯性导航系统的空间轨迹重建方法，其特征在于，通过如下公式计算矫正后的t时刻n系的加速度：
Figure FDA0002387035670000054
其中，
Figure FDA0002387035670000055
为矫正后的t时刻n系的加速度，nat为t时刻n系的加速度，yt为t时刻增量量化值，K为量化精度，κ为金标准与测量值的界限。
18.根据权利要求1所述的基于捷联式惯性导航系统的空间轨迹重建方法，其特征在于，所述根据所述矫正后的t时刻n系的加速度构建空间轨迹，包括：
根据所述矫正后的t时刻n系的加速度计算t时刻n系的速度；
根据所述t时刻n系的速度计算t时刻n系的位移；
根据所述t时刻n系的位移构建空间轨迹。
19.根据权利要求18所述的基于捷联式惯性导航系统的空间轨迹重建方法，其特征在于，通过如下公式计算t时刻n系的速度：
Figure FDA0002387035670000061
其中，nvt为t时刻n系的速度，
Figure FDA0002387035670000062
为矫正后的t时刻n系的加速度。
20.根据权利要求18所述的基于捷联式惯性导航系统的空间轨迹重建方法，其特征在于，通过如下公式计算t时刻n系的位移：
Figure FDA0002387035670000063
其中，nst为t时刻n系的位移，nvt为t时刻n系的速度，nvt-1为t-1时刻n系的速度。
21.根据权利要求1所述的基于捷联式惯性导航系统的空间轨迹重建方法，其特征在于，还包括：
根据所述t时刻从b系到n系的姿态矩阵计算t时刻从b系到n系的姿态逆矩阵；
根据矫正后的t时刻n系的加速度和所述t时刻从b系到n系的姿态逆矩阵计算矫正后的t时刻b系的加速度；
根据矫正后的t时刻b系的加速度和所述t时刻b系的加速度计算加速度预测偏差；
根据所述t时刻b系的加速度和所述加速度预测偏差计算t+1时刻b系的加速度。
22.一种基于捷联式惯性导航系统的空间轨迹重建装置，其特征在于，包括：
获得模块，用于获得t时刻b系的角速度和t时刻b系的加速度，其中，b系为载体坐标系，t≥4；
角速度和加速度计算模块，用于根据所述t时刻b系的角速度和所述t时刻b系的加速度，得到t时刻n系的角速度和t时刻n系的加速度，其中，n系为惯性导航坐标系；
预测样本构建模块，用于根据所述t时刻n系的角速度和所述t时刻n系的加速度构建预测样本；
增量量化值模块，用于将所述预测样本输入已构建的卷积神经网络模型中进行深度学习，得到t时刻增量量化值；
n系加速度矫正模块，用于根据所述t时刻n系的加速度和所述t时刻增量量化值，计算矫正后的t时刻n系的加速度；
空间轨迹构建模块，用于根据所述矫正后的t时刻n系的加速度构建空间轨迹；
所述角速度和加速度计算模块具体包括：
角速度旋转四元数单元，用于根据t-1时刻从b系到n系的惯性旋转四元数和t时刻b系的角速度，计算t时刻从b系到n系的角速度旋转四元数；
加速度旋转四元数单元，用于根据所述t-1时刻从b系到n系的惯性旋转四元数、t-1时刻标准步长和t-1时刻增量参数，计算t时刻从b系到n系的加速度旋转四元数；
惯性旋转四元数单元，用于根据所述t时刻从b系到n系的角速度旋转四元数和所述t时刻从b系到n系的加速度旋转四元数，计算t时刻从b系到n系的惯性旋转四元数；
姿态矩阵单元，用于根据所述t时刻从b系到n系的惯性旋转四元数，计算t时刻从b系到n系的姿态矩阵；
角速度单元，用于根据所述t时刻b系的角速度和所述t时刻从b系到n系的姿态矩阵，计算t时刻n系的角速度；
加速度单元，用于根据所述t时刻b系的加速度和所述t时刻从b系到n系的姿态矩阵，计算t时刻n系的加速度。
23.根据权利要求22所述的基于捷联式惯性导航系统的空间轨迹重建装置，其特征在于，所述加速度旋转四元数单元具体用于：
根据t-2时刻从b系到n系的加速度旋转四元数和t-3时刻从b系到n系的加速度旋转四元数，计算t-1时刻对称正定矩阵；
根据所述t-1时刻对称正定矩阵和t-1时刻从b系到n系的加速度旋转四元数，计算所述t-1时刻增量参数。
24.根据权利要求23所述的基于捷联式惯性导航系统的空间轨迹重建装置，其特征在于，所述加速度旋转四元数单元具体用于：
执行迭代处理：根据所述t-1时刻从b系到n系的惯性旋转四元数和所述t-1时刻增量参数，判断t-2时刻标准步长是否满足预设条件；当所述t-2时刻标准步长满足所述预设条件时，将所述t-2时刻标准步长设置为所述t-1时刻标准步长；当所述t-2时刻标准步长不满足所述预设条件时，将所述t-2时刻标准步长乘以预设常数得到t-2时刻第j+1个标准步长，将所述t-2时刻标准步长设置为所述t-2时刻第j+1个标准步长，j为迭代次数。
25.根据权利要求22所述的基于捷联式惯性导航系统的空间轨迹重建装置，其特征在于，还包括：
训练样本和验证样本构建模块，用于构建训练样本和验证样本，所述训练样本和验证样本均包括t’时刻n系的角速度、t’时刻n系的加速度和t’时刻增量量化值；
训练和验证模块，用于根据所述训练样本对卷积神经网络模型进行训练，得到经过训练的卷积神经网络模型；根据所述验证样本对所述经过训练的卷积神经网络模型进行验证，若所述经过训练的卷积神经网络模型符合预设标准，则所述经过训练的卷积神经网络模型为已构建的卷积神经网络模型；若不符合，则利用所述训练样本对所述经过训练的卷积神经网络模型进行重新训练，直至所述经过训练的卷积神经网络模型符合预设标准为止。
26.根据权利要求25所述的基于捷联式惯性导航系统的空间轨迹重建装置，其特征在于，还包括：
增量量化值计算模块，用于根据t’时刻n系的金标准加速度，t’时刻n系的加速度和量化精度，计算t’时刻增量量化值。
27.根据权利要求25所述的基于捷联式惯性导航系统的空间轨迹重建装置，其特征在于，所述卷积神经网络包括：
多个第一卷积层、多个第二卷积层和全连接层；
所述第一卷积层采用1×wl维卷积窗，所述第二卷积层采用3×wl维卷积窗，所述全连接层有K+1个端口，其中，wl为第一卷积层的卷积窗宽度，K为量化精度。
28.根据权利要求22所述的基于捷联式惯性导航系统的空间轨迹重建装置，其特征在于，所述空间轨迹构建模块具体包括：
速度单元，用于根据所述矫正后的t时刻n系的加速度计算t时刻n系的速度；
位移单元，用于根据所述t时刻n系的速度计算t时刻n系的位移；
空间轨迹单元，用于根据所述t时刻n系的位移构建空间轨迹。
29.根据权利要求22所述的基于捷联式惯性导航系统的空间轨迹重建装置，其特征在于，还包括：
姿态逆矩阵模块，用于根据所述t时刻从b系到n系的姿态矩阵计算t时刻从b系到n系的姿态逆矩阵；
b系加速度矫正模块，用于根据矫正后的t时刻n系的加速度和所述t时刻从b系到n系的姿态逆矩阵计算矫正后的t时刻b系的加速度；
加速度预测偏差模块，用于根据矫正后的t时刻b系的加速度和所述t时刻b系的加速度计算加速度预测偏差；
加速度预测模块，用于根据所述t时刻b系的加速度和所述加速度预测偏差计算t+1时刻b系的加速度。
30.一种计算机设备，包括存储器、处理器及存储在存储器上并在处理器上运行的计算机程序，其特征在于，所述处理器执行所述计算机程序时实现以下步骤：
获得t时刻b系的角速度和t时刻b系的加速度，其中，b系为载体坐标系，t≥4；
根据所述t时刻b系的角速度和所述t时刻b系的加速度，得到t时刻n系的角速度和t时刻n系的加速度，其中，n系为惯性导航坐标系；
根据所述t时刻n系的角速度和所述t时刻n系的加速度构建预测样本；
将所述预测样本输入已构建的卷积神经网络模型中进行深度学习，得到t时刻增量量化值；
根据所述t时刻n系的加速度和所述t时刻增量量化值，计算矫正后的t时刻n系的加速度；
根据所述矫正后的t时刻n系的加速度构建空间轨迹；
所述根据所述t时刻b系的角速度和所述t时刻b系的加速度，得到t时刻n系的角速度和t时刻n系的加速度，包括：
根据t-1时刻从b系到n系的惯性旋转四元数和t时刻b系的角速度，计算t时刻从b系到n系的角速度旋转四元数；
根据所述t-1时刻从b系到n系的惯性旋转四元数、t-1时刻标准步长和t-1时刻增量参数，计算t时刻从b系到n系的加速度旋转四元数；
根据所述t时刻从b系到n系的角速度旋转四元数和所述t时刻从b系到n系的加速度旋转四元数，计算t时刻从b系到n系的惯性旋转四元数；
根据所述t时刻从b系到n系的惯性旋转四元数，计算t时刻从b系到n系的姿态矩阵；
根据所述t时刻b系的角速度和所述t时刻从b系到n系的姿态矩阵，计算t时刻n系的角速度；
根据所述t时刻b系的加速度和所述t时刻从b系到n系的姿态矩阵，计算t时刻n系的加速度。
31.一种计算机可读存储介质，其上存储有计算机程序，其特征在于，所述计算机程序被处理器执行时实现以下步骤：
获得t时刻b系的角速度和t时刻b系的加速度，其中，b系为载体坐标系，t≥4；
根据所述t时刻b系的角速度和所述t时刻b系的加速度，得到t时刻n系的角速度和t时刻n系的加速度，其中，n系为惯性导航坐标系；
根据所述t时刻n系的角速度和所述t时刻n系的加速度构建预测样本；
将所述预测样本输入已构建的卷积神经网络模型中进行深度学习，得到t时刻增量量化值；
根据所述t时刻n系的加速度和所述t时刻增量量化值，计算矫正后的t时刻n系的加速度；
根据所述矫正后的t时刻n系的加速度构建空间轨迹；
所述根据所述t时刻b系的角速度和所述t时刻b系的加速度，得到t时刻n系的角速度和t时刻n系的加速度，包括：
根据t-1时刻从b系到n系的惯性旋转四元数和t时刻b系的角速度，计算t时刻从b系到n系的角速度旋转四元数；
根据所述t-1时刻从b系到n系的惯性旋转四元数、t-1时刻标准步长和t-1时刻增量参数，计算t时刻从b系到n系的加速度旋转四元数；
根据所述t时刻从b系到n系的角速度旋转四元数和所述t时刻从b系到n系的加速度旋转四元数，计算t时刻从b系到n系的惯性旋转四元数；
根据所述t时刻从b系到n系的惯性旋转四元数，计算t时刻从b系到n系的姿态矩阵；
根据所述t时刻b系的角速度和所述t时刻从b系到n系的姿态矩阵，计算t时刻n系的角速度；
根据所述t时刻b系的加速度和所述t时刻从b系到n系的姿态矩阵，计算t时刻n系的加速度。
CN201810658605.8A 2018-06-25 2018-06-25 基于捷联式惯性导航系统的空间轨迹重建方法及装置 Active CN108534775B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN201810658605.8A CN108534775B (zh) 2018-06-25 2018-06-25 基于捷联式惯性导航系统的空间轨迹重建方法及装置

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN201810658605.8A CN108534775B (zh) 2018-06-25 2018-06-25 基于捷联式惯性导航系统的空间轨迹重建方法及装置

Publications (2)

Publication Number Publication Date
CN108534775A CN108534775A (zh) 2018-09-14
CN108534775B true CN108534775B (zh) 2020-04-17

Family

ID=63487193

Family Applications (1)

Application Number Title Priority Date Filing Date
CN201810658605.8A Active CN108534775B (zh) 2018-06-25 2018-06-25 基于捷联式惯性导航系统的空间轨迹重建方法及装置

Country Status (1)

Country Link
CN (1) CN108534775B (zh)

Families Citing this family (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN109188026B (zh) * 2018-10-25 2021-03-09 北京航空航天大学 适用于mems加速度计的自动标定的深度学习方法
CN112325881B (zh) * 2020-09-25 2022-04-12 中国船舶重工集团公司第七0七研究所 一种惯导系统姿态解算方法

Citations (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN101055189A (zh) * 2007-05-30 2007-10-17 中国人民解放军国防科学技术大学 基于微惯组测量单元的飞行器姿态测试系统
KR101294623B1 (ko) * 2012-12-28 2013-08-08 한국항공우주연구원 저궤도 영상촬영위성의 향상된 지상 정밀자세결정 방법
CN103884338A (zh) * 2014-02-21 2014-06-25 中国农业大学 空间轨迹实时追踪装置及方法
CN106052584A (zh) * 2016-05-24 2016-10-26 上海工程技术大学 一种基于视觉及惯性信息融合的轨道空间线形测量方法

Family Cites Families (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US8645063B2 (en) * 2010-12-22 2014-02-04 Custom Sensors & Technologies, Inc. Method and system for initial quaternion and attitude estimation
CN105607760A (zh) * 2015-12-18 2016-05-25 上海开圣影视文化传媒股份有限公司 一种基于微惯性传感器的轨迹还原方法及系统

Patent Citations (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN101055189A (zh) * 2007-05-30 2007-10-17 中国人民解放军国防科学技术大学 基于微惯组测量单元的飞行器姿态测试系统
KR101294623B1 (ko) * 2012-12-28 2013-08-08 한국항공우주연구원 저궤도 영상촬영위성의 향상된 지상 정밀자세결정 방법
CN103884338A (zh) * 2014-02-21 2014-06-25 中国农业大学 空间轨迹实时追踪装置及方法
CN106052584A (zh) * 2016-05-24 2016-10-26 上海工程技术大学 一种基于视觉及惯性信息融合的轨道空间线形测量方法

Non-Patent Citations (7)

* Cited by examiner, † Cited by third party
Title
9轴MEMS-IMU实时姿态估算算法;张金艺 等;《上海大学学报（自然科学版）》;20151031;第21卷(第05期);第547-559页 *
Air-to-Air Tracking Performance with Inertial Navigation and Gimballed Radar: A Kinematic Scenario;Vinod K.Saini 等;《IFAC Proceedings Volumes》;20141231;第47卷(第01期);第234-241页 *
In-Motion Initial Alignment for Odometer-Aided Strapdown Inertial Navigation System Based on Attitude Estimation;Lubin Chang 等;《IEEE Sensors Journal 》;20170101;第13卷(第03期);第766-773页 *
Quaternion-based strap-down integration method for applications of inertial sensing to gait analysis;Sabatini, A. M.;《Medical & Biological Engineering & Computing》;20051231;第43卷(第01期);第94-101页 *
SINS/GPS组合导航方法研究;林琳;《中国优秀硕士学位论文全文数据库工程科技Ⅱ辑》;20090215(第02期);第C031-39页 *
基于对偶四元数的捷联惯导算法研究;李贵珍;《中国优秀硕士学位论文全文数据库 工程科技Ⅱ辑》;20120815(第08期);第C032-23页 *
基于捷联惯导的运动轨迹跟踪技术研究;蒋双;《中国优秀硕士学位论文全文数据库信息科技辑》;20170315(第03期);第I136-2465页 *

Also Published As

Publication number Publication date
CN108534775A (zh) 2018-09-14

Similar Documents

Publication Publication Date Title
CN108731670B (zh) 基于量测模型优化的惯性/视觉里程计组合导航定位方法
Ghobadi et al. Robust attitude estimation from uncertain observations of inertial sensors using covariance inflated multiplicative extended Kalman filter
CN108827299B (zh) 一种基于改进四元数二阶互补滤波的飞行器姿态解算方法
US7890291B2 (en) Method and device for detecting a substantially invariant rotation axis
CN103900574B (zh) 一种基于迭代容积卡尔曼滤波姿态估计方法
CN108132053B (zh) 一种行人轨迹构建方法、系统及惯性测量装置
CN108534775B (zh) 基于捷联式惯性导航系统的空间轨迹重建方法及装置
CN110068326B (zh) 姿态计算方法、装置、电子设备以及存储介质
CN109724602A (zh) 一种基于硬件fpu的姿态解算系统及其解算方法
CN108534772B (zh) 姿态角获取方法及装置
CN112665574B (zh) 基于动量梯度下降法的水下机器人姿态采集方法
CN111189474A (zh) 基于mems的marg传感器的自主校准方法
CN111521196B (zh) 传感器校正方法、装置、虚拟现实设备、存储介质
CN110861081B (zh) 一种欠约束索并联机器人末端执行器的自主定位方法
CN106352897A (zh) 一种基于单目视觉传感器的硅mems陀螺误差估计与校正方法
CN109059910B (zh) 基于惯性和肌电信息并结合机器学习的行人导航系统和方法
CN112363196B (zh) 车辆属性确定方法、装置、存储介质和电子设备
Wöhle et al. A robust quaternion based Kalman filter using a gradient descent algorithm for orientation measurement
CN109506674B (zh) 一种加速度的校正方法及装置
CN110375773B (zh) Mems惯导系统姿态初始化方法
CN114147717B (zh) 机器人运动轨迹估计方法、装置、控制器及存储介质
CN114061571B (zh) 一种自适应梯度下降惯性测量单元的姿态解算方法及系统
CN114459478A (zh) 一种基于姿态运动学模型的惯性测量单元数据融合方法
CN114589702A (zh) 基于动力学参数辨识和导纳控制的协作机器人拖动方法
Yang et al. Multi‐Inertial Sensor‐Based Arm 3D Motion Tracking Using Elman Neural Network

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