JPH0656565B2 - Manipulator controller - Google Patents

Description

BACKGROUND OF THE INVENTION 1. Field of the Invention The present invention relates to a device for controlling a manipulator, and more particularly to an operation for smoothly connecting to a circular arc trajectory when moving to a target position including the circular arc trajectory. Relates to an improved control device.

(Prior Art and Problems to be Solved) FIG. 7 shows an arrival point P from a departure point Ps via a waypoint Pm.
It is explanatory drawing at the time of moving to _E. Hereinafter, the trajectory from the starting point Ps or the current position Ps to the waypoint Pm is referred to as a front trajectory, and the trajectory from the waypoint Pm to the arrival point P _E or the target point P _{E is referred} to as a rear trajectory.

Conventionally, since it is difficult to control the smooth connection between the front track and the rear track, the vehicle is temporarily stopped at the waypoint Pm and moved again on the rear track.

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

Further, in the front and rear trajectories including arcs, it is also possible to connect them by arcs contacting both trajectories in the vicinity of the waypoint Pm, but since the curvature suddenly changes at the connection points, a large force acts on the manipulator to cause unnecessary vibration, etc. Could occur.

(Object of the Invention) The present invention solves such a problem, and smoothes the transit operation near the transit point, and achieves efficiency even if the operation connected to the transit operation is a linear trajectory or an arc trajectory. An object of the present invention is to provide a control device for performing a good manipulator movement.

(Means for Solving Problems) The present invention for achieving such an object is to move from the current position of the manipulator to a target point specified from the outside via a via point specified from the outside. And includes a circular orbit in at least one of the front and rear orbits.

The comparison means determines whether the shorter travel distance of the front trajectory and the rear trajectory is sufficient to travel at a predetermined maximum speed by comparing with the stationary distance.

If the moving distance is too short to reach the maximum speed, the auxiliary point calculating means sets the first auxiliary point on the front track side and the second auxiliary point on the rear track side, which is separated from the via point by half the moving distance. Select. When the moving distance is longer than the stationary distance, the first auxiliary point is selected on the front orbit side and the second auxiliary point on the rear orbit side, which is separated from the via point by the stationary distance. If the front trajectory is a straight line, select the third auxiliary point as the midpoint between the first auxiliary point and the waypoint,
If the previous trajectory is a circular arc, on a straight line that is in contact with the circular arc at the first via point, a distance corresponding to half the distance between the first via point and the via point on the arc is away from the first via point. The point is selected as the third auxiliary point. Fourth if the rear track is straight
Is selected as a midpoint between the second auxiliary point and the via point, and when the rear trajectory is an arc, the second via point on the arc is on a straight line contacting the arc at the second via point. And a distance equivalent to half the distance between the via point and the second via point
Selected as an auxiliary point.

The via trajectory calculating means smoothly contacts the front trajectory at the first auxiliary point, passes near the third assist point, the transit point, and the fourth auxiliary point, and smoothly moves to the rear trajectory at the second auxiliary point. A via trajectory determined by a predetermined approximate spline curve that is in contact with is calculated.

(Operation) The above-mentioned approximate spline curve is determined by using the first to fourth auxiliary points and the waypoints, and the acceleration is zero at the same speed at the first auxiliary point and the second auxiliary point. Therefore, it smoothly progresses from the front orbit to the via orbit and does not generate unnecessary force when entering from the via orbit to the rear orbit.

The comparing means indirectly calculates the moving speed of the manipulator at the first and second auxiliary points, and the auxiliary point calculating means calculates a transit trajectory adapted to the current position, the designated waypoint, the designated target point and the designated maximum speed. Calculate auxiliary points for doing.

