前言

       欧拉角、四元数与旋转矩阵，是构成姿态解算理论体系不可或缺的三大基石，深刻理解其特性与内在转换关系是迈向精准姿态估计的第一步。

欧拉角

定义：用三个绕特定坐标轴的连续旋转角度来描述姿态。最常见的顺序是偏航（Yaw）- 俯仰（Pitch）- 滚动（Roll）。

        偏航（ψ）：绕Z轴旋转，指向前后方向的变化。

        俯仰（θ）：绕Y轴旋转，指向上下方向的变化。

        滚动（φ）：绕X轴旋转，绕自身前后轴线的旋转。

优点：

        非常直观：飞行员和工程师可以直接理解“机头抬升20度，右倾10度”这样的描述。

缺点：

        万向节死锁：这是欧拉角最致命的缺陷。当俯仰角为±90度时，偏航和滚转的旋转轴会重合，丢失一个自由度，导致系统出现奇异性。

        计算复杂：进行多次旋转叠加时，计算效率低。

旋转矩阵

        旋转矩阵是一个3x3的正交矩阵

定义：它的每一列代表新坐标系的X, Y, Z轴单位向量在原坐标系下的坐标。它可以直接用来旋转一个三维向量。

优点：

        无奇异性：没有万向节死锁问题。

        旋转向量直接：与向量乘法即可完成旋转，非常方便。

        组合简单：多个旋转可以直接通过矩阵乘法连接。

缺点：

        冗余：9个数字只表示3个自由度，存在6个约束（正交且行列式为1）。

        效率：存储和计算相比四元数更耗费资源。

        数值误差：多次运算后可能不再严格正交，需要重新正交化。

四元数

        四元数是一种扩展的复数，由一个实部和三个虚部组成，通常表示为 q = [w, x, y, z] 或 q = w + xi + yj + zk。

定义：用于表示旋转的单位四元数满足 w² + x² + y² + z² = 1。它可以被解释为绕一个轴 [x, y, z] 旋转一个角度 θ，其中：

        w = cos(θ/2)

        (x, y, z) = sin(θ/2) * (axis_x, axis_y, axis_z)

优点：

        紧凑：只有4个元素，比旋转矩阵更节省存储空间。

        无奇异性：没有万向节死锁问题。

        计算高效：组合旋转和旋转向量所需的运算量比矩阵少。

        数值稳定：比旋转矩阵更容易保持单位性质。

缺点：

        不直观：人类很难直接理解一个四元数的几何意义。

转换方法

        在姿态解算中，陀螺仪提供的角速度数据是基础。通过特定的积分算法（通常基于四元数微分方程），我们可以从角速度持续地更新出描述整体旋转的四元数。而四元数、欧拉角和旋转矩阵作为描述旋转的不同数学工具，彼此之间存在着确定的转换关系。

其他转换方法可以看这个网址
 

四元数 → 欧拉角

以z y x为顺序 
偏航ψ， 俯仰θ， 横滚φ
/*************************************/
四元数 q = [w, x, y, z]
欧拉角 [ψ, θ, φ]（偏航、俯仰、横滚）
θ = asin(2(wy - xz))
φ = atan2(2(wx + yz), 1 - 2(x² + y²))
ψ = atan2(2(wz + xy), 1 - 2(y² + z²))
/************************************************/
四元数 = [实部, 虚部x, 虚部y, 虚部z]
欧拉角 [偏航角, 俯仰角, 横滚角]
俯仰角 = 反正弦(2(实部×虚部y - 虚部x×虚部z))
横滚角 = 反正切2(2(实部×虚部x + 虚部y×虚部z), 1 - 2(虚部x² + 虚部y²))
偏航角 = 反正切2(2(实部×虚部z + 虚部x×虚部y), 1 - 2(虚部y² + 虚部z²))

四元数 → 旋转矩阵

四元数q = [w, x, y, z]
旋转矩阵R = [
  [1 - 2y² - 2z²,     2xy - 2wz,       2xz + 2wy],
  [2xy + 2wz,         1 - 2x² - 2z²,   2yz - 2wx],
  [2xz - 2wy,         2yz + 2wx,       1 - 2x² - 2y²]
]

