Apollo8.0-Open Space代码实现

自动驾驶打工人

于 2024-05-23 17:28:31 发布

阅读量984

点赞数 12

文章标签：自动驾驶 Apollo open space 代码实现

本文链接：https://blog.csdn.net/NEON7788/article/details/139153447

版权

Apollo8.0-Open Space代码实现

附赠自动驾驶最全的学习资料和量产经验以及100T的资源分享：链接

1. 引言

本文以Apollo8.0的Open Space算法相关代码为例[1][2][3][4]. 详细解释其各个部分的作用与原理. 如果与当前的代码有差别, 以最新的代码为准. 关于Open Space的算法介绍, 请阅读Open Space.

代码目录: modules/planning/open_space/.
可视化工具: modules/tools/open_space_visualization/distance_approach_visualizer.py.

代码中高频出现的英文词汇一览:

单词	解释
Parking Lot	停车场
Parking Spot	停车位
ROI(Region of Interest)	感兴趣区域
ADC(Autonomous Driving Car)	自动驾驶车辆
IOU(Intersection Over Union)	重叠区域 / 联合区域

2. 配置文件

以自主泊车场景为例, 其配置文件如下:

stage_config: {
  stage_type: VALET_PARKING_APPROACHING_PARKING_SPOT
  task_type: OPEN_SPACE_PRE_STOP_DECIDER
  // ...
}
stage_config: {
  stage_type: 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
  // ...
}

自主泊车场景分为两个阶段, 第一个阶段是从普通场景靠近泊车位置附近, 第二个阶段是开始进行泊车.

2.1. `VALET_PARKING_APPROACHING_PARKING_SPOT`

当靠从普通场景走到停车位附近时, 需要先进行停车.

新增加的task是OPEN_SPACE_PRE_STOP_DECIDER, 其作用是找到一个合适的停车位置, 为后续的泊车阶段做准备.

2.2. `VALET_PARKING_PARKING`

正式泊车的阶段, 新增task的作用如下:

OPEN_SPACE_ROI_DECIDER: 确定可行驶区域(道路边界和泊车位的边界确定, 障碍物的边界)

OPEN_SPACE_TRAJECTORY_PROVIDER: 规划一条无碰撞的轨迹.

OPEN_SPACE_TRAJECTORY_PARTITION: 根据轨迹是前进的还是倒车的, 对轨迹进行分割. 根据自车的位置判断发送哪一条轨迹, 以及是否需要切换轨迹. 对轨迹进行平滑.

OPEN_SPACE_FALLBACK_DECIDER: 检查规划的轨迹是否会和障碍物发生碰撞. 如果碰撞了, 就会进入Fallback状态. 规划一条在碰撞位置停车的轨迹. 然后再重新进行路径规划.

3. `OPEN_SPACE_PRE_STOP_DECIDER`

代码见: open_space_pre_stop_decider.cc.

在OpenSpacePreStopDecider::Process中根据不同的stop_type来调用不同场景的所对应的函数.
找到可以停车的位置(CheckXXXPreStop), 并且在该位置设置停止墙(SetXXXStopFence).

4. `OPEN_SPACE_ROI_DECIDER`

作用: 根据不同的开放空间场景创建规划的可行驶区域ROI(Region of Interest), 根据道路边界和泊车位的边界来确定, 最终结果存储在roi_boundary中. ROI的场景类型定义如下:

enum RoiType {
  NOT_DEFINED = 0;
  PARKING = 1;
  PULL_OVER = 2;
  PARK_AND_GO = 3;
  NARROW_STREET_U_TURN = 4;
  DEAD_END = 5;
}

GetParkingSpot, 输出: 停车位的四个角点.

找到目标停车位target_parking_spot, 并安逆时针方向输出停车位的四个角点到parking_vertices中.

5. `OPEN_SPACE_TRAJECTORY_PROVIDER`

本Task的总体目标是在开放空间中生成最终的目标轨迹[5]. 先后调用了混合A*算法和轨迹平滑算法. 代码见: open_space_trajectory_provider.cc.

