CN109001787A - A kind of method and its merge sensor of solving of attitude and positioning - Google Patents

Abstract

A method for attitude angle calculation and positioning and its fusion sensor, the fusion sensor includes a plurality of IMU sensors, a magnetometer and GPS; the method includes: correcting the magnetometer, accelerometer and gyroscope; using redundant information fusion The algorithm performs data fusion on the measurement data of each accelerometer and each gyroscope to obtain acceleration information and angular velocity information; uses the extended Kalman filter algorithm to fuse acceleration information, angular velocity information and magnetic field information of the magnetometer to obtain the fusion attitude angle; The auxiliary positioning information obtains the location information. Since multiple redundant IMU sensors are used for data measurement and data fusion calculation, and the extended Kalman filter algorithm is used to fuse each data to obtain a fusion attitude angle, a higher-precision attitude angle can be obtained under static and dynamic conditions; at the same time, the fusion The attitude angle assists the GPS to obtain corrected position information, which makes the position information more accurate and improves the stability and reliability of the attitude angle calculation and positioning.

Description

A Method of Attitude Angle Calculation and Positioning and Its Fusion Sensor

technical field

The invention relates to the technical field of integrated navigation, in particular to a method for calculating and positioning an attitude angle and a fusion sensor thereof.

Background technique

Since the 1970s, navigation technology has developed rapidly. With the development of information technology and economic construction, transportation, industry, agriculture, surveying and mapping and other fields have put forward more diverse and higher requirements for navigation technology.

At present, in terms of attitude determination and positioning of navigation technology, integrated navigation technology is the most widely used. Integrated navigation refers to the use of one or several of GPS (Global Positioning System, Global Positioning System), radio navigation, celestial navigation, satellite navigation and other systems combined with INS (Inertial Navigation System, inertial navigation system) to form a comprehensive Navigation System. INS is a navigation parameter calculation system based on gyroscopes and accelerometers as sensitive devices. The system establishes a navigation coordinate system based on the output of the gyroscope, and calculates the velocity and position of the carrier in the navigation coordinate system based on the output of the accelerometer.

Among the existing integrated navigation technologies, the combination of GPS and INS is one of the most common ways. However, the existing integrated navigation technology of GPS and INS relies heavily on GPS signals, and the concealment of GPS signals is extremely poor, and the signals are easily interfered and obscured, resulting in a great decline in navigation accuracy; and, the work of GPS receivers Affected by the maneuvering of the aircraft, when the maneuvering of the aircraft exceeds the dynamic range of the GPS receiver, the receiver will lose lock and cannot capture and track satellite signals, so it cannot work. Navigation will be limited. At the same time, in the existing integrated navigation technology of GPS and INS, INS uses gyroscope and accelerometer as sensitive devices, and uses complementary filter fusion algorithm to solve the attitude angle, which has high computational complexity, and under dynamic conditions The error of the obtained attitude angle is relatively large.

Therefore, when using the existing integrated navigation technology for attitude angle calculation and positioning, the accuracy of the attitude angle information and position information obtained is not high, and the stability and reliability of attitude determination and positioning are low.

Contents of the invention

The application provides a method of attitude angle calculation and positioning and its fusion sensor to obtain high-precision and high-stability attitude angle information and position information, and improve the stability and reliability of attitude angle calculation and positioning.

According to the first aspect, an embodiment provides a method for attitude angle calculation and positioning, which is applied to a fusion sensor for attitude angle calculation and positioning, and the fusion sensor includes at least two IMU sensors, a magnetometer and GPS, one of the IMU sensors includes an accelerometer and a gyroscope, and the method includes:

Obtain the measurement data of each accelerometer and each gyroscope, as well as the magnetic field information of the magnetometer and the positioning information of GPS;

According to the confidence degree between the measurement data of each accelerometer and the confidence degree between the measurement data of each gyroscope, the measurement data of each accelerometer and the measurement data of each gyroscope are respectively fused by using the redundant information fusion algorithm , get acceleration information and angular velocity information;

Using the angular velocity information as a state variable and the acceleration information and the magnetic field information as observations, using an extended Kalman filter algorithm to calculate a fused attitude angle;

According to the acceleration information, the angular velocity information, the fused attitude angle and the positioning information, the position information is calculated by using an extended Kalman filter algorithm.

According to the second aspect, an embodiment provides a fusion sensor for attitude angle calculation and positioning, including: at least two IMU sensors, a magnetometer, GPS and a data processor, and an IMU sensor includes an accelerometer and an For the gyroscope, the at least two IMU sensors are connected in a double-sided anti-axis mode or a single-axis multi-sensor mode;

The double-sided anti-axis type method is that 2f IMU sensors are divided into two groups, which are respectively installed on both sides of the plate, and one axis of the IMU sensors on both sides is reversed, and the other two axes are in the same direction, and the f is greater than or equal to 1 an integer of

The single-axis multi-sensor method is to install a plurality of IMU sensors on each orthogonal axis, and each axis of each IMU sensor is the same;

The accelerometer is used to measure the acceleration of the carrier;

The gyroscope is used to measure the angular velocity of the carrier;

The magnetometer is used to measure magnetic field information;

The GPS is used to measure positioning information;

The data processor is used to obtain the measurement data of each accelerometer and each gyroscope, as well as the magnetic field information of the magnetometer and the positioning information of the GPS, respectively according to the degree of confidence between the measurement data of each accelerometer and the relationship between the measurement data of each gyroscope. Confidence between each accelerometer and gyroscope by using a redundant information fusion algorithm for data fusion to obtain acceleration information and angular velocity information, using the angular velocity information as a state quantity and the acceleration information and the magnetic field information is an observation quantity, and the fusion attitude angle is calculated by using the extended Kalman filter algorithm, and is calculated by using the extended Kalman filter algorithm according to the acceleration information, the angular velocity information, the fusion attitude angle and the positioning information output location information.

