02-03-02 传感器融合与姿态解算

1. 核心概念

1.1 多传感器融合概述

传感器融合（Sensor Fusion）是将多个传感器的数据进行融合处理，得到比单一传感器更准确、更可靠的状态估计。

核心传感器：

• IMU：姿态、加速度、角速度
• GPS：位置、速度
• 气压计：高度
• 磁力计：航向角
• 光流：相对位移
• 超声波：相对高度

融合目标：

• 提高精度
• 增强鲁棒性
• 实现互补特性

1.2 传感器特性对比

传感器	优势	劣势	更新频率	典型精度
陀螺仪	高频、无漂移（短期）	积分漂移	1kHz	0.01°/s
加速度计	长期稳定	噪声大、受加速度影响	1kHz	±0.5°
磁力计	提供绝对航向	易受干扰	100Hz	±2°
GPS	绝对位置	低频、室内不可用	10Hz	±2m
气压计	相对高度稳定	受天气影响	50Hz	±0.5m

2. 姿态表示方法

2.1 欧拉角（Euler Angles）

定义：

Roll（横滚）：绕X轴旋转，-180° ~ +180°
Pitch（俯仰）：绕Y轴旋转，-90° ~ +90°
Yaw（偏航）：绕Z轴旋转，-180° ~ +180°

旋转顺序：Z-Y-X（Yaw-Pitch-Roll）

优势：

• 直观易懂
• 便于显示

劣势：

• 万向锁（Gimbal Lock）：Pitch=±90°时失效
• 插值困难
• 计算效率低

2.2 四元数（Quaternion）

定义：

q = w + xi + yj + zk

其中：
- w：实部（标量）
- x, y, z：虚部（矢量）

单位四元数：w² + x² + y² + z² = 1

优势：

• 无万向锁
• 插值平滑（SLERP）
• 计算效率高
• 存储空间小

劣势：

• 不直观
• 需要归一化

欧拉角 ↔ 四元数转换：

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

    q[0] = cr * cp * cy + sr * sp * sy;  // w
    q[1] = sr * cp * cy - cr * sp * sy;  // x
    q[2] = cr * sp * cy + sr * cp * sy;  // y
    q[3] = cr * cp * sy - sr * sp * cy;  // z
}

// 四元数 → 欧拉角
void quaternion_to_euler(float* q, float* roll, float* pitch, float* yaw) {
    // Roll (x-axis rotation)
    float sinr_cosp = 2.0f * (q[0] * q[1] + q[2] * q[3]);
    float cosr_cosp = 1.0f - 2.0f * (q[1] * q[1] + q[2] * q[2]);
    *roll = atan2f(sinr_cosp, cosr_cosp);

    // Pitch (y-axis rotation)
    float sinp = 2.0f * (q[0] * q[2] - q[3] * q[1]);
    if (fabsf(sinp) >= 1.0f)
        *pitch = copysignf(M_PI / 2, sinp);  // 万向锁处理
    else
        *pitch = asinf(sinp);

    // Yaw (z-axis rotation)
    float siny_cosp = 2.0f * (q[0] * q[3] + q[1] * q[2]);
    float cosy_cosp = 1.0f - 2.0f * (q[2] * q[2] + q[3] * q[3]);
    *yaw = atan2f(siny_cosp, cosy_cosp);
}

2.3 旋转矩阵（Rotation Matrix）

定义：

R = [r11  r12  r13]
    [r21  r22  r23]
    [r31  r32  r33]

特性：
- 正交矩阵：R^T = R^(-1)
- 行列式：det(R) = 1

应用：

• 坐标系转换
• 向量旋转

3. 融合算法

3.1 互补滤波（Complementary Filter）

原理：

加速度计：低频准确，高频噪声大
陀螺仪：高频准确，低频漂移

互补滤波 = 高通滤波（陀螺仪） + 低通滤波（加速度计）

一阶互补滤波：

// 简单版本
float alpha = 0.98f;  // 高通滤波系数

angle = alpha * (angle + gyro * dt) + (1 - alpha) * acc_angle;

改进版互补滤波：

