铲装区车辆行驶路径规划方法及可读存储介质与流程

1.本发明涉及自动驾驶路径规划技术领域，具体涉及一种铲装区车辆行驶路径规划方法及可读存储介质。


背景技术：

2.混合a*算法(hybrid a star algorithm)作为一种全局路径规划算法常被应用于商用车自动驾驶的泊车场景，但是应用在矿山车辆的自动驾驶路径规划中则鲜有提及。申请人于2021年12月31日曾经申请了名为“一种基于全局地图更新的矿山卡车路径规划方法”的发明申请，其于2022年4月12日公开，公开号为cn114326744a。该矿山卡车路径规划方法分为全局路径规划和局部路径规划两层，其中全局路径规划使用混合a*算法生成全局路径作为下层局部路径规划的输入。
3.由于铲装区地形复杂、障碍物分布不确定等特殊性，加之混合a*算法又是一种遍历地图上节点的搜索算法，其除了存在路径规划时间长，占用cpu资源高的问题外，还面临着以下挑战：路径端点存在一定的限制条件，直接使用混合a*算法难以保证路径端点处对车辆的位姿约束；路径规划的端点不确定，需要考虑选择合适的端点才能保证规划生成的全局路径最优；规划路径需要考虑前进与倒车两种状态，对于倒车路线，生成的路径过长，会影响铲装区工作效率；铲装区同时存在多辆矿车，存在同时驶入铲装区与驶出铲装区的情况，在此情况下需要避免矿车路径交叉、避免碰撞。


技术实现要素：

