一、前置基础说明

1. 核心前提

只有当传感器静止或匀速直线运动时，加速度计测量到的信号才完全等于重力加速度（约 9.8m/s² 或 1g）。此时，重力加速度在三个轴上的分量分布，唯一决定了传感器相对于水平面的倾斜角度。

2. 标准坐标系定义（消费电子 / 无人机通用）

  • X 轴：向前（手表顶部 / 无人机机头方向）
  • Y 轴：向右（手表右侧 / 无人机右翼方向）
  • Z 轴：向下（手表背面 / 无人机底部方向）
  • 俯仰角 (Pitch, θ)：绕 Y 轴旋转的角度，抬头为正，范围 ±90°
  • 横滚角 (Roll, φ)：绕 X 轴旋转的角度，右倾为正，范围 ±180°

3. 最终计算公式（最常用、最准确）

使用atan2函数（四象限反正切），避免了单纯asin/acos只能覆盖 ±90° 的缺陷：

俯仰角 θ = atan2(-Ax, Az) × (180/π)

横滚角 φ = atan2(Ay, Az) × (180/π)

其中：

  • Ax, Ay, Az：分别为 X、Y、Z 轴的加速度值（单位：g）
  • 180/π：弧度转角度的系数（≈57.2958）

二、四个典型场景的数值计算演示

场景 1：完全水平静止（基准状态）

已知条件：传感器水平放置，无任何倾斜

  • Ax = 0.000 g
  • Ay = 0.000 g
  • Az = 1.000 g（重力完全沿 Z 轴向下）

计算过程：

  1. 俯仰角：θ = atan2 (-0.000, 1.000) × 57.2958 = 0.000°
  2. 横滚角：φ = atan2 (0.000, 1.000) × 57.2958 = 0.000°

物理意义：传感器完全水平，既不抬头也不倾斜。这是所有姿态计算的零点基准。

场景 2：纯抬头 30°（只有俯仰角变化）

已知条件：传感器绕 Y 轴向上旋转 30°，无横滚

  • Ax = -sin (30°) = -0.500 g（重力沿 X 轴向后的分量）
  • Ay = 0.000 g
  • Az = cos (30°) ≈ 0.866 g（重力沿 Z 轴向下的分量）

计算过程：

  1. 俯仰角：θ = atan2 (-(-0.500), 0.866) × 57.2958= atan2 (0.500, 0.866) × 57.2958≈ 0.5236 rad × 57.2958 ≈ 30.00°
  2. 横滚角：φ = atan2 (0.000, 0.866) × 57.2958 = 0.00°

物理意义：传感器精确抬头 30°，左右保持水平。这正是手表 "抬腕亮屏" 功能检测的典型姿态。

场景 3：纯右倾 45°（只有横滚角变化）

已知条件：传感器绕 X 轴向右旋转 45°，无俯仰

  • Ax = 0.000 g
  • Ay = sin (45°) ≈ 0.707 g（重力沿 Y 轴向右的分量）
  • Az = cos (45°) ≈ 0.707 g（重力沿 Z 轴向下的分量）

计算过程：

  1. 俯仰角：θ = atan2 (-0.000, 0.707) × 57.2958 = 0.00°
  2. 横滚角：φ = atan2 (0.707, 0.707) × 57.2958≈ 0.7854 rad × 57.2958 ≈ 45.00°

物理意义：传感器精确向右倾斜 45°，前后保持水平。

场景 4：同时抬头 20°+ 右倾 30°（复杂实际场景）

已知条件：传感器先抬头 20°，再右倾 30°（最接近实际使用情况）

  • Ax = -sin(20°) ≈ -0.342 g
  • Ay = sin(30°) × cos(20°) ≈ 0.5 × 0.940 ≈ 0.470 g
  • Az = cos(30°) × cos(20°) ≈ 0.866 × 0.940 ≈ 0.814 g

计算过程：

  1. 俯仰角：θ = atan2 (-(-0.342), 0.814) × 57.2958= atan2 (0.342, 0.814) × 57.2958≈ 0.397 rad × 57.2958 ≈ 22.7°
  2. 横滚角：φ = atan2 (0.470, 0.814) × 57.2958≈ 0.523 rad × 57.2958 ≈ 30.0°

