[姿态解算]MCU如何利用四元数进行姿态解算？

向量叉积概念

同向量叉积：i×i = 0, j×j = 0, k×k = 0
正向循环    ： i×j = k, j×k = i, k×i = j
反向循环    ： j×i = -k, k×j = -i, i×k = -j

我们知道向量是一个既有大小，也有方向的一个东西，那么就存在一个互相垂直的单位向量i、j、k组成一个坐标系，在这个坐标系的任何向量都可以用单位向量i、j、k来表示，那么向量叉积就需要我们引入i、j、k这三个单位向量，但是要注意，这个i、j、k是向量，不要和四元数中的虚数单位给弄混了。

在这个三维坐标系空间中，我们要表示一个一个向量，怎么表示呢？很明显，就是p = Xi + Yj + Zk，ijk是单位向量，p就是我们要表示向量，由三个互相垂直的单位向量乘以不同的系数组合而成，那么两个这样的向量相乘，会出现什么？

P1 = X1i + Y1j + Z1k

P2 = X2i + Y2j +Z2k

P1 × P2 = (X1i + Y1j + Z1k) × (X2i + Y2j + Z2k)

//这一步有些人会理解错，X1*X2能不能错位等等，因为前面我们学习到，比如 i*j = – j*i ，但我们只需要注意，X1、X2、Y1…这样的只是系数，而不是向量，所以谁在先后都无所谓。

P1 × P2 = X1·X2·(i×i) + X1·Y2·(i×j) + X1·Z2·(i×k)
+ Y1·X2·(j×i) + Y1·Y2·(j×j) + Y1·Z2·(j×k)
+ Z1·X2·(k×i) + Z1·Y2·(k×j) + Z1·Z2·(k×k)

P1 × P2 = 0 + X1·Y2·k + (-X1·Z2·j) + (-Y1·X2·k) + 0 + Y1·Z2·i + Z1·X2·j + (-Z1·Y2·i) + 0

//最终得出结果

P1 × P2 = X1·Y2·k – X1·Z2·j – Y1·X2·k + Y1·Z2·i + Z1·X2·j – Z1·Y2·i

P1 × P2 = (Y1·Z2 – Z1·Y2)·i + (Z1·X2 – X1·Z2)·j + (X1·Y2 – Y1·X2)·k

为了方便我们记忆，可以直接使用行列式进行记忆：

■■■■■| i      j     k  |
■ P1×P2 = | X1 Y1 Z1  |
■■■■■| X2 Y2 Z2 |

向量点积，又称数量积、标量积或内积，是向量代数中最基础且应用广泛的一种运算。和向量乘积不同，向量的点积是满足交换律的。

几何上，向量点积可以表示为两个向量的长度与其夹角余弦的乘积：

有这个点积有什么用呢？它可以让我们判断两个向量的几何关系：

向量归一化是指将向量的模（或长度）变为1，同时保持其方向不变的处理方法。对于任意非零向量   ，其归一化后的向量表示为  ，满足  且 与 方向一致。

那么数学表达式就可以简单的表示为：

四元数是姿态解算中最重要的因素，它可以让MCU“理解”物体是绕哪个点的“轴”，进行多少度的旋转。

四元数是一个超复数，形式为： q=a+bi+cj+dk

其中： * a,b,c,d是实数（分别称为实部和虚部）。

* i,j,k是虚数单位，满足以下乘法规则：

• i² = j² = k² = ijk = -1
• i × j = k, j × k = i, k × i = j
• j × i = -k, k × j = -i, i × k = -j

注意上面的公式，这些不是完全”推导”出来的，而是Hamilton的巧妙定义：

第一步：基本定义

i² = j² = k² = ijk = -1

这是Hamilton设定的基础规则，就像复数中定义i²=-1一样。这里最关键的是ijk = -1这个条件。

第二步：从基础规则推导其他关系

让我们用ijk = -1来推导：

推导ij = k：

• 从ijk = -1开始
• 两边同时右乘k：(ijk)k = -1·k
• 左边变成：ij(k²) = ij(-1) = -ij
• 右边是：-k
• 所以：-ij = -k，因此ij = k

推导jk = i：

• 从ijk = -1开始
• 两边同时左乘i：i(ijk) = i(-1)
• 左边：(i²)jk = (-1)jk = -jk
• 右边：-i
• 所以：-jk = -i，因此jk = i

推导ki = j：

• 从ijk = -1开始
• 两边同时”夹在中间”乘j：j(ijk)j = j(-1)j = -j²= -(-1) = 1
• 但这个推导比较复杂，直接用循环对称性更容易理解

第三步：反向乘法（交换律失效）

四元数乘法不满足交换律！所以：

