JP2018015896A - Robot device and robot control method - Google Patents

Abstract

PROBLEM TO BE SOLVED: To provide a robot device capable of improving an operation speed of a multi-joint robot by appropriately processing a singularity, and a robot control method.SOLUTION: A robot device 1 includes: a multi-joint robot 2; and a controller 3 for driving and controlling the multi-joint robot 2 on the basis of an operation command input from a teaching pendant 4 capable of operating the multi-joint robot 2. The controller 3 includes: a joint angle computing unit 32 for computing each joint angle command for driving the multi-joint robot 2 on the basis of the operation command; a servo control unit 30 for rotatably driving a rotary joint so as to operate the multi-joint robot 2 on the basis of the joint angle command computed by the joint angle computing unit 32; a singularity calculating unit 51 for calculating a distance between the multi-joint robot 2 and a singularity of the multi-joint robot 2; and a maximum joint angle deviation adjust unit 52 for limiting a maximum rotational speed of the rotary joint that is preliminary specified on the basis of a singularity kind, when a singularity distance is smaller than a prescribed value.SELECTED DRAWING: Figure 5

Description

  The present invention relates to a robot apparatus having an articulated robot and a method for controlling the articulated robot.

  In recent years, there have been an increasing number of development cases for robotic devices that allow multi-joint robots to perform work such as assembly of workpieces. The ultimate purpose of such a robot apparatus is to move a multi-joint robot like a human hand and to perform operations such as assembling complex workpieces at high speed.

  However, multi-joint robots have multiple poses called so-called singular points where one position and posture specified in the three-dimensional space is not fixed to one in the joint space. It is a hindrance to move. Therefore, it is important to appropriately handle singular points in the development of robotic devices.

Here, FIG. 13 shows a block diagram of a general control apparatus for controlling the articulated robot. In FIG. 13, the current position C _i indicates the position and orientation of the hand of the manipulator expressed in a three-dimensional space, and the deviation command V _i is the deviation of the position and orientation of the hand for each predetermined control cycle (for example, 2 ms) Is shown. Since the control cycle is constant, the deviation command V _i represents the speed is generated in each control cycle by such teaching pendant.

In such a configuration, the position command calculation unit 150 adds the deviation command V _i to the current position C _i to obtain the next position C _{i + 1} , and the reverse mechanism calculation unit 155 calculates the next position C _{i + 1} by reverse mechanism calculation. The next joint angle command q _{i + 1} is obtained. The articulated robot is driven and controlled by transmitting this to the servo control device 130 that controls the joint of the articulated robot. However, when the articulated robot is in the vicinity of a singular point, there is a problem that the joint angle command q _{i + 1} greatly changes with respect to the previous joint angle command q _i , and there is a risk of abrupt movement.

  For this, a method of limiting the speed of the joint angle command is conceivable. However, if the speed of the joint angle command is limited, it may deviate greatly from the initial target position of the hand. There is a problem that it becomes difficult. On the other hand, a technique is disclosed in which the articulated robot is prevented from abruptly operating near a singular point by limiting the speed at which the joint angular velocity is excessively slowed (see Patent Document 1). ).

JP2003-300193A

  However, since the technique disclosed in Patent Document 1 always limits the angular velocity of the joint, there is a problem that the operation of the articulated robot becomes slow even in a position and orientation other than the singular point.

  Accordingly, an object of the present invention is to provide a robot apparatus and a robot control method capable of appropriately processing singular points and improving the operation speed of an articulated robot.

  The present invention includes an articulated robot having a plurality of rotary joints, and a control device that drives and controls the articulated robot based on an operation command input from an operation unit capable of operating the articulated robot. In the robot apparatus, the control device calculates a joint angle command of each of the plurality of rotary joints for driving the articulated robot based on the operation command, and the joint angle calculation unit And a joint drive control unit that operates each of the articulated robots by rotating and driving each of the plurality of rotary joints based on the joint angle command calculated by: and a singular point angle of the rotary joint relative to a singular point of the joint robot A singular point calculation unit that calculates a singular point distance, and when the singular point angle or singular point distance calculated by the singular point calculation unit is smaller than a predetermined value, And joint velocity restrictor for limiting the rotational speed of the rotary joint specified in advance Zui, characterized by comprising a.

  Further, the present invention provides a robot control method for driving and controlling the articulated robot based on an operation command input from an operation means capable of operating an articulated robot having a plurality of rotary joints. A joint angle calculation step for calculating a joint angle command for each of the plurality of rotary joints to drive the articulated robot based on the command; and a joint angle command calculated by the control unit in the joint angle calculation step A joint control step of rotating each of the plurality of rotary joints to operate the articulated robot, and the control unit includes a singular point angle or a singular point of the rotary joint with respect to a singular point of the joint robot. A singular point calculation step of calculating a distance, and the control unit, when the singular point angle or singular point distance calculated by the singular point calculation step is smaller than a predetermined value, Characterized by comprising a joint velocity limiting step of limiting the rotational speed of the pre-specified rotation joints, based on the type.

  ADVANTAGE OF THE INVENTION According to this invention, the robot apparatus and robot control method which can process the singular point appropriately and can improve the operation speed of an articulated robot can be provided.

