4、环境感知与障碍物检测：基于栅格的障碍物检测、点云聚类、动态障碍物跟踪

环境感知，说白了就是让机器人「看见」周围的世界。我做了这么多年嵌入式路径规划，发现很多同学把精力都放在路径算法上，结果机器人一跑起来就撞墙——问题就出在感知这步没做好。

今天咱们聊聊三个核心模块：栅格法检测障碍物、点云聚类、还有动态障碍物跟踪。嗯，这些都是我实际项目中反复踩过的坑，希望能帮你少走弯路。

4.1 基于栅格的障碍物检测

栅格法是最直观的方法。你把环境划分成一个个小格子，每个格子标记为「占用」或「空闲」。听起来简单，但实际做起来有不少门道。

4.1.1 栅格地图的构建

我习惯用二维数组来表示栅格地图。每个格子存一个概率值，0表示完全空闲，1表示完全占用。为什么用概率而不是二值？因为传感器有噪声，直接判0或1太武断了。

// 栅格地图结构体
typedef struct {
    float occupancy_prob;  // 占用概率 [0, 1]
    uint8_t is_observed;   // 是否被观测过
    uint32_t last_update;  // 最后更新时间戳
} grid_cell_t;

// 更新栅格占用概率
void update_grid_cell(grid_cell_t* cell, float measurement, float prob_hit, float prob_miss) {
    // 贝叶斯更新公式
    float odds = cell->occupancy_prob / (1.0f - cell->occupancy_prob);
    float odds_meas = measurement ? (prob_hit / prob_miss) : ((1 - prob_hit) / (1 - prob_miss));
    odds *= odds_meas;
    cell->occupancy_prob = odds / (1.0f + odds);
    cell->is_observed = 1;
    cell->last_update = get_system_tick();
}

4.1.2 传感器数据到栅格的映射

激光雷达扫一圈，得到的是极坐标下的点。你得把这些点映射到栅格坐标系里。这里有个坑——射线追踪。

举个例子：激光打到3米处有个障碍物，那从机器人到3米之间的所有栅格，都应该标记为「空闲」。只有3米那个格子标记为「占用」。很多新手只标记了障碍物点，结果地图上全是孤立的点，路径规划根本没法用。

// 布雷森汉姆直线算法，标记射线经过的栅格
void mark_ray_free(grid_cell_t* grid, int x0, int y0, int x1, int y1) {
    int dx = abs(x1 - x0), sx = x0 < x1 ? 1 : -1;
    int dy = -abs(y1 - y0), sy = y0 < y1 ? 1 : -1;
    int err = dx + dy, e2;
    
    while (1) {
        // 标记当前栅格为空闲（除了终点）
        if (x0 != x1 || y0 != y1) {
            grid[y0 * GRID_WIDTH + x0].occupancy_prob *= 0.9f;  // 降低占用概率
        }
        if (x0 == x1 && y0 == y1) break;
        e2 = 2 * err;
        if (e2 >= dy) { err += dy; x0 += sx; }
        if (e2 <= dx) { err += dx; y0 += sy; }
    }
}

4.2 点云聚类

激光雷达扫出来的是一堆点，你得把这些点分组成不同的物体。这就是聚类要做的事。

4.2.1 欧几里得聚类

最常用的方法是欧几里得聚类。说白了就是：距离近的点归为一类。我一般用KD-Tree加速搜索，不然在几千个点上做暴力搜索，嵌入式芯片扛不住。

// 欧几里得聚类伪代码
std::vector<Cluster> euclidean_clustering(PointCloud& cloud, float cluster_tolerance, int min_cluster_size) {
    // 1. 构建KD-Tree
    KDTree tree(cloud);
    
    // 2. 标记所有点为未访问
    std::vector<bool> visited(cloud.size(), false);
    std::vector<Cluster> clusters;
    
    // 3. 对每个未访问点进行BFS
    for (int i = 0; i < cloud.size(); i++) {
        if (visited[i]) continue;
        
        Cluster cluster;
        std::queue<int> q;
        q.push(i);
        visited[i] = true;
        
        while (!q.empty()) {
            int idx = q.front(); q.pop();
            cluster.points.push_back(cloud[idx]);
            
            // 搜索半径内的邻居
            std::vector<int> neighbors = tree.radius_search(cloud[idx], cluster_tolerance);
            for (int n : neighbors) {
                if (!visited[n]) {
                    visited[n] = true;
                    q.push(n);
                }
            }
        }
        
        // 过滤掉太小的簇（噪声）
        if (cluster.points.size() >= min_cluster_size) {
            clusters.push_back(cluster);
        }
    }
    
    return clusters;
}

