JPH0724754A - Working robot and control method thereof - Google Patents

Abstract

(57) [Abstract] [Purpose] To make it possible to increase the turning angle of the tip working machine without increasing the weight of the joystick at the tip of the second joint arm. [Structure] A first joint arm 3, a second joint arm 4 and actuators for rotationally driving the respective arms 3 and 4 are provided, and these are connected by a link mechanism.
In a work robot in which the tip of the third joint arm 22 is moved to a position according to the operation command, the middle portion of the third joint arm 22 is connected to the tip portion of the second joint arm 4 and one side of the third joint arm 22 is connected. It is connected to the tip working machine driving cylinder 14 supported at the end by the first joint arm 3 via a link mechanism,
Further, the rotating arm 20 is connected to the tip side of the connecting portion of the third joint arm 22 of the second joint arm 4, and the tip working machine mounting link 21 is connected to the tip of the rotating arm 20,
The other end of the third joint arm 22 is connected to an intermediate portion of the tip working machine attachment link 21.

Description

Detailed Description of the Invention

[0001]

BACKGROUND OF THE INVENTION 1. Field of the Invention The present invention attaches a tip working machine (end effector) as a working machine to the tip of an arm that operates joints to perform the work of attaching boards, panels, etc. to the ceiling, side walls, floor, etc. And a control method thereof.

Conventionally, there are provided a first joint arm, a second joint arm, and an actuator for rotating and driving the joint arms, which are connected by joints, and these are connected by a link mechanism, and are attached to the tip of the second joint arm. A work robot is used in which the tip working machine is attached so as to be rotatable in the same direction as the second joint arm.

FIG. 1 shows an example thereof. A swivel base 2 is provided on a base 1 so as to be swivelable, and a first joint arm 3 whose base end is rotatably connected to the swivel base 2. A second joint arm 4 rotatably connected to the tip of the first joint arm 3 and a rotation rotatably supported coaxially with a rotation fulcrum of the first joint arm 3 on the base end side. Moving body 5 and this rotating piece 5
And a link piece 6 connecting the second joint arm 4 in a parallel link shape, a swing motor 8 for driving the swivel base 2 via a chain 7, and a first swing arm 1 for rotating the first joint arm 3. A second cylinder that rotates the joint arm drive cylinder 9 and the rotating body 5 and rotates the second joint arm 4 through the second cylinder.
It is composed of a joint cylinder drive cylinder 10 and the like. The second joint arm drive cylinder 10 is rotatably supported on the swivel base 2.

In the conventional work robot of this type, as shown in FIG. 1, the tip working machine 11 is rotatably connected to the tip of the second joint arm 4 and is coaxial with the pivot shaft. The rotation motor 12 provided in the above-mentioned structure is designed to rotate.

[0005]

SUMMARY OF THE INVENTION In the above conventional technique,
When the weight is supported by the tip working machine 11, a turning motor 12 having a size commensurate with the weight must be selected, and the turning motor 12 is increased in size. There is a problem that the moment of inertia of the arms 3 and 4 increases. Also, when the work machine is attached to the tip of the work robot, or when the work such as sticking a board is performed, the object such as the board is aligned with the work machine posture along the side wall surface or the floor surface with respect to the reference plane of the space. It is often required to move the robot while keeping it constant.

The present invention has been conceived in view of the above, and the turning angle of the tip working machine can be increased without increasing the weight of the tip end portion of the second joint arm. An object of the present invention is to provide a work machine robot and a control method thereof that can control the posture to be constant.

[0007]

