拓展车道处 生成refpath跳变 debounce

一、修复背景在目的地 / MRC 场景下,参考路径(refpath)由「最近车道中心线 nearest_lane_points」生成。单条车道中心线在感知上往往长度有限(前方被截断),如果直接用这段较短的中心线生成 refpath,会出现两个问题:1. 前方无路可走 : refpath 长度不足,下游规划缺少足够的前视距离。2. 帧间跳变(闪烁) :感知每帧输出的车道中心线端点、拼接结果不稳定,导致 refpath 末端来回抖动。extend_lane_center 函数的作用以当前最近车道中心线的末端为起点,在所有候选车道中心线里贪心地挑选最顺、最近、最贴合的一段不断向前拼接,把 refpath 延长到目的地方向足够远的位置。通过一套 硬阈值过滤 软代价择优 的择路规则,保证每帧挑到的延长段尽量一致,从而抑制 refpath 在拓展车道处的跳变(debounce)。二、输入 / 输出入参\nearest_lane_points\由上游算法计算最进车道线点位我们只做debounce优化与refpath的拼接。入参\lane_centers\全量的候选车道车道中心线点集{TRakcId - points} {nearst_lane_track_id}当前最近点车道的TrackId拼接时忽略自己入参\distance_to_destination\ 到达目的地的距离作为最大允许延长长度 //Robotaxi方向输出/best_id/通过软硬约束过滤之后的最佳拼接车道中心线id输出/best_points/通过软硬约束过滤之后的最佳拼接车道中心线点位输出/best_cost/refpath与最优拼接线最终长度三、 关键常量(阈值)kMinLCExtendDist 候选段起点到当前末端的最大间隙距离 lc_end_distkMaxAngleDiff 当前末端方向与候选段起始方向的最大允许夹角 alphakMaxDistance 候选段起点在当前折线上的横向偏差 pt_l 上限四、算法流程外层外层while (true)循环,每轮尝试拼接一段;拼接成功则以新末端继续,直到无候选或触发终止条件。4.1 计算当前末端方向const auto vec_a planning_math::Vec2d(lane_center_last_point.x - lane_center_second_last_point.x,lane_center_last_point.y - lane_center_second_last_point.y);取 nearest_lane_points 最后两点,构造末端切向向量 vec_a,并计算当前折线总长 current_length。4.2 遍历候选车道中心线过滤择优对 lane_centers中每条候选段(排除自身nearst_lane_track_id_、已用过的 extended_ids、车道中心线点数 2)1. const auto vec_c planning_math::Vec2d(curr_lc_second_point.x - curr_lc_first_point.x,curr_lc_second_point.y - curr_lc_first_point.y);取候选段前两点构造起始方向 vec_c。2. 计算夹角 alpha angle_between_vectors(vec_a, vec_c)。3. 计算间隙距离lc_end_dist候选段起点到当前末端的欧氏距离。4. 通过 GetSL把候选段起点投影到当前折线,得到 pt_s(纵向)、pt_l(横向)。投影失败则跳过。4.3angle_between_vectors函数解析double angle_between_vectors(const planning_math::Vec2d vec1, const planning_math::Vec2d vec2) { // 计算点积和叉积 double dot vec1.InnerProd(vec2); double cross vec1.CrossProd(vec2); // 使用 atan2 计算夹角并归一化到 [-π, π] double angle std::atan2(cross, dot); return npp::planning_math::NormalizeAngle(angle); }通过计算点积叉积最后使用atan2(x,y)计算 极坐标下的夹角4.4软硬约束过滤硬约束过滤alpha kMaxAngleDiff (6°)lc_end_dist kMinLCExtendDist (20m)|pt_l| kMaxDistance (1m)pt_s current_length软代价择优cost |pt_l| * 1.0 |alpha| * 0.05 lc_end_dist * 0.1取 cost 最小者为本轮最优 best_id / best_points,并累计该段自身长度best_extend_length软代价以横向偏差 pt_l 权重最高(1.0),因此优先选与当前中心线最对齐的候选,这是抑制横向跳变的核心。五、源码extend_lane_center( std::vectorplanner::Point nearest_lane_points) { if (nearest_lane_points.size() 2) { return; } constexpr double kMinLCExtendDist 20.0; constexpr double kMaxAngleDiff 6.0; // 最大允许角度差 constexpr double kMaxDistance 1.0; // 最大允许距离 size_t extend_lc_count 0; double max_extend_length session_-world_model().get_destination_info().distance_to_destination; double extend_length 0.0; const auto lane_centers session_-world_model().get_lanemark_navi_manager().lane_centers(); std::unordered_setmaf_landmark::TrackId, MafTrackidKeyHash, MafTrackidKeyEqual extended_ids; bool can_extend true; while (can_extend) { can_extend false; const auto lane_center_last_point nearest_lane_points.back(); const auto lane_center_second_last_point *(nearest_lane_points.rbegin() 1); const auto vec_a planning_math::Vec2d( lane_center_last_point.x - lane_center_second_last_point.x, lane_center_last_point.y - lane_center_second_last_point.y); auto current_nearest_lane_points_vec2d destination::convert_to_vec2d(nearest_lane_points); auto current_length destination::total_length(current_nearest_lane_points_vec2d); const maf_landmark::TrackId *best_id nullptr; const std::vectorplanner::Point *best_points nullptr; double best_cost std::numeric_limitsdouble::max(); double best_extend_length 0.0; for (const auto lane_pair : lane_centers) { const auto track_id lane_pair.first; const auto points lane_pair.second; if (maf_trackid_equal(nearst_lane_track_id_, track_id) || points.size() 2) { continue; } if (extended_ids.find(track_id) ! extended_ids.end()) { continue; } const auto curr_lc_first_point points.front(); const auto curr_lc_second_point points[1]; // const auto vec_b planning_math::Vec2d(curr_lc_first_point.x - // lane_center_last_point.x, // curr_lc_first_point.y - // lane_center_last_point.y); const auto vec_c planning_math::Vec2d(curr_lc_second_point.x - curr_lc_first_point.x, curr_lc_second_point.y - curr_lc_first_point.y); const double alpha destination::angle_between_vectors(vec_a, vec_c) * 180.0 / M_PI; const double lc_end_dist fast_hypot(curr_lc_first_point.x - lane_center_last_point.x, curr_lc_first_point.y - lane_center_last_point.y); double pt_s 0.0, pt_l 0.0; int pt_min_idx -1; const planning_math::Vec2d target_pt{curr_lc_first_point.x, curr_lc_first_point.y}; if (!GetSL(current_nearest_lane_points_vec2d, target_pt, pt_s, pt_l, pt_min_idx, false)) { continue; } // 判断是否满足拼接条件 硬约束阈值过滤 if (std::fabs(alpha) kMaxAngleDiff) { continue; } if (lc_end_dist kMinLCExtendDist) { continue; } if (std::fabs(pt_l) kMaxDistance) { continue; } if (pt_s current_length) { continue; } //软代价横向为主 const double cost std::fabs(pt_l) * 1.0 std::fabs(alpha) * 0.05 lc_end_dist * 0.1; if (cost best_cost){ //计算候选的延长长度 double curr_extend_length 0.0; for (size_t j 1; j points.size(); j) { curr_extend_length fast_hypot(points[j].x - points[j - 1].x, points[j].y - points[j - 1].y); } best_id track_id; best_points points; best_cost cost; best_extend_length curr_extend_length; } } if (best_id ! nullptr) { (void)nearest_lane_points.insert(nearest_lane_points.end(), best_points-begin(), best_points-end()); extend_length best_extend_length; extend_lc_count; extended_ids.insert(*best_id); can_extend true; } // 检查最大延长次数 if (extend_lc_count lane_centers.size()) { break; } // 检查总延长长度 if (extend_length max_extend_length) { break; } } }