JPS63106010A - Control method for path movement of robot - Google Patents

Abstract

PURPOSE:To prevent a robot from being stopped at a passing point, by applying velocity control by velocity vectors toward the passing point and a targeted point, and performing no positioning control at the passing point. CONSTITUTION:The velocity control is performed by the velocity vectors Vdi, Vdi+1, and Vdi+2, from a start point Pio to the passing point Pi, from the passing point Pi to the passing point Pi+1, and from the passing point Pi+1 to the targeted point Po, and the positioning control is performed only at the targeted point Po. Since the velocity control for path movement is performed by using the velocity vector, it is possible to change a moving direction at the passing point according to the velocity vector, and no positioning control is performed. In other words, the coordinate of the passing point is used for the change of the velocity vector, not for positioning. Therefore, it is possible to prevent the robot from being stopped at the passing point unrequired to stop, and also, to smoothly operate each axis (joint).

Description

[Detailed description of the invention] [Table of contents] Overview Industrial field of application Conventional technology Problems to be solved by the invention Means for solving the problems (Fig. 1) Working example (a) One example Explanation (Figs. 2 and 3) (b) Description of other embodiments (Fig. 4) Effects of the invention [Summary] A robot that moves to a target point via a passing point and is controlled to position at the target point. In the route movement control method, speed is controlled by a speed vector toward a passing point and a target point,
By not performing positioning control at passing points, the robot is prevented from stopping at passing points.

[Industrial application field]

The present invention relates to a robot path movement control method for positioning a robot to a target point via passing points on a given path.

A predetermined route is set for the robot to move to a target point, and passing points are given according to the route.

In such route movement, high-speed movement is required.

[Conventional technology]

FIG. 5 is an explanatory diagram of the prior art.

It is assumed that the robot 1 is of a manipulator type and has multiple joints.

That is, joints 14a, 14b, 14c, arms 15a, 1
5b and a hand 10 at the tip.

For example, as part pick-and-place work, this robot 1 grasps a first part BA stored in a parts magazine (or tray) MG with a hand 10, and inserts the first part into a hole HL of a second part BC. When inserting a BA, the route is set as follows.

That is, the starting point P of the work in which the hand 10 grasps the component BA
A starting point Pio, passing points Pi and Pi+1 on the target point PO are set on the path between io and the target point PO where the hand 10 inserts the part BA into the hole HL and releases it.