4.针对现有的使用混合a*算法生成矿车车辆的路径规划方法难以生成符合路径端点处位姿约束的行驶路径的问题，本发明提供一种铲装区车辆行驶路径规划方法及可读存储介质。
5.本发明的技术方案提供一种铲装区车辆行驶路径规划方法，包括如下步骤：
6.s1.确定路径起点与路径终点；
7.s2.生成探索路径步骤：使用混合a*算法从起点开始向终点探索前进，记录探索过程中得到的最优路径为探索路径；若探索过程中前进到当前点时，当前点与终点的距离不小于距离阈值，则继续探索前进；否则转向s3；
8.s3.生成连接路径步骤：若当前点与终点的距离小于距离阈值，根据探索路径在当前点的方向以及车辆在终点处的朝向使用dubins曲线或者reedsshepp曲线生成连接路径；若连接路径上不存在障碍物，转到s4，否则，转到s2；
9.s4.生成全局路径步骤：连接所述探索路径与所述连接路径即为车辆的全局路径。
10.优选地，所述生成探索路径步骤中，混合a*算法所使用的启发式函数为h＝max(d1,d2)。其中,d1为从当前点到终点的考虑避让障碍物的欧式距离，d2为从当前点到终点的不考虑障碍物的动力学距离。
11.优选地，所述生成探索路径步骤中，预设第二阈值，若当前点到起点的距离不超过
第二阈值，使用第一步长推进；若当前点到起点的距离大于第二阈值，使用第二步长推进，且第一步长小于第二步长。
12.优选地，所述确定路径起点与路径终点步骤中，遍历主路确定主路上距离铲装点最近的点为投影点；以投影点为起点向车辆行驶方向的反方向沿主路寻找符合要求的交汇点；
13.若为驶入路径。交汇点为起点，铲装点为终点；若为驶离路径，铲装点为起点，交汇点为终点。
14.优选地，所述交汇点的确定通过自投影点向车辆行驶方向的反方向沿主路以预设步长推进确定；在推进至满足截止条件时，推进终止，终止时到达的点即为交汇点。
15.优选地，所述截止条件至少满足以下任一条件时推进终止：推进的曲线距离大于预定距离值d
t
，推进的角度变化大于预定角度上限。
16.优选地，所述预定距离值d
t
由下式确定：
[0017][0018]
其中，db为基础距离，dr为修正增量；c
angle
与c
dist
分别为朝向修正权重与距离修正权重；a
diff
为铲装点的取向与到投影点的角度值，d
min
为铲装点到投影点的距离，d
std
为铲装点到投影点的距离标准值。
[0019]
优选地，所述生成连接路径步骤中，
[0020]
若reedsshepp曲线的第一段曲线为前进路线且第一段为左转路径，则舍弃该路径，变更当前点后重新生成连接路径；
[0021]
若reedsshepp曲线的第一段曲线为后退路线且第一段为右转路径，则舍弃该路径，变更当前点后重新生成连接路径。
[0022]
优选地，所述生成连接路径步骤中，将reedsshepp曲线五段倒退路线长度相加得倒车距离f
rd
，若f
rd
大于长度阈值,则舍弃该路径，变更当前点后重新生成连接路径。
[0023]
本发明的技术方案还提供一种可读存储介质，存储有计算机程序，所述计算机程序被处理器执行时，使所述处理器执行如权利要求1-9任一项所述的铲装区车辆行驶路径规划方法。
[0024]
本发明的铲装区车辆行驶路径规划方法通过结合混合a*算法与dubins/reedsshepp曲线可以快速生成满足路径起点与终点处位姿要求的行驶路径。由于不需要全程使用混合a*算法遍历地图确定最优路径。而是在路径末端采用了dubins/reedsshepp快速生成连接路径再判断障碍物的方法，可以有效地提升混合a*算法的执行效率。
附图说明
[0025]
图1为本发明的铲装区示意图；
[0026]
图2为本发明的全局路径的示意图；
[0027]
图3为本发明的推进的角度变化的示意图。
具体实施方式
[0028]
以下结合附图和具体实施例，对本发明进行详细说明，在本说明书中，附图尺寸比
例并不代表实际尺寸比例，其只用于体现各部件之间的相对位置关系与连接关系，名称相同或标号相同的部件代表相似或相同的结构，且仅限于示意的目的。
[0029]
图1为一个典型的矿山铲装区的车辆行驶路径。矿山车辆进出铲装区的主路是固定的，所有车辆经主路从外部驶入铲装区，再经主路驶出铲装区。在驶入过程中，车辆首先驶下主路进入铲装区，然后在接近铲装点的时候，倒车进入铲装点，这样使得车辆在铲装点是正向的，方便满车时正向驶出。驶出时，车辆在铲装点与主路之间的铲装区正向行驶，并在将要驶入主路时对正方向切入主路。在驶入铲装区的过程中，将车辆驶出主路的点称为驶出点；在车辆驶出铲装区的过程中，将车辆驶入主路的点称为驶入点。在本技术中，驶入点与驶出点也统称为交汇点。
[0030]
在实际场景中，主路是预先设置的，通常经过了平整压实处理，车辆在主路上行驶可以保证安全性。铲装点对于既定的任务也是确定的。因此全局路径规划的目标是设计沟通主路与铲装点之间的行驶路径，并且由图可知行驶路径分为驶入路径以及驶离路径两种情况。对于避让障碍物的行驶路径的设计而言，可以使用混合a*算法进行路径规划。但是混合a*算法只能在路径起点与终点均确定的情况下使用，而在上述场景中，交汇点并没有确定，因此无法直接应用混合a*算法。此外，在上述场景中，无论是铲装点还是在交汇点，对车辆的位姿都是有要求的，在交汇点如果车辆的朝向偏离过大，会导致车辆无法顺利驶出/驶入主路。在铲装区，车辆朝向不正确，除了会影响驶离路径的规划外，还会影响铲装效率。即使使用混合a*算法能够生成符合车辆朝向的行驶路径，但是由于混合a*算法的遍历推进过程的计算时间较长，算法的运行效率较低，无法实现快速的全局路径生成。而即使在交汇点已经确定的情况下，混合a*算法也无法处理在终点处的方向约束，因此通过混合a*算法搜索得到的路径在终点处(终点可能是铲装点或者交汇点)的朝向可能无法满足要求。
[0031]
对于图1中涉及的驶入路径与驶离路径，驶入路径的起点为交汇点(驶出点)，终点为铲装点；驶离路径的起点为铲装点，终点为交汇点(驶入点)。虽然两者的方向不同，因而起点与终点并不相同，但是两者的路径规划方法是相同的。在后文的描述中，大部分情况下只讨论路径起点与路径终点，本领域技术人员汲基于以上说明可以理解起点与终点在驶入路径与驶离路径中具有特定的含义。
[0032]
为了解决混合a*算法在铲装区路径规划中无法匹配终点车辆位姿的问题，本发明的铲装区车辆行驶路径规划方法通过结合使用混合a*算法与dubins/reedshepp曲线路径规划保证生成的全局路径在目标点处的朝向符合要求。为了达成上述目标，本铲装区车辆行驶路径规划方法的具体执行步骤包括：
[0033]
s1.确定路径起点与路径终点。如前所述，对于驶入路径，起点为交汇点，也就是驶入路径的驶出点，终点为铲装点。对于驶离路径，起点为铲装点，终点为交汇点，也就是驶离路径的驶入点。
[0034]
s2.使用混合a*算法从起点开始向终点探索前进，记录探索过程中得到的最优路径为探索路径；若探索过程中前进到当前点时，当前点与终点的距离不小于距离阈值，则继续探索前进；否则转向s3。对于该过程中距离与距离阈值相等的情况下执行s2或者s3的情形是等价的，因此同样属于本技术的保护范围。
[0035]
s3.若当前点与终点的距离小于距离阈值，根据探索路径在当前点的方向以及车辆在终点处的朝向使用dubins曲线或者reedsshepp曲线生成连接路径；若连接路径上不存
在障碍物，转到s4，否则，转到s2。
[0036]
s4.连接所述探索路径与所述连接路径即为车辆的全局路径。
[0037]
以下具体说明。
[0038]
在s1步骤中，如前所述，交汇点在实际规划时是不确定的，应当作为路径规划的内容之一。交汇点本身是全局路径的起点或者终点。交汇点的选取遵循以下原则：1.交汇点(即驶入点和驶出点)应当在挖机投影点附近，这里挖机的投影点指主路上距离铲装点最近的点；2.驶入点和驶离点应当沿主路相距一定的曲线距离，这样可以最大程度避免驶入路径和驶出路径交叉。参考图2，交汇点可以按如下步骤确定：
[0039]
s11.遍历主路确定主路上距离铲装点最近的点为投影点。
[0040]
s12.以车辆行驶方向的反方向沿主路寻找符合要求的交汇点。具体而言，以图2的示意图为例，对于驶入路径的情况，从投影点向图中路径下方推进寻找交汇点，即驶出点。对于驶出路径的情况，从投影点向图中路径上方推进寻找交汇点，即驶入点。本步骤中，由于驶入点与驶出点分置于主路上投影点的两侧，即兼顾了驶入与驶出两种情况下的路径长度，又保证了尽可能错开驶入路径与驶出路径，避免两路径之间在行进时的干涉。
[0041]
具体的推进过程中通过自投影点向前述特定的方向沿主路以一定步长逐点推进，直至达成预定的终止条件时，推进到达的点即为交汇点。
[0042]
在本技术的技术方案中，满足以下任一条件时，推进过程终止。
[0043]
1.推进的曲线距离大于预定距离值。其中预设距离应当是与铲装点到投影点的距离以及铲装点的角度取向有关的。可以理解的是，当距离越近，铲装点的取向与到投影点的角度越大时，车辆越需要足够的调整空间完成交汇，因此应当设置更大的预定距离值。对此，本技术中预定距离值d
t
通过下式确定。
[0044][0045]
其中，db为基础距离，本技术的具体实施例中可取30m，即以铲装点到主路的距离200m为基准对推进的预设距离进行修正。dr为修正增量，c
angle
与c
dist
分别为朝向修正权重与距离修正权重。adiff为铲装点的取向与到投影点的角度值，d
min
为铲装点到投影点的距离，d
std
为铲装点到投影点的距离标准值。
[0046]
上式中，(1+c
angle
·
sin(a
diff
))为朝向修正系数。当铲装点的取向av与到投影点的取向a
p
的角度值adiff为零时(由于铲装区处挖机朝向ad与车辆朝向，即铲装点取向adiff，在工作时是垂直的，此时，实际上相当于挖机朝向垂直于铲装点与投影点的连线)，该朝向修正系数取1，为标准情况。为距离修正系数，其中距离标准值d
std
在本技术中取200m，并且在铲装点到投影点的距离d
min
为200m的标准距离下，上述的距离修正系数取1。由于朝向修正系数与铲装点之间，距离修正系数与铲装点距离之间均是正相关的，这使得最后经调整后的d
t
能够动态地适应铲装点的情况确定合适的交汇点位置。
[0047]
2.推进的角度变化大于预定角度上限。即最近点的沿运行方向t
p
的切向与交汇点的沿运行方向的切向t
t
之间的夹角a
t
大于预定角度上限。该限制条件主要是用于限制车辆在交汇点的方向与铲装点朝向相差过大时造成的路径过长、效率降低的问题。如图3所示，当a
(
t)过大时，将会造成生成的路径为了匹配交汇点的朝向而过度弯曲，进而导致规划生
成的路径过长，导致车辆运行效率降低。
[0048]
3.推进的步长大于预定步长数。保证程序在极端情况下至少能够正常返回可以接受的结果。
[0049]
4.到达主路端点。此时应当停止推进并直接将相应的端点(主路的入口或出口)作为交汇点。
[0050]
在s2中，启发函数的设置决定了混合a*算法导向终点的速度，对此，本步骤中所使用的启发式函数为h＝max(d1,d2)。其中,d1为从当前点到终点的考虑避让障碍物的欧式距离，d2为从当前点到终点的不考虑障碍物的动力学曲线距离。将两者的最大值作为最终的启发式函数，有利于找到符合动力学约束的最短避障路径。
[0051]
然而，由于铲装区面积较大，铲装点与主路的距离较远，为了压缩算法的运行时间，本技术在s2中采用了变步长设计。以下为变步长的一个简化方案。预设第二阈值，在混合a*算法的推进过程中，若当前点到起点的距离不超过第二阈值，使用第一步长推进。若当前点到起点的距离大于第二阈值，使用第二步长推进。这里第一步长小于第二步长。在当前点距离起点越远时，混合a*算法需要遍历的分支越多，采用如上在远距离下增大步长的方案可以降低需要遍历的分支数量，可以有效地提高算法的运行效率。在本技术中，第二阈值为60m。在混合a*算法中，步长与每一步允许的车辆朝向变化有关，矿车的转弯半径为r＝12m，在不超过第二阈值时，每一步允许的车辆转角为θ＝7
°
，则第一步长为l1＝r
·
rad(θ)＝12
·7·
(π/180)＝1.47m。而在大于第二阈值后，每一步允许的车辆转角为θ＝15
°
，因此，第二步长为l1＝r
·
rad(θ)＝12
·
15
·
(π/180)＝3.14m。同时需要注意的是，在本技术中，步长的扩大对最终的路径影响是可以接受的，不会影响车辆的横纵向跟随。原因在于通过本技术获得的仅为上层全局路径，该路径在下发到矿车等车辆中后，还会在车辆本地通过对全局路径的插值处理完成全局路径的平滑。程序的整体设计中分为路线生成和路径平滑两个步骤。实际上用hybirda*探索出来的点虽然是满足动力学约束但是可能是弯折的，如果直接让车辆进行跟随，可能会出现频繁左右转向的情况，驾驶体验非常糟糕，所以需要进行平滑。通常的做法是将平滑问题转换成一个二次规划问题，进行数学求解。平滑算法比较多，在此不赘述。混合a*的步长计算方式为角度*半径，车辆的转弯取值范围为-45至45度，只要车辆的转弯角度在这个范围内且路线是平滑后的，车辆理论上都可以完全跟随。在a*每一步探索中无论转角还是转弯半径都小于车辆标定值，保证车辆能够跟随，而且实践证明，经上述方案生成的路径经平滑后实现车辆跟随没有问题。基于本技术的混合a*算法与动力学路径结合的方法生成规划路径的方案，一方面，通过末端的混合动力学路径规划避免了在靠近目标点时继续在地图上探索求解最优路径的过程；另一方面，通过合理的可变步长设计使得a*算法探索路径过程中所需要探索的节点相较于原始方案的节点数量成倍减少，从而提高了求解速度。经实际运行比对，基于海博智能矿车地图的路径规划实验中，原始使用混合a*算法的技术耗时39秒左右，经上述优化后的铲装区车辆行驶路径规划方法耗时降低至6秒左右。进一步结合其他方法优化后可以将路径规划时间降低至1秒内，满足矿车在矿场内的高效智能运行。
[0052]
在s3中，跟据实际情况可以使用dubins曲线或者reedsshepp曲线生成从当前点到终点的连接路径。具体到本发明的场景中，如图2所示，驶离路径是全程正向连接到主路的，没有倒车段，因此可以使用dubins或者reedsshepp曲线中的任一种作为连接路径。而对于
驶入路径，为了保证车辆在铲装点处的车头朝外，车辆需要倒车进入。因此必须使用reedsshepp曲线连接。以保证倒车段的顺利生成。然而，无论是dubins曲线还是reedsshepp曲线都是通过车辆在该曲线的两端点的位姿以及车辆的动力学条件得到的路径，均没有也无法考虑路径上的障碍物问题。对此本技术的解决方案是，先生成路径，再判断路径上是否存在障碍物，如果存在障碍物，那么变更起点重新生成。即如果存在障碍物，那么舍弃掉已经生成的连接路径，采用混合a*算法继续向前推进一步，再重复生成操作。
[0053]
同时，如图2所示，在铲装区中同时存在驶入路径与驶离路径。为了避免车辆发生碰撞，最优的解决方案是在全局路径规划时就避免路径交叉，因此对于采用reedsshepp曲线生成连接路径的情况，需要考虑reedsshepp曲线的方向性。若reedsshepp曲线的第一段曲线为前进路线且第一段标记为l_seg(方向盘往左)，则该曲线在前进过程中左转，在图2中表现为靠近驶离路径，两者存在交叉风险，则将该曲线舍弃，变更当前点后重新生成连接路径。同样的，若reedsshepp曲线的第一段曲线为后退路线且第一段标记为r_seg(方向盘往右)，则该曲线在后退过程中右转，在图2中也表现为靠近驶离路径，需要舍弃后变更当前点重新生成连接路径。
[0054]
对于某些特殊铲位，工程实践中会出现reedsshepp曲线生成的连接路径的倒退路线过长的情况，导致矿卡入位时间长。因此在reedsshepp曲线的生成中增加约束条件：将reedsshepp曲线五段倒退路线长度相加得倒车距离f
rd
，若f
rd
》50m,则舍弃当前路线，往前继续探索；否则返回成功路线。
[0055]
本领域内的技术人员应明白，本技术的实施例可提供为方法、系统、或计算机程序产品。因此，本技术可采用完全硬件实施例、完全软件实施例、或结合软件和硬件方面的实施例的形式。而且，本技术可采用在一个或多个其中包含有计算机可用程序代码的计算机可用存储介质(包括但不限于磁盘存储器、cd-rom、光学存储器等)上实施的计算机程序产品的形式。本技术是参照根据本技术实施例的方法、设备(系统)、和计算机程序产品的流程图和/或方框图来描述的。应理解可由计算机程序指令实现流程图和/或方框图中的每一流程和/或方框、以及流程图和/或方框图中的流程和/或方框的结合。可提供这些计算机程序指令到通用计算机、专用计算机、嵌入式处理机或其他可编程数据处理设备的处理器以产生一个机器，使得通过计算机或其他可编程数据处理设备的处理器执行的指令产生用于实现在流程图一个流程或多个流程和/或方框图一个方框或多个方框中指定的功能的装置。这些计算机程序指令也可存储在能引导计算机或其他可编程数据处理设备以特定方式工作的计算机可读存储器中，使得存储在该计算机可读存储器中的指令产生包括指令装置的制造品，该指令装置实现在流程图一个流程或多个流程和/或方框图一个方框或多个方框中指定的功能。这些计算机程序指令也可装载到计算机或其他可编程数据处理设备上，使得在计算机或其他可编程设备上执行一系列操作步骤以产生计算机实现的处理，从而在计算机或其他可编程设备上执行的指令提供用于实现在流程图一个流程或多个流程和/或方框图一个方框或多个方框中指定的功能的步骤。
[0056]
上述内容仅仅是对本发明的优选实施方式进行描述，并非对本发明的范围进行限定，在不脱离本发明设计精神的前提下，本领域普通技术人员对本发明的技术方案作出的各种变形和改进，均应落入本发明的权利要求书确定的保护范围内。

