JP2003311664A - Robot controller - Google Patents

Abstract

<P>PROBLEM TO BE SOLVED: To upgrade processing speed by judging whether or not to pass through the vicinity of a singular point before executing an interpolating operation. <P>SOLUTION: The controller is provided with a position instructing part 10 for storing interpolation sorts, a start position of a control point Cp and a target position by instructing the control point Cp of a horizontal multi-joint type robot, a computing means for deriving a straight line for connecting the start position and the target position by reading out the start position and the target position from the position instructing part 10 before the robot runs, a speed adjustment judging part 14 for judging whether or not the straight line passes through a speed adjustment area Ac set around the singular point of the robot, and a speed adjustment instructing part 18 for generating an adjustment speed instruction Vrc based on the distance from the control point Cp and the singular point. <P>COPYRIGHT: (C)2004,JPO

Description

Detailed Description of the Invention

[0001]

BACKGROUND OF THE INVENTION 1. Field of the Invention The present invention relates to an improvement of a robot control system in which an articulated robot passes near a singular point.

[0002]

2. Description of the Related Art In a linear interpolation operation in which a locus of a control point of a robot draws a straight line, information on the position and orientation of an operation start position and a target position (hereinafter referred to as orthogonal data) is previously stored in a robot controller. The orthogonal data is calculated for each control cycle of the robot so that the control point draws a straight line between the two positions, and the angle of each joint axis (hereinafter referred to as joint data) is converted from the orthogonal data by coordinate conversion. ) Is obtained, and the motor of each axis is controlled so as to match the joint data.

In a multi-joint type robot, there is a position called a singular point where the joint data cannot be uniquely obtained when the coordinates are converted from the orthogonal data to the joint data, and the robot passes through the singular point or the vicinity of the singular point. In some cases, it is known that a particular joint axis must rotate at a very high speed. Therefore, during interpolation operation, the speed command of the joint axis is always calculated, and it is judged from the rate of increase of the speed command whether or not the robot control point approaches the singular point. There has been proposed a means for switching the processing to such an interpolating means, or correcting the speed command according to the distance from the singular point and passing it near the singular point (Japanese Patent Laid-Open No. 5-324044).
Issue).

[0004]

However, in the above-mentioned technical means, the determination processing for the vicinity of the singular point and the speed correction processing are always executed during the interpolation operation. Therefore, since an unnecessary process is originally performed for the interpolation operation that does not pass through the vicinity of the singular point, the calculation time increases because the processing time of the interpolation operation is occupied. Therefore, there is a problem that quick control is restricted.

The present invention has been made in order to solve the above-mentioned problems, and it is judged whether or not the vicinity of a singular point is passed before the execution of the interpolation operation, and only when it is passed, the vicinity of the singular point is passed in the interpolation operation. It is an object of the present invention to provide a control device for a robot that executes processing for.

[0006]

A robot controller according to a first aspect of the present invention teaches a control point of a horizontal articulated robot to store the type of interpolation, the start position and target position of the control point. Storage means, a computing means that reads the start position and the target position from the storage means before the robot operates, and obtains a straight line connecting the start position and the target position, and a singular point of the robot. First determining means for determining whether or not the straight line passes through a speed adjustment area set around, and, when the straight line passes through the speed adjustment area, from the control point to the singular point And a first speed command generating means for generating a first speed command based on the distance.
According to such a robot control device, before the linear interpolation operation is performed, the first determination means determines whether or not the control point of the robot passes through the speed adjustment area. The control point of the robot interpolates according to the first speed command based on the distance to. Therefore, when the control point originally does not pass through the speed adjustment area near the singular point, useless processing for passing through the vicinity of the singular point is not executed during the operation of the robot, which has the effect of smoothing the calculation processing. .

A robot controller according to the second invention is
A second speed command generation unit that generates a second speed command that interpolates the control point at a predetermined speed based on the type of interpolation, the start position, and the target position read from the storage unit, and a straight line by the first determination unit. By selecting the lower speed command by comparing the first speed command and the second speed command while executing the interpolation operation of the control points. To do. According to such a robot controller, the selecting means compares the first speed command and the second speed command and selects the lower speed command to interpolate the control points. Therefore, there is an effect that the control point of the robot can smoothly pass through the speed adjustment region.

