JP2010036293A - Multi-articulated robot - Google Patents

Abstract

Provided is an articulated robot that can move to reach a target position and a target posture.
The joint arm has a plurality of links connected in series by a plurality of joints. The forward kinematics computing unit 22 calculates the current position and current posture of the distal end portion of the joint arm based on the rotation angle of each link detected by the pulse encoders 16a to 16f. The rotation angle update unit 25 repeatedly performs the rotation angle of the link until the deviation of the calculated current position and current posture of the joint arm tip from the target position and target posture of the joint arm tip is equal to or less than a predetermined value. When the deviation is less than or equal to a predetermined value, the link is rotated based on the updated rotation angle. When the deviation is not less than the predetermined value and the number of correction angle calculations exceeds the predetermined number, the deviation determination unit 23 decreases the deviation calculated next time, and the reduced deviation is less than the predetermined value. Judge whether there is.
[Selection] Figure 2

Description

  The present invention relates to an articulated robot including a joint arm having a plurality of joints.

  Until now, there have been many conventional techniques related to articulated robots. In an articulated robot, a plurality of links are connected in series at a joint to form a joint arm, and each link is provided with a drive motor that rotates around the joint with respect to an adjacent link (for example, a patent) Reference 1).

  This control device for the multi-joint robot controls the operation of the drive motor so that the position and posture of the end effector at the tip of the joint arm coincide with the target position and target posture according to the work contents. For this purpose, the rotation angle of each link constituting the joint arm is detected by the angle sensor provided in the drive motor, and the end effector of the end effector is calculated by forward kinematics using the detected rotation angle of each link. Finding the current position and posture.

  The calculated current position and posture of the end effector are compared with the target position and target posture of the end effector, and the difference between the two is calculated. The correction angle of each link is calculated from the difference and the pseudo inverse matrix of the Jacobian matrix, and the rotation angle of each link is updated based on the calculated correction angle. When the above-described difference is equal to or smaller than a certain value, the drive motor is operated to move the end effector based on the updated rotation angle. Here, the Jacobian matrix described above is a matrix of functions for converting the rotation angle of each link into the current position and current posture of the end effector.

If the above difference exceeds a certain value, the current position and posture of the end effector are obtained again from the updated rotation angle of each link, and the target position and posture of the end effector are the same as described above. And the difference between the two is calculated. Hereinafter, the convergence calculation is performed until the difference between the current position and posture of the end effector and the target position and posture becomes a certain value or less, and rotation of each link for setting the end effector to the target position and target posture. Seeking the angle.
JP 2005-230952 A

However, if the joint arm shape is in a singular posture where the end effector's target position and target posture cannot be established, the solution of the link rotation angle cannot be obtained in the convergence calculation described above, and the joint arm is targeted. The position and target posture cannot be reached.
Normally, in an articulated robot, a plurality of interpolation points are formed on a trajectory for moving the end effector from the current position and current posture to the final target position and posture, and the above-described convergence calculation is performed for each interpolation point. is doing. For this reason, even if the solution of the rotation angle of the link that satisfies the final target position and the final target posture is obtained, if the convergence solution is not obtained at any interpolation point, the articulated robot at the interpolation point It will stop.
The present invention has been made in view of the above circumstances, and an object thereof is to provide an articulated robot that can move to reach a target position and a target posture.

Hereinafter, each means suitable for solving the above-described problems will be described with additional effects and the like as necessary.
(Means 1) An articulated robot according to means 1 is:
A joint arm in which a plurality of links are connected in series by a joint;
A drive motor that pivots each of the links about the axis of rotation of the joint relative to the adjacent link;
Motor driving means for supplying electric power to operate the driving motor;
A tip portion calculation means for calculating at least one of a current position and a current posture of the tip portion of the joint arm based on a rotation angle of each link with respect to the adjacent link;
The deviation of the current position or posture of the tip of the joint arm calculated by the tip calculation means with respect to the target position or posture of the tip of the joint arm is calculated, and the calculated deviation satisfies a predetermined condition Deviation judging means for judging whether or not to do,
Rotation angle updating means for calculating a correction angle of each link adjacent to the link based on the calculated deviation, and updating the rotation angle based on the calculated correction angle;
With
The motor driving means is
When the deviation determination means determines that the deviation satisfies the predetermined condition, the drive motor is operated based on the rotation angle updated by the rotation angle update means, and the links are adjacent to each other. Swivel relative to the link to correct the current position or current posture of the tip of the joint arm;
When the deviation determining means determines that the deviation does not satisfy the predetermined condition,
The rotation angle update means includes
Until the deviation is determined to satisfy the predetermined condition, the correction angle of each link adjacent to the link is calculated based on the deviation, and the rotation angle is repeatedly updated based on the correction angle. In an articulated robot,
The deviation determination means includes
When it is determined that the deviation does not satisfy the predetermined condition, the predetermined condition for the deviation calculated next time is relaxed, and it is determined whether or not the predetermined condition is satisfied when the deviation is relaxed. To do.

  Therefore, according to the articulated robot according to the means 1, when the convergence solution cannot be obtained, the rotation angle of the link can be easily obtained by relaxing the predetermined condition. The robot can move to reach the target position and target posture without stopping.

  In addition, even when it is difficult to obtain a convergent solution, the joint arm can be moved by obtaining the rotation angle of each link in a short time, and the operating efficiency can be improved. When the deviation determining means determines that the deviation does not satisfy the predetermined condition, the current position and current posture of the distal end of the joint arm newly calculated by the distal end calculating means is the actual distal end of the joint arm. It is not the position and posture of the part, but the position and posture of the tip of the joint arm for use in the calculation.

