JPH0550386A - Position instruction system for manipulator with seven degrees of freedom - Google Patents

Abstract

PURPOSE:To easily instruct positions and attitude to a manipulator with seven degrees of freedom. CONSTITUTION:A manipulator has seven joints. The shoulder part 11 of the manipulator is provided with three joints J1, J2, and J3 whose rotary axes cross at one point. An elbow part 13, which joints the upper arm part 12 and front arm part 14 connected to the shoulder part 11, is provided with a joint 4. A wrist part is provided with three joints J5, J6, and J7 whose rotary axes cross at one point. When a linear motion command to move the hand in parallel with each axis of a rectangular coordinate system or a rotation command to each axis is input from an instruction operation panel 3, the position and attitude of the handle are changed without changing the rotation angle of the elbow part 13(Os-Ow). When a command to rotate the elbow is input, only the rotation angle of the elbow part without changing the position and attitude of the hand (self motion). By these commands, positions and attitude are instructed to the manipulator.

Description

Detailed Description of the Invention

[0001]

BACKGROUND OF THE INVENTION 1. Field of the Invention The present invention relates to a position teaching system for an articulated manipulator, and more particularly to a position teaching system for a manipulator having 7 degrees of freedom that performs movements equivalent to those of a human arm.

[0002]

2. Description of the Related Art To position an industrial manipulator or a hand (end effector) attached to the tip of an arm of a robot at an arbitrary position in space within an operating range of the manipulator (robot) in an arbitrary direction. It is a necessary and sufficient condition that the arm mechanism of the manipulator (robot) has 6 degrees of freedom. Therefore, existing manipulators and industrial robots are configured with a mechanism having 6 degrees of freedom or less than 6 degrees of freedom with limited movement. However, in a 6-DOF manipulator or robot, when the hand is positioned at a position and posture in the space, the posture of the arm mechanism is limited to a finite number of postures, which may cause interference with an obstacle.

Therefore, a manipulator and a robot having seven degrees of freedom have been developed in order to solve the problems of the six-degree-of-freedom manipulator and the robot (Japanese Patent Publication No. 55-55).
48956). In particular, a teaching-playback type 7-DOF manipulator or robot that performs an operation similar to that of a human arm has been developed by the present applicant (see Japanese Patent Application No. 3-84167). This 7-DOF manipulator or robot has 3 joints in the part corresponding to the human shoulder and 3 joints in the part corresponding to the wrist.
Two joints are provided, and the joint between the shoulder and the wrist is connected via the joint corresponding to the elbow to construct a manipulator or robot with 7 degrees of freedom that performs the same movement as a human arm. In the manipulator or robot having such 7 degrees of freedom, in addition to the 6 degrees of freedom necessary for determining the position and orientation, 1
There is one redundancy, and this one redundancy can realize complicated motions and dexterous motions.

[0006] For example, to explain using a human arm, the human arm also has 7 degrees of freedom, and has 6 redundant degrees of freedom required for determining the position and posture of the hand, and 1 redundant degree of freedom. .. Because of this redundancy, it is possible for a human arm to grasp an object in front and move the elbow while fixing the position and posture of the hand. By positively utilizing the movement of the elbow, it is possible to perform an operation of gripping an object while avoiding an obstacle. As described above, the manipulator or the robot having the redundant degree of freedom like the human arm can move the arm without changing the position and posture of the hand. This movement of the arm is called "self-motion". In the example of the human arm described above, the movement of moving the position of the elbow without moving the position and posture of the hand is self-motion.

On the other hand, as a method of programming the operation of the manipulator or the robot, the teaching / playback method is generally adopted and familiar. In the manipulator or robot of the teaching / playback method, in order for the operator to position the hand of the manipulator or the robot at a predetermined position and posture, the operator usually operates the teaching operation panel to move the position and posture of the hand. 1)
The functions (2) and (2) are prepared. Then, the operator uses these functions to position the hand of the manipulator or the robot at a predetermined position and posture, thereby teaching. (1) Each axis operation A method of individually moving each joint of the manipulator or robot to position the hand in a predetermined position and posture. The teaching operation panel is provided with keys corresponding to the movements of each joint, and when a specific key is pressed, the axis of the corresponding joint moves. (2) Operation of Cartesian coordinate system For a specific Cartesian coordinate system in space, X of this coordinate system
It has the function of moving the hand parallel to each axis of the Y-axis, Z-axis, and the function of rotating the hand around each coordinate axis of the X-axis, Y-axis, and Z-axis. A method of changing the posture of the hand by rotational movement to position the hand at a predetermined position and posture.

