JP3341305B2 - Acceleration / deceleration pattern generation apparatus, acceleration / deceleration pattern generation method, and method for solving inverse kinematics problem and time axis correction method used for the same - Google Patents

Description

DETAILED DESCRIPTION OF THE INVENTION

[0001]

BACKGROUND OF THE INVENTION 1. Field of the Invention The present invention relates to a novel acceleration / deceleration pattern generation apparatus and method which can unify operation control by a general-purpose algorithm which does not depend on a robot structure, and an arithmetic operation. An object of the present invention is to provide a method of solving an inverse kinematics problem and a method of correcting a time axis to reduce a processing load.

[0002]

2. Description of the Related Art When teaching work to a robot,
When the robot operation is divided depending on whether only the end point of the route or the desired route is specified, the PT
P (Poit To Poit) operation (or control) and C
It can be classified into P (Continuous Path) operation (or control).

[0003] In the former PTP operation, only important points on the movement path of the robot are designated, and the path between the points is unquestioned. In the latter CP operation, the movement path of the robot ( Hereafter, the problem is how to specify the “CP route”) and how to accurately trace the robot's hand position along this route.

Further, by adding a function of interpolating between points with a straight line or an arc to the PTP operation, continuous movement of the robot can be reproduced.

[0005]

However, the conventional CP control algorithms have a high degree of dependence on the structure of the robot, which hinders the generalization and standardization of software assets related to CP control. There is a problem.

For example, software related to CP control of an orthogonal robot can be applied to a robot having another structure, such as a vertical articulated robot or the like in its original form, or by making only a small number of changes. It is difficult and it is necessary to individually design a system for CP control software for each robot.

Therefore, the performance of a robot is likely to be influenced by the experience and ability of a designer, and even if good results are obtained, it is the best when this is applied to different types of robots. There is an inconvenience that the method cannot be convinced.

Further, in the CP control, a trapezoidal wave-shaped acceleration / deceleration curve which has been conventionally often used in linear interpolation (that is, a linear speed rise with a predetermined gradient from zero speed at the rising time, and then a constant speed When the acceleration is discontinuous, for example, when the acceleration is discontinuous, for example, when the acceleration is discontinued, There is a problem that an impact is likely to occur at the start point of the start, the end point of the deceleration, and the speed change point at the transition from the acceleration period to the constant speed period or at the transition from the constant speed period to the deceleration period.

This problem is not merely a matter of selecting the acceleration / deceleration curve, but how the robot ability can be maximized for a given CP path to reduce the tact time. Is also involved in the problem.

In other words, in order to prevent the operation from becoming awkward due to the occurrence of unnecessary shocks during the CP control, even if it is necessary to make the acceleration / deceleration curve as smooth as possible, Because there is no established method of how to generate an acceleration / deceleration pattern to satisfy without depending on the individual factors, a number of trial and error factors are necessary to perform appropriate acceleration / deceleration control. You will wander.

[0011] This also causes a problem in connection of the CP route. For example, when transitioning from linear interpolation in a certain section to circular interpolation in a subsequent section, it is necessary to smoothly perform the transition without impact. There is.

The CP control generally requires a considerable amount of calculation (for example, when solving an inverse kinematics problem in which a joint state is determined from the position of a hand of a robot). It is also important to ensure that this does not occur, but to guarantee this without losing the required accuracy.

Further, in the conventional control system, the PTP
Since an algorithm for handling control and CP control unifiedly has not been established, there is a problem that a separate processing routine for each control is required.

[0014]

Therefore, in order to solve the above-mentioned problems, an acceleration / deceleration pattern generating apparatus according to the present invention is directed to a command means for issuing a command relating to a start point and an end point of control with respect to an action point and / or a control curve. A speed peak provisional value determining means for determining a provisional value of the speed peak value on the control curve in accordance with a signal from the command means; and an acceleration / deceleration pattern based on the provisional value of the speed peak value and a control curve. Pattern generation / line sequence calculation means for dividing a line into a number of minute length segments to obtain a line sequence, and solving a forward kinematics problem
Supports homogeneous transformation matrix used for transformation between coordinate systems
Negative feedback file containing calculated operation elements
This gives the command position to the action point and
Kinematics problem analysis means to derive the state of the joint axis corresponding to the speed, and inverse kinematics problem analysis based on the temporary value of the speed peak value
The solution for the joint state obtained by the means is given to a virtual servo system configured by software processing, that is, a simulated servo system corresponding to a real servo system, and is used as a calculation means. Means for determining the optimum value of the speed peak value so that the control accuracy matches the specified or specified accuracy, and the joint state based on the optimum value of the speed peak value. Correction means for performing such recalculation or correction calculation.

In the acceleration / deceleration pattern generating apparatus, the inverse kinematics problem analysis means for obtaining the joint state from the action point position is used when solving a forward kinematics problem for obtaining the position of the action point from the joint state. Negative feedback including an operation element corresponding to the following transformation matrix, a multiplier element for dynamic gain adjustment, an integration element for loop stabilization, and a constant element for adjusting loop convergence speed. It has a filter configuration, and is configured so that a solution of the joint state can be obtained from the preceding stage of the operation element in response to the input of the position information to the inverse kinematics problem analysis means.

In the acceleration / deceleration pattern generation device,
The acceleration / deceleration pattern generation / linear element sequence calculation means includes a curve length calculation means for calculating a curve length of the control curve, a speed peak time point determination means for determining a speed peak time point in the acceleration / deceleration curve according to the curve length, The acceleration pattern related to the joint axis is generated in response to the command regarding the peak point of the time, and the deceleration pattern is generated as a temporally folded pattern by performing a temporally symmetric operation on the acceleration pattern when generating the deceleration pattern. It comprises a pattern generating means for generating, and a line element sequence calculating means for calculating a line element string by dividing the control curve at predetermined time intervals according to the shape of the acceleration / deceleration curve.

In such an acceleration / deceleration pattern generation apparatus, the following acceleration / deceleration pattern generation method is used to generate an acceleration / deceleration pattern for controlling the movement of an action point along a designated control curve.

First, after the minimum unit of the calculation time for the control curve is determined, the length of the curve of the control curve is determined, and the peak time point of the speed corresponding to the length of the curve is determined. A linear element sequence by generating an acceleration / deceleration pattern having dynamic symmetry, determining the provisional value of the speed peak value based on the allowable value of the memory capacity, and dividing the control curve at a unit time interval of the calculation time. Is derived.

When solving the forward kinematics problem based on this line element sequence and the information relating to the position command of the action point , the coordinate system
The operation element corresponding to the homogeneous transformation matrix used for the transformation of
Perform filter calculations for negative feedback, including
By giving the position of the action point, the joint corresponding to this
Solve the inverse kinematics problem of finding the state of the axis. The control accuracy is obtained by giving the solution to a virtual servo system configured by imitating a real servo system by software processing, and based on a magnitude relationship between the control accuracy and a specified (default) or specified accuracy, a speed peak value is determined. Determine the optimal value.

Then, based on the optimum value of the speed peak value, the result of recalculation or correction calculation corresponding to the solution of the joint state is sent as a control command to the servo system.

Further, as a method of solving the inverse kinematics problem, an operation element corresponding to a homogeneous transformation matrix used in solving a forward kinematics problem for finding a position of an action point corresponding to a state of a joint axis, and a dynamic It is assumed that a negative feedback filter including a multiplier element for gain adjustment, an integral element for loop stabilization, and a constant element for adjusting the loop convergence speed is configured.

Then, the line sequence obtained by dividing the control curve at the minimum calculation time interval and the position information of the action point are given as input information to the filter, the filter is started, and the convergence in the loop is awaited. The state of the joint axis is obtained from the previous stage of the calculation element.

By repeating such a procedure for each joint axis, a joint state as a solution to the final inverse kinematics problem is obtained.

In a control system in which the calculation time is operated in the minimum unit, the control curve cannot be treated as a continuous line, and the actual calculation involves discretization by finite difference approximation. In order to guarantee a high approximation accuracy between the trajectory and the commanded control curve, the following time axis correction method is adopted.

First, in the acceleration / deceleration curve, the speed peak value and the moving distance during the acceleration and deceleration periods are modified so that the peak time of the speed and the total moving time are integer multiples of the minimum unit of the calculation time. After sampling the data string indicating the joint state with the minimum unit of the calculation time, the data is held at the minimum unit time interval.

Then, the joint state is calculated by a calculation based on the relationship between the length of the line element sequence obtained by dividing the control curve at the time interval of the minimum unit of calculation time and the shape of the acceleration / deceleration curve. A new time sequence corresponding to the indicated data sequence is generated without being bound by the minimum unit of calculation time.

Then, by re-sampling the set of the new time sequence and the data sequence indicating the joint condition by the minimum unit of the calculation time, the information of the joint condition arranged at equal intervals in time is obtained. .

[0028]

According to the present invention, in the CP control, an acceleration / deceleration pattern that basically defines a speed peak time point corresponding to the length of the control curve is generated, and a line element sequence related to the control curve is calculated. It is possible to know the joint state as a solution to the inverse kinematics problem based on the position command on the control curve and, therefore, treat each joint axis equally regardless of the combination of the arms etc. that compose the robot system This eliminates the inconvenience that the algorithm relating to the generation of the acceleration / deceleration pattern must be largely changed due to the difference in the structure of the robot.

In the present invention, when analyzing the inverse kinematics problem, the operation structure relating to the solution of the forward kinematics problem is used to reflect the robot structure, and the negative feedback filter including the operation element is calculated. Can solve the inverse kinematics problem through, so the algorithm is theoretically clear, and there is room for the designer's ability and experience to intervene in this point (for example, a method to solve the inverse kinematics problem mathematically in advance) The versatility of the algorithm for generating the acceleration / deceleration curve is high, and the calculation process is performed by four arithmetic operations. In addition, since the calculation can be performed by about the trigonometric function calculation, the calculation time can be reduced and the processing load on the CPU can be reduced.

In the present invention, the generation of the acceleration / deceleration pattern
In the method, the time axis is corrected at the time of sampling in order to move the action point along the specified control curve with high accuracy , so that the computational load required for generating an acceleration / deceleration curve, speed control, etc. In addition to being able to respond to faster processing by reducing the time axis correction, the axis movement from the variable related to the curve length of the CP control is simply performed without making any changes to the basic part of the CP control algorithm by interposing this time axis correction. PTP control can be easily realized by performing analogical substitution with variables relating to quantities, and a unified processing system for both controls can be constructed.

[0031]

DETAILED DESCRIPTION OF THE PREFERRED EMBODIMENTS Hereinafter, an acceleration / deceleration pattern generation apparatus and an acceleration / deceleration pattern generation method of the present invention, and a method of solving an inverse kinematics problem and a method of correcting a time axis used therein will be described in detail.

The basic concept of the present invention is that an algorithm for generating an acceleration / deceleration curve is closely related to an algorithm for obtaining a solution relating to the state of the arm system from the position of the hand of the robot. Since further discussions will be developed, these basic items will be sequentially described below before describing the configuration of the apparatus.

First, an algorithm for generating an acceleration / deceleration pattern will be described with reference to FIGS.

A control curve a (hereinafter, referred to as a “CP curve”) shown in FIG.
This shows the locus drawn over.

An arbitrary point on the CP curve a is defined as Pi, and an expression in a rectangular coordinate system fixed to the base axis of the robot or the ground is defined as P
i (xi, yi, zi) (i = 0, 1, 2,..., N
(Natural number), coincides with the starting point S when i = 0, and i =
At the time of N, coincides with the end point E).

A CP curve a between the point Pi and the starting point S
Is "1", and the total length of the CP curve a is "L".

If the length of the n-th line element when the CP curve a is divided into N line elements is “Δln”, then C
The P curve a is represented by a line element sequence ｛Δ1, Δ12, Δ13,.
ΔｌN｝ (hereinafter abbreviated as “｛Δln｝”).

In this mapping, the line element sequence does not represent the shape characteristic of the CP curve a, as is clear from the fact that the line element sequence uses only the length of the segment. (In other words, the fact that the mapping from the curve to the line element sequence is not a one-to-one mapping above means that the line object operation is performed on the CP curve a with respect to the straight line passing through the start point S and the end point E. It is clear that the same series of line element sequences can be obtained by performing the same line element decomposition for the curved line.)

The parameter representing the time axis is represented by "t".
The sampling (sampling) time is “Ts”, and the time required for the speed to reach the peak value from the acceleration start time (hereinafter, referred to as “speed peak arrival time”) in the acceleration / deceleration curve is “Tp”. . The definitions of the above quantities are shown in [Table 1] below.

[0040]

[Table 1]

The distance l is a function L (xi, yi, zi)
It can be expressed using.

FIGS. 4 and 5 are graphs schematically showing an acceleration / deceleration curve (a horizontal axis represents a time axis, and a vertical axis represents a speed (V = dl).
/ Dt). ), Whose geometrical features are:
The point is that time symmetry is recognized between the curve shape during the acceleration period and the curve shape during the deceleration period.

