JP2010221322A - Positioner - Google Patents

Abstract

<P>PROBLEM TO BE SOLVED: To provide a multi-axial positioner capable of adjusting a contact force, a position and a posture of a robot for assembling precise or minute components while positioning them according to assembling operation. <P>SOLUTION: The positioner 4 has a linear actuator 42 for driving a drive shaft 44 and a link mechanism 43 coupled via the drive shaft 44. The linear actuator 42 drives the drive shaft, thereby driving the link mechanism 43 for positioning the components. A controller 41 calculates the impedance of the positioner from a position, velocity and acceleration of a hand obtained by a displacement sensor 45 and a position coordinate conversion arithmetic part 414, and calculates a drive force of the linear actuator 42 from the calculated impedance of the positioner. <P>COPYRIGHT: (C)2011,JPO&INPIT

Description

  The present invention relates to a positioning apparatus for precisely positioning a component such as a semiconductor component or the like mounted by a robot, for example.

  When assembling semiconductor parts and precision parts manufactured by MEMS (Micro Electro Mechanical Systems) technology, precise positioning is required according to the parts, and contact between parts is detected accurately, resulting in excessive contact force. It is necessary to control the positioning so that it does not become.

  As an example of a device that performs such high-precision control, an arm that pressurizes a semiconductor element when mounted on a substrate is attached to an elastic guide constituted by a parallel leaf spring, and a pressure is applied by a voice coil motor. In addition, there is known one in which the pressure applied is controlled by adjusting the current flowing through the coil of the voice coil motor with a current regulator (Patent Document 1 below).

  Further, there is known one that feeds back a signal from a force sensor attached to a wrist of a robot to a servo control system (Patent Document 2 below). This means that while the robot is working, the force is monitored to recognize the position of the part and confirm the success of the work, while the force information is fed back to the control system to realize compliance control and improve the work reliability. It is what I did.

