導航引數

1.導航引數

1.1.實車處理建議說明

下面各參數列的“實車處理建議”以當前 MID360 實物配置為準：use_imu_as_input=False、imu_en=True，建圖時不使用先驗 PCD。切換估計模型、資料格式、先驗地圖或啟動方式後，需要重新核對引數是否參與計算及是否被 launch 覆蓋。

“通常保持”表示初次測試不優先調這個引數；“按裝置型號和驅動核對”仍需要完成一次裝置適配檢查；“上實物前必須核對”則強調實際輸入、單位和座標約定必須明確。核對正確就保持，不代表每次上車都要改數值。

具體檢查事項、生效條件和原始碼疑點寫在“生效條件／備註”列。當前 gravity 的模長已修正，仍需核實方向與目標參考系；gravity_init 在當前 IMU 初始化路徑中不參與計算；lidar_meas_cov 的原始碼含義需要先核實，再決定是否調整。

1.2.章節目錄

• Point_LIO（雷射與IMU融合里程計，建圖與定位演算法）
• loam_interface（Point-LIO輸出的座標與話題適配）
• sensor_scan_generation（雷達點雲換系與底盤、雲臺位姿轉換）
• terrain_analysis（近場地面與障礙高差分析）
• terrain_analysis_ext（擴充套件遠場地形與近遠場結果合併）
• fake_vel_transform（雲臺系速度轉換與底盤自旋）
• small_gicp_relocalization（先驗點雲地圖重定位）
• pointcloud_to_laserscan（三維地形點雲轉二維掃描）
• slam_toolbox（二維地圖生成與軌跡最佳化）
• bt_navigator（行為樹導航任務排程）
• controller_server（路徑跟蹤、進度檢查與到達判定）
• local_costmap（區域性障礙代價地圖）
• global_costmap（全域性規劃代價地圖）
• map_server（已有二維地圖載入與釋出）
• map_saver（二維柵格地圖儲存）
• planner_server（Theta*全域性路徑規劃）
• smoother_server（全域性路徑平滑）
• behavior_server（恢復行為與空閒方向移動）
• waypoint_follower（逐點導航與到點等待）
• velocity_smoother（速度限幅與加減速平滑）
• pb_teleop_twist_joy_node（手柄輸入與可替換控制入口）

1.3.通用引數修改

這些引數分為六類：車體尺寸、低矮凸起篩選、坑窪與欠資料處理、障礙膨脹、豎直感知範圍和地圖網格。它們的取值依據不同，不能全部按底盤長寬直接計算。

車體尺寸：為什麼兩處 robot_radius 建議設為 0.42 m

當前模型的底盤碰撞盒是 0.525 × 0.525 m，參考點位於底盤平面中心。用圓形輪廓表示車輛時，需要覆蓋四個角，因此半徑至少為：

R = √((0.525/2)² + (0.525/2)²)
  ≈ 0.371 m

取 0.42 m，相當於在模型外接圓基礎上留約 5 cm 餘量。這個餘量用於應對定位、地圖離散化和控制誤差，是第一版測試的工程起點，並非已經測定的誤差上限。

區域性、全域性代價地圖應使用一致的車體尺寸：全域性負責規劃能通過的路線，區域性負責跟蹤和碰撞判斷。當前 0.2 m 半徑不足以包住模型底盤；降低車速也不能彌補這個幾何尺寸錯誤。

低矮凸起篩選：為什麼 min_obstacle_intensity 需要驗證

這裡接收的是地形分析點雲，intensity 表示演算法計算的地面高差，單位是米。近場先計算“點高度減去估計地面高度”；擴充套件節點自身遠場對這個差值取絕對值。它是相對於區域性估計地面的結果，不一定等於從正常路面量出來的真實凸起高度或坑深。

當前 0.1 相當於 10 cm。假設地面估計正確，且其他篩選條件滿足：地面上高5 cm的凸起點會低於該門限，高15 cm的點則達到這道門限。因而，幾釐米高、車輛卻不能通過的物體可能沒有進入即時障礙地圖。這不表示深度小於10 cm的坑允許通過，也不保證更深的坑一定被識別。

0.03～0.05 m 是平地低速測試的候選範圍，用來檢查能否保留這些低矮障礙。最終門限應同時滿足：

• 必須繞開的最低障礙能夠被保留。
• 正常地面起伏和測量噪聲不會大量變成假障礙。

例如，若 3 cm 高的物體必須繞開，最終採用 0.05 m 就可能將它過濾掉。區域性、全域性地圖的兩個門限應一起核對，避免兩張地圖對同一障礙判斷不同。

坑窪與欠資料處理：noDataObstacle 能做什麼，不能做什麼

坑窪要區分兩種情況：

