双机械臂协调运动规划方法、装置、电子设备及存储介质

技术领域

本公开属于双机械臂操作任务领域，具体涉及双机械臂协调运动规划方法、装置、电子设备及存储介质。

背景技术

双机械臂比单机械臂具有更大的作业空间和更灵巧的操作能力，因而得到了广泛的关注与研究。然而，双机械臂在操作任务过程中，需要满足关节运动限制、奇异位置、避免碰撞、任务约束等条件。然而，在上述条件限制下，双机械臂如何协调运动规划到合理的合作任务点，并且如何尽可能的按照期望轨迹进行运动规划仍然具有较大的难度。因此，有必要开发一种有效的双机械臂协调运动规划方法，实现复杂作业任务的灵巧、精准操作。

发明内容

本公开旨在解决上述问题之一。

为此，本公开第一方面实施例提供的基于主从方式，可提高双机械臂操作任务规划效率的双机械臂协调运动规划方法，包括：

设置双机械臂的工作空间、主臂的工作空间、副臂的工作空间和双机械臂的合作空间，其中，将机械臂沿着其期望任务轨迹进行运动规划时的作业空间定义为紧任务约束空间，以此设置主臂的紧任务约束空间和副臂的紧任务约束空间；将机械臂围绕其期望任务轨迹的邻域空间，实际位置与期望位置在容许误差范围内进行运动规划时的作业空间称为松任务约束空间，以此设置主臂的松任务约束空间和副臂的松任务约束空间；设置所述主臂的初始位置和所述副臂的初始位置，在所述双机械臂合作空间中随机选择一个任务点作为当前双机械臂期望合作任务点；

当前轮规划开始时刻，主臂和副臂分别根据基于松紧任务约束空间的主臂运动规划策略和副臂运动规划策略进行运动；若所述主臂和所述副臂均运动到所述当前双机械臂期望合作任务点，则规划成功；若所述主臂和所述副臂中存在至少一个没有运动到所述当前双机械臂期望合作任务点，则认为当前轮规划失败，依次使所述副臂和所述主臂回到各自的初始位置，并在所述当前双机械臂期望合作任务点的邻域空间内随机选取一个点作为当前双机械臂期望合作任务点，进行下一轮规划，直至失败次数达到设定的最大次数时，若所述主臂和所述副臂中仍存在至少一个没有运动到所述当前双机械臂期望合作任务点，则认为规划失败。

本公开第一方面实施例提供的双机械臂协调运动规划方法，具有以下特点及有益效果：

本公开方法采用主从方式的协调运动规划方法，提高了双机械臂操作任务的规划效率；基于松紧任务约束空间运动规划方式，能够使机械臂最大程度在紧任务约束空间下沿着期望轨迹进行运动规划，当出现关节运动限制、奇异位置、避免碰撞、任务约束等状况时，能够在松任务约束空间下进行运动规划，并且尽快返回紧任务约束空间下进行运动规划。

在一些实施例中，设置所述双机械臂的工作空间为W、所述主臂的工作空间为W

在一些实施例中，将所述主臂和所述副臂的期望任务轨迹分别均分成若干段，由每段的端点构成相应的期望轨迹点，主臂和副臂的最后一个期望轨迹点为当前双机械臂期望合作任务点；所述主臂运动规划策略和副臂运动规划策略均分别是对各自期望任务轨迹上的各期望轨迹点依次进行如下规划：

求解从所述当前期望轨迹点到下一个期望轨迹点的机械臂逆运动学，如果有解，则使机械臂在该机械臂的紧任务约束空间上从所述当前期望轨迹点运动到所述下一个期望轨迹点，然后进行所述下一个期望轨迹点的规划；如果无解，则在所述当前期望轨迹点的邻域空间内随机选取若干个近邻点，求取所述当前期望轨迹点到各所述近邻点中代价最小的近邻点作为最优近邻点，机械臂在该机械臂的松任务约束空间上从所述当前望轨迹点运动到所述最优近邻点，更新所述当前期望轨迹点为所述最优近邻点，若所述当前期望轨迹点到所述下一个期望轨迹点的距离小于设定的距离阈值，则进行下一个期望轨迹点的规划，若所述当前期望轨迹点到所述下一个期望轨迹点的距离大于或者等于设定的距离阈值，则从所述当前期望轨迹点到所述下一个期望轨迹点的路径上选取一点作为过渡点，机械臂在该机械臂的松任务约束空间上从所述当前期望轨迹点运动到所述过渡点，更新所述当前期望轨迹点为所述过渡点，然后进行下一个期望轨迹点的规划。

