JP5458273B2 - Control device - Google Patents

Description

  The present invention relates to a control device for a manipulator having multiple joints, and more particularly to a control device that calculates an estimated external force acting on the tip of a manipulator.

  In the manufacturing process of automobiles, efforts are being made to automate by manipulators. For example, various processes such as welding and painting are automated by a teaching playback operation that reproduces an operation taught in advance.

  On the other hand, when different operations are possible depending on the external force acting on the manipulator, rather than just teaching playback, the applicable range of the manipulator will be further expanded, and in particular if the force acting on the tip of the manipulator can be detected, It is possible to perform an operation such as gripping the workpiece with an appropriate force.

  In order to detect external force acting on the tip of the manipulator, it is conceivable to provide a force sensor such as a load cell at the point of action. However, this increases the volume of the tip, increases the weight, and complicates the structure. Only the force acting on can be detected. When the volume and weight of the tip end portion are increased, it is inconvenient for operations such as gripping a workpiece.

  Thus, it has been proposed to detect an external force acting on the manipulator by software processing without providing a force sensor (see, for example, Patent Document 1 and Patent Document 2).

  In the control device for the manipulator described in Patent Document 2, the torque generation voltage of the motor that drives the joint is detected by a predetermined sensor, AD-converted and taken into the computer, and the joint torque is calculated via the disturbance observer (90A). . Thereafter, the joint torque is applied to the Jacobian matrix to obtain the external force acting on the tip (100). The Jacobian matrix is a matrix indicating the differential relationship among the minute displacement of the tip of the manipulator, the minute rotation angle of the posture, and the minute rotation angle of each joint, and is described in Non-Patent Document 1, for example.

JP-A-9-179632 JP-A-11-58285 (FIG. 7)

Shigeo Hirose, “Robotics-Vector analysis of mechanical systems”, 3rd edition, Hanabo Co., Ltd., October 25, 1990, p. 172-183

  However, since the control device described in Patent Document 2 uses a complex disturbance observer based on the normative model, the processing procedure is complicated and processing time is required even if a computer is used. Further, if the reference model is not properly defined, a calculation error occurs and sufficient accuracy cannot be obtained. Furthermore, the drive motor for each joint requires a sensor for detecting the torque generation voltage of the driver and an AD converter for reading the signal, which increases costs and delays in control due to detection processing. Will occur.

  The present invention has been made in consideration of such problems, and an object of the present invention is to provide a control device that can determine the force acting on the tip of the manipulator with high accuracy without a force sensor.

  A control device according to the present invention is a control device for a manipulator having multiple joints, a torque acquisition unit for obtaining a torque for each joint, and an angle for each joint to detect a Jacobian matrix related to the posture of the manipulator. A matrix calculation unit to be calculated, and a tip force calculation unit that calculates a force generated at the tip of the manipulator by coordinate conversion using the torque obtained by the torque acquisition unit and the Jacobian matrix obtained by the matrix calculation unit It is characterized by having.

  As described above, according to the process of applying the Jacobian matrix based on the torque for each joint, the external force acting on the tip of the manipulator can be obtained with high accuracy without a force sensor.

  A relation between a drive command for the drive motor provided for each joint and the torque is shown, and a torque map provided for each joint is provided, and the torque acquisition unit refers to the torque map to refer to the torque. You may ask for. By providing such a torque map, the torque of the drive motor can be obtained without a torque sensor, and there is no need for complicated calculation, fast processing can be realized, and there is no decrease in accuracy based on calculation errors. .

  A tip force command unit for obtaining a target tip force to be generated by the tip of the manipulator, a force deviation calculation unit for obtaining a force deviation between the target tip force and a calculated tip force obtained by the tip force calculation unit, and the force deviation Are integrated with each other to obtain a tip position increment of the manipulator, a current position calculator for calculating the tip position of the manipulator from each joint angle detected by a sensor, the tip position increment and the tip position. From the tip position command calculation unit that calculates the target tip position command by adding, the axis angle calculation unit that calculates the command value for each joint by matrix conversion of the tip position command, and the command value for each joint You may have an angle deviation calculation part which subtracts each shaft joint angle and calculates | requires each angle deviation, and a driver which drives the said manipulator based on each said angle deviation. Thereby, the tip of the manipulator can generate a force corresponding to a desired target tip force, which is suitable for gripping a workpiece, for example.

  The torque map shows a relationship between a single generated torque of the drive motor and the drive command, and the torque acquisition unit determines a predetermined disturbance from the generated torque of the single drive motor obtained by referring to the torque map. The torque may be obtained by subtracting force. Thereby, disturbance force (friction of a joint, a reduction gear, etc.) can be removed, and the more accurate external force at the tip can be obtained. Further, the torque map can be measured by a single drive motor without a manipulator, and is easy to create.

  The torque map may indicate a relationship between a composite generated torque of the drive motor and the joint and the drive command. Thereby, the disturbance force actually generated in the joint (including the speed reducer) is removed, and a more accurate external force at the tip can be obtained.

  According to the control device of the present invention, an external force acting on the tip of the manipulator can be obtained with high accuracy even without a force sensor based on a process of applying a Jacobian matrix based on the torque for each joint.