The teaching operation panel is provided with a key for instructing parallel movement along a coordinate axis or rotational movement around the axis, corresponding to each axis. By pressing a predetermined key, the corresponding parallel movement or rotational movement is performed. Do.

[0007]

In each axis movement in teaching, when one joint of the manipulator or robot is moved, both the position and posture of the hand change. Therefore, in order to move the hand to the position and posture desired by the operator, it is necessary to move a plurality of joints at the same time. However, the relationship between the movement of the position and posture of the hand and the movement of each joint is generally complicated, and the operator must be intuitive Is difficult to understand. As the number of joints increases, the relationship between the two becomes more complicated, and in particular, it is very difficult to teach a 7-degree-of-freedom manipulator or a robot having one redundant degree of freedom in each axis operation.

On the other hand, in the case of teaching in the operation of the Cartesian coordinate system, the degree of freedom of the position and orientation of the hand is 6, whereas the manipulator or robot of 7 degrees of freedom has a redundancy of 1 degree of freedom. It is impossible to operate the manipulator or the robot only by instructing the position and posture of the robot. This is because even if the position and posture of the hand are designated, the degree of freedom of "self motion" described above still remains. In order to operate a manipulator or robot with 7 degrees of freedom in a Cartesian coordinate system, it is necessary to specify self-motion in addition to the position and orientation of the hand, but with the conventional teaching method, a manipulator or robot with 6 degrees of freedom or less must be specified. Since it is a target, only the position and orientation of the hand can be specified, and it cannot be applied to a manipulator or robot having 7 degrees of freedom. Therefore, an object of the present invention is to provide a teaching method that can easily teach the position and posture of a hand and the posture of an arm to a manipulator or a robot having 7 degrees of freedom.

[0009]

SUMMARY OF THE INVENTION According to the present invention, a shoulder portion having three joints and a rotation axis of each joint intersects at one point in space,
The elbow portion is composed of one joint connected to the upper arm portion connected to the shoulder portion and connecting the forearm portion, and three joints connected to the other end of the forearm portion, and the rotation axes of the three joints are one. In a position teaching method in a 7-degree-of-freedom manipulator configured with a wrist portion intersecting at a point, a linear movement command means for moving an end effector of the manipulator parallel to each axis of a Cartesian coordinate system, and rotation around each axis. Providing rotation command means and elbow rotation command means for rotating the elbow around a straight line connecting the shoulder and wrist without changing the position and posture of the end effector,
When the linear movement command means or the rotation command means is operated, the position and posture of the end effector is changed without changing the elbow rotation angle around the straight line connecting the shoulder and the wrist, and when the elbow rotation command means is operated. By teaching only the position and posture of the manipulator by changing only the elbow rotation angle without changing the position and posture of the end effector, the 7-DOF manipulator can be easily taught.

[0010]

When the linear movement command means or the rotation command means is operated, the end effector can be moved to an arbitrary position without changing the elbow rotation angle around the straight line connecting the shoulder and the wrist, and the posture of the end effector can be set arbitrarily. Change to
Teaches the position and orientation of the end effector. When the elbow rotation command means is operated, the position and orientation of the end effector is fixed and the elbow rotation angle alone is changed (self-motion) to avoid grasping an object while avoiding obstacles. The manipulator is instructed to perform such an operation.

[0011]