That is, the acceleration / deceleration curve 1 shown in FIG. 4 shows an acceleration pattern portion 2 during the acceleration period until the speed reaches its peak value (this is referred to as “Vp”), and a constant speed pattern during the constant speed period. A speed deceleration pattern portion 4 from the speed Vp until the speed becomes zero, the relationship of acceleration period = deceleration period = Tp is established, and the acceleration pattern portion 2 is decelerated by performing a time-return operation. It can be superimposed on the pattern section 4.

More specifically, the acceleration pattern section 2 is set at t = T
This means that by performing a parallel movement in the time axis direction after folding back by a line symmetric operation with respect to p, it is possible to match with the deceleration pattern portion 4.

The acceleration / deceleration curve 5 shown in FIG. 5 includes an acceleration pattern section 6 and a deceleration pattern section 7 without a constant speed period, and has a clear line symmetry with respect to t = Tp.

Since the area surrounded by the acceleration / deceleration curves 1 and 5 and the time axis corresponds to the total length L of the above curve,
The acceleration / deceleration curve 1 in FIG. 4 is an acceleration / deceleration curve generated when the curve length L is long to some extent.
Is an acceleration / deceleration curve generated when the curve length L is short.

FIG. 6 is a flowchart showing a procedure for obtaining the speed peak arrival time Tp and the line element sequence in obtaining such an acceleration / deceleration curve.

First, the total length L of the curve is obtained in step 1).

In this case, the CP curve is divided into a system-specific curve prepared in advance on the robot system side and a user-specified curve specified by the user.

The former is required for linear interpolation, circular interpolation, and the like. Since the former is often a basic shape such as a straight line or a circular arc in a three-dimensional space, its total length L is easily obtained. be able to. The latter can be approximately determined by the user assuming a straight line between adjacent points based on the coordinates of a plurality of points representing the curve using teaching means such as a teaching pendant.

That is, the coordinates of the (n−1) -th point and the n-th point specified by the user are represented by (Xn−1, Yn−), respectively.
1, Zn-1) and (Xn, Yn, Zn), the total length L is obtained as the sum of the lengths of the element rows as shown in the following equation.

[0052]

(Equation 1)

Incidentally, such a polygon-like approximation is used for simplicity of description, and the spline approximation method can be appropriately used for interpolation between points according to required accuracy.

Next, as shown in procedure 2), the speed peak arrival time Tp corresponding to the entire length L is determined.

For this purpose, as shown in FIG.
It is necessary to preliminarily define the relationship between the speed and the speed peak arrival time Tp.

FIG. 7A shows the relationship between the curve length L on the horizontal axis and the speed peak arrival time on the vertical axis.

As shown, the value of Tp has a lower limit Tpmi.
n, there is an upper limit Tpmax. In other words, the lower limit exists because the response of the servo control is finite, and is defined by the time constant of the servo system, and the upper limit is due to the finite maximum speed of the servo motor. Exists.

FIG. 7B shows the relationship between the curve length L on the horizontal axis and the moving time (this is referred to as "Tm") on the vertical axis. , Line graph 8
Upper point Pmin (Lmin, 2 · Tpmin) and point Pm
As can be seen from the fact that the first derivative is discontinuous at ax (Lmax, 2 · Tpmax), three modes can be divided for the acceleration / deceleration pattern.

FIGS. 8 to 10 are graphs showing the behavior of the acceleration / deceleration curves in each mode.

In the first mode corresponding to the range of 0 ≦ L ≦ Lmin, the traveling time is Tm = 2 · Tpmin regardless of L.
8, the acceleration / deceleration curve is shown in FIG.
As shown in the figure, the height of the mountain increases as the width is constant and L increases.

In the second mode corresponding to the range of Lmin ≦ L ≦ Lmax, the moving time Tm (= 2 · Tp) increases with the moving time L, so that the acceleration / deceleration curves 10, 10,. As shown in FIG. 9, the height and width of the mountain increase as L increases.

In the third mode corresponding to the range of Lmax ≦ L, the value of Tp reaches a plateau, so that the movement time Tm increases according to the length of the constant speed period. As shown in 10, 11, 11,..., The acceleration period and the deceleration period remain unchanged, and the constant speed period increases as L increases. In FIG. 7B, the rate of change of Tm with respect to L (that is, the inclination of the graph in the range of Lmax ≦ L) is the maximum speed of the servomotor (this is referred to as “Vpma
x ”. ).

As described above, since Tm is closely related to Tp in the L-Tm characteristic, the previous procedure 1)
The Tp value corresponding to the curve length L obtained by the above can be calculated for each mode.

Step 3) shows a process related to the generation of an acceleration / deceleration curve corresponding to each mode, which has been disclosed by the present applicant in Japanese Patent Application Laid-Open No. 3-147103 or Japanese Patent Application No. 2-28.
3867 can be followed.

Here, only the essential points will be described. A standard form function relating to the shape of an acceleration / deceleration curve is prepared in advance, and a temporal scaling operation and the above-described temporal symmetric operation are performed on each of the standard functions. Is used to generate an acceleration / deceleration curve corresponding to.

This standard form function considers a cubic function as a transfer function of the servo system, and performs an inverse Laplace transform on a relational expression expressing a response output when a step-like input signal is given in a frequency domain. This is obtained by re-expressing in the time domain, and is expressed as the sum of exponential functions as shown in the following equation.

Incidentally, it is considered that there is not much benefit in going into the explanation of the derivation of the equation, so the explanation is omitted for the details (see Japanese Patent Application No. 2-137615).

If the standardized distance function at L = 1 and Tp = Tpmin is ln (t: Tpmin),
These are the time constants T1min, T2min, T3 of the servo system.
It is expressed by the following equation using min.

[0069]

(Equation 2)

If Tp corresponding to the curve length L is obtained in the procedure 2), the parameter α (= Tp /
Tpmin) is determined, and Ti (i corresponds to the axis index and i = 1, 2, 3) or δ (Tp) defined by the following equation:
To determine.

[0071]

(Equation 3)

The above equation represents a scale conversion operation using the parameter α.

The parameters β, δ (Tp) in the above equation
Β (hereinafter referred to as “basic parameters”) is a constant, and δ (Tp) corresponds to an area surrounded by the curve and the time axis in the acceleration / deceleration curve as shown in FIG. It is equal to the width of a square of height Vp.

The function corresponding to Tp is l (t: Tp) (0
≤ t ≤ Tp), this is represented by the following equation.

[0075]

(Equation 4)

Note that ln (t: Tp) in the above equation is
This is a normalized function in which Tpmin is replaced by Tp in Expression 2.

Further, lp is a movement amount during acceleration / deceleration, and satisfies lp = Vp · δ (Tp).

In this relational expression, note that the amount of movement during deceleration is equal to the amount of movement during acceleration since the deceleration pattern has temporal symmetry with the acceleration pattern. Since β is obtained from lp · ln, if t = Tp is substituted into an expression obtained by differentiating [Equation 4] with respect to time, it can be obtained from the definition expression of δ shown in [Equation 3].

The acceleration / deceleration curve can be generated based on an equation obtained by differentiating the equation (4) with respect to time. That is, the time derivative of the above function ln (t: Tp) is represented by an acceleration period 0 ≦ t ≦ Tp
Because it defines the shape of the acceleration / deceleration curve in
As described with reference to FIG. 8 to FIG. 10 in the procedure 2), the entire acceleration / deceleration pattern is performed by performing a temporal turning operation in the deceleration period and an operation of adding a constant value pattern in the constant speed period in accordance with the Tp value. Can be obtained.

Such a pattern is formed at a predetermined time interval T
A set of data calculated by sampling at s (for example, 10 ms) is stored as time-series data in a memory (RAM).

Then, in the next procedure 4), a line element sequence {Δli} (i = 1, 2, 3,...) Is obtained.

That is, the expression [4] is obtained by calculating the acceleration period 0 ≦ t ≦
Since Tp expresses the movement of the action point on the CP curve a for each sampling time Ts, the length Δln of the line element can be obtained by using this equation as in the following equation.

[0083]

(Equation 5)

Regarding the length of the line element during the deceleration period,
By focusing on the symmetry of the acceleration / deceleration pattern as described above, the line element length Δln calculated during the acceleration period can be easily obtained by going backward in time in a backward direction.

When the acceleration / deceleration pattern includes a constant speed pattern, the line element length Δln is equal to the product of the sampling time Ts and the speed Vp as shown in the following equation.

[0086]

(Equation 6)

Through the above procedure, an acceleration / deceleration curve corresponding to the curve length L and a line element sequence {Δli} (i = 1, 2, 3,...) Obtained by cutting the CP curve a at the sampling time Ts are obtained. Will be.

Next, the solution of the inverse kinematics problem that is essential for CP control will be described.

When the kinematics of the robot is viewed from the viewpoint of the relationship between the hand position and the joint displacement or posture, the problem of obtaining the robot hand position corresponding to this state when the displacement or posture of each joint is given is given. The inverse kinematics problem is a forward kinematics problem. On the other hand, the inverse kinematics problem is a problem of obtaining joint displacement and posture of a robot when a hand position is given.

In general, it is easy to solve the forward kinematics problem. For example, in the case of a scalar robot, the product of a homogeneous transformation matrix described in a link coordinate system fixed to each joint of the robot arm is calculated. That can be reduced.

On the other hand, in the CP control, the trajectory control of the robot along a desired route, that is, the inverse kinematics of how to control each joint of the robot to draw this trajectory is required. He is interested in solving problems.

It is generally difficult to solve the inverse kinematics problem,
In addition, there is a problem that the time required for processing is long due to the large amount of calculation.

In the present invention, a software filter ( hereinafter, referred to as an “invert filter”, which includes a homogenous transformation matrix obtained when solving a forward kinematics problem as an operation element ,
It corresponds to kinematics problem analysis means. See the figure for a specific example.
See FIG. ), And a method of obtaining solutions of the forward kinematics problem and the inverse kinematics problem through this filter (hereinafter, referred to as “invert filter method”).

In this method, the formalization of the mathematical solution is relatively easy, and the processing can be unified regardless of the difference in the robot structure (for example, orthogonal type, scalar type, vertical type, etc.). In the application to each robot, it is possible to easily cope with it only by changing a part of the solution.

In the following, since a homogeneous transformation matrix is required in the configuration of the invert filter, the solution of the forward kinematics problem will be briefly described, and then the configuration of the invert filter will be described.

First, the link coordinate system fixed to the joint axis of the robot and the definitions of symbols used below will be described.

In mathematically expressing the motion state of the robot in the three-dimensional space, a reference coordinate system (hereinafter referred to as a “reference coordinate system”) is set, and the robot is fixed to an object from the reference coordinate system. Coordinate system (hereinafter “object coordinate system”)
That. ), It is necessary to describe the relative relationship between the two when looking at the origin position and each coordinate axis.

An index is attached to each joint of the robot, an object coordinate system is set for each joint, and the displacement and posture of the joint are represented by a state vector, and the hand position of the robot is represented by a position vector in a reference coordinate system. Then, the two can be related by a mapping in which the state vector of the joint is an independent variable and the position vector is a dependent variable.

The link coordinate system is a set of object coordinate systems set for each joint. When a conversion rule between adjacent coordinate systems is known, a homogeneous transformation including a rotation operation and a translation operation is performed. The link coordinate system and the reference coordinate system can be related to each other by clarifying the contents of the above, and this is nothing but solving a forward kinematics problem.

The relative positional relationship between adjacent object coordinates is
It can be described using parameters such as the distance and angle between joints (hereinafter, referred to as “link parameters”), and is referred to as “Denavit-Hartenberg notation”.
It has been known that the link mechanism can be described by four variables.

[0102] assigned index by an integer value n of the reference coordinate system and the object coordinate system is in the _{"Σ n (O n; X n} , Y n,
Z _n )], with the point O _n as the origin, X _n , Y _n ,
_Let Zn be the Cartesian coordinate axis.

The coordinate system Σ _n (O _n ; X _n , Y _n , Z _n )
Is defined as <in, jn, k _n >, and the expression of the vector r in the coordinate system Σ _n is described as “ ⁿ r”.

[0103] Then, the next when viewed Σ _n system basic vectors Σ _m system a set of things that were expressed in the ^{_{<m i n, m j n}} , m k n> and, the Σ _m system from Σ _n system a transformation matrix referred to as ^"n T _m".

The trigonometric functions of the relative rotation angle θ between the coordinate systems are “S (θ) (= SINθ)” and “C (θ)
(= COS θ) ”.

The above definitions are summarized in Table 2 below.

[0106]

[Table 2]

