local_costmap（區域性障礙代價地圖）

1.local_costmap（區域性障礙代價地圖）

1.1.實車先看：地圖上的障礙來自哪裡

當前區域性代價地圖是以gimbal_yaw_fake位置為中心、在odom中表達的約5×5 m滾動網格。 它按順序合成靜態二維地圖、即時地形障礙和障礙周圍的膨脹代價，供controller_server使用。

/map二维占用图 → static_layer ──────────────────────────────┐
                                                         │
terrain_map（odom系地形点，intensity为离地高差）            │
    → 观测缓冲：TF、来源高度筛选、雷达原点                  │
    → intensity_voxel_layer：高度、高差、量程与体素筛选 ───┤
                                                         ↓
                                        inflation_layer → 局部合成代价地图
                                                         ├─ controller_server读内存中的地图
                                                         └─ costmap／costmap_updates／costmap_raw供订阅者

本塊直接接收近場terrain_map；全域性代價地圖接收terrain_map_ext，下一塊再整理。pointcloud_to_laserscan生成的obstacle_scan用於二維建圖，本區域性障礙層不經過那份二維掃描。因此修改掃描轉換器的高差門檻，不會直接修改這裡的門檻。地形分析分工、二維掃描轉換

本次容器內nav2_costmap_2d、nav2_voxel_grid均為1.1.20，pb_nav2_plugins為1.0.1。本地自定義層實現與標頭檔案和上游提交e6778ca44d9e88aa612a3c6c87d536a8cfa21705一致；以下按該版本核對。自定義層實現、體素換算與類定義

1.2.引數

下面保留髮送的完整引數與原註釋。實物／模擬段只有use_sim_time不同；原始碼預設值及配置建議另列。

local_costmap:
  local_costmap:
    ros__parameters:
      use_sim_time: False
      update_frequency: 10.0
      publish_frequency: 5.0
      global_frame: odom
      robot_base_frame: gimbal_yaw_fake
      rolling_window: true
      width: 5
      height: 5
      resolution: 0.05
      robot_radius: 0.2
      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
        terrain_map:
          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
          sensor_frame: front_mid360
          min_obstacle_height: 0.0
          max_obstacle_height: 2.0
          obstacle_max_range: 5.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

兩個local_costmap層級用於匹配名稱空間和節點名。例如機器人名稱空間為/red_standard_robot1時，當前controller_server建立的代價地圖節點為/red_standard_robot1/local_costmap/local_costmap；釋出話題通常位於/red_standard_robot1/local_costmap下面。座標系字串不會隨這個名稱空間自動加字首。

1.2.1.ros__parameters（更新、座標、網格與機器人輪廓）

1.2.1.1.參數列

1.2.1.2.引數詳解

1.2.1.2.1.update_frequency、publish_frequency與always_send_full_costmap（內部10 Hz與話題5 Hz各負責什麼）

內部更新迴圈以WallRate執行，呼叫各層updateBounds、updateCosts合成地圖。controller_server通過持有的local_costmap物件直接讀取這張地圖；它的20 Hz控制迴圈與本塊10 Hz更新各自執行，部分控制輪次可能讀到同一輪地圖結果。

publish_frequency控制更新迴圈中是否到了釋出時間，不是獨立的5 Hz釋出定時器；實際受更新時間、負載和ROS時鐘影響。若以2.5 m/s運動，理想100 ms更新間隔內已移動0.25 m，即5個當前格距；這是時間尺度計算，不是本次實車測量。調頻率需要結合整條感知和執行鏈延遲。更新與釋出迴圈（1.1.20）

False不會保證只發增量：網格原點滾動、尺寸或解析度變化時，OccupancyGrid會走全圖分支；幾何資訊不變且有更新範圍時，才可能發costmap_updates。costmap_raw不走這個增量選擇，有訂閱者就準備完整nav2_msgs/Costmap。使用地圖話題的行為與讀取服務的行為，還應各自核對資料介面。釋出選擇

1.2.1.2.2.global_frame、robot_base_frame與rolling_window（odom中的移動視窗）

網格軸按odom的x、y表達，中心跟蹤gimbal_yaw_fake的平面位置，平面原點以格距對齊滾動；它不是隨底盤姿態旋轉的車體圖。當前虛擬系的方向與速度轉換關係見fake_vel_transform。

