JPH061405B2 - Position control device - Google Patents

Description

Description: TECHNICAL FIELD The present invention relates to a device for controlling a controlled object such as a manipulator, and in particular, includes an arcuate trajectory when an operation path is changed in real time. The present invention relates to a control device improved so as to operate smoothly.

(Prior Art and Problems to Be Solved) FIG. 8 shows that when moving from the departure point Ps to the arrival point P _E ^* ,
In way of movement is an explanatory diagram of the case where the target arrival point was changed to P _E. In this case, since it is difficult to smoothly change the target position, the target position is once stopped to eliminate the influence of the previous operation, and then the movement to the new arrival point is started again. The initial trajectory in the figure is called the front trajectory, and the trajectory reaching the updated target point is called the rear trajectory.

However, since the operation is stopped, the operation time becomes long, and fine operation control cannot be performed.

If an obstacle is interposed between the departure point and the arrival point, an arc motion may be inserted for detouring. In this case, if the target position is changed, more complicated processing is required. There was a problem.

(Object of the Invention) The present invention solves such a problem, and when the manipulator receives a command to change the target position during operation, it smoothly responds in real time without stopping to operate the manipulator. An object is to provide a control device that changes a trajectory.

(Means for Solving Problems) The present invention which achieves such an object is a position control device for controlling the operation of a controlled object according to a command from the outside, in which the original target position and trajectory are changed from the outside. Target position storing means for storing the commanded target position and trajectory to be updated, and presently storing the position information of the controlled object when the updated target position is received and the initial target position. Position storage means, virtual waypoint calculation means for calculating the position to decelerate from this current position and stop on the original orbit, track portion from the virtual waypoint to the updated target position, and the current position And a path section to smoothly connect the track section to the virtual waypoint in accordance with a predetermined procedure, and a renewed target position from the current position via the path curve trajectory. And a control means for moving the controlled body.

(Operation) The virtual waypoint calculation means determines a virtual waypoint serving as a reference for correcting the trajectory to the updated target position by calculation. The trajectory calculating means calculates a via trajectory for smoothly moving to the updated target position as an eighth-order spline curve by a predetermined procedure based on the virtual waypoint and trajectory information.
The control means appropriately controls the speed of the controlled object to move it from the current position to the target position via the via track.

(Example) The present invention will be described below with reference to the drawings.

FIG. 1 is a configuration block diagram showing an embodiment of the present invention. In the figure, 1 is a target position receiver for storing a target position P _E commanded from the outside such as a host computer by communication or the like and whether the trajectory is a circular arc or a straight line, and 2 is a target position receiver 1 for the initial target position P _E. When the target position is updated when _E ^* is stored, the initial target position register 3 that stores the initial target position and trajectory is stored in the robot arm when the target position receiver 1 receives the updated target position. The current position storage means for storing the position information stores the current position P _N , the moving direction and the velocity Vs. 4 is a virtual route point calculating means for calculating a position to stop on the original track is decelerated from the current position (hereinafter referred to as "virtual waypoints"), obtains a virtual waypoint P _m by the following equation, for example, straight track .

_{P m = P N + Dd ×} (P E * -P N) / | P E * -P N | (1) Dd = 3Vs 2 / (4 · Amax) here, Dd is necessary to stop decelerating Distance, Amax
Is the maximum acceleration that occurs when the manipulator moves.

Reference numeral 5 is a transit trajectory calculating means for smoothly connecting the updated trajectory from the target position to the virtual transit point and the trajectory from the current position to the virtual transit point, which will be described in detail later.

Reference numeral 6 denotes a target change trajectory generator that calculates the position to move the manipulator from the current position to the updated target position via the via trajectory, and calculates when the manipulator exists on the via curve obtained by the via trajectory calculating means 5. Reference numeral 7 denotes a normal trajectory generator that calculates the position of the manipulator with respect to a normal trajectory that performs linear motion, and 8 is a control unit that switches the target change trajectory generator 6 and the normal trajectory generator 7 to control the manipulator.

