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

Abstract

The invention provides an adaptive optimal-REQUEST algorithm for inertia-geomagnetic combined attitude calculation. An adaptive adjustment strategy is used to dynamically adjust the weight of the observation vector of the attitude calculation algorithm, so that the attitude calculation algorithm can be adjusted dynamically. When the carrier is stationary, it mainly relies on the accelerometer and the geomagnetic sensor to realize the attitude calculation to improve the static accuracy, and when the carrier is moving, it mainly relies on the gyroscope to realize the attitude calculation to improve the dynamic accuracy. The core of the above-mentioned adaptive adjustment strategy is to judge whether the angle between the observed values of the two vectors, the gravitational acceleration vector and the geomagnetic field vector, is near a given value, and at the same time to judge whether the modulus of the observed value of the gravitational acceleration vector is in the other. Near the given value, if all are near the given value, the carrier is in a static state, otherwise the carrier is in a moving state. The above two given values can be obtained simply by the measured values of the accelerometer and the geomagnetic sensor when the carrier is stationary.

Description

Adaptive optimal-REQUEST algorithm for inertial-geomagnetic combined attitude calculation

technical field

The invention relates to the technical field of attitude calculation algorithms, in particular to an adaptive optimal-REQUEST algorithm for inertial-geomagnetic combined attitude calculation.

Background technique

The inertial-geomagnetic measurement combination is composed of a three-axis gyroscope, a three-axis accelerometer and a three-axis geomagnetic sensor. By sensing the projection of the angular velocity vector, the gravitational acceleration vector, and the geomagnetic field vector on the three axes of the carrier coordinate system, it is determined that the carrier coordinate system is relative to the carrier coordinate system. The pose of the world coordinate system and its changes. Since this measurement combination does not require any external source information, it has been widely used in human body attitude tracking, robots, and small UAVs. The solution algorithm used to fuse sensor data to determine attitude is usually the Kalman algorithm (EKF), which uses Euler angle differential equations or quaternion differential equations (the attitude is obtained from gyroscope data) to construct the state Equation, construct the measurement equation with Gauss-Newton algorithm, TRIAD algorithm, QUEST algorithm or vector coordinate system transformation equation (obtain attitude from accelerometer and geomagnetic sensor data), and finally use Kalman algorithm (if the vector coordinate system transformation equation is used) , the extended Kalman algorithm is used for fusion. The advantages of the above fusion are: 1. The state equation makes full use of the historical measurement information of attitude, and gives play to the performance of the gyroscope in dynamic attitude measurement over the combination of accelerometer and geomagnetic sensor; 2. The measurement equation depends on each sampling The attitude of the carrier coordinate system relative to the world coordinate system is obtained from the accelerometer and geomagnetic sensor data at the moment, so that the attitude divergence phenomenon due to the integration of the gyroscope measurement error can be compensated.

