global_costmap（全局规划代价地图）

global_costmap（全局规划代价地图）

实车先看：整张地图和车周围的实时障碍怎样一起参与规划

当前全局代价地图在map中表达，默认不滚动；接收静态二维地图后，整张网格的尺寸、分辨率与平面原点由静态地图决定。 它先铺静态图，再加入terrain_map_ext筛出的障碍，最后膨胀，供planner_server中的Theta*规划器选择全局路径。

map_server／slam_toolbox的/map → static_layer ───────────────────────┐
    └─ 地图宽高、分辨率、平面原点 → 调整并锁定全局网格              │
                                                                  │
terrain_map_ext（odom系，intensity为地形高差）                       │
    → 按消息时间转到map，并取得front_mid360原点                     │
    → 来源高度、本层高度、高差、三维量程、体素筛选                  │
    → intensity_voxel_layer：加入符合条件的障碍 ────────────────────┤
                                                                  ↓
                                      inflation_layer → 全局合成代价地图
                                                       ├─ Theta*直接读内存规划
                                                       ├─ get_costmap服务提供当前图
                                                       └─ costmap／updates／raw供订阅者

全局图可以覆盖整个静态地图，但本实时障碍来源只保留距雷达严格0.2～10 m之间、又落在地图和高度窗口内的有效点。10 m不是整张地图的宽度，也不保证10 m内每处都已被观测。上游扩展地形当前在4 m内借用近场结果，4 m外使用自己的远场结果，两部分都从这里进入全局障碍层。扩展地形分析

本次容器内nav2_costmap_2d、nav2_voxel_grid、nav2_planner、nav2_theta_star_planner均为1.1.20，pb_nav2_plugins为1.0.1。自定义层实现与头文件和上游提交e6778ca44d9e88aa612a3c6c87d536a8cfa21705一致；Theta*安装头文件也与1.1.20一致。以下按这些版本解释，通用Humble笔记用来对照术语；自定义层的实际使用条件以源码为准。自定义层实现、类与体素索引

参数

下面保留发送的完整38项参数与原注释。当前源码及安装副本的reality段均与此配置一致；simulation段除use_sim_time=True外，还缺少顶层track_unknown_space，不是只差时钟。

global_costmap:
  global_costmap:
    ros__parameters:
      use_sim_time: False
      update_frequency: 5.0
      publish_frequency: 2.0
      global_frame: map
      robot_base_frame: gimbal_yaw_fake
      robot_radius: 0.2
      resolution: 0.05
      track_unknown_space: true
      plugins: ["static_layer", "intensity_voxel_layer", "inflation_layer"]
      intensity_voxel_layer:
        plugin: pb_nav2_costmap_2d::IntensityVoxelLayer
        enabled: true
        track_unknown_space: true
        footprint_clearing_enabled: true
        publish_voxel_map: false
        combination_method: 1
        mark_threshold: 0
        origin_z: 0.0
        unknown_threshold: 5
        z_resolution: 0.05
        z_voxels: 16
        min_obstacle_height: 0.0
        max_obstacle_height: 2.0
        min_obstacle_intensity: 0.1
        max_obstacle_intensity: 2.0
        observation_sources: terrain_map_ext
        terrain_map_ext:
          data_type: PointCloud2
          # '<robot_namespace>' keyword shall be replaced with 'namespace' where user defined.
          # It doesn't need to start with '/'
          topic: <robot_namespace>/terrain_map_ext
          sensor_frame: front_mid360
          min_obstacle_height: 0.0
          max_obstacle_height: 2.0
          obstacle_max_range: 10.0
          obstacle_min_range: 0.2
      static_layer:
        plugin: "nav2_costmap_2d::StaticLayer"
        map_subscribe_transient_local: True
      inflation_layer:
        plugin: "nav2_costmap_2d::InflationLayer"
        cost_scaling_factor: 4.0
        inflation_radius: 0.7
      always_send_full_costmap: False

两个global_costmap层级用于匹配命名空间和节点名。当前planner_server内部创建此地图节点；机器人namespace为/red_standard_robot1时，其全名为/red_standard_robot1/global_costmap/global_costmap，相关地图话题通常在/red_standard_robot1/global_costmap下。坐标系字符串map、gimbal_yaw_fake、front_mid360不会随节点namespace自动加前缀。全局地图节点创建

ros__parameters（更新、坐标、轮廓与未知区域）

参数表

参数详解

update_frequency与publish_frequency（内部5 Hz、话题2 Hz、规划请求各自运行）

