阅读说明：本技术 一种监督型主从机械臂协同控制方法 (Supervision type master-slave mechanical arm cooperative control method ) 是由 肖汉杰 于 2020-05-12 设计创作，主要内容包括：本发明公开了一种监督型主从机械臂协同控制方法,其中多机械臂系统包括主机械臂A、从机械臂B以及从机械臂C；具体的控制方法为：1)分别建立主机械臂A、从机械臂B以及从机械臂C环境下的坐标系统；2)建立主机械臂A与从机械臂B之间的监督映射关系,以及建立主机械臂A与从机械臂C之间的监督映射关系；3)建立从机械臂B与从机械臂C之间的协同映射关系；4)建立主机械臂A与从机械臂B、从机械臂C最终的协同运动关系；5)启动主机械臂A与从机械臂B、从机械臂C。本发明可对多个协同作业的机械臂进行协同控制,同时还可以实现主-从机械臂以及从-从机械臂之间的互相监督作用,从而可以保证机械臂之间不发生碰撞和乱步行为。(The invention discloses a supervision type master-slave mechanical arm cooperative control method, wherein a multi-mechanical arm system comprises a master mechanical arm A, a slave mechanical arm B and a slave mechanical arm C; the specific control method comprises the following steps: 1) respectively establishing coordinate systems of a master mechanical arm A, a slave mechanical arm B and a slave mechanical arm C in the environment; 2) establishing a supervision mapping relation between a master mechanical arm A and a slave mechanical arm B, and establishing a supervision mapping relation between the master mechanical arm A and a slave mechanical arm C; 3) establishing a cooperative mapping relation between the slave mechanical arm B and the slave mechanical arm C; 4) establishing a final cooperative motion relation between the master mechanical arm A and the slave mechanical arm B as well as the slave mechanical arm C; 5) starting the master arm A, the slave arm B and the slave arm C. The invention can carry out cooperative control on a plurality of mechanical arms which work cooperatively, and can realize the mutual supervision function between the master-slave mechanical arm and the slave-slave mechanical arm, thereby ensuring that the mechanical arms do not collide and do not disorderly.)

一种监督型主从机械臂协同控制方法

【

技术领域

】

本发明涉及一种监督型主从机械臂协同控制方法，属于机器人协同控制领域。

【

背景技术

】

机器人产业实际上自上世纪60年代就已经发展起来，在接下来半个多世纪发展迅速，先后出现了各种类型的机器人，包括工业机器人、特种机器人、服务机器人、医疗机器人，每种机器人又有非常庞杂的细分。总体上机器人可以分为两大部分，一部分是硬件结构方面，另一部分是软件控制方面，硬件结构有很多构型，包括串联型、并联型、软体类、仿生类，而软件控制方面其核心就是控制方法，如何保证机器人按照设计者的想法进行安全高效的运作，这是一项非常关键的技术考量因素。现有的大型生产车间，多以多个机器人之间的协同运动实现相应的功能，然而目前的多机器人协同控制方法存在两个弊端，一个是控制方法复杂，这导致机器人运行效率被大打折扣，另一个是多个机械人会发生无法预期的干涉和碰撞，这在机器人精密协同组装中更为明显(机器人彼此之间距离非常接近)。

【

发明内容

】

针对上述问题，本发明提供了一种监督型主从机械臂协同控制方法，具体方法步骤如下：

本发明多机械臂系统包括主机械臂A、从机械臂B以及从机械臂C；其中主机械臂A包括基臂、中间臂、末端臂以及末端执行器，其中主机械臂A的基臂与中间臂之间的夹角定义为θ_A1，中间臂与末端臂之间的夹角定义为θ_A2，；从机械臂B包括基臂、中间臂、末端臂以及末端执行器，其中从机械臂B的基臂与中间臂之间的夹角定义为θ_B1，中间臂与末端臂之间的夹角定义为θ_B2；从机械臂C包括基臂、中间臂、末端臂以及末端执行器，其中从机械臂C的基臂与中间臂之间的夹角定义为θ_C1，中间臂与末端臂之间的夹角定义为θ_C2；

进一步，所述方法具体包括以下步骤：

1)分别建立主机械臂A、从机械臂B以及从机械臂C环境下的坐标系统：

其中L₁₁、L₁₂、L₁₃表示主机械臂A的基臂、中间臂、末端臂的臂长，L₂₁、L₂₂、L₂₃表示从机械臂B的基臂、中间臂、末端臂的臂长，L₃₁、L₃₂、L₃₃表示从机械臂C的基臂、中间臂、末端臂的臂长；θ_A1表示主机械臂A的基臂与中间臂之间的夹角，θ_A2，表示中间臂与末端臂之间的夹角，θ_B1表示从机械臂B的基臂与中间臂之间的夹角，θ_B2表示中间臂与末端臂之间的夹角，θ_C1表示从机械臂C的基臂与中间臂之间的夹角，θ_C2表示中间臂与末端臂之间的夹角；

2)通过步骤1)建立的坐标系统，建立主机械臂A与从机械臂B之间的监督映射关系，以及建立主机械臂A与从机械臂C之间的监督映射关系：

其中α为主机械臂A对从机械臂B的监督映射系数，β为主机械臂A对从机械臂C的监督映射系数，k1/k3/k5/k7为关节臂长度映射调整系数，k2/k4/k6/k8为关节角大小映射调整系数；

3)建立从机械臂B与从机械臂C之间的协同映射关系：

