1. 项目背景与核心价值
在室内机器人导航领域,精确的定位与环境建图一直是技术难点。传统基于激光雷达的SLAM方案虽然精度较高,但设备成本昂贵且对透明/反光物体敏感。而UWB(超宽带)雷达凭借其穿透性强、抗多径效应好的特点,正成为室内定位的新选择。这个项目展示了如何用扩展卡尔曼滤波器(EKF)融合UWB测距数据,实现低成本、高鲁棒性的SLAM系统。
我曾在工业AGV项目中验证过这套方案的实际效果——在30m×50m的仓库环境中,仅需4个UWB锚点就能实现±15cm的定位精度。相比动辄上万元的激光方案,千元级的UWB硬件显然更适合大规模部署。但UWB的原始测距数据噪声较大(特别是动态环境下),这就需要EKF这样的状态估计算法来提升数据质量。
2. 技术方案设计解析
2.1 系统架构设计
整套系统包含三个核心模块:
- 前端传感器层:UWB雷达模块(如DW1000芯片组)负责与环境中部署的锚点通信,获取TOF(飞行时间)测距数据
- 数据预处理层:对原始测距值进行野值剔除、滑动平均滤波
- EKF-SLAM核心层:
- 预测阶段:通过里程计模型估计机器人位姿变化
- 更新阶段:用UWB观测数据修正位姿估计
- 地标管理:动态维护自然特征点的位置估计
关键设计选择:之所以采用EKF而非粒子滤波(PF),是因为UWB的观测噪声相对符合高斯分布,且EKF的计算效率更适合嵌入式平台部署。实测在树莓派4B上,单次滤波耗时仅1.2ms。
2.2 状态向量定义
状态向量包含机器人位姿和地标位置:
code复制X = [x_r, y_r, θ_r, x_l1, y_l1, ..., x_ln, y_ln]^T
其中(x_r,y_r)为机器人坐标,θ_r为朝向角,(x_li,y_li)为第i个地标坐标。在Matlab实现中,这个向量会随着新地标的发现动态扩展。
2.3 运动模型与观测模型
运动模型采用差分驱动模型:
code复制x_k+1 = x_k + Δs*cos(θ_k + Δθ/2)
y_k+1 = y_k + Δs*sin(θ_k + Δθ/2)
θ_k+1 = θ_k + Δθ
其中Δs为位移增量,Δθ为转向增量。
观测模型基于UWB测距原理:
code复制z = sqrt((x_r - x_li)^2 + (y_r - y_li)^2) + v
v为服从N(0,R)的高斯噪声,R需通过传感器标定确定。
3. Matlab实现关键代码解析
3.1 初始化设置
matlab复制% 系统噪声协方差矩阵
Q = diag([0.1, 0.1, 0.05]); % 对应x,y,θ的噪声方差
% 观测噪声协方差
R = 0.3; % UWB测距方差(m^2)
% 初始状态向量
state = [0; 0; 0]; % [x; y; θ]
covariance = eye(3)*0.01; % 初始协方差矩阵
% 地标数据库
landmarks = struct('pos',[], 'cov',[], 'id',[]);
3.2 EKF预测步骤
matlab复制function [state, covariance] = prediction(state, covariance, u, Q)
% u = [Δs; Δθ] 控制输入
theta = state(3);
% 计算雅可比矩阵
F = [1 0 -u(1)*sin(theta + u(2)/2);
0 1 u(1)*cos(theta + u(2)/2);
0 0 1];
% 状态预测
state(1) = state(1) + u(1)*cos(theta + u(2)/2);
state(2) = state(2) + u(1)*sin(theta + u(2)/2);
state(3) = state(3) + u(2);
% 协方差预测
covariance = F * covariance * F' + Q;
end
3.3 数据关联与更新
matlab复制function [state, covariance, landmarks] = update(state, covariance, landmarks, z, id, R)
% z为观测距离,id为地标ID
% 查找已有地标
idx = find([landmarks.id] == id);
if isempty(idx) % 新地标
% 初始化地标位置
lx = state(1) + z*cos(state(3));
ly = state(2) + z*sin(state(3));
% 扩展状态向量
state = [state; lx; ly];
% 扩展协方差矩阵
dim = size(covariance,1);
covariance = [covariance, zeros(dim,2);
zeros(2,dim), eye(2)*100]; % 新地标初始方差设大
% 添加到数据库
landmarks(end+1).pos = [lx; ly];
landmarks(end).cov = eye(2)*100;
landmarks(end).id = id;
idx = length(landmarks);
end
% 计算观测雅可比
lx = state(2*dim+1); ly = state(2*dim+2);
dx = lx - state(1); dy = ly - state(2);
q = dx^2 + dy^2;
H = [-dx/sqrt(q), -dy/sqrt(q), 0, dx/sqrt(q), dy/sqrt(q)];
% 卡尔曼增益
K = covariance * H' / (H * covariance * H' + R);
% 状态更新
innovation = z - sqrt(q);
state = state + K * innovation;
% 协方差更新
covariance = (eye(size(covariance)) - K*H) * covariance;
end
4. 实测效果与调优经验
4.1 典型测试场景
在10m×10m的室内环境部署4个UWB锚点,机器人沿矩形路径运动。对比纯航迹推算(DR)和EKF-SLAM的定位误差:
| 方法 | 平均误差(m) | 最大误差(m) |
|---|---|---|
| DR | 0.82 | 2.15 |
| EKF | 0.18 | 0.35 |
4.2 参数调优技巧
-
Q矩阵设置:建议先用实测算得里程计误差统计量。一个经验公式:
matlab复制Q = diag([0.05*Δs, 0.05*Δs, 0.1*Δθ]); -
数据关联阈值:新观测与已有地标的马氏距离应小于阈值:
matlab复制d_mahalanobis = innovation^2 / (H*P*H' + R); if d_mahalanobis > 9 % 3σ阈值 % 视为新地标 end -
地标管理:定期清理长时间未观测到的地标,避免状态维度过大:
matlab复制for i = 1:length(landmarks) if current_step - landmarks(i).last_seen > 50 % 从状态向量中移除该地标 end end
5. 常见问题排查
5.1 定位发散问题
现象:误差随时间持续增大
排查步骤:
- 检查UWB锚点坐标输入是否正确
- 验证运动模型雅可比矩阵的计算
- 降低Q矩阵中的过程噪声参数
5.2 地标误关联
现象:同一物理地标被重复创建
解决方案:
matlab复制% 在数据关联阶段增加NNSF(Nearest Neighbor Standard Filter)
if min_mahalanobis_dist < threshold && min_mahalanobis_dist < 0.8*second_min_dist
% 确认为同一地标
else
% 视为新地标
end
5.3 实时性不足
优化方案:
- 限制状态向量中地标数量(如仅保留最近20个活跃地标)
- 使用稀疏矩阵运算加速协方差更新
- 将EKF更新频率降至10Hz(UWB原始数据率通常为100Hz)
这套系统我在多个室内服务机器人项目上实际部署过,最大的体会是:UWB锚点的部署高度建议在2.5m以上,避免被移动物体遮挡;同时尽量保证每个位置至少能收到3个锚点信号。Matlab原型验证通过后,可以用C++重写核心算法部署到ROS中,这时需要注意EKF的数值稳定性问题——我通常会加入矩阵条件数检查,当covariance矩阵病态时触发重置逻辑。