DESCRIPTION OF THE PREFERRED EMBODIMENTS FIG. 1 is a block diagram of a 7-DOF manipulator according to an embodiment of the present invention. In the figure, reference numeral 1 is an arm mechanism portion of a manipulator supported by a support B, and 2 is a manipulator control portion for driving and controlling the manipulator, which is an actuator such as a processor, a memory, a servomotor for driving axes of respective joints (see FIG. It is a conventionally known one having an axis control circuit (not shown) and the like. The teaching operation panel 3 moves the manipulator to position it at a predetermined position and posture, and teaches the position and posture.

The arm mechanism portion 1 is composed of a shoulder portion 11, an upper arm 12, an elbow portion 13, a forearm 14, and a wrist portion 15. The shoulder portion 11 is rotatable about a rotation axis a1 directly supported by the support portion B, and is connected to the shoulder first joint J1 via a crank-shaped link with the first joint J1 so as to rotate orthogonal to the rotation axis a1. A shoulder second joint J2 rotatable about the axis a2 and a shoulder second joint J2 connected to the shoulder second joint by a link 11b on the rotation axis a2 and rotatable about a rotation axis a3 orthogonal to the rotation axis a2. 3 joints J3, and the operation axes a1, a2, a3 of the three shoulder joints intersect at a point Os of the shoulder portion 11 at one point. One end of the link forming the upper arm 12 is connected to the shoulder third joint J3 and is connected to the rotation axis a.
It is mounted rotatably around 3. An elbow portion 13 is attached to the other end of the upper arm 12, and the elbow portion 13 is composed of one joint J4 rotatable about a rotation axis a4. A forearm 14 is attached to the elbow portion 13 so as to be rotatable around the rotation axis a4. The axis of the upper arm 12 and the axis of the forearm 14 intersect at one point Oe on the rotation axis a4.

The wrist portion 15 has first, second and third joints J5.
It is composed of J6 and J7. That is, the forearm 14
The first wrist joint J5 is rotatably connected at both ends with the axis of the forearm 14 as the axis of rotation a5, and at the tip of the forearm 14, the second wrist part 2 is rotatable about the axis of rotation a6. A joint J6 is provided, and the second joint J6 has a link 1
5a is connected to the tip of the link 15a.
A joint J7 is connected, and a link 15b is connected to the wrist third joint J7, and the wrist third joint J7 causes a link 15a around a rotation axis a7 coinciding with the axes of the links 15a and 15b. , 15b are relatively rotatable. And the three rotation axes a of the wrist
5, a6, a7 are configured to intersect at a point Ow of the wrist portion 15. An end effector such as a hand is attached to the tip of the link 15b.

With the above structure, the shoulder portion 11 has three joints J1, J2 and J3, and the wrist portion 15 has three joints J.
5, J6, J7, the joint J4 of the elbow 13 between the shoulder 11 and the wrist 15, and the rotation axes a1, a2, a3 of the three joints of the shoulder 11 intersect at one point Os. Wrist 15
Since the rotation axes a5, a6, a7 of the three joints are intersected at one point Ow, the hand position and posture are analyzed by the four joints J1, J2, J3 from the shoulder to the elbow. ，
J4 and the three joints J5, J6 and J7 of the wrist can be divided and handled, and the kinematics calculation becomes simple.
The arm mechanism 1 is capable of exerting an operation function similar to a function from a human shoulder to a hand, and like the human arm, the six freedoms required to determine the position / posture of the hand (end effector). 7 with one degree of freedom for each degree
The degree of freedom becomes, and when the position and the posture of the hand are controlled in the three-dimensional space, it is possible to bypass the obstacle or to wrap around behind the obstacle to perform positioning.

FIG. 2 shows the wrist position (intersection point of the rotational axes a5, a6, a7 of the three joints of the wrist portion 15) Ow by moving the three joints J1, J2, J3 of the shoulder portion 11 in this embodiment. It is explanatory drawing which shows that the position Oe of the elbow part 13 can be moved without changing. Since the wrist position Ow is not changed, this position Ow is a fixed position. Further, the shoulder position Os is a position where the rotational axes a1, a2, a3 of the three joints of the shoulder portion 11 intersect at one point,
The joints J1, J2, J3 of the shoulder 11 are in fixed positions regardless of how they move. Therefore, the straight line connecting the shoulder position Os and the wrist position Ow is constant. On the other hand, the length from the shoulder position Os to the elbow position Oe is the length of the upper arm 12 and is constant. The length from the elbow position Oe to the wrist position Ow is the length of the forearm 14 and is also constant. Therefore, the upper arm 12 and the forearm 14
The sandwiching angle therebetween is constant, and the rotation angle of the joint J4 of the elbow portion 13 is constant. Also, the three joints J of the wrist 15
5, J6 and J7 determine the posture of the wrist 15,
Since it does not contribute to the change of the wrist position Ow, it is a joint that is not related to the movement of fixing the wrist position Ow and moving the position Oe of the elbow portion 13. As a result, by moving the three joints J1, J2, J3 of the shoulder 11 without changing the wrist position Ow, the elbow 13 is moved along the arc Ce around the straight line connecting the shoulder position Os and the wrist position Ow. (For details, see Japanese Patent Application No. 3-841 filed earlier)
67). When the rotation angle of the elbow portion 13 around this straight line is φ, this rotation angle φ becomes a parameter that defines the self-motion described above. Since the direction of the forearm 14 also changes when the elbow 13 is moved, the posture of the hand changes, but the three joints J5, J6, J of the wrist 15 are changed.
By properly controlling 7, it is possible to prevent the posture of the hand from changing.