(Means 2) In the articulated robot according to means 1,
The deviation determination means includes
The predetermined condition for the deviation calculated based on the current posture and the target posture of the joint arm is not relaxed, and the predetermined condition for the deviation calculated based on the current position and the target position of the tip portion of the joint arm is not relaxed. Relax conditions only.
Therefore, according to the articulated robot according to the means 2, since the predetermined condition for the deviation calculated by the current position and the target position of the distal end portion of the joint arm is not relaxed, the distal end portion determines the position on the movement locus. There is no detachment and interference with other members at the tip can be avoided.

(Means 3) In the articulated robot according to means 1 or 2,
The deviation determination means includes
Determining whether the deviation is less than or equal to a predetermined value;
When the deviation determining means determines that the deviation is not less than the predetermined value, until the deviation is determined to be less than or equal to the predetermined value,
The rotation angle update means repeatedly updates the rotation angle,
If the deviation determining means determines that the deviation is not less than or equal to the predetermined value, the deviation calculated next time is decreased by a predetermined amount, and whether the deviation decreased by a predetermined amount is less than or equal to the predetermined value. Judge whether or not.

  The means 3 reduces the comparison condition between the reduced deviation and the predetermined value by reducing the deviation calculated next time by a predetermined amount. In other words, rather than reducing the comparison condition by increasing the predetermined value itself, the “predetermined condition for the next calculated deviation” in the above means is reduced by calculating the deviation as a value smaller than the actual deviation. is doing. Note that “the deviation is equal to or less than a predetermined value” in the means 3 is the “predetermined condition” in the above means.

As described above, according to the articulated robot according to the means 3, when it is determined that the deviation is not less than the predetermined value, the deviation is reduced by a predetermined amount. It can be easily obtained.
Here, reducing by a predetermined amount includes not only reducing the deviation by a certain amount but also reducing the deviation by a certain percentage.

(Means 4) In the articulated robot of means 3,
The deviation determination means includes
When it is determined that the deviation reduced by a predetermined amount is not less than the predetermined value, the deviation calculated next time is further reduced by a predetermined amount, and the deviation further reduced by a predetermined amount is not more than the predetermined value. Determine whether or not.

  Therefore, according to the articulated robot according to the means 4, when it is determined that the deviation reduced by the predetermined amount is not still below the predetermined value, the rotation angle of the link is further reduced by further reducing the deviation by the predetermined amount. Therefore, the rotation angle of each link can be obtained in a shorter time.

(Means 5) In the articulated robot according to means 2,
The deviation determination means includes
When it is determined that the deviation does not satisfy the predetermined condition, it is determined whether or not the deviation calculated from the current posture and the target posture of the distal end portion of the joint arm satisfies the predetermined condition. Instead, it is determined only whether the deviation calculated from the current position and the target position of the tip of the joint arm satisfies the predetermined condition.
Therefore, according to the multi-joint robot according to the means 5, in order to judge only whether or not the deviation calculated by the current position and the target position of the tip of the joint arm satisfies the predetermined condition, the deviation judging means The calculation time can be greatly reduced.

(Means 6) In the articulated robot of means 5,
The deviation determination means includes
If it is determined that the deviation does not satisfy the predetermined condition, the deviation based on the current posture and the target posture of the tip of the joint arm is not calculated, and the current position and the target of the tip of the joint arm are calculated. Only the deviation due to the position is calculated.
Therefore, according to the articulated robot according to the means 6, since the deviation between the current posture and the target posture of the tip of the joint arm is not calculated, the calculation time by the deviation determination means can be shortened and the memory capacity required for the calculation is reduced. it can.

(Means 7) In the articulated robot according to means 2,
The deviation determination means includes
If it is determined that the deviation does not satisfy the predetermined condition, is the deviation calculated by the current posture and the target posture of the tip of the joint arm zero, and whether the predetermined condition is satisfied? Judge whether or not.
Therefore, according to the articulated robot according to the means 7, it is possible to relax the predetermined condition for the deviation calculated by the current posture and the target posture of the tip of the joint arm without performing complicated calculations.

(Means 8) In the articulated robot of means 1 or 2,
The deviation determination means includes
Determining whether the deviation is less than or equal to a comparison value;
When the deviation determining means determines that the deviation is not less than the comparison value, until the deviation is determined to be less than the comparison value,
The rotation angle update means repeatedly updates the rotation angle,
When the deviation determining means determines that the deviation is not less than the comparison value, the deviation value is increased by a predetermined amount, and the deviation calculated next time is not more than the comparison value increased by a predetermined amount. Determine whether or not.

Therefore, according to the articulated robot according to the means 8, when the convergence solution cannot be obtained, it is determined whether the deviation is equal to or less than the comparison value increased by a predetermined amount. An angle convergence solution can be easily obtained.
Here, increasing by a predetermined amount includes a case where the comparison value is increased by a certain amount and a case where the comparison value is increased by a certain ratio.

