slam一篇文章就够了——基于lio-sam的测试(3)

上次把最重要的轨迹评估给整清楚了,这次我们要学习的是liosam里面启动的相关的内容,以及我们如何自己去修改相关的配置文件.以及一些数据处理的分析(主要是提取odometry/gpsodometry/navsat)
首先对于lio-sam来说,启动的内容只是一个launch:

roslaunch lio_sam run.launch

启动了以下的内容:

<launch>

    <arg name="project" default="lio_sam"/>
    
    <!-- Parameters -->
    <rosparam file="$(find lio_sam)/config/params.yaml" command="load" />

    <!--- LOAM -->
    <include file="$(find lio_sam)/launch/include/module_loam.launch" />

    <!--- Robot State TF -->
    <include file="$(find lio_sam)/launch/include/module_robot_state_publisher.launch" />

    <!--- Run Navsat -->
    <include file="$(find lio_sam)/launch/include/module_navsat.launch" />

    <!--- Run Rviz-->
    <include file="$(find lio_sam)/launch/include/module_rviz.launch" />

</launch>

从启动的launch来看还是比较容易理解的,先导入参数的yaml配置,然后启动了loam的系列node,然后是车辆的tf相关的state_publisher,以及gps和其他定位融合的节点robot_localization.最后就是rviz可视化了.我们接下来就按照顺序来进行分析.

params.yaml

lio_sam:

  # Topics
  pointCloudTopic: "points_raw"               # Point cloud data
  imuTopic: "imu_raw"                         # IMU data
  odomTopic: "odometry/imu"                   # IMU pre-preintegration odometry, same frequency as IMU
  # 如果要使用gps数据,需要把odom数据导入.不过都是先处理过后的,原始数据不带odom
  gpsTopic: "odometry/gpsz"                   # GPS odometry topic from navsat, see module_navsat.launch file

  # Frames
  lidarFrame: "base_link"
  baselinkFrame: "base_link"
  odometryFrame: "odom"
  mapFrame: "map"

  # GPS Settings
  useImuHeadingInitialization: true           # if using GPS data, set to "true"
  useGpsElevation: false                      # if GPS elevation is bad, set to "false"
  gpsCovThreshold: 2.0                        # m^2, threshold for using GPS data
  poseCovThreshold: 25.0                      # m^2, threshold for using GPS data
  
  # Export settings
  savePCD: true                              # https://github.com/TixiaoShan/LIO-SAM/issues/3
  savePCDDirectory: "/Downloads/LOAM/"        # in your home folder, starts and ends with "/". Warning: the code deletes "LOAM" folder then recreates it. See "mapOptimization" for implementation

  # Sensor Settings
  sensor: velodyne                            # lidar sensor type, 'velodyne' or 'ouster' or 'livox'
  N_SCAN: 16                                  # number of lidar channel (i.e., Velodyne/Ouster: 16, 32, 64, 128, Livox Horizon: 6)
  Horizon_SCAN: 1800                          # lidar horizontal resolution (Velodyne:1800, Ouster:512,1024,2048, Livox Horizon: 4000)
  downsampleRate: 1                           # default: 1. Downsample your data if too many points. i.e., 16 = 64 / 4, 16 = 16 / 1
  lidarMinRange: 1.0                          # default: 1.0, minimum lidar range to be used
  lidarMaxRange: 1000.0                       # default: 1000.0, maximum lidar range to be used

  # IMU Settings
  imuAccNoise: 3.9939570888238808e-03
  imuGyrNoise: 1.5636343949698187e-03
  imuAccBiasN: 6.4356659353532566e-05
  imuGyrBiasN: 3.5640318696367613e-05
  imuGravity: 9.80511
  imuRPYWeight: 0.01

  # Extrinsics: T_lb (lidar -> imu)
  extrinsicTrans: [0.0, 0.0, 0.0]
  # liosam作者用的imu比较特殊,一般的两个都是同一个值
  extrinsicRot: [-1, 0, 0,
                  0, 1, 0,
                  0, 0, -1] # ← 这就是 R_lidar→imu(即 R_lb)
  extrinsicRPY: [0, -1, 0,
                 1, 0, 0,
                 0, 0, 1] # ← 这其实是 R_imu→lidar(即 R_bl = R_lb⁻¹)
  # 如果两个都是同样的方向上的.
  # extrinsicRot: [1, 0, 0,
  #                 0, 1, 0,
  #                 0, 0, 1]
  # extrinsicRPY: [1, 0, 0,
  #                 0, 1, 0,
  #                 0, 0, 1]

  # LOAM feature threshold
  edgeThreshold: 1.0
  surfThreshold: 0.1
  edgeFeatureMinValidNum: 10
  surfFeatureMinValidNum: 100

  # voxel filter paprams
  odometrySurfLeafSize: 0.4                     # default: 0.4 - outdoor, 0.2 - indoor
  mappingCornerLeafSize: 0.2                    # default: 0.2 - outdoor, 0.1 - indoor
  mappingSurfLeafSize: 0.4                      # default: 0.4 - outdoor, 0.2 - indoor

  # robot motion constraint (in case you are using a 2D robot)
  z_tollerance: 1000                            # meters
  rotation_tollerance: 1000                     # radians

  # CPU Params
  numberOfCores: 4                              # number of cores for mapping optimization
  mappingProcessInterval: 0.15                  # seconds, regulate mapping frequency

  # Surrounding map
  surroundingkeyframeAddingDistThreshold: 1.0   # meters, regulate keyframe adding threshold
  surroundingkeyframeAddingAngleThreshold: 0.2  # radians, regulate keyframe adding threshold
  surroundingKeyframeDensity: 2.0               # meters, downsample surrounding keyframe poses   
  surroundingKeyframeSearchRadius: 50.0         # meters, within n meters scan-to-map optimization (when loop closure disabled)

  # Loop closure
  loopClosureEnableFlag: true
  loopClosureFrequency: 1.0                     # Hz, regulate loop closure constraint add frequency
  surroundingKeyframeSize: 50                   # submap size (when loop closure enabled)
  historyKeyframeSearchRadius: 15.0             # meters, key frame that is within n meters from current pose will be considerd for loop closure
  historyKeyframeSearchTimeDiff: 30.0           # seconds, key frame that is n seconds older will be considered for loop closure
  historyKeyframeSearchNum: 25                  # number of hostory key frames will be fused into a submap for loop closure
  historyKeyframeFitnessScore: 0.3              # icp threshold, the smaller the better alignment

  # Visualization
  globalMapVisualizationSearchRadius: 1000.0    # meters, global map visualization radius
  globalMapVisualizationPoseDensity: 10.0       # meters, global map visualization keyframe density
  globalMapVisualizationLeafSize: 1.0           # meters, global map visualization cloud density