Next, the relationship between the rotational state of each joint and the position of the hand will be described using a scalar type robot as shown in FIG. 12 as an example.

In this example, the first arm is provided so as to be rotatable with respect to the base shaft, and a translational movement is possible at the tip of the second arm which is mounted so as to be rotatable with respect to the first arm. A simple tool mounting shaft is provided, and a rotatable tool is attached to the tip of the shaft. That is, the robot has a link structure in which three rotating joints and one translation joint are connected in series.

[0109] Regarding the coordinate system, four orthogonal coordinate systems Σ fixed to each joint axis according to the above definition are used.
₀ (O ₀ ; X ₀ , Y ₀ , Z ₀ ), Σ ₁ (O ₁ ; X ₁ , Y ₁ ,
Z ₁ ), Σ ₂ (O ₂ ; X ₂ , Y ₂ , Z ₂ ), Σ ₃ (O ₃ ; X ₃ ,
Y ₃ , Z ₃ ) and Σ ₄ (O ₄ ; X ₄ , Y ₄ , Z ₄ ) are set.

Here, the coordinate system Σ ₀ (O ₀ ; X ₀ , Y ₀ , Z
The origin O _{0 of 0} ) is the center of the base shaft portion of the robot, the height thereof is equal to the tip position of the tool mounting axis after the completion of the origin position setting, and the downward direction is the positive direction of the Z ₀ axis. .

The coordinate system Σ ₁ (O ₁ ; X ₁ , Y ₁ , Z ₁ )
Indicates that the origin O ₁ coincides with the origin O ₀ of the coordinate system Σ ₀ (O ₀ ; X ₀ , Y ₀ , Z ₀ ), and the X ₁ axis is selected as the center axis of the first arm.
An axis in a horizontal plane orthogonal to this is defined as the Y ₁ axis, and the Z ₁ axis is an object coordinate selected to coincide with the Z ₀ axis of the coordinate system Σ ₀ (O ₀ ; X ₀ , Y ₀ , Z ₀ ). System.

[0112] Incidentally, the the angle formed between the X ₁ axis and X ₀ axis .theta.1.

The coordinate system Σ ₂ (O ₂ ; X ₂ , Y ₂ , Z ₂ ) has its origin O ₂ placed at the center of the rotary joint connecting the first arm and the second arm, and its height is represented by coordinates. System Σ ₀ (O ₀ ;
X ₀ , Y ₀ , Z ₀ ) is the same as the height of the origin O ₀ , the X ₂ axis is selected as the center axis of the second arm, and the axis in the horizontal plane orthogonal to this is the Y ₂ axis. , Z ₂ axes are in the coordinate system Σ ₀ (O ₀ ; X ₀ ,
Y ₀ , Z ₀ ) is an object coordinate system selected on an axis parallel to the Z ₀ axis. Note that the the angle formed between the X ₂ axis and the X ₁ axis .theta.2.

In the coordinate system Σ ₃ (O ₃ ; X ₃ , Y ₃ , Z ₃ ), the origin O ₃ is the center of the axis on which the tool is mounted, and the height at the time of completion of the origin position setting is the coordinate system Σ ₂ (O 3). ₂ ; X ₂ , Y ₂ , Z ₂ ) are the same as the height of the origin O ₂ , the Z ₃ axis is selected as the vertical axis, and the 2 axes X ₃ axis and the Y ₃ axis in the horizontal plane orthogonal to this are set to Σ ₂ (O ₂ ;
X ₂ , Y ₂ , and Z ₂ ) are object coordinate systems selected in parallel with the two axes X ₂ and Y ₂ .

Note that the length of the tool mounting shaft is “P3”,
The displacement is defined as “P3 · θ3”. Coordinate system Σ ₄ (O ₄ ; X
₄ , Y ₄ , Z ₄ ) has its origin O ₄ in the coordinate system Σ ₃ (O ₃ ; X ₃ ,
Y ₃ , Z ₃ ) coincide with the origin O ₃ , the Z ₄ axis thereof coincides with the Z ₃ axis, and the axes obtained by rotating the X ₃ axis and the Y ₃ axis at an angle of θ4 in the horizontal plane are each represented by the X ₄ axis. , Y ₄ axes.

Note that the length of the first arm is "L1" and the length of the second arm is "L2".

By using the knowledge about the matrix expression about the rotation operation, the translation operation, and the like, a homogeneous transformation matrix when the coordinate system の_i is viewed from the coordinate system Σ _{i + 1} can be obtained.

For example, the homogeneous transformation matrix ⁰ T ₁ is given by the following equation
Also, by generalizing to a homogeneous transformation matrix that takes into account the translation of the arm length in addition to the rotation operation about the vertical axis, the homogeneous transformation matrix ¹ T ₂ is obtained, or the angle parameter is simply replaced. the homogeneous transformation matrix ³ T ₄ and by can be easily obtained.

[0119] Also, ² T ₃ is determined as the following equation as a homogeneous transformation matrix incorporating the translation operation about the X ₂ axis and Z ₂ axes only.

[0120]

(Equation 7)

The product ⁰ T ₁ ¹ T ₂ ² T ₃ ^{3 is} added to these homogeneous transformation matrices.
By calculating T ₄ , ⁰ T ₄ is obtained, and the coordinate system Σ ₀
And Σ ₄ will be related.

Next, the solution to the inverse kinematics problem will be described. In this case, the two are related by a mapping in which the position vector of the hand position is an independent variable and the state vector of the joint is a dependent variable, that is, a formal relationship. Is equivalent to obtaining an inverse mapping of the above-described mapping.

FIG. 13 is a block diagram showing the basic configuration of the invert filter.

Symbols “Xr” and “Yr” in the figure are command values for the X coordinate and Y coordinate, respectively, and “R ² r” is a command value for the distance between the working end position and the origin of the reference coordinate system. And these are selectively fed to an invert filter.

In this example, for the sake of simplicity, the structure of a two-link system is taken up as shown in FIG. 14, and the operation will be limited to the operation in the plane.

FIG. 14 shows a linear model (only the central axis of the arm is shown) of an arm system having two rotary joints in the XY coordinate system, and shows a first position closer to the origin O.
An angle formed by the arm with respect to the X axis is defined as θ1, and an angle formed by the second arm farther from the origin with respect to an extension of the first arm is defined as θ2.

The position of the tip of the second arm is set to a point P (X,
Y), an arbitrary point in the XY plane is defined as a point P '(X', Y ').
And the distance between the origin O and the point P is R. In addition, each arm length is set to 1 for simplification of description.

As shown in FIG. 13, the invert filter includes a transfer function G _F (s) relating to a path used for solving a forward kinematics problem (hereinafter referred to as “forward path”). Value is gain 1 / | G _F (s)
|, An integral operation (indicated by an integral symbol), and a constant gain 1 / Ga to be sent to the forward path G _F (s).

Then, outputs X, Y, and R of the forward path
² is selectively returned to the input nodes of these command values through a negative feedback loop ("ε" means error).

In this example, G _F (s) is ³ T in the equation (7).
_In response to the sum of the nodes Θ and Φ and Φ itself, addition and squaring are performed through trigonometric function elements indicated by C and S (“C” indicates a COS function and “S” indicates a SIN function). each output value X by calculating, Y, configured as R ² can be obtained.

Note that Θ is the phase in the filter and is controlled in the loop, while Φ is the phase fixed for each joint axis.

The phase Θ corresponds to the angle θ2, the phase Φ corresponds to the angle θ1, the phase sum Θ + Φ is sent to the node X through C, and the phase Φ is combined at the node X through C, Further, the phase sum Φ + Φ is sent to the node X through S, and the phase Φ is combined at the node Y through S. The sum of the square of the square and Y of X is taken out in node R ^2.

This can be intuitively understood by assuming that this corresponds to the following expression expressing the X and Y coordinate values using the link parameters θ1 and θ2 when the arm state of FIG. 14 is imagined. it can.

[0134]

(Equation 8)

Of the elements provided before the forward pass in the invert filter, 1 / | G _F (s) | is provided for dynamic gain adjustment.

The integral element is provided to stabilize the loop. Although the integral element is provided as a single stage in the figure, it is apparent from the filter theory that the filter characteristic can be generally changed by employing a plurality of stages. .

In this example, when the square of R is obtained from the equation obtained by replacing the angle parameter of equation (8) with the phase, it is found that the square depends on COS よ う as shown in equation (9).
1 / | G _F (s) | is obtained from an equation obtained by differentiating this with Θ.

[0138]

(Equation 9)

The constant gain 1 / Ga is set to control the speed of loop convergence.

Switching at the input stage of the command values Xr, Yr, R ² r and switching of the output values X, Y, R ² of the forward path are interlocked so as to correspond to each other.

Next, a method for obtaining a solution to the forward kinematics problem and the inverse kinematics problem by the invert filter having such a configuration will be described.

It is easily understood from the above description that the forward kinematics problem can be obtained by using a forward path which forms a part of the above-mentioned invert filter.

That is, when values are given to the nodes Θ and Φ in FIG. 13 while the invert filter is kept open loop, output values X and Y can be obtained, and a result corresponding to [Equation 8] can be obtained.

The following procedure is used to solve the inverse kinematics problem using the invert filter (FIG. 13,
See FIG. ) .

(A1) When it is desired to change the state of each arm from the posture at a certain point of time to the next posture, the angle parameters θ1 and θ2 at the present time (see FIG. 14).
The value of is known. This is because it can be read from the position counter value.

(A2) Then, the phase corresponding to the value of θ1 at that time is set to the node Φ in FIG. (A3) Next, a distance Rr between OP ′ (see FIG. 14) is calculated. The point P 'is given as a target position of the movement command , and is known in advance. Then, the square value of Rr is set as an input value to the invert filter (see FIG. 13) . That is, in the invert filter, switching control is performed so that the command value R ² r and the output value R ^{2 of the} forward path are selected, and a closed loop is formed with respect to the square value of the distance.

(A4) Activate the invert filter and wait until the error ε shown in FIG. 13 converges to zero.
When ε = 0, the value of the node Θ at that time (this is referred to as “Θ ′”) is obtained as a solution to the inverse kinematics problem for the second arm.

(A5) Then, the target position P ' (FIG. 14)
reference. ) Corresponding to the XY coordinates X ′ or Y ′
r or Yr is set as an input value to the invert filter, and a closed loop is formed such that the output value X or Y of the forward path is fed back (“−” in FIG. 13).
See the path indicated by "1". ) .

(A6) At the same time, the value of Θ ′ obtained in (A4) is simultaneously set in the node Φ in FIG. 13 to activate the invert filter, and the value of the node の when the error ε becomes zero is It can be obtained as a solution to the inverse kinematics problem for one arm. (In practice, it is not necessary to perform calculations with a precision higher than the precision. If the error | ε | It is good to do so.)

By the above procedure, the angle parameter value indicating the state of the arm is obtained from the command value of the target position and the distance . When such a procedure is geometrically interpreted with reference to FIG. It is easy to grasp the meaning content.

FIG. 15 shows a model of the movement of the robot arm, similarly to FIG. 14. The rotation state of the arm with respect to the above procedure is shown in (a) to (c) over time. Things.

First, as shown in FIG. 15A, an arc CIR having a radius of OP '(= Rr) is drawn around the origin O of the XY coordinate system.

Next, as shown in FIG. 15 (b), while the value of the angle θ1 of the first arm is fixed, the second arm is rotated counterclockwise about the joint center O ', and the tip position P is changed. The rotation is stopped when the position P1 intersects with the arc CIR.

The angle θ2 ′ of the second arm at this time is the solution of the inverse kinematics problem for the arm.

Since the invert filter is configured as a negative feedback, the second arm is always rotated in the direction in which the point P approaches the target point P ', and is not rotated in the opposite direction. .

The movement so far corresponds to the procedures (A2) to (A4) for the invert filter.

Next, as shown in FIG. 15 (c), the first arm is rotated with the angle θ2 of the second arm fixed at θ2 ′, and the end point of the second arm which has coincided with the point P1 until then. The rotation is stopped when P coincides with the target point P '.

At this time, the angle θ1 ′ of the first arm is the solution of the inverse kinematics problem for the arm. Note that this movement is based on the procedure (A5) and (A5) for the invert filter.
6).

The sequence for realizing such a movement control using the invert filter is the above-described procedure (A1).
Through (A6).

In FIGS. 14 and 15, the number of arms is two.
However, it is a matter of course that the invert filter method can be generalized to a system having N arms.

In this case, if the number of activations of the invert filter is increased according to the number of arms, the inverse kinematics problem can be solved using exactly the same algorithm. For example, when the number of arms is N, if the joint center of the (N-1) th arm corresponds to the origin O in FIG. 14 and the joint center of the Nth arm corresponds to the point O ', respectively. Well, each angle parameter θN, θN
After determining −1, the same filter calculation is performed by reducing the value of N, and an angle parameter sequence θJ (J = N−2, N−) related to the arm near the origin of the reference coordinates of the robot is obtained.
The final solution can be obtained by sequentially determining 3,..., 1).

