Point_LIO（激光與IMU融合里程計，建圖與定位算法）

1.Point_LIO（激光與IMU融合里程計，建圖與定位算法）

1.1.參數

1.1.1.ros__parameters（ROS節點參數）

1.1.1.1.參數表

point_lio:
  ros__parameters:
    use_imu_as_input: False                             # Change to True to use IMU as input of Point-LIO
    prop_at_freq_of_imu: True
    check_satu: True
    init_map_size: 10
    point_filter_num: 8                                 # Options: 4, 3
    space_down_sample: True
    filter_size_surf: 0.2                               # Options: 0.5, 0.3, 0.2, 0.15, 0.1
    filter_size_map: 0.2                                # Options: 0.5, 0.3, 0.15, 0.1
    ivox_nearby_type: 18                                # Options: 0, 6, 18, 26
    runtime_pos_log_enable: False                       # Option: True

1.1.1.2.參數詳解

1.1.1.2.1.use_imu_as_input（IMU參與運動估計的方式）

use_imu_as_input 決定 IMU 怎樣參與濾波。

當前 False 的處理方式是：濾波器內部估計角速度、加速度等狀態，收到 IMU 後用測量糾正這些狀態，再進行運動預測和激光匹配。

設為 True 後，加速度與角速度作為預測輸入，經過單位縮放、偏置處理後推動運動狀態。切換模型還會改變所使用的噪聲參數，所以首輪保持當前 False。

關閉 IMU 對應的是另一個參數 mapping.imu_en。

1.1.1.2.2.prop_at_freq_of_imu（按IMU時刻更新估計不確定性）

prop_at_freq_of_imu 主要影響協方差傳播。

協方差可以理解為濾波器對當前估計的不確定程度。當前 True 時，代碼在處理 IMU 時刻傳播它；狀態仍會推進到激光點的採樣時刻。

改成 False 後，代碼還會在激光逐點時刻增加協方差預測，通常計算量更大。它不是“把 Point-LIO 輸出頻率設成 IMU 頻率”的開關，也不能保證切換後精度更高。

1.1.1.2.3.check_satu（IMU讀數量程保護）

check_satu 是逐軸處理。

當前代碼分別比較三個加速度軸和三個角速度軸：abs(原始轴值) ≥ 0.99 × 对应饱和阈值

被標記的軸停止參與這次 IMU 觀測更新，其他軸繼續使用。因此，它需要配合正確單位和量程的 satu_acc/satu_gyro。這項檢查在當前 output 模型中生效。

1.1.1.2.4.init_map_size（初始化地圖的最少點數）

init_map_size=10 只是一道點數門檻。

代碼判斷的是：init_feats_world->size() >= init_map_size

如果一幀預處理後已有幾千個點，設置 10 或 100 都可能在同一幀滿足條件。因此，增大它不一定增加初始化時間，也不能替代靜止 IMU 初始化。

初始化地圖使用的是預處理後的點雲，並沒有直接使用經過 filter_size_surf 體素降採樣的那份點雲。

三個點數控制參數作用於不同階段。

初始化完成後的常規處理，大致是：

原始点云
 → line/tag 初筛
 → point_filter_num：按间隔取点
 → 距离等过滤
 → filter_size_surf：扫描空间降采样
 → 逐点匹配
 → filter_size_map：地图插入筛选

例如，有約 10,000 個點通過 line/tag 初篩：

• point_filter_num=8：先選擇約 1,250 個，後續過濾可能繼續減少。
• 改為 4：先選擇約 2,500 個。
• 隨後掃描體素降採樣，還會進一步壓縮空間上密集的點。

因此，間隔取點更多，不一定等比例增加最終匹配點數，還要看掃描體素、遮擋和匹配條件。

1.1.1.2.5.ivox_nearby_type（地圖鄰格搜索範圍）

ivox_nearby_type=18 表示鄰接網格類型。

本地實現實際搜索的網格數包含中心網格：

它不表示“找 18 個點”或“搜索 18 米”。

實際空間尺度還取決於 mapping.ivox_grid_resolution，當前為 0.5 m；匹配過程中另有最近鄰和殘差條件。

以後調這組參數時，主要比較輸出延遲、有效環境結構、點雲重影和軌跡連續性。先使用當前值建立基準，再針對具體表現改變一個參數，才能判斷改善來自哪裏。

1.1.2.common（輸入話題與點雲時間處理）

1.1.2.1.參數表

    common:
      lid_topic: "livox/lidar"
      imu_topic: "livox/imu"
      con_frame: False                                  # True: if you need to combine several LiDAR frames into one
      con_frame_num: 1                                  # the number of frames combined
      cut_frame: False                                  # True: if you need to cut one LiDAR frame into several subframes
      cut_frame_time_interval: 0.05                     # should be integral fraction of 1 / LiDAR frequency
      time_diff_lidar_to_imu: 0.0                       # Time offset between LiDAR and IMU calibrated by other algorithms, e.g., LI-Init (find in Readme)

1.1.2.2.參數詳解

1.1.2.2.1.lid_topic 與 imu_topic（雷達與IMU輸入話題）

兩個話題參數：名稱、消息類型和命名空間必須同時對應。

