第三章 传感器基础与数据预处理
各位同学，今天我们来聊聊传感器。说实话，做机器人路径规划这么多年，我踩过最大的坑，往往不是算法本身，而是传感器数据没处理好。你想想看，再牛的算法，喂进去的是垃圾数据，吐出来的只能是更垃圾的结果。
这一章，我会把激光雷达、摄像头、IMU、GPS这几个常用传感器的原理讲清楚，然后重点说说数据怎么滤波、怎么对齐时间戳。嗯，这些都是我实际项目中反复折腾过的东西。
3.1 四大传感器，各有什么脾气？
3.1.1 激光雷达（LiDAR）
激光雷达的原理其实很简单——发射激光束，测量反射回来的时间，算出距离。我习惯把它叫做「机器人的眼睛」，但它跟人眼不一样，它更擅长测距，而不是认东西。
- 工作原理：TOF（飞行时间法）或三角测距法
- 常见类型：单线（2D）、多线（3D）、固态
- 输出数据：点云（Point Cloud），每个点包含(x,y,z)坐标和反射强度
- 典型频率：10Hz~20Hz
重要：激光雷达对反射材质敏感。黑色物体、玻璃、水面，这些都会导致数据异常。我在做园区无人车时，遇到过雨天积水反射导致误检测的情况，折腾了两天才定位到问题。
3.1.2 摄像头
摄像头提供的是最丰富的环境信息——颜色、纹理、语义。但它有个致命弱点：没有深度信息（除非双目或RGB-D）。说白了，单目摄像头就像你闭上一只眼看世界，能认出东西，但不知道多远。
- 工作原理：CMOS/CCD感光元件，将光信号转为电信号
- 常见类型：单目、双目、RGB-D（深度相机）
- 输出数据：图像帧（RGB或灰度），通常为640×480或更高分辨率
- 典型频率：30Hz~60Hz
我的经验：摄像头最怕光照变化。从室内到室外，自动曝光需要时间适应，这期间的数据最好别用。我一般会在代码里加一个「曝光稳定检测」逻辑，等增益稳定了再开始处理。
3.1.3 IMU（惯性测量单元）
IMU是个「闷头干活」的传感器。它不依赖外部信号，只靠自己测量加速度和角速度。但问题在于——它有漂移。你想想看，加速度积分一次得速度，再积分一次得位置，误差会像滚雪球一样越滚越大。
- 工作原理：MEMS微机电系统，测量三轴加速度和三轴角速度
- 输出数据：加速度(ax, ay, az)、角速度(gx, gy, gz)
- 典型频率：100Hz~400Hz（远高于其他传感器）
- 常见问题：零偏、温漂、随机游走
注意：IMU数据不能直接积分使用！必须配合其他传感器做融合。我曾经见过一个新手，直接用IMU积分算位置，跑了10秒就偏了十几米。
3.1.4 GPS（全球定位系统）
GPS提供绝对位置信息，但精度有限。民用GPS在开阔环境下精度约2~5米，有遮挡时可能直接跳到几十米。说白了，GPS只能告诉你「大概在哪」，不能告诉你「精确在哪」。
- 工作原理：卫星三角定位，至少需要4颗卫星
- 输出数据：经纬度、海拔、速度、航向角
- 典型频率：5Hz~10Hz
- 影响因素：多径效应、电离层延迟、卫星几何分布
3.2 数据滤波——卡尔曼滤波入门
为什么要滤波？因为传感器数据都有噪声。你拿激光雷达测同一个点，每次读数都不一样。这时候就需要滤波，把真实信号从噪声中「提纯」出来。
卡尔曼滤波，说白了就是「预测+修正」的循环。我刚开始学的时候觉得公式很复杂，后来发现核心思想就两句话：
- 预测：根据上一时刻的状态，猜一下当前时刻的状态
- 更新：用当前时刻的观测值，修正这个猜测
嗯，就这么简单。但具体实现时，有几个关键点要注意。
3.2.1 卡尔曼滤波的五步公式
我直接给出最常用的离散卡尔曼滤波公式，你们直接套用就行：
// 预测步骤
x_pred = A * x_prev + B * u // 状态预测
P_pred = A * P_prev * A^T + Q // 协方差预测
// 更新步骤
K = P_pred * H^T * (H * P_pred * H^T + R)^(-1) // 卡尔曼增益
x = x_pred + K * (z - H * x_pred) // 状态更新
P = (I - K * H) * P_pred // 协方差更新这里面的参数，我一个个解释：
| 符号 | 含义 | 怎么调？ |
|---|---|---|
| x | 状态向量（比如位置、速度） | 根据你的系统定义 |
| A | 状态转移矩阵 | 运动模型决定，匀速就填[1 dt; 0 1] |
| Q | 过程噪声协方差 | 越大表示对模型越不信任 |
| R | 观测噪声协方差 | 越大表示对传感器越不信任 |
| K | 卡尔曼增益 | 自动计算，不用手动调 |
调参心得：Q和R是卡尔曼滤波里最关键的参数。我一般先固定R（根据传感器手册），然后调Q。Q调大了，滤波结果会更「相信」观测值，响应快但噪声大；Q调小了，结果更平滑但有延迟。具体怎么平衡？看你的应用场景。
3.2.2 一个简单的1D位置滤波例子
假设我们要滤波一个一维位置信号，比如激光雷达测距值。代码长这样：
import numpy as np
class KalmanFilter1D:
def __init__(self):
self.x = 0.0 # 初始位置
self.P = 1.0 # 初始协方差
self.Q = 0.01 # 过程噪声
self.R = 0.1 # 观测噪声
self.A = 1.0 # 状态转移（位置不变）
self.H = 1.0 # 观测矩阵
def predict(self):
self.x = self.A * self.x
self.P = self.A * self.P * self.A + self.Q
def update(self, z):
K = self.P * self.H / (self.H * self.P * self.H + self.R)
self.x = self.x + K * (z - self.H * self.x)
self.P = (1 - K * self.H) * self.P
return self.x
# 使用示例
kf = KalmanFilter1D()
measurements = [1.1, 1.3, 0.9, 1.2, 1.0] # 带噪声的测量值
for z in measurements:
kf.predict()
filtered = kf.update(z)
print(f"测量值: {z:.2f}, 滤波后: {filtered:.2f}")运行一下你会发现，滤波后的值比原始测量值更平滑、更稳定。这就是卡尔曼滤波的威力。
3.3 时间戳同步——一个容易被忽视的大坑
做多传感器融合时，时间戳同步是个大问题。你想想看，激光雷达10Hz，摄像头30Hz，IMU 200Hz，GPS 5Hz——这些数据到达的时间点完全不一样。如果你不做同步，直接拿最近的数据凑合，结果会非常糟糕。
我遇到过最离谱的一次，因为时间戳没对齐，融合出来的位置轨迹像喝醉了酒一样左右摇摆。查了两天才发现，原来是激光雷达和摄像头的时间基准差了50毫秒。
3.3.1 硬件同步 vs 软件同步
| 同步方式 | 原理 | 精度 | 成本 |
|---|---|---|---|
| 硬件同步 | 用PPS信号或触发线，让传感器同时采集 | 微秒级 | 高（需要硬件支持） |
| 软件同步 | 在代码里做时间戳对齐 | 毫秒级 | 低（纯软件实现） |
我个人建议：如果条件允许，优先用硬件同步。精度高，省心。但很多低成本方案只能用软件同步，那就得好好处理了。
3.3.2 软件同步的常用方法
软件同步的核心思路就一个：找到时间上最接近的数据对。具体做法：
- 给每个传感器数据打上统一的时间戳（比如用系统时间）
- 以主传感器（通常是频率最低的那个）的时间为基准
- 对每个主传感器数据，找到其他传感器中时间戳最接近的那个
- 如果时间差超过阈值（比如10ms），就丢弃或插值
def sync_sensors(laser_data, cam_data, max_time_diff=0.01):
"""
激光雷达和摄像头数据同步
laser_data: [(timestamp, pointcloud), ...]
cam_data: [(timestamp, image), ...]
"""
synced_pairs = []
cam_idx = 0
for laser_ts, laser_pc in laser_data:
# 找到时间最接近的摄像头数据
while cam_idx < len(cam_data) - 1:
if abs(cam_data[cam_idx+1][0] - laser_ts) < abs(cam_data[cam_idx][0] - laser_ts):
cam_idx += 1
else:
break
cam_ts, cam_img = cam_data[cam_idx]
time_diff = abs(laser_ts - cam_ts)
if time_diff < max_time_diff:
synced_pairs.append((laser_pc, cam_img, laser_ts))
return synced_pairs避坑指南：我曾经犯过一个错误——直接用ROS的message_filter做同步，没检查时间戳的基准。结果发现激光雷达用的是GPS时间，摄像头用的是系统时间，两者差了十几秒。所以，第一步永远是确认所有传感器的时间基准是否一致。
3.4 本章知识体系总览
为了让大家更直观地理解本章的知识结构，我画了一张流程图：
这张图把本章的核心脉络串起来了。从四个传感器出发，经过数据预处理，再分别做滤波和时间同步，最终输出高质量数据给路径规划用。每一步都环环相扣，缺一不可。
最后提醒一句：传感器数据处理没有银弹。卡尔曼滤波不是万能的，时间同步也不是一次调好就完事了。不同场景、不同传感器组合，参数都得重新调。我做了这么多年，每次换传感器或者换场景，都得重新走一遍调试流程。别嫌麻烦，这是必经之路。
好了，这一章的内容就到这里。传感器基础打牢了，后面讲路径规划算法时，你们会发现轻松很多。