ROG-Map 技术解析：面向大场景高分辨率激光雷达运动规划的机器人中心占据栅格地图

项目名称： ROG-Map（Robocentric Occupancy Grid Map）

作者： Yunfan Ren, Yixi Cai, Fangcheng Zhu, Siqi Liang, Fu Zhang

录用会议： IEEE/RSJ International Conference on Intelligent Robots and Systems（IROS 2024）

项目地址： https://github.com/hku-mars/ROG-Map

论文地址： https://arxiv.org/abs/2302.14819

论文 PDF： https://arxiv.org/pdf/2302.14819

开源协议： GPL-3.0

ROS2 版本： 已集成到 HKU MaRS 的 SUPER 无人机导航系统中

1. 项目简介

ROG-Map 是香港大学 MaRS 实验室开发的三维占据栅格地图，主要用于激光雷达机器人运动规划。ROG 是 Robocentric Occupancy Grid 的缩写，可译为"机器人中心占据栅格地图"。

它没有永久保存整个环境，而是在机器人周围维护一张固定尺寸的局部三维地图。机器人移动后，地图窗口跟随机器人滑动，已经离开窗口的区域被清除，新进入窗口的区域复用原有内存。

ROG-Map 重点解决以下矛盾：

激光雷达量程远、点数多，需要处理大量射线；
运动规划需要高分辨率地图，以检测细杆、树枝和金属网等小障碍物；
三维均匀栅格的内存会随地图尺寸和分辨率快速增长；
障碍物膨胀需要频繁遍历邻域，容易成为实时建图的主要开销；
机载计算平台的 CPU 和内存资源有限。

ROG-Map 的基本思路是：

使用均匀栅格，保留严格的常数时间查询；
使用机器人中心局部窗口，将内存限制在固定范围内；
使用三维循环缓冲区，实现零拷贝地图滑动；
只处理本帧发生状态变化的栅格；
使用邻域计数器增量维护障碍物膨胀结果；
在细分辨率概率地图和较粗分辨率膨胀地图之间建立计数关系。

图 1　搭载 Livox Mid360 的无人机在细金属网环境中自主飞行，右上为 ROG-Map 占据栅格，右下为宽度约 3 mm 的金属网。

图 1 体现了高分辨率三维占据地图对小障碍物检测的重要性。论文中的金属丝宽度只有约 3 mm，虽然体素分辨率为 0.05 m，但激光点落入相应体素后，仍可以在占据地图中形成连续障碍结构，供局部规划器进行碰撞检测。

2. ROG-Map 不是什么

ROG-Map 容易与 SLAM、点云地图或完整导航系统混淆，需要先明确其边界。

2.1 不是 SLAM

ROG-Map 不负责：

激光雷达与 IMU 融合；
点云去畸变；
激光里程计；
点云配准；
回环检测；
全局位姿图优化。

实际使用时，需要由 FAST-LIO2、LIO-SAM、Point-LIO 或其他定位模块提供：

激光雷达位姿；
已经变换到统一世界坐标系下的配准点云。

2.2 不是路径规划器

ROG-Map 本身不负责生成最终轨迹。仓库中的 A* 和 RRT* 只是地图查询接口的示例。

它可以为以下算法提供环境约束：

A*、Dijkstra、JPS；
RRT、RRT*、Informed RRT*；
安全飞行走廊生成；
B 样条轨迹优化；
MPC、MPPI 等局部规划器；
视点规划和 Frontier 探索。

2.3 不是永久全局地图

局部窗口之外的数据会被清除。因此它适合实时避障和局部运动规划，不适合直接替代永久保存全局环境的 OctoMap、TSDF 地图或稀疏全局地图。

3. 论文解决的核心问题

论文所处理的技术矛盾可以概括为：

如何在有限机载计算资源下，同时获得高分辨率、长距离激光雷达更新、快速随机查询、实时障碍物膨胀和可控内存占用。

传统三维占据地图通常分为三类。

地图结构	单次查询复杂度	内存特点	主要问题
Octree	通常为 O ( log ⁡ n ) O(\log n) O(logn)	可压缩空旷区域，内存较省	大量射线更新和随机查询时树遍历开销明显
Hash Map	平均 O ( 1 ) O(1) O(1)，最坏 O ( n ) O(n) O(n)	按需分配	高分辨率稠密地图中哈希冲突和缓存不命中增多
Uniform Grid	严格 O ( 1 ) O(1) O(1)	连续数组，缓存友好	内存随地图物理尺寸快速增长

设地图物理尺寸为 L x × L y × L z L_x \times L_y \times L_z Lx×Ly×Lz，分辨率为 r r r，体素数量为：

N = L x r ⋅ L y r ⋅ L z r N = \frac{L_x}{r} \cdot \frac{L_y}{r} \cdot \frac{L_z}{r} N=rLx⋅rLy⋅rLz

因此空间消耗近似满足：

M ∝ L x L y L z r 3 M \propto \frac{L_x L_y L_z}{r^3} M∝r3LxLyLz

分辨率从 0.10 m 提高到 0.05 m 后，每个方向的栅格数量增加一倍，三维体素总数增加到原来的 8 倍。

例如，论文实飞使用的局部地图为：

30 m × 30 m × 6 m 30\ \text{m} \times 30\ \text{m} \times 6\ \text{m} 30 m×30 m×6 m

分辨率为：

r = 0.05 m r = 0.05\ \text{m} r=0.05 m

对应的体素数量为：

N = 600 × 600 × 120 = 43 200 000 N = 600 \times 600 \times 120 =43\,200\,000 N=600×600×120=43200000

这还只是单张地图的体素数量。实际代码还要维护概率值、膨胀计数器、未知状态计数器、Frontier、ESDF 和可视化缓存，因此内存远大于单个字节数组。

ROG-Map 没有放弃 Uniform Grid，而是通过固定大小局部窗口解决其内存随场景无限增长的问题。

4. 论文主要贡献

论文贡献可以归纳为四点。

4.1 机器人中心均匀栅格地图

使用连续数组保存局部三维均匀栅格，所有点查询和地址计算保持 O ( 1 ) O(1) O(1)。

4.2 三维零拷贝滑动

使用三维循环缓冲区，使地图中心移动时不需要整体复制重叠区域，只清理滑出窗口的内存条带。

4.3 基于状态变化的增量障碍物膨胀

检测由非占据变为占据的 Rising Grid，以及由占据变为非占据的 Falling Grid，只更新这些栅格的邻域计数器。

4.4 完整机载系统验证

论文不只进行了离线数据集测试，还把 ROG-Map 集成到无人机系统中，在细金属网和大范围巡检场景中完成实飞。

5. 系统输入、输出和处理流程

5.1 输入

一次地图更新需要：

激光雷达在世界坐标系中的位置；
已经配准到世界坐标系下的点云。

第 k k k 帧输入可表示为：

x k ∈ R 3 \mathbf{x}_k \in \mathbb{R}^3 xk∈R3

以及：

P k = { p k , 1 , p k , 2 , ... , p k , m } \mathcal{P}k = \left\{ \mathbf{p}{k,1}, \mathbf{p}{k,2}, \ldots, \mathbf{p}{k,m} \right\} Pk={pk,1,pk,2,...,pk,m}

其中 x k \mathbf{x}_k xk 是激光雷达位置， P k \mathcal{P}_k Pk 是当前点云。

5.2 输出

当前开源版本可以提供：

ProbMap：概率占据地图；
InfMap：障碍物膨胀地图；
Unknown 膨胀结果；
增量 Frontier；
局部滑动 ESDF；
点状态查询；
线段碰撞查询；
三维包围盒查询；
A* 和 RRT* 示例；
RViz 可视化结果。

5.3 总体流程

text 复制代码

激光雷达原始点云
        │
        ▼
激光惯性里程计 / SLAM
        │
        ├──────────────► 机器人位姿
        │
        ▼
世界坐标系配准点云
        │
        ▼
距离、强度和时间降采样
        │
        ▼
检查局部地图是否需要滑动
        │
        ▼
清理滑出窗口的循环缓冲区槽位
        │
        ▼
三维快速体素射线遍历
        │
        ▼
缓存每个体素的 Hit / Miss 次数
        │
        ▼
批量更新 Log-Odds
        │
        ▼
检测 Rising Grid / Falling Grid
        │
        ▼
更新多分辨率计数与膨胀计数器
        │
        ├────────► Frontier
        ├────────► ESDF
        └────────► A* / RRT* / 轨迹优化 / 局部避障

6. 项目代码结构

仓库主要由地图核心、ROS 封装和示例程序组成。

text 复制代码

