CN114252073A

CN114252073A - Robot attitude data fusion method

Info

Publication number: CN114252073A
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: 2022-03-29
Anticipated expiration: 2041-11-25
Also published as: CN114252073B

Abstract

The invention discloses a robot attitude data fusion method, which utilizes angular acceleration integral output by a gyroscope to obtain attitude angle quaternion and carries out three-axis acceleration and acceleration true value [0,0,1 ] under a world coordinate system]^TComparing to obtain delta qacc to correct the roll angle and pitch angle, and obtaining delta q according to the output of the magnetometer_magTo correct the yaw angle and then to correct the attitude. The drawn map obtained by data fusion of the invention has obvious edge lines, smooth wall and clear right-angled corners.

Description

Robot attitude data fusion method

Technical Field

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

Background

SLAM (simultaneous localization and mapping) of a mobile robot is one of the key technologies for automatic navigation. With the continuous improvement of artificial intelligence and hardware computing capability, the SLAM technology is more and more diversified. SLAM based on two-dimensional radar has been a hotspot in research, and will face relatively complicated environmental problems and difficult problems. Gmapping is a common filtering-based open source SLAM algorithm framework. 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 data obtained from the motion sensor to estimate the position of the object, the position fix may be biased. In practice, there is also wheel slip and cumulative error. Accurate odometer data is very important for the whole system, and a certain deviation can occur if the odometer is obtained by using the encoder alone.

The IMU is an inertia measuring unit and is a high-sensitivity sensor, and due to physical factors in the manufacturing process, certain deviation exists between the actual coordinate axis and the ideal coordinate axis of the IMU inertia measuring unit, so that IMU noise, scale errors and axis deviation are caused. Therefore, calibration of the collected data is required to compensate for these errors. In the data fusion process of the IMU calibration data, the high-frequency noise of the accelerometer and the magnetometer and the low-frequency noise of the gyroscope can cause a large error of attitude calculation. Therefore, a proper data fusion method needs to be selected.

Disclosure of Invention

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

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

the accelerometer is sensitive to acceleration, and an error of an inclination angle calculated by taking an instantaneous value is large; the acceleration measures the inclination angle, the dynamic response of which is slow, and the signal is not available at high frequency, so that the high frequency can be suppressed by low pass. The angle obtained by integrating the gyroscope is not influenced by the acceleration, but the error caused by integral drift and temperature drift along with the increase of time is larger, the response of the gyroscope is fast, and the inclination angle can be measured after integration.

The invention uses the angular acceleration output by the gyroscope to obtain an angle through integration, and then corrects the roll angle and the pitch angle of the angle through the output of the accelerometer; and finally correcting the yaw angle through the magnetometer.

The method comprises the following specific steps:

1) and (3) calculating the attitude angle quaternion at the K-th moment under the discrete system by adopting the following formula:

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

representing the derivative of quaternion q at time k.

2) The three-axis acceleration is converted to the world coordinate system using the following equation:

to represent

And (4) performing inverse transformation on the quaternion.

3) The obtained estimation of the acceleration under the world coordinate system^Gg_pComparing with actual gravity acceleration, and calculating by using the following formula

Error quaternion of (2):

through the calculation, the method has the advantages that,

4) calculating Δ q using the formula_mag：

Δq_mag＝[Δq_0mag 0 0 Δq_3mag]^T

Through the calculation, the method has the advantages that,

5) and posture correction by the following formula

The invention has the beneficial effects that: the drawn map obtained by data fusion of the invention has obvious edge lines, smooth wall and clear right-angled corners.

Drawings

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

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

FIG. 3 is a rendered map obtained by 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

In order to make the objects, technical solutions and advantages of the embodiments of the present invention clearer, the technical solutions of the present invention will be clearly and completely described below with reference to the embodiments of the present invention, and it is obvious that the described embodiments are some embodiments of the present invention, but not all 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 invention.

Examples

As shown in fig. 4, which is an application scenario of the robot attitude data fusion method of the present invention, the IMU attitude sensor is located on the central axis of the disinfection robot, coaxial with the laser radar, the robot is driven by wheels, the odometer is located in the DS servo driver connected with the hub motors, and the hub motors are distributed on both sides of the central axis, and are symmetric about the central axis.

