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

相关推荐
深蓝易网7 分钟前
为什么制造企业需要用MES管理系统升级改造车间
大数据·运维·人工智能·制造·devops
xiangzhihong815 分钟前
Amodal3R ,南洋理工推出的 3D 生成模型
人工智能·深度学习·计算机视觉
狂奔solar29 分钟前
diffusion-vas 提升遮挡区域的分割精度
人工智能·深度学习
uhakadotcom39 分钟前
Apache Airflow入门指南:数据管道的强大工具
算法·面试·github
资源大全免费分享42 分钟前
MacOS 的 AI Agent 新星,本地沙盒驱动,解锁 macOS 操作新体验!
人工智能·macos·策略模式
跳跳糖炒酸奶1 小时前
第四章、Isaacsim在GUI中构建机器人(2):组装一个简单的机器人
人工智能·python·算法·ubuntu·机器人
AI.NET 极客圈1 小时前
AI与.NET技术实操系列(四):使用 Semantic Kernel 和 DeepSeek 构建AI应用
人工智能·.net
Debroon1 小时前
应华为 AI 医疗军团之战,各方动态和反应
人工智能·华为
俊哥V1 小时前
阿里通义千问发布全模态开源大模型Qwen2.5-Omni-7B
人工智能·ai
绵绵细雨中的乡音1 小时前
动态规划-第六篇
算法·动态规划