CN106647791B - Three-dimensional attitude measurement and control device, mechanical equipment and three-dimensional attitude measurement and control method - Google Patents

Abstract

The invention relates to a three-dimensional attitude measurement and control device, mechanical equipment and a three-dimensional attitude testing method, wherein the device comprises the following steps: a circuit board; the device comprises a circuit board, a three-axis accelerometer, a three-axis magnetometer and three mutually vertical single-axis gyroscopes, wherein the three-axis accelerometer, the three-axis magnetometer and the three mutually vertical single-axis gyroscopes are arranged on one side of the circuit board; the processing control module performs preset deviation compensation on the linear acceleration output by the triaxial accelerometer and the magnetic field intensity output by the triaxial magnetometer, respectively calculates the pitch angle, the roll angle and the course angle of the carrier by taking the gravity vector of the position where the triaxial accelerometer is located and the earth magnetic field vector of the position where the triaxial magnetometer is located as references, performs coordinate conversion on the linear acceleration output by the triaxial accelerometer and the magnetic field intensity output by the triaxial magnetometer, corrects the pitch angle, the roll angle and the course angle of the carrier through a data fusion algorithm, and performs error compensation on the rotating angular velocity output by the triaxial gyroscope. The invention can effectively improve the measurement and regulation precision of the three-dimensional attitude of the carrier.

Description

Three-dimensional attitude measurement and control device, mechanical equipment and three-dimensional attitude measurement and control method

technical field

The invention relates to the field of three-dimensional attitude measurement of construction machinery, in particular to a three-dimensional attitude measurement and control device, mechanical equipment and a three-dimensional attitude testing method.

Background technique

Intelligent control system is a new trend in the production and development of construction machinery (such as industrial robots, road rollers, seeders, etc.). It is supported by information technology and implements a complete set of modern operation and management control systems regularly and quantitatively according to the time and space information of the carrier. , requires the best production and environmental benefits with the most economical input.

The control of the high-precision three-dimensional attitude of the carrier is usually the most important part of the intelligent control system of construction machinery, which requires accurate measurement of the three-dimensional attitude of the carrier.

At present, there are two main application scenarios for 3D attitude measurement of construction machinery: one is attitude abnormality monitoring, such as tilt monitoring of offshore buoys and rollover monitoring of muck trucks; the other is attitude tracking control, such as industrial-grade robotic arms. Control and implement control of farm machinery. The current three-dimensional attitude measurement method is mainly the electronic compass method. The electronic compass generally includes two horizontal accelerometers and a three-axis magnetometer. The two horizontal accelerometers measure the tilt angle of the carrier, including the pitch angle and roll angle of the carrier. Then, the measured value of the three-axis magnetometer is leveled by the pitch angle and roll angle, and the heading angle of the carrier is calculated by the three-axis magnetometer.

Since the electronic compass simply relies on the measurement value of the accelerometer to calculate the pitch angle and roll angle, the system error and measurement noise of the accelerometer will directly affect the calculation accuracy of the pitch angle and roll angle. The calculation of the pitch angle and roll angle is based on the gravity vector, and the accelerometer cannot distinguish the component of the gravity vector and the linear acceleration information of the carrier. Therefore, when the carrier has linear acceleration, the pitch angle and lateral acceleration measured by the accelerometer There will be a large error in the roll angle. The three-axis magnetometer is sensitive to the electromagnetic environment. When the electromagnetic environment around the three-axis magnetometer changes, the measured value of the three-axis magnetometer will have a large error, resulting in an error in the calculation of the heading angle.

SUMMARY OF THE INVENTION

Based on this, it is necessary to provide a three-dimensional attitude measurement and control device, mechanical equipment and a three-dimensional attitude measurement and control method, so as to improve the measurement and control accuracy of the three-dimensional attitude of the carrier.

A three-dimensional attitude measurement and control device, comprising:

circuit board;

a three-axis accelerometer, a three-axis magnetometer and three mutually perpendicular single-axis gyroscopes mounted on one side of the circuit board;