區域性地圖使用odom可以減少全域性重定位修正直接作用到區域性點雲座標的影響，但靜態層仍從map系地圖取樣，需要有效map↔odom。只有odom至機器人TF，不證明三層合成所需TF已經全部可用。

視窗滾動只處理平面x、y。體素origin_z=0仍固定在odom的z基準上，機器人上坡、下坡不會自動平移這段垂直視窗。滾動原點實現

1.2.1.2.3.width、height與resolution（5×5 m，100×100格）

當前約覆蓋參考點兩側各2.5 m，角落的平面距離約3.5 m；實際原點受格距對齊，不能把邊界當成精確連續對稱的圓。5 m觀測量程用於篩點，最終落在網格外的點仍不能標記。

平面共有10000格。自定義層再為每個平面列記錄16個高度狀態，邏輯上約160000個體素；實現用每列32位打包，不是為每個體素存一份完整點雲。減小z_voxels不等於平面列快取記憶體按比例減少。

平面resolution與垂直z_resolution是獨立引數，當前恰好都為0.05 m。改變平面格距還會影響障礙合併、輪廓柵格化、膨脹和控制器區域性路徑處理；它不能增加MID360回波密度或恢復地形分析已經過濾掉的小障礙。

1.2.1.2.4.robot_radius（圍繞虛擬參考點的車體輪廓）

當前預設footprint="[]"，因此用robot_radius=0.2生成近似圓形多邊形，再按預設footprint_padding≈0.01 m填充。padding按多邊形頂點方向實現，不應把最終輪廓簡單等同於精確半徑0.21 m的圓。輪廓建立與填充

該輪廓以gimbal_yaw_fake原點為中心。需按真實底盤、雷達、雲臺及其活動包絡核對0.2是否覆蓋，而不是繼承示例半徑；安裝參考點偏置也要計入。膨脹不能替代正確的車體輪廓。

如果以後改成車體矩形footprint，須處理其座標軸和真實底盤姿態：當前虛擬系在平面上抵消yaw，不會自動隨底盤轉向。可選擇覆蓋轉動包絡的輪廓，或按實際姿態提供正確的動態輪廓，不能直接填一個底盤固連矩形就認定方向已正確。

1.2.2.intensity_voxel_layer（地形高差與三維障礙篩選）

1.2.2.1.參數列（工作方式與合成）

1.2.2.2.參數列（高度、體素與高差）

1.2.2.3.引數詳解

1.2.2.3.1.高度、體素視窗與intensity（三道不同的篩選）

當前完整標記流程包括：來源緩衝高度篩選、本層高度篩選、地形高差篩選、相對感測器的三維量程篩選、平面與高度格範圍，以及mark_threshold。通過其中一項不保證最終進入地圖。

點的z是odom座標；intensity是上游估計地面的高差。 假定量程、平面位置和其他條件都滿足：

當前整數高度索引為floor((z−0)/0.05)，要求索引小於16，故有效範圍為**[0,0.8) m**。z=0.8對應索引16，越界；上界不包含。這段視窗固定在odom，odom的z=0並不憑名稱保證就是實車當前地面。

上坡高處、下坡低處的障礙即使地形高差正確，也可能被高度視窗刪掉。若需要改變覆蓋，必須一起核對來源高度、本層高度、origin_z與16層限制內的z_resolution；只把max_obstacle_height從2調到3不能解決體素上界。也不能為了放行坡面就把所有危險低障礙一起過濾。實際篩點與索引轉換

1.2.2.3.2.min_obstacle_intensity與max_obstacle_intensity（是否把地形高差當障礙）

近場terrain_analysis已經把intensity改成相對估計地面的高差，當前下界0.1即10 cm。它不是“車輛能跨10 cm”，也不是坡度門檻。正常坡面若被錯誤估成15 cm凸起，滿足其他視窗時仍可能成為障礙；應先看地面估計與座標，再按真實車輛和場地調整門檻。

