Baidu Apollo代码解析之Open Space Planner中的Hybrid A*

大家好，我已经把CSDN上的博客迁移到了知乎上，欢迎大家在知乎关注我的专栏慢慢悠悠小马车（https://zhuanlan.zhihu.com/duangduangduang）。希望大家可以多多交流，互相学习。

目录

1. 节点的定义

2. Hybrid A*的规划主流程

3. 碰撞检测

4. ReedShepp曲线加速搜索

5. 扩展相邻的节点

6. 计算节点的代价

7. 路径后处理

7.1 路径分割

7.2 轨迹平滑，计算动态信息，完善轨迹

7.3 计算动态信息，完善轨迹

8. Hybrid A*的调用

Baidu Apollo在发布的5.0版本中丰富了Open Space Planner的内容，用来解决reverse parking and sharp U-turns场景。详见Open Space Planner Readme。其提到的3篇参考文献，其中的《Path Planning for Autonomous Vehicles in Unknown Semi-Structured Environments》和《Optimization-Based Collision Avoidance》在我之前的博文中有简介。

Apollo中的规划渐渐的以一个个的场景为主体来组织，可是现实生活中场景是无数的、是变化的，不知道场景的识别、切换能否cover得住？针对特定场景的特定解决方案与调优是必需的，那么“通用基础规划方法”和“特定场景特定方案特定调优”的结合和分界又在哪呢？

好，言归正传，Open Space Planner 采用了分层规划的思路，首先Hybrid A*规划一条drivable的无碰的路径，为后面的优化作warm start，然后利用数值优化方法求解一条平滑无碰的轨迹。本文主要分析Hybrid A*算法的实现，文件路径为（5.0版本）：apollo\modules\planning\open_space\coarse_trajectory_generator\hybrid_a_star.cc。从文件命名上也能看出来，Hybrid A*输出粗糙的轨迹，使用其他有同等效果的其他规划方法也是可以的。该文件夹内的其他文件这里就略过了，仅需要注意一点：grid_search.cc中定义了2种搜索最短路径的方法，A*（对应GenerateAStarPath()）和Dynamic Programming（对应GenerateDpMap()），但是，A*方法并没有被调用。

同A*算法类似，Hybrid A*的规划是建立在栅格地图基础上的。和A*不同的是，A*在搜索周边节点时并没有考虑运动主体（机器人或车辆）的运动学约束，而Hybrid A*考虑了这种约束，限制了扩展节点时前进的方向，所以其输出轨迹一定是drivable的。对Hybrid A*的理解，大家可以去查阅其论文。

1. 节点的定义

首先在这里介绍“节点”的概念，即Node3d类。

double x_ = 0.0;    //x,y,phi是node的坐标
double y_ = 0.0;
double phi_ = 0.0;
//step_size_是一段路径（如Reed-Shepp曲线）所包含的路径点数，而该node就是这段路径的终点
size_t step_size_ = 1;
std::vector<double> traversed_x_;    //traversed_x,traversed_y,traversed_phi是node连接的一串
std::vector<double> traversed_y_;    //点的坐标集合
std::vector<double> traversed_phi_;

A*中的node单纯的指一个节点或一个状态，一般是其（x，y）坐标。这里的Node3d不同，除了包含一个点坐标（x_，y_，phi_）之外，还包含了一串这样的坐标集合。

Node3d::Node3d(const std::vector<double>& traversed_x,const std::vector<double>& traversed_y,const std::vector<double>& traversed_phi,const std::vector<double>& XYbounds,const PlannerOpenSpaceConfig& open_space_conf) {...x_ = traversed_x.back();y_ = traversed_y.back();phi_ = traversed_phi.back();...traversed_x_ = traversed_x;traversed_y_ = traversed_y;traversed_phi_ = traversed_phi;...step_size_ = traversed_x.size();
}

从上面的构造函数可以看出，（x_，y_，phi_）点就是这一串点中的最后一个。其实，这些点都是在1个grid内的。即，1个grid包含1个Node3d（下文便以node指代），1个Node3d包含了以（x_，y_，phi_）为终点、同在1个grid内的、一串路径点集的信息。 