# Navsat (convert GPS coordinates to Cartesian)
navsat:
  frequency: 50
  wait_for_datum: false
  delay: 0.0
  magnetic_declination_radians: 0
  yaw_offset: 0
  zero_altitude: true
  broadcast_utm_transform: false
  broadcast_utm_transform_as_parent_frame: false
  publish_filtered_gps: false

# EKF for Navsat
ekf_gps:
  publish_tf: false
  map_frame: map
  odom_frame: odom
  base_link_frame: base_link
  world_frame: odom

  frequency: 50
  two_d_mode: false
  sensor_timeout: 0.01
  # -------------------------------------
  # External IMU:
  # -------------------------------------
  imu0: imu_correct
  # make sure the input is aligned with ROS REP105. "imu_correct" is manually transformed by myself. EKF can also transform the data using tf between your imu and base_link
  imu0_config: [false, false, false,
                true,  true,  true,
                false, false, false,
                false, false, true,
                true,  true,  true]
  imu0_differential: false
  imu0_queue_size: 50 
  imu0_remove_gravitational_acceleration: true
  # -------------------------------------
  # Odometry (From Navsat):
  # -------------------------------------
  odom0: odometry/gps
  odom0_config: [true,  true,  true,
                 false, false, false,
                 false, false, false,
                 false, false, false,
                 false, false, false]
  odom0_differential: false
  odom0_queue_size: 10

  #                            x     y     z     r     p     y   x_dot  y_dot  z_dot  r_dot p_dot y_dot x_ddot y_ddot z_ddot
  process_noise_covariance: [  1.0,  0,    0,    0,    0,    0,    0,     0,     0,     0,    0,    0,    0,    0,      0,
                               0,    1.0,  0,    0,    0,    0,    0,     0,     0,     0,    0,    0,    0,    0,      0,
                               0,    0,    10.0, 0,    0,    0,    0,     0,     0,     0,    0,    0,    0,    0,      0,
                               0,    0,    0,    0.03, 0,    0,    0,     0,     0,     0,    0,    0,    0,    0,      0,
                               0,    0,    0,    0,    0.03, 0,    0,     0,     0,     0,    0,    0,    0,    0,      0,
                               0,    0,    0,    0,    0,    0.1,  0,     0,     0,     0,    0,    0,    0,    0,      0,
                               0,    0,    0,    0,    0,    0,    0.25,  0,     0,     0,    0,    0,    0,    0,      0,
                               0,    0,    0,    0,    0,    0,    0,     0.25,  0,     0,    0,    0,    0,    0,      0,
                               0,    0,    0,    0,    0,    0,    0,     0,     0.04,  0,    0,    0,    0,    0,      0,
                               0,    0,    0,    0,    0,    0,    0,     0,     0,     0.01, 0,    0,    0,    0,      0,
                               0,    0,    0,    0,    0,    0,    0,     0,     0,     0,    0.01, 0,    0,    0,      0,
                               0,    0,    0,    0,    0,    0,    0,     0,     0,     0,    0,    0.5,  0,    0,      0,
                               0,    0,    0,    0,    0,    0,    0,     0,     0,     0,    0,    0,    0.01, 0,      0,
                               0,    0,    0,    0,    0,    0,    0,     0,     0,     0,    0,    0,    0,    0.01,   0,
                               0,    0,    0,    0,    0,    0,    0,     0,     0,     0,    0,    0,    0,    0,      0.015]

因为比较长,我们把它划分一下

lio_sam:

  # Topics
  pointCloudTopic: "points_raw"               # Point cloud data
  imuTopic: "imu_raw"                         # IMU data
  odomTopic: "odometry/imu"                   # IMU pre-preintegration odometry, same frequency as IMU
  # 如果要使用gps数据,需要把odom数据导入.不过都是先处理过后的,原始数据不带odom
  gpsTopic: "odometry/gpsz"                   # GPS odometry topic from navsat, see module_navsat.launch file

首先是四大话题的数据信息,包括点云数据,imu数据,imu的惯性导航odom数据和gps的odom数据.

  # Frames
  lidarFrame: "base_link"
  baselinkFrame: "base_link"
  odometryFrame: "odom"
  mapFrame: "map"

