ROG-Map：一种高效的大场景高分辨率 LiDAR 运动规划网格地图

由于这个公式全是乘法和除法，如果经过成千上万次的雷达扫描，计算机处理浮点数乘法不仅非常慢，还容易导致数值下溢（数字小到计算机无法精确表示）。因此，我们需要引入'对数几率'。

第三步：对数几率（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)} \quad (2) $$

对于第三项的 $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} \quad (3) $$

• $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}) \quad （4） $$

通过设定对数几率的上限 $l_{max}$ 和下限 $l_{min}$，我们确保概率既不会无限大也不会无限小。这样哪怕障碍物突然移走，地图也能迅速更新。

论文代码对应参数如下，结合上图和公式一起理解。

# Probabilistc update 概率更新与射线投射。通过多次观测累积概率，处理传感器噪声
raycasting:
  # if disable, the map will only maintain occupied information, and all other grid
  # will be considered as unknown.
  enable: true
  # 启用射线投射（概率地图更新）
  batch_update_size: 1
  # 批量更新大小
  local_update_box: [ 50,50,5 ]
  # 局部更新范围 50×50×5 米
  # The range of raycasting [m].
  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.98。这样做的好处：避免一次误测后永远'翻不了身'，给后续观测留有修正空间。
• 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$ 里面有障碍物的概率是多少。'

四、ROG-MAP 核心创新

这个局部滑动地图在导航中的用处是什么，规划用的是传感器实时在线的每一帧数据，直接转为占栅格使用，然后更新范围内进行障碍物膨胀。

解答：

1. '为什么不直接把当前帧点云转成栅格，而要搞复杂的概率更新'的疑问 如果只用当前一帧点云，地图会存在严重的不确定性和噪声问题：
   • 滤除传感器噪声：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++ 等编程语言中，对负数取模（比如 $-5 \pmod{50}$）得到的结果可能是负的（$-5$）。但是数组的下标不能是负数！所以论文引入了一个 $\text{normalize}$ 函数，它的唯一目的就是把这些带负号的索引'掰正'，强行拉回到 $[0, s_k-1]$ 的合法区间内。

经过这两步，我们得到了局部索引 (Local Index) $i^l$。无论机器人在现实世界飞到了哪里，它对应的栅格下标永远被死死限制在 $0$ 到 $s-1$ 之间。

全局坐标->全局索引->中间索引->局部索引坐标->局部索引坐标转为唯一的 1D 内存地址 转为 1D 内存地址： $$ toAddress(i^l) = i_x^l \cdot s_y \cdot s_z + i_y^l \cdot s_z + i_z^l \quad (5) $$

• 含义推导：这是计算机图形学和三维数组的标配展平公式。
• 物理意义：通过这个公式，任何一个 3D 局部坐标，都能找到内存数组中唯一对应的一个格子。你要找第 $i_x^l$ 栋、第 $i_y^l$ 层、第 $i_z^l$ 个房间，就先跳过前面所有的整栋楼（$i_x^l \cdot s_y \cdot s_z$），再跳过当前楼栋下面的所有整层（$i_y^l \cdot s_z$），最后加上当前层的房间号（$i_z^l$）。

这就好比是一栋大楼：$s_z$ 是每层楼的房间数，$s_y$ 是每栋楼的层数，$s_x$ 是小区的楼栋数。

最核心的魔法：Zero-copy（零拷贝）滑动策略 传统局部地图更新方法：机器人往前 1m，就把局部地图清空，然后更新局部范围内的栅格，并遍历每一个栅格进行障碍物膨胀。

• 在内存的物理层面，数据格子永远不移动位置。
• 当机器人移动导致局部地图的'罩子'发生偏移时（移动距离大于阈值 $d$），一些曾经在无人机背后的旧区域（图 4 中的黄色虚线框）就掉出了地图范围。
• 系统只需要把这些被淘汰的旧内存地址清零（重置）。
• 因为是循环取模（就像表盘转了一圈又回到起点），这些清零后的旧内存空间，马上就可以用来存储无人机正前方刚刚探测到的新区域（图 4 中的绿色虚线框）。

