简介这是一套基于ROS的Range-Only无线传感器网络扩展卡尔曼滤波定位与SLAM学习项目面向机器人导航、传感器融合方向的课程设计、毕业设计及研究者。资源围绕TurtleBot3仿真平台组织涵盖定位与建图所需完整源码和项目说明可帮助读者理解距离量测下的EKF状态估计、无线传感器网络布置以及SLAM任务实现思路。压缩包共354个文件约8.16MB主要包含launch启动脚本、xacro与sdf/world仿真模型、config/yaml参数配置、py/cpp算法节点、rviz可视化配置、msg/srv/action消息定义以及stl/dae三维模型和README等说明文档目录结构覆盖仿真、驱动、建图、定位与可视化模块便于按需查阅和二次开发。目前已有66人学习下载。除源代码外项目说明对调试和运行流程做了交代适合具备一定ROS基础的学习者直接对照仿真环境验证EKF定位效果并在此基础上扩展无线传感器网络SLAM实验。1. 一个测距值能同时解决定位和建图吗先搞清楚这份源码在说什么做室内移动机器人定位激光雷达和视觉slam的资料一抓一大把但真正遇上 UWB 标签、蓝牙信标这种 Range-Only 传感器很多人第一反应是“这不就是三边测量嘛”。实测跑起来才发现锚点坐标不精确、机器人在移动、测距值时不时跳一个野值三边测量根本稳不住。这个标题里的方案是把机器人的位姿和无线传感器网络里各个锚点的坐标放进同一个扩展卡尔曼滤波状态里用距离观测同时修正两边把 SLAM 的思路搬到 Range-Only 传感器上这也是“扩展卡尔曼滤波定位及 SLAM”这句话的核心含义。这篇文章就顺着这个项目包的思路把原理、ROS 节点设计、参数整定和常见踩坑场景一次讲透适合正在做仓储机器人、UWB 室内定位落地或者刚接触 ROS 想从仿真入手跑通一套定位算法的开发者。2. Range-Only 为什么不能套普通卡尔曼观测模型与状态方程拆解2.1 距离观测函数的非线性和雅可比的正确写法Range-Only 的观测只有一个量机器人到锚点的欧氏距离。设机器人位置为 (p_r(x_r,y_r))锚点位置为 (p_a(x_a,y_a))观测模型写作[ h(x)\sqrt{(x_r-x_a)^2(y_r-y_a)^2} ]普通卡尔曼滤波要求观测是线性的这里显然不是。扩展卡尔曼滤波的做法是在当前状态估计附近做一阶泰勒展开所以核心工作就落在 (h(x)) 对状态向量里每一个分量的偏导也就是雅可比矩阵 (H)。对机器人位置求偏导得到的是从锚点指向机器人的单位方向向量对锚点位置求偏导方向正好相反。写代码时我习惯把公式直接展开避免查错索引import numpy as np def h_and_H(x, anchor_id): # 状态向量布局: [xr, yr, theta, v, w, xa1, ya1, xa2, ya2, ...] idx_a 5 2 * anchor_id xr, yr x[0], x[1] xa, ya x[idx_a], x[idx_a 1] dx xr - xa dy yr - ya d np.sqrt(dx * dx dy * dy) # 观测值 z_pred d # H 是 1 x dim 的行向量 dim x.shape[0] H np.zeros((1, dim)) # 对机器人位置的偏导: (dx/d, dy/d)theta/v/w 没有直接依赖 H[0, 0] dx / d H[0, 1] dy / d # 对锚点位置的偏导: (-dx/d, -dy/d) H[0, idx_a] -dx / d H[0, idx_a 1] -dy / d return z_pred, H代码背后的逻辑是(d) 作为分母距离越近雅可比数值越大滤波器对近距离观测的“信任”会剧烈变化而当 (d) 趋向 0 时直接除零这就是后面避坑章节里 NaN 问题的主要来源。另外注意 (H) 对机器人的航向角 (\theta)、速度 (v)、角速度 (w) 的偏导都是 0Range-Only 传感器天生不提供任何角度信息机器人的朝向只能依靠运动模型来累积估计。2.2 状态向量怎么拼、协方差初始值给多大这套方案把锚点当成地图里的路标点来处理状态向量是一个联合向量[ x [x_r,\ y_r,\ \theta,\ v,\ w,\ x_{a1},\ y_{a1},\ x_{a2},\ y_{a2},\ \dots,\ x_{aN},\ y_{aN}]^T ]机器人部分我常用差速运动模型做预测锚点部分在预测阶段保持恒等锚点不会自己移动。真正让锚点坐标变准的是观测更新这一步每次收到距离观测EKF 会同时修正机器人状态和对应锚点的坐标这就是“定位及 SLAM”同时完成的关键。协方差矩阵 (P) 的初始化在这里很讲究。机器人部分的初值来自轮式里程计的噪声水平锚点部分则完全取决于你“预先知道多少”如果锚点是人工拉尺量出来的初始方差给 ((0.1\text{m})^2) 就够了如果锚点位置完全未知、是个黑匣子协方差给到 (100) 也不奇怪。实际调参时你会发现锚点初始协方差给得太小锚点坐标几乎不会被修正SLAM 退化成纯定位给得太大滤波前期会出现几秒到几十秒的明显收敛过程轨迹会有可见的拉拽。2.3 预测与更新的完整 EKF 骨架下面这一段是我在类似项目里常用的最小实现可以直接跑通理解流程也能改造成独立模块import numpy as np class RangeOnlyEKF: def __init__(self, n_anchors): # 状态维度: 机器人5维 每个锚点2维 self.dim 5 2 * n_anchors self.x np.zeros(self.dim) self.P np.eye(self.dim) * 0.1 self.n_anchors n_anchors # 运动噪声: 位移噪声和转向噪声按经验给初值 self.q_xy 0.02 self.q_theta 0.03 def predict(self, v, w, dt): xr, yr, theta self.x[0], self.x[1], self.x[2] # 差速运动模型 self.x[0] v * np.cos(theta) * dt self.x[1] v * np.sin(theta) * dt self.x[2] w * dt # 状态转移雅可比 F F np.eye(self.dim) F[0, 2] -v * np.sin(theta) * dt F[1, 2] v * np.cos(theta) * dt F[2, 4] dt # theta 对 w 的偏导 F[0, 3] np.cos(theta) * dt F[1, 3] np.sin(theta) * dt # 过程噪声协方差 Q Q np.eye(self.dim) * 1e-6 Q[0, 0] Q[1, 1] self.q_xy ** 2 Q[2, 2] self.q_theta ** 2 self.P F self.P F.T Q def update(self, z, anchor_id, R_range): z_pred, H h_and_H(self.x, anchor_id) S H self.P H.T R_range K self.P H.T np.linalg.inv(S) innovation z - z_pred # 简单门限: 防止野值直接击穿滤波器 if abs(innovation) 3.0 * np.sqrt(S[0, 0]): return False self.x self.x K.flatten() * innovation self.P (np.eye(self.dim) - K H) self.P return Truepredict里的 (F) 矩阵左上角是标准差速模型雅可比右下角锚点部分是单位阵因为锚点在预测中不动。(Q) 里给非锚点维度很小的 (10^{-6}) 是为了让矩阵保持正定防止数值退化。update里的R_range是传感器测距噪声方差不是随便拍的数下一章会讲怎么从实验里标定得到。整体节奏上predict按固定频率调用比如 20Hz 或 50Hzupdate由测距话题回调触发两个过程不要混在同一个线程里。3. 把 EKF 拆成 ROS 节点话题设计、坐标约定与最小可运行框架3.1 驱动、滤波、可视化三个节点怎么分工拿到一份项目包先把 ROS 节点结构看懂再跑代码。常见做法是把系统拆成三个节点无线传感器驱动节点负责读 UWB 或蓝牙模块的原始数据打包成距离话题发布EKF 节点订阅这些距离话题结合里程计做预测和更新输出定位结果可视化节点把锚点估计和机器人轨迹通过 RViz 展示出来。这样拆的好处是每个节点都可以单独重启和调试传感器驱动偶发断流时不会把滤波节点一起带崩。ROS 环境本身没有太多新东西要讲Ubuntu 20.04 搭配 Noetic、Ubuntu 22.04 搭配 ROS2 Humble 都是成熟组合。新手装环境可以直接用一键安装脚本把基础工具链五分钟之内拉起来然后把重点放在工作空间里的 catkin 或 colcon 构建流程上。话题设计上我一般不用sensor_msgs/Range这种现成消息因为它的字段是为红外和超声波这类近距离传感器设计的缺少锚点 ID 这个关键信息。更实用的做法是自定义一个带 ID 的消息# RangeStamped.msg std_msgs/Header header float64 range uint8 anchor_id一个话题承载所有锚点的测距值比给每个锚点单独开话题更容易做时间同步。坐标约定上我习惯把锚点坐标系命名为anchor_0、anchor_1这种带编号的 frame机器人本体是base_link定位输出是map。3.2 驱动节点发布距离话题的最简实现驱动节点不需要做任何滤波处理职责就是“把传感器的原始读数转换成 ROS 消息”。这样 EKF 节点可以独立于具体硬件后面在仿真里验证滤波算法时只要把驱动节点换成仿真数据发布器即可。#!/usr/bin/env python3 import rospy import serial from std_msgs.msg import Header from range_only_msgs.msg import RangeStamped def publish_ranges(): rospy.init_node(uwb_driver) pub rospy.Publisher(anchor_ranges, RangeStamped, queue_size10) rate rospy.Rate(20) # 假设串口接了一个多锚点 UWB 模块数据格式: id,range ser serial.Serial(/dev/ttyUSB0, 115200, timeout0.1) while not rospy.is_shutdown(): line ser.readline().decode().strip() if not line: continue parts line.split(,) msg RangeStamped() msg.header Header() msg.header.stamp rospy.Time.now() msg.header.frame_id base_link msg.anchor_id int(parts[0]) msg.range float(parts[1]) pub.publish(msg) rate.sleep()这段代码的重点是时间戳必须取“数据从传感器读到的时刻”而不是取当前系统时间两者看起来差不多但在机器人高速移动时能差出十几厘米的误差。如果硬件本身带时间戳直接透传优先。频率上 UWB 模块一般支持 10Hz 到 50Hz我通常先跑 20Hz频率太高容易暴露通信抖动太低则 EKF 预测步之间机器人位姿变化太大。3.3 EKF 节点怎么订阅、发布和防止多线程崩坏EKF 节点要做三件事订阅里程计话题做预测、订阅距离话题做更新、发布定位结果。里程计一般来自轮式编码器话题是odom或wheel_odom。下面是一个可运行的骨架#!/usr/bin/env python3 import rospy import numpy as np from nav_msgs.msg import Odometry from geometry_msgs.msg import PoseWithCovarianceStamped from range_only_msgs.msg import RangeStamped from ekf_ros.ekf import RangeOnlyEKF class EkfNode: def __init__(self): rospy.init_node(range_only_ekf) n_anchors rospy.get_param(~n_anchors, 4) self.ekf RangeOnlyEKF(n_anchors) self.pub rospy.Publisher(ekf_map, Odometry, queue_size10) self.robot_odom None self.last_time rospy.Time.now() rospy.Subscriber(wheel_odom, Odometry, self.odom_cb) rospy.Subscriber(anchor_ranges, RangeStamped, self.range_cb) rospy.Timer(rospy.Duration(0.05), self.predict_timer) def odom_cb(self, msg): self.robot_odom msg def range_cb(self, msg): # 更新步直接读入R 值从参数服务器加载 R rospy.get_param(~R_range, 0.09) ok self.ekf.update(msg.range, msg.anchor_id, R) if ok: self.publish() def predict_timer(self, event): if self.robot_odom is None: return now rospy.Time.now() dt (now - self.last_time).to_sec() v self.robot_odom.twist.twist.linear.x w self.robot_odom.twist.twist.angular.z self.ekf.predict(v, w, dt) self.last_time now self.publish() def publish(self): msg Odometry() msg.header.stamp rospy.Time.now() msg.header.frame_id map msg.pose.pose.position.x self.ekf.x[0] msg.pose.pose.position.y self.ekf.x[1] msg.pose.pose.orientation.z self.ekf.x[2] self.pub.publish(msg) if __name__ __main__: try: node EkfNode() rospy.spin() except rospy.ROSInterruptException: pass这段代码里藏着两个容易翻车的细节。第一predict_timer和range_cb在 rospy 的不同线程里跑self.ekf会被并发调用我在实际项目里会在回调函数外面加一把线程锁否则会偶发协方差矩阵不对称导致发散。第二R_range从参数服务器加载不用每次改代码调参时直接rosparam set就能生效这对后面参数整定阶段非常关键。4. EKF 参数整定Q、R、锚点初始协方差到底怎么给数值4.1 两个必须从实验里拿的数不要猜很多项目说明文档会把算法原理写得很完整但参数表只有一行“根据实际调整”真正能落地的经验在于这些参数怎么从数据里推出来。过程噪声 (Q) 里的 (q_{xy}) 和 (q_{\theta})反映的是你的运动模型和真实机器人运动之间的误差。最直接的标定方法是把机器人放在原地用轮式里程计记录一段速度指令看机器人静止但里程计位置漂移了多少或者跑一段已知长度的直线对比里程计终点和实际终点把误差折算成单位时间的位置噪声和转角噪声。观测噪声 (R_{range}) 的标定更简单也更关键把传感器放在已知距离处静止采集 100 个测距值计算标准差再平方就是方差。如果用的是 UWB 这类传感器静态标准差通常在 5 厘米到 15 厘米之间对应 (R0.0025) 到 (0.0225)。这里有个很容易掉进去的陷阱如果使用的是蓝牙 RSSI 转距离RSSI 的波动远大于 UWB残差动辄几十厘米此时必须先把 RSSI 和真实距离的标定曲线拟合出来再用拟合残差确定 (R)否则 EKF 会把一个本身噪声巨大的观测当成高精度数据。我一般给出的参数推荐表如下参数推荐初值从哪来设错会怎样(q_{xy})0.01 ~ 0.05里程计直线漂移标定太小轨迹过平滑太大轨迹毛刺多(q_{\theta})0.01 ~ 0.1原地旋转漂移标定影响航向收敛速度(R_{range})0.0025 ~ 0.0225静态测距标准差平方核心参数决定整个滤波可信度锚点初始协方差0.01 ~ 100锚点坐标先验置信度见 4.2给错会让锚点不收敛马氏距离门限5.99卡方分布 2 自由度 95%过小丢真值过大野值打穿滤波4.2 锚点坐标的三种初始化路径和运动激励设计Range-Only SLAM 里锚点坐标并不是从一开始就可观的。机器人静止时一个距离观测只能把锚点约束在一个以机器人为中心的圆上没有任何角度信息所以锚点必须在机器人运动过程中被“激发”出来。常见的锚点初始化有三种做法。第一种是人工先验用激光测距仪或者拉尺量出锚点的大致坐标输入给滤波器协方差给 ((0.3\text{m})^2) 左右。这种方法收敛最快也最稳。第二种是机器人先沿着一条已知路径绕场走一圈每走一步记录当时的机器人坐标和到各锚点的距离离线通过最小二乘估计出各锚点初值再回灌给 EKF。第三种是完全未知起步锚点协方差给 100让 EKF 在滤波前期自己估计缺点是前几秒到十几秒内锚点估计会明显乱跳。从工程稳定性角度我不建议一上来就完全盲估锚点。哪怕现场只量了一个粗糙坐标也比让滤波器冷启动硬猜强出很多。项目说明文档里如果能配一张机器人建议运动路径图通常就是这个意图走“之”字形或者环绕每个锚点转一圈让距离变化率覆盖各个方向信息矩阵才能长起来。4.3 野值剔除马氏距离门限别当摆设无线测距在真实环境里的噪声不是高斯白噪声UWB 遇到金属货架会产生多径蓝牙 RSSI 受人体遮挡影响波动巨大这两类数据都会以野值形式进入滤波器。EKF 最怕的不是高斯噪声而是某个残差达到几米的观测被当成正常数据更新进去。标准做法是用马氏距离做门限判断innovation z - z_pred S H P H.T R_range mahalanobis np.abs(innovation) / np.sqrt(S[0, 0]) if mahalanobis gate: # 拒收该观测 return False这里的gate取值有讲究。理论上单个距离观测的自由度是 195% 置信对应的卡方门限是 3.84但我实际使用时会把软门限放到 5 到 7给多径环境下真实距离留一点余量。更重要的一点是门限拒收不能只做一次连续丢数据会让滤波器对自身估计过于自信协方差不断缩小却在原地踏步。我通常的做法是连续三次拒收同一个锚点的观测时把该锚点在 (P) 中的对应协方差块重置到初始值这相当于给滤波器一次从怀疑中恢复的后悔药。5. Range-Only SLAM 避坑五个常见翻车现场与排查顺序5.1 锚点坐标越估越飘甚至漂到十万八千里外现象滤波器运行几分钟后RViz 里锚点位置开始缓慢向外移动最终停在一个完全错误的坐标上。这通常伴随着定位轨迹仍然自洽但整体偏移。原因锚点可观测性不满足。机器人只在锚点的一侧运动距离观测无法区分“锚点在左边”还是“锚点在右边”EKF 会沿着信息最弱的那个方向不断累积漂移。解决先做一次运动激励测试让机器人按 S 形路径行走覆盖锚点的多个方向如果仍然发漂就在状态向量里固定某一个参考锚点也就是把该锚点协方差锁死为 0只让其他锚点相对它调整。固定参考锚点之后地图整体刚体平移的不可观问题会被约束住这是我在仓储环境下最常用的兜底手法。5.2 协方差矩阵 NaN滤波器直接白屏现象程序运行几十秒到几分钟调试输出里P矩阵出现nan后续所有更新全部失效。原因第一个元凶是距离 (d) 恰好估计成 0h_and_H里除零产生inf再变成 NaN。第二个元凶是协方差矩阵失去对称性或正定性常见于预测和更新在不同线程同时读写、没有加锁。解决在h_and_H里给距离加一个微小下界d np.sqrt(dx*dx dy*dy) 1e-6每次更新后做一次对称化P 0.5 * (P P.T)并用np.linalg.eigvalsh(P)检查最小特征值是否大于 0。这个检查写成一条日志也比裸奔强一旦发现特征值小于 0立刻把锚点对应协方差块重置而不是继续跑。5.3 轨迹形状对但整体朝向差一个角度现象机器人绕场一圈EKF 画出的轨迹形状和小车真实路径完全一致但绕整个场地转了 30 度。这种失败最隐蔽因为残差均值看起来非常小。原因这是 SLAM 的规范自由度问题。所有测距观测对“整个地图绕原点旋转”完全不变也就是说旋转方向上没有信息约束EKF 估计出的地图可以整体任意旋转。这在纯 Range-Only 系统里是数学上的必然不是代码 bug。解决如果锚点先验坐标里有至少一个准确锚点固定它即可如果没有先验就给滤波器加一个弱方向观测比如把轮式里程计的航向或者磁力计航向作为附加的观测方程输进去权重放低一些只用来约束整体旋转不影响局部精度。5.4 静置很稳机器人一移动定位立刻抖动现象机器人停在原地时 EKF 输出平稳一旦直行或拐弯估出的轨迹高频抖动距离残差在正常值和野值之间来回跳。原因移动过程中 UWB 信号在动态多径环境下出现周期性衰落和反射叠加测距值在真实距离上下剧烈波动。更关键的是运动模型会放大这种波动速度越快预测协方差越大对当前观测的依赖越重。解决把马氏距离门限配合滑窗使用。我实际经验是取最近 5 个测距值的中位数作为送入 EKF 的观测值这比单纯提高门限更有效因为中位数天然对孤立的野值免疫。注意不要取均值均值会被野值直接拉偏。5.5 ROS 时间戳错位导致 EKF 预测跳变现象EKF 输出的轨迹出现顿挫感原地待机时位置也会周期性跳动残差呈现锯齿状。原因驱动节点和里程计节点使用了各自不同的时钟基准或者发布时间戳的单位不一致。rospy 里如果某个消息的时间戳比上一次更新的时间戳还旧EKF 的dt会算成负数位姿直接回退。解决在所有节点里统一使用rospy.Time.now()不要混用time.time()。在range_cb里加一条防御逻辑如果消息时间戳小于last_time直接丢弃。对多传感器时间不同步的场景用message_filters.ApproximateTimeSynchronizer把里程计和距离话题对齐。6. 没有真机也能验证Gazebo 仿真数据回放与三个控制指标6.1 写一个简单仿真节点生成带噪声的距离话题没有 UWB 硬件时可以在 Gazebo 里跑一个小车模型用真实位姿和各锚点固定坐标计算距离叠加上高斯噪声发布成和驱动节点完全相同的anchor_ranges话题。这样 EKF 节点的代码一行都不用改就能完成验证。#!/usr/bin/env python3 import rospy import numpy as np from nav_msgs.msg import Odometry from std_msgs.msg import Header from range_only_msgs.msg import RangeStamped def fake_ranges(): rospy.init_node(fake_uwb_sensor) pub rospy.Publisher(anchor_ranges, RangeStamped, queue_size10) anchors [(0.0, 0.0), (5.0, 0.0), (0.0, 5.0), (5.0, 5.0)] noise_std 0.08 # 模拟 UWB 静态噪声 rate rospy.Rate(20) while not rospy.is_shutdown(): odom rospy.wait_for_message(gazebo_odom, Odometry) xr odom.pose.pose.position.x yr odom.pose.pose.position.y for aid, (xa, ya) in enumerate(anchors): d np.sqrt((xr - xa)**2 (yr - ya)**2) d np.random.normal(0.0, noise_std) msg RangeStamped() msg.header Header() msg.header.stamp rospy.Time.now() msg.anchor_id aid msg.range d pub.publish(msg) rate.sleep()生成的数据可以rosbag record -O test_uwb /anchor_ranges /gazebo_odom录制下来离线循环回放反复调参比每次跑仿真快得多。6.2 判断 EKF 是否收敛的三个控制指标仿真环境最大的优势是有真值。我在验证时只看三个数一是距离残差均值滤波收敛后残差均值应该在 0 附近超过 10 厘米说明观测模型或 R 值有问题二是锚点坐标估计误差每个锚点恢复出来的位置和仿真里的真实坐标做对比误差大于 30 厘米说明运动激励不够三是闭环误差让机器人绕场一圈回到起点终点和起点的位置差应该小于半米。这三个指标能准确定位问题出在哪一层残差大是传感器模型问题锚点误差大是路径设计和可观测性问题闭环误差大是航向估计问题。按这个顺序排查基本不用东翻西找。6.3 把 EKF 的输出接进导航框架里的最后一公里定位结果最终要喂给导航模块常见做法是把 EKF 发布的ekf_map话题映射成odom给 move_base 使用。这里有一个容易忽视的细节move_base 默认认为odom的坐标系是连续平滑的而 EKF 输出的map坐标系会因锚点更新产生轻微跳变直接硬接会触发导航 planner 频繁重规划。我的习惯是在两者之间加一个robot_localization的 ekf_localization_node 做二次融合把轮式里程计和 Range-Only 的位姿估计算作两个观测源让底层输出保持平滑。这套系统做到最后经验里最有用的一条是调 Range-Only SLAM 时先固定锚点坐标跑纯定位确认定位轨迹已经足够好再把锚点放开跑 SLAM。如果纯定位阶段残差就下不来问题大概率出在距离标定上而不是滤波参数上。我自己被这条顺序救过很多次每次看到残差震荡都想改 Q 矩阵最后发现只是 UWB 模块的天线朝向歪了几厘米。希望你也能少走这段弯路按这个顺序调能省下大量排查时间。本文还有配套的精品资源点击获取
