CN114770512B - 用于救援清障的移动机器人机械臂搬运最优时间规划方法 - Google Patents

Abstract

本发明公开了一种用于救援清障的移动机器人机械臂搬运任务最优时间规划方法。根据救援清障任务确定搬运操作执行总次数，确定三种位置点的空间坐标以及三种位置点的通过顺序，进而构建成分层有向图，通过分层有向图的处理获得最优循环路径；确定最优循环路径的重复数目和剩余最短路径，将连续重复数目的多个最优循环路径与剩余最短路径进行拼接获得最终最短路径。相比于利用最短路径算法搜索大规模有向图求解，本方法能够显著降低优化过程的算法复杂度，并能快速实现对任务的规划，达到最短的完成时间。

Description

用于救援清障的移动机器人机械臂搬运最优时间规划方法

技术领域

本发明涉及了一种移动机器人机械臂控制方法，尤其是涉及了一种用于救援清障的移动机器人机械臂搬运任务最优时间规划方法。

背景技术

在实际救援搬运场景中，障碍物的清除与拾取放置任务是物体通过移动机器人机械臂进行实现，一个机械臂可以实现该区域不同位置的物体的拾取放置，在搬运过程中需要考虑避障，安全的等因素，防止造成二次伤害。其中每个机械臂所在的位置以及每个机械臂的各个拾取放置位置较为相近的。相邻区域中拾取放置位置之间的连线构成一条路径，障碍物从起点的拾取放置位置传输到终点的拾取放置位置的路径有多条，每条路径上的机械臂时间消耗不同，因此需要寻找一条最优时间消耗路径，进而提高救援的效率。

目前，寻找一条最优时间消耗的方式是将机械臂运行过程拓扑化，构成分层节点有向图，每层表示一空间坐标点对应的关节配置集合。层中节点表示在配置空间中的具体关节配置。寻找分层节点有向图中首层节点到尾层节点的最短路径，用多层节点有向图最短路径来表示机械臂的最优时间路径。现有的求解多层节点有向图的最短路径的方法为基于路径穷举搜索的算法，可以解决从某个节点到其他所有节点之间的最短路径问题。主要原理是以起点为中心向外层层扩展，直到扩展到终点为止。从而得到起点到其余节点的最短路径。但是求解大型的多层节点有向图的最短路径则过于复杂，且耗时较长，并且，求解的结果缺少有序性，在救援应用中时效性较差，因此现有技术缺少一种求解最短路径耗时更短的方法。

发明内容

针对现有技术的缺陷和改进需求，本发明提出了一种用于救援清障的移动机器人机械臂搬运任务最优时间规划方法，将循环多层有向图进行分解以求得全局最短路径。

为实现上述目的，本发明的技术方案包括：

1)根据救援清障任务的需要，确定搬运操作执行总次数N，对障碍物在移动机器人机械臂空间坐标系下的位置进行标定，确定抓取点、避障点和放置点的三种位置点的空间坐标以及三种位置点在救援清障任务中机械臂末端空间路径的通过顺序；

在抓取点和放置点中取一种位置点同时作为机械臂末端空间路径的起始位置点和目标位置点，一般可以选择放置点为起始位置点和目标位置点。

2)根据三种位置点的空间坐标以及三种位置点的通过顺序构建成分层有向图，通过分层有向图的处理获得最优循环路径；

根据抓取点、避障点和放置点的空间坐标，求解获得机械臂关节配置，并求得相邻位置点的不同关节配置之间的运行时间。以三种位置点的关节配置作为节点，运行时间作为边根据三种位置点的通过顺序构建成分层有向图，通过分层有向图的处理获得最优循环路径；

3)根据救援清障任务中的搬运操作执行总次数N，确定最优循环路径的重复数目和剩余最短路径，将连续重复数目的多个最优循环路径与剩余最短路径进行拼接获得最终最短路径。

所述步骤1)具体为：

根据救援清障任务的需要，对障碍物在移动机器人机械臂空间坐标系下的位置进行标定，且根据空间障碍物的分布情况确定机械臂的单一搬运操作的抓取点、避障点和放置点，以及抓取点、避障点和放置点在机械臂末端空间路径的通过顺序；根据需搬运物体的数量确定机械臂的搬运操作执行总次数N。

所述步骤2)具体为：

