从一条超声波测距到 ROS 导航:PointCloud2、碰撞监测与 Costmap

摘要:超声波通常只返回一条距离读数。介绍 ROS Range 的测量语义、圆弧虚拟点云的几何计算,以及 Nav2 碰撞监测与 Costmap 的不同处理方式,并比较适用场景和局限。

在这里插入图片描述

@[toc]

1. 超声波只有一个距离值,ROS 为什么能有三种用法

超声波测距器发射声波并接收回波,通常向上层报告一个标量距离,例如 0.20 m。这个数值说明反射目标可能出现在传感器前方的某个距离位置,但并没有告诉软件,反射点究竟位于左侧、正前方还是右侧。尤其对于具有一定波束宽度的探头,同一条距离读数无法像扫描式激光雷达那样确定每个角度各自的障碍物距离。

ROS 使用 sensor_msgs/Range 表达这类单次测距。其 field_of_view 定义了这条读数可能对应的角度范围,range 是距离值,header.frame_id 标记传感器坐标系,header.stamp 标记采样时间。ROS 的消息定义明确采用“距离值在某段圆弧上有效”的模型,而不是“每个角度都独立测到一个点”。[S1][S2]

以一个纯示例说明数据含义:

Range 字段 示例值 在下游有什么用
range 0.20 m 反射距离,后续几何模型的半径
field_of_view π/3 rad,即 60° 表示可能的左右角范围为 −30°~+30°
min_range / max_range 由具体探头标定 过滤超出有效测距范围的输入
header.frame_id ultrasonic_frame 供 TF 查找探头相对于机器人的位姿
header.stamp 实际测距时间 检查新鲜度与坐标变换时刻

60° 只是下文的教学例子,不是 ROS 或超声波的统一标准。不同探头可能有不同波束特性;软件填写的视场角与真实声场响应也不必然完全一致。

同一条 Range 测量,进入 ROS 后可以沿三条彼此独立的数据路径处理:

在这里插入图片描述

第一条路径创建便于显示和通用接口消费的点云;第二条路径只关心障碍物是否进入机器人周围的碰撞区域;第三条路径将测距视为一种有不确定性的环境观测,为导航规划更新栅格地图。这不是先后执行的三个必经阶段,碰撞监测和 Costmap 都可以不经过 PointCloud2。[S4][S5][S7]

2. 第一种用法:距离如何变成 PointCloud2

2.1 先理解“虚拟障碍点”:它们是画出来的,不是测出来的

设探头朝向本地坐标系的 +X,一次测得 r = 0.20 m。如果软件假设视场角为 60°,就可以在探头前方画一条半径 0.20 m、从 −30° 延伸到 +30° 的圆弧。随后在弧线上挑选几个位置,形成用于显示或避障的数据点。

在这里插入图片描述

这种方法并不是点云重建,也没有解算真实物体的表面形状。它只表达一个保守的几何假设:不知道反射点在扇区中的哪一个方向,因此用一串同距离的点覆盖可能的方向。 图中三个点属于同一次测量,不能把它们理解成三条独立的传感器回波。

2.2 圆弧上到底生成多少个点

一种实现策略是固定圆弧视角,并限制相邻采样点的最大间隔。例如,工程上存在使用固定 60° 圆弧、最大采样间隔约 15 cm 来生成虚拟点的做法。这里仅讨论该几何方案,不涉及具体设备、系统配置或私有工程实现。

先用弧长公式算出整段圆弧的长度:

1
2
3
4
圆弧长度 = 测得距离 × 圆弧角度(弧度)

距离 0.20 m、角度 60° = π/3 rad 时:
圆弧长度 = 0.20 × π/3 ≈ 0.2094 m(20.94 cm)

如果每段弧长不得超过 0.15 m,约 20.94 cm 的弧至少应切成两段,因此选取两端和中间的三个点。一个简单的通用离散方法是:先用 ceil(圆弧长度 / 最大间隔) 算分段数,再对每段进行等角度采样;具体实现也可以采用不同的取整和端点策略,点数因而可能不同。