在一些实施例中，按照下式求取所述当前期望轨迹点到各所述近邻点的最小代价J

其中，

在一些实施例中，所述过渡点按照下式计算得到：

P

其中，P

本公开第二方面实施例提供的双机械臂协调运动规划装置，包括：

参数设定模块，用于设置双机械臂的工作空间、主臂的工作空间、副臂的工作空间和双机械臂的合作空间，其中，将机械臂沿着其期望任务轨迹进行运动规划时的作业空间定义为紧任务约束空间，以此设置主臂的紧任务约束空间和副臂的紧任务约束空间；将机械臂围绕其期望任务轨迹的邻域空间，实际位置与期望位置在容许误差范围内进行运动规划时的作业空间称为松任务约束空间，以此设置主臂的松任务约束空间和副臂的松任务约束空间；设置主臂的初始位置和副臂的初始位置，在所述双机械臂合作空间中随机选择一个任务点作为当前双机械臂期望合作任务点；和

规划模块，用于双机械臂的协调运动规划；对于每一轮规划，使所述主臂和所述副臂分别根据基于松紧任务约束空间的主臂运动规划策略和副臂运动规划策略进行运动；若所述主臂和所述副臂均运动到所述当前双机械臂期望合作任务点，则规划成功；若所述主臂和所述副臂中存在至少一个没有运动到所述当前双机械臂期望合作任务点，则认为当前轮规划失败，依次使所述副臂和所述主臂回到各自的初始位置，并在所述当前双机械臂期望合作任务点的邻域空间内随机选取一个点作为所述当前双机械臂期望合作任务点，进行下一轮规划，直至失败次数达到设定的最大次数时，若所述主臂和所述副臂中仍存在至少一个没有运动到所述当前双机械臂期望合作任务点，则认为规划失败。

本公开第三方面实施例提供的电子设备，包括：

至少一个处理器，以及，与所述至少一个处理器通信连接的存储器；

其中，所述存储器存储有可被所述至少一个处理器执行的指令，所述指令被设置为用于执行上述双机械臂协调运动规划方法。

本公开第四方面实施例提供的计算机可读存储介质，所述计算机可读存储介质存储计算机指令，所述计算机指令用于使所述计算机执行上述双机械臂协调运动规划方法。

附图说明

图1为本公开第一方面实施例提供的双机械臂协调运动规划方法的操作任务示意图。

图2为本公开实施例的双机械臂协调运动规划方法的流程图。

图3为本公开第一实施例的基于松紧任务约束空间的机械臂运动规划策略的流程图。

图4是本公开第二方面实施例提供的双机械臂协调运动规划装置的结构示意图。

图5为本公开第三方面实施例提供的电子设备的结构示意图。

具体实施方式

为了使本申请的目的、技术方案及优点更加清楚明白，以下结合附图及实施例，对本申请进行进一步详细描述。应当理解，此处所描述的具体实施例仅仅用于解释本申请，并不用于限定本申请。

相反，本申请涵盖任何由权利要求定义的在本申请精髓和范围上做的替代、修改、等效方法以及方案。进一步，为了使公众对本申请有更好的了解，在下文对本申请的细节描述中，详尽描述了一些特定的细节部分。对本领域技术人员来说没有这些细节部分的描述也可以完全理解本申请。

本公开第一方面实施例提供的双机械臂协调运动规划方法，其操作任务场景示意参见图1，双机械臂的其中一个机械臂作为主臂，另一个机械臂作为副臂，本公开第一方面实施例提供的双机械臂协调运动规划方法，包括：

