ROG-Map: 一种高效的以机器人为中心的大场景高分辨率 LiDAR 运动规划网格地图
论文摘要
本文提出了一种名为 ROG-Map 的均匀基于网格的占用栅格地图(OGM),旨在解决激光雷达与 OGM 集成的挑战。该地图保持局部地图随机器人移动,实现高效的地图操作并降低大场景自主飞行的内存成本。主要创新点包括:
- 0 拷贝地图滑动策略:仅维护机器人周围的局部地图,适用于大场景任务。
- 增量障碍膨胀方法:显著降低了膨胀的计算成本,在公共数据集上优于最先进(SOTA)方法。
- 计算效率最大化:专注于避障的局部地图,追求计算效率。
1. 引言 (INTRODUCTION)
视觉传感器测量范围较短(约 35m),而激光雷达具有精确和远程探测能力(避开小障碍物和大场景感知)。为了避开小障碍物,分辨率足够高的 OGM 能够感知小障碍物,从而在复杂环境中实现导航和避障。充分利用激光雷达提供远程精确测量的能力,理想的激光雷达占用网格地图(OGM)有望保持高分辨率和高效率的远距离更新。
这一高要求带来了两个问题:地图更新和障碍物膨胀的计算负载显著增加。传统的基于八叉树和哈希表的 OGM 在激光雷达运动规划的实时性中遇到很大问题。但是基于均匀网格的方法有很高的计算效率,但是在大规模环境中由于其内存消耗变得不可行。
主要贡献
- ROG-Map:基于均匀网格的高效计算的 OGM 包。其中的 0 复制地图滑动策略使得有一个随机器人移动的局部地图,在大场景和高分辨率下运行。
- 新的障碍物膨胀方法:实现了 O(N) 的计算复杂度,复杂度更小,计算速度更快。
- 实验验证:在公共数据集上将 ROG-Map 与 SOTA 基线进行比较,展示了其计算和内存效率的优势。此外,ROG-Map 被集成到基于激光雷达的四旋翼中,进行广泛的真实世界测试,以验证其出色的真实世界性能。
- 开源实现:ROG-Map 作为 ROS 包实施,开源。
2. 相关工作 (RELATED WORKS)
A. 占据栅格地图 (Occupancy Grid Map)
现有主流实现占用地图的方法对比时间和空间复杂度。涉及基于八叉树、基于哈希表和均匀的基于网格的占用地图对比。
B. 障碍物膨胀 (Obstacle Inflation)
提出了一种新颖的增量更新方案,提出的方法在公共数据集上将遍历网格的数量减少 70%~97%,显著加快了障碍膨胀过程。
3. 占据栅格地图 (OCCUPANCY GRID MAP)
这里对基本的占据栅格地图是如何更新得到进行了基本公式理论的说明。
问题定义
当机器人的激光雷达(LiDAR)扫过周围环境时,我们如何通过数学计算来准确判断地图上的某一个小方块(栅格)里面到底有没有障碍物?(数学表达:以激光雷达测量数据为条件估计栅格被占概率)
第一步:第 k 次更新
系统会接收到激光雷达的位置 $x_{k}\in\mathbb{R}^{3}$ 及其扫描到的一组点云数据 $\mathcal{P}_{k}$。
- Hit(击中):如果激光雷达的某一条光束打在了某个栅格里,说明那里可能有障碍物,这被称为一次'击中'。
- Miss(穿过):如果光束穿过了某个栅格,说明那里是空的,这被称为一次'未击中'。
第二步:贝叶斯概率更新
因为传感器会有噪声,我们不能仅凭一次扫描就断定某个地方绝对有或绝对没有障碍物。我们需要结合历史数据来推测。假设这是一个马尔可夫过程,我们需要计算在已知前 $k$ 次历史测量数据的情况下,栅格 $n$ 被占据的概率,记作 $P_{1:k}(n)$。
经典的二元贝叶斯更新公式: $$ P_{1:k}(n)=\left[1+P\right]^{-1} $$ $$ P=\frac{P_{1:k}(n)}{1-P_{1:k}(n)}=\frac{1-P_{k}(n)}{P_{k}(n)}\frac{1-P_{1:k-1}(n)}{P_{1:k-1}(n)}\frac{P(n)}{1-P(n)} $$ 其中,$\frac{P_{1:k}(n)}{1-P_{1:k}(n)}$ 定义为几率。
- $P(n)$:没有任何测量时的先验概率。论文中通常假定为 0.5,表示在没有任何信息时,这个栅格有或没有障碍物的概率各占一半。
- $P_{1:k-1}(n)$:在第 k-1 次(上一次)更新后,栅格被占据的历史概率。
- $P_{k}(n)$:逆传感其模型:仅仅基于第 k 次(当前这次)扫描,栅格被占据的概率。
由于这个公式全是乘法和除法,如果经过成千上万次的雷达扫描,计算机处理浮点数乘法不仅非常慢,还容易导致数值下溢(数字小到计算机无法精确表示)。因此,我们需要引入'对数几率'。
第三步:对数几率(Log-odds)
对数几率定义为:计算其'几率'(即发生概率与不发生概率的比值 $\frac{P}{1-P}$),然后再两边同时取对数 $\log$,你会发现原本复杂的连乘,瞬间变成了极其简单的加法。
$$ log\frac{P_{1:k}(n)}{1-P_{1:k}(n)}=log\frac{P_{k}(n)}{1-P_{k}(n)}+log\frac{P_{1:k-1}(n)}{1-P_{1:k-1}(n)}+log\frac{1-P(n)}{P(n)} $$
对于第三项的 P(n) 为在没有雷达测量的先验概率,P(n)=0.5,则第三项为 0,(2) 可以简写成: $$ L_{1:k}(n)=L_{1:k-1}(n)+L_{k}(n) $$
第 k 次更新后的地图状态,就等于上一时刻的地图状态($L_{1:k-1}$),加上当前这次观测带来的状态变化($L_{k}$)!而且这种对数几率是双射的,所以地图里只需要保存 $L_{1:k}(n)$ 的值就可以了。这是一种迭代用激光雷达测量数据进行更新的占用栅格地图。
$$ L_{1:k}=log\frac{P_{1:k}(n)}{1-P_{1:k}(n)}=log\frac{x}{1-x} $$ 其中,$x\in (0,1), L_{1:k-1}\in (-\infty ,+\infty )$。
第四步:计算当前观测的增量
$$ L_{k}(n)=n_{hit}\cdot l_{hit}+n_{miss}\cdot l_{miss} $$
- $l_{hit}$ 和 $l_{miss}$:分别是击中和未击中对应的对数几率常数。需要注意的是,$l_{miss}$ 是一个负值,因为当激光束穿过栅格时,该栅格存在障碍物的概率会降低。
- $n_{hit}$ 和 $n_{miss}$:分别是第 $k$ 次更新时,该栅格被击中和被穿过的次数。
第五步:应对动态环境的截断策略
假设一辆车停在一个栅格里很久,这个栅格的占据概率会被累加到一个天文数字。如果车开走了,我们要花同样长的时间(即接收很多次负的 $l_{miss}$)才能把它的概率降下来,这在动态环境中是致命的。
为了保证地图对静态和动态环境的适应性,论文采用了截断更新策略:限制它的上下界 $$ L_{1:k}(n)=\max(\min(L_{t},l_{max}),l_{min}) $$ 通过设定对数几率的上限 $l_{max}$ 和下限 $l_{min}$,我们确保概率既不会无限大也不会无限小。这样哪怕障碍物突然移走,地图也能迅速更新。
论文代码对应参数如下,结合上图和公式一起理解。
# Probabilistc update 概率更新与射线投射。通过多次观测累积概率,处理传感器噪声
raycasting:
enable: true
batch_update_size: 1
local_update_box: [ 50,50,5 ] # 局部更新范围 50×50×5 米
ray_range: [ 0.0, 999 ] # 射线投射范围:最小 0 米,最大 999 米
p_min: 0.12 # 最小占据概率(下限)
p_miss: 0.49 # 射线未命中时的概率(自由空间)
p_free: 0.499 # 自由空间的概率阈值
p_occ: 0.85 # 占据空间的概率阈值
p_hit: 0.9 # 射线命中时的概率
p_max: 0.98 # 最大占据概率(上限)
p_min与p_max:占据概率下界与上界的作用:防止概率无限接近 0 或 1。一个体素连续多次被看成 free,不会变成绝对 0,只会降到0.12;一个体素连续多次被看成 occupied,不会变成绝对 1,只会升到0.97。这样做的好处:避免一次误测后永远'翻不了身',给后续观测留有修正空间。p_occ和p_free是状态判定阈值。p_hit和p_miss是观测更新参数。p_min和p_max是概率截断边界。
第六步:最终状态的判定
论文根据设定的阈值来判断栅格状态:
- KnownFree(已知空闲):$l_{min}\le L(n)<l_{free}$
- Occupied(已被占据):$l_{occ}\le L(n)\le l_{max}$
- Unknown(未知区域):如果不符合上面两种情况,则是未知状态。
在这里,论文还引出了两个极其重要的概念,这是为了后续'障碍物膨胀'算法做铺垫的:
- 上升栅格 (Rising Grid, RG):状态从 Unknown 或 KnownFree 变成 Occupied 的栅格。
- 下降栅格 (Falling Grid, FG):状态从 Occupied 变成 KnownFree 或 Unknown 的栅格。
问题解答
问题一: $L_{1:k}(n)$ 计算的是单个立方体栅格,还是一个区域? 答案是:它计算的是一个具体的、单一的 3D 立方体栅格(Voxel)被占据的概率。 $L_{1:k}(n)$ 描述的是仅仅针对索引为 n 的这一个特定小方块,它里面存在障碍物的对数几率。
问题二: $P(n|x_{1:k}, \mathcal{P}{1:k})$ 中的 1:k 具体指代什么? 答案是:$1:k$ 指代的是从机器人启动时的第 1 次扫描,一直到当前时刻的第 $k$ 次扫描的'全部历史测量数据'。 所以,$P(n|x{1:k}, \mathcal{P}_{1:k})$ 翻译成大白话就是:'在综合考虑了从一开始到现在(第 $k$ 次)所有的雷达位置和扫描到的所有点云数据之后,栅格 $n$ 里面有障碍物的概率是多少。'
4. ROG-Map 核心创新
这个局部滑动地图在导航中的用处是什么,规划用的是传感器实时在线的每一帧数据,直接转为占栅格使用,然后更新范围内进行障碍物膨胀。
解答:
- '为什么不直接把当前帧点云转成栅格,而要搞复杂的概率更新'的疑问
如果只用当前一帧点云,地图会存在严重的不确定性和噪声问题:
- 滤除传感器噪声:LiDAR 在远距离或面对特定材质时会有噪点。概率更新(Bayesian updating)能通过多帧观测,确认一个栅格是真的被占据还是仅仅是噪点。
- 处理极细小的障碍物:论文中特别强调了对 3mm 细金属网的检测。单帧 LiDAR 扫描可能由于光束发散或采样频率原因,在某一帧漏掉这种细小障碍物。通过历史概率累加,地图能'记住'这些细线,保证导航安全。
- 明确'自由空间'与'未知空间':仅仅将点云体素化只能告诉你哪里'有'障碍。而 ROG-Map 通过 Ray casting(射线投射)能明确告诉你哪里是'确定没障碍'的自由区域,哪里是'没看过的'未知区域。这对于高精度的路径搜索至关重要。
主要两个部分:地图滑动和地图更新(概率更新和递增障碍膨胀)。
A. Robocentric Local Maps
解决了一个极度现实的问题:无人机内存是有限的,但现实世界是无限的。如果无人机飞出几公里,把所有扫描过的地方都存在内存里,电脑瞬间就会崩溃。为了解决这个问题,作者提出了一种以机器人为中心的局部地图(Robocentric Local Maps)。简单来说,就像是机器人在走'跑步机',无论它怎么飞,地图的核心始终罩在它周围,跑出这个罩子的旧地图就被直接扔掉,前面新探测到的地图就补进来。
坐标转换流程:
- 全局坐标->全局索引:$i_k^g = \text{round}(p_k / r)$
- 全局索引->中间索引:$i_k^t = i_k^g \pmod{s_k}$,对地图长度进行取余运算。利用了循环缓冲区(Circular Buffer)的概念。
- 中间索引->局部索引:$i_k^l = \text{normalize}(i_k^t)$,归一化,在 C++ 等编程语言中,对负数取模得到的结果可能是负的。但是数组的下标不能是负数!所以论文引入了一个 normalize 函数,它的唯一目的就是把这些带负号的索引'掰正',强行拉回到 $[0, s_k-1]$ 的合法区间内。
经过这两步,我们得到了局部索引 (Local Index) $i^l$。无论机器人在现实世界飞到了哪里,它对应的栅格下标永远被死死限制在 0 到 $s-1$ 之间。
转为 1D 内存地址: $$ toAddress(i^l) = i_x^l \cdot s_y \cdot s_z + i_y^l \cdot s_z + i_z^l $$
- 含义推导:这是计算机图形学和三维数组的标配展平公式。
- 物理意义:通过这个公式,任何一个 3D 局部坐标,都能找到内存数组中唯一对应的一个格子。你要找第 $i_x^l$ 栋、第 $i_y^l$ 层、第 $i_z^l$ 个房间,就先跳过前面所有的整栋楼,再跳过当前楼栋下面的所有整层,最后加上当前层的房间号。
这就好比是一栋大楼:$s_z$ 是每层楼的房间数,$s_y$ 是每栋楼的层数,$s_x$ 是小区的楼栋数。
最核心的魔法:Zero-copy(零拷贝)滑动策略 传统局部地图更新方法:机器人往前 1m,就把局部地图清空,然后更新局部范围内的栅格,并遍历每一个栅格进行障碍物膨胀。
- 在内存的物理层面,数据格子永远不移动位置。
- 当机器人移动导致局部地图的'罩子'发生偏移时(移动距离大于阈值 $d$),一些曾经在无人机背后的旧区域就掉出了地图范围。
- 系统只需要把这些被淘汰的旧内存地址清零(重置)。
- 因为是循环取模(就像表盘转了一圈又回到起点),这些清零后的旧内存空间,马上就可以用来存储无人机正前方刚刚探测到的新区域。
总结一下: 这套公式本质上建立了一个'无限复用'的内存机制。它通过极低成本的数学取模运算,直接算出任何空间点在内存中的固定位置,从而免去了在内存中大规模搬运数据的昂贵开销。
B. Probabilistic Update and Incremental Inflation
A 解决的是局部地图如何在内存中生成和滑动的问题。而我们现在要讲的 IV.B 节(Probabilistic Update and Incremental Inflation),解决的是在这个已经建好的局部地图里,每次雷达扫到新数据时,如何快速更新概率,并把障碍物'变胖'(膨胀)以保证无人机飞行安全的问题。
第一步:'批量'处理光线投射
传统的笨办法:每一根激光束穿过一个栅格,就立刻去更新一次那个栅格的概率。但是一帧点云有成千上万个点,很多光束会穿过同一个空气栅格。如果每次都去更新对数几率,计算量会非常大。
论文使用了快速体素遍历算法(Fast Voxel Traversal Algorithm)进行光线投射,并引入了一个缓存队列 $\mathcal{C}$。
这样,后续更新时,不管这个栅格被多少条光线穿过,每一帧里它只需要进行一次概率更新的数学计算,大大节省了算力。这体现在算法的 $\mathcal{C} = \text{Raycasting}(x_k, \mathcal{P}k)$ 这一步,并在缓存里直接统计好每个栅格被 Hit 了几次($c{hit}$),被 Miss 了几次($c_{miss}$)。
算法先把当前这一帧所有光束穿过的栅格都找出来,存进 $\mathcal{C}$ 中。
第二步:快速障碍物膨胀
传统的膨胀方法每次雷达扫描后,都要把局部地图里所有的栅格遍历一遍,非常耗时(复杂度极高)。ROG-Map 提出了一种时间复杂度永远是 O(n) 的极速膨胀法($n$ 是状态发生改变的栅格数量)。
-
核心变量:投票计数器 (
inflationCounter) 论文给每一个栅格都配备了一个整数型的计数器,初始值为 0。这个计数器的物理意义是:我的周围(膨胀半径内),有几个真正的障碍物栅格?只要这个计数器 >=1,就说明我挨着障碍物,我必须把自己标记为膨胀占据(InflatedOccupied)。 -
触发机制:只关注'变心'的栅格 (Rising / Falling) 当系统检测到一个栅格 n 变成了上升栅格 (RG) 时:找到栅格 n 的每一个邻居的具体位置:$n_{temp} = n + p$,给所有邻居的计数器加一:$n_{temp}.inflationCounter += 1$。 相反,当栅格 n 变成了下降栅格 (FG) 时:找到栅格 n 的每一个邻居的具体位置:$n_{temp} = n + p$,给所有邻居的计数器减一:$n_{temp}.inflationCounter -= 1$。
总结:ROG-Map 引入的这个加减计数器(Voting System)机制,让每一个栅格的膨胀状态变得完全独立解耦:一个栅格不管周围发生了什么,只要它自己的 inflationCounter 大于 0,它就是危险区域(InflatedOccupied);一旦减到 0,它就立刻变回安全区域。所有操作只有简单的加法和减法,而且只针对极少数发生状态改变的栅格(RG 和 FG)进行操作,这就是它能在算力捉襟见肘的无人机上跑到 50 Hz(仅仅耗时不到 6 毫秒)的终极秘密。
5. 代码实现
以下 3 个函数:负责接收里程计、接收点云、缓存数据,并在定时器线程中触发真正的地图更新。
// 位姿接收:更新机器人状态,把 world -> drone 广播出去。
void ROGMap::odomCallback(const nav_msgs::OdometryConstPtr& odom_msg) {
// 把 ROS 的 odom 消息,转成 ROGMap 内部使用的机器人状态格式。
updateRobotState(std::make_pair(
Vec3f(odom_msg->pose.pose.position.x, odom_msg->pose.pose.position.y, odom_msg->pose.pose.position.z),
Quatf(odom_msg->pose.pose.orientation.w, odom_msg->pose.pose.orientation.x, odom_msg->pose.pose.orientation.y, odom_msg->pose.pose.orientation.z)));
// 它不直接更新地图,只是广播坐标关系。
// 定义一个 TF 变换:父坐标系:world,子坐标系:drone,表示"drone 在 world 下的位置和姿态"。
static tf2_ros::TransformBroadcaster br_map_ego;
geometry_msgs::TransformStamped transformStamped;
transformStamped.header.stamp = ros::Time::now();
transformStamped.header.frame_id = "world";
transformStamped.child_frame_id = "drone";
transformStamped.transform.translation.x = odom_msg->pose.pose.position.x;
transformStamped.transform.translation.y = odom_msg->pose.pose.position.y;
transformStamped.transform.translation.z = odom_msg->pose.pose.position.z;
transformStamped.transform.rotation.x = odom_msg->pose.pose.orientation.x;
transformStamped.transform.rotation.y = odom_msg->pose.pose.orientation.y;
transformStamped.transform.rotation.z = odom_msg->pose.pose.orientation.z;
transformStamped.transform.rotation.w = odom_msg->pose.pose.orientation.w;
br_map_ego.sendTransform(transformStamped);
}
// 点云接收
// 收到点云后,不立即更新地图,先检查里程计超时,再把点云和当时位姿缓存起来
void ROGMap::cloudCallback(const sensor_msgs::PointCloud2ConstPtr& cloud_msg) {
std::thread::id thread_id_A = std::this_thread::get_id();
if (!robot_state_.rcv) { return; }
double cbk_t = ros::Time::now().toSec();
( - robot_state_.rcv_time > cfg_.odom_timeout) {
std::cout << << std::endl;
;
}
PointCloud temp_pc;
pcl::(*cloud_msg, temp_pc);
rc_.updete_lock.();
rc_.pc = temp_pc;
rc_.pc_pose = std::(robot_state_.p, robot_state_.q);
rc_.unfinished_frame_cnt++;
map_empty_ = ;
rc_.updete_lock.();
}
{
(map_empty_) { ; }
(rc_.unfinished_frame_cnt == ) { ; }
(rc_.unfinished_frame_cnt > ) {
std::cout << << std::endl;
}
PointCloud temp_pc;
Pose temp_pose;
rc_.updete_lock.();
temp_pc = rc_.pc;
temp_pose = rc_.pc_pose;
rc_.unfinished_frame_cnt = ;
rc_.updete_lock.();
(temp_pc, temp_pose);
(time_log_file_);
}
这段代码的架构思想很好:
- 里程计和点云解耦:
odomCallback()只更新状态,cloudCallback()只接收点云。模块职责清晰,不把重计算放到传感器回调里面。 - 点云接收和地图更新解耦:点云回调只缓存,定时器线程才真正更新。防止 ROS 回调阻塞,地图更新频率可控。
- 只处理最新帧:强实时。可能会丢帧,当传感器频率大于定时器回调函数时。
一个可能常用的小建模计算方程:代码中,关于对起点到射线终点的这条射线进行范围限制裁减时,使用了标准的射线方程进行计算裁减后的点 p。后续缓存 hit, miss 次数,以及进行对数几率累加,更新体素的状态代码体现出的 ProbMap 设计思想:
- 先缓存,后统一更新。不是一边 raycast 一边立刻改
occupancy_buffer_,而是先统计:operation_cnt,hit_cnt,update_cache_id_g(记录待更新格子的全局索引)。最后统一对一帧点云每个位置对应体素进行对数几率计算累加,并更新体素状态。 - hit 优先于 miss。只要
hit_cnt > 0,就按 hit 更新。 - 使用 log-odds 而不是直接概率。使用标准的 log-odds 累积更新。加法形式简单高效。
6. 数据流架构
固定大小局部地图 + 滑动窗口 + 环形内存复用
优点:
- 内存固定,不会无限增长
- 更新速度快,只有移动大于一个阈值,才会更新局部地图,内存复用,不是每一帧重新构图
- 很适合无人机/机器人实时导航
- 可以始终只维护机器人周围一小块最有用的地图
整体构图流程:
- ProbMap:主地图管理者,它负责:主概率栅格 occupancy_buffer_,射线更新,子地图初始化,滑动地图管理,虚拟地面/天花板索引化。
- InfMap:派生的膨胀地图,用于安全碰撞检测/规划。
- FreeCntMap:frontier 提取辅助图,用于探索或边界统计。
- ESDFMap:距离场图,用于距离查询、优化等。
7. RViz 可视化说明
- 橙色框:Local Map Range,局部地图边界范围。就是论文中对应可视化额定那个滑动局部地图概念。最重要的框,表示局部滑动地图。
- 紫色框:可视化范围,可视化橙色框(局部地图)内想要可视化的膨胀障碍物地图范围。局部地图内的局部可视化范围。仅用于可视化膨胀障碍物。不参与代码逻辑。
- 绿色框:由这一帧裁减的点构成的本帧更新对数几率体素的范围,是一个在动态调整的范围。
- 淡蓝色框:A定义的搜索框,和局部边界橙色框大小一样,但是范围不一样。由起点和终点唯一确定,不滑动。作用:用于在 A搜索的主循环中,判断扩展点的邻居点时都在 A*搜索地图区域内。
- 深蓝色框:esdf 图。
8. 补充理解
updateProbMap()函数是按定时器 1000hz 频率回调运行,如果 batch_update_size: 1,那对每一帧点云都会进行对数几率(概率)更新,但是地图滑动,只有当第一帧和无人机离当前局部地图距离超过一定阈值,才会滑动。- 由于
updateLocalBox(pos)是为后续raycastProcess(cloud, pos);计算raycast_data_.local_update_box_max。绿框不是 local_update_box,而是 cache_box。绿框大小是 cache_box 在 raycastProcess 里动态构建:
// 初始化为机器人坐标(最小的点)
raycast_data_.cache_box_min = cur_odom;
raycast_data_.cache_box_max = cur_odom;
// 遍历每个处理后的点 p(已经过所有裁剪),不断扩展包围盒
raycast_data_.cache_box_min = raycast_data_.cache_box_min.cwiseMin(p);
raycast_data_.cache_box_max = raycast_data_.cache_box_max.cwiseMax(p);
所以绿框 = 这一帧所有有效射线终点的包围盒,其大小受以下约束:
- raycast_range_max(球形截断)
- local_update_box: [50,50,5](AABB 截断)
- virtual_ceiling/ground_height(高度截断)
- 点云实际分布(最终决定因素)
| 变量 | rviz 框颜色 | 含义 | 大小如何确定 |
|---|---|---|---|
| local_map_bound_min/max_d_ | 橙色框 | 整个滑动的局部地图 | map_size 参数 |
| local_update_box_min/max | 不可可视化 | raycast AABB 包围盒裁剪范围 | local_update_box: [50,50,5] |
| cache_box_min/max | 绿色框 | 本帧实际更新体素对数几率格子覆盖范围 | 动态扩展,取决于本帧点云 |


