无人机嵌入式开发实战-姿态解算与稳定控制

13.2.1 姿态解算的理论基础

姿态解算是无人机飞控系统的核心算法，其理论任务是通过融合多种传感器的测量数据，实时、精确地估计无人机在三维空间中的朝向（姿态）。姿态通常用三个角度表示：横滚角（Roll）、俯仰角（Pitch）和偏航角（Yaw），或者用四元数表示。准确的姿态估计是稳定控制的前提。

姿态解算的核心挑战源于单一传感器的固有局限性：

传感器	测量量	优点	缺点
陀螺仪	角速度	动态响应快，不受外部干扰	存在积分漂移，长期精度差
加速度计	线加速度	长期稳定，可测重力方向	动态响应慢，易受振动干扰
磁力计	磁场强度	提供绝对航向参考	易受电磁干扰，需要校准

姿态解算的数学本质是：利用陀螺仪的短期高精度数据通过积分获得姿态，同时利用加速度计和磁力计的长期稳定数据对积分漂移进行校正，从而获得高精度、低延迟的姿态估计。

13.2.2 姿态的数学表示

欧拉角

欧拉角用三个角度表示刚体在三维空间中的朝向，按照特定顺序旋转得到：

• 横滚角φ：绕X轴旋转的角度

• 俯仰角θ：绕Y轴旋转的角度

• 偏航角ψ：绕Z轴旋转的角度

优点 ：直观易懂，三个参数无冗余。
缺点：存在万向锁问题（当俯仰角接近±90°时，横滚和偏航无法区分）；三角函数计算复杂；不便于插值和滤波。

旋转矩阵

旋转矩阵R是3×3的正交矩阵，表示从机体坐标系到导航坐标系的变换：

c

// 旋转矩阵
float R[3][3] = {
    {cosθ*cosψ, sinφ*sinθ*cosψ - cosφ*sinψ, cosφ*sinθ*cosψ + sinφ*sinψ},
    {cosθ*sinψ, sinφ*sinθ*sinψ + cosφ*cosψ, cosφ*sinθ*sinψ - sinφ*cosψ},
    {-sinθ,     sinφ*cosθ,                  cosφ*cosθ}
};

优点 ：无奇异，可用于向量旋转。
缺点：9个参数存在6个约束（正交性），计算冗余。

四元数

四元数是姿态解算中最常用的表示方法，用四个元素表示旋转：

text

q = [q0, q1, q2, q3]^T，满足 q0² + q1² + q2² + q3² = 1

优点：

• 无奇异，避免万向锁

• 计算效率高（只有乘法和加法）

• 便于插值和滤波

四元数与欧拉角的转换：

c

// 四元数转欧拉角
void quaternion_to_euler(float *q, float *roll, float *pitch, float *yaw) {
    *roll = atan2(2.0f * (q[0]*q[1] + q[2]*q[3]), 
                  1.0f - 2.0f * (q[1]*q[1] + q[2]*q[2]));
    *pitch = asin(2.0f * (q[0]*q[2] - q[3]*q[1]));
    *yaw = atan2(2.0f * (q[0]*q[3] + q[1]*q[2]), 
                 1.0f - 2.0f * (q[2]*q[2] + q[3]*q[3]));
}

// 欧拉角转四元数
void euler_to_quaternion(float roll, float pitch, float yaw, float *q) {
    float cr = cos(roll * 0.5f);
    float sr = sin(roll * 0.5f);
    float cp = cos(pitch * 0.5f);
    float sp = sin(pitch * 0.5f);
    float cy = cos(yaw * 0.5f);
    float sy = sin(yaw * 0.5f);

    q[0] = cr * cp * cy + sr * sp * sy;
    q[1] = sr * cp * cy - cr * sp * sy;
    q[2] = cr * sp * cy + sr * cp * sy;
    q[3] = cr * cp * sy - sr * sp * cy;
}

13.2.3 互补滤波姿态解算

互补滤波是一种简单有效的姿态解算方法，其核心思想是利用陀螺仪的高频特性和加速度计的低频特性进行频域互补。

一阶互补滤波

c

typedef struct {
    float q[4];              // 四元数姿态
    float gyro_bias[3];      // 陀螺仪零偏
    float dt;                 // 采样周期
    float alpha;              // 互补系数 (通常0.98)
} ComplementaryFilter_t;