设置双机械臂的工作空间、主臂的工作空间、副臂的工作空间和双机械臂的合作空间，其中，将机械臂沿着其期望任务轨迹进行运动规划时的作业空间定义为紧任务约束空间，以此设置主臂的紧任务约束空间和副臂的紧任务约束空间；将机械臂围绕其期望任务轨迹的邻域空间，实际位置与期望位置在容许误差范围内进行运动规划时的作业空间称为松任务约束空间，以此设置主臂的松任务约束空间和副臂的松任务约束空间；设置主臂的初始位置和副臂的初始位置，在双机械臂合作空间中随机选择一个任务点作为当前双机械臂期望合作任务点；

当前轮规划开始时刻，主臂和副臂分别根据基于松紧任务约束空间的主臂运动规划策略和副臂运动规划策略进行运动；若主臂和副臂均运动到当前双机械臂期望合作任务点，则规划成功；若主臂和副臂中存在至少一个没有运动到当前双机械臂期望合作任务点，则认为当前轮规划失败，依次使副臂和主臂回到各自的初始位置，并在当前双机械臂期望合作任务点的邻域空间内随机选取一个点作为当前双机械臂期望合作任务点，进行下一轮规划，直至失败次数达到设定的最大次数时，若所述主臂和所述副臂中仍存在至少一个没有运动到所述当前双机械臂期望合作任务点，则认为规划失败。

本公开第一方面实施例提供的方法，基于双机械臂协调运动规划到合作任务点，其中，主臂首先运动规划到合作任务点，到达目标后，副臂再运动规划到合作任务点。这种主从方式的协调运动规划方法可以有效避免双机械臂协调运动规划过程中的重复计算量。主臂运动规划时，可以把副臂当做静态障碍物，同理副臂运动规划时，可以把主臂当做静态障碍物，提高了运动规划效率；机械臂优先按照期望轨迹在紧任务约束空间下进行运动规划，保证路径最优，遇到障碍物、奇异位置等使其无法在紧任务约束空间下进行运动规划时，机械臂将会在松任务约束空间下进行运动规划，保证双机械臂操作任务继续工作，此时机械臂虽然不在期望轨迹上运动，但是能够使其在满足紧任务约束空间条件时重新返回到期望轨迹上进行运动规划。

在一些实施例中，参见图2，本公开第一方面实施例提供的双机械臂协调运动规划方法，具体包括以下步骤：

1)将双机械臂中的其中一个机械臂作为主臂M

2)判断fail_count是否小于N_fail，如果fail_count大于或者等于N_fail，则判定双机械臂协调运动规划失败，无法完成操作任务；如果fail_count小于N_fail，则执行步骤3)。

3)主臂M

4)使副臂M2回到副臂的初始状态

5)令fail_count＝fail_count+1，返回步骤2)。

在一些实施例中，将主臂和副臂的期望任务轨迹分别均分成若干段，由每段的端点构成相应的期望轨迹点，主臂和副臂的最后一个期望轨迹点为当前双机械臂期望合作任务点；基于松紧任务约束空间的主臂运动规划策略和副臂运动规划策略均分别是对各自期望任务轨迹上的各期望轨迹点依次进行如下规划：

求解从当前期望轨迹点到下一个期望轨迹点的机械臂逆运动学，如果有解，则使机械臂在该机械臂的紧任务约束空间上从当前期望轨迹点运动到下一个期望轨迹点，然后进行下一个期望轨迹点的规划；如果无解，则在当前期望轨迹点的邻域空间内随机选取若干个近邻点，求取当前期望轨迹点到各近邻点中代价最小的近邻点作为最优近邻点，机械臂在该机械臂的松任务约束空间上从当前望轨迹点运动到最优近邻点，更新当前期望轨迹点为最优近邻点，若当前期望轨迹点到下一个期望轨迹点的距离小于设定的距离阈值，则进行下一个期望轨迹点的规划，若当前期望轨迹点到下一个期望轨迹点的距离大于或者等于设定的距离阈值，则从当前期望轨迹点到下一个期望轨迹点的路径上选取一点作为过渡点，机械臂在该机械臂的松任务约束空间上从当前期望轨迹点运动到过渡点，更新当前期望轨迹点为过渡点，然后进行下一个期望轨迹点的规划。

在一些实施例中，参见图3，基于松紧任务约束空间的机械臂运动规划策略，均分别包括：

