【项目实战】基于 STM32 + MPU6050 的微型惯性导航系统（轨迹跟踪/零速修正/姿态解算）
前言
在机器人定位、动作捕捉和穿戴设备中，惯性导航系统（INS）扮演着重要角色。虽然低成本的 MEMS 惯性测量单元（IMU）如 MPU6050 存在较大的噪声和温漂，但通过合理的算法（如互补滤波、零速修正 ZUPT），我们依然可以在短时间内实现相对精准的轨迹跟踪。
本文将分享一个基于 STM32F103 和 MPU6050 的微型惯导项目，实现了从原始数据采集、姿态解算（AHRS）、捷联惯导解算（SINS）到 MATLAB 上位机轨迹重构的完整技术链路。
1. 系统架构与硬件方案
本系统采用“端侧实时解算 + 上位机后处理验证”的架构：
-
主控芯片：STM32F103C8T6（标准 HAL 库开发）
-
传感器：MPU6050（6 轴 IMU：3 轴陀螺仪 + 3 轴加速度计）
-
通信接口：I2C（传感器读取）+ UART（数据回传）
-
上位机：MATLAB（数据分析、线性去漂移、3D 轨迹绘制）
2. 核心算法原理
惯性导航的核心在于通过积分将加速度转化为速度和位移，但面临两大挑战：姿态误差和积分漂移。本项目采用了以下解决方案：
2.1 姿态解算 (AHRS)
采用 基于四元数的互补滤波算法（类 Mahony/Madgwick 算法）。
-
优势：避免了欧拉角的“万向节死锁”问题，计算效率优于卡尔曼滤波。
-
逻辑：
-
利用加速度计（在静止/匀速时指向重力方向）修正陀螺仪的积分漂移。
-
引入自适应增益：当检测到剧烈运动（加速度模长大幅偏离 1g）时，降低加速度计权重，更信任陀螺仪。
-
2.2 捷联惯导解算 (SINS)
将机体坐标系（Body Frame）下的加速度转换到世界坐标系（World Frame），并去除重力分量。
其中 R 是由四元数构建的旋转矩阵。
2.3 零速修正 (ZUPT) 与动态零偏估计
这是低成本传感器抑制漂移的关键。
-
静止检测：实时计算加速度和角速度的方差/模长，判断设备是否静止。
-
动态零偏 (Dynamic Bias)：当判定为静止时，理论上世界系加速度应为 0。此时将残留的加速度读数视为“零偏”，并通过低通滤波动态更新到 Bias 变量中，从而在运动阶段也能扣除该底噪。
3. 嵌入式软件实现 (STM32)
3.1 核心数据结构
我们在 MPU6050 结构体中扩展了轨迹计算所需的变量：
// Core/Inc/MPU6050.h
typedef struct MPU6050 {
// … 原始数据及姿态角 …
float q0, q1, q2, q3; // 四元数
// 轨迹计算扩展变量
float ax_world, ay_world, az_world; // 世界坐标系加速度
float vx, vy, vz; // 速度
float px, py, pz; // 位置
// 动态误差修正
float bias_ax, bias_ay, bias_az; // 动态估算的加速度零偏
uint8_t is_static; // 静止标志位
} MPU6050;
3.2 姿态更新与加速度旋转
在定时器中断中调用核心解算函数，确保采样周期（dt）稳定：
// Core/Src/MPU6050.c
void MPU6050_Update_Trajectory(MPU6050 *this) {
// 1. 利用四元数将机体加速度旋转到世界坐标系
// (此处省略了根据 q0-q3 构建旋转矩阵的代码，详见源码)
this->ax_world = …;
this->ay_world = …;
this->az_world = …;
// 2. 移除重力分量 (假设 Z 轴垂直向上为正)
this->az_world -= 1.0f;
// 3. 静止检测 (ZUPT)
float acc_norm = sqrtf(ax_body*ax_body + …);
if (fabsf(acc_norm – 1.0f) < ZUPT_ACC_THRES) {
this->is_static = 1;
}
// 4. 动态零偏学习 (关键步骤)
if (this->is_static) {
// 静止时，当前读数即为误差，慢慢学习它
this->bias_ax += (this->ax_world – 0.0f) * BIAS_LEARN_RATE;
this->bias_ay += (this->ay_world – 0.0f) * BIAS_LEARN_RATE;
// 强制速度归零
this->vx = 0; this->vy = 0;
}
// 5. 积分与阻尼 (Leaky Integrator)
if (!this->is_static) {
float real_ax = (this->ax_world – this->bias_ax) * GRAVITY_MSS;
// 引入 VEL_DAMPING (如 0.985) 抑制高频漂移
this->vx = (this->vx * VEL_DAMPING) + real_ax * mpu6050_dt;
this->px += this->vx * mpu6050_dt;
}
}
3.3 启动校准
在上电初期，必须进行一次静态校准以获取初始的加速度零偏：
// Core/Src/main.c
// 采样 400 次，耗时约 2 秒，期间保持静止
MPU6050_Calibrate_Accel_World(&MM, 400);
printf("Calibration Done. Bias: %.3f, %.3f, %.3f\\r\\n", MM.bias_ax, MM.bias_ay, MM.bias_az);
4. MATLAB 上位机验证与仿真
STM32 端由于算力限制，主要做实时的阻尼处理。为了获得更平滑的轨迹，我们将原始数据导出到 MATLAB 进行非因果（Non-causal）处理。
4.1 数据流设计
STM32 串口发送格式：Time, Roll, Pitch, Yaw, Ax_World, Ay_World, Az_World。
4.2 全局线性去漂移 (Linear Drift Removal)
在 MATLAB 中，我们不再简单地积分，而是利用整个时间段的数据来消除漂移。原理是：如果在 t_1 和 t_2 时刻设备都是静止的，那么这两个时刻的速度理论上都应为 0。如果积分结果不为 0，说明存在线性漂移。
MATLAB 核心代码逻辑：
Matlab
% Matlab_MPU6050运动跟踪/get_drift.m
function drift_data = get_drift(data, stationary, time)
% 找到所有的静止区间
drift_start = find(diff(stationary) == -1);
drift_end = find(diff(stationary) == 1);
% 计算两个静止点之间的速度误差斜率
for i = 1:loop_len
vel_end = data(tf+1, :); % 本该为0的结束速度
tg = vel_end / (time(tf) – time(ti)); % 漂移斜率
% … 从整段速度中减去这个斜率 …
end
end
4.3 效果对比
-
纯积分：轨迹会在几秒内迅速发散，“飞”向远方。
-
STM32 端 ZUPT：能有效抑制发散，但在长时间运动后仍有累积误差。
-
MATLAB 全局去漂移：利用未来的数据修正过去的历史，能得到闭合性极好的轨迹（如画圆、写字）。
5. 总结与优化方向
本项目通过 STM32 HAL 库 实现了高频的 IMU 数据采集，并结合 四元数解算 和 零速修正算法，在低成本硬件上实现了基本的惯性导航功能。
遇到的坑与经验：
采样率至关重要：使用定时器中断保证 dt 的恒定，不稳定的 dt 会直接导致积分爆炸。
加速度计校准：上电时的初始重力方向校准必须非常精确，否则去除重力后会有巨大的残余分量。
高通滤波：简单的积分必然漂移，必须引入“漏积分”（Leaky Integrator）或高通滤波器来衰减直流误差。
未来优化：
-
加入磁力计（HMC5883L/QMC5883L）修正 Yaw 角漂移（9轴融合）。
-
引入卡尔曼滤波（EKF）融合位置观测数据（如 UWB 或视觉里程计）。
附录：
-
代码环境：STM32CubeIDE + MATLAB 2024b
-
硬件连线：VCC-5V, GND-GND, SCL-PB6, SDA-PB7
-
百度网盘链接：通过网盘分享的文件：MPU6050轨迹
链接: https://pan.baidu.com/s/1NqEGIvqbxdQPwuf8GvjhoQ?pwd=5678 提取码: 5678
（代码HAL部分只上传了core部分，matlab完整上传）