JP-A-7-86317 Japanese Patent Laid-Open No. 7-24665

  However, the conventional positioning control technique has the following problems.

  With the technique of the above-mentioned Patent Document 1, it is possible to apply a pressing force precisely when stationary, but it is difficult to control the contact force precisely by suppressing vibration during component conveyance or moving while pressing. Moreover, in order to detect a contact, it is necessary to add another contact sensor.

  With the technique of the above-mentioned patent document 2, it is difficult to work while maintaining a minute contact force due to the influence of the arm mass and the frictional force of the speed reducer.

  Also, in any of the techniques of Patent Documents 1 and 2, the position changes or the contact force becomes excessive due to the influence of the mass of the robot's previous hand or jig. In order to avoid such collision due to excessive mass, the work speed must be reduced and the work must be performed carefully, and the work time increases accordingly.

  Furthermore, since both the techniques of Patent Documents 1 and 2 have a single shaft structure, for example, complicated adjustment work is necessary and stable to perform complicated assembly work such as shaft insertion work and fillet soldering. It is difficult to perform the control.

  Specifically, in order to control the shaft insertion operation with a single-axis structure, translation in the XY axis direction and adjustment in the rotation direction around the XY axis are required. In addition, to control fillet soldering with a single-axis structure, it is necessary to control two translational directions orthogonal to the ridgeline. It is difficult to perform such control stably with a single shaft structure.

  In order to solve the above problems, the present invention can adjust the contact force and position / posture of a robot that performs assembly work while positioning precision or minute parts with a multi-axis structure in accordance with the assembly work. An object is to provide a positioning device.

  The present invention is a positioning apparatus for precisely positioning a component, and includes an actuator that drives a plurality of drive shafts, a link mechanism that is connected to the plurality of drive shafts and is driven by the actuators, and the link mechanism A component operating unit that is driven via a positioning unit to position the component, a measuring unit that measures a displacement of the component operating unit, a displacement measured by the measuring unit, a speed and an acceleration calculated from the displacement, and the actuator And a controller for calculating the driving force of the actuator based on the impedance of the mechanical system including the link mechanism and generating an output for driving the actuator in accordance with the calculated driving force.

  In the positioning device of the present invention, the controller includes a displacement of the component operation unit that is connected to a plurality of drive shafts and driven by an actuator, a speed and acceleration calculated from the displacement, an actuator, Based on the impedance of the mechanical system including the link mechanism, the driving force of the actuator is calculated, and an output for driving the actuator is generated according to the calculated driving force.

  In response to this output, the actuator drives a plurality of drive shafts and drives the link mechanism via each drive shaft, so the link mechanism has a plurality of degrees of freedom. The component operation unit is driven by this link mechanism. This makes it possible to finely adjust the position of the component operation unit as compared with the case where the component operation unit is driven by a single drive shaft.

  The controller can control the impedance of the mechanical system from the position, speed, and acceleration of the component operation unit. For this reason, positioning can be executed while maintaining a minute contact force.

  Therefore, according to the positioning device of the present invention, it is possible to adjust the contact force, position, and posture of the robot that performs the assembly operation while positioning a precise or minute part in accordance with the assembly operation.

  In the above invention, the measurement means is attached to each of the plurality of drive shafts, and a displacement sensor that measures the displacement of each drive shaft, and the displacement of each drive shaft measured by the displacement sensor, A position coordinate conversion calculation means for calculating a position, a speed of the component operation section, and an acceleration of the component operation section, and the controller includes a position of the component operation section calculated by the position coordinate conversion calculation means, A multi-degree-of-freedom impedance control calculation unit that calculates a driving force at a contact position based on the speed, the acceleration of the component operation unit, and the impedance of a mechanical system including the plurality of actuators, and the multi-degree-of-freedom impedance control calculation unit Based on the calculated driving force in the component operation unit, a driving force coordinate conversion operation for calculating the driving force for each driving shaft of the actuator is performed. And a drive current generation unit that generates a drive current corresponding to the drive force for each drive axis calculated by the drive force coordinate conversion calculation unit, and drives the actuator by the generated drive current to operate the link mechanism. It is preferable to provide.

  According to this aspect, the displacement sensor measures the displacement of each drive shaft, and the position coordinate conversion calculation means calculates the position of the component operation unit, the speed of the component operation unit from the displacement of each drive shaft measured by the displacement sensor, And the acceleration of the component operation unit is calculated. The multi-degree-of-freedom impedance control calculation means is based on the position of the component operation unit calculated by the position coordinate conversion calculation means, the speed of the component operation unit, the acceleration of the component operation unit, and the impedance of the mechanical system including a plurality of linear actuators. The driving force in the component operation unit is calculated. The driving force coordinate conversion calculating means calculates the driving force for each driving axis of the linear actuator from the driving force in the component operation unit calculated by the multi-degree-of-freedom impedance control calculating means. The drive current generation unit generates a drive current corresponding to the drive force for each drive axis calculated by the drive force coordinate conversion calculation unit, and drives the linear actuator with the generated drive current to operate the link mechanism.

  As a result, even when the position of the component operation unit cannot be directly measured, the position of the component operation unit can be calculated from the positions of a plurality of drive axes, and the driving force in the component operation unit can be calculated. In the apparatus, it is possible to adjust the contact force, position, and posture of a robot that performs assembling work while positioning precision or minute parts in accordance with the assembling work.

  In the above invention, the multi-degree-of-freedom impedance control calculation means includes the position of the component operation unit, the position of the component operation unit, the position of the component operation unit representing the impedance, the speed of the component operation unit, and the acceleration of the component operation unit. It is preferable to calculate the driving force in the component operation unit from the value obtained by multiplying the speed of the component and the acceleration of the component operation unit and the value of the external force set arbitrarily.

  According to this aspect, the multi-degree-of-freedom impedance control calculation means includes the position of the component operation unit, the position of the component operation unit representing the impedance, the speed of the component operation unit, and the acceleration of the component operation unit. The driving force in the component operation unit is calculated from the value obtained by multiplying the speed of the component and the acceleration of the component operation unit, and the value of the external force set arbitrarily.

  As a result, even in a multi-axis positioning apparatus, the contact force, position, and posture of a robot that performs assembly work while positioning precision or minute parts can be adjusted according to the assembly work.

  In the above-mentioned invention, in the positioning device according to claim 3, the multi-degree-of-freedom impedance control calculation means includes a stiffness matrix, a viscosity representing the rigidity, viscosity, and inertia of the link mechanism and the actuator by orthogonal coordinates of the tip of the device. It is preferable to use the matrix and the inertia matrix as feedback gains of the position of the component operation unit representing the impedance, the speed of the component operation unit, and the acceleration of the component operation unit.

  According to this aspect, the multi-degree-of-freedom impedance control calculation means calculates the impedance of the stiffness matrix, the viscosity matrix, and the inertia matrix that represent the rigidity, viscosity, and inertia of the link mechanism and the linear actuator by the orthogonal coordinates of the component operation unit. It is used as each feedback gain of the position of the component operation unit to be represented, the speed of the component operation unit, and the acceleration of the component operation unit.

  As a result, even when the displacement of the component operation unit cannot be directly measured, the position of the component operation unit can be calculated from the displacement of a plurality of drive shafts, and the driving force in the component operation unit can be calculated. In this case, it is possible to adjust the contact force, position, and posture of a robot that performs assembly work while positioning precision or minute parts in accordance with the assembly work.

  In the above invention, the controller includes the position of the component operation unit, the speed of the component operation unit, the acceleration of the component operation unit, the driving force of the component operation unit, the stiffness matrix, the viscosity matrix, and the inertia matrix of the device itself. It is preferable to further include an external force estimation calculation means for calculating an external force estimated value from the estimated value and notifying the outside.

  According to this aspect, the external force estimation calculation means includes the position of the component operation unit, the speed of the component operation unit, the acceleration of the component operation unit, the driving force of the component operation unit, the stiffness matrix, the viscosity matrix, and the inertia of the own device. The estimated value of the external force is calculated from the estimated value of the matrix and notified to the outside.

  As a result, the host controller of the positioning device can recognize the gravity and contact force from the component operation unit to the component from the estimated value of the external force, and can perform control according to the work state.

  According to the present invention, an actuator that drives a plurality of drive shafts, a link mechanism that is coupled to the plurality of drive shafts and is driven by the actuator, and is driven via the link mechanism to position the component. A positioning device control method for precisely positioning a component by driving the actuator and measuring a displacement of the component operating unit; and a displacement measured by the measuring unit And the speed and acceleration calculated from the displacement, and the impedance of the mechanical system including the actuator and the link mechanism, the driving force of the actuator is calculated, and the actuator is driven according to the calculated driving force. And a control step for generating.

  According to the present invention, the displacement of the component operation unit driven by the link mechanism connected to the plurality of drive shafts and driven by the actuator, the speed and acceleration of the component operation unit calculated from the displacement, the actuator, The driving force of the actuator is calculated based on the impedance of the mechanical system including the link mechanism, and an output for driving each actuator is generated according to the calculated driving force.

  Thereby, in the multi-axis positioning apparatus, the contact force, position, and posture of the robot that performs the assembly operation while positioning a precise or minute part can be adjusted according to the assembly operation.

