前言

做这个项目的起因呢是今年智能汽车讯飞组中有个任务是对一个场地中随机出现的板子进行识别，就想着通过对激光雷达扫到的数据进行处理，去完成任务。本文呢打算避开繁琐的公式和原理，直接用浅而易懂的方法一步一步的去解决。

环境：

ubuntu 20.04

ros noetic

一、2D激光雷达在ROS中的消息格式

熟悉ROS的都会知道，雷达消息的话题一般为scan，它的消息类型是sensor_msgs/LaserScan，但是他的具体消息格式却很少有人关注，使用rosmsg info 查看一下如下：

具体格式及其含义为：

std_msgs/Header header 消息头，包含时间戳和坐标系信息。
  uint32 seq
  time stamp
  string frame_id
float32 angle_min  激光扫描的起始角度。
float32 angle_max 激光扫描的终止角度。
float32 angle_increment  每个扫描点之间的角度增量。
float32 time_increment  每个扫描点之间的时间增量。
float32 scan_time   一次完整扫描的时间。
float32 range_min   激光扫描的最近距离。
float32 range_max   激光扫描的最远距离。
float32[] ranges    每个扫描点的距离值，按照从起始角度到终止角度的顺序排列。
float32[] intensities   每个扫描点的强度值，可能为空。

其中比较重要的信息就是消息头中的时间戳和坐标系信息、激光扫描的起始、终止角度，以及每个扫描点的距离值。通过上述的信息描述，我们可以获取到所有物体到雷达坐标系中心点的距离，在通过起始角我们可以获取到所有物体在雷达坐标系的位置。

二、2D激光雷达系统的障碍物检测方法

2D激光雷达系统的障碍物检测方法主要是基于点云的检测方法。

基于点云的检测方法是2D激光雷达系统中最为常用的障碍物检测方法。该方法通过将激光雷达扫描环境中的物体后得到的点云数据进行处理和分析，实现对环境中障碍物的检测和距离测量。点云数据是由一系列点构成的，每个点包含了物体的位置和反射强度等信息。

基于点云的检测方法我做了以下步骤：

1. 点云滤波

由于激光雷达扫描环境时会受到环境光线和噪声等干扰，因此需要对点云数据进行滤波处理，去除噪声和异常点等。

函数入参是初始的雷达点云数据，出参是经过过滤后的数据：

void LaserScan::removeClosedPointCloud(const sensor_msgs::LaserScan &scan_in,
                              sensor_msgs::LaserScan &scan_out, float thres)
{
    //判断入参与出参是不是同一个东西,即地址是否相同
    //如果是就跳过,不是就重新设置大小
    if (&scan_in != &scan_out)
    {
        scan_out.header = scan_in.header;
        scan_out.ranges.resize(scan_in.ranges.size());
    }
    size_t j = 0;
    //重点部分
    for (size_t i = 0; i < scan_in.ranges.size(); ++i)
    {
        //三维坐标系下判断距离,如果小于就跳过,大于就保存下来
        if (scan_in.ranges[i] <  thres)
            continue;
        scan_out.ranges[j] = scan_in.ranges[i];
        scan_out.intensities[j] = scan_in.intensities[j];
        j++;
    }
    //先用in大小的数组容量,后面重新设置容量
    if (j != scan_in.ranges.size())
    {
        scan_out.ranges.resize(j);
        scan_out.intensities.resize(j);
    }
}

2. 雷达消息转为坐标信息

这里呢由于是2D激光雷达，所以又通过起始角和分辨率进行了雷达点云向坐标的转换，得到了每个点在雷达坐标系的坐标，把他们都存放在了2维数组obstacleCoord里
void TransformCoord(const sensor_msgs::LaserScan &laserCloudIn,std::vector<std::vector<float>> &obstacleCoord)
{
    for(size_t i = 0; i < laserCloudIn.ranges.size(); ++i)
    {
        //起始角度
        float startOri = laserCloudIn.angle_min;
        //最后一个点角度
        float endOri = laserCloudIn.angle_max;
        //角度分辨率
        float angleIncrement = laserCloudIn.angle_increment;
​
        std::vector<float> coord;
        float obstacleMayangle = i * angleIncrement + startOri;
        if(obstacleMayangle > M_PI)
        {
            obstacleMayangle -= 2 * M_PI;
        }std_msgs::Int32 Is_start;
        float x = cos(obstacleMayangle) * laserCloudIn.ranges[i];
        float y = sin(obstacleMayangle) * laserCloudIn.ranges[i];
        coord.push_back(x);
        coord.push_back(y);
        obstacleCoord.push_back(coord);
    }
}

3. 计算包围框