The rotation matrix R in SLAM can be represented by a quaternion q, q having a real part and three imaginary parts, written q ═ q0, q1, q2, q3]^TOr q ═ w, x, y, z]^TQ0 is a real part and is a scalar, [ q1, q2, q3]^TIs the imaginary part, which is a vector.

Fig. 1 is a flow chart of the method, and the method specifically includes the following steps:

using a quaternion q to represent the rotation, the attitude angle quaternion at the K-th time in the discrete system can be obtained by the following formula (1):

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

multiplying any vector in the world coordinate system, and rotating the vector from the world coordinate system to the current coordinate system;

the derivative of quaternion q at time k;

the quaternion derivative calculation formula (2) is:

in the formula (2), the formula (3) for calculating the circulant is:

using forward difference calculation in equation (1)

Multiply the circle into a common dot product operation:

using calculation from angular velocity

Converting the three-axis acceleration into a world coordinate system, and utilizing the real value of the converted acceleration [0,0,1 ] in the world coordinate system]^TAnd (gravity acceleration) comparison is carried out to obtain delta qacc, and because the three-axis acceleration data is utilized, the roll angle and the pitch angle can only be calculated, so that the delta qacc only comprises the correction of the roll angle and the pitch angle. Obtained by means of gyroscopes

Will be provided with^La is converted to the world coordinate system:

in the formula (8), the reaction mixture is,

to represent

And (4) performing inverse transformation on the quaternion.

The obtained estimation of the acceleration under the world coordinate system^Gg_pComparing with the actual gravity acceleration to calculate

Error quaternion of (2):

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

finishing to obtain:

let Δ qacc equal to 0, and finally solve:

after superposition, the following can be obtained:

because the accelerometer can only correct the roll angle and the yaw angle, the yaw angle is corrected by adopting the magnetometer, and the output of the magnetometer can be understood as the table or projection of the included angle of the magnetic wire and the three axes of the machine body coordinate system.

Δq_mag＝[Δq_0mag 0 0 Δq_3mag]^T......(14)；

Coupled (14) and (15) to obtain:

gamma is a gamma function in formula (16);

correcting the attitude after acceleration, adding yaw angle correction:

the simulation experiment is carried out in a Turtlebot simulation experiment environment, wherein FIG. 2 corresponds to a drawn map obtained without data fusion according to the present invention, and FIG. 3 corresponds to a drawn map obtained with data fusion according to the present invention. As can be clearly seen from the simulation graph, the map before data fusion has the defects that the deviation of wall identification is unclear and particularly the position of a right-angle corner has a fuzzy condition. The map edge line after data fusion is obvious and the wall body is smooth, and the right-angle corner is also clear.

It is noted that, in this document, 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. Also, 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 an … …" does not exclude the presence of other identical elements in a process, method, article, or apparatus that comprises the element.

The above examples are only intended to illustrate the technical solution of the present invention, but not to limit it; although the present 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 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 of the embodiments of the present invention.

Claims

1. A robot attitude data fusion method is characterized by comprising the following steps:

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

step 2), three-axis acceleration and true acceleration value [0,0,1 ] are calculated under a world coordinate system]^TComparing to obtain delta qacc to correct a rolling angle and a pitching angle;

step 3), obtaining delta q according to output of the magnetometer_magCorrecting the yaw angle;

step 4), adopting the following formula to carry out attitude correction

2. The robot attitude data fusion method according to claim 1, wherein the attitude angle quaternion at the K-th time in the discrete system in step 1) is obtained by the following calculation:

representing the derivative of quaternion q at time k.

3. The method for fusing robot pose data according to claim 1, wherein the three-axis acceleration is converted to the world coordinate system in step 2) by using the following formula:

to represent

And (4) performing inverse transformation on the quaternion.

4. A robot pose data fusion method according to claim 3, wherein the obtained estimation of acceleration under world coordinate system^Gg_pComparing with actual gravity acceleration, and calculating by using the following formula

Error quaternion of (2):

through the calculation, the method has the advantages that,

5. the method as claimed in claim 1, wherein in step 3), Δ q is calculated by using the following formula_mag：

Δq_mag＝[Δq_0mag 0 0 Δq_3mag]^T

Through the calculation, the method has the advantages that,