CN113984051A - Imu与刚体的位姿融合方法、装置、设备及存储介质 - Google Patents

Imu与刚体的位姿融合方法、装置、设备及存储介质 Download PDF

Info

Publication number
CN113984051A
CN113984051A CN202111208097.1A CN202111208097A CN113984051A CN 113984051 A CN113984051 A CN 113984051A CN 202111208097 A CN202111208097 A CN 202111208097A CN 113984051 A CN113984051 A CN 113984051A
Authority
CN
China
Prior art keywords
imu
rigid body
attitude data
measurement
data
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.)
Pending
Application number
CN202111208097.1A
Other languages
English (en)
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.)
Shenzhen Realis Multimedia Technology Co Ltd
Original Assignee
Shenzhen Realis Multimedia 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 Shenzhen Realis Multimedia Technology Co Ltd filed Critical Shenzhen Realis Multimedia Technology Co Ltd
Priority to CN202111208097.1A priority Critical patent/CN113984051A/zh
Publication of CN113984051A publication Critical patent/CN113984051A/zh
Pending legal-status Critical Current

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
    • G01C21/18—Stabilised platforms, e.g. by gyroscope
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C9/00—Measuring inclination, e.g. by clinometers, by levels
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01P—MEASURING LINEAR OR ANGULAR SPEED, ACCELERATION, DECELERATION, OR SHOCK; INDICATING PRESENCE, ABSENCE, OR DIRECTION, OF MOVEMENT
    • G01P15/00—Measuring acceleration; Measuring deceleration; Measuring shock, i.e. sudden change of acceleration
    • G01P15/18—Measuring acceleration; Measuring deceleration; Measuring shock, i.e. sudden change of acceleration in two or more dimensions

