ROG-Map:一种高效的以机器人为中心的大场景高分辨率LiDAR运动规划网格地图（论文阅读）

论文ROG-Map: An Efficient Robocentric Occupancy Grid Map for Large-scene and High-resolution LiDAR-based Motion Planning论文主要创新点1.本文旨在解决将激光雷达与OGM集成的挑战,ROG-Map是一种均匀的基于网格的OGM可以保持局部地图与机器人一起移动从而实现高效的地图操作并降低大场景自主飞行的内存成本2.此外我们提出了一种新的增量障碍膨胀方法该方法显着降低了膨胀的计算成本。该方法在各种公共数据集上优于最先进的SOTA方法。3.0拷贝地图滑动策略该策略仅维护机器人周围的局部地图使ROG-Map适用于大场景任务论文特点只是用于避障的局部地图最求计算效率最大化第一部分介绍 INTRODUCTION视觉测量范围短35m激光雷达精确和远程避开小障碍物和大场景感知。由于要避开小障碍物分辨率足够高的OGM能够感知小障碍物从而在复杂环境中实现导航和避障。充分利用激光雷达提供远程精确测量的能力理想的激光雷达占用网格地图OGM有望保持高分辨率和高效率的远距离更新。这个高要求出现2个问题地图更新和障碍物膨胀的计算负载显着增加。传统的基于八叉树和哈希表的OGM在激光雷达运动规划的实时性中遇到很大问题。但是基于均匀网格的方法有很高的计算效率但是在大规模环境中由于其内存消耗变得不可行。论文主要贡献总结1. ROG-Map基于均匀网格的高效计算的OGM包。其中的0复制地图滑动策略使得有一个随机器人易懂的局部地图在大场景和高分辨率。2. 新的障碍物膨胀方法实现了O(N)的计算复杂度复杂度更小计算速度更快。3. 我们在公共数据集上将ROG-Map与SOTA基线进行比较展示了其计算和内存效率的优势。此外ROG-Map被集成到基于激光雷达的四旋翼中进行广泛的真实世界测试以验证其出色的真实世界性能。4.ROG-Map作为ROS包实施开源。第二部分相关工作 RELATED WORKSA. Occupancy Grid Map现有主流实现占用地图的方法对比时间和空间复杂度。基于八叉树、基于哈希表和均匀的基于网格的占用地图对比。B. Obstacle Inflation提出了一种新颖的增量更新方案提出的方法在公共数据集上将遍历网格的数量减少70%~97%显着加快了障碍膨胀过程。第三部分占据栅格地图 OCCUPANCY GRID MAP这里对基本的占据栅格地图是如何更新得到进行了基本公式理论的说明。问题定义当机器人的激光雷达LiDAR扫过周围环境时我们如何通过数学计算来准确判断地图上的某一个小方块栅格里面到底有没有障碍物数学表达以激光雷达测量数据为条件估计栅格被占概率第一步在第次更新时系统会接收到激光雷达的位置及其扫描到的一组点云数据。Hit击中如果激光雷达的某一条光束打在了某个栅格里说明那里可能有障碍物这被称为一次“击中” 。Miss穿过如果光束穿过了某个栅格说明那里是空的这被称为一次“未击中” 。第二步贝叶斯概率更新因为传感器会有噪声我们不能仅凭一次扫描就断定某个地方绝对有或绝对没有障碍物。我们需要结合历史数据来推测。假设这是一个马尔可夫过程我们需要计算在已知前次历史测量数据的情况下栅格被占据的概率记作。特别说明这里的与自己笔记上的是一个东西。经典的二元贝叶斯更新公式(1)其中定义为几率。逆传感其模型仅仅基于第 k 次当前这次扫描栅格被占据的概率。在第 k-1次上一次更新后栅格被占据的历史概率。没有任何测量时的先验概率。论文中通常假定为 0.5表示在没有任何信息时这个栅格有或没有障碍物的概率各占一半 。由于这个公式全是乘法和除法如果经过成千上万次的雷达扫描计算机处理浮点数乘法不仅非常慢还容易导致数值下溢数字小到计算机无法精确表示。因此我们需要引入“对数几率”。第三步对数几率Log-odds对数几率定义为计算其“几率”即发生概率与不发生概率的比值然后再两边同时取对数你会发现原本复杂的连乘瞬间变成了极其简单的加法。(2)对于第三项的P(n)为在没有雷达测量的先验概率P(n)0.5,则第三项为0(2) 可以简写成第 k次更新后的地图状态就等于上一时刻的地图状态加上当前这次观测带来的状态变化而且这种对数几率是双射的所以地图里只需要保存的值就可以了。这是一种迭代用激光雷达测量数据进行更新的占用栅格地图。其中。第四步计算当前观测的增量这里没有通过计算逆传感器模型计算当前观测的增量(3)和分别是第 $k$ 次更新时该栅格被击中和被穿过的次数 。和分别是击中和未击中对应的对数几率常数 。需要注意的是是一个负值因为当激光束穿过栅格时该栅格存在障碍物的概率会降低第五步应对动态环境的截断策略假设一辆车停在一个栅格里很久这个栅格的占据概率会被累加到一个天文数字。如果车开走了我们要花同样长的时间即接收很多次负的 $l_{miss}$才能把它的概率降下来这在动态环境中是致命的。为了保证地图对静态和动态环境的适应性论文采用了截断更新策略 限制它的上下界4通过设定对数几率的上限和下限我们确保概率既不会无限大也不会无限小。这样哪怕障碍物突然移走地图也能迅速更新。论文代码对应参数如下结合上图和公式一起理解。# 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.97。这样做的好处避免一次误测后永远“翻不了身”给后续观测留有修正空间。p_occ和p_free是状态判定阈值p_hit和p_miss是观测更新参数p_min和p_max是概率截断边界第六步最终状态的判定论文根据设定的阈值来判断栅格状态KnownFree已知空闲Occupied已被占据Unknown未知区域如果不符合上面两种情况则是未知状态 。在这里论文还引出了两个极其重要的概念这是为了后续“障碍物膨胀”算法做铺垫的上升栅格 (Rising Grid, RG)状态从 Unknown 或 KnownFree 变成 Occupied 的栅格 。下降栅格 (Falling Grid, FG)状态从 Occupied 变成 KnownFree 或 Unknown 的栅格 。一个不一样的地方在论文中以及深蓝中最后求的就是几率但是自己笔记部分目的是求解定义的数学问题以激光雷达测量数据为条件估计栅格被占概率在求得几率的基础上继续求出被占概率。问题解答问题一计算的是单个立方体栅格还是一个区域答案是它计算的是一个具体的、单一的 3D 立方体栅格Voxel被占据的概率。描述的是仅仅针对索引为 n的这一个特定小方块它里面存在障碍物的对数几率。问题二中的 1:k 具体指代什么答案是指代的是从机器人启动时的第 1 次扫描一直到当前时刻的第次扫描的“全部历史测量数据” 。所以翻译成大白话就是“在综合考虑了从一开始到现在第次所有的雷达位置和扫描到的所有点云数据之后栅格里面有障碍物的概率是多少。”第四部分THE ROG-MAP论文核心创新点部分这个局部滑动地图在导航中的用处是什么规划用的是传感器实时在线的每一帧数据直接转为占栅格使用然后更新范围内进行障碍物膨胀。解答1.“为什么不直接把当前帧点云转成栅格而要搞复杂的概率更新”的疑问如果只用当前一帧点云地图会存在严重的不确定性和噪声问题滤除传感器噪声LiDAR 在远距离或面对特定材质时会有噪点 。概率更新Bayesian updating能通过多帧观测确认一个栅格是真的被占据还是仅仅是噪点 。处理极细小的障碍物论文中特别强调了对3mm 细金属网的检测 。单帧 LiDAR 扫描可能由于光束发散或采样频率原因在某一帧漏掉这种细小障碍物 。通过历史概率累加地图能“记住”这些细线保证导航安全 。明确“自由空间”与“未知空间”仅仅将点云体素化只能告诉你哪里“有”障碍。而 ROG-Map 通过Ray casting射线投射能明确告诉你哪里是“确定没障碍”的自由区域哪里是“没看过的”未知区域 。这对于高精度的路径搜索至关重要。主要两个部分地图滑动和地图更新概率更新和递增障碍膨胀。A. Robocentric Local Maps解决了一个极度现实的问题无人机内存是有限的但现实世界是无限的。如果无人机飞出几公里把所有扫描过的地方都存在内存里电脑瞬间就会崩溃 。为了解决这个问题作者提出了一种以机器人为中心的局部地图Robocentric Local Maps。简单来说就像是机器人在走“跑步机”无论它怎么飞地图的核心始终罩在它周围跑出这个罩子的旧地图就被直接扔掉前面新探测到的地图就补进来 。全局坐标-全局索引全局索引-中间索引对地图长度进行取余运算。里利用了循环缓冲区Circular Buffer的概念。中间索引-局部索引归一化在 C 等编程语言中对负数取模比如得到的结果可能是负的。但是数组的下标不能是负数所以论文引入了一个函数它的唯一目的就是把这些带负号的索引“掰正”强行拉回到的合法区间内 。经过这两步我们得到了局部索引 (Local Index)。无论机器人在现实世界飞到了哪里它对应的栅格下标永远被死死限制在到之间。全局坐标-全局索引-中间索引-局部索引坐标-局部索引坐标转为唯一的1D内存地址转为1D内存地址(5)含义推导这是计算机图形学和三维数组的标配展平公式。这就好比是一栋大楼是每层楼的房间数是每栋楼的层数是小区的楼栋数。你要找第栋、第层、第个房间就先跳过前面所有的整栋楼再跳过当前楼栋下面的所有整层最后加上当前层的房间号。物理意义通过这个公式任何一个 3D 局部坐标都能找到内存数组中唯一对应的一个格子最核心的魔法Zero-copy零拷贝滑动策略传统局部地图更新方法机器人往前1m就把局部地图清空然后更新局部范围内的栅格并遍历每一个栅格进行障碍物膨胀。在内存的物理层面数据格子永远不移动位置。当机器人移动导致局部地图的“罩子”发生偏移时移动距离大于阈值 $d$一些曾经在无人机背后的旧区域图 4 中的黄色虚线框就掉出了地图范围 。系统只需要把这些被淘汰的旧内存地址清零重置。因为是循环取模就像表盘转了一圈又回到起点这些清零后的旧内存空间马上就可以用来存储无人机正前方刚刚探测到的新区域图 4 中的绿色虚线框。总结一下这套公式本质上建立了一个“无限复用”的内存机制。它通过极低成本的数学取模运算直接算出任何空间点在内存中的固定位置从而免去了在内存中大规模搬运数据的昂贵开销 。B. Probabilistic Update and Incremental InflationA解决的是局部地图如何在内存中生成和滑动的问题 。而我们现在要讲的 IV.B 节Probabilistic Update and Incremental Inflation解决的是在这个已经建好的局部地图里每次雷达扫到新数据时如何快速更新概率并把障碍物“变胖”膨胀以保证无人机飞行安全的问题第一步“批量”处理光线投射这部分不太理解具体还需要看代码理解传统的笨办法每一根激光束穿过一个栅格就立刻去更新一次那个栅格的概率。但是一帧点云有成千上万个点很多光束会穿过同一个空气栅格。如果每次都去更新对数几率计算量会非常大。论文使用了快速体素遍历算法Fast Voxel Traversal Algorithm进行光线投射并引入了一个缓存队列。算法先把当前这一帧所有光束穿过的栅格都找出来存进中 。并在缓存里直接统计好每个栅格被 Hit 了几次被 Miss 了几次 。这样后续更新时不管这个栅格被多少条光线穿过每一帧里它只需要进行一次概率更新的数学计算大大节省了算力 。这体现在算法的这一步第二步快速障碍物膨胀传统的膨胀方法每次雷达扫描后都要把局部地图里所有的栅格遍历一遍非常耗时复杂度极高。ROG-Map 提出了一种时间复杂度永远是 O(n)的极速膨胀法 $n$ 是状态发生改变的栅格数量。1.核心变量投票计数器 (inflationCounter)论文给每一个栅格都配备了一个整数型的计数器初始值为 0 。这个计数器的物理意义是我的周围膨胀半径内有几个真正的障碍物栅格只要这个计数器 1就说明我挨着障碍物我必须把自己标记为膨胀占据InflatedOccupied。2. 触发机制只关注“变心”的栅格 (Rising / Falling)当系统检测到一个栅格 n变成了上升栅格 (RG)时 找到栅格n的每一个邻居的具体位置给所有邻居的计数器加一。相反当栅格 n 变成了下降栅格 (FG)时找到栅格n的每一个邻居的具体位置给所有邻居的计数器减一。总结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::hashstd::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;//未处理点云帧数加1updateCallback 里会把它重置为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::hashstd::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. 只处理最新帧强实时。可能会丢帧当传感器频率大于定时器回调函数时。主要数据流固定大小局部地图 滑动窗口 环形内存复用优点内存固定不会无限增长更新速度快只有移动大于一个阈值才会更新局部地图内存复用不是每一帧重新构图很适合无人机/机器人实时导航可以始终只维护机器人周围一小块最有用的地图整体构图流程ProbMap 是主地图管理者它负责主概率栅格 occupancy_buffer_射线更新子地图初始化滑动地图管理虚拟地面/天花板索引化。InfMap 是派生的膨胀地图用于安全碰撞检测/规划。FreeCntMap 是 frontier 提取辅助图用于探索或边界统计。ESDFMap 是距离场图用于距离查询、优化等。