CN113758484A - 路径规划方法和装置 - Google Patents
路径规划方法和装置 Download PDFInfo
- Publication number
- CN113758484A CN113758484A CN202011380577.1A CN202011380577A CN113758484A CN 113758484 A CN113758484 A CN 113758484A CN 202011380577 A CN202011380577 A CN 202011380577A CN 113758484 A CN113758484 A CN 113758484A
- Authority
- CN
- China
- Prior art keywords
- path
- smooth
- cloud data
- point cloud
- determining
- 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 58
- 238000001514 detection method Methods 0.000 claims abstract description 69
- 238000009499 grossing Methods 0.000 claims abstract description 36
- 238000012545 processing Methods 0.000 claims abstract description 16
- 230000011218 segmentation Effects 0.000 claims description 22
- 230000004888 barrier function Effects 0.000 claims description 10
- 238000004590 computer program Methods 0.000 claims description 9
- 238000004364 calculation method Methods 0.000 claims description 6
- 230000006870 function Effects 0.000 description 103
- 238000010586 diagram Methods 0.000 description 20
- 238000004422 calculation algorithm Methods 0.000 description 10
- 230000008569 process Effects 0.000 description 8
- 238000004891 communication Methods 0.000 description 6
- 238000002939 conjugate gradient method Methods 0.000 description 6
- 230000003287 optical effect Effects 0.000 description 4
- 230000008901 benefit Effects 0.000 description 3
- 230000004048 modification Effects 0.000 description 3
- 238000012986 modification Methods 0.000 description 3
- 238000005457 optimization Methods 0.000 description 3
- 238000012887 quadratic function Methods 0.000 description 3
- 239000013598 vector Substances 0.000 description 3
- 238000010276 construction Methods 0.000 description 2
- 230000007423 decrease Effects 0.000 description 2
- 230000000694 effects Effects 0.000 description 2
- 239000000835 fiber Substances 0.000 description 2
- 238000007726 management method Methods 0.000 description 2
- 230000000644 propagated effect Effects 0.000 description 2
- 239000004065 semiconductor Substances 0.000 description 2
- 230000003068 static effect Effects 0.000 description 2
- 230000008859 change Effects 0.000 description 1
- 238000013461 design Methods 0.000 description 1
- 238000006073 displacement reaction Methods 0.000 description 1
- 230000006872 improvement Effects 0.000 description 1
- 239000004973 liquid crystal related substance Substances 0.000 description 1
- 238000009828 non-uniform distribution Methods 0.000 description 1
- 239000013307 optical fiber Substances 0.000 description 1
- 238000005192 partition Methods 0.000 description 1
- 238000002945 steepest descent method Methods 0.000 description 1
- 238000006467 substitution reaction Methods 0.000 description 1
- 230000000007 visual 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/20—Instruments for performing navigational calculations
-
- 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
Landscapes
- Engineering & Computer Science (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- Automation & Control Theory (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)
Abstract
本发明公开了路径规划方法和装置,涉及计算机技术领域。该方法的一具体实施方式包括:接收路径规划请求,根据路径规划请求,获取始发地和目的地,然后利用栅格地图,确定从始发地到目的地的原始路径;基于平滑路径的惩罚函数,对原始路径进行平滑处理,获得原始路径对应的平滑路径;对平滑路径进行碰撞检测,若碰撞检测通过,则确定平滑路径为请求对应的目标路径,否则,确定原始路径为请求对应的目标路径。该实施方式能够降低栅格地图的分辨率对路径的影响,加强自动化无人搬运设备行驶的安全性,提高工作效率。
Description
技术领域
本发明涉及计算机技术领域,尤其涉及一种路径规划方法和装置。
背景技术
越来越多的自动化无人搬运设备投入仓库中,如AGV小车、机器人等,能够降低人工劳动强度,提升工作效率。为了保证自动化无人搬运设备在仓库中安全行驶,不会发生碰撞,如何规划自动化无人搬运设备在仓库中的行驶路径具有重要意义。
目前,可以利用栅格地图规划自动化无人搬运设备的行驶路径。但是,采用栅格地图进行路径规划时,栅格地图的分辨率对路径的影响比较大,导致规划出的路径贴着障碍物、路径不平滑等问题,无法保证自动化无人搬运设备安全行驶,影响工作效率。
发明内容
有鉴于此,本发明实施例提供一种路径规划方法和装置,能够降低栅格地图的分辨率对路径的影响,加强自动化无人搬运设备行驶的安全性,提高工作效率。
为实现上述目的,根据本发明实施例的一个方面,提供了一种路径规划方法。
本发明实施例的一种路径规划方法,包括:接收路径规划请求,根据所述路径规划请求,获取始发地和目的地,然后利用栅格地图,确定从所述始发地到所述目的地的原始路径;基于平滑路径的惩罚函数,对所述原始路径进行平滑处理,获得所述原始路径对应的平滑路径;对所述平滑路径进行碰撞检测,若碰撞检测通过,则确定所述平滑路径为所述请求对应的目标路径,否则,确定所述原始路径为所述请求对应的目标路径。
可选地,在基于平滑路径的惩罚函数,对所述原始路径进行平滑处理之前,所述方法还包括:设置安全距离阈值,根据所述安全距离阈值,定义对障碍物进行惩罚的第一惩罚函数;设置曲率阈值,根据所述曲率阈值,定义对曲率进行惩罚的第二惩罚函数;定义对路径点进行平滑的第三惩罚函数;分别设置所述第一惩罚函数、所述第二惩罚函数和所述第三惩罚函数对应的权重;根据设置的权重、所述第一惩罚函数、所述第二惩罚函数和所述第三惩罚函数,生成所述平滑路径的惩罚函数。
可选地,所述基于平滑路径的惩罚函数,对所述原始路径进行平滑处理,获得所述原始路径对应的平滑路径,包括:对所述原始路径进行插值处理,获得所述原始路径对应的插值路径;获取所述插值路径包含的多个插值路径点,将所述多个插值路径点代入所述平滑路径的惩罚函数中,然后进行求解,获得所述原始路径对应的平滑路径。
可选地,所述方法还包括:确定所述请求对应的目标场景;采集所述目标场景内的激光雷达点云数据,其中,采集的激光雷达点云数据为障碍物点云数据;利用所述障碍物点云数据对所述目标场景进行区域划分,生成所述目标场景对应的栅格地图。
可选地,所述对所述平滑路径进行碰撞检测,若碰撞检测通过,则确定所述平滑路径为所述请求对应的目标路径,否则,确定所述原始路径为所述请求对应的目标路径,包括:根据所述障碍物点云数据,构造kd-tree;遍历所述平滑路径包含的多个平滑路径点,结合所述kd-tree,判断在每个平滑路径点对应的预设半径范围内,是否存在所述障碍物点云数据;若存在所述障碍物点云数据,则确认碰撞检测不通过,且确定所述原始路径为所述请求对应的目标路径;若不存在所述障碍物点云数据,则确认碰撞检测通过,且确定所述平滑路径为所述请求对应的目标路径。
可选地,若不存在所述障碍物点云数据,所述方法还包括:获取所述目标场景内的动态目标;遍历所述平滑路径包含的多个平滑路径点,判断所述动态目标是否在每个平滑路径点对应的预设半径范围内;若是,则确认碰撞检测不通过,且确定所述原始路径为所述请求对应的目标路径;若否,则确认碰撞检测通过,且确定所述平滑路径为所述请求对应的目标路径。
可选地,所述根据所述障碍物点云数据构造kd-tree,包括:对所述障碍物点云数据对应的两个维度进行方差计算,根据方差值将两个维度分为第一分割轴维度和第二分割轴维度,其中,所述第一分割轴维度对应的方差值大;按照所述第一分割轴维度对所述障碍物点云数据进行分割,确定所述障碍物点云数据的中位数为当前节点,并将小于所述当前节点的障碍物点云数据划分为左支,将大于所述当前节点的障碍物点云数据划分为右支,然后按照所述第二分割轴维度对所述左支和所述右支进行分割,确定所述左支的中位数和所述右支的中位数为所述当前节点对应的左子节点和右子节点;更新所述左子节点和所述右子节点为当前节点,更新所述左支和所述右支,选择更新后的当前节点对应的子节点,执行迭代,直至不存在子节点,构建出所述障碍物点云数据对应的kd-tree。
可选地,所述结合所述kd-tree,判断在每个平滑路径点对应的预设半径范围内,是否存在所述障碍物点云数据,包括:查询所述每个平滑路径点在所述kd-tree中的最近邻;判断查询的所述每个平滑路径点在所述kd-tree中的最近邻与所述每个平滑路径点之间的距离是否在预设半径范围内;若是,则确认存在所述障碍物点云数据,若否,则确认不存在所述障碍物点云数据。
为实现上述目的,根据本发明实施例的再一方面,提供了一种路径规划装置。
本发明实施例的一种路径规划装置,包括:确定模块,用于接收路径规划请求,根据所述路径规划请求,获取始发地和目的地,然后利用栅格地图,确定从所述始发地到所述目的地的原始路径;平滑处理模块,用于基于平滑路径的惩罚函数,对所述原始路径进行平滑处理,获得所述原始路径对应的平滑路径;检测模块,用于对所述平滑路径进行碰撞检测,若碰撞检测通过,则确定所述平滑路径为所述请求对应的目标路径,否则,确定所述原始路径为所述请求对应的目标路径。
可选地,所述装置还包括定义模块,用于:设置安全距离阈值,根据所述安全距离阈值,定义对障碍物进行惩罚的第一惩罚函数;设置曲率阈值,根据所述曲率阈值,定义对曲率进行惩罚的第二惩罚函数;定义对路径点进行平滑的第三惩罚函数;分别设置所述第一惩罚函数、所述第二惩罚函数和所述第三惩罚函数对应的权重;根据设置的权重、所述第一惩罚函数、所述第二惩罚函数和所述第三惩罚函数,生成所述平滑路径的惩罚函数。
可选地,所述平滑处理模块还用于:对所述原始路径进行插值处理,获得所述原始路径对应的插值路径;获取所述插值路径包含的多个插值路径点,将所述多个插值路径点代入所述平滑路径的惩罚函数中,然后进行求解,获得所述原始路径对应的平滑路径。
可选地,所述确定模块还用于:确定所述请求对应的目标场景;采集所述目标场景内的激光雷达点云数据,其中,采集的激光雷达点云数据为障碍物点云数据;利用所述障碍物点云数据对所述目标场景进行区域划分,生成所述目标场景对应的栅格地图。
可选地,所述检测模块还用于:根据所述障碍物点云数据,构造kd-tree;遍历所述平滑路径包含的多个平滑路径点,结合所述kd-tree,判断在每个平滑路径点对应的预设半径范围内,是否存在所述障碍物点云数据;若存在所述障碍物点云数据,则确认碰撞检测不通过,且确定所述原始路径为所述请求对应的目标路径;若不存在所述障碍物点云数据,则确认碰撞检测通过,且确定所述平滑路径为所述请求对应的目标路径。
可选地,所述检测模块还用于:获取所述目标场景内的动态目标;遍历所述平滑路径包含的多个平滑路径点,判断所述动态目标是否在每个平滑路径点对应的预设半径范围内;若是,则确认碰撞检测不通过,且确定所述原始路径为所述请求对应的目标路径;若否,则确认碰撞检测通过,且确定所述平滑路径为所述请求对应的目标路径。
可选地,所述检测模块还用于:对所述障碍物点云数据对应的两个维度进行方差计算,根据方差值将两个维度分为第一分割轴维度和第二分割轴维度,其中,所述第一分割轴维度对应的方差值大;按照所述第一分割轴维度对所述障碍物点云数据进行分割,确定所述障碍物点云数据的中位数为当前节点,并将小于所述当前节点的障碍物点云数据划分为左支,将大于所述当前节点的障碍物点云数据划分为右支,然后按照所述第二分割轴维度对所述左支和所述右支进行分割,确定所述左支的中位数和所述右支的中位数为所述当前节点对应的左子节点和右子节点;更新所述左子节点和所述右子节点为当前节点,更新所述左支和所述右支,选择更新后的当前节点对应的子节点,执行迭代,直至不存在子节点,构建出所述障碍物点云数据对应的kd-tree。
可选地,所述检测模块还用于:查询所述每个平滑路径点在所述kd-tree中的最近邻;判断查询的所述每个平滑路径点在所述kd-tree中的最近邻与所述每个平滑路径点之间的距离是否在预设半径范围内;若是,则确认存在所述障碍物点云数据,若否,则确认不存在所述障碍物点云数据。
为实现上述目的,根据本发明实施例的另一个方面,提供了一种电子设备。
本发明实施例的一种电子设备包括:一个或多个处理器;存储装置,用于存储一个或多个程序,当一个或多个程序被一个或多个处理器执行,使得一个或多个处理器实现本发明实施例的路径规划方法。
为实现上述目的,根据本发明实施例的又一个方面,提供了一种计算机可读介质。
本发明实施例的一种计算机可读介质,其上存储有计算机程序,程序被处理器执行时实现本发明实施例的路径规划方法。
上述发明中的一个实施例具有如下优点或有益效果:可以基于平滑路径的惩罚函数对通过栅格地图得到的原始路径进行平滑处理,获得平滑路径,实现原始路径平滑的目的,还能够降低栅格地图的分辨率对路径的影响,使规划出的平滑路径远离障碍物;还有,对平滑路径进行碰撞检测,得到目标路径,能够加强自动化无人搬运设备行驶的安全性,提高工作效率。
上述的非惯用的可选方式所具有的进一步效果将在下文中结合具体实施方式加以说明。
附图说明
附图用于更好地理解本发明,不构成对本发明的不当限定。其中:
图1是根据本发明实施例的路径规划方法的主要步骤的示意图;
图2是利用栅格地图得到的原始路径的示意图;
图3是根据本发明实施例的对原始路径进行平滑处理的主要步骤的示意图;
图4是根据本发明实施例的对平滑路径进行碰撞检测的主要步骤的示意图;
图5是根据本发明一个实施例的路径规划方法的主要流程的示意图;
图6是根据本发明另一实施例的路径规划方法的主要流程的示意图;
图7是根据本发明实施例的路径规划装置的主要模块的示意图;
图8是本发明实施例可以应用于其中的示例性系统架构图;
图9是适于用来实现本发明实施例的终端设备或服务器的计算机系统的结构示意图。
具体实施方式
以下结合附图对本发明的示范性实施例做出说明,其中包括本发明实施例的各种细节以助于理解,应当将它们认为仅仅是示范性的。因此,本领域普通技术人员应当认识到,可以对这里描述的实施例做出各种改变和修改,而不会背离本发明的范围和精神。同样,为了清楚和简明,以下的描述中省略了对公知功能和结构的描述。
图1是根据本发明实施例的路径规划方法的主要步骤的示意图。如图1所示,路径规划方法可以包括:
步骤S101,接收路径规划请求,根据路径规划请求,获取始发地和目的地,然后利用栅格地图,确定从始发地到目的地的原始路径;
步骤S102,基于平滑路径的惩罚函数,对原始路径进行平滑处理,获得原始路径对应的平滑路径;
步骤S103,对平滑路径进行碰撞检测,若碰撞检测通过,则确定平滑路径为请求对应的目标路径,否则,确定原始路径为请求对应的目标路径。
路径规划请求可以是指自动化无人搬运设备发送的请求,该请求中包括始发地和目的地,如机器人在仓库中行驶时,需要规划其行驶路线,那么机器人向服务器发送从始发地S1到目的地S2的路径规划请求。在获取路径规划请求中的始发地和目的地后,可以利用栅格地图,确定从始发地到目的地的原始路径。
在得到原始路径后,为了降低栅格地图的分辨率对路径的影响,以及为了避免规划出的路径贴着障碍物、路径不平滑等问题,可以对原始路径进行平滑处理。具体地,可以基于平滑路径的惩罚函数,对原始路径进行平滑处理,获得原始路径对应的平滑路径。其中,平滑路径的惩罚函数由第一惩罚函数、第二惩罚函数和第三惩罚函数组成;第一惩罚函数用于表示对障碍物的惩罚,以使得到的平滑路径中的路径点尽可能远离障碍物;第二惩罚函数用于表示对曲率的惩罚,以使得到的平滑路径中的路径点对应的转弯半径尽可能小;第三惩罚函数用于表示对相邻路径点之间的差值进行惩罚,以使得到的平滑路径尽可能平滑。
为了保证路径的安全性,也即,为了保证自动化无人搬运设备不会碰到障碍物,可以对平滑路径进行碰撞检测,即检测平滑路径是否会与障碍物碰撞。若检测平滑路径不会与障碍物碰撞,则确定平滑路径为请求对应的目标路径;若检测平滑路径会与障碍物碰撞,那么确定利用删除地图规划的原始路径为请求对应的目标路径。其中,目标路径为路径规划请求对应的从始发地到目的地的行驶路径,将该目标路径返回至自动化无人搬运设备,然后自动化无人搬运设备可以按照该目标路径行驶。
本发明实施例提供的路径规划技术方案,可以基于平滑路径的惩罚函数对通过栅格地图得到的原始路径进行平滑处理,获得平滑路径,实现原始路径平滑的目的,还能够降低栅格地图的分辨率对路径的影响,使规划出的平滑路径远离障碍物;还有,对平滑路径进行碰撞检测,得到目标路径,能够加强自动化无人搬运设备行驶的安全性,提高工作效率。
栅格地图是地图的一种表示方法,能够把环境划分成一系列栅格,每一栅格给定一个可能值,表示该栅格被占据的概率。或者说,栅格地图可以将环境划分为若干个相同大小的栅格,对于每个栅格确定其中是否存在障碍物。本发明实施例中,可以采用如下方法得到栅格地图:确定请求对应的目标场景;采集目标场景内的激光雷达点云数据,其中,采集的激光雷达点云数据为障碍物点云数据;利用障碍物点云数据对目标场景进行区域划分,生成目标场景对应的栅格地图。
其中,目标场景是指该请求对应的具体环境。比如,始发地和目的地均在1号仓库中,那么目标场景为1号仓库;再比如,始发地和目的地分别在1号仓库和2号仓库中,那么目标场景为1号仓库和2号仓库一起组成的环境。在确定目标场景后,可以利用激光雷达采集该目标场景内的激光雷达点云数据,需要注意的是,此处采集的激光雷达点云数据为障碍物点云数据。接着,利用障碍物点云数据对目标场景进行区域划分,最后生成目标场景对应的栅格地图。
另一方面,基于栅格地图进行路径规划的方法可以包括:A*算法(又称A-Star算法,是一种静态路网中求解最短路径最有效的直接搜索方法,也是解决许多搜索问题的有效算法)及其变种、RRT路径规划算法(即基于快速扩展随机树的路径规划算法,是一种多维空间中有效率的规划方法)及其变种,Dijkstra's Algorithm(即狄克斯特拉算法,是一种找出最快的路径使用算法)等。
采用栅格地图进行路径规划的方法中,栅格地图的分辨率对路径的影响比较大,导致规划出的路径贴着障碍物、路径不平滑等问题。图2是利用栅格地图得到的原始路径的示意图。图2中,有阴影的栅格表示障碍物,1号栅格为始发地,6号栅格为目的地,规划出来的一条原始路径为:1->2->3->4->5->6。可以发现,从3号栅格到4号栅格时,距离上方障碍物太近。假如,将4号栅格换成7号栅格,那么得到的原始路径又距离下方障碍物太近。此外,由于栅格地图是离散的,因此得到的路径会存在不平滑的问题。以图2为例,1->2->3->4->5->6组成的原始路径是不连续的、不平滑的。但是对于自动化无人搬运设备,希望规划出来的路径能够无限接近连续可导。所以,在得到原始路径后,可以基于平滑路径的惩罚函数对原始路径进行平滑处理,得到平滑路径。本发明实施例中,可以按照下述(1)至(4)生成平滑路径的惩罚函数。
(1)设置安全距离阈值,根据安全距离阈值,定义对障碍物进行惩罚的第一惩罚函数。第一惩罚函数可以表示对障碍物的惩罚,即用于惩罚与障碍物的碰撞,保证节点尽可能远离障碍物,其具体公式如下:
其中,c0表示第一惩罚函数;N表示路径节点的个数;Xi表示路径节点,其坐标向量表示为(xi,yi);Oi表示距离路径节点Xi最近的障碍物点云;d表示路径节点与障碍物之间的安全距离阈值;f0可以取二次函数,表示对障碍物的惩罚,二次函数的作用是放大障碍物与节点越来越靠近的效果。从公式1中看出,当路径节点越靠近障碍物,路径节点与障碍物之间的距离值会减小,则距离值与安全距离阈值之间的差值会减小,那么该差值的二次方的变化会更明显。
(2)设置曲率阈值,根据曲率阈值,定义对曲率进行惩罚的第二惩罚函数。其中,曲率为半径的倒数,即最小转弯半径。最小转弯半径可以反映通过最小曲率半径弯曲道路的能力和在狭窄路面上调头行驶的能力。第二惩罚函数可以表示对曲率的惩罚,保证节点对应的转弯半径尽可能小,其具体公式如下:
其中,c1表示第二惩罚函数;N表示路径节点的个数;Xi表示路径节点,其坐标向量表示为(xi,yi);k表示路径节点所允许的曲率阈值,相当于对路径节点的曲率设置的上限值;f1可以取二次函数,表示对曲率的惩罚。第二惩罚函数用于表示对曲率的惩罚,以使得到的节点对应的转弯半径尽可能小若自动化无人搬运设备的转弯半径越小,则说明该自动化无人搬运设备通过狭窄弯曲地带或绕开不可越过的障碍物的能力就越强,就越灵活。
(3)定义对路径点进行平滑的第三惩罚函数。第三惩罚函数可以表示对相邻路径节点之间的差值进行惩罚,保证得到的平滑路径尽可能平滑,其具体公式如下:
其中,c2表示第三惩罚函数;N表示路径节点的个数;Xi表示路径节点,其坐标向量表示为(xi,yi);f2可以取二次函数,计算相邻路径节点之间位移向量的差值的平方,这一项将损失值赋给非均匀分布和方向变化的节点。第三惩罚函数可以表示对相邻路径点之间的差值进行惩罚,保证得到的平滑路径尽可能平滑。
(4)分别设置第一惩罚函数、第二惩罚函数和第三惩罚函数对应的权重,然后根据设置的权重、第一惩罚函数、第二惩罚函数和第三惩罚函数生成平滑路径的惩罚函数,具体公式如下:
C=w0·c0+w1·c1+w2·c2
其中,C表示平滑路径的惩罚函数;w0、w1和w2分别表示第一惩罚函数、第二惩罚函数和第三惩罚函数对应的权重。可以看出,平滑路径的惩罚函数中包含了对障碍物的惩罚、对曲率的惩罚、以及路径点的平滑性。所以,利用生成的平滑路径的惩罚函数进行平滑处理,综合考虑了路径点的安全性、自动化无人搬运设备的运动特性以及平滑性。
图3是根据本发明实施例的对原始路径进行平滑处理的主要步骤的示意图。如图3所示,对原始路径进行平滑处理的主要步骤可以包括步骤S301和步骤S302。
步骤S301:对原始路径进行插值处理,获得原始路径对应的插值路径。
插值是指在离散数据的基础上补插连续函数,使得这条连续曲线通过全部给定的离散数据点。插值是离散函数逼近的重要方法,利用它可通过函数在有限个数据点处的取值状况,估算出函数在其他点处的近似值。由于利用栅格地图得到的原始路径为离散的、不平滑的,因此对原始路径进行插值处理,可以得到连续的路径曲线。
步骤S302:获取插值路径包含的多个插值路径点,将多个插值路径点代入平滑路径的惩罚函数中,然后进行求解,获得原始路径对应的平滑路径。
对平滑路径的惩罚函数进行求解属于一个凸优化的问题,可以采用共轭梯度法求解。共轭梯度法是介于最速下降法与牛顿法之间的一个方法,共轭梯度法不仅是解决大型线性方程组最有用的方法之一,也是解大型非线性最优化最有效的算法之一。在各种优化算法中,共轭梯度法是非常重要的一种,其优点是所需存储量小,具有步收敛性、稳定性高的特点,且不需要任何外来参数。本发明实施例中,将插值路径中包含的插值路径点代入平滑路径的惩罚函数中,然后采用共轭梯度法进行求解平滑路径的惩罚函数的最小值,最终获得原始路径对应的平滑路径。
对原始路径进行平滑处理的技术方案中,可以基于平滑路径的惩罚函数对通过栅格地图得到的原始路径进行平滑处理,获得平滑路径,实现原始路径平滑的目的,还能够降低栅格地图的分辨率对路径的影响,使规划出的平滑路径远离障碍物。
需要注意的是,利用栅格地图得到的原始路径,已经考虑了障碍物点云数据,因此不会发生直接与障碍物碰撞的问题。但是,由于平滑路径的惩罚函数中的第一惩罚函数用于惩罚与障碍物的碰撞,以使节点尽可能远离障碍物,因此利用平滑路径的惩罚函数对原始路径进行平滑处理得到的平滑路径,可能会出现平滑路径与障碍物发生碰撞的问题。为了保证自动化无人搬运设备不会碰到障碍物,需要对平滑路径进行碰撞检测,图4是根据本发明实施例的对平滑路径进行碰撞检测的主要步骤的示意图。如图4所示,本发明实施例中对平滑路径进行碰撞检测的主要步骤可以包括:
步骤S401,根据障碍物点云数据,构造kd-tree;
步骤S402,遍历平滑路径包含的多个平滑路径点,结合构造的kd-tree,判断在每个平滑路径点对应的预设半径范围内,是否存在障碍物点云数据,若是,则执行步骤S403,否则,执行步骤S404;
步骤S403,确认碰撞检测不通过,且确定原始路径为请求对应的目标路径;
步骤S404,确认碰撞检测通过,且确定平滑路径为请求对应的目标路径。
其中,kd-tree是一种分割k维数据空间的数据结构,主要应用于多维空间关键数据的搜索。本发明实施例中,可以按照如下方法根据障碍物点云数据构造kd-tree:
(1)对障碍物点云数据对应的两个维度进行方差计算,根据方差值将两个维度分为第一分割轴维度和第二分割轴维度。其中,第一分割轴维度对应的方差值大。由于本发明实施例的障碍物点云数据为二维坐标,因此对障碍物点云数据对应的两个维度进行方差计算,然后将方差值较大的维度选取为第一分割轴维度,将另一个维度选取为第二分割轴维度。
(2)按照第一分割轴维度对障碍物点云数据进行分割,确定障碍物点云数据的中位数为当前节点,并将小于当前节点的障碍物点云数据划分为左支,将大于当前节点的障碍物点云数据划分为右支,然后按照第二分割轴维度对所述左支和所述右支进行分割,确定左支的中位数和右支的中位数为当前节点对应的左子节点和右子节点。
(3)更新左子节点和右子节点为当前节点,更新左支和右支,选择更新后的当前节点对应的子节点,执行迭代,直至不存在子节点,构建出障碍物点云数据对应的kd-tree。
为了便于理解,假设障碍物点云数据为:{(2,3),(5,4),(9,6),(4,7),(8,1),(7,2)},构建kd-tree的步骤为:
(1)确定第一分割轴维度、第二分割轴维度和当前节点
发现x轴的方差较大,所以确定x轴第一分割轴维度,y轴为第一分割轴维度。因此,从{2,5,9,4,8,7}找中位数,发现{5,7}都可以,选择7作为中位数,那么当前节点为(7,2)。
(2)划分左支和右支
在x轴维度上,比较和7的大小,进行划分得到,左支:{(2,3),(5,4),(4,7)};右支:{(9,6),(8,1)}。
(3)确定子节点
左节点:在左支中找到y轴的中位数(5,4),左支数据更新为{(2,3)},右支数据更新为{(4,7)};右节点:在右支中找到y轴的中位数(9,6),左支数据更新为{(8,1)},右支数据为空。因此,当前节点(7,2)对应的左节点为(5,4),对应的右节点为(9,6)。
(4)更新(5,4)为新的当前节点,确定(5,4)的子节点
左节点:由于只有一个数据,所以左节点为(2,3);右节点:由于只有一个数据,所以右节点为(4,7)。
(5)确定(9,6)为新的当前节点,确定(9,6)的子节点
左节点:由于只有一个数据,所以左节点为(8,1);右节点为空。
按照上述(1)至(5),可以构建整个的kd-tree。
本发明实施例中,结合kd-tree判断在每个平滑路径点对应的预设半径范围内,是否存在所述障碍物点云数据,包括:查询每个平滑路径点在kd-tree中的最近邻;判断查询的每个平滑路径点在kd-tree中的最近邻与每个平滑路径点之间的距离是否在预设半径范围内;若是,则确认存在障碍物点云数据,若否,则确认不存在障碍物点云数据。其中,查询的每个平滑路径点在kd-tree中的最近邻的过程如下分析。
假设,构建障碍物点云数据{(2,3),(5,4),(9,6),(4,7),(8,1),(7,2)}对应的kd-tree,查找平滑路径点(2.1,3.1)的最近邻。首先,计算节点(7,2)与平滑路径点(2.1,3.1)的距离为6.23,根据第一分割轴维度x的维度(2.1<7),选取左支;然后,计算节点(5,4)与平滑路径点(2.1,3.1)的距离为3.03,由于3.03<6.23,根据第二分割轴维度y的维度,选取左支;接着,计算当前节点(2,3)与平滑路径点(2.1,3.1)的距离为0.14,由于0.14<3.03,根据第一分割轴维度x的维度(2.1>2),选取右支,而右支为空,因此回溯上一个节点;最后,计算(2.1,3.1)与(5,4)的分割轴{y=4}的距离,如果0.14小于距离值,说明就是最近值。如果大于距离值,说明还有可能存在值与(2.1,3.1)最近,需要往右支检索,由于0.14<0.9,因此最近邻的值为(2,3),最近距离为0.14。
另一方面,预设半径可以为自动化无人搬运设备的半径。结合构造的kd-tree,可以对平滑路径包含的每个平滑路径点进行分析。针对一个平滑路径点,判断在该平滑路径点对应的预设半径范围内是否存在障碍物点云数据。若在该平滑路径点对应的预设半径范围内存在障碍物点云数据,则说明该平滑路径点与障碍物之间的距离小于自动化无人搬运设备的半径,那么自动化无人搬运设备会与障碍物发生碰撞,此时可以确定原始路径为请求对应的目标路径。遍历平滑路径包含的每个平滑路径点,若每个平滑路径点对应的预设半径范围内均不存在障碍物点云数据,则说明自动化无人搬运设备按照平滑路径行驶,不会与障碍物发生碰撞,此时可以确定平滑路径为请求对应的目标路径。
图5是根据本发明一个实施例的路径规划方法的主要流程的示意图。如图5所示,路径规划方法的主要流程可以包括:
步骤S501,接收路径规划请求,根据路径规划请求,获取始发地和目的地;
步骤S502,利用栅格地图,确定从始发地到目的地的原始路径;
步骤S503,对原始路径进行插值处理,获得原始路径对应的插值路径;
步骤S504,获取插值路径包含的所有插值路径点,将所有插值路径点代入平滑路径的惩罚函数中,然后采用共轭梯度法求解平滑路径的惩罚函数的最小值,获得原始路径对应的平滑路径;
步骤S505,根据栅格地图中的障碍物点云数据,构造kd-tree;
步骤S506,遍历平滑路径包含的所有平滑路径点,结合kd-tree,判断在每个平滑路径点对应的预设半径范围内,是否存在障碍物点云数据,若是,则执行步骤S507,否则,执行步骤S508;
步骤S507,确认碰撞检测不通过,确定原始路径该路径规划请求对应的目标路径;
步骤S508,确认碰撞检测通过,确定平滑路径该路径规划请求对应的目标路径。
其中,栅格地图的生成过程、生成平滑路径的惩罚函数、构造kd-tree和利用kd-tree进行碰撞检测的过程在上文中已经详细解释,此处不再说明。需要注意的是,步骤S505中构造kd-tree的顺序可以根据实际情况调整,但是需要保证在步骤S506之前构造。
考虑到障碍物点云数据多为静态障碍物数据,如墙、柱子等,但是在目标场景内,可能存在动态障碍物,如行驶的设备、工作人员等。因此,为了进一步提高路径的安全性,本发明实施例的路径规划方法还可以包括:若不存在障碍物点云数据,获取目标场景内的动态目标;遍历平滑路径包含的多个平滑路径点,判断动态目标是否在每个平滑路径点对应的预设半径范围内;若是,则确认碰撞检测不通过,且确定原始路径为请求对应的目标路径;若否,则确认碰撞检测通过,且确定平滑路径为请求对应的目标路径。
其中,可以采用激光雷达获取动态目标的点云数据,也可以利用视觉传感器获取动态目标。针对一个平滑路径点,判断在该平滑路径点对应的预设半径范围内是否存在动态目标。若在该平滑路径点对应的预设半径范围内存在动态目标,则说明该平滑路径点与动态目标之间的距离小于自动化无人搬运设备的半径,那么自动化无人搬运设备会与动态目标发生碰撞,此时认为动态目标为动态障碍物,确定原始路径为请求对应的目标路径。需要注意的是,利用栅格地图得到的原始路径,已经考虑了动态目标,因此说明自动化无人搬运设备按照原始路径行驶,不会与动态目标发生碰撞。遍历平滑路径包含的每个平滑路径点,若每个平滑路径点对应的预设半径范围内均不存在动态目标,则说明自动化无人搬运设备按照平滑路径行驶,不会与动态目标发生碰撞,此时可以确定平滑路径为请求对应的目标路径。
图6是根据本发明另一实施例的路径规划方法的主要流程的示意图。如图6所示,路径规划方法的主要流程可以包括:
步骤S601,接收路径规划请求,根据路径规划请求,获取始发地和目的地;
步骤S602,利用栅格地图,确定从始发地到目的地的原始路径;
步骤S603,对原始路径进行插值处理,获得原始路径对应的插值路径;
步骤S604,获取插值路径包含的所有插值路径点,将所有插值路径点代入平滑路径的惩罚函数中,然后采用共轭梯度法求解平滑路径的惩罚函数的最小值,获得原始路径对应的平滑路径;
步骤S605,根据栅格地图中的障碍物点云数据,构造kd-tree;
步骤S606,遍历平滑路径包含的所有平滑路径点,结合kd-tree,判断在每个平滑路径点对应的预设半径范围内,是否存在障碍物点云数据,若是,则执行步骤S607,否则,执行步骤S608;
步骤S607,确认碰撞检测不通过,确定原始路径该路径规划请求对应的目标路径;
步骤S608,获取目标场景内的动态目标;
步骤S609,遍历平滑路径包含的所有平滑路径点,判断动态目标是否在每个平滑路径点对应的预设半径范围内,若是,则执行步骤S607,否则,执行步骤S610;
步骤S610,确认碰撞检测通过,确定平滑路径该路径规划请求对应的目标路径。
其中,栅格地图的生成过程、生成平滑路径的惩罚函数、构造kd-tree和利用kd-tree进行碰撞检测的过程在上文中已经详细解释,此处不再说明。需要注意的是,步骤S605中构造kd-tree的顺序可以根据实际情况调整,但是需要保证在步骤S606之前构造。此外,步骤S601至步骤S610中,首先遍历平滑路径包含的所有平滑路径点,若所有平滑路径点对应的预设半径范围内均不存在障碍物点云数据,则再次遍历平滑路径包含的所有平滑路径点,判断平滑路径点对应的预设半径范围内是否存在动态目标。作为本发明的其他实施例,还可以遍历平滑路径包含的所有平滑路径点,针对每个平滑路径点,同时判断该平滑路径点对应的预设半径范围内是否存在障碍物点云数据或动态目标,若是,则确定碰撞检测不通过。
图7是根据本发明实施例的路径规划装置的主要模块的结构示意图。如图7所示,路径规则装置700的主要模块可以包括:确定模块701、平滑处理模块702和检测模块703。
其中,确定模块701可用于:接收路径规划请求,根据路径规划请求,获取始发地和目的地,然后利用栅格地图,确定从始发地到目的地的原始路径;平滑处理模块702可用于:基于平滑路径的惩罚函数,对原始路径进行平滑处理,获得原始路径对应的平滑路径;检测模块703可用于:对平滑路径进行碰撞检测,若碰撞检测通过,则确定平滑路径为请求对应的目标路径,否则,确定原始路径为请求对应的目标路径。
本发明实施例中,路径规则装置700还可以包括:定义模块(图中未示出)。该定义模块可用于:设置安全距离阈值,根据安全距离阈值,定义对障碍物进行惩罚的第一惩罚函数;设置曲率阈值,根据曲率阈值,定义对曲率进行惩罚的第二惩罚函数;定义对路径点进行平滑的第三惩罚函数;分别设置第一惩罚函数、第二惩罚函数和第三惩罚函数对应的权重;根据设置的权重、第一惩罚函数、第二惩罚函数和第三惩罚函数,生成平滑路径的惩罚函数。
本发明实施例中,平滑处理模块702还可用于:对原始路径进行插值处理,获得原始路径对应的插值路径;获取插值路径包含的多个插值路径点,将多个插值路径点代入平滑路径的惩罚函数中,然后进行求解,获得原始路径对应的平滑路径。
本发明实施例中,确定模块701还可用于:确定请求对应的目标场景;采集目标场景内的激光雷达点云数据,其中,采集的激光雷达点云数据为障碍物点云数据;利用障碍物点云数据对目标场景进行区域划分,生成目标场景对应的栅格地图。
本发明实施例中,检测模块703还可用于:根据障碍物点云数据,构造kd-tree;遍历平滑路径包含的多个平滑路径点,结合kd-tree,判断在每个平滑路径点对应的预设半径范围内,是否存在障碍物点云数据;若存在障碍物点云数据,则确认碰撞检测不通过,且确定原始路径为请求对应的目标路径;若不存在障碍物点云数据,则确认碰撞检测通过,且确定平滑路径为请求对应的目标路径。
本发明实施例中,检测模块703还可用于:获取目标场景内的动态目标;遍历平滑路径包含的多个平滑路径点,判断动态目标是否在每个平滑路径点对应的预设半径范围内;若是,则确认碰撞检测不通过,且确定原始路径为请求对应的目标路径;若否,则确认碰撞检测通过,且确定平滑路径为请求对应的目标路径。
本发明实施例中,检测模块703还可用于:对障碍物点云数据对应的两个维度进行方差计算,根据方差值将两个维度分为第一分割轴维度和第二分割轴维度,其中,第一分割轴维度对应的方差值大;按照第一分割轴维度对障碍物点云数据进行分割,确定障碍物点云数据的中位数为当前节点,并将小于当前节点的障碍物点云数据划分为左支,将大于当前节点的障碍物点云数据划分为右支,然后按照第二分割轴维度对左支和右支进行分割,确定左支的中位数和右支的中位数为当前节点对应的左子节点和右子节点;更新左子节点和右子节点为当前节点,更新左支和右支,选择更新后的当前节点对应的子节点,执行迭代,直至不存在子节点,构建出障碍物点云数据对应的kd-tree。
本发明实施例中,检测模块703还可用于:查询每个平滑路径点在kd-tree中的最近邻;判断查询的每个平滑路径点在kd-tree中的最近邻与每个平滑路径点之间的距离是否在预设半径范围内;若是,则确认存在障碍物点云数据,若否,则确认不存在障碍物点云数据。
本发明实施例的路径规则装置,能够基于平滑路径的惩罚函数对通过栅格地图得到的原始路径进行平滑处理,获得平滑路径,实现原始路径平滑的目的,还能够降低栅格地图的分辨率对路径的影响,使规划出的平滑路径远离障碍物;还有,对平滑路径进行碰撞检测,得到目标路径,能够加强自动化无人搬运设备行驶的安全性,提高工作效率。
图8示出了可以应用本发明实施例的路径规划方法或路径规划装置的示例性系统架构800。
如图8所示,系统架构800可以包括终端设备801、802、803,网络804和服务器805。网络804用以在终端设备801、802、803和服务器805之间提供通信链路的介质。网络804可以包括各种连接类型,例如有线、无线通信链路或者光纤电缆等等。
用户可以使用终端设备801、802、803通过网络804与服务器805交互,以接收或发送消息等。例如,可以利用终端设备701、702、703进行路径规则,确定出目标路径。
终端设备801、802、803可以是具有显示屏并且支持网页浏览的各种电子设备,包括但不限于智能手机、平板电脑、膝上型便携计算机和台式计算机等等。
服务器805可以是提供各种服务的服务器,例如对用户利用终端设备801、802、803所浏览的购物类网站提供支持的后台管理服务器(仅为示例)。后台管理服务器可以对接收到的产品信息查询请求等数据进行分析等处理,并将处理结果(例如目标推送信息、产品信息--仅为示例)反馈给终端设备。
需要说明的是,本发明实施例所提供的路径规划方法一般由服务器805执行,相应地,路径规划装置一般设置于服务器805中。
应该理解,图8中的终端设备、网络和服务器的数目仅仅是示意性的。根据实现需要,可以具有任意数目的终端设备、网络和服务器。
下面参考图9,其示出了适于用来实现本发明实施例的终端设备的计算机系统900的结构示意图。图9示出的终端设备仅仅是一个示例,不应对本发明实施例的功能和使用范围带来任何限制。
如图9所示,计算机系统900包括中央处理单元(CPU)901,其可以根据存储在只读存储器(ROM)902中的程序或者从存储部分908加载到随机访问存储器(RAM)903中的程序而执行各种适当的动作和处理。在RAM 903中,还存储有系统900操作所需的各种程序和数据。CPU 901、ROM 902以及RAM 903通过总线904彼此相连。输入/输出(I/O)接口905也连接至总线904。
以下部件连接至I/O接口905:包括键盘、鼠标等的输入部分906;包括诸如阴极射线管(CRT)、液晶显示器(LCD)等以及扬声器等的输出部分907;包括硬盘等的存储部分908;以及包括诸如LAN卡、调制解调器等的网络接口卡的通信部分909。通信部分909经由诸如因特网的网络执行通信处理。驱动器910也根据需要连接至I/O接口905。可拆卸介质911,诸如磁盘、光盘、磁光盘、半导体存储器等等,根据需要安装在驱动器910上,以便于从其上读出的计算机程序根据需要被安装入存储部分908。
特别地,根据本发明公开的实施例,上文参考流程图描述的过程可以被实现为计算机软件程序。例如,本发明公开的实施例包括一种计算机程序产品,其包括承载在计算机可读介质上的计算机程序,该计算机程序包含用于执行流程图所示的方法的程序代码。在这样的实施例中,该计算机程序可以通过通信部分909从网络上被下载和安装,和/或从可拆卸介质911被安装。在该计算机程序被中央处理单元(CPU)901执行时,执行本发明的系统中限定的上述功能。
需要说明的是,本发明所示的计算机可读介质可以是计算机可读信号介质或者计算机可读存储介质或者是上述两者的任意组合。计算机可读存储介质例如可以是——但不限于——电、磁、光、电磁、红外线、或半导体的系统、装置或器件,或者任意以上的组合。计算机可读存储介质的更具体的例子可以包括但不限于:具有一个或多个导线的电连接、便携式计算机磁盘、硬盘、随机访问存储器(RAM)、只读存储器(ROM)、可擦式可编程只读存储器(EPROM或闪存)、光纤、便携式紧凑磁盘只读存储器(CD-ROM)、光存储器件、磁存储器件、或者上述的任意合适的组合。在本发明中,计算机可读存储介质可以是任何包含或存储程序的有形介质,该程序可以被指令执行系统、装置或者器件使用或者与其结合使用。而在本发明中,计算机可读的信号介质可以包括在基带中或者作为载波一部分传播的数据信号,其中承载了计算机可读的程序代码。这种传播的数据信号可以采用多种形式,包括但不限于电磁信号、光信号或上述的任意合适的组合。计算机可读的信号介质还可以是计算机可读存储介质以外的任何计算机可读介质,该计算机可读介质可以发送、传播或者传输用于由指令执行系统、装置或者器件使用或者与其结合使用的程序。计算机可读介质上包含的程序代码可以用任何适当的介质传输,包括但不限于:无线、电线、光缆、RF等等,或者上述的任意合适的组合。
附图中的流程图和框图,图示了按照本发明各种实施例的系统、方法和计算机程序产品的可能实现的体系架构、功能和操作。在这点上,流程图或框图中的每个方框可以代表一个模块、程序段、或代码的一部分,上述模块、程序段、或代码的一部分包含一个或多个用于实现规定的逻辑功能的可执行指令。也应当注意,在有些作为替换的实现中,方框中所标注的功能也可以以不同于附图中所标注的顺序发生。例如,两个接连地表示的方框实际上可以基本并行地执行,它们有时也可以按相反的顺序执行,这依所涉及的功能而定。也要注意的是,框图或流程图中的每个方框、以及框图或流程图中的方框的组合,可以用执行规定的功能或操作的专用的基于硬件的系统来实现,或者可以用专用硬件与计算机指令的组合来实现。
描述于本发明实施例中所涉及到的模块可以通过软件的方式实现,也可以通过硬件的方式来实现。所描述的模块也可以设置在处理器中,例如,可以描述为:一种处理器包括确定模块、平滑处理模块和检测模块。其中,这些模块的名称在某种情况下并不构成对该模块本身的限定,例如,确定模块还可以被描述为“接收路径规划请求,根据路径规划请求,获取始发地和目的地,然后利用栅格地图,确定从始发地到目的地的原始路径的模块”。
作为另一方面,本发明还提供了一种计算机可读介质,该计算机可读介质可以是上述实施例中描述的设备中所包含的;也可以是单独存在,而未装配入该设备中。上述计算机可读介质承载有一个或者多个程序,当上述一个或者多个程序被一个该设备执行时,使得该设备包括:接收路径规划请求,根据路径规划请求,获取始发地和目的地,然后利用栅格地图,确定从始发地到目的地的原始路径;基于平滑路径的惩罚函数,对原始路径进行平滑处理,获得原始路径对应的平滑路径;对平滑路径进行碰撞检测,若碰撞检测通过,则确定平滑路径为请求对应的目标路径,否则,确定原始路径为请求对应的目标路径。
根据本发明实施例的技术方案,可以基于平滑路径的惩罚函数对通过栅格地图得到的原始路径进行平滑处理,获得平滑路径,实现原始路径平滑的目的,还能够降低栅格地图的分辨率对路径的影响,使规划出的平滑路径远离障碍物;还有,对平滑路径进行碰撞检测,得到目标路径,能够加强自动化无人搬运设备行驶的安全性,提高工作效率。
上述具体实施方式,并不构成对本发明保护范围的限制。本领域技术人员应该明白的是,取决于设计要求和其他因素,可以发生各种各样的修改、组合、子组合和替代。任何在本发明的精神和原则之内所作的修改、等同替换和改进等,均应包含在本发明保护范围之内。
Claims (11)
1.一种路径规划方法,其特征在于,包括:
接收路径规划请求,根据所述路径规划请求,获取始发地和目的地,然后利用栅格地图,确定从所述始发地到所述目的地的原始路径;
基于平滑路径的惩罚函数,对所述原始路径进行平滑处理,获得所述原始路径对应的平滑路径;
对所述平滑路径进行碰撞检测,若碰撞检测通过,则确定所述平滑路径为所述请求对应的目标路径,否则,确定所述原始路径为所述请求对应的目标路径。
2.根据权利要求1所述的方法,其特征在于,在基于平滑路径的惩罚函数,对所述原始路径进行平滑处理之前,所述方法还包括:
设置安全距离阈值,根据所述安全距离阈值,定义对障碍物进行惩罚的第一惩罚函数;
设置曲率阈值,根据所述曲率阈值,定义对曲率进行惩罚的第二惩罚函数;
定义对路径点进行平滑的第三惩罚函数;
分别设置所述第一惩罚函数、所述第二惩罚函数和所述第三惩罚函数对应的权重;
根据设置的权重、所述第一惩罚函数、所述第二惩罚函数和所述第三惩罚函数,生成所述平滑路径的惩罚函数。
3.根据权利要求2所述的方法,其特征在于,所述基于平滑路径的惩罚函数,对所述原始路径进行平滑处理,获得所述原始路径对应的平滑路径,包括:
对所述原始路径进行插值处理,获得所述原始路径对应的插值路径;
获取所述插值路径包含的多个插值路径点,将所述多个插值路径点代入所述平滑路径的惩罚函数中,然后进行求解,获得所述原始路径对应的平滑路径。
4.根据权利要求1-3任一所述的方法,其特征在于,所述方法还包括:
确定所述请求对应的目标场景;
采集所述目标场景内的激光雷达点云数据,其中,采集的激光雷达点云数据为障碍物点云数据;
利用所述障碍物点云数据对所述目标场景进行区域划分,生成所述目标场景对应的栅格地图。
5.根据权利要求4所述的方法,其特征在于,所述对所述平滑路径进行碰撞检测,若碰撞检测通过,则确定所述平滑路径为所述请求对应的目标路径,否则,确定所述原始路径为所述请求对应的目标路径,包括:
根据所述障碍物点云数据,构造kd-tree;
遍历所述平滑路径包含的多个平滑路径点,结合所述kd-tree,判断在每个平滑路径点对应的预设半径范围内,是否存在所述障碍物点云数据;
若存在所述障碍物点云数据,则确认碰撞检测不通过,且确定所述原始路径为所述请求对应的目标路径;
若不存在所述障碍物点云数据,则确认碰撞检测通过,且确定所述平滑路径为所述请求对应的目标路径。
6.根据权利要求5所述的方法,其特征在于,若不存在所述障碍物点云数据,所述方法还包括:
获取所述目标场景内的动态目标;
遍历所述平滑路径包含的多个平滑路径点,判断所述动态目标是否在每个平滑路径点对应的预设半径范围内;
若是,则确认碰撞检测不通过,且确定所述原始路径为所述请求对应的目标路径;
若否,则确认碰撞检测通过,且确定所述平滑路径为所述请求对应的目标路径。
7.根据权利要求5所述的方法,其特征在于,所述根据所述障碍物点云数据,构造kd-tree,包括:
对所述障碍物点云数据对应的两个维度进行方差计算,根据方差值将两个维度分为第一分割轴维度和第二分割轴维度,其中,所述第一分割轴维度对应的方差值大;
按照所述第一分割轴维度对所述障碍物点云数据进行分割,确定所述障碍物点云数据的中位数为当前节点,并将小于所述当前节点的障碍物点云数据划分为左支,将大于所述当前节点的障碍物点云数据划分为右支,然后按照所述第二分割轴维度对所述左支和所述右支进行分割,确定所述左支的中位数和所述右支的中位数为所述当前节点对应的左子节点和右子节点;
更新所述左子节点和所述右子节点为当前节点,更新所述左支和所述右支,选择更新后的当前节点对应的子节点,执行迭代,直至不存在子节点,构建出所述障碍物点云数据对应的kd-tree。
8.根据权利要求7所述的方法,其特征在于,所述结合所述kd-tree,判断在每个平滑路径点对应的预设半径范围内,是否存在所述障碍物点云数据,包括:
查询所述每个平滑路径点在所述kd-tree中的最近邻;
判断查询的所述每个平滑路径点在所述kd-tree中的最近邻与所述每个平滑路径点之间的距离是否在预设半径范围内;
若是,则确认存在所述障碍物点云数据,若否,则确认不存在所述障碍物点云数据。
9.一种路径规划装置,其特征在于,包括:
确定模块,用于接收路径规划请求,根据所述路径规划请求,获取始发地和目的地,然后利用栅格地图,确定从所述始发地到所述目的地的原始路径;
平滑处理模块,用于基于平滑路径的惩罚函数,对所述原始路径进行平滑处理,获得所述原始路径对应的平滑路径;
检测模块,用于对所述平滑路径进行碰撞检测,若碰撞检测通过,则确定所述平滑路径为所述请求对应的目标路径,否则,确定所述原始路径为所述请求对应的目标路径。
10.一种电子设备,其特征在于,包括:
一个或多个处理器;
存储装置,用于存储一个或多个程序,
当所述一个或多个程序被所述一个或多个处理器执行,使得所述一个或多个处理器实现如权利要求1-8中任一所述的方法。
11.一种计算机可读介质,其上存储有计算机程序,其特征在于,所述程序被处理器执行时实现如权利要求1-8中任一所述的方法。
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202011380577.1A CN113758484A (zh) | 2020-11-30 | 2020-11-30 | 路径规划方法和装置 |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202011380577.1A CN113758484A (zh) | 2020-11-30 | 2020-11-30 | 路径规划方法和装置 |
Publications (1)
Publication Number | Publication Date |
---|---|
CN113758484A true CN113758484A (zh) | 2021-12-07 |
Family
ID=78786106
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
CN202011380577.1A Pending CN113758484A (zh) | 2020-11-30 | 2020-11-30 | 路径规划方法和装置 |
Country Status (1)
Country | Link |
---|---|
CN (1) | CN113758484A (zh) |
Citations (12)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
US7197500B1 (en) * | 1996-10-25 | 2007-03-27 | Navteq North America, Llc | System and method for use and storage of geographic data on physical media |
US20100217529A1 (en) * | 2009-02-20 | 2010-08-26 | Matei Nicolai Stroila | Determining Travel Path Features Based on Retroreflectivity |
CN102768536A (zh) * | 2012-07-20 | 2012-11-07 | 哈尔滨工程大学 | 一种基于多目标萤火虫算法的路径规划方法 |
JP2018069428A (ja) * | 2016-11-04 | 2018-05-10 | トヨタ自動車株式会社 | 動作経路計画方法 |
CN108444482A (zh) * | 2018-06-15 | 2018-08-24 | 东北大学 | 一种无人机自主寻路避障方法及系统 |
CN109190704A (zh) * | 2018-09-06 | 2019-01-11 | 中国科学院深圳先进技术研究院 | 障碍物检测的方法及机器人 |
CN109799822A (zh) * | 2019-01-30 | 2019-05-24 | 中国石油大学(华东) | 移动机器人全局平滑路径规划方法 |
CN109828588A (zh) * | 2019-03-11 | 2019-05-31 | 浙江工业大学 | 一种基于多传感器融合的机器人室内路径规划方法 |
CN111811517A (zh) * | 2020-07-15 | 2020-10-23 | 中国科学院上海微系统与信息技术研究所 | 一种动态局部路径规划方法及系统 |
CN111857160A (zh) * | 2020-08-17 | 2020-10-30 | 武汉中海庭数据技术有限公司 | 一种无人车路径规划方法和装置 |
CN111985322A (zh) * | 2020-07-14 | 2020-11-24 | 西安理工大学 | 一种基于激光雷达的道路环境要素感知方法 |
CN112000088A (zh) * | 2019-05-08 | 2020-11-27 | 北京京东乾石科技有限公司 | 路径规划方法与装置 |
-
2020
- 2020-11-30 CN CN202011380577.1A patent/CN113758484A/zh active Pending
Patent Citations (12)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
US7197500B1 (en) * | 1996-10-25 | 2007-03-27 | Navteq North America, Llc | System and method for use and storage of geographic data on physical media |
US20100217529A1 (en) * | 2009-02-20 | 2010-08-26 | Matei Nicolai Stroila | Determining Travel Path Features Based on Retroreflectivity |
CN102768536A (zh) * | 2012-07-20 | 2012-11-07 | 哈尔滨工程大学 | 一种基于多目标萤火虫算法的路径规划方法 |
JP2018069428A (ja) * | 2016-11-04 | 2018-05-10 | トヨタ自動車株式会社 | 動作経路計画方法 |
CN108444482A (zh) * | 2018-06-15 | 2018-08-24 | 东北大学 | 一种无人机自主寻路避障方法及系统 |
CN109190704A (zh) * | 2018-09-06 | 2019-01-11 | 中国科学院深圳先进技术研究院 | 障碍物检测的方法及机器人 |
CN109799822A (zh) * | 2019-01-30 | 2019-05-24 | 中国石油大学(华东) | 移动机器人全局平滑路径规划方法 |
CN109828588A (zh) * | 2019-03-11 | 2019-05-31 | 浙江工业大学 | 一种基于多传感器融合的机器人室内路径规划方法 |
CN112000088A (zh) * | 2019-05-08 | 2020-11-27 | 北京京东乾石科技有限公司 | 路径规划方法与装置 |
CN111985322A (zh) * | 2020-07-14 | 2020-11-24 | 西安理工大学 | 一种基于激光雷达的道路环境要素感知方法 |
CN111811517A (zh) * | 2020-07-15 | 2020-10-23 | 中国科学院上海微系统与信息技术研究所 | 一种动态局部路径规划方法及系统 |
CN111857160A (zh) * | 2020-08-17 | 2020-10-30 | 武汉中海庭数据技术有限公司 | 一种无人车路径规划方法和装置 |
Non-Patent Citations (3)
Title |
---|
SIZE XIAO; ADAM POSTULA; NEIL BERGMANN: "Optimal random sampling based path planning on FPGAs", 2016 26TH INTERNATIONAL CONFERENCE ON FIELD PROGRAMMABLE LOGIC AND APPLICATIONS (FPL), 29 September 2016 (2016-09-29) * |
邓磊,陈宝华,黄思远,等: "一种基于射线模型的图像定位系统", 电子学报, vol. 45, no. 1, 31 January 2017 (2017-01-31) * |
马丽莎;周文晖;龚小谨;刘济林;: "基于运动约束的泛化Field D~*路径规划", 浙江大学学报(工学版), vol. 46, no. 08, 31 August 2012 (2012-08-31) * |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
Isaiah et al. | Motion planning algorithms for the Dubins travelling salesperson problem | |
WO2020164319A1 (zh) | 一种拣货方法和装置 | |
Xu et al. | Heuristic and random search algorithm in optimization of route planning for Robot’s geomagnetic navigation | |
CN110703758A (zh) | 一种路径规划方法和装置 | |
CN110370270B (zh) | 一种防止机器人碰撞的方法和装置 | |
CN113128743A (zh) | 一种拣货路径规划方法和装置 | |
WO2016129078A1 (ja) | 経路選択装置及び経路選択プログラム | |
CN110375735B (zh) | 路径规划方法和装置 | |
CN113137972B (zh) | 路径规划的方法和装置 | |
CN113758484A (zh) | 路径规划方法和装置 | |
CN111830957A (zh) | 一种路径规划方法和装置 | |
Mantha et al. | Task allocation and route planning for robotic service networks with multiple depots in indoor environments | |
CN114036247A (zh) | 高精地图数据关联方法、装置、电子设备及存储介质 | |
Sun et al. | A multi-unmanned aerial vehicle fast path-planning method based on non-rigid hierarchical discrete grid voxel environment modeling | |
CN115127565A (zh) | 高精地图数据生成方法、装置、电子设备及存储介质 | |
CN113739798A (zh) | 路径规划方法和装置 | |
Yang et al. | Multi-agv collision avoidance path optimization for unmanned warehouse based on improved ant colony algorithm | |
CN110375752B (zh) | 一种生成导航点的方法和装置 | |
CN112697155B (zh) | 车道边线生成的方法和装置 | |
KR20220104970A (ko) | 운송 수단과 드론을 사용한 병렬 배송의 최적 해를 탐색하기 위한 rgso 스케줄링 최적화 방법 및 시스템 | |
Bulut | SP‐search‐based path planning algorithm for mobile robots using quintic trigonometric Bézier curves | |
CN111461430A (zh) | 用于生成路线信息的方法及装置 | |
CN113479198B (zh) | 一种无人车控制方法和装置 | |
CN110032177A (zh) | 一种无人设备避障的方法和系统 | |
Rathinam et al. | Optimizing Mission Times for Multiple Unmanned Vehicles with Vehicle-Target Assignment Constraints |
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 |