0.示意图

1.1 原理：它解决什么问题

2D-SLAM 里常见输入是：

/scan    LiDAR 数据，常见 10Hz
/odom    轮速里程计，常见 30Hz / 50Hz
/imu     IMU 数据，常见 100Hz / 200Hz
/tf      坐标变换，常见 30Hz / 50Hz

这些传感器的频率不同，数据到达 ROS2 节点的时间也不同。SLAM 处理一帧 /scan 时，真正需要的是：

这一帧 scan 采集时刻对应的 odom
这一帧 scan 采集时刻对应的 imu
这一帧 scan 采集时刻对应的 tf

所以同步的核心是：

以 header.stamp 表示的“采集时间”为准，而不是以程序收到消息的时间为准。

如果时间没同步，典型表现是：

转弯时地图重影
点云在 RViz 里甩动
scan matching 初值偏差大
定位跳变
TF extrapolation into future / past
bag 回放效果和现场不一致

---

1.2 经典方案一：硬件同步，最可靠

硬件同步是工业机器人、多传感器融合里最推荐的方案。它的核心思想是：不要让每个传感器各用各的时间，而是让所有传感器共享同一个时间基准。

常见硬件同步方式有三种：

PPS 秒脉冲同步
Trigger 外部触发同步
PTP 精密时间协议同步

---

1.3 PPS 同步具体怎么做

PPS 是 Pulse Per Second，也就是每秒一个精准脉冲。它常来自 GNSS 模块、时间同步模块或者主控板。PPS 的作用是告诉所有设备：

这一刻是整秒边界。

具体做法是：

1. 选一个主时间源
   例如 GNSS 模块、时间同步板、主控 MCU、工控机时间模块。

2. 主时间源输出 PPS 信号
   PPS 是一根硬件线，每秒产生一个上升沿或下降沿。

3. 把 PPS 接到 LiDAR / IMU / 主控采集板的同步输入脚
   这些设备如果支持 PPS，会根据 PPS 校准自己的内部时钟。

4. 同时给设备发送绝对时间信息
   PPS 只告诉“秒边界”，还需要串口、以太网或配置协议告诉当前是第几秒。

5. 传感器内部用统一时间打时间戳
   LiDAR 输出 scan 时，header.stamp 就来自这个统一时间；
   IMU 输出 imu 时，header.stamp 也来自同一个时间基准。

6. ROS2 驱动读取设备时间戳，不要随便用 node->now()
   驱动应该把设备内部时间转换成 ROS2 message 的 header.stamp。

硬件连接概念：

GNSS / 时间同步模块
        │
        ├── PPS → LiDAR
        ├── PPS → IMU
        ├── PPS → 主控采集板
        └── 时间信息 → 各传感器 / 工控机

ROS2 驱动里应该这样做：

scan.header.stamp = lidar_device_timestamp;
imu.header.stamp  = imu_device_timestamp;

不推荐这样：

scan.header.stamp = node->now();

因为 node->now() 是驱动收到数据的时间，不一定是传感器真实采集时间。

PPS 同步完成后，要验证：

ros2 topic echo /scan --once
ros2 topic echo /imu --once
ros2 topic echo /odom --once

看三个 topic 的 header.stamp 是否在同一时间体系下。再录 bag，看高速转弯时点云是否明显减少甩动。

---

1.4 Trigger 外部触发同步具体怎么做

Trigger 更适合“让传感器同时采集”。比如主控板每 100ms 发一个触发信号，LiDAR 或相机收到触发后立即采集。

流程是：

1. 主控板产生固定频率触发信号
   例如 10Hz，也就是每 100ms 触发一次。

2. 触发线接到传感器 Trigger Input
   LiDAR / 相机 / 采集模块支持外触发时，可以接入这个信号。

3. 传感器收到触发信号后采集数据
   每一帧数据的采集时刻由触发信号决定。

4. 主控记录触发时间
   每次发 trigger 时，主控记录 timestamp。

5. 驱动发布 ROS2 message 时，把触发时间写入 header.stamp

结构：

主控 MCU / 工控机 GPIO
        │
        ├── Trigger → LiDAR
        ├── Trigger → 相机
        └── Trigger → 采集板

