Epsilon中碰撞检测算法

  1. 首先预测一段时间内自车与周围车辆的行驶轨迹,之后检测每一时刻自车与周边车辆的轨迹是否会发生碰撞,代码如下:

(1)首先将自车与周围车辆的长宽都加一个Offset值,代码中为0.1。

(2)基于当前的状态检查是否会发生碰撞

2.使用OBB算法检测两障碍物是否会发生碰撞,对自车与障碍物使用如下矩形box包络。

(1)车辆的坐标通常表示车辆后轴中心点处的坐标,首先需要计算自车与目标车质心的坐标,如下图所示。

代码如下:

2)分别计算自车与目标车四个角点的位置坐标,代码如下:

以自车右前方角点为例,如下图所示:

O为车辆质心的位置(x,y),计算C点的坐标为:(x+OD+BC,y+AD-AB)

OD = 0.5*obb.Length*cos_theta

BC = 0.5*obb.Width*sin_theta

AD = 0.5*obb.Length*sin_theta

AB = 0.5*obb.Width*cos_theta

其他角点的坐标算法同理。

(3)分别计算自车与目标车x轴,y轴方向的法向量

代码如下:

(4)分别计算自车与目标车的四个顶点在四条轴上的投影,如下图所示:

如上图所示,A,B分别为自车的四个顶点在自车在法向量上的投影点,C,D,E,F分别为目标车在法向量上的投影点,筛选出最小与最大的投影点,假设自车最小的投影点为B,最大的投影点为A,则目标车最小的投影点为F,最大的投影点为C,计算两障碍物最大投影点与最小投影点区间的重叠长度,若存在一侧重叠长度为0,则该目标不会发生碰撞,上图重叠长度为AF,所以该投影算出的结果会发生碰撞。

如上图所示,A,B分别为自车的四个顶点在自车在法向量上的投影点,C,D,E,F分别为目标车在法向量上的投影点,筛选出最小与最大的投影点,假设自车最小的投影点为B,最大的投影点为A,则目标车最小的投影点为F,最大的投影点为C,计算两障碍物最大投影点与最小投影点区间的重叠长度,上图AB与AF之间没有重叠,所以直接返回在该时刻两目标不会发生碰撞。

如上图所示,A,B,C,D分别为自车的四个顶点在自车在法向量上的投影点,E,F分别为目标车在法向量上的投影点,筛选出最小与最大的投影点,假设自车最小的投影点为D,最大的投影点为A,则目标车最小的投影点为F,最大的投影点为E,计算两障碍物最大投影点与最小投影点区间的重叠长度,上图AD与EF之间的重叠长度为ED,所以该投影算出的结果会发生碰撞。

如上图所示,A,B,C,D分别为自车的四个顶点在自车在法向量上的投影点,E,F分别为目标车在法向量上的投影点,筛选出最小与最大的投影点,假设自车最小的投影点为D,最大的投影点为A,则目标车最小的投影点为F,最大的投影点为E,计算两障碍物最大投影点与最小投影点区间的重叠长度,上图AD与EF之间没有重叠,所以直接返回在该时刻两目标不会发生碰撞。

(5)综上所述,该时刻两目标不会发生碰撞,代码如下:

相关推荐
qzhqbb2 分钟前
语言模型的采样方法
人工智能·语言模型·自然语言处理
qzhqbb4 分钟前
基于 Transformer 的语言模型
人工智能·语言模型·自然语言处理·transformer
___Dream5 分钟前
【CTFN】基于耦合翻译融合网络的多模态情感分析的层次学习
人工智能·深度学习·机器学习·transformer·人机交互
极客代码13 分钟前
【Python TensorFlow】入门到精通
开发语言·人工智能·python·深度学习·tensorflow
义小深15 分钟前
TensorFlow|咖啡豆识别
人工智能·python·tensorflow
passer__jw76720 分钟前
【LeetCode】【算法】283. 移动零
数据结构·算法·leetcode
Ocean☾26 分钟前
前端基础-html-注册界面
前端·算法·html
顶呱呱程序34 分钟前
2-143 基于matlab-GUI的脉冲响应不变法实现音频滤波功能
算法·matlab·音视频·matlab-gui·音频滤波·脉冲响应不变法
Tianyanxiao1 小时前
如何利用探商宝精准营销,抓住行业机遇——以AI技术与大数据推动企业信息精准筛选
大数据·人工智能·科技·数据分析·深度优先·零售
爱吃生蚝的于勒1 小时前
深入学习指针(5)!!!!!!!!!!!!!!!
c语言·开发语言·数据结构·学习·计算机网络·算法