Autonomous_Exploration_Development_Environment的学习教程(四)

Autonomous_Exploration_Development_Environment学习教程(四)

本教程主要完成local_planner的程序解析，这是该序列教程的第四篇。由于最近楼主的事情较多，更新的较慢，有感兴趣的同学留言，楼主可以根据你们的需要进行后面内容的更新。

1.local_planner的参数解析

下面是local_planner的参数，具体的参数见下表

#define PLOTPATHSET 1                     // 显示路径集的控制量           

string pathFolder;                        // 使用matlab生成路径集合的文件路径
double vehicleLength = 0.6;               // 车辆的长度，单位m
double vehicleWidth  = 0.6;               // 车辆的宽度，单位m
double sensorOffsetX = 0;                 // 传感器坐标系与车体中心的偏移量
double sensorOffsetY = 0;                 // 传感器坐标系与车体中心的偏移量
bool   twoWayDrive   = true;              // 双向驱动
double laserVoxelSize = 0.05;             // 下采样平面体素栅格叶大小 0.05
double terrainVoxelSize = 0.2;            // 下采样地面体素栅格叶大小 0.2
bool   useTerrainAnalysis = false;        // 是否使用地面分割后的点云信息
bool   checkObstacle = true;              // 是否进行障碍物检测
bool   checkRotObstacle = false;          // 是否检测旋转时障碍物
double adjacentRange = 3.5;               // 裁剪点云时的距离
double obstacleHeightThre = 0.2;          // 障碍物高度阈值
double groundHeightThre = 0.1;            // 地面高度阈值
double costHeightThre = 0.1;              // 计算路径惩罚得分的权重
double costScore = 0.02;                  // 最小惩罚得分
bool   useCost = false;
const int laserCloudStackNum = 1;         // 缓存的激光点云帧数量
int    laserCloudCount = 0;               // 当laserCloudStackNum = 1时,暂时没用到
int    pointPerPathThre = 2;              // 每条路径需要有几个被遮挡的点
double minRelZ = -0.5;                    // 未使用地面分割时，裁剪点云时的最小高度
double maxRelZ = 0.25;                    // 未使用地面分割时，裁剪点云时的最大高度
double maxSpeed = 1.0;                    // 最大速度
double dirWeight        = 0.02;           // 计算得分时转向角度的权重
double dirThre          = 90.0;           // 最大转向角度
bool   dirToVehicle     = false;          // 是否以车辆为主方向计算被遮挡的路径
double pathScale        = 1.0;            // 路径尺度
double minPathScale     = 0.75;           // 最小路径尺度
double pathScaleStep    = 0.25;           // 路径尺度的调整步长
bool   pathScaleBySpeed = true;           // 是否根据速度调整路径尺度
double minPathRange     = 1.0;            // 最小路径距离
double pathRangeStep    = 0.5;            // 路径范围的调整步长
bool   pathRangeBySpeed = true;           // 是否根据速度调整路径的范围
bool   pathCropByGoal   = true;           // 是否根据目标点+ goalClearRange 筛选点云数据
bool   autonomyMode     = false; 
double autonomySpeed    = 1.0;
double joyToSpeedDelay  = 2.0;
double joyToCheckObstacleDelay = 5.0;
double goalClearRange = 0.5;              // 当 pathCropByGoal = true 时,点云距离超过目标点+该值则不被处理
double goalX = 0;                         // 局部路径目标点
double goalY = 0;                         // 局部路径目标点

float joySpeed = 0;                       // 车辆的线速度
float joySpeedRaw = 0;                    // 车辆的角速度
float joyDir = 0;                         // 车辆目标位置和车辆位置的相对夹角