2. Hybrid A*的规划主流程

Hybrid A*的主流程在HybridAStar::Plan()。

//Hybrid A*规划的主流程
bool HybridAStar::Plan(double sx, double sy, double sphi, double ex, double ey, double ephi,const std::vector<double>& XYbounds,const std::vector<std::vector<common::math::Vec2d>>& obstacles_vertices_vec,HybridAStartResult* result) {// clear containers//每次规划，清空之前的缓存数据open_set_.clear();close_set_.clear();open_pq_ = decltype(open_pq_)();final_node_ = nullptr;std::vector<std::vector<common::math::LineSegment2d>> obstacles_linesegments_vec;//构造障碍物轮廓线段for (const auto& obstacle_vertices : obstacles_vertices_vec) {size_t vertices_num = obstacle_vertices.size();std::vector<common::math::LineSegment2d> obstacle_linesegments;//我认为这里有错，少构造了一条线段//(obstacle_vertices[vertices_num - 1], obstacle_vertices[0])for (size_t i = 0; i < vertices_num - 1; ++i) {common::math::LineSegment2d line_segment = common::math::LineSegment2d(obstacle_vertices[i], obstacle_vertices[i + 1]);obstacle_linesegments.emplace_back(line_segment);}obstacles_linesegments_vec.emplace_back(obstacle_linesegments);}obstacles_linesegments_vec_ = std::move(obstacles_linesegments_vec);// load XYboundsXYbounds_ = XYbounds;// load nodes and obstacles//构造规划的起点和终点，并检查其有效性...if (!ValidityCheck(start_node_)) { ... }if (!ValidityCheck(end_node_)) { ... }//使用动态规划DP来计算目标点到某点的启发代价（以目标点为DP的起点），即A*中的H//生成graph的同时获得了目标点到图中任一点的cost，作为缓存，这就是DPMap的用处grid_a_star_heuristic_generator_->GenerateDpMap(ex, ey, XYbounds_,obstacles_linesegments_vec_);  // load open set, pqopen_set_.insert(std::make_pair(start_node_->GetIndex(), start_node_));open_pq_.push(std::make_pair(start_node_->GetIndex(), start_node_->GetCost()));// Hybrid A* begins...while (!open_pq_.empty()) {// take out the lowest cost neighboring nodeconst std::string current_id = open_pq_.top().first;open_pq_.pop();std::shared_ptr<Node3d> current_node = open_set_[current_id];// check if an analystic curve could be connected from current// configuration to the end configuration without collision. if so, search// ends.//true：如果生成了一条从当前点到目标点的ReedShepp曲线，就找到了最短路径//false：否则，继续Hybrid A*扩展节点if (AnalyticExpansion(current_node)) {break;}    close_set_.insert(std::make_pair(current_node->GetIndex(), current_node));for (size_t i = 0; i < next_node_num_; ++i) {//一个grid内的最后一个路径点叫node，该grid内可以有多个路径点，//该node的next_node一定在相邻的其他grid内std::shared_ptr<Node3d> next_node = Next_node_generator(current_node, i);// boundary check failure handleif (next_node == nullptr) {continue;}// check if the node is already in the close setif (close_set_.find(next_node->GetIndex()) != close_set_.end()) {continue;}// collision checkif (!ValidityCheck(next_node)) {continue;}if (open_set_.find(next_node->GetIndex()) == open_set_.end()) {        CalculateNodeCost(current_node, next_node);        open_set_.emplace(next_node->GetIndex(), next_node);open_pq_.emplace(next_node->GetIndex(), next_node->GetCost());}}}if (final_node_ == nullptr) { ... }if (!GetResult(result)) { ... }  return true;
}

3. 碰撞检测

HybridAStar::ValidityCheck() 负责碰撞检测，输入参数节点所连接的、在同一grid内的其他路径点也一起检测了。代码简单，此处略过。

4. ReedShepp曲线加速搜索

HybridAStar::AnalyticExpansion()是对搜索的加速，应该是借鉴了《Path Planning for Autonomous Vehicles in Unknown Semi-Structured Environments》中的技巧。一旦ReedShepp曲线段连接了当前节点和终点，并且通过了安全检测，本轮次的规划结束。

