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

相关推荐
Coovally AI模型快速验证4 分钟前
3D目标跟踪重磅突破!TrackAny3D实现「类别无关」统一建模,多项SOTA达成!
人工智能·yolo·机器学习·3d·目标跟踪·无人机·cocos2d
研梦非凡8 分钟前
CVPR 2025|基于粗略边界框监督的3D实例分割
人工智能·计算机网络·计算机视觉·3d
MiaoChuAI14 分钟前
秒出PPT vs 豆包AI PPT:实测哪款更好用?
人工智能·powerpoint
楼田莉子17 分钟前
C++算法专题学习——分治
数据结构·c++·学习·算法·leetcode·排序算法
一支鱼33 分钟前
leetcode常用解题方案总结
前端·算法·leetcode
fsnine38 分钟前
深度学习——残差神经网路
人工智能·深度学习
荼蘼1 小时前
迁移学习实战:基于 ResNet18 的食物分类
机器学习·分类·迁移学习
ulias2121 小时前
各种背包问题简述
数据结构·c++·算法·动态规划
m0_570466411 小时前
代码随想录算法训练营第二十八天 | 买卖股票的最佳实际、跳跃游戏、K次取反后最大化的数组和
java·开发语言·算法
和鲸社区1 小时前
《斯坦福CS336》作业1开源,从0手搓大模型|代码复现+免环境配置
人工智能·python·深度学习·计算机视觉·语言模型·自然语言处理·nlp