In addition to the Kalman algorithm (EKF), in recent years, scholars in related fields have proposed a number of attitude solving algorithms, and studies have shown that when the state equation or the observation equation has a very serious nonlinearity, the performance of these algorithms must be better than EKF. Although these algorithms are suitable for inertial-geomagnetic combinations, these algorithms are either based on statistical filtering techniques (such as particle filter (PF) and unscented Kalman filtering algorithm (UKF) or least squares techniques (such as backwards-smoothing EKF, extended QUEST, two-step attitude estimator), etc. Considering the calculation time, sampling rate and processor, EKF is still the most widely used attitude solution algorithm for inertial-geomagnetic combination.

The REQUEST algorithm and the optimal-REQUEST algorithm developed on the basis of the QUEST algorithm are mainly used in the aerospace field to achieve attitude determination of space vehicles such as satellites. If the parameters are set reasonably, the dynamic and static performance of the optimal-REQUEST algorithm is comparable to the extended Kalman algorithm (EKF), but the calculation time is less. In terms of the inertial-geomagnetic combination of the processor, the former is undoubtedly more suitable.

SUMMARY OF THE INVENTION

In the case of the carrier moving at high speed, since the gravitational acceleration information measured by the accelerometer is quite inaccurate, the attitude calculation algorithm should discard the accelerometer information in this case, and rely solely on the gyroscope information to realize the attitude calculation.

At present, there are mainly two methods for deciding the choice of accelerometer information, both of which are based on the modulus of the three-dimensional vector formed by the acceleration output. The first method is a discrete method with a threshold. This method continuously records the latest n sampled values of the vector modulus, and judges whether any sampled value in the n sampled values is greater than a preset threshold. If the conclusion is established, the accelerometer output information is completely discarded, otherwise the accelerometer information is completely used. The second method is a continuous method without a threshold. This method does not completely discard the accelerometer information when the carrier is moving, and only records the modulus of the above vector at the current moment, which multiplies the accelerometer information by a value representing its utilization. The weights of the rate, and then the above measurement information is used for attitude calculation. In the first method, the judgment accuracy is generally not high when the sampling rate is low and the measurement noise of the accelerometer is large. In the second method, when the linear acceleration of the carrier is high, the information of the accelerometer is not completely discarded, so the attitude will still be solved. bring larger errors.

The present invention adopts the optimal-REQUEST algorithm as the attitude calculation algorithm, and proposes a method for self-adaptive selection of accelerometer information, and then organically integrates the method with the optimal-REQUEST algorithm, so as to propose a new method suitable for inertia-geomagnetic Combined pose solving algorithm.

The technical scheme adopted in the present invention is: an adaptive optimal-REQUEST algorithm for inertial-geomagnetic combined attitude calculation, including a gyroscope, an accelerometer and a geomagnetic sensor; The observation vector is taken as input variable and consists of the following steps,

Step 1: At each sampling time k, calculate

和

分别为k时刻由加速度计和地磁传感器输出构成的三维观测矢量；||·||表示取模；×表示取向量积；asin()表示取反正弦；

代表

和

这两个矢量的夹角。在载体静止的情况下，由于不受载体线加速度影响，加速度计将只测得重力加速度，即

表示的是重力加速度矢量，又由于重力加速度矢量和地磁场矢量在一定地域内是不变化的，因此其夹角不变，即

是一个恒定值，该恒定值现表示为θ(例如在常州地区，θ＝47.49°)。在载体运动的情况下，由于加速度计的测量值是重力加速度和载体线加速度的叠加，因此

并不表示重力加速度矢量，这就意味着

并不等于θ，两者偏离程度越大，表示载体线加速度越大。因此

可以作为动态调整optimal-REQUEST算法的观测矢量的权值的一个因子(另一个因子是步骤二表示的

)，使得optimal-REQUEST算法在载体运动时降低依靠加速度计和地磁传感器的程度从而主要依靠陀螺仪实现姿态解算，进而提高姿态解算精度。可以说本步骤计算

的目的是为步骤二进一步计算

做准备。in,

and

are the three-dimensional observation vectors formed by the output of the accelerometer and the geomagnetic sensor at time k, respectively; ||·|| represents the modulo; × represents the orientation vector product; asin() represents the inverse sine;

represent

and

The angle between these two vectors. When the carrier is stationary, since it is not affected by the linear acceleration of the carrier, the accelerometer will only measure the gravitational acceleration, that is,

Represents the gravitational acceleration vector, and since the gravitational acceleration vector and the geomagnetic field vector do not change in a certain area, the included angle does not change, that is

is a constant value, which is now expressed as θ (eg, in the Changzhou area, θ=47.49°). In the case of carrier motion, since the measurement value of the accelerometer is the superposition of the acceleration of gravity and the linear acceleration of the carrier, so

does not represent the gravitational acceleration vector, which means that

It is not equal to θ. The greater the degree of deviation between the two, the greater the linear acceleration of the carrier. therefore

It can be used as a factor to dynamically adjust the weight of the observation vector of the optimal-REQUEST algorithm (the other factor is expressed in step 2).

), so that the optimal-REQUEST algorithm reduces the degree of relying on the accelerometer and the geomagnetic sensor when the carrier is moving, and mainly relies on the gyroscope to achieve attitude calculation, thereby improving the attitude calculation accuracy. It can be said that this step calculates

The purpose is to further calculate for step two

prepare.

在[-π/2,π/2]区间内，须利用

的计算结果将式(1)转换至[0,π]区间，以满足本技术方案后续步骤的要求，其中·表示代数积。可以但不建议利用

实现

的估算，因为此时将无法采用经典的卡尔曼滤波技术实现去噪。Due to the dual influence of the output noise of the accelerometer and the geomagnetic sensor, the calculation result of formula (1) will contain a lot of noise, and various filtering techniques can be used to de-noise the calculation result of formula (1). obtained from formula (1)

In the interval [-π/2,π/2], use

The calculation result of , converts the formula (1) into the [0, π] interval to meet the requirements of the subsequent steps of this technical solution, where · represents the algebraic product. Possible but not recommended

accomplish

, because the classical Kalman filtering technique cannot be used for denoising at this time.

Step 2: Calculate

作为动态调整optimal-REQUEST算法的观测矢量的权值的另一个因子的原因同

类似。α₁和α₂为比例因子，下面分析这两个比例因子的取值范围。首先分析α₁的取值范围，由于

因此应取α₁＞1，以保证只要

稍微偏离θ，