上游當前近場正常高差輸出受vehicleHeight=0.5 m限制，所以這裡2 m上限通常不會進一步裁剪正常點。若改輸入、上游規則或出現特殊標記點，需要重新判斷。直接改接原始雷達點雲後，intensity可能變回反射強度，不能沿用“0.1 m／2 m”的解釋。

1.2.2.3.3.mark_threshold（數高度格，不數原始點）

VoxelGrid用位記錄每個平面列的已標記高度格；已標記格數嚴格大於mark_threshold才返回可標障礙。當前0表示一格就夠。設1則至少要兩格，設2至少三格；同一高度格里再來很多點，不會增加高度格計數。標記計數實現

本層每次更新都會先重置二維圖與體素，所以門檻統計來自本輪取得的觀測內容；它不會靠連續多個更新週期逐漸湊夠格數。但最新terrain_map本身包含上游歷史點，不能把“一份最新觀測”理解成一份純即時原始掃描。

低矮物、薄障礙、稀疏回波可能只佔一個高度格，提高門檻可能讓它們消失；能否過濾噪聲應按實物觀察，不保證越大越穩。

1.2.2.3.4.track_unknown_space與unknown_threshold（層級錯誤和未使用的門檻）

自定義層呼叫ObstacleLayer::onInitialize，後者讀的是節點頂層track_unknown_space；Costmap2DROS及StaticLayer也讀該頂層鍵。當前層內True沒有進入這條讀取路徑，頂層未配置時預設False，主圖和障礙層預設按FREE_SPACE重置；靜態圖的未知值也會按該設定轉為空閒。父類讀取位置、靜態未知值解釋

若要表達原配置“跟蹤未知空間”的True意圖，正確層級為：

local_costmap:
  local_costmap:
    ros__parameters:
      track_unknown_space: true

此時應移除無效的層內同名鍵，重新核對未知區在規劃與控制中的處理。若任務選擇不跟蹤未知，則應明確頂層False；錯誤是當前寫入層級，不是要求所有任務必須使用True。 這裡只提供修正說明，沒有修改工程配置。

unknown_threshold另屬自定義成員：初始化儲存5+(16−z_voxels)，當前仍為5，但後續沒有用它清體素射線、統計未知列或生成二維未知狀態。調整5不會改善當前未知空間判定。成員賦值與更新路徑

即使頂層改True，本層也只是以未知預設二維格重置，再標出障礙和輪廓內空閒，不會因此自動得到標準VoxelLayer的完整射線空閒／未知推斷。沒有回波或點被過濾，不等於真實空地。

1.2.2.3.5.每輪重建、輪廓清空與清地圖服務（幾種清除各有範圍）

該自定義updateBounds先resetMaps，再拿marking observations重建障礙，最後清本層輪廓內二維代價。它不呼叫getClearingObservations或raytraceFreespace，直接把來源clearing改True也不會給當前實現補上射線清空。

footprint_clearing_enabled=True只清本層二維輪廓，不擦除已儲存的體素標記；當前Max合成也不會用FREE_SPACE覆蓋static_layer的靜態牆。輪廓、點雲自車濾除和靜態地圖錯誤應分開核對。

清地圖服務還需區分：類的isClearable=False使按區域的ClearAround／ClearExcept服務跳過它；但ClearEntirely呼叫resetLayers，會重置主圖並呼叫所有層的reset，包括本層。前面的BT使用的是ClearEntireCostmap，因此不能說它因為isClearable=False就完全不重置。重置也不丟棄上游地形快取，下一輪相同觀測／靜態圖仍可能把障礙加回來。區域與整圖服務、整圖層重置

本層內部重建不保證主圖所有舊格立即變空；合成還受各層更新範圍、滾動、靜態佔用和最新觀測影響。動態殘留應結合上游terrain_analysis衰減與實際話題觀察。

1.2.2.3.6.combination_method（當前Max保留已有障礙）

當前1呼叫updateWithMax：本層未知格跳過；已知格可以替換主圖未知格，或在代價更高時寫入。FREE_SPACE不會降低前面靜態層的已知障礙；不能簡單把255未知當作普通數字最大值來推斷全部合成結果。Max合成

0為Overwrite，可用本層非未知代價覆蓋之前的圖層；當前頂層預設不跟蹤未知、重置大量格為FREE_SPACE，改0可能擦掉靜態層的已知障礙。其他整數在當前父類switch中不做合成；新版文件中的其他列舉不代表本版本可用。

