自动驾驶-轨迹拼接

自动驾驶在进行规划之前，要确定当前帧轨迹规划的起点，这个起点常被误认为是当前车辆的位置，即每次以车辆的当前位置进行轨迹规划；其实不是这样的，直观上，这会导致本次次规划的轨迹同上次规划的轨迹之间是不连续的，这个不连续传递到控制模块，由于轨迹规划出的轨迹对于控制而言就是参考线，那么由于参考线是不连续的，对控制器而言就是朝令夕改。

轨迹重规划

上述所讲的直接以当前位置进行规划起点，进行本次轨迹规划的方法，其实称为轨迹重规划，即当前位置与上一帧的参考轨迹差距过大，需要重新规划轨迹；对于这种情况，apollo也做了一定优化，根据自车当前实际位置状态信息，通过车辆运动学推导0.1s后的位置状态信息，作为规划起始点状态，向后推导0.1s，这主要是为了减少执行机构的响应延迟问题，将未来的状态作为执行器的参考输入。

参考链接：apollo轨迹拼接

轨迹拼接

轨迹拼接的核心思想是将当前的规划起点，设置在上次(上一帧)规划出轨迹上，从而保证轨迹的连续性，提升控制效果。
上述思想的实现，需要得到当前自车状态，在上一帧轨迹中的匹配点，匹配点的确定有两种方式：

1. 相对时间匹配
   根据当前时间戳和上一帧轨迹起点的时间戳对比，计算当前时间自车在上一帧轨迹中的时间匹配点（下图中的绿色点）及该匹配点在上一帧轨迹中对应的索引值t_index。
2. 相对里程匹配
   结合自车的定位信息与上一帧轨迹信息，将自车信息从笛卡尔坐标系→Frenet坐标(s,d)，得到当前的位置s，根据当前的s在上一帧的轨迹中，即可查询到在里程维度上的匹配点(下图中的蓝色点)。
   根据index，选取min{时间匹配点，里程匹配点}作为当前车辆在上一帧映射的匹配点。如上图所示，由于绿色点索引更小，故即选择绿色点为匹配点。

在选择完规划起点后，为缓解执行机构的延时，同样向前预测del_t时间，以del_t时刻的点，作为起点，进行当前时刻的轨迹规划，并在上一帧帧的轨迹上截取出matched_index往前n个点开始，至forward_rel_time的一段轨迹，作为stitching_trajectory。
现在的疑问是为何要生成一个stitch trajectory呢？即使不选择，也是和直接的轨迹是平滑与连续的啊？
但是规划起点的选择是明晰的。
参考链接：轨迹拼接

轨迹拼接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;
}