1 is a perspective view schematically showing an overall structure of a robot apparatus according to a first embodiment of the present invention. It is explanatory drawing explaining the 1st specific point of the robot arm which concerns on 1st Embodiment. It is explanatory drawing explaining the 2nd specific point of the robot arm which concerns on 1st Embodiment. It is explanatory drawing explaining the 3rd singular point of the robot arm which concerns on 1st Embodiment. It is a block diagram which shows the structure of the joint control apparatus which concerns on 1st Embodiment. It is explanatory drawing explaining the function which calculates the largest joint angle deviation. It is explanatory drawing explaining the joint axis which carries out speed restriction | limiting according to the kind of singular point. It is a block diagram which shows the structure of the joint control apparatus which concerns on 2nd Embodiment. It is a block diagram which shows the structure of the joint control apparatus which concerns on 3rd Embodiment. It is a block diagram which shows the structure of the joint control apparatus which concerns on 4th Embodiment. It is a block diagram which shows the structure of the joint control apparatus which concerns on 5th Embodiment. It is a block diagram which shows the structure of the joint control apparatus which concerns on 6th Embodiment. It is a block diagram of the control apparatus which controls the articulated robot which concerns on a prior art.

<First Embodiment>
Hereinafter, a robot apparatus according to a first embodiment of the present invention will be described with reference to FIGS. First, a schematic configuration of the entire robot apparatus 1 according to the first embodiment will be described with reference to FIG. FIG. 1 is a diagram schematically showing the overall structure of the robot apparatus 1 according to the first embodiment of the present invention.

  As shown in FIG. 1, a robot apparatus 1 includes a 6-axis vertical articulated robot (hereinafter referred to as “articulated robot”) 2 for assembling workpieces, a controller 3 for controlling the articulated robot 2, A teaching pendant 4 capable of operating the joint robot 2.

  The multi-joint robot (multi-joint manipulator) 2 includes a six-axis multi-joint robot arm 20 and an end effector 21 connected to the tip of the robot arm 20. The robot arm 20 includes six rotary joints (J1 to J6) and six actuators (servo in the present embodiment) that rotate around the joint axes (J1 axis to J6 axis). The robot arm 20 moves the end effector 21 to an arbitrary three-dimensional position by selectively driving the servo of each joint. The end effector 21 is detachably attached to the tip of the robot arm 20 and can be replaced based on the work content. In the present embodiment, an end effector called a hand that can hold a workpiece with three fingers is attached.

  The controller (control device) 3 includes a servo control device (see FIG. 5 to be described later) for driving and controlling the servos of each joint of the robot arm 20 and a joint control device (see FIG. 5 to be described later) for controlling the joints of the robot arm 20. 31). Further, the controller 3 includes an end effector control device that controls the end effector 21, a storage unit, a recording medium reading device, and a communication device.

  The joint control device 31 controls each rotary joint so that when the robot arm 20 approaches a singular point, the robot arm 20 moves while avoiding a sudden movement of a specific rotary joint. The joint control device 31 will be described in detail later. The servo control device (joint drive control unit) 30 operates the robot arm 20 by rotating the servo of each rotary joint based on a joint angle command (described later) calculated by the joint control device 31.

  The storage unit stores data such as a program (robot control program) for executing a robot control method, which will be described later, a singular point of the robot arm 20, and an initial teaching point previously taught by the user. The recording medium reading device is used for reading a computer-readable recording medium in which various programs such as a robot control program are recorded, and storing the program, data, and the like recorded on the recording medium in a storage device. The communication device is used when, for example, an update program distributed from the Internet or the like is downloaded via the communication device without using the recording medium as described above.

  The teaching pendant (operation means) 4 is a man-machine interface with the user, and is used by the user to operate the articulated robot 2. The teaching pendant 4 has a function called jog operation. In the jog operation, for example, the hand position of the articulated robot 2 is moved on a straight line in three-dimensional coordinates only while the user presses the button. Can be done. For this reason, the multi-joint robot 2 may approach a singular point during jog operation, and a specific joint may move rapidly when approaching the singular point. At this time, in the present embodiment, the robot control unit 31 drives the robot arm 20 while avoiding an abrupt operation of a specific joint even when approaching a singular point.

  Next, the joint control device 31 described above will be specifically described with reference to FIGS. First, singular points and singular point distances of the robot arm 20 will be described with reference to FIGS. FIG. 2 is an explanatory diagram illustrating a first singular point of the robot arm 20 according to the first embodiment. FIG. 3 is an explanatory diagram illustrating a second singular point of the robot arm 20 according to the first embodiment. FIG. 4 is an explanatory diagram illustrating a third singular point of the robot arm 20 according to the first embodiment.

  Generally, in an articulated robot (articulated manipulator), there are many singular points where the joint angle cannot be calculated for a specific hand position. For example, in the 6-axis articulated robot 2, it is known that there are three singular points (first singular point to third singular point). There is also a case of approaching. Hereinafter, the three singular points of the articulated robot 2 will be described.

  In the following, for the sake of explanation, the direction of the J1 axis is referred to as the vertical direction, and the direction orthogonal to the J1 axis is referred to as the horizontal direction. Further, in general industrial robots, the direction of the J1 axis and the direction of the J2 axis are orthogonal to each other, and the direction of the J2 axis and the direction of the J3 axis are parallel to each other. I will explain. A configuration in which the J2 axis is offset in the horizontal direction with respect to the J1 axis may be used. However, since the discussion is possible in the same manner, the description thereof is omitted here. Further, in general, the J4 axis, the J5 axis, and the J6 axis are often designed to intersect at one point (point P shown in FIGS. 2 to 4) so that the reverse mechanism calculation can be easily calculated. Therefore, it demonstrates based on this condition.

  As shown in FIG. 2, the first singular point is a point where the J4 axis, the J5 axis, and the J6 axis intersect, that is, the point P is on the J1 axis. On the first singular point, the J1 axis, the J4 axis, the J5 axis, and the J6 axis intersect at one point at the P point, so the rotation angles of the four rotary joints J1, J4 to J6 are not determined. This is because in the three-dimensional space, the rotation angle is determined by three types of directions (variables), whereas the first singular point has four variables, which is one more variable. In the vicinity of the first singular point, a slight change in the position and orientation of the hand causes a large angle change of the rotary joints J1, J4 to J6.

  Further, assuming that the rotation angle of the rotary joint J2 necessary for positioning the point P on the J1 axis is δ1, the rotation angle δ1 is a representative index indicating the distance to the first singular point. Therefore, in this embodiment, the rotation angle of a specific rotary joint (hereinafter referred to as “singular point angle”) is used as the singular point distance. Empirically, if the rotation angle (singular point angle) δ1 of the rotary joint J2 is less than 15 degrees, the rotational angles of the rotary joints J1, J4 to J6 change greatly due to slight changes in the position and orientation of the hand. It becomes.

  As shown in FIG. 3, the second singular point is when the J4 axis and the J6 axis are on the same axis. At the second singular point, since the J4 axis and the J6 axis coincide with each other, the rotation angles of the rotary joints J4 and J6 are not fixed to one. In the vicinity of the second singular point, a slight change in the position and orientation of the hand causes a large angle change of the rotary joints J4 and J6. Further, assuming that the rotation angle of the rotary joint J5 required to position the J4 axis and the J6 axis on the same axis is δ2, if the rotation angle (singular point angle) δ2 is less than 15 degrees empirically, Since the rotational angle of the rotary joints J4 and J6 changes greatly due to a change in position and orientation, attention is required.

  As shown in FIG. 4, the third singular point is when the J2 axis and the J3 axis are on the J4 axis. At the third singular point, since the J2 axis and the J3 axis are located on the J4 axis, the rotation angles of the rotary joints J2 and J3 are not determined. In the vicinity of the third singular point, a slight change in the position and orientation of the hand causes a large angle change of the rotary joints J2 and J3. Further, assuming that the rotation angle of the rotary joint J3 required to position the J2 axis and the J3 axis on the J4 axis is δ3, if the rotation angle (singular point angle) δ3 is less than 15 degrees empirically, Since the rotational angle of the rotary joints J2 and J3 changes greatly due to a change in position and orientation, attention is required.

  Next, a joint control device 31 that can be driven and controlled while suppressing the rapid movement of the robot arm 20 when approaching the three singular points described above will be described with reference to FIG. FIG. 5 is a block diagram illustrating a configuration of the joint control device 31 according to the first embodiment.

  First, the configuration of the joint control device 31 will be described. As illustrated in FIG. 5, the joint control device 31 includes a joint angle calculation unit 32 that calculates a joint angle command of each rotary joint for driving the robot arm 20 based on the operation command, and a singular point of the rotary joint. And a singular point calculation unit 51 for calculating a point angle. In addition, the joint control device 31 includes a maximum joint angle deviation adjustment unit 52 that performs a process of reducing the limit value of the maximum joint angle deviation (maximum rotation speed) given in advance for each rotary joint according to the singular point angle. I have.

  The joint angle calculation unit 32 includes a position command calculation unit 50, a correction strength adjustment unit 53, a position command correction unit 54, a reverse mechanism calculation unit 55, a joint angle correction unit 56, a forward mechanism calculation unit 57, and a correction. A quantity calculation unit 58.

  The position command calculation unit 50 calculates a position command by adding a deviation command of the hand to the current position of the hand of the robot arm 20 expressed in three-dimensional coordinates. The correction intensity adjustment unit 53 performs a process of calculating a correction intensity (a correction ratio, hereinafter referred to as “correction ratio”) according to the magnitude of the singular point angle. Specifically, a process of reducing the correction ratio as the singular point angle is smaller is performed. The position command correction unit 54 calculates the corrected position command by adding the position command calculated by the position command calculation unit 50 to the previous correction amount multiplied by the correction ratio. The reverse mechanism calculation unit 55 converts the position command corrected by the position command correction unit 54 into a temporary joint angle command. The joint angle correction unit 56 calculates a joint angle deviation from the temporary joint angle command calculated by the position command correction unit 54 and the previous joint angle command, and the maximum joint angle deviation calculated by the maximum joint angle deviation adjustment unit 52. The next joint angle command is calculated by reducing the temporary joint angle so as not to exceed. The forward mechanism calculation unit 57 calculates the next joint angle command and the next position from the corrected joint angle command. The correction amount calculation unit 58 calculates the next correction amount by adding the correction amount to the difference between the next position calculated by the forward mechanism calculation unit 57 and the position command.

  The maximum joint angle deviation adjustment unit (joint speed limiting unit) 52 determines the maximum joint angle deviation (maximum rotation speed) when the singular point angle of the rotary joint specified in advance based on the type of singular point becomes smaller than a predetermined value. The limit value is made smaller than the preset limit value of the maximum joint angle deviation.

  Next, drive control (robot control method) of the robot arm 20 by the joint control device 31 configured as described above will be described with reference to FIGS. 6 and 7 in addition to FIG. The subscript i of the symbol shown below represents the number of calculations, and is incremented by one every time a series of calculations are completed. Moreover, this calculation is implemented for every control period of 2 ms, for example. FIG. 6 is an explanatory diagram for explaining a function for calculating the maximum joint angle deviation. FIG. 7 is an explanatory diagram for explaining a joint axis whose speed is limited in accordance with the type of singular point.

