slam_toolbox（二維地圖生成與軌跡最佳化）

1.slam_toolbox（二維地圖生成與軌跡最佳化）

1.1.二維圖、PCD與即時定位：各自負責什麼

當前工程的二維柵格地圖由slam_toolbox生成，三維PCD由Point-LIO儲存。 二維地圖記錄每個平面格子的佔用／空閒／未知狀態；PCD保留三維點的位置，兩者用途不同。

建圖時的兩條輸出鏈為：

Point-LIO配准点云
    ├─ 累计并保存三维PCD
    └─ loam_interface → registered_scan
           → terrain_analysis / terrain_analysis_ext（估计地面与高差）
           → pointcloud_to_laserscan（筛选、转到底盘系、压成二维扫描）
           → obstacle_scan → slam_toolbox → /map（二维栅格）

pointcloud_to_laserscan提供二維掃描，slam_toolbox把掃描放到地圖上並生成柵格；前者本身不儲存二維地圖。當前掃描來自地形點雲，不是MID360原始回波直接截一層。

序列化介面分別寫出.posegraph和.data；它們與二維圖片、三維PCD是不同的儲存結果。slam_toolbox序列化實現（2.6.10）

開啟儲存功能不等於檔案已經落盤。 use_map_saver=True提供地圖儲存服務；當前slam_launch還啟動了Nav2的map_saver_server，但launch沒有自動呼叫儲存服務，二維地圖仍需執行儲存操作。地圖儲存功能說明（2.6.10）

PCD這一路，整合slam_launch把pcd_save.pcd_save_en覆蓋為True；當前interval=-1，且scan_publish_en=True，點雲持續累計，在程式正常退出的儲存分支寫出scans.pcd。異常強制結束可能來不及儲存，長時間累計還會增加記憶體佔用；新一輪儲存可能覆蓋同名檔案，需要按場地和輪次歸檔。

當前安裝的Point-LIO程式中，編譯目錄對應的儲存位置為：

/home/tungchiahui/UserFolder/MySource/ysu2026_rm_sentry/ysu2026_rm_sentry_nav/src/pb2025_sentry_nav/point_lio/PCD/scans.pcd

儲存目錄由編譯時ROOT_DIR確定，詳見Point_LIO的PCD儲存說明；在其他路徑重新編譯後需要重新核對。檔案不會自動成為導航launch指定的場地PCD。

已有地圖導航（slam:=False）的關係為：

保存的二维图片 + YAML → map_server → 静态地图 → Nav2规划
实时registered_scan + 保存的三维PCD → small_gicp → map与odom之间的定位变换
实时terrain_map / terrain_map_ext → 局部／全局代价地图 → 更新当前障碍

因此，導航時會讀取二維圖和PCD；即時匹配的是“當前三維點雲與儲存的三維PCD”。 不是把PCD與二維圖片互相匹配，也不是每幀先存一份PCD再匹配。當前small_gicp按約0.5 s的定時器處理累計點雲，只有匹配收斂才更新結果；這不保證每0.5 s都得到正確定位。常規導航不啟動slam_toolbox和這個二維掃描轉換節點，也沒有用它們進行即時二維地圖匹配。接線見slam_launch.py、localization_launch.py。

二維圖和PCD應來自同一場地、採用一致的尺度、原點和方向；同一輪儲存也不能自動證明兩者對齊。Point-LIO輸出內部IMU位姿，PCD處於camera_init中；後續介面還有IMU／雷達／底盤轉換和先驗PCD啟動變換的疑點，應按前面loam_interface、small_gicp章節核對。改二維圖片的大小、原點或旋轉方向時，也必須考慮PCD定位座標，不能只看圖片外觀。

1.2.上下坡能否建圖，是否需要PS改圖

坡道可以在二維圖上留下平面佈局，但二維柵格不記錄坡面高度、坡度或車輛爬坡能力。 單層地圖也不能分別表達同一平面位置上下兩層都可通行的結構。

正常坡面如果被地形節點正確認作地面，高差應接近0，通常被當前min_intensity=0.1 m篩掉；牆、臺階等明顯高差點才進入二維障礙輪廓。坡面被篩掉，並不保證整條坡道已經標成可通行：仍取決於其他有效回波、掃描射線覆蓋，以及下游對空閒／未知區域的處理。

例如，正常坡點被錯誤估成15 cm高差，且通過其他視窗時，會越過10 cm門檻並可能畫成障礙；下坡點若在base_footprint系中低於-0.5 m，又會被min_height刪掉。這分別涉及地面估計和投影高度視窗，不能靠同一項引數處理。

調參的目標是讓真實地面、障礙和缺資料區得到合理表達；不能為了讓坡道變白而把真實臺階或危險落差一起過濾。實際可通行的坡度和落差需要測量並驗證，當前沒有實物坡道結論。

PS等圖片編輯可以用於經過現場確認後的靜態地圖清理，不是必須步驟，也不能修復即時地形識別。 即使把靜態圖上的誤障礙擦白，即時terrain_map／terrain_map_ext仍可能把同一坡面重新加入代價地圖。優先核對座標、單位、時間和上下坡篩選，再評估是否需要人工修圖。

1.3.當前建圖方式與配置來源

整套reality／simulation入口把各自config/reality/nav2_params.yaml、config/simulation/nav2_params.yaml傳給slam_launch，啟動sync_slam_toolbox_node；這6項在兩份配置中一致。預設通過已安裝的pb2025_nav_bringup包目錄尋找配置，指定params_file時以指定檔案為準；當前安裝副本與原始碼對應配置一致。

當前安裝版本為slam_toolbox 2.6.10、Ceres 2.0.0。以下讀取和生效條件按對應版本複核；配置裡的humble分支連結可供查說明，但分支內容可能繼續變化。

另有啟動配置疑點：兩份YAML的slam_toolbox段都沒有顯式use_sim_time。當前安裝的RewrittenYaml只替換已有鍵，而slam_launch沒有給此節點單獨傳入該引數；按當前預設接線，模擬入口傳入True也不會自動補到此節點，標準預設仍為False。這裡是靜態配置判斷，實際節點引數尚未觀察；ROS引數分割槽需繼續核對這一項。

當前use_scan_matching=False、do_loop_closing=False。 從空地圖進行常規建圖時，Karto仍接收掃描並形成柵格，但跳過掃描匹配，也不把新掃描加入位姿圖；因此不會通過自動閉環呼叫Ceres修正軌跡。這裡的圖主要依靠上游里程計擺放掃描，長距離、轉彎後的累計誤差不能指望這組求解器設定自動消除。當前處理路徑（Mapper.cpp，2.6.10）

求解器外掛仍會在節點配置時載入並讀取引數；“已讀入”與“參與了實際最佳化”是兩件事。 載入已有位姿圖或通過其他功能觸發最佳化，應再按對應路徑判斷。這些開關決定本組求解器設定是否有實際最佳化任務。

原檔案的Plugin、ROS、General、Correlation及Scan Matcher分割槽只是註釋分類；引數仍直接寫在ros__parameters下面，不增加一層演算法引數鍵。

1.4.引數

1.4.1.Plugin params（位姿圖求解器設定）

1.4.1.1.參數列

slam_toolbox:
  ros__parameters:
    solver_plugin: solver_plugins::CeresSolver
    ceres_linear_solver: SPARSE_NORMAL_CHOLESKY
    ceres_preconditioner: SCHUR_JACOBI
    ceres_trust_strategy: LEVENBERG_MARQUARDT
    ceres_dogleg_type: TRADITIONAL_DOGLEG
    ceres_loss_function: None

這6項都是型別名稱，沒有米、秒或角度單位，也不存在數值意義上的“調大／調小”。

列舉含義見官方求解器引數說明（2.6.10）；預條件器適用於迭代線性求解的條件見Ceres 2.0.0選項定義。

1.4.1.2.引數詳解

1.4.1.2.1.solver_plugin（選擇二維軌跡最佳化後端）

“位姿圖”可理解為：記錄各幀掃描時車輛的位置、朝向，以及幀與幀之間的相對關係。求解器在有這些約束且觸發最佳化時，調整二維x、y、yaw，使約束更一致。它不最佳化Point-LIO的三維IMU狀態，也不替代small_gicp的PCD定位。

當前Ceres外掛在啟動階段仍需成功載入。官方說明提到的其他外掛名稱，不代表當前安裝包已經提供；不能只改名字就假定能切換。

