激光雷达进行障碍物检测

最新推荐文章于 2025-03-17 09:59:00 发布

南风知奇意

最新推荐文章于 2025-03-17 09:59:00 发布

阅读量1.3k

点赞数 22

分类专栏：机器人文章标签：激光雷达机器人 PCL

本文链接：https://blog.csdn.net/weixin_45784125/article/details/144957205

版权

机器人专栏收录该内容

5 篇文章

订阅专栏

1. 背景

在农业机器人的正前方安装激光雷达，进行障碍物检测，激光雷达的数据向上发送的数据是pointcloud2类型，即xyzi（x、y、z表示点的空间位置，i可能表示反射强度等信息）,下面介绍几种基于xyzi数据进行障碍物检测的方案

2. 基于阈值的简单聚类方法

2.1 原理

对于激光雷达数据中的xyzi（x、y、z表示点的空间位置，i可能表示反射强度等信息），可以首先根据z轴（高度）信息来设置阈值。例如，如果地面是相对平整的，且已知激光雷达安装高度和地面大致高度范围，将z值低于地面高度一定范围的点视为障碍物点的一部分。
然后，对于x和y方向，可以通过计算点之间的距离来进行聚类。如果两个点之间的距离小于某个设定的阈值（例如根据障碍物的最小尺寸估计），则将它们归为同一类。

2.2 步骤示例

首先，读取激光雷达数据文件，解析出xyzi信息存储在合适的数据结构中，如数组或自定义的点云类。
遍历点云数据，根据z轴阈值筛选出可能的障碍物点，将这些点存储在一个新的集合中。
对于新集合中的点，使用距离计算公式（如欧几里得距离）在这里插入图片描述
来判断点之间的距离。如果距离小于设定的聚类阈值，将它们标记为同一聚类。
根据聚类后的结果，可以确定障碍物的大致位置和范围。

2.3 代码

#include <iostream>
#include <vector>
#include <fstream>
#include <cmath>

// 定义点结构体，对应xyzi数据
struct Point {
    float x;
    float y;
    float z;
    float intensity;
};

// 计算两点之间的欧几里得距离
float distance(const Point& p1, const Point& p2) {
    return std::sqrt((p1.x - p2.x) * (p2.x - p1.x) + (p1.y - p2.y) * (p2.y - p1.y));
}

int main() {
    // 假设从文件中读取激光雷达数据，这里文件名可替换
    std::ifstream infile("lidar_data.xyz");
    if (!infile) {
        std::cerr << "无法打开文件" << std::endl;
        return -1;
    }

    std::vector<Point> points;
    float x, y, z, intensity;
    // 读取文件中的点数据
    while (infile >> x >> y >> z >> intensity) {
        Point p = {x, y, z, intensity};
        points.push_back(p);
    }
    infile.close();

    // 假设地面高度范围，这里示例值可根据实际调整
    float ground_height_min = -0.2;
    float ground_height_max = 0.2;
    // 聚类距离阈值
    float cluster_distance_threshold = 0.5; 

    // 存储可能的障碍物点
    std::vector<Point> obstacle_points;
    // 筛选出可能的障碍物点（根据高度）
    for (const auto& p : points) {
        if (p.z > ground_height_max || p.z < ground_height_min) {
            obstacle_points.push_back(p);
        }
    }

    // 聚类标记数组，初始为-1表示未分类
    std::vector<int> labels(obstacle_points.size(), -1);
    int current_label = 0;
    // 进行聚类
    for (size_t i = 0; i < obstacle_points.size(); ++i) {
        if (labels[i] == -1) {
            labels[i] = current_label;
            for (size_t j = i + 1; j < obstacle_points.size(); ++j) {
                if (distance(obstacle_points[i], obstacle_points[j]) < cluster_distance_threshold) {
                    labels[j] = current_label;
                }
            }
            current_label++;
        }
    }

    // 输出聚类结果（简单示例，可根据实际进一步处理）
    std::cout << "共检测到 " << current_label << " 个障碍物聚类" << std::endl;
    for (int label = 0; label < current_label; ++label) {
        std::cout << "聚类 " << label << " 中的点数量: ";
        int count = 0;
        for (size_t i = 0; i < labels.size(); ++i) {
            if (labels[i] == label) {
                count++;
            }
        }
        std::cout << count << std::endl;
    }

    return 0;
}

3. 基于空间划分的方法（如体素网格）

3.1 原理