When an operation command is input to the teaching pendant 4, the deviation command V _i , the current position C _i , the previous correction amount E _i, and the previous joint angle command q _i are obtained from the teaching pendant 4 based on the input operation command. Input to the joint control device 31. Then, the joint control device 31 calculates the next joint angle commands q _{i + 1} , C _{i + 1,} and E _{i + 1} based on the input data and outputs them to the servo control device 30. The servo control device 30 operates the robot arm 20 by rotating the rotary joint based on the input next joint angle command. Hereinafter, a method for calculating the next joint angle commands q _{i + 1} , C _{i + 1,} and E _{i + 1 by} the joint control device 31 will be specifically described.

When the deviation command V _i , the current position C _i , the previous correction amount E _i and the previous joint angle command q _i are input from the teaching pendant 4, first, the hand deviation command V _i and the current position C _{i of the} hand are obtained. In addition, a position command D _i is calculated (position command calculation step). Here, the current position data and the deviation command data are vectors having six components for movement and rotation in the XYZ directions, and represent the position and orientation. As another method for representing such a position and orientation, there is a method using a 4 × 4 homogeneous coordinate transformation matrix. In this case, the sum of the vectors is expressed by matrix multiplication. However, since this is merely a difference in expression and is not related to the gist of the present invention, in the present embodiment, description will be made using a vector expression that is intuitively easy to understand as a position and orientation expression.

  Next, the singular point angle is calculated to determine whether or not the singular point is in the vicinity (singular point calculation step). In this embodiment, the singular point angle of the corresponding rotary joint is calculated according to the three types of singular points. In addition, the present joint angle can be substituted for the singular point angle of the corresponding rotary joint. This is because the control cycle of the multi-joint robot 2 according to the present embodiment is a sufficiently short time such as 2 ms, so that the change in the joint angle is small.

  Once the singular point angle has been calculated, the process of reducing the limit value of the maximum joint angle deviation (maximum rotational speed) given in advance for each rotary joint is then performed according to the singular point angle (joint speed adjustment) Process). Here, due to constraints such as the torque generated by the motor and the maximum rotation speed that the encoder can handle, the maximum rotation speed, that is, the limit value of the maximum joint angle deviation is determined for each rotation joint. Here, the limit value is further reduced according to the singular point angle.