1.4.1.2.2.ceres_linear_solver（圖最佳化內部怎樣解方程）

SPARSE_NORMAL_CHOLESKY可理解為利用方程中大量零項，直接求出一輪修正量。其他暴露選項為SPARSE_SCHUR、ITERATIVE_SCHUR、CGNR，它們是演算法選擇，不是精度或速度檔位；不能籠統說換一個就一定更準、更快。

當前沒有求解耗時或記憶體異常的證據，而且常規建圖沒有自動圖最佳化，先沿用。它不改變地圖解析度、雷達取樣率、地形高差或爬坡能力。

1.4.1.2.3.ceres_preconditioner（迭代求解的加速輔助，當前不用）

預條件器是幫助某些“反覆逼近答案”的線性演算法更快求解的輔助方式。當前採用直接求解，因此SCHUR_JACOBI不會參與這一計算；日誌顯示已讀取，也不等於有實際加速。

若以後切換到迭代求解，需要一起核對預條件器支援範圍；SCHUR_JACOBI適用於ITERATIVE_SCHUR，不能當成所有求解器通用的開關。

1.4.1.2.4.ceres_trust_strategy（圖最佳化的修正策略）

LEVENBERG_MARQUARDT簡稱LM，可以理解為根據誤差改善情況調整每輪軌跡修正的力度。DOGLEG是另一種提出修正步驟的方法；使用它時，才進一步選擇TRADITIONAL_DOGLEG或SUBSPACE_DOGLEG。

1.4.1.2.5.ceres_dogleg_type（Dogleg內部方式，當前不生效）

當前是LM，因此改ceres_dogleg_type不會改變當前結果。這裡的“步長”指計算中的位姿修正量，不是車輛每次走多遠、底盤速度或坡度限制。

1.4.1.2.6.ceres_loss_function（異常圖約束的影響）

None表示不增加魯棒損失，按約束誤差的平方處理。HuberLoss、CauchyLoss用於減弱大誤差約束的影響；它們不能保證刪掉全部錯誤匹配，也會影響大誤差但真實有效的約束。

這項處理的是位姿圖約束，不能直接清除點雲裡的行人、修改坡面高差，或修復IMU外參／點時間。當前自動圖最佳化沒有觸發，切換損失函式也不能使坡道自動變成可通行區域。

讀取路徑為slam_toolbox的setSolver載入外掛，再由CeresSolver::Configure讀取5項ceres設定；該路徑沒有隨引數修改自動重建求解器的回撥。未識別的線性演算法／信賴域／損失名稱可能落回預設分支，不應把“能啟動”當成拼寫正確。引數讀取與配置分支（2.6.10）

• 原始碼能確認： 二維建圖與PCD儲存是兩條輸出鏈；常規導航同時使用二維靜態地圖和三維先驗點雲；當前關閉掃描匹配和閉環；預條件器與Dogleg型別在當前演算法組合下不參與求解。
• 已有驗證範圍： 僅獨立Point-LIO靜止與約20 cm低速往返；二維建圖、坡道投影、長距離軌跡、閉環及這組求解器的實際最佳化效果尚未驗證。
• 實物待核實： 二維圖與PCD座標一致性、真實坡道和臺階區分、車輛上下坡能力、雲臺轉動時的地圖穩定性，以及地圖檔案儲存／重新載入結果。

1.4.2.ROS Parameters（座標介面、建圖模式與續建入口）

1.4.2.1.參數列

前6項是當前啟用值；後3項在YAML中被註釋，示例內容不是實際引數值。另補列ROS標準use_sim_time，說明前面發現的模擬時鐘缺項。

slam_toolbox:
  ros__parameters:
    odom_frame: odom
    map_frame: map
    base_frame: base_footprint
    scan_topic: obstacle_scan
    use_map_saver: true
    mode: mapping

    # 可选：载入此前序列化的地图，从指定位置或第一帧位置继续建图
    # 两种起点同时设置时，优先采用map_start_pose
    # map_file_name: test_steve
    # map_start_pose: [0.0, 0.0, 0.0]
    # map_start_at_dock: true

1.4.2.2.引數詳解

1.4.2.2.1.odom_frame（底盤位姿從哪套座標中取得）

可以把odom理解為“上游里程計持續記錄的位置座標”。本節點在每幀掃描的時間戳查詢odom←base_footprint的TF，得到底盤在odom中的位置和朝向；該時刻查詢失敗就跳過這幀。

它沒有直接訂閱Point-LIO的aft_mapped_to_init，也不靠一個叫/odom的話題來取得位姿。當前鏈中，Point-LIO先經loam_interface適配，sensor_scan_generation再發布odom到base_footprint的TF；只有里程計訊息而缺這條有效變換，仍不足以建圖。

取位姿時只保留x、y和yaw。位置以m表達，yaw以rad表達；上坡時的高度、roll和pitch沒有進入二維建圖位姿。二維位姿讀取實現（2.6.10）

1.4.2.2.2.map_frame（二維地圖採用的參考系）

map是最終二維柵格和建圖位姿的參考系；odom是當前里程計參考系；base_footprint是隨底盤移動的參考點。正常關係可讀成：

map → odom → base_footprint
旧场地图    当前里程计    底盘参考点

實車必須讓二維圖片／YAML、PCD定位與Nav2所理解的map具有一致尺度、原點和方向。將map_frame換成另一個名字，不會把舊地圖或PCD旋轉、平移過去。

當前建圖launch提供單位變換的靜態map→odom，並配置transform_publish_period=0，slam_toolbox不釋出這條動態TF。這裡只說明座標來源；已有地圖導航由small_gicp提供定位關係，和建圖入口不同。續建舊圖若從不同位置或朝向重新啟動，舊map與新odom未必重合，需一起核對啟動鏈，不能只取消map_file_name的註釋。

1.4.2.2.3.base_frame（跟蹤底盤參考點，不是獨立轉動的雲臺）

當前base_frame是base_footprint；pointcloud_to_laserscan的target_frame也是base_footprint，所以obstacle_scan.header.frame_id=base_footprint。虛擬掃描中心與這裡跟蹤的底盤參考點一致，掃描到base的變換為單位變換。

雷達點在上游已通過TF轉換到底盤投影系；這裡不應再次手工疊加front_mid360的安裝偏移或雲臺yaw。若兩邊僅改frame_id而沒有真正變換座標，地圖仍會錯誤；引數名稱一致也不能證明上游IMU／雷達／底盤轉換正確。

MID360仍掛在gimbal_yaw，隨實際yaw轉動、不隨pitch轉動；兩條odom關節固定。建圖使用底盤系，不意味著可以將MCU世界姿態當機械關節角，也不改變這些機械約定。

base_footprint這個名稱不會自動得到水平投影。當前上游保留完整三維姿態，掃描按該系x-y平面投影，而slam_toolbox只取二維x、y、yaw；坡道上是否符合預期，需要核對實際投影平面，不能指望本節點補回已丟失的高度或自動把傾斜掃描拉平。掃描到base的安裝關係由LaserAssistant實現取得。

1.4.2.2.4.scan_topic（接收哪份二維掃描）

當前輸入為：

terrain_map_ext（地形点云）
    → pointcloud_to_laserscan（高度／高差筛选，转到底盘系）
    → obstacle_scan（LaserScan）
    → slam_toolbox

它不是原始三維點雲，也不直接讀取terrain_map_ext。上游已刪掉的坡點、低障礙或缺資料，不能靠更換這個話題名恢復。

當前值obstacle_scan是相對名稱：預設空名稱空間下為/obstacle_scan；若節點在/sentry名稱空間中，則為/sentry/obstacle_scan。前面的轉換器輸出也使用相對名稱，需在一致的名稱空間和重對映下接通。

教程及官方README將scan_topic說明為絕對名稱；當前2.6.10訂閱實現沒有強制這一形式，本工程使用相對名稱obstacle_scan，按ROS規則解析。話題名稱與訊息中的frame_id也是兩件事：名稱決定接收哪條訊息，frame_id決定掃描座標如何解釋。ROS2話題名稱規則

輸入使用SensorDataQoS，並經過TF訊息過濾；既要有LaserScan，也要能在掃描時間取得odom與掃描系／底盤系之間的關係。訂閱與TF過濾實現（2.6.10）

1.4.2.2.5.use_map_saver（提供二維地圖儲存入口）