1.2.3.intensity_voxel_layer.terrain_map（地形觀測源）

1.2.3.1.參數列

1.2.3.2.引數詳解

1.2.3.2.1.topic、data_type與sensor_frame（點雲座標和測距原點分別取哪裡）

bringup_launch先把<robot_namespace>替換為空或“/＋機器人名稱空間”，隨後引數重寫和載入；當前配置通常訂閱/red_standard_robot1/terrain_map。佔位符沒有在層原始碼中自行處理，直接啟動原始引數檔案時必須完成對應替換。

terrain_map的點座標當前在odom中。ObservationBuffer按header.frame_id把點雲轉換到global_frame；另以sensor_frame指定的原點、同一訊息時間戳取得odom中的雷達位置，再供量程計算。因此sensor_frame不會把odom點重新解釋成雷達區域性點。觀測緩衝換系與原點

顯式設定front_mid360後，訊息過濾器還要求對應感測器系可變換。TF、時間域、名稱空間和QoS都需要匹配；有地形話題不證明本層已成功接收緩衝。輸入訂閱採用SensorDataQoS，父類配置佇列深度50；佇列不是50 Hz或50 ms。

1.2.3.2.2.來源高度與量程（高度在odom，距離從雷達量）

來源高度先在轉換後的odom點座標上篩選，並保留包括intensity在內的原點欄位；本層再次檢查自己的高度門檻。當前兩級0～2 m相同，實際可標記的上界仍受0.8 m體素覆蓋制約。

量程使用sqrt((x−x_sensor)²+(y−y_sensor)²+(z−z_sensor)²)，當前嚴格保留0.2～5 m之間的點。與二維掃描轉換器按底盤虛擬中心計算水平距離不同，雷達安裝偏移、雲臺轉動和上下坡會影響這個三維距離。

近場地形輸出和區域性視窗本就有限。增大量程不會擴大5×5 m網格、不能恢復上游被刪的點，也不會讓MID360取樣更遠；最小量程也不能濾掉距雷達超過0.2 m的全部自車結構。

1.2.4.static_layer（靜態二維地圖接入）

1.2.4.1.參數列

1.2.4.2.引數詳解

1.2.4.2.1.static_layer（map圖仍要經過map↔odom變換）

靜態層訂閱完整OccupancyGrid，儲存其尺寸、解析度、原點和header.frame_id。滾動模式不會強行把主圖變成整張靜態地圖的大小；它根據當前odom格子的世界位置，查詢map_frame←odom的最新變換，到靜態圖中取樣。因此local_costmap在odom中執行，仍需正確的全域性定位變換。滾動靜態圖取樣

當前頂層track_unknown_space預設False，靜態圖未知格轉為FREE_SPACE；已知牆仍保留。沒有收到地圖或對應TF查詢失敗時，點雲這一路即使存在，也不能證明靜態層已正常合成。當前controller_server還會等待代價地圖current狀態，地圖輸入和各層狀態要一起看。

1.2.4.2.2.map_subscribe_transient_local（保留地圖訊息與更新話題各自獨立）

True讓較晚啟動的訂閱者可以從相容釋出端取得保留的最近完整地圖。若釋出端只提供Volatile耐久性或可靠性不相容，不能靠延長TF等待取得地圖；應先核對釋出端與訂閱端的QoS。地圖訂閱QoS

當前subscribe_to_updates預設False，不訂閱/map_updates；這不阻止處理隨後收到的完整/map訊息。“static”表示圖層輸入方式，不表示完整地圖永遠不能更新。

1.2.5.inflation_layer（障礙周圍的二維代價）

1.2.5.1.參數列

1.2.5.2.引數詳解

1.2.5.2.1.cost_scaling_factor與inflation_radius（範圍和代價下降速度）

內部代價值與釋出後的OccupancyGrid佔用值不同：

設d為距致命障礙格的實際平面距離、r_ins為實際填充輪廓的內切半徑。障礙處為254，內圈為253，內圈之外到膨脹範圍內按下面計算後取整數：

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