内部更新循环以WallRate运行，目标每200 ms完成一次三层合成；更新时会锁地图，Theta*创建路径也锁同一张地图。增大更新频率需要看整图大小、点云处理、膨胀和规划耗时，不能只凭局部图10 Hz就照搬。

Theta*直接使用planner_server持有的Costmap2D对象；它不靠订阅2 Hz的OccupancyGrid取得地图。当前BackUpFreeSpace配置的global_costmap/get_costmap服务同样提供当前内存图。使用costmap_raw话题的其他消费者，则还受发布周期影响。

publish_frequency只是更新循环中的ROS时间门槛，理想500 ms发布间隔也会受200 ms循环采样和负载影响。若以2.5 m/s运动，理想200 ms内移动0.5 m；这是配置时间尺度计算，未包含感知、TF、规划、控制和执行延迟，也不是本次实测。更新／发布循环

BT中的RateController hz=3控制规划请求节奏，planner_server.expected_planner_frequency主要用于耗时检查，二者都不会把此5 Hz地图循环改成另一个频率。行为树调度

resolution与未配置的rolling_window（最终尺寸来自/map）

当前rolling_window未写，源码缺省False。初始化暂用缺省width=5 m、height=5 m、origin_x=origin_y=0和本块resolution=0.05；这组初始参数不是收到静态地图后的最终全局网格。

非滚动StaticLayer收到完整地图时，如果主图的宽高格数、分辨率、平面原点不匹配，或尚未锁定尺寸，就调用resizeMap并锁定尺寸，所有层随之matchSize。最终依据是：

全局x格数 = /map.info.width
全局y格数 = /map.info.height
全局平面格距 = /map.info.resolution
全局平面原点 = /map.info.origin.position.x、y
平面总格数 = width格数 × height格数

例如假设收到800×600、0.05 m／格的地图，全局覆盖40×30 m、共480000格；它不会缩成5×5 m。这仅说明换算，未读取当前运行/map，不能作为本次实际尺寸。

自定义体素层的x、y格数也跟随整张全局图，不只是为10 m量程单独分配一个局部窗口。每个平面列用32位打包高度状态；在固定物理覆盖下，把实际地图格距减半会使平面格数约4倍，也增加体素列、膨胀缓存和规划负担。

锁尺寸后，运行中设置resolution、width、height、origin_x／y可能改变ROS参数值及节点成员，但当前节点回调不会重新resize已锁地图。后续完整/map元数据改变仍可让StaticLayer重新调整。要核对有效尺寸，应看实际costmap消息／服务元数据及StaticLayer的Resizing日志，不能只看参数列表。地图重建与锁定、运行中尺寸回调

global_frame与robot_base_frame（同一张map中的点云、地图和机器人）

当前完整地图通常以map为header.frame_id，全局网格也选择map。1.1.20的StaticLayer在非滚动分支直接按对应格索引合成；只有滚动分支才查询地图坐标系与global_frame之间的TF并取样。因此不能把本块global_frame随意改为odom，再期待静态层自动重投影整图。非滚动与滚动合成分支

地形点云仍按自己的header.frame_id，经消息时间戳的TF转到map；机器人轮廓放在map中的gimbal_yaw_fake位姿上。应一起核对静态地图、先验定位和点云的坐标约定，以及map↔odom↔front_mid360／gimbal_yaw_fake链。话题名带机器人namespace不证明这些坐标系已经隔离或正确对齐。

robot_radius=0.2生成近似圆形多边形，再按默认footprint_padding约0.01 m填充。实现按顶点方向填充，最终内切半径不能简单等同于精确0.21 m圆。需测量虚拟原点到真实车体边界、云台和转动包络；局部与全局轮廓都应符合这套几何关系。轮廓生成与填充

如果改成矩形footprint，还须处理当前虚拟参考系抵消底盘yaw的设计：底盘固连矩形不会仅因填入顶点就自动按真实底盘方向转动。相关参考系关系见fake_vel_transform。

track_unknown_space（保留未知状态与是否允许规划经过是两件事）

实物顶层True让主图和障碍层以NO_INFORMATION=255为默认值，并让静态图未知像素保留255。已知空闲仍为0，墙仍为254；这不是把整张静态地图都设为未知。

当前planner_server.GridBased实际使用nav2_theta_star_planner/ThetaStarPlanner，未显式配置allow_unknown，1.1.20缺省True。常规搜索会允许未知格参与候选，同时给它较高的经过代价；因此未知区保留为灰色后，仍可能出现穿过它的路径。若任务要求禁止未知区，应单独核对规划器策略，不能用unknown_threshold=5实现。Theta*缺省allow_unknown、搜索中的未知格判断

