JPS63120306A - Manipulator control device - Google Patents

Abstract

PURPOSE:To ensure a smooth passing action at a point near a pass point by securing connection in the length decided by the acceleration characteristics among a pass locus, an arc locus and a linear locus or between arc loci and at the same time changing smoothly the speed with the pass locus. CONSTITUTION:In a controller 40, a distance comparing means A41 receives the present position information, the pass information and the target information from registers R41-R43 respectively. Then, the controller 40 selects the shorter one of the locus distance between the present position Ps and a pass point Pm and that between the point Pm and a target point PE. An auxiliary point arithmetic means 42 sets an auxiliary point P0 on said selected locus at a point distant away from the point Pm by a prescribed condition amount together with an auxiliary point P4 set at a point distant from the point P0 by said condition amount. Then an auxiliary point P3 is set between both points P4 and Pm. A front/back locus speed arithmetic means A43 calculates a speed pattern. A pass locus arithmetic means uses each point obtained by an auxiliary point arithmetic means A42 to decide an 8-degree spline curve. A selector A45 uses both means A43 and A44 to have output of a locus to a drive circuit 50.

Description

Detailed Description of the Invention (Industrial Application Field) The present invention relates to a device for controlling a manipulator,
In particular, the present invention relates to a control device that is improved so as to perform all operations smoothly connected to the arcuate trajectory when moving to a target position tK including the arcuate trajectory.

(Prior Art and Problems to be Solved) FIG. 7 is an explanatory diagram of the case of moving from a starting point P8 to an arrival point P9 via a way point Pmf. Below, the trajectory from the starting point Pg or current position fPs to the way point Pm is the previous trajectory, and the way point P
The trajectory from rn to the arrival point PE or target point PE is called a backward trajectory.

Conventionally, it was difficult to control the front track and the back track to smoothly connect, so the train stopped at the way point Pm and moved on the back track again.

However, since it involves stopping, there are problems in that the operation time becomes long and precise operation control cannot be performed.

In addition, it is also possible to connect the forward and backward orbits by an arc that touches both orbits near the way point Pm, but since the curvature changes suddenly at the connection point, a large force acts on the manipulator, making it useless. This may cause vibrations, etc.

(Objective of the Invention) The present invention solves these problems, and allows for smooth transit operations in the vicinity of transit points, as well as efficient operation even if the operation connected to the transit operation is a straight trajectory or an arcuate trajectory. It is an object of the present invention to provide a control device for controlling the movement of a mechanical manipulator.

(Means for Solving the Problems) The present invention that achieves these objectives is the current state of the manipulator! The present invention relates to a device that moves to an externally specified target point via all of the externally specified route points, and includes an arcuate trajectory in at least one of the front trajectory and the rear trajectory.

The comparison means determines whether the travel distance of either the front orbit or the rear orbit, whichever is shorter, is sufficient to move at a predetermined maximum speed by comparing it with the stationary distance.

If the moving distance is too short to reach the maximum speed, the auxiliary point calculating means separates from the Fi way point by half of the moving distance and sets a first auxiliary point on the front trajectory side and a second auxiliary point 1 on the back trajectory side.
Select. If the moving distance is longer than the static distance, the first auxiliary point is set as a far point on the front orbit side and the second auxiliary point is set on the rear orbit side as far away from the way point by the static distance. If the front trajectory is a straight line, select the third auxiliary point at the midpoint between the first auxiliary point and the waypoint, and if the front trajectory is a circular arc, select the third auxiliary point on the straight line tangent to the arc at the first waypoint, A point separated from the first way point by a distance corresponding to half the distance between the first way point and the way point on the arc is selected as the third auxiliary point.

If the trailing trajectory is a straight line, select the fourth auxiliary point as the son of the second auxiliary point and the waypoint, and if the trailing trajectory is a circular arc, select the fourth auxiliary point on the straight line tangent to the arc at the second waypoint. second on the arc
A point away from the second way point by a distance equivalent to half the distance between the paper origin point and the way point is selected as the fourth auxiliary point.

The transit trajectory calculation means touches the previous trajectory at the first auxiliary point, and the fifth auxiliary point touches the previous trajectory.
auxiliary point, transit point 9 A transit trajectory is calculated that is defined by an 8th order spline curve that passes near the fourth auxiliary point and touches the trailing trajectory at the second auxiliary point.

