JP6267086B2 - Transmission angle calculation method, transmission angle correction device, robot control method, and robot control device - Google Patents

Description

  The present invention relates to a transmission angle calculation method for calculating a transmission angle of a universal ball joint, and a transmission angle calculation device that implements such a method.

  Furthermore, the present invention relates to a robot control method including at least one universal ball joint and a robot control apparatus for implementing such a method.

  Universal ball joints are known as joints that transmit power between two axes that are not collinear. The structure and shape of the universal ball joint are defined in JIS B1454 as “top universal shaft joint”. Such a universal ball joint is used as an element for driving a robot or a parallel mechanism. Patent Document 1 and Patent Document 2 also disclose a parallel mechanism including a universal ball joint.

FIG. 7 is a schematic view of a general universal ball joint. The relationship between the input angle θ and the output angle φ of the universal ball joint shown in FIG. 7 is expressed by the following relational expression (1).
cosαtanφ = tanθ (1)
Here, the character α is an angle formed by the first axis of the input angle θ and the second axis of the output angle φ, and the units of the angles θ, φ, and α are radian.

  When a universal ball joint is used for the robot or parallel mechanism, the output angle φ is calculated from the input angle θ and the angle α formed by the first axis and the second axis based on the relational expression (1) described above. Yes (forward movement). Similarly, the input angle θ can be calculated from the output angle φ and the angle α formed by the first axis and the second axis based on the relational expression (1) (reverse motion).

By the way, the unit direction vectors in space in the XYZ coordinates of FIG. 7 are defined as V1, V2, V3, and V4. Thereby, the relational expression (1) is derived as follows. In FIG. 7 and other drawings, the points A to J are defined as follows.
Point A: Base end of the first shaft Point B: Lower end of one arm portion of the first yoke attached to the end of the first shaft Point C: Other arm portion of the first yoke attached to the end of the first shaft Lower end Point D: Terminal end of the first axis Point E: Base end of the second axis Point F: Lower end of one arm part of the second yoke attached to the terminal end of the second axis Point G: Attached to the terminal end of the second axis Lower end of the other arm of the second yoke Point H: End of the second axis Point J: Intersection of the first axis and the second axis (center of the cross axis)
It is assumed that point B and point C are at positions facing each other on the first yoke, and point F and point G are at positions facing each other on the second yoke.

  First, the unit direction vector V1 in the direction from point A to point J in FIG. 7 is set to (0, 0, −1). The vector V1 and the unit direction vector V2 from the point B to the point C are always orthogonal. Therefore, when the vector V2 when θ = 0 is represented by (1, 0, 0), the vector V2 is represented by (cos θ, sin θ, 0).

  Next, a unit direction vector V3 from point E to point J in FIG. 7 is defined as (sin α, 0, cos α). The vector V3 and the unit direction vector V4 from the point F to the point G are always orthogonal. Therefore, when the vector V4 when φ = 0 is represented by (0, 1, 0), the vector V4 is represented by (−cos α sin φ, cos φ, sin α sin φ). Furthermore, the vector V2 and the vector V4 are always orthogonal. Therefore, the inner product of V2 and V4 is zero. By changing this, the above-described relational expression (1) is obtained.

JP 2008-134839 A JP 2011-101938 A

By the way, when deriving the above-described relational expression (1), the following three things are assumed.
The direction from point A to point J and the direction from point B to point C in FIG. 7 are orthogonal to each other.
The direction from point E to point J and the direction from point F to point G in FIG. 7 are orthogonal to each other.
The direction from point B to point C and the direction from point F to point G in FIG. 7 are orthogonal to each other.

  However, since an actual universal ball joint includes a processing error and an assembly error (hereinafter referred to as a “processing / assembly error”), the aforementioned directions are not completely orthogonal to each other in practice. Therefore, when the forward motion and the reverse motion of the universal joint are calculated using the relational expression (1), the calculated output angle and input angle may include a positioning error. Therefore, it is desirable to calculate the input and output angles without including such positioning errors.

  In patent document 1, the error of the rotation center position of the universal ball joint with respect to the parallel mechanism machine is corrected. However, in Patent Document 1, individual machining and assembly errors of the universal ball joint are not corrected. Therefore, in patent document 1, the positioning error mentioned above cannot be eliminated.

  The present invention has been made in view of such circumstances, and an object thereof is to provide a universal ball joint transmission angle calculation method and a transmission angle calculation apparatus capable of accurately calculating an output angle and an input angle. To do.