31)将机械臂的期望任务轨迹均分成N+1段，由每段的端点构成相应的期望轨迹点P

32)求解从期望轨迹点P

33)若k＝N-1，则输出规划失败，机械臂运动规划结束；否则，转到步骤34)。

34)在当前期望轨迹点P

35)从当前期望轨迹点P

在一些实施例中，按照下式计算当前期望轨迹点P

其中，

在一些实施例中，为了使机械臂的规划轨迹满足平滑稳定，按照下式计算过渡点P

P

其中，a为设定的常数，a∈(0，1)，当a越趋近于0时，选取的过渡点P

参见图4，本公开第二方面实施例提供的双机械臂协调运动规划装置，包括：

参数设定模块，用于设定双机械臂的作业空间、主臂的工作空间、副臂的工作空间和双机械臂的合作空间，其中，将机械臂沿着其期望任务轨迹进行运动规划时的作业空间定义为紧任务约束空间，以此设置主臂的紧任务约束空间和副臂的紧任务约束空间；将机械臂围绕其期望任务轨迹的邻域空间，实际位置与期望位置在容许误差范围内进行运动规划时的作业空间称为松任务约束空间，以此设置主臂的松任务约束空间和副臂的松任务约束空间；设置主臂的初始位置和副臂的初始位置，在双机械臂合作空间中随机选择一个任务点作为当前双机械臂期望合作任务点；和

规划模块，用于双机械臂的协调运动规划；对于每一轮规划，使主臂和副臂分别根据基于松紧任务约束空间的主臂运动规划策略和副臂运动规划策略进行运动；若主臂和副臂均运动到当前双机械臂期望合作任务点，则规划成功；若主臂和副臂中存在至少一个没有运动到当前双机械臂期望合作任务点，则认为当前轮规划失败，依次使副臂和主臂回到各自的初始位置，并在当前双机械臂期望合作任务点的邻域空间内随机选取一个点作为当前双机械臂期望合作任务点，进行下一轮规划，直至失败次数达到设定的最大次数时，若主臂和副臂中仍存在至少一个没有运动到当前双机械臂期望合作任务点，则认为规划失败。

为了实现上述实施例，本公开实施例还提出一种计算机可读存储介质，其上存储有计算机程序，该程序被处理器执行，用于执行上述实施例的双机械臂排爆操作控制方法。

下面参考图5，提出了适于用来实现本公开实施例的电子设备100的结构示意图。其中，需要说明的是，该电子设备100中包括双机械臂排爆操作控制系统，其中，本公开实施例中的电子设备可以包括但不限于诸如移动电话、笔记本电脑、数字广播接收器、PDA(个人数字助理)、PAD(平板电脑)、PMP(便携式多媒体播放器)、车载终端(例如车载导航终端)等等的移动终端以及诸如数字TV、台式计算机、服务器等等的固定终端。图5示出的电子设备仅仅是一个示例，不应对本公开实施例的功能和使用范围带来任何限制。

如图5所示，电子设备100可以包括处理装置(例如中央处理器、图形处理器等)101，其可以根据存储在只读存储器(ROM)102中的程序或者从存储装置108加载到随机访问存储器(RAM)103中的程序而执行各种适当的动作和处理。在RAM 103中，还存储有电子设备100操作所需的各种程序和数据。处理装置101、ROM 102以及RAM 103通过总线104彼此相连。输入/输出(I/O)接口105也连接至总线104。

通常，以下装置可以连接至I/O接口105：包括例如触摸屏、触摸板、键盘、鼠标、摄像头、麦克风等的输入装置106；包括例如液晶显示器(LCD)、扬声器、振动器等的输出装置107；包括例如磁带、硬盘等的存储装置108；以及通信装置109。通信装置109可以允许电子设备100与其他设备进行无线或有线通信以交换数据。虽然图5示出了具有各种装置的电子设备100，但是应理解的是，并不要求实施或具备所有示出的装置。可以替代地实施或具备更多或更少的装置。