void complementary_filter_init(ComplementaryFilter_t *cf, float dt, float alpha) {
    cf->dt = dt;
    cf->alpha = alpha;
    cf->q[0] = 1.0f;  // 初始四元数 (无旋转)
    cf->q[1] = 0.0f;
    cf->q[2] = 0.0f;
    cf->q[3] = 0.0f;
    cf->gyro_bias[0] = 0.0f;
    cf->gyro_bias[1] = 0.0f;
    cf->gyro_bias[2] = 0.0f;
}

void complementary_filter_update(ComplementaryFilter_t *cf,
                                  float gx, float gy, float gz,   // 陀螺仪 (rad/s)
                                  float ax, float ay, float az) { // 加速度计
    // 1. 陀螺仪积分（预测）
    float q_dot[4];
    q_dot[0] = 0.5f * (-cf->q[1] * (gx - cf->gyro_bias[0]) - 
                        cf->q[2] * (gy - cf->gyro_bias[1]) - 
                        cf->q[3] * (gz - cf->gyro_bias[2]));
    q_dot[1] = 0.5f * ( cf->q[0] * (gx - cf->gyro_bias[0]) + 
                        cf->q[2] * (gz - cf->gyro_bias[2]) - 
                        cf->q[3] * (gy - cf->gyro_bias[1]));
    q_dot[2] = 0.5f * ( cf->q[0] * (gy - cf->gyro_bias[1]) - 
                        cf->q[1] * (gz - cf->gyro_bias[2]) + 
                        cf->q[3] * (gx - cf->gyro_bias[0]));
    q_dot[3] = 0.5f * ( cf->q[0] * (gz - cf->gyro_bias[2]) + 
                        cf->q[1] * (gy - cf->gyro_bias[1]) - 
                        cf->q[2] * (gx - cf->gyro_bias[0]));

    cf->q[0] += q_dot[0] * cf->dt;
    cf->q[1] += q_dot[1] * cf->dt;
    cf->q[2] += q_dot[2] * cf->dt;
    cf->q[3] += q_dot[3] * cf->dt;

    // 归一化
    float norm = sqrt(cf->q[0]*cf->q[0] + cf->q[1]*cf->q[1] + 
                      cf->q[2]*cf->q[2] + cf->q[3]*cf->q[3]);
    cf->q[0] /= norm;
    cf->q[1] /= norm;
    cf->q[2] /= norm;
    cf->q[3] /= norm;

    // 2. 加速度计观测（计算期望重力方向）
    // 将当前姿态的重力向量转到机体坐标系
    float gravity[3] = {
        2.0f * (cf->q[1]*cf->q[3] - cf->q[0]*cf->q[2]),
        2.0f * (cf->q[0]*cf->q[1] + cf->q[2]*cf->q[3]),
        cf->q[0]*cf->q[0] - cf->q[1]*cf->q[1] - cf->q[2]*cf->q[2] + cf->q[3]*cf->q[3]
    };

    // 归一化加速度计测量
    float acc_norm = sqrt(ax*ax + ay*ay + az*az);
    if (acc_norm < 1e-6f) return;
    float ax_n = ax / acc_norm;
    float ay_n = ay / acc_norm;
    float az_n = az / acc_norm;

    // 3. 计算误差（叉积）
    float error[3];
    error[0] = ay_n * gravity[2] - az_n * gravity[1];
    error[1] = az_n * gravity[0] - ax_n * gravity[2];
    error[2] = ax_n * gravity[1] - ay_n * gravity[0];

    // 4. 修正陀螺仪积分
    cf->gyro_bias[0] += error[0] * 0.001f * cf->dt;
    cf->gyro_bias[1] += error[1] * 0.001f * cf->dt;
    cf->gyro_bias[2] += error[2] * 0.001f * cf->dt;

    // 5. 互补融合（用误差修正姿态）
    float correction[4];
    correction[0] = 0.5f * (error[0] * cf->q[1] + error[1] * cf->q[2] + error[2] * cf->q[3]);
    correction[1] = 0.5f * (error[0] * cf->q[0] + error[2] * cf->q[2] - error[1] * cf->q[3]);
    correction[2] = 0.5f * (error[1] * cf->q[0] - error[2] * cf->q[1] + error[0] * cf->q[3]);
    correction[3] = 0.5f * (error[2] * cf->q[0] + error[1] * cf->q[1] - error[0] * cf->q[2]);

    cf->q[0] += correction[0] * cf->alpha;
    cf->q[1] += correction[1] * cf->alpha;
    cf->q[2] += correction[2] * cf->alpha;
    cf->q[3] += correction[3] * cf->alpha;

    // 再次归一化
    norm = sqrt(cf->q[0]*cf->q[0] + cf->q[1]*cf->q[1] + 
                cf->q[2]*cf->q[2] + cf->q[3]*cf->q[3]);
    cf->q[0] /= norm;
    cf->q[1] /= norm;
    cf->q[2] /= norm;
    cf->q[3] /= norm;
}

