global_costmap（全局規劃代價地圖）

1.global_costmap（全局規劃代價地圖）

1.1.實車先看：整張地圖和車周圍的實時障礙怎樣一起參與規劃

當前全局代價地圖在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筆記用來對照術語；自定義層的實際使用條件以源碼為準。自定義層實現、類與體素索引

1.2.參數

下面保留髮送的完整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自動加前綴。全局地圖節點創建

1.2.1.ros__parameters（更新、座標、輪廓與未知區域）

1.2.1.1.參數表

1.2.1.2.參數詳解

1.2.1.2.1.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地圖循環改成另一個頻率。行為樹調度

1.2.1.2.2.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日誌，不能只看參數列表。地圖重建與鎖定、運行中尺寸回調

1.2.1.2.3.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。

1.2.1.2.4.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並記錄差異。真正讀取頂層鍵、靜態未知像素解釋

1.2.2.intensity_voxel_layer（擴展地形障礙的篩選與合成）

1.2.2.1.參數表（工作方式）

1.2.2.2.參數表（體素、高度與地形高差）

1.2.2.3.參數詳解

1.2.2.3.1.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無效。實際篩點、體素換算

1.2.2.3.2.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可能是反射強度，不能沿用此處的米單位。

1.2.2.3.3.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的射線空閒推斷。自定義初始化與更新

1.2.2.3.4.combination_method與footprint_clearing_enabled（實時障礙如何加到靜態圖）

當前1為Max，但不是簡單把255當最大數：本層自身未知格被跳過；主圖原來未知、本層為已知時會寫入本層值；兩者都是已知則取較大代價。當前合成順序和默認use_maximum=False使靜態層先寫靜態結果，之後本層疊加實時障礙。Max的未知格處理

本層每輪resetMaps重建，不是把本輪10 m範圍外全部判成空閒。頂層True時，未標記區域默認未知；合成會跳過這些未知格，原靜態圖仍起作用。沒有點或者點被過濾，不能由此證明區域可通行。

輪廓清空僅將本層二維多邊形內設0，不清體素位，也不通過Max降低靜態牆。錯誤或過大的輪廓可能刪掉本層車旁真實障礙；在靜態未知區還會寫出局部已知空閒，需按實際幾何核對。

1.2.2.3.5.每輪重建、舊觀測與清全局圖服務

自定義updateBounds每輪先重置自身二維圖和體素，再取標記觀測重新標記。它不調用getClearingObservations或raytraceFreespace，直接把來源clearing設True不會補上射線清空。默認observation_persistence=0只留最後一份觀測，最後一份並不會立即過期；expected_update_rate=0也不檢查停更。

所以動態障礙殘留可能來自最新terrain_map_ext中的歷史點、最後一份停更觀測或靜態地圖，而不是簡單由“全局圖不滾動”導致。主圖按各層提供的更新邊界重新合成，膨脹層還會擴展到上一輪邊界；本層重置整張內部圖不意味着每輪都完整發布或重寫整張合成圖。合成更新邊界、膨脹更新邊界

本類isClearable=False使ClearAround／ClearExcept等區域清理跳過它；但BT所用ClearEntireCostmap對應整圖服務，會resetLayers並調用所有層reset，包含本層。重置不刪除上游地形緩存，也不把最後觀測徹底丟棄，下一輪相同內容仍可重建障礙；靜態牆也會重新鋪回。區域／整圖清理、整圖重置

1.2.3.intensity_voxel_layer.terrain_map_ext（觀測來源）

1.2.3.1.參數表

1.2.3.2.參數詳解

1.2.3.2.1.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篩選也不能創造上游沒輸出的點，不能把地圖外點寫進網格，不能補回遠處被地形高差上限刪掉的障礙。近場／遠場交接按上游輸入位置的水平距離判斷，此處量程按雷達原點的三維距離判斷，兩種邊界不能混用。

1.2.4.static_layer（完整二維地圖與網格幾何）

1.2.4.1.參數表

1.2.4.2.參數詳解

正常導航時完整圖來自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及更新話題

1.2.5.inflation_layer（全局路徑的障礙距離代價）

