Apollo Open Space Planner 介绍 1-Hybrid A star

Apollo Open Space Planner 介绍 1-Hybrid A star

[TOC]

本文是对 Apollo Open Space Planner 的学习记录之一. 主要内容是 Hybrid A* 生成粗轨迹模块的分析.

本文记录一下 Apollo open space planner 的研读与心得等.

Planner 概况

目前 Apollo 是 scenario-based planning, 对于倒车入库以及 sharp U-turns 场景之前的 planner 不适用, 因此开发了这个 planner. 流程如下图:

os_planner.png

整个 Open Space Planner 挺复杂的, 这篇博文先分析第一步里的 Hybrid A* 生成粗轨迹的模块. 这个模块位于 planing/open_space/coarse_trajectory_generator 目录下.
Apollo 使用了 Hybrid A* 结合车辆动力学搜索出 Path 后进行速度规划, 复杂一点的速度规划利用 QP 求解出 piecewise quintic polynominal, 从而规划出一条粗略的轨迹.

Hybrid A* 搜索后输出为一个结构体(也就是 TDR-OBCA 算法里的T–>temporal profile):

1
2
3
4
5
6
7
8
9
struct HybridAStartResult {
std::vector<double> x; //MPC x_k位置姿态
std::vector<double> y; //MPC x_k位置姿态
std::vector<double> phi; //MPC x_k位置姿态
std::vector<double> v; //MPC x_k位置姿态
std::vector<double> a; //MPC u_k控制输入
std::vector<double> steer;//MPC u_k控制输入
std::vector<double> accumulated_s;
};

类的解读

模块目录下有很多类, 先进行解读.

这里里面既有 A* 也有 Hybrid A*.

Node2d

私有成员变量

1
2
3
4
5
6
7
8
9
double x_ = 0.0;
double y_ = 0.0;
int grid_x_ = 0;
int grid_y_ = 0;
double path_cost_ = 0.0;
double heuristic_ = 0.0;
double cost_ = 0.0;
std::string index_;
std::shared_ptr<Node2d> pre_node_ = nullptr;

是 Node3d 的简化版, 具体参照 Node3d.

Node3d

私有成员变量

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
double x_ = 0.0;
double y_ = 0.0;
double phi_ = 0.0;
size_t step_size_ = 1;
std::vector<double> traversed_x_; //轨迹点x
std::vector<double> traversed_y_; //轨迹点y
std::vector<double> traversed_phi_; //轨迹点phi
int x_grid_ = 0; //grid_index_x
int y_grid_ = 0; //grid_index_y
int phi_grid_ = 0; //grid_index_phi
std::string index_; //grid_name for debugging, 也用在 GridSearch 里的 unordered_map 作 key
double traj_cost_ = 0.0; //移动cost
double heuristic_cost_ = 0.0; //启发cost
double cost_ = 0.0; //总cost
std::shared_ptr<Node3d> pre_node_ = nullptr;//父Node
double steering_ = 0.0 //打舵角
// true for moving forward and false for moving backward
bool direction_ = true; //前进OR倒车

3d 指的是 x, y, phi, 并且包含历史轨迹信息(此设计的意图可能是一个 grid 里面可以包含多个路径点, 但是目前的实现仍旧只包括 2 个点, 第二个为下个 grid 里面的点).

同时比 Node2d 多了前进后退的 flag, 以及 steer 打舵角度, 因为考虑车辆动力学/后向搜索.

成员函数

  1. 成员变量的 get/set 函数, 略.
  2. 构造函数
  • Node3d::Node3d(double x, double y, double phi)
  • Node3d::Node3d(double x, double y, double phi, const std::vector<double>& XYbounds, const double xy_grid_resolution)
    利用 XYbounds/分辨率算出 x_grid_/y_grid_/phi_grid 格子 index.
  • 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 double xy_grid_resolution)
    最全的构造函数
  • 用 x/y/phi 构造出自车 Box2d:GetBoundingBox

GridSearch

私有成员变量

1
2
3
4
5
6
7
8
9
10
double xy_grid_resolution_ = 0.0;	//分辨率
double node_radius_ = 0.0; //碰撞检测时障碍物与自车的临界距离,从PlannerOpenSpaceConfig获取
std::vector<double> XYbounds_; //图边界
double max_grid_x_ = 0.0;
double max_grid_y_ = 0.0;
std::shared_ptr<Node2d> start_node_;
std::shared_ptr<Node2d> end_node_;
std::shared_ptr<Node2d> final_node_;
std::vector<std::vector<common::math::LineSegment2d>> obstacles_linesegments_vec_; //障碍物轮廓线段
std::unordered_map<std::string, std::shared_ptr<Node2d>> dp_map_; //从指定终点到一帧图里所有可达 grid 的 H cost散列表