The second velocity command generating means in the robot controller according to the third aspect of the invention is such that the distance from the singular point to the control point is R and the maximum rotation velocity of the joint axis is Δθ _1max.
And the angle between the operation direction of the linear interpolation and the horizontal direction is φ, the second speed command Vrc is characterized by the following formula: Vrc = R · Δθ _1max / cosφ. According to such a robot control device, the amount of movement in the vertical direction as well as in the horizontal direction is taken into consideration. Therefore, when the control point moves largely in the vertical direction with respect to the horizontal direction in the speed adjustment region, the adjustment speed command only in the horizontal direction On the other hand, there is an effect that the speed of the control point can be appropriately reduced and the speed adjustment region can be operated smoothly.

A robot controller according to a fourth invention is
Second judging means which is set around the singular point and has a singular point area narrower than the speed adjustment area and judges whether or not the singular point area is passed, and whether or not the singular point is passed It is characterized in that it is provided with an alarm means for judging and determining whether to pass and issuing an alarm. When it is determined that the control point of the robot passes through the singularity region, normal linear interpolation cannot be performed, so a warning is issued before the operation starts. Based on such a warning, for example, by changing the start position and target position of the control point or changing the linear interpolation to the joint interpolation, it is possible to prevent the control point from passing through the singular point. is there.

[0010]

BEST MODE FOR CARRYING OUT THE INVENTION Embodiment 1. An embodiment of the present invention will be described with reference to FIGS. In FIG.
The multi-joint type robot 1 has a vertically coaxial movable coaxial J0 provided on a base 3 and a rotatable first joint J1.
And a rotatable second joint J2, a first joint J1 and a second joint
It is composed of an arm 5 for connecting the joint J2 of No. 2 and an arm 7 having a control point Cp at the tip and having the same length as the arm 5 connected to the second joint J2.

In a three-degree-of-freedom horizontal articulated robot having arms 5 and 7 having the same length as shown in FIG. 1, the position where arms 5 and 7 are folded as shown in FIG. It is known that, as shown in FIG. 3, the operation speed at the control point Cp in the states 1 to 7 is kept constant, and the linear interpolation operation is executed so as to pass the singular point in FIG. Then, when moving from the state 4 to the state 5, the first joint J1 must instantaneously rotate 180 degrees. A problem occurs when the control point Cp tries to pass in the vicinity of the singular point in this way. Therefore, the means for executing the interpolation process for eliminating such a problem will be described below.

In FIG. 4, the interpolation processing means includes a command unit 10 that stores an interpolation type and the like by teaching, and a singular point region As near the singular point where the interpolation operation of the control point Cp is singular or wider than the singular point region As. A speed adjustment determination unit 14 that determines whether or not the point speed adjustment region Ac is passed, an interpolation processing unit 12 that performs an interpolation process, and an alarm is issued by determining that the speed adjustment determination unit 14 passes the singular point region As. Alarm unit 1
7, a coordinate conversion unit 26 for converting the calculated interpolation position into a position command for each joint J0 to J2, and a servo control for controlling the motor position of each joint J0 to J2 based on the command from the coordinate conversion unit 26. And part 28.

The command unit 10 is composed of a RAM as a storage means for storing interpolation commands, and has an interpolation type area 10a for storing the type of interpolation and a start position area for storing the start position of the control point Cp to be operated. 10b and a target position area 10c storing the target position of the control point Cp.
The interpolation processing unit 12 includes a speed command unit 13 as a second speed command generation unit that generates a desired second speed command Vc based on the interpolation command from the command unit 10, and a singularity region As.
Alternatively, the first for determining whether or not the vehicle passes through the speed adjustment area Ac
When the speed adjustment determination unit 14 as the second determination means, the speed adjustment region Ac, the singular point region As, the speed adjustment generation unit 16 storing the arithmetic expression for obtaining the adjustment speed command Vrc, and the speed adjustment region Ac are passed. If determined, the speed adjustment command unit 18 as the first speed command generation unit that generates the adjusted speed command Vrc as the first speed command that enables the speed adjustment region Ac to pass and the speed adjustment region Ac are passed. Then, in the interpolation operation of the control point Cp, the adjustment speed command Vr
c is compared with the speed command Vc, and if Vrc <Vc, the changeover switch 22 is closed to the a terminal side, and if Vrc ≧ Vc, a speed for generating a signal to close the changeover switch 22 to the b terminal side is generated. The command determination unit 19 and the interpolation position generation unit 24 that generates an interpolation position based on the speed command Vc or the adjusted speed command Vrc are provided. The speed command determination unit 19 and the changeover switch 22 form a selection unit.