FIG. 3 shows an example of the teaching operation panel 3 used in this embodiment. K1 to K16 are keys, and keys K1 to K1.
K14 is a key for moving the arm mechanism of the manipulator, and keys K15, K16 are keys for switching between each axis operation and Cartesian coordinate system operation during teaching. Reference numeral 31 is a display unit for numerically displaying the position and orientation of the arm mechanism 1 of the manipulator. The relationship between each key and operation is as follows.

Key When operating in the Cartesian coordinate system When operating each axis K1 X-axis hand movement in + direction Joint J1 rotates in + direction K2 X-axis hand movement Joint J1 rotates in-direction K3 X-axis rotation + direction Hand posture changes Joint J2 rotates in + direction K4 X axis rotation Hand posture changes in-direction Joint J2 rotates in-direction K5 Y axis + hand movement Joint J3 rotates in + direction K6 Y axis-direction Hand movement Joint J3 rotates in-direction K7 Y-axis rotation changes hand orientation in + direction K8 joint rotates in + direction K8 Y-axis rotation Hand orientation changes in − direction Joint J4 rotates in − direction K9 Z-axis + direction Hand movement Joint J5 rotates in + direction K10 Z axis Hand movement in − direction Joint J5 rotates in − direction K11 Z axis rotation + Hand direction changes Joint J6 rotates in + direction K12 Z axis rotation − direction Ha Change of posture Joint J6 rotates in-direction K13 Elbow moves in + direction Joint J7 rotates in + direction K14 Elbow moves in-direction Joint J7 rotates in-direction K15 Select "axis operation" ← K16 " Select "Orthogonal coordinate system operation" ← When teaching with such teaching operation panel 3,
When the key K15 is pressed, the teaching mode becomes "each axis operation mode", and when any of the keys K1 to K14 is pressed, the controller 2 causes the corresponding servo of the manipulator to move the joints J1 to J7 of the corresponding manipulator. A command value is sent to an actuator such as a motor. For example, when the key K1 is pressed, the first joint J1 of the shoulder 11 rotates in the + direction around the rotation axis a1. When the key K2 is pressed, the joint J1 rotates around the rotation axis a1 in the-direction. The movements of the keys and the joints in the teaching by the movements of the respective axes are the same as the movement command of the respective axes and the movements of the joints in the conventional 6-DOF manipulator. However, in this embodiment, since there are 7 degrees of freedom, the number of axes is increased by 1 axis as compared with the 6 degrees of freedom manipulator.
Detailed description of each axis operation is omitted.

Next, when the key K16 is pressed, the "orthogonal coordinate system operation mode" is entered, and the processor of the control device 2 executes the processing shown in the flowcharts of FIGS. When K12 is pressed, the hand position and posture are changed without changing the elbow rotation angle φ (self-motion). When the keys K13 and K14 are pressed, the position and posture of the hand are not changed and only the rotation angle φ (self-motion) of the elbow is changed. Therefore, first, the matrices and the like used in the processing shown in FIGS. 4 and 5 will be described. The coordinate system (xi, yi, zi) of the link i (joint Ji) based on the coordinate system (xi-1, yi-1, zi-1) of the link i-1 (joint Ji-1).
), And a simultaneous conversion matrix representing a coordinate system j based on the base coordinate system (x0, y0, z0) is Tj. Further, P is a six-dimensional vector representing the position (x, y, z) and the posture (α, β, γ) of the tool center point (TCP) of the hand attached to the wrist 15. Then, the link parameters (in DH notation) in this embodiment are as follows.

