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

最新推荐文章于 2024-04-22 17:37:28 发布

KPer_Yang

最新推荐文章于 2024-04-22 17:37:28 发布

阅读量6.3k

点赞数 7

分类专栏：控制\SLAM\机器人学信号处理算法数学文章标签：机器学习算法

本文链接：https://blog.csdn.net/KPer_Yang/article/details/125794839

版权

信号处理算法同时被 3 个专栏收录

38 篇文章 17 订阅

订阅专栏

控制\SLAM\机器人学

15 篇文章 3 订阅

订阅专栏

数学

13 篇文章 5 订阅

订阅专栏

1、基本原理

2、捷联矩阵

3、陀螺仪+欧拉角法姿态解算

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

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