<Embodiment 1>
An articulated robot according to Embodiment 1 of the present invention will be described with reference to FIGS. FIG. 1 schematically shows a joint arm 1 (corresponding to the joint arm of the present invention) forming the articulated robot according to the present embodiment by a skeleton diagram. The joint arm 1 is formed by connecting seven links 11a to 11g (corresponding to links of the present invention) in series by six joints 12a to 12f (corresponding to joints of the present invention). The joint arm 1 of the multi-joint robot according to the present embodiment is a robot having six degrees of freedom in which the links 11a to 11g can turn at the six joints 12a to 12f, but is not limited thereto.

  One end of the first link 11a is fixed to the floor surface FL, and the other end is connected to one side of the first joint 12a. One end of the second link 11b is connected to the other side of the first joint 12a, and one side of the second joint 12b is connected to the other end of the second link 11b. Hereinafter, the third link 11c, the fourth link 11d, the fifth link 11e, the sixth link 11f, and the seventh link 11g are respectively connected via the third joint 12c, the fourth joint 12d, the fifth joint 12e, and the sixth joint 16f. Are connected in order.

  As shown by an arrow 15a, the other side of the first joint 12a is rotatable about an axis extending in the vertical direction in FIG. 1, so that the second link 11b is adjacent to the second link 11b. The one link 11a can turn in the direction of the arrow 15a around the rotation axis of the first joint 12a.

  Further, the other side of the second joint 12b is rotatable with respect to one side about an axis extending in a direction perpendicular to the paper surface in FIG. 1, as indicated by an arrow 15b. 11c can turn in the direction of the arrow 15b around the rotation axis of the second joint 12b with respect to the adjacent second link 11b.

  Hereinafter, the third joint 12c, the fourth joint 12d, the fifth joint 12e, and the sixth joint 16f are also rotatable, and the fourth link 11d, the fifth link 11e, the sixth link 11f, and the seventh link 11g are also rotatable. , And can rotate in the directions of arrows 15c, 15d, 15e, and 15f around the rotation axes of the joints 12c, 12d, 12e, and 12f, respectively. Note that throughout the present application, the links 11a to 11g connected via the joints 12a to 12f are referred to as adjacent links 11a to 11g.

A first stepping motor 13a is attached to the first joint 12a, and the second link 11b is turned with respect to the first link 11a when electric power is supplied. Further, a second stepping motor 13b is attached to the second joint 12b, and the third link 11c is turned with respect to the second link 11b by being supplied with electric power.
Hereinafter, similarly, the stepping motors 13c, 13d, 13e, and 13f are respectively connected to the third joint 12c, the fourth joint 12d, the fifth joint 12e, and the sixth joint 16f (the stepping motors 13a to 13f are the drive motors of the present invention). And the link 11d, 11e, 11f, and 11g are turned by being supplied with electric power.

  A tool 14 is attached to the tip of the seventh link 11g. The tool 14 can turn in the direction of the arrow 15f around the rotation axis of the sixth joint 12f together with the seventh link 11g. The tool 14 includes a pair of holding pieces 14a and 14b extending so as to face each other. The sandwiching pieces 14a and 14b are swingable in the directions of arrows 14e and 14f around the pivot shafts 14c and 14d, respectively. The distal ends of the sandwiching pieces 14a and 14b (the distal end portion of the joint arm of the present invention) It is possible to grip a workpiece or the like by approaching each other.

  As described above, the articulated arm 1 rotates the links 11a to 11f by driving the stepping motors 13a to 13f, so that the rotation angles of the links 11a to 11f accumulate and work on the tool 14 at the distal end. The position and posture of the tip 14 can be made to coincide with the target position and target posture corresponding to the work content.

  Next, the electrical configuration of the articulated robot centering on the controller 2 will be described with reference to FIG. The above-described stepping motors 13a to 13f are electrically connected to the controller 2 via motor driving units 3a to 3f (corresponding to the motor driving means of the present invention), respectively. The motor driving units 3a to 3f are motor drivers formed by power transistors, relays, or the like, and supply electric power according to a driving signal from the controller 2 to drive the stepping motors 13a to 13f by a necessary angle.

  The pulse encoders 16a to 16f are built in the stepping motors 13a to 13f, respectively, and are connected to the controller 2. The pulse encoders 16a to 16f detect the rotation angles of the respective stepping motors 13a to 13f, thereby detecting the rotation angles of the links 11a to 11g relative to the adjacent links 11a to 11g, and detecting signals. Can be sent to the controller 2. Instead of providing the pulse encoders 16a to 16f in the stepping motors 13a to 13f, sensors that can directly detect the rotation angles of the links 11a to 11g may be attached to the links 11a to 11g or the joints 12a to 12f.

  The input device 4 connected to the controller 2 is an operation panel having a monitor screen and can be operated by the user. The input device 4 is provided with a power switch for the articulated robot, and the final target position and final target posture of the tip of the tool 14 at the tip of the joint arm 1, the position and posture of the tip of the tool 14 at the interpolation point, etc. Can be input to the controller 2.

  The memory 5 is formed of a volatile memory and a non-volatile memory, is connected to the controller 2, and supplies various stored information to the controller 2. The program storage unit (PM) 51 of the memory 5 stores an operation program for operating the joint arm 1, a forward kinematics calculation execution program, a Jacobian matrix calculation execution program, a pseudo inverse matrix calculation execution program, a rotation angle calculation execution program, and the like. is doing.

Further, the data storage unit (DM) 52 includes operation data for operating the joint arm 1, data for linear interpolation between the current position of the tip of the tool 14 and the final target position, and pulse encoders 16a to 16a. Data for detecting the rotation angle of each link 11a to 11g from the detection signal of 16f is stored.
Further, the coefficient storage unit (CM) 53 stores coefficient data and the like for determining whether or not the deviation satisfies a predetermined condition.

The tool driver 6 is connected to an electric motor (not shown) for operating the holding pieces 14a and 14b of the controller 2 and the tool 14. The tool driver 6 is formed by a motor relay, a transistor module, or the like, and drives the electric motor based on an operation signal from the controller 2 to operate the holding pieces 14a and 14b of the tool 14.
The control unit 21 of the controller 2 is formed by a CPU and controls each calculation unit based on an operation on the input device 4 and transmits to the motor drive units 3 a to 3 f to operate the joint arm 1.

Further, the forward kinematics calculation unit 22 (corresponding to the tip calculation unit of the present invention) is controlled by the control unit 21, and each link 11a to 11f detected by the pulse encoders 16a to 16f according to the specifications of the joint arm 1. Based on the rotation angle of 11 g, the current position and current posture of the tip of the tool 14 are calculated by forward kinematics.
Further, the deviation determination unit 23 (corresponding to the deviation determination means of the present invention) is controlled by the control unit 21, and the current position and posture of the tool 14 calculated by the forward kinematics calculation unit 22 of the tool 14. A deviation with respect to the target position and the target posture is calculated, and it is determined whether or not the calculated deviation satisfies a predetermined condition.