[0019] The variable θi (i = 1, 2, ... 7) is the rotation angle of the joint Ji with the Z axis of the coordinate system as the rotation axis ai, that is, the rotation angle of the joint Ji. α is the joint J with respect to the coordinate system of the joint Ji-1
The twist angle of i around the X axis is a, the distance between the joint Ji (J3) and the joint Ji + 1 (J4), and d is the distance between the joint Ji-1 (J4) and the joint Ji (J5). Then, the A matrix Ai is represented by the following Equation 1,

[0020]

[Equation 1] From the above equation 1, the A matrices A1 to A7 are given by the following equation 2. However, Ci = cos θi and Si = sin θi.

[0021]

[Equation 2] A coordinate system T2 fixed to the link 11b, a coordinate system T3 fixed to the upper arm 12, a coordinate system T4 fixed to the forearm 14, and a coordinate system T7 representing TCP are as follows with the base coordinate system as a reference. .. T2 = A1A2 (3) T3 = A1A2A3 (4) T4 = A1A2A3A4 (5) T7 = A1A2A3A4A5A6A7 (6) From the above formula 3, the position of the shoulder position Os on the base coordinate system is xs = ys = zs = It becomes 0. The position (xe, ye, ze) of the elbow position Oe on the base coordinate system is as follows from the above equation (4). xe = (C1 C2 C3 + S1 S3) a3 ye = (S1 C2 C3 -C1 S3) a3 ze = S2 C3 a3 Also, the position (xw, yw) of the wrist coordinate Ow in the base coordinate system.
, Zw) is as follows from the above equation 5. xw = (-C1 C2 S34 + S1 C34) d5 + (C1 C2 C3 + S1 S3) a3 yw = (-S1 C2 S34-C1 C34) d5 + (S1 C2 C3-C1 S3) a3 zw = -S2 S3 S3d5d , S34 = sin (θ3 + θ4), C34 = cos
(Θ3 + θ4).

The elbow rotation angle φ which is a parameter of self-motion is a plane including the wrist position Ow and the shoulder position Os and perpendicular to the XY plane of the base coordinate system (the elbow position Oe is the highest position or the lowest position). (Corresponding to posture) is a reference plane, and wrist position Ow, shoulder position Os, elbow position Oe
The angle formed by the plane passing through the three points with the reference plane. That is, the rotation axis a4 of the joint J4 of the elbow portion 13 and the upper arm 12,
Since it is orthogonal to the forearm 14, the wrist position Ow,
A normal vector of a plane passing through the three points of the shoulder position Os and the elbow position Oe is the rotation axis a4. The rotation axis a4 is oriented in the Z axis of the coordinate system T3. Therefore, the angle formed by the normal vector indicated by the rotation axis a4 of the joint J4 on the reference plane and the normal vector indicated by the rotation axis a4 when the elbow rotation angle is φ is the elbow rotation angle φ. Using the example shown in FIG. 2,
The position indicated by the broken line is the reference position, and the position indicated by the solid line is the position of the elbow rotation angle φ. The joint J4 of the elbow portion 13 with respect to the plane passing through the three points of the wrist position Ow, the shoulder position Os, and the elbow position Oe.
Since the rotation axis a4 of the above is orthogonal to the rotation axis a4 and the upper arm 12 and the forearm 14, it always indicates the normal direction of the plane,
In addition, as shown in FIG. 2, it indicates the tangential direction of the arc Ce. Then, the rotation axis a4 on the reference plane and the elbow rotation angle φ
The angle formed by the rotation axis a4 at this time indicates the rotation angle φ of the elbow. Therefore, the coordinate system T3 which is the rotation axis a4
The rotation angle φ can be obtained by obtaining the direction of the Z axis of.

