4. 环境感知：点云处理基础、地面分割、障碍物检测、动态物体过滤

环境感知，说白了就是AMR的「眼睛」和「耳朵」。没有它，机器人就是个盲人。我做了这么多年机器人，最深的体会就是：感知做不好，后面规划和控制再牛也白搭。

这一章，我们聚焦激光雷达的点云处理。嗯，别被「点云」这词吓到，其实就是一堆三维坐标点。我们要做的，就是从这堆点里找出地面、障碍物，还得把动态的干扰物过滤掉。

4.1 点云处理基础

点云数据长什么样？说白了就是N个(x, y, z)坐标的集合。我习惯用PCL库来处理，但自己手撸基础算法也很重要。

先看一个最简单的点云读取和可视化代码：

# Python + Open3D 示例
import open3d as o3d
import numpy as np

# 生成随机点云
points = np.random.rand(1000, 3) * 10
pcd = o3d.geometry.PointCloud()
pcd.points = o3d.utility.Vector3dVector(points)

# 可视化
o3d.visualization.draw_geometries([pcd])

实际项目中，点云数据量很大。16线激光雷达每秒能产生30万个点。你想想看，如果每个点都处理，CPU直接冒烟。所以第一步永远是降采样。

# 体素降采样
voxel_size = 0.05
downsampled_pcd = pcd.voxel_down_sample(voxel_size)

还有一个坑：离群点过滤。我在项目里遇到过，激光打到玻璃上会产生飞点，这些点会严重干扰后续处理。用统计滤波可以干掉它们：

# 统计滤波去除离群点
cl, ind = downsampled_pcd.remove_statistical_outlier(
    nb_neighbors=20, 
    std_ratio=2.0
)
filtered_pcd = downsampled_pcd.select_by_index(ind)

4.2 地面分割

地面分割是环境感知的第一步。为什么要做？因为地面点占了点云的一半以上，不剔除的话，障碍物检测根本没法做。

我最早用RANSAC平面拟合来做地面分割。说白了就是随机选三个点拟合平面，然后算其他点到这个平面的距离。距离小于阈值的，就认为是地面点。

# RANSAC平面拟合
from sklearn.linear_model import RANSACRegressor
import numpy as np

# 假设points是Nx3的数组
# 只取z轴作为目标值
X = points[:, :2]  # x, y
y = points[:, 2]   # z

ransac = RANSACRegressor(
    residual_threshold=0.1,  # 距离阈值10cm
    max_trials=100
)
ransac.fit(X, y)
inlier_mask = ransac.inlier_mask_

但RANSAC有个问题——速度慢。每帧都要迭代100次，16线雷达还能扛，要是64线就吃力了。

后来我改用基于角度的方法。原理很简单：激光雷达扫描时，相邻两个点如果都在地面上，它们与激光雷达的连线夹角应该很小。如果夹角突然变大，说明遇到了障碍物。

4.3 障碍物检测

去掉地面后，剩下的点就是障碍物候选。但这时候点还是散的，需要聚类成一个个物体。

我最常用的是DBSCAN聚类。它不需要预先指定类别数，而且能处理任意形状的簇。核心参数就两个：

参数	含义	我的推荐值
eps	邻域半径	0.3m ~ 0.5m
min_points	最小点数	5 ~ 10

# DBSCAN聚类
from sklearn.cluster import DBSCAN

clustering = DBSCAN(
    eps=0.3, 
    min_samples=5
).fit(obstacle_points)

labels = clustering.labels_
# -1表示噪声点，其他为不同障碍物

聚类完成后，我会给每个障碍物生成一个包围盒。嗯，这里要注意：不要用轴对齐包围盒（AABB），要用有向包围盒（OBB）。为什么？因为AABB会把斜着的障碍物框得很大，导致机器人绕路。

4.4 动态物体过滤

这是环境感知里最头疼的部分。行人、车辆、飘动的塑料袋……这些动态物体会让地图变得一团糟。

我常用的方法是基于多帧对比。说白了就是：同一位置，上一帧有、这一帧没有，那就是动态物体。

具体实现分三步：

1. 帧间匹配：用ICP或NDT把当前帧和上一帧对齐
2. 差异检测：计算每个点到最近邻的距离，超过阈值就标记为动态
3. 一致性检查：连续3帧都标记为动态，才确认是动态物体

# 简单的帧间差异检测
def detect_dynamic_points(current_cloud, prev_cloud, threshold=0.2):
    # 构建KD树
    tree = o3d.geometry.KDTreeFlann(prev_cloud)
    
    dynamic_mask = []
    for point in current_cloud.points:
        [_, idx, dist] = tree.search_knn_vector_3d(point, 1)
        dynamic_mask.append(dist[0] > threshold)
    
    return np.array(dynamic_mask)

还有一个技巧：利用激光雷达的反射强度。动态物体（尤其是行人）的反射强度通常比静态物体低。把这个特征加入判断，准确率能提升不少。

知识体系总览

下面这张图是我自己整理的，把整个环境感知的流程串起来了。你跟着这个思路走，基本不会跑偏。

这张图把整个流程串起来了。你注意看，地面分割之后分了两条路：一条做障碍物检测，一条做动态过滤。最后合并成静态障碍物地图，送给后面的路径规划模块。