True會建立slam_toolbox/save_map服務，並訂閱地圖；收到地圖後，服務呼叫才會觸發二維圖片及YAML的儲存。沒有地圖時會報告無法儲存，True本身不會定時落盤。MapSaver實現（2.6.10）

這項只控制slam_toolbox自己的儲存入口。Point-LIO儲存PCD由pcd_save控制；.posegraph與.data由serialize_map介面儲存。設為False也不等於關閉二維建圖或關閉所有儲存渠道。

當前slam_launch另外啟動Nav2的map_saver_server，它是獨立的地圖儲存服務，不能把兩者視為同一個節點。儲存出的二維圖應與對應PCD一起歸檔，供當前導航方案使用。

1.4.2.2.6.mode（內部模式提示與實際啟動流程的區別）

當前保持mapping，與正在啟動的sync_slam_toolbox_node建圖流程一致。本版本中，mode由Ceres外掛讀取；localization會啟用更便於移除約束的內部設定，僅改這個字串不會把當前程式自動變成定位節點。 Ceres模式讀取、同步建圖節點入口

本工程整套流程的選擇來自launch引數：

所以，做已有地圖導航時，這份YAML中的mode仍寫mapping，也不代表slam_toolbox在後臺繼續建圖，因為該節點根本沒有啟動。若以後要使用slam_toolbox自己的定位方案，需要相應定位節點、序列化地圖和匹配流程；那是另一套啟動需求。官方定位節點實現

1.4.2.2.7.map_file_name（續建所需的序列化地圖字首，當前未啟用）

三行示例前面都有#，當前沒有載入舊位姿圖，而是從空地圖開始本次建圖。map_file_name不是儲存目標名稱，也不是map_server的二維地圖YAML路徑，更不是small_gicp的prior_pcd_file。

若設定為/某个目录/site，讀取的是site.posegraph與site.data兩份檔案；字尾由程式補上，不能只給一張PGM或一份PCD。兩份檔案應來自同一套序列化結果。序列化讀寫實現

當前use_scan_matching=False，從空圖建圖時不建立新的匹配圖節點；有序列化檔案也不能直接當作已經驗證可續建／定位的地圖。續建還涉及舊圖結構、起點和新舊座標的關係，當前未驗證。

1.4.2.2.8.map_start_pose（在舊地圖中的二維起點，當前未啟用）

啟用舊圖載入時，陣列含義是[x, y, yaw]，位置用m，朝向用rad，均相對舊地圖；當前base_frame下表示底盤的二維初始位姿。例如yaw=1.5708約為90°，不是填寫90。

它是供舊圖匹配使用的近似起點，不是要求車輛移動到該點，也不是給新地圖定義原點。還需給非空map_file_name；沒有舊圖路徑時，單獨設定這項不會啟用續建。

1.4.2.2.9.map_start_at_dock（從舊地圖第一幀繼續，當前未啟用）

這裡的dock指舊圖第一幀所在位置，不是檢測物理充電樁，更不是自動回樁。True表示新一輪從舊圖的這個起點對接；車輛實際起點和方向仍需對應。

啟用續建時選擇map_start_pose，或選擇map_start_at_dock=True。兩者都給時pose優先；pose數量不是3時，原始碼報錯後採用[0,0,0]，並不會因此自動改用dock。只將dock設為False也不等於關閉舊圖載入，關閉載入的入口是空map_file_name。起點選擇與載入邏輯（2.6.10）

1.4.2.2.10.use_sim_time（ROS標準時鍾引數：模擬段存在缺項）

當前實物和模擬YAML的slam_toolbox段均沒有這一項。slam_launch將use_sim_time交給RewrittenYaml替換，但當前安裝的實現只改已有鍵，沒有為此段補鍵；Node也沒有單獨傳入use_sim_time覆蓋值。

因此，按當前預設啟動接線可推斷該節點採用標準預設False，模擬入口的True沒有作用到這一段。模擬應與上游點雲、機械TF共同使用/clock；實物通常使用False並核對驅動時間。這是根據配置與啟動邏輯得出的判斷，實際執行節點值及模擬效果仍待驗證。

時鐘選擇由use_sim_time控制，改odom_frame、scan_topic或mode不能替代這一設定。

1.4.2.2.11.讀取時機與驗證範圍

座標、話題、儲存開關和Ceres模式在節點配置時讀取；3項舊圖引數在啟動載入入口讀取。相關路徑沒有在引數改動後自動重建訂閱、求解器或重新載入舊圖的回撥；ROS標準use_sim_time由節點時鐘機制處理，應與這些啟動引數分開理解。

• 原始碼能確認： 當前6項實物／模擬配置相同；底盤位姿來自掃描時刻TF，保留二維x、y、yaw；輸入是底盤系虛擬掃描；儲存服務與檔案落盤不同；續建3項未設定；mode不單獨切換整個工作鏈；模擬段缺少顯式use_sim_time。
• 已有驗證範圍： 獨立Point-LIO靜止與約20 cm往返；未驗證本節點建圖、續建、地圖儲存、上下坡投影或實際模擬時鐘值。
• 實物待核實： IMU／雷達／底盤轉換與關節機械角、掃描中心和投影平面、TF與掃描時間一致性、二維圖與PCD共同基準；若需續建，還須核對舊圖第一幀及新一輪起點。

1.4.2.3.參數列（掃描處理、地圖輸出與執行資源）

這13項繼續屬於原檔案ROS Parameters分割槽。掃描接收、加入建圖和輸出地圖是不同階段；以下按當前sync_slam_toolbox_node及2.6.10版本說明，實物與模擬配置值相同。

    debug_logging: false
    throttle_scans: 1
    scan_queue_size: 10
    transform_publish_period: 0.0   # 0：本节点不发布map→odom TF
    map_update_interval: 0.5
    resolution: 0.05
    min_laser_range: 0.3
    max_laser_range: 10.0
    minimum_time_interval: 0.2
    transform_timeout: 0.2
    tf_buffer_duration: 10.
    stack_size_to_use: 40000000     # B，申请的栈软限制目标
    enable_interactive_mode: true

1.4.2.4.引數詳解（掃描處理、地圖輸出與執行資源）

1.4.2.4.1.debug_logging（除錯資訊，不改變建圖精度）

當前False適合普通觀察；True可讓實際Ceres求解輸出完整報告，但當前自動圖最佳化沒有觸發，這一報告不會憑開關自動出現。

原始碼除錯等級設定使用字面日誌器名稱logger_name，而不是節點的slam_toolbox日誌器；因此單開True不能保證節點的DEBUG日誌顯示，還要核對launch傳入的日誌等級。它不是“開啟更準確演算法”的開關。

1.4.2.4.2.throttle_scans（按份數抽取掃描）

N=1不增加抽幀；N=2約每兩份保留一份，N=3約每三份保留一份。計數針對進入篩選函式的掃描，不是MID360的每個點或全部原始訊息，首幀會直接放行。

例如候選輸入20 Hz、N=2，抽幀後的候選上限約10 Hz；後面的0.2 s時間門檻仍可能將其壓到不超過約5 Hz，位移不足時還會更低。改N不會控制雷達釋出頻率，也不保證實際入圖頻率等於輸入頻率除以N。

1.4.2.4.3.scan_queue_size（等待TF的佇列，不能包辦處理積壓）

等待對應時刻TF的掃描，最多先快取10份；超出時丟最舊訊息。增大可緩解短暫TF遲到，但不會增加計算能力，也不會生成缺失變換。Humble MessageFilter實現

TF就緒後，當前同步節點還將通過篩選的掃描放進獨立FIFO佇列q_，再逐份處理。這個佇列沒有受scan_queue_size限制；原始碼在剩餘長度超過10時報警，但沒有在此處自動丟棄積壓。調大前面的等待佇列不能解決後面的處理過慢。同步處理實現（2.6.10）

1.4.2.4.4.transform_publish_period（本節點的map→odom釋出週期）

原配置註釋“if 0 never publishes odometry”容易讓人誤認為關閉了里程計。實際控制的是slam_toolbox自己的map→odom TF；當前0使這一執行緒直接返回，Point-LIO、lidar_odometry和sensor_scan_generation的odom→base_footprint不由它關閉。

正數時，0.05 s約20 Hz、0.1 s約10 Hz；改變釋出節奏不意味著同樣頻率產生新的定位結果。當前建圖launch已有靜態單位map→odom，啟用動態釋出前必須明確這條關係由誰提供，避免兩個來源同時釋出。

