JP2012192518A - Device and method for controlling redundant robot having redundant joint - Google Patents

Abstract

PROBLEM TO BE SOLVED: To provide a method for controlling a redundant robot in which a set portion is kept correctly at an objective fixed position and then an angle of each joint is changed.SOLUTION: The number of the residual joints excepting for a redundant joint among first to fourth joints 9-12 is three, and a subject point Ph is fixed at a three-dimensional position. Each of angles q1-q4 of the first to fourth joints 9-12 is calculated on the basis of: the three-dimensional position (phx, phy, phz) of the subjective point Ph; forward conversion relational expressions for calculating the three-dimensional position of the subjective point Ph by using each of angles q1-q4 of the first, second, third and fourth joints 9-12 as a variable; and the redundant angle of the redundant joint, so that the subjective point is fixed at the three-dimensional position. As a result, ternary simultaneous equations having three unknown values can be solved analytically and the angles q1-q4 of the first to fourth joints 9-12 can correctly be calculated when the subject point Ph is disposed at the predetermined three-dimensional position.

Description

  The present invention relates to a control device for controlling an articulated robot having a plurality of joint parts including redundant joint parts, and a control method for controlling the articulated robot.

  For example, Patent Document 1 discloses a conventional technique related to a control method for an articulated robot having seven or more joint portions. In this conventional technique, the setting portion set for the multi-joint robot having redundancy is maintained at a fixed position set in advance, and a small one of the redundant joint portions of any one of the plurality of axes constituting the robot. It can be moved. When the operator slightly moves the redundant joint part, the robot control apparatus minutely moves the remaining joint parts other than the redundant joint part so that the setting portion is maintained at the fixed position.

  In the prior art disclosed in Patent Document 1, the amount of movement of each remaining joint other than the redundant joint is determined based on the Jacobian matrix. Here, the Jacobian matrix is a function indicating the relationship between the minute change of the set portion and the minute change of each joint portion of the robot. The prior art robot control device sets the minute change of the set part to zero, and the minute change of the redundant set joint part is known in the matrix indicating the minute change of each joint part of the robot, and the minute change of each remaining joint part is known. Is obtained to determine the amount of minute movement of each remaining joint that should maintain the set portion within the fixed position range.

Japanese Patent No. 3379540

  In the related art, an arbitrary joint part is set to be selectable as a redundant joint part among a plurality of joint parts, and the minute movement amount of each remaining joint part other than the redundant joint part is obtained based on the Jacobian matrix. If any joint part can be selected as a redundant joint part, it is difficult to obtain an exact solution for the movement amount of each joint part, and an approximate solution is obtained. If each joint portion is determined based on the approximate solution, there is a problem that the set portion cannot be accurately maintained at the target fixed position when the redundant setting joint portion is displaced.

  The amount of deviation of the set part becomes conspicuous when the redundant joint part moves at a high speed or a long distance. If the setting part is shifted, a problem may occur in the robot operation. Further, when the number of times of convergence calculation is increased in order to prevent the position shift of the set portion, the movement amount of each joint portion cannot be obtained in a short time.

  For example, when a transport object is transported by a redundant robot, position teaching is performed so that the robot arm does not collide with an obstacle. At the time of position teaching, the position and orientation of the object to be transported can be accurately maintained, and the arm near the obstacle cannot be finely moved as a redundant joint, which may cause a transport failure. In addition, for example, for a robot having redundancy, if the joint part to be selected as the redundant joint part is sequentially changed from among a plurality of joint parts, and the minute movement is repeated for each redundant joint part, the setting part shift method becomes complicated, It becomes difficult to return the position of the setting portion to the previous state.

  Therefore, an object of the present invention is to provide a control method and a control apparatus for controlling a redundant robot so that the angle of each joint portion of the robot can be changed while accurately maintaining the setting portion at a target fixed position. is there.

The present invention includes a plurality of joint portions that are connected to each other via an arm body so as to be angularly displaceable and arranged in series, and among the plurality of joint portions, a setting portion that is set to a joint portion at one end portion in the series direction, A controller for an articulated robot in which there are a plurality of combinations of angles that can be taken by the plurality of joints in a fixed state that indicates at least one of a predetermined position and posture,
A fixed condition input means for inputting a fixed condition for fixing the setting portion;
A redundant joint input means for inputting redundant joints, wherein the number of remaining joints excluding the redundant joints among the plurality of joints is the number of degrees of freedom in which the set part is constrained according to a fixed condition Redundant joint portion input means in which at least one of the plurality of joint portions is input as a redundant joint portion so as to match
Redundant angle input means for inputting the angle of the redundant joint as a redundant angle;
Based on the fixed condition input by the fixed condition input means and the redundant angle input by the redundant angle input means, a relational expression for obtaining the position or posture of the set portion using each angle of the plurality of joints as a variable is Unraveling, angle calculation means for calculating the angles of a plurality of joints, each of which should be fixed under fixed conditions,
An articulated robot control apparatus comprising: output means for giving a result of computation calculated by the angle computing means to the articulated robot.

In the present invention, as a fixed condition, a condition in which the three degrees of freedom of the setting portion with respect to the joint portion at the other end in the series direction is input,
The redundant joint portion input means includes one joint portion of two joint portions arranged from one end portion in the series direction toward the other end portion in the series direction, and a series portion from the other end portion in the series direction. One of the two joints arranged toward one end in the direction is configured to be selectable as a redundant joint.

  Further, in the present invention, when the angle calculation means determines that the redundant joint portion reaches the movable limit while angularly displacing the redundant joint portion from the state before the input to the redundant angle input to the redundant angle input means. The angle of the joint part in a state where the movable limit is reached is calculated.

  Further, in the present invention, when the angle calculation means determines that the redundant joint portion escapes from the movable limit while the redundant joint portion is angularly displaced to the redundant angle input to the redundant angle input means, Of the two combinations that can be taken by the angles of the joints other than the redundant joint, a combination of angles different from the combination of angles obtained before reaching the movable limit is calculated.

  The present invention also constrains the set part under a fixed condition based on a Jacobian matrix that converts the angle change of the plurality of joints into a position change of the set part and the redundant angle input to the redundant angle input means. Further, when the redundant joint portion is angularly displaced to a redundant angle, a prediction notification unit that predicts the angular displacement direction of the plurality of joint portions and notifies the prediction result is further provided.

  The present invention is further characterized by further comprising a warning notification means for notifying a determination result when it is determined that any one of the plurality of joints has reached a warning range set in the vicinity of the movable limit.

  Further, the present invention is characterized in that the warning notification means notifies the angular displacement direction in which the joint portion that has reached the warning range moves away from the movable limit.

  The present invention also provides a coaxial joint portion that connects two adjacent arm bodies so as to be rotatable about a rotation axis that is coaxial with the axis of each arm body, and a predetermined angle with respect to the axis of each arm body. An articulated robot including an inclined joint unit rotatably connected around an inclined rotation axis is controlled.

In addition, the present invention includes seven joint portions that are connected to each other via an arm body so as to be angularly displaceable and are arranged in series, and among the seven joint portions, the three joint portions are arranged from one end in the series direction toward the other end in the series direction. A control device for a multi-joint robot with 7 degrees of freedom having three intersection-forming joints that form a three-axis intersection where the rotation axes of two joints intersect at one point,
A fixed condition input means for inputting a position and a posture to be fixed as a fixed condition among the seven joint parts to be set to the joint part at one end in the series direction;
Redundant joint input means in which one of the four basic joints other than the three intersection forming joints is input as a redundant joint;
Redundant angle input means for inputting the angle of the redundant joint as a redundant angle;
3-axis intersection position calculating means for calculating a fixed position of the 3-axis intersection in a state where the setting portion is to be fixed and in a posture;
Based on the position of the three-axis intersection calculated by the three-axis intersection position calculation means and the redundant angle input by the redundant joint input means, the position of the three-axis intersection is determined using each angle of the four basic joints as a variable. A basic joint angle calculating means for calculating the angles of the four basic joint parts so that the three-axis intersection is located at a fixed position by solving the relational expression for obtaining;
Intersection formation joint angle for calculating the angles of the three intersection formation joint sections based on the calculation result calculated by the target joint section angle calculation means and the position and orientation of the set portion input by the fixed condition input means Computing means;
An articulated robot control apparatus comprising: output means for giving a calculation result calculated by each angle calculating means to the articulated robot.

The present invention also includes a plurality of joint portions that are connected to each other via an arm body so as to be angularly displaceable and arranged in series, and among the plurality of joint portions, a setting portion that is set to a joint portion at one end portion in the series direction is provided. A control method for controlling an articulated robot in which a plurality of combinations of angles that can be taken by the plurality of joints of interest are present in a fixed state indicating at least one of a predetermined position and posture,
A fixing condition determining step for determining a fixing condition for fixing the setting portion;
A redundant joint part determining step for determining a redundant joint part, wherein the number of remaining joint parts excluding the redundant joint part among a plurality of joint parts matches the number of degrees of freedom in which the set part is constrained according to a fixed condition A redundant joint determination step for determining at least one of the plurality of joints as a redundant joint; and
A redundant angle determining step of determining the angle of the redundant joint as a redundant angle;
Based on the fixed condition determined by the fixed condition determining step and the redundant angle determined by the redundant angle determining step, solving the relational expression for obtaining the position or posture of the set part using each angle of the plurality of joints as a variable , An angle calculation step for calculating the angles of a plurality of joints, each of which should be fixed under a fixed condition;
And a control step of controlling the robot according to an angle calculation step.