互补系数α的选择决定了陀螺仪和加速度计的权重：

• α越接近1，越信任陀螺仪（动态响应快，但漂移大）

• α越小，越信任加速度计（稳定，但响应慢）

• 典型值：0.98 ~ 0.995

13.2.4 卡尔曼滤波姿态解算

卡尔曼滤波是姿态解算的理论最优方法，通过递归地预测和更新，融合多传感器数据。

扩展卡尔曼滤波（EKF）实现

由于姿态系统是非线性的，需使用扩展卡尔曼滤波：

c

typedef struct {
    // 状态向量 [q0, q1, q2, q3, bgx, bgy, bgz]  (四元数 + 陀螺仪零偏)
    float x[7];
    
    // 状态协方差矩阵 (7x7)
    float P[7][7];
    
    // 过程噪声协方差
    float Q[7];
    
    // 测量噪声协方差 (加速度计3维)
    float R_acc[3];
    
    float dt;  // 采样周期
} EKF_Attitude_t;

void ekf_attitude_init(EKF_Attitude_t *ekf, float dt) {
    ekf->dt = dt;
    
    // 初始姿态 (无旋转)
    ekf->x[0] = 1.0f;  // q0
    ekf->x[1] = 0.0f;  // q1
    ekf->x[2] = 0.0f;  // q2
    ekf->x[3] = 0.0f;  // q3
    ekf->x[4] = 0.0f;  // bgx
    ekf->x[5] = 0.0f;  // bgy
    ekf->x[6] = 0.0f;  // bgz
    
    // 初始化协方差矩阵 (对角矩阵)
    for (int i = 0; i < 7; i++) {
        for (int j = 0; j < 7; j++) {
            ekf->P[i][j] = (i == j) ? 0.1f : 0.0f;
        }
    }
    
    // 过程噪声 (经验值)
    ekf->Q[0] = 1e-6f;  // 四元数噪声
    ekf->Q[1] = 1e-6f;
    ekf->Q[2] = 1e-6f;
    ekf->Q[3] = 1e-6f;
    ekf->Q[4] = 1e-8f;  // 零偏噪声
    ekf->Q[5] = 1e-8f;
    ekf->Q[6] = 1e-8f;
    
    // 测量噪声 (加速度计)
    ekf->R_acc[0] = 0.01f;
    ekf->R_acc[1] = 0.01f;
    ekf->R_acc[2] = 0.01f;
}

void ekf_attitude_predict(EKF_Attitude_t *ekf, float gx, float gy, float gz) {
    // 状态预测 (基于陀螺仪)
    float q0 = ekf->x[0];
    float q1 = ekf->x[1];
    float q2 = ekf->x[2];
    float q3 = ekf->x[3];
    float bgx = ekf->x[4];
    float bgy = ekf->x[5];
    float bgz = ekf->x[6];
    
    // 减去零偏
    gx -= bgx;
    gy -= bgy;
    gz -= bgz;
    
    // 四元数微分
    float dq0 = 0.5f * (-q1*gx - q2*gy - q3*gz);
    float dq1 = 0.5f * ( q0*gx + q2*gz - q3*gy);
    float dq2 = 0.5f * ( q0*gy - q1*gz + q3*gx);
    float dq3 = 0.5f * ( q0*gz + q1*gy - q2*gx);
    
    // 预测状态
    ekf->x[0] += dq0 * ekf->dt;
    ekf->x[1] += dq1 * ekf->dt;
    ekf->x[2] += dq2 * ekf->dt;
    ekf->x[3] += dq3 * ekf->dt;
    // 零偏不变 (随机游走)
    
    // 归一化四元数
    float norm = sqrt(ekf->x[0]*ekf->x[0] + ekf->x[1]*ekf->x[1] + 
                      ekf->x[2]*ekf->x[2] + ekf->x[3]*ekf->x[3]);
    ekf->x[0] /= norm;
    ekf->x[1] /= norm;
    ekf->x[2] /= norm;
    ekf->x[3] /= norm;
    
    // 计算状态转移矩阵F (7x7雅可比)
    float F[7][7] = {0};
    // ... (简化，实际需计算完整雅可比)
    
    // 预测协方差 P = F * P * F^T + Q
    float P_new[7][7] = {0};
    for (int i = 0; i < 7; i++) {
        for (int j = 0; j < 7; j++) {
            for (int k = 0; k < 7; k++) {
                P_new[i][j] += F[i][k] * ekf->P[k][j];
            }
        }
    }
    // ... (完整的矩阵乘法)
    
    memcpy(ekf->P, P_new, sizeof(ekf->P));
    
    // 加过程噪声
    for (int i = 0; i < 7; i++) {
        ekf->P[i][i] += ekf->Q[i];
    }
}

