CN106647791B - Three-dimensional attitude measurement and control device, mechanical equipment and three-dimensional attitude measurement and control method - Google Patents
Three-dimensional attitude measurement and control device, mechanical equipment and three-dimensional attitude measurement and control method Download PDFInfo
- Publication number
- CN106647791B CN106647791B CN201611227121.5A CN201611227121A CN106647791B CN 106647791 B CN106647791 B CN 106647791B CN 201611227121 A CN201611227121 A CN 201611227121A CN 106647791 B CN106647791 B CN 106647791B
- Authority
- CN
- China
- Prior art keywords
- magnetic field
- output
- axis
- angle
- triaxial
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Active
Links
- 238000005259 measurement Methods 0.000 title claims abstract description 55
- 238000000034 method Methods 0.000 title claims description 13
- 230000001133 acceleration Effects 0.000 claims abstract description 57
- 230000005484 gravity Effects 0.000 claims abstract description 48
- 238000012545 processing Methods 0.000 claims abstract description 23
- 230000004927 fusion Effects 0.000 claims abstract description 16
- 238000006243 chemical reaction Methods 0.000 claims abstract description 4
- 238000012937 correction Methods 0.000 claims description 11
- 230000003068 static effect Effects 0.000 claims description 8
- 238000001914 filtration Methods 0.000 claims description 4
- 238000012360 testing method Methods 0.000 abstract description 2
- 238000006880 cross-coupling reaction Methods 0.000 description 7
- 230000008901 benefit Effects 0.000 description 5
- 238000004364 calculation method Methods 0.000 description 5
- 230000036544 posture Effects 0.000 description 4
- 238000012544 monitoring process Methods 0.000 description 3
- 238000004886 process control Methods 0.000 description 3
- 238000004519 manufacturing process Methods 0.000 description 2
- 230000001276 controlling effect Effects 0.000 description 1
- 230000007547 defect Effects 0.000 description 1
- 238000013461 design Methods 0.000 description 1
- 238000011161 development Methods 0.000 description 1
- 238000010586 diagram Methods 0.000 description 1
- 238000005516 engineering process Methods 0.000 description 1
- 230000007613 environmental effect Effects 0.000 description 1
- 230000007774 longterm Effects 0.000 description 1
- 238000000691 measurement method Methods 0.000 description 1
- 238000012986 modification Methods 0.000 description 1
- 230000004048 modification Effects 0.000 description 1
- 230000001105 regulatory effect Effects 0.000 description 1
- 230000009897 systematic effect Effects 0.000 description 1
- 238000010998 test method Methods 0.000 description 1
- 230000009466 transformation Effects 0.000 description 1
Images
Classifications
-
- G—PHYSICS
- G05—CONTROLLING; REGULATING
- G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
- G05D1/00—Control of position, course, altitude or attitude of land, water, air or space vehicles, e.g. using automatic pilots
- G05D1/08—Control of attitude, i.e. control of roll, pitch, or yaw
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
- G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
- G01C21/005—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 with correlation of navigation data from several sources, e.g. map or contour matching
Landscapes
- Engineering & Computer Science (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- Automation & Control Theory (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Aviation & Aerospace Engineering (AREA)
- Gyroscopes (AREA)
- Navigation (AREA)
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
Technical Field
The invention relates to the field of three-dimensional attitude measurement of engineering machinery, in particular to a three-dimensional attitude measurement and control device, mechanical equipment and a three-dimensional attitude test method.
Background
The intelligent control system is a new trend of production development of engineering machinery (such as an industrial robot, a road roller, a seeder and the like), is a control system which is supported by an information technology and implements a set of modern operation and management regularly and quantitatively according to the space-time information of a carrier, and is required to obtain the best production and environmental benefits with the most economic investment.
The control of the three-dimensional attitude of the high-precision carrier is usually the most important loop in an intelligent control system of the engineering machinery, and the three-dimensional attitude of the carrier needs to be accurately measured.
At present, three-dimensional attitude measurement of engineering machinery mainly has two application scenes: one is attitude anomaly monitoring, such as inclination monitoring of an offshore buoy and rollover monitoring of a muck truck; another is attitude tracking control, such as control of industrial grade robotic arms and farm implement control of agricultural machinery. The existing three-dimensional attitude measurement method mainly comprises an electronic compass method, wherein the electronic compass generally comprises two accelerometers in the horizontal direction and a three-axis magnetometer, the inclination angle of a carrier is measured through the two horizontal accelerometers, the inclination angle comprises a pitch angle and a roll angle of the carrier, the measured value of the three-axis magnetometer is leveled through the pitch angle and the roll angle, and the course angle of the carrier is calculated through the three-axis magnetometer.
Because the electronic compass calculates the pitch angle and the roll angle by simply depending on the measured value of the accelerometer, the calculation accuracy of the pitch angle and the roll angle can be directly influenced by the system error and the measurement noise of the accelerometer. The pitch angle and the roll angle are calculated by taking the gravity vector as a reference, and the accelerometer cannot distinguish the component of the gravity vector from the linear acceleration information of the carrier, so that when the carrier has the linear acceleration, the pitch angle and the roll angle measured by the accelerometer have larger errors. The three-axis magnetometer is sensitive to the electromagnetic environment, and when the electromagnetic environment around the three-axis magnetometer changes, the measured value of the three-axis magnetometer has a large error, so that the course angle is calculated wrongly.
Disclosure of Invention
Therefore, it is necessary to provide a three-dimensional attitude measurement and control device, a mechanical device, and a three-dimensional attitude measurement and control method, so as to improve measurement and control accuracy of the three-dimensional attitude of the carrier.
A three-dimensional attitude measurement and control device comprises:
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, and taking the gravity vector of the position of the three-axis accelerometer and the earth magnetic field vector of the position of the three-axis magnetometer as references, calculating the pitch angle, roll angle and course angle of the carrier according to the rotation angular velocities output by the three single-axis gyroscopes, the processing control module performs coordinate conversion on the linear acceleration output by the three-axis accelerometer and the magnetic field intensity output by the three-axis magnetometer according to the pitch angle, the roll angle and the course angle of the carrier, and after respectively obtaining the difference values of the converted linear acceleration and magnetic field intensity and the corresponding gravity vector and the earth magnetic field intensity, and correcting the pitch angle, the roll angle and the course angle of the carrier through a data fusion algorithm and performing error compensation on the rotation angular speed output by the triaxial gyroscope.
In one embodiment, the process 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 intensity 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: 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.
In one embodiment, the step of correcting the pitch angle, the roll angle, and the heading angle of the carrier by the processing control module through a data fusion algorithm and performing error compensation on the rotational angular velocity output by the gyroscope specifically includes:
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.
In one embodiment, the data fusion algorithm is a Kalman filtering data fusion algorithm.
In the three-dimensional attitude measurement and control device, the three-axis accelerometer, the three-axis magnetometer and the three mutually perpendicular single-axis gyroscopes are integrated on one side of the circuit board, and the processing control module is integrated on the other side of the circuit board, so that compared with the case that the three-axis accelerometer, the three-axis magnetometer, the three mutually perpendicular single-axis gyroscopes and the processing control module are integrated on the same side of the circuit board, the area of the circuit board is reduced, and the size of the whole three-dimensional attitude measurement and control device is further reduced; the linear acceleration output by the three-axis accelerometer and the magnetic field intensity output by the three-axis magnetometer are subjected to preset deviation compensation, so that the values output by the three-axis accelerometer and the three-axis magnetometer are more accurate; according to the difference between the linear acceleration output by the triaxial accelerometer and the gravity vector and the difference between the magnetic field intensity output by the triaxial magnetometer and the earth magnetic field, error values of a pitch angle, a roll angle, a course angle and a rotation angle output by the three uniaxial gyroscopes are respectively calculated, so that the accuracy of the measured pitch angle, roll angle, course angle and rotation angle can be improved, the three-dimensional attitude of the carrier is sequentially controlled, and the stability of the three-dimensional attitude of the carrier can be effectively improved.
A mechanical device is provided with the three-dimensional attitude measurement and control device.
The mechanical equipment is provided with the three-dimensional attitude measurement and control device, so that the accuracy of measuring and regulating the three-dimensional attitude of the carrier can be effectively improved.
A three-dimensional attitude measurement and control method is based on the three-dimensional attitude measurement and control device, and comprises the following steps:
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;
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;
and adjusting the three-dimensional attitude of the carrier according to the pitch angle, the roll angle, the course angle and the error values of the rotation angles output by the three single-axis gyroscopes.
In one embodiment, the preset deviation of the linear acceleration output by the triaxial accelerometer and the magnetic field strength output by the triaxial magnetometer is compensated by an ellipsoid correction algorithm.
In one embodiment, the difference between the linear acceleration output by the triaxial accelerometer and the corresponding gravity vector is: 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 intensity output by the three-axis magnetometer and the corresponding earth magnetic field 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.
In the measurement and control method for the three-dimensional attitude, the preset deviation compensation is carried out on the linear acceleration output by the triaxial accelerometer and the magnetic field intensity output by the triaxial magnetometer, so that the values output by the triaxial accelerometer and the triaxial magnetometer are more accurate; according to the difference between the linear acceleration output by the triaxial accelerometer and the gravity vector and the difference between the magnetic field intensity output by the triaxial magnetometer and the earth magnetic field, error values of a pitch angle, a roll angle, a course angle and a rotation angle output by the three uniaxial gyroscopes are respectively calculated, so that the accuracy of the measured pitch angle, roll angle, course angle and rotation angle can be improved, the three-dimensional attitude of the carrier is sequentially controlled, and the stability of the three-dimensional attitude of the carrier can be effectively improved.
Drawings
Fig. 1 is a schematic structural diagram of a three-dimensional attitude measurement and control device according to an embodiment;
fig. 2 is a schematic flow chart of a measurement and control method of a three-dimensional pose according to an embodiment.
Detailed Description
In order to make the objects, technical solutions and advantages of the present invention more apparent, the present invention is described in further detail below with reference to the accompanying drawings and embodiments. It should be understood that the specific embodiments described herein are merely illustrative of the invention and are not intended to limit the invention.
As shown in fig. 1, a three-dimensional posture measurement and control device of an embodiment includes:
a circuit board 150;
a three-axis accelerometer 120, a three-axis magnetometer 130 and three mutually perpendicular single-axis gyroscopes 110 mounted on one side of the circuit board 150;
a process control module 140 mounted at the other side of the circuit board 150;
the process control module 140 performs a preset bias compensation on the linear acceleration output from the three axis accelerometer 120 and the magnetic field strength output from the three axis magnetometer 130, and with reference to the gravity vector of the location of the three axis accelerometer 120 and the earth magnetic field vector of the location of the three axis magnetometer 130, the pitch angle, roll angle and course angle of the carrier are respectively calculated according to the rotation angular velocities output by the three single-axis gyroscopes 110, the processing control module 140 performs coordinate transformation on the linear acceleration output by the three-axis accelerometer 120 and the magnetic field intensity output by the three-axis magnetometer 130 according to the pitch angle, roll angle and course angle of the carrier, and after respectively obtaining the difference values of the converted linear acceleration and magnetic field intensity and the corresponding gravity vector and the earth magnetic field intensity, and correcting the pitch angle, the roll angle and the course angle of the carrier through a data fusion algorithm and compensating errors of the rotation angular speed output by the triaxial gyroscope 110.
In the three-dimensional attitude measurement and control device, the three-axis accelerometer, the three-axis magnetometer and the three mutually perpendicular single-axis gyroscopes are integrated on one side of the circuit board, and the processing control module is integrated on the other side of the circuit board, so that compared with the case that the three-axis accelerometer, the three-axis magnetometer, the three mutually perpendicular single-axis gyroscopes and the processing control module are integrated on the same side of the circuit board, the area of the circuit board is reduced, and the size of the whole three-dimensional attitude measurement and control device is further reduced; the linear acceleration output by the three-axis accelerometer and the magnetic field intensity output by the three-axis magnetometer are subjected to preset deviation compensation, so that the values output by the three-axis accelerometer and the three-axis magnetometer are more accurate; according to the difference between the linear acceleration output by the triaxial accelerometer and the gravity vector and the difference between the magnetic field intensity output by the triaxial magnetometer and the earth magnetic field, error values of a pitch angle, a roll angle, a course angle and a rotation angle output by the three uniaxial gyroscopes are respectively calculated, so that the accuracy of the measured pitch angle, roll angle, course angle and rotation angle can be improved, the three-dimensional attitude of the carrier is sequentially controlled, and the stability of the three-dimensional attitude of the carrier can be effectively improved.
As shown in fig. 1, the arrangement shown in fig. 1 is only the best choice for implementing the present embodiment, wherein one of the three mutually perpendicular single-axis gyroscopes 110 is disposed perpendicular to the circuit board 150, the other two are disposed parallel to the circuit board 150, and the three-axis accelerometer 120 and the three-axis magnetometer 130 are mounted at the vertical included angle between the two parallel single-axis gyroscopes 110. The structural design can reduce the area of the circuit board 150 to the maximum extent, and the volume of the whole three-dimensional posture measurement and control device is reduced.
In this embodiment, the processing control module 140 is a CPU processor, and is installed on the other side of the circuit board 150, and compared with the case where the three-axis accelerometer, the three-axis magnetometer, the three orthogonal single-axis gyroscope, and the processing control module are integrated on the same side of the circuit board, the area of the circuit board is reduced, and the size of the whole three-dimensional attitude measurement and control device is reduced.
It can be known that the three single-axis gyroscopes 110 measure the rotation angular velocities of the carrier in the positive direction around the three orthogonal axes (the three mutually orthogonal axes of the three single-axis gyroscopes), and the angles of the carrier rotating around the three orthogonal axes can be determined through an angle integral operation, so as to measure the variation of the angles. However, the initial attitude of the carrier is required during the test, and the measurement value of the gyroscope contains errors such as zero offset, cross coupling, random noise and the like, and the error of the angle integral operation is accumulated with time, so that the angle calculated by the angle integral operation needs to be corrected, and the measurement error of the gyroscope needs to be compensated. Therefore, the gyroscope has the characteristics of high short-term precision, good dynamic property and long-term error drift.
It will be appreciated that in a static situation, the tri-axial accelerometer 120 may measure the components of the reaction force of gravity in three orthogonal axes of the carrier, thereby determining the tilt of the carrier, including the pitch angle (pitch) and roll angle (roll) of the carrier. The three-axis accelerometer 120 calculates the angle directly from the discrete measurement values without integral operation, so the error of the three-axis accelerometer 120 in calculating the angle does not accumulate over time. Under static conditions, the accuracy of the three-axis accelerometer 120 in calculating the angle may be up to 0.1 ° or more. However, in a dynamic situation, since the triaxial accelerometer 120 can only measure the specific force of the carrier, i.e. the resultant force of the forces other than the gravity, but cannot identify the motion acceleration of the carrier from the measured value, the larger the motion acceleration of the carrier is, the larger the error of the calculated angle is, and the errors of the triaxial accelerometer, such as zero offset, cross coupling and random noise, also affect the accuracy of the calculated angle. Therefore, the triaxial accelerometer has the characteristics of high static accuracy, low dynamic accuracy, system deviation and error which are not accumulated along with time.
It can be known that the three-axis magnetometer 130 can measure components of the earth magnetic field on three orthogonal coordinate axes of the carrier, and the horizontally placed magnetometer can calculate the heading angle through the components of the earth magnetic field measured by the horizontal axis of the three-axis magnetometer 130, so that the heading angle measured by using the three-axis magnetometer 130 needs to be leveled by using the pitch angle and the roll angle of the carrier, and ideally, the accuracy of the heading angle calculated by the three-axis magnetometer can reach more than 1 degree, but errors such as zero offset, cross coupling, random noise and the like of the magnetometer can affect the calculation accuracy of the heading angle, and particularly, when the electromagnetic environment around the magnetometer changes, the measured value of the magnetometer can be obviously interfered to cause a wrong calculation of the heading angle. Therefore, the magnetometer has the characteristic of being easily interfered by the electromagnetic environment when measuring the course angle.
The single-axis gyroscope, the three-axis accelerometer and the three-axis magnetometer can respectively complete partial work of the three-dimensional attitude measurement of the carrier, and have respective obvious advantages. The single-axis gyroscope is high in short-term precision and good in dynamic property; errors of the triaxial accelerometer are not accumulated, and the static precision is high; the three-axis magnetometer can indicate north, and errors are not accumulated. Meanwhile, the single-axis gyroscope, the three-axis accelerometer and the three-axis magnetometer have respective obvious defects, wherein errors of the single-axis gyroscope can be accumulated and initial postures are required; the triaxial accelerometer is affected by the acceleration of the carrier and system errors; three axis magnetometers can be affected by electromagnetic interference and systematic errors.
In the embodiment, the uniaxial gyroscope, the triaxial accelerometer and the triaxial magnetic force are comprehensively arranged, the system errors of the triaxial accelerometer and the triaxial magnetometer, mainly zero bias and cross coupling errors, are compensated through an ellipsoid correction algorithm through the complementarity of the uniaxial gyroscope, the triaxial accelerometer and the triaxial magnetometer, and the measured data of the uniaxial gyroscope, the triaxial accelerometer and the triaxial magnetometer are fused through a data fusion algorithm, so that the advantage complementation of the uniaxial gyroscope, the triaxial accelerometer and the triaxial magnetometer is realized.
Specifically, 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. The ellipsoid correction algorithm can compensate the system deviations such as zero offset, cross coupling and the like of the triaxial accelerometer and the triaxial magnetometer, so that the accuracy of the original observed values of the triaxial accelerometer and the triaxial magnetometer is greatly improved. Furthermore, the pitch angle, roll angle and course angle of the carrier, which are statically output by the three-axis accelerometer and the three-axis magnetometer, can be used as initial values for updating the attitude of the three single-axis gyroscopes.
In general, there is an error in the solution of the three-dimensional attitude, and therefore, after the linear acceleration and the magnetic field intensity are projected to the navigation system, the values thereof do not conform to the gravity vector and the earth magnetic field. Because the short-term precision of the single-axis gyroscope integral calculation is high, whether the three-axis accelerometer and the three-axis magnetometer are interfered or not can be judged by using the difference value, and therefore the robustness of the whole three-dimensional attitude measurement and control device is improved.
After the linear acceleration and the magnetic field vector are projected to the navigation system, the values thereof do not conform to the gravity vector and the value of the earth magnetic field, that is, the error between the output of the triaxial accelerometer 120 and the gravity vector and the error between the output of the triaxial magnetometer and the earth magnetic field. The gravity error of the triaxial accelerometer is the difference value between the acceleration value of the linear acceleration output by the triaxial accelerometer projected to a three-dimensional coordinate system where a gravity vector is located and the gravity vector, the three-dimensional coordinate system has three polar axes, the difference value between the value of the linear acceleration output by the triaxial accelerometer projected to each polar axis and the gravity vector in the corresponding polar axis component is the gravity error value, and the error value has 3 values which are the gravity errors on the three polar axes respectively. The three-axis magnetometer has the same principle that the three-axis magnetometer has three polar axes, the difference between the value of the magnetic field intensity output by the three-axis magnetometer and the component of the earth magnetic field on the corresponding polar axis is the magnetic error value, and the error values are 3 and are respectively the magnetic error on the three polar axes.
In this embodiment, the processing control module calculates error values of a pitch angle, a roll angle, a course angle and a rotation angle output by three single-axis gyroscopes respectively through a Kalman filtering data fusion algorithm, and adjusts the three-dimensional attitude of the carrier according to the error values. Therefore, the measurement and regulation precision of the three-dimensional attitude of the carrier can be effectively improved.
The embodiment also provides a mechanical device, and the mechanical device is provided with the three-dimensional posture measurement and control device.
According to the above, the mechanical equipment is provided with the three-dimensional attitude measurement and control device, so that the accuracy of measuring and controlling the three-dimensional attitude of the carrier can be effectively improved.
As shown in fig. 2, a schematic flow chart of a measurement and control method of a three-dimensional attitude according to an embodiment is shown, where the measurement and control method is based on the three-dimensional attitude measurement and control device. Specifically, the method includes steps S110 to S150.
Step S110, acquiring a pitch angle and a roll angle output by a triaxial accelerometer and a course angle output by a triaxial magnetometer statically;
step S120, performing preset deviation compensation on the linear acceleration output by the triaxial accelerometer and the magnetic field intensity output by the triaxial magnetometer;
step S130, respectively obtaining 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;
step S140, respectively calculating error values of a pitch angle, a roll angle, a course angle and a rotation angle output by the three single-axis gyroscopes according to the difference values;
and S150, adjusting the three-dimensional attitude of the carrier according to the pitch angle, the roll angle, the course angle and the error values of the rotation angles output by the three single-axis gyroscopes.
In the measurement and control method for the three-dimensional attitude, the preset deviation compensation is carried out on the linear acceleration output by the triaxial accelerometer and the magnetic field intensity output by the triaxial magnetometer, so that the values output by the triaxial accelerometer and the triaxial magnetometer are more accurate; according to the difference between the linear acceleration output by the triaxial accelerometer and the gravity vector and the difference between the magnetic field intensity output by the triaxial magnetometer and the earth magnetic field, error values of a pitch angle, a roll angle, a course angle and a rotation angle output by the three uniaxial gyroscopes are respectively calculated, so that the accuracy of the measured pitch angle, roll angle, course angle and rotation angle can be improved, the three-dimensional attitude of the carrier is sequentially controlled, and the stability of the three-dimensional attitude of the carrier can be effectively improved.
In the embodiment, the uniaxial gyroscope, the triaxial accelerometer and the triaxial magnetic force are comprehensively arranged, the system errors of the triaxial accelerometer and the triaxial magnetometer, mainly zero bias and cross coupling errors, are compensated through an ellipsoid correction algorithm through the complementarity of the uniaxial gyroscope, the triaxial accelerometer and the triaxial magnetometer, and the measured data of the uniaxial gyroscope, the triaxial accelerometer and the triaxial magnetometer are fused through a data fusion algorithm, so that the advantage complementation of the uniaxial gyroscope, the triaxial accelerometer and the triaxial magnetometer is realized.
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, step S120 performs preset bias compensation on the three-axis accelerometer and the three-axis magnetometer through an ellipsoid correction algorithm. The ellipsoid correction algorithm can compensate the system deviations such as zero offset, cross coupling and the like of the triaxial accelerometer and the triaxial magnetometer, so that the accuracy of the original observed values of the triaxial accelerometer and the triaxial magnetometer is greatly improved. Furthermore, the pitch angle, roll angle and course angle of the carrier, which are statically output by the three-axis accelerometer and the three-axis magnetometer, can be used as initial values for updating the attitude of the three single-axis gyroscopes.
In general, there is an error in the solution of the three-dimensional attitude, so that the values of the linear acceleration and the magnetic field intensity after being projected to the navigation system do not conform to the components of the gravity vector and the earth magnetic field. Because the short-term precision of the single-axis gyroscope integral calculation is high, whether the three-axis accelerometer and the three-axis magnetometer are interfered or not can be judged by using the difference value, and therefore the robustness of the whole three-dimensional attitude measurement and control device is improved.
After the linear acceleration and the magnetic field intensity are projected to the navigation system, the values of the linear acceleration and the magnetic field intensity are not consistent with the gravity vector and the components of the earth magnetic field, namely, the error between the output of the triaxial accelerometer 120 and the gravity vector and the error between the output of the triaxial magnetometer and the earth magnetic field. The gravity error of the three-axis accelerometer is the difference between an acceleration value projected from a linear acceleration output by the accelerometer to a three-dimensional coordinate system where a gravity vector is located and the gravity vector, the three-dimensional coordinate system has three polar axes, the difference between a value projected from the linear acceleration output by the accelerometer to each polar axis and the gravity vector in a corresponding polar axis component is a gravity error value, and the error values are 3 and are respectively the gravity errors on the three polar axes. The three-axis magnetometer has the same principle that the three-axis magnetometer has three polar axes, the difference between the 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 component of the earth magnetic field on the corresponding polar axis is the magnetic error value, and the number of the error values is 3, namely the magnetic error on the three polar axes. In this embodiment, the processing control module calculates error values of a pitch angle, a roll angle, a course angle and a rotation angle output by three single-axis gyroscopes respectively through a Kalman filtering data fusion algorithm, and adjusts the three-dimensional attitude of the carrier according to the error values. Therefore, the measurement and regulation precision of the three-dimensional attitude of the carrier can be effectively improved.
The technical features of the embodiments described above may be arbitrarily combined, and for the sake of brevity, all possible combinations of the technical features in the embodiments described above are not described, but should be considered as being within the scope of the present specification as long as there is no contradiction between the combinations of the technical features.
The above-mentioned embodiments only express several embodiments of the present invention, and the description thereof is more specific and detailed, but not construed as limiting the scope of the invention. It should be noted that, for a person skilled in the art, several variations and modifications can be made without departing from the inventive concept, which falls within the scope of the present invention. Therefore, the protection scope of the present patent shall 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.
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201611227121.5A CN106647791B (en) | 2016-12-27 | 2016-12-27 | Three-dimensional attitude measurement and control device, mechanical equipment and three-dimensional attitude measurement and control method |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201611227121.5A CN106647791B (en) | 2016-12-27 | 2016-12-27 | Three-dimensional attitude measurement and control device, mechanical equipment and three-dimensional attitude measurement and control method |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN106647791A CN106647791A (en) | 2017-05-10 |
| CN106647791B true CN106647791B (en) | 2020-08-14 |
Family
ID=58832679
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN201611227121.5A Active CN106647791B (en) | 2016-12-27 | 2016-12-27 | Three-dimensional attitude measurement and control device, mechanical equipment and three-dimensional attitude measurement and control method |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN106647791B (en) |
Families Citing this family (20)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN107449424A (en) * | 2017-06-29 | 2017-12-08 | 江西科技师范大学 | The computational methods and system of a kind of three-dimension altitude angle |
| CN107328407A (en) * | 2017-07-11 | 2017-11-07 | 中国地质大学（武汉） | A kind of strapdown rigid body 3 d pose monitors display system and method in real time |
| CN107783150A (en) * | 2017-09-13 | 2018-03-09 | 成都路行通信息技术有限公司 | A kind of determination methods and device of vehicle-mounted GPS equipment Installation posture |
| CN108020224A (en) * | 2017-12-18 | 2018-05-11 | 武汉静磁栅机电制造有限公司 | A kind of autonomous type opening self-detective instrument and its measuring method |
| CN108279618A (en) * | 2018-03-28 | 2018-07-13 | 北京钢铁侠科技有限公司 | It is a kind of for the controller of robot, control method and robot |
| CN110417489B (en) * | 2018-04-26 | 2021-09-17 | 杭州海康机器人技术有限公司 | Calibration method and device for directional antenna of unmanned aerial vehicle |
| CN108801260B (en) * | 2018-05-07 | 2022-01-28 | 约肯机器人（上海）有限公司 | Data processing method and device based on underwater robot |
| CN108836351B (en) * | 2018-06-24 | 2021-10-08 | 北京旷景科技有限公司 | Wearable trunk posture monitoring system |
| CN108836350B (en) * | 2018-06-24 | 2021-10-08 | 北京旷景科技有限公司 | Wearable trunk posture monitoring system and manufacturing method |
| CN108946443A (en) * | 2018-07-13 | 2018-12-07 | 上海拓璞数控科技股份有限公司 | Hanging apparatus and hanging method |
| CN110823181A (en) * | 2018-07-23 | 2020-02-21 | 郑州宇通重工有限公司 | Sanitation vehicle, vehicle body posture detection method thereof, vehicle body stabilization method and corresponding system |
| CN109512435B (en) * | 2018-10-29 | 2021-07-13 | 上海电机学院 | Walking posture correction instrument capable of detecting in real time |
| CN109827468B (en) * | 2018-12-18 | 2021-04-20 | 中国船舶重工集团公司第七一0研究所 | Safety relief method for carrier-borne electromagnetic emission deep bomb |
| CN109901581A (en) * | 2019-03-15 | 2019-06-18 | 智久(厦门)机器人科技有限公司上海分公司 | A kind of scaling method and spin motion control method of AGV vehicle spin angle |
| CN110296683B (en) * | 2019-06-03 | 2023-08-22 | 新纳传感系统有限公司 | Dynamic inclinometer with centrifugal acceleration compensation |
| CN110618379A (en) * | 2019-08-07 | 2019-12-27 | 常州爱特科技股份有限公司 | Disconnecting switch on-off state monitoring system and working method |
| CN111426318B (en) * | 2020-04-22 | 2024-01-26 | 中北大学 | Low-cost AHRS course angle compensation method based on quaternion-extended Kalman filtering |
| CN112013876A (en) * | 2020-08-28 | 2020-12-01 | 上海爱观视觉科技有限公司 | IMU data calibration method, terminal device and computer readable storage medium |
| CN112987944B (en) * | 2021-03-12 | 2024-07-02 | 东莞市攀星智能运动科技有限公司 | Music playing control method based on foot gesture interaction |
| CN117260724B (en) * | 2023-10-09 | 2024-06-07 | 南京迈思物联网科技有限公司 | Attitude control method for deep well exploratory robot |
Family Cites Families (7)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| US20070088496A1 (en) * | 2005-07-20 | 2007-04-19 | Atair Aerospace, Inc. | Automatic heading and reference system |
| EP2520903B1 (en) * | 2011-02-28 | 2014-06-18 | BlackBerry Limited | Portable electronic device adapted to compensate for gyroscope bias |
| CN103776448B (en) * | 2014-02-17 | 2016-08-31 | 武汉元生创新科技有限公司 | A kind of attitude heading reference system |
| CN104197987A (en) * | 2014-09-01 | 2014-12-10 | 北京诺亦腾科技有限公司 | Combined-type motion capturing system |
| CN104567931B (en) * | 2015-01-14 | 2017-04-05 | 华侨大学 | A kind of heading effect error cancelling method of indoor inertial navigation positioning |
| CN104897153A (en) * | 2015-06-23 | 2015-09-09 | 苏州市英富美欣科技有限公司 | Carrier attitude measuring system based on MEMS (micro-electromechanical systems) and MR (magnetic-resistance) sensors |
| CN105509740A (en) * | 2015-12-31 | 2016-04-20 | 广州中海达卫星导航技术股份有限公司 | Measuring method and module for attitude of agriculture machinery vehicle |
-
2016
- 2016-12-27 CN CN201611227121.5A patent/CN106647791B/en active Active
Also Published As
| Publication number | Publication date |
|---|---|
| CN106647791A (en) | 2017-05-10 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN106647791B (en) | Three-dimensional attitude measurement and control device, mechanical equipment and three-dimensional attitude measurement and control method | |
| US20190094264A1 (en) | Heading Confidence Interval Estimation | |
| CN104736963B (en) | mapping system and method | |
| CN104884902B (en) | For three axle magnetometer and the method and apparatus of the data fusion of three axis accelerometer | |
| CN107270893B (en) | Lever arm and time asynchronous error estimation and compensation method for real estate measurement | |
| CN109238262B (en) | Anti-interference method for course attitude calculation and compass calibration | |
| CN104880189B (en) | A kind of antenna for satellite communication in motion low cost tracking anti-interference method | |
| US10302453B2 (en) | Attitude sensor system with automatic accelerometer bias correction | |
| KR20170104621A (en) | How to update the pre-posture angle of an agricultural machine based on a 9-axis MEMS sensor | |
| US20110066395A1 (en) | Systems and methods for gyroscope calibration | |
| CN102981151B (en) | Phased array radar electronic control wave beam stabilizing method | |
| CN105509740A (en) | Measuring method and module for attitude of agriculture machinery vehicle | |
| CA2431564A1 (en) | Monitoring accuracy of an electronic compass | |
| CN110160497B (en) | Iron tower inclination measuring method and device, computer equipment and storage medium | |
| KR101211703B1 (en) | Calibration method of the magnetometer error using a line of sight vector and the integrated navigation system using the same | |
| CN113008227B (en) | Geomagnetic binary measurement method for measuring attitude based on three-axis accelerometer | |
| CN106403952A (en) | Method for measuring combined attitudes of Satcom on the move with low cost | |
| AU2015305864B2 (en) | Earthmoving machine comprising weighted state estimator | |
| CN116147624B (en) | Ship motion attitude calculation method based on low-cost MEMS navigation attitude reference system | |
| JP6983565B2 (en) | Methods and systems for compensating for soft iron magnetic disturbances in vehicle heading reference systems | |
| AU2017232241B1 (en) | Low Cost INS | |
| Sokolović et al. | INS/GPS navigation system based on MEMS technologies | |
| US9863867B2 (en) | Automatically updating hard iron and soft iron coefficients of a magnetic sensor | |
| CN111307114B (en) | Water surface ship horizontal attitude measurement method based on motion reference unit | |
| Allotta et al. | Underwater vehicles attitude estimation in presence of magnetic disturbances |
Legal Events
| Date | Code | Title | Description |
|---|---|---|---|
| PB01 | Publication | ||
| PB01 | Publication | ||
| SE01 | Entry into force of request for substantive examination | ||
| SE01 | Entry into force of request for substantive examination | ||
| GR01 | Patent grant | ||
| GR01 | Patent grant |