这种方式的好处是：传感器采集动作本身被统一控制。
缺点是：不是所有 2D LiDAR 都支持外部 trigger。如果雷达不支持，就只能做 PPS、PTP 或软件同步。

---

1.5 PTP 同步具体怎么做

PTP 是 Precision Time Protocol，常用于以太网设备。它的目标是让网络里的设备时钟对齐。

适合场景：

以太网 LiDAR
工控机
多设备通过交换机连接
设备支持 PTP

典型架构：

PTP Grandmaster Clock
        │
   PTP 交换机
        │
 ├── 工控机
 ├── 以太网 LiDAR
 └── 其他传感器

具体做法：

1. 网络中选一个 PTP 主时钟
   可以是 GNSS 时间服务器，也可以是工控机，也可以是专门的 PTP Grandmaster。

2. 所有设备连接到支持 PTP 的交换机
   普通交换机也能用，但精度可能不如 PTP 交换机。

3. 工控机上启动 PTP 同步服务
   Linux 下常用 linuxptp。

4. 传感器配置为使用 PTP 时间
   例如 LiDAR 配置里选择 PTP / IEEE1588 时间源。

5. ROS2 驱动读取传感器自带时间戳
   发布 /scan 时使用传感器时间，不用接收时间。

Linux 上常见命令形式是：

sudo ptp4l -i eth0 -m
sudo phc2sys -s eth0 -c CLOCK_REALTIME -w -m

含义大概是：

ptp4l：让网卡硬件时钟参与 PTP 同步
phc2sys：把网卡硬件时钟同步到系统时钟

同步后再检查：

timedatectl
ros2 topic echo /scan --once
ros2 topic echo /imu --once

PTP 的优势是不用额外 PPS 线，适合以太网设备；缺点是设备、网卡、交换机都要支持得比较好。

---

1.6 经典方案二：软件缓存 + 插值同步

如果没有硬件同步，工程里最常用的是软件同步。核心是：

高频 odom / imu 先缓存
低频 scan 到来时，用 scan.header.stamp 去缓存里找对应时刻的数据

具体流程：

1. odom 到来时，放进 odom_buffer
2. imu 到来时，放进 imu_buffer
3. scan 到来时，取 t_scan = scan.header.stamp
4. 在 odom_buffer 中找 t_scan 前后两帧 odom
5. 对 x、y、yaw 做插值
6. 在 imu_buffer 中找 t_scan 附近的 imu
7. 查询 t_scan 时刻的 TF
8. 用这些对齐后的数据处理 scan

odom 插值示例：

t_scan = 10.000s
odom_1 = 9.980s
odom_2 = 10.020s

插值比例：

α = (t_scan - t1) / (t2 - t1)

位置：

x = x1 + α * (x2 - x1)
y = y1 + α * (y2 - y1)

角度：

yaw = yaw1 + α * normalize_angle(yaw2 - yaw1)

实际代码结构通常是：

void scanCallback(const LaserScan& scan)
{
    auto t_scan = scan.header.stamp;

    OdomState odom_at_scan;
    bool ok = interpolateOdom(odom_buffer, t_scan, odom_at_scan);

    if (!ok) {
        // 没有对应时刻 odom，丢弃或降级
        return;
    }

    // 查询 t_scan 时刻 base_link -> laser
    // 查询 t_scan 时刻 odom -> base_link
    // 然后做 scan matching
}

这套方法是实际 2D-SLAM 最常见的软件同步方式。

---

1.7 经典方案三：设置时间容忍窗口和超时策略

不能因为想同步就拿相差很远的数据硬配。必须设置时间阈值。

常见配置：

scan - odom 最大时间差：20ms ~ 50ms
scan - imu 最大时间差：5ms ~ 20ms
TF 查询等待时间：50ms ~ 200ms

处理规则：

如果 odom 距离 scan 时间太远：
    本帧不用 odom 初值，或者直接丢弃本帧 scan

如果 imu 距离 scan 时间太远：
    本帧不做 imu 补偿

如果 TF 查不到：
    丢弃本帧 scan，不要硬算

如果连续多帧查不到：
    上报同步异常或进入降级模式