这个类里主要 2 个函数功能:

  1. A* 搜索(GenerateAStarPath() 函数)
    基于 Node2d, 古典算法实现.
    open_set, close_set 使用散列表实现: std::unordered_map<std::string, std::shared_ptr<Node2d>>.
    队列 open_pq 使用优先队列实现: std::priority_queue<std::pair<std::string, double>, std::vector<std::pair<std::string, double>>, cmp>.
    由于主体使用了 Hybrid A*, 此函数弃用.
  2. 启发函数 cost 的 DP MapGenerateDpMap() 函数
    使用 DP 的方式把从指定点出发到所有可达点的 cost 存储到散列表里,std::unordered_map<std::string, std::shared_ptr<Node2d>> dp_map_.
    Hybrid A* 里使用此函数反向搜索终点到一帧图里所有可达 grid 的 H cost, 通过查询减少重复搜索计算, 也就是状态记录(memomrization).

成员函数

  1. GenerateNextNodes()
    生成周边点, 8 个点, 即上下左右(移动代价 1), 斜上下左右(移动代价 $\sqrt{2}$).
  2. CheckConstraints()
    通过点到 LineSegment2d 的距离是否小于 node_radius 判断是否碰撞.
  3. CheckDpMap()
    输出 dp map 中某 grid 的 H cost, 找不到的情况下返回std::numeric_limits<double>::infinity().
  4. LoadGridAStarResult()
    将 A* 搜索结果放入 GridAStartResult 中, 注意此处要回溯逆序.

HybridAStar

私有成员变量:

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
PlannerOpenSpaceConfig planner_open_space_config_;            //配置标定
common::VehicleParam vehicle_param_; //车辆参数
size_t next_node_num_ = 0; //拓展 Node 时的打舵采样点
double max_steer_angle_ = 0.0; //最大车轮转角
double step_size_ = 0.0; //车辆步进长度
double xy_grid_resolution_ = 0.0; //图分辨率
double delta_t_ = 0.0; //速度规划的自变量
double traj_forward_penalty_ = 0.0; //以下为计算 cost 的各项成员
double traj_back_penalty_ = 0.0;
double traj_gear_switch_penalty_ = 0.0;
double traj_steer_penalty_ = 0.0;
double traj_steer_change_penalty_ = 0.0;
double heu_rs_forward_penalty_ = 0.0;
double heu_rs_back_penalty_ = 0.0;
double heu_rs_gear_switch_penalty_ = 0.0;
double heu_rs_steer_penalty_ = 0.0;
double heu_rs_steer_change_penalty_ = 0.0;

std::vector<double> XYbounds_; //XYbounds[0 以及 1]为 x 方向,XYbounds[2 以及 3]为 y 方向
std::shared_ptr<Node3d> start_node_;
std::shared_ptr<Node3d> end_node_;
std::shared_ptr<Node3d> final_node_;
std::vector<std::vector<common::math::LineSegment2d>> obstacles_linesegments_vec_;

std::priority_queue<std::pair<std::string, double>,
std::vector<std::pair<std::string, double>>, cmp> open_pq_;
std::unordered_map<std::string, std::shared_ptr<Node3d>> open_set_;
std::unordered_map<std::string, std::shared_ptr<Node3d>> close_set_;
std::unique_ptr<ReedShepp> reed_shepp_generator_; //使用 RS 曲线判断加速
std::unique_ptr<GridSearch> grid_a_star_heuristic_generator_; //用于计算启发 cost

成员函数

  • 构造函数
    仅初始化列表.

其余函数的部分后续展开说.

ReedSheppPath

此类为给定始终点, 生成最短 RS 曲线的路径的类. 就不用接着搜索了, 直接输出 RS 曲线.

后面有机会详细展开.

规划步骤

接口在 HybridAStar::Plan() 函数.