2.1)根据抓取点、避障点和放置点的空间坐标，求解获得每种位置点下的多种可能的机械臂关节配置，并按照三种位置点的通过顺序，计算顺序下相邻两个位置点的所有可能的关节配置之间的运行时间，以三种位置点的关节配置作为节点，一种位置点对应的所有关节配置作为层，关节配置之间的运行时间作为边，构建分层有向图的基元；

2.2)对分层有向图的基元中进行最短运行时间的赋权路径搜索，求得起始层和结束层之间的不同节点间的最短路径，作为机械臂在起始位置点和结束位置点下的不同关节配置之间的最短路径，组成单次搬运操作下机械臂关节配置的最短路径集合；

2.3)按照救援清障任务中的搬运操作执行总次数N对多次搬运操作的最短路径集合进行适配组合，获得多基元循环路径，选取最优循环路径。

所述步骤2.1)具体为：

根据确定单次搬运操作中的抓取点、避障点和放置点的空间坐标，在配置空间中求取机械臂在每种位置点下的所有可能的关节配置，由于关节配置的不唯一性，机械臂在同一位置点也会得到多个机械臂关节配置。机械臂在一种位置点下的一个关节配置作为多层有向图的一层中的一个节点，一个关节配置作为一个节点，机械臂在一种位置点下的所有多个关节配置组成了多层有向图的一层节点，一种位置点对应一层；

将单次搬运操作中的三种位置点在多层有向图中对应的层按照所述通过顺序进行依次排序，三种位置点中选择其一同时作为起始位置点和目标位置点，且作为起始位置点和目标位置点的位置点在多层有向图中对应的层结构重复两次，分别设置在多层有向图中的起始层和结束层，即起始层的各个节点和结束层的各个节点完全相同，不作为起始位置点和目标位置点的位置点在多层有向图中对应的层均作为中间层；

利用机械臂关节中电机运行的梯形控制曲线求得相邻层的每两个关节配置之间的运行时间，即上一个层的一个关节配置和下一个层的一个关节配置之间的运行时间，这样以两个不同层的两个关节配置之间的运行时间作为两个节点之间的边，从而构成了代表单次搬运操作的分层有向图的基元；这样是由多个起始位置点、目标位置点和中间点的所有关节配置以及之间的运行时间组成了由层、节点、边构成的分层有向图的基元。可以将将连续多次搬运操作对应获得的分层有向图的基元进行依次拼接获得分层有向图。

一个单次搬运操作形成一个分层有向图的基元，多个单次搬运操作的分层有向图的基元形成总的分层有向图。

求取分层有向图的最短路径为最短时间消耗，按照最短路径的位置点的关节配置序列按照时序控制机械臂进行连续多次搬运操作，实现搬运任务最优时间规划。

所述步骤2.2)具体为：

在一个分层有向图的基元中，分别以起始层的一个节点到结束层的一个节点构建作为一对循环节点对，分别求得每对循环节点对的两个节点之间的最短运行时间的路径，路径是由两个节点之间经过的各条边组成作为该对对循环节点对的最短路径；再由每对循环节点对下获得的最短路径组成最短路径集合，作为单次搬运操作下由三种位置点的所有可能的关节配置通过顺序组成序列的所有可能方案。对于一个起始层/结束层有j₁个节点的分层有向图的基元，最短路径集合中的最短路径元素总个数为j₁ ²个。

所述步骤2.3)具体为：

设置搬运操作次数n，在救援清障任务中的搬运操作执行总次数N的约束下遍历所有可能的搬运操作次数n，搬运操作次数n大于等于1小于等于N；在每种搬运操作次数n下，进行以下操作：

多基元循环路径涉及的搬运操作数量，即分层有向图的基元的数量进行遍历，且小于需求的搬运操作执行总次数N。

按照n次搬运操作的顺序将n次搬运操作对应的分层有向图的基元进行依次拼接形成分层有向图，且按照n次搬运操作的顺序将n次搬运操作对应的最短路径集合中的各条最短路径进行依次以首尾约束拼接，一种拼接结果作为一条多基元循环路径，按照排列组合方式获得多种拼接结果，所有拼接结果的多基元循环路径组成了分层有向图对应的多基元循环路径集合；

且拼接过程中，每条多基元循环路径中符合以下约束：

A)分层有向图中，起始的基元在多基元循环路径中对应的首条最短路径的起始层节点和尾条最短路径的结束层节点相同，形成一个循环；