这里是一些坐标系的定义,基本不影响什么,直接用就好.

  # GPS Settings
  useImuHeadingInitialization: true           # if using GPS data, set to "true"
  useGpsElevation: false                      # if GPS elevation is bad, set to "false"
  gpsCovThreshold: 2.0                        # m^2, threshold for using GPS data
  poseCovThreshold: 25.0                      # m^2, threshold for using GPS data
  
  # Export settings
  savePCD: true                              # https://github.com/TixiaoShan/LIO-SAM/issues/3
  savePCDDirectory: "/Downloads/LOAM/"        # in your home folder, starts and ends with "/". Warning: the code deletes "LOAM" folder then recreates it. See "mapOptimization" for implementation

是否使用gps数据,如果使用的话,可以把useImuHeadingInitialization置为true.然后是是否保存pcd地图,以及保存的位置,这个位置是增量式的,不需要加上/home/用户,直接就是~/开始的,所以再填入额外的路径即可.

  # Sensor Settings
  sensor: velodyne                            # lidar sensor type, 'velodyne' or 'ouster' or 'livox'
  N_SCAN: 16                                  # number of lidar channel (i.e., Velodyne/Ouster: 16, 32, 64, 128, Livox Horizon: 6)
  Horizon_SCAN: 1800                          # lidar horizontal resolution (Velodyne:1800, Ouster:512,1024,2048, Livox Horizon: 4000)
  downsampleRate: 1                           # default: 1. Downsample your data if too many points. i.e., 16 = 64 / 4, 16 = 16 / 1
  lidarMinRange: 1.0                          # default: 1.0, minimum lidar range to be used
  lidarMaxRange: 1000.0                       # default: 1000.0, maximum lidar range to be used

雷达传感器的设置,比较重要,并且lio-sam没有支持rslidar,可以自己再补充一个robosense的雷达类型(需要修改源码.)

  # IMU Settings
  imuAccNoise: 3.9939570888238808e-03
  imuGyrNoise: 1.5636343949698187e-03
  imuAccBiasN: 6.4356659353532566e-05
  imuGyrBiasN: 3.5640318696367613e-05
  imuGravity: 9.80511
  imuRPYWeight: 0.01

除了设置lidar,还可以设置imu数据,这里设置的是
imuAccNoise 加速度计测量噪声

imuGyrNoise 陀螺仪测量噪声

imuAccBiasN 加速度计偏置噪声

imuGyrBiasN 陀螺仪偏置噪声

imuGravity 重力加速度值

imuRPYWeight IMU 姿态角融合权重

这里不知道设置会有多少影响,可以先保留参数.然后是lidar->imu的标定参数,这个是最重要的一环,因为很多时候我们都是没有把lidar的坐标系和imu的坐标系对齐的.平移矩阵不重要,主要是旋转矩阵比较重要,一般lidar都是都是前x,左y,上z的坐标系,imu会有一定的旋转,把对应的旋转矩阵填入,注意是lidar->imu的,不是imu->lidar的,并且一般extrinsicRot和extrinsicRPY都是一样的值,作者的imu比较特殊型号,所以两个旋转不一样,如果lidar和imu的方向一致,那么就是注释部分的旋转矩阵.

  # Extrinsics: T_lb (lidar -> imu)
  extrinsicTrans: [0.0, 0.0, 0.0]
  # liosam作者用的imu比较特殊,一般的两个都是同一个值
  extrinsicRot: [-1, 0, 0,
                  0, 1, 0,
                  0, 0, -1] # ← 这就是 R_lidar→imu(即 R_lb)
  extrinsicRPY: [0, -1, 0,
                 1, 0, 0,
                 0, 0, 1] # ← 这其实是 R_imu→lidar(即 R_bl = R_lb⁻¹)
  # 如果两个都是同样的方向上的.
  # extrinsicRot: [1, 0, 0,
  #                 0, 1, 0,
  #                 0, 0, 1]
  # extrinsicRPY: [1, 0, 0,
  #                 0, 1, 0,
  #                 0, 0, 1]

然后就是算法的配置参数了,这里我也没有调过,先分析一下它们的作用.

  # LOAM feature threshold
  edgeThreshold: 1.0
  surfThreshold: 0.1
  edgeFeatureMinValidNum: 10
  surfFeatureMinValidNum: 100

  # voxel filter paprams
  odometrySurfLeafSize: 0.4                     # default: 0.4 - outdoor, 0.2 - indoor
  mappingCornerLeafSize: 0.2                    # default: 0.2 - outdoor, 0.1 - indoor
  mappingSurfLeafSize: 0.4                      # default: 0.4 - outdoor, 0.2 - indoor

  # robot motion constraint (in case you are using a 2D robot)
  z_tollerance: 1000                            # meters
  rotation_tollerance: 1000                     # radians

  # CPU Params
  numberOfCores: 4                              # number of cores for mapping optimization
  mappingProcessInterval: 0.15                  # seconds, regulate mapping frequency

  # Surrounding map
  surroundingkeyframeAddingDistThreshold: 1.0   # meters, regulate keyframe adding threshold
  surroundingkeyframeAddingAngleThreshold: 0.2  # radians, regulate keyframe adding threshold
  surroundingKeyframeDensity: 2.0               # meters, downsample surrounding keyframe poses   
  surroundingKeyframeSearchRadius: 50.0         # meters, within n meters scan-to-map optimization (when loop closure disabled)

  # Loop closure
  loopClosureEnableFlag: true
  loopClosureFrequency: 1.0                     # Hz, regulate loop closure constraint add frequency
  surroundingKeyframeSize: 50                   # submap size (when loop closure enabled)
  historyKeyframeSearchRadius: 15.0             # meters, key frame that is within n meters from current pose will be considerd for loop closure
  historyKeyframeSearchTimeDiff: 30.0           # seconds, key frame that is n seconds older will be considered for loop closure
  historyKeyframeSearchNum: 25                  # number of hostory key frames will be fused into a submap for loop closure
  historyKeyframeFitnessScore: 0.3              # icp threshold, the smaller the better alignment

  # Visualization
  globalMapVisualizationSearchRadius: 1000.0    # meters, global map visualization radius
  globalMapVisualizationPoseDensity: 10.0       # meters, global map visualization keyframe density
  globalMapVisualizationLeafSize: 1.0           # meters, global map visualization cloud density
  • 1. LOAM 特征提取参数 (LOAM Feature Threshold)
    控制前端里程计如何从原始点云中提取特征点。
