UWB自动跟随与IMU、激光雷达等多传感器融合的定位方案

PSICV博赛智行，让机器人和智能出行设备“闭眼”也能找到你

1. 为什么要多传感器融合？

如果你只用过 UWB 跟随，可能会有这样的体验：在空旷环境里，厘米级精度简直让人惊叹，但一旦进入金属货架区、地下车库、或者人群密集的展会现场，定位精度会明显下降，甚至会出现“瞬移”或“走神”。 原因很简单：UWB 虽然抗干扰能力强，但它仍会受到多径效应、非视距（NLOS）和设备自身姿态变化的影响。

而IMU（惯性测量单元）、激光雷达（LiDAR）、甚至视觉（Camera）可以提供额外的环境感知和姿态信息，帮助系统在UWB数据不稳定时“顶上去”，让跟随设备依然保持流畅和可靠。

这就像人走路时，既依赖眼睛（视觉/UWB）看路，也依赖内耳前庭系统（IMU）感知姿态，还会用手电筒（LiDAR）照亮黑暗区域，多重感官一起工作才不容易摔倒。

2. 三种典型定位传感器的优缺点

要做到稳定跟随，理想做法是——UWB 提供全局定位，IMU 负责高频姿态修正，LiDAR 用于环境障碍检测和辅助定位。

3. 多传感器融合的核心数学

最常见的融合算法是扩展卡尔曼滤波（EKF），它把 UWB、IMU、LiDAR 的数据按各自的可信度融合成一个统一的状态估计。

状态向量可以写成：

其中 x,y,z是位置，Vx,Vy,Vz是速度，θ,φ,ψ是姿态角（滚转、俯仰、偏航）。

状态预测公式（由 IMU 加速度积分得到）：

Xk∣k−1​=F⋅Xk−1∣k−1​+B⋅uk​+wk

这里 F 是状态转移矩阵，B是输入矩阵，uk是 IMU 测得的加速度与角速度，wk是过程噪声。

观测更新（由 UWB 或 LiDAR 得到）：

Zk=H⋅Xk∣k−1+vk

H是观测矩阵，vk是测量噪声。

融合时，系统会根据每种传感器的噪声协方差 R 自动调整权重——UWB 信号差时，IMU 的权重会上升；IMU 漂移变大时，LiDAR 权重会接管。

4. 实际案例：自动跟随行李箱

我们曾帮一家行李箱公司做过测试：

单纯用 UWB，在机场人流密集处，行李箱会偶尔跑偏 20cm 左右；

加了 IMU，短时抖动减少了，但长时间排队时会出现漂移；

再加了激光雷达，行李箱能识别障碍并修正漂移，跟随路线更自然。

最终方案是——UWB 全局定位 + IMU 高频姿态 + 激光雷达障碍检测。在实际测试中，跟随误差降到了 5cm 以内，行李箱几乎像长了眼睛。

5. 融合中的几个“坑”

时间同步 传感器数据采样率不同，UWB 可能 10Hz，IMU 可能 200Hz，LiDAR 可能 5Hz。不同步会造成 EKF 发疯，所以必须用时间戳插值对齐。

坐标系对齐 UWB、IMU、LiDAR 的坐标原点和方向可能不同（比如一个 X 轴朝前，一个朝上），必须通过外参标定才能融合。

漂移与失锁 IMU 漂移会在长时间 UWB 信号缺失时被放大；LiDAR 在光滑或透明地面会失效，这些都需要融合策略做异常检测。

6. 未来趋势

我认为未来消费级 UWB 跟随设备大概率会走多传感器轻量化路线——LiDAR 可能换成低成本固态雷达，IMU 继续用 MEMS 方案，配合小型 AI 模块做动态权重调整，这样既能保证精度，又能控制成本。

在无人车、智能轮椅、跟随机器人等场景，多传感器融合已经不是“锦上添花”，而是“必备保险”。毕竟没人希望自己的跟随机器人在人群中“走丢”。