//尝试使用ReedShepp曲线连接当前点与目标点，若成功，则Hybrid A*规划完成
//允许返回false，其实只返回一次true
bool HybridAStar::AnalyticExpansion(std::shared_ptr<Node3d> current_node) {...//ReedShepp曲线都是从当前点到终点的if (!reed_shepp_generator_->ShortestRSP(current_node, end_node_,reeds_shepp_to_check)) { ... }//ReedShepp曲线段的碰撞检测与越界检测if (!RSPCheck(reeds_shepp_to_check)) { ... }// load the whole RSP as nodes and add to the close set//将连接到目标点的一段ReedShepp曲线封装成node，放入Hybrid A*的集合中final_node_ = LoadRSPinCS(reeds_shepp_to_check, current_node);return true;
}

5. 扩展相邻的节点

扩展节点的重点在于把车辆运动学模型的约束考虑进去，根据限定的steering angle 去搜索相邻的grid。 

//扩展节点，扩展一个node就是扩展了一个grid，但是会产生多个在同一grid内的路径点
std::shared_ptr<Node3d> HybridAStar::Next_node_generator(std::shared_ptr<Node3d> current_node, size_t next_node_index) {double steering = 0.0;size_t index = 0;double traveled_distance = 0.0;//steering angle为什么这么算？//首先，根据next_node_index与next_node_num_的对比是可以区分运动方向的//这里的if-else就是区分运动正反方向讨论的（前进和倒车）//其次，车辆在当前的姿态下，既可以向左转、又可以向右转，那么steering angle的取值范围其实是//[-max_steer_angle_, max_steer_angle_]，在正向或反向下，能取next_node_num_/2个有效值//即，把[-max_steer_angle_, max_steer_angle_]分为（next_node_num_/2-1）份//所以，steering = 初始偏移量 + 单位间隔 × index//steering angle的正负取决于车的转向，而非前进的正反if (next_node_index < static_cast<double>(next_node_num_) / 2) {steering = -max_steer_angle_ +(2 * max_steer_angle_ / (static_cast<double>(next_node_num_) / 2 - 1)) *static_cast<double>(next_node_index);traveled_distance = step_size_;} else {index = next_node_index - next_node_num_ / 2;steering = -max_steer_angle_ +(2 * max_steer_angle_ / (static_cast<double>(next_node_num_) / 2 - 1)) *static_cast<double>(index);traveled_distance = -step_size_;}// take above motion primitive to generate a curve driving the car to a different griddouble arc = std::sqrt(2) * xy_grid_resolution_;std::vector<double> intermediate_x;std::vector<double> intermediate_y;std::vector<double> intermediate_phi;double last_x = current_node->GetX();double last_y = current_node->GetY();double last_phi = current_node->GetPhi();intermediate_x.push_back(last_x);intermediate_y.push_back(last_y);intermediate_phi.push_back(last_phi);//从当前grid前进到下一个grid，一个grid内可能有多个路径点for (size_t i = 0; i < arc / step_size_; ++i) {const double next_x = last_x + traveled_distance * std::cos(last_phi);const double next_y = last_y + traveled_distance * std::sin(last_phi);//看车辆运动学模型——自行车模型const double next_phi = common::math::NormalizeAngle(last_phi +traveled_distance / vehicle_param_.wheel_base() * std::tan(steering));intermediate_x.push_back(next_x);intermediate_y.push_back(next_y);intermediate_phi.push_back(next_phi);last_x = next_x;last_y = next_y;last_phi = next_phi;}// check if the vehicle runs outside of XY boundary...std::shared_ptr<Node3d> next_node = std::shared_ptr<Node3d>(new Node3d(intermediate_x, intermediate_y, intermediate_phi, XYbounds_,planner_open_space_config_));next_node->SetPre(current_node);next_node->SetDirec(traveled_distance > 0.0);next_node->SetSteer(steering);return next_node;
}

