1

IMU

BMI160 是一款由 Bosch Sensortec 生产的 6轴惯性测量单元(IMU)。它集成了 三轴加速度计三轴陀螺仪,主要用于精确的姿态、运动检测、导航和传感器融合任务。

三轴加速度计:

  • 负责测量设备在三个正交轴(X、Y、Z)上的加速度。加速度计的常见用途包括检测设备的运动、震动和倾斜。
  • 加速度范围通常有多个可选:±2g, ±4g, ±8g 和 ±16g(g = 重力加速度)。

三轴陀螺仪:

  • 负责测量设备绕三个轴(X、Y、Z)的角速度。陀螺仪的常见用途是检测设备的旋转、角度变化和姿态调整。
  • 陀螺仪的量程通常有多个选项:±125°/s, ±250°/s, ±500°/s 和 ±1000°/s。

获取IMU数据

通过sys虚拟文件系统读取设备获取加速度计的分辨率 (in_accel_scale) 和陀螺仪的分辨率 (in_anglvel_scale),使用 writeSysfs() 函数向设备节点写入配置参数,如启用某些通道、设置触发器等。 readSysfsDouble() 函数用于从 sysfs 中读取浮点值(如传感器的比例因子)。

在设备配置过程中,代码先禁用缓冲区 (/buffer/enable),然后启用它。这是因为在配置传感器参数时,为了避免中途采样,必须先禁用缓冲区。

readFrames() 函数用于从 IIO 设备读取数据帧。数据通过 poll() 系统调用来监听文件描述符(设备的文件描述符),等待数据到达。一旦数据可用,读取缓冲区并将数据填充到 outFrames 向量中。

设备配置

  • configureBmi160IioDevice():该函数执行 BMI160 设备的配置步骤,包括:
    • 关闭缓冲区。
    • 启用设备的各个数据通道(如加速度计、陀螺仪等)。
    • 设置触发器和缓冲区长度。
    • 启用缓冲区。
    • 配置完后,验证配置是否成功。

数据读取

  • readFrames():该函数从设备中读取数据帧。首先通过 poll() 检查设备是否有数据可用,如果有数据就通过 read() 读取并将其存储在 outFrames 中。该函数确保读取的数据对齐且大小正确。

数据写入与读取

  • writeSysfs():用于向设备的 sysfs 文件写入控制指令(如启用通道、设置触发器)。
  • readSysfsDouble():用于从 sysfs 文件读取配置值(如分辨率或比例因子)。

算法处理

1. Mahony滤波算法(姿态估计算法)

首先提到的 Mahony滤波器 是一种常用的 互补滤波 算法,主要用于 姿态估计(即获取设备的滚转角(roll)、俯仰角(pitch)和偏航角(yaw))。这是通过加速度计和陀螺仪的输入数据来实现的。

Mahony 互补滤波是一种轻量级的 IMU 姿态估计算法,由 Robert Mahony 等人提出,属于非线性互补滤波范畴,核心思想是利用加速度计(低频准确)和陀螺仪(高频准确)的频率特性互补,融合得到稳定的姿态估计。相比 EKF,它无需复杂的协方差矩阵计算,仅通过四元数更新和比例 - 积分(PI)误差校正实现,计算量极小(适合嵌入式系统),且能有效抑制陀螺仪漂移和加速度计噪声,是消费级 IMU 姿态解算的主流算法之一。

主要算法流程:

  • 输入:
    • 加速度计数据(accel_x, accel_y, accel_z
    • 陀螺仪数据(gyro_x, gyro_y, gyro_z
  • 输出:
    • 四元数(q0, q1, q2, q3)用于描述设备的方向,或者直接输出欧拉角(roll, pitch, yaw)。

具体步骤:

  1. 角速度积分:
    • 将陀螺仪的角速度积分得到角度变化。
  2. 加速度数据校准:
    • 使用加速度计数据计算设备的朝向(在重力场下,设备的加速度应该与重力方向对齐)。
  3. 滤波:
    • 通过加速度计和陀螺仪的数据来更新四元数。使用滤波增益和陀螺仪角速度调整预测的四元数,结合加速度计数据修正姿态估计。
  4. 四元数更新:
    • 利用互补滤波的原则,结合加速度计和陀螺仪的误差修正,更新四元数表示的设备方向。

Mahony滤波器的实现:

1
2
3
4
5
6
// 更新滤波器,计算新的四元数
mahony_filter.update(sample);

// 获取当前的四元数表示的姿态
float q[4];
mahony_filter.get_quaternion(q[0], q[1], q[2], q[3]);

核心思想:

Mahony滤波器通过加速度计和陀螺仪数据的融合,实现对设备姿态的准确估计。通过不断更新四元数,可以在实时环境下计算出设备的 滚转(roll)俯仰(pitch)偏航(yaw) 角度。

2. 碰撞检测算法(基于加速度的检测)

碰撞检测是通过加速度计的数据来判断是否发生了剧烈的碰撞或振动。这通常是通过检测加速度的 变化幅度 来判断的。

主要算法流程:

  1. 加速度的幅度计算:

    • 每次读取到加速度数据时,计算加速度的 模长(norm),即:

    norm=(accel_x2+accel_y2+accel_z2)\text{norm} = \sqrt{(accel_x^2 + accel_y^2 + accel_z^2)}norm=(accel_x2+accel_y2+accel_z2)

    这表示设备在三维空间中的加速度大小。

  2. 判断碰撞:

    • 如果加速度的模长 大于某个阈值小于某个上限值,则认为发生了可能的碰撞或剧烈振动。比如,阈值设置为 8g 到 10g 之间。
  3. 状态变化:

    • 如果检测到碰撞,则进入碰撞状态,并在一定时间内持续监测。如果加速度值回归到正常范围,则退出碰撞检测状态。
    • 使用一个 计时器collision_timestamp)来记录持续时间,如果持续时间超过阈值,触发 重启 操作(例如,重置滤波器)。
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
// 计算加速度的模长(norm)
float norm = sqrt(sample.accel_x * sample.accel_x +
sample.accel_y * sample.accel_y +
sample.accel_z * sample.accel_z);

// 如果加速度模长在8到10之间,则认为是一次碰撞
if (norm < 10.0f && norm > 8.0f) {
collision_timestamp++;
} else {
collision_timestamp = 0;
}

// 如果持续时间超过100次采样,则认为是有效碰撞,重置IMU
if (collision_timestamp > 100) {
collision_detected = false;
mahony_filter.set_restart(); // 重置滤波器
}