CMU团队开源的局部路径导航算法 localPlanner 代码详解以及如何适用于现实移动机器人
开源了很久的局部规划器算法，实际使用效果也非常不错，不过一直没仔细去阅读，最近有时间对这部分代码仔细梳理一下。
在阅读localPlanner局部路径规划代码之前，首先需要了解其局部路径的生成过程。
详细解析可参考博客：Matlab用采样的离散点做前向模拟三次样条生成路径点
GitHub地址：localPlanner.cpp
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; --- 当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 goalClearRange = 0.5; --- 当 pathCropByGoal = true 时,点云距离超过目标点+该值则不被处理
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;
}
因为在仿真环境中，提供的时在map下的点云数据，但是在局部路径选取和规划时，所用到的局部路径目标点都是在base_link下的，所以对所有的点云信息都旋转变换到车体坐标系下了。
这部分代码实际使用中，也是不需要进行旋转变换的，不过如果想对实时点云数据进行裁剪，也可以根据距离过滤一下，不过意义不大，因为这个系统的实时性已经很高了。
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);
}
}
输入的数据除了实时的点云信息外，还有离线生成的路径信息，在Matlab用采样的离散点做前向模拟三次样条生成路径点这篇博客中，已经对路径信息进行了详细的分析。
主要输入的路径信息包括：
（1）第一次采样的路径点startPaths（2）路径点集合 paths
（3）每条路径的最后一个路径点 pathList（4）路径点和碰撞体素网格的索引关系 correspondences
这样以来，整个系统的初始化工作也就已经完成了，数据也已经完全准备就绪。
第三部分：路径集合的筛选
首先确定几个基本的参数，路径的范围pathRange，目标点relativeGoal坐标和角度，路径的尺度pathScale。这里还是默认目标点是在map坐标系下的，所以需要把它转换到车体坐标系下，也就是一个相对的目标点坐标。
float pathRange = adjacentRange;
if (pathRangeBySpeed) pathRange = adjacentRange * joySpeed;
if (pathRange < minPathRange) pathRange = minPathRange;
float relativeGoalDis = adjacentRange;
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;
float defPathScale = pathScale;
if (pathScaleBySpeed) pathScale = defPathScale * joySpeed;
if (pathScale < minPathScale) pathScale = minPathScale;
接下来便进入到该局部规划器的主要循环，遍历所有的采样路径集合，选取出一条可通行且代价最小的路径作为当前时刻的局部路径。
1.初始化
首先重置几个重要变量，clearPathList代表了会被清除掉的路径信息，pathPenaltyList代表了路径的惩罚项，clearPathPerGroupScore代表了每一组路径的得分。将这些参数每一次循环时都需要重置为0。
在CMU团队自己设计的地面分割算法中，将intensity定义为障碍物点云距离地面点云的相对高度，并不是真正意义上的intensity。因此在点云分割的情况下，当相对高度intensity小于一定障碍物高度阈值obstacleHeightThre 且大于地面高度groundHeightThre的情况下。相当于此处存在坡度，则 pathPenaltyList 惩罚值将设置为该相对高度。
所以在自己的小车上使用时，要注意intensity的定义。
2.通过点云信息确定路径上是否存在障碍物
在获得了plannerCloudCrop点云数据之后，肯定是需要遍历它确定障碍物。在遍历点云数据时，尤其注意的便是h这个变量！在一般情况下，或者说在未使用CMU团队提供的地面分割情况下，intensity代表物体的反射率强度，但是在地面分割后，该值被定义为点云与分割地面的相对高度。
float h = plannerCloudCrop->points[i].intensity;
在获得了点云的 x , y , h {x,y,h} x,y,h后，需要断定该点云是否是有用的，如果此时的目标点距离当前位置只有1m，那可能2m处的点云对一个局部规划器来说就是没用的，其中这个范围的阈值是由goalClearRange参数所决定的。具体的条件如下，dis表示点云距离车辆的距离，当该距离小于定义的路径范围那么也会被认为是无用的。
if (dis < pathRange / pathScale && (dis <= (relativeGoalDis + goalClearRange) / pathScale || !pathCropByGoal)
如果该点云是在车辆范围内的话，就可以被用来判断是否是一个障碍物，且对哪些路径组会有影响。接下来有点难理解，首先定义的clearPathList和pathPenaltyList，其大小都为pathNum * rotDir，那么岂不是对每一条路径都会存储360°下的数据，相对于每一条路径分裂成了36条分布在各个方向的路径。所以对于每一个点云数据，要处理的路径也就变多了，本来就只是一个点，却可能遮挡了多条路径。当然也不能是盲目的处理，这里定义了三种不符合的情况：
（1）角度差值大于车体转向角度的阈值 且 以目标点为方向，计算目标点方向左右的路径；
（2）当前角度大于车体转向角度的阈值 且 当前朝向绝对值小于90度即向前 且 以车辆朝向为方向，计算车辆朝向左右的路径；
（3）当前角度大于车体转向角度的阈值 且 当前朝向绝对值大于90度即向后 且 以车辆朝向为方向，计算车辆朝向左右的路径；
关于参数dirToVehicle应该就是分别处理这两种模式。我理解dirToVehicle = false，就以目标点的方向计算附近方向的路径，可能是不考虑车辆的转弯半径的约束，可以直接转向目标点前进。而dirToVehicle = true，则以当前车辆朝向的方向计算附近方向的路径，意思是目标点只是确定了前向还是后向，车辆带有转弯半径的约束，其可转向角度依然是在当前朝向的附近。
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;
}
float x2 = cos(rotAng) * x + sin(rotAng) * y;
float y2 = -sin(rotAng) * x + cos(rotAng) * y;
然后便是根据车体可能的转向，将激光点转换到相应的坐标系下，也就是转换到每一条路径下。因为在生成路径和体素网格时，已经保存了体素网格和路径之间的索引关系，那么只需要计算网格的ID号，便可以知道哪些路径会被遮挡。具体如何判断会被遮挡如下代码：
// Y方向上的尺度变换是和生成体素网格时保持一致
float scaleY = x2 / gridVoxelOffsetX + searchRadius / gridVoxelOffsetY * (gridVoxelOffsetX - x2) / gridVoxelOffsetX;
// 计算体素网格的索引
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++) {
//未使用地面分割的情况下当前激光点的高度大于obstacleHeightThre阈值,或者未使用地面分割时,则累加
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;
}}}}}
除此之外还存在一种情况，这里应该是对于差速地盘的情况，障碍物不在前面而是在侧面，此时便不能通过路径点集合去寻找，因为在转向的过程中可能会碰撞。所以如果有这种点云的出现，那么需要对此时的局部路径做出一些限制。首先判断是否存在这种点云，即点云距离小于 diameter 车辆半径 ，但点云不在车体内部， 并且超过了障碍物阈值。那么需要根据点云在左侧还是右侧，对此刻的转向进行限制。
其中CW即顺时针旋转（Clock Wise）的方向 与CW反方向旋转时为CCW （Counter Clock Wise）。
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;
}
}
此时对于点云数据的处理，已经完全结束了，得到了障碍物的信息和限制，那么接下来就需要对路径集合进行遍历筛选。
3.筛选最优路径
在筛选最优路径时，和计算路径障碍物是一样的处理方法，也是根据方向减少需要筛选的路径集合，这里就不再介绍。在之前处理点云数据时，如果h > obstacleHeightThre则clearPathList会累加。当一条路径上存在两个障碍点，即 pointPerPathThre=2，该路径才会认为被遮挡。所以只需要对未被遮挡的路径进行筛选。
首先计算惩罚项的得分，前面已经介绍惩罚项是根据地面阈值和障碍物阈值之间的点云高度。所以pathPenaltyList越高，惩罚得分penaltyScore也就越低。
endDirPathList代表了该条路径末端点与当前位置的角度，dirDiff则是该条路径与目标点之间的角度差值，会用于计算路径的得分。除此之外，rotDirW代表了该条路径的方向与当前车辆朝向的角度差，也会被用于路径得分的计算。具体公式如下：
s c o r e = ( 1 − d i r W e i g h t ∗ d i f f 4 ) ∗ r o t D i r W 4 ∗ p e n a l t y S c o r e score =( 1-\sqrt[4]{dirWeight*diff})*rotDirW^4*penaltyScore score=(1−4dirWeight∗diff)∗rotDirW4∗penaltyScore
for (int i = 0; i < 36 * pathNum; i++) {
int rotDir = int(i / pathNum);m; i++) {
...
if (clearPathList[i] < pointPerPathThre) {//pointPerPathThre = 2
float penaltyScore = 1.0 - pathPenaltyList[i] / costHeightThre;//costHeightThre = 0.1
if (penaltyScore < costScore) penaltyScore = costScore;//costscore = 0.02
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;
}
}
}
clearPathPerGroupScore 是一个 7 × 36 7×36 7×36的数组，即7组路径，36个方向。代表了7组在36个方向下的总得分。接下来遍历这 7 × 36 = 252 7×36 = 252 7×36=252条路径，选取一条得分maxScore最高的路径selectedGroupID。同时该路径的方向也要满足之前求取得到的minObsAngCW和minObsAngCCW，防止侧方碰撞。
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组成一条最终的局部路径path，startPaths只代表了第一次采样时的路径点。所以该局部路径的范围，小于等于第一次采样的范围。因为除此之外，还要满足小于设定的pathRange。
而 freePaths 则是所有 clearPathList[i] < pointPerPathThre 的路径，即不存在障碍物的路径。
至此！该局部规划器已经完成。如果需要适用于现实小车，有以下几点需要注意和修改：
（1）点云数据的frame问题：源码中点云数据在map坐标系下。因此会将点云数据转换到车体base_link下，不过现实中都是在雷达坐标系下，与车体坐标系也会存在一定的旋转和平移。
（2）发布路径的frame问题：发布的局部路径也应该在车体base_link坐标系下。
（3）与全局路径的交互：从全局路径裁剪出局部路径的目标点。
后续会继续完善该文件，成为一个独立的局部规划器，感谢CMU团队的开源~