1.4.2.4.5.map_update_interval（二維圖更新間隔，與入圖間隔分開）

當前0.5 s對應名義約2 Hz地圖更新迴圈；minimum_time_interval=0.2 s控制的是掃描篩選，兩個頻率可以不同。同一份已處理掃描集合也可以被多次重新畫成地圖，更新更快不等於獲得更多環境資訊。

當前實現從全部已處理掃描重新生成柵格，並非只改最新一小塊；地圖擴大、歷史掃描增多時，縮短間隔可能明顯增加負擔。無/map訂閱者會跳過生成；當前地圖儲存元件會自行訂閱，所以即使不看RViz也可能持續生成地圖。柵格生成入口

該迴圈使用rclcpp::Rate的主機時鐘，不繫結節點的/clock。地圖訊息戳取最近進入掃描回撥的header.stamp，也不保證那份掃描已完成入圖；應將“訊息變新”“柵格變了”“新掃描已處理”分開判斷。間隔應為正數，0沒有像TF釋出週期那樣的停發分支。Humble Rate時鐘實現

1.4.2.4.6.resolution（二維圖每格多大）

0.05 m表示一格5 cm。調大如0.1 m，格子變粗，小杆、牆邊或窄口可能合併；調小如0.025 m，格子更細，但不會補出雷達沒采到或上游已刪掉的點。

同樣覆蓋面積，格子邊長減半，格數約變為4倍。更細也不等於更準：定位漂移、地形誤分類、投影方向或點時間錯誤仍會把錯誤輪廓畫進更細的格子。

這項不是“允許5 cm臺階”，也不決定坡面能否識別為地面；它隻影響二維表達的顆粒度。生成地圖後，儲存的YAML應記錄對應解析度；僅改舊YAML數字會改變舊圖的實際米制尺寸，不能作為重新建圖的替代。

1.4.2.4.7.min_laser_range（建圖採用的最近距離）

當前下界0.3 m，與轉換器的range_min一致；距離以虛擬掃描中心base_footprint計算。減小這項不能恢復轉換器已刪除的0.3 m內點，原始碼也會將低於訊息range_min的設定提高到訊息下界。

增大可能排掉近處自車回波，但也會排掉車旁障礙；圓形距離門檻不能完整代替車體與雲臺活動包絡的自車過濾。當前Karto柵格射線處理中，讀數小於或等於實際下界會被跳過。

1.4.2.4.8.max_laser_range（遠處回波怎樣參與建圖）

當前閾值10 m，轉換器輸出訊息的range_max也為10 m；超過訊息上限的引數會被壓回該上限。增大這項無法使當前輸入獲得更遠回波，縮短則會減少可用於畫遠處障礙的結構。範圍初始化實現

這項不是簡單的“超過閾值全部刪掉”：若閾值設為5 m，輸入有一份合法8 m回波且訊息上限仍為10 m，柵格射線會截到5 m，不把8 m端點畫成障礙；這段射線仍可參與空閒區域的統計。

當前∞無回波被跳過，不會自動把整條方向畫成10 m空地。 大於／等於訊息上限的值和NaN同樣被跳過。因此，上游將正常坡面濾掉後，若該方向沒有其他有效回波提供射線覆蓋，坡面區域不保證成為空閒；調解析度或輸出頻率不能補出這份觀測。佔用／空閒還依賴柵格累計統計。Karto射線柵格實現

1.4.2.4.9.minimum_time_interval（掃描最短放行間隔，還有位移門檻）

當前按掃描時間戳與上一份放行掃描比較，差值不足0.2 s則拒絕；首幀例外。它只給出通常約5 Hz的時間上限，不是固定5 Hz處理定時器。增大可降低入圖資料量，減小隻放寬時間條件。

當前還有一個與實車原地轉動有關的前置限制： 公共篩選函式檢查水平位移平方，未用yaw放行。當前minimum_travel_distance=0.1 m，對應這裡約8.94 cm的水平位移門檻，另有初期計數限制。以0.2 m/s移動0.2 s只有4 cm，仍可能不夠，所以低速入圖間隔會更長。

首幀之後，原地轉底盤或獨立轉雲臺時，若底盤TF的x、y基本不變，後續掃描會被這層攔住；單獨縮短minimum_time_interval並不能解決。後續Karto層的minimum_travel_heading還沒機會介入，不能憑設定了轉角閾值就認定原地旋轉會正常更新地圖。這裡是原始碼條件，尚無本節點轉彎或獨立yaw驗證。

1.4.2.4.10.transform_timeout（等待TF；條件下還影響釋出戳）

當前0.2 s傳給TF訊息過濾器，表示等對應掃描時刻的變換最多等多久。調大可容忍更晚到達的有效TF，也可能讓舊訊息等待更久；TF根本不存在或時間域不一致時，等待更久沒有修復作用。

若transform_publish_period為正，原始碼還把釋出的map→odom戳設為“最近掃描戳+transform_timeout”；這不是預測未來運動。當前period=0，這個用途沒有參與輸出。它也不會給輸入點雲補運動補償、把逐點時間單位改正確，或將TF歷史快取變長。

1.4.2.4.11.tf_buffer_duration（動態TF歷史保留多久）

10 s表示動態TF歷史快取跨度，用來查詢掃描當時的變換；不是等10 s再處理，也不是允許掃描與TF隨意差10 s。靜態機械變換的儲存規則與動態歷史不同。

積壓掃描太舊時，適當增大可能保留所需歷史，但不會消除處理積壓；減小則可能更快丟掉舊變換。前面已確認模擬段use_sim_time缺項，增大歷史快取不能替代統一時間來源。

1.4.2.4.12.stack_size_to_use（棧限制，通常沿用）

40000000 B為約40 MB，約38.15 MiB。節點入口在現有棧軟限制低於目標時申請提高，原始碼不主動將較高的現有限制降下來；設定更小不一定讓實際棧變小。同步節點入口

它主要為大地圖序列化等呼叫準備，不限制Point-LIO累計PCD、掃描快取或程式總記憶體。系統是否接受申請仍受資源限制影響，入口沒有檢查setrlimit返回值；當前沒有相關失敗證據，通常保持。

1.4.2.4.13.enable_interactive_mode（允許人工改位姿圖，不等於已開啟互動）

當前True允許切換到RViz位姿圖互動功能；啟動時實際interactive_mode=False，不會自動進入拖動模式。進入後處理執行緒暫停，便於修改已有圖節點；退出後恢復，期間新候選掃描仍可能進入同步佇列，移動中使用會造成積壓。互動切換實現

當前use_scan_matching=False，從空圖建圖時沒有新圖節點可供拖動；但enable=True仍會額外儲存已成功處理的原始掃描，即使實際互動未開啟。沒有人工調整需求時，關閉可減少附加快取，Karto自身的歷史掃描仍存在，不能當作總記憶體開關。

人工拖動／最佳化的是二維位姿圖，不會同步修正Point-LIO儲存的三維PCD。當前二維圖與PCD需保持共同基準，也不能用互動修圖替代坡面分類或座標介面核對。

1.4.2.4.14.讀取時機與驗證範圍

debug、抽幀、TF佇列、時間門檻、柵格解析度、TF快取及互動許可在節點配置／執行緒初始化時讀取；兩項雷達範圍在首次建立該掃描系的後設資料時讀取，隨後快取。stack_size_to_use由可執行程式入口讀取。相關路徑沒有引數變化後自動重建這些成員／佇列／雷射後設資料的回撥；互動切換使用專門服務，不能混同為直接改引數即可生效。

• 原始碼能確認： 本組13項實物／模擬配置一致；掃描篩選與地圖輸出分離；等待TF佇列與同步處理佇列不同；當前不釋出動態map→odom；二維範圍受上游量程約束；∞不自動產生空閒射線；位移前置門檻可能攔住原地轉動後的掃描。
• 已有驗證範圍： 獨立Point-LIO靜止與約20 cm低速往返；未驗證slam_toolbox實際處理頻率、原地轉動、坡道空閒覆蓋、長時間佇列／記憶體、序列化或互動功能。
• 實物待核實： 移動與轉動下的入圖連續性、雷達／底盤時間與座標、近場自車回波和低障礙、坡道是否被有效射線覆蓋、地圖細節與計算負載。重力方向、內部IMU轉換及點時間問題仍需先核對。

1.4.3.General Parameters（掃描入圖、匹配與閉環）

