JP2518183B2 - Robot controller - Google Patents

Description

DETAILED DESCRIPTION OF THE INVENTION [Industrial field of application] The present invention is to move a robot arm to a target point,
The present invention relates to a robot control device that moves to a target point via a plurality of designated via points, and particularly relates to an improvement for improving the operation speed.

[Prior Art] The present applicant has applied for a robot control device in Japanese Patent Application No. 62-87552.

This invention makes it possible to specify the maximum speed for each joint when the robot arm passes through a plurality of positions, and moves the joint that requires the longest movement time at the maximum speed of the joint and specifies the maximum speed. It is a robot control device that moves by smoothly changing the speed near the waypoint.

[Problems to be Solved by the Invention] However, in this robot control device, when the acceleration time and the deceleration time cannot be specified independently, and when the representative joint is changed before and after the waypoint, the representative joint is replaced in order to replace the representative joint. There was a problem that there was a low-speed constant velocity section before and after the point.

The present invention has been made to solve such a problem, and the acceleration time and the deceleration time can be set independently, and even if the representative joint is changed before and after the via point, the speed is smoothly changed at the via point. Therefore, it is an object of the present invention to realize a robot controller having a short moving time.

[Means for Solving the Problems] The present invention provides a robot controller that allows a robot arm to smoothly pass through an externally specified waypoint and reach an externally specified target point. The current state recognition unit that stores the information, the command recognition unit that receives and stores the position information of the robot arm target position and the waypoint from the outside, and the current position or waypoint to the waypoint or the target position for each joint of the robot arm. Representative joint selection means that calculates the movement time of the robot and selects the joint that requires the longest movement time as the representative joint of the section, and the acceleration pattern generation that generates the trajectory that accelerates the robot arm from the current speed to the maximum speed. Means and a constant velocity pattern generation means for generating a trajectory in which the robot arm operates at a constant velocity at the maximum speed, A trajectory generating unit comprising a deceleration pattern generating means for generating a trajectory that decelerates until a speed at which a work is started or a stop at a target position, and a waypoint pattern generating means for generating a trajectory based on a predetermined curve near the waypoint. Generation pattern selection means for selecting one of the four pattern generation means from the elapsed time after the start of movement, the moving distance and moving speed of the representative joint, and the trajectory for generating the trajectory of each joint from the moving distance of the representative joint The generation pattern selecting means compares the deceleration stop distance from the moving speed before passing to the stop to the acceleration distance from the stopped state to the speed after passing, whichever is shorter. Comparing means to the via operation distance, if the deceleration stop distance is longer than the via distance, select the trajectory of the constant velocity pattern until reaching the via operation start point,
After that, there is provided a selection means for selecting the trajectory of the waypoint pattern, and the robot control device.

[Examples] The present invention will be described below with reference to the drawings.

FIG. 1 is a configuration block diagram showing an embodiment of the present invention.

In the figure, 10 is a data receiving means for receiving communication from the outside such as a host computer, 20 is a multi-joint robot arm, for example, in the case of 6 joints, the shoulder, elbow, wrist and hand can be rotated up and down, left and right, and twisted. 6 operations are performed.

Reference numeral 30 denotes a position detecting means attached to each joint of the robot arm, and for example, an encoder for setting a rotation angle of the joint or a mark reading device attached to each joint is used.

40 is a current state recognizing unit that stores the current position of the robot arm, 50 is a command recognizing unit that stores the via point and the target position externally commanded via the data receiving means 10, and 60 is the movement time of the joint in each section. It is a representative joint selection unit that calculates and finds a joint having the longest movement time (hereinafter referred to as a representative joint). The above-mentioned section, from the current position to the waypoint,
From waypoint to waypoint (when multiple waypoints are specified), from waypoint to target position, etc.