(Operation) The 8th order spline curve is determined using the first to fourth auxiliary points and way points, and the acceleration is zero at the same speed at the first auxiliary point and the second auxiliary point.

Therefore, the transition from the front orbit to the transit orbit proceeds smoothly, and no unnecessary force is generated when entering the trailing orbit from the transit orbit.

The comparison means indirectly calculates the moving speed of the manipulator at 8t41 and the second auxiliary point, and the auxiliary point calculation means calculates the entire route trajectory adapted to the current position, designated waypoint, designated target point, and designated maximum speed. Calculate supplementary points for

(Example) The present invention will be explained below using the drawings.

FIG. 1 is a block diagram showing an embodiment of the present invention. In the figure, 10 is a manipulator consisting of a joint mechanism with 6 degrees of freedom, and 20 is a host convenience store that transmits information necessary for the operation of the manipulator 10. Specify. A position detector 30 detects the current position Pa of the manipulator 10. For example, each rotation angle of a joint is detected using an encoder and calculated. A controller 50 controls the position of the 40fi manipulator 10, and 50 is a drive circuit that moves the manipulator 10 in accordance with commands from the controller 40.

R41 is the current position PM? output from the position detector 30. The storage register R42 is a register that stores information regarding the way point commanded by the host controller/computer 10. When linear motion is specified, the way point Pm is stored, but when arc movement is specified, the way point Pm is stored. and one other point is stored as the center 0 of the arc trajectory. and radius R1 can be determined. R43 is a register that is commanded by the host controller/computer 10 and stores information regarding target point b. When linear interlocking is specified, target point P is stored, but when circular movement is specified, target point P2 and other One point is stored so that the center 02 and radius R2 of the arc trajectory can be determined. Note that a predetermined maximum speed 3 V max specific to the operation of the manipulator 10 and a static pressure w=ttri required from this maximum speed V max until it comes to a standstill are stored in separate registers, and are recalled and used as appropriate.

A41 receives position information from registers R41, R42゜R44, and selects either the distance I Ps Pm l on the orbit from the current position to the waypoint, or the distance I Pm P E I on the orbit from the waypoint to the target point. The distance comparing means for selecting a short moving distance compares the moving distance Min (I Ps Pn+1.l PmPg) with twice the resting distance t, so that the overall process is different.