const int pathNum = 343;                  // 总共生成的路径数
const int groupNum = 7;                   // 路径的组数
float gridVoxelSize = 0.02;               // 障碍物检测的一个单元格大小
float searchRadius = 0.45;                // 车辆的半径
float gridVoxelOffsetX = 3.2;             // Ｘ方向的偏移量
float gridVoxelOffsetY = 4.5;             // Ｙ方向的偏移量
const int gridVoxelNumX = 161;            // 3.2 / 0.2 = 160
const int gridVoxelNumY = 451;            // 4.5 / 0.2 = 451
const int gridVoxelNum = gridVoxelNumX * gridVoxelNumY; 

2.回调函数的具体解析

总共有９个回调函数，下面具体说明每个函数的具体功能。

函数	函数说明
odometryHandler()	将车辆的odom的信息转换到车辆坐标系下，只进行了坐标系的转换
laserCloudHandler()	对传输过来的点云信息进行滤波，主要完成小于adjacentRange的点云信息的滤波，然后将点云信息进行下采样
terrainCloundHandler()	主要完成对于地面点云信息的滤波，也是完成小于adjacentRange的点云信息的滤波，然后将点云信息进行下采样
joystickHandler()	主要完成关于手柄摇杆的参数配置，主要包括线速度、角速度、自主/遥控模式、障碍物检测的配置
goalHandler()	主要完成当前目标点的配置
speedHandler()	主要完成对于车速的配置和滤波
boundaryHandler()	主要完成对于区域边界的解析，从而判定当前的主要区域边界
addedObstaclesHandler()	主要完成对于障碍物点云信息的检测，将障碍物的点云回波强度设置为200
checkObstacleHandler()	主要完成障碍物检测数据时间的检测，可能由于ROS的时间精度不够，程序全部采用了高精度时间戳，从而需要对各种数据进行时间的检测

3. 路径读取模块

由于在matlab中生成343组初始路径，路径结束点和体素网格和路径点的障碍物关系。所以需要将提前生成的路径等文件加载到程序中

函数	函数说明
readPlyHeader()	这是一个打开文件，读取文件内容的程序，后续的文件都依赖于它进行读取
readStartPaths()	函数主要打开startPaths.ply文件，将里面的初始路径数据读入
readPaths()	函数主要打开Paths.ply文件，将里面的路径数据读入
readPathList()	函数主要打开pathList.ply文件，将里面的开始路径数据读入
readCorrespondences()	函数主要打开correspondences.txt文件，将里面的开始路径数据读入

4.main()函数解析

在while循环中，首先主要进行的重置障碍物信息

    if (newLaserCloud) {
      newLaserCloud = false;

      laserCloudStack[laserCloudCount]->clear();
      *laserCloudStack[laserCloudCount] = *laserCloudDwz;
      laserCloudCount = (laserCloudCount + 1) % laserCloudStackNum;

      plannerCloud->clear();                           // 可能是返回的点云信息是有障碍物的点云信息
      for (int i = 0; i < laserCloudStackNum; i++) {
        *plannerCloud += *laserCloudStack[i];
      }
    }

    if (newTerrainCloud) {
      newTerrainCloud = false;

      plannerCloud->clear();
      *plannerCloud = *terrainCloudDwz;
    }

然后将点云信息的坐标系为车辆坐标系,这是因为在仿真中，点云信息输出的坐标系是全局坐标系。对于实际的车辆而言，点云数据进行标定之后就将点云坐标系直接转变为车辆坐标系，所以这部分内容需要删改。

    pcl::PointXYZI point;
    plannerCloudCrop->clear();         // 用于规划的点云信息
    int plannerCloudSize = plannerCloud->points.size();  // 设置点云的大小
    for (int i = 0; i < plannerCloudSize; i++) {
      float pointX1 = plannerCloud->points[i].x - vehicleX;
      float pointY1 = plannerCloud->points[i].y - vehicleY;
      float pointZ1 = plannerCloud->points[i].z - vehicleZ;

      point.x =  pointX1 * cosVehicleYaw + pointY1 * sinVehicleYaw;
      point.y = -pointX1 * sinVehicleYaw + pointY1 * cosVehicleYaw;
      point.z =  pointZ1;
      point.intensity = plannerCloud->points[i].intensity;

      float dis = sqrt(point.x * point.x + point.y * point.y);
      if (dis < adjacentRange && ((point.z > minRelZ && point.z < maxRelZ) || useTerrainAnalysis)) {
        plannerCloudCrop->push_back(point);            // 修改了点云的坐标系，改为了车辆坐标系
      }
    }

