JP5383911B2 - Robot controller - Google Patents

Description

  The present invention relates to a robot control apparatus that drives a robot using an actuator such as a motor.

  The robot can realize a complex operation such as an assembling operation by teaching the operation to reproduce and execute the operation desired by the operator in the process of passing the operation via point specified by the operator. There are two ways for the operator to teach the robot to move: indirect teaching using the operation panel to specify the position and orientation of each joint of the robot, the tip of the robot and the tool, and the degree of freedom of the tip using a joystick, etc. There is a direct teaching in which an operator operates a pointing device imitating the above or a handle attached to the tip.

  Since direct teaching using the handle is intuitive to the operator, there is an advantage that the time required for teaching is shortened. Direct teaching can be achieved by specifying the position and orientation with the servos of some of the robot's joints in a free state, or applying only the torque necessary to maintain the robot's posture when stationary. There are methods (gravity compensation). However, in order for the operator to move the joint, it is necessary to apply an external force that overcomes the frictional force caused by the joint speed reducer, so the operability is not very good. Therefore, a method of constructing a force control system in which an operation force by an operator is input by providing a force sensor at a handle attachment portion may be used. There is also a case in which a force sensor is further attached to detect a collision between a tool such as a hand and an object (for example, see Patent Document 1).

  However, force sensors are expensive and vulnerable to impacts, so use of force sensors is often avoided. In addition, since the force sensor is often attached near the tip of the robot, the external force applied to the link portion cannot be detected.

  As a method for detecting a collision at the tip of the robot without using an additional sensor, the required drive torque of the joint is calculated from a joint position command or the like, and the calculated required drive torque and a motor for driving each joint are calculated. A method is known in which a collision of the tip of the robot is discriminated by comparing with a driving torque obtained from the current (see, for example, Patent Document 2).

Japanese Patent No. 3307788 Japanese Patent No. 3878054

  Thus, the conventional technique can detect the collision of the tip of the robot. In the case of direct teaching, teaching is required not only at the tip portion but also by operation to each link portion. However, in the prior art, it is not possible to detect a collision of the link portion, that is, an external force applied to the link portion. Therefore, direct instruction to the link part could not be performed.

  Accordingly, an object of the present invention is to provide a robot control apparatus capable of directly teaching not only the tip portion but also each link portion.

  One aspect of the present invention is a robot control apparatus that controls a robot having a joint axis and a drive axis that transmits a driving force to the joint axis, the actuator driving the joint axis for each control period; and the drive axis A drive shaft angle detector that detects the angle of the joint; an joint axis angle calculator that calculates the angle of the joint axis from the angle of the drive shaft; and a tip position calculator that calculates the tip position of the robot from the angle of the joint axis A position error calculating unit that calculates a position error between the position command value of the tip position and the tip position, and a work coordinate system and a joint coordinate system on the joint axis based on an angle of the joint axis. A Jacobian matrix calculation unit that calculates a partial Jacobian matrix between them, integrates the partial Jacobian matrix to calculate a Jacobian matrix between the work coordinate system and the joint coordinate system at the tip position, and calculates a joint angle difference Joint angle difference calculation A torque command value calculation unit that calculates a torque command value of a joint based on the joint angle difference, a drive unit that drives the actuator based on the torque command value, and an angle of the joint axis. A driving torque estimating unit that estimates a driving torque for driving, and an external force acting joint in which an external force is applied from the external torque while calculating a difference between the estimated driving torque and the torque command value as an external torque An external torque calculation unit that estimates an axis, an external force calculation unit that calculates an external force acting on the external force acting joint axis from the external torque and the partial Jacobian matrix, and a compliance model that stores a compliance model in the external force acting joint axis Using the storage unit and the compliance model, the external force acting joint shaft is operated according to the external force. A compliance correction amount calculation unit that calculates a compliance correction amount for a coordinate system position; and a joint angle difference correction amount calculation unit that calculates the joint angle difference correction amount from the compliance correction amount and the partial Jacobian matrix. The angle difference calculation unit calculates a joint angle difference before correction by inverse kinematics calculation from the position error and the Jacobian matrix, and calculates a sum of the joint angle difference before correction and the joint angle difference correction amount as a joint angle difference. Is output as

  According to the present invention, it is possible to provide a robot control apparatus capable of directly teaching not only the front end portion of the robot but also each link portion.