For example, as shown in FIG. 6, the maximum rotation angle deviation (maximum rotation speed) given for each rotary joint is V _max , the rotation angle deviation when the singular point angle is zero is V _min, and the vicinity of the singular point is The angle to be determined is θs. The decelerated maximum rotation angle deviation V _lim with respect to the singular point angle θ is calculated by the following equation.

  When θs for determining the vicinity of the singular point is set to 15 degrees, for example, when the singular point angle is less than 15 degrees, the rotation speed of the specific rotary joint is limited. Further, the process of reducing the limit value is performed for each rotary joint specified in advance based on the type of singularity. For example, as shown in FIG. 7, in the case of the first singular point, the limit value of the maximum joint angle deviation (maximum rotation speed) of the rotary joints J1, J4, J5, and J6 is reduced. The minimum limit value at this time is zero. In the case of the second singular point, the limit value of the maximum joint angle deviation (maximum rotation speed) of the rotary joints J4 and J6 is reduced. The minimum limit value at this time is zero. Further, in the case of the third singular point, the limit value of the maximum joint angle deviation (maximum rotation speed) of the rotary joints J2 and J3 is reduced. Note that the minimum value of the limit value at this time is not zero but is set to a small speed such as 1 degree per second. This is because if this speed is set to zero, it is not possible to escape from this singularity.

  Further, the robot arm 20 may simultaneously approach the third singular point from the first singular point. For example, when the first singular point and the second singular point are approached at the same time, the maximum joint angle deviation (maximum rotational speed) of each rotary joint is calculated, and the smallest one is set as the limit value.

  Next, a correction ratio corresponding to the singular point angle is calculated (correction intensity adjustment step). As described above, there are three singular points, and there are singular point angles corresponding to each. Here, the minimum value is considered as the singular point angle. Further, the correction ratio is a number from 0 to 1, and is zero near the singular point and becomes 1 when moving away from the singular point. For example, if the singular point angle for determining the vicinity of the singular point is θs, and the smallest one of the three singular point angles up to θ is θ, the correction ratio r is

