[go: up one dir, main page]

WO2011161765A1 - ロボット制御装置 - Google Patents

ロボット制御装置 Download PDF

Info

Publication number
WO2011161765A1
WO2011161765A1 PCT/JP2010/060543 JP2010060543W WO2011161765A1 WO 2011161765 A1 WO2011161765 A1 WO 2011161765A1 JP 2010060543 W JP2010060543 W JP 2010060543W WO 2011161765 A1 WO2011161765 A1 WO 2011161765A1
Authority
WO
WIPO (PCT)
Prior art keywords
joint
torque
external force
unit
calculates
Prior art date
Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
Ceased
Application number
PCT/JP2010/060543
Other languages
English (en)
French (fr)
Inventor
淳一郎 大賀
準治 大明
小川　秀樹
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Toshiba Corp
Original Assignee
Toshiba Corp
Priority date (The priority date is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the date listed.)
Filing date
Publication date
Application filed by Toshiba Corp filed Critical Toshiba Corp
Priority to EP10853625.1A priority Critical patent/EP2586577A4/en
Priority to PCT/JP2010/060543 priority patent/WO2011161765A1/ja
Priority to JP2012521201A priority patent/JP5383911B2/ja
Publication of WO2011161765A1 publication Critical patent/WO2011161765A1/ja
Priority to US13/677,761 priority patent/US8489238B2/en
Anticipated expiration legal-status Critical
Ceased legal-status Critical Current

Links

Images

Classifications

    • B—PERFORMING OPERATIONS; TRANSPORTING
    • B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
    • B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
    • B25J9/00—Programme-controlled manipulators
    • B25J9/02—Programme-controlled manipulators characterised by movement of the arms, e.g. cartesian coordinate type
    • B—PERFORMING OPERATIONS; TRANSPORTING
    • B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
    • B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
    • B25J9/00—Programme-controlled manipulators
    • B25J9/16—Programme controls
    • B25J9/1628—Programme controls characterised by the control loop
    • B25J9/1633—Programme controls characterised by the control loop compliant, force, torque control, e.g. combined with position control
    • G—PHYSICS
    • G05—CONTROLLING; REGULATING
    • G05B—CONTROL OR REGULATING SYSTEMS IN GENERAL; FUNCTIONAL ELEMENTS OF SUCH SYSTEMS; MONITORING OR TESTING ARRANGEMENTS FOR SUCH SYSTEMS OR ELEMENTS
    • G05B19/00—Programme-control systems
    • G05B19/02—Programme-control systems electric
    • G05B19/42—Recording and playback systems, i.e. in which the programme is recorded from a cycle of operations, e.g. the cycle of operations being manually controlled, after which this record is played back on the same machine
    • G05B19/423—Teaching successive positions by walk-through, i.e. the tool head or end effector being grasped and guided directly, with or without servo-assistance, to follow a path

Definitions

  • the present invention relates to a robot control apparatus that drives a robot using an actuator such as a motor.
  • the robot can realize complex operations such as assembly work by teaching the operation to reproduce and execute the operation desired by the operator while the robot passes through the operation via point specified by the operator.
  • complex operations such as assembly work by teaching the operation to reproduce and execute the operation desired by the operator while the robot passes through the operation via point specified by the operator.
  • the operator can 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.
  • 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.
  • gravitation compensation 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.
  • 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).
  • 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).
  • the conventional technology can detect the collision of the tip of the robot.
  • teaching is required not only at the tip portion but also by operation to each link portion.
  • 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.
  • 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
  • An output unit 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 the actuator based on an angle of the joint axis
  • a driving torque estimation unit that estimates a driving torque for driving the motor, an external force action 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 a joint 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 that stores a compliance model in the external force acting joint axis Using the model storage unit and the compliance
  • FIG. 1 is a block diagram showing a robot control apparatus according to an embodiment of the present invention.
  • Schematic shows an example of the robot which is a control object.
  • Schematic 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
  • 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.
  • a robot control apparatus 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 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.
  • 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.
  • a driven pulley 304 and a joint axis angle detection unit 305 are attached to a joint axis (joint) 307.
  • the joint shaft 307 is rotationally driven via the drive pulley 303, the transmission mechanism 308, and the driven pulley 304.
  • 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.
  • the value calculation unit 115 is logically provided as a module (logic circuit) that is a hardware resource.
  • 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.
  • 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.
  • 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.
  • the drive shaft speed calculation unit 400 calculates the drive shaft speed by taking, for example, a time difference of the joint angle based on the joint axis angle calculated by the joint axis angle calculation unit 103.
  • 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 inertial force torque calculation unit 402 includes a speed calculation unit 410, an acceleration calculation unit 411, an inertia moment calculation unit 412, and an inertial 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 for compensating 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 of the drive shaft near the tip portion is zero.
  • ⁇ 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).
  • z i is a joint axis rotation direction vector of the joint coordinate system of the i-th joint axis
  • p i is a position vector of the joint coordinate system origin of the i-th joint axis viewed from the base coordinate system.
  • the symbol “x” indicates the outer product of vectors.
  • the 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.
  • the compliance model virtually assumes inertia, viscosity, and rigidity with the contact target as shown in, for example, Expression (6).
  • ⁇ 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 2 ⁇ x / dt 2 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
  • 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 2 ⁇ x / dt 2 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 2 ⁇ x / dt 2 ⁇ Dd ⁇ x / dt) (7)
  • the position command value generation unit 112 reads the target tip position data stored in the position data storage unit 200, and calculates the tip position command value interpolated 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 ⁇ 1 of the Jacobian matrix J, and the previous joint angle difference is calculated based on the joint angle difference based on the error ⁇ x.
  • the correction amount ⁇ comp is added and calculated as in the following equation (10).
  • ⁇ J ⁇ 1 ⁇ 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.
  • step S100 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).
  • 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.
  • 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.
  • 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.
  • 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.
  • 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.
  • step S108 the drive shaft angle detector 102 detects the drive shaft angle.
  • 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.
  • 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.
  • 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.
  • 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.
  • 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.
  • 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). .
  • 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.
  • 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.
  • 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.
  • 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.
  • step S112 shown in FIG. 5 Driving torque estimation process
  • step S200 drive torque estimation processing is started.
  • 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.
  • step S202 the joint angular velocity is calculated by the velocity calculating unit 410 by taking the time difference between the joint axis angles calculated by the joint axis angle calculating unit 103.
  • 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.
  • 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.
  • 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.
  • 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.
  • 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.
  • 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.
  • the compliance model for external force applied by the operator for direct teaching is different from the compliance model at the time of collision, and it is usually handled by changing the parameters of the compliance model.
  • 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.
  • 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.
  • the correction of the compliance model parameters 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.
  • step S301 the compliance parameter correction process is started.
  • 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).
  • step S304 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.
  • 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.
  • the time delay increases and the torque error tends to increase.
  • 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.
  • the drive torque is estimated using the frictional force torque, the inertial force torque, and the gravity torque, but is not limited to this.
  • parameters such as centrifugal force and Coriolis force may be further considered.

