四元数法求解欧拉角

最新推荐文章于 2025-03-20 20:37:44 发布

善良的龙夫山泉2.0

最新推荐文章于 2025-03-20 20:37:44 发布

阅读量2k

点赞数 1

文章标签：算法

本文链接：https://blog.youkuaiyun.com/qq_50093870/article/details/130952325

版权

3 如何通过mpu6050获得角度呢？

1、mpu6050有3轴角速度计，和3轴角速度计，即可以测得3个方向的角速度和加速度共6个数据。

但是按理说，对角速度进行积分就能得到角度了，为什么还要用什么卡尔曼滤波以及四元数法之类的呢？

看看AI的解释：

陀螺仪测得的角速度在积分时会产生累积误差，这是由于陀螺仪本身存在的随机噪声和漂移误差导致的。这些误差会随着时间的移而逐渐累积，导致积分结果与真实值之间存在偏差。

具体来说，陀螺仪测得的角速度信包含两部分：真实角速度和随机噪声。随机噪声是由于陀螺仪本身存在的不确定性和环境干扰等因素导致的，它的大小和方向都是随机的。漂移误差是由于陀螺仪的输出信号存在一个固定的差，它的大小和方向在一段时间内是相对稳定的。

当我们对陀螺仪测得的角速度进行积分时，随机噪声会在积分过程中被积分掉，但漂移误差会随着时间的推移而逐渐累积。这会导致积分结果与真实值之间存在偏差，这个偏差会随着时间的推移而逐渐增大。

为了减小这种误差，通常需要用一些校准和补偿方法。例如，可以通过对陀螺仪进行温度补偿、零偏校准和比例因子校准等来减小漂移误差。此外，还可以采用卡尔曼滤波等方法来对陀螺仪输出信号进行滤波和融合，从而减小随机噪声的影响。

所以使用卡尔曼滤波以及四元数法都是为了减少随机噪音，并且这是一个正值。

这里我已经运用了陀螺仪，剩下的就要用加速度来修正这个噪音了。

2、如何通过加速计获得修正值呢？

通过重力在加速计上在物体移动前后的偏差获得，可以看以下步骤，用四元素表示：

1）先将重力加速度映射到物体当前姿态的坐标系中

在这里插入图片描述

gb为当前坐标系，[0,0,1]为世界坐标系下重力加速度归一化之后的坐标值。

Mq为旋转矩阵，可以用四元数表示，具体表示如下：

在这里插入图片描述

所以可以得到重力加速度的分量如下：

在这里插入图片描述

Gb = （Vx，Vy，Vz）；

2）将加速度计测得的归一化之后的重力加速度与上述加速度进行叉级运行算便能得到误差的一个修正值，将修正值加入测得的角速度中便能得到较为准确的角速度。

    exInt = exInt + ex * Ki * deltaT;
    eyInt = eyInt + ey * Ki * deltaT;
    ezInt = ezInt + ez * Ki * deltaT;

    gx = gx + Kp * ex + exInt;
    gy = gy + Kp * ey + eyInt;
    gz = gz + Kp * ez + ezInt;

3、由于我们整个过程都是通过四元数法描述的三维空间旋转的问题，所以我们求解的就是描述旋转的四元数的值，也就是Q=(q1,q2,q3,q4)的值。

1、什么是四元数？

来看看ai的解释：

四元数是一种数学工具，它可以用来表示旋转。它由一个实部和三个虚部组成，通常表示为q = w + xi + yj + zk，其中w是实部，(x, y, z)是虚部在姿态解算中，四元数通常用来表示旋转的姿态。它可以避免欧拉角的万向锁问题，并且可以方便地进行插值和积分。具体来说，四元数可以通过以下公式与旋转矩阵之间进行转换：

q = [cos(theta/2), sin(theta/2) * ux, sin(theta/2) * uy, sin(theta/2) * uz]

其中，theta是旋转角度，(ux, uy, uz)是旋转轴的单位向量。这个公式可以将旋转矩阵转换为四元数，也可以将四元数转换为旋转矩阵。

在姿态解算中，我们通常使用陀螺仪测量角速度，然后通过积分计算出旋转角度。然后，我们可以将旋转角度转换为四元数，从而得到当前的姿态。在实际应用中，我们通常需要对四元数进行校准和补偿，以减小误差。