这里补充几个关于steering angle设置的注释。

  // max_steer_angle --- vehicle max steer angle  // max_steer_angle_rate --- vehicle max steer rate; how fast can the steering wheel turn.// min_steer_angle_rate --- vehicle min steer rate;// steer_ratio --- ratio between the turn of steering wheel and the 

6. 计算节点的代价

HybridAStar::CalculateNodeCost() 分别计算路径代价和启发代价，就是A*中的G和H。这里计算启发代价的方法很巧妙，用了DP，请参看 grid_a_star_heuristic_generator_->GenerateDpMap() 处的注释。

void HybridAStar::CalculateNodeCost(std::shared_ptr<Node3d> current_node,std::shared_ptr<Node3d> next_node) {//A*中走过的轨迹的代价Gnext_node->SetTrajCost(current_node->GetTrajCost() + TrajCost(current_node, next_node));// evaluate heuristic costdouble optimal_path_cost = 0.0;//A*中从当前点到目标点的启发式代价H，采用了动态规划DP来计算（以目标点为DP的起点）optimal_path_cost += HoloObstacleHeuristic(next_node);next_node->SetHeuCost(optimal_path_cost);
}double HybridAStar::HoloObstacleHeuristic(std::shared_ptr<Node3d> next_node) {return grid_a_star_heuristic_generator_->CheckDpMap(next_node->GetX(),next_node->GetY());
}

Hybrid A*规划给出了初步的路径，仅包含（x，y，phi）等静态信息，接下来进行一些求动态信息、分割、平滑、拼接等后续处理，就是最终的可供车辆行驶的轨迹了。

7. 路径后处理

初步路径的每个node都指向上一个node，HybridAStar::GetResult() 在把这些node反向后（由当前node指向终点node），得到了顺序正确的node集合。注意，此时形成了一个挨一个的node，还不是一个挨一个的轨迹点。因此，要调用 GetTemporalProfile(result) 来完成大部分的后处理，得到最终结果。

//将Hybrid A*计算的轨迹结果，按照行驶的正反方向切换，分割为数段，分别逆向翻转轨迹点
//然后重新拼接在一起，就是最终可以发布供车行驶的轨迹
bool HybridAStar::GetTemporalProfile(HybridAStartResult* result) {std::vector<HybridAStartResult> partitioned_results;if (!TrajectoryPartition(*result, &partitioned_results)) { ... }//将分段的轨迹拼接起来HybridAStartResult stitched_result;...*result = stitched_result;return true;
}

7.1 路径分割

TrajectoryPartition()将Hybrid A*计算的轨迹结果，按照行驶的正反方向切换，分割为数段，并完善轨迹的静态、动态信息。