OpenSpaceTrajectoryProvider::Process(), 本Task的主函数, 被StageVALET_PARKING_PARKING调用.
如果当前正处在Park and Go的检查阶段, 为了保证安全, 生成一个停止的轨迹. GenerateStopTrajectory.
当第一次运行Process()函数的时候开启一个线程. 这会调用GenerateTrajectoryThread()函数来规划第一帧的Open Space轨迹并且更新轨迹的状态. 轨迹的状态有以下三种: trajectory_updated_, trajectory_skipped_, trajectory_error_.
如果自车规划失败而停止, 则调用ComputeReinitStitchingTrajectory重新规划拼接轨迹, 反之则调用ComputeStitchingTrajectory规划拼接轨迹.
根据标志位FLAGS_enable_open_space_planner_thread来确定是否生成轨迹, 在以下几种情况下将生成停止轨迹:
1. Planning线程停止
2. 自车到达终点
3. trajectory_error_触发了超过10秒
4. 上一帧规划失败
调用open_space_trajectory_optimizer_->Plan()生成轨迹, 最终的轨迹信息通过trajectory_data输出.

5.1. 混合A*算法

Hybrid A*代码流程图如下:

Hybrid A* 代码流程图

Hybrid A*算法包含了node3d, grid_search, reeds_shepp_path 和 hybrid_a_star. 其中hybrid_a_star是最重要的组成部分, 它调用了grid_search和reeds_shepp_path[6].

代码文件在http://hybrid_a_star.cc中. 函数入口: HybridAStar::Plan, 在OpenSpaceTrajectoryOptimizer::Plan中被调用.

输入: 当前点(规划起点start point), 目标点(规划终点end point). ROI的XY边界(x和y的最大最小值). 各个障碍物的顶点.

bool HybridAStar::Plan(
double sx, double sy, double sphi, double ex, double ey, double ephi,
const std::vector& XYbounds,
const std::vector<std::vectorcommon::math::Vec2d>& obstacles_vertices_vec,
HybridAStartResult* result);
从单个障碍物中提取点的信息构造直线段, 存储在obstacles_linesegments_vec中用来生成DP Map.
构造规划点start_node和end_node, 这两个成员变量都是Node3d类的对象. 利用ValidityCheck检查合法性之后加入到开放集open_set中.

class Node3d {
public:
Node3d(const double x, const double y, const double phi);
// …
private:
// 位置信息, 角度信息, 代价值
double x_ = 0.0;
double y_ = 0.0;
double phi_ = 0.0;
double cost_ = 0.0;
// 当前节点在栅格中的索引
int x_grid_ = 0;
int y_grid_ = 0;
int phi_grid_ = 0;
// 将上面三个索引组合成字符串
// 用index是否相同来判断两个节点是否是同一个节点
std::string index_;
// 与当前节点所连接的其他的节点的信息
std::vector traversed_x_;
std::vector traversed_y_;
std::vector traversed_phi_;
};
GridSearch::GenerateDpMap: 生成2D节点启发代价表. 基于动态规划算法求解每个二维节点的代价值作为混合A*算法的启发函数.
定义while循环, 每次从优先队列中弹出代价值最小的节点.
1. open_pq是一个优先队列std::priority_queue, 声明中std::pair<std::string, double>表示开放集中的节点顺序, std::vector<std::pair<std::string, double>表示节点的代价值, 以降序排列.
2. AnalyticExpansion: 判断从当前节点到终点是否有无碰撞的RS曲线. 如果找到解析解, 则直接退出循环. 如果没有找到解析解则加入闭合集.
3. Next_node_generator: 从当前节点拓展向外搜索节点. 拓展的节点没有遍历过(通过x y phi三个索引组成的字符串是否相同来判断是否是同一个节点), 也没有和障碍物发生碰撞, 就可以加入到开放集中. 计算节点的代价值.
生成粗糙的轨迹GetResult.
输出: 部分轨迹信息, 具体可在HybridAStartResult结构体中查看.

代码细节:

// 检查是否有碰撞
// 输入: node
// 边界范围判断: 如果节点的x和y超出了边界对应的x和y的范围, 则返回false, 代表无效.
// 边界重叠判断: 如果车辆的边框与任何线段重叠, 则返回 false.
bool ValidityCheck(std::shared_ptr<Node3d> node);

// 生成DP Map
// 输入: 规划终点坐标(ex, ey), x和y的边界XYbounds, 障碍物的边界组成的线段.
// 1. 根据网格分辨率划分XYbounds_网格, 然后获取最大网格值.
// 2. 利用DP(Dynamic Programming动态规划)的方法计算节点的代价值.
bool GenerateDpMap(
    const double ex, const double ey, const std::vector<double>& XYbounds,
    const std::vector<std::vector<common::math::LineSegment2d>>&
        obstacles_linesegments_vec);