三个点对应的局部角度和坐标是:

虚拟点 相对于探头的方向 X(米) Y(米) Z(米)
P0 −30° 0.1732 −0.1000 0
P1 0° 0.2000 0 0
P2 +30° 0.1732 +0.1000 0

计算并不复杂:朝前分量是 x = r × cos(角度),左右分量是 y = r × sin(角度);在二维圆弧模型中先令 z = 0。例如 30° 时,x ≈ 0.20 × 0.866 = 0.1732 m,y = 0.20 × 0.5 = 0.10 m。

下面是独立编写的教学伪代码,用于描述这一通用数学思路,并非摘录任何具体工程的源码:

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
import math

# radius 和 max_gap 单位为米;fov 单位为弧度
radius = 0.20
fov = math.pi / 3
max_gap = 0.15

arc_length = radius * fov
segments = max(1, math.ceil(arc_length / max_gap))

points = []
for k in range(segments + 1):
angle = -fov / 2 + k * fov / segments
x = radius * math.cos(angle)
y = radius * math.sin(angle)
points.append((x, y, 0.0))

这里限制的是软件绘制的点间距,不是超声波的测距精度、角度分辨率,也不代表分成三点之后突然获得了三次测量。即使把采样间隔缩小到 1 cm,新增加的点仍属于同一条测距假设。

另一种公开做法是固定点数。例如 ROS 1 开源项目 ros-range-to-pointcloud 的 populateRangesToPC() 从 Range.field_of_view 计算角度步长,按 number_of_points_per_side 选择采样数量,而不是固定 15 cm 的弧长间距。它还提供可选的 Z 方向扩展,这同样是软件填充点的策略,并不会自动增加硬件实测的空间信息。[S4]

2.3 为什么还要做 TF 坐标变换

到这一步,P0~P2 仍是以探头为原点的局部点。如果探头安装在机器人前端、朝向侧面或具有俯仰角,局部坐标不能直接当作机器人坐标。

ROS 的 TF/TF2 用来表达坐标系之间的位置和朝向关系。一般处理顺序是先按照探头安装姿态旋转,再加上探头相对于机器人底盘的平移。可以用一个简单实例理解:

  • 中间点在探头坐标系中是 (0.20, 0, 0),表示探头正前方 20 cm;
  • 假设探头安装于机器人坐标系 (0.40, 0.10, 0.20),并朝左旋转 90°;
  • 那么这条局部 +X 距离会落在机器人 +Y 方向,变换后的坐标是 (0.40, 0.30, 0.20)。

真实系统需要正确配置或查询传感器到目标坐标系的变换,并使用与测量相匹配的时间;若姿态、安装高度或时间戳错误,发布成点云也无法自动修正。圆弧局部 z=0 仅代表采样发生在探头局部平面,不意味着变换后的所有点高度为零。[S4][S5]

2.4 PointCloud2 实际承担什么职责

在每个点的 XYZ 坐标都计算完成后,程序可以使用 PCL 的 pcl::PointCloud<pcl::PointXYZ> 管理点集合,再转换或适配为 sensor_msgs/PointCloud2。PointCloud2 本身是一种点集合的 ROS 消息格式:fields 描述点字段布局,data 保存实际二进制点数据,width、height 描述组织形态,header 说明时间和坐标系。[S3][S4]

因此需要区分两件事:

  • 几何建模:从单次距离和假设视场生成 XYZ 坐标,核心是圆弧采样与坐标变换;
  • 消息封装:将已有 XYZ 集合装入 PointCloud2,以便 RViz、其他点云节点或支持点云的导航插件读取。

PointCloud2 的存在不意味着点云来自三维雷达;即使数据类型能够保存 XYZ,也无法从一次标量测距恢复真实障碍物的三维轮廓。公开的 ROS 1 转换包提供了完整的 Range → PointCloud2 参考流程,便于直接观察这一边界。[S3][S4]

2.5 这种表示在哪些情况下会误导后续算法

