当前位置：首页 > news >正文

【知识杂记】陀螺仪直接积分就能获得角度吗？

news 2025/8/21 11:24:02

单轴与三轴陀螺仪的姿态解算：从欧拉角到四元数

惯性测量单元（IMU）在导航与控制领域扮演着核心角色，其核心任务之一便是姿态解算。然而，在陀螺仪的姿态解算中，单轴与三轴方案的数学处理方式存在根本差异。本文旨在阐明为何单轴陀螺仪可直接通过积分获取角度，而三轴系统则必须依赖四元数。

单轴陀螺仪的解算原理：平面旋转的简化

当物体运动被严格限定在单一旋转平面时，其姿态解算可被极大地简化。例如，在二维平面上测量偏航角（Yaw）的单轴陀螺仪。在此场景中，不存在轴间耦合效应，且旋转满足可交换性（Commutativity）。

其姿态更新可直接通过角速度对时间进行积分实现：

$dt\theta(t) = \theta(t_0) + \int_{t_0}^{t} \omega(t) \,dt$

这种方法之所以成立，是因为其物理模型已被约束为简单的平面运动，规避了三维旋转的复杂性。

三维旋转的挑战：为何直接积分不可行

相较于平面运动，三维空间中的旋转是一个非线性、非交换的过程。若对三轴陀螺仪测得的角速度（ $ωx,ωy,ωz\omega_x, \omega_y, \omega_z$ ）简单地进行积分并采用欧拉角（Roll, Pitch, Yaw）表示姿态，将面临以下三个核心难题：

万向锁问题（Gimbal Lock）
欧拉角在特定姿态（如俯仰角为 $±90∘\pm 90^\circ$ ）时，会使其中两个旋转轴重合，导致系统失去一个自由度，无法再区分该方向上的旋转。这使得姿态表示出现奇异性，导致解算算法在此状态下失效或不稳定。
旋转的不可交换性（Non-Commutativity）
三维空间中的旋转顺序会影响最终结果。即，先绕 $x$ 轴旋转再绕 $y$ 轴旋转，与先绕 $y$ 轴旋转再绕 $x$ 轴旋转，所得到的最终姿态是不同的。简单的积分无法捕捉这种内在的非交换性，从而导致姿态解算累积误差。
轴间耦合效应
陀螺仪测量的是物体在本体坐标系（Body Frame）下的角速度。当物体旋转时，这个坐标系自身也在随之旋转。这意味着，在一个轴上测得的角速度，实际上是该轴角速度与旋转导致的坐标系变换的综合效应。直接积分无法准确处理这种动态的轴间耦合关系。

四元数的解决方案：无奇异性的姿态表示

为克服欧拉角的局限性，四元数（Quaternion） 作为一种四维超复数被引入，用于表示三维空间中的旋转。其核心优势在于：

无奇异性：四元数可以无缝地表示三维空间中的任何旋转，不存在万向锁问题。
统一更新机制：四元数的姿态更新通过四元数微分方程实现，将三轴角速度作为一个整体进行处理，天然地遵循旋转的非交换性。

$dqdt=12q⊗ω\frac{d\mathbf{q}}{dt} = \frac{1}{2}\mathbf{q} \otimes \omega$

此方程将陀螺仪的测量值（ $ω\omega$ ）作为输入，通过四元数乘法（ $⊗\otimes$ ）实时更新姿态四元数（ $q\mathbf{q}$ ）。通过数值积分求解该方程，即可在避免万向锁的同时，精确地跟踪物体的动态姿态。

四元数的微分方程，如何求解这个方程？

四元数的微分方程

四元数的姿态更新核心在于将陀螺仪在本体坐标系（Body Frame）下的角速度测量值，转换为四元数在惯性坐标系（Inertial Frame）下的变化率。这个关系由四元数的微分方程来描述。

设一个四元数 $q=[q0,q1,q2,q3]T\mathbf{q} = [q_0, q_1, q_2, q_3]^T$ 表示从惯性坐标系到本体坐标系的旋转。设陀螺仪测量的角速度向量为 $ωb=[ωx,ωy,ωz]T\boldsymbol{\omega}_b = [\omega_x, \omega_y, \omega_z]^T$ 。

我们将角速度向量 $ωb\boldsymbol{\omega}_b$ 表示为一个纯四元数（Pure Quaternion），即标量部分为0，向量部分为 $ωb\boldsymbol{\omega}_b$ ：

$ω=[0,ωx,ωy,ωz]T\boldsymbol{\omega} = [0, \omega_x, \omega_y, \omega_z]^T$

四元数的微分方程为：

$q˙=12q⊗ω\dot{\mathbf{q}} = \frac{1}{2} \mathbf{q} \otimes \boldsymbol{\omega}$

其中：

$q˙\dot{\mathbf{q}}$ 是四元数 $q\mathbf{q}$ 对时间的导数，表示其变化率。
$⊗\otimes$ 表示四元数乘法。

