CN107621261B - Adaptive optimal-REQUEST algorithm for inertial-geomagnetic combined attitude solution - Google Patents

Adaptive optimal-REQUEST algorithm for inertial-geomagnetic combined attitude solution Download PDF

Info

Publication number
CN107621261B
CN107621261B CN201710804083.3A CN201710804083A CN107621261B CN 107621261 B CN107621261 B CN 107621261B CN 201710804083 A CN201710804083 A CN 201710804083A CN 107621261 B CN107621261 B CN 107621261B
Authority
CN
China
Prior art keywords
attitude
matrix
calculation
vector
accelerometer
Prior art date
Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
Active
Application number
CN201710804083.3A
Other languages
Chinese (zh)
Other versions
CN107621261A (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.)
Changzhou University
Original Assignee
Changzhou University
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 Changzhou University filed Critical Changzhou University
Priority to CN201710804083.3A priority Critical patent/CN107621261B/en
Publication of CN107621261A publication Critical patent/CN107621261A/en
Application granted granted Critical
Publication of CN107621261B publication Critical patent/CN107621261B/en
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

Landscapes

  • Navigation (AREA)
  • Gyroscopes (AREA)

Abstract

The invention provides a self-adaptive optimal-REQUEST algorithm for inertial-geomagnetic combined attitude calculation, which adopts a self-adaptive adjustment strategy to dynamically adjust the weight of an observation vector of the attitude calculation algorithm, so that the attitude calculation algorithm mainly depends on an accelerometer and a geomagnetic sensor to realize attitude calculation to improve the static accuracy when a carrier is static, and mainly depends on a gyroscope to realize attitude calculation to improve the dynamic accuracy when the carrier moves. The core of the self-adaptive adjustment strategy is to judge whether the included angle of the observed values of the two vectors, namely the gravity acceleration vector and the geomagnetic field vector, is close to a given value, and simultaneously judge whether the modulus of the observed value of the gravity acceleration vector is close to another given value, if the two observed values are close to the given value, the carrier is in a static state, otherwise, the carrier is in a moving state. The two given values can be obtained simply from the measurements of the accelerometer and the geomagnetic sensor when the carrier is stationary.

Description

