CN114252073B

CN114252073B - Robot attitude data fusion method

Info

Publication number: CN114252073B
Application number: CN202111415634.XA
Authority: CN
Inventors: 张琳; 骆敏舟; 杭小树
Original assignee: Institute of Intelligent Manufacturing Technology JITRI
Current assignee: Institute of Intelligent Manufacturing Technology JITRI
Priority date: 2021-11-25
Filing date: 2021-11-25
Publication date: 2023-09-15
Anticipated expiration: 2041-11-25
Also published as: CN114252073A

Abstract

The invention discloses a robot attitude data fusion method, which utilizes angular acceleration integration output by a gyroscope to obtain attitude angle quaternion, and three-axis acceleration and acceleration true value [0, 1 ] are carried out under a world coordinate system] ^T Obtaining delta qacc through comparison, correcting the roll angle and the pitch angle, and obtaining delta q according to magnetometer output _mag To correct the yaw angle and then to correct the attitude. The drawn map obtained by adopting the data fusion method has obvious edge lines, smooth wall bodies and clear right-angle corners.

Description

Robot attitude data fusion method

Technical Field

The invention belongs to the technical field of robots, and particularly relates to a robot gesture data fusion method.

Background

SLAM (synchronous localization and mapping, simultaneous localization and mapping) of mobile robots is one of the key technologies for automatic navigation. With the increasing artificial intelligence and hardware computing power, SLAM technology is becoming increasingly diverse. SLAM based on two-dimensional radar has been a hotspot in research processes, and will face relatively complex environmental and difficult problems. Gmapping is a common framework of filtering-based open source SLAM algorithms. The algorithm can draw an indoor map in real time, and the progress is high when a small scene map is created, but the algorithm is highly dependent on odometer data. If the odometer uses only the data obtained from the motion sensor to estimate the position of the object, a deviation in positioning may occur. In practice, there is also wheel slip and accumulated error. Accurate odometer data is important for the whole system and certain deviations can occur if the odometer is obtained solely by using an encoder.

The IMU is an inertial measurement unit, is a high-sensitivity sensor, and has certain deviation between an actual coordinate axis and an ideal coordinate axis due to physical factors in the manufacturing process, so that IMU noise, scale error and axis deviation are caused. Therefore, calibration of the collected data is required to compensate for these errors. In the data fusion process of IMU calibration data, high-frequency noise of the accelerometer and the magnetometer and low-frequency noise of the gyroscope can cause larger errors in attitude calculation. It is therefore necessary to select an appropriate data fusion method.

Disclosure of Invention

The invention provides a robot gesture data fusion method aiming at the problems in the prior art.

The invention solves the technical problems by the following technical means:

the accelerometer is sensitive to acceleration, and the inclination angle error is relatively large by taking an instantaneous value; the accelerometer measures tilt angle, which is correspondingly slow in its dynamics, and signals are not available at high frequencies, so high frequencies can be suppressed by low pass. The angle obtained by integrating the gyroscope is not influenced by acceleration, but the error caused by the integral drift and the temperature drift is larger along with the increase of time, the gyroscope responds quickly, and the inclination angle can be measured after the integration.

According to the invention, the angular acceleration output by the gyroscope is used, the angle is obtained through integration, and then the rolling angle and the pitch angle of the angle are corrected through the output of the accelerometer; and finally correcting the yaw angle by a magnetometer.

The method comprises the following steps:

1) The attitude angle quaternion at the Kth moment under a discrete system is obtained by adopting the following formula:

g represents the world coordinate system, L represents the current coordinate system,

and when the k moment is represented, the derivative of the quaternion q.

2) The three-axis acceleration is converted to a world coordinate system using:

representation->The quaternion is inversely transformed.

3) Estimation of acceleration in world coordinate system to be obtained ^G g _p Comparing with the actual gravity acceleration, the method is calculated by adopting the following formulaError quaternion of (c):

through the calculation, the method has the advantages that,

4) Delta q is calculated using _mag ：

Δq _mag ＝[Δq _0mag 0 0 Δq _3mag ] ^T

Through the calculation, the method has the advantages that,

5) Posture correction is performed by using the following method

The beneficial effects of the invention are as follows: the drawn map obtained by adopting the data fusion method has obvious edge lines, smooth wall bodies and clear right-angle corners.

Drawings

FIG. 1 is a flow chart of the method of the present invention;

FIG. 2 is a map drawn without the method of the present invention;

FIG. 3 is a map drawn using the method of the present invention;

fig. 4 is an application scenario of the robot pose data fusion method of the present invention.

Detailed Description

For the purpose of making the objects, technical solutions and advantages of the embodiments of the present invention more apparent, the technical solutions of the present invention will be clearly and completely described in conjunction with the embodiments of the present invention, and it is apparent that the described embodiments are some embodiments of the present invention, but not all embodiments. All other embodiments, which can be made by those skilled in the art based on the embodiments of the invention without making any inventive effort, are intended to be within the scope of the invention.

Examples

As shown in fig. 4, in an application scenario of the method for fusing the gesture data of the robot, the IMU gesture sensor is located on the central axis of the disinfection robot, and is coaxial with the laser radar, the robot is driven by a wheel, the odometer is located in a DS servo driver connected with a hub motor, and the hub motor is distributed on two sides of the central axis and is symmetrical about the central axis.