将三维空间划分为规则的小立方体（体素）。对于每个体素，可以统计其中包含的点的数量或者其他属性（如最大z值、平均反射强度等）。如果一个体素中的点数量超过一定阈值，或者其属性符合障碍物的特征（例如最大z值高于地面高度很多），则可以认为这个体素包含障碍物。

3.2 步骤示例

确定体素的大小，这需要根据激光雷达的精度和要检测的障碍物大小来选择。例如，对于高精度激光雷达和较小障碍物，可以选择较小的体素尺寸（如 0.1m×0.1m×0.1m）。
遍历激光雷达数据中的点，将每个点分配到对应的体素中。可以通过计算点的x、y、z坐标除以体素尺寸的整数部分来确定体素索引。
统计每个体素中的点数量等属性。如果点数量超过阈值（如 5 个点以上），标记该体素为可能包含障碍物。
最后，根据标记的体素位置和范围来确定障碍物的位置。

3.3 代码

#include <iostream>
#include <vector>
#include <fstream>
#include <cmath>
#include <unordered_map>

// 定义点结构体，对应xyzi数据
struct Point {
    float x;
    float y;
    float z;
    float intensity;
};

int main() {
    // 假设从文件中读取激光雷达数据，这里文件名可替换
    std::ifstream infile("lidar_data.xyz");
    if (!infile) {
        std::cerr << "无法打开文件" << std::endl;
        return -1;
    }

    std::vector<Point> points;
    float x, y, z, intensity;
    // 读取文件中的点数据
    while (infile >> x >> y >> z >> intensity) {
        Point p = {x, y, z, intensity};
        points.push_back(p);
    }
    infile.close();

    // 体素尺寸（示例值，可根据实际调整）
    float voxel_size = 0.1;
    // 体素内点数量阈值
    int point_count_threshold = 5; 

    // 使用unordered_map来存储体素及其中的点索引
    std::unordered_map<int, std::vector<int>> voxels;
    // 将点分配到体素中并统计点数量
    for (size_t i = 0; i < points.size(); ++i) {
        int voxel_x = std::floor(points[i].x / voxel_size);
        int voxel_y = std::floor(points[i].y / voxel_size);
        int voxel_z = std::floor(points[i].z / voxel_size);
        int voxel_index = voxel_x + voxel_y * 100 + voxel_z * 10000;  // 简单的索引计算示例

        voxels[voxel_index].push_back(i);
    }

    // 检测包含障碍物的体素
    std::vector<int> obstacle_voxels;
    for (const auto& voxel : voxels) {
        if (voxel.second.size() >= point_count_threshold) {
            obstacle_voxels.push_back(voxel.first);
        }
    }

    // 输出包含障碍物的体素信息（简单示例，可根据实际进一步处理）
    std::cout << "检测到 " << obstacle_voxels.size() << " 个体素包含障碍物" << std::endl;
    for (int voxel_index : obstacle_voxels) {
        std::cout << "体素索引: " << voxel_index << ", 包含点数量: " << voxels[voxel_index].size() << std::endl;
    }

    return 0;
}

4. 聚类分割方法（例如欧几里得聚类）

4.1 原理

PCL 库提供了高效的点云处理算法。欧几里得聚类算法基于点之间的空间距离来划分点云。它从一个未标记的点开始，寻找与其距离小于给定阈值的所有点，将这些点标记为同一聚类。然后，继续寻找未标记的点，重复这个过程，直到所有点都被标记。

4.2 步骤示例（使用 PCL 库）

首先，将xyzi数据转换为 PCL 中的点云数据类型（如pcl::PointCloudpcl::PointXYZI）。
创建一个欧几里得聚类提取对象（如pcl::EuclideanClusterExtractionpcl::PointXYZI），设置距离阈值（例如 0.5 米）和最小聚类点数（例如 10 个点）等参数。
调用聚类算法对转换后的点云数据进行处理，得到聚类后的结果。
可以通过遍历聚类结果来获取每个聚类的点集，进而确定障碍物的位置和形状。

4.3 代码

#include <iostream>
#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>
#include <pcl/segmentation/euclidean_cluster_extraction.h>