1 is a block diagram showing a robot control apparatus according to an embodiment of the present invention. Schematic which shows an example of the robot which is a control object. Schematic which shows the movable part of the robot shown in FIG. The block diagram which shows the drive torque estimation part which concerns on one Embodiment. The flowchart which shows operation | movement of the robot control apparatus by one Embodiment. The flowchart which shows an example of the drive torque estimation process which concerns on one Embodiment. The block diagram which shows the robot control apparatus by the modification of one Embodiment. The flowchart which shows the compliance parameter correction amount calculation process which concerns on the modification of one Embodiment. Schematic which shows the other example of a control object.

  Embodiments of the present invention will be described below with reference to the drawings. In the following description of the drawings, the same or similar parts are denoted by the same or similar reference numerals. However, the drawings are schematic, and specific dimensions should be determined in consideration of the following description. Moreover, it is a matter of course that portions having different dimensional relationships and ratios are included between the drawings.

  Further, the following embodiments exemplify apparatuses and methods for embodying the technical idea of the present invention, and the technical idea of the present invention is the material, shape, structure, and arrangement of component parts. Etc. are not specified below. The technical idea of the present invention can be variously modified within the scope of the claims.

  As shown in FIG. 1, a robot control apparatus according to an embodiment of the present invention includes a central processing unit (CPU) 1, an actuator 100, a drive unit (amplifier) 101, a drive shaft angle detection unit 102, a position A data storage unit 200, a link parameter storage unit 201, a friction coefficient storage unit 202, a Jacobian matrix storage unit 203, a force / moment data storage unit 204, and a compliance model storage unit 205 are provided.

  A robot as an example of a control target in this embodiment includes a main body 300 and a movable portion 310 provided in the main body 300 as schematically shown in FIG. The movable portion 310 includes a plurality of links 301 and 302, a plurality of joints 306 and 307 including a driving pulley 303 and a driven pulley 304, and a transmission mechanism (for example, a belt) wound around the driving pulley 303 and the driven pulley 304. 308.

  As schematically shown in FIG. 3, the drive pulley 303, the speed reducer 309, the actuator 100, and the drive shaft angle detection unit 102 are attached to the drive shaft (joint) 306. The actuator 100 is rotationally driven every control cycle, and the speed reducer 309 decreases the rotational speed and increases the torque. On the other hand, a driven pulley 304 and a joint axis angle detection unit 305 are attached to a joint axis (joint) 307. As the drive shaft 306 rotates, the joint shaft 307 is rotationally driven via the drive pulley 303, the transmission mechanism 308, and the driven pulley 304. In the robot control apparatus of this embodiment, a case where the drive shaft 306 and the joint shaft 307 are controlled will be described for the sake of simplicity.

  Each of the drive shaft angle detection unit 102 and the joint shaft angle detection unit 305 can use a position sensor such as an encoder, and may include a filter that removes a predetermined frequency component. The drive shaft angle detection unit 102 detects a displacement amount (drive shaft angle) of the position of the drive shaft 306. The joint axis angle detection unit 305 detects the displacement amount (joint axis angle) of the position of the joint axis 307. Further, the joint axis angle may be calculated using the drive shaft angle, the reduction ratio of the reduction gear 309, and the transmission ratio of the transmission mechanism 308 instead of being detected by the joint axis angle detection unit 305.

  1 includes a joint axis angle calculation unit 103, a tip position calculation unit 104, a Jacobian matrix calculation unit 105, a drive torque estimation unit 106, an external torque calculation unit 107, an external force calculation unit 108, Calculated external force limiting unit 109, compliance correction amount calculating unit 110, joint angle difference correction amount calculating unit 111, position command value generating unit 112, position error calculating unit 113, joint angle difference calculating unit 114, torque command The value calculation unit 115 is logically provided as a module (logic circuit) that is a hardware resource.

  Based on the drive shaft angle calculated by the drive shaft angle detection unit 102, the joint axis angle calculation unit 103 depends on the reduction ratio of the speed reducer 309 and the ratio of the drive shaft 306 and the joint shaft 307 of the transmission mechanism 308. Calculate the joint axis angle. Note that the joint axis angle may be directly calculated by the joint axis angle detection unit 305 attached to the joint axis 307.

  The tip position calculation unit 104 reads the link parameter from the link parameter storage unit 201, and uses the joint axis angle calculated by the joint axis angle calculation unit 103 and the read link parameter to perform the robot by forward kinematics calculation. The tip position in the work coordinate system is calculated.