Min N ps Ptn1. l Pm pEl, 2
t) (11A42 is the total transit orbit $'1
．． This is an auxiliary point calculation means that calculates the auxiliary points necessary for. The selection of this auxiliary point will be explained based on FIGS. 2 and 3. Fig. 2 shows the criteria for selecting auxiliary points divided by the lengths of the front and rear tracks and the shape of the tracks.

If both the front and rear trajectories in the figure are straight lines, this example may be used as is, but usually the simpler condition is used. It's raining. FIG. 3 shows a specific example using a combination of arc-arc trajectory. In the figure, P2=Pm. The auxiliary point calculating means A42 selects the first auxiliary point PO at a point on the front trajectory that is the following distance away from the way point Pm, and selects the first auxiliary point PO at a point that is the following distance away from the way point Pm on the back trajectory.
A second auxiliary point Po is located at a distance away from the first auxiliary point Po under the same conditions as the first auxiliary point Po.
auxiliary point P4 is selected. This distance is defined as the static distance t when Min(I Pa Pm1.l PmPgl)≧2t, and Mln (l Pg Pm1. I Pm PEI)
A fifth auxiliary point P is defined between <27. If the previous trajectory is a straight line, the first auxiliary point P. and the midpoint of the way point Pm as the third
Let it be an auxiliary point P. If the front trajectory is a circular arc, move the third auxiliary point P so that it is tangent to the circular arc at the first auxiliary point P.
, determine the direction.

op-Lpp (2111
o 1 The distance of the third auxiliary point P from the first auxiliary point P0 is set to half the distance between the first auxiliary point P0 and the waypoint Pm on the circular arc trajectory. Last leg second auxiliary point P4 and way point Pm
A fourth auxiliary point P3 is defined between the two points. If the trailing trajectory is a straight line, set the midpoint between the second auxiliary point P0 and the waypoint Pni as the fourth point.
Let it be an auxiliary point P. If the rear trajectory is a circular arc, move the fourth auxiliary point P so that it touches the circular arc at the second auxiliary point P4.
, determine the direction.

o p-Lp5p4t, 3+ The distance of the fourth auxiliary point P3 from the second auxiliary point P4 is set to half the distance between the second auxiliary point P4 and the waypoint Pm on the circular arc trajectory.

A43 is the first auxiliary point P from the current position ps in the previous orbit. and speed calculation means for calculating a speed pattern from the second auxiliary point P4 to the target point P on the trailing trajectory,
For example, using a cubic equation regarding time, the position t p8°Po,
P4. Acceleration IJ at PE [a- is set to zero.

A44 is the point p0. calculated by the auxiliary point calculation means A42. p.

P 2 (two P m ), P,. Point p using P4. ,
A transit trajectory calculation means that calculates a transit trajectory moving at the same speed and zero acceleration in p4, for example, an 8th order Bezier
An 8th order spline curve such as a (Beshuet) curve is determined.

A45 is the speed calculation means A43 on the front orbit, the transit orbit calculation means A44 on the transit orbit, and the speed calculation means 4 on the trailing orbit.
This is a selector that outputs the trajectory using No. 3, and outputs the trajectory to the drive circuit 50.

Fig. 4 is a perspective view of the operation of manipulator O using the device shown in Fig. 1. In this case, the front trajectory is a straight line and the rear trajectory is an arc. It shows. In Figure 4, (a) is the way point Pry
If there are constant velocity sections on both sides of a, (b) is the waypoint P
If there is a constant velocity section on the side of the current position P of m, C→, if there is a constant velocity section on the target position PE side of the way point Pm, then) ti There is no constant speed section on both sides of the way point Pm. The cases will be shown and explained below.

In the figure, P5 is the position where the maximum velocity V+n+axK is reached by accelerating from the current position Ps or the waypoint Pm, and P6 is the maximum continuous f! The deceleration start position Pa is where the vehicle decelerates from tVmax and stops at the target point P2 or the way point Pm, and Pa is the position where deceleration starts, and then it enters a curved trajectory.

P is the point at which the maximum speed vmaxx,j) also reaches a small maximum speed.

(b) If there are constant velocity sections on both sides of the waypoint, the vehicle will be accelerated to the maximum speed vm*x between the current position P and position P. This speed and moving distance are given by the following equation, for example.

v, (t) = J t' + b, t2 +e, t
+ d, (4)s d, (t) = -(Vmax/ (2Ta, ))
t + (Vmax/Ts, ) t (5) Here, T8. is the acceleration time and the maximum value of acceleration f Arna
If x, Ts, = (3/2) ・Vmax/person m
ax, and the distance t required for acceleration is given by the following equation.

t=0.5VmaxlTs, = (3/4) ・Vm
ax/Am*x f6) From position P5 to position P. Until then, uniform motion with a speed of VmaX is performed.

Connection point P. From to the connection point P4, the trajectory (
Point P. , Pl, Pm, P, , P4 and the midpoints of these are determined by a total of 9 points, but the 4 midpoints are the 5 4 points.
Since the point can be determined by the point, it is finally determined by the four auxiliary points obtained by the auxiliary point calculation means 42 and the way point Pm.

) as follows.

