CN115309144A - 路径规划方法及装置、计算机可读存储介质、终端 - Google Patents
路径规划方法及装置、计算机可读存储介质、终端 Download PDFInfo
- Publication number
- CN115309144A CN115309144A CN202210659367.9A CN202210659367A CN115309144A CN 115309144 A CN115309144 A CN 115309144A CN 202210659367 A CN202210659367 A CN 202210659367A CN 115309144 A CN115309144 A CN 115309144A
- Authority
- CN
- China
- Prior art keywords
- circle
- path
- node
- total cost
- cost value
- 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 56
- 238000001514 detection method Methods 0.000 claims abstract description 17
- 238000005070 sampling Methods 0.000 claims description 26
- 230000002441 reversible effect Effects 0.000 claims description 15
- 238000004590 computer program Methods 0.000 claims description 11
- 238000004422 calculation algorithm Methods 0.000 claims description 10
- 230000000694 effects Effects 0.000 abstract description 9
- 238000004364 calculation method Methods 0.000 description 15
- 230000008569 process Effects 0.000 description 14
- 238000010586 diagram Methods 0.000 description 7
- 230000004888 barrier function Effects 0.000 description 6
- 230000006870 function Effects 0.000 description 4
- 230000010355 oscillation Effects 0.000 description 4
- 238000012545 processing Methods 0.000 description 4
- 230000009286 beneficial effect Effects 0.000 description 3
- 230000008859 change Effects 0.000 description 3
- 239000007787 solid Substances 0.000 description 2
- 238000004804 winding Methods 0.000 description 2
- 238000003491 array Methods 0.000 description 1
- 230000006399 behavior Effects 0.000 description 1
- 238000005516 engineering process Methods 0.000 description 1
- 238000000691 measurement method Methods 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
- 238000011160 research Methods 0.000 description 1
- 238000000926 separation method Methods 0.000 description 1
- 230000003068 static effect Effects 0.000 description 1
- 230000001360 synchronised effect Effects 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, 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/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
Landscapes
- Engineering & Computer Science (AREA)
- Aviation & Aerospace Engineering (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Automation & Control Theory (AREA)
- Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)
Abstract
一种路径规划方法及装置、计算机可读存储介质、终端,所述方法包括:确定路径搜索的起始节点与目标节点;自所述起始节点起,进行多次搜索直至到达所述目标节点,其中,在每次搜索中,以当前节点为圆心,采用第一半径形成父圆,并以所述父圆的圆周为圆心,采用第二半径以及多个预设的生成角度生成多个有向子圆,并在满足转向曲率限制且碰撞检测结果为无碰撞的有向子圆中确定第一总代价值最小的有向子圆,以作为下一次搜索的父圆;以首个父圆至末个有向子圆覆盖的区域作为规划子空间,在所述规划子空间内规划车辆的行进路径。本发明可以提高避障效果。
Description
技术领域
本发明涉及数据处理技术领域,尤其涉及一种路径规划方法及装置、计算机可读存储介质、终端。
背景技术
在无人驾驶车辆路径规划模块中,可以根据地图信息、定位信息以及预测信息,以及根据车辆当前的状态,在一定区域内搜索到一条由当前位置到目标终点的无碰撞的、安全可行的路径。
然而,在基于非结构化道路的复杂场景(又可以称为狭窄场景,例如自动泊车的路径规划场景)下,往往会遇到在途中存在障碍物的情况。
在现有的寻路算法中,通常直接基于起始节点与目标节点规划一条线形路径,即使能够避开障碍物,也往往存在车辆与障碍物距离极近的情况,容易出现摩擦车体、划伤车辆等问题。
发明内容
本发明解决的技术问题是提供一种路径规划方法及装置、计算机可读存储介质、终端,可以提高避障效果。
为解决上述技术问题,本发明实施例提供一种路径规划方法,包括:确定路径搜索的起始节点与目标节点;自所述起始节点起,进行多次搜索直至到达所述目标节点,其中,在每次搜索中,以当前节点为圆心,采用第一半径形成父圆,并以所述父圆的圆周为圆心,采用第二半径以及多个预设的生成角度生成多个有向子圆,并在满足转向曲率限制且碰撞检测结果为无碰撞的有向子圆中确定第一总代价值最小的有向子圆,以作为下一次搜索的父圆;以首个父圆至末个有向子圆覆盖的区域作为规划子空间,在所述规划子空间内规划车辆的行进路径。
可选的,采用下述算式,确定各个有向子圆是否满足转向曲率限制:
Δθparent,child≤kmax×dparent,child
其中,Δθparent,child用于表示当前有向子圆的父圆与该有向子圆之间的角度差,kmax用于表示转弯曲率上限值,dparent,child用于表示在地图上所述当前有向子圆的圆心与父圆的圆心之间的距离。
可选的,采用多个预设的生成角度生成多个有向子圆包括:以所述父圆的生成角度为角度中心,在[-π/2,π/2]范围内选择多个角度作为各个有向子圆的生成角度。
可选的,在以首个父圆至末个有向子圆覆盖的区域作为规划子空间,在所述规划子空间内规划车辆的行进路径之前,所述方法还包括:步骤A:将各个计算过所述第一总代价值的有向子圆的信息添加至第一开放列表中,所述有向子圆的信息包括该有向子圆的圆心、生成角度以及第一总代价值;步骤B:每当进行多次搜索直至到达所述目标节点之后,计算当前路径的第一路径总代价值,并且将沿途的父圆移至第一封闭列表中;步骤C:判断所述第一开放列表中是否存在最小的第一总代价值小于所述第一路径总代价值;步骤D:如果存在,则采用该第一总代价值最小的有向子圆进行搜索直至再次到达所述目标节点,并返回步骤B重新计算当前路径的第一路径总代价值,直至计算得到第一路径总代价值小于等于所述第一开放列表中最小的第一总代价值的路径,以作为有效路径;步骤E:采用所述有效路径中的最后一个有向子圆作为所述末个有向子圆。
可选的,在每次搜索时,形成父圆的第一半径大于或等于形成所述有向子圆的第二半径。
可选的,采用下述公式,确定第一总代价值:
f=g+h
其中,f用于表示当前有向子圆的总代价值,g用于表示起始节点至所述当前有向子圆的圆心的已用代价,h用于表示所述当前有向子圆的启发代价。
可选的,采用Reeds-Sheeps曲线算法计算所述当前有向子圆的启发代价。
可选的,在所述规划子空间内规划车辆的行进路径包括:自所述起始节点起,进行多次行进规划直至到达所述目标节点;其中,在每次行进规划中,确定当前次行进的行进距离以及方向盘转角的多个采样角度,以得到多个行进节点,并采用第二总代价值最小的行进节点作为下次行进规划的起点。
可选的,针对每个行进节点,分别确定距离最近且位于所述规划子空间内的有向子圆;在每次行进规划中,确定当前次行进的行进距离以及方向盘转角的多个采样角度包括:根据当前行进节点对应的有向子圆的半径,确定当前次行进的行进距离;其中,所述当前行进节点对应的有向子圆的半径越小,所述当前次行进的行进距离越小。
可选的,在每次行进规划中,确定当前次行进的行进距离以及方向盘转角的多个采样角度包括:以当前次行进的直行方向为角度中心,在[uleft,uright]范围内均匀选择多个采样角度;其中,uleft和uright分别表示在左向和右向的方向盘转角的上限值。
可选的,针对每个行进节点,分别确定距离最近且位于所述规划子空间内的有向子圆;采用下述公式,确定各个行进节点的第二总代价值中的已用代价:
gcost=slength+α×sreverse+β×ssteering+γ×sopposite
其中,slength用于表示自起始节点至每个行进节点之间车辆的行进距离,sreverse用于表示自起始节点至每个行进节点之间所述车辆采用倒车档位行驶的距离,ssteering用于表示自起始节点至每个行进节点之间所述车辆在转向模式下行驶的距离,sopposite用于表示自起始节点至每个行进节点之间,所述车辆基于途径的各个行进节点对应的有向子圆进行后向运动行驶的距离,α、β、γ用于表示预设的权重系数。
可选的,针对每个行进节点,分别确定距离最近且位于所述规划子空间内的有向子圆;采用下述公式,确定各个行进节点的第二总代价值中的启发代价:
h=dstate,mapcircle+dmapcircle,goal
其中,dstate,mapcircle用于表示每个行进节点在地图上的位置与对应的有向子圆的圆心之间的距离,dstate,mapcircle用于表示每个行进节点对应的有向子圆的圆心与所述目标节点之间的距离。
可选的,在所述规划子空间内规划车辆的行进路径还包括:步骤F:将各个计算过所述第二总代价值的行进节点的信息添加至第二开放列表中,所述行进节点的信息包括该行进节点在地图上的位置、对应的有向子圆的信息以及第二总代价值;步骤G:每当进行多次行进规划直至到达所述目标节点之后,计算当前行进路径的第二路径总代价值,并且将沿途的行进节点移至第二封闭列表中;步骤H:判断所述第二开放列表中是否存在最小的第二总代价值小于所述第二路径总代价值;步骤I:如果存在,则采用该第二总代价值最小的行进节点进行行进规划直至再次到达所述目标节点,并返回步骤G重新计算当前行进路径的第二路径总代价值,直至计算得到第二路径总代价值小于等于所述第二开放列表中最小的第二总代价值的路径,以作为有效行进路径。
为解决上述技术问题,本发明实施例提供一种路径规划装置,包括:节点确定模块,用于确定路径搜索的起始节点与目标节点;搜索模块,用于自所述起始节点起,进行多次搜索直至到达所述目标节点,其中,在每次搜索中,以当前节点为圆心,采用第一半径形成父圆,并以所述父圆的圆周为圆心,采用第二半径以及多个预设的生成角度生成多个有向子圆,并在满足转向曲率限制且碰撞检测结果为无碰撞的有向子圆中确定第一总代价值最小的有向子圆,以作为下一次搜索的父圆;规划模块,用于以首个父圆至末个有向子圆覆盖的区域作为规划子空间,在所述规划子空间内规划车辆的行进路径。
为解决上述技术问题,本发明实施例提供一种计算机可读存储介质,其上存储有计算机程序,所述计算机程序被处理器运行时执行上述路径规划方法的步骤。
为解决上述技术问题,本发明实施例提供一种终端,包括存储器和处理器,所述存储器上存储有能够在所述处理器上运行的计算机程序,所述处理器运行所述计算机程序时执行上述路径规划方法的步骤。
与现有技术相比,本发明实施例的技术方案具有以下有益效果:
在本发明实施例中,通过多次搜索,且在每次搜索中均采用第一总代价值最小、满足转向曲率限制且碰撞检测结果为无碰撞的有向子圆作为下一次搜索的父圆,然后以首个父圆至末个有向子圆覆盖的区域作为规划子空间,在所述规划子空间内规划车辆的行进路径,可以在起始节点与目标节点之间找到由多个有向圆串接后形成的无障碍区域,进而在该无障碍区域内规划车辆行进的线形路径,相比于现有技术中,直接基于起始节点与目标节点规划一条线形路径,即使能够避开障碍物,也往往存在车辆与障碍物距离极近的情况,采用本发明实施例的方案,由于先划出的安全区域已经避开障碍物,因此在安全区域再规划行进路径,有助于进一步远离障碍物,从而有效提高避障效果。
进一步,以所述父圆的生成角度为角度中心,在[-π/2,π/2]范围内选择多个角度作为各个有向子圆的生成角度,可以在车辆采用父圆的生成角度作为前向行驶方向的情况下,保持车辆在有向子圆中继续采用前进档位行驶,提高规划到的路径的行驶流畅性。
进一步,采用第一开放列表,记录各个计算过所述第一总代价值的有向子圆,并且每当进行多次搜索直至到达所述目标节点之后,均计算当前路径的第一路径总代价值,由于已经到达目标节点,相比于之前采用启发代价计算得到的第一总代价值更贴近实际情况且更具准确性,进而采用第一路径总代价值与第一开放列表中的各个有向子圆的第一总代价值进行比较,如果不存在更小值,则将当前路径作为有效路径,如果存在更小值,则判断为当前路径并非最优解路径,进而采用第一总代价值最小的有向子圆继续进行搜索,重复这个搜索过程,直至第一路径总代价值小于第一开放列表中的任何有向子圆的第一总代价值,再将该路径作为有效路径,从而提高对规划子空间的规划准确性;且由于将沿途的父圆移至第二封闭列表中,可以避免对同一个有向子圆重复计算,从而提高计算效率。
进一步,形成父圆的第一半径大于或等于形成所述有向子圆的第二半径,可以使各个父圆和对应的子圆采用一致的半径,还可以在离终点较远时采用较大半径搜索,从而提高搜索效率,以及在离终点较近时采用较小半径搜索,从而提高搜索精度和避障效果。
进一步,采用Reeds-Sheeps曲线算法计算所述当前有向子圆的启发代价,可以利用Reeds-Sheeps曲线算法具有车辆运动学约束的特点,提高启发代价的预测准确性。
进一步,在所述规划子空间内规划车辆的行进路径时,进行多次行进规划,并在每次行进规划中,确定当前次行进的行进距离以及方向盘转角的多个采样角度,以得到多个行进节点,并采用第二总代价值最小的行进节点作为下次行进规划的起点,由于之前形成的无障碍区域已经具备满足转向曲率限制、碰撞检测结果为无碰撞的要求,此时在无障碍区域内基于第二总代价值最小继续规划行进路径,能够进一步优化路径规划,并且进一步远离障碍物,更好地提高避障效果。
进一步,根据当前行进节点对应的有向子圆的半径,确定当前次行进的行进距离;其中,所述当前行进节点对应的有向子圆的半径越小,所述当前次行进的行进距离越小,可以在有向子圆的半径较大(例如离终点较远时)采用较大行进距离行进,从而提高行进效率,以及在有向子圆的半径较小(例如离终点较近时)采用较小行进距离行进,从而提高行进精准性和避障效果。
进一步,以当前次行进的直行方向为角度中心,在[uleft,uright]范围内均匀选择多个采样角度,可以基于少量采样角度进行高效判断。
进一步,在确定各个行进节点的第二总代价值中的已用代价过程中,不仅考虑自起始节点至每个行进节点之间车辆的行进距离,还考虑对车辆采用倒车档位行驶的距离较大的路径进行惩罚从而优先选择采用前进档位行进的路径,还考虑对车辆在转向模式下行驶的距离较大的路径进行惩罚从而优先选择采用正向直行或倒车直行路径,以及还考虑对车辆后向运动行驶的距离较大的路径进行惩罚从而在各个行进节点优先选择与对应的有向子圆的生成角度相近的角度行驶的路径,有助于通过适当的惩罚,减轻搜索到的路径绕远、扭曲振荡的情况。
进一步,在确定各个行进节点的第二总代价值中的启发代价过程中,既考虑行进节点与对应的有向子圆的圆心之间的距离,又考虑对应的有向子圆的圆心与所述目标节点之间的距离,相比于仅考虑行进节点与目标节点之间的距离,可以充分利用距离最近且位于所述规划子空间内的有向子圆,使得行进路径向串接形成规划子空间的各个有向子圆的圆心处靠拢,在先划出的安全区域已经避开障碍物的基础上,进一步远离障碍物,提高避障效果。
进一步,采用第二开放列表,记录各个计算过所述第二总代价值的行进节点,并且每当进行多次行进规划直至到达所述目标节点之后,均计算当前行进路径的第二路径总代价值,由于已经到达目标节点,相比于之前采用启发代价计算得到的第二总代价值更贴近实际情况且更具准确性,进而采用第二路径总代价值与第二开放列表中的各个行进节点的第二总代价值进行比较,如果不存在更小值,则将当前行进路径作为有效行进路径,如果存在更小值,则判断为当前行进路径并非最优解行进路径,进而采用第二总代价值最小的行进节点继续进行搜索,重复这个行进规划过程,直至第二路径总代价值小于第二开放列表中的任何行进节点的第二总代价值,再将该行进路径作为有效行进路径,从而提高对车辆路径规划的规划准确性;且由于将沿途的行进节点移至第二封闭列表中,可以避免对同一个行进节点重复计算,从而提高计算效率。
附图说明
图1是本发明实施例中一种路径规划方法的流程图;
图2是本发明实施例中一种父圆和有向子圆的示意图;
图3是本发明实施例中一种规划子空间的示意图;
图4是本发明实施例中另一种路径规划方法的部分流程图;
图5是本发明实施例中又一种路径规划方法的部分流程图;
图6是本发明实施例中一种路径规划装置的结构示意图。
具体实施方式
目前,在进行无人驾驶车辆路径规划的过程中,一般会根据道路环境信息的不同将路径规划的任务分为结构化道路上的路径规划和非结构化道路上的路径规划。结构化道路具有清晰的道路标志线,包括高速公路、城市主干道等;而非结构化道路没有道路标志线,往往指乡村街道、开放环境中的道路等。在非结构化道路上的无人驾驶汽车的行驶速度一般不高,且与高速结构化道路相比,非结构化道路的场景较为复杂,常常会出现障碍物密集、杂乱以及车辆可行驶范围狭窄等情况。
在基于非结构化道路的复杂场景下,往往会遇到在途中存在障碍物的情况。而现有的寻路算法的避障功能较差。
本发明的发明人经过研究发现,现有的狭窄场景辅助驾驶功能通常是为明确定义的任务设计的,例如因此缺乏一般机动问题的灵活性,具体地,通常直接基于起始节点与目标节点规划一条线形路径,即使能够避开障碍物,也往往存在车辆与障碍物距离极近的情况,由于车辆的横向运动受到限制,并且在没有纵向运动的情况下不能改变方向,容易出现摩擦车体、划伤车辆等问题。
在本发明实施例中,通过多次搜索,且在每次搜索中均采用第一总代价值最小、满足转向曲率限制且碰撞检测结果为无碰撞的有向子圆作为下一次搜索的父圆,然后以首个父圆至末个有向子圆覆盖的区域作为规划子空间,在所述规划子空间内规划车辆的行进路径,可以在起始节点与目标节点之间找到由多个有向圆串接后形成的无障碍区域,进而在该无障碍区域内规划车辆行进的线形路径,相比于现有技术中,直接基于起始节点与目标节点规划一条线形路径,即使能够避开障碍物,也往往存在车辆与障碍物距离极近的情况,采用本发明实施例的方案,由于先划出的安全区域已经避开障碍物,因此在安全区域再规划行进路径,有助于进一步远离障碍物,从而有效提高避障效果。
为使本发明的上述目的、特征和有益效果能够更为明显易懂,下面结合附图对本发明的具体实施例做详细的说明。
参照图1,图1是本发明实施例中一种路径规划方法的流程图。所述路径规划方法可以包括步骤S11至步骤S13:
步骤S11:确定路径搜索的起始节点与目标节点;
步骤S12:自所述起始节点起,进行多次搜索直至到达所述目标节点,其中,在每次搜索中,以当前节点为圆心,采用第一半径形成父圆,并以所述父圆的圆周为圆心,采用第二半径以及多个预设的生成角度生成多个有向子圆,在多个所述有向子圆中确定第一总代价值最小、满足转向曲率限制且碰撞检测结果为无碰撞的有向子圆,以作为下一次搜索的父圆;
步骤S13:以首个父圆至末个有向子圆覆盖的区域作为规划子空间,在所述规划子空间内规划车辆的行进路径。
可以理解的是,在具体实施中,所述方法可以采用软件程序的方式实现,该软件程序运行于芯片或芯片模组内部集成的处理器中。
在步骤S11的具体实施中,可以在地图上确定车辆行驶的起点与终点,并分别作为起始节点与目标节点。
在一个非限制性的具体实施方式中,还可以先根据地图信息,采用一定的栅格分辨率,将地图栅格化,从而确定栅格地图,并得到离散的地图信息,然后可以确定栅格地图中路径搜索的起始节点与目标节点。
在步骤S12的具体实施中,可以基于有向圆模型进行启发式搜索,自所述起始节点起,进行多次搜索直至到达所述目标节点。
具体地,在首次搜索中,可以以起始节点为圆心,采用第一半径形成父圆,并以所述父圆的圆周为圆心,采用第二半径以及多个预设的生成角度生成多个有向子圆,在其中选择合适的有向子圆作为第二次搜索的父圆。
然后,自第i次搜索开始,采用第i-1次搜索得到的有向子圆作为第i次搜索的父圆,并以第i次搜索的父圆的圆周为圆心、采用第i+1半径以及多个预设的第i生成角度生成多个有向子圆,并在其中选择合适的有向子圆作为第i+1次搜索的父圆,继续下一次搜索。其中,i为正整数,且i≥2。
参照图2,图2是本发明实施例中一种父圆和有向子圆的示意图。
具体地,父圆和有向子圆均具有生成角度(又可以称为节点朝向),也即具有方向性,因此又可以称为有向圆。在图2中采用箭头方向指示有向圆(包括父圆和各个有向子圆)的生成角度。
更具体而言,每个有向圆的配置空间可以是(x,y,θ,r),其中,(x,y)可以用于表示有向圆的圆心在地图上的位置,θ可以用于表示有向圆的生成角度,r可以用于表示有向圆的半径。
需要指出的是,根据生成角度,车辆在有向圆内的运动可以包括前向运动和后向运动,其中,可以是满足转向曲率限制、且顺着有向圆的生成角度(如顺着图2示出的箭头方向)进行前向运动行驶,还可以是满足转向曲率限制、且逆着有向圆的生成角度(如逆着图2示出的箭头方向)进行后向运动行驶。
具体地,前向运动可以包括在非转向模式下向正前方的正向运动,还可以包括在转向模式下向斜前方转向的机动运动;后向运动可以包括在非转向模式下向正后方的反向运动,还可以包括在转向模式下向斜后方转向的机动运动。
其中,与父圆的生成角度垂直的有向子圆(如图2中左右两个有向子圆),既可以自父圆指向有向子圆,还可以自有向子圆指向父圆。
进一步地,采用多个预设的生成角度生成多个有向子圆可以包括:以所述父圆的生成角度为角度中心,在[-π/2,π/2]范围内选择多个角度作为各个有向子圆的生成角度。
具体地,以图2示出的有向子圆为例,实线有向子圆位于上方的[-π/2,π/2]范围,车辆可以采用前进档位进行前向运动行驶;虚线有向子圆并非位于上方的[-π/2,π/2]范围,而是位于下方的(π/2,-π/2)范围,如果选择虚线有向子圆,需要自前进档位切换至倒车档位进行后向运动行驶。
在本发明实施例中,以所述父圆的生成角度为角度中心,在[-π/2,π/2]范围内选择多个角度作为各个有向子圆的生成角度,可以在车辆采用父圆的生成角度作为前向行驶方向的情况下,保持车辆在有向子圆中继续采用前进档位行驶,提高规划到的路径的行驶流畅性。
进一步地,可以采用下述算式,确定各个有向子圆是否满足转向曲率限制:
Δθparent,child≤kmax×dparent,child
其中,Δθparent,child用于表示当前有向子圆的父圆与该有向子圆之间的角度差,kmax用于表示转弯曲率上限值,dparent,child用于表示在地图上所述当前有向子圆的圆心与父圆的圆心之间的距离。
其中,在本申请中披露的距离可以采用适当的距离度量方法确定两点之间相隔或间隔的长度,例如可以选自:欧式距离(Euclidean Distance)、闵氏距离(EuclideanDistance)、标准化欧式距离。
其中,转弯曲率上限值可以用于表示车辆的转弯能力,从而在车辆能够行驶到达的范围内选择有向子圆。可以理解的是,可以根据具体车辆或同类型车辆预先设置适当的转弯曲率上限值。
其中,在本申请中披露的角度差可以是两个有向圆之间的生成角度的差值,在上述角度差除以上述圆心之间的距离得到的差值小于转弯曲率上限值的情况下,车辆能够从父圆行驶至有向子圆。
在本发明实施例中,通过上述公式,能够基于车辆能力(转弯曲率上限值),判断父圆与有向子圆的角度差是否能在两圆中心距离内完成转向,从而确定各个有向子圆是否满足转向曲率限制。
在本发明实施例中,可以采用适当的方式进行碰撞检测。在一个非限制性的具体实施方式中,可以检测各个有向子圆在地图上与障碍物是否具有重叠区域,如果没有重叠区域,则确定有向子圆为无碰撞有向子圆。
进一步地,可以采用下述公式,确定第一总代价值:
f=g+h
其中,f用于表示当前有向子圆的总代价值,g用于表示起始节点至所述当前有向子圆的圆心的已用代价,h用于表示所述当前有向子圆的启发代价。
具体地,由于无法得知实际的代价值,因此h可以是采用适当的算法得到的、从当前有向子圆到目标节点的启发代价的估计值。
进一步地,可以采用Reeds-Sheeps曲线算法计算所述当前有向子圆的启发代价。
在本发明实施例中,通过采用Reeds-Sheeps曲线算法计算所述当前有向子圆的启发代价,可以利用Reeds-Sheeps曲线算法具有车辆运动学约束的特点,提高启发代价的预测准确性。
具体地,在每次搜索中,选择的有向子圆均符合满足转向曲率限制、碰撞检测结果为无碰撞、第一总代价值最小的条件。换言之,如果在数学意义上计算得到的第一总代价值最小的有向子圆的碰撞检测结果为有碰撞(与障碍物具有重叠区域)或不满足转向曲率限制(基于转弯曲率上限值、父圆与有向子圆之间角度差,车辆难以在两圆中心距离内完成转向),则可以选择第一总代价值次小却满足转向曲率限制和无碰撞的有向子圆。
继续参照图1,在步骤S13的具体实施中,以首个父圆至末个有向子圆覆盖的区域作为规划子空间,在所述规划子空间内规划车辆的行进路径
参照图3,图3是本发明实施例中一种规划子空间的示意图。
在图3中,以起始节点作为首个父圆,进行三次搜索,并在第三次搜索的有向子圆到达目标节点,例如可以是第三次搜索的一个或多个有向子圆的区域覆盖目标节点,还可以是目标节点落在第三次搜索的一个或多个有向子圆的圆周上。
由于每次选择的有向子圆均满足转向曲率限制、碰撞检测结果为无碰撞的条件,因此规划子空间具有避障、能够行驶等特点。
进一步地,在每次搜索时,形成父圆的第一半径大于或等于形成所述有向子圆的第二半径。
在本发明实施例中,形成父圆的第一半径大于或等于形成所述有向子圆的第二半径,可以使各个父圆和对应的子圆采用一致的半径,还可以在离终点较远时采用较大半径搜索,从而提高搜索效率,以及在离终点较近时采用较小半径搜索,从而提高搜索精度和避障效果。
进一步地,在以首个父圆至末个有向子圆覆盖的区域作为规划子空间,在所述规划子空间内规划车辆的行进路径之前,所述方法还可以包括:步骤A:将各个计算过所述第一总代价值的有向子圆的信息添加至第一开放列表中,所述有向子圆的信息包括该有向子圆的圆心、生成角度以及第一总代价值;步骤B:每当进行多次搜索直至到达所述目标节点之后,计算当前路径的第一路径总代价值,并且将沿途的父圆移至第一封闭列表中;步骤C:判断所述第一开放列表中是否存在最小的第一总代价值小于所述第一路径总代价值;步骤D:如果存在,则采用该第一总代价值最小的有向子圆进行搜索直至再次到达所述目标节点,并返回步骤B重新计算当前路径的第一路径总代价值,直至计算得到第一路径总代价值小于等于所述第一开放列表中最小的第一总代价值的路径,以作为有效路径;步骤E:采用所述有效路径中的最后一个有向子圆作为所述末个有向子圆。
参照图4,图4是本发明实施例中另一种路径规划方法的部分流程图。所述另一种路径规划方法可以包括图1中的步骤S11至步骤S13,还可以包括步骤S41至步骤S46。其中,步骤S41至步骤S46可以位于步骤S13之前,例如位于步骤S12与步骤S13之间。以下对各个步骤进行说明。
在步骤S41中,将各个计算过所述第一总代价值的有向子圆的信息添加至第一开放列表中,所述有向子圆的信息包括该有向子圆的圆心、生成角度以及第一总代价值。
具体地,本发明实施例中的路径规划方法遵循启发式搜索过程,可以设置第一开放列表(又可以称为开放集合Sopen)以及第一封闭列表(又可以称为封闭集合Sclose)。所述开放集合中可以用于存放值得考虑的有向子圆,封闭集合中可以用于存放经计算后可丢弃的有向子圆。
在步骤S42中,每当进行多次搜索直至到达所述目标节点之后,计算当前路径的第一路径总代价值,并且将沿途的父圆移至第一封闭列表中。
具体地,每当进行多次搜索直至到达所述目标节点之后,可以计算当前路径的第一路径总代价值,作为目标节点基于当前路径的成本f(Cgoal)。
需要指出的是,由于已经到达目标节点,相比于之前采用启发代价计算得到的第一总代价值,第一路径总代价值更贴近实际情况且更具准确性。并且在得到更贴近实际情况的第一路径总代价值后,可以将沿途的父圆移至第一封闭列表中,以避免对沿途的一系列有向圆进行重复计算。
在步骤S43中,判断第一开放列表中是否存在最小的第一总代价值小于所述第一路径总代价值,如果判断结果为是,则执行步骤S44,如果判断结果为否,则执行步骤S45。
具体地,如果此时第一开放列表中存在有向子圆的成本小于目标节点的成本f(Cgoal),则可以判断为当前路径并非最优解,并选择该有向子圆继续进行搜索。
其中,最小的第一总代价值对应的有向子圆可以是任一次搜索得到的有向子圆。
以图3示出的三次搜索为例,作为一个非限制性的例子,第一次搜索得到两个满足转向曲率限制、碰撞检测结果为无碰撞的有向子圆,且计算得到的第一总代价值分别为5和10,则选择第一总代价值为5的有向子圆作为父圆,进行第二次搜索。在第二次搜索中得到三个满足转向曲率限制、碰撞检测结果为无碰撞的有向子圆,且计算得到的第一总代价值分别为14、16和20,则选择第一总代价值为14的有向子圆作为父圆,进行第三次搜索,并到达目标节点。
需要指出的是,第一总代价值为5和14的两个有向子圆作为沿途的父圆可以移至第一封闭列表中。
由于计算得到的当前路径的第一路径总代价值可能大于在第一次搜索和/或在第二次搜索中预估的第一总代价值,例如第一路径总代价值为15,则第一次搜索得到的另一个有向子圆的第一总代价值为10,小于第一路径总代价值。
在步骤S44中,采用该第一总代价值最小的有向子圆进行搜索直至再次到达所述目标节点。并在执行步骤S44后,返回步骤S42继续执行。
继续以上述具体实施例为例,则可以采用第一次搜索得到的另一个有向子圆进行再次搜索直至到达所述目标节点,然后重新计算再次搜索得到的当前路径的第一路径总代价值,并且将沿途的父圆移至第一封闭列表中。
具体地,可以采用前文以及图1示出步骤进行搜索,可以在每次搜索中,选择的有向子圆均符合满足转向曲率限制、碰撞检测结果为无碰撞、第一总代价值最小的条件。
在步骤S45中,以第一路径总代价值小于等于所述第一开放列表中最小的第一总代价值的路径为有效路径。
在步骤S46中,采用所述有效路径中的最后一个有向子圆作为所述末个有向子圆。
在本发明实施例中,采用第一开放列表,记录各个计算过所述第一总代价值的有向子圆,并且每当进行多次搜索直至到达所述目标节点之后,均计算当前路径的第一路径总代价值,由于已经到达目标节点,相比于之前采用启发代价计算得到的第一总代价值更贴近实际情况且更具准确性,进而采用第一路径总代价值与第一开放列表中的各个有向子圆的第一总代价值进行比较,如果不存在更小值,则将当前路径作为有效路径,如果存在更小值,则判断为当前路径并非最优解路径,进而采用第一总代价值最小的有向子圆继续进行搜索,重复这个搜索过程,直至第一路径总代价值小于第一开放列表中的任何有向子圆的第一总代价值,再将该路径作为有效路径,从而提高对规划子空间的规划准确性;且由于将沿途的父圆移至第二封闭列表中,可以避免对同一个有向子圆重复计算,从而提高计算效率。
在本发明实施例中,通过多次搜索,且在每次搜索中均采用第一总代价值最小、满足转向曲率限制且碰撞检测结果为无碰撞的有向子圆作为下一次搜索的父圆,然后以首个父圆至末个有向子圆覆盖的区域作为规划子空间,在所述规划子空间内规划车辆的行进路径,可以在起始节点与目标节点之间找到由多个有向圆串接后形成的无障碍区域,进而在该无障碍区域内规划车辆行进的线形路径,相比于现有技术中,直接基于起始节点与目标节点规划一条线形路径,即使能够避开障碍物,也往往存在车辆与障碍物距离极近的情况,采用本发明实施例的方案,由于先划出的安全区域已经避开障碍物,因此在安全区域再规划行进路径,有助于进一步远离障碍物,从而有效提高避障效果。
进一步地,在所述规划子空间内规划车辆的行进路径的步骤可以包括:自所述起始节点起,进行多次行进规划直至到达所述目标节点;其中,在每次行进规划中,确定当前次行进的行进距离以及方向盘转角的多个采样角度,以得到多个行进节点,并采用第二总代价值最小的行进节点作为下次行进规划的起点。
在车辆行驶中,采用连续曲率模型作为动力学模型,为了实现平滑运动曲率,连续曲率模型处理与行进距离s成正比的恒定曲率变化us。连续曲率模型考虑了运动曲率的恒定变化率,以实现平滑的转向行为,适用于难以大幅改变其速度或转向的驾驶场景。
作为一个非限制性的例子,每个行进节点的配置空间可以是(x,y,θ,k),其中,(x,y)可以用于表示行进节点在地图上的位置,θ可以用于表示与行进节点对应的有向圆的生成角度,k可以用于表示预设的转弯曲率,例如可以为转弯曲率上限值kmax。
作为一个非限制性的例子,每个行进节点的控制空间可以是(Δs,us),其中,Δs可以用于表示当前次行进的单次行进距离,us可以用于表示当前次行进的方向盘转角角度。可以理解的是,方向盘转角的上限值usmax与转弯曲率上限值kmax相关,可以根据其中一个参数确定另一个参数。
在一种具体实施例中,每次行进规划中的行进距离可以一致,即采用相等的Δs。
在另一种具体实施例中,每次行进规划中的行进距离可以不一致。
进一步地,针对每个行进节点,可以分别确定距离最近且位于所述规划子空间内的有向子圆;在每次行进规划中,确定当前次行进的行进距离以及方向盘转角的多个采样角度的步骤可以包括:根据当前行进节点对应的有向子圆的半径,确定当前次行进的行进距离;其中,所述当前行进节点对应的有向子圆的半径越小,所述当前次行进的行进距离越小。
其中,行进节点与有向子圆的距离可以是行进节点在地图上的位置与有向子圆的圆心在地图上的位置之间的距离。每个行进节点可以根据距离度量将状态映射到规划子空间内最近的有向子圆,也即可以一对一或多对一地与位于规划子空间内的有向子圆相对应。
更进一步地,当行进节点与规划子空间内两个或两个以上有向子圆之间的距离相等时,可以采用与目标节点距离更近的有向子圆作为对应的有向子圆。
在本发明实施例中,根据当前行进节点对应的有向子圆的半径,确定当前次行进的行进距离;其中,所述当前行进节点对应的有向子圆的半径越小,所述当前次行进的行进距离越小,可以在有向子圆的半径较大(例如离终点较远时)采用较大行进距离行进,从而提高行进效率,以及在有向子圆的半径较小(例如离终点较近时)采用较小行进距离行进,从而提高行进精准性和避障效果。
进一步地,在每次行进规划中,确定当前次行进的行进距离以及方向盘转角的多个采样角度的步骤可以包括:以当前次行进的直行方向为角度中心,在[uleft,uright]范围内均匀选择多个采样角度;其中,uleft和uright分别表示在左向和右向的方向盘转角的上限值。
非限制性的,可以设置-uleft=uright。
具体地,uleft和uright分别表示在左向和右向的方向盘转角的上限值,在以当前次行进的直行方向为角度中心的情况下,采用uleft和uright作为采样角度可以对应于车辆左转和右转能够达到的最大转弯曲率。
在一种具体实施方式中,可以先设置采样数量,然后在[uleft,uright]范围内均匀选择多个采样角度。
在另一种具体实施方式中,可以先设置采样精度为Δus,例如可以设定uleft和/或uright为Δus的整数倍,则可以采用us的预设倍数进行采样,有助于确保得到的采样角度不受到采样精度的限制。
在本发明实施例中,以当前次行进的直行方向为角度中心,在[uleft,uright]范围内均匀选择多个采样角度,可以基于少量采样角度进行高效判断。
在本发明实施例中,在所述规划子空间内规划车辆的行进路径时,进行多次行进规划,并在每次行进规划中,确定当前次行进的行进距离以及方向盘转角的多个采样角度,以得到多个行进节点,并采用第二总代价值最小的行进节点作为下次行进规划的起点,由于之前形成的无障碍区域已经具备满足转向曲率限制、碰撞检测结果为无碰撞的要求,此时在无障碍区域内基于第二总代价值最小继续规划行进路径,能够进一步优化路径规划,并且进一步远离障碍物,更好地提高避障效果。
进一步地,针对每个行进节点,分别确定距离最近且位于所述规划子空间内的有向子圆;采用下述公式,确定各个行进节点的第二总代价值中的已用代价:
gcost=slength+α×sreverse+β×ssteering+γ×sopposite
其中,slength用于表示自起始节点至每个行进节点之间车辆的行进距离,sreverse用于表示自起始节点至每个行进节点之间所述车辆采用倒车档位行驶的距离,ssteering用于表示自起始节点至每个行进节点之间所述车辆在转向模式下行驶的距离,sopposite用于表示自起始节点至每个行进节点之间,所述车辆基于途径的各个行进节点对应的有向子圆进行后向运动行驶的距离,α、β、γ用于表示预设的权重系数。
其中,转向模式可以是除向前直行模式和向后直行模式(即倒车模式)之外的行驶模式,用于使车辆向斜前方转向或斜后方转向行驶。
可以理解的是,相比于向前直行或向后直行,在转向模式下控制复杂度较高,更容易发生路径绕远、扭曲振荡的问题。
其中,后向运动行驶的行驶方向可以向前(前进档位行驶),还可以向后(倒车档位行驶),后向运动可以是以对应的有向子圆的生成角度为中心,在(π/2,-π/2)范围内顺时针旋转途径的角度中选择采样角度。以图2示出的有向子圆为例,虚线有向子圆位于下方的(π/2,-π/2)范围,如果选择虚线有向子圆,则行驶方向为自上至下(或自斜上方至斜下方),与有向子圆的方向不一致。
可以理解的是,相比于前向运动行驶,在后向运动行驶下控制复杂度较高,更容易发生路径绕远、扭曲振荡的问题。
其中,α、β、γ用于表示惩罚力度的大小,其取值可以相同或不同,可以根据具体需求对α、β、γ进行设置。
在本发明实施例中,在确定各个行进节点的第二总代价值中的已用代价过程中,不仅考虑自起始节点至每个行进节点之间车辆的行进距离,还考虑对车辆采用倒车档位行驶的距离较大的路径进行惩罚从而优先选择采用前进档位行进的路径,还考虑对车辆在转向模式下行驶的距离较大的路径进行惩罚从而优先选择采用向前直行或向后直行路径,以及还考虑对车辆后向运动行驶的距离较大的路径进行惩罚从而在各个行进节点优先选择与对应的有向子圆的生成角度相近的角度行驶的路径,有助于通过适当的惩罚,减轻搜索到的路径绕远、扭曲振荡的情况。
进一步地,针对每个行进节点,分别确定距离最近且位于所述规划子空间内的有向子圆;采用下述公式,确定各个行进节点的第二总代价值中的启发代价:
h=dstate,mapcircle+dmapcircle,goal
其中,dstate,mapcircle用于表示每个行进节点在地图上的位置与对应的有向子圆的圆心之间的距离,dstate,mapcircle用于表示每个行进节点对应的有向子圆的圆心与所述目标节点之间的距离。
在本发明实施例中,在确定各个行进节点的第二总代价值中的启发代价过程中,既考虑行进节点与对应的有向子圆的圆心之间的距离,又考虑对应的有向子圆的圆心与所述目标节点之间的距离,相比于仅考虑行进节点与目标节点之间的距离,可以充分利用距离最近且位于所述规划子空间内的有向子圆,使得行进路径向串接形成规划子空间的各个有向子圆的圆心处靠拢,在先划出的安全区域已经避开障碍物的基础上,进一步远离障碍物,提高避障效果。
进一步地,在所述规划子空间内规划车辆的行进路径还包括:步骤F:将各个计算过所述第二总代价值的行进节点的信息添加至第二开放列表中,所述行进节点的信息包括该行进节点在地图上的位置、对应的有向子圆的信息以及第二总代价值;步骤G:每当进行多次行进规划直至到达所述目标节点之后,计算当前行进路径的第二路径总代价值,并且将沿途的行进节点移至第二封闭列表中;步骤H:判断所述第二开放列表中是否存在最小的第二总代价值小于所述第二路径总代价值;步骤I:如果存在,则采用该第二总代价值最小的行进节点进行行进规划直至再次到达所述目标节点,并返回步骤G重新计算当前行进路径的第二路径总代价值,直至计算得到第二路径总代价值小于等于所述第二开放列表中最小的第二总代价值的路径,以作为有效行进路径。
参照图5,图5是本发明实施例中又一种路径规划方法的部分流程图。所述又一种路径规划方法可以包括图1中的步骤S11至步骤S13,还可以包括步骤S51至步骤S56。其中,步骤S51至步骤S56可以位于步骤S13之后。以下对各个步骤进行说明。
在步骤S51中,自所述起始节点起,进行多次行进规划直至到达所述目标节点,其中,在每次行进规划中,确定当前次行进的行进距离以及方向盘转角的多个采样角度,以得到多个行进节点,并采用第二总代价值最小的行进节点作为下次行进规划的起点。
在步骤S52中,将各个计算过所述第二总代价值的行进节点的信息添加至第二开放列表中,所述行进节点的信息包括该行进节点在地图上的位置、对应的有向子圆的信息以及第二总代价值。
其中,所述对应的有向子圆可以是针对每个行进节点,分别确定的距离最近且位于所述规划子空间内的有向子圆。
对应的有向子圆的信息可以包括该有向子圆的圆心、生成角度以及第一总代价值。
具体地,本发明实施例中的路径规划方法遵循启发式搜索过程,可以设置第二开放列表(类似于前文所述的开放集合Sopen)以及第二封闭列表(类似于前文所述的封闭集合Sclose)。所述开放集合中可以用于存放值得考虑的行进节点,封闭集合中可以用于存放经计算后可丢弃的行进节点。
在步骤S53中,每当进行多次行进规划直至到达所述目标节点之后,计算当前行进路径的第二路径总代价值,并且将沿途的行进节点移至第二封闭列表中。
具体地,每当进行多次搜索直至到达所述目标节点之后,可以计算当前路径的第二路径总代价值,作为目标节点基于当前路径的成本f(Cgoal)。
需要指出的是,由于已经到达目标节点,相比于之前采用启发代价计算得到的第二总代价值,第二路径总代价值更贴近实际情况且更具准确性。并且在得到更贴近实际情况的第二路径总代价值后,可以将沿途的行进节点移至第二封闭列表中,以避免对沿途的一系列行进节点进行重复计算。
在步骤S54中,判断第二开放列表中是否存在最小的第二总代价值小于所述第二路径总代价值,如果判断结果为是,则执行步骤S55,如果判断结果为否,则执行步骤S56。
具体地,如果此时第二开放列表中存在行进节点的成本小于目标节点的成本f(Cgoal),则可以判断为当前路径并非最优解,并选择该行进节点继续进行搜索。
其中,最小的第二总代价值对应的行进节点可以是任一次行进规划得到的行进节点。
作为一个非限制性的例子,第一次行进规划计算两个行进节点,其第二总代价值分别为5和10,则选择第二总代价值为5的行进节点进行第二次行进规划。在第二次行进规划中计算三个行进节点,其第二总代价值分别为14、16和20,则选择第二总代价值为14的行进节点进行第三次行进规划,并到达目标节点。
需要指出的是,第二总代价值为5和14的两个行进节点作为沿途的行进节点可以移至第二封闭列表中。
由于计算得到的当前路径的第二路径总代价值可能大于在第一次行进规划和/或在第二次行进规划中预估的第二总代价值,例如第二路径总代价值为15,则第一次行进规划得到的另一个行进节点的第二总代价值为10,小于第二路径总代价值。
在步骤S55中,采用该第二总代价值最小的行进节点进行行进规划直至再次到达所述目标节点。并在执行步骤S55后,返回步骤S53继续执行。
继续以上述具体实施例为例,则可以采用第一次行进规划中的另一个行进节点进行再次行进规划直至到达所述目标节点,然后重新计算再次行进规划得到的当前路径的第二路径总代价值,并且将沿途的行进节点移至第二封闭列表中。
具体地,可以采用前文描述的步骤进行行进规划,可以在每次行进规划中,选择第二总代价值最小的行进节点。
在步骤S56中,以第二路径总代价值小于等于所述第二开放列表中最小的第二总代价值的路径为有效规划路径。
采用第二开放列表,记录各个计算过所述第二总代价值的行进节点,并且每当进行多次行进规划直至到达所述目标节点之后,均计算当前行进路径的第二路径总代价值,由于已经到达目标节点,相比于之前采用启发代价计算得到的第二总代价值更贴近实际情况且更具准确性,进而采用第二路径总代价值与第二开放列表中的各个行进节点的第二总代价值进行比较,如果不存在更小值,则将当前行进路径作为有效行进路径,如果存在更小值,则判断为当前行进路径并非最优解行进路径,进而采用第二总代价值最小的行进节点继续进行搜索,重复这个行进规划过程,直至第二路径总代价值小于第二开放列表中的任何行进节点的第二总代价值,再将该行进路径作为有效行进路径,从而提高对车辆路径规划的规划准确性;且由于将沿途的行进节点移至第二封闭列表中,可以避免对同一个行进节点重复计算,从而提高计算效率。
在具体实施中,有关步骤S51至步骤S56的更多详细内容请参照前文以及图1至图4中的步骤描述进行执行,此处不再赘述。
参照图6,图6是本发明实施例中一种路径规划装置的结构示意图。所述路径规划装置可以包括:
节点确定模块61,用于确定路径搜索的起始节点与目标节点;
搜索模块62,用于自所述起始节点起,进行多次搜索直至到达所述目标节点,其中,在每次搜索中,以当前节点为圆心,采用第一半径形成父圆,并以所述父圆的圆周为圆心,采用第二半径以及多个预设的生成角度生成多个有向子圆,并在满足转向曲率限制且碰撞检测结果为无碰撞的有向子圆中确定第一总代价值最小的有向子圆,以作为下一次搜索的父圆;
规划模块63,用于以首个父圆至末个有向子圆覆盖的区域作为规划子空间,在所述规划子空间内规划车辆的行进路径。
在具体实施中,上述装置可以对应于终端中具有数据处理功能的芯片;或者对应于终端中包括具有数据处理功能芯片的芯片模组,或者对应于终端。
关于该路径规划装置的原理、具体实现和有益效果请参照前文所述的关于路径规划方法的相关描述,此处不再赘述。
本发明实施例还提供了一种计算机可读存储介质,其上存储有计算机程序,所述计算机程序被处理器运行时执行上述方法的步骤。所述计算机可读存储介质例如可以包括非挥发性存储器(non-volatile)或者非瞬态(non-transitory)存储器,还可以包括光盘、机械硬盘、固态硬盘等。
具体地,在本发明实施例中,所述处理器可以为中央处理单元(centralprocessing unit,简称CPU),该处理器还可以是其他通用处理器、数字信号处理器(digital signal processor,简称DSP)、专用集成电路(application specificintegrated circuit,简称ASIC)、现成可编程门阵列(field programmable gate array,简称FPGA)或者其他可编程逻辑器件、分立门或者晶体管逻辑器件、分立硬件组件等。通用处理器可以是微处理器或者该处理器也可以是任何常规的处理器等。
还应理解,本申请实施例中的存储器可以是易失性存储器或非易失性存储器,或可包括易失性和非易失性存储器两者。其中,非易失性存储器可以是只读存储器(read-only memory,简称ROM)、可编程只读存储器(programmable ROM,简称PROM)、可擦除可编程只读存储器(erasable PROM,简称EPROM)、电可擦除可编程只读存储器(electricallyEPROM,简称EEPROM)或闪存。易失性存储器可以是随机存取存储器(random accessmemory,简称RAM),其用作外部高速缓存。通过示例性但不是限制性说明,许多形式的随机存取存储器(random access memory,简称RAM)可用,例如静态随机存取存储器(staticRAM,简称SRAM)、动态随机存取存储器(DRAM)、同步动态随机存取存储器(synchronousDRAM,简称SDRAM)、双倍数据速率同步动态随机存取存储器(double data rate SDRAM,简称DDR SDRAM)、增强型同步动态随机存取存储器(enhanced SDRAM,简称ESDRAM)、同步连接动态随机存取存储器(synchlink DRAM,简称SLDRAM)和直接内存总线随机存取存储器(direct rambus RAM,简称DR RAM)。
本发明实施例还提供了一种终端,包括存储器和处理器,所述存储器上存储有能够在所述处理器上运行的计算机程序,所述处理器运行所述计算机程序时执行上述方法的步骤。所述终端包括但不限于手机、计算机、平板电脑、服务器、云平台等终端设备。
应理解,本文中术语“和/或”,仅仅是一种描述关联对象的关联关系,表示可以存在三种关系,例如,A和/或B,可以表示:单独存在A,同时存在A和B,单独存在B这三种情况。另外,本文中字符“/”,表示前后关联对象是一种“或”的关系。
本申请实施例中出现的“多个”是指两个或两个以上。
本申请实施例中出现的第一、第二等描述,仅作示意与区分描述对象之用,没有次序之分,也不表示本申请实施例中对设备个数的特别限定,不能构成对本申请实施例的任何限制。
虽然本发明披露如上,但本发明并非限定于此。任何本领域技术人员,在不脱离本发明的精神和范围内,均可作各种更动与修改,因此本发明的保护范围应当以权利要求所限定的范围为准。
Claims (16)
1.一种路径规划方法,其特征在于,包括:
确定路径搜索的起始节点与目标节点;
自所述起始节点起,进行多次搜索直至到达所述目标节点,其中,在每次搜索中,以当前节点为圆心,采用第一半径形成父圆,并以所述父圆的圆周为圆心,采用第二半径以及多个预设的生成角度生成多个有向子圆,并在满足转向曲率限制且碰撞检测结果为无碰撞的有向子圆中确定第一总代价值最小的有向子圆,以作为下一次搜索的父圆;
以首个父圆至末个有向子圆覆盖的区域作为规划子空间,在所述规划子空间内规划车辆的行进路径。
2.根据权利要求1所述的路径规划方法,其特征在于,采用下述算式,确定各个有向子圆是否满足转向曲率限制:
Δθparent,child≤kmax×dparent,child
其中,Δθparent,child用于表示当前有向子圆的父圆与该有向子圆之间的角度差,kmax用于表示转弯曲率上限值,dparent,child用于表示在地图上所述当前有向子圆的圆心与父圆的圆心之间的距离。
3.根据权利要求1所述的路径规划方法,其特征在于,采用多个预设的生成角度生成多个有向子圆包括:
以所述父圆的生成角度为角度中心,在[-π/2,π/2]范围内选择多个角度作为各个有向子圆的生成角度。
4.根据权利要求1所述的路径规划方法,其特征在于,在以首个父圆至末个有向子圆覆盖的区域作为规划子空间,在所述规划子空间内规划车辆的行进路径之前,所述方法还包括:
步骤A:将各个计算过所述第一总代价值的有向子圆的信息添加至第一开放列表中,所述有向子圆的信息包括该有向子圆的圆心、生成角度以及第一总代价值;
步骤B:每当进行多次搜索直至到达所述目标节点之后,计算当前路径的第一路径总代价值,并且将沿途的父圆移至第一封闭列表中;
步骤C:判断所述第一开放列表中是否存在最小的第一总代价值小于所述第一路径总代价值;
步骤D:如果存在,则采用该第一总代价值最小的有向子圆进行搜索直至再次到达所述目标节点,并返回步骤B重新计算当前路径的第一路径总代价值,直至计算得到第一路径总代价值小于等于所述第一开放列表中最小的第一总代价值的路径,以作为有效路径;
步骤E:采用所述有效路径中的最后一个有向子圆作为所述末个有向子圆。
5.根据权利要求1所述的路径规划方法,其特征在于,在每次搜索时,形成父圆的第一半径大于或等于形成所述有向子圆的第二半径。
6.根据权利要求1所述的路径规划方法,其特征在于,采用下述公式,确定第一总代价值:
f=g+h
其中,f用于表示当前有向子圆的总代价值,g用于表示起始节点至所述当前有向子圆的圆心的已用代价,h用于表示所述当前有向子圆的启发代价。
7.根据权利要求6所述的路径规划方法,其特征在于,采用Reeds-Sheeps曲线算法计算所述当前有向子圆的启发代价。
8.根据权利要求1所述的路径规划方法,其特征在于,在所述规划子空间内规划车辆的行进路径包括:
自所述起始节点起,进行多次行进规划直至到达所述目标节点;
其中,在每次行进规划中,确定当前次行进的行进距离以及方向盘转角的多个采样角度,以得到多个行进节点,并采用第二总代价值最小的行进节点作为下次行进规划的起点。
9.根据权利要求8所述的路径规划方法,其特征在于,针对每个行进节点,分别确定距离最近且位于所述规划子空间内的有向子圆;
在每次行进规划中,确定当前次行进的行进距离以及方向盘转角的多个采样角度包括:
根据当前行进节点对应的有向子圆的半径,确定当前次行进的行进距离;其中,所述当前行进节点对应的有向子圆的半径越小,所述当前次行进的行进距离越小。
10.根据权利要求8所述的路径规划方法,其特征在于,在每次行进规划中,确定当前次行进的行进距离以及方向盘转角的多个采样角度包括:
以当前次行进的直行方向为角度中心,在[uleft,uright]范围内均匀选择多个采样角度;
其中,uleft和uright分别表示在左向和右向的方向盘转角的上限值。
11.根据权利要求8所述的路径规划方法,其特征在于,针对每个行进节点,分别确定距离最近且位于所述规划子空间内的有向子圆;
采用下述公式,确定各个行进节点的第二总代价值中的已用代价:
gcost=slength+α×sreverse+β×ssteering+γ×sopposite
其中,slength用于表示自起始节点至每个行进节点之间车辆的行进距离,sreverse用于表示自起始节点至每个行进节点之间所述车辆采用倒车档位行驶的距离,ssteering用于表示自起始节点至每个行进节点之间所述车辆在转向模式下行驶的距离,sopposite用于表示自起始节点至每个行进节点之间,所述车辆基于途径的各个行进节点对应的有向子圆进行后向运动行驶的距离,α、β、γ用于表示预设的权重系数。
12.根据权利要求8所述的路径规划方法,其特征在于,针对每个行进节点,分别确定距离最近且位于所述规划子空间内的有向子圆;
采用下述公式,确定各个行进节点的第二总代价值中的启发代价:
h=dstate,mapcircle+dmapcircle,goal
其中,dstate,mapcircle用于表示每个行进节点在地图上的位置与对应的有向子圆的圆心之间的距离,dstate,mapcircle用于表示每个行进节点对应的有向子圆的圆心与所述目标节点之间的距离。
13.根据权利要求8所述的路径规划方法,其特征在于,在所述规划子空间内规划车辆的行进路径还包括:
步骤F:将各个计算过所述第二总代价值的行进节点的信息添加至第二开放列表中,所述行进节点的信息包括该行进节点在地图上的位置、对应的有向子圆的信息以及第二总代价值;
步骤G:每当进行多次行进规划直至到达所述目标节点之后,计算当前行进路径的第二路径总代价值,并且将沿途的行进节点移至第二封闭列表中;
步骤H:判断所述第二开放列表中是否存在最小的第二总代价值小于所述第二路径总代价值;
步骤I:如果存在,则采用该第二总代价值最小的行进节点进行行进规划直至再次到达所述目标节点,并返回步骤G重新计算当前行进路径的第二路径总代价值,直至计算得到第二路径总代价值小于等于所述第二开放列表中最小的第二总代价值的路径,以作为有效行进路径。
14.一种路径规划装置,其特征在于,包括:
节点确定模块,用于确定路径搜索的起始节点与目标节点;
搜索模块,用于自所述起始节点起,进行多次搜索直至到达所述目标节点,其中,在每次搜索中,以当前节点为圆心,采用第一半径形成父圆,并以所述父圆的圆周为圆心,采用第二半径以及多个预设的生成角度生成多个有向子圆,并在满足转向曲率限制且碰撞检测结果为无碰撞的有向子圆中确定第一总代价值最小的有向子圆,以作为下一次搜索的父圆;
规划模块,用于以首个父圆至末个有向子圆覆盖的区域作为规划子空间,在所述规划子空间内规划车辆的行进路径。
15.一种计算机可读存储介质,其上存储有计算机程序,其特征在于,所述计算机程序被处理器运行时执行权利要求1至13任一项所述路径规划方法的步骤。
16.一种终端,包括存储器和处理器,所述存储器上存储有能够在所述处理器上运行的计算机程序,其特征在于,所述处理器运行所述计算机程序时执行权利要求1至13任一项所述路径规划方法的步骤。
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202210659367.9A CN115309144A (zh) | 2022-06-10 | 2022-06-10 | 路径规划方法及装置、计算机可读存储介质、终端 |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202210659367.9A CN115309144A (zh) | 2022-06-10 | 2022-06-10 | 路径规划方法及装置、计算机可读存储介质、终端 |
Publications (1)
Publication Number | Publication Date |
---|---|
CN115309144A true CN115309144A (zh) | 2022-11-08 |
Family
ID=83854348
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
CN202210659367.9A Pending CN115309144A (zh) | 2022-06-10 | 2022-06-10 | 路径规划方法及装置、计算机可读存储介质、终端 |
Country Status (1)
Country | Link |
---|---|
CN (1) | CN115309144A (zh) |
-
2022
- 2022-06-10 CN CN202210659367.9A patent/CN115309144A/zh active Pending
Similar Documents
Publication | Publication Date | Title |
---|---|---|
US20200265710A1 (en) | Travelling track prediction method and device for vehicle | |
CN112068545B (zh) | 一种无人驾驶车辆在十字路口的行驶轨迹规划方法、系统及存储介质 | |
CN110728014B (zh) | 使用具有加权几何成本的分段螺旋曲线的参考线平滑方法 | |
US10754341B2 (en) | Systems and methods for accelerated curve projection | |
CN110361013B (zh) | 一种用于车辆模型的路径规划系统及方法 | |
CN111830979B (zh) | 一种轨迹优化方法和装置 | |
CN112577491A (zh) | 一种基于改进人工势场法的机器人路径规划方法 | |
CN110189547B (zh) | 一种障碍物检测方法、装置及车辆 | |
CN112673234A (zh) | 路径规划方法和路径规划装置 | |
CN113932823A (zh) | 基于语义道路地图的无人驾驶多目标点轨迹并行规划方法 | |
CN112327830B (zh) | 车辆自动驾驶变道轨迹的规划方法及电子设备 | |
CN112078594B (zh) | 一种智能泊车系统的曲率连续泊车路径规划装置和方法 | |
CN113895463B (zh) | 一种适用于自动驾驶车辆掉头的路径规划方法 | |
CN113721637A (zh) | 智能车动态避障路径连续规划方法、系统及存储介质 | |
CN111959498A (zh) | 一种自动驾驶车辆的垂直泊车方法及其装置、车辆 | |
CN110262506A (zh) | 预瞄点的确认方法、车辆行驶控制方法、装置及设备 | |
CN113670305A (zh) | 泊车轨迹生成方法、装置、计算机设备和存储介质 | |
CN116182884A (zh) | 基于frenet坐标系横纵解耦的智能车辆局部路径规划方法 | |
CN116476840B (zh) | 变道行驶方法、装置、设备及存储介质 | |
CN115230731A (zh) | 行驶路径确定方法、装置、终端及介质 | |
CN115309144A (zh) | 路径规划方法及装置、计算机可读存储介质、终端 | |
CN116605231A (zh) | 纯跟踪算法的预瞄点确定方法、装置、车辆以及存储介质 | |
CN115542899A (zh) | 车辆路径跟踪的方法、装置、车辆、电子设备及介质 | |
CN114750782A (zh) | 路径规划方法、装置、设备及车辆控制方法、装置、设备 | |
US11767033B2 (en) | System and method for avoiding collision based on vision |
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 |