In order for the robot 1 to move along such a route, conventionally the coordinates of the starting point Pio, passing points Pi, Pi+l, and target point Po are given, and the joints 14a, 14b, 14c are arranged as shown in FIG.
), as shown in θ3 (speed controlled, starting point Pi
Move from o to passing point Pi, stop at passing point Pi, then move to Pi+1, stop at pi+l, move toward target point Po, insert part BA into hole HL, and move to target point Po. After positioning with , part BA was released.

That is, in conventional route movement control, position control is the basis, and the movement is stopped by positioning control even at passing points Pi and Pi+1.

[Problem that the invention seeks to solve]

However, the passing points Pi and Pi+1 are in the middle of the route and are essentially points where there is no need to stop.

Conventionally, there is a problem in that it takes time to move along the route because it stops at a point where it is not necessary to stop, and there is also the problem that it takes extra energy because it accelerates and decelerates to stop at this point. , which reduced the robot's work efficiency.

SUMMARY OF THE INVENTION An object of the present invention is to provide a route movement control method for a robot that enables high-speed route movement and reduces energy consumption by preventing the robot from stopping at passing points.

[Means for solving problems]

FIG. 1 is a diagram explaining the principle of the present invention.

In the present invention, from the starting point Pio to the passing point Pi, the passing point P
From i to passing point pi+l, from passing point Pi+1 to target point P
Velocity vector Vd i, Vdi+l, Vdi+2 to o
The speed is controlled by , and the positioning control is performed only at the target point PO.

In the example of path movement in the pick-and-place work shown in FIG. 5, each joint 14a, 14b, 1
4c's command speed request, /i! 2, θ.

is not subjected to positioning control at passing points Pi and Pi+l, but only velocity controlled according to the velocity vector.

[Effect]

In the present invention, since speed control is performed using a speed vector for route movement, the moving direction can be changed according to the speed vector at a passing point, and positioning control is not performed. That is, the coordinates of the passing point are used to change the velocity vector and are not used for positioning.

Therefore, it is possible to prevent stopping at passing points that do not require stopping.
Moreover, the movement of each axis (joint) also becomes smoother.

〔Example〕

(a) Description of one embodiment FIG. 2 is a detailed explanatory diagram of the present invention.

In the figure, the same components as those shown in FIG. 12 is an actuator for detecting force, which is composed of a motor, and is connected to each joint 14a, 1 of the manipulator 1.
4b, 14C, etc. In this example, since the manipulator 1 is composed of a 6-axis multi-joint type, it is equipped with actuators for 6 axes, and 13 is an encoder that controls each axis. This is to detect the current position (.theta.1, .theta.2, .theta.8, .theta.4, .theta.2, .theta.) of the actuator 12.

Therefore, the manipulator (robot) 1 is composed of a six-axis multi-joint type, and a force sensor (force detection device) 11 is provided between the hand 10 and the wrist.

Reference numeral 2 denotes an arithmetic processor, which is composed of, for example, a DSP (digital signal processor) that performs various arithmetic operations at high speed. , passing point coordinates P i, p i+1-1 The velocity vector Vdi between each point is given as data, the current position (■) from the manipulator 1 is taken in, speed and positioning control is performed, and force control is performed using the detection output FH of the force sensor 11. It is something to do.

20 is a servo control calculation unit, which controls the actuator 12 of the manipulator 1 so as to follow the target position and target speed.
21 is a force signal processing unit that converts the output FH of the force sensor 11 from the hand coordinate system to the reference coordinate system and calculates the force control speed vector Vf in the reference coordinate system; Current position calculation unit, encoder 1
3 is a unit that converts the current position ■ of the joint detected into the current position Pn in the reference orthogonal coordinate system, and 23 is a path control unit that calculates the passing point coordinates Pi given from the host processor.
- and the current position Pn, determine whether the robot 1 has passed the specified passing point, and if it has passed, select the velocity vector Vdi toward the next passing point (or target point). 24 is a synthesis calculation unit, which calculates the velocity vector V
26 is a coordinate conversion calculation unit that converts the velocity vector VR in the reference coordinate system into the velocity vector VH in the hand coordinate system. , 26 is a joint velocity calculation unit which converts the velocity vector VH into the velocity vector center of the joint coordinate system using a Jacobian matrix to be described later. 27 is a function generation calculation unit which calculates the velocity vector V of the joint.
Modify H with an acceleration/deceleration pattern and target point ■. Detects that it is approaching the target point based on the current position and
A deceleration curve for position control is calculated and provided to the servo control calculation section 20.

Note that each of the calculation units 20 to 27 represents a calculation performed by the calculation processor 2 as a block.

Reference numeral 3 denotes a host processor, which is composed of, for example, a personal computer, and which stores teaching data (target point Pa, intermediate passing points Pi--) that instructs the movement of the robot.
, calculate the velocity vector Vdi·- between each point, and further convert the positioning target point PO given in the reference coordinate system to the target coordinates (■) in the joint coordinate system.

, passing point Pi・-・, velocity vector Vdi・−
1 Final target coordinates■. is provided to the arithmetic processor 2, and has a route calculation unit 30 for this purpose.

In this embodiment, the position and speed of the hand 10 are handled in an orthogonal reference coordinate system that is easy for the operator to understand, and the arithmetic processor 2
The coordinate system is converted into a hand coordinate system and further into a joint coordinate system, and the speed of the articulated robot 1 is controlled.

Next, the operation of the embodiment shown in FIG. 2 will be explained using the process flow diagram shown in FIG.

■ When the host processor 3 receives the teaching data (command), the host processor 3 converts each point between Po1-*Pi and Pi-*P
Velocity vector Vd1SVd of i+l, Pi+1-+Po
i+l and Vdi+2 are calculated using the following formula.

1-Poi Vdi=□ ・−・−・−・−−−−−・− (1) However, T is the travel time between each point and is given in advance.

The same applies to Vdi+l and Vdi+2.

Next, the coordinates of the positioning target point PO are converted to the target coordinates O0 of the joint coordinate system.

Since these do not require real-time processing, they are performed by the host processor, and by an arithmetic processor 2 such as a DSP that may have a loss of precision. ■ Accurate position by having the host processor 3 perform the conversion without performing the conversion. get.

The host processor 3 determines the passing point coordinates P i s P i
+1, final target position■. , velocity vector V d between each point
(V d i-) is sent to the arithmetic processor 2.

(2) In the arithmetic processor 2, the path control unit 23 outputs the desired velocity vector Vd to the combination calculation unit 24, and the combination with the force control speed vector Vf from the force signal processing unit 21 is calculated in the combination calculation unit 24, A composite velocity vector VR is obtained.

The resultant velocity vector VR is calculated by the coordinate transformation calculation unit 25,
A conversion operation is performed to a velocity vector VH in the hand coordinate system.

The velocity vector VH in the hand coordinate system is multiplied by the inverse matrix of the Jacobian matrix J of the manipulator 1 in the joint velocity calculation unit 26 and converted into a velocity vector center in the joint coordinate system.

In other words, the Jacobian matrix J is VH=J・■ −1−−−−−・・・−・(2
), the center is found by ==J-1・VH-・−・−・−・・ (3).

■ The velocity vector center of the joints obtained in this way is determined by the function generation calculation unit 27, and the velocity commands b1−·−δ of each joint are
6 is modified with a predetermined acceleration/deceleration curve and given to the servo control calculation unit 20, and the servo control calculation unit 20 uses the current position (θ1−θ,) of the encoder 13 to control the actuator 12 of each joint. Control speed.

Further, the function generation calculation section 27 calculates the current position (■) of the encoder 13 and the given final target position (■). It is determined whether the robot 1 has reached the position control start position near the final target position P o (@) o ).

