基于采样的局部规划与避障算法
⼀、引⾔
⽬前为系统添加了局部路径规划模块，结合之前做的视觉激光雷达信息融合模块，系统需要进⼀步对避障部分进⾏处理。
局部路径规划总的来说是在全局路径规划模块下，结合避障信息重新⽣成局部路径的模块，上层的全局路径规划确定了A到B的⼀个全局路径，不过轨迹跟踪模块(⽐如 pure persuit)实际进⾏跟踪的不能是这个直接⽣成的全局路径，因为系统实际⼯作可能会有其他情况发⽣，轨迹跟踪模块实际跟踪的是结合障碍物信息的局部路径。
局部路径规划算法有好多种，例如⼈⼯势场法，动态窗⼝法等，
这⾥要介绍的是DARPA⽐赛中斯坦福⼤学Stanley⾃动驾驶系统所使⽤局部路径规划算法，是⼀种基于采样的局部路径规划算法，该算法在autoware的op_planner模块也有使⽤，本⽂主要是基于autoware的op_planner模块进⾏介绍。
整体来看op_planner的local_planner主要分为两部分：Rollouts Generator 和 Rollouts Evaluator，前者根据全局中⼼路径⽣成⼀系列平滑的候选局部路径，后者结合障碍物信息和其他因素计算各个Rollout的
代价Cost，从⽽选出最终平滑的，⽆障碍的局部路径。
Rollouts 的含义就是根据中⼼全局路径⽣成的⼀些列候选局部路径。如下图所⽰：
中间的绿线是全局路径规划模块⽣成的全局路径，棕⾊线是⽣成的⼀系列候选局部路径。
在有障碍物存在的情况下，经过计算每个rollout的代价Cost，选出最优的路径如下所⽰：
其中，⽩线就是评估后得到的最优的局部路径。
下⾯针对local_planner的两个主要部分的算法Rollouts Generator 和 Rollouts Evaluator进⾏简要分析，相关代码已经同步到我的github-planning/local_planner
参考论⽂：
Open Source Integrated Planner for Autonomous Navigation in Highly Dynamic Environments
⼆、Rollouts Generator
模块的输⼊信息是车辆当前位置，全局规划路径，rollouts数量，规划距离等等
输出的是n条平滑的，起点位于车辆当前位置，终点位于最⼤规划距离的候选局部规划轨迹。
根据论⽂中的描述，rollout被划分为了三个部分：Cartip，Rollin, Rollout，如下图所⽰：
cartip部分从车辆中⼼点到⽔平采样的起点，这部分的长度决定了车辆切换不同轨迹的平滑程度。
rollin部分从⽔平采样的起点到平⾏采样的起点，这部分的长度和车辆速度密切相关，车辆速度越快，rollin部分应越长，使得轨迹更加平滑。
rollout部分从平⾏采样的起点到最⼤规划距离，这部分⾥每⼀条rollout都是平⾏的，相隔距离由rollout_density来确定。
⽣成rollouts的算法主要包括三个部分：1、截取全局路径，长度为最⼤局部路径规划距离。2、针对截取的全局路径进⾏点采样。3、平滑得到的采样点，⽣成最终候选轨迹。具体如下图所⽰：
主循环函数主要的内容就是extractPartFromTrajectory()和generateRunoffTrajectory()。具体代码细节可以参考github.
/**
* @description: 主循环函数
* @param {type}
* @return:
*/
void RolloutGenerator::run()
{
ros::Rate loop_rate(100);
while (ros::ok())
{
ros::spinOnce();
if (currentPo_flag && globalPaths.size() > 0)
{
globalPathSections.clear();
for (size_t i = 0; i < globalPaths.size(); i++)
{
centralTrajectorySmoothed.clear();
// 截取全局规划路径
extractPartFromTrajectory(globalPaths[i], current_po, 50,
PlanningParams.pathDensity, centralTrajectorySmoothed);
globalPathSections.push_back(centralTrajectorySmoothed);
雪作文400字
}
std::vector<UtilityNS::WayPoint> sampled_points;
// ⽣成候选rollouts
generateRunoffTrajectory(globalPathSections,
塞下曲李益current_po,
speed,
PlanningParams.microPlanDistance,
PlanningParams.carTipMargin,
PlanningParams.speedProfileFactor,
PlanningParams.pathDensity,回的组词
神奇的拼音llOutNumber,
PlanningParams.smoothingDataWeight,飞舞的蒲公英
PlanningParams.smoothingSmoothWeight,
PlanningParams.smoothingToleranceError,
心梗急救最快的方法
rollOuts,
sampled_points);
}
三、Rollouts Evaluator
这部分的输⼊是之前⽣成的Rollouts以及感知模块得到的障碍物信息，输出的是最优的局部规划路径。
这⾥使⽤三个代价函数来评估不同的局部路径:priority cost, collision cost 和 transition cost.
priority cost 代表中间的局部轨迹优先级是最⾼的，在没有障碍物的情况下，优先选择中间的局部轨迹，如下所⽰：
// cal priority cost
for (int i = 0; i < rollouts.size(); i++)
{
tc.index = i;
tc.distance_from_center = llOutDensity * tc.relative_index;
tc.priority_cost = fabs(tc.distance_from_center);
}
transition cost 限制了车辆不会跳跃多个局部路径，确保了车辆前进路径的平滑性。根据车辆当前所处的局部路径位置，转换到临近车道代价较⼩，转换到较远车道代价较⼤。如下所⽰：
// cal transition cost
for (int ki = 0; ki < trajectoryCosts.size(); ki++)
trajectoryCosts[ki].transition_cost = llOutDensity * (currIndex - ki));
collision cost 主要分为lateral_cost 和 longitudinal_cost，前者代表局部轨迹距离障碍物的⽔平距离，后者代表局部轨迹距离最近障碍物的垂直距离
collision cost计算的相关代码细节位于函数 calLateralAndLongitudinalCostsStatic()
for (int i = 0; i < rollOuts.size(); i++)
{
for (int k = 0; k < contourPoints.size(); k++)
{
UtilityNS::RelativeInfo contour_rela_info;
长寿花开花时间
UtilityNS::getRelativeInfo(centerPath, contourPoints[k], contour_rela_info);
if (contour_rela_info.iFront == 0 && contour_rela_info.iBack == 0 && contour_rela_info.direct_distance > 3)
continue;
// 计算当前障碍物点到车辆位置的沿着中⼼轨迹的距离
double longitudinalDist = getTwoPointsDistanceAlongTrajectory(centerPath, car_rela_info, contour_rela_info);
if (contour_rela_info.iFront == 0 && longitudinalDist > 0)
longitudinalDist = -longitudinalDist;
长方形体积怎么算
double distance_from_center = trajectoryCosts[i].distance_from_center;
lateralDist = fabs(contour_rela_info.perp_distance - distance_from_center) * 2;
if (lateralDist < 2 && longitudinalDist < params.minFollowingDistance && longitudinalDist >= -critical_long_back_distance)
trajectoryCosts[i].bBlocked = true;
if (lateralDist != 0)
trajectoryCosts[i].lateral_cost += 1.0 / lateralDist;
if (longitudinalDist != 0)
trajectoryCosts[i].longitudinal_cost += 1.0 / longitudinalDist;
if (longitudinalDist >= -critical_long_back_distance && longitudinalDist < trajectoryCosts[i].clost_obj_distance)
trajectoryCosts[i].clost_obj_distance = longitudinalDist;
}
}
四、实际效果
运⾏节点：
roslaunch local_planner rollout_generator.launch
roslaunch local_planner local_trajectory_generator.launch
roslaunch waypoint_follower pure_persuit.launch
启动rviz，订阅相关话题可以看到如下效果：
其中，⽩⾊线代表最优的局部路径规划轨迹
五、总结
本⽂简要介绍了autoware中的op_planner的局部路径规划算法，对关键的算法步骤进⾏简要说明，这部分模块可以实现基本的避障功能，相⽐于其他的局部路径规划算法，本⽂介绍的基于采样的局部路径规划实现简单，实时性较⾼，可以在实际部署运⾏。
