WO2012121125A1 - 多関節型ロボットの制御装置,制御方法及び制御プログラム - Google Patents
多関節型ロボットの制御装置,制御方法及び制御プログラム Download PDFInfo
- Publication number
- WO2012121125A1 WO2012121125A1 PCT/JP2012/055314 JP2012055314W WO2012121125A1 WO 2012121125 A1 WO2012121125 A1 WO 2012121125A1 JP 2012055314 W JP2012055314 W JP 2012055314W WO 2012121125 A1 WO2012121125 A1 WO 2012121125A1
- Authority
- WO
- WIPO (PCT)
- Prior art keywords
- angle
- axis
- calculated
- speed
- drive system
- Prior art date
Links
- 238000000034 method Methods 0.000 title claims description 153
- 230000001629 suppression Effects 0.000 claims abstract description 84
- 230000001133 acceleration Effects 0.000 claims description 25
- 238000006243 chemical reaction Methods 0.000 claims description 16
- 230000000694 effects Effects 0.000 claims description 6
- 230000005484 gravity Effects 0.000 claims description 4
- 210000000707 wrist Anatomy 0.000 abstract description 27
- 239000012636 effector Substances 0.000 description 97
- 238000012545 processing Methods 0.000 description 57
- 238000003466 welding Methods 0.000 description 42
- 238000005070 sampling Methods 0.000 description 7
- 238000010422 painting Methods 0.000 description 6
- 238000010586 diagram Methods 0.000 description 5
- 238000013459 approach Methods 0.000 description 3
- 238000011084 recovery Methods 0.000 description 3
- 230000002159 abnormal effect Effects 0.000 description 2
- 239000011248 coating agent Substances 0.000 description 2
- 238000000576 coating method Methods 0.000 description 2
- 230000036461 convulsion Effects 0.000 description 2
- 238000007689 inspection Methods 0.000 description 2
- 239000000463 material Substances 0.000 description 2
- 230000004044 response Effects 0.000 description 2
- 230000005540 biological transmission Effects 0.000 description 1
- 238000004891 communication Methods 0.000 description 1
- 230000007423 decrease Effects 0.000 description 1
- 238000013461 design Methods 0.000 description 1
- 238000011156 evaluation Methods 0.000 description 1
- 238000012986 modification Methods 0.000 description 1
- 230000004048 modification Effects 0.000 description 1
- 238000012544 monitoring process Methods 0.000 description 1
- 230000000007 visual effect Effects 0.000 description 1
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/10—Programme-controlled manipulators characterised by positioning means for manipulator elements
-
- 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/1656—Programme controls characterised by programming, planning systems for manipulators
- B25J9/1664—Programme controls characterised by programming, planning systems for manipulators characterised by motion, path, trajectory planning
-
- B—PERFORMING OPERATIONS; TRANSPORTING
- B23—MACHINE TOOLS; METAL-WORKING NOT OTHERWISE PROVIDED FOR
- B23K—SOLDERING OR UNSOLDERING; WELDING; CLADDING OR PLATING BY SOLDERING OR WELDING; CUTTING BY APPLYING HEAT LOCALLY, e.g. FLAME CUTTING; WORKING BY LASER BEAM
- B23K9/00—Arc welding or cutting
- B23K9/12—Automatic feeding or moving of electrodes or work for spot or seam welding or cutting
-
- 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/06—Programme-controlled manipulators characterised by multi-articulated arms
-
- 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/18—Numerical control [NC], i.e. automatically operating machines, in particular machine tools, e.g. in a manufacturing environment, so as to execute positioning, movement or co-ordinated operations by means of programme data in numerical form
- G05B19/41—Numerical control [NC], i.e. automatically operating machines, in particular machine tools, e.g. in a manufacturing environment, so as to execute positioning, movement or co-ordinated operations by means of programme data in numerical form characterised by interpolation, e.g. the computation of intermediate points between programmed end points to define the path to be followed and the rate of travel along that path
- G05B19/4103—Digital interpolation
-
- 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
-
- 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
- G05B2219/00—Program-control systems
- G05B2219/30—Nc systems
- G05B2219/34—Director, elements to supervisory
- G05B2219/34083—Interpolation general
-
- 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
- G05B2219/00—Program-control systems
- G05B2219/30—Nc systems
- G05B2219/40—Robotics, robotics mapping to robotics vision
- G05B2219/40333—Singularity, at least one movement not possible, kinematic redundancy
-
- 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
- G05B2219/00—Program-control systems
- G05B2219/30—Nc systems
- G05B2219/45—Nc applications
- G05B2219/45104—Lasrobot, welding robot
-
- Y—GENERAL TAGGING OF NEW TECHNOLOGICAL DEVELOPMENTS; GENERAL TAGGING OF CROSS-SECTIONAL TECHNOLOGIES SPANNING OVER SEVERAL SECTIONS OF THE IPC; TECHNICAL SUBJECTS COVERED BY FORMER USPC CROSS-REFERENCE ART COLLECTIONS [XRACs] AND DIGESTS
- Y10—TECHNICAL SUBJECTS COVERED BY FORMER USPC
- Y10S—TECHNICAL SUBJECTS COVERED BY FORMER USPC CROSS-REFERENCE ART COLLECTIONS [XRACs] AND DIGESTS
- Y10S901/00—Robots
- Y10S901/30—End effector
- Y10S901/41—Tool
Definitions
- the present invention relates to a control device, a control method, and a control program for an articulated robot, and in particular, when the speed of each drive axis of the wrist axis exceeds an allowable range, the speed of each drive axis is maintained while maintaining the posture of the wrist. It relates to a technique for maintaining within an allowable range.
- the joint axes for controlling the arm axis mounted on the base are J1, A2, and J3 in order from the base.
- the joint axes that control the wrist axis are J4 axis, J5 axis, and J6 axis in order from the arm portion toward the tip of the wrist.
- the end effector in the Cartesian coordinate system (weld attached to the tip of the hand) is obtained by finding a solution to the forward kinematics problem.
- Patent Documents 1 to 3 disclose a method of operating each wrist axis while maintaining a speed within a limit without causing a sudden change.
- Patent Document 1 and Patent Document 2 describe a method of maintaining the position (1) and the attitude angle (3) of the end effector, but the movement speed of (2) can be maintained. If the end effector is equipped with a welder, it will take longer than the originally required time to pass through the work line, and the welding time for other parts will be reduced. However, there is a problem that the strength is reduced. Furthermore, the location where the moving speed is slowed down and the welding time is extended may rise more than necessary, which may affect the next process (for example, the second layer welding process).
- Patent Document 3 describes a method of maintaining either the moving speed or the position of the end effector.
- the position cannot be maintained, and the end effector is
- the present invention has been made in view of the above circumstances, and its purpose is to be particularly necessary for welding and painting work when it is necessary to rapidly change the wrist shaft beyond a predetermined allowable amount.
- An articulated type that maintains the wrist position, wrist tip movement speed and wrist axis allowable speed, and suppresses fluctuations in at least one specific component of the posture angle in the work coordinate system according to the purpose.
- the present invention has a three-drive shaft that has a working portion that moves along the working line of the work object at the tip and is connected to the working portion to change the posture of the working portion. And a second joint drive system having at least three drive shafts connected to the first joint drive system and changing the position of the first joint drive system.
- the present invention is applied to a control device for an articulated robot that controls a robot, and is characterized by comprising the following (11) to (16). (11) In the plurality of interpolation points for interpolating the teaching path connecting the work start position of the working unit and the posture at that time to the work end position and the posture at that time, which are previously taught in the base coordinate system of the articulated robot.
- Interpolation data calculation means for calculating data indicating the position of the working unit and the posture at that time. (12) By obtaining a solution of the inverse kinematics problem from the data calculated by the interpolation data calculating means, the first joint drive system and the second joint system showing the position of the working unit at the interpolation point and the posture at that time Angle calculation means for calculating the angles of all the drive shafts including the joint drive system. (13) The operation is performed at the next interpolation point based on a difference between angles of the drive shafts of the first joint drive system between the next interpolation point calculated by the angle calculation means and the current interpolation point. Speed calculating means for calculating the speed of the two drive shafts at both ends in the first joint drive system when moving the posture of the part.
- the work unit in a work coordinate system including data calculated by the interpolation data calculation means, an axis in a moving direction of the work unit, an axis in a direction perpendicular to the axis, and an axis perpendicular to the two axes.
- Attitude data conversion means for converting into attitude data indicating the attitude of the person.
- the first joint drive system without any change and while suppressing fluctuation of one or two specific components with respect to the posture data of the working unit at the next interpolation point converted by the posture data conversion means
- the angle of each of the drive shafts of the first joint drive system in which the speeds of the two drive shafts at both ends are within the allowable range is calculated again, and the calculated angles of the drive shafts of the first joint drive system Recalculation means for recalculating the angles of the drive shafts of the second joint drive system based on the position of the working unit calculated by the interpolation data calculation means; (16)
- the speeds of the two drive shafts at both ends in the first joint drive system calculated by the speed calculation unit are within the allowable range
- each of the drive shafts calculated by the angle calculation unit When the articulated robot is driven on the basis of the angle, and the speed of one or both of the two drive shafts at both ends in the first joint drive system calculated by the speed calculation means is outside the allowable range
- Drive instruction means for driving the articulated robot based on the angles of the drive
- the position of the working part and the movement of the wrist tip particularly necessary for welding or painting work are required. It is possible to maintain the speed and the allowable speed of the drive shaft of the first joint drive system, and to suppress the fluctuation of at least one specific component of the posture angle in the work coordinate system according to the purpose. Further, when calculating a singularity region, a singularity avoidance path, etc. existing between the work start position and the work end position, an arithmetic unit (CPU) capable of dealing with an excessive load in the pre-calculation.
- CPU arithmetic unit
- the recalculation unit is configured to determine, based on the angle of each drive shaft of the first joint drive system at the current interpolation point, the next interpolation point where the angle of each drive shaft is calculated by the angle calculation unit.
- the angle for following the reference at the angle, and calculating a plurality of angle candidates for each of the drive shafts at which the speed at the interpolation point next to the two drive shafts at both ends in the first joint drive system is within the allowable range
- Attitude data calculation for calculating the attitude data indicating the attitude of the working part at the next interpolation point in the work coordinate system when each of the angle candidates calculated by the candidate calculation means and the angle candidate calculation means is adopted Among the plurality of posture data calculated by the posture data calculation unit, and the posture data of the working unit at the next interpolation point converted by the posture data conversion unit.
- selecting the posture data fluctuation is suppressed most of the specific component, it is conceivable comprising an angle selection means for selecting the angle corresponding to the attitude data as the angle of each said drive shaft
- the angle calculation means is, for example, an angle at the interpolation point next to the drive shaft located at the center of the first joint drive system. Calculates the solution of the inverse kinematics problem having the same sign as the angle of the drive shaft at the work start position. Thereby, it is possible to avoid a singular point by controlling the angle of the drive shaft located at the center of the first joint drive system so as not to pass through 0 °.
- the angle calculation unit may be configured to detect the first joint drive system when the speeds of the two drive shafts at both ends of the first joint drive system calculated by the speed calculation unit are within the allowable range.
- a solution of an inverse kinematics problem in which an angle at the next interpolation point of the drive shaft located at the center has the same sign as the angle of the drive shaft at the work start position is calculated, and the first calculated by the speed calculation means
- the angle at the interpolation point next to the drive shaft located at the center of the first joint drive system is You may calculate the solution of the inverse kinematics problem which becomes a different sign from the angle of this drive shaft in a work start position.
- the angle calculation means is positioned at the center of the first joint drive system when the angle of the drive shaft at the center of the first joint drive system at the work start position and the work end position has the same sign.
- a solution of an inverse kinematics problem in which an angle at the interpolation point next to the drive shaft has the same sign as the angle of the drive shaft at the work start position is calculated, and the first joint at the work start position and the work end position is calculated.
- the speeds of the two drive shafts at both ends in the first joint drive system calculated by the speed calculation means are within the allowable range.
- the recalculation unit is configured to increase the amount of suppression of the drive shaft in order from the specific component having a large predetermined weight among the posture data of the working unit converted by the posture data conversion unit. It is conceivable that the angle is calculated again. Thus, by appropriately setting the weighting, it is possible to appropriately adjust the amount of suppression of the variation of the specific component.
- the working unit is a torch
- the posture data is an axis represented by an outer product of the X-axis and the gravity direction as an axis in the movement direction of the working unit.
- the Y axis the axis in the direction represented by the outer product of the X axis and the Y axis is the Z axis
- the rotation angle around the X axis is the torch tilt angle
- the rotation angle around the Y axis is the torch
- the advancing angle is expressed in a work coordinate system in which the rotation angle around the Z-axis axis is a torch rotation angle
- the specific component includes the torch inclination angle, the torch advance angle, and the torch rotation angle.
- the working unit is moved in the direction of movement from the wrist tip position XYZ and the wrist tip posture ⁇ represented on the orthogonal coordinate system ⁇ base.
- the posture of the working part can be represented as an inclination component with respect to each axis as shown in FIGS.
- the rotation angle around the coordinate axis of the transformed welding line coordinate system ⁇ line is the torch inclination angle Rx with the rotation angle around the Xline axis being the right-handed direction as the positive direction.
- the rotation angle around the Yline axis is the torch advance angle Ry with the right screw direction as the positive direction
- the rotation angle around the Zline axis is the torch rotation angle Rz with the right screw direction as the positive direction.
- the specific component includes the torch inclination angle and the torch forward movement. It is considered to be a horn.
- the specific component includes the torch inclination angle and the torch rotation angle. It is thought that it is. Thereby, the fluctuation
- the difference between the angle of the first joint drive system calculated by the angle calculation means and the angle of the first joint drive system calculated by the recalculation means becomes a predetermined value or less.
- the articulated robot is driven based on the angle of each of the drive shafts calculated by the angle calculation unit instead of the angle of each of the drive shafts calculated by the recalculation unit. It is possible that there is.
- the drive instruction means is based on the angles of the drive axes calculated by the angle calculation means from the drive control of the articulated robot based on the angles of the drive axes calculated by the recalculation means.
- the angle of each drive shaft is gradually changed within a predetermined fluctuation range. Accordingly, it is possible to prevent the work line moving along the work line of the work object from vibrating and not moving along the work line.
- it may be an intermediate angle between the angle calculated by the recalculation unit and the angle calculated by the angle calculation unit from the drive control of the articulated robot.
- the speed calculation means may calculate acceleration instead of speed
- the recalculation means and the drive instruction means may determine based on acceleration instead of speed.
- the present invention relates to an invention of an articulated robot control method for executing each step executed by the articulated robot control device, or an articulated robot control program for causing a computer to execute each step. It can be understood as an invention.
- the wrist tip position and wrist particularly required for welding or painting work are required even when the wrist shaft rapidly changes beyond a predetermined allowable amount. It is possible to maintain the moving speed of the tip and the allowable speed of the wrist axis, and to suppress the fluctuation of at least one specific component of the posture angle in the work coordinate system according to the purpose. Further, when calculating a singularity region, a singularity avoidance path, etc. existing between the work start position and the work end position, an arithmetic unit (CPU) capable of dealing with an excessive load in the pre-calculation.
- CPU arithmetic unit
- the flowchart which shows an example of the control whole process of the control method of an articulated robot.
- the flowchart which shows an example of the speed / angle calculation process of each rotating shaft of an interpolation point.
- the flowchart which shows an example of a speed suppression necessity determination process.
- the flowchart which shows an example of a speed suppression process.
- the figure which shows the other example of a 2nd joint drive shaft The flowchart which shows the other example of the control whole process of the control method of an articulated robot.
- route which passes through a singular point. 6 is a chart showing an example of an angle change of the first joint drive shaft when the control method according to the first embodiment is employed.
- FIG. 10 is a diagram illustrating an example of an end effector having two work points according to the third embodiment.
- the figure which shows an example of the fluctuation
- FIG. 10 is a block diagram illustrating a schematic configuration of an articulated robot X2 controlled by an example of a control method for an articulated robot according to a fourth embodiment.
- FIG. 10 is a block diagram illustrating a schematic configuration of a computer system Y according to a fifth embodiment. The figure for demonstrating the apparatus which alert
- the articulated robot X includes a control unit 10, an operation unit 21, a manipulator body 30, and the like. In addition to the components shown in FIG. 1, the articulated robot X includes other components included in a general articulated robot, but the description thereof is omitted here.
- the control unit 10 includes a main control unit 11, a storage unit 12, and a drive instruction unit 13, and is an example of a control device for an articulated robot according to the present invention.
- the manipulator body 30 has a second joint drive system having a J1 axis 31, J2 axis 32, and J3 axis 33 corresponding to a human arm, and a J4 axis 34, J5 axis 35, and J6 axis 36 corresponding to a human wrist.
- the robot includes a first joint drive system and an end effector 39 that exerts an effect on a work target, and is driven according to the operated content and performs a work similar to that performed by a human upper arm.
- the manipulator body 30 is an example of an articulated robot.
- the J1 axis 31 to J6 axis 36 are constituted by an electric motor or the like, and are rotationally driven in the positive and negative directions according to instructions from the drive instructing unit 13, and the rotation driving of a plurality of axes is linked, thereby allowing human wrists and arms to be rotated. Realize the movement.
- the second joint drive system determines the position (X, Y, Z) of the end effector 39 in the base coordinate system ⁇ base of the articulated robot regardless of the operation of the drive shaft of the first joint drive system. Can be produced freely.
- the end effector 39 is an effector attached to the tip of the manipulator body 30 (the tip of the J6 shaft 36), and is a welder (torch), a painter, a tool, a catcher, a sensor, etc. that operates at one work point. It is.
- the end effector 39 is an example of a working unit.
- a torch having one work point is called a single torch.
- the operation unit 21 is composed of sheet keys, operation buttons, operation levers, and the like operated by the user, and is an input interface that receives an operation input by the user.
- the operation unit 21 has a welding line coordinate system ⁇ line (working coordinate system) according to the work path, work time, and purpose of the work start position / posture of the end effector 39, the work end position / posture, and the route connecting them.
- the information is output to the control unit 10. That is, the operation unit 21 is a means for teaching the work start position and posture, the work end position and posture of the end effector 39, and is a means for setting the weighting information.
- the storage unit 12 is, for example, a volatile memory configured by a hard disk, a DRAM, or the like, and a work path and a work time between a work start position and posture of the end effector 39, a work end position and posture, and a route connecting these, Weighting information for suppressing fluctuation of each component of the posture angle of the end effector 39, interpolation points calculated by the main control unit 11 described later (both a route passing through a singular point and a route avoiding a singular point) The angle and speed (and further acceleration in some cases) of the J1 axis 31 to J6 axis 36 are stored.
- the drive instructing unit 13 receives the end effector at the next interpolation point (one of the path passing through the singular point or the path avoiding the singular point) stored in the storage unit 12 for each sampling period in accordance with the control instruction from the main control unit 11.
- the information on the angles or speeds of the J1 axis 31 to J6 axis 36 is read so as to move 39, and is output to the manipulator body 30.
- the main control unit 11 and the drive instruction unit 13 are an example of drive instruction means for executing a drive instruction process.
- the main control unit 11 includes a CPU, a ROM, and the like.
- the CPU is a calculation means for performing various controls and calculations of the articulated robot X.
- the ROM is a control program or calculation program executed by the CPU. It is a memory that stores data and the like to be referred to when executing these programs.
- S11, S12,... Shown below represent identification codes of processing procedures (steps).
- the general manipulator body 30 is controlled by performing a teaching step (step S11) of teaching the operation locus to the main control unit 11, and further operating to the position / posture of the interpolation point on the taught operation locus.
- step S11 teaching the operation locus to the main control unit 11, and further operating to the position / posture of the interpolation point on the taught operation locus.
- step S12 calculates and store the angle of each drive shaft (step S12), to read out the calculated angle and instruct operation (step S14), and to determine whether or not the work end position / posture has been reached.
- step S15 is performed to operate the manipulator body 30 from the current position / posture of the interpolation point to the position / posture of the next interpolation point, and thereafter, steps S12, S14 are performed every predetermined sampling period. , S15 is repeated to operate the manipulator body 30 from the work start position / posture to the work end position / posture along the operation trajectory. Further, the main control unit 11 executes the work by the end effector 39 by moving the end effector 39 of the manipulator body 30 at a predetermined constant speed.
- the control of the articulated robot X includes the J4 axis 34 and the J6 axis 36 of the first joint drive system, as will be described later, in addition to the steps necessary for the control of the general manipulator body 30 described above.
- step S20 for recalculating and storing the angle of each drive axis to operate the interpolation point on the trajectory, and avoiding the singular point and positioning the manipulator body 30 at the current position / posture of the interpolation point. Is operated to the position / posture of the next interpolation point recalculated from
- Step S11 is an example of the teaching process.
- the main control unit 11 divides and interpolates between the taught points at predetermined sampling periods (for example, 1/16 second, 1/32 second, etc.), and moves the end effector 39 to the next interpolation point.
- the angles of the J1 axis 31 to J6 axis 36 and the speed (according to circumstances, further acceleration) of the J4 axis 34 to J6 axis 36 of the first joint drive system are calculated and stored in the storage unit 12 (step S12).
- the angles of the J1 axis 31 to the J6 axis 36 are expressed as ⁇ 180 °.
- the method of specifying the angle is not limited to ⁇ 180 °, and may be, for example, 0 ° to 360 °. Details of step S12 will be described later.
- the main control unit 11 allows the speed and acceleration of one or both of the J4 axis 34 and the J6 axis 36 of the first joint drive system for moving the end effector 39 calculated in step S12 to the next interpolation point. Whether it is within the range or not (hereinafter referred to as needing / unnecessary for speed suppression processing), or during avoidance processing (hereinafter referred to as speed suppression processing) after it has been determined that there is already a drive shaft outside the allowable range. It discriminate
- step S13 if it is determined that the speed suppression process is necessary or is being performed (step S13; YES), the main control unit 11 is calculated by step S12 without changing the moving speed of the end effector 39.
- One or both of the J4 axis 34 and the J6 axis 36 so that the fluctuation of one or two specific components is suppressed with respect to the posture data of the end effector 39 at the next interpolation point so that the speed and acceleration are within the allowable range. Both of the angles are calculated again, and the speed of the other axis corresponding to this is recalculated and stored in the storage unit 12 (step S20).
- step S20 the speed and acceleration of each axis are taken as a problem as a guideline for determining the necessity / unnecessity of the speed suppression process.
- step S20 An example of the determination will be described.
- step S13 if it is determined that the speed suppression process is not necessary and the speed suppression process is not in progress (step S13; NO) or the speed suppression process (step S20) is performed, the drive instruction unit 13 is instructed by an instruction from the main control unit 11.
- the angles of the J1 axis 31 to J6 axis 36 for moving the end effector 39 to the next interpolation point are read from the storage unit 12 and output to the actuators of the respective drive axes of the manipulator body 30 (step S14).
- step S12 when it is determined that the speed suppression process is not necessary and the speed suppression process is not being performed, based on the angles of the J1 axis 31 to the J6 axis 36 for moving to the next interpolation point calculated in step S12.
- step S20 When the manipulator body 30 is controlled and the speed suppression process is executed when the speed of the J4 axis 34 or the J6 axis 36 is out of the allowable range, J1 for moving to the next interpolation point calculated in step S20.
- the manipulator body 30 is controlled based on the angles of the shafts 31 to J6.
- step S14 is an example of the drive instruction step, and the main control unit 11 and the drive support unit 13 when executing the process correspond to the drive instruction means.
- step S15 determines whether the position and posture of the end effector 39 after movement have reached the working unit end position and posture.
- step S15 determines whether the position and posture of the end effector 39 after movement have reached the working unit end position and posture.
- the main control unit 11 calculates the angles of the J1 axis 31 to the J6 axis 36 for moving the end effector 39 from the current position and posture to the next interpolation point at a predetermined sampling period, thereby The type robot X is operated.
- An example of a processing procedure for calculating the speed of the J4 axis 34 to the J6 axis 36 of the joint drive system will be described.
- the main control unit 11 determines n (arbitrary values based on each sampling period) points between the work start position and posture P 0 and the work end position and posture P n previously taught in step S11. The position and orientation of the end effector 39 interpolated in (5) are calculated.
- Each of the interpolation points is represented by a base coordinate system ⁇ base of the articulated robot X.
- This step S121 is an example of an interpolation data calculation step, and the main control unit 11 when executing such processing corresponds to the interpolation data calculation means.
- the main controller 11 determines the position and orientation of the next interpolation point P i + 1 (X i + 1 , Y i + 1 , Z i + 1 , ⁇ i + 1 , ⁇ i + 1 , i + 1 ) To calculate the angle ( ⁇ 1 , ⁇ 2 , ⁇ 3 , ⁇ 4 , ⁇ 5 , ⁇ 6 ) of each of the J1 axis 31 to J6 axis 36, and the storage unit 12. (Step S122). At this time, two solutions of the inverse kinematics problem are obtained for the J4 axis 34 to the J6 axis 36.
- the main control unit 11 determines the angle of the J5 axis 35 of the first joint drive system among the two solutions.
- the solution of the inverse kinematics problem having the same sign as the angle of the J5 axis 35 at the work start position of the end effector 39 is employed. Thereby, since the angle of the J5 axis 35 changes without passing through 0 °, the passage of the singular point can be avoided.
- the main control unit 11 avoids not only the angle of each of the J1 axis 31 to J6 axis 36 of the interpolation point of the path passing through the singular point, but also the path that avoids the singular point.
- the angles of the J1 axis 31 to J6 axis 36 of the interpolation points are also calculated and stored in the storage unit 12, and are compared when performing a return process (step S310 in FIG. 5 and the flowchart in FIG. 6) described later.
- step S122 is an example of an angle calculation step, and the main control unit 11 when executing such processing corresponds to an angle calculation means.
- the main control unit 11 calculates each J4 axis of the current interpolation point P i stored in the storage unit 12 from the calculated angle of each J4 axis 34 to J6 axis 36 of the next interpolation point P i + 1.
- the angles of the 34 to J6 axes 36 are subtracted, and the speeds of the J4 axes 34 to J6 axes 36 of the first joint drive system are calculated and stored in the storage unit 12 (step S123).
- step S20 when the speed suppression process (step S20) is executed, the speeds of the J4 axis 34 to J6 axis 36 of the first joint drive system at the interpolation point of the system passing through the singular point are calculated and stored. This is stored in the unit 12 and used to calculate a speed allowable range in step S131 described later, or to calculate a speed (acceleration if necessary) as a candidate for the next interpolation point in step S211 or the like.
- step S123 is an example of a speed calculation step, and the main control unit 11 when executing such processing corresponds to the speed calculation means.
- the main control unit 11 allows the speed of the J4 axis 34 and J6 axis 36 of the first joint drive system when moving the position and orientation of the end effector 39 from the current interpolation point Pi to the next interpolation point P i + 1.
- the range is calculated (step S131).
- the allowable range of the speed is calculated by using a value obtained by adding or subtracting a predetermined value or a predetermined ratio to the speed value of the current interpolation point Pi as upper and lower limits.
- the permissible range of the speed of each axis depends on the operating conditions of the end effector 39 (for example, conditions such as the temperature at the time of welding, the thickness of the coating film at the time of painting) and the operation of the articulated robot. It may be determined in advance within a range that does not affect the quality of the work object, such as conditions for preventing abnormal operations such as vibration. Further, individual allowable ranges may be set for each of the J4 axis 34 and the J6 axis 36.
- the main control unit 11 indicates that the speed of one or both of the J4 axis 34 and the J6 axis 36 at the next interpolation point of the first joint drive system exceeds the allowable range, or indicates that the speed suppression process is being performed. It is determined whether or not a flag is set (step S132).
- the main control unit In response to the instruction 11, the drive instruction unit 13 reads out the angles of the J1 axis 31 to J6 axis 36 calculated in step S12 from the storage unit 12 and outputs them to the manipulator body 30 (step S14).
- the main control unit 11 executes the speed suppression process (step S20) described below.
- the speed suppression process of the articulated robot X corresponding to step S20 of FIG. 2 will be described with reference to FIG.
- the speed suppression process is performed when the speed of one or two of the J4 axis 34 and J6 axis 36 of the first joint drive system exceeds the allowable range. Note that when an axis outside the allowable range is first found for the next interpolation point, the speed at the current interpolation point is within the allowable range.
- the main control unit 11 determines whether the drive axis whose speed is out of the allowable range for the next interpolation point is either the J4 axis 34 or the J6 axis 36 or both, or is already in the speed suppression process. If the flag is set, it is determined whether the speed suppression process is executed for one or both of the J4 axis 34 and the J6 axis 36, and the process branches (step S201). Whether the speed is within the allowable range or not is determined according to whether the speed is between the upper limit and the lower limit of the allowable range as described above. Here, if both the J4 axis 34 and the J6 axis 36 are not out of the allowable speed range and the flag for the speed suppression process is not set (step S201; other), the main control unit 11 performs the speed suppression process. finish.
- step S211 If the speed of the J4 axis 34 is outside the allowable range, the process proceeds to step S221, and the J4 axis 34 and the J6 axis If both speeds 36 are outside the allowable range, the process proceeds to step S231. (J6 axis 36 speed suppression processing steps S211 to S217) If the speed of the J6 axis 36 is out of the allowable range or if the flag indicating that the speed suppression processing of the J6 axis 36 is in progress is set (step S201; J6 axis is out of range), the main control unit 11 performs the current interpolation.
- the angle of the J6 axis 36 follows the angle at the next interpolation point calculated in step S12, and the speed at the next interpolation point of the J6 axis 36 is within the allowable range.
- a plurality of inner angle candidates are calculated (step S211).
- the main control unit 11 calculates speed candidates for the J6 axis 36 by the following procedures (21) to (23).
- the suppression reference speed V6b is calculated by the following equation (21a).
- ⁇ now is the angle of the J6 axis 36 at the next interpolation point
- ⁇ old is the angle of the J6 axis 36 at the current interpolation point
- E is a preset coefficient (for example, 0.1).
- V6b E ( ⁇ now ⁇ ⁇ old ) (21a)
- suppression reference speed V6b it is determined whether a speed limiter V max than the predetermined, when it is determined that the velocity limiter V max or more, the suppression reference speed V6b previous Set speed ⁇ old .
- the main controller 11 determines the three J6 axes 36 at the next interpolation point corresponding to each of the speed candidates (V ⁇ 6a , V ⁇ 6b , V ⁇ 6c ) from the angle ⁇ 6 of the J6 axis 36 at the current interpolation point.
- Angle candidates ( ⁇ 6a , ⁇ 6b , ⁇ 6c ) are calculated. That is, as the next interpolation point, at least the suppression reference speed V 6b not exceeding the allowable range or the angle at which the speed at the current interpolation point is continued (which is naturally within the allowable range), and a small amount ( ⁇ D (Corresponding to the absolute value of the change in speed)) is calculated.
- the process of step S211 is an example of an angle candidate calculation step, and the main control unit 11 when executing the process corresponds to an angle candidate calculation unit.
- the main control unit 11 calculates the angles of the other drive axes J1 axis 31 to J5 axis 35 corresponding to the respective candidate angles of the J6 axis 36 at the calculated next interpolation point (step S212). For example, as shown in the following (1) to (3), for each angle candidate ( ⁇ 6a , ⁇ 6b , ⁇ 6c ) of the J6 axis 36, the next interpolation point P i + calculated in step S 122 is used. 1 using the position (X i + 1 , Y i + 1 , Z i + 1 ) of the end effector 39 and the angles ( ⁇ 4 , ⁇ 5 ) of the J4 axis 34 and J5 axis 35 of the first joint drive system.
- angles ( ⁇ 1 , ⁇ 2 , ⁇ 3 , ⁇ 4 , ⁇ 5 , ⁇ 6 ) of the J1 axis 31 to J6 axis 36 of the first joint drive system and the second joint drive system are recalculated.
- the next interpolation point candidate P ′ i + 1 whose speed does not exceed the allowable range is calculated.
- the speed of the J6 axis 36 does not exceed the allowable range, and an exceptional path (see FIG. 8B) that avoids the original interpolation point P i + 1 (singular point) can be obtained. it can.
- three angle candidates ( ⁇ 6a , ⁇ 6b , ⁇ 6c ) at the next interpolation point that maintains the speed of the J6 axis 36 within the allowable range are obtained in this way, which candidate is selected as the next interpolation point. A decision is made on what to do.
- the angles ( ⁇ 4 , ⁇ 5 , ⁇ 6 ) of the J4 axis 34 to J6 axis 36 of the first joint drive system are regarded as problems, but ultimately they are important depending on the type of work. Candidates must be selected focusing on the posture of the end effector 39 being viewed. Therefore, it is necessary to convert the angle of the J1 axis 31 to J6 axis 36 into data indicating the attitude of the end effector 39.
- the main control unit 11 sets the data indicating the attitude of the end effector 39 of the weld line coordinate system ⁇ line at the next interpolation point for each calculated candidate. Are calculated (torch tilt angle Rx, torch advance angle Ry, torch rotation angle Rz), respectively (step S213).
- This step S213 is an example of an attitude data calculation process, and the main control unit 11 when executing such processing corresponds to an attitude data calculation means.
- the weld line coordinate system ⁇ line as shown in FIG.
- the axis in the direction (traveling direction) in which the end effector 39 moves is the X axis, and the outer product of the X axis and the gravity direction (X axis ⁇ gravity).
- 26 is a working coordinate system in which the axis represented by the direction) is the Y axis, and the axis in the direction represented by the outer product of the X axis and the Y axis (X axis ⁇ Y axis) is the Z axis. As shown in FIG.
- each component of data indicating the attitude of the end effector 39 is calculated by the following (1 ′) to (3 ′) with respect to the candidates ⁇ 6a to ⁇ 6c of the above (1) to (3).
- (1 ′) [R xa , R ya , R za ] is calculated from [ ⁇ 1a , ⁇ 2a , ⁇ 3a , ⁇ 4 , ⁇ 5 , ⁇ 6a ]].
- the main control unit 11 represents the position and orientation of the end effector 39 at the next interpolation point Pi + 1 on the original path calculated in step S121 by an orthogonal coordinate system ⁇ base (X i + 1 , Y i + 1 , Z i + 1 , ⁇ i + 1 , ⁇ i + 1 , ⁇ i + 1 ) and the welding line coordinate system ⁇ line (torch inclination angle) with the axis in the direction in which the end effector 39 moves as the X axis Data is converted to be represented by Rx, torch advance angle Ry, torch rotation angle Rz) (step S214).
- This step S214 is an example of the attitude data conversion process, and the main control unit 11 when executing such processing corresponds to the attitude data conversion means.
- Equation 1 the equation for converting from the orthogonal coordinate system ⁇ base to the welding line coordinate system ⁇ line is shown in the following (Equation 1).
- the main control unit 11 includes each component of the data indicating the attitude of the end effector 39 in the welding line coordinate system ⁇ line converted in step S213 (the torch inclination angle Rx, the torch advance angle Ry, Information (weighting information) indicating a component whose fluctuation should be suppressed is acquired from the torch rotation angle Rz) (step S215).
- the main control unit 11 may arbitrarily set which component is to be weighted by an input from the operation unit 21.
- the end factor 39 is a welding torch having one work point, and the angle of the torch with respect to the work object (material) and the traveling speed of the torch are important due to the nature of the welding work using the welding torch.
- the weighting information is set in advance with the torch inclination angle Rx and the torch advance angle Ry as components having a large weight (to be suppressed) and the torch rotation angle Rz as a component having a small weight (not to be suppressed). It shall be.
- This weighting information is used as a coefficient corresponding to each component of the evaluation formula for selecting a candidate that suppresses the fluctuation in step S216 described later.
- the main control unit 11 selects specific components (torch inclination angle Rx, torch advance angle Ry, torch inclination angle Rx, large weight for suppression acquired in step S215 described above from among the plurality of candidates at the calculated next interpolation point.
- Rx, Ry, and Rz are the torch inclination angle, torch advance angle, and torch rotation angle of the next interpolation point on the original path.
- [Fa A (Rx ⁇ Rxa ) 2 + B (Ry ⁇ R ya ) 2 + C (Rz ⁇ R za ) 2 ] (2 ′′)
- [Fb A (Rx ⁇ Rxb ) 2 + B (Ry ⁇ R yb ) 2 + C (Rz ⁇ R zb ) 2 ]
- [Fc A (Rx ⁇ R xc ) 2 + B (Ry ⁇ R yc ) 2 + C (Rz ⁇ R zc ) 2 ]
- the component for suppressing fluctuation is weighted, and the attitude angle of the end effector 39 at the next interpolation point on the original path calculated in step S214, and the step Differences Fa to Fc from the posture angle of each candidate end effector 39 of the next interpolation point
- the selected candidate drive axis J1 axis 31 to J6 axis 36 is adopted as the angle of each axis during the operation to avoid the singular point.
- the process of step S216 is an example of an angle selection process, and the main control unit 11 when executing the process corresponds to an angle selection unit.
- the angles of the selected candidate drive axes J1 axis 31 to J6 axis 36 and the speed of the J6 axis 36 are stored in the storage unit 12.
- step S217 if the flag for the speed suppression process for the J6 axis 36 is not set, the main control unit 11 sets a flag to indicate that the speed suppression process for the J6 axis 36 is being performed (step S217).
- steps S211 to S213, S216, and S217 are an example of a recalculation step, and the main control unit 11 when executing such processing corresponds to the recalculation means.
- steps S211 to S217) as shown in FIGS. 8A and 8B, the next point on the original path from the current interpolation point position Pi via the singular point is obtained.
- the next interpolation point P′i + 1 on the exceptional path that avoids the singular point is found instead of the interpolation point Pi + 1.
- step S201, S211 to S21-7 is repeated until the next interpolation point becomes the interpolation point of the original route (step S201; other), and an exceptional route that avoids a singular point is calculated.
- the calculation process is not executed in advance, but is executed by the main control unit 11 each time according to the current state and the next state. The processing load instantaneously required for the unit 11 is reduced, and the cost can be reduced.
- the suppression reference speed V 6b that can be originally maintained within the allowable range or the J6 axis 36 of the current interpolation point. Since the angle is included, the speed of the J6 axis 36 at the next interpolation point can be maintained within the allowable range by any of the candidates adopted.
- the details of the return process (step S310) from the speed suppression process of the J6 axis 36 executed by the main controller 11 will be described later.
- J4 axis 34 speed suppression processing steps S221 to S227) When the speed of the J4 axis 34 is out of the allowable range or when the flag indicating that the J4 axis 34 is in the speed suppression process is set (step S201; the J4 axis is out of range), the main control unit 11 performs the above-described J6. Similar to the case of calculating the speed and angle candidates of the axis 36, a plurality of J4 axis angle candidates that fall within the allowable range are calculated (step S221).
- the predetermined value G is an absolute value of a change in speed that is known to maintain acceleration within a predetermined allowable range.
- the process of step S221 is an example of an angle candidate calculation step.
- steps S222 to S227 and S320 are the same as the processes in steps S212 to S217 and S310 for the J6 axis 36 described above, and thus the description thereof is omitted.
- J4 axis 34 / J6 axis 36 speed suppression processing steps S231 to S237) When the speeds of both the J4 axis 34 and the J6 axis 36 are out of the allowable range, or the flag indicating that the speed suppression processing of both the J4 axis and the J6 axis 36 is in progress is set (step S201; J4 axis / J6 axis is in the range) Outside), the main control unit 11 calculates speed and angle candidates for one or more J4 axes 34 and J6 axes 36 that fall within the allowable range (step S231).
- J4 axis 34 candidates and J6 axis 36 candidates are combined to obtain ( ⁇ 4a , ⁇ 6a ), ( ⁇ 4a , ⁇ 6b ), ( ⁇ 4a , ⁇ 6c ), ( ⁇ 4b , ⁇ 6a ),
- Nine candidates of ( ⁇ 4b , ⁇ 6b ), ( ⁇ 4b , ⁇ 6c ), ( ⁇ 4c , ⁇ 6a ), ( ⁇ 4c , ⁇ 6b ), ( ⁇ 4c , ⁇ 6c ) are calculated.
- step S231 is an example of an angle candidate calculation step.
- the main control unit 11 calculates the angles of the other drive axes J1 axis 31 to J3 axis 33 and J5 axis 35 corresponding to the respective angle candidates of the J4 axis 34 and J6 axis 36 at the calculated next interpolation point. (Step S232). For example, for each pair of angle candidates ( ⁇ 4a , ⁇ 6a ) to ( ⁇ 4c , ⁇ 6c ) of the J4 axis 34 / J6 axis 36, the end of the next interpolation point P i + 1 calculated in the step S122.
- J1 axis 31 of the second joint drive system using the position (X i + 1 , Y i + 1 , Z i + 1 ) of effector 39 and the angle ( ⁇ 5 ) of J5 axis 35 of the first joint drive system.
- the angles ( ⁇ 1 , ⁇ 2 , ⁇ 3 ) of the J3 axis 33 are recalculated.
- next interpolation point candidate P ′ i + 1 whose speed does not exceed the allowable range is calculated, and the next interpolation point candidate P ′ i + 1 is regarded as the next interpolation point.
- An exceptional path that avoids the next interpolation point Pi + 1 (singular point) that does not exceed the range can be obtained.
- the angle candidates of the J1 axis 31 to J6 axis 36 are converted into data indicating the attitude of the end effector 39.
- the main control unit 11 includes each component of data indicating the attitude of the end effector 39 in the welding line coordinate system at the calculated next interpolation point (torch inclination angle Rx, torch advance angle Ry, torch rotation angle Rz). Is calculated (step S233).
- each component of data indicating the attitude of the end effector 39 is calculated by the following (1 ′) to (9 ′) with respect to the angle candidates (1) to (9).
- (1 ′) [R xaa , R yaa , R zaa ] is calculated from [ ⁇ 1aa , ⁇ 2aa , ⁇ 3aa , ⁇ 4a , ⁇ 5 , ⁇ 6a ]].
- [R xbb , R ybb , R zbb ] is calculated from [ ⁇ 1bb , ⁇ 2bb , ⁇ 3bb , ⁇ 4b , ⁇ 5 , ⁇ 6b ].
- (6 ′) [R xbc , R ybc , R zbc ] is calculated from [ ⁇ 1bc , ⁇ 2bc , ⁇ 3bc , ⁇ 4b , ⁇ 5 , ⁇ 6c ].
- the main control unit 11 includes each component of the data indicating the attitude of the end effector 39 in the weld line coordinate system at the next interpolation point P i + 1 calculated in step S234 (the torch inclination angle Rx, the torch advance angle). Ry, torch rotation angle Rz) is calculated (step S234).
- This step S234 is an example of the posture data conversion step, and the main control unit 11 when executing such processing corresponds to the posture data conversion means.
- the main control unit 11 calculates each component (torch inclination angle Rx, torch advance angle Ry, torch rotation angle Rz) of the data indicating the attitude of the end effector 39 in the welding line coordinate system ⁇ line converted in step S233.
- Information (weighting information) indicating the component whose fluctuation should be suppressed is acquired from the inside (step S235). Further, the main control unit 11 selects a specific component (torch inclination angle Rx, torch advance angle Ry, torch, etc.) having a large weight for suppression obtained in step S235 described above from the plurality of candidates at the calculated next interpolation point. A candidate with the smallest fluctuation in one of the rotation angles Rz) is selected (step S236).
- Differences F aa to F cc from the candidate end effector 39 posture angles at the next interpolation point of the exceptional path calculated in step (3) are obtained, so the smallest value among the differences F aa to F cc (variation) Is selected, and the angles of the selected candidate drive axes J1 axis 31 to J6 axis 36 are set as the angles of the respective axes during the operation to avoid the singular point.
- the process of step S236 is an example of an angle selection process, and the main control unit 11 when executing the process corresponds to an angle selection unit. Further, the angles of the selected candidate drive axes J1 axis 31 to J6 axis 36 and the speeds of the J4 axis 34 and J6 axis 36 are stored in the storage unit 12.
- the main control unit 11 sets a flag to indicate that the speed suppression process for the J4 axis 34 / J6 axis 36 is being performed (step S11). S237).
- step S330 Details of the return processing (step S330) from the speed suppression processing of the J4 axis 34 and J6 axis 36 executed by the main control unit 11 will be described later.
- step S330 an example of the procedure of the return process from the speed suppression process of the articulated robot X corresponding to step S310 in FIG. 5 will be described with reference to FIG.
- FIG. 8 (B) the main control unit 11 starts from the next interpolation point P ′ i + 1 on the exceptional path avoiding the singular point intervals P i to P i + 1 , through the singular point.
- the process further calculates a difference angle between the next interpolated point P 'i + 2 on the further exception route that avoids singularities and interpolated point P i + 2 for the next on the path, the difference is a predetermined range If not, the process proceeds to P i + 3 and ends this process. If it exceeds, the process proceeds to P ′ i + 3 , and this process is repeated as long as the difference exceeds a predetermined range. Eventually when the difference is found next interpolation point that fall within a predetermined range (in this example, P 'if the i + 4 ⁇ P i + 5), the interpolation point P' interpolated point P i from i + 4 Go to +5 and finish this process. Therefore, the case where the current interpolation point P ′ i + 4 on the path avoiding the singular point returns to the next interpolation point P i + 5 on the path passing through the singular point will be mainly described.
- the main control unit 11 selects the angle ⁇ 6 (i + 5) of the J6 axis 36 of the next interpolation point P i + 5 of the system via the singular point calculated in step S122, and is selected in step S214.
- the angle ⁇ ′ 6 (i + 5) of the J6 axis 36 of the next interpolation point P ′ i + 5 in the system that avoids the singular point is acquired (step S311).
- the main control unit 11 of the J6 axis 36 of system that avoids singularities angle ⁇ '6 (i + 5) and the J6 axis 36 of system via the singularity angle ⁇ 6 (i + 5) and the It is determined whether the difference value is within a predetermined range (step S312).
- the predetermined range when actuating the end effector 39 is replaced angle theta '6 of the J6 axis 36 (i + 5) at an angle ⁇ 6 (i + 5), the general operation of such vibration
- the angle difference value is determined within a range where the above inconvenience does not occur. That is, it is determined whether or not the interpolation point on the avoided path has come close enough to return to the range that can be regarded as the interpolation point on the original path via the singular point.
- step S312 when the difference value exceeds the predetermined range (step S312; NO), the main control unit 11 ends the return process of step S310 and proceeds to step S14 (see FIG. 2).
- step S312; YES When the difference value is within the predetermined range (step S312; YES), the main control unit 11 sets the speed at which the angle ⁇ ′ 6 (i + 5) of the J6 axis 36 is replaced with the angle ⁇ 6 (i + 5). Calculate (step S313).
- the angle ⁇ 6 (i + 5) of the J6 axis 36 of the next interpolation point passing through the singular point and the angle ⁇ ′ 6 (i + 4) of the J6 axis 36 of the interpolation point of the path avoiding the singular point are Based on the difference, the speed of the J6 axis 36 is calculated.
- the main control unit 11 calculates the allowable range of the speed of the J6 axis 36 (step S314).
- the allowable range of the speed is calculated by setting a value obtained by adding or subtracting a predetermined value or a predetermined ratio to the speed value of the J6 axis 36 at the interpolation point of the path avoiding the singular point as an upper limit / lower limit.
- the permissible range of the speed of each axis depends on the operating conditions of the end effector 39 (for example, conditions such as the temperature at the time of welding, the thickness of the coating film at the time of painting) and the operation of the articulated robot. It may be determined in advance within a range that does not affect the quality of the work object, such as conditions for preventing abnormal operations such as vibration.
- the main control unit 11 determines whether or not the speed of the J6 shaft 36 is within a predetermined allowable range (step S315).
- the allowable range is a predetermined value within a range in which inconvenience in operation such as vibration does not occur when the end effector 39 is moved.
- the main control unit 11 changes the next interpolation point from P ′ i + 5 to P i + 5 and sets the J6 axis
- the angle of 36 is set to ⁇ 6 (i + 5) , and the angles of all the other drive shafts J1 axis 31 to J5 axis 35 corresponding to this are calculated (step S316).
- the main control unit 11 causes the storage unit 12 to store the calculated angles of all the drive axes J1 axis 31 to J6 axis 36. Further, the main control unit 11 lowers a flag indicating that the speed suppression process for the J6 shaft 36 is in progress (step S317).
- step S315 when the speed of the J6 axis 36 is outside the predetermined allowable range (step S315; NO), the main controller 11 determines an intermediate angle between the angle ⁇ 6 (i + 5) and the angle ⁇ ′ 6 (i + 5).
- ⁇ ′′ 6 (i + 5) is the angle of the J6 axis 36
- the interpolation point including the angle ⁇ ′′ 6 (i + 5) of the J6 axis 36 is the next interpolation point P ′′ i + 5 (step S318).
- the angles of all the other drive axes J1 axis 31 to J5 axis 35 are calculated (step S319), that is, the main controller 11 determines the angle ⁇ ′ 6 (i + 5) of the J6 axis 36 as the angle.
- the angle of the drive shaft J6 axis 36 does not change abruptly from ⁇ ′ 6 (i + 5) to ⁇ 6 (i + 5) , but within a predetermined fluctuation range. Change gradually.
- the main control unit 11 causes the storage unit 12 to store the calculated angles of all the drive axes J1 axis 31 to J6 axis 36.
- the return process from the speed suppression process of the J4 axis 34 corresponding to step S320 is the same as the return process (steps S311 to S319) from the speed suppression process of the J6 axis 36 described above, and thus description thereof is omitted.
- the recovery processing from the speed suppression processing of the J4 axis 34 and J6 axis 36 corresponding to step S330 is the speed suppression processing of the J6 axis 36 except that both the recovery processing of the J4 axis 34 and the recovery processing of the J6 axis 36 are performed together. Since this is the same as the return processing from, the description is omitted.
- the specific components (torch inclination angle Rx, torch advance angle Ry, torch rotation angle Rz) having a large weight for suppressing fluctuations centered on the main control unit 11 of the articulated robot X.
- the interpolation point based on the original teaching point is increased by operating the position and posture of the end effector 39 provided in the manipulator body 30 while suppressing the singularity while suppressing the fluctuation of any one of (2). It is possible to obtain a work trajectory in which the speeds of all the axes are within the allowable range without deviating and without changing the moving speed of the end effector 39.
- the speed of the J4 axis 34 changes abruptly in the vicinity of the singular point when the speed is not limited (one-dot chain line), but when the speed is simply limited (solid line), the control of the present invention is performed.
- the speed is kept within an allowable range.
- the attitude fluctuation angle of the torch advance angle or the torch inclination angle greatly fluctuates.
- the torch rotation angle with a small weight varies greatly, but the torch tilt angle and the torch advance angle with a large weight
- the fluctuation of the posture fluctuation angle can be suppressed.
- the J1 axis 31 may be driven in the vertical direction, and the driving directions of the J2 axis 32 and the J3 axis 33 may be the same direction.
- the J1 axis 31 and the J2 axis 32 may be driven in the horizontal direction, and the J3 axis 33 may be driven in the vertical direction.
- the speed is calculated based not only on the difference in the rotation angle of the motor for moving the drive shaft up and down, but also on the difference in the moving distance in which the drive shaft moves up and down. May be.
- the speed exceeds the allowable range
- the method for suppressing the speed of the other axis by increasing the fluctuation amount of the other axis has been described.
- the speed change amount obtained in the above description is described.
- the amount of fluctuation of the axis may be suppressed to calculate the angle data of each axis so that the acceleration falls within the allowable range.
- the speed may be calculated by substituting the acceleration that is the amount of change.
- step S123 the main control unit 11 calculates acceleration instead of speed.
- step S131 the main control unit 11 calculates an allowable range of acceleration.
- step S132 the main control unit 11 calculates. However, it is determined whether the acceleration calculated in step S123 is outside the allowable range of acceleration calculated in step S131. If it is out of the allowable range, the angle data of each axis is calculated so that the acceleration is within the allowable range.
- the predetermined value D or the predetermined value G used when calculating acceleration candidates is an absolute value of a change in acceleration, which is known to maintain jerk within a predetermined allowable range. It is. Since only the speed is replaced with the acceleration and the acceleration is replaced with the jerk (jump), the detailed description is omitted here.
- the articulated robot X is controlled by the method described in the embodiment, even when the sign of the angle of the J5 axis 35 is different at the work start position and the work end position, the angle of the J5 axis 35 is 0. Since the angle is controlled so as not to pass through the angle, the angle of the J5 axis 35 at the work end position is different from the original target value. In this case, for example, as shown in FIG. 15, the angles of the remaining J4 axis 34 and J6 axis 36 may change abruptly or may exceed the operation limit angle (for example, ⁇ 180 °).
- the operation limit angle for example, ⁇ 180 °
- the change amount of the angle of the J6 axis 36 rapidly increases or decreases.
- the other solution of the J6 axis 36 approaches the current angle of the J6 axis 36. Therefore, in the first embodiment, a configuration for preventing a rapid change in the J4 axis 34 and the J6 axis 36 by using two solutions obtained by the inverse kinematics problem will be described.
- FIG. 14 is a flowchart for explaining another control method of the articulated robot X. Note that the same processing steps as the processing contents of FIG. 2 described in the above embodiment are denoted by the same reference numerals and detailed description thereof is omitted.
- the main control unit 11 moves the end effector 39 to the next interpolation until it is determined in step S13 that the speed suppression process is necessary (No in S13).
- the solution of the inverse kinematic problem is calculated in which the sign of the angle of the J5 axis 35 is the same as the angle of the J5 axis 35 at the work start position ( S12).
- the speeds corresponding to the calculated J4 axis 34 to J6 axis 36 are also calculated.
- the main control unit 11 controls the drive instructing unit 13 to set the angles of the J5 axis 35 as the angles of the J1 axis 31 to the J6 axis 36 for moving the end effector 39 to the next interpolation point.
- a solution in which the sign of the angle has the same sign as the angle of the J5 shaft 35 at the work start position is read from the storage unit 12 and output to the actuator of each drive shaft of the manipulator body 30. Thereby, until the end effector 39 moves to the position and posture at the work end position (Yes side in S15), the sign of the angle of the J5 axis 35 is the J5 axis at the work start position.
- the solution having the same sign as the angle of 35 is operated as a target value (No side of S15).
- Step S411 to S412 On the other hand, if it is determined in step S13 that speed suppression processing is necessary (Yes in S13), the main control unit 11 proceeds to step S411 in the same manner as in step S12, the J1 axis at the next interpolation point. The angles of 31 to J6 axis 36 and the speeds of J4 axis 34 to J6 axis 36 are calculated. In step S411, the speeds corresponding to the calculated J4 axis 34 to J6 axis 36 are also calculated.
- the main control unit 11 determines that the J5 axis 35 of the two solutions of the inverse kinematic problem obtained as the angles of the J1 axis 31 to the J6 axis 36 for moving the end effector 39 to the next interpolation point.
- a solution (another solution) of the inverse kinematic problem in which the sign of the angle is different from the sign of the J5 axis 35 at the work start position is calculated.
- the main control unit 11 determines that the angle at the next interpolation point of the J5 axis 35 is at the work start position.
- the angle at the next interpolation point of the J5 axis 35 is the J5 axis 35 at the work start position.
- step S412 the main controller 11 determines whether the speeds of the J4 axis 34 and J6 axis 36 calculated in step S412 are outside the allowable range, as in step S13, or whether the J4 axis 34, J6
- the necessity of the speed suppression process is determined, for example, whether the speed suppression process for the shaft 36 is being executed. If it is determined that the speed suppression process is necessary (Yes in S412), the process proceeds to step S413. If it is determined that the speed suppression process is not necessary (No in S412), the process is as follows. The process proceeds to step S414.
- Step S413 In step S413, as in step S20 shown in FIG. 5, a speed suppression process for suppressing the speed of one or both of the J4 axis 34 and the J6 axis 36 is executed. However, at this time, the main control unit 11 determines that the J5 axis 35 of the two solutions of the inverse kinematic problem obtained as the angles of the J1 axis 31 to the J6 axis 36 for moving the end effector 39 to the next interpolation point. Since a solution in which the sign of the angle is different from the angle of the J5 axis 35 at the work start position is adopted, the J1 axis 31 to J6 axis 36 operate using the solution as a target value. Note that the processing contents in step S413 are the same as those in step S20 except that the angles of the J4 axis 34 and the J6 axis 36 are different.
- step S414 the main control unit 11 controls the drive instructing unit 13 to move the end effector 39 to the next interpolation point.
- the angles of the J1 axis 31 to J6 axis 36 are as follows. Is read from the storage unit 12 and output to the actuator of each drive shaft of the manipulator body 30. At this time, the angle of the J1 axis 31 to the J6 axis 36 is set so that the sign of the angle of the J5 axis 35 is different from the angle of the J5 axis 35 at the work start position in the step S411 or the step S413. It is calculated as a value. Accordingly, the angle of the J5 axis 35 transits through 0 °.
- FIG. 15 shows a conventional case where the sign of the angle of the J5 axis 35 is different between the work start position and the work end position (when the angle changes from plus to minus and minus to plus), that is, when passing through 0 °.
- An example of the operation result of the J4 axis, the J5 axis, and the J6 axis in the path passing through the singular point is shown.
- the angle of the J5 axis 35 is controlled so as not to pass through 0 °, and the angle of the J5 axis 35 at the work end position is positive. It can be seen that the amount of change in angle is large.
- FIG. 16 shows an example of the result of adopting the control method of the articulated robot X according to the first embodiment when the sign of the angle of the J5 axis 35 is different between the work start position and the work end position. .
- FIG. 16 shows a change in posture data of the end effector 39 when the articulated robot X according to the first embodiment is operated at a constant speed without adopting the control method
- FIG. 6 shows changes in attitude data of the end effector 39 when the control method according to the first embodiment is employed.
- the vertical axis indicates the amount of variation with respect to the posture data of the end effector 39 at the original interpolation point
- the horizontal axis indicates time.
- the control method according to the first embodiment when the control method according to the first embodiment is not employed, the fluctuations of the tilt angle and the advance angle are large. However, as shown in FIG. 18, the control method according to the first embodiment is employed. If the weighting of the inclination angle and the advance angle (the amount of fluctuation suppression) is set large, fluctuations in the inclination angle and advance angle can be suppressed at the expense of accuracy of the rotation angle unnecessary for welding. it can.
- control is performed so as not to pass a singular point where the angle of the J5 axis 35 is near 0 ° between the work start position and the work end position.
- the angle of the J5 axis 35 at the work end position is different from a desired angle.
- the articulated robot X when the control is performed so that the angle of the J5 axis 35 passes through the vicinity of 0 ° between the work start position and the work end position.
- the sign of the angle of the J5 axis 35 at the work start position and the work end position is the same, the angle of the J5 axis 35 at the work end position is different from the desired angle.
- the wires connected to the J1 axis 31 to J6 axis 36 and the end effector 39 of the articulated robot X are entangled, and the drive limit points of the J4 axis 34 to J6 axis 36 are caused by subsequent operations.
- FIG. 19 is a flowchart for explaining the control method of the articulated robot X according to the second embodiment.
- symbol is attached
- the main control unit 11 determines whether the J5 axis 35 of the first joint drive system at the work start position and the work end position is between the Yes side of Step S13 and Steps S20 and S411. Processing for determining whether the sign of the angle is the same sign or a different sign (Yes side in step S511) is executed.
- step S511 When the sign of the angle of the J5 shaft 35 of the first joint drive system at the work start position and the work end position is the same sign (Yes side of step S511), the main control unit 11 performs the step S20. Migrate to In this case, the solution of the inverse kinematics problem is adopted in which the sign of the angle of the J5 axis 35 of the first joint drive system is the same sign as the angle at the work start position.
- Step S511 when the signs of the angles of the J5 axis 35 of the first joint drive system at the work start position and the work end position are different signs (No side of Step S511), the main control unit 11 performs the process at Step S411. To migrate. In this case, a solution to the inverse kinematics problem in which the sign of the angle of the J5 axis 35 of the first joint drive system is different from the angle at the work start position is adopted.
- the sign of the angle of the J5 axis 35 at the taught work end position coincides with the sign as taught.
- the end effector 39A is an effector that is attached to the tip of the manipulator main body 30 (the tip of the J6 shaft 36) and has two work points that give an effect to the work object.
- work points a welder (torch)
- painters, tools, traps, and sensors There are painters, tools, traps, and sensors.
- a torch having two work points is called a tandem torch.
- Step S201 the case where the speed of the J6 axis 36 is out of the allowable range or the flag indicating that the speed suppression processing of the J6 axis 36 is in progress is set (step S201; J6 axis is out of range). To do. Further, step S211 to step S214 are executed.
- the main control section 11 determines the components of the data indicating the attitude of the end effector 39A in the welding line coordinate system ⁇ line converted in the step S213 (torch inclination angle Rx, torch advance angle Ry, torch Information on the component (weighting information) whose fluctuation should be suppressed is acquired from the rotation angle Rz).
- the end factor 39A is a welding torch having two working points, and the rotation angle around the torch axis and the traveling speed of the torch are important due to the nature of the welding work using the welding torch. is there.
- the torch rotation angle Rz is set in advance as a component with a large weight (to be suppressed), and the torch advance angle Ry is set in advance as a component with a small weight (not suppressed).
- step S216 the main control unit 11 selects a specific component (torch inclination angle Rx, torch inclination angle Rx, large weight) for suppression acquired in step S215 described above from among the plurality of candidates at the calculated next interpolation point.
- a specific component such as torch inclination angle Rx, torch inclination angle Rx, large weight
- the main control unit 11 performs the processing after step S217.
- steps S221 to S226 or steps S231 to S236 are executed instead of steps S211 to S216 corresponding to each drive shaft whose speed is determined to be outside the allowable range.
- the main control unit 11 of the articulated robot X1 is the center, and singularities are avoided while suppressing fluctuations in the torch inclination angle Rx and torch rotation angle Rz that are heavily weighted for suppression.
- the position and posture of the end effector 39A provided in the manipulator body 30 are operated, the interpolation point based on the original teaching point is not greatly deviated, and the moving speed of the end effector 39A is not changed. A work trajectory whose speed falls within an allowable range can be obtained.
- the torch advance angle Ry with a small weight for suppression greatly fluctuates, but the posture variation angle of the rotation angle Rz with a large weight for suppression is changed. Can be suppressed.
- An articulated robot X1 shown in FIG. 22 includes a control unit 10, an operation unit 21, a manipulator body 30A, and the like.
- the manipulator main body 30A includes three drive shafts J4 axis 34, J5 axis 35, and J6 axis 36 of the first joint drive system, and four drive axes J1 axis 31, J2 axis 32, and J3 axis of the second joint drive system. 33 and a J7 shaft 37.
- the J7 axis 37 is composed of an electric motor or the like, similar to the J1 axis 31 to the J6 axis 36, and is rotated in the positive and negative directions according to instructions from the drive instructing unit 13, and the rotation driving of a plurality of axes is linked. , Realize human wrist and arm movement.
- the four drive shafts of the second joint drive system including the J7 shaft 37 perform an operation corresponding to a human arm and move the position of the end effector 39.
- each J1 axis 31 to J6 axis 36 as a solution of the inverse kinematic problem from the position and orientation Pi (X i , Y i , Z i , ⁇ i , ⁇ i , ⁇ i ) of the interpolation point ( ⁇ 1 , ⁇ 2 , ⁇ 3 , ⁇ 4 , ⁇ 5 , ⁇ 6 ) are calculated, and the angles ( ⁇ 1 , ⁇ 2 , ⁇ 3 , ⁇ 4 , ⁇ 5 ) of the J1 axis 31 to J6 axis 36 at the interpolation point are calculated.
- the position and orientation Pi (X i , Y i , Z i , ⁇ i , ⁇ i , ⁇ i , ⁇ i ) are calculated as a solution to the forward kinematics problem. From the posture Pi (X i , Y i , Z i , ⁇ i , ⁇ i , ⁇ i ), the angle ( ⁇ 1 , ⁇ 2 , ⁇ 3 , ⁇ ) of each J1 axis 31 to J7 axis 37 is solved as an inverse kinematic problem.
- step 122 the main control unit 11 obtains a solution to the inverse kinematics problem to thereby determine the position and orientation P i + 1 (X i + 1 , Y i + 1 , Z i + 1 ,
- the angle ( ⁇ 1 , ⁇ 2 , ⁇ 3 , ⁇ 4 , ⁇ 5 , ⁇ 6 , ⁇ 7 ) of each J1 axis 31 to J7 axis 37 is calculated from ⁇ i + 1 , ⁇ i + 1 , ⁇ i + 1 ). Then, it is stored in the storage unit 12.
- the same processing as that after step S123 is executed.
- Step S13 when it is determined that the speed suppression process is necessary (S13; YES), the speed of the J6 axis 36 is outside the allowable range or the flag indicating that the speed suppression process of the J6 axis 36 is in progress is set ( Step S201; J6 axis is out of range).
- step S211 it is assumed that a candidate for the angle of the J6 axis 36 has been calculated.
- step S212 the main controller 11 determines the angles of the other drive axes J1 axis 31 to J5 axis 35 and J7 axis 37 corresponding to the respective candidate angles of the J6 axis 36 at the calculated next interpolation point. calculate. For example, for each candidate angle ( ⁇ 6a , ⁇ 6b , ⁇ 6c ) of the J6 axis 36, the position (X i + 1 , Y) of the next interpolation point P i + 1 calculated in step S122 is calculated.
- step S213 the main control unit 11 determines each component (torch inclination angle Rx) of the data indicating the attitude of the end effector 39 of the weld line coordinate system at the next interpolation point of the system that avoids the calculated singular point.
- Torch advance angle Ry torch rotation angle Rz.
- (1 ′) [R xa , R ya , R za ] is calculated from [ ⁇ 1a , ⁇ 2a , ⁇ 3a , ⁇ 4 , ⁇ 5 , ⁇ 6a , ⁇ 7 ].
- step S214 the main controller 11 determines each component (torch of data) indicating the attitude of the end effector 39 in the weld line coordinate system at the next interpolation point P i + 1 of the system that passes through the calculated singular point. (Inclination angle Rx, torch advance angle Ry, torch rotation angle Rz) are calculated. Subsequently, the processes after step S215 are executed. As in the embodiment, steps S221 to S226 or steps S231 to S236 are executed instead of steps S211 to S216 corresponding to each drive shaft whose speed is determined to be outside the allowable range.
- the position and orientation of the end effector 39 are operated while avoiding the singularity while suppressing the fluctuation of any one of the torch inclination angle Rx, the torch advance angle Ry, and the torch rotation angle Rz, and the interpolation point based on the original teaching point is determined. It is possible to obtain a work trajectory in which the speeds of all the axes are within the allowable range without greatly deviating and without changing the moving speed of the end effector 39.
- the position and orientation at the interpolation point Pi (Xi, Yi, Zi, ⁇ i, ⁇ i, ⁇ i) angle of each axis of the first joint drive system from ( ⁇ 4, ⁇ 5, ⁇ 6 ) is obtained as a solution to the inverse kinematics problem
- the position at the interpolation point Pi and the angle of each axis of the first joint drive system (X i , Y i , Z i , ⁇ 4 , ⁇ 5 , ⁇ 6 ) Therefore, it is useful for controlling a manipulator having multiple joint axes obtained by solving the inverse kinematics problem by using the angles ( ⁇ 1 , ⁇ 2 , ⁇ 3 , ⁇ 7 ) of each axis of the second joint drive system.
- the present invention can be applied not only to the above-described articulated robot having 6-axis and 7-axis manipulators but also to an articulated robot having more axes.
- control unit 10 of the articulated robot X can be realized by using a normal computer system Y connected to the network shown in FIG. 29 without using a dedicated system.
- 29 includes, for example, a main control unit 11, a storage unit 12, an external storage unit 103, an input / output unit 104, a display unit 105, a transmission unit 106, and an internal bus 109.
- the main control unit 11 includes a CPU (Central Processing Unit), and the storage unit 12 includes a RAM (Random Access Memory) and the like.
- the main control unit 11 executes a program stored in the storage unit 12 to realize the control unit 10 that is a control device for the articulated robot X.
- a program for executing the above processing is stored and distributed on a computer-readable recording medium (flexible disk, CD-ROM, DVD-ROM, etc.), and the program is installed in the computer.
- You may comprise the control part 10 of the articulated robot X which performs the said process.
- the control unit 10 of the articulated robot X may be configured by storing the program in a storage device included in a server device on a communication network such as the Internet and downloading it by a normal computer system.
- the embodiments of the present invention have been described above, various modifications and combinations required for design reasons and other factors are described in the claimed invention and the embodiments of the invention. It should be understood that the invention falls within the scope of the invention corresponding to the specific example.
- the singularity of avoiding the singularity of the J5 axis 35 so that the speed or acceleration of one or both of the J4 axis 34 and the J6 axis 36 of the first joint drive system does not fall outside the allowable range.
- An avoidance process is in progress.
- the articulated robot may be provided with means for notifying the outside that the singularity avoidance process is being performed.
- the “single point avoidance” indicating that the process for avoiding the singular point is performed as shown in FIG. 30.
- “Medium” is displayed on the display screen M of the operation unit 21 (teaching pendant).
- the display indicating that singularity avoidance processing is being performed is not limited to “under singularity avoidance”, and any representation can be used as long as it is understood that singularity avoidance processing is being performed. Also good.
- the manipulator body 30 is in a state of avoiding the singular point (avoidance welding state) or not in the state of avoiding the singularity (normal welding state).
- the operator can grasp this during welding. Therefore, for example, when performing a quality inspection after welding a work object (material), it is possible to inspect and inspect a place welded in the avoidance welding state, and welding is performed in the avoidance welding state based on the inspection result.
- the welding conditions can be changed appropriately. Further, for example, the operator can monitor the entire movement of the manipulator body 30 in the avoidance welding state by visual observation or the like, and when the manipulator body 30 is moved again in the next welding operation or the like based on this monitoring. The entire operation of the manipulator body 30 can be reviewed.
- the contents of the processing program are displayed on the display screen M so that the processing program can be understood, or the processing program is being executed.
- (Reproduction time) and movement of the end effector 39 may be displayed on the display screen M.
- torch information for example, torch tilt angle, torch advance angle, torch rotation angle, etc.
- current torch information may be displayed on the display screen M.
- the display screen M displays that singularity avoidance processing is being performed, but instead of this, the singularity avoidance may be notified by a speaker or the like. In addition, it may be notified by light such as lighting or flashing of a lamp.
- the fact that singularity avoidance may be performed may be displayed on an external display device different from the teaching pendant 21 of the articulated robot.
- the time “under singularity avoidance” may be stored in the storage unit 12 of the control unit 10 or the like. Then, for example, a time history during singularity avoidance may be displayed on the display screen M of the operation unit 21 (teaching pendant) after the end of welding so that the operator or the like can check the time history during singularity avoidance.
- X, X1, X2 Articulated robot 10: Control unit 11: Main control unit 12: Storage unit 13: Drive instruction unit 21: Operation unit 30, 30A: Manipulator body 31: J1 axis 32: J2 axis 33: J3 axis 34: J4 axis 35: J5 axis 36: J6 axis 37: J7 axis 39, 39A: End effector
Landscapes
- Engineering & Computer Science (AREA)
- Robotics (AREA)
- Mechanical Engineering (AREA)
- Physics & Mathematics (AREA)
- Automation & Control Theory (AREA)
- General Physics & Mathematics (AREA)
- Computing Systems (AREA)
- Theoretical Computer Science (AREA)
- Human Computer Interaction (AREA)
- Manufacturing & Machinery (AREA)
- Plasma & Fusion (AREA)
- Manipulator (AREA)
- Numerical Control (AREA)
Abstract
Description
従って,本発明は上記事情に鑑みてなされたものであり,その目的とするところは,手首軸を所定の許容量を超えて急激に変化させる必要がある場合に,溶接や塗装作業に特に必要な手首先端の位置及び手首先端の移動速度と手首軸の許容速度とを維持し,かつ目的に応じた作業座標系における姿勢角度の少なくとも一つの特定成分の変動を抑制するようにした多関節型ロボットの制御装置,制御方法及び制御プログラムを提供することにある。
(11)前記多関節型ロボットのベース座標系において予め教示された前記作業部の作業開始位置及びその時の姿勢と作業終了位置及びその時の姿勢とを結ぶ教示経路を補間する複数の補間点における前記作業部の位置及びその時の姿勢を示すデータを算出する補間データ算出手段。
(12)前記補間データ算出手段により算出されたデータから逆運動学問題の解を求めることにより,前記補間点における前記作業部の位置及びその時の姿勢を示す前記第1関節駆動系及び前記第2関節駆動系を含む全ての駆動軸の角度を算出する角度算出手段。
(13)前記角度算出手段により算出された次の前記補間点と現在の前記補間点とにおける前記第1関節駆動系の前記各駆動軸の角度の差分に基づいて次の前記補間点に前記作業部の姿勢を動かすときの前記第1関節駆動系における両端の2つの駆動軸の速度を算出する速度算出手段。
(14)前記補間データ算出手段により算出されたデータを,前記作業部の移動方向の軸,該軸に垂直な方向の軸,該2つの軸に垂直な軸を含む作業座標系における前記作業部の姿勢を示す姿勢データに変換する姿勢データ変換手段。
(15)前記速度算出手段により算出された前記第1関節駆動系における両端の2つの駆動軸の一方又は両方の速度が予め定められた許容範囲外である場合に,前記作業部の移動速度を変更することなく,且つ前記姿勢データ変換手段により変換された次の前記補間点における前記作業部の姿勢データに対して一つ又は二つの特定成分の変動を抑制しつつ,前記第1関節駆動系における両端の2つの駆動軸の速度が前記許容範囲内となる前記第1関節駆動系の駆動軸各々の角度を再度算出し,該算出された前記第1関節駆動系の駆動軸各々の角度と前記補間データ算出手段により算出された前記作業部の位置とに基づいて前記第2関節駆動系の駆動軸各々の角度を再度算出する再算出手段。
(16)前記速度算出手段により算出された前記第1関節駆動系における両端の2つの駆動軸の両方の速度が前記許容範囲内である場合は,前記角度算出手段により算出された前記駆動軸各々の角度に基づいて前記多関節型ロボットを駆動させ,前記速度算出手段により算出された前記第1関節駆動系における両端の2つの駆動軸の一方又は両方の速度が前記許容範囲外である場合は,前記再算出手段により算出された前記駆動軸各々の角度に基づいて前記多関節型ロボットを駆動させる駆動指示手段。
ところで,前記第1関節駆動系の駆動軸の角度を±180°で表したとき,前記角度算出手段は,例えば前記第1関節駆動系の中央に位置する駆動軸の次の前記補間点における角度が前記作業開始位置における該駆動軸の角度と同符号になる逆運動学問題の解を算出する。これにより,前記第1関節駆動系の中央に位置する駆動軸の角度が0°を通過しないように制御して特異点を回避することができる。
さらに,前記駆動指示手段が,前記速度許容判別手段により前記許容範囲外の駆動軸があると判別された後は,前記再算出手段により算出された前記駆動軸各々の角度に基づいて前記多関節型ロボットを駆動させ,その後,前記角度算出手段により算出された前記第1関節駆動系の角度と前記再算出手段により算出された前記第1関節駆動系の角度との差分が所定値以下となったことを条件に,前記再算出手段により算出された前記駆動軸各々の角度に代えて前記角度算出手段により算出される前記駆動軸各々の角度に基づいて前記多関節型ロボットを駆動させるものであることが考えられる。
なお,本発明は前記多関節型ロボットの制御装置で実行される各工程を実行する多関節型ロボットの制御方法の発明,或いは,その各工程をコンピュータに実行させる多関節型ロボットの制御プログラムの発明として捉えることができる。
まず,図1に示される概略構成図を参照しつつ,本発明の実施形態に係る多関節型ロボットXの構成について説明する。
また,1つの作業点を備えるトーチをシングルトーチという。
操作部21は,ユーザにより操作されるシートキーや操作ボタン,操作レバー等により構成され,ユーザによる操作入力を受け付ける入力インターフェースである。例えば,操作部21は,前記エンドエフェクタ39の作業開始位置及び姿勢と作業終了位置及び姿勢とこれを結ぶ経路との作業経路や作業時間,目的に応じた溶接線座標系Σline(作業座標系)におけるエンドエフェクタ39の姿勢角度の各成分の変動を抑制するための重み付け情報,これらの内容が記載されたプログラムなどの入力を受け付けて制御部10に出力する。即ち,操作部21は,エンドエフェクタ39の作業開始位置及び姿勢,作業終了位置及び姿勢を教示するための手段であり,前記重み付け情報を設定するための手段である。
次に,図2~図6のフローチャートを参照して,多関節型ロボットXの主制御部11が中心となって,マニピュレータ本体30を作動させる処理手順の一例について説明する。尚,以下に示すS11,S12,…は,処理手順(ステップ)の識別符号を表す。
一般的なマニピュレータ本体30の制御は,後述するように主制御部11に作動軌跡を教示する教示工程(ステップS11)を実施し,更に教示された作動軌跡上の補間点の位置・姿勢に作動させるために各駆動軸の角度を算出・記憶する工程(ステップS12)と,算出された角度を読み出して作動を指示する工程(ステップS14)と,作業終了位置・姿勢に到達したか否かを判別する工程(ステップS15)とを実施して,マニピュレータ本体30を現在の補間点の位置・姿勢から次の補間点の位置・姿勢に作動させ,以後,所定のサンプリング期間毎にステップS12,S14,S15を繰り返すことにより作動軌跡に沿って作業開始位置・姿勢から作業終了位置・姿勢までマニピュレータ本体30を作動させる。また,主制御部11は,マニピュレータ本体30のエンドエフェクタ39を予め設定された一定速度で移動させることにより,該エンドエフェクタ39による作業を実行する。但し,一定速度で移動させる場合には,第1関節駆動系のJ5軸35が0°近傍を通過するとき(特異点)に,J4軸34やJ6軸36の速度が予め定められた許容範囲を超えるおそれがある。
最初に,利用者が,ロボットの作動軌跡を,操作部21によりポイントツーポイントで教示する。即ち,操作部21によって各教示ポイントに移動させ,前記エンドエフェクタ39の作業位置及び姿勢を制御部10の記憶部12に記憶させる(ステップS11)。尚,ステップS11が教示工程の一例である。
次に,図3を参照して,図2のステップS12に該当する多関節型ロボットXの現在の補間点から次の補間点に移動するためのJ1軸31~J6軸36の角度と第1関節駆動系のJ4軸34~J6軸36の速度とを算出する処理手順の一例について説明する。
続いて,主制御部11は,次の補間点Pi+1の位置及び姿勢(Xi+1,Yi+1,Zi+1,αi+1,βi+1,i+1 )から逆運動学問題の解を求めることによって,各J1軸31~J6軸36の角度(θ1,θ2,θ3,θ4,θ5,θ6)を算出して,記憶部12に記憶させる(ステップS122)。このとき,J4軸34~J6軸36について逆運動学問題の2つの解が得られるが,ここでは,主制御部11は,その2つの解のうち第1関節駆動系のJ5軸35の角度が,教示された前記エンドエフェクタ39の作業開始位置におけるJ5軸35の角度と同符号となる逆運動学問題の解を採用する。これにより,J5軸35の角度が0°を通過することなく推移するため特異点の通過を回避することができる。
次に,主制御部11は,算出された次の補間点Pi+1 の各J4軸34~J6軸36の角度から,記憶部12に記憶された現在の補間点Piの各J4軸34~J6軸36の角度を差し引き,第1関節駆動系のJ4軸34~J6軸36の速度を算出して記憶部12に記憶させる(ステップS123)。尚,後述するように,速度抑制処理(ステップS20)を実行する場合には,特異点を経由する系の補間点の第1関節駆動系のJ4軸34~J6軸36の速度を算出し記憶部12に記憶させ,後述するステップS131において速度の許容範囲を算出したり,ステップS211等において次の補間点の候補となる速度(必要に応じて加速度)を算出するために用いる。
次に,図4を参照して,図2のステップS13に該当する速度抑制必要性判別処理手順の一例について説明する。
まず,主制御部11はエンドエフェクタ39の位置及び姿勢を現在の補間点Piから次の補間点Pi+1 に動かす際の第1関節駆動系のJ4軸34,J6軸36の速度の許容範囲を算出する(ステップS131)。例えば,現在の補間点Piの速度の値に所定の値や所定の割合を加減算して得られる値を上限・下限として,速度の許容範囲を算出する。尚,この各軸の速度の許容範囲は,エンドエフェクタ39の作動条件(例えば,溶接する際の温度,塗装する際の塗装膜の厚さなどの条件)や多関節型ロボットが動作する際に振動などの異常動作を起こさないための条件など,作業対象物の品質に影響を及ぼさない範囲内で予め定めておいても良い。さらに,J4軸34,J6軸36ごとに個別の許容範囲を設定しても良い。
ここで,第1関節駆動系の次の補間点におけるJ4軸34及びJ6軸36の両方が前記許容範囲内であり,且つ速度抑制処理中でない場合には(ステップS132;NO),主制御部11の指示により駆動指示部13は記憶部12から前記ステップS12で算出された各J1軸31~J6軸36の角度を読み出しマニピュレータ本体30に出力する(ステップS14)。他方,許容範囲外の軸がある,又は速度抑制処理中である場合には(ステップS132;YES),主制御部11は以下に述べる速度抑制処理(ステップS20)を実行する。
尚,以下では,図24に示したJ5軸35が0度付近にあり,J4軸34やJ6軸36の速度が許容範囲を超えるときに当該速度抑制処理を実行する場合を例に挙げて説明するが,他の理由によってJ4軸34やJ6軸36の速度が許容範囲を超える場合にも同様である。従って,当該速度抑制処理は,第1関節駆動系のJ4軸34,J6軸36の何れか1つ又は2つの駆動軸の速度が許容範囲を超える場合に実施される。尚,最初に次の補間点について許容範囲外の軸が発見された時に,現在の補間点での速度は許容範囲内である。
(J6軸36の速度抑制処理ステップS211~S217)
J6軸36の速度が許容範囲外であるか又は,J6軸36の速度抑制処理中を示すフラグが立っている場合(ステップS201;J6軸が範囲外),主制御部11は,現在の補間点におけるJ6軸36の角度に基づいて,該J6軸36の角度が前記ステップS12で算出された次の補間点における角度に追従し,且つJ6軸36の次の補間点における速度が前記許容範囲内となる角度の候補を複数算出する(ステップS211)。
(21)まず,下記(21a)式により抑制基準速度V6b を算出する。ここに,θnow は次の補間点におけるJ6軸36の角度,θold は現在の補間点におけるJ6軸36の角度,Eは予め設定された係数(例えば0.1)である。
V6b =E(θnow -θold ) …(21a)
(22)次に,抑制基準速度V6b が予め定められた速度リミッタVmax 以上であるか否かを判断し,該速度リミッタVmax 以上であると判断した場合は,抑制基準速度V6b を前回の速度θold に設定する。前記速度リミッタVmax は,例えば前記許容範囲内の速度の最大値である。
(23)そして,抑制基準速度V6b と所定の値D(事前に定められた許容範囲内の加速度を保っていることが分かっている速度の変化分の絶対値など)とにより3つの速度の候補(Vθ6a =V6b -D,Vθ6b =V6b ,Vθ6c =V6b +D)を算出する。前記所定の値Dは,例えば前記速度リミッタVmax の10%程度の値である。また,Vθ6a が前記速度リミッタVmax より大きい場合は,Vθ6a =Vmax とする。
(1)候補θ6a に対して,[Xi+1 ,Yi+1 ,Zi+1 ,θ4,θ5,θ6a ]から[θ1a ,θ2a,θ3a ,θ4,θ5,θ6a ]を算出する。
(2)候補θ6b に対して,[Xi+1 ,Yi+1 ,Zi+1 ,θ4,θ5,θ6b ]から[θ1b ,θ2b,θ3b ,θ4,θ5,θ6b ]を算出する。
(3)候補θ6c に対して,[Xi+1 ,Yi+1 ,Zi+1 ,θ4,θ5,θ6c ]から[θ1c ,θ2c,θ3c ,θ4,θ5,θ6c ]を算出する。
このようにしてJ6軸36の速度を許容範囲内に維持した次の補間点における3つの角度の候補(θ6a ,θ6b ,θ6c )が得られると,次の補間点としてどの候補を選択すべきかの判断が行われる。ここまでの計算では,第1関節駆動系のJ4軸34~J6軸36の角度(θ4,θ5,θ6)が問題とされているが,最終的には作業の種類に応じて重要視されるエンドエフェクタ39の姿勢に焦点を合わせて候補が選択されなければならない。従って,上記J1軸31~J6軸36の角度からエンドエフェクタ39の姿勢を示すデータへの変換が必要である。
例えば,上記の(1)~(3)の候補θ6a ~θ6c に対して,以下の(1’)~(3’)によりエンドエフェクタ39の姿勢を示すデータの各成分を算出する。
(1’)[θ1a ,θ2a ,θ3a ,θ4,θ5,θ6a ]から[Rxa ,Rya ,Rza ]を算出する。
(2’)[θ1b ,θ2b ,θ3b ,θ4,θ5,θ6b ]から[Rxb ,Ryb ,Rzb ]を算出する。
(3’)[θ1c ,θ2c ,θ3c ,θ4,θ5,θ6c ]から[Rxc ,Ryc ,Rzc ]を算出する。
(1”)[Fa=A(Rx-Rxa )2+B(Ry-Rya )2+C(Rz-Rza )2]
(2”)[Fb=A(Rx-Rxb )2+B(Ry-Ryb )2+C(Rz-Rzb )2]
(3”)[Fc=A(Rx-Rxc )2+B(Ry-Ryc )2+C(Rz-Rzc )2]
上記(1”)~(3”)において,変動を抑制する成分に重み付けをして,前記ステップS214で算出された元の経路上の次の補間点のエンドエフェクタ39の姿勢角と,前記ステップS213で算出された例外経路上の次の補間点の各候補のエンドエフェクタ39の姿勢角との差Fa~Fcが求められるので,そのFa~Fcの中で最も値が小さい(変動が小さい)ものを選択し,選択された候補の駆動軸J1軸31~J6軸36の角度を特異点を回避する動作時の各軸の角度として採用する。ここに,係るステップS216の処理が角度選択工程の一例であり,係る処理を実行するときの主制御部11が角度選択手段に相当する。また,選択された候補の駆動軸J1軸31~J6軸36の角度とJ6軸36の速度を記憶部12に保存する。これにより,エンドエフェクタ39の姿勢データのうち予め定められた重み付けが大きい成分から順に変動の抑制量が大きくなるように駆動軸J1軸31~J6軸36の角度が再度算出される。例えば,重み付け係数A=0.2,B=0,C=1である場合,トーチ回転角Rzの変動が最も抑制され,トーチ前進角Ryの変動が最も大きくなる。
尚,ステップS211~S213,S216,S217が,再算出工程の一例であって,係る処理を実行するときの主制御部11が再算出手段に相当する。
上述した処理(ステップS211~S217)を実行することによって,図8(A),図8(B)に示すように,現在の補間点の位置Piから特異点を経由する元の経路上の次の補間点Pi+1 ではなく,特異点を回避する例外経路上の次の補間点P’i+1 が見つかる。
その後に主制御部11が実行するJ6軸36の速度抑制処理からの復帰処理(ステップS310)の詳細については後述する。
J4軸34の速度が許容範囲外であるか又は,J4軸34の速度抑制処理中を示すフラグが立っている場合(ステップS201;J4軸が範囲外),主制御部11は,前述したJ6軸36の速度と角度の候補を算出する場合と同様に,許容範囲内に収まる複数のJ4軸34の角度の候補を算出する(ステップS221)。即ち,J4軸34の抑制基準速度V4b が算出されて,3つの速度の候補(Vθ4a =V4b -G,Vθ4b =V4b ,Vθ4c =V4b+G)が算出され,3つの角度の候補(θ4a ,θ4b ,θ4c )が算出される。なお,所定の値Gは,事前に定められた許容範囲内の加速度を保っていることが分かっている速度の変化分の絶対値などである。ここに,係るステップS221の処理が角度候補算出工程の一例である。
(J4軸34・J6軸36の速度抑制処理ステップS231~S237)
J4軸34及びJ6軸36両方の速度が許容範囲外であるか又は,J4軸及びJ6軸36両方の速度抑制処理中を示すフラグが立っている場合(ステップS201;J4軸・J6軸が範囲外),主制御部11は許容範囲内に収まる1又は複数のJ4軸34とJ6軸36との両方の速度と角度の候補を算出する(ステップS231)。
(1)候補(θ4a ,θ6a )に対して,[Xi+1,Yi+1 ,Zi+1 ,θ4a ,θ5,θ6a ]から[θ1aa ,θ2aa ,θ3aa ,θ4a ,θ5,θ6a ]を算出する。
(2)候補(θ4a ,θ6b )に対して,[Xi+1,Yi+1 ,Zi+1 ,θ4a ,θ5,θ6b ]から[θ1ab ,θ2ab ,θ3ab ,θ4a ,θ5,θ6b ]を算出する。
(3)候補(θ4a ,θ6c )に対して,[Xi+1,Yi+1 ,Zi+1 ,θ4a ,θ5,θ6c ]から[θ1ac ,θ2ac ,θ3ac ,θ4a ,θ5,θ6c ]を算出する。
(4)候補(θ4b ,θ6a )に対して,[Xi+1,Yi+1 ,Zi+1 ,θ4b ,θ5,θ6a ]から[θ1ba ,θ2ba ,θ3ba ,θ4b ,θ5,θ6a ]を算出する。
(5)候補(θ4b ,θ6b )に対して,[Xi+1 ,Yi+1 ,Zi+1 ,θ4b ,θ5,θ-6b]から[θ1bb ,θ2bb ,θ3bb ,θ4b ,θ5,θ6b ]を算出する。
(6)候補(θ4b ,θ6c )に対して,[Xi+1,Yi+1 ,Zi+1 ,θ4b ,θ5,θ6c ]から[θ1bc ,θ2bc ,θ3bc ,θ4b ,θ5,θ6c ]を算出する。
(7)候補(θ4c ,θ6a )に対して,[Xi+1,Yi+1 ,Zi+1 ,θ4c ,θ5,θ6a ]から[θ1ca ,θ2ca ,θ3ca ,θ4c ,θ5,θ6a ]を算出する。
(8)候補(θ4c ,θ6b )に対して,[Xi+1,Yi+1 ,Zi+1 ,θ4c ,θ5,θ6b ]から[θ1cb ,θ2cb ,θ3cb ,θ4c ,θ5,θ6b ]を算出する。
(9)候補(θ4c ,θ6c )に対して,[Xi+1,Yi+1 ,Zi+1 ,θ4c ,θ5,θ6c ]から[θ1cc ,θ2cc ,θ3cc ,θ4c ,θ5,θ6c ]を算出する。
次に,エンドエフェクタ39の姿勢に焦点を合わせて候補を選択するために,J1軸31~J6軸36の角度の候補をエンドエフェクタ39の姿勢を示すデータへ変換する。
例えば,上記の(1)~(9)の角度の候補に対して,以下の(1’)~(9’)によりエンドエフェクタ39の姿勢を示すデータの各成分を算出する。
(1’)[θ1aa ,θ2aa ,θ3aa ,θ4a ,θ5,θ6a ]から[Rxaa ,Ryaa ,Rzaa]を算出する。
(2’)[θ1ab ,θ2ab ,θ3ab ,θ4a ,θ5,θ6b ]から[Rxab ,Ryab ,Rzab]を算出する。
(3’)[θ1ac ,θ2ac ,θ3ac ,θ4a ,θ5,θ6c ]から[Rxac ,Ryac ,Rzac]を算出する。
(4’)[θ1ba ,θ2ba ,θ3ba ,θ4b ,θ5,θ6a ]から[Rxba ,Ryba ,Rzba]を算出する。
(5’)[θ1bb ,θ2bb ,θ3bb ,θ4b ,θ5,θ6b ]から[Rxbb ,Rybb ,Rzbb]を算出する。
(6’)[θ1bc ,θ2bc ,θ3bc ,θ4b ,θ5,θ6c ]から[Rxbc ,Rybc ,Rzbc]を算出する。
(7’)[θ1ca ,θ2ca ,θ3ca ,θ4c ,θ5,θ6a ]から[Rxca ,Ryca ,Rzca]を算出する。
(8’)[θ1cb ,θ2cb ,θ3cb ,θ4c ,θ5,θ6b ]から[Rxcb ,Rycb ,Rzcb]を算出する。
(9’)[θ1cc ,θ2cc ,θ3cc ,θ4c ,θ5,θ6c ]から[Rxcc ,Rycc ,Rzcc]を算出する。
さらに,主制御部11は算出された次の補間点における複数の候補の中から前述したステップS235により取得された抑制するための重み付けが大きい特定成分(トーチ傾斜角Rx,トーチ前進角Ry,トーチ回転角Rzのいずれか)の変動が最も少ない候補を選択する(ステップS236)。例えば,トーチ傾斜角と,トーチ前進との変動を抑制し,トーチ回転角の変動を許容する場合,重み付け係数A=1,B=1,C=0とし,上記の(1’)~(9’)の候補に対して下記の(1”)~(9”)の演算する。
(1”)[Faa =A(Rx-Rxaa)2+B(Ry-Ryaa )2+C(Rz-Rzaa )2 ]
(2”)[Fab =A(Rx-Rxab)2+B(Ry-Ryab )2+C(Rz-Rzab )2 ]
(3”)[Fac =A(Rx-Rxac)2+B(Ry-Ryac )2+C(Rz-Rzac )2 ]
(4”)[Fba =A(Rx-Rxba)2+B(Ry-Ryba )2+C(Rz-Rzba )2 ]
(5”)[Fbb =A(Rx-Rxbb)2+B(Ry-Rybb )2+C(Rz-Rzbb )2 ]
(6”)[Fbc =A(Rx-Rxbc)2+B(Ry-Rybc )2+C(Rz-Rzbc )2 ]
(7”)[Fca =A(Rx-Rxca)2+B(Ry-Ryca )2+C(Rz-Rzca )2 ]
(8”)[Fcb =A(Rx-Rxcb)2+B(Ry-Rycb )2+C(Rz-Rzcb )2 ]
(9”)[Fcc =A(Rx-Rxcc)2+B(Ry-Rycc )2+C(Rz-Rzcc )2 ]
上記(1”)~(9”)において,変動を抑制する成分に重み付けをして,前記ステップS234で算出された元の経路の次の補間点のエンドエフェクタ39の姿勢角と,前記ステップS233で算出された例外経路の次の補間点の各候補のエンドエフェクタ39の姿勢角との差Faa ~Fcc が求められるので,その差Faa ~Fcc の中で最も値が小さい(変動が小さい)ものを選択し,選択された候補の駆動軸J1軸31~J6軸36の角度を特異点を回避する動作時の各軸の角度とする。ここに,係るステップS236の処理が角度選択工程の一例であり,係る処理を実行するときの主制御部11が角度選択手段に相当する。また,選択された候補の駆動軸J1軸31~J6軸36の角度とJ4軸34・J6軸36の速度を記憶部12に保存する。
上述した処理(ステップS231~S237)をすることによって,現在の補間点の位置Piから特異点を経由する元の経路上の次の補間点Pi+1 ではなく,特異点を回避する例外経路上の次の補間点P’i+1 が見つかる。
次に,図6を参照して,図5のステップS310に該当する多関節型ロボットXの速度抑制処理からの復帰処理の手順の一例について説明する。
主制御部11は,図8(B)に示されるように特異点区間Pi~Pi+1 を回避した例外経路上の次の補間点P’i+1 から,特異点を経由する元の経路上の更に次の補間点Pi+2 と特異点を回避した例外経路上の更に次の補間点P’i+2 との角度の差分を計算して,その差が所定の範囲を超えていない場合にはPi+3 へ進んでこの処理を終了するが,超えている場合にはP’i+3 へ進み,差分が所定の範囲を超えている限りこの処理を繰り返す。やがて差分が所定の範囲内に入る次の補間点が見つかった場合に(この例では,P’i+4 ~Pi+5 の場合)は,補間点P’i+4 から補間点Pi+5 へ進みこの処理を終了する。そこで,特異点を回避する経路上の現在の補間点P’i+4 から特異点を経由する経路上の次の補間点Pi+5 に復帰する場合を中心に説明する。
まず,主制御部11は前記ステップS122により算出された特異点を経由した系の次の補間点Pi+5 のJ6軸36の角度θ6(i+5) と,前記ステップS214により選択された特異点を回避した系の次の補間点P’i+5 のJ6軸36の角度θ’6(i+5) とを取得する(ステップS311)。
ここで,J6軸36の速度が所定の許容範囲内の場合(ステップS315;YES),主制御部11は次の補間点をP’i+5 からPi+5 に変更して,J6軸36の角度をθ6(i+5)にし,これに対応する他の全ての駆動軸J1軸31~J5軸35の角度を算出する(ステップS316)。主制御部11は算出された全ての駆動軸J1軸31~J6軸36の角度を記憶部12に記憶させる。更に,主制御部11はJ6軸36の速度抑制処理中を示すフラグを立ち下げる(ステップS317)。
(J4軸34の速度抑制処理からの復帰処理)
ステップS320にあたるJ4軸34の速度抑制処理からの復帰処理は,上述したJ6軸36の速度抑制処理からの復帰処理(ステップS311~S319)と同様の処理をするため説明を省略する。
ステップS330にあたるJ4軸34・J6軸36の速度抑制処理からの復帰処理は,それぞれJ4軸34の復帰処理とJ6軸36の復帰処理との双方を共に実施する以外はJ6軸36の速度抑制処理からの復帰処理と同様のため説明を省略する。
以上に示した制御方法により,多関節型ロボットXの主制御部11が中心となって,変動を抑制するための重み付けが大きい特定成分(トーチ傾斜角Rx,トーチ前進角Ry,トーチ回転角Rzのいずれか一つ又は二つ)の変動を抑えつつ特異点を回避してマニピュレータ本体30に備え付けられたエンドエフェクタ39の位置及び姿勢を作動させることにより,本来の教示ポイントに基づく補間点を大きく外れることがなく,且つエンドエフェクタ39の移動速度を変更せずに全ての軸の速度が許容範囲内に入る作業軌道を得ることができる。
但し,図11に示すように単に速度制限をした場合には,トーチ前進角やトーチ傾斜角の姿勢変動角が大きく変動してしまう。
また,図13(A)に示すように,J1軸31が上下方向に駆動し,J2軸32とJ3軸33との駆動する方向が同じ方向でもよい,更に図13(B)に示すようにJ1軸31,J2軸32が左右方向に駆動し,J3軸33が上下方向に駆動するものでもよい。尚,駆動軸が上下方向に駆動する場合には,各駆動軸を上下させるためのモータが回転する角度の差分のみならず,各駆動軸が上下する移動距離の差分を元に速度を算出してもよい。
前記実施の形態で説明した手法により多関節型ロボットXを制御すると,前記J5軸35の角度の符号が作業開始位置及び作業終了位置で異符号になる場合でも,該J5軸35の角度が0°を通過しないように制御されるため,該作業終了位置における該J5軸35の角度が本来の目標値と異なる。この場合には,例えば図15に示すように,残りのJ4軸34やJ6軸36の角度が急激に変化するおそれや動作限界角度(例えば±180°)を超えるおそれがある。
まず,本実施例1に係る構成でも,前記ステップS13で速度抑制処理が必要であると判断されるまでの間は(S13のNo側),主制御部11が,エンドエフェクタ39を次の補間点に動かすためのJ1軸31~J6軸36の角度として,J5軸35の角度の符号が前記作業開始位置における該J5軸35の角度と同符号となる逆運動学問題の解を算出する(S12)。なお,ステップS12では,その算出されたJ4軸34~J6軸36に対応する速度も算出される。
一方,前記ステップS13で速度抑制処理が必要であると判断された場合(S13のYes側),主制御部11は,続くステップS411において,前記ステップS12と同様に,次の補間点におけるJ1軸31~J6軸36の角度とJ4軸34~J6軸36の速度を算出する。なお,ステップS411では,その算出されたJ4軸34~J6軸36に対応する速度も算出される。
ここで,速度抑制処理が必要であると判断されると(S412のYes側),処理はステップS413に移行し,速度抑制処理が必要でないと判断されると(S412のNo側),処理はステップS414に移行する。
前記ステップS413では,図5に示した前記ステップS20と同様に,J4軸34及びJ6軸36のいずれか一方又は両方の速度を抑制するための速度抑制処理が実行される。但し,このとき,主制御部11は,エンドエフェクタ39を次の補間点に動かすためのJ1軸31~J6軸36の角度として求められる逆運動学問題の2つの解のうち,J5軸35の角度の符号が前記作業開始位置における該J5軸35の角度と異符号となる解を採用するため,J1軸31~J6軸36はその解を目標値として動作することになる。なお,前記ステップS413における処理内容は,J4軸34やJ6軸36の角度が異なるだけであり,その他は前記ステップS20と同様であるためその説明を省略する。
そして,ステップS414では,前記ステップS14と同様に,主制御部11が,駆動指示部13を制御することにより,エンドエフェクタ39を次の補間点に動かすためのJ1軸31~J6軸36の角度が記憶部12から読み出されて,マニピュレータ本体30の各駆動軸のアクチュエータに出力される。このとき,J1軸31~J6軸36の角度は,前記ステップS411又は前記ステップS413において,J5軸35の角度の符号が前記作業開始位置における該J5軸35の角度と異符号となる解を目標値として算出されたものである。従って,J5軸35の角度は0°を通過して遷移することとなる。
ここに,図15は,J5軸35の角度の符号が作業開始位置及び作業終了位置で異なる場合(プラスからマイナス,マイナスからプラスに変化する場合),即ち0°を通過する場合に,従来の特異点を経由する経路のJ4軸J5軸J6軸の動作結果の一例を示している。図15に示す例では,J5軸35の角度が0°を通過しないように制御され,作業終了位置におけるJ5軸35の角度がプラスとなっているため,他のJ4軸34やJ6軸36の角度の変化量が大きくなっていることがわかる。
さらに,図17は,本実施例1に係る多関節型ロボットXの制御手法を採用せずに等速動作させた場合のエンドエフェクタ39の姿勢データの変化を示しており,図18は,本実施例1に係る制御手法を採用した場合のエンドエフェクタ39の姿勢データの変化を示している。なお,図17,図18において縦軸は本来の補間点におけるエンドエフェクタ39の姿勢データに対する変動量,横軸は時間を示している。
そこで,前記作業開始位置及び前記作業終了位置における前記第1関節駆動系のJ5軸35の符号が同符号であるか異符号であるかに応じて,前記実施の形態に係る制御手法と前記実施例1に係る制御手法とを切り替えて実行することが考えられる。
具体的に,主制御部11は,前記ステップS13のYes側と前記ステップS20及び前記ステップS411との間において,前記作業開始位置及び前記作業終了位置における前記第1関節駆動系のJ5軸35の角度の符号が同符号であるか異符号であるかを判断するための処理(ステップS511のYes側)を実行する。
多関節型ロボットX1は,エンドエフェクタ39Aを備え,エンドエフェクタ39以外の他の構成は多関節型ロボットXと同様である。また,前記多関節型ロボットX1の制御は,前記多関節型ロボットXの制御処理とは,図5に示したステップS215,S216(ステップS225,S226及びS235,236を含む)の処理以外は同様の処理をする。尚以下の説明では,多関節型ロボットX1と多関節型ロボットXとの同一の構成及び同一の処理は説明を省略する。
エンドエフェクタ39Aは,マニピュレータ本体30の先端(J6軸36の先)に取り付けられ,作業対象物に対して効果を与える2つの作業点を有する効果器であり,作業点として溶接器(トーチ),塗装器,工具,捕獲器,センサなどがある。尚,2つの作業点を備えるトーチをタンデムトーチという。
まず,主制御部11は,ステップS201を実行する。ここでは,J6軸36の速度が許容範囲外であるか又は,J6軸36の速度抑制処理中を示すフラグが立っている場合(ステップS201;J6軸が範囲外)に該当しているものとする。更にステップS211~ステップS214を実行する。
(1”)[Fa=A(Rx-Rxa )2+B(Ry-Rya )2+C(Rz-Rza )2]
(2”)[Fb=A(Rx-Rxb )2+B(Ry-Ryb )2+C(Rz-Rzb )2]
(3”)[Fc=A(Rx-Rxc )2+B(Ry-Ryc )2+C(Rz-Rzc )2]
(1”)~(3”)において,特異点を経由する経路上の次の補間点のエンドエフェクタ39Aの姿勢角と,特異点を回避する経路上の次の補間点の各候補のエンドエフェクタ39Aの姿勢角との差が求められるので,Fa~Fcの中で最も値が小さい(変動が小さい)ものを選択し,選択された候補の駆動軸J1軸31~J6軸36の角度を特異点を回避する動作時の各軸の角度とする。
以上に示した制御方法により,多関節型ロボットX1の主制御部11が中心となって,抑制するための重み付けが大きいトーチ傾斜角Rx,トーチ回転角Rzの変動を抑えつつ特異点を回避してマニピュレータ本体30に備え付けられたエンドエフェクタ39Aの位置及び姿勢を作動させ,本来の教示ポイントに基づく補間点を大きく外れることがなく,且つエンドエフェクタ39Aの移動速度を変更せずに全ての軸の速度が許容範囲内に入る作業軌道を得ることができる。
まず,図1に示した多関節型ロボットXと図22に示される多関節型ロボットX2との構成が異なる部分について説明する。
J7軸37は,J1軸31~J6軸36と同様に,電気モータ等により構成され,駆動指示部13からの指示により正負の方向に回転駆動し,複数の軸の回転駆動が連関することによって,人間の手首や腕の動きを実現する。その中でも上記J7軸37を含む第2関節駆動系の4つの駆動軸は,人間の腕に相当する動作を行い,エンドエフェクタ39の位置を移動する。
実施形態においては,補間点の位置及び姿勢Pi(Xi,Yi,Zi,αi,βi,γi)から逆運動学問題の解として各J1軸31~J6軸36の角度(θ1,θ2,θ3,θ4,θ5,θ6)を算出し,補間点における各J1軸31~J6軸36の角度(θ1,θ2,θ3,θ4,θ5,θ6)から順運動学問題の解として位置及び姿勢Pi(Xi,Yi,Zi,αi,βi,γi)を算出したが,実施例4では,補間点の位置及び姿勢Pi(Xi,Yi,Zi,αi,βi,γi)から逆運動学問題の解として各J1軸31~J7軸37の角度(θ1,θ2,θ3,θ4,θ5,θ6,θ7)を算出し,補間点における各J1軸31~J7軸37の角度(θ1,θ2,θ3,θ4,θ5,θ6,θ7)から順運動学問題の解として位置及び姿勢Pi(Xi,Yi,Zi,αi,βi,γi)を算出するように,角度の変数の数が6か7かの点が異なる。
次に,前記ステップS123以降と同様の処理を実行する。ここでは,速度抑制処理が必要であると判別され(S13;YES),J6軸36の速度が許容範囲外であるか又は,J6軸36の速度抑制処理中を示すフラグが立っている場合(ステップS201;J6軸が範囲外)に該当しているものとする。ステップS211において,J6軸36の角度の候補が算出されたものとする。
(1)候補θ6a に対して,[Xi+1 ,Yi+1 ,Zi+1 ,θ4,θ5,θ6a ]から[θ1a ,θ2a,θ3a ,θ4,θ5,θ6a ,θ7]を算出する。
(2)候補θ6b に対して,[Xi+1 ,Yi+1 ,Zi+1 ,θ4,θ5,θ6b ]から[θ1b ,θ2b,θ3b ,θ4,θ5,θ6b ,θ7]を算出する。
(3)候補θ6c に対して,[Xi+1 ,Yi+1 ,Zi+1 ,θ4,θ5,θ6c ]から[θ1c ,θ2c,θ3c ,θ4,θ5,θ6c ,θ7]を算出する。
例えば,上記の(1)~(3)の候補θ6a ~θ6c に対して,
(1’)[θ1a ,θ2a ,θ3a ,θ4,θ5,θ6a ,θ7]から[Rxa ,Rya ,Rza ]を算出する。
(2’)[θ1b ,θ2b ,θ3b ,θ4,θ5,θ6b ,θ7]から[Rxb ,Ryb ,Rzb ]を算出する。
(3’)[θ1c ,θ2c ,θ3c ,θ4,θ5,θ6c ,θ7]から[Rxc ,Ryc ,Rzc ]を算出する。
続いて,前記ステップS215以降の処理を実行する。尚,実施形態と同様に,速度が許容範囲外と判別された各駆動軸に対応して,ステップS211~S216の代わりに,ステップS221~S226若しくはステップS231~S236を実行する。
上述した実施形態では、例えば、第1関節駆動系のJ4軸34やJ6軸36の一方又は両方の速度や加速度が許容範囲外とならないように、J5軸35の特異点を回避するという特異点回避の処理を行っている。ここで、特異点回避の処理を行っていることを外部に報知する手段を多関節型ロボットに設けてもよい。
上述した例では、特異点回避の処理を行っていることを表示画面Mに表示させているが、これに代えて、特異点回避を行っていることをスピーカ等によって音で知らせてもよいし、ランプなどの点灯や点滅など光で知らせてもよい。また、特異点回避を行っていることは、多関節型ロボットの教示ペンダント21とは別の外部の表示装置に表示してもよい。
10:制御部
11:主制御部
12:記憶部
13:駆動指示部
21:操作部
30,30A:マニピュレータ本体
31:J1軸
32:J2軸
33:J3軸
34:J4軸
35:J5軸
36:J6軸
37:J7軸
39,39A:エンドエフェクタ
Claims (14)
- 作業対象物の作業線に則して移動させる作業部を先端に有し,その作業部に連結されて前記作業部の姿勢を変化させる3つの駆動軸を備えた第1関節駆動系と,前記第1関節駆動系に連結されて該第1関節駆動系の位置を変化させる少なくとも3つの駆動軸を備えた第2関節駆動系とを有する多関節型ロボットを制御する多関節型ロボットの制御装置であって,
前記多関節型ロボットのベース座標系において予め教示された前記作業部の作業開始位置及びその時の姿勢と作業終了位置及びその時の姿勢とを結ぶ教示経路を補間する複数の補間点における前記作業部の位置及びその時の姿勢を示すデータを算出する補間データ算出手段と,
前記補間データ算出手段により算出されたデータから逆運動学問題の解を求めることにより,前記補間点における前記作業部の位置及びその時の姿勢を示す前記第1関節駆動系及び前記第2関節駆動系を含む全ての駆動軸の角度を算出する角度算出手段と,
前記角度算出手段により算出された次の前記補間点と現在の前記補間点とにおける前記第1関節駆動系の前記各駆動軸の角度の差分に基づいて次の前記補間点に前記作業部の姿勢を動かすときの前記第1関節駆動系における両端の2つの駆動軸の速度を算出する速度算出手段と,
前記補間データ算出手段により算出されたデータを,前記作業部の移動方向の軸,該軸に垂直な方向の軸,該2つの軸に垂直な軸を含む作業座標系における前記作業部の姿勢を示す姿勢データに変換する姿勢データ変換手段と,
前記速度算出手段により算出された前記第1関節駆動系における両端の2つの駆動軸の一方又は両方の速度が予め定められた許容範囲外である場合に,前記作業部の移動速度を変更することなく,且つ前記姿勢データ変換手段により変換された次の前記補間点における前記作業部の姿勢データに対して一つ又は二つの特定成分の変動を抑制しつつ前記第1関節駆動系における両端の2つの駆動軸の速度が前記許容範囲内となる前記第1関節駆動系の駆動軸各々の角度を再度算出し,該算出された前記第1関節駆動系の駆動軸各々の角度と前記補間データ算出手段により算出された前記作業部の位置とに基づいて前記第2関節駆動系の駆動軸各々の角度を再度算出する再算出手段と,
前記速度算出手段により算出された前記第1関節駆動系における両端の2つの駆動軸の両方の速度が前記許容範囲内である場合は,前記角度算出手段により算出された前記駆動軸各々の角度に基づいて前記多関節型ロボットを駆動させ,前記速度算出手段により算出された前記第1関節駆動系における両端の2つの駆動軸の一方又は両方の速度が前記許容範囲外である場合は,前記再算出手段により算出された前記駆動軸各々の角度に基づいて前記多関節型ロボットを駆動させる駆動指示手段と,
を備えてなることを特徴とする多関節型ロボットの制御装置。 - 前記再算出手段が,
現在の前記補間点における前記第1関節駆動系の駆動軸各々の角度に基づいて,該駆動軸各々の角度が前記角度算出手段で算出された次の前記補間点における角度を参照に追従し,且つ前記第1関節駆動系における両端の2つの駆動軸の次の前記補間点における速度が前記許容範囲内となる該駆動軸各々の角度の候補を複数算出する角度候補算出手段と,
前記角度候補算出手段により算出された角度の候補各々を採用したときの次の前記補間点の前記作業部の姿勢を前記作業座標系で示す姿勢データをそれぞれ算出する姿勢データ算出手段と,
前記姿勢データ算出手段により算出された複数の姿勢データのうち,前記姿勢データ変換手段により変換された次の前記補間点における前記作業部の姿勢データに対して前記特定成分の変動が最も抑制された姿勢データを選択し,該姿勢データに対応する角度を前記駆動軸各々の角度として選択する角度選択手段と,
を含んでなる請求項1に記載の多関節型ロボットの制御装置。 - 前記第1関節駆動系の駆動軸の角度を±180°で表したとき,
前記角度算出手段が,前記第1関節駆動系の中央に位置する駆動軸の次の前記補間点における角度が前記作業開始位置における該駆動軸の角度と同符号になる逆運動学問題の解を算出するものである請求項2に記載の多関節型ロボットの制御装置。 - 前記第1関節駆動系の駆動軸の角度を±180°で表したとき,
前記角度算出手段が,前記速度算出手段により算出された前記第1関節駆動系における両端の2つの駆動軸の両方の速度が前記許容範囲内である場合は,前記第1関節駆動系の中央に位置する駆動軸の次の前記補間点における角度が前記作業開始位置における該駆動軸の角度と同符号になる逆運動学問題の解を算出し,
前記速度算出手段により算出された前記第1関節駆動系における両端の2つの駆動軸の一方又は両方の速度が前記許容範囲外である場合は,前記第1関節駆動系の中央に位置する駆動軸の次の前記補間点における角度が前記作業開始位置における該駆動軸の角度と異符号になる逆運動学問題の解を算出するものである請求項2に記載の多関節型ロボットの制御装置。 - 前記第1関節駆動系の駆動軸の角度を±180°で表したとき,
前記角度算出手段が,
前記作業開始位置及び前記作業終了位置における前記第1関節駆動系の中央の駆動軸の角度が同符号である場合は,前記第1関節駆動系の中央に位置する駆動軸の次の前記補間点における角度が前記作業開始位置における該駆動軸の角度と同符号になる逆運動学問題の解を算出し,
前記作業開始位置及び前記作業終了位置における前記第1関節駆動系の中央の駆動軸の角度が異符号である場合において,前記速度算出手段により算出された前記第1関節駆動系における両端の2つの駆動軸の両方の速度が前記許容範囲内である場合は,前記第1関節駆動系の中央に位置する駆動軸の次の前記補間点における角度が前記作業開始位置における該駆動軸の角度と同符号になる逆運動学問題の解を算出し,前記速度算出手段により算出された前記第1関節駆動系における両端の2つの駆動軸の一方又は両方の速度が前記許容範囲外である場合は,前記第1関節駆動系の中央に位置する駆動軸の次の前記補間点における角度が前記作業開始位置における該駆動軸の角度と異符号になる逆運動学問題の解を算出するものである請求項2に記載の多関節型ロボットの制御装置。 - 前記再算出手段が,前記姿勢データ変換手段により変換された前記作業部の姿勢データのうち予め定められた重み付けが大きい前記特定成分から順に変動の抑制量が大きくなるように前記駆動軸の角度を再度算出するものである請求項1~5のいずれかに記載の多関節型ロボットの制御装置。
- 前記作業部がトーチであり,
前記姿勢データが,前記作業部の移動方向の軸をX軸,前記X軸と重力方向との外積によって表される軸をY軸,前記X軸と前記Y軸との外積によって表される方向の軸をZ軸,前記X軸の軸周りの回転角をトーチ傾斜角,前記Y軸の軸周りの回転角をトーチ前進角,前記Z軸の軸周りの回転角をトーチ回転角とする作業座標系で表されるものであって,
前記特定成分が,前記トーチ傾斜角,前記トーチ前進角,前記トーチ回転角のいずれか一つ又は二つである請求項1~6のいずれかに記載の多関節型ロボットの制御装置。 - 前記作業部が,先端に前記作業対象物に対して作業上の効果を与える一つの作業点を有するシングルトーチであり,
前記特定成分が,前記トーチ傾斜角及び前記トーチ前進角である請求項7に記載の多関節型ロボットの制御装置。 - 前記作業部が,先端に前記作業対象物に対して作業上の効果を与える2つの作業点を有するタンデムトーチであり,
前記特定成分が,前記トーチ傾斜角及び前記トーチ回転角である請求項7に記載の多関節型ロボットの制御装置。 - 前記駆動指示手段が,前記速度許容判別手段により前記許容範囲外の駆動軸があると判別された後は,前記再算出手段により算出された前記駆動軸各々の角度に基づいて前記多関節型ロボットを駆動させ,その後,前記角度算出手段により算出された前記第1関節駆動系の角度と前記再算出手段により算出された前記第1関節駆動系の角度との差分が所定値以下となったことを条件に,前記再算出手段により算出された前記駆動軸各々の角度に代えて前記角度算出手段により算出される前記駆動軸各々の角度に基づいて前記多関節型ロボットを駆動させるものである請求項1~9のいずれかに記載の多関節型ロボットの制御装置。
- 前記駆動指示手段が,前記再算出手段により算出された前記駆動軸各々の角度に基づく前記多関節ロボットの駆動制御から前記角度算出手段により算出された前記駆動軸各々の角度に基づく前記多関節ロボットの駆動制御に切り替える際に,該駆動軸各々の角度を所定の変動範囲内で徐々に変動させるものである請求項10に記載の多関節型ロボットの制御装置。
- 前記速度算出手段が,速度に代えて加速度を算出するものであり,前記再算出手段及び前記駆動指示手段が,速度に代えて加速度に基づいて判別するものである請求項1~11のいずれかに記載の多関節型ロボットの制御装置。
- 作業対象物の作業線に則して移動させる作業部を先端に有し,その作業部に連結されて前記作業部の姿勢を変化させる3つの駆動軸を備えた第1関節駆動系と,前記第1関節駆動系に連結されて該第1関節駆動系の位置を変化させる少なくとも3つの駆動軸を備えた第2関節駆動系とを有する多関節型ロボットを制御する多関節型ロボットの制御方法であって,
前記多関節型ロボットのベース座標系において予め教示された前記作業部の作業開始位置及びその時の姿勢と作業終了位置及びその時の姿勢とを結ぶ教示経路を補間する複数の補間点における前記作業部の位置及びその時の姿勢を示すデータを算出する補間データ算出工程と,
前記補間データ算出工程により算出されたデータから逆運動学問題の解を求めることにより,前記補間点における前記作業部の位置及びその時の姿勢を示す前記第1関節駆動系及び前記第2関節駆動系を含む全ての駆動軸の角度を算出する角度算出工程と,
前記角度算出工程により算出された次の前記補間点と現在の前記補間点とにおける前記第1関節駆動系の前記各駆動軸の角度の差分に基づいて次の前記補間点に前記作業部の姿勢を動かすときの前記第1関節駆動系における両端の2つの駆動軸の速度を算出する速度算出工程と,
前記補間データ算出工程により算出されたデータを,前記作業部の移動方向の軸,該軸に垂直な方向の軸,該2つの軸に垂直な軸を含む作業座標系における前記作業部の姿勢を示す姿勢データに変換する姿勢データ変換工程と,
前記速度算出工程により算出された前記第1関節駆動系における両端の2つの駆動軸の一方又は両方の速度が予め定められた許容範囲外である場合に,前記作業部の移動速度を変更することなく,且つ前記姿勢データ変換工程により変換された次の前記補間点における前記作業部の姿勢データに対して一つ又は二つの特定成分の変動を抑制しつつ前記第1関節駆動系における両端の2つの駆動軸の速度が前記許容範囲内となる前記第1関節駆動系の駆動軸各々の角度を再度算出し,該算出された前記第1関節駆動系の駆動軸各々の角度と前記補間データ算出工程により算出された前記作業部の位置とに基づいて前記第2関節駆動系の駆動軸各々の角度を再度算出する再算出工程と,
前記速度算出工程により算出された前記第1関節駆動系における両端の2つの駆動軸の両方の速度が前記許容範囲内である場合は,前記角度算出工程により算出された前記駆動軸各々の角度に基づいて前記多関節型ロボットを駆動させ,前記速度算出工程により算出された前記第1関節駆動系における両端の2つの駆動軸の一方又は両方の速度が前記許容範囲外である場合は,前記再算出工程により算出された前記駆動軸各々の角度に基づいて前記多関節型ロボットを駆動させる駆動指示工程と,
を実行することを特徴とする多関節型ロボットの制御方法。 - 作業対象物の作業線に則して移動させる作業部を先端に有し,その作業部に連結されて前記作業部の姿勢を変化させる3つの駆動軸を備えた第1関節駆動系と,前記第1関節駆動系に連結されて該第1関節駆動系の位置を変化させる少なくとも3つの駆動軸を備えた第2関節駆動系とを有する多関節型ロボットを制御するコンピュータによって実行される多関節型ロボットの制御プログラムであって,
前記多関節型ロボットのベース座標系において予め教示された前記作業部の作業開始位置及びその時の姿勢と作業終了位置及びその時の姿勢とを結ぶ教示経路を補間する複数の補間点における前記作業部の位置及びその時の姿勢を示すデータを算出する補間データ算出工程と,
前記補間データ算出工程により算出されたデータから逆運動学問題の解を求めることにより,前記補間点における前記作業部の位置及びその時の姿勢を示す前記第1関節駆動系及び前記第2関節駆動系を含む全ての駆動軸の角度を算出する角度算出工程と,
前記角度算出工程により算出された次の前記補間点と現在の前記補間点とにおける前記第1関節駆動系の前記各駆動軸の角度の差分に基づいて次の前記補間点に前記作業部の姿勢を動かすときの前記第1関節駆動系における両端の2つの駆動軸の速度を算出する速度算出工程と,
前記補間データ算出工程により算出されたデータを,前記作業部の移動方向の軸,該軸に垂直な方向の軸,該2つの軸に垂直な軸を含む作業座標系における前記作業部の姿勢を示す姿勢データに変換する姿勢データ変換工程と,
前記速度算出工程により算出された前記第1関節駆動系における両端の2つの駆動軸の一方又は両方の速度が予め定められた許容範囲外である場合に,前記作業部の移動速度を変更することなく,且つ前記姿勢データ変換工程により変換された次の前記補間点における前記作業部の姿勢データに対して一つ又は二つの特定成分の変動を抑制しつつ前記第1関節駆動系における両端の2つの駆動軸の速度が前記許容範囲内となる前記第1関節駆動系の駆動軸各々の角度を再度算出し,該算出された前記第1関節駆動系の駆動軸各々の角度と前記補間データ算出工程により算出された前記作業部の位置とに基づいて前記第2関節駆動系の駆動軸各々の角度を再度算出する再算出工程と,
前記速度算出工程により算出された前記第1関節駆動系における両端の2つの駆動軸の両方の速度が前記許容範囲内である場合は,前記角度算出工程により算出された前記駆動軸各々の角度に基づいて前記多関節型ロボットを駆動させ,前記速度算出工程により算出された前記第1関節駆動系における両端の2つの駆動軸の一方又は両方の速度が前記許容範囲外である場合は,前記再算出工程により算出された前記駆動軸各々の角度に基づいて前記多関節型ロボットを駆動させる駆動指示工程と,
をコンピュータに実行させることを特徴とする多関節型ロボットの制御プログラム。
Priority Applications (4)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
KR1020137023668A KR101581096B1 (ko) | 2011-03-08 | 2012-03-02 | 다관절형 로봇의 제어 장치, 제어 방법 및 제어 프로그램을 기록한 컴퓨터 판독가능한 기록 매체 |
EP12755662.9A EP2684649B1 (en) | 2011-03-08 | 2012-03-02 | Control device, control method and control program of articulated robot |
CN201280012110.9A CN103429398B (zh) | 2011-03-08 | 2012-03-02 | 多关节型机器人的控制装置以及控制方法 |
US14/003,449 US9199374B2 (en) | 2011-03-08 | 2012-03-02 | Control unit, control method, and control program of articulated robot |
Applications Claiming Priority (4)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
JP2011-050450 | 2011-03-08 | ||
JP2011050450 | 2011-03-08 | ||
JP2011254806A JP5726052B2 (ja) | 2011-03-08 | 2011-11-22 | 多関節型ロボットの制御装置,制御方法及び制御プログラム |
JP2011-254806 | 2011-11-22 |
Publications (1)
Publication Number | Publication Date |
---|---|
WO2012121125A1 true WO2012121125A1 (ja) | 2012-09-13 |
Family
ID=46798089
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
PCT/JP2012/055314 WO2012121125A1 (ja) | 2011-03-08 | 2012-03-02 | 多関節型ロボットの制御装置,制御方法及び制御プログラム |
Country Status (6)
Country | Link |
---|---|
US (1) | US9199374B2 (ja) |
EP (1) | EP2684649B1 (ja) |
JP (1) | JP5726052B2 (ja) |
KR (1) | KR101581096B1 (ja) |
CN (1) | CN103429398B (ja) |
WO (1) | WO2012121125A1 (ja) |
Cited By (6)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN104128692A (zh) * | 2013-04-30 | 2014-11-05 | 株式会社大亨 | 机器人控制装置及多道焊机器人的补偿值的示教方法 |
CN105643630A (zh) * | 2016-03-22 | 2016-06-08 | 深圳市百事达卓越科技股份有限公司 | 油罐的改造方法 |
CN106456265A (zh) * | 2014-03-17 | 2017-02-22 | 直观外科手术操作公司 | 使用逆运动学在关节极限下的遥控操作手术***和控制方法 |
TWI630053B (zh) * | 2013-04-30 | 2018-07-21 | 日商大亨股份有限公司 | 機器人控制裝置、以及在多層堆銲機器人中的偏移値的教示方法 |
TWI632019B (zh) * | 2013-04-30 | 2018-08-11 | 日商大亨股份有限公司 | 電弧銲裝置 |
US10188471B2 (en) | 1999-09-17 | 2019-01-29 | Intuitive Surgical Operations, Inc. | Tele-operative surgical systems and methods of control at joint limits using inverse kinematics |
Families Citing this family (24)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
JP6128767B2 (ja) * | 2012-07-05 | 2017-05-17 | キヤノン株式会社 | ロボット制御装置、及びロボット制御方法 |
JP2015085427A (ja) * | 2013-10-30 | 2015-05-07 | 株式会社デンソーウェーブ | 6軸ロボットの各軸角度決定方法及び6軸ロボットの制御装置 |
JP5785284B2 (ja) * | 2014-02-17 | 2015-09-24 | ファナック株式会社 | 搬送対象物の落下事故を防止するロボットシステム |
US10052757B2 (en) * | 2014-04-24 | 2018-08-21 | Mitsubishi Electric Corporation | Robot control device and robot control method |
JP6379853B2 (ja) | 2014-08-22 | 2018-08-29 | 株式会社デンソーウェーブ | ロボットの制御装置及び制御方法 |
JP6497102B2 (ja) * | 2015-02-10 | 2019-04-10 | 株式会社デンソーウェーブ | ロボットの制御装置及び制御方法 |
JP6497101B2 (ja) * | 2015-02-10 | 2019-04-10 | 株式会社デンソーウェーブ | ロボットの制御装置及び制御方法 |
US9682476B1 (en) * | 2015-05-28 | 2017-06-20 | X Development Llc | Selecting robot poses to account for cost |
EP3387496A4 (en) | 2015-12-11 | 2019-07-31 | ABB Schweiz AG | PROCESS OFFLINE PROGRAMMING OF A ROBOT AND DEVICE USING THE PROCESS |
JP6507094B2 (ja) * | 2015-12-28 | 2019-04-24 | 株式会社東芝 | マニピュレータ |
JP6565752B2 (ja) | 2016-03-17 | 2019-08-28 | 株式会社安川電機 | ロボット制御装置及びロボット制御方法 |
US10065311B1 (en) * | 2016-06-08 | 2018-09-04 | X Development Llc | Singularity handling for robot jogging |
CN106041932B (zh) * | 2016-07-01 | 2018-04-17 | 广东工业大学 | 一种ur机器人的运动控制方法 |
JP6577522B2 (ja) * | 2017-06-07 | 2019-09-18 | ファナック株式会社 | 制御装置及び機械学習装置 |
CN109693233B (zh) * | 2017-10-20 | 2020-11-24 | 深圳市优必选科技有限公司 | 机器人姿态检测方法、装置、终端设备及计算机存储介质 |
JP2019177436A (ja) * | 2018-03-30 | 2019-10-17 | 日本電産株式会社 | ロボット制御装置、ロボットの関節の角度を求める方法、プログラム |
WO2020138017A1 (ja) * | 2018-12-28 | 2020-07-02 | 川崎重工業株式会社 | ロボット制御装置、ロボットシステム及びロボット制御方法 |
CN109894781A (zh) * | 2019-04-12 | 2019-06-18 | 常熟理工学院 | 多机器人焊接控制方法及*** |
US11667035B2 (en) * | 2019-07-01 | 2023-06-06 | Wisconsin Alumni Research Foundation | Path-modifying control system managing robot singularities |
KR102281119B1 (ko) * | 2019-11-27 | 2021-07-26 | 한국생산기술연구원 | 강화학습을 이용한 7축 로봇 제어 방법 |
US20230211500A1 (en) * | 2020-05-11 | 2023-07-06 | Fanuc Corporation | Program generation device and program generation method |
CN112114539B (zh) * | 2020-09-25 | 2023-11-28 | 成都易慧家科技有限公司 | 一种双电机驱动推拉门窗的控制***及其方法 |
US20230226682A1 (en) * | 2022-01-18 | 2023-07-20 | James Walter Beard, III | Method for Teaching Torch Orientation for Robotic Welding |
CN114750153B (zh) * | 2022-04-13 | 2024-03-19 | 上海电气集团股份有限公司 | 机器人机械臂的运动控制***、协作机器人及存储介质 |
Citations (9)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
JPS62162109A (ja) | 1986-01-10 | 1987-07-18 | Kobe Steel Ltd | 多軸ロボツトの速度制御方法 |
JPS63305407A (ja) * | 1987-06-08 | 1988-12-13 | Fujitsu Ltd | ロボットの軌跡制御方式 |
JPH04205208A (ja) * | 1990-11-30 | 1992-07-27 | Kawasaki Heavy Ind Ltd | ロボットアームの制御方法 |
JPH06324730A (ja) | 1993-05-14 | 1994-11-25 | Yaskawa Electric Corp | 産業用ロボットの制御装置 |
JPH08305423A (ja) * | 1995-05-10 | 1996-11-22 | Hitachi Zosen Corp | 多関節ロボットの塗装姿勢制御方法 |
JP2003300183A (ja) | 2002-04-09 | 2003-10-21 | Nippon Steel Corp | 垂直多関節型マニピュレータの制御装置 |
JP2008036742A (ja) * | 2006-08-03 | 2008-02-21 | Tokyo Institute Of Technology | 逐次検索法による6自由度ロボットアームの逆運動学、およびそれを使用するロボットのシステム、制御方法、プログラム |
JP2009113172A (ja) * | 2007-11-08 | 2009-05-28 | Mitsubishi Electric Corp | 指令値生成装置 |
JP2009166164A (ja) * | 2008-01-15 | 2009-07-30 | Kobe Steel Ltd | 工業用ロボット |
Family Cites Families (12)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
JPS58143981A (ja) * | 1982-02-19 | 1983-08-26 | 株式会社日立製作所 | 産業用ロボツトの制御方法 |
JPS59107884A (ja) * | 1982-12-10 | 1984-06-22 | 株式会社日立製作所 | ロボツトの制御方式 |
US4718078A (en) * | 1985-08-19 | 1988-01-05 | Siemens Aktiengesellschaft | System for controlling motion of a robot |
US4680519A (en) * | 1985-09-23 | 1987-07-14 | General Electric Co. | Recursive methods for world-to-joint transformation for a robot manipulator |
US4716350A (en) * | 1986-12-08 | 1987-12-29 | Ford Motor Company | Method to avoid singularity in a robot mechanism |
JPH02146607A (ja) * | 1988-11-29 | 1990-06-05 | Fanuc Ltd | Nc移動指令補間方式 |
KR100573462B1 (ko) * | 1998-12-31 | 2007-11-12 | 주식회사 로보테크 | 산업용 로봇의 이동 제어방법_ |
US6845295B2 (en) * | 2002-03-07 | 2005-01-18 | Fanuc Robotics America, Inc. | Method of controlling a robot through a singularity |
KR20040034167A (ko) * | 2002-10-21 | 2004-04-28 | 창원대학교 공작기계기술연구센터 | 6축 수직다관절 매니퓰레이터의 직선이동 제어방법 |
JP2009166076A (ja) * | 2008-01-15 | 2009-07-30 | Kobe Steel Ltd | 溶接ロボット |
JP5103237B2 (ja) * | 2008-03-25 | 2012-12-19 | 株式会社神戸製鋼所 | ロボットの動作余裕量演算表示方法及びその装置 |
WO2010057528A1 (en) * | 2008-11-19 | 2010-05-27 | Abb Technology Ab | A method and a device for optimizing a programmed movement path for an industrial robot |
-
2011
- 2011-11-22 JP JP2011254806A patent/JP5726052B2/ja active Active
-
2012
- 2012-03-02 CN CN201280012110.9A patent/CN103429398B/zh active Active
- 2012-03-02 EP EP12755662.9A patent/EP2684649B1/en active Active
- 2012-03-02 KR KR1020137023668A patent/KR101581096B1/ko active IP Right Grant
- 2012-03-02 WO PCT/JP2012/055314 patent/WO2012121125A1/ja active Application Filing
- 2012-03-02 US US14/003,449 patent/US9199374B2/en active Active
Patent Citations (9)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
JPS62162109A (ja) | 1986-01-10 | 1987-07-18 | Kobe Steel Ltd | 多軸ロボツトの速度制御方法 |
JPS63305407A (ja) * | 1987-06-08 | 1988-12-13 | Fujitsu Ltd | ロボットの軌跡制御方式 |
JPH04205208A (ja) * | 1990-11-30 | 1992-07-27 | Kawasaki Heavy Ind Ltd | ロボットアームの制御方法 |
JPH06324730A (ja) | 1993-05-14 | 1994-11-25 | Yaskawa Electric Corp | 産業用ロボットの制御装置 |
JPH08305423A (ja) * | 1995-05-10 | 1996-11-22 | Hitachi Zosen Corp | 多関節ロボットの塗装姿勢制御方法 |
JP2003300183A (ja) | 2002-04-09 | 2003-10-21 | Nippon Steel Corp | 垂直多関節型マニピュレータの制御装置 |
JP2008036742A (ja) * | 2006-08-03 | 2008-02-21 | Tokyo Institute Of Technology | 逐次検索法による6自由度ロボットアームの逆運動学、およびそれを使用するロボットのシステム、制御方法、プログラム |
JP2009113172A (ja) * | 2007-11-08 | 2009-05-28 | Mitsubishi Electric Corp | 指令値生成装置 |
JP2009166164A (ja) * | 2008-01-15 | 2009-07-30 | Kobe Steel Ltd | 工業用ロボット |
Non-Patent Citations (1)
Title |
---|
See also references of EP2684649A4 |
Cited By (8)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
US10188471B2 (en) | 1999-09-17 | 2019-01-29 | Intuitive Surgical Operations, Inc. | Tele-operative surgical systems and methods of control at joint limits using inverse kinematics |
CN104128692A (zh) * | 2013-04-30 | 2014-11-05 | 株式会社大亨 | 机器人控制装置及多道焊机器人的补偿值的示教方法 |
CN104128692B (zh) * | 2013-04-30 | 2018-04-10 | 株式会社大亨 | 机器人控制装置及多道焊机器人的补偿值的示教方法 |
TWI630053B (zh) * | 2013-04-30 | 2018-07-21 | 日商大亨股份有限公司 | 機器人控制裝置、以及在多層堆銲機器人中的偏移値的教示方法 |
TWI632019B (zh) * | 2013-04-30 | 2018-08-11 | 日商大亨股份有限公司 | 電弧銲裝置 |
CN106456265A (zh) * | 2014-03-17 | 2017-02-22 | 直观外科手术操作公司 | 使用逆运动学在关节极限下的遥控操作手术***和控制方法 |
US10617480B2 (en) | 2014-03-17 | 2020-04-14 | Intuitive Surgical Operations, Inc. | Tele-operative surgical systems and methods of control at joint limits using inverse kinematics |
CN105643630A (zh) * | 2016-03-22 | 2016-06-08 | 深圳市百事达卓越科技股份有限公司 | 油罐的改造方法 |
Also Published As
Publication number | Publication date |
---|---|
KR101581096B1 (ko) | 2015-12-29 |
US9199374B2 (en) | 2015-12-01 |
EP2684649B1 (en) | 2021-04-28 |
JP2012196753A (ja) | 2012-10-18 |
JP5726052B2 (ja) | 2015-05-27 |
KR20130122970A (ko) | 2013-11-11 |
US20130338827A1 (en) | 2013-12-19 |
EP2684649A4 (en) | 2016-05-18 |
CN103429398A (zh) | 2013-12-04 |
EP2684649A1 (en) | 2014-01-15 |
CN103429398B (zh) | 2016-10-12 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
JP5726052B2 (ja) | 多関節型ロボットの制御装置,制御方法及び制御プログラム | |
US9242373B2 (en) | Control device, control method, and control program for articulated robot | |
JP6238628B2 (ja) | ロボット装置、ロボット制御方法、ロボット制御プログラム及びロボット装置を用いた部品の製造方法 | |
JP5893666B2 (ja) | 力に応じて動かすロボットのロボット制御装置およびロボットシステム | |
WO2013038544A1 (ja) | ロボットシステム及びロボット制御装置 | |
EP2660013B1 (en) | Method of controlling seven-shaft multi-joint robot, control program, and robot control device | |
JP2019018272A (ja) | モーション生成方法、モーション生成装置、システム及びコンピュータプログラム | |
JP2017094440A (ja) | ロボットのダイレクト教示方法 | |
JP2014018912A (ja) | ロボット制御装置、ロボット制御方法およびロボット制御プログラムならびにロボットシステム | |
JP2007319970A (ja) | 産業用ロボットのツールの位置・姿勢制御方法及び制御システム | |
US10507585B2 (en) | Robot system that displays speed | |
JP2015174184A (ja) | 制御装置 | |
JP2017196722A (ja) | 付加軸の状態が含まれる動作プログラムを表示するロボットの制御装置 | |
JP2006059037A (ja) | 溶接ロボット制御装置 | |
JP2019030931A (ja) | ロボットの制御装置、制御方法およびロボット装置 | |
US20230146685A1 (en) | Circumferential welding method | |
JP6429977B2 (ja) | ロボット装置及びロボット制御方法 | |
JP2020110884A (ja) | ロボット制御装置、ロボット制御方法、及びロボット制御プログラム | |
JP7469457B2 (ja) | ロボットプログラミング装置及びロボットプログラミング方法 | |
US20230398688A1 (en) | Motion trajectory generation method for robot, motion trajectory generation apparatus for robot, robot system, and program | |
US20240066700A1 (en) | Device for simulating operation of robot, device for controlling robot, and method for simulating operation of robot | |
JP2024004614A (ja) | タイムチャート表示装置 | |
JP2021130158A (ja) | ロボットの制御装置及びロボットの制御方法 | |
JP2021137899A (ja) | 位置・力制御システム、位置・力制御方法及びプログラム | |
JPH02157906A (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: 12755662 Country of ref document: EP Kind code of ref document: A1 |
|
WWE | Wipo information: entry into national phase |
Ref document number: 14003449 Country of ref document: US |
|
ENP | Entry into the national phase |
Ref document number: 20137023668 Country of ref document: KR Kind code of ref document: A |
|
NENP | Non-entry into the national phase |
Ref country code: DE |
|
WWE | Wipo information: entry into national phase |
Ref document number: 2012755662 Country of ref document: EP |