大家好，我已经把CSDN上的博客迁移到了知乎上，欢迎大家在知乎关注我的专栏慢慢悠悠小马车（https://zhuanlan.zhihu.com/c_1132958996826546176）。以后，我会把日常的思考放在CSDN上，梳理过的精华文章放在知乎上，希望大家可以多多交流，互相学习。
目录
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 XYbounds
XYbounds_ = 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, pq
open_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 node
const 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 handle
if (next_node == nullptr) {
continue;
}
// check if the node is already in the close set
if (close_set_.find(next_node->GetIndex()) != close_set_.end()) {
continue;
}
// collision check
if (!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 grid
double 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*中走过的轨迹的代价G
next_node->SetTrajCost(current_node->GetTrajCost() + TrajCost(current_node, next_node));
// evaluate heuristic cost
double 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 path
for (auto& result : *partitioned_result) {
//2种不同的方式获取轨迹动态信息，v,a,steer。区别: 前者用数值优化的方法，后者用相邻点静态信息
if (FLAGS_use_s_curve_speed_smooth) {
//使用QP优化方法求frenet系下的轨迹，但是结果只有动态信息 s,v,a,steer
if (!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,steer
if (!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 PathTimeQpProblem
std::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 problem
if (!path_time_qp.Optimize()) { ... }
// Extract output
...
// load steering from phi
for (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 zeros
result->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 velocity
for (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 phi
for (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*轨迹规划结束，接下来由数值优化方法来做平滑。
来源：CSDN
作者：linxigjs
链接：https://blog.csdn.net/linxigjs/article/details/103419697