JP2000187509A - Mechanism controller - Google Patents

Abstract

PROBLEM TO BE SOLVED: To smoothly move a moving member even in a path whose path length is not analytically obtained and to accurately stop the moving member at an end point. SOLUTION: Together with a forward tracing means (and a back tracing means) which respectively calculate speeds on time base (and a reverse direction) according to speed curves 4 (and 5) led from a function in which the speed changes of front (and rear) side speed changing parts are respectively defined from the start point 2 (and the end point 3) of a path at the same time to sequentially generate points P on the path (and stores them in a buffer, this controller traces the points generated on the path by the means to make a trace, also continues forward trace processing on the rear side speed changing part when an estimated speech change point is exceeded before a point on the path crosses a point on the path generated by the back tracing means, calculates deviation on the path from a true speed change point 9 at the point of time when speeds obtained by both means become equal and generates a trace by tracing the points on the path stored in the buffer while correcting them.

Description

DETAILED DESCRIPTION OF THE INVENTION

[0001]

The present invention relates to a mechanism control device. More specifically, the present invention relates to an improvement of a mechanism control device that moves a moving member along a predetermined path while changing a speed.

[0002]

2. Description of the Related Art For example, in a robot apparatus for causing a moving member such as a hand to perform a desired movement, the speed of the moving member must be increased by an accelerating unit, a constant speed unit, and the like. It is common to perform the operation while changing the speed separately for the deceleration section. As means for controlling the speed or acceleration of the moving member at this time, those which analytically determine the deceleration point and those which use a filter are mainly used.

First, the means for analytically finding the deceleration point is as follows:
For example, when there is a speed curve 101 indicating a speed change as shown in FIG. 22A, a necessary deceleration distance is obtained from the speed curve 101. Specifically, since the area of the hatched portion surrounded by the deceleration portion of the speed curve 101 and the time axis corresponds to the deceleration distance 102, FIG.
As shown in (B), the moving member is moved from the starting point 103 to the ending point 10.
4, the deceleration distance 10 from the end point 104
The deceleration point 105 is set at a distance of 2 and the deceleration is started when the moving member approaches the deceleration point 105, thereby stopping at the end point 104.

[0004] On the other hand, in the method using a filter, first, data 106 is generated as shown in FIG. 23 (A) when moving on a route from the start point to the end point at a constant speed. The speed data 106 in this case has a rectangular shape, and the distance from the start point to the end point on the predetermined route is represented by the area. Next, the data 106 is filtered, and the acceleration unit 107, the constant velocity unit 108, and the deceleration unit 1 shown in FIG.
A speed curve 110 having the value 09 is generated. As the filtering at this time, for example, a moving average of several points on a time-speed graph is obtained, and a speed at each time is obtained from the moving average to generate a speed curve 110. As shown in (B), the speed curve 110 can be obtained without changing the area of the graph. In this case, the boundary between the constant velocity unit 108 and the deceleration unit 109 is a deceleration point.

[0005]

However, FIG.
The means for analytically finding the deceleration point 105 cannot accurately stop the moving member at the end point 104 unless the deceleration point 105 is determined accurately as shown in FIG. Need to be kept. For this reason, this means has a problem that only a mathematically simple path such as a straight line or an arc or a combination thereof can be handled.

In the method using a filter, the acceleration or deceleration curve shape is basically determined by the filter characteristics. Therefore, acceleration or deceleration control of a trigonometric function system useful for smoothly moving a moving member is very difficult. Difficult. Further, when the shape of the acceleration unit 107 and the shape of the deceleration unit 109 in the speed curve 110 are different, there is a problem that realization is impossible in principle.

Accordingly, an object of the present invention is to provide a mechanism control device capable of smoothly moving a moving member even on a path whose path length cannot be determined analytically.
Another object of the present invention is to provide a mechanism control device that can accurately stop a moving member at an end point.

[0008]

In order to achieve the above object, according to the first aspect of the present invention, there is provided a mechanism control device which comprises a speed derived from a function in which a speed change of a front speed change unit is defined from a starting point of a path. Forward tracing means for obtaining speed from the curve and sequentially generating points on the route, and at the same time, from the end point of the route, the time axis is derived from the speed curve derived from the function in which the speed change of the rear speed change unit is defined. Conversely, the speed is determined, the back tracing means for sequentially generating points on the path and storing the points in a buffer, and the points generated on the path by the forward tracing means are used as trajectories, and are generated by the forward tracing means. When the point on the route and the point on the route generated by the back tracing means cross the estimated speed change point obtained by the rough calculation before crossing The forward speed tracing process is continued on the rear speed changing portion, and when the speed obtained by the forward tracing device becomes equal to the speed obtained by the back tracing device, the true speed changing point at which the rear speed changing portion is started. And a trajectory generating means for calculating the deviation of the estimated speed change point on the route and correcting the points on the route stored in the buffer to follow the trajectory in reverse.

[0009] In the present invention, for a specified route,
Using the given speed curve, a front speed change portion and a constant speed portion are formed, and in parallel with this, a trajectory whose time axis is reversed while following the speed curve in reverse is generated, and data indicating this trajectory. Is stored in the buffer. And
In the front speed change portion and the constant speed portion, the moving member is moved according to a point generated on the route every time, and after the trajectory of the front side and the rear side intersect, according to the data stored in advance. To move along the trajectory on the rear speed change section. In other words, since the mechanism control means can determine the deceleration point in real time by performing back tracing, the speed or acceleration can be accurately determined even for a path whose path length cannot be determined analytically accurately. Can be assigned to Therefore, it is possible to use a useful speed curve other than the trapezoidal speed curve, such as acceleration / deceleration of a trigonometric function system. Further, different curves can be used for the acceleration section and the deceleration section.

Further, in the present invention, a speed change point such as a deceleration point is preliminarily estimated by a rough calculation in a pre-processing stage, and the forward trace determines the estimated speed change point before the front and rear trajectories cross. If it exceeds, the speed change is started from that point in time, thereby avoiding the delay of the timing by missing the normal speed change point to be changed. Further, in this case, taking into account that the position of the estimated speed change point (temporary deceleration point) deviates from the normal speed change point (true deceleration point), the corrected trajectory generated by the trajectory generation means is used. As a result, the moving member can be finally stopped accurately at the end point.

According to the second aspect of the invention, the deviation on the route is superimposed on the processing data by the forward tracing means, and in the third aspect of the invention, the deviation on the path is superimposed on the processing data by the back tracing means. Claim 1
The position on the route in the description is corrected.
Therefore, for example, even when a delay occurs at the time point at the estimated speed change point by the amount of the deviation, the moving speed is increased from the middle by superimposing the amount of the deviation on the processing data to finally move the moving member to the end point. To stop accurately.

According to a fourth aspect of the present invention, in the mechanism control device according to the second or third aspect, the shift is dispersed and superimposed by a smooth curve. Therefore, the speed can be changed gradually after the speed change point, and the moving member can be moved smoothly.

According to a fifth aspect of the present invention, in the mechanism control device according to the third aspect, the time required for dispersing the deviation on the path and correcting the overlap is longer than the time obtained by the back trace means. I am trying to do it. In this case, the moving member can be accurately stopped at the end point when the initial deceleration time has passed slightly.

[0014]

DETAILED DESCRIPTION OF THE PREFERRED EMBODIMENTS The configuration of the present invention will be described below in detail based on an example of an embodiment shown in the drawings.

1 to 3 show an embodiment of a mechanism control device according to the present invention. This mechanism control device enables a moving member such as a hand of a robot (hereinafter, simply referred to as a “moving member”) to accurately move on the path 1 according to a predetermined speed change and to stop accurately at an end point. It was made. In this case, the speed change is mainly performed by the front speed change portion 6 mainly composed of the acceleration portion, the rear speed change portion 7 mainly composed of the deceleration portion, and the constant speed portion 8 provided therebetween as necessary. Be composed.

First, before actually moving the moving member,
Whether the entire distance of the path 1 from the start point 2 to the end point 3 is sufficient to realize the given speed curves 4 and 5, in other words, the entire distance of the path 1 A pre-process is performed to determine whether or not it has a length necessary to perform. That is, when the moving member moves on the route 1 according to the speed curves 4 and 5 as shown in FIG. 1, it is determined whether or not the maximum speed of the speed curves 4 and 5 is realized.

The mechanism control device of this embodiment has a function of performing this preprocessing and determining whether or not the speed curves 4 and 5 can be realized. This function includes, for example, the total distance required for acceleration / deceleration obtained by integrating the given speed curves 4 and 5, the distance of the path 1 (in this case, the approximate length obtained by approximating the path 1 by a broken line). And analysis means for determining the size of the data. Then, as shown in FIG. 2, the acceleration / deceleration distance is calculated (step 1). After the approximate length of the route 1 is calculated (step 2), the magnitudes of the two are compared (step 3). If the approximate length of the route 1 is longer than the acceleration / deceleration distance (No in the figure), in other words, if the moving speed can reach the maximum value indicated by the speed curves 4 and 5, this preprocessing is directly performed. finish.

In this case, even if both the length of the front speed changing portion 6 and the length of the rear speed changing portion 7 are combined, the route 1
, The two changing parts 6, 6 as shown in FIG.
The constant velocity part 8 is inserted between the parts 7, and the moving member is moved at the maximum speed in the insertion part. Note that it is possible to configure the entire path 1 only by the front speed change unit 6 and the rear speed change unit 7, but the path 1 is formed by three parts including the constant velocity parts 8 inserted in this way. It is common to configure.

On the other hand, if the approximate length of the path 1 is shorter than the acceleration / deceleration distance (Yes in the figure), a scaling operation for deforming and correcting the speed curves 4 and 5 and resetting them is performed (step 4). As the scaling, for example, the speed curves 4 and 5 are reduced in proportion to the distance on the route 1, or one or both of the speed curves 4 and 5 are reduced on the time axis according to the distance on the route 1. This makes it possible to reduce the moving speed and the distance required for acceleration / deceleration to be shorter than the approximate length of the path 1. Further, the speeds of the speed curves 4 and 5 can be reduced in accordance with the distance on the route 1. As the scaling, one that only performs the compression operation in the vertical axis direction and one that changes both the axes can be appropriately used. Desired speed curve 4,
5 or a curve similar thereto is preferable.

In this embodiment, since the approximate length of the path 1 is determined by polygonal line approximation, the determined approximate length is always longer than the true length of the path 1. Therefore, even if the distance required for acceleration / deceleration obtained from the speed curves 4 and 5 after scaling becomes equal to the length of the approximate route 1, the true length of the route 1 is actually longer and the estimated The error acts in the direction of inserting the constant velocity portion 8. For this reason,
Unsuitable speed curves 4 and 5 such that the front speed changing unit 6 and the rear speed changing unit 7 are not connected smoothly are not generated.

After completing the above preprocessing, the process proceeds to the real time processing stage.

For example, as shown in FIG. 1, a book in which the path 1 is divided into a constant speed portion 8, a front speed changing portion 6 before the constant speed portion 8, and a rear speed changing portion 7 after the constant speed portion 8. In the embodiment, the mechanism control device that controls the real-time processing includes a forward tracing unit that sequentially generates points on the path 1 while following the front speed changing unit 6 and a reverse tracing unit that reverses the points on the rear speed changing unit 7. Back tracing means for sequentially generating points on the path 1 while tracing.

First, as shown in FIG.
When moving through the path 1 from the to the end point 3, the moving member moves while accelerating like a predetermined speed curve 4, 5 (or a curve after scaling), performs a constant speed motion once, and then decelerates on the way. It decelerates from point 9 and finally stops at end point 3. If the velocity curve 4 is given in this manner, the position of the moving member on the path 1 at the time t after the start of the movement is uniquely determined. Therefore, a point on the path 1 is defined as P, and P is defined as a parameter p. It is expressed as Equation 1 for convenience.

[0024]

P = f (p) (where 0 ≦ p ≦ 1) Also, when the speed _Vf at a certain time t is obtained as shown in Expression 2 using the formula representing the speed curve 4, the time T The distance l _f that the moving member moves on the path 1 during the period is calculated by the following equation (3) (the attached symbol f indicates forward, ie, the front side). Equation 2 in this case is a function in which the speed change of the front speed changing unit 6 is defined.

[0025]

## EQU2 ## V _f = V _f (t)

[0026]

(Equation 3)

The time t is expressed by the following equation (4).

[0028]

T _n = t _n-1 + Δt where n = 0, 1, 2,
…, T ₀ = 0 At this time, f (p), L _f (t) If known, the position at time t _n of the moving member when the moving on path 1 according to the speed curve 4 is obtained by the procedure of ~ below. That is, the distance l _en that must be moved from the previous time t _n−1 to the current time t _n is obtained by the following Expression 5, and _pn satisfying Expression 6 is obtained.
position on the path 1 of the moving member at time t _n is calculated by substituting p = p _n.

[0029]

(5) l _en = L _f (t _n ) −L _f (t _n-1 )

[0030]

(Equation 6)

As a result, as shown in FIG. 1, points Pn are sequentially generated on the route 1 from P0, which is the starting point 2, to P1, P2,..., And a set of points corresponding to the speed curve 4 is uniquely defined. Is determined. Note that, as described above, P
The means for calculating the coordinate value of n + 1 is, in other words, providing a circumference of radius l _en centered on Pn, and defining the intersection of this circumference and path 1 with Pn +
This corresponds to setting sequentially as 1.

On the other hand, in parallel with the processing by the forward tracing means as described above, the rear speed changing unit 7 also determines successive points P by the back tracing means.

However, the point P in this case is obtained by tracing the time axis in reverse from the end point 3 to the start point 2 of the moving member. It is determined sequentially in the same procedure. In this case, from the viewpoint of time reduction, the point P of the rear speed change unit 7
Is preferably obtained simultaneously with the front speed change unit 6. However, it is possible to generate points P even if they are not at the same time. When the point P of the rear speed changing unit 7 is obtained in this manner, the speed _Vb is expressed as a function defined by the speed change of the rear speed changing unit 7 as shown in Expression 7. Also, determine the distance l _b, Equation 8 below corresponding to Equation 3 above is used (accompanied obtained symbol b indicates that it is a back clogging rear). And
The distance l _en that must be moved from the previous time t _n-1 to the current time t _n is obtained by Expression 9, and such that Expression 6 is satisfied.
give p _n, position on the path 1 of the moving member in a more time t _n of substituting p = p _n in Equation 1 is obtained. Thereby, points Pn are sequentially generated on the route 1 from the end point 3, and
A set of points corresponding to the speed curve 5 is uniquely determined. Then, while determining the fixed point P from the end point 3 side in this way, the obtained points P on the rear speed changing unit 7 are sequentially stored and stored in data storage means such as a buffer.

[0034]

V _b = V _b (t)

[0035]

(Equation 8)

[0036]

L _en = L _b (t _n ) −L _b (t _n-1 ) Next, a point P on the path 1 is generated as described above, and the stored data is output. A flow for controlling the movement of the moving member will be described with reference to a flowchart.

As shown in FIG. 3, first, in step 5, the process proceeds to the processing stage of the point P on the rear speed change unit 7 shown in steps 6 to 8 until the back trace is completed and becomes unnecessary. Then, the moving distance is calculated using Equations 7 and 8 (Step 8), and the position coordinates on the route 1 corresponding to the moving distance (coordinate values relating to the rear speed changing unit 7) are obtained from Equations 9, 6 and 1. _Pb and the parameter value _pb ) are obtained, and the obtained data is sequentially stored in the data storage means.

