JP2000181523A - Mechanism controller - Google Patents

Abstract

PROBLEM TO BE SOLVED: To adopt a velocity curve capable of exactly stopping a moving member at the terminal and smoothly moving the moving member even on a path for which the path length is not analytically determined. SOLUTION: This device is provided with a forward trace means for successively generating points P on a path 1 by finding a velocity from a velocity curve 4 derived from a function defining the velocity change of a front side velocity change part 6 from a start point 2 of the path 1, back trace means for successively generating the point on the path and storing it in a buffer by finding a velocity, while reversing the time base, from a velocity curve 5 derived from a function defining the velocity change of a back side velocity change part 7 from a terminal point 3 of the path 1 at the same time, and track generating means for providing a track by tracing the points P generated on the path 1 by the forward trace means and providing a track by tracing the points P on the path 1 stored in the buffer in a reverse direction at the moment the points P on the path 1 generated by the forward trace means and the points P on the path 1 generated by the back trace means cross with each other.

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. 5A, 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.
When the moving member is moved from the start point 103 to the end point 104 as shown in FIG. 3, a deceleration point 105 is determined by taking a distance from the end point 104 by the deceleration distance 102, and when the moving member approaches the deceleration point 105, By starting the deceleration, it can be stopped at the end point 104.

On the other hand, in the method using a filter, first, data 106 is generated as shown in FIG. 6 (A) when moving on a route at a constant speed from a start point to an end point. In this case, the speed data 106 has a rectangular shape, and the distance from the start point to the end point on the predetermined route is represented by its area. Next, the data 106 is filtered, and an acceleration unit 107, a constant velocity unit 108, and a deceleration unit 109 as shown in FIG.
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, as shown in FIG. 5, in the means for analytically finding the deceleration point 105,
If the deceleration point 105 is not accurately obtained, the moving member is moved to the end point 10
Since the stop cannot be performed accurately at step 4, it is necessary to analytically determine the path length accurately. 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, the acceleration unit 107 and the deceleration unit 1 in the speed curve 110
There is also a problem that the realization is impossible in principle if the shape of 09 is different.

Accordingly, an object of the present invention is to provide a mechanism control device capable of accurately stopping a moving member at an end point 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 employs a speed curve that can smoothly move a moving member.