Adaptive optimal-REQUEST algorithm for inertial-geomagnetic combined attitude solution
Technical Field
The invention relates to the technical field of attitude calculation algorithms, in particular to an adaptive optimal-REQUEST algorithm for inertia-geomagnetic combined attitude calculation.
Background
The inertia-geomagnetic measurement combination comprises a triaxial gyroscope, a triaxial accelerometer and a triaxial geomagnetic sensor, and the attitude and the change of the carrier coordinate system relative to the world coordinate system are determined by sensing the projection of angular velocity vectors, gravity acceleration vectors and geomagnetic field vectors in the three axial directions of the carrier coordinate system. The measurement combination does not need any external source information, so the measurement combination is widely applied to the aspects of human body posture tracking, robots, small unmanned aerial vehicles and the like. The solution algorithm for implementing sensor data fusion to determine the attitude is usually the kalman algorithm (EKF), and the principle of this algorithm is to construct a state equation by using an euler angle differential equation or a quaternion differential equation (acquiring the attitude from gyroscope data), construct a measurement equation by using a Gauss-Newton algorithm, a triac algorithm, a QUEST algorithm, or a vector coordinate system transformation equation (acquiring the attitude from accelerometer and geomagnetic sensor data), and finally perform fusion by using the kalman algorithm (using an extended kalman algorithm if a vector coordinate system transformation equation is used). The fusion brings the following advantages: 1. the state equation fully utilizes historical measurement information of the attitude, and exerts the performance of the gyroscope superior to the combination of the accelerometer and the geomagnetic sensor in the aspect of dynamic attitude measurement; 2. the measurement equation obtains the attitude of the carrier coordinate system relative to the world coordinate system by means of the accelerometer and geomagnetic sensor data at each sampling moment, so that the attitude divergence phenomenon caused by the integral of the measurement error of the gyroscope can be compensated.
In addition to the kalman algorithm (EKF), in recent years, researchers in the related art have proposed a number of attitude solution algorithms, and studies have shown that these algorithms perform better than EKF when state equations or observation equations have very severe non-linearities. Although these algorithms are suitable for the inertia-geomagnetic combination, these algorithms are based on either statistical filtering techniques (such as Particle Filtering (PF) and Unscented Kalman Filtering (UKF) or least square techniques (such as back-smoothing EKF, extended QUEST, two-step attitude estimator), etc., and considering the problems of computation time, sampling rate and processor, the EKF is still the attitude solution algorithm suitable for the inertia-geomagnetic combination and most widely used at present.
The REQUEST algorithm and the optimal-REQUEST algorithm developed on the basis of the QUEST algorithm are mainly applied to the aerospace field and used for realizing attitude determination of space aircrafts such as satellites. If the parameters are set reasonably, the dynamic and static performance of the optimal-REQUEST algorithm is equivalent to that of the extended kalman algorithm (EKF), but the computation time is less, and the computation time of the former is about half that of the latter, so that the former is certainly more suitable for the inertia-geomagnetic combination only provided with an embedded processor.
Disclosure of Invention
In the case of high-speed movement of the carrier, the accelerometer information is rather inaccurate, so the attitude calculation algorithm should discard the accelerometer information in this case, and rely on the gyroscope information alone to realize attitude calculation.
At present, two methods for determining the selection of accelerometer information are available, and both methods use a model of a three-dimensional vector formed by acceleration output as a basis for judgment. The first method is a discrete method with threshold, which continuously records the latest n sampling values of the vector mode, judges whether any one sampling value in the n sampling values is larger than a preset threshold, if yes, the output information of the accelerometer is completely abandoned, otherwise, the information of the accelerometer is completely utilized. The second method is a continuous method without threshold, which does not completely discard the accelerometer information when the carrier is moving, and only records the modulus of the vector at the current moment, multiplies the accelerometer information by a weight representing its utilization rate, and then uses the measurement information for attitude resolution. The first method has low judgment precision when the sampling rate is low and the measurement noise of the accelerometer is large, and the second method still brings large errors to attitude calculation due to incomplete accelerometer information rejection when the carrier linear acceleration is high.
The invention adopts an optimal-REQUEST algorithm as an attitude calculation algorithm, provides a method for adaptively selecting and rejecting accelerometer information, and further organically integrates the method and the optimal-REQUEST algorithm, thereby providing a new attitude calculation algorithm suitable for inertia-geomagnetic combination.
The technical scheme adopted by the invention is as follows: an adaptive optimal-REQUEST algorithm for inertial-geomagnetic combined attitude calculation comprises a gyroscope, an accelerometer and a geomagnetic sensor, wherein a three-dimensional observation vector formed by outputs of the gyroscope, the accelerometer and the geomagnetic sensor is used as an input variable, and the method comprises the following steps of,
the method comprises the following steps: at each sampling instant k, a calculation is made
Figure BDA0001402274560000031
Wherein the content of the first and second substances,
Figure BDA0001402274560000032
and
Figure BDA0001402274560000033
an accelerometer and a geomagnetic sensor respectively at time kOutputting a formed three-dimensional observation vector, wherein the expression of modulus is × expressing the expression of vector product, and asin () expresses the expression of inverse sine;
Figure BDA0001402274560000034
represents
Figure BDA0001402274560000035
And
Figure BDA0001402274560000036
the angle between these two vectors. In the case of a stationary carrier, the accelerometer will only measure the acceleration due to gravity, i.e. without being influenced by the linear acceleration of the carrier
Figure BDA00014022745600000312
The gravity acceleration vector is shown, and the included angle is unchanged because the gravity acceleration vector and the geomagnetic field vector are unchanged in a certain region, namely
Figure BDA0001402274560000037
Is a constant value, which is now denoted as θ (e.g., 47.49 ° in the changzhou region). In the case of a movement of the carrier, the measurement of the accelerometer is a superposition of the acceleration due to gravity and the linear acceleration of the carrier, so that
Figure BDA0001402274560000038
Does not represent the gravitational acceleration vector, which means
Figure BDA0001402274560000039
Not equal to theta, and the larger the deviation of the two, the larger the carrier linear acceleration. Thus, it is possible to provide
Figure BDA00014022745600000310
Can be used as a factor for dynamically adjusting the weight of the observation vector of the optimal-REQUEST algorithm (another factor is represented by step two)
Figure BDA00014022745600000311
)，The degree of dependence on an accelerometer and a geomagnetic sensor is reduced by the optimal-REQUEST algorithm when a carrier moves, so that attitude calculation is realized mainly by the gyroscope, and the attitude calculation precision is improved. It can be said that this step calculates
Figure BDA0001402274560000041
For the purpose of further calculation in step two
Figure BDA0001402274560000042
Preparation is made.
Due to the dual influence of the output noise of the accelerometer and the geomagnetic sensor, the calculation result of the formula (1) contains great noise, and various filtering techniques can be adopted to denoise the calculation result of the formula (1). Obtained by the formula (1)
Figure BDA0001402274560000043
At [ - π/2, π/2]Within the interval, it is necessary to utilize
Figure BDA0001402274560000044
The calculation result of (2) converts the formula (1) into [0, π]Interval, in order to satisfy the requirement of the subsequent step of this technical scheme, wherein. Can but does not suggest to utilize
Figure BDA0001402274560000045
Implementation of
Figure BDA0001402274560000046
Because the classical kalman filtering technique cannot be used to achieve denoising.
Step two: computing
Figure BDA0001402274560000047
Wherein abs (·) represents an absolute value, and g is a local gravity acceleration vector; handle
Figure BDA0001402274560000048
As a dynamic adjustment optimal-REQUEST algorithmThe reason for the other factor of the weight of the observation vector is the same as
Figure BDA0001402274560000049
Similarly, α1And α2To scale factor, the ranges of the two scale factors are analyzed below, first α1Due to the range of values of
Figure BDA00014022745600000410
Therefore α should be taken1> 1, to ensure as long as
Figure BDA00014022745600000411
A slight deviation from the angle theta is provided,
Figure BDA00014022745600000412
it is possible to quickly decay to 0, see the explanation of step three, as to why this value needs to decay to 0.
Analysis α2The value range of (a). Without loss of generality, the three-dimensional vector formed by the output of the accelerometer without noise influence is set as
Figure BDA00014022745600000413
After applying the noise:
Figure BDA00014022745600000414
wherein
Figure BDA00014022745600000415
Is white noise and the variance is the same. Due to the fact that
Figure BDA00014022745600000416
Then
Figure BDA00014022745600000417
Thus, it is possible to provide
Figure BDA0001402274560000051
Where D () represents the variance, it can be seen from equation (5) that 0 < α needs to be taken21/3, so that the presence of acceleration vector measurement noise on pregCausing less impact.
It should be noted that, because
Figure BDA0001402274560000052
Wherein acos () represents an inverted cosine, and is thus obtained from equation (6)
Figure BDA0001402274560000053
As can be seen from the formula (7),
Figure BDA0001402274560000054
loud noise, in addition α1> 1, so the presence of measurement noise will certainly cause pregLarge fluctuations are generated. Step two indicates that the pair must be realized by using various filtering techniques
Figure BDA0001402274560000055
Denoising in (3).
The recommended value of the invention for the two scale factors is α1＝100/π、α2＝0.01。
Calculated in this step
Figure BDA0001402274560000056
The weight of the observation vector of the optimal-REQUEST algorithm is not directly adjusted, and further transformation of the following step three is needed.
Step three: computing
Figure BDA0001402274560000057
As can be seen from the second step, in the second step,
Figure BDA0001402274560000058
has a value range of
Figure BDA0001402274560000059
Thus, it is possible to provide
Figure BDA00014022745600000510
And can not be directly used for adjusting the weight of the observation vector of the optimal-REQUEST algorithm. The purpose of the formula (8) is to
Figure BDA00014022745600000511
The value range is monotonously converted into [0,1 ]]An interval. Obtained
Figure BDA00014022745600000512
Adjustment of the weights of the observation vectors to be used in the optimal-REQUEST algorithm of step four, as can be seen from equation (8), as long as α1And α2Take appropriate values, then once
Figure BDA00014022745600000513
Slightly deviated from theta or
Figure BDA00014022745600000514
Slightly increased to cause
Figure BDA00014022745600000515
The size of the hole is slightly increased, and the hole diameter is slightly increased,
Figure BDA00014022745600000516
it will immediately decay to 0, which is desirable because it can make the optimal-REQUEST algorithm rely mainly on gyroscopes to achieve attitude solution while the vehicle is moving, thereby improving the accuracy of attitude solution.
Step four: obtained according to step three
Figure BDA0001402274560000061
Calculating "innovation" K of gesturekI.e. a 4 × 4 dimensional matrix represented by equation (9)
Figure BDA0001402274560000062
Computing matrix KkThe process of (1) calculating
Figure BDA0001402274560000063
The numerical value represents the sum of weights given to all observation vectors by an optimal-REQUEST algorithm, and is a scalar; (2) according to
Figure BDA0001402274560000064
And m calculated by step (1)kComputing
Figure BDA0001402274560000065
The numerical value is a scalar quantity and has no physical meaning; (3) according to
Figure BDA0001402274560000066
And m calculated by step (1)kComputing
Figure BDA0001402274560000067
The value is a 3 × 3D matrix with no physical significance, (4) calculating S ═ B + B according to the matrix B calculated in step (3)TThe value is still a 3 × 3D matrix with no physical significance (5) based on
Figure BDA0001402274560000068
And m calculated by step (1)kComputing
Figure BDA0001402274560000069
A 3 × 1-dimensional vector having no physical significance, and (6) forming a matrix K according to the formula (9) based on the calculation results of the steps (1) to (5)k. It should be noted that
Figure BDA00014022745600000610
And
Figure BDA00014022745600000611
respectively, the same observation vector is represented in a carrier coordinate system and a world coordinate system. A total of two observation vectors, i.e.
Figure BDA00014022745600000612
And
Figure BDA00014022745600000613
for example, the three-dimensional vector formed by the accelerometer output is
Figure BDA00014022745600000614
Then the vector is represented in the carrier coordinate system
Figure BDA00014022745600000615
And the expression of the vector in the world coordinate system is
Figure BDA00014022745600000616
If the three-dimensional vector is replaced by
Figure BDA00014022745600000617
(formed by geomagnetic sensor output), the representation of the three-dimensional vector under the carrier coordinate system and the world coordinate system is
Figure BDA00014022745600000618
And
Figure BDA00014022745600000619
i in formula (9) is expressed as a 3 × 3-dimensional identity matrix.
The expression (9) means that the weight given to each observation vector of the optimal-REQUEST algorithm is expressed by the expression (8)
Figure BDA00014022745600000620
So that once there is motion of the carrier, then
Figure BDA00014022745600000621
Decays immediately to 0, so that the optimal-REQUEST algorithm no longer worksThe attitude calculation is realized by depending on the observation vector, and is mainly completed by depending on the output of the gyroscope, so that the attitude calculation precision is improved.
Matrix KkWhat is shown is "innovation" for attitude resolution, which is used to implement corrections for attitude "predictions", which are not directly used for attitude resolution, as long as they are not the initial time.
Step five: calculating "predictions" K of posesk/k-1，
If K is 0, i.e. the initial calculation time, since there is no "prediction" of the pose, let K bek/k＝Kk，mk＝mk，Pk/k＝RkWherein R iskTo observe the noise covariance matrix, then jump directly to step seven, using the 4 × 4D matrix Kk/kAnd realizing attitude calculation. m iskFor accumulating the weight sum, m of each time is expressed from the timekAnd accumulating until the current moment. Pk/kA covariance matrix representing the estimation error of the pose post-test estimate. m iskAnd Pk/kAll used for the calculation of step six.
If K is not equal to 0, utilizing the covariance matrix K of the estimation error after the attitude check at the last momentk-1/k-1Computing the "prediction" of attitude, i.e., K in 4 × 4 dimensions, according to equation (10)k/k-1The matrix is a matrix of a plurality of matrices,
Figure BDA0001402274560000071
the attitude "prediction", i.e. the covariance matrix P of the estimation errors of the attitude prior estimate, is then calculated using equation (11)k/k-1，
Figure BDA0001402274560000072
Wherein Q isk-1Is a process noise covariance matrix; phik-1Is a state transition matrix, and phik-1＝exp(Ωk-1Δt)，Ωk-1Is an antisymmetric array omegak-1Is defined as:
Figure BDA0001402274560000073
wherein, ω isk-1Is a three-dimensional vector formed by the output of the gyroscope;
this step has obtained a "prediction" K of the posek/k-1And the last step has also obtained the "innovation" K of the gesturekAnd the next step is to realize the correction of the attitude prediction by using the attitude innovation, namely K is obtainedk/k。
Step six: firstly, the accumulated weight sum m of the last moment is utilizedk-1The weight sum m obtained in the fourth stepkAnd the covariance matrix P of the attitude prior estimation error obtained in the step fivek/k-1Calculating a weighting factor pk，
Figure BDA0001402274560000081
Then, the accumulated weight sum m of the previous moment is utilizedk-1And the weight sum m obtained in the step fourkCalculating the accumulated weight sum m at the current momentk，
mk＝(1-ρk)mk-1+ρkmk(14)
The accumulated weight sum m at the current momentkAnd a weighting factor pkThe correction of the attitude can be realized, and the matrix K is obtainedk/k，
Figure BDA0001402274560000082
Finally, calculating the covariance matrix P of the attitude post-test estimation error at the current momentk/k，
Figure BDA0001402274560000083
Pk/kAnd mkWill be saved for the next round of calculation, since the steps will be repeated the next time comes,and both quantities will be used in step five and step six.
Obtaining Kk/kAfter the matrix, the next step is to calculate the pose at the current time according to the matrix.
Step seven: calculation and matrix Kk/kIs the attitude q expressed in quaternion at the calculation timekAnd finishing the attitude calculation of the current time k. And returning to the step one, and continuing to perform the attitude calculation process at the next moment until the attitude calculation is stopped at a certain moment.
The invention has the beneficial effects that: the self-adaptive optimal-REQUEST algorithm for inertial-geomagnetic combined attitude calculation provided by the invention can reduce the degree of dependence on an accelerometer and a geomagnetic sensor when a carrier moves so as to realize attitude calculation mainly by the gyroscope, and can realize attitude calculation mainly by the accelerometer and the geomagnetic sensor when the carrier is static (because the two sensors have high measurement precision on a gravitational acceleration vector and a geomagnetic field vector), so that the static and dynamic performance of the optimal-REQUEST algorithm is improved.
Drawings
The invention is further illustrated by the following figures and examples.
FIG. 1 is a schematic diagram of a preferred embodiment of the present invention.
Detailed Description
The present invention will now be described in detail with reference to the accompanying drawings. This figure is a simplified schematic diagram, and merely illustrates the basic structure of the present invention in a schematic manner, and therefore it shows only the constitution related to the present invention.
As shown in fig. 1, an adaptive optimal-REQUEST algorithm for combined inertial-geomagnetic attitude solution according to the present invention includes a gyroscope, an accelerometer, and a geomagnetic sensor, uses a three-dimensional observation vector formed by outputs of the gyroscope, the accelerometer, and the geomagnetic sensor as input variables, and includes the following steps,
the method comprises the following steps: at each sampling instant k, a calculation is made
Figure BDA0001402274560000091
Wherein the content of the first and second substances,
Figure BDA0001402274560000092
and
Figure BDA0001402274560000093
the three-dimensional observation vector formed by the output of the accelerometer and the geomagnetic sensor at the time k represents modulus, × represents a vector product, asin () represents an inverse sine;
Figure BDA0001402274560000094
represents
Figure BDA0001402274560000095
And
Figure BDA0001402274560000096
the angle between these two vectors. In the case of a stationary carrier, the accelerometer will only measure the acceleration due to gravity, i.e. without being influenced by the linear acceleration of the carrier
Figure BDA0001402274560000097
The gravity acceleration vector is shown, and the included angle is unchanged because the gravity acceleration vector and the geomagnetic field vector are unchanged in a certain region, namely
Figure BDA0001402274560000098
Is a constant value, which is now denoted as θ (e.g., 47.49 ° in the changzhou region). In the case of a movement of the carrier, the measurement of the accelerometer is a superposition of the acceleration due to gravity and the linear acceleration of the carrier, so that
Figure BDA0001402274560000099
Does not represent the gravitational acceleration vector, which means
Figure BDA0001402274560000101
Not equal to theta, and the larger the deviation of the two, the larger the carrier linear acceleration. Thus, it is possible to provide
Figure BDA0001402274560000102
Can be used as a factor for dynamically adjusting the weight of the observation vector of the optimal-REQUEST algorithm (another factor is represented by step two)
Figure BDA0001402274560000103
) The method has the advantages that the degree of dependence on the accelerometer and the geomagnetic sensor is reduced when the carrier moves by the optimal-REQUEST algorithm, so that the attitude calculation is realized mainly by the gyroscope, and the attitude calculation precision is improved. It can be said that this step calculates
Figure BDA0001402274560000104
For the purpose of further calculation in step two
Figure BDA0001402274560000105
Preparation is made.
Due to the dual influence of the output noise of the accelerometer and the geomagnetic sensor, the calculation result of the formula (1) contains great noise, and various filtering techniques can be adopted to denoise the calculation result of the formula (1). Obtained by the formula (1)
Figure BDA0001402274560000106
At [ - π/2, π/2]Within the interval, it is necessary to utilize
Figure BDA0001402274560000107
The calculation result of (2) converts the formula (1) into [0, π]Interval, in order to satisfy the requirement of the subsequent step of this technical scheme, wherein. Can but does not suggest to utilize
Figure BDA0001402274560000108
Implementation of
Figure BDA0001402274560000109
Because the classical kalman filtering technique cannot be used to achieve denoising.
Step two: computing
Figure BDA00014022745600001010
Wherein abs (·) represents an absolute value, and g is a local gravity acceleration vector; handle
Figure BDA00014022745600001011
The reason for dynamically adjusting the weight of the observation vector of the optimal-REQUEST algorithm is the same as that of the other factor
Figure BDA00014022745600001012
Similarly, α1And α2To scale factor, the ranges of the two scale factors are analyzed below, first α1Due to the range of values of
Figure BDA00014022745600001013
Therefore α should be taken1> 1, to ensure as long as
Figure BDA00014022745600001014
A slight deviation from the angle theta is provided,
Figure BDA00014022745600001015
it is possible to quickly decay to 0, see the explanation of step three, as to why this value needs to decay to 0.
Analysis α2The value range of (a). Without loss of generality, the three-dimensional vector formed by the output of the accelerometer without noise influence is set as
Figure BDA00014022745600001016
After applying the noise:
Figure BDA00014022745600001017
wherein
Figure BDA00014022745600001018
Is white noise and the variance is the same. Due to the fact that
Figure BDA0001402274560000111
Then
Figure BDA0001402274560000112
Thus, it is possible to provide
Figure BDA0001402274560000113
Where D () represents the variance, it can be seen from equation (5) that 0 < α needs to be taken21/3, so that the presence of acceleration vector measurement noise on pregCausing less impact.
It should be noted that, because
Figure BDA0001402274560000114
Wherein acos () represents an inverted cosine, and is thus obtained from equation (6)
Figure BDA0001402274560000115
As can be seen from the formula (7),
Figure BDA0001402274560000116
loud noise, in addition α1> 1, so the presence of measurement noise will certainly cause pregLarge fluctuations are generated. Step two indicates that the pair must be realized by using various filtering techniques
Figure BDA0001402274560000117
Denoising in (3).
The recommended value of the invention for the two scale factors is α1＝100/π、α2＝0.01。
Calculated in this step
Figure BDA0001402274560000118
The weight of the observation vector of the optimal-REQUEST algorithm is not directly adjusted, and further transformation of the following step three is needed.
Step three: computing
Figure BDA0001402274560000119
As can be seen from the second step, in the second step,
Figure BDA00014022745600001110
has a value range of
Figure BDA00014022745600001111
Thus, it is possible to provide
Figure BDA00014022745600001112
And can not be directly used for adjusting the weight of the observation vector of the optimal-REQUEST algorithm. The purpose of the formula (8) is to
Figure BDA00014022745600001113
The value range is monotonously converted into [0,1 ]]An interval. Obtained
Figure BDA0001402274560000121
Adjustment of the weights of the observation vectors to be used in the optimal-REQUEST algorithm of step four, as can be seen from equation (8), as long as α1And α2Take appropriate values, then once
Figure BDA0001402274560000122
Slightly deviated from theta or
Figure BDA0001402274560000123
Slightly increased to cause
Figure BDA0001402274560000124
The size of the hole is slightly increased, and the hole diameter is slightly increased,
Figure BDA0001402274560000125
it will immediately decay to 0, which is desirable because it can make the optimal-REQUEST algorithm rely mainly on gyroscopes to achieve attitude solution while the vehicle is moving, thereby improving the accuracy of attitude solution.
Step four: obtained according to step three
Figure BDA0001402274560000126
A4 × 4D matrix shown in equation (9) is calculated
Figure BDA0001402274560000127
Computing matrix KkThe process of (1) calculating
Figure BDA0001402274560000128
The numerical value represents the sum of weights given to all observation vectors by an optimal-REQUEST algorithm, and is a scalar; (2) according to
Figure BDA0001402274560000129
And m calculated by step (1)kComputing
Figure BDA00014022745600001210
The numerical value is a scalar quantity and has no physical meaning; (3) according to
Figure BDA00014022745600001211
And m calculated by step (1)kComputing
Figure BDA00014022745600001212
The value is a 3 × 3D matrix with no physical significance, (4) calculating S ═ B + B according to the matrix B calculated in step (3)TThe value is still a 3 × 3D matrix with no physical significance (5) based on
Figure BDA00014022745600001213
And m calculated by step (1)kComputing
Figure BDA00014022745600001214
A 3 × 1-dimensional vector having no physical significance, and (6) forming a matrix K according to the formula (9) based on the calculation results of the steps (1) to (5)k. It should be noted that
Figure BDA00014022745600001215
And
Figure BDA00014022745600001216
respectively, the same observation vector is represented in a carrier coordinate system and a world coordinate system. A total of two observation vectors, i.e.
Figure BDA00014022745600001217
And
Figure BDA00014022745600001218
for example, the three-dimensional vector formed by the accelerometer output is
Figure BDA00014022745600001219
Then the vector is represented in the carrier coordinate system
Figure BDA00014022745600001220
And the expression of the vector in the world coordinate system is
Figure BDA00014022745600001221
If the three-dimensional vector is replaced by
Figure BDA00014022745600001222
(formed by geomagnetic sensor output), the representation of the three-dimensional vector under the carrier coordinate system and the world coordinate system is
Figure BDA00014022745600001223
And
Figure BDA00014022745600001224
i in formula (9) is expressed as a 3 × 3-dimensional identity matrix.
The formula (9) meansThe weight value of each observation vector given to the optimal-REQUEST algorithm is expressed by the formula (8)
Figure BDA0001402274560000131
So that once there is motion of the carrier, then
Figure BDA0001402274560000132
And the optical-REQUEST algorithm is immediately attenuated to 0, so that the attitude calculation is not realized by depending on an observation vector any more, but is mainly completed by depending on the output of a gyroscope, and the attitude calculation precision is improved.
Matrix KkWhat is shown is "innovation" for attitude resolution, which is used to implement corrections for attitude "predictions", which are not directly used for attitude resolution, as long as they are not the initial time. If K is 0, i.e. the initial calculation time, since there is no "prediction" of the pose, let K bek/k＝Kk，mk＝mk，Pk/k＝RkWherein R iskTo observe the noise covariance matrix, then jump directly to step seven, using the 4 × 4D matrix Kk/kAnd realizing attitude calculation. m iskFor accumulating the weight sum, m of each time is expressed from the timekAnd accumulating until the current moment. Pk/kA covariance matrix representing the estimation error of the pose post-test estimate. m iskAnd Pk/kAll used for the calculation of step six.
Step five: if K is not equal to 0, utilizing the covariance matrix K of the estimation error after the attitude check at the last momentk-1/k-1Computing the "prediction" of attitude, i.e., K in 4 × 4 dimensions, according to equation (10)k/k-1The matrix is a matrix of a plurality of matrices,
Figure BDA0001402274560000133
the attitude "prediction", i.e. the covariance matrix P of the estimation errors of the attitude prior estimate, is then calculated using equation (11)k/k-1，
Figure BDA0001402274560000134
Wherein Q isk-1Is a process noise covariance matrix; phik-1Is a state transition matrix, and phik-1＝exp(Ωk-1Δt)，Ωk-1Is an antisymmetric array omegak-1Is defined as
Figure BDA0001402274560000135
Wherein, ω isk-1Is a three-dimensional vector formed by the output of the gyroscope;
this step has obtained a "prediction" K of the posek/k-1And the last step has also obtained the "innovation" K of the gesturekAnd the next step is to realize the correction of the attitude prediction by using the attitude innovation, namely K is obtainedk/k。
Step six: firstly, the accumulated weight sum m of the last moment is utilizedk-1The weight sum m obtained in the fourth stepkAnd the covariance matrix P of the attitude prior estimation error obtained in the step fivek/k-1Calculating a weighting factor pk，
Figure BDA0001402274560000141
Then, the accumulated weight sum m of the previous moment is utilizedk-1And the weight sum m obtained in the step fourkCalculating the accumulated weight sum m at the current momentk，
mk＝(1-ρk)mk-1+ρkmk(14)
The accumulated weight sum m at the current momentkAnd a weighting factor pkThe correction of the attitude can be realized, and the matrix K is obtainedk/k，
Figure BDA0001402274560000142
Finally, calculating the covariance matrix P of the attitude post-test estimation error at the current momentk/k，
Figure BDA0001402274560000143
Pk/kAnd mkWill be saved for the next round of calculation since the next moment will come and the above steps will be repeated, and steps five and six will use both quantities.
Obtaining Kk/kAfter the matrix, the next step is to calculate the pose at the current time according to the matrix.
Step seven: calculation and matrix Kk/kIs the attitude q expressed in quaternion at the calculation timekAnd finishing the attitude calculation of the current time k. And returning to the step one, and continuing to perform the attitude calculation process at the next moment until the attitude calculation is stopped at a certain moment.
In light of the foregoing description of preferred embodiments in accordance with the invention, it is to be understood that numerous changes and modifications may be made by those skilled in the art without departing from the scope of the invention. The technical scope of the present invention is not limited to the contents of the specification, and must be determined according to the scope of the claims.

