目录

0 专栏介绍1 AABB碰撞检测2 OBB碰撞检测3 算法仿真与可视化

0 专栏介绍

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

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

矩形包围盒碰撞检测的核心原理是用矩形近似待检测物体轮廓，通过计算两个矩形间是否有重叠区域来进行碰撞判断。常用的包围形式有轴对齐包围盒(Axis-Aligned Bounding Box, AABB)以及更精细化具有角度的方向包围盒(Oriented Bounding Box)

本文的最终效果如下所示

1 AABB碰撞检测

如图所示为AABB碰撞检测的原理，其通过计算两个矩形在对齐的若干条轴上的投影来判碰，当且仅当所有轴的投影都发生重叠时两个AABB产生碰撞。

算法原理如表所示，其中矩形用左上角坐标和长宽来表示

按照算法流程实现核心算法

<code>bool VRectangle::_AABBCollisionDetection(const std::shared_ptr<VRectangle>& other)

{

return lt_pt_.x < other->left_top_point().x + std::fabs(other->length()) &&

other->left_top_point().x < lt_pt_.x + std::fabs(l_) &&

lt_pt_.y < other->left_top_point().y + std::fabs(other->width()) &&

other->left_top_point().y < lt_pt_.y + std::fabs(w_);

}

2 OBB碰撞检测

OBB碰撞检测的核心原理是应用分离轴定理，在矩形的长宽两个方向进行投影(共计4次)，判断投影是否重叠；只要存在一条投影轴不发生重叠，表明找到了至少一条分离轴，即两个OBB不发生碰撞

沿矩形长宽方向设置方向向量以及待投影的几何向量

{

k

1

=

[

cos

⁡

α

sin

⁡

α

]

T

k

2

=

[

sin

⁡

α

−

cos

⁡

α

]

T

k

3

=

[

cos

⁡

β

sin

⁡

β

]

T

k

4

=

[

sin

⁡

β

−

cos

⁡

β

]

T

,

{

v

1

=

l

1

−

k

1

v

2

=

w

1

−

k

2

v

3

=

l

2

−

k

3

v

4

=

w

2

−

k

4

\begin{cases} \boldsymbol{k}_1=\left[ \begin{matrix} \cos \alpha& \sin \alpha\\\end{matrix} \right] ^T\\ \boldsymbol{k}_2=\left[ \begin{matrix} \sin \alpha& -\cos \alpha\\\end{matrix} \right] ^T\\ \boldsymbol{k}_3=\left[ \begin{matrix} \cos \beta& \sin \beta\\\end{matrix} \right] ^T\\ \boldsymbol{k}_4=\left[ \begin{matrix} \sin \beta& -\cos \beta\\\end{matrix} \right] ^T\\\end{cases}, \begin{cases} \boldsymbol{v}_1=l_{1}^{-}\boldsymbol{k}_1\\ \boldsymbol{v}_2=w_{1}^{-}\boldsymbol{k}_2\\ \boldsymbol{v}_3=l_{2}^{-}\boldsymbol{k}_3\\ \boldsymbol{v}_4=w_{2}^{-}\boldsymbol{k}_4\\\end{cases}

⎩⎪⎪⎪⎪⎨⎪⎪⎪⎪⎧​k1​=[cosα​sinα​]Tk2​=[sinα​−cosα​]Tk3​=[cosβ​sinβ​]Tk4​=[sinβ​−cosβ​]T​,⎩⎪⎪⎪⎨⎪⎪⎪⎧​v1​=l1−​k1​v2​=w1−​k2​v3​=l2−​k3​v4​=w2−​k4​​

其中

l

i

−

l_{i}^{-}

li−​、

w

i

−

w_{i}^{-}

wi−​分别是矩形长、宽的一半

设置向量

d

\boldsymbol{d}

d连接两个矩形的中心，用于判断两个矩形的投影是否发生重叠，具体判据是

d

<

(

r

1

−

+

r

2

−

)

d<\left( r_{1}^{-}+r_{2}^{-} \right)

d<(r1−​+r2−​)

其中

d

d

d是向量

d

\boldsymbol{d}

d在给定投影轴的投影长度；

r

i

−

r_{i}^{-}

ri−​是矩形

i

i

i在给定投影轴的投影长度的一半

举个例子，对于下图所示的投影，计算

{

d

=

∣

d

T

k

1

∣

r

1

−

=

∣

v

1

T

k

1

∣

r

2

−

=

∣

v

3

T

k

1

∣

+

∣

v

4

T

k

1

∣

\begin{cases} d=\left| \boldsymbol{d}^T\boldsymbol{k}_1 \right|\\ r_{1}^{-}=\left| \boldsymbol{v}_{1}^{T}\boldsymbol{k}_1 \right|\\ r_{2}^{-}=\left| \boldsymbol{v}_{3}^{T}\boldsymbol{k}_1 \right|+\left| \boldsymbol{v}_{4}^{T}\boldsymbol{k}_1 \right|\\\end{cases}

⎩⎪⎪⎨⎪⎪⎧​d=∣∣∣​dTk1​∣∣∣​r1−​=∣∣​v1T​k1​∣∣​r2−​=∣∣​v3T​k1​∣∣​+∣∣​v4T​k1​∣∣​​

再进行对比即可

遍历矩形的四条边即可得到算法流程

核心代码如下所示

<code>bool VRectangle::_OBBCollisionDetection(const std::shared_ptr<VRectangle>& other)

{

const float shift_x = other->center().x - center_.x;

const float shift_y = other->center().y - center_.y;

const float half_length = 0.5f * std::fabs(l_);

const float half_width = 0.5f * std::fabs(w_);

const float other_half_length = 0.5f * std::fabs(other->length());

const float other_half_width = 0.5f * std::fabs(other->width());

const float dx1 = cos_angle_ * half_length;

const float dy1 = sin_angle_ * half_length;

const float dx2 = sin_angle_ * half_width;

const float dy2 = -cos_angle_ * half_width;

const float dx3 = other->cos_angle() * other_half_length;

const float dy3 = other->sin_angle() * other_half_length;

const float dx4 = other->sin_angle() * other_half_width;

const float dy4 = -other->cos_angle() * other_half_width;

return std::abs(shift_x * cos_angle_ + shift_y * sin_angle_) <= std::abs(dx3 * cos_angle_ + dy3 * sin_angle_) +

std::abs(dx4 * cos_angle_ + dy4 * sin_angle_) +

half_length &&

std::abs(shift_x * sin_angle_ - shift_y * cos_angle_) <= std::abs(dx3 * sin_angle_ - dy3 * cos_angle_) +

std::abs(dx4 * sin_angle_ - dy4 * cos_angle_) +

half_width &&

std::abs(shift_x * other->cos_angle() + shift_y * other->sin_angle()) <=

std::abs(dx1 * other->cos_angle() + dy1 * other->sin_angle()) +

std::abs(dx2 * other->cos_angle() + dy2 * other->sin_angle()) + other_half_length &&

std::abs(shift_x * other->sin_angle() - shift_y * other->cos_angle()) <=

std::abs(dx1 * other->sin_angle() - dy1 * other->cos_angle()) +

std::abs(dx2 * other->sin_angle() - dy2 * other->cos_angle()) + other_half_width;

}

3 算法仿真与可视化

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

开启碰撞检测功能后，验证AABB和OBB碰撞检测算法

AABB无碰撞情形

AABB有碰撞情形

OBB无碰撞情形

OBB有碰撞情形

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

🔥 更多精彩专栏：

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