WO2021218731A1

WO2021218731A1 - Method and apparatus for position-attitude fusion of imu and rigid body, device, and storage medium

Info

Publication number: WO2021218731A1
Application number: PCT/CN2021/088625
Authority: WO
Inventors: 吴迪云; 许秋子
Original assignee: 深圳市瑞立视多媒体科技有限公司
Priority date: 2020-04-30
Filing date: 2021-04-21
Publication date: 2021-11-04
Also published as: CN111504314A; CN111504314B; CN113984051A

Abstract

A method and apparatus for position-attitude fusion of an IMU and a rigid body, a device, and a storage medium, relating to the field of measurement and used for solving the problem of large errors between measured position and attitude data and real position and attitude data of rigid bodies in measurement of position and attitude data of rigid bodies. The method for position-attitude fusion of an IMU and a rigid body comprises: determining a positional relationship between an inertial measurement unit (IMU) and a rigid body (101); when the rigid body carries the IMU in motion, obtaining measured attitude data of the IMU and calculating, by means of the measured attitude data of the IMU, the attitude data to be fused of the IMU (102); in a preset world coordinate system, obtaining measured attitude data of the rigid body and determining candidate attitude data of the rigid body according to the measured attitude data of the IMU, the measured attitude data of the rigid body, and the positional relationship between the IMU and the rigid body (103); and using a fusion algorithm to fuse the attitude data to be fused of the IMU with the candidate attitude data of the rigid body to obtain fused position and attitude data of the rigid body (104).

Description

IMU与刚体的位姿融合方法、装置、设备及存储介质Pose fusion method, device, equipment and storage medium of IMU and rigid body

技术领域Technical field

本发明涉及测量领域，尤其涉及一种IMU与刚体的位姿融合方法、装置、设备及存储介质。The invention relates to the field of measurement, and in particular to a method, device, equipment and storage medium for the pose fusion of an IMU and a rigid body.

背景技术Background technique

惯性测量单元(Inertial measurement unit，IMU)，测量物体三轴姿态角(或角速率)以及加速度的装置。一般的，一个IMU内设置有三个轴向的陀螺仪和三个方向的加速度计，分别用来测量刚体在三维空间中的角速度和加速度，并以此计算出刚体的姿态，因此，IMU的使用范围非常广，从宇航，到汽车及消费电子都有IMU的身影。此外，IMU具有高频率数据采集能力，例如：亚德诺半导体(ADI)ADIS16系列的采集频率能够达到200Hz，部分型号甚至达到400Hz采集频率。在有些应用场景下需要测量刚体高速运动时的位姿数据，此时可以用光学定位装置和IMU融合方式。Inertial measurement unit (IMU), a device that measures the three-axis attitude angle (or angular rate) and acceleration of an object. Generally, there are three axial gyroscopes and three-directional accelerometers in an IMU, which are used to measure the angular velocity and acceleration of a rigid body in three-dimensional space, and to calculate the posture of the rigid body. Therefore, the use of IMU The scope is very wide, from aerospace, to automobiles and consumer electronics, there are IMUs. In addition, the IMU has high-frequency data acquisition capabilities. For example, the acquisition frequency of the Analog Devices ADIS16 series can reach 200 Hz, and some models can even reach 400 Hz. In some application scenarios, it is necessary to measure the pose data of a rigid body when it moves at a high speed. At this time, an optical positioning device and IMU can be used for fusion.

但在具体的实际应用场景下，当刚体高速运动时如果某些摄像装置没有捕捉到刚体的位置或仅仅捕捉到刚体的偏移位置，这样得到刚体的测量位姿数据与真实位姿数据之间的误差偏大，无法得到精确的刚体数据。However, in specific practical application scenarios, when the rigid body is moving at high speed, if some camera devices do not capture the position of the rigid body or only the offset position of the rigid body, the difference between the measured pose data of the rigid body and the real pose data can be obtained. The error of is too large to get accurate rigid body data.

发明内容Summary of the invention

本发明的主要目的在于解决在测量刚体的位姿数据时，刚体的测量位姿数据与真实位姿数据之间误差大的问题。The main purpose of the present invention is to solve the problem of large errors between the measured pose data of the rigid body and the actual pose data when measuring the pose data of a rigid body.

本发明第一方面提供了一种IMU与刚体的位姿融合方法，包括：确定惯性测量单元IMU与刚体之间的位置关系；当所述刚体携带所述IMU运动时，获取所述IMU的测量姿态数据，并通过所述IMU的测量姿态数据计算得到所述IMU的待融合姿态数据；在预置的世界坐标系中，获取刚体的测量姿态数据，并根据所述IMU的测量姿态数据、所述刚体的测量姿态数据以及所述IMU与刚体之间的位置关系确定刚体的候选姿态数据；利用融合算法将所述IMU的待融合姿态数据与所述刚体的候选姿态数据融合，得到刚体的融合位姿数据。The first aspect of the present invention provides a pose fusion method of an IMU and a rigid body, including: determining the positional relationship between an inertial measurement unit IMU and a rigid body; when the rigid body carries the IMU and moves, obtaining the measurement of the IMU The posture data of the IMU is calculated from the measured posture data of the IMU to obtain the posture data of the IMU to be fused; in the preset world coordinate system, the measured posture data of the rigid body is obtained, and according to the measured posture data of the IMU, The measured posture data of the rigid body and the positional relationship between the IMU and the rigid body determine the candidate posture data of the rigid body; the fusion algorithm is used to fuse the posture data of the IMU to be fused with the candidate posture data of the rigid body to obtain the fusion of the rigid body Pose data.

可选的，在本发明第一方面的第一种实现方式中，所述当所述刚体携带所述IMU运动时，获取所述IMU的测量姿态数据，并通过所述IMU的测量姿态数据计算得到所述IMU的待融合姿态数据包括：获取IMU的标准姿态数据，所述标准位姿态据包括标准加速度偏置、标准角速度偏置、标准加速度噪声及标准角速度噪声；当所述刚体携带所述IMU运动时，获取所述IMU的测量姿态数据，并根据所述IMU的测量姿态数据以及所述标准姿态数据确定待融合姿态数据，其中，所述待融合姿态数据包括待融合加速度以及待融合角速度。Optionally, in the first implementation manner of the first aspect of the present invention, when the rigid body carries the IMU in motion, the measured posture data of the IMU is acquired, and the measured posture data of the IMU is used to calculate Obtaining the to-be-fused posture data of the IMU includes: obtaining the standard posture data of the IMU, and the standard posture data includes standard acceleration offset, standard angular velocity offset, standard acceleration noise, and standard angular velocity noise; when the rigid body carries the When the IMU is in motion, obtain the measured posture data of the IMU, and determine the posture data to be fused according to the measured posture data of the IMU and the standard posture data, wherein the posture data to be fused includes the acceleration to be fused and the angular velocity to be fused .

可选的，在本发明第一方面的第二种实现方式中，所述当所述刚体携带所述IMU运动时，获取所述IMU的测量姿态数据，并根据所述IMU的测量姿态数据以及所述标准姿态数据确定待融合姿态数据，其中，所述待融合姿态数据包括待融合加速度以及待融合角速度包括：获取所述刚体携带所述IMU运动时所述IMU的测量姿态数据，其中，所述IMU的测量姿态数据包括测量加速度及测量角速度；根据第一预置公式、所述标准姿态数据及所述IMU的测量姿态数据计算待融合加速度，所述第一预置公式为：

其中，a _t表示测量加速度，a _t(real)表示待融合加速度，

表示t时刻的标准加速度偏置，

表示t时刻IMU姿态下重力方向分量，n _a为标准加速度噪声；根据第二预置公式、所述标准姿态数据及所述IMU的测量姿态数据计算待融合角速度，所述第二预置公式为：

其中，w _t表示测量角速度，w _t(real)表示待融合角速度，

表示t时刻的标准角速度偏置，n _w表示标准加速度噪声；将所述待融合加速度以及所述待融合角速度合并，得到待融合姿态数据。 Optionally, in the second implementation manner of the first aspect of the present invention, when the rigid body is carrying the IMU in motion, the measured posture data of the IMU is acquired, and the measured posture data of the IMU and The standard posture data determines the posture data to be fused, where the posture data to be fused includes acceleration to be fused and angular velocity to be fused includes: acquiring the measured posture data of the IMU when the rigid body is moving with the IMU, wherein The measured attitude data of the IMU includes measured acceleration and measured angular velocity; the acceleration to be fused is calculated according to a first preset formula, the standard attitude data, and the measured attitude data of the IMU, and the first preset formula is:

Wherein, a _t represents a measure acceleration, a _{t (real)} represents an acceleration to be fused,

Represents the standard acceleration bias at time t,

Represents the gravity direction component of the IMU attitude at time t, and n _a is the standard acceleration noise; the angular velocity to be fused is calculated according to the second preset formula, the standard attitude data and the measured attitude data of the IMU, the second preset formula is :

Among them, w _t represents the measured angular velocity, w _{t (real)} represents the angular velocity to be fused,

Represents the standard angular velocity offset at time t, and _nw represents the standard acceleration noise; the acceleration to be fused and the angular velocity to be fused are combined to obtain the posture data to be fused.

可选的，在本发明第一方面的第三种实现方式中，所述在预置的世界坐标系中，获取刚体的测量姿态数据，并根据所述IMU的测量姿态数据、所述刚体的测量姿态数据以及所述IMU与刚体之间的位置关系确定刚体的候选姿态数据包括：获取刚体的测量姿态数据及所述IMU的测量姿态数据，在所述IMU的测量姿态数据中提取第0帧IMU测量姿态数据以及第n帧IMU测量姿态数据，在所述刚体的测量姿态数据中提取第0帧刚体到世界的测量姿态数据以及第n帧刚体到世界的测量姿态数据；在预置的世界坐标系中，根据关系等式，计算第0帧IMU到世界的测量姿态数据，以及所述IMU从IMU 坐标到刚体坐标的旋转矩阵，其中，所述关系等式为：Optionally, in a third implementation manner of the first aspect of the present invention, the measured posture data of the rigid body is acquired in the preset world coordinate system, and the measured posture data of the rigid body is obtained according to the measured posture data of the IMU and the rigid body. Measured posture data and the positional relationship between the IMU and the rigid body. Determining candidate posture data of the rigid body includes: acquiring the measured posture data of the rigid body and the measured posture data of the IMU, and extracting the 0th frame from the measured posture data of the IMU IMU measurement posture data and the nth frame IMU measurement posture data, and extract the 0th frame rigid body to the world measurement posture data and the nth frame rigid body to the world measurement posture data from the rigid body measurement posture data; in the preset world In the coordinate system, the measured posture data from the 0th frame IMU to the world and the rotation matrix of the IMU from the IMU coordinates to the rigid body coordinates are calculated according to the relation equation, where the relation equation is:

其中，

表示第n帧刚体到世界的测量姿态数据，

表示IMU测量姿态数据从IMU坐标到刚体坐标的旋转矩阵，[R _i] _n表示第n帧IMU测量姿态数据，

表示第0帧刚体到世界的测量姿态数据，[R _i] ₀表示第0帧IMU测量姿态数据，

表示第0帧IMU到世界的测量姿态数据，

表示第n帧IMU测量姿态数据相对于第0帧IMU测量姿态数据的逆矩阵，E用于指示单位矩阵；根据姿态转换公式将IMU的姿态数据转化为刚体的姿态数据，得到刚体的候选姿态数据，所述姿态转换公式为： in,

Represents the measured posture data of the n-th rigid body to the world,

Represents the rotation matrix of IMU measurement posture data from IMU coordinates to rigid body coordinates, [R _i ] _n represents the nth frame IMU measurement posture data,

Represents the measured attitude data of the rigid body to the world in the 0th frame, [R _i ] ₀ represents the measured attitude data of the IMU in the 0th frame,

Represents the measured attitude data from the 0th frame IMU to the world,

Represents the inverse matrix of the nth frame IMU measured attitude data relative to the 0th frame IMU measured attitude data, E is used to indicate the identity matrix; according to the attitude conversion formula, the IMU attitude data is converted into rigid body attitude data to obtain the rigid body candidate attitude data , The posture conversion formula is:

其中，

表示经过转换后刚体的候选姿态数据，

表示第0帧IMU到世界的测量姿态数据，

表示第n帧IMU测量姿态数据相对于第0帧IMU测量姿态数据的逆矩阵，