重要说明：当同时存在俯仰和横滚时，计算出的俯仰角会与原始旋转角度略有差异（22.7° vs 20°）。这是因为旋转顺序会影响最终姿态，属于正常的欧拉角特性。

三、关键注意事项

  1. 无法测量偏航角：加速度计只能测量俯仰角和横滚角，完全无法测量偏航角（绕 Z 轴的旋转）。因为重力加速度在水平面上没有分量，无论传感器如何旋转，只要保持水平，Ax 和 Ay 始终为 0。偏航角需要磁力计来测量。

  2. 动态运动误差：上述公式仅适用于静止或匀速直线运动。当传感器加速运动时（如走路、跑步、无人机飞行），加速度计会同时测量到重力加速度和运动加速度，导致计算出的姿态角出现严重误差。这就是为什么实际设备中必须结合陀螺仪进行数据融合（卡尔曼滤波 / 互补滤波）。

  3. 角度范围：

    • 俯仰角：使用atan2(-Ax, Az)计算，范围为 **-180°~+180°**，但实际物理意义上，当俯仰角超过 ±90° 时，横滚角会发生 180° 跳变（万向锁问题）。
    • 横滚角：使用atan2(Ay, Az)计算，范围为 **-180°~+180°**。

  4. 校准的重要性：实际的 MEMS 加速度计存在零点偏移和灵敏度误差。例如，水平放置时 Az 可能不是 1.000g，而是 0.980g 或 1.020g。必须进行校准才能得到准确的角度计算结果。

四、实际应用中的数值阈值示例

  • 手表抬腕亮屏：当检测到俯仰角在 **20°~60°之间，且横滚角在-30°~+30°** 之间时，触发亮屏。
  • 无人机悬停控制：飞行控制系统会努力将俯仰角和横滚角控制在 **±0.5°** 以内，以保持稳定悬停。
  • 跌倒检测：当检测到俯仰角或横滚角突然变化超过60°，且伴随自由落体（Az≈0g）和撞击（加速度 > 5g）时，判断为跌倒。

五、三轴加速度计角度计算与低通滤波 C 语言实现

#include <stdio.h>
#include <math.h>

// 数学常数定义
#define PI 3.14159265358979323846f
#define RAD_TO_DEG (180.0f / PI)
#define DEG_TO_RAD (PI / 180.0f)

// 低通滤波系数(0.0~1.0)
// 数值越小，滤波效果越强，但延迟越大
// 可穿戴设备推荐: 0.1~0.3
// 无人机推荐: 0.3~0.5
#define LPF_ALPHA 0.2f

// 三轴加速度计数据结构体
typedef struct {
    float x;       // X轴原始加速度(g)
    float y;       // Y轴原始加速度(g)
    float z;       // Z轴原始加速度(g)
    float x_filtered; // X轴滤波后加速度(g)
    float y_filtered; // Y轴滤波后加速度(g)
    float z_filtered; // Z轴滤波后加速度(g)
    float pitch;   // 俯仰角(度)
    float roll;    // 横滚角(度)
} AccelData;

/**
 * @brief 一阶低通滤波函数
 * @param raw 原始输入值
 * @param prev_filtered 上一次滤波后的值
 * @param alpha 滤波系数(0.0~1.0)
 * @return 本次滤波后的值
 */
float low_pass_filter(float raw, float prev_filtered, float alpha) {
    return alpha * raw + (1.0f - alpha) * prev_filtered;
}

/**
 * @brief 计算俯仰角和横滚角
 * @param data 加速度计数据结构体指针
 * @note 使用滤波后的加速度数据进行计算
 */
void calculate_angles(AccelData* data) {
    // 防止除以零错误(当Z轴加速度接近0时)
    if (fabsf(data->z_filtered) < 0.001f) {
        data->z_filtered = 0.001f;
    }

    // 计算俯仰角(绕Y轴)
    data->pitch = atan2f(-data->x_filtered, data->z_filtered) * RAD_TO_DEG;

    // 计算横滚角(绕X轴)
    data->roll = atan2f(data->y_filtered, data->z_filtered) * RAD_TO_DEG;
}

/**
 * @brief 初始化加速度计数据结构体
 * @param data 加速度计数据结构体指针
 */