As for the elbow rotation angle φ, the elbow portion 13 is rotated with the wrist position Ow at a constant position, as described with reference to FIG. 2, a line connecting the shoulder position Os and the wrist position Ow, That is, the rotation angle around the vector k from the shoulder position Os to the wrist position Ow is the elbow rotation angle φ. In the vector k, the shoulder position Os is (0,0,0) on the base coordinate system.
Therefore, it is represented by the wrist position Ow. In addition, the coordinate system T fixed to the upper arm regarding the elbow position
3 can be a function of the elbow rotation angle φ. Then, it is expressed as T3 (φ) = A1 (θ1) A2 (θ2) A3 (θ3) (7), and Δφ around the vector k from this current position.
In the case of rotating by only .phi., The following relationship is established from the simultaneous conversion matrix Rot (k, .DELTA..phi.) For performing the conversion of rotating .phi.

T3 (φ + Δφ) = Rot (k, Δφ) T3 (φ) (8) Then, when the elbow rotation angle becomes (φ + Δφ), the rotation angles θ1 of the joints J1, J2, J3 of the shoulder portion 11 ', Θ2', θ3 'can be calculated by the following formula.

T3 (φ + Δφ) = A1 (θ1 ′) A2 (θ2 ′) A3 (θ3 ′) (9) In this way, only the rotation angle φ of the elbow is changed without changing the position of the hand. The rotation angles of the shoulder joints J1 to J3 can be calculated.

Next, the processing of the "orthogonal coordinate system operation mode" will be described with reference to the flowcharts of FIGS. When the key K16 is pressed to enter the "orthogonal coordinate system operation mode", the processor of the controller 2 determines whether the keys K1 to K15 have been pressed (steps S1 to S4), and if the key K15 is pressed, each axis is pressed. Move to move mode.

Assuming that the self-motion for moving only the elbow while fixing the position / posture of the hand is performed, when the key 13 for moving the elbow in the plus direction is pressed, the rotation angle of the elbow is moved within the processing cycle. The amount + Δφ set as the amount Δφ is stored in the register as the movement amount (step S5). Also, key 1 to move the elbow in the minus direction
When 3 is pressed, -Δφ is set in the register as the movement amount (step S6). Next, from the current position register that stores the current positions of the angular joints J1 to J7, the current positions θ1 to θ
7 is read (step S7), and first, the coordinate system T3 fixed to the upper arm 12 related to the current elbow position is calculated by the seventh equation (step S8). Then, the position (xw, yw, zw) of the above-described wrist position Ow in the base coordinate system is calculated from the equation (5) and the vector k is calculated (step S9). Then, the simultaneous conversion matrix Ro that rotates the elbow by the rotational movement amount Δφ around the coordinate system T3 (φ) and the vector k obtained in step S8.
From the t (k, Δφ), the above equation 8 is calculated to obtain the coordinate system T
3 (φ + Δφ) is calculated (step S10). Coordinate system T3
Is also obtained by the fourth equation, the shoulder portion 11 having the coordinate system T3 (φ + Δφ) obtained in step S10
The rotation angles .theta.1 'to .theta.3' of the joints J1 to J3 are calculated by the equation 9 (step S11). Next, shoulder position Os
Since the distance between the wrist position and the wrist position Ow does not change,
The rotation angle θ4 of the joint J4 of the elbow portion 13 does not change even if the elbow rotation angle changes from φ to (φ + Δφ), and the rotation angle θ after the change.
4 '= θ4. Therefore, the coordinate system T4 when the elbow rotation angle of the coordinate system T4 fixed to the forearm 14 becomes (φ + Δφ)
4'is calculated as follows (step S12).

T4 ′ = A1 (θ1 ′) A2 (θ2 ′) A3 (θ3 ′) A4 (θ4 ′) = T3 (φ + Δφ) A4 (θ4 ′) Further, since the position / posture of the hand does not change. , The coordinate system T7 representing the position / posture P of the hand does not change, and is obtained from the joint rotation angles θ1 to θ7 obtained in step S7, and the following equation is established.

T7 = A1 (θ1) A2 (θ2) A3 (θ3) A4 (θ4) A5 (θ5) A6 (θ6) A1 (θ7) = T4′A5 (θ5 ′) A6 (θ6 ′) A7 (θ7 ′ ) (10) The rotational angles θ5 ′ to θ6 ′ of the angular joints J5 to J7 of the wrist that do not change the posture of the hand are calculated from the tenth expression.

