一种基于轨迹基元分割的高效碰撞检测方法及系统技术方案

本发明专利技术涉及一种基于轨迹基元分割的高效碰撞检测方法及系统，属于碰撞检测技术领域。根据轨迹点的曲率生成不同的基元，分别使用更紧密的圆弧包围盒和矩形包围盒，减少由于包围盒的冗余空间过大造成的无效检测次数，运用树型结构描述轨迹和障碍物，并设计粗检测与细检测相结合的遍历策略，快速确定发生碰撞的碰撞位置和碰撞时间，减少遍历所有轨迹与障碍物的时间，在障碍物较多的狭窄通道场景中能够更高效、快速的完成碰撞检测。快速的完成碰撞检测。快速的完成碰撞检测。

【技术实现步骤摘要】
一种基于轨迹基元分割的高效碰撞检测方法及系统


[0001]本专利技术涉及碰撞检测
，特别是涉及一种基于轨迹基元分割的高效碰撞检测方法及系统。

技术介绍

[0002]碰撞检测模块是自动驾驶车辆运动规划中必不可少的模块之一，可以检查车辆轨迹与周边环境是否存在碰撞风险，决定了自动驾驶车辆的行驶安全性。但由于环境中障碍物复杂多变，以及检测单元精度有限，碰撞检测模块在运动规划过程中要占用大量时间以及算力。
[0003]目前，国内外学者对于自动驾驶领域的碰撞检测方法已经有了大量研究，占用栅格方法通过将平面离散为网格单元，判断栅格是否被占据而进行检测，但在障碍物众多的狭窄空间内，对网格大小的选择要求很高，栅格过大，检测精度下降；栅格过小，计算复杂度增加。路径采样方法通过对路径进行一系列采样得到离散单元构型，然后用这些构型与其它物体构型比较来判断是否发生碰撞，但将连续轨迹离散化然后进行一对一检测，很依赖采样的方式和步长，当轨迹数量上升时，计算复杂度大大增加。层次包围盒方法能够以较低的代价快速确定发生碰撞的大致范围，然后在该区域内进行更详细的检测，但使用矩形去近似轨迹，包围盒内冗余空间依然很大，影响了碰撞检测的效率。
[0004]基于此，亟需一种能够高效进行碰撞检测的技术。

技术实现思路