It is a schematic diagram of the holding | gripping apparatus controlled by the control apparatus which concerns on this Embodiment. It is a block block diagram of the control apparatus which concerns on this Embodiment. It is a figure which shows a torque map. It is a figure which shows a mode that a workpiece | work is hold | gripped with four fingers. It is a figure which shows the experimental result by the control apparatus which concerns on this Embodiment.

  Hereinafter, an embodiment of the control device according to the present invention will be described with reference to FIGS.

  As shown in FIG. 1, the control device 10 according to the present embodiment is a device that controls four fingers (manipulators) 14 included in the gripping device 12. The gripping device 12 grips and transports the workpiece 16 stored on the pallet using the four fingers 14. The gripping device 12 is provided at the tip of the articulated robot 17, so that the workpieces 16 in a stacked state (position and orientation are undefined) can be transported in a stable state in cooperation with the articulated robot 17. In FIG. 1, the workpiece 16 is shown in a simple cylindrical shape, but the shape, size, and orientation are not limited.

  Depending on the design conditions, the control device 10 may control the gripping device 12 by partially using data obtained by offline teaching by the external computer 11.

  The gripping device 12 includes a base plate 18 and fingers 14 provided at four corners on the lower surface of the base plate 18.

  The finger 14 is provided so as to protrude downward from the lower surface of the base plate 18. A first shaft 14 a that can pivot horizontally with the base plate 18 at the base end portion, and a direction orthogonal to the first shaft 14 a. A second shaft 14b that can be rotated and bends the first arm 20 and a third shaft 14c that can be rotated in the same direction as the second shaft 14b and bend the second arm 22 with respect to the first arm 20 are provided. The finger 14 further includes a motor 24a, a motor 24b and a motor 24c as drive motors for the first shaft 14a, the second shaft 14b and the third shaft 14c (hereinafter simply referred to as joints), the first shaft 14a, An angle sensor 26a that detects angles of the second shaft 14b and the third shaft 14c, an angle sensor 26b, and an angle sensor 26c are provided.

  Thus, the finger 14 has a multi-joint configuration having three axes, and the tip of the second arm 22 abuts on the workpiece 16. The four fingers 14 have the same configuration and are provided symmetrically with respect to the center portion of the base plate 18. There are no sensors (such as a force sensor or a torque sensor) in addition to the angle sensors 26a to 26c, and the finger 14 has a simple configuration. The motors 24a to 24c and the angle sensors 26a to 26c are not necessarily provided directly on the joint, and may be connected via a predetermined rotation transmission member. The motors 24a to 24c may be provided with a reduction gear. It is preferable to use servo motors for the motors 24a to 24c.

  Next, the control device 10 will be described. The control device 10 has a control unit for each finger 14, and a part or all of it is realized by software processing by a computer. The control device 10 is connected to a workpiece recognition device (not shown). The workpiece recognition device identifies individual workpieces 16 using optical means or the like, and supplies information indicating the position and orientation to the control device 10. Based on the information obtained from the workpiece recognition device, the control device 10 can be switched between the position control mode and the force control mode depending on the situation in order to properly grip the workpiece 16. FIG. 2 shows a block diagram of the control device 10. In FIG. 2, the configuration of the control device 10 for one finger 14 is representatively shown, but the other three are configured in the same manner. FIG. 2 shows a block diagram in the force control mode. Since the position control mode is not the gist of the invention of the present application, detailed description thereof is omitted. FIG. 2 is in the force control mode, but the portion below the broken line portion constitutes a position control loop as a basic configuration.

  As shown in FIG. 2, the control device 10 includes each axis angle calculation unit 62, control compensation units 64a, 64b, and 64c for each axis, angle deviation calculation units 65a, 65b, and 65c for each axis, It has drivers 66a, 66b, 66c for each axis.

  Each axis angle calculation unit 62 obtains the target angle of each axis from the tip position command Yc obtained by the tip position command calculation unit 60 by the inverse matrix of matrix conversion of the current position calculation unit 68.

  The control compensators 64a to 64c obtain the compensation amounts from the deviations obtained by the angle deviation calculators 65a to 65c using the PID elements, and supply them to the motor commands u1, u2, and u3, and supply them to the drivers 66a to 66c and a torque acquisition unit 72 described later. To do. The drivers 66a to 66c drive the motors 24a to 24c based on the input signals, and thereby the joints rotate.

  In FIG. 2, the portion above the broken line in the control device 10 is a portion for obtaining the tip position command Yc, and the tip position command calculation unit 60, force deviation calculation unit 69, tip force command unit 70, torque The acquisition unit 72, the tip force calculation unit 74, the matrix calculation unit 76, the force deviation compensation unit 78, the integration unit 80, and the current position calculation unit 68 are included.

  The current position calculation unit 68 reads the signals of the angle sensors 26a to 26c and obtains the calculation tip position Yf by a known matrix transformation.

  The tip position command calculation unit 60 calculates a tip position command Yc = Yf + ΔYc from the tip position increment ΔYc which is a change amount of the tip position command obtained by each function unit described later and the calculated tip position Yf obtained by the current position calculation unit 68. .

  The tip force command unit 70 determines the target tip force Fc to be generated by the second arm 22 at the tip of the finger 14 to be controlled, based on the operation state of the gripping device 12 and the state of the other three fingers 14. Ask.

  The force deviation calculation unit 69 obtains a force deviation εf between the target tip force Fc obtained from the tip force command unit 70 and the calculated tip force Ff obtained from the tip force calculation unit 74. The target tip force Fc and the calculated tip force Ff are vector quantities indicating the three-dimensional size and direction generated by the second arm 22 that is the tip portion of the finger 14.

  The force deviation compensation unit 78 obtains a compensation amount for the force deviation εf by the PID element, and supplies the compensation amount to the integration unit 80. The integration unit 80 converts the amount of force into a position amount by integrating the obtained compensation amount, and obtains the tip position increment ΔYc. The tip position command Yc is calculated by Yc = Yf + ΔYc of the tip position command calculation unit 60 and supplied to each axis angle calculation unit 62. The control compensation format in the control compensation units 64a to 64c and the force deviation compensation unit 78 is not limited to PID, but may be other formats such as PI according to design conditions.

  The torque acquisition unit 72 includes torque maps 82a, 82b, and 82c for each axis, and correction units 84a, 84b, and 84c.