After one coordinate is provided on the rear speed changing unit 7 as described above, the processing of the point P on the front speed changing unit 6 is performed until the forward trace is completed and becomes unnecessary (step S1). 9, 10). Here, obtains a moving distance using Equation 1 Equation 7, but get this to the position coordinates on the path 1 corresponding (coordinate values for the rear speed change section 7 P _f and the parametric values p _f), which The data obtained by is used for comparison without storage.

[0039] Then, to compare the magnitudes of both parametric p _b and p _f obtained in Step 7 and Step 10 (Step 1
1). As described above, the point P on the front speed change unit 6 is directed toward the end point 3, and the point on the rear speed change unit 7 is the start point 2.
While sequentially generated so as to approach each other toward the side, larger than the p _f towards p _b when compared in step 11 (if in Fig No), the Ryoten has not yet passed each other over path 1 Becomes Therefore, in this case, the data on the forward trace side is output as it is as the forward trace data, and the process ends (step 16). In this embodiment,
Thus, the data obtained by the forward tracing means is not stored, but is used as data for moving the moving member to the predetermined point Pn as it is. On the other hand, as described above, the data obtained by the back trace means is stored in the data storage means in parallel with this.

When the steps are completed as described above, one point P is generated in each of the front speed change unit 6 and the rear speed change unit 7. Therefore, the flow shown in FIG. 3 is made into one routine so that the point P can be sequentially generated, and after the end, the process returns to the start point and is repeated.