B)分层有向图中，位于中间的基元在多基元循环路径中对应的中间最短路径的起始层节点和结束层节点互不相同，且和起始的基元在多基元循环路径中对应的首条最短路径的起始层节点也不同；即中间的基元的起始层节点和结束层节点没有重复节点。

C)搬运操作次数n小于单个基元中起始层/结束层的节点总数j₁；

根据以上约束，求得当前分层有向图和多基元循环路径集合下的多基元循环路径的总数为：

其中，n为分层有向图所含基元数目，j₁为单个基元中起始层/结束层的节点总数；

从个多基元循环路径中选取最短的多基元循环路径作为当前的搬运操作次数n下的最短循环路径/>

在不同的搬运操作次数n下获得各自的最短循环路径再求取不同的搬运操作次数n下最短循环路径/>的平均基元路径长度：

选取其中最短的平均基元路径长度对应的最短循环路径作为当前救援清障任务下的最优循环路径。

这样对于一个起始层节点为j、基元数目为n的情况，通过罗列所有个多基元循环路径，选取其中的最短的作为起始层节点为j、基元数目为n的最短循环路径/>重复上述步骤，遍历所有的j和n，分别获得不同起始层节点、不同基元数目的最短循环路径并组成集合，对集合内的元素分别求其平均基元路径长度，选取集合中最短的平均基元路径长度对应的多基元循环路径作为最优循环路径。

本发明通过上述处理，能够在多次搬运动作中寻找到全局最优，避免了每次搬运动作仅限于在起始位置点和目标位置点之间的局部最优的问题。

所述步骤3)中，将搬运操作执行总次数N除以最优循环路径对应的分层有向图中的基元数，获得余数，按照余数下的搬运操作次数通过搜索算法获得剩余最短路径。所述的剩余最短路径可以采用如步骤2)中的相同方式处理获得。

所述步骤3)中，根据搬运操作执行总次数N，以及步骤2)所求的最优循环路径的基元数目n，按照以下确定最优循环路径的重复数目为t：

其中，表示取整符号；

从而获得剩余最短路径的基元数目m：

m＝N mod n

其中，mod表示取余符号。

根据最优循环路径的循环特性，改变最优循环路径中基元的排列顺序并保证路径长度不变。对于剩余路径，其首尾层点是最优循环路径内部的任意基元的首层点，考虑所有情况利用搜素的算法求得m基元的剩余最短路径d_m。

所述步骤3)中，按照以下公式拼接获得救援清障任务下对应的全循环多层图的最终最短路径为：

其中，D为最终的最短路径，为最优循环路径长度，t为最优循环路径重复数目，d_m为最短末端路径。

本发明用于救援清障任务中用机械臂越过障碍物搬动物体。

本发明的有益效果是：

针对救援搬运任务，本方法可以迅速求得多次搬运任务的最短时间，和最优的搬运任务执行方法，进而提高救援机器人的搬运效率。保证机器人在救援清障场景中能够高效，安全，快速的完成任务。同时，相比于利用最短路径算法搜索大规模有向图求解，本方法能够显著降低优化过程的算法复杂度。能快速实现对任务的规划，达到最短的完成时间。

附图说明

图1是实施例构建的一个分层有向图基元示意图。

图2是实施例构建的一个分层有向图下的多基元循环路径后处理过程示意图。

具体实施方式

本发明结合具体实例和附图对本发明进行进一步说明。

按照本发明发明内容完整方法的具体实施例情况如下：

1)根据救援清障任务的需要，对障碍物在移动机器人机械臂空间坐标系下的位置进行标定。并根据障碍物数目，确定循环次数为N＝99。考虑空间障碍物的分布情况，确定抓取点A(-144，130，861)，避障点B(405，194，810)和放置点C(456，402，650)。单一循环任务机械臂末端通过各个坐标的顺序为A→B→C→A。

2)根据抓取点、避障点和放置点的空间坐标，求解获得每种位置点下的多种可能的机械臂关节配置，并按照三种位置点的通过顺序，计算顺序下相邻两个位置点的所有可能的关节配置之间的运行时间，构建分层有向图的基元。

根据确定的抓取点，避障点和放置点的坐标，对于一个三轴机械臂，其在配置空间中求取关节配置如表所示。

利用机械臂关节电机的梯形控制曲线，求得相邻两关节配置的运行时间，构建的分层有向图基元如图1所示。