(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, 10 is a manipulator consisting of, for example, a 6-degree-of-freedom joint mechanism, 20 is a host computer that transmits information necessary for the operation of the manipulator 10, and designates a target position P _E , a via position P m, and whether the trajectory is an arc or a straight line. To do. A position detector 30 detects the current position Ps of the manipulator 10. For example, each rotation angle of the joint is detected by an encoder and calculated. Reference numeral 40 is a controller that controls the position of the manipulator 10, and 50 is a drive circuit that moves the manipulator 10 according to a command from the controller 40.

R41 is a register for storing the current position Ps output from the position detector 30, and R42 is a register for storing information about a waypoint commanded by the host computer 10. When a linear motion is commanded, the waypoint Pm is stored. However, when the circular arc motion is commanded, the via point Pm and another point are stored, and the center O ₁ and the half meridian R _{1 of} the circular arc trajectory can be determined. R43 is a register for storing information on the target point instructed by the host computer 10. When the linear motion is designated, the target point P _E is stored, but when the circular motion is designated, the target point P _E and other points are stored. One point is stored so that the center O ₂ and radius R _{2 of} the circular arc trajectory can be determined. The predetermined maximum velocity Vmax peculiar 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 and are called and used as appropriate.

A41 receives position information from registers R41, R42, R44,
Distance on the orbit from the current position to the waypoint | PsP
m | and the distance on the orbit from the waypoint to the target point | P
This moving distance Min (| PsPm |, | PmP _E ) is detected by a distance comparing unit that selects a shorter moving distance of mP _E |.
Is compared with twice the stationary distance l, the following processing is performed as a whole.

Min (| PsPm |, | PmP _E |, 2l)
(1) A42 is an auxiliary point calculation 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 FIGS. 2 and 3. FIG. 2 shows the criteria for selecting auxiliary points, which are divided according to the lengths of the front and rear tracks and the shape of the track. If both the front and rear trajectories in the figure are straight lines, this embodiment may be used as it is, but usually a simple method is used, and here, common conditions when an arc trajectory is included are displayed and The columns show the conditions specific to each combination. FIG. 3 shows a specific example using a combination of arc-arc arc trajectories. In the figure, P ₂ = Pm. The auxiliary point calculation means A42 selects the first auxiliary point P ₀ at a point on the front track which is a distance away from the via point Pm by the following distance, and the same condition as the first auxiliary point P ₀ on the rear track from the via point Pm to the first auxiliary point P _0. The second auxiliary point P ₄ is selected at a point distant by the distance. This distance is a stationary distance 1 when Min (| PsPm |, | PmP _E |) ≧ 2l, and a moving distance Min (| PsPm when Min (| PsPm |, | PmP _E |) <2l. |, | PmP _E
Half of |). Next, the first auxiliary point P ₀ and the waypoint Pm
And a third auxiliary point P ₁ is defined between and. When the front trajectory is a straight line, the midpoint of the first auxiliary point P ₀ and the waypoint Pm is set as the third auxiliary point P ₁ . When the front trajectory is a circular arc, the third auxiliary point P ₁ is contacted with the circular arc at the first auxiliary point P ₀ .
Determine the direction of.

O ₁ P ₁ ⊥ P ₀ P ₁
(2) The distance between the third auxiliary point P ₁ and the first auxiliary point P _{0 is set} to half the distance between the first auxiliary point P ₀ and the waypoint Pm on the circular arc trajectory. Finally, the second auxiliary point P ₄ and the waypoint Pm
And a fourth auxiliary point P ₃ is defined between and. When the rear trajectory is a straight line, the midpoint between the second auxiliary point P ₀ and the waypoint Pm is set as the fourth auxiliary point P ₃ . When the rear trajectory is a circular arc, the fourth auxiliary point P ₃ is contacted with the circular arc at the second auxiliary point P ₄ .
Determine the direction of.

O ₂ P ₄ ⊥ P ₃ P ₄
(3) The distance of the fourth auxiliary point P ₃ from the second auxiliary point P _{4 is set} to half the distance between the second auxiliary point P ₄ and the waypoint Pm on the circular arc trajectory.

A43 is the first auxiliary point P from the current position Ps in the previous trajectory
₀ , and a velocity calculation means for calculating a velocity pattern from the second auxiliary point P ₄ to the target point P _E in the rear trajectory, for example, by using a cubic formula with respect to time, the positions P _s , P _o ,
The accelerations at P ₄ and P _E are set to zero.

A44 is the points P _o , P ₁ , P obtained by the auxiliary point calculation means A42.
_{2 (= Pm), P 3} , the point with the _{P 4 P} o, via orbital calculation means for calculating a through track to move at the same speed and acceleration zero at _{P 4,} for example, the eighth-order Bezier (Bejue) curve such The 8th-order spline curve of is defined.

A45 is a selector that outputs the trajectory using the speed calculation means A43 for the front track, the transit track calculation means A44 for the transit track, and the speed calculation means A43 for the rear track, and outputs it to the drive circuit 50.

FIG. 4 is an operation explanatory view of the manipulator 10 by the apparatus of FIG. 1, and here shows a case where the front trajectory is a straight line and the rear trajectory is an arc. In FIG. 4, (a) shows a constant velocity section on both sides of the waypoint Pm, (b) shows a constant velocity section on the current position Ps side of the waypoint Pm, and (c) shows a waypoint. P
When a constant velocity section exists on the target position P _E side of m, (d) shows a case where there is no constant velocity section on both sides of the waypoint Pm.
These are divided below.

In the figure, P ₅ is the position where the vehicle reaches the maximum speed Vmax by accelerating from the current position Ps or the waypoint Pm, and P ₆ is the maximum speed Vm.
The deceleration start position where the vehicle decelerates from ax and stops at the target point P _E or the waypoint Pm, and P _d is the position where deceleration starts, and then enters a curved trajectory. P _d is a point at which the maximum speed smaller than the maximum speed Vmax is reached.

(A) When there is a constant velocity section on both sides of the waypoint, the vehicle is accelerated to the maximum velocity Vmax between the current position Ps and the position P ₅ . The speed and the moving distance are given by the following equations, for example.

Here, Ts ₁ is the acceleration time, and when the maximum value of acceleration is Amax, Ts ₁ = (3/2) · Vmax / Amax, and the distance l required for acceleration is given by the following equation.

From the position P ₅ to the position P ₀ , constant velocity motion of the speed Vmax is performed.

From the connection point P ₀ to the connection point P ₄ , a trajectory that can be represented by an 8th-order Bezier curve among the 8th-order spline curves (points P ₀ , P
₁ , Pm, P ₃ , P ₄ and the midpoints of these are defined as a total of 9 points, but the 4 of the midpoints can be determined by the 5 given points, and thus the 4 auxiliary points obtained by the auxiliary point calculation means 42. And the waypoint Pm. ) Move up as follows.

P (t) = K ₁ (t / T) ⁸ + K ₂ (t / T) ⁷ + K ₃ (t / T) ⁶ + K ₄ (t / T) ⁵ + K ₅ (t / T) ⁴ + K ₆ (t / T) ³ + K ₇ (t / T) + P ₀ (0 ≦ t ≦ T) (7) where T = 4 | P ₀ −P ₁ | / V ₀ (8). Here, V ₀ is the initial velocity at the first auxiliary point P ₀ .

That is, P ₀ , Pm, P ₄ at a constant speed without stopping at the waypoint Pm.
And moves through P ₀ to P ₄ in the same time required to move. In addition, | P ' ₍₀₎ | = | P' _(T) | = Vmax
(9) P ″ ₍₀₎ = P ″ _(T) = 0
Since (10), the acceleration is zero at the connection points P ₀ and P ₄ , the velocities are smoothly connected, and the absolute values of the velocities are the same.

From the position P ₄ to the position P ₆ , constant velocity motion of the 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.

(B) When there is a constant velocity section on the side of the current position of the waypoint, the vehicle accelerates from the current position Ps, reaches the maximum speed Vmax at the position P ₅ which is a distance l away, starts decelerating at the position P ₆ , and starts the via track P _0.
-Stop at the target position P _E via P ₄ .

The velocity V _{E at the} positions P ₀ and P ₄ is determined by the length of the section P ₄ P _E. The motion of the section P ₄ P _E is It is represented by. Here, Ts ₃ = (3/2) · V _E / Amax. From this, | P ₄ −P _E | = (3/4) · V ² _E ▼ / Amax, and the speed V _E is given by the following equation.

In the section P ₀ P ₄ , the vehicle moves on the 8th-order Bezier curve according to the equation (7). The deceleration start position P ₆ is determined as follows. Section P ₆
At P ₀ , exercise according to the following equation.

Therefore, the length of the section P ₆ P ₀ is given by the following equation.

(C) When there is a constant velocity section on the target position side of the waypoint, the vehicle accelerates from the current position P _s to the position P ₀ and reaches the speed Vs.
This motion is given by

Here, the velocity Vs is obtained by integrating the equation (18), Given in.

In the section P ₀ P ₄ , the 8th-order Bezier according to the equation (7) of the velocity Vs
Move in a curve. In the section P ₄ P ₅ , the vehicle accelerates from the speed Vs to reach the maximum speed Vmax. The motion in this section is given by the following equation.

When this formula is integrated, the section length is as follows.

It is the same as (a) that the uniform velocity motion is performed from the positions P ₅ to P _{6 and} then the speed is decelerated to stop at the target position P _E.

(D) There is no constant velocity section on both sides of the waypoint. This is the case where the maximum speed is not reached with the allowable acceleration Amax due to the short travel distance. In this case, the operations of (b) and (c) on the side not including the constant velocity section are combined.

The change of the section can be determined by the elapsed time from the start of the operation or the moving distance.

FIG. 5 shows the speed of movement on the eighth-order Bezier curve and the generated acceleration. As shown in Eq. (9), the speed is smoothly connected to the anterior-posterior orbit and is smaller than the traveling speed on the anterior-posterior orbit on the transit orbit. As shown in equation (10), the acceleration acts because it moves on the curve, but it is zero at the connection point.

In the above embodiment, the case where there is one waypoint is shown. However, even if there are a plurality of waypoints, the points adjacent to each waypoint may be regarded as the current position and the target position to calculate the curved trajectory, and the velocity pattern may be calculated. May be set appropriately according to the condition of the orbit instead of stopping.

Also, an 8th-order Bezier curve is shown as the curve, but if 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 0, another approximate spline It may be a curve or a polynomial of degree 9 or higher.
The approximate spline curve here does not necessarily pass through the point sequence P ₀ , ..., P ₄ given as control points.
It is different from the interpolated spline curve that passes through this sequence of points.

FIG. 6 illustrates how to obtain the moving distance on the trajectory by interpolating between the starting point and the ending point of the circular arc trajectory. In the figure, O ₁ is the center, R ₁ is the radius, P ₀ is the start point, P ₁ is the end point, and P (t) is the point to be interpolated on the arc trajectory, Is the normal vector of the plane of the arc. When the point P ₀ is rotated by θ ₁ around the normal line passing through the center O ₁ , it coincides with the point P ₁ .

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

d (t) =-0.5 × VsTs ₁ (t / Ts ₁ ) ⁴ + VsTs ₁ (t / Ts ₁ ) ³
(21) With this, the rotation angle θ (t) is expressed by the following equation.

θ (t) = d (t) / R (2
2) Then, an arbitrary point P (t) on the arc is expressed as P (t) = R ( ₁ , θ (t)) · P ₀ (23). 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) Next, the velocity P ′ (t) and the acceleration P ″ (t) of the eighth-order Bezier curve of the equation (7) are Given by the formula.

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

