CN115837921A - 车辆轨迹碰撞检测方法、装置、设备及存储介质 - Google Patents
车辆轨迹碰撞检测方法、装置、设备及存储介质 Download PDFInfo
- Publication number
- CN115837921A CN115837921A CN202310154043.4A CN202310154043A CN115837921A CN 115837921 A CN115837921 A CN 115837921A CN 202310154043 A CN202310154043 A CN 202310154043A CN 115837921 A CN115837921 A CN 115837921A
- Authority
- CN
- China
- Prior art keywords
- vehicle
- track
- vehicle track
- target
- preset time
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Granted
Links
- 238000001514 detection method Methods 0.000 title claims abstract description 80
- 238000000034 method Methods 0.000 claims abstract description 46
- 238000004590 computer program Methods 0.000 claims description 15
- 238000004422 calculation algorithm Methods 0.000 claims description 11
- 230000008569 process Effects 0.000 abstract description 5
- 238000004364 calculation method Methods 0.000 description 22
- 238000010586 diagram Methods 0.000 description 13
- 230000006870 function Effects 0.000 description 4
- 238000005516 engineering process Methods 0.000 description 3
- 230000003068 static effect Effects 0.000 description 3
- 238000004891 communication Methods 0.000 description 2
- 230000007423 decrease Effects 0.000 description 2
- 230000009286 beneficial effect Effects 0.000 description 1
- 230000008859 change Effects 0.000 description 1
- 230000009977 dual effect Effects 0.000 description 1
- 230000000694 effects Effects 0.000 description 1
- 230000006872 improvement Effects 0.000 description 1
- 239000011159 matrix material Substances 0.000 description 1
- 238000012986 modification Methods 0.000 description 1
- 230000004048 modification Effects 0.000 description 1
- 230000003287 optical effect Effects 0.000 description 1
- 230000036544 posture Effects 0.000 description 1
- 238000010187 selection method Methods 0.000 description 1
- 239000007787 solid Substances 0.000 description 1
Images
Classifications
-
- Y—GENERAL TAGGING OF NEW TECHNOLOGICAL DEVELOPMENTS; GENERAL TAGGING OF CROSS-SECTIONAL TECHNOLOGIES SPANNING OVER SEVERAL SECTIONS OF THE IPC; TECHNICAL SUBJECTS COVERED BY FORMER USPC CROSS-REFERENCE ART COLLECTIONS [XRACs] AND DIGESTS
- Y02—TECHNOLOGIES OR APPLICATIONS FOR MITIGATION OR ADAPTATION AGAINST CLIMATE CHANGE
- Y02T—CLIMATE CHANGE MITIGATION TECHNOLOGIES RELATED TO TRANSPORTATION
- Y02T10/00—Road transport of goods or passengers
- Y02T10/10—Internal combustion engine [ICE] based vehicles
- Y02T10/40—Engine management systems
Landscapes
- Traffic Control Systems (AREA)
Abstract
本发明提供一种车辆轨迹碰撞检测方法、装置、设备及存储介质,方法包括:获取各个车辆轨迹;第一阶段,根据其他车辆轨迹与自车轨迹的OBB包围盒是否相交来排除一定不会发生碰撞的安全车辆轨迹;第二阶段,排除安全车辆轨迹后,获取其余车辆轨迹与自车轨迹在各相同时刻的距离,并根据同时刻距离与两车车身长宽的比较关系来检测出一定发生碰撞的轨迹以及一定不发生碰撞的轨迹;第三阶段,当第二阶段不能确定的可疑碰撞轨迹,采用具体车辆图形的方式来进行车辆轨迹碰撞检测。本发明通过首先采用精度相对不高但高效率的检测方法来快速减少计算量,再通过高精度的检测方法对剩余的轨迹进行碰撞检测,从而能够大大提高车辆轨迹碰撞检测整体过程的效率。
Description
技术领域
本发明涉及碰撞检测技术领域,尤其是涉及一种车辆轨迹碰撞检测方法、装置、设备及存储介质。
背景技术
随着技术的不断进步,越来越多的自动驾驶技术与产品开始服务于人们生活,特别是L2、L2++自动驾驶相关功能大幅量产,人们对高性价比的产品越来越青睐,那么使用更廉价算力更低的处理器有利于降低产品价格,但这对相关算法就提出了更高的要求,需要在保证功能的前提下,最大限度降低计算量。
轨迹规划是自动驾驶技术中的重要环节,通常轨迹规划算法需要输出一条光滑、舒适、无碰撞的轨迹,以作为控制模块控制车辆的输入。为生成一条无碰撞的运动轨迹,通常会用到碰撞检测算法,如果所规划的轨迹与静态障碍物或者动态障碍物发生碰撞,则规划的该条轨迹即为失败的不能够使用的轨迹。常用的碰撞检测算法包括栅格法,射线法,SAT法,GJK法,然而直接计算自车每个时刻的轨迹点与其他障碍物是否发生碰撞会耗费大量算力资源,碰撞检测的效率不高。因此亟需一种能够提高碰撞检测效率的方案。
发明内容
本发明旨在提供一种车辆轨迹碰撞检测方法、装置、设备及存储介质,以解决上述技术问题,从而能够提高车辆轨迹碰撞检测的效率。
为了解决上述技术问题,本发明提供了一种车辆轨迹碰撞检测方法,包括:
获取未来预设时段内的自车轨迹以及目标车辆轨迹;其中,所述目标车辆包括未来预设时段内距离自车预设范围内的其他所有车辆;
获取所有车辆轨迹的OBB包围盒,基于自车轨迹与目标车辆轨迹的OBB包围盒的相交性判断结果识别出安全车辆轨迹并排除;或,获取各条目标车辆轨迹与自车轨迹在未来预设时段内的最短距离,基于两车车身平均长度与对应的最短距离的比较关系识别出安全车辆轨迹并排除;其中,所述两车车身平均长度为基于自车车身长度与对应的目标车辆车身长度相加的和的一半进行确定得到;
对于排除安全车辆轨迹后的其余各条目标车辆轨迹,以预设时间间隔求取自车轨迹与目标车辆轨迹在相同时刻的距离;
若判断所有相同时刻的距离均大于两车车身平均长度,则确定该目标车辆轨迹与自车轨迹在未来预设时段内不存在碰撞情况;其中,所述两车车身平均长度为基于自车车身长度与对应的目标车辆车身长度相加的和的一半进行确定得到;
若判断存在至少一相同时刻的距离小于或等于两车车身平均宽度,则确定该目标车辆轨迹与自车轨迹在未来预设时段内存在碰撞情况;其中,所述两车车身平均宽度为基于自车车身宽度与对应的目标车辆车身宽度相加的和的一半进行确定得到;
若判断所有相同时刻的距离均大于两车车身平均宽度,且存在某一相同时刻的距离小于两车车身平均长度,则基于当前时刻的自车图形与目标车辆图形的相交性判断结果进行车辆轨迹碰撞检测。
进一步地,所述OBB包围盒的获取方法具体包括:
对于每一车辆轨迹,基于车辆中心坐标以及车辆yaw角确定车辆角点坐标,将该车辆轨迹对应的所有的车辆角点坐标作为输入数据计算该车辆轨迹对应的OBB包围盒。
进一步地,所述OBB包围盒的获取方法具体包括:
对于每一车辆轨迹,以车辆轨迹上的点作为圆心、以车身长度作为直径画圆形,在圆形上均匀取点作为关键点,将该车辆轨迹对应的所有的关键点作为输入数据计算该车辆轨迹对应的OBB包围盒。
进一步地,所述OBB包围盒的获取方法具体包括:
对于每一车辆轨迹,确定车辆轨迹的起始点、终点和极值点,并在车辆轨迹上的起始点、终点和极值点之间均匀插入若干个辅助点,以车身长度为直径在起始点、终点、极值点和辅助点处分别作车辆轨迹的垂直平分线段,并以各个垂直平分线段的顶点作为关键点,同时以车身长度的一半分别在起始点之前以及终点之后延伸出一个关键点,将该车辆轨迹对应的所有的关键点作为输入数据计算该车辆轨迹对应的OBB包围盒。
进一步地,所述基于自车轨迹与目标车辆轨迹的OBB包围盒的相交性判断结果识别出安全车辆轨迹并排除,具体包括:
利用SAT算法判断自车轨迹OBB包围盒与各条目标车辆轨迹的OBB包围盒是否相交,若不相交,则将该目标车辆轨迹识别为安全车辆轨迹并将其进行排除。
进一步地,所述获取各条目标车辆轨迹与自车轨迹在未来预设时段内的最短距离,基于两车车身平均长度与对应的最短距离的比较关系识别出安全车辆轨迹并排除,具体包括:
在自车轨迹上按预设的时间间隔进行取点,并分别获取各点到目标车辆轨迹的距离最短值,从所有距离最短值中选取最小值作为目标车辆轨迹与自车轨迹在未来预设时段内的最短距离;
若判断目标车辆轨迹与自车轨迹在未来预设时段内的最短距离大于两车车身平均长度,则将该目标车辆轨迹识别为安全车辆轨迹并将其进行排除。
进一步地,所述基于当前时刻的自车图形与目标车辆图形的相交性判断结果进行车辆轨迹碰撞检测,具体包括:
根据当前时刻的自车质点坐标和自车yaw角获取当前时刻的自车图形,根据当前时刻的目标车辆质点坐标和目标车辆yaw角获取当前时刻的目标车辆图形;
判断当前时刻的自车图形与目标车辆图形是否相交;
若是,则确定该目标车辆轨迹与自车轨迹在未来预设时段内存在碰撞情况;
若否,则确定该目标车辆轨迹与自车轨迹在未来预设时段内不存在碰撞情况。
本发明还提供一种车辆轨迹碰撞检测装置,包括:
轨迹获取模块,用于获取未来预设时段内的自车轨迹以及目标车辆轨迹;其中,所述目标车辆包括未来预设时段内距离自车预设范围内的其他所有车辆;
第一阶段检测模块,用于:获取所有车辆轨迹的OBB包围盒,基于自车轨迹与目标车辆轨迹的OBB包围盒的相交性判断结果识别出安全车辆轨迹并排除;或,获取各条目标车辆轨迹与自车轨迹在未来预设时段内的最短距离,基于两车车身平均长度与对应的最短距离的比较关系识别出安全车辆轨迹并排除;其中,所述两车车身平均长度为基于自车车身长度与对应的目标车辆车身长度相加的和的一半进行确定得到;
第二阶段检测模块,用于:对于排除安全车辆轨迹后的其余各条目标车辆轨迹,以预设时间间隔求取自车轨迹与目标车辆轨迹在相同时刻的距离;
若判断所有相同时刻的距离均大于两车车身平均长度,则确定该目标车辆轨迹与自车轨迹在未来预设时段内不存在碰撞情况;其中,所述两车车身平均长度为基于自车车身长度与对应的目标车辆车身长度相加的和的一半进行确定得到;
若判断存在至少一相同时刻的距离小于或等于两车车身平均宽度,则确定该目标车辆轨迹与自车轨迹在未来预设时段内存在碰撞情况;其中,所述两车车身平均宽度为基于自车车身宽度与对应的目标车辆车身宽度相加的和的一半进行确定得到;
第三阶段检测模块,用于:若判断所有相同时刻的距离均大于两车车身平均宽度,且存在某一相同时刻的距离小于两车车身平均长度,则基于当前时刻的自车图形与目标车辆图形的相交性判断结果进行车辆轨迹碰撞检测。
本发明还提供一种终端设备,包括处理器和存储有计算机程序的存储器,所述处理器执行所述计算机程序时实现任一项所述的车辆轨迹碰撞检测方法。
本发明还提供一种非暂态计算机可读存储介质,其上存储有计算机程序,该计算机程序被处理器执行时实现任一项所述的车辆轨迹碰撞检测方法。
与现有技术相比,本发明具有如下有益效果:
本发明提供了一种车辆轨迹碰撞检测方法、装置、设备及存储介质,所述方法包括:获取各个车辆轨迹;第一阶段,根据其他车辆轨迹与自车轨迹的OBB包围盒是否相交来排除一定不会发生碰撞的安全车辆轨迹;第二阶段,排除安全车辆轨迹后,获取其余车辆轨迹与自车轨迹在相同时刻的距离,并根据同时刻距离与两车车身长宽的比较关系来检测出一定发生碰撞的轨迹以及一定不发生碰撞的轨迹;第三阶段,当第二阶段不能确定的可疑碰撞轨迹,采用具体车辆图形的方式来进行车辆轨迹碰撞检测。本发明通过首先采用精度相对不高但高效率的检测方法来快速排除一定不发生碰撞的轨迹,以快速减少计算量,再逐步通过高精度的检测方法对剩余的轨迹进行碰撞检测,从而能够大大提高车辆轨迹碰撞检测整体过程的效率。
附图说明
图1是本发明提供的车辆轨迹碰撞检测方法的流程示意图;
图2是本发明提供的双车道变道场景示意图;
图3是本发明提供的车辆轨迹OBB包围盒的示意图;
图4是本发明提供的基于圆形计算OBB包围盒的示意图;
图5是本发明提供的基于关键点计算OBB包围盒的示意图;
图6是本发明提供的两条轨迹最短距离求解示意图;
图7是本发明提供的车辆轨迹相交存在碰撞可能性的示意图;
图8是本发明提供的两车轨迹同时刻距离计算示意图之一;
图9是本发明提供的两车轨迹同时刻距离计算示意图之二;
图10是本发明提供的两车轨迹同时刻距离计算示意图之三;
图11是本发明提供的通过四边形对两车轨迹进行碰撞检测计算的示意图;
图12是本发明提供的基于车辆边缘点计算OBB包围盒的示意图;
图13是本发明提供的利用圆形拟合矩形进行车辆碰撞检测的示意图;
图14是本发明提供的车辆轨迹碰撞检测装置的结构示意图。
具体实施方式
下面将结合本发明实施例中的附图,对本发明实施例中的技术方案进行清楚、完整的描述,显然,所描述的实施例仅仅是本发明一部分实施例,而不是全部的实施例。基于本发明中的实施例,本领域普通技术人员在没有作出创造性劳动前提下所获得的所有其他实施例,都属于本发明保护的范围。
请参见图1,本发明实施例提供了一种车辆轨迹碰撞检测方法,可以包括步骤:
S1、获取未来预设时段内的自车轨迹以及目标车辆轨迹;其中,所述目标车辆包括未来预设时段内距离自车预设范围内的其他所有车辆;
S2、(1)获取所有车辆轨迹的OBB包围盒,基于自车轨迹与目标车辆轨迹的OBB包围盒的相交性判断结果识别出安全车辆轨迹并排除;或,(2)获取各条目标车辆轨迹与自车轨迹在未来预设时段内的最短距离,基于两车车身平均长度与对应的最短距离的比较关系识别出安全车辆轨迹并排除;其中,所述两车车身平均长度为基于自车车身长度与对应的目标车辆车身长度相加的和的一半进行确定得到;
S3、对于排除安全车辆轨迹后的其余各条目标车辆轨迹,以预设时间间隔求取自车轨迹与目标车辆轨迹在相同时刻的距离;
S4、若判断所有相同时刻的距离均大于两车车身平均长度,则确定该目标车辆轨迹与自车轨迹在未来预设时段内不存在碰撞情况;其中,所述两车车身平均长度为基于自车车身长度与对应的目标车辆车身长度相加的和的一半进行确定得到;
S5、若判断存在至少一相同时刻的距离小于或等于两车车身平均宽度,则确定该目标车辆轨迹与自车轨迹在未来预设时段内存在碰撞情况;其中,所述两车车身平均宽度为基于自车车身宽度与对应的目标车辆车身宽度相加的和的一半进行确定得到;
S6、若判断所有相同时刻的距离均大于两车车身平均宽度,且存在某一相同时刻的距离小于两车车身平均长度,则基于当前时刻的自车图形与目标车辆图形的相交性判断结果进行车辆轨迹碰撞检测。
需要说明的是,在本发明实施例中,自车轨迹指的是自动驾驶算法本次规划出来的轨迹,所述车辆轨迹碰撞检测方法用于判断这个规划轨迹是否会与其他车辆轨迹相撞,若存在碰撞则不采用这一规划轨迹,若不存在碰撞则可以作为备选规划轨迹。另外,由于是未来预设时段内的轨迹,因此其他车辆的轨迹(目标车辆轨迹)是根据算法预测的轨迹,至于如何获取自车规划轨迹和其他车辆预测轨迹的具体方法不在本发明实施例讨论范围内。
需要说明的是,步骤2中的(1)和(2)对应于独立的两种方案,采用方法(1)和方法(2)的任一种均能实现本发明的效果。
需要说明的是,本发明实施例的车辆轨迹碰撞检测方法分为三个检测阶段,第一阶段,根据其他车辆轨迹与自车轨迹的OBB包围盒是否相交来快速排除一定不会发生碰撞的安全车辆轨迹,快速减少计算量;第二阶段,排除安全车辆轨迹后,获取其余车辆轨迹与自车轨迹之间在各个相同时刻的距离,并根据同时刻距离与两车车身长宽的比较关系来检测出一定发生碰撞的轨迹以及一定不发生碰撞的轨迹,通过第二阶段也能较快地检测出必然发生碰撞和一定不会发生碰撞的情况,若检测到一定不会发生碰撞的情况则进一步减少了计算量;第三阶段,当第二阶段排除了一定不会发生碰撞的情况后,尚且还存在可疑碰撞轨迹的情况下,采用具体车辆图形的方式来进一步进行车辆轨迹碰撞检测,此阶段为具体车身数据进行碰撞检测,计算量较多也最精确,但由于前期第一第二阶段已经通过高效算法排除了一定的计算量,使得最终的碰撞检测阶段的计算量相对于传统方法大量减少。本发明能够确保车辆碰撞轨迹的检测精确性的前提下,大大提高整体检测过程的效率。
在本发明实施例中,进一步地,所述OBB包围盒的获取方法具体包括:
对于每一车辆轨迹,基于车辆中心坐标以及车辆yaw角确定车辆角点坐标,将该车辆轨迹对应的所有的车辆角点坐标作为输入数据计算该车辆轨迹对应的OBB包围盒。
需要说明的是,在本发明实施例中,通过根据车辆中心坐标以及车辆yaw角来确定车辆的四个角点坐标,再获取该车辆在未来预设时段内的每个时刻(按一定时间间隔取点)的角点坐标,作为输入数据进行计算该车辆轨迹对应的OBB包围盒。本方法所利用的数据较多,精确度相对较高,但这样相对来说会具有较大的计算量。
在本发明实施例中,进一步地,所述OBB包围盒的获取方法具体包括:
对于每一车辆轨迹,以车辆轨迹上的点作为圆心、以车身长度作为直径画圆形,在圆形上均匀取点作为关键点,将该车辆轨迹对应的所有的关键点作为输入数据计算该车辆轨迹对应的OBB包围盒。
在本发明实施例中,通过基于车身长度在轨迹上连续作圆形,再在圆形上均匀取点作为计算该车辆轨迹OBB包围盒的输入数据。本方法相对上述实施例减少了一定的计算量,提高了计算效率。
在本发明实施例中,进一步地,所述OBB包围盒的获取方法具体包括:
对于每一车辆轨迹,确定车辆轨迹的起始点、终点和极值点,并在车辆轨迹上的起始点、终点和极值点之间均匀插入若干个辅助点,以车身长度为直径在起始点、终点、极值点和辅助点处分别作车辆轨迹的垂直平分线段,并以各个垂直平分线段的顶点作为关键点,同时以车身长度的一半分别在起始点之前以及终点之后延伸出一个关键点,将该车辆轨迹对应的所有的关键点作为输入数据计算该车辆轨迹对应的OBB包围盒。
本发明实施例中,通过取起始点、终点和极值点并适当添加少量辅助点,最后获取轨迹对应的少量关键点,作为输入数据来计算轨迹的OBB包围盒。本方法相对于上述实施例进一步减少了计算量,进一步提高了计算效率。
在本发明实施例中,进一步地,所述基于自车轨迹与目标车辆轨迹的OBB包围盒的相交性判断结果识别出安全车辆轨迹并排除,具体包括:
利用SAT算法判断自车轨迹OBB包围盒与各条目标车辆轨迹的OBB包围盒是否相交,若不相交,则将该目标车辆轨迹识别为安全车辆轨迹并将其进行排除。
在本发明实施例中,进一步地,所述获取各条目标车辆轨迹与自车轨迹在未来预设时段内的最短距离,基于两车车身平均长度与对应的最短距离的比较关系识别出安全车辆轨迹并排除,具体包括:
在自车轨迹上按预设的时间间隔进行取点,并分别获取各点到目标车辆轨迹的距离最短值,从所有距离最短值中选取最小值作为目标车辆轨迹与自车轨迹在未来预设时段内的最短距离;
若判断目标车辆轨迹与自车轨迹在未来预设时段内的最短距离大于两车车身平均长度,则将该目标车辆轨迹识别为安全车辆轨迹并将其进行排除。
在本发明实施例中,进一步地,所述基于当前时刻的自车图形与目标车辆图形的相交性判断结果进行车辆轨迹碰撞检测,具体包括:
根据当前时刻的自车质点坐标和自车yaw角获取当前时刻的自车图形,根据当前时刻的目标车辆质点坐标和目标车辆yaw角获取当前时刻的目标车辆图形;
判断当前时刻的自车图形与目标车辆图形是否相交;
若是,则确定该目标车辆轨迹与自车轨迹在未来预设时段内存在碰撞情况;
若否,则确定该目标车辆轨迹与自车轨迹在未来预设时段内不存在碰撞情况。
需要说明的是,“当前时刻”指的是“当判断所有相同时刻的距离均大于两车车身平均宽度,且存在某一相同时刻的距离小于两车车身平均长度”中的“相同时刻”,可以理解的是,对同一条目标车辆轨迹来说,可能会存在多个这样的时刻,对于任一这样的时刻,均采用上述方法进行碰撞检测。
基于上述方案,为便于更好的理解本发明实施例提供的车辆轨迹碰撞检测方法,以下进行详细说明:
需要说明的是,本发明实施例提出了一种阶梯性的碰撞检测方法,首先使用精度较差但高效的检测方法快速排除一定不发生碰撞的静态与动态物体(常见场景下50%以上的静态和动态障碍物可以通过精度相对较低但效率高的检测方法检测),然后采用精度较高的检测方法计算可能发生碰撞的轨迹,从而避免了对所有姿态的数据都进行碰撞检测计算的低效率问题,提高碰撞检测效率。
本发明实施例可以通过以下步骤实现:
如图2所示为双车道变道场景,A为自车,B、C为车道上行驶的其他车辆,三条曲线分别为对应车辆未来8s时间内的轨迹,现需要对所规划的轨迹进行碰撞检测,即自车按照规划的轨迹进行行驶,在未来8s内会不会与其实车辆发生碰撞。
图2中自车A与车辆B轨迹存在重合,存在发生碰撞的可能性,自车A与车辆C轨迹不存在重合,一定不会发生碰撞,该阶段的目的是快速将如车辆C这样的一定不会发生碰撞目标排除掉。
阶段一(方法1):
如图3所示,求解自车A轨迹二维OBB包围盒以及车辆C轨迹二维包围盒,利用SAT算法判断自车A轨迹包围盒与车辆C轨迹包围盒是否相交,如不相交则认为自车A和车辆C在未来8s内一定不会发生碰撞,可以排除在外。
然而,图3中的计算OBB包围盒的方法把车辆假设为了质点,但实际并非如此,在车辆的碰撞检测中,车辆的长宽不能忽略,因此做出图4所示改进。
如图4所示,La和Lc分别为自车A和车辆C的车身长,通过车辆yaw角和车辆中心坐标可以准确计算车辆四个角点位置坐标,以车辆角点为输入计算轨迹的包围盒能够得到更加精确的结果,然而这样会加大计算量;作为优选方案,本发明实施例以轨迹上点为圆心,直径为车身长画圆,将圆上取点(可以在圆上均匀的取四个点或者6个点等)作为计算包围盒的输入,减少计算量。
由于点的数量越多,计算OBB的时间越长,为减少计算量,本发明实施例优选地只选取关键点作为计算OBB的输入,如图5所示,曲线为车辆轨迹,曲线外的点为选取的关键点,方框为OBB包围盒,关键点的选取方法为:1,首先选取轨迹的起始点和终点;2,取曲线的极值点,如图中P、Q两点;3,在起始点与极值点,极值点与极值点之间插入辅助点,如图中M、N两点;4,因为车身长宽不能忽略,所以在现有各个点的上下各插入一个关键点,两点间的距离为车身长L(两点连线与轨迹切线垂直),在起始点和终点前后(沿切线方向)分别插入一个关键点,距离为车身长L/2。依据图中所有关键点作为输入求解OBB包围盒,计算方法如下
假设一共得到m个关键点,每个点的坐标为:(Xi,Yi);
求解协方差矩形C:
分别求解协方差矩阵C特征向量为e1,e2,
将e1,e2作为新的一组基,在新基下的新坐标系为uv坐标系,将原来空间下的(Xi,Yi)都转到新的uv空间:
计算(Ui,Vi)所有点的u和v的最大值和最小值,分别为umax,vmax,umin,vmin,组成四个顶点,这四个顶点的坐标为(umax,vmax),(umax,vmin),(umin,vmax),(umin,vmax),将这四个点通过:
转到xy空间,即可得到OBB包围盒的四个顶点。
阶段一(方法2):
如图6所示,通过求解两条轨迹任意点的最短距离来判断自车A和车辆C未来8s是否存在发生碰撞的可能性,一般车辆轨迹多用五次多项式表示,较难找到求解两条多项式最短距离的一般解,本发明实施例采用数值求解方法-黄金分割法求解最小值,在自车轨迹上每间隔0.1s取一个点,依次计算该点到车辆C轨迹上距离的最短值Lt(0<t<8),图6中tm时刻的距离最小值为Ltm= Lm,tn时刻的距离最小值为Ltn= Ln,最终两条轨迹距离最小值Lmin=min(L0.1,L0.2,L0.3,L0.4......L8)。如果Lmin>(La+Lc)/2,则自车A和车辆C未来8s内一定不会发生碰撞,其中La和Lc分别为自车A和车辆C的车身长。
阶段二:
如图7所示,自车A与车辆B的轨迹包围盒OBB相交,存在碰撞的可能性。
针对OBB相交的情况,以0.1s为间隔依次计算相同时刻两条轨迹上点的距离,如图8所示,t1时刻两车中心点之间的距离为L1,t2时刻两车中心点之间的距离为L2,tn时刻两车中心点之间的距离为Ln,如果在0-8s内,都满足Ln>(La+Lb)/2,则自车和车辆B在未来8S内不会发生碰撞。
如出现图9所示场景,在t=t4时刻,L4<=(Wa+Wb)/2,其中Wa为自车车身宽度,Wb为车辆B车身宽度,则自车与车辆B两条轨迹必然存在碰撞。
阶段三:
如图10所示,如果在0-8s内,都满足Ln>(Wa+Wb)/2,且存在某时刻Ln<(La+Lb)/2,则需进一步进行判断,对所有t时刻满足(Wa+Wb)/2<Ln<(La+Lb)/2的轨迹点进行计算。
如图11所示,t3时刻满足(Wa+Wb)/2<Ln<(La+Lb)/2,根据自车坐标以及自车yaw角计算该时刻车辆的四个角点,同样方法获取车辆B的四个角点,利用SAT方法判断两个矩形是否相交;如所有t时刻满足(Wa+Wb)/2<Ln<(La+Lb)/2的轨迹点的四个角点组成的四边形都不相交,则自车和车辆B在未来8S内不发生碰撞,如存在某时刻两个四边形相交,则自车和车辆B在未来8s内发生碰撞。
另外,对于上述实施例的补充方案如下:
针对阶段一的方法一,图5所示,为了加入对车身长和宽的因素,分别在已有关键点的上下各插入了一个点,这样虽然可以快速计算,但也引入较大误差,使最终计算的OBB包围盒大于真实情况,可对关键点的选取做如下改进,如图12所示,关键点根据车辆yaw角计算,以车长边缘中心作为关键点,轨迹起始点和终点的车头和车尾终点也选取为关键点。
针对阶段三,图11所示,计算两个矩形是否相交,利用SAT方法计算,同样可以使用圆覆盖长方形方法,如图13所示,用圆O1,O2,O3覆盖t时刻车辆A,用圆O4,O5,O6覆盖t时刻车辆A,分别计算O1,O2,O3是否与O4,O5,O6相交,如果相交则表示两个矩形相交,否则不相交,此方法提高计算速度但降低精度。
需要说明的是,对于以上方法或流程实施例,为了简单描述,故将其都表述为一系列的动作组合,但是本领域技术人员应该知悉,本发明实施例并不受所描述的动作顺序的限制,因为依据本发明实施例,某些步骤可以采用其他顺序或者同时进行。其次,本领域技术人员也应该知悉,说明书中所描述的实施例均属于可选实施例,所涉及的动作并不一定是本发明实施例所必须的。
请参见图14,本发明实施例还提供一种车辆轨迹碰撞检测装置,包括:
轨迹获取模块1,用于获取未来预设时段内的自车轨迹以及目标车辆轨迹;其中,所述目标车辆包括未来预设时段内距离自车预设范围内的其他所有车辆;
第一阶段检测模块2,用于:获取所有车辆轨迹的OBB包围盒,基于自车轨迹与目标车辆轨迹的OBB包围盒的相交性判断结果识别出安全车辆轨迹并排除;或,获取各条目标车辆轨迹与自车轨迹在未来预设时段内的最短距离,基于两车车身平均长度与对应的最短距离的比较关系识别出安全车辆轨迹并排除;其中,所述两车车身平均长度为基于自车车身长度与对应的目标车辆车身长度相加的和的一半进行确定得到;
第二阶段检测模块3,用于:对于排除安全车辆轨迹后的其余各条目标车辆轨迹,以预设时间间隔求取自车轨迹与目标车辆轨迹在相同时刻的距离;
若判断所有相同时刻的距离均大于两车车身平均长度,则确定该目标车辆轨迹与自车轨迹在未来预设时段内不存在碰撞情况;其中,所述两车车身平均长度为基于自车车身长度与对应的目标车辆车身长度相加的和的一半进行确定得到;
若判断存在至少一相同时刻的距离小于或等于两车车身平均宽度,则确定该目标车辆轨迹与自车轨迹在未来预设时段内存在碰撞情况;其中,所述两车车身平均宽度为基于自车车身宽度与对应的目标车辆车身宽度相加的和的一半进行确定得到;
第三阶段检测模块4,用于:若判断所有相同时刻的距离均大于两车车身平均宽度,且存在某一相同时刻的距离小于两车车身平均长度,则基于当前时刻的自车图形与目标车辆图形的相交性判断结果进行车辆轨迹碰撞检测。
可以理解的是上述装置项实施例,是与本发明方法项实施例相对应的,本发明实施例提供的一种车辆轨迹碰撞检测装置,可以实现本发明任意一项方法项实施例提供的车辆轨迹碰撞检测方法。
本发明还提供一种非暂态计算机可读存储介质,其上存储有计算机程序,该计算机程序被处理器执行时实现任一项所述的车辆轨迹碰撞检测方法。
需说明的是,以上所描述的装置实施例仅仅是示意性的,其中所述作为分离部件说明的单元可以是或者也可以不是物理上分开的,作为单元显示的部件可以是或者也可以不是物理单元,即可以位于一个地方,或者也可以分布到多个网络单元上。可以根据实际的需要选择其中的部分或者全部模块来实现本实施例方案的目的。另外,本发明提供的装置实施例附图中,模块之间的连接关系表示它们之间具有通信连接,具体可以实现为一条或多条通信总线或信号线。本领域普通技术人员在不付出创造性劳动的情况下,即可以理解并实施。
所述领域的技术人员可以清楚地了解到,为了方便和简洁,上述描述的装置的具体工作过程,可参考前述方法实施例中对应的过程,在此不再赘述。
终端设备可以是桌上型计算机、笔记本、掌上电脑及云端服务器等计算设备。所述终端设备可包括,但不仅限于,处理器、存储器。
所称处理器可以是中央处理单元(Central Processing Unit,CPU),还可以是其他通用处理器、数字信号处理器 (Digital Signal Processor,DSP)、专用集成电路(Application SpecificIntegrated Circuit,ASIC)、现成可编程门阵列 (Field-Programmable Gate Array,FPGA) 或者其他可编程逻辑器件、分立门或者晶体管逻辑器件、分立硬件组件等。通用处理器可以是微处理器或者该处理器也可以是任何常规的处理器等,所述处理器是所述终端设备的控制中心,利用各种接口和线路连接整个终端设备的各个部分。
所述存储器可用于存储所述计算机程序,所述处理器通过运行或执行存储在所述存储器内的计算机程序,以及调用存储在存储器内的数据,实现所述终端设备的各种功能。所述存储器可主要包括存储程序区和存储数据区,其中,存储程序区可存储操作系统、至少一个功能所需的应用程序等;存储数据区可存储根据手机的使用所创建的数据等。此外,存储器可以包括高速随机存取存储器,还可以包括非易失性存储器,例如硬盘、内存、插接式硬盘,智能存储卡(Smart Media Card, SMC),安全数字(Secure Digital, SD)卡,闪存卡(Flash Card)、至少一个磁盘存储器件、闪存器件、或其他易失性固态存储器件。
所述存储介质为计算机可读存储介质,所述计算机程序存储在所述计算机可读存储介质中,该计算机程序在被处理器执行时,可实现上述各个方法实施例的步骤。其中,所述计算机程序包括计算机程序代码,所述计算机程序代码可以为源代码形式、对象代码形式、可执行文件或某些中间形式等。所述计算机可读介质可以包括:能够携带所述计算机程序代码的任何实体或装置、记录介质、U盘、移动硬盘、磁碟、光盘、计算机存储器、只读存储器(ROM,Read-Only Memory)、随机存取存储器(RAM,Random AccessMemory)、电载波信号、电信信号以及软件分发介质等。需要说明的是,所述计算机可读介质包含的内容可以根据司法管辖区内立法和专利实践的要求进行适当的增减,例如在某些司法管辖区,根据立法和专利实践,计算机可读介质不包括电载波信号和电信信号。
以上所述是本发明的优选实施方式,应当指出,对于本技术领域的普通技术人员来说,在不脱离本发明原理的前提下,还可以做出若干改进和润饰,这些改进和润饰也视为本发明的保护范围。
Claims (10)
1.一种车辆轨迹碰撞检测方法,其特征在于,包括:
获取未来预设时段内的自车轨迹以及目标车辆轨迹;其中,所述目标车辆包括未来预设时段内距离自车预设范围内的其他所有车辆;
获取所有车辆轨迹的OBB包围盒,基于自车轨迹与目标车辆轨迹的OBB包围盒的相交性判断结果识别出安全车辆轨迹并排除;或,获取各条目标车辆轨迹与自车轨迹在未来预设时段内的最短距离,基于两车车身平均长度与对应的最短距离的比较关系识别出安全车辆轨迹并排除;其中,所述两车车身平均长度为基于自车车身长度与对应的目标车辆车身长度相加的和的一半进行确定得到;
对于排除安全车辆轨迹后的其余各条目标车辆轨迹,以预设时间间隔求取自车轨迹与目标车辆轨迹在相同时刻的距离;
若判断所有相同时刻的距离均大于两车车身平均长度,则确定该目标车辆轨迹与自车轨迹在未来预设时段内不存在碰撞情况;其中,所述两车车身平均长度为基于自车车身长度与对应的目标车辆车身长度相加的和的一半进行确定得到;
若判断存在至少一相同时刻的距离小于或等于两车车身平均宽度,则确定该目标车辆轨迹与自车轨迹在未来预设时段内存在碰撞情况;其中,所述两车车身平均宽度为基于自车车身宽度与对应的目标车辆车身宽度相加的和的一半进行确定得到;
若判断所有相同时刻的距离均大于两车车身平均宽度,且存在某一相同时刻的距离小于两车车身平均长度,则基于当前时刻的自车图形与目标车辆图形的相交性判断结果进行车辆轨迹碰撞检测。
2.根据权利要求1所述的车辆轨迹碰撞检测方法,其特征在于,所述OBB包围盒的获取方法具体包括:
对于每一车辆轨迹,基于车辆中心坐标以及车辆yaw角确定车辆角点坐标,将该车辆轨迹对应的所有的车辆角点坐标作为输入数据计算该车辆轨迹对应的OBB包围盒。
3.根据权利要求1所述的车辆轨迹碰撞检测方法,其特征在于,所述OBB包围盒的获取方法具体包括:
对于每一车辆轨迹,以车辆轨迹上的点作为圆心、以车身长度作为直径画圆形,在圆形上均匀取点作为关键点,将该车辆轨迹对应的所有的关键点作为输入数据计算该车辆轨迹对应的OBB包围盒。
4.根据权利要求1所述的车辆轨迹碰撞检测方法,其特征在于,所述OBB包围盒的获取方法具体包括:
对于每一车辆轨迹,确定车辆轨迹的起始点、终点和极值点,并在车辆轨迹上的起始点、终点和极值点之间均匀插入若干个辅助点,以车身长度为直径在起始点、终点、极值点和辅助点处分别作车辆轨迹的垂直平分线段,并以各个垂直平分线段的顶点作为关键点,同时以车身长度的一半分别在起始点之前以及终点之后延伸出一个关键点,将该车辆轨迹对应的所有的关键点作为输入数据计算该车辆轨迹对应的OBB包围盒。
5.根据权利要求1所述的车辆轨迹碰撞检测方法,其特征在于,所述基于自车轨迹与目标车辆轨迹的OBB包围盒的相交性判断结果识别出安全车辆轨迹并排除,具体包括:
利用SAT算法判断自车轨迹OBB包围盒与各条目标车辆轨迹的OBB包围盒是否相交,若不相交,则将该目标车辆轨迹识别为安全车辆轨迹并将其进行排除。
6.根据权利要求1所述的车辆轨迹碰撞检测方法,其特征在于,所述获取各条目标车辆轨迹与自车轨迹在未来预设时段内的最短距离,基于两车车身平均长度与对应的最短距离的比较关系识别出安全车辆轨迹并排除,具体包括:
在自车轨迹上按预设的时间间隔进行取点,并分别获取各点到目标车辆轨迹的距离最短值,从所有距离最短值中选取最小值作为目标车辆轨迹与自车轨迹在未来预设时段内的最短距离;
若判断目标车辆轨迹与自车轨迹在未来预设时段内的最短距离大于两车车身平均长度,则将该目标车辆轨迹识别为安全车辆轨迹并将其进行排除。
7.根据权利要求1所述的车辆轨迹碰撞检测方法,其特征在于,所述基于当前时刻的自车图形与目标车辆图形的相交性判断结果进行车辆轨迹碰撞检测,具体包括:
根据当前时刻的自车质点坐标和自车yaw角获取当前时刻的自车图形,根据当前时刻的目标车辆质点坐标和目标车辆yaw角获取当前时刻的目标车辆图形;
判断当前时刻的自车图形与目标车辆图形是否相交;
若是,则确定该目标车辆轨迹与自车轨迹在未来预设时段内存在碰撞情况;
若否,则确定该目标车辆轨迹与自车轨迹在未来预设时段内不存在碰撞情况。
8.一种车辆轨迹碰撞检测装置,其特征在于,包括:
轨迹获取模块,用于获取未来预设时段内的自车轨迹以及目标车辆轨迹;其中,所述目标车辆包括未来预设时段内距离自车预设范围内的其他所有车辆;
第一阶段检测模块,用于:获取所有车辆轨迹的OBB包围盒,基于自车轨迹与目标车辆轨迹的OBB包围盒的相交性判断结果识别出安全车辆轨迹并排除;或,获取各条目标车辆轨迹与自车轨迹在未来预设时段内的最短距离,基于两车车身平均长度与对应的最短距离的比较关系识别出安全车辆轨迹并排除;其中,所述两车车身平均长度为基于自车车身长度与对应的目标车辆车身长度相加的和的一半进行确定得到;
第二阶段检测模块,用于:对于排除安全车辆轨迹后的其余各条目标车辆轨迹,以预设时间间隔求取自车轨迹与目标车辆轨迹在相同时刻的距离;
若判断所有相同时刻的距离均大于两车车身平均长度,则确定该目标车辆轨迹与自车轨迹在未来预设时段内不存在碰撞情况;其中,所述两车车身平均长度为基于自车车身长度与对应的目标车辆车身长度相加的和的一半进行确定得到;
若判断存在至少一相同时刻的距离小于或等于两车车身平均宽度,则确定该目标车辆轨迹与自车轨迹在未来预设时段内存在碰撞情况;其中,所述两车车身平均宽度为基于自车车身宽度与对应的目标车辆车身宽度相加的和的一半进行确定得到;
第三阶段检测模块,用于:若判断所有相同时刻的距离均大于两车车身平均宽度,且存在某一相同时刻的距离小于两车车身平均长度,则基于当前时刻的自车图形与目标车辆图形的相交性判断结果进行车辆轨迹碰撞检测。
9.一种终端设备,包括处理器和存储有计算机程序的存储器,其特征在于,所述处理器执行所述计算机程序时实现权利要求1至7任一项所述的车辆轨迹碰撞检测方法。
10.一种非暂态计算机可读存储介质,其上存储有计算机程序,其特征在于,该计算机程序被处理器执行时实现如权利要求1至7任一项所述的车辆轨迹碰撞检测方法。
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202310154043.4A CN115837921B (zh) | 2023-02-23 | 2023-02-23 | 车辆轨迹碰撞检测方法、装置、设备及存储介质 |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202310154043.4A CN115837921B (zh) | 2023-02-23 | 2023-02-23 | 车辆轨迹碰撞检测方法、装置、设备及存储介质 |
Publications (2)
Publication Number | Publication Date |
---|---|
CN115837921A true CN115837921A (zh) | 2023-03-24 |
CN115837921B CN115837921B (zh) | 2023-06-02 |
Family
ID=85580107
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
CN202310154043.4A Active CN115837921B (zh) | 2023-02-23 | 2023-02-23 | 车辆轨迹碰撞检测方法、装置、设备及存储介质 |
Country Status (1)
Country | Link |
---|---|
CN (1) | CN115837921B (zh) |
Citations (8)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN102806912A (zh) * | 2011-06-01 | 2012-12-05 | 通用汽车环球科技运作有限责任公司 | 用于连接的自主车辆和手动车辆的快速碰撞检测技术 |
CN113978457A (zh) * | 2021-12-24 | 2022-01-28 | 深圳佑驾创新科技有限公司 | 一种碰撞风险预测方法及装置 |
CN114261389A (zh) * | 2021-11-29 | 2022-04-01 | 东风商用车有限公司 | 自动驾驶商用车的行驶轨迹碰撞风险分析方法 |
CN115027464A (zh) * | 2022-07-29 | 2022-09-09 | 西安电子科技大学芜湖研究院 | 一种基于方向包围盒的自动驾驶碰撞检测方法 |
CN115027457A (zh) * | 2022-06-29 | 2022-09-09 | 广州小鹏自动驾驶科技有限公司 | 碰撞检测方法、装置、车辆及存储介质 |
CN115092130A (zh) * | 2022-06-16 | 2022-09-23 | 阿波罗智能技术(北京)有限公司 | 车辆碰撞预测方法、装置、电子设备、介质和车辆 |
CN115237928A (zh) * | 2022-09-26 | 2022-10-25 | 北京理工大学 | 一种基于轨迹基元分割的高效碰撞检测方法及系统 |
CN115617042A (zh) * | 2022-10-10 | 2023-01-17 | 上海仙途智能科技有限公司 | 碰撞检测方法及装置、终端设备、计算机可读存储介质 |
-
2023
- 2023-02-23 CN CN202310154043.4A patent/CN115837921B/zh active Active
Patent Citations (9)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN102806912A (zh) * | 2011-06-01 | 2012-12-05 | 通用汽车环球科技运作有限责任公司 | 用于连接的自主车辆和手动车辆的快速碰撞检测技术 |
US20120306663A1 (en) * | 2011-06-01 | 2012-12-06 | GM Global Technology Operations LLC | Fast Collision Detection Technique for Connected Autonomous and Manual Vehicles |
CN114261389A (zh) * | 2021-11-29 | 2022-04-01 | 东风商用车有限公司 | 自动驾驶商用车的行驶轨迹碰撞风险分析方法 |
CN113978457A (zh) * | 2021-12-24 | 2022-01-28 | 深圳佑驾创新科技有限公司 | 一种碰撞风险预测方法及装置 |
CN115092130A (zh) * | 2022-06-16 | 2022-09-23 | 阿波罗智能技术(北京)有限公司 | 车辆碰撞预测方法、装置、电子设备、介质和车辆 |
CN115027457A (zh) * | 2022-06-29 | 2022-09-09 | 广州小鹏自动驾驶科技有限公司 | 碰撞检测方法、装置、车辆及存储介质 |
CN115027464A (zh) * | 2022-07-29 | 2022-09-09 | 西安电子科技大学芜湖研究院 | 一种基于方向包围盒的自动驾驶碰撞检测方法 |
CN115237928A (zh) * | 2022-09-26 | 2022-10-25 | 北京理工大学 | 一种基于轨迹基元分割的高效碰撞检测方法及系统 |
CN115617042A (zh) * | 2022-10-10 | 2023-01-17 | 上海仙途智能科技有限公司 | 碰撞检测方法及装置、终端设备、计算机可读存储介质 |
Also Published As
Publication number | Publication date |
---|---|
CN115837921B (zh) | 2023-06-02 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
CN110316193B (zh) | 预瞄距离的设置方法、装置、设备及计算机可读存储介质 | |
US10838424B2 (en) | Charging station identifying method and robot | |
CN114764249B (zh) | 实时避障路径规划方法、系统、设备及介质 | |
US11372414B2 (en) | Robotic motion control method and apparatus and robot using the same | |
CN110962858B (zh) | 目标识别方法及设备 | |
CN115326051A (zh) | 一种基于动态场景的定位方法、装置、机器人及介质 | |
CN115617042A (zh) | 碰撞检测方法及装置、终端设备、计算机可读存储介质 | |
CN111319614A (zh) | 一种自动泊车路径规划方法、装置和存储介质 | |
CN116382306B (zh) | 全覆盖作业农机的轨迹跟踪控制方法、装置、设备及介质 | |
CN115837921B (zh) | 车辆轨迹碰撞检测方法、装置、设备及存储介质 | |
CN112363516A (zh) | 虚拟墙生成方法、装置、机器人及存储介质 | |
US20230266763A1 (en) | Method for navigating root through limited space, robot and computer-readable storage medium | |
CN117022249A (zh) | 平行车位自动泊车轨迹规划方法、系统、介质及设备 | |
CN109831717B (zh) | 一种降噪处理方法、系统及终端设备 | |
CN112800351B (zh) | 轨迹相似度判断方法、系统及计算机介质 | |
US20210286078A1 (en) | Apparatus for tracking object based on lidar sensor and method therefor | |
CN114489060A (zh) | 无人船编队控制方法、终端设备及计算机可读存储介质 | |
CN110728288B (zh) | 一种基于三维激光点云的角点特征提取方法及其应用 | |
CN114296452B (zh) | 自动泊车路径规划方法、设备及计算机可读存储介质 | |
CN117657217A (zh) | 泊车规划方法、电子设备和存储介质 | |
CN116382308B (zh) | 智能化移动机械自主寻径与避障方法、装置、设备及介质 | |
CN115755755B (zh) | Agv小车b样条曲线路径快速生成方法、存储介质及系统 | |
CN117111053B (zh) | 一种毫米波雷达跟踪结果的处理方法、装置以及处理设备 | |
CN117519119A (zh) | 一种路径规划方法、装置及计算机程序产品 | |
CN117762121A (zh) | 车辆的路径规划方法及装置、存储介质、车辆、终端 |
Legal Events
Date | Code | Title | Description |
---|---|---|---|
PB01 | Publication | ||
PB01 | Publication | ||
SE01 | Entry into force of request for substantive examination | ||
SE01 | Entry into force of request for substantive examination | ||
GR01 | Patent grant | ||
GR01 | Patent grant | ||
CP03 | Change of name, title or address |
Address after: Floor 25, Block A, Zhongzhou Binhai Commercial Center Phase II, No. 9285, Binhe Boulevard, Shangsha Community, Shatou Street, Futian District, Shenzhen, Guangdong 518000 Patentee after: Shenzhen Youjia Innovation Technology Co.,Ltd. Address before: 518051 401, building 1, Shenzhen new generation industrial park, No. 136, Zhongkang Road, Meidu community, Meilin street, Futian District, Shenzhen, Guangdong Province Patentee before: SHENZHEN MINIEYE INNOVATION TECHNOLOGY Co.,Ltd. |
|
CP03 | Change of name, title or address |