當前實車驅動設置 xfer_format=4、multi_topic=0，源碼發佈：

livox/lidar             → Livox CustomMsg
livox/lidar/pointcloud  → PointCloud2
livox/imu              → sensor_msgs/Imu

Point-LIO 當前 lidar_type=1，訂閲的是 CustomMsg，因此配置中的 livox/lidar 與驅動匹配。

這兩個配置都是相對話題名：

驅動和 Point-LIO 的命名空間或話題重映射要一致。話題名負責選擇輸入源，消息中的 frame_id 表示座標名稱；選擇話題不會自動進行座標變換。

1.1.2.2.2.con_frame 與 con_frame_num（多幀點雲合併）

con_frame 的實際行為與註釋不完全一致。

當前 Livox 合幀分支執行的是：

1. 收到前10幀，累計它們的點。
2. 把每幀逐點時間調整到第一幀的起始時間。
3. 第11次回調時，把累計點雲放入處理隊列。
4. 這次回調收到的第11幀沒有加入累計點雲，然後計數清零。

所以即使設置：

con_frame: True
con_frame_num: 1

也不會按“合併1幀”執行。代碼仍累計10幀，再由下一次回調觸發輸出。

假設驅動穩定20 Hz，這段邏輯理論上約每11個回調輸出一個合併包，即約 1.82 Hz，累計點雲覆蓋約 0.5秒。這會增加等待時間和單次處理點數，因此首輪保持 False。

1.1.2.2.3.cut_frame 與 cut_frame_time_interval（點雲拆幀設置）

拆幀函數存在，但 YAML 開關沒有連接到它。

實際代碼檢查：

if (cut_frame_init) 
{
    process_cut_frame_livox(..., cut_frame_num, ...);
}

其中 cut_frame_init 默認 False。當前沒有發現把 common.cut_frame 賦給它，也沒有發現使用 cut_frame_time_interval。 此外，這個內部函數按點數分段，使用 cut_frame_num，並不是直接按照 YAML 中的0.05秒切分。 因此，單獨修改這兩個 YAML 參數不會實現註釋描述的拆幀行為。

1.1.2.2.4.time_diff_lidar_to_imu（雷達與IMU時間偏移）

時間差參數沒有接入，不代表系統不需要同步。

IMU 回調實際修正時間戳的表達式是：

修正后时间 = 原始时间
           − timediff_imu_wrt_lidar
           − time_lag_IMU_wtr_lidar

它使用的是另外兩個變量，目前初始化為零；沒有使用 common.time_diff_lidar_to_imu。

Point-LIO 仍然會根據時間戳等待 IMU 覆蓋點雲掃描末尾。但這只是按時間戳組織數據，不能證明兩種傳感器的物理採樣時間已經對齊。

以後實車需要測量實際同步狀態；如果發現偏移，必須先確認校正方式及其符號，再處理對應代碼或驅動配置。當前把 time_diff_lidar_to_imu 填成非零值，不會完成校正。

1.1.3.prior_pcd（先驗點雲地圖）

1.1.3.1.參數表

    prior_pcd:
      enable: False
      # NOTE: `prior_pcd_map_path` will be provided in the launch file
      # prior_pcd_map_path: ""
      init_pose: [ 0.0, 0.0, 0.0 ]

1.1.3.2.參數詳解

1.1.3.2.1.enable（先驗地圖加載開關）

enable 改變的是初始匹配地圖的來源。

當前 False 的流程是：

IMU 初始化
 → 累计当前扫描点
 → 达到 init_map_size 门槛
 → 用这些点建立地图
 → 后续匹配并更新地图

開啓後，達到同樣的初始化門檻時，代碼改為讀取 PCD，並把其中的點加入地圖搜索結構。

加載先驗地圖不會自動完成全局重定位，也不會自動確定機械人在整張地圖中的位置。初始位置、姿態和地圖座標必須足夠一致，後續局部匹配才有基礎。

另外，開啓先驗後，代碼會延後新點插入；相關有效處理計數超過200後，仍會繼續更新地圖。因此這個開關也不意味着地圖永久凍結。

先驗 PCD 加載時沒有自動做座標旋轉或單位縮放，地圖還必須與當前 Point-LIO 參考系一致。源碼對 PCD 讀取失敗返回的空指針也沒有在調用處檢查，所以啓用前需要確認文件有效。

1.1.3.2.2.prior_pcd_map_path（先驗點雲地圖路徑）

路徑註釋只對相應啓動鏈成立。

localization_launch.py 會將啓動參數 prior_pcd_file 傳給：prior_pcd.prior_pcd_map_path

但它沒有因此自動把 prior_pcd.enable 改成 True。當前配置即使獲得路徑，Point-LIO 仍不加載它。

而 slam_launch.py 會明確覆蓋：prior_pcd.enable: False

這與從當前掃描建圖的用途一致。

同一定位啓動鏈中的 small_gicp 也會接收先驗文件，但它是另一個節點。Point-LIO 關閉先驗，不代表 small_gicp 沒有使用地圖。

1.1.3.2.3.init_pose（內部IMU的初始位置）

init_pose 有三個容易誤解的地方：

