技术文档｜规划场景之开放空间轨迹分区

从开发套件出发，手把手教你如何实践Apollo自动驾驶技术，给对自动驾驶研究感兴趣的开发者带来实质帮助，加速每一位开发者的研发进程。本文将介绍 开放空间轨迹分区。主要从以下几点为大家进行详细讲解：

• 概览
• 代码阅读

概览

Apollo 规划是基于场景的，其中每个驾驶用例都被视为不同的驾驶场景。

开放空间轨迹划分任务用于对开放空间轨迹提供任务得到的拼接轨迹进行划分和优化。

代码阅读

1、输入:缝合的轨迹(没有优化)/车辆位置信息。

2、通过调用 InterpolateTrajectory() 增加缝合轨迹点，得到插补轨迹。

void InterpolateTrajectory(
    const DiscretizedTrajectory& stitched_trajectory_result,
    DiscretizedTrajectory* interpolated_trajectory);

3、根据航向角和跟踪角，可以确定换挡点。使用std::vector存储齿轮信息和轨迹点，然后从换挡点将轨迹划分为一组轨迹。

void PartitionTrajectory(const DiscretizedTrajectory& trajectory,
                         std::vector<TrajGearPair>* partitioned_trajectories);

4、如果由于回退停止而重新规划，初始状态位置将设置为False。

当重新规划成功时，使用AdjustRelativeTimeAndS()来调整从步骤3中获得的分区轨迹。

void AdjustRelativeTimeAndS(
    const std::vector<TrajGearPair>& partitioned_trajectories,
    const size_t current_trajectory_index,
    const size_t closest_trajectory_point_index,
    DiscretizedTrajectory* stitched_trajectory_result,
    TrajGearPair* current_partitioned_trajectory);

5、当不需要回退时，选择最近的分割轨迹。

根据ADC的位置、航向、车身尺寸和速度信息，获取车辆中心点信息和运动方向。

void UpdateVehicleInfo();

编码分区的轨迹;

   bool EncodeTrajectory(const DiscretizedTrajectory& trajectory,
                        std::string* const encoding);

为了找到轨迹上的最近点，需要确定搜索范围。要求路径终点与ADC进入点的距离、航向搜索差、航向航迹差均在阈值范围内。

基于ADC框和路径点框，计算搜索范围内每个路径点的IOU（Intersection Over Union）。如果轨迹终点的 IOU 大于阈值，并且划分的轨迹组有其他轨迹可以使用，则表示 ADC 到达了一个轨迹的末端，可以使用另一个轨迹。然后更新轨迹历史以存储已使用的轨迹。

    bool CheckReachTrajectoryEnd(const DiscretizedTrajectory& trajectory,
                                const canbus::Chassis::GearPosition& gear,
                                const size_t trajectories_size,
                                const size_t trajectories_index,
                                size_t* current_trajectory_index,
                                size_t* current_trajectory_point_index);

    bool CheckTrajTraversed(const std::string& trajectory_encoding_to_check);

    void UpdateTrajHistory(const std::string& chosen_trajectory_encoding);

当ADC不需要切换到下一个轨迹时，使用上面提到的IOU信息找到最近的轨迹点（最大的IOU点）跟随。

如果最近的轨迹点不属于当前轨迹，或者由于一些不正常的情况找不到最近的轨迹点，我们用它UseFailSafeSearch()来获得一个安全的轨迹跟随。

使用该功能时，只关心路径终点与ADC输入点的距离来寻找搜索范围，不再限制角度差。

    bool UseFailSafeSearch(
        const std::vector<TrajGearPair>& partitioned_trajectories,
        const std::vector<std::string>& trajectories_encodings,
        size_t* current_trajectory_index, size_t* current_trajectory_point_index);

如果FLAGS_use_gear_shift_trajectory()设置为true，GenerateGearShiftTrajectory()则会添加一小部分通过调用获得的轨迹，以使车辆在换档过程中平稳行驶。否则我们使用AdjustRelativeTimeAndS()调整分区轨迹。

    bool InsertGearShiftTrajectory(
        const bool flag_change_to_next, const size_t current_trajectory_index,
        const std::vector<TrajGearPair>& partitioned_trajectories,
        TrajGearPair* gear_switch_idle_time_trajectory);

    void GenerateGearShiftTrajectory(
        const canbus::Chassis::GearPosition& gear_position,
        TrajGearPair* gear_switch_idle_time_trajectory);

    void AdjustRelativeTimeAndS(
        const std::vector<TrajGearPair>& partitioned_trajectories,
        const size_t current_trajectory_index,
        const size_t closest_trajectory_point_index,
        DiscretizedTrajectory* stitched_trajectory_result,
        TrajGearPair* current_partitioned_trajectory);

6、返回进程状态。

7、输出：分区轨迹。

本文部分内容参考链接

• 《开放空间轨迹分区》

https://github.com/ApolloAuto/apollo/blob/master/docs/technical_documents/open_space_trajectory_partition.md