三轴陀螺仪解算姿态（四元数）_陀螺仪四元数

原理

三轴陀螺仪可以测量载体在三个轴上的角速度分量，对这些角速度进行积分就可以得到旋转的角度，应用到载体上就可以得到载体的姿态。

方法

假设导航坐标系为东北天，载体坐标系为右前上。

初始载体坐标系和导航坐标系重合，对应的四元数为q=[1,0,0,0]，使用此四元数表示载体在导航坐标系下的旋转。

三轴陀螺仪测量的三个角速度分量可以合成一个角速度向量，可以理解为载体绕着这个角速度向量进行旋转，旋转的角度为角速度向量模的积分。

设 g y r o → = [ ω x b ω y b ω z b ] \overrightarrow{gyro}=

$$\begin{bmatrix} \omega_{xb} \\ \omega_{yb} \\ \omega_{zb} \end{bmatrix}$$ gyro ​= ​ωxb​ωyb​ωzb​​ ​为陀螺仪测得的载体旋转的角速度向量，时间间隔为$dt$，则旋转向量为

ω b → = [ ω x b ω y b ω z b ] ⋅ d t \overrightarrow{\omega_{b}} =

$$\begin{bmatrix} \omega_{xb} \\ \omega_{yb} \\ \omega_{zb} \end{bmatrix}$$ \cdot dt ωb​ ​= ​ωxb​ωyb​ωzb​​ ​⋅dt

将其转换到导航坐标系
$$\overrightarrow{{\omega }_n}=q\otimes \overrightarrow{{\omega }_b}\otimes q^{\ast }$$

其中$\overrightarrow{{\omega }_n}$为旋转轴，$\left|\overrightarrow{{\omega }_n}\right|$为旋转的角度$\theta$，转换成四元数为

q ′ = [ sin ⁡ ( θ 2 ) ω → ⋅ sin ⁡ ( θ 2 ) ] q^{'}=

$$\begin{bmatrix} \sin(\frac{\theta}{2}) & \overrightarrow{\omega} \cdot \sin(\frac{\theta}{2})\\ \end{bmatrix}$$ q′=[sin(2θ​)​ω ⋅sin(2θ​)​]

其中$\overrightarrow{{\omega }_n}$需要归一化，将其应用到初始四元数即可得到当前姿态的四元数
$$q=q^{{}^{\prime }}\otimes q$$

以下是使用Eigen3的代码示例

#include "Eigen/Core"
#include "Eigen/Geometry"
#include <cmath>

// 陀螺仪测量数据
float gyro[3];

// 初始四元数
Eigen::Quaternionf quaternion = Eigen::Quaternionf(1.0f, 0.0f, 0.0f, 0.0f);

while (1) {
	// 读取陀螺仪数据，单位mdegree/s
	read_data(gyro);

	// 转为向量，单位degree/s
	Eigen::Vector3f gyroscope = Eigen::Vector3f(gyro[0] / 1000.0f, gyro[1] / 1000.0f, gyro[2] / 1000.0f);

	// 时间间隔dt，单位s
    float dt = get_dt() / 1000.0f;
    
    // 角速度向量，单位rad/s
    Eigen::Vector3f omega = gyroscope * M_PI / 180.0f * dt;
    
    // 转换到导航坐标系
    omega = quaternion.toRotationMatrix() * omega;
    
    // 旋转角度
    float theta = omega.norm();
	// 旋转对应的四元数
    omega = omega.normalized() * std::sin(theta / 2.0f);
    Eigen::Quaternionf q = Eigen::Quaternionf(std::cos(theta / 2.0f), omega.x(), omega.y(), omega.z());

	// 应用旋转
    quaternion = (q * quaternion).normalized();
}
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35

陀螺仪可以解算出载体的俯仰角、滚转角和偏航角，但是因为积分的原因，误差会进行积累，时间一长姿态就会不准确。