當前use_scan_matching=False、do_loop_closing=False。 slam_toolbox仍按上游里程計擺放掃描並生成二維圖；距離與轉角門檻仍會檢查，但普通掃描匹配、圖節點關聯和自動閉環分支被跳過。表中“當前執行不使用”表示引數已讀入、作用分支沒有執行，不是引數拼寫無效。

這些引數決定二維掃描如何取樣和關聯，不判斷車輛能否爬坡。坡面／障礙區分仍由前面的地形分析與二維掃描篩選處理。

1.4.3.1.參數列

    # General Parameters
    use_scan_matching: false
    use_scan_barycenter: true
    minimum_travel_distance: 0.1
    minimum_travel_heading: 0.1
    scan_buffer_size: 10
    scan_buffer_maximum_scan_distance: 10.0
    link_match_minimum_response_fine: 0.1
    link_scan_maximum_distance: 1.5
    loop_search_maximum_distance: 3.0
    do_loop_closing: false
    loop_match_minimum_chain_size: 10
    loop_match_maximum_variance_coarse: 3.0
    loop_match_minimum_response_coarse: 0.35
    loop_match_minimum_response_fine: 0.45

1.4.3.2.引數詳解

1.4.3.2.1.use_scan_matching（是否用二維輪廓修正里程計）

開啟後的掃描匹配，可以理解為：先按里程計估計把當前二維輪廓擺上去，再與近期已有輪廓比較，尋找更吻合的位置和朝向。它可能減輕二維里程計誤差，也可能在重複走廊、稀疏輪廓或誤分類地形上找到錯誤對應。

當前False沿用上游Point-LIO提供的運動估計，仍會生成二維柵格；原始碼只是跳過本節點的二維匹配、圖節點和關聯構建。不能據此說整車SLAM已關閉，也不能據此證明長距離二維圖不會漂移。普通處理路徑（2.6.10）

是否需要開啟，應結合長距離、轉彎和重訪結果判斷。二維修正不會回寫Point-LIO儲存的PCD；當前建圖又採用靜態單位map→odom，啟用前需一併核對二維圖、PCD和定位參考系的關係。該開關不能修復重力、外參、點時間或坡面誤分類。

1.4.3.2.2.use_scan_barycenter（用回波重心衡量掃描間距，當前不用）

True時，將一份掃描中有效回波端點的平均位置作為圖距離判斷參考；False用掃描中心。這裡的重心屬於“看到的環境點”，不是底盤質量重心，也不把導航參考系改成雲臺系。

遮擋、可見牆面或動態物體變化會讓點重心改變，即使車輛位移很小。它會影響圖關聯中的近鄰範圍，但當前use_scan_matching=False不走這一路；入圖位移仍按底盤／虛擬掃描中心判斷。參考點定義

1.4.3.2.3.minimum_travel_distance（走多遠才採用下一份掃描）

當前有兩道篩選，不能只按“每10 cm一幀”理解：

1. 外層slam_toolbox入口：通常要求掃描戳間隔至少0.2 s，還要求相對上一次放行掃描的水平位移至少約8.94 cm，並受抽幀和初期計數限制。
2. 內層Karto：相對上一次實際採用的掃描，約10 cm位移或約5.73°朝向變化可放行；這裡使用的是掃描中心，當前與base_footprint重合。

外層先執行，且它在放行後就更新自己的比較基準；內層若拒絕該掃描，兩層基準會暫時不同。因此實際步距還取決於速度、轉角、輸入週期和處理結果。調大通常減少入圖份數，調小通常增加；過稀可能讓轉彎附近的小結構覆蓋不足。

還有一個版本相關的區別：ROS引數minimum_time_interval=0.2用於外層；當前SMapper::configure沒有將它下發給Karto。新建地圖的Karto內部時間兜底仍是預設3600 s，普通短時間建圖主要依靠內層距離／轉角條件。0.2 s引數仍有效，但不能當作兩層都採用同一時間門檻。ROS到Karto引數接線

1.4.3.2.4.minimum_travel_heading（入圖角度間隔，0.1約為5.73°）

單位是rad，0.1不是0.1°。內層比較兩份掃描的平面yaw變化；調小可讓更小轉角滿足內層條件，調大要求轉得更多，但它不能繞過前面的時間／位移篩選。

首幀之後，原地轉底盤或獨立轉雲臺時，若底盤TF的x、y不變，掃描會先被外層攔住；即使內層轉角本可達標，也沒有機會參與判斷。只調小minimum_travel_heading，不能解決當前入口對原地轉動掃描的限制。 這裡涉及建圖取樣，不控制底盤wz、雲臺機械yaw，也不是可爬坡角。

1.4.3.2.5.scan_buffer_size（近期匹配保留幾份掃描，當前不用）

啟用匹配後，最多保留10份近期掃描供連續匹配；它和之前scan_queue_size=10含義不同，後者快取等待TF的訊息。這裡也不是“整張地圖只保留10份歷史”，移出近期視窗的掃描仍可留在建圖歷史中。

如果直線入圖步距恰好0.1 m，10份掃描的首尾約隔9段，即0.9 m；實際步距未必為0.1 m，所以不能由份數直接推成固定米數或秒數。當前不向該匹配視窗加入掃描，先不調整。

1.4.3.2.6.scan_buffer_maximum_scan_distance（近期視窗首尾間距，當前不用）

啟用後，近期視窗同時受10份和首尾中心10 m間距限制，超限就從最舊掃描開始移出，直到兩個條件都滿足。距離是首尾直線距離，繞圈回到附近不等於累計路程小；它也不是雷達回波的10 m量程。

份數上限先達到時，增大10 m通常不會讓視窗更大；兩項需結合真實入圖步距理解。當前匹配關閉，視窗刪除邏輯不參與結果。

1.4.3.2.7.link_match_minimum_response_fine（普通近鄰鏈關聯門檻，當前不用）

啟用圖關聯後，當前掃描與附近歷史掃描鏈匹配，分數夠高才接納這類關聯。0.1是輪廓匹配的歸一化分數，不是“有10%機率匹配正確”。

它不是每幀連續匹配的統一質量檢查。 當前版本的逐幀MatchScan返回分數沒有在Mapper::Process中用這項拒幀；近鄰鏈門檻也不能兜住所有錯誤逐幀匹配。當前整條匹配分支關閉，改這項不會改善現有地圖。

1.4.3.2.8.link_scan_maximum_distance（普通關聯的近鄰範圍，當前不用）

啟用後，只考慮參考點相距約1.5 m內的近鄰鏈關聯；當前barycenter=True時，參考點是環境回波重心。調大可納入更遠候選，也可能增加重複結構歧義；調小則更侷限。

它不是“雷射只看1.5 m”，也不是車輛碰撞距離或允許地圖誤差；改變這項不改變上游掃描量程。

1.4.3.2.9.loop_search_maximum_distance（從哪些舊區域尋找閉環，當前不用）

開啟閉環後，3 m用於篩選當前掃描與舊掃描參考點的距離，不代表在3 m內就認定回到舊位置。候選還需經過鏈和匹配條件；後續匹配搜尋網格的範圍是另一組引數。

例如車物理上回到舊位置，但估計參考點已經漂得較遠，舊區域可能落在3 m候選範圍外；增大能放寬尋找範圍，卻也會增加把相似走廊誤當同一區域的機會。當前不執行自動閉環，先不調整。

1.4.3.2.10.do_loop_closing（自動重訪識別與閉環修正）

閉環可理解為：繞行後認出曾經經過的區域，用這條新約束重新調整二維軌跡，使前後輪廓更一致。成功閉環可能減少累積偏差；錯誤閉環也可能把整片地圖拉錯。

當前False，且它位於use_scan_matching=True的分支內，因此只將do_loop_closing改成True仍不會執行。反過來，開啟掃描匹配但保持閉環False，仍可執行近期匹配和普通近鄰鏈關聯，只是不自動走閉環這一路。

這是slam_toolbox自己的二維功能，不是small_gicp即時點雲對先驗PCD的定位。即使成功閉環，也不自動修正獨立儲存的三維PCD或解決當前靜態map→odom的座標銜接。

1.4.3.2.11.loop_match_minimum_chain_size（歷史鏈至少幾份，當前不用）

10表示候選歷史鏈的掃描份數，不是10個雷達點或10 m。增加要求更多歷史結構，減少則允許較短的鏈參與。