As this operation is repeated, the point P in the front speed change section 6 and the point P in the rear speed change section 7 gradually approach, and the point generated on the path 1 by the forward trace means and the back trace means is determined. The two orbits will intersect. That is, the size is reversed parametric at the time where the forward trace data and backtrace data intersect, since p _f is larger than p _b, now proceeds to step 13 side in step 11, the data storage means Retrieve the stored data sequentially from the latest one.

At this time, the extracted data is sequentially deleted from the data storage means (step 14).
Since point P has already been obtained on all paths 1, both forward tracing and back tracing are stopped (step 15). Thereafter, since both tracing operations become unnecessary, even if the flow is repeated, the process goes to the No side in Steps 5 and 9 and data obtained by the back tracing means is sequentially output from the data storage means as data output. (Step 16). The output data is processed by a trajectory generating means that traces a point on the path 1 in reverse and makes the trajectory operate an actuator or the like that moves the moving member. Therefore, the moving members after this will follow the data that is sequentially taken out,
The vehicle decelerates as shown by the speed curve 5 and stops accurately at a predetermined position, that is, at the end point 3 for a predetermined time.

As described above, in the mechanism control device of the present embodiment, the points P in the front speed change unit 6 and the rear speed change unit 7 are sequentially obtained from the start point 2 and the end point 3, respectively. Since the trajectory intersects near the middle of the route 1, the point P on the rear speed change unit 7 is stopped at this time, and the point P on the rear speed change unit 7 stored in the data storage unit is stopped. Data is sequentially extracted and moved on the route 1 according to the data. That is,
The moving member moves in accordance with the speed indicated by the speed curve 4 up to the vicinity of the middle, and thereafter moves in accordance with the speed curve 5 while taking out data from the intersection position. Accordingly, the moving member decelerates while following the predetermined point P, and can stop at the end point 3 at the same time when the speed becomes 0 in the predetermined time. In addition, the deceleration point 9
, It is possible to accurately assign the speed or acceleration even for the route 1 whose length cannot be determined analytically accurately, and to accurately stop the moving member at the end point 3 of the route 1. Can be. Therefore, the movable member can be smoothly moved using a useful speed curve other than the trapezoidal speed curve, such as acceleration / deceleration of a trigonometric function system. Further, different curves can be used for the acceleration section and the deceleration section.

