planning模块-动态障碍物碰撞检测

planning模块-静态障碍物碰撞检测上一篇介绍了静态障碍物碰撞检测的逻辑,接下来介绍动态障碍物的碰撞检测逻辑,动态障碍物需要考虑动态障碍物的预测轨迹.

BuildTrajectoryStBoundary函数

bool Obstacle::BuildTrajectoryStBoundary(const ReferenceLine& reference_line,
                                         const double adc_start_s,
                                         STBoundary* const st_boundary)

reference_line:参考线
adc_start_s:自车AABB矩形框,后端的s值
st_boundary:出参,要获取的动态障碍物s-t边界

for (int i = 1; i < trajectory_points.size(); ++i) {
    ADEBUG << "last_sl_boundary: " << last_sl_boundary.ShortDebugString();

    const auto& first_traj_point = trajectory_points[i - 1];
    const auto& second_traj_point = trajectory_points[i];
    const auto& first_point = first_traj_point.path_point();
    const auto& second_point = second_traj_point.path_point();

    double object_moving_box_length =
        object_length + common::util::DistanceXY(first_point, second_point);

因为障碍物在t_{i-1} 到 t_i这一小段时间内是移动的，为了保证安全，代码将这段位移连同障碍物本身的长度“拉伸”成一个更大的 moving box
确保在两个离散预测点之间的时间里，障碍物扫过的所有空间都被考虑在内

common::math::Vec2d center((first_point.x() + second_point.x()) / 2.0,
                               (first_point.y() + second_point.y()) / 2.0);
    common::math::Box2d object_moving_box(
        center, first_point.theta(), object_moving_box_length, object_width);

first_point/second_point 是障碍物在时间段 [t_i-1, t_i] 内的运动,用中点当box中心,object_moving_box_length为box的长度,object_width为box的宽度.

const double distance_xy =
        common::util::DistanceXY(trajectory_points[last_index].path_point(),
                                 trajectory_points[i].path_point());

计算当前轨迹点(索引为 i)与上一个参考轨迹点(索引为 last_index)之间的欧几里得距离(直角坐标系下的直线距离).

if (last_sl_boundary.start_l() > distance_xy || 
    last_sl_boundary.end_l() < -distance_xy) {
  continue;
}

last_sl_boundary是障碍物在SL坐标系下的边界.l表示横向偏移(相对于中心线的左右距离）

    if (last_sl_boundary.start_l() > distance_xy ||
        last_sl_boundary.end_l() < -distance_xy) {
      continue;
    }

如果这个障碍物预测轨迹点离我们的参考线非常远(远于 distance_xy),说明它在这一瞬间根本不可能“横跨”进我们的车道.

const double mid_s =
        (last_sl_boundary.start_s() + last_sl_boundary.end_s()) / 2.0;
    const double start_s = std::fmax(0.0, mid_s - 2.0 * distance_xy);
    const double end_s = (i == 1) ? reference_line.Length()
                                  : std::fmin(reference_line.Length(),
                                              mid_s + 2.0 * distance_xy);

这段代码的目的是在纵向(S轴)上划定一个"搜索缓冲区"
简单来说,当系统发现障碍物在横向上(L轴)有威胁时,它需要确定:"在我的路线上，从哪儿到哪儿需要避开这个障碍物?"

mid_s:计算障碍物box中点在参考线上的纵向距离
确定开始位置 (start_s):从中心点往回(向车辆后方)倒退2倍的移动距离
确定结束位置 (end_s):从中心点往前(向车辆前方)延伸2倍的移动距离

GetApproximateSLBoundary函数

box:障碍物在笛卡尔坐标系下的box
start/end_s:搜索区间
GetProjectionWithHueristicParams函数是根据搜索区间计算box几何中心点基于参考线的横纵向距离.如果获取横纵向距离的方法忘记了,回头再复习一下planning模块(1)-参考线的生成

  auto projected_point = map_path_.GetSmoothPoint(s);
  auto rotated_box = box;
  rotated_box.RotateFromCenter(-projected_point.heading());

上面逻辑可以理解为是将笛卡尔坐标系的x轴旋转到box中心点在参考线上匹配到的参考点的切线方向,这样{x}'轴与frenet坐标系下的s轴平行,{y}'轴与frenet坐标下的l平行.

  std::vector<common::math::Vec2d> corners;
  rotated_box.GetAllCorners(&corners);

获取{x}',{y}'坐标系下的角点坐标

  for (const auto& point : corners) {
    // x <--> s, y <--> l
    // Because the box is rotated to align the reference line
    min_s = std::fmin(min_s, point.x() - rotated_box.center().x() + s);
    max_s = std::fmax(max_s, point.x() - rotated_box.center().x() + s);
    min_l = std::fmin(min_l, point.y() - rotated_box.center().y() + l);
    max_l = std::fmax(max_l, point.y() - rotated_box.center().y() + l);
  }

旋转后{x}'轴与frenet坐标系下的s轴平行,{y}'轴与frenet坐标下的l平行,计算角点与box中心点坐标在{x}',{y}'坐标系下的偏移量,然后计算每个角点的frennet坐标.

  sl_boundary->set_start_s(min_s);
  sl_boundary->set_end_s(max_s);
  sl_boundary->set_start_l(min_l);
  sl_boundary->set_end_l(max_l);

计算出障碍物在SL坐标系下的边界 object_boundary.这包括了障碍物在参考线方向上的起始点start_s、终点 end_s以及侧向的start_l和end_l.

static constexpr double kSkipLDistanceFactor = 0.4;
const double skip_l_distance =
    (object_boundary.end_s() - object_boundary.start_s()) *
        kSkipLDistanceFactor +
    adc_width / 2.0;

这一步是计算一个动态的横向安全边界
adc_width / 2.0：本车（ADC）中心到侧边的距离
(end_s - start_s) * 0.4：根据障碍物的长度增加一个冗余偏移量。障碍物越长，给予的横向余量越大
skip_l_distance：最终得到的总阈值.如果障碍物的横向距离超过这个值，说明它离参考线足够远，本车可以安全通过
本车半宽 (adc\_width / 2.0)这是最基本的物理边界.SL 坐标系的 L=0 是参考线.如果障碍物的 L 值小于本车半宽，说明它在物理上已经侵入了本车的车身投影范围
((end\_s - start\_s) \times 0.4):它利用了障碍物在 S 方向（纵向）的长度来增加横向的容错空间.物理直觉：物体越长（例如大卡车或长围栏），本车在经过它时，侧向碰撞的风险和压迫感就越大.计算逻辑：(end\_s - start\_s) 代表障碍物沿参考线方向的投影长度。乘以系数 0.4 后，作为横向的额外“安全垫”。目的：长形障碍物在转弯或由于感知误差波动时，更容易发生边界重叠。通过增加这个偏移量，可以更稳健地识别潜在风险.

if (!IsCautionLevelObstacle() &&
    (std::fmin(object_boundary.start_l(), object_boundary.end_l()) >
         skip_l_distance ||
     std::fmax(object_boundary.start_l(), object_boundary.end_l()) <
         -skip_l_distance)) {
  continue;
}

!IsCautionLevelObstacle()：首先确认该障碍物不是“caution级”障碍物（即风险较低）
横向位置判断：
左侧跳过：如果障碍物的最小值 fmin(start_l, end_l) 都大于 skip_l_distance，说明整个障碍物都在参考线左侧很远的地方
右侧跳过：如果障碍物最大值 fmax(start_l, end_l) 都小于 -skip_l_distance，说明整个障碍物都在参考线右侧很远的地方
continue：如果满足上述任一条件，程序认为该障碍物不会阻碍本车行驶，直接跳过，不参与后续的碰撞预测或减速逻辑
这段代码的作用是减少计算开销并防止误触发报警。通过计算障碍物在 SL 坐标系下的分布，剔除掉那些虽然在附近、但在横向上离本车行驶路径足够远的障碍物（例如马路牙子外的行人或隔壁车道的车辆）


    if (!IsCautionLevelObstacle() && object_boundary.end_s() < 0) {
      // skip if behind reference line
      continue;
    }

忽略在参考线后方的非caution级别的动态障碍物

static constexpr double kSparseMappingS = 20.0;
const double st_boundary_delta_s =
    (std::fabs(object_boundary.start_s() - adc_start_s) > kSparseMappingS)
        ? kStBoundarySparseDeltaS
        : kStBoundaryDeltaS;

根据障碍物与本车(adc)的距离来决定采样精度

    const double object_s_diff =
        object_boundary.end_s() - object_boundary.start_s();
    if (object_s_diff < st_boundary_delta_s) {
      continue;
    }

如果障碍物的投影长度 object_s_diff 比当前的采样步长 st_boundary_delta_s 还要小，则直接跳过该障碍物

double low_s = std::max(object_boundary.start_s() - adc_half_length, 0.0);

当自车车头碰到障碍物尾部时,自车中心点位置

double high_s =
        std::min(object_boundary.end_s() + adc_half_length, FLAGS_st_max_s);

当自车车尾离开障碍物头部时,自车中心点位置

while (low_s + st_boundary_delta_s < high_s && !(has_low && has_high)) {
      if (!has_low) {
        auto low_ref = reference_line.GetReferencePoint(low_s);
        has_low = object_moving_box.HasOverlap(
            {low_ref, low_ref.heading(), adc_length,
             adc_width + FLAGS_nonstatic_obstacle_nudge_l_buffer});
        low_s += st_boundary_delta_s;
      }

它不是只看一眼，而是从障碍物的 s 轴两端向中间逼近，通过 HasOverlap 函数不断检查：“在 s 这个位置，自车如果在这，会和障碍物撞上吗？”碰撞检测：它模拟了自车在不同 s 位置的 Box

HasOverlap函数

has_low = object_moving_box.HasOverlap(
            {low_ref, low_ref.heading(), adc_length,
             adc_width + FLAGS_nonstatic_obstacle_nudge_l_buffer});

参数是自车box

bool Box2d::HasOverlap(const Box2d &box) const {
  if (box.max_x() < min_x() || box.min_x() > max_x() || box.max_y() < min_y() ||
      box.min_y() > max_y()) {
    return false;
  }

说明障碍物box和自车box无交集

const double shift_x = box.center_x() - center_.x();
const double shift_y = box.center_y() - center_.y();

从"本车中心"指向"目标物体中心"的向量 \vec{L}

  const double dx1 = cos_heading_ * half_length_;
  const double dy1 = sin_heading_ * half_length_;
  const double dx2 = sin_heading_ * half_width_;
  const double dy2 = -cos_heading_ * half_width_;

A:障碍物box,B:自车box

  const double dx3 = box.cos_heading() * box.half_length();
  const double dy3 = box.sin_heading() * box.half_length();
  const double dx4 = box.sin_heading() * box.half_width();
  const double dy4 = -box.cos_heading() * box.half_width();

std::abs(shift_x * cos_heading_ + shift_y * sin_heading_)

障碍物box和自车box中点间距离在障碍物box方向角方向上的投影长度的绝对值

A:障碍物box,B:自车box
shift_x:蓝线
shift_y:红线
将蓝色和红线都平移到障碍物box中心点为起点的位置就容易看出来了

std::abs(dx3 * cos_heading_ + dy3 * sin_heading_)

自车box半车长在障碍物box方向角方向上的投影长度的绝对值
同样,把dx3,dy3平移到中心点为起点的位置就容易看出来了

std::abs(dx4 * cos_heading_ + dy4 * sin_heading_)

自车box半车宽在障碍物box方向角方向上的投影长度的绝对值
同样,把dx4,dy4平移到中心点为起点的位置就容易看出来了

std::abs(dx3 * cos_heading_ + dy3 * sin_heading_) +
                 std::abs(dx4 * cos_heading_ + dy4 * sin_heading_)

加到一起表示的就是,从自车box的中点开始自车在障碍物box方向角方向上的总投影长度

std::abs(dx3 * cos_heading_ + dy3 * sin_heading_) +
                 std::abs(dx4 * cos_heading_ + dy4 * sin_heading_) +
                 half_length_

表示的是当两个矩形在“障碍物纵轴方向”上恰好发生碰撞（接触）时，它们中心点之间允许的“最大纵向距离”临界值

std::abs(shift_x * cos_heading_ + shift_y * sin_heading_) <=
             std::abs(dx3 * cos_heading_ + dy3 * sin_heading_) +
                 std::abs(dx4 * cos_heading_ + dy4 * sin_heading_) +
                 half_length_

如果两个盒子的中心点在障碍物box纵轴上的投影距离大于这个值，它们就一定没撞上

std::abs(shift_x * sin_heading_ - shift_y * cos_heading_)

障碍物box和自车box中点间距离在障碍物box垂直于方向角方向上(横向)的投影长度的绝对值,从代码看是右侧为正

std::abs(dx3 * sin_heading_ - dy3 * cos_heading_) +
                 std::abs(dx4 * sin_heading_ - dy4 * cos_heading_)

自车box的长和宽在障碍物box横向方向上的总投影长度的绝对值

std::abs(dx3 * sin_heading_ - dy3 * cos_heading_) +
                 std::abs(dx4 * sin_heading_ - dy4 * cos_heading_) +
                 half_width_

表示的是两个矩形在“障碍物横轴（侧向）方向”上恰好发生碰撞（接触）时，它们中心点之间允许的“最大侧向距离”临界值

std::abs(shift_x * sin_heading_ - shift_y * cos_heading_) <=
             std::abs(dx3 * sin_heading_ - dy3 * cos_heading_) +
                 std::abs(dx4 * sin_heading_ - dy4 * cos_heading_) +
                 half_width_

如果两个盒子的中心点在障碍物box横轴上的投影距离大于这个值，它们就一定没撞上

std::abs(shift_x * box.cos_heading() + shift_y * box.sin_heading()) <=
             std::abs(dx1 * box.cos_heading() + dy1 * box.sin_heading()) +
                 std::abs(dx2 * box.cos_heading() + dy2 * box.sin_heading()) +
                 box.half_length()

同理,表示的是当两个矩形在“自车纵轴方向”上恰好发生碰撞（接触）时，它们中心点之间允许的“最大纵向距离”临界值
如果两个盒子的中心点在自车box纵轴上的投影距离大于这个值，它们就一定没撞上

std::abs(shift_x * box.sin_heading() - shift_y * box.cos_heading()) <=
             std::abs(dx1 * box.sin_heading() - dy1 * box.cos_heading()) +
                 std::abs(dx2 * box.sin_heading() - dy2 * box.cos_heading()) +
                 box.half_width()

同理,表示的是两个矩形在“自车横轴（侧向）方向”上恰好发生碰撞（接触）时，它们中心点之间允许的“最大侧向距离”临界值
如果两个盒子的中心点在自车box横轴上的投影距离大于这个值，它们就一定没撞上
上面的算法叫SAT分离轴定理:
SAT 的全称是 Separating Axis Theorem

序号	轴名称 (Axis)	判断内容 (Interpretation)	几何意义 (Geometry)	判定条件 (Condition)	说明
1	障碍物纵轴 (Obstacle Heading Axis)	检查自车在障碍物“车头方向”上是否靠得足够近	防止纵向追尾或正面碰撞（以障碍物为参考）	\lvert \text{中心点在障碍物纵轴上距离} \rvert \leq \text{障碍物半长} + \text{自车在该轴的综合投影半径}	评估在障碍物前进方向上的纵向重叠风险
2	障碍物横轴 (Obstacle Side Axis)	检查自车在障碍物“左右宽度”方向上是否靠得足够近	防止侧向剐蹭（以障碍物为参考）	\lvert \text{中心点在障碍物横轴上距离} \rvert \leq \text{障碍物半宽} + \text{自车在该轴的综合投影半径}	评估在障碍物宽度方向上的横向重叠风险
3	自车纵轴 (ADC Heading Axis)	从自车行驶方向看，障碍物是否挡在正前方	防止纵向碰撞（以自车为参考，考虑自车姿态）	\lvert \text{中心点在自车纵轴上的距离} \rvert \leq \text{自车半长} + \text{障碍物在该轴的综合投影半径}	换位思考，评估自车前进方向上的风险
4	自车横轴 (ADC Side Axis)	从自车侧面看，障碍物是否离车门过近	防止侧向剐蹭（以自车为参考，保护车角）	\lvert \text{中心点在自车横轴上的距离} \rvert \leq \text{自车半宽} + \text{障碍物在该轴的综合投影半径}	换位思考，评估自车侧面方向上的风险

边界扩张

low_s -= st_boundary_delta_s;
high_s += st_boundary_delta_s;

在原始的障碍物占据范围 low_s 和 high_s 基础上，分别向上下扩展一个增量 st_boundary_delta_s
给规划算法留出的“容错空间”。即使自车按照规划行驶，也会因为这个 Buffer 而与障碍物保持一定的纵向距离，而不是贴着障碍物走

double low_t =
          (first_traj_point.relative_time() +
           std::fabs((low_s - object_boundary.start_s()) / object_s_diff) *
               delta_t);

object_s_diff：是障碍物在这一个采样周期内移动的总 s 距离
(low_s - object_boundary.start_s()) / object_s_diff：计算的是“碰撞点距离起点”占“总移动距离”的比例.
delta_t:轨迹两个点之间的时间差
通过空间位移的比例关系，反推回时间的细分偏移量加上first_traj_point.relative_time()得到的是精确碰撞时间

polygon_points.emplace_back(
          std::make_pair(STPoint{low_s - adc_start_s, low_t},
                         STPoint{high_s - adc_start_s, low_t}));

在 low_t 时刻，记录障碍物占据的 s 区间. low_s - adc_start_s/ high_s - adc_start_s是为了将全局坐标转为相对于自车起点的相对坐标

double high_t =
          (first_traj_point.relative_time() +
           std::fabs((high_s - object_boundary.start_s()) / object_s_diff) *
               delta_t);
if (high_t - low_t > 0.05) {
        polygon_points.emplace_back(
            std::make_pair(STPoint{low_s - adc_start_s, high_t},
                           STPoint{high_s - adc_start_s, high_t}));
      }

如果说 low_t 算出的是障碍物“开始挡住你”的时间，那么 high_t 往往代表障碍物“离开”或“完成覆盖”某个特定位点的时间
如果计算出的两个边界时间点之间存在足够的时间差（大于 50ms），则在 ST 图中为该障碍物额外增加一对顶点

std::sort(polygon_points.begin(), polygon_points.end(),
              [](const std::pair<STPoint, STPoint>& a,
                 const std::pair<STPoint, STPoint>& b) {
                return a.first.t() < b.first.t();
              });

按时间对polygon_points进行排序

auto last = std::unique(polygon_points.begin(), polygon_points.end(),
                            [](const std::pair<STPoint, STPoint>& a,
                               const std::pair<STPoint, STPoint>& b) {
                              return std::fabs(a.first.t() - b.first.t()) <
                                     kStBoundaryDeltaT;
                            });
polygon_points.erase(last, polygon_points.end());

如果两个polygon_point时间相差很小则认为是重复polygon_point点,会通过unique把重复的点移动到后面,然后把重复的点都从polygon_points中删除掉
polygon_points的size要大于2保证存在一个碰撞区间
到这动态障碍物的碰撞检测就介绍完了.ReferenceLineInfo::AddObstacle也介绍完了这部分是planning模块-路径边界障碍物 ReferenceLineInfo::AddObstacle函数的收尾.