Link actuator and method for driving link actuator

Abstract

To provide a link operation device having a parallel link mechanism which improves positioning accuracy while suppressing the increase in a load in adjustment.SOLUTION: A link operation device 50 includes a link hub 2 on a base end side, a link hub 3 on a tip side, link mechanisms 4 for connecting the link hub 2 and the link hub 3, an actuator 51, and a control device 100. The actuator 51 includes motors provided on the respective link mechanisms 4. The control device 100 stores a map storing drive command values of the respective motors corresponding to a plurality of discrete positions, in a movable range of the link hub 3. When receiving a movement command value to a position not coinciding with the plurality of positions in the movable range, the control device 100 interpolates a region surrounded by four points at the plurality of positions on the map by a polynomial curved surface expression, and thereby determines the drive command values of the respective motors.SELECTED DRAWING: Figure 16

Description

[Technical Field]

[0001] The present disclosure relates to a link actuator and a drive method thereof, and more particularly to a technique for improving the positioning accuracy of a link actuator. [Background technology]

[0002] Link actuators are used in medical equipment, industrial equipment, and the like that require precision and a wide operating range. A link actuator consists of a drive source and a link mechanism. A parallel link mechanism is known as one type of link mechanism. For example, a link actuator as shown in Japanese Patent Application Laid-Open No. 2015-194207 (Patent Document 1) has been proposed as a link actuator that is compact in configuration yet capable of precision and operation over a wide operating range. Such a link actuator is attached to the end of the arm of an industrial robot, for example, and is used as a wrist joint mechanism that controls the posture of the end effector. [Prior art documents] [Patent documents]

[0003] [Patent Document 1] Japanese Patent Application Laid-Open No. 2015-194207 [Patent Document 2] Japanese Patent Application Publication No. 60-205713 Summary of the Invention [Problem to be solved by the invention]

[0004] When driving a link actuation device, it is necessary to calculate drive command values ​​for driving each link from the input target position by inverse transformation. However, since a link actuation device has a complex configuration consisting of a combination of multiple link mechanisms, positioning errors depending on how the machine is moved can occur for each machine on an actual basis due to manufacturing errors, assembly errors, and the effects of deflection of the link mechanisms that are unique to the machine. Therefore, there are cases where the desired positioning accuracy cannot be achieved using drive command values ​​derived from a theoretical inverse kinematic function.

[0005] Japanese Patent Laid-Open Publication No. 60-205713 (Patent Document 2) discloses a method for an industrial robot in which the amount of error correction at each positioning point is measured in advance and stored as a three-dimensional map, and the positioning error relative to an input target position is corrected by referring to the three-dimensional map.

[0006] However, in the 3D map used in JP 60-205713 A (Patent Document 2), it is virtually impossible to set correction amounts for all positions in space; the 3D map is defined as a collection of discrete points. Therefore, if a position not stored in the 3D map is specified as a target position, it may be impossible to perform appropriate positioning correction. On the other hand, if the number of measurement points used to generate the 3D map is increased and correction amounts are set for more target points, the time required to set the correction amounts becomes enormous, placing a heavy burden on the operator performing the adjustment. Furthermore, the amount of data required to perform positioning correction becomes enormous, requiring a storage device with a large storage capacity.

[0007] The present disclosure has been made to solve these problems, and its purpose is to improve the positioning accuracy in a link actuation device having a parallel link mechanism while suppressing an increase in the amount of data in maps for positioning correction and the burden during adjustment. [Means for solving the problem]

[0008] A link actuation device according to a first aspect of the present disclosure includes a first link hub at a base end, a second link hub at a tip end, at least three link mechanisms connecting the first link hub and the second link hub, a drive device for driving the at least three link mechanisms, and a control device for controlling the drive device. The drive device includes a motor provided for each of the at least three link mechanisms. Each of the at least three link mechanisms includes a first end link member rotatably coupled to the first link hub, a second end link member rotatably coupled to the second link hub, and a central link member rotatably coupled to each of the first end link member and the second end link member. In the at least three link mechanisms, at least three central axes of the rotation pairs between the first link hub and the first end link member and the central axis of the rotation pair at one end of the central link member intersect at the center point of the first link hub, and at least three central axes of the rotation pairs between the second link hub and the second end link member and the central axis of the rotation pair at the other end of the central link member intersect at the center point of the second link hub. The control device stores a map in which drive command values ​​for each motor corresponding to a plurality of discrete positions within the movable range of the second link hub are stored. When the control device receives a movement command value for a position that does not match the plurality of positions within the movable range, it determines the drive command value for each motor by interpolating an area surrounded by four points at the plurality of positions on the map using a polynomial surface equation.