void accel_init(AccelData* data) {
    data->x = 0.0f;
    data->y = 0.0f;
    data->z = 1.0f; // 初始水平状态
    data->x_filtered = 0.0f;
    data->y_filtered = 0.0f;
    data->z_filtered = 1.0f;
    data->pitch = 0.0f;
    data->roll = 0.0f;
}

/**
 * @brief 更新加速度计数据并计算角度
 * @param data 加速度计数据结构体指针
 * @param new_x 新的X轴原始加速度(g)
 * @param new_y 新的Y轴原始加速度(g)
 * @param new_z 新的Z轴原始加速度(g)
 */
void accel_update(AccelData* data, float new_x, float new_y, float new_z) {
    // 更新原始数据
    data->x = new_x;
    data->y = new_y;
    data->z = new_z;

    // 应用低通滤波
    data->x_filtered = low_pass_filter(data->x, data->x_filtered, LPF_ALPHA);
    data->y_filtered = low_pass_filter(data->y, data->y_filtered, LPF_ALPHA);
    data->z_filtered = low_pass_filter(data->z, data->z_filtered, LPF_ALPHA);

    // 计算角度
    calculate_angles(data);
}

/**
 * @brief 打印加速度计数据和角度
 * @param data 加速度计数据结构体指针
 */
void print_accel_data(AccelData* data) {
    printf("原始加速度: X=%.3fg, Y=%.3fg, Z=%.3fg\n", 
           data->x, data->y, data->z);
    printf("滤波后加速度: X=%.3fg, Y=%.3fg, Z=%.3fg\n", 
           data->x_filtered, data->y_filtered, data->z_filtered);
    printf("姿态角: 俯仰角=%.2f°, 横滚角=%.2f°\n\n", 
           data->pitch, data->roll);
}

int main() {
    AccelData accel;
    accel_init(&accel);

    printf("=== 三轴加速度计角度计算与低通滤波演示 ===\n\n");

    // 场景1: 完全水平静止
    printf("场景1: 完全水平静止\n");
    accel_update(&accel, 0.000f, 0.000f, 1.000f);
    print_accel_data(&accel);

    // 场景2: 纯抬头30°
    printf("场景2: 纯抬头30°\n");
    accel_update(&accel, -0.500f, 0.000f, 0.866f);
    print_accel_data(&accel);

    // 场景3: 纯右倾45°
    printf("场景3: 纯右倾45°\n");
    accel_update(&accel, 0.000f, 0.707f, 0.707f);
    print_accel_data(&accel);

    // 场景4: 同时抬头20°+右倾30°
    printf("场景4: 同时抬头20°+右倾30°\n");
    accel_update(&accel, -0.342f, 0.470f, 0.814f);
    print_accel_data(&accel);

    // 模拟带噪声的实际传感器数据
    printf("场景5: 带噪声的水平数据(模拟实际传感器)\n");
    // 模拟5次采样，每次加入±0.02g的随机噪声
    for (int i = 0; i < 5; i++) {
        float noise_x = ((float)rand() / RAND_MAX - 0.5f) * 0.04f;
        float noise_y = ((float)rand() / RAND_MAX - 0.5f) * 0.04f;
        float noise_z = ((float)rand() / RAND_MAX - 0.5f) * 0.04f;
        accel_update(&accel, 0.0f + noise_x, 0.0f + noise_y, 1.0f + noise_z);
        printf("第%d次采样:\n", i+1);
        print_accel_data(&accel);
    }

    return 0;
}

# 算法
Logo
AI硬件创业社区

智能硬件社区聚焦AI智能硬件技术生态，汇聚嵌入式AI、物联网硬件开发者，打造交流分享平台，同步全国赛事资讯、开展 OPC 核心人才招募，助力技术落地与开发者成长。

更多推荐

cover

【STM32F4】基于RTOS的智能门禁系统实战：人脸识别+RFID+OTA 升级

avatar AI硬件创业社区
cover

基于多模态生物识别的智能门锁系统设计|毕设答辩|单片机项目|毕业设计

avatar AI硬件创业社区
cover

基于多态传感器数据融合的智慧仓储火灾预警系统 |毕设答辩|单片机项目|毕业设计

avatar AI硬件创业社区