ROG-Map
├── rog_map
│   ├── include/rog_map
│   │   ├── rog_map.h
│   │   ├── prob_map.h
│   │   ├── inf_map.h
│   │   ├── free_cnt_map.h
│   │   ├── esdf_map.h
│   │   └── rog_map_core
│   ├── src/rog_map
│   │   ├── rog_map.cpp
│   │   ├── prob_map.cpp
│   │   ├── inf_map.cpp
│   │   ├── counter_map.cpp
│   │   ├── sliding_map.cpp
│   │   └── esdf_map.cpp
│   ├── config
│   ├── CMakeLists.txt
│   └── package.xml
├── examples
│   ├── rog_map_example
│   │   ├── Apps
│   │   ├── config
│   │   ├── launch
│   │   └── PCD
│   └── MARSIM
├── misc
├── scripts
└── README.md

核心模块作用如下。

模块	作用
`SlidingMap`	坐标离散化、循环索引、地图滑动和越界内存清理
`ProbMap`	射线遍历、Hit/Miss 缓存、Log-Odds 更新和基础状态维护
`CounterMap`	在不同分辨率地图之间维护 Occupied、Unknown 等子栅格计数
`InfMap`	维护占据区域和未知区域的增量膨胀状态
`ESDFMap`	维护机器人附近的局部欧氏距离场
`ROGMap`	ROS 订阅、参数读取、可视化、统一地图更新和上层查询接口

仓库 README 将当前版本的主要功能概括为：

zero-copy map sliding；
incremental obstacle inflation；
counter-based multi-resolution map；
incremental Frontier generation；
sliding ESDF map generation。

7. 占据概率更新

7.1 Hit 与 Miss

一条激光束从传感器位置指向测量点：

r ( t ) = x k + t ( p k , j − x k ) , t ∈ $0 , 1$ \mathbf{r}(t) =\mathbf{x}k + t \left(\mathbf{p}{k,j} -\mathbf{x}_k \right) ,\qquad t \in $0,1$ r(t)=xk+t(pk,j−xk),t∈ $0,1$

射线终点所在栅格记为 Hit，射线经过的中间栅格记为 Miss。

Hit：说明栅格更可能被占据；
Miss：说明激光束穿过栅格，栅格更可能为空闲。

ROG-Map 使用 Amanatides-Woo 快速体素遍历算法依次访问射线穿过的三维栅格。

7.2 贝叶斯递推

设栅格 n n n 在第 k k k 次更新后的占据概率为：

P 1 : k ( n ) = P ( n ∣ x 1 : k , P 1 : k ) P_{1:k}(n) =P \left( n \mid \mathbf{x}{1:k}, \mathcal{P}{1:k} \right) P1:k(n)=P(n∣x1:k,P1:k)

在马尔可夫假设下，论文中的递推形式为：

P 1 : k ( n ) = $1 + 1 - P k ( n ) P k ( n ) \cdot 1 - P 1 : k - 1 ( n ) P 1 : k - 1 ( n ) \cdot P ( n ) 1 - P ( n )$ − 1 P_{1:k}(n) =\left $1 + \\frac{1-P_k(n)}{P_k(n)} \\cdot \\frac{1-P_{1:k-1}(n)}{P_{1:k-1}(n)} \\cdot \\frac{P(n)}{1-P(n)} \\right$ ^{-1} P1:k(n)= $1+Pk(n)1-Pk(n)\cdotP1:k-1(n)1-P1:k-1(n)\cdot1-P(n)P(n)$ −1

其中 P ( n ) P(n) P(n) 是先验占据概率，通常取：

P ( n ) = 0.5 P(n)=0.5 P(n)=0.5

7.3 Log-Odds 表示

直接在概率空间中连续执行乘除容易出现数值问题。ROG-Map 使用 Log-Odds：

L ( ⋅ ) ( n ) = log ⁡ ( P ( ⋅ ) ( n ) 1 − P ( ⋅ ) ( n ) ) L_{(\cdot)}(n) =\log \left( \frac{P_{(\cdot)}(n)} {1-P_{(\cdot)}(n)} \right) L(⋅)(n)=log(1−P(⋅)(n)P(⋅)(n))

贝叶斯递推可以化为加法：

L 1 : k ( n ) = L 1 : k − 1 ( n ) + L k ( n ) L_{1:k}(n) =L_{1:k-1}(n) + L_k(n) L1:k(n)=L1:k−1(n)+Lk(n)

设本帧中该栅格被命中的次数为 n hit n_{\text{hit}} nhit，被射线穿过的次数为 n miss n_{\text{miss}} nmiss，则：

L k ( n ) = n hit l hit + n miss l miss L_k(n) =n_{\text{hit}} l_{\text{hit}} + n_{\text{miss}} l_{\text{miss}} Lk(n)=nhitlhit+nmisslmiss

其中：

l hit = log ⁡ ( p hit 1 − p hit ) l_{\text{hit}} =\log \left( \frac{p_{\text{hit}}} {1-p_{\text{hit}}} \right) lhit=log(1−phitphit)

l miss = log ⁡ ( p miss 1 − p miss ) l_{\text{miss}} =\log \left( \frac{p_{\text{miss}}} {1-p_{\text{miss}}} \right) lmiss=log(1−pmisspmiss)

一般设置：

p hit > 0.5 p_{\text{hit}} > 0.5 phit>0.5

p miss < 0.5 p_{\text{miss}} < 0.5 pmiss<0.5

因此：

l hit > 0 l_{\text{hit}} > 0 lhit>0

l miss < 0 l_{\text{miss}} < 0 lmiss<0

Hit 会增加占据置信度，Miss 会降低占据置信度。

7.4 概率截断

若一个障碍物长期被命中，Log-Odds 会不断增大。障碍物移走后，需要大量 Miss 才能恢复为空闲，这会导致动态障碍残影。

ROG-Map 对 Log-Odds 设置上下限：

L t = L 1 : k − 1 ( n ) + L k ( n ) L_t =L_{1:k-1}(n) + L_k(n) Lt=L1:k−1(n)+Lk(n)

L 1 : k ( n ) = max ⁡ ( min ⁡ ( L t , l max ⁡ ) , l min ⁡ ) L_{1:k}(n) =\max \left( \min \left( L_t, l_{\max} \right), l_{\min} \right) L1:k(n)=max(min(Lt,lmax),lmin)

这相当于限制地图对历史观测的记忆强度，使地图能够适应一定程度的动态变化。

7.5 三类栅格状态

ROG-Map 将栅格划分为 KnownFree、Occupied 和 Unknown。

