JPH09244725A - Method and device for track interpolation, and controller - Google Patents

Abstract

PROBLEM TO BE SOLVED: To smoothly and precisely reproduce the target track of a moving object and controlling the position of the moving object according to the target track as to the device and method for track interpolation, and controller. SOLUTION: The target track of the moving object 7 can smoothly and precisely be reproduced by providing a target track generating means 3 which calculates target values of a position x1 and a speed v1 after one cycle T of the moving target 7 from initial information on a position x0 and a speed v0 and a target track interpolating means 5 which interpolates the target track of the moving object 7 in one cycle T by approximating it with a polynomial of at least third order or more according to the initial information and target values.

Description

Detailed Description of the Invention

[0001]

BACKGROUND OF THE INVENTION 1. Field of the Invention The present invention relates to a trajectory interpolating apparatus, a method therefor, and a control apparatus, which are suitable for use in position control of an industrial robot arm, for example.

[0002]

2. Description of the Related Art Conventionally, in a servo control device for an industrial robot arm (hereinafter referred to as a manipulator), a control CPU (C
Due to lack of computing power of the entral processing unit), the target trajectory of the manipulator is generated at a cycle of several times the control sampling frequency, and it is interpolated for each sampling. Recently, it has become more common to introduce feedforward control of speed and acceleration in order to shorten the positioning of the manipulator. In this case, it is necessary to generate a target trajectory such that the velocity and acceleration are smooth.

[0003]

By the way, as shown in FIG. 3, when the cycle T for generating the target trajectory by the function generator is longer than the cycle ΔT of the position control by the position controller, generally as shown in FIG. In addition, the function generator generates only the target value of the position, and the function interpolator provided immediately before the position controller interpolates the target value of the position by a linear function, that is, a straight line. However, when the position control is actually performed by this method, the manipulator is position-controlled by the trajectory on the polygonal line at every cycle T, and therefore the movement becomes not smooth. Therefore, there is a problem that the deviation from the originally intended target trajectory becomes large and the control accuracy deteriorates.

Further, for example, the one disclosed in Japanese Examined Patent Publication No. 6-95294 carries out interpolation using not only the present target position but also the interpolated target positions of the past two times of the position controller. However, in this method, the trajectory processing unit generates only the target position, so that even if the interpolated trajectory is smooth, a deviation from the original target trajectory is likely to occur at a time outside the cycle T of the function generator. There is.

Further, for example, in the one disclosed in Japanese Examined Patent Publication No. 202202/1990, all target values of the number of position control cycles are calculated in one cycle of the function generator and transferred to the position controller at one time. , A method of storing in buffer is proposed. However, this method has a problem in that the amount of calculation of the function generator is required to be several times as large as that in the conventional method, and the amount of data transfer increases.

[0006] For example, has been disclosed in Japanese Unexamined Patent Publication No. 5-53623, when controlling the Manipiyureta, it generates only by connexion target angle to the trajectory processing unit at a period T _1, a period shorter than the cycle T ₁ T ₂ Then, the trajectory interpolation unit interpolates the target angle, the target angular velocity, and the target angular acceleration to generate the target angle. Further period T ₂
The target angle, the target angular velocity, and the target angular acceleration are separately interpolated by a linear function in the servo processing unit in a cycle of a shorter cycle T ₃ . However, in this method, only the target angle is generated in the trajectory processing unit, and therefore there is a problem that even if the trajectory interpolated by the trajectory interpolation unit is a smooth trajectory, a deviation from the original target trajectory is likely to occur. Also, since the target angle, target angular velocity, and target angular acceleration are separately interpolated by a straight line in the servo processing unit, the relationship between angle, angular velocity, and angular acceleration (corresponding to position, velocity, and acceleration) becomes inaccurate, and the feedback When forward control or the like is performed, there is a problem that the accuracy of servo control is reduced.

The present invention has been made in consideration of the above points, and it is an object of the present invention to propose a trajectory interpolating device, a method thereof, and a control device capable of accurately reproducing a target trajectory and achieving smooth position control at high speed. It is a thing.

[0008]