G(θ_C1 θ_C2)＝ω1(θ_B1 θ_B2)·λ1(L₂₁ L₂₂ L₂₃)，其中从机械臂C服从协同从机械臂B，

G(θ_B1 θ_B2)＝ω1(B_C1 B_C2)·λ1(L₃₁ L₃₂ L₃₃)，其中从机械臂B服从协同从机械臂C；

其中ω1为关节角协同映射系数，λ1为关节臂长度协同映射系数；

4)建立主机械臂A与从机械臂B、从机械臂C最终的协同运动关系：

或

其中μ为调和参数；

5)启动主机械臂A与从机械臂B、从机械臂C，并通过上述步骤4)中公式达到机械臂之间互相监督和协同作用，以便保证不发生碰撞和乱步行为。

进一步，主机械臂A、从机械臂B以及从机械臂C采用以太网通信方式进行通信，并通过总控制装置4进行控制。

本发明具有如下有益技术效果：本发明控制方法可对多个协同作业的机械臂进行协同控制，同时还可以实现主-从机械臂以及从-从机械臂之间的互相监督作用，从而可以保证机械臂之间不发生碰撞和乱步行为。。

【

附图说明

】

图1为本发明手术机械臂系统示意性附图；

图2为本发明控制方法流程图；

【

具体实施方式

】

参照图1，本发明多机械臂系统包括主机械臂A(1)、从机械臂B(2)以及从机械臂C(3)，采用三个机械臂，当然也可以采用多个数量的机械臂，现以三个机械臂为例进行说明，其中一个时主机械臂，两个是从机械臂。

其中主机械臂A(1)包括基臂(11)、中间臂(12)、末端臂(13)以及末端执行器，其中主机械臂A(1)的基臂(11)与中间臂(12)之间的夹角定义为θ_A1，中间臂(12)与末端臂(13)之间的夹角定义为θ_A2，；从机械臂B(2)包括基臂(21)、中间臂(22)、末端臂(23)以及末端执行器，其中从机械臂B(2)的基臂(21)与中间臂(22)之间的夹角定义为θ_B1，中间臂(22)与末端臂(23)之间的夹角定义为θ_B2；从机械臂C(3)包括基臂(31)、中间臂(32)、末端臂(33)以及末端执行器，其中从机械臂C(3)的基臂(31)与中间臂(32)之间的夹角定义为θ_C1，中间臂(32)与末端臂(33)之间的夹角定义为θ_C2。

参照图2，本发明所述方法具体包括以下步骤：

1)分别建立主机械臂A(1)、从机械臂B(2)以及从机械臂C(3)环境下的坐标系统：

其中L₁₁、L₁₂、L₁₃表示主机械臂A(1)的基臂(11)、中间臂(12)、末端臂(13)的臂长，L₂₁、L₂₂、L₂₃表示从机械臂B(2)的基臂(21)、中间臂(22)、末端臂(23)的臂长，L₃₁、L₃₂、L₃₃表示从机械臂C(3)的基臂(31)、中间臂(32)、末端臂(33)的臂长；θ_A1表示主机械臂A(1)的基臂(11)与中间臂(12)之间的夹角，θ_A2表示中间臂(12)与末端臂(13)之间的夹角，θ_B1表示从机械臂B(2)的基臂(21)与中间臂(22)之间的夹角，θ_B2表示中间臂(22)与末端臂(23)之间的夹角，θ_C1表示从机械臂C(3)的基臂(31)与中间臂(32)之间的夹角，θ_C2表示中间臂(32)与末端臂(33)之间的夹角；

2)通过步骤1)建立的坐标系统，建立主机械臂A(1)与从机械臂B(2)之间的监督映射关系，以及建立主机械臂A(1)与从机械臂C(3)之间的监督映射关系：

其中α为主机械臂A(1)对从机械臂B(2)的监督映射系数，β为主机械臂A(1)

对从机械臂C(3)的监督映射系数，k1/k3/k5/k7为关节臂长度映射调整系数，k2/k4/k6/k8

为关节角大小映射调整系数；

3)建立从机械臂B(2)与从机械臂C(3)之间的协同映射关系：

G(θ_C1 θ_C2)＝ω1(θ_B1 θ_B2)·λ1(L₂₁ L₂₂ L₂₃)，其中从机械臂C(3)服从协同从机械臂B

(2)，或

G(θ_B1 θ_B2)＝ω1(B_C1 B_C2)·λ1(L₃₁ L₃₂ L₃₃)，其中从机械臂B(2)服从协同从机械臂C

(3)；

其中ω1为关节角协同映射系数，λ1为关节臂长度协同映射系数；

4)建立主机械臂A(1)与从机械臂B(2)、从机械臂C(3)最终的协同运动关系：

或

其中μ为调和参数；

5)启动主机械臂A(1)与从机械臂B(2)、从机械臂C(3)，并通过上述步骤4)中公式达到机械臂之间互相监督和协同作用，以便保证不发生碰撞和乱步行为。

本发明主机械臂A(1)、从机械臂B(2)以及从机械臂C(3)采用以太网通信方式进行通信，并通过总控制装置4进行控制。

本发明控制方法可对多个协同作业的机械臂进行协同控制，同时还可以实现主-从机械臂以及从-从机械臂之间的互相监督作用，从而可以保证机械臂之间不发生碰撞和乱步行为。。

最后需要说明，凡是不脱离本发明核心技术构思的发明创造，均应当认为包含在本发明保护围内。