Further, the matrix calculation unit 24 is controlled by the control unit 21, and from the detected rotation angle of each link 11 a to 11 g and the current position and current posture of the tip of the tool 14 calculated by the forward kinematics calculation unit 22, The Jacobian matrix and its pseudo inverse matrix are calculated.
Further, the rotation angle update unit 25 (corresponding to the rotation angle update unit of the present invention) is controlled by the control unit 21, and based on the deviation calculated by the deviation determination unit 23, the link 11a adjacent to each link 11a to 11g. Calculate correction angles for ˜11 g and update these rotation angles.

Next, based on FIG. 3 thru | or FIG. 6, the operation control method by the controller 2 of the articulated robot by this embodiment is demonstrated.
First, the final target position (goal point) F at the tip of the tool 14 in three dimensions and the posture of the tool 14 at the final target position F are input to the controller 2 by a user operation on the input device 4. . In consideration of avoiding interference with obstacles around the joint arm 1, the control unit 21 considers the current position (start point) S of the tip of the tool 14 and the final target of the tip of the tool 14 as shown in FIG. 3. A movement locus H of the tip of the tool 14 is formed in three dimensions so as to connect the position F.

Next, a plurality of interpolation points P1 to Pn are provided on the movement locus H by linearly interpolating the movement locus H on which the control unit 21 is formed. Alternatively, the user may operate the input device 4 to form the interpolation points P1 to Pn by linearly interpolating the movement locus H. At each of the interpolation points P1 to Pn, the position and posture of the tip of the tool 14 (target position and target posture: Rw ^ref ) are determined in consideration of avoidance of interference with surrounding obstacles.

Next, in order to move the tip of the tool 14 to the nearest interpolation points P1 to Pn from the current position, a convergence operation for obtaining an angle: θ ^now for rotating each link 11a to 11g with respect to the adjacent links 11a to 11g is performed. Do. This rotation angle: θ ^now needs to be a value that allows the tip of the tool 14 to satisfy the target position and target posture: Rw ^ref on the interpolation points P1 to Pn.
For example, consider a case where the tip of the tool 14 is moved to the interpolation point P1 when the tip of the tool 14 is currently at the start point S. As shown in FIG. 5, first, the input device 4 inputs a target position and target posture: Rw ^ref on the interpolation point P1. Further, the current rotation angle: θ ^now with respect to the links 11a to 11g adjacent to the links 11a to 11g on the start point S is input to the controller 2 by the pulse encoders 16a to 16f (step S501).

Next, k in the memory counter of the control unit 21 is set to 0 (step S502), and then k is incremented by 1 (step S503). Next, the forward kinematics calculation unit 22 calculates the current position and the current posture: Rw ^now of the tip of the tool 14 based on the current rotation angle: θ ^now of each link 11a to 11g (step S504, in FIG. 4). (Indicated by reference numeral 75).

Next, from the target position and target posture of the tip of the tool 14: Rw ^ref and the current position and current posture of the tip of the tool 14: Rw ^now , the tool 14 of the current position and current posture of the tool 14: Rw ^now the tip of the target position and target posture: deviation to ^{Rw ref: ΔRw = Rw ref -Rw} now is calculated (step S505, indicated by reference numeral 71 in FIG. 4). More specifically, as indicated by reference numeral 72 in FIG. 4, the deviation: ΔRw is composed of a position deviation parameter: ΔPw and an attitude deviation parameter: ΔQw (hereinafter referred to as position deviation: ΔPw, attitude deviation: ΔQw, respectively). (ΔRw = (ΔPw, ΔQw)). Further, if the current position of the tip of the tool 14 is Pw ^now , the target position is Pw ^ref , the current posture is Qw ^now , and the target posture is Qw ^ref , ΔPw = Pw ^ref −Pw ^now and ΔQw = Qw ^ref −Qw ^now. .

Next, when the deviation: ΔRw is compared with a predetermined comparison value: ε, and the deviation: ΔRw is equal to or less than the comparison value: ε (corresponding to the predetermined condition of the present invention), it is determined that a convergence solution has been obtained (step S506). . More specifically, the positional deviation: ΔPw and the attitude deviation: ΔQw constituting the deviation: ΔRw are compared with predetermined comparison values: εp, εQ, respectively. If the position deviation: ΔPw (= Pw ^ref −Pw ^now ) is the comparison value: εp or less and the attitude deviation: ΔQw (= Qw ^ref −Qw ^now ) is the comparison value: εQ or less, the convergence solution is determined. .

As a result, the motor drive units 3a to 3f operate the stepping motors 13a to 13f based on the rotation angle updated by the rotation angle update unit 25: θ ^now so that each link 11a to 11g is changed to the adjacent link 11a to 11g. Then, the current position of the tip of the tool 14 and the current posture: Rw ^now are corrected, and the movement to the interpolation point P1 is completed (step S508). After the current position and current posture: Rw ^{now at} the tip of the tool 14 are corrected, the control by the main routine is finished.
When the convergence solution is obtained in the first round of the main routine, in step S508, the stepping motors 13a to 13f are operated based on the input current rotation angle: θ ^now. Does not turn.

  On the other hand, when the deviation: ΔRw is larger than the comparison value: ε (when the position deviation: ΔPw is larger than the comparison value: εp or the posture deviation: ΔQw is larger than the comparison value: εQ), the convergence solution is obtained. In step S507, it is determined whether the calculated number of correction angles: Δθ of the links 11a to 11g has exceeded N times. If the calculated number of correction angles: Δθ of the links 11a to 11g is N or less, the process proceeds to step S509.