As described above, the moving positions .theta.1 'to .theta.3' are determined for the shoulder joints J1 to J3 obtained in step S11, and the moving position .theta.4 is equal to the elbow joint J4 which is not changed.
4 ', the movement positions θ5' to θ7 'obtained in step S13 are output to the wrist joints J5 to J7 to drive the manipulator.
Hereinafter, as long as the key K13 or the key 14 is pressed, the above process is repeated.

Next, when one of the keys K1 to K12 is pressed, in this case, the position / posture of the hand is changed without changing the rotation angle φ of the elbow (self-motion).
It will work. First, the current rotation angles θ of the angular joints J1 to J7
1 to θ7 are read (step S15), and this rotation angle θ1
~ The current hand position / posture P is calculated from θ7, and
The current elbow rotation angle φ is obtained from the Z-axis direction of the coordinate system T3 (step S16). Next, the movement amount ΔP set for the axis corresponding to the internally pressed key of the keys K1 to K12 is added to the position / orientation P of the hand obtained in step S16 to add a new hand position / orientation. P'is obtained (step S17), and each joint J is calculated from this hand position / posture P '.
The rotation angles of 1 to J7 are calculated, but three joints J of the shoulder 11
If the inner joint J2 of 1 to J3 is fixed (θ2 = constant), the structure is the same as that of the conventional 6-DOF manipulator, so the rotation angle θ2 of this joint J2 is fixed at the current position, and the hand position / posture is The rotation angles θ1 and θ3 to θ7 of the remaining 6 joints are calculated in the same manner as the conventional one so as to be P ′ (step S1).
8). Based on the rotation angles θ1, θ3 to θ7 thus determined and θ2 not changed, the elbow rotation angle φ ′ at the determined rotation angles θ1 to θ7 is determined (step S19) in the same manner as in step S16. The rotation angle φ'is subtracted from the elbow rotation angle φ before changing the position / posture of the hand obtained in step S16, and the change amount Δ of the elbow rotation angle is calculated.
φ is obtained (step S20). Then, step S8
By performing the following process, the commanded position / posture P ′ of the hand is held, and the elbow rotation angle is rotated (returned) by the change amount Δφ obtained in step S20. By doing so, the elbow rotation amount does not change, and only the position / orientation of the hand moves by the unit amount in the processing cycle for which the command is given, until φ is reached.

That is, in step S8, step S18
The coordinate system T3 (φ) is calculated from the new rotation angles θ1 and θ3 of each joint and the unchanged rotation angle θ2, and the wrist position Ow is calculated from the respective rotation angles θ1 to θ4 obtained in step S18. k (step S9), the coordinate system T3 (φ) calculated in step S8, the vector k, and the change amount Δ of the elbow rotation angle calculated in step S20.
From the simultaneous conversion matrix Rot (k, Δφ) that rotates by Δφ around the vector k obtained by φ, the equation 8 is calculated to obtain the coordinate system T3 (φ + Δφ) (step S1
0). After that, steps S11 to S14 described above are performed.
(In step S13, the coordinate system T7 determines the rotation angles θ1 and θ3 to θ7 of the new joints obtained in step S18.
And the rotation angle θ2 that has not been changed).
Move from 1'to θ7 '. Hereinafter, while one of the keys K1 to K12 is continuously pressed, steps S1 to S4 and step S
The processing of 15 to S20 and steps S8 to S14 is repeated every cycle, and the hand position / position is changed without changing the elbow rotation angle φ.
Change only the posture.

In the above embodiment, the first and second joints of the shoulder portion are rotary joints whose rotational axes are the same as the axial direction of the link, and the third joint is rotated around the rotational axis of the joint. The swivel joint is used, the swivel joint is used as the elbow joint, the rotary joint is used as the first joint of the wrist, the swivel joint is used as the second joint, and the rotary joint is used as the third joint, but the combination of the joints may be other configurations. .. That is, it suffices that the three rotation axes of the shoulder joint intersect at one point and the three rotation axes of the wrist intersect at one point. For example, the above-mentioned patent application and Japanese Patent Application No. The present invention can also be applied to various 7-degree-of-freedom manipulators shown in -84167.

[0034]