[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 a point on the route and a point on the route generated by the backtrace intersect, the point on the route stored in the buffer is traced in reverse to form a trajectory. In which and a road generation means.

[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 the speed of a path whose path length cannot be determined analytically accurately can be determined. Acceleration can be accurately assigned. 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.

According to a second aspect of the present invention, in the mechanism control device of the first aspect, a point on the path generated by the forward tracing means and a back point at the end of the front speed change section and the rear speed change section. When points on the route generated by the tracing means do not intersect, a constant velocity portion is inserted between the front speed change section and the rear speed change section. Therefore, it is possible to cope with a case where the moving member is moved on routes of various distances.

Further, the invention according to claim 3 is the invention according to claim 1.
In the mechanism control device described above, it is determined whether or not the distance of the route is sufficient to realize a given speed curve. If the distance is insufficient, the speed curve is deformed, and the forward trace means and the back trace A point on the route is generated by means. In this case, it is impossible to move the moving member as indicated by the speed curve depending on the distance of the given route, so that a sufficient speed is secured by deforming the speed curve according to the route distance. It can be stopped exactly at the end point.

According to a fourth aspect of the present invention, a speed curve in the mechanism control device according to the third aspect is reduced in proportion to a distance on a route. This allows
The moving member can be accurately stopped at the end point while securing a sufficient speed.

The invention according to claim 5, wherein the time axis of the speed curve is reduced according to the distance on the route, or the speed of the speed curve is reduced according to the distance on the route. According to the invention described in the sixth aspect, the moving member can be accurately stopped at the end point while securing a sufficient speed.

[0015]

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 the mechanism control device of 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 change portion 6 and the length of the rear speed change 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 / 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 the present 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 pre-processing, 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.

[0025]

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.

[0026]

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

[0027]

(Equation 3)

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

[0029]

T _n = t _n-1 + Δt where n = 0, 1, 2,
.., T ₀ = 0 At this time, if f (p) and L _f (t) are known, the position of the moving member at time t _n when moving on the route 1 according to the speed curve 4 is as follows: Is determined by the following procedure. 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.

[0030]

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

[0031]

(Equation 6)

As a result, as shown in FIG. 1, points Pn are sequentially generated on the path 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 following 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.

[0035]

V _b = V _b (t)

[0036]

(Equation 8)

[0037]

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, the moving distance is obtained by using Expressions 1 to 7, and the corresponding position coordinates on the route 1 (the coordinate value P _f and the parameter value p _f relating to the front speed change unit 6) are obtained. The data obtained is used for comparison without storage.

[0040] 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.

Then, as this operation is repeated, the point P in the front speed change unit 6 and the rear speed change unit 7 gradually approaches, and the point generated on the path 1 by the forward trace unit and the back trace unit 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 (Step 1
3).

At this time, the retrieved 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 according to 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 moving 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 points 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.

Although the above embodiment is an example of a preferred embodiment of the present invention, the present invention is not limited to this embodiment, and various modifications can be made without departing from the gist of the present invention. For example, in the above-described embodiment, the back trace is performed until the forward trace data and the back trace data (data on the back trace side) intersect.
The method of controlling 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.

In the present embodiment, the speed curves 4 and 5 are relatively monotonous as shown in FIG. 1 consisting of only an acceleration section and a deceleration section. , 5 are analyzed to sequentially generate the point P on the path 1, so that the monotonous velocity curves 4, 5 needless to say, and furthermore, the acceleration section—the acceleration section 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.

[0048]

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. Even for a path that cannot be determined analytically accurately, the speed or acceleration can be accurately assigned, and the moving member can be accurately stopped at the end point of the path. Therefore, the moving 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.

According to the mechanism control device of the present invention, at the end of the front speed change section and the rear speed change section, the points on the path generated by the forward trace means and the back trace means are generated. When the points on the route do not intersect, since the constant velocity portion is inserted between the front speed change portion and the rear speed change portion, even when moving the moving member on the route of various distances Can correspond.

Further, according to the mechanism control device of the third aspect, it is determined whether or not the distance of the route is sufficient to realize the given speed curve. If the distance is insufficient, the speed curve is deformed. Then, a point on the route is generated by the forward trace means and the back trace means. Therefore, even if it is impossible to move the moving member as indicated by the speed curve due to the distance of the given route, a sufficient speed is secured by deforming the speed curve according to the route distance, and then the end point is obtained. Can be stopped accurately.

In the mechanism control device according to the fourth aspect, the speed curve is reduced in proportion to the distance on the route, so that the moving member is accurately stopped at the end point while securing a sufficient speed. Can be.

The mechanism control device according to claim 5, wherein the time axis of the speed curve is reduced according to the distance on the route, or the speed of the speed curve is reduced according to the distance on the route. According to the mechanism control device of the sixth aspect, the moving member can be accurately stopped at the end point while securing a sufficient speed.

[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 conventional means for analytically obtaining 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.

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

[Explanation of symbols]

 1 route 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 part P (on the route)

 ────────────────────────────────────────────────── ─── Continuing on the front page (72) Inventor Hirokazu Watanabe 10801 Hara-mura, Suwa-gun, Nagano F-term (reference) 3F059 FB05 FC02 5H269 AB33 EE01 EE13 QA17 5H303 AA10 BB03 CC03 EE03 EE07 KK14 9A001 GG11 KK32

Claims (6)

[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 a point on the path generated by the forward tracing means intersects with a point on the path generated by the back tracing means. And a trajectory generating means for reversely tracing a point on the path stored in the buffer to form a trajectory. 2. When a point on the path generated by the forward tracing means and a point on the path generated by the back tracing means do not intersect at the end of the front speed changing section and the rear speed changing section. 2. The mechanism control device according to claim 1, wherein a constant velocity portion is inserted between the front speed change portion and the rear speed change portion. Determining whether or not the distance of the route is sufficient to realize a given speed curve, and if the distance is not sufficient, deforming the speed curve to obtain the forward trace and the back trace; 2. The mechanism control device according to claim 1, wherein a point on the path is generated by the means. 4. The mechanism control device according to claim 3, wherein the speed curve is reduced in proportion to the distance on the route. 5. The mechanism control device according to claim 3, wherein the time axis of the speed curve is reduced according to the distance on the route. 6. The mechanism control device according to claim 3, wherein the speed of the speed curve is reduced according to the distance on the route.