It can be expressed as Note that the form of this function is the same as that in FIG. 6 where V _max = 1 and V _min = 0, so that the description thereof is omitted. Further, θs is set to 15 degrees, for example.

Next, the corrected position command F _i is calculated by adding the previous correction amount E _i and the correction ratio r to the position command D _i calculated in the position command calculation step (position command correction step). .

  Since the correction ratio is small near the singular point in the correction intensity adjustment step, the correction amount is small near the singular point by multiplying the previous correction amount by the correction ratio. Therefore, in the conventional technique, there is a risk that the movement of the joint becomes unstable due to an increase in the correction amount in the vicinity of the singular point. It can be stabilized.

Next, the position command F _i corrected in the position command correction step is used as a temporary joint angle command.

  (Inverse mechanism calculation process). Next, temporary joint angle command

The joint angle deviation is calculated from the previous joint angle command q _i, and the temporary joint angle deviation is subtracted so that this does not exceed the maximum rotation angle deviation V _lim calculated in the maximum joint angle deviation adjustment step. Command q _{i + 1} is calculated (joint angle correction step). Here, the following equation must be established from the condition that the joint angle deviation does not exceed the upper limit.

In order to satisfy this, the next joint angle command q _{i + 1} is calculated as follows.

Once the next joint angle command q _{i + 1} is calculated, the next position C _{i + 1} is calculated from the next joint angle command q _{i + 1} (forward mechanism calculation step). Then, following the position C _{i + 1} calculated by the forward mechanism calculation step, the difference between the position command D _{i + 1,} by adding the correction amount E _i, to calculate the next correction amount E _{i + 1} (correction amount calculating step).

  Here, if it is not near a singular point, the correction ratio is 1, so the corrected position command is

