JP7082365B2 - Parallel link robot - Google Patents

Description

The present invention relates to a parallel link robot, which is one of industrial robots.

The mechanism of the parallel link robot is, for example, a motor device fixed to a base for fixing the robot, a link mechanism having an arm portion connected to a movable part of the motor device, and an action member connected to the link mechanism such as an end effector. Is attached and is composed of a moving part (traveling part) that can move in the orthogonal three-axis direction.

Compared to a parallel link robot in which joint mechanisms are arranged in series, there is no need to install a motor for each joint connecting the arm parts, and there is no need to swing each arm part and each motor, so there is an advantage that it can be made lighter. be. In addition, the parallel link robot can have a very high rigidity by forming the arm portion with a triangular pyramid structure, and can move the end effector at a very high speed.

Japanese Unexamined Patent Publication No. 2007-272116 JP-A-2015-142952

When a parallel link robot having such a configuration is used for assembling mechanical parts and the like, it is desired to use an end effector to control a force such as a pressing force. Therefore, as disclosed in Patent Document 1 and Patent Document 2, a method of attaching a force sensor to an end effector and performing feedback control using a reaction force applied to the end effector is known.

However, in this method, since a force sensor must be attached near the end effector of the parallel link robot, there is a problem that the weight of the end effector is increased and the shape is limited, so that the application is limited. And even if the arm part hits an obstacle, the force sensor does not detect this, so it was not possible to place the parallel link robot in the process of collaborating with humans. Therefore, the inventor is developing a torque sensor that is integrated with each motor of the parallel link robot to control the force of the end effector.

The present invention has been made in view of the above problems, and an object of the present invention is to provide a parallel link robot that can remove structural restrictions and further control the force of an end effector with high accuracy. be.

In order to achieve the above object, the parallel link robot of the present invention is used.
A plurality of motor units of at least three or more, a rotation position detection unit that detects the rotation position of each motor unit and outputs a rotation position detection value, and a torque detection value that detects the torsional torque of the output shaft of each motor unit. A parallel link robot in which a moving unit that attaches an end effector is connected in parallel by a plurality of link mechanisms to a plurality of motor devices that have a torque detection unit that outputs a torque detector and is fixed to a base.
The control means for controlling each motor device is
A command value calculation means that calculates and outputs each torque command value commanded to each motor unit by inputting the target value of the force generated in the moving unit and each rotation position detection value of each motor unit.
A torque value subtracting means that outputs a torque deviation by subtracting each torque detection value from each torque command value,
Motor control means that outputs control commands to each motor unit using torque deviation as input, and
Is configured to include.

Then, the command value calculation means outputs the torque command value by multiplying the transposed matrix of the Jacobi matrix, which converts the derivative of the rotational position coordinate variable of each motor unit into the derivative of the position coordinate variable of the moving unit, by the target value of the force. It is configured to do.

Further , the Jacobian determinant can be obtained by dividing the difference between the positions of the moving parts corresponding to the two positions before and after the distance of 1/2 of the current rotation position of each motor part by the predetermined distance. It is configured in.

Further, the command value calculation means calculates and outputs each rotation position command value by inputting the target value of the movement destination of the moving unit and each rotation position detection value of each motor unit.
The control means is
A rotation position value subtraction means that outputs a rotation position deviation by subtracting each rotation position detection value from each rotation position command value, and
It includes a control switching means for switching between an output from the torque value subtracting means and an output from the rotating position value subtracting means.
The motor control means is configured to output a control command to each motor unit based on either the rotation position deviation or the torque deviation switched by the control switching means.

Further, the command value calculation means outputs a speed limit value to the motor control means according to the switching state by the control switching means.
The motor control means is configured to output a control command to each motor unit based on the speed limit value.

According to the parallel link robot of the present invention, the target value of the force generated in the moving part and each rotation position detection value of each motor part are input, and each torque command value commanded to each motor part is calculated and output. Since the command value calculation means is provided and the feedback control is performed by subtracting each torque detection value, the structural restrictions related to the end effector can be removed and the force of the end effector can be controlled with high accuracy. can.

Overall perspective view of the parallel link robot according to the embodiment of the present invention. Perspective view of the mechanism of the parallel link robot according to the embodiment of the present invention. A control block diagram of a parallel link robot according to an embodiment of the present invention. Side view of the mechanism part of the parallel link robot according to the embodiment of the present invention.

