WO2015137167A1 - ロボットのシミュレーション装置及び方法、制御装置、及びロボットシステム - Google Patents

ロボットのシミュレーション装置及び方法、制御装置、及びロボットシステム Download PDF

Info

Publication number
WO2015137167A1
WO2015137167A1 PCT/JP2015/055929 JP2015055929W WO2015137167A1 WO 2015137167 A1 WO2015137167 A1 WO 2015137167A1 JP 2015055929 W JP2015055929 W JP 2015055929W WO 2015137167 A1 WO2015137167 A1 WO 2015137167A1
Authority
WO
WIPO (PCT)
Prior art keywords
command value
angle
joint
joints
hand position
Prior art date
Application number
PCT/JP2015/055929
Other languages
English (en)
French (fr)
Inventor
夏樹 松波
智宏 田見
川内　直人
Original Assignee
三菱重工業株式会社
Priority date (The priority date is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the date listed.)
Filing date
Publication date
Application filed by 三菱重工業株式会社 filed Critical 三菱重工業株式会社
Priority to US15/117,800 priority Critical patent/US20170120449A1/en
Publication of WO2015137167A1 publication Critical patent/WO2015137167A1/ja

Links

Images

Classifications

    • B—PERFORMING OPERATIONS; TRANSPORTING
    • B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
    • B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
    • B25J9/00—Programme-controlled manipulators
    • B25J9/16—Programme controls
    • B25J9/1656—Programme controls characterised by programming, planning systems for manipulators
    • B25J9/1671—Programme controls characterised by programming, planning systems for manipulators characterised by simulation, either to verify existing program or to create and verify new program, CAD/CAM oriented, graphic oriented programming systems
    • B—PERFORMING OPERATIONS; TRANSPORTING
    • B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
    • B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
    • B25J9/00—Programme-controlled manipulators
    • B25J9/02—Programme-controlled manipulators characterised by movement of the arms, e.g. cartesian coordinate type
    • B25J9/04—Programme-controlled manipulators characterised by movement of the arms, e.g. cartesian coordinate type by rotating at least one arm, excluding the head movement itself, e.g. cylindrical coordinate type or polar coordinate type
    • B25J9/046—Revolute coordinate type
    • B—PERFORMING OPERATIONS; TRANSPORTING
    • B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
    • B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
    • B25J9/00—Programme-controlled manipulators
    • B25J9/16—Programme controls
    • B25J9/1674—Programme controls characterised by safety, monitoring, diagnostic
    • B25J9/1676—Avoiding collision or forbidden zones
    • 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/40314—Simulation of program locally before remote operation
    • 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/40317—For collision avoidance and detection
    • 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/40373—Control of trajectory in case of a limb, joint disturbation, failure
    • 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/40495—Inverse kinematics model controls trajectory planning and servo system
    • 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/40515—Integration of simulation and planning

