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. 流程如下图:
整个 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 | struct HybridAStartResult { |
类的解读
模块目录下有很多类, 先进行解读.
这里里面既有 A* 也有 Hybrid A*.
Node2d
私有成员变量
1 | double x_ = 0.0; |
是 Node3d 的简化版, 具体参照 Node3d.
Node3d
私有成员变量
1 | double x_ = 0.0; |
3d 指的是 x, y, phi, 并且包含历史轨迹信息(此设计的意图可能是一个 grid 里面可以包含多个路径点, 但是目前的实现仍旧只包括 2 个点, 第二个为下个 grid 里面的点).
同时比 Node2d 多了前进后退的 flag, 以及 steer 打舵角度, 因为考虑车辆动力学/后向搜索.
成员函数
- 成员变量的 get/set 函数, 略.
- 构造函数
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 | double xy_grid_resolution_ = 0.0; //分辨率 |
这个类里主要 2 个函数功能:
- 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*, 此函数弃用. - 启发函数 cost 的 DP
MapGenerateDpMap()
函数
使用 DP 的方式把从指定点出发到所有可达点的 cost 存储到散列表里,std::unordered_map<std::string, std::shared_ptr<Node2d>> dp_map_
.
Hybrid A* 里使用此函数反向搜索终点到一帧图里所有可达 grid 的 H cost, 通过查询减少重复搜索计算, 也就是状态记录(memomrization).
成员函数
GenerateNextNodes()
生成周边点, 8 个点, 即上下左右(移动代价 1), 斜上下左右(移动代价 $\sqrt{2}$).CheckConstraints()
通过点到LineSegment2d
的距离是否小于node_radius
判断是否碰撞.CheckDpMap()
输出 dp map 中某 grid 的 H cost, 找不到的情况下返回std::numeric_limits<double>::infinity()
.LoadGridAStarResult()
将 A* 搜索结果放入GridAStartResult
中, 注意此处要回溯逆序.
HybridAStar
私有成员变量:
1 | PlannerOpenSpaceConfig planner_open_space_config_; //配置标定 |
成员函数
- 构造函数
仅初始化列表.
其余函数的部分后续展开说.
ReedSheppPath
此类为给定始终点, 生成最短 RS 曲线的路径的类. 就不用接着搜索了, 直接输出 RS 曲线.
后面有机会详细展开.
规划步骤
接口在 HybridAStar::Plan()
函数.
通过 Hybrid A* 搜索规划 Path
- 数据准备:
clear containers, 初始化
obstacles_linesegments_vec_
.node reset(使用智能指针的
reset
函数), 检测 node 是否碰撞.生成
GenerateDpMap
, 放入open_set_
,open_pq_
.
开始循环, pop 出优先级最高的 node, 先判断是否可以直接用无碰撞 RS 曲线(
AnalyticExpansion()
), 是的话结束返回 RS 曲线.将 current_node 放入
close_set_
.拓展可达 grid
Next_node_generator
, 这部分是与古典 A* 算法不同的地方, 下面对源码进行分析.
1 | std::shared_ptr<Node3d> HybridAStar::Next_node_generator( |
这部分是在 steering 角度上进行了采样. 首先车辆由于可以前进也可以后退, 所以行进分为 2 半,前半部分前进(traveled_distance = step_size_;
), 后半部分倒车(traveled_distance = -step_size_;
). 在前进/倒车再在方向盘转角上采样 (next_node_num_) / 2 = 5
个得到能够到达的格子. 格子的分辨率为 0.3m. 如下图(图未按实际尺寸, 只是示意图).
前进方向的打舵采样角点:
$$
-\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 | double arc = std::sqrt(2) * xy_grid_resolution_; |
$$
\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 | if (intermediate_x.back() > XYbounds_[1] || |
这里有个有意思的问题, 知乎上有个网友留言说按照一般的 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 里的查找. 因此这个设计没有什么逻辑问题.
- 计算 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
, 通过 DpMap 的CheckDpMap()
函数查询得到.
搜索完成后回溯得到轨迹结果
GetResult()
函数, 回溯过程注意将相邻节点的重叠的一个轨迹点 pop 出去(这样的话每个 grid 里面只有一个路径点了). 具体先 reverse, 然后 pop_back, 接着 insert 到主 vector 中, 最后 insert 起点 Node, 再反转 reverse 回来即可得到正序无重叠的 vector.速度规划
GetTemporalProfile()
函数首先对轨迹按照挡位的变化为条件对轨迹进行切割, 即TrajectoryPartition()
函数, 通过tracking_angle
与heading_angle
也就是行驶方向角度与初始 Node 的自车航向角的关系来判断是否换挡.换挡的节点处的处理方式值得借鉴, vector 容器先增加空元素再具体添加元素内容. 如下:
1 | partitioned_result->emplace_back(); |
速度规划有 2 种, 根据 FLAGS_use_s_curve_speed_smooth
标志位进行选择.
GenerateSCurveSpeedAcceleration()
通过 QP 求解五次多项式的形式规划出速度轨迹.
这里面用到了 SpeedData
的数据结构. 稍微讲一下, 用法值得学习一下, 下面是其头文件.
1 | //直接继承 STL 的 vector 类(Apollo 里有挺多直接继承 STL 的数据结构类).SpeedPoint 为 Protolbuf 数据类型. 同时使用 std::sort() 按照时间 t 排序. |
速度规划的输入是按照换挡为节点分段的 Path 曲线, 也就是静止-加速-匀速-刹停的过程.
求解
accumulated_s
这个容许我得吐槽一下, 为什么直接用欧式距离分别求各点到段初始点的accumulated_s
, 为啥不在上面推点列用自车动力学模型的时候计算每段的单独长度. 然后再累加起来得到accumulated_s
.初始/边界确定
1 | // assume static initial state初始静止 |
每段时间跨度
$$
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 的住.
- 使用 OSQP 求解. 这部分我暂时没有仔细看,估计与官方的qp_spline_st_speed_optimizer说明是类似的.
使用专门的PiecewiseJerkSpeedProblem
求解类求解.(OSQP 求解piecewise 5 次多项式参考博文)
速度规划的简单示意图如下, 上面的为 path 图, 下面的为 TS 图:
- 把求解所得的 SpeedData 与 Path 合并成为最终的轨迹.
整个过程是数据的对齐与线性插值, 然后放到HybridAStartResult
类型数据里面. 看的实在懵了, 遂决定梳理一下这个数据结构闪转腾挪的过程, 如下图.
图中的 v(s)
代表 std::vector<double> s
.
这里将 SpeedData
与 DiscretizedPath
合并的原则是考虑后面的 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 的线性插值方法移植到它的数据结构里, 结果还需要使用其他数据结构里的函数 DiscretizedPath
的 Evaluate(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)
$$
原理如下图:
最后把所有结果 copy 到最终结果中. 返回 HybridAStartResult
数据.
1 | std::copy(result.x.begin(), result.x.end() - 1, std::back_inserter(stitched_result.x)); |
观码感触
使用
decltype
clear 一种容器. 例如dp_map_ = decltype(dp_map_)();
.继承 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
参考链接
Apollo Open Space Planner 介绍 1-Hybrid A star
https://www.chuxin911.com/apollo_open_space_planner_intro_1_hybrid_A_star_20211017/