The speed adjustment generation unit 16 is a circular area from the singular point, and a speed adjustment area 16a storing a speed adjustment area Ac wider than the singular point area As and a singular point which is a circular area from the singular point. Singular point region 1 that stores the region As
6b and a RAM having a speed adjustment arithmetic expression area 16c in which a predetermined arithmetic expression to be described later for obtaining the adjustment speed command Vrc when the control point Cp passes through the speed adjustment area Ac is stored. Here, in the speed adjustment area Ac, when the robot shown in FIG. 1 is horizontally operated by linear interpolation along the X axis, the control point Cp of the robot is the one of the singular points of the first joint J ₁ axis. The position closest to the position is when the control point Cp reaches the Y-axis as shown in FIG. The speed of the control point Cp at the moment when this is reached is the speed command Vc. Here, as shown in FIG. 5, a radius centered on the first joint J ₁
Considering the circle of R, the maximum velocity Vrmax (m / sec) that can be generated by rotating the _first joint J ₁ axis at a position separated from the first joint J ₁ axis by a distance R is as follows: _When the maximum rotation speed of _one axis is Δθ _1max (rad / sec), the following formula is obtained. On the Y-axis, the directions of the maximum speed Vrmax and the speed command Vc match. Vrmax = _{RΔθ 1max} (1) A circular speed adjustment area Ac for the speed command Vc is set around a singular point, and its radius Rc is obtained by modifying the expression (1) to obtain the following expression. Become. Rc = Vc / Δθ _1max (2) For simplicity, when the speed command Vc is substituted into the above formula (2) as a fixed value which is the maximum command speed Vcmax of the robot, the radius Rc is given by the following formula. Become. Rc = Vcmax / Δθ _1max (3) Further, the singular point region As set inside the speed adjustment region Ac
The radius Rs has the following relationship. 0 <Rs <Rc (4) If the radius Rs is set to a large value within the range satisfying the expression (4), the operation range of the robot is limited, so that the range is set to a predetermined range. The value of the radius Rs is, for example, 2 mm.

The speed adjustment command unit 18 generates an adjusted speed command Vrc when the locus of the start position and the target position as the locus of the control point Cp passes through the speed adjustment area Ac. When the control point Cp is diagonally linearly interpolated as shown in FIG. 6, the speed command Vc is generated along the locus of the control point Cp. Since the speed command Vc is for the operation speed Vh in the horizontal direction, when the angle formed by the operation direction of the linear interpolation and the horizontal direction is φ, the following expression is obtained. Vh = Vc · cosφ (5) From the equation (5), if the adjustment speed command Vrc is used, the adjustment speed command Vrc is given by the following formula and is proportional to the radius R from the singular point to the control point Cp. Vrc = R ・ Δθ _1max / cosφ ・ ・ ・ ・ (6)

The speed adjustment determination unit 14 determines the singular point region As,
It is determined whether or not the interpolation passes through the speed adjustment area Ac. As shown in FIG. 7, the start positions P1s to P5s and the target position P are determined.
Straight lines L1 to L5 connecting 1t to P5t are calculated (calculation means), and it is determined whether or not the straight lines L1 to L5 pass through the speed adjustment region Ac. If it is determined that the straight lines L1 to L5 do not pass through the speed adjustment region Ac, switching is performed. The switch 22 is turned on to the b terminal side.

Since the X and Y coordinates of the start position and target position of the taught control point Cp take arbitrary values, it is judged whether or not the straight line connecting the start position and the target position passes through the speed adjustment area Ac. Hard to do. Therefore, as shown in FIG. 7, the inclination α between the movement amount ΔX in the X coordinate direction and the movement amount ΔY in the Y coordinate direction is obtained, and the inclination α is used to calculate the straight lines L1, ...
X'and Y'which are rotated by-?

[0018]

[Equation 1]

By such coordinate conversion, straight lines connecting the operation start positions P1s ', P2s', P3s ', P4s', P5s 'and the target positions P1t', P2t ', P3t', P4t ', P5t', respectively. L1 ', L2', L3 ', L4' and L5 'are as shown in FIG.