The present invention also includes a plurality of joint portions that are connected to each other via an arm body so as to be angularly displaceable and arranged in series, and among the plurality of joint portions, a setting portion that is set to a joint portion at one end portion in the series direction is provided. Calculating the angle of each joint in an articulated robot in which there are a plurality of combinations of angles that can be taken by a plurality of the joints of interest in a fixed state that indicates at least one of a predetermined position and posture. An arithmetic method,
A fixing condition determining step for determining a fixing condition for fixing the setting portion;
A redundant joint part determining step for determining a redundant joint part, wherein the number of remaining joint parts excluding the redundant joint part among a plurality of joint parts matches the number of degrees of freedom in which the set part is constrained according to a fixed condition A redundant joint determination step for determining at least one of the plurality of joints as a redundant joint; and
A redundant angle determining step of determining the angle of the redundant joint as a redundant angle;
Based on the fixed condition determined by the fixed condition determining step and the redundant angle determined by the redundant angle determining step, solving the relational expression for obtaining the position or posture of the set part using each angle of the plurality of joints as a variable An angle calculation step for calculating the angles of a plurality of joint portions, each of which should be fixed under a fixed condition, and an angle calculation method for each joint portion of the articulated robot.

  According to the first aspect of the present invention, the articulated robot has redundancy, and there are a plurality of combinations of angles that each joint portion can take in a state where the setting portion is fixed under a predetermined fixing condition. In the present invention, the redundant joint portion is input by the redundant joint portion input means. As a result, the number of remaining joint portions excluding the redundant joint portion among the plurality of joint portions matches the number of degrees of freedom in which the set portion is constrained according to the fixed condition. Further, the fixing condition input means makes known the fixing condition for fixing the set portion, and the redundant angle input means makes the redundant angle of the redundant joint part known.

  When various types of information are input to the input means in this way, the angle of each joint portion other than the redundant joint portion among the plurality of joint portions is substituted by substituting the redundant angle of the redundant joint portion and the fixed condition. There are calculation formulas that can be obtained analytically. The angle calculation means stores a calculation program representing the calculation formula, and by following the calculation program, by substituting various information given to each input means into the calculation formula, the set part is constrained under a fixed condition. In this case, it is possible to analytically determine the angle of the joint part of interest.

  The output means gives information about the redundant angle input by the redundant angle input means and each angle of the remaining joint calculated by the angle calculating means to the articulated robot. The multi-joint robot drives angular displacement of a plurality of joints based on information given from the output means. As a result, the set portion is constrained under a predetermined fixing condition, and each joint portion of the articulated robot can be driven to be displaced so as to keep the redundant joint portion at a redundant angle. Here, since each angle of the remaining joint portion is obtained as an analytical solution, that is, an exact solution, the redundant joint portion can be angularly displaced to the redundant angle while the set portion is accurately maintained under a predetermined fixed condition. .

  As described above, according to the present invention, the analytical solution for the angle of each joint can be calculated, so that the setting portion is determined in advance compared to the case of calculating the angle of each joint based on the approximate solution. It is possible to accurately maintain the fixed state and to make the redundant joint portion have a redundant angle. That is, it is possible to prevent the setting portion from being shifted. For example, even when the angle of the redundant joint portion is moved at a high speed or moved a long distance from the state before the input, the fixed state of the setting portion can be accurately maintained. Further, accompanying the determination of the analytical solution, it is possible to accurately determine the singular point of each joint, and the convenience can be improved.

  Further, for example, in order to avoid the redundant joint portion and the arm body connected to the redundant joint portion from colliding with an obstacle, the redundant joint portion can be angularly displaced while the position of the setting portion is fixed. In this case, since the interference between the obstacle and the arm body can be avoided while accurately maintaining the position of the setting portion, it is possible to prevent a malfunction of the robot due to the positional deviation of the setting portion.

  According to the second aspect of the present invention, the three degrees of freedom of the setting portion are constrained as a fixed condition. As a constraint with three degrees of freedom, for example, the x-coordinate, y-coordinate, and z-coordinate of the set portion with respect to the joint at the other end in the series direction are determined. Further, for example, the x-coordinate and y-coordinate and the posture around the Y axis set at the joint at the other end with respect to the joint at the other end in the series direction are determined.

  By making one joint part of the two joint parts on the one end side in the series direction and one joint part of the two joint parts on the other end side in the series direction selectable as the redundant joint part, By adjusting the redundant angles of the two redundant joint portions, it is possible to calculate all combinations of angles that can be taken by the plurality of joint portions of interest while maintaining the setting portion in a fixed state.

  As described above, according to the present invention, two of the four joint portions can be selected as redundant joint portions, so that combinations of all angles that can be taken by each joint portion can be obtained. Therefore, it is not necessary to make all four joint portions selectable as redundant joint portions, and convenience can be improved. For example, the operability for the operator can be improved and the number of calculation programs stored in the angle calculation means can be reduced.

  According to the third aspect of the present invention, since the angle of each joint for maintaining the set portion in a fixed state can be analytically determined, it is possible to accurately determine whether the redundant joint has reached the movable limit. Can be judged. When it is determined that the redundant joint portion reaches the movable limit, the angle calculating means calculates the angle of the joint portion in a state where the redundant joint portion has reached the movable limit. Even if a redundant angle exceeding the movable limit is input, the angle of the joint portion in a state where the movable limit is reached is calculated.

  As described above, according to the present invention, the angle of the joint portion in a state that is within the movable range of the redundant joint portion is calculated. As a result, it is possible to prevent the movement command value that cannot be moved mechanically from being given to the articulated robot and to prevent damage to the joint portion. Further, the movable limit can be accurately obtained based on analytically obtained singular points. Therefore, the angle calculation means can accurately determine whether or not the redundant joint has reached the movable limit, and can angularly displace the redundant joint to a region close to the movable limit.

  According to the fourth aspect of the present invention, when the redundant joint portion escapes from the movable limit and moves into the movable range by the input of the redundant angle from the redundant angle input means, the angle calculating means Calculates a combination of angles different from the combination of angles obtained before reaching the movable limit. As a result, when the articulated robot is in a rotational swing state with the setting portion fixed, if the joint portion selected as the redundant joint portion is displaced by the swing angle, before the redundant joint portion reaches the movable limit. Then, after the escape from the movable limit, the form of the robot can be changed, and all combinations of angles that can be taken by a plurality of joints can be calculated. Therefore, it is not necessary to select all of the plurality of joint portions as redundant joint portions, and convenience can be improved.

  As described above, according to the present invention, it is possible to calculate all combinations of angles that can be taken by a plurality of joint portions by selecting only one redundant joint portion. Therefore, it is not necessary to select all of the plurality of joint portions as redundant joint portions, and convenience can be improved. For example, the operability for the operator can be improved and the number of calculation programs stored in the angle calculation means can be reduced.

  According to this invention of Claim 5, the prediction result regarding the angular displacement of each joint part is alert | reported by a prediction alerting | reporting means. Thereby, the operator can know the prediction information of each joint part, for example, the angular displacement direction and the angle, and the convenience can be improved. For example, when the operator specifies the redundant angle of the redundant joint part while confirming whether or not the robot collides with an obstacle, the motion of each joint part is predicted and notified. This makes it easy for the operator to grasp whether or not the joint portion collides with the obstacle when the redundant angle is designated, and convenience can be improved. Further, the information shown as the prediction result is not required to be more accurate than the case where the angle of each joint is actually determined. Therefore, an approximate expression based on the Jacobian matrix is sufficient, and based on the Jacobian matrix, the prediction result can be calculated easily and in a short time.

  According to the sixth aspect of the present invention, when it is determined that any of the plurality of target joints has reached the warning range, the warning notification means notifies that the joint has reached the warning range. Accordingly, the operator can determine that the joint portion has approached the warning limit, and can improve convenience. For example, the operator can set the redundant angle so that the focused joint part moves away from the movable limit while the set part is maintained in the fixed state, and the joint part is maintained in the fixed state. Can be moved away from the movable limit, and a margin for the movable limit can be given to the robot.

  According to the seventh aspect of the present invention, when it is determined that any of the plurality of joints of interest has reached the warning range, an angular displacement direction in which the joint part moves away from the movable limit is notified. Thus, the operator can easily avoid the joint portion from reaching the movable limit by determining the direction in which the joint portion moves away from the movable limit, and the convenience can be improved.

  According to the eighth aspect of the present invention, the articulated robot has an inclined joint and a coaxial joint. In the case of such an articulated robot, the movement of the articulated robot is complicated, and it is difficult to manually give a command for angularly displacing each joint portion while maintaining the setting portion in a fixed state. On the other hand, if the control device of the present invention is used as described above, the fixed state of the setting portion is accurately maintained by giving the redundant joint portion selection command and the redundant angle of the redundant joint portion, Each joint part can be angularly displaced, and convenience can be improved.

  According to the ninth aspect of the present invention, the articulated robot is a 7-degree-of-freedom articulated robot including seven joint parts, and each joint part is fixed to a predetermined position and posture. There are several combinations of angles that can be taken. In the present invention, the position and orientation at which the setting portion is to be fixed is input by the fixed condition input means. When the position and orientation of the set part are determined, the three-dimensional intersection position calculation means can uniquely determine the three-dimensional position of the three-axis intersection point.

  Further, one of the four joints to be noted other than the three intersection forming joints is input as a redundant joint by the redundant joint input means. As a result, the number of the remaining joint parts excluding the redundant joint part among the four target joint parts is three, and the number of degrees of freedom restrained at the three-axis intersection, that is, the number of coordinates restricted at the three-axis intersection. Matches. Further, the redundant angle of the redundant joint portion is known by the redundant angle input means.

  In this case, the angle of each joint part other than the redundant joint part is analytically obtained by substituting the redundant angle of the redundant joint part and the three-dimensional position of the three-axis intersection. There are formulas that can do this. The target joint angle calculation means stores a calculation program representing the calculation formula, and the three-axis intersection is located at a three-dimensional position based on various information input to each input means by following the calculation program The angles of the four joints of interest can be determined analytically.

  Also, there are calculation formulas that can analytically determine the angles of the three-axis intersection forming joints by substituting the angles of the four joints of interest and the positions and orientations of the set parts. The intersection joint angle calculation means stores a calculation program representing the calculation formula, and according to the calculation program, based on the calculation result of the target joint angle calculation means and the information input to the fixed condition input means, When the setting portion is located at a predetermined position and posture, the angles of the three-axis intersection forming joint portions can be obtained analytically.

  The output unit gives the articulated robot the redundant angle input by the redundant angle input unit and each angle of the joint calculated by each angle calculating unit. As a result, the articulated robot can fix the set portion at a predetermined position and posture, and can drive each joint portion so as to keep the redundant joint portion at a redundant angle. Here, the angle of each joint is obtained based on an analytical solution, that is, an exact solution, so that the redundant joint can be angularly displaced to a redundant angle while the set portion is accurately maintained under a predetermined fixed condition. it can.

  As described above, according to the present invention, the setting portion can be accurately maintained in a predetermined fixed state after the redundant joint portion has a redundant angle. For example, when the redundant joint portion is angularly displaced in order to prevent the redundant joint portion and the arm body connected to the redundant joint portion from colliding with an obstacle while the position and posture of the setting portion are fixed, the position of the setting portion In addition, it is possible to avoid interference between the obstacle and the arm body while maintaining the posture accurately.

  According to the tenth aspect of the present invention, the fixing condition for fixing the setting portion is determined, and the redundant joint portion and the redundant angle are determined from the plurality of joint portions. In this case, the number of remaining joint portions excluding the redundant joint portion among the plurality of joint portions matches the number of degrees of freedom in which the set portion is constrained according to the fixed condition. Next, by substituting the fixed condition and redundant angle determined by each input step into the relational expression for obtaining the position or orientation of the set part using each angle of the plurality of joints as a variable, the set part becomes the fixed condition The angles of a plurality of joints that should be fixed by are calculated respectively. Thus, the angles of the plurality of joint portions can be analytically obtained. In accordance with the calculation result thus obtained, the plurality of joint portions are angularly displaced, whereby the redundant joint portions can be made to have a redundant angle while the set portion is fixed under a fixed condition.

  According to the present invention, since the angles of the remaining joint portions other than the redundant joint portion are obtained based on the analytical solution, that is, the exact solution, the redundant joint portion is maintained in a state where the set portion is accurately maintained under a predetermined fixed condition. Can be angularly displaced to a redundant angle. Therefore, it is possible to prevent the set portion from deviating from the fixed condition to be fixed, as compared with the case where the angles of the remaining joint portions are determined based on the approximate solution. In addition, the singular point of each joint portion can be accurately determined due to obtaining the analytical solution, and convenience can be improved.

  According to the present invention, the fixing condition for fixing the setting portion is determined, and the redundant joint portion and the redundant angle are determined from the plurality of joint portions. In this case, the number of remaining joint portions excluding the redundant joint portion among the plurality of joint portions matches the number of degrees of freedom in which the set portion is constrained according to the fixed condition. Next, by substituting the fixed condition and redundant angle determined by each input step into the relational expression for obtaining the position or orientation of the set part using each angle of the plurality of joints as a variable, the set part becomes the fixed condition The angles of a plurality of joints to be focused on are calculated respectively. This makes it possible to analytically determine the angles of a plurality of joints of interest.

  According to the present invention, since the angles of the remaining joint portions other than the redundant joint portion are obtained based on the analytical solution, that is, the exact solution, the redundant joint portion is maintained in a state where the set portion is accurately maintained under a predetermined fixed condition. Can be angularly displaced to a redundant angle. Therefore, it is possible to prevent the set portion from deviating from the fixed condition to be fixed, as compared with the case where the angles of the remaining joint portions are determined based on the approximate solution. In addition, the singular point of each joint portion can be accurately determined due to obtaining the analytical solution, and convenience can be improved.