It becomes. Further, since the joint angle correction process does not decelerate, the process returns to the original by the reverse mechanism calculation process and the forward mechanism calculation process, so that C _{i + 1} = D _i + E _i . If this C _{i + 1} is substituted into the above equation, E _{i + 1} becomes zero. In other words, the correction amount becomes zero as the distance from the singular point increases.

  As described above, the robot apparatus 1 according to the first embodiment calculates the singular point angle (distance), adjusts the maximum joint angle deviation according to the singular point angle, and makes the joint so as to be within the maximum value. Correct the angular deviation. Therefore, for example, if the maximum joint angle deviation is set small in the vicinity of the singular point and is set large in other cases, the problem that the operation is slowed at other than the singular point can be solved. Thereby, the operation speed of the articulated robot can be improved. Further, in the vicinity of the singular point, the maximum joint angle deviation, that is, the joint angular velocity is limited for each rotary joint specified based on the type of singular point, so that the robot arm can be prevented from moving suddenly.

  Further, by correcting the joint angle deviation, the hand position is deviated from the initial target, and it is necessary to correct the hand position. In the prior art, the hand position is corrected even in the vicinity of a singular point, which causes a large change in joint speed. On the other hand, in this embodiment, the singular point angle is calculated, the correction ratio is adjusted accordingly, and the position command is corrected in consideration of the correction ratio. For example, the correction ratio is set small in the vicinity of the singular point, and large in other cases. Therefore, since the correction amount is small near the singular point, it is possible to prevent the joint speed from increasing. Moreover, since it can prevent stopping in order to avoid that joint speed becomes large, productivity can be improved. Further, since the correction strength increases as the distance from the singular point increases, the position of the hand can be made to follow the initial target position.

  Moreover, since the robot apparatus 1 according to the first embodiment calculates the singular point angle for each type of singular point, it limits different joint angle deviations, that is, the rotational speed of the rotary joint for each type of singular point. Can do. Therefore, it is possible to quickly escape from the singular point using a rotating joint that is not decelerated even in the vicinity of the singular point.

  Further, since the robot apparatus 1 according to the first embodiment can reduce the correction amount corresponding to the case where a plurality of singular point angles are minimum, the calculation is stable near the singular point and there is no need to reduce the hand speed. Therefore, the operation speed can be improved.

Second Embodiment
Next, a robot apparatus according to a second embodiment will be described with reference to FIG. The second embodiment is different from the first embodiment in that the forward mechanism calculation unit 57 of the joint control device 31 is replaced with a Jacobian matrix calculation unit 59 and a Jacobian matrix multiplication unit 60. Therefore, in the second embodiment, the Jacobian matrix calculation unit 59 and the Jacobian matrix multiplication unit 60 will be described. FIG. 8 is a block diagram illustrating a configuration of a joint control device 31A according to the second embodiment.

  The forward mechanism calculation calculates the position and orientation of the hand by sequentially calculating the position and orientation from the base joint, and it is known that this can be approximated by multiplication with the Jacobian matrix. The Jacobian matrix calculation unit 59 calculates a Jacobian matrix. In the case of the 6-axis multi-joint robot arm 20, the Jacobian matrix has 6 rows and 6 columns, and when the joint angle deviation vector (joint speed) is applied, the hand tip position deviation vector (hand speed) is obtained.

  The Jacobian matrix multiplication unit 60 multiplies the Jacobian matrix by the difference between the joint angle command calculated in the joint angle correction process and the previous joint angle command, that is, the joint angle deviation vector and the Jacobian matrix, and adds the current position to this. To calculate the next position. For example,

  Can be used to calculate. According to the second embodiment, since the forward mechanism calculation is a complex coordinate transformation including a trigonometric function, the amount of calculation can be reduced by substituting the forward mechanism calculation unit 57 for the Jacobian matrix calculation unit 59 and the Jacobian matrix multiplication unit 60. Can do.

<Third Embodiment>
Next, a robot apparatus according to a third embodiment will be described with reference to FIG. The third embodiment is different from the first embodiment in that the correction amount correction unit 61 is added to reduce the correction amount. Therefore, in the third embodiment, the correction amount correction unit 61 will be described. FIG. 9 is a block diagram illustrating a configuration of a joint control device 31B according to the third embodiment.

  Here, it is assumed that the operator moves the hand position (end effector 21) of the articulated robot 2 by pressing the button of the teaching pendant 4. The deviation command always outputs a non-zero number while the button of the teaching pendant 4 is being pressed. If the robot arm 20 continues to move and approaches a singular point, the speed of the hand is reduced due to the speed limitation of the rotating joint, but the amount that could not be moved at this time is accumulated in the correction amount. And when it leaves | separates from a singular point, since a correction amount will be canceled now, a hand speed will increase. For example, when the correction amount returns to zero, the hand speed also returns to the original speed.

Considering this series of movements, since the robot arm 20 unexpectedly increases in the moment of leaving from the vicinity of the singular point, it is preferable not to accumulate the correction amount in the direction of the movement intended by the operator. In the third embodiment, a correction amount correction unit 61 that removes a direction component from the deviation command is provided, and the correction amount correction unit 61 performs the following vector calculation. First, calculate the direction vector _{v i} of difference command _{V i.}

  Next, the correction amount before correction

Then, the corrected amount of correction E _{i + 1} is calculated from the direction vector v _i .

  The meaning of this equation is to remove the component in the direction of the deviation command from the correction amount. According to the third embodiment, since the correction amount can be reduced, an unnecessary speed increase at the moment of leaving from the vicinity of the singular point can be suppressed. In particular, from the viewpoint of the operator of the teaching pendant, it is preferable because the speed increase in the direction intended by the operator, that is, the direction of the deviation command can be suppressed. Further, since the correction amount is small, the correction calculation near the singular point becomes more stable. Furthermore, since the correction amount is small, the time for restoring the original position can be shortened.