Hereinafter, the parallel link robot of the present invention will be described in detail with reference to the drawings. FIG. 1 is a perspective view of the parallel link robot 18 according to the embodiment of the present invention as viewed from diagonally above, and FIG. 2 is a perspective view of the parallel link robot 18 as viewed from diagonally below. The parallel link robot 18 is composed of a mechanism unit attached to the base 1 and a control means 100. In FIG. 1, the electrical connection between the mechanical member and the control means 100 is drawn with three lines for convenience, but in reality, a plurality of signal lines are transmitted / received and power is supplied by a multi-core cable or the like. ..

First, the mechanical part of the parallel link robot 18 will be described. The base 1 is a flat plate-shaped metal member that supports and fixes the mechanism portion, and is fixed to a structure in which a frame or the like is assembled. On the lower surface side of the base 1, three motor devices 11a to 11c are radially arranged at equal intervals of 120 degrees and with the same dimensions in the radial direction around a certain point on the lower plane of the base 1. It is fixedly arranged by the brackets 13a to 13c.

The motor devices 11a to 11c are located on the opposite side of the motor units 2a to 2c, the torque detection units 4a to 4c that detect the torsional torque of the output shafts 12a to 12c and output the torque signal, and the motor units 2a to 2c. It is configured to include rotation position detection units 3a to 3c that detect the rotation position and output a rotation position signal. The motor units 2a to 2c are, for example, a combination of a motor and a speed reducer. The motors included in the motor units 2a to 2c are, for example, AC servomotors. Further, since the speed reducers included in the motor units 2a to 2c finely rotate in both forward and reverse directions to perform torque control, it is preferable to use, for example, a wave speed reducer that basically has no backlash.

The rotation position detection units 3a to 3c are so-called optical encoders, which detect the rotation position of the motor units 2a to 2c and output the rotation position detection value. In the present embodiment, the reflection type optical type is used, but the transmission type may be used, and the optical type may be used as well as a magnetic force or other method.

The torque detection units 4a to 4c are provided with a strain-causing portion for elastic deformation on the output shafts 12a to 12c from the motor units 2a to 2c, a strain gauge is arranged in the strain-causing portion, and the output shafts 12a to 12c are deformed. The amount of strain at the time is converted into an electric signal to obtain a torsion torque and output as a torque detection value. Since a continuous load is applied to the output shafts 12a to 12c of the motor devices 11a to 11c, the shaft itself may undergo creep deformation with time, and the magnetostriction method or the optical strain detection method is accurate. The torque value may not be obtained. When detection is performed using a strain gauge compared to the magnetostriction method or the optical strain detection method, self-compensation characteristics for temperature changes and creep deformation can be imparted, so a highly accurate torsion torque value can be obtained. Be done.

The rod-shaped first arms 15a to 15c are connected to the output shafts 12a to 12c of the motor devices 11a to 11c, respectively. The lengths of the first arms 15a to 15c in the arm direction are configured to have the same dimensions, so that one end of the first arms 15a to 15c can rotate in the angle range restricted by the output shafts 12a to 12c by the connecting portions 5a to 5c. It is connected to the. Two rod-shaped second arms 16a to 16c arranged substantially in parallel are connected to the other end side of the first arms 15a to 15c via joints 6a to 6c and joints 7a to 7c, respectively. The lengths of the arms of the second arms 16a to 16c are also configured to have the same dimensions.

The second arms 16a to 16c are connected to the moving portion 14 via the joints 6a to 6c and the joints 7a to 7c. These joints 6a to 6c and joints 7a to 7c allow the second arms 16a to 16c and the moving portion 14 to swing and rotate within a restricted angle range. The connection portions 5a to 5c, the joints 6a to 6c, and the joints 7a to 7c are joints having, for example, balls or bearings.

