JPH0631663A - Track control device for profile control robot - Google Patents

Abstract

PURPOSE:To provide a track control device for a profile control robot wherein building work sequence is facilitated by controlling a projecting point so that copying operation between fixed positions on a curved surface is performed in an assigned time, relating to forming a track of the profile control robot of performing the copying operation of grinder work or the like. CONSTITUTION:A track control device for a profile control robot of performing track control of a control object is constituted by having an operating part 2 for actuating the control object 1, track control part 5 for supervising a time and a route controlled of copying operation of the control object 1, force control part 6 for controlling force received by the control object 1 and a control direction calculating part 7 for calculating a direction when the control object 1 comes into contact with an objective member, and the track control part 5 controls the copying operation between assigned positions so as to be performed in a preset time.

Description

Detailed Description of the Invention

[0001]

BACKGROUND OF THE INVENTION 1. Field of the Invention The present invention relates to a trajectory control device for a copying control robot, and more particularly to a fixed position on a curved surface in the trajectory generation of a copying control robot for performing copying operations such as grinder work, deburring work and polishing work. The present invention relates to a trajectory control device for a copying control robot in which a copying operation is performed within a set time and the trajectory control performance during the copying operation is improved, and a work sequence is easily assembled.

[0002]

2. Description of the Related Art In order to realize a copying operation using a force control robot, a method has already been proposed in which the control direction of the robot is calculated from the detected contact force and position control and force control are performed based on each control direction. (Japanese Patent Laid-Open No. 3-18
4786).

FIG. 6 is a block diagram of a conventional trajectory control device for a copying control robot. In the figure, a trajectory control device of a copying control robot of the conventional example is a control target (a copying control robot that performs processing along the surface of a target member,
Hereinafter, referred to as a robot) 1, an operation unit 2 that operates the robot 1, and a force detection unit 3 that detects a force received by the robot 1.
And a position detection unit 4 for detecting the position, posture, etc. of the robot 1.
And a control direction calculation unit 107 that calculates a normal vector of a contact point between the robot 1 and a target member to calculate a scanning coordinate system and a moving direction vector of the robot 1 along the scanning coordinate system; Operation sequence generation unit 10 for transmitting force and position commands to the robot and transferring various parameters
8, a force control unit 106 that controls the force applied to the robot 1 based on the force detected by the force detection unit 3, and a position control unit that controls the position of the robot 1 based on the position coordinates detected by the position detection unit 4. And 105.

That is, the position detector 4 and the position controller 10
5 and the operation unit 2 form a position feedback loop for moving the tip position of the robot 1 according to the target position instructed from the operation sequence generation unit 108, and
The force detection unit 3, the force control unit 106, and the operation unit 2 make the contact force between the tip of the robot 1 and the target member detected by the force detection unit 3 match the target force generated by the operation sequence generation unit 108. It forms a force feedback loop for force control.

The control direction calculator 108 calculates the control direction based on the contact force detected by the force detector 3. If the force control is performed in the direction in which the movement is restrained and the position control is performed in the direction in which the movement is not restrained, the copying operation can be executed based on the calculated control direction.

The operation sequence section 108 has a built-in copying operation end judging section, and judges the end of the copying operation under the following conditions. (1) When a certain period of time has passed (2) When an instruction is given by the operator (3) When the above is completed When applying the robot 1 to an assembly line or the like, one operation of the robot 1 is performed within a certain takt time. Is required to end. However, under the above judgment conditions,
When the surface shape of the target member is unknown, it is not possible to execute the copying operation in which the operation time, the operation start position, and the target position of the robot 1 are designated. In other words, in order to move from the operation start position to the end position instructed within a certain time, it is necessary to generate an appropriate operation speed in the control device, but it is unknown when only the start and end positions of the operation are known. When tracing the curved surface of, the operation speed cannot be set because the operation path and the movement distance are unknown. Therefore, the conventional track control device does not compensate for the operation time, and therefore cannot be applied to a work such as an assembly line where a constant tact time is required.

[0007]