参数名默认值含义与作用调节建议
edgeThreshold1.0边缘特征提取阈值
用于判断点是否为边缘(基于曲率)。值越大,只提取非常锋利的边缘。
环境特征不明显时减小;噪声大时增大。
surfThreshold0.1平面特征提取阈值
用于判断点是否为平面。值越小,对平坦度要求越严。
室内环境平面较多,可适当减小以提高精度。
edgeFeatureMinValidNum10最小有效边缘点数
一帧中最少需要提取到的边缘点数量,否则该帧可能被视为无效。
场景空旷或特征少(如长走廊)容易跟丢时,尝试减小此值。
surfFeatureMinValidNum100最小有效平面点数
一帧中最少需要提取到的平面点数量。
通常保持默认,平面点通常很充足。
    1. 体素滤波参数 (Voxel Filter Params)
      通过体素网格滤波器(Voxel Grid Filter)对点云进行降采样,平衡精度与计算量。
参数名默认值含义与作用调节建议
odometrySurfLeafSize0.4里程计平面点降采样
用于前端里程计地图构建。
室内建议 0.2,室外建议 0.4。值越小精度越高,但计算越慢。
mappingCornerLeafSize0.2建图边缘点降采样
用于后端建图优化的边缘点。
室内建议 0.1,室外建议 0.2。边缘特征对约束很重要,通常比平面设得更细。
mappingSurfLeafSize0.4建图平面点降采样
用于后端建图优化的平面点。
室内建议 0.2,室外建议 0.4。
    1. 机器人运动约束 (Robot Motion Constraint)
      用于将 3D SLAM 强制约束为 2D 模式(适用于地面车辆 AGV)。
参数名默认值含义与作用调节建议
z_tollerance1000Z轴(高度)容差
设为极大值(1000)表示不限制。
如果是地面机器人且 Z 轴漂移严重,设为极小值(如 0.1)以强制高度不变。
rotation_tollerance1000旋转(Roll/Pitch)容差
设为极大值表示允许 3D 旋转。
地面车辆可设为小值,忽略 Roll/Pitch 变化,防止地图倾斜。
    1. CPU 与处理参数 (CPU Params)
      控制后端优化的并行计算与处理频率。
参数名默认值含义与作用调节建议
numberOfCores4并行核心数
用于 GTSAM 优化和局部地图构建的多线程加速。
根据机载电脑性能设置(如 NUC 设为 4-8)。
mappingProcessInterval0.15建图处理间隔(s)
后端建图线程的频率,0.15s 代表约 6-7Hz。
CPU 负载过高时,可增大此值(如 0.3),牺牲实时性换取流畅度。
    1. 局部地图与关键帧 (Surrounding Map)
      决定滑动窗口地图的大小及关键帧选取策略。
参数名默认值含义与作用调节建议
surroundingkeyframeAddingDistThreshold1.0关键帧距离阈值
移动超过 1m 添加新关键帧。
室内狭窄环境减小(0.5),室外开阔环境增大。
surroundingkeyframeAddingAngleThreshold0.2关键帧角度阈值
旋转超过 0.2 rad 添加新关键帧。
旋转剧烈时需要更多关键帧维持跟踪。
surroundingKeyframeDensity2.0局部地图关键帧稀疏度
对选取的关键帧位置进行降采样,防止局部图太密。
较小值会使局部地图更密,匹配更稳但更慢。
surroundingKeyframeSearchRadius50.0局部地图搜索半径
仅将半径 50m 内的关键帧拼成局部地图进行 Scan-to-Map。
室外建议保持 50 或更大,室内可减小。
    1. 闭环检测 (Loop Closure)
      抑制累积误差(Drift)的关键模块。
参数名默认值含义与作用调节建议
loopClosureEnableFlagtrue开启/关闭闭环不需要全局一致性或算力受限时设为 false
loopClosureFrequency1.0闭环检测频率(Hz)算力不足可降至 0.5。
surroundingKeyframeSize50闭环子地图大小
用于 ICP 匹配的子地图包含的关键帧数。
越大越鲁棒,耗时增加。
historyKeyframeSearchRadius15.0历史帧搜索半径
只检测当前位置 15m 范围内的历史帧。
GPS 误差较大时需适当增大。
historyKeyframeSearchTimeDiff30.0时间间隔阈值(s)
必须是 30s 之前的帧才视为回环(防止与刚经过的路径误匹配)。
仅在短时间内重复经过同一地点时才需减小。
historyKeyframeSearchNum25回环匹配帧数
取候选帧周围 25 帧拼图进行 ICP。
默认即可。
historyKeyframeFitnessScore0.3ICP 得分阈值
误差小于 0.3 才认为回环有效。越小越严格
若出现错误回环(地图重影/炸裂),请减小此值(如 0.15)。
    1. 可视化 (Visualization)
      仅影响 Rviz 显示效果,不影响算法精度。
