前言:
Apollo (阿波罗)是一个开放的、完整的、安全的平台,将帮助汽车行业及自动驾驶领域的合作伙伴结合车辆和硬件系统,快速搭建一套属于自己的自动驾驶系统。Apollo 自动驾驶开放平台为开发者提供了丰富的车辆、硬件选择,强大的环境感知、高精定位、路径规划、车辆控制等自动驾驶软件能力以及高精地图、仿真、数据流水线等自动驾驶云服务,帮助开发者从 0 到 1 快速搭建一套自动驾驶系统。
本系列文章将记录笔者在调试Apollo决策规划算法Planning模块的所见所思所想,不仅会分析决策规划算法具体的代码逻辑,还会使用Vscode对Apollo决策规划算法进行断点调试,从而揭示关键变量的数据结构以及取值。
PathBoundsDecider 是Apollo Planning规划算法的第四个task,PathBoundsDecider根据lane borrow决策器的输出、本车道以及相邻车道的宽度、障碍物的左右边界,来计算path 的boundary,从而将path 搜索的边界缩小,将复杂问题转化为凸空间的搜索问题,方便后续使用QP算法求解。它的作用主要是:
- 根据借道信息、道路宽度生成FallbackPathBound
- 根据借道信息、道路宽度生成、障碍物边界生成PullOverPathBound、LaneChangePathBound、RegularPathBound中的一种
PathBound的生成对于后续的路径规划,以及规划算法的整体表现,是沿着当前道路行驶还是借道超车,有着决定性的作用。所以本文将深入PathBound内部,以GenerateRegularPathBound为例,详细解析与调试这个task,从而帮助大家来深入了解这个task。
如果对Apollo Planning规划算法的调试细节感兴趣,可以关注与订阅下面专栏:
https://blog.csdn.net/nn243823163/category_11807746.htmlhttps://blog.csdn.net/nn243823163/category_11807746.html如果对Apollo Planning规划算法的代码解析感兴趣,可以关注与订阅下面专栏:
1、首先判断当前场景是否在laneborrow状态:
if (reference_line_info->is_path_lane_borrow()) {
const auto& path_decider_status =
injector_->planning_context()->planning_status().path_decider();
for (const auto& lane_borrow_direction :
path_decider_status.decided_side_pass_direction()) {
if (lane_borrow_direction == PathDeciderStatus::LEFT_BORROW) {
lane_borrow_info_list.push_back(LaneBorrowInfo::LEFT_BORROW);
} else if (lane_borrow_direction == PathDeciderStatus::RIGHT_BORROW) {
lane_borrow_info_list.push_back(LaneBorrowInfo::RIGHT_BORROW);
}
}
}
主要是根据 path_decider_status.decided_side_pass_direction() 读到的可以借道的方向更新lane_borrow_info_list,更新后如下:
2、接下来便是对lane_borrow_info_list进行遍历,根据lane_borrow_info_list的数量与方向,生成对应的PathBound,生成的PathBound便是之后路径优化的边界
for (const auto& lane_borrow_info : lane_borrow_info_list) {
PathBound regular_path_bound;
std::string blocking_obstacle_id = "";
std::string borrow_lane_type = "";
......
......
......
// RecordDebugInfo(regular_path_bound, "", reference_line_info);
candidate_path_boundaries.back().set_label(
absl::StrCat("regular/", path_label, "/", borrow_lane_type));
candidate_path_boundaries.back().set_blocking_obstacle_id(
blocking_obstacle_id);
}
3、GenerateRegularPathBound()
GenerateRegularPathBound() 是生成路径边界的主要逻辑承载函数。它的主要逻辑如下:
- 1、用numeric 的最大最小值初始化path bound
- 2、用当前车道宽度计算path bound,同时考虑lane borrow,如果借道将目标车道的宽度叠加
- 3、在换道结束前,用ADC坐标到目标道路边界的距离,修正path bound
- 4、 根据path上的障碍物修正path_bound,遍历path上每个点,并在每个点上遍历障碍物;如果目标在左边将left_bound 设置为目标右边界; 如果目标在右边,将right_bound设置为目标的左边界
4、GetBoundaryFromLanesAndADC()
GetBoundaryFromLanesAndADC的作用是用当前车道宽度计算path bound,同时考虑lane borrow,如果借道将目标车道的宽度叠加。
计算方法之前做过分享:
(1)根据车道生成左右的lane_bound,从地图中获得;根据自车状态生成adc_bound,
adc_bound = adc_l_to_lane_center_ + ADC_speed_buffer + adc_half_width + ADC_buffer
上式中的各项:
adc_l_to_lane_center_ - 自车
adc_half_width - 车宽
adc_buffer - 0.5
ADCSpeedBuffer表示横向的瞬时位移, 公式如下:
其中kMaxLatAcc = 1.5
(2)根据ADC_bound 与lane_bound 生成基本的bound
左侧当前s对应的bound取MAX(left_lane_bound,left_adc_bound), 即取最左边的
右侧当前s对应的bound取MIN(right_lane_bound,right_adc_bound),即取最右边的
(3)path_bounds由一系列采样点组成,数据结构如下,这组数据里共有0~199个200个采样点,每两个点的间隔是0.5m;每个点由3个变量组成,分别是根据参考线建立的s-l坐标系的s坐标,右边界的l取值,左边界的s取值:
(4)由于车道中心线和参考线不一定是重合的,所以以参考线为基准,左右边界的宽度如下,其中offset_to_lane_center是参考线到车道中心线的距离:
(5)经过修正后,以车道中线为基准,车道的左右边界值如下:
修正代码如下:
reference_line.GetOffsetToMap(curr_s, &offset_to_lane_center);
curr_lane_left_width += offset_to_lane_center;
curr_lane_right_width -= offset_to_lane_center;
past_lane_left_width = curr_lane_left_width;
past_lane_right_width = curr_lane_right_width;
(6) 经过adcbound 修正后的path_bound:
(7) 对于pathbound上的每个点,都要检查自车是否被pathbound block,如果blocke则记录下pathblockedidx对应pathbound上点的index,并且停止后续path_bound的生成;
// 4. Update the boundary.
if (!UpdatePathBoundaryWithBuffer(i, curr_left_bound, curr_right_bound,
path_bound, is_left_lane_boundary,
is_right_lane_boundary)) {
path_blocked_idx = static_cast<int>(i);
}
if (path_blocked_idx != -1) {
break;
}
(8) 对于lane_borrow 对应的path_bound,会有相应的逻辑作处理,将旁边车道的宽度考虑进来,最后生成的path_bound 如下:
double ADC_speed_buffer = (adc_frenet_ld_ > 0 ? 1.0 : -1.0) *
adc_frenet_ld_ * adc_frenet_ld_ /
kMaxLateralAccelerations / 2.0;
double curr_left_bound_lane =
curr_lane_left_width + (lane_borrow_info == LaneBorrowInfo::LEFT_BORROW
? curr_neighbor_lane_width
: 0.0);
5、下篇文章介绍如何根据障碍物来修pathbound
6、欢迎关注我的公众号:L5Player,来进一步交流学习。