The Jacobian matrix calculation unit 105 calculates a Jacobian matrix from the joint axis angle calculated by the joint axis angle calculation unit 103 and stores it in the Jacobian matrix storage unit 203. The Jacobian matrix is a matrix expressing a minute displacement relationship between the work coordinate system and the joint coordinate system of the robot. When the Jacobian matrix is J, the error Δx of the tip position of the robot and the joint angle difference Δθ satisfy the relationship of Expression (1).

Δx = JΔθ (1)

  The drive torque estimation unit 106 is necessary for driving the joint axis 307 of the robot using the drive axis angle calculated by the drive axis angle detection 102 and the joint axis angle calculated by the joint axis angle calculation unit 103. The correct driving torque. A specific example of the drive torque estimating unit 106 is shown in FIG. As shown in FIG. 4, the drive torque estimation unit 106 of this specific example includes a drive shaft speed calculation unit 400, a frictional force torque calculation unit 401, an inertial force torque calculation unit 402, a gravity torque calculation unit 403, and an addition. Part 404.

  Based on the joint axis angle calculated by the joint axis angle calculation unit 103, the drive axis speed calculation unit 400 calculates the drive axis speed by taking, for example, a time difference between the joint angles. The frictional force torque calculation unit 401 reads the friction coefficient stored in the friction coefficient storage unit 202, and uses the drive shaft speed calculated by the drive shaft speed calculation unit 400 and the read friction coefficient to perform Coulomb friction. Friction force torque corresponding to viscous friction or the like is calculated.

  The inertia force torque calculation unit 402 includes a speed calculation unit 410, an acceleration calculation unit 411, an inertia moment calculation unit 412, and an inertia force torque calculation unit 413. The speed calculation unit 410 calculates a joint angular velocity using the joint axis angle calculated by the joint axis angle calculation unit 103. The acceleration calculation unit 411 calculates joint angular acceleration based on the joint angular velocity calculated by the speed calculation unit 410. The inertia moment calculation unit 412 reads the link parameter from the link parameter storage unit 201, and calculates the inertia moment of each link 301, 302 using the link parameter and the joint axis angle calculated by the joint axis angle calculation unit 103. . The inertial force torque calculation unit 413 calculates the inertial force torque using the joint angular acceleration calculated by the acceleration calculation unit 411 and the inertia moment calculated by the inertia moment calculation unit 412.

  The gravitational torque calculation unit 403 reads the link parameter from the link parameter storage unit 201, and uses the read link parameter and the joint axis angle calculated by the joint axis angle calculation unit 103, to each link 301, 302. The acting gravity is calculated, and a gravity torque that compensates for the calculated gravity is calculated.

  The addition unit 404 adds the friction force torque calculated by the friction force torque calculation unit 401, the inertia force torque calculated by the inertia force torque calculation unit 413, and the gravity torque calculated by the gravity torque calculation unit 403. The sum is output as the estimated driving torque.

  The external torque calculation unit 107 illustrated in FIG. 1 calculates a difference between the drive torque estimated by the drive torque estimation unit 106 and the torque command value calculated by the torque command value calculation unit 116 as an external torque.