void ekf_attitude_update(EKF_Attitude_t *ekf, float ax, float ay, float az) {
    // 归一化加速度计测量
    float norm = sqrt(ax*ax + ay*ay + az*az);
    if (norm < 1e-6f) return;
    float ax_n = ax / norm;
    float ay_n = ay / norm;
    float az_n = az / norm;
    
    // 预测的加速度 (从姿态计算期望重力方向)
    float q0 = ekf->x[0];
    float q1 = ekf->x[1];
    float q2 = ekf->x[2];
    float q3 = ekf->x[3];
    
    float hx = 2.0f * (q1*q3 - q0*q2);
    float hy = 2.0f * (q0*q1 + q2*q3);
    float hz = q0*q0 - q1*q1 - q2*q2 + q3*q3;
    
    // 测量残差
    float y[3];
    y[0] = ax_n - hx;
    y[1] = ay_n - hy;
    y[2] = az_n - hz;
    
    // 计算观测矩阵H (3x7)
    float H[3][7] = {0};
    // ... (计算雅可比)
    
    // 计算卡尔曼增益
    float S[3][3] = {0};
    for (int i = 0; i < 3; i++) {
        for (int j = 0; j < 3; j++) {
            for (int k = 0; k < 7; k++) {
                for (int l = 0; l < 7; l++) {
                    S[i][j] += H[i][k] * ekf->P[k][l] * H[j][l];
                }
            }
            if (i == j) S[i][j] += ekf->R_acc[i];
        }
    }
    
    // 计算卡尔曼增益 K = P * H^T * S^{-1}
    float K[7][3] = {0};
    // ... (矩阵求逆和乘法)
    
    // 状态更新
    for (int i = 0; i < 7; i++) {
        for (int j = 0; j < 3; j++) {
            ekf->x[i] += K[i][j] * y[j];
        }
    }
    
    // 重新归一化四元数
    norm = sqrt(ekf->x[0]*ekf->x[0] + ekf->x[1]*ekf->x[1] + 
                ekf->x[2]*ekf->x[2] + ekf->x[3]*ekf->x[3]);
    ekf->x[0] /= norm;
    ekf->x[1] /= norm;
    ekf->x[2] /= norm;
    ekf->x[3] /= norm;
    
    // 更新协方差 P = (I - K*H) * P
    float I_KH[7][7] = {0};
    for (int i = 0; i < 7; i++) I_KH[i][i] = 1.0f;
    // ... (矩阵计算)
    
    float P_new[7][7] = {0};
    for (int i = 0; i < 7; i++) {
        for (int j = 0; j < 7; j++) {
            for (int k = 0; k < 7; k++) {
                P_new[i][j] += I_KH[i][k] * ekf->P[k][j];
            }
        }
    }
    memcpy(ekf->P, P_new, sizeof(ekf->P));
}

卡尔曼滤波的优势：

• 理论上最优，精度高

• 可估计陀螺仪零偏

• 能处理非线性系统

劣势：

• 计算量大，对MCU要求高

• 需要精确的噪声模型

• 调参复杂

13.2.5 磁力计融合与航向估计

磁力计提供绝对航向参考，但易受电磁干扰，需特殊处理。

磁力计校准

c

typedef struct {
    float hard_iron[3];      // 硬磁干扰 (偏移)
    float soft_iron[3][3];    // 软磁干扰 (缩放和旋转)
    float mag_max[3];         // 各轴最大值 (用于椭球拟合)
    float mag_min[3];         // 各轴最小值
} MagCalib_t;

void mag_calib_init(MagCalib_t *calib) {
    for (int i = 0; i < 3; i++) {
        calib->mag_max[i] = -10000.0f;
        calib->mag_min[i] = 10000.0f;
        calib->hard_iron[i] = 0.0f;
        for (int j = 0; j < 3; j++) {
            calib->soft_iron[i][j] = (i == j) ? 1.0f : 0.0f;
        }
    }
}