参数名默认值含义与作用
globalMapVisualizationSearchRadius1000.0只显示当前位置 1000m 内的地图点云。
globalMapVisualizationPoseDensity10.0轨迹显示稀疏度,每隔 10m 画一个坐标轴。
globalMapVisualizationLeafSize1.0全局地图显示稀疏度,1.0 表示显示的地图很稀疏(省显存)。

最后的配置是navsat和ekf_gps定位的模块,这两个定位模块非常的重要,但是要想看懂又需要非常清晰的认识robot_localization,这里我们暂时先不考虑修改,后面在创建自己的数据的时候还会重新分析这个配置.


# Navsat (convert GPS coordinates to Cartesian)
navsat:
  frequency: 50
  wait_for_datum: false
  delay: 0.0
  magnetic_declination_radians: 0
  yaw_offset: 0
  zero_altitude: true
  broadcast_utm_transform: false
  broadcast_utm_transform_as_parent_frame: false
  publish_filtered_gps: false

# EKF for Navsat
ekf_gps:
  publish_tf: false
  map_frame: map
  odom_frame: odom
  base_link_frame: base_link
  world_frame: odom

  frequency: 50
  two_d_mode: false
  sensor_timeout: 0.01
  # -------------------------------------
  # External IMU:
  # -------------------------------------
  imu0: imu_correct
  # make sure the input is aligned with ROS REP105. "imu_correct" is manually transformed by myself. EKF can also transform the data using tf between your imu and base_link
  imu0_config: [false, false, false,
                true,  true,  true,
                false, false, false,
                false, false, true,
                true,  true,  true]
  imu0_differential: false
  imu0_queue_size: 50 
  imu0_remove_gravitational_acceleration: true
  # -------------------------------------
  # Odometry (From Navsat):
  # -------------------------------------
  odom0: odometry/gps
  odom0_config: [true,  true,  true,
                 false, false, false,
                 false, false, false,
                 false, false, false,
                 false, false, false]
  odom0_differential: false
  odom0_queue_size: 10

  #                            x     y     z     r     p     y   x_dot  y_dot  z_dot  r_dot p_dot y_dot x_ddot y_ddot z_ddot
  process_noise_covariance: [  1.0,  0,    0,    0,    0,    0,    0,     0,     0,     0,    0,    0,    0,    0,      0,
                               0,    1.0,  0,    0,    0,    0,    0,     0,     0,     0,    0,    0,    0,    0,      0,
                               0,    0,    10.0, 0,    0,    0,    0,     0,     0,     0,    0,    0,    0,    0,      0,
                               0,    0,    0,    0.03, 0,    0,    0,     0,     0,     0,    0,    0,    0,    0,      0,
                               0,    0,    0,    0,    0.03, 0,    0,     0,     0,     0,    0,    0,    0,    0,      0,
                               0,    0,    0,    0,    0,    0.1,  0,     0,     0,     0,    0,    0,    0,    0,      0,
                               0,    0,    0,    0,    0,    0,    0.25,  0,     0,     0,    0,    0,    0,    0,      0,
                               0,    0,    0,    0,    0,    0,    0,     0.25,  0,     0,    0,    0,    0,    0,      0,
                               0,    0,    0,    0,    0,    0,    0,     0,     0.04,  0,    0,    0,    0,    0,      0,
                               0,    0,    0,    0,    0,    0,    0,     0,     0,     0.01, 0,    0,    0,    0,      0,
                               0,    0,    0,    0,    0,    0,    0,     0,     0,     0,    0.01, 0,    0,    0,      0,
                               0,    0,    0,    0,    0,    0,    0,     0,     0,     0,    0,    0.5,  0,    0,      0,
                               0,    0,    0,    0,    0,    0,    0,     0,     0,     0,    0,    0,    0.01, 0,      0,
                               0,    0,    0,    0,    0,    0,    0,     0,     0,     0,    0,    0,    0,    0.01,   0,
                               0,    0,    0,    0,    0,    0,    0,     0,     0,     0,    0,    0,    0,    0,      0.015]

这些参数主要用于 robot_localization 包,通常在 LIO-SAM 等系统中用于融合 GPS 和 IMU 数据,以获得全局一致的里程计。

    1. Navsat (GPS 坐标转换)
      配置 navsat_transform_node 节点,负责将 GPS 的经纬度(WGS84)转换为机器人坐标系下的笛卡尔坐标(通常是 UTM 或局部 ENU)。
参数名含义与作用调节建议
frequency50运行频率 (Hz)
节点的主循环频率。
通常设为与 IMU 或 EKF 频率一致或略低。
wait_for_datumfalse是否等待原点
true: 需要手动设置原点;false: 取第一帧 GPS 信号作为地图原点 (0,0,0)。
一般设为 false 以便自动启动。
delay0.0时间延迟
用于补偿数据传输延迟。
除非有明确的时间同步问题,否则设为 0。
magnetic_declination_radians0磁偏角 (弧度)
用于修正磁北与真北的夹角。
非常重要。需根据所在地的经纬度查询磁偏角并填入,否则 GPS 轨迹的方向会偏。
yaw_offset0偏航角偏移 (弧度)
如果 IMU 安装朝向与车头不一致,需在此修正。
根据 IMU 安装方向设置(0 表示 IMU X轴朝前)。
zero_altitudetrue高度归零
true: 忽略 GPS 的高度信息,强制 Z=0;false: 使用 GPS 高度。
2D 导航或 GPS 高度漂移严重时设为 true
broadcast_utm_transformfalse发布 UTM TF
是否发布 utm -> map 的坐标变换。
只要局部坐标系够用,通常设为 false 以避免 TF 树冲突。
publish_filtered_gpsfalse发布滤波后的 GPS
是否发布融合后的 GPS 坐标用于调试。
调试时可开启。