Definitions

  • the present invention relates to control of a robot provided with an articulated manipulator.
  • FIG. 1 shows a reference example of an articulated manipulator.
  • the articulated manipulator 101 includes a plurality of links L101 to L106 connected in series.
  • a pair of adjacent links (for example, links L101 and L102) are movably connected to each other by a joint (joint J102) provided therebetween.
  • joint J102 joint
  • FIG. 1 an articulated manipulator 101 having six rotary joints (joints J101 to J106) is depicted.
  • one end of the support portion 103 is attached to the fixed base portion 102.
  • One side of the first joint J101 is attached to the other end of the support portion 103.
  • One end of the first link L101 is attached to the other side of the first joint J101.
  • One side of the second joint J102 is attached to the other end of the first link L101.
  • one side of the sixth joint J106 is attached to the other end of the fifth link L105.
  • One end of the sixth link L106 is attached to the other side of the sixth joint J106.
  • the end effector 104 is attached to the other end of the link L106.
  • the operator specifies the position (position command value) in the world coordinate system of the hand (position of the end effector tip, etc.) to the control device.
  • the control device calculates angle command values of the joints J101 to J106 so that the hand moves in the direction of the position command value by inverse kinematics calculation.
  • Each joint J101 to J106 is driven by a motor or the like according to the angle command value.
  • Non-Patent Document 1 describes a method using linear feedback control and a method using two-stage control of linearization and servo compensation as a robot position control method.
  • the simulation device is a simulation device for an articulated manipulator having a plurality of joints.
  • the simulation apparatus includes, for each of the plurality of joints, a storage unit that stores an angle movable range and a safety range defined inside the movable range in a storage device, and an arithmetic unit as an angle command value calculation unit.
  • the angle command value calculation unit generates an angle command value for each of the plurality of joints based on current angle data indicating the current angle of each of the plurality of joints and a hand position command value for the articulated manipulator.
  • the angle command value calculation unit generates an angle command value for the stop candidate joint when a stop candidate joint is generated that is a joint whose angle command value exceeds the safety range among any of the plurality of joints. Obstacle avoidance control is performed to reduce the change rate of the command value.
  • the simulation method is a simulation method of an articulated manipulator having a plurality of joints.
  • the simulation method includes a step of storing, in a storage device, a movable range of angles and a safety range defined inside the movable range for each of the plurality of joints; And a step of generating an angle command value for each of a plurality of joints based on current angle data indicating the above and a hand position command value of the articulated manipulator.
  • the obstacle avoidance control for reducing the change speed of the angle command value is performed.
  • FIG. 1 shows a reference example of an articulated manipulator.
  • FIG. 2 shows a reference example of an articulated manipulator.
  • FIG. 3 shows the time change of the joint angle command value.
  • FIG. 4 shows a robot system.
  • FIG. 5 is an explanatory diagram of calculation in joint control.
  • FIG. 6 shows the time change of the joint angle command value.
  • FIG. 7 shows the time change of the joint angle command value.
  • FIG. 8 shows functional blocks implemented by a computer.
  • FIG. 9 shows a safety margin table.
  • FIG. 10 is a flowchart of failure avoidance control.
  • FIG. 11 shows a data table.
  • FIG. 12 is an explanatory diagram of the simulation calculation of the articulated manipulator.
  • FIG. 13 shows an example of an articulated manipulator image.
  • FIG. 14 is a flowchart of the trajectory correction process.
  • FIG. 15 shows an example of an articulated manipulator image.
  • FIG. 16 is a flowchart of the waypoint setting process.
  • FIG. 17 is an explanatory diagram of a waypoint setting method.
  • FIG. 2 shows an articulated manipulator 101 in a reference example.
  • the control device calculates a joint angle command value so that the hand 105 moves to the target point 110.
  • each joint is driven so that the hand 105 reaches the target point 110.
  • FIG. 2 shows a trajectory 111 drawn by the hand 105 in this operation.
  • control device gives a command value for the position and posture of the hand 105, and based on the command value, calculates a command value for the angle of each joint by inverse kinematics.
  • the articulated manipulator 101 is driven by the command value of each joint angle.
  • the command value can be calculated easily and systematically by using the Jacobian matrix that gives the relationship between the hand speed and the joint angular speed.
  • the command value of the angular displacement (angle change amount) ⁇ of each joint is obtained by time-integrating the joint angular velocity calculated using the Jacobian matrix.
  • the command value of the joint angle is obtained.
  • FIG. 3 shows an example of angles of the joints J102, J104, and J106 calculated by the above method when the hand 105 moves while drawing the trajectory 111.
  • the horizontal line in FIG. 3 indicates time.
  • An angle 121 depicted by a curve in FIG. 3A indicates a time change of the angle ⁇ 1 of the joint J102.
  • An angle 122 depicted by a curve in FIG. 3B indicates a time change of the angle ⁇ 2 of the joint J104.
  • An angle 123 depicted by a curve in FIG. 3C indicates a time change of the angle ⁇ 3 of the joint J106.
  • FIG. 3 shows the movable ranges of the joints J102, J104, and J106.
  • the angle ⁇ 1 of the joint J102 is movable in a range between the lower limit value LL101 and the upper limit value UL101, and cannot exceed the range.
  • the angle ⁇ 2 of the joint J104 is movable in a range between the lower limit value LL102 and the upper limit value UL102
  • the angle ⁇ 3 of the joint J106 is movable in a range between the lower limit value LL103 and the upper limit value UL103.
  • the angle ⁇ 1 of the joint J102 is below the lower limit value LL101 at time t1. Therefore, even if the joint angle for reaching the target point 110 is calculated according to the forward kinematics calculation and the reverse kinematic calculation, the joint J102 exceeds the movable range at time t1. In such a case, it is common to provide an interlock that stops the operation at that time. Therefore, when the articulated manipulator 101 is controlled while performing forward kinematics and inverse kinematics calculation using the Jacobian matrix in real time, the joint motion stops at time t1.
  • the angles ⁇ 1 to ⁇ 3 of all the joints are It is within the movable range. That is, although the command value of the joint angle once exceeds the movable range, the angles of the joints J102, J104, and J106 are movable in the posture of the multi-joint manipulator 101 when the hand 105 reaches the target point 110. Within range. Therefore, if the track 111 on the way is different, the hand 105 may have been moved to the target point 110.
  • FIG. 2 and 3 are diagrams used for the sake of convenience to explain the matters recognized by the inventor. Therefore, FIG. 2 and FIG. 3 do not show a known technique before the application of the present application.
  • FIG. 4 shows a robot system including an articulated manipulator 1, a computer D1, and a display device D2 in the embodiment.
  • the articulated manipulator 1 includes a base 2 fixed to a floor surface or the like.
  • One end of the support portion 3 is fixed to the base portion 2.
  • the base 2 can be moved by a carriage or the like, the following description can be applied in the same manner as long as coordinate conversion is performed for the movement.
  • the other end of the support part 3 is fixed to one side of the joint J1.
  • One end of the first link L1 is attached to the other side of the joint J1.
  • One side of the second joint J2 is attached to the other end of the first link L1.
  • one side of the sixth joint J6 is attached to the other end of the fifth link L5.
  • One end of a sixth link L6 is attached to the other side of the sixth joint J6.
  • the end effector 4 is attached to the other end of the sixth link L6.
  • the multi-joint manipulator 1 including six joints J1 to J6 is depicted, but there are n or less (n is a natural number of 1 or more) joints J1 to Jn.
  • the multi-joint manipulator 1 having n degrees of freedom may be used.
  • the operator uses a position command value and a posture command indicating a desired position (movement target position) and a desired posture (target posture) in the world coordinate system of the hand 5 set at the tip of the end effector 4 of the articulated manipulator 1 or the like.
  • a value (final target value) is specified to the control device.
  • the control device generates angle command values for the joints J1 to J6 so that the hand 5 moves toward the state indicated by the position command value and the posture command value.
  • Each of the joints J1 to J6 is driven by a motor or the like according to the angle command value.
  • the computer D1 is connected to the articulated manipulator 1.
  • the computer D1 includes a tangible, non-transitory storage medium (storage device) such as a hard disk, and an arithmetic device including a hardware processor.
  • the computer D1 (more specifically, the computing device of the computer) executes the software (program) stored in the storage medium to virtually display the state or operation of the articulated manipulator 1 on the display device D2 ( Simulation display).
  • the operator can confirm in advance the state or operation of the articulated manipulator 1 on the screen of the display device D2 by the simulation.
  • the operator looks at the articulated manipulator image 6 displayed on the screen, moves the hand 5 on the screen to a desired location by a graphical user interface such as a pointer, and further designates the posture of the hand 5. By such a screen operation, the position command value and posture command value of the hand 5 of the articulated manipulator 1 can be set.
  • the computer D1 (or the computer D1 and the display device D2) realizes a simulation device that performs simulation of the articulated manipulator 1, a data generation device for controlling the articulated manipulator 1, and a control device for the articulated manipulator 1.
  • the computer D1, the display device D2, and the articulated manipulator 1 constitute a robot system.
  • FIG. 5 shows a general flow of calculation in the control of the joints J1 to J6 when the command value of the hand 5 is input.
  • the multi-joint manipulator 1 can detect the joint angle ⁇ indicating the current posture of each of the joints J1 to J6 using an encoder or the like.
  • the computer D1 acquires the detected value of the joint angle ⁇ of each joint J1 to J6 from the multi-joint manipulator 1 (A0).
  • the computer D1 calculates a current hand position and hand posture in the world coordinate system by performing forward kinematics calculation based on the joint angle ⁇ and a link parameter (such as the length of each link) stored in advance ( A1).
  • the operator uses the computer D1 to input a hand command indicating the final target position and target posture of the hand 5 while viewing the simulation image of the display device D2.
  • the computer D1 generates trajectory data of the position until the hand 5 of the articulated manipulator 1 reaches the final target position and final target attitude from the current position and current attitude.
  • the trajectory data may include data indicating a temporal change in the position of the hand and data indicating a temporal change in the posture of the hand.
  • a command value (hand command value) of the hand position and hand posture to be targeted in the next control cycle is given to the current hand 5 (A8).
  • the computer D1 calculates a deviation E (the deviation E includes a position deviation and a posture deviation) of the hand position and hand posture command values with respect to the current hand position and hand posture calculated in A1 (A2). .
  • the computer D1 further multiplies the deviation E by a preset proportional gain KP for position and orientation control (A3).
  • a Jacobian matrix [J] indicating the relationship between the angular velocity of the joint and the hand velocity in the orthogonal coordinate system is calculated (A4). Further, an inverse matrix [J] ⁇ 1 of the Jacobian matrix (a pseudo inverse matrix when the articulated manipulator 1 has redundant degrees of freedom) is calculated (A5). Using this inverse Jacobian matrix [J] ⁇ 1 , a command value for the joint angular velocity is calculated from the position / posture deviation KP ⁇ E multiplied by the gain (A6).
  • the command value of the amount of change in angle of each joint J1 to J6 is calculated by time integration of the command value of the joint angular velocity.
  • a command value for the joint angle is obtained by adding the command value for the amount of change in the angle to the current value of the angle for each joint J1 to J6 input in block A0 (A7).
  • K in A7 in FIG. 5 is a coefficient.
  • the computer D1 transmits a command value for the joint angle to the multi-joint manipulator 1.
  • the motion control device of the multi-joint manipulator 1 controls the motors and the like of the joints J1 to J6 based on the command value. With the above control, the end effector 4 can be moved so as to take the target position and target posture designated by the operator.
  • FIG. 6 shows the time change of the command value of the angle ⁇ when it is assumed that the movable range is not limited for a certain joint. However, actually, it is assumed that the movable range of this joint is limited to a range between the lower limit LL1 and the upper limit UL1.
  • a virtual limit is set inside. That is, the virtual lower limit LL2 is set to a value slightly larger than the lower limit LL1. The virtual upper limit UL2 is set to a value slightly smaller than the upper limit UL1.
  • Such virtual upper limit UL2 and lower limit LL2 are set for each joint according to the characteristics of each joint.
  • the angle 21 (command value) falls below the virtual lower limit LL2 at time t1, and the angle 21 falls below the actual lower limit LL1 at time t2. Thereafter, at time t3, the angle 21 exceeds the actual lower limit LL1, and returns to the movable range. Furthermore, the virtual lower limit LL2 is exceeded at time t4 and the safety zone 22 is entered. Finally, at the stage where the hand 5 reaches the final target point at time t5, the angle 21 is inside the movable range.
  • the failure avoidance control is performed when the angle 21 is less than the virtual lower limit LL2 or exceeds the virtual upper limit UL2. For example, in FIG. 6, since the angle 21 falls below the virtual lower limit LL2 at time t1, switching from normal control (non-failure avoidance control that is control other than failure avoidance control) to failure avoidance control is performed.
  • FIG. 7 shows an example of the change of the angle 21 in the obstacle avoidance control.
  • the change speed of the angle command value of the joint approaching the limit is decreased.
  • normal control is continued for the other joints.
  • An angle 21 after time t1 in FIG. 7 indicates a control result based on such an angle command value. As a result of such control, there is a time margin until the angle 21 approaches the lower limit LL1.
  • an angle 23 indicated by a dotted line indicates a change in angle when the normal control is continued even after time t1.
  • the joint angle 23 reaches the lower limit LL1 at time t2, and the operation of the articulated manipulator 1 stops.
  • the change speed of the joint angle is reduced, so that there is still a margin before reaching the lower limit LL1 even at time t2.
  • the posture of the multi-joint manipulator 1 changes as the other joints rotate.
  • the change in the joint angle 21 turns away from the lower limit LL1 during the period in which the obstacle avoidance control is being performed, and enters the safe range 22 above the virtual lower limit LL2 at time t3. At this time t3, the joint control is returned from the failure avoidance control to the normal control.
  • FIG. 8 shows functional blocks implemented by the computer D1 for performing failure avoidance control.
  • the computer D1 functions as the storage unit 11, the angle command value calculation unit 12, the obstacle avoidance control unit 13, the trajectory data generation unit 14, the simulation unit 15, the faulty joint display unit 16, and the trajectory correction unit 17.
  • Each of these functional blocks can be realized by reading and executing software (program) stored in a non-transitional storage medium by the arithmetic control device of the computer D1.
  • the computer D1 (more specifically, the computing device of the computer) executes the above-described program, thereby causing trajectory data generation processing, angle command value calculation processing, virtual limit determination processing, simulation processing, limit determination described later. Processing, trajectory correction processing, obstacle joint display processing, etc. are realized.
  • FIG. 9 shows the safety zone table 25 stored in the storage unit 11.
  • the upper and lower limits (actual limits) of the movable range and the upper and lower limits of the safety range 22 set inside thereof are registered. Is done.
  • the failure avoidance control may be applied by registering the safety zone 22 only in a part of the joints J1, J2,.
  • the movable range may be registered by a pair of a limit angle in the right rotation direction and a limit angle in the left rotation direction instead of the upper limit and the lower limit.
  • FIG. 10 is a flowchart of failure avoidance control.
  • the trajectory data generation unit 14 executes trajectory data generation processing for generating trajectory data indicating command values for the position of the hand 5 and the posture of the hand of the articulated manipulator 1.
  • This trajectory data is used to move the hand of the multi-joint manipulator 1 from a current hand position to a target hand position along a predetermined trajectory (for example, constant velocity linear motion).
  • Such trajectory data is given, for example, as command values for the position of the hand 5 and the posture of the hand for each control cycle of the articulated manipulator 1.
  • the computer D1 controls the articulated manipulator 1, the trajectory data generated in this way is input.
  • FIG. 11 shows a data table 24 representing data used in the control in the present embodiment.
  • a data table 24 may be stored in the storage unit 11.
  • hand command values at least hand position command values indicating hand positions, more generally hand position command values indicating hand positions and hand postures
  • A1 to An indicate trajectory data.
  • the trajectory data is a series of hand position command values Ai (i is 1) from the hand command value A1 at the first time T1 close to the current hand position to the hand command value An at the nth time Tn close to the target hand position. An integer of n or less).
  • Each hand command value Ai includes three values (X, Y, Z) indicating the position and three values (A, B, C) indicating the posture, and can be expressed by Euler angles or the like in a three-dimensional space. Indicating an angle).
  • Step S2 Calculation of Current Hand Position / Posture
  • a detection device such as an encoder for detecting the rotation angle is attached to each joint J1 to J6 of the multi-joint manipulator 1.
  • the computer D1 reads the detection values of the rotation angles of the joints J1 to J6 from the multi-joint manipulator 1 in real time (for example, in a cycle of the order of 10 ms).
  • the detected values of the angles are shown in the data table 24 as current angle data C1 to Cn indicating the current angles of the plurality of joints.
  • step S4 the angle command value calculation unit 12 executes calculation for calculating the joint angle command value by the method shown in FIG. 5 (in other words, the angle command value calculation unit 12 calculates the joint angle (An angle command value calculation process for calculating the command value is executed.)
  • the current position of the hand 5 of the articulated manipulator 1 is calculated from the joint angle detection value read from the articulated manipulator 1 by forward kinematics calculation.
  • the angle command value calculation unit 12 further calculates the current posture of the link of the hand 5 (link L6 provided with the end effector 4 in FIG. 4). These correspond to the processing of A1 in FIG.
  • Step S3 Calculation of Joint Angular Velocity Command Value
  • V ⁇ unit: radians per second [rad / s]
  • Step S4 Calculation of Joint Angle Command Value
  • a command value ⁇ [ rad] is calculated.
  • the angle command value ⁇ [rad] of each of the plurality of joints is generated by adding the command value ⁇ of the angle change amount to the current value of the angle of each joint J1 to J6 input in step S2.
  • This angle command value ⁇ is shown as an angle command value Bi at each time Ti in the data table 24 of FIG.
  • Step S5 Check virtual limit of joint angle command value
  • the failure avoidance control unit 13 determines whether or not the angle command value calculated in step S5 is within the virtual limit range (region between the virtual lower limit LL2 and the virtual upper limit UL2) for each joint J1 to J6.
  • a virtual limit determination process is performed to determine whether or not. This determination is made using the safety zone values of the joints J1 to Jn registered in the safety zone table 25.
  • the upper and lower limits of the movable range of each joint J1 to Jn registered in the safety zone table 25 are used, and the angle command value of each joint is a predetermined range close to the upper limit or lower limit of the movable range ( For example, when a predetermined range in which the width to the upper limit is within 10% of the entire movable range, or a predetermined range in which the width to the lower limit is within 10% of the entire movable range) is entered, the virtual limit It may be determined that it is within the range.
  • Step S6 Branching by command value
  • the process is performed to perform normal control (in other words, non-failure avoidance control) for all the joints J1 to J6.
  • the process proceeds to S8.
  • the angle command value corresponding to any one of the plurality of joints J1 to J6 exceeds the virtual limit, it is determined that the joint that has exceeded the virtual limit is a stop candidate joint that may stop. In order to perform failure avoidance control for the stop candidate joint, the process proceeds to step S7.
  • Step S7 Integration coefficient setting for failure avoidance control
  • Step S8 Normal Control Integration Coefficient Setting
  • Step S9 Output command value to controller
  • the computer D1 outputs the angle command values for the joints J1 to J6 calculated in step S4 to the multi-joint manipulator 1.
  • the controller of the multi-joint manipulator 1 controls the angles of the joints J1 to J6 using the angle command value.
  • the simulation unit 15 in FIG. 8 executes a simulation process for virtually realizing the operation of the articulated manipulator 1 by simulation.
  • the computer D1 functions as a simulation device that virtually realizes the operation of the articulated manipulator 1 by simulation by executing a program stored in a storage medium (storage device).
  • FIG. 12 the simulation process realized by the simulation unit (simulation apparatus) will be described. Compared to FIG. 5, the following two points are different. (1) A feedback process FB is added in which the joint angle command value ⁇ generated in the block A7 is fed back as the current value ⁇ of each joint angle. (2) The current values of the angles of the joints J1 to J6 are acquired from the actual machine of the articulated manipulator 1 in the block A0 of FIG. Instead, in the block A9 of FIG. 12, the joint angle command value ⁇ calculated in the block A7 using the joint angle command value ⁇ and the hand command value at the time Ti becomes the joint angle at the next time Ti + 1. Is given as the current value ⁇ .
  • FIG. 5 The difference in calculation between FIG. 5 and FIG. 12 has the following meaning.
  • A In FIG. 5, a joint angle command value is generated in block A7 by calculation at time Ti, and the articulated manipulator 1 is controlled by the command value. The detected value of each joint angle obtained as a result of the control was used for the control calculation at the next time Ti + 1.
  • B On the other hand, in FIG. 12, the ⁇ command is artificially updated and used as the current value of ⁇ at the next time, and the convergence calculation of the Jacobian matrix is performed. By such calculation, the operation of the articulated manipulator 1 is realized in a pseudo manner inside the computer D1.
  • Such a simulation can be performed as follows with reference to the data table 24 of FIG.
  • the simulation unit 15 performs virtual operation of the articulated manipulator using the hand command values A1 to An sequentially.
  • the simulation unit 15 further obtains an angle command value Bi of each of the plurality of joints at the time Ti as a calculation result based on the hand command value Ai in the angle command value calculation unit.
  • the angle command value Bi is set as the current angle data Ci + 1 of each of the plurality of joints at the next time Ti + 1, the feedback process FB of FIG. 12 is performed.
  • FIG. 13 shows an example of an articulated manipulator image 6 calculated by the computer D1 and displayed on the display device D2.
  • a solid line indicates the start position of control
  • a dotted line indicates the multi-joint manipulator image 6 at the midpoint of control.
  • the control start position the position of the hand 5 and the posture of the hand are given.
  • a final target position that is the position of the hand at the target point 32 and a final target attitude that is the posture of the hand at the target point are set.
  • a trajectory 31 connecting the start position and the target point 32 is set.
  • the computer D1 executes the simulation shown in FIG. 12 by using these settings and information defining the articulated manipulator 1 such as link parameters and movable ranges of the joints J1 to J4. As a result, the movement of the articulated manipulator 1 when the hand 5 moves along the trajectory 31 to the target point 32 can be virtually realized.
  • the simulation unit 15 determines that at least one of the plurality of joints has reached the movable range limit.
  • a limit determination process is performed to determine whether or not).
  • the obstacle joint display unit 16 in FIG. 8 determines whether or not each joint J1 to J4 is within the movable range for the simulation motion of the articulated manipulator 1. If it is within the movable range, continue the simulation. If the angle command value (angle command value Bi in FIG. 11) of any of the joints J1 to J4 exceeds the movable range, it is determined that a faulty joint has occurred.
  • the simulation is stopped and the multi-joint manipulator image 6 at the time Ti is displayed.
  • a dotted line in FIG. 13 shows the multi-joint manipulator image 6 in a state where the simulation is stopped because the joint (for example, the joint J2) exceeds the movable range when the hand reaches the stop point 33.
  • FIG. 14 is a flowchart of the simulation method of the articulated manipulator 1 and the trajectory correction process (trajectory correction process executed by the trajectory correction unit 17) executed in the method.
  • the trajectory data generation unit 14 executes trajectory data generation processing for generating trajectory data (step S21). Similar to steps S2 to S4 in FIG. 10, angle command values for the joints J1 to J4 are calculated by kinematic calculation (angle command value calculation processing is executed). However, in the case of FIG. 10, the angle command value is calculated using the current value of the joint angle acquired from the actual machine, whereas in the case of FIG. 14, the artificial joint manipulator is simulated by the method shown in FIG. 12. 1 is realized and displayed as an operation of the articulated manipulator image 6 (step S22).
  • the simulation unit 15 determines whether or not the angle command value calculated in step S22 is within the movable range for each of the joints J1 to J4 (in other words, the simulation unit 15 includes at least one of the plurality of joints).
  • One performs limit determination processing for determining whether or not the limit of the movable range has been reached.) This determination is made using the upper and lower limits of the movable range registered in the safety zone table 25 (step S23).
  • step S24 If the angle command values are within the movable range for all the joints J1 to J4 (step S24 Yes), the process proceeds to step S27. When the angle command value Bi at the time Ti exceeds the movable range for any of the joints J1 to J4, the process proceeds to step S25 (No in step S24).
  • Step S25 Stop on CG screen
  • the simulation is stopped and the multi-joint manipulator image 6 at that time is displayed.
  • FIG. 15 shows a display example when the angle command value Bi of the joint J3 exceeds the upper limit.
  • the disabled joint display unit 16 displays the disabled joint indicating the joint on the screen for easy understanding of the joint J3 whose angle has reached the limit (in other words, the disabled joint display unit 16 has the angle limited
  • the faulty joint display process for indicating the joint that has reached is executed.
  • a display for example, display in a different color
  • a failure direction display is performed to indicate whether the angle of the joint J3 exceeds the end on one side or the other end of the movable range.
  • the failure direction display when the angle of the joint (for example, joint J3) reaches the lower limit, the lower limit display 34 indicated by a downward arrow is used, and the angle of the joint (for example, joint J3) is set to the upper limit. When it reaches, an upper limit display 34 indicated by an upward arrow is used.
  • Step S26 Storage of Hand Command Value
  • the hand command value is corrected and stored so as to be out of the limit, that is, toward the center of the movable range.
  • a new waypoint is set on the hand trajectory so that the angle of the joint J3 falls within the movable range.
  • FIG. 16 is a flowchart of such a via point setting process (note that the via point setting process is executed by the trajectory correcting unit 17 of the computer D1).
  • the hand position at the stop point 33 is read (step S31).
  • it is determined whether the angle of the joint J3 is the lower limit or the upper limit of the movable range step S32).
  • the near point 35 as illustrated in FIG. 17 is set.
  • the near point 35 is set by the hand command value Ai-k at a time Ti-k (k is an integer equal to or greater than 1) that is a predetermined period before that. (Step S33).
  • the via point 36 is set so that the joint J3 that has reached the limit at the stop point 33 is in a direction away from the limit (the disengagement direction 37 in FIG. 17) compared to the track 31 before correction.
  • the via point is set so that the joint J3 has an angle closer to the center of the movable range.
  • step S34 when the angle of the joint J3 reaches the upper limit when the hand 5 is located at the stop point 33, all other joints J1, J2, J4 are fixed (for example, the joint at the position of the stop point 33).
  • the position of the hand 5 when the angle of the joint J3 is reduced by a predetermined angle ⁇ is set as the via point 36 (fixed to the angles of J1, J2, J4) (step S34).
  • a corrected trajectory is generated by connecting the front point 35 and the via point 36 with a trajectory 38 such as a straight line, and connecting the via point 36 and the target point 32 with a trajectory 39 such as a straight line.
  • Such setting of the via point 36 (step S35) may be automatically performed by the trajectory correcting unit 17, or at least a part of processing may be performed by an input operation by an operator (for example, via point 36). May be performed by an input operation by an operator.
  • the trajectory correcting unit 17 stores the hand command values of the corrected trajectories 38 and 39 (step S26 in FIG. 14).
  • the trajectory correction process described above is executed every time one of the joints J1 to J4 reaches the limit. Therefore, two or more waypoints 36 may be set.
  • step S21 When the corrected trajectory is generated, the processing from step S21 is repeated. However, the subsequent calculation returns to the trajectory 31 at the time of the near point 35 and is executed using the corrected trajectory.
  • step S24 Yes if the angle command values of all the joints J1 to J4 are within the movable range in step S24 (step S24 Yes), whether or not the hand 5 has reached the target point 32 (final hand target value). Determination is made (step S27). If the hand 5 has not reached the target point 32, the processing from step S21 is repeated (No in step S27). When the hand 5 reaches the target point 32 (step S27 Yes), the process proceeds to step S28.
  • step S26 When the trajectory is corrected in step S26, the trajectory passing through the via point 36 (the trajectory 31 up to the front point 35 in the example of FIG. 17 and the corrected trajectories 38 and 39 after the front point 35) is connected. A hand command value for drawing the corrected trajectory is generated (step S28).
  • the computer D1 transmits this hand command value to the articulated manipulator 1 (actually articulated manipulator) as an actual operation command (step S29).
  • the hand command value is corrected so that the joints J1 to J4 do not exceed the movable range, it is highly possible that the hand 5 can reach the target point 32 without stopping the operation even in the actual machine.
  • the failure avoidance process described with reference to FIG. 10 can be applied.
  • the failure avoidance process in step S7 is executed. Thereafter, the processing after step S23 is performed.
  • step S26 If any joint angle approaches the virtual limit as a result of such processing, failure avoidance processing is first executed. Nevertheless, the trajectory correction process in step S26 is performed only when the joint reaches the actual limit. Therefore, the trajectory correction process is reduced, and the hand 5 can be guided to the target point 32 through a natural route close to the original trajectory.