表示IMU测量姿态数据从IMU坐标到刚体坐标的旋转矩阵的逆变换。 in,

Represents the candidate pose data of the rigid body after transformation,

Represents the measured attitude data from the 0th frame IMU to the world,

Represents the inverse matrix of the IMU measurement attitude data of the nth frame relative to the IMU measurement attitude data of the 0th frame,

Represents the inverse transformation of the rotation matrix of IMU measurement posture data from IMU coordinates to rigid body coordinates.

可选的，在本发明第一方面的第四种实现方式中，所述利用融合算法将所述IMU的待融合姿态数据与所述刚体的候选姿态数据融合，得到刚体的融合位姿数据包括：提取所述刚体的候选姿态数据中的姿态四元数，并分别获取刚体的候选位置数据以及候选位移数据、IMU的姿态四元数及IMU从IMU坐标到刚体坐标的旋转矩阵；根据融合算法中的位置融合公式、IMU的测量姿态数据、所述IMU的待融合姿态数据计算刚体的融合位置，所述位置融合公式为：Optionally, in the fourth implementation manner of the first aspect of the present invention, the fusion algorithm using a fusion algorithm to fuse the posture data of the IMU to be fused with the candidate posture data of the rigid body to obtain the fused posture data of the rigid body includes : Extract the posture quaternion in the candidate posture data of the rigid body, and obtain the candidate position data and candidate displacement data of the rigid body, the posture quaternion of the IMU, and the rotation matrix of the IMU from the IMU coordinates to the rigid body coordinates respectively; according to the fusion algorithm The position fusion formula in, the measured posture data of the IMU, and the posture data to be fused of the IMU calculate the fusion position of the rigid body, and the position fusion formula is:

其中，

表示在k+1时刻时刚***于预置的世界坐标系中的候选位置，

表示在k时刻时刚***于预置的世界坐标系中的候选位置，α为位置融合系数，

表示IMU从IMU坐标到刚体坐标的旋转矩阵，

表示在k时刻时刚体的候选位移，Δt _k表示时间间隔，Δt＝t _k+1-t _k，a _t表示测量加速度，

表示k时刻IMU的速度，

表示t时刻的标准加速度偏置，n _a为标准加速度噪声，

表示t时刻IMU姿态下重力方向分量，

表示将IMU的速度和待融合加速度投影到预置的世界坐标系下时，[t _k，t _k+1]时间段中待融合加速度产生的位移量；根据融合算法中的姿态融合公式、IMU的测量姿态数据、待融合姿态数据计算刚体的融合姿态，所述姿态融合公式为： in,

Indicates the candidate position of the rigid body in the preset world coordinate system at time k+1,

Indicates the candidate position of the rigid body in the preset world coordinate system at time k, and α is the position fusion coefficient,

Represents the IMU rotation matrix from IMU coordinates to rigid body coordinates,

K represents a time when the displacement of the rigid candidate, _k [Delta] t represents the time _{interval, Δt = t k + 1 -t} k, a t represents the measurement of acceleration,

Represents the speed of the IMU at time k,

Represents the standard acceleration bias at time t, n _a is the standard acceleration noise,

Represents the gravity direction component of the IMU attitude at time t,

Indicates the displacement generated by the acceleration to be fused in the time period _{[t k} , t _k+1 ] when the velocity of the IMU and the acceleration to be fused are projected to the preset world coordinate system; according to the attitude fusion formula and IMU in the fusion algorithm Calculate the fusion posture of the rigid body based on the measured posture data and the posture data to be fused, and the posture fusion formula is:

其中，

表示在k+1时刻时刚***于预置的世界坐标系中的姿态四元数，

表示在k时刻时刚***于预置的世界坐标系中的姿态四元数，

表示四元数的乘法计算符号，β表示姿态融合系数，

表示IMU从IMU坐标到刚体坐标的旋转矩阵，

为k时刻IMU的姿态四元数，w _t表示测量角速度，

表示t时刻的标准角速度偏置，n _w表示标准加速度噪声，

表示[t _k，t _k+1]时段中IMU产生的旋转变化；将所述刚体的融合位置与所述刚体的融合姿态合并，确定刚体的融合位姿数据。 in,

Represents the posture quaternion of the rigid body in the preset world coordinate system at time k+1,

Represents the posture quaternion of the rigid body in the preset world coordinate system at time k,

Represents the quaternion multiplication calculation symbol, β represents the attitude fusion coefficient,

Is the attitude quaternion of the IMU at time k, w _t represents the measured angular velocity,

Represents the standard angular velocity offset at time t, n _w represents the standard acceleration noise,

Represents the rotation change produced by the IMU during the period [t _k , t _k+1 ]; merge the fusion position of the rigid body with the fusion posture of the rigid body to determine the fusion pose data of the rigid body.

本发明第二方面提供了一种IMU与刚体的位姿融合装置，包括：确定模块，用于确定惯性测量单元IMU与刚体之间的位置关系；获取并计算模块，当所述刚体携带所述IMU运动时，用于获取所述IMU的测量姿态数据，并通过所述IMU的测量姿态数据计算得到所述IMU的待融合姿态数据；获取并确定模块，用于在预置的世界坐标系中，获取刚体的测量姿态数据，并根据所述IMU的测量姿态数据、所述刚体的测量姿态数据以及所述IMU与刚体之间的位置关系确定刚体的候选姿态数据；融合模块，用于利用融合算法将所述IMU的待融合姿态数据与所述刚体的候选姿态数据融合，得到刚体的融合位姿数据。The second aspect of the present invention provides a pose fusion device of an IMU and a rigid body, including: a determination module for determining the positional relationship between the inertial measurement unit IMU and the rigid body; an acquisition and calculation module, when the rigid body carries the When the IMU is moving, it is used to obtain the measured posture data of the IMU, and calculate the to-be-fused posture data of the IMU through the measured posture data of the IMU; the acquisition and determination module is used for setting in the preset world coordinate system , Obtain the measured posture data of the rigid body, and determine the candidate posture data of the rigid body according to the measured posture data of the IMU, the measured posture data of the rigid body, and the positional relationship between the IMU and the rigid body; the fusion module is used to use the fusion The algorithm fuses the posture data to be fused of the IMU with the candidate posture data of the rigid body to obtain the fused posture data of the rigid body.

可选的，在本发明第二方面的第一种实现方式中，所述获取并计算模块包括：获取单元，用于获取IMU的标准姿态数据，所述标准位姿态据包括标准加速度偏置、标准角速度偏置、标准加速度噪声及标准角速度噪声；确定单元，当所述刚体携带所述IMU运动时，用于获取所述IMU的测量姿态数据，并根据所述IMU的测量姿态数据以及所述标准姿态数据确定待融合姿态数据，其中，所述待融合姿态数据包括待融合加速度以及待融合角速度。Optionally, in the first implementation manner of the second aspect of the present invention, the acquiring and calculating module includes: an acquiring unit configured to acquire standard posture data of the IMU, and the standard posture data includes standard acceleration offset, Standard angular velocity offset, standard acceleration noise, and standard angular velocity noise; a determining unit, when the rigid body carries the IMU, is used to obtain the measured posture data of the IMU, and according to the measured posture data of the IMU and the The standard posture data determines the posture data to be fused, where the posture data to be fused includes the acceleration to be fused and the angular velocity to be fused.

可选的，在本发明第二方面的第二种实现方式中，所述确定单元具体用于：获取所述刚体携带所述IMU运动时所述IMU的测量姿态数据，其中，所述IMU的测量姿态数据包括测量加速度及测量角速度；根据第一预置公式、所述标准姿态数据及所述IMU的测量姿态数据计算待融合加速度，所述第一预置公式为：

其中，a _t表示测量加速度，a _t(real)表示待融合加速度，

表示t时刻的标准加速度偏置，

其中，w _t表示测量角速度，w _t(real)表示待融合角速度，

表示t时刻的标准角速度偏置，n _w表示标准加速度噪声；将所述待融合加速度以及所述待融合角速度合并，得到待融合姿态数据。 Optionally, in the second implementation manner of the second aspect of the present invention, the determining unit is specifically configured to: obtain the measured posture data of the IMU when the rigid body carries the IMU in motion, wherein the IMU The measured attitude data includes measured acceleration and measured angular velocity; the acceleration to be fused is calculated according to the first preset formula, the standard attitude data and the measured attitude data of the IMU, and the first preset formula is:

Represents the standard acceleration bias at time t,

可选的，在本发明第二方面的第三种实现方式中，所述获取并确定模块具体用于：获取刚体的测量姿态数据及所述IMU的测量姿态数据，在所述IMU的测量姿态数据中提取第0帧IMU测量姿态数据以及第n帧IMU测量姿态数据，在所述刚体的测量姿态数据中提取第0帧刚体到世界的测量姿态数据以及第n帧刚体到世界的测量姿态数据；在预置的世界坐标系中，根据关系等式，计算第0帧IMU到世界的测量姿态数据，以及所述IMU从IMU坐标到刚体坐标的旋转矩阵，其中，所述关系等式为：Optionally, in the third implementation of the second aspect of the present invention, the acquiring and determining module is specifically configured to: acquire the measured posture data of the rigid body and the measured posture data of the IMU, and the measured posture of the IMU Extract the 0th frame IMU measurement posture data and the nth frame IMU measurement posture data from the data, extract the 0th frame rigid body to the world measurement posture data and the nth frame rigid body to the world measurement posture data from the rigid body measurement posture data ; In the preset world coordinate system, according to the relationship equation, calculate the measured posture data of the 0th frame IMU to the world, and the rotation matrix of the IMU from the IMU coordinates to the rigid body coordinates, where the relationship equation is:

其中，

表示第n帧刚体到世界的测量姿态数据，

表示第0帧IMU到世界的测量姿态数据，

Represents the measured posture data of the n-th rigid body to the world,

Represents the measured attitude data from the 0th frame IMU to the world,

其中，

表示经过转换后刚体的候选姿态数据，

表示第0帧IMU到世界的测量姿态数据，

Represents the candidate pose data of the rigid body after transformation,

Represents the measured attitude data from the 0th frame IMU to the world,

可选的，在本发明第二方面的第四种实现方式中，所述获取并确定模块具体用于：提取所述刚体的候选姿态数据中的姿态四元数，并分别获取刚体的候选位置数据以及候选位移数据、IMU的姿态四元数及IMU从IMU坐标到刚体坐标的旋转矩阵；根据融合算法中的位置融合公式、IMU的测量姿态数据、所述IMU的待融合姿态数据计算刚体的融合位置，所述位置融合公式为：Optionally, in the fourth implementation manner of the second aspect of the present invention, the acquiring and determining module is specifically configured to: extract the pose quaternion in the candidate pose data of the rigid body, and obtain the candidate positions of the rigid body respectively Data and candidate displacement data, IMU posture quaternion and IMU rotation matrix from IMU coordinates to rigid body coordinates; calculate the rigid body’s position fusion formula according to the position fusion formula in the fusion algorithm, the measured posture data of the IMU, and the posture data to be fused of the IMU Fusion position, the position fusion formula is:

其中，

表示在k+1时刻时刚***于预置的世界坐标系中的候选位置，

表示IMU从IMU坐标到刚体坐标的旋转矩阵，

表示k时刻IMU的速度，

表示t时刻的标准加速度偏置，n _a为标准加速度噪声，

表示t时刻IMU姿态下重力方向分量，

Represents the speed of the IMU at time k,

Represents the gravity direction component of the IMU attitude at time t,

其中，

表示在k+1时刻时刚***于预置的世界坐标系中的姿态四元数，

表示在k时刻时刚***于预置的世界坐标系中的姿态四元数，

表示四元数的乘法计算符号，β表示姿态融合系数，

表示IMU从IMU坐标到刚体坐标的旋转矩阵，

为k时刻IMU的姿态四元数，w _t表示测量角速度，

表示t时刻的标准角速度偏置，n _w表示标准加速度噪声，

本发明第三方面提供了一种IMU与刚体的位姿融合设备，包括：存储器和至少一个处理器，所述存储器中存储有指令，所述存储器和所述至少一个处理器通过线路互连；所述至少一个处理器调用所述存储器中的所述指令，以使得所述IMU与刚体的位姿融合设备执行上述的IMU与刚体的位姿融合方法。A third aspect of the present invention provides a pose fusion device of an IMU and a rigid body, including: a memory and at least one processor, the memory stores instructions, and the memory and the at least one processor are interconnected by wires; The at least one processor invokes the instructions in the memory, so that the device for fusing the pose of the IMU and the rigid body executes the above-mentioned pose fusion method of the IMU and the rigid body.