ここで、ｚ_ｉは第ｉ番目の関節軸の関節座標系の関節軸回転方向ベクトル、ｐ_ｉはベース座標系から見た第ｉ番目の関節軸の関節座標系原点の位置ベクトルである。式（４）において、記号「×」はベクトルの外積を示す。第ｉ番目の関節軸を外力作用関節と呼ぶと、外力作用関節において作用する外力ｆ_ｄｉは、次の式（５）を用いて外力算出部１０８によって算出することができる。

ｆ_ｄｉ＝（Ｊ_ｉ ^Ｔ）^−１τ_ｄｉ …（５）
The external force calculation unit 108 calculates an external force using the external torque calculated by the external torque calculation unit 107 and the Jacobian matrix calculated by the Jacobian matrix calculation unit 105. From the principle of virtual work, the external force f _d is calculated by multiplying the external torque τ _d by the inverse matrix of the transposed matrix J ^T of the Jacobian matrix J as shown in the following equation (2).

f _d = (J ^T ) ⁻¹ τ _d (2)

Here, when an external force acts on the link portion instead of the tip portion of the robot, the external torque of the drive shaft near the tip portion is zero. Therefore, a partial vector that does not become zero in each element of the external torque vector τ _d is denoted by τ _di, and this partial vector τ _di is represented by the following equation (3).

τ _di = (τ _di1 ,..., τ _dii ) (3)

That is, the external force acting on the i-th joint axis from the base is not zero, but the external force acting on the joint axis from the (i + 1) -th to the tip is zero. In this case, an external force acting on the i-th joint axis is obtained by multiplying the inverse matrix of the transposed matrix J _i ^T of the partial Jacobian matrix J _i up to the i-th joint axis. The partial Jacobian matrix J _i from the base to the i-th joint axis is a part of the Jacobian matrix, and is obtained using the following equation (4).

Here, z _i is a joint axis rotation direction vector of the joint coordinate system of the i-th joint axis, and p _i is a position vector of the joint coordinate system origin of the i-th joint axis viewed from the base coordinate system. In Expression (4), the symbol “x” indicates the outer product of vectors. When the i-th joint axis is referred to as an external force acting joint, the external force f _di acting on the external force acting joint can be calculated by the external force calculating unit 108 using the following equation (5).

f _di = (J _i ^T ) ⁻¹ τ _di (5)

  109 reads the allowable force and allowable moment data (allowable value) stored in the force / moment data storage unit 204, and performs saturation processing if the calculated external force exceeds the allowable value. That is, the allowable value is output as the calculated external force. If it is smaller than the set minimum value, the calculated external force is calculated as 0 (dead zone process). When the calculated external force is not less than the minimum value and not more than the allowable value, the calculated external force is output as it is.

The compliance correction amount calculation unit 111 reads a compliance model from the compliance model storage unit 205 and calculates a position correction amount according to the output of the calculated external force limit unit 109 using the read compliance model. Here, the compliance model virtually assumes inertia, viscosity, and rigidity with the contact target as shown in, for example, Expression (6).

Md ² Δx / dt ² + DdΔx / dt + KΔx = K _f Δf (6)

Here, Δx is an error in the work coordinate system set for the external force acting joint, dΔx / dt is a speed in the work coordinate system, d ² Δx / dt ² is an acceleration vector in the work coordinate system, M is an inertia matrix, D is a viscosity coefficient matrix, K is a stiffness coefficient matrix, and _Kf is a force feedback gain matrix. Compliance selection matrix for switching an axis to the action of force and location is in a form including a force feedback gain matrix K _f. The error speed dΔx / dt and the acceleration vector d ² Δx / dt ² can be approximated by a one-time difference and a two-time difference with respect to the time of the position error vector Δx, respectively. Therefore, the compliance correction amount Δx _comp at the external force acting joint is It can be calculated using the equation (7).

Δx _comp = 1 / K (K _f Δf−Md ² Δx / dt ² −DdΔx / dt) (7)