伪代码：

double dt = abs((t_scan - odom_time).seconds());

if (dt > 0.05) {
    RCLCPP_WARN(logger, "odom and scan not synchronized");
    return;
}

这个策略能防止“错误同步”比“不同步”更严重。

---

1.8 经典方案四：bag 回放时统一 ROS 时间

bag 回放也属于时间同步问题。

正确操作：

ros2 bag play slam_debug_001 --clock

SLAM 节点设置：

ros2 param set /slam_node use_sim_time true

RViz 也设置：

ros2 param set /rviz use_sim_time true

launch 里写：

parameters=[
    {'use_sim_time': True}
]

如果不这样做，bag 里的数据是历史时间，SLAM 节点却用当前系统时间，TF 查询必然容易错。

---

2. 数据异常 / 丢帧 / 噪声处理

2.1 原理：解决“数据脏了、断了、跳了怎么办”

真实 LiDAR 可能出现：

nan / inf
超量程点
反光点
玻璃点
动态障碍物点
单点尖峰
连续丢帧

odom 可能出现：

编码器跳变
轮子打滑
机器人没动但 odom 漂
直走 yaw 偏

IMU 可能出现：

震动噪声
零偏漂移
坐标轴方向错误

这些异常如果直接进入 SLAM，会造成地图重影、定位跳变、匹配失败。

---

2.2 经典方案一：LaserScan 前处理

每帧 /scan 进入 SLAM 前先清洗。

具体处理：

1. 检查 frame_id 是否为空
2. 检查 ranges 是否为空
3. 检查 angle_increment 是否正常
4. 逐个过滤 nan / inf
5. 过滤小于 range_min 的点
6. 过滤大于 range_max 的点
7. 过滤机器人自身 footprint 内的点
8. 可选：屏蔽雷达被车体遮挡的角度范围

处理逻辑：

for each range r:
    if r is nan or inf:
        mark invalid

    if r < range_min or r > range_max:
        mark invalid

    theta = angle_min + i * angle_increment
    x = r * cos(theta)
    y = r * sin(theta)

    if point inside robot footprint:
        mark invalid

机器人自身过滤很重要。例如雷达扫到车体外壳，如果不滤掉，SLAM 会把机器人自己建进地图。

---

2.3 经典方案二：中值滤波 + 离群点过滤

单点尖峰用中值滤波处理。

例如窗口大小为 5：

r_filtered[i] = median(r[i-2], r[i-1], r[i], r[i+1], r[i+2])

适合：

偶发反光点
单束激光跳变
局部测距尖峰

离群点判断：

如果 r_i 和 r_{i-1} 差很大，
并且 r_i 和 r_{i+1} 差也很大，
则认为 r_i 是孤立异常点。

处理方式：

直接丢弃
或者替换为邻域中值
或者降低该点匹配权重

注意窗口不要太大。窗口太大会把墙角、货架边缘这种真实结构抹平。

---

2.4 经典方案三：丢帧检测 + 降级状态机

假设 LiDAR 是 10Hz，正常周期是 0.1s。可以设置：

dt > 0.2s：认为丢帧
连续 3 次丢帧：认为雷达不稳定
超过 1s 没有 scan：认为 LiDAR 超时

处理流程：

每收到一帧 scan：
    计算 current_stamp - last_stamp
    如果超过阈值，lost_count++
    否则 lost_count 清零

如果 lost_count = 1：
    跳过当前 LiDAR 更新

如果 lost_count >= 3：
    进入 LiDAR 不稳定状态

如果超过 1s 没有 scan：
    停止高置信定位输出
    上报异常
    必要时停车

伪代码：

double dt = current_scan_time - last_scan_time;

if (dt > 2.0 * expected_period) {
    lost_scan_count++;
} else {
    lost_scan_count = 0;
}

if (lost_scan_count >= 3) {
    lidar_status = UNSTABLE;
}

这套机制很重要，因为不能拿旧 scan 继续做地图更新。旧 scan 对应的是过去的位置，机器人已经移动了，继续使用会把地图拉坏。

---

2.5 经典方案四：鲁棒匹配，剔除大残差点

