z

zsy2yyl

V1

2023/03/05阅读：48主题：默认主题

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

## 引言

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

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;}

z
V1