The moving portion 14 is a plate-shaped member that can move in the orthogonal triaxial direction, and is a second arm via joints 6a to 6c and joints 7a to 7c at an angle interval of 120 degrees from the center of the moving portion 14. 16a to 16c are connected. Therefore, these connecting portions 5a to 5c, joints 6a to 6c, joints 7a to 7c, first arms 15a to 15c, and second arms 16a to 16c are links that connect the motor devices 11a to 11c and the moving portion 14 in parallel. It is a mechanism 17. Various end effectors (not shown) are attached to the moving portion 14. In the present embodiment, the force sensor is not attached to the moving portion 14, and there are few restrictions on the shape and attachment position of the end effector. Moreover, the weight of the moving portion 14 and the entire end effector can be reduced, and these can be driven at high speed.

Next, the control of the parallel link robot 18 will be described with reference to FIG. The control of the parallel link robot 18 is performed by the control means 100. The control means 100 includes a command value calculation means 106, a torque value subtraction means 104a to 104c, a rotation position value subtraction means 105a to 105c, a control switching means 103a to 103c, a motor control means 102a to 102c, and a drive means 101a. It is configured to include ~ 101c.

The command value calculation means 106 tells the rotation position detection values θ _a , θ _b , and θ _c output from the rotation position detection units 3a to 3c, the target position _Ref of the movement destination of the movement unit 14, and the movement unit 14. By inputting the target value _Ref of the force to be generated and the input, the operation of calculating the command value for controlling each of the motor units 2a to 2c is performed. The details of the calculation will be described later.

The command value calculation means 106 has, as command values for controlling each motor unit 2a to 2c, each torque command value τ _{a ref} , τ _{b ref} , τ _{c ref} , each rotation position command value θ _{a ref} , θ _{b ref} . , Θ _{c ref} , each speed limit value v _{a limit} , v _{b limit} , v _{c limit} , signals are output. Since the control of the motor devices 11a to 11c is the same below, the parts related to the motor device 11a will be described as a representative.

The torque value subtracting means 104a subtracts the torque detection value τ _a from the torque command value τ _{a ref} output from the command value calculating means 106, and outputs the torque deviation. On the other hand, the rotation position value subtracting means 105a subtracts the rotation position detection value θ _a from the rotation position command value θ _{a ref} output from the command value calculation means 106, and outputs the rotation position deviation.

The control switching means 103a switches and outputs the torque deviation output from the torque value subtracting means 104a and the rotation position deviation output from the rotation position value subtracting means 105a as inputs so as to select either one. Then, this switching is performed simultaneously by the control switching means 103a to 103c, and the control switching means 103a to 103c simultaneously switch to either the torque deviation or the rotation position deviation and output.

The motor control means 102a receives either the torque deviation or the rotation position deviation switched by the control switching means 103a to 103c as an input, and performs a control calculation to make this deviation zero. At the same time, the motor control means 102a also performs a control calculation so that the speed of the motor unit _2a does not exceed the speed limit value va _limit by inputting the speed limit value va limit from the command value calculation means 106. The speed limit value va _limit of the motor unit 2a output by the command value calculation means 106 corresponds to the switching state of the control switching means 103a to 103c. Then, the motor control means 102a outputs a control command value for driving the motor unit 2a to the drive means 101a, and the drive means 101a drives the motor unit 2a by current control. As described above, the control of the motor devices 11b to 11c is the same as that of the motor device 11a.

Next, with reference to FIG. 4, a method for the command value calculation means 106 to calculate each torque command value τ _{a ref} , τ _{b ref} , τ _{c ref} will be described.

そして移動部１４の各仮想変位をδｐ＝（δｘ，δｙ，δｚ）^Ｔと定義する。 The external force applied to the moving unit 14 is F = (F _x , F _y , F _z ) ^T ,
The driving force (torque) of the motor units 2a to 2c is τ = (τ _a , τ _b , τ _c ) ^T ,
The coordinates of each motor unit 2a to 2c are set to θ = (θ _a , θ _b , θ _c ) ^T ,
If the virtual displacement of each motor unit 2a to 2c is defined as δθ = (δθ _a , δθ _b , δθ _c ) ^T , the virtual work δW _τ due to the driving force of each motor unit 2a to 2c is expressed by Equation 1. To.

Then, each virtual displacement of the moving portion 14 is defined as δp = (δx, δy, δz) ^T.

ここでδＷ_τ＝－δＷ_ｆ であるから式１と式２から式３が導かれる。

Then, the virtual work due to the external force applied to the moving unit 14 is expressed by Equation 2.

Here, since δW _τ = −δW _f , Equation 3 is derived from Equation 1 and Equation 2.