The joint angle difference correction amount calculation unit 111 calculates the joint angle difference correction amount from the compliance correction amount Δx _comp to the external force acting joint using the inverse matrix of the partial Jacobian matrix J _i using the following equation (8). .

Δθ _comp = J _i ⁻¹ Δx _comp (8)

  The position command value generation unit 112 reads the target tip position data stored in the position data storage unit 200, and calculates an interpolated tip position command value in each control cycle from the target tip position data.

Position error calculator 113, a tip end position command value x _R generated by the position command value generating unit 112, based on the current tip position x calculated by the distal end position calculating section 104, the following equation (9) The position error Δx is calculated using this.

Δx = x _R −x (9)

The joint angle difference calculation unit 114 uses the error Δx calculated by the position error calculation unit 113 and the inverse matrix J ⁻¹ of the Jacobian matrix J, and calculates the previous joint angle difference to the joint angle difference based on the error Δx. The correction amount Δθ _comp is added and calculated as in the following equation (10).

Δθ = J ⁻¹ Δx + Δθ _comp (10)

The torque command value calculation unit 115 generates a torque command value (control target value) by integrating the joint angle difference calculated by the joint angle difference calculation unit 114. The drive unit 101 drives the actuator 100 for each control cycle according to the torque command value calculated by the torque command value calculation unit 115.

  Examples of the position data storage unit 200, the link parameter storage unit 201, the friction coefficient storage unit 202, the Jacobian matrix storage unit 203, the force / moment data storage unit 204, and the compliance model storage unit 205 include a semiconductor memory, a magnetic disk, an optical disk, A magneto-optical disk or a magnetic tape can be used.

  The position data storage unit 200 stores a target tip position data string used by the position command value generation unit 111 to generate a tip position command value. The link parameter storage unit 201 stores link parameters related to the robot links 301 and 302. The friction coefficient storage unit 202 stores friction coefficient data used in advance by the friction force torque calculation unit 401 to calculate the friction force torque, which is obtained in advance from the speed-torque relationship in the constant speed operation. The Jacobian matrix storage unit 203 stores the Jacobian matrix calculated by the Jacobian matrix calculation unit 105. The force / moment data storage unit 204 stores a target tip force data string used by the force command value generation unit 109 to generate a tip position force command value. The compliance model storage unit 205 stores a preset compliance model.

