CN112710309A - Attitude heading parameter estimation method - Google Patents

Attitude heading parameter estimation method Download PDF

Info

Publication number
CN112710309A
CN112710309A CN202011421921.7A CN202011421921A CN112710309A CN 112710309 A CN112710309 A CN 112710309A CN 202011421921 A CN202011421921 A CN 202011421921A CN 112710309 A CN112710309 A CN 112710309A
Authority
CN
China
Prior art keywords
coordinate system
angle estimate
quaternion
estimate
pass filter
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.)
Granted
Application number
CN202011421921.7A
Other languages
Chinese (zh)
Other versions
CN112710309B (en
Inventor
石岗
于云华
胡长松
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Shengli College China University of Petroleum
Original Assignee
Shengli College China University of Petroleum
Priority date (The priority date 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 date listed.)
Filing date
Publication date
Application filed by Shengli College China University of Petroleum filed Critical Shengli College China University of Petroleum
Priority to CN202011421921.7A priority Critical patent/CN112710309B/en
Publication of CN112710309A publication Critical patent/CN112710309A/en
Application granted granted Critical
Publication of CN112710309B publication Critical patent/CN112710309B/en
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

Classifications

    • 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/20—Instruments for performing navigational calculations
    • 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/24—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 specially adapted for cosmonautical navigation

Abstract

The invention relates to a method for estimating attitude heading parameters, which processes data of an accelerometer, a gyroscope and a magnetic sensor in the process of estimating the attitude heading, estimates the attitude heading parameters by adopting a vector method and an integral method respectively, and performs combined estimation on the estimation result of the vector method and the estimation result of the integral method by a complementary filter, thereby realizing the estimation of the attitude heading parameters in three modes, and the three modes correspond to different sensor combinations and can be suitable for different working conditions. The invention has the advantages of multiple working modes, good attitude estimation effect and less adjustment parameters.

Description

Attitude heading parameter estimation method
Technical Field
The invention belongs to the technical field of navigation attitude parameter detection, and particularly relates to a navigation attitude parameter estimation method.
Background
The attitude heading parameter can quantitatively describe the angular position of an object in a reference coordinate system, and is widely applied to production and life. The accelerometer, the gyroscope and the magnetic sensor are common attitude and heading reference sensors, and the measurement characteristics and the measurement information of the sensors are different, so that the sensors can be used independently and can be combined and applied through an information fusion technology.
The application of the literature improved complementary filtering in the three-dimensional attitude estimation (Chenwei et al. control engineering, 2019,26 (5): 910-. In the research on attitude complementary filtering algorithms in the maneuvering process of small unmanned aerial vehicles in the literature (Wang Yongjun et al, electronic measurement and instruments, 2020,34 (7): 141-149), attitude complementary filtering based on different transfer functions and various attitude representation forms is summarized into a unified generalized complementary filtering algorithm, multiplicative attitude errors in the algorithms are analyzed, and a motion acceleration compensation method is introduced. The literature adjusts the handover frequency of the filter by adaptively adjusting the proportional integral parameter based on the traditional complementary filter in the attitude estimation (Chen Guanwu. control theory and application, 2019,36 (7): 1096-.
The processes used in the above documents all have the following disadvantages: (1) the attitude heading estimation method has only one working mode, needs an accelerometer, a gyroscope and a magnetic sensor to work in a matching way, and cannot adapt to other sensor combination forms. (2) The low-frequency wave characteristics of the accelerometer and the magnetic sensor are not considered, the frequency domain processing of sensor data is insufficient, and the estimation effect on the navigation attitude is poor. (3) A proportional-integral link is adopted to realize a second-order complementary filter, and more adjustment parameters are provided.
Disclosure of Invention
The invention provides a multi-working-mode attitude heading reference parameter estimation method aiming at the problems of single working mode, poor attitude heading reference estimation effect, more adjustment parameters and the like in the prior art.
In order to achieve the purpose, the invention provides a navigation attitude parameter estimation method, which comprises the following specific steps:
acquiring an accelerometer measurement value a, a magnetic sensor measurement value m and a gyroscope measurement value omega;
taking an accelerometer measured value a and a magnetic sensor measured value m as input, calculating a pitch angle estimated value by a gravity acceleration vector in a reference coordinate system by utilizing a vector coordinate transformation relation
Figure BDA0002822743890000021
And roll angle estimate
Figure BDA0002822743890000022
Converting the geomagnetic vector in the object coordinate system to a horizontal coordinate system, and calculating a heading angle estimation value by using the geomagnetic vector in the reference coordinate system and the geomagnetic vector in the horizontal coordinate system
Figure BDA0002822743890000023
Taking a gyro measurement value omega as input, constructing a quaternion recursion calculation model by using a navigation attitude quaternion dynamic recursion equation, calculating navigation attitude quaternion at each moment, and calculating a course angle estimation value by using the navigation attitude quaternion
Figure BDA0002822743890000024
Pitch angle estimate
Figure BDA0002822743890000025
And roll angle estimate
Figure BDA0002822743890000026
Respectively constructing a low-pass filter and a high-pass filter according to the transfer function sum being 1, wherein the low-pass filter uses a course angle estimated value
Figure BDA0002822743890000027
Pitch angle estimate
Figure BDA0002822743890000028
And roll angle estimate
Figure BDA0002822743890000029
For input, the high pass filter uses the course angle estimate
Figure BDA00028227438900000210
Pitch angle estimate
Figure BDA00028227438900000211
Roll angle estimate
Figure BDA00028227438900000212
For input, the estimation result of the attitude heading parameters processed by the low-pass filter and the estimation result of the attitude heading parameters processed by the high-pass filter are added to obtain an estimation value of a heading angle
Figure BDA00028227438900000213
Pitch angle estimate
Figure BDA00028227438900000214
And roll angle estimate
Figure BDA00028227438900000215
Preferably, the pitch angle estimate is calculated using a vector coordinate transformation
Figure BDA00028227438900000216
Roll angle estimate
Figure BDA00028227438900000217
And course angle estimation
Figure BDA00028227438900000218
The method comprises the following specific steps:
let the reference coordinate system be or-xryrzrCoordinate system of object is ob-xbybzbThe coordinate of the space vector in the reference coordinate system is vrThe coordinate in the object coordinate system is vbThen, the transformation relationship between the two coordinates is:
Figure BDA0002822743890000031
in the formula (I), the compound is shown in the specification,
Figure BDA0002822743890000032
a transformation matrix from a reference coordinate system to an object coordinate system;
construction of transformation matrices using euler angles
Figure BDA0002822743890000033
Expressed as:
Figure BDA0002822743890000034
where cos represents a cosine function, sin represents a sine function, ψ is a course angle,
Figure BDA0002822743890000035
is pitch angle, gamma is roll angle, course angle psi, pitch angle
Figure BDA0002822743890000036
The transverse rolling angle gamma forms a group of Euler angles;
let the gravity acceleration vector in the reference coordinate system be ar,ar＝[0 0 9.8]Geomagnetic vector is mrAnd the geomagnetic model is calculated to obtain the following data:
Figure BDA0002822743890000037
substituting equation (2) into equation (3) yields:
Figure BDA0002822743890000038
Figure BDA0002822743890000039
in the formula, axIs the x-axis component of the accelerometer measurement a, ayIs the y-axis component of the accelerometer measurement a, azIs the z-axis component of the accelerometer measurement a;
using pitch angle estimates
Figure BDA00028227438900000310
And roll angle estimate
Figure BDA00028227438900000311
Transformation matrix from construct coordinate system to horizontal coordinate system
Figure BDA00028227438900000312
Transformation matrix
Figure BDA00028227438900000313
Expressed as:
Figure BDA00028227438900000314
calculating the geomagnetic vector m in the object coordinate systembTransformed into a geomagnetic vector m in a horizontal coordinate systempThen, there are:
Figure BDA00028227438900000315
substituting equation (6) into equation (7) yields:
Figure BDA0002822743890000041
in the formula, mpxAs a geomagnetic vector mpX-axis component of (1), mpyAs a geomagnetic vector mpY-axis component of (1), mrxAs a geomagnetic vector mrX-axis component of (1), mryAs a geomagnetic vector mrA y-axis component of;
calculating to obtain the pitch angle estimated value by the formula (4), the formula (5) and the formula (8)
Figure BDA0002822743890000049
Roll angle estimate
Figure BDA0002822743890000042
Course angle estimation
Figure BDA0002822743890000043
Preferably, before the accelerometer measurement value a and the magnetic sensor measurement value m are subjected to vector coordinate transformation, frequency domain compensation is respectively performed on the accelerometer measurement value a and the magnetic sensor measurement value m through a compensation filter.
Preferably, the specific method for performing frequency domain compensation by the compensation filter comprises: inputting the accelerometer measured value a into a compensation filter, wherein the output of the compensation filter is the compensated accelerometer measured value a, inputting the magnetic sensor measured value m into the compensation filter, the output of the compensation filter is the compensated accelerometer measured value m, and the transfer function of the compensation filter is (T)1s+1)/(T2s+1)。
Preferably, the heading angle estimated value is calculated by utilizing the attitude quaternion
Figure BDA0002822743890000044
Pitch angle estimate
Figure BDA0002822743890000045
And roll angle estimate
Figure BDA0002822743890000046
The method comprises the following specific steps:
taking a gyro measurement value omega as input, and constructing a quaternion recursion calculation model by utilizing a navigation attitude quaternion dynamic recursion equation, wherein the quaternion recursion calculation model is expressed as follows:
Figure BDA0002822743890000047
in the formula, qkIs a quaternion of k sampling instants, qk+1Is the quaternion of k +1 sampling time, k is the sampling time, k is 0,1,2,3, Δ t is the sampling interval, ω iskFor the gyro measurement at the time of k sampling, [ omega ]k×]For gyro measurements omegakAn antisymmetric matrix of (a);
construction of transformation matrices using quaternions
Figure BDA0002822743890000048
Expressed as:
Figure BDA0002822743890000051
in the formula, q0Real number component of quaternion, q1The first component being the imaginary part of the quaternion, q2A second component being the imaginary part of the quaternion, q3A third component that is an imaginary part of the quaternion;
simultaneous equations (2) and (4) yield:
Figure BDA0002822743890000052
Figure BDA0002822743890000053
Figure BDA0002822743890000054
substituting quaternion obtained by calculation of formula (9) into formulas (12) - (14) to obtain course angle estimated value
Figure BDA0002822743890000055
Pitch angle estimate
Figure BDA0002822743890000056
Roll angle estimate
Figure BDA0002822743890000057
Preferably, a course angle estimate is obtained
Figure BDA0002822743890000058
Pitch angle estimate
Figure BDA0002822743890000059
And roll angle estimate
Figure BDA00028227438900000510
The method comprises the following specific steps:
constructing a low-pass filter and a high-pass filter of single parameter and second order by applying first-order element square according to the principle that the sum of transfer functions is 1, wherein the transfer function of the low-pass filter is FlThe transfer function of the high-pass filter is Fh；
Low pass filter estimates course angle
Figure BDA00028227438900000511
Pitch angle estimate
Figure BDA00028227438900000512
And roll angle estimate
Figure BDA00028227438900000513
For input, the high pass filter uses the course angle estimate
Figure BDA00028227438900000514
Pitch angle estimate
Figure BDA00028227438900000515
Roll angle estimate
Figure BDA00028227438900000516
Adding the estimation result of the attitude heading reference parameters processed by the low-pass filter and the estimation result of the attitude heading reference parameters processed by the high-pass filter for input, wherein the estimation results comprise:
Figure BDA00028227438900000517
Figure BDA00028227438900000518
Figure BDA00028227438900000519
the course angle estimated value is expressed by the formulas (15) - (17)
Figure BDA00028227438900000520
Pitch angle estimate
Figure BDA00028227438900000521
And roll angle estimate
Figure BDA0002822743890000061
The transfer functions of the low-pass filter and the high-pass filter are respectively expressed as:
Figure BDA0002822743890000062
Figure BDA0002822743890000063
in the formula, τ is a frequency domain characteristic adjusting parameter, and s is a complex frequency domain variable.
Compared with the prior art, the invention has the advantages and positive effects that:
(1) in the attitude heading estimating process, the data of the accelerometer, the gyroscope and the magnetic sensor are processed, attitude heading parameters are estimated by adopting a vector method and an integral method respectively, and meanwhile, the estimation result of the vector method and the estimation result of the integral method are combined and estimated through a complementary filter, so that the attitude heading parameter estimation in three modes is realized, the three modes correspond to different sensor combinations, different working conditions can be applied, and the attitude heading estimating effect is good.
(2) The invention processes the data measured by the accelerometer and the magnetic sensor, and improves the estimation effect of high-frequency-band parameters.
(3) The invention applies the single-parameter second-order complementary filter, only adjusts one parameter when adjusting the frequency domain characteristic of the complementary filter, has simple operation and less adjustment parameters.
Drawings
FIG. 1 is a flowchart of a method for estimating attitude parameters according to an embodiment of the present invention;
FIG. 2 is a schematic diagram of a course angle estimation result according to an embodiment of the present invention;
FIG. 3 is a diagram illustrating a pitch angle estimation result according to an embodiment of the present invention;
fig. 4 is a schematic diagram of a roll angle estimation result according to an embodiment of the present invention.
Detailed Description
The invention is described in detail below by way of exemplary embodiments. It should be understood, however, that elements, structures and features of one embodiment may be beneficially incorporated in other embodiments without further recitation.
Referring to fig. 1, the present invention provides a method for estimating attitude parameters, which has three working modes, namely: the measured data of the accelerometer and the magnetic sensor are used as input, a vector method is adopted to process the measured data and the data by utilizing a vector coordinate transformation relation, and a pitch angle estimated value is obtained through calculation
Figure BDA0002822743890000071
Roll angle estimate
Figure BDA0002822743890000072
Course angle estimation
Figure BDA0002822743890000073
And estimating the attitude heading reference. And a second working mode: the gyro measurement data is used as input, the attitude heading quaternion at each moment is gradually calculated from the attitude heading initial value by using an integral method and an attitude heading quaternion dynamic recursion equation, and a heading angle estimation value is calculated by using an attitude heading parameter conversion relation
Figure BDA0002822743890000074
Pitch angle estimate
Figure BDA0002822743890000075
Roll angle estimationEvaluating value
Figure BDA0002822743890000076
And estimating the attitude heading reference. And a third working mode: using the measured data of the accelerometer, the gyroscope and the magnetic sensor as input, and using the complementary filter to combine the estimation results of the vector method and the integral method to obtain a combined estimation result of the attitude heading parameters, namely a heading angle estimation value
Figure BDA0002822743890000077
Pitch angle estimate
Figure BDA0002822743890000078
And roll angle estimate
Figure BDA0002822743890000079
And estimating the attitude heading reference. The three modes of operation of the above method are described in detail below.
Estimating the attitude heading reference parameters by adopting a vector method (namely a first working mode), and specifically comprising the following steps:
and S1, acquiring an accelerometer measurement value a and a magnetic sensor measurement value m.
S2, calculating the pitch angle estimated value by the gravity acceleration vector in the reference coordinate system by using the accelerometer measured value a and the magnetic sensor measured value m as input and utilizing the vector coordinate transformation relation
Figure BDA00028227438900000710
And roll angle estimate
Figure BDA00028227438900000711
Converting the geomagnetic vector in the object coordinate system to a horizontal coordinate system, and calculating a heading angle estimation value by using the geomagnetic vector in the reference coordinate system and the geomagnetic vector in the horizontal coordinate system
Figure BDA00028227438900000712
And finishing the estimation of the attitude and heading parameters.
Specifically, pitch is calculated using vector coordinate transformationAngle estimation value
Figure BDA00028227438900000713
Roll angle estimate
Figure BDA00028227438900000714
And course angle estimation
Figure BDA00028227438900000715
The method comprises the following specific steps:
let the reference coordinate system be or-xryrzrCoordinate system of object is ob-xbybzbThe coordinate of the space vector in the reference coordinate system is vrThe coordinate in the object coordinate system is vbThen, the transformation relationship between the two coordinates is:
Figure BDA0002822743890000081
in the formula (I), the compound is shown in the specification,
Figure BDA0002822743890000082
a transformation matrix from a reference coordinate system to an object coordinate system;
construction of transformation matrices using euler angles
Figure BDA0002822743890000083
Expressed as:
Figure BDA0002822743890000084
where cos represents a cosine function, sin represents a sine function, ψ is a course angle,
Figure BDA0002822743890000085
is pitch angle, gamma is roll angle, course angle psi, pitch angle
Figure BDA0002822743890000086
Roll with transverse rollerThe angle gamma forms a group of Euler angles;
let the gravity acceleration vector in the reference coordinate system be ar,ar＝[0 0 9.8]Geomagnetic vector is mrAnd the geomagnetic model is calculated to obtain the following data:
Figure BDA0002822743890000087
substituting equation (2) into equation (3) yields:
Figure BDA0002822743890000088
Figure BDA0002822743890000089
in the formula, axIs the x-axis component of the accelerometer measurement a, ayIs the y-axis component of the accelerometer measurement a, azIs the z-axis component of the accelerometer measurement a;
using pitch angle estimates
Figure BDA00028227438900000810
And roll angle estimate
Figure BDA00028227438900000811
Transformation matrix from construct coordinate system to horizontal coordinate system
Figure BDA00028227438900000812
Transformation matrix
Figure BDA00028227438900000813
Expressed as:
Figure BDA00028227438900000814
calculating the geomagnetic vector m in the object coordinate systembIs transformed intoGeomagnetic vector m in horizontal coordinate systempThen, there are:
Figure BDA00028227438900000815
substituting equation (6) into equation (7) yields:
Figure BDA00028227438900000816
in the formula, mpxAs a geomagnetic vector mpX-axis component of (1), mpyAs a geomagnetic vector mpY-axis component of (1), mrxAs a geomagnetic vector mrX-axis component of (1), mryAs a geomagnetic vector mrA y-axis component of;
calculating to obtain the pitch angle estimated value by the formula (4), the formula (5) and the formula (8)
Figure BDA0002822743890000091
Roll angle estimate
Figure BDA0002822743890000092
Course angle estimation
Figure BDA0002822743890000093
Specifically, before vector coordinate transformation is carried out on the accelerometer measured value a and the magnetic sensor measured value m, frequency domain compensation is respectively carried out on the accelerometer measured value a and the magnetic sensor measured value m through a compensation filter. Specifically, the specific method for performing frequency domain compensation by the compensation filter is as follows: inputting the accelerometer measured value a into a compensation filter, wherein the output of the compensation filter is the compensated accelerometer measured value a, inputting the magnetic sensor measured value m into the compensation filter, the output of the compensation filter is the compensated accelerometer measured value m, and the transfer function of the compensation filter is (T)1s+1)/(T2s +1), in application, T can be determined according to the measurement characteristics of the accelerometer and the magnetic sensor1And T2And (4) taking values. By compensation filteringThe device function carries out frequency domain compensation on the measurement information, compensates information loss, effectively inhibits measurement noise, improves a high-frequency parameter estimation result, and further improves a navigation attitude estimation effect.
Estimating the attitude heading reference parameters by adopting an integral method (namely a second working mode), and specifically comprising the following steps:
and S1, acquiring a gyro measurement value omega.
S2, taking a gyro measurement value omega as input, constructing a quaternion recursion calculation model by using a navigation attitude quaternion dynamic recursion equation, calculating a navigation attitude quaternion at each moment, and calculating a course angle estimation value by using the navigation attitude quaternion
Figure BDA0002822743890000094
Pitch angle estimate
Figure BDA0002822743890000095
And roll angle estimate
Figure BDA0002822743890000096
Specifically, heading angle estimation value is calculated by using attitude quaternion
Figure BDA0002822743890000097
Pitch angle estimate
Figure BDA0002822743890000098
And roll angle estimate
Figure BDA0002822743890000099
The method comprises the following specific steps:
taking a gyro measurement value omega as input, and constructing a quaternion recursion calculation model by utilizing a navigation attitude quaternion dynamic recursion equation, wherein the quaternion recursion calculation model is expressed as follows:
Figure BDA00028227438900000910
in the formula, qkIs a quaternion of k sampling instants, qk+1Sample k +1Quaternion of time, k is the sampling time, k is 0,1,2,3, Δ t is the sampling interval, ω iskFor the gyro measurement at the time of k sampling, [ omega ]k×]For gyro measurements omegakAn antisymmetric matrix of (a);
construction of transformation matrices using quaternions
Figure BDA0002822743890000101
Expressed as:
Figure BDA0002822743890000102
in the formula, q0Real number component of quaternion, q1The first component being the imaginary part of the quaternion, q2A second component being the imaginary part of the quaternion, q3A third component that is an imaginary part of the quaternion;
simultaneous equations (2) and (4) yield:
Figure BDA0002822743890000103
Figure BDA0002822743890000104
Figure BDA0002822743890000105
substituting quaternion obtained by calculation of formula (9) into formulas (12) - (14) to obtain course angle estimated value
Figure BDA0002822743890000106
Pitch angle estimate
Figure BDA0002822743890000107
Roll angle estimate
Figure BDA0002822743890000108
The method comprises the following steps of (1) using a complementary filter to combine estimation results of a vector method and an integral method (namely a third working mode) to estimate attitude heading parameters, wherein the method specifically comprises the following steps:
and S1, acquiring an accelerometer measurement value a, a magnetic sensor measurement value m and a gyro measurement value omega.
S2, calculating the pitch angle estimated value by the gravity acceleration vector in the reference coordinate system by using the accelerometer measured value a and the magnetic sensor measured value m as input and utilizing the vector coordinate transformation relation
Figure BDA0002822743890000109
And roll angle estimate
Figure BDA00028227438900001010
Converting the geomagnetic vector in the object coordinate system to a horizontal coordinate system, and calculating a heading angle estimation value by using the geomagnetic vector in the reference coordinate system and the geomagnetic vector in the horizontal coordinate system
Figure BDA00028227438900001011
And finishing the estimation of the attitude and heading parameters.
Specifically, a pitch angle estimate is calculated using vector coordinate transformation
Figure BDA00028227438900001012
Roll angle estimate
Figure BDA0002822743890000111
And course angle estimation
Figure BDA0002822743890000112
The method comprises the following specific steps:
let the reference coordinate system be or-xryrzrCoordinate system of object is ob-xbybzbThe coordinate of the space vector in the reference coordinate system is vrThe coordinate in the object coordinate system is vbThen, the transformation relationship between the two coordinates is:
Figure BDA0002822743890000113
in the formula (I), the compound is shown in the specification,
Figure BDA0002822743890000114
a transformation matrix from a reference coordinate system to an object coordinate system;
construction of transformation matrices using euler angles
Figure BDA0002822743890000115
Expressed as:
Figure BDA0002822743890000116
where cos represents a cosine function, sin represents a sine function, ψ is a course angle,
Figure BDA0002822743890000117
is pitch angle, gamma is roll angle, course angle psi, pitch angle
Figure BDA0002822743890000118
The transverse rolling angle gamma forms a group of Euler angles;
let the gravity acceleration vector in the reference coordinate system be ar,ar＝[0 0 9.8]Geomagnetic vector is mrAnd the geomagnetic model is calculated to obtain the following data:
Figure BDA0002822743890000119
substituting equation (2) into equation (3) yields:
Figure BDA00028227438900001110
Figure BDA00028227438900001111
in the formula, axIs the x-axis component of the accelerometer measurement a, ayIs the y-axis component of the accelerometer measurement a, azIs the z-axis component of the accelerometer measurement a;
using pitch angle estimates
Figure BDA00028227438900001112
And roll angle estimate
Figure BDA00028227438900001113
Transformation matrix from construct coordinate system to horizontal coordinate system
Figure BDA00028227438900001114
Transformation matrix
Figure BDA00028227438900001115
Expressed as:
Figure BDA00028227438900001116
calculating the geomagnetic vector m in the object coordinate systembTransformed into a geomagnetic vector m in a horizontal coordinate systempThen, there are:
Figure BDA0002822743890000121
substituting equation (6) into equation (7) yields:
Figure BDA0002822743890000122
in the formula, mpxAs a geomagnetic vector mpX-axis component of (1), mpyAs a geomagnetic vector mpY-axis component of (1), mrxAs a geomagnetic vector mrX-axis component of (1), mryAs a geomagnetic vector mrA y-axis component of;
is prepared from formula (4), formula (5) and formulaCalculating to obtain a pitch angle estimated value by the formula (8)
Figure BDA0002822743890000123
Roll angle estimate
Figure BDA0002822743890000124
Course angle estimation
Figure BDA0002822743890000125
Specifically, before vector coordinate transformation is carried out on the accelerometer measured value a and the magnetic sensor measured value m, frequency domain compensation is respectively carried out on the accelerometer measured value a and the magnetic sensor measured value m through a compensation filter. Specifically, the specific method for performing frequency domain compensation by the compensation filter is as follows: inputting the accelerometer measured value a into a compensation filter, wherein the output of the compensation filter is the compensated accelerometer measured value a, inputting the magnetic sensor measured value m into the compensation filter, the output of the compensation filter is the compensated accelerometer measured value m, and the transfer function of the compensation filter is (T)1s+1)/(T2s +1), in application, T can be determined according to the measurement characteristics of the accelerometer and the magnetic sensor1And T2And (4) taking values. The measurement information is subjected to frequency domain compensation through the compensation filter function, information loss is compensated, measurement noise is effectively suppressed, the high-frequency parameter estimation result is improved, and the attitude and heading estimation effect is further improved.
S3, taking a gyro measurement value omega as input, constructing a quaternion recursion calculation model by using a navigation attitude quaternion dynamic recursion equation, calculating a navigation attitude quaternion at each moment, and calculating a course angle estimation value by using the navigation attitude quaternion
Figure BDA0002822743890000126
Pitch angle estimate
Figure BDA0002822743890000127
And roll angle estimate
Figure BDA0002822743890000128
Specifically, heading angle estimation value is calculated by using attitude quaternion
Figure BDA0002822743890000129
Pitch angle estimate
Figure BDA00028227438900001210
And roll angle estimate
Figure BDA00028227438900001211
The method comprises the following specific steps:
taking a gyro measurement value omega as input, and constructing a quaternion recursion calculation model by utilizing a navigation attitude quaternion dynamic recursion equation, wherein the quaternion recursion calculation model is expressed as follows:
Figure BDA0002822743890000131
in the formula, qkIs a quaternion of k sampling instants, qk+1Is the quaternion of k +1 sampling time, k is the sampling time, k is 0,1,2,3, Δ t is the sampling interval, ω iskFor the gyro measurement at the time of k sampling, [ omega ]k×]For gyro measurements omegakAn antisymmetric matrix of (a);
construction of transformation matrices using quaternions
Figure BDA0002822743890000132
Expressed as:
Figure BDA0002822743890000133
in the formula, q0Real number component of quaternion, q1The first component being the imaginary part of the quaternion, q2A second component being the imaginary part of the quaternion, q3A third component that is an imaginary part of the quaternion;
simultaneous equations (2) and (4) yield:
Figure BDA0002822743890000134
Figure BDA0002822743890000135
Figure BDA0002822743890000136
substituting quaternion obtained by calculation of formula (9) into formulas (12) - (14) to obtain course angle estimated value
Figure BDA0002822743890000137
Pitch angle estimate
Figure BDA0002822743890000138
Roll angle estimate
Figure BDA0002822743890000139
S4, respectively constructing a low-pass filter and a high-pass filter according to the transfer function and the principle that the transfer function is 1, wherein the low-pass filter uses the course angle estimated value
Figure BDA00028227438900001310
Pitch angle estimate
Figure BDA00028227438900001311
And roll angle estimate
Figure BDA00028227438900001312
For input, the high pass filter uses the course angle estimate
Figure BDA00028227438900001313
Pitch angle estimate
Figure BDA00028227438900001314
Roll angle estimate
Figure BDA00028227438900001315
For input, the estimation result of the attitude heading parameters processed by the low-pass filter and the estimation result of the attitude heading parameters processed by the high-pass filter are added to obtain an estimation value of a heading angle
Figure BDA00028227438900001316
Pitch angle estimate
Figure BDA00028227438900001317
And roll angle estimate
Figure BDA00028227438900001318
Specifically, a course angle estimation value is obtained
Figure BDA00028227438900001319
Pitch angle estimate
Figure BDA00028227438900001320
And roll angle estimate
Figure BDA00028227438900001321
The method comprises the following specific steps:
constructing a low-pass filter and a high-pass filter of single parameter and second order by applying first-order element square according to the principle that the sum of transfer functions is 1, wherein the transfer function of the low-pass filter is FlThe transfer function of the high-pass filter is Fh；
Low pass filter estimates course angle
Figure BDA0002822743890000141
Pitch angle estimate
Figure BDA0002822743890000142
And roll angle estimate
Figure BDA0002822743890000143
For input, the high pass filter uses the course angle estimate
Figure BDA0002822743890000144
Pitch angle estimate
Figure BDA0002822743890000145
Roll angle estimate
Figure BDA0002822743890000146
Adding the estimation result of the attitude heading reference parameters processed by the low-pass filter and the estimation result of the attitude heading reference parameters processed by the high-pass filter for input, wherein the estimation results comprise:
Figure BDA0002822743890000147
Figure BDA0002822743890000148
Figure BDA0002822743890000149
calculating course angle estimated value by formulas (15) - (17)
Figure BDA00028227438900001410
Pitch angle estimate
Figure BDA00028227438900001411
And roll angle estimate
Figure BDA00028227438900001412
The transfer functions of the low-pass filter and the high-pass filter are respectively expressed as:
Figure BDA00028227438900001413
Figure BDA00028227438900001414
in the formula, τ is a frequency domain characteristic adjusting parameter, and s is a complex frequency domain variable.
The estimation effect of the above method is explained below with specific examples. Setting an acceleration frequency domain compensation parameter T1Is 0.18, T20.05, frequency domain compensation parameter T of magnetic sensor1Is 0.27, T2Is 0.05 and the complementary filter parameter tau is 0.8. Taking an example of acquiring measurement data of an accelerometer, a magnetic sensor and a gyroscope through a BWT901CL type sensor (including the accelerometer, the gyroscope and the magnetic sensor), the attitude heading estimation result is shown in fig. 2-4, in the figure, a dotted line is the attitude heading estimation result of the first working mode, and a dotted line is the attitude heading estimation result of the second working mode, so that the attitude heading estimation result of the third working mode is realized.
The above-described embodiments are intended to illustrate rather than to limit the invention, and any modifications and variations of the present invention are possible within the spirit and scope of the claims.