In the present embodiment, when the trajectories of the point P generated by the forward tracing means and the back tracing means do not intersect, if the front speed changing unit 6 and the rear speed changing unit 7 Since the moving member 8 is inserted so as to move at a constant speed after passing through the accelerating portion, it is possible to cope with the case where the moving member moves on the route 1 at various distances.

The above embodiment is an example of a preferred embodiment of the present invention, but the present invention is not limited to this, and various modifications can be made without departing from the gist of the present invention. For example, in the above-described embodiment, the back tracing is performed until the forward tracing data and the back tracing data (the processing data on the back tracing side) intersect. However, the control method of the mechanism is not limited to this, and the rear speed changing unit 7 , The generation of the point P may be terminated. In this case, as shown in FIG. 4, a process for determining whether or not the speed curve 5, that is, the deceleration portion has been completed is provided (step 8-2), and if it has been completed, the back trace may be terminated (step 8-2). 8-3) This makes it possible to eliminate waste of the processing operation.

Further, in the present embodiment, the speed curves 4 and 5 are exemplified by a relatively monotonous one having only the acceleration section and the deceleration section as shown in FIG. , 5 are sequentially generated to generate the point P on the path 1, so that the monotonous velocity curves 4, 5 needless to say, and furthermore, the acceleration section-acceleration section continuation is performed. It is possible to cope with various kinds of complicated speed curves in which several speed change portions are combined, such as a speed curve that is performed and a continuous speed curve of a deceleration section-a deceleration section.