(Robot control method)
Next, the operation of the robot control apparatus according to the present embodiment will be described with reference to the flowchart shown in FIG.

  (A) First, control calculation is started, and the target tip position data string is read from the position data storage unit 200 by the position command value generation unit 112. Based on the target tip position data string, the tip position in each control cycle A command value is generated (steps S100 and S101). Next, in step S <b> 102, an error between the tip position command value generated by the position command value generation unit 112 and the tip position calculated by the tip position calculation unit 104 is calculated by the position error calculation unit 113. Subsequently, in step S103, with respect to the error calculated by the position error calculation unit 113, the joint angle difference calculation unit 114 uses the Jacobian matrix read from the Jacobian matrix storage unit 203 as shown in Expression (10). Then, an inverse kinematic calculation is performed to calculate a joint angle difference in consideration of a joint angle difference correction amount described later.

  Thereafter, in step S104, the torque command value is calculated by the torque command value calculation unit 115 by integrating the joint angle difference calculated by the joint angle difference calculation unit 114. Next, in step S105, the actuator 100 is driven by the drive unit 101 using the torque command value calculated by the torque command value calculation unit 115 as a control target value, whereby the drive shaft 306 is driven and the tip position is controlled. The Subsequently, in step S106, the end of the control calculation is confirmed, and in step S107, the servo process is ended. If the control calculation is not completed in step S106, the process proceeds to step S108 described later.

  (B) Next, in step S108, the drive shaft angle detector 102 detects the drive shaft angle. Next, in step S109, the joint axis angle is calculated by the joint axis angle calculation unit 103 from the drive axis angle calculated by the drive axis angle detection unit 102 based on the reduction ratio of the speed reducer. In step S110, the tip position calculation unit 104 reads the link parameter from the link parameter storage unit 201, and uses the read link parameter and the joint axis angle calculated by the joint axis angle calculation unit 103. The tip position is calculated by forward kinematics calculation. In step S <b> 111, the Jacobian matrix is calculated by the Jacobian matrix calculator 105 using the joint axis angle calculated by the joint axis angle calculator 103.

  (C) Next, in step S112, the drive torque estimation unit 106 uses the drive shaft angle calculated by the drive shaft angle detection unit 102 and the joint axis angle calculated by the joint axis angle calculation unit 103 to drive torque. Is estimated. In step S <b> 113, the external torque calculation unit 107 calculates the external torque from the difference between the drive torque estimated by the drive torque estimation unit 106 and the actual torque command value calculated by the torque command value calculation unit 116. In step S114, the external force calculated by the external torque calculation unit 107 and the partial Jacobian matrix calculated by the Jacobian matrix calculation unit 105 are used by the external force calculation unit 108 to calculate the external force shown in Expression (5). .

  (D) Next, in step S115, an allowable force / moment data string is read from the force / moment data storage unit 204, and is calculated by the read allowable force / moment data string and the external force calculation unit 108. The external force is compared by 109, and the limited external force is output based on the comparison result. In other words, saturation processing or dead zone processing is performed by the calculated external force limiting unit 109.

  (E) Next, in step S116, the compliance correction amount calculation unit 110 reads out the compliance model from the compliance model storage unit 205, and uses the compliance model to limit by the calculated external force limiting unit 109 as shown in Expression (7). The amount of correction at the external force acting joint according to the external force is calculated by the compliance correction amount calculation unit 110. In step S117, the joint angle difference correction amount is calculated by the joint angle difference correction amount calculation unit 111 as shown in Expression (8) using the partial Jacobian matrix from the compliance correction amount. Returning to step S103, as shown in Expression (9), an inverse matrix of the Jacobian matrix is added to the error between the tip position command value generated by the position command value generation unit 112 and the tip position calculated by the tip position calculation unit 104. The position error calculation unit 113 calculates a joint angle difference in consideration of the correction amount calculated from the joint angle difference correction amount calculation unit 111.

(Driving torque estimation process)
Next, the drive torque estimation process in step S112 shown in FIG. 5 will be described with reference to the flowchart shown in FIG.

  (A) In step S200, a drive torque estimation process is started. In step S201, the drive shaft speed calculation unit 400 calculates the drive shaft speed by taking a time difference from the drive shaft angle detected by the drive shaft angle detection unit 102. Calculated. Further, the friction coefficient is read from the friction coefficient storage unit 202, and the friction force torque calculation unit 401 uses the read friction coefficient and the drive shaft speed calculated by the drive shaft speed calculation unit 400 to generate the friction force. Torque is calculated.

  (B) In step S202, the joint angular velocity is calculated by the velocity calculation unit 410 by taking the time difference between the joint axis angles calculated by the joint axis angle calculation unit 103. In step S 203, the joint angular acceleration is calculated by the acceleration calculating unit 411 by taking the time difference of the joint angular velocity by the speed calculating unit 410. In step S204, the link parameter is read from the link parameter storage unit 201, and the moment of inertia calculation unit 412 uses the joint axis angle calculated by the joint axis angle calculation unit 103 and the read link parameter. The moment of inertia of the link at each joint is calculated. In step S205, the inertial force torque calculation unit 413 calculates the inertial force torque based on the joint angular acceleration calculated by the acceleration calculation unit 411 and the inertial moment calculated by the inertial moment calculation unit 412.

  (C) In step S206, the link parameter is read from the link parameter storage unit 201, and based on the joint axis angle calculated by the joint axis angle calculation unit 103 and the read link parameter, gravity is determined. The torque calculating unit 403 calculates the gravity acting on each of the links 301 and 302, and calculates the gravity torque that compensates for this.

  (D) In step S207, the friction force torque calculated by the friction force torque calculation unit 401, the inertia force torque calculated by the inertia force torque calculation unit 413, and the gravity torque calculated by the gravity torque calculation unit 403 are obtained. It is added by the adding unit 404 and calculated as an estimated driving torque. In step S208, the driving torque estimation process ends.

  As described above, according to the present embodiment, it is possible to estimate the external force acting on each link, and by performing a flexible operation with the compliance model set based on the estimated external force, Simple direct teaching can be realized.