In order to achieve the above-described object, according to the first invention, of the first rod-shaped member and the second rod-shaped member connected via the universal ball joint, the first angle around the center line of the first rod-shaped member. And measuring a second angle corresponding to the first angle around the center line of the second rod-shaped member, the first angle, the second angle, and the first rod-shaped member and the second rod-shaped Information on relative angles with the member is acquired as a set of posture information, a plurality of sets of posture information are acquired by changing the first angle around the center line of the first rod-shaped member, and the universal Based on the relational expression for the first angle and the second angle, which includes a ball joint processing and assembly error, and the acquired plurality of sets of posture information, the processing and assembly error is calculated and calculated. Said processing Provided is a method for calculating a transmission angle of a universal ball joint, which creates a corrected relational expression that takes into account a vertical error and calculates a corrected second angle corresponding to the first angle based on the corrected relational expression. The
According to a second aspect, in the first aspect, at least one of the first angle and the second angle is measured by an angle detector.
According to a third aspect, in the first aspect, at least one of the first angle and the second angle is measured by a visual sensor.
According to the fourth invention, the first angle measuring unit that measures the first angle around the center line of the first rod-shaped member among the first rod-shaped member and the second rod-shaped member connected via the universal ball joint. A second angle measuring unit that measures a second angle corresponding to the first angle around the center line of the second rod-shaped member, the first angle, the second angle, and the first rod-shaped member A posture information acquisition unit that acquires information on a relative angle with the second rod-shaped member as a set of posture information, the posture information acquisition unit around the center line of the first rod-shaped member A plurality of sets of posture information are acquired by changing the first angle, and further, a relational expression for the first angle and the second angle, including processing and assembly errors of the universal ball joint, and acquisition The plurality of sets Based on the posture information, a machining assembly error calculation unit that calculates the machining assembly error, a corrected relational expression creation unit that creates a corrected relational expression that takes into account the calculated machining and assembly error, and the corrected There is provided a transmission angle calculation device for a universal ball joint, comprising: a corrected second angle calculation unit that calculates a corrected second angle corresponding to the first angle based on a relational expression.
According to a fifth aspect, in the fourth aspect, at least one of the first angle measurement unit and the second angle measurement unit is an angle detector.
According to a sixth aspect, in the fourth aspect, at least one of the first angle measurement unit and the second angle measurement unit is a visual sensor.
According to a seventh aspect, in the robot control method including at least one universal ball joint, of the first rod-shaped member and the second rod-shaped member connected via the universal ball joint, the first rod-shaped member Measuring a first angle around the center line, measuring a second angle corresponding to the first angle around the center line of the second rod-shaped member, and measuring the first angle, the second angle, and the first angle; Information on the relative angle between the rod-shaped member and the second rod-shaped member is acquired as a set of posture information, and a plurality of pairs of postures are obtained by changing the first angle around the center line of the first rod-shaped member. Information is acquired, and the relational expression about the first angle and the second angle, including the processing and assembly error of the universal ball joint, and the acquired plurality of sets of posture information Then, the machining assembly error is calculated, a corrected relational expression that takes into account the calculated machining assembly error is created, the required second angle, and the first rod-like member and the second rod-like member, Is obtained as a set of input information, a first angle corresponding to the input information is calculated based on the corrected relational expression, and the first angle is calculated based on the calculated first angle. There is provided a robot control method that realizes the required second angle around the center line of the second rod-like member by controlling the angle around the center line of the one rod-like member.
According to an eighth aspect, in the seventh aspect, the first angle is measured by an angle detector, and the second angle is measured by a visual sensor.
According to a ninth invention, in the seventh or eighth invention, the robot control method according to claim 7 or 8, wherein the robot has a parallel link mechanism structure.
According to a tenth aspect of the present invention, in the robot control device including at least one universal ball joint, the center of the first rod-shaped member among the first rod-shaped member and the second rod-shaped member connected via the universal ball joint. A first angle measuring unit for measuring a first angle around the line, a second angle measuring unit for measuring a second angle corresponding to the first angle around the center line of the second rod-shaped member, and the first An attitude information acquisition unit that acquires information on an angle, the second angle, and a relative angle between the first rod-shaped member and the second rod-shaped member as a set of posture information, and obtains the posture information The section acquires a plurality of sets of posture information by changing the first angle around the center line of the first rod-shaped member, and further, processing and assembly errors of the universal ball joint The machining assembly error calculation unit that calculates the machining assembly error based on the relational expression about the first angle and the second angle and the acquired plurality of sets of posture information, and the calculated A corrected relational expression creating unit for creating a corrected relational expression that takes into account machining / assembly errors, a required second angle, and information on a relative angle between the first rod-shaped member and the second rod-shaped member. An input information acquisition unit that acquires as a set of input information, and a first angle calculation unit that calculates a first angle corresponding to the input information based on the corrected relational expression, There is provided a robot control device that realizes the required second angle around the center line of the second rod-shaped member by controlling the angle around the center line of the first rod-shaped member based on one angle. The
According to an eleventh aspect, in the tenth aspect, the first angle is measured by an angle detector, and the second angle is measured by a visual sensor.
According to a twelfth aspect, in the tenth or eleventh aspect, the robot has a parallel link mechanism structure.

