planning模块-基于静态障碍物进一步缩放路径边界

```cpp
auto indexed_obstacles = reference_line_info_->path_decision()->obstacles();

indexed_obstacles就是在planning模块-路径边界障碍物介绍的.通过

Obstacle* ReferenceLineInfo::AddObstacle(const Obstacle* obstacle) {
...
auto* mutable_obstacle = path_decision_.AddObstacle(*obstacle);

添加的障碍物id和障碍物信息.

  for (const auto* obs : indexed_obstacles.Items()) {
    const auto& sl_bound = obs->PerceptionSLBoundary();
    for (int i = 0; i < sl_bound.boundary_point_size(); i++) {
      std::string name = obs->Id() + "_obs_sl_boundary";
      print_curve.AddPoint(name, sl_bound.boundary_point(i).s(),
                           sl_bound.boundary_point(i).l());
    }
  }

obs->PerceptionSLBoundary()也是在planning模块-路径边界障碍物介绍的ReferenceLine::GetSLBoundary函数

sl_bound:获取的是障碍物的AABB矩形框(红框),frenet坐标.

sl_bound.boundary_point_size():是单个障碍物的边界点如(p0, p1, p2, p3, mid1)

GetSLPolygons函数

for (const auto* obstacle : obstacles.Items()) {
    if (!IsWithinPathDeciderScopeObstacle(*obstacle)) {
      continue;
    }

过滤掉虚拟障碍物、动态障碍物以及被忽略的障碍物

if (obstacle->PerceptionSLBoundary().end_s() < adc_back_edge_s) {
      continue;
    }

过滤掉自车后方的障碍物

const auto obstacle_sl = obstacle->PerceptionSLBoundary();
    polygons->emplace_back(obstacle_sl, obstacle->Id(),
                           obstacle->Perception().type());

将该障碍物的 SL的AABB矩形框、ID 和类型（如车辆、行人、未知物）存入 polygons列表

  sort(polygons->begin(), polygons->end(),
       [](const SLPolygon& a, const SLPolygon& b) {
         return a.MinS() < b.MinS();
       });

可以理解为按照障碍物纵向位置的最小值从近到远排序
但其实MinS()是每个障碍物bounddary_point的s最小的点的纵向位置

GetBoundaryFromStaticObstacles函数

  if (!PathBoundsDeciderUtil::GetBoundaryFromStaticObstacles(
          *reference_line_info_, &obs_sl_polygons, init_sl_state_, &path_bound,
          &blocking_obstacle_id, &path_narrowest_width)) {
    const std::string msg =
        "Failed to decide fine tune the boundaries after "
        "taking into consideration all static obstacles.";
    AERROR << msg;
    return false;
  }

UpdatePathBoundaryBySLPolygon函数

  if (reference_line_info.IsChangeLanePath() ||
      path_boundary->label().find("regular/left") != std::string::npos ||
      path_boundary->label().find("regular/right") != std::string::npos) {
    center_l.push_back(
        (path_boundary->front().l_upper.l + path_boundary->front().l_lower.l) *
        0.5);
    max_nudge_check_distance = FLAGS_max_nudge_check_distance_in_lk;
  } else {
    center_l.push_back(0.0);
    max_nudge_check_distance = FLAGS_max_nudge_check_distance_in_lc;
  }

根据当前车辆是在“车道保持”还是“换道/绕行”状态，来确定路径规划的“中心参考基准”center_l和“避让检查的纵向距离阈值”max_nudge_check_distance.
lk:表示lane_keep车道保持
lc:表示lane_change换道

path_boundary:是在planning模块-路径边界障碍物通过GetBoundaryFromSelfLane函数获取到的,主要包含的是平滑后参考线两侧的可通行空间的路径边界信息.

*narrowest_width =
      path_boundary->front().l_upper.l - path_boundary->front().l_lower.l;
  double mid_l =
      (path_boundary->front().l_upper.l + path_boundary->front().l_lower.l) / 2;

计算车辆当前位置(即规划起始点)的“可用车道宽度”及其“中心位置横向距离”
path_boundary->front().l_upper.l:上图left_bound
path_boundary->front().l_lower.l:上图right_bound
注意:frenet坐标系l轴是左正右负

size_t nudge_check_count =
      size_t(max_nudge_check_distance / FLAGS_path_bounds_decider_resolution);
  double last_max_nudge_l = center_l.front();

确定在纵向(s轴)上需要检查多少个点(4/0.5=8)，以判断是否存在障碍物阻挡，并初始化横向偏移(l轴)的起始基准.

auto begin_it =
        center_l.end() - std::min(nudge_check_count, center_l.size());
    last_max_nudge_l = *std::max_element(
        begin_it, center_l.end(),
        [](double a, double b) { return std::fabs(a) < std::fabs(b); });

每次检查center_l中的nudge_check_count个点,并获取nudge_check_count个点中的center_l绝对值最大值存入last_max_nudge_l.

 for (size_t j = 0; j < sl_polygon->size(); j++) {
      if (sl_polygon->at(j).NudgeInfo() == SLPolygon::IGNORE) {
        AINFO << "UpdatePathBoundaryBySLPolygon, ignore obs: "
              << sl_polygon->at(j).id();
        continue;
      }

遍历所有障碍物的sl_polygon,并检查当前障碍物的避让决策(NudgeInfo).如果已经被标记为IGNORE,则直接跳过.

double min_s = sl_polygon->at(j).MinS();
      double max_s =
          sl_polygon->at(j).MaxS() + FLAGS_obstacle_lon_end_buffer_park;

min_s / max_s：可以理解为障碍物在 S 轴上的起点和终点
FLAGS_obstacle_lon_end_buffer_park：这是一个纵向安全 Buffer.在障碍物的最远s值后面额外延长一段距离，确保车辆绕行后有足够的空间切回原车道，不至于过早转向导致车尾剐蹭.

if (max_s - min_s < FLAGS_path_bounds_decider_resolution) {
        max_s += FLAGS_path_bounds_decider_resolution;
        min_s -= FLAGS_path_bounds_decider_resolution;
      }

如果障碍物在 S 方向非常短(比如一个小路灯杆或交通锥），甚至小于路径规划的分辨率.
放大它的 S 范围.为了防止采样遗漏.如果障碍物太小，正好落在两个采样点之间，规划器会“看不见”它.

if (max_s < path_boundary_s) {
        continue;
      }

如果障碍物的最远端已经在当前 path_boundary_s 后面位置了.说明这个障碍物已经“过时”了，继续检查 sl_polygon 列表中的下一个

  if (min_s > path_boundary_s) {
        break;
      }

如果当前这个障碍物的起点 min_s 已经超过了当前path_boundary_s,sl_polygon是按 MinS()从小到大排序的,如果当前sl_polygon的min_s超过了path_boundary_s,那后面的所有sl_polygon的min_s也都会超过path_boundary_s,所以break.

GetBufferBetweenADCCenterAndEdge函数

double PathBoundsDeciderUtil::GetBufferBetweenADCCenterAndEdge() {
  double adc_half_width =
      VehicleConfigHelper::GetConfig().vehicle_param().width() / 2.0;

  return (adc_half_width + FLAGS_obstacle_lat_buffer);
}

自车宽度的一半加上障碍物横向缓冲距离(0.4m)

UpdatePassableInfo函数

void SLPolygon::UpdatePassableInfo(double left_bound, double right_bound,
                                   double buffer) {
  if (!is_passable_[0] && !is_passable_[1]) {
    return;
  }
  is_passable_[0] = left_bound > max_l_point_.l() + buffer;
  is_passable_[1] = right_bound < min_l_point_.l() - buffer;
}

判断障碍物的左侧和右侧是否还存在足够的空间让车辆通过
判断车道左边界是否大于障碍物的最左端点max_l_point_.l()加上安全Buffer
判断车道右边界是否小于障碍物的最右端点min_l_point_.l()减去安全 Buffer

GetRightBoundaryByS/GetLeftBoundaryByS

GetInterpolatedSFromBoundary函数

给定一个纵向距离 s，在障碍物的 SL 边界点集合中，精确计算出该 s 对应位置的横向 L 值

double ret = last_iter->l() + (s - last_iter->s()) *
                                    (iter->l() - last_iter->l()) /
                                    (iter->s() - last_iter->s());

线性插值计算

根据 s 在这段区间内的比例，推算出精确的 L 值

double l_lower = sl_polygon->at(j).GetRightBoundaryByS(path_boundary_s);
      double l_upper = sl_polygon->at(j).GetLeftBoundaryByS(path_boundary_s);

获取当前给定path_boundary_s位置的障碍物左右边界[l_upper, l_lower],这里的左右边界是把障碍物看成一个点插值出来的.

PathBoundPoint obs_left_nudge_bound(
          path_boundary_s, l_upper + adc_obs_edge_buffer, left_bound.l);

PathBoundPoint(double s_init, double l_min, double l_max) {
    s = s_init;
    l_lower.l = l_min;
    l_upper.l = l_max;
  }

path_boundary_s:当前位置
l_upper + adc_obs_edge_buffer:障碍物左边界加上自车半宽加上横向缓冲区间
left_bound.l:自车左边界
为车辆划定了一个向左绕行的横向通行的窗口
左边界:由路径边界决定（left_bound.l)
右边界:由障碍物位置决定（l_upper + buffer)

obs_left_nudge_bound.towing_l = path_boundary->at(i).towing_l;

上面没看到有赋值的地方,先默认为0

PathBoundPoint obs_right_nudge_bound(path_boundary_s, right_bound.l,
                                           l_lower - adc_obs_edge_buffer);

为车辆划定了一个向右绕行的横向通行的窗口
左边界:由障碍物位置决定（l_lower - adc_obs_edge_buffer）
右边界:由路径边界决定(right_bound.l)

obs_right_nudge_bound.towing_l = path_boundary->at(i).towing_l;

上面没看到有赋值的地方,先默认为0

枚举项	含义	场景示例
LEFT_NUDGE	向左微调 / 向左避让	障碍物在道路右侧（如右侧停靠车辆、路边施工锥），或者需要从左侧超越前方慢车，车辆向左小幅偏移以保持安全距离。
RIGHT_NUDGE	向右微调 / 向右避让	障碍物在道路左侧（如左侧占道行人、自行车），车辆向右偏移以保持安全距离。
UNDEFINED	未定义 / 未知	初始规划阶段，尚未对障碍物完成分析，或者避让方向尚未计算完成时的默认状态。
BLOCKED	被阻挡	左右两侧都无法安全避让（例如道路过窄、两侧都有障碍物），车辆只能减速直至停车，等待障碍物清除或重新规划。
IGNORE	忽略	障碍物距离较远、在安全范围之外，或不在当前行驶路径上（如旁路静态物体），对行驶无影响，无需采取避让动作。

if (sl_polygon->at(j).NudgeInfo() == SLPolygon::UNDEFINED)

初始状态SLPolygon::UNDEFINED

double obs_l = (l_lower + l_upper) / 2;

obs_l
障碍物在 Frenet 坐标系中的横向中心
l_lower / l_upper 是障碍物在 l 方向的范围
取中点代表障碍物“整体在左还是右”
sl_polygon->at(j).is_passable()[RIGHT_INDEX]
障碍物右侧可通行
sl_polygon->at(j).is_passable()[LEFT_INDEX]
障碍物左侧可通行

mid_l
当前路径边界的中点 l
init_sl_state.second[0]
规划起点的横向位置l

if (fabs(obs_l - mid_l) < 0.4 &&
    fabs(path_boundary_s - init_sl_state.first[0]) < 5.0)

障碍物在当前路径边界的中点±0.4m
last_max_nudge_l
获取nudge_check_count个点中的center_l绝对值最大值

 if (init_sl_state.second[0] < obs_l) {
   sl_polygon->at(j).SetNudgeInfo(SLPolygon::RIGHT_NUDGE);
   AINFO << sl_polygon->at(j).id()
         << " right nudge with init_sl_state";
 } else {
   sl_polygon->at(j).SetNudgeInfo(SLPolygon::LEFT_NUDGE);
   AINFO << sl_polygon->at(j).id()
         << " left nudge width init_sl_state";
 }

如果自车在障碍物的右侧,则标记为向右绕行,如果自车在障碍物的左侧,则标记为向左绕行.

不符合条件
if (std::fabs(obs_l - mid_l) < 0.4 &&
std::fabs(path_boundary_s - init_sl_state.first[0]) < 5.0)时

else {
 if (last_max_nudge_l < obs_l) {
   sl_polygon->at(j).SetNudgeInfo(SLPolygon::RIGHT_NUDGE);
   AINFO << sl_polygon->at(j).id()
         << " right nudge, according max_nudge_l";
 } else {
   sl_polygon->at(j).SetNudgeInfo(SLPolygon::LEFT_NUDGE);
   AINFO << sl_polygon->at(j).id()
         << " left nudge, according max_nudge_l";
 }
}

(例如障碍物还比较远，或者不在路中心），程序就会执行这个 else.
此时，它不再看车辆现在在哪，而是看 last_max_nudge_l,它代表了车辆之前的“意图”或“趋势”.
向右绕行 (Right Nudge)：如果上一帧的最优路径横向位移(last_max_nudge_l）比障碍物的中心线(obs_l)更靠右,那么这一帧继续维持向右绕行.
向左绕行 (Left Nudge)：反之，如果之前的路径已经在左侧了，则继续维持向左绕行.

else {
     sl_polygon->at(j).SetNudgeInfo(SLPolygon::RIGHT_NUDGE);
     AINFO << sl_polygon->at(j).id()
           << " right nudge, left is not passable";
   }

当左侧不可通行时,就只能标记向右绕行

else {
          sl_polygon->at(j).SetNudgeInfo(SLPolygon::LEFT_NUDGE);
          AINFO << sl_polygon->at(j).id()
                << " left nudge, right is not passable";
        }

当右侧不可通行时,就只能标记向左绕行

if (sl_polygon->at(j).NudgeInfo() == SLPolygon::RIGHT_NUDGE) {
        // right nudge
        if (obs_right_nudge_bound.l_upper.l < path_boundary->at(i).towing_l) {
          sl_polygon->at(j).SetOverlapeWithReferCenter(true);
          sl_polygon->at(j).SetOverlapeSizeWithReference(
              path_boundary->at(i).towing_l - obs_right_nudge_bound.l_upper.l);
        }

obs_right_nudge_bound.l_upper.l:表示障碍物右侧可通行区域的左边界,把障碍物当成一个点插值出来的障碍物右边界l_lower值减去(自车半宽加上缓冲区)就是障碍物实际的右侧可通行区域左边界l值.
path_boundary->at(i).towing_l:是理想的自车要行驶的l位置,这里一直是0,也就是参考线的位置.
因为此时已经决定向右绕行,说明障碍物在自车的左侧,当obs_right_nudge_bound.l_upper.l < path_boundary->at(i).towing_l时,说明障碍物压上了参考线,为什么单凭obs_right_nudge_bound.l_upper.l < path_boundary->at(i).towing_l时就可以说明障碍物压线了,因为前提已经决定向右绕行,如果整个障碍物在参考线的右侧,那自车肯定要向左绕行,如果选择向右绕行说明障碍物并未完全在参考线右侧.说明障碍物的左边缘在参考线左侧,obs_right_nudge_bound.l_upper.l < path_boundary->at(i).towing_l表示障碍物的右边缘在参考线右侧.
将此障碍物标记为Overlap,并计算偏移的横向距离
标记 Overlap 是为了告诉后续的优化器：自车需要往右偏绕过障碍物

 if (!sl_polygon->at(j).is_passable()[RIGHT_INDEX]) {
   // boundary is blocked
   *blocked_id = sl_polygon->at(j).id();
   AINFO << "blocked at " << *blocked_id << ", s: " << path_boundary_s
         << ", left bound: " << left_bound.l
         << ", right bound: " << right_bound.l;
   sl_polygon->at(j).SetNudgeInfo(SLPolygon::BLOCKED);
   break;
 }

通行检查,决策是向右绕行,但这里会做最后的“物理检查”
如果该障碍物的右侧(RIGHT_INDEX)在地图或静态环境里其实无法行驶
直接判定为BLOCKED(路径阻断)这意味着“右绕”方案不可行
通行性是在上面UpdatePassableInfo函数更新的

if (obs_right_nudge_bound.l_upper.l < left_bound.l) {
          AINFO << "update left_bound: s " << path_boundary_s << ", l "
                << left_bound.l << " -> " << obs_right_nudge_bound.l_upper.l;
          left_bound.l = obs_right_nudge_bound.l_upper.l;
          left_bound.type = BoundType::OBSTACLE;
          left_bound.id = sl_polygon->at(j).id();
          *narrowest_width =
              std::min(*narrowest_width, left_bound.l - right_bound.l);
        }

left_bound.l:路径边界的左边界
obs_right_nudge_bound.l_upper.l:障碍物右侧可通行区域的左边界
因为决策是向右绕行,如果obs_right_nudge_bound.l_upper.l < left_bound.l就需要将自车的左边界收缩为障碍物右侧可通行区域的左边界,并将边界类型标记为 OBSTACLE,并记录障碍物id.
left_bound.l - right_bound.l 计算出当前 s 点处，左右边界之间的净距离
通过 std::min 不断迭代，找到整条规划路径上最窄的宽度narrowest_width

else if (sl_polygon->at(j).NudgeInfo() == SLPolygon::LEFT_NUDGE) {
 // left nudge
 if (obs_left_nudge_bound.l_lower.l > path_boundary->at(i).towing_l) {
   sl_polygon->at(j).SetOverlapeWithReferCenter(true);
   sl_polygon->at(j).SetOverlapeSizeWithReference(
       obs_left_nudge_bound.l_lower.l - path_boundary->at(i).towing_l);
 }

obs_left_nudge_bound.l_lower.l:是障碍物左侧通行区间的右边界
因为此时已经决定向左绕行,说明障碍物在自车的右侧,当obs_left_nudge_bound.l_lower.l > path_boundary->at(i).towing_l时,说明障碍物压上了参考线.
将此障碍物标记为Overlap,并计算偏移的横向距离
标记 Overlap 是为了告诉后续的优化器：自车需要往左偏绕过障碍物

 if (!sl_polygon->at(j).is_passable()[LEFT_INDEX]) {
          // boundary is blocked
          *blocked_id = sl_polygon->at(j).id();
          AINFO << "blocked at " << *blocked_id << ", s: " << path_boundary_s
                << ", left bound: " << left_bound.l
                << ", right bound: " << right_bound.l;
          sl_polygon->at(j).SetNudgeInfo(SLPolygon::BLOCKED);
          break;
        }

通行检查,决策是向左绕行,但这里会做最后的“物理检查”
如果该障碍物的左侧(LEFT_INDEX)在地图或静态环境里其实无法行驶
直接判定为BLOCKED(路径阻断)这意味着“左绕”方案不可行

if (obs_left_nudge_bound.l_lower.l > right_bound.l) {
          AINFO << "update right_bound: s " << path_boundary_s << ", l "
                << right_bound.l << " -> " << obs_left_nudge_bound.l_lower.l;
          right_bound.l = obs_left_nudge_bound.l_lower.l;
          right_bound.type = BoundType::OBSTACLE;
          right_bound.id = sl_polygon->at(j).id();
          *narrowest_width =
              std::min(*narrowest_width, left_bound.l - right_bound.l);
        }

因为决策是向左绕行,如果obs_left_nudge_bound.l_lower.l > right_bound.l就需要将自车的右边界收缩为障碍物左侧可通行区域的右边界,并将边界类型标记为 OBSTACLE,并记录障碍物id.
left_bound.l - right_bound.l 计算出当前 s 点处，左右边界之间的净距离
通过 std::min 不断迭代，找到整条规划路径上最窄的宽度narrowest_width

last_max_nudge_l = std::fabs((left_bound.l + right_bound.l) / 2.0 -
                    mid_l) > std::fabs(last_max_nudge_l - mid_l)
              ? (left_bound.l + right_bound.l) / 2.0
              : last_max_nudge_l;

在当前的纵向位置（s 点），更新并记录整条路径中“偏离中心线最远”的那个横向位置

    if (!blocked_id->empty()) {
      TrimPathBounds(i, path_boundary);
      *narrowest_width = default_width;
      return false;
    }

如果有无法通行的位置,,就会把这个路径边界点及之后的路径边界点都裁剪掉

center_l.push_back((left_bound.l + right_bound.l) / 2.0);

更新当前路径边界的center_l

AddExtraPathBound函数

RelaxEgoPathBoundary函数

 double min_radius =
     std::max(veh_param.min_turn_radius(),
              std::tan(veh_param.max_steer_angle() / veh_param.steer_ratio()) /
                  veh_param.wheel_base());

max_steer_angle:方向盘的最大转角(弧度)
steer_ratio:转向传动比
veh_param.max_steer_angle() / veh_param.steer_ratio():前轮偏转的最大角度
在阿克曼转向几何中,车辆的曲率\kappa与前轮偏转角的关系:
\delta:前轮偏转角
L:轴距
\kappa = \frac{\tan \delta }{L}
转弯半径
R = \frac{1}{\kappa}
这块有一个问题就是代码是\frac{tan \delta}{L}这是曲率而不是转弯半径,或许最小转弯半径只想使用配置中的veh_param.min_turn_radius()

double init_frenet_kappa =
  init_sl_state.second[2] /
  std::pow(1 + std::pow(init_sl_state.second[1], 2), 1.5);

second[1]是{l}'
second[2]是{l}''
曲线曲率公式:
\kappa=\frac{{f}''(x)}{(1+{f}'(x)^{2})^{\frac{3}{2} }}
它算出的是当前瞬间,车辆相对于参考线的弯曲程度

if (init_frenet_kappa < 0) {
    init_frenet_kappa = std::min(
        -1.0 / (min_radius + FLAGS_relax_ego_radius_buffer), init_frenet_kappa);
  } else {
    init_frenet_kappa = std::max(
        1.0 / (min_radius + FLAGS_relax_ego_radius_buffer), init_frenet_kappa);
  }

右转时(init_frenet_kappa < 0)
-1.0 / (min_radius + FLAGS_relax_ego_radius_buffer):通过配置中的转弯半径计算的曲率
无论是左转还是右转,计算车辆当前真实的转弯程度(曲率），并强制将其“限制”在车辆物理上能达到的最大极限范围内

for (size_t i = 1; i < path_boundary->size(); ++i) {
    auto& left_bound = path_boundary->at(i).l_upper;
    auto& right_bound = path_boundary->at(i).l_lower;
    double delta_s = path_boundary->at(i).s - init_pt.s;
    if (delta_s > FLAGS_relax_path_s_threshold) {
      left_calculate_success = false;
      right_calculate_success = false;
      break;
    }

只对自车前方FLAGS_relax_path_s_threshold(默认5m)范围的路径边界进行调整

    if (left_calculate_success &&
        (left_bound.type == BoundType::OBSTACLE ||
         path_boundary->at(i).is_nudge_bound[LEFT_INDEX])) {
      left_calculate_success = RelaxBoundaryPoint(
          &path_boundary->at(i), true, init_pt_l, init_frenet_heading, delta_s,
          init_frenet_kappa, min_radius);
    }

这段代码判断是否需要对左边界进行松弛：
BoundType::OBSTACLE: 如果左侧边界是由障碍物(如路边停靠的车辆)限制的
is_nudge_bound: 如果该点是因为避让(Nudge)障碍物而生成的边界

RelaxBoundaryPoint函数

bool PathBoundsDeciderUtil::RelaxBoundaryPoint(
    PathBoundPoint* const path_bound_point, bool is_left, double init_l,
    double heading, double delta_s, double init_frenet_kappa,
    double min_radius)

path_bound_point:当前遍历到的路径边界点
is_left:默认是true
init_l:车辆当前的初始横向位置
heading:是自车车头方向与自车在参考线上的投影点切线方向上的夹角

delta_s:当前位置边界点与第一个边界点纵向距离
init_frenet_kappa:车辆相对于参考线的曲率
min_radius:最小转弯半径

if (is_left) {
    if (init_frenet_kappa > 0 && heading < 0) {
      is_success = util::left_arc_bound_with_heading_with_reverse_kappa(
          delta_s, min_radius, heading, init_frenet_kappa,
          &protective_restrict);
    } else {
      is_success = util::left_arc_bound_with_heading(delta_s, radius, heading,
                                                     &protective_restrict);
    }

    relax_constraint =
        std::max(path_bound_point->l_upper.l, init_l + protective_restrict);
    AINFO << "init_pt_l: " << init_l
          << ", left_bound: " << path_bound_point->l_upper.l
          << ",  diff s: " << delta_s << ", radius: " << radius
          << ", protective_restrict: " << protective_restrict
          << ", left_obs_constraint: " << relax_constraint;

    if (path_bound_point->is_nudge_bound[LEFT_INDEX]) {
      old_buffer = std::max(FLAGS_obstacle_lat_buffer,
                            FLAGS_static_obstacle_nudge_l_buffer);
    }

    relax_constraint =
        std::min(path_bound_point->l_upper.l + old_buffer - new_buffer,
                 relax_constraint);
    AINFO << "left_obs_constraint: " << relax_constraint;
    path_bound_point->l_upper.l = relax_constraint;
  }

当前车辆正向左转(kappa > 0），但车头已经朝向右侧(heading < 0）

left_arc_bound_with_heading_with_reverse_kappa函数

bool left_arc_bound_with_heading_with_reverse_kappa(double delta_x, double r,
                                                    double heading,
                                                    double kappa,
                                                    double* result) {
  if (heading > 0 || kappa < 0 ||
      delta_x > r * (1.0 - std::sin(heading)) - 1e-6) {
    *result = std::numeric_limits<double>::lowest();
    return false;
  }
  if (delta_x < -r * std::sin(heading)) {
    *result = r * std::cos(heading) -
              std::sqrt(r * r - std::pow(delta_x - r * std::sin(heading), 2));
  } else {
    *result = std::sqrt(r * r - std::pow(delta_x + r * std::sin(heading), 2)) -
              r * (2 - std::cos(heading));
  }
  return true;
}

当车辆最终是左转(kappa > 0），但由于某种原因(比如之前的避障动作)车头已经向右撇(heading < 0)时，计算车身向左偏移的最大极限距离, 车身不可以超过这个最大偏移.
delta_x:当前位置边界点与第一个边界点纵向距离
r:最小转弯半径
heading:是自车车头方向与自车在参考线上的投影点切线方向上的夹角
init_frenet_kappa:车辆相对于参考线的曲率

这个函数主要是用了两个圆来进行自车左边界的限定,当前自车在圆心.当delta_x小于-rsin\theta时使用红色圆弧作为限定边界,否则使用绿色圆弧作为限定边界.

left_arc_bound_with_heading函数

如果此时自车最终是右转或者此时车头朝向是左转,则使用left_arc_bound_with_heading函数获取限定边界

bool left_arc_bound_with_heading(double delta_x, double r, double heading,
                                 double* result) {
  if (delta_x > r * (1.0 + std::sin(heading)) - 1e-6) {
    *result = std::numeric_limits<double>::lowest();
    return false;
  }

  *result = std::sqrt(r * r - std::pow(delta_x - r * std::sin(heading), 2)) -
            r * std::cos(heading);
  return true;
}

这个函数主要通过上面的圆来限定左边界,如果自车行驶的纵向距离超出了此圆的范围,则返回false,否则通过圆的方程来获取横向最大偏移.

relax_constraint =
        std::max(path_bound_point->l_upper.l, init_l + protective_restrict);

代码通过 std::max 在两个候选边界值中选择更宽松（L 值更大）的那一个：
候选 A(物理硬约束）：path_bound_point->l_upper.l这是基于地图（如路缘、车道线)和感知到的障碍物计算出的理想最大左边界.
候选 B(状态自适应约束）：init_l + protective_restrict这是以车辆当前实际位置（init_l)为基准，向左强制预留的一个最小生存空间(protective_restrict 缓冲区).

relax_constraint =
        std::min(path_bound_point->l_upper.l + old_buffer - new_buffer,
                 relax_constraint);
path_bound_point->l_upper.l = relax_constraint;

还原障碍物边缘位置： path_bound_point->l_upper.l 是已经扣除过 old_buffer (0.4m)的边界.那么l_upper.l + old_buffer 就回到了障碍物真实的物理边缘位置.
应用新的缓冲区：从障碍物边缘再减去更宽的 new_buffer(max(0.25, 0.4)).
最后,获取一个更小空间,作为左边界.
上面是松弛左边界点的逻辑,松弛右边界与其相似就不详细介绍了.到这RelaxBoundaryPoint函数就介绍完了
到这GetBoundaryFromStaticObstacles函数也介绍完了.

// 4. Append some extra path bound points to avoid zero-length path data.
int counter = 0;
while (!blocking_obstacle_id.empty() &&
       path_bound.size() < temp_path_bound.size() &&
       counter < FLAGS_num_extra_tail_bound_point) {
  path_bound.push_back(temp_path_bound[path_bound.size()]);
  counter++;
}

如果前方有障碍物彻底堵死了道路,计算出的安全边界 path_bound 可能会在障碍物处截断，导致长度极短甚至为零.
上面逻辑的含义是即便被堵住了，也会从原始的、未考虑该障碍物的候选边界 temp_path_bound 中强行取几个点(数量由FLAGS_num_extra_tail_bound_point 决定(默认是20))补在后面.

if (!blocking_obstacle_id.empty()) {
    double current_time = ::apollo::cyber::Clock::NowInSeconds();
    lane_follow_status->set_block_obstacle_id(blocking_obstacle_id);
    if (lane_follow_status->lane_follow_block()) {//上一帧自车已经被障碍物阻塞了
      lane_follow_status->set_block_duration(
          lane_follow_status->block_duration() + current_time -
          lane_follow_status->last_block_timestamp());
    } else {//当前帧自车被障碍物阻塞
      lane_follow_status->set_block_duration(0);
      lane_follow_status->set_lane_follow_block(true);
    }
    lane_follow_status->set_last_block_timestamp(current_time);
  }

lane_follow_status->lane_follow_block()为true:表示自车在上一帧就被障碍物阻塞了
lane_follow_status->set_block_duration(
lane_follow_status->block_duration() + current_time -
lane_follow_status->last_block_timestamp()):计算阻塞了多久

} else { // 障碍物消失或路通了
    if (lane_follow_status->lane_follow_block()) {
        lane_follow_status->set_block_duration(0); // 清零时长
        lane_follow_status->set_lane_follow_block(false);
    }
}