(Modification)
A robot control apparatus according to a modification of this embodiment is shown in FIG. The robot control device of this modification is a control device capable of changing the parameters of the compliance model. The robot control apparatus of this modification has a configuration in which a compliance parameter correction unit 116 is newly provided in the robot control apparatus of this embodiment shown in FIG.

  A compliance model for an external force applied by an operator for direct teaching is different from a compliance model at the time of collision, and is usually handled by changing parameters of the compliance model. However, without the use of additional sensors, it is difficult to clearly distinguish between the intended external force and the external force caused by a collision. Can not work.

  Therefore, as in the modification shown in FIG. 7, by providing the compliance parameter correction unit 116, it is possible to operate stably even in the event of a collision. The compliance parameter correction unit 116 changes the parameters of the compliance model from the history of the compliance correction amount calculated by the compliance correction amount calculation unit 110, and writes this in the compliance model storage unit 205. At the start of operation, it is a parameter of the compliance model for collision, and the correction amount is generated by direct teaching, so the parameter is changed only for the generated external force component, so that flexibility is improved and operability is improved. Can do.

  Further, the correction of the parameters of the compliance model may be performed at regular intervals. The period for correcting this parameter may take several times the control period. As a result, there is an effect that the operation of the robot does not vibrate by correcting the parameters.

  Next, the operation of the compliance parameter correction unit of this modification will be described with reference to the flowchart shown in FIG.

  In step S301, the compliance parameter correction process is started. In step S302, it is confirmed whether or not the parameters of the compliance model are to be corrected in this control cycle (parameter correction cycle). When not correcting, it progresses to step S304 and complete | finishes. In the case of correction, the process proceeds to step S303, the compliance parameter correction unit 116 corrects the parameters of the compliance model, the correction result is written in the compliance model storage unit 205, and the process ends in step S304.

  As described above, the embodiments of the present invention have been described. However, it should not be understood that the descriptions and drawings constituting a part of this disclosure limit the present invention. From this disclosure, various alternative embodiments, examples and operational techniques will be apparent to those skilled in the art.

  For example, in the embodiment of the present invention, when calculating the inertial force torque, the joint angular acceleration calculated by the twice difference regarding the time of the joint axis angle by the speed calculating unit 410 and the acceleration calculating unit 411 is used. Depending on the control cycle, the time delay increases and the torque error tends to increase. In that case, as shown in FIG. 9, the acceleration of the link 301 is detected from an acceleration sensor 311 mounted on the link 301, and the acceleration calculation unit 411 converts the detected acceleration into a joint angular acceleration. The inertial force torque may be calculated based on the basis.

  In addition, as an example of the drive torque estimation process, an example is shown in which the drive torque is estimated using the frictional force torque, the inertial force torque, and the gravity torque, but is not limited thereto. For example, parameters such as centrifugal force and Coriolis force may be further considered.

  The CPU 1, the position data storage unit 200, the link parameter storage unit 201, the friction coefficient storage unit 202, the Jacobian matrix storage unit 203, the force / moment data storage unit 204, the compliance model storage unit 205, etc. It may be embedded in and integrated. Further, the CPU 1, the position data storage unit 200, the link parameter storage unit 201, the friction coefficient storage unit 202, the Jacobian matrix storage unit 203, the force / moment data storage unit 204, the compliance model storage unit 205, etc. It is also possible to remotely control the robot by wire or wireless.

  As described above, the present invention naturally includes various embodiments not described herein. Therefore, the technical scope of the present invention is defined only by the invention specifying matters according to the scope of claims reasonable from the above description.