//将Hybrid A*计算的轨迹结果，按照行驶的正反方向切换，分割为数段，并完善轨迹的静态、动态信息
bool HybridAStar::TrajectoryPartition(const HybridAStartResult& result,std::vector<HybridAStartResult>* partitioned_result) {const auto& x = result.x;const auto& y = result.y;const auto& phi = result.phi;...size_t horizon = x.size();partitioned_result->clear();partitioned_result->emplace_back();auto* current_traj = &(partitioned_result->back());double heading_angle = phi.front();const Vec2d init_tracking_vector(x[1] - x[0], y[1] - y[0]);double tracking_angle = init_tracking_vector.Angle();bool current_gear =std::abs(common::math::NormalizeAngle(tracking_angle - heading_angle)) < (M_PI_2);//此时的result只有路径静态信息，x,y,phi//将Hybrid A*计算的轨迹结果，按照行驶的正反方向切换，分割为数段for (size_t i = 0; i < horizon - 1; ++i) {heading_angle = phi[i];const Vec2d tracking_vector(x[i + 1] - x[i], y[i + 1] - y[i]);tracking_angle = tracking_vector.Angle();bool gear =std::abs(common::math::NormalizeAngle(tracking_angle - heading_angle)) <(M_PI_2);if (gear != current_gear) {current_traj->x.push_back(x[i]);current_traj->y.push_back(y[i]);current_traj->phi.push_back(phi[i]);partitioned_result->emplace_back();current_traj = &(partitioned_result->back());current_gear = gear;}current_traj->x.push_back(x[i]);current_traj->y.push_back(y[i]);current_traj->phi.push_back(phi[i]);}current_traj->x.push_back(x.back());current_traj->y.push_back(y.back());current_traj->phi.push_back(phi.back());// Retrieve v, a and steer from pathfor (auto& result : *partitioned_result) {//2种不同的方式获取轨迹动态信息，v,a,steer。区别: 前者用数值优化的方法，后者用相邻点静态信息if (FLAGS_use_s_curve_speed_smooth) {//使用QP优化方法求frenet系下的轨迹，但是结果只有动态信息 s,v,a,steerif (!GenerateSCurveSpeedAcceleration(&result)) { ... }//原来的result中包含x,y,phi等路径静态信息，QP优化后添加了动态信息 s,v,a,steer//为什么下面还要再次将已知的动静态信息combine呢？//我认为：QP优化是根据静态信息求动态，求得动态信息后，再积分反推出静态信息，减小误差      SpeedData speed_data;std::vector<common::PathPoint> path_points;const size_t size_x = result.x.size();for (size_t i = 0; i < size_x; ++i) {common::PathPoint path_point = common::util::MakePathPoint(result.x[i], result.y[i], 0.0, result.phi[i], 0.0, 0.0, 0.0);path_point.set_s(result.accumulated_s[i]);path_points.emplace_back(std::move(path_point));speed_data.AppendSpeedPoint( ... );}      DiscretizedPath discretized_path(path_points);//合并静态路径和速度等动态信息if (!CombinePathAndSpeedProfile(discretized_path, speed_data, &result)) { ... }      } else {//根据result中的静态信息x,y,phi，利用相邻点、逐点求动态信息v,a,steerif (!GenerateSpeedAcceleration(&result)) { ... }}}return true;
}

7.2 轨迹平滑，计算动态信息，完善轨迹

求动态信息的第一种方式：QP优化，与参考线平滑是一个道理。

//使用QP优化方法求frenet系下的轨迹，但是结果只有动态信息 s,v,a,steer
bool HybridAStar::GenerateSCurveSpeedAcceleration(HybridAStartResult* result) {...const size_t x_size = result->x.size();double accumulated_s = 0.0;std::vector<std::pair<double, double>> x_bounds;std::vector<std::pair<double, double>> dx_bounds;// Setup for initial point.result->accumulated_s.push_back(0.0);result->v.push_back(0.0);x_bounds.emplace_back(-10.0, 10.0);dx_bounds.emplace_back(0.0, 0.0);for (size_t i = 0; i + 1 < x_size; ++i) {//求s轴速度，不同方向的速度怎么可以直接相加求合速度呢？const double discrete_v = ((result->x[i + 1] - result->x[i]) / delta_t_) *std::cos(result->phi[i]) +((result->y[i + 1] - result->y[i]) / delta_t_) *std::sin(result->phi[i]);accumulated_s += discrete_v * delta_t_;result->v.push_back(discrete_v);result->accumulated_s.push_back(accumulated_s);//设置取值范围，在优化时添加各点处取值范围不等式约束x_bounds.emplace_back(accumulated_s - 10, accumulated_s + 10);dx_bounds.emplace_back(discrete_v - 10, discrete_v + 10);}// Force last point velocity to be zero//我觉得最后的点速度和加速度设置为0，是为了安全。如果后续不再有轨迹计算和输出，车自然停止//因为规划是高频快速进行的，后面输出的轨迹会覆盖前面还没有实际走完的轨迹，车不会停止，正常行驶result->v[x_size - 1] = 0.0;dx_bounds[x_size - 1] = {0.0, 0.0};std::array<double, 3> init_s = {result->accumulated_s[0], result->v[0],(result->v[1] - result->v[0]) / delta_t_};// Start a PathTimeQpProblemstd::array<double, 3> end_s = {result->accumulated_s[x_size - 1], 0.0, 0.0};//使用QP优化方法求frenet系下的轨迹PiecewiseJerkSpeedProblem path_time_qp(x_size, delta_t_, init_s);//设置各种权重和约束条件...// Solve the problemif (!path_time_qp.Optimize()) { ... }// Extract output...// load steering from phifor (size_t i = 0; i + 1 < x_size; ++i) {double discrete_steer = (result->phi[i + 1] - result->phi[i]) *vehicle_param_.wheel_base() / step_size_;if (result->v[i] > 0.0) {discrete_steer = std::atan(discrete_steer);} else {discrete_steer = std::atan(-discrete_steer);}result->steer.push_back(discrete_steer);}return true;
}