[0005]本专利技术的目的是提供一种基于轨迹基元分割的高效碰撞检测方法及系统，能够高效的进行碰撞检测。
[0006]为实现上述目的，本专利技术提供了如下方案：一种基于轨迹基元分割的高效碰撞检测方法，所述高效碰撞检测方法包括：获取车辆行驶轨迹信息以及障碍物信息；所述车辆行驶轨迹信息包括每一个轨迹点的位置、航向和曲率；所述障碍物信息包括每一个障碍物的位置和形状；对于每一所述轨迹点，根据所述轨迹点的曲率将所述轨迹点划分为左转基元、右转基元和直行基元；连续的若干个所述左转基元组成左转轨迹段，连续的若干个所述右转基元组成右转轨迹段，连续的若干个所述直行基元组成直行轨迹段；按照单调性原则对每一所述左转轨迹段进行分割，得到若干个左转段；按照所述单调性原则对每一所述右转轨迹段进行分割，得到若干个右转段；所述单调性原则为每一段内的轨迹点的曲率单调不增或者单调不减；利用二分法对所述左转段进行迭代分割，直至所得到的左转子段的端点曲率的倍数差小于预设倍数差，得到若干个左转子段；利用二分法对所述右转段进行迭代分割，直至所得到的右转子段的端点曲率的倍数差小于所述预设倍数差，得到若干个右转子段；以所有所述轨迹点作为根节点，以所述左转段、所述右转段和所述直行轨迹段作
为所述根节点的子节点，以第1次迭代得到的所述左转子段作为所述左转段的子节点，以第N次迭代得到的所述左转子段作为第N
‑
1次迭代得到的所述左转子段的子节点，以第1次迭代得到的所述右转子段作为所述右转段的子节点，以第N次迭代得到的所述右转子段作为第N
‑
1次迭代得到的所述右转子段的子节点，并建立所述左转段、所述右转段、所述左转子段和所述右转子段的圆弧包围盒，建立所述直行轨迹段的矩形包围盒，得到轨迹包围盒树；其中，N大于1；建立包含所有所述障碍物的AABB包围盒；对所述AABB包围盒进行迭代分割，直至分割得到的空间内仅包含一个障碍物；以所有所述障碍物作为根节点，以第1次迭代得到的所述空间内的障碍物作为所述根节点的子节点，以第N次迭代得到的所述空间内的障碍物作为第N
‑
1次迭代得到的所述空间内的障碍物的子节点，并建立仅包含一个障碍物的子节点的OBB包围盒，建立包含若干个障碍物的子节点的AABB包围盒，得到障碍物包围盒树；对所述轨迹包围盒树的轨迹包围盒和所述障碍物包围盒树的障碍物包围盒进行碰撞检测，确定若干个碰撞检测对；所述碰撞检测对由所述轨迹包围盒树的叶子节点的轨迹包围盒与所述障碍物包围盒树的叶子节点的障碍物包围盒组成；对每一所述碰撞检测对进行碰撞检测，确定车辆行驶轨迹是否与障碍物发生碰撞，并在发生碰撞时，确定碰撞位置和碰撞时间。
[0007]一种基于轨迹基元分割的高效碰撞检测系统，所述高效碰撞检测系统包括：信息获取模块，用于获取车辆行驶轨迹信息以及障碍物信息；所述车辆行驶轨迹信息包括每一个轨迹点的位置、航向和曲率；所述障碍物信息包括每一个障碍物的位置和形状；轨迹包围盒树构建模块，用于对于每一所述轨迹点，根据所述轨迹点的曲率将所述轨迹点划分为左转基元、右转基元和直行基元；连续的若干个所述左转基元组成左转轨迹段，连续的若干个所述右转基元组成右转轨迹段，连续的若干个所述直行基元组成直行轨迹段；按照单调性原则对每一所述左转轨迹段进行分割，得到若干个左转段；按照所述单调性原则对每一所述右转轨迹段进行分割，得到若干个右转段；所述单调性原则为每一段内的轨迹点的曲率单调不增或者单调不减；利用二分法对所述左转段进行迭代分割，直至所得到的左转子段的端点曲率的倍数差小于预设倍数差，得到若干个左转子段；利用二分法对所述右转段进行迭代分割，直至所得到的右转子段的端点曲率的倍数差小于所述预设倍数差，得到若干个右转子段；以所有所述轨迹点作为根节点，以所述左转段、所述右转段和所述直行轨迹段作为所述根节点的子节点，以第1次迭代得到的所述左转子段作为所述左转段的子节点，以第N次迭代得到的所述左转子段作为第N
‑
1次迭代得到的所述左转子段的子节点，以第1次迭代得到的所述右转子段作为所述右转段的子节点，以第N次迭代得到的所述右转子段作为第N
‑
1次迭代得到的所述右转子段的子节点，并建立所述左转段、所述右转段、所述左转子段和所述右转子段的圆弧包围盒，建立所述直行轨迹段的矩形包围盒，得到轨迹包围盒树；其中，N大于1；障碍物包围盒树构建模块，用于建立包含所有所述障碍物的AABB包围盒；对所述AABB包围盒进行迭代分割，直至分割得到的空间内仅包含一个障碍物；以所有所述障碍物作为根节点，以第1次迭代得到的所述空间内的障碍物作为所述根节点的子节点，以第N次
迭代得到的所述空间内的障碍物作为第N
‑
1次迭代得到的所述空间内的障碍物的子节点，并建立仅包含一个障碍物的子节点的OBB包围盒，建立包含若干个障碍物的子节点的AABB包围盒，得到障碍物包围盒树；粗检测模块，用于对所述轨迹包围盒树的轨迹包围盒和所述障碍物包围盒树的障碍物包围盒进行碰撞检测，确定若干个碰撞检测对；所述碰撞检测对由所述轨迹包围盒树的叶子节点的轨迹包围盒与所述障碍物包围盒树的叶子节点的障碍物包围盒组成；细检测模块，用于对每一所述碰撞检测对进行碰撞检测，确定车辆行驶轨迹是否与障碍物发生碰撞，并在发生碰撞时，确定碰撞位置和碰撞时间。
[0008]根据本专利技术提供的具体实施例，本专利技术公开了以下技术效果：本专利技术用于提供一种基于轨迹基元分割的高效碰撞检测方法及系统，根据轨迹点的曲率生成不同的基元，分别使用更紧密的圆弧包围盒和矩形包围盒，减少由于包围盒的冗余空间过大造成的无效检测次数，运用树型结构描述轨迹和障碍物，并设计粗检测与细检测相结合的遍历策略，快速确定发生碰撞的碰撞位置和碰撞时间本文档来自技高网...