原始碼中的普通近鄰鏈LinkNearChains也使用它，所以即使以後do_loop_closing仍False，只要開啟掃描匹配，它也可能影響普通圖關聯。當前這兩條關聯路徑都未啟用，先不調整。

1.4.3.2.12.loop_match_maximum_variance_coarse（配置先平方，內部門檻9 m²）

粗匹配可能有多個近似答案。當前2.6.10讀取YAML值3.0後，setParamLoopMatchMaximumVarianceCoarse會先平方，內部方差門檻實際為3.0²=9.0 m²。按賦值鏈與方差比較的量綱，配置值按位置分散尺度（m）理解，內部才使用m²。

這裡檢查演算法給出的x、y方差各自是否小於9.0 m²；正值範圍內，配置值越小要求結果越集中。不檢查yaw方差，也不保證實際位置誤差小於3 m。平方賦值與方差判定原始碼

它與small_gicp的max_dist_sq含義及配置尺度都不同：本項配置3.0先平方為9.0 m²的匹配結果方差門檻；max_dist_sq的配置值本身就是m²，3.0直接表示3.0 m²的點對應距離平方門檻。不能因為兩項都寫3.0就認為容差相同。當前閉環關閉，無須調整工程數值。

1.4.3.2.13.loop_match_minimum_response_coarse（粗匹配進入精匹配的門檻，當前不用）

分數須大於0.35，且x、y方差同時通過上一項，才進入精匹配。增大更嚴格，減小放寬；分數來自輪廓與相關性網格的吻合程度，範圍歸一化到0～1，不是經過標定的定位置信度。

1.4.3.2.14.loop_match_minimum_response_fine（最終閉環匹配門檻，當前不用）

粗匹配通過後再精匹配，最終分數達到0.45才進入閉環連線與軌跡最佳化。更嚴格可能漏掉有效重訪，更寬鬆可能錯連重複結構；不能把達到0.45理解為位置已準確。

這3項質量門檻共同作用，不能只看粗分數、細分數或方差中的一個；粗、細兩級使用的匹配過程也不同，兩個分數不宜直接當成同一置信度尺度比較。候選關聯與閉環判定原始碼

1.4.3.2.15.讀取時機與驗證範圍

引數名按當前原始碼區分大小寫：do_loop_closing與小寫loop_是實際鍵名；教程中的do_loop_close、Loop_等寫法不能直接作為本工程引數名。

本組14項經SMapper::configure讀取並設定到Karto。常規從空圖建圖時，use_scan_matching=False使近期匹配、圖關聯及閉環分支跳過；距離和轉角仍用於入圖判斷。相關路徑沒有動態引數更新回撥，外層還靜態快取距離門檻，不能把執行中設定新值等同於這些條件已全部更新。

• 原始碼能確認： 當前配置關閉二維匹配和自動閉環；距離／轉角仍有作用但受兩層門檻約束；重心只改變圖距離參考；近期視窗不限制全部歷史；鏈長度還用於普通關聯；閉環配置尺度按m理解，先平方為m²方差門檻，當前3.0對應9.0 m²；響應值不是成功機率。
• 已有驗證範圍： 僅獨立Point-LIO靜止與約20 cm低速往返；未驗證slam_toolbox長距離、轉彎、獨立雲臺yaw、閉環或與PCD的一致性。
• 實物待核實： 移動／原地轉動時實際入圖連續性，長距離及重訪誤差，坡道有效二維輪廓、遮擋與動態干擾；若需啟用匹配／閉環，還須核對二維修正與PCD及定位參考系的銜接。

1.4.4.Correlation Parameters - Correlation Parameters（普通掃描匹配搜尋）

這3項可理解為：附近多大範圍找位置、隔多遠試一次、輪廓稍微錯開時還給多少匹配分值。 它們控制內部匹配網格，與輸出地圖resolution=0.05 m不是同一個網格。

當前use_scan_matching=False，不執行普通匹配評分；但首次合法掃描進入Karto時仍會建立匹配器，使用這3項分配網格並檢查數值組合。因此，當前先保持，不能把“未執行匹配”理解為任意設定都無影響。

1.4.4.1.參數列

    # Correlation Parameters - Correlation Parameters
    correlation_search_space_dimension: 0.5
    correlation_search_space_resolution: 0.01
    correlation_search_space_smear_deviation: 0.1

1.4.4.2.引數詳解

1.4.4.2.1.correlation_search_space_dimension（在估計位置附近找多大範圍）

以當前內部二維掃描位姿為中心，在建圖平面的x、y方向嘗試平移。0.5 m表示粗搜尋正方形邊長，所以兩個方向各約±0.25 m；搜尋中心來自里程計／已有掃描修正，不是地圖原點、雲臺機械零位或Nav2目標點。

例如，真實輪廓在估計位置旁偏了約15 cm，範圍上可能包含正確平移；若一次匹配需要修正約40 cm，預設粗搜尋可能夠不到。包含候選只表示“會嘗試”，不保證能在重複走廊或稀疏點中選對位置。

這是粗平移範圍，不直接擴大yaw搜尋角，也不要求車輛移動到候選位置。精匹配還會在粗結果附近再搜尋；當前區域性精搜尋平移約±1 cm，因此±0.25 m也不是最終修正量的絕對邊界。

1.4.4.2.2.correlation_search_space_resolution（1 cm格距，粗搜尋實際按2 cm試）

當前內部相關網格每格1 cm。原始碼粗平移每次跨兩格，即2 cm；正常精化階段回到1 cm步距，在粗結果周圍小範圍細找。搜尋範圍與粗細步距實現

因此，不能把0.01簡單理解為“整個0.5 m範圍每隔1 cm全部精搜”，也不能把它當成1 cm定位精度。輸出二維圖片仍採用resolution=0.05 m，Point-LIO與地形降取樣尺度也不會因此改變。

在有效組合內，固定搜尋範圍而步距減半，平移候選數量約增為4倍；總工作量還受角度樣本和掃描點數影響。相關網格還要覆蓋有效回波範圍，並非只分配0.5×0.5 m的小方格，減小格距也會增加初始化記憶體。

1.4.4.2.3.correlation_search_space_smear_deviation（給輪廓周圍漸弱的匹配分值）

可以理解為給內部匹配輪廓加一圈“漸弱的軟邊”：當前點沒有恰好落在舊輪廓上，稍微錯開仍能得到分值，離得更遠則分值更低。這裡用的是高斯標準差，σ=0.1 m不是“10 cm內全部算匹配、外面全部不算”。

較寬的軟邊可能幫助稀疏／有小噪聲的輪廓匹配，也會削弱相近位置的區別。它隻影響內部匹配評分，不會把儲存的二維牆體直接加厚，不是Nav2障礙膨脹，也不清除動態物體或改變坡面高差。

當前關閉匹配，不靠調它改善現有地圖。以後若啟用，仍應先核對IMU／雷達／底盤轉換、點時間和有效二維輪廓；擴大搜索或塗抹不能代替這些介面核對。

1.4.4.2.4.三項必須配套：當前數值合法，塗抹已到上限

當前版本有這些組合約束：

• 維度與格距需為正；原始碼按dimension/resolution建立離散網格，斷言要求這個比值為整數，並使相關網格邊長為奇數。按該建立邏輯，應選正偶整數比值；當前0.5/0.01=50滿足條件。
• 塗抹標準差須在0.5×格距～10×格距內，邊界可取。當前格距0.01 m，對應σ為0.005～0.1 m，當前0.1 m處於上限。
• 例如，只把格距改為0.005 m而保持σ=0.1 m，此時允許上限變為0.05 m，初始化會拋異常；即使掃描匹配關閉，也不能規避這項檢查。

範圍或格距組合的檢查使用assert，實際是否觸發還取決於編譯時是否啟用斷言；不能僅憑程式能啟動就判斷組合合理。塗抹範圍檢查則明確拋runtime_error，錯誤文案可能寫“too small”，即使實際是超過上限。塗抹核及取值限制

1.4.4.2.5.讀取與使用路徑

3項在SMapper::configure中讀取並設定到Karto，非正值分別回退到0.5、0.01、0.1；網格在首次有效掃描初始化時建立，沒有動態修改引數後自動重建的回撥。引數讀取實現

啟用掃描匹配後，這套普通匹配器用於近期匹配和普通近鄰關聯；若以後啟用閉環，閉環的精匹配也會複用它。後面的loop_search_space_*控制另一個閉環匹配器，分割槽不同不代表兩條流程完全獨立。

