★ FEATURED ARTICLE 去年我带着一个室内巡检机器人项目从激光雷达方案换到UWB雷达折腾了三四个月最终把整套EKF SLAM跑通。回头复盘发现市面上关于“UWB雷达做SLAM”的资料非常零散要么只在讲UWB定位原理要么只给个Matlab仿真框架真正把雷达回波、点地标提取、EKF更新串成一条线的文章几乎没有。这篇就把我实际走过的路写出来怎么把UWB雷达的自然点地标映射进地图、怎么在室内环境里用扩展卡尔曼滤波器做状态估计、以及在Matlab里调试时踩过的那些坑。内容适合正在做移动机器人定位、传感器融合课程设计、或者想从激光SLAM转到雷达方案的同学不需要你已经有很深的滤波基础但如果你看过一点卡尔曼滤波的介绍读起来会更顺。1. 核心思路为什么是UWB雷达为什么是EKF1.1 UWB雷达在室内场景里解决什么问题UWB超宽带雷达和日常讨论的UWB标签定位是两回事。标签定位需要机器人上装标签、环境里布基站本质是个定位网络而UWB雷达是主动发射纳秒级窄脉冲通过接收反射回波来感知周围物体。它最大的特点是能测距而且测距精度能到厘米级同时不像激光雷达那样依赖高反光率目标——室内常见的墙角、柱面、金属门框、泡沫板都能形成稳定的回波峰。室内环境对定位来说有个天然麻烦长期稳定不动的“几何特征”很多比如柱子、墙角、门窗边缘它们可以当路标用。但视觉SLAM在低光照和白色墙面环境下容易退化激光雷达对玻璃和黑色吸光材料无能为力而UWB雷达恰恰对材质不敏感只要目标在几何上能反射电磁波就能形成点地标。这是我最终决定用UWB雷达做SLAM的直接原因——它解决的是一类“视觉和激光都不太舒服”的室内场景。不过这也就引出一个问题UWB雷达不像激光雷达那样直接给你一帧均匀角分辨率点云。它给的是一维回波幅度序列你得自己从中找出“哪些峰值对应真实目标”“哪些是多径反射”这一步本质上是在做特征提取处理不好后面EKF再漂亮也没用。1.2 自然点地标与地图表示的选择自然点地标指的是环境中本来就存在的、不需要人工铺设反光板或信标的结构性物体。在UWB雷达回波里这些地标表现为某个距离单元上的局部峰值且该峰值在连续多帧中保持稳定。选用“点地标”而不是“栅格地图”来建图原因是EKF的经典框架天然适合路标地图——每个地标在状态向量里占据一个二维位置数据关联成功后就做一次量测更新复杂度可控。地图表示上我做了一个很省事的选择用全局坐标系下的稀疏点集。每个地标节点包含三个字段全局坐标、最后一次观测时间、累计被观测次数。它不像栅格地图那样能支撑路径规划但它能精确回答“机器人现在在哪”而这恰恰是UWB雷达后期做回环修正的前提。如果你后续想接自主导航可以在这个稀疏地图基础上再叠加占据栅格。地图坐标系的基准我固定在机器人上电时刻的初始位姿也就是世界坐标系。所有UWB雷达量测先转换到机器人坐标系再通过当前机器人位姿变换到全局坐标这个环节不少人搞反后面会单独说。1.3 EKF相比其它滤波方案的优势与代价做室内SLAM可选方案很多PF粒子滤波、UKF无迹卡尔曼滤波、EKF以及图优化SLAM。我为什么在项目里锚定EKF一个字稳。在状态维数不高这里的状态是3维机器人位姿加2N维地标坐标的情况下EKF的计算量最小Matlab单线程都能实时跑。UKF不需要算雅可比但在状态接近线性的场合精度优势体现不出来反而增加采样开销。PF适合全局定位和非高斯噪声但对地标数量敏感粒子数稍微一多单步更新就卡顿。图优化在回环修正上更强可它需要先有一个能用的里程计与观测融合框架EKF就是那个框架本身。代价也很明显EKF的线性化误差在处理雷达剧烈跳变时会被放大而且它对数据关联错误极其敏感——一次误关联就能让协方差矩阵发散。为了压住这个问题我在滤波更新前加了一个基于马氏距离的关联门控后面代码里会讲。2. 系统架构与状态方程设计2.1 硬件组成与数据流整个系统由四部分串起来。运动底盘我用的双轮差速提供编码器里程计UWB雷达模块负责发射和接收回波通过串口上报处理后的ADC幅度序列主控板跑Matlab脚本执行特征提取和EKF最后是一个可视化界面实时画机器人轨迹和地标。数据流的时序是这样的每个控制周期20ms读一次编码器算一次位姿预测雷达单独有一个数据线程每100ms出一帧回波到了就触发一次量测更新。注意UWB雷达帧率和控制周期不同步所以必须用时间戳对齐否则会出现“用了上一帧的雷达数据做当前时刻的更新”这种细微但致命的错误。这个流程里最容易忽视的点是“底盘里程计系和雷达安装系之间的外参”。雷达在机器人上不可能是绝对原点安装位置和朝向有偏差这些外参在仿真里一两个角度看不出问题实际装车时偏差个5cm、3度地图就会逐渐扭曲。2.2 状态向量定义与运动学建模EKF的完整状态向量是[ \mathbf{x} \begin{bmatrix} x_r y_r \theta_r x_{L1} y_{L1} \cdots x_{LN} y_{LN} \end{bmatrix}^T ]前三维是机器人全局位姿后面是每个地标的二维坐标。这是标准EKF-SLAM的增广状态写法。好处是机器人和所有地标一起被估计地标的定位误差会自动和机器人位姿耦合比“两步走”先定位后建图在理论上有更强的精度。运动模型我用差速机器人的经典模型[ \begin{aligned} x_{k1} x_k v\Delta t \cos(\theta_k) \ y_{k1} y_k v\Delta t \sin(\theta_k) \ \theta_{k1} \theta_k \omega \Delta t \end{aligned} ]v和\omega来自编码器解算。雷达SLAM不依赖外部里程计也可以做但那样状态方程会很难写初学阶段不建议一步到位。2.3 雷达观测模型极坐标下的非线性量测UWB雷达量测输出人为定义成距离r和方位角α。注意这里的方位角不一定是雷达直接测出来的很多UWB雷达只能给距离剖面角度信息需要通过多接收天线相位差或波束扫描来获取。如果硬件只有单天线那观测就是纯距离我只能通过多位置观测三角化地标这会明显降低单步更新的信息量。我的代码里假设雷达能同时输出距离和相对角度这也是多数商用UWB雷达模块的常见配置。观测方程[ \begin{aligned} r \sqrt{(x_L - x_r)^2 (y_L - y_r)^2} \ \alpha \operatorname{atan2}(y_L - y_r, x_L - x_r) - \theta_r \end{aligned} ]两式对机器人状态和地标状态都是非线性的这正是需要EKF的地方——用一阶泰勒展开把非线性方程在当前估计点线性化。3. 从UWB回波到可用的点地标3.1 原始回波预处理UWB雷达输出的原始数据通常是一串幅度值对应不同距离区间。在预处理阶段我做了三件事静止杂波对消、滑动平均平滑、一维峰值检测。静止杂波对消很关键。室内无论有没有人总有墙壁、家具这类固定反射体它们产生的回波在幅度上可能比待检测的小目标还强。办法是维护一个背景帧用指数移动平均持续更新然后用当前帧减去背景帧再检测。这个背景更新率不能太高否则移动目标也会被“对大对消”抹掉我在项目里把更新系数设为0.05效果比较稳定。3.2 峰值检测与地标候选生成我用的峰值检测不是简单找局部极大值而是带两个门限幅度门限和最小距离间隔门限。幅度门限要动态计算取当前帧幅度均值加2.5倍标准差低于这个水平的峰一律忽略。最小距离间隔设为0.3米避免同一个目标因为反射体多径在相邻距离单元形成多个峰。候选点出来后还要做一个“时间稳定性”确认连续三帧都检出且位置变化小于某个阈值才升级为正式地标候选。这一步非常有效能滤掉人走动带来的瞬态反射以及多径信号的随机起伏。我在代码里是用一个计数器数组记录的每次雷达帧到来就更新一次计数值。3.3 数据关联地标匹配的“身份证”机制EKF SLAM 里数据关联的质量直接决定系统是否发散。关联的依据是预测观测与真实观测的差异马氏距离小于门限且最小的一对认定为匹配[ d (\mathbf{z} - \mathbf{h}(\hat{\mathbf{x}}))^T \mathbf{S}^{-1} (\mathbf{z} - \mathbf{h}(\hat{\mathbf{x}})) ]S是创新协方差由雅可比、状态协方差和量测噪声共同算得。我做了一个小改进除了距离门限外还要求匹配对的“地标观测量测次数”之差不能太悬殊避免新鲜地标和老地标之间因为数值相近发生误关联。这个技巧在杂波较多的室内环境里非常管用。4. EKF SLAM 的核心推导与实现细节4.1 预测步的雅可比矩阵预测步是线性模型所以雅可比其实就是运动模型对状态向量的导数。不需要对整状态2N3维都求一遍而是只在机器人的3维子块上算一个3x3矩阵[ F_x \begin{bmatrix} 1 0 -v\Delta t\sin\theta \ 0 1 v\Delta t\cos\theta \ 0 0 1 \end{bmatrix} ]完整的预测协方差更新是[ P_{k1} F_x , P , F_x^T F_v , Q , F_v^T ]其中F_v是控制噪声对状态的影响矩阵Q是对角化的控制噪声协方差。这个地方常见的错误是忽略了F_v导致协方差只减不增滤波看起来越来越自信实际上早就不准了。4.2 量测更新的雅可比矩阵量测更新阶段要对当前关联的每个地标分别计算H矩阵。以地标L_i为例H大小是2行乘(2N3)列。第一行对机器人位置求导得到[ \frac{\partial r}{\partial x_r} \frac{x_r - x_{Li}}{\sqrt{(x_{Li} - x_r)^2 (y_{Li} - y_r)^2}} ]第二行对应方位角求导用atan2的偏导公式。更严谨地说还应考虑偏航角对方位角的影响即相对于θ_r那一列要填-1。很多入门代码在这里漏掉负号看似微小累积之后地图会发生整体旋转。注意系数H矩阵在P矩阵更新时是通过稀疏方式拼装而成的。你可以在Matlab里预先分配一个全零矩阵然后把对应列填进数值。千万别直接写循环动态增广矩阵那样维度一变协方差矩阵就会错位。4.3 初始化新地标的“第一次观测”当一个新地标被发现滤波器的状态向量要即时扩展。如何从首次观测反推地标全局坐标[ \begin{aligned} x_L x_r r\cos(\theta_r \alpha) \ y_L y_r r\sin(\theta_r \alpha) \end{aligned} ]同时初始化协方差块这一块的初值误差半径大概在0.1-0.2米量级具体取决于雷达测距噪声。这里有一个我最初做错的点新地标的协方差不能设成零。如果设零意味着滤波器完全相信第一次观测后续的地标位置更新幅度会被严重钳制。新增地标的协方差块可以写成[ P_{new} G_x P_r G_x^T G_z R G_z^T ]G是观测对地标状态的反算雅可比这一步在代码里通常用一个函数单独实现。5. Matlab代码实现从框架到调试5.1 主程序整体结构我把整个工程拆成四个脚本模块化命名方便单步调试main_ekf_slam.m主循环负责数据读取、时间戳管理、调用预测与更新predict_ekf.m运动模型预测update_ekf.m基于雷达量测更新状态data_association.m马氏距离匹配与门控主循环的核心顺序是读当前雷达帧提取地标候选做数据关联然后执行EKF更新在每个控制周期开始前先执行一次里程计预测。这个顺序很讲究设计成“先预测、后更新”让滤波器中等待雷达数据的间隙也能通过里程计推进位姿估计。5.2 核心EKF预测代码function [x, P] predict_ekf(x, P, v, w, dt) theta x(3); x(1) x(1) v*dt*cos(theta); x(2) x(2) v*dt*sin(theta); x(3) x(3) w*dt; Fx eye(length(x)); Fx(1,3) -v*dt*sin(theta); Fx(2,3) v*dt*cos(theta); Q diag([0.02^2, 0.02^2, 0.02^2]); % 控制噪声 Fv zeros(3,3); Fv(1,1) dt*cos(theta); Fv(2,2) dt*sin(theta); Fv(3,3) dt; P(1:3,1:3) Fx(1:3,1:3)*P(1:3,1:3)*Fx(1:3,1:3) Fv*Q*Fv; % 注意地标块协方差在预测步保持不变 end这里面有个细节预测更新P的时候要完整计算Fx P Fx^T不能只更新3x3的角块因为机器人和地标之间的互协方差会因为机器人运动发生变化不更新会导致互协方差失真、量测更新时增益算不准。5.3 量测更新代码与增益计算量测更新的Matlab实现要保证雅可比矩阵的维度匹配。我的做法是预先分配H、全零然后逐地标回填function [x, P] update_ekf_with_landmark(x, P, z, idx, R) % z: [range, angle] % idx: 地标在状态向量中的索引起点 xr x(1); yr x(2); thr x(3); xl x(idx); yl x(idx1); dx xl - xr; dy yl - yr; r_pred sqrt(dx^2 dy^2); alpha_pred atan2(dy, dx) - thr; Hr zeros(2, length(x)); Hr(1,1) -dx/r_pred; Hr(1,2) -dy/r_pred; Hr(1,idx) dx/r_pred; Hr(1,idx1) dy/r_pred; Hr(2,1) dy/r_pred^2; Hr(2,2) -dx/r_pred^2; Hr(2,idx) -dy/r_pred^2; Hr(2,idx1) dx/r_pred^2; Hr(2,3) -1; % 偏航角对方位角的贡献 z_pred [r_pred; alpha_pred]; yerr z - z_pred; yerr(2) wrapToPi(yerr(2)); S Hr * P * Hr R; K P * Hr / S; x x K * yerr; P (eye(length(x)) - K*Hr) * P; endwrapToPi那行非常关键角度残差不整理的话在零度附近稍微一抖动滤波器的更新就会被一个接近2π的假残差摧毁。5.4 参数调节的“坑”R矩阵是雷达量测噪声协方差直接影响Kalman增益的权重。距离噪声相对于角度噪声在不同环境中变化非常大空旷走廊角度噪声小转角密集区域多径严重角度噪声大。我最终把R设成对角矩阵对角线值用实测数据的标准差来标定而不是拍脑袋。Q矩阵方面控制噪声协方差按轮式里程计线性标定来定不能一味往小里调。有人为了让轨迹看起来“平滑”把Q调到很小结果是协方差P收缩雷达的新信息影响权重变大反而更容易振荡甚至发散。6. 仿真与实车实验中的效果对比6.1 仿真场景走廊和“回”字型房间我先在Matlab里用纯仿真数据验证算法流程。雷达模拟模块按真实采样率产生带噪声的距离和角度观测地标模拟了门口边缘、墙角、消防栓盒共8个。仿真跑下来机器人沿“回”字型路径走一圈EKF轨迹和真实轨迹的平均位置误差约4厘米地标位置误差约6厘米。噪声参数增大一倍后误差并没有线性增长这是因为EKF的重复观测会不断修正地标位置量测越多单个观测精度的影响就越弱。这验证了SLAM相对纯定位的“越看越准”特性。6.2 实测数据跑出来的效果与局限实测时用的是某UWB雷达模块测距标准差约3厘米角度标准差约2度。在6米x8米室内办公环境放置桌椅和纸箱若干自然点地标选取了三个铁皮柜边缘和两处墙角。结果是轨迹闭合误差约0.15米地标位置与手工测量的参考误差在0.1米左右这个精度对室内巡检定位场景完全可用。但局限也明显雷达对软质材料布料沙发、泡沫板反射弱特征提取阶段很难稳定检测而金属物体反射过强旁瓣会形成伪峰。单纯依赖UWB雷达特征在极简毛坯房里地标数量不够容易退化。如果你在大面积空旷场地做建议雷达和轮式里程计再加一个IMU做航向参考三个传感器互补性很好。7. 常见问题排查与调试技巧7.1 协方差发散90%是数据关联出问题协方差发散几乎都是误关联造成的不是滤波器本身写错了。因为EKF的增益公式是精确保留的一次更新只会对正确的观测和预测残差做线性化近似不会无故爆掉。排查步骤我的习惯是先把数据关联的门限值调大看轨迹是否平滑如果门限变大后轨迹依然发散那问题大概率在H矩阵的偏航角列或负号把量测更新拆出来单测。7.2 地图出现旋转检查方向角补全地图整体旋转是编码器航向和雷达方位角之间不一致的典型症状。要么是外参的安装角没标定要么是方位角定义基准和机器人坐标系定义不一致。我在Matlab里加了一段自动外参标定脚本让机器人自旋一周对比雷达测到的一个强反射点的方位角来回波动反推安装偏角。7.3 新地标重复添加关联门限太紧门限太小会频繁把老地标当成新地标地图里出现一堆“重复点”。解决方案是两步第一步检查新观测到所有已存在地标预测位置的距离若小于某个物理距离比如0.5米直接沿用老地标第二步用马氏距离做精细化匹配。千万别直接把门限放宽那样不同地标也会合并。7.4 真实环境中的多径干扰UWB雷达在金属墙面附近会有多径峰看起来像真实目标。预处理时我用两个后手幅度稳定性检验和回波时延连续检验。后者很简单真实目标距离变化是连续的多径虚影却可能测距波动剧烈。这个案例告诉我滤波前端的信号处理质量决定滤波后端精度的上限。8. 后续扩展方向与个人心得如果想把这套系统推向更高精度最直接的扩展是加入回环检测模块。EKF-SLAM本质是增量式的误差累积无法消除当机器人回到之前位置时若能用雷达特征做一次全局重定位再通过图优化做全局地图修正精度还能再上一个大台阶。第二个扩展方向是引入多传感器融合在EKF框架里同时接受IMU观测编码器里程计和雷达点状态向量不变只需扩充量测方程。IMU对角速度的短时精度很高正好弥补雷达帧率不足带来的预测误差。个人调试这台系统下来最有价值的经验只有一条——不要一上来就调滤波器参数。先把雷达数据可视化和数据关联结果看清楚只要数据关联是稳的EKF的调参过程通常几个小时就能搞定数据关联不稳定调一个月参数也没用。我最后分享一个这轮做下来的小技巧写代码时把协方差矩阵对角线单独画出来实时监控。一旦对角方差开始指数上升几乎可以断定前置数据出了问题——这比你盯着轨迹图判断发散要早几十个周期能帮你快速定位是哪个环节开始失稳的。
阅读完成 · 觉得有帮助？
