[具身智能-615]：MU 九轴惯性测量传感器：9轴原始数据-＞物理量换算 -＞四元数 -＞ 欧拉角(角度) 过程详细解析

原始数据 → 物理量换算 → 四元数 → 欧拉角一步一步拆解，原理 + 公式 + 代码完全对应，看完就能彻底懂。

适用传感器：MPU9250 / ICM20948 / BNO055适用主控：树莓派 4B/5、RK3568/RK3588

0. 总流程（4 步黄金链路）

plaintext

九轴原始值 (Raw) ↓ 物理量换算 (g, °/s, μT) ↓ 姿态融合算法 → 输出 四元数 (Quaternion) ↓ 四元数转欧拉角 (Pitch / Roll / Yaw)

最终输出：俯仰、横滚、航向角

1. 第一步：九轴原始数据 → 物理量换算

1.1 原始数据是什么？

IMU 输出9 个 16 位有符号整数：

plaintext

raw_ax, raw_ay, raw_az 加速度 raw_gx, raw_gy, raw_gz 陀螺仪 raw_mx, raw_my, raw_mz 磁力计

范围：-32768 ~ +32767

1.2 换算公式（必须背）

① 加速度计（默认 ±2g）

plaintext

ax = raw_ax / 16384.0 # 单位 g ay = raw_ay / 16384.0 az = raw_az / 16384.0

• 静止时az ≈ 1.0
• 含义：重力加速度 + 运动加速度

② 陀螺仪（默认 ±250°/s）

plaintext

gx = raw_gx / 131.0 # 单位 °/s gy = raw_gy / 131.0 gz = raw_gz / 131.0

• 静止时 ≈ 0
• 含义：绕 X/Y/Z 轴的转动速度

③ 磁力计（默认 ±4912μT）

plaintext

mx = raw_mx / 0.15 # 单位 μT my = raw_my / 0.15 mz = raw_mz / 0.15

• 含义：地球磁场分量 → 用于找正北

2. 第二步：物理量 → 四元数（姿态融合核心）

2.1 什么是四元数？

q = [w, x, y, z]

• 4 个浮点数
• 表示3D 空间姿态
• 无万向锁、计算快
• 是 IMU 算法的标准输出

2.2 怎么得到四元数？

使用姿态融合算法，输入9 轴物理量。

最常用算法：Mahony 滤波

plaintext

q = mahony.updateIMU( q, gx, gy, gz, ax, ay, az, mx, my, mz )

算法内部做了什么（精简原理）

1. 陀螺仪→ 预测姿态
2. 加速度计→ 修正 Pitch/Roll
3. 磁力计→ 修正 Yaw（正北）
4. 输出稳定的四元数 q

3. 第三步：四元数 → 欧拉角（最终角度）

3.1 欧拉角是什么？

• Pitch 俯仰：抬头 / 低头
• Roll 横滚：左倾 / 右倾
• Yaw 偏航：朝向（正北 0°）

3.2 转换公式（标准通用）

已知四元数q = [w, x, y, z]

① Pitch (俯仰)

plaintext

pitch = asin( -2*x*z + 2*w*y ) * 57.2958

② Roll (横滚)

plaintext

roll = atan2( 2*y*z + 2*w*x, -2*x*x -2*y*y + 1 ) * 57.2958

③ Yaw (偏航 / 航向)

plaintext

yaw = atan2( 2*x*y + 2*w*z, -2*y*y -2*z*z + 1 ) * 57.2958

57.2958=180/π弧度转角度

4. 全流程代码逐行对应（最清晰）

python

运行

# ========== 1 原始数据 ========== raw_ax, raw_ay, raw_az, raw_gx, raw_gy, raw_gz, raw_mx, raw_my, raw_mz = read_9axis_raw() # ========== 2 原始 → 物理量 ========== ax = raw_ax / 16384.0 ay = raw_ay / 16384.0 az = raw_az / 16384.0 gx = raw_gx / 131.0 gy = raw_gy / 131.0 gz = raw_gz / 131.0 mx = raw_mx / 0.15 my = raw_my / 0.15 mz = raw_mz / 0.15 # ========== 3 物理量 → 四元数 ========== q = mahony.updateIMU(q, gx, gy, gz, ax, ay, az, mx, my, mz) w, x, y, z = q # ========== 4 四元数 → 欧拉角 ========== pitch = math.asin(-2*x*z + 2*w*y) * 57.2958 roll = math.atan2(2*y*z + 2*w*x, -2*x*x-2*y*y+1) * 57.2958 yaw = math.atan2(2*x*y + 2*w*z, -2*y*y-2*z*z+1) * 57.2958

5. 一张图记住整个流程

plaintext

[ 原始16位整数 ] ↓ 除以量程系数 [ 物理量 g, °/s, μT ] ↓ Mahony/Madgwick 融合 [ 四元数 q(w,x,y,z) ] ↓ 三角函数转换 [ 欧拉角 Pitch / Roll / Yaw ]

6. 你只需要记住这 4 条

1. 原始值必须转成物理量才能用
2. 加速度算倾角，陀螺仪算平滑，磁力计定正北
3. 算法输出四元数，不直接输出角度
4. 四元数转欧拉角用固定公式即可