就能够快速衰减至0，至于为何需要该值衰减至0，请见步骤三的解释。Among them, abs( ) means taking the absolute value, and g is the local gravitational acceleration vector;

As another factor for dynamically adjusting the weight of the observation vector of the optimal-REQUEST algorithm, the reason is the same

similar. α ₁ and α ₂ are scale factors, and the value ranges of these two scale factors are analyzed below. First analyze the value range of α ₁ , because

Therefore, α ₁ > 1 should be taken to ensure that as long as

slightly off θ,

It can quickly decay to 0. As for why this value needs to decay to 0, please see the explanation in step 3.

The value range of α ₂ is analyzed below. Without loss of generality, let the three-dimensional vector composed of the output of the accelerometer without the influence of noise be

其中

为白噪声，且方差相同。由于After applying noise:

in

is white noise with the same variance. because

but

therefore

where D() represents the variance. It can be seen from formula (5) that 0<α ₂ ≤1/3 needs to be taken, so that the existence of the acceleration vector measurement noise has less influence on pre _g .

It should be pointed out that since

where acos() represents the inverse cosine, so it is obtained from equation (6)

噪声较大，加之α₁＞1，因此测量噪声的存在肯定会使pre_g产生较大波动。步骤二已经指出，必须利用各种滤波技术实现对

的去噪。From equation (7), it can be seen that,

The noise is relatively large, and α ₁ >1, so the existence of measurement noise will definitely make pre _g fluctuate greatly. Step 2 has already pointed out that various filtering techniques must be used to achieve

denoising.

The recommended values of the present invention for the above two scale factors are α ₁ =100/π, α ₂ =0.01.

并不直接用于调整optimal-REQUEST算法的观测矢量的权值，还需要下面的步骤三的进一步转化。Calculated in this step

It is not directly used to adjust the weight of the observation vector of the optimal-REQUEST algorithm, and further transformation in the following step 3 is required.

Step 3: Calculate

的取值范围为

因此

并不能直接用于调整optimal-REQUEST算法的观测矢量的权值。式(8)的目的是将

取值范围单调地转化为[0,1]区间。所得到的

将用于步骤四的optimal-REQUEST算法的的观测矢量的权值的调整。由式(8)可以看出，只要α₁和α₂取得合适的值，那么一旦

稍微偏离θ或者

稍微增大而导致

稍微增大，

就会立刻衰减至0，这一结果正是我们想要的，因为它可以使得optimal-REQUEST算法在载体运动时主要依靠陀螺仪实现姿态解算，从而提高姿态解算精度。As can be seen from step 2,

The value range of is

therefore

It cannot be directly used to adjust the weights of the observation vector of the optimal-REQUEST algorithm. The purpose of formula (8) is to convert

The range of values is monotonically converted to the interval [0,1]. obtained

It will be used for the adjustment of the weights of the observation vector of the optimal-REQUEST algorithm in step 4. It can be seen from formula (8) that as long as α ₁ and α ₂ obtain appropriate values, once

slightly off theta or

slightly increased due to

slightly increased,

It will immediately decay to 0, which is exactly what we want, because it can make the optimal-REQUEST algorithm mainly rely on the gyroscope to achieve attitude calculation when the carrier is moving, thereby improving the attitude calculation accuracy.

计算姿态的“新息”δK_k，即式(9)所示的4×4维矩阵Step 4: Obtained according to Step 3

Calculate the "innovation" δK _k of the attitude, that is, the 4×4-dimensional matrix shown in equation (9)

该数值表示optimal-REQUEST算法赋予所有观测向量的权值的和，为一个标量；(2)根据

和由步骤(1)计算出的δm_k计算

该数值为一个标量，没有物理意义；(3)根据

和由步骤(1)计算出的δm_k计算

该数值为一个3×3维的矩阵，没有物理意义；(4)根据步骤(3)计算出的矩阵B计算S＝B+B^T，该数值仍为一个3×3维的矩阵，没有物理意义；(5)根据

和由步骤(1)计算出的δm_k计算

该数值为一个3×1维的向量，没有物理意义；(6)根据步骤(1)-(5)的计算结果，按照式(9)构成矩阵δK_k。需要注意的是

和

分别为同一观测矢量在载体坐标系和世界坐标系下的表示。一共有两个观测矢量，即

和

举个例子，由加速度计输出构成的三维矢量为

那么该向量在载体坐标系下的表示即为

而该矢量在世界坐标系下的表示则为

而如果将上述三维矢量换成

(由地磁传感器输出构成)，则该三维矢量在载体坐标系和世界坐标系下的表示则分别为

和

式(9)中的I表示为3×3维的单位矩阵。The process of calculating the matrix δK _k is, (1) calculate

This value represents the sum of the weights assigned to all observation vectors by the optimal-REQUEST algorithm, which is a scalar; (2) According to