The operation of the apparatus thus configured will be described below. Second
The figure is a plan view of the movement of the manipulator, and FIG. 3 is an explanatory view of the moving speed of the manipulator.

Initially, the manipulator starts moving from the starting point Ps to the initial target position P _E ^* and reaches a predetermined maximum speed Vmax. When the target position is updated on the way, P _E and the virtual change point P _m are calculated, and the 8 th spline obtained by a predetermined procedure for smoothly connecting the first movement and the updated movement based on this is calculated. Find the curve. On this via orbit, the manipulator first reduces the speed and then accelerates to move so as to be the same before and after the connection point (t = 0, T _p ). After that, the robot arm stops at the updated target position.

Next, a detailed configuration of the route trajectory calculation means 5 will be described. Fourth
The figure is a block diagram showing the internal structure of the route trajectory calculation means 5. In FIG. 4, the same reference numerals are given to those having the same functions as those in FIG. 1, and the description thereof will be omitted.

In the figure, reference numeral 51 receives position information from the position registers 1, 2, and 3, and the distance | P _N P _m | on the track from the current position to the virtual waypoint and the distance on the track from the virtual waypoint to the target point | P _m P _E | at half distance comparing means for selecting either a short movement distance, the moving distance M _in (|
P _N P _m · 1/2 | P _m P _E |) and the stationary distance l are compared, and the smaller one is calculated as the reference distance l _o .

l _o = M _in (| P _N P _m · 1/2 | P _m P _E |, l) (2) 52 is an auxiliary point calculating means for calculating an auxiliary point necessary for calculation of a transit trajectory. The selection of this auxiliary point will be described with reference to FIG. FIG. 5 shows a specific example in which the initial trajectory is a linear trajectory (front trajectory) and the updated trajectory is an arc trajectory (rear trajectory). In the figure, P ₂ = P _m .

The auxiliary point calculation means 52 selects the first auxiliary point P _o at a point on the previous trajectory that is distant from the virtual via point by the reference distance l _o . In many cases, P _o coincides with the current position P _N , but it becomes another point when the updated target position P _E and the virtual waypoint P _m are close. Next, the second auxiliary point P ₄ is selected on the rear trajectory at a point separated from the virtual via point P _m by the same reference distance l _o as the first auxiliary point P _o . Next, a third auxiliary point P ₁ is provided between the first auxiliary point P _o and the virtual waypoint P _m.
Determine. When the front trajectory is a straight line, the middle point of the first auxiliary point P _o and the virtual waypoint P _m is set as the third auxiliary point P ₁ . When the front trajectory is a circular arc, the direction of the third auxiliary point P ₁ is determined so as to contact the circular arc at the first auxiliary point P _o .

_{O 1 P o ⊥P o P 1} (3) Distance from the third auxiliary point P _o of the auxiliary point P ₁ is, half the distance between the route point P _m and the first auxiliary point P _o on the circular arc track Take Finally, a fourth auxiliary point P ₃ is defined between the second auxiliary point P ₄ and the waypoint P _m . When the rear trajectory is a straight line, the midpoint of the second auxiliary point P ₄ and the waypoint P _m is set as the fourth auxiliary point P ₃ . Rear trajectory if an arc defining a fourth direction of the auxiliary point P ₃ in contact with the arc in the second auxiliary point P _4.

O ₂ P ₄ ⊥ P ₃ P ₄ (4) The distance between the fourth auxiliary point P ₃ and the second auxiliary point P ₄ is the distance between the second auxiliary point P ₄ and the waypoint P _m on the circular arc trajectory. Take half the distance.

Reference numeral 53 denotes the points P _o , P ₁ , P ₂ (= P _m ), P obtained by the auxiliary point calculation means 52.
A route trajectory calculating means for calculating a route trajectory having the same velocity and zero acceleration at points P _o and P ₄ using ₃ and P ₄ , for example, an 8th order spline curve such as an 8th order Bezier curve. Establish.