In the above-described example, only the arm structure having the rotary joint is shown. However, the present invention is not limited to this. An arm system including only a linear joint, a rotary joint and a linear joint may be used. The invert filter method can be applied to an arm system in which is appropriately combined. At this time, the parameter to be solved for the linear joint is displacement.

As described above, since the algorithm related to the invert filter method is basically independent of the arm structure, it is highly versatile, and therefore can be applied to various manipulators such as the orthogonal type, the scalar type, and the vertical articulated type. it can.

By the way, in the above example, only the CP control for the hand position of the robot has been described for the sake of simplicity, but according to the invert filter method,
It is also possible to realize CP control for an arbitrary position on the work (however, in general, if a point on the work is given in the work coordinate system, the work is adjusted by adding an offset based on the hand position. The upper position can be easily obtained.)

For example, as shown in FIG. 16, a disk is provided at the end of the second arm so as to be rotatable around a point O ″, and a point PS on the disk is set to a point PE. Assume a case of moving. Note that the rotation angle of the disk with respect to the point O ″ is θ3.

The procedure for solving the inverse kinematics problem for this example is as follows.

(B1) First, the joint center O 'of the second arm
Is drawn, and an arc CIR1 having a radius equal to the distance between this point and the target point PE is drawn. This operation corresponds to a process of setting the square of the distance between the point O and the point PE as an input to the invert filter, and fixing the value of the angle parameter θ2 of the second arm to the node Φ. (B2) Before starting the invert filter, a difference between the distance between the points O ′ and O ″ and the distance between the points O ″ and the points PS is obtained, and the squared value is used as a limiter value. This is because it is theoretically impossible that the distance between the point O 'and the point PS becomes larger than the limiter value.
When the distance between S reaches the limiter value, it is necessary to stop the operation of the filter there and to solve the angle θ3 at that time.

(B3) The invert filter is activated. That is, this corresponds to rotating the disk about the θ3 axis in FIG.

(B4) When ε = 0, the distance between the point O ′ and the point PS is equal to the distance between the point O ′ and the point PE. Therefore, the operation of the filter is stopped at this point. The value of the node Θ at this time is a solution for θ3. When the distance between the point O 'and the point PS does not reach ε = 0 and reaches the limiter value of the above (B2), if the operation of the filter is stopped at this point, a solution of θ3 can be obtained at the node Θ.

FIG. 16 shows an example corresponding to the former case. A point P'S on an arc CIR1 having a radius equal to the distance between point O 'and point PE and centered on point O' is solved. It is a corresponding point.

(B5) By rotating the second arm while fixing θ3, the point P ′S moves on the circular arc CIR1 and finally coincides with the point PE, and the angle θ2 at this time becomes a solution. . If this movement is made to correspond to the operation of the invert filter, the command value Rr is set to a value equal to the distance between the point O′-PE, Φ is set to the phase value corresponding to θ1, and the filter is started, and ε = 0 Becomes θ at the node Θ
It is nothing but a solution of 2.

In this example, the locus and the circular arc CI are set at the point PS.
Since R1 intersects at the intersection P'S, it suffices only to find a solution for θ3 and θ2. However, if there is no intersection because the distance between the point O ′ and the point PS is small, three angles θ are used.
3, θ2, and θ1 (the procedure will be clear from the algorithm described above).

The features of the above-described invert filter method can be summarized as follows in a bulleted list.

(C1) The input value and feedback amount of the invert filter that can be selectively switched are set as parameters suitable for the joint structure, and the present invention can be applied to various robots only by changing the forward path according to the link structure. , CP control can be standardized.

(C2) Since a forward path is provided in the invert filter and the forward kinematics problem can be solved from the open loop of the invert filter, the inverse kinematics problem solving routine and the forward kinematics problem solving routine Does not need to be prepared as a completely independent device, and processing can be partially shared.

(C3) The solution can be obtained efficiently.
In other words, solving the inverse kinematics problem is nothing less than finding the arm state from the robot's hand position. However, as described above, the joint axis closer to the hand with smaller inertia has a larger motion and is closer to the base shaft portion with larger inertia. Thus, a unique solution can be obtained such that the movement becomes smaller as the joint axis becomes smaller (for example, the amount of rotation of the first arm is zero in FIG. 16).

(C4) A solution for an arbitrary point on the work can be obtained. That is, a solution can be obtained not only for the CP control for the origin position of the coordinate system fixed to the tool tip but also for an arbitrary point in the coordinate system. This is as described in FIG. Also, for example,
Circular interpolation can be applied to the origin or any point of the coordinate system of the operating end at the time of circular interpolation.

(C5) Solving can be performed using a forward function included in a forward path without using an inverse function. As a method of solving the inverse kinematics problem, a geometrical solution or an algebraic solution is known, but it is known that this requires a considerable amount of computational power and takes a long time to process.

For example, when a solution for the arm system as shown in FIG. 14 is obtained according to a geometric solution, the angle θ
Although 1, and θ2 are represented using the coordinate values X and Y, an inverse trigonometric function, a reciprocal of a square root, and the like appear. If the same problem is solved by an algebraic method, then ATAN2 (a, b) ≡ARG
Since a function defined by (b + j * a) (where j is an imaginary unit) or a square root appears, the complexity in formulating a solution is not eliminated.

In these solution methods, basically, a program is created such that a formula of the solution is mathematically calculated manually, a solution is obtained by software processing, and the solution is evaluated to obtain a final solution. There is a need.

Therefore, it takes time to find the formula, and it is complicated to prepare a function appearing in the formula. If such an analysis work is not performed for each type of robot, No.

In the present invert filter method, on the other hand, a process in accordance with a common algorithm independent of the robot structure is performed using an invert filter when obtaining a solution. Performing multiple times on leads to the final solution.

In other words, by sequentially taking the procedure of extracting the subsystems of the link system that are the central elements and finding the solutions, the solutions can be accumulated to obtain the final solution.

In this sense, it can be said that this method is a dimension reduction method for obtaining a solution while reducing the degree of freedom in the link coordinate system.

For example, as shown in FIG. 17, in a diagram in which a vertical articulated robot is linearly modeled, a point PS (XS, YS, ZS) on one meridian is shifted to a point P on another meridian.
Assume movement to E (XE, YE, ZE).

In FIG. 17, the rectangular coordinate system XYZ is used.
The arm closer to the origin O is called the first arm, and the arm ahead is called the second arm. The angle formed by the projection of the first arm on the XY plane with respect to the X axis is θ1, An angle between one arm and the Z axis is θ2, and an angle between an extension axis of the first arm and the second arm is θ3.

In this case, it can be easily understood that the point PS and the point PE are not on the same plane by comparing the coordinate value ratios YS / XS and YE / XE, and the relationship between the two points is three-dimensional. First, it is necessary to reduce the dimension so as to cause a two-dimensional problem, that is, activate the invert filter so that the solution of θ3 and θ2 is obtained in the plane including the meridian and then the solution of θ1 is obtained.

As described above, it is possible to obtain a degenerate solution by lowering the degree of a problem, and then take the degeneration to proceed to a final solution.

Next, an algorithm combining the procedure for generating the acceleration / deceleration curve of the CP control and the procedure relating to the invert filter method will be described with reference to FIG.

(D1) First, the calculation time interval of the CP curve,
That is, the sampling time Ts is determined.

This time Ts can be shortened with an increase in the processing speed of the CPU used for controlling the robot, but at present Ts = 10 milliseconds, and the CP curve is calculated at this time interval. The cut information is sent to the servo system.

In the servo system, loop calculation is performed at each time interval of the servo loop sampling time (this is referred to as “Tss”), and Tss is smaller than Ts.

Therefore, it is ideal that Ts is reduced to Ts = Tss (the CP curve can be made smoother). However, considering that the storage capacity of the memory becomes larger, Ts is taken into account. Should be determined.

(D2) An acceleration / deceleration curve is generated according to the length of the CP curve, and a line element sequence at that time is calculated.

Since the algorithm relating to the generation of the acceleration / deceleration curve has already been described, the details thereof are omitted, and here, the determination of the peak speed Vp in the third mode in which the acceleration / deceleration curve has a constant speed pattern portion will be described.

Assuming that the joints are rotary joints, the ideal value for Vp is such that the angular velocity of each joint does not exceed its maximum value and the phase of each joint is within the allowable range of the servo system and the mechanical system. The value is as large as possible under the condition of being settled.

In other words, the maximum speed at which the trajectory and speed of the CP path are within the certain accuracy as specified by the command value is ideal.

However, at this point, it is not possible to know the value of Vp, so a provisional value Vp ⁰ of Vp is set, a line element sequence {Δln} is calculated using the provisional value Vp ⁰ , and the result is stored in a memory. To memorize.

It is desirable that the provisional value Vp ⁰ be varied in accordance with the shape of the CP curve, and a value which is about half of the maximum speed value in the CP control is a guideline for the time being. The method will be described later in detail because it relates to the memory capacity.

Here, the provisional value V is calculated by some method.
The description is continued assuming that p ⁰ has been determined.

(D3) Based on the line element sequence {Δln}, a joint angle sequence {θin} related to the i-th joint axis is obtained using an invert filter.

The line element sequence {Δln} on the CP curve is obtained by (D2), and the coordinate value (Xn, Yn, Zn) of the division point when the given CP curve is cut at the sampling time Ts And the distance Rn from the origin of the reference coordinate system to each division point can be easily calculated. If these are input as command values to the invert filter, the joint angle sequence {θin} can be calculated as described above. Such a joint angle sequence can be obtained for each joint axis by activating the invert filter a plurality of times.

That is, the above-described invert filter method may be applied to the CP curve at every division point.

Then, at this time, the angular velocity train ｛dθ
in / dt} and an angular acceleration sequence {d ² θin / dt ² }, and a phase error sequence (referred to as {Δθin}) when the joint angle sequence is input to the servo loop system. Store it in memory. Incidentally, for these columns, only the maximum value of each of the columns may be stored in the memory.

FIGS. 19 and 20 show visualized examples of the results obtained by the above processing.

In these figures, for simplicity, a two-dimensional operation relating to the arm system as shown in FIG. 14 is shown, and FIG. 19 shows the temporal change of the working end position r of the arm and the velocity dr / dt. Is shown in comparison with the temporal change of the data.

FIG. 20 shows an example of a temporal change of the joint angles θ1 and θ2.

The time te in both figures indicates the end point of the operation.

The joint angle sequence 関節 θi obtained in this manner
As described above, n｝ is calculated on the basis of the temporary value Vp ⁰ of Vp. Therefore, using the joint angle as it is does not sufficiently derive the motion ability of the robot. After some evaluation of
Correction processing such as correcting the temporary value of p and calculating the joint angle sequence again is required.

(D4) A method of determining the optimum value of Vp using the accuracy of CP control as an evaluation amount will be described.

The joint angle sequence is finally given as a command value to the servo system, and the phase Θ corresponding to the joint angle θi (t)
Let G (s) be a function linking i (s) and the phase error ΔΘi (s) corresponding to the error output εi (t), and the relationship between the two is represented by the following equation [Equation 10]. When defined, by applying the inverse Laplace transform to this, the equation [11] is obtained.

[0212]

(Equation 10)

[0213]

[Equation 11]

[Equation 11] In the equation, "L ^-1 " is an inverse Laplace operator, and g (τ) is the inverse Laplace transform of G (s). This is the configuration of the servo loop. Is known from the above, and Δθi (t) can be calculated by using [Equation 11].

In the equation (10), the joint angle is treated as a continuous quantity. However, the data is actually a discrete value by sampling, and is described by a mathematical expression in a discrete system.

These values do not use the results actually obtained as the response of the servo system, but a virtual motor (hereinafter, referred to as "simulation motor") assembled by software processing as a simulation of the servo system. ) Or using an expression obtained by discretizing Expression (11).

Every time θi (t) is determined at a certain time t, Δθi (t) is obtained, and at the same time, the position error ε (t) relating to the CP curve is calculated using this.

For this, a relational expression between the linear velocity of the CP curve and the joint angular velocity is used.

If the position vector of the working end of the robot is vector “r” and the joint angle vector is vector “θ”, the relationship between the two can be related to r = F (θ) using a function F. Therefore, by differentiating this with time t, [Equation 12] can be obtained.

[0220]

(Equation 12)

Note that ∂F / ∂θ in the above equation is a so-called Jacobian.

The pattern shape related to the vector dr / dt is given by an acceleration / deceleration curve, and the maximum value is Vp.