The external view which shows the state which attached the positioning device to the front-end | tip of the arm of a robot. The schematic diagram which shows the example of 1 structure of the positioning device. Explanatory drawing of the dynamic model of a positioning device. The block diagram which shows the control system of a positioning device.

  As shown in FIG. 1, the positioning device according to the embodiment of the present invention includes a positioning mechanism 4 attached to the tip of an arm 2 of a robot 1 and a hand 3 that grips a part. The hand 3 corresponds to the component operation unit in the present invention. The positioning mechanism 4 gives compliance to the hand 3.

  As shown in FIG. 2, the positioning mechanism 4 includes n (n is a natural number of 2 or more, n = 2 in the present embodiment) linear actuators 42 (hereinafter, two actuators) attached to the arm 2 of the robot 1. 4-1, 42-2), a link mechanism 43 attached to the hand 3 and driven by the linear actuator 42, and a controller 41 (FIG. 4) for controlling the operation of the linear actuator 42.

  The linear actuator 42 is configured by, for example, a servo motor, a voice coil motor (VCM), or a cylinder mechanism. The link mechanism 43 has a plurality of degrees of freedom and is connected to the linear actuator 42 by two drive shafts 44 (hereinafter, the two drive shafts are indicated by 44-1 and 44-2).

  In FIG. 2, the link mechanism 43 has one end of a link 431 and one end of a link 434 connected at a connection point a, and the other end of the link 431 and one end of a link 432 are connected at a connection point b. The other end of the link 432 and one end of the link 433 are connected, and the other end of the link 433 and the other end of the link 434 are connected at the connection point d, and the parallel link is connected to the connection point d. Link 435 to be provided.

  The parallel link is connected to the first drive shaft 44-1 driven by the first linear actuator 42-1 at the connection point a. The other end of the link 435 whose one end is connected to the parallel link at the connection point d is connected to the second drive shaft 44-2 driven by the second linear actuator 42-2 at the point e. A parallel link 433 is connected to the hand 3. Accordingly, the link mechanism 43 has two degrees of freedom with respect to the X-Z axis and is driven by the two linear actuators 42-1 and 42-2.

  The operation of the control system of the positioning device is executed by arithmetic processing in the controller 41 based on the following dynamic model, which will be described below.

  In FIG. 3, when the right direction of the coordinate x representing the position of the hand 3 of the positioning device is positive (+), the equation of motion of the positioning device is expressed as the following equation (1).