typedef struct {
    float alpha;      // 滤波系数
    float angle;      // 当前角度
} complementary_filter_t;

void complementary_filter_update(complementary_filter_t* cf,
                                 float gyro_rate,
                                 float acc_angle,
                                 float dt) {
    // 自适应滤波系数
    float acc_magnitude = fabsf(acc_angle - cf->angle);

    // 加速度变化大时，降低加速度计权重
    float adaptive_alpha = cf->alpha;
    if (acc_magnitude > 10.0f) {
        adaptive_alpha = 0.99f;
    }

    // 互补滤波
    cf->angle = adaptive_alpha * (cf->angle + gyro_rate * dt) +
               (1 - adaptive_alpha) * acc_angle;
}

3.2 卡尔曼滤波（Kalman Filter）

算法框架：

1. 预测（Prediction）：
   x̂(k|k-1) = Ax̂(k-1|k-1) + Bu(k)
   P(k|k-1) = AP(k-1|k-1)A^T + Q

2. 更新（Update）：
   K(k) = P(k|k-1)H^T(HP(k|k-1)H^T + R)^(-1)
   x̂(k|k) = x̂(k|k-1) + K(k)(z(k) - Hx̂(k|k-1))
   P(k|k) = (I - K(k)H)P(k|k-1)

其中：
- x̂：状态估计
- P：误差协方差
- K：卡尔曼增益
- Q：过程噪声协方差
- R：测量噪声协方差

一维卡尔曼滤波实现：

typedef struct {
    float x;      // 状态估计
    float P;      // 误差协方差
    float Q;      // 过程噪声
    float R;      // 测量噪声
} kalman_filter_t;

void kalman_filter_init(kalman_filter_t* kf, float Q, float R) {
    kf->x = 0;
    kf->P = 1;
    kf->Q = Q;
    kf->R = R;
}

float kalman_filter_update(kalman_filter_t* kf, float measurement) {
    // 预测
    // x̂(k|k-1) = x̂(k-1|k-1)
    // P(k|k-1) = P(k-1|k-1) + Q
    kf->P += kf->Q;

    // 更新
    // K = P(k|k-1) / (P(k|k-1) + R)
    float K = kf->P / (kf->P + kf->R);

    // x̂(k|k) = x̂(k|k-1) + K * (z - x̂(k|k-1))
    kf->x += K * (measurement - kf->x);

    // P(k|k) = (1 - K) * P(k|k-1)
    kf->P *= (1 - K);

    return kf->x;
}

姿态卡尔曼滤波：

typedef struct {
    float angle;      // 角度估计
    float bias;       // 陀螺仪偏差估计
    float P[2][2];    // 误差协方差矩阵
    float Q_angle;    // 角度过程噪声
    float Q_bias;     // 偏差过程噪声
    float R_measure;  // 测量噪声
} attitude_kalman_t;

void attitude_kalman_init(attitude_kalman_t* ak) {
    ak->angle = 0;
    ak->bias = 0;
    ak->P[0][0] = 1;
    ak->P[0][1] = 0;
    ak->P[1][0] = 0;
    ak->P[1][1] = 1;
    ak->Q_angle = 0.001f;
    ak->Q_bias = 0.003f;
    ak->R_measure = 0.03f;
}

float attitude_kalman_update(attitude_kalman_t* ak,
                             float gyro_rate,
                             float acc_angle,
                             float dt) {
    // 预测
    ak->angle += (gyro_rate - ak->bias) * dt;

    ak->P[0][0] += dt * (dt * ak->P[1][1] - ak->P[0][1] - ak->P[1][0] + ak->Q_angle);
    ak->P[0][1] -= dt * ak->P[1][1];
    ak->P[1][0] -= dt * ak->P[1][1];
    ak->P[1][1] += ak->Q_bias * dt;

    // 更新
    float S = ak->P[0][0] + ak->R_measure;
    float K[2];
    K[0] = ak->P[0][0] / S;
    K[1] = ak->P[1][0] / S;

    float y = acc_angle - ak->angle;
    ak->angle += K[0] * y;
    ak->bias += K[1] * y;

    float P00_temp = ak->P[0][0];
    float P01_temp = ak->P[0][1];

    ak->P[0][0] -= K[0] * P00_temp;
    ak->P[0][1] -= K[0] * P01_temp;
    ak->P[1][0] -= K[1] * P00_temp;
    ak->P[1][1] -= K[1] * P01_temp;

    return ak->angle;
}