The normal trajectory generator 6 is the first from the current position P _N in the previous trajectory.
Up to the auxiliary point P _o and the speed pattern from the second auxiliary point P ₄ in the rear trajectory to the target point P _E , for example, by using a cubic formula with respect to time, the positions P _N , P _o , P ₄ , P
The acceleration at _E is zero.

The target update command is accepted only when the acceleration at the current position P _N is zero.

If a straight trajectory is specified in the front trajectory, the initial target position P _E ^* is stored in the register 2, and if an arc trajectory is specified, the initial target position P _E ^* and another point on the arc. Is indicated so that the center O ₁ and radius R _{1 of} the circular arc trajectory can be determined. In the case of a straight track also in the rear track, the target position P _E is indicated, and in the case of an arc track, the updated target position P _E and another point on the arc are stored in the register 1 and the center O _{2 of the} arc track and The radius R ₂ can be determined. In addition, the predetermined maximum velocity Vmax unique to the operation of the manipulator 10 and the stationary distance l required to stand still from this maximum velocity Vmax are stored in another register,
Call and use as appropriate.

FIG. 5 is a diagram for explaining the operation of the manipulator 10 by the apparatus of FIG. 1, in which the front trajectory is a straight line and the rear trajectory is an arc.

In the figure, P ₅ is a position that accelerates from the current position P _s or virtual waypoint P _m to reach the maximum speed Vmax, and P ₆ decelerates from the maximum speed Vmax and stops at the target position P _E or the virtual waypoint P _m . This is the deceleration start position.

This is a case where the target position P _E is updated to a point sufficiently distant from the current position P _N after reaching the predetermined maximum speed Vmax. In this case, since the current position P _N and the first auxiliary point P _o coincide with each other, the route trajectory calculation means 5 uses the first to fourth auxiliary points to determine the 8th-order Bezier curve among the 8th-order spline curves. Orbits that can be represented by (points P _o , P ₁ , P _m , P ₃ , P ₄ and these midpoints are defined as a total of 9 points, but 4 of the midpoints can be determined with 5 given points, so after all it will be assisted. point computing means 42 is determined by the auxiliary point 4 and the virtual waypoint P _m obtained.) to move as the top of the next.

P (T) = K ₁ (t / T) ⁸ + K ₂ (t / T) ⁷ + K ₃ (t / T) ⁶ + K ₄ (t / T) ⁵ + K ₅ (t / T) ⁴ + K ₆ (t / T) ³ + K ₇ (t / T) + P _o (O ≦ t ≦ T) (5) where And Here, V _o is the initial velocity at the first auxiliary point P _o .

That is, the operation is performed from P _o to P ₄ in the same time required to move at a constant speed to P _o , P _m , and P ₄ without stopping at the waypoint P _m . Also, | P ′ (o) | = | P ′ (T) | = Vmax (7) | P ″ (o) | = | P ″ (T) | = O (8), so the connection point P _o , P ₄ has zero acceleration, the velocities are smoothly connected, and the absolute values of velocities are equal.

From position P ₄ to position P ₆ , constant velocity motion of speed Vmax is performed.

From the position P ₆ to the target position P _E , the vehicle is decelerated from the maximum speed Vmax and stopped. The speed and the moving distance are given by the following equations.

V ₂ (t) = a ₂ t ³ + b ₂ t ² + c ₂ t + d ₂ (9) Here, T _s1 is the acceleration time, and is given by the following equation, where Amax is the maximum value of the acceleration.

T _s1 = (3/2) ・ Vmax / Amax (10) l = (3/4) ・ V ² max / Amax (11) <Explanation of Bezier curve> Fig. 6 shows the speed to move on the 8th-order Bezier curve. And the generated acceleration. As shown in Eq. (7), the velocity is smoothly connected to the anterior-posterior orbit and is smaller than the traveling velocity on the anterior-posterior orbit on the transit orbit. As shown in Eq. (8), acceleration acts because it moves on a curve, but it is zero at the connection point.