接下来，将计算得到的边界信息转变为车辆坐标系下，程序如下

    int boundaryCloudSize = boundaryCloud->points.size();
    for (int i = 0; i < boundaryCloudSize; i++) {
      point.x = ((boundaryCloud->points[i].x - vehicleX) * cosVehicleYaw 
              + (boundaryCloud->points[i].y - vehicleY) * sinVehicleYaw);
      point.y = (-(boundaryCloud->points[i].x - vehicleX) * sinVehicleYaw 
              + (boundaryCloud->points[i].y - vehicleY) * cosVehicleYaw);
      point.z = boundaryCloud->points[i].z;
      point.intensity = boundaryCloud->points[i].intensity;

      float dis = sqrt(point.x * point.x + point.y * point.y);
      if (dis < adjacentRange) {
        plannerCloudCrop->push_back(point);
      }
    }

同时将障碍物信息的坐标系转变为车辆坐标系，程序如下

    int addedObstaclesSize = addedObstacles->points.size();
    for (int i = 0; i < addedObstaclesSize; i++) {
      point.x = ((addedObstacles->points[i].x - vehicleX) * cosVehicleYaw 
              + (addedObstacles->points[i].y - vehicleY) * sinVehicleYaw);
      point.y = (-(addedObstacles->points[i].x - vehicleX) * sinVehicleYaw 
              + (addedObstacles->points[i].y - vehicleY) * cosVehicleYaw);
      point.z = addedObstacles->points[i].z;
      point.intensity = addedObstacles->points[i].intensity;

      float dis = sqrt(point.x * point.x + point.y * point.y);
      if (dis < adjacentRange) {
        plannerCloudCrop->push_back(point);
      }
    }

接下来设置路径的相关参数，这里对于预先生成的路径参数进行了缩放，从而满足了不同速度下的路径参数，由于这里的速度变化不是很大，所以路径的变化也不大，基本满足设定路径的生成。

    float pathRange = adjacentRange;  // 设置了点云探索的边界值
    if (pathRangeBySpeed) pathRange = adjacentRange * joySpeed;
    if (pathRange < minPathRange) pathRange = minPathRange;
    float relativeGoalDis = adjacentRange;   // 将点云探索的边界值赋予相对的目标距离

接着将目标点转换为车辆坐标系。

    if (autonomyMode) {
      float relativeGoalX = ( (goalX - vehicleX) * cosVehicleYaw + (goalY - vehicleY) * sinVehicleYaw);
      float relativeGoalY = (-(goalX - vehicleX) * sinVehicleYaw + (goalY - vehicleY) * cosVehicleYaw);

      relativeGoalDis = sqrt(relativeGoalX * relativeGoalX + relativeGoalY * relativeGoalY);
      joyDir = atan2(relativeGoalY, relativeGoalX) * 180 / PI;

      if (!twoWayDrive) {
        if (joyDir > 90.0) joyDir = 90.0;
        else if (joyDir < -90.0) joyDir = -90.0;
      }
    }