According to the attitude angle calculation and positioning method of the above-mentioned embodiment and its fusion sensor, the calculation of acceleration information and angular velocity information is performed through redundant information fusion algorithms of multiple IMU sensors, so that the calculated acceleration information and angular velocity information are more accurate. On this basis, with the aid of the magnetometer, the extended Kalman filter algorithm is used to fuse the acceleration information, angular velocity information and magnetic field information of the magnetometer to obtain the fusion attitude angle, which greatly improves the accuracy and stability of the system. At the same time, the final position information is obtained from the positioning information of GPS assisted by the high-precision fusion attitude angle, and the GPS positioning information is corrected, so that the obtained position information More accurate; thus, high-precision and high-stability attitude angle information and position information can be obtained, which improves the stability and reliability of attitude angle calculation and positioning.

Description of drawings

Fig. 1 is a schematic structural diagram of a fusion sensor for attitude angle resolution and positioning in an embodiment of the present invention;

Fig. 2 is a schematic diagram of the structure of the IMU sensor connected by a double-sided anti-axis type in an embodiment of the present invention;

Fig. 3 is a structural schematic diagram of IMU sensors connected by a single-axis multi-sensor mode in an embodiment of the present invention;

Fig. 4 is a flow chart of the method for attitude angle calculation and positioning in an embodiment of the present invention;

Fig. 5 is the flowchart of the method that adopts Allan variance method to correct gyroscope zero bias error in the embodiment of the present invention;

6 is a flow chart of a method for data fusion of accelerometer measurement data using a redundant information fusion algorithm in an embodiment of the present invention;

7 is a flowchart of a method for calculating a fusion attitude angle in an embodiment of the present invention;

Fig. 8 is the flow chart that utilizes extended Kalman filter recursive equation to calculate fusion attitude angle in the embodiment of the present invention;

FIG. 9 is a flow chart of calculating location information by using an extended Kalman filter algorithm in an embodiment of the present invention.

Detailed ways

The present invention will be further described in detail below through specific embodiments in conjunction with the accompanying drawings.

In the embodiment of the present invention, multi-redundant (at least two) IMU sensors are used to measure the acceleration and angular velocity of the carrier, and the redundant information fusion algorithm is used to perform data fusion on the measurement data of each accelerometer and each gyroscope respectively to obtain a fusion The acceleration information and angular velocity information of the magnetometer; then use the magnetometer as an auxiliary, use the extended Kalman filter algorithm to fuse the acceleration information, angular velocity information and the magnetic field information measured by the magnetometer to obtain the fusion attitude angle; then use the fusion attitude angle The positioning information measured by the assisted GPS is calculated by using the extended Kalman filter algorithm to obtain the position information.

An embodiment of the present invention provides a fusion sensor for attitude angle calculation and positioning, please refer to Figure 1, which shows a schematic structural diagram of the fusion sensor for attitude angle calculation and positioning, as shown in Figure 1, the attitude angle The fusion sensor for solving and positioning includes N IMU sensors 1, a magnetometer 2, GPS 3 and a data processor 4, wherein, N is an integer greater than or equal to 2; wherein the IMU sensor 1 includes an accelerometer 11 and a Gyroscope 12, accelerometer 11 is used to measure the acceleration of the carrier, gyroscope 12 is used to measure the angular velocity of the carrier, N IMU sensors 1 are connected in a double-sided anti-axis mode, or in a single-axis multi-sensor mode; magnetometer 2 For measuring magnetic field information, GPS 3 is used for measuring positioning information, and data processor 4 is used for filtering, amplifying, calculating, analyzing and other processing of signals detected by IMU sensor 1, magnetometer 2 and GPS 3; in this embodiment Among them, the data processor 4 includes an acquisition module 41, a data fusion module 42 and a calculation module 43, wherein the acquisition module 41 is used to acquire the measurement data of each accelerometer 11 and each gyroscope 12 and the magnetic field information of the magnetometer 2 and GPS3. Positioning information; the data fusion module 42 is used for according to the degree of confidence between the measurement data of each accelerometer 11, utilizes redundant information fusion algorithm to carry out data fusion to the measurement data of each accelerometer 11, obtains acceleration information, according to each gyroscope 12 Confidence between the measured data, use redundant information fusion algorithm to carry out data fusion on the measured data of each gyroscope 12, obtain angular velocity information; calculation module 43 is used to use angular velocity information as state quantity and acceleration information and magnetic field information as observation The fused attitude angle is calculated by using the extended Kalman filter algorithm, and the position information is calculated by using the extended Kalman filter algorithm according to the fused attitude angle, acceleration information, angular velocity information and positioning information. Preferably, the data processor 4 also includes a correction module, which is used to perform error compensation to the magnetometer 2 and the accelerometer 11, and to carry out zero bias correction to the gyroscope 12, so that the magnetometer 2, the accelerometer 11 and the gyroscope The instrument 12 is not affected by its own error when performing data measurement, and the measured data is more accurate.

Specifically, Fig. 2 shows a schematic diagram of the structure of the IMU sensor connected by a double-sided anti-axis type. As shown in Fig. 2, N=2f identical IMU sensors form a redundant sensor system, where f is greater than or equal to 1 Integer, the system is a flat-panel structure, N IMU sensors are divided into two groups, f in each group, installed on both sides of the flat panel, one axis direction of the IMU sensors on these two sides is opposite, and the direction of the other two axes is the same For example, in Figure 2, the coordinate directions of all IMU sensors on side A are the same, and the coordinate directions of all IMU sensors on side B are also the same, while the IMU sensors installed on side A and side B respectively have opposite z-axis directions, The directions of the x-axis and the y-axis are respectively the same.