If the position has been reached, the positioning control in step (2) begins.

On the other hand, if the position control start position has not been reached, the current position 0 detected by the encoder 13 is converted by the current position calculation unit 22 via the servo control calculation unit 20 into the current position Pn in the reference orthogonal coordinate system. Ru.

This current position Pn is determined by the already given intermediate passing point Pi,
The coordinates of Pi+1 are compared with the route control unit 23 to determine whether the robot 1 has passed the designated passing point. If the robot 1 has not passed the designated passing point, the process returns to step (2) and speed control is performed.

(2) Conversely, if it is determined that the vehicle has passed the passing point, the route control unit 23 selects a velocity vector heading toward the next passing point (or the target point after passing the final passing point), and returns to step (2).

(2) On the other hand, when it is determined that the position control start position has been reached in step (2), the function generation calculation section 27 issues a speed command according to the deceleration curve toward the final target position O0, and gives it to the servo control calculation section 20 to perform positioning control.

And the target position ■. positioning control.

The arithmetic processor 2 repeats the above flow every sampling time.

In this way, the speed is controlled using the velocity vector toward the passing point or the target point, and when the passing point is reached, the speed vector is switched to the next passing point or target point, and the route is moved by speed control. On the other hand, near the target point, speed control is switched to position control, and positioning at the target point is performed.

Therefore, positioning is not performed at the passing point, and the coordinates of the passing point are used to switch the velocity vector.

Therefore, since the vehicle does not stop at passing points, the route travel time is shortened and energy consumption for stopping is eliminated. Moreover, the movement of each joint is facilitated, and smooth movement is also possible.

Furthermore, speed control can be performed in real time using an arithmetic processor such as a DSP.

(b) Description of other embodiments FIG. 4 is an explanatory diagram of another embodiment of the present invention.

In the figure, the same parts as those shown in FIG. 5 are indicated by the same symbols.

In this embodiment, the moving route for the work shown in FIG. 5 is changed. That is, the passing point Pi+2 is provided between the passing points Pi and Pi+1, and the change in direction at each point is made small.

In control using velocity vectors, the smaller the change in direction, the less acceleration and deceleration caused by the change in direction, and the faster the speed and the smoother the operation.

Therefore, in order to avoid a change in direction as close to a right angle as possible, the route is set as shown in FIG. 4.

This increases the overall path length, but since acceleration and deceleration are reduced, high-speed movement is possible, operations can be executed more smoothly, and less energy is wasted.

In the above-described embodiment, the force sensor 11 is provided to facilitate the insertion of the component BA, but it is not necessary to provide the force sensor 11, and the operation is not limited to component insertion work or big-and-place work.

Further, the route is not limited to that of the embodiment, but can be applied to various other routes, and various numbers of passing points can be adopted.

Furthermore, the robot (manipulator) l is not limited to the articulated type, and can be applied to well-known scalar type and orthogonal ring types.

Although the present invention has been described above using examples, the present invention can be modified in various ways according to the gist of the present invention, and these are not excluded from the present invention.

〔Effect of the invention〕

As explained above, according to the present invention, the speed is controlled using the velocity vector and the position is controlled only at the target point, so it is possible to avoid stopping movement at passing points, so high-speed route movement is possible. It has the effect of making it possible to move, and it also has the effect of reducing the energy required for movement.
It enables smooth route movement and greatly contributes to improving the work efficiency of robots.

[Brief explanation of the drawing]

Fig. 1 is an explanatory diagram of the principle of the present invention, Fig. 2 is a detailed explanatory diagram of the present invention, Fig. 3 is a processing flow diagram of one embodiment of the present invention, and Fig. 4 is an explanatory diagram of another embodiment of the present invention. , FIG. 5 is an explanatory diagram of the prior art. In the figure, 1--robot (manipulator), 2-- arithmetic processor, 10-- hand, P i - passing point, P o - target point.

Claims (1)

[Claims] Passing points and target points are given as a movement route, and the robot (
1) A path movement control method for a robot in which the robot (1) is moved via a passing point and positioned at a target point, the robot (1) being speed-controlled by a velocity vector (Vdi) directed toward the passing point and the target point; A method for controlling path movement of a robot, characterized in that positioning control is performed only for the target point.