文章目录
前言
在ai这个版本中，局部路径规划有两个模块，分别是lattice_planner模块和waypoint_planner模块，使用起来是二选一的。
两个局部路径规划模块都是接收上一个模块输出的全局路径/base_waypoints和障碍物信息等，然后输出最终路径点/final_waypoints，再由后续的mpc等跟踪算法进行跟踪。
这篇文章我们先来讲lattice_planner这个局部路径规划模块。
lattice_planner模块又分为了三个节点：
- lattice_velocity_set
- lattice_trajectory_gen
- path_select
这三个节点的工作并不是串联的，而是第一个节点工作之后发布消息，第二和第三个节点是并列的，他们都会接收第一个节点的消息并处理，然后发布，所以说第二和第三个节点二选一就行，我们就根据这个顺序依次对这三个节点进行讲解。
需要注意的是，局部路径规划中主要内容就是障碍物规划，而这里的障碍物规划直接使用的是雷达的原始点云数据，这里和universe有很大区别，universe是有个单独完整的感知识别模块进行相关障碍物处理，然后再将分类好的障碍物信息给规划模块。
一、lattice_velocity_set–速度规划
在之前的模块中，得到了带有停止点的全局路径/base_waypoints，这个路径是上一个模块规划得到的，带有红绿灯停止点的路径。
然后就给到了这个模块，这个模块会以此路径为基础，然后结合障碍物信息，进行速度规划。
也就是说，这个局部路径规划模块遇到障碍物只做减速，不做绕障。
规划完之后输出/temporal_waypoints：带有速度信息的临时路径点
1.输入输出
输入：
/base_waypoints：从lane_select获取的基础路径
/vscan_points：障碍物点云数据
/current_pose：车辆当前位置
/current_velocity：车辆当前速度
输出：
/temporal_waypoints：带有速度信息的临时路径点
2.流程逻辑
在main函数中，一开始是订阅和发布的各种调用函数，这部分不再细讲。
后续则是主处理流程：
1.设置交叉路口点（vmap.setCrossWalkPoints()）
2.获取最近路径点（getClosestWaypoint）
3.如果启用了交叉路口检测，查找前方交叉路口（findCrossWalk()）
4.进行障碍物检测（obstacleDetection()）
5.根据检测结果修改路径点（changeWaypoint(detection_result)）
其中最重要的就是障碍物检测函数obstacleDetection()和路径点修改函数changeWaypoint(detection_result)
obstacleDetection()–障碍物检测函数
这个函数的处理逻辑：
1.调用vscanDetection()进行点云障碍物检测
2.显示检测范围（displayDetectionRange）
3.根据当前和历史检测结果决定控制策略（KEEP、STOP或DECELERATE）
4.包含防抖动逻辑（false_count计数器）
其中最重要的就是调用了vscanDetection()函数
EControl obstacleDetection()
{
// 静态变量，用于记录连续未检测到障碍物的次数
static int false_count = 0;
// 静态变量，用于记录上一次的检测结果
static EControl prev_detection = KEEP;
// 调用 vscanDetection 函数进行障碍物检测
EControl vscan_result = vscanDetection();
// 显示检测范围和结果，参数包括检测到的斑马线 ID、最近航点编号和检测结果
displayDetectionRange(vmap.getDetectionCrossWalkID(), g_closest_waypoint, vscan_result);
// 判断上一次的检测结果是否为 KEEP（保持当前状态）
if (prev_detection == KEEP)
{
// 如果检测结果不是 KEEP（即检测到障碍物）
if (vscan_result != KEEP)
{
// 显示障碍物信息
displayObstacle(vscan_result);
// 更新上一次的检测结果为当前检测结果
prev_detection = vscan_result;
// 可以在这里调用 SoundPlay() 发出警报声
// SoundPlay();
// 重置连续未检测到障碍物的计数器
false_count = 0;
// 返回检测结果
return vscan_result;
}
else
{ // 如果没有检测到障碍物
// 保持上一次的检测结果为 KEEP
prev_detection = KEEP;
// 返回检测结果
return vscan_result;
}
}
else
{ // 如果上一次的检测结果是 STOP 或 DECELERATE（停车或减速）
// 如果检测结果不是 KEEP（即检测到障碍物）
if (vscan_result != KEEP)
{
// 显示障碍物信息
displayObstacle(vscan_result);
// 更新上一次的检测结果为当前检测结果
prev_detection = vscan_result;
// 重置连续未检测到障碍物的计数器
false_count = 0;
// 返回检测结果
return vscan_result;
}
else
{ // 如果没有检测到障碍物
// 增加连续未检测到障碍物的计数器
false_count++;
// 安全机制：如果连续未检测到障碍物的次数超过一定阈值（LOOP_RATE / 2）
if (false_count >= LOOP_RATE / 2)
{
// 清除障碍物航点编号
g_obstacle_waypoint = -1;
// 重置连续未检测到障碍物的计数器
false_count = 0;
// 将上一次的检测结果重置为 KEEP
prev_detection = KEEP;
// 返回检测结果
return vscan_result;
}
else
{
// 显示其他类型的障碍物信息
displayObstacle(OTHERS);
// 返回上一次的检测结果（保持之前的停车或减速状态）
return prev_detection;
}
}
}
}
changeWaypoint(detection_result)–路径点修改函数
这个函数根据上一个函数的障碍物检测结果修改路径点，逻辑如下：
1.STOP：计算停止点位置，调用changeWaypoints设置停止点
2.DECELERATE：调用setDeceleration设置减速
3.KEEP/ACELERATE：保持原路径，但避免突然加速和突然制动
void changeWaypoint(EControl detection_result)
{
// 获取障碍物所在的航点编号
int obs = g_obstacle_waypoint;
// 根据检测结果调整航点
if (detection_result == STOP)
{ // 如果检测结果为 STOP（停车）
// 计算停车点的航点编号，停车点距离障碍物一定距离（g_others_distance）
int stop_waypoint = obs - ((int)(g_others_distance / g_path_change.getInterval()));
// 调整航点，使车辆在停车点停车
g_path_change.changeWaypoints(stop_waypoint);
// 避免突然刹车
g_path_change.avoidSuddenBraking();
// 设置临时航点
g_path_change.setTemporalWaypoints();
// 发布临时航点信息
g_temporal_waypoints_pub.publish(g_path_change.getTemporalWaypoints());
}
else if (detection_result == DECELERATE)
{ // 如果检测结果为 DECELERATE（减速）
// 设置当前路径为当前航点
g_path_change.setPath(g_path_dk.getCurrentWaypoints());
// 设置减速模式
g_path_change.setDeceleration();
// 设置临时航点
g_path_change.setTemporalWaypoints();
// 发布临时航点信息
g_temporal_waypoints_pub.publish(g_path_change.getTemporalWaypoints());
}
else
{ // 如果检测结果为 ACELERATE（加速）或 KEEP（保持）
// 设置当前路径为当前航点
g_path_change.setPath(g_path_dk.getCurrentWaypoints());
// 避免突然加速
g_path_change.avoidSuddenAceleration();
// 避免突然刹车
g_path_change.avoidSuddenBraking();
// 设置临时航点
g_path_change.setTemporalWaypoints();
// 发布临时航点信息
g_temporal_waypoints_pub.publish(g_path_change.getTemporalWaypoints());
}
return;
}
vscanDetection()–点云处理函数
这个函数是直接处理的激光雷达的原始数据，数据格式是pcl::PointCloudpcl::PointXYZ，这个和之前universe中的就完全不一样了，在universe会有一个单独的感知模块进行障碍物识别分类，然后再传给规划模块。
这里面会遍历点云中的点，然后如果有足够多的点，就认为有障碍物。
处理点云数据检测障碍物：
处理交叉路口区域的特殊检测（crossWalkDetection）
将点云转换到车辆坐标系
根据点云中点的位置判断是否有障碍物
确定障碍物位置对应的路径点索引
返回控制策略（KEEP、STOP或DECELERATE）
EControl vscanDetection()
{
// 如果激光雷达数据为空，或者最近航点编号无效，直接返回保持状态
if (g_vscan.empty() == true || g_closest_waypoint < 0)
return KEEP;
// 初始化变量，用于记录是否需要减速或停车
int decelerate_or_stop = -10000;
// 定义从减速到停车所需的航点数量
int decelerate2stop_waypoints = 15;
// 遍历从最近航点开始的一定范围内的航点
for (int i = g_closest_waypoint; i < g_closest_waypoint + g_search_distance; i++)
{
// 清除障碍物的停车点
g_obstacle.clearStopPoints();
// 如果尚未决定是否减速，清除减速点
if (!g_obstacle.isDecided())
g_obstacle.clearDeceleratePoints();
// 更新减速或停车的计数器
decelerate_or_stop++;
// 如果计数器超过减速到停车的航点数量，或者到达路径末尾，返回减速指令
if (decelerate_or_stop > decelerate2stop_waypoints || (decelerate_or_stop >= 0 && i >= g_path_dk.getSize() - 1) ||
(decelerate_or_stop >= 0 && i == g_closest_waypoint + g_search_distance - 1))
return DECELERATE;
// 如果航点编号超出路径范围，返回保持状态
if (i > g_path_dk.getSize() - 1)
return KEEP;
// 检测斑马线
if (i == vmap.getDetectionWaypoint())
{
// 如果斑马线检测结果为停车
if (crossWalkDetection(vmap.getDetectionCrossWalkID()) == STOP)
{
// 设置障碍物航点编号为当前航点
g_obstacle_waypoint = i;
// 返回停车指令
return STOP;
}
}
// 获取当前航点的相对坐标
geometry_msgs::Point waypoint = calcRelativeCoordinate(g_path_dk.getWaypointPosition(i), g_localizer_pose.pose);
// 将航点坐标转换为 tf::Vector3 类型
tf::Vector3 tf_waypoint = point2vector(waypoint);
// 设置 Z 坐标为 0（二维平面检测）
tf_waypoint.setZ(0);
// 初始化停车点和减速点计数器
int stop_point_count = 0;
int decelerate_point_count = 0;
// 遍历激光雷达数据中的每个点
for (pcl::PointCloud<pcl::PointXYZ>::const_iterator item = g_vscan.begin(); item != g_vscan.end(); item++)
{
// 将激光雷达点转换为 tf::Vector3 类型
tf::Vector3 vscan_vector((double)item->x, (double)item->y, 0);
// 计算航点与激光雷达点之间的二维距离
double dt = tf::tfDistance(vscan_vector, tf_waypoint);
// ---停车障碍物检测---
if (dt < g_detection_range)
{
// 如果距离小于检测范围，增加停车点计数器
stop_point_count++;
// 保存停车点的绝对坐标
geometry_msgs::Point vscan_temp;
vscan_temp.x = item->x;
vscan_temp.y = item->y;
vscan_temp.z = item->z;
g_obstacle.setStopPoint(calcAbsoluteCoordinate(vscan_temp, g_localizer_pose.pose));
}
// 如果停车点计数器超过阈值，设置障碍物航点编号并返回停车指令
if (stop_point_count > g_threshold_points)
{
g_obstacle_waypoint = i;
return STOP;
}
// 如果减速范围无效，跳过减速检测
if (g_deceleration_range < 0.01)
continue;
// 如果超出减速搜索范围或已经决定减速，跳过当前航点
if (i > g_closest_waypoint + g_deceleration_search_distance || decelerate_or_stop >= 0)
continue;
// ---减速障碍物检测---
if (dt > g_detection_range && dt < g_detection_range + g_deceleration_range)
{
bool count_flag = true;
// 检测检测范围与减速范围是否有重叠
for (int waypoint_search = -5; waypoint_search <= 5; waypoint_search++)
{
// 如果超出路径范围或重叠点无效，跳过
if (i + waypoint_search < 0 || i + waypoint_search >= g_path_dk.getSize() || !waypoint_search)
continue;
// 获取重叠点的相对坐标
geometry_msgs::Point temp_waypoint =
calcRelativeCoordinate(g_path_dk.getWaypointPosition(i + waypoint_search), g_localizer_pose.pose);
tf::Vector3 waypoint_vector = point2vector(temp_waypoint);
waypoint_vector.setZ(0);
// 如果重叠点在检测范围内，优先考虑检测范围
if (tf::tfDistance(vscan_vector, waypoint)_vector < g_detection_range)
{
count_flag = false;
break;
}
}
// 如果没有重叠，增加减速点计数器
if (count_flag)
{
decelerate_point_count++;
// 保存减速点的绝对坐标
geometry_msgs::Point vscan_temp;
vscan_temp.x = item->x;
vscan_temp.y = item->y;
vscan_temp.z = item->z;
g_obstacle.setDeceleratePoint(calcAbsoluteCoordinate(vscan_temp, g_localizer_pose.pose));
}
}
// 如果减速点计数器超过阈值，设置障碍物航点编号并标记为已决定减速
if (decelerate_point_count > g_threshold_points)
{
g_obstacle_waypoint = i;
decelerate_or_stop = 0; // 为搜索附近的停车障碍物重置计数器
g_obstacle.setDecided(true);
}
}
}
// 如果没有检测到障碍物，返回保持状态
return KEEP;
}
二、lattice_trajectory_gen–生成多条可能的轨迹
上一个模块根据障碍物信息，输出了带有速度信息的路径。
然后这个模块就结合当前车辆的位置和速度，和前面的全局路径，生成几条备选路径。
这里需要注意的是，实车模式下只会生成一条主路径，而模拟状态下除了一条主路径外还会生成30条备选路径。
举个例子：
想象您正在高速公路上行驶，前方有一个缓慢移动的车辆：
全局路径：告诉您沿着这条车道直行
速度规划（lattice_velocity_set）：检测到前方车辆，决定减速
轨迹生成（lattice_trajectory_gen）：
生成多条可能的轨迹：
一条保持在当前车道减速
几条稍微偏移到左侧车道的轨迹（为变道做准备）
几条稍微偏移到右侧车道的轨迹
每条轨迹都是平滑的三次样条曲线，考虑了车辆的转向能力和舒适度
轨迹选择：后续的path_select模块会从这些候选轨迹中选择最优的一条
主要处理流程：
- 获取下一个目标点：
通过getNextWaypoint函数计算前方适当距离的路径点作为目标点 - 计算目标状态：
使用computeWaypointGoal函数计算目标点的状态（位置、朝向、曲率等） - 估计当前状态：
通过computeVeh函数估计车辆当前状态 - 生成基准轨迹：
使用waypointTrajectory函数生成从当前状态到目标状态的三次样条曲线轨迹
通过迭代优化找到满足约束的轨迹参数 - 生成候选轨迹：
通过扰动目标点的y坐标生成多条候选轨迹（在模拟模式下）
这些候选轨迹提供了不同的横向偏移选择 - 轨迹可视化：
使用drawSpline函数将轨迹可视化
1.输入输出
输入：
/temporal_waypoints：从lattice_velocity_set获取的路径
/current_pose：车辆当前位置
/current_velocity：车辆当前速度
输出：
/final_waypoints：最终选择的轨迹
可视化信息：轨迹、候选轨迹等
2.waypointTrajectory–计算轨迹
这个函数便是生成轨迹的主要计算函数。
static union Spline waypointTrajectory(union State veh, union State goal, union Spline curvature, int next_waypoint)
{
curvature.success = TRUE; // 初始化曲率计算成功标志为真
bool convergence = FALSE; // 初始化收敛标志为假
int iteration = 0; // 初始化迭代次数
union State veh_next; // 定义车辆的下一个状态
double dt = step_size; // 定义时间步长
veh.v = goal.v; // 将目标速度赋值给车辆当前速度
// While循环用于计算轨迹参数
while (convergence == FALSE && iteration < 4)
{
// 设置时间范围
double horizon = curvature.s / veh.vdes; // 计算时间范围
ROS_INFO_STREAM("vdes: " << veh.vdes); // 输出期望速度
ROS_INFO_STREAM("horizon: " << horizon); // 输出时间范围
// 运行运动模型
veh_next = motionModel(veh, goal, curvature, dt, horizon, 0); // 计算车辆的下一个状态
// 确定收敛条件
convergence = checkConvergence(veh_next, goal); // 检查是否收敛到目标状态
// 如果运动模型没有使车辆到达目标，重新计算参数
if (convergence == FALSE)
{
// 更新参数
curvature = generateCorrection(veh, veh_next, goal, curvature, dt, horizon); // 生成新的轨迹参数
iteration++; // 增加迭代次数
// 如果雅可比矩阵条件差，退出循环
if (curvature.success == FALSE)
{
ROS_INFO_STREAM("Init State: sx " << veh.sx << " sy " << veh.sy << " theta " << veh.theta << " kappa " << veh.kappa << " v " << veh.v); // 输出初始状态
ROS_INFO_STREAM("Goal State: sx " << goal.sx << " sy " << goal.sy << " theta " << goal.theta << " kappa " << goal.kappa << " v " << goal.v); // 输出目标状态
break;
}
}
}
// 如果未收敛
if (convergence == FALSE)
{
ROS_INFO_STREAM("Next State: sx " << veh_next.sx << " sy " << veh_next.sy << " theta " << veh_next.theta << " kappa " << veh_next.kappa << " v " << veh_next.v); // 输出下一个状态
ROS_INFO_STREAM("Init State: sx " << veh.sx << " sy " << veh.sy << " theta " << veh.theta << " kappa " << veh.kappa); // 输出初始状态
ROS_INFO_STREAM("Goal State: sx " << goal.sx << " sy " << goal.sy << " theta " << goal.theta << " kappa " << goal.kappa); // 输出目标状态
curvature.success = FALSE; // 设置曲率计算失败标志
}
else
{
ROS_INFO_STREAM("Converged in " << iteration << " iterations"); // 输出收敛迭代次数
#ifdef LOG_OUTPUT
// 设置时间范围
double horizon = curvature.s / v_0; // 计算时间范围
// 运行运动模型并记录数据用于绘图
veh_next = motionModel(veh, goal, curvature, 0.1, horizon, 1); // 计算车辆的下一个状态
fmm_sx << "0.0 \n"; // 记录x坐标
fmm_sy << "0.0 \n"; // 记录y坐标
#endif
}
return curvature; // 返回计算的曲率
}
三、path_select–这个节点根本没写完
这个节点非常简单，因为打开发现它根本没写完，里面光写了要接收/temporal_waypoints，然后发布/final_waypoints，但里面的回调函数啥的都没写，所以这个看来应该是当时还没开发完就已经ai停止开发了。
扫一扫
5227
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
