JP2007233613A - Speed control method and speed controller - Google Patents

Abstract

<P>PROBLEM TO BE SOLVED: To provide a speed control method for moving an object at a speed appropriate for a curvature at each point on a passing trace of the object. <P>SOLUTION: The speed control method for the object comprises inputting coordinate data of the track; calculating a curvature at each point on the trace; calculating a speed of the object at each point from the calculated curvature at each point by an equation (2) using an angular speed represented by an equation (1); and outputting the calculated speed at each point to the object. <P>COPYRIGHT: (C)2007,JPO&INPIT

Description

The present invention relates to a method and apparatus for controlling the velocity of an object (ie, the tangential velocity of a trajectory).

Conventionally, when moving an object such as a hand of a robot hand or a transport carriage, at the start and end of the movement of the object, the acceleration is constant until the speed of the object reaches a predetermined speed. Is a trapezoidal speed control in which the acceleration is 0, that is, the speed is constant (see, for example, Patent Document 1).

JP-A-1-237806

However, in the trapezoidal speed control method as in the prior art, a place where the path of the object is greatly bent, that is, where the path (trajectory) of the object is a curve or has a corner, that is, the curvature of Since the speed is constant even in a large place, if the object moves at a high speed, there is a problem that the centrifugal force increases and deviates from the determined trajectory.

The present invention has been made in view of such circumstances, and an object thereof is to provide a speed control method and apparatus for moving an object at a speed corresponding to the curvature at each point on a trajectory through which the object passes.

The speed control method according to the present invention that meets the above-mentioned object is a speed control method for an object that passes through a preset trajectory,
A first step of inputting coordinate data of the trajectory;
A second step of calculating a curvature at each point on the locus;
A third step of calculating the velocity of the object at each point from the calculated curvature at each point according to equation (2) using an angular velocity represented by equation (1);
And a fourth step of outputting the calculated speed at each point to the object.

Here, α is less than 1, preferably more than 1/3 and less than 4/5, more preferably more than 1/2 and less than 3/4. Within this range, when α is close to 1, the difference in speed (that is, tangential speed) between a place with a small curvature and a place with a large curvature is smaller than that observed by human movement, and it was applied to a humanoid robot. When α is 1, the velocity v of the object is always constant regardless of the magnitude of the curvature c. When α exceeds 1, the velocity increases as the curvature increases. Become. When α is 1/3 or less, the change in speed becomes too large with respect to the change in curvature. In the present invention, the locus means a line connecting positions occupied by an object (one point on a moving part, generally a functional point or a representative point) in space during movement.

In the speed control method according to the present invention, the speed of the object at each point on the trajectory can be calculated to generate the trajectory of the object.
In the speed control method according to the present invention, the maximum speed of the object is input in the first step, and the speed data of the object at each point calculated by the equation (2) in the third step is When exceeding the maximum speed, it is preferable to correct the speed of the object below the maximum speed.
In the speed control method according to the present invention, in the third step, the speed of the object is corrected so that a rate of change (that is, acceleration) of the speed of the object according to the equation (2) is a predetermined value or less. Also good.

In the speed control method according to the present invention, in the second step, the curvature at each point on the locus is corrected so that the change rate of the curvature is a predetermined value or less, and then the correction is performed in the third step. The velocity of the object may be calculated from the calculated curvature.
In the speed control method according to the present invention, in the formula (2), α is preferably 2/3. Here, when α is 2/3, equation (1) is the third-square law, which is a psychophysical empirical rule found by Viviani P. and others from observation of how to move the human hand. Since (The 2/3 power low) is obeyed, the equation (2) using the angular velocity shown in the equation (1) also follows the square law.
The speed control method according to the present invention may be used to control the speed of a robot arm or a transport vehicle (an example of an object).
Here, examples of the transport vehicle include a transport cart and a trackless cart that does not move on the rail.