特别地，根据本公开的实施例，上文参考流程图描述的过程可以被实现为计算机软件程序。例如，本实施例包括一种计算机程序产品，其包括承载在计算机可读介质上的计算机程序，该计算机程序包含用于执行流程图中所示方法的程序代码。在这样的实施例中，该计算机程序可以通过通信装置109从网络上被下载和安装，或者从存储装置108被安装，或者从ROM102被安装。在该计算机程序被处理装置101执行时，执行本公开实施例的方法中限定的上述功能。

需要说明的是，本公开上述的计算机可读介质可以是计算机可读信号介质或者计算机可读存储介质或者是上述两者的任意组合。计算机可读存储介质例如可以是——但不限于——电、磁、光、电磁、红外线、或半导体的系统、装置或器件，或者任意以上的组合。计算机可读存储介质的更具体的例子可以包括但不限于：具有一个或多个导线的电连接、便携式计算机磁盘、硬盘、随机访问存储器(RAM)、只读存储器(ROM)、可擦式可编程只读存储器(EPROM或闪存)、光纤、便携式紧凑磁盘只读存储器(CD-ROM)、光存储器件、磁存储器件、或者上述的任意合适的组合。在本公开中，计算机可读存储介质可以是任何包含或存储程序的有形介质，该程序可以被指令执行系统、装置或者器件使用或者与其结合使用。而在本公开中，计算机可读信号介质可以包括在基带中或者作为载波一部分传播的数据信号，其中承载了计算机可读的程序代码。这种传播的数据信号可以采用多种形式，包括但不限于电磁信号、光信号或上述的任意合适的组合。计算机可读信号介质还可以是计算机可读存储介质以外的任何计算机可读介质，该计算机可读信号介质可以发送、传播或者传输用于由指令执行系统、装置或者器件使用或者与其结合使用的程序。计算机可读介质上包含的程序代码可以用任何适当的介质传输，包括但不限于：电线、光缆、RF(射频)等等，或者上述的任意合适的组合。

上述计算机可读介质可以是上述电子设备中所包含的；也可以是单独存在，而未装配入该电子设备中。

上述计算机可读介质承载有一个或者多个程序，当上述一个或者多个程序被该电子设备执行时，使得该电子设备：

设置双机械臂的工作空间、主臂的工作空间、副臂的工作空间和双机械臂的合作空间，其中，将机械臂沿着其期望任务轨迹进行运动规划时的作业空间定义为紧任务约束空间，以此设置主臂的紧任务约束空间和副臂的紧任务约束空间；将机械臂围绕其期望任务轨迹的邻域空间，实际位置与期望位置在容许误差范围内进行运动规划时的作业空间称为松任务约束空间，以此设置主臂的松任务约束空间和副臂的松任务约束空间；设置所述主臂的初始位置和所述副臂的初始位置，在所述双机械臂合作空间中随机选择一个任务点作为当前双机械臂期望合作任务点；

当前轮规划开始时刻，主臂和副臂分别根据主臂运动规划策略和副臂运动规划策略进行运动；若所述主臂和所述副臂均运动到所述当前双机械臂期望合作任务点，则规划成功；若所述主臂和所述副臂中存在至少一个没有运动到所述当前双机械臂期望合作任务点，则认为当前轮规划失败，依次使所述副臂和所述主臂回到各自的初始位置，并在所述当前双机械臂期望合作任务点的邻域空间内随机选取一个点作为当前双机械臂期望合作任务点，进行下一轮规划，直至失败次数达到设定的最大次数时，若所述主臂和所述副臂中仍存在至少一个没有运动到所述当前双机械臂期望合作任务点，则认为规划失败。

可以以一种或多种程序设计语言或其组合来编写用于执行本公开的操作的计算机程序代码，上述程序设计语言包括面向对象的程序设计语言—诸如Java、Smalltalk、C++、python，还包括常规的过程式程序设计语言—诸如“C”语言或类似的程序设计语言。程序代码可以完全地在用户计算机上执行、部分地在用户计算机上执行、作为一个独立的软件包执行、部分在用户计算机上部分在远程计算机上执行、或者完全在远程计算机或服务器上执行。在涉及远程计算机的情形中，远程计算机可以通过任意种类的网络——包括局域网(LAN)或广域网(WAN)—连接到用户计算机，或者，可以连接到外部计算机(例如利用因特网服务提供商来通过因特网连接)。