1.2.5.1.參數表

1.2.5.2.參數詳解

1.2.5.2.1.兩項膨脹參數與當前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*路徑評分，兩張圖的調參效果不能直接互推。當前局部控制器用法

1.2.5.2.2.未知格與膨脹開關（當前不把未知本身當障礙種子）

當前inflate_around_unknown缺省False，不主動以255未知格為膨脹起點；inflate_unknown缺省False，已知障礙擴到未知格時，不用1～252外圍軟代價覆蓋未知，但仍可能寫入≥253的內圈高代價。

這兩個開關獨立於頂層track_unknown_space，也獨立於Theta*的allow_unknown。未知身份保留、邊界膨脹和規劃經過策略需一起檢查；改其中一個不能默認得到另外兩個的效果。未知格寫入條件

1.2.5.2.3.always_send_full_costmap（固定整圖也可以發增量）

False時，尺寸、分辨率和平面原點不變且有更新範圍，可發佈costmap_updates；首次發佈或完整地圖幾何變化仍走全圖分支。當前沒有隨車滾動原點，比局部圖更容易滿足幾何不變條件，但不能保證每次都是增量。

costmap_raw有訂閲者時始終準備完整nav2_msgs/Costmap，不受此開關改成增量；內部地圖和get_costmap服務也不按該開關縮成更新塊。因此這個開關主要影響OccupancyGrid發佈方式，不能提高內部障礙刷新或控制Theta*讀圖頻率。全圖／增量與raw發佈

1.2.6.相關源碼默認項（原YAML未顯式配置）

1.2.6.1.參數表

下面20項為網格初始化、輪廓、觀測緩衝和未知膨脹的源碼默認設置；最後一項屬於planner_server，用於解釋本圖的當前消費者。不是本次向原工程追加了這些參數。

observation_persistence=0和expected_update_rate=0共同意味着：消息停止時最後一份觀測可能繼續被重建，且不會因該來源超時自動變為不current。地圖繼續發佈不證明感知繼續更新，應觀察terrain_map_ext時間戳、來源緩衝與系統的數據停更處理。緩存保留和current判斷

1.2.7.與local_costmap的關係、動態修改與驗證範圍

1.2.7.1.當前兩張地圖的主要區別

障礙出現差異時，先沿“上游輸入→TF後的z→高差→量程→平面範圍→合成→消費者”定位。只讓兩張地圖的YAML數字相同，不能保證實際選點、未知策略或路徑通過條件相同。局部代價地圖

1.2.7.2.動態修改的實際範圍

頂層transform_tolerance雖有節點成員更新分支，已建ObservationBuffer、消息過濾器及靜態層另有初始化時保存的值；一次修改不保證所有等待同步更新。節點動態回調、障礙父類回調

1.2.7.3.啓動來源與本次核對

• 本次已核對： 相關安裝包版本、已有自定義層註冊／庫／頭文件；本地自定義實現與固定上游提交一致；Theta*安裝頭文件與1.1.20一致；源碼和安裝副本的兩套global_costmap段一致，實物／仿真差異為時鐘與頂層未知鍵。
• 源碼能確認： 38項原參數的實際讀取／使用條件；靜態圖重建並鎖定非滾動網格；實物頂層未知開關生效、層內同名鍵與unknown_threshold未參與相應計算；map垂直標記窗口[0,0.8) m；三維嚴格量程；每輪重建但不射線清空；舊觀測保留；Max合成、膨脹方向及當前Theta*的未知／軟代價使用。
• 本次驗證範圍： 未啓動新的導航或建圖任務，未取得運行/map與costmap元數據、有效參數快照或路徑對比；沒有實測map←odom的高度偏移、坡道篩點、動態障礙殘留或未知區經過行為。文中尺寸、高度和時間例子均為配置／源碼計算。
• 實物待核實： 實際靜態圖尺寸／格距／原點與座標約定、先驗地圖對齊、map高度基準、真實輪廓與虛擬參考點、點雲字段和TF時間、來源停更處理、近遠場交接、未知規劃策略以及全局路徑到局部執行的通過空間。