a processing control module installed on the other side of the circuit board;

The processing control module performs preset deviation compensation on the linear acceleration output by the three-axis accelerometer and the magnetic field strength output by the three-axis magnetometer, and uses the gravity vector at the location of the three-axis accelerometer and the The earth's magnetic field vector at the location of the three-axis magnetometer is used as a reference, and the pitch angle, roll angle, and heading angle of the carrier are calculated according to the rotational angular velocities output by the three single-axis gyroscopes. The pitch angle, roll angle and heading angle perform coordinate transformation on the linear acceleration output by the three-axis accelerometer and the magnetic field strength output by the three-axis magnetometer, and obtain the converted linear acceleration and magnetic field strength and the corresponding gravity respectively. After the difference between the vector and the strength of the earth's magnetic field, the pitch angle, roll angle and heading angle of the carrier are corrected through a data fusion algorithm, and the error compensation is performed on the rotational angular velocity output by the three-axis gyroscope.

In one of the embodiments, the processing control module is a CPU.

In one embodiment, the processing control module performs preset deviation compensation on the linear acceleration output by the three-axis accelerometer and the magnetic field strength output by the three-axis magnetometer through an ellipsoid correction algorithm.

In one embodiment, the difference between the converted linear acceleration and the corresponding gravity vector is: the difference between the acceleration value of the linear acceleration output by the accelerometer projected to the three-dimensional coordinate system where the gravity vector is located and the gravity vector; The difference between the converted magnetic field intensity and the corresponding earth magnetic field intensity is: the difference between the magnetic field intensity value of the magnetic field intensity output by the magnetometer projected to the three-dimensional coordinate system where the earth magnetic field is located and the earth magnetic field intensity.

In one embodiment, the processing and control module corrects the pitch angle, roll angle and heading angle of the carrier and performs error compensation on the rotational angular velocity output by the gyroscope through a data fusion algorithm. Specifically:

The processing control module fuses the measured values of the three single-axis gyroscopes, the three-axis magnetometer and the three-axis accelerometer through the data fusion algorithm, and the three single-axis The pitch angle, roll angle, and heading angle calculated by the gyroscope are corrected and the errors of the rotation angles output by the three single-axis gyroscopes are compensated.

In one embodiment, the data fusion algorithm is a Kalman filter data fusion algorithm.

In the above-mentioned three-dimensional attitude measurement and control device, one side of the circuit board integrates a three-axis accelerometer, a three-axis magnetometer and three mutually perpendicular single-axis gyroscopes, and the other side integrates a processing control module, which is connected to the three-axis accelerometer. Compared with the integration of the three-axis magnetometer, three mutually perpendicular single-axis gyroscopes and processing control modules on the same side of the circuit board, the area of the circuit board is reduced, thereby reducing the volume of the entire three-dimensional attitude measurement and control device; The linear acceleration output by the axis accelerometer and the magnetic field strength output by the three-axis magnetometer are preset deviation compensation, which can make the output value of the three-axis accelerometer and the three-axis magnetometer more accurate; according to the linear acceleration and gravity output by the three-axis accelerometer The difference between the vector and the difference between the magnetic field strength output by the three-axis magnetometer and the earth's magnetic field, the error values of the pitch angle, roll angle, heading angle and the rotation angle output by the three single-axis gyroscopes can be calculated respectively. Improve the accuracy of the measured pitch angle, roll angle, heading angle and rotation angle, and control the three-dimensional attitude of the carrier in turn, which can effectively improve the stability of the three-dimensional attitude of the carrier.

A mechanical device is provided with the above-mentioned three-dimensional attitude measurement and control device.

The mechanical equipment is equipped with the above-mentioned three-dimensional attitude measurement and control device, which can effectively improve the accuracy of measuring and regulating the three-dimensional attitude of the carrier.

A three-dimensional attitude measurement and control method, based on the three-dimensional attitude measurement and control device described above, the method includes:

Obtain the pitch angle and roll angle of the static output of the three-axis accelerometer and the heading angle of the static output of the three-axis magnetometer;