从输入输出来看这个节点的细节内容:

方向默认话题名 (Topic)消息类型 (Type)描述 (Description)
Inputimu/datasensor_msgs/Imu必要的方向参考。提供磁力计或计算出的绝对朝向,用于确定 GPS 坐标系与机器人坐标系的对齐关系。
Inputgps/fixsensor_msgs/NavSatFix原始 GPS 数据。包含经度、纬度、高度信息。
Inputodometry/filterednav_msgs/Odometry(可选) EKF 反馈。如果需要发布 map->utm 的静态 TF,需要此反馈。
Outputodometry/gpsnav_msgs/Odometry核心输出。将经纬度转换为局部笛卡尔坐标系(平面坐标)。通常作为 ekf_localization_node 的输入。
Outputgps/filteredsensor_msgs/NavSatFix(可选) 反向转换。将 EKF 融合后的里程计位置反算回经纬度,用于地图显示。

EKF for Navsat (扩展卡尔曼滤波)
配置 ekf_localization_node 节点,用于融合 IMU 和 转换后的 GPS 里程计数据。

  • 2.1 基础设置与 TF 树
参数名含义与作用
publish_tffalse是否发布 TF 变换。在 LIO-SAM 中通常由 Lidar 里程计发布 TF,这里仅用于计算,所以设为 false。
map_framemap地图坐标系名称。
odom_frameodom里程计坐标系名称。
base_link_framebase_link机器人基座标系名称。
world_frameodom世界坐标系。EKF 输出的姿态是相对于这个坐标系的。这里设为 odom 表示融合结果基于局部里程计。
  • 2.2 传感器输入配置 (The Configuration Matrix)

这是 EKF 最核心的配置,决定融合哪些数据。矩阵顺序为:
[X, Y, Z, Roll, Pitch, Yaw, dX, dY, dZ, dRoll, dPitch, dYaw, ddX, ddY, ddZ]
(位置 XYZ, 姿态 RPY, 线速度 XYZ, 角速度 RPY, 线加速度 XYZ)

  • IMU 输入 (imu0)

配置:imu_correct (通常是去畸变或转系后的 IMU 数据)

状态量配置值解释
位置 (X, Y, Z)false, false, falseIMU 不提供绝对位置。
姿态 (R, P, Y)true, true, true使用 IMU 的绝对姿态(欧拉角)。
线速度 (dX, dY, dZ)false, false, falseIMU 不直接提供线速度。
角速度 (dR, dP, dY)false, false, true仅使用 Yaw (Z轴) 的角速度。通常用于修正航向变化。
线加速度 (ddX, ddY, ddZ)true, true, true使用 IMU 的线性加速度。
  • imu0_differential: false。使用绝对测量值(直接由 IMU 提供的数值),而不是计算两帧之间的差量。
  • imu0_remove_gravitational_acceleration: true非常关键。因为上面的配置使用了加速度 (ddX...),必须告知 EKF 移除重力加速度的影响,否则静止时 Z 轴会有巨大的加速度输入导致飞车。
  • GPS 里程计输入 (odom0)

配置:odometry/gps (这是 navsat_transform_node 输出的话题)

状态量配置值解释
位置 (X, Y, Z)true, true, true使用 GPS 转换来的绝对坐标 (X, Y, Z)。
姿态false...这里的 GPS 里程计不提供可靠姿态,交给 IMU。
速度/加速度false...不使用 GPS 的速度,噪声通常较大。

    1. 过程噪声协方差 (Process Noise Covariance)

process_noise_covariance 定义了系统预测模型的“不确定性”。

  • 这是一个对角矩阵,分别对应 [x, y, z, r, p, y, dx, dy, dz, dr, dp, dy, ddx, ddy, ddz]
  • 数值含义:数值越 ,表示系统越 不信任 运动模型的预测(允许状态发生突变);数值越 ,表示系统认为运动是平滑的,更 信任 预测值。
索引对应状态分析
0, 1X, Y 位置1.0允许水平位置有一定的预测误差。
2Z 位置10.0非常大。表示对 Z 轴高度的预测非常不确定(允许高度剧烈跳变)。这是为了快速适应 GPS 的高度修正。
3, 4Roll, Pitch0.03较小,认为机器人不会发生剧烈的翻滚或俯仰。
5Yaw0.1允许航向角有一定的动态变化。
(速度和加速度的噪声通常根据传感器精度设定)
14Z 加速度0.015相对较小,平滑 Z 轴加速度。

配置的内容还是比较多的,同样的我们来总结一下输入输出.

方向默认话题名 (Topic)消息类型 (Type)描述 (Description)
Inputodometry/gpsnav_msgs/Odometry来自 navsat_transform_node。提供绝对的位置观测 (X, Y, Z),用于消除累积误差。
Inputimu/datasensor_msgs/Imu来自传感器。提供姿态 (RPY)、角速度和线加速度。用于高频状态预测。
Inputodometry/wheelnav_msgs/Odometry(可选) 轮式里程计。提供局部速度或位置增量。
Outputodometry/filterednav_msgs/Odometry核心输出。融合后的、平滑且无漂移的全局里程计状态。
Output/tftf2_msgs/TFMessage坐标变换。发布 odom -> base_linkmap -> odom 的 TF 变换。

从实际的使用发现,这两个节点是相互需要的,所以启动的时候也需要一并启动.