p(t) = K, (t/'r) + K2(t/'r
) To ten, (t/'r)'+ K4(t/T)+ K
, (t/r) + K6(t/11')'+ K7ft
, /'p) + po(o≦t≦T) (
71 Here, T = 41 Po-P, I/V0 (assumed to be 81. Here, K Vofl is the initial velocity at the first auxiliary point P.

That is, P, Pm at a constant speed without stopping at the way point Pm.

P4 and the time required to move # and In[-. From P4
works via. Also, l p/<o = I P'(T) 1. = V
max (9)P”(
0) = P"(T) = 0
01, the connection point p0. At p4, the acceleration is zero, the velocities are smoothly connected, and the absolute values of the velocities are equal.

From position P4 to position t p6, the velocity is constant velocity position [
The speed is decelerated from the maximum speed v max and stopped between P6 and the target position PE. This speed and moving distance are given by the following equation.

P2(t)= a2t +b2t→e2t+d2Qυd
2(t)=(Vmax/(2Ts,))t+(Vmax
/Ts1)t+Vmax 03t = (3/4
) ・Vmax/Amax (1
3 (b) If there is an equidistant point on the current position side of the way point, it accelerates from the current position Ps, reaches the maximum speed v m + ax at a distance t''f & distance fP5, starts decelerating at position P6, and moves through the path Ps. .-P4 and then stops at the target position PE.

Position P. , P4, the speed v2 is in the section p4p.

determined by the length of The motion of section P4P is v
, (t)-a, t +c, t+d,
It is expressed as α. Here is T8. = (6/2)9vE/Am&x. From now on, I P4 PE I = (3/4) ・vg
/personmax, and the speed vE is given by the following equation.

Section P. In P4, the 8th order Bezi according to equation (7)
Move in an er curve. The deceleration start position P6 is determined as follows. Interval p, p. Then, move using the following equation.

P4(t)=a, t +b4t +e4t+d,
ae Therefore, the length of the section PG Po is given by the following equation.

l P6-Pol=(3/4) ・V(Vmax-VB
)/A, max lη (c) If there is a distant cause such as the target position side of the way point, the current position P is 111tP. It accelerates to a speed of vm. This motion is given by the following equation.

Vs(j) IL5t +b5t +c5t+
d5 (tl where velocity [V, 1j
Integrating the a8 formula, it is given by.

Section P. At P4, it moves with an 8th order Bezier curve according to equation (7) of velocity V. In interval p4p,
It accelerates from the speed V, and reaches the maximum speed [v max. The motion in this section is given by the following equation.

When this equation is integrated, the interval length becomes:

I P4 P51 = (3/4) ・(Vmax-V
a )/Amax Position P5 to P6 is a uniform motion, and then it decelerates and stops at the target position @PE is (
It is the same as b).

(b) When there is no equidistant distance on both sides of the way point, the maximum velocity cannot be reached with the allowable acceleration Amax because the travel distance is short. In this case, the motions of (b) and (c) that do not include equidistant causes are combined.

Incidentally, the ninth change in section can be determined based on the elapsed time from the start of the operation or the distance traveled.

FIG. 5 shows the speed of movement on the 8th-order Bezier curve and the generated acceleration. As shown in equation (9), the speed is smoothly connected to the forward and backward orbits, and the moving speed on the intermediate orbit is smaller than the moving speed on the forward and backward orbits.

As shown in the α1 equation, acceleration acts because the object moves on a curve, but it becomes zero at the connection point.

In addition, in the above embodiment, the case where there is one way point is shown, but even if there are multiple way points, each way point 1a4 is regarded as the adjacent point, the current position, and the target position, and a curved trajectory is formed. The speed pattern can be set as appropriate depending on the trajectory conditions rather than stopping.

In addition, although an 8th-order Bezier curve is shown as a curve, other spline curves can be used as long as the 0th-order and 1st-order differential coefficients are continuous, the 2nd-order differential coefficient is zero, and the curve is connected to a straight line. Alternatively, it may be a polynomial of degree 9 or higher. Note that the spline curve here refers to a curve defined by four points when a plurality of points are given.

FIG. 3 explains how to interpolate between the starting point and the ending point of a circular arc trajectory to determine the moving distance on the trajectory. In the figure, 0. is the center, R1 is the radius, PO is the starting point, P is the ending point, p (t) is the interpolated point on the arc trajectory, and n is the normal vector of the plane formed by the arc. The normal line passing through point P0 and center O1 is θ, t, and point P.

matches.

The moving distance d(t) on the circular arc can be set in the same way as in 203 for linear interpolation.

d(t)""-0,5XVs Ts, (t/Ts
, ) '→-Vs Ts, (t/Ts, )'
('') The tile edge rotation angle θ(1) is expressed by the following equation.

θ(t) = d(t)/R@ Therefore, any point p(t) on the arc is P(t)=
R(n, , θ(t))・Po U
Let it rain. Here, R(n,.θ(t))fl is a known generalized rotation matrix, n, (n, * R2+ R5
), it can be calculated using the following formula.

r,, =n," + (1-n,') cosθ<
1) rt2=n1n2(1e08θ(t))-n, s
inθ(1)? , 3 = n, n3 (1-C!O1lθ
(t)) + 12's inθ(1)r21''
n1n2 (I C!08θ(t)) +n, si
nθ(1)r2□=n2+ (1-R2) eosθ(
1) r25 ” n2n5 (1e08θ(t)) - n
, sinθ(1)r, =n, n, (1-co-(
t))-n2afnθ(1)r52 ” n2n3 (
1e(115θ(t)) +n, sinθ(1)r5
5 '''n5' + (1nX) Cogθ (1) Next, the speed P' of the 8th-order Bezier curve in equation (7) is
t) and acceleration P"(t)tj, which are given by the following equations.

P'(t)=8-ft/T)7+7c(t/T)6
+6 drunkenness (t/T) 5T T
T+5 K-! -tt/T)
'+4-! -(t/T)'+3 -! -(t7T)2
1'T
T+end) P"(to56ft/T16+42-so(t/Tl'
+30-! -<t/T+1)2 T2
4 to T2 +20- (t7'r)'+125 (t/T)2+6 to 1'! (t/T)(c)2T2T2 Then, the velocity and acceleration at t=0.7Ic are determined as follows.

” (o)=4 (P + Po) /T
@P'(T)=4 (P4-P,
l/T (to) P”(0)=
P"(T)=o B Therefore, "O" ■)5 If P4 is assumed, the speed of the pixel multiplied by 1 is the same.

(Effect of the invention) As explained above, the present invention has the following effects.

(A) Since the transit trajectory connects the circular arc trajectory and the straight trajectory or the circular arc trajectory with a length determined by the acceleration characteristics, it is possible to pass very close to the transit point, and when operating when avoiding obstacles etc. 1■ j becomes shorter.

(B) Since the intermediate trajectory changes the speed smoothly, unnecessary force is not applied to the yanidulator, and smooth operation can be performed without generating vibration or sound.

[Brief explanation of the drawing]

FIG. 1 is a configuration block diagram showing one embodiment of the present invention, and FIG.
Is the device in Figure 1 the auxiliary point? 3 is a specific example of auxiliary point selection, FIG. 4 is an explanatory diagram of the operation of the manipulator using the device in FIG. FIG. 3 is an explanatory diagram of the interpolation method for the arc trajectory 'yM, and FIG. 7 is an explanatory diagram of a conventional example. ￥ shakuhatsei

Claims (1)

[Scope of Claims] From the current position of the manipulator, a straight line or an arcuate trajectory passes through an externally specified way point to an externally specified target point, or a circular trajectory from the current position to the way point. In a manipulator control device that moves the manipulator along a straight trajectory when transferring, the distance on the trajectory from the current position to the waypoint, or the distance on the trajectory from the waypoint to the target point. a comparison means for comparing the shorter moving distance with a resting distance from a predetermined maximum speed specified in the movement until it comes to rest; and using this comparison means, determining that the moving distance is sufficiently larger than the resting distance. If it is judged that the stationary distance is not large enough, then it is a point that is half of the moving distance from the way point on the two orbits, and the current position side is the first auxiliary point and the target. Select the point side as the second auxiliary point, and
Auxiliary point calculating means for calculating the following third and fourth auxiliary points, the first and third auxiliary points, the way point, and the fourth auxiliary point.
and a transit trajectory calculation means for calculating an octic spline curve determined in this order using the second auxiliary point, and moving from the current position to the first auxiliary point on the predetermined trajectory, moving from the first auxiliary point to the second auxiliary point on the transit trajectory determined by the transit trajectory calculation means, and moving from the second auxiliary point to the target point on the predetermined trajectory; A manipulator control device featuring: Note (1) The third auxiliary point is the midpoint between the first auxiliary point and the way point if the trajectory connecting the current position and the way point is a straight line, and the third auxiliary point is the midpoint between the first auxiliary point and the way point if the trajectory connecting the current position and the way point is an arc. In some cases, the distance between the midpoint of the first auxiliary point and the waypoint on the trajectory is the first
A point that is away from the auxiliary point on the way point side, and is a point on a straight line that touches the arc at the first auxiliary point. (2) If the trajectory connecting the way point and the target point is a straight line, the fourth auxiliary point is the midpoint between the second auxiliary point and the way point, and the trajectory connecting the way point and the target point is an arc. In this case, it is a point that is separated from the second auxiliary point toward the way point by the distance on the trajectory of the midpoint between the second auxiliary point and the way point, and is on a straight line tangent to the arc at the second auxiliary point. point.