70 is a trajectory generation unit that calculates the trajectory pattern of acceleration, constant velocity, deceleration, and transit operation required for movement of each section of the current position, target position, and waypoint, and 80 is the trajectory pattern of the trajectory generation unit 70 for each section. A pattern selection unit 90 that selects appropriately is a data storage unit that stores various data necessary for pattern selection.

100 is a joint trajectory generating means for generating a trajectory pattern of each joint including a representative joint in response to the output of the trajectory generating unit 70,
110 is a drive circuit for driving the robot arm 20 based on a signal from the joint trajectory generating means 100.

 Next, a detailed configuration of each unit will be described.

In the current state recognition unit 40, 41 is a current position register that stores the current position of the robot arm 20 from the signal of the position detection means 30, and stores, for example, the rotation angle of each joint. Reference numeral 42 is a previous target position register that stores the value of the previous target position.

In the command recognition unit 50, 51 is a target position register that stores the target position commanded by the data receiving means 10, 52 is a waypoint register that stores the waypoints specified by the data receiving means 10, and there are a plurality of waypoints. May be specified.

In the representative joint selection unit 60, 61 is a movement time calculating means for calculating the movement required time in each section for each joint of the robot arm 20, for example, by dividing the movement amount by the maximum speed predetermined for each joint. Looking for. Reference numeral 62 denotes a representative joint selecting means, which selects the joint having the maximum value from the values of the movement time calculating means 61 as the representative joint.

In the trajectory generation unit 70, 71 is an acceleration pattern generation means for generating a trajectory that accelerates from the current speed to the maximum speed of the representative joint, 72 is a constant velocity pattern generation means for generating a trajectory that operates at a constant speed at the maximum speed, and 73 is A deceleration pattern generating means for generating a trajectory for stopping smoothly from the maximum speed to the transit speed or at a target point, and 74 is a trajectory for performing a transit operation based on a predetermined curve, for example, a quartic spline curve, near the transit point Is a waypoint pattern generating means for generating.

 The operation of the apparatus thus configured will be described.

FIG. 2 is a diagram showing a trajectory when one waypoint is designated. In the figure, the positions given in advance are Is. Maximum speed other than these Deceleration start point And end point of via operation Is calculated. Sections I to VII have these points as boundaries. Also, P _a1,, P _b1,, P _c1,, P _d1 , P
_a2 , P _b2 , P _c2 , P _d2 are respectively This is the distance the robot arm moves before reaching the position.

Less than, The following describes how this device calculates a point in the middle of the movement route when is given.

(1) Operation of the moving time calculating means The moving time calculating means 61 _{judges the} reference distance D _isti for judging whether or not each joint i moves at a constant maximum speed.
Is calculated from the following formula.

D _iSti = (1/2) x (T _UP + T _DN ) x V _{maxi Then} , The distance D _{i of} each section determined by is compared with the discrimination reference distance D _iSti, and when D _iSti <D _i , driving including constant speed driving (hereinafter,
Trapezoidal driving) is performed, and when D _iSti ≧ D _i , driving that does not include constant speed driving (hereinafter, triangular driving) is performed.

The travel time calculation means 61 determines the travel time T _i from the following equation in each case. The current position here Only between

(I) In case of trapezoidal drive (Ii) Triangular drive Here, each sign of each equation is defined as follows.

T _UP : Acceleration time T _DN : Deceleration time V _maxi : Maximum speed for i-th joint T _i : i-th joint movement time i = 1,2,3 ... Is also given, Similarly to the case where is given, the moving time of each joint is calculated after passing.

(2) Operation of Representative Joint Selection Means The representative joint selection means 62 compares the movement time T _i of each joint and selects the maximum value T _k among these. That is, the calculation of T _K = max {T ₁ , T ₂ , ... T _n } is performed to select the representative function k, and D = D _k D _iSt = D _iStk V _max = V _maxk and K _UP = (1 / 2) ・ (V _max / T _UP ): Coefficient during acceleration K _DN = (1/2) ・ (V _max / T _DN ): Coefficient during deceleration S _lanti ＝ D _i / D: Allocation to other joints Calculate and store the coefficient.