7.3 计算动态信息，完善轨迹

求动态信息的第二种方式：根据前后相邻点的静态信息朴素地计算速度、加速度等动态信息。 

//根据result中的静态信息x,y,phi，利用相邻点、逐点求动态信息v,a,steer
bool HybridAStar::GenerateSpeedAcceleration(HybridAStartResult* result) {...const size_t x_size = result->x.size();// load velocity from position// initial and end speed are set to be zerosresult->v.push_back(0.0);for (size_t i = 1; i + 1 < x_size; ++i) {//求s轴速度，不同方向的速度怎么可以直接相加求合速度呢？double discrete_v = (((result->x[i + 1] - result->x[i]) / delta_t_) *std::cos(result->phi[i]) +((result->x[i] - result->x[i - 1]) / delta_t_) *std::cos(result->phi[i])) / 2.0 +//上面是x方向上，利用连续3点的坐标求中间点的速度，平均速度(((result->y[i + 1] - result->y[i]) / delta_t_) *std::sin(result->phi[i]) +((result->y[i] - result->y[i - 1]) / delta_t_) *std::sin(result->phi[i])) / 2.0;//上面是y方向上，利用连续3点的坐标求中间点的速度，平均速度result->v.push_back(discrete_v);}result->v.push_back(0.0);// load acceleration from velocityfor (size_t i = 0; i + 1 < x_size; ++i) {const double discrete_a = (result->v[i + 1] - result->v[i]) / delta_t_;result->a.push_back(discrete_a);}// load steering from phifor (size_t i = 0; i + 1 < x_size; ++i) {double discrete_steer = (result->phi[i + 1] - result->phi[i]) *vehicle_param_.wheel_base() / step_size_;if (result->v[i] > 0.0) {discrete_steer = std::atan(discrete_steer);} else {discrete_steer = std::atan(-discrete_steer);}result->steer.push_back(discrete_steer);}return true;
}

 至此，Hybrid A*轨迹规划结束，接下来由数值优化方法来做平滑。

8. Hybrid A*的调用

Planning模块关于规划场景的设置、规划方法的调用比较复杂，建议在看这一小节之前，先看一下这一篇 Baidu Apollo代码解析之Planning的结构与调用流程 ，搞懂Scenario、Stage、Task之间的关系。同时要注意，Apollo中的PublicRoadPlanner其实是涵盖了OpenSpacePlanner的，OpenSpacePlanner用来处理public road所包含的u-turn和valet parking 2种场景。

在apollo/modules/planning/conf/planning_config.pb.txt 配置文件中，可以看到PublicRoadPlanner 支持 VALET_PARKING 场景。而该场景分为2个stage：VALET_PARKING_APPROACHING_PARKING_SPOT 和 VALET_PARKING_PARKING。其中，VALET_PARKING_PARKING stage 包含 OPEN_SPACE_TRAJECTORY_PROVIDER 这个task，该task就是OpenSpacePlanner的工作。

在 apollo/modules/planning/tasks/optimizers/open_space_trajectory_generation/open_space_trajectory_provider.h 和 open_space_trajectory_optimizer.h 文件中分别定义了OpenSpaceTrajectoryProvider 和 OpenSpaceTrajectoryOptimizer类。在OpenSpaceTrajectoryProvider::GenerateTrajectoryThread()中会调用open_space_trajectory_optimizer_->Plan()。OpenSpaceTrajectoryOptimizer类有一个std::unique_ptr<HybridAStar> warm_start_ 成员变量。在 OpenSpaceTrajectoryOptimizer::Plan()中，会调用 warm_start_->Plan()，从而进入第2小节所述的Hybrid A*的处理流程。