当前位置：网站首页>Attitude solution - gyroscope + Euler method

Attitude solution - gyroscope + Euler method

2022-08-11 07:14:00 【KPer_Yang】

Table of Contents

3. Attitude solution by gyroscope + Euler angle method

Refer to the doctoral dissertation "Research on the Attitude and Navigation Information Fusion Algorithm of Multi-rotor UAV" Zhang Xin

1. Fundamentals

Attitude solution refers to finding the angle of rotation of the navigation coordinate system (generally the geographic coordinate system, represented by n) and the carrier coordinate system (represented by b) in three axes: pitch angle, roll angle and headinghorn.Use $\theta , \gamma ,\psi$ respectively.Attitude solution is the application of Euler angle method and quaternion in navigation, and Euler angle method and quaternion method can be used to represent the rotation relationship between arbitrary coordinate systems, and the quaternion method has other more extensiveApplications.Generally, two combinations of gyroscope and accelerometer + magnetometer are used, and then Kalman filter (or an improved algorithm of Kalman filter, such as extended Kalman filter, unscented Kalman filter algorithm) is used for sensor fusion to obtain higher accuracy.The gyroscope has the characteristics of high short-term accuracy.However, it is greatly affected by carrier vibration and temperature drift, so there will be a large cumulative error for a long time.

2. Strapdown matrix

The strapdown matrix refers to the relationship matrix of the carrier coordinate system->geographical coordinate system conversion: the rotation axis is rotated in different orders to obtain different strapdown matrices, assuming that the order of the geographic coordinate system->carrier coordinate system follows \theta ->\psi" class=mathcode src="//img.inotgo.com/imagesLocal/202208/11/202208110517347776_2.gif"> , then you get the following strapdownmatrix.The attitude angle can be reversed according to the sensor output and the strapdown matrix.

$C^n_b = (C^b_n)^T = \begin{bmatrix} cos\psi cos\theta & cos\psi sin\theta sin\gamma-sin\psi cos\gamma & cos\psi sin\theta cos\gamma+sin\psi sin\gamma \\ sin\psi cons\theta & sin\psi sin\theta sin\gamma+cos\psi cos\gamma & sin\psi sin \theta cos\gamma-cos\psi sin\gamma \\ -sin\theta & cos\theta sin\gamma & cos\theta cos\gamma \\ \end {bmatrix}$

3. Gyro + Euler angle method attitude solution

Assume the order of geographic coordinate system -> carrier coordinate system is in accordance with $\psi -> \theta -> \gamma$ Order, then the angular rate vector of the carrier coordinate system relative to the geographic coordinate system:

$\begin{bmatrix} \omega_x \\ \omega_y \\ \omega_z \end{bmatrix} = \begin{bmatrix} \ \dot{\gamma} \\ 0 \\ 0 \end{bmatrix} + C_\gamma\begin{bmatrix} 0\\ \dot{\theta} \\ 0\end{bmatrix} + C_\gamma C_\theta \begin{bmatrix}0 \\ 0 \\ \dot{\psi} \end{bmatrix}$

Therefore, the differential equation is obtained:

$\begin{bmatrix} \dot{\gamma} \\ \dot{\theta} \\ \dot{\psi} \end{bmatrix}= \frac{1}{cos\theta} \begin{bmatrix} cos\theta & sin\gamma sin\theta & cos\gamma sin\theta \\ 0 & cos\gamma cos\theta & -sin\gamma cos\theta \\ 0 & sin\gamma & cos\gamma \end{bmatrix}\begin{bmatrix} \omega_x \\ \omega_y \\ \omega_z \end{bmatrix}$

Use the first-order difference method to discretize and solve the attitude angle.But when $\theta = 90^0$ , denominator=0, which is EulerThe singularity of the angle cannot be measured at the full pose.