1 Central processing unit (CPU)
DESCRIPTION OF SYMBOLS 100 Actuator 101 Drive part 102 Drive shaft angle detection part 103 Joint axis angle calculation part 104 Tip position calculation part 105 Jacobian matrix calculation part 106 Drive torque estimation part 107 External torque calculation part 108 External force calculation part 109 Calculated external force restriction part 110 Compliance correction amount Calculation unit 111 Joint angle difference correction amount calculation unit 112 Position command value generation unit 113 Position error calculation unit 114 Joint angle difference calculation unit 115 Torque command value calculation unit 115 Compliance parameter correction amount calculation unit 200 Position data storage unit 201 Link parameter storage unit 202 Friction coefficient storage unit 203 Jacobian matrix storage unit 204 Force / moment data storage unit 205 Compliance model storage unit 300 Main body 301, 302 Link 303 Drive pulley 304 Driven pulley 305 Joint shaft angle detection Protruding part 306 Drive shaft (joint)
307 Joint axis (joint)
308 Transmission mechanism 309 Reducer 310 Movable part 311 Acceleration sensor 400 Drive shaft speed calculation part 401 Friction force torque calculation part 402 Inertia force torque calculation part 403 Gravity torque calculation part 404 Addition part 410 Speed calculation unit 411 Acceleration calculation unit 412 Inertia moment calculation Unit 413 Inertial torque calculation unit

Claims (4)

A robot control apparatus for controlling a robot having a joint axis and a drive axis that transmits a driving force to the joint axis,
An actuator for driving the joint axis for each control cycle;
A drive shaft angle detector for detecting the angle of the drive shaft;
A joint axis angle calculator that calculates the angle of the joint axis from the angle of the drive axis;
A tip position calculator that calculates the tip position of the robot from the angle of the joint axis;
A position error calculation unit for calculating a position error between the position command value of the tip position and the tip position;
A Jacobian matrix calculation unit that calculates a Jacobian matrix between the working coordinate system and the joint coordinate system at the end position based on the angle of the joint axis,
A joint angle difference calculating unit for calculating a joint angle difference;
A torque command value calculation unit for calculating a torque command value of a joint based on the joint angle difference;
A drive unit for driving the actuator based on the torque command value;
A drive torque estimation unit that estimates a drive torque necessary to drive the joint axis from an angle of the joint axis ;
An external torque calculator that calculates the difference between the estimated drive torque and the torque command value as an external torque;
From the external torque and the Jacobian matrix, a partial vector of the external torque in which the element of the external torque is not 0 is obtained, and a partial Jacobian matrix related to an external force acting joint axis on which an external force is applied in relation to the partial vector is obtained. An external force calculation unit that calculates an external force acting on the external force acting joint axis from a partial Jacobian matrix and the partial vector ;
A compliance model storage unit for storing a compliance model in the external force acting joint axis;
Using the compliance model, a compliance correction amount calculation unit that calculates a compliance correction amount for a work coordinate system position at the external force acting joint axis according to the external force;
A joint angle difference correction calculation unit for calculating the compliance correction amount and the partial Jacobian matrix or Luo Takashi Seki angle difference correction amount,
With
The joint angle difference calculation unit calculates a joint angle difference before correction by inverse kinematics calculation from the position error and the Jacobian matrix, and calculates a sum of the joint angle difference before correction and the joint angle difference correction amount as a joint. robot controller you output as the angular difference. A correction amount storage unit for storing the compliance correction amount;
A compliance parameter correction unit for correcting a compliance parameter from a plurality of correction amounts stored in the correction amount storage unit;
Further comprising Ru claim 1, wherein the robot control device. The compliance parameter modifying section is a robot control apparatus according to claim 2, wherein that run in larger cycle than the control cycle. The external force calculated by the external force calculation unit is compared with an allowable value. When the external force exceeds the allowable value, the external force is set as an allowable value, and when the external force is less than the minimum value, the external force is set as 0. the robot control apparatus according to any one of claims 1 to 3 calculates the external force limiting unit that further include that.

Images