• 它是 [x,y,z]，三個數都是米。 第三個數不是 yaw，也不包含 roll/pitch/yaw。
• 它對應內部 IMU 原點在 camera_init 中的位置。 不是直接設置 base_footprint 的導航位置。
• 本地代碼的賦值時機比較特殊。 它在首次輸出位姿時直接修改 kf_output.x_.pos，並非在加載先驗、開始匹配之前統一完成初始化。

尤其在你當前“高頻里程計開啓、path 關閉”的設置下，這次賦值發生在首次成功匹配後的里程計輸出中。因此，未來啓用先驗時，不能只填一個位置就認為首輪匹配已經從該位置開始；這段初始化順序需要另行審查。

1.1.4.preprocess（雷達格式與點雲預處理）

1.1.4.1.參數表

    preprocess:
      lidar_type: 1                                     # 1 for Livox serials LiDAR, 2 for Velodyne LiDAR, 3 for ouster LiDAR
      scan_line: 4
      timestamp_unit: 3                                 # the unit of time/t field in the PointCloud2 rostopic: 0-second, 1-milisecond, 2-microsecond, 3-nanosecond.
      blind: 0.5

當前 lidar_type=1、scan_line=4 與本地 MID360 驅動實現一致；blind=0.5 可以先作為基準。 timestamp_unit=3 在當前 Livox CustomMsg 分支中沒有參與換算，但逐點時間仍被正確地按 ns 處理。

1.1.4.2.參數詳解

1.1.4.2.1.lidar_type（雷達類型與輸入格式）

lidar_type：選擇消息格式和解析分支。 本地支持的值是：

它不僅決定字段怎樣解析，還決定訂閲 CustomMsg 還是 PointCloud2。當前實車驅動在 livox/lidar 發佈 CustomMsg，所以設置為 1。

仿真使用 2，是因為轉換器生成了 Velodyne 樣式字段，並不代表仿真中換了一台 Velodyne 雷達。

1.1.4.2.2.scan_line（有效掃描線數量）

scan_line：限制接受的 line 編號。 當前判斷條件是：msg->points[i].line < N_SCANS

N_SCANS 來自 scan_line，所以設置4時允許：line = 0、1、2、3

本地驅動對 MID360 定義 kLineNumberMid360=4，並按：point.line = i % pkt.line_num;生成 line 字段。因此，當前4這個值有驅動代碼依據。

例如：

• 改成2：只接受 line 0、1，丟棄 line 2、3。
• 改成8：如果輸入仍只有0～3，就不會多出新的點。 它主要是輸入格式匹配參數。調節點數密度應首先看前面講過的 point_filter_num 和體素尺寸。

1.1.4.2.3.timestamp_unit（點內時間單位）

timestamp_unit：解釋逐點時間字段的單位。 本地枚舉及轉換關係為：

這個選擇在普通 PointCloud2 分支中使用。例如 Velodyne 的 time、Ouster 的 t 會經過相應換算。

但你當前的 Livox CustomMsg 路徑直接執行：curvature = offset_time / 1000000.0;也就是固定從 ns 轉成 ms。因此，當前修改 timestamp_unit 不會改變這條輸入路徑的時間解釋。

舉個純數值例子：

offset_time = 25,000,000 ns
           = 25 ms
           = 0.025 s

該點的處理時刻就是：

点云 header.stamp + 0.025 s

逐點時間參與運動預測與點雲匹配，不能在輸入轉換中隨意丟棄；時間單位換算也不會完成雷達與 IMU 的時差校準。

1.1.4.2.4.blind（近距離盲區過濾）

blind：近距離排除半徑。 當前代碼計算：

$$r^2=x^2+y^2+z^2$$

當：

$$r^2<\texttt{blind}^2$$

就丟棄該點。因此 blind=0.5 表示排除距離雷達點雲座標原點不足0.5 m的點，範圍是一個球形區域。

例如：

• 距離0.3 m的點：被這項過濾排除。
• 距離0.6 m的點：通過距離下限檢查，之後仍可能被其他條件排除。

這裏的距離是三維距離，與點的高度、機械人中心距離不同。這個過濾同時作用於機械人點和環境點，所以增大它時，也會丟失近處牆面、地面和障礙物的有效點。

實車到位後，先觀察0.5 m過濾後是否仍有明顯車體干擾、近處有效結構是否不足，再決定是否調整。當前沒有這些實測證據，先保持0.5。

1.1.5.mapping（運動狀態估計與點雲匹配）

1.1.5.1.實車上先看哪些問題

這塊先負責讓 Point-LIO 把位置、運動和地圖估計對；坡道能不能通行，隨後才由地形分析和代價地圖判斷。實車遇到問題時，先按下面的現象找參數，不必先讀噪聲公式。

當前使用的是 MID360 內部 IMU，保持 imu_en=True、use_imu_as_input=False。MCU 世界姿態不能代替機械關節角；實際 yaw 在 gimbal_yaw_joint，實際 pitch 在 gimbal_pitch_joint，兩個 odom 關節固定，MID360 隨 yaw、不隨獨立 pitch 轉動。

下面的噪聲參數主要決定“系統更信測量，還是更相信原來的運動趨勢”。先確認單位、時間與安裝關係，再討論噪聲；它們不會讓鬆動支架變穩，也不是車輛速度或坡度限制。本輪沒有實物，不據這些現象指定新的調參值。