通过 Hybrid A* 搜索规划 Path

  1. 数据准备:
  • clear containers, 初始化 obstacles_linesegments_vec_.

  • node reset(使用智能指针的 reset 函数), 检测 node 是否碰撞.

  • 生成 GenerateDpMap, 放入 open_set_, open_pq_.

  1. 开始循环, pop 出优先级最高的 node, 先判断是否可以直接用无碰撞 RS 曲线(AnalyticExpansion()), 是的话结束返回 RS 曲线.

  2. 将 current_node 放入 close_set_.

  3. 拓展可达 grid Next_node_generator, 这部分是与古典 A* 算法不同的地方, 下面对源码进行分析.

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
std::shared_ptr<Node3d> HybridAStar::Next_node_generator(
std::shared_ptr<Node3d> current_node, size_t next_node_index) {
double steering = 0.0;
double traveled_distance = 0.0;
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 {
size_t 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_;
}

这部分是在 steering 角度上进行了采样. 首先车辆由于可以前进也可以后退, 所以行进分为 2 半,前半部分前进(traveled_distance = step_size_;), 后半部分倒车(traveled_distance = -step_size_;). 在前进/倒车再在方向盘转角上采样 (next_node_num_) / 2 = 5 个得到能够到达的格子. 格子的分辨率为 0.3m. 如下图(图未按实际尺寸, 只是示意图).

hybrid_A_star_get_new_node.JPG

前进方向的打舵采样角点:

$$
-\theta_{max} + \frac{2\theta_{max}}{\frac{n}{2}-1} t
$$

倒车方向的打舵采样角点:

$$
-\theta_{max} + \frac{2\theta_{max}}{\frac{n}{2}-1} (t-\frac{n}{2})
$$

还需要注意的是, 每个格子里可能不止只有一个点, 主要看 step_size_ 的选择. Apollo 里默认 step_size_=0.5m, 格子内最大行驶弧长用格子的对角线近似, 因此实际上每个 Node 里只有 2 个点, 一个是在当前 grid 里, 另一个在下个 grid 里. 然后是把行进点压入行进 vector 中. 推点列需要用到车辆动力学模型. Apollo 里使用的是以后轮轴为中心的自行车模型.

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
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);
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;
}
vehicle_modle.jpg

$$
\left[
\begin{matrix}
x’ \\
y’ \\
\theta’
\end{matrix}
\right] =
s \cdot
\left[
\begin{matrix}
\cos(\theta) \\
\sin(\theta) \\
\frac{\tan \phi}{L}
\end{matrix}
\right]
$$

最后把求得的 Node 信息构造成一个 Node, 并设其父节点. 这样便得到一系列(10 个)新的拓展点. 如果 Node 已经在 close_set_ 里跳过, 通过 ValidityCheck 过滤碰撞不通过的, 最后如果在 open_set_ 中也没有存进去过的话, 计算其 cost 放入优先队列.

1
2
3
4
5
6
7
8
9
10
11
12
13
if (intermediate_x.back() > XYbounds_[1] ||
intermediate_x.back() < XYbounds_[0] ||
intermediate_y.back() > XYbounds_[3] ||
intermediate_y.back() < XYbounds_[2]) {
return nullptr;
}
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;

这里有个有意思的问题, 知乎上有个网友留言说按照一般的 A* 搜索, 是需要把计算好 cost 的节点从 open_set 拿出来放到 close_set 里的, 这里为什么没有放?

我的理解是如那位网友所说, 单说 grid 元素的话, open_set = close_set + open_pq_, open_set 与 open_pq_ 有明显的交集, 这么设置有什么意义?

我推测意义在于用空间换时间的效率. open_pq_ 为优先队列(实现为堆)查找效率较低(除了堆顶元素的查找以外), 但适合管理优先级. open_set 为散列表查找效率很高, 保留 open_set 可以提升查找效率. 并且这么设置也不会产生重复查找之类的问题. 因为已经算好的 Node 已经在 close_set 里了, 不会再进行下一步的 open_set 里的查找. 因此这个设计没有什么逻辑问题.

  1. 计算 cost
  • 移动 cost

    分为前进/后退 cost,换挡 cost,方向盘打舵 cost,打舵角变化 cost,公式如下:

    $$
    Fcost_{next} = Fcost_{cur} + F_{lag1}w_{forward}\cdot A_{step} \cdot (S_{tepsize}-1) + F_{lag2}w_{backward}\cdot A_{step} \cdot (S_{tepsize}-1) + \\
    F_{lag3}w_{swtich} + w_{phi}|\phi_{next}| + w_{ste}|\phi_{next} - \phi_{cur}|
    $$

  • 启发 cost, HoloObstacleHeuristic, 通过 DpMapCheckDpMap() 函数查询得到.

  1. 搜索完成后回溯得到轨迹结果

    GetResult() 函数, 回溯过程注意将相邻节点的重叠的一个轨迹点 pop 出去(这样的话每个 grid 里面只有一个路径点了). 具体先 reverse, 然后 pop_back, 接着 insert 到主 vector 中, 最后 insert 起点 Node, 再反转 reverse 回来即可得到正序无重叠的 vector.

    速度规划

    GetTemporalProfile() 函数首先对轨迹按照挡位的变化为条件对轨迹进行切割, 即 TrajectoryPartition() 函数, 通过 tracking_angleheading_angle 也就是行驶方向角度与初始 Node 的自车航向角的关系来判断是否换挡.

    换挡的节点处的处理方式值得借鉴, vector 容器先增加空元素再具体添加元素内容. 如下:

1
2
partitioned_result->emplace_back();
auto* current_traj = &(partitioned_result->back());

速度规划有 2 种, 根据 FLAGS_use_s_curve_speed_smooth 标志位进行选择.

GenerateSCurveSpeedAcceleration()

通过 QP 求解五次多项式的形式规划出速度轨迹.

这里面用到了 SpeedData 的数据结构. 稍微讲一下, 用法值得学习一下, 下面是其头文件.

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
//直接继承 STL 的 vector 类(Apollo 里有挺多直接继承 STL 的数据结构类).SpeedPoint 为 Protolbuf 数据类型. 同时使用 std::sort() 按照时间 t 排序.
class SpeedData : public std::vector<common::SpeedPoint> {
public:
SpeedData() = default;
virtual ~SpeedData() = default;
//必须显式构造, 不然编译器推断风险很大.
explicit SpeedData(std::vector<common::SpeedPoint> speed_points);
//追加点使用了 std::mutex,UNIQUE_LOCK_MULTITHREAD 线程锁.
void AppendSpeedPoint(const double s, const double time, const double v,
const double a, const double da);
//定位到 time 的位置后进行线性插值, 这里的 set_XXX 是 protolbuf 的默认访问方式, 没有直接定义也可以用.
bool EvaluateByTime(const double time,
common::SpeedPoint* const speed_point) const;
//这里假设 s 是单调递增的, 因此对 path 进行了切割, 倒车的部分 s 也是从 0 开始单调递增的.
// Assuming spatial traversed distance is monotonous, which is the case for
// current usage on city driving scenario
bool EvaluateByS(const double s, common::SpeedPoint* const speed_point) const;
double TotalTime() const;
// Assuming spatial traversed distance is monotonous
double TotalLength() const;
virtual std::string DebugString() const;
};

速度规划的输入是按照换挡为节点分段的 Path 曲线, 也就是静止-加速-匀速-刹停的过程.

  1. 求解 accumulated_s
    这个容许我得吐槽一下, 为什么直接用欧式距离分别求各点到段初始点的 accumulated_s, 为啥不在上面推点列用自车动力学模型的时候计算每段的单独长度. 然后再累加起来得到 accumulated_s.

  2. 初始/边界确定

1
2
3
4
5
6
7
8
9
10
// assume static initial state初始静止
const double init_v = 0.0;
const double init_a = 0.0;
// v,a的边界
const double max_forward_v = 2.0;
const double max_reverse_v = 1.0;
const double max_forward_acc = 2.0;
const double max_reverse_acc = 1.0;
const double max_acc_jerk = 0.5;
const double delta_t = 0.2;

每段时间跨度

$$
t_{m} = \max( 1.5 \frac{v_m^2 + s_m a_m}{a_m v_m} , 10)
$$

这里计算应该是按照下面的方式推算的. 考虑到每段都是静止-加速-匀速-刹停的过程, 满足下面的方程:

$$
\frac{1}{2} a_m t_1^2 + v_m t_2 + v_m t_3 + \frac{1}{2} a_m t_3^2 = s_m \\
t_1 = t_3 = \frac{v_m}{a_m}
$$

求解得:

$$
t = t_1 + t_2 + t_3 = \frac{a_m s_m + v_m^2}{a_m v_m}
$$

