CN111531550B - 运动规划方法及装置、存储介质、电子装置 - Google Patents

运动规划方法及装置、存储介质、电子装置 Download PDF

Info

Publication number
CN111531550B
CN111531550B CN202010654872.5A CN202010654872A CN111531550B CN 111531550 B CN111531550 B CN 111531550B CN 202010654872 A CN202010654872 A CN 202010654872A CN 111531550 B CN111531550 B CN 111531550B
Authority
CN
China
Prior art keywords
node
path
random tree
root node
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.)
Active
Application number
CN202010654872.5A
Other languages
English (en)
Other versions
CN111531550A (zh
Inventor
骆思宇
汪鹏飞
刘羽
卢维
任宇鹏
殷俊
周璐
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Zhejiang Dahua Technology Co Ltd
Original Assignee
Zhejiang Dahua Technology Co Ltd
Priority date (The priority date 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 date listed.)
Filing date
Publication date
Application filed by Zhejiang Dahua Technology Co Ltd filed Critical Zhejiang Dahua Technology Co Ltd
Priority to CN202010654872.5A priority Critical patent/CN111531550B/zh
Publication of CN111531550A publication Critical patent/CN111531550A/zh
Application granted granted Critical
Publication of CN111531550B publication Critical patent/CN111531550B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

Classifications

    • B—PERFORMING OPERATIONS; TRANSPORTING
    • B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
    • B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
    • B25J9/00—Programme-controlled manipulators
    • B25J9/16—Programme controls
    • B25J9/1694—Programme controls characterised by use of sensors other than normal servo-feedback from position, speed or acceleration sensors, perception control, multi-sensor controlled systems, sensor fusion
    • B25J9/1697—Vision controlled systems
    • B—PERFORMING OPERATIONS; TRANSPORTING
    • B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
    • B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
    • B25J9/00—Programme-controlled manipulators
    • B25J9/16—Programme controls
    • B25J9/1656—Programme controls characterised by programming, planning systems for manipulators
    • B25J9/1664—Programme controls characterised by programming, planning systems for manipulators characterised by motion, path, trajectory planning
    • B25J9/1666—Avoiding collision or forbidden zones
    • B—PERFORMING OPERATIONS; TRANSPORTING
    • B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
    • B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
    • B25J9/00—Programme-controlled manipulators
    • B25J9/16—Programme controls
    • B25J9/1674—Programme controls characterised by safety, monitoring, diagnostic
    • B25J9/1676—Avoiding collision or forbidden zones

Landscapes

  • Engineering & Computer Science (AREA)
  • Robotics (AREA)
  • Mechanical Engineering (AREA)
  • Manipulator (AREA)
  • Management, Administration, Business Operations System, And Electronic Commerce (AREA)

Abstract

本发明提供了一种运动规划方法及装置、存储介质、电子装置，所述方法包括：在采样空间内确定起点位形和终点位形；将所述起点位形作为根节点建立第一随机树，将所述终点位形作为根节点建立第二随机树；在所述第一随机树中的第一目标节点和所述第二随机树中的第二目标节点之间的连线与障碍物未碰撞的情况下，确定出第一路径；对所述第一路径进行路径优化，得到满足预设约束的路径。通过本发明，解决了路径规划的效率不高且耗时的问题，进而达到了加快收敛速度和优化搜索路径的的效果。

Description

运动规划方法及装置、存储介质、电子装置
技术领域
本发明涉及机器人技术、路径规划领域，具体而言，涉及一种运动规划方法及装置、存储介质、电子装置。
背景技术
运动规划是指对于给定的起点和终点，使计算出满足约束的路径。比如针对固定场景，通过机器视觉技术构建环境，通过运动规划完成机械臂的自主移动。
对于多自由度的机械臂的运动规划，由于会进行多次的碰撞检测，将会耗费大量时间，降低了运动规划的效率。
针对相关技术中，路径规划的效率不高且耗时的问题，目前尚未存在有效的解决方案。
发明内容
本发明实施例提供了一种运动规划方法及装置、存储介质、电子装置，以至少解决相关技术中路径规划的效率不高且耗时的问题。
根据本发明的一个实施例，提供了一种运动规划方法，包括：在采样空间内确定起点位形和终点位形；将所述起点位形作为第一根节点建立第一随机树，将所述终点位形作为第二根节点建立第二随机树；在所述第一随机树中的第一目标节点和所述第二随机树中的第二目标节点之间的连线与障碍物无碰撞的情况下，确定出第一路径；对所述第一路径进行路径优化，得到满足预设约束的路径。
在本发明的一个可选实施例中，对所述第一路径进行路径优化，得到满足预设约束的路径，包括：在所述第一路径为初始路径的情况下，通过缩短所述初始路径得到作为满足所述预设约束的路径的优化路径。
在本发明的一个可选实施例中，通过缩短所述初始路径得到优化路径，之后还包括：将所述采样空间约束在根据所述优化路径构造的预设形状的立体空间内。
在本发明的一个可选实施例中，将所述起点位形作为第一根节点建立第一随机树，将所述终点位形作为第二根节点建立第二随机树，包括：在所述采样空间内进行随机采样得到第一集合，其中，所述第一集合中是待添加到第一随机树中的多个无碰撞的采样点集合；将所述起点位形作为第一根节点添加到第二集合中得到第一随机树，其中，所述第二集合中是已经被添加到第一随机树中的节点集合；将所述终点位形作为第二根节点添加到第二集合中得到第二随机树，其中，所述第二集合中是已经被添加到第二随机树中的节点集合。
在本发明的一个可选实施例中，确定在所述第一随机树中的第一目标节点和所述第二随机树中的第二目标节点，包括：在所述第一集合中搜索第一根节点的邻点和第二根节点的邻点，并将第一根节点的邻点和第二根节点的邻点作为新节点与相应的根节点相连；在所述新节点与所述相应的根节点之间的连线无碰撞的情况下，将所述邻点从所述第一集合中删除并添加到所述第二集合中；根据所述第二集合中确定出所述第一目标节点和第二目标节点。
在本发明的一个可选实施例中，确定在所述第一随机树中的第一目标节点和所述第二随机树中的第二目标节点之后还包括：搜索所述第二集合中新节点的邻点，将在预设范围内未找到无碰撞邻点的新节点从所述第二集合中删除，并将所述新节点添加到所述第一集合中。
在本发明的一个可选实施例中，在所述第一随机树中的第一目标节点和所述第二随机树中的第二目标节点之间的连线与障碍物无碰撞的情况下，之后包括：在所述第一随机树中的第一目标节点和所述第二随机树中的第二目标节点之间的连线与障碍物碰撞的情况下，继续将所述起点位形作为第一根节点建立第一随机树，并继续将所述终点位形作为第二根节点建立第二随机树；在所述第一随机树中的第三目标节点和所述第二随机树中的第四目标节点之间的连线与障碍物无碰撞的情况下，确定出第二路径；通过缩短所述第二路径得到作为满足所述预设约束的路径的优化路径。
根据本发明的另一个实施例，提供了一种运动规划装置，包括：第一确定模块，用于在采样空间内确定起点位形和终点位形；建立模块，用于将所述起点位形作为第一根节点建立第一随机树，将所述终点位形作为第二根节点建立第二随机树；第二确定模块，用于在所述第一随机树中的第一目标节点和所述第二随机树中的第二目标节点之间的连线与障碍物无碰撞的情况下，确定出第一路径；优化模块，用于对所述第一路径进行路径优化，得到满足预设约束的路径。
根据本发明的又一个实施例，还提供了一种存储介质，所述存储介质中存储有计算机程序，其中，所述计算机程序被设置为运行时执行上述任一项方法实施例中的步骤。
根据本发明的又一个实施例，还提供了一种电子装置，包括存储器和处理器，所述存储器中存储有计算机程序，所述处理器被设置为运行所述计算机程序以执行上述任一项方法实施例中的步骤。
通过本发明，由于通过将起点位形和终点位形分别作为第一根节点和第二根节点建立第一随机树和第二随机树，能够在所述第一随机树中的第一目标节点和所述第二随机树中的第二目标节点之间的连线与障碍物无碰撞的情况下，确定出第一路径，并通过对所述第一路径优化得到满足预设约束的路径。因此，可以解决路径规划的效率不高且耗时的问题，达到加快收敛速度和优化搜索路径的效果。
附图说明
此处所说明的附图用来提供对本发明的进一步理解，构成本申请的一部分，本发明的示意性实施例及其说明用于解释本发明，并不构成对本发明的不当限定。在附图中：
图1是本发明实施例的一种运动规划方法的移动终端的硬件结构框图；
图2是根据本发明实施例的运动规划方法的流程图；
图3是根据本发明实施例的运动规划装置的结构框图；
图4a-图4d是根据本发明实施例中的初始路径生长示意图；
图5a-图5f是根据本发明可选实施例的包裹障碍物法示意图；
图6是优化后采样空间发的二维平面图；
图7为根据本发明优选实施例的一种可选的运动规划方法的生成过程示意图。
具体实施方式
下文中将参考附图并结合实施例来详细说明本发明。需要说明的是，在不冲突的情况下，本申请中的实施例及实施例中的特征可以相互组合。
需要说明的是，本发明的说明书和权利要求书及上述附图中的术语“第一”、“第二”等是用于区别类似的对象，而不必用于描述特定的顺序或先后次序。
实施例1
本申请实施例一所提供的方法实施例可以在移动终端、计算机终端或者类似的运算装置中执行。以运行在移动终端上为例，图1是本发明实施例的一种运动规划方法的移动终端的硬件结构框图。如图1所示，移动终端10可以包括一个或多个（图1中仅示出一个）处理器102（处理器102可以包括但不限于微处理器MCU或可编程逻辑器件FPGA等的处理装置）和用于存储数据的存储器104，可选地，上述移动终端还可以包括用于通信功能的传输设备106以及输入输出设备108。本领域普通技术人员可以理解，图1所示的结构仅为示意，其并不对上述移动终端的结构造成限定。例如，移动终端10还可包括比图1中所示更多或者更少的组件，或者具有与图1所示不同的配置。
存储器104可用于存储计算机程序，例如，应用软件的软件程序以及模块，如本发明实施例中的运动规划方法对应的计算机程序，处理器102通过运行存储在存储器104内的计算机程序，从而执行各种功能应用以及数据处理，即实现上述的方法。存储器104可包括高速随机存储器，还可包括非易失性存储器，如一个或者多个磁性存储装置、闪存、或者其他非易失性固态存储器。在一些实例中，存储器104可进一步包括相对于处理器102远程设置的存储器，这些远程存储器可以通过网络连接至移动终端10。上述网络的实例包括但不限于互联网、企业内部网、局域网、移动通信网及其组合。
传输装置106用于经由一个网络接收或者发送数据。上述的网络具体实例可包括移动终端10的通信供应商提供的无线网络。在一个实例中，传输装置106包括一个网络适配器（Network Interface Controller，简称为NIC），其可通过基站与其他网络设备相连从而可与互联网进行通讯。在一个实例中，传输装置106可以为射频（Radio Frequency，简称为RF）模块，其用于通过无线方式与互联网进行通讯。
在本实施例中提供了一种运行于上述移动终端的运动规划方法，图2是根据本发明实施例的运动规划方法的流程图，如图2所示，该流程包括如下步骤：
步骤S202，在采样空间内确定起点位形和终点位形；
需要注意的是，在采样空间内的起点位形和终点位形对于本领域技术人员而言是公知的。比如以机械臂为例，在机械臂末端执行器在关节空间中的起点位形x start 和机械臂在笛卡尔空间中所要达到的任务终，再根据逆运动学方程得到任务终点在关节空间中的位形x goal 。
步骤S204，将所述起点位形作为第一根节点建立第一随机树，将所述终点位形作为第二根节点建立第二随机树；
即分别根据所述起点位形和所述终点位形作为第一根节点和第二根节点向两个方向拓展建立随机数。
步骤S206，在所述第一随机树中的第一目标节点和所述第二随机树中的第二目标节点之间的连线与障碍物无碰撞的情况下，确定出第一路径；
在目标节点之间的连线与障碍物无碰撞的情况下，即若两点之间的连线未与障碍物发生碰撞，得到一条无碰撞的初始路径。
步骤S208，对所述第一路径进行路径优化，得到满足预设约束的路径。
由于路径是通过随机搜索产生的，所以相比于最优路径有较大差距，需要进行优化，才能能够得到满足预设约束的路径。
通过上述步骤，由于通过将起点位形和终点位形分别作为第一根节点和第二根节点建立第一随机树和第二随机树，能够在所述第一随机树中的第一目标节点和所述第二随机树中的第二目标节点之间的连线与障碍物未碰撞的情况下，确定出第一路径，并通过对所述第一路径优化得到满足预设约束的路径。因此，可以解决路径规划的效率不高且耗时的问题，达到加快收敛速度和优化搜索路径的效果。
为了进一步对路径进行优化，对所述第一路径进行路径优化，得到满足预设约束的路径，包括：在所述第一路径为初始路径的情况下，通过缩短所述初始路径得到作为满足所述预设约束的路径的优化路径。
即双向建立随机树的方式，可以快速得到的一条初始路径，但这条路径具有很大的随意性，与最优路径差距较大。需要对初始路径进行优化，并且在路径接近最优时，再次使用包裹障碍物法来优化路径，既避免因过多使用包裹障碍物法增加碰撞检测时间，又提高了效率。
具体实施时，将所述起点位形作为第一根节点建立第一随机树，将所述终点位形作为第二根节点建立第二随机树，包括：在所述采样空间内进行随机采样得到第一集合，其中，所述第一集合中是待添加到第一随机树中的多个无碰撞的采样点集合；将所述起点位形作为第一根节点添加到第二集合中得到第一随机树，其中，所述第二集合中是已经被添加到第一随机树中的节点集合；将所述终点位形作为第二根节点添加到第二集合中得到第二随机树，其中，所述第二集合中是已经被添加到第二随机树中的节点集合。
进一步地，确定在所述第一随机树中的第一目标节点和所述第二随机树中的第二目标节点，包括：在所述第一集合中搜索所述第一根节点的邻点和第二根节点的邻点，并将所述第一根节点的邻点和第二根节点的邻点作为新节点与相应的根节点相连；在所述新节点与所述相应的根节点之间的连线无碰撞的情况下，将所述邻点从所述第一集合中删除并添加到所述第二集合中；根据所述第二集合中确定出所述第一目标节点和第二目标节点。
进一步地，确定在所述第一随机树中的第一目标节点和所述第二随机树中的第二目标节点之后还包括：搜索所述第二集合中新节点的邻点，将在预设范围内未找到无碰撞邻点的新节点从所述第二集合中删除，并将所述新节点添加到所述第一集合中。
可选地，可以使用包裹障碍物法有效缩短初始路径，并在路径接近最优时，再次使用包裹障碍物法来优化路径，既避免因过多使用包裹障碍物法增加碰撞检测时间，又提高了计算效率。
如图5a-图5f所示，表示二维平面中通过逼近包裹障碍物来优化路径的过程。原始路径(如图5a所示），经过二次优化后的路径（如图5f）示)。
具体实施包裹障碍物法时，（如图5a）首先假设通过初始路径P 0 一共有四个节点,需要替换的节点是除了起点与终点之外的节点，即节点x2和 x3，每个节点的替换过程分为如下两步。
第一步：离散节点x2和x3之间的路径，得到n个新的节点，将它们记为x2i（ i∈（1，n）），然后将这些离散点依次与x2的前一个节点相连，对这条连线进行碰撞检测，找到发生碰撞检测的第一个节点记为x‘ 2（如图5b）；
第二步：离散x2前一个节点与节点x‘ 2之间的路径，得到n个新的节点，将它们记为x’2i（i∈（1，n）），然后将这些离散点依次与x3相连，对这条连线进行碰撞检测，找到发生碰撞检测的第一个节点记为x2new（如图5c）；经过这两步处理就可以将节点x2替换成新节点x2new，同时节点x3的处理过程相同（如图5d，图5e）。根据三角不等式原理，二次处理后的路径P 1 明显优于初始路径P 0 。
进一步，通过缩短所述初始路径得到优化路径，之后还包括：将所述采样空间约束在根据所述优化路径构造的预设形状的立体空间内。
优选地，预设形状的立体空间可以是椭球体。具体地，根据当前路径构建椭球体来优化采样空间，从而确保只从能优化当前路径的空间内采样。
具体地，实施上述立体空间的构造时，将初始的采样空间用下式来表示
Figure DEST_PATH_IMAGE002
即正是椭球体的表达式，所以又可以称将搜索空间约束在由当前路径构造的椭球体内，式中Xfree表示无碰撞的采样空间，C max 表示路径P 1 的路径代价，优化后采样空间的二维平面图如图6所示。
在图6中，其中C min 为起点与终点之间忽略障碍物后的直线距离，起点Cstart和终点Xgool分别为椭球体的两个焦点，C max 为椭球体的长轴，
Figure DEST_PATH_IMAGE004
为椭球体的短轴。
为了提高搜索效率，同时使得路径具有渐进优化的性质，可根据优化后的路径来优化采样空间，在所述第一随机树中的第一目标节点和所述第二随机树中的第二目标节点之间的连线与障碍物碰撞的情况下，继续将所述起点位形作为第一根节点建立第一随机树，并继续将所述终点位形作为第二根节点建立第二随机树；即无法确定出第一路径的情况下，需要再次进行随机树的生长。在继续将所述起点位形作为第一根节点建立第一随机树，并继续将所述终点位形作为第二根节点建立第二随机树的情况下，在所述第一随机树中的第三目标节点和所述第二随机树中的第四目标节点之间的连线与障碍物未碰撞的情况下，确定出第二路径；通过缩短所述第二路径得到作为满足所述预设约束的路径的优化路径。即通过第二路径对采样空间进行优化，实现了提高搜索效率，同时使得路径具有渐进优化的性质。
通过以上的实施方式的描述，本领域的技术人员可以清楚地了解到根据上述实施例的方法可借助软件加必需的通用硬件平台的方式来实现，当然也可以通过硬件，但很多情况下前者是更佳的实施方式。基于这样的理解，本发明的技术方案本质上或者说对现有技术做出贡献的部分可以以软件产品的形式体现出来，该计算机软件产品存储在一个存储介质（如ROM/RAM、磁碟、光盘）中，包括若干指令用以使得一台终端设备（可以是手机，计算机，服务器，或者网络设备等）执行本发明各个实施例所述的方法。
实施例2
在本实施例中还提供了一种运动规划装置，该装置用于实现上述实施例及优选实施方式，已经进行过说明的不再赘述。如以下所使用的，术语“模块”可以实现预定功能的软件和/或硬件的组合。尽管以下实施例所描述的装置较佳地以软件来实现，但是硬件，或者软件和硬件的组合的实现也是可能并被构想的。
图3是根据本发明实施例的运动规划装置的结构框图，如图3所示，该装置包括
第一确定模块30，用于在采样空间内确定起点位形和终点位形；
建立模块32，用于将所述起点位形作为第一根节点建立第一随机树，将所述终点位形作为第二根节点建立第二随机树；
第二确定模块34，用于在所述第一随机树中的第一目标节点和所述第二随机树中的第二目标节点之间的连线与障碍物未碰撞的情况下，确定出第一路径；
优化模块36，用于对所述第一路径进行路径优化，得到满足预设约束的路径。
所述第一确定模块30中需要注意的是，在采样空间内的起点位形和终点位形对于本领域技术人员而言是公知的。比如以机械臂为例，在机械臂末端执行器在关节空间中的起点位形x start 和机械臂在笛卡尔空间中所要达到的任务中，再根据逆运动学方程得到任务终点在关节空间中的位形x goal 。
所述建立模块32中即分别根据所述起点位形和所述终点位形作为第一根节点和第二根节点向两个方向拓展建立随机数。
所述第二确定模块34中在目标节点之间的连线与障碍物未碰撞的情况下，即若两点之间的连线未与障碍物发生碰撞，得到一条无碰撞的初始路径。
所述优化模块36中由于路径是通过随机搜索产生的，所以相比于最优路径有较大差距，需要进行优化，才能能够得到满足预设约束的路径。
通过上述模块，由于通过将起点位形和终点位形分别作为第一根节点和第二根节点建立第一随机树和第二随机树，能够在所述第一随机树中的第一目标节点和所述第二随机树中的第二目标节点之间的连线与障碍物未碰撞的情况下，确定出第一路径，并通过对所述第一路径优化得到满足预设约束的路径。因此，可以解决路径规划的效率不高且耗时的问题，达到加快收敛速度和优化搜索路径的效果。
需要说明的是，上述各个模块是可以通过软件或硬件来实现的，对于后者，可以通过以下方式实现，但不限于此：上述模块均位于同一处理器中；或者，上述各个模块以任意组合的形式分别位于不同的处理器中。
为了更好的理解上述运动规划方法的生成流程，以下结合优选实施例对上述技术方案进行解释说明，但不用于限定本发明实施例的技术方案。
本发明优选实施例中，采用双向扩展策略，加快了收敛速度。同时，使用包裹障碍物法来优化生成的初始路径，从而有效地优化了初次的搜索结果。最后使用椭球体构建采用空间，优化了搜索效率，使得搜索路径趋近于最优。解决了路径规划的效率不高且耗时的问题。
图7为根据本发明优选实施例的一种可选的运动规划方法的生成过程示意图，如图7所示，
步骤S701，开始。
开始阶段，以多自由度机械臂为例。机械臂运动学建模，根据多自由度机械臂的DH参数，建立正逆运动学方程，获得机械臂关节空间与笛卡尔空间的对应关系和机械臂末端执行器在其基坐标系中的位置和方向。
步骤S702，采样空间内随机采样得到集合W。
已知机械臂末端执行器在关节空间中的起点位形x start 和其在笛卡尔空间中所要达到的任务终点如图4a所示，根据逆运动学方程，求解出任务终点在关节空间中的位形x goal 。
以起点和终点裁剪采样空间，将采样范围限制在如图4b所示的椭圆范围内，在该范围内进行随机采样，得到n个无碰撞的采样点，构成集合W，表示潜在的树节点。集合H表示已经被添加到树中的节点的集合。
步骤S703，将起点和终点添加到集合H和树T s T g 。
以添加到树Ts为例，将起点x start 作为树T s 的第一根节点，添加到集合H中，在集合W中搜索第一根节点的邻点。生长树采用由在采样空间内随机搜索得到的各个随机节点通过一定的生长策略构成，在本申请的实施例中并不进行具体限定。
步骤S704，在集合W中搜索H中新节点的邻点。
搜索集合H中新节点的邻点，将在一定范围内未找到无碰撞邻点的新节点从集合H删除，并将之添加到集合W中。将终点x goal 作为树T g 的第二根节点，重复步骤上述的随机树生长过程，如图4d所示。
步骤S705，是否发生碰撞或者没有邻点；如果是再执行步骤S704。
步骤S706，如果没有发生碰撞或者有邻点，将邻点加入H和树中并从W中删除。
尝试连接两棵树的最新节点，若两点之间的连线与障碍物发生了碰撞，则说明两树之间仍有一定的距离，需要按照随机树建立过程继续生长，若两点之间的连线未与障碍物发生碰撞，则得到了一条无碰撞的初始路径P0。
步骤S707，在集合W中搜索H中新节点xnew的邻点。
步骤S708，是否发生碰撞或没有邻点，如果是进入步骤S709，如果不是进入步骤S710。
步骤S709，将xnew从H中删除并添加到W中。
步骤S710，将邻点加入H和树中并从W中删除。
步骤S711，树T s 和树T g 的新节点的连线是否碰撞，如果否则进入步骤S712，如果是则返回步骤S704。
将无碰撞的邻点作为新节点与根节点相连，对两点之间的连线进行碰撞检测，若无碰撞，则将该邻点从集合W中删除并添加到集合H中，如图4c所示。
步骤S712，是否为初始路径。
步骤S713，如果是，则优化路径。
由于路径是通过随机搜索产生的，它相比于最优路径有较大差距，需要进行优化。优选地，采用包裹障碍物法来进行优化。
步骤S714，优化采样空间，进入步骤S702。
为了提高算法的搜索效率，同时使得路径具有渐进优化的性质，根据路径P 1 来优化采样空间，确保算法只从可能改善当前路径代价的空间内进行采样。
步骤S715，如果不是，是否趋近最优。
在新的采样空间中重复随机树生长，得到新的路径，若其路径代价小于上一条路径，则用这条新的路径来优化采样空间后，在优化后的采样空间中继续采样，生成新的路径。
当新的路径趋近于最优时，再次使用包裹障碍物法来优化路径，最终得到一条趋近于最优的路径。该步骤需还要注意的是如何判断路径已经趋近于最优。
可选地 ，采用一种启发式的方法来判断路径是否已经趋近于最优：对于某些微小的正εc (εc的取值与规划空间的大小呈正相关)，存在
Figure DEST_PATH_IMAGE005
其中，ci表示第i条路径的路径代价，那么可以认为算法已经趋近于最优，此时使用包裹障碍物法来快速得到最优路径。
步骤S716，优化路径。
优选地，采用包裹障碍物法来进行优化。
步骤S717，结束。
本发明的实施例还提供了一种存储介质，该存储介质中存储有计算机程序，其中，该计算机程序被设置为运行时执行上述任一项方法实施例中的步骤。
可选地，在本实施例中，上述存储介质可以被设置为存储用于执行以下步骤的计算机程序：
S1，在采样空间内确定起点位形和终点位形；
S2，将所述起点位形作为第一根节点建立第一随机树，将所述终点位形作为第二根节点建立第二随机树；
S3，在所述第一随机树中的第一目标节点和所述第二随机树中的第二目标节点之间的连线与障碍物未碰撞的情况下，确定出第一路径。
S4，对所述第一路径进行路径优化，得到满足预设约束的路径。
可选地，存储介质还被设置为存储用于执行以下步骤的计算机程序：
S1，在所述第一路径为初始路径的情况下，通过缩短所述初始路径得到作为满足所述预设约束的路径的优化路径；
S2，将所述采样空间约束在根据所述优化路径构造的预设形状的立体空间内。
可选地，在本实施例中，上述存储介质可以包括但不限于：U盘、只读存储器（Read-Only Memory，简称为ROM）、随机存取存储器（Random Access Memory，简称为RAM）、移动硬盘、磁碟或者光盘等各种可以存储计算机程序的介质。
本发明的实施例还提供了一种电子装置，包括存储器和处理器，该存储器中存储有计算机程序，该处理器被设置为运行计算机程序以执行上述任一项方法实施例中的步骤。
可选地，上述电子装置还可以包括传输设备以及输入输出设备，其中，该传输设备和上述处理器连接，该输入输出设备和上述处理器连接。
可选地，在本实施例中，上述处理器可以被设置为通过计算机程序执行以下步骤：
S1，在采样空间内确定起点位形和终点位形；
S2，将所述起点位形作为第一根节点建立第一随机树，将所述终点位形作为第二根节点建立第二随机树；
S3，在所述第一随机树中的第一目标节点和所述第二随机树中的第二目标节点之间的连线与障碍物未碰撞的情况下，确定出第一路径；
S4，对所述第一路径进行路径优化，得到满足预设约束的路径。
可选地，本实施例中的具体示例可以参考上述实施例及可选实施方式中所描述的示例，本实施例在此不再赘述。
显然，本领域的技术人员应该明白，上述的本发明的各模块或各步骤可以用通用的计算装置来实现，它们可以集中在单个的计算装置上，或者分布在多个计算装置所组成的网络上，可选地，它们可以用计算装置可执行的程序代码来实现，从而，可以将它们存储在存储装置中由计算装置来执行，并且在某些情况下，可以以不同于此处的顺序执行所示出或描述的步骤，或者将它们分别制作成各个集成电路模块，或者将它们中的多个模块或步骤制作成单个集成电路模块来实现。这样，本发明不限制于任何特定的硬件和软件结合。
以上所述仅为本发明的优选实施例而已，并不用于限制本发明，对于本领域的技术人员来说，本发明可以有各种更改和变化。凡在本发明的原则之内，所作的任何修改、等同替换、改进等，均应包含在本发明的保护范围之内。

Claims (7)

1.一种运动规划方法，其特征在于，包括：
在采样空间内确定起点位形和终点位形；
将所述起点位形作为第一根节点建立第一随机树，将所述终点位形作为第二根节点建立第二随机树，其中，包括：在所述采样空间内进行随机采样得到第一集合，其中，所述第一集合中是待添加到第一随机树和第二随机树中的多个无碰撞的采样点集合；将所述起点位形作为第一根节点添加到第二集合中得到第一随机树，其中，所述第二集合中是已经被添加到第一随机树中的节点集合；将所述终点位形作为第二根节点添加到第二集合中得到第二随机树，其中，所述第二集合中是已经被添加到第二随机树中的节点集合；
确定在所述第一随机树中的第一目标节点和所述第二随机树中的第二目标节点，包括：在所述第一集合中搜索所述第一根节点的第一邻点和所述第二根节点的第二邻点，并将所述第一邻点作为第一新节点与所述第一根节点相连，将所述第二邻点作为第二新节点与所述第二根节点相连；在所述第一新节点与所述第一根节点之间的连线与障碍物无碰撞的情况下，将所述第一邻点从所述第一集合中删除并添加到所述第二集合中，在所述第二新节点与所述第二根节点之间的连线与障碍物无碰撞的情况下，将所述第二邻点从所述第一集合中删除并添加到所述第二集合中；根据所述第二集合中的第一新节点确定出所述第一目标节点，根据所述第二集合中的第二新节点确定出所述第二目标节点；
确定在所述第一随机树中的第一目标节点和所述第二随机树中的第二目标节点之后还包括：搜索所述第二集合中第一新节点的第一邻点，将在预设范围内未找到无碰撞邻点的第一新节点从所述第二集合中删除，并将所述第一新节点添加到所述第一集合中；搜索所述第二集合中第二新节点的第二邻点，将在预设范围内未找到无碰撞邻点的第二新节点从所述第二集合中删除，并将所述第二新节点添加到所述第一集合中；
在所述第一随机树中的第一目标节点和所述第二随机树中的第二目标节点之间的连线与障碍物无碰撞的情况下，确定出第一路径；
对所述第一路径进行路径优化，得到满足预设约束的路径。
2.根据权利要求1所述的方法，其特征在于，对所述第一路径进行路径优化，得到满足预设约束的路径，包括：
在所述第一路径为初始路径的情况下，通过缩短所述初始路径得到作为满足所述预设约束的路径的优化路径。
3.根据权利要求2所述的方法，其特征在于，通过缩短所述初始路径得到优化路径，之后还包括：
将所述采样空间约束在根据所述优化路径构造的预设形状的立体空间内。
4.根据权利要求1所述的方法，其特征在于，还包括：
在所述第一随机树中的第一目标节点和所述第二随机树中的第二目标节点之间的连线与障碍物碰撞的情况下，继续将所述起点位形作为第一根节点建立第一随机树，并继续将所述终点位形作为第二根节点建立第二随机树；
在所述第一随机树中的第三目标节点和所述第二随机树中的第四目标节点之间的连线与障碍物无碰撞的情况下，确定出第二路径；通过缩短所述第二路径得到作为满足所述预设约束的路径的优化路径。
5.一种运动规划装置，其特征在于，包括：
第一确定模块，用于在采样空间内确定起点位形和终点位形；
建立模块，用于将所述起点位形作为第一根节点建立第一随机树，将所述终点位形作为第二根节点建立第二随机树，其中，所述建立模块用于：在所述采样空间内进行随机采样得到第一集合，其中，所述第一集合中是待添加到第一随机树和第二随机树中的多个无碰撞的采样点集合；将所述起点位形作为第一根节点添加到第二集合中得到第一随机树，其中，所述第二集合中是已经被添加到第一随机树中的节点集合；将所述终点位形作为第二根节点添加到第二集合中得到第二随机树，其中，所述第二集合中是已经被添加到第二随机树中的节点集合；
所述装置通过以下方式确定在所述第一随机树中的第一目标节点和所述第二随机树中的第二目标节点：在所述第一集合中搜索所述第一根节点的第一邻点和所述第二根节点的第二邻点，并将所述第一邻点作为第一新节点与所述第一根节点相连，将所述第二邻点作为第二新节点与所述第二根节点相连；在所述第一新节点与所述第一根节点之间的连线与障碍物无碰撞的情况下，将所述第一邻点从所述第一集合中删除并添加到所述第二集合中，在所述第二新节点与所述第二根节点之间的连线与障碍物无碰撞的情况下，将所述第二邻点从所述第一集合中删除并添加到所述第二集合中；根据所述第二集合中的第一新节点确定出所述第一目标节点，根据所述第二集合中的第二新节点确定出所述第二目标节点；
所述装置还用于在确定在所述第一随机树中的第一目标节点和所述第二随机树中的第二目标节点之后：搜索所述第二集合中第一新节点的第一邻点，将在预设范围内未找到无碰撞邻点的第一新节点从所述第二集合中删除，并将所述第一新节点添加到所述第一集合中；搜索所述第二集合中第二新节点的第二邻点，将在预设范围内未找到无碰撞邻点的第二新节点从所述第二集合中删除，并将所述第二新节点添加到所述第一集合中；
第二确定模块，用于在所述第一随机树中的第一目标节点和所述第二随机树中的第二目标节点之间的连线与障碍物无碰撞的情况下，确定出第一路径；
优化模块，用于对所述第一路径进行路径优化，得到满足预设约束的路径。
6.一种存储介质，其特征在于，所述存储介质中存储有计算机程序，其中，所述计算机程序被设置为运行时执行所述权利要求1至4任一项中所述的方法。
7.一种电子装置，包括存储器和处理器，其特征在于，所述存储器中存储有计算机程序，所述处理器被设置为运行所述计算机程序以执行所述权利要求1至4任一项中所述的方法。
CN202010654872.5A 2020-07-09 2020-07-09 运动规划方法及装置、存储介质、电子装置 Active CN111531550B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202010654872.5A CN111531550B (zh) 2020-07-09 2020-07-09 运动规划方法及装置、存储介质、电子装置

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202010654872.5A CN111531550B (zh) 2020-07-09 2020-07-09 运动规划方法及装置、存储介质、电子装置

Publications (2)

Publication Number Publication Date
CN111531550A CN111531550A (zh) 2020-08-14
CN111531550B true CN111531550B (zh) 2020-11-06

Family

ID=71978393

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202010654872.5A Active CN111531550B (zh) 2020-07-09 2020-07-09 运动规划方法及装置、存储介质、电子装置

Country Status (1)

Country Link
CN (1) CN111531550B (zh)

Families Citing this family (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN114526750B (zh) * 2022-01-10 2024-02-27 深圳市灵星雨科技开发有限公司 一种路径优化方法及装置
CN114700937B (zh) * 2022-01-13 2024-02-13 深圳市越疆科技有限公司 机械臂及其运动路径规划方法、控制系统、介质及机器人

Family Cites Families (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN109877836B (zh) * 2019-03-13 2021-06-08 浙江大华技术股份有限公司 路径规划方法、装置、机械臂控制器和可读存储介质
CN110370274A (zh) * 2019-07-03 2019-10-25 浙江大学 一种基于快速行进树的机械臂动态避障控制方法
CN110497403A (zh) * 2019-08-05 2019-11-26 上海大学 一种改进双向rrt算法的机械臂运动规划方法
CN110986953B (zh) * 2019-12-13 2022-12-06 达闼机器人股份有限公司 路径规划方法、机器人及计算机可读存储介质

Also Published As

Publication number Publication date
CN111531550A (zh) 2020-08-14

Similar Documents

Publication Publication Date Title
CN109947100B (zh) 路径规划方法、系统及终端设备
CN111531550B (zh) 运动规划方法及装置、存储介质、电子装置
Basu et al. Movement control algorithms for realization of fault-tolerant ad hoc robot networks
Wong et al. Gateway placement for latency and energy efficient data aggregation [wireless sensor networks]
US20060146791A1 (en) Network coding approach to rapid information dissemination
CN113281620B (zh) 基于自适应鲸鱼算法的故障区段定位方法、系统及介质
CN110772791A (zh) 三维游戏场景的路线生成方法、装置和存储介质
da Silva et al. On best drone tour plans for data collection in wireless sensor network
CN112436971A (zh) 基于蒙特卡洛树搜索的全域指控网络协作拓扑生成方法
CN107153889B (zh) 水质采样巡航船路径规划最优化方法
CN113188555A (zh) 一种移动机器人路径规划方法
CN111159577B (zh) 一种社群划分方法、装置、存储介质及电子装置
CN114430581B (zh) 基于蚁群策略的ac-olsr路由方法、设备及介质
CN115987375A (zh) 融合网络的电力设备关联与资源优化方法、系统及介质
CN115334591A (zh) 一种多终端多无人机分层调度辅助边缘计算资源分配方法
CN111861018A (zh) 一种基于路由信息协议的仓库拣选路径优化方法、存储介质及设备
Chen et al. A novel approach for path plan of mobile chargers in wireless rechargeable sensor networks
Tusi et al. Using ABC and RRT algorithms to improve mobile robot path planning with danger degree
Chaurasia et al. A hybrid heuristic for dominating tree problem
CN116627132A (zh) 基于rrt改进的移动机器人路径规划方法
CN114532898B (zh) 机器人的建图方法、机器人、存储介质、电子装置
CN116652927A (zh) 路径规划方法及装置、可读存储介质
WO2022110853A1 (zh) 可通行区域的探索方法、装置、存储介质及电子装置
Gugan et al. Towards the development of a robust path planner for autonomous drones
Chen et al. On time-constrained data harvesting in wireless sensor networks: Approximation algorithm design

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
GR01 Patent grant
GR01 Patent grant