[0009] A second aspect of the present disclosure relates to a method for driving a link actuation device. The link actuation device includes a first link hub on the base end side, a second link hub on the tip end side, at least three link mechanisms connecting the first link hub and the second link hub, and a drive device for driving the at least three link mechanisms. The drive device includes a motor provided for each of the at least three link mechanisms. Each of the at least three link mechanisms includes a first end link member rotatably connected to the first link hub, and No. 2The at least three link mechanisms include a second end link member rotatably connected to the link hub, and a central link member rotatably connected to each of the first end link member and the second end link member. In the at least three link mechanisms, at least three central axes of the rotation pairs between the first link hub and the first end link member and the central axis of the rotation pair at one end of the central link member intersect at the center point of the first link hub, and at least three central axes of the rotation pairs between the second link hub and the second end link member and the central axis of the rotation pair at the other end of the central link member intersect at the center point of the second link hub. The method includes the steps of: (1) generating and storing a map in which drive command values ​​for each motor corresponding to a plurality of discrete positions within the movable range of the second link hub are stored; (2) receiving a target movement position for the second link hub; (3) if the target movement position is a position that does not match any of the plurality of positions within the movable range, interpolating an area surrounded by four points at the plurality of positions on the map using a polynomial surface equation; (4) determining a drive command value for each motor using the interpolated map; and (5) driving each motor using the determined command value for each motor. [Effects of the Invention]

[0010] According to the link actuation device of the present disclosure, a map storing drive command values ​​for each motor corresponding to a plurality of discrete positions within the movable range of the second link hub is used to interpolate an area surrounded by four points at the plurality of positions using a polynomial surface equation to determine a drive command value for the motor to drive the link mechanism. In other words, the drive command value can be determined by interpolation using a polynomial surface equation without increasing the number of measurement points during adjustment. Therefore, the positioning accuracy can be improved while suppressing an increase in the burden during adjustment. Furthermore, the amount of data in the map for positioning correction can be reduced, thereby suppressing an increase in the memory capacity required to store the map data. [Brief explanation of the drawings]

[0011] [Figure 1] FIG. 10 is a front view of the link actuator in a certain position. [Figure 2] FIG. 2 is a diagram showing a representative configuration of a link actuator corresponding to one link mechanism. [Figure 3] FIG. 2 is a cross-sectional view illustrating a base-end link hub and a base-end end link member of the parallel link mechanism. [Figure 4] This is a schematic diagram in which one of the three link mechanisms of the parallel link mechanism is extracted and represented by a straight line. [Figure 5] FIG. 10 is a perspective view of the link actuator in a state where the central axis of the link hub on the base end side and the central axis of the link hub on the tip end side are on the same line (original position). [Figure 6] FIG. 2 is a schematic diagram of a link actuator in an original posture. [Figure 7] FIG. 1 is a perspective view of a link actuator in an arbitrary position (bending angle θ, turning angle φ). [Figure 8] This is a model diagram of Figure 7. [Figure 9] FIG. 10 is a diagram illustrating the operating space of a link actuator. [Figure 10] 10 is a diagram for explaining a motor space formed by motor command values ​​for realizing the motion space of FIG. 9. FIG. [Figure 11] FIG. 1 is a diagram illustrating a Bezier curve. [Figure 12] FIG. 2 is a diagram for explaining a cubic Bezier surface used in the present disclosure. [Figure 13] FIG. 1 is a first diagram for explaining a method for calculating control points. [Figure 14] FIG. 2 is a second diagram for explaining a method for calculating control points. [Figure 15] 10 is a flowchart illustrating a method for generating a three-dimensional map. [Figure 16] 10 is a flowchart for explaining a control method when operating a real machine. DETAILED DESCRIPTION OF THE INVENTION

[0012] Hereinafter, embodiments of the present disclosure will be described with reference to the drawings. In the following drawings, the same or corresponding parts are designated by the same reference numerals, and description thereof will not be repeated.

[0013] [Link Actuator Configuration] The configuration of a link actuation device 50 including a parallel link mechanism 1 according to an embodiment of the present disclosure will be described with reference to FIGS. 1 to 3. FIG.

[0014] 1 is a front view of a link actuation device 50 in a certain position. Referring to FIG. 1, the link actuation device 50 includes a parallel link mechanism 1, an actuator 51 for changing the position of the parallel link mechanism 1, a speed reduction mechanism 52 for reducing the driving force of the actuator and transmitting the reduced force to the parallel link mechanism, and a control device 100 for controlling the actuator 51.

[0015] The parallel link mechanism 1 has a link hub 3 on the tip side connected to a link hub 2 on the base side via three link mechanisms 4 so that its posture can be changed. The number of link mechanisms 4 may be four or more. An end effector 61 is attached to the link hub 3 on the tip side.

[0016] The control device 100 includes a CPU (Central Processing Unit) 101 and a memory 102. The CPU 101 controls the attitude of the parallel link mechanism 1 by executing a program stored in the memory 102. More specifically, the control device 100 calculates a rotation angle β of each link mechanism 4 to achieve a target position command (bend angle θ, swing angle φ) that determines the attitude of the parallel link mechanism 1, which is sent from the outside, and controls the actuator 51 to achieve the rotation angle β.