On the other hand, if the Jacobi matrix that converts the derivative of the rotational position coordinate system θ = φ of the motor unit to the derivative of the position coordinate system p of the moving unit 14 is J (φ), then δp = J (φ) δθ. Δp ^T = (J (φ) δθ) ^T = δθ ^T · J (φ) ^T , and Eq. 4 is derived.

Therefore, the torque command value τ _ref is the target value F of the force of the Jacobian determinant J (φ) that converts the derivative of each rotation position coordinate variable of each motor unit 2a to 2c into the derivative of the position coordinate variable of the moving unit 14. It can be obtained by multiplying _ref . Therefore, the torque command value τ _ref can be expressed by Equation 5.

Next, a method for obtaining J (φ) ^T will be described. The coordinate system of the moving unit 14 is represented by a function f of the coordinate system of each of the motor units 2a to 2c. (X, y, z) = f (θ _a , θ _b , θ _c ) This function f is geometrically calculated from the design value. The coordinate system of the moving unit 14 is defined by an XY plane including the axes of the output axes 12a to 12c of the motor devices 11a to 11c and a Z axis orthogonal to the XY plane and whose vertical upward direction is positive. ..

Radius R _a , R centered on each joint point J _a (x _a , y _a , z _a ), J _b (x _b , y _b , z _b ), J _c (x _c , y _c , z _c ) The intersection Q (x, y, z) of the spheres of _b and _Rc can be obtained by solving the three-dimensional simultaneous equations of the following equations 6 to 8. In the parallel link robot 18 of the present embodiment, the intersections of the axes of the output shafts 12a to 12c of the three motor devices 11a to 11c and the axes of the three first arms 15a to 15c (arrangement of the motor devices 11a to 11c). (Center) and each distance projected onto the XY plane are the axes of the joints 6a to 6c connecting the moving portion 14 and the second arms 16a to 16c, and the center point of the moving portion 14 in the XY plane. All have the same dimensions as each distance projected above. If these distances are different, the offset should be geometrically applied.

A quadratic equation with respect to z can be obtained by subtracting equation 6 from these equations 7 and 8 and rearranging them in the form of x = A + Bz and y = C + Dz, and substituting them into equation 6 and rearranging them. Two coordinates of z can be obtained to solve the quadratic equation, but since each joint point moves only in the negative region, the negative coordinate is used, and the intersection Q (x, y) of the sphere from this z is used. , Z) can be obtained.

最終的には次式を計算することになる。

The command value calculation means 106 sets the following near points separated from the current coordinate axes φ = (φ _a , φ _b , φ _c ) of the motor units 2a, 2b, and 2c by 1/2 of the predetermined distance δφ, that is, δφ / 2. It is defined by 6 points such as ψ ₁ to ψ ₆ .
ψ ₁ = (φ _a + δ φ / 2, φ _b , φ _c ), ψ ₂ = (φ _a ―δ φ / 2, φ _b , φ _c ),
ψ ₃ = (φ _a , φ _b + δ φ / 2, φ _c ), ψ ₄ = (φ _a , φ _b -δ φ / 2, φ _c ),
ψ ₅ = (φ _a , φ _b , φ _c + δ φ / 2), ψ ₆ = (φ _a , φ _b , φ _c − δ φ / 2). The position Pi of the moving portion 14 corresponding to each of these six points is _Pi = (x _i , y _i , z _i ) = f (ψ _i ) ( _i =) by using the above-mentioned method of finding the intersection of spheres. It can be obtained in 1 to 6). Therefore, J (φ) can be obtained by dividing the difference of f (ψ _i ) at two nearby points separated by δφ / 2 by a predetermined distance δφ and calculating the x, y, and z components thereof (). Equation 9). However, f _x (ψ _i ), f _y (ψ _i ), and f _z (ψ _i ) (i = 1 to 6) are x, y, and z components of f (ψ _i ), respectively.

Finally, the following equation will be calculated.

In this embodiment, J (φ) is obtained using a value of about δφ = ^1x10-6 . In this method, since the index is calculated from the positions before and after the current position, a highly accurate command value can be obtained.