当前仿真文件缺少顶层track_unknown_space，缺省False会把静态图未知值转为空闲，并让障碍层默认空闲。此时原来的未知身份已经丢掉，之后只把规划器allow_unknown改False也不能从0恢复它。需要一起明确地图保留未知的方式、膨胀怎样处理未知、规划器是否放行。

这些差异是配置与源码核对结果，未对运行节点做参数快照。切换到仿真时不要只检查use_sim_time；是否统一未知策略取决于任务，本文保留原YAML并记录差异。真正读取顶层键、静态未知像素解释

intensity_voxel_layer（扩展地形障碍的筛选与合成）

参数表（工作方式）

参数表（体素、高度与地形高差）

参数详解

map中的z、上游高差与体素覆盖（三个不同概念）

ObservationBuffer先把terrain_map_ext从消息坐标系转到map，再按来源0～2 m筛z；本层再次按0～2 m筛z，同时按intensity=0.1～2 m筛地形高差。通过这些门槛之后，还需满足三维量程、平面地图边界、垂直格索引和mark_threshold。

高度窗口用的是map中的点z；intensity沿用上游计算的地形高差。 TF改变XYZ，不会把intensity重算成map地面高差。地形点并未被压平到地面上。

当前高度格索引为floor((z_map−0)/0.05)，要求0≤索引<16；两级高度下界又排除了负z，因此有效标记区间为map中的[0,0.8) m。z=0.8对应索引16，越界。网格不会随底盘爬坡自动向上移动，map的z=0也不能仅凭名称认定就是每处实际地面。

局部使用odom中的[0,0.8)，全局使用map中的[0,0.8)。只有map←odom变换保持相同z且不混合竖直方向时，这两个窗口才选中相同高度。若有z平移或roll／pitch修正，同一个odom点转入map后的z可能不同；定位修正也可能让地图上的实时障碍位置变化。

例如假设odom点z=0.6 m、map←odom只有z平移+0.3 m，则全局看到z_map=0.9 m并丢掉，而局部仍可能标记。这只是坐标换算例子，未测得当前TF偏移。实车应先核对静态图对齐、地面高度基准和定位变换，再决定怎样共同配置两级高度、origin_z与z_resolution；只把max_obstacle_height从2调到3无效。实际筛点、体素换算

min_obstacle_intensity（10 cm高差并不等于可通过台阶）

当前4 m内借入近场terrain_map，远处自己计算abs(点高度−估计地面高度)。远场的较高侧和较低侧都可能出现0.15 m高差，满足其他窗口时都可成为障碍；正常坡面也可能因分片地面估计而过门槛。4 m交接处还存在两套上游网格、点密度和高度规则的差异。近远场交接及绝对高差

0.1只决定高差筛选下界，不是车辆可跨10 cm或可爬某坡度的结论。先看地面估计和真实障碍回波，再结合车辆能力调整；提高门槛可能放掉需要避让的低矮物。

上游当前正常近场高差不足0.5 m，自算远场不足1.0 m，所以本层2.0 m高差上限通常不会进一步裁剪正常输出。若换输入或改上游规则，就要重新解释。接原始雷达点云时intensity可能是反射强度，不能沿用此处的米单位。

mark_threshold与unknown_threshold（已标记高度格和未使用成员）

mark_threshold按一个平面列中不同高度格的占用数量判断，严格超过门槛才标二维障碍。当前0表示1个高度格就够，1表示至少2格，2表示至少3格；同一高度格里有100个点也只算1格。每轮体素重置后重新计数，不靠多个更新周期累计凑数；最新地形点云自身仍可能包含历史点。VoxelGrid标记计数

unknown_threshold在初始化保存为5+(16−z_voxels)，当前为5；本自定义实现后续没有使用它计算未知列、清射线或投影二维未知状态。调这个值不会改变当前未知区域，也不会改变规划器的allow_unknown。它与节点顶层track_unknown_space的使用路径不同。

层内track_unknown_space=True同样没有被父类读取；实物已有有效的顶层True，所以本块是冗余键，不能照搬局部那块“缺少顶层设置”的结论。即使顶层True，本层也不会自动获得标准VoxelLayer的射线空闲推断。自定义初始化与更新

combination_method与footprint_clearing_enabled（实时障碍如何加到静态图）

当前1为Max，但不是简单把255当最大数：本层自身未知格被跳过；主图原来未知、本层为已知时会写入本层值；两者都是已知则取较大代价。当前合成顺序和默认use_maximum=False使静态层先写静态结果，之后本层叠加实时障碍。Max的未知格处理