1.1.5.2.參數表

當前採用 use_imu_as_input=False 的 output 模型，以下按這個運行分支解釋；“當前值”來自實物配置，不是本輪讀取的運行節點值。表中的 g₀ 是上方 YAML 中的重力向量，當前模長約9.81 m/s²。

噪聲參數的單位按本地運動方程及協方差公式推導；它們不都等同於傳感器手冊中的噪聲密度。

    mapping:
      imu_en: True
      extrinsic_est_en: False                           # for aggressive motion, set this variable False
      imu_time_inte: 0.005                              # = 1 / frequency of IMU
      lidar_time_inte: 0.1
      satu_acc: 4.0                                     # the saturation value of IMU's acceleration. not related to the units
      satu_gyro: 35.0                                   # the saturation value of IMU's angular velocity. not related to the units
      acc_norm: 1.0                                     # 1.0 for g as unit, 9.81 for m/s^2 as unit of the IMU's acceleration
      lidar_meas_cov: 0.01                              # 0.001
      acc_cov_output: 500.0
      gyr_cov_output: 1000.0
      b_acc_cov: 0.0001
      b_gyr_cov: 0.0001
      imu_meas_acc_cov: 0.1
      imu_meas_omg_cov: 0.1
      gyr_cov_input: 0.1                                # for IMU as input model
      acc_cov_input: 0.1                                # for IMU as input model
      plane_thr: 0.1                                    # 0.05, the threshold for plane criteria, the smaller, the flatter a plane
      match_s: 81.0
      ivox_grid_resolution: 0.5
      # 测量平均值是[0.00423894,0.71588876,0.69592434]经过公式计算得出[-0.04166067, -7.03407005, -6.83785222]
      gravity: [-0.04166067, -7.03407005, -6.83785222]        # gravity to be aligned # rpy = [0, pi/6, 0]
      gravity_init: [-0.04166067, -7.03407005, -6.83785222]   # preknown gravity in the first IMU body frame, use when imu_en is False or start from a non-stationary state
      extrinsic_T: [ -0.011, -0.02329, 0.04412 ]
      extrinsic_R: [ 1.0, 0.0, 0.0,
                     0.0, 1.0, 0.0,
                     0.0, 0.0, 1.0 ]

1.1.5.3.參數詳解

1.1.5.3.1.imu_en（啓用雷達內部IMU）

當前 True 表示使用 MID360 內部 IMU。雷達連續掃描時，IMU 幫助算法判斷傳感器正在怎樣轉動、加減速，也參與初始方向判斷。

它與 use_imu_as_input=False 不衝突：IMU仍在使用，只是算法使用它的方式不同。實車有振動時，先檢查安裝和數據，不應把“關閉 IMU”作為普通的抗振辦法。

1.1.5.3.2.extrinsic_est_en（在線估計雷達與IMU外參）

這項決定“雷達和內部 IMU 之間的位置、朝向關係，是固定使用，還是讓算法邊運行邊估計”。當前 False使用配置中的 extrinsic_T/R。

MID360 的雷達與內部 IMU 裝在同一台設備裏，整機傾斜安裝、底盤轉動和雲台 yaw 轉動都不會改變兩者之間的固定關係。整台雷達在車上如何安裝、隨哪個關節轉動，由機械人 TF描述，不能靠開啓這項替代。

1.1.5.3.3.imu_time_inte（IMU標稱時間間隔）

0.005秒對應200 Hz的標稱週期，但當前源碼只讀入這個值，沒有用它計算。改成更小的數不會讓實物 IMU輸出得更快；實際數據間隔要看消息時間戳。當前暫不調整。

1.1.5.3.4.lidar_time_inte（異常點雲的回退時間窗口）

它主要給失去點雲、沒有有效點等回退分支提供時間窗口。正常掃描的結束時間由每個點的時間計算，不是由這個0.1秒強制規定。

因此不能只看到驅動發佈20 Hz，就按1/20把它改成0.05。真實頻率、逐點時間和回退窗口是不同事項，當前先保留。

1.1.5.3.5.satu_acc（加速度讀數飽和門檻）

可以把它理解為“加速度讀數快到量程邊緣時，算法先別完全相信這一軸”。當前代碼逐軸比較原始讀數，達到 0.99×4.0=3.96 g 就屏蔽該軸的這次 IMU更新。

上實物時，它與顛簸、急加減速和振動有關，但4.0不代表已經確認硬件量程就是4 g，更不是車輛允許的最大加速度。要先核對型號／實際量程設置、輸入單位和正常運動峯值。閾值太低可能把正常顛簸數據屏蔽，太高可能繼續接受已限幅的讀數。

原註釋“與單位無關”不成立。當前輸入是 g；如果換成 m/s²仍照用4.0，正常靜止時某軸讀數就可能超過門檻。單位改變時必須一起復核這項。

1.1.5.3.6.satu_gyro（角速度讀數飽和門檻）

這是角速度的同類保護，當前實際門檻為 0.99×35.0=34.65 rad/s，約1985°/s、5.51圈/秒。比較的是各軸 IMU讀數，不是下發給底盤的自旋速度。