• 雷達看到了坑底。 當前 terrain_analysis.ros__parameters.considerDrop=False，近場低於估計地面的負高差點會被過濾。降低 min_obstacle_intensity 不能恢復已經被上游刪掉的點。即使以後考慮開啟 considerDrop，地面估計、坑底能否被看到和下游高度篩選仍會影響結果，不能把取絕對值當作完整的防坑檢測。
• 雷達沒有看到坑底或地面。 此時沒有有效點可用來計算坑深。noDataObstacle 針對的是這類地面樣本不足的區域，但遮擋、掃描盲區、點雲稀疏和點被前級過濾，同樣可能造成樣本不足；欠資料不等於已經發現真實坑洞。

noDataObstacle 的完整配置位置是：

terrain_analysis:
  ros__parameters:
    noDataObstacle: False

它的兩種取值含義是：

• False：本節點不為欠資料格合成額外障礙點。靜態地圖、其他障礙來源或下游未知區域策略仍可能阻止通行，不能因此斷言“沒有點就一定能走”。
• True：完成無資料處理初始化後，將地面高度樣本數不足 minBlockPointNum 的格子作為欠資料候選，再按 noDataBlockSkipNum 的邊界跳過規則合成障礙點。當前 minBlockPointNum=10、noDataBlockSkipNum=0；它判斷的是樣本是否足夠，不是隻判斷點數是否為零，也沒有計算坑的深度。

開啟後可以讓欠資料區域更保守，但也可能把正常的遮擋或稀疏地面變成障礙，導致原本能走的區域被封住。合成點還需通過下游高度、量程和體素視窗等篩選，最後才可能進入代價地圖。因此，應分別檢查地形點雲和最終代價地圖，不能只看開關是否為 True。

該開關屬於近場 terrain_analysis，terrain_analysis_ext 沒有同名開關。擴充套件節點會合並近場範圍內的地形結果，因此合成點也可能隨之進入全域性障礙鏈路，但這不等於遠場全範圍都具備同樣的欠資料保護。遠場對高差取絕對值，也不能保證近車的坑、斷崖或樓梯邊緣會被可靠識別。

第一版在已確認無坑和斷崖的平整場地進行低速測試時，可暫保留當前 False。若後續確實需要防坑，再單獨驗證開啟後的地形輸出、代價地圖和停車行為；既不能用10 cm門限判斷坑是否可通過，也不能把 noDataObstacle=True 當作已經具備可靠的防跌落保護。 實現細節見 近場地形分析 與 擴充套件地形分析。

障礙外圍代價：為什麼先保留 inflation_radius=0.7、cost_scaling_factor=4.0

這兩個值目前沿用已有配置，尚沒有證據需要為首次低速測試調整；它們不是根據底盤尺寸推算出來的唯一正確值。

inflation_radius=0.7 m 表示障礙周圍寫入膨脹代價的範圍。它大於擬採用的車體半徑，在車體碰撞邊界之外留出外圍代價區域，供規劃器考慮靠近障礙的代價。這個範圍並非全部都是硬性禁止區。

cost_scaling_factor=4.0 控制外圍代價下降的速度。數值越大，代價隨距離下降越快；數值越小，較高代價維持得更遠。

你的跟蹤控制器主要檢查高代價格子，沒有直接利用全部外圍軟代價給軌跡打分。因此，先正確配置 robot_radius；擴大膨脹範圍並不能替代車體尺寸適配，也不保證控制器自動更早減速。

豎直感知範圍：為什麼高度引數暫時保留

terrain_analysis.vehicleHeight=0.5 和 terrain_analysis_ext.vehicleHeight=1.0，限制的是輸出地形點相對估計地面的高度差。它們不表示底盤厚度，也不是車輛能跨越的臺階高度。

這兩個值暫時保留，是因為單憑底盤長寬無法推出合適的高度篩選範圍。後續需要結合整車外形，檢查可能碰到車身或雲臺的結構是否仍被保留。不能直接把底盤厚度 0.20 m 填進去。

兩張代價地圖的豎直體素引數則決定另一道空間視窗：

origin_z = 0.0 m
z_resolution = 0.05 m
z_voxels = 16

竖直覆盖总高 = 0.05 × 16 = 0.8 m

這些高度按對應地圖座標系解釋。當前平地首測可以先保留，但上坡、下坡或地圖高度基準變化時，需要重新核對視窗是否仍覆蓋有效障礙。即使前置篩選寫了 max_obstacle_height=2.0，點也仍要通過這道體素視窗。

地圖網格：為什麼先保留 5 × 5 m 和 0.05 m

區域性地圖的 width=5、height=5 表示整張滾動地圖覆蓋約 5 × 5 m，中心兩側各約 2.5 m。這是觀察和計算範圍，與車輛外形大小無關。