Perform preset deviation compensation on the linear acceleration output by the three-axis accelerometer and the magnetic field strength output by the three-axis magnetometer;

Obtain the difference between the linear acceleration output by the three-axis accelerometer and the corresponding gravity vector, and the difference between the magnetic field strength output by the three-axis magnetometer and the corresponding Earth's magnetic field strength;

Calculate the error values of the pitch angle, roll angle, heading angle and the rotation angles output by the three single-axis gyroscopes according to the difference values;

The three-dimensional attitude of the carrier is adjusted according to the error values of the pitch angle, roll angle, heading angle and the rotation angle output by the three single-axis gyroscopes.

In one embodiment, the linear acceleration output by the three-axis accelerometer and the magnetic field strength output by the three-axis magnetometer are pre-set offset compensation through an ellipsoid correction algorithm.

In one embodiment, the difference between the linear acceleration output by the three-axis accelerometer and the corresponding gravity vector is: the acceleration value of the linear acceleration output by the three-axis accelerometer projected to the three-dimensional coordinate system where the gravity vector is located and the gravity The difference value of the vector; the difference between the magnetic field intensity output by the three-axis magnetometer and the corresponding earth's magnetic field is: the magnetic field intensity value of the magnetic field intensity output by the three-axis magnetometer projected to the three-dimensional coordinate system where the earth's magnetic field is located and the magnetic field intensity value of the earth's magnetic field The difference in magnetic field strength.

In the above-mentioned three-dimensional attitude measurement and control method, the preset deviation compensation is performed on the linear acceleration output by the three-axis accelerometer and the magnetic field strength output by the three-axis magnetometer, so that the values output by the three-axis accelerometer and the three-axis magnetometer can be more accurate. ;According to the difference between the linear acceleration output by the three-axis accelerometer and the gravity vector and the difference between the magnetic field strength output by the three-axis magnetometer and the earth's magnetic field, the pitch angle, roll angle, heading angle and three single The error value of the rotation angle output by the axis gyroscope can improve the accuracy of the measured pitch angle, roll angle, heading angle and rotation angle, and control the three-dimensional attitude of the carrier in turn, which can effectively improve the stability of the three-dimensional attitude of the carrier. .

Description of drawings

1 is a schematic structural diagram of a three-dimensional attitude measurement and control device according to an embodiment;

FIG. 2 is a schematic flowchart of a three-dimensional attitude measurement and control method according to an embodiment.

Detailed ways

In order to make the objectives, technical solutions and advantages of the present invention clearer, the present invention will be further described in detail below with reference to the accompanying drawings and embodiments. It should be understood that the specific embodiments described herein are only used to explain the present invention, but not to limit the present invention.

As shown in FIG. 1 , a three-dimensional attitude measurement and control device according to an embodiment includes:

circuit board 150;

The three-axis accelerometer 120, the three-axis magnetometer 130 and the three mutually perpendicular single-axis gyroscopes 110 are installed on one side of the circuit board 150;

the processing control module 140 installed on the other side of the circuit board 150;

The processing control module 140 performs preset deviation compensation on the linear acceleration output by the triaxial accelerometer 120 and the magnetic field intensity output by the triaxial magnetometer 130, and uses the triaxial accelerometer 120 to measure the gravity vector at the location and the triaxial magnetometer. The earth magnetic field vector at the position of 130 is a reference, and the pitch angle, roll angle, and heading angle of the carrier are respectively calculated according to the rotational angular velocity output by the three single-axis gyroscopes 110. The angle and heading angle perform coordinate transformation on the linear acceleration output by the triaxial accelerometer 120 and the magnetic field strength output by the triaxial magnetometer 130, and obtain the difference between the converted linear acceleration and magnetic field strength and the corresponding gravity vector and the earth's magnetic field strength respectively. After the value is determined, the pitch angle, roll angle and heading angle of the carrier are corrected by the data fusion algorithm, and the error compensation of the rotational angular velocity output by the three-axis gyroscope 110 is performed.