In the first and fourth inventions, the machining assembly error is calculated using a plurality of sets of posture information obtained by changing the first angle (input angle). Then, the second angle (output angle) is calculated based on the post-correction relational expression reflecting the machining / assembly error. Therefore, in the case of forward motion, the second angle (output angle) of the universal ball joint can be calculated with high accuracy. By the same method, the first angle (input angle) of the universal ball joint can be accurately calculated in the case of reverse motion.
In the second and fifth inventions, since the angle detector, for example, the encoder is usually attached to the motor, the first angle or the second angle can be obtained at low cost.
In the third and sixth inventions, the first angle or the second angle can be accurately obtained by processing the image acquired by the visual sensor.
In the seventh and tenth inventions, the machining / assembly error is calculated using a plurality of sets of posture information acquired by changing the first angle (input angle). Then, the first angle (input angle) of the universal ball joint is accurately calculated based on the corrected relational expression reflecting the machining / assembly error and the desired second angle. By controlling to be such a first angle (input angle), the second angle (output angle) can be realized with high accuracy in the universal ball joint in the case of forward motion. By a similar method, it is possible to realize the first angle (input angle) with high accuracy in the universal ball joint in the case of reverse motion.
In the eighth and eleventh inventions, the output angle and the input angle can be accurately calculated with a relatively simple configuration.
In the ninth and twelfth inventions, the seventh and tenth inventions can be applied to a parallel link robot.

  These and other objects, features and advantages of the present invention will become more apparent from the detailed description of exemplary embodiments of the present invention illustrated in the accompanying drawings.

It is a figure which shows the system containing the control apparatus based on this invention. 1 is a schematic view of a universal ball joint in a first embodiment of the present invention. It is a flowchart which shows operation | movement of the control apparatus based on 1st embodiment of this invention. It is a 1st figure which shows the relationship between 1st angle (theta) and the function f. It is a 2nd figure which shows the relationship between 1st angle (theta) and the function f. It is a 3rd figure which shows the relationship between 1st angle (theta) and the function f. It is a figure which shows the system containing the control apparatus based on 2nd embodiment of this invention. It is a flowchart which shows other operation | movement of the control apparatus based on 2nd embodiment of this invention. 1 is a schematic view of a general universal ball joint.

Embodiments of the present invention will be described below with reference to the accompanying drawings. In the following drawings, the same members are denoted by the same reference numerals. In order to facilitate understanding, the scales of these drawings are appropriately changed.
FIG. 1 shows a system 1 including a control device according to the present invention. As shown in FIG. 1, the system 1 mainly includes a control device 10 and a robot 20 connected to the control device 10. The robot 20 shown in FIG. 1 includes at least one universal ball joint 30.

  A control device 10 shown in FIG. 1 is a digital computer and controls the robot 20. The control device 10 includes a first angle around the center line of the first rod-shaped member of the first rod-shaped member (first shaft) and the second rod-shaped member (second shaft) coupled via the universal ball joint 30; A posture information acquisition unit that acquires, as a set of posture information, information on the second angle around the center line of the second rod-shaped member corresponding to the first angle and the relative angle between the first rod-shaped member and the second rod-shaped member. 11 is included.

  Furthermore, the control device 10 sets the parameters based on the relational expressions for the first angle and the second angle, which include the machining / assembly error of the universal ball joint 30 as a parameter, and the acquired plurality of sets of posture information. A machining assembly error calculation unit 12 that calculates a machining assembly error by identification is included. Further, the control device 10 obtains a corrected relational expression 13 that obtains a corrected relational expression that takes into account the calculated machining assembly error, and a corrected relational expression corresponding to the first angle based on the corrected relational expression. And a corrected second angle calculation unit 14 for calculating the second angle.

  Further, as shown in FIG. 1, the control device 10 receives the required second angle and the information on the relative angle between the first rod-shaped member and the second rod-shaped member as a set of input information. The information acquisition part 15 and the 1st angle calculation part 16 which calculates the 1st angle corresponding to input information based on the corrected relational expression are included.

  FIG. 2 is a schematic view of the universal ball joint in the first embodiment of the present invention. As shown in FIG. 2, the universal ball joint 30 includes a first shaft 31a, a first yoke 31b attached to the end thereof, a second shaft 32a, and a second yoke 32b attached to the end thereof. It mainly includes a first yoke 31b and a cross shaft 33 engaged with the second yoke 32b.

