目录

概述

1 欧拉角基本概念

2 从四元数计算欧拉角

3 欧拉角的实际应用

3.1 姿态显示和监控

3.2 姿态控制应用

3.3 姿态限制和安全检查

3.4 数据滤波和平滑处理

3.5  姿态历史记录和趋势分析

4 注意事项和最佳实践

---

概述

欧拉角是描述物体在三维空间中姿态的一种常用方法，通过三个角度值来表示物体相对于参考坐标系的旋转。在MPU6050 DMP应用中，欧拉角提供了直观的姿态信息。

1 欧拉角基本概念

三个旋转角度：

1. 俯仰角(Pitch) - 绕y轴旋转的角度

   • 描述"点头"动作

   • 正值表示向前倾斜，负值表示向后倾斜

2. 横滚角(Roll) - 绕x轴旋转的角度

   • 描述"侧倾"动作

   • 正值表示向右倾斜，负值表示向左倾斜

3. 偏航角(Yaw) - 绕Z轴旋转的角度

   • 描述"摇头"动作

   • 正值表示向右转，负值表示向左转

2 从四元数计算欧拉角

MPU6050的DMP输出四元数数据，需要转换为更直观的欧拉角：

// 将四元数转换为欧拉角（弧度）
void QuaternionToEuler(float q0, float q1, float q2, float q3, 
                      float* roll, float* pitch, float* yaw)
{
    // 计算俯仰角 (pitch)
    *pitch = asin(2.0f * (q0 * q2 - q3 * q1));
    
    // 计算横滚角 (roll)
    *roll = atan2(2.0f * (q0 * q1 + q2 * q3), 
                  1.0f - 2.0f * (q1 * q1 + q2 * q2));
    
    // 计算偏航角 (yaw)
    *yaw = atan2(2.0f * (q0 * q3 + q1 * q2), 
                 1.0f - 2.0f * (q2 * q2 + q3 * q3));
}

// 将弧度转换为角度
void RadiansToDegrees(float* roll, float* pitch, float* yaw)
{
    *roll *= 57.295779513f;  // 180/π
    *pitch *= 57.295779513f;
    *yaw *= 57.295779513f;
}

3 欧拉角的实际应用

3.1 姿态显示和监控

// 显示欧拉角姿态
void DisplayEulerAngles(float roll, float pitch, float yaw)
{
    printf("Roll: %.2f°\n", roll);
    printf("Pitch: %.2f°\n", pitch);
    printf("Yaw: %.2f°\n\n", yaw);
    
    // 简单的文本姿态指示器
    printf("姿态: ");
    if (fabs(roll) < 5.0f && fabs(pitch) < 5.0f) {
        printf("水平");
    } else if (pitch > 30.0f) {
        printf("前倾");
    } else if (pitch < -30.0f) {
        printf("后仰");
    } else if (roll > 30.0f) {
        printf("右倾");
    } else if (roll < -30.0f) {
        printf("左倾");
    }
    printf("\n\n");
}

3.2 姿态控制应用

// 基于欧拉角的简单平衡控制
typedef struct {
    float kp;       // 比例系数
    float ki;       // 积分系数
    float kd;       // 微分系数
    float error_sum; // 误差积分
    float last_error;// 上次误差
} PIDController;

void Init_PID(PIDController* pid, float kp, float ki, float kd)
{
    pid->kp = kp;
    pid->ki = ki;
    pid->kd = kd;
    pid->error_sum = 0;
    pid->last_error = 0;
}

float Update_PID(PIDController* pid, float setpoint, float current_value, float dt)
{
    float error = setpoint - current_value;
    pid->error_sum += error * dt;
    float derivative = (error - pid->last_error) / dt;
    pid->last_error = error;
    
    return pid->kp * error + pid->ki * pid->error_sum + pid->kd * derivative;
}

// 自平衡机器人控制示例
void BalanceRobotControl(float pitch, float roll)
{
    static PIDController pitch_pid, roll_pid;
    static bool initialized = false;
    
    if (!initialized) {
        Init_PID(&pitch_pid, 1.0f, 0.1f, 0.2f);  // 俯仰PID参数
        Init_PID(&roll_pid, 1.0f, 0.1f, 0.2f);   // 横滚PID参数
        initialized = true;
    }
    
    float dt = 0.01f;  // 假设10ms控制周期
    
    // 计算控制输出 (目标是保持0度，即水平)
    float pitch_output = Update_PID(&pitch_pid, 0.0f, pitch, dt);
    float roll_output = Update_PID(&roll_pid, 0.0f, roll, dt);
    
    // 应用控制输出到电机
    ApplyMotorControl(pitch_output, roll_output);
}

3.3 姿态限制和安全检查

// 检查姿态是否在安全范围内
typedef struct {
    float max_roll;
    float max_pitch;
    float max_yaw_rate;
} AttitudeLimits;