// 基于RS曲线, 检查分析该曲线是否能在不发生碰撞的情况下从当前点连接到终点. 如果是则结束搜索.
// 输入: current_node即为规划的起点.
// 1. 调用ShortestRSP()生成最短路径
// 2. 调用ValidityCheck()检查是否有碰撞风险.
// 3. 将RS路径加载为节点, 并将该节点添加到闭合集
bool AnalyticExpansion(std::shared_ptr<Node3d> current_node);

// 根据当前节点, 方向盘角度的均匀采样, 车辆运动学等信息生成下一个节点.
// 输入: 当前节点和下一节点的索引.
// 1. 根据采样点的数量和顺序计算转向角.
// 2. 根据运动学模型生成下一个节点.
// 3. 检查下一节点是否超出XY边界.
std::shared_ptr<Node3d> Next_node_generator(
    std::shared_ptr<Node3d> current_node, size_t next_node_index);

// 计算节点的代价值.
// 输入: 当前节点(车辆位置)和下一节点.
// 包括轨迹代价值和启发代价值
// 1. 轨迹代价由采样距离, 档位切换和转向角变化率决定
// 2. 启发代价来自地图
void CalculateNodeCost(std::shared_ptr<Node3d> current_node,
                        std::shared_ptr<Node3d> next_node);

5.2. RS曲线

相关代码见: http://reeds_shepp_path.cc

RS的曲线生成函数ShortestRSP被AnalyticExpansion调用.

在GenerateRSP中依次调用了SCS, CSC, CCC, CCCC, CCSC这几种模式. 所有的48中模式见Open Space中整理的表格. 生成的可能的Path存储在all_possible_paths中.

5.3. 轨迹平滑算法

输入: open_space_trajectory_provider提供的拼接轨迹, 规划的目标位置end_pose, X和Y的边界, 和停车位置相关的旋转角度, 原始参考点, 障碍物信息.

common::Status Plan(
const std::vectorcommon::TrajectoryPoint& stitching_trajectory,
const std::vector& end_pose, const std::vector& XYbounds,
double rotate_angle, const common::math::Vec2d& translate_origin,
const Eigen::MatrixXi& obstacles_edges_num,
const Eigen::MatrixXd& obstacles_A, const Eigen::MatrixXd& obstacles_b,
const std::vector<std::vectorcommon::math::Vec2d>&
obstacles_vertices_vec,
double* time_latency);
在平滑之前, 需要进行异常情况的处理, 同时做一些预处理的工作.
1. 异常情况: 输入数据为空
2. 异常情况: 规划初始点和终点很接近: IsInitPointNearDestination
3. PathPointNormalizing: 对拼接轨迹的末端进行旋转和平移, 并根据停车位的转角转换轨迹信息.
根据混合 A* 算法的暖启动方法生成粗轨迹: warm_start_->Plan().
当标志位FLAGS_enable_parallel_trajectory_smoothing为false时:
1. 通过LoadHybridAstarResultInEigen()函数将 hybrid_a_star 中初始轨迹点的 (�,�,�,�) 和(steer,a)分别存储到Eigen::MatrixXd xWS和Eigen::MatrixXd uWS中，并使用xWS和uWS生成后续的平滑轨迹, xWS和uWS分别表示状态量和控制量.
2. 通过GenerateDistanceApproachTraj()生成平滑的轨迹.
当标志位FLAGS_enable_parallel_trajectory_smoothing为true时:
1. Trajectorypartition()函数用于分割初始轨迹.
2. 使用loadhybridastarresultineigen()函数将分割后的轨迹分别存储到xWS和uWS中.
3. 设置每条轨迹的初始信息 (�,�) .
4. 第一条轨迹的初始信息为拼接轨迹的终点.
5. 在下一条轨迹中, 初始信息设为零. 在轨迹开始时, 车辆处于静止状态.
输出: 规划后的轨迹

6. `OPEN_SPACE_TRAJECTORY_PARTITION`

本模块的作用是分割和优化从OPEN_SPACE_TRAJECTORY_PROVIDER任务得到的轨迹[5].