As described above, the input angle θ and the output angle φ of the universal ball joint 30 shown in FIG. 2 satisfy the relational expression (1) described above.
cosαtanφ = tanθ (1)

  In the present invention, a motor M, for example, a servo motor, is attached to the base end A of the first shaft 31 a that is the uppermost end of the universal ball joint 30. As a result, the positions of the proximal end A and the distal end D of the first shaft 31a are fixed, and the universal ball joint 30 can freely rotate in the direction of the first angle θ.

  Further, an angle detector, for example, an encoder 41 is attached to the motor M. The encoder 41 serves as a first angle measuring unit that measures the first angle θ around the center line of the first shaft 31a.

  A jig 45 is engaged with the second shaft 32 a of the universal ball joint 30. As shown in the figure, the jig 45 includes a ring portion having an opening portion approximately equal to the diameter of the second shaft 32a, and rods extending from both sides thereof. The second shaft 32 a is inserted into the ring portion of the jig 45. Further, at least one of the rods is fixed to a fixed portion (not shown).

  Since such a jig 45 is engaged with the second shaft 32a, the positions of the proximal end E and the distal end H of the second shaft 32a can be fixed. Therefore, even when the motor M rotates, the value of the angle α formed by the first shaft 31a and the second shaft 32a is constant. The angle α is, for example, 30 degrees.

  A thin plate target T is fixed to the base end E of the second shaft 32a which is the lowest part of the universal ball joint 30. The target T is disposed perpendicular to the second axis 32a. The target T is preferably a disc having a scale capable of grasping the rotation angle of the second shaft 32a. Alternatively, the target T may be a regular polygonal thin plate.

  Then, as shown in the drawing, the target T is imaged by a visual sensor, for example, the camera 42. For this reason, it is preferable that the camera 42 is arrange | positioned, for example on the extension line of the 2nd axis | shaft 32a. And the image imaged with the camera 42 is sent to the control apparatus 10, and is analyzed. Thereby, the direction of the target T is grasped, and as a result, the second angle φ shown in FIG. 2 can be acquired. Accordingly, the camera 42 serves as a second angle measurement unit that measures the second angle φ around the center line of the second axis 32a.

  FIG. 3 is a flowchart showing the operation of the control device according to the first embodiment of the present invention. The operation in the first embodiment of the present invention will be described below with reference to FIG. First, the positions of the proximal end A and the distal end D of the first shaft 31a and the proximal end E and the distal end H of the second shaft 32a are fixed as described above.

  In step S11, the motor M is driven to rotate the first shaft 31a from the initial position by a predetermined angle, for example, 30 degrees. Next, in step S12, the first angle θ of the first shaft 31a after rotation is acquired by the encoder 41. Furthermore, in step S13, in this state, the target T is imaged using the camera 42, thereby measuring the second angle φ of the rotated second axis 32a. In step S14, the posture information acquisition unit 11 acquires and stores the first angle θ, the second angle φ, and the relative angle α between the first shaft 31a and the second shaft 32a as a set of posture information. As described above, in the first embodiment, the angle α is constant.

  Thereafter, in step S15, it is determined whether or not the posture information acquisition unit 11 has acquired a predetermined number of posture information. If the predetermined number of posture information has not been acquired, the process returns to step S11. And the process of step S11-step S14 shall be repeated until a predetermined number of attitude | position information is acquired.

  When a predetermined number of posture information, for example, twelve sets of posture information is acquired, the process proceeds to step S16. In addition, when rotating the 1st axis | shaft 31a 30 degree | times at a time, if the 1st axis | shaft 31a rotates only 1 round, 12 sets of attitude | position information will be obtained. As a matter of course, the rotation angle of the first shaft 31a may be smaller than 30 degrees, and more than 12 pieces of posture information may be acquired. In this case, it will be understood that the calculation described later can be performed more precisely.

In step S16, the machining / assembly error calculation unit 12 derives a relational expression of the angle θ, the angle φ, and the angle α, assuming that the universal ball joint 30 includes the following machining / assembly errors ε1, ε2, and ε3.
The angle formed by the direction from point A to point J and the direction from point B to point C in FIG. 2 is (90 + ε1) degrees.
The angle formed between the direction from point E to point J and the direction from point F to point G in FIG. 2 is (90 + ε2) degrees.
The angle formed by the direction from point B to point C and the direction from point F to point G in FIG. 2 is (90 + ε3) degrees.