3)对分层有向图的基元中进行最短运行时间的赋权路径搜索，求得起始层和结束层之间的不同节点间的最短路径，作为机械臂在起始位置点和结束位置点下的不同关节配置之间的最短路径，组成单次搬运操作下机械臂关节配置的最短路径集合。

基元中首末层节点数目为4个，分别计算求解两两首末层节点的最短路径。A₁→A₁，A₁→A₂…A₄→A₄.共计16个结果，将其组成集合D_s。

D_s＝{d_1,1,d_1,2,…,d_4,4}

D_s集合中的元素如表所示。

4)按照救援清障任务中的搬运操作执行总次数N对多次搬运操作的最短路径集合进行适配组合，获得多基元循环路径，选取最优循环路径。

具体是根据所求取的集合中的最短路径元素，根据首尾约束，进行组合拼接，组成多基元循环路径。

对于以节点A₁为首尾，基元数目n＝3的所有情况如图2所示。

其有种组合，通过求得其中的最短路径，可以得到以A₁为首尾3基元的最短循环路径/>

重复上述步骤，遍历所有j和n并求得相应的最短路径组成最短循环路径集合D_L。

分别对集合内的元素分别求其平均基元路径长度：

获得平均最短循环路径集合

选取集合中最短的平均基元路径长度对应的多基元循环路径作为最优循环路径。通过计算，本算例最优循环路径为A3→A2→A3，路径长度为78.26s，基元数目为n＝2。

5)根据任务执行数目为N＝99，最优循环路径基元数n＝2，可以求得最优循环路径重复数目为t＝49，末端基元数目为m＝1。

6)根据最优循环路径A3→A2→A3，改变最优循环路径中基元的排列顺序A2→A3→A2二者路径长度一致。

因此对于末端路径基元，其起始点可以是最短循环路径所含任意基元的首层点，考虑所有情况，根据末端路径基元数目m＝1，利用搜索算法，基元数目为1并可以和最短循环路径拼接的最短路径为A3→A3。路径长度为41.17s。最终，全循环多层图的最短路径长度为：

3875.91s为最短运行时间。

针对救援清障任务，具体实施求得了机器人最优的任务执行时间为3875.91s，具体的执行序列为重复执行A3→A2→A3路径49次，并执行A3→A3一次。针对对现场具体情况，本发明方案可以快速得到想要的执行方案，并且可以大大的提高救援清障搬运的执行效率。

Claims (7)

1.一种用于救援清障的移动机器人机械臂搬运最优时间规划方法，其特征在于包括以下步骤：

1)根据救援清障任务的需要，确定搬运操作执行总次数N，对障碍物在机械臂空间坐标系下的位置进行标定，确定抓取点、避障点和放置点的三种位置点的空间坐标以及三种位置点在救援清障任务中机械臂末端空间路径的通过顺序；

2)根据三种位置点的空间坐标以及三种位置点的通过顺序构建成分层有向图，通过分层有向图的处理获得最优循环路径；

3)根据救援清障任务中的搬运操作执行总次数N，确定最优循环路径的重复数目和剩余最短路径，将连续重复数目的多个最优循环路径与剩余最短路径进行拼接获得最终最短路径；

所述步骤1)具体为：

根据救援清障任务的需要，对障碍物在机械臂空间坐标系下的位置进行标定，且根据障碍物的分布情况确定机械臂的单一搬运操作的抓取点、避障点和放置点，以及抓取点、避障点和放置点在机械臂末端空间路径的通过顺序；根据需搬运物体的数量确定机械臂的搬运操作执行总次数N；

所述步骤2)具体为：

2.1)根据抓取点、避障点和放置点的空间坐标，求解获得每种位置点下的多种可能的机械臂关节配置，并按照三种位置点的通过顺序，计算顺序下相邻两个位置点的所有可能的关节配置之间的运行时间，以三种位置点的关节配置作为节点，一种位置点对应的所有关节配置作为层，关节配置之间的运行时间作为边，构建分层有向图的基元；

2.2)对分层有向图的基元中进行最短运行时间的赋权路径搜索，求得起始层和结束层之间的不同节点间的最短路径，组成单次搬运操作下机械臂关节配置的最短路径集合；

2.3)按照救援清障任务中的搬运操作执行总次数N对多次搬运操作的最短路径集合进行适配组合，获得多基元循环路径，选取最优循环路径。