3.3 扩展卡尔曼滤波（EKF）

适用场景：

• 非线性系统
• 多传感器融合（IMU + GPS + 气压计）

算法步骤：

class ExtendedKalmanFilter:
    """扩展卡尔曼滤波器"""

    def __init__(self):
        # 状态向量：[roll, pitch, yaw, gyro_bias_x, gyro_bias_y, gyro_bias_z]
        self.x = np.zeros(6)

        # 误差协方差矩阵
        self.P = np.eye(6)

        # 过程噪声协方差
        self.Q = np.diag([0.001, 0.001, 0.001, 0.0001, 0.0001, 0.0001])

        # 测量噪声协方差
        self.R = np.diag([0.1, 0.1, 0.5])  # [roll, pitch, yaw]

    def predict(self, gyro, dt):
        """预测步骤"""
        # 状态转移函数
        roll, pitch, yaw = self.x[0], self.x[1], self.x[2]
        bias = self.x[3:6]

        # 去除偏差的陀螺仪数据
        gyro_corrected = gyro - bias

        # 状态预测（欧拉角积分）
        self.x[0] += gyro_corrected[0] * dt
        self.x[1] += gyro_corrected[1] * dt
        self.x[2] += gyro_corrected[2] * dt

        # 雅可比矩阵F
        F = np.eye(6)
        F[0:3, 3:6] = -np.eye(3) * dt

        # 协方差预测
        self.P = F @ self.P @ F.T + self.Q

    def update(self, acc, mag):
        """更新步骤"""
        # 从加速度计计算roll和pitch
        acc_roll = np.arctan2(acc[1], acc[2])
        acc_pitch = np.arctan2(-acc[0], np.sqrt(acc[1]**2 + acc[2]**2))

        # 从磁力计计算yaw
        mag_yaw = np.arctan2(-mag[1], mag[0])

        # 测量向量
        z = np.array([acc_roll, acc_pitch, mag_yaw])

        # 预测测量
        h = self.x[0:3]

        # 测量雅可比矩阵H
        H = np.zeros((3, 6))
        H[0:3, 0:3] = np.eye(3)

        # 卡尔曼增益
        S = H @ self.P @ H.T + self.R
        K = self.P @ H.T @ np.linalg.inv(S)

        # 状态更新
        y = z - h
        self.x += K @ y

        # 协方差更新
        self.P = (np.eye(6) - K @ H) @ self.P

    def get_attitude(self):
        """获取姿态角"""
        return self.x[0:3] * 180 / np.pi  # 转换为度

# 使用示例
ekf = ExtendedKalmanFilter()

for _ in range(1000):
    # 读取IMU数据
    gyro = np.array([0.01, -0.02, 0.005])  # rad/s
    acc = np.array([0, 0, 9.8])  # m/s²
    mag = np.array([25, -5, 40])  # μT

    # EKF更新
    ekf.predict(gyro, dt=0.01)
    ekf.update(acc, mag)

    # 获取姿态
    attitude = ekf.get_attitude()
    print(f"Roll: {attitude[0]:.2f}°, Pitch: {attitude[1]:.2f}°, Yaw: {attitude[2]:.2f}°")

3.4 Mahony滤波器

特点：

• 基于四元数
• 计算效率高
• 适合嵌入式系统

算法实现：

typedef struct {
    float q[4];      // 四元数 [w, x, y, z]
    float Kp;        // 比例增益
    float Ki;        // 积分增益
    float integral[3];  // 积分误差
} mahony_filter_t;