下面设置参考路径，主要配置一下车辆的半径、路径的缩放、高度差等

    bool pathFound = false;
    float defPathScale = pathScale;
    if (pathScaleBySpeed) pathScale = defPathScale * joySpeed;
    if (pathScale < minPathScale) pathScale = minPathScale;

    while (pathScale >= minPathScale && pathRange >= minPathRange) {
      for (int i = 0; i < 36 * pathNum; i++) {  //
        clearPathList[i] = 0;
        pathPenaltyList[i] = 0;
      }
      for (int i = 0; i < 36 * groupNum; i++) {  //
        clearPathPerGroupScore[i] = 0;
      }

      float minObsAngCW  = -180.0;
      float minObsAngCCW =  180.0;
      float diameter     = sqrt(vehicleLength / 2.0 * vehicleLength / 2.0 + vehicleWidth / 2.0 * vehicleWidth / 2.0); // 车辆的直径
      float angOffset    = atan2(vehicleWidth, vehicleLength) * 180.0 / PI;   // 车辆的夹角
      int plannerCloudCropSize = plannerCloudCrop->points.size();
      for (int i = 0; i < plannerCloudCropSize; i++) {
        float x = plannerCloudCrop->points[i].x / pathScale;   // 将雷达的探测距离缩短，v越大，探测效果越小
        float y = plannerCloudCrop->points[i].y / pathScale;   // 将雷达的探测距离缩短，v越大，探测效果越小
        float h = plannerCloudCrop->points[i].intensity;       // 高度差
        float dis = sqrt(x * x + y * y);
        ...
       }
     }

5. 路径选择的功能模块

上面所有的功能都是为这一部分做的铺垫，所以这部分内容楼主做个详细的解析。

     if (dis < pathRange / pathScale && (dis <= (relativeGoalDis + goalClearRange) / pathScale || !pathCropByGoal) && checkObstacle) {
         for (int rotDir = 0; rotDir < 36; rotDir++) {
           float rotAng = (10.0 * rotDir - 180.0) * PI / 180;        // 10度角一区分，进行360度环扫
           float angDiff = fabs(joyDir - (10.0 * rotDir - 180.0));   // 计算了当前角度与环视角度的差值
           if (angDiff > 180.0) {
              angDiff = 360.0 - angDiff;                              // 差值取正
           }
		  ....
         }
      }

从上述程序可以看出，程序首先进行了一个判断，符合要求后开启了一个for循环。首先我们看一下判断的条件:

• 1.点云距离小于探测缩放后的距离
• 2.点云距离小于（相对目标距离+目标距离）/缩放比例　 || 　!路径剪短
• 3.进行障碍物检测

上述的程序中将360度分割成36份，每份10度。所以开启了36次循环。接下来排除一些不需要的角度。

     // dirThre = 90 ,表示与当前方向的垂直方向
     // dirToVehicle  车辆前进方向
     if ((angDiff > dirThre && !dirToVehicle) || (fabs(10.0 * rotDir - 180.0) > dirThre && fabs(joyDir) <= 90.0 && dirToVehicle) ||
          ((10.0 * rotDir > dirThre && 360.0 - 10.0 * rotDir > dirThre) && fabs(joyDir) > 90.0 && dirToVehicle)) {
       continue;
     }

从上述的可以看出排除了三种情况下的路径，排除了三种情况下的状态:

• 1.与目标连线的方向夹角大于转向角度的阈值　且　目标点不在前进方向的方向上的所有路径:
• 2.当前角度大于车辆转向角度阈值　且　目标点与车辆连线夹角绝对值小于90度　且　车辆向前运动的左右侧的路径;
• 3.当前角度大于车辆转向角度的阈值　且　目标点与车辆连线夹角绝对值小于90度　且　车辆朝向为方向的左右侧的路径。

这边有点绕，其实理解起来很简单，主要是在车辆前进方向上在转向阈值内的目标点可以直接生成路径，除此之外，在车辆前进方向后方的目标点的路径直接排除、还有在车辆前进方向上车辆转向阈值之外的点，且目标点与车辆方向的夹角小于90度的路径直接排除，还有就是车辆转向阈值之外的，车辆行驶方向之后的目标点排除。

	float scaleY = x2 / gridVoxelOffsetX + searchRadius / gridVoxelOffsetY * (gridVoxelOffsetX - x2) / gridVoxelOffsetX;