• 如果ij = k，那么ji = -k
• 如果jk = i，那么kj = -i
• 如果ki = j，那么ik = -j

记忆技巧

正向循环（顺时针）：i→j→k→i

• ij = k, jk = i, ki = j

反向循环（逆时针）：i→k→j→i

• ji = -k, kj = -i, ik = -j

可以想象i,j,k按顺序排在圆圈上，顺时针相乘得正结果，逆时针相乘得负结果。

这就是Hamilton在1843年走过都柏林的Brougham桥时突然想到的数学结构——一个既能描述3D旋转，又保持代数完整性的美妙系统！

三轴加速度传感器是一种基于微机电系统技术制造的线性加速度传感器。

核心功能： 它的核心作用是精确测量物体在三个相互垂直的空间轴（通常定义为 X 轴、Y 轴和 Z 轴）上的线性加速度。

• 简单来说，它就像一个精准的运动状态探测器，专门感知你是否在移动、往哪个方向移动、移动得有多快或多慢。
• 当设备发生任何形式的加速度变化时（包括静止时受到的重力加速度、突然启动、刹车、上下颠簸等），三轴加速度传感器都能感知到这种变化，并输出相应的电信号来表示这三个轴向上的加速度大小（单位通常是 g 或 m/s²）。

特别提示： 即使设备完全静止，加速度传感器也会检测到地球重力加速度（约9.8 m/s²），这也是它能用来判断设备倾斜角度的原理。

三轴陀螺仪是一种基于微机电系统技术制造的角速度传感器。

核心功能：

它的核心作用是精确测量物体绕其自身在三个相互垂直的空间轴（通常定义为 X 轴、Y 轴和 Z 轴）旋转的角速度。

• 简单来说，它就像一个精准的旋转测量仪，专门探测你如何转、往哪个方向转、转得有多快。
• 当设备发生旋转时（无论是轻微抖动还是快速旋转），三轴陀螺仪都能感知到这个动作，并输出相应的电信号来表示这三个轴向上的旋转速度（单位通常是 度/秒 或 rad/s）。

零偏就是传感器在静止状态下的非零读数。理想情况下，静止的陀螺仪应该读数为0，但实际总是有偏差。

就像一个不准的秤，真实重量: 0kg 电子秤显示: 0.2kg ← 这个0.2kg就是”零偏”。

typedef struct {
    Vector3 accel_bias;
    Vector3 gyro_bias;
    bool calibrated;
} BiasCorrection;

BiasCorrection bias = {0};

void calibrateBias() {
    printf("开始校正，请保持设备静止...\n");
    
    Vector3 accel_sum = {0, 0, 0};
    Vector3 gyro_sum = {0, 0, 0};
    int samples = 1000;
    
    // 采集1000个静态样本
    for (int i = 0; i < samples; i++) {
        RawIMU raw = readSensors();
        
        accel_sum.x += raw.accel_x;
        accel_sum.y += raw.accel_y;
        accel_sum.z += raw.accel_z;
        
        gyro_sum.x += raw.gyro_x;
        gyro_sum.y += raw.gyro_y;
        gyro_sum.z += raw.gyro_z;
        
        delay(1);  // 1ms间隔
    }
    
    // 计算平均值作为零偏
    bias.gyro_bias.x = gyro_sum.x / samples;
    bias.gyro_bias.y = gyro_sum.y / samples;
    bias.gyro_bias.z = gyro_sum.z / samples;
    
    // 加速度计校正（考虑重力）
    bias.accel_bias.x = accel_sum.x / samples;
    bias.accel_bias.y = accel_sum.y / samples;
    bias.accel_bias.z = accel_sum.z / samples - 9.8f;  // 减去重力
    
    bias.calibrated = true;
    printf("校正完成！\n");
    
    // 保存到EEPROM供下次使用
    saveBiasToEEPROM(bias);
}

不同的传感器、应用和计算标准使用不同的坐标系，需要统一到同一坐标系中。

某些IMU芯片：
X轴：向右
Y轴：向前
Z轴：向上

另一些IMU芯片：
X轴：向前
Y轴：向右
Z轴：向下

// 原始坐标系
// X: 向右, Y: 向前, Z: 向上

// 目标：标准机体坐标系  
// X: 向前, Y: 向右, Z: 向下

Vector3 transformMPU6050ToStandard(Vector3 sensor_data) {
    Vector3 standard;
    
    // 坐标轴重新映射
    standard.x = sensor_data.y;   // 前 = 传感器的Y
    standard.y = sensor_data.x;   // 右 = 传感器的X  
    standard.z = -sensor_data.z;  // 下 = 传感器的-Z
    
    return standard;
}