Figure 3 shows a schematic diagram of the structure of an IMU sensor connected by a single-axis multi-sensor method. As shown in Figure 3, N=a+b+c identical IMU sensors form a redundant sensor system, where N is greater than or equal to 2 is an integer; the N identical IMU sensors are divided into three groups of a, b and c, the number of IMU sensors in each group can be set according to needs, and the three groups of IMU sensors are respectively installed in the redundant sensor system Install a, b, and c IMU sensors on the three orthogonal axes, that is, on the three orthogonal axes X, Y, and Z of the redundant sensor system, and each axis of the N IMU sensors is The same, for example, each axis is the same as the corresponding axis of the xyz coordinate system.

Based on the above-mentioned fusion sensor for attitude angle calculation and positioning, Figure 4 shows a flow chart of the method for attitude angle calculation and positioning in an embodiment of the present invention, as shown in Figure 4, the method may include the following steps:

Step S11: Error compensation.

The correction module in the data processor 4 performs error compensation on the magnetometer 2 and each accelerometer 11 . Specifically, the error compensation of the magnetometer 2 can adopt the method of twelve-plane calibration, and the method of ellipsoid fitting can also be used; the error compensation of the accelerometer 11 can adopt the method of Kalman filter, that is, the acceleration error is established as The measurement equation of the state variable is optimally estimated by the Kalman filter to output correction information to compensate the error of the accelerometer 11 .

Step S12: zero offset correction.

The correction module in the data processor 4 performs zero bias correction on each gyroscope 12 at the same time, specifically, the zero bias error of the gyroscope 12 can be corrected by the Allan variance method. In this embodiment, the gyroscope 12 can be a micromachine (MicroElectro Mechanical systems, MEMS) gyroscope, and the noise of the MEMS gyroscope can be divided into 5 independent parts, respectively quantization noise, angle random walk, deviation instability , rate random walk and rate ramp, the size of these five independent noises is represented by quantization noise coefficient Q, angle random walk coefficient N, bias instability coefficient B, rate random walk coefficient K and rate ramp coefficient R. Since the five noises are independent of each other, the following relationship (1) exists:

Among them, σ ² (τ) is the total noise of the gyroscope, that is, the zero bias error, is the quantization noise, Angular random walk, is bias instability, is a rate random walk, is the rate ramp.

Thus, Allan's mean square error can be defined as In this way, the log-log curve of σ(τ) can clearly describe the various error components and different error terms of the gyroscope. Specifically, FIG. 5 shows a method for correcting the zero bias error of the gyroscope using the Allan variance method. As shown in FIG. 5, the method may include the following steps S121 to S127, specifically:

Step S121: collecting static drift data.

When the Allan variance method is used to correct the zero bias error of the gyroscope, the sampling rate is 3 to 6 times the bandwidth of the gyroscope, which can ensure the accuracy of the processed data. Since the bandwidth of the gyroscope is about 100Hz, the sampling rate is set to 500Hz. When collecting data, install the gyroscope on a high-precision turntable with good vibration isolation effect, and then continuously collect the static drift data of the gyroscope. The collection time can be determined according to the needs, such as continuous collection for 1 hour.

Step S122: averaging the static drift data.

After collecting the static drift data of the gyroscope, in order to improve the post-operation speed, the collected data can be averaged for 1 second, and the data in the warm-up period can be eliminated, for example, the warm-up data of 0.1h can be eliminated, so as to obtain the static drift data to be processed .

Step S123: Calculate the mean value.

After obtaining the static drift data to be processed, group the static drift data to be processed according to the preprocessing parameters, and then calculate the mean value of each group of static drift data to be processed, wherein the preprocessing parameters may include sampling period, scaling factor, etc.

Step S124: Calculate the Allan mean square error.

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 square root operation is performed on the Allan variance to obtain the Allan mean square error.

Step S125: Draw the log-log curve of the Allan mean square error.

Step S126: Draw a fitting curve.

After obtaining the double logarithmic curve of the Allan mean square error, the double logarithmic curve is fitted, and the fitting curve of the double logarithmic curve is drawn.

Step S127: Calculate the zero bias error of the gyroscope.

Calculate the error coefficient of the gyroscope from the fitted curve of the double-logarithmic curve drawn, that is, the above-mentioned quantization noise coefficient Q, angle random walk coefficient N, deviation instability coefficient B, rate random walk coefficient K and rate The slope factor R, obtained from these error factors and Therefore, the total noise σ ² (τ) of the gyroscope is calculated according to the relation (1), that is, the zero bias error of the gyroscope is obtained.

Step S128: Correcting the zero bias error of the gyroscope.

The obtained zero bias error of the gyroscope is subtracted from the measured data of the gyroscope, thereby correcting the zero bias error of the gyroscope, and obtaining the corrected measured data of the gyroscope.

In practical applications, the above-mentioned processing of the static drift data of the gyroscope can also be carried out on the MATLAB platform, and the zero bias error of the gyroscope can be obtained by calculating the Allan variance data processing algorithm written.

Step S13: Obtain measurement data.

The acquisition module 41 acquires the measurement data of each accelerometer, the measurement data of each gyroscope, the magnetic field information measured by the magnetometer, and the positioning information measured by the GPS.

Step S14: Calculate acceleration information and angular velocity information.

After the acquisition module 41 acquires 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 uses the confidence degree between the measurement data of each accelerometer to use The redundant information fusion algorithm performs data fusion on the measurement data of each accelerometer to obtain acceleration information, and at the same time, according to the confidence between the measurement data of each gyroscope, the redundant information fusion algorithm is used to perform data fusion on the measurement data of each gyroscope , to get the angular velocity information.