快速轉底盤或雲台時，應看真實峯值是否接近設備量程、是否出現限幅；當前 check_satu=True 才執行這項保護。調大它不會使車輛允許轉得更快，也不能直接解決航向漂移。

1.1.5.3.7.acc_norm（加速度單位換算基準）

這項最重要的作用是單位換算。當前本地驅動把 MID360 的加速度以 g發佈，Point-LIO內部按 m/s²計算，所以 acc_norm=1.0 把原始1 g換成約9.81 m/s²。Livox 驅動的數據單位定義

實物靜止時，三個軸合起來的大小應接近1 g。雷達傾斜安裝後，重力會分到多個軸上，不要求某一個軸必須等於1，也不要求另外兩個軸都為0。這裏的 acc_norm 與 gravity 長度約9.81並不衝突：一個對應原始讀數單位，一個對應內部米制計算。

若仍是 g輸入卻把它改成9.81，原始1 g會被換成約1 m/s²，比例就錯了近10倍。當前保持1.0；這項不能用來“壓住漂移”或消除振動。換驅動、做了單位轉換或換傳感器後，才按實際輸入重新確認。

1.1.5.3.8.lidar_meas_cov（激光匹配對位姿的影響）

這項影響“激光匹配結果對位姿有多大作用”。但本地代碼的兩個求解分支方向不一致，不能簡單記成“調大就少信雷達”。

當前一次更新有效匹配少於30個點時，調大通常降低激光影響；達到30個點時，另一條公式的影響方向相反。這裏統計的是一次逐點／時間組更新，不是整幀點數。源碼不一致已確認，實際觸發和影響尚未測試，當前保留0.01。

下面的 IMU噪聲可以先用兩句話理解：測量噪聲管“有多相信這次讀數”；運動狀態的過程噪聲管“允許原來的運動估計多快改變”。它們作用不同，不應因為名字都有 cov 就一起加大。表中單位按本地公式推導，也不能直接與廠家噪聲密度或原始 g讀數比較。

1.1.5.3.9.acc_cov_output（加速度估計的變化不確定性）

它管“估計中的加速度是否容易改變”。車急加速、剎車或顛簸時，調大通常允許測量更快修正原來的估計，調小更傾向維持原來的趨勢；如果讀數主要是振動，允許變化過快也可能讓估計跟着抖。

當前500是程序內部的不確定程度係數，不是給車添加500的加速度，也不是已測得的傳感器噪聲。應與 imu_meas_acc_cov 一起理解，先排除單位、時間和安裝錯誤。

1.1.5.3.10.gyr_cov_output（轉速估計的變化不確定性）

它與上一項類似，管“估計中的轉速是否容易改變”。突然轉底盤或雲台時，調大通常允許更快跟隨觀測，調小則更相信原有轉動趨勢。

當前1000不是車輛轉速上限，也不是陀螺實際噪聲的測量結果。需要與 imu_meas_omg_cov、角速度峯值和時間接口一起檢查，不能僅憑轉動時重影就認定該項要增大。

1.1.5.3.11.b_acc_cov（加速度零偏變化的噪聲）

“零偏”就是傳感器讀數中持續存在的一點偏差。它允許算法估計的加速度零偏多快變化：調大變化更容易，調小更穩定但適應漂移較慢。過大時可能把真實運動解釋成偏差，當前先保持0.0001。

1.1.5.3.12.b_gyr_cov（陀螺零偏變化的噪聲）

它管陀螺零偏估計的變化速度。實物靜止時角速度若持續偏離零，需要先核對真實數據和偏差來源；這項不保證調大就能消除航向漂移。當前先保持0.0001。

1.1.5.3.13.imu_meas_acc_cov（加速度讀數可信度）

它管“有多相信這次加速度讀數”。調大表示更不相信讀數，減少其修正作用；調小更相信讀數，也更容易受振動影響。

當前0.1是算法採用的噪聲假設，不是已經量出的傳感器精度；它作用於換算後的 m/s²數據。與 acc_cov_output 配合時，要分清“讀數可信不可信”和“估計能不能跟上真實變化”這兩件事。

1.1.5.3.14.imu_meas_omg_cov（轉速讀數可信度）

它管“有多相信這次轉速讀數”。調大減少角速度測量的影響，調小更相信它。實物轉動或振動時，應同時觀察讀數是否乾淨、運動估計是否跟得上，再結合 gyr_cov_output 判斷。當前0.1也不是廠家的精度承諾。

1.1.5.3.15.gyr_cov_input（IMU預測模型的轉速噪聲）

當前 use_imu_as_input=False，這項不參與當前模型的預測。改它不能改善本輪轉動表現，暫不調整。換到另一種模型後才重新解釋和核對。

1.1.5.3.16.acc_cov_input（IMU預測模型的加速度噪聲）

同樣屬於另一種模型，當前不參與預測。不要因為加速度有抖動就先改這項；當前應檢查實際生效的加速度噪聲項、單位和安裝。

1.1.5.3.17.plane_thr（地圖鄰點的平整度門檻）

它判斷“附近這5個地圖點，能不能算作一張比較平整的面”。當前0.1 m表示每個鄰點偏離擬合面不應超過10 cm。