fa + fd = Max "+ Dax '+ Kax (1)
However, x ′ and x ″ represent the speed and acceleration of the positioning device.

Ma: Inertia matrix of the positioning device (n × n)
Da: Viscosity matrix of the positioning device (n × n)
Ka: Stiffness matrix of the positioning device (n × n)
n: degree of freedom of the link mechanism 43 fa: driving force generated by the positioning device fd: external force (gravity, contact force, etc.)
The inertia matrix, the viscosity matrix, and the stiffness matrix represent the inertia, viscosity, and rigidity of the link mechanism 43 and the linear actuator 42 by orthogonal coordinates of the tip portion (position of the hand 3) of the positioning device.

  The driving force fa is expressed by the following equation.

fa = -Mcx "-Dcx'-Kcx + fc (2)
Here, fc is a force necessary for gravity cancellation and work, Mc, Dc, and Kc are feedback gains of acceleration, speed, and position, and are parameters that are arbitrarily determined.

  Hereinafter, the position x, velocity x ′, and acceleration x ″ of the tip of the positioning device may be referred to as device position, device speed, and device acceleration, respectively.

From the above formulas (1) and (2),
fd = Mvx ″ + Dvx ′ + Kvx−fc (3)
Is guided. However, Mv, Dv, and Kv are the impedances of the dynamic model represented by the following formulas (4) to (6).

Mv = Ma + Mc (4)
Dv = Da + Dc (5)
Kv = Ka + Kc (6)
Accordingly, by appropriately setting the parameters Mc, Dc, and Kc, impedances (Mv, Dv, Kv) that determine the response of the mechanical system to the external force can be determined. That is, the controller 41 can control the impedance of the mechanical system.

  The driving force fa may be expressed by the following equation without the terms of acceleration feedback (−Mcx ″) and velocity feedback (Dcx ′) in equation (2).

fa = -Kcx + fc (7)
In the case of the above formula (7), the response is the inertia and viscosity inherent in the mechanical system.

  In the link mechanism 43, it may be difficult to attach a sensor to the tip of the positioning device (in this case, the hand 3), and the device position x, device speed x ′, and device acceleration x ″ may not be directly measured. In such a case, a sensor is attached to the tip of the linear actuator of each axis, and the device position x, device speed x ′ and device acceleration x ″ are obtained from the position (angle) of the linear actuator. Good.

  In general, the device position x, the device speed x ′, and the device acceleration x ″ are expressed by the following equations (8) to (10) using the axial position q.