However, some of the composite speed curves 4 and 5 have a slower speed curve 5 and a longer time occupied by the speed curve 5 than the speed curve 4, for example. 2. If the processing is performed in parallel from the end point 3, the generated point P may intersect on the rear speed change unit 7. Then, the deceleration starts after the forward trace has already passed the regular deceleration point 9, and it becomes impossible to accurately stop the moving member at the end point 3, and it is necessary to avoid this in advance. . Hereinafter, other embodiments of the present invention that can cope with such a case will be described.

The mechanism control device here has a function of adjusting the number of calculations in accordance with the ratio of the acceleration / deceleration time, and the point P generated by the forward tracing means and the point P generated by the back tracing means. Can be adjusted so that they do not intersect after the estimated speed change point. in this case,
The estimated speed change point is an estimated speed change point obtained by performing approximate calculation to approximate the path 1 in a broken line in the preprocessing stage and obtaining the approximate speed change point from the end point 3 (hereinafter, this point is referred to as “temporary deceleration”). Point 9 '). According to the acceleration / deceleration time ratio, the forward side is calculated once, whereas the back side is calculated a plurality of times, and the point P generated on the route 1 is at least the forward side, that is, the constant speed from the temporary deceleration point 9 '. Intersect at part 8.

On the other hand, depending on the form of the speed curves 4 and 5, the ratio of the acceleration / deceleration time may be too extreme. In this case, instead of adjusting the number of calculations, the speed curves 4 and 5 are made similar. You may make it deform. For the curve deformation in this case, a trajectory generating means provided in the mechanism control device is used so that the trajectory of the path 1 obtained as described above and the new trajectory can be corrected. The trajectory generating means calculates the temporary deceleration point obtained by the rough calculation before the point P on the path 1 generated by the forward tracing means and the point P on the path 1 generated by the back tracing means pass (intersect). When the speed exceeds 9 ', the forward tracing process is continued on the rear speed changing unit 7, and when the speed obtained from the forward tracing unit and the speed obtained from the back tracing unit become equal, the rear speed changing unit 7 starts. The true speed change point to be started (hereinafter,
The deviation between the "true deceleration point" 9 and the temporary deceleration point 9 'on the path 1 is calculated, and the point P on the path 1 stored in the buffer is corrected and traced in reverse to form a trajectory.

