CN109001787B - Attitude angle resolving and positioning method and fusion sensor thereof - Google Patents
Attitude angle resolving and positioning method and fusion sensor thereof Download PDFInfo
- Publication number
- CN109001787B CN109001787B CN201810517173.9A CN201810517173A CN109001787B CN 109001787 B CN109001787 B CN 109001787B CN 201810517173 A CN201810517173 A CN 201810517173A CN 109001787 B CN109001787 B CN 109001787B
- Authority
- CN
- China
- Prior art keywords
- information
- fusion
- data
- gyroscope
- attitude angle
- 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
Images
Classifications
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
- G01S19/00—Satellite radio beacon positioning systems; Determining position, velocity or attitude using signals transmitted by such systems
- G01S19/38—Determining a navigation solution using signals transmitted by a satellite radio beacon positioning system
- G01S19/39—Determining a navigation solution using signals transmitted by a satellite radio beacon positioning system the satellite radio beacon positioning system transmitting time-stamped messages, e.g. GPS [Global Positioning System], GLONASS [Global Orbiting Navigation Satellite System] or GALILEO
- G01S19/42—Determining position
- G01S19/45—Determining position by combining measurements of signals from the satellite radio beacon positioning system with a supplementary measurement
- G01S19/47—Determining position by combining measurements of signals from the satellite radio beacon positioning system with a supplementary measurement the supplementary measurement being an inertial measurement, e.g. tightly coupled inertial
-
- 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/10—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration
- G01C21/12—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning
- G01C21/16—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation
- G01C21/165—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation combined with non-inertial navigation instruments
Landscapes
- Engineering & Computer Science (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Automation & Control Theory (AREA)
- Computer Networks & Wireless Communication (AREA)
- Navigation (AREA)
Abstract
A method for resolving and positioning attitude angle and a fusion sensor thereof are provided, wherein the fusion sensor comprises a plurality of IMU sensors, a magnetometer and a GPS; the method comprises the following steps: calibrating the magnetometer, accelerometer and gyroscope; respectively carrying out data fusion on the measurement data of each accelerometer and each gyroscope by using a redundant information fusion algorithm to obtain acceleration information and angular velocity information; fusing acceleration information, angular velocity information and magnetic field information of the magnetometer by using an extended Kalman filtering algorithm to obtain a fused attitude angle; and obtaining position information by fusing the attitude angle auxiliary positioning information. Because the multi-redundancy IMU sensor is used for data measurement and data fusion calculation, and the extended Kalman filtering algorithm is used for fusing each data to obtain a fusion attitude angle, a higher-precision attitude angle can be obtained under both static and dynamic conditions; meanwhile, the fused attitude angle is utilized to assist the GPS to obtain the corrected position information, so that the position information is more accurate, and the stability and reliability of attitude angle calculation and positioning are improved.
Description
Technical Field
The invention relates to the technical field of integrated navigation, in particular to an attitude angle resolving and positioning method and a fusion sensor thereof.
Background
In the last 70 th century, navigation technology has been developed rapidly, and with the development of information technology and economic construction, more diversity and higher requirements are put forward on navigation technology in the fields of transportation, industry, agriculture, surveying and mapping and the like.
At present, the combined navigation technology is most widely applied in the aspects of posture determination and positioning of the navigation technology. The integrated Navigation is a comprehensive Navigation System formed by combining an INS (Inertial Navigation System) with one or more of a GPS (Global Positioning System), a radio Navigation System, an astronomical Navigation System, a satellite Navigation System, and the like. The INS is a navigation parameter calculation system with a gyroscope and an accelerometer as sensitive devices, the system establishes a navigation coordinate system according to the output of the gyroscope, and calculates the speed and the position of a carrier in the navigation coordinate system according to the output of the accelerometer.
In existing combined navigation techniques, the combination of GPS and INS is one of the most common ways. However, the existing GPS and INS combined navigation technology greatly depends on GPS signals, the concealment of the GPS signals is extremely poor, and the signals are easily interfered and shielded, so that the navigation precision is greatly reduced; moreover, the operation of the GPS receiver is affected by the maneuvering of the aircraft, and when the maneuvering of the aircraft exceeds the dynamic range of the GPS receiver, the receiver may lose lock and fail to capture and track satellite signals, thereby failing to operate, and thus, in an occasion with a high requirement for reliability, GPS navigation may be limited. Meanwhile, in the existing combined navigation technology of the GPS and the INS, the INS uses a gyroscope and an accelerometer as sensitive devices, and utilizes a complementary filtering fusion algorithm to solve attitude angles, so that the calculation complexity is high, and the error of the attitude angles obtained under a dynamic condition is large.
Therefore, when the existing integrated navigation technology is used for resolving and positioning the attitude angle, the accuracy of the obtained attitude angle information and position information is not high, and the stability and reliability of attitude determination and positioning are low.
Disclosure of Invention
The application provides a method for resolving and positioning attitude angles and a fusion sensor thereof, which are used for obtaining attitude angle information and position information with high precision and high stability and improving the stability and reliability of resolving and positioning attitude angles.
According to a first aspect, an embodiment provides an attitude angle solution and positioning method, applied in a fused sensor for attitude angle solution and positioning, the fused sensor comprising at least two IMU sensors, one magnetometer and GPS, one IMU sensor comprising one accelerometer and one gyroscope, the method comprising:
acquiring measurement data of each accelerometer and each gyroscope, magnetic field information of each magnetometer and positioning information of a GPS;
respectively carrying out data fusion on the measurement data of each accelerometer and the measurement data of each gyroscope by utilizing a redundant information fusion algorithm according to the confidence coefficient between the measurement data of each accelerometer and the confidence coefficient between the measurement data of each gyroscope to obtain acceleration information and angular velocity information;
calculating by using an extended Kalman filtering algorithm to obtain a fusion attitude angle by taking the angular velocity information as a state variable and the acceleration information and the magnetic field information as observed quantities;
and calculating position information by using an extended Kalman filtering algorithm according to the acceleration information, the angular velocity information, the fusion attitude angle and the positioning information.
According to a second aspect, an embodiment provides a fusion sensor for attitude angle solution and localization, comprising: the system comprises at least two IMU sensors, a magnetometer, a GPS and a data processor, wherein one IMU sensor comprises an accelerometer and a gyroscope, and the at least two IMU sensors are connected in a double-sided reverse axis type mode or a single-axis multi-sensor mode;
the double-sided reverse axis type mode is that 2f IMU sensors are divided into two groups and are respectively arranged on two sides of a flat plate, one axis of the IMU sensors on the two sides is reverse, the other two axes are in the same direction, and f is an integer greater than or equal to 1;
the single-axis multi-sensor mode is that a plurality of IMU sensors are respectively arranged on each orthogonal axis, and each axial direction of each IMU sensor is the same;
the accelerometer is used for measuring the acceleration of the carrier;
the gyroscope is used for measuring the angular speed of the carrier;
the magnetometer is used for measuring magnetic field information;
the GPS is used for measuring positioning information;
the data processor is used for acquiring the measurement data of each accelerometer and each gyroscope, the magnetic field information of each magnetometer and the positioning information of the GPS, performing data fusion on the measurement data of each accelerometer and the measurement data of each gyroscope respectively by using a redundant information fusion algorithm according to the confidence coefficient between the measurement data of each accelerometer and the confidence coefficient between the measurement data of each gyroscope to obtain acceleration information and angular velocity information, calculating by using an extended Kalman filtering algorithm to obtain a fusion attitude angle by using the angular velocity information as a state quantity and the acceleration information and the magnetic field information as an observed quantity, and calculating the position information by using the extended Kalman filtering algorithm according to the acceleration information, the angular velocity information, the fusion attitude angle and the positioning information.
According to the attitude angle resolving and positioning method and the fusion sensor thereof of the embodiment, the acceleration information and the angular velocity information are calculated through the redundant information fusion algorithm of the IMU sensors, so that the calculated acceleration information and the calculated angular velocity information are more accurate, on the basis, the magnetometer is used as an assistant, and the acceleration information, the angular velocity information and the magnetic field information of the magnetometer are fused by using the extended Kalman filtering algorithm to obtain the fusion attitude angle, so that the precision and the stability of the system are greatly improved, and the fusion attitude angle with higher precision can be obtained under static and dynamic conditions; meanwhile, the final position information is obtained by fusing the positioning information of the attitude angle auxiliary GPS with higher precision, and the positioning information of the GPS is corrected, so that the obtained position information is more accurate; therefore, the attitude angle information and the position information with high precision and high stability can be obtained, and the stability and the reliability of attitude angle calculation and positioning are improved.
Drawings
FIG. 1 is a schematic structural diagram of a fusion sensor for attitude angle calculation and positioning according to an embodiment of the invention;
FIG. 2 is a schematic structural diagram of an IMU sensor connected in a double-sided, anti-pivot manner in an embodiment of the present invention;
FIG. 3 is a schematic structural diagram of an IMU sensor connected in a single-axis multi-sensor manner according to an embodiment of the present invention;
FIG. 4 is a flow chart of a method of attitude angle resolution and positioning in an embodiment of the invention;
FIG. 5 is a flowchart of a method for correcting a zero offset error of a gyroscope by using an Allan variance method according to an embodiment of the present invention;
FIG. 6 is a flowchart of a method for performing data fusion on measurement data of an accelerometer by using a redundant information fusion algorithm according to an embodiment of the present invention;
FIG. 7 is a flowchart of a method for calculating a fusion attitude angle according to an embodiment of the present invention;
FIG. 8 is a flowchart illustrating calculation of a fusion attitude angle using an extended Kalman filter recursion equation in an embodiment of the present invention;
FIG. 9 is a flowchart illustrating a method for calculating position information using an extended Kalman filter algorithm according to an embodiment of the present invention.
Detailed Description
The present invention will be described in further detail with reference to the following detailed description and accompanying drawings.
In the embodiment of the invention, a plurality of redundant (at least two) IMU sensors are adopted to measure the acceleration and the angular velocity of a carrier, and a redundant information fusion algorithm is utilized to respectively carry out data fusion on the measured data of each accelerometer and each gyroscope to obtain fused acceleration information and angular velocity information; then, taking the magnetometer as an assistant, and performing information fusion on the acceleration information, the angular velocity information and the magnetic field information measured by the magnetometer by using an extended Kalman filtering algorithm to obtain a fusion attitude angle; and then, the fused attitude angle is used for assisting positioning information measured by a GPS, and the position information is calculated by utilizing an extended Kalman filtering algorithm.
An embodiment of the present invention provides an attitude angle resolving and positioning fusion sensor, please refer to fig. 1, fig. 1 shows a schematic structural diagram of the attitude angle resolving and positioning fusion sensor, as shown in fig. 1, the attitude angle resolving and positioning fusion sensor includes N IMU sensors 1, one magnetometer 2, GPS 3 and a data processor 4, where N is an integer greater than or equal to 2; the IMU sensors 1 comprise an accelerometer 11 and a gyroscope 12, the accelerometer 11 is used for measuring the acceleration of a carrier, the gyroscope 12 is used for measuring the angular velocity of the carrier, and the N IMU sensors 1 are connected in a double-sided reverse axis type mode or a single-axis multi-sensor mode; the magnetometer 2 is used for measuring magnetic field information, the GPS 3 is used for measuring positioning information, and the data processor 4 is used for filtering, amplifying, calculating, analyzing and the like signals detected by the IMU sensor 1, the magnetometer 2 and the GPS 3; in this embodiment, the data processor 4 includes an obtaining module 41, a data fusion module 42 and a calculating module 43, where the obtaining module 41 is configured to obtain measurement data of each accelerometer 11 and each gyroscope 12, and magnetic field information of the magnetometer 2 and positioning information of the GPS 3; the data fusion module 42 is configured to perform data fusion on the measurement data of each accelerometer 11 by using a redundant information fusion algorithm according to the confidence degree between the measurement data of each accelerometer 11 to obtain acceleration information, and perform data fusion on the measurement data of each gyroscope 12 by using a redundant information fusion algorithm according to the confidence degree between the measurement data of each gyroscope 12 to obtain angular velocity information; the calculation module 43 is configured to calculate a fusion attitude angle by using the extended kalman filter algorithm with the angular velocity information as a state quantity and the acceleration information and the magnetic field information as observed quantities, and calculate the position information by using the extended kalman filter algorithm according to the fusion attitude angle, the acceleration information, the angular velocity information, and the positioning information. Preferably, the data processor 4 further includes a correction module, which is configured to perform error compensation on the magnetometer 2 and the accelerometer 11, and perform zero offset correction on the gyroscope 12, so that the magnetometer 2, the accelerometer 11, and the gyroscope 12 are not affected by errors of their own during data measurement, and the measured data is more accurate.
Specifically, fig. 2 shows a schematic structural diagram of the IMU sensors connected in a double-sided reverse axis type manner, as shown in fig. 2, N =2f identical IMU sensors form a redundant sensor system, where f is an integer greater than or equal to 1, the system is a flat plate type structure, N IMU sensors are divided into two groups, each group is f, and the N IMU sensors are respectively installed on two sides of a flat plate, one axes of the IMU sensors on the two sides are opposite in direction, and the two axes are identical in direction, for example, in fig. 2, the coordinate directions of all IMU sensors on the a side are identical, the coordinate directions of all IMU sensors on the B side are identical, and the IMU sensors respectively installed on the a side and the B side are opposite in direction of the z axis, and the directions of the x axis and the y axis are identical.
Fig. 3 shows a schematic structural view of IMU sensors connected by a single-axis multi-sensor approach, where N = a + b + c identical IMU sensors form a redundant sensor system, as shown in fig. 3, where N is an integer greater than or equal to 2; the N identical IMU sensors are divided into three groups of a, b and c, the number of the IMU sensors in each group can be set as required, the three groups of IMU sensors are respectively installed on three orthogonal axes of the redundant sensor system, that is, the three orthogonal axes X, Y and Z of the redundant sensor system are respectively installed with a, b and c IMU sensors, and each axial direction of the N IMU sensors is the same, for example, each axial direction is the same as the corresponding axial direction of the xyz coordinate system.
Based on the fusion sensor for attitude angle solution and positioning, fig. 4 shows a flowchart of a method for attitude angle solution and positioning in an embodiment of the present invention, and as shown in fig. 4, the method may include the following steps:
step S11: and (5) error compensation.
The correction module in the data processor 4 performs error compensation for the magnetometer 2 and each accelerometer 11. Specifically, the error compensation of the magnetometer 2 can be performed by a twelve-surface calibration method or an ellipsoid fitting method; the error compensation of the accelerometer 11 may be performed by using a kalman filter method, that is, a measurement equation using the acceleration error as a state variable is established, and the correction information is output through the optimal estimation of the kalman filter to compensate the error of the accelerometer 11.
Step S12: and (5) zero offset correction.
The correction module in the data processor 4 performs zero offset correction on each gyroscope 12 at the same time, specifically, the zero offset error of the gyroscope 12 can be corrected by an Allan variance method. In this embodiment, the gyroscope 12 may be a Micro Electro Mechanical Systems (MEMS) gyroscope, noise of the MEMS gyroscope may be divided into 5 independent parts, which are respectively quantization noise, angle random walk, bias instability, rate random walk and rate ramp, and the 5 independent noises are respectively characterized by a quantization noise coefficient Q, an angle random walk coefficient N, a bias instability coefficient B, a rate random walk coefficient K and a rate ramp coefficient R. Since these 5 noises are independent of each other, the following relation (1) exists:
wherein σ 2 (τ) is the total gyroscope noise, i.e., zero offset error,
in order to quantify the noise,
the angle is randomly walked away, and the angle is randomly walked,
in order to be free of bias instability,
in order for the rate to be random walk,
is a rate ramp.
From this, the mean square error of Allan can be defined as
Thus, the log-log curve of σ (τ) clearly describes the various error components and the different error terms of the gyroscope. Specifically, fig. 5 shows a method for correcting a zero offset error of a gyroscope by using an Allan variance method, and as shown in fig. 5, the method may include the following steps S121 to S127, specifically:
step S121: static drift data is collected.
When the Allan variance method is adopted to correct the zero offset error of the gyroscope, the sampling rate is 3-6 times of the bandwidth of the gyroscope, so that the accuracy of data processing can be ensured, and the sampling rate is set to be 500Hz because the bandwidth of the gyroscope is about 100 Hz. During data acquisition, the gyroscope is installed on a high-precision rotary table with a good vibration isolation effect, then the static drift data of the gyroscope is continuously acquired, and the acquisition time can be determined as required, such as continuous acquisition for 1 hour.
Step S122: the averaging processes the static drift data.
After the static drift data of the gyroscope is collected, the collected data can be averaged for 1s in order to improve the later operation speed, and the data in the preheating time period, such as 0.1h of preheating data, is removed, so that the static drift data to be processed is obtained.
Step S123: and (6) calculating an average value.
After the static drift data to be processed is obtained, grouping the static drift data to be processed according to the preprocessing parameters, and then solving the mean value of each group of static drift data to be processed, wherein the preprocessing parameters can comprise sampling periods, scale factors and the like.
Step S124: the mean square error of alan was calculated.
And after the mean value of the static drift data to be processed is obtained, the Allan variance is calculated according to the mean value, and then the Allan variance is subjected to an evolution operation to obtain the Allan mean square error.
Step S125: the log-log plot of mean square error of Allan was plotted.
Step S126: and drawing a fitting curve.
And after a double-logarithmic curve of the mean square error of the Allan is obtained, fitting the double-logarithmic curve, and drawing a fitting curve of the double-logarithmic curve.
Step S127: and solving the zero offset error of the gyroscope.
The error coefficients of the gyroscope, namely the quantization noise coefficient Q, the angle random walk coefficient N, the deviation instability coefficient B, the rate random walk coefficient K and the rate slope coefficient R are obtained from the drawn fitting curve of the log-log curve, and the error coefficients are obtained according to the error coefficients
And
thereby obtaining the total noise sigma of the gyroscope by calculation according to the relation (1) 2 And (tau), namely obtaining the zero offset error of the gyroscope.
Step S128: and correcting the zero offset error of the gyroscope.
And subtracting the obtained zero offset error of the gyroscope from the measurement data of the gyroscope, thereby correcting the zero offset error of the gyroscope and obtaining the corrected measurement data of the gyroscope.
In practical application, the processing of the static drift data of the gyroscope can also be performed in an MATLAB platform, and the zero offset error of the gyroscope is calculated through a written Allan variance data processing algorithm.
Step S13: measurement data is acquired.
The acquisition module 41 acquires measurement data of each accelerometer, measurement data of each gyroscope, magnetic field information measured by the magnetometer, and positioning information measured by the GPS.
Step S14: acceleration information and angular velocity information are calculated.
After the obtaining module 41 obtains the measurement data of each accelerometer, the measurement data of each gyroscope, the magnetic field information of the magnetometer, and the positioning information of the GPS, the data fusion module 42 performs data fusion on the measurement data of each accelerometer by using a redundant information fusion algorithm according to the confidence degrees between the measurement data of each accelerometer to obtain acceleration information, and performs data fusion on the measurement data of each gyroscope by using a redundant information fusion algorithm according to the confidence degrees between the measurement data of each gyroscope to obtain angular velocity information.
When the redundant information fusion algorithm is used for respectively fusing the measurement data of each accelerometer and the measurement data of each gyroscope, the advantage of membership function range determination in the fuzzy set theory is fully utilized, so that a fuzzy index confidence function is defined, the mutual confidence of each sensor in the sensors of the same type is quantized, the comprehensive confidence of the output data of each sensor is measured through a confidence matrix, the weight occupied by the measurement data of each sensor in the sensors of the same type in the data fusion process is reasonably distributed, the final expression of data fusion is obtained, the data fusion of the measurement data of multiple redundant sensors is realized, and the acceleration information and the angular velocity information are obtained.
Specifically, a plurality of same sensors are arranged to measure the same parameter, and the data measured by the ith sensor and the jth sensor are respectively c i And c j When c is i The higher the authenticity of c i The higher the degree of trust by the rest of the data. C is i Quilt c j The degree of trust, from c j To see c i This degree of confidence between the measured data of multiple redundant sensors is referred to as confidence, as the degree of likelihood of the true data. In order to further uniformly quantify the confidence level between the measured data of multiple redundant sensors, a confidence function k can be defined ij Is used to represent c i Quilt c j The degree of trust. At the same time, an upper limit value S is set, and S>0, when | c i ﹣c j If the value of | exceeds the upper limit value S, it can be considered that the two data are no longer mutually trusted, and a confidence function k can be obtained ij The expression of (c) is:
according to the expression (2), if k ij =0, then the i-th sensing is consideredThe device and the jth sensor are mutually untrusted; if k is ij If =1, the ith sensor is deemed to trust the jth sensor. When one sensor is not trusted by other sensors or is trusted by only a few sensors, the measurement data of the sensor is removed during data fusion, so that abnormal data can be automatically removed during data fusion of the measurement data of each sensor, and the measurement data participating in the data fusion is reliable data.
Therefore, data processing is carried out on the sensors through a redundant information fusion algorithm, the dimension of the target feature vector is increased, the whole measuring system obtains independent feature information which cannot be obtained by any single sensor, the uncertainty and the limitation of the single sensor are overcome, and the performance of the system is obviously improved.
On the basis of this theory, fig. 6 shows a flowchart of a method for performing data fusion on the measurement data of the N accelerometers by using a redundant information fusion algorithm, and as shown in fig. 6, the method may include steps S141 to S143, specifically:
step S141: and establishing a confidence coefficient matrix K.
N accelerometers measure the acceleration of the carrier simultaneously, a confidence matrix K is established according to a confidence function between measurement data of each accelerometer, and the expression of K is as follows:
wherein N = N, k ij And representing the confidence degree function between the measured data of each accelerometer and the confidence degree of the ith accelerometer trusted by the jth accelerometer, wherein i and j are integers.
Step 142: a weight matrix E is calculated.
With E i Measurement data a representing the ith accelerometer i Weight taken up during data fusion, E i Is a size reflecting the measurement data of other accelerometers to the measurement data a of the ith accelerometer i The overall degree of trust. In the confidence matrix K, setBelief function k ij Representing only the measurement data a j To a i Does not reflect all the measured data pairs a of the N accelerometers i Degree of trust of, and a i Should actually be represented by k i1 ，k i2 ，…，k in The measured data of other accelerometers to the measured data a of the ith accelerometer i The confidence level of (c) may be expressed as:
E i ＝m 1 k i1 +m 2 k i2 +…+m n k in (3)
wherein m is 1 ，m 2 ，…，m n Is a set of non-negative numbers, which can be expressed in matrix form as M = [ M ] 1 ,m 2 ,…,m n ] T Similarly, E i Expressed in matrix form E = [ ] 1 ,e 2 ,…,e n ] T Thus, the formula (3) can be expressed in a matrix form as:
E＝KM
and because of k ij Is more than or equal to 0, so the confidence matrix K is a non-negative matrix, and the symmetric matrix K has the maximum module characteristic value lambda (lambda)>0) And the eigenvector corresponding to the eigenvalue is the solution of M, so that a weight matrix E can be obtained through the confidence matrix K and the non-negative vector M.
Step S143: acceleration information is calculated.
When a weight matrix E = [ E ] is obtained 1 ,e 2 ,…,e n ] T And then, according to a data fusion formula (4), carrying out weighted summation on the measurement data of the accelerometer by using a weight matrix E, thereby obtaining acceleration information a, wherein the data fusion formula (4) is as follows:
similarly, the process of carrying out data fusion on the measurement data of the N gyroscopes by using the redundant information fusion algorithm and the measurement of the N accelerometers by using the redundant information fusion algorithmThe data fusion process is the same, namely a confidence matrix is established according to a confidence function between the measured data of each gyroscope
Wherein N = N, k' ij Representing the trust degree of the ith gyroscope by the jth gyroscope as a confidence function between the measurement data of each gyroscope, wherein i and j are integers; and calculating a weight matrix E ' = [ E ' according to a matrix multiplication formula E ' = K ' M ' 1 ，e′ 2 …e′ i …，e′ n ] T Wherein M '= [ M' 1 ，m′ 2 …m′ j …，m′ n ] T The characteristic vector corresponding to the maximum modulus characteristic value of the confidence coefficient matrix K 'is obtained, and M' is a non-negative vector; then according to a data fusion formula
Weighting and summing the measurement data of the gyroscope by using a weight matrix E' to obtain angular velocity information omega, wherein omega i Is measured data of the ith gyroscope, e' i Represents omega i The weight occupied in the data fusion.
Step S15: and calculating a fusion attitude angle.
After obtaining the acceleration information and the angular velocity information, the calculation module 43 calculates a fusion attitude angle by using the angular velocity information as a state quantity and using the acceleration information and the magnetic field information measured by the magnetometer as an observed quantity, specifically, as shown in fig. 7, the process of calculating the fusion attitude angle may include steps S151 to S155, specifically:
step S151: an initial attitude angle is calculated.
Before the fused attitude angle is calculated by using the extended Kalman filtering algorithm, the attitude angle must be obtained under the condition of no filtering method, namely, the initial attitude angle is directly calculated on the basis of the acceleration information measured by the accelerometer and the magnetic field information measured by the magnetometer.
Specifically, the attitude angle solution directly performed by the three-axis accelerometer and the magnetometer is performed in a carrier coordinate system, so that the attitude quantity calculated by the accelerometer and the magnetometer needs to be converted into a navigation coordinate system; defining the carrier coordinate system as b and the navigation coordinate system as d, the transformation is realized by a transformation matrix from b to d, which is rotated by taking the sequence of the X-Y-Z axes as a standard, and the obtained transformation matrix can be expressed as:
wherein the content of the first and second substances,
theta and psi represent pitch angle, roll angle and yaw angle respectively,
the resulting transformation matrix.
When the carrier is in a static or uniform linear motion state, it has a slight acceleration in addition to the acceleration of gravity, and the transformation matrix can be calculated from the acceleration information of the accelerometer and the magnetic field information of the magnetometer
The calculation formula is as follows:
wherein g is the acceleration of gravity, a x 、a y And a z Acceleration information measured for the accelerometers represents the three-axis acceleration of the accelerometers, respectively.
Calculated by the formula (6)
The initial pitch angle can be obtained by combining the formula (5)
And initial roll angle θ 0 The calculation formulas of (A) and (B) are respectively as follows:
θ 0 ＝arctan(a x /g)
further, according to the calculated
θ 0 And the magnetic field information measured by the magnetometer, and calculating the initial yaw angle psi by using a formula I and a formula II 0 The first formula is as follows:
wherein m is x 、m y And m z The magnetic field information measured by the magnetometer respectively represents the magnetic field values of the three axes of the magnetometer.
The second formula is: psi 0 ＝arctan(H y /H x )。
From this, the initial attitude angle can be calculated
θ 0 And Ψ 0 。
Step S152: and calculating four elements of the initial posture.
According to the calculated initial attitude angle
θ 0 And Ψ 0 Calculating to obtain an attitude four-element q at an initial moment by using a corresponding relation between an attitude angle and four elements, wherein q = q is defined 0 +q 1 i+q 2 j+q 3 k，q 0 、q 1 、q 2 And q is 3 Represents four elements.
In the solution of attitude anglesIn order to avoid the problem of euler angle singularity, the solution of the attitude angle is usually completed by using quaternion, and four elements q = q are rotated according to a unit 0 +q 1 i+q 2 j+q 3 k, the transformation matrix from b to d can be expressed as:
the corresponding relation between the attitude angle and the four elements can be obtained by corresponding the formula with the formula (5), and the four elements q are obtained by calculating the corresponding relation 0 、q 1 、q 2 And q is 3 And further obtaining the posture four-element q at the initial moment.
Step S153: and updating the posture four elements.
When the attitude four-element is used for resolving the attitude angle, the attitude four-element needs to be continuously updated, and the attitude four-element can be updated through a quaternion differential equation based on gyroscope data. Specifically, an angular velocity matrix W may be established according to angular velocity information measured by a gyroscope, where an expression of W is:
wherein, ω is x 、ω y And omega z The angular velocity information of the gyroscope represents the angular velocities of the three axes of the gyroscope, respectively.
Then, using the update equation
And updating the posture four-element to obtain an updated posture four-element q'.
Step S154: and establishing an observation equation.
Taking a continuously updated posture four-element q' as a state variable X, taking acceleration information measured by a redundant accelerometer and magnetic field information measured by a magnetometer as an observed quantity Z, and establishing an observation equation, wherein the established observation equation is as follows:
Z＝H(X)+V
wherein the observed quantity Z = [ a ] x a y a z Ψ] T ，
g is the acceleration of gravity, a x 、a y And a z The acceleration information represents the three-axis acceleration of the accelerometer, psi is the heading angle calculated according to the magnetic field information, and V is the measurement noise.
Discretizing the observation equation to obtain a discretized observation equation:
Z(k)＝H(k)X(k)+V(k)
wherein X (k) is a state value at a time k, V (k) is a measurement noise at the time k,
the jacobian matrix is H (X), and represents H (X (k)) at time k to calculate a partial derivative of X (k).
Step S155: and calculating a fusion attitude angle.
Specifically, fig. 8 shows a flowchart for calculating the fusion attitude angle by using the extended kalman filter recursion equation, and as shown in fig. 8, the method may include the following steps:
step SA1: and predicting the state in one step.
According to the formula
Performing state prediction to obtain a state prediction value at the time k
F (k-1) is a state transition matrix at the moment k-1, X (k-1) is a state value at the moment k-1, and U (k-1) is state noise at the moment k-1. Wherein, the state transition matrix at the time k can be expressed as:
wherein, ω is x (k)、ω y (k) And omega z (k) Is the angular velocity information of the gyroscope at the moment k.
Step SA2: and (4) error covariance prediction.
According to the formula
Estimating the error covariance to obtain the predicted value of the error covariance matrix
Wherein P (k-1) is an error covariance matrix at the time k-1, F T (k-1) is the transpose of F (k-1), and Q (k-1) is the system noise covariance matrix at time k-1.
Step SA3: a filter gain matrix is calculated.
According to the formula
Computing a filter gain matrix K (K), where H T (k) For the transpose of H (k), R (k) is the measurement noise covariance at time k.
Step SA4: and calculating a fusion attitude angle.
Firstly according to the formula
And calculating a state value X (k) at the moment k to obtain the attitude four elements at the moment k, and then calculating to obtain a fusion attitude angle at the moment k according to the corresponding relation between the attitude four elements and the attitude angle.
Step SA5: the error covariance is updated.
According to the formula
And updating the error covariance to obtain an error covariance matrix at the moment K for the update calculation of the next moment, wherein A = I-K (K) H (K), and I is an identity matrix.
Therefore, the fusion attitude angle can be continuously updated under the dynamic condition to obtain the fusion attitude angle under the dynamic condition.
Step S16: and fusing the attitude angles to assist in calculating the position information.
The calculation module 43 calculates the position information by using the extended kalman filter algorithm according to the calculated fusion attitude angle, acceleration information, angular velocity information, and positioning information measured by the GPS. Specifically, fig. 9 shows a process of calculating the position information, and as shown in fig. 9, the process may include steps S161 to S165 as follows:
step S161: a state variable is determined.
The calculation module 43 first obtains three axial velocity components υ from the acceleration information x 、υ y And upsilon z Then, the positioning information of GPS is obtained according to the velocity component
And
and parameter errors of the fusion sensor in three axial directions
And
establishing a state variable X', of which
And
respectively representing longitude, latitude and altitude, and establishing state variables of
Step S162: and establishing a state transition matrix.
The calculation module 43 acquires the speed components in the three axial directionsAfter measurement, a state transition matrix is also established
Wherein F N For the system matrix containing velocity components, fused attitude angles and positioning information, F S Is a unit matrix, F M Is a matrix consisting of the output frequencies of the gyroscopes and accelerometers.
Step S163: and calculating carrier positioning information.
The calculation module 43 calculates carrier positioning information according to the acceleration information and the angular velocity information obtained after data fusion. Specifically, the calculation module 43 integrates the acceleration information to obtain the carrier movement distance, and performs double integration on the angular velocity information to obtain the carrier movement direction, so that the calculation module 43 can obtain the carrier positioning information L according to the carrier movement distance and the carrier movement direction t 、λ t And h t And obtaining the longitude, latitude and height of the carrier.
Step S164: and establishing an observation equation.
In calculating carrier positioning information L t 、λ t And h t Then, the calculating module 43 first calculates the carrier positioning information and the positioning information measured by the GPS according to the calculated carrier positioning information and the calculated positioning information
And
using a formula
Establishing a position observation equation Z' p (t) wherein R M Is the radius of curvature of the ellipsoid meridian, R N The radius of curvature of the prime circle is the Mao-unitary meridian radius of curvature; then, the velocity information upsilon of the carrier is obtained through the GPS Gx 、υ Gy And upsilon Gz And then according to the velocity component v x 、υ y And upsilon z Velocity information v with carrier Gx 、υ Gy And upsilon Gz Difference between them establishes speedObservation equation Z' v (t), velocity Observation equation Z 'established' v (t) is
Observing equation Z 'at established good position' p (t) and speed Observation equation Z' v (t) thereafter, Z' p (t) and Z' v (t) taken together, the overall observation equation Z' (t) is obtained, specifically:
where X ' (t) is a state equation corresponding to the state quantity X ', and V ' (t) is measurement noise.
Discretizing the observation equation Z ' (t) to obtain a discretized observation equation Z ' (k), wherein Z ' (k) can be expressed as:
Z′(k)＝H′(k)X′(k)+V′(k)
where H ' (k) is a jacobian matrix, and X ' (k) and V ' (k) are a state value and measurement noise at the time k, respectively.
Step S165: position information is calculated.
And calculating by using an extended Kalman filtering recursion equation according to the state variable X ', the state transition matrix F ' and the discretization observation equation Z ' (k) to obtain the position information. Specifically, when the fused attitude angle is calculated by using the extended kalman filter recursion equation, the process is the same as the process of calculating the fused attitude angle by using the extended kalman filter recursion equation, which is described in step SA1 to step SA5 and is not described herein again.
Therefore, the position information can be continuously updated under the dynamic condition to obtain the position information under the dynamic condition. In addition, in the process of calculating the position information, the positioning information measured by the GPS is corrected by the calculated high-precision and high-stability fused attitude angle, so that the finally obtained position information also has high precision and high stability.
According to the attitude angle resolving and positioning method and the fusion sensor thereof provided by the embodiment of the invention, a plurality of IMU sensors are adopted for measuring the acceleration and the angular velocity, and the dimension of the target characteristic vector is increased, so that the whole measuring system obtains independent characteristic information which cannot be obtained by any single sensor. Further, in the calculation process of the attitude angle, firstly, error compensation is carried out on the magnetometer and each accelerometer, and zero offset correction is carried out on each gyroscope, so that the influence of the self errors of the magnetometer, the accelerometer and the gyroscope on the measured data is avoided; the measurement data of a plurality of IMU sensors are coordinated, complemented and combined through a redundant information fusion algorithm, and the final acceleration information and angular velocity information are obtained through fusion, so that abnormal data in measurement are automatically eliminated, the uncertainty and the limitation of a single sensor are overcome, and the accuracy and the stability of attitude angle calculation are greatly improved; then, the acceleration information, the angular velocity information and the magnetic field information obtained by the magnetometer are fused by using an extended Kalman filtering algorithm, the traditional complementary filtering fusion algorithm and a gradient descent method are abandoned, the calculation complexity is relatively low, the high-precision attitude angle can be obtained under the static and dynamic conditions, and the error of the attitude angle is greatly reduced; and finally, calculating the obtained positioning information of the fusion attitude angle assisted GPS to obtain final position information, and compensating the positioning information of the GPS, so that the obtained position information is more accurate and has better stability. Therefore, the attitude angle calculation and positioning method and the fusion sensor thereof provided by the embodiment of the invention can obtain high-precision and high-stability attitude angle information and position information in the attitude angle calculation process, and improve the stability and reliability of the attitude angle calculation and positioning.
Those skilled in the art will appreciate that all or part of the functions of the various methods in the above embodiments may be implemented by hardware, or may be implemented by computer programs. When all or part of the functions of the above embodiments are implemented by a computer program, the program may be stored in a computer-readable storage medium, and the storage medium may include: a read only memory, a random access memory, a magnetic disk, an optical disk, a hard disk, etc., and the program is executed by a computer to realize the above functions. For example, the program may be stored in a memory of the device, and when the program in the memory is executed by the processor, all or part of the functions described above may be implemented. In addition, when all or part of the functions in the above embodiments are implemented by a computer program, the program may be stored in a storage medium such as a server, another computer, a magnetic disk, an optical disk, a flash disk, or a portable hard disk, and may be downloaded or copied to a memory of a local device, or may be version-updated in a system of the local device, and when the program in the memory is executed by a processor, all or part of the functions in the above embodiments may be implemented.
The present invention has been described in terms of specific examples, which are provided to aid understanding of the invention and are not intended to be limiting. For a person skilled in the art to which the invention pertains, several simple deductions, modifications or substitutions may be made according to the idea of the invention.
Claims (9)
1. A method for attitude angle solution and positioning is applied to a fusion sensor for attitude angle solution and positioning, the fusion sensor comprises at least two IMU sensors, a magnetometer and a GPS, one IMU sensor comprises an accelerometer and a gyroscope, and the method comprises the following steps:
acquiring measurement data of each accelerometer and each gyroscope, magnetic field information of each magnetometer and positioning information of a GPS;
respectively carrying out data fusion on the measurement data of each accelerometer and the measurement data of each gyroscope by utilizing a redundant information fusion algorithm according to the confidence coefficient between the measurement data of each accelerometer and the confidence coefficient between the measurement data of each gyroscope to obtain acceleration information and angular velocity information;
calculating by using an extended Kalman filtering algorithm to obtain a fusion attitude angle by taking the angular velocity information as a state quantity and the acceleration information and the magnetic field information as observed quantities;
calculating position information by using an extended Kalman filtering algorithm according to the acceleration information, the angular velocity information, the fusion attitude angle and the positioning information;
the redundant information fusion algorithm comprises the following steps:
calculating a weight matrix E = [ E ] according to a matrix multiplication formula E = KM 1 ，e 2 ···e i ···，e n ] T Wherein, the
For a confidence matrix, k, built from a confidence function between the measured data ij Representing the degree of confidence of the ith measurement data by the jth measurement data as a confidence function between the measurement data, n representing the number of the measurement data, i and j being integers, wherein M = [ M ] 1 ，m 2 ···m j ···，m n ] T The characteristic vector corresponding to the maximum modulus characteristic value of the confidence coefficient matrix K is obtained, and M is a non-negative vector;
according to a data fusion formula, carrying out weighted summation on the measurement data by using a weight matrix E to obtain fusion data a;
the data fusion formula is as follows:
wherein, a i For the ith measurement data, e i Represents a i The weight occupied in the data fusion;
the confidence function k ij Obtained by the following formula:
wherein, c i For the data measured by the i-th sensor, c j For the data measured by the jth sensor, S is a set upper limit value, and S>0。
2. The method of claim 1, wherein prior to acquiring measurement data for each accelerometer and each gyroscope and magnetic field information for a magnetometer and positioning information for a GPS, the method further comprises:
error compensation is performed on the magnetometer and the accelerometer, and zero offset correction is performed on the gyroscope.
3. The method of claim 2, wherein the zero bias correction of the gyroscope comprises:
collecting static drift data of a gyroscope;
carrying out 1s averaging processing on the static drift data, and eliminating data in a preheating time period to obtain static drift data to be processed;
grouping the static drift data to be processed according to preprocessing parameters, and solving the average value of each group of static drift data to be processed, wherein the preprocessing parameters comprise sampling periods and scale factors;
calculating an Allan variance according to the mean value;
calculating an Allan mean square error according to the Allan variance;
drawing a log-log curve of the mean square error of the Allan;
fitting the double logarithmic curve to obtain a fitted curve;
solving an error coefficient of the gyroscope according to the fitting curve, wherein the error coefficient comprises a quantization noise coefficient, an angle random walk coefficient, a deviation instability coefficient, a rate random walk coefficient and a rate slope coefficient;
calculating the total noise of the gyroscope according to the error coefficient of the gyroscope to obtain the zero offset error of the gyroscope;
the zero offset error is subtracted from the gyroscope measurement data to correct the gyroscope zero offset error.
4. The method of claim 1, wherein the calculating a fusion attitude angle using an extended kalman filter algorithm with the angular velocity information as a state quantity and the acceleration information and the magnetic field information as observed quantities comprises:
calculating an initial attitude angle according to the acceleration information and the magnetic field information;
according to the initial attitude angle, calculating to obtain an attitude four-element q at the initial moment by utilizing the corresponding relation between the attitude angle and the four elements, wherein q = q 0 +q 1 i+q 2 j+q 3 k，q 0 、q 1 、q 2 And q is 3 Represents four elements;
establishing an angular velocity matrix W according to the angular velocity information;
using an update equation based on the angular velocity matrix W
Updating the posture four-element to obtain an updated posture four-element q';
establishing an observation equation by taking q' as a state variable X and acceleration information and magnetic field information as an observed quantity Z;
discretizing the observation equation to obtain a discretized observation equation Z (k);
calculating by using an extended Kalman filtering recursion equation according to the X and the Z (k) to obtain a fusion attitude angle;
the angular velocity matrix W is
Wherein, ω is x 、ω y And ω z Representing the angular speeds of three axes of a gyroscope respectively for the angular speed information;
the observation equation is Z = H (X) + V, where the observed quantity Z = [ a = [/] x a y a z Ψ] T ，
g is the acceleration of gravity, a x 、a y And a z The acceleration information represents three-axis acceleration of the accelerometer, and psi is a heading angle calculated according to the magnetic field informationV is measurement noise;
the discretized observation equation is Z (k) = H (k) X (k) + V (k), where X (k) is the state value at time k, V (k) is the measurement noise at time k,
the jacobian matrix is H (X), and represents H (X (k)) at time k to calculate a partial derivative of X (k).
5. The method of claim 4, wherein said calculating an initial attitude angle from said acceleration information and said magnetic field information comprises:
according to
And theta 0 ＝arctan(a x /g) calculating initial pitch angles respectively
And initial roll angle θ 0 ；
According to the above
θ 0 And magnetic field information, and calculating the initial yaw angle psi by using a formula I and a formula II 0 ；
The first formula is as follows:
wherein m is x 、m y And m z Representing the magnetic field values of the three axes of the magnetometer respectively for the magnetic field information;
the second formula is: Ψ 0 ＝arctan(H y /H x )。
6. The method of claim 4, wherein said calculating a fused attitude angle from said X and said Z (k) using an extended Kalman Filter recursion equation comprises:
according to the formula
Performing state prediction to obtain state prediction value at k moment
F (k-1) is a state transition matrix at the moment of k-1, X (k-1) is a state value at the moment of k-1, and U (k-1) is state noise at the moment of k-1;
according to the formula
Estimating the error covariance to obtain the predicted value of the error covariance matrix
Wherein P (k-1) is an error covariance matrix at the time k-1, F T (k-1) is the transpose of F (k-1), and Q (k-1) is the covariance matrix of the system noise at the time of k-1;
according to the formula
Computing a filter gain matrix K (K), where H T (k) Is the transpose of H (k), and R (k) is the measurement noise covariance at time k;
according to the formula
Calculating a state value X (k) at the moment k to obtain four attitude elements at the moment k;
calculating to obtain a fusion attitude angle at the k moment according to the corresponding relation between the four attitude elements and the attitude angle;
according to the formula
Updating the error covariance to obtain an error covariance matrix at the moment K, wherein A = I-K (K) H (K), and I is an identity matrix。
7. The method of claim 1, wherein calculating position information from the acceleration information, the angular velocity information, the fused attitude angle, and the positioning information using an extended kalman filter algorithm comprises:
acquiring a velocity component in the acceleration information, and taking the velocity component, the positioning information and a parameter error of a fusion sensor as a state variable X';
establishing a state transition matrix F' according to a formula I;
calculating carrier positioning information according to the acceleration information and the angular velocity information;
according to the carrier positioning information and the positioning information of the GPS, a position observation equation Z 'is established by using a formula II' p (t)；
Acquiring speed information of a carrier through a GPS (global positioning system), and establishing a speed observation equation Z 'according to a difference value of the speed component and the speed information' v (t)；
According to Z' p (t) and Z' v (t), obtaining an observation equation Z' (t) by using a formula III;
discretizing the Z '(t) to obtain a discretized observation equation Z' (k);
calculating to obtain position information by utilizing an extended Kalman filtering recursion equation according to the state variable X ', the state transition matrix F ' and the discretized observation equation Z ' (k);
the described
Wherein upsilon is x 、υ y And upsilon z For the purpose of said velocity component,
and
for the positioningInformation, representing longitude, latitude and altitude, respectively,
and
is the parameter error of the fusion sensor;
the first formula is as follows:
wherein, F N For the system matrix containing the velocity component, the fusion attitude angle, and the positioning information, F S Is an identity matrix, said F M Is a matrix composed of the output frequencies of the gyroscope and accelerometer;
the second formula is:
wherein L is t 、λ t And h t For locating information for said carrier, R M Is the radius of curvature of the ellipsoid meridian, R N The radius of curvature of the prime meridian unitary, and L is the initial latitude of the carrier;
the third formula is:
where V' (t) is measurement noise;
z ' (k) = H ' (k) X ' (k) + V ' (k), where H ' (k) is a jacobian matrix, and X ' (k) and V ' (k) are a state value and a measurement noise at time k, respectively.
8. The method of claim 7, wherein said calculating carrier positioning information from said acceleration information and said angular velocity information comprises:
integrating the acceleration information to obtain a carrier movement distance;
carrying out double integration on the angular speed information to obtain the motion direction of the carrier;
and obtaining carrier positioning information according to the carrier movement distance and the carrier movement direction.
9. An attitude angle resolving and positioning fusion sensor, comprising: the system comprises at least two IMU sensors, a magnetometer, a GPS and a data processor, wherein one IMU sensor comprises an accelerometer and a gyroscope, and the at least two IMU sensors are connected in a double-sided reverse axis type mode or a single-axis multi-sensor mode;
the double-sided reverse axis type mode is that 2f IMU sensors are divided into two groups and are respectively arranged on two sides of a flat plate, one axis of the IMU sensors on the two sides is reverse, the other two axes are in the same direction, and f is an integer greater than or equal to 1;
the single-shaft multi-sensor mode is that a plurality of IMU sensors are respectively arranged on each orthogonal shaft, and the axial directions of the IMU sensors are the same;
the accelerometer is used for measuring the acceleration of the carrier;
the gyroscope is used for measuring the angular speed of the carrier;
the magnetometer is used for measuring magnetic field information;
the GPS is used for measuring positioning information;
the data processor is used for acquiring the measurement data of each accelerometer and each gyroscope, the magnetic field information of each magnetometer and the positioning information of the GPS, respectively carrying out data fusion on the measurement data of each accelerometer and the measurement data of each gyroscope by using a redundant information fusion algorithm according to the confidence degree between the measurement data of each accelerometer and the confidence degree between the measurement data of each gyroscope to obtain acceleration information and angular velocity information, calculating by using an extended Kalman filtering algorithm to obtain a fusion attitude angle by using the angular velocity information as a state quantity and the acceleration information and the magnetic field information as observed quantities, and calculating the position information by using the extended Kalman filtering algorithm according to the acceleration information, the angular velocity information, the fusion attitude angle and the positioning information;
the redundant information fusion algorithm comprises the following steps:
calculating a weight matrix E = [ E ] according to a matrix multiplication formula E = KM 1 ，e 2 ···e i ···，e n ] T Wherein, the
For a confidence matrix, k, built from a confidence function between the measured data ij Representing the degree of confidence of the ith measurement data by the jth measurement data as a confidence function between the measurement data, n representing the number of the measurement data, i and j being integers, wherein M = [ M ] 1 ，m 2 ···m j ···，m n ] T The characteristic vector corresponding to the maximum modulus characteristic value of the confidence coefficient matrix K is obtained, and M is a non-negative vector;
according to a data fusion formula, carrying out weighted summation on the measurement data by using a weight matrix E to obtain fusion data a;
the data fusion formula is as follows:
wherein, a i For the ith measurement data, e i Represents a i The weight occupied in the data fusion;
the confidence function k ij Obtained by the following formula:
wherein, c i For the data measured by the i-th sensor, c j For the data measured by the jth sensor, S is a set upper limit value, and S>0。
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201810517173.9A CN109001787B (en) | 2018-05-25 | 2018-05-25 | Attitude angle resolving and positioning method and fusion sensor thereof |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201810517173.9A CN109001787B (en) | 2018-05-25 | 2018-05-25 | Attitude angle resolving and positioning method and fusion sensor thereof |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN109001787A CN109001787A (en) | 2018-12-14 |
| CN109001787B true CN109001787B (en) | 2022-10-21 |
Family
ID=64573366
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN201810517173.9A Active CN109001787B (en) | 2018-05-25 | 2018-05-25 | Attitude angle resolving and positioning method and fusion sensor thereof |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN109001787B (en) |
Families Citing this family (48)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN109596090B (en) * | 2019-01-07 | 2021-10-22 | 武汉虹信科技发展有限责任公司 | Individual posture measuring method and device |
| CN109827545B (en) * | 2019-03-22 | 2020-12-29 | 北京壹氢科技有限公司 | Online inclination angle measuring method based on double MEMS accelerometers |
| CN111897412B (en) * | 2019-05-05 | 2022-02-11 | 清华大学 | Motion capture device |
| CN110440805B (en) * | 2019-08-09 | 2021-09-21 | 深圳市道通智能航空技术股份有限公司 | Method and device for fusing yaw angles and aircraft |
| CN110377058B (en) * | 2019-08-30 | 2021-11-09 | 深圳市道通智能航空技术股份有限公司 | Aircraft yaw angle correction method and device and aircraft |
| CN110471123B (en) * | 2019-09-02 | 2020-12-18 | 临沂大学 | Data diagnosis and processing method for gravity gradiometer of rotating accelerometer |
| CN110674888B (en) * | 2019-10-11 | 2022-04-05 | 中国人民解放军海军航空大学青岛校区 | Head posture recognition method based on data fusion |
| CN111076722B (en) * | 2019-11-18 | 2022-07-19 | 广州南方卫星导航仪器有限公司 | Attitude estimation method and device based on self-adaptive quaternion |
| CN110792430B (en) * | 2019-11-20 | 2021-08-17 | 中国地质大学（北京） | While-drilling inclination measurement method and device based on multi-sensor data fusion |
| CN111007863B (en) * | 2019-12-06 | 2023-05-02 | 广州市申迪计算机系统有限公司 | Robot course angle measuring method, device and storage medium |
| CN111177669A (en) * | 2019-12-11 | 2020-05-19 | 宇龙计算机通信科技(深圳)有限公司 | Terminal identification method and device, terminal and storage medium |
| CN111551174A (en) * | 2019-12-18 | 2020-08-18 | 无锡北微传感科技有限公司 | High-dynamic vehicle attitude calculation method and system based on multi-sensor inertial navigation system |
| CN110954102B (en) * | 2019-12-18 | 2022-01-07 | 无锡北微传感科技有限公司 | Magnetometer-assisted inertial navigation system and method for robot positioning |
| CN111024122B (en) * | 2019-12-18 | 2024-01-19 | 上海聿毫信息科技有限公司 | Ultrasonic pen inclination deviation compensation method based on Bluetooth and nine-axis gyroscope |
| CN111895988A (en) * | 2019-12-20 | 2020-11-06 | 北京空天技术研究所 | Unmanned aerial vehicle navigation information updating method and device |
| CN110987267B (en) * | 2019-12-23 | 2022-04-22 | 佳讯飞鸿（北京）智能科技研究院有限公司 | Point switch stress signal detection device and method and electronic equipment |
| CN110879066A (en) * | 2019-12-26 | 2020-03-13 | 河北美泰电子科技有限公司 | Attitude calculation algorithm and device and vehicle-mounted inertial navigation system |
| CN111189473A (en) * | 2020-01-08 | 2020-05-22 | 湖北三江航天红峰控制有限公司 | Heading and attitude system gyro error compensation method based on magnetic sensor and additional meter |
| CN111399005A (en) * | 2020-03-09 | 2020-07-10 | 展讯通信（上海）有限公司 | Attitude determination positioning method, system, medium and electronic device for mobile terminal |
| CN111338320B (en) * | 2020-03-11 | 2023-03-28 | 西安应用光学研究所 | Stabilized platform fault protection system and method |
| CN113514049A (en) * | 2020-04-10 | 2021-10-19 | 北京三快在线科技有限公司 | Unmanned aerial vehicle attitude measurement method and device, unmanned aerial vehicle and storage medium |
| CN111337018B (en) * | 2020-05-21 | 2020-09-01 | 上海高仙自动化科技发展有限公司 | Positioning method and device, intelligent robot and computer readable storage medium |
| CN113739817B (en) * | 2020-05-29 | 2023-09-26 | 上海华依智造动力技术有限公司 | Online automatic debugging method for signal fusion algorithm parameters of automobile integrated navigation equipment |
| CN111709517B (en) * | 2020-06-12 | 2022-07-29 | 武汉中海庭数据技术有限公司 | Method and device for enhancing redundancy fusion positioning based on confidence prediction system |
| CN111966130B (en) * | 2020-06-28 | 2023-06-27 | 上海伊涵家居饰品有限公司 | Automatic seat homing control method, system and storage medium thereof |
| CN111947658B (en) * | 2020-06-30 | 2022-12-27 | 北京航天控制仪器研究所 | Low-cost autonomous navigation device and navigation method for communication-assisted positioning |
| CN114074693B (en) * | 2020-08-13 | 2023-05-05 | 比亚迪股份有限公司 | Multi-sensor fusion train positioning method, device, positioning system and train |
| CN112346479B (en) * | 2020-11-18 | 2023-08-22 | 大连海事大学 | Unmanned aircraft state estimation method based on centralized Kalman filtering |
| CN112630813B (en) * | 2020-11-24 | 2024-05-03 | 中国人民解放军国防科技大学 | Unmanned aerial vehicle attitude measurement method based on strapdown inertial navigation and Beidou satellite navigation system |
| CN113405553B (en) * | 2020-11-30 | 2023-05-26 | 辽宁工业大学 | Unmanned special vehicle attitude measurement method |
| CN112629538B (en) * | 2020-12-11 | 2023-02-14 | 哈尔滨工程大学 | Ship horizontal attitude measurement method based on fusion complementary filtering and Kalman filtering |
| CN112945225A (en) * | 2021-01-19 | 2021-06-11 | 西安理工大学 | Attitude calculation system and method based on extended Kalman filtering |
| CN114789439B (en) * | 2021-01-26 | 2024-03-19 | 深圳市普渡科技有限公司 | Slope positioning correction method, device, robot and readable storage medium |
| CN112923924B (en) * | 2021-02-01 | 2023-06-30 | 杭州电子科技大学 | Method and system for monitoring posture and position of anchoring ship |
| CN113063416B (en) * | 2021-02-05 | 2023-08-08 | 重庆大学 | Robot posture fusion method based on self-adaptive parameter complementary filtering |
| CN113029127B (en) * | 2021-03-10 | 2023-05-09 | 中国人民解放军海军航空大学 | Aircraft autonomous attitude estimation method based on distributed circulating structure |
| CN113229804A (en) * | 2021-05-07 | 2021-08-10 | 陕西福音假肢有限责任公司 | Magnetic field data fusion circuit and method for joint mobility |
| CN113108790B (en) * | 2021-05-14 | 2024-06-25 | 深圳中智永浩机器人有限公司 | Robot IMU angle measurement method and device, computer equipment and storage medium |
| CN114252073B (en) * | 2021-11-25 | 2023-09-15 | 江苏集萃智能制造技术研究所有限公司 | Robot attitude data fusion method |
| CN113936044B (en) * | 2021-12-17 | 2022-03-18 | 武汉锐科光纤激光技术股份有限公司 | Method and device for detecting motion state of laser equipment, computer equipment and medium |
| CN114217628A (en) * | 2021-12-24 | 2022-03-22 | 北京理工大学重庆创新中心 | Double-path IMU unit unmanned aerial vehicle controller based on 5G communication and control method |
| CN114279311A (en) * | 2021-12-27 | 2022-04-05 | 深圳供电局有限公司 | GNSS deformation monitoring method and system based on inertia |
| CN114526729B (en) * | 2022-01-14 | 2023-11-17 | 重庆邮电大学 | Course optimization method of MEMS inertial positioning system based on redundancy technology |
| CN114485641B (en) * | 2022-01-24 | 2024-03-26 | 武汉梦芯科技有限公司 | Attitude calculation method and device based on inertial navigation device navigation azimuth fusion |
| WO2023220972A1 (en) * | 2022-05-18 | 2023-11-23 | 北京小米移动软件有限公司 | Mobile device, pose estimation method and apparatus therefor, and storage medium |
| CN118057120A (en) * | 2022-11-18 | 2024-05-21 | 优奈柯恩(北京)科技有限公司 | Method and apparatus for estimating device pose |
| CN116625407B (en) * | 2023-06-05 | 2024-02-20 | 泉州职业技术大学 | Intelligent micro-attitude measurement method and system |
| CN118219707A (en) * | 2024-05-24 | 2024-06-21 | 南京信息工程大学 | Intelligent vehicle control method and device, intelligent vehicle and storage medium |
Citations (9)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN101835158A (en) * | 2010-04-12 | 2010-09-15 | 北京航空航天大学 | Sensor network trust evaluation method based on node behaviors and D-S evidence theory |
| CN104764452A (en) * | 2015-04-23 | 2015-07-08 | 北京理工大学 | Hybrid position-posture tracking method based on inertia and optical tracking systems |
| CN104880190A (en) * | 2015-06-02 | 2015-09-02 | 无锡北微传感科技有限公司 | Intelligent chip for accelerating inertial navigation attitude fusion |
| CN105190237A (en) * | 2013-03-13 | 2015-12-23 | 应美盛股份有限公司 | Heading confidence interval estimation |
| CN105607106A (en) * | 2015-12-18 | 2016-05-25 | 重庆邮电大学 | Low-cost high-precision BD/MEMS integration attitude measurement method |
| EP3126780A1 (en) * | 2014-03-31 | 2017-02-08 | Intel Corporation | Inertial measurement unit for electronic devices |
| CN107421537A (en) * | 2017-09-14 | 2017-12-01 | 桂林电子科技大学 | Object motion attitude cognitive method and system based on inertial sensor rigid body grid |
| CN107532907A (en) * | 2015-03-13 | 2018-01-02 | 桑男 | Posture detection equipment |
| CN107655476A (en) * | 2017-08-21 | 2018-02-02 | 南京航空航天大学 | Pedestrian's high accuracy foot navigation algorithm based on Multi-information acquisition compensation |
Family Cites Families (4)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| US8253985B2 (en) * | 2011-01-27 | 2012-08-28 | Seiko Epson Corporation | Image registration parameters and confidence estimation from sensor data |
| WO2014139170A1 (en) * | 2013-03-15 | 2014-09-18 | Google Inc. | System and method for attitude correction |
| CN106250667A (en) * | 2016-06-29 | 2016-12-21 | 中国地质大学（武汉） | The monitoring method of a kind of landslide transition between states of paddling and device |
| US10209078B2 (en) * | 2016-08-31 | 2019-02-19 | Yost Labs Inc. | Local perturbation rejection using time shifting |
-
2018
- 2018-05-25 CN CN201810517173.9A patent/CN109001787B/en active Active
Patent Citations (9)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN101835158A (en) * | 2010-04-12 | 2010-09-15 | 北京航空航天大学 | Sensor network trust evaluation method based on node behaviors and D-S evidence theory |
| CN105190237A (en) * | 2013-03-13 | 2015-12-23 | 应美盛股份有限公司 | Heading confidence interval estimation |
| EP3126780A1 (en) * | 2014-03-31 | 2017-02-08 | Intel Corporation | Inertial measurement unit for electronic devices |
| CN107532907A (en) * | 2015-03-13 | 2018-01-02 | 桑男 | Posture detection equipment |
| CN104764452A (en) * | 2015-04-23 | 2015-07-08 | 北京理工大学 | Hybrid position-posture tracking method based on inertia and optical tracking systems |
| CN104880190A (en) * | 2015-06-02 | 2015-09-02 | 无锡北微传感科技有限公司 | Intelligent chip for accelerating inertial navigation attitude fusion |
| CN105607106A (en) * | 2015-12-18 | 2016-05-25 | 重庆邮电大学 | Low-cost high-precision BD/MEMS integration attitude measurement method |
| CN107655476A (en) * | 2017-08-21 | 2018-02-02 | 南京航空航天大学 | Pedestrian's high accuracy foot navigation algorithm based on Multi-information acquisition compensation |
| CN107421537A (en) * | 2017-09-14 | 2017-12-01 | 桂林电子科技大学 | Object motion attitude cognitive method and system based on inertial sensor rigid body grid |
Non-Patent Citations (7)
| Title |
|---|
| Attitude measure system based on extended Kalman filter;Zhang Tiemin;《ELSEVIER》;20171231;19-26 * |
| High Accuracy Extend Kalman Filter for Posture Measurement based on Attitude and Heading Reference System;Yahua Huang;《IEEE Xplore》;20170718;262-266 * |
| High Precision Tri-axial MEMS Gyroscope Module based on Redundant Implementation and Sensor Fusion;Wei Yan;《IEEE Xplore》;20160622;376-379 * |
| MHD角速度传感器角振动信号的检测与降噪技术研究;徐亚东;《中国优秀博硕士学位论文全文数据库(硕士) 信息科技辑》;20180315;18-22 * |
| 一种基于D-S证据理论的QoS可信度评估方法;刘昕民等;《哈尔滨工业大学学报》;20130331(第03期);102-107 * |
| 基于卡尔曼滤波的动态脉搏波处理和脉率提取;蒋伟平;《计算机与现代化》;20111231;84-88、96 * |
| 基于置信度加权的拖拉机组合导航融合定位方法;朱忠祥等;《农业机械学报》;20131001;216-221、229 * |
Also Published As
| Publication number | Publication date |
|---|---|
| CN109001787A (en) | 2018-12-14 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN109001787B (en) | Attitude angle resolving and positioning method and fusion sensor thereof | |
| CN109556632B (en) | INS/GNSS/polarization/geomagnetic integrated navigation alignment method based on Kalman filtering | |
| CN107655476B (en) | Pedestrian high-precision foot navigation method based on multi-information fusion compensation | |
| WO2018184467A1 (en) | Method and device for detecting posture of ball head | |
| Hyyti et al. | A DCM Based Attitude Estimation Algorithm for Low‐Cost MEMS IMUs | |
| US9417091B2 (en) | System and method for determining and correcting field sensors errors | |
| WO2004063669A2 (en) | Attitude change kalman filter measurement apparatus and method | |
| CN112945225A (en) | Attitude calculation system and method based on extended Kalman filtering | |
| CN112146655B (en) | Elastic model design method for BeiDou/SINS tight integrated navigation system | |
| CN111189442B (en) | CEPF-based unmanned aerial vehicle multi-source navigation information state prediction method | |
| CN112066985B (en) | Initialization method, device, medium and electronic equipment for combined navigation system | |
| Sokolović et al. | INS/GPS navigation system based on MEMS technologies | |
| CN116007620A (en) | Combined navigation filtering method, system, electronic equipment and storage medium | |
| CN114526731A (en) | Inertia combination navigation direction positioning method based on moped | |
| de Celis et al. | Attitude determination algorithms through accelerometers, GNSS sensors, and gravity vector estimator | |
| CN113566850B (en) | Method and device for calibrating installation angle of inertial measurement unit and computer equipment | |
| Allotta et al. | Underwater vehicles attitude estimation in presence of magnetic disturbances | |
| CN112284388A (en) | Multi-source information fusion navigation method for unmanned aerial vehicle | |
| CN107747944B (en) | Airborne distributed POS transfer alignment method and device based on fusion weight matrix | |
| Zheng et al. | Multi-rotor uav attitude calculation based on extended kalman filter | |
| Wang et al. | Compounded calibration based on FNN and attitude estimation method using intelligent filtering for low cost MEMS sensor application | |
| Rahimi et al. | Improving the calibration process of inertial measurement unit for marine applications | |
| CN112629521A (en) | Modeling method for dual-redundancy combined navigation system of rotor aircraft | |
| CN113218387A (en) | Terrain reference navigation system with universal terrain sensors for correcting inertial navigation solutions | |
| Bieliakov | Simulation of platform-free inertial navigation system of unmanned aerial vehicles based on neural network algorithms |
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 |