Therefore, according to the embodiment of the present invention, the command value calculation means 106 wants to move the rotation position detection values θ _a , θ _b , θ _c and the moving unit 14 output from the rotation position detection units 3a to 3c. With the target position _Pref and the target value _Fref of the force to be generated in the moving unit 14 as inputs, the calculation is performed by the above equation, and each torque command value τ _{a ref} , τ _{b ref} , τ _{c ref} , each. The rotation position command values θ _{a ref} , θ _{b ref} , and θ _{c ref} are output. As an example, the control switching means 103a selects position control by each rotation position command value θ _{a ref} , θ _{b ref} , θ _{c ref until} the moving unit 14 reaches the target position Pref, and the moving unit 14 is the target. When the position _Pref is reached, switching is performed so as to perform force control (torque control) by each torque command value τ _{a ref} , τ _{b ref} , τ _{c ref} .

Further, as a modification, when the control switching means 103a frequently switches between the position control and the force control targeting the zero force during the period until the moving unit 14 reaches the target position _Pre , the moving unit 14 frequently switches. Even when 14 collides with an obstacle, the force can be released instantly. With such a control method, it is possible to prevent damage to each arm, each link mechanism, and the object of the parallel link robot 18. Since the force can be instantly relaxed even when the moving unit 14 hits a human, it is possible to perform cooperative work with the human and increase the degree of freedom in arranging the parallel link robot 18.

Furthermore, since the moving unit 14 is not provided with a force sensor, it is lightweight and capable of high-speed operation, there are few restrictions on the shape of the end effector attached to the moving unit 14, and the target force values _Ref are x and y. Since it can be freely set in the z direction, it is not only pick-and-place, but also a wide range that requires force control such as press-fitting parts into holes facing vertically, and brushing adhesives and paints on three-dimensional parts. It is possible to provide a parallel link robot applicable to the work of.

Although the embodiments relating to the present invention have been described above, the present invention is not limited to the above-described embodiments and can be modified in various ways.

As an example of utilization of the present invention, it can be applied to devices that require force control such as press fitting of parts and application of adhesives and paints.

1: Base 2a, 2b, 2c: Motor unit 3a, 3b, 3c: Rotation position detection unit 4a, 4b, 4c: Torque detection unit 5a, 5b, 5c: Connection unit 6a, 6b, 6c: Joint 7a, 7b, 7c: Joints 11a, 11b, 11c: Motor devices 12a, 12b, 12c: Output shafts 13a, 13b, 13c: Motor bracket 14: Moving parts 15a, 15b, 15c: First arm 16a, 16b, 16c: Second arm 17: Link mechanism 18: Parallel link robot 100: Control means 101a, 101b, 101c: Drive means 102a, 102b, 102c: Motor control means 103a, 103b, 103c: Control switching means 104a, 104b, 104c: Torque value subtraction means 105a , 105b, 105c: Rotational position value subtraction means 106: Command value calculation means

Claims (2)

At least three or more motor units, a rotation position detection unit that detects the rotation position of each motor unit and outputs a rotation position detection value, and a torque that detects the torsional torque of the output shaft of each motor unit. A parallel link robot in which a moving unit that attaches an end effector is connected in parallel by a plurality of link mechanisms to a plurality of motor devices that have a torque detection unit that outputs a detected value and is fixed to a base.
The control means for controlling each motor device is
A command value calculation means that calculates and outputs each torque command value commanded to each motor unit by inputting the target value of the force generated in the moving unit and the rotation position detection value of each motor unit. When,
A torque value subtracting means that outputs a torque deviation by subtracting each torque detection value from each torque command value.
Includes a motor control means that outputs a control command to each motor unit by inputting the torque deviation.
The command value calculation means calculates and outputs each rotation position command value by inputting the target value of the movement destination of the moving unit and the rotation position detection value of each motor unit.
The control means is
A rotation position value subtracting means that outputs a rotation position deviation by subtracting each rotation position detection value from each rotation position command value, and
A control switching means for switching between an output from the torque value subtracting means and an output from the rotating position value subtracting means is included.
The motor control means is a parallel link robot characterized in that a control command is output to each of the motor units based on either the rotation position deviation or the torque deviation switched by the control switching means . The command value calculation means outputs a speed limit value to the motor control means according to the switching state by the control switching means.
The parallel link robot according to claim 1, wherein the motor control means outputs a control command to each of the motor units based on the speed limit value.

Images