为了更好的理解lio_sam中的除lidar之外的模块,我们可以把他们专门提取出来进一步的分析.也就是ekf和navsat两个节点,以及lio-sam提供的专门的数据集.
首先是ekf节点的相关的信息,启动的是run_ekf_gps.launch:

<launch>
    <!-- 发布odometry/navsat -->
    <node pkg="robot_localization" type="ekf_localization_node" name="ekf_gps" respawn="true" output="screen">
        
        <rosparam command="load" file="$(find lio_sam)/config/ekf_gps_params.yaml" />

        <remap from="odometry/filtered" to="odometry/navsat" />
        
    </node>
</launch>

配置与上面的是一样的.yaml如下:

# EKF 节点通常负责发布 odom -> base_link 的变换。
# 但如果你有 LIO-SAM 或其他 SLAM 算法已经在发布这个 TF,这里必须设为 false,否则 TF 树会打架(闪烁)。
publish_tf: true # 由于我们只启动gps imu,所以这里需要用true

# --- 坐标系定义 ---
# map_frame: 全局地图坐标系 (通常是 map)
# odom_frame: 里程计坐标系 (通常是 odom)
# base_link_frame: 机器人自身的中心坐标系
# world_frame: EKF 输出的数据是基于哪个坐标系的?对于 GPS 融合,通常填 odom。
map_frame: map
odom_frame: odom
base_link_frame: base_link
world_frame: odom

frequency: 50
# --- 2D 模式 ---
# true = 强制把 Z, Roll, Pitch 设为 0。适用于只在平地跑的扫地机。
# false = 处理 3D 数据(即便是在地面,如果有坡度,也建议设为 false)。
two_d_mode: false
sensor_timeout: 0.01 # 如果传感器 0.01秒没数据,就认为超时

# -------------------------------------
# External IMU:
# -------------------------------------
imu0: imu_correct # 订阅的话题名
# imu_correct needs to be aligned with ROS REP105 (ENU)
imu0_config: [false, false, false, # 不融合 IMU 的位置 (IMU 不知道自己在哪里)
              true,  true,  true,  # 融合 IMU 的 姿态 (Roll/Pitch/Yaw) -> 核心!
              false, false, false, # 不融合 IMU 的 线速度 (由积分得来,误差大)
              false, false, true,  # 融合 IMU 的 角速度 (Yaw rate, Z轴旋转速度)
              true,  true,  true]  # 融合 IMU 的 加速度 (用于检测运动状态)

# 是否是差分模式?
# false = 直接使用 IMU 输出的绝对角度。
# true = 只关心上一帧到这一帧变了多少度 (如果有磁场干扰导致绝对角度不准,可以开这个)。
imu0_differential: false
imu0_queue_size: 50 
# 是否移除重力加速度?
# true = IMU 一般会测到 9.8 的重力,EKF 需要把它减掉,否则会以为车子在疯狂向上加速。
imu0_remove_gravitational_acceleration: true

# -------------------------------------
# Odometry (From Navsat node):
# -------------------------------------
odom0: odometry/gps # 订阅 Navsat 节点发布的话题
odom0_config: [true,  true,  true, # 融合 GPS 的 X, Y, Z 位置 -> 核心!
               false, false, false, # 不融合 姿态 (GPS 不知道车头朝向)
               false, false, false, # 不融合 速度
               false, false, false,
               false, false, false]


odom0_differential: false # GPS 是绝对位置,不能开差分
odom0_queue_size: 10

#                            x     y     z     r     p     y   x_dot  y_dot  z_dot  r_dot p_dot y_dot x_ddot y_ddot z_ddot
process_noise_covariance: [  1.0,  0,    0,    0,    0,    0,    0,     0,     0,     0,    0,    0,    0,    0,      0,
                             0,    1.0,  0,    0,    0,    0,    0,     0,     0,     0,    0,    0,    0,    0,      0,
                             0,    0,    10.0, 0,    0,    0,    0,     0,     0,     0,    0,    0,    0,    0,      0,
                             0,    0,    0,    0.03, 0,    0,    0,     0,     0,     0,    0,    0,    0,    0,      0,
                             0,    0,    0,    0,    0.03, 0,    0,     0,     0,     0,    0,    0,    0,    0,      0,
                             0,    0,    0,    0,    0,    0.1,  0,     0,     0,     0,    0,    0,    0,    0,      0,
                             0,    0,    0,    0,    0,    0,    0.25,  0,     0,     0,    0,    0,    0,    0,      0,
                             0,    0,    0,    0,    0,    0,    0,     0.25,  0,     0,    0,    0,    0,    0,      0,
                             0,    0,    0,    0,    0,    0,    0,     0,     0.04,  0,    0,    0,    0,    0,      0,
                             0,    0,    0,    0,    0,    0,    0,     0,     0,     0.01, 0,    0,    0,    0,      0,
                             0,    0,    0,    0,    0,    0,    0,     0,     0,     0,    0.01, 0,    0,    0,      0,
                             0,    0,    0,    0,    0,    0,    0,     0,     0,     0,    0,    0.5,  0,    0,      0,
                             0,    0,    0,    0,    0,    0,    0,     0,     0,     0,    0,    0,    0.01, 0,      0,
                             0,    0,    0,    0,    0,    0,    0,     0,     0,     0,    0,    0,    0,    0.01,   0,
                             0,    0,    0,    0,    0,    0,    0,     0,     0,     0,    0,    0,    0,    0,      0.015]