因为全是按照最大速度/加速度推算的时间最小, 以及 $s_m$ 是欧式距离肯定比实际值小不少, 因此乘以一个系数 1.5.
然后强行把时间上限设置为 10s, 如果考虑平行库位 3 段式泊车的话, 最多仅仅需要 30s 就能完成. 速度算是非常快了, 不知道后面的碰撞检测能不能 hold 的住.

  1. 使用 OSQP 求解. 这部分我暂时没有仔细看,估计与官方的qp_spline_st_speed_optimizer说明是类似的.
    使用专门的 PiecewiseJerkSpeedProblem 求解类求解.(OSQP 求解piecewise 5 次多项式参考博文)
    速度规划的简单示意图如下, 上面的为 path 图, 下面的为 TS 图:
parking_route.JPG TS_grah.JPG
  1. 把求解所得的 SpeedData 与 Path 合并成为最终的轨迹.
    整个过程是数据的对齐与线性插值, 然后放到 HybridAStartResult 类型数据里面. 看的实在懵了, 遂决定梳理一下这个数据结构闪转腾挪的过程, 如下图.
data_flow.JPG

图中的 v(s) 代表 std::vector<double> s.

这里将 SpeedDataDiscretizedPath 合并的原则是考虑后面的 dual warm start non-linear optimization(MPC) 的 Horizon 是等距的时间列. 因此先对 SpeedData 进行时间采样然后基于 s 对 DiscretizedPath 进行线性插值.

求解 steer 角的函数, 还是基于后轴中心的自行车模型.

$$
\text{Steer} = \frac{(\phi_{i+1} - \phi_i) l_{wb}}{s_{i+1} -s_i}
$$

我感觉整个 open space planner 还在深度开发中的一个佐证就是, 没有把基于 s 的线性插值方法移植到它的数据结构里, 结果还需要使用其他数据结构里的函数 DiscretizedPathEvaluate(s) 函数. 所以显得非常凌乱. 还有已经有 STPoint 这个数据结构专门描述 TS 图里的点, 这里又设置了一个新的 SpeedData 来存储同性质的信息.

我有一个地方没有搞明白, 为什么只把最后一个 a 给 pop 出去, 其他 s, v 不变呢?

1
combined_result.a.pop_back();

GenerateSpeedAcceleration()

上面的速度规划有点复杂, 那简化一下假设车辆形式轨迹不是符合动力学的圆弧而是直线呢. 这个方法即是基于此思路.

私以为可以在 QP 求解器算不出来的时候使用这个方法作为 backup.

上图中的 partitioned_result 中每个元素 result 中只有 v(x),v(y),v(phi) 信息, 基于这三个信息直接算出 v(v),v(a),v(jerk),v(steer) 即可.
(因为每段的 $\Delta s$ 都为 step_size_, v(s) 就很明显了). 记得在 v(v) 的最后插一个 0, 保证最终是停车.

$$
v_i = \frac{ \frac{x_{i+1} - x_i}{\Delta t} \cos \phi_i + \frac{x_{i} - x_{i-1}}{\Delta t} \cos \phi_i }{2} +
\frac{ \frac{y_{i+1} - y_i}{\Delta t} \sin \phi_i + \frac{y_{i} - y_{i-1}}{\Delta t} \sin \phi_i }{2} \\
a_i = \frac{v_{i+1} - v_i}{\Delta t} \\
jerk_i = \frac{a_{i+1} - a_i}{\Delta t} \\
Steer =arctan \Big( \frac{(\phi_{i+1} - \phi_i) l_{wb}}{s_{step}} \Big)
$$
原理如下图:

GenerateSpeedAcceleration.png

最后把所有结果 copy 到最终结果中. 返回 HybridAStartResult 数据.

1
std::copy(result.x.begin(), result.x.end() - 1, std::back_inserter(stitched_result.x));

观码感触

  1. 使用 decltype clear 一种容器. 例如 dp_map_ = decltype(dp_map_)();.

  2. 继承 STL 的构造函数

1
DiscretizedPath::DiscretizedPath(std::vector<PathPoint> path_points): std::vector<PathPoint>(std::move(path_points)) {}

后续

下面的 MPC 步骤直接啃源码, ハードルが少し高い(有点难), 所以先啃论文搞明白思路与公式再往下分析代码. 因此这篇先到这里.

ps. 本文思路后来看 Apollo 的论文才发现上面的思路都在论文里….推了大半天….

Apollo论文: TDR-OBCA: A Reliable Planner for Autonomous Driving in Free-Space Environment

参考链接

https://zhuanlan.zhihu.com/p/95747418

作者

cx

发布于

2021-10-17

更新于

2023-02-15

许可协议