百度Apollo规划算法——轨迹拼接

引言

在apollo的规划算法中，在每一帧规划开始时会调用一个轨迹拼接函数，返回一段拼接轨迹点集，并告诉我们是否重规划以及重规划的原因，那大家是否深入了解并思考过什么是轨迹拼接、为什么要进行轨迹拼接以及是如何进行轨迹拼接的呢？本篇文章便是针对这几个问题，在参考了前辈的一些文章和Apollo代码的基础上进行解答。

轨迹拼接

1、什么是轨迹拼接？

在每一帧轨迹规划的开始，我们需要确定规划的起始点 p 0 = ( t , x , y , v , a , h e a d i n g , k a p p a ) p_0=(t,x,y,v,a,heading,kappa) p0​=(t,x,y,v,a,heading,kappa)，起始点 p 0 p_0 p0​给定了规划的初始状态，然后结合当前帧的环境信息并调用规划算法规划生成当前帧轨迹。
那么问题来了，每一帧规划的起始点 p 0 p_0 p0​是如何确定的呢？在Apollo规划算法中采用了两种方案：
方案1：
根据ADC（Autonomous Driving Car）实际位置状态信息或根据ADC实际位置状态信息，通过车辆运动学推导0.1s后的位置状态信息，作为规划起始点状态，在Apollo中，这一方案叫做重规划（Replan），但是它属于轨迹拼接的一种，只是拼接点集只有一个点 p 0 p_0 p0​而已。
方案2：
根据当前帧时间戳以及ADC实际位置信息，在上一帧轨迹中寻找匹配点，将上一帧轨迹中匹配点往后0.1s所对应的轨迹点作为当前帧的规划起始状态点，待当前帧规划生成轨迹后，再和上一帧轨迹中所选择的规划起始点前一段距离的轨迹点进行拼接，形成一条完整的车辆运动轨迹，发送给下游控制模块。
严格来说，以上两种方案都属于轨迹拼接，区别是方案1拼接轨迹只有一个点，而方案2的拼接轨迹是上一帧的部分有序点集，但是在Apollo代码中，称第一种方案叫做重新初始化拼接点（也叫重规划）。

2、为什么要进行轨迹拼接？

在了解了什么是轨迹拼接，不知道大家有没有思考过，轨迹拼接的方法为什么有两种，是基于什么原因想法设计的呢？
在轨迹拼接中纳入第二种方法主要考虑的是连续帧之间轨迹的平滑性和连续性，因为轨迹的这两个特性会严重影响到下游控制的效果。理论上，如果控制跟踪和定位完美，不会出现任何误差，则完全可以使用方案1选择规划起始点，但是，由于定位误差和控制误差/滞后的原因，会导致每一规划时刻ADC位置大概率的偏离上一帧所规划的轨迹点，如果此时仍采用方案1，会导致当前帧所规划轨迹和上一帧规划轨迹的不平滑和不连续，从而导致控制的抖动，也可能会引起ADC行驶轨迹发散，跟踪误差越来越大，偏离设定参考线。而方案2可以保证ADC附近一段范围内前后两帧轨迹的平滑性，从而不断引导ADC沿着期望轨迹行驶。

3、结合Apollo代码为例理解轨迹拼接的细节。

结合图1和TrajectoryStitcher::ComputeStitchingTrajectory这个函数，理解哪些情况下选择方案1、哪些情况选择方案2。
选择方案1的情况有：
a.没有使能方案2进行轨迹拼接
b.上一帧不存在轨迹（一般是起步或上一帧规划失败的情况）
c.没有处在自动驾驶模式
d.上一帧存在轨迹但是没有轨迹点
e.当前时间相对于上一帧的相对时间戳小于上一帧起点相对时间戳
f.时间匹配点超出上一帧轨迹点的终点实际范围
g.匹配点处不存在匹配的path_point
h.ADC实际位置和匹配点横纵向偏差超过给定阈值
方案1生成轨迹拼接点主要有两种情况：
情况1——起步阶段：由于起步阶段属于规划算法执行的第一帧，且速度、加速度值很小，可以直接使用当前状态点作为规划的起始状态点；
情况2——非起步阶段：则根据当前ADC状态点结合运动学模型外推 T T T时间之后的状态点作为本帧规划起始点。
[ x t + 1 y t + 1 ψ t + 1 ] = [ x t y t ψ t ] + [ c o s ( ψ ) s i n ( ψ ) t a n ( δ f ) l ] ⋅ v \begin{bmatrix}x_{t+1}\\y_{t+1}\\\psi_{t+1} \end{bmatrix}= \begin{bmatrix}x_{t}\\y_{t}\\\psi_{t} \end{bmatrix}+\begin{bmatrix}cos(\psi)\\sin(\psi)\\\frac{tan(\delta_f)}{l} \end{bmatrix}\cdot v ​xt+1​yt+1​ψt+1​​ ​= ​xt​yt​ψt​​ ​+ ​cos(ψ)sin(ψ)ltan(δf​)​​ ​⋅v