In the above-mentioned three-dimensional attitude measurement and control device, one side of the circuit board integrates a three-axis accelerometer, a three-axis magnetometer and three mutually perpendicular single-axis gyroscopes, and the other side integrates a processing control module, which is connected to the three-axis accelerometer. Compared with the integration of the three-axis magnetometer, three mutually perpendicular single-axis gyroscopes and processing control modules on the same side of the circuit board, the area of the circuit board is reduced, thereby reducing the volume of the entire three-dimensional attitude measurement and control device; The linear acceleration output by the axis accelerometer and the magnetic field strength output by the three-axis magnetometer are preset deviation compensation, which can make the output value of the three-axis accelerometer and the three-axis magnetometer more accurate; according to the linear acceleration and gravity output by the three-axis accelerometer The difference between the vector and the difference between the magnetic field strength output by the three-axis magnetometer and the earth's magnetic field, the error values of the pitch angle, roll angle, heading angle and the rotation angle output by the three single-axis gyroscopes can be calculated respectively. Improve the accuracy of the measured pitch angle, roll angle, heading angle and rotation angle, and control the three-dimensional attitude of the carrier in turn, which can effectively improve the stability of the three-dimensional attitude of the carrier.

As shown in FIG. 1 , the arrangement shown in FIG. 1 is only an optimal choice for implementing this embodiment, wherein one of the three mutually perpendicular single-axis gyroscopes 110 is set perpendicular to the circuit board 150 , and the other two The three-axis accelerometer 120 and the three-axis magnetometer 130 are installed at the vertical angle between the two single-axis gyroscopes 110 arranged in parallel with the circuit board 150 . Such a structural design can minimize the area of the circuit board 150 and reduce the volume of the entire three-dimensional attitude measurement and control device.

In this embodiment, the processing control module 140 is a CPU processor, which is installed on the other side of the circuit board 150, and is connected with the three-axis accelerometer, the three-axis magnetometer, the three mutually perpendicular single-axis gyroscopes, and the processing control module. Compared with being integrated on the same side of the circuit board, the area of the circuit board is reduced, thereby reducing the volume of the entire three-dimensional attitude measurement and control device.

It can be known that the three single-axis gyroscopes 110 measure the rotational angular velocity of the carrier around the three orthogonal axes (three mutually orthogonal axes of the three single-axis gyroscopes) in the positive direction, and the carrier can be determined through the angle integral operation. The angle rotated about three orthogonal axes, thereby measuring the change in angle. However, the initial attitude of the carrier is required during the test, and since the measurement value of the gyroscope contains errors such as zero offset, cross-coupling and random noise, the error of the angle integral operation will accumulate over time, so the angle calculated by the angle integral operation needs to be calculated. Correction and compensation for the measurement error of the gyroscope. Therefore, the attitude measurement of gyroscope usually has the characteristics of high short-term accuracy, good dynamic performance and long-term drift of error.

It can be known that, under static conditions, the three-axis accelerometer 120 can measure the components of the reaction force of gravity on the three orthogonal coordinate axes of the carrier, so as to determine the inclination of the carrier, including the pitch and roll of the carrier. angle (roll). The triaxial accelerometer 120 directly calculates the angle through discrete measurement values, and does not need to undergo an integral operation. Therefore, the error of the triaxial accelerometer 120 in calculating the angle will not accumulate over time. Under static conditions, the accuracy of the angle calculated by the three-axis accelerometer 120 can reach more than 0.1°. However, in a dynamic situation, since the three-axis accelerometer 120 can only measure the specific force of the carrier, that is, the resultant force of forces other than gravity, and cannot identify the motion acceleration of the carrier from its measured value, the greater the motion acceleration of the carrier, the more The error of the angle is also larger, and the errors such as the zero offset, cross-coupling and random noise of the three-axis accelerometer will also affect the accuracy of the angle calculation. Therefore, usually three-axis accelerometers have the characteristics of high static accuracy, low dynamic accuracy, and systematic deviation and error that do not accumulate over time.