Claims (6)

1. A navigation attitude parameter estimation method is characterized by comprising the following specific steps:
acquiring an accelerometer measurement value a, a magnetic sensor measurement value m and a gyroscope measurement value omega;
taking an accelerometer measured value a and a magnetic sensor measured value m as input, calculating a pitch angle estimated value by a gravity acceleration vector in a reference coordinate system by utilizing a vector coordinate transformation relation
Figure FDA0002822743880000011
And roll angle estimate
Figure FDA0002822743880000012
Converting the geomagnetic vector in the object coordinate system to a horizontal coordinate system, and calculating a heading angle estimation value by using the geomagnetic vector in the reference coordinate system and the geomagnetic vector in the horizontal coordinate system
Figure FDA0002822743880000013
Taking a gyro measurement value omega as input, constructing a quaternion recursion calculation model by using a navigation attitude quaternion dynamic recursion equation, calculating navigation attitude quaternion at each moment, and calculating a course angle estimation value by using the navigation attitude quaternion
Figure FDA0002822743880000014
Pitch angle estimate
Figure FDA0002822743880000015
And roll angle estimate
Figure FDA0002822743880000016
Respectively constructing a low-pass filter and a high-pass filter according to the transfer function sum being 1, wherein the low-pass filter uses a course angle estimated value
Figure FDA0002822743880000017
Pitch angle estimate
Figure FDA0002822743880000018
And roll angle estimate
Figure FDA0002822743880000019
For input, the high pass filter uses the course angle estimate
Figure FDA00028227438800000110
Pitch angle estimate
Figure FDA00028227438800000111
Roll angle estimate
Figure FDA00028227438800000112
For input, the estimation result of the attitude heading parameters processed by the low-pass filter and the estimation result of the attitude heading parameters processed by the high-pass filter are added to obtain an estimation value of a heading angle
Figure FDA00028227438800000113
Pitch angle estimate
Figure FDA00028227438800000114
And roll angle estimate
Figure FDA00028227438800000115
2. The attitude heading parameter estimation method of claim 1, wherein the pitch angle estimate is calculated using vector coordinate transformation
Figure FDA00028227438800000116
Roll angle estimate
Figure FDA00028227438800000117
And course angle estimation
Figure FDA00028227438800000118
The method comprises the following specific steps:
let the reference coordinate system be or-xryrzrCoordinate system of object is ob-xbybzbThe coordinate of the space vector in the reference coordinate system is vrThe coordinate in the object coordinate system is vbThen, the transformation relationship between the two coordinates is:
Figure FDA00028227438800000119
in the formula (I), the compound is shown in the specification,
Figure FDA00028227438800000120
a transformation matrix from a reference coordinate system to an object coordinate system;
construction of transformation matrices using euler angles
Figure FDA00028227438800000121
Expressed as:
Figure FDA0002822743880000021
where cos represents a cosine function, sin represents a sine function, ψ is a course angle,
Figure FDA0002822743880000022
is pitch angle, gamma is roll angle, course angle psi, pitch angle
Figure FDA0002822743880000023
The transverse rolling angle gamma forms a group of Euler angles;
let the gravity acceleration vector in the reference coordinate system be ar,ar＝[0 0 9.8]Geomagnetic vector is mrAnd the geomagnetic model is calculated to obtain the following data:
Figure FDA0002822743880000024
substituting equation (2) into equation (3) yields:
Figure FDA0002822743880000025
Figure FDA0002822743880000026
in the formula, axIs the x-axis component of the accelerometer measurement a, ayIs the y-axis component of the accelerometer measurement a, azIs the z-axis component of the accelerometer measurement a;
using pitch angle estimates
Figure FDA0002822743880000027
And roll angle estimate
Figure FDA0002822743880000028
Transformation matrix from construct coordinate system to horizontal coordinate system
Figure FDA0002822743880000029
Transformation matrix
Figure FDA00028227438800000210
Expressed as:
Figure FDA00028227438800000211
calculating the geomagnetic vector m in the object coordinate systembTransformed into a geomagnetic vector m in a horizontal coordinate systempThen, there are:
Figure FDA00028227438800000212
substituting equation (6) into equation (7) yields:
Figure FDA00028227438800000213
in the formula, mpxAs a geomagnetic vector mpX-axis component of (1), mpyAs a geomagnetic vector mpY-axis component of (1), mrxAs a geomagnetic vector mrX-axis component of (1), mryAs a geomagnetic vector mrA y-axis component of;
calculating to obtain the pitch angle estimated value by the formula (4), the formula (5) and the formula (8)
Figure FDA0002822743880000031
Roll angle estimate
Figure FDA0002822743880000032
Course angle estimation
Figure FDA0002822743880000033
3. A method for estimating attitude heading reference according to claim 2, wherein the accelerometer measurement a and the magnetic sensor measurement m are frequency-domain compensated by compensation filters before being subjected to vector coordinate transformation.
4. A method for estimating attitude heading reference parameters according to claim 3, wherein the specific method for performing frequency domain compensation by the compensation filter is: inputting the accelerometer measured value a into a compensation filter, wherein the output of the compensation filter is the compensated accelerometer measured value a, inputting the magnetic sensor measured value m into the compensation filter, the output of the compensation filter is the compensated accelerometer measured value m, and the transfer function of the compensation filter is (T)1s+1)/(T2s+1)。
5. The method of claim 2, wherein the heading angle estimate is calculated using heading quaternions
Figure FDA0002822743880000034
Pitch angle estimate
Figure FDA0002822743880000035
And roll angle estimate
Figure FDA0002822743880000036
The method comprises the following specific steps:
taking a gyro measurement value omega as input, and constructing a quaternion recursion calculation model by utilizing a navigation attitude quaternion dynamic recursion equation, wherein the quaternion recursion calculation model is expressed as follows:
Figure FDA0002822743880000037
in the formula, qkIs a quaternion of k sampling instants, qk+1Is the quaternion of k +1 sampling time, k is the sampling time, k is 0,1,2,3, Δ t is the sampling interval, ω iskFor the gyro measurement at the time of k sampling, [ omega ]k×]For gyro measurements omegakAn antisymmetric matrix of (a);
construction of transformation matrices using quaternions
Figure FDA0002822743880000038
Expressed as:
Figure FDA0002822743880000039
in the formula, q0Real number component of quaternion, q1The first component being the imaginary part of the quaternion, q2A second component being the imaginary part of the quaternion, q3A third component that is an imaginary part of the quaternion;
simultaneous equations (2) and (4) yield:
Figure FDA0002822743880000041
Figure FDA0002822743880000042
Figure FDA0002822743880000043
substituting quaternion obtained by calculation of formula (9) into formulas (12) - (14) to obtain course angle estimated value
Figure FDA0002822743880000044
Pitch angle estimate
Figure FDA0002822743880000045
Roll angle estimate
Figure FDA0002822743880000046
6. A method for estimating attitude heading parameters according to claim 5, wherein the estimate of the heading angle is obtained
Figure FDA0002822743880000047
Pitch angle estimate
Figure FDA0002822743880000048
And roll angle estimate
Figure FDA0002822743880000049
The method comprises the following specific steps:
constructing a low-pass filter and a high-pass filter of single parameter and second order by applying first-order element square according to the principle that the sum of transfer functions is 1, wherein the transfer function of the low-pass filter is FlThe transfer function of the high-pass filter is Fh；
Low pass filter estimates course angle
Figure FDA00028227438800000410
Pitch angle estimate
Figure FDA00028227438800000411
And roll angle estimate
Figure FDA00028227438800000412
For input, the high pass filter uses the course angle estimate
Figure FDA00028227438800000413
Pitch angle estimate
Figure FDA00028227438800000414
Roll angle estimate
Figure FDA00028227438800000415
Adding the estimation result of the attitude heading reference parameters processed by the low-pass filter and the estimation result of the attitude heading reference parameters processed by the high-pass filter for input, wherein the estimation results comprise:
Figure FDA00028227438800000416
Figure FDA00028227438800000417
Figure FDA00028227438800000418
the course angle estimated value is expressed by the formulas (15) - (17)
Figure FDA00028227438800000419
Pitch angle estimate
Figure FDA00028227438800000420
And roll angle estimate
Figure FDA00028227438800000421
The transfer functions of the low-pass filter and the high-pass filter are respectively expressed as:
Figure FDA00028227438800000422
Figure FDA0002822743880000051
in the formula, τ is a frequency domain characteristic adjusting parameter, and s is a complex frequency domain variable.
CN202011421921.7A 2020-12-08 2020-12-08 Attitude heading parameter estimation method Active CN112710309B (en)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202011421921.7A CN112710309B (en) 2020-12-08 2020-12-08 Attitude heading parameter estimation method

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202011421921.7A CN112710309B (en) 2020-12-08 2020-12-08 Attitude heading parameter estimation method