这里说明一下，欧拉角通常是我们直接使用的数据量，旋转矩阵通常是我们做姿态转换或坐标系转换的重要数学工具。

欧拉角 → 四元数

以z y x为顺序 
偏航ψ， 俯仰θ， 横滚φ
四元数_x = [cos(φ/2), sin(φ/2), 0, 0]
四元数_y = [cos(θ/2), 0, sin(θ/2), 0]
四元数_z = [cos(ψ/2), 0, 0, sin(ψ/2)]
四元数 = 四元数_z × 四元数_y × 四元数_x
/*********************************************/
实部 = cos(ψ/2)cos(θ/2)cos(φ/2) + sin(ψ/2)sin(θ/2)sin(φ/2)
虚部x = cos(ψ/2)cos(θ/2)sin(φ/2) - sin(ψ/2)sin(θ/2)cos(φ/2)
虚部y = cos(ψ/2)sin(θ/2)cos(φ/2) + sin(ψ/2)cos(θ/2)sin(φ/2)
虚部z = sin(ψ/2)cos(θ/2)cos(φ/2) - cos(ψ/2)sin(θ/2)sin(φ/2)

姿态解算原理

        虽然从理论上讲，通过对陀螺仪测得的角速度进行积分即可实现姿态解算，但陀螺仪的误差会导致积分结果产生严重的漂移，因此在实际应用中必须引入加速度计和磁力计数据进行修正。尽管如此，忽略噪声和漂移的理想角速度积分模型，仍然是理解姿态解算核心原理最直接、最有效的方法。

理论：

       通过对比欧拉角、四元数和旋转矩阵的特性可知，四元数因其无万向节锁、计算效率高及插值性能好等优点，尤为适合在计算资源有限的嵌入式平台上进行姿态解算。基于此，解算流程可设计如下：首先，系统获取陀螺仪测量的角速度并设定初始四元数；接着，通过离散化的四元数微分方程对当前四元数进行更新；最后，将更新后的四元数转换为欧拉角，以供后续应用。

方法：

基本概念：
四元数微分方程：用于描述四元数如何随时间变化的
公式：dq/dt = 0.5 · q ⊗ [0,ω_x, ω_y, ω_z]
q ：是当前的四元数
ω_x, ω_y, ω_z ：绕X轴、Y轴、Z轴的角速度
⊗： 表示四元数乘法

前期准备

在通过离散化的四元数微分方程前需要确定初始四元数，这里可以使用加速度计得到，也可以直接设为单位四元数 q_old = [1, 0, 0, 0]通过跟新迭代逐渐得到正确的四元数(会有四元数收敛时间过长且期间姿态估计都是错误的问题)。

使用加速度计确定初始四元数

// 加速度计初始姿态确定流程

步骤1：采集静止状态下的加速度计数据
在系统静止时，连续采集多组加速度计数据并取平均：
a_x_avg = 平均(a_x1, a_x2, ..., a_xN)
a_y_avg = 平均(a_y1, a_y2, ..., a_yN)  
a_z_avg = 平均(a_z1, a_z2, ..., a_zN)

步骤2：计算横滚角和俯仰角
横滚角 φ = atan2(a_y_avg, a_z_avg)
俯仰角 θ = atan2(-a_x_avg, sqrt(a_y_avg² + a_z_avg²))

步骤3：设定初始偏航角（航向角）
// 如果没有磁力计，通常将初始偏航角设为0
偏航角 ψ = 0

步骤4：将欧拉角转换为四元数
// 使用Z-Y-X旋转顺序（偏航-俯仰-横滚）
cy = cos(ψ/2), sy = sin(ψ/2)
cp = cos(θ/2), sp = sin(θ/2)  
cr = cos(φ/2), sr = sin(φ/2)

初始四元数 q_old = [
    w = cr*cp*cy + sr*sp*sy,
    x = sr*cp*cy - cr*sp*sy,
    y = cr*sp*cy + sr*cp*sy, 
    z = cr*cp*sy - sr*sp*cy
]

如果可以确定要测量的物体的初始状态，可以直接记录它的欧拉角转换成四元数(或者直接记录四元数)作为初始四元数。

伪代码

