当前位置：网站首页>姿态解算-陀螺仪+欧拉法

姿态解算-陀螺仪+欧拉法

2022-08-11 05:35:00 【KPer_Yang】

参考博士论文《多旋翼无人机的姿态与导航信息融合算法研究》张欣

1、基本原理

姿态解算指的是求出导航坐标系（一般选择地理坐标系，用n表示）和载体坐标系（用b表示）在三个轴旋转的角度：俯仰角、横滚角和航向角。分别使用 $\theta , \gamma ,\psi$ 表示。姿态解算是欧拉角法和四元数在导航中的应用，而欧拉角法和四元数法可以运用在表示任意坐标系之间的旋转关系，并且四元数法还有其他更广泛的应用。一般使用陀螺仪和加速度计+磁力计两种组合，再使用卡尔曼滤波（或者卡尔曼滤波的改进算法，例如扩展卡尔曼滤波、无迹卡尔曼滤波算法）进行传感器融合得到更高的精度。陀螺仪具有短时精度高的特点。但是受到载体震动、温漂的影响较大，所以长时间会有较大的累积误差。

2、捷联矩阵

捷联矩阵指的是载体坐标系->地理坐标系转换的关系矩阵：旋转轴按照不同顺序进行转动得到不同的捷联矩阵，假设地理坐标系->载体坐标系的顺序按照 $\gamma ->\theta ->\psi$ 的顺序，则得到如下的捷联矩阵。根据传感器输出和捷联矩阵可以反求姿态角。

$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、陀螺仪+欧拉角法姿态解算

假设地理坐标系->载体坐标系的顺序按照 $\psi -> \theta -> \gamma$ 的顺序，则载体坐标系相对于地理坐标系的角速率向量：

$\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}$

从而得到微分方程：

$\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}$