• 原始碼能確認： 搜尋邊長、粗細步距、塗抹標準差與組合限制；當前不執行評分，但仍初始化匹配網格。不能把這3項全當作未讀取引數。
• 已有驗證範圍： 獨立Point-LIO靜止與約20 cm低速往返；未驗證本節點的搜尋成功率、資源佔用、轉彎、獨立yaw或坡道輪廓匹配。
• 實物待核實： 若需啟用匹配，檢查真實位姿修正量是否在可用範圍、二維結構是否足夠區分、噪聲／遮擋與計算負擔，並核對二維修正與PCD定位基準的一致性。

1.4.5.Correlation Parameters - Loop Closure Parameters（閉環粗匹配搜尋）

這3項用於繞行後重訪舊區域時的大範圍粗搜尋：找多大的位置範圍、用多細的網格、舊輪廓周圍給多寬的匹配分值。後續精匹配還會複用上一組普通匹配器，兩個分割槽並非完全獨立。

當前實物／模擬配置均為use_scan_matching=False、do_loop_closing=False，不執行自動閉環評分；首次有效掃描初始化時仍建立閉環匹配網格，數值組合和記憶體分配仍有影響。

1.4.5.1.參數列

    # Correlation Parameters - Loop Closure Parameters
    loop_search_space_dimension: 8.0
    loop_search_space_resolution: 0.05
    loop_search_space_smear_deviation: 0.03

1.4.5.2.引數詳解

1.4.5.2.1.loop_search_space_dimension（8 m搜尋邊長，與3 m候選距離不同）

閉環先挑選可能重訪的歷史掃描，再移動當前掃描去對比舊輪廓。這兩步分別受不同引數控制：

• loop_search_maximum_distance=3.0 m： 篩選附近的歷史候選，比較掃描參考點之間的距離。當前use_scan_barycenter=True，參考點採用有效回波端點的重心。
• loop_search_space_dimension=8.0 m： 對已經選出的候選，以當前估計掃描位姿為中心，在建圖平面的x、y方向各約±4 m內嘗試平移；這是8×8 m的粗搜尋正方形。

因此，擴大8 m搜尋區不能找回已被3 m候選距離排除的舊區域。8 m也不是車輛必須走夠的距離、雷達量程或繞地圖全域性找位置的範圍；角度搜索另有引數。候選篩選與平移搜尋實現

實物若經過重複走廊、相似柱子或大量動態障礙，範圍更大可能找到更多“看起來像”的位置。應先確認真實二維輪廓是否穩定、位置偏差是否超出搜尋範圍，再考慮調整；這些引數不負責識別坡度或判定坡道可通行。

1.4.5.2.2.loop_search_space_resolution（5 cm網格，閉環先按10 cm粗搜）

此版本粗搜尋跨兩格，所以當前實際平移步距為2×0.05=0.1 m。閉環流程為：

1. 用本組8 m範圍、5 cm網格完成粗匹配；這一階段沒有在本組網格上再做5 cm步距的內部精化。
2. 粗分數和x、y方差通過前面門檻後，以粗結果為中心，呼叫普通匹配器：使用上一組0.5 m範圍、1 cm網格，先按2 cm粗搜，再在所得結果附近約±1 cm內按1 cm細搜。
3. 精分數通過門檻後，才連線閉環並最佳化二維軌跡。

因此，0.05既不代表最終定位誤差為5 cm，也不控制後續全部精搜尋。輸出二維地圖resolution同為0.05 m，只是數值相同，屬於另一引數。閉環粗細匹配呼叫

固定邊長而將格距減半，平移候選數約增為4倍，網格記憶體也增加；總計算量還受角度和點數影響。最終精匹配還可能越過原粗搜尋邊緣，±4 m不是最終修正量的絕對上限。

1.4.5.2.3.loop_search_space_smear_deviation（給舊輪廓加一圈漸弱的評分）

與普通匹配的塗抹含義相同，但這裡只用於閉環粗匹配。σ=0.03 m可理解為評分輪廓較窄，稍微錯開仍有分值；不能理解成“3 cm內全部正確、外面全部錯誤”。

塗抹會按網格離散：當前組合生成3×3格的核，從中心向x、y各擴一格，即5 cm，鄰格分值逐漸降低。因此，標準差3 cm與核的實際覆蓋邊界不是同一量；它也不直接加厚儲存的牆體或改變導航障礙膨脹。

當前5 cm格距允許σ為0.025～0.5 m，0.03 m合法。例如，單獨把格距增到0.1 m，仍保留σ=0.03 m，新下限會變成0.05 m，初始化將拋異常；即使閉環關閉也會檢查。容許範圍與塗抹核實現

1.4.5.2.4.組合限制、讀取與實物核對

範圍與格距還需符合前一組的離散網格約束：按建立邏輯選正偶整數比值；當前8/0.05=160滿足條件。相關assert是否執行取決於編譯設定，塗抹越界則明確拋異常。

3項在SMapper::configure讀取並設定到Karto，非正值分別回退為8.0、0.05、0.03；首次有效掃描建立網格，沒有執行中修改這3項後自動重建的引數回撥。引數讀取實現

• 原始碼能確認： 當前不執行閉環評分，但仍初始化網格；8 m與3 m分別控制粗搜尋和歷史候選；粗搜尋步距10 cm，閉環精匹配複用普通匹配器。
• 已有驗證範圍： 僅獨立Point-LIO靜止和約20 cm低速往返，未觀察本節點閉環、長距離重訪或計算負擔。
• 實物待核實： 若以後需要閉環，檢查重訪偏差、重複結構與動態遮擋、坡道／雲臺轉動時二維輪廓變化，以及計算負擔。二維閉環不會自動修正獨立儲存的PCD，仍須核對兩者定位基準的一致性。

1.4.6.Scan Matcher Parameters（掃描匹配評分與搜尋）

這兩項可以理解為：輪廓匹配想把估計位置／朝向改動一些時，要扣多少分。 在輪廓吻合程度接近時，懲罰使演算法更傾向於當前估計附近的答案。

最容易看反的是：配置值越大，扣分越輕；配置值越小，扣分越重。 當前use_scan_matching=False，這兩項雖被讀取，但不參與匹配評分，暫不調整。

1.4.6.1.位姿偏離懲罰參數列

    # Scan Matcher Parameters
    distance_variance_penalty: 5.5
    angle_variance_penalty: 5.0

1.4.6.2.位姿偏離懲罰詳解

1.4.6.2.1.distance_variance_penalty（估計位置附近的候選要扣多少分）

例如，估計掃描位置在A，演算法嘗試把當前輪廓放到旁邊B去與舊輪廓對齊。此項根據A、B之間的平面距離扣分；不按車從上一幀實際走了多少米扣分，也不把候選位置發給底盤執行。

值較小會更偏向A附近，可能減少被相似結構拉走的機會，也可能讓正確的位姿修正受到壓制；值較大則減輕這項約束。它只改變已有候選的分值，不代替前面的搜尋範圍、輪廓質量或匹配門檻。

1.4.6.2.2.angle_variance_penalty（候選朝向偏離估計時要扣多少分）

含義相同，但比較的是內部二維掃描位姿的yaw候選與本次搜尋中心朝向之差。估計已經包含的正常轉動不會直接當作這項偏離；懲罰針對演算法額外嘗試的朝向修正。

這裡不是序列埠JointState中的雲臺機械相對yaw，也不是Nav2／spin輸出的wz。調它不能規定底盤或雲臺最多轉幾度，不能修復隨雲臺轉動時雷達與底盤轉換錯誤。

1.4.6.2.3.配置值先平方，單位與名稱容易混淆

按當前2.6.10的賦值鏈，setParamDistanceVariancePenalty和setParamAngleVariancePenalty都會先將傳入值平方。表中的m、rad按這個賦值鏈及評分公式的量綱解釋；內部使用的才是m²、rad²，不能因為名稱帶variance就把YAML值直接當方差。賦值與評分實現

設d為候選相對搜尋中心的平面偏移，θ為朝向偏移（弧度）。使用當前配置，計算方式為：

• 距離係數：max(0.5, 1 − 0.2 × d² / 30.25)。
• 角度係數：max(0.9, 1 − 0.2 × θ² / 25)。
• 候選分值＝輪廓吻合原始分值×距離係數×角度係數。

