2-37 IMU 足部惯性导航与步态轨迹跟踪

一句话介绍： 这套程序读取安装在脚部的 x-IMU 三轴陀螺仪和三轴加速度计数据，先检测脚掌静止区间，再用四元数 AHRS 估计传感器姿态，把加速度转换到地球坐标系，并通过 ZUPT 零速约束和步间线性漂移补偿得到速度，最后再次积分重建脚部的三维行走轨迹。

先看它解决什么问题

如果把一个 IMU 固定在脚上，我们能够直接测到的只有：

三轴角速度；三轴加速度。

但真正希望得到的是：人走了多远、向哪个方向走、有没有上楼，以及脚在三维空间中的运动轨迹。

这并不能直接从某一列传感器数据读出来。

程序需要连续完成：

陀螺仪 + 加速度计 → 静止检测 → 姿态估计 → 坐标系旋转 → 去重力 → 加速度积分 → 零速修正 → 漂移补偿 → 速度积分 → 三维位置

1. 工程中最核心的文件是什么？

虽然工程包含一套 x-IMU 数据读取库和四元数工具函数，但最终理解程序只需要抓住几个核心文件：

2. 工程自带了哪些步行数据？

Datasets 中提供 3 组实验数据：

straightLine：直线行走；

stairsAndCorridor：楼梯与走廊；

spiralStairs：螺旋楼梯。

每组同时保留：.BIN 原始记录；_CalInertialAndMag.csv 标定后的惯性与磁场数据。

当前 Script.m 实际通过 x-IMU MATLAB library 读取的是：

标定后的 CSV 数据。

陀螺仪 X/Y/Z + 加速度计 X/Y/Z + 磁力计 X/Y/Z

不过主脚本真正参与轨迹计算的是：陀螺仪 + 加速度计。

3. 当前默认分析数据？

源码默认启用：straightLine

并截取：6 s ～ 26 s

采样频率为：fs = 256 Hz

所以采样周期：Δt = 1/256 ≈ 0.00390625 s

20 s 的分析窗口包含：5121 个采样点

4. 第一步为什么必须先找到“脚不动”的时刻？

惯性导航最大的难点是：

很小的加速度误差经过一次积分会变成速度误差，再积分一次就会变成越来越大的位置漂移。

脚部行走有一个非常有利的特点：每走一步，脚掌通常都会有一段时间落地并近似静止。

如果程序能够找到这些时刻，就知道：真实脚速应该约等于 0。

这给惯性积分提供了周期性的绝对约束。

这就是程序后面 ZUPT 的基础。

5. 程序怎样检测脚是否静止？

先计算三轴加速度合成模：a_mag = √(aₓ² + aᵧ² + a_z²)

随后经过三步处理：0.001 Hz 一阶高通 → 取绝对值 → 5 Hz 一阶低通

得到平滑的运动强度：a_filt

最后使用固定阈值：a_filt < 0.05

判断为：stationary = 1

也就是脚处于静止状态。

这个处理的直观意义是：

高通去掉接近恒定的重力 / 直流成分；

绝对值只保留运动偏离大小；

低通去掉快速尖锐波动；

最后用阈值区分“脚落地静止”和“摆动”。

默认直线数据窗口中，按源码等价复算约有：41% 的采样点被判定为静止。

6. 为什么还要估计 IMU 姿态？

加速度计输出是在：传感器自身坐标系中测得的。

但走路过程中脚会不断：

抬起；俯仰；转动。例如传感器 X 轴上的加速度，并不永远对应地面的前进方向。

所以必须先知道每一时刻传感器相对于地球坐标系的姿态，然后把：Body Frame 加速度旋转成：

Earth Frame 加速度。

程序用四元数：q = [q₀, q₁, q₂, q₃]表示这个姿态。

7. AHRS 怎样融合陀螺仪和加速度计？

AHRS.m 首先根据当前四元数估计重力方向：

v = gravity direction predicted by q

然后把测得的加速度归一化，计算：e = v × a_normalized

这个叉乘误差表示：

当前姿态估计出来的重力方向，与加速度计观察到的重力方向偏了多少。