and δm _k calculated by step (1)

The value is a scalar and has no physical meaning; (3) According to

and δm _k calculated by step (1)

The value is a 3×3-dimensional matrix, which has no physical meaning; (4) Calculate S=B+ ^BT according to the matrix B calculated in step (3), and the value is still a 3×3-dimensional matrix without physical meaning. meaning; (5) according to

and δm _k calculated by step (1)

The value is a 3×1-dimensional vector, which has no physical meaning; (6) According to the calculation results of steps (1)-(5), a matrix δK _k is formed according to formula (9). have to be aware of is

and

are the representations of the same observation vector in the carrier coordinate system and the world coordinate system, respectively. There are two observation vectors in total, namely

and

For example, the three-dimensional vector formed by the output of the accelerometer is

Then the representation of the vector in the carrier coordinate system is

And the representation of the vector in the world coordinate system is

And if the above three-dimensional vector is replaced by

(composed of the output of the geomagnetic sensor), then the representation of the three-dimensional vector in the carrier coordinate system and the world coordinate system is respectively

and

I in Equation (9) is represented as a 3×3-dimensional identity matrix.

因此一旦载体存在运动，那么

立即衰减至0，从而使得optimal-REQUEST算法不再依靠观测矢量实现姿态解算，而主要依靠陀螺仪的输出来完成，从而提高姿态解算精度。Equation (9) means that the weight of each observation vector assigned to the optimal-REQUEST algorithm is expressed by Equation (8).

So once the carrier has motion, then

Immediately attenuates to 0, so that the optimal-REQUEST algorithm no longer relies on the observation vector to achieve attitude calculation, but mainly relies on the output of the gyroscope to complete the attitude calculation accuracy.

The matrix δK _k represents the "innovation" used for attitude calculation, which is used to realize the correction of attitude "prediction". As long as it is not the initial moment, it is not directly used for attitude calculation.

Step 5: Calculate the "predicted" K _k/k-1 of the pose,

If k=0, that is, at the initial calculation moment, since there is no "prediction" of attitude, then let K _k/k =δK _k , m _k =δm _k , P _k/k =R _k , where R _k is the observation noise covariance Then jump directly to step 7, and use the 4×4-dimensional matrix K _k/k to realize the attitude calculation. m _k is the sum of the accumulated weights, which means that from the moment, the δm _k of each moment is accumulated until the current moment. P _k/k represents the covariance matrix of the estimation error of the posterior estimation of the pose. Both m _k and P _k/k are used in the calculation of step six.

If k≠0, use the covariance matrix K _k-1/k-1 of the posterior estimation error of the attitude at the previous moment to calculate the "prediction" of the attitude according to equation (10), that is, the 4 × 4-dimensional K _{k/k -1} matrix,

Then use equation (11) to calculate the attitude "prediction", that is, the covariance matrix P _k/k-1 of the estimation error of the attitude prior estimation,

Among them, Q _k-1 is the process noise covariance matrix; Φ _k-1 is the state transition matrix, and Φ _k-1 =exp(Ω _k-1 Δt), Ω _k-1 is an antisymmetric matrix, and the antisymmetric matrix Ω _k-1 is defined as:

Among them, ω _k-1 is a three-dimensional vector formed by the output of the gyroscope;

In this step, the "prediction" K _k/k-1 of the attitude has been obtained, and the "innovation" δK _k of the attitude has also been obtained in the previous step, and the next step will use the "innovation" of the attitude to realize the "prediction" of the attitude ” to get K _k/k .

Step 6: First use the accumulated weight sum m _k-1 at the previous moment, the weight sum δm _k obtained in step 4, and the covariance matrix P _k/k-1 of the attitude prior estimation error obtained in step 5 to calculate the weighting factor ρ _k ,

Then use the accumulated weight sum m _k-1 at the previous moment and the weight sum δm _k obtained in step 4 to calculate the accumulated weight sum m _k at the current moment,

m _k =(1-ρ _k )m _k-1 +ρ _k δm _k (14)

With the accumulated weight value and m _k and weighting factor ρ _k of the current moment, the attitude correction can be realized, that is, the matrix K _k/k is obtained,

Finally, the covariance matrix P _k/k of the posterior estimation error of the attitude at the current moment is calculated,

P _k/k and m _k will be saved for the next round of calculation, because the above steps will be repeated when the next time comes, and these two quantities will be used in steps 5 and 6.

After obtaining the K _k/k matrix, the next step will calculate the pose at the current moment based on the matrix.

Step 7: Calculate the normalized eigenvector corresponding to the maximum eigenvalue of the matrix K _k/k , where the eigenvector is the attitude q _k represented by the quaternion at the calculation moment. At this point, the calculation of the attitude at the current moment k is completed. Returning to step 1, the attitude calculation process at the next moment is continued until the attitude calculation is stopped at a certain moment.

