3、传感器数据采集与预处理：激光雷达、超声波、IMU的数据读取与滤波、坐标变换
各位同学，咱们今天聊聊传感器数据采集。说实话，这部分是嵌入式路径规划里最「脏」的活。你算法写得再漂亮，传感器数据一塌糊涂，那全是白搭。我在项目里见过太多人，花三个月调PID，最后发现是IMU的零偏没校准——嗯，这种坑我踩过不止一次。
3.1 激光雷达数据读取与滤波
激光雷达，说白了就是拿激光束扫一圈，测出周围物体的距离。常见的比如RPLIDAR A1、A2，或者思岚的S系列。数据格式通常是极坐标：(角度, 距离)。
读取时要注意什么？我建议你直接用串口DMA接收。为什么？因为激光雷达一秒钟转5到10圈，每圈好几百个点，CPU如果一个个轮询，基本干不了别的了。
// 伪代码：激光雷达数据读取
void lidar_data_callback(uint8_t *buf, uint16_t len) {
// 解析协议帧
if (buf[0] == 0xFA && buf[1] == 0xA0) {
float angle = (buf[2] << 8 | buf[3]) / 64.0f;
float dist = (buf[4] << 8 | buf[5]) / 4.0f;
// 存入环形缓冲区
ringbuf_push(&lidar_buf, angle, dist);
}
}
滤波方面，我个人习惯用中值滤波。为什么不用均值？你想想看，激光雷达偶尔会打到玻璃或者镜面，返回一个离谱的跳变值。均值滤波会把这种异常值「平均」进去，中值滤波直接把它扔掉。
关键点：激光雷达的无效数据（距离为0或极大值）一定要在预处理阶段剔除。我在项目中遇到过，一个无效点导致路径规划算法直接算出一条穿墙的路径——那场面，挺尴尬的。
3.2 超声波传感器数据读取与滤波
超声波，便宜、简单，但坑也多。HC-SR04这种经典模块，原理就是发一个40kHz的脉冲，等回波。测距公式：距离 = 时间 × 声速 / 2。
读取时有个细节：声速受温度影响。25°C时声速约346m/s，0°C时只有331m/s。如果你在冬天和夏天用同一个声速值，误差能到5%。我建议你加个温度补偿：
float speed_of_sound(float temp_c) {
return 331.3f * sqrt(1.0f + temp_c / 273.15f);
}
滤波方面，超声波有个老大难问题——多径反射。超声波打到墙角，弹来弹去，测出来的距离可能比实际远很多。怎么办？
- 时间窗口法：只接收第一个回波，忽略后面的。因为第一个回波通常是直达波。
- 滑动窗口滤波：取最近5次测量的中位数。我曾经试过均值，效果不好，因为异常值太离谱了。
注意：超声波有盲区。HC-SR04的盲区大约是2cm。也就是说，物体离得太近，它测不出来。我在做避障小车时，就因为这个盲区，小车直接怼墙上了——嗯，后来加了红外传感器做互补。
3.3 IMU数据读取与滤波
IMU，惯性测量单元，包含加速度计和陀螺仪。常见的比如MPU6050、ICM-20948。读取方式一般是I2C或SPI。我个人推荐SPI，速度快，而且不容易被其他I2C设备干扰。
IMU的数据有多「脏」？加速度计有振动噪声，陀螺仪有温漂。你直接拿原始数据做积分算姿态，几分钟就飘到天上去。
滤波方案我分两步走：
- 低通滤波：去除高频振动噪声。比如加速度计，车体振动频率通常在几十Hz，而运动频率只有几Hz。用一阶低通：
out = 0.9 * out_prev + 0.1 * in。 - 互补滤波：融合加速度计和陀螺仪的数据。加速度计长期稳定但短期噪声大，陀螺仪短期准确但长期漂移。互补滤波就是取长补短。
// 互补滤波示例
void complementary_filter(float ax, float ay, float az, float gx, float gy, float gz, float dt) {
// 由加速度计计算角度
float acc_pitch = atan2(-ax, sqrt(ay*ay + az*az));
float acc_roll = atan2(ay, az);
// 陀螺仪积分
gyro_pitch += gx * dt;
gyro_roll += gy * dt;
// 融合
float alpha = 0.98f; // 权重
pitch = alpha * (pitch + gx * dt) + (1 - alpha) * acc_pitch;
roll = alpha * (roll + gy * dt) + (1 - alpha) * acc_roll;
}
小技巧：互补滤波的alpha值不是固定的。我习惯在运动剧烈时降低alpha（更信任陀螺仪），静止时提高alpha（更信任加速度计）。这叫自适应互补滤波，效果比固定值好不少。
3.4 坐标变换
传感器装在小车上，但路径规划是在全局坐标系下做的。所以你得把传感器数据从「传感器坐标系」变换到「车体坐标系」，再变换到「全局坐标系」。
举个例子：激光雷达装在车头前方10cm处，它测到一个障碍物在正前方50cm。那这个障碍物相对于车体中心的位置是多少？很简单，平移一下：(0, 50 + 10)。
但如果传感器有旋转呢？比如IMU装在车上，但装歪了5度。那就要用旋转矩阵：
// 二维旋转矩阵
x_global = x_local * cos(theta) - y_local * sin(theta);
y_global = x_local * sin(theta) + y_local * cos(theta);
三维情况更复杂，要用四元数或欧拉角。我个人偏好四元数，因为它没有万向锁问题。你想想看，欧拉角在俯仰90度时，横滚和偏航就分不清了——这在无人机里可是要命的。
核心原则：所有传感器数据，最终都要统一到同一个坐标系下。我见过一个项目，激光雷达用极坐标，超声波用直角坐标，IMU用四元数，最后融合时坐标系没对齐，路径规划出来的轨迹全是歪的。排查了三天才发现是坐标变换矩阵写反了。
3.5 多传感器时间同步
最后提一个容易被忽略的问题：时间同步。激光雷达10Hz，超声波20Hz，IMU 100Hz。你拿到的数据，可能激光雷达是100ms前的，超声波是50ms前的，IMU是10ms前的。如果不做时间对齐，融合出来的结果就是「时空错乱」。
我常用的方法：
- 硬件时间戳：每个传感器数据到达时，记录当前系统时间（微秒级）。
- 插值对齐：以最慢的传感器（通常是激光雷达）为基准，对其他传感器数据做线性插值，对齐到同一时刻。
| 传感器 | 典型频率 | 延迟 | 对齐方式 |
|---|---|---|---|
| 激光雷达 | 5-15 Hz | 10-50 ms | 基准 |
| 超声波 | 10-40 Hz | 5-20 ms | 线性插值 |
| IMU | 50-200 Hz | 1-5 ms | 最近邻或插值 |
好了，传感器数据采集与预处理就讲到这里。下一章咱们聊聊路径规划的核心算法——A*和Dijkstra。到时候我会拿一个实际项目里的地图，带你们一步步调参。下课。