CN112947406A - 一种基于FLOYD和Astar的混合路径规划方法 - Google Patents
一种基于FLOYD和Astar的混合路径规划方法 Download PDFInfo
- Publication number
- CN112947406A CN112947406A CN202110049821.4A CN202110049821A CN112947406A CN 112947406 A CN112947406 A CN 112947406A CN 202110049821 A CN202110049821 A CN 202110049821A CN 112947406 A CN112947406 A CN 112947406A
- Authority
- CN
- China
- Prior art keywords
- path
- vehicle
- algorithm
- floyd
- global
- 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.)
- Pending
Links
- 238000000034 method Methods 0.000 title claims abstract description 35
- 238000004422 calculation algorithm Methods 0.000 claims abstract description 100
- 239000011159 matrix material Substances 0.000 claims abstract description 43
- 238000013461 design Methods 0.000 claims abstract description 8
- 238000005516 engineering process Methods 0.000 claims description 7
- 230000008569 process Effects 0.000 claims description 7
- 238000001514 detection method Methods 0.000 claims description 6
- 238000012886 linear function Methods 0.000 claims description 5
- 238000013459 approach Methods 0.000 claims description 3
- 230000001502 supplementing effect Effects 0.000 claims description 2
- 238000004364 calculation method Methods 0.000 abstract description 6
- 230000006870 function Effects 0.000 description 25
- 238000010586 diagram Methods 0.000 description 4
- 230000008859 change Effects 0.000 description 3
- 230000007547 defect Effects 0.000 description 2
- 230000007613 environmental effect Effects 0.000 description 2
- 238000005457 optimization Methods 0.000 description 2
- 238000005070 sampling Methods 0.000 description 2
- 238000012360 testing method Methods 0.000 description 2
- 238000004458 analytical method Methods 0.000 description 1
- 238000003491 array Methods 0.000 description 1
- 238000013528 artificial neural network Methods 0.000 description 1
- 230000004888 barrier function Effects 0.000 description 1
- 230000009286 beneficial effect Effects 0.000 description 1
- 230000002708 enhancing effect Effects 0.000 description 1
- 238000002474 experimental method Methods 0.000 description 1
- 238000012804 iterative process Methods 0.000 description 1
- 238000010801 machine learning Methods 0.000 description 1
- 238000005259 measurement Methods 0.000 description 1
- 238000012545 processing Methods 0.000 description 1
- 238000011160 research Methods 0.000 description 1
- 238000010845 search algorithm Methods 0.000 description 1
- 238000004088 simulation Methods 0.000 description 1
- 239000007787 solid Substances 0.000 description 1
- 238000006467 substitution reaction Methods 0.000 description 1
Images
Classifications
-
- G—PHYSICS
- G05—CONTROLLING; REGULATING
- G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
- G05D1/00—Control of position, course or altitude of land, water, air, or space vehicles, e.g. automatic pilot
- 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/0217—Control of position or course in two dimensions specially adapted to land vehicles with means for defining a desired trajectory in accordance with energy consumption, time reduction or distance reduction criteria
-
- G—PHYSICS
- G05—CONTROLLING; REGULATING
- G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
- G05D1/00—Control of position, course or altitude of land, water, air, or space vehicles, e.g. automatic pilot
- 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
Abstract
本发明公开的一种基于FLOYD和Astar的混合路径规划方法,首先建立车辆数学模型和构建高精度地图,在此条件下,将Floyd算法和A*算法相结合,使用混合型算法进行自动驾驶车辆的路径规划。常见的路径规划算法中,Floyd算法时间复杂高,其计算得到的距离矩阵和路径矩阵无法满足自动驾驶车辆动态行驶时的高实时性要求;A*算法由于存在逻辑设计问题,其规划结果不一定是全局最优的。本发明提出的方法针对传统路径规划算法中Floyd算法与A*算法的问题,提出了一种结合两种方法特点的混合型路径规划算法,同时兼顾了全局最优性和实时性,增强了算法对环境的适应性,以此实现自动驾驶参考路径的在线规划和动态全局避障。
Description
技术领域
本发明涉及自动驾驶技术领域,特别涉及一种基于FLOYD和Astar的混合路径规划方法。
背景技术
路径规划是自动驾驶的关键技术之一,是重点和难点。自动驾驶车辆路径规划是指基于一定的环境模型和行车规则策略,给定行驶起始点与目标点后,按照特定性能指标,在保证行车安全和效率最大的基础上,规划出一条无碰撞、能安全到达目标点的可行驶路径。路径规划主要包含两个步骤,一是建立包含障碍物区域与自由区域的环境地图,二是结合环境地图选择合适的路径搜索算法,快速实时地搜索可行驶路径。自动驾驶车辆路径规划包括全局路径规划和局部路径规划。全局路径规划是基于环境信息(如地图、障碍物、道路边界)规划出满足车辆行驶约束的最优可行驶路径,具有导航能力。局部路径规划是指在全局路径的基础上,车辆为躲避障碍物而快速规划出的安全可行驶路径。参考机器人移动技术和机器学习技术,局部路径规划主要有人工势场算法、蚁群算法和神经网络算法等。
在传统算法中,Floyd路径规划算法时间复杂度高,道路节点分布均匀,距离矩阵和路径矩阵的迭代过程无法满足自动驾驶车辆动态行驶规划的高实时性要求,不能及时适应环境模型的改变;A*(A-Star)算法结合启发搜索思想和Dijkstra算法,是典型的启发式搜索算法,通过构建代价函数F搜索每一步代价最小的节点从而找到最短路径,但其规划结果可能非全局最优,存在因算法逻辑设计而出现的方向性或结构性结果倾向问题,这对于自动驾驶路径规划而言存在不可跟踪性路径;快速探索随机树(RRT)是基于随机采样理论的一种路径规划算法,它存在采样节点密集,路径曲折复杂等问题。此外,在传统路径规划算法中,通常将环境内的物体进行“轮廓膨胀处理”,将物体占用位置面积按一定比例增大,以此进行参考路径行驶时的动态规划,但会使得环境模型有一定的失真,狭窄路段的通过性降低。
针对单个算法存在缺陷,江鹏程等在专利申请“无人车混合路径规划算法(CN110609557A)”中,提出了结合蚁群算法、A*算法和VFH算法的无人车混合路径规划算法,首先利用周围环境信息构建栅格地图,然后用改进蚁群算法进行两次全局路径规划,若两次得到的结果不完全重合,则判定为陷入局部最优,此时引入A*算法进行对比和优化,得到最终最优路径。对于行驶过程中的未知障碍物,采用VFH+算法进行局部路径规划。与单个算法相比,该方法减少路径搜索时长、增强了自动驾驶车辆实时性、避免陷入局部最优解,但仍存在以下问题:将周围环境信息构建为栅格地图,无法保证路径的可行驶性;使用改进蚁群算法进行了两次全局路径规划,且在陷入局部最优后再进行A*算法的重新优化,耗时仍较长;改进蚁群算法在进行节点选择时,仍包含有随机数选择的形式,是一种盲目性的选择,缺乏启发信息。
发明内容
本发明提供一种基于FLOYD和Astar的混合路径规划方法,通过将传统的Floyd算法和A*算法相结合,解决当前传统算法中存在的实时性与全局最优性问题。
针对上述描述的路径规划算法的实时性和全局最优性问题,本发明提供的一种基于FLOYD和Astar的混合路径规划方法,其特征在于,包括以下步骤:
根据车辆的几何特征与车身角点约束,建立车辆数学几何模型,用以分析车辆的运动覆盖空间;
建立包含有道路层语义信息的电子地图,并在电子地图的道路上设定行驶航点;
使用Floyd算法获得全局距离矩阵以及最优路径矩阵,然后通过A*算法递归求取最优全局参考路径,完成全局路径规划;
导入实时环境信息和车辆行驶状态,使用碰撞判断规则和通行判断规则,对全局路径规划基于避障目的进行决策与重规划,实现可行驶参考路径的在线动态规划;
对最优全局参考路径进行数学描述,其中针对直线车道和非直车道采用分段函数描述参考路径:直线车道采用固定某一轴数值的直线函数,非直车道则采用均匀细分样条曲线函数。
作为本发明的进一步优选方案:所述车辆数学几何模型为车辆四个角点的位置表达式,定义车辆的四个角点分别为车辆左前角点A、车辆右前角点B、车辆右后角点C和车辆左后角点D,则各角点的位置表达式如下:
车辆左前角点A点(XA,YA)为:
车辆右前角点B点(XB,YB)为:
车辆右后角点C点(XC,YC)为:
车辆左后角点D点(XD,YD)为:
作为本发明的进一步优选方案:所述建立包含有道路层语义信息的电子地图,具体包括:
测量并绘制所要规划区域的平面地图;
基于OpenDRIVE技术补充道路层语义信息;
定义坐标原点,绘制所要规划区域的高精度电子地图。
作为本发明的进一步优选方案:所述在电子地图的道路上设定行驶航点中,交叉路口道路的航点分布密集,长直道上的航点分布较交叉路口道路的稀疏。
作为本发明的进一步优选方案:所述使用Floyd算法计算全局距离矩阵以及最优路径矩阵,然后通过A*算法递归求取最优路径,完成全局路径规划,具体包括:
通过Floyd算法计算获得全局距离矩阵以及最优路径矩阵;
将全局距离矩阵引入到A*算法的估计代价函数中,启发式地搜索给定起止点的最优路径,得到路径点集Path;
对路径点集Path进行递归和判断,将路径点集Path中的任意两点与通过Floyd算法得到的最优路径矩阵进行递归比较,若最优路径中的该两点的距离短于通过Floyd算法得到的最优路径矩阵中该两点的距离,则将Floyd算法得到的最优路径片段引入替换通过A*算法得到的最优全局参考路径中相应的路径片段,得到最终的最优全局参考路径。
作为本发明的进一步优选方案:所述通过A*算法递归求取最优路径中,A*算法采用的启发函数为欧几里德距离,A*算法的估计代价函数用于表示从x到y经过点k的估计代价,具体为:
Fxy(k)=G(x,k)+αH(k,y)
其中,G(x,k)为Floyd算法中距离矩阵从x到k的距离权重,α为偏重调节因子,H(k,y)为从k到y的欧几里德距离。
作为本发明的进一步优选方案:所述偏重调节因子α为85/cm。
作为本发明的进一步优选方案:所述碰撞判断规则的设计方式如下:
构建含有时刻信息的Frenet坐标系;
根据当前自动驾驶车辆的导航轨迹,将前向环境检测目标集合投影到Frenet坐标系,得到记录有障碍物目标的坐标、速度、时刻以及目标宽度信息的数据集,设定允许自动驾驶车辆的最小安全通行间距为S,t1、t2分别为车辆追上目标及超过的时间,t3、t4分别为车辆横向运动追上目标及超过的时间,设计规则如下:
当t4<t1<t3时,则在目标靠近的过程中,有碰撞可能性;
当t4<t2<t3时,则在目标远离的过程中,有碰撞可能性;
当t3>0或t1>0时,则目标远离车辆,不会碰撞,保持轨迹跟踪。
作为本发明的进一步优选方案:t1、t2、t3、t4的表达式分别如下:
作为本发明的进一步优选方案:所述通行判断规则的设计方式如下:
基于车辆约束和道路约束,对存在碰撞可能的目标进行避让空间判断,车辆宽度W,已知障碍物信息为Λi=(x,y,width),径向路沿坐标信息为(xl,xr),则通行判断规则如下:
当xi-xl-0.5width-(W+S)>0,则关于障碍物的道路左侧有足够空间通行;
当xr-xi-0.5width-(W+S)>0,则关于障碍物的道路右侧有足够空间通行;
除上述以外的其它情况,则无法避让障碍物,考虑减速停车或者重新规划路径,绕过该障碍物。
与现有技术相比,本发明能够实现的有益效果至少如下:
考虑了车身角点约束,建立了车辆几何模型,可完整地表示车辆运动时的覆盖面,为后续的碰撞判断规则的设计提供条件;使用OpenDRIVE构建了含路网信息的高精度电子地图,并依据车辆行驶情况,合理布置道路航点的稀疏,路径规划的最优性和可控跟踪性;针对传统Floyd算法和A*算法存在的实时性和全局最优性缺陷,设计了一种两种方法相结合的混合路径规划算法,将Floyd的全局距离矩阵、路径最优矩阵引入到A*算法的估计代价函数中,启发式地搜索给定起止点的最优路径;在A*算法输出其最优路径后,将路径中任意两点与Floyd路径最优矩阵进行递归比较,若某两节点的A*规划的距离比Floyd规划的距离更短,则将Floyd规划的路径片段引入替代,确保路径的全局最优性;设计了碰撞判断规则和通行判断规则,实现自动驾驶车辆行驶参考路径的在线规划和动态全局避障。
附图说明
图1为本发明实施例提供的一种基于FLOYD和Astar的混合路径规划方法的流程示意图。
图2为本发明实施例中车辆几何关系示意图。
图3为本发明实施例所选区域的道路连接图。
图4为本发明实施例所选区域的高精度电子地图。
图5为本发明实施例所选区域的道路航点布置图。
图6为Floyd+A*混合路径规划算法流程图。
图7为车辆遇障时重新规划路径的实验图。
图8为车辆遇障时无法重新规划路径的实验图。
具体实施方式
下面结合实施例及附图对本发明作进一步详细的描述,但本发明的实施方式不限于此。
本发明的思路是:首先根据车辆角点约束建立几何模型,构建指定区域高精度电子地图,然后在区域内,利用Floyd算法计算全局距离矩阵以及最优路径矩阵,再通过A*算法递归求取最优路径,确保路径最优解的同时提高算法迭代速度,完成全局路径规划;最后依据自行设计的碰撞判断规则和通行判断规则,实现自动驾驶车辆行驶参考路径的在线规划和动态全局避障。
如图1所示,本发明实施例提供的一种用于自动驾驶车辆的混合路径规划方法,包括以下步骤:
步骤S1:根据车辆的几何特征与车身角点约束,建立简化的车辆数学几何模型,用以分析车辆的运动覆盖空间。
车辆运动学分析及模型建立主要参考点为车辆后轴中点,但是车辆具有长宽高等几何特征,所以后轴中心的点运动不能表现整个车辆的运动。进而根据车身角点与后轴中心的几何关系分析整个车辆的运动覆盖空间,以此评价整车在运动过程中的碰撞情况。设在惯性坐标系中,(Xr,Yr)为车辆后轴中心坐标,为车身横摆角,l为车辆轴距,lf为车辆前轮轮距,lr为车辆后轮轮距。忽略车辆高度、简化车辆轮廓后,车身角点与车辆后轴中心的几何关系如图2所示,得到四个角点的数学表达式即车辆数学几何模型如下:
车辆左前角点A点(XA,YA)为:
车辆右前角点B点(XB,YB)为:
车辆右后角点C点(XC,YC)为:
车辆左后角点D点(XD,YD)为:
步骤S2:测量绘制区域平面地图,使用OpenDRIVE技术建立包含有道路层语义信息的高精度电子地图,并在地图关键道路上设定行驶航点,以提供路径节点信息。
高精度电子地图为自动驾驶车辆感知模块提供先验环境信息,帮助车辆获得更为准确有效的当前位置。
本实施例以某室外停车场作为研究实例,通过测量,绘制该停车场2D平面图,然后基于OpenDRIVE技术补充道路层语义信息,其定义了行车参考线、车道、道路连接关系和交叉路口信息,停车场道路网络如图3所示。最后,定义坐标原点,本实施例中定义停车场左下角为坐标零点,借助Matlab软件绘制出室外停车场高精度电子地图,同时,XML文件记录了整个停车场的路网信息,所建立的高精度电子地图如图4所示。
在实验停车场关键道路上设定了行驶航点。航点作为路径节点信息,必须满足强覆盖、强关联和强有效,保证路径规划的最优性和可控跟踪性,满足无人车在指定区域中的行驶要求;设置航点的规则是:交叉路口智能车可转向可直行,路径规划较灵活,所以交叉路口道路航点分布较密集;相对应的智能车在长直道上处于前向直行状态,道路航点分布较稀疏。航点属性包括车道方向、转向属性、坐标信息以及航点联接关系。本实施例中,如图5所示,在关键道路上设有168个航点,航点由“·”符号表示。
步骤S3:使用Floyd算法计算全局距离矩阵以及最优路径矩阵,然后通过A*算法递归求取最优全局参考路径,确保路径最优解的同时提高算法迭代速度,完成全局路径规划。
本步骤具体包括:
通过Floyd算法计算获得全局距离矩阵以及最优路径矩阵;
将全局距离矩阵引入到A*算法的估计代价函数中,启发式地搜索给定起止点的最优路径,得到路径点集Path;
对路径点集Path进行递归和判断,将路径点集Path中的任意两点与通过Floyd算法得到的最优路径矩阵进行递归比较,若最优路径中的该两点的距离短于通过Floyd算法得到的最优路径矩阵中该两点的距离,则将Floyd算法得到的最优路径片段引入替换通过A*算法得到的最优路径中相应的路径片段,得到最终的最优全局参考路径。
其中,A*算法的计算基于估计代价函数,用于计算当前点到目标点的最小代价候选点。假设当前的路径点集Path为P=(p1,p2,…,pn),pi为对应节点的序号,其代价函数为:
F(pi)=G(pi)+H(pi)
其中,F(pi)为起始点经过pi到达目标点的总代价估计,G(pi)为起始点到pi已付出的路径代价,H(pi)为pi到目标点的估计代价。在代价函数中,H(pi)影响着节点搜索的方向和深度,具备启发函数的作用。本发明考虑到路网节点信息包含距离权重、联接节点,选择欧几里德距离作为启发函数。启发函数对下一个节点的搜索和选择提供指向性,当前节点到达目标节点的路径代价更重要。因此,本文为H(pi)设定偏重调节因子,增大以高精度地图为前提的路网导向性。改进后的A*算法的估计代价函数用于表示从x到y经过点k的估计代价,具体为:
Fxy(k)=G(x,k)+αH(k,y) (3-10)
其中,Fxy(k)为从起始点x经过k到达目标点y的总代价估计,G(x,k)为Floyd算法中全局距离矩阵从x到k的距离权重,α为偏重调节因子,本实施例中α取值为85/cm,H(k,y)为从k到y的欧几里德距离。
优化后的Floyd+A*混合路径规划算法过程如图6所示。算法前半部分与A*算法相同,首先构造OPEN列表、CLOSED列表和Path列表,其中,OPEN列表存储已获得估算代价值但未被扩展的节点,CLOSED列表存储已被扩展无需再关注的节点,Path列表存储已找到的路径节点。从起始点开始,对当前节点的所有相邻节点进行搜索评估和存储中间值并放入OPEN列表。根据上述估计代价函数的计算,选择OPEN列表中成本最小的节点进行扩展,如此往复,直到扩展到目标节点为止,待搜索结束后即可输出当前最优路径点集Path。A*算法完成后,使用Floyd求得距离矩阵和全局路径矩阵,对路径点集进行递归和判断。将路径点集Path中的任意两点与通过Floyd算法得到的最优路径矩阵进行递归比较。由于Floyd算法能确保车辆全局路径最优,故当路径点集Path中的两点的距离disA*(x,y)短于通过Floyd最优路径矩阵中该两点的距离disFy(x,y)时,则将Floyd算法得到的最优路径片段引入替换通过A*算法得到的最优路径中相应的路径片段,得到优化后的最优全局参考路径。
步骤S4:导入实时环境信息和车辆行驶状态,使用碰撞判断规则和通行判断规则,对全局路径规划基于避障目的进行决策与重规划,实现可行使参考路径的在线动态规划。具体如下:
构建行驶轨迹坐标系,即Frenet坐标系,通过构建Frenet坐标,可以得到更精确的碰撞检测规则和通行检测规则。如图5所示,根据当前自动驾驶车辆的导航轨迹将前向环境检测目标集合Ck投影到Frenet坐标系,得到数据集Xk,数据集Xk中任一数据xi=(d,s,t,wd,ws),其中d为沿道路轨迹的横向偏移量,s为沿道路轨迹的切向偏移量,t为时刻,wd,ws分别为Frenet坐标系内目标的横向与切向的宽度。结合前述建立的将车辆数学几何模型,将四个角点的函数沿参考轨迹进行移动,得到运动覆盖空间,检查是否会与检测到的障碍物坐标出现相交现象,若相交则会发生碰撞,此时需要对当前参考路径进行重新调整与规划。对于未来时域的碰撞判断与预测,设自动驾驶车辆实时方位状态量为其中d0,s0为当前时刻车辆在Frenet坐标系下的横向与切向偏移量,为当前时刻车辆的横向与切向速度,设定允许的最小安全通行间距为S,车身长度为L,t1、t2分别为车辆追上目标及超过的时间,t3、t4分别为车辆横向运动追上目标及超过的时间,则
其中,符号变化对应坐标系的正负值域。由于前向目标检测只保留目标宽度信息,无长度信息,碰撞判断以t3和t4为主。碰撞规则主要有:
当t4<t1<t3时,则在目标靠近的过程中,有碰撞可能性;
当t4<t2<t3时,则在目标远离的过程中,有碰撞可能性;
当t3>0或t1>0时,则目标远离车辆,不会碰撞,保持轨迹跟踪。
根据设定的通行判断规则,基于车辆约束和道路约束,对存在碰撞可能的目标进行避让空间判断,车辆宽度为W,实验场地道路宽度为6.5m,允许通行的最小间距S为1.1m。已知障碍物信息为Λi=(di,si,width),di,si分别表示该障碍物在Frenet坐标系下的沿道路轨迹的横向、切向偏移量,width表示障碍物的宽度,径向路沿坐标信息为(xl,xr),其中xl为左侧路沿的坐标,xr为右侧路沿的坐标,设计通行判断规则:
a)di-xl-0.5width-(W+S)>0,则关于障碍物的道路左侧有足够空间通行;
xr-di-0.5width-(W+S)>0,则关于障碍物的道路右侧有足够空间通行;
b)除上述以外的其它情况,则无法避让障碍物,考虑减速停车或者重新规划路径,绕过该点。
综合前向数据、碰撞判断规则和通行判断规则,预判前向检测到的目标与自动驾驶车辆行驶空间是否发生位置干涉,如果没有干涉,保持原有全局路径规划得到的最优全局参考路径进行跟踪控制,否则通过全局路径规划进行动态路径更新,若全局路径规划无法获取可行路径,则指示车辆减速停车。
步骤S5:对最优全局参考路径进行数学描述,其中针对直线车道和非直车道采用分段函数描述:
直线车道上,将参考路径上的节点用直线线段进行连接,并以固定某一轴数值的直线函数描述该段路径(直线函数通过位置坐标来描述该段路径),自动驾驶车辆在相应路径内根据该路径的直线函数进行跟踪行驶;
非直车道则采用SPCRV(Spline Curve By Uniform Subdivision,均匀细分样条曲线)函数。
SPCRV函数的实现是在具有B样条系数c的k阶均匀B样条曲线f上生成密集的点序列f(t),具体为:
根据f(t)数组所蕴含的道路航点和路径描述函数信息,即可生成任意路段的参考路径函数和相对应需要驶过的航点序列,并传递给自动驾驶的跟踪控制模块,跟踪控制模块将依此控制车辆执行器,使车辆可沿参考路径的跟踪行驶。
本实施例结合Floyd算法与A*算法,将Floyd算法计算得到的全局距离矩阵以及最优路径矩阵导入为先验信息,结合A*的启发式搜索思想,完成高效的最优路径搜索任务。仿真试验中,根据前向行驶环境和高精度电子地图,对试验环境中的起止点与障碍物进行了不同的组合与改变,混合型路径规划算法能够根据环境的变化,在车辆自身约束和碰撞规则的限制下,实时给出重新规划后的最优全局路线,实验结果如图7、图8所示。图7中,先进行一次全局路径规划,得到的最优全局参考路径如图中以虚线表示的初始参考路径所示。在车辆开始行驶之前,模拟放置障碍物(实心矩形)在车辆的前方,通过碰撞判断规则与通行判断规则的计算,原路径无法通行,故根据当前位置与环境信息,对路径进行重新规划,得到新的全局参考路径如实线所示,执行全局避让操作。图8中,与图7情况类似,先进行一次全局路径规划,得到虚线所示的初始参考路径,然后模拟放置障碍物在车辆前方。同样的,碰撞判断规则与通行判断规则的计算,原路径无法通行,且当前没有其它路径可到达目标点,车辆执行减速停车等待的操作,待障碍物清除后继续按原路径行驶,本次实验中,在3分钟后,人为移除障碍物后,车辆判断出前方可按原路径完成通行,则采取参考路径保持继续行驶。
综上,本实施例提供的一种基于FLOYD和Astar的混合路径规划方法,通过在道路上布置航点的方式,不同航点间可直接使用曲线连接,使得具备更好的可行驶性和可控跟踪性;本发明只需通过一次A*算法完成全局路径规划,并通过后续与Floyd计算的迭代替换,在使用较少时间的同时保证了全局最优性;本发明结合环境信息、路网信息,设计了包含启发性的A*的估计代价函数,在搜索过程中会根据代价估算有方向性地进行节点选择。
上面结合附图和实施例对本发明的实施方式坐了详细说明,但本发明并不限于上述实施方式,在本领域普通技术人员所具备的知识范围内,还可以在不脱离本发明宗旨的前提下做出各种变化。
Claims (10)
1.一种基于FLOYD和Astar的混合路径规划方法,其特征在于,包括以下步骤:
根据车辆的几何特征与车身角点约束,建立车辆数学几何模型,用以分析车辆的运动覆盖空间;
建立包含有道路层语义信息的电子地图,并在电子地图的道路上设定行驶航点;
使用Floyd算法获得全局距离矩阵以及最优路径矩阵,然后通过A*算法递归求取最优全局参考路径,完成全局路径规划;
导入实时环境信息和车辆行驶状态,使用碰撞判断规则和通行判断规则,对全局路径规划基于避障目的进行决策与重规划,实现可行驶参考路径的在线动态规划;
对最优全局参考路径进行数学描述,其中针对直线车道和非直车道采用分段函数描述参考路径:直线车道采用固定某一轴数值的直线函数,非直车道则采用均匀细分样条曲线函数。
3.根据权利要求1所述的一种基于FLOYD和Astar的混合路径规划方法,其特征在于,所述建立包含有道路层语义信息的电子地图,具体包括:
测量并绘制所要规划区域的平面地图;
基于OpenDRIVE技术补充道路层语义信息;
定义坐标原点,绘制所要规划区域的高精度电子地图。
4.根据权利要求1所述的一种基于FLOYD和Astar的混合路径规划方法,其特征在于,所述在电子地图的道路上设定行驶航点中,交叉路口道路的航点分布密集,长直道上的航点分布较交叉路口道路的稀疏。
5.根据权利要求1所述的一种基于FLOYD和Astar的混合路径规划方法,其特征在于,所述使用Floyd算法计算全局距离矩阵以及最优路径矩阵,然后通过A*算法递归求取最优路径,完成全局路径规划,具体包括:
通过Floyd算法计算获得全局距离矩阵以及最优路径矩阵;
将全局距离矩阵引入到A*算法的估计代价函数中,启发式地搜索给定起止点的最优路径,得到路径点集Path;
对路径点集Path进行递归和判断,将路径点集Path中的任意两点与通过Floyd算法得到的最优路径矩阵进行递归比较,若最优路径中的该两点的距离短于通过Floyd算法得到的最优路径矩阵中该两点的距离,则将Floyd算法得到的最优路径片段引入替换通过A*算法得到的最优全局参考路径中相应的路径片段,得到最终的最优全局参考路径。
6.根据权利要求1所述的一种基于FLOYD和Astar的混合路径规划方法,其特征在于,所述通过A*算法递归求取最优路径中,A*算法采用的启发函数为欧几里德距离,A*算法的估计代价函数用于表示从x到y经过点k的估计代价,具体为:
Fxy(k)=G(x,k)+αH(k,y)
其中,G(x,k)为Floyd算法中距离矩阵从x到k的距离权重,α为偏重调节因子,H(k,y)为从k到y的欧几里德距离。
7.根据权利要求6所述的一种基于FLOYD和Astar的混合路径规划方法,其特征在于,所述偏重调节因子α为85/cm。
8.根据权利要求1所述的一种基于FLOYD和Astar的混合路径规划方法,其特征在于,所述碰撞判断规则的设计方式如下:
构建含有时刻信息的Frenet坐标系;
根据当前自动驾驶车辆的导航轨迹,将前向环境检测目标集合投影到Frenet坐标系,得到记录有障碍物目标的坐标、速度、时刻以及目标宽度信息的数据集,设定允许自动驾驶车辆的最小安全通行间距为S,t1、t2分别为车辆追上目标及超过的时间,t3、t4分别为车辆横向运动追上目标及超过的时间,设计规则如下:
当t4<t1<t3时,则在目标靠近的过程中,有碰撞可能性;
当t4<t2<t3时,则在目标远离的过程中,有碰撞可能性;
当t3>0或t1>0时,则目标远离车辆,不会碰撞,保持轨迹跟踪。
10.根据权利要求1-9任一所述的一种基于FLOYD和Astar的混合路径规划方法,其特征在于:所述通行判断规则的设计方式如下:
基于车辆约束和道路约束,对存在碰撞可能的目标进行避让空间判断,车辆宽度W,已知障碍物信息为Λi=(x,y,width),径向路沿坐标信息为(xl,xr),则通行判断规则如下:
当xi-xl-0.5width-(W+S)>0,则关于障碍物的道路左侧有足够空间通行;
当xr-xi-0.5width-(W+S)>0,则关于障碍物的道路右侧有足够空间通行;
除上述以外的其它情况,则无法避让障碍物,考虑减速停车或者重新规划路径,绕过该障碍物。
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202110049821.4A CN112947406A (zh) | 2021-01-14 | 2021-01-14 | 一种基于FLOYD和Astar的混合路径规划方法 |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202110049821.4A CN112947406A (zh) | 2021-01-14 | 2021-01-14 | 一种基于FLOYD和Astar的混合路径规划方法 |
Publications (1)
Publication Number | Publication Date |
---|---|
CN112947406A true CN112947406A (zh) | 2021-06-11 |
Family
ID=76235294
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
CN202110049821.4A Pending CN112947406A (zh) | 2021-01-14 | 2021-01-14 | 一种基于FLOYD和Astar的混合路径规划方法 |
Country Status (1)
Country | Link |
---|---|
CN (1) | CN112947406A (zh) |
Cited By (8)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN113485383A (zh) * | 2021-09-02 | 2021-10-08 | 北京三快在线科技有限公司 | 一种无人驾驶设备的控制方法及控制装置 |
CN113778102A (zh) * | 2021-09-22 | 2021-12-10 | 重庆长安汽车股份有限公司 | Avp全局路径规划系统、方法、车辆及存储介质 |
CN113848985A (zh) * | 2021-11-01 | 2021-12-28 | 西南交通大学 | 基于Hybird A*算法的多舰载无人机路径规划方法 |
CN113920768A (zh) * | 2021-10-09 | 2022-01-11 | 四川智胜慧旅科技有限公司 | 一种适用于自驾型景区的车辆调度方法及系统 |
CN114355909A (zh) * | 2021-12-22 | 2022-04-15 | 的卢技术有限公司 | 一种路径规划的方法、装置、计算机设备和存储介质 |
CN114706400A (zh) * | 2022-04-12 | 2022-07-05 | 重庆文理学院 | 一种越野环境下基于改进的a*算法的路径规划方法 |
CN114808830A (zh) * | 2022-06-30 | 2022-07-29 | 湖北省高创公路工程咨询监理有限公司 | 一种用于公路桥梁的防冰冻系统 |
CN114877901A (zh) * | 2022-03-31 | 2022-08-09 | 北京工业大学 | 基于地图栅格化融合与A-star搜索的城市应急路径规划方法 |
Citations (5)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
US20170108865A1 (en) * | 2015-10-20 | 2017-04-20 | Robert Bosch Gmbh | Method for selecting an optimized trajectory |
CN110782707A (zh) * | 2020-01-02 | 2020-02-11 | 中智行科技有限公司 | 汽车变道控制方法和装置、汽车、存储介质 |
DE102018220581A1 (de) * | 2018-11-29 | 2020-06-04 | Robert Bosch Gmbh | Verfahren und Vorrichtung zum Prüfen einer Trajektorie für ein Fahrzeug |
CN111731305A (zh) * | 2019-03-22 | 2020-10-02 | 采埃孚汽车德国有限公司 | 用于控制机动车的系统的方法及控制设备 |
CN111795699A (zh) * | 2019-11-26 | 2020-10-20 | 北京京东乾石科技有限公司 | 无人车的路径规划方法、装置和计算机可读存储介质 |
-
2021
- 2021-01-14 CN CN202110049821.4A patent/CN112947406A/zh active Pending
Patent Citations (5)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
US20170108865A1 (en) * | 2015-10-20 | 2017-04-20 | Robert Bosch Gmbh | Method for selecting an optimized trajectory |
DE102018220581A1 (de) * | 2018-11-29 | 2020-06-04 | Robert Bosch Gmbh | Verfahren und Vorrichtung zum Prüfen einer Trajektorie für ein Fahrzeug |
CN111731305A (zh) * | 2019-03-22 | 2020-10-02 | 采埃孚汽车德国有限公司 | 用于控制机动车的系统的方法及控制设备 |
CN111795699A (zh) * | 2019-11-26 | 2020-10-20 | 北京京东乾石科技有限公司 | 无人车的路径规划方法、装置和计算机可读存储介质 |
CN110782707A (zh) * | 2020-01-02 | 2020-02-11 | 中智行科技有限公司 | 汽车变道控制方法和装置、汽车、存储介质 |
Non-Patent Citations (1)
Title |
---|
艾皖东: "区域自动代客泊车引导行驶控制应用研究", 万方数据 * |
Cited By (11)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN113485383A (zh) * | 2021-09-02 | 2021-10-08 | 北京三快在线科技有限公司 | 一种无人驾驶设备的控制方法及控制装置 |
CN113485383B (zh) * | 2021-09-02 | 2021-12-31 | 北京三快在线科技有限公司 | 一种无人驾驶设备的控制方法及控制装置 |
CN113778102A (zh) * | 2021-09-22 | 2021-12-10 | 重庆长安汽车股份有限公司 | Avp全局路径规划系统、方法、车辆及存储介质 |
CN113778102B (zh) * | 2021-09-22 | 2023-05-12 | 重庆长安汽车股份有限公司 | Avp全局路径规划系统、方法、车辆及存储介质 |
CN113920768A (zh) * | 2021-10-09 | 2022-01-11 | 四川智胜慧旅科技有限公司 | 一种适用于自驾型景区的车辆调度方法及系统 |
CN113848985A (zh) * | 2021-11-01 | 2021-12-28 | 西南交通大学 | 基于Hybird A*算法的多舰载无人机路径规划方法 |
CN113848985B (zh) * | 2021-11-01 | 2022-09-09 | 西南交通大学 | 基于Hybird A*算法的多舰载无人机路径规划方法 |
CN114355909A (zh) * | 2021-12-22 | 2022-04-15 | 的卢技术有限公司 | 一种路径规划的方法、装置、计算机设备和存储介质 |
CN114877901A (zh) * | 2022-03-31 | 2022-08-09 | 北京工业大学 | 基于地图栅格化融合与A-star搜索的城市应急路径规划方法 |
CN114706400A (zh) * | 2022-04-12 | 2022-07-05 | 重庆文理学院 | 一种越野环境下基于改进的a*算法的路径规划方法 |
CN114808830A (zh) * | 2022-06-30 | 2022-07-29 | 湖北省高创公路工程咨询监理有限公司 | 一种用于公路桥梁的防冰冻系统 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
CN112947406A (zh) | 一种基于FLOYD和Astar的混合路径规划方法 | |
CN110333714B (zh) | 一种无人驾驶汽车路径规划方法和装置 | |
Stahl et al. | Multilayer graph-based trajectory planning for race vehicles in dynamic scenarios | |
JP7140849B2 (ja) | 確率的オブジェクト追跡及び予測フレームワーク | |
CN110081894B (zh) | 一种基于道路结构权值融合的无人车轨迹实时规划方法 | |
CN108983781B (zh) | 一种无人车目标搜索系统中的环境探测方法 | |
Zhao et al. | Dynamic motion planning for autonomous vehicle in unknown environments | |
CN107063280A (zh) | 一种基于控制采样的智能车辆路径规划系统及方法 | |
CN112577506B (zh) | 一种自动驾驶局部路径规划方法和系统 | |
CN107310550A (zh) | 道路交通工具行驶控制方法和装置 | |
CN109084798A (zh) | 网络下发带有道路属性的控制点的路径规划方法 | |
Li et al. | Human-like trajectory planning on curved road: Learning from human drivers | |
Ben-Messaoud et al. | Smooth obstacle avoidance path planning for autonomous vehicles | |
CN111896004A (zh) | 一种狭窄通道车辆轨迹规划方法及系统 | |
Guan et al. | Integrated decision and control: Towards interpretable and computationally efficient driving intelligence | |
CN114578834B (zh) | 基于目标分层双感知域的强化学习的无人车路径规划方法 | |
Ren et al. | Self-learned intelligence for integrated decision and control of automated vehicles at signalized intersections | |
CN114519931B (zh) | 一种路口环境下目标车辆行为预测方法及装置 | |
Park et al. | Path generation algorithm based on crash point prediction for lane changing of autonomous vehicles | |
Yu et al. | Hierarchical framework integrating rapidly-exploring random tree with deep reinforcement learning for autonomous vehicle | |
CN113419521A (zh) | 一种自动驾驶车辆局部避障路径的规划及跟踪方法 | |
CN116331264A (zh) | 一种未知障碍物分布的避障路径鲁棒规划方法及系统 | |
Smit et al. | Informed sampling-based trajectory planner for automated driving in dynamic urban environments | |
Fu et al. | Unmanned Driving Technology in Coal Mine Based on Semantic Information Method | |
Li et al. | Decision making for autonomous vehicles |
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 |