1. 项目概述：UWB与IMU紧耦合定位系统
在室内定位领域，厘米级精度的实时位置追踪一直是技术难点。这个项目通过融合超宽带（UWB）的到达时间（TOA）测距数据和惯性测量单元（IMU）的运动感知数据，实现了二维平面内目标物体的高精度轨迹定位。我在工业AGV导航项目中实测发现，单纯UWB定位在遮挡环境下误差可达30cm以上，而加入IMU数据融合后能将误差控制在8cm以内。
系统核心包含三个技术层：UWB基站网络通过TOA算法计算原始距离数据，IMU模块提供加速度和角速度信息，紧耦合卡尔曼滤波器将两类传感器数据进行时空对齐和最优估计。MATLAB实现方案特别适合算法验证阶段，可以快速调整滤波器参数和评估不同运动模型效果。
关键优势：在UWB信号被遮挡时，IMU的短时高精度特性可以维持定位连续性；当IMU出现累积误差时，UWB的绝对位置信息又能进行校正。这种互补性使系统在复杂室内环境中表现稳定。
2. 核心原理与技术选型
2.1 UWB TOA定位原理
TOA(Time of Arrival)测距基于信号飞行时间计算，其距离公式为：
code复制d = c × (t_r - t_t)
其中c为光速(3×10^8 m/s)，t_r和t_t分别是接收和发送时间戳。四个以上基站构成定位网络时，通过最小二乘法解算目标坐标：
matlab复制% TOA定位最小二乘解算示例
A = [2*(x2-x1), 2*(y2-y1);
2*(x3-x1), 2*(y3-y1)];
b = [d1^2 - d2^2 + x2^2 - x1^2 + y2^2 - y1^2;
d1^2 - d3^2 + x3^2 - x1^2 + y3^2 - y1^2];
pos = (A'*A)\A'*b; % 目标位置估计
实测中需要校准天线延迟（通常0.5-2ns），并采用双边双向测距(DS-TWR)消除时钟偏差。Decawave DW1000芯片的TOA测距精度在视距环境下可达±10cm。
2.2 IMU运动模型构建
IMU数据积分得到位置的过程存在明显的误差累积。以MPU9250为例，其加速度计噪声密度约200μg/√Hz，短时间内（<3秒）相对位移计算较准确。采用四元数姿态解算可避免欧拉角奇异值问题：
matlab复制% 四元数更新姿态
q = [1 0 0 0]; % 初始四元数
gyro = [wx, wy, wz] * dt; % 角增量
q = q + 0.5 * quatmultiply(q, [0, gyro]);
q = q / norm(q); % 归一化
2.3 紧耦合卡尔曼滤波设计
不同于松耦合（位置级融合），紧耦合直接在原始观测层进行数据融合。状态向量包含：
code复制X = [x, y, vx, vy, ax, ay, θ, ω]'
观测模型同时处理UWB距离测量和IMU加速度/角速度。关键参数包括：
- 过程噪声Q：IMU误差特性决定
- 观测噪声R：UWB测距方差约0.01m²，IMU加速度噪声约0.1m²/s⁴
扩展卡尔曼滤波(EKF)的预测和更新流程如下：
matlab复制% EKF预测步骤
F = buildF(dt); % 状态转移矩阵
X = F * X;
P = F * P * F' + Q;
% 更新步骤（UWB观测）
H = buildH_TOA(anchor_pos);
K = P * H' / (H * P * H' + R);
X = X + K * (z - h(X));
P = (eye(8) - K*H) * P;
3. 系统实现与MATLAB代码解析
3.1 硬件配置方案
典型测试环境需要：
- 4个UWB基站（建议部署成10m×10m正方形）
- 1个移动标签（集成UWB模块和IMU）
- 主机运行MATLAB处理数据
基站坐标标定误差应小于5cm，建议使用全站仪测量。IMU安装需与运动方向对齐，否则需要标定安装偏角。
3.2 数据同步处理
硬件时间同步方案：
- UWB基站通过有线触发同步时钟
- IMU数据通过硬件中断与UWB数据对齐
- 软件层采用插值补偿剩余微小偏差
MATLAB同步处理代码片段：
matlab复制% 时间对齐处理
imu_time = 0:0.01:10; % IMU 100Hz
uwb_time = 0:0.1:10; % UWB 10Hz
uwb_sync = interp1(uwb_time, uwb_data, imu_time, 'spline');
3.3 核心算法实现
完整定位流程包含：
- 数据预处理（野值剔除、零偏校准）
- IMU机械编排（姿态解算、速度位置积分）
- EKF融合定位
- 误差统计与可视化
关键MATLAB函数说明：
initEKF(): 初始化滤波器参数predictIMU(): 基于IMU的状态预测updateUWB(): TOA观测更新trajPlot(): 绘制真实/估计轨迹
实测技巧：在滤波器收敛前（约前3秒），可暂时增大过程噪声协方差Q，加快初始定位速度。
4. 性能评估与调优
4.1 误差统计方法
定位精度评估指标：
- 绝对位置误差：RMSE
- 相对轨迹误差：Frechet距离
- 连续性指标：遮挡恢复时间
MATLAB统计实现：
matlab复制err = sqrt((x_est - x_gt).^2 + (y_est - y_gt).^2);
rmse = sqrt(mean(err.^2));
4.2 典型场景测试
测试案例设计：
- 直线匀速运动（验证IMU动态性能）
- 急转弯测试（验证角速度融合效果）
- 基站遮挡测试（验证鲁棒性）
某次实测数据对比：
| 场景 | 纯UWB误差 | 融合后误差 |
|---|---|---|
| 开阔区域 | 12cm | 5cm |
| 单基站遮挡 | 38cm | 9cm |
| 快速转向 | 22cm | 7cm |
4.3 参数调优指南
关键调优参数及影响：
-
Q矩阵中的IMU噪声参数：
- 增大：提高IMU权重，响应更快但噪声大
- 减小：更依赖UWB，稳定但延迟增加
-
R矩阵中的UWB方差：
- 信号差时应适当增大
- 视距环境下取芯片标称值
-
运动模型选择：
- 匀速模型：适合低速平稳移动
- 匀加速模型：适合AGV等有启停场景
调试时建议保存原始数据，用tic/toc记录各模块耗时，优先优化计算量大的部分（如四元数运算）。
5. 常见问题与解决方案
5.1 定位发散问题
现象：误差随时间持续增大
可能原因：
- IMU零偏未校准 → 重新进行静态校准
- 基站坐标误差大 → 用全站仪重新标定
- 滤波器参数不合理 → 调整Q/R矩阵
5.2 遮挡恢复延迟
优化方案：
- 增加IMU预测权重
- 引入运动约束（如地面机器人z轴固定）
- 使用多模型滤波(IMM)
代码调整示例：
matlab复制function Q = adaptQ(uwb_valid)
if sum(uwb_valid) < 2 % 少于2个基站
Q(1:2,1:2) = eye(2)*0.1; % 增大位置噪声
end
end
5.3 计算资源优化
加速技巧：
- 预计算雅可比矩阵
- 使用C-MEX编写核心算法
- 降低IMU输出频率（不低于50Hz）
内存管理建议：
matlab复制% 避免在循环中动态扩展数组
prealloc = zeros(10000,8);
for k = 1:10000
prealloc(k,:) = X';
end
6. 工程应用扩展
在实际部署中发现几个实用技巧：
- 基站高度差异能改善z轴可观测性
- 标签运动时天线方向图变化需补偿
- 金属环境下的UWB多径抑制方法：
- 增加FIR滤波器
- 使用多天线分集
对于需要更高精度的场景，可以考虑：
- 加入轮速里程计信息
- 使用因子图优化代替EKF
- 引入深度学习误差补偿
MATLAB代码已封装成可配置的定位引擎，通过修改config.m文件即可适配不同硬件平台。在树莓派上实测运行时间小于5ms/帧，完全满足实时性要求。