autoware中的静态绕障路线有点太长了,看到障碍物后提前很多开始绕障,并且绕完之后会很远才能回去。
对于这个绕障路线,有些参数可以修改,将其缩短,本文进行总结。
需要注意的是,这部分参数如果修改的太小,就无法生成一条可行的路径,需要注意。
关于绕障线路的计算和逻辑,在官方文档中讲的很清楚,本文主要是在代码中的修改进行补充。
绕障逻辑官方文档
参数文件中有些参数可以修改。
主要涉及的参数文件有两个:分别是static_obstacle中的参数文件,还有start_planner中的参数文件
两个文件中,主要可修改的参数有:
# avoidance is performed for the object type with true
target_object:
...
lateral_margin:
soft_margin: 0.3 # [m]
hard_margin: 0.2 # [m]
hard_margin_for_parked_vehicle: 0.7 # [m]
...
# For target object filtering
target_filtering:
...
# detection area generation parameters
detection_area:
static: false # [-]
min_forward_distance: 50.0 # [m]
max_forward_distance: 150.0 # [m]
backward_distance: 10.0 # [m]
如果你只是想单纯的调整参数的话,就直接转到第二节。
简单说一下相关逻辑,这部分主要是参考官方文档进行说明。
检测区域的宽度 #
获取所有类别(汽车、卡车、…)中横向边际最大值。边际值为 soft_margin和 hard_margin_for_parked_vehicle.
检测区域宽度是自我车辆宽度和最大横向边距的总和。
而纵向距离:
纵向距离取决于最大横向移位长度、横向抖动力约束和当前自我的速度。此外,它还必须考虑准备阶段使用的距离。
...
const auto max_shift_length = std::max(
std::abs(parameters_->max_right_shift_length), std::abs(parameters_->max_left_shift_length));
const auto dynamic_distance =
PathShifter::calcLongitudinalDistFromJerk(max_shift_length, getLateralMinJerkLimit(), speed);
return std::clamp(
1.5 * dynamic_distance + getNominalPrepareDistance(),
parameters_->object_check_min_forward_distance,
parameters_->object_check_max_forward_distance);
# 参考 avoidance_module_data.hpp 中的 AvoidanceParameters 描述。
/**:
ros__parameters:
avoidance: # 避障相关参数
resample_interval_for_planning: 0.3 # [m] 规划时的重采样间隔(开发者用)
resample_interval_for_output: 4.0 # [m] 输出时的重采样间隔(开发者用)
# 可行驶车道设置。该模块不仅可使用当前车道,还可使用右侧/左侧车道(如果当前车道与右侧/左侧车道在 HDMap 中共享边界)。
# "current_lane" : 仅使用当前车道。该模块不使用相邻车道避障。
# "same_direction_lane" : 如果需要,该模块使用同方向车道避障。
# "opposite_direction_lane": 该模块使用同方向和反方向车道。
use_lane_type: "opposite_direction_lane"
# 可行驶区域设置
use_intersection_areas: true # 是否使用交叉路口区域
use_hatched_road_markings: true # 是否使用虚线区域
use_freespace_areas: true # 是否使用自由空间区域
# 针对设置为 true 的目标类型执行避障操作
target_object:
car: # 汽车
th_moving_speed: 1.0 # [m/s] 判定为移动的最小速度
th_moving_time: 2.0 # [s] 判定为移动的最小时间
longitudinal_margin: 0.0 # [m] 纵向安全距离
lateral_margin: # 横向安全距离
soft_margin: 0.3 # [m] 软安全距离
hard_margin: 0.2 # [m] 硬安全距离
hard_margin_for_parked_vehicle: 0.7 # [m] 针对停车车辆的硬安全距离
max_expand_ratio: 0.0 # [-] 最大扩展比例(开发者用)
envelope_buffer_margin: 0.5 # [m] 包络缓冲距离(开发者用)
th_error_eclipse_long_radius: 0.6 # [m] 椭圆误差的长轴半径
truck: # 卡车
th_moving_speed: 1.0
th_moving_time: 2.0
longitudinal_margin: 0.0
lateral_margin:
soft_margin: 0.3
hard_margin: 0.2
hard_margin_for_parked_vehicle: 0.7
max_expand_ratio: 0.0
envelope_buffer_margin: 0.5
th_error_eclipse_long_radius: 0.6
bus: # 公交车
th_moving_speed: 1.0
th_moving_time: 2.0
longitudinal_margin: 0.0
lateral_margin:
soft_margin: 0.3
hard_margin: 0.2
hard_margin_for_parked_vehicle: 0.7
max_expand_ratio: 0.0
envelope_buffer_margin: 0.5
th_error_eclipse_long_radius: 0.6
trailer: # 挂车
th_moving_speed: 1.0
th_moving_time: 2.0
longitudinal_margin: 0.0
lateral_margin:
soft_margin: 0.3
hard_margin: 0.2
hard_margin_for_parked_vehicle: 0.7
max_expand_ratio: 0.0
envelope_buffer_margin: 0.5
th_error_eclipse_long_radius: 0.6
unknown: # 未知类型
th_moving_speed: 0.28
th_moving_time: 1.0
longitudinal_margin: 0.0
lateral_margin:
soft_margin: 0.7
hard_margin: -0.2
hard_margin_for_parked_vehicle: -0.2
max_expand_ratio: 0.0
envelope_buffer_margin: 0.1
th_error_eclipse_long_radius: 0.6
bicycle: # 自行车
th_moving_speed: 0.28
th_moving_time: 1.0
longitudinal_margin: 1.0
lateral_margin:
soft_margin: 0.7
hard_margin: 0.5
hard_margin_for_parked_vehicle: 0.5
max_expand_ratio: 0.0
envelope_buffer_margin: 0.5
th_error_eclipse_long_radius: 0.6
motorcycle: # 摩托车
th_moving_speed: 1.0
th_moving_time: 1.0
longitudinal_margin: 1.0
lateral_margin:
soft_margin: 0.7
hard_margin: 0.3
hard_margin_for_parked_vehicle: 0.3
max_expand_ratio: 0.0
envelope_buffer_margin: 0.5
th_error_eclipse_long_radius: 0.6
pedestrian: # 行人
th_moving_speed: 0.28
th_moving_time: 1.0
longitudinal_margin: 1.0
lateral_margin:
soft_margin: 0.7
hard_margin: 0.5
hard_margin_for_parked_vehicle: 0.5
max_expand_ratio: 0.0
envelope_buffer_margin: 0.5
th_error_eclipse_long_radius: 0.6
lower_distance_for_polygon_expansion: 30.0 # [m] 多边形扩展的下限距离(开发者用)
upper_distance_for_polygon_expansion: 100.0 # [m] 多边形扩展的上限距离(开发者用)
# 目标对象过滤
target_filtering:
# 避障目标类型
target_type:
car: true # 是否将汽车作为目标
truck: true
bus: true
trailer: true
unknown: true
bicycle: true
motorcycle: true
pedestrian: true
# 检测范围
object_check_goal_distance: 20.0 # [m] 目标检测距离
object_check_return_pose_distance: 20.0 # [m] 返回姿态检测距离
# 丢失目标补偿
max_compensation_time: 2.0 # 最大补偿时间
# 检测区域生成参数
detection_area:
static: false # 是否静态检测区域
min_forward_distance: 50.0 # [m] 最小前向距离
max_forward_distance: 150.0 # [m] 最大前向距离
backward_distance: 10.0 # [m] 后向距离
# 过滤停车对象
parked_vehicle:
th_offset_from_centerline: 1.0 # [m] 偏离中心线的阈值
th_shiftable_ratio: 0.8 # 可移动比例阈值
min_road_shoulder_width: 0.5 # [m] 最小道路边缘宽度(开发者用)
# 合流/偏离车辆
merging_vehicle:
th_overhang_distance: 0.0 # [m] 悬挂距离阈值
# 针对停车状态不明确的车辆的避障参数
avoidance_for_ambiguous_vehicle:
# 针对停车状态不明确的车辆的自车行为策略
# "auto" : 生成候选路径。如果 RTC 处于自动模式,则自车自动避障。
# "manual" : 生成候选路径,即使 RTC 处于自动模式,也等待操作员批准。
# "ignore" : 从不避障。
policy: "auto"
condition:
th_stopped_time: 3.0 # [s] 停止时间阈值
th_moving_distance: 1.0 # [m] 移动距离阈值
ignore_area:
traffic_light:
front_distance: 100.0 # [m] 交通灯前方距离
crosswalk:
front_distance: 30.0 # [m] 斑马线前方距离
behind_distance: 30.0 # [m] 斑马线后方距离
wait_and_see:
target_behaviors: ["MERGING", "DEVIATING"] # 目标行为
th_closest_distance: 10.0 # [m] 最近距离阈值
# 交叉路口内目标对象过滤
intersection:
yaw_deviation: 0.349 # [rad] 偏航偏差(默认 20.0 度)
frees
/**
* 这是自动驾驶车辆起始规划器(start_planner)的参数配置文件。
* 包括到达目标点的条件、碰撞检测、路径生成、避障策略等参数。
*/
ros__parameters:
start_planner: # 起始规划器相关参数
# 到达目标点的条件
th_arrived_distance: 1.0 # [m] 判定为到达目标点的距离阈值
th_stopped_velocity: 0.01 # [m/s] 判定为停止的速度阈值
th_stopped_time: 1.0 # [s] 判定为停止的时间阈值
# 碰撞检测相关参数
collision_check_margins: [2.0, 1.0, 0.5, 0.1] # 碰撞检测的安全距离(多个值,用于不同的场景)
collision_check_margin_from_front_object: 5.0 # [m] 前方物体的碰撞检测安全距离
extra_width_margin_for_rear_obstacle: 0.5 # [m] 后方障碍物的额外宽度安全距离
th_moving_object_velocity: 1.0 # [m/s] 判定为移动物体的速度阈值
# 需要检查的目标类型
object_types_to_check_for_path_generation:
check_car: true # 是否检查汽车
check_truck: true # 是否检查卡车
check_bus: true # 是否检查公交车
check_trailer: true # 是否检查拖车
check_bicycle: true # 是否检查自行车
check_motorcycle: true # 是否检查摩托车
check_pedestrian: true # 是否检查行人
check_unknown: true # 是否检查未知类型物体
th_distance_to_middle_of_the_road: 0.5 # [m] 判定为靠近道路中间的距离阈值
center_line_path_interval: 1.0 # [m] 中心线路径的采样间隔
# Shift 拉出(Shift Pull Out)相关参数
enable_shift_pull_out: true # 是否启用 Shift 拉出功能
check_shift_path_lane_departure: true # 是否检查 Shift 路径是否偏离车道
allow_check_shift_path_lane_departure_override: false # 是否允许覆盖 Shift 路径偏离车道的检查
shift_collision_check_distance_from_end: -10.0 # [m] Shift 碰撞检测距离终点的距离
minimum_shift_pull_out_distance: 0.0 # [m] Shift 拉出的最小距离
maximum_longitudinal_deviation: 1.0 # [m] 最大纵向偏差(注意:应与规划验证器中的 "longitudinal_distance_deviation" 相同或更小)
lateral_jerk: 0.5 # [m/s^3] 横向加速度变化率
lateral_acceleration_sampling_num: 3 # 横向加速度采样数量
minimum_lateral_acc: 0.15 # [m/s^2] 最小横向加速度
maximum_lateral_acc: 0.5 # [m/s^2] 最大横向加速度
maximum_curvature: 0.07 # [1/m] 最大曲率
end_pose_curvature_threshold: 0.01 # [1/m] 终点姿态的曲率阈值
# 几何拉出(Geometric Pull Out)相关参数
enable_geometric_pull_out: true # 是否启用几何拉出功能
geometric_collision_check_distance_from_end: 0.0 # [m] 几何碰撞检测距离终点的距离
arc_path_interval: 1.0 # [m] 弧路径的采样间隔
divide_pull_out_path: true # 是否分割拉出路径
geometric_pull_out_velocity: 1.0 # [m/s] 几何拉出的速度
lane_departure_margin: 0.2 # [m] 车道偏离的容差
lane_departure_check_expansion_margin: 0.0 # [m] 车道偏离检查的扩展容差
backward_velocity: -1.0 # [m/s] 后退速度
pull_out_max_steer_angle: 0.26 # [rad] 拉出时的最大转向角(15度)
# 搜索起始姿态(向后搜索)
enable_back: true # 是否启用向后搜索
search_priority: "efficient_path" # 搜索优先级("efficient_path" 或 "short_back_distance")
max_back_distance: 20.0 # [m] 向后搜索的最大距离
backward_search_resolution: 2.0 # [m] 向后搜索的分辨率
backward_path_update_duration: 3.0 # [s] 向后路径更新的持续时间
ignore_distance_from_lane_end: 15.0 # [m] 忽略距离车道末端的距离
# 转向灯相关参数
prepare_time_before_start: 0.0 # [s] 启动前的准备时间
# 自由空间规划器(FreeSpace Planner)相关参数
freespace_planner:
enable_freespace_planner: true # 是否启用自由空间规划器
end_pose_search_start_distance: 20.0 # [m] 终点姿态搜索的起始距离
end_pose_search_end_distance: 30.0 # [m] 终点姿态搜索的结束距离
end_pose_search_interval: 2.0 # [m] 终点姿态搜索的间隔
freespace_planner_algorithm: "astar" # 自由空间规划算法(选项:astar、rrtstar)
velocity: 1.0 # [m/s] 规划速度
vehicle_shape_margin: 1.0 # [m] 车辆形状的容差
time_limit: 3000.0 # [ms] 时间限制
max_turning_ratio: 0.7 # 最大转向比例
turning_steps: 1 # 转向步骤
# 搜索配置
search_configs:
theta_size: 144 # 角度搜索的大小
angle_goal_range: 6.0 # [rad] 角度目标范围
curve_weight: 1.2 # 曲线权重
reverse_weight: 1.0 # 反向权重
lateral_goal_range: 0.5 # [m] 横向目标范围
longitudinal_goal_range: 2.0 # [m] 纵向目标范围
# 成本图配置
costmap_configs:
obstacle_threshold: 30 # 障碍物阈值
# A* 搜索配置
astar:
search_method: "forward" # 搜索方法(选项:forward、backward)
only_behind_solutions: false # 是否仅考虑后方解决方案
use_back: false # 是否使用向后搜索
distance_heuristic_weight: 1.0 # 距离启发式权重
# RRT* 搜索配置
rrtstar:
enable_update: true # 是否启用更新
use_informed_sampling: true # 是否使用有信息的采样
max_planning_time: 150.0 # [ms] 最大规划时间
neighbor_radius: 8.0 # [m] 邻域半径
margin: 1.0 # [m] 容差
# 停止条件
stop_condition:
maximum_deceleration_for_stop: 1.0 # [m/s^2] 停止时的最大减速度
maximum_jerk_for_stop: 1.0 # [m/s^3] 停止时的最大加速度变化率
# 路径安全性检查
path_safety_check:
# 自车预测路径
ego_predicted_path:
min_velocity: 0.0 # [m/s] 最小速度
min_acceleration: 1.0 # [m/s^2] 最小加速度
time_horizon_for_front_object: 10.0 # [s] 前方物体的时间范围
time_horizon_for_rear_object: 10.0 # [s] 后方物体的时间范围
time_resolution: 0.5 # [s] 时间分辨率
delay_until_departure: 1.0 # [s] 出发前的延迟时间
# 目标对象过滤
target_filtering:
safety_check_time_horizon: 5.0 # [s] 安全检查的时间范围
safety_check_time_resolution: 1.0 # [s] 安全检查的时间分辨率
# 检测范围
object_check_forward_distance: 10.
这个函数在static_obstacle中的shift_lane文件,其中可以进行一些参数的调整,来对绕障路线的长度的进行调整。
就是下面这部分代码,按照我的注释进行修改即可。
AvoidLine al_avoid;
{
const auto constant_distance = helper_->getFrontConstantDistance(o);
const auto to_shift_end = o.longitudinal - constant_distance;
const auto path_front_to_ego = data.arclength_from_ego.at(data.ego_closest_path_index);
// start point (use previous linear shift length as start shift length.)这个是开始绕障的起点,包含了障碍物的纵向距离,前方常数距离,最小避障距离,和当前已有的偏移量
// 有几种方法可以让开始绕障的起点离障碍物更近:
// 减小nearest_avoid_distance的计算中的15.0这个常数值
// 修改minimum_avoid_distance的计算方式
// 直接在计算结果上应用一个缩放因子
al_avoid.start_longitudinal = [&]() {
const auto nearest_avoid_distance =
// std::max(to_shift_end - feasible_shift_profile.value().second, 1e-3);
std::max(to_shift_end - std::min(feasible_shift_profile.value().second, 15.0), 1e-3);
if (data.to_start_point > to_shift_end) {
return nearest_avoid_distance;
}
const auto minimum_avoid_distance = helper_->getMinAvoidanceDistance(
feasible_shift_profile.value().first - current_ego_shift);
const auto furthest_avoid_distance = std::max(to_shift_end - minimum_avoid_distance, 1e-3);
return std::clamp(data.to_start_point, nearest_avoid_distance, furthest_avoid_distance);
}();
al_avoid.start_idx = utils::static_obstacle_avoidance::findPathIndexFromArclength(
data.arclength_from_ego, al_avoid.start_longitudinal + path_front_to_ego);
al_avoid.start = data.reference_path.points.at(al_avoid.start_idx).point.pose;
al_avoid.start_shift_length = helper_->getLinearShift(al_avoid.start.position);
// end point
al_avoid.end_shift_length = feasible_shift_profile.value().first;
al_avoid.end_longitudinal = to_shift_end;
// misc
al_avoid.id = generateUUID();
al_avoid.object = o;
al_avoid.object_on_right = utils::static_obstacle_avoidance::isOnRight(o);
}
AvoidLine al_return;
{
const auto constant_distance = helper_->getRearConstantDistance(o);
const auto to_shift_start = o.longitudinal + constant_distance + 0.0;//回归路线的起点,包含了障碍物的纵向距离,前方常数距离,和额外的一个安全距离3m
// start point
al_return.start_shift_length = feasible_shift_profile.value().first;
al_return.start_longitudinal = to_shift_start;
// end point,结束绕障的终点,回归起点加上一个固定距离(代码中是20米)
al_return.end_longitudinal = [&]() {
return to_shift_start+15;// 注意这里有硬编码的20米
// 下面的代码实际上不会执行,因为上面有return语句
if (data.to_return_point > to_shift_start) {
return std::clamp(
data.to_return_point, to_shift_start, feasible_return_distance + to_shift_start);
}
return to_shift_start + feasible_return_distance;
}();
al_return.end_shift_length = 0.0;
// misc
al_return.id = generateUUID();
al_return.object = o;
al_return.object_on_right = utils::static_obstacle_avoidance::isOnRight(o);
}
这部分的参数修改能做到的程度还是有限,无法做到贴着障碍物绕行。