4、加速度计姿态解算：利用重力矢量计算横滚角与俯仰角
加速度计是惯性测量单元中最直观的传感器之一，它测量的是比力（Specific Force），即物体所受的非引力加速度之和。在静止或匀速直线运动状态下，加速度计的输出矢量恰好与重力矢量大小相等、方向相反。这一物理特性使得我们可以利用加速度计的三轴输出，直接解算出载体相对于水平面的横滚角（Roll）与俯仰角（Pitch）。本章将深入剖析这一解算过程的数学原理、工程实现方法、典型应用场景以及常见误区，帮助你在嵌入式系统中稳健地实现基于加速度计的初始姿态获取。
4.1 核心原理：从比力到姿态角的数学映射
加速度计的核心测量模型可以表示为：a_meas = g - a_linear，其中 a_meas 是加速度计输出的比力矢量，g 是重力加速度矢量（在导航坐标系中恒指向地心），a_linear 是载体自身的线性加速度。当载体处于静止或匀速运动状态时，a_linear = 0，此时加速度计的输出矢量与重力矢量大小相等、方向相反。由于重力矢量在导航坐标系（通常取北东地或东北天）中的方向是已知且恒定的，因此通过测量加速度计三轴输出在载体坐标系中的投影，就可以反推出载体坐标系相对于导航坐标系的旋转关系，进而提取出横滚角和俯仰角。
设载体坐标系为 b系（Xb前、Yb右、Zb下），导航坐标系为 n系（北东地）。当载体存在横滚角 φ 和俯仰角 θ 时，重力矢量在载体坐标系中的投影可以表示为：
ax = -g · sin(θ)
ay = g · sin(φ) · cos(θ)
az = g · cos(φ) · cos(θ)其中 g 为重力加速度大小（约 9.8 m/s²）。从上述方程组中，我们可以直接解算出横滚角和俯仰角：
φ = atan2(ay, az)
θ = atan2(-ax, sqrt(ay² + az²))这里使用 atan2 函数而非普通的反正切函数，是因为 atan2 能够根据两个参数的符号自动判断角度所在的象限，从而输出范围在 [-π, π] 之间的完整角度值，避免了因象限判断错误导致的姿态角跳变。俯仰角的定义域为 [-90°, 90°]，而横滚角的定义域为 [-180°, 180°]，这与 atan2 的输出范围完全匹配。
从几何意义上看，俯仰角描述的是载体前向轴（Xb）与水平面之间的夹角。当载体俯仰时，重力在 Xb 轴上的投影为 -g·sin(θ)，而在 Yb-Zb 平面上的投影分量为 g·cos(θ)。因此，使用 atan2(-ax, sqrt(ay²+az²)) 可以同时利用 Yb 和 Zb 轴的信息来估计俯仰角，使得估计结果对横滚角的变化不敏感，提高了俯仰角解算的鲁棒性。
4.2 工程实现：从原始数据到姿态角的完整流程
在实际嵌入式系统中，加速度计姿态解算并非简单的公式套用，而是需要经过一系列预处理步骤才能获得稳定可靠的姿态角。完整的工程实现流程如下：
- 数据采集与预处理：通过 I2C 或 SPI 接口读取加速度计原始寄存器值，将其转换为物理单位（g 或 m/s²）。通常需要减去零偏（Offset）并乘以灵敏度（Sensitivity）。例如，对于 ADXL345 传感器，在 ±2g 量程下，灵敏度为 256 LSB/g，则物理值 = 原始值 / 256。
- 低通滤波：加速度计输出包含高频机械噪声和电气噪声。在静态姿态解算中，通常采用一阶低通滤波器（如 y[n] = α·x[n] + (1-α)·y[n-1]）或滑动平均滤波器来平滑数据。滤波截止频率一般设置在 5-20 Hz 之间，具体取决于应用场景的振动特性。
- 矢量归一化：将滤波后的三轴加速度矢量除以其模长，得到单位矢量。归一化可以消除加速度计灵敏度误差和温度漂移带来的影响，同时使后续的三角函数计算更加稳定。
- 姿态角计算：将归一化后的分量代入上述公式，使用 atan2 函数计算横滚角和俯仰角。在 C 语言中，atan2 函数位于
math.h头文件中，返回值为弧度制，需要乘以 180/π 转换为角度制。 - 角度平滑与限幅：对于静态应用，可以直接输出计算得到的角度；对于动态应用，可以增加一个简单的限幅滤波器，防止因瞬时冲击导致的姿态角跳变。
| 处理步骤 | 输入 | 输出 | 关键参数/公式 | 作用 |
|---|---|---|---|---|
| 原始数据读取 | 寄存器值 (LSB) | 物理量 (g) | 物理值 = (原始值 - 零偏) / 灵敏度 | 统一单位，消除零偏 |
| 低通滤波 | 物理量序列 | 平滑后的物理量 | α = Δt / (RC + Δt) | 抑制高频噪声 |
| 归一化 | 三轴加速度 (g) | 单位矢量 | norm = √(ax²+ay²+az²) | 消除灵敏度误差 |
| 姿态解算 | 单位矢量分量 | 横滚角/俯仰角 | φ = atan2(ay, az); θ = atan2(-ax, √(ay²+az²)) | 提取姿态信息 |
4.3 案例拆解：基于 MPU6050 的静态平台水平校准
下面通过一个实际工程案例来演示加速度计姿态解算的完整应用。假设我们需要为一台两轴云台设计水平校准程序，云台上安装有 MPU6050 六轴传感器，要求在校准模式下输出当前平台相对于水平面的横滚角和俯仰角，精度要求为 ±0.5°。
背景与约束：云台工作环境存在小幅振动（振幅约 0.02g，频率 50 Hz），传感器采样率为 100 Hz。MCU 为 STM32F103，主频 72 MHz，需要在校准模式下实时显示角度。
初始现象：直接使用原始加速度数据计算角度时，角度输出存在明显的抖动，峰峰值达到 ±2.3°，远超精度要求。进一步分析发现，抖动主要来源于两个方面：一是传感器本身的测量噪声（约 0.005g RMS），二是云台电机的微小振动耦合到传感器。
关键输入数据：在静止水平状态下，采集 1000 个样本，统计得到三轴加速度的均值和标准差：ax_mean = 0.012g, ay_mean = -0.008g, az_mean = 0.999g；标准差分别为 0.004g, 0.005g, 0.006g。
分析判断过程：首先，az 的均值接近 1g，说明平台基本水平，但 ax 和 ay 的均值不为零，表明存在约 0.8° 的初始安装倾斜。其次，标准差在 0.004-0.006g 之间，对应的角度噪声约为 atan(0.006/1) ≈ 0.34°，这解释了角度输出的抖动来源。
实际操作步骤：
- 在初始化阶段，采集 200 个样本计算零偏，并存储为
offset_x,offset_y,offset_z。 - 设计二阶 Butterworth 低通滤波器，截止频率设为 10 Hz。滤波器系数通过 MATLAB 计算后以浮点数形式嵌入代码。
- 对滤波后的数据进行归一化处理，并代入姿态解算公式。
- 增加角度平滑算法：采用一阶低通滤波对角度输出进行平滑，系数 α = 0.2。
// 伪代码示例
float ax, ay, az; // 原始物理量
float fx, fy, fz; // 滤波后数据
float norm;
float roll, pitch;
// 1. 读取原始数据并减去零偏
ax = (float)(raw_x - offset_x) / 16384.0f;
ay = (float)(raw_y - offset_y) / 16384.0f;
az = (float)(raw_z - offset_z) / 16384.0f;
// 2. 低通滤波 (一阶)
fx = 0.8f * fx + 0.2f * ax;
fy = 0.8f * fy + 0.2f * ay;
fz = 0.8f * fz + 0.2f * az;
// 3. 归一化
norm = sqrt(fx*fx + fy*fy + fz*fz);
fx /= norm; fy /= norm; fz /= norm;
// 4. 姿态解算
roll = atan2f(fy, fz) * 180.0f / M_PI;
pitch = atan2f(-fx, sqrtf(fy*fy + fz*fz)) * 180.0f / M_PI;结果对比：经过上述处理后，角度输出的峰峰值抖动从 ±2.3° 降低到 ±0.3°，满足 ±0.5° 的精度要求。同时，由于加入了零偏校准，初始安装倾斜也被正确补偿，平台在水平状态下的角度输出接近 0°。
风险与代价：低通滤波器的引入带来了约 20 ms 的相位延迟，这对于静态校准场景可以接受，但如果用于动态姿态跟踪，则需要考虑延迟对实时性的影响。此外，零偏校准需要在平台静止状态下进行，如果校准过程中平台发生移动，会导致零偏估计错误。
复盘经验：加速度计姿态解算的精度瓶颈往往不在算法本身，而在于前端的数据质量。合理的滤波策略和零偏校准是获得高精度姿态角的前提。同时，atan2 函数的使用避免了象限判断的麻烦，但在嵌入式平台上需要注意其计算耗时（约 2-3 μs），对于高速控制循环可能需要优化。
4.4 常见误区与总结复盘
在实际工程应用中，加速度计姿态解算存在几个高频误区，需要特别注意：
加速度计测量的是比力，而非纯粹的重力。当载体存在平移加速度（如车辆加速、机械臂运动）时，加速度计输出会叠加线性加速度分量，导致解算出的姿态角严重失真。例如，一辆汽车以 0.5g 的加速度前进时，俯仰角解算结果会产生约 26.5° 的误差。因此，加速度计姿态解算仅适用于静态或准静态场景，动态场景必须与陀螺仪融合。
不同传感器厂商的坐标系定义可能不同，有的采用 X 右、Y 上、Z 前，有的采用 X 前、Y 左、Z 上。如果直接套用公式而不进行坐标轴映射，会导致横滚角和俯仰角计算错误甚至符号反转。在工程实现中，务必先确认传感器的坐标系定义，并建立明确的坐标变换矩阵。
总结复盘：加速度计姿态解算是惯性导航中最基础也最实用的技术之一。其核心思想是利用重力矢量在载体坐标系中的投影来反推姿态角，数学表达简洁、计算量小，非常适合嵌入式实时系统。然而，它的局限性同样明显——无法感知偏航角（绕重力轴的旋转），且对线性加速度敏感。在实际应用中，加速度计解算的姿态角通常作为低频参考，与陀螺仪的高频积分结果进行互补融合，从而获得全姿态、高带宽的姿态估计。理解加速度计解算的原理和边界条件，是后续学习数据融合算法的重要基础。