void mag_calib_collect(MagCalib_t *calib, float mx, float my, float mz) {
    // 收集数据点 (旋转无人机采集多方向数据)
    calib->mag_max[0] = fmax(calib->mag_max[0], mx);
    calib->mag_min[0] = fmin(calib->mag_min[0], mx);
    calib->mag_max[1] = fmax(calib->mag_max[1], my);
    calib->mag_min[1] = fmin(calib->mag_min[1], my);
    calib->mag_max[2] = fmax(calib->mag_max[2], mz);
    calib->mag_min[2] = fmin(calib->mag_min[2], mz);
}

void mag_calib_compute(MagCalib_t *calib) {
    // 硬磁补偿：椭球中心偏移
    for (int i = 0; i < 3; i++) {
        calib->hard_iron[i] = (calib->mag_max[i] + calib->mag_min[i]) / 2.0f;
    }
    
    // 软磁补偿：椭球缩放 (简化)
    float scale[3];
    for (int i = 0; i < 3; i++) {
        scale[i] = (calib->mag_max[i] - calib->mag_min[i]) / 2.0f;
    }
    
    // 归一化到球体
    float avg_scale = (scale[0] + scale[1] + scale[2]) / 3.0f;
    for (int i = 0; i < 3; i++) {
        calib->soft_iron[i][i] = avg_scale / scale[i];
    }
}

void mag_calib_apply(MagCalib_t *calib, float *mx, float *my, float *mz) {
    // 减去硬磁干扰
    float m[3] = {*mx - calib->hard_iron[0], 
                  *my - calib->hard_iron[1], 
                  *mz - calib->hard_iron[2]};
    
    // 应用软磁补偿
    float mc[3] = {0};
    for (int i = 0; i < 3; i++) {
        for (int j = 0; j < 3; j++) {
            mc[i] += calib->soft_iron[i][j] * m[j];
        }
    }
    
    *mx = mc[0];
    *my = mc[1];
    *mz = mc[2];
}

磁力计融合姿态解算

c

// 在卡尔曼滤波或互补滤波中加入磁力计更新
void attitude_update_with_mag(EKF_Attitude_t *ekf,
                               float ax, float ay, float az,
                               float mx, float my, float mz,
                               float mag_declination) {
    // 先进行加速度计更新 (如13.2.4节)
    ekf_attitude_update_acc(ekf, ax, ay, az);
    
    // 磁力计更新
    // 将磁力计从机体坐标系转到导航坐标系
    float q0 = ekf->x[0];
    float q1 = ekf->x[1];
    float q2 = ekf->x[2];
    float q3 = ekf->x[3];
    
    // 旋转磁场向量到导航系
    float mx_n = (q0*q0 + q1*q1 - q2*q2 - q3*q3) * mx + 
                 2*(q1*q2 - q0*q3) * my + 
                 2*(q1*q3 + q0*q2) * mz;
    float my_n = 2*(q1*q2 + q0*q3) * mx + 
                 (q0*q0 - q1*q1 + q2*q2 - q3*q3) * my + 
                 2*(q2*q3 - q0*q1) * mz;
    
    // 期望的磁场方向 (北向)
    float hx = cos(mag_declination);
    float hy = sin(mag_declination);
    
    // 计算误差
    float error = hx * my_n - hy * mx_n;
    
    // 用误差修正偏航角
    // ... (更新姿态)
}

13.2.6 串级PID稳定控制

串级PID控制是实现无人机稳定飞行的核心算法，通过内环角速度环和外环角度环嵌套，实现高精度姿态控制。

串级PID控制器实现

c

typedef struct {
    // PID参数
    float kp_angle;      // 角度环比例
    float ki_angle;      // 角度环积分
    float kd_angle;      // 角度环微分
    
    float kp_rate;       // 角速度环比例
    float ki_rate;       // 角速度环积分
    float kd_rate;       // 角速度环微分
    
    // 限制
    float angle_limit;   // 角度环输出限制 (期望角速度最大值)
    float rate_limit;    // 角速度环输出限制 (PWM占空比)
    
    // 状态
    float integral_angle[3];
    float integral_rate[3];
    float prev_error_angle[3];
    float prev_error_rate[3];
} CascadePID_t;