本层每轮resetMaps重建，不是把本轮10 m范围外全部判成空闲。顶层True时，未标记区域默认未知；合成会跳过这些未知格，原静态图仍起作用。没有点或者点被过滤，不能由此证明区域可通行。

轮廓清空仅将本层二维多边形内设0，不清体素位，也不通过Max降低静态墙。错误或过大的轮廓可能删掉本层车旁真实障碍；在静态未知区还会写出局部已知空闲，需按实际几何核对。

每轮重建、旧观测与清全局图服务

自定义updateBounds每轮先重置自身二维图和体素，再取标记观测重新标记。它不调用getClearingObservations或raytraceFreespace，直接把来源clearing设True不会补上射线清空。默认observation_persistence=0只留最后一份观测，最后一份并不会立即过期；expected_update_rate=0也不检查停更。

所以动态障碍残留可能来自最新terrain_map_ext中的历史点、最后一份停更观测或静态地图，而不是简单由“全局图不滚动”导致。主图按各层提供的更新边界重新合成，膨胀层还会扩展到上一轮边界；本层重置整张内部图不意味着每轮都完整发布或重写整张合成图。合成更新边界、膨胀更新边界

本类isClearable=False使ClearAround／ClearExcept等区域清理跳过它；但BT所用ClearEntireCostmap对应整图服务，会resetLayers并调用所有层reset，包含本层。重置不删除上游地形缓存，也不把最后观测彻底丢弃，下一轮相同内容仍可重建障碍；静态墙也会重新铺回。区域／整图清理、整图重置

intensity_voxel_layer.terrain_map_ext（观测来源）

参数表

参数详解

topic、sensor_frame与TF（点在哪里，距离从哪里量）

工程bringup先用机器人namespace替换<robot_namespace>，再加载参数；绕过该启动流程时需完成替换。当前terrain_map_ext输出header.frame_id为odom；ObservationBuffer按消息时间把XYZ转到map，并按同一时间取得front_mid360原点在map中的位置，保留包括intensity在内的点字段。

sensor_frame决定量程原点，不覆盖消息header.frame_id。显式设置front_mid360后，消息过滤器还要求此系可变换。输入采用SensorDataQoS，父类队列深度50；有点云话题不证明本层缓冲已经成功接收，也不证明TF时间匹配。观测换系与原点

量程计算为sqrt((x−x_sensor)²+(y−y_sensor)²+(z−z_sensor)²)，严格保留0.2<距离<10 m。相同刚体变换下距离保持不变，但变换后的绝对z可以改变；所以量程通过不证明map高度窗口通过。

10 m筛选也不能创造上游没输出的点，不能把地图外点写进网格，不能补回远处被地形高差上限删掉的障碍。近场／远场交接按上游输入位置的水平距离判断，此处量程按雷达原点的三维距离判断，两种边界不能混用。

static_layer（完整二维地图与网格几何）

参数表

参数详解

正常导航时完整图来自map_server，建图时来自slam_toolbox。缺省层私有map_topic为空，转用节点默认的机器人namespace/map，通常/red_standard_robot1/map。它不是global_costmap/costmap，更不是先验PCD直接输入。

当前默认trinary_costmap=True、lethal_cost_threshold=100、unknown_cost_value=255：OccupancyGrid的-1转成255后按顶层track_unknown_space保留未知或转空闲；已知值达到100为致命障碍，其余低于门槛的已知值按三态规则转为空闲。占用概率与膨胀后的内部软代价不是同一套数值。静态像素解释

当前use_maximum=False使非滚动静态层在更新区域先完整覆写静态结果，再由本实时层Max添加障碍；不要把本层combination_method=1误认为静态层也使用同一个参数。静态层自己的footprint_clearing_enabled缺省False，本块明确开启的是实时层的轮廓清空。

True的地图订阅QoS使晚启动的订阅者可取得兼容发布端保留的完整图。如果发布端耐久性或可靠性不兼容，应核对QoS；延长TF等待无法修复订阅不兼容。当前subscribe_to_updates=False不订阅map_topic加_updates的增量，但仍处理后来收到的完整/map；建图产生的新完整地图也可更新尺寸与内容。订阅QoS及更新话题

inflation_layer（全局路径的障碍距离代价）

参数表

参数详解

两项膨胀参数与当前Theta*（软代价实际参与路径评分）

内部代价0表示空闲，1～252通常称软代价，253为轮廓内切半径的高代价，254为致命障碍，255为未知。发布成OccupancyGrid后数值经过转换，不能拿RViz消息中的占用数直接代替内部值。

设d为距致命障碍格的实际平面距离，r_ins为填充后轮廓的内切半径。障碍格为254，内圈为253；内圈外、膨胀范围内按下式计算并取整数：