程序再修正陀螺仪：ω_ref = ω_gyr - Kp·e

最后使用四元数微分关系：q̇ = 0.5 · q ⊗ [0, ω_ref]

并按采样周期积分：q(k) = normalize[q(k-1) + q̇Δt]

这样就能连续得到每个采样点的姿态。

8. 为什么运动时 Kp=0，静止时 Kp=0.5？

这是当前程序很有特点的一处设计。

当脚静止时：Kp = 0.5

程序相信此时加速度计主要测到重力，因此可以利用重力方向修正陀螺仪姿态漂移。

当脚正在摆动时：Kp = 0

因为这时加速度计同时包含明显的脚部运动加速度。

如果仍把它当成重力方向，反而会把错误信息反馈给姿态估计。

因此程序采用：静止 → 陀螺仪 + 重力方向校正、运动 → 主要依靠陀螺仪积分的策略。

在正式处理全部数据之前，还会用最初约 2 s 的平均加速度重复更新：2000 次让初始四元数先收敛到正确的重力方向。

9. 磁力计为什么没有进入当前姿态估计？

虽然 CSV 中保存了三轴磁场数据，但主程序调用的是：

UpdateIMU(Gyroscope, Accelerometer)

而不是包含磁力计的完整 AHRS 更新。

实际上 AHRS.m 中的三传感器 Update 方法也没有实现。

所以当前姿态估计只有：陀螺仪 + 加速度计

这意味着：

横滚和俯仰可以在静止阶段利用重力方向校正；

航向角没有磁北等绝对方向约束，主要依赖陀螺仪积分。

这也是理解长距离二维轨迹方向误差时必须知道的一点。

10. 怎样把脚上的加速度转换到地球坐标系？

程序利用四元数旋转：

a_E = q ⊗ [0, a_B] ⊗ q*

其中：

a_B：IMU 本体坐标系加速度；

a_E：地球坐标系加速度；

q / q*：姿态四元数及其共轭。

转换完成后，再把单位：g乘以：9.81得到：m/s²

最后从 z 轴减去重力：a_z ← a_z - 9.81于是剩下的才主要是脚在地球坐标系中的平移加速度。

11. 为什么不能直接把加速度一直积分？

最简单的速度计算是：v(k) = v(k-1) + a(k)Δt

但哪怕只有一点点：

传感器零偏；姿态误差；重力残差；

连续积分后速度都会慢慢偏离真实值。

所以程序每次检测到：脚处于静止

就直接强制：v = [0, 0, 0]

这就是：ZUPT，Zero Velocity Update，零速更新。

对于脚载 IMU 来说，这个约束非常自然，因为脚掌落地的静止阶段真实速度确实接近 0。

12. 仅仅把落地速度设成 0 为什么还不够？

假设一次摆腿持续 N 个采样点。

由于积分误差，程序可能在下一次落地前得到：v_end ≠ 0

虽然落地时强制清零，但这段摆腿期间已经积累了一条错误速度曲线。

程序进一步假设：

一次非静止区间中的积分漂移近似线性增长。

所以先计算：driftRate = v_end / N

然后构造：v_drift(i) = i · driftRate

最后修正：v_corrected = v_integrated - v_drift

这样一次摆动区间的速度误差就从开头的 0 逐渐补偿到结尾的 v_end。

这一步是程序能够重建较稳定步态轨迹的核心。

13. 最终位置怎样得到？

完成：姿态修正 → 去重力 → 加速度积分 → ZUPT → 线性速度漂移补偿

之后，再对速度做一次积分：p(k) = p(k-1) + v(k)Δt

得到：pos = [x, y, z]

三列位置。因此这里是典型的双重积分：加速度 → 速度 → 位置

但和最简单的双积分不同，程序在中间加入了足部静止约束和每一步的速度漂移修正。

14. 默认直线行走数据能得到什么结果？

对源码默认：straightLine，6～26 s

按照程序公式和同一组 CSV 数据做等价数值复算：

样本数：5121；

分析时长：20 s；

检测到约 13 个非静止步态区间；

最终相对位置约为：[18.72, 0.74, 0.12] m