2.根据权利要求1所述的一种用于救援清障的移动机器人机械臂搬运最优时间规划方法，其特征在于：所述步骤2.1)具体为：根据确定单次搬运操作中的抓取点、避障点和放置点的空间坐标，求取机械臂在每种位置点下的所有可能的关节配置，机械臂在一种位置点下的一个关节配置作为多层有向图的一层中的一个节点，机械臂在一种位置点下的所有多个关节配置组成了多层有向图的一层节点；将单次搬运操作中的三种位置点在多层有向图中对应的层按照所述通过顺序进行依次排序，且作为起始位置点和目标位置点的位置点在多层有向图中对应的层结构重复两次，分别设置在多层有向图中的起始层和结束层，不作为起始位置点和目标位置点的位置点在多层有向图中对应的层作为中间层；利用梯形控制曲线求得相邻层的每两个关节配置之间的运行时间作为两个节点之间的边，从而构成了代表单次搬运操作的分层有向图的基元。

3.根据权利要求1所述的一种用于救援清障的移动机器人机械臂搬运最优时间规划方法，其特征在于：所述步骤2.2)具体为：

在一个分层有向图的基元中，分别以起始层的一个节点到结束层的一个节点构建作为一对循环节点对，分别求得每对循环节点对的两个节点之间的最短运行时间的路径作为该对对循环节点对的最短路径；再由每对循环节点对下获得的最短路径组成最短路径集合。

4.根据权利要求1所述的一种用于救援清障的移动机器人机械臂搬运最优时间规划方法，其特征在于：所述步骤2.3)具体为：

设置搬运操作次数n，在救援清障任务中的搬运操作执行总次数N的约束下遍历所有可能的搬运操作次数n，搬运操作次数n大于等于1小于等于N；在每种搬运操作次数n下，进行以下操作：

按照n次搬运操作的顺序将n次搬运操作对应的分层有向图的基元进行依次拼接形成分层有向图，且按照n次搬运操作的顺序将n次搬运操作对应的最短路径集合中的各条最短路径进行依次拼接，一种拼接结果作为一条多基元循环路径，按照排列组合方式获得多种拼接结果，所有拼接结果的多基元循环路径组成了分层有向图对应的多基元循环路径集合；

且拼接过程中，每条多基元循环路径中符合以下约束：

A)分层有向图中，起始的基元在多基元循环路径中对应的首条最短路径的起始层节点和尾条最短路径的结束层节点相同，形成一个循环；

B)分层有向图中，位于中间的基元在多基元循环路径中对应的中间最短路径的起始层节点和结束层节点互不相同，且和起始的基元在多基元循环路径中对应的首条最短路径的起始层节点也不同；

C)搬运操作次数n小于单个基元中起始层/结束层的节点总数j₁；

根据以上约束，求得当前分层有向图和多基元循环路径集合下的多基元循环路径的总数为：

其中，n为分层有向图所含基元数目，j₁为单个基元中起始层/结束层的节点总数；

从个多基元循环路径中选取最短的多基元循环路径作为当前的搬运操作次数n下的最短循环路径/>

在不同的搬运操作次数n下获得各自的最短循环路径再求取不同的搬运操作次数n下最短循环路径/>的平均基元路径长度：

选取其中最短的平均基元路径长度对应的最短循环路径作为当前救援清障任务下的最优循环路径。

5.根据权利要求1所述的一种用于救援清障的移动机器人机械臂搬运最优时间规划方法，其特征在于：

所述步骤3)中，将搬运操作执行总次数N除以最优循环路径对应的分层有向图中的基元数，获得余数，按照余数下的搬运操作次数通过搜索算法获得剩余最短路径。

6.根据权利要求1所述的一种用于救援清障的移动机器人机械臂搬运最优时间规划方法，其特征在于：

所述步骤3)中，根据搬运操作执行总次数N，以及步骤2)所求的最优循环路径的基元数目n，按照以下确定最优循环路径的重复数目为t：

其中，表示取整符号；

从而获得剩余最短路径的基元数目m：

m＝N mod n

其中，mod表示取余符号。

7.根据权利要求1所述的一种用于救援清障的移动机器人机械臂搬运最优时间规划方法，其特征在于：

所述步骤3)中，按照以下公式拼接获得救援清障任务下对应的全循环多层图的最终最短路径为：

其中，D为最终的最短路径，为最优循环路径长度，t为最优循环路径重复数目，d_m为最短末端路径。