在现代自动跟随系统的研发中，单一传感器技术已难以满足复杂环境下的高精度定位需求。UWB（Ultra-Wideband，超宽带）技术凭借厘米级定位能力，已成为主流的核心定位手段之一。然而，在实际应用中，复杂环境下的遮挡、多径效应及动态场景变化，可能导致 UWB 定位精度下降。为了应对这些挑战，我们采用了UWB 与 IMU、激光雷达等多传感器融合的定位方案，有效提升系统的精度、稳定性与鲁棒性。

1 方案概述

本方案将 UWB 高精度测距作为主定位源，同时引入：

• IMU（惯性测量单元）：提供高速姿态、加速度与角速度信息，实现短时高频位置推算；
• 激光雷达（LiDAR）：进行环境建图（SLAM）与障碍物检测，辅助定位与路径规划；
• 融合算法：采用扩展卡尔曼滤波（EKF）或因子图优化（Factor Graph Optimization）实现多源数据融合，兼顾实时性与精度。

2 核心技术原理

2.1 UWB 定位原理

UWB 定位可采用多种测距算法：

2.1.1 TOF（Time of Flight）

通过测量无线信号传播时间计算距离：

d=c×t

 

其中 c 为光速，t为信号单程传播时间。

2.1.2 TDOA（Time Difference of Arrival）

利用多个基站的到达时间差计算目标位置，适合异步系统。

2.1.3 PDOA（Phase Difference of Arrival）

通过信号相位差推算距离差，适合短距离高精度场景。

在三边定位模型中，假设已知三个基站坐标 (x_i,y_i)和测得的距离 d_i，目标位置 (x,y)可由以下方程组求解：

 

(x-x₁)²+(y-y₁)²=d₁²

(x-x₂)²+(y-y₂)²=d₂²

(x-x₃)²+(y-y₃)²=d₃²

 

2.2 IMU位置推算

IMU 提供高频加速度 a 和角速度 ω，可通过积分得到速度与位置变化量：

 

 

IMU 在短时内精度高，但存在累积漂移问题，因此需与 UWB 互补：UWB 提供全局位置校正，IMU 提供高频更新。

2.3 激光雷达辅助

激光雷达可通过SLAM（同时定位与建图）获得局部地图和相对位置坐标，常用算法包括 GMapping、Hector SLAM、Cartographer。在 GPS 或 UWB 信号弱化的区域，LiDAR 提供相对位置修正。

3 融合算法设计

在本方案中，融合框架以 扩展卡尔曼滤波（EKF）为核心，该方法可在复杂场景下将定位误差控制在 ±5 cm 内，同时显著提高姿态估计精度。

在 IMU 数据融合 中，常用的公式主要分为两个部分：

• IMU原始数据到位姿变化的计算公式
• 与其他传感器融合的数学模型（如卡尔曼滤波）

3.1 IMU原始数据计算公式

IMU 的核心数据来自 三轴加速度计（a_x,a_y,a_z）、三轴陀螺仪（ω_x,ω_y,ω_z）和有时包含的三轴磁力计（m_x,m_y,m_z）。

它们提供的原始信号需要经过积分和坐标系变换，才能得到位姿。

3.1.1 姿态更新（陀螺仪积分）

陀螺仪给出的是角速度，单位一般是 rad/s，姿态（四元数）更新公式为：

 

 

其中：

q = 四元数 [q_w,q_x,q_y,q_z]

ω=[0,ω_x,ω_y,ω_z]

⊗ 表示四元数乘法

Δt = 采样时间间隔

更新后通常需要归一化：

 

3.1.2 加速度积分计算位移

加速度计测到的是线加速度（包含重力），先去掉重力分量：

1. 将四元数转换为旋转矩阵 R，再将加速度投影到全局坐标系：

其中 g=[0,0,9.81]m/s²

2. 两次积分得到位移：

 

这个方法容易出现漂移，因为积分会放大传感器噪声，所以单独使用IMU计算位移不可靠，必须融合其他传感器。

3.2 IMU与其他传感器融合（以扩展卡尔曼滤波EKF为例）

EKF 是机器人和无人机中最常用的 IMU 融合算法，用于将 IMU 与 UWB / GPS / 激光雷达 等传感器融合。

3.2.1 状态方程

定义状态向量：

其中：

p = 位置

v= 速度

q = 四元数姿态

b_a = 加速度计零偏

b_ω = 陀螺仪零偏

预测模型：

其中控制输入 u_t = IMU 测量的加速度 & 角速度，w_t = 过程噪声。

3.2.2 测量方程

当有 UWB / 激光雷达数据时，测量模型为：

其中：

• 对于 UWB 三边测距：

• 对于激光雷达：

 

3.2.3 EKF 更新公式

预测步骤：

 

更新步骤：

 

 

4 系统架构

1. 定位模块：UWB 基站 + 移动端标签；

2. 惯性模块：9 轴 IMU（加速度计 + 陀螺仪 + 磁力计）；

3. 环境感知模块：2D/3D 激光雷达；

4. 融合计算模块：嵌入式高性能计算平台（如 NVIDIA Jetson 或 RK3588）；

5. 通信模块：低延迟无线传输（ BLE 或专用 UWB 通道）。

5 应用价值

• 智能出行：Soffofel随福自动跟随电动代步车、智能行李箱，实现室内外连续跟随；
• 仓储物流：PSICV 随辅 自动搬运机器人在高货架遮挡环境中仍保持精确导航；
• 工业巡检：在金属设备密集区域维持厘米级定位精度；
• 消费电子：运动相机、智能穿戴设备的实时位置跟踪。

6 实际部署注意事项

• UWB 基站布置需避免金属大面积反射区，建议采用高低错落布置减少多径干扰；
• IMU 标定需在部署前完成，确保姿态解算精度；
• LiDAR 与 UWB 时间同步是融合算法性能的关键，可采用硬件触发或高精度时钟同步（PTP）；
• 滤波器参数调优需结合具体场景反复测试。

 

通过多传感器融合方案，PSICV博赛智行成功实现了在复杂环境中依然保持高精度、低延迟的自动跟随能力，为智能出行、工业自动化及消费电子等领域提供了可落地的技术支撑。