CN109960261B - 一种基于碰撞检测的动态障碍物避让方法 - Google Patents
一种基于碰撞检测的动态障碍物避让方法 Download PDFInfo
- Publication number
- CN109960261B CN109960261B CN201910222739.XA CN201910222739A CN109960261B CN 109960261 B CN109960261 B CN 109960261B CN 201910222739 A CN201910222739 A CN 201910222739A CN 109960261 B CN109960261 B CN 109960261B
- Authority
- CN
- China
- Prior art keywords
- vehicle
- track
- path
- collision
- collision detection
- 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.)
- Active
Links
- 238000001514 detection method Methods 0.000 title claims abstract description 86
- 238000000034 method Methods 0.000 title claims abstract description 46
- 230000004888 barrier function Effects 0.000 claims abstract description 34
- 238000005070 sampling Methods 0.000 claims description 22
- 230000001133 acceleration Effects 0.000 claims description 9
- 206010048669 Terminal state Diseases 0.000 claims description 3
- 239000011159 matrix material Substances 0.000 claims description 3
- 230000000737 periodic effect Effects 0.000 claims description 3
- 230000000694 effects Effects 0.000 abstract description 3
- 238000010586 diagram Methods 0.000 description 7
- 241000208199 Buxus sempervirens Species 0.000 description 4
- 230000036461 convulsion Effects 0.000 description 4
- 238000005516 engineering process Methods 0.000 description 4
- 238000004364 calculation method Methods 0.000 description 3
- 238000005457 optimization Methods 0.000 description 3
- 230000003068 static effect Effects 0.000 description 2
- 238000004458 analytical method Methods 0.000 description 1
- 230000009286 beneficial effect Effects 0.000 description 1
- 239000000470 constituent Substances 0.000 description 1
- 230000004927 fusion Effects 0.000 description 1
- 238000009499 grossing Methods 0.000 description 1
- 238000012544 monitoring process Methods 0.000 description 1
- 238000012887 quadratic function Methods 0.000 description 1
- 238000006467 substitution reaction Methods 0.000 description 1
- 230000002123 temporal effect Effects 0.000 description 1
Images
Classifications
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
- G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
- G01C21/26—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 specially adapted for navigation in a road network
- G01C21/34—Route searching; Route guidance
- G01C21/3407—Route searching; Route guidance specially adapted for specific applications
- G01C21/3415—Dynamic re-routing, e.g. recalculating the route when the user deviates from calculated route or after detecting real-time traffic data or accidents
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
- G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
- G01C21/26—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 specially adapted for navigation in a road network
- G01C21/34—Route searching; Route guidance
- G01C21/3446—Details of route searching algorithms, e.g. Dijkstra, A*, arc-flags, using precalculated routes
-
- G—PHYSICS
- G05—CONTROLLING; REGULATING
- G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
- G05D1/00—Control of position, course, altitude or attitude of land, water, air or space vehicles, e.g. using automatic pilots
- G05D1/02—Control of position or course in two dimensions
- G05D1/021—Control of position or course in two dimensions specially adapted to land vehicles
- G05D1/0212—Control of position or course in two dimensions specially adapted to land vehicles with means for defining a desired trajectory
- G05D1/0214—Control of position or course in two dimensions specially adapted to land vehicles with means for defining a desired trajectory in accordance with safety or protection criteria, e.g. avoiding hazardous areas
-
- G—PHYSICS
- G05—CONTROLLING; REGULATING
- G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
- G05D1/00—Control of position, course, altitude or attitude of land, water, air or space vehicles, e.g. using automatic pilots
- G05D1/02—Control of position or course in two dimensions
- G05D1/021—Control of position or course in two dimensions specially adapted to land vehicles
- G05D1/0212—Control of position or course in two dimensions specially adapted to land vehicles with means for defining a desired trajectory
- G05D1/0221—Control of position or course in two dimensions specially adapted to land vehicles with means for defining a desired trajectory involving a learning process
-
- G—PHYSICS
- G05—CONTROLLING; REGULATING
- G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
- G05D1/00—Control of position, course, altitude or attitude of land, water, air or space vehicles, e.g. using automatic pilots
- G05D1/02—Control of position or course in two dimensions
- G05D1/021—Control of position or course in two dimensions specially adapted to land vehicles
- G05D1/0212—Control of position or course in two dimensions specially adapted to land vehicles with means for defining a desired trajectory
- G05D1/0223—Control of position or course in two dimensions specially adapted to land vehicles with means for defining a desired trajectory involving speed control of the vehicle
-
- G—PHYSICS
- G05—CONTROLLING; REGULATING
- G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
- G05D1/00—Control of position, course, altitude or attitude of land, water, air or space vehicles, e.g. using automatic pilots
- G05D1/02—Control of position or course in two dimensions
- G05D1/021—Control of position or course in two dimensions specially adapted to land vehicles
- G05D1/0276—Control of position or course in two dimensions specially adapted to land vehicles using signals provided by a source external to the vehicle
Landscapes
- Engineering & Computer Science (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- Automation & Control Theory (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Aviation & Aerospace Engineering (AREA)
- Traffic Control Systems (AREA)
Abstract
本发明涉及一种基于碰撞检测的动态障碍物避让方法,属于无人车辆技术领域,通过初规划构造车辆期望轨迹,并根据接收的动态障碍物预测轨迹和所述车辆期望轨迹,进行碰撞检测,对车辆的行驶速度和/或路径进行重新规划,得到车辆无碰撞行驶速度和/或路径数据,重新构造车辆期望轨迹,避让动态障碍物;实现无人车辆自主避让动态障碍物。本发明立足于自动驾驶车辆在动态环境下,所采用碰撞检测方法保证既不失碰撞检测精度,又可以提升碰撞检测效率;所采用重规划方法可以兼顾通行效率、通行平顺性和通行安全性效果。
Description
技术领域
本发明涉及无人车辆技术领域,尤其涉及一种基于碰撞检测的动态障碍物避让方法。
背景技术
无人驾驶技术作为当今车辆的前沿技术受到了各国学者的广泛关注。无人车辆的运动规划技术是无人驾驶车辆的核心技术之一,可被细分为路径规划与速度规划。其中路径规划系统的目标是实现无人驾驶车辆在空间可行区域的行驶,速度规划系统的目标是实现无人驾驶车辆对于时间可行区域的行驶。
目前对于无人驾驶车辆运动规划系统大部分只针对静态障碍物的处理,对于动态障碍物大多采用近似将其看做静态障碍物作反应式躲避处理。但由于动态障碍物具有一定的速度,反应式处理往往导致避让动态障碍物失败。
发明内容
鉴于上述的分析,本发明旨在提供一种基于碰撞检测的动态障碍物避让方法,实现无人车自主避让动态障碍物。
本发明的目的主要是通过以下技术方案实现的:
一种基于碰撞检测的动态障碍物避让方法,包括:
初规划步骤:在车辆行驶过程中对车辆的行驶路径和速度进行初步规划,得到车辆的期望路径、期望速度数据,构造车辆期望轨迹;
碰撞检测步骤:基于接收的动态障碍物预测轨迹和所述车辆期望轨迹,进行碰撞检测,判断车辆是否会与障碍物发生碰撞;
重规划步骤;当判断会发生碰撞后,对车辆的行驶速度和/或路径进行重新规划,得到车辆无碰撞行驶速度和/或路径数据,重新构造车辆期望轨迹,避让动态障碍物。
进一步地,所述碰撞检测为分层碰撞检测,包括二维空间碰撞检测和三维时间空间碰撞检测。
进一步地,所述碰撞检测中的二维空间碰撞检测,根据车辆期望轨迹的路径数据和动态障碍物预测轨迹的路径数据,计算车辆与障碍物的最近距离,若最近距离小于碰撞距离阈值,则进行三维时间空间碰撞检测;否则,判断不会发生碰撞。
进一步地,所述碰撞检测中的三维时间空间碰撞检测包括:
构造三维时空轨迹;三维时空的x轴和y轴为路径点的横、纵坐标,z轴为到达每个路径点的时间;在三维时空中,根据动态障碍物的几何信息、预测轨迹数据以及车辆的几何信息、期望轨迹数据,得到动态障碍物预测轨迹和车辆期望轨迹的点阵序列;
粗略检测;根据动态障碍物预测轨迹和车辆期望轨迹的点阵序列,进行三维重构,形成以三维轨迹点为边界的动态障碍物三维轨迹凸体和车辆三维轨迹凸体,对两个三维轨迹凸体之间的冲突区域进行检测,得到粗略的发生碰撞的轨迹点;
精细检测;锁定冲突区域,进行精细阶段的碰撞检测,得到精确的发生碰撞的轨迹点。
进一步地,所述重规划步骤中,当判断会与动态障碍物轨迹发生碰撞时,
1)首先启动速度重规划,进行纵向时间层面的状态空间采样,基于同一条路径形成多条不同时间序列的轨迹,与动态障碍物进行碰撞检测,得到车辆无碰撞速度数据,控制车辆避让动态障碍物;
2)若全部轨迹都发生碰撞,则此时切换成路径重规划,进行横向空间层面的状态空间采样,形成多条基于不同路径的轨迹,并与动态障碍物进行碰撞检测,得到车辆无碰撞行进路径和速度数据,控制车辆避让动态障碍物;
3)若全部轨迹都发生碰撞,则计算当前所处轨迹与动态障碍物碰撞的最近距离,基于当前所处轨迹减速停车。
进一步地,所述粗略检测阶段,采用基于BVH的快速碰撞检测方法;所述精细检测阶段,采用GJK算法进行碰撞检测。
进一步地,所述初规划步骤中行驶路径的初步规划为根据车辆当前状态,生成一条从车辆当前位置到参考线的行驶曲线,具体包括:
1)给定车辆当前车辆状态qinit(xinit,yinit,θinit,κinit)、车辆终点状态qend(xend,yend,θend,κend);其中xinit,yinit为车辆当前的x,y坐标,θinit为车辆当前航向角,κinit为车辆当前轨迹曲率;其中xend,yend为车辆终点位于参考线上的x,y坐标,θend为车辆终点航向角,κend为车辆终点轨迹曲率;
函数κ(p)的参数a为已知的车辆当前状态所对应的曲率,q为车辆状态;通过求解得到函数κ(p)的参数b、c、d、sf;
进一步地,所述初规划步骤中行驶速度的初步规划为包括:
2)对每个路径点限制横向加速度和纵向加、减速度:
3)根据相邻三个路径点进行冲击度计算,得到每个路径点的冲击度ji;
4)判断冲击度ji值,当冲击度ji>jmax时,根据最大冲击度jmax重新确定该点的速度,当冲击度ji<-jmax时,根据最小冲击度-jmax重新确定该点的速度;得到周期速度序列Vf;
5)计算本周期速度序列Vf与上周期的差值,判断差值是否小于输入的阈值ε;如果小于则生成速度曲线输出;否则,返回2)再次进行速度迭代;其中初始周期的路径点速度值为其对应的最大速度值νmax。
进一步地,所述速度重规划方法包括:
1)将初步规划产生的轨迹作为初始轨迹;
2)进行纵向的状态空间采样,形成同一空间,不同时间的多条车辆期望轨迹;
3)将多条车辆期望轨迹与动态障碍物轨迹进行碰撞检测;
4)判断碰撞检测结果,如存在无碰撞期望轨迹,则将无碰撞最优轨迹作为最终轨迹输出;如果不存在无碰撞期望轨迹,则进行路径重规划。
进一步地,所述路径重规划方法包括:
1)根据参考线,进行横向状态空间采样,得到终点状态集qend(θend,κend,xend,yend),
2)根据初步规划中的路径规划法生成多条根据车辆当前位置到多个目标采样状态的路径;
3)将多条路径按照初步规划中的速度规划法形成多条轨迹,与动态障碍物轨迹进行碰撞检测;
4)如果存在无碰撞轨迹,则选取最优无碰撞轨迹,作为最终轨迹输出;如果不存在无碰撞期望轨迹,则计算当前所处轨迹与动态障碍物碰撞的最近距离,基于当前所处轨迹减速停车。
本发明至少可实现如下有益效果之一:
本发明立足于自动驾驶车辆在动态环境下,提出了一套自动驾驶车辆动态障碍物避让框架。
在初步运动规划阶段,采用非线性优化和基于迭代约束的方法生成期望轨迹,实现自动驾驶车辆能够在无动态障碍物的情况下生成一条平滑、舒适的局部行驶轨迹;
在存在动态障碍物时,提出采用构造融合碰撞检测的方法,首先在二维空间筛选无碰撞轨迹,为三维碰撞检测减小计算负担,然后在三维空间构造层次包围盒树(BVH)的方法进行快速碰撞检测,保证既不失碰撞检测精度,又可以提升碰撞检测效率;
在避让动态障碍物阶段,本方法提出速度重规划优先的思路,优先采用纵向状态空间采样,进行速度重规划,选取最贴近原期望轨迹但又与动态障碍物轨迹不发生碰撞的速度重规划轨迹进行动态障碍物躲避,实现通行效率与平顺性的效果。
当纵向速度曲线束全部与动态障碍物轨迹发生碰撞时,进行路径重规划,选取最贴近原期望轨迹但又与动态障碍物轨迹不发生碰撞的路径重规划轨迹,实现安全躲避的效果。
附图说明
附图仅用于示出具体实施例的目的,而并不认为是对本发明的限制,在整个附图中,相同的参考符号表示相同的部件。
图1为本发明实施例中的动态障碍物避让方法流程图;
图2为本发明实施例中的速度规划方法流程图;
图3为本发明实施例中的二维空间碰撞检测示例图;
图4为本发明实施例中的构造三维时空轨迹示例图;
图5为本发明实施例中的BVH层次包围盒树示例图;
图6为本发明实施例中的粗略阶段确定的存在碰撞的区域示意图;
图7为本发明实施例中的精细阶段确定的存在碰撞的区域示意图;
图8为本发明实施例中的动态障碍物避让方法细节流程图。
具体实施方式
下面结合附图来具体描述本发明的优选实施例,其中,附图构成本申请一部分,并与本发明的实施例一起用于阐释本发明的原理。
本发明实施例公开了一种基于碰撞检测的动态障碍物避让方法,可用于无人车实时躲避动态障碍物的运动规划。
如图1所示,具体方法包括:
S101、初规划步骤:在车辆行驶过程中对车辆的行驶路径和速度进行初步规划,得到车辆的期望路径、期望速度数据,构造车辆期望轨迹;
S102、碰撞检测步骤:基于接收的动态障碍物预测轨迹和所述车辆期望轨迹,进行碰撞检测,判断车辆是否会与障碍物发生碰撞;
对于具有动态障碍物监测功能的无人车辆通常能够探测到的动态障碍物,并预测出动态障碍物的运动路径和运动速度,构造出动态障碍物预测轨迹。本实施例中对动态障碍物轨迹预测可采用目前公开文献中的方法。
S103、重规划步骤;当判断会发生碰撞后,对车辆的行驶速度和/或路径进行重新规划,得到车辆无碰撞行驶速度和/或路径数据,重新构造车辆期望轨迹,避让动态障碍物。
具体的,在初规划步骤中,对无人车的正常行进中局部路径和速度进行规划;
对于路径规划采用基于参考线根据非线性优化生成局部路径;
其中,参考线是根据无人车当前位置,从无人车辆全局路径规划结果中截取的一段局部参考路径;
其中,全局路径规划可以在无人车辆行驶之前,根据设置的车辆起点、车辆终点以及起点到终点的道路情况,进行事先规划并存储于无人车辆中,用于引导车辆行驶。
在无人车行驶过程中,根据车辆当前状态去生成一条从车辆当前位置到参考线的曲线。对曲线上路径点的X坐标、Y坐标、车辆航向角θ和曲线曲率κ进行规划。
通常将车辆的纵向运动描述成一维的非线性方程:s=f(t);
根据弧长s可以表示曲线上的x的坐标:
同理可得:
对于航向角:
由此可以得到,
将曲线上的曲率表示成随弧长变化的三次曲线:
κ(s)=a+bs+cs2+ds3
笛卡尔坐标系下路径的弧长参数化表达形式为:
r(p)=(x(p),y(p))
s.t.
以车辆当前位置作为原点,因此x0=y0=0,a为已知的车辆当前状态所对应的曲率,b、c、d、sf是需要优化的变量,sf同时也是到目标状态的弧长。
因此,给定当前车辆状态qinit和车辆终点状态qend,其中车辆终点状态可以在参考线上选取,路径生成问题可表示为:
给定qinit=(xinit,yinit,θinit,kinit)
qend=(xend,yend,θend,κend);
s.t.q=qinit,s=0;
q=qend,s=sf;
0≤sf≤smax;
对于速度规划,
考虑到实车应用的时效性,采用基于多约束的迭代方法进行正常的速度规划。如图2所示,具体包括如下步骤:
2)对每个路径点限制横向加速度和纵向加、减速度:
3)根据相邻三个路径点进行冲击度计算,得到每个路径点的冲击度ji;
假设可以插入一个基于弧长变量的速度二次函数:
ν=α·s2+β·s+γ
则:
其中s是弧长,v是速度,从上面的函数表示可以得到,当未知变量α、β、γ计算出来后,冲击度j就可以计算表示出来。
因此在计算第i个路点的冲击度jerk的时候,我们可以根据前一个点i-1以及后一个点i+1的信息,计算三个未知量(其中首点和末点jerk均为0):
也就得到:
4)根据最大冲击度jerk来检查每个点的速度是否满足要求
判断冲击度ji值,当冲击度ji>jmax时,根据最大冲击度jmax重新确定该点的速度,当冲击度ji<-jmax时,根据最小冲击度-jmax重新确定该点的速度;得到周期速度序列Vf;
当ji>jmax时:
当ji<-jmax时:
5)计算本周期速度序列Vf与上周期的差值判断差值是否小于输入的阈值ε;如果小于则生成速度曲线输出;否则,返回2)再次进行速度迭代,其中每个路径点迭代初始速度值为其对应的最大速度值νmax。
本实施例中的碰撞检测为分层碰撞检测,包括二维空间碰撞检测和三维时间空间碰撞检测。
具体的,二维空间碰撞检测,根据车辆期望轨迹的路径数据和动态障碍物预测轨迹的路径数据,计算车辆与障碍物的最近距离,若最近距离小于碰撞距离阈值,则进行三维时间空间碰撞检测;否则,判断不会发生碰撞。
可选的,碰撞距离阈值可选车辆和障碍物二者的半径和,二维空间碰撞检测示例图如图3所示。
具体的,三维时间空间碰撞检测可包括:
构造三维时空轨迹;三维时空的x轴和y轴为路径点的横、纵坐标,z轴为到达每个路径点的时间;在三维时空中,根据动态障碍物的几何信息、预测路径、预测速度数据以及车辆的几何信息、期望路径、期望速度数据,得到动态障碍物预测轨迹和车辆期望轨迹的点阵序列;
如图4所示,为构造三维时空轨迹示例。
粗略检测;根据动态障碍物预测轨迹和车辆期望轨迹的点阵序列,进行三维重构,形成以三维轨迹点为边界的动态障碍物三维轨迹凸体和车辆三维轨迹凸体,其中基本构成单元为三角片。对两个三维轨迹凸体之间的冲突区域进行检测,得到粗略的发生碰撞的轨迹点。
优选的,粗略检测采用基于BVH的快速碰撞检测方法,根据动态障碍物的轨迹凸体以及无人驾驶车辆的轨迹凸体,形成BVH层次包围盒树;如图5所示。BVH迭代的终止条件为每个叶节点只包含一个三角面片单元。因为层次包围盒树是二叉树结构,具有二叉树的一系列特点,在根节点不发生碰撞的情况下,其子节点也一定不会发生碰撞,可以通过此特点加速碰撞检测的进行。
如图6所示,为粗略阶段确定的存在碰撞的区域示意图。
精细检测;锁定冲突区域,进行精细阶段的碰撞检测,得到精确的发生碰撞的轨迹点。
其中精细阶段的碰撞检测采用GJK算法,从而得到真正发生碰撞的轨迹点。如图7所示,为精细阶段确定的存在碰撞的区域示意图。
具体的,本实施例的重规划步骤中,当判断会与动态障碍物轨迹发生碰撞时,
1)首先启动速度重规划,进行纵向时间层面的状态空间采样,基于同一条路径形成多条不同时间序列的轨迹,与动态障碍物进行碰撞检测,得到车辆无碰撞速度数据,控制车辆避让动态障碍物;
2)若全部轨迹都发生碰撞,则此时切换成路径重规划,进行横向空间层面的状态空间采样,形成多条基于不同路径的轨迹,并与动态障碍物进行碰撞检测,得到车辆无碰撞行进路径和速度数据,控制车辆避让动态障碍物;
3)若全部轨迹都发生碰撞,则计算当前所处轨迹与动态障碍物碰撞的最近距离,基于当前所处轨迹减速停车。
优选的,速度重规划方法包括:
1)将正常速度规划产生的轨迹作为初始轨迹;
2)进行纵向的状态空间采样,形成同一空间,不同时间的多条车辆期望轨迹;
3)将多条车辆期望轨迹与动态障碍物轨迹进行碰撞检测;
碰撞检测优选三维时间空间碰撞检测;
4)判断碰撞检测结果,如存在无碰撞期望轨迹,则将无碰撞最优轨迹作为最终轨迹输出;如果不存在无碰撞期望轨迹,则进行路径重规划。
在速度重规划中,碰撞最优无轨迹的选取,按照纵向上终点时间与原期望轨迹偏差最小原则进行选取。
更具体的,由于纵向状态空间采样的终点状态为qend(s,t),基于约束迭代的速度规划在期望速度曲线确定前无法确定到达终点的最终时间,所以无法将终点状态qend作为约束进行迭代计算。因此,速度重规划采用二次规划的优化方法进行速度曲线生成。与路径规划表示路径的曲线模型相似,可以将车辆的纵向运动表示为关于弧长关于时间的五次多项式:s=f(t)=a+bt+ct2+dt3+et4+ft5;
s.t.
1)初始状态:qinit=(sinit,tinit,vinit)
2)末端状态:qend=(send,tend),
3)上下限约束:在tinit和tend间采样n个点:
求解后得到[a b c d e f]T即可得到速度曲线表达式。
优选的,路径重规划算法包括:
2)根据正常路径规划法生成多条根据车辆当前位置到多个目标采样状态的路径;
3)将多条路径按照正常速度进行速度规划,形成多条轨迹,与动态障碍物轨迹进行碰撞检测;
4)如果存在无碰撞轨迹,则选取最优无碰撞轨迹,作为最终轨迹输出;如果不存在无碰撞期望轨迹,则计算当前所处轨迹与动态障碍物碰撞的最近距离,基于当前所处轨迹减速停车。
在路径重规划中,最优无碰撞轨迹的选取,按照与初规划轨迹(原期望轨迹)横向上距离偏差最小的原则进行选取。
如图8所示,为本实施例基于碰撞检测的动态障碍物避让方法更为细节的流程示意图。
以上所述,仅为本发明较佳的具体实施方式,但本发明的保护范围并不局限于此,任何熟悉本技术领域的技术人员在本发明揭露的技术范围内,可轻易想到的变化或替换,都应涵盖在本发明的保护范围之内。
Claims (9)
1.一种基于碰撞检测的动态障碍物避让方法,其特征在于,包括:
初规划步骤:在车辆行驶过程中对车辆的行驶路径和速度进行初步规划,得到车辆的期望路径、期望速度数据,构造车辆期望轨迹;
碰撞检测步骤:基于接收的动态障碍物预测轨迹和所述车辆期望轨迹,进行碰撞检测,判断车辆是否会与障碍物发生碰撞;
重规划步骤:当判断会发生碰撞后,对车辆的行驶速度和/或路径进行重新规划,得到车辆无碰撞行驶速度和/或路径数据,重新构造车辆期望轨迹,避让动态障碍物;
所述初规划步骤中行驶路径的初步规划为根据车辆当前状态,生成一条从车辆当前位置到参考线的行驶曲线,具体包括:
1)给定车辆当前车辆状态qinit(xinit,yinit,θinit,κinit)、车辆终点状态qend(xend,yend,θend,κend);其中xinit,yinit为车辆当前的x,y坐标,θinit为车辆当前航向角,κinit为车辆当前轨迹曲率;其中xend,yend为车辆终点位于参考线上的x,y坐标,θend为车辆终点航向角,κend为车辆终点轨迹曲率;
s.t.q=qinit,s=0;
q=qend,s=sf;
0≤sf≤smax;smax为曲线弧长的上界;
函数κ(p)的参数a为已知的车辆当前状态所对应的曲率,q为车辆状态;通过求解得到函数κ(p)的参数b、c、d、sf;
2.根据权利要求1所述的动态障碍物避让方法,其特征在于,所述碰撞检测为分层碰撞检测,包括二维空间碰撞检测和三维时间空间碰撞检测。
3.根据权利要求2所述的动态障碍物避让方法,其特征在于,
所述碰撞检测中的二维空间碰撞检测,根据车辆期望轨迹的路径数据和动态障碍物预测轨迹的路径数据,计算车辆与障碍物的最近距离,若最近距离小于碰撞距离阈值,则进行三维时间空间碰撞检测;否则,判断不会发生碰撞。
4.根据权利要求2所述的动态障碍物避让方法,其特征在于,所述碰撞检测中的三维时间空间碰撞检测包括:
构造三维时空轨迹;三维时空的x轴和y轴为路径点的横、纵坐标,z轴为到达每个路径点的时间;在三维时空中,根据动态障碍物的几何信息、预测轨迹数据以及车辆的几何信息、期望轨迹数据,得到动态障碍物预测轨迹和车辆期望轨迹的点阵序列;
粗略检测;根据动态障碍物预测轨迹和车辆期望轨迹的点阵序列,进行三维重构,形成以三维轨迹点为边界的动态障碍物三维轨迹凸体和车辆三维轨迹凸体,对两个三维轨迹凸体之间的冲突区域进行检测,得到粗略的发生碰撞的轨迹点;
精细检测;锁定冲突区域,进行精细阶段的碰撞检测,得到精确的发生碰撞的轨迹点。
5.根据权利要求4所述的动态障碍物避让方法,其特征在于,所述重规划步骤中,当判断会与动态障碍物轨迹发生碰撞时,
1)首先启动速度重规划,进行纵向时间层面的状态空间采样,基于同一条路径形成多条不同时间序列的轨迹,与动态障碍物进行碰撞检测,得到车辆无碰撞速度数据,控制车辆避让动态障碍物;
2)若全部轨迹都发生碰撞,则此时切换成路径重规划,进行横向空间层面的状态空间采样,形成多条基于不同路径的轨迹,并与动态障碍物进行碰撞检测,得到车辆无碰撞行进路径和速度数据,控制车辆避让动态障碍物;
3)若全部轨迹都发生碰撞,则计算当前所处轨迹与动态障碍物碰撞的最近距离,基于当前所处轨迹减速停车。
6.根据权利要求5所述的动态障碍物避让方法,其特征在于,所述粗略检测阶段,采用基于BVH的快速碰撞检测方法;所述精细检测阶段,采用GJK算法进行碰撞检测。
7.根据权利要求1所述的动态障碍物避让方法,其特征在于,所述初规划步骤中行驶速度的初步规划包括:
2)对每个路径点限制横向加速度和纵向加、减速度:
3)根据相邻三个路径点进行冲击度计算,得到每个路径点的冲击度ji;
4)判断冲击度ji值,当冲击度ji>jmax时,根据最大冲击度jmax重新确定该点的速度,当冲击度ji<-jmax时,根据最小冲击度-jmax重新确定该点的速度;得到周期速度序列Vf;
5)计算本周期速度序列Vf与上周期的差值,判断差值是否小于输入的阈值ε;如果小于则生成速度曲线输出;否则,返回2)再次进行速度迭代;其中初始周期的路径点速度值为其对应的最大速度值νmax。
8.根据权利要求1所述的动态障碍物避让方法,其特征在于,所述速度重规划方法包括:
1)将初步规划产生的轨迹作为初始轨迹;
2)进行纵向的状态空间采样,形成同一空间,不同时间的多条车辆期望轨迹;
3)将多条车辆期望轨迹与动态障碍物轨迹进行碰撞检测;
4)判断碰撞检测结果,如存在无碰撞期望轨迹,则将无碰撞最优轨迹作为最终轨迹输出;如果不存在无碰撞期望轨迹,则进行路径重规划。
9.根据权利要求8所述的动态障碍物避让方法,其特征在于,所述路径重规划方法包括:
1)根据参考线,进行横向状态空间采样,得到终点状态集qend(θend,κend,xend,yend),
2)根据初步规划中的路径规划法生成多条根据车辆当前位置到多个目标采样状态的路径;
3)将多条路径按照初步规划中的速度规划法形成多条轨迹,与动态障碍物轨迹进行碰撞检测;
4)如果存在无碰撞轨迹,则选取最优无碰撞轨迹,作为最终轨迹输出;如果不存在无碰撞期望轨迹,则计算当前所处轨迹与动态障碍物碰撞的最近距离,基于当前所处轨迹减速停车。
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN201910222739.XA CN109960261B (zh) | 2019-03-22 | 2019-03-22 | 一种基于碰撞检测的动态障碍物避让方法 |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN201910222739.XA CN109960261B (zh) | 2019-03-22 | 2019-03-22 | 一种基于碰撞检测的动态障碍物避让方法 |
Publications (2)
Publication Number | Publication Date |
---|---|
CN109960261A CN109960261A (zh) | 2019-07-02 |
CN109960261B true CN109960261B (zh) | 2020-07-03 |
Family
ID=67024646
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
CN201910222739.XA Active CN109960261B (zh) | 2019-03-22 | 2019-03-22 | 一种基于碰撞检测的动态障碍物避让方法 |
Country Status (1)
Country | Link |
---|---|
CN (1) | CN109960261B (zh) |
Families Citing this family (57)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN110488816B (zh) * | 2019-08-06 | 2021-12-31 | 华为技术有限公司 | 自动驾驶纵向规划方法及相关设备 |
CN110497906B (zh) * | 2019-08-30 | 2021-11-02 | 北京百度网讯科技有限公司 | 车辆控制方法、装置、设备和介质 |
CN112541371A (zh) * | 2019-09-18 | 2021-03-23 | 财团法人车辆研究测试中心 | 目标物意图预测方法及其系统 |
CN110466524B (zh) * | 2019-09-18 | 2021-04-02 | 北京小马慧行科技有限公司 | 车辆控制方法、装置和系统、存储介质及处理器 |
GB2588229A (en) | 2019-10-18 | 2021-04-21 | Sony Interactive Entertainment Inc | Navigation system and method |
CN110834631A (zh) * | 2019-11-01 | 2020-02-25 | 中国第一汽车股份有限公司 | 一种行人避让方法、装置、车辆及存储介质 |
CN111091591B (zh) * | 2019-12-23 | 2023-09-26 | 阿波罗智联(北京)科技有限公司 | 一种碰撞检测方法、装置、电子设备及存储介质 |
CN113119964B (zh) | 2019-12-30 | 2022-08-02 | 宇通客车股份有限公司 | 一种自动驾驶车辆碰撞预测判断方法及装置 |
CN114489044A (zh) * | 2019-12-31 | 2022-05-13 | 华为技术有限公司 | 一种轨迹规划方法及装置 |
CN111176285B (zh) * | 2020-01-02 | 2024-04-16 | 北京汽车集团有限公司 | 一种行进路径规划的方法及装置、车辆、可读存储介质 |
CN111243309A (zh) * | 2020-01-10 | 2020-06-05 | 北京航空航天大学 | 一种基于自动驾驶车辆移动检测的快速路交通流全样本轨迹重构方法 |
CN111469836B (zh) * | 2020-02-28 | 2022-12-20 | 广东中科臻恒信息技术有限公司 | 基于车载单元和路侧单元的避障方法、设备、存储介质 |
CN111402601A (zh) * | 2020-03-10 | 2020-07-10 | 合肥联宝信息技术有限公司 | 智能交通控制方法及系统 |
CN113031583B (zh) * | 2020-03-13 | 2022-11-25 | 青岛慧拓智能机器有限公司 | 一种结构化道路避障方法 |
CN111591307B (zh) * | 2020-04-15 | 2021-10-01 | 毫末智行科技有限公司 | 一种避障轨迹规划方法、系统及车辆 |
CN111694356B (zh) * | 2020-06-10 | 2023-06-27 | 上海高仙自动化科技发展有限公司 | 一种行驶控制方法、装置、电子设备及存储介质 |
EP3832232B1 (en) | 2020-06-11 | 2022-11-09 | Beijing Baidu Netcom Science And Technology Co., Ltd. | Refrigerating system |
CN111724598B (zh) * | 2020-06-29 | 2022-04-05 | 北京百度网讯科技有限公司 | 用于自动驾驶规划路径的方法、装置、设备以及存储介质 |
CN114076603B (zh) * | 2020-08-18 | 2024-04-16 | 财团法人车辆研究测试中心 | 轨迹决定方法 |
CN112305911B (zh) * | 2020-09-11 | 2023-03-03 | 北京智行者科技股份有限公司 | 一种复杂环境下的反馈预测控制方法及其装置、车辆 |
CN112193244B (zh) * | 2020-09-30 | 2021-07-20 | 浙江大学 | 基于线性约束的自动驾驶车辆运动规划方法 |
CN112269384B (zh) * | 2020-10-23 | 2021-09-14 | 电子科技大学 | 一种结合障碍物行为意图的车辆动态轨迹规划方法 |
CN112577506B (zh) * | 2020-10-30 | 2023-04-07 | 上汽大众汽车有限公司 | 一种自动驾驶局部路径规划方法和系统 |
CN112649011B (zh) * | 2020-12-01 | 2022-06-28 | 禾多科技(北京)有限公司 | 车辆避障方法、装置、设备和计算机可读介质 |
CN112631276A (zh) * | 2020-12-03 | 2021-04-09 | 长沙行深智能科技有限公司 | 一种无人车动态障碍决策方法、系统、介质及设备 |
CN112327886A (zh) * | 2020-12-04 | 2021-02-05 | 盐城中科高通量计算研究院有限公司 | 一种巡逻车用自主避障决策方法 |
CN112665592B (zh) * | 2020-12-16 | 2023-10-20 | 郑州大学 | 一种基于多智能体的时空路径规划方法 |
CN114637283B (zh) * | 2020-12-16 | 2024-07-26 | 宇通客车股份有限公司 | 一种基于动态避障的车辆路径规划方法及装置 |
CN112572443B (zh) * | 2020-12-22 | 2021-12-07 | 北京理工大学 | 一种高速公路车辆换道实时避撞轨迹规划方法及系统 |
CN112859845A (zh) * | 2021-01-04 | 2021-05-28 | 广州小鹏自动驾驶科技有限公司 | 一种轨迹的规划方法、装置和车辆 |
CN112810630B (zh) * | 2021-02-05 | 2022-03-04 | 山东大学 | 一种自动驾驶车辆轨迹规划方法及系统 |
CN112744217B (zh) * | 2021-03-10 | 2022-12-06 | 北京车和家信息技术有限公司 | 碰撞检测方法、行驶路径的推荐方法及其装置和存储介质 |
CN113077643B (zh) * | 2021-03-23 | 2022-05-03 | 上海振华重工(集团)股份有限公司 | 一种交通控制方法、交通控制系统及存储介质 |
CN113085850B (zh) * | 2021-06-08 | 2021-09-21 | 新石器慧通(北京)科技有限公司 | 车辆避障方法、装置、电子设备及存储介质 |
CN113267199B (zh) * | 2021-06-24 | 2023-01-06 | 上海欧菲智能车联科技有限公司 | 行驶轨迹规划方法及装置 |
CN113359763A (zh) * | 2021-07-05 | 2021-09-07 | 广州蓝胖子移动科技有限公司 | 轮式机器人及动态障碍物轨迹预测方法、装置、存储介质 |
CN113799797B (zh) * | 2021-07-27 | 2022-07-12 | 北京三快在线科技有限公司 | 轨迹规划方法、装置、存储介质及电子设备 |
CN113442924A (zh) * | 2021-08-03 | 2021-09-28 | 浙江吉利控股集团有限公司 | 一种车辆轨迹的规划方法及规划系统 |
CN113419546B (zh) * | 2021-08-24 | 2021-11-26 | 新石器慧通(北京)科技有限公司 | 一种无人车的控制方法、装置、介质及电子设备 |
CN113715814B (zh) * | 2021-09-02 | 2023-03-21 | 北京百度网讯科技有限公司 | 碰撞检测方法、装置、电子设备、介质及自动驾驶车辆 |
CN115993815A (zh) * | 2021-10-18 | 2023-04-21 | 灵动科技(北京)有限公司 | 用于移动机器人的运动控制方法及计算机程序产品 |
CN113815647B (zh) * | 2021-10-28 | 2023-03-14 | 广州文远知行科技有限公司 | 一种车辆速度规划方法、装置、设备和介质 |
CN114103893A (zh) * | 2021-11-26 | 2022-03-01 | 河北春玖智能科技有限公司 | 一种无人车的轨迹预测防撞方法 |
CN114379584B (zh) * | 2021-12-17 | 2023-12-22 | 阿波罗智联(北京)科技有限公司 | 车辆控制方法、装置、车辆、设备和存储介质 |
CN114155732B (zh) * | 2021-12-28 | 2023-04-21 | 东风汽车有限公司东风日产乘用车公司 | 车辆周围障碍物监测方法、电子设备及存储介质 |
CN114235441A (zh) * | 2021-12-31 | 2022-03-25 | 深兰人工智能(深圳)有限公司 | 无人车实时碰撞检测方法及相关装置 |
CN115525047B (zh) * | 2022-03-21 | 2023-07-25 | 江苏集萃清联智控科技有限公司 | 一种具备多型避障方式的车辆局部轨迹规划方法及系统 |
CN115123217B (zh) * | 2022-09-02 | 2022-11-25 | 青岛慧拓智能机器有限公司 | 矿区障碍车辆行使轨迹生成方法、装置及计算机设备 |
CN115237928B (zh) * | 2022-09-26 | 2022-12-06 | 北京理工大学 | 一种基于轨迹基元分割的高效碰撞检测方法及系统 |
CN116399352B (zh) * | 2023-04-06 | 2024-01-19 | 深圳市森歌数据技术有限公司 | 一种智慧无人停车场agv的路径规划方法、装置及存储介质 |
CN116263605B (zh) * | 2023-04-21 | 2024-06-21 | 杭州国辰机器人科技有限公司 | 基于非线性优化的移动机器人平滑、实时碰撞规避方法 |
CN116817955B (zh) * | 2023-06-30 | 2024-04-12 | 北京易航远智科技有限公司 | 车辆路径规划方法、装置、电子设备及存储介质 |
CN116572994B (zh) * | 2023-07-10 | 2023-09-22 | 之江实验室 | 一种车辆速度规划方法、装置及计算机可读介质 |
CN116627145B (zh) * | 2023-07-25 | 2023-10-20 | 陕西欧卡电子智能科技有限公司 | 无人游船的自主航行控制方法及系统 |
CN117075619B (zh) * | 2023-10-17 | 2024-01-16 | 之江实验室 | 一种局部路径规划方法、装置和介质 |
CN117340890A (zh) * | 2023-11-22 | 2024-01-05 | 北京交通大学 | 一种机器人运动轨迹控制方法 |
CN117719500B (zh) * | 2024-02-07 | 2024-05-28 | 北京易控智驾科技有限公司 | 车辆碰撞检测方法、装置、电子设备及存储介质 |
Citations (5)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN101075352A (zh) * | 2007-06-29 | 2007-11-21 | 中国科学院计算技术研究所 | 一种用于动态实体的分层避障方法 |
US9230369B2 (en) * | 2011-08-01 | 2016-01-05 | Autodesk, Inc. | Dynamic obstacle avoidance for crowd simulation using lane coordinates |
CN205325689U (zh) * | 2015-12-24 | 2016-06-22 | 华中科技大学 | 一种双机器人实时动态避障装置 |
CN106949893A (zh) * | 2017-03-24 | 2017-07-14 | 华中科技大学 | 一种三维避障的室内机器人导航方法和系统 |
CN108664020A (zh) * | 2018-04-11 | 2018-10-16 | 上海大学 | 一种基于速度障碍法和动态窗口法的无人艇动态避障算法 |
Family Cites Families (2)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN105479490A (zh) * | 2015-12-24 | 2016-04-13 | 华中科技大学 | 一种双机器人实时动态避障装置及其避障方法 |
CN107121980B (zh) * | 2017-03-17 | 2019-07-09 | 北京理工大学 | 一种基于虚拟约束的自动驾驶车辆路径规划方法 |
-
2019
- 2019-03-22 CN CN201910222739.XA patent/CN109960261B/zh active Active
Patent Citations (5)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN101075352A (zh) * | 2007-06-29 | 2007-11-21 | 中国科学院计算技术研究所 | 一种用于动态实体的分层避障方法 |
US9230369B2 (en) * | 2011-08-01 | 2016-01-05 | Autodesk, Inc. | Dynamic obstacle avoidance for crowd simulation using lane coordinates |
CN205325689U (zh) * | 2015-12-24 | 2016-06-22 | 华中科技大学 | 一种双机器人实时动态避障装置 |
CN106949893A (zh) * | 2017-03-24 | 2017-07-14 | 华中科技大学 | 一种三维避障的室内机器人导航方法和系统 |
CN108664020A (zh) * | 2018-04-11 | 2018-10-16 | 上海大学 | 一种基于速度障碍法和动态窗口法的无人艇动态避障算法 |
Non-Patent Citations (2)
Title |
---|
基于BVH的快速碰撞检测技术的研究;卓婧等;《科技视界》;20120925(第27期);全文 * |
基于GJK的凸体快速连续碰撞检测研究;刘丽等;《河北科技大学学报》;20141028;第35卷(第5期);第440-446页 * |
Also Published As
Publication number | Publication date |
---|---|
CN109960261A (zh) | 2019-07-02 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
CN109960261B (zh) | 一种基于碰撞检测的动态障碍物避让方法 | |
JP7150067B2 (ja) | 車両の制御システム、車両を制御する方法、及び非一時的コンピュータ可読メモリ | |
CN112068545B (zh) | 一种无人驾驶车辆在十字路口的行驶轨迹规划方法、系统及存储介质 | |
CN107792065B (zh) | 道路车辆轨迹规划的方法 | |
CN112965476B (zh) | 一种基于多窗口模型的高速无人车轨迹规划系统及方法 | |
CN112810630B (zh) | 一种自动驾驶车辆轨迹规划方法及系统 | |
Wei et al. | A behavioral planning framework for autonomous driving | |
CN113932823A (zh) | 基于语义道路地图的无人驾驶多目标点轨迹并行规划方法 | |
WO2019042295A1 (zh) | 一种无人驾驶路径规划方法、系统和装置 | |
CN115525047B (zh) | 一种具备多型避障方式的车辆局部轨迹规划方法及系统 | |
CN113960996A (zh) | 行驶装置的避障路径的规划方法和装置 | |
CN116465427B (zh) | 一种基于时空风险量化的智能车辆换道避障路径规划方法 | |
CN113895463B (zh) | 一种适用于自动驾驶车辆掉头的路径规划方法 | |
CN114237256B (zh) | 一种适用于欠驱动机器人的三维路径规划与导航方法 | |
Lin et al. | Sampling-based trajectory repairing for autonomous vehicles | |
Fu et al. | Model predictive trajectory optimization and tracking in highly constrained environments | |
Bertolazzi et al. | Efficient re-planning for robotic cars | |
Kim et al. | A hierarchical motion planning framework for autonomous driving in structured highway environments | |
Wu et al. | Motion planning of autonomous vehicles under dynamic traffic environment in intersections using probabilistic rapidly exploring random tree | |
CN109556623B (zh) | 融合触须算法与切线图法的迭代式路径规划算法 | |
CN114995415A (zh) | 一种基于时空可达集理论的自动驾驶汽车轨迹规划方法 | |
CN117141489B (zh) | 一种基于最小作用量原理的智能车辆轨迹分层规划方法 | |
Nagasaka et al. | Towards safe, smooth, and stable path planning for on-road autonomous driving under uncertainty | |
CN117360546A (zh) | 一种车辆控制方法、车辆、电子设备及存储介质 | |
Esposto et al. | Hybrid path planning for non-holonomic autonomous vehicles: An experimental evaluation |
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 |