碰撞检测 | 详解圆-矩形碰撞检测与N圆覆盖模型(附ROS C++可视化)

news2024/9/24 23:29:40

0 专栏介绍

🔥课设、毕设、创新竞赛必备！🔥本专栏涉及更高阶的运动规划算法轨迹优化实战，包括：曲线生成、碰撞检测、安全走廊、优化建模(QP、SQP、NMPC、iLQR等)、轨迹优化(梯度法、曲线法等)，每个算法都包含代码实现加深理解

🚀详情：运动规划实战进阶：轨迹优化篇

本期实现如下的碰撞检测效果

在这里插入图片描述

1 $N$ 圆覆盖碰撞模型

在车辆的路径规划过程中，需要评估和避开可能的障碍物。 $N$ 圆覆盖碰撞检测算法可以快速检测和响应路径上的障碍物，从而优化行驶路线。

直观地，如图所示，采用单个外接圆包围物体，此时只需要检查圆心和半径的关系即可实现碰撞检测。然而这种方法容易造成自由空间狭窄，更精细的做法是利用个圆盘覆盖物体，对这些圆依次进行单圆碰撞检测，如图所示

在这里插入图片描述

形式化地，设自车后轴中心坐标为 $(x, y)$ ，由几何关系可知

$\begin{cases} x_{i}^{\mathrm{disc}}=x+\left( \frac{2i-1}{2N}\cdot \left( L_1+L_2 \right) -L_1 \right) \cos \theta\\ y_{i}^{\mathrm{disc}}=y+\left( \frac{2i-1}{2N}\cdot \left( L_1+L_2 \right) -L_1 \right) \sin \theta\\\end{cases}\,\,i=1,2,\cdots ,N$

其中 $\theta$ 是航向角； $N$ 是覆盖圆的数量， $N$ 越大碰撞检测越精细但同时计算负担更大。圆的半径由 $N$ 和自车几何形状唯一确定

$R^{\mathrm{disc}}=\sqrt{\left( \frac{L_1+L_2}{2N} \right) ^2+\left( \frac{W}{2} \right) ^2}$

2 圆与矩形的碰撞检测

如图所示，核心原理是计算圆心与矩形的最短距离 $\left| \boldsymbol{u} \right|$ ，若 $\left| \boldsymbol{u} \right|<r$ 则两者相交。算法上，首先考虑无旋转的矩形，不失一般性地将圆投影到第一象限，得到

$\boldsymbol{v}=\left[ \begin{matrix} \left| p_x-c_x \right|& \left| p_y-c_y \right|\\\end{matrix} \right] ^T$

其中 $\boldsymbol{p}$ 与 $\boldsymbol{c}$ 分别是矩形和圆的中心向量。设 $l^-$ 、 $w^-$ 分别为矩形长、宽的一半，则矩形中心到第一象限顶点向量为 $\boldsymbol{h}=\left[ \begin{matrix} l^-& w^-\\\end{matrix} \right]$ ，从而得到最近距离向量

$\boldsymbol{u}=\left[ \begin{matrix} \max \left( v_x-h_x, 0 \right)& \max \left( v_y-h_y, 0 \right)\\\end{matrix} \right] ^T$

即将负数分量设为0；再比较 $\left| \boldsymbol{u} \right|$ 和圆的半径大小关系即可

在这里插入图片描述

推广到一般情形，设矩形旋转角度为 $\alpha$ ，则只需要将 $\boldsymbol{v}$ 反向旋转 $\alpha$ 角度即可转换为无旋转的场景，算法流程如下所示

在这里插入图片描述

3 算法仿真与可视化

3.1 核心算法

核心算法如下所示

圆与矩形的碰撞检测

auto other_rect = std::dynamic_pointer_cast<VRectangle>(other);

for (const auto& disc : discs_)
{
  auto v = disc.first - other_rect->center();

  // rotate ang project first quadrant
  float theta = -other_rect->angle();
  float rotate_vx = std::fabs(v.x * std::cos(theta) - v.y * std::sin(theta));
  float rotate_vy = std::fabs(v.x * std::sin(theta) + v.y * std::cos(theta));

  // right-top point of rectangle
  float h_x = std::fabs(other_rect->length()) / 2.0f;
  float h_y = std::fabs(other_rect->width()) / 2.0f;

  // closest vector
  float u_x = std::max(0.0f, rotate_vx - h_x);
  float u_y = std::max(0.0f, rotate_vy - h_y);
  if (std::hypot(u_x, u_y) < disc.second)
    return true;
}
return false;

圆与圆的碰撞检测

auto other_circle = std::dynamic_pointer_cast<VCircle>(other);
const auto& other_circle_center = other_circle->center();
const auto& other_circle_radius = other_circle->radius();

for (const auto& disc : discs_)
{
  if (std::hypot(other_circle_center.x - disc.first.x, other_circle_center.y - disc.first.y) <=
      disc.second + other_circle_radius)
    return true;
}
return false;