专业的会议网站建设,香蜜湖附近网站建设,企业网站网站建设价格,电子商务网站建设考试试题百度Apollo规划算法——轨迹拼接引言轨迹拼接1、什么是轨迹拼接#xff1f;2、为什么要进行轨迹拼接#xff1f;3、结合Apollo代码为例理解轨迹拼接的细节。参考引言 在apollo的规划算法中#xff0c;在每一帧规划开始时会调用一个轨迹拼接函数#xff0c;返回一段拼接轨迹…
百度Apollo规划算法——轨迹拼接引言轨迹拼接1、什么是轨迹拼接2、为什么要进行轨迹拼接3、结合Apollo代码为例理解轨迹拼接的细节。参考引言 在apollo的规划算法中在每一帧规划开始时会调用一个轨迹拼接函数返回一段拼接轨迹点集并告诉我们是否重规划以及重规划的原因那大家是否深入了解并思考过什么是轨迹拼接、为什么要进行轨迹拼接以及是如何进行轨迹拼接的呢本篇文章便是针对这几个问题在参考了前辈的一些文章和Apollo代码的基础上进行解答。 轨迹拼接
1、什么是轨迹拼接
在每一帧轨迹规划的开始我们需要确定规划的起始点p0(t,x,y,v,a,heading,kappa)p_0(t,x,y,v,a,heading,kappa)p0(t,x,y,v,a,heading,kappa)起始点p0p_0p0给定了规划的初始状态然后结合当前帧的环境信息并调用规划算法规划生成当前帧轨迹。 那么问题来了每一帧规划的起始点p0p_0p0是如何确定的呢在Apollo规划算法中采用了两种方案 方案1 根据ADCAutonomous Driving Car实际位置状态信息或根据ADC实际位置状态信息通过车辆运动学推导0.1s后的位置状态信息作为规划起始点状态在Apollo中这一方案叫做重规划Replan但是它属于轨迹拼接的一种只是拼接点集只有一个点p0p_0p0而已。 方案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状态点结合运动学模型外推TTT时间之后的状态点作为本帧规划起始点。 [xt1yt1ψt1][xtytψt][cos(ψ)sin(ψ)tan(δf)l]⋅v\begin{bmatrix}x_{t1}\\y_{t1}\\\psi_{t1} \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 vxt1yt1ψt1xtytψtcos(ψ)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。
图1 轨迹拼接Apollo轨迹拼接代码
/* Planning from current vehicle state if:
1. the auto-driving mode is off
(or) 2. we dont have the trajectory from last planning cycle
(or) 3. the position deviation from actual and target is too high
*/
std::vectorTrajectoryPoint 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 trajectorys 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 trajectorys last time;return ComputeReinitStitchingTrajectory(planning_cycle_time, vehicle_state);}auto time_matched_point prev_trajectory-TrajectoryPointAt(static_castuint32_t(time_matched_index));//g.判断时间匹配点处是否存在path_pointif (!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_castuint32_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::vectorTrajectoryPoint stitching_trajectory(prev_trajectory-begin() std::max(0, static_castint(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