It is to be noted that looking at the lower equation obtained when the scaling operation on the time axis is performed by dividing both sides of the upper equation of the [Equation 12] by the parameter α, the vectors dr / dt, dθ / dt and Note that this will expand and contract at the same ratio (α) (this fact will be used later). A column such as Δθ = [Δθ1 Δθ2...] ^T ("T" means transposed value). Expressed as a vector, [Equation 1
2] The position error ε (t) is defined by the following equation using the equation, and the maximum value of ε (t) is set to εmax.

[0224]

(Equation 13)

On the other hand, the accuracy of the CP control for the robot is determined by the specification (default value).
If AX is used, the evaluation of Vp can be divided into three cases depending on the magnitude relationship between εmax and εMAX.

I) In the case of εmax <εMAX CP control accuracy higher than the specification accuracy is realized. Therefore, the robot may be operated by adopting the temporary value of Vp as it is. From a point of view, this provisional value may be too slow.

Ii) The case where εmax> εMAX.

Since the CP control accuracy does not satisfy the accuracy of the specification and the provisional value of Vp is too fast, it is necessary to make the value smaller than this.

Iii) The case where εmax = εMAX.

The CP control accuracy just satisfies the specification accuracy, and the provisional value is also moderate in speed.

From the above, in order to maximize the robot's ability for any shape of the CP curve, iii)
It can be seen that the condition (1) is a necessary and sufficient condition.

Then, the optimum value of the Vp value is determined by the following method.

First, it will be examined how θi (s) is converted by a temporal scale conversion t → α · t using the parameter α.

When the Laplace transform is applied to θi (α · t) obtained by multiplying θi (t) by α with respect to the time axis from the Laplace transform definition equation, the following equation is obtained.

[0235]

[Equation 14]

It should be noted that α · t = τ is replaced during the calculation.

[Equation 14] indicates that the operation of multiplying θi (t) by α with respect to the time axis is the Laplace expression Θi of θi (t).
(S) is reduced to 1 / α, and the frequency is set to α.
This indicates that the operation corresponds to the operation of reducing the value by 1 (the Laplace variable becomes s / α).

FIG. 21 shows this conversion rule visually, where Θi (s), Θi (s / α), Θi (s / α)
From the comparison with / α, it can be seen that the gain and the frequency are compressed together.

In other words, if the error Δ 伸縮 i (t) needs to be expanded or contracted by 1 / α, the above-mentioned scale conversion may be performed, and this conversion reduces the joint angular velocity to 1 / α as shown in the following equation (Equation 15). can do. At this time, G (s)
The reason why the conversion property is not a problem is that G (s) represents the characteristics of the servo loop itself. Therefore, the characteristics cannot be easily changed due to the design so as to secure the frequency characteristics. Therefore, it is possible to adjust the gain of the output by extending the time axis of the input signal of G (s).

[0240]

(Equation 15)

From equation (12), setting the joint angular velocity to 1 / α means setting the CP curve speed to 1 / α. Therefore, by appropriately selecting the value of α, the position error ε (t) can be obtained. Can be determined so that the maximum value εmax of the above becomes equal to the specification accuracy εMAX (this is referred to as “Vpmax”).

That is, as shown in the following expression [Equation 16], V
The temporary value of p multiplied by the accuracy ratio εMAX / εmax is given by V
pmax, and the above-described processes (2) to (4) may be performed again to determine the joint angle sequence.

[0243]

(Equation 16)

The following is a summary of the method of determining the above Vp value.

The expression on the frequency axis for the phase error is [Equation 10], and the expression on the time axis is [Equation 1].
1], the position error ε (t) of the CP curve is related to the phase error ΔΘ as shown in [Equation 13], and it is noted that the relationship between the two is linear.
To make (t) 1 / α, the phase error Δθi
It can be seen that it is sufficient to set the value to 1 / α.

From the expression [14], it is known how the phase changes in the expression of the phase on the frequency axis by the temporal scale conversion operation. Therefore, the expression of the phase error associated with the phase by the expression [10] is obtained. The scale conversion rule becomes clear.

That is, when the joint angle θi (t) is multiplied by α with respect to the time axis (that is, θi (α · t)), the phase error Δθ (t), the position error ε (t) and its maximum value εma
x can be reduced to 1 / α.

According to the equation (12), the joint angle and the CP curve speed are also reduced by a factor of α by such a scale conversion operation.
It can be understood that V becomes as shown in [Equation 16].
The temporary value of p and its appropriate value are converted to a parameter α relating to the accuracy ratio.
Can be tied together.

Therefore, it is possible to obtain a CP curve speed that exactly satisfies the specification of the robot CP control accuracy with respect to a given CP curve, and realize control for maximizing the performance of the robot.

At this time, the maximum speed in the acceleration / deceleration curve differs depending on the CP curve, and the maximum speed also changes if the control accuracy changes (or the maximum speed changes if the control accuracy is artificially operated). (Which can be varied on purpose) is a noteworthy matter.

For example, when the user sets the Vp value and the accuracy εMA
Assuming that the system is constructed so that X can be designated, a temporary acceleration / deceleration curve based on this Vp value is generated, εmax is calculated by a calculation process using a simulation motor, and the magnitude of εmax and εMAX is calculated. It becomes possible to modify Vp according to the relationship.

(D5) The temporary value of Vp is determined, the joint angle sequence is obtained on a trial basis, and the optimal Vp value is determined using the accuracy as an evaluation amount. However, this means that such a correction process is required as a concept, but does not mean that exactly the same calculation must be actually performed twice.

For example, by using a joint angle sequence obtained using the provisional value Vp ⁰ and performing a time axis correction on this,
It is possible to derive a joint angle sequence that would be obtained if Vp was at an optimal value.

For this, the following method is used.

The joint angle sequence {θin} is sent to the servo system for each sampling time Ts, but this Ts is a basic time for the CPU and the servo system, and cannot be changed freely.

Therefore, a new sampling time ts defined by the following equation is prepared on software.

[0257]

[Equation 17]

If both sides of the equation (17) are multiplied by n, n · t
s = α · n · Ts, and the increment of ts is made every sampling of the Ts interval.

Therefore, at time n · Ts, θi (n ·
Ts) without sending it to the servo system, and θi (n · ts) = θi
If (α · n · Ts) is sent to the servo system, the expansion / contraction operation can be performed at α times the time axis.

In FIG. 22, α = 2 with respect to θi (n · Ts).
5 shows a state when a temporal zoom operation of α = と is performed.

It should be noted that the joint angle sequence to be obtained by such an operation is closely related to the use of the temporal scale conversion for the generation of the acceleration / deceleration curve as described above. is there.

Also, this method can be applied to an override method in which the CP control speed is controlled at a desired level, for example, at 1% intervals based on the highest speed (in this case, the same data is used). It is desirable to perform linear interpolation or the like as necessary because the data is sent to the servo system several times every Ts or, conversely, data is thinned out.)

Although the joint angle sequence {θin} has been obtained as described above, when the acceleration / deceleration curve has a pattern as shown in FIG. 10, the joint angular velocity becomes the maximum of the drive motor of the joint shaft. There is a risk that the speed will be exceeded, and if this is not dealt with, the motor may be damaged.

For example, in a stepped phase command as shown in FIG. 23A, the value θi at a certain sampling time
Difference Δθ between n−1 and value θin at the next sampling time
The value obtained by dividing i by the sampling time Ts corresponds to the angular velocity.
It may be larger than θimax.

FIG. 23 (b) shows this situation on the CP curve, and the difference in the distance between the n-th point Pn and the (n + 1) -th point Pn + 1 exceeds the allowable value (note that FIG. , The figure emphasizes the difference between the CP curve and the trajectory to facilitate understanding of the problem.)

Therefore, the following method is adopted as a method for regulating the joint angular velocity so as not to exceed the maximum value.

First, as shown in FIG. 24A, coordinate values obtained by dividing the total length L of the CP curve into N equal parts are stored in the memory. The figure shows an example in which N coordinate value storage memories (that is, a capacity of N × 4) for preparing coordinate values (X, Y, Z, R) for four axes are prepared.

The results of passing the data in the memory for storing coordinate values through the above-mentioned inverting filter section are stored in the memory for storing joint angles in order. In this case, if a sufficient amount of memory is secured, a memory for storing coordinate values and a memory for storing joint angles can be separately provided, but if the amount of memory is limited, both memories can be used together. Just do it.

[0269] Then, after performing the data memory for joint angle storage (described later in detail) the time axis correction process under temporary speed Vp ⁰ as shown in FIG. 24 (b), and inputs to the simulation motor . This process is performed simultaneously for each joint axis.

FIG. 24B is a signal flow graph showing an example of the configuration of the simulation motor.
Nodes εi and ωi indicate a phase error and a joint angular velocity of the joint axis specified by the index i, respectively.

By constructing such a simulation motor as software processing, the convolution calculation, phase error, speed, acceleration, and the like can be obtained as required, and the node value can be easily known. , Nodes εi and ωi are constantly monitored,
Each of the maximum values of the space curve accuracy ε and ωi (these values are referred to as “εma
x ”and“ ωimax ”. ).

The accuracy, angular velocity, and Vp designated by the user are set to “εMAX”, “ωiMAX”, “VpMAX”, respectively.
X ”,“ α ”defined by the following equation [18]
(Ε) ”,“ α (ωi) ”(i = 1 to 4),“ α (V
p)) as the maximum value of “αm
“ax” can be used as the final value of the time-axis scaling parameter (this is guaranteed by Equations (12), (13), and the like). No more problems.

[0273]

(Equation 18)

[0274]

[Equation 19]

It should be noted that the equation [18] may include a limitation on the joint angular acceleration, that is, the motor current.

FIGS. 1 and 2 show an outline of the acceleration / deceleration pattern generation device 12 according to the present invention.

The command issued from the command section 13 is
It is sent to a Vp ⁰ determination unit 14 that determines a provisional value Vp ^{0 of} p.

The acceleration / deceleration pattern generation / line sequence calculating unit 15 in the subsequent stage is a part for calculating the line sequence {Δln} based on the provisional value Vp ⁰ , and calculates the curve length corresponding to each process of the above-mentioned algorithm. It comprises a unit 15a, a Tp value determination unit 15b, a pattern generation unit 15c, and a line element sequence calculation unit 15d (see FIG. 2).

That is, the line element sequence has the curve length L as described above.
The CP curve is obtained by dividing the CP curve into segments at intervals of the sampling time according to the shape function of the acceleration / deceleration curve generated based on the Tp value corresponding to the Tp value and calculating the respective lengths (however, a time axis correction described later) According to the processing, the line element sequence of the CP curve is equally divided according to the memory amount.)