According to the present invention, the movement (position / posture) of the hand (end effector) is changed without changing the rotation angle of the elbow (the self-motion is not changed). Can be commanded to change only the rotation angle of the elbow (only self-motion is changed) without changing, and this command easily teaches the position / orientation etc. to the manipulator (robot) with 7 degrees of freedom. be able to.

[Brief description of drawings]

FIG. 1 is a configuration diagram of a 7-degree-of-freedom manipulator according to an embodiment of the present invention.

FIG. 2 is an explanatory diagram of self-motion in the same embodiment.

FIG. 3 is an external view of a teaching operation panel according to the embodiment.

FIG. 4 is a part of a flowchart of a process in a Cartesian coordinate system operation mode in the embodiment.

5 is a continuation of the flowchart shown in FIG.

[Explanation of symbols]

 1 Arm Mechanism 2 Manipulator Control Device 3 Teaching Control Panel 11 Shoulder 12 Upper Arm 13 Elbow 14 Forearm 15 Wrist J1 Shoulder 1st Joint J2 Shoulder 2nd Joint J3 Shoulder 3rd Joint J4 Elbow Joint J5 wrist 1st joint J6 wrist 2nd joint J7 wrist 3rd joint K1 to K16 keys

Claims (3)

[Claims] 1. From a joint that has three joints and whose rotation axes intersect at one point in space, and one joint that connects the upper arm connected to the shoulder and joins the forearm In a position teaching method in a seven-degree-of-freedom manipulator, which includes an elbow portion and a wrist portion that has three joints connected to the other end of the forearm portion and rotation axes of the three joints intersect at one point,
Linear movement command means for moving the end effector of the manipulator parallel to each axis of the Cartesian coordinate system, rotation command means for rotating about each axis, and connecting the shoulder and wrist without changing the position and orientation of the end effector. The elbow rotation command means for rotating the elbow around the straight line is provided, and when the linear movement command means or the rotation command means is operated, the position of the end effector without changing the elbow rotation angle around the straight line connecting the shoulder and the wrist. And the posture is changed, and when the elbow rotation command means is operated, only the elbow rotation angle is changed without changing the position and the posture of the end effector to teach the position / posture to the manipulator. Position teaching method. 2. A means for switching to each axis operation is provided, and when the operation is switched to each axis operation, each joint is individually driven by a command from the movement command means for each joint, and the position and orientation of the end effector are changed. The position teaching method for a 7-DOF manipulator according to claim 1, wherein the manipulator is taught the position and orientation by changing the position. 3. When the elbow rotation command means is operated, a coordinate system fixed to the upper arm is obtained from the rotation angle of each joint of the shoulder portion at that time, and the coordinate system is set to the rotation axis of the shoulder joint. The rotation angle of each joint of the shoulder when the elbow is rotated around a straight line connecting the intersection and the intersection of the rotation axes of the wrist joints, and the rotation angle of each joint of the shoulder and the joint of the elbow obtained The rotation angles of the three joints of the wrist are determined by the rotation angle that does not change and the coordinate system that represents the position and posture of the end effector before rotating the elbow, and each joint is moved to the determined rotation angle. When only the elbow rotation angle is changed without changing the position and orientation of the end effector, and when the linear movement command means or the rotation command means is operated, the rotation angle of the second joint of the shoulder is fixed and the command is issued. Function and position of the end effector In addition to determining the rotation angle of the joint, the amount of change in the elbow rotation angle is determined, and the coordinate system fixed to the upper arm is determined from the rotation angles determined for the shoulder joints. The command rotation angle of each shoulder joint when the elbow is rotated by the above-described amount of change around the straight line connecting the intersection of the rotation axis of and the intersection of the rotation axis of the wrist joint to obtain the original rotation angle of the shoulder joint, The obtained rotation angle that is the obtained command rotation angle of each joint of the shoulder and the command rotation angle of the elbow joint, and
The coordinate system that represents the position and orientation of the end effector before the elbow rotation angle is returned by the amount of change described above is used to determine the 3
A position / posture is taught to a manipulator by obtaining a command rotation angle of one joint, moving each joint to the obtained command rotation angle, and changing the position and posture of the end effector without changing the elbow rotation angle. A position teaching method for a 7-DOF manipulator according to the first or second item.