In order to achieve the above object, a working machine robot according to the present invention comprises a first joint arm 3, a second joint arm 4, and actuators for rotationally driving the respective arms 3, 4. In the working robot, which is equipped with a link mechanism and moves the tip of the second joint arm 4 to a position according to the operation command,
The intermediate portion of the third joint arm 22 is connected to the tip portion of the second joint arm 4, and the first end is connected to one end of the third joint arm 22.
Cylinder 14 for driving the tip working machine supported by the joint arm
To the tip of the second joint arm 4 from the joint of the third joint arm 22 of the second joint arm 4, and the tip of the rotary arm 20 is attached to the tip working machine attachment link. 21. A working robot, characterized in that 21 is connected, and the other end of the third joint arm 22 is connected to the intermediate portion of the tip working machine attachment link 21. Further, an operation control device for giving an operation command of the tip of the second joint arm 4 and an operation command of the tip working machine attachment link 21 is provided at the tip of the second joint arm 4. On the other hand, the method of controlling the work robot having the above-described configuration catches the change in the tip working machine attachment link 21 due to the rotation of the first joint arm 3 and the second joint arm 4. The control table of the working machine angle is calculated by using the relational table of the third joint angle θ ₃ and the working machine angle θ ₃₄ which is created in advance, and the working machine angle is controlled to be constant. And in the above control method,
The working machine angle is controlled by a coordinate system which has a connecting point of the joint arm 22 as an origin and which has two axes of a central axis of the second arm 4 and an axis orthogonal to the central axis.

[0008]

[Operation] Third joint arm 22 by actuator
By rotating, the tip working machine mounting link 21 rotates together with the rotating arm 20 connected to the tip side of the second joint arm 4, and rotates in a large operating range. Further, the control of the tip working machine attachment link 21 is performed at the tip of the second joint arm 4 at the connection point of the third joint arm 22. In the above control, the joint angles θ ₀ , θ ₁ , θ ₂ , and θ ₃ are detected by the potentiometer provided in each joint, and the tip working machine attachment link 21 is changed from the detected values.
The working machine angle is controlled so as to be constant by using the relational table of the third joint angle θ ₃ and the working machine angle θ _{34 which} is created in advance.

[0009]

EXAMPLE An example of the present invention will be described with reference to FIGS. The same components as those shown in FIG. 1 used to describe the conventional technique are designated by the same reference numerals and the description thereof will be omitted.

A tip working machine drive cylinder 14 is connected to the first joint arm 3, and the first joint arm 3 and the second joint arm 3 are connected to each other.
An intermediate arm 15 is rotatably provided coaxially with the rotation axis of the joint arm 4, a tip working machine driving cylinder 14 is provided on one side in the direction of rotation of the intermediate arm 15, and a second joint arm is provided on the other side. One end of a parallel link 16 which is parallel to 4 is connected.

A rotating arm 20 is attached to the tip of the second joint arm 4.
One end of the rotary arm 20 is rotatably connected to the rotary arm 20.
One end of a tip working machine attachment link 21 is rotatably connected to the tip of the. On the other hand, an intermediate portion of the L-shaped third joint arm 22 is rotatably supported at a position separated from the tip end of the second joint arm 4 toward the rotation base side. The front end of the parallel link 16 is connected to one arm member 22a, and the other arm member 22a
b is connected to an intermediate portion of the tip working machine attachment link 21. The tip working machine 23 is fixed to the tip of the tip working machine attachment link 21.

The other arm member 22b of the third joint arm 22 is formed in a yoke shape so as to sandwich the tip working machine attachment link 21, and the second arm member 22b of the third joint arm 22 is a rotating arm. It is possible to rotate beyond the connecting portion between 20 and the tip working machine attachment link 21.

A first joint potentiometer 24 for detecting the rotation angle of the first arm 3 is provided at the base end of the first joint arm 3, and a support portion for the swivel base 2 of the second joint arm drive cylinder 10 is provided. Is a second joint potentiometer 2 for detecting the rotation angle of the second joint arm drive cylinder 10.
5 further includes third joint potentiometers 26 for detecting the rotation angle of the third joint arm 22 on the rotation support portion of the third joint arm 22, respectively. A swivel potentiometer 27 for detection is provided.

In the above structure, the cylinders 9 and 1 for driving the first joint arm and for driving the second joint arm, respectively.
By operating 0, the first joint arm 3 and the second joint arm 4 rotate, and the tip end portion of the second joint arm 4 operates over a predetermined range. On the other hand, by operating the tip working machine drive cylinder 14, the third link is operated through the parallel link 16.
As the joint arm 22 rotates, as shown in FIG. 3, the tip working machine attachment link 21 changes its direction while changing the position of the connection point A with the rotation arm 20.