Abstract

　多関節マニピュレータの各関節について、角度の可動範囲と、その内側に定義される安全域とを設定する。各関節の現在角度と、手先位置指令値とに基づいて、各関節の角度指令値を生成する。角度指令値が安全域を超えた場合、その関節の角度指令値を生成する際に、角度指令値の変化速度を小さくする障害回避制御を行う。

Description

ロボットのシミュレーション装置及び方法、制御装置、及びロボットシステム
　本発明は、多関節マニピュレータを備えたロボットの制御に関する。
　多関節マニピュレータ（多関節アーム）を備えたロボットが知られている。図１は、多関節マニピュレータの参考例を示す。多関節マニピュレータ１０１は、直列的に接続された複数のリンクＬ１０１～Ｌ１０６を備える。隣接する一対のリンク（例えばリンクＬ１０１とＬ１０２）は、その間に設けられた関節（関節Ｊ１０２）によって互いに可動的に接続される。図１の例では、６つの回転関節（関節Ｊ１０１～Ｊ１０６）を有する多関節マニピュレータ１０１が描かれている。
　具体的には、固定された基部１０２に、支持部１０３の一端が取り付けられる。支持部１０３の他端に、第１の関節Ｊ１０１の一方側が取り付けられる。第１の関節Ｊ１０１の他方側に、第１のリンクＬ１０１の一端が取り付けられる。第１のリンクＬ１０１の他端に、第２の関節Ｊ１０２の一方側が取り付けられる。以下、同様にして、第５のリンクＬ１０５の他端に第６の関節Ｊ１０６の一方側が取り付けられる。第６の関節Ｊ１０６の他方側に第６のリンクＬ１０６の一端が取り付けられる。リンクＬ１０６の他端にエンドエフェクタ１０４が取り付けられる。
　作業者は、手先（エンドエフェクタの先端などの位置）のワールド座標系における位置（位置指令値）を制御装置に対して指定する。制御装置は、逆運動学計算（ｉｎｖｅｒｓｅ　ｋｉｎｅｍａｔｉｃｓ ｃａｌｃｕｌａｔｉｏｎ）により、手先が位置指令値の方向に移動するように、各関節Ｊ１０１～Ｊ１０６の角度指令値を計算する。各関節Ｊ１０１～Ｊ１０６は、その角度指令値に従ってモータ等により駆動される。このような制御により、多関節マニピュレータ１０１の手先を所望の位置に移動することができる。
　非特許文献１には、ロボットの位置制御方法として、線形フィードバック制御を用いる方法、線形化とサーボ補償の２段階制御を用いる方法が記載されている。
吉川恒夫『ロボット制御基礎論』、コロナ社、１９８８年１１月２５日発行
　いくつかの実施形態において、シミュレーション装置は、複数の関節を備える多関節マニピュレータのシミュレーション装置である。シミュレーション装置は、複数の関節の各々について、角度の可動範囲と、可動範囲の内側に定義される安全域とを記憶装置に記憶する記憶部と、角度指令値計算部としての演算装置とを具備する。角度指令値計算部は、複数の関節の各々の現在の角度を示す現在角度データと、多関節マニピュレータの手先位置指令値とに基づいて、複数の関節の各々の角度指令値を生成する。角度指令値計算部は、複数の関節のいずれかの内、角度指令値が安全域を超えた関節である停止候補関節が発生したとき、停止候補関節の角度指令値を生成する際に、角度指令値の変化速度を小さくする障害回避制御を行う。
　いくつかの実施形態において、シミュレーション方法は、複数の関節を備える多関節マニピュレータのシミュレーション方法である。シミュレーション方法は、複数の関節の各々について、角度の可動範囲と、可動範囲の内側に定義される安全域とを記憶装置に記憶する工程と、演算装置が、複数の関節の各々の現在の角度を示す現在角度データと、多関節マニピュレータの手先位置指令値とに基づいて、複数の関節の各々の角度指令値を生成する工程とを備える。角度指令値を生成する工程において、複数の関節のいずれかの内、角度指令値が安全域を超えた関節である停止候補関節が発生したとき、停止候補関節の角度指令値を生成する際に、角度指令値の変化速度を小さくする障害回避制御を行う。
　本発明により、多関節マニピュレータの制御において、関節の可動範囲による運動の制限を緩和する技術が提供される。
　添付の図面は、実施形態の説明を助けるために本明細書に組み込まれる。なお、図面は、本発明を、図示された例および説明された例に限定するものとして解釈されるべきではない。
図１は、多関節マニピュレータの参考例を示す。 図２は、多関節マニピュレータの参考例を示す。 図３は、関節の角度指令値の時間変化を示す。 図４は、ロボットシステムを示す。 図５は、関節の制御における計算の説明図である。 図６は、関節の角度指令値の時間変化を示す。 図７は、関節の角度指令値の時間変化を示す。 図８は、コンピュータによって実現される機能ブロックを示す。 図９は、安全域テーブルを示す。 図１０は、障害回避制御のフローチャートである。 図１１は、データテーブルを示す。 図１２は、多関節マニピュレータのシミュレーション計算の説明図である。 図１３は、多関節マニピュレータ画像の一例を示す。 図１４は、軌道修正処理のフローチャートである。 図１５は、多関節マニピュレータ画像の一例を示す。 図１６は、経由点設定処理のフローチャートである。 図１７は、経由点の設定方法の説明図である。
（発明者によって認識された事項）
　図２は、参考例における多関節マニピュレータ１０１を示す。手先１０５を目標点１１０に移動する制御を考える。作業者が目標点１１０を入力すると、制御装置は手先１０５が目標点１１０に移動するように関節角度の指令値を計算する。その指令値に従って、手先１０５が目標点１１０に到達するように各関節が駆動される。図２には、この動作において手先１０５が描く軌道１１１が描かれている。
　このような制御においては、制御装置が手先１０５の位置及び姿勢について指令値を与え、その指令値に基づいて、各関節の角度の指令値を逆運動学により計算する。その各関節角度の指令値により、多関節マニピュレータ１０１が駆動される。
　各関節角度を計算する逆運動学計算において、手先速度と関節角速度の関係付けを与えるヤコビ行列を用いると、容易かつシステマティックに指令値を計算することができる。この場合、ヤコビ行列を用いて算出された関節角速度を時間積分することにより、各関節の角度変位（角度の変化量）δθの指令値が求められる。この変化量δθの指令値と関節の角度の現在値を足すことにより、関節の角度の指令値が得られる。
　次に、このように指令値を算出する場合に発生し得る問題について説明する。説明を簡単にするために、図２の多関節マニピュレータ１０１において３つの関節Ｊ１０２、Ｊ１０４、Ｊ１０６のみが駆動されることと仮定する。図３は、手先１０５が軌道１１１を描いて動く場合に、上記のような方法で算出された各関節Ｊ１０２、Ｊ１０４、Ｊ１０６の角度の例を示す。図３の横線は時間を示す。図３（ａ）に曲線で描かれている角度１２１は関節Ｊ１０２の角度θ１の時間変化を示す。図３（ｂ）に曲線で描かれている角度１２２は関節Ｊ１０４の角度θ２の時間変化を示す。図３（ｃ）に曲線で描かれている角度１２３は関節Ｊ１０６の角度θ３の時間変化を示す。
　多関節マニピュレータ１０１の各関節Ｊ１０２、Ｊ１０４、Ｊ１０６の可動範囲が狭い場合などの場合を考えると、手先１０５が目標点１１０に接近することが難しい可能性がある。図３には、各関節Ｊ１０２、Ｊ１０４、Ｊ１０６の可動範囲が示されている。関節Ｊ１０２の角度θ１は、下限値ＬＬ１０１と上限値ＵＬ１０１の間の範囲で可動であり、その範囲を超えることは出来ないものとする。同様に、関節Ｊ１０４の角度θ２は、下限値ＬＬ１０２と上限値ＵＬ１０２の間の範囲で可動であり、関節Ｊ１０６の角度θ３は、下限値ＬＬ１０３と上限値ＵＬ１０３の間の範囲で可動である。
　図３の例では、時刻ｔ１において関節Ｊ１０２の角度θ１が下限値ＬＬ１０１を下回っている。従って、順運動計算（ｆｏｒｗａｒｄ　ｋｉｎｅｍａｔｉｃｓ　ｃａｌｃｕｌａｔｉｏｎ）及び逆運動計算に従って目標点１１０に到達するための関節角度を計算しても、時刻ｔ１において関節Ｊ１０２が可動範囲を超えてしまう。このような場合、その時刻で動作停止をさせるインターロックを設けることが普通である。そのため、ヤコビ行列を用いた順運動学及び逆運動学計算をリアルタイムに行いながら多関節マニピュレータ１０１を制御している場合には、時刻ｔ１で関節の運動が停止してしまう。
　図３の例では、仮に関節Ｊ１０２、Ｊ１０４、Ｊ１０６の可動範囲に制限が無いと考えた場合に手先１０５が目標点１１０に到達する時刻ｔ２において、すべての関節の角度θ１～θ３は、それぞれの可動範囲の内側に収まっている。すなわち、関節角度の指令値が一旦は可動範囲を超えているものの、手先１０５が目標点１１０に達した段階での多関節マニピュレータ１０１の姿勢においては、各関節Ｊ１０２、Ｊ１０４、Ｊ１０６の角度は可動範囲内である。従って、途中の軌道１１１が異なっていれば、手先１０５を目標点１１０に移動することができた可能性がある。
　なお、図２および図３は、発明者によって認識された事項を説明するために便宜的に使用された図である。よって、図２および図３は、本願出願前における公知技術を示すものではない。
　以下、添付図面を参照して、実施形態を説明する。以下の詳細な説明においては、実施形態の包括的な理解を提供するために、説明の目的で多くの詳細な特定事項が開示される。しかし、一又は複数の実施形態は、これらの詳細な特定事項なしで実行可能であることが明らかである。
　図４は、実施形態における多関節マニピュレータ１と、コンピュータＤ１と、表示装置Ｄ２とを備えたロボットシステムを示す。多関節マニピュレータ１は、床面等に固定された基部２を備える。基部２に、支持部３の一端が固定される。基部２が台車等によって移動可能な場合には、その移動についての座標変換を施せば、以下の説明は同様に適用できる。
　支持部３の他端は、関節Ｊ１の一方側に固定される。関節Ｊ１の他方側に、第１のリンクＬ１の一端が取り付けられる。第１のリンクＬ１の他端に、第２の関節Ｊ２の一方側が取り付けられる。以下、同様にして、第５のリンクＬ５の他端に第６の関節Ｊ６の一方側が取り付けられる。第６の関節Ｊ６の他方側に、第６のリンクＬ６の一端が取り付けられる。第６のリンクＬ６の他端に、エンドエフェクタ４が取り付けられる。図４の例では、６個の関節Ｊ１～Ｊ６を備える多関節マニピュレータ１が描かれているが、これより多い、またはこれより少ないｎ個（ｎは、１以上の自然数）の関節Ｊ１～Ｊｎを備えたｎ自由度の多関節マニピュレータ１を用いてもよい。
　作業者は、多関節マニピュレータ１のエンドエフェクタ４の先端などに設定される手先５のワールド座標系における所望の位置（移動目標位置）及び所望の姿勢（目標姿勢）を示す位置指令値及び姿勢指令値（最終的な目標値）を制御装置に対して指定する。制御装置は、手先５が位置指令値及び姿勢指令値に示された状態に向かうように、各関節Ｊ１～Ｊ６の角度指令値を生成する。各関節Ｊ１～Ｊ６は、その角度指令値に従ってモータ等により駆動される。このような制御により、多関節マニピュレータ１の手先５を所望の位置に移動することができる。
　多関節マニピュレータ１に、コンピュータＤ１が接続される。コンピュータＤ１は、ハードディスク等の有体の非遷移的（ｔａｎｇｉｂｌｅ，ｎｏｎ－ｔｒａｎｓｉｔｏｒｙ）な記憶媒体（記憶装置）と、ハードウェアプロセッサを含む演算装置とを備える。コンピュータＤ１（より具体的には、コンピュータの演算装置）は、記憶媒体に格納されたソフトウェア（プログラム）を実行することによって、多関節マニピュレータ１の状態または動作を、表示装置Ｄ２上に仮想表示（シミュレーション表示）することができる。作業者は、そのシミュレーションにより、多関節マニピュレータ１の状態または動作を、表示装置Ｄ２の画面上で予め確認することができる。作業者は、その画面上に表示された多関節マニピュレータ画像６を見て、ポインタ等のグラフィカルユーザインタフェースによって、画面上の手先５を所望の場所に移動し、更に手先５の姿勢を指定する。このような画面操作により、多関節マニピュレータ１の手先５の位置指令値及び姿勢指令値を設定することができる。
　コンピュータＤ１（または、コンピュータＤ１及び表示装置Ｄ２）により、多関節マニピュレータ１のシミュレーションを行うシミュレーション装置、多関節マニピュレータ１の制御用データ生成装置、及び多関節マニピュレータ１の制御装置が実現される。コンピュータＤ１、表示装置Ｄ２、及び多関節マニピュレータ１により、ロボットシステムが構成される。
　図５は、手先５の指令値を入力したときの関節Ｊ１～Ｊ６の制御における計算の一般的な流れを示す。多関節マニピュレータ１は、エンコーダなどによって、関節Ｊ１～Ｊ６の各々の現在の姿勢を示す関節角度θを検出することができる。コンピュータＤ１は、多関節マニピュレータ１から、各関節Ｊ１～Ｊ６の関節角度θの検出値を取得する（Ａ０）。コンピュータＤ１は、その関節角度θと、予め記憶したリンクパラメータ（各リンクの長さ等）に基づいて順運動学計算を行うことにより、ワールド座標系における現在の手先位置及び手先姿勢を計算する（Ａ１）。
　一方、作業者はコンピュータＤ１を用い、表示装置Ｄ２のシミュレーション画像を見ながら、手先５の最終的な目標位置及び目標姿勢を示す手先指令を入力する。コンピュータＤ１は、多関節マニピュレータ１の手先５が現在位置および現在姿勢から最終目標位置および最終目標姿勢に到達するまでの位置の軌道データを生成する。なお、軌道データには、手先の位置の時間変化を示すデータと、手先の姿勢の時間変化を示すデータとが含まれていてもよい。この軌道データに基づいて、現在の手先５に対して、次の制御周期で目標とすべき手先位置及び手先姿勢の指令値（手先指令値）が与えられる（Ａ８）。コンピュータＤ１は、Ａ１で算出された現在の手先位置及び手先姿勢に対する、手先位置及び手先姿勢の指令値の偏差Ｅ（偏差Ｅは、位置偏差、および、姿勢偏差を含む）を計算する（Ａ２）。コンピュータＤ１は更に、予め設定された位置姿勢制御用の比例ゲインＫＰを偏差Ｅに掛ける（Ａ３）。
　各関節Ｊ１～Ｊ６の現在の角度の検出値に基づいて、関節の角速度と直交座標系における手先速度との関係を示すヤコビ行列[Ｊ]を計算する（Ａ４）。更に、そのヤコビ行列の逆行列[Ｊ]－１（多関節マニピュレータ１が冗長自由度を持っている場合は疑似逆行列）を計算する（Ａ５）。この逆ヤコビ行列[Ｊ]－１を用いて、ゲインを掛けた位置姿勢偏差ＫＰ・Ｅから関節角速度の指令値を算出する（Ａ６）。
　関節角速度の指令値を時間積分することにより、各関節Ｊ１～Ｊ６の角度の変化量の指令値が算出される。ブロックＡ０で入力された各関節Ｊ１～Ｊ６の角度の現在値に、この角度の変化量の指令値を足すことにより、関節角度の指令値が得られる（Ａ７）。なお、図５のＡ７における「Ｋ」は、係数である。係数は、通常制御においては、例えば、Ｋ＝１に設定される。コンピュータＤ１は、関節角度の指令値を多関節マニピュレータ１に送信する。多関節マニピュレータ１の動作制御装置は、その指令値に基づいて、各関節Ｊ１～Ｊ６のモータ等を制御する。以上の制御により、作業者が指定した目標位置及び目標姿勢を取るようにエンドエフェクタ４を動かすことが可能である。
　次に、本実施形態において関節Ｊ１～Ｊ６の可動限界付近における制御について説明する。まず図６を参照して、本実施形態における制御の基本的な考えを説明する。図６は、或る関節について、可動範囲が制限されていないと仮定した場合の角度θの指令値の時間変化を示す。しかし実際には、この関節は、可動範囲が下限ＬＬ１と上限ＵＬ１との間の範囲に制限されているものとする。その内側に、仮想的なリミットを設定する。すなわち、下限ＬＬ１よりやや大きい値に仮想的な下限ＬＬ２を設定する。上限ＵＬ１よりやや小さい値に仮想的な上限ＵＬ２を設定する。このような仮想的な上限ＵＬ２、下限ＬＬ２は、各関節の特性に応じて、関節毎に設定される。
　関節の角度２１が仮想的な上限ＵＬ２と下限ＬＬ２の間の値を取っている領域では、実際の上限ＵＬ１、下限ＬＬ１まで余裕幅があるので、通常の順運動学・逆運動学の計算によって指令値を算出して制御を行う。
　図６の例では、時刻ｔ１において角度２１（指令値）が仮想的な下限ＬＬ２を下回り、時刻ｔ２において角度２１が実際の下限ＬＬ１を下回る。その後、時刻ｔ３において角度２１が実際の下限ＬＬ１を上回り、可動範囲に戻る。更に、時刻ｔ４において仮想的な下限ＬＬ２を上回り、安全域２２に入る。最終的に、時刻ｔ５において手先５が最終目標点に到達した段階では、角度２１は可動範囲の内側に入っている。
　このような制御において、関節の角度２１が上限ＵＬ１又は下限ＬＬ１に近づいたときに、何らかの対処を取ることが望まれる。そのため、角度２１が仮想的な下限ＬＬ２を下回るか、又は仮想的な上限ＵＬ２を上回ったときに、障害回避制御が行われる。例えば図６では、時刻ｔ１において角度２１が仮想的な下限ＬＬ２を下回るので、通常制御（障害回避制御以外の制御である非障害回避制御）から障害回避制御への切り替えが行われる。
　図７は、障害回避制御における角度２１の変化の一例を示す。障害回避制御が開始される時刻ｔ１において、リミットに近づいた関節の角度指令値の変化速度を小さくする。このとき、その他の関節については通常制御が続行される。図７の時刻ｔ１以降の角度２１は、このような角度指令値による制御結果を示す。このような制御の結果、角度２１が下限ＬＬ１に接近するまでに時間の余裕が生じる。
　図７において、点線で示した角度２３は、時刻ｔ１以降においても通常制御を続行した場合の角度の変化を示す。この場合、時刻ｔ２において関節の角度２３が下限ＬＬ１に達して、多関節マニピュレータ１の動作が停止してしまう。しかしながら、障害回避制御を適用した場合、関節の角度の変化速度が低下しているために、時刻ｔ２になってもまだ下限ＬＬ１に到達するまでには余裕幅がある。このような状態において、他の関節が回転することにより、多関節マニピュレータ１の姿勢が変化する。その結果、障害回避制御を行っている関節に対する角度指令値が変化するため、角度指令値がリミットに達することを回避できる可能性がある。図７の例では、障害回避制御を行っている期間に関節の角度２１の変化が下限ＬＬ１から離れる方向に転じ、時刻ｔ３において仮想的な下限ＬＬ２を上回って安全域２２に入る。この時刻ｔ３において、関節の制御が障害回避制御から通常の制御に戻される。
　以上のような障害回避制御について、以下により具体的に説明する。図８は、障害回避制御を行うためにコンピュータＤ１によって実現される機能ブロックを示す。コンピュータＤ１は、記憶部１１、角度指令値計算部１２、障害回避制御部１３、軌道データ生成部１４、シミュレーション部１５、障害関節表示部１６、及び軌道修正部１７として機能する。これらの各機能ブロックは、コンピュータＤ１の演算制御装置が非遷移的な記憶媒体に格納されたソフトウェア（プログラム）を読み出して実行することによって実現することができる。
　すなわち、コンピュータＤ１（より具体的には、コンピュータの演算装置）は、上述のプログラムを実行することにより、後述の軌道データ生成処理、角度指令値算出処理、仮想リミット判定処理、シミュレーション処理、リミット判定処理、軌道修正処理、障害関節表示処理等を実現する。
　図９は、記憶部１１が記憶する安全域テーブル２５を示す。多関節マニピュレータ１が備えるｎ個の関節Ｊ１、Ｊ２、…Ｊｎの各々について、可動範囲の上限と下限（実際のリミット）と、それより内側に設定された安全域２２の上限と下限とが登録される。関節Ｊ１、Ｊ２、…Ｊｎのうちの一部にのみ安全域２２を登録して障害回避制御を適用してもよい。上限・下限ではなく右回転方向の限界角度と左回転方向の限界角度の対などによって可動範囲を登録してもよい。
［ステップＳ１：軌道データ入力］
　図１０は、障害回避制御のフローチャートである。軌道データ生成部１４は、多関節マニピュレータ１の手先５の位置及び手先の姿勢の指令値を示す軌道データを生成する軌道データ生成処理を実行する。この軌道データは、多関節マニピュレータ１の手先を、現在手先位置から目標手先位置まで所定の軌道（例えば等速直線運動）に沿って移動させるために用いられる。このような軌道データは、例えば、多関節マニピュレータ１の制御周期毎の手先５の位置及び手先の姿勢の指令値として与えられる。コンピュータＤ１が多関節マニピュレータ１の制御を行うとき、このようにして生成された軌道データを入力する。
　図１１に、本実施形態における制御で用いるデータを表したデータテーブル２４を示す。このようなデータテーブル２４を記憶部１１に格納してもよい。データテーブル２４において、手先指令値（少なくとも手先の位置を示す手先位置指令値、より一般的には手先の位置および手先の姿勢を示す手先位置指令値）Ａ１～Ａｎが、軌道データを示す。軌道データは、現在手先位置に近い第１番目の時刻Ｔ１における手先指令値Ａ１から目標手先位置に近い第ｎ番目の時刻Ｔｎにおける手先指令値Ａｎまでの一連の手先位置指令値Ａｉ（ｉは１以上ｎ以下の整数）からなる。各手先指令値Ａｉは、位置を示す３つの値（Ｘ、Ｙ、Ｚと記す）と、姿勢を示す３つの値（Ａ、Ｂ、Ｃと記し、オイラー角などにより表現できる三次元空間内の角度を示す）とを含む。
［ステップＳ２：手先の現在位置・姿勢の計算］
　多関節マニピュレータ１の各関節Ｊ１～Ｊ６には、その回転角度を検出するエンコーダ等の検出装置が取り付けられる。コンピュータＤ１は、各関節Ｊ１～Ｊ６の回転角度の検出値を多関節マニピュレータ１からリアルタイムで（例えば１０ｍｓのオーダーの周期で）読み込む。この角度の検出値が、データテーブル２４に、複数の関節の各々の現在の角度を示す現在角度データＣ１～Ｃｎとして示されている。
　以下、ステップＳ４まで、角度指令値計算部１２は、図５に示した方法によって、関節角度指令値を算出するための計算を実行する（換言すれば、角度指令値計算部１２は、関節角度指令値を算出する角度指令値算出処理を実行する。）。まず、多関節マニピュレータ１から読み込まれた関節角度の検出値から、順運動学計算によって、多関節マニピュレータ１の手先５の現在位置が算出される。角度指令値計算部１２は更に、手先５のリンク（図４ではエンドエフェクタ４を備えたリンクＬ６）の現在姿勢を算出する。これらは図５のＡ１の処理に相当する。
［ステップＳ３：関節角速度指令値の算出］
　ステップＳ１で入力された軌道データ指令値と、ステップＳ２で生成された手先５の位置・姿勢の現在値とに基づいて、図５のＡ２～Ａ５の計算を行うことにより、各関節Ｊ１～Ｊ６に対する回転の速度を示す角速度指令値Ｖθ（単位はラジアン毎秒［ｒａｄ／ｓ］）が算出される。これは図５のＡ６に相当する。
［ステップＳ４：関節角度指令値の算出］
　複数の関節Ｊ１～Ｊ６の各々の関節角速度指令値Ｖθに、図５のＡ７のように係数Ｋを掛けて時間積分することにより、次の制御周期における関節の角度の変化量の指令値δθ［ｒａｄ］が算出される。ステップＳ２で入力された各関節Ｊ１～Ｊ６の角度の現在値に、角度の変化量の指令値δθを加えることにより、複数の関節の各々の角度指令値θ［ｒａｄ］が生成される。この角度指令値θが、図１１のデータテーブル２４において、各時刻Ｔｉにおける角度指令値Ｂｉとして示されている。通常制御（非障害回避制御）で使用される係数Ｋは、例えばＫ＝１に設定される。
［ステップＳ５：関節角度指令値の仮想リミットをチェック］
　障害回避制御部１３は、各関節Ｊ１～Ｊ６について、ステップＳ５で算出された角度指令値が仮想リミットの範囲内（仮想の下限ＬＬ２と仮想の上限ＵＬ２との間の領域）に入っているか否かを判定する仮想リミット判定処理を実行する。この判定は、安全域テーブル２５に登録された各関節Ｊ１～Ｊｎの安全域の値を用いて行われる。又は他の方法として、安全域テーブル２５に登録された各関節Ｊ１～Ｊｎの可動範囲の上限と下限を用い、各関節の角度指令値が、可動範囲のうちの上限又は下限に近い所定範囲（例えば、上限までの幅が可動範囲全体の幅の１０パーセント以内である所定範囲、または、下限までの幅が可動範囲全体の幅の１０パーセント以内である所定範囲）に入ったときに、仮想リミットの範囲内に入ったと判定してもよい。
［ステップＳ６：指令値による分岐処理］
　すべての関節Ｊ１～Ｊ６について角度指令値が仮想リミット以内であった場合（ステップＳ６Ｙｅｓ）、すべての関節Ｊ１～Ｊ６について通常制御（換言すれば、非障害回避制御）を行うために、処理はステップＳ８に移行する。複数の関節Ｊ１～Ｊ６のうちいずれかの関節に対応する角度指令値が仮想リミットを超えていた場合、仮想リミットを超えていた関節を停止する可能性のある停止候補関節であると判定する。その停止候補関節について障害回避制御を行うため、処理はステップＳ７に移行する。
［ステップＳ７：障害回避制御の積分係数設定］
　ステップＳ６において角度指令値が仮想リミット外であると判定された関節に対して、障害回避制御が適用される。具体的には、ステップＳ４の計算で用いられた積分係数Ｋを小さくする。例えば、Ｋ＝０．１に設定する。換言すれば、障害回避制御で使用する係数の値（例えば、Ｋ＝０．１）は、通常制御で使用する係数の値（例えば、Ｋ＝１）よりも小さい。他の関節については、通常制御が続けられる。このような設定により、ステップＳ４（図５のＡ７）で算出される関節の回転角度がリミットに近づく速度が遅くなり、図７の期間ｔ１～ｔ３における角度２１のような制御を実現することができる。
［ステップＳ８：通常制御の積分係数設定］
　ステップＳ５で関節角度指令値を生成する積分計算時に用いた係数Ｋとして、通常制御の値（例：Ｋ＝１）に設定する。前回の制御周期までに通常制御を行っていた関節については、係数Ｋ＝１は変化しない。前回の制御周期までに障害回避制御を行っていた関節については、ステップＳ７で今回新たに角度指令値が仮想リミット内であると判定された場合、障害回避制御時の積分係数の値（例：Ｋ＝０．１）が通常制御時の値Ｋ＝１に戻される。
［ステップＳ９：指令値をコントローラへ出力］
　コンピュータＤ１は、ステップＳ４で算出された関節Ｊ１～Ｊ６の角度指令値を多関節マニピュレータ１に出力する。多関節マニピュレータ１のコントローラは、その角度指令値を用いて各関節Ｊ１～Ｊ６の角度を制御する。
［多関節マニピュレータの動作のシミュレーション］
　次に、多関節マニピュレータ１の動作のシミュレーションについて説明する。上記の図１０では、多関節マニピュレータ１の実機の動作中に関節がリミットに近づいた場合の制御について説明した。それに対して、実機の動作を伴わずに、関節がリミットに近づいた場合の多関節マニピュレータ１の動作をシミュレーションによって仮想的に実現することも可能である。
　図８のシミュレーション部１５は、多関節マニピュレータ１の動作をシミュレーションによって仮想的に実現するシミュレーション処理を実行する。換言すれば、コンピュータＤ１は、記憶媒体（記憶装置）に記憶されたプログラムを実行することにより、多関節マニピュレータ１の動作をシミュレーションによって仮想的に実現するシミュレーション装置として機能する。図１２を参照して、シミュレーション部（シミュレーション装置）により実現されるシミュレーション処理について説明する。図５と比較して、以下の２点が異なっている。
（１）ブロックＡ７で生成された関節の角度指令値θが各関節角度の現在値θとしてフィードバックされるフィードバック処理ＦＢが追加されている。
（２）図５のブロックＡ０において多関節マニピュレータ１の実機から各関節Ｊ１～Ｊ６の角度の現在値を取得していた。その代わりに、図１２のブロックＡ９においては、時刻Ｔｉにおける関節の角度指令値θと手先指令値を用いてブロックＡ７で算出された関節角度の指令値θが、次の時刻Ｔｉ＋１における各関節角度の現在値θとして与えられる。
　図５と図１２における計算の違いは、以下のような意味を有する。
（ａ）図５では、時刻Ｔｉにおける計算により、ブロックＡ７において関節角度の指令値を生成し、その指令値によって多関節マニピュレータ１を制御した。その制御の結果として得られた各関節角度の検出値が、次の時刻Ｔｉ＋１における制御計算に用いられた。
（ｂ）それに対して図１２では、疑似的にθ指令を更新して次の時刻におけるθの現在値として用い、ヤコビ行列の収束計算を実施する。このような計算により、多関節マニピュレータ１の動作をコンピュータＤ１の内部で疑似的に実現する。
　このようなシミュレーションは、図１１のデータテーブル２４を参照して、以下のように行うこともできる。シミュレーション部１５は、手先指令値Ａ１からＡｎまでを順次用いて、多関節マニピュレータの動作の仮想的な実施を行う。シミュレーション部１５は更に、角度指令値計算部において手先指令値Ａｉに基づく計算結果として、時刻Ｔｉにおける複数の関節の各々の角度指令値Ｂｉを得る。その角度指令値Ｂｉを、次の時刻Ｔｉ＋１における複数の関節の各々の現在角度データＣｉ＋１として設定することにより、図１２のフィードバック処理ＦＢが行われる。
　このようなシミュレーションにより、多関節マニピュレータ１の手先５が現在の位置姿勢から最終目標位置姿勢に到達するまでの動作を仮想的かつ近似的に実現することができる。そのため、その動作において各関節について可動範囲を外れる可能性があるか否かを確認することができる。
　図１３は、コンピュータＤ１によって計算され表示装置Ｄ２に表示される多関節マニピュレータ画像６の一例を示す。実線は制御の開始位置、点線は制御の途中位置における多関節マニピュレータ画像６を示す。制御の開始位置において、手先５の位置および手先の姿勢が与えられる。更に、目標点３２における手先の位置である最終目標位置及び目標点における手先の姿勢である最終目標姿勢が設定される。更に、開始位置と目標点３２との間を繋ぐ軌道３１が設定される。
　コンピュータＤ１は、これらの設定と、リンクパラメータや各関節Ｊ１～Ｊ４の可動範囲などの多関節マニピュレータ１を定義する情報とを用いることにより、図１２に示したシミュレーションを実行する。その結果、手先５が軌道３１を目標点３２まで移動するときの多関節マニピュレータ１の運動を仮想的に実現することができる。
　このようなシミュレーションにおいて、関節が可動範囲のリミットに達したか否かの判定が行われる（換言すれば、シミュレーション部１５は、複数の関節のうちの少なくとも１つが、可動範囲のリミットに達したか否かを判定するリミット判定処理を実行する）。図８の障害関節表示部１６は、多関節マニピュレータ１のシミュレーション上の運動について、各関節Ｊ１～Ｊ４が可動範囲に入っているか否かを判定する。可動範囲に入っている場合は、シミュレーションを続行する。いずれかの関節Ｊ１～Ｊ４の角度指令値（図１１の角度指令値Ｂｉ）が可動範囲を超えた場合は、障害関節が発生したと判定する。障害関節が発生した場合、シミュレーションを停止し、その時点Ｔｉでの多関節マニピュレータ画像６を表示する。図１３における点線は、手先が停止点３３に到達したときに、関節（例えば関節Ｊ２）が可動範囲を超えたため、シミュレーションが停止された状態での多関節マニピュレータ画像６を示す。
　このようなシミュレーションにおいて、図１０を用いて説明した障害回避処理を仮想的に実現することも可能である。
［軌道修正処理］
　このようなシミュレーションにおいて、いずれかの関節Ｊ１～Ｊ４の角度が可動範囲を超える場合、すべての関節Ｊ１～Ｊ４の角度が可動範囲内に収まるように、軌道３１を修正することが望まれる。図１４は、多関節マニピュレータ１のシミュレーション方法と、その方法中で実行される軌道修正処理（軌道修正部１７によって実行される軌道修正処理）のフローチャートである。
　図１０のステップＳ１と同様に、軌道データ生成部１４は、軌道データを生成する軌道データ生成処理を実行する（ステップＳ２１）。図１０のステップＳ２～Ｓ４と同様に、運動学計算により、各関節Ｊ１～Ｊ４の角度指令値が計算される（角度指令値算出処理が実行される。）。但し、図１０の場合では実機から取得した関節角度の現在値を用いて角度指令値を計算したのに対し、図１４の場合には、図１２に示した方法により、疑似的に多関節マニピュレータ１の動作が実現され、多関節マニピュレータ画像６の動作として表示される（ステップＳ２２）。
　シミュレーション部１５は、各関節Ｊ１～Ｊ４について、ステップＳ２２で算出された角度指令値が可動範囲に入っているか否かを判定する（換言すれば、シミュレーション部１５は、複数の関節のうちの少なくとも１つが、可動範囲のリミットに達したか否かを判定するリミット判定処理を実行する。）。この判定は、安全域テーブル２５に登録された可動範囲の上限と下限の値を用いて行われる（ステップＳ２３）。
　すべての関節Ｊ１～Ｊ４について角度指令値が可動範囲内であった場合（ステップＳ２４Ｙｅｓ）、処理はステップＳ２７に移行する。関節Ｊ１～Ｊ４のいずれかについて、時刻Ｔｉにおける角度指令値Ｂｉが可動範囲を超えた場合、処理はステップＳ２５に移行する（ステップＳ２４Ｎｏ）。
［ステップＳ２５：ＣＧ画面での停止］
　関節Ｊ１～Ｊ４のいずれかの角度指令値Ｂｉが可動範囲を超えた場合、シミュレーションが停止され、その時点での多関節マニピュレータ画像６が表示される。図１５は、関節Ｊ３の角度指令値Ｂｉが上限を超えた場合の表示例を示す。障害関節表示部１６は、角度がリミットに達した関節Ｊ３について、分かりやすいように、画面上でその関節を指示する障害関節表示を行う（換言すれば、障害関節表示部１６は、角度がリミットに達した関節を指示する障害関節表示処理を実行する。）。例えば、障害関節を他の関節と区別する表示（例えば違う色での表示）がなされる。更に、関節Ｊ３の角度が可動範囲の一方側の端を超えたか他方側の端を超えたかを示す障害方向表示を行う。図１５の例では、障害方向表示として、関節（例えば、関節Ｊ３）の角度が下限に達した場合に下向き矢印で示す下限表示３４が用いられ、関節（例えば、関節Ｊ３）の角度が上限に達した場合に上向き矢印で示す上限表示３４が用いられている。
［ステップＳ２６：手先指令値の記憶］
　次に、角度がリミットに達した関節Ｊ３について、リミットから外れるように、すなわち可動範囲の中心側に向かうように手先指令値を修正して記憶する。具体的には、手先の軌道上に新たに経由点を設定することにより、関節Ｊ３の角度が可動範囲に収まるようにする。図１６は、そのような経由点設定処理（なお、経由点設定処理は、コンピュータＤ１の軌道修正部１７によって実行される。）のフローチャートである。まず、停止点３３における手先位置が読み込まれる（ステップＳ３１）。次に、関節Ｊ３の角度が可動範囲の下限か上限かが判定される（ステップＳ３２）。
　次に、図１７に例示されるような手前点３５を設定する。手前点３５は、例えば、時刻Ｔｉにおける角度指令値Ｂｉによってシミュレーションが停止した場合、それよりも所定期間だけ以前の時刻Ｔｉ－ｋ（ｋは１以上の整数）における手先指令値Ａｉ－ｋによって設定される（ステップＳ３３）。
　次に、経由点３６が設定される。経由点３６は、修正を施す前の軌道３１と比べて、停止点３３でリミットに達した関節Ｊ３がリミットから離れる方向（図１７の離脱方向３７）となるように設定される。言い換えれば、関節Ｊ３がその可動範囲の中心により近い角度となるように経由点が設定される。
　例えば、手先５が停止点３３に位置する時点で関節Ｊ３の角度が上限に達してした場合、他の全ての関節Ｊ１、Ｊ２、Ｊ４を固定して（例えば、停止点３３の位置での関節Ｊ１、Ｊ２、Ｊ４の角度に固定して）、関節Ｊ３の角度を所定の角度Δθだけ小さい値にしたときの手先５の位置が、経由点３６として設定される（ステップＳ３４）。手前点３５と経由点３６とを直線などの軌道３８で繋ぎ、経由点３６と目標点３２とを直線などの軌道３９で繋ぐことにより、修正された軌道が生成される。このような経由点３６の設定（ステップＳ３５）は、軌道修正部１７が自動的に行ってもよいし、少なくとも一部の処理を作業者による入力操作によって行ってもよい（例えば、経由点３６の設定処理を作業者による入力操作により実施してもよい。）。
　軌道修正部１７は、修正された軌道３８、３９の手先指令値を記憶する（図１４のステップＳ２６）。以上の軌道修正処理は、いずれかの関節Ｊ１～Ｊ４がリミットに達する度に実行される。そのため、２以上の経由点３６が設定されることもある。
　修正された軌道が生成されると、ステップＳ２１からの処理が繰り返される。但し、以後の計算は、手前点３５の時点の軌道３１まで戻って、修正された軌道を用いて実行される。
　図１４に戻って、ステップＳ２４において全ての関節Ｊ１～Ｊ４の角度指令値が可動範囲内であった場合（ステップＳ２４Ｙｅｓ）、手先５が目標点３２（最終手先目標値）に到達したか否か判定される（ステップＳ２７）。手先５が目標点３２に到達していない場合は、ステップＳ２１からの処理が繰り返される（ステップＳ２７Ｎｏ）。手先５が目標点３２に到達した場合（ステップＳ２７Ｙｅｓ）、処理はステップＳ２８に移行する。
　ステップＳ２６で軌道修正が行われた場合、経由点３６を通る軌道（図１７の例では手前点３５までは軌道３１、手前点３５以降では修正された軌道３８、３９）を連結することにより、修正された軌道を描く手先指令値が生成される（ステップＳ２８）。
　コンピュータＤ１は、この手先指令値を多関節マニピュレータ１（現実に存在する多関節マニピュレータ）に実動作指令として送信する（ステップＳ２９）。シミュレーション上、関節Ｊ１～Ｊ４が可動範囲を超えないように手先指令値が修正されているため、実機でも動作が停止せずに手先５が目標点３２まで到達できる見込みが高い。
　図１４を用いて説明した軌道修正処理において、図１０を用いて説明した障害回避処理を適用することもできる。その場合、ステップＳ２３において関節角度指令値が可動範囲内か否かを判定する前に、ステップＳ５、Ｓ６における関節角度指令値が仮想リミット以内か否かの判定が行われる。関節角度指令値が仮想リミットを超えていた場合、ステップＳ７における障害回避処理が実行される。その後、ステップＳ２３以降の処理が行われる。
　このような処理により、いずれかの関節の角度が仮想リミットに近づいた場合には、まず障害回避処理が実行される。それでもその関節が実際のリミットに到達した場合にのみ、ステップＳ２６での軌道修正処理が行われる。そのため、軌道修正処理が少なくなり、当初の軌道に近い自然な経路で手先５を目標点３２に導くことができる。
　本発明は上記各実施の形態に限定されず、本発明の技術思想の範囲内において、各実施の形態は適宜変形又は変更され得ることは明らかである。また、各実施形態又は変形例で用いられる種々の技術は、技術的矛盾が生じない限り、他の実施形態又は変形例にも適用可能である。
　本出願は、２０１４年３月１４日に出願された日本国特許出願第２０１４－５２５４６号を基礎とする優先権を主張し、当該基礎出願の開示の全てを引用により本出願に取り込む。

