上次把最重要的轨迹评估给整清楚了,这次我们要学习的是liosam里面启动的相关的内容,以及我们如何自己去修改相关的配置文件.以及一些数据处理的分析(主要是提取odometry/gps和odometry/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)
控制前端里程计如何从原始点云中提取特征点。
| 参数名 | 默认值 | 含义与作用 | 调节建议 |
|---|---|---|---|
edgeThreshold | 1.0 | 边缘特征提取阈值。 用于判断点是否为边缘(基于曲率)。值越大,只提取非常锋利的边缘。 | 环境特征不明显时减小;噪声大时增大。 |
surfThreshold | 0.1 | 平面特征提取阈值。 用于判断点是否为平面。值越小,对平坦度要求越严。 | 室内环境平面较多,可适当减小以提高精度。 |
edgeFeatureMinValidNum | 10 | 最小有效边缘点数。 一帧中最少需要提取到的边缘点数量,否则该帧可能被视为无效。 | 场景空旷或特征少(如长走廊)容易跟丢时,尝试减小此值。 |
surfFeatureMinValidNum | 100 | 最小有效平面点数。 一帧中最少需要提取到的平面点数量。 | 通常保持默认,平面点通常很充足。 |
-
- 体素滤波参数 (Voxel Filter Params)
通过体素网格滤波器(Voxel Grid Filter)对点云进行降采样,平衡精度与计算量。
- 体素滤波参数 (Voxel Filter Params)
| 参数名 | 默认值 | 含义与作用 | 调节建议 |
|---|---|---|---|
odometrySurfLeafSize | 0.4 | 里程计平面点降采样。 用于前端里程计地图构建。 | 室内建议 0.2,室外建议 0.4。值越小精度越高,但计算越慢。 |
mappingCornerLeafSize | 0.2 | 建图边缘点降采样。 用于后端建图优化的边缘点。 | 室内建议 0.1,室外建议 0.2。边缘特征对约束很重要,通常比平面设得更细。 |
mappingSurfLeafSize | 0.4 | 建图平面点降采样。 用于后端建图优化的平面点。 | 室内建议 0.2,室外建议 0.4。 |
-
- 机器人运动约束 (Robot Motion Constraint)
用于将 3D SLAM 强制约束为 2D 模式(适用于地面车辆 AGV)。
- 机器人运动约束 (Robot Motion Constraint)
| 参数名 | 默认值 | 含义与作用 | 调节建议 |
|---|---|---|---|
z_tollerance | 1000 | Z轴(高度)容差。 设为极大值(1000)表示不限制。 | 如果是地面机器人且 Z 轴漂移严重,设为极小值(如 0.1)以强制高度不变。 |
rotation_tollerance | 1000 | 旋转(Roll/Pitch)容差。 设为极大值表示允许 3D 旋转。 | 地面车辆可设为小值,忽略 Roll/Pitch 变化,防止地图倾斜。 |
-
- CPU 与处理参数 (CPU Params)
控制后端优化的并行计算与处理频率。
- CPU 与处理参数 (CPU Params)
| 参数名 | 默认值 | 含义与作用 | 调节建议 |
|---|---|---|---|
numberOfCores | 4 | 并行核心数。 用于 GTSAM 优化和局部地图构建的多线程加速。 | 根据机载电脑性能设置(如 NUC 设为 4-8)。 |
mappingProcessInterval | 0.15 | 建图处理间隔(s)。 后端建图线程的频率,0.15s 代表约 6-7Hz。 | CPU 负载过高时,可增大此值(如 0.3),牺牲实时性换取流畅度。 |
-
- 局部地图与关键帧 (Surrounding Map)
决定滑动窗口地图的大小及关键帧选取策略。
- 局部地图与关键帧 (Surrounding Map)
| 参数名 | 默认值 | 含义与作用 | 调节建议 |
|---|---|---|---|
surroundingkeyframeAddingDistThreshold | 1.0 | 关键帧距离阈值。 移动超过 1m 添加新关键帧。 | 室内狭窄环境减小(0.5),室外开阔环境增大。 |
surroundingkeyframeAddingAngleThreshold | 0.2 | 关键帧角度阈值。 旋转超过 0.2 rad 添加新关键帧。 | 旋转剧烈时需要更多关键帧维持跟踪。 |
surroundingKeyframeDensity | 2.0 | 局部地图关键帧稀疏度。 对选取的关键帧位置进行降采样,防止局部图太密。 | 较小值会使局部地图更密,匹配更稳但更慢。 |
surroundingKeyframeSearchRadius | 50.0 | 局部地图搜索半径。 仅将半径 50m 内的关键帧拼成局部地图进行 Scan-to-Map。 | 室外建议保持 50 或更大,室内可减小。 |
-
- 闭环检测 (Loop Closure)
抑制累积误差(Drift)的关键模块。
- 闭环检测 (Loop Closure)
| 参数名 | 默认值 | 含义与作用 | 调节建议 |
|---|---|---|---|
loopClosureEnableFlag | true | 开启/关闭闭环。 | 不需要全局一致性或算力受限时设为 false。 |
loopClosureFrequency | 1.0 | 闭环检测频率(Hz)。 | 算力不足可降至 0.5。 |
surroundingKeyframeSize | 50 | 闭环子地图大小。 用于 ICP 匹配的子地图包含的关键帧数。 | 越大越鲁棒,耗时增加。 |
historyKeyframeSearchRadius | 15.0 | 历史帧搜索半径。 只检测当前位置 15m 范围内的历史帧。 | GPS 误差较大时需适当增大。 |
historyKeyframeSearchTimeDiff | 30.0 | 时间间隔阈值(s)。 必须是 30s 之前的帧才视为回环(防止与刚经过的路径误匹配)。 | 仅在短时间内重复经过同一地点时才需减小。 |
historyKeyframeSearchNum | 25 | 回环匹配帧数。 取候选帧周围 25 帧拼图进行 ICP。 | 默认即可。 |
historyKeyframeFitnessScore | 0.3 | ICP 得分阈值。 误差小于 0.3 才认为回环有效。越小越严格。 | 若出现错误回环(地图重影/炸裂),请减小此值(如 0.15)。 |
-
- 可视化 (Visualization)
仅影响 Rviz 显示效果,不影响算法精度。
- 可视化 (Visualization)
| 参数名 | 默认值 | 含义与作用 |
|---|---|---|
globalMapVisualizationSearchRadius | 1000.0 | 只显示当前位置 1000m 内的地图点云。 |
globalMapVisualizationPoseDensity | 10.0 | 轨迹显示稀疏度,每隔 10m 画一个坐标轴。 |
globalMapVisualizationLeafSize | 1.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 数据,以获得全局一致的里程计。
-
- Navsat (GPS 坐标转换)
配置navsat_transform_node节点,负责将 GPS 的经纬度(WGS84)转换为机器人坐标系下的笛卡尔坐标(通常是 UTM 或局部 ENU)。
- Navsat (GPS 坐标转换)
| 参数名 | 值 | 含义与作用 | 调节建议 |
|---|---|---|---|
frequency | 50 | 运行频率 (Hz)。 节点的主循环频率。 | 通常设为与 IMU 或 EKF 频率一致或略低。 |
wait_for_datum | false | 是否等待原点。true: 需要手动设置原点;false: 取第一帧 GPS 信号作为地图原点 (0,0,0)。 | 一般设为 false 以便自动启动。 |
delay | 0.0 | 时间延迟。 用于补偿数据传输延迟。 | 除非有明确的时间同步问题,否则设为 0。 |
magnetic_declination_radians | 0 | 磁偏角 (弧度)。 用于修正磁北与真北的夹角。 | 非常重要。需根据所在地的经纬度查询磁偏角并填入,否则 GPS 轨迹的方向会偏。 |
yaw_offset | 0 | 偏航角偏移 (弧度)。 如果 IMU 安装朝向与车头不一致,需在此修正。 | 根据 IMU 安装方向设置(0 表示 IMU X轴朝前)。 |
zero_altitude | true | 高度归零。true: 忽略 GPS 的高度信息,强制 Z=0;false: 使用 GPS 高度。 | 2D 导航或 GPS 高度漂移严重时设为 true。 |
broadcast_utm_transform | false | 发布 UTM TF。 是否发布 utm -> map 的坐标变换。 | 只要局部坐标系够用,通常设为 false 以避免 TF 树冲突。 |
publish_filtered_gps | false | 发布滤波后的 GPS。 是否发布融合后的 GPS 坐标用于调试。 | 调试时可开启。 |
从输入输出来看这个节点的细节内容:
| 方向 | 默认话题名 (Topic) | 消息类型 (Type) | 描述 (Description) |
|---|---|---|---|
| Input | imu/data | sensor_msgs/Imu | 必要的方向参考。提供磁力计或计算出的绝对朝向,用于确定 GPS 坐标系与机器人坐标系的对齐关系。 |
| Input | gps/fix | sensor_msgs/NavSatFix | 原始 GPS 数据。包含经度、纬度、高度信息。 |
| Input | odometry/filtered | nav_msgs/Odometry | (可选) EKF 反馈。如果需要发布 map->utm 的静态 TF,需要此反馈。 |
| Output | odometry/gps | nav_msgs/Odometry | 核心输出。将经纬度转换为局部笛卡尔坐标系(平面坐标)。通常作为 ekf_localization_node 的输入。 |
| Output | gps/filtered | sensor_msgs/NavSatFix | (可选) 反向转换。将 EKF 融合后的里程计位置反算回经纬度,用于地图显示。 |
EKF for Navsat (扩展卡尔曼滤波)
配置 ekf_localization_node 节点,用于融合 IMU 和 转换后的 GPS 里程计数据。
- 2.1 基础设置与 TF 树
| 参数名 | 值 | 含义与作用 |
|---|---|---|
publish_tf | false | 是否发布 TF 变换。在 LIO-SAM 中通常由 Lidar 里程计发布 TF,这里仅用于计算,所以设为 false。 |
map_frame | map | 地图坐标系名称。 |
odom_frame | odom | 里程计坐标系名称。 |
base_link_frame | base_link | 机器人基座标系名称。 |
world_frame | odom | 世界坐标系。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, false | IMU 不提供绝对位置。 |
| 姿态 (R, P, Y) | true, true, true | 使用 IMU 的绝对姿态(欧拉角)。 |
| 线速度 (dX, dY, dZ) | false, false, false | IMU 不直接提供线速度。 |
| 角速度 (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 的速度,噪声通常较大。 |
-
- 过程噪声协方差 (Process Noise Covariance)
process_noise_covariance 定义了系统预测模型的“不确定性”。
- 这是一个对角矩阵,分别对应
[x, y, z, r, p, y, dx, dy, dz, dr, dp, dy, ddx, ddy, ddz]。 - 数值含义:数值越 大,表示系统越 不信任 运动模型的预测(允许状态发生突变);数值越 小,表示系统认为运动是平滑的,更 信任 预测值。
| 索引 | 对应状态 | 值 | 分析 |
|---|---|---|---|
| 0, 1 | X, Y 位置 | 1.0 | 允许水平位置有一定的预测误差。 |
| 2 | Z 位置 | 10.0 | 非常大。表示对 Z 轴高度的预测非常不确定(允许高度剧烈跳变)。这是为了快速适应 GPS 的高度修正。 |
| 3, 4 | Roll, Pitch | 0.03 | 较小,认为机器人不会发生剧烈的翻滚或俯仰。 |
| 5 | Yaw | 0.1 | 允许航向角有一定的动态变化。 |
| … | … | … | (速度和加速度的噪声通常根据传感器精度设定) |
| 14 | Z 加速度 | 0.015 | 相对较小,平滑 Z 轴加速度。 |
配置的内容还是比较多的,同样的我们来总结一下输入输出.
| 方向 | 默认话题名 (Topic) | 消息类型 (Type) | 描述 (Description) |
|---|---|---|---|
| Input | odometry/gps | nav_msgs/Odometry | 来自 navsat_transform_node。提供绝对的位置观测 (X, Y, Z),用于消除累积误差。 |
| Input | imu/data | sensor_msgs/Imu | 来自传感器。提供姿态 (RPY)、角速度和线加速度。用于高频状态预测。 |
| Input | odometry/wheel | nav_msgs/Odometry | (可选) 轮式里程计。提供局部速度或位置增量。 |
| Output | odometry/filtered | nav_msgs/Odometry | 核心输出。融合后的、平滑且无漂移的全局里程计状态。 |
| Output | /tf | tf2_msgs/TFMessage | 坐标变换。发布 odom -> base_link 或 map -> 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

1万+

被折叠的 条评论
为什么被折叠?