总结一下： 这套公式本质上建立了一个'无限复用'的内存机制。它通过极低成本的数学取模运算，直接算出任何空间点在内存中的固定位置，从而免去了在内存中大规模搬运数据的昂贵开销。

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$ 是状态发生改变的栅格数量）。

1. 核心变量：投票计数器 (inflationCounter) 论文给每一个栅格都配备了一个整数型的计数器，初始值为 0。这个计数器的物理意义是：我的周围（膨胀半径内），有几个真正的障碍物栅格？只要这个计数器 >=1，就说明我挨着障碍物，我必须把自己标记为膨胀占据（InflatedOccupied）。

2. 触发机制：只关注'变心'的栅格 (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 毫秒）的终极秘密。

五、代码实现

//以下 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();
    ROS_WARN(">>> [Timer cloudCallback] Start execution - Time: %.3f, Thread ID: %lu", ros::Time::now().toSec(), std::hash<std::thread::id>{}(thread_id_A));
    if (!robot_state_.rcv) {
        return;//没有收到机器人状态就直接返回，因为点云更新需要知道机器人当前位姿：1.射线投射起点为机器人，2.局部地图构建是以机器人为中心的。3.地图滑动也是计算机器人位置与地图中心位置的偏移来决定的。
    }
    double cbk_t = ros::Time::now().toSec();
    if (cbk_t - robot_state_.rcv_time > cfg_.odom_timeout) {
        std::cout << YELLOW << " -- [ROS] Odom timeout, skip cloud callback." << RESET << std::endl;
        return;//如果'当前时间 - 最近一次收到 odom 的时间'超过阈值 cfg_.odom_timeout，就说明里程计太旧了。点云和位姿不同步，或者位姿已经不可信。
    }
    PointCloud temp_pc;
    pcl::fromROSMsg(*cloud_msg, temp_pc);
    rc_.updete_lock.lock();//这两个线程都要访问 rc_ 里的共享数据，所以必须加锁防止竞争。
    rc_.pc = temp_pc;//缓存点云，只保留最新一帧，不追求处理每一帧。
    rc_.pc_pose = std::make_pair(robot_state_.p, robot_state_.q);//缓存这帧点云对应的位姿
    rc_.unfinished_frame_cnt++;//未处理点云帧数加 1，updateCallback 里会把它重置为 0。这个计数器主要是为了监控 updateCallback 的处理速度是否跟得上 cloudCallback 的接收速度。
    map_empty_ = false;//标记地图不再为空，说明系统已经收到过至少一帧点云了。
    rc_.updete_lock.unlock();//共享缓存写完，释放锁。
}//cloudCallback() 可能在 ROS 回调线程中执行，updateCallback() 可能在定时器线程中执行

//异步地图更新。定时取出缓存中的最新点云和位姿，真正执行地图更新。
//定时线程取出最新一帧，调用 updateProbMap 做真正更新
void ROGMap::updateCallback(const ros::TimerEvent& event) {
    std::thread::id thread_id_B = std::this_thread::get_id();
    ROS_WARN(">>> [Timer updateCallback] Start execution - Time: %.3f, Thread ID: %lu", ros::Time::now().toSec(), std::hash<std::thread::id>{}(thread_id_B));
    //如果地图还是空的，就不更新
    if (map_empty_) {
        static double last_print_t = ros::Time::now().toSec();
        double cur_t = ros::Time::now().toSec();
        if (cfg_.ros_callback_en && (cur_t - last_print_t > 1.0)) {
            std::cout << YELLOW << " -- [ROG WARN] No point cloud input, check the topic name." << RESET << std::endl;
            last_print_t = cur_t;
        }
        return;
    }
    //如果没有未处理帧，直接返回
    if (rc_.unfinished_frame_cnt == 0) {
        return;
    } else if (rc_.unfinished_frame_cnt > 1) {//如果积压超过 1 帧，打印警告，说明你的地图线程跟不上传感器频率，系统正在丢帧。
        std::cout << RED << " -- [ROG WARN] Unfinished frame cnt > 1, the map may not work in real-time" << RESET << std::endl;
    }
    static PointCloud temp_pc;//只初始化一次，但是生命周期贯穿整个程序
    static Pose temp_pose;
    rc_.updete_lock.lock();//加锁，取出最新缓存
    temp_pc = rc_.pc;
    temp_pose = rc_.pc_pose;
    rc_.unfinished_frame_cnt = 0;
    rc_.updete_lock.unlock();
    updateProbMap(temp_pc, temp_pose);//真正更新概率地图，核心函数，更新完后会同步膨胀层并更新 ESDF
    writeTimeConsumingToLog(time_log_file_);//把本次处理耗时记录到日志文件。
}

