阅读说明：本技术 一种监督型主从机械臂协同控制方法 (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.)

1. A supervision type master-slave mechanical arm cooperative control method comprises a master mechanical arm A (1), a slave mechanical arm B (2) and a slave mechanical arm C (3);

wherein the main mechanical arm A (1) comprises a base arm (11), a middle arm (12), an end arm (13) and an end actuator, wherein an included angle between the base arm (11) and the middle arm (12) of the main mechanical arm A (1) is defined as theta_A1The angle between the intermediate arm (12) and the end arm (13) is defined as theta_A2，；

The slave arm B (2) comprises a base arm (21), a middle arm (22), an end arm (23) and an end effector, wherein the included angle between the base arm (21) and the middle arm (22) of the slave arm B (2) is defined as theta_B1The angle between the intermediate arm (22) and the end arm (23) is defined as theta_B2；

The slave mechanical arm C (3) comprises a base arm (31), a middle arm (32), an end arm (33) and an end effector, wherein the included angle between the base arm (31) and the middle arm (32) of the slave mechanical arm C (3) is defined as theta_C1The angle between the intermediate arm (32) and the end arm (33) is defined as theta_C2。

The method is characterized in that: the method specifically comprises the following steps:

1) respectively establishing coordinate systems of a master mechanical arm A (1), a slave mechanical arm B (2) and a slave mechanical arm C (3) under the environments:

wherein L is₁₁、L₁₂、L₁₃The arm lengths L of the base arm (11), the intermediate arm (12) and the end arm (13) of the master arm A (1)₂₁、L₂₂、L₂₃L represents the arm length of the base arm (21), the intermediate arm (22), and the end arm (23) of the robot arm B (2)₃₁、L₃₂、L₃₃Represents the arm length of a base arm (31), an intermediate arm (32), and a tip arm (33) of a slave arm C (3); theta_A1Represents an angle theta between a base arm (11) and an intermediate arm (12) of a master arm A (1)_A2，Represents the angle between the intermediate arm (12) and the end arm (13), theta_B1Represents an angle theta between the base arm (21) and the intermediate arm (22) of the robot arm B (2)_B2Represents the angle between the intermediate arm (22) and the end arm (23), theta_C1Represents an angle theta between the base arm 31 and the intermediate arm 32 of the robot arm C3_C2Represents the angle between the middle arm (32) and the end arm (33);

2) establishing a supervision mapping relation between a master mechanical arm A (1) and a slave mechanical arm B (2) and establishing a supervision mapping relation between the master mechanical arm A (1) and a slave mechanical arm C (3) through the coordinate system established in the step 1):

wherein alpha is a supervision mapping coefficient of the master mechanical arm A (1) to the slave mechanical arm B (2), beta is a supervision mapping coefficient of the master mechanical arm A (1) to the slave mechanical arm C (3), k1/k3/k5/k7 is a joint arm length mapping adjustment coefficient, and k2/k4/k6/k8 is a joint angle size mapping adjustment coefficient;

3) establishing a cooperative mapping relation between the slave mechanical arm B (2) and the slave mechanical arm C (3):

G(θ_C1 θ_C2)＝ω1(θ_B1 θ_B2)·λ1(L₂₁ L₂₂ L₂₃) Wherein the slave arm C (3) is subject to cooperation with the slave arm B (2), or

G(θ_B1 θ_B2)＝ω1(B_C1 B_C2)·λ1(L₃₁ L₃₂ L₃₃) Wherein slave arm B (2) is subject to cooperation from arm C (3);

wherein omega 1 is a joint angle cooperative mapping coefficient, and lambda 1 is a joint arm length cooperative mapping coefficient;

4) establishing the final cooperative motion relationship of the master mechanical arm A (1), the slave mechanical arm B (2) and the slave mechanical arm C (3):

or

Wherein mu is a harmonic parameter;

5) and (3) starting the master mechanical arm A (1), the slave mechanical arm B (2) and the slave mechanical arm C (3), and achieving mutual supervision and synergistic action among the mechanical arms through the formula in the step 4) so as to ensure that collision and desynchrony behaviors are not generated.

2. A supervised master-slave manipulator cooperative control method as recited in claim 1, wherein the master manipulator a (1), the slave manipulator B (2) and the slave manipulator C (3) communicate in the ethernet communication manner and are controlled by a master control device (4).

[ technical field ] A method for producing a semiconductor device

The invention relates to a supervision type master-slave mechanical arm cooperative control method, and belongs to the field of robot cooperative control.

[ background of the invention ]