It can be known that the three-axis magnetometer 130 can measure the components of the earth's magnetic field on the three orthogonal coordinate axes of the carrier, and the horizontally placed magnetometer can calculate the heading based on the components of the earth's magnetic field measured by the horizontal axis of the three-axis magnetometer 130. Therefore, to use the three-axis magnetometer 130 to measure the heading angle, it is first necessary to use the pitch angle and roll angle of the carrier to level the measured value of the three-axis magnetometer 130. Ideally, the heading angle calculated by the three-axis magnetometer is equal to The accuracy can reach more than 1°, but errors such as the zero offset of the magnetometer, cross-coupling and random noise will affect the calculation accuracy of the heading angle, especially when the electromagnetic environment around the magnetometer changes, the measured value of the magnetometer will be significantly affected. interference, which results in an incorrect heading angle calculation. Therefore, magnetometer measurement of heading angle has the characteristics of being susceptible to electromagnetic environment interference.

The single-axis gyroscope, the three-axis accelerometer and the three-axis magnetometer can each complete part of the work of the three-dimensional attitude measurement of the carrier, and have their own obvious advantages. Among them, the single-axis gyroscope has high short-term accuracy and good dynamic performance; the three-axis accelerometer does not accumulate errors and has high static accuracy; the three-axis magnetometer can point north and the error does not accumulate. At the same time, the single-axis gyroscope, the three-axis accelerometer and the three-axis magnetometer also have their own obvious shortcomings. Among them, the single-axis gyroscope error will accumulate and require an initial attitude; the three-axis accelerometer will be affected by the carrier acceleration and system errors; Triaxial magnetometers are subject to electromagnetic interference and systematic errors.

In this embodiment, a single-axis gyroscope, a three-axis accelerometer, and a three-axis magnetic force are comprehensively provided. Through the complementarity of the three, the system errors of the three-axis accelerometer and the three-axis magnetometer are compensated by the ellipsoid correction algorithm. Zero bias and cross-coupling error, and then fuse the measurement data of single-axis gyroscope, three-axis accelerometer and three-axis magnetometer through data fusion algorithm to realize the integration of single-axis gyroscope, three-axis accelerometer and three-axis magnetometer. Complementary advantages.

Specifically, in this embodiment, the processing control module 140 performs preset deviation compensation on the linear acceleration output by the triaxial accelerometer and the magnetic field strength output by the triaxial magnetometer by using an ellipsoid correction algorithm. The ellipsoid correction algorithm can compensate system deviations such as zero offset and cross-coupling of the triaxial accelerometer and the triaxial magnetometer, thereby greatly improving the accuracy of the original observation values of the triaxial accelerometer and the triaxial magnetometer. Further, the pitch angle, roll angle and heading angle of the carrier, which are statically output by the three-axis accelerometer and the three-axis magnetometer, can be used as the initial values for the attitude update of the three single-axis gyroscopes.

Usually, there is an error in the calculation of the three-dimensional attitude. Therefore, after projecting the linear acceleration and the magnetic field strength to the navigation system, their values are inconsistent with the gravity vector and the earth's magnetic field. In this embodiment, the difference is used to estimate the three-dimensional attitude through the Kalman filtering data fusion algorithm. And the error of the gyroscope instrument parameters and the three-dimensional attitude and gyroscope parameters are corrected. Because of the high short-term accuracy of the single-axis gyroscope integral solution, the difference value can be used to determine whether the three-axis accelerometer and the three-axis magnetometer are disturbed, thereby improving the robustness of the entire three-dimensional attitude measurement and control device.