斜坡也可以是一張平整的斜面。 源碼沒有用這項限制面的傾斜角，所以它不是最大坡度，也不是“高出地面10 cm就禁止通行”的障礙門檻；後者屬於地形與代價地圖的判斷。

實車牆角、曲面、雜亂點或稀疏點附近匹配不足時可檢查它。調大接受更不平整的點羣，也可能接受錯誤的面；調小更嚴格，可用匹配可能減少。

1.1.5.3.18.match_s（新點與地圖平面的匹配門檻）

在地圖鄰點已經組成一個面後，它再判斷“當前雷達點離這個面是不是太遠”。地圖牆面重影、錯誤匹配或有效匹配很少時，才結合這項分析。

當前81越大，條件越嚴格。代碼允許的距離誤差還隨點到雷達的距離變化，例如1 m處約11.1 cm、4 m處約22.2 cm；這些是匹配門檻，不是定位精度或車必須離牆多遠。

plane_thr 檢查地圖鄰點自己是否平整；match_s 檢查新點能否對上這個面。兩項不能只因為地圖不好看就一起放寬，時間和外參錯誤也會造成類似現象。

1.1.5.3.19.ivox_grid_resolution（地圖搜索網格大小）

它把地圖分塊，方便查找附近的匹配點。當前網格邊長0.5 m，與鄰格搜索方式一起決定每次查找的覆蓋範圍。

調大能覆蓋更大範圍，但可能查找更多點；調小範圍變小，可能找不到足夠的鄰點。它不會直接把小於0.5 m的障礙刪掉，也不是當前掃描的降採樣尺寸，先結合匹配點數量與計算負擔觀察。

1.1.5.3.20.gravity與gravity_init（目標參考系重力與初始化重力）

gravity 最重要的實物含義是：Point-LIO建立自己的參考系時，應該把哪邊理解為“向下”，以及重力的大小按多少計算。 它會影響初始姿態和加速度換算；方向弄錯時，地面、高低和後續坡道識別都可能受到影響，不能先靠放寬地形高度範圍掩蓋。

當前向量長度已經約9.81 m/s²，不需要再改大小。雷達整體傾斜時，IMU自己的幾個軸上會同時看到重力分量；車上坡後各軸分量也會改變。這不意味着要讓配置中的 gravity 每次跟着讀數改，它描述的是所選目標參考系的關係。

需要分清兩個東西：

• 初始 IMU自己看到的重力方向： 當前 imu_en=True，代碼從一段初始加速度讀數的平均值歸一化、取反，並按9.81換算得到。它仍是 IMU自身座標中的方向。
• Point-LIO參考系中希望採用的重力方向： 由 mapping.gravity 給出。代碼會計算兩者之間的初始轉動關係，後續接口還要把結果正確接到導航使用的 odom。

所以，靜止讀數取反能告訴我們“重力在初始IMU系中指向哪裏”，不能未經座標核對就直接等同於 mapping.gravity。同樣，也不能看到導航通常讓 z朝上，就擅自改成[0,0,-9.81]；還需核對 Point-LIO參考系、整機安裝和後續接口怎樣連接。本輪保留方向與數值，待核實。

gravity_init 則是在不使用 IMU時提供的初始重力預設。當前參數雖然被讀入，但初始化採用真實 IMU平均加速度，gravity_init 不參與這條分支；改它不能修正當前系統的上下方向。

下面保留原日誌的安裝理論和測量記錄作為線索。本輪沒有執行其中的命令，也沒有新增實物測量；這些記錄所用的設備、安裝方向和驅動配置仍需核對。

當前模型把整台 MID360的安裝 roll設為-π/4，即-45°；模型設置是否與舊實物記錄的實際軸向一致，還需要核對。

方法一：在座標軸約定一致、初始車體水平、只存在上述 roll時的理論計算

機械人靜止時，IMU 的 linear_acceleration 本質上測的是與重力反向的“比力”。如果只繞 X 軸 roll 一個角度 r，那麼靜止時大致有：

ax = 0
ay = g·sin(r)
az = g·cos(r)

g = 9.81 m/s²
r = -45°


ax = 0

ay = 9.81 × sin(-45°)
   ≈ -6.9367

az = 9.81 × cos(-45°)
   ≈ +6.9367

仿真

方法二：測量法

仿真環境下imu測量：

ros2 topic echo /red_standard_robot1/livox/imu --once

linear_acceleration:
  x: 1.877548187489191e-05
  y: -6.929646440459081
  z: 6.929646470797371

這組舊仿真記錄的 Y負、Z正，與上述理論條件下的計算一致；它沒有驗證實物安裝，也不能單獨證明導航參考系的上下方向正確。

然後取反得到初始 IMU自身座標中的重力。下面舊代碼片段的 gravity標籤應按這個含義理解，不是要求直接填到 mapping.gravity：

gravity
≈ [0, +6.93, -6.93]

靜止測量有助於核對 IMU實際軸向，但測得的初始 IMU方向仍要與目標參考系區別開；不能只用測量結果代替整條座標關係檢查。

舊實物記錄（安裝與數據來源待核對）

方法二：測量法

source install/setup.bash
ros2 launch livox_ros_driver2 msg_MID360_launch.py