获取到每个点的x、y的坐标后，我们就要开始计算包围框了，所谓包围框就是指一个简单几何空间，在三维点云中，里面包含一系列点集，为点集构建包围框能够有效提取障碍物几何属性给跟踪模块作为观测值。

对于下图仿真，包围框就是车身周围的蓝色矩形框，也可以看到障碍物的点云进入了包围框。我这里只采用了最为简单的矩形框，具体可以根据实际情况选取更复杂的包围框。

具体代码实现如下：

void LaserScan::resetRange(const std::vector<std::vector<float>> &obstacleMaycoord_in,
                              std::vector<std::vector<float>> &obstacleMaycoord_out,
                              float max_x_f,float max_x_b,float max_y_l,float max_y_r)
{
    size_t j = 0;
    for(size_t i = 0; i < obstacleMaycoord_in.size(); i++)
    {
        float x = obstacleMaycoord_in[i][0];
        float y = obstacleMaycoord_in[i][1];
        if(x >= -max_x_b && x<=max_x_f && y >= -max_y_l && y <= max_y_r)
        {
            obstacleMaycoord_out[j] = obstacleMaycoord_in[i]; 
            //这里注意要把取出的下标也存入，便于判断是否突变
            obstacleMaycoord_out[j].push_back(i);
            j++; 
        }
    }
    if (j != obstacleMaycoord_in.size())
    {
        obstacleMaycoord_out.resize(j);std_msgs::Int32 Is_start;
    }
}

计算过包围框之后，我们变该把再包围框的雷达点云提取出来，即进行点云分割

4. 点云分割及分类

点云分割是将点云数据分为不同的部分，每个部分代表一个物体或者一组物体。点云分割可以通过聚类算法、分割算法等方法实现。

这里呢我们通过点云标号判断是否是连续的点，把不连续的点即角点，记录下来实现障碍物分割。其中不要忘记第一个点和最后一个点也要记录下来。

void LaserScan::laserCloudHandler(const sensor_msgs::LaserScanConstPtr &laserScan)
{
    //systemInited初始值为false
    //如果没有初始化就等几帧
    if (!systemInited)
    { 
        systemInitCount++;
​
        if (systemInitCount >= systemDelay)
        {
            systemInited = true;
        }
        else
            return;
    }
    sensor_msgs::LaserScan laserCloudIn = *laserScan;
    std::vector<int> indices;
    //去除点云中的nan点
    //pcl::removeNaNFromPointCloud(laserCloudIn, laserCloudIn, indices);
    //去除距离小于阈值的点
    //--为自定义函数
    removeClosedPointCloud(laserCloudIn, laserCloudIn, MINIMUM_RANGE);
    std::vector<std::vector<float>> obstacleCoord;
    TransformCoord(laserCloudIn,obstacleCoord);
    resetRange(obstacleCoord,obstacleCoord,MAX_X_FRONT,MAX_X_BACK,MAX_Y_LEFT,MAX_Y_RIGHT);
    std::vector<std::vector<float>> obstacleCornerPoint;
    geometry_msgs::PoseArray obstacleCornerPose;
    geometry_msgs::Pose tempPose;
    for(size_t i = 0; i < obstacleCoord.size(); i++)
    {
        if(i == obstacleCoord.size()-1) break;
        if(debug)
        std::cout << obstacleCoord[i][0] << " , " << obstacleCoord[i][1] << " , "<< obstacleCoord[i][2] << std::endl;
        size_t j = 0;
        if(i==0)
        {
            obstacleCornerPoint.push_back(obstacleCoord[i]);
​
        }
        else if(obstacleCoord[i+1][2] - obstacleCoord[i][2] > 4.0)
        {
            obstacleCornerPoint.push_back(obstacleCoord[i]);
            obstacleCornerPoint.push_back(obstacleCoord[i+1]);
​
        }
        if(i==obstacleCoord.size()-2)
        {
            if(obstacleCornerPoint[obstacleCornerPoint.size()-1][2] != obstacleCoord[i+1][2])
            {
                obstacleCornerPoint.push_back(obstacleCoord[i]);
            }
        }
    }
    
    ROS_INFO("-----------wait------------");
​
}

接着分割后的点云数据进行分类，然后进行对物体进行判断，将其识别为障碍物或非障碍物。障碍物检测可以根据实际需求进行调整，例如设置障碍物的大小、形状、距离等参数。通过一次遍历，我们把获取到的每个障碍物的中心点，和通过斜率计算的这个障碍物的角度存放在了obstacleCenterPoint容器中。