When the redundant information fusion algorithm is used to fuse the measurement data of each accelerometer and the measurement data of each gyroscope, the advantage of determining the range of the membership function in the fuzzy set theory is fully utilized, thus defining a fuzzy type Exponential confidence function to quantify the mutual confidence of each sensor in the same type of sensors, and measure the comprehensive confidence of each sensor output data through the confidence matrix, and reasonably allocate the measurement data of each sensor in the same type of sensors in the data fusion process The final expression of the data fusion is obtained, and the data fusion of the measurement data of the multi-redundant sensors is realized, so as to obtain the acceleration information and the angular velocity information.

Specifically, assuming multiple identical sensors measure the same parameter, the data measured by the i-th sensor and the j-th sensor are respectively c _i and c _j , when the authenticity of c _i is higher, c _i is replaced by the rest of the data The higher the level of trust. The so-called degree of trust of c _i by c _j refers to the possibility of c _i being real data from c _j . The degree of trust between the measurement data of multiple redundant sensors is called confidence. In order to further uniformly quantify the degree of trust between the measurement data of multiple redundant sensors, a confidence function k _ij can be defined, which is used to represent the degree of trust of c _i by c _j . At the same time, set an upper limit S, and S>0, when the value of |c _i -c _j | exceeds the upper limit S, it can be considered that the two data no longer trust each other, and the confidence can be obtained The expression of the function k _ij is:

According to the expression (2), if k _ij =0, it is considered that the i-th sensor and the j-th sensor do not trust each other; if k _ij =1, it is considered that the i-th sensor trusts the j-th sensor. When a sensor is not trusted by other sensors, or is only trusted by a few sensors, the measurement data of the sensor will be eliminated during data fusion, so that abnormalities can be automatically eliminated during data fusion of the measurement data of each sensor Data, measurement data involved in data fusion will be reliable data.

Therefore, the redundant information fusion algorithm is used to process the data of multiple sensors, which increases the dimension of the target feature vector, so that the entire measurement system can obtain independent feature information that cannot be obtained by any single sensor, and overcome the uncertainty of a single sensor. and limitations, significantly improving the performance of the system.

On the basis of this theory, Fig. 6 shows the flow chart of the method for data fusing the measurement data of N accelerometers using redundant information fusion algorithm, as shown in Fig. 6, may include step S141 ~ step S143, specifically for:

Step S141: Establish a confidence matrix K.

N accelerometers measure the acceleration of the carrier at the same time, and establish a confidence matrix K according to the confidence function between the measurement data of each accelerometer, and the expression of K is:

Wherein, n=N, k _ij is the confidence function between the measurement data of each accelerometer, which represents the trust degree of the i-th accelerometer by the j-th accelerometer, and i and j are integers.

Step 142: Calculate the weight matrix E.

Use E _i to represent the weight of the i-th accelerometer's measurement data a _i in the data fusion process, and the size of E _i reflects the comprehensive trust of other accelerometers' measurement data on the i-th accelerometer's measurement data a _i degree. In the trust degree matrix K, the confidence function k _ij only indicates the degree of trust of the measurement data a _j to a _i , and does not reflect the degree of trust of all N accelerometer measurement data to a _i , while the true degree of a _{i In} fact, it should be reflected by k _i1 , k _i2 ,..., kin comprehensively. At this time, the degree of trust of the measurement data of other accelerometers on the measurement data a _i of the i-th accelerometer can be expressed as:

E _i ＝m ₁ k _i1 +m ₂ k _i2 +…+m _n k _in (3)

Among them, m ₁ , m ₂ ,..., m _n are a group of non-negative numbers, which can be expressed as M=[m ₁ ,m ₂ ,...,m _n ] ^T in the form of a matrix. Similarly, E _i is expressed as a matrix The form of is E=[e ₁ ,e ₂ ,…,e _n ] ^T , thus, formula (3) can be expressed in matrix form as:

E=KM

And because k _ij ≥ 0, the confidence matrix K is a non-negative matrix, and the symmetric matrix K has the largest modulus eigenvalue λ(λ>0), and the eigenvector corresponding to the eigenvalue is the solution of M, thus Then the weight matrix E can be obtained through the confidence matrix K and the non-negative vector M.

Step S143: Calculate acceleration information.

After obtaining the weight matrix E=[e ₁ ,e ₂ ,…,e _n ] ^T , according to the data fusion formula (4), use the weight matrix E to weight and sum the measurement data of the accelerometer, so as to obtain the acceleration information a, The data fusion formula (4) is:

Similarly, the process of using redundant information fusion algorithm to fuse the measurement data of N gyroscopes is the same as the process of using redundant information fusion algorithm to fuse the measurement data of N accelerometers. Confidence function between the measured data to build a confidence matrix Among them, n=N, k' _ij is the confidence function between the measurement data of each gyroscope, which represents the degree of confidence that the i-th gyroscope is trusted by the j-th gyroscope, and i and j are integers; then according to the matrix multiplication formula E'=K'M' calculates the weight matrix E'=[e' ₁ , e' ₂ ...e' _i ..., e' _n ] ^T , where M'=[m' ₁ , m' ₂ ...m' _j ..., m′ _n ] ^T is the eigenvector corresponding to the maximum modulus eigenvalue of the confidence matrix K′, and M′ is a non-negative vector; then according to the data fusion formula The angular velocity information ω is obtained by weighting and summing the measured data of the gyroscopes using the weight matrix E′, where ω _i is the measured data of the i-th gyroscope, and e′ _i represents the weight of ω _i in data fusion.

Step S15: Calculate the fusion attitude angle.

After obtaining the acceleration information and the angular velocity information, the calculation module 43 uses the angular velocity information as the state quantity and the acceleration information and the magnetic field information measured by the magnetometer as the observation quantity, and uses the extended Kalman filter algorithm to calculate the fused attitude angle. Specifically, Figure 7 shows the process of calculating the fusion attitude angle, as shown in Figure 7, the process may include steps S151 to S155, specifically:

Step S151: Calculate the initial attitude angle.

Before using the extended Kalman filter algorithm to calculate the fusion attitude angle, the attitude angle must be obtained without filtering method, that is, the initial attitude angle can be calculated directly based on 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 magnetometer is carried out in the carrier coordinate system, so it is necessary to convert the attitude calculated by the accelerometer and magnetometer into the navigation coordinate system; the carrier coordinate system Defined as b, the navigation coordinate system is defined as d, then the transformation is realized by the transformation matrix from b to d, which is rotated in the order of the X-Y-Z axis, and the obtained transformation matrix can be expressed as:

in, θ and ψ represent pitch angle, roll angle and yaw angle respectively, is the obtained transformation matrix.

When the carrier is at rest or in a uniform linear motion state, it has a slight acceleration in addition to the acceleration of gravity, and the conversion matrix can be calculated by the acceleration information of the accelerometer and the magnetic field information of the magnetometer Its calculation formula is:

Among them, g is the gravitational acceleration, a _x , a _y and a _z are the acceleration information measured by the accelerometer, which respectively represent the three-axis acceleration of the accelerometer.

Calculated by formula (6) Combined with formula (5), the initial pitch angle can be obtained and the calculation formulas of the initial roll angle θ ₀ are:

θ ₀ =arctan(a _x /g)

Further, according to the calculated θ ₀ and the magnetic field information measured by the magnetometer, use Formula 1 and Formula 2 to calculate the initial yaw angle Ψ ₀ , where Formula 1 is:

Among them, m _x , my _y and m _z are the magnetic field information measured by the magnetometer, respectively representing the magnetic field values of the three axes of the magnetometer.

Formula 2 is: Ψ ₀ =arctan(H _y /H _x ).

From this, the initial attitude angle can be calculated θ ₀ and Ψ ₀ .

Step S152: Calculate the four elements of the initial pose.

According to the calculated initial attitude angle θ ₀ and Ψ ₀ , use the correspondence between the attitude angle and the four elements to calculate the four elements of attitude q at the initial moment, where, define q=q ₀ +q ₁ i+q ₂ j+q ₃ k, q ₀ , q ₁ , q ₂ and q ₃ represent four elements.

In the calculation of the attitude angle, in order to avoid the problem of the singularity of the Euler angle, the quaternion is usually used to complete the calculation of the attitude angle, and the four elements are rotated according to the unit q=q ₀ +q ₁ i+q ₂ j+q ₃ k, the transformation matrix from b to d can be expressed as:

Corresponding the formula with formula (5) can get the corresponding relationship between the attitude angle and the four elements, and then use the corresponding relationship to calculate the four elements q ₀ , q ₁ , q ₂ and q ₃ , and then get the attitude angle at the initial moment. element q.

Step S153: Update the four elements of posture.

When using the four elements of attitude to calculate the attitude angle, the four elements of attitude need to be updated continuously, which can be updated through the quaternion differential equation based on the gyroscope data. Specifically, the angular velocity matrix W can be established based on the angular velocity information measured by the gyroscope, and the expression of W is:

Among them, ω _x , ω _y and ω _z are the angular velocity information of the gyroscope, respectively representing the angular velocity of the three axes of the gyroscope.

Then, using the update equation The four elements of attitude are updated to obtain the updated four elements of attitude q′.

Step S154: Establish an observation equation.

Taking the continuously updated attitude four-element q′ as the state variable X, and taking the acceleration information measured by the redundant accelerometer and the magnetic field information measured by the magnetometer as the observation value Z, the observation equation is established, and the established observation equation is:

Z=H(X)+V

Among them, the observed quantity Z=[a _x a _y a _z Ψ] ^T , g is the acceleration of gravity, a _x , a _y and a _z are acceleration information, which respectively represent the three-axis acceleration of the accelerometer, Ψ is the heading angle calculated according to the magnetic field information, and V is the measurement noise.

The observation equation is discretized, and the discretized observation equation is obtained as:

Z(k)=H(k)X(k)+V(k)

Among them, X(k) is the state value at time k, V(k) is the measurement noise at time k, is the Jacobian matrix of H(X), which represents the partial derivative of H(X(k)) at time k with respect to X(k).

Step S155: Calculate the fusion attitude angle.

According to the state variable X and the discretized observation equation Z(k), the fused attitude angle is calculated by using the extended Kalman filter recursive equation. Specifically, Fig. 8 shows the calculation of the fused attitude angle by using the extended Kalman filter recursive equation The flow chart, as shown in Figure 8, may include the following steps:

Step SA1: State one-step prediction.

According to the formula Perform state prediction to obtain the state prediction value at time k Among them, F(k-1) is the state transition matrix at time k-1, X(k-1) is the state value at time k-1, and U(k-1) is the state noise at time k-1. Among them, the state transition matrix at time k can be expressed as:

Among them, ω _x (k), ω _y (k) and ω _z (k) are the angular velocity information of the gyroscope at time k.

Step SA2: Error covariance prediction.

According to the formula Estimate the error covariance and get the predicted value of the error covariance matrix Among them, P(k-1) is the error covariance matrix at time k-1, F ^T (k-1) is the transpose of F(k-1), Q(k-1) is the system at time k-1 Noise covariance matrix.

Step SA3: Calculate the filter gain matrix.

According to the formula Calculate the 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.

Step SA4: Calculate the fused attitude angle.

first according to the formula Calculate the state value X(k) at time k to obtain the four elements of attitude at time k, and then calculate the fusion attitude angle at time k according to the correspondence between the four elements of attitude and the attitude angle.

Step SA5: Update the error covariance.

According to the formula The error covariance is updated to obtain the error covariance matrix at time k, which is used for update calculation at the next time, where A=IK(k)H(k), and I is the identity matrix.

In this way, the fused attitude angle can be continuously updated under dynamic conditions to obtain the fused attitude angle under dynamic conditions.

Step S16: Fuse the attitude angle 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 GPS. Specifically, FIG. 9 shows the process of calculating location information. As shown in FIG. 9, the process may include the following steps S161 to S165:

Step S161: Determine state variables.

The calculation module 43 first obtains the velocity components ν _x , ν _y and ν _z of the three axes from the acceleration information, and then uses the velocity components, GPS positioning information and And the parameter error of the fusion sensor in the three axes and Create a state variable X′, where and Represent longitude, latitude and altitude respectively, and the established state variables are

Step S162: Establish a state transition matrix.

After the calculation module 43 obtains the velocity components of the three axes, it also establishes the state transition matrix Among them, F _N is a system matrix containing velocity components, fused attitude angles and positioning information, F _S is an identity matrix, and F _M is a matrix composed of the output frequencies of gyroscopes and accelerometers.

Step S163: Calculate carrier location information.

The calculation module 43 calculates carrier positioning information according to the acceleration information and angular velocity information obtained after data fusion. Specifically, the computing module 43 integrates the acceleration information to obtain the moving distance of the carrier, and at the same time performs double integration on the angular velocity information to obtain the moving direction of the carrier. In this way, the computing module 43 can obtain the carrier moving distance and the moving direction of the carrier The positioning information L _t , λ _t and h _t are the longitude, latitude and height of the carrier.

Step S164: Establish an observation equation.

After calculating the carrier positioning information L _t , λ _t and h _t , the calculation module 43 first calculates the carrier positioning information and the positioning information measured by GPS and use the formula Establish the position observation equation Z′ _p (t), where _RM is the radius of curvature of the meridian of the ellipsoid, and R _N is the radius of curvature of the meridian of the ellipsoid; then, the velocity information υ _Gx , υ _Gy and υ _Gz of the carrier are acquired through GPS , and then establish the velocity observation equation Z′ _v (t) according to the difference between the velocity components υ _x , υ _y and υ _z and the carrier’s velocity information υ _Gx , υ _Gy and υ _Gz , the established velocity observation equation Z′ _v (t) for

After establishing the position observation equation Z′ _p (t) and velocity observation equation Z′ _v (t), combining Z′ _p (t) and Z′ _v (t) together can obtain the total observation equation Z′( t), specifically:

Among them, X'(t) is the state equation corresponding to the state quantity X', and V'(t) is the measurement noise.

The observation equation Z'(t) is discretized to obtain the discretized observation equation Z'(k), and Z'(k) can be expressed as:

Z'(k)=H'(k)X'(k)+V'(k)

Among them, H'(k) is the Jacobian matrix, X'(k) and V'(k) are the state value and measurement noise at time k, respectively.

Step S165: Calculate location information.

According to the state variable X', the state transition matrix F' and the discretized observation equation Z'(k), the position information is calculated by using the extended Kalman filter recurrence equation. Specifically, with X' as the state variable, F' as the state transition matrix, and Z'(k) as the discretized observation equation, when using the extended Kalman filter recurrence equation to calculate the fusion attitude angle, the process is the same as using the extended Kalman filter The process of calculating the fused attitude angle by the Mann filtering recursive equation is the same, please refer to step SA1 to step SA5, which will not be repeated here.

In this way, the location information can be continuously updated under dynamic conditions, and the location information under dynamic conditions can be obtained. Moreover, in the process of calculating position information, the position information measured by GPS is corrected with the calculated high-precision and high-stability fused attitude angle. Therefore, the final position information also has high precision and relatively high high stability.

The attitude angle calculation and positioning method and its fusion sensor provided by the embodiment of the present invention use multiple IMU sensors to measure acceleration and angular velocity, which increases the dimension of the target feature vector, so that the entire measurement system can obtain any single sensor Independent characteristic information that cannot be obtained. Furthermore, in the process of calculating the attitude angle, firstly, error compensation is performed on the magnetometer and each accelerometer, and the zero bias correction is performed on each gyroscope, so as to avoid the influence of the errors of the magnetometer, accelerometer, and gyroscope on the measurement data. Influence; and then coordinate, complement and combine the measurement data of multiple IMU sensors through the redundant information fusion algorithm to obtain the final acceleration information and angular velocity information, automatically eliminate the abnormal data in the measurement, and overcome the uncertainty of a single sensor and limitations, greatly improving the accuracy and stability of the attitude angle calculation; then use the extended Kalman filter algorithm to fuse the acceleration information, angular velocity information and magnetic field information obtained by the magnetometer, abandoning the traditional complementary filter fusion algorithm and gradient Descent method, the calculation complexity is relatively low, and the attitude angle with high precision can be obtained under static and dynamic conditions, which greatly reduces the error of the attitude angle. The position information of the GPS is compensated for the positioning information of the GPS, so that the obtained position information is more accurate and more stable. Therefore, the method of attitude angle calculation and positioning provided by the embodiment of the present invention and its fusion sensor can obtain high-precision and high-stability attitude angle information and position information during the attitude angle calculation process, which improves the attitude angle calculation process. and positioning stability and reliability.

Those skilled in the art can understand that all or part of the functions of the various methods in the foregoing implementation manners can be realized by means of hardware, or by means of computer programs. When all or part of the functions in the above embodiments are implemented by means of a computer program, the program can be stored in a computer-readable storage medium, and the storage medium can include: read-only memory, random access memory, magnetic disk, optical disk, hard disk, etc., through The computer executes the program to realize the above-mentioned functions. For example, the program is stored in the memory of the device, and when the processor executes the program in the memory, all or part of the above-mentioned functions can be realized. In addition, when all or part of the functions in the above embodiments are realized by means of a computer program, the program can also be stored in a storage medium such as a server, another computer, a magnetic disk, an optical disk, a flash disk, or a mobile hard disk, and saved by downloading or copying. To the memory of the local device, or to update the version of the system of the local device, when the processor executes the program in the memory, all or part of the functions in the above embodiments can be realized.

The above uses specific examples to illustrate the present invention, which is only used to help understand the present invention, and is not intended to limit the present invention. For those skilled in the technical field to which the present invention belongs, some simple deduction, deformation or replacement can also be made according to the idea of the present invention.

Claims (10)

1. A method for attitude angle calculation and positioning, characterized in that, it is applied in the fusion sensor of attitude angle calculation and positioning, and the fusion sensor includes at least two IMU sensors, a magnetometer and GPS, and a described The IMU sensor contains an accelerometer and a gyroscope, and the method includes: Obtain the measurement data of each accelerometer and each gyroscope, as well as the magnetic field information of the magnetometer and the positioning information of GPS; According to the confidence degree between the measurement data of each accelerometer and the confidence degree between the measurement data of each gyroscope, the measurement data of each accelerometer and the measurement data of each gyroscope are respectively fused by using the redundant information fusion algorithm , get acceleration information and angular velocity information; Using the angular velocity information as a state quantity and the acceleration information and the magnetic field information as observation quantities, calculate a fusion attitude angle by using an extended Kalman filter algorithm; According to the acceleration information, the angular velocity information, the fused attitude angle and the positioning information, the position information is calculated by using an extended Kalman filter algorithm. 2. The method according to claim 1, wherein, before obtaining the measurement data of each accelerometer and each gyroscope and the magnetic field information of the magnetometer and the positioning information of the GPS, the method also includes: Error compensation is performed on the magnetometer and accelerometer, and zero bias correction is performed on the gyroscope. 3. The method according to claim 2, wherein said correcting the zero bias of the gyroscope comprises: Collect the static drift data of the gyroscope; Performing 1s averaging processing on the static drift data, removing the data in the warm-up time period, to obtain the static drift data to be processed; The static drift data to be processed is grouped according to preprocessing parameters, and the mean value of each group of static drift data to be processed is calculated, and the preprocessing parameters include a sampling period and a scaling factor; Calculation of the Allan variance from the mean; Calculate the Allan mean square error according to the Allan variance; Draw the log-log curve of the Allan mean square error; Fitting the logarithmic curve to obtain a fitted curve; Calculate the error coefficient of the gyroscope according to the fitting curve, the error coefficient includes quantization noise coefficient, angle random walk coefficient, deviation instability coefficient, rate random walk coefficient and rate slope coefficient; Calculate the total noise of the gyroscope according to the error coefficient of the gyroscope, and obtain the zero bias error of the gyroscope; The zero bias error is subtracted from the measurement data of the gyroscope to correct the zero bias error of the gyroscope. 4. The method according to claim 1, wherein the redundant information fusion algorithm comprises: Calculate the weight matrix E=[e ₁ , e ₂ ...e _i ..., e _n ] ^T according to the matrix multiplication formula E=KM, wherein, the is the confidence _matrix established according to the confidence function between the measurement data, kij is the confidence function between the measurement data, which represents the degree of trust of the i-th measurement data by the j-th measurement data, and n represents the measurement data The number of , i and j are integers, the M=[m ₁ , m ₂ ... m _j ..., m _n ] ^T is the eigenvector corresponding to the largest modulus eigenvalue of the confidence matrix K, and M is a non-negative vector ; According to the data fusion formula, the measurement data is weighted and summed using the weight matrix E to obtain the fusion data a; The data fusion formula is: Among them, a _i is the i-th measurement data, and e _i represents the weight of a _i in data fusion. 5. The method according to claim 1, wherein the angular velocity information is used as a state quantity and the acceleration information and the magnetic field information are observed quantities, and the extended Kalman filter algorithm is used to calculate the fusion attitude angle ,include: calculating an initial attitude angle according to the acceleration information and the magnetic field information; According to the initial attitude angle, the four element q of the attitude at the initial moment is calculated by using the correspondence between the attitude angle and the four elements, wherein, q=q ₀ +q ₁ i+q ₂ j+q ₃ k, q ₀ , q ₁ , q ₂ and q ₃ represent four elements; Establishing an angular velocity matrix W according to the angular velocity information; According to the angular velocity matrix W, using the update equation Update the four elements of attitude to obtain the updated four elements of attitude q′; Taking q' as the state variable X, and the acceleration information and magnetic field information as the observation quantity Z, establish the observation equation; Discretize the observation equation to obtain a discretized observation equation Z(k); According to described X and Z (k), utilize extended Kalman filtering recursive equation to calculate and obtain fusion attitude angle; The angular velocity matrix W is Wherein, ω _x , ω _y and ω _z are the angular velocity information, representing the angular velocity of the three axes of the gyroscope respectively; The observation equation is Z=H(X)+V, wherein the observation quantity Z=[a _x a _y a _z Ψ] ^T , g is the acceleration of gravity, a _x , a _y and a _z are the acceleration information, which represent the three-axis acceleration of the accelerometer respectively, Ψ is the heading angle calculated according to the magnetic field information, and V is the 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, and V(k) is the measurement noise at time k , is the Jacobian matrix of H(X), which represents the partial derivative of H(X(k)) at time k with respect to X(k). 6. The method according to claim 5, wherein said calculating an initial attitude angle according to said acceleration information and said magnetic field information comprises: according to and θ ₀ =arctan(a _x /g) respectively calculate the initial pitch angle and initial roll angle θ ₀ ; according to the θ ₀ and magnetic field information, using formula 1 and formula 2 to calculate the initial yaw angle Ψ ₀ ; The first formula is: Wherein, m _x , m _y and m _z are the magnetic field information, representing the magnetic field values of the three axes of the magnetometer respectively; The second formula is: Ψ ₀ =arctan(H _y /H _x ). 7. method as claimed in claim 5, is characterized in that, described according to described X and described Z (k), utilizes Extended Kalman filter recursive equation to calculate and obtain fusion attitude angle, comprising: According to the formula Perform state prediction to obtain the state prediction value at time k Among them, F(k-1) is the state transition matrix at k-1 time, X(k-1) is the state value at k-1 time, U(k-1) is the state noise at k-1 time; According to the formula Estimate the error covariance and get the predicted value of the error covariance matrix Among them, P(k-1) is the error covariance matrix at time k-1, F ^T (k-1) is the transpose of F(k-1), Q(k-1) is the system at time k-1 noise covariance matrix; According to the formula Calculate the filter gain matrix K(k), where ^HT (k) is the transpose of H(k), and R(k) is the measurement noise covariance at time k; According to the formula Calculate the state value X(k) at time k to obtain the four elements of attitude at time k; Calculate the fusion attitude angle at time k according to the corresponding relationship between the attitude four elements and the attitude angle; According to the formula The error covariance is updated to obtain the error covariance matrix at time k, where A=IK(k)H(k), and I is the identity matrix. 8. The method according to claim 1, wherein, according to the acceleration information, the angular velocity information, the fusion attitude angle and the positioning information, the position information is calculated by using an extended Kalman filter algorithm, include: Acquiring the velocity component in the acceleration information, using the velocity component, the positioning information, and the parameter error of the fusion sensor as a state variable X'; Establish the state transition matrix F' according to formula 1; calculating carrier positioning information according to the acceleration information and the angular velocity information; According to the location information of described carrier positioning information and GPS, utilize formula two to establish position observation equation Z' _p (t); Acquire the speed information of the carrier by GPS, and establish a speed observation equation Z' _v (t) according to the difference between the speed component and the speed information; According to said _Z'p (t) and said _Z'v (t), utilize formula three to obtain observation equation Z'(t); Discretizing the Z'(t) to obtain a discretized observation equation Z'(k); According to the state variable X', the state transition matrix F' and the discretized observation equation Z'(k), the position information is calculated by using the extended Kalman filter recurrence equation; said Wherein, υ _x , υ _y and υ _z are the velocity components, and For the positioning information, respectively represent longitude, latitude and height, ▽ _x , ▽ _y and ▽ _z are the parameter errors of the fusion sensor; The first formula is: Wherein, F _N is a system matrix including the velocity component, the fusion attitude angle and the positioning information, the F _S is an identity matrix, and the F _M is a matrix composed of the output frequencies of the gyroscope and the accelerometer ; The second formula is: Wherein, L _t , λ _t and h _t are the positioning information of the carrier, _RM is the radius of curvature of the meridian of the ellipsoid, R _N is the radius of curvature of the meridian of the ellipse, and L is the initial latitude of the carrier; The third formula is: Among them, V'(t) is the measurement noise; Said Z'(k)=H'(k)X'(k)+V'(k), wherein, H'(k) is a Jacobian matrix, X'(k) and V'(k) are respectively State value and measurement noise at time k. 9. The method according to claim 8, wherein said calculating carrier positioning information according to said acceleration information and said angular velocity information comprises: Integrating the acceleration information to obtain the moving distance of the carrier; Carrying out double integration on the angular velocity information to obtain the moving direction of the carrier; Carrier positioning information is obtained according to the moving distance of the carrier and the moving direction of the carrier. 10. A fusion sensor for attitude angle calculation and positioning, characterized in that it includes: at least two IMU sensors, a magnetometer, GPS and a data processor, an IMU sensor includes an accelerometer and a gyroscope, the At least two IMU sensors are connected by a double-sided anti-axis method or a single-axis multi-sensor method; The double-sided anti-axis type method is that 2f IMU sensors are divided into two groups, which are respectively installed on both sides of the plate, and one axis of the IMU sensors on both sides is reversed, and the other two axes are in the same direction, and the f is greater than or equal to 1 an integer of The single-axis multi-sensor method is to install a plurality of IMU sensors on each orthogonal axis, and each axis of each IMU sensor is the same; The accelerometer is used to measure the acceleration of the carrier; The gyroscope is used to measure the angular velocity of the carrier; The magnetometer is used to measure magnetic field information; The GPS is used to measure positioning information; The data processor is used to obtain the measurement data of each accelerometer and each gyroscope, as well as the magnetic field information of the magnetometer and the positioning information of the GPS, respectively according to the degree of confidence between the measurement data of each accelerometer and the relationship between the measurement data of each gyroscope. Confidence between each accelerometer and gyroscope by using a redundant information fusion algorithm for data fusion to obtain acceleration information and angular velocity information, using the angular velocity information as a state quantity and the acceleration information and the magnetic field information is an observation quantity, and the fusion attitude angle is calculated by using the extended Kalman filter algorithm, and is calculated by using the extended Kalman filter algorithm according to the acceleration information, the angular velocity information, the fusion attitude angle and the positioning information output location information.