Claims (2)

1. An adaptive optimal-REQUEST algorithm for inertial-geomagnetic combined attitude solution, characterized by: comprises a gyroscope, an accelerometer and a geomagnetic sensor, uses a three-dimensional observation vector formed by the outputs of the gyroscope, the accelerometer and the geomagnetic sensor as an input variable, and comprises the following steps,
the method comprises the following steps: at each sampling instant k, a calculation is made
Figure FDA0002579218740000011
Wherein the content of the first and second substances,
Figure FDA0002579218740000012
and
Figure FDA0002579218740000013
the three-dimensional observation vector formed by the output of the accelerometer and the geomagnetic sensor at the time k represents modulus, × represents a vector product, asin () represents an inverse sine;
Figure FDA0002579218740000014
represents
Figure FDA0002579218740000015
And
Figure FDA0002579218740000016
the angle between these two vectors;
step two: computing
Figure FDA0002579218740000017
Wherein, α1And α2Is a scale factor, abs (·) represents the absolute value, g is the local gravity acceleration vector;
step three: to get in step two
Figure FDA0002579218740000018
The value range is monotonously converted into [0,1 ]]Intervals, therefore, calculating
Figure FDA0002579218740000019
Step four: obtained according to step three
Figure FDA00025792187400000110
Calculating "innovation" K of gesturek，
Figure FDA00025792187400000111
In the formula, each symbol is defined as follows:
Figure FDA00025792187400000112
the numerical value represents the weight sum of all observation vectors given by the optimal-REQUEST algorithm;
Figure FDA00025792187400000113
S＝B+BT，
Figure FDA00025792187400000114
i is an identity matrix;
Figure FDA00025792187400000115
and
Figure FDA00025792187400000116
respectively representing the same observation vector in a carrier coordinate system and a world coordinate system; wherein
Figure FDA00025792187400000117
The expressions under the carrier coordinate system and the world coordinate system are respectively
Figure FDA00025792187400000118
And
Figure FDA00025792187400000119
while
Figure FDA00025792187400000120
The expressions under the carrier coordinate system and the world coordinate system are respectively
Figure FDA00025792187400000121
And
Figure FDA00025792187400000122
step five: calculating "predictions" K of posesk/k-1If K is 0, i.e. the initial calculation time, let K bek/k＝Kk，mk＝mk，Pk/k＝RkWherein R iskDirectly jumping to the seventh step for observing the noise covariance matrix, or else using the covariance matrix K of the estimation error after the attitude check at the previous momentk-1/k-1Computing the "prediction" of attitude, i.e., K in 4 × 4 dimensions, according to equation (10)k/k-1The matrix is a matrix of a plurality of matrices,
Figure FDA0002579218740000021
the attitude "prediction", i.e. the covariance matrix P of the estimation errors of the attitude prior estimate, is then calculated using equation (11)k/k-1，
Figure FDA0002579218740000022
Wherein Q isk-1Is a process noise covariance matrix; phik-1Is a state transition matrix, and phik-1＝exp(Ωk-1Δt)，Ωk-1Is an antisymmetric array omegak-1Is defined as
Figure FDA0002579218740000023
Wherein, ω isk-1Is a three-dimensional vector formed by the output of the gyroscope;
step six: correcting the attitude prediction by utilizing the attitude innovation to obtain Kk/k；
Firstly, the accumulated weight sum m of the last moment is utilizedk-1The weight sum m obtained in the fourth stepkAnd the covariance matrix P of the attitude prior estimation error obtained in the step fivek/k-1Calculating a weighting factor pk，
Figure FDA0002579218740000024
Then LiUsing the accumulated weight sum m of the previous momentk-1And the weight sum m obtained in the step fourkCalculating the accumulated weight sum m at the current momentk，
mk＝(1-ρk)mk-1+ρkmk(14)
The accumulated weight sum m at the current momentkAnd a weighting factor pkThe correction of the attitude can be realized, and the matrix K is obtainedk/k，
Figure FDA0002579218740000025
Finally, calculating the covariance matrix P of the attitude post-test estimation error at the current momentk/k，
Figure FDA0002579218740000031
Preservation of Pk/kAnd mkTo facilitate the next round of calculation;
step seven: obtaining Kk/kAfter the matrix is formed, the attitude of the current moment is calculated according to the matrix, and a matrix K is calculatedk/kThe feature vector is a posture calculation result expressed by quaternion at the calculation moment; and returning to the step one to carry out attitude calculation at the next moment until the user stops attitude calculation at a certain moment.
2. The adaptive optimal-REQUEST algorithm for inertial-geomagnetic combined attitude solution, according to claim 1, characterized in that: and (3) denoising the calculation result of the formula (1) by adopting a filtering technology.
CN201710804083.3A 2017-09-08 2017-09-08 Adaptive optimal-REQUEST algorithm for inertial-geomagnetic combined attitude solution Active CN107621261B (en)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN201710804083.3A CN107621261B (en) 2017-09-08 2017-09-08 Adaptive optimal-REQUEST algorithm for inertial-geomagnetic combined attitude solution

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN201710804083.3A CN107621261B (en) 2017-09-08 2017-09-08 Adaptive optimal-REQUEST algorithm for inertial-geomagnetic combined attitude solution

