CMU团队开发的局部路径导航算法 localPlanner 代码详解以及如何适用于现实小车

开源了很久的局部规划器算法，实际使用效果也非常不错，不过一直没仔细去阅读，最近有时间对这部分代码仔细梳理一下。

在阅读localPlanner局部路径规划代码之前，首先需要了解其局部路径的生成过程。
详细解析可参考博客：Matlab用采样的离散点做前向模拟三次样条生成路径点

GitHub地址：https://github.com/HongbiaoZ/autonomous_exploration_development_environment/tree/noetic/src/local_planner
Autonomous Exploration Development Environment and the Planning Algorithms 这篇论文好像还没发表，所以关于这部分只能通过代码慢慢啃了。

通过对该部分代码的阅读，在我的理解中，该路径规划器的主要思想，就是通过点云数据寻找障碍物，然后剔除被障碍物遮挡的路径线条。保留可通行的路径，在所有可通行的路径中，选择一条更适合的局部路径！

第一部分：对参数的理解

在 localPlanner 中定义了太多的参数，根据自己的理解介绍一下部分函数。有些参数理解错误或者不理解的地方，欢迎指正～

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;
double terrainVoxelSize = 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;
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;
bool autonomyMode = false;
double autonomySpeed = 1.0;
double joyToSpeedDelay = 2.0;
double joyToCheckObstacleDelay = 5.0;
double goalClearRange = 0.5;
double goalX = 0;
double goalY = 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;
const int gridVoxelNumY = 451;
const int gridVoxelNum = gridVoxelNumX * gridVoxelNumY;

第二部分：数据的输入

首先关于 localPlanner 中的坐标系问题，由于是在仿真环境下运行，点云数据是在map下的，但是作为一个局部的规划器，在用于现实小车时，点云数据应该是在激光雷达的坐标系下，或者说是在base_link下的，后面应用时会对其进行更改。

从数据的输入开始，localPlanner 可接收原始点云数据，也可接收经过地面分割后的点云数据，可通过useTerrainAnalysis参数进行调整，后面在代码中会介绍两种输入数据类型的差异。

在terrainCloudHandler激光点云的回调函数中，前面已经说到，在该仿真环境下，给到该局部规划器的是一副完整的在map坐标系的点云地图，因此在这里对完整的点云图根据车辆位置进行裁剪，只保留在车辆附近 dis < adjacentRange 的点云数据。在实际使用过程中，这部分的代码是可以删去的。对地面分割后的点云数据的处理也是一样的。

void terrainCloudHandler(const sensor_msgs::PointCloud2ConstPtr& terrainCloud2)
{
	...
	for (int i = 0; i < terrainCloudSize; i++) {
	  point = terrainCloud->points[i];
	
	  float pointX = point.x;
	  float pointY = point.y;
	  float pointZ = point.z;
	
	  float dis = sqrt((pointX - vehicleX) * (pointX - vehicleX) + (pointY - vehicleY) * (pointY - vehicleY));
	  if (dis < adjacentRange && (point.intensity > obstacleHeightThre || useCost)) {
	    point.x = pointX;
	    point.y = pointY;
	    point.z = pointZ;
	    terrainCloudCrop->push_back(point);
	  }
	}
	...
}

最终所得到的原始点云数据或者是经过地面分割后的点云数据，都会被添加到plannerCloud中。
不过当选择使用经过地面分割后的点云数据时，即当 newTerrainCloud = true 时，便不会再使用原始的点云数据了。

if (newLaserCloud) {
	newLaserCloud = false;
	plannerCloud->clear();
	*plannerCloud = *laserCloudStack[i];
}
if (newTerrainCloud) {
	newTerrainCloud = false;
	plannerCloud->clear();
	*plannerCloud = *terrainCloudDwz;
}

输入的数据除了实时的点云信息外，还有离线生成的路径信息，在Matlab用采样的离散点做前向模拟三次样条生成路径点这篇博客中，已经对路径信息进行了详细的分析。
主要输入的路径信息包括：（1）路径点集合 （2）碰撞检测体素网格 （3）路径点和碰撞体素网格的索引关系

未完待更…

标签：point,int,double,路径,localPlanner,详解,bool,点云,CMU
来源： https://blog.csdn.net/qq_39266065/article/details/120405868