<Fourth embodiment>
Next, a robot apparatus according to a fourth embodiment will be described with reference to FIG. The fourth embodiment is different from the first embodiment in that a maximum joint angle deviation change adjusting unit 62 that restricts a deviation change in addition to the maximum joint angle deviation is provided. Therefore, in the fourth embodiment, the maximum joint angle deviation change adjustment unit 62 will be described. FIG. 10 is a block diagram illustrating a configuration of a joint control device 31C according to the fourth embodiment.

  The maximum joint angle deviation change adjustment unit 62 reduces the maximum joint angle deviation change (maximum rotation speed change) given in advance for each rotary joint according to the singular point angle. Since the control cycle is constant, the change in the joint angle deviation is acceleration, and the maximum value is determined for each joint based on conditions such as torque that can be generated by the motor of the rotary joint. Here, the limit value is further reduced according to the singular point angle, and the vehicle is decelerated. The method of specifically decelerating is the same as the method described with reference to FIG. 6 in the case of deviation (rotational speed).

The maximum rotation angle deviation change given for each rotary joint is A _max , the joint angle deviation change when the singular point angle θ on the horizontal axis is zero is A _min, and the singular point angle for determining the vicinity of the singular point is θs. To do. The decelerated maximum rotation angle deviation change A _lim with respect to the singular point angle θ is

Calculated by For example, if θs is set to 15 degrees, the rotational speed of the rotary joint becomes abrupt when the singular point angle is less than 15 degrees. In the fourth embodiment, in addition to limiting the maximum joint angle deviation, the maximum joint angle deviation change is also limited. The deviation change is calculated by the next difference from the next joint angle command q _{i + 1} , the previous joint angle command q and the previous joint angle command q _i−1 .

  From the condition that this does not exceed the limit value calculated by the maximum joint angle deviation change adjustment unit 62,

Q _{i + 1} is calculated as the following equation so as to satisfy this equation.

  Furthermore, the maximum joint angle deviation is limited. Since this is the same description as the joint angle correction process of the first embodiment, it is omitted. In the fourth embodiment, the restriction of the maximum joint angle deviation is calculated after the restriction of the deviation change, so that the restriction of the maximum joint angle deviation has priority. . According to the fourth embodiment, since the maximum joint angle deviation change is limited in addition to the maximum joint angle deviation limit, smoother speed control is possible.

<Fifth Embodiment>
Next, a robot apparatus according to a fifth embodiment will be described with reference to FIG. The fifth embodiment is different from the first embodiment in that the position command correction step and the reverse mechanism calculation step are performed before the singular point calculation step. Therefore, in the fifth embodiment, a case where the position command correction step and the reverse mechanism calculation step are performed before the singular point calculation step will be described. FIG. 11 is a block diagram illustrating a configuration of a joint control device 31D according to the fifth embodiment.

Since the singular point angle can also be obtained by inverse mechanism calculation, if the singular point angle is calculated after this calculation, the singular point angle can be calculated with the latest joint angle. In the fifth embodiment, the position command correction step calculates the position command F _i corrected using the previous correction ratio r _i .

  Even when the previous correction ratio is used, there is no problem because the control cycle is sufficiently short, such as 2 ms. This is because the change in the joint angle between 2 ms is small, the change in the singular point angle is small, and the change in the correction ratio is also small. According to the fifth embodiment, the same effect as that of the first embodiment can be obtained.

<Sixth Embodiment>
Next, a robot apparatus according to a sixth embodiment will be described with reference to FIG. The sixth embodiment is different from the first embodiment in that an attenuation step is provided after the correction amount calculation step. Therefore, in the sixth embodiment, an attenuation calculation unit 63 that performs an attenuation calculation step will be described. FIG. 12 is a block diagram illustrating a configuration of a joint control device 31E according to the sixth embodiment.

  In the first embodiment, the correction ratio is small in the vicinity of the singular point and increases as the distance from the singular point increases. Here, consider a case where the singular point is stopped for a long time and then left. In this case, the correction works for the previous correction amount for a long time, and the robot arm 20 moves. For example, when the operator of the articulated robot 2 instructs a deviation command, it is not preferable for the operator if the robot arm 20 moves after a long time has passed. In this case, it is natural to attenuate the correction amount according to time.