The procedure of the above-mentioned curve deformation is as follows. First, a temporary deceleration point 9 'is obtained in the preprocessing stage, and the moving member is moved in accordance with the speed curve 5 from the time when the forward trace approaches the temporary deceleration point 9'. Start deceleration. However, the temporary deceleration point 9 'is obtained by using the approximate length slightly longer than the actual length of the route 1, and is located before the true deceleration point 9; As shown in a time-position curve 10 shown in FIG. 5, the moving member when the deceleration is started from before the target position (end point 3), that is, the error (deviation) 13 is shorter by a time shorter than the predetermined time. Will stop at

Therefore, the error 13 is obtained as described below so that the moving member can be accurately stopped at the end point 3. First, the time-position curve 11 in the figure is obtained by back-tracing the rear speed change portion 7 from the end point 3 and represents the relationship between the true time and the position. If it moves according to 11, it will arrive at the end point 3 later than the case of the curve 10 as shown. In this case, the curve 10 obtained by the forward trace and the curve 11 obtained by the back trace are both calculated from the same speed curve 5 and have the same shape. Therefore, if the curve 11 is shifted like the curve 12 in order to align the time axes, the difference between the positions of the curves 10 and 12 at the same time t is always the same as the magnitude of the error 13 at the final destination. That is, the magnitude of the error 13 corresponds to the estimation error of the temporary deceleration point 9 '.

Therefore, the moving member is finally shifted from the forward trace curve 10 to the back trace curve (after being shifted) 12 during the deceleration time Td required for deceleration so that the moving member is accurately stopped at the end point 3. I do. However, it is preferable that this transfer operation is as smooth as possible from the viewpoint of reducing vibration to the moving member. For example, as shown in FIG.
After passing, it is also assumed that when the forward tracing speed and the back tracing speed become equal to each other, the transition to the back tracing curve 12 is almost instantaneous. In this embodiment, however, a smooth curve is used. The deviation on the path is dispersed and superimposed.

For example, in FIG.
As shown in (A) to (C), the form of the time-position curve when the speed is changed halfway is shown, and the speed change becomes smoother.
These are to disperse the deviation on the route 1 and to correct the position by superimposing it on the processing data by the back trace means. As a result of the superimposition, the speed after data superimposition is increased as a whole. Have been. In this case or in FIG. 7A and FIG. 7B, the areas S1 and S2 of the enclosed portions of the respective graphs before and after the speed change are shown.
The speed change is set so as to be equal as shown in (1), and the moving member stops accurately at the end point 3 after the deceleration time Tb. On the other hand, it is also possible to stop at the end point 3 with a time lag as shown in FIG. Note that tf and td in FIGS. 5 and 6 respectively indicate the elapsed time after passing the temporary deceleration point 7 ′ in the forward trace and the elapsed time from the start of the back trace.

As described above, the error 1 on the path 1
3 may be superimposed on the data processed by the back trace means, or may be superimposed on the data processed by the forward trace means to correct the position. In this case, route 1
It is preferable that the time required for dispersing the above deviation and performing the superimposition correction be longer than the time obtained by the back trace means.

According to the mechanism control device of the embodiment described above, the forward tracing is performed after the back tracing enters the constant velocity portion 8 by adjusting the number of times of calculation of both tracings in accordance with the ratio of the acceleration / deceleration time. Therefore, it is possible to realize the moving operation according to the desired speed curves 4 and 5. In addition, for a speed change for which such calculation adjustment is not suitable, the speed change point is estimated in advance by a rough calculation in the preprocessing stage, and the forward trace is estimated by the forward trace before the front and rear orbits intersect. When the speed exceeds the speed change point, the speed change is started from that point in time, thereby avoiding the delay of the timing by missing the normal speed change point to be changed. Further, the speed curve applied to the route 1 itself is deformed so as to be moved to the target position, and an error 13 which may occur when the temporary deceleration point 9 'is set is absorbed. In addition, at the time of curve deformation, by dispersing and superimposing the error 13, the speed can be increased on average and the moving member can be moved smoothly.