(3) Operation of Acceleration Pattern Generation Means The acceleration pattern generation means 71 is the generation pattern selection means 80.
From the above, given the moving distance Pa of the representative joint up to the present, the current speed V _o of the representative joint, and the acceleration end time T _U , the moving distance P (t) of the representative joint is calculated according to the following equation.

(4) Operation of constant velocity pattern generating means The constant velocity pattern generating means 72 is the generation pattern selecting means 80.
From the above, given the moving distance P _b of the representative joint up to the present, the current speed V _o of the representative joint, and the constant velocity end time T _E , the moving distance P (t) of the representative joint is calculated according to the following equation.

P (t) = P _b + V _o · t 0 ≦ t ≦ T _E (5) Operation of deceleration pattern generating means The deceleration pattern generating means 73 is the generation pattern selecting means 80.
From the above, given the moving distance Pc of the representative joint up to the present, the current speed V _o of the representative joint, and the deceleration end time T _D , the moving distance P (t) of the representative joint is calculated according to the following equation.

P (t) = P _C + K _DN (t ⁴ / T _D ² ) = 2 · K _DN (t ³ / T _D ) + V _O · t 0 ≦ t ≦ T _D (6) Via point pattern generation means operation The dot pattern generation means 74 is a generation pattern selection means.
Current joint position from 80 Via position And the via end time T _B is given, the joint position according to the following formula Is calculated.

(7) Operation of joint trajectory generation means The joint trajectory generation means 100 is the waypoint pattern generation means 74
Other than the pattern generation means other than the moving distance P of the representative joint
Given (t), the position P _i (t) of each joint including the representative joint is calculated according to the following equation.

P _i (t) = P _Si + P (t) · S _{lanti In the} above equation, S _lanti = 1 when the position of the representative joint is _obtained .

(8) Operation of generation pattern selection means Position in FIG. Is specified, The operation up to is described in order.

The following data are given from the moving time calculating means 61 and the representative joint selecting means 62.

D _N , D _iStN , V _maxN , K _UPN , K _DNN , S _lantN D _P , D _iStP , V _maxP , K _UPP , K _DNP , _SlantP V _max □: Designated maximum speed of representative joint D □: Representative joint movement distance D _iSt □: V _max □ Moving distance K _UP □: Coefficient during acceleration K _DN □: Coefficient during deceleration □ is _N or _P or less. The operation in each section will be described.

(I) Section I Given the above data, the generation pattern selection means 80
Then, P _a = 0 P _b = P _a + (1/2) (V ₁ + V _o ) T _U V _o = 0 T _U = (V ₁ −V _o ) (T _UP / V _maxN ) where: As the acceleration pattern generating means 71 is selected. When the generation of the acceleration pattern is completed, it moves to section II.

(Ii) Section II First, it is checked whether or not there is a constant velocity section.

D _N -P _a> (1/4) when × of (V ₁ ² / K _DNN), it constant speed section exists, The constant velocity pattern generating means 72 is selected as P _c = P _b + V ₁ · T _E. When the generation of the constant velocity pattern ends, the process moves to section III.

D _N -P _a ≦ (1/4) when × of (V ₁ ² / K _DNN) is not present constant speed section, as P _c = P _b, moves to zone III.

(Iii) Section III Section leading to It is quite possible that the representative joints are different in the section up to. Also, The deceleration stop distance from the speed V ₁ to the stop in the section up to In the section up to, the acceleration distance from the stopped state to accelerating to the speed V ₃ is different (V ₃ is the speed in the next constant speed section). Therefore, in order to smoothly replace the joints, the relationship of V ₂ = min {V ₁ , V ₃ } is given, and further, And Here, the via operation distances called the claims are | P ₀ −P ₁ | and | P ₁ −P ₂ | in the above equation. The deceleration stop distance and the acceleration distance are (1/4) ・ (V ₂ ² / K _DNN ) and (1 /
4) · (V ₂ ² / K _UPP ). In the above formula, the smaller of the deceleration stop distance and the acceleration distance is the via operation distance. Therefore,
The part that performs the above equation is the comparison means in the claims. The following processing is performed to satisfy the above relationship.

