4、激光雷达：2D/3D激光雷达原理、点云数据基础

好，咱们进入第四讲。这一章聊的是激光雷达，也就是我们无人机在GPS拒止环境下的“眼睛”。说实话，在室内、隧道、或者密林里飞，GPS信号一丢，很多传感器都抓瞎，但激光雷达不会。它靠的是自己发射激光，自己接收回波，完全不受外界光照和无线电干扰。我个人觉得，这是目前最可靠的自主导航传感器之一。

4.1 2D激光雷达：原理与局限

先说说2D激光雷达。这东西原理其实不复杂：一个激光发射器，一个旋转的反射镜，或者整个模块旋转。它在一个平面上扫描，测量距离。你想想看，就像拿个手电筒在黑屋子里转一圈，根据光打到墙上的时间判断距离。

核心原理就是飞行时间法（ToF）。发射一束激光，打到物体上反射回来，接收器捕捉到回波。时间差乘以光速，再除以2，就是距离。公式很简单：

距离 = (光速 × 飞行时间) / 2

嗯，这里要注意，光速是3×10⁸ m/s，时间精度要求非常高。1纳秒的误差，对应15厘米的距离偏差。所以2D激光雷达的测距精度，很大程度上取决于计时电路的精度。

我在项目中遇到过一个问题：一台2D激光雷达在室内测距很准，拿到室外阳光下就飘了。为什么？因为太阳光里有大量红外成分，会干扰接收器。后来我加了窄带滤光片，只让激光波长通过，问题才解决。这个坑，大家以后做室外部署时一定要留意。

2D激光雷达的局限很明显：它只能感知一个平面。无人机在三维空间里飞行，光靠一个平面数据，根本不够用。比如你飞过一个门框，2D雷达只能看到门框两侧的柱子，看不到门框上沿。所以，对于无人机导航，3D激光雷达才是主流。

4.2 3D激光雷达：多线扫描与固态技术

3D激光雷达，说白了就是让激光在三维空间里扫描。目前主流有两种方式：机械旋转式和固态式。

机械旋转式，就是堆叠多个激光发射器，比如16线、32线、64线甚至128线。每个发射器负责一个垂直角度，整个模块水平旋转。这样一次扫描，就能获得一个三维点云。我最早接触的是Velodyne的16线雷达，那家伙转起来嗡嗡响，但数据是真漂亮。

固态式，没有旋转部件，靠光学相控阵（OPA）或者微机电系统（MEMS）来偏转激光束。优点是体积小、寿命长、抗震动。缺点嘛，目前视场角还比较小，成本也高。我个人习惯，在无人机上优先用固态雷达，因为机械式的旋转部件在剧烈机动时容易出问题。

不管是哪种，核心原理还是ToF。但3D雷达的数据量比2D大得多。一个16线雷达，每秒能产生30万个点。处理这些数据，对嵌入式系统的算力是个考验。

4.3 点云数据基础：坐标、强度与时间戳

点云数据，就是一堆三维点的集合。每个点通常包含三个基本信息：

• 三维坐标 (x, y, z)：以雷达自身为原点，通常x轴朝前，y轴朝左，z轴朝上。注意，不同厂家的坐标系定义可能不同，用之前一定要确认。
• 强度值 (intensity)：回波信号的强度，反映物体表面的反射特性。白色墙面强度高，黑色轮胎强度低。这个值在做地面分割和特征提取时很有用。
• 时间戳 (timestamp)：每个点被采集的时刻。对于运动中的无人机，时间戳至关重要，因为雷达在旋转，无人机也在移动，点云会发生畸变。

我举个例子，一个典型的点云数据包，结构大概是这样：

struct PointXYZI {
    float x;      // 前向距离，单位米
    float y;      // 侧向距离，单位米
    float z;      // 高度，单位米
    float intensity; // 强度值，0-255
    double timestamp; // 时间戳，单位秒
};

嗯，这里要提醒一下。很多初学者拿到点云数据，直接就用。结果发现SLAM建图总是飘。为什么？因为忽略了运动畸变。雷达在旋转时，无人机也在飞。比如雷达转一圈需要0.1秒，这0.1秒内无人机已经移动了0.5米。那么这一圈点云里，不同角度采集的点，对应的无人机位姿是不同的。如果不做运动补偿，点云就是扭曲的。

4.4 点云预处理：滤波与下采样

原始点云数据量太大，直接做SLAM，嵌入式芯片扛不住。所以必须先做预处理。我常用的步骤有三个：

1. 直通滤波：根据距离范围，去掉太远或太近的点。比如我只关心30米以内的障碍物，30米外的点直接扔掉。这能减少一半以上的数据量。
2. 体素滤波：把空间划分成小立方体（体素），每个体素内只保留一个点（通常是重心点）。体素边长设0.1米，数据量能减少到原来的十分之一。我习惯用PCL库里的VoxelGrid滤波器。
3. 离群点移除：有些点明显是噪声，比如孤立的、周围没有邻居的点。用统计滤波，计算每个点到邻居的平均距离，如果偏离均值太多，就认为是离群点，删掉。

代码示例（基于PCL库）：

// 体素滤波
pcl::VoxelGrid<pcl::PointXYZI> voxel_filter;
voxel_filter.setInputCloud(cloud);
voxel_filter.setLeafSize(0.1f, 0.1f, 0.1f); // 体素边长10cm
voxel_filter.filter(*cloud_filtered);

// 统计滤波移除离群点
pcl::StatisticalOutlierRemoval<pcl::PointXYZI> sor;
sor.setInputCloud(cloud_filtered);
sor.setMeanK(50);          // 每个点考虑50个邻居
sor.setStddevMulThresh(1.0); // 标准差倍数阈值
sor.filter(*cloud_clean);

我在项目里踩过一个坑：体素滤波的体素大小设得太小，比如1厘米，结果数据量没减多少，SLAM还是卡。设得太大，比如50厘米，特征点都被抹平了，定位精度下降。后来我总结出一个经验：体素大小设为激光雷达角分辨率的2-3倍，效果最好。

4.5 从点云到特征：为什么需要特征提取

点云预处理完了，还是几万个点。直接拿这些点做匹配，计算量太大。所以我们需要提取特征。什么是特征？就是那些在空间中具有显著几何特性的点，比如角点、边缘点、平面点。

我常用的特征提取方法是基于曲率和法向量。计算每个点的局部曲率，曲率大的点可能是边缘或角点，曲率小的点可能是平面上的点。具体做法：

• 对每个点，找到它周围的k个邻居（比如k=10）
• 计算这些邻居的协方差矩阵
• 协方差矩阵的特征值，反映了点云在三个方向上的分布
• 如果最大特征值远大于其他两个，说明是线状特征（边缘）
• 如果最小特征值远小于其他两个，说明是面状特征（平面）
• 如果三个特征值差不多，说明是散乱点（可能是树叶、草丛）

为什么要区分这些？因为在SLAM中，平面特征适合做匹配，边缘特征适合做约束。我做过一个项目，在全是树叶的树林里飞，点云全是散乱点，特征提取不出来，SLAM直接崩了。后来我加了强度阈值，只保留树干和地面的点，才勉强跑起来。