As described above, in the conventional trajectory control device for the copying control robot, in order to move from the operation start position to the end position instructed within a certain time,
It is necessary to generate an appropriate operating speed in the control device, and when copying an unknown curved surface, the operating speed cannot be set because the operating path and movement distance are unknown, and the operating time cannot be compensated. There was a problem that it could not be applied to work requiring a certain takt time.

The present invention solves the above problems.
An object of the present invention is to provide a trajectory control device of a copying control robot in which a copying operation between fixed positions on a curved surface is performed within a designated time by controlling a projection point, and a work sequence is easily assembled. .

[0009]

In order to solve the above-mentioned problems, the trajectory control device of the copying control robot of the first feature of the present invention is a copying control for performing the trajectory control of the controlled object 1 as shown in FIG. A trajectory control device for a robot, comprising an operation unit 2 for operating the controlled object 1, a trajectory control unit 5 for monitoring and controlling an operation time and an operation path of a copying operation of the controlled object 1, and the controlled object 1. Force control unit 6 for controlling the force received
And a control direction calculation unit 7 that calculates a control direction when the control target 1 and the target member are in contact with each other,
The trajectory control unit 5 controls to perform a copying operation between designated positions within a set time.

Further, the trajectory control device of the copying control robot of the second feature of the present invention is, as shown in FIG. 1, a trajectory control device of the copying control robot for controlling the trajectory of the robot 1 to be controlled. An operation unit 2 that operates the robot 1, a force detection unit 3 that detects the force received by the robot 1, and a force control unit that controls the force applied to the robot 1 based on the force detected by the force detection unit 3. 6, a position detector 4 for detecting the position, orientation, etc. of the robot 1, and the operation time and operation path of the copying operation of the robot 1 are monitored and controlled based on the position coordinates detected by the position detector 4. The trajectory control unit 5, a control direction calculation unit 7 that calculates the restraining direction and the moving direction of the tip of the arm of the robot 1 from the contact force when the robot 1 and the target member are in contact, and the robot controller. 1 and a motion sequence generation unit 8 for transmitting a force command and a position command and various parameters, and the trajectory control unit 5 controls to perform a copying motion between designated positions within a set time. To do.

A trajectory control device for a copying control robot according to a third aspect of the present invention is the trajectory control device for a copying control robot according to claim 1 or 2, wherein the trajectory control unit 5 is:
Control is performed so that the velocity function of the projection point obtained by projecting the scanning trajectory on the projection plane becomes trapezoidal with respect to time change.

A trajectory control apparatus for a copying control robot according to a fourth aspect of the present invention is the trajectory control apparatus for a copying control robot according to claim 1 or 2, wherein the trajectory control unit 5 projects the copying trajectory. The velocity function of the projection point projected on the plane is controlled so as to be expressed by a polynomial function of time.

Further, the trajectory control device of the copying control robot of the fifth feature of the present invention is the trajectory control device of the copying control robot according to claim 1 or 2, wherein the trajectory control unit 5 projects the copying trajectory. The velocity function of the projection point projected on the plane is controlled so as to be expressed by a transcendental function of time.

[0014]

The trajectory control device of the copying control robot of the present invention is
As shown in FIG. 1, a force feedback loop including a force detection unit 3, a force control unit 6, and an operation unit 2 that perform control so as to follow the target force from the operation sequence generation unit 8.
A position feedback loop including a position detection unit 4, a trajectory control unit 5, and an operation unit 2 that control so as to follow the target position from the operation sequence generation unit 8, and the robot 1
When the target member and the target member are in contact with each other, the control direction calculation unit 7 that calculates the restraining direction and the movable direction from the contact force, and the operation sequence generation unit 8 that generates the control command for executing the work.
It consists of and.

The trajectory control unit 5 performs control such that the projection point when the contact point with the target member is projected on a certain plane advances along a linear trajectory. The projected plane is defined by the work coordinate system set at the copying motion start position of the robot 1, and includes the motion start position, the target position, and the locus of the projection point. Considering a linear trajectory on a plane, the moving position can be calculated from the motion start position and the target position, and the motion speed required to complete the motion in the designated time can be determined. The actual copying control is performed by converting the moving speed of the projection point into the speed on the copying coordinate system calculated by the control direction calculating unit 7.