Claims (13)

  1. 　複数の関節を備える多関節マニピュレータのシミュレーション装置であって、
    　前記複数の関節の各々について、角度の可動範囲と、前記可動範囲の内側に定義される安全域とを記憶装置に記憶する記憶部と、
    　角度指令値計算部としての演算装置と
    　を具備し、
    　前記角度指令計算部は、前記複数の関節の各々の現在の角度を示す現在角度データと、前記多関節マニピュレータの手先位置指令値とに基づいて、前記複数の関節の各々の角度指令値を生成し、
    　前記角度指令値計算部は、前記複数の関節のいずれかの内、前記角度指令値が前記安全域を超えた関節である停止候補関節が発生したとき、前記停止候補関節の前記角度指令値を生成する際に、前記角度指令値の変化速度を小さくする障害回避制御を行う
    　シミュレーション装置。
  2. 　請求項１に記載されたシミュレーション装置であって、
    　前記角度指令値計算部は、前記複数の関節の各々の角速度の指令値に係数を掛けて時間積分することによって前記角度指令値を生成し、
    　前記障害回避制御で使用する前記係数の値は、前記障害回避制御以外の制御において使用する前記係数の値よりも小さい
    　シミュレーション装置。
  3. 　請求項１又は２に記載されたシミュレーション装置であって、
    　更に、前記多関節マニピュレータの手先を、現在手先位置から目標手先位置まで所定の軌道に沿って移動させるための軌道データであって、前記現在手先位置に近い第１番目の時刻Ｔ１における手先位置指令値Ａ１から前記目標手先位置に近い第ｎ番目の時刻Ｔｎにおける手先位置指令値Ａｎまでの一連の手先位置指令値Ａｉ（ｉは１以上ｎ以下の整数）からなる軌道データを生成する軌道データ生成部と、
    　前記手先位置指令値Ａ１からＡｎまでを順次用いて、前記多関節マニピュレータの動作の仮想的な実現を行うシミュレーション部とを具備し、
    　前記シミュレーション部は、
    　前記角度指令値計算部において前記手先位置指令値Ａｉに基づく計算結果として、時刻Ｔｉにおける前記複数の関節の各々の角度指令値Ｂｉを得、
    　前記角度指令値Ｂｉを、次の時刻Ｔｉ＋１における前記複数の関節の各々の前記現在角度データＣｉ＋１として設定する
    　シミュレーション装置。
  4. 　請求項３に記載されたシミュレーション装置であって、
    　更に、前記仮想的な実現において、前記角度指令値Ｂｉが前記可動範囲を超えた関節である障害関節が発生したとき、時刻Ｔｉにおける前記多関節マニピュレータを示すシミュレーション画像を表示し、且つ、前記シミュレーション画像において前記障害関節を指示する障害関節表示を行う障害関節表示部
    　を具備するシミュレーション装置。
  5. 　請求項４に記載されたシミュレーション装置であって、
    　前記可動範囲は、一方側の端と他方側の端とによって定義され、
    　前記障害関節表示部は更に、前記障害関節が、前記一方側の端と前記他方側の端とのうちのいずれを超えたかを示す障害方向表示を行う
    　シミュレーション装置。
  6. 　請求項３乃至５のいずれか一項に記載されたシミュレーション装置であって、
    　更に、前記角度指令値Ｂｉが前記可動範囲を超えたとき前記軌道データに修正を施す軌道修正部を具備し、
    　前記修正において、時刻Ｔｉよりも以前の時刻Ｔｉ－ｋ（ｋは１以上の整数）における前記手先位置指令値Ａｉ－ｋが、前記修正を施す前と比べて前記複数の関節のうち前記可動範囲を超えた関節の角度が前記可動範囲の中心側により近い角度となるように修正される
    　シミュレーション装置。
  7. 　請求項１乃至６のいずれか一項に記載されたシミュレーション装置によって生成された前記角度指令値によって現実に存在する前記多関節マニピュレータを制御する制御装置。
  8. 　請求項７に記載された制御装置と、前記多関節マニピュレータとを具備するロボットシステム。
  9. 　複数の関節を備える多関節マニピュレータのシミュレーション方法であって、
    　前記複数の関節の各々について、角度の可動範囲と、前記可動範囲の内側に定義される安全域とを記憶装置に記憶する工程と、
    　演算装置が、前記複数の関節の各々の現在の角度を示す現在角度データと、前記多関節マニピュレータの手先位置指令値とに基づいて、前記複数の関節の各々の角度指令値を生成する工程とを具備し、
    　前記角度指令値を生成する工程において、前記複数の関節のいずれかの内、前記角度指令値が前記安全域を超えた関節である停止候補関節が発生したとき、前記停止候補関節の前記角度指令値を生成する際に、前記角度指令値の変化速度を小さくする障害回避制御を行う
    　シミュレーション方法。
  10. 　請求項９に記載されたシミュレーション方法であって、
    　前記角度指令値を生成する工程において、前記複数の関節の各々の角速度の指令値に係数を掛けて時間積分することによって前記角度指令値を生成し、
    　前記障害回避制御において使用する前記係数の値は、前記障害回避制御以外の制御において使用する前記係数の値より小さい
    　シミュレーション方法。
  11. 　請求項９又は１０に記載されたシミュレーション方法であって、
    　更に、前記多関節マニピュレータの手先を、現在手先位置から目標手先位置まで所定の軌道に沿って移動させるための軌道データであって、前記現在手先位置に近い第１番目の時刻Ｔ１における手先位置指令値Ａ１から前記目標手先位置に近い第ｎ番目の時刻Ｔｎにおける手先位置指令値Ａｎまでの一連の手先位置指令値Ａｉ（ｉは１以上ｎ以下の整数）からなる軌道データを生成する工程と、
    　前記手先位置指令値Ａ１からＡｎまでを順次用いて、前記多関節マニピュレータの動作の仮想的な実現を行う工程とを具備し、
    　前記仮想的な実現を行う工程は、
    　前記角度指令値を計算する工程において前記手先位置指令値Ａｉに基づく計算結果として、時刻Ｔｉにおける前記複数の関節の各々の角度指令値Ｂｉを得る工程と、
    　前記角度指令値Ｂｉを、次の時刻Ｔｉ＋１における前記複数の関節の各々の前記現在角度データＣｉ＋１として設定する工程とを具備する
    　シミュレーション方法。
  12. 　請求項９乃至１１のいずれか一項に記載されたシミュレーション方法で生成された前記角度指令値によって現実に存在する前記多関節マニピュレータを制御する制御方法。
  13. 　請求項９乃至１１のいずれかに記載されたシミュレーション方法をコンピュータに実行させるプログラム。