for(size_t i = 0; i < obstacleCornerPoint.size(); i++)
{
    std::cout <<  " x is " << obstacleCornerPoint[i][0] << " y is " << obstacleCornerPoint[i][1] << std::endl;
}
std::cout <<  " this " << obstacleCornerPoint.size() << " obstacle " << std::endl;
for(size_t i = 0; i < obstacleCornerPoint.size(); i+=2)
{
    count += 1;
    float yaw;
    std::vector<float> CenterPoint;
    if(i == obstacleCornerPoint.size()-1) break;
    float x1 = obstacleCornerPoint[i][0];
    float y1 = obstacleCornerPoint[i][1];
    float x2 = obstacleCornerPoint[i+1][0];
    float y2 = obstacleCornerPoint[i+1][1];
    float center_x = (x1 + x2) * 0.5;
    float center_y = (y1 + y2) * 0.5;
    // ROS_INFO("%d coord is %f, %f",count,x1,y1);
    // ROS_INFO("%d coord is %f, %f",count,x2,y2);
    if(x1 == x2)
    {
    yaw = 0.5 * M_PI;
    }
    else
    yaw = atan((y1-y2)/(x1-x2));
​
    CenterPoint.push_back(center_x);
    CenterPoint.push_back(center_y);
    CenterPoint.push_back(yaw);
    obstacleCenterPoint.push_back(CenterPoint);
​
}

5. 目标点的确认和发布

由于我的任务是需要停到板子前约0.5m处的位置，所以我应该计算好停的位置存下来然后发布出去，位置如图所示：

deltaX = a * sinθ
deltaY = a * cosθ

x' = x + deltaX

y' = y + deltaY

这样我们就获得了目标点坐标，其中θ是板子通过计算后的倾斜角。

然后我们将其存下来，通过坐标变换将目标点挨个存下来，

for(size_t i = 0; i < obstacleCenterPoint.size(); i++)
{
    float yaw = obstacleCenterPoint[i][2];
    geometry_msgs::Quaternion quaternion;
    quaternion = tf::createQuaternionMsgFromRollPitchYaw(0.0, 0.0, yaw);
    geometry_msgs::PoseStamped scan_goal;
    scan_goal.header.frame_id = "laser_frame";
    scan_goal.header.stamp = ros::Time::now();
    scan_goal.pose.position.x = obstacleCenterPoint[i][0] * 0.4;
    scan_goal.pose.position.y = obstacleCenterPoint[i][1] * 0.4;
    scan_goal.pose.position.z = 0.0;
    scan_goal.pose.orientation.w = quaternion.w;
    scan_goal.pose.orientation.x = quaternion.x;
    scan_goal.pose.orientation.y = quaternion.y;
    scan_goal.pose.orientation.z = quaternion.z;
    geometry_msgs::PoseStamped map_goal;
    try{
        tf_listener.waitForTransform("map", "laser_frame", ros::Time::now(), ros::Duration(2.0));
        tf_listener.transformPose("map",  ros::Time(0), scan_goal, "laser_frame" ,map_goal);
        std::cout << map_goal.header.frame_id << std::endl;
        ROS_INFO("yaw2 :, %f,%f",map_goal.pose.position.x,map_goal.pose.position.y);
    }
    catch(tf::TransformException& ex){
    ROS_ERROR("Failed to transform point: %s", ex.what());
    }
​
    map_goal_ptr[i] = map_goal;
    PUB = true;
}

当可以发布时进行目标点的发布，同时通过amcl订阅的话题判断是否成功抵达进行下一个点的切换：

if(PUB)
{
    int num = obstacleCenterPoint.size();
    if(goal_reached && goal_ind < num)
    {
        goal_ind += 1;
        geometry_msgs::Quaternion quaternion;
        geometry_msgs::PoseStamped map_goal;
        map_goal = map_goal_ptr[goal_ind-1];
        
        quaternion.x = map_goal.pose.orientation.x;
        quaternion.y = map_goal.pose.orientation.y;
        quaternion.z = map_goal.pose.orientation.z;
        quaternion.w = map_goal.pose.orientation.w;
​
        double roll, pitch, current_yaw;
        tf::Quaternion tf_quaternion(quaternion.x, quaternion.y, quaternion.z, quaternion.w);
        tf::Matrix3x3(tf_quaternion).getRPY(roll, pitch, current_yaw);
        ROS_INFO("yaw2 :%f",current_yaw);
        goal_yaw = current_yaw;
        this->goal_pos.x = map_goal.pose.position.x;
        this->goal_pos.y = map_goal.pose.position.y;
        goal_pub.publish(map_goal);
        goal_received = true;
        goal_reached = false;
    }
}

通过下图我们可以看到目标点被成功的发布了：

最后通过终端打印的信息我们可以看到，检测到了两个角点，并把map坐标系的目标点进行了发布

这份代码经过实车测验也是没问题的哦，至此，关于2D激光雷达扫描障碍并进行追踪的内容就到这里结束啦。