resolution=0.05 m 表示每個格子邊長為 5 cm。區域性地圖大約包含：

(5 / 0.05) × (5 / 0.05) = 100 × 100 个格子

這組設定可以作為低速首測的起點。減小格距能表達更細的障礙，但會增加計算量；擴大地圖範圍同樣會增加處理開銷。目前先觀察實際障礙表達與計算負載，再決定是否調整。

全域性地圖最終使用的格距還可能受靜態地圖影響，應以執行時實際地圖資訊為準。

1.4.測試

1.4.1.低速測試

以下用於第一版實車導航測試：保留北極熊演算法原始碼和原有 launch 接線，通過導航引數降速，並在自己的序列埠節點增加最終限幅。限制的是目標指令，實車速度和停車響應仍需驗證。

1. 導航引數

配置檔案：src/pb2025_sentry_nav/pb2025_nav_bringup/config/reality/nav2_params.yaml。表中的 FollowPath 引數位於 controller_server.ros__parameters.FollowPath 下。

x、y 各設 0.14，是因為平滑器按軸限速，對角合速度約為 sqrt(0.14² + 0.14²) = 0.198 m/s，沿單軸移動的上限則為 0.14 m/s。 當前 enable_rotation=false 且自旋項為零時，正常跟蹤下發的底盤角速度為零。

2. 恢復動作單獨降速，保持原 launch 接線

以下兩個行為樹都使用相同的低速恢復引數：

• src/pb2025_sentry_nav/pb2025_nav_bringup/behavior_trees/navigate_to_pose_w_replanning_and_recovery.xml
• src/pb2025_sentry_nav/pb2025_nav_bringup/behavior_trees/navigate_through_poses_w_replanning_and_recovery.xml

<BackUp backup_dist="0.20" backup_speed="0.10"/>

本輪沿用實車預設的 use_composition=True。該分支中，恢復速度經過雲臺座標變換後到達序列埠，但繞過 velocity_smoother； 因此，上表中的平滑器加減速度引數不約束這條恢復路徑。 Nav2 Humble 官方預設啟動接線同樣讓恢復動作繞過速度平滑器，本輪不要求修改 launch 將恢復動作接入平滑器。 非組合啟動分支不在本輪驗證範圍內。

3. 序列埠最終限幅與斷流保護

修改檔案：src/serial_comm/src/serial_comm.cpp。 以下兩項是速度回撥中的 C++ 常量，不屬於 nav2_params.yaml；CMD_VEL_TIMEOUT 是序列埠節點自己的接收超時門限。

速度保護按以下順序執行：

1. 收到速度時，先檢查 vx、vy、wz 是否都是有限值；任一欄位為 NaN/Inf，就清零速度並標記為無效，不重新整理有效指令的接收時間。
2. 在 double 精度下計算平移合速度，超限則同比縮小 vx、vy；再限制 wz，最後轉換成協議使用的 float32。
3. 在傳送定時器中檢查接收超時，過期後同時傳送無效標誌和零速度，保留原協議結構及雲臺角度欄位。

vx、vy 表示底盤平移速度在雲臺 yaw 座標系下的分量，單位為 m/s；wz 是底盤角速度，單位為 rad/s；rel_pitch、rel_yaw 是雲臺目標相對角，單位為 rad。序列埠速度限幅不會替代正常加減速控制。

4. 載入與測試

修改 C++ 後，需要在實際執行環境重新編譯並重啟 serial_comm。導航 YAML 和行為樹 XML 也需要重新載入：若安裝目錄使用符號連結，重啟導航即可；若安裝目錄是複製檔案，則先重新安裝對應包。修改控制器速度上限後應重啟導航，使 PID 重新讀取上下限。

啟動後核對實際引數；使用名稱空間時，為節點名加上對應字首：

ros2 param get /controller_server FollowPath.v_linear_max
ros2 param get /controller_server min_y_velocity_threshold
ros2 param get /velocity_smoother max_velocity
ros2 param get /velocity_smoother velocity_timeout
ros2 topic info /cmd_vel --verbose

對應預期為 0.2、0.001、[0.14, 0.14, 0.2]、0.2。檢查 /cmd_vel 釋出者，避免獨立啟動的手柄轉換節點與導航直接競爭同一個出口；需要手柄接管時，應使用明確的指令仲裁。

先架空核對方向、單位、零指令和斷流停車，再落地進行短距離單目標測試。重點觀察實際速度、取消導航、到點停車以及恢復動作結束後的表現；保持硬體急停可用。

本輪保留的 fake_vel_transform 舊速度快取問題尚未修改：序列埠超時只能檢測自己是否持續收到訊息，無法識別上游重發的舊非零指令。 因此，序列埠限速和斷流清零不能代替對整套導航取消、到點停車行為的驗證。