As shown in FIG. 3, the torque map 82a is a map showing the relationship between the torque T1 ₀ to motor command u1 and the motor 24a is generated by itself, are recorded in a predetermined storage unit. Similarly, the torque maps 82b and 82c are maps showing the relationship between the motor commands u2 and u3 and the torques T2 ₀ and T3 ₀ generated by the motors 24b and 24c alone. The torque maps 82a to 82c are not limited to the map format, and may be experimental formulas or the like.

Correction unit 84a is a portion for correcting the disturbance force of the friction force and the like of the first shaft 14a which is driven by the motor 24a with respect to the torque T1 _0, subtracting the friction torque, such as the first shaft 14a and a predetermined reduction gear Then, the torque T1 substantially generated by the first shaft 14a is obtained. Similarly, the correction units 84b and 84c correct the torques T2 ₀ and T3 ₀ based on the friction torque of the second shaft 14b, the third shaft 14c, the speed reducer, etc., and the second shaft 14b and the third shaft 14c. Torques T2 and T3 that substantially generate are obtained.

The frictional forces of these mechanisms include relatively large static friction and relatively small dynamic friction, and the actual generation principle is complicated. In the control device 10 according to the present embodiment, torques T1 ₀ , T2 ₀ and T3 are provided in order to allow a margin and to simplify the calculation so that the force generated by the fingers 14 does not eventually become weak. _The static friction force is subtracted from ₀ . The compensation values by the correction units 84a to 84c may be included in the torque maps 82a to 82c in advance.

  In such a torque acquisition unit 72, the control compensators 64a to 64c remove disturbance forces (such as friction of joints and reducers), and the torques T1 to T3 are accurately obtained. Further, the torque maps 82a to 82c can be measured by the motors 24a to 24c alone without the finger 14, and can be easily created.

  The torque maps 82a to 82c may be set based on the combined torque generated by the motors 24a to 24c and each joint, the motor commands u1 to u3, and the actual measurement data. Thereby, the disturbance force actually generated in the joint (including the speed reducer) is removed.

  The matrix calculation unit 76 obtains the Jacobian matrix J of the finger 14 expressed by the equation (1) based on the angle signals supplied from the angle sensors 26a to 26c.

  Here, q is a joint angle of the first shaft 14a, the second shaft 14b, and the third shaft 14c obtained by the angle sensors 26a to 26c, and p is a tip position of the second arm 22. T indicates a transposed matrix. Based on this equation (1), the following equation (2) is derived.