Among them, after the linear acceleration and magnetic field vector are projected to the navigation system, their values do not match the values of the gravity vector and the earth's magnetic field, that is, the error between the output of the three-axis accelerometer 120 and the gravity vector and the error between the output of the three-axis magnetometer and the earth's magnetic field . The gravity error of the three-axis accelerometer is the difference between the acceleration value of the linear acceleration output by the three-axis accelerometer projected to the three-dimensional coordinate system where the gravity vector is located and the gravity vector. The three-dimensional coordinate system has three polar axes, and the three-axis accelerometer outputs The difference between the value of the linear acceleration projected to each polar axis and the gravity vector in the corresponding polar axis component is the gravity error value, and there are three error values, which are the gravity errors on the three polar axes. The magnetic error of the triaxial magnetometer is the difference between the magnetic field strength value of the magnetic field strength output by the triaxial magnetometer projected to the three-dimensional coordinate system where the earth's magnetic field is located and the magnetic field strength of the earth. Similarly, the three-dimensional coordinate system has three polar axes , the difference between the magnetic field intensity output by the magnetometer projected to each polar axis and the component of the earth's magnetic field on the corresponding polar axis is the magnetic error value, and the error value is 3, which are the magnetic forces on the three polar axes. error.

In this embodiment, the processing control module calculates the error values of the pitch angle, roll angle, heading angle and the rotation angle output by the three single-axis gyroscopes through the Kalman filtering data fusion algorithm, and adjusts the three-dimensional attitude of the carrier according to the error values. . Therefore, this embodiment can effectively improve the measurement and control accuracy of the three-dimensional attitude of the carrier.

This embodiment also provides a mechanical device, and the mechanical device is installed with the three-dimensional attitude measurement and control device as described above.

As can be seen from the above, the mechanical equipment is installed with the above-mentioned three-dimensional attitude measurement and control device, which can effectively improve the accuracy of measuring and regulating the three-dimensional attitude of the carrier.

As shown in FIG. 2 , it is a schematic flowchart of a three-dimensional attitude measurement and control method according to an embodiment, and the measurement and control method is based on the above-mentioned three-dimensional attitude measurement and control device. Specifically, the method includes steps S110 to S150.

Step S110, acquiring the pitch angle and roll angle output by the three-axis accelerometer and the heading angle statically output by the three-axis magnetometer;

Step S120, performing preset deviation compensation on the linear acceleration output by the three-axis accelerometer and the magnetic field intensity output by the three-axis magnetometer;

Step S130, respectively obtaining the difference between the linear acceleration output by the triaxial accelerometer and the corresponding gravity vector and the difference between the magnetic field intensity output by the triaxial magnetometer and the corresponding earth magnetic field intensity;

Step S140, calculate the error values of the pitch angle, the roll angle, the heading angle and the rotation angles output by the three single-axis gyroscopes respectively according to the difference values;

Step S150: Adjust the three-dimensional attitude of the carrier according to the error values of the pitch angle, roll angle, heading angle and the rotation angle output by the three single-axis gyroscopes.

In the above-mentioned three-dimensional attitude measurement and control method, the preset deviation compensation is performed on the linear acceleration output by the three-axis accelerometer and the magnetic field strength output by the three-axis magnetometer, so that the values output by the three-axis accelerometer and the three-axis magnetometer can be more accurate. ;According to the difference between the linear acceleration output by the three-axis accelerometer and the gravity vector and the difference between the magnetic field strength output by the three-axis magnetometer and the earth's magnetic field, the pitch angle, roll angle, heading angle and three The error value of the rotation angle output by the single-axis gyroscope can improve the accuracy of the measured pitch angle, roll angle, heading angle and rotation angle, and control the three-dimensional attitude of the carrier in turn, which can effectively improve the stability of the three-dimensional attitude of the carrier. sex.

In this embodiment, a single-axis gyroscope, a three-axis accelerometer, and a three-axis magnetic force are comprehensively provided. Through the complementarity of the three, the system errors of the three-axis accelerometer and the three-axis magnetometer are compensated by the ellipsoid correction algorithm. Zero bias and cross-coupling error, and then fuse the measurement data of single-axis gyroscope, three-axis accelerometer and three-axis magnetometer through data fusion algorithm to realize the integration of single-axis gyroscope, three-axis accelerometer and three-axis magnetometer. Complementary advantages.