The rotation matrix R in SLAM can be represented by a quaternion q, q having one real part and three imaginary parts, written as q= [ q0, q1, q2, q3] ^T Or q= [ w, x, y, z] ^T Q0 is the real part and is a scalar, [ q1, q2, q3 ]] ^T Is the imaginary part, which is the vector.

FIG. 1 is a flow chart of the method, and the specific steps of the method include:

using the quaternion q to represent rotation, the attitude angle quaternion at the kth time under the discrete system can be calculated by the following formula (1):

in the formula (1), G represents a world coordinate system, and L represents a current coordinate system;

multiplying by any vector in the world coordinate system, the vector can be rotated from the world coordinate system to the current coordinate system;

when the k moment is represented, the derivative of the quaternion q;

the quaternion derivative calculation formula (2) is:

in the formula (2), the calculation formula (3) of the circle is:

using the forward differential equation (1)

The circle is multiplied into a common point multiplication operation:

using calculation of angular velocityConverting the three-axis acceleration into the world coordinate system, and utilizing the acceleration true value [0, 1 ] in the converted world coordinate system] ^T And (g) comparing to obtain delta qacc, wherein the delta qacc only comprises correction of the roll angle and the pitch angle because the roll angle and the pitch angle can be calculated by using the three-axis acceleration data. Using gyroscopes to get->Will be ^L a conversion to world coordinate system:

in the formula (8), the amino acid sequence of the compound,representation->The quaternion is inversely transformed.

Estimation of acceleration in world coordinate system to be obtained ^G g _p Comparing with the actual gravity acceleration, calculatingError quaternion of (c):

r (q) represents a rotation matrix corresponding to the quaternion q, and the specific formula is as follows:

and (3) finishing to obtain:

let Δqacc=0, and finally solve for:

after superposition, finally the method can be obtained:

because the accelerometer can only correct the roll angle and the yaw angle, and the magnetometer is used for correcting the yaw angle, the output of the magnetometer can be understood as the top of the magnetic line or projection of the three-axis included angle of the machine body coordinate system.

Δq _mag ＝[Δq _0mag 0 0 Δq _3mag ] ^T ......(14)；

And (4) combining the two (14) and (15), and obtaining:

gamma is a gamma function in formula (16);

correcting the accelerated posture, and adding yaw angle correction:

simulation experiments are carried out in a Turtlebot simulation experiment environment, wherein FIG. 2 corresponds to a drawing map obtained by data fusion without adopting the invention, and FIG. 3 corresponds to a drawing map obtained by adopting the data fusion. As can be clearly seen from the simulation diagram, the map before data fusion has the defects that the wall identification deviation is unclear at some edge lines, and particularly, the position of a right-angle corner is blurred. The map edge line after data fusion is obvious, the wall is smooth, and the right-angle corner is clear.

It is noted that relational terms such as first and second, and the like, if any, are used solely to distinguish one entity or action from another entity or action without necessarily requiring or implying any actual such relationship or order between such entities or actions. Moreover, the terms "comprises," "comprising," or any other variation thereof, are intended to cover a non-exclusive inclusion, such that a process, method, article, or apparatus that comprises a list of elements does not include only those elements but may include other elements not expressly listed or inherent to such process, method, article, or apparatus. Without further limitation, an element defined by the phrase "comprising one … …" does not exclude the presence of other like elements in a process, method, article, or apparatus that comprises the element.

The above embodiments are only for illustrating the technical solution of the present invention, and are not limiting; although the invention has been described in detail with reference to the foregoing embodiments, it will be understood by those of ordinary skill in the art that: the technical scheme described in the foregoing embodiments can be modified or some technical features thereof can be replaced by equivalents; such modifications and substitutions do not depart from the spirit and scope of the technical solutions of the embodiments of the present invention.

Claims

1. The robot posture data fusion method is characterized by comprising the following steps of:

step 1), obtaining an attitude angle quaternion by utilizing angular acceleration integration output by a gyroscope;

step 2), three-axis acceleration and acceleration true value [0, 1 ] are carried out under the world coordinate system] ^T Obtaining delta qacc through comparison to correct the rolling angle and the pitch angle;

step 3), obtaining delta q according to magnetometer output _mag To correct the yaw angle;

step 4), carrying out posture correction by adopting the following formula:

in step 2), three-axis acceleration is converted into a world coordinate system by adopting the following steps:

representation->Inverse transforming quaternions;

estimation of acceleration in world coordinate system to be obtained ^G g _p Comparing with the actual gravity acceleration, the method is calculated by adopting the following formulaError quaternion of (c):

through the calculation, the method has the advantages that,

in step 3), Δq is calculated using the formula _mag ：

Δq _mag ＝[Δq _0mag 0 0 Δq _3mag ] ^T

Through the calculation, the method has the advantages that,

2. the method for fusing robot pose data according to claim 1, wherein the pose angle quaternion at the kth time under the discrete system in step 1) is calculated by the following formula:

and when the k moment is represented, the derivative of the quaternion q.