首先,弧上每个点代表一种可能位置,而不是每个位置都已确认存在障碍物。如果把整条弧无条件标成多个真实障碍点,导航栅格或碰撞检测可能比原始信息更保守;尤其在扇区边缘、细小目标、镜面反射、多径和斜入射情况下,实际回波方向并不知道。

其次,距离超量程、无回波、数值无效、超时以及旧样本都应在生成点之前区分。Range 消息规定了有效测距范围;另外,固定距离(二值)传感器的 −Inf(触发)和 +Inf(未触发)有专门语义,不能直接代入圆弧半径算三角函数。[S1][S2]

最后,测量时间与点云发布时间不一定相同;TF 的时间选择、数据缓存的新鲜度和多探头样本对齐都会影响障碍物的位置与可靠性。虚拟点更多只是视觉上更密集,不能据此提高对物体存在性的统计置信度。

3. 第二种用法:Collision Monitor 只需内部障碍点,不必发布点云

在移动机器人接近桌腿、墙边或行人时,有时业务需求很简单:附近危险区域有没有障碍?需要立即停车还是降低速度? 这与给全局路径规划器构建地图是两件事。

Nav2(ROS 2 导航框架)的 nav2_collision_monitor 提供这类近场碰撞监测。它可以订阅激光扫描、PointCloud2 或 Range。对 Range 类型输入,当前公开源码在 Range::getData() 中检查读数范围和时间有效性,根据 field_of_view、距离及 obstacles_angle 在圆弧上生成点,借助 TF 变换到机器人基坐标系,随后将这些点加入内部的 std::vector<Point>,供区域判断使用;这条路径并不要求先发布 PointCloud2 Topic。[S5][S6]

Nav2 文档中,obstacles_angle 表示圆弧相邻点的角度步长,常见默认值为 π/180 rad,即 1°。它与上节“最大弧长间距 15 cm”的方法不同:角度等间隔意味着距离越远,相邻点之间实际相隔越远;弧长间隔受限则会随距离变化调整分段数量。[S6]

内部点集随后会与机器人周围配置的区域进行几何判断:

机制 主要问题 处理结果
Stop 停车区内出现足够多的障碍点了吗? 阻止或归零速度命令
Slowdown 减速区内有障碍了吗? 按配置比例降低速度
Limit 是否需要限制线速度或角速度? 将速度限制在配置上限内
Approach 按当前运动预计接近碰撞了吗? 根据预测接近时间限制速度

区域可以是多边形或圆形,动作逻辑由 Collision Monitor 的配置确定;它通常位于速度命令链中,绕开普通规划路径对危险情况作快速处理。与之同一软件包中的 Collision Detector 则只报告检测结果,不修改速度。[S6]

一个特别重要的工程含义是:min_points 统计的是模型生成的点数,并不是独立测距次数。 假设一次 60° 的测距按约 1° 步长离散,便可能产生约六十个同源虚拟点。如果简单按“检测到四个点就停车”配置阈值,实际上可能只是同一个回波模型的四个点进入了危险区域,不能解释成四个独立传感器均确认有障碍。[S5][S6]

因此内部采样密度、区域形状、点数阈值和样本有效期必须一起理解;不能单靠增密提高避障可靠性。Nav2 官方也明确指出 Collision Monitor 只是运行在通用计算平台上的附加安全功能,不应将其等同于经过认证的硬件安全回路、急停或功能安全系统。[S6]

4. 第三种用法:Costmap 将测距的不确定性放进网格

4.1 为什么规划器更需要栅格,而不是一串圆弧点

碰撞监测只需要知道附近危险区域是否出现障碍;路径规划则需要比较不同路线上的可通行程度。Costmap(代价地图)将环境离散为许多二维栅格,每个单元保存相应的障碍或通行代价,供局部/全局规划器使用。障碍信息还可能经过 Inflation Layer(膨胀层)扩展安全距离。[S7][S8]

一种最直接的办法是先生成虚拟 PointCloud2,再交给支持点云的 Obstacle Layer,将点对应的栅格当作障碍物;但这种做法容易把“测距可能来自弧上任何方向”的方向不确定性压扁成“弧上多个位置都存在障碍”的强标记。