The speed control device according to the present invention that meets the above-mentioned object is a speed control device for an object that passes through a preset trajectory,
An initial value input unit for inputting the coordinate data of the locus;
A curvature calculator for calculating the curvature at each point on the locus;
A velocity calculation unit that calculates the velocity of the object at each point from the calculated curvature at each point according to equation (2) using an angular velocity represented by equation (1);
An output unit that outputs the calculated speed at each point to the object.
In the speed control device according to the present invention, the output unit may output the calculated speed at each point to a robot arm or a transport vehicle (an example of an object).

In the speed control method according to any one of claims 1 to 7, the curvature at each point on the trajectory is calculated from the input coordinate data of the trajectory, and the object at each point is calculated from the calculated curvature at each point. Since the speed is calculated, the object can be moved at a speed according to the curvature, and particularly when the trajectory is bent largely, the speed of the object is reduced and it is difficult to deviate from the predetermined trajectory.
In particular, in the speed control method according to claim 2, the trajectory for controlling the speed of the object at each point on the trajectory is generated by calculating the speed at each point on the trajectory of the object and generating the trajectory of the object. Can be created.
In the speed control method according to claim 3, when the speed data of the object at each point calculated in the third step exceeds the input maximum speed, the speed of the object is corrected to be equal to or lower than the maximum speed. Speed does not exceed where the curvature is small.

In the speed control method according to claim 4, since the speed of the object is corrected in the third step so that the rate of change of the speed of the object is not more than a predetermined value, the speed is adjusted so that the object does not suddenly accelerate or decelerate. be able to.
In the speed control method according to claim 5, after correcting the curvature at each point on the trajectory in the second step so that the rate of change of the curvature is equal to or less than a predetermined value, the curvature of the object is calculated from the curvature corrected in the third step. Since the speed is calculated, the speed can be adjusted so that the object does not accelerate or decelerate suddenly.
In the speed control method according to the sixth aspect, since α is 2/3, the movement of the object is smooth according to the second-square law, and in particular, a humanoid robot can approach a human movement. .

In the speed control device according to claim 8 and 9, a curvature calculator that calculates the curvature at each point on the trajectory from the input coordinate data of the trajectory, and at each point from the calculated curvature at each point. It has a speed calculation unit that calculates the speed of the object, so that the object can be moved at a speed according to the curvature. It can be difficult to deviate from the trajectory.

Next, embodiments of the present invention will be described with reference to the accompanying drawings for understanding of the present invention.
Here, FIG. 1 is a block diagram of a speed control device according to an embodiment of the present invention, FIG. 2 is an explanatory diagram showing a locus of an object in a speed control method according to an embodiment of the present invention, and FIG. FIG. 4 is a graph showing the relationship between the path length of the object and the curvature in the speed control method, FIG. 4 is a graph showing the relationship between the path length of the object and the tangential speed in the speed control method, and FIGS. FIGS. 6A to 6D are explanatory diagrams when the tangential speed is corrected by the same speed control method, and FIGS. 6A to 6D are explanatory diagrams when the curvature is corrected by the same speed control method. B) is a conceptual diagram of an object in a simulation applying the same speed control method, an explanatory diagram of a secondary system, FIG. 8 is a graph showing the trajectory of an object given in advance to the object speed control device, and FIG. FIG. 10 is a graph showing the calculation result of the velocity of the object. Figure 11 is a graph showing the trajectories of moving the object.

With reference to FIGS. 1-4, the speed control apparatus 10 of the object which concerns on one embodiment of this invention is demonstrated.
As shown in FIG. 1, the speed control device 10 inputs a preset trajectory A (see FIG. 2) through which an object (not shown) (for example, the hand of the robot hand) passes and an initial value for inputting the maximum speed v _{max of the} object. An input unit 11, a curvature calculation unit 12 that calculates a curvature c at each point X on the locus A input to the initial value input unit 11, and a maximum velocity v _max and a curvature calculation unit that are input to the initial value input unit 11 Based on the curvature c calculated in 12, the speed calculator 13 that calculates the velocity v of the object at each point X, and the velocity v calculated by the velocity calculator 13 to the object controller (not shown). And an output unit 14 for outputting, and the speed of the object is controlled so that the object moves fast and accurately along the trajectory A.