x = T (q) (8)
x ′ = Jq ′ (9)
x ″ = J′q ′ + Jq ″ (10)
However,
T: Position conversion formula q: Position (angle) of each axis
q ′: speed of each axis q ″: acceleration of each axis J: Jacobian matrix J ′: time derivative of J.

  FIG. 4 is a functional block diagram including a control device (controller) of the positioning mechanism 4 shown in FIG.

  The control system of the present embodiment includes a link mechanism 43 having n = 2 degrees of freedom with respect to the XZ axis, a multi-degree-of-freedom impedance control calculation unit 411, a driving force coordinate conversion calculation unit 412, a driving current generation unit 413, A controller 41 having a position coordinate conversion calculation unit 414 and an external force estimation calculation unit 415, two linear actuators 42-1 and 42-2 controlled by the controller 41 to drive the link mechanism 43, and attached to each linear actuator. Displacement sensors 45-1 and 45-2 for measuring the position (displacement) of each linear actuator are included. As the displacement sensor 45, for example, an proximity sensor, a strain cage, a linear encoder, or the like is used.

  Note that the displacement sensor 45 and the position coordinate conversion calculation unit 414 constitute a measuring means in the present invention.

  As described above, since the embodiment is a positioning device with n = 2 degrees of freedom, the linear actuators 42-1 and 42-1 are provided and the drive shafts 44-1 and 44 connected to each linear actuator. The link mechanism 43 is driven via -2. In the case of n = 3, there are three sets of linear actuators, displacement sensors, and drive shafts. In other words, the positioning device has at least the number of linear actuators and drive shafts corresponding to the degree of freedom n.

  The displacement sensor 45 is attached to the linear actuator 42 and measures the position (angle) q of the tip of the linear actuator 42. In the illustrated example, two displacement sensors 45-1 and 45-2 are attached to the linear actuators 42-1 and 42-2. Hereinafter, the position of the tip of the linear actuator 42 measured by the displacement sensor 45 may be referred to as the position of the drive shaft.

  The position coordinate conversion calculation unit 414 uses the above equations (8) to (10) to calculate the device position x and the device speed from the position (position of each drive shaft) q of each linear actuator 42 measured by the displacement sensor 45. x ′ and device acceleration x ″ are calculated.

  The multi-degree-of-freedom impedance control calculation unit 411 receives the device position x, the device speed x ′, the device acceleration x ″ calculated by the position coordinate conversion calculation unit 414, and the host controller (in this case, the controller of the robot 1). Based on the parameter information, the driving force to be generated by the positioning device is obtained, where the parameter information is information necessary for the calculations according to the above formulas (1) to (6) for gravity cancellation and work. And the parameters Mc, Dc, and Kc indicating the feedback gain.

  The driving force coordinate conversion calculation unit 412 converts the driving force obtained by the multi-degree-of-freedom impedance control calculation unit 411 into the driving force of the linear actuator 42. The driving force required by the multi-degree-of-freedom impedance control calculation unit 411 is a driving force to be generated by the positioning device, that is, a driving force at work coordinates (coordinates of the device position x).

  Therefore, the driving force coordinate conversion calculation unit 412 converts the driving force at the work coordinates into a driving force for each linear actuator 42. Further, the driving force coordinate conversion calculation unit 412 converts the driving force for each linear actuator 42 into a current instruction value indicating the value of the current that drives the linear actuator 42.

  The driving force for each linear actuator is expressed by the following equation.

τ = J ^T fa (11)
However,
τ: Driving force of each driving shaft J: Jacobian matrix (x ′ = Jq ′)
J ^T : J transposition matrix q: Position (angle) of each drive shaft.

  The driving force coordinate conversion calculation unit 412 obtains the driving force for each linear actuator using the above equation (10), and converts the obtained driving force into a current instruction value.

  The drive current generation unit 413 generates a drive current corresponding to the current instruction value for each linear actuator converted by the drive force coordinate conversion calculation unit 412 and supplies the drive current to the linear actuator to drive the linear actuator. To do. Thereby, the linear actuator 42 operates the link mechanism 43 to perform positioning.

  The external force estimation calculation unit 415 is based on the driving force obtained by the multi-degree-of-freedom impedance control calculation unit 411 and the device position x, device speed x ′, and device acceleration x ″ calculated by the position coordinate conversion calculation unit 414. Estimate the external force.

The external force is obtained from the above equation (1). Fd = Max ″ + Dax ′ + Kax−fa (12)
It is represented by

  However, since the true values of the inertia matrix Ma, the viscosity matrix Da, and the stiffness matrix Ka of the positioning device are not known in practice, the values of the inertia matrix Ma, the viscosity matrix Da, and the stiffness matrix Ka in the above equation (12) are estimated values. Is used.

  Thereby, the estimated value <fd> of the external force is expressed by the following equation (13).

