terrain_analysis_ext（擴充套件遠場地形與近遠場結果合併）

1.terrain_analysis_ext（擴充套件遠場地形與近遠場結果合併）

1.1.實車先看：坡道、落差與近遠場銜接

這個節點給全域性代價地圖提供地形點。當前4 m內借用 terrain_analysis 的結果，4 m外才輸出自己計算的結果。 因此，改本塊的地面估計或高度篩選引數，不會同時改變近場；4 m是兩種結果的交接距離，不是雷達最大量程。

這些引數幫助系統看清地形，不能給出實車允許的坡度或臺階高度。車輛爬坡能力還取決於底盤、重心、附著力和實際測量；目前沒有坡道或實物驗證。

1.2.引數

1.2.1.ros__parameters（ROS節點引數）

1.2.1.1.參數列

terrain_analysis_ext:
  ros__parameters:
    scanVoxelSize: 0.1
    decayTime: 0.2
    noDecayDis: 0.0
    clearingDis: 20.0
    useSorting: true
    quantileZ: 0.2
    vehicleHeight: 1.0
    voxelPointUpdateThre: 100
    voxelTimeUpdateThre: 2.0
    lowerBoundZ: -1.5
    upperBoundZ: 0.5
    disRatioZ: 0.2
    checkTerrainConn: False
    terrainUnderVehicle: -0.75
    terrainConnThre: 0.5
    ceilingFilteringThre: 2.0
    localTerrainMapRadius: 4.0

1.2.1.2.引數詳解

1.2.1.2.1.輸入、輸出和高度基準

自身遠場保留點的 x、y、z，不把點移到地面上；改寫的是 intensity，它已經不是雷達反射強度。當前全域性代價地圖的 IntensityVoxelLayer 使用本輸出，並以 min_obstacle_intensity=0.1、max_obstacle_intensity=2.0 篩選高度差。達到10 cm只是滿足其中一項條件，還要通過座標高度、距離等篩選才可能成為障礙。

下文的“高於／低於雷達”都指點的 odom z 減去輸入位姿 z；距離指 odom 的水平距離。原始碼沒有在這裡查詢雷達→底盤 TF，也沒有按雲臺姿態重新定義豎直方向。上游 loam_interface 把輸入當雷達位姿，而 Point-LIO 原始輸出是內部 IMU 位姿；此前記錄的 IMU→雷達轉換缺口仍會影響本塊的高度基準，不能靠擴大高度範圍代替修正介面。

1.2.1.2.2.lowerBoundZ、upperBoundZ 與 disRatioZ：能不能看到遠處上下坡

這三項先決定坡點有沒有資格留下，再談坡會不會被當成障礙。當前上下邊界相對輸入位姿，並隨距離擴大：

比如5 m外的下坡點比輸入原點低2.0 m，能通過當前視窗；低3.0 m則會被刪。5 m外的上坡點高1.0 m能通過，高2.0 m則會被刪。這裡只舉高度篩選，仍需滿足後面的輸出條件。

所以：下坡地面缺失，檢查下界；上坡地面缺失，檢查上界；越遠越缺，連同距離放寬比例一起檢查。 下界的“調大”是數值變大，例如-1.5到-1.0，反而縮小向下保留範圍。

與上一塊不同，擴充套件節點在入快取、快取更新、選地面和自身遠場輸出時，都使用這個隨距離擴大的視窗；沒有在最終輸出時退回固定上下界。disRatioZ=0.2 表示每增加1 m水平距離，上下各放寬0.2 m，不代表允許的坡度或坡面識別結果。

上實物前應先核對原點離地高度、odom 豎直方向和位姿轉換。視窗過窄會丟坡面，過寬會引入頂棚或其他樓層；是否成為障礙還取決於區域性地面估計及後續篩選。當前不據這些數值直接修改重力方向或安裝外參。

1.2.1.2.3.useSorting 與 quantileZ：正常坡為什麼也可能像障礙

擴充套件節點把水平地面分成寫死的0.4 m格子，每格收集附近高度，選一個“這片區域的地面高度”。當前先排序，再取 floor(0.2×样本数) 處的高度；10個樣本取從低到高第3個。False 則取最低高度。格子和鄰域內有坡、障礙或離群點時，這個代表高度未必等於每個點下面的真實地面。

本節點自己的遠場始終計算 abs(点高度−估计地面高度)，沒有 considerDrop 開關。 一個點比估計地面高15 cm，或低15 cm，都會輸出0.15 m的高度差；滿足其他條件時，兩者都可能達到全域性代價地圖的10 cm障礙門檻。

