Version1.0.0
FileBMI088_driver.hBMI088_driver.cppBMI088_middleware.hBMI088_middleware.cppBMI088_reg.hpyro_ins.hpyro_ins.cpp
PYRo INS + BMI088 惯性导航系统
本文档描述 PYRo 机器人惯性导航系统的完整技术栈，从底层 BMI088 六轴 IMU 传感器驱动，到中间层 INS 组件（ins_drv_t）的数据采集与坐标映射，再到上层 QuaternionEKF 姿态融合算法，逐层串联。适用于理解系统架构、移植硬件以及定制姿态控制参数。
嵌入式开发前置知识：了解 SPI 总线协议、IMU 传感器工作原理、四元数姿态表示、扩展卡尔曼滤波（EKF）基本原理
系统架构总览
┌──────────────────────────────────────────────────────────────┐
│ Robot Modules │
│ (gimbal / chassis / booster) │
│ │
│ ins_drv_t::get_rads_b() get_gyro_b() get_quaternion() │
└──────────────────────────┬───────────────────────────────────┘
│
┌──────────────────────────▼───────────────────────────────────┐
│ ins_drv_t (INS 组件) │
│ Component/INS/pyro_ins.cpp │
│ │
│ • 轴向映射 (8 种安装方向) │
│ • FreeRTOS 1kHz 任务 │
│ • 调用 BMI088_Read() → QuaternionEKF_Update() │
│ • 对外提供角度/角速度/加速度/四元数接口 │
└──────────────────────────┬───────────────────────────────────┘
│
┌────────────────┼────────────────┐
▼ ▼
┌─────────────────────┐ ┌─────────────────────────┐
│ BMI088 Driver │ │ QuaternionEKF │
│ Device/BMI088/ │ │ Algorithm/EKF/ │
│ │ │ │
│ • SPI 寄存器读写 │ │ • 四元数姿态更新 │
│ • 零偏校准 │ │ • 陀螺仪偏置在线估计 │
│ • 原始数据读取 │ │ • 卡方检验异常检测 │
│ • 温度补偿 │ │ • 自适应渐消因子 │
└─────────────────────┘ └─────────────────────────┘Part 1: BMI088 底层驱动
1.1 文件结构
| 文件 | 职责 |
|---|---|
BMI088_reg.h | 寄存器地址与配置位掩码，与数据手册一一对应 |
BMI088_middleware.h/.cpp | 硬件抽象层：SPI 收发、CS 片选、延时函数，隔离 HAL 依赖 |
BMI088_driver.h/.cpp | 核心驱动：初始化、校准、数据读取，不直接操作 GPIO |
1.2 关键数据结构
// 全局 IMU 数据实例
typedef struct {
float Accel[3]; // 加速度 m/s² (体坐标系, 已校准)
float Gyro[3]; // 角速度 rad/s (体坐标系, 已去零偏)
float TempWhenCali; // 校准时刻温度
float Temperature; // 当前温度 °C
float AccelScale; // 加速度缩放因子 (gNorm 校准后自动计算)
float GyroOffset[3]; // 陀螺仪零偏 rad/s
float gNorm; // 重力加速度模长
} IMU_Data_t;
// 校准配置
typedef struct {
uint8_t calibrate; // 1=自动校准, 0=使用预设值
float gx_offset; // 预设陀螺仪 X 零偏
float gy_offset; // 预设陀螺仪 Y 零偏
float gz_offset; // 预设陀螺仪 Z 零偏
float g_norm; // 预设重力模长
} bmi088_config_t;1.3 加速度计与陀螺仪独立管理
BMI088 芯片内部集成两个独立传感器，通过共享 SPI 总线 + 独立 CS 片选访问：
| 传感器 | CHIP_ID | CS 引脚 | 默认配置 |
|---|---|---|---|
| 加速度计 | 0x1E | PC0 (CS1_ACCEL) | ±6g, 800Hz ODR, 正常模式 |
| 陀螺仪 | 0x0F | PC3 (CS1_GYRO) | ±2000°/s, 2000Hz ODR / 230Hz BW |
通信方式通过 BMI088_USE_SPI / BMI088_USE_IIC 宏切换（当前仅实现 SPI）。中间件层提供 CS 片选宏和 SPI 单字节收发函数，驱动层通过宏封装读写流程：
// 加速度计单寄存器写入（CS 自动拉低/拉高）
BMI088_accel_write_single_reg(BMI088_ACC_PWR_CTRL, BMI088_ACC_ENABLE_ACC_ON);
// 陀螺仪多字节突发读取（CS 自动拉低/拉高）
BMI088_gyro_read_muli_reg(BMI088_GYRO_CHIP_ID, buf, 8);1.4 初始化流程
两个传感器的初始化分别独立执行，但共享同一种"写入-回读-比对"的错误检测模式：
┌─ 读 CHIP_ID (×2, 去旧值) ──┐
│ 软复位 (写 0xB6) │
│ 等 80ms │
bmi088_accel_init() ─┤ 重读 CHIP_ID 验证 ├──→ 任一寄存器不匹配 → error |= 对应码
bmi088_gyro_init() ─│ 顺序配 6 个寄存器 │
│ 每次写后回读比对 │
└──────────────────────────┘加速度计配置序列：
| 寄存器 | 写入值 | 含义 |
|---|---|---|
ACC_PWR_CTRL (0x7D) | 0x04 | 使能加速度计 |
ACC_PWR_CONF (0x7C) | 0x00 | 激活模式 |
ACC_CONF (0x40) | 0xA8 | 正常模式 + 800Hz + 必须置位 |
ACC_RANGE (0x41) | 0x01 | ±6g |
INT1_IO_CTRL (0x53) | 0x08 | INT1 推挽输出 |
INT_MAP_DATA (0x58) | 0x04 | DRDY 中断 → INT1 |
陀螺仪配置序列：量程 2000°/s, 带宽 230Hz, 正常模式, DRDY 中断启用并映射到 INT3。
1.5 零偏校准算法
┌───────────┐ 抖动超标
│ 采 6000 次 │───────────────┐
│ 求 Gyro │ │
│ Offset │ │
│ 均值 + │ ┌───────────▼──────────────┐
│ gNorm │ │ 抖动检测: │
└─────┬─────┘ │ gyroDiff > 0.15 rad/s? │
│ │ gNormDiff > 0.5 m/s²? │
┌─────▼─────┐ └───────────┬──────────────┘
│ 有效性验证 │ 否 │ 是 → 重采样
│ |gNorm- │ │
│ 9.8|<0.5? │ │
│ |Offset| │ │
│ <0.01? │ │
└─────┬─────┘ │
是 │ │
┌─────▼─────┐ │
│ 计算 │ │
│ AccelScale│ │
│ = 9.81/ │ │
│ gNorm │ │
└─────┬─────┘ │
│ 超时 10s │
┌─────▼─────┐ │
│ 完成 │◄──────────────┘
└───────────┘ (回退预设值)核心设计要点：
- 抖动检测：校准过程中持续跟踪陀螺仪和加速度模长的 max/min。若传感器在采样期间有明显晃动，该轮数据无效并重新采样——这确保了校准仅在静止状态下完成。
- 收敛条件：
gNormDiff < 0.5、|gNorm - 9.8| < 0.5、|GyroOffset[i]| < 0.01、每个轴的gyroDiff[i] < 0.15——全部满足才认为校准有效。 - 超时保护：超过 10 秒未收敛，回退到
bmi088_config_t中预设的 offset 值。 - 温度记录：校准结束时记录
TempWhenCali，供判断温漂补偿时机。
1.6 数据读取与转换
BMI088_Read() 在一个函数调用中完成加速度计、陀螺仪、温度的全部读取：
// 加速度: raw → m/s²
Accel[i] = raw_int16 * BMI088_ACCEL_SEN * AccelScale;
// 其中 BMI088_ACCEL_SEN = 0.001794 g/LSB (6g 量程)
// AccelScale = 9.81 / gNorm (校准后自动缩放)
// 陀螺仪: raw → rad/s
Gyro[i] = raw_int16 * BMI088_GYRO_SEN - GyroOffset[i];
// 其中 BMI088_GYRO_SEN = 0.001065 rad/s/LSB (2000°/s 量程)
// 温度: raw_11bit → °C
Temperature = raw * 0.125f + 23.0f;
// 负温处理: raw > 1023 时 raw -= 2048Part 2: INS 组件 (ins_drv_t)
ins_drv_t 是连接 BMI088 驱动与上层机器人模块的桥梁。它不属于 Peripheral（外设驱动），而是 Component（组件），负责：
- 管理 BMI088 的初始化和校准配置
- 创建 FreeRTOS 任务以 1kHz 周期运行
- 处理 IMU 在机器人上的 8 种安装方向（轴向映射）
- 调用 QuaternionEKF 完成姿态融合
- 向上层提供线程安全的数据读取接口
2.1 类结构
class ins_drv_t {
// ─── 原始数据 ───
IMU_Data_t imu_data; // BMI088 驱动填充的原始数据
// ─── 姿态估计结果 ───
float _q[4]; // 四元数 [w, x, y, z]
float _gyro_b[3]; // 体坐标系角速度 (rad/s)
float _acc_b[3]; // 体坐标系加速度 (m/s²)
float _angle_n[3]; // 导航系欧拉角 Roll/Pitch/Yaw (°)
// ─── 时间管理 ───
float _dt; // 实际采样周期 (s), 由 DWT 测量
float _t; // 累计时间
uint32_t _dwt_cnt; // DWT 计数器基准值
// ─── 轴向映射 ───
uint8_t _imu_x_idx; // 物理 X 轴 → 逻辑轴索引
uint8_t _imu_y_idx;
uint8_t _imu_z_idx;
bool _x_neg, _y_neg, _z_neg; // 各轴是否需要取反
// ─── 配置 ───
ins_config_t _config;
// ─── FreeRTOS 任务 ───
static TaskHandle_t _ins_task_handle;
};2.2 轴向映射——关键设计
机器人设计中 IMU 的物理安装方向与机器人坐标系的对应关系不是固定的。ins_config_t::direct 枚举提供了 8 种预定义方向：
| direct | 逻辑 X (Roll) | 逻辑 Y (Pitch) | 逻辑 Z (Yaw) | 典型安装 |
|---|---|---|---|---|
DIRECT_1 | 物理 X → X | 物理 Y → Y | 物理 Z → Z | 默认正向 |
DIRECT_2 | 物理 Y → X (反) | 物理 X → Y | 物理 Z → Z | 水平旋转 90° |
DIRECT_3 | 物理 X → X (反) | 物理 Y → Y (反) | 物理 Z → Z | 翻转 |
| ... | ... | ... | ... | ... |
DIRECT_8 | 物理 Y → X | 物理 X → Y (反) | 物理 Z → Z (反) | ... |
映射逻辑在 __ins_task() 循环中执行：先根据 _imu_x/y/z_idx 从 imu_data.Accel[3] / imu_data.Gyro[3] 取出对应物理轴的数据，再根据 _x/y/z_neg 决定是否取反，最终赋值到 _acc_b[X/Y/Z] / _gyro_b[X/Y/Z]。
2.3 初始化流程
status_t ins_drv_t::init(const ins_config_t &config)
{
// 1. 解析轴向映射
switch (config.direct) { ... }
// 2. 构造 BMI088 配置
bmi088_config_t bmi_cfg;
bmi_cfg.calibrate = config.calibrate;
bmi_cfg.gx_offset = config.gx_offset;
// ...
// 3. 重试初始化 BMI088 (最多 10 次)
for (count = 0; BMI088_init(&hspi2, &bmi_cfg, &imu_data) != BMI088_NO_ERROR; count++)
{ if (count >= 255) return PYRO_ERROR; }
// 4. 创建 FreeRTOS 任务 (仅一次)
xTaskCreate(__static_ins_task, "ins_task", 512, this, 2, &_ins_task_handle);
// 5. 若启用了校准, 返回 PYRO_WARNING (提示上位机校准耗时较长)
return config.calibrate ? PYRO_WARNING : PYRO_OK;
}2.4 运行循环 (__ins_task)
1kHz 循环 (vTaskDelayUntil 1ms)
┌────────────────────────────────────────┐
│ │
│ 1. _dt = dwt_drv_t::get_delta_t() │ ← DWT 高精度测时
│ │
│ 2. BMI088_Read(&imu_data) │ ← 读加速度+陀螺仪+温度
│ │
│ 3. 轴向映射 + 取反 │ ← _acc_b[3], _gyro_b[3]
│ │
│ 4. IMU_QuaternionEKF_Update( │ ← EKF 姿态融合
│ gx,gy,gz, ax,ay,az, _dt) │
│ │
│ 5. memcpy(_q, QEKF_INS.q) │ ← 提取四元数结果
│ _angle_n = QEKF_INS.Roll/Pitch/Yaw │ ← 提取欧拉角
│ │
└────────────────────────────────────────┘循环采用简单的 while(1) + vTaskDelay(1) 而非 vTaskDelayUntil，实际周期由 DWT 精确测量（_dt），传递给 EKF 的是真实时间增量，不依赖操作系统 tick 精度。
2.5 对外接口
INS 提供两类坐标系的数据访问。_b 后缀表示体坐标系 (Body frame)——与机器人固连的坐标系；_n 后缀表示导航坐标系 (Navigation frame)——水平面参考系。当前实现中体坐标系与导航坐标系的角速度和加速度值相同（因为 EKF 内部已按体坐标系解算），欧拉角则以导航坐标系角度输出。
| 接口 | 返回内容 | 坐标参考 |
|---|---|---|
get_rads_b/n(yaw,pitch,roll) | Roll/Pitch/Yaw 弧度 | 导航系欧拉角 (rad) |
get_angles_b/n(yaw,pitch,roll) | Roll/Pitch/Yaw 角度 | 导航系欧拉角 (°) |
get_gyro_b/n(yaw,pitch,roll) | 三轴角速度 | 体坐标系 (rad/s) |
get_accel_b/n(x,y,z) | 三轴加速度 | 体坐标系 (m/s²) |
get_quaternion(q0,q1,q2,q3) | 四元数 | [w, x, y, z] |
所有 getter 均做空指针检查，返回 PYRO_ERROR 表示参数非法。
Part 3: QuaternionEKF 姿态融合算法
姿态解算使用 Wang Hongxi 的 QuaternionEKF 库（Algorithm/EKF/QuaternionEKF.*），核心是四元数扩展卡尔曼滤波器，融合陀螺仪角速度和加速度计重力矢量观测。
3.1 状态与观测
- 状态向量 (7 维)：四元数
[q0,q1,q2,q3]+ 陀螺仪偏置[bx,by,bz] - 过程模型：角速度积分预测姿态；陀螺仪偏置建模为随机游走
- 观测模型：加速度计测量重力方向矢量（归一化后与预测重力方向比较）
3.2 关键参数
void IMU_QuaternionEKF_Init(
float process_noise1, // Q1: 四元数过程噪声 → 影响姿态收敛速度
float process_noise2, // Q2: 陀螺仪偏置过程噪声 → 影响偏置估计速度
float measure_noise, // R: 加速度计量测噪声 → 影响加速度信任度
float lambda, // 渐消因子 → 增强机动响应
float lpf // 加速度低通滤波系数 → 抑制振动干扰
);
// INS 中的实际参数:
IMU_QuaternionEKF_Init(
10, // Q1 — 较高过程噪声，快速姿态收敛
0.001, // Q2 — 低偏置噪声，缓慢偏置估计
10000000, // R — 极高量测噪声，弱化加速度计影响（高机动场景）
0.9996, // λ — 渐消因子，接近 1 = 弱渐消
0.004f // LPF — 强低通滤波，抑制高频振动
);3.3 卡方检验异常检测
滤波器内部实现了卡方检验 (ChiSquareTest) 用于检测加速度计异常（如强烈振动、冲击）。当检测函数超过阈值时，通过渐消因子 lambda 自动降低加速度计量测的权重，防止异常值污染姿态估计。
3.4 输出结果
extern QEKF_INS_t QEKF_INS;
QEKF_INS.q[4]; // 四元数估计值
QEKF_INS.Roll; // 横滚角 (°)
QEKF_INS.Pitch; // 俯仰角 (°)
QEKF_INS.Yaw; // 偏航角 (°)
QEKF_INS.GyroBias[3]; // 陀螺仪偏置在线估计 (rad/s)
QEKF_INS.ConvergeFlag; // 滤波器收敛标志
QEKF_INS.StableFlag; // 姿态稳定标志Part 4: 快速使用
4.1 初始化
// pyro_init_thread.cpp
#include "pyro_ins.h"
#include "pyro_dwt_drv.h"
void pyro_init_thread(void *argument)
{
// 1. 必须先初始化 DWT（INS 循环中的 dt 测量依赖 DWT）
pyro::dwt_drv_t::init(480);
// 2. 配置 INS
ins_drv = ins_drv_t::get_instance();
ins_config_t ins_cfg;
ins_cfg.calibrate = IMU_CALIBRATION_EN; // 由 CMake 编译指令传入
ins_cfg.direct = ins_config_t::imu_direct_t::DIRECT_2;
ins_cfg.gx_offset = -0.000977554359f;
ins_cfg.gy_offset = -0.00342695904f;
ins_cfg.gz_offset = 0.0015946941f;
ins_cfg.g_norm = 9.78534603f;
}4.2 读取姿态数据
// 在模块的 _update_feedback() 或 _fsm_execute() 中调用
void my_module::_update_feedback()
{
auto *ins = pyro::ins_drv_t::get_instance();
// 方式1: 获取欧拉角（度）
float yaw_deg, pitch_deg, roll_deg;
ins->get_angles_b(&yaw_deg, &pitch_deg, &roll_deg);
// 方式2: 获取欧拉角（弧度）—— 推荐用于控制计算
float yaw_rad, pitch_rad, roll_rad;
ins->get_rads_b(&yaw_rad, &pitch_rad, &roll_rad);
// 方式3: 获取角速度
float gyro_yaw, gyro_pitch, gyro_roll;
ins->get_gyro_b(&gyro_yaw, &gyro_pitch, &gyro_roll);
// 方式4: 获取加速度
float acc_x, acc_y, acc_z;
ins->get_accel_b(&acc_x, &acc_y, &acc_z);
// 方式5: 获取四元数（用于相对姿态解算）
float q0, q1, q2, q3;
ins->get_quaternion(&q0, &q1, &q2, &q3);
}4.3 切换安装方向
如果 IMU 在 PCB 上的安装方向发生物理变化，只需修改 direct 枚举：
在 ins 中，根据安装方向的不同，ins_cfg.direct 有8种可选项，以 H7 的 typec 指向车头，正面朝上为方向1，顺时针旋转一周为1-4.反面朝上为方向5，顺时针旋转一圈为5-8
4.4 校准参数
BMI088 每次上电执行自动校准需要 4~6 秒静止时间。在实际部署中，一旦某台机器人的 IMU 校准参数确定后，物理上不会再变化。因此推荐的工作流是：首次校准 → VSCode 调试器读取参数 → 后续启动直接使用预设值，跳过校准。
校准完成后，IMU_Data_t imu_data（ins_drv_t 的私有成员）中的以下四个值即为该传感器在当前位置的参数（当环境变化比较大的时候可能需要重新校准，问题表现是静置 imu，读取角度，角度值一直偏移）：
| 字段 | 含义 | 对应 ins_config_t 字段 |
|---|---|---|
imu_data.GyroOffset[0] | 陀螺仪 X 轴零偏 (rad/s) | gx_offset |
imu_data.GyroOffset[1] | 陀螺仪 Y 轴零偏 (rad/s) | gy_offset |
imu_data.GyroOffset[2] | 陀螺仪 Z 轴零偏 (rad/s) | gz_offset |
imu_data.gNorm | 静止时重力加速度模长 (m/s²) | g_norm |
步骤 1：启用校准，断点等待
在 CMake 中启用 calibrate = true，确保机器人完全静止（放在水平面上），使用 VSCode 进入调试模式
步骤 2：在 INS 任务循环中设断点，通过 Locals 窗口查看
等待半分钟左右后，在 pyro_ins.cpp 的 __ins_task() 循环体中设断点——建议打在 BMI088_Read 之后、EKF 更新之前的任意位置（此时 this 指针可访问，imu_data 已填充校准后的值）
将 GyroOffset[0]、GyroOffset[1]、GyroOffset[2] 和 gNorm 四个值记录下来。
步骤 3：将参数硬编码到初始化代码中
将调试器中读取的四个值填入初始化代码，关闭校准：
Part 5: 注意事项
软件层面
初始化顺序：必须先
dwt_drv_t::init()，再ins_drv_t::init()。INS 任务中的dt计算和 BMI088 校准中的延时都依赖 DWT。任务栈大小：INS 任务栈默认 512 words。若添加自定义的传感器后处理逻辑，注意栈使用情况（EKF 内部包含大量矩阵运算）。