<fd> = <Ma> x ″ + <Da> x ′ + <Ka> x−fa (13)
However,
<Ma>: Inertial matrix estimate
<Da>: Estimated viscosity matrix
<Ka>: Estimated value of stiffness matrix The external force estimation calculation unit 415 obtains an external force estimated value using the above equation (13).

  Next, the operation of the control system of this embodiment will be described.

  First, the position of the linear actuator 42 is measured by the displacement sensor 45. The displacement sensor 45 notifies the position coordinate conversion calculation unit 414 of the measured position of the linear actuator 42.

  The position coordinate conversion calculation unit 414 uses the position of the linear actuator 42 of each axis measured by the displacement sensor 45 and the above equations (8) to (10) to calculate the device position x, the device speed x ′, and the device acceleration x ″. The position coordinate conversion calculation unit 414 notifies the calculated device position x, device speed x ′, and device acceleration x ″ to the multi-degree-of-freedom impedance control calculation unit 411 and the external force estimation calculation unit 415.

  The multi-degree-of-freedom impedance control calculation unit 411 receives the notified device position x, device speed x ′ and device acceleration x ″, and parameter information input from the host controller (gravity cancellation and force fc required for work, and Using the parameters Mc, Dc, Kc), a driving force fa (n-dimensional vector) to be generated by the positioning device is calculated based on the above equation (2), and the calculated driving force is converted into a driving force coordinate conversion calculation unit. 412 and the external force estimation calculation unit 415.

  The driving force coordinate transformation calculation unit 412 uses the notified driving force fa, device position x, device speed x ′, and device acceleration x ″, according to the above equation (11), to drive force τ (axis drive) of each linear actuator. Then, the calculated driving force τ of each linear actuator is converted into a current instruction value indicating a current value for driving the linear actuator, and the driving force coordinate conversion calculation unit 412 is a current instruction value. Is notified to the linear actuator 42.

  The conversion from the driving force τ to the current instruction value may be performed based on a predetermined formula, or a conversion table in which the driving force and the current instruction value are associated in advance is created. A conversion table may be used.

  The drive current generation unit 413 generates a drive current corresponding to the notified current instruction value, and outputs (supplies) to the linear actuator 42. As a result, the linear actuator 42 operates the link mechanism 43 in accordance with the driving force calculated by the driving force coordinate conversion calculation unit 412 to position the hand 3.

  On the other hand, the external force estimation calculation unit 415 receives the notified driving force fa, device position x, device speed x ′, device acceleration x ″, inertia matrix estimated value <Ma>, viscosity matrix estimated value <Da>, and stiffness. The estimated external force value <fd> is calculated based on the above equation (13) using the estimated value <Ka> of the matrix.

  The estimated values <Ma>, <Da>, <Ka> of the inertia matrix, viscosity matrix, and stiffness matrix are compliance design values. The controller 41 determines an operation mode and a set value for compliance according to a command from the host controller. The external force estimation calculation unit 415 uses the compliance set values determined by the controller 41 as these estimated values <Ma>, <Da>, and <Ka>.

  The external force estimation calculation unit 415 notifies the calculated external force estimated value <fd> to the host controller. The host controller detects the work state from the estimated external force value notified from the external force estimation calculation unit 415, confirms the work result, and determines the operation of the robot 1.

  As described above, in this embodiment, the controller 41 detects the displacement of the hand 3 driven by the linear actuator 42 via the drive shaft 44 and the link mechanism 43, and the speed and acceleration of the component operation unit calculated from this displacement. Then, based on the impedance of the mechanical system including the linear actuator 42 and the link mechanism 43, the driving force of the linear actuator 42 is calculated, and an output for driving each linear actuator is generated according to the obtained driving force.

  Therefore, in the positioning device of the embodiment, the linear actuator 42 that drives the drive shaft 44 and the link mechanism 43 are connected by the drive shaft 44, and the link mechanism 43 is driven by the linear actuator 42 driving the drive shaft 44. The parts are positioned by the hand 3 as a part operation unit.

  Here, the displacement sensor 45 measures the displacement of each drive shaft 44 that connects the linear actuator 42 and the link, and the position coordinate conversion calculation unit 414 determines the hand from the displacement of each drive shaft 44 measured by the displacement sensor 33. A device position x, a device speed x ′, and a device acceleration x ″ represented by the position 3 are calculated.

  The multi-degree-of-freedom impedance control calculation unit 411 is based on the device position x, the device speed x ′ and the device acceleration x ″ calculated by the driving force coordinate conversion calculation unit 412 and the impedance of the mechanical system including the plurality of linear actuators 42. A driving force fa at the position of the hand 3 is calculated.

  The driving force coordinate conversion calculation unit 412 calculates a driving force for each driving shaft 44 of the linear actuator from the driving force at the position of the hand 3 calculated by the multi-degree-of-freedom impedance control calculation unit 411.

  The drive current generation unit 413 generates a drive current corresponding to the drive force for each drive shaft 44 calculated by the drive force coordinate conversion calculation unit 412, and drives the linear actuator 42 with the generated drive current to cause the link mechanism 43 to operate. The position of the hand 3 is determined by operating.

  As described above, since the linear actuator 42 drives the drive shaft 44 and drives the link mechanism 43 via each drive shaft 44, the link mechanism 43 has a plurality of degrees of freedom. The hand 3 is driven by the link mechanism 43. Therefore, the position of the hand 3 can be finely adjusted as compared with the case where the hand 3 is driven by a single drive shaft.

  Further, the multi-degree-of-freedom impedance control calculation unit 411 can control the impedance from the position, speed, and acceleration of the hand 3. Therefore, it is possible to execute positioning while maintaining a minute contact force.

  Further, since the position coordinate conversion calculation unit 414 calculates the position, speed, and acceleration of the hand 3 from the position of each drive shaft 44 measured by the displacement sensor 45, even if the position of the hand 3 cannot be directly measured, 3 position, velocity and acceleration can be obtained.

  Thereby, even when the position of the hand 3 cannot be directly measured, the position of the hand 3 can be calculated from the positions of the plurality of drive shafts 44, and the driving force in the hand 3 can be calculated. It is possible to adjust the contact force, position, and posture of a robot that performs assembling work while positioning precision or minute parts in accordance with the assembling work.

  Further, the multi-degree-of-freedom impedance control calculation unit 411 multiplies the feedback gains Mc, Dc, Kc of the device position x, the device speed x ′, and the device acceleration x ″ by the device position x, the device speed x ′, and the device acceleration x ″. The driving force at the position of the hand 3 is calculated from the value obtained in this way and the value of the external force set arbitrarily.

  That is, the multi-degree-of-freedom impedance control calculation unit 411 converts the rigidity matrix, the viscosity matrix, and the inertia matrix of the link mechanism 43 and the linear actuator 42 by the orthogonal coordinates of the tip of the apparatus, the apparatus position x, the apparatus These are used as feedback gains for velocity x ′ and device acceleration x ″.

  As a result, even when the position of the hand 3 cannot be directly measured, the position of the hand 3 can be calculated from the positions of the plurality of drive shafts 44 and the driving force in the hand 3 can be calculated. It is possible to adjust the contact force and position / posture of a robot that performs assembly work while positioning precision or minute parts in accordance with the assembly work.

  Further, in the present embodiment, the external force estimation calculation unit 415 includes the device position x, the device speed x ′, the device acceleration x ″, the driving force of the hand 3, the estimated values of the device stiffness matrix, the viscosity matrix, and the inertia matrix. Then, the estimated value of the external force is calculated and notified to the host controller.

  Thereby, the host controller can recognize the gravity and contact force acting on the parts from the hand 3 attached to the link mechanism 43 controlled by the positioning device from the estimated value of the external force, and control according to the work state. Can be performed.

  Although the embodiment has been described as described above, the present invention is not limited to the above embodiment.

  For example, in the above embodiment, the actuator that drives the drive shaft is a linear actuator, and the linear actuator and the drive shaft are connected in a one-to-one relationship. However, the actuator that drives the drive shaft is not limited to a linear actuator. Any drive shaft can be used.

  Further, the connection between the actuator and the drive shaft is not limited to one-to-one, and a plurality of drive shafts may be driven by one actuator.

  DESCRIPTION OF SYMBOLS 1 ... Robot, 2 ... Arm, 3 ... Hand, 4 ... Positioning mechanism, 41 ... Controller, 42-1, 42-2 ... Linear actuator, 43 ... Link mechanism, 44-1, 44-2 ... Drive shaft, 45- DESCRIPTION OF SYMBOLS 1,45-2 ... Displacement sensor, 411 ... Multi-degree-of-freedom impedance control calculating part, 412 ... Driving force coordinate conversion calculating part, 413 ... Driving current generation part, 414 ... Position coordinate conversion calculating part, 415 ... External force estimation calculating part.