Publications (2)

Publication Number Publication Date
CN112710309A true CN112710309A (en) 2021-04-27
CN112710309B CN112710309B (en) 2023-03-28

Family

ID=75542695

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202011421921.7A Active CN112710309B (en) 2020-12-08 2020-12-08 Attitude heading parameter estimation method

Country Status (1)

Country Link
CN (1) CN112710309B (en)

Cited By (3)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN112859139A (en) * 2019-11-28 2021-05-28 中移物联网有限公司 Attitude measurement method and device and electronic equipment
CN112985380A (en) * 2021-05-14 2021-06-18 中国石油大学胜利学院 Attitude and heading calculation method based on incomplete measurement vector
CN115420306A (en) * 2022-11-07 2022-12-02 浙江芯昇电子技术有限公司 Digital filtering mode-based gyroscope temperature drift compensation implementation method and system

Citations (11)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US20030128780A1 (en) * 2002-01-08 2003-07-10 Communications Res. Lab., Ind. Admin. Inst. Transmission method with fading distortion or frequency offset compensation
US20060070424A1 (en) * 2004-10-04 2006-04-06 Mts Systems Corporation Transducer acceleration compensation with frequency domain amplitude and/or phase compensation
WO2012167367A1 (en) * 2011-06-09 2012-12-13 Trusted Positioning Inc. Method and apparatus for real-time positioning and navigation of a moving platform
CN103956755A (en) * 2014-04-23 2014-07-30 国家电网公司 Design method for power system stabilizer capable of suppressing ultra-low frequency oscillation
CN104197927A (en) * 2014-08-20 2014-12-10 江苏科技大学 Real-time navigation system and real-time navigation method for underwater structure detection robot
CN105890593A (en) * 2016-04-06 2016-08-24 浙江大学 MEMS inertial navigation system and track reconstruction method based on same
CN106525079A (en) * 2016-11-29 2017-03-22 北京科技大学 Three-axis magnetic sensor calibration method and device
CN106679649A (en) * 2016-12-12 2017-05-17 浙江大学 Hand movement tracking system and tracking method
CN107478223A (en) * 2016-06-08 2017-12-15 南京理工大学 A kind of human body attitude calculation method based on quaternary number and Kalman filtering
CN108827299A (en) * 2018-03-29 2018-11-16 南京航空航天大学 A kind of attitude of flight vehicle calculation method based on improvement quaternary number second order complementary filter
US20200109948A1 (en) * 2018-10-03 2020-04-09 Furuno Electric Co., Ltd. Navigation device, method of generating navigation support information, and navigation support information generating program

