CN114355925B - 路径规划方法、装置、设备及计算机可读存储介质 - Google Patents
路径规划方法、装置、设备及计算机可读存储介质 Download PDFInfo
- Publication number
- CN114355925B CN114355925B CN202111632682.4A CN202111632682A CN114355925B CN 114355925 B CN114355925 B CN 114355925B CN 202111632682 A CN202111632682 A CN 202111632682A CN 114355925 B CN114355925 B CN 114355925B
- Authority
- CN
- China
- Prior art keywords
- path
- pose
- segment
- jth
- path section
- 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
- 238000000034 method Methods 0.000 title claims abstract description 92
- 238000003860 storage Methods 0.000 title abstract description 25
- 238000005304 joining Methods 0.000 claims description 17
- 238000004891 communication Methods 0.000 claims description 12
- 238000004904 shortening Methods 0.000 claims description 12
- 238000004590 computer program Methods 0.000 claims description 5
- 239000000758 substrate Substances 0.000 claims 1
- 230000036544 posture Effects 0.000 description 20
- 238000010586 diagram Methods 0.000 description 16
- 230000006870 function Effects 0.000 description 11
- 238000012545 processing Methods 0.000 description 8
- 238000005516 engineering process Methods 0.000 description 4
- 238000004519 manufacturing process Methods 0.000 description 3
- 230000003287 optical effect Effects 0.000 description 3
- 238000010845 search algorithm Methods 0.000 description 3
- 238000012512 characterization method Methods 0.000 description 2
- 238000011161 development Methods 0.000 description 2
- 230000002068 genetic effect Effects 0.000 description 2
- 230000002093 peripheral effect Effects 0.000 description 2
- 239000007787 solid Substances 0.000 description 2
- 238000012546 transfer Methods 0.000 description 2
- 241000760358 Enodes Species 0.000 description 1
- 230000009286 beneficial effect Effects 0.000 description 1
- 239000013256 coordination polymer Substances 0.000 description 1
- 238000012888 cubic function Methods 0.000 description 1
- 230000001419 dependent effect Effects 0.000 description 1
- 238000001914 filtration Methods 0.000 description 1
- 230000001788 irregular Effects 0.000 description 1
- 238000005457 optimization Methods 0.000 description 1
- 239000002245 particle Substances 0.000 description 1
- 238000012887 quadratic function Methods 0.000 description 1
- 239000004065 semiconductor Substances 0.000 description 1
- 230000007704 transition Effects 0.000 description 1
Landscapes
- Manipulator (AREA)
- Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)
Abstract
本申请实施例公开了一种路径规划方法、装置、设备及计算机可读存储介质,属于路径规划领域。该路径规划方法基于一种分布式的路径规划思想,先考虑路径规划效率,快速确定起点和终点之间的通行路径。然后,考虑移动物在通行路径上移动时的安全性,对该通行路径中较长的第一路径段,考虑移动物的长宽信息,确定第一路径段的起止位姿。对于未首尾相接的相邻两个第一路径段,也考虑移动物的长宽信息,确定这两个第一路径段之间的衔接路径,进而基于衔接路径更新通行路径。如此,在路径规划的过程中,充分考虑移动物的尺寸信息,对通行路径进行优化,使得移动物在优化后的通行路径上移动时不会与障碍物发生碰撞,路径规划更合理可靠。
Description
技术领域
本申请实施例涉及路径规划领域,特别涉及一种路径规划方法、装置、设备及计算机可读存储介质。
背景技术
随着科技的发展,移动机器人在物流、仓储、工厂生产等方面的应用越来越普遍。
有关移动机器人的路径规划是指为了完成某一搬运任务,综合考虑路径长度、遇障风险(距离障碍物距离)、执行效率等因素规划出移动机器人从起始位姿到终止位姿的通行路径。其中,位姿包括移动机器人所处的位置和移动姿态。
目前大部分的路径规划不会考虑移动机器人负载的货物(货架)的尺寸大小,而是默认移动机器人在规划出的路径上的任意一点上可以进行原地旋转而不会碰撞障碍物。
因此,亟需制定一套新的路径规划方法,以根据实际搬运货物的环境,规划出搬运货物的移动机器人从起始位姿到终止位姿的通行路径。
发明内容
本申请实施例提供了一种路径规划方法、装置、设备及计算机可读存储介质,能够在考虑移动机器人和其正在搬运的货物的整体尺寸的情况下,规划出移动机器人可以顺利通行的路径,所述技术方案如下:
一方面,提供了一种路径规划方法,所述方法包括:
根据起始位置和终点位置,确定出通行路径,所述通行路径包括多个首尾相接的路径段,且所述通行路径中相邻两个所述路径段之间存在拐点,单个所述路径段内不存在拐点;
确定所述通行路径中的第一路径段,所述第一路径段是长度不小于阈值的路径段;
根据移动物的长宽信息和所述第一路径段的通道信息,确定所述移动物在各个第一路径段上的起始位姿和终止位姿;
若存在未首尾相接的第i个第一路径段与第i+1个第一路径段,则基于所述移动物的长宽信息和第二路径段的通道信息,确定用于衔接所述第i个第一路径段上的终止位姿与所述第i+1个第一路径段上的起始位姿的衔接路径,所述第二路径段是所述通行路径中位于所述第i个第一路径段和所述第i+1个第一路径段之间的路径段;
基于所述衔接路径更新所述通行路径,得到修正后的通行路径。
可选地,所述根据所述移动物的长宽信息和所述第一路径段的通道信息,确定所述移动物在各个第一路径段上的起始位姿和终止位姿,包括:
对于所述通行路径中的第j个第一路径段,根据所述移动物的长宽信息和所述第j个第一路径段的通道信息,以符合第一移动条件为目标,确定第一起始位姿和第一终止位姿,所述第j个第一路径段的通道信息包括所述第j个第一路径段的轨迹坐标和通道宽度;
其中,所述第一起始位姿为所述移动物在所述第j个第一路径段上的起始位姿,所述第一终止位姿为所述移动物在所述第j个第一路径段上的终止位姿,所述第一起始位姿的位置为所述第j个第一路径段的起点位置,所述第一终止位姿的位置为所述第j个第一路径段的终止位置,所述第一起始位姿的姿态与所述第一终止位姿的姿态一致;
所述第一移动条件包括:所述移动物从所述第j个第一路径段上的起始位姿移动到所述第j个第一路径段上的终止位姿的过程中,所述移动物上距离障碍物最近的点与障碍物之间的距离不小于安全距离。
可选地,在无法确定出符合所述第一移动条件的第一起始位姿和第一终止位姿的情况下,所述方法还包括:
按照路径调整步长,调整所述第j个第一路径段的参考端的位置,以对所述第j个第一路径段进行缩短,得到缩短后的第j个第一路径段,所述参考端为所述第j个第一路径段的两端中的一端,所述缩短后的第j个第一路径段的长度不小于所述阈值;
根据所述移动物的长宽信息和所述缩短后的第j个第一路径段的通道信息,以符合所述第一移动条件为调整目标,对所述第一起始位姿和所述第一终止位姿进行调整,调整后的第一终止位姿为所述移动物在所述第j个第一路径段上的终止位姿,调整后的第一起始位姿为所述移动物在所述第j个第一路径段上的起始位姿。
可选地,所述参考端为所述第j个第一路径段的两端中距离所述第j个第一路径段所在通道中障碍物较近的一端;或者,所述参考端为所述第j个第一路径段的两端中的任意一端。
可选地,所述基于所述移动物的长宽信息和第二路径段的通道信息,确定用于衔接所述第i个第一路径段上的终止位姿与所述第i+1个第一路径段上的起始位姿的衔接路径,包括:
基于所述移动物的长宽信息和所述第二路径段的通道信息,以符合第二移动条件为目标,确定所述用于衔接所述第i个第一路径段上的终止位姿与所述第i+1个第一路径段上的起始位姿的衔接路径;
其中,所述第二移动条件包括:所述移动物从所述第二路径段上的起始位姿移动到所述第二路径段上的终止位姿的过程中,所述移动物上距离障碍物最近的点与障碍物之间的距离不小于安全距离。
可选地,在无法确定出符合所述第二移动条件且能够衔接所述第i个第一路径段上的终止位姿与所述第i+1个第一路径段上的起始位姿的衔接路径的情况下,所述方法还包括:
调整所述移动物在所述第i+1个第一路径段上的起始位姿的位置,以缩短所述第i+1个第一路径段,得到调整后的第i+1个第一路径段上的起始位姿,以符合所述第二移动条件为目标,确定用于衔接所述第i个第一路径段上的终止位姿与所述调整后的第i+1个第一路径段上的起始位姿的衔接路径,缩短后的第i+1个第一路径段的长度不小于所述阈值;或者,
调整所述移动物在所述第i个第一路径段上的终止位姿的位置,以缩短所述第i个第一路径段,得到调整后的第i个第一路径段上的终止位姿,以符合所述第二移动条件为目标,确定用于衔接所述调整后的第i个第一路径段上的终止位姿与所述第i+1个第一路径段上的起始位姿的衔接路径,缩短后的第i个第一路径段的长度不小于所述阈值;或者,
调整所述移动物在所述第i+1个第一路径段上的起始位姿的位置,以缩短所述第i+1个第一路径段,得到调整后的第i+1个第一路径段上的起始位姿,以及调整所述移动物在所述第i个第一路径段上的终止位姿的位置,以缩短所述第i个第一路径段,得到调整后的第i个第一路径段上的终止位姿,以符合所述第二移动条件为目标,确定用于衔接所述调整后的第i个第一路径段上的终止位姿与所述调整后的第i+1个第一路径段上的起始位姿的衔接路径,缩短后的第i+1个第一路径段和缩短后的第i个第一路径段的长度均不小于所述阈值。
另一方面,提供了一种路径规划装置,所述装置包括:
第一确定模块,用于根据起始位置和终点位置,确定出通行路径,所述通行路径包括多个首尾相接的路径段,且所述通行路径中相邻两个所述路径段之间存在拐点,单个所述路径段内不存在拐点;
第二确定模块,用于确定所述通行路径中的第一路径段,所述第一路径段是长度不小于阈值的路径段;
第三确定模块,用于根据移动物的长宽信息和所述第一路径段的通道信息,确定所述移动物在各个第一路径段上的起始位姿和终止位姿;
第四确定模块,用于若存在未首尾相接的第i个第一路径段与第i+1个第一路径段,则基于所述移动物的长宽信息和第二路径段的通道信息,确定用于衔接所述第i个第一路径段上的终止位姿与所述第i+1个第一路径段上的起始位姿的衔接路径,所述第二路径段是所述通行路径中位于所述第i个第一路径段和所述第i+1个第一路径段之间的路径段;
路径更新模块,用于基于所述衔接路径更新所述通行路径,得到修正后的通行路径。
可选地,所述第三确定模块包括:
位姿确定子模块,用于对于所述通行路径中的第j个第一路径段,根据所述移动物的长宽信息和所述第j个第一路径段的通道信息,以符合第一移动条件为目标,确定第一起始位姿和第一终止位姿,所述第j个第一路径段的通道信息包括所述第j个第一路径段的轨迹坐标和通道宽度;
其中,所述第一起始位姿为所述移动物在所述第j个第一路径段上的起始位姿,所述第一终止位姿为所述移动物在所述第j个第一路径段上的终止位姿,所述第一起始位姿的位置为所述第j个第一路径段的起点位置,所述第一终止位姿的位置为所述第j个第一路径段的终止位置,所述第一起始位姿的姿态与所述第一终止位姿的姿态一致;
所述第一移动条件包括:所述移动物从所述第j个第一路径段上的起始位姿移动到所述第j个第一路径段上的终止位姿的过程中,所述移动物上距离障碍物最近的点与障碍物之间的距离不小于安全距离。
可选地,所述第三确定模块还包括:
路径段缩短子模块,用于在无法确定出符合所述第一移动条件的第一起始位姿和第一终止位姿的情况下,按照路径调整步长,调整所述第j个第一路径段的参考端的位置,以对所述第j个第一路径段进行缩短,得到缩短后的第j个第一路径段,所述参考端为所述第j个第一路径段的两端中的一端,所述缩短后的第j个第一路径段的长度不小于所述阈值;
位姿调整子模块,用于根据所述移动物的长宽信息和所述缩短后的第j个第一路径段的通道信息,以符合所述第一移动条件为调整目标,对所述第一起始位姿和所述第一终止位姿进行调整,调整后的第一终止位姿为所述移动物在所述第j个第一路径段上的终止位姿,调整后的第一起始位姿为所述移动物在所述第j个第一路径段上的起始位姿。
可选地,所述参考端为所述第j个第一路径段的两端中距离所述第j个第一路径段所在通道中障碍物较近的一端;或者,所述参考端为所述第j个第一路径段的两端中的任意一端。
可选地,所述第四确定模块包括:
衔接路径确定子模块,用于基于所述移动物的长宽信息和所述第二路径段的通道信息,以符合第二移动条件为目标,确定所述用于衔接所述第i个第一路径段上的终止位姿与所述第i+1个第一路径段上的起始位姿的衔接路径;
其中,所述第二移动条件包括:所述移动物从所述第二路径段上的起始位姿移动到所述第二路径段上的终止位姿的过程中,所述移动物上距离障碍物最近的点与障碍物之间的距离不小于安全距离。
可选地,在无法确定出符合所述第二移动条件且能够衔接所述第i个第一路径段上的终止位姿与所述第i+1个第一路径段上的起始位姿的衔接路径的情况下,所述第四确定模块还包括:
第一衔接路径调整子模块,用于调整所述移动物在所述第i+1个第一路径段上的起始位姿的位置,以缩短所述第i+1个第一路径段,得到调整后的第i+1个第一路径段上的起始位姿,以符合所述第二移动条件为目标,确定用于衔接所述第i个第一路径段上的终止位姿与所述调整后的第i+1个第一路径段上的起始位姿的衔接路径,缩短后的第i+1个第一路径段的长度不小于所述阈值;或者,
第二衔接路径调整子模块,用于调整所述移动物在所述第i个第一路径段上的终止位姿的位置,以缩短所述第i个第一路径段,得到调整后的第i个第一路径段上的终止位姿,以符合所述第二移动条件为目标,确定用于衔接所述调整后的第i个第一路径段上的终止位姿与所述第i+1个第一路径段上的起始位姿的衔接路径,缩短后的第i个第一路径段的长度不小于所述阈值;或者,
第三衔接路径调整子模块,用于调整所述移动物在所述第i+1个第一路径段上的起始位姿的位置,以缩短所述第i+1个第一路径段,得到调整后的第i+1个第一路径段上的起始位姿,以及调整所述移动物在所述第i个第一路径段上的终止位姿的位置,以缩短所述第i个第一路径段,得到调整后的第i个第一路径段上的终止位姿,以符合所述第二移动条件为目标,确定用于衔接所述调整后的第i个第一路径段上的终止位姿与所述调整后的第i+1个第一路径段上的起始位姿的衔接路径,缩短后的第i+1个第一路径段和缩短后的第i个第一路径段的长度均不小于所述阈值。
另一方面,提供了一种计算机设备,所述计算机设备包括处理器、通信接口、存储器和通信总线;所述处理器、所述通信接口和所述存储器通过所述通信总线完成相互间的通信;所述存储器用于存放计算机程序;所述处理器用于执行所述存储器上所存放的程序,以实现上述所述的路径规划方法。
另一方面,提供了一种计算机可读存储介质,所述计算机可读存储介质中存储有至少一条程序代码,所述程序代码由处理器加载并执行以实现上述所述的路径规划方法。
另一方面,提供了一种计算机程序产品,当所述计算机程序产品被执行时,用于实现上述所述的路径规划方法。
本申请实施例提供的技术方案带来的有益效果至少包括:
本申请实施例提供的路径规划方法基于一种分布式的路径规划思想,先考虑路径规划效率,快速确定起点和终点之间的通行路径。然后,考虑移动物在通行路径上移动时的安全性,对该通行路径中较长的第一路径段,考虑移动物的长宽信息,确定第一路径段的起止位姿。对于未首尾相接的相邻两个第一路径段,也考虑移动物的长宽信息,确定这两个第一路径段之间的衔接路径,进而基于衔接路径更新通行路径。如此,在路径规划的过程中,充分考虑移动物的尺寸信息,对通行路径进行优化,使得移动物在优化后的通行路径上移动时不会与障碍物发生碰撞,路径规划更合理可靠。
附图说明
为了更清楚地说明本申请实施例中的技术方案,下面将对实施例描述中所需要使用的附图作简单地介绍,显而易见地,下面描述中的附图仅仅是本申请的一些实施例,对于本领域普通技术人员来讲,在不付出创造性劳动的前提下,还可以根据这些附图获得其他的附图。
图1是本申请实施例提供的一种移动物的尺寸示意图;
图2是本申请实施例提供的一种路径规划方法的流程图;
图3是本申请实施例提供的一种通行路径的示意图;
图4是本申请实施例提供的一种A*搜索算法的路径搜索示意图;
图5是本申请实施例提供的一种位姿示意图;
图6是本申请实施例提供的一种确定关键位姿对的方法流程图;
图7是本申请实施例提供的一种确定出的关键位姿对的示意图;
图8是本申请实施例提供的一种通过贝塞尔曲线确定衔接路径的示意图;
图9是本申请实施例提供的一种路径规划装置的结构示意图;
图10是本申请实施例提供的一种计算机设备的结构框图。
具体实施方式
为使本申请实施例的目的、技术方案和优点更加清楚,下面将结合附图对本申请实施例的实施方式作进一步地详细描述。
在对本申请实施例进行详细地解释说明之前,先对本申请实施例涉及的相关名词和应用场景予以说明。
1、路径规划
路径规划是指为了完成某一任务,综合考虑路径长度、遇障风险(距离障碍物距离)、执行效率等因素规划出从起始位置到终点位置之间的通行路径的方法。
路径规划本质是优化问题,也即是为移动物寻找一条从起始位置移动到目标位置之间的最优路径。最优路径可以是路径长度最短的一个通行路径,如此,可以缩短移动物从起始位置移动到终点位置的移动距离;最优路径也可以是遇障风险最小的一个通行路径,如此,可以保证移动物从起始位置移动到终点位置的过程中,不会与障碍物发生碰撞,安全性最高;最优路径也可以是基于多种影响因素下的路径总评分,确定的一条通行路径,移动物在该通行路径上移动时执行效率最佳,本申请实施例对此不做限制,可以在路径规划过程中,根据需求设置相应的节点代价函数。
其中,常见的路径规划方法包括两大类:搜索类算法(如A*、D*等)和进化计算类算法(遗传算法、蚁群算法等)。
2、局促环境
局促环境是指无法满足移动物在规划的通行路径上的所有点都可以实现原地旋转360°而不碰到障碍物(通行路径的边缘)的环境。
也即是,在局促环境中,规划移动物的通行路径时,需要考虑移动物在该通行路径上是否会与障碍物发生碰撞,需要根据移动物的外围尺寸,以及通行路径的长度和宽度,调整移动物在通行路径上不同位置处的移动姿态。
3、位姿
位姿即位置和姿态,在二维坐标系中,位置通常表示为(x,y),姿态(也可以称为航向或偏航)通常表示为θ。
在本申请的实施例中,针对二维平面地图,移动物的位姿包括移动物所处的位置,以及移动物在该位置的移动姿态,位姿可以表示为(x,y,θ)。
在介绍完本申请实施例涉及的相关名词后,接下来,对本申请实施例的应用场景进行介绍。
随着科技的发展,移动机器人在物流、仓储、工厂生产等方面的应用越来越普遍,在使用移动机器人搬运货物时,需要考虑移动机器人和货物的整体尺寸,以及通行路径的长度和宽度,对移动机器人和货物的整体位姿进行调整,以保证搬运货物的移动机器人可以在规划的通行路径上顺利行驶,不会碰撞到障碍物。
为了简化描述,本申请实施例在规划路径时,对于需要搬运有货物的移动机器人来说,可以将移动机器人和货物看作一个整体,即移动物,对于无需搬运货物的移动机器人来说,可以将移动机器人看作一个移动物。需要说明的是,在接下来的实施例中,以移动物包括待搬运的货物和用于搬运货物的移动机器人为例进行介绍。
参见图1,图1是本申请实施例提供的一种移动物的尺寸示意图,假设移动机器人的长度为Lc,宽度为Wc,货物的长度为Lg,宽度为Wg,图1中的虚线框表示货物,图1中的实线框表示移动机器人。
则移动机器人和货物的组合方式包括图1中(1)至(4)四种可能的情况。
(1)货物的长度小于移动机器人的长度,且货物的宽度小于移动机器人的宽度。在不考虑高度的情况下,货物和移动机器人的整体尺寸就是移动机器人的外围尺寸,此时,移动物的长度为Lc,宽度为Wc。
(2)货物的长度小于移动机器人的长度,且货物的宽度大于移动机器人的宽度。在不考虑高度的情况下,货物和移动机器人的整体长度是为移动机器人的长度,整体宽度为货物的宽度,此时,移动物的长度为Lc,宽度为Wg。
(3)货物的长度大于移动机器人的长度,且货物的宽度小于移动机器人的宽度。在不考虑高度的情况下,货物和移动机器人的整体长度为货物的长度,整体宽度为移动机器人的宽度,此时,移动物的长度为Lg,宽度为Wc。
(4)货物的长度大于移动机器人的长度,且货物的宽度大于移动机器人的宽度。在不考虑高度的情况下,货物和移动机器人的整体尺寸就是货物的外围尺寸,此时,移动物的长度为Lg,宽度为Wg。
其中,上述情况(1)和(4)可以看成一种情形,如图1中的“情形1”所示,上述情况(2)和(3)可以看成另一种情形,如图1中的“情形2”所示,针对情形2,在规划移动物的通行路径时,可以用移动物外接矩形的长度a和宽度b来表示移动物的尺寸信息。除此之外,对于其他不规则形状的货物或移动机器人组合成的移动物,都可以根据移动物的外接矩形,表示移动物的尺寸信息,尺寸信息即长宽信息。
需要说明的是,本申请实施例提供的路径规划方法可以由移动机器人来执行,也可以由任一计算机设备来执行,本申请实施例对此不做限制。
当由移动机器人来规划通行路径时,移动机器人根据地图数据、搬运货物的移动机器人的起始位置和搬运货物的移动机器人的终点位置,规划出搬运货物的移动机器人从起始位置到终点位置的通行路径。
当由计算机设备来规划通行路径时,计算机设备根据地图数据、搬运货物的移动机器人的起始位置和搬运货物的移动机器人的终点位置,规划出搬运货物的移动机器人从起点位置到终点位置的通行路径,并将该通行路径发送给搬运货物的至少一个移动机器人,移动机器人按照规划的通行路径,将待搬运的货物从起点位置搬运至终点位置。
其中,移动机器人可以是任一可以按照规划的通行路径移动的装置。示例性地,移动机器人可以是AGV(Automated Guided Vehicle,自动导引车),AGV是指装备有电磁或光学等自动导引装置、能够沿规定的导引路径行驶,具有安全保护以及各种移载功能的运输车。移动机器人可以是一个独立的机器人,也可以是包括多个协同工作的机器人,本申请对此不作限定。
基于上述应用场景,接下来,结合附图对本申请提供的路径规划方法进行解释说明。
图2是本申请实施例提供的一种路径规划方法的流程图,该方法可以用于可以执行路径规划算法的计算机设备或移动机器人中。参见图2,该方法包括:
步骤201:根据起始位置和终点位置,确定出通行路径,该通行路径包括多个首尾相接的路径段,且该通行路径中相邻两个路径段之间存在拐点,单个路径段内不存在拐点。
可选地,移动物包括待搬运的货物和用于搬运待搬运货物的移动机器人,当然,移动物也可以仅包括移动机器人。
在一种可能的实现方式中,将移动物看做一个质点,根据起始位置和终点位置,采用路径搜索算法确定出移动物的通行路径,其中路径搜索算法包括A*(A-Star)、D*(D-Star)、遗传算法、蚁群算法中至少一种。
在使用A*算法进行路径搜索之前,需要对货物所处环境的地图数据进行预处理,以明确搜索区域。
地图数据的表示可以包括:特征地图、拓扑地图、栅格地图或直接表征法。特征地图用有关的几何特征(如点、直线、面)表示环境;拓扑地图把室内环境表示为带结点和相关连接线的拓扑结构图,其中结点表示环境中的重要位置点(拐角、门、电梯、楼梯等),边表示结点间的连接关系,如走廊等。栅格地图则是把环境划分成一系列栅格,其中每一栅格给定一个可能值,表示该栅格被占据的概率。直接表征法省去了特征或栅格表示这一中间环节,直接用传感器读取的数据来构造机器人的位姿空间。
每种方法各有自己的特点和适用范围,其中特征地图和栅格地图应用最普遍,本申请实施例在此是根据地图数据栅格化处理后的搜索区域,搜索通行路径。
搜索区域栅格化可以将搜索区域简化为2维数组,数组的每一项代表一个格子,它的状态就是可走(walkalbe)和不可走(unwalkable)。假设移动物的起始位置在A点,终点位置在B点,则可以通过计算出从A点到B点需要走过哪些方格,即可找到A点到B点之间的通行路径。当找到通行路径时,移动物就可以从一个方格的中心移动到另一个方格的中心,直至到达终点位置(B点)。
其中,方格的中心点称为“节点(nodes)”。上述搜索区域还可以栅格化为其他多边形,例如可以是六边形、矩形等。节点可以放在任意多边形里面,可以放在多边形的中心,也可以放在多边形的边上,本申请实施例对此不作限制。
基于栅格化的搜索区域,本申请实施例以A*算法为例,对根据起始位置和终点位置,确定出移动物的通行路径的过程进行解释说明,旨在解释并非限制,也可以采用其他的路径搜索算法。
A*算法的路径搜索过程为:
(1)将起点加入开放列表(open list)。
其中,open list里的节点的路径可能是沿途经过的,也有可能不经过。也即是,open list是路径搜索中一个待检查的方格列表。
(2)重复如下过程:
a、遍历开放列表,查找节点总代价最小的节点1,把节点1作为当前要处理的节点。
b、将节点1移到封闭列表(close list),close list中的每个方格都是此时无需再关注的。
c、搜索区域中与节点1相邻的每一个节点,如果该节点不可抵达,或者该节点位于close list中,则忽略该节点。否则,继续检查该节点是否在open list中,若它不在openlist中,则将该节点加入open list,把节点1设置为它的父亲,同时记录该节点的节点总代价。如果该节点已经在open list中,则检查这条路径(即经由节点1到达该节点)是否更好,用节点代价函数中各个代价分量的值作参考,判断该路径是否为更好的路径。如果是更好的路径,则把当前节点1设置为它的父亲,并重新计算它的节点总代价,open list中加入的节点,并依据节点总代价对open list中的节点重新排序。
d、当把终点加入到open list中时,起点和终点之间的通行路径已经找到,则停止遍历搜索区域。或者,当查找终点失败,且open list为空时,说明从起点至终点之间没有可以通行的路径,也停止遍历搜索区域。若搜索到通行路径,则保存路径。
其中,节点代价函数可以是预先根据需求设置好的函数,用于在路径搜索过程中选择下一个更优的节点。
示例性地,本申请实施例提供的节点代价函数可以为下述公式(1)所示。
F(p)=αG(p)+βL(p)+γT(p)+δO(p) (1)
其中,F(p)为节点总代价,G(p)为目标代价分量,若该节点距离目标点(如图4中的B点)越近,则该代价分量的值越小,否则该代价分量的值越大;L(p)为路径代价分量,即该节点和其父节点、爷节点……组成的路径越长,该代价分量的值越大,否则该代价分量的值越小;T(p)为转弯代价分量,即该节点和其父节点、爷节点……组成的路径中转弯数量越多,该代价分量的值越大,否则该代价分量的值越小;O(p)为碰撞代价分量,即该节点和其父节点、爷节点……组成的路径离障碍物越近,该代价分量的值越大,否则该代价分量的值越小。
需要说明的是,可以通过调节各分量的权重α、β、γ和δ来调整路径规划时优先考虑的因素。比如,在移动物移动的过程中,优先考虑安全性,尽可能的避免移动物和障碍物之间发生碰撞,则在使用上述节点代价函数选择下一个节点时,加大碰撞代价分量的权重δ。
(3)从终点开始,每个方格沿着父节点移动直至起点,即为起点到终点之间的通行路径。
作为一个示例,在实际实施环境中,基于上述A*算法,搜索得到的通行路径如图3所示,图3是本申请实施例提供的一种通行路径的示意图,该通行路径包括首尾相接的5个路径段:L1、L2、L3、L4和L5。这5个路径段中任意相邻两个路径段之间存在拐点,单个路径段内不存在拐点。需要说明的是,在本申请实施例中,路径段之间的拐点为导数不连续的点。
接下来,结合图4,对根据起始位置和终点位置,确定出通行路径的实现过程做进一步说明。参见图4,图4是本申请实施例提供的一种A*搜索算法的路径搜索示意图,根据地图数据,将搜索区域栅格化,即得到如图4所示的黑白格子图,其中,黑色格子为障碍物,移动物在白色格子中可以通行,在黑色格子中不能通行。假设移动物的起点为A点,终点为B点。
首先,从起点A开始,并把A点加入到开放列表中。此时,open list里只有一项,即起点A,后续路径搜索过程中会不断加入更多的项。
接下来,将A节点从开放列表中移除,加入到封闭列表中。
当A点所在的方格被加入到了封闭列表后,与它相邻的a、b、c、d、e、f、g、h方格是需要被检查的,加入到开放列表中,a、b、c、d、e、f、g、h方格中都有一个指针指向他们的父节点,也即是起点A。
基于节点代价函数,从open list中选一个与起点A相邻的方格。
作为示例,节点代价函数可以为下述公式(2)所示。
F(p)=10G(p)+20O(p) (2)
其中,F(p)为节点总代价,G(p)为目标代价分量,若该节点距离目标点(图4中的B点)越近,则该代价分量的值越小,否则该代价分量的值越大;O(p)为碰撞代价分量,即该节点和其父节点、爷节点……组成的路径离障碍物越近,该代价分量的值越大,否则该代价分量的值越小。
接下来,遍历开放列表,选择F(p)值最小的方格。
在本示例中,将横向和纵向的移动权重设置为10,对角线的移动权重设置为14。既然是沿着到达指定方格B的路径来计算G(p)值,那么计算出该方格的G(p)值的方法即找出其父节点的G(p)值,然后按父节点是直线方向还是斜线方向,为其父节点的G(p)值加上10或14,即可得到该节点的G(p)值。此外,可以进一步确定每个节点的O(p)值,进而将节点的G(p)值和O(p)值相加,得到每个节点的F(p)值。
在本示例中,计算A节点周围相邻节点a、b、c、d、e、f、g、h的F(p)值为74、60、54、60、40、74、60和54,F(p)值标示在该节点对应方格的左上角,G(p)值标示在左下角,图4中并未示出O(p),暂以标示的F(p)值和G(p)值进行说明。
由于e节点的F(p)值为40,是目前开放列表中F(p)值最小的一个节点,因此,将该e节点从开放列表中取出,加入到封闭列表中。检查与e节点相邻的方格,e节点右边方格是障碍物,忽略该方格,对其不进行任何处理。e节点左边的方格是起点A位于封闭列表中,也忽略该方格,对其不进行任何处理。将与e节点相邻的c节点和h节点加入开放列表中。进一步检查经由e节点所处的方格到达终点B的路径是否更好,可以使用G(p)值来判定。
c节点的G(p)值为14,经由e节点到达c节点,G(p)值为20(其中,10为从起点A到达e节点所处的G(p)值,此外还要加上从e节点纵向移动到c节点的G(p)值10)。而从A点直接到达h节点,G(p)值为14(其中,14为从起点A沿斜线方向到达h节点的G(p)值),显然20比14大,因此从A点到e节点,再到c节点并不是最优的路径,直接从起点A沿对角线移动节点h,比先横向移动至下一个方格,再纵向移动至另一个方格的路径要更优一些。
继续遍历开放列表,从除e节点外的7个方格(节点a、b、c、d、f、g、h所处的方格)中继续选择F(p)值最小的一个方格。但c节点所处的方格和h节点所处的方格的F(p)值都为54,可以继续根据O(p)值对其进行筛选。假设根据O(p)值选择我们选择起点A右下方的方格h作为下一个父节点。不断重复上述确定节点总代价F(p)值的过程,直到把终点B也加入到开放列表中,则结束遍历过程。
基于上述搜索区域的遍历结果,从终点B开始,按着图4中指针所示的方向,从一个方格的中心移动到父节点对应方格的中心,直至到达起点A,即可确定出通行路径。也即是,从起点A移动到终点B就是简单从起点A开始,按照该通行路径指示的一个方格的中心移动到另一个方格的中心,直至移动到终点B。
需要说明的是,在上述步骤201中,所确定出的通行路径中的多个首尾相接的路径段均为直线段,或者,该多个首尾相接的路径段中存在曲线段。该多个首尾相接的路径段中是否存在曲线段与路径搜索算法、地图数据等多种因素相关,本申请实施例对此不作限定。
基于上述步骤201,为了使移动物在该通行路径上移动的过程中,尽可能的以一个姿态移动较长的距离,中途无需进行姿态调整,并且为了保证移动物在移动过程中不会与障碍物碰撞,可以对该通行路径进行优化。接下来将通过步骤202至步骤205对此进行介绍。
步骤202:确定该通行路径中的第一路径段,第一路径段是长度不小于阈值的路径段。
在本申请实施例中,为了使移动物尽可能的以一个姿态在一个路径段上移动较长的距离,首先确定出该通行路径中长度不小于阈值的第一路径段。其中,该阈值用于表征移动物在不改变姿态的情况下在一个路径段上所需行驶的最小长度,该阈值可以预先设置,也可以基于该通行路径中各个路径段的长度来确定,例如将该通行路径中各个路径段的平均长度,或平均长度的一半,确定为该阈值,或者也可以按照其他方式来确定该阈值,本申请实施例对此不作限定。
作为一个示例,基于图3所示的通行路径,L2和L4是拐角处的路径段,L2和L4的长度均小于阈值,L1、L3和L5的长度不小于阈值,那么确定L1、L3和L5作为第一路径段。
步骤203:根据移动物的长宽信息和第一路径段的通道信息,确定移动物在各个第一路径段上的起始位姿和终止位姿。
在本申请实施例中,为了使移动物尽可能的以一个姿态在一个路径段上移动较长的距离,除了确定出长度大于阈值的第一路径段之外,还需要确定出移动物在各个第一路径段上的起始位姿和终止位姿,也即是,确定移动物在各个第一路径段上的关键位姿对,关键位姿对包括起始位姿和终止位姿。
在一种可能的实现方式中,上述步骤203的实现过程为:对于该通行路径中的第j个第一路径段,根据移动物的长宽信息和第j个第一路径段的通道信息,以符合第一移动条件为目标,确定第一起始位姿和第一终止位姿。其中,第j个第一路径段的通道信息包括第j个第一路径段的轨迹坐标和通道宽度。第一起始位姿为移动物在第j个第一路径段上的起始位姿,第一终止位姿为移动物在第j个第一路径段上的终止位姿。第一起始位姿的位置为第j个第一路径段的起点位置,第一终止位姿的位置为第j个第一路径段的终止位置,第一起始位姿的姿态与第一终止位姿的姿态一致。另外,第一移动条件包括:移动物从第j个第一路径段上的起始位姿移动到第j个第一路径段上的终止位姿的过程中,移动物上距离障碍物最近的点与障碍物之间的距离不小于安全距离。
需要说明的是,障碍物包括通道壁、通道内放置的杂物等,在确定通道宽度时已考虑通道中障碍物的位置。移动物距离障碍物最近时实质上可能是处于通道宽度最窄的地方。另外,安全距离可以是预先设定的距离值dmin,例如,安全距离可以为30cm或50cm等。
在上述确定第一起始位姿和第一终止位姿中,根据第一路径段的轨迹坐标,将第一路径段的起点坐标(即起点位置的坐标),作为第一起始位姿的位置,将第一路径段的终点坐标(即终止位置的坐标),作为第一终止位姿的位置。另外,第一起始位姿的姿态与第一终止位姿的姿态一致。例如,在第j个第一路径段为直线段的情况下,第一起始位姿的姿态与第一终止位姿的姿态相同,在第j个第一路径段为曲线段的情况下,第一起始位姿的姿态与第一终止位姿的姿态均与曲线段中相应位置的曲线切线方向相同。以第j个第一路径段为直线段为例,调整第一起始位姿的姿态与第一终止位姿的姿态相同,且以该姿态从第j个第一路径段的起点位置移动到第j个第一路径段的终止位置的过程中,移动物上距离障碍物最近的点与障碍物之间的距离最大,也即是,使得第一起始位姿与第一终止位姿满足第一移动条件。需要说明的是,在本申请实施例中,移动物的中心始终在路径段的轨迹上。
参见图5,图5是本申请实施例提供的一种位姿示意图,在确定移动物在该路径段上的起始位姿和终止位姿的位置时,将起始位姿的位置设置在该路径段的起点位置处,将终止位姿的位置设置在该路径段的终止位置处。在确定移动物在该路径段上的起始位姿和终止位姿的姿态时,假设移动物可以360°旋转,以移动物在该路径段上移动并旋转的过程中距离障碍物的距离最大为调节前提,调节移动物的姿态。
作为一个示例,如图5所示,假设移动物的姿态可以为姿态A、姿态B、姿态C和姿态D,假设移动过程中,当移动物以姿态A在第一路径段上移动时,移动物的一边可能会和障碍物发生碰撞;当移动物以姿态B在第一路径段上移动时,移动物的两边均会和障碍物发生碰撞;当移动物以姿态C在第一路径段上移动时,移动物的边缘中距离障碍物最近的点和障碍物之间的距离为d1;当移动物以姿态D在第一路径段上移动时,移动物的边缘中距离障碍物最近的点和障碍物之间的距离为d2。其中,d2>d1,也即是,当移动物以姿态D在第一路径段上移动时,安全性最高,移动物和障碍物发生碰撞的可能性最低,那么,将姿态D确定为第一起始位姿和第一终止位姿的姿态。
示例性地,通过步骤202确定的第一起始位姿可以为(x1,y1,θ1),(x1,y1)为移动物的中心点在第j个第一路径段的起点位置的轨迹坐标,θ1为移动物在该起点位置的姿态。同理,第一终止位姿可以表示为(x2,y2,θ2),(x2,y2)为移动物的中心点在第j个第一路径段的终止位置的轨迹坐标,θ2为移动物在该终止位置的姿态。
需要说明的是,基于当前的第j个第一路径段可能能够确定出符合第一移动条件的第一起始位姿和第一终止位姿,也可能无法确定出符合第一移动条件的第一起始位姿和第一终止位姿。例如,不论移动物以何种姿态在第j个第一路径段上移动的过程中,移动物上距离障碍物最近的点与障碍物之间的距离均小于安全距离,这种情况下,基于当前的第j个第一路径段无法确定出符合第一移动条件的第一起始位姿和第一终止位姿。
在本申请实施例中,如果不论移动物以何种姿态在第j个第一路径段上移动的过程中,移动物上距离障碍物最近的点与障碍物之间的距离小于0,表示移动物不能通过第j个第一路径段,则确定路径规划失败。如果不论移动物以何种移动姿态在第一路径段上移动的过程中,移动物上距离障碍物最近的点与障碍物之间的距离大于0,小于安全距离,则可以通过调整第一起始位姿和第一终止位姿,以使调整后的第一起始位姿和第一终止位姿符合第一移动条件。
在一种实现方式中,在无法确定出符合第一移动条件的第一起始位姿和第一终止位姿的情况下,按照路径调整步长,调整第j个第一路径段的参考端的位置,以对第j个第一路径段进行缩短,得到缩短后的第j个第一路径段。其中,参考端为第j个第一路径段的两端中的一端,缩短后的第j个第一路径段的长度不小于阈值。根据移动物的长宽信息和缩短后的第j个第一路径段的通道信息,以符合第一移动条件为调整目标,对第一起始位姿和第一终止位姿进行调整,调整后的第一终止位姿为移动物在第j个第一路径段上的终止位姿,调整后的第一起始位姿为移动物在第j个第一路径段上的起始位姿。
需要说明的是,本申请实施例不限定参考端为第j个第一路径段中的两端中的哪一端。在本申请实施例中,参考端为第j个第一路径段的两端中距离第j个第一路径段所在通道中障碍物较近的一端。也即是,在存在距离第j个第一路径段所在通道中障碍物较近的一端的情况下,调整无法规避障碍物的一端的位置,这样,便于快速确定出符合第一移动条件的第一起始位姿和第一终止位姿,也就更利于快速规划出使得移动物安全通过的通行路径。需要说明的是,若第j个第一路径段的通道宽度是固定值,表示不存在距离第j个第一路径段所在通道中障碍物较近的一端。在这种情况下,参考端为第j个第一路径段的两端中的任意一端。或者,在这种情况下,若j个第一路径段的一端相接有第二路径段,则参考端为第j个第一路径段的两端中相接有第二路径段的一端。或者,在这种情况下,参考端为第j个第一路径段的两端中的指定端,指定端为起始端或终止端。
在另一些实施例中,无需考虑是否存在距离障碍物较近的一端,参考端为第j个第一路径段的两端中的任意一端。或者,在j个第一路径段的一端相接有第二路径段的情况下,参考端为第j个第一路径段的两端中相接有第二路径段的一端。或者,参考端为第j个第一路径段的两端中的指定端,指定端为起始端或终止端,即,预先设置在需要缩短第一路径段的情况下,默认缩短第一路径段的起始端或终止端。
其中,得到缩短后的第j个第一路径段之后,将缩短处理后的第j个第一路径段的起点位置,调整为第一起始位姿的位置,将缩短处理后的第j个第一路径段的终止位置,调整为第一终止位姿的位置。此外,将第一起始位姿和第一终止位姿的姿态,调整到以该姿态在缩短后的第j个第一路径段上移动的过程中,移动物上距离障碍物最近的点与障碍物之间的距离最大。
需要说明的是,如果通过调整仍无法确定出符合第一移动条件的第一起始姿态和第一终止姿态,则以保证第j个第一路径段的长度不小于阈值为前提,继续缩短第j个第一路径段,并在缩短第j个第一路径段后,继续以符合第一移动条件为调整目标,调整第一起始位姿和第一终止位姿,直至确定出符合第一移动条件的第一起始位姿和第一终止位姿。如果在这个过程中,第j个第一路径段再缩短一次长度就会小于阈值,这种情况下,仍没有确定出符合第一移动条件的第一起始位姿和第一终止位姿的话,则确定路径规划失败。
在上述对第j个第一路径段的缩短处理中,将第j个第一路径段的参考端(如无法规避障碍物的一端,即距离第j个第一路径段所在通道中障碍物较近的一端)按照路径调整步长进行位置调整,以缩短第j个第一路径段。另外,路径调整步长可以为定值或变量。示例性地,路径调整步长为预设的定值,例如,20cm、30cm等。或者,路径调整步长根据预设函数确定。
以上以第j个第一路径段为例,介绍了确定移动物在第j个第一路径段上的关键位姿对(包括起始位姿和终止位姿)的实现方式,对于该通行路径中其他的第一路径段,均可以按照以上介绍的方法确定移动物在相应第一路径段上的关键位姿对。例如,按照各个第一路径段在该通行路径中的顺序,依次确定移动物在各个第一路径段上的关键位姿对。需要说明的是,对于按照上述方法仍无法确定出关键位姿对的第一路径段,将不再作为第一路径段。
图6是本申请实施例提供的一种确定关键位姿对的方法流程图。参见图6,按照通行路径中的N个第一路径段的顺序,依次对该N个第一路径段进行处理。对于第j个第一路径段Lj,以符合第一移动条件为目标,确定关键位姿对,若对于第一路径段Lj无法确定出符合移动条件的关键位姿对,则在第一路径段Lj满足长度不小于阈值的情况下,对第一路径段Lj的长度L进行缩短处理,每次缩短步长为Δl。其中,在第一路径段Lj上无法规避障碍物的一端缩短Δl。基于缩短后的第一路径段Lj,对关键位姿对进行调整,直至调整到在第一路径段Lj的长度不小于阈值的情况下,确定出符合第一移动条件的关键位姿对为止。如果通过不断调整,调整到路径小于阈值,仍无法确定出符合第一移动条件的关键位姿对,则确定路径段Lj不再作为第一路径段。其中,Δl为路径调整步长。
以图3所示的通行路径为例,经过图6所示的方法对该通行路径中的第一路径段L1、L3和L5处理后,确定出到L1、L3和L5的关键位姿对,L1、L3和L5的关键位姿对如图7所示。图7是本申请实施例提供的一种确定出的关键位姿对的示意图。其中,位姿1和位姿2为L1的关键位姿对,位姿1为L1的起始位姿,位姿2为L1的终止位姿;位姿3和位姿4为L3的关键位姿对,位姿3为L3的起始位姿,位姿4为L3的终止位姿;位姿5和位姿6为L5的关键位姿对,位姿5为L5的起始位姿,位姿6为L5的终止位姿。
步骤204:若存在未首尾相接的第i个第一路径段与第i+1个第一路径段,则基于移动物的长宽信息和第二路径段的通道信息,确定用于衔接第i个第一路径段上的终止位姿与第i+1个第一路径段上的起始位姿的衔接路径,第二路径段是该通行路径中位于第i个第一路径段和第i+1个第一路径段之间的路径段。
也即是,若通过步骤203确定各个第一路径段的起始位姿和终止位姿之后,若存在未首尾相接的第i个第一路径段和第i+1个第一路径段,说明该通行路径还未优化完成,需要确定出用于衔接第i个第一路径段上的终止位姿与第i+1个第一路径段上的起始位姿的衔接路径。在本申请实施例中,衔接路径为未首尾相接的相邻两个第一路径段之间的过渡路径,移动物也需要安全通过衔接路径。
需要说明的是,该通行路径中位于第i个第一路径段和第i+1个第一路径段之间的所有路径段为第二路径段,即,第二路径段包括至少一个路径段。
在一种可能的实现方式中,上述步骤204的实现过程为:基于移动物的长宽信息和第二路径段的通道信息,以符合第二移动条件为目标,确定用于衔接第i个第一路径段上的终止位姿与第i+1个第一路径段上的起始位姿的衔接路径。其中,第二移动条件包括:移动物从第二路径段上的起始位姿移动到第二路径段的终止位姿的过程中,移动物上距离障碍物最近的点与障碍物之间的距离不小于安全距离。
其中,在确定用于衔接第i个第一路径段上的终止位姿与第i+1个第一路径段上的起始位姿的衔接路径之前,获取第一关键位姿对中的终止位姿作为第二路径段上的起始位姿,以及获取第二关键位姿对中的起始位姿作为第二路径段上的终止位姿。确定用于衔接第i个第一路径段上的终止位姿与第i+1个第一路径段上的起始位姿的衔接路径,即,确定用于衔接第二路径段的起始位姿与第二路径段的终止位姿的衔接路径。其中,第一关键位姿对为第i个第一路径段的关键位姿对,第二关键位姿对为第i+1个第一路径段的关键位姿对。
以图3所示的通行路径为例,路径段L2和L4为两个第二路径段,针对第二路径段L2,其相邻的两个第一路径段为L1和L3,参见图7,获取路径段L1的关键位姿对中的位姿2,作为路径段L2上的起始位姿,获取路径段L3的关键位姿对中的位姿3,作为路径段L2上的终止位姿。以符合第二移动条件为目标,确定用于衔接位姿2与位姿3的衔接路径,也即是,确定路径段L1与L3之间的衔接路径。其中,衔接路径能够使得移动物从路径段L1上的位姿2,移动到路径段L3上的位姿3的过程中不碰撞到障碍物。
同理,针对第二路径段L4,其相邻的两个第一路径段为L3和L5,参见图7,获取路径段L3的关键位姿对中位姿4,作为路径段L4上的起始位姿,获取路径段L5的关键位姿对中的位姿5,作为路径段L5上的终止位姿。以符合第二移动条件为目标,确定用于衔接位姿4与位姿5的衔接路径,也即是,确定路径段L3与L5之间的衔接路径。其中,衔接路径能够使得移动物从路径段L3上的位姿4,移动到路径段L5上的位姿5的过程中不碰撞到障碍物。
示例性地,在确定用于衔接位姿2与位姿3之间的衔接路径时,可以通过调节贝塞尔曲线的两个控制点以期生成移动物可以通过的衔接路径。
图8是本申请实施例提供的一种通过贝塞尔曲线确定通行路径的示意图。如图8所示,本申请实施例以位姿2的位置坐标(x2,y2)为三阶贝塞尔曲线的一个端点,以位姿3的位置坐标(x3,y3)为三阶贝塞尔曲线的另一个端点,这两个点之间可以使用三阶贝塞尔曲线来连接,通过多段贝塞尔曲线相连,就可以得到路径段L1和路径段L3之间的衔接路径。该衔接路径即为连接端点(x2,y2)和端点(x3,y3)的最优曲线。
如图8中(1)所示,一阶贝塞尔曲线:给定点P0和P1,一阶贝塞尔曲线只是一条两点之间的直线,这条直线由下述公式(3)式给出:
P(t)=P0+(P1-P0)t=(1-t)P0+tP1,t∈[0,1] (3)
其中P0和P1为两个端点,P对应于贝塞尔曲线上的点,随着t在[0,1]中变化,P点的集合构成一条连接P0与P1的线段。
如图8中(2)所示,二阶贝塞尔曲线:当在定点P0和P2之间引入一个控制点P1的时候,就可以生成二阶贝塞尔曲线,它是一个由二次函数描述的曲线,最多有一个顶点,这条曲线由下述公式(4)式给出:
P(t)=(1-t)2P0+2t(1-t)P1+t2P2,t∈[0,1] (4)
其中,已知端点P0、端点P2以及控制点P1,P1为P0和P2外的一点。可选地,控制点P1可以根据第二路径段的通道信息选定,例如,控制点P1为第二路径段所在通道内的中心点或其他点。根据贝塞尔曲线的定义,使得P0A/P0P1=t,P1B/P1P2=t,则可以确定点A与点B。然后连接AB,取AP/AB=t,则P(t)点就是二阶贝塞尔曲线上的点了。
如图8中(3)所示,在依据端点P0、端点P3、控制点P1以及控制点P2,三阶贝塞尔曲线可以用一个三次函数描述,最多拥有两个拐点,这条曲线由下述公式(5)式给出:
P(t)=(1-t)3P0+3P1t(1-t)2+3P2t2(1-t)+P3t3,t∈[0,1] (5)
其中,已知端点P0和端点P3以及控制点P1和P2,P1和P2可以为P0和P3外的点,基于三阶贝塞尔曲线的定义,确定出P(t)即得到三阶贝塞尔曲线。点P满足:取P0P的中点A,取P3P的中点B,并连接点A和点B,在AB上取点C,使得AC/CB=P0P/P3P,将直线AB按照矢量CP平移到通过P点,并且使得平移后的点C和点P重合,平移后的A点与B点即控制点P1和P2。
基于上述步骤204,在通过贝塞尔曲线确定衔接路径的轨迹后,需要根据移动物的长宽信息以及第二路径段的通道信息,判断移动物在该衔接路径上移动的过程中会不会碰撞到障碍物。若移动物在该衔接路径上移动的过程中不会碰撞到障碍物,说明确定出了符合第二移动条件的衔接路径。若移动物在该衔接路径上移动的过程中会碰撞到障碍物,说明当前还无法确定出符合第二移动条件的衔接路径。这种情况下,可以通过对第一关键位姿对中的终止位姿和/或第二关键位姿对中的起始位姿进行位姿调整,重新确定衔接路径。
在一种可能的实现方式中,在无法确定出符合第二移动条件且能够衔接第i个第一路径段上的终止位姿与第i+1个第一路径段上的起始位姿的衔接路径的情况下,调整移动物在第i+1个第一路径段上的起始位姿的位置,以缩短第i+1个第一路径段,得到调整后的第i+1个第一路径段上的起始位姿。以符合第二移动条件为目标,确定用于衔接第i个第一路径段上的终止位姿与调整后的第i+1个第一路径段上的起始位姿的衔接路径。其中,缩短后的第i+1个第一路径段的长度不小于阈值。可选地,按照路径调整步长来缩短第i+1个第一路径段。
在另一种可能的实现方式中,在无法确定出符合第二移动条件且能够衔接第i个第一路径段上的终止位姿与第i+1个第一路径段上的起始位姿的衔接路径的情况下,调整移动物在第i个第一路径段上的终止位姿的位置,以缩短第i个第一路径段,得到调整后的第i个第一路径段上的终止位姿。以符合第二移动条件为目标,确定用于衔接调整后的第i个第一路径段上的终止位姿与第i+1个第一路径段上的起始位姿的衔接路径。其中,缩短后的第i个第一路径段的长度不小于阈值。可选地,按照路径调整步长来缩短第i个第一路径段。
在又一种可能的实现方式中,调整移动物在第i+1个第一路径段上的起始位姿的位置,以缩短第i+1个第一路径段,得到调整后的第i+1个第一路径段上的起始位姿,以及调整移动物在第i个第一路径段上的终止位姿的位置,以缩短第i个第一路径段,得到调整后的第i个第一路径段上的终止位姿。以符合第二移动条件为目标,确定用于衔接调整后的第i个第一路径段上的终止位姿与调整后的第i+1个第一路径段上的起始位姿的衔接路径。其中,缩短后的第i+1个第一路径段和缩短后的第i个第一路径段的长度均不小于阈值。可选地,按照路径调整步长来缩短第i个第一路径段和第i+1个第一路径段。
作为一个示例,在根据如图7所示路径段L1的位姿2和路径段L3的位姿3,无法得到满足移动物尺寸的衔接路径时,也即无法确定出符合第二移动条件的衔接路径时,对路径段L1和/或L3进行缩短处理,缩短步长为Δl。假设路径段L1和L3均缩短,那么,位姿2和位姿3的位置也相应进行了调整,位姿2和位姿3的姿态未进行调整。也即是,位姿2的位置调整到了缩短后的路径段L1的终止位置,位姿3的位置调整到了缩短后的路径段L3的起点位置。基于调整后的位姿2和调整后的位姿3,重新确定用于衔接调整后的位姿2和调整后的位姿3的衔接路径,例如,重新确定调整后的位姿2和调整后的位姿3之间的三阶贝塞尔曲线,该三阶贝塞尔曲线为重新确定的衔接路径。
需要说明的是,若将第i个第一路径段和/或第i+1个第一路径段缩短至不能再缩短,也即再缩短的话长度将小于阈值,仍无法确定出符合第二移动条件的衔接路径的话,则确定路径规划失败。例如,若将路径段L1和/或路径段L3的长度缩短至不能再缩短(长度L小于阈值Lmin)时,根据移动物的尺寸,无法获取到满足移动物不碰撞障碍物要求的衔接路径时,则确定路径规划失败。
以上以未首尾相接的第i个第一路径段和第i+1个第一路径段为例,介绍了确定衔接路径的实现方式,对于该通行路径中其他的未首尾相接的相邻两个第一路径段,均可以按照以上介绍的方法确定衔接路径。例如,按照各个路径段在该通行路径中的顺序,依次确定各个衔接路径。
步骤205:基于衔接路径更新该通行路径,得到修正后的通行路径。
在确定各个衔接路径后,用各个衔接路径替换该通行路径中相应的第二路径段,得到修正后的通行路径。示例性地,在确定出用于衔接第i个第一路径段上的终止位姿与第i+1个第一路径段上的起始位姿的衔接路径之后,用该衔接路径替换该通信路径中位于第i个第一路径段和第i+1个第一路径段之间的第二路径段,以修正该通行路径。修正后的通行路径是规划的满足移动物在移动过程中不碰撞到障碍物,且保持有安全距离的最优路径。
需要说明的是,若衔接路径是通过缩短第一路径段,进而根据缩短后的第一路径段的关键位姿对确定出来的,则依据最终确定的各个第一路径段和各个衔接路径在通行路径中的顺序,将各个第一路径段与衔接路径连接起来,得到修正后的通行路径。
综上,本申请实施提供的路径规划方法基于一种分布式的路径规划思想,先考虑路径规划效率,快速确定起点和终点之间的通行路径。然后,考虑移动物在通行路径上移动时的安全性,对该通行路径中较长的第一路径段,考虑移动物的长宽信息,确定第一路径段的起止位姿。对于未首尾相接的相邻两个第一路径段,也考虑移动物的长宽信息,确定这两个第一路径段之间的衔接路径,进而基于衔接路径更新通行路径。如此,在路径规划的过程中,充分考虑移动物的尺寸信息,对通行路径进行优化,使得移动物在优化后的通行路径上移动时不会与障碍物发生碰撞,路径规划更合理可靠。
上述所有可选技术方案,均可按照任意结合形成本发明的可选实施例,本发明实施例对此不再一一赘述。
图9是本申请实施例提供的一种路径规划装置的结构示意图,该装置900包括:
第一确定模块901,用于根据起始位置和终点位置,确定出通行路径,该通行路径包括多个首尾相接的路径段,且该通行路径中相邻两个路径段之间存在拐点,单个路径段内不存在拐点;
第二确定模块902,用于确定该通行路径中的第一路径段,第一路径段是长度不小于阈值的路径段;
第三确定模块903,用于根据移动物的长宽信息和第一路径段的通道信息,确定移动物在各个第一路径段上的起始位姿和终止位姿;
第四确定模块904,用于若存在未首尾相接的第i个第一路径段与第i+1个第一路径段,则基于移动物的长宽信息和第二路径段的通道信息,确定用于衔接第i个第一路径段上的终止位姿与第i+1个第一路径段上的起始位姿的衔接路径,第二路径段是该通行路径中位于第i个第一路径段和第i+1个第一路径段之间的路径段;
路径更新模块905,用于基于衔接路径更新通行路径,得到修正后的通行路径。
可选地,第三确定模块903包括:
位姿确定子模块,用于对于通行路径中的第j个第一路径段,根据移动物的长宽信息和第j个第一路径段的通道信息,以符合第一移动条件为目标,确定第一起始位姿和第一终止位姿,第j个第一路径段的通道信息包括第j个第一路径段的轨迹坐标和通道宽度;
其中,第一起始位姿为移动物在第j个第一路径段上的起始位姿,第一终止位姿为移动物在第j个第一路径段上的终止位姿,第一起始位姿的位置为第j个第一路径段的起点位置,第一终止位姿的位置为第j个第一路径段的终止位置,第一起始位姿的姿态与第一终止位姿的姿态一致;
第一移动条件包括:移动物从第j个第一路径段上的起始位姿移动到第j个第一路径段上的终止位姿的过程中,移动物上距离障碍物最近的点与障碍物之间的距离不小于安全距离。
可选地,第三确定模块903还包括:
路径段缩短子模块,用于在无法确定出符合第一移动条件的第一起始位姿和第一终止位姿的情况下,按照路径调整步长,调整第j个第一路径段的参考端的位置,以对第j个第一路径段进行缩短,得到缩短后的第j个第一路径段,参考端为第j个第一路径段的两端中的一端,缩短后的第j个第一路径段的长度不小于阈值;
位姿调整子模块,用于根据移动物的长宽信息和缩短后的第j个第一路径段的通道信息,以符合第一移动条件为调整目标,对第一起始位姿和第一终止位姿进行调整,调整后的第一终止位姿为移动物在第j个第一路径段上的终止位姿,调整后的第一起始位姿为移动物在第j个第一路径段上的起始位姿。
可选地,参考端为第j个第一路径段的两端中距离第j个第一路径段所在通道中障碍物较近的一端;或者,参考端为第j个第一路径段的两端中的任意一端。
可选地,第四确定模块904包括:
衔接路径确定子模块,用于基于移动物的长宽信息和第二路径段的通道信息,以符合第二移动条件为目标,确定用于衔接第i个第一路径段上的终止位姿与第i+1个第一路径段上的起始位姿的衔接路径;
其中,第二移动条件包括:移动物从第二路径段上的起始位姿移动到第二路径段上的终止位姿的过程中,移动物上距离障碍物最近的点与障碍物之间的距离不小于安全距离。
可选地,在无法确定出符合第二移动条件且能够衔接第i个第一路径段上的终止位姿与第i+1个第一路径段上的起始位姿的衔接路径的情况下,第四确定模块904还包括:
第一衔接路径调整子模块,用于调整移动物在第i+1个第一路径段上的起始位姿的位置,以缩短第i+1个第一路径段,得到调整后的第i+1个第一路径段上的起始位姿,以符合第二移动条件为目标,确定用于衔接第i个第一路径段上的终止位姿与调整后的第i+1个第一路径段上的起始位姿的衔接路径,缩短后的第i+1个第一路径段的长度不小于阈值;或者,
第二衔接路径调整子模块,用于调整移动物在第i个第一路径段上的终止位姿的位置,以缩短第i个第一路径段,得到调整后的第i个第一路径段上的终止位姿,以符合第二移动条件为目标,确定用于衔接调整后的第i个第一路径段上的终止位姿与第i+1个第一路径段上的起始位姿的衔接路径,缩短后的第i个第一路径段的长度不小于阈值;或者,
第三衔接路径调整子模块,用于调整移动物在第i+1个第一路径段上的起始位姿的位置,以缩短第i+1个第一路径段,得到调整后的第i+1个第一路径段上的起始位姿,以及调整移动物在第i个第一路径段上的终止位姿的位置,以缩短第i个第一路径段,得到调整后的第i个第一路径段上的终止位姿,以符合第二移动条件为目标,确定用于衔接调整后的第i个第一路径段上的终止位姿与调整后的第i+1个第一路径段上的起始位姿的衔接路径,缩短后的第i+1个第一路径段和缩短后的第i个第一路径段的长度均不小于阈值。
综上,本申请实施提供的路径规划方法基于一种分布式的路径规划思想,先考虑路径规划效率,快速确定起点和终点之间的通行路径。然后,考虑移动物在通行路径上移动时的安全性,对该通行路径中较长的第一路径段,考虑移动物的长宽信息,确定第一路径段的起止位姿。对于未首尾相接的相邻两个第一路径段,也考虑移动物的长宽信息,确定这两个第一路径段之间的衔接路径,进而基于衔接路径更新通行路径。如此,在路径规划的过程中,充分考虑移动物的尺寸信息,对通行路径进行优化,使得移动物在优化后的通行路径上移动时不会与障碍物发生碰撞,路径规划更合理可靠。
需要说明的是:上述实施例提供的路径规划装置在规划通行路径时,仅以上述各功能模块的划分进行举例说明,实际应用中,可以根据需要而将上述功能分配由不同的功能模块完成,即将装置的内部结构划分成不同的功能模块,以完成以上描述的全部或者部分功能。另外,上述实施例提供的路径规划装置与路径规划方法实施例属于同一构思,其具体实现过程详见方法实施例,这里不再赘述。
图10是本申请实施例提供的计算机设备的结构框图,该计算机设备1000可用于实现上述路径规划方法示例的功能。具体来讲:
该计算机设备1000包括处理单元(如CPU(Central Processing Unit,中央处理器)、GPU(Graphics Processing Unit,图形处理器)和FPGA(Field Programmable GateArray,现场可编程逻辑门阵列)等)1001、包括RAM(Random-Access Memory,随机存储器)1002和ROM(Read-Only Memory,只读存储器)1003的系统存储器1004,以及连接系统存储器1004和中央处理单元1001的系统总线1005。该计算机设备1000还包括帮助计算计算机设备内的各个器件之间传输信息的I/O系统(Input Output System,基本输入/输出系统)1006,和用于存储操作系统1013、应用程序1014和其他程序模块1015的大容量存储设备1007。
该基本输入/输出系统1006包括有用于显示信息的显示器1008和用于用户输入信息的诸如鼠标、键盘之类的输入设备1009。其中,该显示器1008和输入设备1009都通过连接到系统总线1005的输入输出控制器1010连接到中央处理单元1001。该基本输入/输出系统1006还可以包括输入输出控制器1010以用于接收和处理来自键盘、鼠标、或电子触控笔等多个其他设备的输入。类似地,输入输出控制器1010还提供输出到显示屏、打印机或其他类型的输出设备。
该大容量存储设备1007通过连接到系统总线1005的大容量存储控制器(未示出)连接到中央处理单元1001。该大容量存储设备1007及其相关联的计算机可读介质为计算机设备1000提供非易失性存储。也就是说,该大容量存储设备1007可以包括诸如硬盘或者CD-ROM(Compact Disc Read-Only Memory,只读光盘)驱动器之类的计算机可读介质(未示出)。
不失一般性,该计算机可读介质可以包括计算机存储介质和通信介质。计算机存储介质包括以用于存储诸如计算机可读指令、数据结构、程序模块或其他数据等信息的任何方法或技术实现的易失性和非易失性、可移动和不可移动介质。计算机存储介质包括RAM、ROM、EPROM(Erasable Programmable Read-Only Memory,可擦写可编程只读存储器)、EEPROM(Electrically Erasable Programmable Read-Only Memory,电可擦写可编程只读存储器)、闪存或其他固态存储技术,CD-ROM、DVD(Digital Video Disc,高密度数字视频光盘)或其他光学存储、磁带盒、磁带、磁盘存储或其他磁性存储设备。当然,本领域技术人员可知该计算机存储介质不局限于上述几种。上述的系统存储器1004和大容量存储设备1007可以统称为存储器。
根据本申请实施例,该计算机设备1000还可以通过诸如因特网等网络连接到网络上的远程计算机运行。也即计算机设备1000可以通过连接在该系统总线1005上的网络接口单元1011连接到网络1012,或者说,也可以使用网络接口单元1010来连接到其他类型的网络或远程计算机系统(未示出)。
该存储器还包括至少一条指令、至少一段程序、代码集或指令集,该至少一条指令、至少一段程序、代码集或指令集存储于存储器中,且经配置以由一个或者一个以上处理器执行,以实现上述路径规划方法。
本领域技术人员可以理解,图10中示出的结构并不构成对计算机设备1000的限定,可以包括比图示更多或更少的组件,或者组合某些组件,或者采用不同的组件布置。
在示例性的实施例中,还提供了一种计算机可读存储介质,所述计算机可读存储介质上存储有指令,所述指令被处理器执行时实现上述路径规划方法。
在示例性实施例中,还提供了一种计算机程序产品,当该计算机程序产品被执行时,其用于实现上述路径规划方法。
应当理解的是,在本文中提及的“多个”是指两个或两个以上。“和/或”,描述关联对象的关联关系,表示可以存在三种关系,例如,A和/或B,可以表示:单独存在A,同时存在A和B,单独存在B这三种情况。字符“/”一般表示前后关联对象是一种“或”的关系。
本领域普通技术人员可以理解实现上述实施例的全部或部分步骤可以通过硬件来完成,也可以通过程序来指令相关的硬件完成,所述的程序可以存储于一种计算机可读存储介质中,上述提到的存储介质可以是只读存储器,磁盘或光盘等。
以上所述仅为本申请的较佳实施例,并不用以限制本申请,凡在本申请的精神和原则之内,所作的任何修改、等同替换、改进等,均应包含在本申请的保护范围之内。
Claims (11)
1.一种路径规划方法,其特征在于,所述方法包括:
根据起始位置和终点位置,确定出通行路径,所述通行路径包括多个首尾相接的路径段,且所述通行路径中相邻两个所述路径段之间存在拐点,单个所述路径段内不存在拐点;
确定所述通行路径中的第一路径段,所述第一路径段是长度不小于阈值的路径段;
对于所述通行路径中的第j个第一路径段,根据移动物的长宽信息和所述第j个第一路径段的通道信息,以符合第一移动条件为目标,确定第一起始位姿和第一终止位姿,所述第j个第一路径段的通道信息包括所述第j个第一路径段的轨迹坐标和通道宽度;其中,所述第一起始位姿为所述移动物在所述第j个第一路径段上的起始位姿,所述第一终止位姿为所述移动物在所述第j个第一路径段上的终止位姿,所述第一起始位姿的位置为所述第j个第一路径段的起点位置,所述第一终止位姿的位置为所述第j个第一路径段的终止位置,所述第一起始位姿的姿态与所述第一终止位姿的姿态一致;所述第一移动条件包括:所述移动物从所述第j个第一路径段上的起始位姿移动到所述第j个第一路径段上的终止位姿的过程中,所述移动物上距离障碍物最近的点与障碍物之间的距离不小于安全距离;
若存在未首尾相接的第i个第一路径段与第i+1个第一路径段,则基于所述移动物的长宽信息和第二路径段的通道信息,确定用于衔接所述第i个第一路径段上的终止位姿与所述第i+1个第一路径段上的起始位姿的衔接路径,所述第二路径段是所述通行路径中位于所述第i个第一路径段和所述第i+1个第一路径段之间的路径段;
基于所述衔接路径更新所述通行路径,得到修正后的通行路径。
2.根据权利要求1所述的方法,其特征在于,在无法确定出符合所述第一移动条件的第一起始位姿和第一终止位姿的情况下,所述方法还包括:
按照路径调整步长,调整所述第j个第一路径段的参考端的位置,以对所述第j个第一路径段进行缩短,得到缩短后的第j个第一路径段,所述参考端为所述第j个第一路径段的两端中的一端,所述缩短后的第j个第一路径段的长度不小于所述阈值;
根据所述移动物的长宽信息和所述缩短后的第j个第一路径段的通道信息,以符合所述第一移动条件为调整目标,对所述第一起始位姿和所述第一终止位姿进行调整,调整后的第一终止位姿为所述移动物在所述第j个第一路径段上的终止位姿,调整后的第一起始位姿为所述移动物在所述第j个第一路径段上的起始位姿。
3.根据权利要求2所述的方法,其特征在于,
所述参考端为所述第j个第一路径段的两端中距离所述第j个第一路径段所在通道中障碍物较近的一端;或者,
所述参考端为所述第j个第一路径段的两端中的任意一端。
4.根据权利要求1-3任一所述的方法,其特征在于,所述基于所述移动物的长宽信息和第二路径段的通道信息,确定用于衔接所述第i个第一路径段上的终止位姿与所述第i+1个第一路径段上的起始位姿的衔接路径,包括:
基于所述移动物的长宽信息和所述第二路径段的通道信息,以符合第二移动条件为目标,确定所述用于衔接所述第i个第一路径段上的终止位姿与所述第i+1个第一路径段上的起始位姿的衔接路径;
其中,所述第二移动条件包括:所述移动物从所述第二路径段上的起始位姿移动到所述第二路径段上的终止位姿的过程中,所述移动物上距离障碍物最近的点与障碍物之间的距离不小于安全距离。
5.根据权利要求4所述的方法,其特征在于,在无法确定出符合所述第二移动条件且能够衔接所述第i个第一路径段上的终止位姿与所述第i+1个第一路径段上的起始位姿的衔接路径的情况下,所述方法还包括:
调整所述移动物在所述第i+1个第一路径段上的起始位姿的位置,以缩短所述第i+1个第一路径段,得到调整后的第i+1个第一路径段上的起始位姿,以符合所述第二移动条件为目标,确定用于衔接所述第i个第一路径段上的终止位姿与所述调整后的第i+1个第一路径段上的起始位姿的衔接路径,缩短后的第i+1个第一路径段的长度不小于所述阈值;或者,
调整所述移动物在所述第i个第一路径段上的终止位姿的位置,以缩短所述第i个第一路径段,得到调整后的第i个第一路径段上的终止位姿,以符合所述第二移动条件为目标,确定用于衔接所述调整后的第i个第一路径段上的终止位姿与所述第i+1个第一路径段上的起始位姿的衔接路径,缩短后的第i个第一路径段的长度不小于所述阈值;或者,
调整所述移动物在所述第i+1个第一路径段上的起始位姿的位置,以缩短所述第i+1个第一路径段,得到调整后的第i+1个第一路径段上的起始位姿,以及调整所述移动物在所述第i个第一路径段上的终止位姿的位置,以缩短所述第i个第一路径段,得到调整后的第i个第一路径段上的终止位姿,以符合所述第二移动条件为目标,确定用于衔接所述调整后的第i个第一路径段上的终止位姿与所述调整后的第i+1个第一路径段上的起始位姿的衔接路径,缩短后的第i+1个第一路径段和缩短后的第i个第一路径段的长度均不小于所述阈值。
6.一种路径规划装置,其特征在于,所述装置包括:
第一确定模块,用于根据起始位置和终点位置,确定出通行路径,所述通行路径包括多个首尾相接的路径段,且所述通行路径中相邻两个所述路径段之间存在拐点,单个所述路径段内不存在拐点;
第二确定模块,用于确定所述通行路径中的第一路径段,所述第一路径段是长度不小于阈值的路径段;
第三确定模块,用于对于所述通行路径中的第j个第一路径段,根据移动物的长宽信息和所述第j个第一路径段的通道信息,以符合第一移动条件为目标,确定第一起始位姿和第一终止位姿,所述第j个第一路径段的通道信息包括所述第j个第一路径段的轨迹坐标和通道宽度;其中,所述第一起始位姿为所述移动物在所述第j个第一路径段上的起始位姿,所述第一终止位姿为所述移动物在所述第j个第一路径段上的终止位姿,所述第一起始位姿的位置为所述第j个第一路径段的起点位置,所述第一终止位姿的位置为所述第j个第一路径段的终止位置,所述第一起始位姿的姿态与所述第一终止位姿的姿态一致;所述第一移动条件包括:所述移动物从所述第j个第一路径段上的起始位姿移动到所述第j个第一路径段上的终止位姿的过程中,所述移动物上距离障碍物最近的点与障碍物之间的距离不小于安全距离;
第四确定模块,用于若存在未首尾相接的第i个第一路径段与第i+1个第一路径段,则基于所述移动物的长宽信息和第二路径段的通道信息,确定用于衔接所述第i个第一路径段上的终止位姿与所述第i+1个第一路径段上的起始位姿的衔接路径,所述第二路径段是所述通行路径中位于所述第i个第一路径段和所述第i+1个第一路径段之间的路径段;
路径更新模块,用于基于所述衔接路径更新所述通行路径,得到修正后的通行路径。
7.根据权利要求6所述的装置,其特征在于,在无法确定出符合所述第一移动条件的第一起始位姿和第一终止位姿的情况下,所述第三确定模块还包括:
路径段缩短子模块,用于按照路径调整步长,调整所述第j个第一路径段的参考端的位置,以对所述第j个第一路径段进行缩短,得到缩短后的第j个第一路径段,所述参考端为所述第j个第一路径段的两端中的一端,所述缩短后的第j个第一路径段的长度不小于所述阈值;
位姿调整子模块,用于根据所述移动物的长宽信息和所述缩短后的第j个第一路径段的通道信息,以符合所述第一移动条件为调整目标,对所述第一起始位姿和所述第一终止位姿进行调整,调整后的第一终止位姿为所述移动物在所述第j个第一路径段上的终止位姿,调整后的第一起始位姿为所述移动物在所述第j个第一路径段上的起始位姿。
8.根据权利要求7所述的装置,其特征在于,
所述参考端为所述第j个第一路径段的两端中距离所述第j个第一路径段所在通道中障碍物较近的一端;或者,
所述参考端为所述第j个第一路径段的两端中的任意一端。
9.根据权利要求6-8任一所述的装置,其特征在于,所述第四确定模块包括:
衔接路径确定子模块,用于基于所述移动物的长宽信息和所述第二路径段的通道信息,以符合第二移动条件为目标,确定所述用于衔接所述第i个第一路径段上的终止位姿与所述第i+1个第一路径段上的起始位姿的衔接路径;
其中,所述第二移动条件包括:所述移动物从所述第二路径段上的起始位姿移动到所述第二路径段上的终止位姿的过程中,所述移动物上距离障碍物最近的点与障碍物之间的距离不小于安全距离。
10.根据权利要求9所述的装置,其特征在于,在无法确定出符合所述第二移动条件且能够衔接所述第i个第一路径段上的终止位姿与所述第i+1个第一路径段上的起始位姿的衔接路径的情况下,所述第四确定模块还包括:
第一衔接路径调整子模块,用于调整所述移动物在所述第i+1个第一路径段上的起始位姿的位置,以缩短所述第i+1个第一路径段,得到调整后的第i+1个第一路径段上的起始位姿,以符合所述第二移动条件为目标,确定用于衔接所述第i个第一路径段上的终止位姿与所述调整后的第i+1个第一路径段上的起始位姿的衔接路径,缩短后的第i+1个第一路径段的长度不小于所述阈值;或者,
第二衔接路径调整子模块,用于调整所述移动物在所述第i个第一路径段上的终止位姿的位置,以缩短所述第i个第一路径段,得到调整后的第i个第一路径段上的终止位姿,以符合所述第二移动条件为目标,确定用于衔接所述调整后的第i个第一路径段上的终止位姿与所述第i+1个第一路径段上的起始位姿的衔接路径,缩短后的第i个第一路径段的长度不小于所述阈值;或者,
第三衔接路径调整子模块,用于调整所述移动物在所述第i+1个第一路径段上的起始位姿的位置,以缩短所述第i+1个第一路径段,得到调整后的第i+1个第一路径段上的起始位姿,以及调整所述移动物在所述第i个第一路径段上的终止位姿的位置,以缩短所述第i个第一路径段,得到调整后的第i个第一路径段上的终止位姿,以符合所述第二移动条件为目标,确定用于衔接所述调整后的第i个第一路径段上的终止位姿与所述调整后的第i+1个第一路径段上的起始位姿的衔接路径,缩短后的第i+1个第一路径段和缩短后的第i个第一路径段的长度均不小于所述阈值。
11.一种计算机设备,其特征在于,所述计算机设备包括处理器、通信接口、存储器和通信总线;所述处理器、所述通信接口和所述存储器通过所述通信总线完成相互间的通信;所述存储器用于存放计算机程序;所述处理器用于执行所述存储器上所存放的程序,以实现权利要求1至5任一项所述方法的步骤。
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202111632682.4A CN114355925B (zh) | 2021-12-29 | 2021-12-29 | 路径规划方法、装置、设备及计算机可读存储介质 |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202111632682.4A CN114355925B (zh) | 2021-12-29 | 2021-12-29 | 路径规划方法、装置、设备及计算机可读存储介质 |
Publications (2)
Publication Number | Publication Date |
---|---|
CN114355925A CN114355925A (zh) | 2022-04-15 |
CN114355925B true CN114355925B (zh) | 2024-03-19 |
Family
ID=81103300
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
CN202111632682.4A Active CN114355925B (zh) | 2021-12-29 | 2021-12-29 | 路径规划方法、装置、设备及计算机可读存储介质 |
Country Status (1)
Country | Link |
---|---|
CN (1) | CN114355925B (zh) |
Citations (9)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
JP2007328486A (ja) * | 2006-06-07 | 2007-12-20 | Yaskawa Electric Corp | 移動ロボット |
CN105867372A (zh) * | 2016-04-07 | 2016-08-17 | 重庆大学 | 全方位移动机器人狭窄直角弯下转向运动规划方法及系统 |
CN110440824A (zh) * | 2019-08-27 | 2019-11-12 | 广州小鹏汽车科技有限公司 | 一种路径规划方法及路径规划系统 |
CN110440823A (zh) * | 2019-08-27 | 2019-11-12 | 广州小鹏汽车科技有限公司 | 一种路径规划方法及路径规划系统 |
CN111830957A (zh) * | 2019-04-19 | 2020-10-27 | 北京京东尚科信息技术有限公司 | 一种路径规划方法和装置 |
CN111896004A (zh) * | 2020-07-28 | 2020-11-06 | 湖南大学 | 一种狭窄通道车辆轨迹规划方法及系统 |
CN112783144A (zh) * | 2019-10-22 | 2021-05-11 | 舜宇光学(浙江)研究院有限公司 | 路径生成方法和路径规划方法及其系统和设备 |
CN113219975A (zh) * | 2021-05-08 | 2021-08-06 | 珠海市一微半导体有限公司 | 一种路线优化方法、路径规划方法、芯片及机器人 |
CN113492830A (zh) * | 2020-04-08 | 2021-10-12 | 华为技术有限公司 | 一种车辆泊车路径规划方法及相关设备 |
Family Cites Families (1)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
US10001780B2 (en) * | 2016-11-02 | 2018-06-19 | Brain Corporation | Systems and methods for dynamic route planning in autonomous navigation |
-
2021
- 2021-12-29 CN CN202111632682.4A patent/CN114355925B/zh active Active
Patent Citations (9)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
JP2007328486A (ja) * | 2006-06-07 | 2007-12-20 | Yaskawa Electric Corp | 移動ロボット |
CN105867372A (zh) * | 2016-04-07 | 2016-08-17 | 重庆大学 | 全方位移动机器人狭窄直角弯下转向运动规划方法及系统 |
CN111830957A (zh) * | 2019-04-19 | 2020-10-27 | 北京京东尚科信息技术有限公司 | 一种路径规划方法和装置 |
CN110440824A (zh) * | 2019-08-27 | 2019-11-12 | 广州小鹏汽车科技有限公司 | 一种路径规划方法及路径规划系统 |
CN110440823A (zh) * | 2019-08-27 | 2019-11-12 | 广州小鹏汽车科技有限公司 | 一种路径规划方法及路径规划系统 |
CN112783144A (zh) * | 2019-10-22 | 2021-05-11 | 舜宇光学(浙江)研究院有限公司 | 路径生成方法和路径规划方法及其系统和设备 |
CN113492830A (zh) * | 2020-04-08 | 2021-10-12 | 华为技术有限公司 | 一种车辆泊车路径规划方法及相关设备 |
CN111896004A (zh) * | 2020-07-28 | 2020-11-06 | 湖南大学 | 一种狭窄通道车辆轨迹规划方法及系统 |
CN113219975A (zh) * | 2021-05-08 | 2021-08-06 | 珠海市一微半导体有限公司 | 一种路线优化方法、路径规划方法、芯片及机器人 |
Non-Patent Citations (1)
Title |
---|
基于二段多项式的窄空间平行泊车路径规划方法;熊莹;毛雪松;;计算机系统应用(08);215-220 * |
Also Published As
Publication number | Publication date |
---|---|
CN114355925A (zh) | 2022-04-15 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
CN108241375B (zh) | 一种自适应蚁群算法在移动机器人路径规划中的应用方法 | |
Draganjac et al. | Decentralized control of multi-AGV systems in autonomous warehousing applications | |
Han et al. | Mobile robot path planning with surrounding point set and path improvement | |
Hornung et al. | Anytime search-based footstep planning with suboptimality bounds | |
Martínez-Barberá et al. | Autonomous navigation of an automated guided vehicle in industrial environments | |
JP4138541B2 (ja) | 分散型経路計画装置及び方法、分散型経路計画プログラム | |
Dakulović et al. | Complete coverage D* algorithm for path planning of a floor-cleaning mobile robot | |
Le et al. | Multi-robot motion planning with dynamics via coordinated sampling-based expansion guided by multi-agent search | |
Song et al. | T $^\star $: Time-Optimal Risk-Aware Motion Planning for Curvature-Constrained Vehicles | |
Oliveira et al. | Combining lattice-based planning and path optimization in autonomous heavy duty vehicle applications | |
CN114460965B (zh) | 一种基于改进人工势场法的无人机三维避障方法 | |
WO2021185657A2 (en) | Using static scores to control vehicle operations | |
Lin et al. | Sampling-based trajectory repairing for autonomous vehicles | |
CN113741454A (zh) | 一种基于搜索的多智能体路径规划方法及系统 | |
Mohamed et al. | Autonomous navigation of agvs in unknown cluttered environments: log-mppi control strategy | |
CN114355925B (zh) | 路径规划方法、装置、设备及计算机可读存储介质 | |
Han et al. | Path regeneration decisions in a dynamic environment | |
Qi et al. | Hierarchical motion planning for autonomous vehicles in unstructured dynamic environments | |
JP2024045465A (ja) | 走行制御装置、走行制御方法及びコンピュータプログラム | |
Gonzalez et al. | Search-based planning with provable suboptimality bounds for continuous state spaces | |
CN115388889A (zh) | 路径规划方法及装置 | |
Bigaj et al. | A memetic algorithm based procedure for a global path planning of a movement constrained mobile robot | |
CN115273066A (zh) | 货架识别方法、装置、电子设备及机器可读存储介质 | |
Pittol et al. | Loop-Aware Exploration Graph: A concise representation of environments for exploration and active loop-closure | |
Bhat et al. | Safe, efficient waypoint manipulation for path planning of non-holonomic robots |
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 | ||
CB02 | Change of applicant information |
Address after: 310051 room 304, B / F, building 2, 399 Danfeng Road, Binjiang District, Hangzhou City, Zhejiang Province Applicant after: Hangzhou Hikvision Robot Co.,Ltd. Address before: 310051 room 304, B / F, building 2, 399 Danfeng Road, Binjiang District, Hangzhou City, Zhejiang Province Applicant before: HANGZHOU HIKROBOT TECHNOLOGY Co.,Ltd. |
|
CB02 | Change of applicant information | ||
GR01 | Patent grant | ||
GR01 | Patent grant |