JP3480215B2 - Positioning control device - Google Patents

Description

Detailed Description of the Invention

[0001]

BACKGROUND OF THE INVENTION 1. Field of the Invention The present invention relates to a positioning control device for controlling the moving position of a moving body.

[0002]

2. Description of the Related Art The moving distance of a moving body can be calculated by multiplying a moving time and a moving speed pattern, which is generated by a positioning control device, and which indicates the moving speed of the moving body. Then, based on the calculation result, the positioning control device controls the position where the moving body stops. At this time, the positioning control device does not suddenly start moving the moving body at a predetermined speed. First, the positioning control device sequentially increases the speed and accelerates until the speed of the moving body reaches a predetermined constant speed. Then, the positioning control device moves the moving body at a predetermined constant speed set by the operator, and decelerates and stops it in the vicinity of the target stop position in the same manner as during acceleration.

That is, the positioning control device generates a trapezoidal velocity pattern for moving the moving body. However, when the speed pattern output by the positioning control device is trapezoidal, when the moving body is started and stopped,
Due to the inertia of the moving body, the moving body is shocked, and an error that does not work for the moving distance set by the operator occurs. Therefore, in order to control the start and stop with high accuracy, the positioning control device uses an S-shaped or other stage acceleration change as shown in FIG. 4 as a gentle acceleration pattern so as to minimize the shock due to inertia. Output a speed pattern with.

However, since the velocity pattern is generated at a constant cycle and the moving distance is calculated, the control becomes discrete. Therefore, the deceleration point for decelerating the moving body with respect to the target stop position is obtained in the calculation cycle for generating the speed pattern. However, the deceleration point at time tm shown in FIG. 4 obtained in this calculation cycle deviates from the deceleration point at time ts shown in FIG. 4 obtained from the acceleration / deceleration time and the speed pattern designated in advance. That is, the time t at the calculation cycle for the moving distance
A quantization error occurs due to the moving distance between s and time tm. Therefore, there is a problem that the stop position deviates from the designated value.

Therefore, there is a method in which a speed pattern is generated from the given predetermined speed and the given acceleration / deceleration time, and the moving position is sequentially calculated. The remaining distance to the designated stop position is obtained from the moving distance obtained each time the speed pattern is generated. Then, after the deceleration point at which deceleration is started is detected from the remaining distance to this stop position, the moving distance required to stop from this deceleration point is obtained. Then, in order to correct the quantization error generated in the calculation cycle, the positioning control device finely adjusts the speed pattern obtained in each calculation cycle according to the remaining distance to the stop position in each calculation cycle. It uses a method of moving while moving and stopping at a specified stop position.

[0006]

However, in the above positioning control device, calculation of the speed pattern during deceleration of the moving body becomes complicated, and addition and subtraction with respect to the speed pattern are performed even when the final speed pattern is generated. Therefore, there is a drawback that the quantization error in the position control cannot be completely corrected. The present invention has been made under such a background, and provides a positioning control device capable of completely correcting a quantization error by calculating a deceleration point of a stopping operation of a moving body in position control and generating a smooth velocity pattern. The purpose is to do.

[0007]

The invention according to claim 1 is
An acceleration region that accelerates the moving speed of the moving body to a predetermined value,
The moving speed of the moving object is decelerated from a predetermined value, in the deceleration range to stop at the specified stop position, the positioning control apparatus performs speed regulation of the moving body, and controls the moving position of the moving body in a predetermined cycle Yes, first calculating means for calculating speed data indicating the moving speed of the moving body in the acceleration region, and a moving distance indicating the moving distance of the moving body from the speed data obtained by the first calculating means. A second calculation means for calculating data, a storage means for storing the movement distance data obtained by the second calculation means, and a deceleration area.
There are, and the remaining travel distance data obtained by subtracting the position data of the moving object starting point of the stop position data or al before Symbol deceleration region,
The difference from the moving distance data stored in the storage means
As an error, a correction means for correcting the movement amount for every fixed period is provided.

According to a second aspect of the present invention, in the positioning control apparatus according to the first aspect, the first calculating means calculates the speed data for each cycle obtained by dividing a predetermined period by a constant interval. Characterize. The invention according to claim 3 is
The positioning control device according to claim 2, wherein the speed data, which is a moving speed of the moving body, is calculated by the first calculating means based on a specified time and a specified distance until the speed data reaches a specified constant speed. , Is calculated for each cycle and is accelerated. According to a fourth aspect of the present invention, in the positioning control device according to any of the first to third aspects, the second calculating means integrates the movement distance of the moving body during the period in which the moving body is being accelerated. It is characterized in that it is calculated. The invention according to claim 5 is
The positioning control device according to any one of claims 1 to 4, further comprising: a relational table of movement distance data and a correction coefficient for correcting the movement distance, which is used for correction obtained empirically, and is based on the relational table. The correction means corrects the movement distance data stored in the storage device.

[0009]