In order to solve such a problem, in the present invention, a target trajectory generating means for calculating a target value relating to the position and speed of a moving object after one cycle is calculated based on initial information of the position and speed. , The target trajectory of the movement target within a cycle is provided with a target trajectory interpolation means for approximating and interpolating the target trajectory of the movement target based on the initial information and the target value by a polynomial of at least a third degree or more. Can be smoothly and accurately reproduced.

Further, in the above-described embodiment, the target trajectory generating means for calculating the first target value relating to the position and the velocity of the moving object after one cycle based on the initial information of the position and the velocity, and the movement within the one period. Based on the initial information and the first target value, the target trajectory of the target is approximated and interpolated by at least a third-order or higher polynomial, and the second target value relating to the position and velocity based on the polynomial is set to the above-described cycle. A target trajectory interpolating means which is generated in a cycle divided into shorter parts,
By including the position control means for controlling the movement of the moving object based on the target value of, the moving object can be controlled accurately and at high speed along the smooth target trajectory.

[0010]

DETAILED DESCRIPTION OF THE PREFERRED EMBODIMENTS An embodiment of the present invention will be described below in detail with reference to the drawings.

(1) First Embodiment In FIG. 1, reference numeral 1 generally indicates a control device according to a first embodiment of the present invention, in which a target trajectory generating section 3 in a function generator 2 uses calculation software. Then, the target position and the target speed of the manipulator 7 at the time after the time T is calculated for each cycle T, and the calculation result is sent to the position controller 4 at the cycle T. On the other hand, the position controller 4 recalculates the interpolated position, the interpolated speed, and the interpolated acceleration in the cycle ΔT shorter than the cycle T based on the calculation results of the target position and the target speed in the function interpolator 5. The servo processing unit 6 servo-controls the manipulator 7 based on the interpolation target trajectory obtained from this calculation result.

In the actual interpolation of the target trajectory, first, at time t ₀ , the function interpolator 2 obtains the position x ₀ and velocity v ₀ of the manipulator 7 at the time t ₀ from the servo processing unit 6, and then the target trajectory generation unit 3 Is the target position x of the manipulator 7 at time t ₁ after the time T by the calculation software.
₁ and the target velocity v ₁ are generated and sent to the function interpolator 5. At this time, the target position x (t), the target speed v (t), and the target acceleration a at the time t within the time t ₀ to the time t ₁ .
(T) is the following polynomial

[Equation 1]

[Equation 2]

(Equation 3) Can be used for interpolation.

In FIG. 2, when the target trajectory is generated by S-shaped acceleration / deceleration every 10 [msec], the above method is used.
An example is shown in which interpolation is performed by dividing into 20 at 0.5 [msec] intervals.
From this figure, according to this interpolation method, the target position x ₁ and the target speed v ₁ are calculated in the function generator 2 for each cycle T, so that the target track intended by the target track generation unit 3 for the position and the speed is calculated. It can be faithfully reproduced.

According to the above construction, the function generator 2 only generates the target position x ₁ and the target speed v ₁ for each cycle T based on the position x ₀ and the speed v ₀ of the manipulator 7 at the time t ₀ . It is possible to faithfully reproduce the intended target trajectory in the target trajectory generation unit 3 with respect to the position and the speed with a small amount of calculation. Further, the function interpolator 5 can easily interpolate a smooth target trajectory on the basis of the cubic function (Equation 1), thereby suppressing the occurrence of vibration during the control of the manipulator 7. Thus, the servo processing unit 6 can control the manipulator 7 accurately and at high speed by using the target trajectory based on the smooth cubic function.

(2) Second Embodiment As a second embodiment, the function interpolator 2 in the control device 1 is used.
However, the case where the position x ₀ , the speed v _0, and the acceleration a ₀ of the manipulator 7 at the time t ₀ are obtained from the servo processing unit 6 will be described. In this case, the target trajectory generation unit 3 uses the calculation software to calculate the target position x ₁ and the target speed v ₁ at the time t ₁ after the time T ₀ from the time t ₀ to the position x ₀ and the speed v _0.
And the acceleration a ₀ are calculated and sent to the function interpolator 5. At this time, the target position x (t), the target speed v (t), and the target acceleration a at the time t within the time t ₀ to the time t ₁ .
(T) is the following polynomial

