如何利用realsense相机进行机器人的导航避障

zllz0907

已于 2023-03-14 11:31:31 修改

阅读量1.7k

点赞数

分类专栏：机器人开发文章标签：机器人自动驾驶 Powered by 金山文档

于 2023-03-10 10:41:08 首次发布

本文链接：https://blog.csdn.net/zhaoliang38/article/details/129437794

版权

机器人开发专栏收录该内容

12 篇文章 1 订阅

订阅专栏

一、使用Realsense相机进行机器人导航和避障的流程：

获取深度图像和彩色图像：Realsense相机可以同时输出深度图像和彩色图像。深度图像可以用于生成点云数据，而彩色图像可以用于图像处理和识别。

生成点云数据：使用深度图像和相机内参矩阵，可以将深度图像转换为点云数据。点云数据可以表示物体的三维形状和位置。这一步可以借助realsense的官方驱动直接输出。

进行障碍物检测：使用点云数据可以检测机器人周围的障碍物。一种常见的方法是使用障碍物检测算法，例如基于聚类的算法，来识别点云中的障碍物。可以参考我的另一篇文章超详细的激光点云地面分割（可行驶区域提取）方案

进行路径规划：一旦识别了障碍物，机器人需要规划避障路径。路径规划算法可以根据机器人的当前位置和目标位置，以及障碍物的位置和形状，生成一条安全的路径。可以借助movebase框架实现。

控制机器人移动：最后，机器人需要根据路径规划结果控制移动。这可以通过控制机器人的轮子或其他执行器来实现。

二、如何进行障碍物的检测

在使用Realsense相机进行机器人导航和避障时，一种常见的障碍物检测方法是使用基于点云数据的聚类算法，例如欧几里得聚类或基于区域的聚类。

这些算法可以将点云数据分成多个聚类，每个聚类代表一个物体或障碍物。聚类算法通常包括以下步骤：

对点云数据进行下采样：由于点云数据往往非常密集，因此需要对其进行下采样，以减少计算量和提高算法效率。可以使用VoxelGrid过滤器等方法进行下采样。

对下采样后的点云数据进行聚类：使用聚类算法将点云数据分成多个聚类。欧几里得聚类是一种常见的算法，它将点云数据分成多个聚类，每个聚类的点之间的欧几里得距离小于给定的阈值。基于区域的聚类算法则将点云数据分成多个区域，每个区域包含一组相邻的点。

过滤掉小的聚类：通常，小的聚类不代表障碍物，因此需要过滤掉它们。可以根据聚类中的点数或聚类的体积大小来过滤掉小的聚类。

对聚类进行分类：根据聚类的形状和位置，可以将聚类分类为障碍物或其他物体。例如，可以使用机器学习算法对聚类进行分类，或者使用预定义的规则来判断聚类是否为障碍物。

输出障碍物的位置和形状：最后，可以将障碍物的位置和形状输出给路径规划算法，以便生成安全的路径。

三、示例代码

#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <pcl/filters/voxel_grid.h>
#include <pcl/segmentation/region_growing.h>
#include <pcl/segmentation/extract_clusters.h>

pcl::PointCloud<pcl::PointXYZ>::Ptr obstacleDetection(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_in, float leaf_size, float cluster_tolerance, int min_cluster_size, int max_cluster_size)
{
    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_downsampled(new pcl::PointCloud<pcl::PointXYZ>);
    pcl::VoxelGrid<pcl::PointXYZ> sor;
    sor.setInputCloud(cloud_in);
    sor.setLeafSize(leaf_size, leaf_size, leaf_size);
    sor.filter(*cloud_downsampled);//下采样

    pcl::search::Search<pcl::PointXYZ>::Ptr tree = boost::shared_ptr<pcl::search::Search<pcl::PointXYZ> >(new pcl::search::KdTree<pcl::PointXYZ>);
    pcl::PointCloud<pcl::Normal>::Ptr normals(new pcl::PointCloud<pcl::Normal>);
    pcl::NormalEstimation<pcl::PointXYZ, pcl::Normal> ne;
    ne.setSearchMethod(tree);
    ne.setInputCloud(cloud_downsampled);
    ne.setKSearch(50);
    ne.compute(*normals);//点云的法线估计

    pcl::IndicesPtr indices(new std::vector <int>);
    pcl::PassThrough<pcl::PointXYZ> pass;
    pass.setInputCloud(cloud_downsampled);
    pass.setFilterFieldName("z");
    pass.setFilterLimits(0.0, 1.0);
    pass.filter(*indices);//去除离地面太近或太远的点

    pcl::RegionGrowing<pcl::PointXYZ, pcl::Normal> reg;
    reg.setMinClusterSize(min_cluster_size);
    reg.setMaxClusterSize(max_cluster_size);
    reg.setSearchMethod(tree);
    reg.setNumberOfNeighbours(30);
    reg.setInputCloud(cloud_downsampled);
    reg.setInputNormals(normals);
    reg.setIndices(indices);
    reg.setSmoothnessThreshold(7.0 / 180.0 * M_PI);
    reg.setCurvatureThreshold(1.0);

    std::vector <pcl::PointIndices> clusters;
    reg.extract(clusters);//基于区域生长的聚类算法来检测障碍物

    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_clusters(new pcl::PointCloud<pcl::PointXYZ>);
    for (std::vector<pcl::PointIndices>::const_iterator it = clusters.begin(); it != clusters.end(); ++it)
    {
        pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_cluster(new pcl::PointCloud<pcl::PointXYZ>);
        for (std::vector<int>::const_iterator pit = it->indices.begin(); pit != it->indices.end(); ++pit)
            cloud_cluster->points.push_back(cloud_downsampled->points[*pit]);
        cloud_cluster->width = cloud_cluster->points.size();
        cloud_cluster->height = 1;
        cloud_cluster->is_dense = true;
        cloud_clusters->points.insert(cloud_clusters->points.end(), cloud_cluster->points.begin(), cloud_cluster->points.end());
    }

    return cloud_clusters;
}