Patent Citations (11)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US20030128780A1 (en) * 2002-01-08 2003-07-10 Communications Res. Lab., Ind. Admin. Inst. Transmission method with fading distortion or frequency offset compensation
US20060070424A1 (en) * 2004-10-04 2006-04-06 Mts Systems Corporation Transducer acceleration compensation with frequency domain amplitude and/or phase compensation
WO2012167367A1 (en) * 2011-06-09 2012-12-13 Trusted Positioning Inc. Method and apparatus for real-time positioning and navigation of a moving platform
CN103956755A (en) * 2014-04-23 2014-07-30 国家电网公司 Design method for power system stabilizer capable of suppressing ultra-low frequency oscillation
CN104197927A (en) * 2014-08-20 2014-12-10 江苏科技大学 Real-time navigation system and real-time navigation method for underwater structure detection robot
CN105890593A (en) * 2016-04-06 2016-08-24 浙江大学 MEMS inertial navigation system and track reconstruction method based on same
CN107478223A (en) * 2016-06-08 2017-12-15 南京理工大学 A kind of human body attitude calculation method based on quaternary number and Kalman filtering
CN106525079A (en) * 2016-11-29 2017-03-22 北京科技大学 Three-axis magnetic sensor calibration method and device
CN106679649A (en) * 2016-12-12 2017-05-17 浙江大学 Hand movement tracking system and tracking method
CN108827299A (en) * 2018-03-29 2018-11-16 南京航空航天大学 A kind of attitude of flight vehicle calculation method based on improvement quaternary number second order complementary filter
US20200109948A1 (en) * 2018-10-03 2020-04-09 Furuno Electric Co., Ltd. Navigation device, method of generating navigation support information, and navigation support information generating program