Landscapes

  • Engineering & Computer Science (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Automation & Control Theory (AREA)
  • Length Measuring Devices With Unspecified Measuring Means (AREA)
  • Length Measuring Devices By Optical Means (AREA)

Abstract

本发明涉及测量领域，公开了一种IMU与刚体的位姿融合方法、装置、设备及存储介质，用于解决在测量刚体的位姿数据时，刚体的测量位姿数据与真实位姿数据之间误差大的问题。IMU与刚体的位姿融合方法包括：确定惯性测量单元IMU与刚体之间的位置关系；当刚体携带IMU运动时，获取IMU的测量姿态数据，并通过IMU的测量姿态数据计算得到IMU的待融合姿态数据：在预置的世界坐标系中，获取刚体的测量姿态数据，并根据IMU的测量姿态数据、刚体的测量姿态数据以及IMU与刚体之间的位置关系确定刚体的候选姿态数据；利用融合算法将IMU的待融合姿态数据与刚体的候选姿态数据融合，得到刚体的融合位姿数据。

Description

IMU与刚体的位姿融合方法、装置、设备及存储介质
技术领域
本发明涉及测量领域，尤其涉及一种IMU与刚体的位姿融合方法、装置、设备及存储介质。
背景技术
惯性测量单元(Inertial measurement unit，IMU)，测量物体三轴姿态角(或角速率)以及加速度的装置。一般的，一个IMU内设置有三个轴向的陀螺仪和三个方向的加速度计，分别用来测量刚体在三维空间中的角速度和加速度，并以此计算出刚体的姿态，因此，IMU的使用范围非常广，从宇航，到汽车及消费电子领域中都有IMU的身影。此外，IMU具有高频率数据采集能力，例如：亚德诺半导体(ADI)ADIS16系列的采集频率能够达到200Hz，部分型号甚至达到400Hz采集频率。在有些应用场景下需要测量刚体高速运动时的位姿数据，此时可以用光学定位装置和IMU融合方式。
但在具体的实际应用场景下，当刚体高速运动时如果某些摄像装置没有捕捉到刚体的位置或仅仅捕捉到刚体的偏移位置，这样得到刚体的测量位姿数据与真实位姿数据之间的误差偏大，无法得到精确的刚体数据。
发明内容
本发明的主要目的在于解决在测量刚体的位姿数据时，刚体的测量位姿数据与真实位姿数据之间误差大的问题。
本发明第一方面提供了一种IMU与刚体的位姿融合方法，包括：确定惯性测量单元IMU与刚体之间的位置关系；当所述刚体携带所述IMU运动时，获取所述IMU的测量姿态数据，并通过所述IMU的测量姿态数据计算得到所述IMU的待融合姿态数据；在预置的世界坐标系中，获取刚体的测量姿态数据，并根据所述IMU的测量姿态数据、所述刚体的测量姿态数据以及所述IMU与刚体之间的位置关系确定刚体的候选姿态数据；利用融合算法将所述IMU的待融合姿态数据与所述刚体的候选姿态数据融合，得到刚体的融合位姿数据。
可选的，在本发明第一方面的第一种实现方式中，所述当所述刚体携带所述IMU运动时，获取所述IMU的测量姿态数据，并通过所述IMU的测量姿态数据计算得到所述IMU的待融合姿态数据包括：获取IMU的标准姿态数据，所述标准位姿态据包括标准加速度偏置、标准角速度偏置、标准加速度噪声及标准角速度噪声；当所述刚体携带所述IMU运动时，获取所述IMU的测量姿态数据，并根据所述IMU的测量姿态数据以及所述标准姿态数据确定待融合姿态数据，其中，所述待融合姿态数据包括待融合加速度以及待融合角速度。
可选的，在本发明第一方面的第二种实现方式中，所述当所述刚体携带所述IMU运动时，获取所述IMU的测量姿态数据，并根据所述IMU的测量姿态数据以及所述标准姿态数据确定待融合姿态数据，其中，所述待融合姿态数据包括待融合加速度以及待融合角速度包括：获取所述刚体携带所述IMU运动时所述IMU的测量姿态数据，其中，所述IMU的测量姿态数据包括测量加速度及测量角速度；根据第一预置公式、所述标准姿态数据及所述IMU的测量姿态数据计算待融合加速度，所述第一预置公式为：
Figure BDA0003305978760000021
其中，at表示测量加速度，at(real)表示待融合加速度，
Figure BDA0003305978760000022
表示t时刻的标准加速度偏置，
Figure BDA0003305978760000023
表示t时刻IMU姿态下重力方向分量，na为标准加速度噪声；根据第二预置公式、所述标准姿态数据及所述IMU的测量姿态数据计算待融合角速度，所述第二预置公式为：
Figure BDA0003305978760000024
其中，wt表示测量角速度，wt(real)表示待融合角速度，
Figure BDA0003305978760000025
表示t时刻的标准角速度偏置，nw表示标准加速度噪声；将所述待融合加速度以及所述待融合角速度合并，得到待融合姿态数据。
可选的，在本发明第一方面的第三种实现方式中，所述在预置的世界坐标系中，获取刚体的测量姿态数据，并根据所述IMU的测量姿态数据、所述刚体的测量姿态数据以及所述IMU与刚体之间的位置关系确定刚体的候选姿态数据包括：获取刚体的测量姿态数据及所述IMU的测量姿态数据，在所述IMU的测量姿态数据中提取第0帧IMU测量姿态数据以及第n帧IMU测量姿态数据，在所述刚体的测量姿态数据中提取第0帧刚体到世界的测量姿态数据以及第n帧刚体到世界的测量姿态数据；在预置的世界坐标系中，根据关系等式，计算第0帧IMU到世界的测量姿态数据，以及所述IMU从IMU坐标到刚体坐标的旋转矩阵，其中，所述关系等式为：
Figure BDA0003305978760000031
其中，
Figure BDA0003305978760000032
表示第n帧刚体到世界的测量姿态数据，
Figure BDA0003305978760000033
表示IMU测量姿态数据从IMU坐标到刚体坐标的旋转矩阵，[Ri]n表示第n帧IMU测量姿态数据，
Figure BDA0003305978760000034
表示第0帧刚体到世界的测量姿态数据，[Ri]0表示第0帧IMU测量姿态数据，
Figure BDA0003305978760000035
表示第0帧IMU到世界的测量姿态数据，
Figure BDA0003305978760000036
表示第n帧IMU测量姿态数据相对于第0帧IMU测量姿态数据的逆矩阵，E用于指示单位矩阵；根据姿态转换公式将IMU的姿态数据转化为刚体的姿态数据，得到刚体的候选姿态数据，所述姿态转换公式为：
Figure BDA0003305978760000037
其中，
Figure BDA0003305978760000038
表示经过转换后刚体的候选姿态数据，
Figure BDA0003305978760000039
表示第0帧IMU到世界的测量姿态数据，
Figure BDA00033059787600000310
表示第n帧IMU测量姿态数据相对于第0帧IMU测量姿态数据的逆矩阵，
Figure BDA00033059787600000311
表示IMU测量姿态数据从IMU坐标到刚体坐标的旋转矩阵的逆变换。
可选的，在本发明第一方面的第四种实现方式中，所述利用融合算法将所述IMU的待融合姿态数据与所述刚体的候选姿态数据融合，得到刚体的融合位姿数据包括：提取所述刚体的候选姿态数据中的姿态四元数，并分别获取刚体的候选位置数据以及候选位移数据、IMU的姿态四元数及IMU从IMU坐标到刚体坐标的旋转矩阵；根据融合算法中的位置融合公式、IMU的测量姿态数据、所述IMU的待融合姿态数据计算刚体的融合位置，所述位置融合公式为：
Figure BDA00033059787600000312
其中，
Figure BDA00033059787600000313
表示在k+1时刻时刚体位于预置的世界坐标系中的候选位置，
Figure BDA00033059787600000314
表示在k时刻时刚体位于预置的世界坐标系中的候选位置，α为位置融合系数，
Figure BDA00033059787600000315
表示IMU从IMU坐标到刚体坐标的旋转矩阵，
Figure BDA00033059787600000316
表示在k时刻时刚体的候选位移，Δtk表示时间间隔，Δt＝tk+1-tk，at表示测量加速度，
Figure BDA00033059787600000317
表示k时刻IMU的速度，
Figure BDA00033059787600000318
Figure BDA00033059787600000319
表示t时刻的标准加速度偏置，na为标准加速度噪声，
Figure BDA00033059787600000320
表示t时刻IMU姿态下重力方向分量，
Figure BDA00033059787600000321
表示将IMU的速度和待融合加速度投影到预置的世界坐标系下时，[tk，tk+1]时间段中待融合加速度产生的位移量；根据融合算法中的姿态融合公式、IMU的测量姿态数据、待融合姿态数据计算刚体的融合姿态，所述姿态融合公式为：
Figure BDA0003305978760000041
其中，
Figure BDA0003305978760000042
表示在k+1时刻时刚体位于预置的世界坐标系中的姿态四元数，
Figure BDA0003305978760000043
表示在k时刻时刚体位于预置的世界坐标系中的姿态四元数，
Figure BDA0003305978760000044
表示四元数的乘法计算符号，β表示姿态融合系数，
Figure BDA0003305978760000045
表示IMU从IMU坐标到刚体坐标的旋转矩阵，
Figure BDA0003305978760000046
为k时刻IMU的姿态四元数，wt表示测量角速度，
Figure BDA0003305978760000047
表示t时刻的标准角速度偏置，nw表示标准加速度噪声，
Figure BDA0003305978760000048
表示[tk，tk+1]时段中IMU产生的旋转变化；将所述刚体的融合位置与所述刚体的融合姿态合并，确定刚体的融合位姿数据。
本发明第二方面提供了一种IMU与刚体的位姿融合装置，包括：确定模块，用于确定惯性测量单元IMU与刚体之间的位置关系；获取并计算模块，当所述刚体携带所述IMU运动时，用于获取所述IMU的测量姿态数据，并通过所述IMU的测量姿态数据计算得到所述IMU的待融合姿态数据；获取并确定模块，用于在预置的世界坐标系中，获取刚体的测量姿态数据，并根据所述IMU的测量姿态数据、所述刚体的测量姿态数据以及所述IMU与刚体之间的位置关系确定刚体的候选姿态数据；融合模块，用于利用融合算法将所述IMU的待融合姿态数据与所述刚体的候选姿态数据融合，得到刚体的融合位姿数据。
可选的，在本发明第二方面的第一种实现方式中，所述获取并计算模块包括：获取单元，用于获取IMU的标准姿态数据，所述标准位姿态据包括标准加速度偏置、标准角速度偏置、标准加速度噪声及标准角速度噪声；确定单元，当所述刚体携带所述IMU运动时，用于获取所述IMU的测量姿态数据，并根据所述IMU的测量姿态数据以及所述标准姿态数据确定待融合姿态数据，其中，所述待融合姿态数据包括待融合加速度以及待融合角速度。
可选的，在本发明第二方面的第二种实现方式中，所述确定单元具体用于：获取所述刚体携带所述IMU运动时所述IMU的测量姿态数据，其中，所述IMU的测量姿态数据包括测量加速度及测量角速度；根据第一预置公式、所述标准姿态数据及所述IMU的测量姿态数据计算待融合加速度，所述第一预置公式为：
Figure BDA0003305978760000049
其中，at表示测量加速度，at(real)表示待融合加速度，
Figure BDA00033059787600000410
表示t时刻的标准加速度偏置，
Figure BDA00033059787600000411
表示t时刻IMU姿态下重力方向分量，na为标准加速度噪声；根据第二预置公式、所述标准姿态数据及所述IMU的测量姿态数据计算待融合角速度，所述第二预置公式为：
Figure BDA0003305978760000051
其中，wt表示测量角速度，wt(real)表示待融合角速度，
Figure BDA0003305978760000052
表示t时刻的标准角速度偏置，nw，表示标准加速度噪声；将所述待融合加速度以及所述待融合角速度合并，得到待融合姿态数据。
可选的，在本发明第二方面的第三种实现方式中，所述获取并确定模块具体用于：获取刚体的测量姿态数据及所述IMU的测量姿态数据，在所述IMU的测量姿态数据中提取第0帧IMU测量姿态数据以及第n帧IMU测量姿态数据，在所述刚体的测量姿态数据中提取第0帧刚体到世界的测量姿态数据以及第n帧刚体到世界的测量姿态数据；在预置的世界坐标系中，根据关系等式，计算第0帧IMU到世界的测量姿态数据，以及所述IMU从IMU坐标到刚体坐标的旋转矩阵，其中，所述关系等式为：
Figure BDA0003305978760000053
其中，
Figure BDA0003305978760000054
表示第n帧刚体到世界的测量姿态数据，
Figure BDA0003305978760000055
表示IMU测量姿态数据从IMU坐标到刚体坐标的旋转矩阵，[Ri]n表示第n帧IMU测量姿态数据，
Figure BDA0003305978760000056
表示第0帧刚体到世界的测量姿态数据，[Ri]0表示第0帧IMU测量姿态数据，
Figure BDA0003305978760000057
表示第0帧IMU到世界的测量姿态数据，
Figure BDA0003305978760000058
表示第n帧IMU测量姿态数据相对于第0帧IMU测量姿态数据的逆矩阵，E用于指示单位矩阵；根据姿态转换公式将IMU的姿态数据转化为刚体的姿态数据，得到刚体的候选姿态数据，所述姿态转换公式为：
Figure BDA0003305978760000059
其中，
Figure BDA00033059787600000510
表示经过转换后刚体的候选姿态数据，
Figure BDA00033059787600000511
表示第0帧IMU到世界的测量姿态数据，
Figure BDA00033059787600000512
表示第n帧IMU测量姿态数据相对于第0帧IMU测量姿态数据的逆矩阵，
Figure BDA00033059787600000513
表示IMU测量姿态数据从IMU坐标到刚体坐标的旋转矩阵的逆变换。
可选的，在本发明第二方面的第四种实现方式中，所述获取并确定模块具体用于：提取所述刚体的候选姿态数据中的姿态四元数，并分别获取刚体的候选位置数据以及候选位移数据、IMU的姿态四元数及IMU从IMU坐标到刚体坐标的旋转矩阵；根据融合算法中的位置融合公式、IMU的测量姿态数据、所述IMU的待融合姿态数据计算刚体的融合位置，所述位置融合公式为：
Figure BDA00033059787600000514
其中，
Figure BDA0003305978760000061
表示在k+1时刻时刚体位于预置的世界坐标系中的候选位置，
Figure BDA0003305978760000062
表示在k时刻时刚体位于预置的世界坐标系中的候选位置，α为位置融合系数，
Figure BDA0003305978760000063
表示IMU从IMU坐标到刚体坐标的旋转矩阵，
Figure BDA0003305978760000064
表示在k时刻时刚体的候选位移，Δtk表示时间间隔，Δt＝tk+1-tk，at表示测量加速度，
Figure BDA0003305978760000065
表示k时刻IMU的速度，
Figure BDA0003305978760000066
Figure BDA0003305978760000067
表示t时刻的标准加速度偏置，na为标准加速度噪声，
Figure BDA0003305978760000068
表示t时刻IMU姿态下重力方向分量，
Figure BDA0003305978760000069
表示将IMU的速度和待融合加速度投影到预置的世界坐标系下时，[tk，tk+1]时间段中待融合加速度产生的位移量；根据融合算法中的姿态融合公式、IMU的测量姿态数据、待融合姿态数据计算刚体的融合姿态，所述姿态融合公式为：
Figure BDA00033059787600000610
其中，
Figure BDA00033059787600000611
表示在k+1时刻时刚体位于预置的世界坐标系中的姿态四元数，
Figure BDA00033059787600000612
表示在k时刻时刚体位于预置的世界坐标系中的姿态四元数，
Figure BDA00033059787600000613
表示四元数的乘法计算符号，β表示姿态融合系数，
Figure BDA00033059787600000614
表示IMU从IMU坐标到刚体坐标的旋转矩阵，
Figure BDA00033059787600000615
为k时刻IMU的姿态四元数，wt表示测量角速度，
Figure BDA00033059787600000616
表示t时刻的标准角速度偏置，nw表示标准加速度噪声，
Figure BDA00033059787600000617
表示[tk，tk+1]时段中IMU产生的旋转变化；将所述刚体的融合位置与所述刚体的融合姿态合并，确定刚体的融合位姿数据。
本发明第三方面提供了一种IMU与刚体的位姿融合设备，包括：存储器和至少一个处理器，所述存储器中存储有指令，所述存储器和所述至少一个处理器通过线路互连；所述至少一个处理器调用所述存储器中的所述指令，以使得所述IMU与刚体的位姿融合设备执行上述的IMU与刚体的位姿融合方法。
本发明的第四方面提供了一种计算机可读存储介质，所述计算机可读存储介质中存储有指令，当其在计算机上运行时，使得计算机执行上述的IMU与刚体的位姿融合方法。
本发明提供的技术方案中，确定惯性测量单元IMU与刚体之间的位置关系；当所述刚体携带所述IMU运动时，获取所述IMU的测量姿态数据，并通过所述IMU的测量姿态数据计算得到IMU的待融合姿态数据；在预置的世界坐标系中，获取刚体的测量姿态数据，并根据所述IMU的测量姿态数据、所述刚体的测量姿态数据以及所述IMU与刚体之间的位置关系确定刚体的候选姿态数据；利用融合算法将所述IMU的待融合姿态数据与所述刚体的候选姿态数据融合，得到刚体的融合位姿数据。本发明实施例中，通过获取刚体携带IMU运动时IMU的测量姿态数据，并根据IMU与刚体之间的转换关系确定刚体的候选姿态数据，最后利用融合函数将IMU的测量姿态数据与刚体的候选姿态数据进行融合，得到刚体的融合位姿数据，减少了刚体的测量位姿数据与真实位姿数据之间的误差，提高了测量位姿数据的精确度。
附图说明
图1为本发明实施例中IMU与刚体的位姿融合方法的一个实施例示意图；
图2为本发明实施例中IMU与刚体的位姿融合方法的另一个实施例示意图；
图3为本发明实施例中IMU与刚体的位姿融合装置的一个实施例示意图；
图4为本发明实施例中IMU与刚体的位姿融合装置的另一个实施例示意图；
图5为本发明实施例中IMU与刚体的位姿融合设备的一个实施例示意图。
具体实施方式
本发明实施例提供了一种IMU与刚体的位姿融合方法、装置、设备及存储介质，通过获取刚体携带IMU运动时IMU的测量姿态数据，并根据IMU与刚体之间的转换关系确定刚体的候选姿态数据，最后利用融合函数将IMU的测量姿态数据与刚体的候选姿态数据进行融合，得到刚体的融合位姿数据，减少了刚体的测量位姿数据与真实位姿数据之间的误差，提高了测量位姿数据的精确度。
本发明的说明书和权利要求书及上述附图中的术语“第一”、“第二”、“第三”、“第四”等(如果存在)是用于区别类似的对象，而不必用于描述特定的顺序或先后次序。应该理解这样使用的数据在适当情况下可以互换，以便这里描述的实施例能够以除了在这里图示或描述的内容以外的顺序实施。此外，术语“包括”或“具有”及其任何变形，意图在于覆盖不排他的包含，例如，包含了一系列步骤或单元的过程、方法、系统、产品或设备不必限于清楚地列出的那些步骤或单元，而是可包括没有清楚地列出的或对于这些过程、方法、产品或设备固有的其它步骤或单元。
为便于理解，下面对本发明实施例的具体流程进行描述，请参阅图1，本发明实施例中IMU与刚体的位姿融合方法的一个实施例包括：
101、确定惯性测量单元IMU与刚体之间的位置关系；
可以理解的是，本发明的执行主体可以为IMU与刚体的位姿融合装置，还可以是终端或者服务器，具体此处不做限定。本发明实施例以服务器为执行主体为例进行说明。
服务器确定惯性测量单元IMU与刚体之间的位置关系。
需要说明的是，惯性测量单元(Inertial measurement unit，IMU)为测量物体三轴姿态角(或角速率)以及加速度的装置。通常一个IMU包含了三个单轴的加速度计和三个单轴的陀螺仪，加速度计分别测量刚体在载体坐标系统独立三个轴向的加速度数据，而陀螺仪则测量载体相对于导航坐标系的角速度数据，由此可测量得到刚体的IMU姿态数据，包括刚体在三维空间中三个轴向上的角速度和加速度。IMU具有不同的型号，每种型号采用的采集频率可能不同，在本申请中不对IMU的型号进行限定。
刚体(rigid body)是一种有限尺寸，不论是否感受到外力都可以忽略形变的固体，这里的忽略形变指的是在刚体的内部，质点与质点之间的距离不会改变。
在检测刚体的位姿数据时，需要将IMU固定在刚体上，当刚体携带IMU运动时，IMU会检测出IMU的测量位姿数据，进而进行后续步骤的操作。这里的刚体携带IMU做的运动可以为匀速直线运动，也可以为变速运动，在本申请中并不对刚体携带IMU所做的运动种类进行限定。
102、当刚体携带IMU运动时，获取IMU的测量姿态数据，并通过IMU的测量姿态数据计算得到IMU的待融合姿态数据；
当刚体携带IMU运动时，服务器获取IMU的测量姿态数据，并通过IMU的测量姿态数据计算得到IMU的待融合姿态数据。
可以理解的是，当刚体携带IMU运动时，IMU可以通过测量得到运动时的加速度以及角速度，并根据IMU原始设置的标准加速度偏置、标准角速度偏置、标准加速度噪声及标准角速度噪声，计算得到IMU的待融合姿态数据。
进一步说明的是，这里的标准加速度偏置、标准角速度偏置、标准加速度噪声及标准角速度噪声均是已知的，一般情况下，IMU硬件制造厂均会对出厂的IMU设备进行测试，并通过大量的测试数据确定IMU的标准加速度偏置、标准角速度偏置、标准加速度噪声及标准角速度噪声，因此，在本申请中，并不对标准加速度偏置、标准角速度偏置、标准加速度噪声及标准角速度噪声的数值进行限定，不同的IMU设有不同的数值。
103、在预置的世界坐标系中，获取刚体的测量姿态数据，并根据IMU的测量姿态数据、刚体的测量姿态数据以及IMU与刚体之间的位置关系确定刚体的候选姿态数据；
服务器在预置的世界坐标系中，获取刚体的测量姿态数据，并根据IMU的测量姿态数据、刚体的测量姿态数据以及IMU与刚体之间的位置关系确定刚体的候选姿态数据。
需要说明的是，因IMU与刚体所处的位置不同，当刚体携带IMU运动时，两者所处的坐标系不同，因此需要对IMU的姿态进行坐标格式的转换，将IMU的姿态转换为刚体的姿态，也就是利用获取到的测量姿态数据计算得到IMU从IMU坐标系中转化到预置世界坐标系中的转化矩阵，实现两者在坐标格式上的标定，由此得到IMU与刚体之间的姿态转化关系，进而将IMU的姿态与刚体的姿态进行融合标定时，可更快速地得到刚体更为精确的位姿数据。
104、利用融合算法将IMU的待融合姿态数据与刚体的候选姿态数据融合，得到刚体的融合位姿数据。
服务器利用融合算法将IMU的待融合姿态数据与刚体的候选姿态数据融合，得到刚体的融合位姿数据。
在服务器获取到IMU的待融合姿态数据以及刚体的候选姿态数据之后，需要进一步的将两者融合，进而得到刚体的融合位姿数据。刚体的融合位姿数据包括刚体的位置数据以及刚体的姿态数据，影响刚体位置数据的因素有刚体的候选位置数据、候选位移数据以及IMU从IMU坐标到刚体坐标的旋转矩阵，根据上述因素以及位置融合公式即可计算得到刚体的位置数据；影响刚体姿态数据的因素有刚体的姿态四元数、IMU从IMU坐标到刚体坐标的旋转矩阵以及IMU的带融合姿态数据，根据上述因素以及姿态融合公式即可计算得到刚体的姿态数据，将刚体的位置数据与刚体的姿态数据相结合，得到刚体的融合位姿数据。
本发明实施例中，通过获取刚体携带IMU运动时IMU的测量姿态数据，并根据IMU与刚体之间的转换关系确定刚体的候选姿态数据，最后利用融合函数将IMU的测量姿态数据与刚体的候选姿态数据进行融合，得到刚体的融合位姿数据，减少了刚体的测量位姿数据与真实位姿数据之间的误差，提高了测量位姿数据的精确度。
请参阅图2，本发明实施例中IMU与刚体的位姿融合方法的另一个实施例包括：
201、确定惯性测量单元IMU与刚体之间的位置关系；
服务器确定惯性测量单元IMU与刚体之间的位置关系。其中，惯性测量单元为测量物体三轴姿态角以及加速度的装置。通常一个IMU包含了三个单轴的加速度计和三个单轴的陀螺仪，加速度计分别测量刚体在载体坐标系统独立三个轴向的加速度数据，而陀螺仪则测量载体相对于导航坐标系的角速度数据，由此可测量得到刚体的IMU姿态数据，包括刚体在三维空间中三个轴向上的角速度和加速度。IMU具有不同的型号，每种型号采用的采集频率可能不同，在本申请中不对IMU的型号进行限定。
刚体是一种有限尺寸，不论是否感受到外力都可以忽略形变的固体，这里的忽略形变指的是在刚体的内部，质点与质点之间的距离不会改变。
在检测刚体的位姿数据时，需要将IMU固定在刚体上，当刚体携带IMU运动时，IMU会显示出IMU的测量位姿数据，进而进行后续步骤的操作。这里的刚体携带IMU做的运动可以为匀速直线运动，也可以为变速运动，在本申请中并不对刚体携带IMU所做的运动种类进行限定。
202、获取IMU的标准姿态数据，标准位姿态据包括标准加速度偏置、标准角速度偏置、标准加速度噪声及标准角速度噪声；
服务器获取IMU的标准姿态数据，标准位姿态据包括标准加速度偏置、标准角速度偏置、标准加速度噪声及标准角速度噪声。
这里IMU的标准姿态数据为已知的参数，其中，标准位姿态据包括：标准加速度偏置、标准角速度偏置、标准加速度噪声及标准角速度噪声，一般情况下，IMU硬件制造厂均会对出厂的IMU设备进行测试，并通过大量的测试数据确定IMU的标准加速度偏置、标准角速度偏置、标准加速度噪声及标准角速度噪声。
203、当刚体携带IMU运动时，获取IMU的测量姿态数据，并IMU的测量姿态数据以及标准姿态数据确定待融合姿态数据，其中，待融合姿态数据包括待融合加速度以及待融合角速度；
服务器当刚体携带IMU运动时，获取IMU的测量姿态数据，并根据IMU的测量姿态数据以及标准姿态数据确定待融合姿态数据，其中，待融合姿态数据包括待融合加速度以及待融合角速度。具体的：
服务器获取刚体携带IMU运动时IMU的测量姿态数据，其中，IMU的测量姿态数据包括测量加速度及测量角速度；服务器根据第一预置公式、标准姿态数据及测量姿态数据计算待融合加速度，第一预置公式为：
Figure BDA0003305978760000111
其中，at表示测量加速度，at(real)表示待融合加速度，
Figure BDA0003305978760000112
表示t时刻的标准加速度偏置，
Figure BDA0003305978760000113
表示t时刻IMU姿态下重力方向分量，na为标准加速度噪声；服务器根据第二预置公式、标准姿态数据及测量姿态数据计算待融合角速度，第二预置公式为：
Figure BDA0003305978760000114
其中，wt表示测量角速度，wt(real)表示待融合角速度，
Figure BDA0003305978760000115
表示t时刻的标准角速度偏置，nw表示标准加速度噪声；服务器将待融合加速度以及待融合角速度合并，得到待融合姿态数据。
在刚体携带IMU做移动运动时，获取IMU的测量姿态数据，这里的测量姿态数据包括IMU输出的测量加速度以及测量角速度，因在刚体携带IMU进行运动时，会受到刚体重量以及加速度噪声的影响，导致测量的加速度变小，因此，服务器通过第一预置公式计算理论上的真实加速度也就是待融合加速度。同理可知，在计算刚体的角速度时，也会受到刚体重量以及角速度噪声的影响，导致测量的角速度变小，因而，服务器通过第二预置公式计算理论上的真实角速度也就是待融合角速度。
根据大量的测量姿态数据发现，标准加速度噪声na及标准加速度噪声nw均服从高斯分布，其中，标准加速度噪声服从的分布为：
Figure BDA0003305978760000116
标准角速度噪声服从的分布为：
Figure BDA0003305978760000117
这里的σa与σw为高斯白噪声的方差，具体的数值由IMU的型号而定。此外，标准加速度偏置
Figure BDA0003305978760000118
及标准角速度偏置
Figure BDA0003305978760000119
的建模为随机游走，其中，标准加速度偏置的随机游走为
Figure BDA00033059787600001110
Figure BDA00033059787600001111
服从高斯分布
Figure BDA00033059787600001112
标准角速度偏置的随机游走为
Figure BDA00033059787600001113
Figure BDA00033059787600001114
服从高斯分布
Figure BDA00033059787600001115
这里的
Figure BDA00033059787600001116
与
Figure BDA00033059787600001117
分别为标准加速度偏置和标准角速度偏置的随机游走高斯白噪声的方差，具体的数值由IMU的型号而定。
可以理解的是，IMU以及刚体未运动，或者运动不明显，此时所获得的测量姿态数据是不变的，可认为是无效的姿态数据，而IMU以及刚体处于运动状态时获得的测量姿态数据才是有效的姿态数据。
204、在预置的世界坐标系中，获取刚体的测量姿态数据，并根据IMU的测量姿态数据、刚体的测量姿态数据以及IMU与刚体之间的位置关系确定刚体的候选姿态数据；
服务器在预置的世界坐标系中，获取刚体的测量姿态数据，并根据IMU的测量姿态数据、刚体的测量姿态数据以及IMU与刚体之间的位置关系确定刚体的候选姿态数据。具体的：
服务器首先获取刚体的测量姿态数据及IMU的测量姿态数据，在IMU的测量姿态数据中提取第0帧IMU测量姿态数据以及第n帧IMU测量姿态数据，在刚体的测量姿态数据中提取第0帧刚体到世界的测量姿态数据以及第n帧刚体到世界的测量姿态数据；然后服务器在预置的世界坐标系中，根据关系等式，计算第0帧IMU到世界的测量姿态数据，以及IMU从IMU坐标到刚体坐标的旋转矩阵，其中，关系等式为：
Figure BDA0003305978760000121
其中，
Figure BDA0003305978760000122
表示第n帧刚体到世界的测量姿态数据，
Figure BDA0003305978760000123
表示IMU测量姿态数据从IMU坐标到刚体坐标的旋转矩阵，[Ri]n表示第n帧IMU测量姿态数据，
Figure BDA0003305978760000124
表示第0帧刚体到世界的测量姿态数据，[Ri]0表示第0帧IMU测量姿态数据，
Figure BDA0003305978760000125
表示第0帧IMU到世界的测量姿态数据，
Figure BDA0003305978760000126
表示第n帧IMU测量姿态数据相对于第0帧IMU测量姿态数据的逆矩阵，E用于指示单位矩阵；最后服务器根据姿态转换公式将IMU的姿态数据转化为刚体的姿态数据，得到刚体的候选姿态数据，姿态转换公式为：
Figure BDA0003305978760000127
其中，
Figure BDA0003305978760000128
表示经过转换后刚体的候选姿态数据，
Figure BDA0003305978760000129
表示第0帧IMU到世界的测量姿态数据，
Figure BDA00033059787600001210
表示第n帧IMU测量姿态数据相对于第0帧IMU测量姿态数据的逆矩阵，
Figure BDA00033059787600001211
表示IMU测量姿态数据从IMU坐标到刚体坐标的旋转矩阵的逆变换。
可以理解的是，在利用刚体的测量姿态数据进行坐标格式的转换时，测量姿态数据中包含了多个帧的测量姿态数据，其中，0帧IMU测量姿态数据为IMU初始测得的位置，n帧IMU测量姿态数据为IMU在第n帧时的位置，第0帧刚体到世界的测量姿态数据为刚体初始测得的位置。服务器通过关系等式计算IMU测量姿态数据从IMU坐标到刚体坐标的旋转矩阵，以及计算第0帧IMU到世界的测量姿态数据，其中，关系等式为：
Figure BDA0003305978760000131
其中，
Figure BDA0003305978760000132
表示第n帧刚体到世界的测量姿态数据，
Figure BDA0003305978760000133
表示IMU测量姿态数据从IMU坐标到刚体坐标的旋转矩阵，[Ri]n[Ri]n表示第n帧IMU测量姿态数据，
Figure BDA0003305978760000134
表示第0帧刚体到世界的测量姿态数据，[Ri]0表示第0帧IMU测量姿态数据，
Figure BDA0003305978760000135
表示第0帧IMU到世界的测量姿态数据，
Figure BDA0003305978760000136
表示第n帧IMU测量姿态数据相对于第0帧IMU测量姿态数据的逆矩阵，E用于指示单位矩阵。
具体的：服务器将第0帧IMU测量姿态数据、第n帧IMU测量姿态数据、第0帧刚体到世界的测量姿态数据以及第n帧刚体到世界的测量姿态数据代入关系等式，并通过最小二乘法，当关系等式的等号两边数据之差的平方和为最小时求得
Figure BDA0003305978760000137
知
Figure BDA0003305978760000138
其中，最小二乘法公式Q包括：
Figure BDA0003305978760000139
进一步的：服务器在计算得到IMU测量姿态数据从IMU坐标到刚体坐标的旋转矩阵，以及第0帧IMU到世界的测量姿态数据后，即可将两者代入姿态转换公式，确定IMU测量姿态数据从IMU坐标转化到刚体坐标时的转换公式，其中，姿态转换公式为：
Figure BDA00033059787600001310
其中，
Figure BDA00033059787600001311
为IMU姿态数据从IMU坐标到刚体坐标的旋转矩阵的逆变换，
Figure BDA00033059787600001312
为经过转换后的刚体的姿态数据。
205、利用融合算法将IMU的待融合姿态数据与刚体的候选姿态数据融合，得到刚体的融合位姿数据。
服务器利用融合算法将IMU的待融合姿态数据与刚体的候选姿态数据融合，得到刚体的融合位姿数据。具体的：
首先服务器提取刚体的候选姿态数据中的姿态四元数，并分别获取刚体的候选位置数据以及候选位移数据、IMU的姿态四元数及IMU从IMU坐标到刚体坐标的旋转矩阵；其次服务器根据融合算法中的位置融合公式、IMU的测量姿态数据、IMU的待融合姿态数据计算刚体的融合位置，位置融合公式为：
Figure BDA0003305978760000141
其中，
Figure BDA0003305978760000142
表示在k+1时刻时刚体位于预置的世界坐标系中的候选位置，
Figure BDA0003305978760000143
表示在k时刻时刚体位于预置的世界坐标系中的候选位置，α为位置融合系数，
Figure BDA0003305978760000144
表示IMU从IMU坐标到刚体坐标的旋转矩阵，
Figure BDA0003305978760000145
表示在k时刻时刚体的候选位移，Δtk表示时间间隔，Δt＝tk+1-tk，at表示测量加速度，
Figure BDA0003305978760000146
表示k时刻IMU的速度，
Figure BDA0003305978760000147
Figure BDA0003305978760000148
表示t时刻的标准加速度偏置，na为标准加速度噪声，
Figure BDA0003305978760000149
表示t时刻IMU姿态下重力方向分量，
Figure BDA00033059787600001410
表示将IMU的速度和待融合加速度投影到预置的世界坐标系下时，[tk，tk+1]时间段中待融合加速度产生的位移量；然后服务器根据融合算法中的姿态融合公式、IMU的测量姿态数据、待融合姿态数据计算刚体的融合姿态，姿态融合公式为：
Figure BDA00033059787600001411
其中，
Figure BDA00033059787600001412
表示在k+1时刻时刚体位于预置的世界坐标系中的姿态四元数，
Figure BDA00033059787600001413
表示在k时刻时刚体位于预置的世界坐标系中的姿态四元数，
Figure BDA00033059787600001414
表示四元数的乘法计算符号，β表示姿态融合系数，
Figure BDA00033059787600001415
表示IMU从IMU坐标到刚体坐标的旋转矩阵，
Figure BDA00033059787600001416
为k时刻IMU的姿态四元数，wt表示测量角速度，
Figure BDA00033059787600001417
表示t时刻的标准角速度偏置，nw表示标准加速度噪声，
Figure BDA00033059787600001418
表示[tk，tk+1]时段中IMU产生的旋转变化；最后服务器将刚体的融合位置与刚体的融合姿态合并，确定刚体的融合位姿数据。
在服务器获取到IMU的待融合姿态数据以及刚体的候选姿态数据之后，需要进一步的将两者融合，进而得到刚体的融合位姿数据。首先服务器先进行刚体的位置融合，服务器通过IMU的测量加速度计算IMU的测量速度，通过对测量加速度at积分即可得到速度，因IMU在进行测量时，有噪声以及重力的影响使得速度值偏小，因此由该公式计算k时刻IMU的速度，该公式为：
Figure BDA00033059787600001419
在式中，at表示测量加速度，
Figure BDA00033059787600001420
表示k时刻IMU的速度，
Figure BDA00033059787600001421
表示t时刻的标准加速度偏置，na为标准加速度噪声，
Figure BDA00033059787600001422
表示t时刻IMU姿态下重力方向分量。将计算得到的
Figure BDA00033059787600001423
以及获取到的测量姿态参数代入位置融合公式中，确定刚体的位姿数据。需要说明的是，α为位置融合系数，即刚体在k时刻的候选位移与IMU在k时刻的测量位移的比例，根据IMU帧间位移积分精度决定取值，在本申请中的取值为0.15。
待服务器得到融合位置数据后，服务器进行姿态数据的融合。服务器首先获取刚体在k时刻时刚体位于预置的世界坐标系中的姿态四元数
Figure BDA0003305978760000151
再获取k时刻IMU的姿态四元数
Figure BDA0003305978760000152
并将两者以及获取到的其他姿态数据代入姿态融合公式中，确定刚体的姿态数据。需要说明的是，β为姿态融合系数，根据光学姿态计算精度和IMU姿态数据精度情况设置，在本发明中的取值为0.75。
本发明实施例中，通过获取刚体携带IMU运动时IMU的测量姿态数据，并根据IMU与刚体之间的转换关系确定刚体的候选姿态数据，最后利用融合函数将IMU的测量姿态数据与刚体的候选姿态数据进行融合，得到刚体的融合位姿数据，减少了刚体的测量位姿数据与真实位姿数据之间的误差，提高了测量位姿数据的精确度。
上面对本发明实施例中IMU与刚体的位姿融合方法进行了描述，下面对本发明实施例中IMU与刚体的位姿融合装置进行描述，请参阅图3，本发明实施例中IMU与刚体的位姿融合装置一个实施例包括：
确定模块301，用于确定惯性测量单元IMU与刚体之间的位置关系；
获取并计算模块302，当刚体携带IMU运动时，用于获取IMU的测量姿态数据，并通过IMU的测量姿态数据计算得到IMU的待融合姿态数据；
获取并确定模块303，用于在预置的世界坐标系中，获取刚体的测量姿态数据，并根据IMU的测量姿态数据、刚体的测量姿态数据以及IMU与刚体之间的位置关系确定刚体的候选姿态数据；
融合模块304，用于利用融合算法将IMU的待融合姿态数据与刚体的候选姿态数据融合，得到刚体的融合位姿数据。
本发明实施例中，通过获取刚体携带IMU运动时IMU的测量姿态数据，并根据IMU与刚体之间的转换关系确定刚体的候选姿态数据，最后利用融合函数将IMU的测量姿态数据与刚体的候选姿态数据进行融合，得到刚体的融合位姿数据，减少了刚体的测量位姿数据与真实位姿数据之间的误差，提高了测量位姿数据的精确度。
请参阅图4，本发明实施例中IMU与刚体的位姿融合装置的另一个实施例包括：
确定模块301，用于确定惯性测量单元IMU与刚体之间的位置关系；
获取并计算模块302，当刚体携带IMU运动时，用于获取IMU的测量姿态数据，并通过IMU的测量姿态数据计算得到IMU的待融合姿态数据；
获取并确定模块303，用于在预置的世界坐标系中，获取刚体的测量姿态数据，并根据IMU的测量姿态数据、刚体的测量姿态数据以及IMU与刚体之间的位置关系确定刚体的候选姿态数据；
融合模块304，用于利用融合算法将IMU的待融合姿态数据与刚体的候选姿态数据融合，得到刚体的融合位姿数据。
可选的，获取并计算模块302包括：
获取单元3021，用于获取IMU的标准姿态数据，标准位姿态据包括标准加速度偏置、标准角速度偏置、标准加速度噪声及标准角速度噪声；
确定单元3022，当刚体携带IMU运动时，用于获取IMU的测量姿态数据，并根据IMU的测量姿态数据以及标准姿态数据确定待融合姿态数据，其中，待融合姿态数据包括待融合加速度以及待融合角速度。
可选的，确定单元3022还可以具体用于：
获取刚体携带IMU运动时IMU的测量姿态数据，其中，IMU的测量姿态数据包括测量加速度及测量角速度；
根据第一预置公式、标准姿态数据及IMU的测量姿态数据计算待融合加速度，第一预置公式为：
Figure BDA0003305978760000161
其中，at表示测量加速度，at(real)表示待融合加速度，
Figure BDA0003305978760000162
表示t时刻的标准加速度偏置，
Figure BDA0003305978760000163
表示t时刻IMU姿态下重力方向分量，na为标准加速度噪声；
根据第二预置公式、标准姿态数据及IMU的测量姿态数据计算待融合角速度，第二预置公式为：
Figure BDA0003305978760000164
其中，wt表示测量角速度，wt(real)表示待融合角速度，
Figure BDA0003305978760000165
表示t时刻的标准角速度偏置，nw表示标准加速度噪声；
将待融合加速度以及待融合角速度合并，得到待融合姿态数据。
可选的，获取并确定模块303还可以具体用于：
获取刚体的测量姿态数据及IMU的测量姿态数据，在IMU的测量姿态数据中提取第0帧IMU测量姿态数据以及第n帧IMU测量姿态数据，在刚体的测量姿态数据中提取第0帧刚体到世界的测量姿态数据以及第n帧刚体到世界的测量姿态数据；
在预置的世界坐标系中，根据关系等式，计算第0帧IMU到世界的测量姿态数据，以及IMU从IMU坐标到刚体坐标的旋转矩阵，其中，关系等式为：
Figure BDA0003305978760000171
其中，
Figure BDA0003305978760000172
表示第n帧刚体到世界的测量姿态数据，
Figure BDA0003305978760000173
表示IMU测量姿态数据从IMU坐标到刚体坐标的旋转矩阵，[Ri]n表示第n帧IMU测量姿态数据，
Figure BDA0003305978760000174
表示第0帧刚体到世界的测量姿态数据，[Ri]0表示第0帧IMU测量姿态数据，
Figure BDA0003305978760000175
表示第0帧IMU到世界的测量姿态数据，
Figure BDA0003305978760000176
表示第n帧IMU测量姿态数据相对于第0帧IMU测量姿态数据的逆矩阵，E用于指示单位矩阵；
根据姿态转换公式将IMU的姿态数据转化为刚体的姿态数据，得到刚体的候选姿态数据，姿态转换公式为：
Figure BDA0003305978760000177
其中，
Figure BDA0003305978760000178
表示经过转换后刚体的候选姿态数据，
Figure BDA0003305978760000179
表示第0帧IMU到世界的测量姿态数据，
Figure BDA00033059787600001710
表示第n帧IMU测量姿态数据相对于第0帧IMU测量姿态数据的逆矩阵，
Figure BDA00033059787600001711
表示IMU测量姿态数据从IMU坐标到刚体坐标的旋转矩阵的逆变换。
可选的，融合模块304还可以具体用于：
提取刚体的候选姿态数据中的姿态四元数，并分别获取刚体的候选位置数据以及候选位移数据、IMU的姿态四元数及IMU从IMU坐标到刚体坐标的旋转矩阵；
根据融合算法中的位置融合公式、IMU的测量姿态数据、IMU的待融合姿态数据计算刚体的融合位置，位置融合公式为：
Figure BDA00033059787600001712
其中，
Figure BDA00033059787600001713
表示在k+1时刻时刚体位于预置的世界坐标系中的候选位置，
Figure BDA00033059787600001714
表示在k时刻时刚体位于预置的世界坐标系中的候选位置，α为位置融合系数，
Figure BDA00033059787600001715
表示IMU从IMU坐标到刚体坐标的旋转矩阵，
Figure BDA00033059787600001716
表示在k时刻时刚体的候选位移，Δtk表示时间间隔，Δt＝tk+1-tk，at表示测量加速度，
Figure BDA00033059787600001717
表示k时刻IMU的速度，
Figure BDA00033059787600001718
Figure BDA00033059787600001719
表示t时刻的标准加速度偏置，na为标准加速度噪声，
Figure BDA00033059787600001720
表示t时刻IMU姿态下重力方向分量，
Figure BDA00033059787600001721
表示将IMU的速度和待融合加速度投影到预置的世界坐标系下时，[tk，tk+1]时间段中待融合加速度产生的位移量；
根据融合算法中的姿态融合公式、IMU的测量姿态数据、待融合姿态数据计算刚体的融合姿态，姿态融合公式为：
Figure BDA0003305978760000181
其中，
Figure BDA0003305978760000182
表示在k+1时刻时刚体位于预置的世界坐标系中的姿态四元数，
Figure BDA0003305978760000183
表示在k时刻时刚体位于预置的世界坐标系中的姿态四元数，
Figure BDA0003305978760000184
表示四元数的乘法计算符号，β表示姿态融合系数，
Figure BDA0003305978760000185
表示IMU从IMU坐标到刚体坐标的旋转矩阵，
Figure BDA0003305978760000186
为k时刻IMU的姿态四元数，wt表示测量角速度，
Figure BDA0003305978760000187
表示t时刻的标准角速度偏置，nw表示标准加速度噪声，
Figure BDA0003305978760000188
表示[tk，tk+1]时段中IMU产生的旋转变化；
将刚体的融合位置与刚体的融合姿态合并，确定刚体的融合位姿数据。
本发明实施例中，通过获取刚体携带IMU运动时IMU的测量姿态数据，并根据IMU与刚体之间的转换关系确定刚体的候选姿态数据，最后利用融合函数将IMU的测量姿态数据与刚体的候选姿态数据进行融合，得到刚体的融合位姿数据，减少了刚体的测量位姿数据与真实位姿数据之间的误差，提高了测量位姿数据的精确度。
上面图3和图4从模块化功能实体的角度对本发明实施例中的IMU与刚体的位姿融合装置进行详细描述，下面从硬件处理的角度对本发明实施例中IMU与刚体的位姿融合设备进行详细描述。
图5是本发明实施例提供的一种IMU与刚体的位姿融合设备的结构示意图，该IMU与刚体的位姿融合设备500可因配置或性能不同而产生比较大的差异，可以包括一个或一个以上处理器(central processing units，CPU)510(例如，一个或一个以上处理器)和存储器520，一个或一个以上存储应用程序533或数据532的存储介质530(例如一个或一个以上海量存储设备)。其中，存储器520和存储介质530可以是短暂存储或持久存储。存储在存储介质530的程序可以包括一个或一个以上模块(图示没标出)，每个模块可以包括对IMU与刚体的位姿融合设备500中的一系列指令操作。更进一步地，处理器510可以设置为与存储介质530通信，在IMU与刚体的位姿融合设备500上执行存储介质530中的一系列指令操作。
IMU与刚体的位姿融合设备500还可以包括一个或一个以上电源540，一个或一个以上有线或无线网络接口550，一个或一个以上输入输出接口560，和/或，一个或一个以上操作系统531，例如Windows Serve，Mac OS X，Unix，Linux，FreeBSD等等。本领域技术人员可以理解，图5示出的IMU与刚体的位姿融合设备结构并不构成对IMU与刚体的位姿融合设备的限定，可以包括比图示更多或更少的部件，或者组合某些部件，或者不同的部件布置。
本发明还提供一种计算机可读存储介质，该计算机可读存储介质可以为非易失性计算机可读存储介质，该计算机可读存储介质也可以为易失性计算机可读存储介质，所述计算机可读存储介质中存储有指令，当所述指令在计算机上运行时，使得计算机执行所述IMU与刚体的位姿融合方法的步骤。
所属领域的技术人员可以清楚地了解到，为描述的方便和简洁，上述描述的系统，装置和单元的具体工作过程，可以参考前述方法实施例中的对应过程，在此不再赘述。
所述集成的单元如果以软件功能单元的形式实现并作为独立的产品销售或使用时，可以存储在一个计算机可读取存储介质中。基于这样的理解，本发明的技术方案本质上或者说对现有技术做出贡献的部分或者该技术方案的全部或部分可以以软件产品的形式体现出来，该计算机软件产品存储在一个存储介质中，包括若干指令用以使得一台计算机设备(可以是个人计算机，服务器，或者网络设备等)执行本发明各个实施例所述方法的全部或部分步骤。而前述的存储介质包括：U盘、移动硬盘、只读存储器(read-only memory，ROM)、随机存取存储器(random access memory，RAM)、磁碟或者光盘等各种可以存储程序代码的介质。
以上所述，以上实施例仅用以说明本发明的技术方案，而非对其限制；尽管参照前述实施例对本发明进行了详细的说明，本领域的普通技术人员应当理解：其依然可以对前述各实施例所记载的技术方案进行修改，或者对其中部分技术特征进行等同替换；而这些修改或者替换，并不使相应技术方案的本质脱离本发明各实施例技术方案的精神和范围。

Claims (10)

1.一种IMU与刚体的位姿融合方法，其特征在于，所述IMU与刚体的位姿融合方法包括：
确定惯性测量单元IMU与刚体之间的位置关系；
当所述刚体携带所述IMU运动时，获取所述IMU的测量姿态数据，并通过所述IMU的测量姿态数据计算得到所述IMU的待融合姿态数据；
获取刚体的测量姿态数据及所述IMU的测量姿态数据，在所述IMU的测量姿态数据中提取第0帧IMU测量姿态数据以及第n帧IMU测量姿态数据，在所述刚体的测量姿态数据中提取第0帧刚体到世界的测量姿态数据以及第n帧刚体到世界的测量姿态数据；
在预置的世界坐标系中，根据预设关系等式，计算第0帧IMU到世界的测量姿态数据，以及所述IMU从IMU坐标到刚体坐标的旋转矩阵，根据预设姿态转换公式将IMU的姿态数据转化为刚体的姿态数据，得到刚体的候选姿态数据；
提取所述刚体的候选姿态数据中的姿态四元数，并分别获取刚体的候选位置数据以及候选位移数据、IMU的姿态四元数及IMU从IMU坐标到刚体坐标的旋转矩阵；
根据融合算法中的位置融合公式、IMU的测量姿态数据、所述IMU的待融合姿态数据计算刚体的融合位置；
根据融合算法中的姿态融合公式、IMU的测量姿态数据、待融合姿态数据计算刚体的融合姿态；
将所述刚体的融合位置与所述刚体的融合姿态合并，确定刚体的融合位姿数据。
2.根据权利要求1所述的IMU与刚体的位姿融合方法，其特征在于，所述当所述刚体携带所述IMU运动时，获取所述IMU的测量姿态数据，并通过所述IMU的测量姿态数据计算得到所述IMU的待融合姿态数据包括：
获取IMU的标准姿态数据，所述标准位姿态据包括标准加速度偏置、标准角速度偏置、标准加速度噪声及标准角速度噪声；
当所述刚体携带所述IMU运动时，获取所述IMU的测量姿态数据，并根据所述IMU的测量姿态数据以及所述标准姿态数据确定待融合姿态数据，其中，所述待融合姿态数据包括待融合加速度以及待融合角速度。
3.根据权利要求2所述的IMU与刚体的位姿融合方法，其特征在于，所述当所述刚体携带所述IMU运动时，获取所述IMU的测量姿态数据，并根据所述IMU的测量姿态数据以及所述标准姿态数据确定待融合姿态数据，其中，所述待融合姿态数据包括待融合加速度以及待融合角速度包括：
获取所述刚体携带所述IMU运动时所述IMU的测量姿态数据，其中，所述IMU的测量姿态数据包括测量加速度及测量角速度；
根据第一预置公式、所述标准姿态数据及所述IMU的测量姿态数据计算待融合加速度，所述第一预置公式为：
Figure FDA0003305978750000021
其中，at表示测量加速度，at(real)表示待融合加速度，
Figure FDA0003305978750000022
表示t时刻的标准加速度偏置，
Figure FDA0003305978750000023
表示t时刻IMU姿态下重力方向分量，na为标准加速度噪声；
根据第二预置公式、所述标准姿态数据及所述IMU的测量姿态数据计算待融合角速度，所述第二预置公式为：
Figure FDA0003305978750000024
其中，wt表示测量角速度，wt(real)表示待融合角速度，
Figure FDA0003305978750000025
表示t时刻的标准角速度偏置，nw表示标准加速度噪声；
将所述待融合加速度以及所述待融合角速度合并，得到待融合姿态数据。
4.根据权利要求1-3中任一项所述的IMU与刚体的位姿融合方法，其特征在于，所述位置融合公式为：
Figure FDA0003305978750000026
其中，
Figure FDA0003305978750000027
表示在k+1时刻时刚体位于预置的世界坐标系中的候选位置，
Figure FDA0003305978750000028
表示在k时刻时刚体位于预置的世界坐标系中的候选位置，α为位置融合系数，
Figure FDA0003305978750000029
表示IMU从IMU坐标到刚体坐标的旋转矩阵，
Figure FDA00033059787500000210
表示在k时刻时刚体的候选位移，Δtk表示时间间隔，Δt＝tk+1-tk，at表示测量加速度，
Figure FDA00033059787500000211
表示k时刻IMU的速度，
Figure FDA00033059787500000212
Figure FDA00033059787500000213
表示t时刻的标准加速度偏置，na为标准加速度噪声，
Figure FDA00033059787500000214
表示t时刻IMU姿态下重力方向分量，
Figure FDA00033059787500000215
表示将IMU的速度和待融合加速度投影到预置的世界坐标系下时，[tk，tk+1]时间段中待融合加速度产生的位移量。
5.根据权利要求1-4中任一项所述的IMU与刚体的位姿融合方法，其特征在于，所述姿态融合公式为：
Figure FDA0003305978750000031
其中，
Figure FDA0003305978750000032
表示在k+1时刻时刚体位于预置的世界坐标系中的姿态四元数，
Figure FDA0003305978750000033
表示在k时刻时刚体位于预置的世界坐标系中的姿态四元数，
Figure FDA0003305978750000034
表示四元数的乘法计算符号，β表示姿态融合系数，
Figure FDA0003305978750000035
表示IMU从IMU坐标到刚体坐标的旋转矩阵，
Figure FDA0003305978750000036
为k时刻IMU的姿态四元数，wt表示测量角速度，
Figure FDA0003305978750000037
表示t时刻的标准角速度偏置，nw表示标准加速度噪声，
Figure FDA0003305978750000038
表示[tk，tk+1]时段中IMU产生的旋转变化。
6.一种IMU与刚体的位姿融合装置，其特征在于，所述IMU与刚体的位姿融合装置包括：
确定模块，用于确定惯性测量单元IMU与刚体之间的位置关系；
获取并计算模块，当所述刚体携带所述IMU运动时，用于获取所述IMU的测量姿态数据，并通过所述IMU的测量姿态数据计算得到所述IMU的待融合姿态数据；
获取并确定模块，用于在预置的世界坐标系中，获取刚体的测量姿态数据，并根据所述IMU的测量姿态数据、所述刚体的测量姿态数据以及所述IMU与刚体之间的位置关系确定刚体的候选姿态数据；
融合模块，用于利用融合算法将所述IMU的待融合姿态数据与所述刚体的候选姿态数据融合，得到刚体的融合位姿数据。
7.根据权利要求6所述的IMU与刚体的位姿融合装置，其特征在于，所述获取并计算模块包括：
获取单元，用于获取IMU的标准姿态数据，所述标准位姿态据包括标准加速度偏置、标准角速度偏置、标准加速度噪声及标准角速度噪声；
确定单元，当所述刚体携带所述IMU运动时，用于获取所述IMU的测量姿态数据，并根据所述IMU的测量姿态数据以及所述标准姿态数据确定待融合姿态数据，其中，所述待融合姿态数据包括待融合加速度以及待融合角速度。
8.根据权利要求7所述的IMU与刚体的位姿融合装置，其特征在于，所述确定单元具体用于：
获取所述刚体携带所述IMU运动时所述IMU的测量姿态数据，其中，所述IMU的测量姿态数据包括测量加速度及测量角速度；
根据第一预置公式、所述标准姿态数据及所述IMU的测量姿态数据计算待融合加速度，所述第一预置公式为：
Figure FDA0003305978750000041
其中，at表示测量加速度，at(real)表示待融合加速度，
Figure FDA0003305978750000042
表示t时刻的标准加速度偏置，
Figure FDA0003305978750000043
表示t时刻IMU姿态下重力方向分量，na为标准加速度噪声；
根据第二预置公式、所述标准姿态数据及所述IMU的测量姿态数据计算待融合角速度，所述第二预置公式为：
Figure FDA0003305978750000044
其中，wt表示测量角速度，wt(real)表示待融合角速度，
Figure FDA0003305978750000045
表示t时刻的标准角速度偏置，nw表示标准加速度噪声；
将所述待融合加速度以及所述待融合角速度合并，得到待融合姿态数据。
9.一种IMU与刚体的位姿融合设备，其特征在于，所述IMU与刚体的位姿融合设备包括：存储器和至少一个处理器，所述存储器中存储有指令，所述存储器和所述至少一个处理器通过线路互连；
所述至少一个处理器调用所述存储器中的所述指令，以使得所述IMU与刚体的位姿融合设备执行如权利要求1-5中任意一项所述的IMU与刚体的位姿融合方法。
10.一种计算机可读存储介质，所述计算机可读存储介质上存储有计算机程序，其特征在于，所述计算机程序被处理器执行时实现如权利要求1-5中任一项所述IMU与刚体的位姿融合方法。
CN202111208097.1A 2020-04-30 2020-04-30 Imu与刚体的位姿融合方法、装置、设备及存储介质 Pending CN113984051A (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202111208097.1A CN113984051A (zh) 2020-04-30 2020-04-30 Imu与刚体的位姿融合方法、装置、设备及存储介质

Applications Claiming Priority (2)

Application Number Priority Date Filing Date Title
CN202111208097.1A CN113984051A (zh) 2020-04-30 2020-04-30 Imu与刚体的位姿融合方法、装置、设备及存储介质
CN202010367408.8A CN111504314B (zh) 2020-04-30 2020-04-30 Imu与刚体的位姿融合方法、装置、设备及存储介质

Related Parent Applications (1)

Application Number Title Priority Date Filing Date
CN202010367408.8A Division CN111504314B (zh) 2020-04-30 2020-04-30 Imu与刚体的位姿融合方法、装置、设备及存储介质

Publications (1)

Publication Number Publication Date
CN113984051A true CN113984051A (zh) 2022-01-28

Family

ID=71868013

Family Applications (2)

Application Number Title Priority Date Filing Date
CN202111208097.1A Pending CN113984051A (zh) 2020-04-30 2020-04-30 Imu与刚体的位姿融合方法、装置、设备及存储介质
CN202010367408.8A Active CN111504314B (zh) 2020-04-30 2020-04-30 Imu与刚体的位姿融合方法、装置、设备及存储介质

Family Applications After (1)

Application Number Title Priority Date Filing Date
CN202010367408.8A Active CN111504314B (zh) 2020-04-30 2020-04-30 Imu与刚体的位姿融合方法、装置、设备及存储介质

Country Status (2)

Country Link
CN (2) CN113984051A (zh)
WO (1) WO2021218731A1 (zh)

Families Citing this family (3)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN113984051A (zh) * 2020-04-30 2022-01-28 深圳市瑞立视多媒体科技有限公司 Imu与刚体的位姿融合方法、装置、设备及存储介质
CN112781589B (zh) * 2021-01-05 2021-12-28 北京诺亦腾科技有限公司 一种基于光学数据和惯性数据的位置追踪设备及方法
CN113873637A (zh) * 2021-10-26 2021-12-31 上海瑾盛通信科技有限公司 定位方法、装置、终端和存储介质

Citations (11)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
JP2009276282A (ja) * 2008-05-16 2009-11-26 Sumitomo Electric Ind Ltd 姿勢特定装置、移動方位特定装置、位置特定装置、コンピュータプログラム及び姿勢特定方法
CN101907714A (zh) * 2010-06-25 2010-12-08 陶洋 基于多传感器数据融合的gps辅助定位系统及其定位方法
KR101769602B1 (ko) * 2016-08-09 2017-08-18 아이디어주식회사 옵티컬 플로우와 imu 및 초음파센서를 이용한 호버링용 위치보정장치 및 위치보정방법
CN107316319A (zh) * 2017-05-27 2017-11-03 北京小鸟看看科技有限公司 一种刚体追踪的方法、装置和系统
CN108253958A (zh) * 2018-01-18 2018-07-06 亿嘉和科技股份有限公司 一种稀疏环境下的机器人实时定位方法
CN108759826A (zh) * 2018-04-12 2018-11-06 浙江工业大学 一种基于手机和无人机多传感参数融合的无人机运动跟踪方法
CN109000633A (zh) * 2017-06-06 2018-12-14 大连理工大学 基于异构数据融合的人体姿态动作捕捉算法设计
CN110366104A (zh) * 2019-08-12 2019-10-22 中南大学湘雅医院 定位方法、装置、系统及电子设备及计算机可读存储介质
CN110455301A (zh) * 2019-08-01 2019-11-15 河北工业大学 一种基于惯性测量单元的动态场景slam方法
CN110567484A (zh) * 2019-07-25 2019-12-13 深圳市瑞立视多媒体科技有限公司 标定imu与刚体姿态的方法、设备以及可读存储介质
CN111504314A (zh) * 2020-04-30 2020-08-07 深圳市瑞立视多媒体科技有限公司 Imu与刚体的位姿融合方法、装置、设备及存储介质

Family Cites Families (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US9607401B2 (en) * 2013-05-08 2017-03-28 Regents Of The University Of Minnesota Constrained key frame localization and mapping for vision-aided inertial navigation
CN107102735B (zh) * 2017-04-24 2018-06-19 广东虚拟现实科技有限公司 一种对齐方法及对齐装置
CN107289931B (zh) * 2017-05-23 2019-09-27 北京小鸟看看科技有限公司 一种定位刚体的方法、装置和系统
CN110823214B (zh) * 2019-10-18 2021-05-25 西北工业大学 一种空间完全非合作目标相对位姿和惯量估计方法

Patent Citations (11)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
JP2009276282A (ja) * 2008-05-16 2009-11-26 Sumitomo Electric Ind Ltd 姿勢特定装置、移動方位特定装置、位置特定装置、コンピュータプログラム及び姿勢特定方法
CN101907714A (zh) * 2010-06-25 2010-12-08 陶洋 基于多传感器数据融合的gps辅助定位系统及其定位方法
KR101769602B1 (ko) * 2016-08-09 2017-08-18 아이디어주식회사 옵티컬 플로우와 imu 및 초음파센서를 이용한 호버링용 위치보정장치 및 위치보정방법
CN107316319A (zh) * 2017-05-27 2017-11-03 北京小鸟看看科技有限公司 一种刚体追踪的方法、装置和系统
CN109000633A (zh) * 2017-06-06 2018-12-14 大连理工大学 基于异构数据融合的人体姿态动作捕捉算法设计
CN108253958A (zh) * 2018-01-18 2018-07-06 亿嘉和科技股份有限公司 一种稀疏环境下的机器人实时定位方法
CN108759826A (zh) * 2018-04-12 2018-11-06 浙江工业大学 一种基于手机和无人机多传感参数融合的无人机运动跟踪方法
CN110567484A (zh) * 2019-07-25 2019-12-13 深圳市瑞立视多媒体科技有限公司 标定imu与刚体姿态的方法、设备以及可读存储介质
CN110455301A (zh) * 2019-08-01 2019-11-15 河北工业大学 一种基于惯性测量单元的动态场景slam方法
CN110366104A (zh) * 2019-08-12 2019-10-22 中南大学湘雅医院 定位方法、装置、系统及电子设备及计算机可读存储介质
CN111504314A (zh) * 2020-04-30 2020-08-07 深圳市瑞立视多媒体科技有限公司 Imu与刚体的位姿融合方法、装置、设备及存储介质

Non-Patent Citations (2)

* Cited by examiner, † Cited by third party
Title
皮金柱: "基于单目视觉融合惯导的定位技术研究", 《中国优秀硕士学位论文全文数据库 信息科技辑》, no. 12, pages 140 - 209 *
盛淼: ""基于双目视觉惯导的SLAM算法研究"", 《中国优秀硕士学位论文全文数据库 信息科技辑》, no. 03, pages 140 - 446 *

Also Published As

Publication number Publication date
CN111504314A (zh) 2020-08-07
WO2021218731A1 (zh) 2021-11-04
CN111504314B (zh) 2021-11-12

Similar Documents

Publication Publication Date Title
CN111504314B (zh) Imu与刚体的位姿融合方法、装置、设备及存储介质
JP4876204B2 (ja) 小型姿勢センサ
JP6019504B2 (ja) 移動体の進行方向推定装置及び進行方向推定方法
KR100898169B1 (ko) 관성항법시스템의 초기정렬 방법
CN110221302B (zh) 环境探测装置及其修正方法、系统、便携设备及存储介质
KR20190041315A (ko) 관성 기반 항법 장치 및 상대사전적분에 따른 관성 기반 항법 방법
CN102269592A (zh) 基于传感器的定向系统
CN111197984A (zh) 一种基于环境约束下的视觉-惯性运动估计方法
CN112815834B (zh) 一种光学定位系统
CN113188505B (zh) 姿态角度的测量方法、装置、车辆及智能臂架
Amirsadri et al. Practical considerations in precise calibration of a low-cost MEMS IMU for road-mapping applications
CN107402004B (zh) 传感器的姿态信息获取方法及装置
JP2009186244A (ja) 傾斜角度推定システム、相対角度推定システム及び角速度推定システム
CN111272199A (zh) 一种imu的安装误差角的标定方法及装置
CN113959464B (zh) 一种陀螺仪辅助的加速度计现场校准方法和系统
CN113227714B (zh) 用于表征惯性测量单元的方法
CN114137592A (zh) 一种多源传感器融合定位的切换方法及系统
Akeila et al. Implementation, calibration and testing of GFINS models based on six-accelerometer cube
CN111964676A (zh) 航姿解算方法、装置、电子设备及计算机可读存储介质
JP3783061B1 (ja) 傾斜角と並進加速度の検出方法および検出装置
CN110672096A (zh) 一种基于惯性测量单元的室内物体定位方法和系统
CN113029133B (zh) 机器人、校正方法及计算机可读存储介质
JP5424226B2 (ja) 傾斜角度推定システム
Wang et al. An improved six-position calibration method of accelerometer
McLean et al. A robotic joint sensor

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