动态障碍物、人、叉车、反光点，通常和静态地图不一致。匹配时可以计算点到地图的残差：

把 scan 点转到 map 坐标系
查最近障碍物 / 最近 occupied cell
计算距离 residual

处理规则：

residual < 0.05m：正常点，参与匹配
0.05m ~ 0.20m：降低权重
> 0.20m：认为是异常点或动态点，剔除

这样做的效果是：

静态墙面点参与定位
动态人腿点不影响定位
反光异常点不把位姿拉偏

一帧匹配完成后，还要判断整体质量：

有效点比例是否足够
平均残差是否过大
本次位姿修正是否过大
匹配是否收敛

如果质量差：

不更新 map -> odom
不写地图
只保留 odom 预测
降低定位置信度
等待下一帧

---

3. 传感器标定

3.1 原理：解决“传感器装在哪里、朝哪里”的问题

LiDAR 数据在 laser 坐标系下，机器人底盘是 base_link，地图是 map。SLAM 必须知道：

base_link -> laser
base_link -> imu_link
odom -> base_link
map -> odom

如果外参错了，点云会被投错位置。算法本身没问题，也会出现地图歪、定位偏、转弯重影。

---

3.2 经典方案一：LiDAR 外参测量 + static TF 发布

先确定 base_link 原点，一般取机器人底盘中心：

x 向前
y 向左
z 向上

然后测量 LiDAR 相对底盘中心的位置：

x：雷达在前方多少米
y：雷达在左侧多少米
z：雷达高度
yaw：雷达正方向和车头方向夹角

例如：

x = 0.20m
y = 0.00m
z = 0.15m
yaw = 0

发布静态 TF：

ros2 run tf2_ros static_transform_publisher \
0.20 0.00 0.15 0 0 0 base_link laser

检查：

ros2 run tf2_ros tf2_echo base_link laser
ros2 run tf2_tools view_frames

如果 laser 没挂在 base_link 下面，说明 TF 没发布成功或 frame 名字不一致。

---

3.3 经典方案二：RViz 直墙验证 + 原地转圈验证

打开 RViz：

rviz2

设置：

Fixed Frame = base_link
添加 LaserScan：/scan
添加 TF

验证方法：

1. 机器人正对一堵直墙
   scan 显示的墙应该在机器人正前方。

2. 机器人平行靠近墙
   scan 的墙线应该和机器人运动方向稳定平行。

3. 机器人原地慢速转圈
   墙体不应该出现明显双层、甩动、偏心旋转。

根据现象判断：

墙整体有固定角度偏差：
    LiDAR yaw 外参可能错

原地旋转时点云绕偏心点甩：
    LiDAR x / y 外参可能错

低速正常，转弯重影：
    可能是 yaw 外参错，也可能是时间延迟

yaw 微调时注意单位是弧度：

1° = 0.01745 rad
3° = 0.05236 rad
5° = 0.08726 rad

例如 yaw 修正 3°：

ros2 run tf2_ros static_transform_publisher \
0.20 0.00 0.15 0 0 0.05236 base_link laser

---

3.4 经典方案三：底盘 odom 标定

odom 是 scan matching 的初值。如果 odom 很差，SLAM 前端容易匹配失败。

差速底盘主要标：

wheel_radius：轮半径
wheel_separation：轮距
encoder_resolution：编码器分辨率
gear_ratio：减速比

直线标定 wheel_radius

操作：

1. 地上量出 1m 或 2m
2. 让机器人低速直走
3. 记录 odom 显示走了多少
4. 多测几次取平均

修正公式：

new_wheel_radius = old_wheel_radius × actual_distance / odom_distance

例子：

实际走 1.00m
odom 显示 0.95m
old_wheel_radius = 0.05m

new_wheel_radius = 0.05 × 1.00 / 0.95 = 0.05263m

原地旋转标定 wheel_separation

操作：

1. 让机器人原地转 360°
2. 用地面标记确认实际转了一圈
3. 看 odom yaw 显示多少

如果实际 360°，odom 只显示 330°，说明旋转估计偏小，需要调整轮距参数。

修正思路：

new_wheel_separation = old_wheel_separation × odom_yaw / actual_yaw

