自动驾驶-轨迹拼接
发布时间
阅读量：
阅读量
自动驾驶在路径规划之前需要确定当前帧轨迹规划的关键点——通常被错误地认为是车辆当前位置；实际操作中则是基于当前位置来进行轨迹设计；然而实际上这会导致相邻两次规划出的路径之间出现断层；这种断层会被传递至控制系统中去；由于参考线的存在断层，在控制系统看来这就是频繁更改指令
轨迹重规划
在之前所述中采用当前位置作为规划起点的方法，在本次轨迹规划中被应用。这种情况下被称为轨迹重规划技术。 Apollo系统为此进行了相应的优化工作，在实时定位到车辆自身的位置状态下（即当前行驶路线），通过车辆运动学模型推导出0.1秒后的状态信息作为初始点。这种方法的主要目的是为了减少执行机构在响应过程中的延迟问题，在后续的控制过程中将未来的状态设置为执行机构的动作参考依据。
参考链接：apollo轨迹拼接
轨迹拼接
轨迹拼接的主要理念是将当前规划起点放置于上一帧所规划轨迹线上方位置坐标处，并以此确保轨迹连接处连续性的同时提升整体控制效果。
上述思路的具体实施过程需要获得当前车辆的状态信息以及在上一帧轨迹中找到对应的匹配点位置坐标。其中匹配点定位的方法主要有两种：
基于当前时间戳与上一个运动片段起始时刻的时间戳进行比较，在前一个运动片段中找到与当前时间段相对应的重叠区域（如图所示），并确定该重叠区域对应的时间索引值t_index。
基于自车定位数据及前一帧运动轨迹信息的基础上, 将车辆状态转换至Frenet坐标系（s,d），从而获得当前位置参数s. 通过该s值在前一运动轨迹中可确定里程维度上的匹配点（如图所示中的蓝点）.
基于index ，采用较小的时间与里程匹配值作为当前车辆在上一帧的映射关联。由于绿色标记的索引较小，从而选择该绿色标记作为当前帧的映射关联。
在选择完规划起点后, 为了解决执行机构存在的延迟问题, 先向前预测一个时间段\deltat, 并以此\deltat时刻的位置作为起始点, 进行当前时刻的运动路径规划, 并从上一阶段中提取匹配索引开始前n个点的部分路径, 用于拼接运动轨迹的一段
疑问的核心在于为什么要生成一个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;
}
cpp
全部评论 (0)
还没有任何评论哟~