Point_LIO（激光与IMU融合里程计，建图与定位算法）

Point_LIO（激光与IMU融合里程计，建图与定位算法）

参数

ros__parameters（ROS节点参数）

参数表

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

参数详解

use_imu_as_input（IMU参与运动估计的方式）

use_imu_as_input 决定 IMU 怎样参与滤波。

当前 False 的处理方式是：滤波器内部估计角速度、加速度等状态，收到 IMU 后用测量纠正这些状态，再进行运动预测和激光匹配。

设为 True 后，加速度与角速度作为预测输入，经过单位缩放、偏置处理后推动运动状态。切换模型还会改变所使用的噪声参数，所以首轮保持当前 False。

关闭 IMU 对应的是另一个参数 mapping.imu_en。

prop_at_freq_of_imu（按IMU时刻更新估计不确定性）

prop_at_freq_of_imu 主要影响协方差传播。

协方差可以理解为滤波器对当前估计的不确定程度。当前 True 时，代码在处理 IMU 时刻传播它；状态仍会推进到激光点的采样时刻。

改成 False 后，代码还会在激光逐点时刻增加协方差预测，通常计算量更大。它不是“把 Point-LIO 输出频率设成 IMU 频率”的开关，也不能保证切换后精度更高。

check_satu（IMU读数量程保护）

check_satu 是逐轴处理。

当前代码分别比较三个加速度轴和三个角速度轴：abs(原始轴值) ≥ 0.99 × 对应饱和阈值

被标记的轴停止参与这次 IMU 观测更新，其他轴继续使用。因此，它需要配合正确单位和量程的 satu_acc/satu_gyro。这项检查在当前 output 模型中生效。

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 个。
• 随后扫描体素降采样，还会进一步压缩空间上密集的点。

因此，间隔取点更多，不一定等比例增加最终匹配点数，还要看扫描体素、遮挡和匹配条件。

ivox_nearby_type（地图邻格搜索范围）

ivox_nearby_type=18 表示邻接网格类型。

本地实现实际搜索的网格数包含中心网格：

它不表示“找 18 个点”或“搜索 18 米”。

实际空间尺度还取决于 mapping.ivox_grid_resolution，当前为 0.5 m；匹配过程中另有最近邻和残差条件。

以后调这组参数时，主要比较输出延迟、有效环境结构、点云重影和轨迹连续性。先使用当前值建立基准，再针对具体表现改变一个参数，才能判断改善来自哪里。

common（输入话题与点云时间处理）

参数表

    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)

参数详解

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 表示坐标名称；选择话题不会自动进行坐标变换。

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。

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 参数不会实现注释描述的拆帧行为。

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 填成非零值，不会完成校正。

prior_pcd（先验点云地图）

参数表

    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 ]

参数详解

enable（先验地图加载开关）

enable 改变的是初始匹配地图的来源。

当前 False 的流程是：

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

开启后，达到同样的初始化门槛时，代码改为读取 PCD，并把其中的点加入地图搜索结构。

加载先验地图不会自动完成全局重定位，也不会自动确定机器人在整张地图中的位置。初始位置、姿态和地图坐标必须足够一致，后续局部匹配才有基础。

另外，开启先验后，代码会延后新点插入；相关有效处理计数超过200后，仍会继续更新地图。因此这个开关也不意味着地图永久冻结。

先验 PCD 加载时没有自动做坐标旋转或单位缩放，地图还必须与当前 Point-LIO 参考系一致。源码对 PCD 读取失败返回的空指针也没有在调用处检查，所以启用前需要确认文件有效。

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 没有使用地图。

init_pose（内部IMU的初始位置）

init_pose 有三个容易误解的地方：

• 它是 x,y,z，三个数都是米。 第三个数不是 yaw，也不包含 roll/pitch/yaw。
• 它对应内部 IMU 原点在 camera_init 中的位置。 不是直接设置 base_footprint 的导航位置。
• 本地代码的赋值时机比较特殊。 它在首次输出位姿时直接修改 kf_output.x_.pos，并非在加载先验、开始匹配之前统一完成初始化。

尤其在你当前“高频里程计开启、path 关闭”的设置下，这次赋值发生在首次成功匹配后的里程计输出中。因此，未来启用先验时，不能只填一个位置就认为首轮匹配已经从该位置开始；这段初始化顺序需要另行审查。

preprocess（雷达格式与点云预处理）

参数表

    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 处理。

参数详解

lidar_type（雷达类型与输入格式）

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

它不仅决定字段怎样解析，还决定订阅 CustomMsg 还是 PointCloud2。当前实车驱动在 livox/lidar 发布 CustomMsg，所以设置为 1。