The beneficial effects of the present invention are: an adaptive optimal-REQUEST algorithm for inertial-geomagnetic combined attitude calculation provided by the present invention can make the optimal-REQUEST algorithm reduce the degree of relying on the accelerometer and the geomagnetic sensor when the carrier moves, thereby reducing the It mainly relies on the gyroscope to realize the attitude calculation, and when the carrier is stationary, it can mainly rely on the accelerometer and the geomagnetic sensor to realize the attitude calculation (because these two sensors have high measurement accuracy for the gravitational acceleration vector and the geomagnetic field vector), Thereby improving the static and dynamic performance of the optimal-REQUEST algorithm.

Description of drawings

The present invention will be further described below with reference to the accompanying drawings and embodiments.

Figure 1 is a schematic diagram of a preferred embodiment of the present invention.

Detailed ways

The present invention will now be described in detail with reference to the accompanying drawings. This figure is a simplified schematic diagram, and only illustrates the basic structure of the present invention in a schematic manner, so it only shows the structure related to the present invention.

As shown in FIG. 1, an adaptive optimal-REQUEST algorithm for inertial-geomagnetic combined attitude calculation of the present invention includes a gyroscope, an accelerometer and a geomagnetic sensor, and is composed of the outputs of the gyroscope, the accelerometer and the geomagnetic sensor. The 3D observation vector is used as input variable and consists of the following steps,

Step 1: At each sampling time k, calculate

和

分别为k时刻由加速度计和地磁传感器输出构成的三维观测矢量；||·||表示取模；×表示取向量积；asin()表示取反正弦；

代表

和

这两个矢量的夹角。在载体静止的情况下，由于不受载体线加速度影响，加速度计将只测得重力加速度，即

表示的是重力加速度矢量，又由于重力加速度矢量和地磁场矢量在一定地域内是不变化的，因此其夹角不变，即

是一个恒定值，该恒定值现表示为θ(例如在常州地区，θ＝47.49°)。在载体运动的情况下，由于加速度计的测量值是重力加速度和载体线加速度的叠加，因此

并不表示重力加速度矢量，这就意味着

并不等于θ，两者偏离程度越大，表示载体线加速度越大。因此

可以作为动态调整optimal-REQUEST算法的观测矢量的权值的一个因子(另一个因子是步骤二表示的

)，使得optimal-REQUEST算法在载体运动时降低依靠加速度计和地磁传感器的程度从而主要依靠陀螺仪实现姿态解算，进而提高姿态解算精度。可以说本步骤计算

的目的是为步骤二进一步计算

做准备。in,

and

are the three-dimensional observation vectors formed by the output of the accelerometer and the geomagnetic sensor at time k, respectively; ||·|| represents the modulo; × represents the orientation vector product; asin() represents the inverse sine;

represent

and

The angle between these two vectors. When the carrier is stationary, since it is not affected by the linear acceleration of the carrier, the accelerometer will only measure the gravitational acceleration, that is,

Represents the gravitational acceleration vector, and since the gravitational acceleration vector and the geomagnetic field vector do not change in a certain area, the included angle does not change, that is

is a constant value, which is now expressed as θ (eg, in the Changzhou area, θ=47.49°). In the case of carrier motion, since the measurement value of the accelerometer is the superposition of the acceleration of gravity and the linear acceleration of the carrier, so

does not represent the gravitational acceleration vector, which means that

It is not equal to θ. The greater the degree of deviation between the two, the greater the linear acceleration of the carrier. therefore

It can be used as a factor to dynamically adjust the weight of the observation vector of the optimal-REQUEST algorithm (the other factor is expressed in step 2).

), so that the optimal-REQUEST algorithm reduces the degree of relying on the accelerometer and the geomagnetic sensor when the carrier is moving, and mainly relies on the gyroscope to achieve attitude calculation, thereby improving the attitude calculation accuracy. It can be said that this step calculates

The purpose is to further calculate for step two

prepare.

在[-π/2,π/2]区间内，须利用

的计算结果将式(1)转换至[0,π]区间，以满足本技术方案后续步骤的要求，其中·表示代数积。可以但不建议利用

实现

的估算，因为此时将无法采用经典的卡尔曼滤波技术实现去噪。Due to the dual influence of the output noise of the accelerometer and the geomagnetic sensor, the calculation result of formula (1) will contain a lot of noise, and various filtering techniques can be used to de-noise the calculation result of formula (1). obtained from formula (1)

In the interval [-π/2,π/2], use