PCT/JP2015/055929 2014-03-14 2015-02-27 ロボットのシミュレーション装置及び方法、制御装置、及びロボットシステム WO2015137167A1 (ja)

Priority Applications (1)

Application Number Priority Date Filing Date Title
US15/117,800 US20170120449A1 (en) 2014-03-14 2015-02-27 Simulation apparatus of robot, simulation method of robot, control unit and robot system

Applications Claiming Priority (2)

Application Number Priority Date Filing Date Title
JP2014052546A JP2015174185A (ja) 2014-03-14 2014-03-14 ロボットのシミュレーション装置及び方法、制御装置、及びロボットシステム
JP2014-052546 2014-03-14

Publications (1)

Publication Number Publication Date
WO2015137167A1 true WO2015137167A1 (ja) 2015-09-17

Family

ID=54071615

Family Applications (1)

Application Number Title Priority Date Filing Date
PCT/JP2015/055929 WO2015137167A1 (ja) 2014-03-14 2015-02-27 ロボットのシミュレーション装置及び方法、制御装置、及びロボットシステム

Country Status (3)

Country Link
US (1) US20170120449A1 (ja)
JP (1) JP2015174185A (ja)
WO (1) WO2015137167A1 (ja)

Cited By (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN108481323A (zh) * 2018-03-14 2018-09-04 清华大学天津高端装备研究院洛阳先进制造产业研发基地 基于增强现实的机器人运动轨迹自动编程系统及方法
WO2021260898A1 (ja) * 2020-06-25 2021-12-30 株式会社日立ハイテク ロボット教示装置及び作業教示方法

Families Citing this family (11)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US10303180B1 (en) * 2017-04-20 2019-05-28 X Development Llc Generating and utilizing non-uniform volume measures for voxels in robotics applications
CN107186711B (zh) * 2017-05-12 2021-06-15 广州视源电子科技股份有限公司 机械臂的限位保护方法、装置及机器人
JP6818708B2 (ja) 2018-02-28 2021-01-20 株式会社東芝 マニピュレータシステム、制御装置、制御方法、およびプログラム
CN112313046A (zh) * 2018-06-26 2021-02-02 发纳科美国公司 使用增强现实可视化和修改操作界定区域
KR102221089B1 (ko) * 2018-12-05 2021-02-26 (주)미래컴퍼니 수술용 슬레이브 암을 원격으로 제어하는 방법 및 시스템
US20230046827A1 (en) * 2020-03-05 2023-02-16 Fanuc Corporation Robot programming device and robot programming method
CN112472298B (zh) * 2020-12-15 2022-06-24 深圳市精锋医疗科技股份有限公司 手术机器人及其控制装置、控制方法
JP7449448B2 (ja) 2021-03-29 2024-03-13 新明和工業株式会社 旅客搭乗橋
US11845190B1 (en) * 2021-06-02 2023-12-19 Google Llc Injecting noise into robot simulation
CN114367975A (zh) * 2021-11-15 2022-04-19 上海应用技术大学 串联工业机器人控制算法的验证系统
WO2024048286A1 (ja) * 2022-08-30 2024-03-07 ローレルバンクマシン株式会社 多関節ロボットの制御方法、ロボットシステム、プログラム、及び、物品の製造方法

Citations (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
JP2007203380A (ja) * 2006-01-30 2007-08-16 Kawasaki Heavy Ind Ltd ロボットの教示支援装置
JP2012192518A (ja) * 2012-07-12 2012-10-11 Kawasaki Heavy Ind Ltd 冗長関節部を有する冗長ロボットの制御装置および制御方法
WO2013038544A1 (ja) * 2011-09-15 2013-03-21 株式会社安川電機 ロボットシステム及びロボット制御装置
JP2014018912A (ja) * 2012-07-18 2014-02-03 Seiko Epson Corp ロボット制御装置、ロボット制御方法およびロボット制御プログラムならびにロボットシステム

Patent Citations (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
JP2007203380A (ja) * 2006-01-30 2007-08-16 Kawasaki Heavy Ind Ltd ロボットの教示支援装置
WO2013038544A1 (ja) * 2011-09-15 2013-03-21 株式会社安川電機 ロボットシステム及びロボット制御装置
JP2012192518A (ja) * 2012-07-12 2012-10-11 Kawasaki Heavy Ind Ltd 冗長関節部を有する冗長ロボットの制御装置および制御方法
JP2014018912A (ja) * 2012-07-18 2014-02-03 Seiko Epson Corp ロボット制御装置、ロボット制御方法およびロボット制御プログラムならびにロボットシステム

Cited By (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN108481323A (zh) * 2018-03-14 2018-09-04 清华大学天津高端装备研究院洛阳先进制造产业研发基地 基于增强现实的机器人运动轨迹自动编程系统及方法
CN108481323B (zh) * 2018-03-14 2021-04-27 清华大学天津高端装备研究院洛阳先进制造产业研发基地 基于增强现实的机器人运动轨迹自动编程系统及方法
WO2021260898A1 (ja) * 2020-06-25 2021-12-30 株式会社日立ハイテク ロボット教示装置及び作業教示方法
JP7454046B2 (ja) 2020-06-25 2024-03-21 株式会社日立ハイテク ロボット教示装置及び作業教示方法

Also Published As

Publication number Publication date
US20170120449A1 (en) 2017-05-04
JP2015174185A (ja) 2015-10-05

Similar Documents

Publication Publication Date Title
WO2015137167A1 (ja) ロボットのシミュレーション装置及び方法、制御装置、及びロボットシステム
US11116593B2 (en) Robot system
US9149931B2 (en) Robot system, robot control device and method for controlling robot
US20130245829A1 (en) Robot control method, robot control device, and robot control system
US10836038B2 (en) Learning path control
JP6669715B2 (ja) 振動抑制装置
JP2015030078A (ja) ロボット装置及びロボット制御方法
US10384344B2 (en) Processing device, parameter adjusting method, and storage medium
JP6379853B2 (ja) ロボットの制御装置及び制御方法
US20130211595A1 (en) Control method of robot apparatus and robot apparatus
US10406688B2 (en) Offline programming apparatus and method having workpiece position detection program generation function using contact sensor
US20200023516A1 (en) Automatic path generation device
JP2016028842A (ja) アクチュエータシステム
WO2015137162A1 (ja) 制御装置、ロボットシステム、および制御用データ生成方法
JP4888374B2 (ja) ロボットの動作制御装置及びその動作制御方法
JP5774552B2 (ja) ロボット制御装置
US10022868B2 (en) Inverse kinematic solution for multi-joint link mechanism, and teaching-data creating device using the inverse kinematic solution
JP5382148B2 (ja) ロボットの動作制御装置及びその動作制御方法
US20200238512A1 (en) Teaching Data Generation System For Vertical Multi-Joint Robot
Suzumura et al. Workspace control of a wheel-legged mobile robot for gyrating locomotion with movable leg
TW202333008A (zh) 控制裝置、教示裝置、及機械系統
JP5829968B2 (ja) 多関節ロボット、その関節協調制御装置および方法
JP7232704B2 (ja) ロボットプログラム評価装置、ロボットプログラム評価方法及びロボットプログラム評価プログラム
US11964391B2 (en) Robot system
WO2022009785A1 (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: 15760777

Country of ref document: EP

Kind code of ref document: A1

WWE Wipo information: entry into national phase

Ref document number: 15117800

Country of ref document: US

NENP Non-entry into the national phase

Ref country code: DE

122 Ep: pct application non-entry in european phase

Ref document number: 15760777

Country of ref document: EP

Kind code of ref document: A1