As described above, by controlling the projection point, the copying operation between the fixed positions on the curved surface can be performed within a designated time, and the trajectory control performance during the copying operation can be improved. As a result, it is possible to realize the trajectory control device of the copying control robot in which the work sequence is easily assembled.

[0017]

Embodiments of the present invention will now be described with reference to the drawings. FIG. 1 shows a block diagram of a trajectory control device for a copying control robot according to an embodiment of the present invention. In the figure, the same parts as those in FIG. 6 (conventional example) are designated by the same reference numerals.

In FIG. 1, a trajectory control device for a copying control robot according to the present embodiment includes a control target (a copying control robot that performs processing or the like along the surface of a target member, which will be referred to as a robot hereinafter) 1 and a robot 1. The operation unit 2 to be operated,
A force detection unit 3 that detects the force received by the robot 1, a position detection unit 4 that detects the position, orientation, etc. of the robot 1, and a force that controls the force applied to the robot 1 based on the force detected by the force detection unit 3. The control unit 6, the trajectory control unit 5 that monitors and controls the operation time and operation path of the copying operation of the robot 1 based on the position coordinates detected by the position detection unit 4, and the robot 1
From the control direction calculation unit 7 that calculates the restraining direction and the movable direction from the contact force when the target member is in contact with the target member, and the operation sequence generation unit 8 that transfers the force and position command to the robot 1 and various parameters. It is configured.

The position detection unit 4, the trajectory control unit 5, and the operation unit 2 determine the tip position of the robot 1 by the operation sequence generation unit 8
A position feedback loop for performing control to follow the target position instructed from is formed, and the force detection unit 3, the force control unit 6, and the operation unit 2 form the tip of the robot 1 detected by the force detection unit 3. A force feedback loop for performing force control is formed so that the contact force between the target member and the target member matches the target force generated by the operation sequence generation unit 8.

The position detecting section 4 is composed of an encoder and a counter for detecting the rotation angle of the motor of the joint portion of the robot arm to be controlled, and a coordinate conversion section for converting the coordinates from the rotation angle to the reference coordinate system. The tip position of the robot 1 in the coordinate system is detected.

The force detection unit 3 comprises a force sensor connected to the tip of the robot arm and a force signal calculation unit for calculating the contact force in the reference coordinate system from the electric signal detected by the force sensor. Detects the contact force in the coordinate system. The calculation of the contact force in the reference coordinate system will be described later.

The robot arm is driven by the operation unit 2. The operation unit 2 is composed of a servo motor, a power amplifier, a D / A converter, and a compensator, and is composed of a position control unit 9 and a force control unit 6. The joint part of the robot is driven so as to follow the generated speed command in the reference coordinate system.

The trajectory control unit 5 calculates the projection point on the position control plane calculated from the tip position X of the robot 1 detected by the position detection unit 4, and the target position X _O given by the motion sequence generation unit 8. operation time, and based on the position control parameters such as position control gain to generate a speed command signal of the position control direction of the scanning coordinate system ^L V _P.

The force control unit 6 generates a speed command signal V _f in the force control direction based on the force signal detected by the force detection unit 3, the set force (force command F _O ) and the force control parameter. Further, the control direction calculation unit 7 calculates the control direction for performing the scanning trajectory from the force signal of the contact force and the vector o _OP preset for determining the scanning trajectory.

Further, an operation procedure for work is set by the operator in the operation sequence generator 8 and the following processing is performed. (1) Generation of force command F _O , transfer of force control parameter (2) Generation of position command X _O , transfer of position control parameter (3) Switching between position control and force control (4) Settings Next, a detailed configuration diagram of the control direction calculation unit 7 of the present embodiment is shown in FIG.
Shown in.