0.5、0.9來自後面的minimum_distance_penalty、minimum_angle_penalty，表示係數下限：距離項至多將分值乘0.5，角度項至多乘0.9，並非允許距離或角度門檻。

純公式算例：偏移20 cm時，距離係數約0.99974；朝向偏移10°時，角度係數約0.99976。當前數值對這種程度的偏移扣分很輕；這是公式結果，不是定位精度或實車驗證結果。

粗搜尋參考當前估計掃描位姿，內部精搜尋參考粗匹配所得位姿；並非兩個階段始終相對同一個原始里程計位置扣分。

1.4.6.2.4.生效路徑與實物核對

近期掃描匹配預設啟用這項懲罰；普通近鄰鏈關聯、自動閉環的粗匹配和精匹配呼叫則明確關閉懲罰。因此，開啟掃描匹配後也不是所有匹配都使用這兩項；閉環雖複用普通匹配器精化，仍會跳過這裡的扣分。

實物／模擬nav2_params.yaml及安裝副本的兩項當前值一致，由slam_launch傳給sync_slam_toolbox_node。SMapper::configure讀取後設置到Karto；ROS包裝層未提供引數時的預設值為0.5、1.0，與本工程5.5、5.0不同。引數讀取實現

應使用有限正數：此讀取鏈沒有非正值自動回退；負數會被平方，0會使評分分母為0。不能用0表示關閉懲罰，也不能把執行中設定ROS引數視為內部評分尺度已同步更新。

• 原始碼能確認： 先平方再評分、正值越大懲罰越輕；參考本次搜尋中心；當前未參與評分，近鄰關聯與閉環也不使用這項懲罰。
• 已有驗證範圍： 獨立Point-LIO靜止與約20 cm低速往返，未驗證本節點的平移／轉角修正或懲罰效果。
• 實物待核實： 若以後啟用掃描匹配，觀察是否被重複結構拉偏、是否壓制必要修正；先核對座標轉換、點時間和穩定二維輪廓。坡道識別仍應檢查地形分析與掃描生成，不能靠這兩個評分尺度補救。

1.4.6.3.角度搜索與扣分下限參數列

當前use_scan_matching=False，這6項會被讀取，但不參與當前自動建圖流程的匹配搜尋和評分。若以後開啟匹配，當前角度搜索可概括為：粗搜左右各約20°，每2°試一次；精搜在粗結果左右各約1°內，每0.2°試一次。

    fine_search_angle_offset: 0.00349
    coarse_search_angle_offset: 0.349
    coarse_angle_resolution: 0.0349
    minimum_angle_penalty: 0.9
    minimum_distance_penalty: 0.5
    use_response_expansion: true

1.4.6.4.角度搜索與扣分下限詳解

1.4.6.4.1.fine_search_angle_offset（精搜尋每隔約0.2°試一次）

名稱和部分教程把它說成“精搜尋角度範圍”，但當前原始碼將它傳給角度步距位置；真正的精搜尋半範圍取0.5×coarse_angle_resolution，當前為0.01745 rad，約1°。

因此，當前是在粗匹配朝向附近約±1°內，每0.00349 rad（約0.2°）試一次，共11個角度候選。減小本項只讓這段範圍內試得更密，不會擴大範圍，也不表示最終朝向誤差能達到0.2°。粗細搜尋的實際呼叫

這裡的內部精搜尋用於普通匹配器精化；自動閉環的大範圍粗匹配不會在閉環網格上執行這一精化，後續呼叫普通匹配器時才使用它。

1.4.6.4.2.coarse_search_angle_offset（估計朝向左右各約20°粗搜）

0.349是弧度，約20°，表示當前估計二維掃描朝向兩側的粗搜尋半範圍。它搜的是估計還需要修正多少角度，不是車輛兩幀之間允許轉多少度。

例如車正常轉彎30°，若里程計已正確反映這次轉動，匹配無需額外修正30°。不能看到底盤或雲臺轉得多，就直接把此項按同樣角度增大。它也不負責pitch／roll、坡度識別或雲臺機械限位。

1.4.6.4.3.coarse_angle_resolution（粗搜步距約2°，還影響精搜範圍）

當前粗搜尋從約−20°到+20°，每約2°取一個候選，共21個角度；隨後在粗結果兩側各半個粗步距，即約±1°內精化。

調整時要一起看前兩項：本項變大，粗候選減少，但內部精搜尋範圍會變寬；本項變小，粗候選增多，但精搜尋範圍會變窄。原始碼按round(總範圍／步距)+1取候選數，宜保持範圍與步距能夠合理分段，不能把粗、細兩項當成互不相關的獨立旋鈕。

1.4.6.4.4.minimum_angle_penalty（角度項至少保留多少分）

0.9表示僅角度懲罰這一項最多扣掉10%。實際係數取“公式算出的係數”和0.9中較大的一個：若公式算0.98，就用0.98，並非每次固定乘0.9；若算0.7，則保留到0.9。

它不是角度門檻或最終匹配分數下限。後面還要乘距離係數，所以總分並不保證保留90%。

1.4.6.4.5.minimum_distance_penalty（距離項至少保留多少分）

同理，0.5表示僅距離懲罰這一項最多扣掉50%。增大下限會減輕觸底後的懲罰，減小才允許扣更多分。

結合前一塊已經核實的平方分母，當前普通粗搜尋x、y各約±0.25 m時，距離係數最低約0.99917；即使粗角度擴至約±80°，角度係數仍約0.9844。二者均高於當前0.5、0.9下限，所以即使以後開啟普通匹配，單改這兩個下限也可能看不到變化。這是按當前搜尋邊界計算的結果，未做執行驗證。

兩項下限按正常評分語義取0～1。設1會使對應項不再扣分；設0僅允許係數降到0，不代表關閉懲罰。原始碼沒有完整的區間校驗，不應以超出區間的值表示更強或更弱的正常懲罰。評分系數與下限實現

1.4.6.4.6.use_response_expansion（粗響應接近0時，只擴大角度重試）

開啟後，原始碼先完成正常粗搜；僅當最佳粗響應與0近似相等，才追加嘗試：

• 每次把粗搜尋半範圍再增加20°，最多3次。當前約±20°起步，依次嘗試約±40°、±60°、±80°。
• 一旦粗響應不再近似為0，就停止追加擴角；不是低於閉環0.35／0.45分數門檻就一直擴充套件。
• 平移範圍、角度步距和雷達量程保持原值；它不去全圖尋找位置，也不屬於small_gicp的PCD重定位。

這些角度引數及擴充套件可用於普通匹配、普通近鄰關聯和自動閉環搜尋；兩項扣分下限則只用於啟用懲罰的呼叫，普通近鄰關聯和閉環呼叫仍跳過扣分。約±80°描述粗搜尋，後續精化還能略越過邊緣，不能把它當成車輛轉動限制。

擴角可能增加耗時或匹配到相似輪廓；拿到非零響應也不證明定位正確。若實物中輪廓缺失、動態遮擋，或雲臺轉動／坡道導致二維輪廓變化，擴大yaw範圍不會恢復缺失資訊。零響應擴角與停止條件

1.4.6.4.7.讀取、教程差異與驗證範圍

實物／模擬nav2_params.yaml及安裝副本的6項值一致，隨slam_launch傳給sync_slam_toolbox_node。SMapper::configure讀取後直接設定到Karto；minimum_distance_penalty的ROS包裝層預設值為0.05，本工程顯式覆蓋為0.5。引數讀取實現

fine和coarse步距應使用有限正數；讀取鏈沒有對非正值自動回退的處理，也沒有執行中修改這些ROS引數後同步更新內部匹配器的回撥。教程裡的rough_search_angle_offset不屬於此處實際讀取的引數名，當前鍵名為coarse_search_angle_offset；“自動增大網格”的泛化描述也應按這裡實際只擴角理解。

• 原始碼能確認： 弧度單位、粗搜約±20°／2°步距、精搜約±1°／0.2°步距；下限控制最多扣分；響應近0才擴角，最多3次，平移不擴充套件。當前自動匹配未啟用。
• 已有驗證範圍： 僅獨立Point-LIO靜止與約20 cm低速往返，未驗證本節點轉彎匹配、擴角、閉環或耗時。
• 實物待核實： 若以後啟用匹配，觀察殘餘朝向誤差、重複結構誤配、輪廓變化與計算耗時；先核對雷達／IMU／底盤轉換及點時間，雲臺機械相對角不應直接替代內部二維掃描朝向。