In each of the embodiments described above, the first half,
Although the speed curves 4 and 5 each including a gentle acceleration portion and a deceleration portion in the latter half have been described as examples, the form of the curve is not particularly limited to this. Therefore, the following describes various speed curves that can be supported by the mechanism control device of the present invention, and movements when the moving member is moved from the start point 2 to the end point 3 according to the curves. The change in the moving speed of the moving member is indicated by the interval between diamond marks plotted on the route 1 at regular intervals.

First, as shown in FIG. 8, when both the first half and the second half of the velocity curve are acceleration parts (acceleration-acceleration), the moving member is moved along the path 1 from the start point 2 to the end point 3 according to this. FIG. 9 shows a curve indicating the position at the time of the movement, and the moving speed increases as approaching the end point 3.

When the latter half is a deceleration section (acceleration-
Deceleration) is the same as in the above-described embodiment (FIG. 10), and the speed decreases near the start point 2 and the end point 3 as shown in FIG. On the other hand, when the first half is a deceleration section and the second half is an acceleration section (deceleration-acceleration) as shown in FIG. 12, on the other hand, it moves near the start point 2 and the end point 3 as shown in FIG. The speed is fast and the slowest in the constant velocity part. Also,
Contrary to that shown in FIG. 8, when both the first half and the second half are deceleration parts as shown in FIG. 14 (deceleration-deceleration), the moving member is fastest near the start point 2 as shown in FIG. 3
The slowest in the vicinity.

Further, the speed curve may have only one changing part. For example, as shown in FIG. 16, the relationship between time and position in the case where only the first half is the acceleration part and thereafter the velocity curve moves at a constant speed is as shown in FIG. 17, while only the latter half is shown in FIG. 18. In the case of the deceleration section, the moving member moves as shown in FIG. It is needless to say that the moving member moves at a constant speed as shown in FIG. 21 if the speed curve has only the constant speed portion as shown in FIG.

Since the mechanism control device described above moves the moving member based on data obtained by the forward trace and the back trace, various speed curves as exemplified above, and furthermore, the first half. It is possible to cope with a complex speed curve in which the part and the latter half include an acceleration part and a deceleration part. In this case, the speed change point at the boundary between the constant speed portion 8 and the rear speed change portion 7 and expressed as the deceleration point 9 for convenience in the above description corresponds to various speed curves, and Of course, it can be an acceleration part.

[0061]

As is apparent from the above description, according to the mechanism control device of the first aspect, the deceleration point can be obtained in real time by performing back tracing. Speeds or accelerations are accurately assigned to routes that cannot be determined analytically accurately, and useful speed curves other than trapezoidal speed curves, such as acceleration and deceleration of trigonometric functions, can be used. Therefore, the moving member can be smoothly moved even on a path whose path length cannot be determined analytically.

In the present invention, the speed change point is estimated in advance by a rough calculation in the preprocessing stage, and if the forward trace exceeds the estimated speed change point before the front and rear orbits intersect, By starting the speed change from that point in time, the timing is prevented from being delayed by missing the normal speed change point to be changed. Further, in consideration of the fact that the position of the estimated speed change point (temporary deceleration point) deviates from the normal speed change point (true deceleration point), the corrected trajectory generated by the trajectory generation means is used. Therefore, the moving member can be finally stopped accurately at the end point.

According to the mechanism control device of the second or third aspect, the position on the path is corrected by superimposing the deviation on the path on the processing data, and the moving member is finally accurately stopped at the end point. Can be done.

According to the mechanism control device of the fourth aspect, by dispersing and superimposing the deviation by a smooth curve, the speed can be changed gradually after the speed change point, and the moving member can be moved smoothly. it can.

According to the mechanism control device of the fifth aspect, the time required for dispersing the deviation on the path and correcting the superposition is made longer than the time obtained by the back tracing means, thereby reducing the initial deceleration. The moving member can be accurately stopped at the end point after a short time.

[Brief description of the drawings]

FIG. 1 is a diagram showing an embodiment of a mechanism control device according to the present invention, showing a speed curve including an acceleration unit and a deceleration unit, and a path along which a moving member is moved according to the speed curve.

FIG. 2 is a flowchart showing a preprocessing stage.

FIG. 3 is a flowchart showing a flow of a step of generating a point on a path by a back trace and a forward trace and accurately moving a moving member.

FIG. 4 is a flowchart showing another embodiment of the present invention.