因此另一类办法是让 Costmap 直接读取 Range,并为视场中的栅格计算占用概率,而不是先画一条二值障碍弧线。ROS 1 的 range_sensor_layer 以及 ROS 2 Nav2 中的 nav2_costmap_2d::RangeSensorLayer 都提供了这种思路。[S7][S8][S9]

4.2 Range Sensor Layer 如何处理一条距离

以 Nav2 当前 range_sensor_layer.cpp 的机制说明。一条有效 Range 进入插件后,程序先依据 field_of_view 和 range 确定测量对应的前方区域,再结合 TF 将该区域放进 Costmap 使用的坐标系。随后遍历区域覆盖的栅格,按照栅格到探头的距离以及栅格偏离探头正前方的角度,计算测量对该栅格占用状态的支持程度;再与该格此前的占用概率组合。[S7]

可以直观理解为:探头测得 0.20 m 时,在测量距离附近、位于视场内的格子可能得到“更像有障碍物”的证据;靠近探头、尚未到达反射距离的格子可能得到“更像空闲”的证据;视场外没有被当前读数观测到的格子,不应因为这条读数被当作已测量区域。这些只是定性解释,具体概率要依赖实现的 sensor_model() 和参数。

在这里插入图片描述

Nav2 的源码使用 sensor_model(r, phi, theta),其中 r 是超声波测距,phi 是格子到探头的距离,theta 是格子方向相对于探头的偏角。update_cell() 从现有格子概率(先验)出发,依据传感器模型提供的证据计算新概率(后验),再写入代价图。直观地说,“此前认为有障碍的概率”和“这一帧测量提供了多强的证据”共同决定更新结果,不是每收到一个点就直接判为障碍。[S7]

经过概率更新后,插件还用阈值决定什么时候把栅格标记为空闲或占用。例如 Nav2 文档公开的 clear_threshold 默认值为 0.2、mark_threshold 默认值为 0.8;这两个参数是栅格判定阈值,不是超声波距离阈值。未越过阈值时,某次观测不一定改变最终的明确占用分类。[S8]

4.3 “没测到东西”能不能清除旧障碍

不能无条件认为“新读数等于最大量程,就说明扇形内所有格子可以清空”。在 Nav2 的 Range Sensor Layer 中,clear_on_max_reading 控制是否将最大量程读数作为清除证据,文档所列默认值为 false。此外,真正的更新效果还取决于传感器类型、输入合法性、TF、现有格子概率和 Costmap 的图层合成行为。[S7][S8]

这一点也体现了它与虚拟点云的区别:虚拟圆弧点往往强调可能有障碍的边界,而概率栅格既能表达占用证据,也能表达未观测、不确定或偏空闲的状态。但是概率模型不等于“真值”,其可靠性仍取决于探头物理特性、测量频率、更新模型以及对超时/无回波的处理。

4.4 直接 Range Layer 与 PointCloud2 → Obstacle Layer 不相同

两条路径最终都可能影响 Costmap,但含义不同:

  • Range → RangeSensorLayer:直接消费测距和视场角,对覆盖区域进行传感器概率建模;更容易保留单次测距的角度不确定性。
  • Range → 虚拟 PointCloud2 → ObstacleLayer:先人为离散成多个 XYZ,再把点作为障碍物观测;便于复用已有点云管线,但若不额外设计过滤、清除和证据策略,可能把模型假定点当成真实独立目标点。

Nav2 同时提供 Range Sensor、Obstacle、Voxel 和 Inflation 等不同 Costmap 插件。选哪条路径应由传感器物理语义和下游算法要求决定,而不能只因为“点云能被 Costmap 读取”就默认需要先生成点云。[S8][S10]

5. 三种使用方式的对比与选择