Then, based on this line element sequence, the joint angle sequence {θi
After obtaining n 求 め, the optimum Vp value determination unit 17 determines an appropriate value of Vp based on a calculation using a simulation motor, performs a correction calculation for the line element sequence, and finally relates to the acceleration / deceleration pattern. The control command is sent to the servo system and the mechanism system 18.

The correction calculation based on the appropriate value of Vp can be obtained in principle by re-calculation through the acceleration / deceleration pattern generation / line element sequence calculation unit 15 and the invert filter unit 16 as described above. If a correction calculation equivalent to this is performed by an extension operation on the time axis, the calculation result of the correction calculation unit 19 can be sent to the servo system and the mechanism system 18 as it is, as shown by a two-dot chain line in FIG. .

The configuration example of the invert filter section 16 is as shown in FIG.

Next, with regard to the speed of the CP control, it is necessary to consider the following possibilities.

In the acceleration / deceleration curves described so far, the control in which the speed value during the constant speed period is fixed has been taken up. However, the speed is decelerated as necessary during the constant speed control to shift to the stop. If you want to perform high-speed control when the curvature of a segment with a CP curve is close to zero and form a straight line, and if you want to perform control with reduced tension so that the speed is reduced in a segment with a large curvature There is.

In the following, a method of modulating the sampling time (hereinafter, referred to as a “sampling time modulation method”) will be described as a solution to this request and the above-described limitation of the joint angular velocity.

First, as shown in the acceleration / deceleration curve of FIG.
A description will be given of deceleration control in the case where it is necessary to decelerate from the middle (at this point of time “T0” and the subsequent deceleration curve is indicated by a broken line in the figure) during the constant speed period after acceleration due to some necessity.

For example, when an emergency stop is applied, or when it is desired to stop the operation at an arbitrary point on the closed curve during the control of rotating the working end of the robot periodically and many times along the closed curve. And the like.

As described above, since the joint angle sequence {θin} obtained as a solution to the inverse kinematics problem is stored in the memory, if this data is sent to the servo system as it is, deceleration will occur in the middle of the constant speed period. Can not be transferred to.

Therefore, after t = T0, the joint angle sequence {θin} must be newly calculated at sampling time intervals to obtain a deceleration pattern as shown by the broken line in FIG.

However, there is a restriction that the sampling time Ts is a fixed value, and there is a demand for shortening the recalculation time.

Therefore, an algorithm is used that enables a deceleration pattern to be generated by reusing the already calculated joint angle sequence {θin} before starting the CP control.

In the acceleration / deceleration curve including the constant speed period, the speed during the constant speed period is Vp, the movement amount between Ts is Δl, and t = T
Time t = n · Ts for deceleration pattern after 0
When speed and V _EXP (n · Ts) in, defined as the following expression modified time sequence {tsn}.

[0293]

(Equation 20)

Here, V _EXP (n · Ts) and Vp are known.

FIG. 26 illustrates the meaning of this time sequence {tsn}. In the time example {tsn}, the area of speed V _EXP (n · Ts) × time tsn drawn in the process of deceleration is Δ
This is a time sequence defined so as to have an area equal to 1 (the amount of movement between Ts) (in the figure, the area is shown as the area of the diagonally shaded portions having different inclination directions).

Therefore, if the sampling time is not a fixed value but the joint angle sequence is sent to the servo system at intervals of this time sequence {tsn}, an accurate deceleration pattern can be drawn. Since the values are fixed as described above, a method as shown in FIG. 27 is adopted.

FIG. 27 shows the trajectory and the trajectory of the deceleration pattern by the sampling time modulation method together with the trajectory of the joint row that moves at a constant speed from the joint angle at t = T0. The time axis t is divided at intervals of Ts, and the phase of the joint angle shown on the vertical axis is sampled for each Ts.

The positions indicated by the black circles indicate the locus of the constant speed pattern, and the phase value is held in each section, and the section has a step-like shape.

The position indicated by a triangle in the figure is a time sequence Δt
t = tsn and Θ = Θ when sn｝ is taken on the time axis t
n indicates the intersection with n, and indicates the locus of the deceleration pattern corresponding to the above [Equation 20]. This trajectory is a step-like pattern in which the phase value is held in each section of the time sequence {tsn}.

A data string approximated by resampling this trajectory at intervals of the sampling time Ts is indicated by white circles, and Δn for each sampling time Ts.
Of the time sequence {tsn} can be obtained while holding the value of the time sequence several times.

As is apparent from FIG. 27, the difference between the two is at most about the product of Ts and phase, and is a relatively good approximation.

FIG. 28 conceptually shows a block configuration related to the sampling time modulation method. The solution of the inverse kinematics problem is stored in the memory 20, and these are the timing switch 21 at Ts intervals and the 0th-order hold at the subsequent stage. Part 22
, After being modulated by the tsn timing switch 23 (that is, modulation by speed),
Next hold unit 24, Ts interval timing switch 25
Is sent to the servo system via.

[0303] The reason why the zero-order hold units 22 and 24 are used is to respond to actual demands in consideration of simplification of processing and speeding up. In general, a higher-order hold can be used. .

Since the joint angle sequence θin is sampled at intervals of Ts and already stored in the memory, when the acceleration / deceleration curve is determined, the time sequence {tsn} and the corresponding phase sequence are determined as shown in FIG. Thus, a stepped waveform sampled and held at the timing of tsn is obtained (see the triangles in FIG. 27).

By sampling this waveform again at intervals of Ts, a phase sequence indicated by white circles in FIG. 27 is obtained.

In order to reduce the error between the phase sequence of the time sequence {tsn} and the resampled phase sequence, linear interpolation is used (that is, the 0-order hold unit 24 on the subsequent stage in FIG. 28 is used). If an interpolation function is provided), the approximation is further improved.

Now, a variation δv of the velocity v = Δθ / Δt when the movement amount is Δθ and the movement time Δt is obtained (this δ means the variation and is different from the above-mentioned basic parameter δ). , As shown below.

[0308]

(Equation 21)

That is, at most (Δθ / (Δt) ² ) · T
It can be seen that a change in s occurs and is proportional to Δθ.

If the change is too large, a smooth operation cannot be guaranteed. To reduce the change as much as possible, a linear interpolation as shown in FIG. 29 is performed.

That is, time n · Ts, (n + 1) · Ts
Are both Θn and the phase value at time (n + 2) · Ts is Θn + 1 (indicated by a black circle in FIG. 27), the phase values at time (n + 1) · Ts are Θn and Θn
If the value is set to a value intermediate between +1 (shown by a square in FIG. 29), a smooth operation can be realized without any problem in the accuracy of the CP control.

[0312] In short, this interpolation method can be said to be a method of reducing the speed change δv by reducing Δθ in the equation (21) and smoothing the operation.

In FIG. 27, the phase is plotted on the vertical axis, but from the viewpoint of accuracy, the vertical axis represents the coordinates (X,
If the sampling time modulation method described above is applied by taking Y, Z), linear interpolation on the CP curve can be performed, and improvement in accuracy can be expected. Of course, if the memory capacity can be increased, necessary and sufficient accuracy can be obtained.

The following functions can be realized by using the sampling time modulation method described above.

(E1) Smooth deceleration control after multi-rotation control along a closed curve.

(E2) CP control speed override method (variable speed method).

(E3) To freely change the CP control speed so that the CP control accuracy is constant.

(E4) User's designation regarding the moving distance during the acceleration / deceleration period.

Hereinafter, description will be made in this order.

(E1) First, in the case of performing periodic CP control, that is, performing rotation control many times along a closed curve, for example,
In the case where the paste is performed from the second rotation, if the application is stopped at the end of the rotation, the work can be performed without being affected by the robot operation in the acceleration / deceleration pattern.

FIG. 30 shows an acceleration / deceleration curve including a constant speed pattern of the peak speed Vp when the sum of the movement amounts in the CP curve length L, the acceleration period Tp, and the deceleration period Tp is 1 _Tp .

If such an acceleration / deceleration curve is connected over a certain range, it is not possible to secure a constant speed period sufficient for the work. Will happen.

Therefore, in the multi-rotation control, a plurality of rotation trajectories are regarded as one CP curve, and only a joint angle sequence in a constant speed pattern is prepared, and the above-described sampling time at the rise of the acceleration / deceleration curve is obtained. If the modulation method is used, the same algorithm can be used at the time of acceleration / deceleration.

The time sequence Δts at the time of acceleration or deceleration
n｝ is C since the values of Tp and l _Tp are known in advance.
It may be calculated and obtained before the start of the P control,
Alternatively, the processing may be performed in real time during Ts.

(E2) Next, an override method relating to CP control will be described.

When the CP control is performed according to the acceleration / deceleration pattern as shown in FIG. 30, the following two types of control methods can be used to freely vary the control speed with respect to the maximum speed.

[0327] First, when the total travel time is T,
If you want to increase this by p%, set the time axis to 100 / p
It is a method of operating twice.

That is, the new sampling time is set to “Ts
When “p” is used, the calculation may be performed as in the following equation.

[0329]

(Equation 22)

However, since the sampling time Ts is a fixed value so as to be repeated many times, the sampling time modulation method is applied also in this case.

In this method, the total movement time can be varied at a predetermined ratio, for example, in steps of 1%, and the speed can be reduced accordingly. There is a drawback that it is not easy to know whether it has fallen.

On the other hand, it is considered that changing the CP control speed directly is more practical, and the other method can cope with such a situation. Now, the time t = n · Ts before the override is applied
Is the speed at v (n · Ts), and the phase value at that time is Θ
Assuming that (n · Ts), the speed when the p% override is applied is V (n · Ts) · p / 100.

Therefore, the modified sampling time tsn for performing the override so that the locus follows the given CP curve can be obtained as shown in the following equation.

[0334]

(Equation 23)

After the time sequence {tsn} is obtained in this manner, the algorithm of the sampling modulation method may be applied as it is.

(E3) In the CP control, generally, the accuracy of the trajectory and speed is an important evaluation factor, and the working time zone of the robot is often a constant speed pattern portion of an acceleration / deceleration curve.

However, it is conceivable that the user may encounter a situation where he wants to operate the robot at a speed as fast as possible within a certain accuracy from the control start point to the end point on the CP curve.

That is, it is desired to enable control such that a straight segment on the CP curve is traced at a high speed by increasing the speed, and a segment having a large degree of bend is traced at a low speed by decreasing the speed.

FIG. 31 shows the definition of error vectors and various quantities between the command locus of the CP curve and the actual locus.

The point S in the figure indicates the start point of the CP curve, and the point E
Indicates the end point.

The vector ε (t) represents an error vector between the CP curve and the locus as a function of time.
Its magnitude is | ε (t) | ≧ 0.

A vector r (t) drawn from the origin O toward an arbitrary point on the CP curve indicates a position vector at that point.

The phase error of the motor is represented by ΔΘ (t) =
[ΔΘ1 ΔΘ2...] ^T, and a transfer function that defines the relationship between phase input and output for the servo system is G (s).

Also, let εMAX be the position accuracy of the CP control, and let εu be the accuracy that can be specified by the user using a teaching pendant or the like.

As can be seen from the equations [11] and [13], the value of | ε (t) | is expressed as the following equation as the absolute value of the product of the convolution calculation result Δθ and the Jacobian.

[0346]

(Equation 24)

FIG. 32 is a graph showing an example of the temporal change of | ε (t) |.

The zooming parameter on the time axis is α
Assuming that (t) is time-dependent, the following equation can be obtained assuming that an equation obtained by dividing both sides of equation (24) by this α (t) is equal to the user-specified precision εu.

[0349]

(Equation 25)

That is, by performing a scaling operation of the parameter α (t) depending on the time axis with respect to time, εu
Can be made constant.

Therefore, both sides of the upper equation of the equation (12) are expressed by α
From the following equation (26) obtained by dividing by (t), the CP control speed changes under the condition that εu = constant.

[0352]

(Equation 26)

For example, when the position error ε (t) is large, α (t) becomes a large value, so that the speed becomes low.

By performing modulation by such an operation on the time axis, a new time tn with respect to the sampling time Ts is obtained as in the following equation.

[0355]

[Equation 27]

The intended purpose can be achieved by applying the sampling time modulation method based on this new time sequence {tn}.

At this time, if a limit value is not set for tn, α (t) = 0 when | ε (t) | = 0, which may hinder the operation. It costs.

Next, the peak speed V which has been reserved
A method of determining a temporary value of p and the relationship between the method and the sampling time modulation method will be described.

The fact that the determination of Vp is closely related to the memory capacity can be inferred from the large amount of memory consumption when Vp is small.

In the following, the provisional value Vp ⁰ of Vp, the optimum speed V
The relationship between _pOPT and the memory capacity will be described.

The following table defines the symbols and their meanings.

[0362]

[Table 3]

Now, assuming that the movement is controlled at the same speed Vp ⁰ along the CP curve of the start point S and the end point E as shown in FIG. 33 (this is of course a virtual one), The moving time T _all ⁰ is calculated by setting the curve length L to the provisional value Vp of Vp.
It is divided by ⁰ , and it becomes like the following formula.

[0364]

[Equation 28]

Therefore, the CP curve has a sampling time Ts
Is divided into Nn pieces as shown in the following equation.

[0366]

(Equation 29)

If this Nn is exactly equal to the memory capacity Nm per axis, it means that the curve is divided with the maximum accuracy using the memory.

That is, the provisional value Vp ⁰ may be determined as in the following equation.

[0369]

[Equation 30]

When the curve length L is small, if the value obtained from the above equation [30] is used as it is, the division accuracy will be high, but it will take a long time to calculate. Therefore, the maximum value Lmax of L is obtained. May be determined in advance, and the memory capacity Nm is consumed at the value Lmax, and the memory capacity nm per unit length may be determined as shown in the following equation.

[0371]

(Equation 31)

If the provisional value Vp ⁰ is determined in this manner, CP
The number of divisions per unit length for a curve can be made constant, and the calculation time for a CP curve with a short curve length can be shortened.

Next, generation of an acceleration / deceleration pattern by the sampling time modulation method will be described. The joint angle sequence {θin} obtained for each division point on the CP curve divided into Nm pieces as described above is stored in the memory after the processing in the invert filter unit.

As is clear from the fact that the subscript i of the joint angle sequence {θin} distinguishes each joint axis and n specifies the division point, this sequence is a set composed of Nm × i elements. .

