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)综上所述,该时刻两目标不会发生碰撞,代码如下:

相关推荐
艾莉丝努力练剑6 分钟前
【LeetCode&数据结构】单链表的应用——反转链表问题、链表的中间节点问题详解
c语言·开发语言·数据结构·学习·算法·leetcode·链表
_殊途2 小时前
《Java HashMap底层原理全解析(源码+性能+面试)》
java·数据结构·算法
橡晟4 小时前
深度学习入门:让神经网络变得“深不可测“⚡(二)
人工智能·python·深度学习·机器学习·计算机视觉
墨尘游子4 小时前
神经网络的层与块
人工智能·python·深度学习·机器学习
Leah01054 小时前
什么是神经网络,常用的神经网络,如何训练一个神经网络
人工智能·深度学习·神经网络·ai
Leah01054 小时前
机器学习、深度学习、神经网络之间的关系
深度学习·神经网络·机器学习·ai
PyAIExplorer4 小时前
图像亮度调整的简单实现
人工智能·计算机视觉
珊瑚里的鱼5 小时前
LeetCode 692题解 | 前K个高频单词
开发语言·c++·算法·leetcode·职场和发展·学习方法
Striker_Eureka5 小时前
DiffDet4SAR——首次将扩散模型用于SAR图像目标检测,来自2024 GRSL(ESI高被引1%论文)
人工智能·目标检测
Rvelamen6 小时前
LLM-SECURITY-PROMPTS大模型提示词攻击测评基准
人工智能·python·安全