Here, τ is a torque generated at the joint, that is, torques T1, T2, and T3, and a calculated tip force Ff is a force generated at the tip of the second arm 22, and is a three-dimensional vector quantity. When J ^T is shown in detail, the following equation (3) is obtained.

Here, ⁰ S ₁ , ⁰ S ₂ , ⁰ S ₃ are rotational direction vectors of the first shaft 14 a, the second shaft 14 b, and the third shaft 14 c, and ⁰ P ₁ , ⁰ P ₂ , ⁰ P ₃ are the first The position vectors of the axis 14a, the second axis 14b, and the third axis 14c, x is an operator indicating a vector product, and r is an identifier indicating the tip of the second arm 22. Based on the equations (2) and (3), the following equation (4) is derived.

Here, J (q) ^{T +} is a pseudo inverse matrix of J (q) ^T.

  It will be understood that the calculated tip force Ff as the force generated by the tip portion can be calculated by the torques T1, T2, and T3 according to the equation (4).

  The matrix calculator 76 obtains a determinant corresponding to the expression (4) based on the signals from the angle sensors 26a to 26c.

  Next, the tip force calculation unit 74 obtains the calculated tip force Ff by applying the torques T1, T2, and T3 to the determinant obtained by the matrix calculation unit 76, and supplies the calculated tip force Ff to the force deviation calculation unit 69. Next, the tip force calculation unit 74 and the matrix calculation unit 76 are distinguished so as to facilitate understanding, but may be substantially performed by one processing unit.

  Next, the operation of the control device 10 configured as described above will be described.

  First, the target postures of each of the four fingers 14 are determined by a predetermined position control mode, controlled so as to be in the postures, and the workpiece 16 is gripped.

  Next, for one or more fingers 14, a target tip force Fc as a vector amount is obtained by the tip force command unit 70 based on a predetermined gripping condition or the like.

  Since there are four fingers 14, for example, as shown in FIG. 4, the positions of the first finger 15a and the fourth finger 15d adjacent to each other are fixed in the position control mode, and the first finger 15a and the fourth finger 15d are fixed. For the second finger 15b and the third finger 15c, the target tip force Fc may be set and controlled as the force control mode. Thereby, the first finger 15a and the fourth finger 15d at the position can generate the reaction force Fx according to the force generated by the first finger 15a and the fourth finger 15d, and can grip the workpiece 16. it can.

  On the other hand, the torque acquisition unit 72 obtains torques T1, T2, and T3 generated in the joint at that time from the motor commands u1, u2, and u3 by the torque maps 82a, 82b, and 82c and the correction units 84a, 84b, and 84c. The matrix calculation unit 76 obtains an expression corresponding to the expression (4) based on the angle signals obtained from the angle sensors 26a to 26c.

  The tip force calculation unit 74 obtains the calculated tip force Ff at that time based on the formula obtained by the matrix calculation unit 76 and the torques T1 to T3 supplied from the torque acquisition unit 72.

  The force deviation calculation unit 69 obtains the force deviation εf, the force deviation compensation unit 78 performs a predetermined compensation process, and the integration unit 80 converts the force amount into a position amount to obtain the tip position increment ΔYc. In the tip position command calculating unit 60, the tip position increment ΔYc and the calculated tip position Yf are added to obtain a tip position command Yc, the angle of the joint is obtained from each tip angle command Yc by each axis angle calculating unit 62, For each axis, the angle deviation is calculated by the angle deviation calculation units 65a to 65c, and the motor commands u1, u2, and u3 are obtained by the control compensation units 64a to 64c.

  According to the control device 10 as described above, since a force feedback loop is formed with the force deviation calculation unit 69 as a subtraction point, the calculated tip force Ff gradually approaches the target tip force Fc, and the force deviation εf converges. Will do.

  Further, the calculated tip force Ff is obtained based on the angle sensors 26a to 26c and the motor commands u1 to u3, and a current sensor, a torque sensor, a force sensor and the like are unnecessary, and these input interfaces are also unnecessary. . In particular, since no sensor is required for the second arm 22 at the tip, the second arm 22 can be formed to be lightweight and thin, and can be inserted in a narrow space, and the weight load is small, so that the controllability is excellent.

  The finger 14 is provided with angle sensors 26a to 26c, which are naturally provided in a general manipulator, and this does not particularly complicate the configuration.

  Furthermore, when calculating the calculation tip force Ff, calculation based on a complicated normative model is unnecessary, processing time is short, and there is no calculation error due to the normative model.

  Furthermore, in the control device 10, the torques T1, T2, and T3 as a reference for obtaining the calculated tip force Ff can be easily and quickly obtained by referring to the torque maps 82a to 82c in the torque acquisition unit 72. The torque maps 82a to 82c are set based on the outputs of the actual motors 24a to 24c, and the correction units 84a to 84c compensate for external forces such as frictional forces. The calculated tip force Ff finally obtained is highly accurate.

  The inventor conducted an experiment for confirming the accuracy of the calculated tip force Ff obtained in the control process of the control device 10, and obtained the result shown in FIG. In this experiment, the target tip force Fc was changed stepwise from 0 to 20N, and the responses of the actual tip force Fr and the calculated tip force Ff were recorded. The tip force Fr was measured by providing a force sensor at the tip of the second arm 22.

  As a result, the tip force Fr finally becomes about 27N, and it has been confirmed that even if there is no dedicated sensor, an accuracy with which there is no practical problem in grasping the workpiece 16 is obtained. The reason why the calculated tip force Ff is calculated to be smaller than the tip force Fr as a whole is that the correction units 84a to 84c of the torque acquisition unit 72 subtract the external force corresponding to the static friction force. Therefore, in the correction units 84a to 84c, if the dynamic friction is subtracted according to the situation, the calculated tip force Ff is obtained with higher accuracy.

  In addition, the calculated front end force Ff obtained by calculation converges to 20 N, which is the same as the target front end force Fc. Furthermore, the convergence speed is about 0.4 sec and is sufficiently high, and there is almost no wasteful overshoot. Although illustration is omitted, similar preferable results were obtained when the target tip force Fc was 5N, 10N, and 15N.

  The number of fingers 14 of the gripping device 12 controlled by the control device 10 is not limited to four, and may be two or three, for example. Further, the movable finger 14 may be one, and the counterpart member that sandwiches the workpiece 16 may be a fixed member.

  The number of joints of the finger 14 is not limited to 3, and may be 2 or 4 or more.

  The control device according to the present invention is not limited to the above-described embodiment, and it is needless to say that various configurations can be adopted without departing from the gist of the present invention.