void mahony_filter_init(mahony_filter_t* mf) {
    mf->q[0] = 1.0f;
    mf->q[1] = 0.0f;
    mf->q[2] = 0.0f;
    mf->q[3] = 0.0f;
    mf->Kp = 2.0f;
    mf->Ki = 0.01f;
    mf->integral[0] = 0;
    mf->integral[1] = 0;
    mf->integral[2] = 0;
}

void mahony_filter_update(mahony_filter_t* mf,
                         float gyro[3], float acc[3], float mag[3],
                         float dt) {
    float q0 = mf->q[0], q1 = mf->q[1], q2 = mf->q[2], q3 = mf->q[3];

    // 归一化加速度计
    float norm = sqrtf(acc[0]*acc[0] + acc[1]*acc[1] + acc[2]*acc[2]);
    acc[0] /= norm;
    acc[1] /= norm;
    acc[2] /= norm;

    // 重力参考向量（机体坐标系）
    float v[3];
    v[0] = 2.0f * (q1*q3 - q0*q2);
    v[1] = 2.0f * (q0*q1 + q2*q3);
    v[2] = q0*q0 - q1*q1 - q2*q2 + q3*q3;

    // 误差计算（叉乘）
    float e[3];
    e[0] = acc[1] * v[2] - acc[2] * v[1];
    e[1] = acc[2] * v[0] - acc[0] * v[2];
    e[2] = acc[0] * v[1] - acc[1] * v[0];

    // 积分误差
    if (mf->Ki > 0) {
        mf->integral[0] += e[0] * dt;
        mf->integral[1] += e[1] * dt;
        mf->integral[2] += e[2] * dt;
    }

    // 陀螺仪补偿
    float gyro_corrected[3];
    gyro_corrected[0] = gyro[0] + mf->Kp * e[0] + mf->Ki * mf->integral[0];
    gyro_corrected[1] = gyro[1] + mf->Kp * e[1] + mf->Ki * mf->integral[1];
    gyro_corrected[2] = gyro[2] + mf->Kp * e[2] + mf->Ki * mf->integral[2];

    // 四元数微分方程
    float qDot[4];
    qDot[0] = 0.5f * (-q1 * gyro_corrected[0] - q2 * gyro_corrected[1] - q3 * gyro_corrected[2]);
    qDot[1] = 0.5f * ( q0 * gyro_corrected[0] + q2 * gyro_corrected[2] - q3 * gyro_corrected[1]);
    qDot[2] = 0.5f * ( q0 * gyro_corrected[1] - q1 * gyro_corrected[2] + q3 * gyro_corrected[0]);
    qDot[3] = 0.5f * ( q0 * gyro_corrected[2] + q1 * gyro_corrected[1] - q2 * gyro_corrected[0]);

    // 四元数积分
    mf->q[0] += qDot[0] * dt;
    mf->q[1] += qDot[1] * dt;
    mf->q[2] += qDot[2] * dt;
    mf->q[3] += qDot[3] * dt;

    // 归一化四元数
    norm = sqrtf(mf->q[0]*mf->q[0] + mf->q[1]*mf->q[1] +
                 mf->q[2]*mf->q[2] + mf->q[3]*mf->q[3]);
    mf->q[0] /= norm;
    mf->q[1] /= norm;
    mf->q[2] /= norm;
    mf->q[3] /= norm;
}

4. 行业案例

案例1：DJI姿态解算

算法选择：

• 低端（Phantom 3）：互补滤波
• 中端（Mavic）：EKF
• 高端（Inspire 2）：非线性观测器

性能指标：

• 姿态精度：±0.02°
• 更新频率：400Hz
• 传感器：双IMU冗余

案例2：Pixhawk EKF2

特点：

• 24维状态向量
• 多传感器融合（IMU+GPS+气压计+磁力计+光流）
• 自适应噪声估计

性能：

• 位置精度：±0.5m（GPS有效）
• 高度精度：±0.3m
• 计算效率：STM32F4可实时运行

5. 算法对比

算法	计算量	精度	鲁棒性	适用场景
互补滤波	低	中	中	简单飞行器
卡尔曼滤波	中	高	高	通用飞控
EKF	高	很高	很高	高级飞控、多传感器
Mahony	低	高	高	嵌入式实时系统