The robot industry has actually developed since the last 60 s, and in the next more than half century, various types of robots, including industrial robots, special robots, service robots, and medical robots, have appeared in succession, and each robot has a very numerous and complicated subdivision. Generally, the robot can be divided into two parts, one part is a hardware structure aspect, the other part is a software control aspect, the hardware structure has a plurality of configurations including a series type, a parallel type, a software type and a bionic type, and the core of the software control aspect is a control method, so that how to ensure the robot to safely and efficiently operate according to the idea of a designer is a very key technical consideration factor. In the existing large production workshop, the corresponding functions are realized by the cooperative motion among a plurality of robots, however, the existing multi-robot cooperative control method has two disadvantages, one is that the control method is complex, which results in great reduction of the operation efficiency of the robots, and the other is that a plurality of robots can generate unexpected interference and collision, which is more obvious in the precise cooperative assembly of the robots (the robots are very close to each other in distance).

[ summary of the invention ]

Aiming at the problems, the invention provides a supervision type master-slave mechanical arm cooperative control method, which comprises the following specific steps:

the multi-mechanical arm system comprises a master mechanical arm A, a slave mechanical arm B and a slave mechanical arm C; wherein the main mechanical arm A comprises a base arm, a middle arm, an end arm and an end executor, wherein the included angle between the base arm and the middle arm of the main mechanical arm A is defined as theta_A1The angle between the middle arm and the end arm is defined as theta_A2B, carrying out the following steps of; the slave mechanical arm B comprises a base arm, a middle arm, an end arm and an end effector, wherein the included angle between the base arm and the middle arm of the slave mechanical arm B is defined as theta_B1The angle between the middle arm and the end arm is defined as theta_B2(ii) a The slave mechanical arm C comprises a base arm, a middle arm, an end arm and an end effector, wherein the included angle between the base arm and the middle arm of the slave mechanical arm C is defined as theta_C1The angle between the middle arm and the end arm is defined as theta_C2；

Further, the method specifically 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 under the environment:

wherein L is₁₁、L₁₂、L₁₃The arm lengths L of the base arm, the intermediate arm and the end arm of the master arm A₂₁、L₂₂、L₂₃The arm lengths L of the base arm, the intermediate arm and the end arm of the slave arm B₃₁、L₃₂、L₃₃Indicates the arm lengths of the base arm, the intermediate arm, and the end arm of the slave arm C; theta_A1Represents an angle between the base arm and the intermediate arm of the main robot arm A, theta_A2Denotes the angle between the middle arm and the end arm, theta_B1Represents an angle between the base arm and the intermediate arm of the slave arm B, theta_B2Denotes the angle between the middle arm and the end arm, theta_C1Represents an angle between the base arm and the intermediate arm of the slave arm C, theta_C2Representing the angle between the middle arm and the end arm;

2) establishing a supervision mapping relation between the master mechanical arm A and the slave mechanical arm B and establishing a supervision mapping relation between the master mechanical arm A and the slave mechanical arm C through the coordinate system established in the step 1):

wherein alpha is a supervision mapping coefficient of the master mechanical arm A to the slave mechanical arm B, beta is a supervision mapping coefficient of the master mechanical arm A to the slave mechanical arm C, k1/k3/k5/k7 is a joint arm length mapping adjustment coefficient, and k2/k4/k6/k8 is a joint angle size mapping adjustment coefficient;

3) establishing a cooperative mapping relation between the slave mechanical arm B and the slave mechanical arm C:

G(θ_C1 θ_C2)＝ω1(θ_B1 θ_B2)·λ1(L₂₁ L₂₂ L₂₃) Wherein the slave arm C is compliant with the slave arm B,

G(θ_B1 θ_B2)＝ω1(B_C1 B_C2)·λ1(L₃₁ L₃₂ L₃₃) Wherein slave arm B is compliant with slave arm C;

wherein omega 1 is a joint angle cooperative mapping coefficient, and lambda 1 is a joint arm length cooperative mapping coefficient;

4) establishing the final cooperative motion relationship of the master mechanical arm A, the slave mechanical arm B and the slave mechanical arm C:

or

Wherein mu is a harmonic parameter;

5) and starting the master mechanical arm A, the slave mechanical arm B and the slave mechanical arm C, and achieving mutual supervision and synergistic action among the mechanical arms through the formula in the step 4) so as to ensure that collision and disordering behaviors do not occur.

Further, the master arm a, the slave arm B, and the slave arm C communicate with each other in an ethernet communication manner, and are controlled by the master control device 4.

The invention has the following beneficial technical effects: the control method can carry out cooperative control on a plurality of mechanical arms which are operated 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 have disorder behavior. .

[ description of the drawings ]

FIG. 1 is a schematic illustration of a surgical robotic arm system of the present invention;