Next, the velocity P ′ (t) and the acceleration P ″ (t) of the eighth-order Bezier curve of the equation (5) are given by the following equations.

Then, the speed and acceleration at t = O and T are calculated as follows.

P ′ (o) = 4 (P ₁ -P _o ) / T (14) P ′ (T) = 4 (P ₄ -P ₃ ) / T (15) P ″ (o) = P ″ (T) = 0 (16) Therefore, if P ₁ P _o = P ₃ P ₄ , the speeds at both points match.

<Distance on Circular Orbit> FIG. 7 illustrates how to obtain the moving distance on the orbit by interpolating between the start point and the end point of the circular orbit. In the figure, O ₁ is the center, R ₁ is the radius, P _o is the start point, P ₁ is the end point, P (t) is the point to be interpolated on the arc trajectory, and ₁ is the normal vector of the plane formed by the arc. Θ around the normal line passing through the center O ₁ of the point P _o
1 turn corresponds to point P ₁ .

The moving distance d (t) on the arc can be set in the same manner as the linear interpolation formula (10).

d (t) =-0.5 × VsTs ₁ (t / Ts ₁ ) ⁴ + VsTs ₁ (t / Ts ₁ ) ³ (17) Therefore, the rotation angle θ (t) can be expressed by the following equation.

θ (t) = d (t) / R (18) Then, an arbitrary point P (t) on the arc is expressed as P (t) = R ( ₁ , θ (t)) · P _o (19) It Here, R ( ₁ , θ (t)) is a well-known generalized rotation matrix, and is represented by the following equation where ₁ ^t = (n ₁ , n ₂ , n ₃ ).

r ₁₁ = n ₁ ² + (1-n ₁ ² ) cos θ (t) r ₁₂ = n ₁ n ₂ (1-cos θ (t))-n ₃ sin θ (t) r ₁₃ = n ₁ n ₃ (1- cos θ (t)) + n ₂ sin θ (t) r ₂₁ = n ₁ n ₂ (1-cos θ (t)) + n ₃ sin θ (t) r ₂₂ = n ₂ ² + (1-n ₂ ² ) cos θ ( t) r ₂₃ = n ₂ n ₃ (1-cos θ (t))-n ₁ sin θ (t) r ₃₁ = n ₁ n ₃ (1-cos θ (t))-n ₂ sin θ (t) r ₃₂ = n ₂ n ₃ (1-cos θ (t)) + n ₁ sin θ (t) r ₃₃ = n ₃ ² + (1-n ₃ ² ) cos θ (t) In the above embodiment, the manipulator is described as an example.
The present invention is not limited to this, and may be a numerical control device, an XY stage, a plotter, or the like as long as it controls at least two axes.

Also, the positional relationship on the plane is shown, but the same applies to a three-dimensional space.

Although an 8th-order Bezier curve is shown as a curve, other spline curves can be used as long as the curve is a curve that is connected to a straight line with 0th-order and 1st-order differential coefficients being continuous, and 2nd-order differential coefficient being zero. Alternatively, it may be expressed by a polynomial of 9th order or higher. The term "spline curve" as used herein means a curve defined by a plurality of points when a plurality of points are given.

<Effects of the Invention> As described above, the present invention has the following effects.

(A) Since the target position and route can be changed in real time by sensor input or commands from the interrupt processing program, manipulator operation that flexibly responds to changes in external conditions can be realized.

(B) The ability to change the target position and the distinction between linear and circular trajectories in real time, when applied to a manipulator,
Useful for a wide range of applications.

(C) Since the route orbit changes the speed smoothly, unnecessary force is not applied to the manipulator, and smooth operation without vibration or sound can be performed.