Publications (2)

Publication Number Publication Date
CN107621261A CN107621261A (en) 2018-01-23
CN107621261B true CN107621261B (en) 2020-09-08

Family

ID=61088431

Family Applications (1)

Application Number Title Priority Date Filing Date
CN201710804083.3A Active CN107621261B (en) 2017-09-08 2017-09-08 Adaptive optimal-REQUEST algorithm for inertial-geomagnetic combined attitude solution

Country Status (1)

Country Link
CN (1) CN107621261B (en)

Families Citing this family (3)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN108871319B (en) * 2018-04-26 2022-05-17 李志� Attitude calculation method based on earth gravity field and earth magnetic field sequential correction
CN109029435B (en) * 2018-06-22 2021-11-02 常州大学 Method for improving inertia-geomagnetic combined dynamic attitude determination precision
CN112082529A (en) * 2020-07-29 2020-12-15 上海谷感智能科技有限公司 Small household appliance attitude measurement method based on inertial sensor and attitude identification module

Citations (7)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN102168979A (en) * 2010-12-08 2011-08-31 北京航空航天大学 Isoline matching method for passive navigation based on triangular constraint model
CN102322858A (en) * 2011-08-22 2012-01-18 南京航空航天大学 Geomagnetic matching navigation method for geomagnetic-strapdown inertial navigation integrated navigation system
CN102826064A (en) * 2012-08-30 2012-12-19 黑龙江省博凯科技开发有限公司 Heavy-duty vehicle rollover warning device based on micro-inertia/satellite/geomagnetic combined measurement
US9014975B2 (en) * 2012-05-23 2015-04-21 Vectornav Technologies, Llc System on a chip inertial navigation system
JP5933398B2 (en) * 2012-08-31 2016-06-08 本田技研工業株式会社 Variable field motor and electric vehicle
CN106248081A (en) * 2016-09-09 2016-12-21 常州大学 A kind of blind person's indoor navigation method combining Wi Fi auxiliary positioning based on inertial navigation
CN107084722A (en) * 2017-04-24 2017-08-22 常州大学 It is a kind of to be used to improve the method that inertia earth magnetism combines quiet dynamic comprehensive performance