Here, in order to simplify the derivation of the relational expression, the three-dimensional unit vectors Ex, Ey, Ez and the three-dimensional rotation matrices Rx (W), Ry (P), Rz (R) are defined as follows. .

  According to these definitions, the direction V1 from point A to point J in FIG. 2 is represented by “−Ez”. The angle formed by the direction V1 and the direction V2 from the point B toward the point C is always (90 + ε1) degrees. Therefore, when the direction V2 when θ = 0 is represented by Ry (ε1) Ex, the direction V2 can be represented by Rz (θ) Ry (ε1) Ex.

  Further, the direction V3 from the point E to the point J in FIG. 2 is represented by Ry (α) Ez. The angle formed by the direction V3 and the direction V4 from the point F toward the point G is always (90 + ε2) degrees. Therefore, when the direction V4 when φ = 0 is represented by Ry (α) Rx (ε2) Ey, the direction V4 can be represented by Ry (α) Rz (φ) Rx (ε2) Ey.

Since the angle between the direction V2 and the direction V4 is always (90 + ε3) degrees, the following equation (2) is established.
(The inner product of V2 and V4) = sinε3 (2)

  Here, the equation (2) is an equation including the angles θ, φ, α and the machining assembly errors ε1, ε2, ε3 as parameters. When the machining assembly error ε1 = ε2 = ε3 = 0, the relational expression (1) and the expression (2) coincide with each other.

When the equation (2) is transformed, the second angle φ is expressed by the following function f with the angles θ and α and the machining assembly errors ε1, ε2, and ε3 as variables.
φ = f (θ, α, ε1, ε2, ε3) (3)

  Here, using the twelve sets of posture information acquired by the posture information acquisition unit 11, the machining assembly error calculation unit 12 can perform a square sum of “φi−f (θi, α, ε1, ε2, ε3)”. The angle α and the machining assembly errors ε1, ε2, and ε3 are determined by the Newton method or the like so as to be as small as possible. In this case, the letter i is an integer from 1 to 12. When the Newton method is used, the initial values of the angle α and the machining assembly errors ε1, ε2, and ε3 are preferably 30 degrees, 0 degrees, 0 degrees, and 0 degrees, respectively.

  In step S17, the corrected relational expression creating unit 13 inputs the calculated angle α and the machining assembly errors ε1, ε2, and ε3 into the above-described expression (3) to create a corrected relational expression f ′. To do. Next, in step S <b> 18, the corrected second angle calculation unit 14 inputs the first angle θ into the corrected relational expression, and calculates the second angle φ. Thus, the correct second angle φ after correction can be calculated based on the first angle θ that is the input angle.

  The second angle φ calculated in this way is an accurate output angle that does not include the machining assembly errors ε1, ε2, and ε3, unlike the second angle φ acquired using the camera 42. Therefore, in the present invention, the output angle of the forward motion can be calculated with high accuracy. It will be clear that the input angle of the reverse motion can be calculated with high accuracy by the same method.

4A to 4C are diagrams illustrating the relationship between the first angle θ and the function f. The horizontal axis of FIGS. 4A to 4C indicates the first angle θ, and its unit is deg. The vertical axis | shaft of FIG. 4A-FIG. 4C is as follows, respectively, The unit is deg.
FIG. 4A f (θ, 30, 0.5, 0, 0) −f (θ, 30, 0, 0, 0)
FIG. 4B f (θ, 30, 0, 0.5, 0) −f (θ, 30, 0, 0, 0)
FIG. 4C f (θ, 30, 0, 0, 0.5) −f (θ, 30, 0, 0, 0)

  In other words, in FIG. 4A, a function f in which only ε1 of the machining assembly errors ε1, ε2, and ε3 is set to 0.5 deg, and a function in which all the machining assembly errors ε1, ε2, and ε3 are set to zero. The deviation between them is on the vertical axis. In FIG. 4B, the deviation between the function f in which only ε2 of the machining assembly errors ε1, ε2, and ε3 is set to 0.5 deg and the function in which all the machining assembly errors ε1, ε2, and ε3 are set to zero is used. Is on the vertical axis. Further, in FIG. 4C, between the function f in which only ε3 of the machining assembly errors ε1, ε2, and ε3 is set to 0.5 deg and the function in which all the machining assembly errors ε1, ε2, and ε3 are set to zero. Is the vertical axis. Solid lines A1 to A3 shown in FIGS. 4A to 4C indicate such deviations.

