【自動運転】衝突検出アルゴリズム

参照リンク：

【自動運転】衝突検知アルゴリズム

[計画]Box2d:: HasOverlap（）衝突検出の詳細な説明interface_lemon_zyのブログ-CSDNblog_box2d衝突検出

一般的な方法は、超平面分離定理です。これは、N次元ユークリッド空間に凸集合の共通部分があるかどうかを検出するために使用されます。厳密な定義については、 Wikipediaを参照してください。その2次元の場合は、分離軸定理と呼ばれ、次のように簡単に説明されます。

2つの平面凸集合の非結合性の必要十分条件は、特定の線（つまり、分離軸）があり、その線上の2つの平面の射影が交差しないことであり、否定的な命題を書くことができます。同様に。明らかに、交差点を証明するには、すべての線を使い果たす必要がありますが、これは不可能です。幸い、凸多角形の場合、エッジが存在する線が分離線（分離軸ではない）であるかどうかを確認するだけで済みます。（凹多角形と円を実装する方法を考えることができます）

自律走行の範囲では、障害物は一般に平面の長方形で表され（平面の凸多角形がより高い精度で使用されます）、2つの長方形の場合、4つの側面（各長方形に隣接する2つの長方形）を横断するだけで済みます。 、4つの辺の1つが分離軸である限り、2つの長方形は交差しません。逆に、2つの長方形が交差する場合、4つの辺は分離軸ではありません（自我車両の投影長と分離軸上のターゲット輪郭の合計の半分と、投影距離のサイズを比較してください。投影距離はすべて投影よりも小さく、オーバーラップはhasOverLap ）です。つまり、投影距離はすべて一致します。

平らな長方形を効率的に表現するために、通常、次のように定義します。

struct Box2d {
  float center_x_;
  float center_y_;
  float length_;
  float width_;
  float half_length_;
  float half_width_;
  float heading_;
  float cos_heading_;
  float sin_heading_;
  float min_x_;
  float max_x_;
  float min_y_;
  float max_y_;
  struct Vec2d corners_[4];
};

投射原理：車両の隣接する2つの側面と障害物の隣接する2つの側面を投射します

 エゴビークルの投影長と分離軸上のターゲット輪郭の合計の半分、および投影距離のサイズを比較するだけで、2つの投影が交差するかどうかを判断できます。明らかに、分離軸上のエゴカーの投影長はlength_です。

次のコードは、対応する車両の隣接する2つの側面での投影計算です。

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_ &&
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_

 つまり、車両のどのポイントも、障害物に対応するオレンジ色の点線のボックス内にありません。

 

次のコードは、障害物に対応する2つの隣接するエッジでの投影計算です。

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() &&
         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()

 

Box2d::HasOverlap(const Box2d &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;
  }

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

  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_;
  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();

  return 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_ &&
         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_ &&
         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() &&
         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();
}