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時間、來源停更處理、近遠場交接、未知規劃策略以及全域性路徑到區域性執行的通過空間。