其中主要位移集中在 x 方向。也就是说，这组默认数据被重建成：

约 18.7 m 的主要直线前进行走，同时存在较小的横向和垂向残差。

这里的位置是从：[0,0,0]开始积分得到的相对轨迹，并不是 GPS 绝对坐标。

15. 楼梯与走廊数据体现了什么？

将源码切换到：stairsAndCorridor，5～53 s

共有约：12289 个采样点

按同样算法复算，末端相对位置约为：[26.76, -5.53, 3.00] m

其中 z 方向最终约：+3.0 m说明三维积分不仅能够表示平面前进和转弯，也能够反映：

上下楼过程中产生的垂向高度变化。

这也是三维惯性轨迹相对于单纯“计步 + 步长估计”的优势。

16. 六自由度动画在展示什么？

位置计算完成以后，程序把：pos和：quat → 旋转矩阵一起送入：SixDofAnimation

因此动画中的每一个时刻同时包含：

IMU 在哪里；

IMU 朝哪个方向。

程序还保留整条运动轨迹，并让观察视角在动画过程中旋转约：120°

所以最终展示的不是单独一条二维折线，而是：

带实时脚部姿态方向的三维 6-DOF 运动轨迹。

动画末尾还额外重复最终姿态约：20 s让最后结果不会立即消失。

17. 为什么这种方法特别适合“脚载 IMU”？

优势一：步态天然提供周期性零速条件

如果 IMU 安装在腰部，走路时传感器几乎一直在动，很难知道真实速度什么时候严格为 0。

但脚掌每一步都会落地，所以 ZUPT 约束非常强。

优势二：不依赖摄像机和外部定位基站

只要有脚上的陀螺仪和加速度计，就能离线重建轨迹。

优势三：姿态和位置在同一条链上计算

四元数不仅用于画姿态，还决定如何正确地把加速度从脚部坐标系变换到地球坐标系。

所以姿态估计和位置跟踪并不是两个独立模块，而是前后依赖的。

18. 程序最终能得到什么？

运行主脚本后，最重要的结果变量包括：

stationary：每个采样点是否静止；

quat：每个采样点的姿态四元数；

acc：地球坐标系去重力后的加速度；

vel：ZUPT 与漂移修正后的三轴速度；

pos：最终三维相对位置。

整个程序的核心输出可以总结为：

传感器原始惯性数据 → 脚部姿态 + 三维速度 + 三维步行轨迹

最后还可以通过 SixDofAnimation 动态查看完整六自由度运动。

19. 怎么运行？

进入：Gait Tracking With x-IMU

目录后，直接运行：Script.m

默认会分析：Datasets/straightLine

如果要换数据，只需要在脚本开头切换 filePath、startTime 和 stopTime。

程序需要 Signal Processing Toolbox 中的 butter、filtfilt，并依赖工程自带的：Quaternions + ximu_matlab_library

切换到源码预设的 spiralStairs 时，当前 0.05 静止阈值在 4～47 s 片段开头没有先检测到静止状态，会造成漂移补偿中的静止起止点数量不配对；要运行这组数据，需要先让选取区间从有效静止段开始，或在漂移补偿循环中处理首尾未配对区间。

20. 一句话看懂这个项目

这是一个 足部 x-IMU 三维步态轨迹重建程序：采样频率为 256 Hz，先对三轴加速度模执行 0.001 Hz 高通、取绝对值和 5 Hz 低通，并以 0.05 阈值检测脚掌静止；四元数 AHRS 在静止时使用 Kp=0.5 的重力方向反馈修正姿态、运动时主要依靠陀螺仪积分，再把机体系加速度旋转到地球坐标系并减去 9.81 m/s² 重力。随后通过 v(k)=v(k-1)+a(k)Δt 积分速度，在落地时强制 v=0，并对每个摆动区间线性补偿积分漂移，最后再次积分得到三维位置。默认 straightLine 的 6～26 s 共 5121 点，按源码等价复算末端相对位置约为 [18.72, 0.74, 0.12] m；stairsAndCorridor 的 5～53 s 末端约为 [26.76, -5.53, 3.00] m。