比较维度 虚拟 PointCloud2 Collision Monitor 内部点集 RangeSensorLayer → Costmap
典型输入 Range 或驱动给出的距离与视角 sensor_msgs/Range(也支持其他源) sensor_msgs/Range
核心处理 圆弧采样、TF、点集合封装 圆弧采样、TF、危险区域判定 视场覆盖、传感器概率模型、栅格更新
主要结果 发布 XYZ 点云消息 内部障碍点与停车/减速等动作 栅格占用/通行代价
必须发布 PointCloud2 吗 是,此路径的目标就是点云输出 不需要 不需要
对角度不确定性的处理 用多个虚拟点近似表示 使用点集进入区域判断 用视场与概率模型更新格子
主要用途 RViz 显示、点云生态接口 机器人近场碰撞监测 为路径规划提供障碍代价
典型风险 把模型点误当作真实物体表面 点数阈值受采样密度影响 模型参数或清除策略不当,导致残留/漏标
代表性公开实现 ROS 1 ros-range-to-pointcloud ROS 2 Nav2 nav2_collision_monitor ROS 2 Nav2 RangeSensorLayer;ROS 1 range_sensor_layer

这些方案的边界值得记住:

需要 RViz 显示或已有点云消费者时,选择 PointCloud2 表达方式较方便,但应明确虚拟点的来源与证据强度。需要对危险距离快速减速或停车时,可以让 Collision Monitor 直接消费 Range,但必须一起检查采样步长、检测区域和点数门槛。需要将超声波融入路径规划时,优先评估 Range Sensor Layer 的视场概率模型是否符合传感器及规划需求;若已有统一点云管线,也可评估 PointCloud2 经 Obstacle Layer 的路线,但不应默认两者语义等价。[S4][S5][S7][S8]

这三条处理链还可以并行存在。例如原始 Range 一路供碰撞监测,一路交给 Costmap 进行环境更新,同时转换出一份点云用于调试显示。其前提是下游知道哪些点属于同源虚拟采样,且不会错误地将同一测量在多个路径中当作相互独立的证据重复计数。

不同 ROS 发行版还需要分开理解:本文的 ros-range-to-pointcloud 与 ROS 1 的历史 range_sensor_layer 属于 ROS 1/Catkin 生态;Nav2 Collision Monitor 和 Nav2 Range Sensor Layer 属于 ROS 2/Nav2 生态。它们的基本几何思想可以互相借鉴,但节点接口、参数、TF 和插件机制不能直接混用。[S4][S5][S7][S9]

6. 总结:距离还是那一个距离,变化的是上层表示

超声波回波最终只贡献一条测距信息,虚拟圆弧点并不会自动增加传感器分辨率。固定 60° 圆弧、约 15 cm 最大采样间隔是一种可用的几何表达方式,其本质是把方向不确定的距离显示为多个候选位置;它不是超声波必选标准,更不是三维物体重建。

ROS 为这条数据提供了三个不同的使用方向:PointCloud2 负责把点表达出来,Collision Monitor 负责围绕危险区域做速度约束,Costmap 的 Range Sensor Layer 负责把视场内的观测转为可规划的栅格信息。三者关注的问题、内部假设和错误后果不同。工程上最重要的原则不是“多生成一些点”,而是保留原始测距语义、明确模型假设、正确处理时间与坐标系,并根据消费者的目标选择恰当的数据表示。

资料来源

[S1] ROS 1 sensor_msgs/Range 消息定义

  • 类型:ROS 官方消息定义
  • 版本:noetic-devel,访问日期 2026-10-18。
  • 链接:ROS 1 Range.msg。
  • 使用位置:第1节、2.5节(Range 测量语义与特殊值)
  • 支撑内容:解释单次距离在圆弧内有效的语义、range、field_of_view、min_range、max_range、时间戳,以及固定距离传感器的无穷大编码。

[S2] ROS 2 sensor_msgs/Range 消息定义

  • 类型:ROS 官方消息定义
  • 版本:rolling,访问日期 2026-10-18。
  • 链接:ROS 2 Range.msg。
  • 使用位置:第1节、2.5节(ROS 2 Range 对照)
  • 支撑内容:与 ROS 1 的距离/视场语义交叉核对,并说明当前 ROS 2 消费者的数据输入边界。