因为在matlab中的体素网格生成时，Ｙ轴是一条斜线，所以ｙ轴需要有一个缩放的比例，这个比例与ｘ轴的位置相关，是一个线性关系。具体见上一篇教程。

    int indX = int((gridVoxelOffsetX + gridVoxelSize / 2 - x2) / gridVoxelSize);
    int indY = int((gridVoxelOffsetY + gridVoxelSize / 2 - y2 / scaleY) / gridVoxelSize);
    if (indX >= 0 && indX < gridVoxelNumX && indY >= 0 && indY < gridVoxelNumY) {
      int ind = gridVoxelNumY * indX + indY;　// 得到索引序号
      int blockedPathByVoxelNum = correspondences[ind].size();　// 当前序号的体素网格,占据了多少条路径
      for (int j = 0; j < blockedPathByVoxelNum; j++) {
        // if(地面分割高度 > 障碍物高度门槛 || ！使用地面分割)
        if (h > obstacleHeightThre || !useTerrainAnalysis) {
          clearPathList[pathNum * rotDir + correspondences[ind][j]]++;
        } 
        else {
      　　// 在使用了地面分割且激光点分割后高度小于障碍物高度阈值obstacleHeightThre时
      　　// 并且 当前点云高度大于原有地面分割高度值,且大于地面高度阈值groundHeightThre
          if (pathPenaltyList[pathNum * rotDir + correspondences[ind][j]] < h && h > groundHeightThre) {
              pathPenaltyList[pathNum * rotDir + correspondences[ind][j]] = h;
          }
        }
      }
    }

还存在一种情况，由目标点在车辆转向阈值之外的侧面，所以车辆需要进行原地转向，从而实现车辆向目标点位置转向。

    if (dis < diameter / pathScale && (fabs(x) > vehicleLength / pathScale / 2.0 || fabs(y) > vehicleWidth / pathScale / 2.0) && 
        (h > obstacleHeightThre || !useTerrainAnalysis) && checkRotObstacle) {
      float angObs = atan2(y, x) * 180.0 / PI;
      if (angObs > 0) {
        if (minObsAngCCW > angObs - angOffset)          minObsAngCCW = angObs - angOffset;
        if (minObsAngCW  < angObs + angOffset - 180.0)  minObsAngCW = angObs + angOffset - 180.0;
      } else {
        if (minObsAngCW  < angObs + angOffset)          minObsAngCW = angObs + angOffset;
        if (minObsAngCCW > 180.0 + angObs - angOffset)  minObsAngCCW = 180.0 + angObs - angOffset;
      }
    }

其中CW即顺时针旋转（Clock Wise）的方向 与CW反方向旋转时为CCW （Counter Clock Wise）。

6.选择最优路径

在筛选最优路径时，和计算路径障碍物是一样的处理方法，也是根据方向减少需要筛选的路径集合，这里就不再介绍。在之前处理点云数据时，如果h > obstacleHeightThre则clearPathList会累加。当一条路径上存在两个障碍点，即 pointPerPathThre=2，该路径才会认为被遮挡。所以只需要对未被遮挡的路径进行筛选。

首先计算惩罚项的得分，前面已经介绍惩罚项是根据地面阈值和障碍物阈值之间的点云高度。所以pathPenaltyList越高，惩罚得分penaltyScore也就越低。
endDirPathList代表了该条路径末端点与当前位置的角度，dirDiff则是该条路径与目标点之间的角度差值，会用于计算路径的得分。除此之外，rotDirW代表了该条路径的方向与当前车辆朝向的角度差，也会被用于路径得分的计算。具体公式如下:

$$score=（1-\sqrt[4]{dirWeight\times diff}）\times rotDir{W}^4\times penaltyScore$$

    int rotDir = int(i / pathNum);
    float angDiff = fabs(joyDir - (10.0 * rotDir - 180.0));
    if (angDiff > 180.0) {
      angDiff = 360.0 - angDiff;
    }
    if ((angDiff > dirThre && !dirToVehicle) || (fabs(10.0 * rotDir - 180.0) > dirThre && fabs(joyDir) <= 90.0 && dirToVehicle) ||
        ((10.0 * rotDir > dirThre && 360.0 - 10.0 * rotDir > dirThre) && fabs(joyDir) > 90.0 && dirToVehicle)) {
      continue;
    }

    if (clearPathList[i] < pointPerPathThre) {
      float penaltyScore = 1.0 - pathPenaltyList[i] / costHeightThre;
      if (penaltyScore < costScore) penaltyScore = costScore;

      float dirDiff = fabs(joyDir - endDirPathList[i % pathNum] - (10.0 * rotDir - 180.0));
      if (dirDiff > 360.0) {
        dirDiff -= 360.0;
      }
      if (dirDiff > 180.0) {
        dirDiff = 360.0 - dirDiff;
      }

      float rotDirW;
      if (rotDir < 18) rotDirW = fabs(fabs(rotDir - 9) + 1);
      else rotDirW = fabs(fabs(rotDir - 27) + 1);
      float score = (1 - sqrt(sqrt(dirWeight * dirDiff))) * rotDirW * rotDirW * rotDirW * rotDirW * penaltyScore;
      if (score > 0) {
        clearPathPerGroupScore[groupNum * rotDir + pathList[i % pathNum]] += score;
      }
    }
  }

上述程序主要完成了所有路径的得分计算。

    float maxScore = 0;
    int selectedGroupID = -1;
    for (int i = 0; i < 36 * groupNum; i++) {
      int   rotDir = int(i / groupNum);
      float rotAng = (10.0 * rotDir - 180.0) * PI / 180;
      float rotDeg = 10.0 * rotDir;
      if (rotDeg > 180.0) rotDeg -= 360.0;
      if (maxScore < clearPathPerGroupScore[i] && ((rotAng * 180.0 / PI > minObsAngCW && rotAng * 180.0 / PI < minObsAngCCW) || 
          (rotDeg > minObsAngCW && rotDeg < minObsAngCCW && twoWayDrive) || !checkRotObstacle)) {
        maxScore = clearPathPerGroupScore[i];             // 设置最大的得分情况
        selectedGroupID = i;                              // 选出了最大得分的那一组
      }
    }

再选出最优路径后，会选择startPaths组成一条最终的路径。

    if (selectedGroupID >= 0) {
      int   rotDir = int(selectedGroupID / groupNum);
      float rotAng = (10.0 * rotDir - 180.0) * PI / 180;

      selectedGroupID = selectedGroupID % groupNum;
      int selectedPathLength = startPaths[selectedGroupID]->points.size();
      path.poses.resize(selectedPathLength);
      for (int i = 0; i < selectedPathLength; i++) {
        float x = startPaths[selectedGroupID]->points[i].x;
        float y = startPaths[selectedGroupID]->points[i].y;
        float z = startPaths[selectedGroupID]->points[i].z;
        float dis = sqrt(x * x + y * y);

        if (dis <= pathRange / pathScale && dis <= relativeGoalDis / pathScale) {
          path.poses[i].pose.position.x = pathScale * (cos(rotAng) * x - sin(rotAng) * y);
          path.poses[i].pose.position.y = pathScale * (sin(rotAng) * x + cos(rotAng) * y);
          path.poses[i].pose.position.z = pathScale * z;
        } else {
          path.poses.resize(i);
          break;
        }
      }

    path.header.stamp = ros::Time().fromSec(odomTime);
    path.header.frame_id = "/vehicle";
    pubPath.publish(path);

因为startPaths表示第一次采样的路径，而且还需要保证该路径小于设定的pathRange。然后将freePaths会输出clearPathList[i]<pointPerPathThre的路径，这部分主要是在rviz界面路面显示。

感谢大家的阅读，如果喜欢楼主的，记得点赞关注！！！楼主会持续进行相关代码的解析，如果大家有什么推荐的也可以发给楼主！！！