[0017] FIG. 2 is a diagram representatively illustrating the configuration of one link mechanism 4 among the configurations of the link actuation device 50. Referring to FIG. 2, each link mechanism 4 includes a base-side end link member 5, a tip-side end link member 6, and a central link member 7, and constitutes a four-bar chain link mechanism consisting of four revolute pairs. One end of the base-side end link member 5 is rotatably connected to the base-side link hub 2. Similarly, one end of the tip-side end link member 6 is rotatably connected to the tip-side link hub 3. One end of the central link member 7 is rotatably connected to the other end of the base-side end link member 5. The other end of the tip-side end link member 6 is rotatably connected to the other end of the central link member 7.

[0018] As shown in Figures 1 and 2, the base-end link hub 2 is composed of a flat base 10 and three rotating shaft connecting members 11 arranged at equal intervals in the circumferential direction on the base 10. A rotating shaft 12, whose axis intersects with the central axis QA of the link hub 2, is rotatably connected to each rotating shaft connecting member 11. One end of the base-end end link member 5 is connected to the rotating shaft 12. The other end of the base-end end link member 5 is connected to a rotating shaft 15, which is rotatably connected to one end of the central link member 7. In this embodiment, the rotating shaft connecting members 11 are arranged at equal intervals in the circumferential direction on the base 10, but this is not necessarily the case.

[0019] The rotary shaft 22 of the link hub 3 and the rotary shaft 25 of the central link member 7 also have the same shapes as the rotary shafts 12 and 15, respectively.

[0020] As shown in Figures 1 and 2, the tip-side link hub 3 is composed of a flat tip member 20 and three rotating shaft connecting members 21 arranged at equal intervals in the circumferential direction on the tip member 20. A rotating shaft 22 whose axis intersects with the central axis QB of the link hub 3 is rotatably connected to each rotating shaft connecting member 21. One end of the tip-side end link member 6 is connected to the rotating shaft 22 of the link hub 3. The other end of the tip-side end link member 6 is connected to a rotating shaft 25 which is rotatably connected to the other end of the central link member 7.

[0021] The central axis O2(A) of the rotation pair between the end link member 5 and the central link member 7 and the central axis O2(B) of the rotation pair between the end link member 6 and the central link member 7 intersect at point A at an axis angle γ.

[0022] Fig. 3 is a cross-sectional view showing the base-end link hub 2 and the base-end end link member 5 of the parallel link mechanism. Fig. 3 shows the parallel link mechanism in the position shown in Fig. 1 with the tip-end link hub 3, tip-end end link member 6, and central link member 7 removed. Note that for each of the three end link members 5, a cross section taken along a plane including the rotation axes O1 and O2 of the rotation pairs at both ends is shown.

[0023] 1 and 3, each of the three link mechanisms 4 is provided with a posture-changing actuator 51 that arbitrarily changes the posture of the distal link hub 3 relative to the proximal link hub 2. Each actuator 51 is provided with a speed reduction mechanism 52. Each actuator 51 is a rotary actuator, and is installed coaxially with the rotating shaft 12 on the upper surface of the base 10 of the proximal link hub 2. The actuator 51 and the speed reduction mechanism 52 are provided integrally, and the speed reduction mechanism 52 is fixed to the base 10 by a motor fixing member 53. Note that if at least two of the three link mechanisms 4 are provided with actuators 51, the posture of the distal link hub 3 relative to the proximal link hub 2 can be determined.

[0024] In Figure 3, the reduction gear mechanism 52 has a large-diameter output shaft 52a that forms a flange joint. The tip surface of the output shaft 52a forms a flat flange surface 54 that is perpendicular to the center line of the output shaft 52a. The output shaft 52a is connected to the rotating shaft support member 31 on the outer diameter side of the end link member 5 on the base end side with a bolt 56 via a spacer 55. The rotating shaft 12 at the rotation pair between the link hub 2 and the end link member 5 consists of a large-diameter portion 12a and a small-diameter portion 12b. The small-diameter portion 12b is inserted into the inner ring of the bearing, and the large-diameter portion 12a is fitted into an inner-diameter groove 57 provided in the output shaft 52a of the reduction gear mechanism 52.

[0025] The end link member 5 has an L-shape. The end link member 5 is composed of one curved member 30 and a total of four rotating shaft support members 31 fixed to the outer diameter side surface and inner diameter side surface of both ends of the curved member 30, respectively. The four rotating shaft support members 31 are not the same shape, and the outer diameter side rotating shaft support member 31A provided at the rotation pair with the base end side link hub 2 has a flange mounting surface 58 that is connected to the flange surface 54 of the reduction mechanism 52 via a spacer 55. In this embodiment, the end link member 5 has an L-shape, but it does not necessarily have to be L-shaped.

[0026] 1, for example, an end effector 61 is attached to the distal link hub 3 of this link actuator 50. The actuator 51 changes the attitude of the distal link hub 3 relative to the proximal link hub 2, thereby controlling the angle of the end effector 61 with two degrees of freedom.