在本说明书的描述中，参考术语“一个实施例”、“一些实施例”、“示例”、“具体示例”、或“一些示例”等的描述意指结合该实施例或示例描述的具体特征、结构、材料或者特点包含于本申请的至少一个实施例或示例中。在本说明书中，对上述术语的示意性表述不必须针对的是相同的实施例或示例。而且，描述的具体特征、结构、材料或者特点可以在任一个或多个实施例或示例中以合适的方式结合。此外，在不相互矛盾的情况下，本领域的技术人员可以将本说明书中描述的不同实施例或示例以及不同实施例或示例的特征进行结合和组合。

此外，术语“第一”、“第二”仅用于描述目的，而不能理解为指示或暗示相对重要性或者隐含指明所指示的技术特征的数量。由此，限定有“第一”、“第二”的特征可以明示或者隐含地包括至少一个该特征。在本申请的描述中，“多个”的含义是至少两个，例如两个，三个等，除非另有明确具体的限定。

流程图中或在此以其他方式描述的任何过程或方法描述可以被理解为，表示包括一个或更多个用于实现特定逻辑功能或过程的步骤的可执行指令的代码的模块、片段或部分，并且本申请的优选实施方式的范围包括另外的实现，其中可以不按所示出或讨论的顺序，包括根据所涉及的功能按基本同时的方式或按相反的顺序，来执行功能，这应被本申请的实施例所属技术领域的技术人员所理解。

在流程图中表示或在此以其他方式描述的逻辑和/或步骤，例如，可以被认为是用于实现逻辑功能的可执行指令的定序列表，可以具体实现在任何计算机可读介质中，以供指令执行系统、装置或设备(如基于计算机的系统、包括处理器的系统或其他可以从指令执行系统、装置或设备取指令并执行指令的系统)使用，或结合这些指令执行系统、装置或设备而使用。就本说明书而言，“计算机可读介质”可以是任何可以包含、存储、通信、传播或传输程序以供指令执行系统、装置或设备或结合这些指令执行系统、装置或设备而使用的装置。计算机可读介质的更具体的示例(非穷尽性列表)包括以下：具有一个或多个布线的电连接部(电子装置)，便携式计算机盘盒(磁装置)，随机存取存储器(RAM)，只读存储器(ROM)，可擦除可编辑只读存储器(EPROM或闪速存储器)，光纤装置，以及便携式光盘只读存储器(CDROM)。另外，计算机可读介质甚至可以是可在其上打印程序的纸或其他合适的介质，因为可以例如通过对纸或其他介质进行光学扫描，接着进行编辑、解译或必要时以其他合适方式进行处理来以电子方式获得程序，然后将其存储在计算机存储器中。

应当理解，本申请的各部分可以用硬件、软件、固件或它们的组合来实现。在上述实施方式中，多个步骤或方法可以用存储在存储器中且由合适的指令执行系统执行的软件或固件来实现。例如，如果用硬件来实现，和在另一实施方式中一样，可用本领域公知的下列技术中的任一项或他们的组合来实现：具有用于对数据信号实现逻辑功能的逻辑门电路的离散逻辑电路，具有合适的组合逻辑门电路的专用集成电路，可编程门阵列(PGA)，现场可编程门阵列(FPGA)等。

本技术领域的普通技术人员可以理解实现上述实施例方法携带的全部或部分步骤，可以通过程序来指令相关的硬件完成，所开发的程序可以存储于一种计算机可读存储介质中，该程序在执行时，包括方法实施例的步骤之一或其组合。

此外，在本申请各个实施例中的各功能单元可以集成在一个处理模块中，也可以是各个单元单独物理存在，也可以两个或两个以上单元集成在一个模块中。上述集成的模块既可以采用硬件的形式实现，也可以采用软件功能模块的形式实现。集成的模块如果以软件功能模块的形式实现并作为独立的产品销售或使用时，也可以存储在一个计算机可读取存储介质中。

上述提到的存储介质可以是只读存储器，磁盘或光盘等。尽管上面已经示出和描述了本申请的实施例，可以理解的是，上述实施例是示例性的，不能理解为对本申请的限制，本领域的普通技术人员在本申请的范围内可以对上述实施例进行变化、修改、替换和变型。