Next, an object speed control method using the speed control apparatus 10 will be described.
(First step)
First, the trajectory A and the maximum velocity v _{max of the} object are input to the initial value input unit 11. Here, as shown in FIG. 2, the trajectory A is composed of two straight lines connected at the intermediate point M, and has a structure in which a corner is formed by bending at the intermediate point M. The locus A is two-dimensional, and a plurality of points X _i (x _i , y _i ) from the start point X ₀ (x ₀ , y ₀ ) to the end point X ₁ (x ₁ , y ₁ ) on the locus A ) (However, 0 ≦ i ≦ 1) constitutes the coordinate data of the locus A. The initial value input unit 11 receives data of coordinates of all points on the locus A. The trajectory may be given in three dimensions, or may be given by a mathematical expression such as a parametric curve such as a Bezier curve. A numerical value indicating the coordinates of each point on the trajectory A is sent from the initial value input unit 11 to the curvature calculating unit 12, and a numerical value indicating the maximum speed v _max is sent to the speed calculating unit 13.

(Second step)
Next, the curvature calculator 12 calculates the curvature at each point X on the locus A.
First, it is assumed that the object travels on the trajectory A from the start point X ₀ to the end point X ₁ at a constant speed, and the speed v (t) at the time t when the point on the trajectory A is sampled with a minute step δt is obtained. (3) It calculates | requires by Formula. Further, since the acceleration can be calculated in the same manner, the curvature c (t) at each point can be obtained by the equation (4).

Here, the distance from the starting point X ₀ of an arbitrary point X on the trajectory A is set as the path length s, and a point on the trajectory A is sampled with a step width δs during the step time δt. When the point X is reached, the tangential velocity v (s) at the position of the path length s is expressed by equation (5). Accordingly, the curvature c (s) at the position of the path length s when only the trajectory A is given is expressed by the equation (6), and the curvature c (s) at the path length s of the trajectory A is as shown in FIG. Shown in Data of the calculated curvature c (s) is sent to the speed calculation unit 13.