這就是下坡尤其要看的地方：正常坡的較低側，可能只是低於這一小片的代表地面，並不是真正的坑。反過來，真正坑窪只有被雷達看到且留下有效回波，才可能通過這個高度差表現出來；沒有回波的斷崖不因此自動得到可靠識別。擴充套件節點也沒有近場那種 noDataObstacle 合成障礙功能，沒有點的區域如何影響規劃，還取決於下游的未知區域設定。

把 quantileZ 調大，通常會抬高代表地面：較高點的高差可能減小，較低點的高差卻可能增大。因此不能照搬“調大就更容易上下坡”。應一起看正常坡面、小臺階和低障礙，確認地面估計是否偏高或偏低。這裡沒有正負高差相加抵消，也沒有允許上坡／禁止下坡的專用開關。

1.2.1.2.4.vehicleHeight：哪些障礙點能進入遠場輸出

當前自身遠場只保留絕對高差 h<1.0 m 的點，1.0 m邊界不保留。這個值沒有更改機器人尺寸，也不表示能跨越1 m臺階。

例如：障礙有離地20 cm的有效回波，可能進入輸出；若它只有離地1.2 m處的回波，而“地面高度”選得正確，這些回波會被本項刪掉。調小可能漏掉部分障礙，調大可能把懸空橫梁等結構引入結果。應結合真實車輛需要避讓的高度、雷達能看見的部位和地面估計核對，不能直接把名字當整車高度填入。

與下游10 cm門檻共同作用時，自身遠場進入障礙篩選的高差範圍為 0.1～不足1.0 m，仍需通過其他條件。4 m內借入的近場點沿用近場當前0.5 m上限，不受本項重新篩選。

1.2.1.2.5.localTerrainMapRadius：為什麼4 m附近要單獨看

當前以輸入里程計位置為中心：d≤4 m 直接複製 terrain_map 點；d>4 m 使用擴充套件節點自算點。近場點不會再次套用本節點的 vehicleHeight、排序、高度視窗、衰減或連線檢查。

兩側當前有這些區別：

因此，同一坡面或物體隨著車輛接近，跨過4 m時可能改變高度差或可見點；這是需要觀察的原始碼差異，目前沒有確認發生實際交接故障。擴大半徑會借用更多近場結果，縮小會更早使用較粗遠場；它不改變代價地圖大小、雷達量程或車輛半徑。

還有兩個介面前提：近場沒有點，擴充套件節點不會用自己的4 m內計算補洞；近場停止更新，原始碼也不檢查其年齡。 它會繼續合入最近收到的近場結果，並把合併訊息標成當前掃描時間。不能僅憑 terrain_map_ext 的時間戳新，就斷定其中所有點都新。

1.2.1.2.6.checkTerrainConn 及三個配套引數：當前先知道沒有生效

當前 checkTerrainConn=False，因此 terrainUnderVehicle、terrainConnThre、ceilingFilteringThre 都不影響輸出。其他高度篩選仍在執行；這裡只是專門的連線檢查與頂棚篩選關閉。

若以後考慮啟用，它從輸入位置附近的地面出發，嘗試把高度接近的地面區域連起來；自身遠場只輸出連上的區域。三項含義是：

• terrainUnderVehicle=-0.75 m：僅當中心格沒有地面樣本時，假定地面在輸入原點下方0.75 m，作為檢查起點。若使用前提成立，應測量該原點到地面的關係；它不是底盤離地間隙，也不是直接讀取的實車高度。
• terrainConnThre=0.5 m：候選兩片地面的高度差小於0.5 m才接上。過小可能斷開正常坡，過大可能接上真實臺階或跨過缺口；不是允許爬50 cm臺階。
• ceilingFilteringThre=2.0 m：連線檢查中，高度相差超過2.0 m的候選可被標記排除。原始碼用絕對差，遠低於當前地面的區域也可能命中；不是“高於雷達2 m就刪”的頂棚線，也不是車頂淨空。

原始碼一次能比較橫縱各±10個地面格，即各±4 m，並非只檢查緊貼的小格；中間是否連續有地面沒有逐段證明。因此這個檢查也不能直接當作車輛可通行性或可靠斷崖判據。當前關閉，無需優先調整這三項；啟用的適用性要另行驗證。

1.2.1.2.7.scanVoxelSize、decayTime 與更新門檻：細障礙和舊點

