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