(Third step)
Here, since the equation (5) is a constant velocity motion, the tangential velocity v _d (s) corresponding to the curvature c (s) at each point X is calculated by the equation (7) (in FIG. (Indicated by a chain line). Note that the velocity v (u) of the object when the parameter is u is calculated by the equation (2) using the angular velocity ω shown in the equation (1), and the parameter of the equation (2) is changed to the path length s. (7) Formula is obtained by substituting. Further, since the curvature c is the reciprocal of the curvature radius r, the velocity v _d (s) can also be expressed by equation (8). Further, although the magnitude of the velocity v _d (s) is different from the velocity v (s), their directions coincide with each other, and therefore the x-direction component x _d (s) and the y-direction component y _{d of the} velocity v _d (s). (S) is expressed by Equation (9) and Equation (10).
Thus, since the velocity v _d (s) of the object is determined according to the curvature c (s), the tangential velocity of the object is slowed down when the locus A is bent greatly, that is, where the curvature c is large. Thus, it is possible to prevent the object from deviating from the locus A due to the centrifugal force.

Here, in the formula (7), α is less than 1, preferably more than 1/3 and less than 4/5, more preferably more than 1/2 and less than 3/4. In addition, when α is 2/3, the angular velocity ω shown in the equation (1) follows the square-third rule that is a psychophysical empirical rule obtained from observation of how to move the human hand. The equation (2) using the angular velocity ω shown in the equation (1) also follows the square law. As a result, when the object is moved at the speed v calculated by the equation (2), the object moves smoothly as it approaches the movement of a person. When α is 1/3 or less, the speed is too small even in a place where the curvature is not so large. When α is 1, the speed is constant regardless of the curvature, and α exceeds 1. Is not suitable because the curvature increases, that is, the speed increases as the curvature radius r decreases.

Further, in the speed calculation unit 13, when the speed v _d (s) exceeds the maximum speed v _max of the object input to the initial value input unit 11, the speed v _d (s) is less than or equal to the maximum speed v _max (for example, Maximum speed v _max ). Thereby, the speed at which the object moves can be suppressed, and in particular, the speed of the object is slowed in a place where the curvature c is large, that is, a place where the curvature radius is small, thereby preventing deviation from the locus due to centrifugal force. Can do. In this way, a graph of the path length s and the speed v _d (s) is obtained as shown in FIG.

(4th process)
The speed calculated by the speed calculation unit 13 is output from the output unit 14 to the control unit that controls the movement of the object. Since the control unit of the object moves the object at a given speed, the object can move quickly and accurately along the trajectory A. The trajectory is generated as described above.

(First correction method)
As shown in FIG. 4, at the start point and the end point, acceleration / deceleration is performed regardless of the value of curvature. Further, even when the locus A is bent sharply, that is, where the curvature c is sharply increased (middle point M), the rate of change (acceleration) of the speed of the object becomes large, and rapid acceleration / deceleration must be performed. . Here, the rate of change of the speed of the object is acceleration, and is indicated by the slope of the graph in FIG.

In such a place where the object suddenly accelerates or decelerates, the object may idle on the trajectory and slow down, or it may deviate from the trajectory due to centrifugal force. Correct the front and rear speeds where the rate of change of speed is large so that the object can move quickly and accurately on the trajectory by making it below the predetermined value (allowable value) (ie, the slope is small in FIG. 4). To do. Note that the predetermined value of the rate of change in speed differs depending on the object and also on the locus set for the same object. The predetermined value of the rate of change (acceleration) of the speed can exceed 10 G (G indicates gravitational acceleration) in a machine tool or the like, but is preferably within a few G in a vehicle or the like on which a person rides.

Therefore, in the third step, the speed calculation unit 13 provides the acceleration / deceleration section B before and after the place where the rate of change of the speed v _d (s) is large (the place where the inclination is large in FIG. 4), and sudden acceleration / deceleration is performed. To prevent it from happening. In the acceleration / deceleration section B, for example, as shown in FIG. 5 (A), the speed v _d (s) may be converted by a polynomial to be corrected to an S shape, and as shown in FIG. 5 (B), The velocity v _d (s) is corrected linearly to prevent sudden acceleration / deceleration of the object. As a result, the object can move quickly and accurately on the trajectory.

(Second correction method)
Further, as shown in FIG. 3, at the intermediate point M where the curvature c on the locus A is steeply increased, the rate of change of the curvature of the locus A, that is, the inclination of the graph is increased. In such a place, as described above, the rate of change of the velocity v _d (s) of the object also increases, and the object is suddenly accelerated or decelerated, so that the object idles on the trajectory or deviates from the trajectory. Thus, it cannot move quickly and accurately on the trajectory. Therefore, in the second step, the curvature calculator 12 corrects the curvature c so that the curvature change rate, that is, the gradient of the curvature c becomes gentle, and then in the third step, based on the corrected curvature c, The speed of the object is calculated so that the object can be moved quickly and accurately on the trajectory without sudden acceleration / deceleration. Note that the maximum value of the rate of change of curvature that achieves such a speed is referred to as a predetermined value. Here, since the relationship of the above equation (7) is established between the rate of change in speed and the rate of change in curvature, the predetermined value of the rate of change in curvature is obtained from the predetermined value of the rate of change in speed described above. be able to.

Further, the speed calculation unit 13 calculates the tangential speed v _d (s) at each point X on the trajectory A based on the obtained curvature c (s) by the equation (7), and the speed v _d ( When s) exceeds the maximum velocity v _max of the object input to the initial value input unit 11, the velocity v is set to v _max or less. As a result, as shown in FIGS. 6A and 6C, when the curvature c (s) is corrected to an S shape, the speed v _d (s) becomes an S shape, and FIG. As shown in (D), when the curvature c (s) is corrected to a linear shape with a large inclination, the velocity v _d (s) becomes a linear shape, preventing sudden acceleration / deceleration of the object, The object can be moved quickly and accurately on the trajectory.

Next, a simulation was performed in order to confirm the effect of the present invention.
As shown in FIG. 7A, a weight of the object 23 is used by using an object 23 having a carriage 20, a spring 21 provided on the upper part of the carriage 20, and a weight 22 attached to the tip of the spring 21. The tangential velocity V _d (s) at each point on the trajectory K is obtained so that 22 passes along the trajectory K shown in FIG. 8, and the object 23 is moved at the obtained velocity V _d (s). The trajectory K ′ was obtained.

As shown in FIG. 7B, the object 23 can be approximated to a secondary system of a mass point, a spring, and a damper, and the equations of motion are expressed by equations (11) and (12). Here, for example, when the carriage 20 is a machine tool, the movement path of the carriage 20 can be set as a preset path K, and the movement path of the weight 22 can be set as a path K ′ where the object 23 actually moves. Further, when the carriage 20 is a vehicle that moves on a preset trajectory K, the weight 22 can be a human being on the vehicle, and the movement path of the person at this time is represented by the trajectory K ′.

A trajectory K through which the carriage 20 of the object 23 passes is given as a Bezier curve (B-spline function). The Bezier curve is created by designating the end points x ₁ and x ₃ and the control point x _{2 in the} middle thereof, and is expressed as in equation (13). The locus K is created by combining a plurality of, for example, three Bezier curves. The coordinates of each point on the created locus K were input to the initial value input unit 11 as an array of x and y coordinates from the start point. Further, the maximum speed v _max of the moving body 23 (that is, the carriage 20) is input to the initial value input unit 11.

The coordinate data of the trajectory K and the maximum velocity v _max input by the initial value input unit 11 are sent to the curvature calculator 12 and the velocity calculator 13, respectively. First, the curvature calculator 12 assumes that the carriage 20 moves on the trajectory K every time, and calculates the tangential velocity v (t) by the equation (3) and the curvature c (t) by the equation (4). calculate. Here, since the object 23 is moving on the locus K at a constant speed, it is considered that the obtained curvature c (t) is a function of the path length s. Accordingly, the curvature c (s) is obtained from the equation (6), and the result is shown in FIG. 9 (shown as before correction in FIG. 9).

As shown in FIG. 9, the obtained curvature c (s) has two places that are steeply increased. Here, the rate of change of the curvature of the locus K becomes a predetermined value or more, and the object 23 Since the change in the speed v (s) becomes large and rapid acceleration / deceleration has to be performed, correction is performed as follows. First, let c ₁ (i) be curvature data before correction. Here, i represents a sample point of the data, and ranges from 1 to the number N of samples. First, c ₂ (i) = c ₁ (i) is set, and the equation (14) is calculated while i is increased from 2 to N.
c ₂ (i) = βc ₂ (i−1) (14)
Here, β is an attenuation constant, and 0 <β <1.

If c ₂ (i) <c ₁ (i), then c ₂ (i) = c ₁ (i). Next, c ₃ (i) = c ₂ (i) is set, and the equation (15) is calculated while i is decreased from N−1 to 1.
c ₃ (i) = βc ₂ (i + 1) (15)
If c ₃ (i) <c ₂ (i), then c ₃ (i) = c ₂ (i). By performing such processing, when β is large, the rate of change of curvature can be made gentle, and the corrected curvature c (s) can form a gentle base as shown in FIG. did it.

The corrected curvature c (s) is sent to the speed calculation unit 13, and the tangential speed v _d (s) corresponding to the curvature c is calculated by equation (7). The result is shown in FIG. Here, with reference to the maximum speed v _max of the object 23 sent from the initial value input unit 11, correction is performed to the maximum speed v _max portions exceeding the maximum velocity v _max.
Further, the corrected velocity v _d (s) is output from the output unit 14 to the control unit (not shown) of the object 23, and the calculation by the computer is performed when the carriage 20 of the object 23 is moved. FIG. 11 shows the locus K ′ of the weight 22 at that time.

As a comparative example, the trajectory K ″ through which the weight passes when the object is moved at a speed obtained by the conventional trapezoidal speed control is calculated by a computer. In the comparative example, when the object 23 starts moving. When the speed of the object 23 reaches a predetermined speed (maximum speed v _max ), the acceleration is constant, and when the object 23 reaches the predetermined speed, the acceleration is zero, that is, the speed is constant (see FIG. As shown in Fig. 11, when the speed of the object 23 is controlled by the comparative example, the weight 22 of the object 23 passes on the locus K ".

As shown in FIG. 11, in the simulation to which the speed control method of the object of the present invention is applied, the vibration of the weight 22 is small and close to the locus K given first, but the conventional trapezoidal speed control method is applied. In this case, the vibration of the weight 22 is increased.

The present invention is not limited to the above-described embodiment, and can be changed without changing the gist of the present invention. For example, some or all of the above-described embodiments and modifications are possible. A case where the speed control method of the present invention is configured by combining the above is also included in the scope of rights of the present invention.
For example, in the object speed control method of the above-described embodiment, the object to be applied is a chassis with a weight attached via the tip of a robot hand and a spring, but the object moves in a curved line, for example, It can also be applied to transport vehicles such as transport carts and trackless carts that do not move on rails.

1 is a block diagram of a speed control device according to an embodiment of the present invention. It is explanatory drawing which shows the locus | trajectory of the object in the speed control method which concerns on one embodiment of this invention. It is a graph which shows the relationship between the path | route length of an object and curvature by the same speed control method. It is a graph which shows the relationship between the path | route length of an object and the tangential speed in the same speed control method. (A), (B) is explanatory drawing at the time of correct | amending the tangential speed by the same speed control method, respectively. (A)-(D) is explanatory drawing at the time of correcting the curvature by the same speed control method, respectively. (A), (B) is the conceptual diagram of the object in the simulation which applied the same speed control method, respectively, and explanatory drawing of a secondary system. It is a graph which shows the locus | trajectory of the object previously given to the speed control apparatus of the object. It is a graph which shows the calculation result of a curvature. It is a graph which shows the calculation result of the speed of an object. It is a graph which shows the locus | trajectory at the time of moving an object.

Explanation of symbols

10: Object speed control device, 11: Initial value input unit, 12: Curvature calculation unit, 13: Speed calculation unit, 14: Output unit, 20: Dolly, 21: Spring, 22: Weight, 23: Object

Claims (9)

A method for controlling the speed of an object passing through a preset trajectory,
A first step of inputting coordinate data of the trajectory;
A second step of calculating a curvature at each point on the locus;
A third step of calculating the velocity of the object at each point from the calculated curvature at each point according to equation (2) using an angular velocity represented by equation (1);
And a fourth step of outputting the calculated speed at each point to the object.

The speed control method according to claim 1, wherein a speed of each point on the locus of the object is calculated to generate a trajectory of the object. 3. The speed control method according to claim 1, wherein a maximum speed of the object is input in the first step, and each point calculated by the expression (2) in the third step is set. When the speed data of the object exceeds the maximum speed, the speed of the object is corrected to be equal to or less than the maximum speed. The speed control method according to any one of claims 1 to 3, wherein in the third step, the speed of the object is set so that a rate of change of the speed of the object according to the formula (2) is a predetermined value or less. A speed control method characterized by correcting the above. The speed control method according to any one of claims 1 to 3, wherein in the second step, the curvature at each point on the locus is corrected so that the rate of change of the curvature is a predetermined value or less. Thereafter, the speed of the object is calculated from the corrected curvature in the third step. The speed control method according to claim 1, wherein α is 2/3 in the formula (2). A speed control method using the speed control method according to any one of claims 1 to 6 for controlling a speed of a robot arm or a transport vehicle. A speed control device for an object passing through a preset trajectory,
An initial value input unit for inputting the coordinate data of the locus;
A curvature calculator for calculating the curvature at each point on the locus;
A velocity calculation unit that calculates the velocity of the object at each point from the calculated curvature at each point according to equation (2) using an angular velocity represented by equation (1);
And an output unit that outputs the calculated speed at each point to the object.

9. The speed control apparatus according to claim 8, wherein the output unit outputs the calculated speed at each point to a robot arm or a transport vehicle.

Images