本发明的第四方面提供了一种计算机可读存储介质，所述计算机可读存储介质中存储有指令，当其在计算机上运行时，使得计算机执行上述的IMU与刚体的位姿融合方法。A fourth aspect of the present invention provides a computer-readable storage medium having instructions stored in the computer-readable storage medium, which when run on a computer, cause the computer to execute the above-mentioned method for fusing the pose of the IMU and the rigid body.

本发明提供的技术方案中，确定惯性测量单元IMU与刚体之间的位置关系；当所述刚体携带所述IMU运动时，获取所述IMU的测量姿态数据，并通过所述IMU的测量姿态数据计算得到IMU的待融合姿态数据；在预置的世界坐标系中，获取刚体的测量姿态数据，并根据所述IMU的测量姿态数据、所述刚体的测量姿态数据以及所述IMU与刚体之间的位置关系确定刚体的候选姿态数据；利用融合算法将所述IMU的待融合姿态数据与所述刚体的候选姿态数据融合，得到刚体的融合位姿数据。本发明实施例中，通过获取刚体携带IMU运动时IMU的测量姿态数据，并根据IMU与刚体之间的转换关系确定刚体的候选姿态数据，最后利用融合函数将IMU的测量姿态数据与刚体的候选姿态数据进行融合，得到刚体的融合位姿数据，减少了刚体的测量位姿数据与真实位姿数据之间的误差，提高了测量位姿数据的精确度。In the technical solution provided by the present invention, the positional relationship between the inertial measurement unit IMU and the rigid body is determined; when the rigid body carries the IMU in motion, the measured attitude data of the IMU is acquired, and the measured attitude data of the IMU is passed The posture data to be fused of the IMU is calculated; in the preset world coordinate system, the measured posture data of the rigid body is obtained, and the measured posture data of the IMU, the measured posture data of the rigid body, and the relationship between the IMU and the rigid body are obtained. The positional relationship of determining the candidate pose data of the rigid body; using a fusion algorithm to fuse the to-be-fused pose data of the IMU with the candidate pose data of the rigid body to obtain the fused pose data of the rigid body. In the embodiment of the present invention, the measured posture data of the IMU is acquired when the rigid body carries the IMU, and the candidate posture data of the rigid body is determined according to the conversion relationship between the IMU and the rigid body. Finally, the fusion function is used to combine the measured posture data of the IMU with the rigid body candidate The pose data is fused to obtain the fused pose data of the rigid body, which reduces the error between the measured pose data of the rigid body and the real pose data, and improves the accuracy of the measured pose data.

附图说明Description of the drawings

图1为本发明实施例中IMU与刚体的位姿融合方法的一个实施例示意图；FIG. 1 is a schematic diagram of an embodiment of a method for fusing the pose and pose of an IMU and a rigid body in an embodiment of the present invention;

图2为本发明实施例中IMU与刚体的位姿融合方法的另一个实施例示意图；FIG. 2 is a schematic diagram of another embodiment of the method for fusing the pose of the IMU and the rigid body in the embodiment of the present invention;

图3为本发明实施例中IMU与刚体的位姿融合装置的一个实施例示意图；FIG. 3 is a schematic diagram of an embodiment of a device for fusing the pose and pose of an IMU and a rigid body in an embodiment of the present invention;

图4为本发明实施例中IMU与刚体的位姿融合装置的另一个实施例示意图；FIG. 4 is a schematic diagram of another embodiment of a device for fusing the pose and pose of an IMU and a rigid body in an embodiment of the present invention;

图5为本发明实施例中IMU与刚体的位姿融合设备的一个实施例示意图。Fig. 5 is a schematic diagram of an embodiment of a device for fusion of pose and pose of IMU and rigid body in the embodiment of the present invention.

具体实施方式Detailed ways

本发明实施例提供了一种IMU与刚体的位姿融合方法、装置、设备及存储介质，通过获取刚体携带IMU运动时IMU的测量姿态数据，并根据IMU与刚体之间的转换关系确定刚体的候选姿态数据，最后利用融合函数将IMU的测量姿态数据与刚体的候选姿态数据进行融合，得到刚体的融合位姿数据，减少了刚体的测量位姿数据与真实位姿数据之间的误差，提高了测量位姿数据的精确度。The embodiment of the present invention provides a method, device, equipment and storage medium for the posture fusion of an IMU and a rigid body. By acquiring the measured posture data of the IMU when the rigid body is carrying the IMU in motion, the rigid body’s position is determined according to the conversion relationship between the IMU and the rigid body. Candidate pose data, and finally use the fusion function to fuse the measured pose data of the IMU with the candidate pose data of the rigid body to obtain the fused pose data of the rigid body, which reduces the error between the measured pose data of the rigid body and the real pose data, and improves The accuracy of measuring pose data is improved.

本发明的说明书和权利要求书及上述附图中的术语“第一”、“第二”、“第三”、“第四”等(如果存在)是用于区别类似的对象，而不必用于描述特定的顺序或先后次序。应该理解这样使用的数据在适当情况下可以互换，以便这里描述的实施例能够以除了在这里图示或描述的内容以外的顺序实施。此外，术语“包括”或“具有”及其任何变形，意图在于覆盖不排他的包含，例如，包含了一系列步骤或单元的过程、方法、***、产品或设备不必限于清楚地列出的那些步骤或单元，而是可包括没有清楚地列出的或对于这些过程、方法、产品或设备固有的其它步骤或单元。The terms "first", "second", "third", "fourth", etc. (if any) in the description and claims of the present invention and the above-mentioned drawings are used to distinguish similar objects, and do not need to be used To describe a specific order or sequence. It should be understood that the data used in this way can be interchanged under appropriate circumstances so that the embodiments described herein can be implemented in a sequence other than the content illustrated or described herein. In addition, the terms "including" or "having" and any variations thereof are intended to cover non-exclusive inclusion. For example, a process, method, system, product, or device that includes a series of steps or units is not necessarily limited to those clearly listed. Steps or units, but may include other steps or units that are not clearly listed or are inherent to these processes, methods, products, or equipment.

为便于理解，下面对本发明实施例的具体流程进行描述，请参阅图1，本发明实施例中IMU与刚体的位姿融合方法的一个实施例包括：For ease of understanding, the following describes the specific process of the embodiment of the present invention. Please refer to Fig. 1. In the embodiment of the present invention, an embodiment of the method for fusing the pose and pose of the IMU and the rigid body includes:

101、确定惯性测量单元IMU与刚体之间的位置关系；101. Determine the positional relationship between the inertial measurement unit IMU and the rigid body;

可以理解的是，本发明的执行主体可以为IMU与刚体的位姿融合装置，还可以是终端或者服务器，具体此处不做限定。本发明实施例以服务器为执行主体为例进行说明。It is understandable that the execution subject of the present invention may be a pose fusion device of an IMU and a rigid body, and may also be a terminal or a server, which is not specifically limited here. The embodiment of the present invention is described by taking the server as the execution subject as an example.

服务器确定惯性测量单元IMU与刚体之间的位置关系。The server determines the positional relationship between the inertial measurement unit IMU and the rigid body.

需要说明的是，惯性测量单元(Inertial measurement unit，IMU)为测量物体三轴姿态角(或角速率)以及加速度的装置。通常一个IMU包含了三个单轴的加速度计和三个单轴的陀螺仪，加速度计分别测量刚体在载体坐标***独立三个轴向的加速度数据，而陀螺仪则测量载体相对于导航坐标系的角速度数据，由此可测量得到刚体的IMU姿态数据，包括刚体在三维空间中三个轴向上的角速度和加速度。IMU具有不同的型号，每种型号采用的采集频率可能不同，在本申请中不对IMU的型号进行限定。It should be noted that an inertial measurement unit (IMU) is a device that measures the three-axis attitude angle (or angular rate) and acceleration of an object. Usually an IMU contains three single-axis accelerometers and three single-axis gyroscopes. The accelerometer measures the acceleration data of the rigid body in the independent three axes of the carrier coordinate system, while the gyroscope measures the carrier relative to the navigation coordinate system. The angular velocity data of the rigid body can be measured to obtain the IMU posture data of the rigid body, including the angular velocity and acceleration of the rigid body in the three axial directions in the three-dimensional space. The IMU has different models, and the acquisition frequency used by each model may be different, and the model of the IMU is not limited in this application.

刚体(rigid body)是一种有限尺寸，不论是否感受到外力都可以忽略形变的固体，这里的忽略形变指的是在刚体的内部，质点与质点之间的距离不会改变。A rigid body is a solid with a finite size, and the deformation can be ignored regardless of external force. The ignoring deformation here means that the distance between the mass point and the mass point does not change in the interior of the rigid body.

在检测刚体的位姿数据时，需要将IMU固定在刚体上，当刚体携带IMU运动时，IMU会检测出IMU的测量位姿数据，进而进行后续步骤的操作。这里的刚体携带IMU做的运动可以为匀速直线运动，也可以为变速运动，在本申请中并不对刚体携带IMU所做的运动进行限定。When detecting the pose data of a rigid body, the IMU needs to be fixed on the rigid body. When the rigid body is moving with the IMU, the IMU will detect the measured pose data of the IMU, and then proceed to the subsequent steps. The motion performed by the rigid body carrying the IMU may be a uniform linear motion or a variable speed motion. The motion performed by the rigid body carrying the IMU is not limited in this application.

102、当刚体携带IMU运动时，获取IMU的测量姿态数据，并通过IMU的测量姿态数据计算得到IMU的待融合姿态数据；102. When the rigid body is moving with the IMU, obtain the measured posture data of the IMU, and calculate the to-be-fused posture data of the IMU through the measured posture data of the IMU;

当刚体携带IMU运动时，服务器获取IMU的测量姿态数据，并通过IMU的测量姿态数据计算得到IMU的待融合姿态数据。When the rigid body carries the IMU in motion, the server obtains the measured posture data of the IMU, and calculates the to-be-fused posture data of the IMU through the measured posture data of the IMU.

可以理解的是，当刚体携带IMU运动时，IMU可以通过测量得到运动时的加速度以及角速度，并根据IMU原始设置的标准加速度偏置、标准角速度偏置、标准加速度噪声及标准角速度噪声，计算得到IMU的待融合姿态数据。It is understandable that when a rigid body is moving with an IMU, the acceleration and angular velocity of the IMU can be obtained by measuring the acceleration and angular velocity during movement, and calculated according to the standard acceleration offset, standard angular velocity offset, standard acceleration noise and standard angular velocity noise originally set by the IMU The posture data of the IMU to be fused.

进一步说明的是，这里的标准加速度偏置、标准角速度偏置、标准加速度噪声及标准角速度噪声均是已知的，一般情况下，IMU硬件制造厂均会对出厂的IMU设备进行测试，并通过大量的测试数据确定IMU的标准加速度偏置、标准角速度偏置、标准加速度噪声及标准角速度噪声，因此，在本申请中，并不对标准加速度偏置、标准角速度偏置、标准加速度噪声及标准角速度噪声的数值进行限定，不同的IMU设有不同的数值。It is further explained that the standard acceleration offset, standard angular velocity offset, standard acceleration noise, and standard angular velocity noise are all known here. Generally, IMU hardware manufacturers will test the IMU equipment shipped and pass A large amount of test data determines the standard acceleration offset, standard angular velocity offset, standard acceleration noise, and standard angular velocity noise of the IMU. Therefore, in this application, the standard acceleration offset, standard angular velocity offset, standard acceleration noise, and standard angular velocity The value of noise is limited, and different IMUs have different values.

103、在预置的世界坐标系中，获取刚体的测量姿态数据，并根据IMU的测量姿态数据、刚体的测量姿态数据以及IMU与刚体之间的位置关系确定刚体的候选姿态数据；103. Obtain the measured posture data of the rigid body in the preset world coordinate system, and determine the candidate posture data of the rigid body according to the measured posture data of the IMU, the measured posture data of the rigid body, and the position relationship between the IMU and the rigid body;