技术特征：
1.一种铲装区车辆行驶路径规划方法，其特征在于，包括如下步骤：s1.确定路径起点与路径终点；s2.生成探索路径步骤：使用混合a*算法从起点开始向终点探索前进，记录探索过程中得到的最优路径为探索路径；若探索过程中前进到当前点时，当前点与终点的距离不小于距离阈值，则继续探索前进；否则转向s3；s3.生成连接路径步骤：若当前点与终点的距离小于距离阈值，根据探索路径在当前点的方向以及车辆在终点处的朝向使用dubins曲线或者reedsshepp曲线生成连接路径；若连接路径上不存在障碍物，转到s4，否则，转到s2；s4.生成全局路径步骤：连接所述探索路径与所述连接路径即为车辆的全局路径。2.如权利要求1所述的铲装区车辆行驶路径规划方法，其特征在于，所述生成探索路径步骤中，混合a*算法所使用的启发式函数为h＝max(d1,d2)。其中,d1为从当前点到终点的考虑避让障碍物的欧式距离，d2为从当前点到终点的不考虑障碍物的动力学距离。3.如权利要求2所述的铲装区车辆行驶路径规划方法，其特征在于，所述生成探索路径步骤中，预设第二阈值，若当前点到起点的距离不超过第二阈值，使用第一步长推进；若当前点到起点的距离大于第二阈值，使用第二步长推进，且第一步长小于第二步长。4.如权利要求1所述的铲装区车辆行驶路径规划方法，其特征在于，所述确定路径起点与路径终点步骤中，遍历主路确定主路上距离铲装点最近的点为投影点；以投影点为起点向车辆行驶方向的反方向沿主路寻找符合要求的交汇点；若为驶入路径。交汇点为起点，铲装点为终点；若为驶离路径，铲装点为起点，交汇点为终点。5.如权利要求4所述的铲装区车辆行驶路径规划方法，其特征在于，所述交汇点的确定通过自投影点向车辆行驶方向的反方向沿主路以预设步长推进确定；在推进至满足截止条件时，推进终止，终止时到达的点即为交汇点。6.如权利要求5所述的铲装区车辆行驶路径规划方法，其特征在于，所述截止条件至少满足以下任一条件时推进终止：推进的曲线距离大于预定距离值d
t
，推进的角度变化大于预定角度上限。7.如权利要求6所述的铲装区车辆行驶路径规划方法，其特征在于，所述预定距离值d
t
由下式确定：其中，d
b
为基础距离，d
r
为修正增量；c
angle
与c
dist
分别为朝向修正权重与距离修正权重；a
diff
为铲装点的取向与到投影点的角度值，d
min
为铲装点到投影点的距离，d
std
为铲装点到投影点的距离标准值。8.如权利要求1所述的铲装区车辆行驶路径规划方法，其特征在于，所述生成连接路径步骤中，若reedsshepp曲线的第一段曲线为前进路线且第一段为左转路径，则舍弃该路径，变更当前点后重新生成连接路径；若reedsshepp曲线的第一段曲线为后退路线且第一段为右转路径，则舍弃该路径，变更当前点后重新生成连接路径。
9.如权利要求1所述的铲装区车辆行驶路径规划方法，其特征在于，所述生成连接路径步骤中，将reedsshepp曲线五段倒退路线长度相加得倒车距离f
rd
，若f
rd
大于长度阈值,则舍弃该路径，变更当前点后重新生成连接路径。10.一种可读存储介质，其特征在于，存储有计算机程序，所述计算机程序被处理器执行时，使所述处理器执行如权利要求1-9任一项所述的铲装区车辆行驶路径规划方法。

技术总结
本发明提供一种铲装区车辆行驶路径规划方法及可读存储介质，其铲装区车辆行驶路径规划方法包括如下步骤：S1.确定路径起点与路径终点；S2.使用混合A*算法探索路径；若当前点与终点的距离不小于距离,继续探索前进；否则转向S3；S3.生成连接路径；若连接路径上不存在障碍物，转到S4，否则，转到S2；S4.连接所述探索路径与所述连接路径即为车辆的全局路径。该铲装区车辆行驶路径规划方法通过结合混合A*算法与动力学曲线可以快速生成满足路径起点与终点处位姿要求的行驶路径。由于不需要全程使用混合A*算法遍历地图确定最优路径。而是在路径末端采用了动力学曲线快速生成连接路径再判断障碍物的方法，可以有效地提升混合A*算法的执行效率。执行效率。执行效率。


技术研发人员：黄蓉 黄瑞卿 王杨 王炜杰 夏齐国 王明
受保护的技术使用者：安徽海博智能科技有限责任公司
技术研发日：2023.08.01
技术公布日：2023/10/19