2021-11-20发表2023-02-15更新 cx Apollo23 分钟读完 (大约3391个字)0次访问

Apollo Open Space Planner中主要TASK类执行的过程解读

[TOC]
前几篇博客介绍了open space planner 的中的 HybridAStar, DualVariableWarmStartProblem, IterativeAnchoringSmoother 都是 OpenSpaceTrajectoryOptimizer 类里调用的算法.
本篇补充一下 planner 主要 TASK 类执行的过程, 包括 OpenSpaceRoiDecider, OpenSpaceTrajectoryProvider, OpenSpaceTrajectoryOptimizer, OpenSpaceTrajectoryPartition 还有 OpenSpaceFallbackDecider 类.

整体流程

众所周知 Apollo 的规划是按照 scenario->stage->task 的顺序对场景类别进行规划.其中应用到 open space planner 的主要为带有 parking 的场景(包括 PARK_AND_GO, PULL_OVER, VALET_PARKING), 狭窄车道掉头场景(NARROW_STREET_U_TURN, v6.0 暂时还是空的).

选取 VALET_PARKING 代客泊车场景为例. 包含两个 stage: VALET_PARKING_APPROACHING_PARKING_SPOT 与 VALET_PARKING_PARKING, 以及所含的 Tasks(如下, 基本上见名知义).

FILE:modules/planning/conf/scenario/valet_parking_config.pb.txt

VALET_PARKING_APPROACHING_PARKING_SPOT
task_type: OPEN_SPACE_PRE_STOP_DECIDER
task_type: PATH_LANE_BORROW_DECIDER
task_type: PATH_BOUNDS_DECIDER
task_type: PIECEWISE_JERK_PATH_OPTIMIZER
task_type: PATH_ASSESSMENT_DECIDER
task_type: PATH_DECIDER
task_type: RULE_BASED_STOP_DECIDER
task_type: ST_BOUNDS_DECIDER
task_type: SPEED_BOUNDS_PRIORI_DECIDER
task_type: SPEED_HEURISTIC_OPTIMIZER
task_type: SPEED_DECIDER
task_type: SPEED_BOUNDS_FINAL_DECIDER
task_type: PIECEWISE_JERK_SPEED_OPTIMIZER

VALET_PARKING_PARKING
task_type: OPEN_SPACE_ROI_DECIDER
task_type: OPEN_SPACE_TRAJECTORY_PROVIDER
task_type: OPEN_SPACE_TRAJECTORY_PARTITION
task_type: OPEN_SPACE_FALLBACK_DECIDER

每个 task 的执行是通过 TaskFactory 类, 使用工厂模式统一调度实现的, 后面我会专门写一篇博文梳理这个设计模式. 还是先聚焦 VALET_PARKING_PARKING stage (主要泊车).
该 stage 分为 4 步:

OPEN_SPACE_ROI_DECIDER
通过 OpenSpaceRoiDecider 类实现, 主要是为后面的算法提供数据整理等准备工作.
OPEN_SPACE_TRAJECTORY_PROVIDER
通过 OpenSpaceTrajectoryProvider 类实现, 主要提供状态机(stop/replan 等)以及多线程计算的功能. 里面会去调用 OpenSpaceTrajectoryOptimizer 类, 具体实现规划算法, 是核心部分, 其中又分为四个计算模块:

HybridAStar: Hybrid A* 搜索获取粗轨迹.
DualVariableWarmStartProblem: 使用 Hybrid A* 搜索的结果算出避障问题对偶问题下的 warm start(QCQP 问题).
DistanceApproachProblem: 利用 warm start 进行 MPC 优化求解.
IterativeAnchoringSmoother: 对 Hybrid A* 搜索的结果分段后进行离散点光滑, 然后分段使用 DistanceApproachProblem 优化.

OPEN_SPACE_TRAJECTORY_PARTITION
通过 OpenSpaceTrajectoryPartition 类实现. 主要作用是按照换档时机分段, 插值/修改/拼接 s,t 等点的信息, 形成可以发给下游的 stitched_trajectory.
OPEN_SPACE_FALLBACK_DECIDER
通过 OpenSpaceFallbackDecider 类实现. 主要作用是执行一段 5s 的预测, 检查碰撞可能性. 如果存在碰撞, 生产 fallback 轨迹, 在最大限度地保证刹停舒适度的前提下停车.

下面分类解读.
之前的博文对代码的讲解都是通过头文件结合解说文件进行的, 后来发现即便是自己后面翻阅也不是很方便, 于是这次尝试使用流程图的形式总结.

OpenSpaceRoiDecider

流程如下图:

需要补充说明的是计算后面优化所用到的广义障碍物 $A^T_mx=b_m$(存储在 obstacles_vertices_vec 中).
存放的顺序是 parking_boundaries(直线), 然后是 perception_obstacles(四边形).
对于 parking_boundaries, 做的处理主要包括, 对 origin 的坐标系转换, 去除重复/包含的线段, 把方向一致位置连续的线段连成一条线段等.
对于 perception_obstacles, 做的处理主要包括对 origin 的坐标系转换, 适当的膨胀, 够造成 closed convex set 即把第一个向量加在末尾重复一遍.
最后设置 set_obstacles_num=parking_boundaries_num +perception_obstacles_num.

OpenSpaceTrajectoryProvider

此类的运行流程如下图:

因为涉及到状态机所以看起来有些杂乱.

其中使用到了多线程, 但是实际看下来多线程好像被删掉了, 并且 IPOPT 求解器官方称不是线程安全的, 因此我猜测应该不是为了求某个解而新建线程, 可能是为了多个库位平行求解, 然后逐步确定选择泊入某个库位, 虽然代码里没有看到多库位输入的蛛丝马迹? 还是分段优化求解, 保留所有段中的最优解?

删的不太彻底的证据之一就是把线程共享数据锁住后来了一个全复制 thread_data = thread_data_;.

//生成新线程的函数
void OpenSpaceTrajectoryProvider::GenerateTrajectoryThread() {
  while (!is_generation_thread_stop_) {
    if (!trajectory_updated_ && data_ready_) {
      OpenSpaceTrajectoryThreadData thread_data;{
        std::lock_guard<std::mutex> lock(open_space_mutex_);
        thread_data = thread_data_;//直接全部粗暴copy
      }
      double time_latency;
      Status status = open_space_trajectory_optimizer_->Plan(
          thread_data.stitching_trajectory, thread_data.end_pose,
          thread_data.XYbounds, thread_data.rotate_angle,
          thread_data.translate_origin, thread_data.obstacles_edges_num,
          thread_data.obstacles_A, thread_data.obstacles_b,
          thread_data.obstacles_vertices_vec, &time_latency);
      frame_->mutable_open_space_info()->set_time_latency(time_latency);
      if (status == Status::OK()) {
        std::lock_guard<std::mutex> lock(open_space_mutex_);
        trajectory_updated_.store(true);
      } else {
        if (status.ok()) {
          std::lock_guard<std::mutex> lock(open_space_mutex_);
          trajectory_skipped_.store(true);
        } else {
          std::lock_guard<std::mutex> lock(open_space_mutex_);
          trajectory_error_.store(true);
        }
      }
    }
  }
}

产生新线程的语句如下:

1 2	task_future_ = cyber::Async( &OpenSpaceTrajectoryProvider::GenerateTrajectoryThread, this);

其中 cyber::Async 为 Apollo 对 std::async 的封装. 下面为封装后的函数模板(函数对象类型为 F, 参数们为 Args, ... 表明为长度可变参数).
通过 IsRealityMode 区分是实车运行还是仿真运行, 仿真运行的话, 通过 std::launch::async 强制新开线程.
实车运行的话通过 TaskManager 类管理/生成线程.

template <typename F, typename... Args>
static auto Async(F&& f, Args&&... args)
    -> std::future<typename std::result_of<F(Args...)>::type> {
  return GlobalData::Instance()->IsRealityMode()
             ? TaskManager::Instance()->Enqueue(std::forward<F>(f),
                                                std::forward<Args>(args)...)
             : std::async(
                   std::launch::async,
                   std::bind(std::forward<F>(f), std::forward<Args>(args)...));
}

对于多线程的运作通过如下成员变量猜测:

OpenSpaceTrajectoryThreadData thread_data_; 
//线程共享输入数据,可读的话不用锁,但是使用到了互斥锁因此删之前的线程应该会修改某些数据
std::future<void> task_future_; //future
std::atomic<bool> is_generation_thread_stop_{false};//线程计算意外失败抛出停止标志位
std::atomic<bool> trajectory_updated_{false};//计算成功
std::atomic<bool> data_ready_{false};//共享数据准备成功
std::atomic<bool> trajectory_error_{false};//计算不出轨迹
std::atomic<bool> trajectory_skipped_{false};//计算结果不符合要求,跳过
std::mutex open_space_mutex_;

OpenSpaceTrajectoryOptimizer

整个 planner 算法的核心部分. 包含了 Hybrid A* 搜索, warm start QCQP 问题计算, 分段光滑, 以及最终的 MPC 求解. 流程如下图, 四个计算模块位置涂蓝.