[S3] ROS 2 sensor_msgs/PointCloud2 消息定义

  • 类型:ROS 官方消息定义
  • 版本:rolling,访问日期 2026-10-18。
  • 链接:ROS 2 PointCloud2.msg。
  • 使用位置:第2.4节(PointCloud2 消息格式)
  • 支撑内容:说明 PointCloud2 是二进制点集合消息,并非点云几何重建算法。

[S4] ros-range-to-pointcloud 公开转换实现

  • 类型:开源源码
  • 版本:main,commit 07a9dbeae55ca36ac1266d07d61fde722686f6e9。
  • 链接:README;src/range_to_pointcloud.cpp。
  • 使用位置:第1节、2.2节、2.3节、2.4节及第5节(公开 ROS 1 转换示例)
  • 支撑内容:RangeToPointCloud::populateRangesToPC() 说明固定点数的视场采样、姿态变换和点云发布;该公开仓库不是文中固定 60° / 15 cm 几何示例的来源。

[S5] Nav2 Collision Monitor 的 Range 数据处理

  • 类型:Nav2 官方源码
  • 版本:main,commit cea9ed9d2e55b4e084be705a495a721368021f19。
  • 链接:nav2_collision_monitor/src/range.cpp。
  • 使用位置:第1节、2.3节、第3节和第5节(Collision Monitor Range 源)
  • 支撑内容:Range::getData()(部分 Nav2 版本中 API 名称不同)说明如何按视场与角度间隔生成内部点、过滤距离与时间并执行 TF 转换。

[S6] Nav2 Collision Monitor 官方指南

  • 类型:Nav2 官方文档
  • 版本:rolling,访问日期 2026-10-18。
  • 链接:Collision Monitor 说明;Collision Monitor 参数;使用教程。
  • 使用位置:第3节、第5节(监测动作与参数)
  • 支撑内容:核对 range 类型观察源、obstacles_angle、Stop/Slowdown/Limit/Approach、危险区域、点数门槛和安全功能边界。

[S7] Nav2 Range Sensor Layer 实现

  • 类型:Nav2 官方源码
  • 版本:main,commit cea9ed9d2e55b4e084be705a495a721368021f19。
  • 链接:nav2_costmap_2d/plugins/range_sensor_layer.cpp。
  • 使用位置:第1节、第4节、第5节(Nav2 概率层实现)
  • 支撑内容:processVariableRangeMsg()、updateCostmap()、sensor_model()、update_cell() 说明有效测距筛选、视场栅格遍历、概率更新和阈值合成。

[S8] Nav2 Range Sensor Layer 配置文档

  • 类型:Nav2 官方文档
  • 版本:rolling,访问日期 2026-10-18。
  • 链接:Range Sensor Parameters。
  • 使用位置:第4节、第5节(Range 插件配置)
  • 支撑内容:核对 topics、clear_threshold、mark_threshold、clear_on_max_reading、input_sensor_type 的职责与文档默认值。

[S9] ROS 1 range_sensor_layer 公开实现

  • 类型:开源源码
  • 版本:melodic,commit b4e17b4aedf1b64c7b8c5691e8aef8f139311815。
  • 链接:DLu/navigation_layers:range_sensor_layer.cpp。
  • 使用位置:第4节、第5节(ROS 1 早期 Range Layer 对照)
  • 支撑内容:说明 ROS 1 生态同样存在从 Range 到概率栅格的路径,区别于 ROS 2/Nav2 的具体插件接口。

[S10] Nav2 Costmap 概览

  • 类型:Nav2 官方文档
  • 版本:rolling,访问日期 2026-10-18。
  • 链接:Costmap 2D 与插件说明。
  • 使用位置:第4.1节、4.4节(Nav2 Costmap 图层)
  • 支撑内容:区分 Range Sensor、Obstacle、Voxel、Inflation 等图层职责,以及 PointCloud2 作为 Obstacle Layer 观测源的另一条处理路线。