JP2667153B2 - Direct teaching method for multiple arm device - Google Patents

Description

BACKGROUND OF THE INVENTION 1. Field of the Invention The present invention relates to an industrial robot having a plurality of arms, in which when a plurality of arms holds one work, a plurality of arms are operated according to an external force acting on the work. The present invention relates to a method of determining a motion and moving a work, and more specifically, direct teaching of a multi-arm device for operating an arm by applying a force to a work in a direction in which the operator wants to move the work when teaching the operation of a plurality of arms. It is about the method. 2. Description of the Related Art Conventionally, there has been a master-slave type robot that performs a work by simultaneously operating two arms. In this method, when one arm called a master arm is moved, the other arm called a slave arm operates according to the operation of the master arm. FIG. 9 is an explanatory view of a case where a work is supported by a master-slave type multi-arm system. Slave arm
41 follows the operation of the master arm 40 and adjusts the force acting on the work 45 by correcting the tip position of the slave arm according to the force detected by the force sensor 44 attached to the base of the hand 41 of the slave arm. ing. Problems to be Solved by the Invention In order to operate a plurality of arms by the master-slave method, it is necessary to first operate and move the master arm. When direct teaching is performed with a work supported by a plurality of arms, a force is applied between the work and the hand of each arm, so the operator operates the master arm by directly applying force to the master arm. Then, there is a problem that it is difficult to distinguish between the force acting on the workpiece and the force by the operator. When teaching the work of positioning a work by operating two arms at the same time, it is easier and more convenient for the operator to operate each arm by directly operating the work itself than to apply force to the arms. Often good. Means for Solving Problems In order to solve the above problems, the first aspect of the present invention
The present invention is a method for directly teaching a multi-arm device that holds a work by hands at the ends of a plurality of arms, and includes a first step of applying an external force to the work, and a method provided for each hand. A second step of detecting an external force applied to each hand by the force sensor means and a force consisting of the work's own weight;
A third step of calculating the external force applied in the first step based on the force of each hand detected in the second step and a force given to the hand by the work's own weight, and a third step; It comprises a fourth step of issuing a command for each hand to move a predetermined distance in the direction of the applied external force. According to a second aspect of the present invention, there is provided a method for directly teaching a multi-arm device for holding a work by a hand at a tip of a plurality of arms, and a method for applying an external force including a force and a moment to the work. A first step, a second step of detecting an external force applied to each hand by the force sensor means provided on each hand, and a force and moment due to the work's own weight, and a force of each hand detected in the second step And a third step of obtaining the external force applied in the first step based on the force and the moment given by the moment and the work weight to each hand, and a predetermined distance of each hand in the direction of the external force obtained in the third step. The fourth step is to issue a command to move / rotate by an angle. Further, a third invention of the present invention is a method for directly teaching a multi-arm device for holding a work by a hand at the tip of a plurality of arms, wherein a method for applying an external force consisting of a force and a moment to the work is provided. 1 step, a second step of detecting an external force applied to each hand by the 6-axis force sensor means provided in each hand, a force and a moment due to the work weight, and each hand detected by the second step. A third step of obtaining the external force applied in the first step based on the force and moment of the workpiece and the force and moment given to each hand by the work's own weight, and each hand in the direction of the external force obtained in the third step. A fourth step of issuing a command to move / rotate by a predetermined distance / angle. Operation FIG. 1 is an explanatory diagram relating to the configuration of the present invention, and FIG. 2 is a flowchart showing processing when direct teaching of a multi-arm device is performed using the direct teaching method of a multi-arm device of the present invention. First, the forces detected by the respective force sensor means 1 and 2 while the work is supported by the plurality of arms 13 and 14 are used to calculate the force acting between the work and each hand by the detected force coordinate conversion means 3 and 4, The resultant force calculating means 7 determines the gravity acting on the work, that is, the work's own weight, and this is stored by the target resultant force setting means 9 as the target resultant force. Then, the following processing is repeated until the end of the direct teaching is supported. The force detected by the force sensor means 1 and 2 is converted into a force acting between the work and each hand by detected force coordinate conversion means 3 and 4, and the resultant force of the force acting on the work is calculated by the resultant force calculation means 7. calculate. Each of the arm control units 5 and 6 is determined by the detection force operation control unit 8 in accordance with the target net force stored first, that is, the force obtained by subtracting the work's own weight from the calculated net force.
Operation command. In direct teaching, an operation command is issued to move the hand position in the same direction as the force obtained by subtracting the target resultant force.However, if there are restrictions on the direction of operation, specify the operable direction and subtract the target resultant force. It is also possible to projectively transform the applied force in a designated direction, and issue an operation command to each of the arm control means 5 and 6 accordingly. By repeating the above processing, each arm operates according to the force of the operator on the work, and the operator can directly teach the operation of a plurality of arms by applying a force to the work. In the second invention, a moment is detected together with the force acting between the work and each hand using the force sensor means,
The detected force coordinate conversion means 3 and 4 convert the coordinates into a force / moment relating to one point on the work, and subtract the force / moment set by the target resultant force setting means 9 to obtain a force / moment.
Operate each arm according to the moment. To detect the moment and use this to determine the operation of each arm,
A plurality of arms can be operated in accordance with the rotational force on the work. FIG. 3 is an explanatory diagram relating to the configuration of the third invention of the present invention. Here, the six-axis force sensor means detects a force in three orthogonal directions and a moment around the three axes as shown in FIG. In the third invention, the weight and center of gravity of the work are stored by the work weight setting means 12, and the six-axis force sensor means 10, 11 attached to the hand base of each arm act between the work and each hand. The detected force and moment are detected, and the detected force coordinate conversion means 3 and 4 convert the coordinates into a force / moment related to one point on the workpiece.
The resultant of the forces and moments acting on the work is calculated by the resultant force calculating means 7, and the factor of the force and moment due to the work's own weight calculated from the weight of the work and the position of the center of gravity is subtracted from the resultant of the forces and moments acting on the work. The detection force operation control means 8 issues an operation command for each arm according to the obtained value. Memorize the weight and center of gravity of the work and subtract the force / moment due to the work's own weight from the resultant force of the work / moment so that the work can be taught directly even if the posture of the work changes without affecting the work's own weight. The movement and rotation of the work are determined according to the obtained values. Embodiment FIG. 5 is an explanatory diagram of a direct teaching method of a multi-arm device according to an embodiment of the present invention. Each of the two arms is a six-axis vertical articulated robot arm 16, 17, which can take the tip hand in any position and orientation within the allowable range of operation.
The position / orientation of the hand is represented by the following 4 × 4 matrix. Where N _i , O _i , and A _i are coordinate systems fixed to the hand of arm i
X-axis of H _i, Y-axis, a unit vector in the Z-axis positive direction, P _i is the coordinate system
Is the position vector of the origin of the H _i. 6-axis force sensor means 10 and 11 are attached to the hand base of each arm to detect the force acting between the workpiece and the hand by removing the force and moment due to the weight of the hand according to the posture of the hand. Can be. Sixth Embodiment
The axial force sensor means is a sensor in which a strain gauge is attached to an elastic body, and is a sensor for detecting a force in three orthogonal directions and a moment about the three axes from the output of the strain gauge as shown in FIG. is there. FIG. 6 is an explanatory diagram of the configuration of the control device 15 in the present embodiment. The control device 15 includes a plurality of microcomputer units 18, 19, 20 (hereinafter, referred to as MCUs) and motor control circuits 22, 23 for driving joints of the respective arms. Each M
The CUs are connected to a common bus 21 and can communicate data with each other via a 2-port RAM, and each MCU has a microcomputer, a memory for storing programs and data, and an I / O interface. The detection force coordinate conversion means 3 and 4 of the present invention are realized by programs of the MCUs 19 and 20, and the arm control means 5 and 6 are provided by the MCUs 19 and 20.
Is realized by a program and motor control circuits 22 and 23, and the resultant force calculation means 7, the detection force operation control means 8 and the target resultant force setting means 9 or the work weight setting means 12 are realized by a program of the MCU 18. The detection force coordinate conversion means 3 and 4 convert the force / moment in the sensor coordinate system detected by the 6-axis force sensor means 10 and 11 via the 6-axis force sensor I / O interfaces 24 and 25 into the current hand position. A factor due to gravity acting on the hand according to the posture is removed, and an appropriate coordinate transformation is performed to calculate a force / moment for one point on the work. When there are _two coordinate systems C ₁ and C ₂ , the conversion of the force / moment related to the origin from the coordinate system C ₁ to the coordinate system C ₂ is performed as follows. Coordinate system in the sedentary system C ₁
C ₂ of the X-axis Y-axis Z-axis positive direction of the unit vector respectively N, O,
In A, when the position vector of the origin of the coordinate system C ₂ is represented by P, the force vector F ₁ = (f _x1 , f _y1 , f _z1 ) ^T , the moment vector M ₁ = (m _x1 , m in the coordinate system C _1
y1 , m _z1 ) ^T is the force vector of the coordinate system C2 And moment vectors Is converted to As for the factor due to the gravity acting on the hand, the weight of the hand and the position of the center of gravity of the hand are stored in advance in the control device, and the gravity of the hand at the position of the center of gravity of the hand is converted into a sensor coordinate system according to the hand posture to convert the 6-axis force sensor. It is removed by subtracting from the detected value. In response to a command from the MCU 18, the arm control means 5 and 6 calculate the arm joint angle target value according to the target hand position so as to move the hand to the specified position, and the motor control circuit 22 according to the joint angle target value. , 23 are operated. The motors that drive the joints are controlled by the motor control circuits 22 and 23 so that the arms operate and the hand moves to the target hand position. The simultaneous operation of a plurality of arms is executed by the MCU 18 sending target hand position data of each arm to the arm control means 5 and 6 of the MCUs 19 and 20 corresponding to the respective arms and issuing a synchronized operation command. The two arms supporting the work can move the supported work by operating while maintaining the relative position between the hands. For this purpose, first, a coordinate system fixed to one point on the work is designated as a movement reference point. The movement route of the work is designated by the movement route of the movement reference point.
The movement reference point W is represented by a 4 × 4 matrix similarly to the hand position. Where N _W , O _W , and A _W are the X axis and Y of the moving reference point coordinate system, respectively.
A unit vector in the positive direction of the axis and the Z axis, and P _W is a position vector of the origin of the moving reference point coordinate system. In the operation of moving the work, the MCU 18 issues an operation command for each arm with a position having a certain relative position from the movement reference point as a target hand position. The relative positions R _i of the hand position H _i from the mobile reference point W, similar to the hand position represented by the 4 × 4 matrix, while supporting the workpiece ^{_{R i = W -1 · H i}} (i = 1 , 2). The target hand position H _i of each arm with respect to the target operation position W ′ of the movement reference point is calculated using R _i as H _i = W ′ · R _i (i = 1,2). Next, a case where direct teaching is performed using the third invention in this embodiment will be described. First, the work weight setting means 12
Thus, the work weight and the position of the center of gravity are stored and stored in the memory. The position of the center of gravity is stored in the moving reference point coordinate system. Then, the following processing is repeated until a signal for directly instructing the end of teaching is received from the operator. The force / moment detected by the six-axis force sensor means 10, 11 is detected. The force coordinate conversion means 3, 4 removes the factor of gravity acting on the hand and moves reference point coordinates acting between the work and the hand. It is converted to the force and moment of the system, and the sum is added to calculate the resultant force and moment acting on the work. Then, based on the weight of the work and the center of gravity of the work stored in advance, the gravity of the work, that is, the force acting on the center of gravity of the work by the weight of the work is converted into the force / moment of the moving reference point coordinate system and calculated. Subtract from the resultant force / moment. If the force / moment vectors thus obtained are F and M, the motion target position W ′ of the movement reference point is calculated, the target hand position of each arm is calculated, and the motion command is given as follows. Do. If the current position of the moving reference point is Then Where Rot (M ') is | M' around vector M '.
3 × 3 transformation matrix rotated by |, f (| F |), g (| M |)
Are functions as shown in FIG. 7, respectively, and are functions that determine the amount of movement and amount of rotation of the movement reference point with respect to the magnitude of the force and moment acting on the work. FIG. 8 shows a flowchart of the above processing. When teaching directly using the second invention of the present invention,
In the above embodiment, if the direction of gravity acting on the work is constant, the force / moment at the start of teaching can be regarded as a factor due to the gravity of the work. Therefore, this is stored as the target resultant force, and the detection force operation control means 8 is used. Subtract from the resultant.
In this case, the rotation operation is only rotation around the direction of gravity. Further, when teaching is directly performed using the first invention of the present invention, the processing relating to the moment and the rotation may be omitted in the above embodiment. As described above, in the present invention, the force sensor means is provided at the hand base of each of the plurality of arms, and the resultant force of the forces acting on the work is obtained by the detected force coordinate conversion means and the resultant force calculation means, and based on this. Since an operation command is issued to each arm by the detection force operation control means, the arms supported by the plurality of arms can be moved according to the force acting on the work, and the operator can directly move the plurality of arms by applying a force to the work. Teaching becomes possible. Further, in the second invention, a force sensor means for detecting a moment together with a force is provided, and a rotation operation can be performed by directly teaching the work. Further, in the third invention, the 6-axis force sensor means is provided on the hand base of each arm, and the work weight and the work center of gravity stored by the work weight setting means are used to detect the work weight by the work weight in accordance with the change in the posture of the work. Since the factors of force and moment are calculated and removed, direct teaching of the work including rotation can be performed regardless of the gravity of the work.

BRIEF DESCRIPTION OF THE DRAWINGS FIG. 1 is an explanatory diagram relating to the configuration of the first invention of the present invention, FIG. 2 is a processing flowchart of direct teaching, and FIG. 3 is an explanatory diagram relating to the configuration of the third invention of the present invention. FIG. 4 is an explanatory diagram of a six-axis force sensor means, FIG. 5 is an explanatory diagram of a multi-arm device according to an embodiment of the present invention, and FIG. 6 is an explanatory diagram of a control device according to an embodiment of the present invention. , FIG. 7 is an explanatory diagram of a function used in one embodiment of the present invention, FIGS. 8a and b are flowcharts of a process of a direct teaching method of a multi-arm device in one embodiment of the present invention, FIG. FIG. 4 is an explanatory diagram of a case where a master-slave method operates while supporting a work. 1,2 ... force sensor means, 3,4 ... detected force coordinate conversion means, 5,
6 ... arm control means, 7 ... resultant force calculation means, 8 ... detected force operation control means, 9 ... target resultant force setting means, 10, 11 ...
... 6 axis force sensor means, 12 ... Work weight setting means, 13,1
4 ... arm, 15 ... control device, 16,17 ... 6-axis vertical articulated robot arm. 18, 19, 20 ... microcomputer unit, 21 ... bus, 22, 23 ... motor control circuit, 24,
25 ... force sensor interface, 26,27,28,29,30,31,
32,33,34,35,36,37 ... motor, 38,39,42,43 ... hand, 40 ... master arm, 41 ... slave arm, 44
…… force sensor, 45 …… work.

Claims (1)

(57) [Claims] A method for directly teaching a multi-arm device for holding a work by hands at the ends of a plurality of arms, wherein a first step of applying an external force to the work and force sensor means provided for each hand A second step of detecting a force consisting of the external force applied to each hand and the work's own weight; a first step based on the force of each hand detected in the second step and the force given to the hand by the work's own weight. Direct operation of the multi-arm device including a third step of calculating the external force applied in the step and a fourth step of issuing a command to move each hand by a predetermined distance in the direction of the external force obtained in the third step. Teaching method. 2. A method for directly teaching a multi-arm device for holding a work by a hand at a tip of a plurality of arms, wherein a first force for applying an external force consisting of a force and a moment of the work is provided.
A second step of detecting the external force applied to each hand by the force sensor means provided on each hand and a force and moment due to the weight of the work, and a force and a moment of each hand detected in the second step. A third step of obtaining the external force applied in the first step based on the moment and the force and moment given to each hand by the work's own weight, and each hand moving a predetermined distance in the direction of the external force obtained in the third step. A direct teaching method for a multi-arm device comprising a fourth step of issuing a command to move / rotate by an angle. 3. A method for directly teaching a multi-arm device for holding a workpiece by a hand at a tip of a plurality of arms, wherein a first force for applying an external force including a force and a moment to the workpiece is provided.
A second step of detecting the external force applied to each hand by the six-axis force sensor means provided on each hand and a force and moment due to the weight of the work, and a step of detecting each hand detected in the second step. A third step of obtaining the external force applied in the first step based on the force and moment and the force and moment given to each hand by the work's own weight, and each hand is moved in the direction of the external force obtained in the third step. A direct display method for a multi-arm device comprising a fourth step of issuing a command to move / rotate by a predetermined distance / angle.