Claims (6)

A positioning device for precisely positioning a component,
An actuator for driving a plurality of drive shafts;
A link mechanism coupled to the plurality of drive shafts and driven by the actuator;
A component operation unit that is driven via the link mechanism to position the component;
Measuring means for measuring the displacement of the component operation unit;
Based on the displacement measured by the measuring means, the speed and acceleration calculated from the displacement, and the impedance of the mechanical system including the actuator and the link mechanism, the driving force of the actuator is calculated, and according to the calculated driving force And a controller for generating an output for driving the actuator. The positioning device according to claim 1,
The measuring means includes
A displacement sensor attached to each of the plurality of drive shafts for measuring the displacement of each drive shaft;
Position coordinate conversion calculation means for calculating the position of the component operation unit, the speed of the component operation unit, and the acceleration of the component operation unit from the displacement of each drive shaft measured by the displacement sensor,
The controller is
The driving force at the contact position is calculated based on the position of the component operation unit, the speed of the component operation unit, the acceleration of the component operation unit, and the impedance of the mechanical system including the plurality of actuators calculated by the position coordinate conversion calculation unit. A multi-degree-of-freedom impedance control calculating means for calculating;
A driving force coordinate conversion calculating means for calculating a driving force for each driving axis of the actuator from a driving force in the component operation unit calculated by the multi-degree-of-freedom impedance control calculating means;
Drive current generation means for generating a drive current corresponding to the drive force for each drive axis calculated by the drive force coordinate conversion calculation means, and driving the actuator by the generated drive current to operate the link mechanism. A positioning device. The positioning device according to claim 2, wherein
The multi-degree-of-freedom impedance control calculation means includes:
Obtained by multiplying the feedback gains of the position of the component operation unit, the speed of the component operation unit, and the acceleration of the component operation unit representing the impedance by the position of the component operation unit, the speed of the component operation unit, and the acceleration of the component operation unit. A positioning device that calculates a driving force in a component operation unit from a value obtained and an arbitrarily set external force value. The positioning device according to claim 3, wherein
The multi-degree-of-freedom impedance control calculation means includes:
The rigidity matrix, the viscosity matrix, and the inertia matrix representing the rigidity, viscosity, and inertia of the link mechanism and the actuator by the orthogonal coordinates of the tip of the apparatus, the position of the component operation unit representing the impedance, the speed of the component operation unit, and A positioning apparatus characterized by being used as each feedback gain of acceleration of a component operation unit. In the positioning device according to any one of claims 1 to 4,
The controller is
From the position of the component operation unit, the speed of the component operation unit, the acceleration of the component operation unit, the driving force of the component operation unit, and the estimated values of the stiffness matrix, the viscosity matrix, and the inertia matrix of the own device, the estimated value of the external force A positioning apparatus further comprising external force estimation calculation means for calculating and notifying the outside. An actuator for driving a plurality of drive shafts;
A link mechanism coupled to the plurality of drive shafts and driven by the actuator;
And a component operation unit that is driven via the link mechanism to position the component, and is a control method of a positioning device that precisely positions the component by driving the actuator,
A measurement step for measuring the displacement of the component operation unit;
Based on the displacement measured by the measuring means, the speed and acceleration calculated from the displacement, and the impedance of the mechanical system including the actuator and the link mechanism, the driving force of the actuator is calculated, and the calculated driving force is calculated. And a control step of generating an output for driving the actuator according to the control method.

Images