作用是把两个odom进行融合,从配置上来看,就是融合的是imu_correct(使用姿态)和odometry/gps(使用位置),也就是说需要这两个话题来输入,但是我们的原始数据中是没有/odometry/gps话题的,此时默认融合,定位就是只使用了imu来作为odom进行累积的.输出的/odometry/navsat也就是不准确的.但是依然是可以输出.

为了得到准确的输入/odometry/gps,需要启动navsat,run_navsat.launch:

<launch>
    <node pkg="robot_localization" type="navsat_transform_node" name="navsat" respawn="true" output="screen">
        <rosparam command="load" file="$(find lio_sam)/config/navsat_params.yaml" />
        <remap from="imu/data" to="imu_correct" />
        <remap from="gps/fix" to="gps/fix" />
        <remap from="odometry/filtered" to="odometry/navsat" />
    </node>
</launch>

三个remap都是输入,注意这里的imu给的是imu_correct,做了转换过后的imu数据,从原始数据中:

---
header: 
  seq: 2725122
  stamp: 
    secs: 1574795807
    nsecs: 579388103
  frame_id: "base_link"
orientation: 
  x: -0.025122905329903338
  y: 0.01724436221212363
  z: -0.6902692226680864
  w: 0.7229106942333039
orientation_covariance: [0.01, 0.0, 0.0, 0.0, 0.01, 0.0, 0.0, 0.0, 0.01]
angular_velocity: 
  x: -0.018783332780003548
  y: -0.16673213243484497
  z: -0.32905349135398865
angular_velocity_covariance: [0.01, 0.0, 0.0, 0.0, 0.01, 0.0, 0.0, 0.0, 0.01]
linear_acceleration: 
  x: 3.921087267994881
  y: -3.389918012022972
  z: 10.362218062877655
linear_acceleration_covariance: [0.01, 0.0, 0.0, 0.0, 0.01, 0.0, 0.0, 0.0, 0.01]

可以知道,是把imu_link转到了base_link后的数据.odometry/navsat需要ekf节点提供.gps/fix是原始的gps数据:

---
header: 
  seq: 26557
  stamp: 
    secs: 1574795921
    nsecs:  79819917
  frame_id: "navsat_link"
status: 
  status: 2
  service: 1
latitude: 40.66351444833333
longitude: -74.56353641
altitude: 37.669
position_covariance: [0.003856410000000001, 0.0, 0.0, 0.0, 0.004435559999999999, 0.0, 0.0, 0.0, 207.36]
position_covariance_type: 1
---

输出话题是odometry/gps,但是一般启动了run_navsat.launch是没有odometry/gps话题的输出的,那是因为odometry/filtered(remap到了odometry/navsat)话题没有输入进来,也就是还没有启动ekf.

所以他们俩必须得是同时启动,定位才是准确的.

<launch>

    <!--- Robot State TF -->
    <include file="$(find lio_sam)/launch/include/module_robot_state_publisher.launch" />
    
    <arg name="project" default="lio_sam"/>
    
    <!-- EKF GPS-->
    <node pkg="robot_localization" type="ekf_localization_node" name="ekf_gps" respawn="true">
        <remap from="odometry/filtered" to="odometry/navsat" />
    </node>

    <!-- Navsat -->
    <node pkg="robot_localization" type="navsat_transform_node" name="navsat" respawn="true">
        <!-- <rosparam param="datum">[42.35893211, -71.09345588, 0.0, world, base_link]</rosparam> -->
        <remap from="imu/data" to="imu_correct" />
        <remap from="gps/fix" to="gps/fix" />
        <remap from="odometry/filtered" to="odometry/navsat" />
    </node>

</launch>

注意,这里的launch还启动了module_robot_state_publisher.launch,这里面的车辆的tf也需要获得,因为navsat节点是需要使用这个tf关系的.

同时启动之后,再播放park_dataset.bag(https://drive.google.com/file/d/177bE5K638EJKv2KzlFdljTqGzRpOCdJP/view?usp=sharing)这个包,就能得到正确的odom数据了:
在这里插入图片描述
这是正确启动后的两个odom的样子,差别是非常小的,然后有一个是截断了高度的.这样,我们就有了能够完美输入lio-sam的额外odom数据了!
其频率也是由输入话题的频率来控制的.

cyun@cyun:~/Documents/ego-planner/slam_ws$ rostopic hz /odometry/gps 
subscribed to [/odometry/gps]
average rate: 5.128
	min: 0.180s max: 0.220s std dev: 0.01656s window: 5
average rate: 4.999
	min: 0.180s max: 0.220s std dev: 0.01334s window: 10
^Caverage rate: 5.046
	min: 0.180s max: 0.220s std dev: 0.01337s window: 12
cyun@cyun:~/Documents/ego-planner/slam_ws$ rostopic hz /odometry/navsat 
subscribed to [/odometry/navsat]
average rate: 50.053
	min: 0.019s max: 0.020s std dev: 0.00025s window: 50
average rate: 50.016
	min: 0.019s max: 0.021s std dev: 0.00023s window: 100
average rate: 50.014
	min: 0.019s max: 0.021s std dev: 0.00022s window: 151
评论
添加红包

请填写红包祝福语或标题

红包个数最小为10个

红包金额最低5元

当前余额3.43前往充值 >
需支付:10.00
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包

打赏作者

白云千载尽

你的鼓励将是我创作的最大动力

¥1 ¥2 ¥4 ¥6 ¥10 ¥20
扫码支付:¥1
获取中
扫码支付

您的余额不足,请更换扫码支付或充值

打赏作者

实付
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明:

1.余额是钱包充值的虚拟货币,按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载,可以购买VIP、付费专栏及课程。

余额充值