4.2.2 点云预处理

聚类之前，我建议先做两步预处理：

1. 直通滤波：只保留感兴趣区域内的点。比如机器人高度1米，那高于2米的点基本是天花板，可以直接扔掉。
2. 体素滤波：把空间划分成小立方体，每个立方体只保留一个点。这样能大幅减少点数，提高聚类速度。

我记得有个项目，原始点云有5万个点，聚类一次要200毫秒。做了体素滤波后，点云降到3000个点，聚类只要15毫秒。效果几乎没差别。

4.3 动态障碍物跟踪

静态障碍物好处理，麻烦的是动态的——行人、车辆、突然飞来的球。你得知道它们往哪走、速度多快，才能提前避让。

4.3.1 卡尔曼滤波跟踪

我习惯用卡尔曼滤波做跟踪。每个障碍物维护一个状态向量：[x, y, vx, vy]，分别表示位置和速度。

// 卡尔曼滤波跟踪障碍物
typedef struct {
    float x, y;      // 位置
    float vx, vy;    // 速度
    float P[4][4];   // 协方差矩阵
    uint32_t last_seen;  // 最后观测时间
} tracked_object_t;

void predict(tracked_object_t* obj, float dt) {
    // 状态预测：假设匀速运动
    obj->x += obj->vx * dt;
    obj->y += obj->vy * dt;
    
    // 协方差预测
    float F[4][4] = {{1,0,dt,0}, {0,1,0,dt}, {0,0,1,0}, {0,0,0,1}};
    float Q[4][4] = {{dt*dt/4,0,dt/2,0}, {0,dt*dt/4,0,dt/2}, 
                     {dt/2,0,1,0}, {0,dt/2,0,1}};
    // 简化：P = F * P * F^T + Q
    // 实际代码需要矩阵运算
}

void update(tracked_object_t* obj, float zx, float zy) {
    // 观测更新
    float y[2] = {zx - obj->x, zy - obj->y};  // 残差
    float S[2][2], K[4][2];  // 创新协方差、卡尔曼增益
    // ... 矩阵运算更新状态
}

4.3.2 数据关联

跟踪的关键是数据关联——怎么把当前帧检测到的障碍物和上一帧的跟踪对象对应起来。我常用的方法是最近邻关联：

1. 计算每个检测点和每个跟踪对象的马氏距离
2. 构建距离矩阵
3. 用匈牙利算法做最优匹配
4. 未匹配的检测点创建新跟踪对象
5. 未匹配的跟踪对象标记为丢失

嗯，这里有个细节：马氏距离比欧氏距离好。因为它考虑了协方差矩阵，能反映跟踪的不确定性。说白了，如果某个方向上的位置不确定度大，那这个方向上的距离权重就小一些。

4.3.3 运动预测与碰撞检测

有了跟踪结果，就可以做碰撞检测了。我一般用「时间到碰撞」（TTC）来判断：

// 计算与动态障碍物的碰撞时间
float compute_ttc(tracked_object_t* obj, float robot_x, float robot_y, float robot_vx, float robot_vy) {
    float rel_x = obj->x - robot_x;
    float rel_y = obj->y - robot_y;
    float rel_vx = obj->vx - robot_vx;
    float rel_vy = obj->vy - robot_vy;
    
    // 相对距离
    float dist = sqrt(rel_x*rel_x + rel_y*rel_y);
    // 相对速度在连线方向上的分量
    float rel_v = (rel_x*rel_vx + rel_y*rel_vy) / dist;
    
    if (rel_v >= 0) return INFINITY;  // 远离中，不会碰撞
    return -dist / rel_v;  // 碰撞时间
}

我记得有一次做AGV项目，有个工人突然从货架后面冲出来。TTC算出来只有0.8秒，机器人紧急刹车，距离工人只有20厘米。要是没有动态跟踪，后果不堪设想。

小结

环境感知这块，说白了就是三个步骤：

• 栅格法把环境变成机器人能理解的地图
• 点云聚类把散乱的点变成有意义的物体
• 动态跟踪让机器人能预测未来，提前避让

每一步都有坑，但只要你动手做过一遍，就会发现其实没那么难。我建议你先从栅格法入手，跑通一个简单的障碍物检测，再逐步加上聚类和跟踪。别想一口吃成胖子，嵌入式开发最怕的就是「想太多，做太少」。

下一章咱们聊聊路径规划的核心算法，到时候会用到今天讲的这些感知结果。嗯，到时候见。