Next, based on the relational expression: J · θ ^now = Rw ^now , the current rotation angle: θ ^now of each link 11a to 11g, and the current position and the current position of the tip of the tool 14 obtained by the forward kinematics calculation unit 22 The Jacobian matrix: J is calculated using the posture: Rw ^now (step S509). As described above, the Jacobian matrix: J is a matrix of functions for converting the rotation angle: θ ^now of each link 11a to 11g into the current position and the current attitude of the tip of the tool 14: Rw ^now. . In the case of the present embodiment, the Jacobian matrix: J is a 6 × 6 (6 × 6) matrix.

Next, the obtained Jacobian: from J, relationship: based on the ^{J ♯ = J T · (J} · J T) -1, Jacobian matrix: J pseudo inverse matrix: J ^♯ is determined (step S510) . In the above equation, for example, A ^T represents a transposed matrix of the matrix A, and A ⁻¹ represents an inverse matrix of the matrix A.
Next, from the pseudo inverse matrix: J ^# and the deviation: ΔRw, the correction angle: Δθ of the links 11a to 11g is obtained based on the relational expression: Δθ = J ^# · ΔRw (step S511, denoted by reference numeral 73 in FIG. 4). The rotation angle: θ ^now of the links 11a to 11g is updated with the obtained correction angle: Δθ (step S512, indicated by reference numeral 74 in FIG. 4).

Next, k in the memory counter of the control unit 21 is incremented by 1 (step S503), and the arithmetic processing after step S504 is executed again. That is, the forward kinematics calculation unit 22 newly calculates the current position of the tip of the tool 14 and the current posture: Rw ^now based on the rotation angle: θ ^now of the links 11a to 11g updated last time. The rotation angle update unit 25 repeatedly updates the rotation angle: θ ^now until it is determined that the newly calculated deviation: ΔRw is equal to or less than the comparison value: ε (until the convergence solution is obtained).