四元数姿态更新流程


离散化更新过程
步骤1：计算四元数的变化率（导数）
实部(w)的变化率：
dw/dt = 0.5 × (-ω_x·x - ω_y·y - ω_z·z)
虚部x的变化率
dx/dt = 0.5 × (ω_x·w + ω_y·z - ω_z·y)
虚部y的变化率：
dy/dt = 0.5 × (ω_y·w - ω_x·z + ω_z·x)
虚部z的变化率：
dz/dt = 0.5 × (ω_z·w + ω_x·y - ω_y·x)

步骤2：数值积分更新四元数
更新实部w：
w_新 = w_旧 + (dw/dt) × Δt
更新虚部x：
x_新 = x_旧 + (dx/dt) × Δt
更新虚部y：
y_新 = y_旧 + (dy/dt) × Δt
更新虚部z：
z_新 = z_旧 + (dz/dt) × Δt
其中Δt是采样时间间隔。

步骤3：归一化处理
由于数值积分会引入误差，导致四元数模长偏离1，必须进行归一化：
计算模长：
模长 = √(w_新² + x_新² + y_新² + z_新²)
归一化每个分量：
w_归一化 = w_新 / 模长
x_归一化 = x_新 / 模长
y_归一化 = y_新 / 模长
z_归一化 = z_新 / 模长

步骤4：转换为其他表示（根据需要）
四元数 → 旋转矩阵
四元数 → 欧拉角

实际计算示例

假设：
当前四元数：q = [0.707, 0, 0.707, 0]
角速度：ω_x = 0.1, ω_y = 0.2, ω_z = 0.3 rad/s
时间间隔：Δt = 0.01秒

计算变化率：
dw/dt = 0.5 × (-0.1×0 - 0.2×0.707 - 0.3×0) = -0.0707
dx/dt = 0.5 × (0.1×0.707 + 0.2×0 - 0.3×0.707) = -0.0707
dy/dt = 0.5 × (0.2×0.707 - 0.1×0 + 0.3×0) = 0.0707
dz/dt = 0.5 × (0.3×0.707 + 0.1×0.707 - 0.2×0) = 0.1414
更新四元数：
w_新 = 0.707 + (-0.0707)×0.01 = 0.7063
x_新 = 0 + (-0.0707)×0.01 = -0.0007
y_新 = 0.707 + 0.0707×0.01 = 0.7077
z_新 = 0 + 0.1414×0.01 = 0.0014
归一化：
模长 = √(0.7063² + (-0.0007)² + 0.7077² + 0.0014²) ≈ 1.000

六轴姿态解算       

        六轴姿态传感器通常集成了三轴陀螺仪与三轴加速度计。然而，在动态环境下，加速度计的Z轴测量值难以有效区分物体运动加速度与重力加速度，导致其无法独立、可靠地用于估算绕Z轴的偏航角。因此，在仅使用六轴数据解算姿态时，通常只能精确修正由陀螺仪积分产生的横滚角与俯仰角的漂移。

        基于此，六轴姿态解算普遍采用以陀螺仪积分为主体、以加速度计测量为修正的策略。其核心原理是利用互补滤波或卡尔曼滤波等算法，融合陀螺仪优秀的短期动态特性与加速度计提供的长期静态基准，以此抑制姿态估计的累积漂移。

陀螺仪与加速度计

陀螺仪(角速度)

作用：可以直接测量姿态变化，可通过积分得到角度变化量，短期精度高，动态性能好，不受线性运动影响

缺点（致命弱点）：漂移随着时间不断累积，导致最终的误差越来越大

加速度计(加速度)

作用：可以提供绝对姿态基准（只有俯仰和横滚），无漂移，长期稳定的数值不会随时间漂移

缺点：对振动和线性加速度极度敏感，动态响应差，无法提供航向角

解决方法

通过上面的分析，我们可以清晰地看到：

        陀螺仪：短期准，长期飘。

        加速度计：长期准，短期抖。

它们俩的优缺点几乎是完美互补的。因此，在姿态解算中需要将它们的数据融合起来(角速度为主加速度为辅)。且由于角速度与加速度的特性，一般而言角速度是用于追踪快速变化，而加速度则是用于提供绝对基准和校正漂移。