仿真使用 2，是因为转换器生成了 Velodyne 样式字段，并不代表仿真中换了一台 Velodyne 雷达。

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 和体素尺寸。

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 的时差校准。

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。

mapping（运动状态估计与点云匹配）

实车上先看哪些问题

这块先负责让 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 转动。

下面的噪声参数主要决定“系统更信测量，还是更相信原来的运动趋势”。先确认单位、时间与安装关系，再讨论噪声；它们不会让松动支架变稳，也不是车辆速度或坡度限制。本轮没有实物，不据这些现象指定新的调参值。

参数表

当前采用 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 ]

参数详解

imu_en（启用雷达内部IMU）

当前 True 表示使用 MID360 内部 IMU。雷达连续扫描时，IMU 帮助算法判断传感器正在怎样转动、加减速，也参与初始方向判断。

它与 use_imu_as_input=False 不冲突：IMU仍在使用，只是算法使用它的方式不同。实车有振动时，先检查安装和数据，不应把“关闭 IMU”作为普通的抗振办法。

extrinsic_est_en（在线估计雷达与IMU外参）

这项决定“雷达和内部 IMU 之间的位置、朝向关系，是固定使用，还是让算法边运行边估计”。当前 False使用配置中的 extrinsic_T/R。

MID360 的雷达与内部 IMU 装在同一台设备里，整机倾斜安装、底盘转动和云台 yaw 转动都不会改变两者之间的固定关系。整台雷达在车上如何安装、随哪个关节转动，由机器人 TF描述，不能靠开启这项替代。

imu_time_inte（IMU标称时间间隔）

0.005秒对应200 Hz的标称周期，但当前源码只读入这个值，没有用它计算。改成更小的数不会让实物 IMU输出得更快；实际数据间隔要看消息时间戳。当前暂不调整。

lidar_time_inte（异常点云的回退时间窗口）

它主要给失去点云、没有有效点等回退分支提供时间窗口。正常扫描的结束时间由每个点的时间计算，不是由这个0.1秒强制规定。

因此不能只看到驱动发布20 Hz，就按1/20把它改成0.05。真实频率、逐点时间和回退窗口是不同事项，当前先保留。

satu_acc（加速度读数饱和门槛）

可以把它理解为“加速度读数快到量程边缘时，算法先别完全相信这一轴”。当前代码逐轴比较原始读数，达到 0.99×4.0=3.96 g 就屏蔽该轴的这次 IMU更新。

上实物时，它与颠簸、急加减速和振动有关，但4.0不代表已经确认硬件量程就是4 g，更不是车辆允许的最大加速度。要先核对型号／实际量程设置、输入单位和正常运动峰值。阈值太低可能把正常颠簸数据屏蔽，太高可能继续接受已限幅的读数。

原注释“与单位无关”不成立。当前输入是 g；如果换成 m/s²仍照用4.0，正常静止时某轴读数就可能超过门槛。单位改变时必须一起复核这项。

satu_gyro（角速度读数饱和门槛）

这是角速度的同类保护，当前实际门槛为 0.99×35.0=34.65 rad/s，约1985°/s、5.51圈/秒。比较的是各轴 IMU读数，不是下发给底盘的自旋速度。

快速转底盘或云台时，应看真实峰值是否接近设备量程、是否出现限幅；当前 check_satu=True 才执行这项保护。调大它不会使车辆允许转得更快，也不能直接解决航向漂移。

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；这项不能用来“压住漂移”或消除振动。换驱动、做了单位转换或换传感器后，才按实际输入重新确认。

lidar_meas_cov（激光匹配对位姿的影响）

这项影响“激光匹配结果对位姿有多大作用”。但本地代码的两个求解分支方向不一致，不能简单记成“调大就少信雷达”。

当前一次更新有效匹配少于30个点时，调大通常降低激光影响；达到30个点时，另一条公式的影响方向相反。这里统计的是一次逐点／时间组更新，不是整帧点数。源码不一致已确认，实际触发和影响尚未测试，当前保留0.01。

下面的 IMU噪声可以先用两句话理解：测量噪声管“有多相信这次读数”；运动状态的过程噪声管“允许原来的运动估计多快改变”。它们作用不同，不应因为名字都有 cov 就一起加大。表中单位按本地公式推导，也不能直接与厂家噪声密度或原始 g读数比较。

acc_cov_output（加速度估计的变化不确定性）

它管“估计中的加速度是否容易改变”。车急加速、刹车或颠簸时，调大通常允许测量更快修正原来的估计，调小更倾向维持原来的趋势；如果读数主要是振动，允许变化过快也可能让估计跟着抖。

当前500是程序内部的不确定程度系数，不是给车添加500的加速度，也不是已测得的传感器噪声。应与 imu_meas_acc_cov 一起理解，先排除单位、时间和安装错误。

