CN112945231A - IMU and rigid body posture alignment method, device, equipment and readable storage medium - Google Patents

IMU and rigid body posture alignment method, device, equipment and readable storage medium Download PDF

Info

Publication number
CN112945231A
CN112945231A CN202110117061.6A CN202110117061A CN112945231A CN 112945231 A CN112945231 A CN 112945231A CN 202110117061 A CN202110117061 A CN 202110117061A CN 112945231 A CN112945231 A CN 112945231A
Authority
CN
China
Prior art keywords
rigid body
imu
posture
quaternion
solving
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
CN202110117061.6A
Other languages
Chinese (zh)
Inventor
黄少光
许秋子
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
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 CN202110117061.6A priority Critical patent/CN112945231A/en
Publication of CN112945231A publication Critical patent/CN112945231A/en
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
    • G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G01C21/005—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 with correlation of navigation data from several sources, e.g. map or contour matching
    • 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/165—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 combined with non-inertial navigation instruments
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C25/00—Manufacturing, calibrating, cleaning, or repairing instruments or devices referred to in the other groups of this subclass
    • G01C25/005—Manufacturing, calibrating, cleaning, or repairing instruments or devices referred to in the other groups of this subclass initial alignment, calibration or starting-up of inertial devices

Landscapes

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

Abstract

The application provides a method for aligning IMU (inertial measurement Unit) with rigid body postures, which comprises the following steps: fixing the position relation between the IMU and the rigid body, and recording the initial position of the rigid body; acquiring attitude data of a rigid body and an IMU; solving quaternion Q of rotation relation matrix according to preset equation set modelir(ii) a And then obtaining the posture of the rigid body at the time t according to a plurality of preset formulas, thereby obtaining correct rigid body pose information. By the method, the fusion calibration of the posture of the IMU and the posture of the rigid body and the alignment of the rigid body and the IMU are realized, the accuracy of the calculation of the posture of the rigid body is improved, and the measurement data of the posture and the position of the rigid body are stabilized and corrected finally.

Description