The control direction of the copying operation is the copying at the contact point.
It is determined from the coordinate system. The scanning coordinate system is defined by the contact point.
It is a rectangular coordinate system that is used for force control.
Order vector n_L, Unit of movement direction of the tip of robot 1
Vector o_L, Vector n _LAnd o_LOrthogonal to
The unit vector of one position control direction is a_LAnd unit
Vector n_L, O_L, And a_LIs a_L= N_L× o_L Meet

The normal vector n _L at a certain contact point is obtained as follows. The method of calculating the normal vector n _L is the same as the method disclosed in Japanese Patent Laid-Open No. 3-184786.

Unit normal vector n_LIs in the reference coordinate system
Stated contact force^OCalculated from F. Force sense of force detector 3
In the sensor, at the contact point described in the force sensor coordinate system,
Reaction force ^SX of F_S, Y_S, And Z_SEach component of the direction
f_X, F_Y, And f_Z, And Maume around each coordinate axis
Nt m_X, M_Y, And m_ZIs detected. Where the coordinates
Axis X_S, Y_S, And Z_SRepresents the force sensor coordinate system. Since
Below, regarding the moment component detected by the force sensor
Is omitted.

Reaction force^SWhen F is displayed as a vector (S is a force
Sensor coordinate system O_S-X_S Y_S Z _SDescribed in
),^S F = (^Sf_X ^Sf_Y ^Sf_Z)^T Becomes Contact force described in the force sensor coordinate system^SBased on F
Expressed in a quasi-coordinate system,^O F =^OA_S・^SF = (^Of_X ^Of_Y ^Of_Z)^T Is. Coordinate transformation matrix^OA_SDepends on the posture of the force sensor.
The robot arm joint angle θ_iA homogeneous transformation
(Or Denavit-Hartenberg notation)
You can Below, contact force^OLet F be F, and its components are F = (f_X f_Y f_Z)^T It is represented by.

The unit normal vector n _L of the scanning coordinate system is a vector in the direction opposite to the contact force F.

[0031]

[Equation 1]

It is Next, using the moving direction vector o _OP given by the operator and the obtained normal vector n _L ,
The moving direction vector o _L is calculated. However, the moving direction vector o _OP and the normal vector n _L do not satisfy o _OP = n _L or o _OP = −n _L. Moving direction vector o _L is perpendicular to the normal vector n _L, is a vector in a plane formed by the moving direction vector o _OP and the normal vector n _L. At this time, a _L , which is one of the unit vectors representing the coordinate axes of the scanning coordinate system, is a _L = (n _L × o _OP ) / | n _L using the normal vector n _L and the moving direction vector o _OP. It is represented by × o _OP |. The moving direction vector o _L is obtained by o _L = a _L × n due to the orthogonal relationship between the vectors n _L and a _L.

Next, FIG. 3 shows a detailed configuration diagram of the force control unit 6 of the present embodiment. As shown in the figure, the force control unit 6, a transposed coordinate transformation matrix (R _L ^T) calculation unit 61, a force compensator 62
, A selection matrix (S _f ) operation unit 63, and a coordinate transformation matrix (R
_L ) arithmetic unit 64.

In the force control unit 6, the calculated scanning coordinate system n
Force control is performed using _L , o _L , and a _L , and the contact force F detected by the force detection unit 3. Force signal, by the coordinate transformation matrix R _L ^T sought transposed coordinate transformation matrix (R _L ^T) calculation unit 61, is converted into the coordinate system copying from the reference coordinate system. The coordinate transformation matrix R _L ^T is a unit vector n _L ,
Given by o _L and a _L , for example, in the case of a translational 3 degrees of freedom and a rotation 1 degree of freedom like a SCARA robot

[0035]

[Equation 2]

It becomes Incidentally, the coordinate transformation matrix R _L ^T, the transpose matrix of the coordinate transformation matrix R _L to convert the vector to the reference coordinate system from copying the coordinate system, the coordinate transformation matrix R _L,

[0037]

[Equation 3]

It is represented by The force compensator 62 generates a force / velocity command signal based on the contact force F, the target force F _O , and the force control gain G described in the scanning coordinate system.

The selection matrix (S _f) selection matrix S _f obtained by the arithmetic unit 63 is used to extract only the force control direction component of the force velocity command signal. The selection matrix S _f is