The angle between the second arm member 22b of the third joint arm 22 and the second joint arm 4 is θ, and the angle between the tip working machine attachment link 21 and the second joint arm 4 is θ.
_{When set to 34} , as shown in FIG. 3, θ ₃ is (±) 50
When changed over °, the tip work machine mounting link 2
1 changes from (+) 72.8 ° to (−) 84.3 °, and can take an operating angle range of about 1.6 times that in the case of the parallel link 16 shown in FIG. It was

Since the tip work implement mounting link 21 can be rotated over a wide range in this manner, for example, in the case of work such as board pasting, one work robot performs from downward floor construction to upward ceiling construction. It becomes possible to do it.

As shown in FIG. 4, the intersection of the tip working machine mounting link 21 and the rotary arm 20 is A depending on the posture of the rotary arm 20 even if the third joint arm 22 has the same posture.
There are two points, ₁ and A ₂ , but which intersection is to be taken depends on the initial assembly. Therefore, it is possible to change the construction depending on the construction purpose, that is, the required working angle range of the tip working machine.

As shown in FIG. 5, each of the above-mentioned operations is performed by providing a three-axis joystick controller 28 at the tip of the second joint arm 4 with its center aligned with the intersection B of the third joint arm 22. By operating this, each operation described above is performed. In this case, the joystick 29
By this, the left / right, left / right turn, and up / down (X, Y, Z) of the position command of the rotation fulcrum of the third joint arm 22 are given, and the target angle at each joint is calculated by the well-known coordinate conversion method. Command value is output. Here, the outputs of the joystick 29 in the left and right directions and the up and down directions are respectively assigned to the second joint arm 4 direction and the directions (X, Z) perpendicular to the second joint arm 4 and once converted to values in the basic coordinate (XZ) system. , Convert the coordinates to angles again. The rotary shaft of the joystick 29 is used for outputting a command value to the tip working machine drive cylinder 14, and a uniaxial joystick may be separately provided for turning.

FIG. 6 shows an example in which a suction pad 30 is mounted as a tip working machine. In this case, if a free shaft that can be opened by a mechanical lock is provided at the tip, the suction target can be delicately aligned with the wall surface or the like, which facilitates the work. In the above embodiment, the base 1 that supports the swivel base 2 may be fixed to the floor surface, but the floor surface may be movable using rails and wheels.

Next, an embodiment of a control method for the working machine robot configured as described above will be described with reference to FIGS. 2 and 7 to 12. The information obtained from the work robot shown in FIG. 2 is the turning joint angle θ ₀ from the swivel potentiometer 27 and the first information from the first joint potentiometer 24.
Joint angle θ ₁ Second from the second joint potentiometer 25
The joint angle θ ₂ , the third joint angle θ ₃ from the third joint potentiometer 26, and the working machine angle θ ₃₄ .

The second joint angle θ ₂ is the first joint angle θ ₁ and the second joint angle θ 2.
From the tilt angle θ ₂ ′ of the joint arm drive cylinder 10, and the working machine angle θ ₃₄ from the third joint angle θ ₃ and the angle θ ₄ of the third joint arm 22 of the tip working machine mounting link 21 to the second arm member 22b. Each is calculated. However, the working machine total angle θ _{34 with} respect to the third joint angle θ ₃ is shown in FIG.
The calculation formula differs depending on the mounting posture of the rotating arm 20 as shown in FIG.

FIG. 7 shows the main flow of the control program. (Step 1) Create a relational table of the third joint angle θ ₃ to the working machine angle θ ₃₄ . At this time, within the operating range of the third joint angle θ ₃ (θ ₃ min to θ ₃ max), (θ ₃ max-θ ₃ m
in) is divided into N to make Δθ _3, and θ ₃ min + n · Δθ
₃ , θ ₃₄ corresponding to (n: 0 → N−1) is obtained and stored in the array as shown below.

[0023]