BEST MODE FOR CARRYING OUT THE INVENTION Embodiments of the present invention will be described below with reference to the drawings. FIG. 1 is a block diagram showing the configuration of a positioning control device according to an embodiment of the present invention.
In this figure, 1 is an MPU (Micro Process)
ssor Unit). Reference numeral 4 denotes a save table storage unit that saves the position data of the moving body at every calculation cycle of the position data when the moving body is accelerated.

Reference numeral 5 denotes a position command calculation unit, which performs a speed calculation including a correction value from the position data saved in the save table storage unit 4 at the time of deceleration, every time the position data is calculated. Reference numeral 6 denotes a quantization error calculation unit, which corrects a quantization error generated when the deceleration point is obtained for each calculation of position data by using a predetermined graph shown in FIG. FIG. 3 shows the correction values of the moving distance data, details of which will be described later. Reference numeral 7 denotes a motor control device, which controls the start and stop of the motor 8 and the number of revolutions of the motor 8 by a signal from the MPU 1.

Next, the operation of the above embodiment will be described with reference to FIGS. 1, 2 and 3 and according to the timing chart of FIG. FIG. 2 is a diagram showing the configuration of the save table storage unit. Further, FIG. 3 is a diagram showing a relationship between the correction coefficient A of the quantization error generated when the deceleration point is obtained and the normalized number of the error of the moving distance. In this figure, the horizontal axis is the normalized movement distance error caused by the quantization error. First, the operator sets a certain constant speed data,
The acceleration / deceleration time and the movement distance are input to the positioning control device. In the acceleration period from time t0 to time tk, the MPU 1 sequentially calculates speed pattern calculation at each time t1 to tk so that smooth acceleration is performed based on the input information.

Then, the MPU 1 sends a control signal to the motor control device 7 in order to control the motor 8 so that the speed has the calculated speed pattern. Here, the integrated value P of the moving distance data is calculated by the speed pattern and the moving time.
[k] is calculated as P [k] = P [k-1] + T * (Vk-Vk-1) / 2 (1). Here, Vk (k is a natural number) is speed data at time tk calculated as a speed pattern, Vk-1 is speed data at time t-1, and T is time between each time. Width.

First, the MPU 1 sets the initial value P [0] of the integrated value of the moving distance data to the area 2 of the save table storage unit 4.
Store to 0. This is '0'. And MP
U1 is the movement amount data P [1] from time t0 to t1 and time t0 based on the obtained speed pattern according to the equation (1).
Movement amount data P [2] from time t0 to time t2, and time t0 to time t
The integrated values of the movement amount data up to 3, tk-2, tk-1, and tk are calculated, and the areas 21, 22, 23, 2 are respectively calculated.
4, 25 and 26 respectively.

At time tk, the speed of the moving body becomes the value Vk of the speed data designated by the operator. Next, the MPU 1 accumulates the moving amount of the moving body at the speed Vk at each calculation time, and the time when the condition of θR-Rc ≤ P [k] (2) is established is the deceleration point. To do. here,
θR is the target value of the moving distance, and Rc is the current position of the moving body. Since the deceleration is performed after the deceleration point is detected, the speed pattern at each time is not obtained from the deceleration period and the stop time tm + k at each periodic time as in the acceleration period. .

That is, the movement amount at each time is calculated based on the integrated value of the movement amount at each time saved in the save table storage unit 4. That is, from time tm
The movement amount Dm + 1 up to tm + 1 can be obtained by Dm + 1 = P [k] -P [k-1] (3). However, when the deceleration point is determined at the time tm, the time t0 to
It is unlikely that the integrated value of the movement amount between tk and the time tm to tm + k in the deceleration region will be the same distance. That is, the moving distance from time ts to time tm shown in FIG. 4 becomes the quantization error in the deceleration region.

In the above case, the integrated value of the moving amount in the deceleration region is smaller than the integrated value of the moving amount in the acceleration region, and the value of the reduced moving amount is the error Δθ = Rc of the moving amount due to the quantization error. + P [k] -θR (4) This quantization error appears most noticeably at time tm + k, and if an attempt is made to correct it between times tm and tm + 1, the speed change abruptly, so that the moving speed of the moving body is smoothly decelerated. Is not done. Therefore, the error Δθ of the movement amount is corrected in the deceleration region between time tm and tm + k-1.

The correction of each movement value in the deceleration region is carried out by the quantization error calculation unit 6 using the correction coefficient A generated from the empirical value corresponding to the point where each position in FIG. [k] = P [k] -A [k] x Δθ (5) P '[k-1] = P [k-1] -A [k-1] x Δθ・ It is required to be (6).

The amount of movement from time tm to tm + 1 is calculated by the position command calculator 5 from the equations (5) and (6) as D'm + 1 = P '[k] -P' [k-1. ] = P [k] -P [k-1]-(A [k] -A [k-1]) x Δθ (7) Recalculate the movement distance with the correction value at each time. .

Next, the position command calculator 5 outputs to the motor control device 7 operation data for moving the moving body to the motor 8 at the time tm to tm + 1 by the corrected moving distance D'm + 1. . Then, the motor control device 7 drives the motor 8 to move the moving body in the time width T by the moving distance D′ m + 1.