[0040]

[Equation 4]

Is given by Further, the force / velocity command signal is converted from the scanning coordinate system to the reference coordinate system by the coordinate conversion matrix R _L.

Next, FIG. 4 shows a detailed configuration diagram of the trajectory control unit 5 of the present embodiment. As shown in FIG.
A function generator 51, the selection matrix (S _P) and the operation unit 52, the transformation matrix (T _V) and the arithmetic unit 53, a coordinate transformation matrix ^(O R _L)
The calculation unit 54, the integrator 55, and the transposed coordinate transformation matrix ( ^OR
And _W ^T) calculation unit 56, and a force compensator 57.

First, the work coordinate system determined at the start of operation
A unit vector n_W, O_W, And a_WTo the calculation method of
explain about. Unit vector n_WAt the start of operation
It is a vector that indicates the control direction of the force indicated by the operator.
The target force vector specified by the operator^OF_OPTosu
Then, n_W=^OF_OP/ ｜^OF_OP| X is the start position of the copying operation in the reference coordinate system.
(T = 0), the target position is X _OPAnd Point X_OPUnit is
Torn_WTo a plane that is perpendicular to and includes the starting position X (t = 0)
Shadow and project the projection point to X₀, The projection point of the copy trajectory is
Unit vector a indicating the direction of travel_WIs

[0044]

[Equation 5]

Is given by Unit vector o_WIs the unit
Vector n_WAnd a_WIs represented by. o_W= A_W× n_W The positional relationship of each coordinate system is shown in FIG. OX in the figure
_O Y_O Z_OIs the reference coordinate system, n_W, O_W, And a_WIs
Working coordinate system, n_L, O_L, And a_LIs the copying coordinate system, point X
(T = 0) is the operation start position, point X_OPIs the target position and point X is
Current position, point X ₀Is the target position X_OPO_W-A_WTo the plane
The projection point, Voaw, is the velocity calculated by the function generator 51,
Represent each.

As shown in the figure, the trajectory control unit 5 determines that the projection point of the current position X of the contact point on the o _W -a _W plane is the motion start position X (t = 0) and the target position X _OP . The trajectory of the copying operation is controlled so as to coincide with the target projection position on the straight line connecting the projection points X ₀ .

The moving speed of the target projection position at each time is set by the function generator 51. Here, as an example, it is assumed that the function generator 51 provides a time function of the velocity of the target projection position in a trapezoidal shape as shown in FIG. Operation time t _{O of} copying operation and movement distance | X ₀
Given −X (t = 0) |, in the function generator 51,

[0048]

[Equation 6]

T ₁ and t ₂ are determined so as to satisfy the above condition. When t ₁ and t ₂ are calculated from the above equation, t ₁ = {t _O − (t _O ² −4 | X ₀ −X (t = 0) | / acc) ^1/2 } / 2 t ₂ = t _O −t ₁ Vow = acc · t ₁ . However, the acceleration acc is preset by the operator. Since the target projection position moves in the direction of the unit vector a _W of the work coordinate system, the velocity vector generated by the function generator 51 is as follows.

[0 0 Voaw]^T A unit vector a of the target projection position and the current position X_WShooting at
Due to the mechanical characteristics of the robot 1
Differences may occur. So position feedback
To improve the followability to the target projection position.
It The absolute position of the target projection position described in the working coordinate system is
Transformation matrix R from the reference coordinate system to the working coordinate system _W ^TUsing
And R_W ^T-It is represented by X (t = 0) + ∫ Voaw dτ. If the current position X is described in the working coordinate system, R_W ^TSince X, the deviation ΔX is ΔX = R_W ^T・ X (t = 0) + ∫ Voaw dτ-R_W ^T・
X = R_W ^T・ {X (t = 0) −X} + ∫Voaw dτ. The above equation is calculated by the integrator 55 and
And the transposed coordinate transformation matrix (^OR_W ^T) Implemented by the computing unit 56
It Further, the deviation ΔX is the position formed by a regulator or the like.
The compensator 57 converts the speed amount to Vpw and
Configure a croup.