It is a block diagram which shows the control apparatus 2 of the articulated robot 30 which is 1st Embodiment of this invention. 3 is a front view showing an input unit 20. FIG. 7 is a flowchart for explaining a procedure of a form changing operation of the central processing unit 19; It is a front view which shows the state of the articulated robot 30 which carried out the angular displacement of the 1st joint part 9 in the state which fixed the position of the target point Ph to the predetermined position. It is a graph which shows angle q2-q4 of the 2nd-4th joint parts 10-12 in case the angle q1 of the 1st joint part 9 is set as a redundant angle. It is a front view which shows the 1 axis redundant robot 60 for demonstrating the kind of state which the articulated robot 30 can take when the target point Ph is fixed. It is a figure which shows the combination of the angle which each joint part can take when the 1 axis redundant robot 60 will be in a rotation rocking | fluctuation state. In the multi-joint robot 30 shown in FIG. 1, the first joint portion 9 is a redundant joint portion, and the angle q1 is sequentially angularly displaced. It is a graph which shows the time change of the angles q1-q4 of each joint part 9-12 in FIG. In the multi-joint robot 30 shown in FIG. 1, it is a front view which shows the state which made the 4th joint part 12 the redundant joint part, and the angle q4 was sequentially angularly displaced. It is a graph which shows the time change of the angles q1-q4 of each joint part 9-12 in FIG. 10 is a flowchart for explaining a procedure of another form changing operation of the central processing unit 19. It is a graph which shows the angles q1-q4 of the 1st-4th joint parts 9-12 in case the central processing part 19 performs the procedure of the form operation | movement shown in FIG. It is a flowchart for demonstrating the procedure of the prediction alerting | reporting operation | movement of the central processing part 19 in form change operation | movement. It is a block diagram which shows the control apparatus 2 of the articulated robot 1 which is 2nd Embodiment of this invention. It is a front view which shows the input part 20 of the control apparatus 2 which controls the articulated robot shown in FIG. It is a flowchart for demonstrating the procedure of the form change operation | movement of the central processing part 19 in 2nd Embodiment. It is a flowchart for demonstrating the operation | movement procedure of the central processing part 19 in the case of performing movement prediction operation | movement, form change operation | movement, and movement teaching operation | movement. 4 is a diagram showing a display screen displayed on the display unit 42 of the input unit 20. FIG.

  FIG. 1 is a block diagram showing the control device 2 of the articulated robot 30 according to the first embodiment of the present invention. The multi-joint robot 30 controlled by the control device 2 is a single-axis redundant robot having four degrees of freedom, and four joint parts that rotatably connect four arm bodies 3 to 6 and two adjacent arm bodies. 9 to 12 and a base 17. Each arm body 3-6 is connected, respectively, and comprises the arm structure 16 extended in series. The base 17 is connected to the base end portion of the arm structure 16 and is fixed in advance to a fixed position such as a wall surface or a floor surface.

  Each joint part 9-12 is either a coaxial joint part or an inclined joint part. The coaxial joint portion connects two adjacent arm bodies so as to be rotatable around a rotation axis coaxial with the axis of each arm body. The inclined joint portion connects one of two adjacent arm bodies so as to be conically rotatable about a rotation axis inclined with respect to the axis of each arm body.

  The joint portions 9 to 12 are arranged in the order of the first to fourth joint portions 9 to 12 from the base 17. The first joint portion 9 is a coaxial joint portion. Moreover, the 2nd joint part 10, the 3rd joint part 11, and the 4th joint part 12 are inclination joint parts. In the present invention, the term “rotation” includes not only a rotation of 360 degrees or more around the rotation axis but also a state of angular displacement around the axis by an angle of 360 degrees or less.

  One end portion of the first arm body 3 is connected to the base 17 by the first joint portion 9 so as to be rotatable around a first rotation axis A9 coaxial with the axis A3 of the first arm body 3. Also, one end of the second arm body 4 is at a predetermined angle with respect to the axis A3 of the first arm body 3 and the axis A4 of the second arm body 4 by the second joint section 10, and 45 degrees in this embodiment. The second arm 3 is connected to the other end of the first arm body 3 so as to be rotatable around the inclined second rotation axis A10. Also, one end of the third arm body 5 is at a predetermined angle with respect to the axis A4 of the second arm body 4 and the axis A5 of the third arm body 5 by the third joint portion 11, 45 degrees in the present embodiment. The second arm body 4 is coupled to the other end of the second arm body 4 so as to be rotatable around the inclined third rotation axis A11.

  One end portion of the fourth arm body 6 is inclined by the fourth joint portion 12 at a predetermined angle with respect to the axis A5 of the third arm body 5 and the axis A6 of the fourth arm body 6, 45 degrees in the present embodiment. The third arm body 5 is coupled to the other end of the third arm body 5 so as to be rotatable around the fourth rotation axis A12. A target point Ph serving as a setting portion is set in advance at the other end of the fourth arm body 6. Therefore, the target point Ph is disposed at the distal end portion of the arm structure 16, and the base 17 is disposed at the proximal end portion of the arm structure 16.

  Thus, each joint part 9-12 connects adjacent arm bodies so that rotation is mutually possible. Further, as the joint portions 9 to 12 are angularly displaced, as shown in FIG. 1, the arm structure 16 is in a state in which the respective axes A <b> 3 to A <b> 6 of the arm bodies 3 to 6 are arranged coaxially and extend in a straight line. It is configured to be deformable. In a state where the axes A3 to A6 of the arm bodies 3 to 6 extend in a straight line, the second rotation axis A10 and the third rotation axis A11 are parallel, and the third rotation axis A11 and the fourth rotation axis A12 are vertical. It becomes.

  The multi-joint robot 30 can rotate the joint portions 9 to 12 to deform the arm structure 16 like a snake and position the target point Ph at an arbitrary three-dimensional position. For example, the multi-joint robot 30 is preferably used when the facilities and the like are complicated and the work route is complicated, or when the operation range cannot be increased by being arranged in parallel with other robots.

  Since the articulated robot 30 having such four joint portions 9 to 12 has four degrees of freedom, the fourth arm body 6 is in a state where the target point Ph is aligned with a predetermined three-dimensional position within the movable range. There are several possible postures. In other words, the articulated robot 30 has a plurality of combinations of angles that each of the four joint portions 9 to 12 can take in a state where the target point Ph is aligned with a predetermined three-dimensional position.

  Each arm body 3-6 incorporates a motor that individually rotates and drives each arm body 3-6. The motor is configured to be able to displace each of the arm bodies 3 to 6 at a predetermined arbitrary angle.

  The rotation mechanism of each joint portion 9-12 for rotatably connecting each arm body 3-6 around the corresponding rotation axis A9-A12 includes an input side member and an output side member, which are relative to each other. Is rotatably provided. The input side member is connected to one of the two members connected by the joint portions 9 to 12, and the output side member is connected to the other of the two members. When the rotational force from the motor is applied to the input side member, the input side member and the output side member rotate relatively. Thereby, one member and the other member can be rotated relatively. In the coaxial joint portion 9, the rotation axis A9 coincides with the arm body connected to the input side member and the output side member. In the inclined joint portions 10 to 12, the rotation axes A <b> 10 to A <b> 12 are inclined with respect to the axis of the arm body connected to the input side member and the output side member, respectively.

  By providing such a rotation mechanism, two adjacent members can be relatively rotated. Here, the “angle of the joint part” in the present invention means an angle of the output side member with respect to the input side member in the joint part. In other words, it means an angle around the rotation axis of the joint portion of the other member connected to the output side member with respect to one member connected to the input side member of the two members connected by the joint portion. Further, in the present invention, “movement of the joint portion” indicates angular displacement movement of the angle of the joint portion.

  The control device 2 includes an input unit 20, a central processing unit 19, and an output unit 22. The input unit 20 and the output unit 22 are electrically connected to the central processing unit 19. The input unit 20 receives various command information from the operator and gives the input various information to the central processing unit 19. In the present embodiment, the central processing unit 19 and the output unit 22 are provided in the control device main body 21, and the input unit 20 is provided separately from the control device main body 21. Specifically, the input unit 20 is realized by a teach pendant for an operator to directly control the articulated robot, and is electrically connected to the robot controller which is the control device main body 21 by a cable.

  The central processing unit 19 performs calculations necessary for controlling the articulated robot 30 in response to various information given from the input unit 20 according to an operation program stored in advance. In the present embodiment, the central processing unit 19 determines the angles q1 to q4 of the joints 9 to 12 necessary to move the target point Ph set on the fourth arm body 6 to a predetermined target position. Calculate each. The central processing unit 19 is realized including a processing circuit such as a CPU and a storage circuit such as a memory. The storage circuit stores, together with the operation program, a calculation formula necessary for the calculation, the length of the arm body, the type of joint portion, and the like.

  The output unit 22 serves as an output unit that gives the calculation result of the central processing unit 19 to the articulated robot 30. In the present embodiment, the output unit 22 gives the calculation result of the central processing unit 19 to the power supply circuit provided in the articulated robot 30. The power supply circuit individually gives power corresponding to the calculation result of the central processing unit 19 to each motor provided for each joint portion 9 to 12. Each motor to which power is applied causes the corresponding joint to be angularly displaced by a predetermined angle according to the calculation result of the central processing unit 19.

  Further, the control device 2 calculates the angles of the joint portions 9 to 12 to be rotated when the target point Ph of the fourth arm body 6 is moved along the movement path designated in advance. This movement path is input from the input unit 20 to the central processing unit 19 by the operator before the robot moves. Further, the multi-joint robot 30 has four joint portions 9 to 12, so that a plurality of combinations of angles that can be taken by each joint portion can be obtained with the target point Ph fixed at a predetermined position with respect to the base 17. Exist.

  The control device 2 of the present embodiment is used to perform a teaching operation for teaching the operation of the robot, and performs a form changing operation for the teaching operation. The form change operation is a state in which the target point Ph is fixed under a predetermined fixing condition, and the external form such as the shape and posture of the robot is changed by changing the angles of the first to fourth joint parts 9 to 12. Is the action. In the present embodiment, the form changing operation is an operation of changing the angles of the first to fourth joint portions 9 to 12 in a state where the three-dimensional position of the target point Ph with respect to the base coordinates set on the base 17 is fixed. Become. In other words, in the form changing operation, the target point Ph set at one end in the series direction in the arm structure 16 is fixed under the constraint condition obtained according to the coordinate system set at the other end of the arm structure 16. Is done.

  In performing the shape changing operation, the control device 2 includes one redundant joint portion selected from the first to fourth joint portions 9 to 12, a redundant angle that is an angle of the redundant joint portion, and the target point Ph. Is given via the input unit 20. The central processing unit 19 calculates the angles of the remaining joint portions when the redundant joint portion has a redundant angle after fixing the target point Ph in a fixed state to be fixed. The output unit 22 gives the calculation result of the central processing unit 19 to the articulated robot 30. The articulated robot 30 given the calculation result sets the redundant joint part to the redundant angle and aligns the angles of the remaining joint parts according to the calculation result to set the target point Ph to the specified fixed state. .

  By performing the form changing operation, the operator selects the redundant joint part via the input unit 20 and inputs the redundant angle, thereby fixing the target point Ph and the form of the arm structure 16. Can be changed. In this case, the operator avoids a collision with an obstacle in a state where the target point Ph is fixed by causing the central processing unit 19 to perform a shape changing operation and confirming the shape of the articulated robot 30. The angle of each joint part 9-12 which implement | achieves the form of the arm structure 16 can be determined easily. When the operator determines the angles q1 to q4 of the joints 9 to 12 that avoid the collision with the obstacle, the angles q1 to q4 of the joints 9 to 12 are used as information when the robot moves. Use.

  FIG. 2 is a front view showing the input unit 20. The input unit 20 includes an operation unit 41 to which information and instructions for robot teaching are input, and a display unit 42 that displays an operation screen. The operation unit 41 includes each joint operation switch 43, a tool movement switch 44, and a form change switch 45. The operation unit 41 further includes a command key for inputting a robot operation program, a cursor key, a numeric keypad, and the like.

  The joint part operation switch 43 is a switch for inputting the angles q1 to q4 of the joint parts 9 to 12, and the angle of a predetermined angular displacement amount in each of the joint parts 9 to 12 in the plus direction and the minus direction. A displacement command is given. The tool movement switch 44 is given a movement command of the target point Ph according to the base coordinate axis or the tool coordinate axis. Here, the base coordinate axis is a coordinate set on the base 17, and the tool coordinate axis is a coordinate set on the fourth arm body 6. The form change switch 45 is given a form change command for changing the form of the arm structure 16 while the target point Ph is fixed at a predetermined position.

  When performing the form changing operation, the tool movement switch 44 serves as a fixed condition input means for inputting information on a position where the target point Ph should be fixed. Further, when the angular displacement amount of one joint portion is designated by the joint portion operation switch 43 in a state where the form change switch 45 is pressed, the designated joint portion is input as a redundant joint portion. In addition, an angle corresponding to the angular displacement is input as the angle of the redundant joint portion. Therefore, the form change switch 45 and the joint operation switch 43 constitute a redundant joint input means for inputting a redundant joint and a redundant angle input for inputting a redundant angle that is an angle of the redundant joint. .

  FIG. 3 is a flowchart for explaining the procedure of the form changing operation of the central processing unit 19. First, in step a0, when the central processing unit 19 determines that the form change switch 45 has been pressed in a state where the articulated robot 30 can be controlled, the process proceeds to step a1 and starts the form change operation.

  In step a <b> 1, when the movement command for the target point Ph is given by the tool movement switch 44, the central processing unit 19 gives a calculation result for moving the target point Ph to the designated position to the output unit 22. If there is no movement command by the tool movement switch 44, the current position of the target point Ph is maintained. When determining that the joint operation switch 43 has been pressed, the central processing unit 19 determines the position of the target point Ph at that time as a fixed condition to be fixed, and proceeds to step a2.

  In step a2, the central processing unit 19 determines the joint part designated by the joint part operation switch 43 as a redundant joint part. The redundant joint portion is any one of the first to fourth joint portions 9 to 12. Further, the angle designated by the joint operation switch 43 is determined as the redundant angle. In the present embodiment, when the plus direction movement switch is pressed among the joint portion operation switches 43, the redundant joint portion is in the plus direction with a predetermined angular displacement from the state before the joint portion operation switch 43 is pressed. The angle that is angularly displaced around the axis is determined as the redundant angle. When the minus direction movement switch is pressed, the angle at which the redundant joint portion is angularly displaced about the axis in the minus direction by a predetermined angular displacement amount from the state before the joint operation switch 43 is pressed is determined as the redundant angle. To do. When the redundant joint portion and the redundant angle are thus determined, the process proceeds to step a3.

  In step a3, the central processing unit 19 determines the angles q1 to q4 of the first to fourth joint portions 9 to 12 when the target point Ph is located at the three-dimensional position of the target point Ph determined in step a1. Ask. Specifically, the forward conversion relational expression for obtaining the three-dimensional position of the target point Ph using the angles q1 to q4 of the first to fourth joint portions 9 to 12 as variables, and the redundant joint determined in step a2 The angles q1 to q4 of the first to fourth joint portions 9 to 12 can be analytically determined based on the redundant angle of the portion and the three-dimensional position of the target point Ph determined in step a1.

  By solving the forward conversion problem, the x coordinate expression representing the x coordinate position of the target point Ph, the y coordinate expression representing the y coordinate position of the target point Ph, and the z coordinate expression representing the z coordinate position of the target point Ph are the first -It represents with the function which makes each angle q1-q4 of the 4th joint parts 9-12 a variable. By substituting the three-dimensional position of the target point Ph obtained in step a1 into the x-coordinate expression, the y-coordinate expression, and the z-coordinate expression based on the forward conversion problem, these coordinate expressions are obtained from the first to fourth joint parts 9 to The number of twelve angles q1 to q4, that is, a ternary simultaneous equation having four unknowns.

  In addition, since one of the angles q1 to q4 of the first to fourth joint portions 9 to 12 is determined as the redundant angle in step a2, by substituting the redundant angle into the ternary simultaneous equation described above, It becomes a ternary simultaneous equation having three unknowns. By modifying this ternary simultaneous equation, it is possible to derive a calculation expression representing an equation for obtaining each of the three unknowns. By substituting the coordinates of the target point Ph and the angles of the redundant joint portion and the redundant joint portion into this equation, the angles of the joint portions other than the redundant joint portion among the first to fourth joint portions 9 to 12 are determined. Each can be determined analytically.

  The central processing unit 19 substitutes the redundant angle of the redundant joint part and the x coordinate, y coordinate, and z coordinate of the target point Ph, so that the first joint part 9 to 12 other than the redundant joint part. A calculation program representing an equation capable of analytically obtaining the angle of each joint is stored. The central processing unit 19 analyzes the angle of each joint part other than the redundant joint part among the first to fourth joint parts 9 to 12 according to the calculation program of the first to fourth joint parts 9 to 12. To step a4. Since the angle of the redundant joint has already been determined in step a2, it can be obtained without calculation. As described above, the central processing unit 19 serves as angle calculation means for calculating the angles q1 to q4 of the first to fourth joint portions 9 to 12, respectively.

  In step a4, when the central processing unit 19 determines that the obtained angles q1 to q4 of the joints 9 to 12 have not reached the movable limit, the central processing unit 19 proceeds to step a5 and selects one of the angles q1 to q4. When it is determined that has reached the movable limit or exceeds the movable limit, the process proceeds to step a6. For example, the central processing unit 19 determines that the movable limit has been reached when the angle of each joint determined in step a3 reaches the analytically determined maximum value or minimum value. If the angle of each joint cannot be obtained, it is determined that the movable limit is exceeded.

  In step a5, the angles q1 to q4 of the first to fourth joint portions 9 to 12 analytically obtained in step a3 are given to the output unit 22 as calculation results. The output unit 22 gives the given calculation result to the articulated robot 30. As a result, the articulated robot 30 sets the redundant joint portion to the redundant angle in a state where the joint portions 9 to 12 are angularly displaced and the target point Ph is maintained at the fixed position. At this time, the angles q1 to q4 of the first to fourth joint portions 9 to 12 are obtained by an analytical calculation method, unlike an approximation method such as a convergence calculation. Therefore, before and after the movement of the redundant joint portion, Thus, the target point Ph does not deviate from the fixed position. As described above, when the operation result is given to the output unit 22 to operate the articulated robot 30, the process proceeds to step a6.

  In step a6, the central processing unit 19 determines whether or not an end condition for ending the form changing operation is satisfied. For example, it may be determined that the end condition is satisfied by pressing the form change release switch. If the central processing unit 19 determines that the end condition of the shape change operation is not satisfied, the central processing unit 19 returns to step a1 and waits for the joint operation switch 43 to be pressed again. In this state, when the joint operation switch 43 is pressed again, steps a1 to a5 are sequentially performed according to the instruction. If it is determined in step a6 that the end condition is satisfied, the process proceeds to step a7, and the form change operation is ended in step a7.

  In this way, when the central processing unit 19 performs the form changing operation, the operator designates the fixed position of the target point Ph, the redundant joint portion, and the redundant angle thereof, whereby the target point Ph is set. It is possible to change the form of the articulated robot 30 by angularly displacing the joints 9 to 12 while the position is securely fixed. At this time, since the angles q1 to q4 of the joint portions 9 to 12 can be obtained as analytical solutions, the angles of the joint portions 9 to 12 can be obtained in a shorter time compared to the case of obtaining the solution using the convergence calculation. The shape of the robot can be changed smoothly while the position of the target point Ph is fixed.

  In addition, the operator can cause the central processing unit 19 to perform a form changing operation while confirming the form of the articulated robot 30. Thus, the operator can easily select one from a plurality of combinations of the angles q1 to q4 of the joint portions 9 to 12 that can position the target point Ph at the fixed position. Thus, the operator can place the target point Ph at a fixed position in the form of the articulated robot 30 that avoids collision with an obstacle, and obtains the angles q1 to q4 of the joint portions 9 to 12. Can do.

  FIG. 4 is a front view showing a state of the articulated robot 30 in which the first joint portion 9 is angularly displaced in a state where the position of the target point Ph is fixed at a predetermined position. As the angle q1 of the first joint portion 9 is angularly displaced in the plus direction, the articulated robot 30 is deformed in the order of FIGS. 4 (1) to 4 (8). FIG. 5 is a graph showing the angles q2 to q4 of the second to fourth joint portions 10 to 12 when the angle q1 of the first joint portion 9 is set as a redundant angle. FIG. 5A shows the angle q1 of the first joint portion 9 on the horizontal axis and the angle q2 of the second joint portion 10 on the vertical axis. FIG. 5B shows the angle q1 of the first joint portion 9 on the horizontal axis and the angle q3 of the third joint portion 11 on the vertical axis. FIG. 5 (3) shows the angle q1 of the first joint part 9 on the horizontal axis and the angle q4 of the fourth joint part 12 on the vertical axis.

  Although details will be described later, in the present embodiment, it is possible to determine an analytical solution for the angles q1 to q4 of the joint portions 9 to 12 that keeps the target point Ph fixed at a predetermined three-dimensional position. . As a result, it is possible to prevent the target point Ph from deviating from the fixed position as compared with the case where the approximate solutions of the angles q1 to q4 of the joint portions 9 to 12 are determined. For example, in step a2, when the plus or minus direction movement switch of the joint part operation switch 43 is pressed once, even if the angular displacement amount at which the redundant joint part is angularly displaced is set large, The fixed state of the target point Ph can be accurately maintained. Similarly, in step a2, even when the redundant joint portion moves at a high speed, the fixed state of the target point Ph can be accurately maintained.

  As a result, when the obstacle avoidance form of the multi-joint robot 30 is searched by the form changing operation, the joint portions 9 to 12 can be angularly displaced while the position of the target point Ph is accurately fixed. In addition, since the interference between the obstacle and each of the arm bodies 3 to 6 can be avoided while accurately keeping the target point Ph at the fixed position, it is possible to prevent malfunction of the articulated robot due to the positional deviation of the target point Ph. be able to.

  In addition, the central processing unit 19 can determine whether or not the redundant joint portion has reached the movable limit in association with obtaining the analytical solution of the angles q1 to q4 of the joint portions 9 to 12. As shown in step a4, when determining that the redundant joint portion reaches the movable limit, the central processing unit 19 calculates the angle of the joint portion in a state where the redundant joint portion has reached the movable limit. Even if a redundant angle exceeding the movable limit is input, the angle of the joint portion in a state where the movable limit is reached is calculated.

  As a result, the central processing unit 19 can be prevented from giving a movement command value that is mechanically incapable of movement to the articulated robot, and damage to the joint can be prevented. Further, since the central processing unit 19 can accurately determine whether or not the redundant joint has reached the movable limit, the redundant joint can be angularly displaced to an area close to the movable limit, and the robot can move freely. The range can be widened.

  When the central processing unit 19 determines that the redundant joint has reached the warning area near the movable limit before the redundant joint reaches the movable limit, the central processing unit 19 displays the determination result on the display unit 42 of the input unit 20. You may notify via. For example, the central processing unit 19 determines that the warning area has been reached when the redundant angle has reached a predetermined angle before the movable limit angle of the redundant joint determined by the calculation. In this case, the central processing unit 19 serves as warning notification means for determining and notifying that the warning range has been reached.

  For example, in the case of FIG. 5, the movable limit is reached when the redundant joint is the first joint 9 and the angle q3 of the third joint 11 is 180 degrees. In this case, when the angle q3 of the third joint unit 11 reaches 175 degrees, the central processing unit 19 determines that the warning area has been reached, and causes the display unit 42 to display the determination result. Thus, the operator who operates the input unit 20 can determine that the redundant joint portion is approaching the movable limit, and can improve convenience. Furthermore, when the redundant joint part enters the warning range, the central processing unit 19 preferably displays the angular displacement direction in which the angle of the joint part escapes the warning range on the display unit 42 of the input unit 20.

  Since the articulated robot 30 controlled by the control device 2 of the present embodiment includes an inclined joint part, the deformed form of the articulated robot 30 in the form changing operation is irregular. In this case, it is difficult for the operator to intuitively predict the deformation of the joint portions 9 to 12. In the present embodiment, the central processing unit 19 notifies the warning range and the angular displacement direction for exiting the warning range, so that the operator can easily determine the warning range and the escape direction. Can be improved.

  Next, details of the calculation process of the first to fourth angles q1 to q4 in step a3 will be described. When the respective angles q1 to q4 of the first to fourth joint portions 9 to 12 are determined based on the forward conversion problem, the x coordinate position phx, the y coordinate position phy, and the z coordinate position phz of the target point Ph are determined. It is determined uniquely. That is, the equations for obtaining the x-coordinate, y-coordinate, and z-coordinate of the target point Ph are expressed by functions having the respective angles q1 to q4 of the first to fourth joint portions 9 to 12 as variables. In the form, the following relationship (1) is satisfied.

  In the equation (1), the angle of the first joint portion 9 is q1, the angle of the second joint portion 10 is q2, the angle of the third joint portion 11 is q3, and the angle of the fourth joint portion 12 is q4. The distance of the first arm body 3 from the first joint portion 9 to the second joint portion 10 is L1, the distance of the second arm body 4 from the second joint portion 10 to the third joint portion 11 is L2, and the first The distance of the 3rd arm body 5 from the 3 joint part 11 to the 4th joint part 12 is set to L3, and the distance from the 4th joint part 12 to the object point Ph is set to L4. Further, sin (θ) represents a sine function (sine function) of θ, and cos (θ) represents a cosine function (cosine function) of θ.

  In the equation (1), the coordinates phx, phy, phz of the target point Ph are coordinates when the representative point p1 of the first arm body 3 is set as the origin according to the base coordinate axis set on the base 17. Here, the base coordinate axis is an axis that extends along a plane perpendicular to the z-axis and passes through the z-axis with the axis extending coaxially with the straight line in a state where the axes of the arm bodies are arranged in a straight line. Are the x-axis and the y-axis. The x axis and the y axis are orthogonal to each other. Further, the angles q1 to q4 of the joint portions 9 to 12 are angles at which the joint portions are angularly displaced from a state in which the axes of the arm bodies 3 to 6 extend in a straight line. The representative point p1 of the first arm body 3 is set at the intersection of the axis A1 of the first arm body and the rotation axis A10 of the second joint portion 10.

  By substituting numerical values of the x-coordinate phx, y-coordinate phy, and z-coordinate phz of the position where the target point Ph should be fixed into the equation (1), the four angles q1 to q4 of the first to fourth joint portions 9 to 12 are obtained. Becomes a three-way simultaneous equation that becomes an unknown. By determining one of the first to fourth joint portions 9 to 12 as a redundant joint portion and substituting the redundant angle of the redundant joint portion into the equation (1), the first to fourth joint portions 9 to 12 are determined. One of the four angles q1 to q4 is known, and the unknown becomes three ternary simultaneous equations. Thus, by simultaneously solving the equation (1), the angles q1 to q4 of the joint portions 9 to 12 where the target point Ph should be located at the fixed position can be analytically obtained.

  For example, the first joint portion 9 is determined as the redundant joint portion, and the angle q1 at which the first joint portion 9 should be angularly displaced is determined as the redundant angle. In this case, by transforming equation (1), the angle q1, q4 of the first joint portion 9 and the fourth joint portion 12 is transformed into an unknown equation as shown in the following equation (2). Can do. By substituting the z-coordinate phz of the target point Ph and the angle q1 of the first joint 9 serving as the redundant joint into the equation (2), the angle q4 of the fourth joint 12 can be obtained. The obtained angle q4 of the fourth joint portion 12 is that of the fourth joint portion 12 when the target point Ph is positioned at a fixed position and the first joint portion 9 is angularly displaced to the first angle q1 which is a redundant angle. It means the angle q4.

  The angle q4 of the fourth joint portion 12 can be obtained by transforming the equation (2) into the following equation (3) and substituting the angle q1 of the first joint portion 9.

  Here, arccos means an inverse cosine function (arc cosine function). In addition, two plus and minus codes in the expression (3) are referred to as discrete form codes. In the case where the target point Ph is fixed at a fixed position by determining one of the two discrete form codes based on a predetermined condition, for example, the angle q4 of the fourth joint portion 12 set before the calculation. The angle q4 of the fourth joint portion 12 can be calculated.

  Further, when there is only one solution in the equation (3), that is, when the arc cosine variable is 1 or −1, the fourth joint portion 12 is located at a singular point. Specifically, when the angle q4 of the fourth joint portion 12 is 0 degree or 180 degrees, a singular point is obtained. When the absolute value of the arc cosine variable exceeds 1, no solution exists. By making such a determination, the central processing unit 19 can determine whether there is a solution that is not a singular point, whether the solution is a singular point, or whether there is no solution.

  Next, by transforming equation (1), it is transformed into an equation in which the angles q3 and q4 between the third joint portion 11 and the fourth joint portion 12 are unknown as shown in the following equation (4). Can do. By substituting the angle q4 of the fourth joint part 12 determined based on the expression (3) into the expression (4), the angle q3 of the third joint part 11 can be obtained. The obtained angle q3 of the third joint part 11 is the angle of the third joint part 11 when the target point Ph is kept at a fixed position and the first joint part 9 is angularly displaced to the first angle q1 which is a redundant angle. q3 is meant.

  Next, an angle q1 of the first joint portion 9 set as a redundant joint portion is obtained by combining any two of the respective equations for obtaining phx, phy, and phz among the equations (1), and ( By substituting the angle q4 of the fourth joint portion 12 obtained based on the equation 3) and the angle q3 of the third joint portion 11 obtained based on the equation (4), the angle q2 of the second joint portion 10 is obtained. Cos (q2) and sin (q2) can be obtained, and the angle q2 of the second joint portion 10 can be obtained from these.

  In this way, the angles q1 to q4 of the first to fourth joint portions 9 to 12 can be obtained, respectively. In this case, the central processing unit 19 stores calculation programs indicating the above-described equations (3), (4), and the equation for obtaining the angle q2 of the second joint unit 10, so that the first joint unit 9 By determining the first angle q1, when there is a solution, the angles q2 to q4 of the second to fourth joint portions 10 to 12 when the target point Ph is maintained at the fixed position can be obtained.

  Further, when the redundant angle of the redundant joint portion is gradually angularly displaced in the positive or negative direction, the central processing unit 19 has a maximum or minimum value that can be taken by the redundant joint portion. In this case, the angle can be determined as the movable limit. For example, when the redundant angle is increased, it may be impossible to obtain the solution of the angles of the remaining joint portions other than the redundant joint portion. In this case, the central processing unit 19 determines the maximum value as the movable limit among the redundant angles that can obtain the solution of the angle of each joint. Similarly, when the redundant angle is reduced, it may become impossible to obtain the solution of the angles of the remaining joint portions other than the redundant joint portion. In this case, the central processing unit 19 determines the minimum value as the movable limit among the redundant angles that can obtain the solution of the angle of each joint.

  The central processing unit 19 can calculate the movable limit of the redundant joint portion in a state where the above-described step a1 and step a2 are completed. Then, the deviation between the designated redundant angle and the movable limit angle can be obtained. In this case, when the deviation is equal to or less than a predetermined value, it can be determined that the redundant joint has reached the warning area set near the movable limit.

  Further, the central processing unit 19 checks the amount of change in the deviation when the redundant angle is increased in the positive direction and when the redundant angle is increased in the negative direction with respect to the current redundant angle. Thus, the angular displacement direction of the redundant angle where the deviation becomes large can be obtained. The central processing unit 19 can determine the angular displacement direction of the redundant angle where the deviation increases as the direction in which the redundant angle moves away from the movable limit and escapes from the warning area.

  If a joint other than the first joint is selected as the redundant joint, the equation (1) is similarly modified to solve the simultaneous equations. The angle of each joint part can be obtained. Therefore, the central processing unit 19 stores a calculation program for obtaining the remaining joints according to the joints selected as the redundant joints, so that any of the first to fourth joints 9 to 12 is redundant. Even if the joint portion is selected, the angles of the remaining joint portions can be calculated when the target point Ph is maintained at a fixed position and the redundant joint portion has a redundant angle.

  FIG. 6 is a front view showing a single-axis redundant robot 60 for explaining the types of states that can be taken by the articulated robot 30 when the target point Ph is fixed. The single-axis redundant robot shown in FIG. 6 includes first to fourth arm bodies 50 to 53 and a plurality of joint portions 54 to 57 that connect adjacent arm bodies. Each arm body 50-53 is arrange | positioned on xy plane containing a predetermined x-axis and y-axis. The base end portion 50a of the first arm body 50 is connected to the fixing portion 58 by the first joint portion 54, and can be angularly displaced about the z axis perpendicular to the xy plane with respect to the fixing portion 58 by the first joint portion 54. It becomes.

  The proximal end portion 51 a of the second arm body 51 is connected to the distal end portion 50 b of the first arm body 50 by the second joint portion 55, and is connected to the distal end portion 50 b of the first arm body 50 by the second joint portion 55. , Angular displacement about the z-axis becomes possible. The proximal end portion 52 a of the third arm body 52 is connected to the distal end portion 51 b of the second arm body 51 by the third joint portion 56, and is connected to the distal end portion 51 b of the second arm body 51 by the third joint portion 56. , Angular displacement about the z-axis becomes possible. The proximal end portion 53 a of the fourth arm body 53 is connected to the distal end portion 52 b of the third arm body 52 by the fourth joint portion 57, and is connected to the distal end portion 52 b of the third arm body 52 by the fourth joint portion 57. , Angular displacement about the z-axis becomes possible.

  Therefore, the single-axis redundant robot 60 shown in FIG. 6 is a four-joint planar robot that can move the fourth arm body 53 in the xy plane and can rotate the fourth arm body 53 about the z-axis.

  With respect to such a one-axis redundant robot 60, a fixing condition for fixing the x-coordinate and y-coordinate of the tip 53b of the fourth arm body 53 and the posture of the fourth arm body 53 around the z-axis, that is, three degrees of freedom. Set a fixed condition to constrain Consider the angles of the first to fourth joint portions 54 to 57 when the fourth arm 53 is fixed under this fixing condition.

  The single-axis redundant robot 60 has a plurality of combinations that the first to fourth joint portions 54 to 57 can take in the state where the fourth arm body 53 is fixed under the above-described fixing conditions. Here, if one angle of the four joint portions 54 to 57 is set, the angle of each joint portion that satisfies the fixed condition can be obtained by inverse transformation.

  In the fixed state in which the three degrees of freedom of the target point Ph are constrained, the single-axis redundant robot 60 having four degrees of freedom has a length of each arm body and a distance between the first joint portion and the fourth joint portion. Accordingly, three forms can be taken.

  In the first form, a rotational swing state is established. In this case, as shown in FIG. 6A, the single-axis redundant robot 60 can rotate the first arm body 50 with respect to the fixed portion 58 by 360 degrees or more with the fourth arm body 53 fixed. The third arm body 52 can be displaced by a swing angle of 360 degrees or less with respect to the fourth arm body 53. The second arm body 51 can rotate 360 degrees or more with respect to the first arm body 50, and the third arm body 52 can be displaced by a swing angle of less than 360 degrees with respect to the second arm body 51.

  The distance between the first joint part 54 and the second joint part 55 is L1, the distance between the second joint part 55 and the third joint part 56 is L2, and the third joint part 56 and the fourth joint part If the distance between the first joint portion 54 and the fourth joint portion 57 is Lw, Lw + L2> L1 + L3, Lw> L2, L1> L3, Lw−L2 <L3-L1 In this case, the rotating and swinging state is established.

  In the second embodiment, a double swing state is established. In this case, as shown in FIG. 6B, the single-axis redundant robot 60 has the fourth arm body 53 fixed, and the first arm body 50 swings less than 360 degrees with respect to the fixed portion 58. The third arm body 52 can be displaced by a swing angle of less than 360 degrees with respect to the fourth arm body 53. Further, the second arm body 51 can be displaced by less than 360 degrees with respect to the first arm body 50, and the third arm body 52 can be displaced by less than 360 degrees with respect to the second arm body 51. Become.

  Moreover, in the 3rd form, it will be in a double rotation state. In this case, as shown in FIG. 6 (3), the single-axis redundant robot 60 can rotate the first arm body 50 with respect to the fixed portion 58 by 360 degrees or more with the fourth arm body 53 fixed. The third arm body 52 can rotate 360 degrees or more with respect to the fourth arm body 53. Further, the second arm body 51 can rotate 360 degrees or more with respect to the first arm body 50, and the third arm body 52 can rotate with respect to the second arm body 51 by 360 degrees or more.

  In the double rocking state and the double rotation state, if any one angle of the first to fourth joint portions 54 to 57 is set, the first arm 4th when the fourth arm body 53 is in a fixed state. There is one combination of angles that the joint portions 54 to 57 can take. Therefore, by setting one redundant joint portion among the first to fourth joint portions 54 to 57 and designating the angle thereof, it is possible to calculate all the ranges that the single-axis redundant robot can take. On the other hand, in the rotation and swing state, it is not possible to calculate the entire range that the single-axis redundant robot 60 can take only by setting one redundant joint portion from the first to fourth joint portions 54 to 57. .

  FIG. 7 is a diagram illustrating combinations of angles that can be taken by the joints when the single-axis redundant robot 60 is in a rotationally swinging state. FIG. 7A shows a case where the single-axis redundant robot 60 is in an initial state, and shows a state in which the first arm body 50 is angularly displaced with respect to the fixed portion 58 as shown in FIGS. In order.

  When the fourth joint portion 57 is selected as the redundant joint portion, as shown in FIG. 7 (3), the third arm body 52 is angled with respect to the fourth arm body 53 in one direction (counterclockwise in FIG. 7). When the displacement reaches the swing limit angle, the third arm body 52 cannot be angularly displaced in one direction with respect to the fourth arm body 53 any more. In this case, the first arm body 50 can no longer be angularly displaced in one direction with respect to the fixed portion 58. When the third arm body 52 is angularly displaced in the other direction (clockwise in FIG. 7) with respect to the fourth arm body 53 from the state where the swing limit angle has been reached, the first arm body 50 is also moved relative to the fixing portion 58. Angular displacement in the other direction. When the fourth joint portion is selected as the redundant joint portion in this way, the first arm body 50 cannot be rotated 360 degrees with respect to the fixed portion 58, and all possible ranges of the single-axis redundant robot 60 are calculated. I can't. On the other hand, when selecting the 1st joint part 54 as a redundant joint part, as shown to FIG. 7 (1)-FIG. 7 (5), the 1st arm body 50 is rotated with respect to the fixing | fixed part 58 once. It is possible to calculate all possible ranges of the single-axis redundant robot 60.

  Therefore, when the single-axis redundant robot 60 is in the rotational swing state, any one of the first joint portion 54 and the second joint portion 55, and any one of the third joint portion 56 and the fourth joint portion 57. By making two of them selectable as redundant joints, one of the two can be selected as redundant joints where the arm rotates 360 degrees or more in the rotating and swinging state. All possible ranges of the single-axis redundant robot 60 can be calculated.

  Even if the lengths of the arm bodies 50 to 53 of the single-axis redundant robot 60 are determined, the length Lw between the first joint portion 54 and the fourth joint portion 57 changes to change the length. It changes whether it becomes a rocking | fluctuation state, a double rocking | fluctuation state, and a double rotation state. As described above, one of the first joint portion 54 and the second joint portion 55 and one of the third joint portion 56 and the fourth joint portion 57 are replaced with the redundant joint portion. As a result, it is possible to calculate the entire range that the single-axis redundant robot 60 can take even if it is in a rotational swing state. This is common to uniaxial redundant robots having four joints. Accordingly, the same applies to the multi-joint robot 30 shown in FIG.

  FIG. 8 is a front view showing a state where the first joint portion 9 is a redundant joint portion and the angle q1 thereof is sequentially angularly displaced in the multi-joint robot 30 shown in FIG. It changes in the order of (3). Moreover, FIG. 9 is a graph which shows the time change of the angles q1-q4 of each joint part 9-12 in FIG. 9 (1) shows the time change of the angle q1 of the first joint part 9, FIG. 9 (2) shows the time change of the angle q2 of the second joint part 10, and FIG. 9 (3) shows the first change. The time change of the angle q3 of the 3 joint part 11 is shown, and FIG. 9 (4) shows the time change of the angle q4 of the 4th joint part 12. FIG.

  8 and 9, when the target point Ph is located at a fixed position, the multi-joint robot 30 is in a rotational swing state, and the first joint portion 9 and the second joint portion 10 can swing. The case where the 3rd joint part 11 and the 4th joint part 12 are rotatable is shown. In this case, when the angle q1 of the first joint portion 9 is sequentially changed by a predetermined amount of angular displacement every predetermined time, with the angular displacement of the first joint portion 9 in a state where the target point Ph is located at a predetermined position. The second to fourth joint portions 10 to 12 are angularly displaced.

  When the angle q1 of the first joint portion 9 is angularly displaced, for example, in the minus direction from the initial state shown in FIG. 8 (1), the articulated robot 30 is sequentially shown in FIGS. 8 (2) to 8 (3). The form of changes. When the angle q1 of the first joint portion 9 is angularly displaced in the minus direction, the first joint portion 9 becomes 0 degrees as shown in FIG. In this case, when the angle q4 of the fourth joint portion 12 is 180 degrees, the first arm body 3 and the second arm body 4 are stretched, and further angular displacement is impossible.

  FIG. 10 is a front view showing a state where the fourth joint portion 12 is a redundant joint portion and the angle q4 is sequentially angularly displaced in the articulated robot 30 shown in FIG. It changes in the order of (5). Moreover, FIG. 11 is a graph which shows the time change of the angles q1-q4 of each joint part 9-12 in FIG. FIG. 11 (1) shows the time change of the angle q1 of the first joint part 9, FIG. 11 (2) shows the time change of the angle q2 of the second joint part 10, and FIG. The time change of the angle q3 of the three joint portions 11 is shown, and FIG. 11 (4) shows the time change of the angle q4 of the fourth joint portion 12.

  10 and 11, when the target point Ph is positioned at a fixed position, the articulated robot is in a rotational swinging state, and the first joint unit 9 and the second joint unit 10 can swing, The case where the 3rd joint part 11 and the 4th joint part 12 can rotate is shown. In this case, when the fourth joint portion 12 is sequentially changed by a predetermined angular displacement amount at predetermined time intervals, the first joint portion 12 is moved together with the angular displacement of the fourth joint portion 12 in a state where the target point Ph is located at a predetermined position. The third joint portions 9 to 11 are angularly displaced.

  When the angle q4 of the fourth joint portion 12 is angularly displaced, for example, in the plus direction from the initial state shown in FIG. 11 (1), the multi-joint robot 30 is sequentially shown in FIGS. 11 (2) to 11 (5). The form of changes. When the angle q4 of the fourth joint portion 12 is designated and angularly displaced in the plus direction, even if the angle q4 of the fourth joint portion 12 is 180 degrees or more, the first arm body 3 and the second arm body As compared with the case where the first joint portion 9 is selected as the redundant joint portion, further angular displacement of the fourth joint portion 12 is possible.

  In this way, by selecting the fourth joint portion 12 as the redundant joint portion, all the possible points that the first to fourth joint portions 9 to 12 can take after maintaining the target point Ph in a fixed state. A combination of angles can be calculated. Actually, depending on the fixed position of the target point Ph, it is possible to select all the angles that the first to fourth joint portions 9 to 12 can take when the first or second joint portion is selected as the redundant joint portion. In some cases, combinations can be calculated.

  Therefore, by selecting one of the two of the first joint part 9 and the second joint part 10 and one of the two of the third joint part 11 and the fourth joint part 12 as redundant joint parts, It is possible to calculate all combinations of angles that can be taken by the first to fourth joint portions 9 to 12 while maintaining the target point Ph in a fixed state.

  Therefore, as a second embodiment of the present invention, the joint part operation switch 43 shown in FIG. 2 includes one of the first joint part 9 and the second joint part 10, and the third joint part 11 and the fourth joint part 12. Two of them are arranged to be selectable. Thus, even if only two switches are provided instead of four, all combinations of angles that can be taken by the first to fourth joint portions 9 to 12 can be calculated. Accordingly, the operator does not need to be able to select all of the four joint portions 9 to 12 as redundant joint portions, and can improve the operability of the input unit 20 in the form changing operation. Further, by changing the joint operation switch 43 from four to two switches, it is possible to reduce the number of switches necessary for the shape change operation, improve the operability, and reduce the number of parts necessary for the switch. .

  Further, the central processing unit 19 is configured so that one of the first and second joints 9 and 10 is selected as the redundant joint, and one of the third and fourth joints 11 and 12 is the redundant joint. It is only necessary to calculate each joint part in the case where it is selected, and it is possible to make two calculation programs necessary to calculate the angle of each joint part, compared with the case where four calculation programs are prepared. Thus, the storage capacity required for the central processing unit 19 can be reduced. In the present embodiment, the first joint portion 9 and the fourth joint portion 12 are set to be selectable as redundant joint portions. By making the first joint portion 9 and the fourth joint portion 12 selectable as redundant joint portions in this way, the angle of each joint portion is calculated as compared to selecting other joint portions as redundant joint portions. The calculation program for doing so can be simplified, and the angle of each joint can be obtained in a short time.

  FIG. 12 is a flowchart for explaining the procedure of another form changing operation of the central processing unit 19. First, in step b0, when the central processing unit 19 determines that the form change switch 45 has been pressed in a state where the articulated robot 30 can be controlled, the process proceeds to step b1 to start the form change operation.

  The central processing unit 19 sequentially performs the same procedure as steps a1 to a2 described above in steps b1 and b2, and proceeds to step b3. In step b3, the central processing unit 19 performs the first to fourth joint portions 9 to 9 according to the calculation program for obtaining the angles of the first to fourth joint portions 9 to 12, in the same manner as in step a3 described above. 12, the angles of the joint portions other than the redundant joint portion are analytically obtained. Moreover, since the angle of the redundant joint has already been determined in step b2, it can be obtained without calculation. When the angles q1 to q4 of the first to fourth joint portions 9 to 12 are obtained in this way, the process proceeds to step b4.

  In step b4, when the central processing unit 19 determines that the angle of the redundant joint determined in step b2 has not reached the movable limit, based on the calculation result in step b3, as in step a4. The process proceeds to step b5. If it is determined that the angle of the redundant joint determined in step b2 has reached or exceeded the movable limit, the process proceeds to step b9.

  In step b5, it is determined whether or not the current angle of the redundant joint portion has reached the movable limit. If it is determined that the current redundant joint has not reached the movable limit, the process proceeds to step b6. If it is determined that the current redundant joint has reached the movable limit, the process proceeds to step b7.

  In step b6, when two angles calculated in each joint part 9-12 are obtained, two discrete form codes previously used for obtaining the angles of the joint parts other than the redundant joint part are used. One angle is selected from the angles. For example, when the previous angle is determined using a positive discrete form code, in step b6, the angle obtained when the positive discrete form code is used is selected as the angle at which the joint portion should move. When one angle is selected from the two angles that can be taken by the joint, the process proceeds to step b8.

  In step b7, when two angles calculated in each joint part 9-12 are obtained, a discrete form code obtained by inverting the discrete form code used to obtain the angles of the joint parts other than the redundant joint part previously. Is used to select one angle from two angles. For example, when the previous angle is determined using a positive discrete form code, in step b7, the angle obtained when the negative discrete form code is used is selected as the angle at which the joint portion should move. When one angle is selected from the two angles that can be taken by the joint, the process proceeds to step b8.

  In step b8, the angles q1 to q4 of the first to fourth joint portions 9 to 12 obtained in step b6 or step b7 are given to the output unit 22 as calculation results, and the process proceeds to step b9. The output unit 22 gives the given calculation result to the articulated robot 30. As a result, the articulated robot 30 angularly displaces each of the joints 9 to 12 so that the designated redundant joint has a redundant angle while maintaining the position of the target point Ph.

  In step b9, the central processing unit 19 determines whether or not an end command for ending the form changing operation is given. For example, it may be determined that the end command is given by pressing the form change switch 45 again. If the central processing unit 19 determines that a command to end the form changing operation is not given, the central processing unit 19 returns to step b1 and waits for the joint operation switch 43 to be pressed again. If it is determined in step b9 that a command to end the form changing operation is given, the process proceeds to step b10, and the form changing operation is ended in step b10.

  FIG. 13 is a graph showing the angles q1 to q4 of the first to fourth joint portions 9 to 12 when the central processing unit 19 performs the procedure of the morphological operation shown in FIG. FIG. 13 (1) shows the time change of the angle q1 of the first joint part 9, FIG. 13 (2) shows the time change of the angle q2 of the second joint part 10, and FIG. The time change of the angle q3 of the three joint portions 11 is shown, and FIG. 13 (4) shows the time change of the angle q4 of the fourth joint portion 12.

  With the position of the target point Ph determined, the operator operates the input unit 20 and once presses the minus direction movement switch corresponding to the first joint unit 9. In this case, the central processing unit 19 determines the first joint portion 9 as a redundant joint portion. Further, the central processing unit 19 determines a position where the first joint unit 9 is angularly displaced in the minus direction with a predetermined angular displacement amount from the angle before the minus direction movement switch is pressed as a redundant angle.

  When the procedure of the morphological operation shown in FIG. 12 is performed, when the minus direction movement switch is sequentially pressed, the central processing unit 19 performs the operation from step b1 to step b6 through step b6 and step b8. As a result, the central processing unit 19 keeps the position of the target point Ph and decreases the angle q1 of the first joint portion 9 over time as shown in FIG. 13 (1). As shown in (4), the angles q2 to q4 of the second joint part 10 to the fourth joint part 12 are changed.

  If the minus direction movement switch is continuously pressed, the angle q1 of the first joint portion 9 reaches an angle at which the movable limit is reached (0 degrees in FIG. 13). In this case, the central processing unit 19 performs an operation reaching step b9 without going through steps b5 to b8. As a result, the central processing unit 19 does not calculate the angular displacement command of the joints, and maintains the angular displacements of the joints 9 to 12 in a stopped state. Even after the first time t1 when the movable limit is reached, even if the minus direction movement switch is further pressed, the angles of the joint portions 9 to 12 maintain the position where the movable limit is reached.

  When the plus direction movement switch corresponding to the first joint unit 9 is pressed at the second time t2 after the first time t1, the central processing unit 19 moves the first joint unit 9 in the plus direction. A position that is angularly displaced in the positive direction with a predetermined angular displacement amount from the angle before the switch is pressed is determined as a redundant angle. As a result, the angle q1 of the first joint portion 9 escapes from the movable limit. At the second time t2, the central processing unit 19 performs an operation from step b1 to step b9 via steps b7 and b8. In this case, the central processing unit 19 obtains the angle q4 of the fourth joint portion 12 by inverting the discrete form code used for obtaining the angle q4 of the fourth joint portion 12 before the first time t1.

  If the plus direction movement switch is continuously pressed after the second time t2, the central processing unit 19 performs an operation from step b1 to step b9 via step b6 and step b8. In this case, the central processing unit 19 obtains the angle q4 of the fourth joint unit 12 using the discrete form code used at the second time t2. As a result, the central processing unit 19 keeps the position of the target point Ph and increases the angle q1 of the first joint portion 9 over time as shown in FIG. 13 (1). As shown in (4), the angles q2 to q4 of the second joint part 10 to the fourth joint part 12 are changed.

  As described above, the discrete form codes used for obtaining the angles q1 to q4 of the joint portions 9 to 12 are different between before the first time t1 and after the second time. Therefore, by selecting only one redundant joint part, it is possible to calculate all combinations of angles that the first to fourth joint parts 9 to 12 can take while maintaining the target point Ph in a fixed state. Become. In other words, even when the multi-joint robot 30 is in a rotationally oscillating state and a joint part whose oscillating angle is displaced is selected, the first to fourth joint parts 9 to 12 can take. All combinations of angles can be calculated. Therefore, the operator only needs to select one of the four joint portions 9 to 12, and does not need to be able to select all as redundant joint portions, improving the operability of the input unit 20 in the form changing operation. can do.

  When the central processing unit 19 performs the above-described form changing operation, the joint operation switch 43 can be a single switch. In this case, it is possible to reduce the number of switches necessary for the form changing operation, and it is possible to reduce the number of parts necessary as a switch. In addition, it is possible to make one calculation program necessary for calculating the angle of each joint. Further, the calculation program can be simplified by making the first joint portion selectable as the redundant joint portion.

  FIG. 14 is a flowchart for explaining the procedure of the prediction notification operation of the central processing unit 19 in the form changing operation. In the present embodiment, in the form changing operation, the central processing unit 19 notifies the direction in which the remaining joint part is angularly displaced when the redundant joint part is determined, and the movement amount and movement direction of each arm body. It is possible to perform a predictive notification operation for informing the user. The prediction notification operation can be performed in combination with any of the form change operations shown in FIGS. Therefore, the central processing unit 19 serves as a prediction notification unit that predicts and notifies the direction of angular displacement of the plurality of joints.

  First, in step c0, when the central processing unit 19 determines that the operation command for the prediction notification operation is given in a state where the position of the target point Ph is determined, the process proceeds to step c1 and starts the prediction notification operation. . In step c1, the central processing unit 19 determines the movement amount of the redundant angle to be moved, and proceeds to step c2. Hereinafter, the movement amount includes the movement direction. For example, for example, if the plus direction movement switch or the minus direction movement switch of the joint part operation switch 43 is pressed immediately before, the amount of movement by which the redundant joint part is angularly displaced when the same movement switch is pressed next time. Determine the direction of movement.

  In step c2, the central processing unit 19 determines that the target point Ph is a fixed position based on the first Jacobian matrix Jh indicating the relationship between the movement amount of the target point Ph and the angles q1 to q4 of the joints 9 to 12. The movement directions of the angles q1 to q4 of the joint portions 9 to 12 are determined. Specifically, the central processing unit 19 can determine the amount of movement of the remaining angle by substituting the amount of movement obtained in step c1 into the determinant obtained by modifying the first Jacobian matrix Jh. When the movement amount of the redundant joint portion is determined in this way, when the movement amount of the remaining angle is calculated, the process proceeds to step c3.

  In step c3, the central processing unit 19 determines that the target point Ph is based on the second Jacobian matrix Je indicating the relationship between the set amount of movement of the arm body and the angles q1 to q4 of the joints 9 to 12. The movement amounts of the angles q1 to q4 of the joint portions 9 to 12 in the case of the fixed position are determined. Specifically, the central processing unit 19 is set by substituting the angles q1 to q4 of the joints 9 to 12 obtained in steps c1 and c2 into the determinant based on the second Jacobian matrix Je. The amount of movement of the arm body can be obtained. When the movement amount of the redundant joint portion is determined in this way, when the movement amount of the arm body to be set is calculated, the process proceeds to step c4.

  In step c4, the central processing unit 19 notifies the calculation result calculated in steps c2 and c3 by the display unit 42 of the input unit 20, and returns to step c1. The central processing unit 19 repeats steps c1 to c4 until a predetermined end condition is satisfied. When the end condition is satisfied, the predictive notification operation is ended. For example, the central processing unit 19 ends the predictive notification operation while ending the form changing operation.

  By performing the prediction notification operation in this way, in the form change operation, when the redundant joint is angularly displaced in the plus or minus direction, the remaining joint is informed in the plus or minus direction. And operability can be improved.

  Similarly, it can be notified whether the arm body set in the same way is angularly displaced in the plus or minus direction, and the operability can be improved. For example, the arm bodies to be set may be all of the first to fourth arm bodies 3 to 6 or may be any arm body that the operator wants to pay attention to. In the present embodiment, the central processing unit 19 notifies the approximate amount of the articulated robot 30 by notifying the movement amount of the elbow position p2 that is a portion between the third arm body 5 and the fourth arm body 6. The form can be grasped.

  By checking the notified information, the operator can easily grasp the direction in which each joint part and the arm body to be set will move, and the convenience can be improved. For example, when deforming an articulated robot to a form that avoids colliding with an obstacle by performing a form changing operation, the robot can predict a change in the form of the robot, so that the operator can collide with the obstacle. Can be prevented more reliably.

  Further, the calculation in the above-described prediction notification operation is performed based on the Jacobian matrices Jh and Je. Therefore, the amount of movement of each joint part 9 to 12 and the amount of movement of the arm body to be set, which are calculation results, are approximate solutions rather than analytical solutions. However, the broadcast information is desired to be calculated easily in a short time without requiring strict information. As shown in the present embodiment, the calculation result used for the prediction notification is obtained based on the Jacobian matrix, so that the calculation can be easily performed in a short time.

  Next, the detail of the movement amount calculation process of each joint part 9-12 in step c2 is demonstrated. The first Jacobian matrix Jh for obtaining the movement amount of the target point Ph based on the above-described equation (1) is represented by the following equation (5).

  Here, phx, phy, and phz are functions defined by equation (1), and Jh indicates the first Jacobian matrix.

  Further, the second Jacobian matrix Je for obtaining the elbow position p2 between the fourth arm body 5 and the third arm body 6 is expressed as L4 in the function of phx, phy and phz of the first Jacobian matrix Jh shown in the equation (5). = Represented by a matrix substituted with zero.

  Between the minute movement amount Δph of the target point Ph and the minute movement amount Δq of each joint part, there is a relationship of the following equation (6).

  In the form changing operation, the position of the target point Ph is fixed, so the left side of the equation (6) is zero. As a result, the expression (6) becomes the following expression (7).

  Here, Δq1 is the minute movement amount Δq1 of the first joint portion 9. This minute movement amount Δq1 is determined in step c1. For example, when the speed of moving the joint is determined in advance, the speed is a value obtained by multiplying the speed by the control period. When the first joint portion 9 is moved in the plus direction, Δq1 is positive, and when it is moved in the minus direction, Δq1 is negative.

  By substituting the minute movement amount Δq1 of the first joint portion 9 into the equation (7), the minute movement amounts Δq2 to q4 of the second to fourth joint portions 10 to 12 can be obtained. The central processing unit 19 stores equation (7). In step c2, by substituting the movement amount Δq1 of the redundant joint portion into the equation (7), the minute movement amounts Δq2 to Δq4 of the joint portions 10 to 12 other than the first joint portion which is the redundant joint portion are obtained. it can. By displaying on the display unit 42 the sign of the obtained movement direction of each joint part, the operator can grasp the movement direction in which the remaining joint parts move when the movement direction of the redundant joint part is selected. .

  Next, details of the movement amount calculation step of each joint 9 to 12 in step c3 will be described. The minute movement amount Δpe at the elbow position p2 and the minute movement amount Δq of each joint have the relationship shown in the following equation (8).

  Here, ΔPe is the minute movement amount ΔPe at the elbow position p2, and Je is the above-described second Jacobian matrix Je. By substituting into the equation (8) the minute movement amounts Δq2 to q4 of the joint portions 10 to 12 determined by the equation (7) and the minute movement amount Δq1 of the first joint portion 9 that is the redundant joint portion, The minute movement amount ΔPe at the position p2 can be obtained.

  The central processing unit 19 stores equation (8). In step c3, the movement amount Δq1 of the redundant joint portion and the minute movement amounts Δp1 to Δp4 of the joint portions 9 to 12 obtained in step c1 and step c2 are substituted into the equation (8), thereby moving the elbow position p2 slightly. The quantity ΔPe can be determined. By displaying the obtained moving direction of the elbow position p2 on the display unit 42, when the operator selects the moving direction of the redundant joint, the moving direction in which the elbow position p2 moves, for example, the base 17 is used as a reference. The direction of movement up, down, left and right can be grasped.

  FIG. 15 is a block diagram showing the control device 2 of the articulated robot 1 according to the second embodiment of the present invention. The multi-joint robot 1 and the control device 2 shown in FIG. 15 have a configuration similar to that of the multi-joint robot 30 and the control device 2 shown in FIG. 1, and the similar configurations are denoted by the same reference numerals and the description thereof is omitted. To do.

  The multi-joint robot 1 shown in FIG. 15 is a redundant robot having seven degrees of freedom, and includes six arm bodies 3 to 8 and seven joint portions 9 to 15 that rotatably connect two adjacent arm bodies. Have. Each arm body 3-8 is connected, respectively, and comprises the arm structure 16 extended in series. The articulated robot 1 includes a base 17 connected to the base end of the arm component 16 and an end effector 18 connected to the tip of the arm component 16.

  Each joint part 9-15 is either a coaxial joint part or an inclined joint part. Specifically, the first joint part 9, the fifth joint part 13 and the seventh joint part 15 are coaxial joint parts, and the second joint part 10, the third joint part 11, the fourth joint part 12 and the sixth joint part. The joint part 14 is an inclined joint part. Therefore, the 1st-4th arm bodies 3-6 and the 1st-4th joint parts 9-12 have the structure similar to the articulated robot 30 shown in FIG.

  One end portion of the fourth arm body 6 is inclined by the fourth joint portion 12 at a predetermined angle with respect to the axis A5 of the third arm body 5 and the axis A6 of the fourth arm body 6, 45 degrees in the present embodiment. The third arm body 5 is coupled to the other end of the third arm body 5 so as to be rotatable around the fourth rotation axis A12. One end portion of the fifth arm body 7 is rotatable by the fifth joint portion 13 about a fifth rotation axis A13 coaxial with the axis A6 of the fourth arm body 6 and the axis A7 of the fifth arm body 7, It is connected to the other end of the arm body 6.

  One end portion of the sixth arm body 8, which is the other end portion of the arm structure 16, has a predetermined angle with respect to the axis A 7 of the fifth arm body 7 and the axis A 8 of the sixth arm body 8 by the sixth joint portion 14. In this embodiment, it is connected to the other end portion of the fifth arm body 7 so as to be rotatable around the sixth rotation axis A14 inclined at 45 degrees. The end effector 18 serving as the tip of the articulated robot 1 is moved around the seventh rotational axis A15 coaxial with the axis A8 of the sixth arm body 8 by the seventh joint 15 at the other end of the sixth arm body 8. It is connected rotatably. The end effector 18 is a setting portion in the articulated robot 1 whose position and posture should be controlled.

  Thus, each joint part 9-15 is connected via an arm body so that angular displacement is mutually possible. Further, as the joint portions 9 to 15 are angularly displaced, the arm structure 16 is configured to be deformable into a state in which the respective axes of the arm bodies are arranged coaxially and extend in a straight line as shown in FIG. The In a state where the axes A3 to A6 of the arm bodies 3 to 6 extend in a straight line, the second rotation axis A10 and the third rotation axis A11 are parallel, and the third rotation axis A11 and the fourth rotation axis A12 are vertical. It becomes. In addition, the target point Ph set in the fourth arm body 6 is disposed at the distal end portion of the arm constituent body 16, and the base 17 is disposed at the other end portion of the arm constituent body 16.

  The rotation axes of the three joints arranged from one end in the series direction toward the other end in the series direction, that is, the rotation axis A6 of the fifth joint 13, the rotation axis A7 of the sixth joint 14, and the seventh It intersects with the rotation axis A8 of the joint portion 15 at a three-axis intersection P that is a predetermined point. As described above, the fifth to seventh joint portions 13 to 15 become three three-axis intersection forming joint portions constituting the three-axis intersection point P.

  The articulated robot 1 has seven degrees of freedom, and the end effector 18 can be positioned at an arbitrary position and posture by rotating the joint portions 9 to 15. The multi-joint robot 1 is suitably used when the equipment and the like are complicated and the work route is complicated, or when the operation range cannot be increased by being arranged in parallel with other robots. The end effector 18 may be a hand device such as a welding torch and a handling device. A rotation mechanism for rotating the arm bodies 3 to 8 has the same configuration as the articulated robot 30 shown in FIG. The configuration of the control device 2 is the same as that of the control device 2 shown in FIG.

  The central processing unit 19 performs calculations necessary for controlling the articulated robot 1 in response to various information provided from the input unit 20 according to an operation program stored in advance. In the present embodiment, the central processing unit 19 calculates angles q1 to q7 of the joints 9 to 15 necessary for moving the end effector 18 to a predetermined target position and target posture, respectively.

  The control device 2 calculates angles q1 to q7 of the joint portions 9 to 15 to be rotated when the end effector 18 is moved along a movement path designated in advance. This movement path is input from the input unit 20 to the central processing unit 19 by the operator before the robot moves. The articulated robot 1 controlled by the control device 2 according to the present invention has seven joint portions 9 to 15 and thus has redundancy. That is, there are a plurality of combinations of angles q1 to q7 that can be taken by the joint portions 9 to 15 with the end effector 18 fixed to the base 17 at a predetermined position and posture.

  The control device 2 of the present embodiment can perform the above-described form changing operation. In performing the shape changing operation, the control device 2 determines the redundant joint portion selected from the plurality of joint portions 9 to 15, the redundant angle of the redundant joint portion, and the position and posture where the end effector 18 is to be fixed. Is given via the input unit 20. The central processing unit 19 is a calculation unit that calculates the angle of the remaining joint part when the redundant joint part has a redundant angle after the end effector 18 is fixed in a fixed state. The output unit 22 gives the calculation result of the central processing unit 19 to the articulated robot. The articulated robot to which the calculation result is given sets the redundant joint portion to the redundant angle, and aligns the angles of the remaining joint portions according to the calculation result, thereby setting the end effector 18 in the designated fixed state.

  By performing the form changing operation in this manner, the form such as the shape and posture of the arm structure 16 can be changed while the end effector 18 is fixed. The operator confirms the form of the robot while operating the robot, and thereby the joint portions 9 to 15 that realize the form of the arm structure 16 that avoids the collision with the obstacle while the end effector 18 is fixed. Can be easily determined.

  FIG. 16 is a front view showing the input unit 20 of the control device 2 that controls the articulated robot shown in FIG. The configuration of the input unit 20 has the same configuration as that in FIG. Specifically, the input unit 20 includes an operation unit 41 and a display unit 42. The operation unit 41 includes each joint operation switch 43, a tool movement switch 44, and a form change switch 45. The operation unit 41 further includes a command key for inputting a robot operation program, a cursor key, a numeric keypad, and the like.

  The joint part operation switch 43 is a switch for inputting the angles q1 to q7 of the joint parts 9 to 15, and for each joint part 9 to 15, the angle of a predetermined angular displacement amount in the plus direction and the minus direction. A displacement command is given. The tool movement switch 44 is given a movement command for the end effector 18 in accordance with the base coordinate axis or the tool coordinate axis. The form change switch 45 is given a form change command for changing the form of the arm structure 16 while the target point Ph is fixed at a predetermined position.

  When performing the form changing operation, the tool movement switch 44 serves as a fixed condition input means for inputting the position and posture to which the end effector 18 should be fixed. The form change switch 45 and the joint operation switch 43 constitute a redundant joint input means for inputting a redundant joint part and a redundant angle input means for inputting a redundant angle that is an angle of the redundant joint part. .

  FIG. 17 is a flowchart for explaining the procedure of the form changing operation of the central processing unit 19 in the second embodiment. First, in step d0, when the central processing unit 19 determines that the form change switch 45 is pressed in a state where the articulated robot 1 can be controlled, the process proceeds to step d1 and starts the form change operation.

  In step d1, when the movement command of the end effector 18 is given by the tool movement switch 44, the central processing unit 19 gives the output unit 22 a calculation result for moving the end effector 18 to the designated position and posture. . If there is no movement command by the tool movement switch 44, the current position and posture of the end effector 18 are maintained. When the central processing unit 19 determines that the joint operation switch 43 has been pressed, the central processing unit 19 determines the position and posture of the end effector 18 at that time as fixing conditions, and proceeds to step d2.

  In step d2, the central processing unit 19 determines the joint designated by the joint operation switch 43 as a redundant joint. The redundant joint portion is any one of the first to fourth joint portions 9 to 12. Further, the angle designated by the joint operation switch 43 is determined as the redundant angle. When the redundant joint portion and the redundant angle are thus determined, the process proceeds to step d3.

  In step d3, the central processing unit 19 calculates the coordinates of the three-axis intersection P based on the position and orientation of the end effector 18 determined in step d1. Since the triaxial intersection point P is a point where the rotation axes A13, A14, A15 of the fifth joint part 13, the sixth joint part 14, and the seventh joint part 15 intersect at one point, the position and posture of the end effector 18 are determined. Then, it can be determined uniquely. Thus, the central processing unit 19 becomes a three-axis intersection calculation means for calculating the three-axis intersection P, and when the three-axis intersection P is obtained, the process proceeds to step d4.

  In step d4, the central processing unit 19 determines the angles q1 to q1 of the first to fourth joint portions 9 to 12 when the three-axis intersection P is located at the three-dimensional position of the three-axis intersection P calculated in step d3. q4 is obtained. Specifically, the forward conversion relational expression representing the coordinates of the three-axis intersection P with the angles q1 to q4 of the first to fourth joint parts 9 to 12 as variables, and the redundancy of the redundant joint part determined in step d2 Based on the angle and the coordinates of the triaxial intersection P calculated in step d3, the angles q1 to q4 of the first to fourth joint portions 9 to 12 can be obtained analytically. When analytically obtaining the angles q1 to q4 of the first to fourth joint portions 9 to 12, the same method as in step a3 described above is used by replacing the coordinates of the target point Ph with the coordinates of the triaxial intersection P. Can be obtained.

  Therefore, the central processing unit 19 substitutes the redundant angle of the redundant joint portion and the x coordinate, y coordinate, and z coordinate of the three-axis intersection point P, so that the redundant joint among the first to fourth joint portions 9 to 12 is used. A calculation program representing an equation capable of analytically obtaining the angles of the joint portions other than the portion is stored. The central processing unit 19 analyzes the angle of each joint part other than the redundant joint part among the first to fourth joint parts 9 to 12 according to the calculation program of the first to fourth joint parts 9 to 12. To step d5. As described above, the central processing unit 19 serves as a target joint angle calculation unit that calculates the angles q1 to q4 of the first to fourth joints 9 to 12 to which attention is paid.

  In step d5, when the central processing unit 19 determines that the redundant angle of the redundant joint portion determined in step d2 has not reached the movable limit, the central processing unit 19 proceeds to step d6 and has reached the movable limit or is movable. If it is determined whether the limit is exceeded, the process proceeds to step d8.

  In step d6, the central processing unit 19 obtains angles q5 to q7 of the fifth to seventh joint portions 13 to 15 at which the end effector 18 is in the fixed position and the fixed posture. The posture of the fourth arm body 6 can be obtained from the angles q1 to q4 of the first to fourth joint portions 9 to 12 obtained in step d4. In a state where the posture of the fourth arm body 6 is determined, the angles q5 to q7 of the fifth to seventh joint portions 13 to 15 when the end effector 18 assumes a predetermined position and posture can be uniquely determined. it can.

  Specifically, the central processing unit 19 uses the angles q1 to q4 of the first to fourth joint portions 9 to 12 and the position and orientation of the end effector 18 to use the fifth to seventh joint portions 13. The fifth to seventh joint calculation programs representing inverse transformation equations capable of analytically obtaining the angles of ˜15 are stored.

  The central processing unit 19 obtains angles q5 to q7 of the fifth to seventh joint portions 13 to 15 according to the fifth to seventh joint portion calculation programs, respectively. As described above, the central processing unit 19 serves as intersection forming joint portion angle calculating means for calculating the angles q5 to q7 of the fifth to seventh joint portions 13 to 15, and proceeds to step d7 when each angle is obtained.

  In step d7, the angles q1 to q7 of the first to seventh joint portions 9 to 15 obtained in step d4 and step d6 are given to the output unit 22 as calculation results. The output unit 22 gives the given calculation result to the articulated robot 1. As a result, in the articulated robot 1, the joint portions 9 to 15 are angularly displaced, and the redundant joint portion has a redundant angle in a state where the end effector 18 is maintained at the fixed position and the fixed posture. At this time, the angles q1 to q7 of the first to seventh joint portions 9 to 15 are obtained by an analytical calculation method, unlike an approximation method such as a convergence calculation. Therefore, before and after the movement of the redundant joint portion, Thus, the end effector 18 is not displaced from the fixed position and the fixed posture. When the calculation result is given to the output unit 22 and the articulated robot 1 is operated in this way, the process proceeds to step d8.

  In step d8, the central processing unit 19 determines whether or not an end condition for ending the form changing operation is satisfied. For example, it may be determined that the end condition is satisfied by pressing the form change release switch. When determining that the end condition is not satisfied, the central processing unit 19 returns to step d1 and waits for the joint unit operation switch 43 to be pressed. When the joint operation switch 43 is pressed again in this state, steps d1 to d8 are repeated according to the instruction. When it is determined in step a7 that a command to end the form changing operation is given, the process proceeds to step a8 and the form changing operation is ended.

  In this way, the central processing unit 19 performs the form changing operation, so that the joint parts 9 to 15 are angularly displaced while the position and posture of the end effector 18 are fixed, and the form of the articulated robot 1 is changed. It can be changed. Therefore, the same effect as the first embodiment can be obtained.

  Further, the multi-joint robot 1 shown in FIG. 15 cannot be obtained analytically if one of the fifth to seventh joint portions 13 to 15 is selected as the redundant joint portion. On the other hand, in this Embodiment, an analytical solution can be obtained by selecting any one of the 1st-4th joint parts 9-12 as a redundant joint part. In addition, operability can be improved by selecting one of the four joint portions 9 to 12 rather than selecting one of the seven joint portions 9 to 15.

  As shown in FIG. 16, when the redundant joint portion is determined using each joint portion operation switch 43, the first joint portion to the fourth joint portions 9 to 12 are in a state where the form change switch 45 is pressed. The selection switch is enabled and the selection switches of the fifth to seventh joint portions 13 to 15 are disabled. As a result, the redundant joint portion can be selected from any of the first to fourth joint portions 9 to 12, and malfunctions can be prevented.

  Moreover, in this Embodiment, although the triaxial intersection was comprised by the 5th-7th joint parts 13-15, it is not restricted to this. For example, even if the first to third joint portions 9 to 11 constitute a three-axis intersection, an analytical solution can be similarly obtained. In this case, one redundant joint portion is selected from the fourth to seventh joint portions 12 to 15. Then, by converting from the coordinate system set in the end effector 18 to a coordinate system inverted to the coordinate system in which the first joint portion is viewed, it can be similarly obtained analytically.

  That is, it includes seven joint portions that are connected to each other via an arm body so as to be angularly displaceable and are arranged in series, and among the seven joint portions 9 to 15, the three joint portions are arranged from one end portion in the series direction toward the other end portion in the series direction. In a 7-DOF multi-joint robot having three intersection forming joints that form a three-axis intersection where the rotation axes of the two joints intersect at one point, the position and posture of the other end relative to one end of the arm structure are fixed When input as a condition, one of the four target joints other than the three intersection forming joints is input as a redundant joint, so that the angles of the joints 9 to 15 in the shape change operation Can be obtained analytically.

  As described above, even in the second embodiment, any one of the first and second joint portions 9 and 10 and any one of the third and fourth joint portions 11 and 12 are used as redundant joint portions. You may make it select. Moreover, the form change operation | movement shown in FIG. 12 may be performed, and a movement prediction operation | movement may be performed. The movement prediction operation and the shape change operation may be performed in combination with a movement teaching operation for teaching the movement path by moving the end effector 18 along the movement path.

  FIG. 18 is a flowchart for explaining the operation procedure of the central processing unit 19 when performing the movement prediction operation, the form change operation, and the movement teaching operation. First, at step e0, the central processing unit 19 proceeds to step e1 when the movement command of the end effector 18 is given by the tool movement switch 44 of the input unit 20 in a state where the articulated robot 1 can be controlled.

  In step e1, the central processing unit 19 calculates a movement path for moving the end effector 18 in accordance with the information given to the tool movement switch 44. When the end effector 18 moves according to the designated movement path, there may be a plurality of combinations of angles of the joint portions. In this case, the central processing unit 19 selects one from a combination of a plurality of angles according to a predetermined rule. For example, the combination of angles is selected so that the angular displacement of the joint portion on the end effector 18 side is reduced. The central processing unit 19 gives the calculation result to the articulated robot 1 via the output unit 22, and proceeds to step e2. Thereby, the articulated robot 1 moves the end effector 18 along the designated movement path.

  In step e2, it is determined whether any of the first to fourth joint portions 9 to 12 reaches the warning range near the singular point. For example, the central processing unit 19 has the absolute value of the determinant of the Jacobian matrix that converts the angles q1 to q4 of the first to fourth joint portions 9 to 12 into the three-axis intersection position is equal to or less than a predetermined set value. When the determination is made, it is determined that any one of the angles q1 to q4 of the first to fourth joint portions 9 to 12 has reached the warning area near the singular point.

  If it is determined in step e2 that any of the first to fourth joint portions 9 to 12 has reached the warning range, the process proceeds to step e3, and if not, the process proceeds to step e6. In step e3, the prediction notification operation shown in FIG. 14 is performed. FIG. 19 is a diagram showing a display screen displayed on the display unit 42 of the input unit 20. For example, as shown in FIG. 19, the central processing unit 19 displays the redundant joint portion that is approaching the movable limit. Further, the central processing unit 19 displays the angular displacement direction in which the joint portion approaching the movable limit escapes from the singular point and the direction in which the elbow position p2 moves when moving in the escape direction. When the prediction information is displayed in this way, the process proceeds to step e4.

  In step e4, the central processing unit 19 determines whether or not a form change command has been given. If it is determined that a form change has been given, the process proceeds to step e5, and if not, the process proceeds to step e6. In step e5, the movement of the end effector 18 is stopped, the form change operation shown in FIG. 3 or FIG. 12 is started, and if it is determined that the form deformation operation has ended, the process proceeds to step e6.

  In step e6, the central processing unit 19 determines whether or not an end condition for ending the moving operation is satisfied. If the central processing unit 19 determines that the moving operation end condition is not satisfied, the central processing unit 19 returns to step e1 and waits for the tool movement switch 44 to be pressed again. When the tool movement switch 44 is pressed again in this state, steps e11 to e6 are sequentially performed according to the instruction. If it is determined in step e6 that the end condition is satisfied, the process proceeds to step e7, and the moving operation is ended in step e7.

  In this way, the operator changes the robot configuration while fixing the position of the end effector 18 during the movement operation, moves the redundant joint part away from the movable limit, and again in the direction in which the movement command is given. The end effector 18 can be moved. Thus, the convenience can be improved by causing the movement operation to perform the prediction operation and the form change operation by the central processing unit 19.

  The above-described embodiment is merely an example of the invention, and the configuration can be changed within the scope of the invention. For example, in the present embodiment, online teaching, that is, the angle of each joint portion is obtained while operating the robot, and is used for teaching the robot operation plan. However, the present invention is not limited to this. Therefore, the computer which executes the program which calculates | requires the angle of each joint part among the central processing parts 19 belongs to this invention. Each joint part in which the redundant joint part has a redundant angle by keeping the target point Ph and the end effector 18 in a predetermined fixed state with the computer input conditions such as the type of joint of the robot and the length of the arm body. The angle may be obtained analytically, and the analysis result may be given to the robot. That is, the method of the present invention can also be used for off-line teaching and robot motion simulation. In the present embodiment, the relational expression for obtaining the position of the target point shown in the equation (1) is modified, and the angle of each joint is obtained based on the calculation formula obtained by solving the simultaneous equations. The relational expression for finding the posture of the point, the relational expression for finding a part of the position of the target point and a part of the posture are modified, and the angle of each joint is calculated based on the calculation formula that solves the simultaneous equations. You may ask for it.

  In the present embodiment, the multi-joint robot having the tilt joint portion and the coaxial joint portion has been described. However, even in the case of another robot having redundancy, the angle of each joint portion is obtained analytically in the same manner. be able to. For example, even in the case of a four-joint robot shown in FIG. 6, the angles of the joint portions can be obtained in the same manner, and the posture of the setting portion may be fixed in addition to the position.

  In the present embodiment, the description has been given of the single-axis redundant robot. However, even a robot having two or more axes of redundancy can be obtained similarly. In this case, the solution of each joint part can be analytically obtained by selecting two or more redundant joint parts. That is, the plurality of joints of interest so that the number of remaining joint portions excluding the redundant joint portion among the plurality of joints of interest matches the number of degrees of freedom in which the setting portion is constrained according to a predetermined fixed condition. By inputting at least one of the parts as a redundant joint part, simultaneous equations can be solved, and the angle of each joint part can be obtained analytically.

  That is, among a plurality of (N) joints of interest, the number (N−n) excluding the input number (n) of redundant joints is the number of degrees of freedom (M) in which the set part is constrained. Must match. In this case, if a variable included in the forward conversion relational expression is an unknown, the number of forward conversion relational expressions (M) each having a degree of freedom component to which the set part is constrained as a solution, and an unknown number included in the forward conversion relational expression Is equal to the number of (N−n). By solving these plurality of forward conversion relational expressions simultaneously, the value of each variable, that is, each angle of the remaining joint portion can be analytically determined. For example, when performing online teaching in a two-axis redundant robot, two joint portions are determined as redundant joint portions, of which the first redundant joint portion is fixed at an arbitrary angle, and the second redundant joint portion is changed. Accordingly, a calculation method similar to the calculation method described above can be applied, and analytical solutions of combinations of angles that can be taken by each joint portion when the second redundant angle is changed can be sequentially obtained.