Patent Citations (7)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN102168979A (en) * 2010-12-08 2011-08-31 北京航空航天大学 Isoline matching method for passive navigation based on triangular constraint model
CN102322858A (en) * 2011-08-22 2012-01-18 南京航空航天大学 Geomagnetic matching navigation method for geomagnetic-strapdown inertial navigation integrated navigation system
US9014975B2 (en) * 2012-05-23 2015-04-21 Vectornav Technologies, Llc System on a chip inertial navigation system
CN102826064A (en) * 2012-08-30 2012-12-19 黑龙江省博凯科技开发有限公司 Heavy-duty vehicle rollover warning device based on micro-inertia/satellite/geomagnetic combined measurement
JP5933398B2 (en) * 2012-08-31 2016-06-08 本田技研工業株式会社 Variable field motor and electric vehicle
CN106248081A (en) * 2016-09-09 2016-12-21 常州大学 A kind of blind person's indoor navigation method combining Wi Fi auxiliary positioning based on inertial navigation
CN107084722A (en) * 2017-04-24 2017-08-22 常州大学 It is a kind of to be used to improve the method that inertia earth magnetism combines quiet dynamic comprehensive performance

Non-Patent Citations (2)

* Cited by examiner, † Cited by third party
Title
基于多传感器融合的动态手势识别研究分析;马正华等;《计算机工程与应用》;20170930(第17期);第153-159页 *
基于惯性-地磁组合的运动体姿态测量算法分析;戎海龙等;《计算机应用研究》;20140630;第31卷(第6期);第1657-1660页 *