The calculation result of , converts the formula (1) into the [0, π] interval to meet the requirements of the subsequent steps of this technical solution, where · represents the algebraic product. Possible but not recommended

accomplish

, because the classical Kalman filtering technique cannot be used for denoising at this time.

Step 2: Calculate

作为动态调整optimal-REQUEST算法的观测矢量的权值的另一个因子的原因同

类似。α₁和α₂为比例因子，下面分析这两个比例因子的取值范围。首先分析α₁的取值范围，由于

因此应取α₁＞1，以保证只要

稍微偏离θ，

就能够快速衰减至0，至于为何需要该值衰减至0，请见步骤三的解释。Among them, abs( ) means taking the absolute value, and g is the local gravitational acceleration vector;

As another factor for dynamically adjusting the weight of the observation vector of the optimal-REQUEST algorithm, the reason is the same

similar. α ₁ and α ₂ are scale factors, and the value ranges of these two scale factors are analyzed below. First analyze the value range of α ₁ , because

Therefore, α ₁ > 1 should be taken to ensure that as long as

slightly off θ,

It can quickly decay to 0. As for why this value needs to decay to 0, please see the explanation in step 3.

The value range of α ₂ is analyzed below. Without loss of generality, let the three-dimensional vector composed of the output of the accelerometer without the influence of noise be

其中

为白噪声，且方差相同。由于After applying noise:

in

is white noise with the same variance. because

but

therefore

where D() represents the variance. It can be seen from formula (5) that 0<α ₂ ≤1/3 needs to be taken, so that the existence of the acceleration vector measurement noise has less influence on pre _g .

It should be pointed out that since

where acos() represents the inverse cosine, so it is obtained from equation (6)

噪声较大，加之α₁＞1，因此测量噪声的存在肯定会使pre_g产生较大波动。步骤二已经指出，必须利用各种滤波技术实现对

的去噪。From equation (7), it can be seen that,

The noise is relatively large, and α ₁ >1, so the existence of measurement noise will definitely make pre _g fluctuate greatly. Step 2 has already pointed out that various filtering techniques must be used to achieve

denoising.

The recommended values of the present invention for the above two scale factors are α ₁ =100/π, α ₂ =0.01.

并不直接用于调整optimal-REQUEST算法的观测矢量的权值，还需要下面的步骤三的进一步转化。Calculated in this step

It is not directly used to adjust the weight of the observation vector of the optimal-REQUEST algorithm, and further transformation in the following step 3 is required.

Step 3: Calculate

的取值范围为

因此

并不能直接用于调整optimal-REQUEST算法的观测矢量的权值。式(8)的目的是将

取值范围单调地转化为[0,1]区间。所得到的

将用于步骤四的optimal-REQUEST算法的的观测矢量的权值的调整。由式(8)可以看出，只要α₁和α₂取得合适的值，那么一旦

稍微偏离θ或者

稍微增大而导致

稍微增大，

就会立刻衰减至0，这一结果正是我们想要的，因为它可以使得optimal-REQUEST算法在载体运动时主要依靠陀螺仪实现姿态解算，从而提高姿态解算精度。As can be seen from step 2,

The value range of is

therefore

It cannot be directly used to adjust the weights of the observation vector of the optimal-REQUEST algorithm. The purpose of formula (8) is to convert

The range of values is monotonically converted to the interval [0,1]. obtained

It will be used for the adjustment of the weights of the observation vector of the optimal-REQUEST algorithm in step 4. It can be seen from formula (8) that as long as α ₁ and α ₂ obtain appropriate values, once

slightly off theta or

slightly increased due to

slightly increased,

It will immediately decay to 0, which is exactly what we want, because it can make the optimal-REQUEST algorithm mainly rely on the gyroscope to achieve attitude calculation when the carrier is moving, thereby improving the attitude calculation accuracy.

计算式(9)所示的4×4维矩阵Step 4: Obtained according to Step 3

Calculate the 4×4-dimensional matrix shown in equation (9)

该数值表示optimal-REQUEST算法赋予所有观测向量的权值的和，为一个标量；(2)根据

和由步骤(1)计算出的δm_k计算

该数值为一个标量，没有物理意义；(3)根据

和由步骤(1)计算出的δm_k计算

该数值为一个3×3维的矩阵，没有物理意义；(4)根据步骤(3)计算出的矩阵B计算S＝B+B^T，该数值仍为一个3×3维的矩阵，没有物理意义；(5)根据

和由步骤(1)计算出的δm_k计算

该数值为一个3×1维的向量，没有物理意义；(6)根据步骤(1)-(5)的计算结果，按照式(9)构成矩阵δK_k。需要注意的是

和

分别为同一观测矢量在载体坐标系和世界坐标系下的表示。一共有两个观测矢量，即

和