DESCRIPTION OF SYMBOLS 1 Articulated robot 2 Control apparatus 3 1st arm body 4 2nd arm body 5 3rd arm body 6 4th arm body 9 1st joint part 10 2nd joint part 11 3rd joint part 12 4th joint part 19 Central calculation Processing unit 20 Input unit 22 Output unit ph Target point

Claims (11)

A plurality of joint portions that are connected to each other through an arm body so as to be angularly displaceable and arranged in series, and among the plurality of joint portions, a setting portion that is set to a joint portion at one end portion in the series direction is determined in advance and A controller for an articulated robot in which there are a plurality of combinations of angles that can be taken by the plurality of joint portions in a fixed state that indicates at least one of postures,
A fixed condition input means for inputting a fixed condition for fixing the setting portion;
A redundant joint input means for inputting redundant joints, wherein the number of remaining joints excluding the redundant joints among the plurality of joints is the number of degrees of freedom in which the set part is constrained according to a fixed condition Redundant joint portion input means in which at least one of the plurality of joint portions is input as a redundant joint portion so as to match
Redundant angle input means for inputting the angle of the redundant joint as a redundant angle;
Based on the fixed condition input by the fixed condition input means and the redundant angle input by the redundant angle input means, a relational expression for obtaining the position or posture of the set portion using each angle of the plurality of joints as a variable is Unraveling, angle calculation means for calculating the angles of a plurality of joints, each of which should be fixed under fixed conditions,
A control apparatus for an articulated robot, comprising: output means for giving the result of calculation calculated by the angle calculating means to the articulated robot. As a fixed condition, a condition in which the three degrees of freedom of the setting portion with respect to the joint portion at the other end in the series direction is constrained,
The redundant joint portion input means includes one joint portion of two joint portions arranged from one end portion in the series direction toward the other end portion in the series direction, and a series portion from the other end portion in the series direction. 2. The control apparatus for an articulated robot according to claim 1, wherein one of the two joints arranged toward one end in the direction can be selected as a redundant joint.   When the angle calculation means determines that the redundant joint portion reaches the movable limit while angularly displacing the redundant joint portion from the state before the input to the redundant angle input to the redundant angle input means, the angle calculating means reaches the movable limit. The multi-joint robot according to claim 1 or 2, wherein the angle of the joint part in the reached state is calculated.   If the angle calculating means determines that the redundant joint portion escapes from the state where the movable joint has reached the movable limit while the redundant joint portion is angularly displaced to the redundant angle input to the redundant angle input means, 4. The control apparatus for an articulated robot according to claim 3, wherein a combination of angles different from the combination of angles obtained before reaching the movable limit is calculated from the two combinations of angles of the joint portion.   Based on the Jacobian matrix that converts the angle change of the plurality of joint portions into the position change of the setting portion and the redundant angle input to the redundant angle input means, the setting portion is constrained under a fixed condition, and the redundant joint portion is 5. The apparatus according to claim 1, further comprising a prediction notifying unit that predicts a direction of angular displacement of the plurality of joints and notifies a prediction result when angularly changing to a redundant angle. Multi-joint robot control device.   6. The apparatus according to claim 1, further comprising: a warning notification unit that notifies a determination result when it is determined that any one of the plurality of joints has reached a warning range set near a movable limit. The control apparatus of the articulated robot as described in one.   7. The articulated robot control device according to claim 6, wherein the warning notifying means notifies the angular displacement direction in which the joint portion reaching the warning range moves away from the movable limit. A coaxial joint that rotatably connects two adjacent arm bodies around a rotation axis that is coaxial with the axis of each arm body, and a rotation axis that is inclined at a predetermined angle with respect to the axis of each arm body The articulated robot control apparatus according to claim 1, wherein the articulated robot includes an inclined joint unit rotatably connected to the articulated robot. Rotation of three joint portions arranged in series from one end in the series direction to the other end in the series direction, including seven joint portions that are connected to each other via an arm body so as to be angularly displaceable and arranged in series A control device for a multi-joint robot with 7 degrees of freedom having three intersection-forming joints that form three-axis intersections whose axes intersect at one point,
A fixed condition input means for inputting a position and a posture to be fixed as a fixed condition among the seven joint parts to be set to the joint part at one end in the series direction;
Redundant joint input means in which one of the four basic joints other than the three intersection forming joints is input as a redundant joint;
Redundant angle input means for inputting the angle of the redundant joint as a redundant angle;
3-axis intersection position calculating means for calculating a fixed position of the 3-axis intersection in a state where the setting portion is to be fixed and in a posture;
Based on the position of the three-axis intersection calculated by the three-axis intersection position calculation means and the redundant angle input by the redundant joint input means, the position of the three-axis intersection is determined using each angle of the four basic joints as a variable. A basic joint angle calculating means for calculating the angles of the four basic joint parts so that the three-axis intersection is located at a fixed position by solving the relational expression for obtaining;
Intersection formation joint angle for calculating the angles of the three intersection formation joint sections based on the calculation result calculated by the target joint section angle calculation means and the position and orientation of the set portion input by the fixed condition input means Computing means;
An apparatus for controlling an articulated robot, comprising: output means for giving the result of computation calculated by each angle computing means to the articulated robot. A plurality of joint portions that are connected to each other through an arm body so as to be angularly displaceable and arranged in series, and among the plurality of joint portions, a setting portion that is set to a joint portion at one end portion in the series direction is determined in advance and A control method for controlling an articulated robot in which a plurality of combinations of angles that can be taken by the plurality of joints of interest exist in a fixed state that indicates at least one of postures,
A fixing condition determining step for determining a fixing condition for fixing the setting portion;
A redundant joint part determining step for determining a redundant joint part, wherein the number of remaining joint parts excluding the redundant joint part among a plurality of joint parts matches the number of degrees of freedom in which the set part is constrained according to a fixed condition A redundant joint determination step for determining at least one of the plurality of joints as a redundant joint; and
A redundant angle determining step of determining the angle of the redundant joint as a redundant angle;
Based on the fixed condition determined by the fixed condition determining step and the redundant angle determined by the redundant angle determining step, solving the relational expression for obtaining the position or posture of the set part using each angle of the plurality of joints as a variable , An angle calculation step for calculating the angles of a plurality of joints, each of which should be fixed under a fixed condition;
And a control step of controlling the robot according to an angle calculation step. A plurality of joint portions that are connected to each other through an arm body so as to be angularly displaceable and arranged in series, and among the plurality of joint portions, a setting portion that is set to a joint portion at one end portion in the series direction is determined in advance and A calculation method for calculating an angle of each joint part in a multi-joint robot in which a plurality of combinations of angles that can be taken by the plurality of joint parts of interest exist in a fixed state that indicates at least one of postures. ,
A fixing condition determining step for determining a fixing condition for fixing the setting portion;
A redundant joint part determining step for determining a redundant joint part, wherein the number of remaining joint parts excluding the redundant joint part among a plurality of joint parts matches the number of degrees of freedom in which the set part is constrained according to a fixed condition A redundant joint determination step for determining at least one of the plurality of joints as a redundant joint; and
A redundant angle determining step of determining the angle of the redundant joint as a redundant angle;
Based on the fixed condition determined by the fixed condition determining step and the redundant angle determined by the redundant angle determining step, solving the relational expression for obtaining the position or posture of the set part using each angle of the plurality of joints as a variable An angle calculation step of calculating the angles of a plurality of joint portions, each of which should be fixed under a fixed condition, and an angle calculation method for each joint portion of the articulated robot.

Images