gyr_cov_output（转速估计的变化不确定性）

它与上一项类似，管“估计中的转速是否容易改变”。突然转底盘或云台时，调大通常允许更快跟随观测，调小则更相信原有转动趋势。

当前1000不是车辆转速上限，也不是陀螺实际噪声的测量结果。需要与 imu_meas_omg_cov、角速度峰值和时间接口一起检查，不能仅凭转动时重影就认定该项要增大。

b_acc_cov（加速度零偏变化的噪声）

“零偏”就是传感器读数中持续存在的一点偏差。它允许算法估计的加速度零偏多快变化：调大变化更容易，调小更稳定但适应漂移较慢。过大时可能把真实运动解释成偏差，当前先保持0.0001。

b_gyr_cov（陀螺零偏变化的噪声）

它管陀螺零偏估计的变化速度。实物静止时角速度若持续偏离零，需要先核对真实数据和偏差来源；这项不保证调大就能消除航向漂移。当前先保持0.0001。

imu_meas_acc_cov（加速度读数可信度）

它管“有多相信这次加速度读数”。调大表示更不相信读数，减少其修正作用；调小更相信读数，也更容易受振动影响。

当前0.1是算法采用的噪声假设，不是已经量出的传感器精度；它作用于换算后的 m/s²数据。与 acc_cov_output 配合时，要分清“读数可信不可信”和“估计能不能跟上真实变化”这两件事。

imu_meas_omg_cov（转速读数可信度）

它管“有多相信这次转速读数”。调大减少角速度测量的影响，调小更相信它。实物转动或振动时，应同时观察读数是否干净、运动估计是否跟得上，再结合 gyr_cov_output 判断。当前0.1也不是厂家的精度承诺。

gyr_cov_input（IMU预测模型的转速噪声）

当前 use_imu_as_input=False，这项不参与当前模型的预测。改它不能改善本轮转动表现，暂不调整。换到另一种模型后才重新解释和核对。

acc_cov_input（IMU预测模型的加速度噪声）

同样属于另一种模型，当前不参与预测。不要因为加速度有抖动就先改这项；当前应检查实际生效的加速度噪声项、单位和安装。

plane_thr（地图邻点的平整度门槛）

它判断“附近这5个地图点，能不能算作一张比较平整的面”。当前0.1 m表示每个邻点偏离拟合面不应超过10 cm。

斜坡也可以是一张平整的斜面。 源码没有用这项限制面的倾斜角，所以它不是最大坡度，也不是“高出地面10 cm就禁止通行”的障碍门槛；后者属于地形与代价地图的判断。

实车墙角、曲面、杂乱点或稀疏点附近匹配不足时可检查它。调大接受更不平整的点群，也可能接受错误的面；调小更严格，可用匹配可能减少。

match_s（新点与地图平面的匹配门槛）

在地图邻点已经组成一个面后，它再判断“当前雷达点离这个面是不是太远”。地图墙面重影、错误匹配或有效匹配很少时，才结合这项分析。

当前81越大，条件越严格。代码允许的距离误差还随点到雷达的距离变化，例如1 m处约11.1 cm、4 m处约22.2 cm；这些是匹配门槛，不是定位精度或车必须离墙多远。

plane_thr 检查地图邻点自己是否平整；match_s 检查新点能否对上这个面。两项不能只因为地图不好看就一起放宽，时间和外参错误也会造成类似现象。

ivox_grid_resolution（地图搜索网格大小）

它把地图分块，方便查找附近的匹配点。当前网格边长0.5 m，与邻格搜索方式一起决定每次查找的覆盖范围。

调大能覆盖更大范围，但可能查找更多点；调小范围变小，可能找不到足够的邻点。它不会直接把小于0.5 m的障碍删掉，也不是当前扫描的降采样尺寸，先结合匹配点数量与计算负担观察。

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，该矩阵不是有效的三维旋转矩阵。这个极端分支需单独核实；本轮没有证明现有配置触发它，也没有修改源码。

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前有没有额外变换；不能把云台到雷达的安装距离填到这里。现有数值保留，不作为本轮新标定结论。

extrinsic_R（雷达与内部IMU的轴向关系）

它描述雷达坐标轴与内部 IMU坐标轴怎样对应。当前单位矩阵表示两路输入按轴向一致使用。整台 MID360斜装时，雷达与内部IMU一起倾斜，不能因此再把整机的-45°安装角填进这项。

还要核对驱动是否已经旋转了数据：本地驱动有对IMU进行旋转的代码，当前所读 MID360配置的旋转三项均为0。后续换配置时，要检查点云和IMU是否仍按预期对应，不能只看它们的消息名称。

整机安装角属于机器人 TF，内部轴向对应属于 extrinsic_R。旋转矩阵要由明确的坐标关系给出，不能逐个数字随意调大或调小。