[0027] FIG. 3 shows the relationship between the central axis O1 of the rotation pair between the base-end link hub 2 and the base-end end link member 5, the central axis O2 of the rotation pair between the end link member 5 and the central link member 7, and the link hub center point PA for the three link mechanisms 4. As shown in FIG. 3, the three central axes O1, which are the rotation axes of the three actuators 51, and the link hub center point PA are on the same plane. As shown in FIG. 2, the central axis O2 passes through the link hub center point PA on this plane from an obliquely upward direction. The shapes of the tip-end link hub 3 and the tip-end end link member 6 and their positional relationship (not shown) are also similar to those of the base-end side shown in FIG. 3. In the example of FIG. 3, the angle α (arm angle) formed by the central axis O1 and the central axis O2 is 90°, but the angle α may be other than 90°.

[0028] The parallel link mechanism 1 is a structure that combines two spherical link mechanisms. The central axis O1 of the revolute pair between the link hub 2 and the end link member 5 and the central axis O2 of the revolute pair between the end link member 5 and the central link member 7 intersect at the link hub center point PA (Figs. 2 and 3) on the base end side. Also, on the base end side, the distance along the axis O1 between the revolute pair between the link hub 2 and the end link member 5 and the link hub center point PA is the same as the distance along the axis O2 between the revolute pair between the end link member 5 and the central link member 7 and the link hub center point PA.

[0029] 3 is omitted, similarly, the central axis of the revolute pair between the link hub 3 and the end link member 6 and the central axis O2(B) of the revolute pair between the end link member 6 and the central link member 7 intersect at the link hub center point PB (FIG. 2) on the tip side. Also, on the tip side, the distance between each revolute pair between the link hub 3 and the end link member 6 and the link hub center point PB is the same as the distance between the revolute pair between the end link member 6 and the central link member 7 and the link hub center point PB.

[0030] 4 is a schematic diagram in which one of the three link mechanisms 4 of the parallel link mechanism 1 is extracted and represented by a straight line. The bending angle θ and the turning angle φ will be explained using FIG.

[0031] The three link mechanisms 4 have geometrically identical symmetrical shapes. "Geometrically identical symmetrical shapes" means that, as shown in FIG. 4, a geometric model in which the end link members 5, 6 and the central link member 7 are represented by straight lines and the rotation pairs are represented by circles has a shape in which the base end portion and the tip end portion are symmetrical with respect to the bisecting plane. The parallel link mechanism 1 of this embodiment is a rotationally symmetrical type, and the positional relationship between the base end link hub 2 and the base end link member 5 and the tip end link hub 3 and the tip end link member 6 is rotationally symmetric with respect to the center line C (corresponding to PL1 in FIG. 2) of the central link member 7.

[0032] The base-end link hub 2, the tip-end link hub 3, and the three link mechanisms 4 form a two-degree-of-freedom mechanism in which the tip-end link hub 3 can rotate freely around two orthogonal axes relative to the base-end link hub 2. In other words, a two-degree-of-freedom mechanism is a mechanism that can freely change the attitude of the tip-end link hub 3 relative to the base-end link hub 2 with two degrees of freedom. This two-degree-of-freedom mechanism is compact, yet can widen the range of movement of the tip-end link hub 3 relative to the base-end link hub 2.

[0033] For example, a line that passes through the link hub center point PA and intersects at a right angle with the central axis O1 (FIG. 3) of the rotation pair between the link hub 2 and the end link member 5 is defined as the central axis QA of the link hub 2. Also, a line that passes through the link hub center point PB and intersects at a right angle with the central axis (not shown) of the rotation pair between the link hub 3 and the end link member 6 is defined as the central axis QB of the link hub 3.

[0034] In this case, the maximum value of the bend angle θ (Fig. 4) between the central axis QA of the base-end link hub 2 and the central axis QB of the tip-end link hub 3 can be set to approximately ±90°. The pivot angle φ (Fig. 4) of the tip-end link hub 3 relative to the base-end link hub 2 can be set within the range of 0° to 360°. The bend angle θ is the angle at which the central axis QB is inclined relative to the central axis QA on a vertical plane including the central axes QA and QB. The pivot angle φ is the angle formed by the projection of the central axis QB onto a horizontal plane with respect to a straight line L0 indicating the reference position of the pivot angle φ.

[0035] The orientation of the distal link hub 3 relative to the proximal link hub 2 is changed around the intersection PC of the central axis QA of the link hub 2 and the central axis QB of the link hub 3. Even if the orientation changes, the distance D (Figure 4) between the central points PA and PB of the proximal and distal link hubs does not change.