这段代码的架构思想很好：

1. 里程计和点云解耦：odomCallback() 只更新状态，cloudCallback() 只接收点云。模块职责清晰，不把重计算放到传感器回调里面。
2. 点云接收和地图更新解耦：点云回调只缓存，定时器线程才真正更新。防止 ROS 回调阻塞（这个很重要），地图更新频率可控。
3. 只处理最新帧：强实时。可能会丢帧，当传感器频率大于定时器回调函数时。

一个可能常用的小建模计算方程： 代码中，关于对起点到射线终点的这条射线进行范围限制裁减时，使用了标准的射线方程进行计算裁减后的点 p。

后续缓存 hit, miss 次数，以及进行对数几率累加，更新体素的状态代码体现出的 ProbMap 设计思想：

1. 先缓存，后统一更新。不是一边 raycast 一边立刻改 occupancy_buffer_，而是先统计：operation_cnt，hit_cnt，update_cache_id_g（记录待更新格子的全局索引）。最后统一对一帧点云每个位置对应体素进行对数几率计算累加，并更新体素状态。优点：避免同一格子一帧内反复更新。
2. hit 优先于 miss。只要 hit_cnt > 0，就按 hit 更新。
3. 使用 log-odds 而不是直接概率。使用标准的 log-odds 累积更新。加法形式简单高效。

六、数据流与架构

固定大小局部地图 + 滑动窗口 + 环形内存复用

优点：

• 内存固定，不会无限增长
• 更新速度快，只有移动大于一个阈值，才会更新局部地图，内存复用，不是每一帧重新构图
• 很适合无人机/机器人实时导航
• 可以始终只维护机器人周围一小块最有用的地图

整体构图流程：

• ProbMap：是主地图管理者，它负责：主概率栅格 occupancy_buffer_，射线更新，子地图初始化，滑动地图管理，虚拟地面/天花板索引化。
• InfMap：是派生的膨胀地图，用于安全碰撞检测/规划。
• FreeCntMap：是 frontier 提取辅助图，用于探索或边界统计。
• ESDFMap：是距离场图，用于距离查询、优化等。

七、RViz 可视化说明

• 橙色框：Local Map Range，局部地图边界范围。就是论文中对应可视化额定那个滑动局部地图概念。最重要的框，表示局部滑动地图。
• 紫色框：可视化范围，可视化橙色框（局部地图）内想要可视化的膨胀障碍物地图范围。局部地图内的局部可视化范围。仅用于可视化膨胀障碍物。不参与代码逻辑。
• 绿色框：由这一帧裁减的点构成的本帧更新对数几率体素的范围，是一个在动态调整的范围。
• 淡蓝色框：A定义的搜索框，和局部边界橙色框大小一样，但是范围不一样。由起点和终点唯一确定，不滑动。作用：用于在 A搜索的主循环中，判断扩展点的邻居点时都在 A*搜索地图区域内。
• 深蓝色框：esdf 图。

八、实现细节分析

1. updateProbMap() 函数是按定时器 1000hz 频率回调运行，如果 batch_update_size: 1，那对每一帧点云都会进行对数几率（概率）更新，但是地图滑动，只有当第一帧和无人机离当前局部地图距离超过一定阈值，才会滑动。
2. 由于 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);

所以绿框 = 这一帧所有有效射线终点的包围盒，其大小受以下约束：

1. raycast_range_max（球形截断）
2. local_update_box: [50,50,5]（AABB 截断）
3. virtual_ceiling/ground_height（高度截断）
4. 点云实际分布（最终决定因素）