举个例子，由加速度计输出构成的三维矢量为

那么该向量在载体坐标系下的表示即为

而该矢量在世界坐标系下的表示则为

而如果将上述三维矢量换成

(由地磁传感器输出构成)，则该三维矢量在载体坐标系和世界坐标系下的表示则分别为

和

式(9)中的I表示为3×3维的单位矩阵。The process of calculating the matrix δK _k is, (1) calculate

This value represents the sum of the weights assigned to all observation vectors by the optimal-REQUEST algorithm, which is a scalar; (2) According to

and δm _k calculated by step (1)

The value is a scalar and has no physical meaning; (3) According to

and δm _k calculated by step (1)

The value is a 3×3-dimensional matrix, which has no physical meaning; (4) Calculate S=B+ ^BT according to the matrix B calculated in step (3), and the value is still a 3×3-dimensional matrix without physical meaning. meaning; (5) according to

and δm _k calculated by step (1)

The value is a 3×1-dimensional vector, which has no physical meaning; (6) According to the calculation results of steps (1)-(5), a matrix δK _k is formed according to formula (9). have to be aware of is

and

are the representations of the same observation vector in the carrier coordinate system and the world coordinate system, respectively. There are two observation vectors in total, namely

and

For example, the three-dimensional vector formed by the output of the accelerometer is

Then the representation of the vector in the carrier coordinate system is

And the representation of the vector in the world coordinate system is

And if the above three-dimensional vector is replaced by

(composed of the output of the geomagnetic sensor), then the representation of the three-dimensional vector in the carrier coordinate system and the world coordinate system is respectively

and

I in Equation (9) is represented as a 3×3-dimensional identity matrix.

因此一旦载体存在运动，那么

立即衰减至0，从而使得optimal-REQUEST算法不再依靠观测矢量实现姿态解算，而主要依靠陀螺仪的输出来完成，从而提高姿态解算精度。Equation (9) means that the weight of each observation vector assigned to the optimal-REQUEST algorithm is expressed by Equation (8).

So once the carrier has motion, then

Immediately attenuates to 0, so that the optimal-REQUEST algorithm no longer relies on the observation vector to achieve attitude calculation, but mainly relies on the output of the gyroscope to complete the attitude calculation accuracy.

The matrix δK _k represents the "innovation" used for attitude calculation, which is used to realize the correction of attitude "prediction". As long as it is not the initial moment, it is not directly used for attitude calculation. If k=0, that is, at the initial calculation moment, since there is no "prediction" of attitude, then let K _k/k =δK _k , m _k =δm _k , P _k/k =R _k , where R _k is the observation noise covariance Then jump directly to step 7, and use the 4×4-dimensional matrix K _k/k to realize the attitude calculation. m _k is the sum of the accumulated weights, which means that from the moment, the δm _k of each moment is accumulated until the current moment. P _k/k represents the covariance matrix of the estimation error of the posterior estimation of the pose. Both m _k and P _k/k are used in the calculation of step six.

Step 5: If k≠0, use the covariance matrix K _k-1/k-1 of the posterior estimation error of the attitude at the previous moment to calculate the "prediction" of the attitude according to formula (10), that is, the 4×4-dimensional K _k/k-1 matrix,

Then use equation (11) to calculate the attitude "prediction", that is, the covariance matrix P _k/k-1 of the estimation error of the attitude prior estimation,

Among them, Q _k-1 is the process noise covariance matrix; Φ _k-1 is the state transition matrix, and Φ _k-1 =exp(Ω _k-1 Δt), Ω _k-1 is an antisymmetric matrix, and the antisymmetric matrix Ω _k-1 is defined as

Among them, ω _k-1 is a three-dimensional vector formed by the output of the gyroscope;

In this step, the "prediction" K _k/k-1 of the attitude has been obtained, and the "innovation" δK _k of the attitude has also been obtained in the previous step, and the next step will use the "innovation" of the attitude to realize the "prediction" of the attitude ” to get K _k/k .

Step 6: First use the accumulated weight sum m _k-1 at the previous moment, the weight sum δm _k obtained in step 4, and the covariance matrix P _k/k-1 of the attitude prior estimation error obtained in step 5 to calculate the weighting factor ρ _k ,

Then use the accumulated weight sum m _k-1 at the previous moment and the weight sum δm _k obtained in step 4 to calculate the accumulated weight sum m _k at the current moment,

m _k =(1-ρ _k )m _k-1 +ρ _k δm _k (14)

With the accumulated weight value and m _k and weighting factor ρ _k of the current moment, the attitude correction can be realized, that is, the matrix K _k/k is obtained,

Finally, the covariance matrix P _k/k of the posterior estimation error of the attitude at the current moment is calculated,

