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

相关推荐
HPC_fac1305206781623 分钟前
以科学计算为切入点:剖析英伟达服务器过热难题
服务器·人工智能·深度学习·机器学习·计算机视觉·数据挖掘·gpu算力
网易独家音乐人Mike Zhou3 小时前
【卡尔曼滤波】数据预测Prediction观测器的理论推导及应用 C语言、Python实现(Kalman Filter)
c语言·python·单片机·物联网·算法·嵌入式·iot
小陈phd3 小时前
OpenCV从入门到精通实战(九)——基于dlib的疲劳监测 ear计算
人工智能·opencv·计算机视觉
Guofu_Liao4 小时前
大语言模型---LoRA简介;LoRA的优势;LoRA训练步骤;总结
人工智能·语言模型·自然语言处理·矩阵·llama
Swift社区7 小时前
LeetCode - #139 单词拆分
算法·leetcode·职场和发展
Kent_J_Truman7 小时前
greater<>() 、less<>()及运算符 < 重载在排序和堆中的使用
算法
IT 青年8 小时前
数据结构 (1)基本概念和术语
数据结构·算法
ZHOU_WUYI8 小时前
3.langchain中的prompt模板 (few shot examples in chat models)
人工智能·langchain·prompt
Dong雨8 小时前
力扣hot100-->栈/单调栈
算法·leetcode·职场和发展
如若1238 小时前
主要用于图像的颜色提取、替换以及区域修改
人工智能·opencv·计算机视觉