Since the position control of the projection point is performed in the o _W -a _W plane, the position control direction component of the speed generated by the function generator 51 and the position compensator 57 is calculated by the selection matrix ( _SP ). Extracted by the unit 52. The selection matrix S _P is given by the following equation.

[0052]

[Equation 7]

Next, the position / speed command of the work coordinate system description generated by the above method is described in the scanning coordinate system. The position control speed command vector ^L V _{P of the} scanning coordinate system and the position control speed command vector ^W V _P of the working coordinate system satisfy the following equation.

[0054] In ^{_{W V P = W R L ·}} L V P where, ^W R _L is a coordinate transformation matrix for transforming the vectors of the copying coordinate system to the working coordinate system ^{_{representation, W R L = W R O}} · O ^{_{R L = (O R W)}} T · O R L O R W = [O n W O o W O a W] O R L = a ^{_{[O n L O o L O}} a L]. Position control of scanning coordinate system Speed command vector ^L V _P
Has only velocity components in the o _L and a _L directions that are position control directions,

[0055]

[Equation 8]

It is represented by Further, the position control speed command vector ^W V _P of the work coordinate system which satisfies the above equation when ingredient labeling,

[0057]

[Equation 9]

It becomes Where V _PwX , V _PwY , V _PwZ
Is a speed command component calculated by the function generator 51 and the phase compensator 57 described in the working coordinate system. From the above relationship, the speeds V _PlY , V in the position control direction of the scanning coordinate system
_PlZ is the speed command component V in the position control direction of the working coordinate system
It is expressed using _PwY and V _PwZ .

[0059]

[Equation 10]

Therefore,

[0061]

[Equation 11]

From the above, using the transformation matrix T _V , the relationship between the position control speed command vector ^W V _{P of} the working coordinate system and the position control speed command vector ^L V _P of the scanning coordinate system is expressed as the following equation.

^L V _P = T _V · ^W V _P

[0064]

[Equation 12]

[0065] When representing the position control speed command vector ^L V _P of calculated scanning coordinate system reference coordinate system, the _{^{O V P = O R L ·}} L V P.

In the above description, the function generator 51 of the trajectory control unit 5 is assumed to generate a trapezoidal function with respect to time change. However, the present invention is not limited to this, and the tracing trajectory is projected onto the projection plane. The velocity function of the projected point may be configured as a function expressed by a polynomial function of time or a transcendental function.

[0067]

As described above, according to the present invention,
A force feedback loop consisting of a force detection unit, a force control unit, and an operation unit that controls to follow the target force from the motion sequence generation unit, and control to follow the target position from the motion sequence generation unit. A position feedback loop including a position detection unit, a trajectory control unit, and an operation unit, a control direction calculation unit that calculates the restraining direction and the movable direction from the contact force when the robot and the target member are in contact, and performs work. And a motion sequence generation unit that generates a control command for controlling the motion of the motion control unit so that the projection point when the contact point with the target member is projected onto a certain plane is a straight line trajectory. Since it was decided to calculate the movement position from the start position and the target position and determine the operation speed required to complete the operation in the specified time, it is possible to control the projection point. As a result, the copying operation between the fixed positions on the curved surface can be performed within a specified time, the trajectory control performance during the copying operation can be improved, and as a result, the copying control robot in which the work sequence is easily assembled It is possible to provide the orbit control device.

[Brief description of drawings]

FIG. 1 is a configuration diagram of a trajectory control device for a copying control robot according to an embodiment of the present invention.

FIG. 2 is a detailed configuration diagram of a control direction calculation unit according to the embodiment.

FIG. 3 is a detailed configuration diagram of a force control unit according to the embodiment.

FIG. 4 is a detailed configuration diagram of a trajectory control unit according to the embodiment.

FIG. 5 (1) is an explanatory diagram of a velocity function generated by a function generator of a trajectory control unit, and FIG. 5 (2) is an explanatory diagram of a positional relationship between a reference coordinate system, a work coordinate system, and a scanning coordinate system. is there.