Also Published As

Publication number Publication date
CN107621261A (en) 2018-01-23

Similar Documents

Publication Publication Date Title
US10595784B2 (en) Object pose measurement system based on MEMS IMU and method thereof
Jin et al. The adaptive Kalman filter based on fuzzy logic for inertial motion capture system
Abyarjoo et al. Implementing a sensor fusion algorithm for 3D orientation detection with inertial/magnetic sensors
Schepers et al. Ambulatory human motion tracking by fusion of inertial and magnetic sensing with adaptive actuation
US7890291B2 (en) Method and device for detecting a substantially invariant rotation axis
Han et al. A novel method to integrate IMU and magnetometers in attitude and heading reference systems
CN104718431B (en) Gyroscope regulation and gyroscope camera alignment
JP6471749B2 (en) Position / orientation estimation apparatus, image processing apparatus, and position / orientation estimation method
CN107621261B (en) Adaptive optimal-REQUEST algorithm for inertial-geomagnetic combined attitude solution
US20140229138A1 (en) Initializing an inertial sensor using soft constraints and penalty functions
CN107339987B (en) Rigid body attitude calculation method based on function iteration integral
EP2676103A2 (en) Inertial navigation sculling algorithm
Laidig et al. VQF: Highly accurate IMU orientation estimation with bias estimation and magnetic disturbance rejection
CN108731676A (en) A kind of posture fusion enhancing measurement method and system based on inertial navigation technology
CN107084722B (en) Method for improving inertia-geomagnetic combined static and dynamic comprehensive performance
Kopniak et al. Natural interface for robotic arm controlling based on inertial motion capture
CN111854741B (en) GNSS/INS tight combination filter and navigation method
CN110610513B (en) Invariance center differential filter method for vision SLAM of autonomous mobile robot
CN110375773B (en) Attitude initialization method for MEMS inertial navigation system
Brückner et al. Evaluation of inertial sensor fusion algorithms in grasping tasks using real input data: Comparison of computational costs and root mean square error
US9086281B1 (en) Using accelerometer data to determine movement direction
CA2983574C (en) Initializing an inertial sensor using soft constraints and penalty functions
Bruckner et al. Evaluation of inertial sensor fusion algorithms in grasping tasks using real input data: Comparison of computational costs and root mean square error
CN114440878B (en) SINS and DVL integrated navigation method, device and system
VB et al. About inertial-satellite navigation system without rate gyros

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