Then, using the corrected relational expression f ′ created by identifying the machining assembly errors ε1, ε2, and ε3 by the above-described calculation method based on the present invention, the following deviations are obtained in each of FIGS. 4A to 4C. Get in the same way.
FIG. 4A f ′ (θ, 30, 0.5, 0, 0) −f ′ (θ, 30, 0, 0, 0)
FIG. 4B f ′ (θ, 30, 0, 0.5, 0) −f ′ (θ, 30, 0, 0, 0)
FIG. 4C f ′ (θ, 30, 0, 0, 0.5) −f ′ (θ, 30, 0, 0, 0)

  The deviation calculated in this way becomes substantially zero as shown by broken lines B1 to B3 in FIGS. 4A to 4C. Accordingly, it will be understood that the present invention can effectively eliminate the effects of machining and assembly errors.

  FIG. 5 is a diagram showing a system including a control device according to the second embodiment of the present invention. In FIG. 5, a parallel link robot 20 is shown as the robot 20. The parallel link robot 20 includes a base portion 21, a movable plate 22, and three link portions 5 a to 5 c that connect the base portion 21 and the movable plate 22. The base portion 21 includes three servo motors (not shown) that drive the link portions 5a to 5c, respectively.

  Each link part 5a-5c contains the drive link 51a-51c and the passive link 52a-52c, respectively. The drive links 51 a to 51 c extend radially outward from the base portion 21. One ends of the drive links 51a to 51c are connected to the output shaft of the servo motor. One end portions of the passive links 52a to 52c are connected to the other end portions of the drive links 51a to 51c through spherical bearings, respectively. The other ends of the passive links 52a to 52c are connected to the movable plate 22 through spherical bearings. When the servo motor is driven, the drive links 51 a to 51 c and the passive links 52 a to 52 c rotate in the vertical plane, and the movable plate 22 can be displaced to an arbitrary position in the three-dimensional point space below the base portion 21. it can.

  As shown in FIG. 5, a motor M having an encoder 41 is attached to the passive link 52a. The output shaft of the motor M is connected to a drive shaft as the first shaft 31a. The drive shaft 31a is used to rotate an attachment member shaft 32a as a second shaft 32a extending downward from the movable plate 22. It is assumed that an attachment member (not shown) for attaching the end effector is disposed at the tip of the attachment member shaft 32a.

  As shown in FIG. 5, the rotation direction of the drive shaft 31a and the rotation direction of the attachment member shaft 32a are different from each other. For this reason, the universal ball joint 30 fixed to the movable plate 22 is provided between the drive shaft 31a and the mounting member shaft 32a. By rotating the motor M, the attachment member shaft 32a can be rotated without changing the rotation center position of the universal ball joint 30.

  FIG. 6 is a flowchart showing another operation of the control device according to the second embodiment of the present invention. Since steps S11 to S17 in FIG. 6 are the same as described above, a part of the description is omitted. In the second embodiment, the encoder 41 measures the rotation angle of the motor M as the input angle θ (step S12). At the same time, the camera 42 arranged immediately below the tip of the attachment member shaft 32a acquires an image of the attachment member shaft 32a. And the control apparatus 10 performs an image process, and acquires the rotation angle of the shaft 32a for attachment members as output angle (phi) (step S13).

  Further, the angle α between the drive shaft 31a and the attachment member shaft 32a can be estimated from the position and orientation of the passive link 52a. The position and orientation of the passive link 52a can be calculated from the values of the rotation angles of the joint portions of the link portions 5a to 5b.

  Thus, even when the parallel link robot 20 is used as the robot, the input angle θ, the output angle α, and the angle α can be acquired. Then, machining assembly errors ε1, ε2, and ε3 are calculated by the same method as described above (steps S14 to S16). Therefore, it is possible to calculate the output angle of the forward motion and the input angle of the reverse motion with high accuracy using the corrected relational expression in consideration of these (step S17). That is, it will be understood that the same effect as described above can be obtained in the second embodiment.

  Further, in the second embodiment, in step S21, the operator inputs a desired second angle φ to the control device 10 using an input device such as a keyboard. Then, the input information acquisition unit 15 acquires the angle α and the desired second angle φ as a set of input information. Next, in step S22, the first angle calculation unit 16 applies input information to the corrected relational expression created in step S17, and calculates a first angle corresponding to the input information.

  Next, in step S23, the drive shaft 31a is rotated around its center line by the calculated first angle. As a result, a desired second angle φ can be realized around the center line of the attachment member shaft 32a. In other words, in the second embodiment, it will be understood that the desired second angle φ can be realized easily and with high accuracy. Note that it is possible to realize the first angle θ (input angle) in the universal ball joint with high accuracy in the case of reverse motion by a similar method.