输入: 拼接好但还没有优化的轨迹; 车辆的位置信息.
调用InterpolateTrajectory来插值出更多的点.
根据heading的角度和tracking的角度信息, 可以找到车辆档位切换的点. 用TrajGearPair存储档位信息和与之相对应的轨迹点, 然后利用档位切换点将轨迹进行分割.

void PartitionTrajectory(const DiscretizedTrajectory& trajectory,
std::vector* partitioned_trajectories);
如果由于Fallback导致重规划, 那么position_init将会被置为false. 如果重规划成功, 我们用AdjustRelativeTimeAndS来调整从第3步得到的分割后的轨迹并直接返回.
如果没有Fallback, 选择最近的分割轨迹去跟踪.
1. UpdateVehicleInfo: 根据自车的位置, 几何朝向, 车身大小和速度, 得到自车的中心点和速度朝向.
2. EncodeTrajectory: 对分割的轨迹进行编码, 将一段轨迹的起始点和末尾点的位置和朝向转换成字符串进行存储.
3. 为了找到轨迹上最近的点, 需要先确定一个搜索范围. 它需要path的末尾点和自车的进入点. 且航向角的偏差要都在阈值范围内.
  1. ComputeIoU: 通过自车的Box和轨迹上点形成的Box, 计算得到IOU.
  2. CheckReachTrajectoryEnd: 如果一段轨迹的末尾点计算得到的IOU大于阈值, 说明自车到达了这一段轨迹的终点, 可以开始使用下一段轨迹了.
  3. UpdateTrajHistory: 更新轨迹历史, 存储哪些轨迹已经被使用过了.
4. 如果自车不需要切换到下一条轨迹, 使用上述的IOU信息来寻找轨迹上最近的点(最大的IOU所在的点)来跟踪.
5. 如果最近的轨迹点不属于当前轨迹或者在一些异常场景下找不到最近的轨迹点. 通过UseFailSafeSearch来得到一个安全的轨迹进行跟踪. 当使用这个函数时, 去除了角度差的限制, 只考虑Path末端点和自车进入点之间的距离.
6. 如果FLAGS_use_gear_shift_trajectory被设置成true, 则通过调用GenerateGearShiftTrajectory来生成一段小的轨迹使得档位切换时候更加的平滑. 否则, 使用AdjustRelativeTimeAndS来调整分割的轨迹.
如果执行到最后, 返回成功的状态
输出: 分割后的轨迹

7. `OPEN_SPACE_FALLBACK_DECIDER`

检查规划的轨迹是否会和障碍物产生碰撞. 如果有碰撞, 会进入Fallback状态, 规划一条在碰撞位置停车的轨迹.

相关代码见: http://open_space_fallback_decider.cc

代码细节:

// 利用障碍物的预测轨迹计算不同时间下不同障碍物的Box信息
// 输入: 障碍物
// 输出: 障碍物的预测Box信息, 外层vector是不同的时间, 内层vector是不同的障碍物
void BuildPredictedEnvironment(const std::vector<const Obstacle*>& obstacles,
                                std::vector<std::vector<common::math::Box2d>>&
                                    predicted_bounding_rectangles);

// 判断轨迹和障碍物是否没有碰撞
// 如果有碰撞, 输出第一个碰撞点的索引号, 并返回false.
bool IsCollisionFreeTrajectory(
    const TrajGearPair& trajectory_pb,
    const std::vector<std::vector<common::math::Box2d>>&
        predicted_bounding_rectangles,
    size_t* current_idx, size_t* first_collision_idx);

自动驾驶打工人

关注

12
点赞
踩
20

收藏

觉得还不错? 一键收藏
0
评论
Apollo8.0-Open Space代码实现

本文以Apollo8.0的Open Space算法相关代码为例[1][2][3][4]. 详细解释其各个部分的作用与原理. 如果与当前的代码有差别, 以最新的代码为准. 关于Open Space的算法介绍, 请阅读。: 检查规划的轨迹是否会和障碍物发生碰撞. 如果碰撞了, 就会进入Fallback状态. 规划一条在碰撞位置停车的轨迹. 然后再重新进行路径规划.: 根据轨迹是前进的还是倒车的, 对轨迹进行分割. 根据自车的位置判断发送哪一条轨迹, 以及是否需要切换轨迹. 对轨迹进行平滑.
复制链接

扫一扫