服务器在预置的世界坐标系中，获取刚体的测量姿态数据，并根据IMU的测量姿态数据、刚体的测量姿态数据以及IMU与刚体之间的位置关系确定刚体的候选姿态数据。The server obtains the measured posture data of the rigid body in the preset world coordinate system, and determines the candidate posture data of the rigid body according to the measured posture data of the IMU, the measured posture data of the rigid body, and the positional relationship between the IMU and the rigid body.

需要说明的是，因IMU与刚体所处的位置不同，当刚体携带IMU运动时，两者所处的坐标系不同，因此需要对IMU的姿态进行坐标格式的转换，将IMU的姿态转换为刚体的姿态，也就是利用获取到的测量姿态数据计算得到IMU从IMU坐标系中转化到预置世界坐标系中的转化矩阵，实现两者在坐标格式上的标定，由此得到IMU与刚体之间的姿态转化关系，进而将IMU的姿态与刚体的姿态进行融合标定时，可更快速地得到刚体更为精确的位姿。It should be noted that because the position of the IMU and the rigid body are different, when the rigid body carries the IMU, the coordinate system of the two is different. Therefore, the coordinate format of the IMU posture needs to be converted, and the posture of the IMU is converted to a rigid body. The posture, that is, the conversion matrix of the IMU from the IMU coordinate system to the preset world coordinate system is calculated by using the acquired measurement posture data, so as to realize the calibration of the two in the coordinate format, and thus obtain the gap between the IMU and the rigid body The posture transformation relationship of the IMU and the posture of the rigid body can be fused and calibrated to obtain the more accurate posture of the rigid body more quickly.

104、利用融合算法将IMU的待融合姿态数据与刚体的候选姿态数据融合，得到刚体的融合位姿数据。104. Use a fusion algorithm to fuse the posture data of the IMU to be fused with the candidate posture data of the rigid body to obtain the fused posture data of the rigid body.

服务器利用融合算法将IMU的待融合姿态数据与刚体的候选姿态数据融合，得到刚体的融合位姿数据。The server uses a fusion algorithm to fuse the posture data of the IMU to be fused with the candidate posture data of the rigid body to obtain the fused posture data of the rigid body.

在服务器获取到IMU的待融合姿态数据以及刚体的候选姿态数据之后，需要进一步的将两者融合，进而得到刚体的融合位姿数据。刚体的融合位姿数据包括刚体的位置数据以及刚体的姿态数据，影响刚***置数据的因素有刚体的候选位置数据、候选位移数据以及IMU从IMU坐标到刚体坐标的旋转矩阵，根据上述因素以及位置融合公式即可计算得到刚体的位置数据；影响刚体姿态数据的因素有刚体的姿态四元数、IMU从IMU坐标到刚体坐标的旋转矩阵以及IMU的带融合姿态数据，根据上述因素以及姿态融合公式即可计算得到刚体的姿态数据，将刚体的位置数据与刚体的姿态数据相结合，得到刚体的融合位姿数据。After the server obtains the posture data of the IMU to be fused and the candidate posture data of the rigid body, it needs to further fuse the two to obtain the fused posture data of the rigid body. The fusion pose data of the rigid body includes the position data of the rigid body and the posture data of the rigid body. The factors that affect the position data of the rigid body include the candidate position data of the rigid body, the candidate displacement data and the IMU rotation matrix from the IMU coordinates to the rigid body coordinates. According to the above factors and position The position data of the rigid body can be calculated by the fusion formula; the factors that affect the rigid body posture data include the posture quaternion of the rigid body, the rotation matrix of the IMU from the IMU coordinate to the rigid body coordinate, and the fusion posture data of the IMU. According to the above factors and the posture fusion formula The posture data of the rigid body can be calculated, and the position data of the rigid body is combined with the posture data of the rigid body to obtain the fused pose data of the rigid body.

本发明实施例中，通过获取刚体携带IMU运动时IMU的测量姿态数据，并根据IMU与刚体之间的转换关系确定刚体的候选姿态数据，最后利用融合函数将IMU的测量姿态数据与刚体的候选姿态数据进行融合，得到刚体的融合位姿数据，减少了刚体的测量位姿数据与真实位姿数据之间的误差，提高了测量位姿数据的精确度。In the embodiment of the present invention, the measured posture data of the IMU is acquired when the rigid body carries the IMU, and the candidate posture data of the rigid body is determined according to the conversion relationship between the IMU and the rigid body. Finally, the fusion function is used to combine the measured posture data of the IMU with the rigid body candidate The pose data is fused to obtain the fused pose data of the rigid body, which reduces the error between the measured pose data of the rigid body and the real pose data, and improves the accuracy of the measured pose data.

请参阅图2，本发明实施例中IMU与刚体的位姿融合方法的另一个实施例包括：Please refer to FIG. 2, another embodiment of the method for fusing the pose of the IMU and the rigid body in the embodiment of the present invention includes:

201、确定惯性测量单元IMU与刚体之间的位置关系；201. Determine the positional relationship between the inertial measurement unit IMU and the rigid body;

服务器确定惯性测量单元IMU与刚体之间的位置关系。其中，惯性测量单元为测量物体三轴姿态角以及加速度的装置。通常一个IMU包含了三个单轴的加速度计和三个单轴的陀螺仪，加速度计分别测量刚体在载体坐标***独立三个轴向的加速度数据，而陀螺仪则测量载体相对于导航坐标系的角速度数据，由此可测量得到刚体的IMU姿态数据，包括刚体在三维空间中三个轴向上的角速度和加速度。IMU具有不同的型号，每种型号采用的采集频率可能不同，在本申请中不对IMU的型号进行限定。The server determines the positional relationship between the inertial measurement unit IMU and the rigid body. Among them, the inertial measurement unit is a device that measures the three-axis attitude angle and acceleration of an object. Usually an IMU contains three single-axis accelerometers and three single-axis gyroscopes. The accelerometer measures the acceleration data of the rigid body in the independent three axes of the carrier coordinate system, while the gyroscope measures the carrier relative to the navigation coordinate system. The angular velocity data of the rigid body can be measured to obtain the IMU posture data of the rigid body, including the angular velocity and acceleration of the rigid body in the three axial directions in the three-dimensional space. The IMU has different models, and the acquisition frequency used by each model may be different, and the model of the IMU is not limited in this application.

刚体是一种有限尺寸，不论是否感受到外力都可以忽略形变的固体，这里的忽略形变指的是在刚体的内部，质点与质点之间的距离不会改变。A rigid body is a solid with a finite size, and the deformation can be ignored regardless of external force. The ignoring deformation here means that the distance between the mass point and the mass point does not change in the interior of the rigid body.

在检测刚体的位姿数据时，需要将IMU固定在刚体上，当刚体携带IMU运动时，IMU会显示出IMU的测量位姿数据，进而进行后续步骤的操作。这里的刚体携带IMU做的运动可以为匀速直线运动，也可以为变速运动，在本申请中并不对刚体携带IMU所做的运动进行限定。When detecting the pose data of a rigid body, the IMU needs to be fixed on the rigid body. When the rigid body carries the IMU in motion, the IMU will display the measured pose data of the IMU, and then proceed to the subsequent steps. The motion performed by the rigid body carrying the IMU may be a uniform linear motion or a variable speed motion. The motion performed by the rigid body carrying the IMU is not limited in this application.

202、获取IMU的标准姿态数据，标准位姿态据包括标准加速度偏置、标准角速度偏置、标准加速度噪声及标准角速度噪声；202. Obtain the standard posture data of the IMU. The standard posture data includes standard acceleration offset, standard angular velocity offset, standard acceleration noise, and standard angular velocity noise;

服务器获取IMU的标准姿态数据，标准位姿态据包括标准加速度偏置、标准角速度偏置、标准加速度噪声及标准角速度噪声。The server obtains the standard posture data of the IMU, and the standard posture data includes standard acceleration offset, standard angular velocity offset, standard acceleration noise, and standard angular velocity noise.

这里IMU的标准姿态数据为已知的参数，其中，标准位姿态据包括：标准加速度偏置、标准角速度偏置、标准加速度噪声及标准角速度噪声，一般情况下，IMU硬件制造厂均会对出厂的IMU设备进行测试，并通过大量的测试数据确定IMU的标准加速度偏置、标准角速度偏置、标准加速度噪声及标准角速度噪声。The standard attitude data of the IMU here are known parameters. Among them, the standard attitude data includes: standard acceleration offset, standard angular velocity offset, standard acceleration noise, and standard angular velocity noise. In general, the IMU hardware manufacturer will The IMU equipment is tested, and a large amount of test data is used to determine the standard acceleration offset, standard angular velocity offset, standard acceleration noise and standard angular velocity noise of the IMU.

203、当刚体携带IMU运动时，获取IMU的测量姿态数据，并IMU的测量姿态数据以及标准姿态数据确定待融合姿态数据，其中，待融合姿态数据包括待融合加速度以及待融合角速度；203. When the rigid body is moving with the IMU, acquire the measured posture data of the IMU, and determine the posture data to be fused by the measured posture data of the IMU and the standard posture data, where the posture data to be fused includes the acceleration to be fused and the angular velocity to be fused;

服务器当刚体携带IMU运动时，获取IMU的测量姿态数据，并根据IMU的测量姿态数据以及标准姿态数据确定待融合姿态数据，其中，待融合姿态数据包括待融合加速度以及待融合角速度。具体的：The server acquires the measured posture data of the IMU when the rigid body is moving with the IMU, and determines the posture data to be fused according to the measured posture data of the IMU and the standard posture data, where the posture data to be fused includes the acceleration to be fused and the angular velocity to be fused. specific:

服务器获取刚体携带IMU运动时IMU的测量姿态数据，其中，IMU的测量姿态数据包括测量加速度及测量角速度；服务器根据第一预置公式、标准姿态数据及测量姿态数据计算待融合加速度，第一预置公式为：

其中，a _t表示测量加速度，a _t(real)表示待融合加速度，

表示t时刻的标准加速度偏置，

表示t时刻IMU姿态下重力方向分量，n _a为标准加速度噪声；服务器根据第二预置公式、标准姿态数据及测量姿态数据计算待融合角速度，第二预置公式为：

其中，w _t表示测量角速度，w _t(real)表示待融合角速度，

表示t时刻的标准角速度偏置，n _w表示标准加速度噪声；服务器将待融合加速度以及待融合角速度合并，得到待融合姿态数据。 The server obtains the measured posture data of the IMU when the rigid body is carrying the IMU. The measured posture data of the IMU includes the measured acceleration and the measured angular velocity; the server calculates the acceleration to be fused according to the first preset formula, the standard posture data and the measured posture data, and the first preset The setting formula is:

Represents the standard acceleration bias at time t,

Represents the gravity direction component in the IMU attitude at time t, and n _a is the standard acceleration noise; the server calculates the angular velocity to be fused according to the second preset formula, the standard attitude data, and the measured attitude data. The second preset formula is:

It represents the standard angular velocity offset at time t, and _nw represents the standard acceleration noise; the server combines the acceleration to be fused and the angular velocity to be fused to obtain the posture data to be fused.

在刚体携带IMU做移动运动时，获取IMU的测量姿态数据，这里的测量姿态数据包括IMU输出的测量加速度以及测量角速度，因在刚体携带IMU进行运动时，会受到刚体重量以及加速度噪声的影响，导致测量的加速度变小，因此，服务器通过第一预置公式计算理论上的真实加速度也就是待融合加速度。同理可知，在计算刚体的角速度时，也会受到刚体重量以及角速度噪声的影响，导致测量的角速度变小，因而，服务器通过第二预置公式计算理论上的真实角速度也就是待融合角速度。When a rigid body carries an IMU for movement, obtain the measured posture data of the IMU. The measured posture data here includes the measured acceleration and the measured angular velocity output by the IMU. When the rigid body carries the IMU, it will be affected by the weight of the rigid body and acceleration noise. As a result, the measured acceleration becomes smaller. Therefore, the server calculates the theoretical real acceleration through the first preset formula, which is the acceleration to be fused. In the same way, when calculating the angular velocity of a rigid body, it will also be affected by the weight of the rigid body and the angular velocity noise, which will cause the measured angular velocity to become smaller. Therefore, the server calculates the theoretical real angular velocity through the second preset formula, which is the angular velocity to be fused.

根据大量的测量姿态数据发现，标准加速度噪声n _a及标准加速度噪声n _w均服从高斯分布，其中，标准加速度噪声服从的分布为：

