A method, device, equipment and readable storage medium for aligning IMU with rigid body posture
Through the alignment method of IMU with rigid body posture and position, the problems of rigid body posture calculation error and instability are solved, and the high-precision effect of motion capture is achieved.
Patent Information
- Application Number
- CN202110117061.6
- Authority / Receiving Office
- CN · China
- Patent Type
- Patents(China)
- Current Assignee / Owner
- Filing Date
- 2021-01-28
- Publication Date
- 2024-12-31
- Estimated Expiration
- 2041-01-28
AI Technical Summary
In the prior art, there are large errors and instability in the calculation of rigid position postures, especially when the reflective ball is blocked or shaken, which affects the accuracy of motion capture.
Through the alignment method of IMU and rigid body attitude and position, the position relationship between IMU and rigid body is fixed, the initial position is recorded, the attitude data is collected, the rotation relationship matrix is solved using the preset equation model, the angle threshold is judged, and tracking, positioning and correction are performed. , improve the accuracy of rigid position pose calculation.
The stable fusion calibration of IMU and rigid body posture is achieved, which improves the accuracy and stability of rigid body posture calculations and ensures high accuracy of motion capture.
Smart Images
Figure CN112945231B_ABST
Abstract
Description
Technical Field
[0001] The present application relates to the field of measurement, and in particular to a method, device, equipment and readable storage medium for aligning an IMU with a rigid body posture. Background Art
[0002] Currently, optical motion capture is achieved through optical-based rigid body posture tracking technology. Most large-space VR devices use multiple infrared camera perspectives to identify the positions of multiple reflective balls (usually more than three reflective balls form a rigid body) fixed on a rigid body in space, calculate the posture of the rigid body, and then complete motion capture. IMU is an inertial sensor that can obtain a stable posture. An IMU contains three single-axis accelerometers and three single-axis gyroscopes. The accelerometer can detect the acceleration signal of the object in the independent three-axis of the carrier coordinate system, and the gyroscope can detect the angular velocity signal of the carrier relative to the navigation coordinate system, thereby measuring the angular velocity and acceleration of the object in three-dimensional space, and using this to solve the object's posture.
[0003] The rigid body pose includes the posture and position information of the rigid body. The accuracy of motion capture is inseparable from the accurate calculation of the rigid body pose. However, in current practical applications, it is found that the calculation of the rigid body pose still has large errors. In particular, in the process of rigid body tracking, there are problems such as the reflective ball being blocked and shaking, which leads to large errors and instability in the calculation of the rigid body pose. Even if the center position of the rigid body is relatively stable and accurate most of the time, the rigid body pose still has severe shaking. In addition, the correctness of the rigid body's pose information also affects the positioning of the rigid body light ball, and then affects the positioning of the rigid body. Therefore, how to improve the accuracy of the rigid body pose calculation is a key link in achieving high-precision motion capture. Summary of the invention
[0004] 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 present invention mainly proposes a method, device, equipment and readable storage medium for aligning the posture and position of an IMU with a rigid body in order to improve the accuracy of the posture and position calculation of the rigid body.
[0005] In a first aspect, the present application provides a method for aligning an IMU with a rigid body posture, the method comprising:
[0006] Fix the position relationship between IMU and rigid body, record the initial position of rigid body P0 = {p1, p2, ..., p m}, where p m =(x m ,y m , z m ) represents the three-dimensional coordinate data of the rigid body marking point, and m is the number of rigid body marking points;
[0007] Collect the attitude data of the rigid body and IMU: in, The set of quaternions representing the rigid body and IMU attitude at the nth moment, is the quaternion of the rigid body posture at the nth moment, is the quaternion of the IMU attitude at the nth moment;
[0008] According to the preset equation model and the Solve the quaternion Q of the rotation relationship matrix ir ;
[0009] According to the preset first formula, the and stated The angle between and If the angle between them is less than a set threshold, it is determined that the Q ir The solution is correct, otherwise, the solution is wrong;
[0010] If the Q ir If the solution is correct, the rigid body is tracked and positioned to obtain the quaternion Q of the IMU attitude at time t. it and the center position T of the rigid body t ;
[0011] According to the preset second formula, the posture R of the rigid body at time t is obtained rt And the corresponding quaternion Q rt .
[0012] Multiple and There is a rotation relationship of more than 30° between different postures.
[0013] The preset equation group model and the Solve the quaternion Q of the rotation relationship matrix ir Including: the preset equation group model is:
[0014] is a quaternion multiplication operation, and the SVD decomposition is used to solve the equation model to obtain Q ir Or the rotation relationship matrix R ir ,in,
[0015] The first formula is used to obtain the and stated The angle between and If the angle between them is less than a set threshold, it is determined that the Qir The solution is correct, otherwise, the solution is wrong, including:
[0016] According to the preset first formula: Calculate the and stated The angle between in, is the inverse of the quaternion of the IMU attitude at the initial moment, is the inverse of the quaternion of the rotation relationship matrix, if all and The angle d between n are all less than a set threshold T d , then judge the Q ir The solution is correct, otherwise, the solution is wrong.
[0017] The posture R of the rigid body at time t is obtained according to the preset second formula rt And the corresponding quaternion Q rt include:
[0018] According to the preset second formula: Get the posture R of the rigid body at time t rt And the corresponding quaternion Q rt .
[0019] If we judge that Q ir If the solution fails, the position relationship between the IMU and the rigid body is re-fixed, and the posture data of the rigid body and the IMU are re-collected to make the Q ir The solution is correct.
[0020] In a second aspect, the present application provides a device for aligning an IMU with a rigid body posture, the device comprising:
[0021] The fixed unit is used to fix the position relationship between the IMU and the rigid body and record the initial position of the rigid body P0 = {p1, p2, ..., p m}, where p m =(x m ,y m , z m ) represents the three-dimensional coordinate data of the rigid body marking point, and m is the number of rigid body marking points;
[0022] The acquisition unit is used to acquire the posture data of the rigid body and the IMU: in, The set of quaternions representing the rigid body and IMU attitude at the nth moment, is the quaternion of the rigid body posture at the nth moment, is the quaternion of the IMU attitude at the nth moment;
[0023] A calculation unit is used to calculate the value of the equation according to the preset equation group model and the Solve the quaternion Q of the rotation relationship matrix ir ; According to the preset first formula, the and stated The angle between and If the angle between them is less than a set threshold, it is determined that the Q ir The solution is correct, otherwise, the solution is wrong; if the Q ir If the solution is correct, the rigid body is tracked and positioned to obtain the quaternion Q of the IMU attitude at time t. it and the center position T of the rigid body t ; According to the preset second formula, the posture R of the rigid body at time t is obtained rt And the corresponding quaternion Q rt .
[0024] In a third aspect, the present application provides a device for IMU and rigid body posture alignment, the device comprising a memory, a processor, and an IMU and rigid body posture alignment program stored in the memory and executable on the processor, wherein the IMU and rigid body posture alignment program, when executed by the processor, implements the method described in the first aspect above.
[0025] In a fourth aspect, the present application provides a computer-readable storage medium on which an IMU and rigid body attitude alignment program is stored. When the IMU and rigid body attitude alignment program is executed by a processor, it implements the method provided in the first aspect of the present application or any possible implementation method of the first aspect.
[0026] It can be seen from the above technical solutions that the present application has the following beneficial effects:
[0027] After fixing the positional relationship between the IMU and the rigid body, record the initial position of the rigid body; collect the attitude data of the rigid body and the IMU; solve the quaternion Q of the rotation relationship matrix based on the preset equation model and attitude data ir ; According to the preset first formula, the angle between the rigid body and the IMU quaternion of different postures is obtained. If all angles are less than a set threshold, Q is judged ir The solution is correct, and then the rigid body is tracked and positioned to obtain the quaternion Q of the IMU attitude at time t it and the center position of the rigid body, and thus the posture R of the rigid body at time t is obtained according to the preset second formula rt And the corresponding quaternion Q rt The above method realizes the fusion calibration of the IMU posture and the rigid body posture and the alignment of the rigid body and the IMU, improves the accuracy of the rigid body posture calculation, and finally realizes the stabilization and correction of the rigid body posture and position measurement data. BRIEF DESCRIPTION OF THE DRAWINGS
[0028] Figure 1 A schematic diagram of a process of aligning an IMU with a rigid body posture according to the present application is shown;
[0029] Figure 2 A schematic diagram of the structure of a device for aligning an IMU with a rigid body posture according to the present application is shown;
[0030] Figure 3 A structural schematic diagram of a device for aligning an IMU with a rigid body posture according to the present application is shown. DETAILED DESCRIPTION
[0031] The present application provides a method, device, equipment and readable storage medium for aligning the posture and position of an IMU with a rigid body, which are used to fuse, calibrate and align the posture of a rigid body with that of an IMU to improve the calculation accuracy of the rigid body's posture.
[0032] The following will be combined with the drawings in the embodiments of the present application to clearly and completely describe the technical solutions in the embodiments of the present application. Obviously, the described embodiments are only part of the embodiments of the present application, not all of the embodiments. Based on the embodiments in the present application, all other embodiments obtained by those skilled in the art without creative work are within the scope of protection of this application.
[0033] The terms "first", "second", etc. in the specification and claims of this application and the above-mentioned drawings are used to distinguish similar objects, and are not necessarily used to describe a specific order or sequence. It should be understood that the data used in this way can be interchangeable where appropriate, so that the embodiments described here can be implemented in an order other than that illustrated or described here. In addition, the terms "including" and "having" and any of their variations are intended to cover non-exclusive inclusions, for example, a process, method, system, product or device that includes a series of steps or modules is not necessarily limited to those steps or modules clearly listed, but may include other steps or modules that are not clearly listed or inherent to these processes, methods, products or devices. The naming or numbering of steps in this application does not mean that the steps in the method flow must be executed in the time / logical sequence indicated by the naming or numbering. The process steps that have been named or numbered can change the execution order according to the technical purpose to be achieved, as long as the same or similar technical effects can be achieved.
[0034] The division of modules in this application is a logical division. There may be other division methods when it is implemented in actual applications. For example, multiple modules can be combined or integrated into another system, or some features can be ignored or not executed. In addition, the mutual coupling or direct coupling or communication connection shown or discussed can be through some interfaces, and the indirect coupling or communication connection between modules can be electrical or other similar forms, which are not limited in this application. In addition, the modules or submodules described as separate components may or may not be physically separated, may or may not be physical modules, or may be distributed in multiple circuit modules, and some or all of the modules may be selected according to actual needs to achieve the purpose of the present application.
[0035] First of all, before introducing the present application, the camera device, IMU, rigid body and equipment for calibrating the IMU and rigid body posture involved in the present application are first introduced.
[0036] The camera device is a device that may be involved in optical motion capture processing, including one or more cameras, such as an omnidirectional camera or multiple motion capture cameras. The camera device can capture objects in the scene where it is located.
[0037] 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 detects the acceleration signal of the object in the independent three axes of the carrier coordinate system, while the gyroscope detects the angular velocity signal of the carrier relative to the navigation coordinate system. The IMU attitude data of the object can be measured, including the angular velocity and acceleration of the object in three-dimensional space.
[0038] A rigid body is an object whose shape and size remain unchanged during motion and after being subjected to force, and the relative positions of its internal points remain unchanged.
[0039] During the camera task of the camera device, the camera device is in or temporarily in motion and shoots objects in the scene. The IMU and the rigid body can be configured on the camera device, or relatively independent of the camera device, and move with the movement of the camera device to measure the corresponding original posture data. Of course, in actual applications, the IMU and the rigid body can also be independent of the camera device. When the camera device is not in motion or not involved, the IMU and the rigid body can also be in motion and measure the corresponding original posture data.
[0040] The device is a device with data processing capability such as a host, a server or a user equipment (UE), and the method of calibrating the IMU and the rigid body attitude of the present application is applied.
[0041] UE is a terminal device such as a desktop computer, laptop computer, all-in-one computer, tablet computer or smart phone.
[0042] Secondly, we will introduce how to obtain the stable posture information of the rigid body by fusing the posture data of the IMU and the rigid body based on the existing set of equations model.
[0043] To obtain the posture of the rigid body, it is necessary to align the IMU with the posture of the rigid body. At this time, it is necessary to align the camera coordinate system and the IMU coordinate system. Suppose the posture of the rigid body at time t is R rt , the quaternion corresponding to the rigid body's posture is Q rt , when the rigid body is bound to the camera device at t = 0, the rigid body posture unit matrix (that is, the initial posture) is R r0 , the posture of IMU at t = 0 (that is, the initial posture) is R i0 , then the corresponding inverse is The inverse of the quaternion of the IMU attitude at t = 0 is The posture of IMU at time t is R it , the quaternion corresponding to the IMU attitude is Q it , then we need to find the corresponding rotation relationship matrix R ir , the quaternion corresponding to the rotation relationship matrix is Q ir , so that It is represented by quaternion in, It is a quaternion multiplication operation. The posture of the rigid body and IMU is generally expressed by a matrix. After the posture is obtained, the corresponding quaternion and inverse can be obtained.
[0044] According to the left and right multiplication methods of quaternions, we can get Q ir The solution model:
[0045]
[0046] Assume that at the nth moment: in, is the quaternion of the IMU attitude at the nth moment, is the quaternion of the rigid body posture at the nth moment, then the homogeneous equation model is obtained:
[0047] Right now The homogeneous equation model can be solved by SVD decomposition (singular value decomposition method) to obtain Q ir or R ir .
[0048] If the correct rotation relationship matrix R is obtained ir or quaternion Q ir , then the posture R of the rigid body at time t rtIt can be obtained through IMU, that is: or Since the IMU attitude data is relatively accurate and stable, as long as accurate Q ir or R ir The data combined with the above method can obtain the posture of the rigid body more accurately and stably.
[0049] Next, based on the above background description, the method of aligning the IMU with the rigid body posture and position of the present application will be introduced in detail.
[0050] See also Figure 1 , Figure 1 A schematic flow chart of a method for aligning the IMU with a rigid body posture in the present application is shown. Specifically, the method may include the following steps:
[0051] Step S101, fix the positional relationship between the IMU and the rigid body, and record the initial position of the rigid body P0 = {p1, p2, ..., p m}, where p m =(x m ,y m , z m ) represents the three-dimensional coordinate data of the rigid body marking point, and m is the number of rigid body marking points;
[0052] It can be understood that the IMU and the rigid body are hardware components. After fixing the positional relationship between the IMU and the rigid body, the initial position P0 of the rigid body can be recorded. m}, where p m =(x m ,y m , z m ) represents the three-dimensional coordinate data of the rigid body marker point (i.e., the reflective ball), and m is the number of rigid body marker points. Then, the subsequent steps of the method for aligning the IMU and rigid body posture of the present application can be executed.
[0053] Step S102, collecting the posture data of the rigid body and IMU: n≥8, where The set of quaternions representing the rigid body and IMU attitude at the nth moment, is the quaternion of the rigid body posture at the nth moment, is the quaternion of the IMU attitude at the nth moment;
[0054] After fixing the positional relationship between the IMU and the rigid body, the calibration data can be collected in the optical multi-camera system, including collecting the posture of the bound rigid body and the posture of the IMU. The postures of the rigid body and the IMU are more than 8 different postures, and multiple and There is a rotation relationship of more than 30° between different postures, so the collected posture data of the rigid body and IMU are: It means that more than 8 different rigid body and IMU attitude data were collected.
[0055] Step S103, according to the preset equation group model and Solve the quaternion Q of the rotation relationship matrix ir ;
[0056] After collecting more than 8 different posture data in step S102, the quaternion set of the rigid body and IMU posture at the nth moment can be The quaternion of the rigid body posture at the nth moment And the quaternion of the IMU attitude at the nth moment Bring it into the above solution model, that is Thus, the corresponding homogeneous equations model is obtained, which is the preset equations model:
[0057] Right now Using SVD decomposition to solve the homogeneous equations model, we can get Q ir or R ir .
[0058] Step S104, obtain according to the preset first formula and The angle between and If the angle between them is less than a set threshold, then Q ir The solution is correct, otherwise, the solution is wrong;
[0059] After solving the rotation relationship matrix quaternion Q in step S103, ir Afterwards, it is necessary to further verify whether the solution value is correct to ensure the subsequent successful rigid body tracking and positioning.
[0060] Preset the first formula: in, is the inverse of the quaternion of the IMU attitude at the initial moment (this data can be obtained after binding the rigid body and IMU position relationship at the initial moment), is the inverse of the quaternion of the rotation relationship matrix, from which we can get and The angle relationship between If all and The angle d between n are all less than a set threshold T d , then judge Qir The solution is correct, that is, the posture alignment between the rigid body and the IMU is successful, otherwise, the solution is wrong.
[0061] Step S105, if Q ir If the solution is correct, the rigid body is tracked and positioned to obtain the quaternion Q of the IMU attitude at time t. it and the center position T of the rigid body t ;
[0062] If Q ir If the solution value is correct, the rigid body can be tracked and positioned in the optical multi-camera system. If the tracking and positioning is successful, the quaternion Q of the IMU attitude at time t can be obtained. it , and the rigid body center position T t .
[0063] Step S106, obtaining the posture R of the rigid body at time t according to the preset second formula rt And the corresponding quaternion Q rt ;
[0064] In seeking Q ir The correct value of and the quaternion Q of the IMU attitude at time t it After that, you can use the preset second formula: Get the posture R of the rigid body at time t rt And the corresponding quaternion Q rt .
[0065] It should be noted that after obtaining the above rigid body posture, the rigid body posture R rt Correct the position of the rigid body, see step S107 for details:
[0066] Step S107, calculate the position of the rigid body marker point at time t according to the preset third formula, compare the position of the rigid body marker point at time t with the position of the rigid body marker point in the actual 3D modeling, and judge whether the rigid body tracking is successful according to the comparison result. If the tracking is successful, the position of the rigid body marker point at time t is used to replace the position of the rigid body marker point in the actual 3D modeling, thereby obtaining the correct rigid body position information.
[0067] According to the preset third formula: p′ mt =R rt ·p m +T t , the position p′ of the rigid body marker point at time t can be obtained mt , p m is the three-dimensional coordinate data of the rigid body marking point in step S101, and p′ mt Compared with the position p of the rigid body marker in the actual 3D modeling mtCompare, that is, find the distance, and judge whether the rigid body tracking is successful based on the comparison result. If the tracking is successful, use p′ mt Replace p mt , so as to obtain the correct rigid body position information.
[0068] It should be noted that according to the position p′ of the rigid body marker at time t mt Compared with the position p of the rigid body marker in the actual 3D modeling mt The distance between the two can be obtained as d = ||p mt -p mt ′||, if the distance d>Th, Th is the preset threshold, the tracking is successful, otherwise the tracking fails. If the tracking is successful, it can be used to correct the position of the rigid body marker points obtained in 3D modeling, realize the fusion of the rigid body and IMU, and finally achieve the result of stabilizing and correcting the rigid body posture by using the posture of IMU.
[0069] In addition, if we judge Q ir If the solution is wrong, return to step S101, re-fix the position relationship between the IMU and the rigid body, and re-collect the posture data of the rigid body and the IMU to make Q ir The solution is correct.
[0070] The above is an introduction to the method of aligning the IMU of the present application with the posture and position of a rigid body. Now, the device for aligning the IMU of the present application with the posture of a rigid body will be introduced.
[0071] See also Figure 2 , Figure 2 A schematic diagram of a structure of a device for aligning the IMU with a rigid body posture of the present application is shown. Specifically, the device may include the following structure:
[0072] The fixing unit 201 is used to fix the position relationship between the IMU and the rigid body, and record the initial position P0 of the rigid body = {p1, p2, ..., p m}, where p m =(x m ,y m , z m ) represents the three-dimensional coordinate data of the rigid body marking points, and m is the number of rigid body marking points;
[0073] The acquisition unit 202 is used to collect the posture data of the rigid body and the IMU: in, The set of quaternions representing the rigid body and IMU attitude at the nth moment, is the quaternion of the rigid body posture at the nth moment, is the quaternion of the IMU attitude at the nth moment;
[0074] The calculation unit 203 is used to calculate the Solve the quaternion Q of the rotation relationship matrix ir ; According to the preset first formula, and The angle between and If the angle between them is less than a set threshold, then Q ir The solution is correct, otherwise, the solution is wrong; if Q ir If the solution is correct, the rigid body is tracked and positioned to obtain the quaternion Q of the IMU attitude at time t. it and the center position T of the rigid body t ; According to the preset second formula, the posture R of the rigid body at time t is obtained rt And the corresponding quaternion Q rt .
[0075] The collection, calculation and other processes involved in the above-mentioned device refer to the above-mentioned method steps S101-S107, which will not be repeated here.
[0076] See also Figure 3 , Figure 3 The present application shows a device for aligning an IMU with a rigid body posture. Specifically, the device includes a processor 301, a memory 302, and an IMU with a rigid body posture alignment program stored in the memory 302 and executable on the processor 301. When the IMU with a rigid body posture alignment program is executed by the processor 301, the following is achieved: Figure 1 The corresponding steps of the method for aligning the IMU with the rigid body posture in any embodiment.
[0077] Exemplary, IMU and rigid body attitude alignment program, the computer program can be divided into one or more modules / units, one or more modules / units are stored in the memory 302, and executed by the processor 301 to complete the present application. One or more modules / units can be a series of computer program instruction segments that can complete specific functions, and the instruction segments are used to describe the execution process of the computer program in the computer device.
[0078] The device for aligning the IMU with the rigid body posture may include, but is not limited to, the processor 301 and the memory 302. Those skilled in the art will appreciate that the illustration is merely an example of a computer device and does not constitute a limitation on the device for aligning the IMU with the rigid body posture, and may include more or fewer components than shown in the illustration, or a combination of certain components, or different components, for example, the device for aligning the IMU with the rigid body posture may also include input and output devices, network access devices, buses, etc., and the processor 301, the memory 302, the input and output devices, and the network access devices, etc. are connected via a bus.
[0079] The processor 301 may be a central processing unit (CPU), or other general-purpose processors, digital signal processors (DSP), application-specific integrated circuits (ASIC), field-programmable gate arrays (FPGA) or other programmable logic devices, discrete gates or transistor logic devices, discrete hardware components, etc. The general-purpose processor may be a microprocessor or any conventional processor, etc. The processor is the control center of the device for aligning the IMU with the rigid body posture, and uses various interfaces and lines to connect the various parts of the device for aligning the entire IMU with the rigid body posture.
[0080] The memory 302 can be used to store computer programs and / or modules. 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 calling the data stored in the memory 302. The memory 302 can mainly include a program storage area and a data storage area, wherein the program storage area can store an operating system, an application required for at least one function (such as a sound playback function, an image playback function, etc.), etc.; the data storage area can store data created according to the use of the mobile phone (such as audio data, video data, etc.), etc. In addition, the memory can include a high-speed random access memory, and can also include a non-volatile memory, such as a hard disk, a memory, a plug-in hard disk, a smart memory card (Smart Media Card, SMC), a secure digital (Secure Digital, SD) card, a flash card (FlashCard), at least one disk storage device, a flash memory device, or other volatile solid-state storage devices.
[0081] The present application also provides a readable storage medium on which is stored an IMU and rigid body posture alignment program, which when executed by a processor implements the following Figure 1 The method for aligning the IMU with the rigid body posture in any embodiment corresponds to.
[0082] It can be understood that 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 application is essentially or the part that contributes to the prior art 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 a number of instructions for a computer device (which can be a personal computer, a server, or a network device, etc.) to perform all or part of the steps of each method embodiment of the present application. The aforementioned storage medium includes: U disk, mobile hard disk, read-only memory (ROM), random access memory (RAM), disk or optical disk and other media that can store program code.
[0083] As described above, the above embodiments are only used to illustrate the technical solutions of the present application, rather than to limit them. Although the present application has been described in detail with reference to the aforementioned embodiments, those skilled in the art should understand that they can still modify the technical solutions described in the aforementioned embodiments, or make equivalent replacements for some of the technical features therein. However, these modifications or replacements do not deviate the essence of the corresponding technical solutions from the spirit and scope of the technical solutions of the embodiments of the present application.
Claims
1. A method for aligning an IMU with a rigid body posture, characterized in that: The method comprises: Fix the positional relationship between the IMU and the rigid body, and record the initial position of the rigid body; Collect the attitude data of the rigid body and IMU: n≥8, where The set of quaternions representing the rigid body and IMU attitude at the nth moment, is the quaternion of the rigid body posture at the nth moment, is the quaternion of the IMU attitude at the nth moment; According to the preset equation model and the Solve the quaternion Q of the rotation relationship matrix ir ; According to the preset first formula: get and stated The angle between and If the angle between them is less than a set threshold, it is determined that the Q ir The solution is correct, otherwise, the solution is wrong; is the inverse of the quaternion of the IMU attitude at the initial moment, is the inverse of the quaternion of the rotation relationship matrix; If the Q ir If the solution is correct, the rigid body is tracked and positioned to obtain the quaternion Q of the IMU attitude at time t. it ; According to the preset second formula, the posture R of the rigid body at time t is obtained rt And the corresponding quaternion Q rt ; The preset equation group model is: is the quaternion multiplication operation, where The preset second formula is:
2. The method according to claim 1, characterized in that The initial position of the rigid body is P0 = {p1, p2, ..., p m }, where p m =(x m ,y m ,z m ) represents the three-dimensional coordinate data of the rigid body marker points, and m is the number of rigid body marker points.
3. The method according to claim 1, characterized in that Multiple and There is a rotation relationship of more than 30° between different postures.
4. The method according to claim 1, characterized in that: The preset equation group model and the Solve the quaternion Q of the rotation relationship matrix ir Including: using SVD decomposition to solve the equation model to obtain Q ir Or the rotation relationship matrix R ir .
5. The method according to claim 1, characterized in that The first formula is used to obtain the and stated The angle between and If the angle between them is less than a set threshold, it is determined that the Q ir The solution is correct, otherwise, the solution is wrong, including: According to the preset first formula: Calculate the and stated The angle between If all and The angle d between n are all less than a set threshold T d , then judge the Q ir The solution is correct, otherwise, the solution is wrong.
6. The method according to claim 1, characterized in that The posture R of the rigid body at time t is obtained according to the preset second formula rt And the corresponding quaternion Q rt include: According to the preset second formula: Get the posture R of the rigid body at time t rt And the corresponding quaternion Q rt .
7. The method according to any one of claims 1 to 6, characterized in that: If we judge that Q ir If the solution is wrong, the position relationship between the IMU and the rigid body is re-fixed, and the posture data of the rigid body and the IMU are re-collected to make the Q ir The solution is correct.
8. A device for aligning an IMU with a rigid body, characterized in that: The device comprises: The fixed unit is used to fix the position relationship between the IMU and the rigid body and record the initial position of the rigid body; The acquisition unit is used to acquire the attitude data of the rigid body and the IMU: n≥8, where The set of quaternions representing the rigid body and IMU attitude at the nth moment, is the quaternion of the rigid body posture at the nth moment, is the quaternion of the IMU attitude at the nth moment; A calculation unit is used to calculate the value of the equation according to the preset equation group model and the Solve the quaternion Q of the rotation relationship matrix ir ; According to the preset first formula: get and stated The angle between and If the angle between them is less than a set threshold, it is determined that the Q ir The solution is correct, otherwise, the solution is wrong; if the Q ir If the solution is correct, the rigid body is tracked and positioned to obtain the quaternion Q of the IMU attitude at time t. it ; According to the preset second formula, the posture R of the rigid body at time t is obtained rt And the corresponding quaternion Q rt ;in, is the inverse of the quaternion of the IMU attitude at the initial moment, is the inverse of the quaternion of the rotation relationship matrix; The preset equation group model is: is the quaternion multiplication operation, where The preset second formula is:
9. A device for aligning an IMU with a rigid body, characterized in that: The device includes a memory, a processor, and an IMU and rigid body attitude alignment program stored in the memory and executable on the processor, wherein the IMU and rigid body attitude alignment program implements the method as described in any one of claims 1 to 7 when executed by the processor.
10. A computer-readable storage medium, characterized in that: An IMU and rigid body posture alignment program is stored thereon, and when the IMU and rigid body posture alignment program is executed by a processor, a method as described in any one of claims 1 to 7 is implemented.