ros2 topic echo /livox/imu --once

平均 linear_acceleration ≈
x = 0.00424
y = 0.71589
z = 0.69592

這組舊記錄的 Y為正，而上面-45°理論和仿真記錄的 Y為負。atan2(0.716, 0.696) ≈ 45.8° 不能只解釋為0.8°的小加工誤差：還存在方向符號差異。應先核對當時實物安裝、座標軸、啓動配置和驅動有無額外旋轉，再判斷記錄是否能直接比較；本輪沒有實物證據解釋其來源。

下面是Point-LIO代碼：

tmp_gravity =
    -mean_acc / mean_acc.norm() * G_m_s2;

就是三步：先求平均加速度 → 求這個向量的模 → 歸一化後取反。

先求平均加速度mean_acc = [0.00423894,0.71588876,0.69592434] 再求這個mean_acc的模，mean_acc.norm() =sqrt(0.00423894²+ 0.71588876²+ 0.69592434²) ≈ 0.99841142 ≈ 1.0 原始三軸合起來約1 g；代碼內部使用 G_m_s2≈9.81 m/s²進行換算。這裏的1與9.81對應不同單位。 所以代入公式得[0.04166067, 7.03407005, 6.83785222]，公式裏有個負號，取反後得出[-0.04166067, -7.03407005, -6.83785222]

這得到的是舊記錄對應的初始 IMU自身重力向量。當前 YAML確實採用了同一組數，但是否符合選定的 Point-LIO／導航參考系仍待核實；長度約9.81正確，不意味着方向關係也已經通過驗證。本輪不改方向。

源碼補充：初始兩重力方向恰好完全反向時，Set_init 的一個分支返回 -Eye3d，該矩陣不是有效的三維旋轉矩陣。這個極端分支需單獨核實；本輪沒有證明現有配置觸發它，也沒有修改源碼。

1.1.5.3.21.extrinsic_T（雷達與內部IMU的位置關係）

它描述雷達原點相對 MID360 內部 IMU原點的位置，當前按代碼使用 p_I=R_IL·p_L+t_IL。[-0.011,-0.02329,+0.04412] m 換成毫米就是[-11,-23.29,+44.12] mm。

這組內部關係與雷達裝在底盤／雲台上的位置是兩回事。換支架、把整機傾斜或轉動雲台，不會讓內部兩個原點之間的距離變化；整機安裝位置應在機械人 TF中描述。值或方向弄錯，轉動時可能出現位置偏差和重影，但同樣現象也可能來自時間或後續接口。

實物需要核對設備型號、點雲與IMU原點的定義，以及數據進入 Point-LIO前有沒有額外變換；不能把雲台到雷達的安裝距離填到這裏。現有數值保留，不作為本輪新標定結論。

1.1.5.3.22.extrinsic_R（雷達與內部IMU的軸向關係）

它描述雷達座標軸與內部 IMU座標軸怎樣對應。當前單位矩陣表示兩路輸入按軸向一致使用。整台 MID360斜裝時，雷達與內部IMU一起傾斜，不能因此再把整機的-45°安裝角填進這項。

還要核對驅動是否已經旋轉了數據：本地驅動有對IMU進行旋轉的代碼，當前所讀 MID360配置的旋轉三項均為0。後續換配置時，要檢查點雲和IMU是否仍按預期對應，不能只看它們的消息名稱。

整機安裝角屬於機械人 TF，內部軸向對應屬於 extrinsic_R。旋轉矩陣要由明確的座標關係給出，不能逐個數字隨意調大或調小。

1.1.5.3.23.vel_cov（速度估計的變化不確定性）

這項在當前 YAML中沒寫，源碼默認20，當前 output模型使用它描述速度狀態的不確定程度。它不是車輛最高速度：調大允許估計速度更容易被觀測改變，調小更傾向維持原來的估計。先保持默認值；加減速反應異常也應先排除單位、時間和位姿轉換問題。

1.1.5.4.本塊實物核對與驗證範圍

實物到位、另行獲准檢查後，這塊優先核對：靜止三軸加速度合起來是否約1 g、轉速讀數是否接近零且單位正確；點雲和IMU是否按預期軸向發佈；整機安裝與內部外參有沒有混填；車體／雲台轉動或顛簸時，讀數是否出現限幅、位姿和牆面是否跟得上。

當前不能把“地面斜了、牆面重影、靜止漂移”等任何單一現象直接當成某個參數必須修改的證據。原始單位、時間、安裝和後續 IMU／雷達位姿接口都會影響表現，其中源碼缺口需要單獨處理，不能通過盲調噪聲保證解決。

• 源碼能確認： 當前 IMU輸入單位、加速度換算、飽和門檻、各參數的生效分支、內部外參用途和已記錄的求解／初始化疑點。容器中只核對了安裝包與當前實物 YAML的來源，沒有讀取運行節點參數。
• 仿真已觀察： 2026-10-08 的靜止和約20 cm往返有 Point-LIO位姿與配準點雲輸出；未驗證坡道、長距離、快速轉動、獨立雲台 yaw或實際振動。本塊舊命令與讀數作為歷史記錄保留，不擴大本輪驗證範圍。
• 必須實物核實： 真實安裝、軸向與原點定義、量程設置、靜止讀數和運動峯值。重力大小已約9.81，方向與目標參考系尚待核對；沒有新增測量，也沒有改動源碼、配置或參數值。