标准角速度噪声服从的分布为：

这里的σ _a与σ _w为高斯白噪声的方差，具体的数值由IMU的型号而定。此外，标准加速度偏置

及标准角速度偏置

的建模为随机游走，其中，标准加速度偏置的随机游走为

服从高斯分布

标准角速度偏置的随机游走为

服从高斯分布

这里的

与

分别为标准加速度偏置和标准角速度偏置的随机游走高斯白噪声的方差，具体的数值由IMU的型号而定。 The number of measurements found attitude data, and the standard criteria acceleration noise n _a n _w acceleration noise are Gaussian, wherein the standard compliant acceleration noise distribution:

The distribution of standard angular velocity noise is:

Here σ _a and σ _w are the variances of Gaussian white noise, and the specific value depends on the model of the IMU. In addition, the standard acceleration bias

And standard angular velocity offset

The modeling of is a random walk, where the random walk of the standard acceleration bias is

Obey the Gaussian distribution

The random walk of the standard angular velocity offset is

Obey the Gaussian distribution

here

and

They are the variances of the standard acceleration offset and the standard angular velocity offset of the random walk Gaussian white noise. The specific value depends on the model of the IMU.

可以理解的是，IMU以及刚体未运动，或者运动不明显，此时所获得的测量姿态数据是不变的，可认为是无效的姿态数据，而IMU以及刚体处于运动状态时获得的测量姿态数据才是有效的姿态数据。It is understandable that if the IMU and the rigid body are not moving, or the movement is not obvious, the measured posture data obtained at this time is unchanged and can be considered as invalid posture data, while the measured posture data obtained when the IMU and the rigid body are in motion It is valid posture data.

204、在预置的世界坐标系中，获取刚体的测量姿态数据，并根据IMU的测量姿态数据、刚体的测量姿态数据以及IMU与刚体之间的位置关系确定刚体的候选姿态数据；204. Obtain the measured posture data of the rigid body in the preset world coordinate system, and determine the candidate posture data of the rigid body according to the measured posture data of the IMU, the measured posture data of the rigid body, and the position relationship between the IMU and the rigid body;

服务器在预置的世界坐标系中，获取刚体的测量姿态数据，并根据IMU的测量姿态数据、刚体的测量姿态数据以及IMU与刚体之间的位置关系确定刚体的候选姿态数据。具体的：The server obtains the measured posture data of the rigid body in the preset world coordinate system, and determines the candidate posture data of the rigid body according to the measured posture data of the IMU, the measured posture data of the rigid body, and the positional relationship between the IMU and the rigid body. specific:

服务器首先获取刚体的测量姿态数据及IMU的测量姿态数据，在IMU的测量姿态数据中提取第0帧IMU测量姿态数据以及第n帧IMU测量姿态数据，在刚体的测量姿态数据中提取第0帧刚体到世界的测量姿态数据以及第n帧刚体到世界的测量姿态数据；然后服务器在预置的世界坐标系中，根据关系等式，计算第0帧IMU到世界的测量姿态数据，以及IMU从IMU坐标到刚体坐标的旋转矩阵，其中，关系等式为：The server first obtains the measured attitude data of the rigid body and the measured attitude data of the IMU, extracts the 0th frame IMU measured attitude data and the nth frame IMU measured attitude data from the IMU measured attitude data, and extracts the 0th frame from the rigid body measured attitude data The measured posture data of the rigid body to the world and the measured posture data of the nth rigid body to the world; then the server calculates the measured posture data of the 0th frame IMU to the world according to the relationship equation in the preset world coordinate system, and the IMU from The rotation matrix from IMU coordinates to rigid body coordinates, where the relational equation is:

其中，

表示第n帧刚体到世界的测量姿态数据，

表示第0帧IMU到世界的测量姿态数据，

表示第n帧IMU测量姿态数据相对于第0帧IMU测量姿态数据的逆矩阵，E用于指示单位矩阵；最后服务器根据姿态转换公式将IMU的姿态数据转化为刚体的姿态数据，得到刚体的候选姿态数据，姿态转换公式为： in,

Represents the measured posture data of the n-th rigid body to the world,

Represents the measured attitude data from the 0th frame IMU to the world,

Represents the inverse matrix of the IMU measured attitude data of the nth frame relative to the 0th frame IMU measured attitude data, E is used to indicate the identity matrix; finally the server converts the IMU attitude data into the attitude data of the rigid body according to the attitude conversion formula, and obtains the candidate of the rigid body The posture data, the posture conversion formula is:

其中，

表示经过转换后刚体的候选姿态数据，

表示第0帧IMU到世界的测量姿态数据，

Represents the candidate pose data of the rigid body after transformation,

Represents the measured attitude data from the 0th frame IMU to the world,

可以理解的是，在利用刚体的测量姿态数据进行坐标格式的转换时，测量姿态数据中包含了多个帧的测量姿态数据，其中，0帧IMU测量姿态数据为IMU初始测得的位置，n帧IMU测量姿态数据为IMU在第n帧时的位置，第0帧刚体到世界的测量姿态数据为刚体初始测得的位置。服务器通过关系等式计算IMU测量姿态数据从IMU坐标到刚体坐标的旋转矩阵，以及计算第0帧IMU到世界的测量姿态数据，其中，关系等式为：It is understandable that when using the measured posture data of a rigid body to convert the coordinate format, the measured posture data contains multiple frames of measured posture data. Among them, the 0 frame IMU measured posture data is the initial measured position of the IMU, n The measured posture data of the frame IMU is the position of the IMU in the nth frame, and the measured posture data of the rigid body to the world in the 0th frame is the initial measured position of the rigid body. The server calculates the rotation matrix of the IMU measured posture data from IMU coordinates to rigid body coordinates through the relational equation, and calculates the measured posture data from the 0th frame IMU to the world, where the relational equation is:

其中，

表示第n帧刚体到世界的测量姿态数据，

表示IMU测量姿态数据从IMU坐标到刚体坐标的旋转矩阵，[R _i] _n[R _i] _n表示第n帧IMU测量姿态数据，

表示第0帧IMU到世界的测量姿态数据，

表示第n帧IMU测量姿态数据相对于第0帧IMU测量姿态数据的逆矩阵，E用于指示单位矩阵。 in,

Represents the measured posture data of the n-th rigid body to the world,

Represents the rotation matrix of IMU measurement posture data from IMU coordinates to rigid body coordinates, [R _i ] _n [R _i ] _n represents the nth frame of IMU measurement posture data,

Represents the measured attitude data from the 0th frame IMU to the world,

Represents the inverse matrix of the IMU measurement attitude data of the nth frame relative to the IMU measurement attitude data of the 0th frame, and E is used to indicate the identity matrix.

具体的：服务器将第0帧IMU测量姿态数据、第n帧IMU测量姿态数据、第0帧刚体到世界的测量姿态数据以及第n帧刚体到世界的测量姿态数据代入关系等式，并通过最小二乘法，当关系等式的等号两边数据之差的平方和为最小时求得

和

其中，最小二乘法公式Q包括：

Specifically: the server substitutes the 0th frame IMU measurement posture data, the nth frame IMU measurement posture data, the 0th frame rigid body to the world measurement posture data, and the nth frame rigid body to the world measurement posture data into the relation equation, and passes the minimum Two multiplication method, when the sum of the squares of the difference between the data on both sides of the equal sign of the relational equation is the smallest

with

Among them, the least squares formula Q includes:

进一步的：服务器在计算得到IMU测量姿态数据从IMU坐标到刚体坐标的旋转矩阵，以及第0帧IMU到世界的测量姿态数据后，即可将两者代入姿态转换公式，确定IMU测量姿态数据从IMU坐标转化到刚体坐标时的转换公式，其中，姿态转换公式为：Further: After the server has calculated the rotation matrix of the IMU measured attitude data from IMU coordinates to rigid body coordinates, and the measured attitude data of the 0th frame IMU to the world, it can substitute the two into the attitude conversion formula to determine the IMU measured attitude data from The conversion formula when IMU coordinates are converted to rigid body coordinates. Among them, the posture conversion formula is:

其中，

为IMU姿态数据从IMU坐标到刚体坐标的旋转矩阵的逆变换，

为经过转换后的刚体的姿态数据。 in,

Is the inverse transformation of the rotation matrix of IMU posture data from IMU coordinates to rigid body coordinates,

It is the posture data of the rigid body after transformation.

205、利用融合算法将IMU的待融合姿态数据与刚体的候选姿态数据融合，得到刚体的融合位姿数据。205. Use a fusion algorithm to fuse the posture data of the IMU to be fused with the candidate posture data of the rigid body to obtain the fused posture data of the rigid body.

服务器利用融合算法将IMU的待融合姿态数据与刚体的候选姿态数据融合，得到刚体的融合位姿数据。具体的：The server uses a fusion algorithm to fuse the posture data of the IMU to be fused with the candidate posture data of the rigid body to obtain the fused posture data of the rigid body. specific:

首先服务器提取刚体的候选姿态数据中的姿态四元数，并分别获取刚体的候选位置数据以及候选位移数据、IMU的姿态四元数及IMU从IMU坐标到刚体坐标的旋转矩阵；其次服务器根据融合算法中的位置融合公式、IMU的测量姿态数据、IMU的待融合姿态数据计算刚体的融合位置，位置融合公式为：First, the server extracts the posture quaternion in the candidate posture data of the rigid body, and obtains the candidate position data and the candidate displacement data of the rigid body, the posture quaternion of the IMU, and the rotation matrix of the IMU from the IMU coordinate to the rigid body coordinate; The position fusion formula in the algorithm, the measured posture data of the IMU, and the posture data to be fused of the IMU calculate the fusion position of the rigid body. The position fusion formula is:

其中，

表示在k+1时刻时刚***于预置的世界坐标系中的候选位置，

表示IMU从IMU坐标到刚体坐标的旋转矩阵，

表示k时刻IMU的速度，

表示t时刻的标准加速度偏置，n _a为标准加速度噪声，

表示t时刻IMU姿态下重力方向分量，

表示将IMU的速度和待融合加速度投影到预置的世界坐标系下时，[t _k，t _k+1]时间段中待融合加速度产生的位移量；然后服务器根据融合算法中的姿态融合公式、IMU的测量姿态数据、待融合姿态数据计算刚体的融合姿态，姿态融合公式为： in,

Represents the speed of the IMU at time k,

Represents the gravity direction component of the IMU attitude at time t,

_{Indicates the displacement generated by the acceleration to be fused in the time period [t k} , t _k+1 ] when the velocity of the IMU and the acceleration to be fused are projected to the preset world coordinate system; then the server according to the attitude fusion formula in the fusion algorithm , IMU's measured posture data and posture data to be fused to calculate the fusion posture of the rigid body, the posture fusion formula is:

其中，

表示在k+1时刻时刚***于预置的世界坐标系中的姿态四元数，

表示在k时刻时刚***于预置的世界坐标系中的姿态四元数，

表示四元数的乘法计算符号，β表示姿态融合系数，

表示IMU从IMU坐标到刚体坐标的旋转矩阵，

为k时刻IMU的姿态四元数，w _t表示测量角速度，

表示t时刻的标准角速度偏置，n _w表示标准加速度噪声，

表示[t _k，t _k+1]时段中IMU产生的旋转变化；最后服务器将刚体的融合位置与刚体的融合姿态合并，确定刚体的融合位姿数据。 in,

Represents the rotation change of the IMU during the period of [t _k , t _k+1 ]; finally, the server merges the fusion position of the rigid body with the fusion posture of the rigid body to determine the fusion pose data of the rigid body.

在服务器获取到IMU的待融合姿态数据以及刚体的候选姿态数据之后，需要进一步的将两者融合，进而得到刚体的融合位姿数据。首先服务器先进行刚体的位置融合，服务器通过IMU的测量加速度计算IMU的测量速度，通过对测量加速度a _t积分即可得到速度，因IMU在进行测量时，有噪声以及重力的影响使得速度值偏小，因此由该公式计算k时刻IMU的速度，该公式为： After the server obtains the posture data of the IMU to be fused and the candidate posture data of the rigid body, it needs to further fuse the two to obtain the fused posture data of the rigid body. First, the location server first rigid body fusion server computing IMU measurement speed by measuring the acceleration of the IMU, a _t by integrating the acceleration measuring velocity can be obtained, because during the IMU measurement, there is the influence of noise and bias of gravity so that the speed values Is small, so the IMU speed at time k is calculated by this formula, which is:

在式中，a _t表示测量加速度，

表示k时刻IMU的速度，

表示t时刻的标准加速度偏置，n _a为标准加速度噪声，

表示t时刻IMU姿态下重力方向分量。将计算得到的

以及获取到的测量姿态参数代入位置融合公式中，确定刚体的位姿数据。需要说明的是，α为位置融合系数，即刚体在k时刻的候选位移与IMU在k时刻的测量位移的比例，根据IMU帧间位移积分精度决定取值，在本申请中的取值为0.15。 In the formula, a _t represents the measurement of acceleration,

Represents the speed of the IMU at time k,

Represents the gravity direction component of the IMU attitude at time t. Will be calculated

And the acquired measured attitude parameters are substituted into the position fusion formula to determine the pose data of the rigid body. It should be noted that α is the position fusion coefficient, that is, the ratio of the candidate displacement of the rigid body at time k to the measured displacement of the IMU at time k. The value is determined according to the displacement integration accuracy between IMU frames, and the value in this application is 0.15 .

待服务器得到融合位置数据后，服务器进行姿态数据的融合。服务器首先获取刚体在k时刻时刚***于预置的世界坐标系中的姿态四元数

再获取k时刻IMU的姿态四元数

并将两者以及获取到的其他姿态数据代入姿态融合公式中，确定刚体的姿态数据。需要说明的是，β为姿态融合系数，根据光学姿态计算精度和IMU姿态数据精度情况设置，在本发明中的取值为0.75。 After the server obtains the fusion position data, the server performs the fusion of the posture data. The server first obtains the posture quaternion of the rigid body in the preset world coordinate system at time k

Then get the quaternion of the posture of the IMU at time k

The two and other acquired posture data are substituted into the posture fusion formula to determine the posture data of the rigid body. It should be noted that β is the attitude fusion coefficient, which is set according to the calculation accuracy of the optical attitude and the accuracy of the IMU attitude data, and the value is 0.75 in the present invention.

上面对本发明实施例中IMU与刚体的位姿融合方法进行了描述，下面对本发明实施例中IMU与刚体的位姿融合装置进行描述，请参阅图3，本发明实施例中IMU与刚体的位姿融合装置一个实施例包括：The pose fusion method of the IMU and the rigid body in the embodiment of the present invention is described above, and the pose fusion device of the IMU and the rigid body in the embodiment of the present invention is described below. An embodiment of the attitude fusion device includes:

确定模块301，用于确定惯性测量单元IMU与刚体之间的位置关系；The determining module 301 is used to determine the positional relationship between the inertial measurement unit IMU and the rigid body;

获取并计算模块302，当刚体携带IMU运动时，用于获取IMU的测量姿态数据，并通过IMU的测量姿态数据计算得到IMU的待融合姿态数据；The obtaining and calculating module 302 is used to obtain the measured posture data of the IMU when the rigid body is moving with the IMU, and calculate the to-be-fused posture data of the IMU through the measured posture data of the IMU;

获取并确定模块303，用于在预置的世界坐标系中，获取刚体的测量姿态数据，并根据IMU的测量姿态数据、刚体的测量姿态数据以及IMU与刚体之间的位置关系确定刚体的候选姿态数据；The obtaining and determining module 303 is used to obtain the measured posture data of the rigid body in the preset world coordinate system, and determine the candidate of the rigid body according to the measured posture data of the IMU, the measured posture data of the rigid body, and the positional relationship between the IMU and the rigid body Attitude data

融合模块304，用于利用融合算法将IMU的待融合姿态数据与刚体的候选姿态数据融合，得到刚体的融合位姿数据。The fusion module 304 is used for fusing the posture data of the IMU to be fused with the candidate posture data of the rigid body by using the fusion algorithm to obtain the fused posture data of the rigid body.

请参阅图4，本发明实施例中IMU与刚体的位姿融合装置的另一个实施例包括：Please refer to Fig. 4, another embodiment of the device for fusing the pose of the IMU and the rigid body in the embodiment of the present invention includes:

可选的，获取并计算模块302包括：Optionally, the acquiring and calculating module 302 includes:

获取单元3021，用于获取IMU的标准姿态数据，标准位姿态据包括标准加速度偏置、标准角速度偏置、标准加速度噪声及标准角速度噪声；The obtaining unit 3021 is used to obtain standard posture data of the IMU. The standard posture data includes standard acceleration offset, standard angular velocity offset, standard acceleration noise, and standard angular velocity noise;

确定单元3022，当刚体携带IMU运动时，用于获取IMU的测量姿态数据，并根据IMU的测量姿态数据以及标准姿态数据确定待融合姿态数据，其中，待融合姿态数据包括待融合加速度以及待融合角速度。The determining unit 3022 is used to obtain the measured posture data of the IMU when the rigid body is moving with the IMU, and determine the posture data to be fused according to the measured posture data of the IMU and the standard posture data, where the posture data to be fused includes acceleration to be fused and acceleration to be fused Angular velocity.

可选的，确定单元3022还可以具体用于：Optionally, the determining unit 3022 may also be specifically configured to:

获取刚体携带IMU运动时IMU的测量姿态数据，其中，IMU的测量姿态数据包括测量加速度及测量角速度；Obtain the measured posture data of the IMU when the rigid body is moving with the IMU, where the measured posture data of the IMU includes the measured acceleration and the measured angular velocity;

根据第一预置公式、标准姿态数据及IMU的测量姿态数据计算待融合加速度，第一预置公式为：

其中，a _t表示测量加速度，a _t(real)表示待融合加速度，

表示t时刻的标准加速度偏置，

表示t时刻IMU姿态下重力方向分量，n _a为标准加速度噪声； The acceleration to be fused is calculated according to the first preset formula, the standard attitude data and the measured attitude data of the IMU. The first preset formula is:

Represents the standard acceleration bias at time t,

Indicates the component of the direction of gravity in the IMU attitude at time t, and n _a is the standard acceleration noise;

根据第二预置公式、标准姿态数据及IMU的测量姿态数据计算待融合角速度，第二预置公式为：

其中，w _t表示测量角速度，w _t(real)表示待融合角速度，

表示t时刻的标准角速度偏置，n _w表示标准加速度噪声； The angular velocity to be fused is calculated according to the second preset formula, the standard attitude data and the measured attitude data of the IMU. The second preset formula is:

Represents the standard angular velocity offset at time t, n _w represents the standard acceleration noise;

将待融合加速度以及待融合角速度合并，得到待融合姿态数据。The acceleration to be fused and the angular velocity to be fused are combined to obtain the posture data to be fused.

可选的，获取并确定模块303还可以具体用于：Optionally, the acquiring and determining module 303 may also be specifically used for:

获取刚体的测量姿态数据及IMU的测量姿态数据，在IMU的测量姿态数据中提取第0帧IMU测量姿态数据以及第n帧IMU测量姿态数据，在刚体的测量姿态数据中提取第0帧刚体到世界的测量姿态数据以及第n帧刚体到世界的测量姿态数据；Obtain the measured attitude data of the rigid body and the measured attitude data of the IMU, extract the 0th frame IMU measured attitude data and the nth frame IMU measured attitude data from the IMU measured attitude data, and extract the 0th frame from the rigid body measured attitude data to The measured attitude data of the world and the measured attitude data of the nth frame from the rigid body to the world;

在预置的世界坐标系中，根据关系等式，计算第0帧IMU到世界的测量姿态数据，以及IMU从IMU坐标到刚体坐标的旋转矩阵，其中，关系等式为：In the preset world coordinate system, calculate the measured posture data from the 0th frame IMU to the world and the rotation matrix of the IMU from the IMU coordinates to the rigid body coordinates according to the relation equation. The relation equation is:

其中，

表示第n帧刚体到世界的测量姿态数据，

表示第0帧IMU到世界的测量姿态数据，

表示第n帧IMU测量姿态数据相对于第0帧IMU测量姿态数据的逆矩阵，E用于指示单位矩阵； in,

Represents the measured posture data of the n-th rigid body to the world,

Represents the measured attitude data from the 0th frame IMU to the world,

Represents the inverse matrix of the IMU measurement attitude data of the nth frame relative to the IMU measurement attitude data of the 0th frame, and E is used to indicate the identity matrix;

根据姿态转换公式将IMU的姿态数据转化为刚体的姿态数据，得到刚体的候选姿态数据，姿态转换公式为：According to the posture conversion formula, the posture data of the IMU is converted into the posture data of the rigid body, and the candidate posture data of the rigid body is obtained. The posture conversion formula is:

其中，

表示经过转换后刚体的候选姿态数据，

表示第0帧IMU到世界的测量姿态数据，

Represents the candidate pose data of the rigid body after transformation,

Represents the measured attitude data from the 0th frame IMU to the world,

可选的，融合模块304还可以具体用于：Optionally, the fusion module 304 may also be specifically used for:

提取刚体的候选姿态数据中的姿态四元数，并分别获取刚体的候选位置数据以及候选位移数据、IMU的姿态四元数及IMU从IMU坐标到刚体坐标的旋转矩阵；Extract the posture quaternion in the candidate posture data of the rigid body, and obtain the candidate position data and the candidate displacement data of the rigid body, the posture quaternion of the IMU, and the rotation matrix of the IMU from the IMU coordinates to the rigid body coordinates respectively;

根据融合算法中的位置融合公式、IMU的测量姿态数据、IMU的待融合姿态数据计算刚体的融合位置，位置融合公式为：Calculate the fusion position of the rigid body according to the position fusion formula in the fusion algorithm, the measured posture data of the IMU, and the posture data to be fused of the IMU. The position fusion formula is:

其中，

表示在k+1时刻时刚***于预置的世界坐标系中的候选位置，

表示IMU从IMU坐标到刚体坐标的旋转矩阵，

表示k时刻IMU的速度，

表示t时刻的标准加速度偏置，n _a为标准加速度噪声，

表示t时刻IMU姿态下重力方向分量，

表示将IMU的速度和待融合加速度投影到预置的世界坐标系下时，[t _k，t _k+1]时间段中待融合加速度产生的位移量； in,

Represents the speed of the IMU at time k,

Represents the gravity direction component of the IMU attitude at time t,

Indicates the displacement generated by the acceleration to be fused in the time period _{[t k} , t _k+1 ] when the velocity of the IMU and the acceleration to be fused are projected to the preset world coordinate system;

根据融合算法中的姿态融合公式、IMU的测量姿态数据、待融合姿态数据计算刚体的融合姿态，姿态融合公式为：According to the posture fusion formula in the fusion algorithm, the measured posture data of the IMU, and the posture data to be fused, the fusion posture of the rigid body is calculated. The posture fusion formula is:

其中，

表示在k+1时刻时刚***于预置的世界坐标系中的姿态四元数，

表示在k时刻时刚***于预置的世界坐标系中的姿态四元数，

表示四元数的乘法计算符号，β表示姿态融合系数，

表示IMU从IMU坐标到刚体坐标的旋转矩阵，

为k时刻IMU的姿态四元数，w _t表示测量角速度，

表示t时刻的标准角速度偏置，n _w表示标准加速度噪声，

表示[t _k，t _k+1]时段中IMU产生的旋转变化； in,

Represents the rotation change produced by the IMU in the period of [t _k , t _{k+1 ];}

将刚体的融合位置与刚体的融合姿态合并，确定刚体的融合位姿数据。The fusion position of the rigid body is merged with the fusion posture of the rigid body to determine the fusion pose data of the rigid body.

上面图3和图4从模块化功能实体的角度对本发明实施例中的IMU与刚体的位姿融合装置进行详细描述，下面从硬件处理的角度对本发明实施例中IMU与刚体的位姿融合设备进行详细描述。The above figures 3 and 4 describe in detail the pose fusion device of the IMU and rigid body in the embodiment of the present invention from the perspective of the modular functional entity. The following describes the pose fusion device of the IMU and rigid body in the embodiment of the present invention from the perspective of hardware processing. Give a detailed description.