void cascade_pid_init(CascadePID_t *pid) {
    pid->kp_angle = 4.0f;
    pid->ki_angle = 0.02f;
    pid->kd_angle = 0.0f;
    
    pid->kp_rate = 0.15f;
    pid->ki_rate = 0.01f;
    pid->kd_rate = 0.0f;
    
    pid->angle_limit = 200.0f;   // 200 deg/s
    pid->rate_limit = 1000.0f;    // 1000 PWM
    
    memset(pid->integral_angle, 0, sizeof(pid->integral_angle));
    memset(pid->integral_rate, 0, sizeof(pid->integral_rate));
    memset(pid->prev_error_angle, 0, sizeof(pid->prev_error_angle));
    memset(pid->prev_error_rate, 0, sizeof(pid->prev_error_rate));
}

void cascade_pid_update(CascadePID_t *pid,
                         float roll_des, float pitch_des, float yaw_des,
                         float roll_meas, float pitch_meas, float yaw_meas,
                         float gyro_x, float gyro_y, float gyro_z,
                         float dt,
                         float *pwm_roll, float *pwm_pitch, float *pwm_yaw) {
    
    // 1. 角度环 (外环) - 计算期望角速度
    float rate_des[3];
    float error_angle[3];
    
    // 横滚通道
    error_angle[0] = roll_des - roll_meas;
    pid->integral_angle[0] += error_angle[0] * dt;
    // 积分限幅
    float integral_max_angle = pid->angle_limit / pid->ki_angle;
    if (pid->integral_angle[0] > integral_max_angle)
        pid->integral_angle[0] = integral_max_angle;
    if (pid->integral_angle[0] < -integral_max_angle)
        pid->integral_angle[0] = -integral_max_angle;
    
    // 微分 (使用角速度反馈替代微分)
    float derivative_angle = -gyro_x;  // 微分先行
    
    rate_des[0] = pid->kp_angle * error_angle[0] + 
                  pid->ki_angle * pid->integral_angle[0] + 
                  pid->kd_angle * derivative_angle;
    
    // 限幅
    if (rate_des[0] > pid->angle_limit) rate_des[0] = pid->angle_limit;
    if (rate_des[0] < -pid->angle_limit) rate_des[0] = -pid->angle_limit;
    
    // 俯仰通道 (类似)
    error_angle[1] = pitch_des - pitch_meas;
    pid->integral_angle[1] += error_angle[1] * dt;
    // ... 积分限幅
    rate_des[1] = pid->kp_angle * error_angle[1] + 
                  pid->ki_angle * pid->integral_angle[1] - 
                  pid->kd_angle * gyro_y;
    // ... 限幅
    
    // 偏航通道 (独立处理，或使用角速度控制)
    error_angle[2] = yaw_des - yaw_meas;
    // 归一化角度差到[-π, π]
    if (error_angle[2] > M_PI) error_angle[2] -= 2*M_PI;
    if (error_angle[2] < -M_PI) error_angle[2] += 2*M_PI;
    
    pid->integral_angle[2] += error_angle[2] * dt;
    // ... 积分限幅
    rate_des[2] = pid->kp_angle * error_angle[2] + 
                  pid->ki_angle * pid->integral_angle[2] - 
                  pid->kd_angle * gyro_z;
    // ... 限幅
    
    // 2. 角速度环 (内环) - 计算PWM输出
    float error_rate[3];
    
    // 横滚通道
    error_rate[0] = rate_des[0] - gyro_x;
    pid->integral_rate[0] += error_rate[0] * dt;
    // 积分限幅
    float integral_max_rate = pid->rate_limit / pid->ki_rate;
    if (pid->integral_rate[0] > integral_max_rate)
        pid->integral_rate[0] = integral_max_rate;
    if (pid->integral_rate[0] < -integral_max_rate)
        pid->integral_rate[0] = -integral_max_rate;
    
    // 微分 (直接计算)
    float derivative_rate = (error_rate[0] - pid->prev_error_rate[0]) / dt;
    
    *pwm_roll = pid->kp_rate * error_rate[0] + 
                pid->ki_rate * pid->integral_rate[0] + 
                pid->kd_rate * derivative_rate;
    
    // 限幅
    if (*pwm_roll > pid->rate_limit) *pwm_roll = pid->rate_limit;
    if (*pwm_roll < -pid->rate_limit) *pwm_roll = -pid->rate_limit;
    
    pid->prev_error_rate[0] = error_rate[0];
    
    // 俯仰、偏航通道类似...
    // ... 计算pwm_pitch, pwm_yaw
}