[0036] The following conditions are met in the parallel link mechanism 1. That is, the angle between the central axis O1 and central axis O2 of the end link member on the base end side of each link mechanism 4 is equal to the angle between the central axis O1 and central axis O2 of the end link member on the tip end side. The lengths from the link hub center points PA and PB to the rotation pairs are equal. The central axis O1(A) of the rotation pair between the link hub 2 and end link member 5 of each link mechanism 4 intersects the link hub center point PA at the base end. The central axis O2(A) of the rotation pair between the end link member 5 and central link member 7 of each link mechanism 4 intersects the link hub center point PA at the base end. The central axis O1(B) of the rotation pair between the link hub 3 and end link member 6 of each link mechanism 4 intersects the link hub center point PB at the tip end. The central axis O2(B) of the rotation pair between the end link member 6 and central link member 7 of each link mechanism 4 intersects the link hub center point PB at the tip end. The base end link member 5 and the tip end link member 6 have the same geometric shape, and the shape of the central link member 7 is also the same on the tip side of the base end. When these conditions are met, if the angular positional relationship between the central link member 7 and the end link members 5, 6 with respect to the plane of symmetry of the central link member 7 is the same on the base end and tip end, then due to geometric symmetry, the base end link hub 2 and base end link member 5, and the tip end link hub 3 and tip end link member 6 will move symmetrically and in the same way with respect to the bisecting plane.

[0037] FIG. 5 is a perspective view of the link actuator in a state in which the base end side link hub central axis QA and the tip end side link hub central axis QB are on the same line.

[0038] The origin position of the link actuator 50 is shown in Figure 5. In this specification, the origin position refers to a position in which the central axis QA of the base-end link hub 2 and the central axis QB of the tip-end link hub 3 are aligned. In other words, the origin position is a position in which the bending angle θ of the link actuator 50 is 0 degrees.

[0039] Figure 6 is a schematic diagram of the link actuator in the original position. Figure 2 shows a front view of only one of the three link mechanisms in the original position of Figure 1, and Figure 6 shows a simplified model diagram of Figure 2. The link mechanism can be simplified and represented by link hubs on the base end and tip end sides, end link members on the base end and tip end sides, and a central link member.

[0040] The parallel link mechanism 1 of the link actuation device 50 is configured to be mirror-symmetrical with respect to the bisecting plane PL1, which is a plane formed by the intersection of the base-side spherical link GA, which is centered on the base-side link hub center point PA, and the tip-side spherical link GB, which is centered on the tip-side link hub center point PB. Point A, where the central axis O2(A) of the rotation pair between the base-side end link member 5 and the central link member 7 intersects with the central axis O2(B) of the rotation pair between the tip-side end link member 6 and the central link member 7, is located on the bisecting plane PL1. The angle formed by the central axis O2(A) of the rotation pair between the base-side end link member 5 and the central link member 7 and the central axis O2(B) of the rotation pair between the tip-side end link member 6 and the central link member 7 is referred to as the shaft angle γ. The angle formed by the central link member 7 is referred to as the central angle d. To be precise, the central angle d is the angle formed by the intersection, on the bisector, of a line perpendicular to the central axis O2(A) of the rotation pair between the base-end end link member 5 and the central link member 7, and a line perpendicular to the central axis O2(B) of the rotation pair between the tip-end end link member 6 and the central link member 7. The shaft angle γ and the central angle d are constants determined when designing the parallel link mechanism 1. Also, from Figure 6, the central angle d can be expressed as d = π - γ (rad) using the shaft angle γ.

[0041] Figure 7 is a perspective view of the link actuator in an arbitrary position (bend angle θ, pivot angle φ). Figure 8 is a model diagram of Figure 7. In an arbitrary position (bend angle θ, pivot angle φ), the link hub central axis QB on the tip side forms a certain angle (bend angle θ) with respect to the link hub central axis QA on the base end side.

[0042] Point A always exists on the bisecting plane PL1, and this point A can be considered as one two-degree-of-freedom joint. When the bending angle is θ, the angle formed by the line connecting the base-end link hub center point PA and the tip-end link hub center point PB and the central axis QA of the base-end link hub 2 is θ / 2. Also, the angle formed by the line connecting the base-end link hub center point PA and the tip-end link hub center point PB and the line passing through the base-end link hub center point PA and point A is d / 2. The parallel link mechanism 1 is a mechanism that moves while maintaining these relationships.

[0043] Because the parallel link mechanism 1 has two degrees of freedom, once the two arm rotation angles β1 and β2 are determined, the position of the tip-side link hub 3 is determined. In Figure 8, the center PB of the tip-side link hub 3 of the parallel link mechanism 1 moves on a spherical surface GP of radius D (Figure 4), with the center O being the center PA of the base-side link hub 2.

[0044] The above has been described with respect to the actual center PB and point A, but the calculation can be simplified by projecting these points onto a unit sphere and using a model of a triangle on the unit sphere.

[0045] [Explanation of 3D map] (Real space and motor space) 9 is a diagram showing the operating space of the center PB of the tip-side link hub 3 in a link actuation device 50 equipped with the above-described parallel link mechanism 1. When the tip coordinates (θ, φ) of the center PB of the tip-side link hub 3 are changed while the base-side link hub 2 is fixed as shown in FIG. 1, the center PB moves on a hemisphere GP with the origin position (0, 0) as its vertex.