1.1.6.odometry（里程計發佈設置）

1.1.6.1.參數表

    odometry:
      publish_odometry_without_downsample: True

1.1.6.2.參數詳解

1.1.6.2.1.publish_odometry_without_downsample（里程計發佈節拍）

這個開關控制里程計輸出的節拍：

• True：每次成功的逐點／時間組匹配後發佈，時間戳為當前採樣時刻，輸出更密集，實際頻率取決於成功更新次數。
• False：每幀處理結束後發佈一次，時間戳為掃描末尾；輸入20 Hz且每幀正常處理時，通常約20 Hz輸出。

當前先保持 True。後續若輸出和接收負載過高，再考慮 False；改變的是輸出節拍，逐點估計與點雲降採樣仍然執行。

1.1.7.publish（軌跡、點雲與TF輸出開關）

1.1.7.1.參數表

    publish:
      path_en: False                                    # False: close the path output
      scan_publish_en: True                             # False: close all the point cloud output
      scan_bodyframe_pub_en: False                      # True: output the point cloud scans in IMU-body-frame
      tf_send_en: False                                  # True: send transform from 'camera_init' to 'aft_mapped'

1.1.7.2.參數詳解

1.1.7.2.1.path_en（里程計軌跡發佈）

True 時發佈 path 累計軌跡，方便在 RViz 中觀察機械人傳感器的運動路線；False 時關閉該軌跡輸出，里程計仍照常發佈。

觀察 SLAM 軌跡時可以按需開啓。軌跡沒有固定長度上限，長時間累計會增加內存和顯示負擔。

1.1.7.2.2.scan_publish_en（參考系中的配準點雲發佈）

當前保持 True，發佈 cloud_registered 配準掃描，供 RViz 和後續接口使用。

False 時關閉當前配準掃描及依賴它的 body 點雲輸出，內部估計和建圖仍繼續。原註釋“關閉全部點雲輸出”不完全準確：初始化的 Laser_map 仍可發佈。另外，本地 PCD 保存所需的點雲累計也放在這個分支內，後續啓用保存時應保持 True。

1.1.7.2.3.scan_bodyframe_pub_en（內部IMU自身系點雲發佈）

True 時額外發布 cloud_registered_body，將預處理點雲通過內部外參轉換到 IMU 座標系，主要用於輔助檢查點雲和外參；需要 scan_publish_en 同時為 True。

這裏的 body 指 MID360 內部 IMU 座標系。當前沒有這項調試需求，先保持 False。

1.1.7.2.4.tf_send_en（Point-LIO位姿TF發佈）

True 時隨里程計發佈 camera_init → aft_mapped 的 TF；False 時關閉這條 TF，里程計和配準掃描仍可繼續輸出。

當前集成鏈先保持 False，底盤 TF 由其他接口節點發布。單獨調試需要顯示 Point-LIO 自身位姿 TF 時，再考慮開啓；這條 TF 表達內部 IMU 位姿，不是 odom → base_footprint。

1.1.8.pcd_save（點雲地圖保存）

1.1.8.1.參數表

    pcd_save:
      pcd_save_en: False
      interval: -1                                      # how many LiDAR frames saved in each pcd file;
                                                        # -1 : all frames will be saved in ONE pcd file, may lead to memory crash when having too much frames.

1.1.8.2.參數詳解

1.1.8.2.1.pcd_save_en（點雲地圖保存開關）

False 時關閉 PCD 保存，估計和建圖仍繼續；True 時累計 camera_init 座標系中的配準掃描，保存為二進制 PCD。

當前先保持 False，需要存檔時再開啓，並保持 publish.scan_publish_en=True。注意：slam_launch.py 會把該參數覆蓋為 True，所以是否保存還要看實際啓動方式。

1.1.8.2.2.interval（點雲保存的幀數間隔）

• -1：持續累計點雲，正常退出時保存到 scans.pcd。長時間運行會增加內存佔用，需要正常結束程序才能執行退出保存。
• 大於0：每累計指定次數的掃描處理就保存為 scans_1.pcd、scans_2.pcd 等，並清空累計緩存；正常退出時，剩餘點仍保存到 scans.pcd。

這個計數按進入保存分支的掃描處理次數增加。例如穩定20 Hz且每幀正常處理時，100次約為5秒，但 interval 本身不是時間參數。本地僅在 interval>0 時分批保存，0及其他負值也不會觸發週期保存，使用默認特殊值 -1 更清楚。

保存目錄由編譯時ROOT_DIR確定：CMake使用當時的Point-LIO源碼目錄，保存代碼再拼接PCD/。當前安裝程序的輸出目錄為：

/src/pb2025_sentry_nav/point_lio/PCD/

這不是相對於運行目錄的路徑；在其他目錄重新編譯後需重新核對安裝產物。

保存內容是多幀配準掃描的累計點雲；文件不會自動放到導航先驗地圖目錄。輸出文件名固定，需保留不同輪次結果時，應先歸檔已有文件。