Therefore, in this set, elements having the same n, for example, in the case of a four-axis link configuration, (θ1n,
By taking out (θ2n, θ3n, θ4n) and sending them to the servo system, the working end can be moved to the n-th division point in the CP curve shown in FIG.

If the data of the joint angle train is sent to the servo system at intervals of Ts, the action point moves on the CP curve at a constant speed Vp ⁰ .

If data is taken out from the set {θin} at a certain time interval and sent to the servo system, the action point can be moved along the CP curve at a speed corresponding to this time interval.

For example, if data is sent to the servo system at intervals of 2 · Ts from n = 1, Vp / V
The action point can be moved at a speed of 2.

Therefore, the time interval tsn in the acceleration / deceleration period will be obtained for a case where the acceleration / deceleration curve has a shape as shown in [Equation 4].

In the acceleration / deceleration curve shown in FIG.
in is the minimum value of the Tp value, and is selected so as to be K (integer) times the sampling time Ts as shown in the following equation.

[0381]

(Equation 32)

Assuming that the movement amount between Tpmin is 1 min, this is as follows.

[0383]

[Equation 33]

Note that this equation is equivalent to the above-mentioned equation lp = Vp · δ
(Delta) (Tp).

Therefore, the moving distance l (t) between 0 ≦ t ≦ Tpmin and the speed dl (t) / dt are as shown in the following equation.

[0386]

(Equation 34)

Note that lmin (t) is a standard function of the acceleration / deceleration curve, and has a form, for example, as shown in [Equation 2].

The time sequence is expressed by the following equation, assuming that the moving distance within the time tsn is equal to the moving distance between Ts of the provisional value Vp ⁰ when t = n · Ts in the relational expression relating to the speed of [Equation 34]. Can be obtained as follows.

[0389]

(Equation 35)

Therefore, the set ｛θi according to the time sequence tsn
If the n-th data from n is sent to the servo system, it will rise on the CP curve with the designated action point with an acceleration period of Tpmin.

The value of θ at t = tsn is represented by θin (ts
n), this is not immediately sent to the servo system, but is subjected to the temporal scale conversion (t → α · t) after the above-described processing for determining the optimum value of Vp, and further, After resampling processing by Ts is performed, it is finally sent to the servo system.

It should be noted that the interpolation process for θin may be performed as necessary before receiving the resampling process by Ts.

The point to be noted in this series of processing is that
After calculating the set {θin} based on the temporary value Vp ⁰ ,
The point is that the final result of θin can be obtained only by performing an operation on the time axis by the sampling time modulation method.

The processing after the expression (35) will be specifically described. The error Δθ is calculated based on the convolution expression of the expression (11).
i (t) is calculated for each joint axis (or Δθi (t) is calculated by inputting a sequence of {θin} to the simulation motor according to the speed Vp ⁰ ).

[0395] [Delta] [theta] = [.DELTA..theta.1 .DELTA..theta.2 · · ·] Error action end time of ^T epsilon (t), i.e. CP control accuracy, for example, is defined as the upper equation [Expression 13] where it In the case where the value is reduced to 1 / α, the lower expression of Expression 13 may be used.

The processing is realized by temporal scale conversion. Specifically, an algorithm of the sampling modulation method is applied.

Then, after the optimum value of Vp is determined in accordance with the formula [16] (it may be considered by replacing Vp → Vp ⁰ ), the scaling operation of the time axis related to the joint angle sequence is also performed by the sampling time modulation method. Can be performed.

Thus, an acceleration / deceleration curve as shown in FIG. 34 is obtained.

Although the case where Tp = Tpmin has been described in this example, application to general Tp values is easy if the time scale conversion of the standard function of the acceleration / deceleration curve is considered, and the deceleration pattern As described above, it is only necessary to apply a symmetric operation based on temporal wrapping, and thus description thereof is omitted.

(E4) Generally, the user generally makes the robot work during the constant speed period of the acceleration / deceleration curve, and the acceleration period and the deceleration period are considered to be transitional periods.

For this reason, it is preferable to prepare a user interface that responds to a request to calculate the moving distance Tp · Vp / 2 during the acceleration period or the deceleration period and to grasp the moving distance during the constant speed period.

Therefore, it is assumed that the user can designate the moving distance l _Tp during the acceleration period or the deceleration period, and a method for automatically calculating the appropriate value of Vp according to the l _Tp and the shape of the CP curve will be described below. I do. At this time, the user is notified of the total travel time and Vp as necessary.

Incidentally, since no special restriction is imposed on the shape of the acceleration / deceleration curve according to the present invention, in order to simplify the explanation,
A trapezoidal wave-like acceleration / deceleration curve will be described as an example.

First, the meaning of the newly introduced parameter will be described. "N" means the memory capacity (word unit) for each joint axis, and the larger the N value, the smoother the CP curve can be drawn. .

The provisional value Vp ⁰ of Vp is determined so as to satisfy the following equation.

[0406]

[Equation 36]

In the trapezoidal wave-shaped acceleration / deceleration curve shown in FIG. 35, the moving distance l _Tp specified by the user corresponds to the moving distance in a period other than the constant speed period (this is referred to as “T □”). Since the area is equal to the area of an isosceles triangle formed between the hypotenuse indicating the acceleration pattern and the deceleration pattern and the time axis as shown by the broken line in the drawing, it can be expressed by the following equation.

[0408]

(37)

[0409] "Tcp" in Fig. 35 is the total travel time required for CP control.

As described above, the user can specify the accuracy εMAX of the CP control according to the work content, and obtain the appropriate value of Vp from this and the calculation using the simulation motor.

By the way, the moving amount l _Tp and the total moving time Tc
Since p is not generally an integral multiple of the sampling time Ts, a sampling time modulation method is also required here. Now, it is considered that Tp and Tcp are made to be integral multiples of Ts by slightly changing l _Tp and Vp.

That is, the corrected peak velocity is defined as Vp ', and l _Tp is defined as l _Tp ' as follows.

[0413]

(38)

In the above equation, “np” and “n □” are positive integers and are determined as shown in the following equation.

[0415]

[Equation 39]

Here, [x] is a so-called Gaussian symbol defined by an integer not exceeding x.

[0417] When Vp 'and _lTp ' are obtained from the equation (38), the following equation is obtained.

[0418]

(Equation 40)

Note that this equation holds even when n □ = 0, and
In this way Vp ', l _Tp' can be obtained acceleration and deceleration pattern in synchronization with the Ts be determined (see FIG. 36.).

Although l _Tp ′ is different from the l _Tp value specified by the user, the difference between the two is small and does not greatly deviate from the user's intention. You don't need to sharpen.

Since the trapezoidal wave acceleration / deceleration pattern has been obtained in synchronization with Ts, the method of obtaining the time sequence tsn will be described in three cases: acceleration period, constant speed period, and deceleration period. I do.

I) The acceleration period (0 ≦ t ≦ np · Ts).

Assuming that the speed in this period is Vac (t), since the slope of the acceleration pattern in FIG. 36 is constant, it is expressed by the following equation proportional to time t.

[0424]

[Equation 41]

Therefore, at a certain point in the acceleration pattern shown in FIG. 36, the movement amount corresponds to the area enclosed between the acceleration / deceleration curve and the time axis up to this point.
The line element length Δln is constant in this example (this is “Δl”
And ), Tsn is obtained as in the following equation.

[0426]

(Equation 42)

Ii) Constant speed period (np · Ts ≦ t ≦ (np
+ N □) · Ts) Vp ′ = constant, and tsn is determined from the relationship of the area with respect to the acceleration / deceleration curve as shown in the following equation.

[0428]

[Equation 43]

Iii) Deceleration period ((np + n □) · Ts
≦ t ≦ (2 · np + n □) · Ts) Assuming that the speed is Vdc (t) during this period, it can be expressed as the following equation.

[0430]

[Equation 44]

Thus, tsn can be obtained from the relationship between the areas related to the acceleration / deceleration curve in the same manner as described above. However, the fact that the acceleration / deceleration pattern is synchronized with Ts and the difference between the acceleration pattern and the deceleration pattern When attention is paid to the temporal symmetry of tsn, tsn can be easily obtained by using Expression 42.

[0432]

[Equation 45]

Thus, the time sequence {tsn} in the sampling time modulation method is obtained, and after resampling, the joint angle sequence can be obtained and sent to the servo system.

In the graph shown in FIG. 37, a tV diagram showing an acceleration / deceleration curve is arranged in the upper part, and the distance ln is shown in the lower part.
Is expressed in Δl units (vertical axis), and the time axis (horizontal axis) is expressed in units of Ts. It is assumed that N = 18 for simplification of the description.

A line OP ⁰ connecting the origin O and the point P ⁰ in the figure is a virtual line that indicates a locus when the action point of the robot is moved at the temporary speed value Vp ⁰ .

A curve OP connecting the origin O and the point P ⁰ indicates a trajectory when the action point of the robot is moved according to a trapezoidal wave-like acceleration / deceleration curve.

[0437] The white circles on the curve OP indicate the positions at time tsn calculated using the equations [42] to [45].

FIG. 38 is a plot of corrected positions (indicated by triangles) obtained by re-sampling the data indicated by white circles in FIG. 37 at t = n · Ts, that is, every Ts.

When the position data indicated by the triangles is passed to the servo system, a locus approximating the CP curve can be drawn. However, in order to reduce the degree of fluctuation, linear interpolation is used as described above.

The positions indicated by black circles in FIG. 38 indicate the positions after linear interpolation has been performed.

That is, the time point of tsn that comes earlier in time with respect to time t = n · Ts is tsn−, and the time point of tsn that comes later is
If the time point n is defined as tsn +, it is a position obtained by proportionally dividing the front and rear positions as shown in the following equation.

[0442]

[Equation 46]

By transmitting the position data thus obtained to the servo system, it is possible to draw a CP curve more accurately.

Next, a problem concerning the connection between a certain CP curve and another CP curve will be described.

[0445] So far, description has been made mainly on one CP curve. However, in an actual situation, the working end of the robot is often moved along a curve connecting a plurality of CP curve segments.

FIG. 39 shows an example in which one control curve is formed by connecting a plurality of arcs having different curvatures. Five arcs C1, C from the start point S to the end point E are shown.
2, C3, C4 and C5 are connected.

In the present invention, an acceleration / deceleration pattern is generated for each curve located from the start point S to the end point E,
Instead of performing CP control as a pattern in which these are linked, they are treated as one CP curve to generate one acceleration / deceleration pattern for this.

As a result, it is possible to reduce the impact at the connection portion of each CP curve and to smooth the operation.

Of course, the same method can be used for connecting a linear segment and an arc segment.

Finally, the relationship between the sampling time modulation method and the PTP operation will be additionally described.

As mentioned above, it has been mentioned that the various algorithms related to the CP control are not completely unrelated to those of the PTP operation. From the viewpoint of the control for moving, it is possible to find commonality in the algorithms.

In other words, both operations can be handled by a unified algorithm through the sampling time modulation method.

A procedure for realizing the PTP control according to the algorithm of the sampling time modulation method will be described below.

(1) First, the current position and the target position of the hand position of the robot are given to the control device. That is,
The current position is known because the state of the robot is constantly monitored, and the target position is specified by a teaching operation or the like.

(2) The coordinate information of the target position is set in the invert filter to determine the displacement of the joint angle (X axis,
The components of the Y axis and the Z axis are ΔΘx, Δ 、 y, and ΔΘz, and the rotation axis component of the tool tip is ΔΘr. ).

(3) The amount of movement (ΔΘx, ΔΘy, ΔΘz,
ΔΘr) is stored in N memory addresses (word units).

In other words, as shown in FIG. 40, a set of constant angular velocities (ωx ⁰ , ωy ⁰ , ωz ⁰ , ω)
r ⁰ ) to store the position data sequence in N memory addresses so that the working end of the robot is virtually moved (that is, 4 · N words are used).

Incidentally, since the N-division calculation can be performed in a short time, storing in a memory is not essential. However, this method is adopted in order to attach importance to commonality with CP control.

(4) The acceleration / deceleration pattern, Tp, etc. are determined in consideration of the movement amount of each axis.

It should be noted that this procedure and the sampling time modulation method are completely independent from each other, as described above, because the latter is an algorithm after the determination of the acceleration / deceleration pattern.

(5) Regarding the obtained acceleration / deceleration pattern, T
Since the p-value and the total movement time are not always integral multiples of Ts, the peak value of the angular velocity is slightly corrected to establish a synchronous relationship with Ts and correct the acceleration / deceleration pattern.

[0460] In this case, V → ω, L → Δθ
May be replaced as appropriate.

(6) Attention is paid to the position data of one of the four axes, for example, the X-axis component ΔΘx, and the following equation (42) to (Equation 4) are obtained by the sampling time modulation method.
3] A calculation equivalent to the equation is performed.

In other words, in each equation, Δl → Δθx (= ΔΘ
x / N), L → ΔΘx, etc.

(7) If resampling processing is performed at Ts based on tsn, a data string having a synchronous relationship with Ts can be finally obtained, and the same Tp can be obtained for all four axes.
And a PTP operation of simultaneous start and simultaneous stop can be realized.

Although the modulation based on the velocity pattern has been performed as described with reference to FIG. 28 as an application example of the sampling time modulation method so far, the modulation is performed based on the curvature of the CP curve, so that it is fast on a straight road and fast on a curve. When performing control such as slowing down, or performing cooperative work such as a dual-armed robot, avoid interference between arms by applying modulation based on the speed or distance of other robots or arms. it can.