其中 FLAGS_enable_parallel_trajectory_smoothing 标志位用来判断是否把 Hybrid A* 搜索的结果分段优化 MPC 与否.
是按照前进后退分段, 因此有一点注意: 除了第一段需要把 v,a,steer 按照上一帧的 stiching trajectory 赋值外, 其余段的 v,a,steer 都为 0.
如果分段的话再根据 FLAGS_use_iterative_anchoring_smoother 标志位判断是否先光滑再进行优化. 光滑使用 IterativeAnchoringSmoother 类进行光滑, 介绍的博文. 否则按段进行 DistanceApproachProblem 类的 MPC 求解. 这里分段后可以考虑使用多线程对每段开线程求解, 源码中没有这么做.

OpenSpaceTrajectoryPartition

对 OpenSpaceTrajectoryProvider 算出来的 stitched_trajectory 并不符合最终发给控制模块的标准, 关于轨迹拼接模块(Trajectory Stitching)的解读,可以参考博文. 这个模块我觉得叫 OpenSpaceTrajectoryStitching 可能更贴切. 主要功能是按照换挡时机进行分段, 根据每时刻最近的点调整下一帧的 s,t 等信息, 同时包括换挡执行期间的轨迹插值.

轨迹数据有 2 层结构: 依据换挡的分段 Trajectories, 每段 Trajectory 里又有点列 Points. 依据这个结构, 安排了 2 个优先队列, closest_point 与 closest_point_on_trajs 分别记录每段 Trajectory 里中路径点距离自车的”远近”以及一整段的”远近”.
其中 cost 为符合距离要求(横纵向距离, 航向角偏差等, 相当于 overlap ratio 的剪枝了)内的所有点处的 Pose Box 与当前自车 Pose Box 的 overlap ratio.
这里可能会有疑问, 为什么要设置 closest_point_on_trajs 优先队列呢? 为什么只对下一段求即可呢? 虽然我没有实际设计过, 但存在狭窄空间里的掉头场景(想起了在日产考社内工程驾照时的 40s 狭窄空间掉头的考试了…), 求出来的轨迹很密集, 如果求解出来的轨迹其实被相邻同向轨迹的取代性很高的话, 其实直接把相邻的后面的轨迹点发给控制就可以了, 不会造成明明一把完成的动作, 实际微调了 2-3 把. 当然仅仅是我的猜测, 需要实际构造场景去验证.

同时, 与我上面的猜想有些符合的一点是, Partition 里对每个点进行了编号, 已经经过的点会放到历史记录里 partitioned_trajectories_index_history, 检查没经过才会作为下一帧的 stitched_trajectory 发出去.
编号的代码如下:

static constexpr double encoding_origin_x = 58700.0; //这个数字是怎么得到的?
static constexpr double encoding_origin_y = 4141000.0; //这个数字是怎么得到的?
const auto& init_path_point = trajectory.front().path_point();
const auto& last_path_point = trajectory.back().path_point();
const int init_point_x =
    static_cast<int>((init_path_point.x() - encoding_origin_x) * 1000.0);
const int init_point_y =
    static_cast<int>((init_path_point.y() - encoding_origin_y) * 1000.0);
const int init_point_heading =
    static_cast<int>(init_path_point.theta() * 10000.0);
const int last_point_x =
    static_cast<int>((last_path_point.x() - encoding_origin_x) * 1000.0);
const int last_point_y =
    static_cast<int>((last_path_point.y() - encoding_origin_y) * 1000.0);
const int last_point_heading =
    static_cast<int>(last_path_point.theta() * 10000.0);
*encoding = absl::StrCat(
    // init point
    init_point_x, "_", init_point_y, "_", init_point_heading, "/",
    // last point
    last_point_x, "_", last_point_y, "_", last_point_heading);

检查是否到达每段终点是利用函数 CheckReachTrajectoryEnd().
检查分2步:
第一步,距离每段 Trajectory 最后一个点的 lateral_offset, longitudinal_offset, heading_search_to_trajs_end 横纵向转角误差都小于标定值, 自车速度小于标定值.
第二步, 构造 Box2d path_end_point_box, 计算自车 Box 与最后一个点构造的 Box 重叠占比 end_point_iou_ratio = Polygon2d(ego_box_).ComputeIoU(Polygon2d(path_end_point_box)); 占比大于标定值.
到达每段终点的话, 如果是最后一段 Trajectory 把 Trajectory 的 index 还有 Point 的 index 都放到最大, 否则分别置于下一段 Trajectory 的 index 以及 Point index0. 否则返回 false.

如下图,橘色为 trajectory, 蓝色为自车中心点到终点向量:

最后放上流程图:

OpenSpaceFallbackDecider