In this embodiment, the processing control module 140 performs preset deviation compensation on the linear acceleration output by the three-axis accelerometer and the magnetic field strength output by the three-axis magnetometer through an ellipsoid correction algorithm. That is, in step S120, preset deviation compensation is performed on the three-axis accelerometer and the three-axis magnetometer through the ellipsoid correction algorithm. The ellipsoid correction algorithm can compensate system deviations such as zero offset and cross-coupling of the triaxial accelerometer and the triaxial magnetometer, thereby greatly improving the accuracy of the original observation values of the triaxial accelerometer and the triaxial magnetometer. Further, the pitch angle, roll angle and heading angle of the carrier, which are statically output by the three-axis accelerometer and the three-axis magnetometer, can be used as the initial values for the attitude update of the three single-axis gyroscopes.

Usually, there is an error in the calculation of the three-dimensional attitude. Therefore, after projecting the linear acceleration and the magnetic field strength to the navigation system, the values are inconsistent with the gravity vector and the components of the earth's magnetic field. This embodiment uses their difference to estimate by the Kalman filtering data fusion algorithm. The error of 3D attitude and gyroscope instrument parameters is obtained and the 3D attitude and gyroscope parameters are corrected. Because of the high short-term accuracy of the single-axis gyroscope integral solution, the difference value can be used to determine whether the three-axis accelerometer and the three-axis magnetometer are disturbed, thereby improving the robustness of the entire three-dimensional attitude measurement and control device.

Among them, after the linear acceleration and magnetic field strength are projected to the navigation system, their values do not match the gravity vector and the components of the earth's magnetic field, that is, the error between the output of the three-axis accelerometer 120 and the gravity vector and the output of the three-axis magnetometer and the existence of the earth's magnetic field are inconsistent. error. The gravity error of the three-axis accelerometer is the difference between the acceleration value of the linear acceleration output by the accelerometer projected to the three-dimensional coordinate system where the gravity vector is located and the gravity vector. The three-dimensional coordinate system has three polar axes, and the linear acceleration output by the accelerometer is projected. The difference between the value of each polar axis and the gravity vector in the corresponding polar axis component is the gravity error value, and there are three error values, which are the gravity errors on the three polar axes. The magnetic error of the three-axis magnetometer is the difference between the magnetic field strength value of the magnetic field intensity output by the magnetometer projected to the three-dimensional coordinate system where the earth's magnetic field is located and the magnetic field intensity of the earth. Similarly, the three-dimensional coordinate system has three polar axes. The difference between the magnetic field intensity output by the meter projected to each polar axis and the component of the earth's magnetic field on the corresponding polar axis is the magnetic error value, and the error values are 3, which are the magnetic errors on the three polar axes. In this embodiment, the processing control module calculates the error values of the pitch angle, roll angle, heading angle and the rotation angle output by the three single-axis gyroscopes through the Kalman filtering data fusion algorithm, and adjusts the three-dimensional attitude of the carrier according to the error values. . Therefore, this embodiment can effectively improve the measurement and control accuracy of the three-dimensional attitude of the carrier.

The technical features of the above-described embodiments can be combined arbitrarily. For the sake of brevity, all possible combinations of the technical features in the above-described embodiments are not described. However, as long as there is no contradiction between the combinations of these technical features, All should be regarded as the scope described in this specification.

The above-mentioned embodiments only represent several embodiments of the present invention, and the descriptions thereof are specific and detailed, but should not be construed as a limitation on the scope of the invention patent. It should be pointed out that for those of ordinary skill in the art, without departing from the concept of the present invention, several modifications and improvements can also be made, which all belong to the protection scope of the present invention. Therefore, the protection scope of the patent of the present invention should be subject to the appended claims.

Claims (6)

1. A three-dimensional attitude measurement and control device is characterized by comprising:

a circuit board;

the three-axis accelerometer, the three-axis magnetometer and the three mutually vertical single-axis gyroscopes are arranged on one side of the circuit board;

the processing control module is arranged on the other side of the circuit board;