所以四元数可以描述三维物体的旋转

2、至于四元数是如何描述空间旋转的呢？来看看AI 举得例子

假设我们有一个三维物体，它的初始姿态为单位矩阵（即没有旋转）。现在，我们想要将这个物体绕着y轴旋转90度。我们可以使用以下公来计算旋转四元数：

q = [cos(45), 0, sin(45), 0]

其中，45度是90度的一半，因为四元数中的虚部是旋转轴的单位向量，所以y轴的单位向量可以表示为[0, 1, 0]。这个公式可以将旋转矩阵转换为四元数。

然后，我们可以将旋转四元数应用到物体上，从而实现旋转。具体来说，我们可以将物体的初始姿态表示为一个四元数q0，然后将旋转四元数q应用到q0上，得到新的姿态q1。这个过程可以表示为：

q1 = q * q0 * q'

其中，*表示四元数的乘法，q’q的共轭。最后，我们可以将新的姿态q1转换为旋转矩阵，从而得到旋转后的物体。

其中q我们称为旋转四元数：

具体的解释如下：旋转四元数是一种四元数，它可以用来表示三维空间中的旋转。具体来说，旋转四元数由一个实部和三个虚部组，通常表示为q = w + xi + yj + zk，其中w是实部，(x, y,)是虚部。

旋转四元的虚部是旋转轴的单位向量，实部则是旋转的角度的余弦值的一半。因此，旋转四元数用来表示绕任意轴旋转一定角度的旋转变换。

旋转四元数的优点于它可以避免欧拉角的万向锁问题，并且可以方便地进行插值和积分。在实际应用中，我们通常使用四元数来表示姿态，并且可以使用四元数进行插值和分等操作。

需要注意的是，旋转四元数并不是唯一的，因为它们可以通过乘一个常数因子来表示相同的旋转。因此，在实际应用中，通常对旋转四元数进行归一化，以保它们具有单位长度。

所以我们要求的就是这个旋转四元数，怎么求呢？

4、旋转四元数的求解：

通过已知的旋转轴和旋转角可以求得旋转四元数，但是问题的核心在于我们求的是旋转角怎么可能已知旋转角呢？虽然不知道旋转角，但是我们知道旋转的的角速度，能否通过旋转的角速度求得呢？

在这里插入图片描述

这里有以一个关于旋转四元数和角速度的微分方程。

首先需要了解的一点是如何求解微分方程的近似解——龙格库塔公式

龙格-库塔（Runge-Kutta）方法是一种数值求解常微分方程的方法，它通过迭代逼近精确解。龙格-库塔方法的基本思想是将微分方程转化一系列的差分方程，然后使用迭代方法逐步逼近精确解。

最常用的龙格-库塔方法是四阶龙格-库塔方法，它的公式如下：

k1 = f(tn, yn)
k2 = f(tn + h/2, yn + h/2*k1)
k3 = f(tn + h/2, yn + h/2*k2)
k4 = f(tn + h, yn + h*k3)
yn+1 = yn + h/6*(k1 + 2*k2 + 2*k3 + k4)

其中，tn表示当前时刻，yn表示当前状态，h表示时间步长，f(tn, yn)表示微分方程的右侧函数。这个公式中，k1、k2、k3和k4是中间变量，用来计算yn+1。具体来说，k1表示在当前状态下微分方程的斜率，k2、k3和k4表示在当前状态下微分方程的斜率的加权平均值。

所以我们通过龙格库塔公式动态的更新旋转四元数
在这里插入图片描述

5、旋转四元数转欧拉角

得到旋转的四元数之后在通过公式即可获得欧拉角

yaw = atan2(2 * q1 * q2 + 2 * q0 * q3, -2 * q2 * q2 - 2 * q3 * q3 + 1) * 180 / pi;  // 航向角
pitch = asin(-2 * q1 * q3 + 2 * q0 * q2) * 180 / pi;  // 俯仰角
roll = atan2(2 * q2 * q3 + 2 * q0 * q1, -2 * q1 * q1 - 2 * q2 * q2 + 1) * 180 / pi;  // 翻滚角