这两种情况都属于重新初始化轨迹拼接点。

当以上情况均不满足时，采取方案2进行轨迹拼接，方案2轨迹拼接的具体方法为：
该方案同时考虑了时间匹配和位置匹配：
对于时间匹配：
根据当前时间戳和上一帧轨迹起点的时间戳对比，计算当前时间ADC应该到达的时间匹配点（图1中t_pos）及该匹配点在上一帧轨迹中对应的索引值t_index，若t_pos不存在路径点，则选择方案1。
对于位置匹配：
根据ADC当前位置点在上一帧轨迹中寻找最近的匹配点（图1中p_pos）及该匹配点在上一帧轨迹中对应的索引值p_index，并比较实际位置点和匹配点之间的横纵向位置偏差，若任一偏差超过给定阈值，则选择方案1。

若以上判断均通过，根据 （当前时刻的绝对时间-上一时刻轨迹的初始时间+planning_cycle_time） 得到forward_rel_time，选取时间匹配索引和位置匹配索引中的较小索引作为匹配点索引值matched_index，在上一帧的轨迹上截取出matched_index往前n个点开始，至forward_rel_time的一段轨迹，作为stitching_trajectory。最后，遍历这段轨迹，对其relative_time和s进行赋值。最终，使用stitching_trajectory的最后一个点，即图1中的p_0，作为当前帧轨迹规划的起始点。

选择当前时间+planning_cycle_time的时间对应的上一帧轨迹的位置点作为本帧规划起点的原因是，当本帧规划需要时间planning_cycle_time，将生成轨迹送到规划模块时对应的实际时间理论上就是当前时间+planning_cycle_time。

Apollo轨迹拼接代码