FIG. 5 is a diagram showing another embodiment of the present invention for accommodating a composite speed curve, and shows a time-position curve obtained by a forward trace and a back trace, and a time in which a time axis is shifted. -Shows a position curve.

FIGS. 6A and 6B are diagrams respectively illustrating modes of a time-position curve when changing the speed halfway.

FIG. 7 is a diagram showing a speed change in which a shift on a route is superimposed, and (A) to (C) correspond to in FIG. 6, respectively.

FIG. 8 is a diagram showing an example of an acceleration-acceleration type speed curve.

FIG. 9 is a diagram showing a progress of a position on a route when a moving member is moved corresponding to the speed curve of FIG. 8;

FIG. 10 is a diagram showing an example of an acceleration-deceleration type speed curve.

11 is a diagram showing a progress of a position on a route when a moving member is moved corresponding to the speed curve of FIG. 10;

FIG. 12 is a diagram illustrating an example of a speed curve of a deceleration-acceleration type.

FIG. 13 is a diagram showing a progress of a position on a route when a moving member is moved corresponding to the speed curve of FIG. 12;

FIG. 14 is a diagram illustrating an example of a deceleration-deceleration type speed curve.

FIG. 15 is a diagram showing a progress of a position on a route when the moving member is moved corresponding to the speed curve of FIG. 14;

FIG. 16 is a diagram illustrating an example of a speed curve having one changing unit (acceleration unit).

FIG. 17 is a diagram showing a progress of a position on a route when a moving member is moved corresponding to the speed curve of FIG. 16;

FIG. 18 is a diagram illustrating an example of a speed curve in which there is one changing section (deceleration section).

19 is a diagram showing a progress of a position on a route when the moving member is moved corresponding to the speed curve of FIG. 18;

FIG. 20 is a diagram illustrating an example of a speed curve including only a speed portion.

FIG. 21 is a diagram showing a progress of a position on a route when a moving member is moved corresponding to the speed curve of FIG. 20;

FIG. 22 is a diagram showing conventional means for analytically finding a deceleration point for controlling the speed or acceleration of a moving member,
(A) shows a speed curve, and (B) shows a deceleration point and the like.

23A and 23B are diagrams showing conventional means for obtaining a deceleration point or the like using a filter for controlling the speed or acceleration of a moving member, wherein FIG. 23A shows speed data before filtering, and FIG. 23B shows speed data after filtering. .

[Description of Signs] 1 Path 2 Start point 3 End point 4 Speed curve 5 Speed curve 6 Front speed change section 7 Rear speed change section 8 Constant speed section 9 Deceleration point (true speed change point) 9 'Temporary deceleration point ( Speed change point) 13 Deviation (on the route) P Point (on the route)

 ────────────────────────────────────────────────── ─── Continuing on the front page (72) Inventor Hirokazu Watanabe 10801 Haramura, Suwa-gun, Nagano 2F, 3F059 FC02 5H269 AB33 BB03 EE01 5H303 AA10 EE03 EE07 KK14 KK17 MM05 9A001 GG03 HH03 HH19 KK32

Claims (5)

[Claims] 1. A forward tracing means for obtaining a speed from a speed curve derived from a function in which a speed change of a front speed change unit is defined from a starting point of a route and sequentially generating points on the route, From the end point of the back trace means to reverse the time axis from the speed curve derived from the function in which the speed change of the rear speed change unit is defined from the time curve, and to sequentially generate points on the route and store them in the buffer ,
The point generated on the path by the forward tracing means is followed as a trajectory, and before the point on the path generated by the forward tracing means and the point on the path generated by the back tracing means intersect, ,
When the estimated speed change point obtained by the rough calculation is exceeded, the forward tracing process is continued on the rear speed change portion, and when the speed obtained from the forward tracing means is equal to the speed obtained from the back tracing means. so,
Calculate the deviation on the route between the true speed change point at which the rear speed change unit is started and the estimated speed change point, and follow the path while correcting the points on the route stored in the buffer in reverse. A mechanism control device comprising a trajectory generating means. 2. The mechanism control device according to claim 1, wherein the position on the path is corrected by dispersing the deviation on the path and superimposing the deviation on processing data by forward tracing means. 3. The mechanism control device according to claim 1, wherein the position on the path is corrected by dispersing the deviation on the path and superimposing the deviation on the processing data by the back trace means. 4. The mechanism control device according to claim 2, wherein the deviation is distributed and superimposed by a smooth curve. 5. The mechanism control device according to claim 3, wherein the time required for dispersing the deviation on the path and performing the superimposition correction is longer than the time obtained by the back trace means.