IMU and rigid body posture alignment method, device, equipment and readable storage medium
Technical Field
The present application relates to the field of measurement, and in particular, to a method, an apparatus, a device, and a readable storage medium for aligning an IMU with a rigid body pose.
Background
Currently, optical motion capture is realized by an optical-based rigid body pose tracking technology, and most of large-space VR equipment identifies the positions of a plurality of (generally, more than three light reflecting balls form a rigid body) light reflecting balls fixed on the rigid body in a space through a plurality of infrared camera view angles, calculates the pose of the rigid body, and then completes motion capture. The IMU is an inertial sensor capable of acquiring stable postures, and comprises three single-axis accelerometers and three single-axis gyroscopes, wherein the accelerometers can detect acceleration signals of an object in three independent axes of a carrier coordinate system, and the gyroscopes can detect angular velocity signals of the carrier relative to a navigation coordinate system, so that the angular velocity and the acceleration of the object in a three-dimensional space are measured, and the posture of the object is calculated.
The pose of the rigid body comprises pose and position information of the rigid body, the precision of motion capture and the precise calculation of the pose of the rigid body, but the calculation of the pose of the rigid body still has larger errors found in the current practical application. Particularly, in the rigid body tracking process, the problems that a reflective ball is easily shielded and shakes easily exist, so that the calculation of the rigid body posture has large errors and instability, and even under the condition that the central position of the rigid body is stably and accurately positioned most of the time, the rigid body posture still shakes violently. In addition, the accuracy of the posture information of the rigid body also affects the positioning of the rigid body photosphere and further affects the positioning of the rigid body. Therefore, how to improve the accuracy of rigid body pose calculation is a key link for realizing high-precision motion capture.
Disclosure of Invention
In view of this, if the stable posture of the IMU can be obtained and then aligned with the posture and position of the rigid body, the accuracy and stability of the posture calculation of the rigid body can be improved. The invention mainly provides a method, a device, equipment and a readable storage medium for aligning an IMU (inertial measurement Unit) with the posture and the position of a rigid body aiming at improving the calculation accuracy of the posture and the position of the rigid body.
In a first aspect, the present application provides a method for aligning an IMU with a rigid body pose, the method comprising:
fixing the position relation between IMU and rigid body, and recording the initial position P of rigid body0＝{p1，p2，...，pmIn which p ism＝(xm，ym，zm) Three marked points representing rigid bodyDimension coordinate data, wherein m is the number of rigid body mark points;
acquiring attitude data of the rigid body and the IMU:
Figure BDA0002921017260000021
n is more than or equal to 8, wherein,
Figure BDA0002921017260000022
a set of quaternions representing the rigid body and IMU poses at time n,
Figure BDA0002921017260000023
is a quaternion of the rigid body posture at the nth time,
Figure BDA0002921017260000024
quaternion of IMU posture at the nth moment;
modeling according to a predetermined set of equations and
Figure BDA0002921017260000025
solving quaternion Q of rotation relation matrixir；
Obtaining the said according to a preset first formula
Figure BDA0002921017260000026
And said
Figure BDA0002921017260000027
Angle therebetween if combined
Figure BDA0002921017260000028
All of
Figure BDA0002921017260000029
And
Figure BDA00029210172600000210
all included angles between the two are smaller than a set threshold value, then the Q is judgedirSolving correctly, otherwise, solving incorrectly;
if said Q isirIf the solution is correct, the rigid body is carried outTracking and positioning to obtain quaternion Q of IMU posture at t momentitAnd the center position T of the rigid bodyt；
Obtaining the posture R of the rigid body at the time t according to a preset second formulartAnd corresponding quaternion Qrt。
A plurality of
Figure BDA00029210172600000211
And
Figure BDA00029210172600000212
have a rotational relationship of an angle of more than 30 degrees between different postures.
The model according to a preset equation set and the
Figure BDA00029210172600000213
Solving quaternion Q of rotation relation matrixirThe method comprises the following steps: the preset equation set model is as follows:
Figure BDA00029210172600000214
st：||Qir||＝1，
Figure BDA00029210172600000215
for quaternion multiplication operation, solving the equation set model by SVD to obtain QirOr a rotational relationship matrix RirWherein, in the step (A),
Figure BDA00029210172600000216
Figure BDA00029210172600000217
the first formula is obtained according to a preset first formula
Figure BDA00029210172600000218
And said
Figure BDA00029210172600000219
Included angle therebetween, if anyCombination of Chinese herbs
Figure BDA00029210172600000220
All of
Figure BDA00029210172600000221
And
Figure BDA00029210172600000222
all included angles between the two are smaller than a set threshold value, then the Q is judgedirAnd solving for correct, otherwise, solving for error, comprising:
according to a preset first formula:
Figure BDA00029210172600000223
calculate the
Figure BDA00029210172600000224
And said
Figure BDA00029210172600000225
Angle therebetween
Figure BDA00029210172600000226
Wherein the content of the first and second substances,
Figure BDA00029210172600000227
is the inverse of the quaternion of the IMU attitude at the initial time,
Figure BDA00029210172600000228
is the inverse of the quaternion of the rotational relationship matrix, if aggregated
Figure BDA00029210172600000229
All of
Figure BDA00029210172600000230
And
Figure BDA00029210172600000231
angle d between themnAre all less than a set threshold value TdThen, judge said QirSolving forAnd if not, solving for an error.
Obtaining the posture R of the rigid body at the time t according to a preset second formulartAnd corresponding quaternion QrtThe method comprises the following steps:
according to a preset second formula:
Figure BDA0002921017260000031
obtaining the posture R of the rigid body at the time trtAnd corresponding quaternion Qrt。
If the Q is judgedirIf the solution fails, the position relation between the IMU and the rigid body is fixed again, and the posture data of the rigid body and the IMU are collected again, so that the Q is ensuredirAnd solving for correctness.
In a second aspect, the present application provides an apparatus for rigid body pose alignment of an IMU, the apparatus comprising:
a fixing unit for fixing the position relationship between IMU and rigid body and recording the initial position P of rigid body0＝{p1，p2，...，pmIn which p ism＝(xm，ym，zm) Representing three-dimensional coordinate data of rigid body mark points, wherein m is the number of the rigid body mark points;
the acquisition unit is used for acquiring the attitude data of the rigid body and the IMU:
Figure BDA0002921017260000032
n is more than or equal to 8, wherein,
Figure BDA0002921017260000033
a set of quaternions representing the rigid body and IMU poses at time n,
Figure BDA0002921017260000034
is a quaternion of the rigid body posture at the nth time,
Figure BDA0002921017260000035
quaternion of IMU posture at the nth moment;
a calculation unit for calculating the model according to a preset equation set and the
Figure BDA0002921017260000036
Solving quaternion Q of rotation relation matrixir(ii) a Obtaining the said according to a preset first formula
Figure BDA0002921017260000037
And said
Figure BDA0002921017260000038
Angle therebetween if combined
Figure BDA0002921017260000039
All of
Figure BDA00029210172600000310
And
Figure BDA00029210172600000311
all included angles between the two are smaller than a set threshold value, then the Q is judgedirSolving correctly, otherwise, solving incorrectly; if said Q isirIf the solution is correct, the rigid body is tracked and positioned, and the quaternion Q of the IMU posture at the moment t is obtaineditAnd the center position T of the rigid bodyt(ii) a Obtaining the posture R of the rigid body at the time t according to a preset second formulartAnd corresponding quaternion Qrt。
In a third aspect, the present application provides an apparatus for rigid body pose alignment of an IMU, the apparatus comprising a memory, a processor, and an IMU-to-rigid body pose alignment program stored on the memory and executable on the processor, the IMU-to-rigid body pose alignment program when executed by the processor implementing the method of the first aspect.
In a fourth aspect, the present application provides a computer readable storage medium having an IMU-to-rigid body pose alignment program stored thereon, which when executed by a processor, implements a method as provided in the first aspect of the present application or any one of the possible implementations of the first aspect.
According to the technical scheme, the method has the following beneficial effects:
recording the initial position of the rigid body after fixing the position relationship between the IMU and the rigid body; acquiring attitude data of a rigid body and an IMU; solving quaternion Q of rotation relation matrix according to preset equation set model and attitude datair(ii) a Obtaining included angles between the rigid body and the IMU in different attitude quaternions according to a preset first formula, and judging Q if all included angles are smaller than a set threshold valueirSolving correctly, then tracking and positioning the rigid body, and obtaining the quaternion Q of the IMU posture at the moment titAnd the central position of the rigid body, thereby obtaining the posture R of the rigid body at the time t according to a preset second formulartAnd corresponding quaternion Qrt. By the method, the fusion calibration of the posture of the IMU and the posture of the rigid body and the alignment of the rigid body and the IMU are realized, the accuracy of the calculation of the posture of the rigid body is improved, and the measurement data of the posture and the position of the rigid body are stabilized and corrected finally.
Drawings
FIG. 1 is a flow chart illustrating a method for IMU posture alignment with a rigid body according to the present application;
FIG. 2 illustrates a schematic diagram of an embodiment of an IMU alignment rigid body posture device according to the present disclosure;
fig. 3 shows a schematic diagram of a structure of an apparatus for aligning an IMU with a rigid body posture according to the present application.
Detailed Description
The application provides a method, a device and equipment for aligning postures and positions of an IMU (inertial measurement Unit) and a rigid body, and a readable storage medium, which are used for performing fusion calibration and alignment on the postures of the rigid body and the IMU so as to improve the calculation accuracy of the posture of the rigid body.
The technical solutions in the embodiments of the present application will be clearly and completely described below with reference to the drawings in the embodiments of the present application, and it is obvious that the described embodiments are only a part of the embodiments of the present application, and not all of the embodiments. All other embodiments, which can be derived by a person skilled in the art from the embodiments given herein without making any creative effort, shall fall within the protection scope of the present application.
The terms "first," "second," and the like in the description and in the claims of the present application and in the above-described drawings are used for distinguishing between similar elements and not necessarily for describing a particular sequential or chronological order. It will be appreciated that the data so used may be interchanged under appropriate circumstances such that the embodiments described herein may be practiced otherwise than as specifically illustrated or described herein. Moreover, the terms "comprises," "comprising," and any other variation thereof, are intended to cover a non-exclusive inclusion, such that a process, method, system, article, or apparatus that comprises a list of steps or modules is not necessarily limited to those steps or modules explicitly listed, but may include other steps or modules not expressly listed or inherent to such process, method, article, or apparatus. The naming or numbering of the steps appearing in the present application does not mean that the steps in the method flow have to be executed in the chronological/logical order indicated by the naming or numbering, and the named or numbered process steps may be executed in a modified order depending on the technical purpose to be achieved, as long as the same or similar technical effects are achieved.
The division of the modules presented in this application is a logical division, and in practical applications, there may be another division, for example, multiple modules may be combined or integrated into another system, or some features may be omitted, or not executed, and in addition, the shown or discussed coupling or direct coupling or communication connection between each other may be through some interfaces, and the indirect coupling or communication connection between the modules may be in an electrical or other similar form, which is not limited in this application. The modules or sub-modules described as separate components may or may not be physically separated, may or may not be physical modules, or may be distributed in a plurality of circuit modules, and some or all of the modules may be selected according to actual needs to achieve the purpose of the present disclosure.
First, before the present application is described, an imaging device, an IMU, a rigid body, and an apparatus for calibrating the posture of the IMU and the rigid body according to the present application will be described.
The image capturing device is a device that may be involved in an optical motion capture process, and includes one or more cameras, such as an omni-directional camera or a plurality of motion capture cameras, and the image capturing device can capture an object in a scene where the image capturing device is located.
An IMU is a device that measures the three-axis attitude angles (or angular rates) and acceleration of an object. Typically, an IMU comprises three single-axis accelerometers and three single-axis gyroscopes, the accelerometers detecting acceleration signals of the object in three independent axes of the carrier coordinate system, and the gyroscopes detecting angular velocity signals of the carrier relative to the navigation coordinate system, and the IMU attitude data of the object including angular velocity and acceleration of the object in three-dimensional space can be measured.
The rigid body is an object which has unchanged shape and size and the relative position of each point in the rigid body after movement and stress.
During the shooting task of the camera device, the camera device is in a motion state or temporarily, shoots objects, IMUs and rigid bodies of a scene, and the IMUs and the rigid bodies can be arranged on the camera device or arranged independently of the camera device, move along with the motion of the camera device, and respectively measure corresponding original attitude data. Of course, in practical applications, the IMU and the rigid body may be independent from the imaging device, and when the imaging device is not in a motion state or is not involved, the IMU and the rigid body may be in a motion state, and corresponding original posture data may be measured.
The device is a device with data processing capability, such as a host, a server or User Equipment (UE), and the method for calibrating the posture of the IMU and the rigid body is applied.
The UE is terminal equipment such as a desktop computer, a notebook computer, a computer all-in-one machine, a tablet computer or a smart phone.
Next, how to obtain the posture information of the rigid body stability by fusing the posture data of the IMU and the rigid body according to the existing equation set model is introduced.
To obtain the posture of the rigid body, the IMU and the rigid body posture need to be aligned, at this time, the camera coordinate system and the IMU coordinate system need to be aligned, and the posture of the rigid body at the time t is set as RrtThe quaternion corresponding to the posture of the rigid body is QrtWhen a rigid body is bound to the imaging device when t is 0, the rigid body posture identity matrix (i.e., the initial posture) is Rr0When t is 0, the attitude (i.e., initial attitude) of the IMU is Ri0Then the corresponding inverse is
Figure BDA0002921017260000061
the inverse of the quaternion of the IMU attitude when t is 0
Figure BDA0002921017260000062
IMU posture at time t is RitQuaternion corresponding to IMU attitude is QitThen the corresponding rotation relation matrix R needs to be foundirThe quaternion corresponding to the rotation relation matrix is QirSo as to make
Figure BDA0002921017260000063
Expressed as a quaternion
Figure BDA0002921017260000064
Wherein the content of the first and second substances,
Figure BDA0002921017260000065
for quaternion multiplication operation, the postures of the rigid body and the IMU are generally expressed by a matrix, and after the postures are obtained, the corresponding quaternion and the inverse are obtained.
Obtaining Q by the method of left-and right-multiplication of quaternionirThe solution model of (2):
Figure BDA0002921017260000066
setting the nth time:
Figure BDA0002921017260000067
wherein the content of the first and second substances,
Figure BDA0002921017260000068
is a quaternion of the IMU pose at time n,
Figure BDA0002921017260000069
and obtaining a homogeneous equation set model if the quaternion of the rigid body posture at the nth moment is obtained:
Figure BDA0002921017260000071
namely, it is
Figure BDA0002921017260000072
st：||Qir1, solving the homogeneous equation set model by using SVD (singular value decomposition) so as to obtain QirOr Rir。
If the correct rotation relation matrix R is obtainedirOr quaternion QirThen the attitude R of the rigid body at time trtThe IMU can be used to obtain the following results:
Figure BDA0002921017260000073
or
Figure BDA0002921017260000074
Because the attitude data of the IMU is accurate and stable, the accurate Q is obtained only by a proper modeirOr RirThe posture of the rigid body obtained by combining the data and the method is more accurate and stable.
Next, based on the above background description, the detailed description of the method for aligning the posture and position of the IMU with the rigid body according to the present invention will be started.
Referring to fig. 1, fig. 1 shows a schematic flow chart of the present method for aligning an IMU with a rigid body posture, and in particular, the method may include the following steps:
step S101, fixing the position relation between the IMU and the rigid body, and recording the initial position P of the rigid body0＝{p1，p2，...，pmIn which p ism＝(xm，ym，zm) Representing three-dimensional coordinate data of rigid body mark points, wherein m is the number of the rigid body mark points;
it is understood that IMU and rigid body are hardware components, in which the IMU and rigid body are fixedAfter the positional relationship between the rigid bodies, the initial position P of the rigid bodies can be recorded0＝{p1，p2，...，pmIn which p ism＝(xm，ym，zm) Representing the three-dimensional coordinate data of the rigid body mark points (namely, the light reflecting balls), wherein m is the number of the rigid body mark points, and then the subsequent steps of the IMU and rigid body posture alignment method can be executed.
Step S102, acquiring posture data of the rigid body and the IMU:
Figure BDA0002921017260000075
n is more than or equal to 8, wherein,
Figure BDA0002921017260000076
a set of quaternions representing the rigid body and IMU poses at time n,
Figure BDA0002921017260000077
is a quaternion of the rigid body posture at the nth time,
Figure BDA0002921017260000078
quaternion of IMU posture at the nth moment;
after the position relation between the IMU and the rigid body is fixed, the calibration data can be acquired in the optical multi-camera system, wherein the acquisition comprises acquiring the postures of the bound rigid body and the postures of the IMU, and the postures of the rigid body and the IMU are respectively more than 8 different postures and a plurality of postures
Figure BDA0002921017260000079
And
Figure BDA00029210172600000710
the rotational relationship of an angle of more than 30 degrees exists between different postures, and the acquired posture data of the rigid body and the IMU are as follows:
Figure BDA0002921017260000081
n is more than or equal to 8, which indicates that more than 8 different rigid bodies and IMU posture data are respectively collected.
Step S103, modeling according to a preset equation set
Figure BDA0002921017260000082
Solving quaternion Q of rotation relation matrixir；
After more than 8 different attitude data are collected in step S102, the quaternion sets of the rigid body and IMU attitude at the nth time can be collected
Figure BDA0002921017260000083
Quaternion of rigid body attitude at the nth time
Figure BDA0002921017260000084
And quaternion of IMU attitude at time n
Figure BDA0002921017260000085
Into the solution model described above, i.e.
Figure BDA0002921017260000086
Thus obtaining a corresponding homogeneous equation set model, namely a preset equation set model:
Figure BDA0002921017260000087
namely, it is
Figure BDA0002921017260000088
st：||QirObtaining Q by solving a homogeneous equation set model by SVD (singular value decomposition) 1irOr Rir。
Step S104, obtaining a first formula according to a preset first formula
Figure BDA0002921017260000089
And
Figure BDA00029210172600000810
angle therebetween if combined
Figure BDA00029210172600000811
All of
Figure BDA00029210172600000812
And
Figure BDA00029210172600000813
all included angles between the two are smaller than a set threshold value, then Q is judgedirSolving correctly, otherwise, solving incorrectly;
in the case of a quaternion Q for solving the rotational relationship matrix according to step S103irAfterwards, whether the solution value is correct needs to be further verified to ensure that the rigid body tracking and positioning can be successfully carried out subsequently.
Presetting a first formula:
Figure BDA00029210172600000814
wherein, in the step (A),
Figure BDA00029210172600000815
is the inverse of the quaternion of the IMU posture at the initial moment (the data can be obtained after the rigid body and the IMU position relation are bound at the initial moment),
Figure BDA00029210172600000816
is the inverse of the quaternion of the rotation relation matrix, and can be obtained by the formula
Figure BDA00029210172600000817
And
Figure BDA00029210172600000818
angle relationship between them, i.e. angle
Figure BDA00029210172600000819
If set
Figure BDA00029210172600000820
All of
Figure BDA00029210172600000821
And
Figure BDA00029210172600000822
angle d between themnAre all less than a set threshold value TdThen, Q is determinedirAnd (4) solving correctly, namely the posture alignment of the rigid body and the IMU is successful, otherwise, solving incorrectly.
Step S105, if QirIf the solution is correct, the rigid body is tracked and positioned, and the quaternion Q of the IMU posture at the moment t is obtaineditAnd the center position T of the rigid bodyt；
If Q isirIf the solution value is correct, the rigid body can be tracked and positioned in the optical multi-camera system, and if the tracking and positioning are successful, the quaternion Q of the IMU posture at the moment t can be obtaineditAnd rigid body center position Tt。
Step S106, obtaining the posture R of the rigid body at the time t according to a preset second formulartAnd corresponding quaternion Qrt；
In finding QirCorrect value of (d) and quaternion Q of IMU attitude at time titThen, according to a preset second formula:
Figure BDA0002921017260000091
obtaining the posture R of the rigid body at the time trtAnd corresponding quaternion Qrt。
After the above rigid body posture is obtained, the rigid body posture R may be usedrtThe position of the rigid body is corrected, as detailed in step S107:
and S107, obtaining the position of the rigid body mark point at the time t according to a preset third formula, comparing the position of the rigid body mark point at the time t with the position of the rigid body mark point in the actual 3D modeling, judging whether rigid body tracking is successful according to a comparison result, and if the rigid body tracking is successful, replacing the position of the rigid body mark point in the actual 3D modeling with the position of the rigid body mark point at the time t, thereby obtaining correct rigid body position information.
According to a preset third formula: p'mt＝Rrt·pm+TtThe position p 'of the rigid body mark point at time t can be obtained'mt，pmIs a rigid body in step S101Marking three-dimensional coordinate data of points, p'mtPosition p of rigid body mark point in actual 3D modelingmtComparing, namely obtaining the distance, judging whether the rigid body tracking is successful according to the comparison result, and if the rigid body tracking is successful, adopting p'mtSubstitution of pmtThereby obtaining correct rigid body position information.
Note that, the position p 'of the rigid body mark point is determined by time t'mtPosition p of rigid body mark point in actual 3D modelingmtThe distance d | | | p between the two can be obtainedmt-pmt' | |, if the distance d is larger than Th and Th is a preset threshold value, the tracking is successful, otherwise, the tracking is failed. If the tracking is successful, the method can be used for correcting the rigid body mark point position obtained in the 3D modeling, the fusion of the rigid body and the IMU is realized, and finally, the result of stabilizing and correcting the pose of the rigid body by utilizing the pose of the IMU is realized.
In addition, if Q is judgedirAnd if the solution error is found, returning to the step S101, re-fixing the position relation between the IMU and the rigid body, and re-collecting the attitude data of the rigid body and the IMU so as to ensure that Q is equal to QirAnd solving for correctness.
The above is an introduction of the method for aligning the IMU with the posture and position of the rigid body according to the present application, and a description of the apparatus for aligning the IMU with the posture of the rigid body according to the present application is started below.
Referring to fig. 2, fig. 2 shows a schematic structural diagram of the apparatus for aligning an IMU with a rigid body posture according to the present application, and in particular, the apparatus may include the following structure:
a fixing unit 201 for fixing the position relationship between the IMU and the rigid body and recording the initial position P of the rigid body0＝{p1，p2，...，pmIn which p ism＝(xm，ym，zm) Representing three-dimensional coordinate data of rigid body mark points, wherein m is the number of the rigid body mark points;
an acquisition unit 202, configured to acquire pose data of the rigid body and the IMU:
Figure BDA0002921017260000101
n is more than or equal to 8, wherein,
Figure BDA0002921017260000102
a set of quaternions representing the rigid body and IMU poses at time n,
Figure BDA0002921017260000103
is a quaternion of the rigid body posture at the nth time,
Figure BDA0002921017260000104
quaternion of IMU posture at the nth moment;
a calculation unit 203 for modeling and calculating according to a preset equation set
Figure BDA0002921017260000105
Solving quaternion Q of rotation relation matrixir(ii) a According to a preset first formula
Figure BDA0002921017260000106
And
Figure BDA0002921017260000107
angle therebetween if combined
Figure BDA0002921017260000108
All of
Figure BDA0002921017260000109
And
Figure BDA00029210172600001010
all included angles between the two are smaller than a set threshold value, then Q is judgedirSolving correctly, otherwise, solving incorrectly; if QirIf the solution is correct, the rigid body is tracked and positioned, and the quaternion Q of the IMU posture at the moment t is obtaineditAnd the center position T of the rigid bodyt(ii) a Obtaining the posture R of the rigid body at the time t according to a preset second formulartAnd corresponding quaternion Qrt。
The processes of acquisition, calculation and the like related to the device are referred to the method steps S101-S107, and are not described herein again.
Referring to fig. 3, fig. 3 illustrates an apparatus for rigid body pose alignment of an IMU according to the present application, which specifically includes a processor 301, a memory 302, and an IMU and rigid body pose alignment program stored in the memory 302 and executable on the processor 301, where the IMU and rigid body pose alignment program, when executed by the processor 301, implements the steps of the method for rigid body pose alignment of an IMU according to any of the embodiments corresponding to fig. 1.
Illustratively, the IMU may be a rigid body pose alignment program, which may be partitioned into one or more modules/units, which are stored in the memory 302 and executed by the processor 301 to accomplish the present application. One or more modules/units may be a series of computer program instruction segments capable of performing certain functions, the instruction segments being used to describe the execution of a computer program in a computer device.
Devices for aligning IMUs to rigid body poses may include, but are not limited to, processor 301, memory 302. Those skilled in the art will appreciate that the illustration is merely an example of a computer apparatus and is not intended to limit the devices in which the IMU is aligned with the rigid body pose and may include more or fewer components than those shown, or some components may be combined, or different components, for example, the devices in which the IMU is aligned with the rigid body pose may also include input output devices, network access devices, buses, etc., and the processor 301, memory 302, input output devices, network access devices, etc., are connected via buses.
The Processor 301 may be a Central Processing Unit (CPU), other general purpose Processor, a Digital Signal Processor (DSP), an Application Specific Integrated Circuit (ASIC), an off-the-shelf Programmable Gate Array (FPGA) or other Programmable logic device, discrete Gate or transistor logic, discrete hardware components, etc. The general purpose processor may be a microprocessor or the processor may be any conventional processor or the like that is the control center for the rigid body pose aligned device of the IMU, with various interfaces and lines connecting the various parts of the device that align the entire IMU with the rigid body pose.
The memory 302 may be used to store computer programs and/or modules, and the processor 301 implements various functions of the computer device by running or executing the computer programs and/or modules stored in the memory 302 and invoking data stored in the memory 302. The memory 302 may mainly include a program storage area and a data storage area, wherein the program storage area may store an operating system, an application program required by at least one function (such as a sound playing function, an image playing function, etc.), and the like; the storage data area may store data (such as audio data, video data, etc.) created according to the use of the cellular phone, etc. In addition, the memory may include high speed random access memory, and may also include non-volatile memory, such as a hard disk, a memory, a plug-in hard disk, a Smart Media Card (SMC), a Secure Digital (SD) Card, a Flash memory Card (Flash Card), at least one magnetic disk storage device, a Flash memory device, or other volatile solid state storage device.
The present application also provides a readable storage medium having an IMU to rigid body pose alignment program stored thereon, which when executed by a processor, implements a method of IMU to rigid body pose alignment as in any of the corresponding embodiments of fig. 1.
It will be appreciated that the integrated unit, if implemented as a software functional unit and sold or used as a stand-alone product, may be stored in a computer readable storage medium. Based on such understanding, the technical solution of the present application may be substantially implemented or contributed to by the prior art, or all or part of the technical solution may be embodied in the form of a software product stored in a storage medium and including instructions for causing a computer device (which may be a personal computer, a server, or a network device) to execute all or part of the steps of the method embodiments of the present application. And the aforementioned storage medium includes: a U-disk, a removable hard disk, a Read-only Memory (ROM), a Random Access Memory (RAM), a magnetic disk or an optical disk, and other various media capable of storing program codes.
The above embodiments are only used for illustrating the technical solutions of the present application, and not for limiting the same; although the present application has been described in detail with reference to the foregoing embodiments, it should be understood by those of ordinary skill in the art that: the technical solutions described in the foregoing embodiments may still be modified, or some technical features may be equivalently replaced; and such modifications or substitutions do not depart from the spirit and scope of the corresponding technical solutions in the embodiments of the present application.

Claims (10)

1. A method of rigid body pose alignment of an IMU, the method comprising:
fixing the position relation between the IMU and the rigid body, and recording the initial position of the rigid body;
acquiring attitude data of the rigid body and the IMU:
Figure FDA0002921017250000011
n is more than or equal to 8, wherein,
Figure FDA0002921017250000012
a set of quaternions representing the rigid body and IMU poses at time n,
Figure FDA0002921017250000013
is a quaternion of the rigid body posture at the nth time,
Figure FDA0002921017250000014
quaternion of IMU posture at the nth moment;
modeling according to a predetermined set of equations and
Figure FDA0002921017250000015
solving quaternion Q of rotation relation matrixir；
Obtaining the said according to a preset first formula
Figure FDA0002921017250000016
And said
Figure FDA0002921017250000017
Angle therebetween if combined
Figure FDA0002921017250000018
All of
Figure FDA0002921017250000019
And
Figure FDA00029210172500000110
all included angles between the two are smaller than a set threshold value, then the Q is judgedirSolving correctly, otherwise, solving incorrectly;
if said Q isirIf the solution is correct, the rigid body is tracked and positioned, and the quaternion Q of the IMU posture at the moment t is obtainedit；
Obtaining the posture R of the rigid body at the time t according to a preset second formulartAnd corresponding quaternion Qrt。
2. The method of claim 1, wherein the initial rigid body position is P0＝{p1，p2，...，pmIn which p ism＝(xm，ym，zm) Representing the three-dimensional coordinate data of the rigid body mark points, wherein m is the number of the rigid body mark points.
3. The method of claim 1, wherein a plurality of said
Figure FDA00029210172500000111
And
Figure FDA00029210172500000112
have a rotational relationship of an angle of more than 30 degrees between different postures.
4. The method of claim 1, wherein the step of applying the coating comprises applying a coating to the substrateThe model according to a preset equation set and the
Figure FDA00029210172500000113
Solving for the quaternion Qir of the rotational relationship matrix includes: the preset equation set model is as follows:
Figure FDA00029210172500000114
Figure FDA00029210172500000115
for quaternion multiplication operation, solving the equation set model by SVD to obtain QirOr a rotational relationship matrix RirWherein, in the step (A),
Figure FDA00029210172500000116
5. method according to claim 1, wherein said deriving said first formula is according to a preset first formula
Figure FDA00029210172500000117
And said
Figure FDA00029210172500000118
Angle therebetween if combined
Figure FDA00029210172500000119
All of
Figure FDA00029210172500000120
And
Figure FDA00029210172500000121
the included angles between the two are all smaller than a set threshold value, then the Qir is judged to be solved correctly, otherwise, the solving is wrong, and the method comprises the following steps:
according to a preset first formula:
Figure FDA00029210172500000122
calculate the
Figure FDA00029210172500000123
And said
Figure FDA00029210172500000124
Angle therebetween
Figure FDA0002921017250000021
Wherein the content of the first and second substances,
Figure FDA0002921017250000022
is the inverse of the quaternion of the IMU attitude at the initial time,
Figure FDA0002921017250000023
is the inverse of the quaternion of the rotational relationship matrix, if aggregated
Figure FDA0002921017250000024
All of
Figure FDA0002921017250000025
And
Figure FDA0002921017250000026
angle d between themnAre all less than a set threshold value TdThen, judge said QirAnd solving correctly, otherwise, solving incorrectly.
6. The method according to claim 1, wherein the posture R of the rigid body at the time t is obtained according to a preset second formulartAnd corresponding quaternion QrtThe method comprises the following steps:
according to a preset second formula:
Figure FDA0002921017250000027
obtaining the posture R of the rigid body at the time trtAnd corresponding quaternion Qrt。
7. The method of any one of claims 1-6, wherein Q is determined if Q is determinedirAnd if the solution error is found, the position relation between the IMU and the rigid body is fixed again, and the posture data of the rigid body and the IMU are collected again, so that the Q is ensuredirAnd solving for correctness.
8. An apparatus for rigid body pose alignment of an IMU, the apparatus comprising:
the fixing unit is used for fixing the position relation between the IMU and the rigid body and recording the initial position of the rigid body;
the acquisition unit is used for acquiring the attitude data of the rigid body and the IMU:
Figure FDA0002921017250000028
n is more than or equal to 8, wherein,
Figure FDA0002921017250000029
a set of quaternions representing the rigid body and IMU poses at time n,
Figure FDA00029210172500000210
is a quaternion of the rigid body posture at the nth time,
Figure FDA00029210172500000211
quaternion of IMU posture at the nth moment;
a calculation unit for calculating the model according to a preset equation set and the
Figure FDA00029210172500000212
Solving quaternion Q of rotation relation matrixir(ii) a Obtaining the said according to a preset first formula
Figure FDA00029210172500000213
And said
Figure FDA00029210172500000214
Angle therebetween if combined
Figure FDA00029210172500000215
All of
Figure FDA00029210172500000216
And
Figure FDA00029210172500000217
all included angles between the two are smaller than a set threshold value, then the Q is judgedirSolving correctly, otherwise, solving incorrectly; if said Q isirIf the solution is correct, the rigid body is tracked and positioned, and the quaternion Q of the IMU posture at the moment t is obtainedit(ii) a Obtaining the posture R of the rigid body at the time t according to a preset second formulartAnd corresponding quaternion Qrt。
9. An IMU to rigid body pose alignment apparatus comprising a memory, a processor, and an IMU to rigid body pose alignment program stored on the memory and executable on the processor, the IMU to rigid body pose alignment program when executed by the processor implementing a method as claimed in any one of claims 1 to 7.
10. A computer readable storage medium having an IMU-to-rigid body pose alignment program stored thereon, which when executed by a processor implements a method as claimed in any one of claims 1 to 7.
CN202110117061.6A 2021-01-28 2021-01-28 IMU and rigid body posture alignment method, device, equipment and readable storage medium Pending CN112945231A (en)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202110117061.6A CN112945231A (en) 2021-01-28 2021-01-28 IMU and rigid body posture alignment method, device, equipment and readable storage medium

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202110117061.6A CN112945231A (en) 2021-01-28 2021-01-28 IMU and rigid body posture alignment method, device, equipment and readable storage medium

Publications (1)

Publication Number Publication Date
CN112945231A true CN112945231A (en) 2021-06-11

Family

ID=76238520

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202110117061.6A Pending CN112945231A (en) 2021-01-28 2021-01-28 IMU and rigid body posture alignment method, device, equipment and readable storage medium

Country Status (1)

Country Link
CN (1) CN112945231A (en)

Cited By (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN115388911A (en) * 2022-08-24 2022-11-25 Oppo广东移动通信有限公司 Precision measurement method and device of optical motion capture system and electronic equipment
TWI812369B (en) * 2021-07-28 2023-08-11 宏達國際電子股份有限公司 Control method, tracking system and non-transitory computer-readable storage medium

Citations (12)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN102162847A (en) * 2011-01-14 2011-08-24 中国人民解放军理工大学 High-efficiency track correlation method based on singular value decomposition
CN102542604A (en) * 2010-09-22 2012-07-04 卡西欧计算机株式会社 AR process apparatus, AR process method and storage medium
JP2015227813A (en) * 2014-05-30 2015-12-17 アニマ株式会社 Sensor module position acquiring method and device, and operation measurement method and device
CN105549057A (en) * 2015-12-07 2016-05-04 韩厚增 Inertial auxiliary GPS/BDS fusion large-scale measurement device and method for quickly measuring land parcel
US20160273919A1 (en) * 2014-10-09 2016-09-22 Honeywell International Inc. Systems and methods for producing two independent dissimilar attitude solutions, two independent dissimilar inertial solutions or both from one improved navigation device
CN106871859A (en) * 2017-01-03 2017-06-20 电子科技大学 A kind of rigid space fixed-axis rotation angle detecting method
CN107941212A (en) * 2017-11-14 2018-04-20 杭州德泽机器人科技有限公司 A kind of vision and inertia joint positioning method
CN108605098A (en) * 2016-05-20 2018-09-28 深圳市大疆创新科技有限公司 system and method for rolling shutter correction
CN108629793A (en) * 2018-03-22 2018-10-09 中国科学院自动化研究所 The vision inertia odometry and equipment demarcated using line duration
CN110956666A (en) * 2019-11-12 2020-04-03 深圳市瑞立视多媒体科技有限公司 Motion data calibration method and device, terminal equipment and storage medium
CN110986925A (en) * 2019-12-02 2020-04-10 北京自动化控制设备研究所 Initial attitude optimal estimation method
CN111882608A (en) * 2020-07-14 2020-11-03 中国人民解放军军事科学院国防科技创新研究院 Pose estimation method between augmented reality glasses tracking camera and human eyes

Patent Citations (12)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN102542604A (en) * 2010-09-22 2012-07-04 卡西欧计算机株式会社 AR process apparatus, AR process method and storage medium
CN102162847A (en) * 2011-01-14 2011-08-24 中国人民解放军理工大学 High-efficiency track correlation method based on singular value decomposition
JP2015227813A (en) * 2014-05-30 2015-12-17 アニマ株式会社 Sensor module position acquiring method and device, and operation measurement method and device
US20160273919A1 (en) * 2014-10-09 2016-09-22 Honeywell International Inc. Systems and methods for producing two independent dissimilar attitude solutions, two independent dissimilar inertial solutions or both from one improved navigation device
CN105549057A (en) * 2015-12-07 2016-05-04 韩厚增 Inertial auxiliary GPS/BDS fusion large-scale measurement device and method for quickly measuring land parcel
CN108605098A (en) * 2016-05-20 2018-09-28 深圳市大疆创新科技有限公司 system and method for rolling shutter correction
CN106871859A (en) * 2017-01-03 2017-06-20 电子科技大学 A kind of rigid space fixed-axis rotation angle detecting method
CN107941212A (en) * 2017-11-14 2018-04-20 杭州德泽机器人科技有限公司 A kind of vision and inertia joint positioning method
CN108629793A (en) * 2018-03-22 2018-10-09 中国科学院自动化研究所 The vision inertia odometry and equipment demarcated using line duration
CN110956666A (en) * 2019-11-12 2020-04-03 深圳市瑞立视多媒体科技有限公司 Motion data calibration method and device, terminal equipment and storage medium
CN110986925A (en) * 2019-12-02 2020-04-10 北京自动化控制设备研究所 Initial attitude optimal estimation method
CN111882608A (en) * 2020-07-14 2020-11-03 中国人民解放军军事科学院国防科技创新研究院 Pose estimation method between augmented reality glasses tracking camera and human eyes

Cited By (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
TWI812369B (en) * 2021-07-28 2023-08-11 宏達國際電子股份有限公司 Control method, tracking system and non-transitory computer-readable storage medium
CN115388911A (en) * 2022-08-24 2022-11-25 Oppo广东移动通信有限公司 Precision measurement method and device of optical motion capture system and electronic equipment

Similar Documents

Publication Publication Date Title
US20200250429A1 (en) Attitude calibration method and device, and unmanned aerial vehicle
EP3028252B1 (en) Rolling sequential bundle adjustment
US20200264011A1 (en) Drift calibration method and device for inertial measurement unit, and unmanned aerial vehicle
CN106525074B (en) A kind of compensation method, device, holder and the unmanned plane of holder drift
US9378559B2 (en) System and method for motion estimation
CN104025151B (en) Method and electronic device for creating combination image
US10841570B2 (en) Calibration device and method of operating the same
CN110717861B (en) Image splicing method and device, electronic equipment and computer readable storage medium
CN112945231A (en) IMU and rigid body posture alignment method, device, equipment and readable storage medium
CN109840884A (en) A kind of image split-joint method, device and electronic equipment
WO2013130208A1 (en) Scene structure-based self-pose estimation
CN110956666B (en) Motion data calibration method and device, terminal equipment and storage medium
CN106570907B (en) Camera calibration method and device
CN108604374B (en) Image detection method and terminal
US9443349B2 (en) Electronic apparatus and method for incremental pose estimation and photographing thereof
JP2023502635A (en) CALIBRATION METHOD AND APPARATUS, PROCESSOR, ELECTRONICS, STORAGE MEDIUM
CN113393563B (en) Method, system, electronic device and storage medium for automatically labeling key points
CN113409391A (en) Visual positioning method and related device, equipment and storage medium
CN110567484B (en) Method and device for calibrating IMU and rigid body posture and readable storage medium
CN113516692A (en) Multi-sensor fusion SLAM method and device
CN112991441A (en) Camera positioning method and device, electronic equipment and storage medium
CN113407030B (en) Visual positioning method, related device, equipment and storage medium
CN111275769B (en) Monocular vision parameter correction method and device
EP3718302B1 (en) Method and system for handling 360 degree image content
CN112923923A (en) Method, device and equipment for aligning posture and position of IMU (inertial measurement Unit) and rigid body and readable storage medium

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