所以4變大時，外圍代價更快降低；變小時，較高軟代價維持得更遠。0.7決定影響範圍，4決定範圍內的衰減，不能把兩個引數當同一作用。代價公式（1.1.20）

僅為說明公式，假設r_ins=0.2 m、d=0.4 m：當前4對應約113，改為8約50，改為2約169。實際輪廓含padding、柵格化和最近障礙距離，不能把這個假設值當執行讀數。

0.7 m以內並非全部致命或253，也不是保證車體距牆至少0.7 m。輪廓定義、真實包絡、地圖誤差和路徑使用方式都影響實際通過距離。

1.2.5.2.2.當前PID怎樣使用這些代價（軟梯度不等於已經參與避障評分）

當前OmniPidPursuitController從區域性路徑抽10個點，檢查落格代價是否達到INSCRIBED_INFLATED_OBSTACLE，即≥253；它不按1～252的軟梯度給候選軌跡打分。改變區域性cost_scaling_factor或擴大外圍軟代價，可能改變RViz顏色，卻沒有直接改變本外掛的碰撞通過條件。

robot_radius及padding改變的內切半徑會影響253內圈；靜態牆／即時障礙本身也參與高代價檢查。全域性路徑又由另一張global_costmap規劃，其膨脹設定需下一塊單獨核對，不能把兩張地圖的引數影響混為一談。當前控制器地圖使用方式

前文還記錄了抽樣數量及地圖外點的處理缺口。這裡的膨脹範圍不是完整車體掃掠碰撞檢查的替代，也不補回本障礙層遺漏的高度點。

1.2.6.相關原始碼預設項（原YAML未顯式配置）

1.2.6.1.參數列

以下12項補充真正讀取的節點頂層設定、觀測緩衝與未知膨脹行為；不表示本次已向工程YAML追加。

1.2.6.2.觀測停更與預設未知狀態

observation_persistence=0只刪除比最新觀測更舊的觀測，保留最後一份；expected_update_rate=0直接將緩衝判為current。於是點雲停止到達時，本層可能繼續每輪重建同一份舊地形，而不是自動清空障礙或停止控制。還需結合靜態層、其他層狀態和完整系統的資料監測判斷，不能只看地圖話題仍釋出。快取保留與current判斷

如果以後設定正的expected_update_rate，按秒的訊息間隔理解，再核對正常資料週期與延遲；不能填10就當成10 Hz。只改變buffer保留時間也不去除terrain_map內部已經累計的歷史點。

1.2.7.動態引數、啟動來源與驗證範圍

1.2.7.1.動態修改的實際範圍

自定義層沒有另寫覆蓋體素、高差和釋出器的動態回撥，使用父類ObstacleLayer的回撥；因此執行中引數設定成功，不代表所有成員已經更新。

頂層transform_tolerance有成員更新分支，但已經建立的來源緩衝、訊息過濾器和靜態層各有初始化時儲存的值；不能認為一次改值讓所有TF等待同步變化。節點回調、父類層回撥

1.2.7.2.啟動與核對範圍

• 本次已核對： 當前包版本、外掛註冊與liblayers.so存在；自定義層安裝標頭檔案與本地一致，本地實現與上游固定提交一致；Nav2的體素、合成與膨脹安裝標頭檔案匹配1.1.20；兩份local_costmap段與安裝副本一致，模擬僅時鐘不同。
• 原始碼能確認： 40項原配置的讀取／使用條件；層內未知開關位置無效、unknown_threshold未參與計算；當前odom垂直標記區間[0,0.8) m；按不同高度格計標記；每輪重建但不射線清空；區域與整圖清理不同；預設舊觀測複用、膨脹方向和全圖／增量釋出條件。
• 本次驗證範圍： 未啟動新的地圖或導航任務，未觀察執行節點有效引數、坡道點、未知區、動態障礙消失時間或車旁碰撞；視窗、體素和膨脹例子是配置／原始碼計算。
• 實物待核實： odom地面高度基準、上下坡與高低障礙是否過視窗、真實車體輪廓與虛擬參考點、點雲欄位與TF時間、觀察源停更處理、靜態圖對齊／QoS、未知區策略、動態殘留以及控制器實際地圖使用。