BuildPredictedEnvironment()
数据准备, 对 frame 里所有的障碍物从当前帧开始以 FLAGS_trajectory_time_resolution 为单位帧构造 BOX.
存在 std::vector<std::vector<common::math::Box2d>> predicted_bounding_rectangles 里. 时间跨度为 5.0s.
IsCollisionFreeTrajectory()
一个简单的预测环节. 把 OpenSpaceTrajectoryPartition 分完段的轨迹中接下来的一段拿出来 frame->open_space_info().chosen_partitioned_trajectory()
与 predicted_bounding_rectangles 比对.
注意轨迹与障碍物 vector 的对齐不是靠时间点, 而是靠实际自车位置在轨迹上的起始 index 开始与时刻 0 的障碍物对齐.
检查的循环自增量是障碍物的时刻分辨率. 这种方式明显更严格一些, 相比于时间点对齐的方式. 如果 $i$ 时刻上自车 Box 与障碍物 Box HasOverlap(), 并且轨迹时间点 $t_i$- 障碍物时刻点 <5.0s 的话, 判断有碰撞风险.并返回预测碰撞时刻 $i$.
IsCollisionFreeTrajectory()
判断是否需要 fallback 轨迹, 以及生成 fallback 轨迹. 此处并没有粗暴地原地将减速读拉满, 期望速度设为 0, 而是尽量保证刹停舒适度的情况下, 对刹车过程进行了阶段划分.
对不同阶段与情形生成独特的轨迹. 同时还要考虑前进/后退的区别(下面只考虑了 D 档,R 档的 s/v 等为负之外其他逻辑一致).
阶段的划分见下图:

collosion_poisition 为 IsCollisionFreeTrajectory() 的输出位置 $i$.
保证在碰撞前停止位置为 stop, 在 $d_1$ 距离还够的情况下尽量压低 $a$ 的理想停止位置为 ideal_stop. 每段距离的计算如下图. 虽然图里面是直线(方便示意), 实际是曲线.

$$
d_0 = s_i - s_0 \\
d_1 = \max(\frac{v_0^2}{8.0}, d_0 - 1.0) \\
d_2 = 0.5 a_{de} t_j^2 + v_0 t_j \\
\text{where} \quad a_{de} = (\frac{v_0^2}{2}, a_{max}=|\pm4.0|), t_j = t_0 + j \Delta t
$$
这里的 $\frac{v_0^2}{8.0}$ 如果按照源码里的 $a_{max}=4.0$ 的话, 刹停距离为 $\frac{v_0^2}{2a_{max}}$, 与 $\frac{3v_0^2}{2a_{max}}$ 的计算值相比小了 67%.
不大清楚这个 $\frac{v_0^2}{8.0}$ 是如何得出的. 暂且当作标定值理解吧.
对于 current~fallback_start 阶段: 更改每个路径点的 v 为 fallback_start的 $v_0$, a 为最大减速度 $a_{de}$.
其他阶段, 上面的图是最理想的状态. 实际可能没有那么乐观.
例如, 假如 $d_0 \approx 0$, 直接 $a_{de}$ 刹停, 剪掉 fallback_start 之后的所有路径点, 重新放入 20 个点(拷贝 fallback_start 点, 时间上 0.5s 间隔, 共 10s), 第 $i$ 点处的时间为 $t_i = 0.5i+0.5+t_0$.
如果 $d_1 \approx 0$, 设 $v_0 = 0, a_0 = a_{de}$, 重复上面剪掉过程.
如果 $d_1 > 0$, 如果能找到解出二次方程的 $t_j$ ,则按照二次方程依次赋值 v,a. 否则找不到 $t_j$ 的话, 按照 $d_1 \approx 0$ 的情况生成轨迹.
但是代码里有一处我觉得有些问题:

//modules/planning/tasks/deciders/open_space_decider/open_space_fallback_decider.cc:162
// 1. Set fallback start speed to 0, acceleration to max acceleration.
AINFO << "Stop distance within safety buffer, stop now!";
fallback_start_point.set_v(0.0);
fallback_start_point.set_a(0.0);
fallback_trajectory_pair_candidate.first[stop_index].set_v(0.0);
fallback_trajectory_pair_candidate.first[stop_index].set_a(0.0); //这里a不是max acceleration吗?

后面应该是把生成的 stitched_trajectory 发送给下游. 完成本周期的规划.

Apollo Open Space Planner中主要TASK类执行的过程解读

https://www.chuxin911.com/apollo_open_space_planner_intro_3_code_flow_20211120/

作者

发布于

2021-11-20

更新于

2023-02-15

许可协议

Apollo Open Space Planner中主要TASK类执行的过程解读

整体流程

OpenSpaceRoiDecider

OpenSpaceTrajectoryProvider

OpenSpaceTrajectoryOptimizer

OpenSpaceTrajectoryPartition

OpenSpaceFallbackDecider

作者

发布于

更新于

许可协议

目录

分类

最新文章

目录

标签

订阅更新