int main() {
    // 读取PCD格式的点云文件（这里假设数据已保存为PCD格式，也可从其他格式转换过来）
    pcl::PointCloud<pcl::PointXYZI>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZI>);
    if (pcl::io::loadPCDFile<pcl::PointXYZI>("lidar_data.pcd", *cloud) == -1) {
        PCL_ERROR("无法读取点云文件\n");
        return (-1);
    }

    // 创建欧几里得聚类提取对象
    pcl::EuclideanClusterExtraction<pcl::PointXYZI> ec;
    ec.setInputCloud(cloud);
    // 设置距离阈值（单位：米，示例值可调整）
    ec.setClusterTolerance(0.5);
    // 设置最小聚类点数
    ec.setMinClusterSize(10);
    // 设置最大聚类点数（可选，这里示例设一个较大值）
    ec.setMaxClusterSize(1000000);

    std::vector<pcl::PointIndices> cluster_indices;
    // 执行聚类算法
    ec.extract(cluster_indices);

    // 输出聚类结果
    std::cout << "共检测到 " << cluster_indices.size() << " 个障碍物聚类" << std::endl;
    for (size_t i = 0; i < cluster_indices.size(); ++i) {
        std::cout << "聚类 " << i << " 中的点数量: " << cluster_indices[i].indices.size() << std::endl;
    }

    return 0;
}

5. 基于模型拟合的方法（如平面拟合用于地面去除后检测障碍物）

5.1 原理

先使用 PCL 库中的平面拟合算法（如随机抽样一致性算法 - RANSAC）来拟合地面平面。RANSAC 算法可以从点云中随机选择一些点来拟合模型（平面方程）在这里插入图片描述
，然后计算其他点到该平面的距离。通过多次迭代，找到最佳拟合平面。去除地面点后，剩余的点可以通过聚类等方法来检测障碍物。

5.2 步骤示例

将xyzi数据转换为 PCL 点云类型。
创建一个平面拟合对象（如pcl::SACSegmentationpcl::PointXYZI），设置模型类型为平面（pcl::SACMODEL_PLANE），距离阈值（例如 0.05 米，表示到拟合平面距离超过这个值的点被视为非平面点）和 RANSAC 迭代次数（例如 100 次）等参数。
运行平面拟合算法，得到地面平面的参数和地面点的索引。
从原始点云中去除地面点，然后使用聚类等障碍物检测方法对剩余点进行处理。

5.3 代码

#include <iostream>
#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>
#include <pcl/ModelCoefficients.h>
#include <pcl/sample_consensus/method_types.h>
#include <pcl/sample_consensus/model_types.h>
#include <pcl/segmentation/sac_segmentation.h>
#include <pcl/filters/extract_indices.h>

int main() {
    // 读取PCD格式的点云文件（这里假设数据已保存为PCD格式，也可从其他格式转换过来）
    pcl::PointCloud<pcl::PointXYZI>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZI>);
    if (pcl::io::loadPCDFile<pcl::PointXYZI>("lidar_data.pcd", *cloud) == -1) {
        PCL_ERROR("无法读取点云文件\n");
        return (-1);
    }

    // 创建平面拟合对象（基于随机抽样一致性算法 - RANSAC）
    pcl::SACSegmentation<pcl::PointXYZI> seg;
    seg.setOptimizeCoefficients(true);
    // 设置输入点云
    seg.setInputCloud(cloud);
    // 设置模型类型为平面
    seg.setModelType(pcl::SACMODEL_PLANE);
    // 设置距离阈值（单位：米，示例值可调整）
    seg.setDistanceThreshold(0.05);
    // 设置RANSAC算法的迭代次数
    seg.setMaxIterations(100);

    pcl::ModelCoefficients::Ptr coefficients(new pcl::ModelCoefficients);
    pcl::PointIndices::Ptr inliers(new pcl::PointIndices);
    // 执行平面拟合算法，获取地面平面参数和地面点索引
    seg.segment(*inliers, *coefficients);
    if (inliers->indices.size() == 0) {
        PCL_ERROR("未能找到合适的地面平面\n");
        return (-1);
    }

    // 创建点云提取对象，用于去除地面点
    pcl::ExtractIndices<pcl::PointXYZI> extract;
    extract.setInputCloud(cloud);
    extract.setIndices(inliers);
    extract.setNegative(true);  // 设置为true表示提取非地面点
    pcl::PointCloud<pcl::PointXYZI>::Ptr obstacle_cloud(new pcl::PointCloud<pcl::PointXYZI>);
    extract.filter(*obstacle_cloud);

    // 这里可以对去除地面后的点云（obstacle_cloud）进一步使用聚类等方法检测障碍物
    // 例如下面简单输出去除地面后点云的点数作为示例
    std::cout << "去除地面后，剩余点云点数: " << obstacle_cloud->size() << std::endl;

    return 0;
}