cost = 252 × exp(−cost_scaling_factor × (d − r_ins))

因此4增大时外圈代价更快降到低值，减小时高代价保持得更远；0.7决定写代价的范围。仅为说明公式，假设r_ins=0.2 m、d=0.4 m，系数4约得113，8约得50，2约得169；实际轮廓含padding，格距以/map为准，不能把这些假设数当实测。膨胀代价公式

当前Theta*常规搜索使用地图代价计算经过费用，当前配置w_traversal_cost=10.0，因此外围代价变化可以改变全局路径选择。1.1.20常规搜索将普通格的内部代价252及以上视为不可用，另对255按allow_unknown处理；这与当前局部PID抽样使用≥253阈值不同。“非致命软代价”不保证每一种规划器都放行。Theta*代价与通行判断

0.7 m膨胀区不会全部变成障碍，也不保证路径距墙至少0.7 m。需要结合有效轮廓、地图格距、定位误差、规划器代价权重和实际路径观察。局部图中改变外圈颜色不一定改变当前PID的抽样通过条件；全局图中这些代价则会进入Theta*路径评分，两张图的调参效果不能直接互推。当前局部控制器用法

未知格与膨胀开关（当前不把未知本身当障碍种子）

当前inflate_around_unknown缺省False，不主动以255未知格为膨胀起点；inflate_unknown缺省False，已知障碍扩到未知格时，不用1～252外围软代价覆盖未知，但仍可能写入≥253的内圈高代价。

这两个开关独立于顶层track_unknown_space，也独立于Theta*的allow_unknown。未知身份保留、边界膨胀和规划经过策略需一起检查；改其中一个不能默认得到另外两个的效果。未知格写入条件

always_send_full_costmap（固定整图也可以发增量）

False时，尺寸、分辨率和平面原点不变且有更新范围，可发布costmap_updates；首次发布或完整地图几何变化仍走全图分支。当前没有随车滚动原点，比局部图更容易满足几何不变条件，但不能保证每次都是增量。

costmap_raw有订阅者时始终准备完整nav2_msgs/Costmap，不受此开关改成增量；内部地图和get_costmap服务也不按该开关缩成更新块。因此这个开关主要影响OccupancyGrid发布方式，不能提高内部障碍刷新或控制Theta*读图频率。全图／增量与raw发布

相关源码默认项（原YAML未显式配置）

参数表

下面20项为网格初始化、轮廓、观测缓冲和未知膨胀的源码默认设置；最后一项属于planner_server，用于解释本图的当前消费者。不是本次向原工程追加了这些参数。

observation_persistence=0和expected_update_rate=0共同意味着：消息停止时最后一份观测可能继续被重建，且不会因该来源超时自动变为不current。地图继续发布不证明感知继续更新，应观察terrain_map_ext时间戳、来源缓冲与系统的数据停更处理。缓存保留和current判断

与local_costmap的关系、动态修改与验证范围

当前两张地图的主要区别

障碍出现差异时，先沿“上游输入→TF后的z→高差→量程→平面范围→合成→消费者”定位。只让两张地图的YAML数字相同，不能保证实际选点、未知策略或路径通过条件相同。局部代价地图

动态修改的实际范围

顶层transform_tolerance虽有节点成员更新分支，已建ObservationBuffer、消息过滤器及静态层另有初始化时保存的值；一次修改不保证所有等待同步更新。节点动态回调、障碍父类回调

启动来源与本次核对

• 本次已核对： 相关安装包版本、已有自定义层注册／库／头文件；本地自定义实现与固定上游提交一致；Theta*安装头文件与1.1.20一致；源码和安装副本的两套global_costmap段一致，实物／仿真差异为时钟与顶层未知键。
• 源码能确认： 38项原参数的实际读取／使用条件；静态图重建并锁定非滚动网格；实物顶层未知开关生效、层内同名键与unknown_threshold未参与相应计算；map垂直标记窗口[0,0.8) m；三维严格量程；每轮重建但不射线清空；旧观测保留；Max合成、膨胀方向及当前Theta*的未知／软代价使用。
• 本次验证范围： 未启动新的导航或建图任务，未取得运行/map与costmap元数据、有效参数快照或路径对比；没有实测map←odom的高度偏移、坡道筛点、动态障碍残留或未知区经过行为。文中尺寸、高度和时间例子均为配置／源码计算。
• 实物待核实： 实际静态图尺寸／格距／原点与坐标约定、先验地图对齐、map高度基准、真实轮廓与虚拟参考点、点云字段和TF时间、来源停更处理、近远场交接、未知规划策略以及全局路径到局部执行的通过空间。