Therefore, in the sixth embodiment, the attenuation calculation unit 63 is caused to calculate the attenuation of the correction amount at a predetermined magnification. Specifically, nE _{i + 1} is substituted for the corrected correction amount E _{i + 1} . The magnification is n = 0.5 ^ (1 / n) when it is attenuated in half by n times. For example, when the control period is 2 ms and attenuated in half in 4 seconds, the number of times n is 4 / 0.002 = 2000 times. The magnification is n = 0.999653.

  As mentioned above, although embodiment of this invention was described, this invention is not limited to embodiment mentioned above. In addition, the effects described in the embodiments of the present invention only list the most preferable effects resulting from the present invention, and the effects of the present invention are not limited to those described in the embodiments of the present invention.

For example, in the first embodiment has been described with reference to the current position C _i calculated in the previous control cycle, it calculates the actual rotation angle servo controller may determine the current position in the order mechanism calculation. Even in this case, the gist of the present invention is not changed and the effect is the same.

1 Robot device 2 Articulated robot 3 Controller (control device)
4 Teaching pendant (operating means)
30 Servo control device (joint drive controller)
32 Joint angle calculation unit 51 Singular point calculation unit 52 Maximum joint angle deviation adjustment unit (joint speed limiting unit)

The present invention includes an articulated robot having a plurality of rotary joints, and a control device that drives and controls the articulated robot based on an operation command input from an operation unit capable of operating the articulated robot. In the robot apparatus, the control device calculates a joint angle command of each of the plurality of rotary joints for driving the articulated robot based on the operation command, and the joint angle calculation unit And a joint drive control unit that operates each of the articulated robots by rotating and driving each of the plurality of rotary joints based on the joint angle command calculated by: and a singular point angle of the rotary joint with respect to a singular point of the joint robot or a singular point calculation unit that calculates a singular point distance, when the singular point angle or specific point distance the singular point calculation unit has calculated is smaller than a predetermined value, pre Me particular Comprising a joint velocity limit unit for limiting the rotational speed of the rotary joint which the said joint angle computing unit includes a position command calculating unit for calculating a position command based on the operation command, the singular point calculation unit calculates A position command correction unit that corrects the position command according to the singular point angle or the singular point distance, a reverse mechanism calculation unit that converts the corrected position command into a temporary joint angle command, a temporary joint angle command and the previous A joint angle correction unit that calculates a joint angle deviation from a joint angle command, calculates a next joint angle command by subtracting a temporary joint angle command so that the joint angle deviation does not exceed the rotation speed, and a corrected joint A forward mechanism calculation unit that calculates the next position from the angle command, and a correction amount calculation unit that calculates the next correction amount by adding the previous correction amount to the difference between the next position and the position command. It is characterized by.

Further, the present invention is based on the operation command input articulated robots operable operating means having a plurality of rotary joints, a robot control method in which the control unit controls driving the articulated robot, the control unit A joint angle calculation step of calculating each joint angle command of each of the plurality of rotary joints for driving the articulated robot based on the operation command, and the control unit calculates the joint angle calculation step. rotationally drives each of the plurality of rotational joints based on the joint angle command, and articulation control step of operating the articulated robot, the control section singularity angle before Symbol rotary joint of the joint robot or a singular point calculation step of calculating a singular point distance, the control section, when the singular point angle or specific point distance the singular point calculation process is calculated is smaller than a predetermined value, Comprising a joint velocity limiting step of limiting the rotational speed of the rotary joint is because specific, and the joint angle computing process, a position command calculating step of calculating a position command based on the operation command, the singular point calculating step A correction amount is obtained according to the singular point angle or the singular point distance calculated in step S1, a position command correction step for correcting a position command based on the correction amount, and the corrected position command as a temporary joint angle command. Calculate the joint angle deviation from the reverse mechanism calculation step for conversion, the temporary joint angle command and the previous joint angle command, and subtract the temporary joint angle command so that the joint angle deviation does not exceed the rotation speed. The joint angle correction process to calculate the joint angle command of the next, the forward mechanism calculation process to calculate the next position from the calculated next joint angle command, the previous correction amount is added to the difference between the next position and the position command , Correction to calculate the next correction amount It has a calculation step, a
It is characterized by that.

Claims (1)

In a robot apparatus comprising: an articulated robot having a plurality of rotary joints; and a control device that drives and controls the articulated robot based on an operation command input from an operation unit capable of operating the articulated robot.
The controller is
A joint angle calculation unit for calculating a joint angle command of each of the plurality of rotary joints for driving the articulated robot based on the operation command;
A joint drive control unit for operating the multi-joint robot by rotating each of the plurality of rotary joints based on a joint angle command calculated by the joint angle calculation unit;
A singular point calculation unit for calculating a singular point angle or a singular point distance of the rotary joint with respect to the singular point of the joint robot;
When the singular point angle or singular point distance calculated by the singular point calculation unit is smaller than a predetermined value, a joint speed limiting unit that limits the rotational speed of the rotary joint specified in advance based on the type of singular point, Prepared,
A robot apparatus characterized by that.

Images