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

0 专栏介绍

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

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

---

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

---

1 $N$圆覆盖碰撞模型

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

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

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

$$\{\begin{array}{l}{x}_i^{\mathrm{d}\mathrm{i}\mathrm{s}\mathrm{c}}=x+\left(\frac{2i-1}{2N}\cdot \left(L_1+L_2\right)-L_1\right)\cos \theta \\ y_i^{\mathrm{d}\mathrm{i}\mathrm{s}\mathrm{c}}=y+\left(\frac{2i-1}{2N}\cdot \left(L_1+L_2\right)-L_1\right)\sin \theta \end{array}i=1,2,\cdots ,N$$

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

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

2 圆与矩形的碰撞检测

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

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

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

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

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

推广到一般情形，设矩形旋转角度为$\alpha$，则只需要将$\mathbf{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 quadrantfloat 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 rectanglefloat h_x = std::fabs(other_rect->length()) / 2.0f;float h_y = std::fabs(other_rect->width()) / 2.0f;// closest vectorfloat 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;
  

3.2 仿真实验

通过Rviz->Add New Tool添加Polygon Simulation插件

开启碰撞检测功能后，验证$N$圆覆盖碰撞检测算法

• 单圆碰撞与无碰撞情形

• $N$圆覆盖模型与圆的碰撞检测

• $N$圆覆盖模型与矩形的碰撞检测

完整工程代码请联系下方博主名片获取

---

🔥 更多精彩专栏：

• 《ROS从入门到精通》
• 《Pytorch深度学习实战》
• 《机器学习强基计划》
• 《运动规划实战精讲》
• …

👇源码获取 · 技术交流 · 抱团学习 · 咨询分享 请联系👇