/* Planning from current vehicle state if:
1. the auto-driving mode is off
(or) 2. we don't have the trajectory from last planning cycle
(or) 3. the position deviation from actual and target is too high
*/
std::vector<TrajectoryPoint> TrajectoryStitcher::ComputeStitchingTrajectory(
const VehicleState& vehicle_state, const double current_timestamp,
const double planning_cycle_time, const size_t preserved_points_num,
const bool replan_by_offset, const PublishableTrajectory* prev_trajectory,
std::string* replan_reason) {
    //a.是否使能轨迹拼接
    if (!FLAGS_enable_trajectory_stitcher) {
        *replan_reason = "stitch is disabled by gflag.";
        return ComputeReinitStitchingTrajectory(planning_cycle_time, vehicle_state);
    }

    //b.上一帧是否生成轨迹
    if (!prev_trajectory) {
        *replan_reason = "replan for no previous trajectory.";
        return ComputeReinitStitchingTrajectory(planning_cycle_time, vehicle_state);
    }

    //c.是否处于自动驾驶模式
    if (vehicle_state.driving_mode() != canbus::Chassis::COMPLETE_AUTO_DRIVE) {
        *replan_reason = "replan for manual mode.";
        return ComputeReinitStitchingTrajectory(planning_cycle_time, vehicle_state);
    }

    //d.上一帧是否存在轨迹点 
    size_t prev_trajectory_size = prev_trajectory->NumOfPoints();
    if (prev_trajectory_size == 0) {
        *replan_reason = "replan for empty previous trajectory.";
        return ComputeReinitStitchingTrajectory(planning_cycle_time, vehicle_state);
    }

    const double veh_rel_time = current_timestamp - prev_trajectory->header_time();
    size_t time_matched_index = prev_trajectory->QueryLowerBoundPoint(veh_rel_time);

    //e.判断当前时间相对于上一帧的相对时间戳是否小于上一帧起点相对时间戳
    if (time_matched_index == 0 &&
        veh_rel_time < prev_trajectory->StartPoint().relative_time()) {
        *replan_reason =
            "replan for current time smaller than the previous trajectory's first "
            "time.";
        return ComputeReinitStitchingTrajectory(planning_cycle_time, vehicle_state);
    }
    
    //f.判断时间匹配点是否超出上一帧轨迹点范围
    if (time_matched_index + 1 >= prev_trajectory_size) {
        *replan_reason =
            "replan for current time beyond the previous trajectory's last time";
        return ComputeReinitStitchingTrajectory(planning_cycle_time, vehicle_state);
    }

    auto time_matched_point = prev_trajectory->TrajectoryPointAt(
    static_cast<uint32_t>(time_matched_index));

    //g.判断时间匹配点处是否存在path_point
    if (!time_matched_point.has_path_point()) {
        *replan_reason = "replan for previous trajectory missed path point";
        return ComputeReinitStitchingTrajectory(planning_cycle_time, vehicle_state);
    }

    size_t position_matched_index = prev_trajectory->QueryNearestPointWithBuffer(
    {vehicle_state.x(), vehicle_state.y()}, 1.0e-6);

    //计算实际位置点和匹配点的横纵向偏差
    auto frenet_sd = ComputePositionProjection(
    vehicle_state.x(), vehicle_state.y(),
    prev_trajectory->TrajectoryPointAt(
    static_cast<uint32_t>(position_matched_index)));

    //h.判断横纵向偏差
    if (replan_by_offset) {
        auto lon_diff = time_matched_point.path_point().s() - frenet_sd.first;
        auto lat_diff = frenet_sd.second;
        //h.1:横向偏差不满足条件
        if (std::fabs(lat_diff) > FLAGS_replan_lateral_distance_threshold) {
            const std::string msg = absl::StrCat(
            "the distance between matched point and actual position is too "
            "large. Replan is triggered. lat_diff = ",
            lat_diff);
            *replan_reason = msg;
            return ComputeReinitStitchingTrajectory(planning_cycle_time,
            vehicle_state);
    	}
        //h.2:纵向偏差不满足条件
        if (std::fabs(lon_diff) > FLAGS_replan_longitudinal_distance_threshold) {
            const std::string msg = absl::StrCat(
            "the distance between matched point and actual position is too "
            "large. Replan is triggered. lon_diff = ",
            lon_diff);
            *replan_reason = msg;
            return ComputeReinitStitchingTrajectory(planning_cycle_time,
            vehicle_state);
    	}
    } else {
        ADEBUG << "replan according to certain amount of lat and lon offset is "
        "disabled";
    }

    //计算当前时刻后T时刻的时间，并计算其在上一帧轨迹中对应的索引值
    double forward_rel_time = veh_rel_time + planning_cycle_time;
    size_t forward_time_index =
    prev_trajectory->QueryLowerBoundPoint(forward_rel_time);

    ADEBUG << "Position matched index:\t" << position_matched_index;
    ADEBUG << "Time matched index:\t" << time_matched_index;

    //选择时间匹配索引和位置匹配索引中的较小索引作为匹配点索引
    auto matched_index = std::min(time_matched_index, position_matched_index);

    //构建拼接轨迹，<匹配索引点前n个点，当前时刻后的T时刻所对应的匹配点>
    std::vector<TrajectoryPoint> stitching_trajectory(
    prev_trajectory->begin() +
    std::max(0, static_cast<int>(matched_index - preserved_points_num)),
    prev_trajectory->begin() + forward_time_index + 1);

    const double zero_s = stitching_trajectory.back().path_point().s();
    for (auto& tp : stitching_trajectory) {
        if (!tp.has_path_point()) {
            *replan_reason = "replan for previous trajectory missed path point";
            return ComputeReinitStitchingTrajectory(planning_cycle_time,
            vehicle_state);
        }
        //适配时间和s值
        tp.set_relative_time(tp.relative_time() + prev_trajectory->header_time() -
        current_timestamp);
        tp.mutable_path_point()->set_s(tp.path_point().s() - zero_s);
    }
    return stitching_trajectory;
}

参考

【1】Apollo 6.0 轨迹拼接算法解析
【2】规划控制之轨迹拼接
【3】第二章第三节(中) 控制接口，轨迹拼接
【4】轨迹拼接(Trajectory Stitching)
【5】Apollo轨迹拼接模块(Trajectory Stitching)研读
【6】Apollo6.0