[Equation 1]

(Step 2) Each joint angle and the current tip position are detected from the current potentiometer values. Then, each current potentiometer value is set as an initial target value of the joint angle (cylinder angle).

(Step 3) The command value from the joystick is fetched and added to the current tip position as an increment value,
A new target arm tip position is set and inversely converted to calculate the target joint angle. In FIG. 8, the basic coordinate system is XY-
Z, the tip point of the second joint arm 4 in this coordinate system is (t _x , t _y , y _z ), and XYZ is θ around the Z axis.
_The coordinate system rotated by ₀ is X'-Y'-Z ', and the tip point of the second joint arm 4 in this coordinate system is (x _p , y _p , z _p )
Further, a system in which the central axis of the second joint arm 4 is X ″ and the axis perpendicular to this is Z ″ with this arm tip point as the origin is X ″ −Y ″ −Z ″. At this time, X ″ -Y "-
Increase the joystick command value in the Z ″ system (which increases in proportion to the lever tilt angle) by ΔX ″, ΔY ″.
(= ΔY ′) and ΔZ ″, new coordinate values (tx ′, ty ′, tz ′) in the basic coordinate system XYZ corresponding thereto
= (Tx + Δtx, ty + Δtg, tz + Δtz) is as follows from FIG.

[0026] _{tx '= {txcosθ 0 + tysinθ} 0 + △ X "cos (θ 1 + θ 2) - △ Z" sin (θ 1 + θ 2)} cosθ 0 - △ Y "sinθ 0 ... (1) ty' = { _{txcosθ 0 + tysinθ 0 + △ X} "cos (θ 1 + θ 2) - △ Z" sin (θ 1 + θ 2)} sinθ 0 + △ Y "cosθ 0 ... (2) tz '= tz + △ X" sin (θ 1 + Θ ₂ ) + ΔZ ″ cos (θ ₁ + θ ₂ ) ... (3)

Therefore, if the coordinate target value in the basic coordinate system with respect to the joystick command value is obtained from the above equation, this is inversely converted to calculate each target joint angle (cylinder angle), and the potentiometer target value for each axis is determined. It will be good.

In this embodiment, as shown in FIG.
Use the joystick levers for up / down, left / right, and rotation 3-axis output, and apply the former two directions to the ΔX ″ and ΔZ ″ commands.
Since the rotation is assigned to the rotation command of the tip working machine, a separate joystick is provided to control the turning independently. Therefore, in this case, ΔY ″ in the above equation is zero (Δ
Y ″ = 0). If one axis can be further output in the direction of the dotted arrow in FIG. 5, three-axis composite operation by the above formula is possible. Further, regarding the rotation of the tip working machine, the current value θ _The target value θ ₃ + Δθ ₃ for ₃ is given directly with the joystick.

(Step 4) Work implement constant control The routine of FIG. 10 is added after step 3.
Whether or not the working machine is operated means that the rotation command is forcibly given by the joystick, and if so, this command value is directly output. In the case of correction of the working machine angle due to the movement of the first joint arm 3 and the second joint arm 4 instead of a compulsory rotation command, first, whether the working machine angle is within a preset range. Check. If it is within the range, the new work implement angle is set to the new work implement angle θ ₃₄ = the target angle of the tip work implement relative to the ground θ−
The first joint angle θ ₁ + the second joint angle θ ₂ is calculated, and the third joint angle θ ₃ with respect to the working machine angle θ ₃₄ is calculated based on the table created in (step 1).

That is, θ ₃₄ is converted into a table θ ₃₄ [i],
By sequentially comparing from i = 0, θ ₃ is obtained by the linear interpolation method shown in FIG. 11 from i when θ ₃₄ ≧ θ ₃₄ [i]. θ ₃ = θ ₃ min + i Δθ ₃ + θ ₃₄ −θ ₃₄ [i] / θ ₃₄
[I + 1] −θ ₃₄ [i]

Next, even if the current working machine angle is not within the set range, if the working machine is moved back to the set range by moving the working machine, a similar process for obtaining a new working machine target value is performed.

(Step 5) The calculated target value of each joint angle is converted into a potentiometer value, which is output, added with a feedback signal from each joint angle detection potentiometer to obtain a deviation, which is multiplied by a gain to drive the servo valve. Signal. FIG. 12 is a control system diagram corresponding to FIG. 7.

According to the work robot of the present invention, the working angle range of the tip working machine can be expanded by the composite link mechanism at the tip of the arm. By arranging the actuator for operating the tip working machine at the base of the working robot, the weight (moment of inertia) of the arm tip can be reduced and other actuators can be downsized. . Also, by providing operation command output means such as a joystick at the tip of the arm,
It is possible to facilitate the delicate operation of the work robot during work.

Further, according to the work robot control method of the present invention, it is possible to improve work efficiency by controlling the attitude of the tip working machine.

[Brief description of drawings]

FIG. 1 is a configuration explanatory view showing a conventional example.

FIG. 2 is a configuration explanatory view showing an embodiment of a work robot according to the present invention.

FIG. 3 is an operation explanatory view showing a rotation range of the tip working machine.

FIG. 4 is an explanatory view of the action of a third joint arm section.

FIG. 5 is a perspective view showing a joystick control device.

FIG. 6 is a side view showing an example of a tip working machine.

FIG. 7 is a main flow chart of the method of the present invention.

FIG. 8 is an explanatory diagram showing a coordinate system showing a state in which the tip end of the second joint arm in the basic coordinate system is rotated about the Z axis by θ ₀ .

FIG. 9 is an explanatory diagram showing a new coordinate system in the basic coordinate system.

FIG. 10 is a flowchart showing a routine for constant work machine control.

FIG. 11 is a diagram showing a line complement method.

FIG. 12 is a control system diagram of the method of the present invention.

[Explanation of symbols]

1 ... Base, 2 ... Revolving base, 3 ... 1st joint arm, 4 ... 2nd
Joint arm, 5 ... Rotating body, 6 ... Link piece, 7 ... Chain,
8 ... Rotating motor, 9 ... First joint arm drive cylinder, 10 ... Second joint arm drive cylinder, 14 ... Tip working machine drive cylinder, 15 ... Intermediate arm, 16 ... Parallel link, 20 ... Rotating arm , 21 ... Tip working machine attachment link, 22 ... Third joint arm, 22a, 22b ... Arm member, 23 ... Tip working machine, 24, 25, 26.27 ... Potentiometer, 28 ... Joystick control device, 2
9 ... Joystick, 30 ... Suction pad.

Claims (4)

[Claims] 1. A first joint arm 3, a second joint arm 4, and actuators for rotationally driving the respective arms 3, 4 are connected by a link mechanism, and the tip of the second joint arm 4 is used as an operation command. In the work robot configured to move to a position corresponding to the first joint, the distal end of the second joint arm 4 is connected to the intermediate portion of the third joint arm 22, and one end of the third joint arm 22 is connected to the first joint. The tip working machine driving cylinder 14 supported by the arm 3 is connected via a link mechanism, and the rotary arm 20 is connected to the tip side of the connecting portion of the third joint arm 22 of the second joint arm 4. A working robot in which a tip working machine attachment link 21 is connected to the tip of the rotating arm 20, and the other end of the third joint arm 22 is joined to an intermediate portion of the tip working machine attachment link 21. 2. An operation control device for giving an operation command of the tip of the second joint arm 4 and an operation command of the tip working machine attachment link 21 is provided at the tip of the second joint arm 4. The work robot described. 3. A third joint angle θ ₃ and a working machine angle θ _{34, which are} created in advance, by catching a change in the tip working machine mounting link 21 due to the rotation of the first joint arm 3 and the second joint arm 4. The control method of the working robot, wherein the control amount of the working machine angle is calculated by using the relation table of 1. to control the working machine angle to be constant. 4. The working machine angle is controlled by a coordinate system having two axes of a central axis of the second arm 4 and an axis orthogonal to the central axis of the third arm 22 as an origin. The method for controlling a work robot according to claim 3.