From this, the transit speed V ₂ is calculated.

 Then, d is calculated from the following equation.

When V ₂ ≧ V ₁ V ₂ <V ₁ Then, the following process is performed.

Case of D _N −P _C −d> 0 In this case, description will be given separately for the case of V ₂ ≧ V ₁ and the case of V ₂ <V ₁ .

(A) When V ₂ ≧ V ₁ At this time, the speed before passing is less than or equal to the speed after passing.

At this time, since d = (1/4) · (V ₂ ² / K _UPP ), D _N −P _C > (1/4) · (V ₂ ² / K _UPP ) (1). D _N −P _{C in} equation (1) is the deceleration stop distance, (1/4) ・
(V ₂ ² / K _UPP ) is the acceleration distance. In the formula (1), (deceleration stop distance)> (acceleration distance).

Here, as described above, the shorter of the deceleration stop distance and the acceleration distance is set as the via operation distance. Therefore, when (deceleration stop distance)> (acceleration distance), the acceleration distance becomes the transit operation distance. Therefore, (deceleration stop distance)> (passage operation distance).

(B) When V ₂ <V ₁ At this time, the speed before passing is higher than the speed after passing.

At this time, d = (1/4) · (V ₂ ² / K _UPP ) + (1/2) · [(V ₁ ² + V ₂ ² ) / V _maxN ] · T _DN , so D _N − P _C − (1/2) ・ [(V ₁ ² + V ₂ ² ) / V _maxN ] ・ T _DN > (1/4) ・ (V ₂ ² / K _UPP ) (2)

(1/2) · [(V ₁ ² + V ₂ ² ) / V _maxN ] · T _DN in equation (2) is the deceleration distance required to reduce the speed before passing to the specified speed. In the case of (b), since the speed before passing is higher than the speed after passing, such a deceleration distance is provided.

Formula (2) is (deceleration stop distance from the speed after deceleration)>
(Acceleration distance). As in (a),
(Deceleration stop distance from the speed after deceleration)> (via operation distance).

From the above, in both (a) and (b), the deceleration stop distance becomes longer than the via operation distance.

In these cases, the constant velocity pattern generating means 72 is selected as P _b = P _C V ₀ = V ₁ T _E = (D _N −P _C −d) / V ₁ P _C = P _b + V ₁ · T _E . When the generation of the constant velocity pattern ends, the process moves to section IV.

The selecting means in the scope of claims corresponds to a portion that performs the process of selecting the constant velocity pattern generating means 72 when D _N -P _C -d> 0.

D _N −P _C −d ≦ 0 When V ₁ ≦ V ₂ , set to P _d = P _C and move to section IV.

When V ₁ > V ₂ , V ₀ = V ₁ The deceleration pattern generating means 73 is selected as. When the generation of the deceleration pattern ends, the process moves to section IV.

(Iv) Section IV In this section, T _B = 2 (D _N −P _d ) / V ₂ P _a = The via point pattern generating means 74 is selected as the moving distance P ₂ of the representative joint after the via.

 When the generation of the waypoint pattern is completed, the process moves to the section V.

(V) Position to perform the operation after passing through section V At the position To each. In addition, various coefficients □ _P are □ _N
To (the part that represents the coefficient).

 Then, the following processing is performed.

When V ₂ = V ₃ , P _b = P _a and V ₀ = V ₃ are set, and the process moves to section VI.

When V ₂ <V ₃ , V ₀ = V ₂ , V ₁ = V ₃ The acceleration pattern generating means 71 is selected as. When the generation of the acceleration pattern ends, the section moves to section VI.

(Vi) Section VI Perform the same processing as Section II and move to Section VII.