[0046] Fig. 10 is a diagram showing a motor space formed by motor command values ​​of actuator 51 for realizing each posture in the operation space of Fig. 9. Motor command values ​​m1 to m3 for the motors provided in each of the three link mechanisms 4 are shown on each axis in Fig. 10. When the motor command values ​​corresponding to each operation point in Fig. 9 are plotted in Fig. 10, a three-dimensional curved surface such as that shown by dashed line LN10 in Fig. 10 is obtained.

[0047] When operating a link actuation device, actuator command values ​​are generally calculated using an inverse kinematics function (e.g., spherical trigonometry) that geometrically determines the rotation angle of each joint from a target position (θ, φ). However, the link actuation device 50 described above is configured by combining multiple link mechanisms formed by combining members with complex shapes. Therefore, even if the motor is set to the geometrically determined rotation angle, a positioning error may occur with respect to the target position on an actual machine due to the influence of manufacturing errors of each member, assembly errors of the link mechanism, and / or deflection or deformation of each member. To improve the absolute positioning accuracy of the link actuation device, it is necessary to correct such positioning errors. However, because manufacturing errors, assembly errors, and the like differ for each machine, it is difficult to correct positioning errors using a common inverse kinematics function for each machine.

[0048] As a method for addressing such issues, a method is known in which the positioning error is individually corrected for each machine using a three-dimensional map of motor command values ​​as shown in Fig. 10, as in the above-mentioned Patent Document 2 (Japanese Patent Laid-Open Publication No. 60-205713). However, since it is difficult to actually measure and represent all positions in the operating space in such a three-dimensional map, the three-dimensional map is defined as a set of discrete points in a mesh pattern as shown in Fig. 10. Therefore, if a position not stored in the three-dimensional map is specified as a target position, it may not be possible to perform appropriate positioning correction.

[0049] On the other hand, if the number of measurement points for generating a 3D map is increased and correction amounts are set for more target positions, the time required to set the correction amounts will become enormous, placing a heavy burden on the worker performing the adjustments.

[0050] Therefore, in this embodiment, a method is adopted in which, in a three-dimensional map of motor command values ​​as shown in Fig. 10, points on the map that have not been actually measured are calculated from the mesh-like measurement points that make up the map by an interpolation formula using a polynomial surface formula, thereby correcting errors inherent to the machine while suppressing an increase in measurement points, thereby improving positioning accuracy. Note that, in this embodiment, an example will be described in which a bicubic Bezier surface is used as the interpolation formula, but the interpolation formula is not limited to this, and Ferguson The Bezier surface formula, the Coons formula, the B-Spline surface, the NURBS surface, etc. may be used. The degree of the two parameters may be one or more, and the degrees of the two parameters may be different. Also, even when using a Bezier surface formula, the degree of the two parameters may be two or more, and the degrees may be different from each other.

[0051] (Interpolation using Bezier surface formula) Next, an overview of the Bezier surface formula will be explained. Fig. 11 is a diagram for explaining how to draw a cubic Bezier curve on a plane. Fig. 11 explains the case where a Bezier curve connects points P0 and P3.

[0052] First, set control points (P1, P2) different from points P0 and P3. Then, let points P4, P5, and P6 be the points that divide line segments P0-P1, P1-P2, and P2-P3 internally at t:(1-t), respectively (where 0≦t≦1). Furthermore, let points P7 and P8 be the points that divide line segments P4-P5 and P5-P6 internally at t:(1-t), respectively, and let point P9 be the point that divides line segment P7-P8 internally at t:(1-t). In this case, the locus of point P9 when t is changed from 0 to 1 is a Bezier curve.

[0053] This relationship can be expressed as the following equation (1).

[0054]

number

[0055] The Bezier surface is an extension of the above formula (1) to a surface using two variables, and can be expressed as the following formula (2).

[0056]

number

[0057] FIG. 12 is a diagram for conceptually explaining the cubic Bezier surface of the above formula (2), and shows four points P 00 ,P 03 ,P 30 ,P 33 Let us consider the case where the surface within the area (hereinafter also referred to as a "patch") surrounded by a point P is expressed by a cubic Bezier surface. 00 From point P 30 The locus to point P 00 The locus is the one when the variable u is changed from 0 to 1 while the variable v is fixed. 00 From point P 03 The locus to point P 00 This is the trajectory when the variable v is changed from 0 to 1 while the variable u is fixed. Similarly, P 03 From P 33 The locus to point P 03 The locus when the variable u is changed from 0 to 1 while the variable v is fixed is the point P 30 From point P 33 The locus to point P 30 This is the trajectory when the variable v is changed from 0 to 1 while the variable u is fixed.

[0058] These four points P 00 ,P 03 ,P 30 ,P 33 For control point P 01 ,P 02 ,P 10 ~P 13 ,P 20 ~P 23 ,P 31 ,P 32 By setting the point P 00 ,P 03 ,P30 ,P 33 Then, the control points P 01 ,P 02 ,P 10 ~P 13 ,P 20 ~P 23 ,P 31 ,P 32 By changing the value of , the shape of the resulting curved surface can be adjusted.