By performing the above operation at each time, MP
U1 obtains necessary moving distance data at each time until the moving body stops based on the moving amount error Δθ and the correction coefficient A. Correction of moving distance data is performed at time tm + k-1
End by. That is, the correction coefficient A at P [0] is "0". Then, at time tm + k, the moving distance of the moving body becomes "0", the speed becomes "0", and the moving body stops.

[0021]

According to the invention of claim 1, the acceleration region for accelerating the moving speed of the moving body to a predetermined value, and the moving speed of the moving body is decelerated from the predetermined value and stopped at the designated stop position. Positioning control device that controls the moving position of the moving body by adjusting the speed of the moving body at regular intervals with the deceleration region
And a first calculation means for calculating speed data indicating a moving speed of the mobile body in the acceleration region, and a movement indicating a distance traveled by the mobile body from the speed data obtained by the first calculation means. a second calculating means for calculating the distance data, storage means for storing the travel distance data obtained by the second calculating means, in the deceleration region, the movement of the start point of the stop position data or et previous SL deceleration region Remaining movement distance data obtained by subtracting body position data, and the storage means
The difference from the moving distance data stored in
Since the stop position of the moving body obtained by the calculation is calculated without any error because the moving body is provided with the correcting means for correcting the moving amount for each fixed period , the moving body can be smoothly stopped. .

According to the second aspect of the present invention, the first calculation means calculates the speed data for each cycle obtained by dividing a predetermined period at a constant interval, so that a multi-step speed pattern is created. The effect that smooth acceleration and deceleration of the moving body can be obtained. According to the invention of claim 3, until the speed data, which is a moving speed of the moving body, reaches a specified constant speed, the first calculation means is operated based on a specified time and a specified distance. Since it is calculated and accelerated in each of the above-mentioned cycles, it is possible to obtain the effect of smoothly accelerating and decelerating the moving body. According to the invention of claim 4, the second calculating means calculates by integrating the moving distance of the accelerating period of the moving body for each cycle, so that the correction is made in the same cycle in the deceleration section. By subtracting the moving distance, the smooth deceleration operation can be achieved. According to the invention of claim 5, there is provided a relational table of the movement distance data used for the correction obtained empirically and the correction coefficient for correcting the movement distance, and the correction means is based on the relational table, In order to correct the moving distance data stored in, the predetermined distance allocated for each calculation cycle is corrected, so that the moving body can be smoothly stopped at the designated stop position.

[Brief description of drawings]

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

FIG. 2 is a diagram showing a save table storage unit in a storage device of a constituent block according to an embodiment of the present invention.

FIG. 3 is a diagram showing a relationship between a correction coefficient of moving distance data used in one embodiment of the present invention and a normalized number of moving distance errors.

FIG. 4 is a timing chart showing acceleration / deceleration according to the embodiment of the present invention.

[Explanation of symbols]

1 MPU 4 Evacuation table storage 5 Position command calculator 6 Quantization error calculator 7 Motor control device 8 motor 20, 21, 22, 23, 24 area 25, 26 area

Front page continuation (58) Fields surveyed (Int.Cl. ⁷ , DB name) G05D 3/00-3/20 G05B 19/18 G05B 19/407 B25J 3/00-3/04

Claims (5)

(57) [Claims] In a method according to claim 1] acceleration region for accelerating the moving speed of the moving body to a predetermined value, the moving speed of the moving object is decelerated from a predetermined value, a deceleration region to stop at the specified stop position, one
A positioning control device that controls the moving position of the moving body by adjusting the speed of the moving body in each fixed cycle , and first calculating means for calculating speed data indicating the moving speed of the moving body in the acceleration region. A second calculating means for calculating moving distance data indicating the distance traveled by the moving body from the speed data obtained by the first calculating means; and moving distance data obtained by the second calculating means are stored. storage means, in the deceleration region, and the remaining travel distance data obtained by subtracting the position data of the moving object starting point of the stop position data or al before Symbol deceleration region, the moving distance data stored in the storage means
A positioning control device comprising: a correction unit that corrects a movement amount for each constant period using a difference from the data as an error . 2. The positioning control device according to claim 1, wherein the first calculation means calculates the speed data for each cycle in which a predetermined period is divided at constant intervals. 3. The first calculation means, based on a specified time and a specified distance, for each cycle until the speed data, which is the moving speed of the moving body, reaches a specified constant speed. The positioning control device according to claim 2, wherein the positioning control device is calculated and accelerated. 4. The method according to claim 1, wherein the second calculating unit calculates by integrating the moving distance of the moving body during the acceleration period for each cycle. Positioning control device. 5. A relational table of movement distance data and a correction coefficient for correcting the movement distance, which is used for empirically obtained correction, is provided, and the correction means is stored in the storage device based on the relational table. The positioning control device according to any one of claims 1 to 4, wherein the movement distance data is corrected.