Landscapes

  • Engineering & Computer Science (AREA)
  • Robotics (AREA)
  • Mechanical Engineering (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Automation & Control Theory (AREA)
  • Manipulator (AREA)

Abstract

［課題］先端部だけでなく、各リンク部への直接教示ができるロボット制御装置を提供することを可能にする。［解決手段］先端位置と位置指令値との位置誤差から関節角差分を算出する関節角差分算出部（１１４）と、関節角差分からトルク指令値を算出するトルク指令値算出部（１１５）と、アクチュエータ（１００）の駆動トルクを推定する駆動トルク推定部（１０６）と、駆動トルク及びトルク指令値との差を外部トルクとして算出するとともに外部トルクから外力が作用している外力作用関節軸を推定する外部トルク算出部（１０７）と、関節軸角度に基づいて部分ヤコビ行列を算出するとともにヤコビ行列を算出するヤコビ行列算出部（１０５）と、部分ヤコビ行列及び外部トルクから外力作用関節軸に作用する外力を算出する外力算出部（１０８）と、コンプライアンスモデルを用いて外力に応じて外力作用関節軸での作業座標系位置に対するコンプライアンス修正量を算出するコンプライアンス修正量算出部（１１０）と、コンプライアンス修正量と部分ヤコビ行列から関節角差分修正量を算出する関節角差分修正量算出部（１１１）と、を備え、関節角差分算出部は、位置誤差とヤコビ行列から逆運動学計算により修正前の関節角差分を算出し、この修正前の関節角差分と関節角差分修正量との和を関節角差分として出力する。

Description

ロボット制御装置
　本発明は、モータなどのアクチュエータを用いてロボットを駆動するロボット制御装置に関する。
　操作者により指定された動作経由点をロボットが通過していく過程で、操作者が所望する動作の再現実行を行う動作の教示により、ロボットは組立作業などの複雑な動作を実現できる。操作者がロボットに動作を教示する方法としては、操作盤を用いてロボットの各関節やロボットの先端およびツールの位置および姿勢を指定していく間接教示と、ジョイスティックなどを用いた先端の自由度を模したポインティング装置あるいは先端部に取り付けたハンドルを操作者が操作する直接教示とがある。
　ハンドルを用いた直接教示は操作者にとって直感的であるため、教示にかかる時間を短縮させるというメリットがある。直接教示を実現させる方法としては、ロボットの一部の関節のサーボをフリー状態にして位置姿勢を指定する方法、静止時においてロボットの姿勢を維持するために必要なトルクのみを関節に与えておく方法(重力補償)などがある。しかし、操作者が関節を動かすためには、関節の減速機に起因する摩擦力に打ち勝つだけの外力を与える必要があるため、操作性はあまり良くない。そこでハンドルの取り付け部に力覚センサを設けて、操作者による操作力を入力とする力制御系を構築する方法が用いられることがある。またハンドなどのツールと対象物との衝突を検出するためにさらに力覚センサを取り付けているケースもある（例えば、特許文献１参照）。
　しかし力覚センサは高価である上に、衝撃に対して脆弱であるため、力覚センサの使用が敬遠されることが多い。また、力覚センサはロボットの先端部付近に取り付けられることが多いため、リンク部へ付加された外力の検出はできない。
　付加的なセンサを用いずに、ロボットの先端部の衝突を検出する方法としては、関節の位置指令などから関節の必要駆動トルクを算出し、算出した必要駆動トルクと、各関節を駆動するモータの電流から求まる駆動トルクとを比較することでロボットの先端部の衝突の判別を行う方法が知られている（例えば、特許文献２参照）。
特許第３３０７７８８号公報 特許第３８７８０５４号公報
　このように、従来の技術は、ロボットの先端部の衝突の検出を行うことができる。直接教示の場合においては、先端部だけでなく、各リンク部への操作による教示が必要となるが、従来技術では、リンク部の衝突、すなわちリンク部へ付加された外力を検出することができないので、リンク部への直接教示を行うことができなかった。
　そこで、本発明は、先端部だけでなく、各リンク部への直接教示が可能なロボット制御装置を提供することを目的とする。
　本発明の一態様は、関節軸及び前記関節軸に駆動力を伝達する駆動軸を有するロボットを制御するロボット制御装置であって、前記関節軸を制御周期毎に駆動するアクチュエータと、前記駆動軸の角度を検出する駆動軸角度検出部と、前記駆動軸の角度から前記関節軸の角度を算出する関節軸角度算出部と、前記関節軸の角度から前記ロボットの先端位置を算出する先端位置算出部と、前記先端位置の位置指令値と、前記先端位置との位置誤差を算出する位置誤差算出部と、前記関節軸の角度に基づいて前記関節軸での作業座標系と関節座標系との間の部分ヤコビ行列を算出し、前記部分ヤコビ行列を統合して前記先端位置での作業座標系と関節座標系との間のヤコビ行列を算出するヤコビ行列算出部と、関節角差分を算出する関節角差分算出部と、前記関節角差分に基づいて関節のトルク指令値を算出するトルク指令値算出部と、前記トルク指令値に基づいて前記アクチュエータを駆動する駆動部と、前記関節軸の角度から前記アクチュエータを駆動するための駆動トルクを推定する駆動トルク推定部と、推定された前記駆動トルクと前記トルク指令値との差を外部トルクとして算出するとともに、前記外部トルクから外力が作用している外力作用関節軸を推定する外部トルク算出部と、前記外部トルクと前記部分ヤコビ行列から、前記外力作用関節軸に作用する外力を算出する外力算出部と、前記外力作用関節軸におけるコンプライアンスモデルを記憶するコンプライアンスモデル記憶部と、前記コンプライアンスモデルを用いて、前記外力に応じて前記外力作用関節軸での作業座標系位置に対するコンプライアンス修正量を算出するコンプライアンス修正量算出部と、前記コンプライアンス修正量と前記部分ヤコビ行列から前記関節角差分修正量を算出する関節角差分修正量算出部と、を備え、前記関節角差分算出部は、前記位置誤差と前記ヤコビ行列から逆運動学計算により修正前の関節角差分を算出し、前記修正前の関節角差分と前記関節角差分修正量との和を関節角差分として出力することを特徴とする。
　本発明によれば、ロボットの先端部のみならず、各リンク部への直接教示が可能なロボット制御装置を提供することができる。
本発明の一実施形態によるロボット制御装置を示すブロック図。 制御対象であるロボットの一例を示す概略図。 図２に示すロボットの可動部を示す概略図。 一実施形態に係る駆動トルク推定部を示すブロック図。 一実施形態によるロボット制御装置の動作を示すフローチャート。 一実施形態に係る駆動トルク推定処理の一例を示すフローチャート。 一実施形態の変形例によるロボット制御装置を示すブロック図。 一実施形態の変形例に係るコンプライアンスパラメータ修正量算出処理を示すフローチャート。 制御対象の他の例を示す概略図。
　本発明の実施形態を以下に図面を参照して説明する。以下の図面の記載において、同一又は類似の部分には同一又は類似の符号を付している。ただし、図面は模式的なものであり、具体的な寸法は以下の説明を参酌して判断すべきものである。又、図面相互間においても互いの寸法の関係や比率が異なる部分が含まれていることはもちろんである。
　また、以下に示す実施形態は、この発明の技術的思想を具体化するための装置や方法を例示するものであって、この発明の技術的思想は、構成部品の材質、形状、構造、配置等を下記のものに特定するものでない。この発明の技術的思想は、特許請求の範囲において、種々の変更を加えることができる。
　本発明の一実施形態によるロボット制御装置は、図１に示すように、中央演算処理装置（ＣＰＵ）１と、アクチュエータ１００と、駆動部（アンプ）１０１と、駆動軸角度検出部１０２と、位置データ記憶部２００と、リンクパラメータ記憶部２０１と、摩擦係数記憶部２０２と、ヤコビ行列記憶部２０３と、力／モーメントデータ記憶部２０４と、コンプライアンスモデル記憶部２０５と、を備えている。
　この実施形態において制御対象の一例とするロボットは、図２に模式的に示すように、本体３００と、本体３００に設けられた可動部３１０と、を備えている。可動部３１０は、複数のリンク３０１、３０２と、駆動プーリ３０３及び従動プーリ３０４から構成される複数の関節３０６、３０７と、駆動プーリ３０３及び従動プーリ３０４に巻きつけた伝達機構（例えば、ベルト）３０８と、を有する。
　図３に模式的に示すように、駆動プーリ３０３、減速機３０９、アクチュエータ１００及び駆動軸角度検出部１０２が、駆動軸（関節）３０６に取り付けられている。アクチュエータ１００が制御周期毎に回転駆動し、減速機３０９が回転数を落としトルクを上げる。一方、従動プーリ３０４及び関節軸角度検出部３０５が関節軸（関節）３０７に取り付けられている。駆動軸３０６の回転により、駆動プーリ３０３、伝達機構３０８及び従動プーリ３０４を介して関節軸３０７が回転駆動する。本実施形態のロボット制御装置においては、簡単のために、駆動軸３０６及び関節軸３０７に対して制御する場合について説明する。
　駆動軸角度検出部１０２及び関節軸角度検出部３０５のそれぞれは、エンコーダ等の位置センサが使用可能であり、所定の周波数成分を除去するフィルタを備えていても良い。駆動軸角度検出部１０２は、駆動軸３０６の位置の変位量（駆動軸角度）を検出する。関節軸角度検出部３０５は、関節軸３０７の位置の変位量（関節軸角度）を検出する。また関節軸角度は、関節軸角度検出部３０５によって検出する代わりに、駆動軸角度と、減速機３０９の減速比および伝達機構３０８の伝達比とを用いて算出しても良い。
　図１に示したＣＰＵ１は、関節軸角度算出部１０３と、先端位置算出部１０４と、ヤコビ行列算出部１０５と、駆動トルク推定部１０６と、外部トルク算出部１０７と、外力算出部１０８と、算出外力制限部１０９と、コンプライアンス修正量算出部１１０と、関節角差分修正量算出部１１１と、位置指令値生成部１１２と、位置誤差算出部１１３と、関節角差分算出部１１４と、トルク指令値算出部１１５と、をハードウェア資源であるモジュール（論理回路）として論理的に備えている。
　関節軸角度算出部１０３は、駆動軸角度検出部１０２により算出された駆動軸角度に基づいて、減速機３０９の減速比と、伝達機構３０８のもつ駆動軸３０６と関節軸３０７の比とに応じた関節軸角度を算出する。なお、関節軸３０７に取り付けた関節軸角度検出部３０５により関節軸角度を直接算出してもよい。
　先端位置算出部１０４は、リンクパラメータ記憶部２０１からリンクパラメータを読み出し、関節軸角度算出部１０３により算出された関節軸角度と、読み出されたリンクパラメータとを用いて、順運動学計算によりロボットの作業座標系における先端位置を算出する。
　ヤコビ行列算出部１０５は、関節軸角度算出部１０３により算出された関節軸角度から、ヤコビ行列を算出し、ヤコビ行列記憶部２０３に格納する。ヤコビ行列とは、ロボットの作業座標系と関節座標系との間の微小変位関係を表現した行列である。ヤコビ行列をＪとして、ロボットの先端位置の誤差Δｘと、関節角差分Δθは式（１）の関係を満たしている。　

　　　 Δｘ＝ＪΔθ　　　　 …　　（１）
　駆動トルク推定部１０６は、駆動軸角度検出１０２により算出された駆動軸角度と、関節軸角度算出部１０３により算出された関節軸角度とを用いて、ロボットの関節軸３０７を駆動するのに必要な駆動トルクを推定する。この駆動トルク推定部１０６の一具体例を図４に示す。この具体例の駆動トルク推定部１０６は、図４に示すように、駆動軸速度算出部４００と、摩擦力トルク算出部４０１と、慣性力トルク算出部４０２と、重力トルク算出部４０３と、加算部４０４と、を備えている。
　駆動軸速度算出部４００は、関節軸角度算出部１０３により算出された関節軸角度に基づいて、例えば関節角度の時間差分を取ることにより駆動軸速度を算出する。摩擦力トルク算出部４０１は、摩擦係数記憶部２０２に格納された摩擦係数を読み出して、駆動軸速度算出部４００により算出された駆動軸速度と、読み出された摩擦係数とを用いてクーロン摩擦、粘性摩擦等に相当する摩擦力トルクを算出する。
　慣性力トルク算出部４０２は、速度算出ユニット４１０と、加速度算出ユニット４１１と、慣性モーメント算出ユニット４１２と、慣性力トルク算出ユニット４１３と、を備えている。速度算出ユニット４１０は、関節軸角度算出部１０３により算出された関節軸角度を用いて関節角速度を算出する。加速度算出ユニット４１１は、速度算出ユニット４１０により算出された関節角速度に基づいて関節角加速度を算出する。慣性モーメント算出ユニット４１２は、リンクパラメータ記憶部２０１からリンクパラメータを読み出し、リンクパラメータと、関節軸角度算出部１０３により算出された関節軸角度とを用いて各リンク３０１、３０２の慣性モーメントを算出する。慣性力トルク算出ユニット４１３は、加速度算出ユニット４１１により算出された関節角加速度と、慣性モーメント算出ユニット４１２により算出された慣性モーメントとを用いて慣性力トルクを算出する。
　重力トルク算出部４０３は、リンクパラメータ記憶部２０１からリンクパラメータを読み出し、読み出されたリンクパラメータと、関節軸角度算出部１０３により算出された関節軸角度とを用いて、各リンク３０１、３０２に作用する重力を算出し、この算出された重力を補償する重力トルクを算出する。
　加算部４０４は、摩擦力トルク算出部４０１により算出された摩擦力トルクと、慣性力トルク算出ユニット４１３により算出された慣性力トルクと、重力トルク算出部４０３により算出された重力トルクとを加算し、その和を推定した駆動トルクとして出力する。
　図１に示した外部トルク算出部１０７は、駆動トルク推定部１０６により推定された駆動トルクと、トルク指令値算出部１１６により算出されたトルク指令値との差を外部トルクとして算出する。
　外力算出部１０８は、外部トルク算出部１０７により算出された外部トルクと、ヤコビ行列算出部１０５により算出されたヤコビ行列とを用いて外力を算出する。仮想仕事の原理より、次の式（２）のように外部トルクτｄにヤコビ行列Ｊの転置行列ＪＴの逆行列を乗じることにより外力ｆｄが算出される。　

　　　 ｆｄ＝（ＪＴ）－１τｄ　　　　　　…（２）

ここで、ロボットの先端部ではなくリンク部に外力が作用した場合、先端部に近い駆動軸の外部トルクは０である。そこで外部トルクベクトルτｄの各要素において０とならない部分ベクトルをτｄｉとし、この部分ベクトルτｄｉを次の式（３）のように表す。　

　　　 τｄｉ＝（τｄｉ１、　・・・、　τｄｉｉ） 　　　…（３）

すなわち、ベースから第ｉ番目の関節軸に働く外力は０ではないが、第ｉ＋１番目から先端部までの関節軸に働く外力が０とする。この場合、第ｉ番目の関節軸までの部分ヤコビ行列Ｊｉの転置行列Ｊｉ Ｔの逆行列を乗じることで第ｉ番目の関節軸において作用する外力が求められる。ベースから第ｉ番目の関節軸までの部分ヤコビ行列Ｊｉはヤコビ行列の一部であり、次の式（４）を用いて求められる。
Figure JPOXMLDOC01-appb-M000001
 ここで、ｚｉは第ｉ番目の関節軸の関節座標系の関節軸回転方向ベクトル、ｐｉはベース座標系から見た第ｉ番目の関節軸の関節座標系原点の位置ベクトルである。式（４）において、記号「×」はベクトルの外積を示す。第ｉ番目の関節軸を外力作用関節と呼ぶと、外力作用関節において作用する外力ｆｄｉは、次の式（５）を用いて外力算出部１０８によって算出することができる。　

　　　 ｆｄｉ＝（Ｊｉ Ｔ）－１τｄｉ 　　…（５）
　１０９は、力／モーメントデータ記憶部２０４に記憶された許容力および許容モーメントデータ（許容値）を読み出して、もし算出された外力が許容値を超えている場合は飽和処理を行う。すなわち、許容値を算出された外力として出力する。また、設定された最小値よりも小さい場合は算出された外力を０として算出する（不感帯処理）。算出された外力が最小値以上で許容値以下の場合には、算出された外力をそのまま出力する。
　コンプライアンス修正量算出部１１１は、コンプライアンスモデル記憶部２０５からコンプライアンスモデルを読み出して、読み出されたコンプライアンスモデルを用いて、算出外力制限部１０９の出力に応じた位置修正量を算出する。ここで、コンプライアンスモデルとは、例えば式（６）に示すように接触対象との間において仮想的に慣性、粘性、剛性を想定したものである。　

　　Ｍｄ２Δｘ／ｄｔ２＋ＤｄΔｘ／ｄｔ＋ＫΔｘ＝ＫｆΔｆ　　 …（６）

ここで、Δｘは外力作用関節に設定された作業座標系での誤差、ｄΔｘ／ｄｔは作業座標系での速度、ｄ２Δｘ／ｄｔ２は作業座標系での加速度ベクトル、Ｍは慣性行列、Ｄは粘性係数行列、Ｋは剛性係数行列、Ｋｆは力フィードバックゲイン行列である。力と位置の作用させる軸を切り替えるコンプライアンス選択行列は力フィードバックゲイン行列Ｋｆに含んだ形になっている。誤差の速度ｄΔｘ／ｄｔ及び加速度ベクトルｄ２Δｘ／ｄｔ２は、それぞれ位置誤差ベクトルΔｘの時間に関する一回差分、二回差分で近似できることから、外力作用関節でのコンプライアンス修正量Δｘｃｏｍｐは、次の式（７）を用いて算出することができる。　

　　Δｘｃｏｍｐ＝１／Ｋ（ＫｆΔｆ－Ｍｄ２Δｘ／ｄｔ２－ＤｄΔｘ／ｄｔ）　 …（７）
　関節角差分修正量算出部１１１は、コンプライアンス修正量Δｘｃｏｍｐから部分ヤコビ行列Ｊｉの逆行列を用いて外力作用関節までの関節角差分修正量を、次の式（８）を用いて算出する。　

　　　 Δθｃｏｍｐ＝Ｊｉ －１Δｘｃｏｍｐ　　 　　　…（８）
　位置指令値生成部１１２は、位置データ記憶部２００に記憶された目標先端位置データを読み出して、この目標先端位置データから各制御周期における補間された先端位置指令値を算出する。
　位置誤差算出部１１３は、位置指令値生成部１１２により生成した先端位置指令値ｘＲと、先端位置算出部１０４により算出された現在の先端位置ｘとに基づいて、次の式（９）を利用して位置誤差Δｘを算出する。　

　　　 Δｘ＝ｘＲ－ｘ 　　　　　　　…（９）
　関節角差分算出部１１４は、位置誤差算出部１１３により算出された誤差Δｘと、ヤコビ行列Ｊの逆行列Ｊ－１とを用いて算出した、誤差Δｘによる関節角差分に、先の関節角差分修正量Δθｃｏｍｐを加えて次の式（１０）のように算出する。　

　　　 Δθ＝Ｊ－１Δｘ＋Δθｃｏｍｐ　　　　　　　…（１０）

　トルク指令値算出部１１５は、関節角差分算出部１１４により算出された関節角差分を積分することによりトルク指令値（制御目標値）を生成する。駆動部１０１は、トルク指令値算出部１１５により算出されたトルク指令値にしたがってアクチュエータ１００を制御周期毎に駆動する。
　位置データ記憶部２００、リンクパラメータ記憶部２０１、摩擦係数記憶部２０２、ヤコビ行列記憶部２０３、力／モーメントデータ記憶部２０４、及びコンプライアンスモデル記憶部２０５としては、例えば半導体メモリ、磁気ディスク、光ディスク、光磁気ディスクや磁気テープ等が採用可能である。
　位置データ記憶部２００は、位置指令値生成部１１１が先端位置指令値を生成するために使用する目標先端位置データ列を記憶している。リンクパラメータ記憶部２０１は、ロボットのリンク３０１、３０２に関するリンクパラメータを記憶している。摩擦係数記憶部２０２は、摩擦力トルク算出部４０１が摩擦力トルクを算出するために使用する、予め定速運転において速度－トルク関係から求めておいた摩擦係数データを記憶している。ヤコビ行列記憶部２０３は、ヤコビ行列算出部１０５により算出されるヤコビ行列を記憶している。力／モーメントデータ記憶部２０４は、力指令値生成部１０９が先端位置力指令値を生成するために使用する目標先端力データ列を記憶している。コンプライアンスモデル記憶部２０５は、予め設定されたコンプライアンスモデルを記憶している。
（ロボット制御方法）
　次に、本実施形態によるロボット制御装置の動作を、図５に示すフローチャートを参照しながら説明する。
　（ａ）まず、制御演算を開始し、位置指令値生成部１１２によって、位置データ記憶部２００から目標先端位置データ列が読み出されて、目標先端位置データ列に基づいて各制御周期における先端位置指令値が生成される（ステップ、Ｓ１００、Ｓ１０１）。次に、ステップＳ１０２において、位置指令値生成部１１２により生成された先端位置指令値と、先端位置算出部１０４により算出された先端位置との誤差が位置誤差算出部１１３によって算出される。続いて、ステップＳ１０３において、位置誤差算出部１１３により算出された誤差に対して、関節角差分算出部１１４によって、ヤコビ行列記憶部２０３から読み出したヤコビ行列を用いて式（１０）に示すように、逆運動学計算を行って関節角差分が、後述する関節角差分修正量を考慮して算出される。
　その後、ステップＳ１０４において、関節角差分算出部１１４により算出された関節角差分を積分することによりトルク指令値がトルク指令値算出部１１５によって算出される。次に、ステップＳ１０５において、トルク指令値算出部１１５により算出されたトルク指令値を制御目標値として駆動部１０１によってアクチュエータ１００が駆動されることにより、駆動軸３０６が駆動され、先端位置が制御される。続いて、ステップＳ１０６において、制御演算の終了を確認し、ステップＳ１０７でサーボ処理を終了する。ステップＳ１０６において制御演算が終了でなければ後述するステップＳ１０８に進む。
　（ｂ）次に、ステップＳ１０８において、駆動軸角度検出部１０２によって駆動軸角度を検出する。次に、ステップＳ１０９において、駆動軸角度検出部１０２により算出された駆動軸角度から、減速機の減速比などに基づいて関節軸角度算出部１０３によって関節軸角度が算出される。ステップＳ１１０において、先端位置算出部１０４によって、リンクパラメータ記憶部２０１からリンクパラメータが読み出され、この読み出されたリンクパラメータと、関節軸角度算出部１０３により算出された関節軸角度とを用いて、順運動学計算により、先端位置が算出される。ステップＳ１１１において、関節軸角度算出部１０３により算出された関節軸角度を用いて、ヤコビ行列算出部１０５によってヤコビ行列が算出される。
　（ｃ）次に、ステップＳ１１２において、駆動軸角度検出部１０２により算出された駆動軸角度と、関節軸角度算出部１０３により算出された関節軸角度とを用いて駆動トルク推定部１０６によって駆動トルクが推定される。ステップＳ１１３において、駆動トルク推定部１０６により推定された駆動トルクと、トルク指令値算出部１１６により算出された実際のトルク指令値との差から外部トルク算出部１０７によって外部トルクが算出される。ステップＳ１１４において、外部トルク算出部１０７により算出された外部トルクと、ヤコビ行列算出部１０５により算出された部分ヤコビ行列とを用いて、外力算出部１０８によって式（５）に示す外力が算出される。
　（ｄ）次に、ステップＳ１１５において、力／モーメントデータ記憶部２０４から許容力／モーメントデータ列が読み出されて、この読み出された許容力／モーメントデータ列と、外力算出部１０８によって算出された外力とが１０９によって比較され、この比較結果に基づいて、制限された外力が出力される。すなわち、算出外力制限部１０９によって飽和処理または不感帯処理が行われる。
　（ｅ）次に、ステップＳ１１６において、コンプライアンス修正量算出部１１０によってコンプライアンスモデル記憶部２０５からコンプライアンスモデルが読み出され、コンプライアンスモデルを用いて式（７）に示すように算出外力制限部１０９により制限されら外力に応じた外力作用関節での修正量がコンプライアンス修正量算出部１１０によって算出される。ステップＳ１１７において、コンプライアンス修正量から部分ヤコビ行列を用いて式（８）に示すように関節角差分修正量が関節角差分修正量算出部１１１によって算出される。ステップＳ１０３に戻り、式（９）に示すように、位置指令値生成部１１２により生成された先端位置指令値と、先端位置算出部１０４により算出された先端位置との誤差にヤコビ行列の逆行列を乗じたものに、関節角差分修正量算出部１１１から算出された修正量を考慮した関節角差分が位置誤差算出部１１３によって算出される。
（駆動トルク推定処理）
　次に、図５に示したステップＳ１１２の駆動トルク推定処理を図６に示すフローチャートを参照しながら説明する。
　（ａ）ステップＳ２００において、駆動トルク推定処理が開始され、ステップＳ２０１において、駆動軸角度検出部１０２により検出された駆動軸角度から時間差分を取ることにより駆動軸速度算出部４００によって駆動軸速度が算出される。更に、摩擦係数記憶部２０２から摩擦係数が読み出され、この読み出された摩擦係数と、駆動軸速度算出部４００により算出された駆動軸速度とを用いて摩擦力トルク算出部４０１によって摩擦力トルクが算出される。
　（ｂ）ステップＳ２０２において、関節軸角度算出部１０３により算出された関節軸角度の時間差分を取ることにより速度算出ユニット４１０によって関節角速度が算出される。ステップＳ２０３において、速度算出ユニット４１０により関節角速度の時間差分を取ることにより加速度算出ユニット４１１によって関節角加速度が算出される。ステップＳ２０４において、リンクパラメータ記憶部２０１からリンクパラメータが読み出され、関節軸角度算出部１０３により算出された関節軸角度と、読み出されたリンクパラメータと、を用いて慣性モーメント算出ユニット４１２によって、各関節におけるリンクの慣性モーメントが算出される。ステップＳ２０５において、加速度算出ユニット４１１により算出された関節角加速度と、慣性モーメント算出ユニット４１２により算出された慣性モーメントとに基づいて、慣性力トルク算出ユニット４１３によって慣性力トルクが算出される。
　（ｃ）ステップＳ２０６において、リンクパラメータ記憶部２０１からリンクパラメータが読み出され、関節軸角度算出部１０３により算出された関節軸角度関節軸角度と、読み出されたリンクパラメータとに基づいて、重力トルク算出部４０３によって各リンク３０１、３０２に作用する重力が算出され、これを補償する重力トルクが算出される。
　（ｄ）ステップＳ２０７において、摩擦力トルク算出部４０１により算出された摩擦力トルクと、慣性力トルク算出ユニット４１３により算出された慣性力トルクと、重力トルク算出部４０３により算出された重力トルクとが加算部４０４によって加算されて、推定した駆動トルクとして算出される。ステップＳ２０８において駆動トルク推定処理が終了する。
　以上説明したように、本実施形態によれば、それぞれのリンクへ作用する外力を推定することが可能となり、推定した外力に基づいて設定したコンプライアンスモデルにより柔軟な動作を行うことで、ロボットへの簡便な直接教示を実現することができる。
（変形例）
　本実施形態の変形例によるロボット制御装置を図７に示す。この変形例のロボット制御装置は、コンプライアンスモデルのパラメータ変更を行うことが可能な制御装置である。この変形例のロボット制御装置は、図１に示す本実施形態のロボット制御装置において、コンプライアンスパラメータ修正部１１６を新たに設けた構成を有している。
　直接教示のために操作者によって加えられる外力に対するコンプライアンスモデルと、衝突時のコンプライアンスモデルとは異なっており、通常、コンプライアンスモデルのパラメータを変更することで対応している。しかし付加的なセンサを用いない場合、意図された外力と衝突による外力とを明確に区別することは困難であり、またそれぞれのパラメータセットを連続的に変化させなければ、パラメータの切り替え時に安定して動作することができない。
　そこで図７に示す変形例のように、コンプライアンスパラメータ修正部１１６を設けることにより衝突時においても安定して動作することを可能にする。このコンプライアンスパラメータ修正部１１６は、コンプライアンス修正量算出部１１０より算出されたコンプライアンス修正量の履歴から、コンプライアンスモデルのパラメータを変更し、これをコンプライアンスモデル記憶部２０５に書き込んでいく。動作開始時においては衝突時用のコンプライアンスモデルのパラメータであり、修正量は直接教示により発生しているため、発生した外力成分のみパラメータが変更され、より柔軟性を増して操作性を向上させることができる。
　また、コンプライアンスモデルのパラメータの修正は、定期的な周期で行ってもよい。このパラメータを修正する周期は、制御周期の数倍程度を取ることもある。これによりパラメータの修正によりロボットの動作が振動的にならない効果がある。
　次に、この変形例のコンプライアンスパラメータ修正部の動作を図８に示すフローチャートを参照しながら説明する。
　ステップＳ３０１において、コンプライアンスパラメータ修正処理を開始する。ステップＳ３０２において、本制御周期（パラメータを修正する周期）においてコンプライアンスモデルのパラメータを修正するかどうかを確認する。修正しない場合はステップＳ３０４へ進み、終了する。修正する場合はステップＳ３０３へ進み、コンプライアンスパラメータ修正部１１６によってコンプライアンスモデルのパラメータが修正され、コンプライアンスモデル記憶部２０５に修正結果が書き込まれ、ステップＳ３０４にて終了する。
　上記のように、本発明の実施形態を説明したが、この開示の一部をなす論述及び図面はこの発明を限定するものであると理解すべきではない。この開示から当業者には様々な代替実施の形態、実施例及び運用技術が明らかとなろう。
　例えば、本発明の実施形態において、慣性力トルクの算出の際に、速度算出ユニット４１０及び加速度算出ユニット４１１による関節軸角度の時間に関する二回差分で算出された関節角加速度を用いているが、制御周期によっては時間遅れが大きくなりトルク誤差が大きくなりやすくなる。その場合は、図９に示すように、リンク３０１に搭載した加速度センサ３１１からリンク３０１の加速度を検出し、検出した加速度を加速度算出ユニット４１１が関節角加速度へ変換して、この関節角加速度を基に慣性力トルクを算出してもよい。
　また、駆動トルク推定処理としては、摩擦力トルクと、慣性力トルクと、重力トルクと用いて駆動トルクを推定する一例を示したが、これに限定されるものではない。例えば、遠心力やコリオリの力等のパラメータを更に考慮しても良い。
　また、ＣＰＵ１、位置データ記憶部２００、リンクパラメータ記憶部２０１、摩擦係数記憶部２０２、ヤコビ行列記憶部２０３、力／モーメントデータ記憶部２０４及びコンプライアンスモデル記憶部２０５等が制御対象であるロボットの内部に埋め込まれて一体化されていても良い。また、ＣＰＵ１、位置データ記憶部２００、リンクパラメータ記憶部２０１、摩擦係数記憶部２０２、ヤコビ行列記憶部２０３、力／モーメントデータ記憶部２０４及びコンプライアンスモデル記憶部２０５等が制御対象であるロボットの外部にあり、ロボットを有線或いは無線で遠隔制御することも可能である。
　このように、本発明はここでは記載していない様々な実施形態等を含むことは勿論である。したがって、本発明の技術的範囲は上記の説明から妥当な特許請求の範囲に係る発明特定事項によってのみ定められるものである。
　１　中央演算処理装置（ＣＰＵ）
　１００　アクチュエータ
　１０１　駆動部
　１０２　駆動軸角度検出部
　１０３　関節軸角度算出部
　１０４　先端位置算出部
　１０５　ヤコビ行列算出部
　１０６　駆動トルク推定部
　１０７　外部トルク算出部
　１０８　外力算出部
　１０９　算出外力制限部
　１１０　コンプライアンス修正量算出部
　１１１　関節角差分修正量算出部
　１１２　位置指令値生成部
　１１３　位置誤差算出部
　１１４　関節角差分算出部
　１１５　トルク指令値算出部
　１１５　コンプライアンスパラメータ修正量算出部
　２００　位置データ記憶部
　２０１　リンクパラメータ記憶部
　２０２　摩擦係数記憶部
　２０３　ヤコビ行列記憶部
　２０４　力／モーメントデータ記憶部
　２０５　コンプライアンスモデル記憶部
　３００　本体
　３０１、３０２　リンク
　３０３　駆動プーリ
　３０４　従動プーリ
　３０５　関節軸角度検出部
　３０６　駆動軸（関節）
　３０７　関節軸（関節）
　３０８　伝達機構
　３０９　減速機
　３１０　可動部
　３１１　加速度センサ
　４００　駆動軸速度算出部
　４０１　摩擦力トルク算出部
　４０２　慣性力トルク算出部
　４０３　重力トルク算出部
　４０４　加算部
　４１０　速度算出ユニット
　４１１　加速度算出ユニット
　４１２　慣性モーメント算出ユニット
　４１３　慣性力トルク算出ユニット

Claims (4)

  1. 　関節軸及び前記関節軸に駆動力を伝達する駆動軸を有するロボットを制御するロボット制御装置であって、
    　前記関節軸を制御周期毎に駆動するアクチュエータと、
    　前記駆動軸の角度を検出する駆動軸角度検出部と、
    　前記駆動軸の角度から前記関節軸の角度を算出する関節軸角度算出部と、
    　前記関節軸の角度から前記ロボットの先端位置を算出する先端位置算出部と、
    　前記先端位置の位置指令値と、前記先端位置との位置誤差を算出する位置誤差算出部と、
    　前記関節軸の角度に基づいて前記関節軸での作業座標系と関節座標系との間の部分ヤコビ行列を算出し、前記部分ヤコビ行列を統合して前記先端位置での作業座標系と関節座標系との間のヤコビ行列を算出するヤコビ行列算出部と、
    　関節角差分を算出する関節角差分算出部と、
    　前記関節角差分に基づいて関節のトルク指令値を算出するトルク指令値算出部と、
    　前記トルク指令値に基づいて前記アクチュエータを駆動する駆動部と、
    　前記関節軸の角度から前記アクチュエータを駆動するための駆動トルクを推定する駆動トルク推定部と、
    　推定された前記駆動トルクと前記トルク指令値との差を外部トルクとして算出するとともに、前記外部トルクから外力が作用している外力作用関節軸を推定する外部トルク算出部と、
    　前記外部トルクと前記部分ヤコビ行列から、前記外力作用関節軸に作用する外力を算出する外力算出部と、
    　前記外力作用関節軸におけるコンプライアンスモデルを記憶するコンプライアンスモデル記憶部と、
    　前記コンプライアンスモデルを用いて、前記外力に応じて前記外力作用関節軸での作業座標系位置に対するコンプライアンス修正量を算出するコンプライアンス修正量算出部と、
    　前記コンプライアンス修正量と前記部分ヤコビ行列から前記関節角差分修正量を算出する関節角差分修正量算出部と、
    　を備え、
    　前記関節角差分算出部は、前記位置誤差と前記ヤコビ行列から逆運動学計算により修正前の関節角差分を算出し、前記修正前の関節角差分と前記関節角差分修正量との和を関節角差分として出力することを特徴とするロボット制御装置。
  2. 　前記コンプライアンス修正量を記憶する修正量記憶部と、
    　前記修正量記憶部に格納された複数の修正量からコンプライアンスパラメータを修正するコンプライアンスパラメータ修正部と、
    を更に備えることを特徴とする請求項１記載のロボット制御装置。
  3. 　前記コンプライアンスパラメータ修正部が前記制御周期よりも大きな周期で実行されることを特徴とする請求項２記載のロボット制御装置。
  4. 　前記外力算出部によって算出された前記外力と許容値とを比較し、前記外力が前記許容値を超えている場合には前記外力を許容値とし、最小値以下の場合には前記外力を０とする算出外力制限部を更に備えていることを特徴とする請求項１記載のロボット制御装置。
PCT/JP2010/060543 2010-06-22 2010-06-22 ロボット制御装置 Ceased WO2011161765A1 (ja)

Priority Applications (4)

Application Number Priority Date Filing Date Title
EP10853625.1A EP2586577A4 (en) 2010-06-22 2010-06-22 ROBOT CONTROL DEVICE
PCT/JP2010/060543 WO2011161765A1 (ja) 2010-06-22 2010-06-22 ロボット制御装置
JP2012521201A JP5383911B2 (ja) 2010-06-22 2010-06-22 ロボット制御装置
US13/677,761 US8489238B2 (en) 2010-06-22 2012-11-15 Robot control apparatus

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
PCT/JP2010/060543 WO2011161765A1 (ja) 2010-06-22 2010-06-22 ロボット制御装置

Related Child Applications (1)

Application Number Title Priority Date Filing Date
US13/677,761 Continuation US8489238B2 (en) 2010-06-22 2012-11-15 Robot control apparatus

Publications (1)

Publication Number Publication Date
WO2011161765A1 true WO2011161765A1 (ja) 2011-12-29

Family

ID=45370978

Family Applications (1)

Application Number Title Priority Date Filing Date
PCT/JP2010/060543 Ceased WO2011161765A1 (ja) 2010-06-22 2010-06-22 ロボット制御装置

Country Status (4)

Country Link
US (1) US8489238B2 (ja)
EP (1) EP2586577A4 (ja)
JP (1) JP5383911B2 (ja)
WO (1) WO2011161765A1 (ja)

Cited By (6)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
JP2014054714A (ja) * 2012-09-13 2014-03-27 Honda Motor Co Ltd 運動環境生成システム
CN105034010A (zh) * 2014-04-30 2015-11-11 发那科株式会社 进行机器人的灵活控制的控制装置
JP2016168651A (ja) * 2015-03-13 2016-09-23 キヤノン株式会社 ロボット制御方法、ロボット装置、プログラム及び記録媒体
JP2016168650A (ja) * 2015-03-13 2016-09-23 キヤノン株式会社 ロボット装置、ロボット制御方法、プログラム及び記録媒体
JP2018097722A (ja) * 2016-12-15 2018-06-21 三菱重工業株式会社 制御装置、制御方法、プログラム
JP2019181664A (ja) * 2018-04-18 2019-10-24 ファナック株式会社 ロボットの制御装置およびロボットの制御方法

Families Citing this family (25)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US9119655B2 (en) 2012-08-03 2015-09-01 Stryker Corporation Surgical manipulator capable of controlling a surgical instrument in multiple modes
KR102603224B1 (ko) 2012-08-03 2023-11-16 스트리커 코포레이션 로봇 수술을 위한 시스템 및 방법
US9226796B2 (en) 2012-08-03 2016-01-05 Stryker Corporation Method for detecting a disturbance as an energy applicator of a surgical instrument traverses a cutting path
FR3002048B1 (fr) * 2013-02-14 2016-07-01 Commissariat Energie Atomique Procede de detection amelioree de collision d'un robot avec son environnement, systeme et produit programme d'ordinateur mettant en œuvre le procede
JP6399437B2 (ja) 2014-06-04 2018-10-03 パナソニックＩｐマネジメント株式会社 制御装置及びそれを用いた作業管理システム
US9987749B2 (en) * 2014-08-15 2018-06-05 University Of Central Florida Research Foundation, Inc. Control interface for robotic humanoid avatar system and related methods
JP5926346B2 (ja) * 2014-09-25 2016-05-25 ファナック株式会社 人間協調ロボットシステム
PL3017920T3 (pl) * 2014-11-07 2018-02-28 Comau S.P.A. Robot przemysłowy i sposób sterowania robotem przemysłowym
DE102014224122B4 (de) * 2014-11-26 2018-10-25 Siemens Healthcare Gmbh Verfahren zum Betreiben eines robotischen Gerätes und robotisches Gerät
DE102015108010B3 (de) * 2015-05-20 2016-06-02 Cavos Bagatelle Verwaltungs Gmbh & Co. Kg Steuern und Regeln von Aktoren eines Roboters unter Berücksichtigung von Umgebungskontakten
AU2016351584B2 (en) 2015-11-11 2021-09-23 Mako Surgical Corp. Robotic system and method for backdriving the same
JPWO2017104199A1 (ja) * 2015-12-18 2018-04-12 シャープ株式会社 ロボット、ロボットの制御方法、およびプログラム
US20170259433A1 (en) * 2016-03-11 2017-09-14 Seiko Epson Corporation Robot control device, information processing device, and robot system
DE102016209833B4 (de) * 2016-06-03 2019-10-31 Volkswagen Aktiengesellschaft Verfahren und Vorrichtung zum Bestimmen eines Fahrerhandmoments an einem Lenkrad eines Fahrzeugs
CN106003055B (zh) * 2016-08-10 2019-02-05 广东工业大学 一种基于折弯机机械手的运动关节角求解方法
US11202682B2 (en) 2016-12-16 2021-12-21 Mako Surgical Corp. Techniques for modifying tool operation in a surgical robotic system based on comparing actual and commanded states of the tool relative to a surgical site
JP2018202504A (ja) * 2017-05-31 2018-12-27 ソニー株式会社 医療用支持アームシステム、医療用支持アームの制御方法、および医療用支持アームの制御装置
EP3435183A1 (en) * 2017-07-26 2019-01-30 ABB Schweiz AG Method for controlling a robot arm and robot system with a robot arm
CN107901037B (zh) * 2017-10-30 2020-09-15 北京精密机电控制设备研究所 一种机器人关节动力学模型修正方法
IT201800005091A1 (it) 2018-05-04 2019-11-04 "Procedimento per monitorare lo stato di funzionamento di una stazione di lavorazione, relativo sistema di monitoraggio e prodotto informatico"
CN109648549A (zh) * 2018-12-30 2019-04-19 江苏集萃智能制造技术研究所有限公司 机器人轨迹规划方法和机器人
WO2020211914A1 (en) 2019-04-17 2020-10-22 Universal Robots A/S Method of controlling a robot arm based on adaptive friction
CN110032816B (zh) * 2019-04-19 2023-06-06 中科新松有限公司 机械臂扭矩估算方法及估算系统
CN110633514B (zh) * 2019-08-28 2022-09-27 南京理工大学 一种基于螺纹过盈量的套管上扣控制方法
CN112571412B (zh) * 2019-09-30 2024-03-26 中电九天智能科技有限公司 一种智能制造设备的控制系统

Citations (3)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
JPS61224012A (ja) * 1985-03-29 1986-10-04 Agency Of Ind Science & Technol マニピユレ−タの駆動方式
JPH09248322A (ja) * 1996-03-15 1997-09-22 Yaskawa Electric Corp 肢体駆動装置
JP2002283276A (ja) * 2001-03-21 2002-10-03 Daihen Corp 多関節ロボットにおける衝突検出・停止制御法

Family Cites Families (13)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US4975856A (en) * 1986-02-18 1990-12-04 Robotics Research Corporation Motion controller for redundant or nonredundant linkages
JP3307788B2 (ja) 1994-09-19 2002-07-24 株式会社安川電機 アーム駆動装置
US6522952B1 (en) * 1999-06-01 2003-02-18 Japan As Represented By Secretary Of Agency Of Industrial Science And Technology Method and system for controlling cooperative object-transporting robot
US8004229B2 (en) * 2005-05-19 2011-08-23 Intuitive Surgical Operations, Inc. Software center and highly configurable robotic systems for surgery and other uses
JP3878054B2 (ja) 2001-05-08 2007-02-07 三菱電機株式会社 ロボット制御装置
US7390309B2 (en) * 2002-09-23 2008-06-24 Honda Motor Co., Ltd. Human assist system using gravity compensation control system and method using multiple feasibility parameters
JP4587738B2 (ja) * 2003-08-25 2010-11-24 ソニー株式会社 ロボット装置及びロボットの姿勢制御方法
JP2005100143A (ja) * 2003-09-25 2005-04-14 Kobe Steel Ltd モータ駆動装置の制御方法及び制御装置
US8924021B2 (en) * 2006-04-27 2014-12-30 Honda Motor Co., Ltd. Control of robots from human motion descriptors
JP4243309B2 (ja) * 2006-07-04 2009-03-25 パナソニック株式会社 ロボットアームの制御装置
CN101909829B (zh) * 2008-02-28 2012-08-29 松下电器产业株式会社 机器人手臂的控制装置及控制方法、机器人、机器人手臂的控制程序、及机器人手臂控制用集成电子电路
JP5242342B2 (ja) * 2008-10-31 2013-07-24 株式会社東芝 ロボット制御装置
JP4947073B2 (ja) * 2009-03-11 2012-06-06 トヨタ自動車株式会社 ロボット装置及びその制御方法

Patent Citations (3)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
JPS61224012A (ja) * 1985-03-29 1986-10-04 Agency Of Ind Science & Technol マニピユレ−タの駆動方式
JPH09248322A (ja) * 1996-03-15 1997-09-22 Yaskawa Electric Corp 肢体駆動装置
JP2002283276A (ja) * 2001-03-21 2002-10-03 Daihen Corp 多関節ロボットにおける衝突検出・停止制御法

Cited By (10)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
JP2014054714A (ja) * 2012-09-13 2014-03-27 Honda Motor Co Ltd 運動環境生成システム
CN105034010A (zh) * 2014-04-30 2015-11-11 发那科株式会社 进行机器人的灵活控制的控制装置
JP2015208836A (ja) * 2014-04-30 2015-11-24 ファナック株式会社 ロボットの柔軟制御を行う制御装置
CN105034010B (zh) * 2014-04-30 2016-10-12 发那科株式会社 进行机器人的灵活控制的控制装置
US9676099B2 (en) 2014-04-30 2017-06-13 Fanuc Corporation Control device for performing flexible control of robot
JP2016168651A (ja) * 2015-03-13 2016-09-23 キヤノン株式会社 ロボット制御方法、ロボット装置、プログラム及び記録媒体
JP2016168650A (ja) * 2015-03-13 2016-09-23 キヤノン株式会社 ロボット装置、ロボット制御方法、プログラム及び記録媒体
JP2018097722A (ja) * 2016-12-15 2018-06-21 三菱重工業株式会社 制御装置、制御方法、プログラム
JP2019181664A (ja) * 2018-04-18 2019-10-24 ファナック株式会社 ロボットの制御装置およびロボットの制御方法
JP7111498B2 (ja) 2018-04-18 2022-08-02 ファナック株式会社 ロボットの制御装置およびロボットの制御方法

Also Published As

Publication number Publication date
JPWO2011161765A1 (ja) 2013-08-19
US8489238B2 (en) 2013-07-16
US20130073084A1 (en) 2013-03-21
JP5383911B2 (ja) 2014-01-08
EP2586577A4 (en) 2013-12-04
EP2586577A1 (en) 2013-05-01

Similar Documents

Publication Publication Date Title
JP5383911B2 (ja) ロボット制御装置
JP5242342B2 (ja) ロボット制御装置
JP4319232B2 (ja) パワーアシスト装置およびその制御方法
JP5238885B2 (ja) ロボット制御装置
JP4271249B2 (ja) 嵌合装置
US10300600B2 (en) Control system having learning control function and control method
JP6484265B2 (ja) 学習制御機能を備えたロボットシステム及び学習制御方法
JP6648913B2 (ja) 制御方法、ロボット装置、プログラム、記録媒体、物品の製造方法、及び駆動装置
JP6044511B2 (ja) ロボットの制御方法及びロボットシステム
JP5893666B2 (ja) 力に応じて動かすロボットのロボット制御装置およびロボットシステム
JP5916583B2 (ja) 多関節ロボットのウィービング制御装置
JP5231935B2 (ja) ロボット制御装置
CN104589304A (zh) 机器人控制装置以及机器人
JP4997145B2 (ja) パワーアシスト装置およびその制御方法
JP4822063B2 (ja) ロボットの直接教示装置
JP2017056549A (ja) ロボット装置、ロボット制御方法、プログラム、記録媒体及び組立部品の製造方法
JP6088601B2 (ja) 走行軸付きロボットにおけるツール先端の振れを抑制するロボット制御装置
JP5614232B2 (ja) ロボットハンドの制御装置、制御方法、及び制御用プログラム
WO2016199228A1 (ja) 医療用マニピュレータ制御装置
JP6238110B2 (ja) ロボットハンドの制御方法と制御装置
JP2007136564A (ja) マニピュレータ制御方法および装置
WO2020170306A1 (ja) ロボット制御装置
WO2020240846A1 (ja) 制御装置、制御方法及びロボットシステム
WO2014126216A1 (ja) 多関節ロボットの制御方法

Legal Events

Date Code Title Description
121 Ep: the epo has been informed by wipo that ep was designated in this application

Ref document number: 10853625

Country of ref document: EP

Kind code of ref document: A1

WWE Wipo information: entry into national phase

Ref document number: 2012521201

Country of ref document: JP

REEP Request for entry into the european phase

Ref document number: 2010853625

Country of ref document: EP

WWE Wipo information: entry into national phase

Ref document number: 2010853625

Country of ref document: EP

NENP Non-entry into the national phase

Ref country code: DE