不同底盘运动学实现可能方向不同，所以改完要重新验证。

---

3.5 经典方案四：IMU 坐标方向验证

IMU 主要检查方向，不只是位置。

ROS 常用约定：

base_link:
x 向前
y 向左
z 向上

验证操作：

ros2 topic echo /imu --once
ros2 topic hz /imu

然后做动作：

机器人左转：
    angular_velocity.z 应该按 ROS 约定变化

机器人向前加速：
    linear_acceleration.x 应该有对应变化

机器人左右倾斜：
    roll / pitch 方向应该合理

如果 IMU 方向反了，处理方式只有一个原则：在一个地方修，不要多个地方重复修。

可以选择：

修改 IMU 驱动轴映射
或者修改 base_link -> imu_link 的 TF
或者修改融合节点参数

不要驱动里修一次，TF 里又修一次，否则容易修反。

---

4. 数据对齐

4.1 原理：解决“数据能不能放到同一个坐标和语义体系里”

数据对齐包括：

时间对齐：是不是同一时刻
坐标对齐：是不是能转到同一坐标系
频率对齐：不同频率怎么配合
语义对齐：topic、frame_id、单位、方向是否一致

2D-SLAM 最终要把 scan 点从 laser 转到 map：

T_map_laser = T_map_odom × T_odom_base × T_base_laser

任何一段错，点云都会放错位置。

---

4.2 经典方案一：统一 TF 树

目标结构：

map
 └── odom
      └── base_link
           ├── laser
           └── imu_link

检查：

ros2 run tf2_tools view_frames
ros2 run tf2_ros tf2_echo base_link laser
ros2 run tf2_ros tf2_echo odom base_link
ros2 run tf2_ros tf2_echo map odom

判断：

查不到 base_link -> laser：
    静态外参没发布

查不到 odom -> base_link：
    底盘 odom 没发布 TF

查不到 map -> odom：
    SLAM 没正常输出定位修正

laser 单独孤立：
    scan.header.frame_id 和 TF 名字不一致

---

4.3 经典方案二：统一 frame_id

逐个检查：

ros2 topic echo /scan --once
ros2 topic echo /odom --once
ros2 topic echo /imu --once
ros2 topic echo /map --once

应该类似：

scan.header.frame_id = laser
odom.header.frame_id = odom
odom.child_frame_id = base_link
imu.header.frame_id = imu_link
map.header.frame_id = map

常见错误：

/scan 写 laser_frame，TF 里叫 laser
odom child_frame_id 写 base_footprint，SLAM 参数写 base_link
map frame 写 map_2d，RViz Fixed Frame 选 map

解决方式：

优先改驱动、URDF、SLAM 参数里的 frame 配置
不要靠大量 remap 硬绕
整个系统保持一套命名

---

4.4 经典方案三：统一单位和方向

ROS 里默认：

距离：m
角度：rad
线速度：m/s
角速度：rad/s

机器人坐标：

x 向前
y 向左
z 向上

验证：

机器人向前走：
    odom x 应该增加

机器人左转：
    yaw / angular_velocity.z 应该按约定变化

LiDAR 正前方有障碍：
    点应该落在 base_link 的 x 正方向

如果方向反了，不要在 SLAM 内部临时补。应该回到驱动、URDF、TF 或底盘运动学里修正。

---

4.5 经典方案四：以 LiDAR 帧为主频率对齐

实际 2D-SLAM 通常这样跑：

LiDAR 每来一帧，触发一次 SLAM 更新
odom 高频缓存，用作运动初值
IMU 高频缓存，用作角速度 / 姿态辅助
TF buffer 提供坐标变换

也就是：

/scan 是主更新节拍
/odom 和 /imu 是辅助预测数据

不要让每条 IMU 都触发一次 scan matching，也不要把低频 scan 强行和高频 odom 一一绑定。

5. 总结

这些岗位要求里说的“多传感器时间同步、数据异常 / 丢帧 / 噪声处理、传感器标定、时间同步、数据对齐”，本质是在考察你能不能处理真实机器人 SLAM 的工程问题。真正项目里，算法本身只是其中一部分。就算 scan matching、ICP、粒子滤波、图优化写得没问题，只要数据时间没同步、外参没标好、TF 树断了、LiDAR 数据有脏点、odom 初值很差，最后照样会表现成定位不准、地图重影、轨迹跳变、转弯漂移。