Non-Patent Citations (7)

* Cited by examiner, † Cited by third party
Title
E.HEDBERG，J.NORÉN*M.NORRLÖF，S.GUNNARSSON: "《Industrial Robot Tool Position Estimation using Inertial Measurements in a Complementary Filter and an EKF》" *
王巍;尹艳玲;刘凇佐;王;乔钢;: "基于频域变采样的OFDM水声移动通信多普勒补偿算法" *
石岗，李希胜等: "《磁传感器输出姿态信息修正方法研究》", 《仪器仪表学报》 *
赵英伟;王省书;黄宗升;吴伟;刘士伟;: "基于LabVIEW的音圈电机驱动小角度转台系统辨识方法" *
陈亮，杨柳庆等: "《基于梯度下降法和互补滤波的航向姿态参考系统》", 《电子设计工程》 *
陈光武，李少远等: "《基于递推最小二乘与互补滤波的姿态估计》", 《控制理论与应用》 *
黄镇，张浩磊等: "《一种二阶互补滤波与卡尔曼滤波的姿态解算方法设计》", 《电子工艺技术》 *

Cited By (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN112859139A (en) * 2019-11-28 2021-05-28 中移物联网有限公司 Attitude measurement method and device and electronic equipment
CN112859139B (en) * 2019-11-28 2023-09-05 中移物联网有限公司 Gesture measurement method and device and electronic equipment
CN112985380A (en) * 2021-05-14 2021-06-18 中国石油大学胜利学院 Attitude and heading calculation method based on incomplete measurement vector
CN112985380B (en) * 2021-05-14 2021-07-27 中国石油大学胜利学院 Attitude and heading calculation method based on incomplete measurement vector
CN115420306A (en) * 2022-11-07 2022-12-02 浙江芯昇电子技术有限公司 Digital filtering mode-based gyroscope temperature drift compensation implementation method and system

Also Published As

Publication number Publication date
CN112710309B (en) 2023-03-28

Similar Documents

Publication Publication Date Title
CN112710309B (en) Attitude heading parameter estimation method
CN108225308B (en) Quaternion-based attitude calculation method for extended Kalman filtering algorithm
WO2020253854A1 (en) Mobile robot posture angle calculation method
CN108731670B (en) Inertial/visual odometer integrated navigation positioning method based on measurement model optimization
CN110030994B (en) Monocular-based robust visual inertia tight coupling positioning method
WO2017063387A1 (en) Method for updating all attitude angles of agricultural machine on the basis of nine-axis mems sensor
CN108061855B (en) MEMS sensor based spherical motor rotor position detection method
CN111551174A (en) High-dynamic vehicle attitude calculation method and system based on multi-sensor inertial navigation system
CN109931957B (en) Self-alignment method of SINS strapdown inertial navigation system based on LGMKF
CN111462231A (en) Positioning method based on RGBD sensor and IMU sensor
US20170241799A1 (en) Systems and methods to compensate for gyroscope offset
CN110702113B (en) Method for preprocessing data and calculating attitude of strapdown inertial navigation system based on MEMS sensor
CN106052685A (en) Two-stage separation fusion attitude and heading estimation method
CN110567461B (en) Non-cooperative spacecraft attitude and parameter estimation method considering no gyroscope
CN111552293A (en) Mobile robot formation control method based on images under visual field constraint
CN112683269A (en) MARG attitude calculation method with motion acceleration compensation
CN109656132A (en) A kind of robot for space finite time control method for coordinating
CN110861081B (en) Autonomous positioning method for under-constrained cable parallel robot end effector
CN110967017B (en) Cooperative positioning method for rigid body cooperative transportation of double mobile robots
CN110375773B (en) Attitude initialization method for MEMS inertial navigation system
CN108871319B (en) Attitude calculation method based on earth gravity field and earth magnetic field sequential correction
CN107389092B (en) Gyro calibration method based on assistance of magnetic sensor
CN110672127B (en) Real-time calibration method for array type MEMS magnetic sensor
Wöhle et al. A robust quaternion based Kalman filter using a gradient descent algorithm for orientation measurement
CN108426584B (en) Calibration method for multiple sensors of automobile

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