这个方程的意义在于，它用一种简洁优雅的形式，将**当前姿态（ $q\mathbf{q}$ ）和当前旋转角速度（ $ω\boldsymbol{\omega}$ ）**联系起来，给出了姿态随时间的变化率。

将其展开为矩阵形式，可以更直观地看出其计算过程：

$[q˙0q˙1q˙2q˙3]=12[0−ωx−ωy−ωzωx0ωz−ωyωy−ωz0ωxωzωy−ωx0][q0q1q2q3]\begin{bmatrix} \dot{q}_0 \\ \dot{q}_1 \\ \dot{q}_2 \\ \dot{q}_3 \end{bmatrix} = \frac{1}{2} \begin{bmatrix} 0 & -\omega_x & -\omega_y & -\omega_z \\ \omega_x & 0 & \omega_z & -\omega_y \\ \omega_y & -\omega_z & 0 & \omega_x \\ \omega_z & \omega_y & -\omega_x & 0 \end{bmatrix} \begin{bmatrix} q_0 \\ q_1 \\ q_2 \\ q_3 \end{bmatrix}$

微分方程的求解：两种主要方法

在实际应用中，陀螺仪的角速度是离散采样的，因此我们需要在每个采样周期内求解这个微分方程。常用的求解方法有两种：一阶近似和二阶近似。

设采样周期为 $Δt\Delta t$ ，在时刻 $k$ 的角速度为 $ωk\boldsymbol{\omega}_k$ 。

1. 一阶龙格-库塔法（欧拉法）

这是最简单、最常用的求解方法，本质上是将微分方程进行一阶泰勒展开。

原理：我们假设在短时间 $Δt\Delta t$ 内，角速度 $ωk\boldsymbol{\omega}_k$ 保持恒定。那么四元数的更新可以近似为：

$qk+1=qk+Δt⋅q˙k\mathbf{q}_{k+1} = \mathbf{q}_k + \Delta t \cdot \dot{\mathbf{q}}_k$

具体步骤：
1. 将陀螺仪测量的角速度 $ωk=[ωx,ωy,ωz]T\boldsymbol{\omega}_k = [\omega_x, \omega_y, \omega_z]^T$ 转换为一个增量旋转四元数 $Δqk\Delta\mathbf{q}_k$ 。这个增量四元数表示在 $Δt\Delta t$ 时间内的微小旋转。
$Δqk=[cos⁡(∥ωk∥Δt2),ωx∥ωk∥sin⁡(∥ωk∥Δt2),ωy∥ωk∥sin⁡(∥ωk∥Δt2),ωz∥ωk∥sin⁡(∥ωk∥Δt2)]T\Delta\mathbf{q}_k = \left[ \cos\left(\frac{\left\lVert \boldsymbol{\omega}_k \right\rVert \Delta t}{2}\right), \frac{\omega_x}{\left\lVert \boldsymbol{\omega}_k \right\rVert} \sin\left(\frac{\left\lVert \boldsymbol{\omega}_k \right\rVert \Delta t}{2}\right), \frac{\omega_y}{\left\lVert \boldsymbol{\omega}_k \right\rVert} \sin\left(\frac{\left\lVert \boldsymbol{\omega}_k \right\rVert \Delta t}{2}\right), \frac{\omega_z}{\left\lVert \boldsymbol{\omega}_k \right\rVert} \sin\left(\frac{\left\lVert \boldsymbol{\omega}_k \right\rVert \Delta t}{2}\right) \right]^T$
1. 利用四元数乘法来更新姿态。
$qk+1=qk⊗Δqk\mathbf{q}_{k+1} = \mathbf{q}_k \otimes \Delta\mathbf{q}_k$
优点：计算简单、实时性好。
缺点：当角速度较大时，其近似误差也会随之增大。

2. 二阶龙格-库塔法

为了提高精度，尤其是在高动态运动下，可以使用二阶或更高阶的求解方法。这里以二阶法为例。

原理：它利用了当前时刻和下一个时刻的角速度信息来提高求解精度。
具体步骤：
1. 计算初始的增量旋转四元数 $Δq1\Delta\mathbf{q}_1$ (与一阶法相同)。
$]T\Delta\mathbf{q}_1 = \left[ \cos\left(\frac{\left\lVert \boldsymbol{\omega}_k \right\rVert \Delta t}{2}\right), \dots \right]^T$
1. 计算中间的角速度。首先用 $Δq1\Delta\mathbf{q}_1$ 预测一个中间姿态 $qmid=qk⊗Δq1\mathbf{q}_{mid} = \mathbf{q}_k \otimes \Delta\mathbf{q}_1$ 。然后将陀螺仪在 $k + 1$ 时刻的测量值 $ωk+1\boldsymbol{\omega}_{k+1}$ 转换为本体坐标系下的角速度，并计算一个修正的增量四元数 $Δq2\Delta\mathbf{q}_2$ 。
2. 结合两者，得到最终的更新四元数。由于计算复杂，通常在工程中会使用近似或等效的等效旋转矢量方法来简化计算。
优点：精度显著高于一阶方法，尤其适用于高动态环境。
缺点：计算量增加。