bool CheckAttitudeSafety(float roll, float pitch, float yaw_rate, AttitudeLimits limits)
{
    // 检查横滚角是否超出限制
    if (fabs(roll) > limits.max_roll) {
        printf("警告: 横滚角超出限制! %.2f° > %.2f°\n", fabs(roll), limits.max_roll);
        return false;
    }
    
    // 检查俯仰角是否超出限制
    if (fabs(pitch) > limits.max_pitch) {
        printf("警告: 俯仰角超出限制! %.2f° > %.2f°\n", fabs(pitch), limits.max_pitch);
        return false;
    }
    
    // 检查偏航角变化率是否过大
    static float last_yaw = 0;
    static uint32_t last_time = 0;
    uint32_t current_time = HAL_GetTick();
    float dt = (current_time - last_time) / 1000.0f; // 转换为秒
    
    if (dt > 0) {
        float yaw_rate_calculated = fabs(yaw_rate - last_yaw) / dt;
        if (yaw_rate_calculated > limits.max_yaw_rate) {
            printf("警告: 偏航角变化过快! %.2f°/s > %.2f°/s\n", 
                   yaw_rate_calculated, limits.max_yaw_rate);
            return false;
        }
    }
    
    last_yaw = yaw_rate;
    last_time = current_time;
    
    return true;
}

3.4 数据滤波和平滑处理

// 欧拉角的滤波处理
typedef struct {
    float roll;
    float pitch;
    float yaw;
    float alpha; // 滤波系数
} FilteredEulerAngles;

void InitEulerFilter(FilteredEulerAngles* filter, float alpha)
{
    filter->roll = 0;
    filter->pitch = 0;
    filter->yaw = 0;
    filter->alpha = alpha;
}

void UpdateEulerFilter(FilteredEulerAngles* filter, float new_roll, float new_pitch, float new_yaw)
{
    // 应用一阶低通滤波
    filter->roll = filter->alpha * new_roll + (1 - filter->alpha) * filter->roll;
    filter->pitch = filter->alpha * new_pitch + (1 - filter->alpha) * filter->pitch;
    filter->yaw = filter->alpha * new_yaw + (1 - filter->alpha) * filter->yaw;
}

// 处理角度跳变（超过180度的情况）
float NormalizeAngle(float angle)
{
    while (angle > 180.0f) angle -= 360.0f;
    while (angle < -180.0f) angle += 360.0f;
    return angle;
}

3.5  姿态历史记录和趋势分析

// 记录姿态历史用于分析
#define HISTORY_SIZE 100

typedef struct {
    float roll[HISTORY_SIZE];
    float pitch[HISTORY_SIZE];
    float yaw[HISTORY_SIZE];
    int index;
} AttitudeHistory;

void InitAttitudeHistory(AttitudeHistory* history)
{
    for (int i = 0; i < HISTORY_SIZE; i++) {
        history->roll[i] = 0;
        history->pitch[i] = 0;
        history->yaw[i] = 0;
    }
    history->index = 0;
}

void UpdateAttitudeHistory(AttitudeHistory* history, float roll, float pitch, float yaw)
{
    history->roll[history->index] = roll;
    history->pitch[history->index] = pitch;
    history->yaw[history->index] = yaw;
    
    history->index = (history->index + 1) % HISTORY_SIZE;
}

// 计算姿态变化趋势
void CalculateAttitudeTrend(AttitudeHistory* history, float* roll_trend, float* pitch_trend, float* yaw_trend)
{
    float roll_sum = 0, pitch_sum = 0, yaw_sum = 0;
    int count = HISTORY_SIZE;
    
    for (int i = 0; i < count; i++) {
        roll_sum += history->roll[i];
        pitch_sum += history->pitch[i];
        yaw_sum += history->yaw[i];
    }
    
    // 简单趋势计算：当前值与平均值的差异
    *roll_trend = history->roll[(history->index - 1 + HISTORY_SIZE) % HISTORY_SIZE] - (roll_sum / count);
    *pitch_trend = history->pitch[(history->index - 1 + HISTORY_SIZE) % HISTORY_SIZE] - (pitch_sum / count);
    *yaw_trend = history->yaw[(history->index - 1 + HISTORY_SIZE) % HISTORY_SIZE] - (yaw_sum / count);
}

4 注意事项和最佳实践

1. 万向节锁问题：当俯仰角接近±90°时，横滚和偏航会失去一个自由度，导致万向节锁。在这种情况下，考虑使用四元数代替欧拉角。

2. 角度归一化：始终将角度归一化到-180°到+180°范围，避免角度跳变。

3. 坐标系定义：明确MPU6050的安装方向和坐标系定义，确保欧拉角的解释正确。

4. 滤波选择：根据应用需求选择合适的滤波算法和参数，平衡响应速度和稳定性。

5. 单位一致性：在整个系统中保持角度单位的一致性（度或弧度）。

通过合理使用欧拉角，可以开发出直观且响应迅速的姿态感知应用，如无人机飞行控制、机器人平衡、虚拟现实交互等。