(Vii) Section VII Since the same processing as section III stops, V ₂ = V
₃ = 0, and the deceleration pattern generating means 73 is selected. It stops after the pattern is generated.

In addition, The pattern generating means is not always selected in the order shown in FIG. 2 depending on the positional relationship of 1), the maximum speed of each joint, and the relationship of acceleration / deceleration time. For example, When the position is approaching, the constant velocity pattern of II is not selected and the acceleration section I is immediately moved to the deceleration section III. Also the position When V ₁ <V ₃ , the deceleration pattern is not selected and the waypoint pattern is immediately selected to enter section IV.

Also, in FIG. Is a joint position vector of the _{n-dimensional, V 1, V 2, V} 3 is the velocity of representative _{joints, P a, P b, P} c, is P _d indicates the moving distance of the representative joints.

Further, in the embodiment, the case of interpolating the joint position of the robot having n joints has been described, but it may be applied to the case of interpolating the position represented by the orthogonal coordinate system.

Further, in the embodiment, the case where there is one waypoint has been described, but it is possible to specify a plurality of waypoints.

[Effect] According to the present invention, the acceleration pattern generation means and the deceleration pattern generation means independently generate the acceleration pattern and the deceleration pattern, respectively. And the orbit control suitable for the application becomes possible.

Also, compare the deceleration stop distance from the moving speed before transit to the stop with the acceleration distance from the stopped state to accelerate to the speed after transit, and set the shorter one as the transit operation distance, and the deceleration stop distance is the transit operation. If the distance is longer than the distance, the trajectory of the constant velocity pattern is selected until the start point of the via operation is selected, and then the transit operation is performed, so that the speed in the via operation changes more smoothly and the traveling time is shortened.

[Brief description of drawings]

FIG. 1 is a diagram showing the configuration of an embodiment of the present invention, and FIG. 2 is an operation explanatory diagram of the apparatus of FIG. 10 ... Command position data receiving means, 20 ... Robot arm, 30 ... Position detecting means, 40 ... Current situation recognition section, 50 ... Command recognition section, 60 ... Representative joint selection section, 70 ... Orbit generation section ,
71 ... Acceleration pattern generation means, 72 ... Constant velocity pattern generation means, 73 ... Deceleration pattern generation means, 74 ... Waypoint pattern generation means, 80 ... Generation pattern selection means, 100 ...
Joint trajectory generating means, 110 ... Drive circuit.

Claims (1)

(57) [Claims] 1. A robot control device for smoothly passing a robot arm through a via point designated from the outside to reach a target point designated from the outside, and a current state recognizing unit for storing a current position of the robot arm, A command recognition unit that receives and stores the position information of the robot arm's target position and waypoint from the outside, and calculates the travel time from the current position or waypoint to the waypoint or target position for each joint of the robot arm,
The representative joint selection means that selects the joint that requires the longest movement time as the representative joint of the section, the acceleration pattern generation means that generates the trajectory that accelerates the robot arm from the current speed to the maximum speed, and the robot arm Constant velocity pattern generating means for generating a trajectory that operates at a constant speed at a speed, deceleration pattern generating means for generating a trajectory that decelerates from the maximum speed to a speed at which a via operation is started, or until a stop at a target position, and the vicinity of a waypoint , A waypoint pattern generating means for generating a trajectory based on a predetermined curve in, and an elapsed time after the start of operation is determined by the moving distance and the moving speed of the representative joint. And a trajectory generating means for generating a trajectory of each joint from the moving distance of the representative joint. Generating pattern selecting means, a deceleration stop distance to stop the movement speed before via,
A comparison means that compares the acceleration distance from the stopped state until the vehicle is accelerated to the speed after passing, and uses the shorter one as the via working distance, and if the deceleration stop distance is longer than the via working distance, A robot controller, comprising: a selecting unit that selects a constant-velocity pattern trajectory until reaching, and then selects a waypoint pattern trajectory.