[0059] When applying a Bezier surface to the three-dimensional map of motor command values ​​shown in Fig. 10, the measurement points at the four corners of a patch surrounded by four points on the map obtained by actual measurement are designated as points P 00 ,P 03 ,P 30 ,P 33 The surface of the patch is expressed by setting the remaining control points appropriately. Then, by setting the variable u as a parameter related to the bend angle θ and the variable v as a parameter related to the turning angle φ, it is possible to obtain each motor command value for the target position (θ, φ) within the patch.

[0060] That is, by adjusting and storing the control points for each aircraft in advance, points other than the measurement points on the 3D map can be interpolated using a Bezier surface formula, which makes it possible to reduce the positioning error for each aircraft relative to the target position within each patch.

[0061] (Control point calculation method) In order to improve positioning accuracy by interpolating a three-dimensional map of motor command values ​​using a cubic Bezier surface formula, that is, to make the Bezier surface closer to the movement trajectory of the actual device, it is important to determine how to set the 12 control points, which are unknown quantities other than the four corner points of the patch.

[0062] 13 and 14 are diagrams for explaining a method of calculating control points in this embodiment. In this embodiment, as shown in FIG. 13, in a patch defined by four points, positions of the bending angle θ and the turning angle φ corresponding to the parameters u and v that divide each of the surrounding line segments into three equal parts are selected, and the motor command values ​​are measured when the actual machine is moved to each of the selected positions. These positions are actual points that exist on the actual curved surface within the patch. These points are represented by S as shown in FIG. 01 ,S 02 ,S 10 ~S 13 ,S 20 ~S 23 ,S 31 ,S 32 Then, by substituting the motor command value at each position into equation (2), the 12 unknowns P ij A simultaneous equation for the following is generated. By solving this simultaneous equation, it is possible to find 12 control points to realize a Bézier surface that includes 16 points on the surface, including the four corners of the patch. Note that while FIG. 13 shows an example in which the motor command value is measured by selecting positions that divide the line segments around the patch into thirds, the selected positions do not necessarily have to be positions that divide the line segments into thirds, and any 12 points within the patch may be selected.

[0063] By storing the control points calculated in this manner in memory 102 in addition to the actual measurement points that form the three-dimensional map shown in Figure 10, even if a target position that does not match the discrete actual measurement points in the three-dimensional map is set, the stored control points and the Bezier surface formula of equation (2) can be used to interpolate the curved surface within the patch to obtain the corresponding drive command value for each motor.

[0064] In addition, the point S on the curved surface to calculate the control point 01 ,S 02 ,S 10 ~S 13 ,S 20 ~S 23 ,S 31 ,S 32Although the operating points may be set (measured) separately from the actual measurement points forming the three-dimensional map, the time required to actually measure the operating points can be shortened by using some of the actual measurement points forming the three-dimensional map.

[0065] [Explanation of control method] Next, a control method for the actual device will be described with reference to FIGS.

[0066] (preparation process) FIG. 15 is a flowchart for explaining the procedure in the preparation step for generating the above-mentioned three-dimensional map and calculating the control points.

[0067] Referring to FIG. 15, in step (hereinafter, step will be abbreviated as "ST") 11, the actual motor command values ​​for actuator 51 when the target position (θ, φ) is changed at a predetermined pitch are stored, and a three-dimensional map such as that shown in FIG. 10 is generated.

[0068] At this time, for measurement at each target position, an external sensor is used to detect the absolute position of the center point PB of the distal end link hub 3 in the space where the link actuator 50 is placed, and the motor command value when the center point PB reaches the target position is stored. The external sensor may be an inclination angle sensor or a 3D vision sensor.

[0069] Next, in ST12, for each patch in the map obtained in ST11, 12 points on the curved surface excluding the four corners are set as shown in FIG. 13, and the motor command values ​​when operated to the absolute positions of each point are substituted into the Bezier surface formula of formula (2). Then, by solving the simultaneous equations consisting of the obtained 12 formulas, the 12 control points P for the target patch are determined. ij is calculated.

[0070] In ST13, the generated three-dimensional map (command value map) and the calculated control points are stored in the memory 102.

[0071] Regarding the control points, the values ​​calculated by the above calculation may be used as they are, but it is also possible to actually perform test operations multiple times using the calculated control points and then use control points that have been corrected based on the absolute positions at that time.

[0072] Furthermore, in the above method, a method for generating a map of motor command values ​​when an actual machine is placed at a target absolute position has been described, but instead, it is also possible to calculate the error between the motor command values ​​when the actual machine is placed at a target absolute position and motor command values ​​theoretically derived using an inverse kinematic function, and generate a correction value map for correcting the error. Even when a correction value map is used, the calculation of control points is the same as above.

[0073] (Actual machine control process) 16 is a flowchart for explaining a control method for operating an actual machine according to a target command value using the 3D map and control points set as described above. The processing in FIG. 16 is executed by the CPU 101 in the control device 100.