In addition, various sensor outputs (temperature, pressure,
By applying modulation in accordance with the current value of the motor, etc., it is possible to realize control in consideration of the state of the robot.

[0468] Finally, according to the present embodiment, the CP
In both control and PTP control, the hand position is managed by the memory, so a safety zone is provided by setting an allowable range for the hand movement range,
It is possible to restrict the hand position from inadvertently deviating from the allowable range, thereby making it possible to ensure safety measures.

[0469]

As is apparent from the above description, according to the present invention, in the CP control, an acceleration / deceleration pattern in which the speed peak time is defined according to the length of the control curve is generated, and the CP curve is generated. Is divided into small intervals at predetermined time intervals to calculate a line element sequence, and the joint state as a solution to the inverse kinematics problem can be known based on the line element sequence and a position command on the control curve.

At this time, when analyzing the inverse kinematics problem according to the present invention, the operation elements used for solving the forward kinematics problem are provided in a negative feedback filter to reflect the robot structure, and the line element sequence and the curve The above position command is set in the filter, activated by the number of times corresponding to the joint axis, and after the filter converges, the inverse kinematics problem solution can be obtained from the input stage of the operation element.

Therefore, the joint axes can be handled equally regardless of the combination of the arms and the like constituting the robot system, and the algorithm for generating the acceleration / deceleration pattern is greatly reduced due to the structural difference of the robot. The inconvenience of having to make a significant change is eliminated.

[0472] The algorithm is theoretically clear, and there is no inconvenience that the quality of the control is easily influenced by the ability and experience of the designer, but there is no inconvenience. Since the calculation process can be performed by four arithmetic operations or trigonometric function calculations, the calculation time can be reduced and the processing load on the CPU can be reduced.

In the present invention, in order to move the action point with high accuracy along the designated control curve, the time axis is corrected at the time of sampling, so that the calculation processing required for the generation of the acceleration / deceleration curve and the speed control is performed. The processing can be speeded up by reducing the load, and the CP control algorithm and the PTP control algorithm can be unified by interposing the time axis correction.

The technical scope of the present invention is not construed as being narrowed by the above-described embodiments, and the present invention can of course be widely applied to machine tools, various drive mechanisms, and the like.

[Brief description of the drawings]

FIG. 1 is a block diagram illustrating a configuration example of an acceleration / deceleration pattern generation device according to the present invention.

FIG. 2 is a block diagram showing a configuration of an acceleration / deceleration pattern generation / line element sequence calculation unit.

FIG. 3 is a diagram illustrating an example of a CP curve.

FIG. 4 is a graph showing an acceleration / deceleration curve including a constant speed pattern.

FIG. 5 is a graph showing an acceleration / deceleration curve not including a constant speed pattern.

FIG. 6 is a flowchart illustrating processing relating to generation of an acceleration / deceleration curve.

FIG. 7A is a graph showing a relationship between a curve length L and a speed peak arrival time, and FIG. 7B is a graph showing a relationship between the curve length L and a movement time Tm.

FIG. 8 is a graph showing a change in an acceleration / deceleration curve according to a distance (0 ≦ L ≦ Lmin).

FIG. 9 is a graph showing a change in an acceleration / deceleration curve according to a distance (Lmin ≦ L ≦ Lmax).

FIG. 10 is a graph showing a change in an acceleration / deceleration curve according to a distance (Lmax ≦ L).

FIG. 11 is a graph showing the physical meaning of a basic parameter δ (Tp).

FIG. 12 is a diagram illustrating a setting example of a coordinate system for a scalar robot.

FIG. 13 is a block diagram illustrating a configuration example of an invert filter.

FIG. 14 is a schematic diagram illustrating a model of a two-axis arm system.

FIG. 15 is a diagram illustrating the movement of the robot arm corresponding to the invert filter processing in order from (a) to (c).

FIG. 16 is a diagram showing a motion of a robot for describing CP control relating to an arbitrary position on a work.

FIG. 17 is a diagram for explaining movement of a hand position of the vertical articulated robot.

FIG. 18 is a flowchart generally showing a method of generating an acceleration / deceleration curve and a method of solving an inverse kinematics problem.

FIG. 19 is a graph showing an example of a temporal change in the position and speed of the action point of the arm.

FIG. 20 is a graph showing temporal changes in joint angles θ1 and θ2.

FIG. 21 is a graph showing a conversion rule of Θi (s) relating to temporal scale conversion.

FIG. 22 is a graph for explaining temporal scale conversion related to θi (n · Ts).

FIG. 23A is a graph showing a temporal change of a stepped phase command, and FIG. 23B is a diagram showing an interval between a point Pn and a point Pn + 1 on a CP curve.

24A is a conceptual diagram showing the relationship between the division of a CP curve and a memory for storing coordinates, and FIG. 24B shows a flow of processing from a memory for storing joint angles to a simulation motor through a time axis correction process. FIG.

FIG. 25 is a graph showing that it is necessary to shift to deceleration in the middle of a constant speed period after acceleration in the acceleration / deceleration curve.

FIG. 26 is a graph for explaining the meaning of a corrected time sequence {tsn}.

FIG. 27 is a graph for explaining a trajectory of a deceleration pattern by a sampling time modulation method.

FIG. 28 is a block diagram conceptually illustrating a sampling time modulation method.

FIG. 29 is an explanatory diagram of linear interpolation.

FIG. 30 is a diagram illustrating an example of an acceleration / deceleration curve.

FIG. 31 is a diagram illustrating a command trajectory, an actual trajectory, an error, and the like regarding a CP curve.

FIG. 32 is a graph showing an example of a temporal change of an error | ε (t) |.

FIG. 33 is a diagram showing virtual control for moving an action point at a constant speed on a CP curve.

FIG. 34 is a graph showing an example of an acceleration / deceleration curve obtained by using a sampling time modulation method.

FIG. 35 is a graph showing a trapezoidal waveform acceleration / deceleration pattern.

FIG. 36 is a graph showing a trapezoidal acceleration / deceleration pattern corrected so that Tp or Tcp becomes an integral multiple of the sampling time.

FIG. 37 is a graph showing a trapezoidal acceleration / deceleration pattern and a locus of action points.

38 is a graph showing a trajectory corrected for the action point of FIG. 37 together with a trapezoidal acceleration / deceleration pattern.

FIG. 39 is a diagram for describing connection of a plurality of arcs having different curvatures.

FIG. 40 is a graph showing an angular velocity stabilized in the relationship between the PTP control and the sampling time modulation method.

[Explanation of symbols]

 a Control curve 1, 5, 9, 10, 11 Acceleration / deceleration pattern 12 Acceleration / deceleration pattern generation device 13 Command means 14 Speed peak temporary value determination means 15 Acceleration / deceleration pattern generation / line element sequence calculation means 16 Inverse kinematics problem analysis means 17 Optimal speed peak value determination means 19 Correction means 15a Curve length calculation means 15b Speed peak time point determination means 15c Pattern generation means 15d Line element sequence calculation means

Continuation of the front page (58) Field surveyed (Int. Cl. ⁷ , DB name) G05B 19/416 G05B 19/18 B25J 13/00

Claims (7)

(57) [Claims] 1. A command means for issuing a command relating to a start point and an end point of control of an action point and / or a control curve, and a provisional value of a speed peak value on a control curve is determined according to a signal from the command means. Speed peak temporary value determination means, and an acceleration / deceleration pattern generation / line which generates an acceleration / deceleration pattern based on the temporary value of the speed peak value and divides the control curve into a number of minute length segments to obtain a line element sequence. Sequence calculation means,
Is a filter structure of a negative feedback comprising a calculation element corresponding to the homogeneous transformation matrix used in the conversion between the coordinate system in solving the forward kinematics problem, the command for the action point
Inverse kinematics problem analysis means that derives the position of the joint axis corresponding to the position and based on the provisional value of the speed peak value
About joint state obtained by inverse kinematics problem analysis means
Software solution to imitate real servo system
This provision or the optimal speed peak value determining means for determining an optimum value of the speed peak value to match the specified precision, optimal value of the speed peak value with determining the control accuracy by giving the virtual servo system comprising Te An acceleration / deceleration pattern generation apparatus, comprising: a correction unit that performs recalculation or correction calculation on the joint state based on the calculation. 2. An acceleration / deceleration pattern generating apparatus according to claim 1, wherein said inverse kinematics problem analysis means is used to solve a forward kinematics problem for obtaining a position of an action point from a joint state. , A multiplier element for dynamic gain adjustment, an integral element for loop stabilization, and a negative feedback filter configuration including a constant element for adjusting the loop convergence speed. An acceleration / deceleration pattern generation apparatus characterized in that a solution of a joint state can be obtained from a preceding stage of a calculation element in response to input of position information to inverse kinematics problem analysis means. 3. The acceleration / deceleration pattern generation device according to claim 1, wherein the acceleration / deceleration pattern generation / linear element sequence calculation means calculates a curve length of a control curve, and the acceleration / deceleration. A speed peak time point determining means for determining a speed peak time point in the curve according to the curve length, and generating an acceleration pattern related to the joint axis in response to a command relating to the speed peak time point; A pattern generation means for generating a deceleration pattern as a temporally folded pattern by performing a temporally symmetric operation on the other hand, and a line by dividing a control curve into predetermined time intervals according to the shape of the acceleration / deceleration curve. An acceleration / deceleration pattern generation device, comprising: a line element sequence calculating means for calculating an element sequence. 4. An acceleration / deceleration pattern generation method for generating an acceleration / deceleration pattern to control the movement of an action point along a specified control curve, wherein (a) first, a minimum unit of calculation time related to the control curve is determined. After the determination, (b) the curve length of the control curve is determined, the peak time point of the speed corresponding to the curve length is determined, and an acceleration / deceleration pattern having a temporal symmetry between the acceleration pattern and the deceleration pattern is generated. At the same time, the provisional value of the speed peak value is determined based on the allowable value of the memory capacity, and the control curve is divided at unit time intervals of the calculation time to derive a line element sequence. Negative feedback including an operation element corresponding to a homogeneous transformation matrix used for transformation between coordinate systems when solving a forward kinematics problem, based on information related to the sequence command and the position command of the action point. perform a filter calculation To the door
To give the position of the action point and the state of the joint axis corresponding to this
Computes the solution of the inverse kinematics problem of finding, the control accuracy by giving the virtual servo system configured to simulate a real servo system through software processing solutions for joint state obtained in (d) (iii) Determine the optimum value of the speed peak value so that it matches the specified or specified accuracy. (E) Based on the optimum value of the speed peak value, (b) and (c) A method for generating an acceleration / deceleration pattern, wherein a result of calculation or a correction calculation corresponding thereto is sent as a control command to a servo system. 5. A method for solving an inverse kinematics problem in which a state of a joint axis corresponding to a position of an action point scheduled to move along a specified control curve is solved. The operation element corresponding to the homogeneous transformation matrix used in solving the forward kinematics problem to find the position of the corresponding action point, the multiplier element for dynamic gain adjustment, and the integral element for loop stabilization , A negative feedback filter including a constant element for adjusting the convergence speed of the loop, and (b) dividing the control curve with the minimum calculation time interval to obtain the line element sequence and the position information of the action point. (A) is given as input information to the filter, the filter is activated, and after waiting for convergence in the loop, the state of the joint axis is obtained from the previous stage of the operation element. (C) (a), (b)
A procedure for solving the inverse kinematics problem, wherein the joint state as a solution of the final inverse kinematics problem is obtained by repeating the above procedure for each joint axis. 6. An acceleration / deceleration pattern generation method according to claim 4, wherein the solution according to claim 5 is used as a solution to the inverse kinematics problem in (c). . 7. A method for generating an acceleration / deceleration pattern according to claim 4.
For a control system that is used in the calculation method and is operated with the minimum unit of calculation time, the movement control is performed so that the locus of the action point draws a curve that approximates the specified control curve within a predetermined accuracy.
A time base correction method for your, (b) acceleration peak time and the total travel time of the velocity in the curve, the speed peak value to be an integral multiple of the minimum unit of calculation time and acceleration and deceleration After correcting the movement distance in the period and sampling the data sequence indicating the joint state with the minimum unit of calculation time, the data is held at the minimum unit time interval, and (b) the control curve is calculated as the minimum unit of calculation time. Calculates a new time sequence corresponding to the data sequence indicating the joint state by calculation based on the relationship between the length of the line element sequence obtained by dividing at the time interval and the shape of the acceleration / deceleration curve (C) Generate a new time sequence and data sequence indicating the joint state obtained in (b) and (b) without being bound by the minimum unit of time, and perform resampling using the minimum unit of calculation time By time And to obtain information arranged joint state interval, time base correction method characterized by.