FIG. 2 is a flow chart of a control method of the present invention;

[ detailed description ] embodiments

Referring to fig. 1, the multi-robot system of the present invention includes a master robot a (1), a slave robot B (2), and a slave robot C (3), and three robots are used, but a plurality of robots may be used, and three robots are described as an example, where one robot is the master robot and two robots are the slave robots.

Wherein the main mechanical arm A (1) comprises a base arm (11), a middle arm (12), an end arm (13) and an end actuator, wherein an included angle between the base arm (11) and the middle arm (12) of the main mechanical arm A (1) is defined as theta_A1The angle between the intermediate arm (12) and the end arm (13) is defined as theta_A2B, carrying out the following steps of; the slave arm B (2) comprises a base arm (21), a middle arm (22), an end arm (23) and an end effector, wherein the included angle between the base arm (21) and the middle arm (22) of the slave arm B (2) is defined as theta_B1The angle between the intermediate arm (22) and the end arm (23) is defined as theta_B2(ii) a The slave mechanical arm C (3) comprises a base arm (31), a middle arm (32), an end arm (33) and an end effector, wherein the included angle between the base arm (31) and the middle arm (32) of the slave mechanical arm C (3) is defined as theta_C1The angle between the intermediate arm (32) and the end arm (33) is defined as theta_C2。

Referring to fig. 2, the method of the present invention specifically includes the following steps:

1) respectively establishing coordinate systems of a master mechanical arm A (1), a slave mechanical arm B (2) and a slave mechanical arm C (3) under the environments:

wherein L is₁₁、L₁₂、L₁₃The arm lengths L of the base arm (11), the intermediate arm (12) and the end arm (13) of the master arm A (1)₂₁、L₂₂、L₂₃L represents the arm length of the base arm (21), the intermediate arm (22), and the end arm (23) of the robot arm B (2)₃₁、L₃₂、L₃₃Represents the arm length of a base arm (31), an intermediate arm (32), and a tip arm (33) of a slave arm C (3); theta_A1Represents an angle theta between a base arm (11) and an intermediate arm (12) of a master arm A (1)_A2Represents the angle between the intermediate arm (12) and the end arm (13), theta_B1Represents an angle theta between the base arm (21) and the intermediate arm (22) of the robot arm B (2)_B2Represents the angle between the intermediate arm (22) and the end arm (23), theta_C1Represents an angle theta between the base arm 31 and the intermediate arm 32 of the robot arm C3_C2Represents the angle between the middle arm (32) and the end arm (33);

2) establishing a supervision mapping relation between a master mechanical arm A (1) and a slave mechanical arm B (2) and establishing a supervision mapping relation between the master mechanical arm A (1) and a slave mechanical arm C (3) through the coordinate system established in the step 1):

wherein alpha is the supervision mapping coefficient of the master mechanical arm A (1) to the slave mechanical arm B (2), and beta is the master mechanical arm A (1)

For the supervision mapping coefficient of the slave mechanical arm C (3), k1/k3/k5/k7 is the length mapping adjustment coefficient of the articulated arm, and k2/k4/k6/k8

Mapping adjustment coefficients for joint angle sizes;

3) establishing a cooperative mapping relation between the slave mechanical arm B (2) and the slave mechanical arm C (3):

G(θ_C1 θ_C2)＝ω1(θ_B1 θ_B2)·λ1(L₂₁ L₂₂ L₂₃) Wherein the slave arm C (3) is compliant with the slave arm B

(2) Or is or

G(θ_B1 θ_B2)＝ω1(B_C1 B_C2)·λ1(L₃₁ L₃₂ L₃₃) Wherein slave arm B (2) is compliant with slave arm C

(3)；

Wherein omega 1 is a joint angle cooperative mapping coefficient, and lambda 1 is a joint arm length cooperative mapping coefficient;

4) establishing the final cooperative motion relationship of the master mechanical arm A (1), the slave mechanical arm B (2) and the slave mechanical arm C (3):

or

Wherein mu is a harmonic parameter;

5) and (3) starting the master mechanical arm A (1), the slave mechanical arm B (2) and the slave mechanical arm C (3), and achieving mutual supervision and synergistic action among the mechanical arms through the formula in the step 4) so as to ensure that collision and desynchrony behaviors are not generated.

The master arm A (1), the slave arm B (2) and the slave arm C (3) communicate in an Ethernet communication mode and are controlled by a master control device 4.

The control method can carry out cooperative control on a plurality of mechanical arms which are operated 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 have disorder behavior. .

Finally, it should be noted that any invention not departing from the core technical idea of the invention should be considered as included in the protection enclosure of the invention.