[Brief description of drawings]

FIG. 1 is a configuration block diagram showing an embodiment of the present invention, and FIG.
FIG. 4 is an explanatory diagram when the device of FIG. 1 selects an auxiliary point, FIG. 3 is an explanatory diagram of the moving speed of the manipulator, and FIG. 4 is a block diagram showing the details of the route trajectory calculation means 5, and FIG. Is an explanatory view of a specific example of selecting an auxiliary point, FIG. 6 is an explanatory view of velocity and acceleration on a Beschwe curve, FIG. 7 is an explanatory view of an interpolation method of an arc trajectory, and FIG. 8 is an explanatory view of a conventional example. is there. 1 ... Target position recording means, 2, 3 ... Current position storage means, 4
... virtual waypoint calculation means, 5 ... via trajectory calculation means, 6,
7, 8 ... Control means. Ps ... the starting point, Pn ... current position, P _m ... virtual via point, P _E ^* ...
Original target position, P _E ^* … Updated target position, P ₀ , P ₁ , P ₃ ,
P ₄ … Auxiliary point.

Continuation of front page (56) Reference JP-A 61-265287 (JP, A) JP-A 61-157909 (JP, A) JP-A 61-97708 (JP, A) JP-A 59-231608 (JP , A) JP 59-36811 (JP, A) JP 58-195208 (JP, A) JP 58-195209 (JP, A) JP 59-68014 (JP, A) JP 58-3002 (JP, A) JP-A-60-262213 (JP, A) "NC software", edited by Tsuyoshi Kishi, 1972, published by Industrial Research Board, page 198.

Claims (1)

[Claims] 1. A position control device for controlling the operation of a controlled object according to a command from the outside, which stores that the target position and the trajectory updated in place of the original target position and the trajectory are commanded from the outside. Target position storage means, current position storage means for storing the position information of the controlled object and the initial target position when the updated target position is received, and the initial trajectory decelerated from the current position. A virtual waypoint calculating means for calculating a position to stop at above, a track portion from the virtual waypoint to the updated target position, and a route for smoothly connecting the current position and the track portion to the virtual waypoint. It is characterized by comprising a routed trajectory calculation means for computing a trajectory according to the following, and a control means for moving the controlled object from the current position to the updated target position via the routed curved trajectory. Position controller according to. Note: The distance on the trajectory from the current position to the virtual waypoint, or the half of the distance on the trajectory from the virtual waypoint to the updated target point, whichever is shorter, or the movement is specified. Comparing the magnitude with the stationary distance from the predetermined maximum speed to the stationary state, and calculating whichever is smaller as the reference distance. A first auxiliary point is provided at a position distant by a reference distance from the virtual waypoint to the current position on the orbit, and a first auxiliary point is provided at a position distant from the virtual waypoint on the track by the reference distance on the side of the updated target position. Compute 2 auxiliary points. When the trajectory connecting the current position and the virtual waypoint is a straight line, the middle point between the first auxiliary point and the virtual waypoint, and when the trajectory connecting the current position and the virtual waypoint is an arc, the first assistance A third point is a point separated from the first auxiliary point to the virtual via point side by the distance on the trajectory between the midpoints of the point and the virtual via point, and the point on the straight line tangent to the arc at the first auxiliary point. Compute the auxiliary points of. When the trajectory connecting the virtual waypoint and the updated target point is a straight line, the middle point between the second auxiliary point and the virtual waypoint, and the trajectory connecting the virtual waypoint and the updated target position is an arc. Is a point distant from the second auxiliary point to the virtual intermediate point by the distance on the trajectory between the second auxiliary point and the virtual intermediate point, and is a straight line tangent to the arc at the second auxiliary point. Compute the fourth auxiliary point at the upper point. To calculate an eighth-order spline curve determined in this order by the first and third auxiliary points, the virtual waypoint, and the fourth and second auxiliary points.