混控器设计

混控器将姿态控制器的输出转换为各电机的PWM指令，以常见的四旋翼X型布局为例：

c

typedef struct {
    // 电机布局 (前置左、前置右、后置左、后置右)
    float motor_scale[4];  // 电机推力系数
    float frame_arm;        // 机臂长度 (用于力矩计算)
} Mixer_t;

void mixer_quadcopter_x(Mixer_t *mixer,
                         float throttle,     // 总油门
                         float roll_cmd,     // 横滚力矩
                         float pitch_cmd,    // 俯仰力矩
                         float yaw_cmd,      // 偏航力矩
                         float *motor_out) {
    
    // 基础油门
    float base = throttle;
    
    // 四旋翼X型混控
    // 电机1 (前置左): 顺时针旋转
    motor_out[0] = base + roll_cmd + pitch_cmd + yaw_cmd;
    // 电机2 (前置右): 逆时针旋转
    motor_out[1] = base - roll_cmd + pitch_cmd - yaw_cmd;
    // 电机3 (后置左): 逆时针旋转
    motor_out[2] = base + roll_cmd - pitch_cmd - yaw_cmd;
    // 电机4 (后置右): 顺时针旋转
    motor_out[3] = base - roll_cmd - pitch_cmd + yaw_cmd;
    
    // 归一化到0-1范围
    float max_val = 0;
    for (int i = 0; i < 4; i++) {
        if (motor_out[i] > max_val) max_val = motor_out[i];
    }
    
    if (max_val > 1.0f) {
        for (int i = 0; i < 4; i++) {
            motor_out[i] /= max_val;
        }
    }
    
    // 确保非负
    for (int i = 0; i < 4; i++) {
        if (motor_out[i] < 0) motor_out[i] = 0;
    }
}

13.2.7 振动抑制与滤波

无人机飞行中的振动会严重影响姿态估计和控制性能，需采用多种滤波技术抑制。

陷波滤波器

陷波滤波器用于抑制特定频率的振动（如螺旋桨旋转频率）：

c

typedef struct {
    float freq;      // 陷波中心频率 (Hz)
    float bw;        // 陷波带宽 (Hz)
    float b[3];      // 分子系数
    float a[3];      // 分母系数
    
    float x[3];      // 输入历史
    float y[3];      // 输出历史
} NotchFilter_t;

void notch_filter_init(NotchFilter_t *nf, float freq, float bw, float dt) {
    nf->freq = freq;
    nf->bw = bw;
    
    float omega = 2 * M_PI * freq * dt;
    float alpha = sin(omega) * sinh(bw * dt * M_PI);
    
    // 双线性变换系数
    float b0 = 1 - alpha;
    float b1 = 2 * cos(omega) * (1 - alpha);
    float b2 = 1 - alpha;
    float a0 = 1 + alpha;
    float a1 = -2 * cos(omega) * (1 + alpha);
    float a2 = 1 + alpha;
    
    nf->b[0] = b0 / a0;
    nf->b[1] = b1 / a0;
    nf->b[2] = b2 / a0;
    nf->a[1] = a1 / a0;
    nf->a[2] = a2 / a0;
    
    memset(nf->x, 0, sizeof(nf->x));
    memset(nf->y, 0, sizeof(nf->y));
}

float notch_filter_update(NotchFilter_t *nf, float input) {
    // 更新输入历史
    nf->x[2] = nf->x[1];
    nf->x[1] = nf->x[0];
    nf->x[0] = input;
    
    // 计算输出
    float output = nf->b[0] * nf->x[0] + nf->b[1] * nf->x[1] + nf->b[2] * nf->x[2] -
                   nf->a[1] * nf->y[0] - nf->a[2] * nf->y[1];
    
    // 更新输出历史
    nf->y[1] = nf->y[0];
    nf->y[0] = output;
    
    return output;
}

低通滤波器

一阶低通滤波器用于平滑传感器数据，去除高频噪声：

c

typedef struct {
    float cutoff;      // 截止频率 (Hz)
    float alpha;       // 滤波系数
    float prev;        // 前一输出
} LowPassFilter_t;

void lowpass_filter_init(LowPassFilter_t *lpf, float cutoff, float dt) {
    lpf->cutoff = cutoff;
    // 一阶低通系数: alpha = dt / (RC + dt), RC = 1/(2πfc)
    lpf->alpha = (2 * M_PI * cutoff * dt) / (2 * M_PI * cutoff * dt + 1);
    lpf->prev = 0;
}