【技术保护点】

【技术特征摘要】
1.一种基于轨迹基元分割的高效碰撞检测方法，其特征在于，所述高效碰撞检测方法包括：获取车辆行驶轨迹信息以及障碍物信息；所述车辆行驶轨迹信息包括每一个轨迹点的位置、航向和曲率；所述障碍物信息包括每一个障碍物的位置和形状；对于每一所述轨迹点，根据所述轨迹点的曲率将所述轨迹点划分为左转基元、右转基元和直行基元；连续的若干个所述左转基元组成左转轨迹段，连续的若干个所述右转基元组成右转轨迹段，连续的若干个所述直行基元组成直行轨迹段；按照单调性原则对每一所述左转轨迹段进行分割，得到若干个左转段；按照所述单调性原则对每一所述右转轨迹段进行分割，得到若干个右转段；所述单调性原则为每一段内的轨迹点的曲率单调不增或者单调不减；利用二分法对所述左转段进行迭代分割，直至所得到的左转子段的端点曲率的倍数差小于预设倍数差，得到若干个左转子段；利用二分法对所述右转段进行迭代分割，直至所得到的右转子段的端点曲率的倍数差小于所述预设倍数差，得到若干个右转子段；以所有所述轨迹点作为根节点，以所述左转段、所述右转段和所述直行轨迹段作为所述根节点的子节点，以第1次迭代得到的所述左转子段作为所述左转段的子节点，以第N次迭代得到的所述左转子段作为第N
‑
1次迭代得到的所述左转子段的子节点，以第1次迭代得到的所述右转子段作为所述右转段的子节点，以第N次迭代得到的所述右转子段作为第N
‑
1次迭代得到的所述右转子段的子节点，并建立所述左转段、所述右转段、所述左转子段和所述右转子段的圆弧包围盒，建立所述直行轨迹段的矩形包围盒，得到轨迹包围盒树；其中，N大于1；建立包含所有所述障碍物的AABB包围盒；对所述AABB包围盒进行迭代分割，直至分割得到的空间内仅包含一个障碍物；以所有所述障碍物作为根节点，以第1次迭代得到的所述空间内的障碍物作为所述根节点的子节点，以第N次迭代得到的所述空间内的障碍物作为第N
‑
1次迭代得到的所述空间内的障碍物的子节点，并建立仅包含一个障碍物的子节点的OBB包围盒，建立包含若干个障碍物的子节点的AABB包围盒，得到障碍物包围盒树；对所述轨迹包围盒树的轨迹包围盒和所述障碍物包围盒树的障碍物包围盒进行碰撞检测，确定若干个碰撞检测对；所述碰撞检测对由所述轨迹包围盒树的叶子节点的轨迹包围盒与所述障碍物包围盒树的叶子节点的障碍物包围盒组成；对每一所述碰撞检测对进行碰撞检测，确定车辆行驶轨迹是否与障碍物发生碰撞，并在发生碰撞时，确定碰撞位置和碰撞时间。2.根据权利要求1所述的一种基于轨迹基元分割的高效碰撞检测方法，其特征在于，所述根据所述轨迹点的曲率将所述轨迹点划分为左转基元、右转基元和直行基元具体包括：若所述轨迹点的曲率大于或等于第一预设值且小于第二预设值，则所述轨迹点为右转基元；若所述轨迹点的曲率大于或等于所述第二预设值且小于或等于第三预设值，则所述轨迹点为直行基元；若所述轨迹点的曲率大于所述第三预设值且小于或等于第四预设值，则所述轨迹点为左转基元。
3.根据权利要求1所述的一种基于轨迹基元分割的高效碰撞检测方法，其特征在于，所述利用二分法对所述左转段进行迭代分割，直至所得到的左转子段的端点曲率的倍数差小于预设倍数差具体包括：以所述左转段作为初始段；利用二分法对所述初始段进行分割，得到第一子段和第二子段；判断所述第一子段的端点曲率的倍数差是否小于预设倍数差；若是，则结束对所述第一子段的分割过程；若否，则以所述第一子段作为下一迭代的初始段，返回“利用二分法对所述初始段进行分割”的步骤；判断所述第二子段的端点曲率的倍数差是否小于所述预设倍数差；若是，则结束对所述第二子段的分割过程；若否，则以所述第二子段作为下一迭代的初始段，返回“利用二分法对所述初始段进行分割”的步骤；第N次迭代得到的所述第一子段和所述第二子段即为第N次迭代得到的左转子段。4.根据权利要求1所述的一种基于轨迹基元分割的高效碰撞检测方法，其特征在于，建立所述左转段的圆弧包围盒具体包括：计算所述左转段的起始轨迹点和终止轨迹点的曲率的平均值，并选取所述左转段中曲率与所述平均值的差值最小的中间轨迹点作为目标轨迹点，确定同时经过所述起始轨迹点、所述目标轨迹点和所述终止轨迹点的圆的圆心；对于所述左转段中的每一轨迹点，根据所述轨迹点的位置以及航向，利用矩形拟合车辆外形；定义车辆行驶方向为前，靠近所述圆心的一侧为内侧；计算每一所述轨迹点的车辆外形的外侧前端的顶点与所述圆心的距离，并选取所述距离的最大值作为距离最大值；对于所述左转段中的每一轨迹点，计算所述圆心和所述轨迹点的连线与所述圆心到所述轨迹点的航向的垂线之间的夹角，并计算所述圆心到所述夹角最小的轨迹点的车辆外形的内侧边的距离，得到距离最小值；分别连接所述圆心与所述起始轨迹点的车辆外形的后端的两个顶点，选取符合第一预设条件的连线作为起始边界；分别连接所述圆心与所述终止轨迹点的车辆外形的前端的两个顶点，选取符合所述第一预设条件的连线作为终止边界；所述第一预设条件为所述左转段中的所有所述轨迹点的车辆外形位于所述连线的同一侧；根据所述起始边界、所述终止边界、所述距离最小值和所述距离最大值建立所述左转段的圆弧包围盒。5.根据权利要求1所述的一种基于轨迹基元分割的高效碰撞检测方法，其特征在于，所述建立所述直行轨迹段的矩形包围盒具体包括：对于所述直行轨迹段中的每一轨迹点，根据所述轨迹点的位置以及航向，利用矩形拟合车辆外形；以包围所述直行轨迹段中的所有所述轨迹点的车辆外形且长边与所述直行轨迹段中的曲率绝对值最小的轨迹点的航向平行的最小矩形作为所述直行轨迹段的矩形包围盒。6.根据权利要求1所述的一种基于轨迹基元分割的高效碰撞检测方法，其特征在于，所述对所述AABB包围盒进行迭代分割，直至分割得到的空间内仅包含一个障碍物具体包括：以所述AABB包围盒作为初始包围盒；
利用所述初始包围盒的长边的垂直平分线对所述初始包围盒进行分割，得到第一空间和第二空间；判断所述第一空间是否仅包含一个障碍物；若是，则结束对所述第一空间的分割过程；若否，则建立包含所述第一空间内的障碍物的第一AABB包围盒，并以所述第一AABB包围盒作为下一迭代中的初始包围盒，返回“利用所述初始包围盒的长边的垂直平分线对所述初始包围盒...

【专利技术属性】
技术研发人员：吴绍斌，闫泽新，李世豪，龚建伟，齐建永，王博洋，陈恺宇，耿家琳，
申请(专利权)人：慧动星球北京科技有限公司，
类型：发明
国别省市：