After it is determined that the deviation: ΔRw is larger than the comparison value: ε, the control by the subroutine shown in FIG. 6 is started when it is determined that the correction angle: Δθ of the links 11a to 11g exceeds N times. Is done.
In the subroutine shown in FIG. 6, first, U in the memory counter of the control unit 21 is set to 0 (step S601), and then U is incremented by 1 to start the first-round routine (step S602). ). Further, M in the memory counter of the control unit 21 is set to 0 (step S603), and then M is incremented by 1 (step S604).
Next, as in the case of the main routine described above, the Jacobian matrix: J is calculated (step S605), and the pseudo inverse matrix: J ^{# of the} Jacobian matrix: J is obtained from the obtained Jacobian matrix: J (step S605). S606).

Next, from the pseudo inverse matrix: J ^# and the deviation: ΔRw calculated in the main routine, the correction angle: Δθ of the links 11a to 11g is obtained based on the relational expression: Δθ = J ^# · ΔRw (step S607, The rotation angle: θ ^now of the links 11a to 11g is updated with the obtained correction angle: Δθ (step S608, indicated by reference numeral 79 in FIG. 4).
Next, the forward kinematics calculation unit 22 calculates the current position and the current posture: Rw ^now of the tip of the tool 14 based on the newly updated rotation angle: θ ^now of the links 11a to 11g (step S609, FIG. 4 at 80).

Next, from the target position and target posture of the tip of the tool 14: Rw ^ref and the current position and current posture of the tip of the tool 14: Rw ^now , the tool at the current position and current posture of the tool 14: Rw ^now The deviation: ΔRw with respect to the target position and target posture: Rw ^ref of the tip 14 is calculated (step S610, indicated by reference numeral 76 in FIG. 4).
In this case, the deviation: ΔRw is composed of a position deviation: ΔPw and an attitude deviation: ΔQw ′. Further, assuming that the current position of the tip of the tool 14 is Pw ^now and the target position is Pw ^ref , the positional deviation is ΔPw = Pw ^ref −Pw ^now .

However, assuming that the current posture at the tip of the tool 14 is Qw ^now and the target posture is Qw ^ref , the posture deviation: ΔQw ′ is calculated by the relational expression: ΔQw ′ = 0.9 · (Qw ^ref −Qw ^now ) ( (Indicated by reference numeral 77 in FIG. 4). That is, as in the case of the main routine, if ΔQw = Qw ^ref −Qw ^now , then ΔQw ′ = 0.9 · ΔQw. Here, 0.9 is a weighting factor: Wq, and is set to 0.9 in the first round of the subroutine.

Next, the positional deviation: ΔPw and the attitude deviation: ΔQw ′ constituting the deviation: ΔRw are compared with predetermined comparison values: εp, εQ, respectively. When the position deviation: ΔPw (= Pw ^ref −Pw ^now ) is a comparison value: εp or less and the posture deviation: ΔQw ′ (= 0.9 · (Qw ^ref −Qw ^now )) is a comparison value: εQ or less, It is assumed that the convergence solution has been obtained (step S611). As can be seen, in the control by the subroutine, the comparison condition for the position deviation: ΔPw comparison value: εp is not relaxed compared to the control by the main routine, but the posture deviation: ΔQw ′ is decreased. : Comparative value of ΔQw ′: The comparison condition for εQ is relaxed.

When the convergence solution is obtained, the motor driving units 3a to 3f operate the stepping motors 13a to 13f on the basis of the rotation angle: θ ^now updated by the rotation angle update unit 25, so that the links 11a to 11g are adjacent to each other. It is made to turn with respect to the links 11a to 11g, and the current position and the current posture: Rw ^now at the tip of the tool 14 are corrected (step S614). After the current position and current posture at the tip of the tool 14 are corrected: Rw ^now , the control by the subroutine ends.

  On the other hand, when the position deviation: ΔPw is larger than the comparison value: εp or the posture deviation: ΔQw ′ is larger than the comparison value: εQ, the convergence solution is not obtained, and the correction angle of the links 11a to 11g: Δθ It is determined whether or not the number of calculations exceeds L (step S612). Correction angle of links 11a to 11g: When the number of times Δθ is calculated is L or less, M is further incremented by one, and the above-described calculation is repeated again until a convergent solution is obtained (steps S604 to S611, in FIG. 4). Indicated by reference numerals 76 to 80).

  When M exceeds L without obtaining a convergence solution (step S612), it is determined whether U exceeds S (step S613). If U in the memory counter is equal to or less than S, U Is incremented by one, the second routine of the subroutine is started, and the above-described calculation is repeated again (steps S604 to S612).

In second round routines subroutines, position deviation:? Pw similarly relationship with the initial routine: ΔPw = Pw ^ref -Pw but is calculated by ^now, attitude deviation: DerutaQw' is relation: ΔQw' = 0 .9 ² · (Qw ^ref −Qw ^now ) (step S610, indicated by reference numeral 82 in FIG. 4). That is, the weighting factor: defined as Wq = 0.9 ^2.

Also in the second round of the subroutine, the position deviation: ΔPw and the attitude deviation: ΔQw ′ are respectively compared with the comparison values: εp, εQ. Position ^{deviation: ΔPw (= Pw ref -Pw now} ) comparison value: .epsilon.p below and attitude ^{deviation: ΔQw' (= 0.9 2 · (} Qw ref -Qw now)) is compared values: IpushironQ if: It is assumed that a convergent solution has been obtained (step S611). As can be seen, in the control by the routine of the second round of the subroutine, the convergence calculation is performed in a state in which the comparison condition for the posture deviation: ΔQw ′ comparison value: εQ is further relaxed compared to the control by the main routine. .

Thereafter, in the subroutine, the attitude deviation: ΔQw ′ in the routine of the U-th round is obtained by the relational expression: ΔQw ′ = 0.9 ^U · (Qw ^ref −Qw ^now ) (indicated by reference numeral 87 in FIG. 4). The rotation angle update unit 25 repeatedly corrects until it is determined that the position deviation: ΔPw is equal to or less than the comparison value: εp and the posture deviation: ΔQw ′ is equal to or less than the comparison value: εQ (until the convergence solution is obtained). Angle: Δθ is calculated.

  It is determined that the position deviation: ΔPw is larger than the comparison value: εp, or the posture deviation: ΔQw ′ is larger than the comparison value: εQ (step S611), and the correction angle of the links 11a to 11g: the number of calculations of Δθ is L. After it is determined that the number of times has exceeded (step S612), if it is further determined that U in the memory counter has exceeded S, the converged solution is not obtained and the subroutine ends (step S613).

According to the present embodiment, the deviation determination unit 23 determines that the deviation: ΔRw is greater than or equal to the comparison value: ε, and the number of calculations of the correction angle: Δθ by the rotation angle update unit 25 exceeds N times. For the next time (initial routine in the subroutine), the comparison condition for deviation: ΔRw is compared with ε and the comparison condition for ε is relaxed, and it is determined whether deviation: ΔRw satisfies the relaxed comparison condition.
Therefore, when the number of times of calculation of the correction angle: Δθ exceeds N without obtaining the convergence solution, the rotation angle: θ ^now of the links 11a to 11g can be easily obtained by relaxing the comparison condition. Therefore, the joint arm 1 can move so as to reach the target position and target posture without stopping in the middle of the movement locus H.

Further, even when it is difficult to obtain a convergent solution, the joint arm 1 can be moved in a short time by obtaining the rotation angle: θ ^now of each link 11a to 11g, and the operation efficiency can be improved.
Further, only the comparison condition for the posture deviation: ΔQw calculated from the current posture: Qw ^now and the target posture: Qw ^ref of the tool 14 is relaxed, and the current position: Pw ^now and the target position: Pw of the tool 14 tip. ^{Since the} predetermined condition for the positional deviation calculated by ^ref : ΔPw is not relaxed, the tip of the tool 14 does not deviate from the position on the movement locus H, and interference with other members of the tip can be avoided. .

Further, when it is determined that the deviation: ΔRw is not equal to or less than the comparison value: ε, and the number of calculation of the correction angle: Δθ exceeds N times, the posture deviation: ΔQw ′ calculated next time is decreased by a predetermined amount. In order to determine whether the attitude deviation: ΔQw ′ reduced by a predetermined amount is equal to or less than the comparison value: εQ, the rotation angle of the links 11a to 11g: θ The convergence solution of ^now can be easily found.

Further, even if the convergence calculation is performed using the deviation: ΔRw constituted by the posture deviation: ΔQw ′ reduced by a predetermined amount, it is calculated next time when it is determined that the deviation: ΔRw is not equal to or less than the comparison value: ε. Posture deviation: ΔQw ′ is further decreased by a predetermined amount. Further, a convergence calculation is performed using the posture deviation: ΔQw ′ further reduced by a predetermined amount, and it is further determined whether or not the posture deviation: ΔQw ′ is equal to or less than the comparison value: εQ. rotation angle: theta ^now for tends Motomari, the rotation angle of each link 11a~11g more in a short time: it is possible to obtain the theta ^now.

<Embodiment 2>
Based on FIG. 7, an articulated robot according to Embodiment 2 of the present invention will be described. Only the configuration different from that of the first embodiment will be described below. In the control by the subroutine of the present embodiment, the positional deviation: ΔPw of the tip of the tool 14 is calculated from the target position: Pw ^ref of the tip of the tool 14 and the current position of the tip of the tool 14: Pw ^now. ΔPw is set as the deviation of the tip of the tool 14: ΔRw (step S708).

That is, in this embodiment, when calculating the deviation: ΔRw, the deviation of the tip of the tool 14: ΔQw based on the target posture of the tip of the tool 14: Qw ^ref and the current posture of the tip of the tool 14: Qw ^now is calculated. It has not been.
Accordingly, only the positional deviation: ΔPw is compared with the predetermined comparison value: εp, and if the positional deviation: ΔPw is equal to or less than the comparison value: εp, it is determined that the convergence solution has been obtained (step S709). On the other hand, the posture deviation at the tip of the tool 14: ΔQw and the comparison value: εQ are not compared.

According to this embodiment, attitude deviation ΔQw comparison value: IpushironQ without determining in whether or less, the current position Pw ^now and the target position Pw position deviation ΔPw calculated by ^ref of the tip of the tool 14, compared Since it is determined only whether or not the value is equal to or less than εp, the calculation time by the deviation determination unit 23 can be greatly shortened.
Further, the deviation determination unit 23 calculates the position deviation ΔPw between the current position Pw ^now and the target position Pw ^ref without calculating the attitude deviation ΔQw based on the current attitude Qw ^now and the target attitude Qw ^ref at the tip of the tool 14. As a result, the memory capacity of the rotation angle update unit 25 required for calculation can be reduced.

In the case of the present embodiment, the Jacobian matrix: J and its pseudo inverse matrix J ^# calculated in steps S704 and S705 are 3 × 6 matrices (3 × 6), respectively.
In the present embodiment, the deviation determination unit 23 calculates the position deviation ΔPw, calculates the posture deviation ΔQw based on the current posture Qw ^now and the target posture Qw ^ref of the tip of the tool 14, and compares the posture deviation ΔQw with the comparison value. Only the comparison between the position deviation ΔPw and the comparison value εp may be performed without comparing with εQ.

<Embodiment 3>
An articulated robot according to Embodiment 3 of the present invention will be described with reference to FIG. Only the configuration different from that of the first embodiment will be described below. In the control by the subroutine of the present embodiment, as in the first embodiment, the tip deviation: ΔRw of the tool 14 is composed of the position deviation: ΔPw and the posture deviation: ΔQw of the tip of the tool 14.
The position deviation ΔPw at the tip of the tool 14 is calculated from the target position Pw ^ref at the tip of the tool 14 and the current position Pw ^now at the tip of the tool 14. On the other hand, the posture deviation ΔQw at the tip of the tool 14 is always 0 (step S808).

Therefore, the deviation formed by the position deviation: ΔPw calculated from the target position of the tip of the tool 14: Pw ^ref and the current position of the tip of the tool 14: Pw ^now and the attitude deviation: ΔQw always set to 0: If ΔRw is compared with a predetermined comparison value: ε and deviation: ΔRw is less than or equal to the comparison value: ε, it is determined that a convergent solution has been obtained (step S809). Therefore, if the positional deviation: ΔPw is substantially equal to or less than the comparison value: εp, it is determined that the convergence solution has been obtained.
According to this embodiment, in order to determine whether or not the deviation: ΔRw formed by the position deviation: ΔPw and the attitude deviation: ΔQw always set to 0 is equal to or less than the comparison value: ε, a complicated calculation is performed. Without performing this, it is possible to relax the comparison condition with respect to the attitude deviation ΔQw of the tip of the tool 14.

<Embodiment 4>
Based on FIG. 9, an articulated robot according to Embodiment 4 of the present invention will be described. Only the configuration different from that of the first embodiment will be described below. In the control by the subroutine of the present embodiment, as in the first embodiment, the tip deviation: ΔRw of the tool 14 is composed of the position deviation: ΔPw and the posture deviation: ΔQw of the tip of the tool 14. Deviation at the tip of the tool 14: ΔRw is obtained from the current position and current posture at the tip of the tool 14: Rw ^now and the target position and target posture: Rw ^ref by a relational expression: ΔRw = Rw ^ref −Rw ^now ( Step S910).

In the first round of the subroutine in which U in the memory counter is 1, the deviation: ΔRw is compared with a predetermined comparison value: 1.1 × ε, and the deviation: ΔRw is a comparison value: 1.1 × ε or less. In this case, it is assumed that a convergence solution has been obtained (step S911). Specifically, the positional deviation: ΔPw and the attitude deviation: ΔQw constituting the deviation: ΔRw are compared with predetermined comparison values: 1.1 × εp and 1.1 × εQ, respectively. Position deviation: ΔPw (= Pw ^ref −Pw ^now ) is a comparison value: 1.1 × εp or less and posture deviation: ΔQw (= Qw ^ref −Qw ^now ) is a comparison value: 1.1 × εQ or less It is said that the convergence solution has been obtained.

In the second and subsequent U-routines of the subroutine, the convergence calculation is performed with the comparison value further increased by setting the comparison value to 1.1 ^U × ε. In the U-th routine of the subroutine of the present embodiment, the position deviation: ΔPw (= Pw ^ref −Pw ^now ) is less than the comparison value: εp and the attitude deviation: ΔQw (= Qw ^ref −Qw ^now ) is When the comparison value is 1.1 ^U × εQ or less, a convergent solution may be obtained.

According to the present embodiment, when it is determined that the deviation: ΔRw is not equal to or less than the comparison value ε and the number of calculations of the correction angle Δθ exceeds N, the comparison value ε is increased by a predetermined amount, In order to determine whether or not the calculated deviation: ΔRw is equal to or less than the comparison value: 1.1 × ε increased by a predetermined amount, a converged solution of the rotation angles: θ ^now of the links 11a to 11g is obtained by a simple calculation. It can be easily obtained.

<Other embodiments>
The present invention is not limited to the above-described embodiments, and can be modified or expanded as follows.
The articulated robot according to the present invention is applicable as long as the tip of the joint arm 1 has a three-dimensional position and a two-dimensional posture and is a five-axis or more robot.
In the main routine described above, when the number of times of calculation of the correction angle: Δθ exceeds the predetermined number, the predetermined condition for the posture deviation: ΔQw of the tool 14 is not relaxed in the subroutine, and the position deviation of the tip of the tool 14 is not relaxed. : Only the predetermined condition for ΔPw may be relaxed.

Schematic of the articulated arm of the articulated robot of the present invention Block diagram showing the electrical configuration of an articulated robot Schematic showing the method of forming interpolation points on the movement trajectory Block diagram showing how to find the convergence solution for the link correction angle The flowchart showing the main routine of the control method of the articulated robot by Embodiment 1. Flow chart showing control method subroutine The flowchart showing the subroutine of the control method by Embodiment 2. The flowchart showing the subroutine of the control method by Embodiment 3 The flowchart showing the subroutine of the control method by Embodiment 4

Explanation of symbols

  In the drawings, 1 is a joint arm, 3a to 3f are motor drive units (motor drive means), 11a to 11g are links, 12a to 12f are joints, 13a to 13f are stepping motors (drive motors), and 14 is a tool (joint arm). , 22 is a forward kinematics computing unit (tip computing unit), 23 is a deviation determining unit (deviation judging unit), and 25 is a rotation angle updating unit (rotation angle updating unit).

Claims (8)

A joint arm in which a plurality of links are connected in series by a joint;
A drive motor that pivots each of the links about the axis of rotation of the joint relative to the adjacent link;
Motor driving means for supplying electric power to operate the driving motor;
A tip portion calculation means for calculating at least one of a current position and a current posture of the tip portion of the joint arm based on a rotation angle of each link with respect to the adjacent link;
The deviation of the current position or posture of the tip of the joint arm calculated by the tip calculation means with respect to the target position or posture of the tip of the joint arm is calculated, and the calculated deviation satisfies a predetermined condition Deviation judging means for judging whether or not to do,
Rotation angle updating means for calculating a correction angle of each link adjacent to the link based on the calculated deviation, and updating the rotation angle based on the calculated correction angle;
With
The motor driving means is
When the deviation determination means determines that the deviation satisfies the predetermined condition, the drive motor is operated based on the rotation angle updated by the rotation angle update means, and the links are adjacent to each other. Swivel relative to the link to correct the current position or current posture of the tip of the joint arm;
When the deviation determining means determines that the deviation does not satisfy the predetermined condition,
The rotation angle update means includes
Until the deviation is determined to satisfy the predetermined condition, the correction angle of each link adjacent to the link is calculated based on the deviation, and the rotation angle is repeatedly updated based on the correction angle. In an articulated robot,
The deviation determination means includes
When it is determined that the deviation does not satisfy the predetermined condition, the predetermined condition for the deviation calculated next time is relaxed, and it is determined whether or not the predetermined condition is satisfied when the deviation is relaxed. An articulated robot characterized by The deviation determination means includes
The predetermined condition for the deviation calculated based on the current posture and the target posture of the joint arm is not relaxed, and the predetermined condition for the deviation calculated based on the current position and the target position of the tip portion of the joint arm is not relaxed. The articulated robot according to claim 1, wherein only a condition is relaxed. The deviation determination means includes
Determining whether the deviation is less than or equal to a predetermined value;
When the deviation determining means determines that the deviation is not less than the predetermined value, until the deviation is determined to be less than or equal to the predetermined value,
The rotation angle update means repeatedly updates the rotation angle,
If the deviation determining means determines that the deviation is not less than or equal to the predetermined value, the deviation calculated next time is decreased by a predetermined amount, and whether the deviation decreased by a predetermined amount is less than or equal to the predetermined value. The articulated robot according to claim 1, wherein it is determined whether or not. The deviation determination means includes
When it is determined that the deviation reduced by a predetermined amount is not less than the predetermined value, the deviation calculated next time is further reduced by a predetermined amount, and the deviation further reduced by a predetermined amount is not more than the predetermined value. The articulated robot according to claim 3, wherein it is determined whether or not. The deviation determination means includes
When it is determined that the deviation does not satisfy the predetermined condition, it is determined whether or not the deviation calculated from the current posture and the target posture of the distal end portion of the joint arm satisfies the predetermined condition. 3. The articulated robot according to claim 2, wherein it is determined whether or not the deviation calculated based on a current position and a target position of the tip of the joint arm satisfies the predetermined condition. . The deviation determination means includes
If it is determined that the deviation does not satisfy the predetermined condition, the deviation based on the current posture and the target posture of the tip of the joint arm is not calculated, and the current position and the target of the tip of the joint arm are calculated. 6. The articulated robot according to claim 5, wherein only a deviation due to the position is calculated. The deviation determination means includes
If it is determined that the deviation does not satisfy the predetermined condition, is the deviation calculated by the current posture and the target posture of the tip of the joint arm zero, and whether the predetermined condition is satisfied? The articulated robot according to claim 2, wherein it is determined whether or not. The deviation determination means includes
Determining whether the deviation is less than or equal to a comparison value;
When the deviation determining means determines that the deviation is not less than the comparison value, until the deviation is determined to be less than the comparison value,
The rotation angle update means repeatedly updates the rotation angle,
When the deviation determining means determines that the deviation is not less than the comparison value, the deviation value is increased by a predetermined amount, and the deviation calculated next time is not more than the comparison value increased by a predetermined amount. The articulated robot according to claim 1, wherein it is determined whether or not.

Images