The speed adjustment determination unit 14 is a straight line L1'-L5 '.
Whether or not the vehicle passes through the speed adjustment area Ac and the singularity area As is determined as follows. ． The X coordinate values X1S and X1t of the straight line L1 'are the speed adjustment area Ac.
Is larger than the radius Rc of, that is, X1S, X1t>
In the case of Rc, it is determined that the speed adjustment area Ac is not passed. ． The product of the Y coordinate values Y2s and Y2t of the straight line L2 ', that is,
If Y2s · Y2t ≦ 0, it is determined that the straight line L2 ′ crosses the X axis, and if X coordinate values X2s and X2t <Rc, it is determined that the speed adjustment area Ac is passed. Similarly, Y of straight line L3 '
If the product of coordinate values Y3s and Y3t, that is, if Y3s · Y3t ≦ 0, it is determined that the straight line L3 ′ crosses the X axis, and the X coordinate value X3
When s, X3t <Rs, it is determined that the singular point region As is passed. ． Coordinate values Xxs ', Yxs', Xxt ', Y of the straight line Lx'
When xt 'exists in the speed adjustment area Ac, that is, L
When c1≤Rc or Lc2≤Rc, it is determined that the straight line Lx 'passes through the speed adjustment area Ac. For example, straight line L4 '
Is applicable. Here, Lc1 = (Xxs ' ² + Yxs' ² ) ^1/2 Lc2 = (Xxt ' ² + Yxt' ² ) ^1/2 Also, the coordinate values Xxs ', Yxs', Xxt ', of the straight line Lx',
When Yxt 'exists in the unique region As, that is, Lc1
When ≤Rs or Lc2≤Rs, it is determined that the straight line Lx 'passes through the singular point region As. For example, the straight line L5 'is applicable.

The operation of the robot controller constructed as described above will be described with reference to FIGS. First, the operator stores the linear interpolation as the interpolation type in the interpolation type area 10a, teaches the control point Cp and stores the start positions P1s to P5s in the start position area 10b, and sets the target positions P1t to P5t in the target position area. Store in 10c. The speed adjustment area Ac is stored in the speed adjustment area 16a of the speed generation unit 16, and the singular point area 1
The singular point area As is stored in 6b, and the arithmetic expression for speed adjustment of the above equation (6) is stored in the speed adjustment arithmetic expression area 16c.

The speed adjustment determination unit 14 reads linear interpolation from the interpolation type area 10a of the command unit 10 and starts the start position P1 from the start position area 10b and the target position area 10c, respectively.
s, the target position P1t is read and the X and Y coordinate movement amount Δ
A straight line L1 obtained by rotating the straight line L1 by -α around the origin by the calculation of the equation (7) by obtaining the inclination α from X and ΔY
′ Is obtained, and X and Y coordinate values X1s ′, X1t ′, Y1s ′, Y1t ′ of the positions P1s ′, P1t ′ of the straight line L1 ′ are obtained. Since the X coordinate value X1s'> the radius Rc of the speed adjustment area Ac, the speed adjustment determination unit 14 determines that the speed adjustment area Ac is not passed and turns on the changeover switch 22 to the b terminal side. The operation of the robot is started, and the control point Cp is linearly interpolated by the speed command Vc from the speed commander 13 via the interpolation position generator 24, the coordinate converter 26, and the servo controller 28.

Further, the speed adjustment determination unit 14 reads the start position P2s and the target position P2t as described above, and then the straight line L2
′ Is obtained, and X and Y coordinate values X2s ′, X2t ′, Y2s ′, Y2t ′ of the positions P2s ′, P2t ′ of the straight line L2 ′ are obtained. The speed adjustment determination unit 14 obtains the product of the Y coordinate values Y2s 'and Y2t', and since the value of the product is negative, it is determined that the straight line L2 'crosses the X axis, and the X coordinate value X1s'<speed. It is determined that the adjustment area Ac is reached. That is, it is determined that the straight line L2 'passes through the speed adjustment area Ac. Start the robot movement,
In the interpolation operation, the speed command determination unit 19 determines whether the speed command unit 1
The speed command Vc from 3 and the adjusted speed command Vrc from the speed adjustment command unit 18 are compared. If Vrc <Vc, the changeover switch 22 is turned on to the terminal a side, and if Vrc ≧ Vc,
The control point Cp is linearly interpolated via the interpolation position generation unit 24, the coordinate conversion unit 26, and the servo control unit 28 while appropriately selecting the speed command Vc and the adjusted speed command Vrc by turning on the changeover switch 22 to the terminal b side. .

Further, the speed adjustment judging section 14 reads out the start position P3s and the target position P3t as described above to obtain the straight line L.
3'is obtained, and X and Y coordinate values X3s', Y3s', values X3t ', Y3t' of the positions P3s', P3t 'of the straight line L3' are obtained. The speed adjustment determination unit 14 obtains the product of the Y coordinate values Y3s 'and Y3t', and since the value of the product is negative, it is determined that the straight line L2 'crosses the X axis, and the X coordinate value X3s'<singular. Judge as the dot area As. That is, since the speed adjustment determination unit 14 determines that the straight line L3 'passes through the singularity region As, the alarm unit 17 issues an alarm. Based on the warning, for example, the control point Cp does not pass through the singular point by changing the start position and the target position of the control point Cp or changing the linear interpolation to the joint interpolation.