(Equation 4)

(Equation 5)

(Equation 6) Can be used for interpolation.

In this case, with respect to the position and speed of the manipulator 7, the intended target trajectory in the target trajectory generator 3 can be faithfully reproduced in terms of position and speed, as in the first embodiment. Furthermore, in this case, position x ₀ and velocity v ₀
In addition to the above, by calculating the target trajectory in the function generator 2 using the data of the acceleration a _0, the continuity of the acceleration after interpolation can be maintained. Further, by interpolating based on a quintic function in the function interpolator 5, it is possible to smoothly reproduce the target trajectory and suppress the occurrence of vibration during control of the manipulator 7. Thus, the servo processing unit 6 can control the manipulator 7 accurately and at high speed by using the target trajectory smoothly reproduced based on the quintic function (Equation 4).

(3) Other Embodiments In the above-mentioned embodiments, the case where the target trajectory of the manipulator 7 is interpolated has been described, but the present invention is not limited to this, and is widely and generally used for moving a flying object, for example. It can be applied in predicting a target trajectory for a moving trajectory.

[0018]

As described above, according to the present invention, target trajectory generating means for calculating a target value relating to the position and speed of a moving object after one cycle based on initial position and speed information,
The target trajectory of the moving object is provided by including a target trajectory interpolating unit that approximates the target trajectory of the moving object within one cycle based on the initial information and the target value by at least a cubic polynomial or more. It is possible to realize a trajectory interpolation device and a method thereof that can be smoothly and accurately reproduced.

Further, in the above-described embodiment, the target trajectory generating means for calculating the first target value relating to the position and the velocity of the moving object after one cycle based on the initial information of the position and the velocity, and the movement within the one period. Based on the initial information and the first target value, the target trajectory of the target is approximated and interpolated by at least a third-order or higher polynomial, and the second target value relating to the position and velocity based on the polynomial is set to the above-described cycle. A target trajectory interpolating means which is generated in a cycle divided into shorter parts,
By providing the position control means for controlling the movement of the moving object based on the target value of, it is possible to realize a control device capable of controlling the moving object along a smooth target trajectory with high accuracy and at high speed.

[Brief description of drawings]

FIG. 1 is a block diagram showing an overall configuration of a control device according to the present invention.

FIG. 2 is a diagram for explaining interpolation of a target trajectory according to the present invention.

FIG. 3 is a diagram for explaining function generation and position control.

FIG. 4 is a diagram for explaining conventional interpolation of a target trajectory.

[Explanation of symbols]

1 ... Control device, 2 ... Function generator, 3 ... Target trajectory generation unit, 4 ... Position controller, 5 ... Function interpolator, 6 ... Servo processing unit, 7 ... Manipulator.

Claims (4)

[Claims] 1. A target trajectory generating means for calculating a target value relating to a position and a velocity of a moving object after one cycle based on the initial information of the position and velocity, and a target trajectory of the moving object within the one period. A trajectory interpolating device, comprising: a target trajectory interpolating means for performing an approximation by at least a cubic polynomial based on the initial information and the target value. 2. A target trajectory generating means for calculating a target value relating to the position, velocity and acceleration of the moving object after one cycle based on the initial information of the position, velocity and acceleration, and the moving object of the moving object within the one cycle. A trajectory interpolating device, comprising: a target trajectory interpolating means for approximating the target trajectory based on the initial information and the target value by a polynomial of at least a third degree or more. 3. A target value relating to the position and speed of the moving object after one cycle is calculated based on the initial information of the position and speed, and a target trajectory of the moving object within the one cycle is defined as the initial information and the target. An orbital interpolation method characterized by performing approximation by at least a cubic polynomial based on a value and performing interpolation. 4. A target trajectory generating means for calculating a first target value relating to the position and speed of the moving object after one cycle based on the initial information of the position and speed, and a target of the moving object within the one cycle. At least 3 trajectories based on the initial information and the first target value
Target trajectory interpolating means for approximating and interpolating by a polynomial of the following or higher, and generating a second target value relating to position and velocity based on the polynomial at each cycle of intervals shorter than the above cycle, A control device comprising position control means for position-controlling the movement target based on a second target value.