vel_cov（速度估计的变化不确定性）

这项在当前 YAML中没写，源码默认20，当前 output模型使用它描述速度状态的不确定程度。它不是车辆最高速度：调大允许估计速度更容易被观测改变，调小更倾向维持原来的估计。先保持默认值；加减速反应异常也应先排除单位、时间和位姿转换问题。

本块实物核对与验证范围

实物到位、另行获准检查后，这块优先核对：静止三轴加速度合起来是否约1 g、转速读数是否接近零且单位正确；点云和IMU是否按预期轴向发布；整机安装与内部外参有没有混填；车体／云台转动或颠簸时，读数是否出现限幅、位姿和墙面是否跟得上。

当前不能把“地面斜了、墙面重影、静止漂移”等任何单一现象直接当成某个参数必须修改的证据。原始单位、时间、安装和后续 IMU／雷达位姿接口都会影响表现，其中源码缺口需要单独处理，不能通过盲调噪声保证解决。

• 源码能确认： 当前 IMU输入单位、加速度换算、饱和门槛、各参数的生效分支、内部外参用途和已记录的求解／初始化疑点。容器中只核对了安装包与当前实物 YAML的来源，没有读取运行节点参数。
• 仿真已观察： 2026-10-08 的静止和约20 cm往返有 Point-LIO位姿与配准点云输出；未验证坡道、长距离、快速转动、独立云台 yaw或实际振动。本块旧命令与读数作为历史记录保留，不扩大本轮验证范围。
• 必须实物核实： 真实安装、轴向与原点定义、量程设置、静止读数和运动峰值。重力大小已约9.81，方向与目标参考系尚待核对；没有新增测量，也没有改动源码、配置或参数值。

odometry（里程计发布设置）

参数表

    odometry:
      publish_odometry_without_downsample: True

参数详解

publish_odometry_without_downsample（里程计发布节拍）

这个开关控制里程计输出的节拍：

• True：每次成功的逐点／时间组匹配后发布，时间戳为当前采样时刻，输出更密集，实际频率取决于成功更新次数。
• False：每帧处理结束后发布一次，时间戳为扫描末尾；输入20 Hz且每帧正常处理时，通常约20 Hz输出。

当前先保持 True。后续若输出和接收负载过高，再考虑 False；改变的是输出节拍，逐点估计与点云降采样仍然执行。

publish（轨迹、点云与TF输出开关）

参数表

    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'

参数详解

path_en（里程计轨迹发布）

True 时发布 path 累计轨迹，方便在 RViz 中观察机器人传感器的运动路线；False 时关闭该轨迹输出，里程计仍照常发布。

观察 SLAM 轨迹时可以按需开启。轨迹没有固定长度上限，长时间累计会增加内存和显示负担。

scan_publish_en（参考系中的配准点云发布）

当前保持 True，发布 cloud_registered 配准扫描，供 RViz 和后续接口使用。

False 时关闭当前配准扫描及依赖它的 body 点云输出，内部估计和建图仍继续。原注释“关闭全部点云输出”不完全准确：初始化的 Laser_map 仍可发布。另外，本地 PCD 保存所需的点云累计也放在这个分支内，后续启用保存时应保持 True。

scan_bodyframe_pub_en（内部IMU自身系点云发布）

True 时额外发布 cloud_registered_body，将预处理点云通过内部外参转换到 IMU 坐标系，主要用于辅助检查点云和外参；需要 scan_publish_en 同时为 True。

这里的 body 指 MID360 内部 IMU 坐标系。当前没有这项调试需求，先保持 False。

tf_send_en（Point-LIO位姿TF发布）

True 时随里程计发布 camera_init → aft_mapped 的 TF；False 时关闭这条 TF，里程计和配准扫描仍可继续输出。

当前集成链先保持 False，底盘 TF 由其他接口节点发布。单独调试需要显示 Point-LIO 自身位姿 TF 时，再考虑开启；这条 TF 表达内部 IMU 位姿，不是 odom → base_footprint。

pcd_save（点云地图保存）

参数表

    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.

参数详解

pcd_save_en（点云地图保存开关）

False 时关闭 PCD 保存，估计和建图仍继续；True 时累计 camera_init 坐标系中的配准扫描，保存为二进制 PCD。

当前先保持 False，需要存档时再开启，并保持 publish.scan_publish_en=True。注意：slam_launch.py 会把该参数覆盖为 True，所以是否保存还要看实际启动方式。

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/

这不是相对于运行目录的路径；在其他目录重新编译后需重新核对安装产物。

保存内容是多帧配准扫描的累计点云；文件不会自动放到导航先验地图目录。输出文件名固定，需保留不同轮次结果时，应先归档已有文件。