融合方案

角速度前期处理

        将角速度转换为四元数，具体步骤请看上文。

加速度前期处理

        需要求出归一化后的重力向量

//加速度计数据的预处理流程

步骤1：原始数据读取
从MPU6050读取加速度计原始数据：
a_x_raw, a_y_raw, a_z_raw  // 原始ADC值或已经过初步校准的值

步骤2：单位转换和校准
转换为标准单位：
a_x = (a_x_raw - 零偏_x) × 尺度因子_x
a_y = (a_y_raw - 零偏_y) × 尺度因子_y  
a_z = (a_z_raw - 零偏_z) × 尺度因子_z
校准参数获取：
零偏：传感器在无加速度时的输出值
尺度因子：将原始值转换为实际加速度值的比例系数

步骤3：低通滤波
去除高频噪声：
a_x_filt = α × a_x + (1-α) × a_x_prev
a_y_filt = α × a_y + (1-α) × a_y_prev
a_z_filt = α × a_z + (1-α) × a_z_prev
其中 α 是滤波系数（通常0.1-0.3）

步骤4：归一化处理
将加速度向量归一化为单位向量：
模长 = √(a_x_filt² + a_y_filt² + a_z_filt²)
a_x_norm = a_x_filt / 模长
a_y_norm = a_y_filt / 模长  
a_z_norm = a_z_filt / 模长
归一化后的重力向量：
重力向量 = [a_x_norm, a_y_norm, a_z_norm]

融合算法一——互补滤波法

计算加速度计提供的姿态角:
俯仰角_accel = atan2(-a_x_norm, √(a_y_norm² + a_z_norm²))
横滚角_accel = atan2(a_y_norm, a_z_norm)

互补滤波法
// 陀螺仪积分姿态(这里建议使用四元数积分法)
俯仰角_gyro = 俯仰角_prev + ω_y × Δt
横滚角_gyro = 横滚角_prev + ω_x × Δt
// 互补融合
俯仰角 = α × 俯仰角_gyro + (1-α) × 俯仰角_accel
横滚角 = α × 横滚角_gyro + (1-α) × 横滚角_accel

融合算法二——卡尔曼滤波法

/*
扩展卡尔曼滤波大致流程
*/
更新四元数

状态方程建立：
状态向量 x = [w, x, y, z, bwx, bwy, bwz]^T
四元数的四个分量 w, x, y, z
陀螺仪三个轴的零偏bwx, bwy, bwz

观测模型建立
重力向量作为观测值：
观测向量 z = [a_x_norm, a_y_norm, a_z_norm]ᵀ
观测方程：z = h(x) + v
h(x) 是从状态向量（姿态）预测的重力向量
v 是观测噪声

预测的重力向量
从当前姿态四元数 q = [w, x, y, z] 预测重力向量：
预测重力向量 = [
  2×(x×z - w×y),
  2×(w×x + y×z), 
  w×w - x×x - y×y + z×z
]

残差 = 测量重力向量 - 预测重力向量

其他融合方法与关键问题

其他的融合方法还有Mahony滤波法、Madgwick滤波法，这四个方法是最常用的方法了。

关键问题：当载体有线性加速度时，加速度计测量值 ≠ 重力向量

加速度模长 = √(a_x² + a_y² + a_z²)
if |加速度模长 - 9.8| > 阈值 then
    // 存在明显运动加速度
    降低加速度计权重或暂停校正
else
    // 主要受重力影响，可信度高
    正常使用加速度计校正
endif

关键问题：根据运动状态动态调整融合系数

运动强度 = √(ω_x² + ω_y² + ω_z²) + |加速度模长 - 9.8|

if 运动强度 < 低阈值 then
    α = 0.95  // 静态，信任加速度计
else if 运动强度 > 高阈值 then  
    α = 0.99  // 剧烈运动，信任陀螺仪
else
    α = 0.98  // 正常运动
endif

常用加速运算方法：

避免重复计算平方根和三角函数

定期归一化四元数

固定时间间隔处理

结束语

        本篇文章主要是记录六轴姿态传感器是如何从数学理论模型到实际实现的思路，主要由伪代码构成。