P _k/k and m _k will be saved for the next round of calculation, because the above steps will be repeated when the next time comes, and these two quantities will be used in steps 5 and 6.

After obtaining the K _k/k matrix, the next step will calculate the pose at the current moment based on the matrix.

Step 7: Calculate the normalized eigenvector corresponding to the maximum eigenvalue of the matrix K _k/k , where the eigenvector is the attitude q _k represented by the quaternion at the calculation moment. At this point, the calculation of the attitude at the current moment k is completed. Returning to step 1, the attitude calculation process at the next moment is continued until the attitude calculation is stopped at a certain moment.

Taking the above ideal embodiments according to the present invention as inspiration, and through the above description, relevant personnel can make various changes and modifications without departing from the scope of the present invention. The technical scope of the present invention is not limited to the contents in the specification, and the technical scope must be determined according to the scope of the claims.

Claims (2)

1. a self-adaptive optimal-REQUEST algorithm for inertial-geomagnetic combined attitude solution is characterized in that: comprise gyroscope, accelerometer and geomagnetic sensor, with the three-dimensional observation vector that gyroscope, accelerometer and geomagnetic sensor output form as input variables and include the following steps, Step 1: At each sampling time k, calculate

in,

and

are the three-dimensional observation vectors formed by the output of the accelerometer and the geomagnetic sensor at time k, respectively; ||·|| represents the modulo; × represents the orientation vector product; asin() represents the inverse sine;

represent

and

the angle between these two vectors; Step 2: Calculate

Among them, α ₁ and α ₂ are scale factors, abs( ) represents the absolute value, and g is the local gravitational acceleration vector; Step 3: In order to convert the

The value range is monotonically transformed into the [0,1] interval, therefore, computing

Step 4: Obtained according to Step 3

Calculate the "innovation" δK _k of the attitude,

In the formula, each symbol is defined as follows:

This value represents the sum of the weights assigned to all observation vectors by the optimal-REQUEST algorithm;

S=B+B ^T ,

I is the identity matrix;

and

are the representations of the same observation vector in the carrier coordinate system and the world coordinate system; where

The representations in the carrier coordinate system and the world coordinate system are respectively

and

and

The representations in the carrier coordinate system and the world coordinate system are respectively

and

Step 5: Calculate the "prediction" K _k/k-1 of the attitude, if k=0, that is, the initial calculation moment, then let K _k/k =δK _k , m _k =δm _k , P _k/k =R _k , where R _k is the observation noise covariance matrix, then skip directly to step 7, otherwise use the covariance matrix K _k-1/k-1 of the post-estimation error of the attitude at the previous moment to calculate the "prediction" of the attitude according to equation (10). ”, that is, a 4×4-dimensional K _k/k-1 matrix,

Then use equation (11) to calculate the attitude "prediction", that is, the covariance matrix P _k/k-1 of the estimation error of the attitude prior estimation,

Among them, Q _k-1 is the process noise covariance matrix; Φ _k-1 is the state transition matrix, and Φ _k-1 =exp(Ω _k-1 Δt), Ω _k-1 is an antisymmetric matrix, and the antisymmetric matrix Ω _k-1 is defined as

Among them, ω _k-1 is a three-dimensional vector formed by the output of the gyroscope; Step 6: Utilize the "innovation" of the attitude to realize the correction of the attitude "prediction", namely obtain K _k/k ; First, use the accumulated weight sum m _k-1 at the previous moment, the weight sum δm _k obtained in step 4, and the covariance matrix P _k/k-1 of the attitude prior estimation error obtained in step 5 to calculate the weighting factor ρ _k ,

Then use the accumulated weight sum m _k-1 at the previous moment and the weight sum δm _k obtained in step 4 to calculate the accumulated weight sum m _k at the current moment, m _k =(1-ρ _k )m _k-1 +ρ _k δm _k (14) With the accumulated weight value and m _k and weighting factor ρ _k of the current moment, the attitude correction can be realized, that is, the matrix K _k/k is obtained,

Finally, the covariance matrix P _k/k of the posterior estimation error of the attitude at the current moment is calculated,

Save P _k/k and m _k for the next round of calculation; Step 7: After the K _k/k matrix is obtained, the attitude at the current moment will be calculated according to the matrix, and the normalized eigenvector corresponding to the maximum eigenvalue of the matrix K _k/k will be calculated, and the eigenvector is the calculation moment. The attitude calculation result represented by the quaternion; return to step 1 for the next moment attitude calculation, until the user stops the attitude calculation at a certain moment. 2 . The adaptive optimal-REQUEST algorithm for inertial-geomagnetic combined attitude calculation according to claim 1 , wherein the calculation result of formula (1) is denoised by using a filtering technique. 3 .

Images