[0074] 16, in ST21, the control device 100 receives a movement target position (θ, φ) as a setting from a user or a command from a higher-level system. Then, in ST22, the control device 100 sets a patch including the target position using the command value map set in the preparation step of FIG. 15, and derives a Bezier surface formula (interpolation formula) using control points for the patch. In ST23, the control device 100 determines each motor command value from a point corresponding to the target position in the derived interpolation formula. Then, in ST24, the control device 100 controls the actuator using the determined motor command value, thereby positioning the link actuation device 50 at the target position.

[0075] As described above, in a link actuation device equipped with a parallel link mechanism, the positioning accuracy of the link actuation device can be improved by using the motor command value map and control points generated in advance in a preparation process to determine motor command values ​​for unmeasured positions on the map by interpolation using a Bezier surface equation. Furthermore, by using an interpolation equation, the amount of data required for positioning correction can be reduced, thereby suppressing an increase in memory capacity within the control device.

[0076] The embodiments disclosed herein should be considered to be illustrative in all respects and not restrictive. The scope of the present invention is defined by the claims, not by the description of the above embodiments, and is intended to include all modifications within the meaning and scope of the claims. [Explanation of symbols]

[0077] 1 parallel link mechanism, 2, 3 link hub, 4 link mechanism, 5, 6 end link member, 7 central link member, 10 base, 11, 21 rotating shaft connecting member, 12, 15, 22, 25 rotating shaft body, 12a large diameter portion, 12b small diameter portion, 20 tip member, 30 curved member, 31, 31A rotating shaft support member, 50 link actuator, 51 actuator, 52 reduction mechanism, 52a output shaft body, 53 motor fixing member, 54 flange surface, 55 spacer, 56 bolt, 57 inner diameter groove, 58 flange mounting surface, 61 end effector, 100 control device, 101 CPU, 102 memory, GA, GB spherical link.

Claims

1. a first link hub on the base end side; a second link hub on the tip side; At least three link mechanisms connecting the first link hub and the second link hub; a drive unit for driving the at least three link mechanisms; a control device for controlling the drive device, the drive device includes a motor provided for each of the at least three link mechanisms; Each of the at least three link mechanisms comprises: a first end link member rotatably coupled to the first link hub; a second end link member rotatably coupled to the second link hub; a central link member rotatably connected to each of the first end link member and the second end link member; In the at least three link mechanisms, at least three central axes of the rotation pairs between the first link hub and the first end link member and the central axis of the rotation pair at one end of the central link member intersect at a center point of the first link hub; at least three central axes of the rotational pairs between the second link hub and the second end link member and the central axis of the rotational pair at the other end of the central link member intersect at a center point of the second link hub; the control device stores a map in which drive command values ​​for each motor corresponding to a plurality of discrete positions within the movable range of the second link hub are stored, When the control device receives a movement command value to a position that does not match the multiple positions within the movable range, the control device determines a drive command value for each motor by interpolating an area surrounded by four points at the multiple positions on the map using a polynomial surface equation.

2. each of the plurality of positions is defined by an angle θ between a normal line passing through the first link hub center point and a normal line passing through the second link hub center point, and a rotation angle φ of the second link hub center point around the normal line passing through the first link hub center point; 2. The link actuator according to claim 1, wherein the polynomial surface expression is a cubic Bezier surface expression expressed by the following relational expression: [Equation 1]

3. 1. A method for driving a link actuator, comprising: The link actuator is a first link hub on the base end side; a second link hub on the tip side; At least three link mechanisms connecting the first link hub and the second link hub; a drive device for driving the at least three link mechanisms, the drive device includes a motor provided for each of the at least three link mechanisms; Each of the at least three link mechanisms comprises: a first end link member rotatably coupled to the first link hub; a second end link member rotatably coupled to the second link hub; a central link member rotatably connected to each of the first end link member and the second end link member; In the at least three link mechanisms, at least three central axes of the rotation pairs between the first link hub and the first end link member and the central axis of the rotation pair at one end of the central link member intersect at a center point of the first link hub; at least three central axes of the rotational pairs between the second link hub and the second end link member and the central axis of the rotational pair at the other end of the central link member intersect at a center point of the second link hub; The method comprises: generating and storing a map in which drive command values ​​for each motor corresponding to a plurality of discrete positions within the movable range of the second link hub are stored; receiving a target movement position of the second link hub; When the target movement position does not match the plurality of positions within the movable range, an area surrounded by four points at the plurality of positions on the map is interpolated using a polynomial surface equation; determining a drive command value for each motor using the interpolated map; and driving each motor using the determined command value for each motor.

4. each of the plurality of positions is defined by an angle θ between a normal line passing through the first link hub center point and a normal line passing through the second link hub center point, and a rotation angle φ of the second link hub center point around the normal line passing through the first link hub center point; The method according to claim 3 , wherein the polynomial surface expression is a cubic Bezier surface expression expressed by the following relation: [Equation 2]

Images