scanVoxelSize=0.1 m 是自身累計點雲的三維降取樣尺度，不是地面格子大小，也不保證10 cm以上障礙都能看見。細杆、障礙邊緣和遠處稀疏回波應重點觀察；調小能保留更多細節，但點數與計算量會上升。

歷史快取按寫死的2 m水平分塊管理。每塊新入點達到100，或距上次更新達到2.0秒，或發生清圖，才重新降取樣並檢查年齡；這些步驟仍由新掃描驅動。decayTime=0.2 s 是檢查時使用的過期門檻，不能覆蓋這個觸發條件。

所以，近處點密集的塊可能很快更新，遠處點少的塊可能等到時間門檻，舊點保留超過0.2秒。沒有新掃描時也不會單靠計時繼續處理。noDecayDis=0.0 沒有免衰減區域；以後調大，只讓指定範圍的自身歷史繞過年齡檢查，不會自動恢復近場缺失的輸出。

這兩個更新門檻不是釋出頻率，原始碼100 Hz迴圈也不等於每秒輸出100幀。先沿用門檻；若實物發現舊障礙殘留、地面缺點或負擔過大，再結合點密度、衰減時間一起判斷。擴充套件節點沒有近場那組 clearDyObs 動態篩選引數，不能把它當成同一套清理演算法。

1.2.1.2.8.clearingDis：按事件清半徑內，且只清自己

當前20 m表示收到清圖事件後，刪除水平距離小於20 m的自身快取點。平時沒有自動執行，也不是最大輸出範圍。

joy 第6個按鈕可觸發；cloud_clearing 的 Float32 訊息還會用訊息數值覆蓋半徑。近場節點對應話題是 map_clearing，二者不同。擴充套件節點清圖後仍會借入 terrain_map 的近場結果，因此近場點可以繼續出現在合併輸出中；不能把清自身快取理解為清掉整套地圖。本輪沒有傳送清圖事件。

1.2.1.2.9.啟動來源與當前驗證範圍

整套導航 launch 會同時安排兩個地形分析節點，擴充套件節點本身始終是獨立程序，不隨導航元件開關變成元件。本輪在 ysu_ros_humble 中只查詢環境與檔案，確認包路徑、安裝後的獨立 launch、導航 launch 和兩份 YAML 與當前原始碼檔案一致；沒有啟動節點或讀取執行節點的有效引數。

17項都在啟動時宣告和讀取，沒有動態引數更新回撥；執行時僅寫同名引數不會更新演算法中的成員變數。clearingDis 可由清圖訊息改變數，這與重新讀取 ROS 引數是兩回事。衰減計時使用輸入點雲時間戳，不是電腦牆鍾；本塊未列 use_sim_time，也不能據此證明全鏈路時鐘或點內時間單位已經正確。

輸入里程計與掃描沒有時間配對或插值，也沒有等待有效里程計的保護；近場點雲同樣沒有同步與新鮮度檢查。節點直接使用輸入座標，未校驗參考系，最後卻固定寫 odom。這些前提對坡面高度和距離分割槽都有影響。

• 原始碼能確認： 17項讀取與當前門控、隨距離擴大的高度視窗、遠場絕對高差、4 m近遠場合並、衰減與清圖觸發方式，以及三個未生效引數；上面的行為解釋按當前原始碼複核。
• 模擬已觀察： 2026-10-08 的靜止和約20 cm往返只覆蓋獨立 Point-LIO 輸出，沒有驗證擴充套件地形、上下坡、坑窪、頂棚、4 m交接或全域性代價地圖。
• 必須實物核實： 輸入原點到地面的高度、內部 IMU→雷達→底盤關係、odom 豎直方向、坡面及低障礙的回波密度、下坡與真實落差是否被正確區分、交接處一致性和障礙離開後的殘留。MID360 隨實際 yaw、不隨實際 pitch 轉動的機械約定繼續保留；本輪未作執行測試。

原始碼位置：src/pb2025_sentry_nav/terrain_analysis_ext/src/terrainAnalysisExt.cpp；啟動來源：src/pb2025_sentry_nav/pb2025_nav_bringup/launch/navigation_launch.py 和兩份 nav2_params.yaml；下游高度差篩選：src/pb2025_sentry_nav/pb_nav2_plugins/src/layers/intensity_voxel_layer.cpp。本塊僅更新日誌解釋，工程原始碼、配置和引數值均保留。