图5是本发明实施例提供的一种IMU与刚体的位姿融合设备的结构示意图，该IMU与刚体的位姿融合设备500可因配置或性能不同而产生比较大的差异，可以包括一个或一个以上处理器(central processing units，CPU)510(例如，一个或一个以上处理器)和存储器520，一个或一个以上存储应用程序533或数据532的存储介质530(例如一个或一个以上海量存储设备)。其中，存储器520和存储介质530可以是短暂存储或持久存储。存储在存储介质530的程序可以包括一个或一个以上模块(图示没标出)，每个模块可以包括对IMU与刚体的位姿融合设备500中的一系列指令操作。更进一步地，处理器510可以设置为与存储介质530通信，在IMU与刚体的位姿融合设备500上执行存储介质530中的一系列指令操作。Figure 5 is a schematic structural diagram of an IMU and rigid body pose fusion device provided by an embodiment of the present invention. The IMU and rigid body pose fusion device 500 may have relatively large differences due to different configurations or performances, and may include one or One or more central processing units (CPU) 510 (for example, one or more processors) and memory 520, one or more storage media 530 for storing application programs 533 or data 532 (for example, one or one storage device with a large amount of storage ). Among them, the memory 520 and the storage medium 530 may be short-term storage or persistent storage. The program stored in the storage medium 530 may include one or more modules (not shown in the figure), and each module may include a series of instruction operations on the IMU and rigid body pose fusion device 500. Furthermore, the processor 510 may be configured to communicate with the storage medium 530, and execute a series of instruction operations in the storage medium 530 on the IMU and the rigid body pose fusion device 500.

IMU与刚体的位姿融合设备500还可以包括一个或一个以上电源540，一个或一个以上有线或无线网络接口550，一个或一个以上输入输出接口560，和/或，一个或一个以上操作***531，例如Windows Serve，Mac OS X，Unix，Linux，FreeBSD等等。本领域技术人员可以理解，图5示出的IMU与刚体的位姿融合设备结构并不构成对IMU与刚体的位姿融合设备的限定，可以包括比图示更多或更少的部件，或者组合某些部件，或者不同的部件布置。The IMU and rigid body pose fusion device 500 may also include one or more power supplies 540, one or more wired or wireless network interfaces 550, one or more input and output interfaces 560, and/or, one or more operating systems 531 , Such as Windows Serve, Mac OS X, Unix, Linux, FreeBSD and so on. Those skilled in the art can understand that the structure of the pose fusion device of the IMU and the rigid body shown in FIG. 5 does not constitute a limitation on the pose fusion device of the IMU and the rigid body, and may include more or fewer components than shown in the figure, or Combining certain components, or different component arrangements.

本发明还提供一种计算机可读存储介质，该计算机可读存储介质可以为非易失性计算机可读存储介质，该计算机可读存储介质也可以为易失性计算机可读存储介质，所述计算机可读存储介质中存储有指令，当所述指令在计算机上运行时，使得计算机执行所述IMU与刚体的位姿融合方法的步骤。The present invention also provides a computer-readable storage medium. The computer-readable storage medium may be a non-volatile computer-readable storage medium, and the computer-readable storage medium may also be a volatile computer-readable storage medium. The computer-readable storage medium stores instructions, and when the instructions run on a computer, the computer executes the steps of the method for fusing the pose of the IMU and the rigid body.

所属领域的技术人员可以清楚地了解到，为描述的方便和简洁，上述描述的***，装置和单元的具体工作过程，可以参考前述方法实施例中的对应过程，在此不再赘述。Those skilled in the art can clearly understand that, for the convenience and conciseness of the description, the specific working process of the above-described system, device, and unit can refer to the corresponding process in the foregoing method embodiment, which will not be repeated here.

所述集成的单元如果以软件功能单元的形式实现并作为独立的产品销售或使用时，可以存储在一个计算机可读取存储介质中。基于这样的理解，本发明的技术方案本质上或者说对现有技术做出贡献的部分或者该技术方案的全部或部分可以以软件产品的形式体现出来，该计算机软件产品存储在一个存储介质中，包括若干指令用以使得一台计算机设备(可以是个人计算机，服务器，或者网络设备等)执行本发明各个实施例所述方法的全部或部分步骤。而前述的存储介质包括：U盘、移动硬盘、只读存储器(read-only memory，ROM)、随机存取存储器(random access memory，RAM)、磁碟或者光盘等各种可以存储程序代码的介质。If the integrated unit is implemented in the form of a software functional unit and sold or used as an independent product, it can be stored in a computer readable storage medium. Based on this understanding, the technical solution of the present invention essentially or the part that contributes to the existing technology or all or part of the technical solution can be embodied in the form of a software product, and the computer software product is stored in a storage medium. , Including several instructions to make a computer device (which may be a personal computer, a server, or a network device, etc.) execute all or part of the steps of the method in each embodiment of the present invention. The aforementioned storage media include: U disk, mobile hard disk, read-only memory (read-only memory, ROM), random access memory (random access memory, RAM), magnetic disks or optical disks and other media that can store program codes. .

以上所述，以上实施例仅用以说明本发明的技术方案，而非对其限制；尽管参照前述实施例对本发明进行了详细的说明，本领域的普通技术人员应当理解：其依然可以对前述各实施例所记载的技术方案进行修改，或者对其中部分技术特征进行等同替换；而这些修改或者替换，并不使相应技术方案的本质脱离本发明各实施例技术方案的精神和范围。As mentioned above, the above embodiments are only used to illustrate the technical solutions of the present invention, but not to limit them. Although the present invention has been described in detail with reference to the foregoing embodiments, those of ordinary skill in the art should understand that: The technical solutions recorded in the embodiments are modified, or some of the technical features are equivalently replaced; these modifications or replacements do not cause the essence of the corresponding technical solutions to deviate from the spirit and scope of the technical solutions of the embodiments of the present invention.

Claims

一种IMU与刚体的位姿融合方法，其特征在于，所述IMU与刚体的位姿融合方法包括：A method for fusing the pose of an IMU and a rigid body is characterized in that the method for fusing the pose of the IMU and a rigid body includes:

确定惯性测量单元IMU与刚体之间的位置关系；Determine the positional relationship between the inertial measurement unit IMU and the rigid body;

当所述刚体携带所述IMU运动时，获取所述IMU的测量姿态数据，并通过所述IMU的测量姿态数据计算得到所述IMU的待融合姿态数据；When the rigid body carries the IMU in motion, acquiring the measured posture data of the IMU, and calculating the to-be-fused posture data of the IMU through the measured posture data of the IMU;

在预置的世界坐标系中，获取刚体的测量姿态数据，并根据所述IMU的测量姿态数据、所述刚体的测量姿态数据以及所述IMU与刚体之间的位置关系确定刚体的候选姿态数据；In the preset world coordinate system, obtain the measured posture data of the rigid body, and determine the candidate posture data of the rigid body according to the measured posture data of the IMU, the measured posture data of the rigid body, and the positional relationship between the IMU and the rigid body ；

利用融合算法将所述IMU的待融合姿态数据与所述刚体的候选姿态数据融合，得到刚体的融合位姿数据。A fusion algorithm is used to fuse the to-be-fused posture data of the IMU with the candidate posture data of the rigid body to obtain the fused posture data of the rigid body.
根据权利要求1所述的IMU与刚体的位姿融合方法，其特征在于，所述当所述刚体携带所述IMU运动时，获取所述IMU的测量姿态数据，并通过所述IMU的测量姿态数据计算得到所述IMU的待融合姿态数据包括：The pose fusion method of an IMU and a rigid body according to claim 1, wherein when the rigid body carries the IMU in motion, the measured posture data of the IMU is acquired, and the measured posture of the IMU is passed The posture data to be fused of the IMU obtained by data calculation includes:

获取IMU的标准姿态数据，所述标准位姿态据包括标准加速度偏置、标准角速度偏置、标准加速度噪声及标准角速度噪声；Obtain standard posture data of the IMU, where the standard posture data includes standard acceleration offset, standard angular velocity offset, standard acceleration noise, and standard angular velocity noise;

当所述刚体携带所述IMU运动时，获取所述IMU的测量姿态数据，并根据所述IMU的测量姿态数据以及所述标准姿态数据确定待融合姿态数据，其中，所述待融合姿态数据包括待融合加速度以及待融合角速度。When the rigid body carries the IMU and moves, obtain the measured posture data of the IMU, and determine the posture data to be fused according to the measured posture data of the IMU and the standard posture data, wherein the posture data to be fused includes The acceleration to be fused and the angular velocity to be fused.
根据权利要求2所述的IMU与刚体的位姿融合方法，其特征在于，所述当所述刚体携带所述IMU运动时，获取所述IMU的测量姿态数据，并根据所述IMU的测量姿态数据以及所述标准姿态数据确定待融合姿态数据，其中，所述待融合姿态数据包括待融合加速度以及待融合角速度包括：The pose fusion method of IMU and rigid body according to claim 2, characterized in that, when the rigid body carries the IMU in motion, the measured posture data of the IMU is acquired, and the measured posture of the IMU is obtained according to the measured posture of the IMU The data and the standard attitude data determine the attitude data to be fused, where the attitude data to be fused includes the acceleration to be fused and the angular velocity to be fused includes:

获取所述刚体携带所述IMU运动时所述IMU的测量姿态数据，其中，所述IMU的测量姿态数据包括测量加速度及测量角速度；Acquiring the measured posture data of the IMU when the rigid body carries the IMU in motion, wherein the measured posture data of the IMU includes measured acceleration and measured angular velocity;

根据第一预置公式、所述标准姿态数据及所述IMU的测量姿态数据计算待融合加速度，所述第一预置公式为：
其中，a _t表示测量加速度，a _t(real)表示待融合加速度，
表示t时刻的标准加速度偏置，
表示t时刻IMU姿态下重力方向分量，n _a为标准加速度噪声； The acceleration to be fused is calculated according to the first preset formula, the standard posture data and the measured posture data of the IMU, and the first preset formula is:
Wherein, a _t represents a measure acceleration, a _{t (real)} represents an acceleration to be fused,
Represents the standard acceleration bias at time t,
Indicates the component of the direction of gravity in the IMU attitude at time t, and n _a is the standard acceleration noise;

根据第二预置公式、所述标准姿态数据及所述IMU的测量姿态数据计算待融合角速度，所述第二预置公式为：
其中，w _t表示测量角速度，w _t(real)表示待融合角速度，
表示t时刻的标准角速度偏置，n _w表示标准加速度噪声； The angular velocity to be fused is calculated according to the second preset formula, the standard attitude data and the measured attitude data of the IMU, and the second preset formula is:
Among them, w _t represents the measured angular velocity, w _{t (real)} represents the angular velocity to be fused,
Represents the standard angular velocity offset at time t, n _w represents the standard acceleration noise;

将所述待融合加速度以及所述待融合角速度合并，得到待融合姿态数据。The acceleration to be fused and the angular velocity to be fused are combined to obtain posture data to be fused.
根据权利要求1-3中任一项所述的IMU与刚体的位姿融合方法，其特征在于，所述在预置的世界坐标系中，获取刚体的测量姿态数据，并根据所述IMU的测量姿态数据、所述刚体的测量姿态数据以及所述IMU与刚体之间的位置关系确定刚体的候选姿态数据包括：The pose fusion method of an IMU and a rigid body according to any one of claims 1-3, wherein the measurement pose data of the rigid body is acquired in the preset world coordinate system, and the measured pose data of the rigid body is acquired according to the IMU The measured posture data, the measured posture data of the rigid body, and the positional relationship between the IMU and the rigid body to determine the candidate posture data of the rigid body include:

获取刚体的测量姿态数据及所述IMU的测量姿态数据，在所述IMU的测量姿态数据中提取第0帧IMU测量姿态数据以及第n帧IMU测量姿态数据，在所述刚体的测量姿态数据中提取第0帧刚体到世界的测量姿态数据以及第n帧刚体到世界的测量姿态数据；Obtain the measured posture data of the rigid body and the measured posture data of the IMU, extract the 0th frame IMU measured posture data and the nth frame IMU measured posture data from the measured posture data of the IMU, and in the measured posture data of the rigid body Extract the measured posture data from the rigid body to the world in the 0th frame and the measured posture data from the rigid body to the world in the nth frame;

在预置的世界坐标系中，根据关系等式，计算第0帧IMU到世界的测量姿态数据，以及所述IMU从IMU坐标到刚体坐标的旋转矩阵，其中，所述关系等式为：In the preset world coordinate system, calculate the measured posture data from the 0th frame IMU to the world and the rotation matrix of the IMU from the IMU coordinates to the rigid body coordinates according to the relation equation, where the relation equation is:

其中，
表示第n帧刚体到世界的测量姿态数据，
表示IMU测量姿态数据从IMU坐标到刚体坐标的旋转矩阵，[R _i] _n表示第n帧IMU测量姿态数据，
表示第0帧刚体到世界的测量姿态数据，[R _i] ₀表示第0帧IMU测量姿态数据，
表示第0帧IMU到世界的测量姿态数据，
表示第n帧IMU测量姿态数据相对于第0帧IMU测量姿态数据的逆矩阵，E用于指示单位矩阵； in,
Represents the measured posture data of the n-th rigid body to the world,
Represents the rotation matrix of IMU measurement posture data from IMU coordinates to rigid body coordinates, [R _i ] _n represents the nth frame IMU measurement posture data,
Represents the measured attitude data of the rigid body to the world in the 0th frame, [R _i ] ₀ represents the measured attitude data of the IMU in the 0th frame,
Represents the measured attitude data from the 0th frame IMU to the world,
Represents the inverse matrix of the IMU measurement attitude data of the nth frame relative to the IMU measurement attitude data of the 0th frame, and E is used to indicate the identity matrix;

根据姿态转换公式将IMU的姿态数据转化为刚体的姿态数据，得到刚体的候选姿态数据，所述姿态转换公式为：According to the posture conversion formula, the posture data of the IMU is converted into the posture data of the rigid body to obtain the candidate posture data of the rigid body. The posture conversion formula is:

其中，
表示经过转换后刚体的候选姿态数据，
表示第0帧IMU到世界的测量姿态数据，
表示第n帧IMU测量姿态数据相对于第0帧IMU测量姿态数据的逆矩阵，
表示IMU测量姿态数据从IMU坐标到刚体坐标的旋转矩阵的逆变换。 in,
Represents the candidate pose data of the rigid body after transformation,
Represents the measured attitude data from the 0th frame IMU to the world,
Represents the inverse matrix of the IMU measurement attitude data of the nth frame relative to the IMU measurement attitude data of the 0th frame,
Represents the inverse transformation of the rotation matrix of IMU measurement posture data from IMU coordinates to rigid body coordinates.
根据权利要求4所述的IMU与刚体的位姿融合方法，其特征在于，所述利用融合算法将所述IMU的待融合姿态数据与所述刚体的候选姿态数据融合，得到刚体的融合位姿数据包括：The pose fusion method of an IMU and a rigid body according to claim 4, wherein the fusion algorithm is used to fuse the to-be-fused pose data of the IMU with the candidate pose data of the rigid body to obtain the fused pose of the rigid body The data includes:

提取所述刚体的候选姿态数据中的姿态四元数，并分别获取刚体的候选位置数据以及候选位移数据、IMU的姿态四元数及IMU从IMU坐标到刚体坐标的旋转矩阵；Extracting the posture quaternion in the candidate posture data of the rigid body, and respectively obtaining the candidate position data and the candidate displacement data of the rigid body, the posture quaternion of the IMU, and the rotation matrix of the IMU from the IMU coordinates to the rigid body coordinates;

根据融合算法中的位置融合公式、IMU的测量姿态数据、所述IMU的待融合姿态数据计算刚体的融合位置，所述位置融合公式为：The fusion position of the rigid body is calculated according to the position fusion formula in the fusion algorithm, the measured posture data of the IMU, and the to-be-fused posture data of the IMU, and the position fusion formula is:

其中，
表示在k+1时刻时刚***于预置的世界坐标系中的候选位置，
表示在k时刻时刚***于预置的世界坐标系中的候选位置，α为位置融合系数，
表示IMU从IMU坐标到刚体坐标的旋转矩阵，
表示在k时刻时刚体的候选位移，Δt _k表示时间间隔，Δt＝t _k+1-t _k，a _t表示测量加速度，
表示k时刻IMU的速度，
表示t时刻的标准加速度偏置，n _a为标准加速度噪声，
表示t时刻IMU姿态下重力方向分量，
表示将IMU的速度和待融合加速度投影到预置的世界坐标系下时，[t _k，t _k+1]时间段中待融合加速度产生的位移量； in,
Indicates the candidate position of the rigid body in the preset world coordinate system at time k+1,
Indicates the candidate position of the rigid body in the preset world coordinate system at time k, and α is the position fusion coefficient,
Represents the IMU rotation matrix from IMU coordinates to rigid body coordinates,
K represents a time when the displacement of the rigid candidate, _k [Delta] t represents the time _{interval, Δt = t k + 1 -t} k, a t represents the measurement of acceleration,
Represents the speed of the IMU at time k,
Represents the standard acceleration bias at time t, n _a is the standard acceleration noise,
Represents the gravity direction component of the IMU attitude at time t,
Indicates the displacement generated by the acceleration to be fused in the time period _{[t k} , t _k+1 ] when the velocity of the IMU and the acceleration to be fused are projected to the preset world coordinate system;

根据融合算法中的姿态融合公式、IMU的测量姿态数据、待融合姿态数据计算刚体的融合姿态，所述姿态融合公式为：According to the posture fusion formula in the fusion algorithm, the measured posture data of the IMU, and the posture data to be fused, the fusion posture of the rigid body is calculated, and the posture fusion formula is:

其中，
表示在k+1时刻时刚***于预置的世界坐标系中的姿态四元数，
表示在k时刻时刚***于预置的世界坐标系中的姿态四元数，
表示四元数的乘法计算符号，β表示姿态融合系数，
表示IMU从IMU坐标到刚体坐标的旋转矩阵，
为k时刻IMU的姿态四元数，w _t表示测量角速度，
表示t时刻的标准角速度偏置，n _w表示标准加速度噪声，
表示[t _k，t _k+1]时段中IMU产生的旋转变化； in,
Represents the posture quaternion of the rigid body in the preset world coordinate system at time k+1,
Represents the posture quaternion of the rigid body in the preset world coordinate system at time k,
Represents the quaternion multiplication calculation symbol, β represents the attitude fusion coefficient,
Represents the IMU rotation matrix from IMU coordinates to rigid body coordinates,
Is the attitude quaternion of the IMU at time k, w _t represents the measured angular velocity,
Represents the standard angular velocity offset at time t, n _w represents the standard acceleration noise,
Represents the rotation change produced by the IMU in the period of [t _k , t _{k+1 ];}

将所述刚体的融合位置与所述刚体的融合姿态合并，确定刚体的融合位姿数据。The fusion position of the rigid body is merged with the fusion posture of the rigid body to determine the fusion pose data of the rigid body.
一种IMU与刚体的位姿融合装置，其特征在于，所述IMU与刚体的位姿融合装置包括：A posture fusion device of an IMU and a rigid body, characterized in that the posture fusion device of an IMU and a rigid body includes:

确定模块，用于确定惯性测量单元IMU与刚体之间的位置关系；The determination module is used to determine the positional relationship between the inertial measurement unit IMU and the rigid body;

获取并计算模块，当所述刚体携带所述IMU运动时，用于获取所述IMU的测量姿态数据，并通过所述IMU的测量姿态数据计算得到所述IMU的待融合姿态数据；An acquisition and calculation module, when the rigid body carries the IMU and moves, it is used to acquire the measured posture data of the IMU, and calculate the to-be-fused posture data of the IMU through the measured posture data of the IMU;

获取并确定模块，用于在预置的世界坐标系中，获取刚体的测量姿态数据，并根据所述IMU的测量姿态数据、所述刚体的测量姿态数据以及所述IMU与刚体之间的位置关系确定刚体的候选姿态数据；The acquiring and determining module is used to acquire the measured posture data of the rigid body in the preset world coordinate system, and according to the measured posture data of the IMU, the measured posture data of the rigid body, and the position between the IMU and the rigid body The relationship determines the candidate pose data of the rigid body;

融合模块，用于利用融合算法将所述IMU的待融合姿态数据与所述刚体的候选姿态数据融合，得到刚体的融合位姿数据。The fusion module is used for fusing the to-be-fused posture data of the IMU with the candidate posture data of the rigid body using a fusion algorithm to obtain the fused posture data of the rigid body.
根据权利要求6所述的IMU与刚体的位姿融合装置，其特征在于，所述获取并计算模块包括：The IMU and rigid body pose fusion device according to claim 6, wherein the acquisition and calculation module comprises:

获取单元，用于获取IMU的标准姿态数据，所述标准位姿态据包括标准加速度偏置、标准角速度偏置、标准加速度噪声及标准角速度噪声；An acquiring unit for acquiring standard posture data of the IMU, the standard posture data including standard acceleration offset, standard angular velocity offset, standard acceleration noise, and standard angular velocity noise;

确定单元，当所述刚体携带所述IMU运动时，用于获取所述IMU的测量姿态数据，并根据所述IMU的测量姿态数据以及所述标准姿态数据确定待融合姿态数据，其中，所述待融合姿态数据包括待融合加速度以及待融合角速度。The determining unit is used to obtain the measured posture data of the IMU when the rigid body is moving with the IMU, and determine the posture data to be fused according to the measured posture data of the IMU and the standard posture data, wherein the The posture data to be fused includes the acceleration to be fused and the angular velocity to be fused.
根据权利要求7所述的IMU与刚体的位姿融合装置，其特征在于，所述确定单元具体用于：The IMU and rigid body pose fusion device according to claim 7, wherein the determining unit is specifically configured to:

获取所述刚体携带所述IMU运动时所述IMU的测量姿态数据，其中，所述IMU的测量姿态数据包括测量加速度及测量角速度；Acquiring the measured posture data of the IMU when the rigid body carries the IMU in motion, wherein the measured posture data of the IMU includes measured acceleration and measured angular velocity;

根据第一预置公式、所述标准姿态数据及所述IMU的测量姿态数据计算待融合加速度，所述第一预置公式为：
其中，a _t表示测量加速度，a _t(real)表示待融合加速度，
表示t时刻的标准加速度偏置，
表示t时刻IMU姿态下重力方向分量，n _a为标准加速度噪声； The acceleration to be fused is calculated according to the first preset formula, the standard posture data and the measured posture data of the IMU, and the first preset formula is:
Wherein, a _t represents a measure acceleration, a _{t (real)} represents an acceleration to be fused,
Represents the standard acceleration bias at time t,
Indicates the component of the direction of gravity in the IMU attitude at time t, and n _a is the standard acceleration noise;

根据第二预置公式、所述标准姿态数据及所述IMU的测量姿态数据计算待融合角速度，所述第二预置公式为：
其中，w _t表示测量角速度，w _t(real)表示待融合角速度，
表示t时刻的标准角速度偏置，n _w表示标准加速度噪声； The angular velocity to be fused is calculated according to the second preset formula, the standard attitude data and the measured attitude data of the IMU, and the second preset formula is:
Among them, w _t represents the measured angular velocity, w _{t (real)} represents the angular velocity to be fused,
Represents the standard angular velocity offset at time t, n _w represents the standard acceleration noise;

将所述待融合加速度以及所述待融合角速度合并，得到待融合姿态数据。The acceleration to be fused and the angular velocity to be fused are combined to obtain posture data to be fused.
一种IMU与刚体的位姿融合设备，其特征在于，所述IMU与刚体的位姿融合设备包括：存储器和至少一个处理器，所述存储器中存储有指令，所述存储器和所述至少一个处理器通过线路互连；A pose fusion device of an IMU and a rigid body, characterized in that the pose fusion device of the IMU and a rigid body includes a memory and at least one processor, the memory stores instructions, the memory and the at least one The processors are interconnected by wires;

所述至少一个处理器调用所述存储器中的所述指令，以使得所述IMU与刚体的位姿融合设备执行如权利要求1-5中任意一项所述的IMU与刚体的位姿融合方法。The at least one processor invokes the instructions in the memory, so that the device for fusing the pose of the IMU and the rigid body executes the method for fusing the pose of the IMU and the rigid body according to any one of claims 1-5 .
一种计算机可读存储介质，所述计算机可读存储介质上存储有计算机程序，其特征在于，所述计算机程序被处理器执行时实现如权利要求1-5中任一项所述IMU与刚体的位姿融合方法。A computer-readable storage medium having a computer program stored on the computer-readable storage medium, wherein when the computer program is executed by a processor, the IMU and the rigid body described in any one of claims 1 to 5 are implemented. The pose fusion method.