P ′ (o) = 4 (P ₁ −P _o ) / T (2
_{7) P '(T) =} 4 (P 4 -P 3) / T (2
8) P ″ (o) = P ″ (T) = 0 (2
9) Then, Then, the speeds at both points are the same.

(Effects of the Invention) As described above, the present invention has the following effects.

(A) Since the via trajectory is connected to the arc trajectory and the straight trajectory or the arc trajectory with a length determined by the acceleration characteristics, it is possible to pass in the immediate vicinity of the transit point and shorten the operation time when avoiding obstacles etc. Become.

(B) Since the speed of the routed trajectory smoothly changes, 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.
The figure is an explanatory view when the device of FIG. 1 selects an auxiliary point, FIG. 3 is a specific example of selection of an auxiliary point, FIG. 4 is an operation explanatory diagram of a manipulator by the device of FIG. 1, and FIG. FIG. 6 is an explanatory diagram of the velocity and acceleration on the curve, FIG. 6 is an explanatory diagram of an interpolation method of a circular arc trajectory, and FIG. 7 is an explanatory diagram of a conventional example.

Claims (1)

[Claims] 1. From the current position (Ps) of the manipulator,
The manipulator is moved to a target point (P _E ) specified from the outside via a via point (Pm) specified from the outside by a linear trajectory or an arc trajectory, and a linear trajectory from the current position to the via point In the manipulator control device that moves in a circular orbit from the via point to the target point at the time, and when the current position to the via point is an arc orbit, moves in a linear or circular trajectory from the via point to the target point, The distance from the current position to the waypoint on the orbit, the distance on the orbit from the waypoint to the target point, whichever is shorter, or the predetermined maximum speed specified by the movement The comparison means (A41) for comparing the magnitude with the stationary distance up to and the stationary distance if the comparison means determines that the moving distance is sufficiently larger than the stationary distance. When it is determined that the distance is not large, it is a point distant from the waypoint on the two orbits by half the moving distance, the current position side is the first auxiliary point (P ₀ ), and the target point side is the second position. Is selected as the auxiliary point (P ₄ ), and the third and fourth auxiliary points (P ₁ ,
Calculating an auxiliary point calculating means for calculating a P ₃₎ (A42), the first and third points of support, the via-point, by the fourth and second auxiliary points, the approximate spline curve defined in this order At the same time, the approximated spline curve is connected smoothly to the straight line or arcuate trajectory at a first auxiliary point, and is smoothly connected to the straight line or arcuate trajectory at a second auxiliary point. The route from the current position to the first auxiliary point on the linear trajectory or the arc trajectory, and from the first auxiliary point to the second auxiliary point obtained by the transit trajectory calculating means. A manipulator control device that moves on a trajectory and moves from the second auxiliary point to the target point on the linear trajectory or the arc trajectory. Note The third auxiliary point is a midpoint between the first auxiliary point and the waypoint when the trajectory connecting the current position and the waypoint is a straight line, and a trajectory connecting the current position and the waypoint is an arc. Is a point separated from the first auxiliary point to the via point side by a distance on the trajectory between the midpoints of the first auxiliary point and the via point, and is a point on a straight line which is in contact with the arc at the first auxiliary point. And The fourth auxiliary point is a midpoint between the second auxiliary point and the via point when the trajectory connecting the via point and the target point is a straight line, and is a circular arc when the trajectory connecting the via point and the target point is an arc. And a point on the straight line which is distant from the second auxiliary point to the via point side by a distance on the trajectory between the midpoints of the second auxiliary point and the via point, and which is on a straight line which is in contact with the arc at the second auxiliary point To do.