By the way, in the first and second embodiments described above, the following matters are premised when the machining assembly errors ε1, ε2, and ε3 are calculated.
The three points A, D, and J shown in FIG. 2 are on a straight line.
The three points B, C, and J shown in FIG. 2 are on a straight line.
The three points E, H, and J shown in FIG. 2 are on a straight line.
The three points F, G, and J shown in FIG. 2 are on a straight line.
However, since the actual universal ball joint 30 includes processing and assembly errors, the above-described three points are not positioned on a straight line.

  In the third embodiment (not shown) of the present invention, it is assumed that the point A and the point D are on the Z axis of the coordinate system shown in FIG. 2 when the input angle θ is zero. At this time, the point B and the point C are respectively (X, Y, Z) = (L + λ, ζ, 0), (X, Y, Z) = (− L + λ, ζ, 0) in the coordinate system shown in FIG. ).

  Here, the letter L is a design distance between the point B and the point J and a design distance between the point C and the point J. Characters λ and ζ are error parameters, and their values are zero by design. However, in reality, the error parameters λ and ζ are not zero. On the other hand, for example, when the error parameter ζ is not zero, the points A, B, C, and D included in the first axis 31a and the first yoke 31b of the universal ball joint 30 are on one plane in design. Is not actually on a single plane.

  In the third embodiment, a relational expression between the input angle θ and the output angle φ considering the error parameters λ and ζ is considered. In a situation where these error parameters λ and ζ are assumed not to be 0, the input angle θ is changed and the points A and B are rotated while the positions of the points A, B and E are fixed.

  In this case, the angle α does not become constant, and the angle α oscillates with a period of 360 degrees with respect to the input angle θ. The reason is that focusing on the second axis 32a of the universal ball joint 30 including the point E and the point H, the point J rotates around the first axis 31a as the input angle θ is changed around the first axis 31a. Because.

  However, the greater the distance between point E and point J, the smaller the vibration at angle α. Therefore, the influence of the error parameters λ and ζ on the input angle θ and the output angle φ is small. Therefore, in FIG. 2, if the point E is fixed by the jig 45, the equation of φ = f (θ, α, λ, ζ) (the modified equation (3)) is obtained as in the first embodiment. ) To identify the error parameters λ and ζ. It is also possible to simultaneously identify the error parameters λ, ζ and machining assembly errors ε1, ε2, ε3 using the relational expression of φ = f (θ, α, λ, ζ, ε1, ε2, ε3). . Therefore, it is obvious that the same effect as described above can be obtained also in the third embodiment.

  Although the present invention has been described using exemplary embodiments, those skilled in the art can make the above-described changes and various other changes, omissions, and additions without departing from the scope of the invention. You will understand.

DESCRIPTION OF SYMBOLS 1 System 5a-5c Link part 10 Control apparatus, transmission angle calculation apparatus 11 Posture information acquisition part 12 Processing assembly error calculation part 13 Post-correction relational expression preparation part 14 Second angle calculation part after correction 15 Input information acquisition part 16 1st angle Calculation unit 20 Robot, parallel link robot 21 Base unit 22 Movable plate 30 Universal ball joint 31a First axis, drive axis (first rod-shaped member)
31b First yoke 32a Second shaft, mounting member shaft (second rod-shaped member)
32b Second yoke 33 Cross shaft 41 Encoder (Angle detector)
42 Camera (visual sensor)
51a to 51c Drive link 52a to 52c Passive link M Motor θ Input angle (first angle)
φ Output angle (second angle)
ε1, ε2, ε3 Processing assembly error

Claims (12)