float lowpass_filter_update(LowPassFilter_t *lpf, float input) {
    float output = lpf->alpha * input + (1 - lpf->alpha) * lpf->prev;
    lpf->prev = output;
    return output;
}

动态陷波

动态陷波滤波器可根据当前电机转速自动调整陷波频率：

c

void dynamic_notch_update(NotchFilter_t *nf, float motor_rpm) {
    // 根据电机转速计算主振动频率 (通常是转速的倍数)
    float freq = motor_rpm / 60.0f * 2.0f;  // 2倍转频
    
    // 重新初始化陷波滤波器
    notch_filter_init(nf, freq, freq * 0.2f, DT);
}

13.2.8 常见问题与最佳实践

常见问题与解决方案

问题1：姿态估计漂移

现象：无人机静止时姿态角缓慢变化。

根因分析：陀螺仪零偏未校准，或互补滤波系数过大。

解决方案：

• 上电时静止采集陀螺仪数据，计算零偏

• 降低互补滤波的alpha值，增加加速度计权重

• 使用卡尔曼滤波估计零偏

问题2：飞行中剧烈振荡

现象：无人机飞行时高频抖动，无法稳定。

根因分析：控制增益过高，或传感器噪声大。

解决方案：

• 降低内环（角速度环）比例增益

• 增加陷波滤波器抑制机械共振

• 检查传感器减震安装

问题3：偏航角漂移

现象：悬停时机头缓慢转动。

根因分析：磁力计未校准，或受电机磁场干扰。

解决方案：

• 执行磁力计椭球校准

• 在飞控安装时远离大电流导线

• 使用GPS航向（有移动时）辅助

问题4：姿态响应滞后

现象：无人机反应迟钝，控制不灵敏。

根因分析：控制周期过长，或滤波过重。

解决方案：

• 缩短控制周期（目标1kHz以上）

• 降低滤波强度，或使用预测滤波

• 优化代码执行效率

问题5：高度控制不稳

现象：无人机上下波动，无法定高。

根因分析：气压计噪声大，或油门补偿不足。

解决方案：

• 使用超声波/激光辅助定高

• 增加气压计滤波

• 加入垂直加速度积分

最佳实践指南

实践1：传感器数据处理流程

text

原始数据 → 硬件滤波(RC) → 软件低通滤波 → 陷波滤波 → 校准 → 融合算法

实践2：控制参数整定顺序

1. 内环（角速度环）：从小到大增加Kp，直至出现轻微振荡，然后减少30%

2. 外环（角度环）：从零开始增加Kp，观察响应速度

3. 积分项：逐渐增加Ki，消除稳态误差

4. 微分项：根据需要增加Kd，抑制超调

实践3：飞行前校准清单

• 加速度计水平校准

• 陀螺仪零偏采集

• 磁力计椭球校准

• 电调行程校准

• 遥控器通道映射

实践4：调试工具

• 地面站软件：Mission Planner、QGroundControl

• 日志分析：查看姿态、控制输出、振动频谱

• 硬件在环仿真：HITL测试控制参数

13.2.9 本章小结

姿态解算与稳定控制是无人机飞控系统的核心，其理论体系涵盖传感器数据融合、姿态表示、滤波算法和控制律设计等多个层面。本章系统阐述了从基础理论到工程实践的技术方法。

姿态解算通过融合陀螺仪、加速度计和磁力计数据，实时估计无人机姿态。互补滤波实现简单、计算量小，适合资源受限平台；卡尔曼滤波精度高、可估计零偏，适合高性能飞控。

姿态表示中，四元数因其无奇异、计算高效的特性，成为飞控中的首选表示方法。

稳定控制采用串级PID架构，内环角速度环快速响应扰动，外环角度环保证姿态跟踪精度。混控器将控制输出映射到各电机，实现期望力矩。

振动抑制通过陷波滤波器和低通滤波器，有效降低机械振动对姿态估计和控制的影响。

从实践角度看，姿态解算与稳定控制的开发遵循明确的技术路径：传感器驱动与校准→数据预处理与滤波→姿态解算算法实现→控制律设计与整定→系统联调与优化。每个环节都需要将理论原理转化为具体的工程决策。

优秀的姿态控制系统应具备高精度（误差<1°）、高带宽（响应<100ms）和强鲁棒性（抗振动、抗干扰），为无人机的稳定飞行和自主任务奠定基础。