多传感器时间同步里面，最可靠的是硬件同步。PPS 是给传感器一个统一的秒脉冲，LiDAR、IMU、主控板都根据同一个 PPS 校准自己的时钟；Trigger 是由主控统一发采集触发信号，让传感器在同一个节拍下采集；PTP 是通过以太网让工控机、LiDAR、其他网络设备的时钟对齐。硬件同步的关键不是只接一根线，而是要让传感器最终输出的数据时间戳来自统一时间源，然后 ROS2 驱动把这个设备时间正确写入 header.stamp。如果驱动还是用收到数据时的 node->now()，硬件同步效果就浪费了。没有硬件同步时，则要做软件缓存和插值：以 /scan 的 header.stamp 为主，从 odom / imu 缓存中找对应时刻的数据，必要时对 odom 的 x、y、yaw 做插值，同时设置时间容忍窗口，超过窗口就丢弃或降级，不能强行融合。

数据异常、丢帧和噪声处理，核心是不要让脏数据直接进入 SLAM。LiDAR 的 ranges[] 要过滤 nan、inf、超量程点、机器人自身结构点；单点尖峰可以用中值滤波或邻域离群点过滤；动态障碍物点可以通过点到地图的残差门限剔除，不让人、叉车、临时货物把定位拉偏或写进静态地图。丢帧要做状态机，不是简单打印一个 warning。比如 10Hz 雷达正常 0.1s 一帧，如果超过 0.2s 没有新帧，就认为丢帧；连续丢帧就降低定位置信度，只保留 odom 预测；长时间无 scan 就报警或停车。匹配时还要看有效点比例、平均残差、位姿修正量，如果一帧匹配质量差，就不要更新 map -> odom，否则很容易把定位拉飞。

传感器标定解决的是空间关系。最重要的是 base_link -> laser，它决定 LiDAR 点能不能正确转到底盘坐标系；其次是 base_link -> imu_link，它决定 IMU 的角速度和加速度方向是否符合机器人坐标约定；再往下是底盘 odom 的轮径、轮距、编码器分辨率等参数。LiDAR 外参不是写个 static TF 就完事了，还要用 RViz 验证：面对直墙看墙是否在正前方，沿墙移动看墙线是否稳定，原地转圈看墙体是否双层或甩动。底盘 odom 也要实际标定：直走 1m 看 odom 距离，原地转 360° 看 yaw 是否准确。IMU 则要通过左转、前进加速、左右倾斜来验证轴方向。方向错、外参错、轮距错，最后都会反映到 SLAM 匹配初值和地图质量上。

数据对齐则是把时间、坐标、频率、语义统一起来。系统里应该有清晰的 TF 树：map -> odom -> base_link -> laser / imu_link。每个 topic 的 frame_id 要和 TF 里一致，/scan 不能一会儿叫 laser，一会儿叫 laser_frame；odom.child_frame_id 要和 SLAM 参数里的 base frame 对上；单位要统一成米、弧度、m/s、rad/s；方向要符合 ROS 约定。频率上，2D-SLAM 通常以 LiDAR 为主节拍，每来一帧 scan 做一次更新，odom 和 IMU 作为高频缓存辅助预测。这样整条链路才是稳定的：数据按正确时间进入，按正确坐标转换，经过过滤和质量判断，再参与匹配和地图更新。

所以实际排查时，顺序应该是：先看 /scan /odom /imu 有没有数据、频率稳不稳、时间戳正不正常；再查 TF 树是否完整，base_link -> laser、odom -> base_link、map -> odom 是否存在；再看外参和底盘参数是否合理；然后录 bag，用 --clock 和 use_sim_time=true 离线复现；最后才去调 scan matching 搜索窗口、残差阈值、粒子数、图优化权重这些算法参数。真正的 2D-SLAM 工程能力，就是能从数据采集、时间同步、异常过滤、外参标定、TF 对齐、bag 复现这一整条链路里快速定位问题，而不是只盯着算法公式。