碰撞检测-apollo

最新推荐文章于 2023-08-04 22:11:37 发布

henry.zhu51

最新推荐文章于 2023-08-04 22:11:37 发布

阅读量2.6k

点赞数 15

分类专栏：规划算法文章标签： apollo 碰撞检测

本文链接：https://blog.csdn.net/zghforever/article/details/110237199

版权

规划算法专栏收录该内容

5 篇文章 22 订阅

订阅专栏

概述

本文主要分析aplollo代码中关于障碍物碰撞检测相关算法，该算法通过将车辆抽象成二维矩形盒(Box)，建立点与矩形盒、线段与矩形盒、矩形盒与矩形盒的碰撞检测算法。

碰撞判断

点与矩形盒

关于点与矩形盒子的位置关系,可能情况为点在矩形盒内,点在矩形边框上和点在矩形盒外部,如果点在矩形盒的外部,可以求取点到矩形盒的距离值.

判定点是否在矩形盒内

如下图所示，红色圆点与矩形盒的关系如下图左侧，为了便于判定红点是否在矩形盒内,将下图左侧整体按照航向角顺时针旋转 $\theta$ 角度,得到下图右侧状态.此时，只需要判定红点的坐标 $(d x, d y)$ 是否满足 $\mid dx \mid <= length_h$ 或者 $\mid dy \mid <= width_h$ ，则说明红点位于矩形盒内。
point_in_box
其中,

$length_{half}$ :车长的一半;
$width_{half}$ : 车宽的一半;

判定点是否在矩形盒边界上

如下图所示,当红点位于矩形盒的边界上时,先以航向角顺时针旋转 $\theta$ 角度,然后通过判断旋转后的红点坐标 $(d x, d y)$ 是否满足 $\mid \mid dx \mid - length_h \mid == 0$ 或者 $\mid \mid dy \mid - width_h \mid == 0$ ,则说明点红点位于矩形盒边界上.

point_on_box_bound

其中,

$length_{half}$ :车长的一半;
$width_{half}$ : 车宽的一半;

计算点到矩形盒的距离

如下图所示,点到矩形盒的距离可以分为如下三种情况:

dx <= 0:如果红点在 $x$ 轴上的投影小于等于车长距离的一半,则此时的距离可以表示为 $\mid dy \mid$ ;
dy <= 0:如果条件1不满足,即dx > 0,则判断红点在 $y$ 轴上的投影是否小于等于车宽的一半,如果满足条件,则距离可以表示为 $\mid dx \mid$ ;
dx > 0 and dy > 0: 如果条件1和2都不满足,则距离可以表示为红点与矩形盒边角点的距离,即 $\sqrt{dx^2 + dy^2}$ ;

point_to_box_distance

线段与矩形盒

判定线段与矩形盒是否交叠

关于某线段与矩形盒关系的判定,先计算线段的长度,如果线段的长度为0,则可以把该线段当作一个点,利用点和矩形盒的距离来判断是否交叠;如果线段的长度不为0,则先进行线段与矩形盒的粗判定,即线段的起始点与终点是否落在矩形盒的一边,如下图所示,表示线段位于矩形盒一边的四种情况.如果线段不满足如下关系,则交叠关系可以通过距离来判断.

line_to_box_outside

计算线段到矩形盒的距离

区域划分

如下图所示,将矩形盒的外围分成如下8个区域,关于区域的划分,可以根据线段起始点和终点分别在 $x$ 轴和 $y$ 轴的投影,表示为 $p_x,p_y)$ ;以车辆中心 $o$ 为原点,则四个边角点可以表示为 $box_x,box_y)$ , $box_x,-box_y)$ , $box_x,box_y)$ , $box_x,-box_y)$ .

其中,根据点在 $x$ 轴的投影,可以将区域值表示如下:
$g_x=\begin{cases} 1 & \text{if } (p_x >= box_x),\\ 0 & \text{if } (-box_x < p_x < box_x),\\ -1 & \text{if } (p_x <= -box_x),\\ \end{cases}$

line_to_box_group

根据上图,区域的值表示如下:

区域	gx	gy
1	1	1
2	0	1
3	-1	1
4	-1	0
5	-1	-1
6	0	-1
7	1	-1
8	1	0

其中,区域 $(0, 0)$ 位于矩形盒内,即线段中有一点位于矩形盒内,必定与矩形盒交叠.因此去除点位于区域 $(0, 0)$ 的情形,考虑线段两个端点与矩形盒的关系共有 $\times 8 = 64$ 种情况.

对称变换

首先将起始点通过 $x$ 轴和 $y$ 轴对称变换,将落在区域3,4,5,6,7的点变换到区域1,2,8中.对于区域2和8,进行 $x y$ 轴对称变换,将区域2变换为区域8.所以经过三种轴对称转换后,起始点将会落在区域1和区域8两种区域状态.

区域1

当起始点位于区域1,则将终点进行 $x y$ 轴对称变换,将位于区域2,3,4的终点,对称变换到区域8,7,6中.因此关于终点的所在区域的情况只需考虑落在区域1,8,7,6,5,这五个区域的情况;

区域8

当起始点位于区域8,对终点进行 $y$ 轴对称变换,将位于 $y$ 轴下方的区域变换到 $y$ 轴上方,即区域5,6,7对称变换到区域3,2,1.因此终点只需考虑区域8,1,2,3,4五个区域的请况.

通过上述对称变换,可以将原先的64种线段与矩形的关系减少到10种,从而减少相关计算量.

线段与矩形盒距离计算

如下图所示,线段起始点与矩形盒边角点之间的向量表示为 $\vec{sb}$ ,起始点与终点之间的向量表示为 $\vec{sg}$ .

当 $\vec{sb}\cdot\vec{sg} <= 0$ 时,即下图浅绿色表示的区域,此时,线段与矩形盒的距离表示为 $\mid \vec{sb}\mid$ .

vector_cdot_less_zero

当 $\vec{sb}\cdot\vec{sg} >= \vec{sg} \cdot \vec{sg}$ ,即下图浅绿色区域,线段与矩形盒的距离可以表示为 $\mid \vec{sb}-\vec{sg}\mid$ .

vector_cdot_length_two

当上述条件都不满足时,即位于下图浅黄色区域,线段与矩形盒的距离可以表示为 $\mid \vec{sb} \mid \sin{\theta}$ ,可使用向量叉乘得到 $\frac{\vec{sb}\times\vec{sg}}{\mid \vec{sg} \mid}$ .

vector_times

起始点区域1

当终点位于区域1时,直接使用上述方法计算距离值;
当终点位于区域8时,先判定起始点与终点的 $x$ 轴坐标的关系,当起始点坐标的 $x$ 轴坐标大于终点的 $x$ 轴坐标,则距离值为 $g_x - b_x \mid$ ,否则按照上述方法计算距离值;
当终点位于区域7时,先判定起始点与终点的 $x$ 轴坐标的关系,选取不同的矩形盒的边角点;
当终点位于区域6时,先判定线段与矩形盒是否交叠,如下图所示,通过计算向量叉乘 $\vec{sg} \times \vec{sb}$ ,判断线段是否与矩形盒交叠.

vector_times_less_zero

当终点位于区域5时,矩形盒的边角点分两种,首先判断与右下角的边角点的关系,如果不交叠则直接按上述方法计算距离值,否则判断与左上角边角点的关系,如果无交叠则按上述方法计算距离值。

vector_times_two_bound_zero

起始点区域8

当终点位于区域1时,如下图所示,先判定起始点与终点的 $x$ 轴坐标的关系,如果起始点小于终点,则距离值为 $s_x - b_x \mid$ ,否则计算线段与矩形盒的距离;
当终点位于区域8时,选取起始点与终点中离矩形盒最近的点到矩形盒的距离,即 $min(s_x,g_x) -b_x \mid$ ;
当终点位于区域2和3时,如下图所示,通过右上边角点与线段的起始点,终点形成的矢量,进行矢量叉乘,通过数值的正负判断线段是否与矩形盒交叠;