Of the first rod-shaped member (31a) and the second rod-shaped member (32a) connected via the universal ball joint (30), the first angle around the center line of the first rod-shaped member is measured,
Measuring a second angle corresponding to the first angle around the center line of the second rod-shaped member;
Obtaining the first angle, the second angle, and information on the relative angle between the first rod-shaped member and the second rod-shaped member as a set of posture information;
Obtaining a plurality of sets of posture information by changing the first angle around the center line of the first rod-shaped member;
Based on the relational expression for the first angle and the second angle, including the processing and assembly error of the universal ball joint, and the plurality of sets of posture information acquired, the processing and assembly error is calculated,
Create a modified relational expression that takes into account the calculated machining assembly error,
A transmission angle calculation method for a universal ball joint, wherein a corrected second angle corresponding to the first angle is calculated based on the corrected relational expression.   The transmission angle calculation method according to claim 1, wherein at least one of the first angle and the second angle is measured by an angle detector (41).   The transmission angle calculation method according to claim 1, wherein at least one of the first angle and the second angle is measured by a visual sensor (42). Of the first rod-like member (31a) and the second rod-like member (32a) connected via the universal ball joint (30), a first angle measurement for measuring a first angle around the center line of the first rod-like member. And
A second angle measuring unit for measuring a second angle corresponding to the first angle around the center line of the second rod-shaped member;
A posture information acquisition unit (11) that acquires information on the first angle, the second angle, and the relative angle between the first rod-shaped member and the second rod-shaped member as a set of posture information. ,
The posture information acquisition unit acquires a plurality of sets of posture information by changing the first angle around the center line of the first rod-shaped member,
further,
Processing that calculates the processing / assembly error based on the relational expressions for the first angle and the second angle, including the processing / assembly error of the universal ball joint, and the acquired plurality of sets of posture information. An assembly error calculator (12);
A corrected relational expression creating unit (13) for creating a corrected relational expression in consideration of the calculated machining and assembling error;
A universal ball joint transmission angle calculation device (10), comprising: a corrected second angle calculation unit (14) that calculates a corrected second angle corresponding to the first angle based on the corrected relational expression; ).   The transmission angle calculation device according to claim 4, wherein at least one of the first angle measurement unit and the second angle measurement unit is an angle detector (41).   The transmission angle calculation device according to claim 4, wherein at least one of the first angle measurement unit and the second angle measurement unit is a visual sensor (42). In a control method of a robot (20) including at least one universal ball joint (30),
Of the first rod-shaped member (31a) and the second rod-shaped member (32a) connected via the universal ball joint, measure the first angle around the center line of the first rod-shaped member,
Measuring a second angle corresponding to the first angle around the center line of the second rod-shaped member;
Obtaining the first angle, the second angle, and information on the relative angle between the first rod-shaped member and the second rod-shaped member as a set of posture information;
Obtaining a plurality of sets of posture information by changing the first angle around the center line of the first rod-shaped member;
Based on the relational expression for the first angle and the second angle, including the processing and assembly error of the universal ball joint, and the plurality of sets of posture information acquired, the processing and assembly error is calculated,
Create a modified relational expression that takes into account the calculated machining assembly error,
Obtaining the required second angle and information on the relative angle between the first rod-shaped member and the second rod-shaped member as a set of input information,
Based on the corrected relational expression, a first angle corresponding to the input information is calculated,
Control of the robot that realizes the required second angle around the center line of the second rod-shaped member by controlling the angle around the center line of the first rod-shaped member based on the calculated first angle Method.   The robot control method according to claim 7, wherein the first angle is measured by an angle detector (41), and the second angle is measured by a visual sensor (42).   The robot control method according to claim 7, wherein the robot has a parallel link mechanism structure. In a control device (10) of a robot (20) comprising at least one universal ball joint (30),
Of the first rod-shaped member (31a) and the second rod-shaped member (32a) connected through the universal ball joint, a first angle measuring unit that measures a first angle around the center line of the first rod-shaped member;
A second angle measuring unit for measuring a second angle corresponding to the first angle around the center line of the second rod-shaped member;
A posture information acquisition unit (11) that acquires information on the first angle, the second angle, and a relative angle between the first rod-shaped member and the second rod-shaped member as a set of posture information; And
The posture information acquisition unit acquires a plurality of sets of posture information by changing the first angle around the center line of the first rod-shaped member,
further,
Processing that calculates the processing / assembly error based on the relational expressions for the first angle and the second angle, including the processing / assembly error of the universal ball joint, and the acquired plurality of sets of posture information. An assembly error calculator (12);
A corrected relational expression creating unit (13) for creating a corrected relational expression in consideration of the calculated machining and assembling error;
An input information acquisition unit (15) that acquires information on a second angle required and information on a relative angle between the first rod-shaped member and the second rod-shaped member as a set of input information;
A first angle calculator (16) that calculates a first angle corresponding to the input information based on the corrected relational expression,
Control of the robot that realizes the required second angle around the center line of the second rod-shaped member by controlling the angle around the center line of the first rod-shaped member based on the calculated first angle apparatus.   The robot control device according to claim 10, wherein the first angle is measured by an angle detector (41), and the second angle is measured by a visual sensor (42).   The robot control apparatus according to claim 10, wherein the robot has a parallel link mechanism structure.

Images