the processing control module carries out preset deviation compensation on the linear acceleration output by the triaxial accelerometer and the magnetic field intensity output by the triaxial magnetometer through an ellipsoid correction algorithm, calculates the pitch angle, the roll angle and the course angle of a carrier respectively according to the rotation angular velocity output by the three uniaxial gyroscopes by taking the gravity vector at the position of the triaxial accelerometer and the earth magnetic field vector at the position of the triaxial magnetometer as references, carries out coordinate conversion on the linear acceleration output by the triaxial accelerometer and the magnetic field intensity output by the triaxial magnetometer according to the pitch angle, the roll angle and the course angle of the carrier, corrects the pitch angle, the roll angle and the course angle of the carrier through a data fusion algorithm and carries out error compensation on the rotation angular velocity output by the triaxial magnetometer after respectively obtaining the difference values of the converted linear acceleration and the converted magnetic field intensity, the gravity vector and the earth magnetic field intensity (ii) a

The difference value between the converted linear acceleration and the corresponding gravity vector is as follows: projecting the linear acceleration output by the accelerometer to the difference value between the acceleration value of the three-dimensional coordinate system where the gravity vector is located and the gravity vector; the difference value between the converted magnetic field strength and the corresponding earth magnetic field strength is as follows: and the magnetic field intensity output by the magnetometer is projected to the difference value between the magnetic field intensity value of the three-dimensional coordinate system where the earth magnetic field is located and the earth magnetic field intensity.

2. The three-dimensional attitude measurement and control device according to claim 1, wherein the processing control module is a CPU.

3. The three-dimensional attitude measurement and control device according to claim 1, wherein the processing and control module corrects the pitch angle, roll angle and course angle of the carrier through a data fusion algorithm and performs error compensation on the rotational angular velocity output by the gyroscope, specifically:

the processing control module fuses the measurement values of the three single-axis gyroscopes, the three-axis magnetometers and the three-axis accelerometers through a data fusion algorithm, corrects the pitch angle, the roll angle and the course angle calculated by the three single-axis gyroscopes through the measurement values of the three-axis accelerometers and the measurement values of the three-axis magnetometers, and compensates errors of the rotating angles output by the three single-axis gyroscopes.

4. The three-dimensional attitude measurement and control device according to claim 3, wherein the data fusion algorithm is a Kalman filtering data fusion algorithm.

5. A mechanical device, characterized in that the mechanical device is provided with a three-dimensional attitude measurement and control device according to any one of claims 1 to 4.

6. A three-dimensional attitude measurement and control method based on the three-dimensional attitude measurement and control device of any one of claims 1 to 4, the method comprising:

acquiring a pitch angle and a roll angle of the static output of the triaxial accelerometer and a course angle of the static output of the triaxial magnetometer;

performing preset deviation compensation on the linear acceleration output by the triaxial accelerometer and the magnetic field intensity output by the triaxial magnetometer through an ellipsoid correction algorithm;

respectively acquiring the difference value between the linear acceleration output by the triaxial accelerometer and the corresponding gravity vector and the difference value between the magnetic field strength output by the triaxial magnetometer and the corresponding earth magnetic field strength;

calculating error values of the pitch angle, the roll angle, the course angle and the rotation angle output by the three single-axis gyroscopes according to the difference values;

adjusting the three-dimensional attitude of the carrier according to the pitch angle, the roll angle, the course angle and the error value of the rotation angle output by the three single-axis gyroscopes;

the difference value between the linear acceleration output by the triaxial accelerometer and the corresponding gravity vector is as follows: projecting the linear acceleration output by the triaxial accelerometer to the difference value between the acceleration value of the three-dimensional coordinate system where the gravity vector is located and the gravity vector; the difference value between the magnetic field strength output by the three-axis magnetometer and the magnetic field strength of the corresponding earth is as follows: and the difference value between the magnetic field intensity value of the three-dimensional coordinate system where the earth magnetic field is located and the earth magnetic field intensity is projected by the magnetic field intensity output by the three-axis magnetometer.

Images