Further, the speed adjustment judging section 14 reads out the start position P4s and the target position P4t as described above to obtain the straight line L.
4'is obtained, X and Y coordinate values X4s', Y4s', values X4t ', Y4t' of positions P4s', P4t 'of the straight line L4' are obtained, and the straight line L
The coordinate values X4s 'and Y4s' of 4'are the radius R of the speed adjustment area Ac.
It is within the range of c. When the robot operation is started, the control point Cp interpolates as shown by the straight line L2 '.

Further, the speed adjustment judgment unit 14 reads the start position P5s and the target position P5t as described above, and the straight line L5
′ Is obtained, and X and Y coordinate values X5s ′, Y5s ′, X5t ′, Y5t ′ of the positions P5s ′, P5t ′ of the straight line L5 ′ are obtained. Straight line L
The coordinate values X5s 'and Y5s' of 5'are the radius Rs of the singular point region As.
Therefore, an alarm is issued from the alarm unit 17.

In the above embodiment, the speed adjustment determination unit 14
Determines that the straight lines L1 'to L5' pass through the speed adjustment region Ac, the speed command determination unit 19 compares the speed command Vc with the adjusted speed command Vrc while executing the interpolation operation of the control point Cp, Select one speed command and control point Cp
The interpolation operation of was executed. However, the speed adjustment determination unit 14 may execute the interpolation operation of the control point Cp only by the adjustment speed command Vrc by determining that the straight lines L1 'to L5' pass through the speed adjustment area Ac.

[Brief description of drawings]

FIG. 1 is an overall conceptual configuration diagram of a three-degree-of-freedom horizontal articulated robot showing an embodiment of the present invention.

FIG. 2 is a plan view showing a state where arms of the robot shown in FIG. 1 are folded.

FIG. 3 is a diagram showing a movement of a horizontal arm unit when the robot shown in FIG. 1 passes through a singular point in FIG. 2 by linear interpolation.

FIG. 4 is a block diagram for interpolating control points of the robot shown in FIG.

FIG. 5 is a diagram showing a speed command when the control point of the robot shown in FIG. 1 comes closest to a singular point, and a speed generated by rotation of the J1 axis at that position.

6 is a diagram showing velocity components in a horizontal direction and a vertical direction when the control points of the robot shown in FIG. 1 linearly interpolate diagonally.

7 is a diagram showing a relationship between a straight line connecting a start position of a control point and a target position of the robot shown in FIG. 1 and a speed adjustment area.

FIG. 8 is a diagram showing an example in which the straight lines L1 to L4 shown in FIG. 7 are rotated around the origin and the value of the X axis is constant.

FIG. 9 is a diagram used to determine whether or not a straight line connecting a start position of a control point and a target position of the robot shown in FIG. 1 passes through a speed adjustment region.

[Explanation of symbols]

10 command unit (RAM, storage unit), 14 speed adjustment determination unit (calculation unit, first and second determination unit), 17 alarm unit, 18 speed adjustment command unit (speed command adjustment unit).

Claims (4)

[Claims] 1. A storage means for storing a type of interpolation, a start position and a target position of the control point by teaching control points of a horizontal articulated robot, and the storage means before the robot operates. From the start position and the target position to obtain a straight line connecting the start position and the target position, and the straight line passes through a speed adjustment area set around a singular point of the robot. A first determining means for determining whether or not the first speed generating means generates a first speed command based on a distance from the control point to the singular point when the straight line passes through the speed adjusting region. A robot control device comprising: a command generating means; 2. A second speed command generation for generating a second speed command for interpolating the control point at a predetermined speed based on the type of interpolation, the start position, and the target position read from the storage means. Means for determining that the straight line passes, the first speed command and the second speed command are compared with each other while executing the interpolation operation of the control point. The robot control apparatus according to claim 1, further comprising: a selection unit that selects a speed command. 3. The second speed command generating means sets a distance from the singular point to the control point to R, a maximum rotation speed of the joint shaft to Δθ _1max , and a linear interpolation operation direction and a horizontal direction. _3. The robot controller according to claim 1, wherein the second speed command Vrc is Vrc = _{R.Δθ 1max} / cosφ, where φ is an angle formed by. 4. Set around the singularity,
Second determining means for determining whether or not the straight line passes through the singular point region before the robot operates, the singular point region having a singular point region narrower than the speed adjusting region; The robot control apparatus according to claim 1, further comprising: an alarm unit that determines whether or not the alarm is passed and issues an alarm when the robot passes.