DESCRIPTION OF SYMBOLS 10 ... Control apparatus 12 ... Grasp apparatus 14 ... Finger 14a ... 1st axis | shaft 14b ... 2nd axis | shaft 14c ... 3rd axis | shaft 16 ... Workpiece | work 18 ... Base plate 20 ... 1st arm 22 ... 2nd arm 24a-24c ... Motor 26a-26c ... angle sensor 60 ... tip position command calculation unit 62 ... shaft angle calculation units 64a to 64c ... control compensation unit 66a to 66c ... driver 68 ... current position calculation unit 69 ... force deviation calculation unit 70 ... tip force command unit 72 ... torque acquisition Unit 74 ... tip force calculation unit 76 ... matrix calculation unit 78 ... force deviation compensation unit 80 ... integration units 82a to 82c ... torque map 84a to 84c ... correction unit Fc ... target tip force Ff ... calculation tip force Fr ... tip force T1 T3 ... Torque Yc ... Tip position command Yf ... Calculation tip position u1-u3 ... Motor command

Claims (5)

A control device for a manipulator having multiple joints,
A torque acquisition unit for obtaining torque for each joint;
A matrix calculator that detects an angle for each joint and obtains a Jacobian matrix related to the posture of the manipulator;
A tip force calculator that calculates a force generated at the tip of the manipulator by coordinate conversion using the torque obtained by the torque acquisition unit and the Jacobian matrix obtained by the matrix calculator;
A control device comprising: The control device according to claim 1,
Showing the relationship between the drive command for the drive motor provided for each joint and the torque, and having a torque map provided for each joint;
The said torque acquisition part calculates | requires the said torque with reference to the said torque map, The control apparatus characterized by the above-mentioned. The control device according to claim 2 , wherein
The torque map indicates a relationship between a single generated torque of the drive motor and the drive command,
The said torque acquisition part subtracts the predetermined disturbance force from the single generation | occurrence | production torque of the said drive motor obtained with reference to the said torque map, The control apparatus characterized by the above-mentioned. The control device according to claim 2 , wherein
The said torque map shows the relationship between the composite generation torque of the said drive motor and the said joint, and the said drive command, The control apparatus characterized by the above-mentioned. In the control device according to any one of claims 1 to 4 ,
A tip force command unit for obtaining a target tip force to be generated by the tip of the manipulator;
A force deviation calculation unit for obtaining a force deviation between the target tip force and the calculated tip force obtained by the tip force calculation unit;
An integration unit for integrating the force deviation to obtain a tip position increment of the manipulator;
A current position calculation unit for calculating a tip position of the manipulator from each joint angle detected by a sensor;
A tip position command calculation unit for adding the tip position increment and the tip position to obtain a target tip position command;
Each axis angle calculation unit for matrix conversion of the tip position command to obtain a command value for each joint;
An angle deviation calculator that subtracts each joint angle from the command value for each joint to obtain each angle deviation;
A driver for driving the manipulator based on each angular deviation;
Control device comprising a call with.

Images