12、姿态角计算：从陀螺仪数据计算俯仰角、横滚角、偏航角

好，咱们今天聊点硬核的。陀螺仪数据拿到手了，原始的三轴角速度摆在那，然后呢？

说白了，我们真正想要的是——手机到底转了多少度？是抬头了（俯仰），还是侧翻了（横滚），或者是原地转圈（偏航）？

嗯，这就是姿态角计算要干的事。我个人习惯把这一步叫做「从物理量到语义量」的转换。你想想看，角速度是瞬时的，而角度是累积的。怎么累积？积分。

12.1 姿态角的基本概念

先统一一下口径。在手机传感器领域，我们通常用三个角度来描述手机的朝向：

• 俯仰角（Pitch，θ）：绕 X 轴旋转。说白了就是手机抬头或低头。正值为仰起，负值为俯下。
• 横滚角（Roll，φ）：绕 Y 轴旋转。就是手机左右侧翻。正值为右侧抬起，负值为左侧抬起。
• 偏航角（Yaw，ψ）：绕 Z 轴旋转。手机在水平面内转圈。正值为顺时针旋转。

我在项目中遇到过不少新手，上来就问「为什么我的偏航角一直在飘？」——别急，后面我会专门讲这个问题。

这里 ω[n] 是当前时刻的角速度（单位：rad/s），Δt 是采样间隔（单位：s）。

12.2 从角速度到姿态角：积分计算

好，咱们直接上代码。假设你已经从 SensorManager 拿到了三个轴的角速度（单位是 rad/s）：

// 假设采样间隔 dt = 0.01s（100Hz）
float dt = 0.01f;

// 累积角度（初始为0）
float pitch = 0.0f;
float roll  = 0.0f;
float yaw   = 0.0f;

// 每次传感器数据回调时执行
void updateAttitude(float gyroX, float gyroY, float gyroZ) {
    pitch += gyroX * dt;  // 绕X轴
    roll  += gyroY * dt;  // 绕Y轴
    yaw   += gyroZ * dt;  // 绕Z轴
}

嗯，就这么简单？对，原理上就是这么简单。但实际用起来，你会发现一个问题——

误差会累积。

为什么会这样？因为陀螺仪有零偏（bias）。哪怕手机静止不动，输出的角速度也不是完美的 0，而是一个很小的非零值。这个值积分久了，角度就会一直漂移。

12.3 零偏校准：让积分更靠谱

怎么校准？说白了就是让手机静止，采集一段时间的数据，取平均值作为零偏值。然后每次读数都减去这个值。

// 校准阶段：采集N个样本取平均
float biasX = 0, biasY = 0, biasZ = 0;
int sampleCount = 100;

for (int i = 0; i < sampleCount; i++) {
    biasX += gyroX_samples[i];
    biasY += gyroY_samples[i];
    biasZ += gyroZ_samples[i];
}
biasX /= sampleCount;
biasY /= sampleCount;
biasZ /= sampleCount;

// 使用校准后的值
float calibratedX = gyroX - biasX;
float calibratedY = gyroY - biasY;
float calibratedZ = gyroZ - biasZ;

pitch += calibratedX * dt;
roll  += calibratedY * dt;
yaw   += calibratedZ * dt;

我个人习惯在校准阶段至少采集 200 个样本，时间大概 2 秒左右。太短了噪声大，太长了用户等得不耐烦。

12.4 姿态角计算的完整流程

来，我画了一张图，把整个流程串起来：

这张图把整个流程拆成了四个阶段：原始数据 → 零偏校准 → 离散积分 → 姿态角输出。右侧还标注了误差来源和解决方案。

12.5 实际项目中的注意事项

好，理论说完了，咱们聊聊实战中会遇到哪些坑。

问题	表现	我的建议
零偏漂移	静止时角度缓慢变化	每次启动时做零偏校准，必要时做温度补偿
积分累积误差	长时间运行后角度偏差大	配合加速度计做互补滤波，或者用卡尔曼滤波
采样率不稳定	Δt 忽大忽小，积分不准	用系统时间戳计算实际 Δt，不要用固定值
万向锁	俯仰角接近 ±90° 时横滚/偏航异常	考虑用四元数代替欧拉角

12.6 代码示例：完整的姿态角计算类

最后，我贴一个我常用的工具类。它包含了零偏校准和积分逻辑，你直接拿去用就行：

public class AttitudeCalculator {
    private float biasX, biasY, biasZ;
    private boolean isCalibrated = false;
    private int calibCount = 0;
    private static final int CALIB_SAMPLES = 200;

    private float pitch = 0, roll = 0, yaw = 0;
    private long lastTimestamp = 0;

    public void calibrate(float gx, float gy, float gz) {
        if (calibCount < CALIB_SAMPLES) {
            biasX += gx;
            biasY += gy;
            biasZ += gz;
            calibCount++;
        } else {
            biasX /= CALIB_SAMPLES;
            biasY /= CALIB_SAMPLES;
            biasZ /= CALIB_SAMPLES;
            isCalibrated = true;
        }
    }

    public void update(float gx, float gy, float gz, long timestamp) {
        if (!isCalibrated) return;

        if (lastTimestamp == 0) {
            lastTimestamp = timestamp;
            return;
        }

        float dt = (timestamp - lastTimestamp) / 1000.0f; // ms → s
        lastTimestamp = timestamp;

        float cx = gx - biasX;
        float cy = gy - biasY;
        float cz = gz - biasZ;

        pitch += cx * dt;
        roll  += cy * dt;
        yaw   += cz * dt;
    }

    public float getPitch() { return pitch; }
    public float getRoll()  { return roll; }
    public float getYaw()   { return yaw; }
}

嗯，代码不长，但该有的都有了。你把它集成到你的 SensorEventListener 里，在 onSensorChanged 里调用 calibrate 和 update 就行。

最后提醒一句：纯陀螺仪积分算出来的姿态角，只适合短时间使用。如果你需要长时间稳定跟踪，一定要融合加速度计和磁力计的数据。那个话题，咱们后面再聊。

---

公众号:蓝海资料掘金营，微信deep3321