FIG. 6 is a configuration diagram of a conventional trajectory control device for a copying control robot.

[Explanation of symbols]

DESCRIPTION OF SYMBOLS 1 ... Control object (robot) 2 ... Operation part 3 ... Force detection part 4 ... Position detection part 5 ... Orbit control part 105 ... Position control part 6, 106 ... Force control part 7 ... Control direction calculation part 8 ... Operation sequence generation part X ... Robot tip position detected by the position detection unit _XO ... Target position given by the operation sequence generation unit F ... Contact force _F0 ... Set force (force command) _Vf given by the operation sequence generation unit _Vf ... Force control direction of the velocity command signal 51 ... function generator 52 ... selection matrix (S _P) calculation unit 53 ... transform matrix (T _V) calculating unit 54 ... coordinate transformation matrix ^(O R _L) calculation unit 55 ... integrator 56 ... transposed coordinates transformation matrix ^(O R _W ^T) calculation portion 57 ... force compensator 61 ... transposed coordinate transformation matrix (R _L ^T) calculation portion 62 ... force compensator 63 ... selection matrix (S _f) calculating unit 64 ... coordinate transformation matrix (R _L) calculation unit O ... reference coordinate system W ... work seat System L ... copying coordinate system o _OP ... moving direction vector G ... force control gain _{n W, o W, a W} ... unit vector n _L of the work coordinate system, o _L, a _L ... units of the scanning coordinate system vector X (t = 0) ... Operation start position X _OP ... Target position X ... Current position X ₀ ... Projection point of target position X _OP on o _W -a _W plane Voaw ... Velocity calculated by function generator acc ... Acceleration ΔX ... Deviation vpw ... output of the position compensator (velocity amount) ^W V _P ... position control speed command vector position control speed command vector ^O V _P ... reference coordinate system of the position control speed command vector ^L V _P ... scanning coordinate system of the work coordinate system

Claims (5)

[Claims] 1. A trajectory control device of a copying control robot for performing trajectory control of a controlled object (1), comprising: an operating section (2) for operating the controlled object (1); and copying of the controlled object (1). The trajectory control unit (5) that monitors and controls the operation time and the operation route of the operation, the force control unit (6) that controls the force received by the control target (1), the control target (1) and the target member And a control direction calculation unit (7) that calculates a control direction when in contact, and the trajectory control unit (5) controls to perform a copying operation between designated positions within a set time. Trajectory control device for a characteristic copying control robot. 2. A trajectory control device for a copying control robot for controlling the trajectory of a robot (1) to be controlled, comprising: an operating section (2) for operating the robot (1); and the robot (1). A force detection unit (3) for detecting a force received, a force control unit (6) for controlling a force applied to the robot (1) based on the force detected by the force detection unit (3), and the robot (1 ) Position detection unit (4) for detecting the position, posture, etc., and monitoring and controlling the operation time and operation path of the copying operation of the robot (1) based on the position coordinates detected by the position detection unit (4). And a control direction calculation unit (5) that calculates the restraining direction and the moving direction of the tip of the arm of the robot (1) from the contact force when the robot (1) and the target member are in contact with each other. 7) and the robot (1 And a motion sequence generation unit (8) for transmitting various force and position commands to various parameters, and the trajectory control unit (5) controls to perform a copying motion between designated positions within a set time. A trajectory control device for a copying control robot, which is characterized by: 3. The trajectory control unit (5) controls the velocity function of a projection point obtained by projecting a scanning trajectory on a projection plane so as to have a trapezoidal shape with respect to time change. Alternatively, the trajectory control device of the copying control robot described in 2. 4. The trajectory control unit (5) controls such that a velocity function of a projection point obtained by projecting a scanning trajectory on a projection plane is expressed by a polynomial function of time. 2. A trajectory control device for the copying control robot according to 2. 5. The trajectory control unit (5) controls so that a velocity function of a projection point obtained by projecting a scanning trajectory on a projection plane is represented by a transcendental function of time.
Alternatively, the trajectory control device of the copying control robot described in 2.