state ⁡ ( n ) = { KnownFree , l min ⁡ ≤ L ( n ) < l free Occupied , l occ ≤ L ( n ) ≤ l max ⁡ Unknown , 其他情况 \operatorname{state}(n) =\begin{cases} \text{KnownFree}, & l_{\min} \leq L(n) < l_{\text{free}} \\ $4pt$ \text{Occupied}, & l_{\text{occ}} \leq L(n) \leq l_{\max} \\ $4pt$ \text{Unknown}, & \text{其他情况} \end{cases} state(n)=⎩ ⎨ ⎧KnownFree,Occupied,Unknown,lmin≤L(n)<lfreelocc≤L(n)≤lmax其他情况

图 2　Log-Odds、占据概率和三类栅格状态之间的关系。

Unknown 不是简单的"非占据"。它表示当前证据不足。

保守避障可把 Unknown 视为障碍；
主动探索可允许机器人向 Unknown 移动；
Frontier 正是 KnownFree 与 Unknown 的边界。

8. 射线缓存与批量概率更新

同一帧点云中，多条激光束可能重复经过同一个栅格。若每经过一次就立即写入地图，会造成大量随机内存访问。

ROG-Map 在射线遍历阶段建立缓存：

C = { ( n , c hit ( n ) , c miss ( n ) ) } \mathcal{C} =\left\{ \left( n, c_{\text{hit}}(n), c_{\text{miss}}(n) \right) \right\} C={(n,chit(n),cmiss(n))}

缓存记录：

本帧需要更新的唯一栅格；
每个栅格的 Hit 次数；
每个栅格的 Miss 次数。

射线处理完成后，每个候选栅格只执行一次概率写入。

这项设计的作用不是减少射线数量，而是减少：

重复数组寻址；
重复概率读取；
重复 Log-Odds 写入；
CPU 缓存失效；
多线程环境中的同步压力。

论文对 FIIMap 的复现还指出一个容易忽略的工程细节：即使访问栅格数量较少，若使用链式队列等缓存局部性较差的数据结构，整体执行时间仍可能更长。ROG-Map 大量使用连续数组，能够获得更高的 CPU Cache 命中率。

9. 机器人中心局部地图

9.1 固定尺寸局部窗口

设局部地图的栅格数量为：

s = ( s x , s y , s z ) ∈ Z + 3 \mathbf{s} =\left( s_x, s_y, s_z \right) \in \mathbb{Z}_{+}^{3} s=(sx,sy,sz)∈Z+3

分辨率为 r r r，局部地图物理尺寸约为：

L = r s \mathbf{L} =r\mathbf{s} L=rs

地图只维护机器人周围的固定范围。设地图当前中心为 o \mathbf{o} o，机器人位置为 x k \mathbf{x}_k xk，当距离超过滑动阈值 d d d 时触发滑动：

∥ o − x k ∥ 2 > d \left\|\mathbf{o} -\mathbf{x}_k \right\|_2>d ∥o−xk∥2>d

9.2 全局体素索引

空间点为：

p = ( p x , p y , p z ) \mathbf{p} =\left( p_x, p_y, p_z \right) p=(px,py,pz)

每个方向的全局离散索引为：

i k g = round ⁡ ( p k r ) , k ∈ { x , y , z } i_k^g =\operatorname{round} \left( \frac{p_k}{r} \right) ,\qquad k\in\{x,y,z\} ikg=round(rpk),k∈{x,y,z}

这里的"全局索引"只表示它与世界坐标固定，并不表示系统保存了完整全局地图。

9.3 循环索引映射

论文先计算中间索引：

i k t = i k g m o d s k i_k^t =i_k^g \bmod s_k ikt=ikgmodsk

然后使用 normalize() 映射到局部数组范围：

i k l = normalize ⁡ ( i k t , s k ) i_k^l =\operatorname{normalize} \left( i_k^t, s_k \right) ikl=normalize(ikt,sk)

i k l ∈ $0 , s k - 1$ i_k^l \in \left $0, s_k-1 \\right$ ikl∈ $0,sk-1$

设：

h = ⌊ s 2 ⌋ h =\left\lfloor \frac{s}{2} \right\rfloor h=⌊2s⌋

论文给出的归一化函数为：

normalize ⁡ ( x , s ) = { x − h , x > h x + h , − h ≤ x ≤ h x + 3 h , x < − h \operatorname{normalize}(x,s) =\begin{cases} x-h, & x>h \\ $4pt$ x+h, & -h \leq x \leq h \\ $4pt$ x+3h, & x<-h \end{cases} normalize(x,s)=⎩ ⎨ ⎧x−h,x+h,x+3h,x>h−h≤x≤hx<−h

三维索引最终展平为一维数组地址：

toAddress ⁡ ( i l ) = i x l s y s z + i y l s z + i z l \operatorname{toAddress} \left( \mathbf{i}^l \right) =i_x^l s_y s_z + i_y^l s_z + i_z^l toAddress(il)=ixlsysz+iylsz+izl

图 3　无限范围全局索引到固定长度局部数组索引的循环映射。

全局索引到局部索引是多对一映射。只要同时知道当前地图中心和局部窗口范围，就可以判断某个局部内存槽当前对应哪个全局体素。

10. 零拷贝地图滑动

10.1 普通滑动方法的问题

若每次移动局部地图中心都把重叠区域复制到新数组，复杂度接近：

O ( s x s y s z ) O \left( s_x s_y s_z \right) O(sxsysz)

高分辨率三维地图包含数千万体素，整体复制会产生明显延迟和内存带宽开销。

10.2 ROG-Map 的处理方式

ROG-Map 使用循环缓冲区：

更新局部地图在世界坐标系中的中心和边界；
不移动仍然位于新窗口内的数据；
计算离开局部窗口的栅格条带；
清空这些内存槽；
新进入地图的空间直接复用被清空的槽位。

图 4　局部地图滑动前后，重叠区域仍保留在原内存地址中，只清空离开窗口的区域。

图中的红色虚线区域在滑动前后保持同一内存地址；黄色区域离开局部地图后被清空，其内存随后用于保存绿色新区域。

10.3 滑动复杂度

设一次滑动后新旧窗口不重叠的体素数量为 N out N_{\text{out}} Nout，则清理成本约为：

O ( N out ) O \left( N_{\text{out}} \right) O(Nout)

而不是对整张地图执行：

O ( s x s y s z ) O \left( s_x s_y s_z \right) O(sxsysz)

当滑动距离较小时， N out N_{\text{out}} Nout 只是一层或数层体素条带。

10.4 空间遗忘

局部窗口之外的占据信息会被清除。论文将其称为一种 spatial forgetting mechanism。

它不会影响只依赖机器人附近环境的局部避障，但会影响：

完整全局探索；
返回旧区域时直接复用地图；
长时间全局覆盖统计；
全局语义地图维护；
回环后一致性修正。

论文提出的后续方向是构建混合地图：

稀疏数据结构保存全局信息；
ROG-Map 负责实时高分辨率局部更新。

11. Rising Grid 与 Falling Grid

增量障碍物膨胀的关键不是处理所有 Occupied 栅格，而是检测离散状态变化。

11.1 Rising Grid

栅格由 Unknown 或 KnownFree 变为 Occupied：

RG ⁡ ( n ) ⟺ s k − 1 ( n ) ≠ Occupied ∧ s k ( n ) = Occupied \operatorname{RG}(n) \iff s_{k-1}(n) \neq \text{Occupied} \land s_k(n) =\text{Occupied} RG(n)⟺sk−1(n)=Occupied∧sk(n)=Occupied

11.2 Falling Grid

栅格由 Occupied 变为 KnownFree 或 Unknown：

FG ⁡ ( n ) ⟺ s k − 1 ( n ) = Occupied ∧ s k ( n ) ≠ Occupied \operatorname{FG}(n) \iff s_{k-1}(n) =\text{Occupied} \land s_k(n) \neq \text{Occupied} FG(n)⟺sk−1(n)=Occupied∧sk(n)=Occupied

若栅格在相邻两帧中始终保持 Occupied，则其膨胀邻域不需要重复计算。

12. 计数器式增量障碍物膨胀

12.1 障碍物膨胀的含义

运动规划常把机器人近似为质点。为了保证真实机器人不会碰撞，需要将障碍物按机器人外形和安全余量膨胀。

设原始占据集合为 O \mathcal{O} O，机器人安全半径为 R R R，膨胀集合为：

O i n f = { x | ∃ o ∈ O , ∥ x − o ∥ 2 ≤ R } \mathcal{O}_{\mathrm{inf}} =\left\{ \mathbf{x} \ \middle|\ \exists \mathbf{o} \in \mathcal{O}, \left\| \mathbf{x} -\mathbf{o} \right\|_2 \leq R \right\} Oinf={x ∣ ∃o∈O,∥x−o∥2≤R}

这等价于障碍物集合与机器人形状的 Minkowski Sum。

12.2 传统方法

传统方法在每帧更新后遍历点云包围盒或局部更新框：

找出其中所有 Occupied 栅格；
对每个 Occupied 栅格遍历膨胀邻域；
重新标记 InflatedOccupied；
清除已经失效的膨胀区域。

激光雷达点云的包围盒通常很大，高分辨率三维地图中会访问大量未发生变化的栅格。

12.3 邻域查找表

膨胀半径和地图分辨率固定时，离散邻域偏移可以提前计算：

L = { Δ i ∈ Z 3 | ∥ Δ i ∥ 2 ≤ n R } \mathcal{L} =\left\{ \Delta\mathbf{i} \in \mathbb{Z}^3 \ \middle|\ \left\| \Delta\mathbf{i} \right\|_2 \leq n_R \right\} L={Δi∈Z3 ∥Δi∥2≤nR}

其中：

n R = ⌈ R r i n f ⌉ n_R =\left\lceil \frac{R} {r_{\mathrm{inf}}} \right\rceil nR=⌈rinfR⌉

运行时只需遍历偏移表 L \mathcal{L} L，不再重复计算球形邻域。

12.4 膨胀计数器

对膨胀地图中的每个栅格 q q q，维护计数器：

c i n f ( q ) = ∑ o ∈ O 1 $q - o \in L$ c_{\mathrm{inf}}(q) =\sum_{\mathbf{o}\in\mathcal{O}} \mathbf{1} \left $q-\\mathbf{o} \\in \\mathcal{L} \\right$ cinf(q)=o∈O∑1 $q-o\inL$

它表示有多少个 Occupied 栅格的膨胀邻域覆盖了 q q q。

当某个栅格成为 Rising Grid 时：

c i n f ( q ) ← c i n f ( q ) + 1 c_{\mathrm{inf}}(q) \leftarrow c_{\mathrm{inf}}(q) + 1 cinf(q)←cinf(q)+1

当某个栅格成为 Falling Grid 时：

c i n f ( q ) ← c i n f ( q ) − 1 c_{\mathrm{inf}}(q) \leftarrow c_{\mathrm{inf}}(q) -1 cinf(q)←cinf(q)−1

膨胀状态由计数器直接判断：

q ∈ O i n f ⟺ c i n f ( q ) ≥ 1 q \in \mathcal{O}{\mathrm{inf}} \iff c{\mathrm{inf}}(q) \geq 1 q∈Oinf⟺cinf(q)≥1

图 5　膨胀计数器示意图。橙色为膨胀占据栅格，蓝色为原始占据栅格，数字表示被多少个障碍物邻域覆盖。

12.5 为什么计数器不会错误删除重叠膨胀区域

假设两个障碍物同时覆盖某个栅格，则：

c i n f ( q ) = 2 c_{\mathrm{inf}}(q)=2 cinf(q)=2

其中一个障碍物消失后：

c i n f ( q ) ← 2 − 1 = 1 c_{\mathrm{inf}}(q) \leftarrow 2-1 =1 cinf(q)←2−1=1

因为计数器仍大于 0，该栅格继续保持 InflatedOccupied。只有最后一个相关障碍物也消失后，计数器才降为 0。

12.6 从离散卷积角度理解

设障碍物指示函数为：

χ O ( i ) = { 1 , i ∈ O 0 , i ∉ O \chi_{\mathcal{O}}(\mathbf{i}) =\begin{cases} 1, & \mathbf{i}\in\mathcal{O} \\ $4pt$ 0, & \mathbf{i}\notin\mathcal{O} \end{cases} χO(i)={1,0,i∈Oi∈/O

设膨胀结构元素为：

K ( Δ i ) = 1 $Δ i \in L$ K(\Delta\mathbf{i}) =\mathbf{1} \left $\\Delta\\mathbf{i} \\in \\mathcal{L} \\right$ K(Δi)=1 $Δi\inL$

则计数器可以写成离散卷积：

c i n f = χ O ∗ K c_{\mathrm{inf}} =\chi_{\mathcal{O}} * K cinf=χO∗K

二值膨胀结果为：

χ O i n f ( q ) = 1 $c i n f ( q ) \> 0$ \chi_{\mathcal{O}_{\mathrm{inf}}}(q) =\mathbf{1} \left $c_{\\mathrm{inf}}(q)\>0 \\right$ χOinf(q)=1 $cinf(q)\>0$

当单个栅格状态变化时，只需给卷积结果增减一个局部结构元素。这说明计数器方法不是近似膨胀，而是对标准二值膨胀结果的精确增量维护。

12.7 复杂度

设当前帧中状态发生变化的栅格数量为 n n n，邻域查找表大小为 ∣ L ∣ |\mathcal{L}| ∣L∣，计算复杂度为：

O ( n ∣ L ∣ ) O \left( n \left| \mathcal{L} \right| \right) O(n∣L∣)

当膨胀半径和分辨率固定时， ∣ L ∣ |\mathcal{L}| ∣L∣ 为常数，因此：

O ( n ) O \left( n \right) O(n)

论文对比的 FIIMap 在 Falling Grid 处理中可能检查邻居及邻居的邻居，最坏复杂度为：

O ( n 2 ) O \left( n^2 \right) O(n2)

ROG-Map 在所有情况下都保持与变化栅格数量线性相关。

12.8 伪代码

text 复制代码

输入：
    当前点云 Pk
    激光雷达位置 xk
    球形邻域偏移表 L

1. C ← RayCasting(xk, Pk)

2. 对每个候选栅格 n ∈ C：

       old_state ← state(n)

       根据 hit_count 和 miss_count
       更新 n 的 Log-Odds

       new_state ← state(n)

       若 old_state 非 Occupied
       且 new_state 为 Occupied：

           对每个偏移 Δi ∈ L：
               inflation_counter[n + Δi] += 1

       若 old_state 为 Occupied
       且 new_state 非 Occupied：

           对每个偏移 Δi ∈ L：
               inflation_counter[n + Δi] -= 1

13. 多分辨率计数地图

开源版本允许概率地图和膨胀地图使用不同分辨率，例如：

yaml 复制代码

resolution: 0.1
inflation_resolution: 0.2

此时：

ProbMap 使用 0.1 m 分辨率保存精细占据状态；
InfMap 使用 0.2 m 分辨率供规划器高频碰撞查询。

设一个粗栅格包含 N N N 个细栅格，占据子栅格数为：

C o c c = ∑ j = 1 N 1 $s j = Occupied$ C_{\mathrm{occ}} =\sum_{j=1}^{N} \mathbf{1} \left $s_j =\\text{Occupied} \\right$ Cocc=j=1∑N1 $sj=Occupied$

未知子栅格数为：

C u n k = ∑ j = 1 N 1 $s j = Unknown$ C_{\mathrm{unk}} =\sum_{j=1}^{N} \mathbf{1} \left $s_j =\\text{Unknown} \\right$ Cunk=j=1∑N1 $sj=Unknown$

粗栅格状态可表示为：

s c = { Occupied , C o c c > 0 Unknown , C o c c = 0 ∧ C u n k > τ u n k N KnownFree , 其他情况 s_c =\begin{cases} \text{Occupied}, & C_{\mathrm{occ}}>0 \\ $4pt$ \text{Unknown}, & C_{\mathrm{occ}}=0 \land C_{\mathrm{unk}}>\tau_{\mathrm{unk}}N \\ $4pt$ \text{KnownFree}, & \text{其他情况} \end{cases} sc=⎩ ⎨ ⎧Occupied,Unknown,KnownFree,Cocc>0Cocc=0∧Cunk>τunkN其他情况

其中 τ u n k \tau_{\mathrm{unk}} τunk 对应参数 unk_thresh。

这种结构带来三个好处：

细地图保留小障碍物；
粗地图减少规划查询和膨胀开销；
细栅格状态变化后，只增量修改所属粗栅格的计数。

unk_thresh 的含义需要特别注意：

接近 0：只要存在少量 Unknown 子栅格，粗栅格就可能判为 Unknown，策略更保守；
接近 1：只有绝大多数或全部子栅格为 Unknown，粗栅格才判为 Unknown，策略更激进。

14. Unknown 膨胀

ROG-Map 可以对未知区域执行膨胀：

yaml 复制代码

unk_inflation_en: false
unk_inflation_step: 1

其处理逻辑与障碍物膨胀类似，只是维护的是 Unknown 邻域计数。

Unknown 膨胀适合：

高速无人机；
传感器盲区明显的场景；
定位误差较大时；
安全优先、禁止进入未观测区域的任务。

但在探索任务中，若把所有 Unknown 都视为膨胀障碍，规划器容易无路可走。

15. Frontier 增量生成

Frontier 是 KnownFree 与 Unknown 的边界。可定义为：

F = { v | s ( v ) = KnownFree , ∃ u ∈ N ( v ) , s ( u ) = Unknown } \mathcal{F} =\left\{ v \ \middle|\ s(v) =\text{KnownFree}, \exists u \in \mathcal{N}(v), s(u) =\text{Unknown} \right\} F={v ∣ s(v)=KnownFree,∃u∈N(v),s(u)=Unknown}

ROG-Map 使用增量方式提取 Frontier。栅格状态发生变化时，只检查其附近区域，而不是每帧遍历整张地图。

Frontier 可用于：

未知环境探索；
下一最佳视点选择；
覆盖规划；
感知范围边界显示；
局部探索目标生成。

开启参数：

yaml 复制代码

frontier_extraction_en: true

16. 滑动 ESDF

16.1 ESDF 定义

ESDF 是 Euclidean Signed Distance Field，即欧氏有符号距离场。

自由空间点到最近障碍物的距离为：

D ( x ) = min ⁡ o ∈ O ∥ x − o ∥ 2 D(\mathbf{x}) =\min_{\mathbf{o}\in\mathcal{O}} \left\| \mathbf{x} -\mathbf{o} \right\|_2 D(x)=o∈Omin∥x−o∥2

有符号距离场通常将障碍物内部设为负值，自由空间设为正值。

16.2 障碍代价

轨迹优化器可以根据 ESDF 构造连续障碍代价：

J o b s ( x ) = { 0 , D ( x ) ≥ d s ( d s − D ( x ) ) 2 , 0 < D ( x ) < d s J max ⁡ , D ( x ) ≤ 0 J_{\mathrm{obs}}(\mathbf{x}) =\begin{cases} 0, & D(\mathbf{x}) \geq d_s \\ $4pt$ \left( d_s -D(\mathbf{x}) \right)^2, & 0 < D(\mathbf{x}) <d_s \\ $4pt$ J_{\max}, & D(\mathbf{x}) \leq 0 \end{cases} Jobs(x)=⎩ ⎨ ⎧0,(ds−D(x))2,Jmax,D(x)≥ds0<D(x)<dsD(x)≤0

其中 d s d_s ds 是安全距离。

16.3 距离梯度

若规划器需要梯度，可使用中心差分：

∂ D ∂ x ≈ D ( x + r e x ) − D ( x − r e x ) 2 r \frac{\partial D} {\partial x} \approx \frac{ D \left( \mathbf{x} + r\mathbf{e}_x \right) -D \left(\mathbf{x} -r\mathbf{e}_x \right) } {2r} ∂x∂D≈2rD(x+rex)−D(x−rex)

对 y y y 和 z z z 方向同理。

16.4 ESDF 与膨胀地图的区别

地图	查询结果	适合用途
膨胀占据地图	是否碰撞	A、RRT、路径剪枝、硬约束
ESDF	到最近障碍物的连续距离	轨迹优化、梯度下降、软安全代价

ROG-Map 只在机器人附近维护滑动 ESDF，避免为完整概率地图计算距离场。

示例参数：

yaml 复制代码

esdf:
  enable: true
  resolution: 0.1
  local_update_box: [5, 5, 3]

17. 论文基准实验设计

17.1 对比方法

论文比较了五种地图：

OctoMap：Octree 占据地图；
HashMap：基于 Voxel Hashing 思路实现的占据地图；
UniformMap：固定原点均匀栅格地图；
FIIMap：带增量障碍物膨胀的均匀栅格地图；
ROG-Map：机器人中心均匀栅格与计数器增量膨胀。

HashMap 和 FIIMap 的公开实现不满足论文测试需求，因此由作者依据对应论文重新实现。结果代表作者实现下的对比，不应简单等同于所有哈希地图和所有 FIIMap 实现的绝对性能。

17.2 测试指标

论文使用以下指标：

t t o t t_{\mathrm{tot}} ttot：每帧地图总更新时间；
t u t_u tu：概率占据更新耗时；
n i n f n_{\mathrm{inf}} ninf：障碍物膨胀访问栅格数；
t i n f t_{\mathrm{inf}} tinf：障碍物膨胀耗时；
t q t_q tq：100000 次随机查询耗时；
m m m：内存占用。

17.3 New College Dataset

场景尺寸：

250 m × 161 m × 33 m 250\ \text{m} \times 161\ \text{m} \times 33\ \text{m} 250 m×161 m×33 m

地图分辨率：

r = 0.2 m r = 0.2\ \text{m} r=0.2 m

最大射线距离：

R r a y = 20 m R_{\mathrm{ray}} = 20\ \text{m} Rray=20 m

障碍物膨胀距离：

R i n f = 0.2 m R_{\mathrm{inf}} = 0.2\ \text{m} Rinf=0.2 m

平均每帧点数约为 156.2。

17.4 HKU RSC Dataset

场景尺寸：

74 m × 42 m × 20 m 74\ \text{m} \times 42\ \text{m} \times 20\ \text{m} 74 m×42 m×20 m

地图分辨率：

r = 0.05 m r = 0.05\ \text{m} r=0.05 m

最大射线距离：

R r a y = 20 m R_{\mathrm{ray}} = 20\ \text{m} Rray=20 m

障碍物膨胀距离：

R i n f = 0.2 m R_{\mathrm{inf}} = 0.2\ \text{m} Rinf=0.2 m

平均每帧点数约为 23157.6。

两组测试中，ROG-Map 局部窗口均设为：

40 m × 40 m × 12 m 40\ \text{m} \times 40\ \text{m} \times 12\ \text{m} 40 m×40 m×12 m

18. 基准实验结果

表1 论文基准实验原始表格。
! $在这里插入图片描述$ (https://i-blog.csdnimg.cn/direct/34fc04b3346c417da8ee714d6342ce6c.png)

18.1 New College Dataset

方法	总时间/ms	概率更新/ms	膨胀访问栅格数	膨胀时间/ms	10 万次查询/ms	内存/MB
OctoMap	2.563	0.983	34991.262	1.625	61.463	640.809
HashMap	1.719	0.218	34991.262	1.501	48.771	2101.527
UniformMap	0.224	0.095	34991.262	0.129	21.688	3519.195
FIIMap	0.119	0.106	1617.898	0.013	22.708	3519.230
ROG-Map	0.101	0.097	305.917	0.004	22.981	64.387

在该数据集上：

ROG-Map 总更新时间约为 OctoMap 的 1/25；
膨胀时间约为 OctoMap 的 1/406；
相比 FIIMap，访问栅格数量减少约 81%；
随机查询速度接近 UniformMap；
机器人中心局部窗口使内存显著低于固定全局 UniformMap。

18.2 HKU RSC Dataset

方法	总时间/ms	概率更新/ms	膨胀访问栅格数	膨胀时间/ms	10 万次查询/ms	内存/MB
OctoMap	2255.409	1502.598	27258042.859	752.811	182.306	186.700
HashMap	2010.611	1165.105	27258042.859	845.506	152.322	2219.953
UniformMap	133.514	70.897	27258042.859	62.617	23.896	11643.707
FIIMap	267.990	63.759	1346163.411	204.231	22.104	11678.153
ROG-Map	68.187	67.416	36059.518	0.771	20.467	1046.243

在 0.05 m 高分辨率数据集上：

ROG-Map 总更新时间约为 OctoMap 的 1/33；
ROG-Map 总更新时间约为 HashMap 的 1/29；
ROG-Map 的膨胀时间比 UniformMap 低约 81 倍；
ROG-Map 的膨胀时间比 FIIMap 低约 265 倍；
ROG-Map 只访问遍历式方法约 0.13% 的膨胀栅格；
ROG-Map 只访问 FIIMap 约 2.67% 的膨胀栅格；
10 万次随机查询只需约 20.467 ms；
OctoMap 内存仍最低，但其更新时间无法满足实时要求；
固定全局 UniformMap 和 FIIMap 的内存超过 11 GB；
ROG-Map 以约 1.05 GB 内存换取 0.05 m 分辨率下的实时更新。

HKU RSC 数据频率为 10 Hz，每帧预算为 100 ms。ROG-Map 平均更新时间为 68.187 ms，是所有对比方法中唯一满足该测试实时预算的方法。

18.3 实验结果的真正含义

ROG-Map 的优势不是所有单项指标都绝对最优。

内存最低的是 OctoMap；
概率更新方面，UniformMap、FIIMap 和 ROG-Map 接近；
随机查询方面，均匀栅格方法明显优于树和哈希方法；
ROG-Map 最突出的优势是障碍物膨胀和局部固定内存；
高点数、高分辨率场景越能体现增量处理的价值。

19. 机载无人机系统组成

论文将 ROG-Map 集成到完整四旋翼系统中。

19.1 硬件

机载计算机：Intel NUC；
CPU：Intel i7-10710U；
激光雷达：Livox Mid360；
IMU：Pixhawk 内置 IMU；
无人机质量：约 1.5 kg；
推重比：大于 4.0。

19.2 状态估计

感知模块使用修改版 FAST-LIO2：

状态估计频率：100 Hz；
配准点云频率：50 Hz；
激光雷达与 IMU 外参、时间偏移使用论文引用的方法标定。

19.3 控制与规划

轨迹跟踪：On-Manifold Model Predictive Control；
局部规划：作者此前多分辨率搜索规划器的修改版本；
ROG-Map 用于寻找无碰撞路径和生成安全飞行走廊。

这说明 ROG-Map 处在系统中的环境表示层：

text 复制代码

FAST-LIO2
    │
    ├── 位姿
    └── 配准点云
          │
          ▼
       ROG-Map
          │
          ├── 占据查询
          ├── 膨胀查询
          └── 安全走廊
                 │
                 ▼
          多分辨率局部规划
                 │
                 ▼
          On-Manifold MPC
                 │
                 ▼
              四旋翼

20. 实飞实验

论文一共完成 8 次成功实验，正文展示了两类代表性测试。

20.1 统一地图参数

所有实飞实验使用：

30 m × 30 m × 6 m 30\ \text{m} \times 30\ \text{m} \times 6\ \text{m} 30 m×30 m×6 m

地图分辨率：

r = 0.05 m r = 0.05\ \text{m} r=0.05 m

局部更新距离：

R u p d a t e = 15 m R_{\mathrm{update}} = 15\ \text{m} Rupdate=15 m

20.2 Test 1：细金属网避障

场景为矮树密集的小树林，路线中放置了三张由约 3 mm 金属丝制成的细网。

图 7　细金属网实飞实验，包含局部地图、俯视轨迹和 FPV 画面。

无人机能够检测金属网并完成避障，最大飞行速度超过：

3.7 m/s 3.7\ \text{m/s} 3.7 m/s

该实验验证了：

0.05 m 体素对小障碍物的表达能力；
50 Hz 点云输入下的地图实时性；
膨胀地图对无人机尺寸的安全补偿；
高速运动下局部地图更新的稳定性。

20.3 Test 2：大范围多航点巡检

用户指定多个巡检目标，无人机在大范围环境中自动导航和避障。

图 8　大场景巡检点云与飞行轨迹，环境范围约 300 m × 170 m。

总飞行距离为：

502.23 m 502.23\ \text{m} 502.23 m

局部地图物理尺寸只有 30 m × 30 m × 6 m，但无人机仍能完成 500 m 以上的运动，说明循环缓冲区和地图滑动不依赖场景总长度。

20.4 实飞计算时间

图 9　实飞数据下 ROG-Map、HashMap 和 OctoMap 的膨胀时间、膨胀访问栅格数和总更新时间。

点云频率为 50 Hz，每帧时间预算为：

T f r a m e = 20 ms T_{\mathrm{frame}} =20\ \text{ms} Tframe=20 ms

ROG-Map 平均更新时间：

Test 1：5.96 ms；
Test 2：3.46 ms。

其中障碍物膨胀时间：

Test 1：0.172 ms；
Test 2：0.060 ms。

HashMap 和 OctoMap 在相同实飞记录数据上无法稳定满足 20 ms 帧预算。

20.5 论文摘要中的数值书写问题

论文摘要写道，障碍物膨胀占一帧时间的 0.33%，并在括号中写为 0.66 ms。

但：

20 ms × 0.33 % = 0.066 ms 20\ \text{ms} \times 0.33\% =0.066\ \text{ms} 20 ms×0.33%=0.066 ms

正文 Test 2 报告的膨胀时间为 0.060 ms，与 0.066 ms 接近。因此摘要中的"0.66 ms"应为小数点书写误差，或者百分比应为 3.3%。引用该数据时建议使用正文给出的 0.060 ms，而不要直接照抄 0.66 ms。

21. 算法创新点分析

21.1 创新不在占据概率公式

贝叶斯占据更新、Log-Odds、射线遍历和障碍物膨胀都不是论文首次提出。

论文创新在于把这些成熟技术重新组织为适合高频三维激光雷达规划的工程结构。

21.2 零拷贝滑动解决内存问题

均匀栅格查询快，但全局地图内存高。ROG-Map 通过局部循环数组，使空间复杂度与行驶距离无关：

M = O ( s x s y s z ) M =O \left( s_x s_y s_z \right) M=O(sxsysz)

局部尺寸固定后，机器人移动 100 m 或 1000 m，地图核心数组大小不变。

21.3 增量膨胀把问题从"地图有多大"变为"本帧变化了多少"

传统膨胀计算量与局部更新区域和占据栅格数量相关。ROG-Map 只处理状态发生变化的 RG 和 FG，计算量取决于：

n c h a n g e d n_{\mathrm{changed}} nchanged

在静态场景中，相邻帧的大多数障碍物状态不变，因此：

n c h a n g e d ≪ n o c c u p i e d n_{\mathrm{changed}} \ll n_{\mathrm{occupied}} nchanged≪noccupied

这正是其膨胀加速幅度远高于概率更新加速幅度的原因。

21.4 计数器同时解决增加与删除

很多增量方法容易处理新障碍物，却难以正确删除旧障碍物，特别是多个膨胀区域重叠时。

ROG-Map 的计数器保存覆盖次数，使添加和删除都只需要执行加一或减一。

21.5 连续数组带来硬件层面的优势

ROG-Map 的速度不仅来自大 O 复杂度，还来自：

连续内存；
简单地址计算；
更好的 CPU Cache 命中率；
较少的指针跳转；
较少的动态内存分配；
邻域偏移查找表。

这也是 UniformMap 和 ROG-Map 的随机查询明显快于 OctoMap、HashMap 的原因。

22. 项目优点

22.1 查询复杂度稳定

均匀数组不受树深和哈希冲突影响，单点查询严格为 O ( 1 ) O(1) O(1)。

22.2 内存不随场景长度持续增长

局部地图尺寸固定，适合长距离巡检和大范围飞行。

22.3 高分辨率小障碍物表达

0.05 m 地图能够把细金属网、树枝和细杆等目标转换为规划可查询的占据体素。

22.4 障碍物膨胀速度快

只处理 RG、FG 及其固定邻域，避免重复扫描整个更新区域。

22.5 支持多分辨率

细概率地图负责感知，较粗膨胀地图负责高频规划查询。

22.6 面向规划的接口完整

开源版本已经包含：

膨胀地图；
Unknown 策略；
Frontier；
ESDF；
A*；
RRT*；
MARSIM；
ROS 参数和 RViz 可视化。

23. 项目限制

23.1 丢失局部窗口外信息

离开地图窗口的占据信息会被清空，无法单独承担永久全局地图任务。

23.2 依赖高质量位姿

位姿漂移会造成：

障碍物重影；
墙体变厚；
空闲区域无法清除；
膨胀结果扩大；
路径被错误阻塞。

23.3 点云必须正确配准

输入点云若仍在激光雷达坐标系中，地图会随机器人运动和旋转，无法形成稳定世界地图。

23.4 固定窗口仍可能占用较大内存

机器人中心地图只保证内存不会随路程无限增长，并不代表局部高分辨率地图本身很小。

23.5 动态障碍仍可能残留

动态障碍清除速度取决于：

p_miss；
p_max；
新射线是否穿过障碍物原位置；
遮挡关系；
点云频率；
状态估计精度。

23.6 基准结果与具体实现有关

论文中的 HashMap 和 FIIMap 是作者依据论文重新实现的版本。不同数据布局、内存池和并行策略可能改变绝对时间，但不改变 ROG-Map 增量更新思路本身。

24. 主要参数

参数	含义	调整影响
`resolution`	概率地图分辨率	越小越精细，内存和射线访问量按三次方增长
`inflation_resolution`	膨胀地图分辨率	越粗越快，但边界更保守
`inflation_step`	障碍物膨胀步数	越大越安全，狭窄通道越容易被封闭
`unk_inflation_en`	是否膨胀 Unknown	开启后更保守
`unk_inflation_step`	Unknown 膨胀步数	增大后未知区域安全边界扩大
`map_size`	局部地图物理尺寸	应覆盖感知、制动和规划前视范围
`fix_map_origin`	禁用滑动时的固定地图原点	仅固定地图使用
`map_sliding/enable`	是否启用地图滑动	大范围任务一般开启
`map_sliding/threshold`	触发滑动的最小位移	太小会频繁清理，太大使机器人偏离窗口中心
`frontier_extraction_en`	是否提取 Frontier	非探索任务可关闭
`esdf/enable`	是否计算 ESDF	只做离散搜索时可关闭
`esdf/local_update_box`	ESDF 更新范围	应覆盖局部轨迹优化窗口
`cloud_topic`	配准点云话题	必须与实际话题一致
`odom_topic`	位姿或里程计话题	应与点云时间和坐标系匹配
`odom_timeout`	点云与里程计允许时间差	太小容易丢帧，太大容易错配
`intensity_thresh`	点云强度阈值	可过滤低强度噪声，也可能删除有效低反射点
`point_filt_num`	点云时间降采样比例	越大计算越少，但小障碍物更容易漏检
`raycasting/enable`	是否执行射线更新	关闭后只能维护占据点，无法可靠区分 Free 与 Unknown
`batch_update_size`	批量更新相关参数	影响更新频率与缓存处理
`local_update_box`	单次概率更新范围	限制远处点参与建图
`ray_range`	射线最小和最大距离	应与传感器有效量程匹配
`p_hit`	Hit 概率	越大，障碍建立越快
`p_miss`	Miss 概率	越小，障碍清除越快
`p_min/p_max`	概率截断范围	控制地图记忆强度
`p_occ`	占据阈值	越低越容易判为 Occupied
`p_free`	空闲阈值	与 `p_occ` 之间形成 Unknown 区域
`unk_thresh`	粗栅格 Unknown 比例阈值	越低越保守，越高越激进
`visualization/time_rate`	定时可视化频率	过高会增加 CPU 和通信开销

25. 配置示例

下面的配置来自仓库 RRT* 示例，可作为起点。

yaml 复制代码

rog_map:
  resolution: 0.1
  inflation_resolution: 0.2

  inflation_step: 1
  unk_inflation_en: false
  unk_inflation_step: 1

  map_size: [50, 50, 6]
  fix_map_origin: [0, 0, 0]

  frontier_extraction_en: true

  virtual_ceil_height: 9999
  virtual_ground_height: -9999

  load_pcd_en: true

  map_sliding:
    enable: true
    threshold: 0.3

  esdf:
    enable: true
    resolution: 0.1
    local_update_box: [5, 5, 3]

  ros_callback:
    enable: true
    cloud_topic: "/cloud_registered"
    odom_topic: "/lidar_slam/odom"
    odom_timeout: 2.0

  visualization:
    enable: true
    use_dynamic_reconfigure: false
    time_rate: 10
    frame_rate: 0
    range: [50, 50, 6]
    frame_id: "world"
    pub_unknown_map_en: false

  intensity_thresh: -10
  point_filt_num: 5

  raycasting:
    enable: true
    batch_update_size: 1
    local_update_box: [50, 50, 5]
    ray_range: [0.0, 999]

    p_min: 0.12
    p_miss: 0.49
    p_free: 0.499
    p_occ: 0.85
    p_hit: 0.90
    p_max: 0.98

    unk_thresh: 1.0

该配置不能直接视为所有机器人通用参数。实际项目需要根据传感器、机器人尺寸、速度和 CPU 性能重新测试。

26. 参数调节方法

26.1 地图分辨率

分辨率应小于需要可靠表达的障碍尺度，同时考虑内存和更新频率。

常见范围：

户外无人机避障：0.10～0.20 m；
细障碍和狭窄环境：0.05～0.10 m；
地面移动机器人三维局部地图：0.05～0.15 m。

26.2 局部地图尺寸

地图半径应满足：

R m a p > R s e n s o r + R b r a k e + R p l a n R_{\mathrm{map}}>R_{\mathrm{sensor}} + R_{\mathrm{brake}} + R_{\mathrm{plan}} Rmap>Rsensor+Rbrake+Rplan

其中：

R s e n s o r R_{\mathrm{sensor}} Rsensor：有效感知距离；
R b r a k e R_{\mathrm{brake}} Rbrake：最大速度下的制动距离；
R p l a n R_{\mathrm{plan}} Rplan：规划器前视余量。

26.3 安全膨胀距离

安全距离可分解为：

R s a f e = R r o b o t + R l o c a l i z a t i o n + R c o n t r o l + R m a r g i n R_{\mathrm{safe}} =R_{\mathrm{robot}} + R_{\mathrm{localization}} + R_{\mathrm{control}} + R_{\mathrm{margin}} Rsafe=Rrobot+Rlocalization+Rcontrol+Rmargin

膨胀步数为：

n i n f = ⌈ R s a f e r i n f ⌉ n_{\mathrm{inf}} =\left\lceil \frac{ R_{\mathrm{safe}} }{ r_{\mathrm{inf}} } \right\rceil ninf=⌈rinfRsafe⌉

26.4 障碍物建立太慢

可尝试：

增大 p_hit；
降低 p_occ；
减小 point_filt_num；
减小地图分辨率数值；
检查强度阈值是否过滤了有效点。

26.5 动态障碍残影严重

可尝试：

适当减小 p_miss；
适当降低 p_max；
确认启用 Raycasting；
检查新射线是否能穿过障碍物原位置；
检查点云与位姿时间同步；
排查定位漂移和重影。

26.6 CPU 占用过高

优先调整：

增大 point_filt_num；
缩短 ray_range；
缩小 raycasting/local_update_box；
缩小 esdf/local_update_box；
降低 RViz 可视化频率；
关闭不需要的 Frontier 或 ESDF；
使用 Release 编译；
过滤无效远点和低质量点。

27. 编译环境

官方 ROS1 仓库面向 Ubuntu 20.04 和 ROS Noetic，README 给出的常用环境包括：

Ubuntu 20.04；
ROS Noetic；
GCC/G++ 9.4；
Eigen 3.3.7-2 或 3.4.1；
PCL；
catkin；
C++。

安装依赖：

bash 复制代码

sudo apt update

sudo apt install -y \
  ros-noetic-rosfmt \
  libglfw3-dev \
  libglew-dev \
  libeigen3-dev \
  libdw-dev

创建 Eigen 软链接前先检查：

bash 复制代码

ls /usr/include/eigen3/Eigen
ls /usr/include/Eigen

若 /usr/include/Eigen 不存在，再执行：

bash 复制代码

sudo ln -s /usr/include/eigen3/Eigen /usr/include/Eigen

28. 下载和编译

28.1 创建工作空间

bash 复制代码

mkdir -p ~/rog_ws/src
cd ~/rog_ws/src

28.2 克隆项目

bash 复制代码

git clone https://github.com/hku-mars/ROG-Map.git

28.3 退出 Conda

Conda 动态库可能与 ROS、PCL、Eigen 冲突：

bash 复制代码

conda deactivate

若已经在 Conda 环境中编译失败，先清除旧目录：

bash 复制代码

cd ~/rog_ws
rm -rf build devel

28.4 编译

bash 复制代码

cd ~/rog_ws
catkin_make -DBUILD_TYPE=Release

28.5 加载环境

bash 复制代码

source ~/rog_ws/devel/setup.bash

加入 ~/.bashrc：

bash 复制代码

echo "source ~/rog_ws/devel/setup.bash" >> ~/.bashrc
source ~/.bashrc

28.6 检查版本

bash 复制代码

cd ~/rog_ws/src/ROG-Map/scripts
bash check_version.sh

提交编译问题时，应附带该脚本输出。

29. 运行 A* 示例

启动：

bash 复制代码

cd ~/rog_ws
source devel/setup.bash
roslaunch rog_map_example astar_example.launch

在 RViz 中：

按 G；
启用 3D Nav Goal；
选择第一个点作为起点；
选择第二个点作为终点。

每选择两个点，程序执行一次三维 A*。

显示搜索过程：

yaml 复制代码

astar:
  visualize_process_en: true

A* 示例只是展示如何查询 ROG-Map，并不是论文的核心规划算法。

30. 运行 RRT* 示例

启动：

bash 复制代码

cd ~/rog_ws
source devel/setup.bash
roslaunch rog_map_example rrt_example.launch

在 RViz 中按 G，连续选择起点和终点。

显示采样过程：

yaml 复制代码

rrt_star:
  visualize_process_en: true

RRT* 主要使用两类地图接口：

采样点是否碰撞；
两个节点之间的线段是否无碰撞。

31. 运行 MARSIM 示例

31.1 启动 MARSIM

终端 1：

bash 复制代码

cd ~/rog_ws
source devel/setup.bash
roslaunch test_interface single_drone_os128.launch

31.2 启动 ROG-Map

终端 2：

bash 复制代码

cd ~/rog_ws
source devel/setup.bash

sudo chmod +x -R src
roslaunch rog_map_example marsim_example.launch

键盘控制：

W：前进；
S：后退；
A：左移；
D：右移；
空格：停止；
Q 或 Ctrl+C：退出。

31.3 ROS 话题控制

仿真控制话题：

text 复制代码

/planning/pos_cmd

消息类型：

text 复制代码

quadrotor_msgs/PositionCommand

调用示例：

bash 复制代码

cd ~/rog_ws/src/ROG-Map

# x y z
bash scripts/call_pos_cmd.sh 1 2 2

官方示例中的无人机模型是理想运动模型，能够无延迟跟踪给定位置和姿态，不等同于真实飞行动力学。

32. 集成到自己的 ROS1 工程

32.1 `package.xml`

xml 复制代码

<build_depend>rog_map</build_depend>
<exec_depend>rog_map</exec_depend>

32.2 `CMakeLists.txt`

cmake 复制代码

find_package(catkin REQUIRED COMPONENTS
  roscpp
  std_msgs
  pcl_ros
  geometry_msgs
  nav_msgs
  rog_map
)

include_directories(
  ${catkin_INCLUDE_DIRS}
)

target_link_libraries(your_node
  ${catkin_LIBRARIES}
)

32.3 创建地图对象

cpp 复制代码

#include <memory>

#include <ros/ros.h>
#include <pcl/console/print.h>

#include "rog_map/rog_map.h"

int main(int argc, char** argv) {
  ros::init(argc, argv, "rog_map_node");
  ros::NodeHandle nh("~");

  pcl::console::setVerbosityLevel(
      pcl::console::L_ALWAYS);

  rog_map::ROGMap::Ptr map =
      std::make_shared<rog_map::ROGMap>(nh);

  ros::AsyncSpinner spinner(0);
  spinner.start();

  ros::waitForShutdown();
  return 0;
}

构造函数会从 ROS 参数服务器读取 rog_map 参数。

32.4 Launch 文件

xml 复制代码

<launch>
  <node name="rog_map_node"
        pkg="your_package"
        type="your_node"
        output="screen">

    <rosparam command="load"
              file="$(find your_package)/config/rog_map.yaml"/>
  </node>
</launch>

33. 两种更新方式

33.1 ROS 话题自动更新

yaml 复制代码

rog_map:
  ros_callback:
    enable: true
    cloud_topic: "/cloud_registered"
    odom_topic: "/lidar_slam/odom"
    odom_timeout: 0.05

适合标准 ROS 管线，地图内部完成订阅和更新。

33.2 手动更新

关闭自动回调：

yaml 复制代码

rog_map:
  ros_callback:
    enable: false

在自己的同步回调中调用：

cpp 复制代码

map->updateMap(cloud, pose);

手动更新适合：

使用 message_filters 严格同步；
多激光雷达融合；
点云需要额外过滤和去畸变；
外部统一控制更新频率；
自定义坐标变换流程。

34. 常用查询方式

34.1 单点占据状态

cpp 复制代码

Eigen::Vector3d p(1.0, 2.0, 0.8);

bool occupied = map->isOccupied(p);
bool unknown = map->isUnknown(p);
bool free = map->isKnownFree(p);

34.2 膨胀地图

cpp 复制代码

bool occupied_inf =
    map->isOccupiedInflate(p);

bool unknown_inf =
    map->isUnknownInflate(p);

规划器通常应查询膨胀地图，而不是直接查询原始 Occupied 状态。

34.3 线段碰撞

cpp 复制代码

Eigen::Vector3d start(0.0, 0.0, 1.0);
Eigen::Vector3d goal(5.0, 2.0, 1.5);

bool line_free =
    map->isLineFree(start, goal);

适合：

RRT* 边连接；
路径剪枝；
视线检测；
局部目标选择；
安全走廊种子线段检查。

34.4 获取最后安全位置

部分接口可以在目标不可达时返回线段上的最后安全点：

cpp 复制代码

Eigen::Vector3d free_local_goal;

bool fully_free = map->isLineFree(
    start,
    goal,
    free_local_goal,
    20.0);

上层局部规划器可以把 free_local_goal 作为临时目标。

34.5 包围盒查询

boxSearch() 和 boxSearchInflate() 可以提取三维范围内的指定状态栅格，用于：

局部障碍物收集；
安全飞行走廊生成；
RViz 显示；
局部占据率统计；
轨迹周围碰撞检查。

35. 与路径规划和轨迹优化的关系

35.1 A*

A* 在体素网格中扩展相邻节点。每次扩展都需要查询目标体素：

text 复制代码

Occupied          → 禁止扩展
InflatedOccupied  → 禁止扩展
Unknown           → 根据安全策略决定
KnownFree         → 允许扩展

35.2 RRT*

RRT* 需要：

点碰撞检查；
线段碰撞检查；
邻域重连检查。

ROG-Map 的常数时间数组查询适合大量随机采样。

35.3 轨迹优化

可构造总目标：

J = J s m o o t h + λ o b s J o b s + λ d y n J d y n J =J_{\mathrm{smooth}} + \lambda_{\mathrm{obs}} J_{\mathrm{obs}} + \lambda_{\mathrm{dyn}} J_{\mathrm{dyn}} J=Jsmooth+λobsJobs+λdynJdyn

其中：

膨胀地图提供硬碰撞约束；
ESDF 提供连续障碍距离；
ESDF 梯度将轨迹推离障碍物；
动力学和时间参数仍由轨迹优化器处理。

35.4 对地面无人车的意义

对于只在平面运动的无人车：

只有二维激光雷达时，二维代价地图通常更简单；
有三维激光雷达和悬空障碍时，ROG-Map 更有价值；
可将三维占据结果投影到二维代价地图；
也可以直接在一定高度范围内执行三维包围盒查询。

36. 常见问题排查

36.1 Eigen 编译错误

bash 复制代码

cd ~/rog_ws/src/ROG-Map/scripts
bash check_version.sh

确认：

bash 复制代码

ls /usr/include/eigen3/Eigen
ls /usr/include/Eigen

36.2 Conda 导致链接错误

bash 复制代码

conda deactivate

cd ~/rog_ws
rm -rf build devel

catkin_make -DBUILD_TYPE=Release

36.3 `VizCfg` 生成失败

bash 复制代码

cd ~/rog_ws

catkin_make \
  -DCATKIN_DEVEL_PREFIX:PATH=$HOME/rog_ws/devel \
  -DBUILD_TYPE=Release

36.4 RViz 没有地图

bash 复制代码

rostopic list

rostopic hz /cloud_registered
rostopic hz /lidar_slam/odom

rostopic echo -n 1 /cloud_registered/header
rostopic echo -n 1 /lidar_slam/odom/header

检查：

话题名称；
点云是否发布；
里程计是否发布；
时间戳是否接近；
frame_id 是否正确；
ros_callback/enable 是否开启；
RViz Fixed Frame；
可视化范围；
点云是否在世界坐标系。

36.5 地图跟随机器人旋转

通常是把传感器坐标系原始点云直接输入地图。ROG-Map 需要世界坐标系中的配准点云。

36.6 墙体出现双层或重影

检查：

LIO 漂移；
LiDAR-IMU 外参；
点云去畸变；
点云与里程计同步；
世界坐标系和里程计坐标系是否混用。

36.7 狭窄通道被封死

检查：

inflation_step 是否过大；
inflation_resolution 是否过粗；
Unknown 是否被膨胀；
位姿漂移是否使墙体变厚；
点云噪声是否形成虚假障碍；
机器人安全半径是否设置过大。

36.8 地图无法清除动态障碍

检查：

是否启用 Raycasting；
p_miss 是否过于接近 0.5；
p_max 是否过高；
障碍物移走后是否有射线穿过原位置；
点云是否被遮挡；
位姿是否漂移。

37. ROS2 版本

ROS1 Noetic 版本位于当前 ROG-Map 仓库。

官方在 2025 年 1 月更新说明中指出，ROS2 版本已经集成到 HKU MaRS 的 SUPER 无人机导航系统：

https://github.com/hku-mars/SUPER

ROS2 项目应优先参考 SUPER 中的实现，而不是只把 ROS1 API 机械替换为 ROS2 API。移植时还需要处理：

catkin 到 ament_cmake；
ROS 参数声明；
QoS；
点云与里程计同步；
TF2；
RViz2；
多线程 Executor；
动态参数接口。

38. 适用场景

ROG-Map 适合：

四旋翼和多旋翼无人机；
三维激光雷达地面机器人；
四足机器人；
履带机器人；
大范围巡检；
森林和园区导航；
狭窄三维环境；
小障碍物避障；
需要 ESDF 或安全走廊的轨迹优化系统。

不适合单独承担：

完整全局建图；
长期地图存储；
回环一致性维护；
全环境语义地图；
不允许遗忘历史区域的全局探索。

39. 总结

ROG-Map 的核心价值不是提出新的 SLAM 或新的路径规划算法，而是针对激光雷达运动规划重新设计三维占据地图的数据结构和更新流程。

其完整技术路线为：

使用均匀栅格获得严格 O ( 1 ) O(1) O(1) 查询；
使用机器人中心固定窗口控制内存；
使用三维循环缓冲区实现零拷贝滑动；
使用快速体素遍历生成 Hit 和 Miss；
使用缓存合并同一帧中的重复栅格访问；
使用 Log-Odds 批量更新占据概率；
使用 Rising Grid 和 Falling Grid 检测状态变化；
使用邻域计数器精确维护增量障碍物膨胀；
使用多分辨率计数地图平衡精度和效率；
提供 Frontier、ESDF 和规划碰撞查询接口。

论文实验表明，ROG-Map 的概率更新速度与其他均匀栅格方法接近，但增量膨胀访问的栅格数量显著减少。在 0.05 m 高分辨率 HKU RSC 数据上，其平均每帧更新时间为 68.187 ms，而 OctoMap 和 HashMap 均超过 2 s。实飞中，ROG-Map 在 50 Hz 点云输入下平均只需 3.46～5.96 ms 完成一帧局部地图更新。

对于高分辨率三维激光雷达局部规划，ROG-Map 的优势很明确：它用可控的局部内存换取均匀栅格的高速查询，再通过状态变化检测和计数器更新解决障碍物膨胀的实时性问题。

参考资料

ROG-Map GitHub：https://github.com/hku-mars/ROG-Map
ROG-Map 论文：https://arxiv.org/abs/2302.14819
ROG-Map 论文 PDF：https://arxiv.org/pdf/2302.14819
SUPER：https://github.com/hku-mars/SUPER
MARSIM：https://github.com/hku-mars/MARSIM
FAST-LIO2：https://github.com/hku-mars/FAST_LIO
OctoMap：https://octomap.github.io/

BibTeX

bibtex 复制代码

@article{ren2023rogmap,
  title={
    ROG-Map: An Efficient Robocentric Occupancy Grid Map
    for Large-scene and High-resolution LiDAR-based Motion Planning
  },
  author={
    Yunfan Ren and
    Yixi Cai and
    Fangcheng Zhu and
    Siqi Liang and
    Fu Zhang
  },
  journal={arXiv preprint arXiv:2302.14819},
  year={2023}
}