WO2023145309A1 - 動作経路生成装置、方法、及びプログラム - Google Patents

動作経路生成装置、方法、及びプログラム Download PDF

Info

Publication number
WO2023145309A1
WO2023145309A1 PCT/JP2022/046970 JP2022046970W WO2023145309A1 WO 2023145309 A1 WO2023145309 A1 WO 2023145309A1 JP 2022046970 W JP2022046970 W JP 2022046970W WO 2023145309 A1 WO2023145309 A1 WO 2023145309A1
Authority
WO
WIPO (PCT)
Prior art keywords
contact state
contact
main object
sub
state
Prior art date
Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
Ceased
Application number
PCT/JP2022/046970
Other languages
English (en)
French (fr)
Inventor
将佳 阿部
真司 川上
聡庸 金井
ゆみ 齊藤
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Omron Corp
Original Assignee
Omron Corp
Omron Tateisi Electronics Co
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 Omron Corp, Omron Tateisi Electronics Co filed Critical Omron Corp
Priority to CN202280086529.2A priority Critical patent/CN118450969A/zh
Priority to US18/724,598 priority patent/US20250086341A1/en
Priority to EP22924158.3A priority patent/EP4470730A4/en
Publication of WO2023145309A1 publication Critical patent/WO2023145309A1/ja
Anticipated expiration legal-status Critical
Ceased legal-status Critical Current

Links

Classifications

    • B—PERFORMING OPERATIONS; TRANSPORTING
    • B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
    • B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
    • B25J9/00—Program-controlled manipulators
    • B25J9/16—Program controls
    • B25J9/1656—Program controls characterised by programming, planning systems for manipulators
    • B25J9/1664—Program controls characterised by programming, planning systems for manipulators characterised by motion, path, trajectory planning
    • G—PHYSICS
    • G06—COMPUTING OR CALCULATING; COUNTING
    • G06F—ELECTRIC DIGITAL DATA PROCESSING
    • G06F30/00—Computer-aided design [CAD]
    • G06F30/10—Geometric CAD
    • G06F30/17—Mechanical parametric or variational design
    • B—PERFORMING OPERATIONS; TRANSPORTING
    • B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
    • B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
    • B25J9/00—Program-controlled manipulators
    • B25J9/16—Program controls
    • B25J9/1679—Program controls characterised by the tasks executed
    • B25J9/1687—Assembly, peg and hole, palletising, straight line, weaving pattern movement
    • 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/40033—Assembly, microassembly
    • 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 disclosure relates to a motion path generation device, a motion path generation method, and a motion path generation program.
  • a path output method that automatically generates a motion path of the robot from the starting state to the target state.
  • This path output method uses an arithmetic unit having an external interface, a storage unit, and an arithmetic unit to determine the route when a robot having a plurality of movable axes conveys an object from a first position to a second position.
  • a calculation unit acquires the first position and the second position using an external interface, and acquires a transport condition when the robot transports the object from the first position to the second position. Then, the robot generates a path along which the object is transported from the first position to the second position and satisfies the transport conditions over the entire path (Japanese Patent No. 6860863).
  • Patent Document 1 since a human generates the motion path, a person with specialized knowledge is required when teaching the robot the assembly work.
  • a motion path is set up in which a so-called probing motion occurs such that the target assembly state is reached while adjusting the positional error of the object, the probing time is long and varies, which slows down task execution. There is a problem that the time required for the task cannot be estimated.
  • the technique described in Patent Document 2 generates a path that avoids contact with the surrounding environment, so it cannot be applied to the generation of a motion path for a task for assembling objects.
  • the present disclosure has been made in view of the above points, and aims to generate a motion path that can be executed at high speed without probing motion among existing paths for assembling work between objects.
  • a motion path generation apparatus provides a main object of the gripping unit in an assembly operation of a main object gripped by the gripping unit of a robot and a sub-object to be assembled.
  • an acquisition unit that acquires a start position and orientation relative to an object, an end position and orientation with respect to an object, and shape information of a work environment including the main object and the sub object; Based on this, the state of contact between the main object and the sub-object from the state of contact between the main object and the sub-object to the state of non-contact between the main object and the sub-object.
  • the grip unit is shifted from the initial state at the start position and orientation.
  • a generating unit that generates an action path of the robot that reaches any of the contact states included in the transition of the contact states, traces the transition of the contact states including the one of the contact states, and reaches a target state.
  • the acquisition unit performs an assembly operation of a main object gripped by a gripping unit of a robot and a sub-object to be assembled, and the main object of the gripping unit acquires the relative starting position and orientation, the ending position and orientation of and the shape information of the work environment including the main object and the sub-object, and a simulation unit based on the information acquired by the acquiring unit transition of the contact state between the main object and the sub-object from the contact state between the main object and the sub-object to the non-contact state between the main object and the sub-object.
  • the method includes generating a motion path of the robot that reaches any contact state included in the transition of the contact state, traces the transition of the contact state including the contact state, and reaches a target state.
  • the motion path generation program causes a computer to perform an assembling operation between a main object gripped by a gripping unit of a robot and a sub-object to be assembled.
  • an acquisition unit for acquiring a relative start position and orientation, an end position and orientation, and shape information of a working environment including the main object and the sub object; based on the information acquired by the acquisition unit, the simulating the transition of the contact state between the main object and the sub-object from the contact state between the main object and the sub-object to the non-contact state between the main object and the sub-object;
  • the contact state from the initial state in which the gripping unit is at the start position and posture based on the information acquired by the simulation unit, the acquisition unit, and the transition of the contact state simulated by the simulation unit.
  • a program for functioning as a generation unit that generates a movement path of the robot that reaches any contact state included in the transition of and traces the transition of the contact state including the contact state to reach the target state be.
  • the motion path generation device, method, and program according to the present disclosure it is possible to generate a motion path that can be executed at high speed without probing motion among existing paths for assembly work between objects.
  • FIG. 1 is a schematic configuration diagram of a robot control system 1; FIG. It is a figure for demonstrating an example of the operation
  • FIG. 3 is a diagram showing an example of a three-dimensional representation configuration space; 3 is a block diagram showing the hardware configuration of the motion path generation device; FIG. 3 is a block diagram showing an example of a functional configuration of a motion path generation device; FIG. It is a figure which shows an example of surface contact. It is a figure which shows an example of surface contact. It is a figure which shows an example of surface contact. It is a figure which shows an example of surface contact. It is a figure which shows an example of surface contact.
  • FIG. 10 is a diagram showing an example of edge contact; FIG. 10 is a diagram showing an example of edge contact; FIG. 10 is a diagram showing an example of edge contact; FIG.
  • FIG. 10 is a diagram showing an example of edge contact;
  • FIG. 10 is a diagram showing an example of edge contact;
  • FIG. 10 is a diagram showing an example of edge contact; It is a figure for demonstrating a maintenance displacement and a detachment displacement. It is a figure which shows an example of a 1st presentation screen. It is a figure which shows an example of a 2nd presentation screen.
  • 9 is a flowchart showing an example of motion path generation processing;
  • 9 is a flowchart showing an example of branching tree generation processing;
  • FIG. 10 is a diagram for explaining limitation of a contact state maintenance range;
  • FIG. 10 is a diagram for explaining a specific example of branching tree generation processing;
  • FIG. 10 is a diagram for explaining a specific example of branching tree generation processing;
  • FIG. 10 is a diagram for explaining a specific example of branching tree generation processing;
  • FIG. 10 is a diagram for explaining a specific example of branching tree generation processing;
  • FIG. 10 is
  • FIG. 10 is a diagram for explaining a specific example of branching tree generation processing;
  • FIG. 4 is a diagram for explaining a branching tree;
  • FIG. 4 is a diagram for explaining the relationship between the maintenance range and resolution;
  • FIG. 4 is a diagram for explaining the relationship between the maintenance range and resolution;
  • FIG. 4 is a diagram for explaining robot control that does not consider contact with the environment;
  • FIG. 4 is a diagram for explaining admittance control as an example of implementation of a force control system as an implementation target of the present disclosure;
  • FIG. 4 is a diagram for explaining impedance control as an example of implementation of a force control system as an implementation target of the present disclosure;
  • the robot control system 1 includes a motion path generation device 10, a robot control device 40, a robot 42, and a sensor group 50.
  • the robot 42 includes a robot arm 44 and a hand section 46.
  • the hand part 46 is an example of the "gripping part" of the present disclosure.
  • the robot arm 44 includes links and joints that connect the links and rotate or linearly expand and contract by being driven by a motor.
  • the motor is driven according to the command value output from the robot controller 40, and the rotation angle or expansion/contraction state of the joint is changed.
  • the hand unit 46 is controlled to assume a specified position and a specified posture in the three-dimensional space.
  • two orthogonal directions in the horizontal plane are the X axis and the Y axis
  • the vertical direction is the Z axis
  • rotation about the X axis is ⁇
  • rotation about the Y axis is ⁇
  • rotation about the Z axis is ⁇ .
  • the position and orientation shall be expressed as .
  • the hand part 46 is a tool provided at the tip of the robot arm 44 and capable of gripping the main object 90A.
  • the hand unit 46 may be, for example, an articulated multi-fingered robot hand, a gripper-type robot hand, a suction pad, or the like.
  • the robot 42 is preferably a soft robot that adapts to the external environment by pushing the joints and links down due to the reaction force from the external environment caused by contact, or a hand part 46 that has such properties. It can be a soft robot.
  • the sensor group 50 includes multiple types of sensors, and the sensor data acquired by each sensor is output to the robot control device 40 as time-series data.
  • the sensor group 50 is conceptually represented by one block near the hand unit 46, but each sensor included in the sensor group 50 is located at a position corresponding to the type and function of the sensor. be provided.
  • the sensor group 50 includes sensors capable of detecting an external force at the time of contact between the main object 90A and the environment, such as a force sensor and a sensor (pad sensor) capable of measuring the amount of deformation of the pad.
  • a motor encoder can also be used as a sensor capable of detecting an external force. In this case, the difference between the output value of the encoder of the motor and the command value to the motor is detected as the external force.
  • the sensor group 50 includes, as other sensors, sensors capable of recognizing relative errors (hereinafter referred to as "position and orientation errors") regarding the position and orientation of the main object 90A with respect to the control target, such as vision sensors and proximity sensors.
  • position and orientation errors sensors capable of recognizing relative errors
  • the pad sensor can also be used as a sensor capable of recognizing position and orientation errors.
  • the robot control device 40 generates a command value based on the motion path generated by the motion path generation device 10 and outputs it to the robot 42 .
  • FIG. 2 is an operation path for inserting a columnar shaft, which is the main object 90A, into a hole (insertion position) of a columnar ring, which is the subobject 90B.
  • a schematic side view showing the positional relationship between the main object 90A and the sub-object 90B and the configuration space during that operation are shown side by side.
  • the coordinates in the configuration space representing the position and orientation of the right side of the bottom surface of the shaft, which is the main object 90A, at the start of each motion are indicated by circles.
  • the initial state is a state in which the inclination ( ⁇ ) of the main object 90A gripped by the hand portion 46 is 0° and the main object 90A and the sub-object 90B are not in contact with each other.
  • Operation (1) is an operation for parallelly moving the main object 90A from the initial state to above the insertion position of the sub object 90B.
  • Operation (2) is an operation of tilting the main object 90A by a predetermined angle (45° in the example of FIG. 2).
  • Operation (3) is an operation of lowering the main object 90A toward the insertion position until the main object 90A and the sub-object 90B come into contact with each other.
  • the configuration space of action (3) represents that the primary object 90A is tilted so that the primary object 90A cannot reach the bottom of the hole in the secondary object 90B.
  • Operation (4) maintains the inclination of the main object 90A and the contact between the main object 90A and the sub-object 90B, while maintaining the main object 90A until another part of the main object 90A contacts the sub-object 90B. This is an operation to move the object 90A toward the insertion position.
  • Operation (5) uses the alignment of the main object 90A and the sub-object 90B to tilt the main object 90A so that the main object 90A is aligned with the inner circumference of the insertion position (hole) of the sub-object 90B. It is an operation to return to an angle (10° in the example of FIG. 2) that can be lowered along.
  • Operation (6) is an operation of moving the main object 90A downward along the inner periphery of the insertion position (hole) of the sub-object 90B.
  • Operation (7) is an operation to bring the main object 90A to the target angle ⁇ ° (0° in the example of FIG. 2). As described above, in the present embodiment, it is assumed that the motion path is based on the contact between the main object 90A and the sub-object 90B, which is one of the environments.
  • FIG. 2 shows the configuration space two-dimensionally
  • the configuration space in FIG. 2 also has an axis ⁇ in the direction perpendicular to the paper surface.
  • FIG. 3 shows an example of a three-dimensional representation configuration space.
  • the coordinates in the configuration space representing the position and orientation of the right side of the bottom surface of the shaft, which is the main object 90A, at the start of each of the actions (1) to (7) in FIG. 2 are indicated by circles. .
  • FIG. 4 is a block diagram showing the hardware configuration of the motion path generation device 10 according to this embodiment.
  • the motion path generation device 10 includes a CPU (Central Processing Unit) 12, a memory 14, a storage device 16, an input/output I/F (Interface) 18, an input/output device 20, a storage medium reader 22, and a communication I/F 24.
  • Each component is communicatively connected to each other via a bus 26 .
  • the storage device 16 stores a motion path generation program for executing motion path generation processing, which will be described later.
  • the CPU 12 is a central processing unit that executes various programs and controls each configuration. That is, the CPU 12 reads a program from the storage device 16 and executes the program using the memory 14 as a work area. The CPU 12 performs control of the above components and various arithmetic processing according to programs stored in the storage device 16 .
  • the memory 14 is composed of RAM (Random Access Memory) and temporarily stores programs and data as a work area.
  • the storage device 16 is composed of ROM (Read Only Memory), HDD (Hard Disk Drive), SSD (Solid State Drive), etc., and stores various programs including an operating system and various data.
  • the input/output I/F 18 is an interface for connecting the motion path generation device 10 and the robot control device 40 .
  • the motion path generated by the motion path generation device 10 is output to the robot control device 40 via the input/output I/F 18 .
  • the input/output device 20 is, for example, an input device such as a keyboard and a mouse for performing various inputs, a display device such as a display, and an output device for outputting various information such as a printer.
  • a touch panel display as an output device, it may be made to function as an input device.
  • the storage medium reading device 22 reads data stored in various storage media such as CD (Compact Disc)-ROM, DVD (Digital Versatile Disc)-ROM, Blu-ray Disc, USB (Universal Serial Bus) memory, etc. writes data to the
  • the communication I/F 24 is an interface for communicating with other devices, and uses standards such as Ethernet (registered trademark), FDDI, and Wi-Fi (registered trademark), for example.
  • FIG. 5 is a block diagram showing an example of the functional configuration of the motion path generation device 10.
  • the motion path generation device 10 includes information on the position and orientation of the hand unit 46 relative to the main object 90A at the start position (hereinafter referred to as "start position/attitude"), and information on the main object of the hand unit 46 at the end position. Information on the position and orientation relative to the object 90A (hereinafter referred to as "end position and orientation”) is input. Further, environment information indicating the shape and arrangement of the work environment including the main object 90A and the sub-object 90B is input to the motion path generation device 10 .
  • the environment information includes CAD data for each of the main object 90A and the subobject 90B, a configuration space representing a work environment in which obstacles are arranged, and the like.
  • Information for specifying the resolution of the motion of the robot 42 (hereinafter referred to as “resolution specification information”) is input to the motion path generation device 10 .
  • the resolution specifying information includes at least one of the resolution of the sensors included in the sensor group 50 and the control accuracy of actuators for driving the robot 42 .
  • the motion path generation device 10 includes an acquisition unit 32, a simulation unit 34, a generation unit 36, and a display control unit 38 as functional configurations.
  • Each functional configuration is realized by the CPU 12 reading out the motion path generation program stored in the storage device 16, developing it in the memory 14, and executing it.
  • the acquisition unit 32 acquires the start position/orientation, the end position/orientation, and the environment information input to the motion path generation device 10 and transfers them to the simulation unit 34 .
  • the acquisition unit 32 also acquires the resolution specifying information input to the motion path generation device 10 and passes it to the generation unit 36 .
  • the simulation unit 34 simulates the main object 9 based on the information passed from the acquisition unit 32. A transition of the contact state between the main object 90A and the sub-object 90B from the contact state between the main object 90A and the sub-object 90B to the non-contact state between the main object 90A and the sub-object 90B is simulated.
  • the contact state between the main object 90A and the sub-object 90B will be defined.
  • a contact state in which the surface of the main object 90A and the surface of the sub object 90B are in contact is called surface contact.
  • a contact state in which at least one side (or point) of the main object 90A and the sub-object 90B contacts the other of the main object 90A and the sub-object 90B is called side (or point) contact.
  • the term “(or point)” of the side (or point) will be omitted and will simply be referred to as “side”.
  • a plane perpendicular to the X axis is called an "X plane”
  • a plane perpendicular to the Y axis is called a “Y plane”
  • a plane perpendicular to the Z axis is called a "Z plane”.
  • a thick line represents a contact point of surface contact
  • a black circle represents a contact point of side contact.
  • FIGS. 6A to 6C An example of surface contact is shown in FIGS. 6A to 6C.
  • the main object 90A and the secondary object 90B are in contact with each other on the Z planes
  • the main object 90A and the secondary object 90B are in contact with each other on the X surfaces.
  • the contact surface is different, it is treated as surface contact in different contact states in FIGS. 6A and 6B.
  • the entire Z-plane of the main object 90A is in contact with the secondary object 90B
  • FIG. 6C only a portion of the Z-plane is in contact. In such a case, FIG. 6A and FIG. 6C are treated as surface contact in different contact states.
  • FIGS. 7A to 7F An example of edge contact is shown in FIGS. 7A to 7F.
  • FIGS. 7A and 7B are side contacts where the main object 90A and the sub-object 90B are in contact on one side.
  • Such a side contact that is in contact on one side is hereinafter referred to as a "single side contact".
  • FIGS. 7C, 7D, and 7E are edge contacts in which the main object 90A and the sub-object 90B are in contact with each other on two sides.
  • a side contact in which two sides are in contact in this way is hereinafter referred to as a "two-side contact".
  • One side on the left side of the plane is in contact with the right side X plane of the main object 90A.
  • FIG. 7F is an example of three-sided contact in which the main object 90A and the sub-object 90B are in contact with each other on three sides.
  • the simulation unit 34 starts from the target state, and when the contact state between the main object 90A and the sub-object 90B reaches a state where the main object 90A and the sub-object 90B do not move even if force is applied in a certain direction (two-sided contact to surface contact state). , continue to apply force in different directions, and when it reaches a state where it does not move, continue to apply force in a different direction from that state, thereby simulating the transition of the contact state.
  • Displacements of the main object 90A with respect to the sub-object 90B that should be taken into consideration when searching for such a contact state transition include a maintenance displacement and a detachment displacement as shown in FIG. Maintained displacement is to displace the main object 90A while maintaining the same contact state.
  • the maintenance displacement includes a case of parallel translation of the main object 90A while maintaining surface contact, and a case of rotating the main object 90A while maintaining one-side contact or two-side contact.
  • Detachment displacement is to displace the main object 90A in a direction in which the degree of freedom of the contact state increases.
  • the detachment displacement includes parallel movement of the main object 90A in a direction intersecting the contact surface with the sub-object 90B that is in surface contact and in a direction away from the sub-object 90B.
  • the detachment displacement includes a case where the main object 90A is rotated from surface contact or two-side contact to one-side contact.
  • the simulation unit 34 generates a branch tree having nodes corresponding to different contact states, with the target state as the root node, while searching for contact state transitions as described above. More specifically, the simulation unit 34 moves from any state of two-sided contact, three-sided contact, or surface contact to a state immediately after the movement of the separation displacement, or the movement of the separation displacement. Continue executing the maintenance displacement in the direction to simulate whether a new contact state is reached or not. In addition, when a new contact state is reached, if the new contact state is two-sided contact, three-sided contact, or surface contact, the simulation unit 34 similarly searches (generates a branch tree) for that contact state. ).
  • the simulation unit 34 determines that the contact state immediately after the movement of the separation displacement is one-side contact, and the new state that the maintenance displacement is continuously executed in the direction in which the separation displacement is performed is the contact with nothing. If the state is not reached, only the one-side contact state is registered in the branch tree. For each node of the branch tree, the simulation unit 34 obtains a range in which the main object 90A can be maintained and displaced in the contact state corresponding to that node (hereinafter referred to as "maintenance range"), and further associates it with that node. The simulation unit 34 obtains the maintenance range for each of the translation directions (X, Y, Z) and the rotation directions ( ⁇ , ⁇ , ⁇ ).
  • the generation unit 36 determines the transition of the contact state from the initial state in which the main object 90A is in the starting position/orientation.
  • a motion path of the robot is generated that reaches any of the included contact states, traces the transition of the contact states including any of the contact states, and reaches the target state.
  • the generation unit 36 identifies a node associated with a maintenance range larger than the motion resolution of the robot 42 in the branch tree. When multiple nodes are specified, the generation unit 36 specifies the closest node from the root node of the branching tree.
  • the generation unit 36 identifies the corresponding contact state transition from the identified node to the root node in the branching tree, and generates the path of the robot 42 corresponding to the identified contact state transition.
  • the generation unit 36 also generates a route from the position in the initial state to the contact state corresponding to the specified node, avoiding surrounding obstacles.
  • the generation unit 36 sets the path from the initial state to the contact state corresponding to the specified node to the path from the contact state corresponding to the specified node to the target state as the operation path.
  • a path is a series of positions and postures of a predetermined portion (for example, hand) of the hand portion 46 of the robot 42 .
  • the generation unit 36 when the generation unit 36 receives a user-registered position/orientation, which will be described later, from the display control unit 38 and is instructed to regenerate the motion path, the generation unit 36 reflects the user-registered position/orientation in the generated motion path, Regenerate motion path. Although the details will be described later, the user-registered position/orientation is registered correction of the position and orientation of the main object 90A at a specific time in the motion path.
  • the generation unit 36 may regenerate the motion path by modifying the generated motion path so that it passes through the specified position and orientation.
  • the generation unit 36 searches the branch tree for a node associated with the contact state corresponding to the user registered position and orientation, and performs an action based on the contact state transition via the contact state corresponding to the searched node. Routes may be regenerated.
  • the display control unit 38 displays the motion path generated by the generation unit 36 on the display device.
  • the display control unit 38 displays, for example, a first presentation screen 60 as shown in FIG. 9 on the display device.
  • the first presentation screen 60 includes a three-dimensional view 61 .
  • the three-dimensional positions (X, Y, Z) and pose ( ⁇ , ⁇ , ⁇ ) are previewed in a three-dimensional animation.
  • a three-dimensional image of the environment other than the main object 90A and the sub object 90B may also be displayed.
  • the positions and orientations of the main object 90A and the sub-object 90B may be moved with a mouse like three-dimensional CAD software.
  • the first presentation screen 60 also includes a time display 62 that displays the playback time of the animation displayed in the three-dimensional view 61. That is, the time display 62 displays the time associated with the frame displayed in the three-dimensional view 61 out of the time associated with each frame of the animation displayed in the three-dimensional view 61 . Alternatively, an arbitrary time may be input to the time display 62 and the frame corresponding to that time may be displayed on the three-dimensional view 61 .
  • the first presentation screen 60 includes a phase display 63 .
  • the phase display 63 is a uniaxial display such as a band-shaped display indicating the elapsed time from the start to the end of the motion indicated by the motion path, and the contact state between the main object 90A and the sub-object 90B changes along the motion path.
  • a first mark (broken line in phase display 63 in FIG. 9) is displayed at a position corresponding to time.
  • the phase display 63 displays a second mark (a solid line and a black triangle in the phase display 63 in FIG.
  • phase display 63 at a position corresponding to the time displayed in the time display 62 .
  • the time displayed on the time display 62 can be specified.
  • a third mark (dotted line in the phase display 63 in FIG. 9) indicating the user registered position and orientation described later is displayed at a position corresponding to the target time of the user registered position and orientation.
  • the first presentation screen 60 also includes a registration button 64.
  • the user designates a specific time, displays the positions and orientations of the main object 90A and the subobject 90B at that time in the three-dimensional view 61, and corrects the position and orientation of the main object 90A by operating the mouse or the like. Then select the registration button 64 . As a result, the position and orientation of the main object 90A corrected by the user are registered with respect to the specified time within the motion path. This registration is called "user registered position and orientation".
  • the first presentation screen 60 also includes a regeneration button 65 .
  • the display control unit 38 receives the user-registered position and orientation, and generates the received user-registered position and orientation in the generation unit 36 . and instructs to regenerate the motion path.
  • the display control unit 38 displays, for example, a second presentation screen 70 as shown in FIG. 10 on the display device.
  • the second presentation screen 70 includes a time display 62, a phase display 63, a registration button 64, and a regeneration button 65, like the first presentation screen 60 does.
  • the second presentation screen 70 also includes a three-dimensional view 71 .
  • the three-dimensional view 71 displays a configuration space whose three axes are three variables selected from three degrees of freedom of position (X, Y, Z) and three degrees of freedom of orientation ( ⁇ , ⁇ , ⁇ ). .
  • the user can register the position and orientation in the three-dimensional view 71 as well.
  • the user can correct the position and orientation of the main object 90A by moving the coordinate points (black dots in the three-dimensional view 71 of FIG. 10) on the configuration space displayed in the three-dimensional view 71.
  • the second presentation screen 70 also includes a selection area 72 for selecting three variables corresponding to the axes of the configuration space displayed in the three-dimensional view 71 .
  • the display control unit 38 may simultaneously display the first presentation screen 60 and the second presentation screen 70 in separate windows on the display device.
  • the display control unit 38 may simultaneously display a plurality of second presentation screens 70 in which the three variables to be selected are different, respectively, in separate windows.
  • the display control unit 38 synchronizes the time display 62 of the first presentation screen 60 and the second presentation screen 70 corresponding to each window.
  • the window display may be linked.
  • FIG. 11 is a flow chart showing the flow of motion path generation processing executed by the CPU 12 of the motion path generation device 10 .
  • the CPU 12 reads out the motion path generation program from the storage device 16, expands it in the memory 14, and executes it, whereby the CPU 12 functions as each functional configuration of the motion path generation device 10, and the motion path generation process shown in FIG. 11 is executed. be done.
  • step S10 the acquisition unit 32 acquires the start position/orientation, the end position/orientation, the environment information, and the resolution specifying information input to the motion path generation device 10.
  • step S12 the simulation unit 34 identifies a target state in which the main object 90A and the sub object 90B are assembled based on the end position/orientation and environment information acquired in step S10. Specifically, the simulation unit 34 identifies, as the target state, the state in which the number of surfaces in surface contact is the largest among the states in which the main object 90A and the sub-object 90B are assembled in the end position/posture. . In addition, the simulation unit 34 specifies the state in which the number of side contacts is the largest as the target state when there is no surface contact. If there are multiple goal state candidates, any state may be identified as the goal state.
  • step S14 the simulation unit 34 executes branching tree generation processing. Now, with reference to FIG. 12, the branching tree generation processing will be described.
  • the simulation unit 34 determines whether or not the main object 90A is in contact with the sub object 90B. If they are in contact, the process proceeds to loop processing L1, and if they are not in contact, the branch tree generation processing is terminated and returns.
  • the loop processing L1 starts at step L1S, and the transition of the contact state from the current contact state to the direction (including rotation) of the separation displacement while maintaining surface contact or side contact (hereinafter referred to as “L1 transition”), steps S142 to S145 and loop processing L2 are executed. Then, the loop processing L1 ends at step L1E when there is no transitionable direction other than the previously searched directions for the current contact state.
  • step S142 the simulation unit 34 determines whether or not the contact state after the transition of L1 has been registered in the branch tree. If registered, the process proceeds to step S146, and if not registered, the process proceeds to step S143.
  • step S143 the simulation unit 34 obtains the contact state maintenance range after the transition of L1. Then, the simulation unit 34 associates and registers the post-transition contact state and maintenance range of L1 with a node connected downstream of the node corresponding to the pre-transition contact state of L1 in the branch tree.
  • the simulation unit 34 When obtaining the contact state maintenance range after the transition of L1, the simulation unit 34 maintains the range in which the same contact state as before the transition of L1 can be restored when the transition in the direction of returning the transition of L1 is performed.
  • Limit the scope For example, as shown in (a) of FIG. 13, assume that the contact state before the transition of L1 is surface contact in which a part of the lower Z surface of the main object 90A is in contact with the sub object 90B. . From that state, as shown in (b), the main object 90A is kept in contact with the upper Z surface of the sub-object 90B on one side, and the main object Suppose that the contact state is changed by rotating 90A.
  • the main object 90A is translated in the X-axis direction as shown in (c) as the maintained displacement.
  • the rotation which is the transition of L1
  • the entire surface of the lower Z surface of the main object 90A contacts the sub-object 90B, resulting in surface contact ( A contact state different from a) is obtained. Therefore, in this case, the state of (c) is not included in the maintenance range of (b).
  • step S144 the simulation unit 34 determines whether or not the contact state after the transition of L1 is one-sided contact. If there is contact on one side, the process proceeds to loop processing L2, and if there is no contact on one side, that is, in the case of contact on two or more sides or surface contact, the process proceeds to step S145.
  • step S145 the simulation unit 34 recursively executes the branching tree generation process with the contact state after the transition of L1 as the contact state at the start of the branching tree generation process.
  • step S146 if the contact state after the transition of L1 is other than one-side contact, the simulation unit 34 determines the node corresponding to the contact state after the transition of L1, which has already been registered in the branch tree, and the node to be registered this time. replaces the subtree from the node farther from the root node to the leaf node to the node closer to the root node of the branch tree. Then, the process proceeds to loop processing L2. Note that if the contact state after the transition of L1 is one-sided contact, the processing of this step is skipped.
  • the loop processing L2 starts at step L2S, and for each of the contact state transitions (hereinafter referred to as "transitions of L2") where a transition in the same direction results in a new surface contact or two or more side contacts, steps S147 to S150 is executed. Then, the loop processing L2 ends at step L2E when the new surface contact or the side contact of two or more sides is not reached.
  • step S147 the simulation unit 34 determines whether step S143 has been executed for the contact state before the transition of L2. If so, the process proceeds to step S149; if not, the process proceeds to step S148.
  • step S148 similarly to the process of step S143, the contact state and maintenance range before the transition of L2 are registered in the branch tree, and the process proceeds to step S149.
  • step S149 the simulation unit 34 registers the contact state and maintenance range after the transition of L2 in the branch tree, as in the process of step S143.
  • step S150 the simulation unit 34 recursively executes branching tree generation processing with the contact state after transition of L2 as the contact state at the start of branching tree generation processing.
  • FIGS. 14 to 16 show only the contact states necessary for explanation, centering on the transitions in the X, Z, and ⁇ directions, and illustrate the contact states considering transitions in other directions. are omitted.
  • the simulation unit 34 targets a surface contact state in which two surfaces, the left X surface and the lower Z surface, of the main object 90A are in contact with the sub object 90B. It is assumed that the state is specified and branch tree generation processing is started. Next, the simulation unit 34 performs, as the transition of L1, for example, a transition in which the main object 90A is translated in the X-axis direction while maintaining surface contact on the lower Z surface. (b) shows the contact state after the transition of L1 in this case. The simulation unit 34 registers the contact state (b) in the branch tree by connecting a node associated with the contact state (b) and its maintenance range downstream of the root node corresponding to the target state (step S143). ). Since the contact state (b) is not one-side contact (negative determination in step S144), the simulation unit 34 executes the transition of L1 from the contact state (b) (loop processing L1 in step S145).
  • the simulation unit 34 executes, as a transition of L1 from the contact state (b), for example, a transition in which the main object 90A is rotated while maintaining one-side contact on the right side of the lower Z plane (under the right X plane). do. (c) shows the contact state after the transition of L1 in this case.
  • the simulation unit 34 registers the contact state (c) in a node downstream of the node corresponding to the contact state (b) (step S143). Since the contact state (c) is one-side contact (affirmative determination in step S144), loop processing L2 is executed. Continuing the transition of L2 in the same direction as the transition from (b) to (c) results in a two-sided contact condition, as shown in (d).
  • step S147 Since the contact state (c) before the transition of L2 has already been registered in the branch tree (affirmative determination in step S147), the simulation unit 34 adds contact state (d) is registered (step S149). Next, the simulation unit 34 executes the transition of L1 from the contact state (d) (loop processing L1 in step S150).
  • step S144 the simulation unit 34 terminates the loop processing L2, returns to the loop processing L1 of the branch generation processing in which the contact state (d) is set as the initial contact state, and returns to the transition from (d) to (e) in a direction different from the transition from (d) to (e). Execute the transition of L1 to .
  • the contact state (g) is the same as the contact state (c)
  • it has already been registered in the branch tree (affirmative determination in step S142), and since it is a contact on one side, step S146 is skipped, and loop processing is performed.
  • step S143 Since the contact state (g) before the transition of L2 does not pass through step S143, it is not registered in the branch tree (negative determination in step S147). Therefore, the simulation unit 34 registers the contact state (g) in the branching tree (step S148), and then registers the contact state (h) in the branching tree (step S149). For the contact state (h), branching tree generation processing is executed in the same manner as described above (step S150 in step S150).
  • the simulation unit 34 performs the transition in the same direction as the transition of L1 from (a) to (b). Perform L2 transition to .
  • a new surface contact appears in which the two surfaces of the lower Z surface and the right X surface are in contact with each other.
  • the simulation unit 34 registers the contact state (i) in the branch tree (step S149), and recursively executes branch tree generation processing with the contact state (i) as the initial contact state (step S150).
  • the simulation unit 34 registers the contact state (j) in the branch tree by connecting the nodes associated with the contact state (j) and its maintenance range (step S143). Since the contact state (j) is not one-sided contact (negative determination in step S144), the simulation unit 34 recursively executes the branch tree generation process with the contact state (j) as the initial contact state (step S145). ).
  • step S146 is skipped and loop processing is performed. Move to L2. Then, when the transition of L2 continues in the same direction as the transition of L1, a two-sided contact as shown in (l) appears. Since the contact state (k) before the transition of L2 does not pass through step S143, it is not registered in the branch tree (negative determination in step S147).
  • the simulation unit 34 registers the contact state (k) in the branching tree (step S148), and then registers the contact state (l) in the branching tree (step S149).
  • step S150 branching tree generation processing is executed in the same manner as described above (step S150 in step S150).
  • the contact state (m) From the contact state (l), the contact state ( m). Since the contact state (m) is the same as the contact state (e), and no new surface contact or edge contact appears in the transition of L2, the contact state (m) is not registered in the branch tree.
  • (n) shows the contact state after the transition from the contact state (l) to the rotation of the left side X surface of the main object 90A while maintaining the one-side contact is executed as the transition of L1.
  • the simulation unit 34 registers the contact state (n) in the branch tree (step S143), and proceeds to loop processing L2. Then, when the transition of L2 continues in the same direction as the transition of L1, a new surface contact appears as shown in (o).
  • the simulation unit 34 registers the contact state (o) in the branch tree (step S149), and recursively executes the branch tree generation process with the contact state (o) as the initial contact state (step S150).
  • (p) shows the contact state after the transition from the contact state (o) to the rotation of the main object 90A while maintaining one-side contact on the left X surface is executed as the transition of L1.
  • the simulation unit 34 registers the contact state (p) in the branch tree (step S143), and proceeds to loop processing L2. Then, when the transition of L2 continues in the same direction as the transition of L1, a new two-sided contact appears as shown in FIG. 16(q).
  • the simulation unit 34 registers the contact state (q) in the branch tree (step S149), and recursively executes branch tree generation processing with the contact state (q) as the initial contact state (step S150).
  • (r) shows the contact state after executing the transition of moving upward in the Z-axis direction while maintaining one-side contact with the right X surface of the main object 90A from the contact state (q) as the transition of L1.
  • the simulation unit 34 registers the contact state (r) in the branch tree (step S143), and proceeds to loop processing L2. As shown in (s), no new face contact or edge contact appears in the transition from the contact state (r) to L2, so it is not registered in the branch tree.
  • step S146 is skipped and loop processing is performed. Move to L2. Then, when the transition of L2 continues in the same direction as the transition of L1, no new surface contact or two-sided contact appears and the no-contact state occurs. Therefore, the simulation unit 34 terminates the loop processing L2, returns to the loop processing L1 of the branch generation processing in which the contact state (q) is the initial contact state, and returns to the transition from (q) to (t) in a direction different from the transition from (q) to (t).
  • (u) shows the contact state after the transition of L1 from the contact state (p) to rotate while maintaining two-sided contact between the left side of the X plane and the right side of the X plane (right side of the Z plane).
  • the simulation unit 34 registers the contact state (p) in the branch tree (step S143), and the simulation unit 34 recursively executes the branch tree generation process with the contact state (u) as the initial contact state (step S145).
  • the simulation unit 34 starts the left side X plane.
  • the transition of moving the main object 90A upward in the Z-axis while maintaining contact is executed as the transition of L1.
  • FIG. 14(w) shows the contact state after the transition of L1 in this case.
  • the contact state (w) is the same as the contact state (o) in FIG. Therefore, the simulation unit 34 creates a subtree from the node corresponding to the contact state (o) to the leaf node at the node of the contact state (w), which is the node closer to the root node (contact state (a)) of the branch tree. , to the node in contact state (w) (step S146).
  • a branch tree is generated in which nodes corresponding to each contact state are connected, as shown in FIGS. 14 to 16.
  • circles represent nodes corresponding to respective contact states, and arrows represent transitions of L1 or L2.
  • the nodes represented by hatched circles are in the one-side contact state among the contact states found by searching (corresponding to states (a) to (w) in FIGS. 14 to 16). is the corresponding node.
  • the nodes with white circles correspond to two-sided contact, three-sided contact, or surface contact among the contact states found by the search.
  • the number of transitions from the one-side contact state is always one and does not branch. That is, only the states corresponding to the nodes circled in white in FIG. 17 are branched. However, even if the state corresponds to the node of the white circle, the state may not branch.
  • leaf nodes states with no further transitions, terminal
  • the white circled node becomes a leaf node. It is possible to transition from the one-side contact state to the one-side contact state. Also, it is possible that the state of contact with two or more sides (the state corresponding to the node indicated by the white circle) transitions to the state of contact with two or more sides.
  • the generation unit 36 determines the motion resolution of the robot based on the resolution specifying information acquired in step S10. Specifically, when the main target object 90A and the sub-target object 90B among the sensors included in the sensor group 50 are in a non-contact state, the generation unit 36 calculates the positions of the main target object 90A and the sub-object 90B.
  • i is an identification number assigned to each sensor.
  • the generation unit 36 identifies a contact state whose maintenance range is greater than the resolution in the branching tree. For example, the generation unit 36 identifies a node associated with a maintenance range that can be included in a region with one side of ⁇ sd from the branch tree. If there are multiple applicable nodes, the node closest to the root node of the branch tree is specified.
  • the node closest to the root node of the branching tree may be, for example, a node with a small number of nodes traced from the root node, or may be the closest node by measuring the physical distance.
  • the former has the advantage of high control robustness, while the latter has the advantage of short execution time. This embodiment adopts the former.
  • the generation unit 36 then identifies the contact state associated with the identified node.
  • a probing motion may be required to reach the specified contact state from the non-contact state.
  • the specified contact state maintenance range cannot be reached.
  • the non-contact state reliably reaches the specified contact state maintenance range based on the position and orientation errors that can be recognized by the sensor. be able to.
  • step S20 the generation unit 36 generates an action path based on the contact state transition from the specified contact state to the target state. Specifically, the generation unit 36 identifies the position and posture of the hand unit 46 from the contact state corresponding to each node from the node corresponding to the contact state identified in step S18 to the root node in the branch tree. By arranging them in such a way, a trajectory from the specified contact state to the target state is generated.
  • step S22 the generation unit 36 creates a path from the starting position/orientation acquired in step S10 to the position and orientation corresponding to the contact state identified in step S18, and avoids obstacles. Generate a route. Then, the generation unit 36 combines the path generated in step S20 and the path generated in this step, and generates an action path by associating the contact state with each position and orientation.
  • step S24 the display control unit 38 displays at least one of the first presentation screen 60 and the second presentation screen on the display device based on the motion path generated in step S22. Then, the display control unit 38 receives the user registered position and orientation, which is the correction of the position and orientation of the main object 90A by the user.
  • step S26 if the user-registered position/orientation is accepted in step S24, the generation unit 36 applies the accepted user-registered position/orientation to the motion path generated in step S24. Regenerate route. Then, the generation unit 36 outputs the finally generated motion path to the robot control device 40, and the motion path generation process ends.
  • the robot control device 40 generates a command value for force control that realizes the motion path input from the motion path generation device 10 and outputs it to the robot 42 .
  • FIG. 21 shows a case of admittance control as an example of implementation of a force control system to which the present disclosure is implemented.
  • the target position ⁇ ref specified by the user is added to the corrected value obtained by subtracting the external torque ⁇ e received from the environment from the target force ⁇ ref according to the motion path and corrected by the force controller.
  • the position target value is calculated by subtracting the current position ⁇ m of .
  • the position controller calculates the motor torque for realizing the position target value to control the robot.
  • the current position ⁇ m of the motor is obtained from the controlled robot and fed back to the position controller.
  • the joint angle ⁇ act after output from the speed reducer (having springiness) is obtained, and the value obtained by subtracting ⁇ act from ⁇ m is used as the springiness of the gear of the robot and the torque sensor. is converted into an external torque ⁇ e received from the environment and fed back to the force controller.
  • FIG. 22 shows a case of impedance control as an example of implementation of a force control system to which the present disclosure is implemented.
  • the correction value obtained by subtracting the joint angle ⁇ act after the reduction gear output (having springiness) from the target position ⁇ ref corresponding to the motion path is corrected by the position controller, and the target force ⁇
  • the desired force value is calculated by adding ref and subtracting from it the external torque ⁇ e received from the environment.
  • the motor torque for realizing the force target value is calculated by the force controller to control the robot.
  • the current position ⁇ m of the motor is acquired from the controlled robot, and the value obtained by subtracting ⁇ act from ⁇ m is converted to the external torque ⁇ e received from the environment based on the springiness of the gear of the robot and the torque sensor. and feed back to the force controller. Also, the joint angle ⁇ act after output from the speed reducer (having elasticity) is fed back to the position controller.
  • the motion path generation device performs the motion of the gripping unit in the assembling operation of the main object gripped by the gripping unit of the robot and the sub-object to be assembled.
  • a start position and orientation relative to the main object, an end position and orientation, and shape information of the work environment including the main object and the sub-object are obtained.
  • the motion path generation device generates a motion path between the main object and the sub-object from the contact state of the main object and the sub-object to the non-contact state between the main object and the sub-object. Simulate the transition of the contact state with the target.
  • the main object reaches one of the contact states included in the transition of the contact state from the initial state in which the main object is in the starting position and orientation, and eventually A movement path of the robot that reaches the target state is generated by following the transition of the contact state including the contact state.
  • it is possible to generate a motion path that can be executed at high speed without a probing motion among the existing paths for the assembly work of the objects.
  • the position and orientation of the intermediate point may be set as the target state when generating the branching tree.
  • the end position/posture can be reached by tracing the contact state from the intermediate point, a branch tree representing the transition of the contact state including the state of the end position/posture is generated. If the generated branch tree does not include the end position/posture, it means that the end position/posture cannot be reached from the specified intermediate point.
  • the motion path generation device and the robot control device are separate devices, but the motion path generation device may be incorporated into the robot control device. Also, each functional configuration of the motion path generation device may be implemented by different devices, and the above motion path generation processing may be implemented by distributed processing.
  • various processors other than the CPU may execute the motion path generation processing executed by the CPU by reading the software (program) in the above embodiment.
  • the processor is a PLD (Programmable Logic Device) whose circuit configuration can be changed after manufacturing, such as an FPGA (Field-Programmable Gate Array), and an ASIC (Application Specific Integrated Circuit) to execute specific processing.
  • a dedicated electric circuit or the like which is a processor having a specially designed circuit configuration, is exemplified.
  • the motion path generation processing may be executed by one of these various processors, or a combination of two or more processors of the same or different type (for example, multiple FPGAs and a combination of a CPU and an FPGA). combination, etc.). More specifically, the hardware structure of these various processors is an electric circuit in which circuit elements such as semiconductor elements are combined.
  • the motion path generation program has been pre-stored (installed) in the storage device, but the present invention is not limited to this.
  • the program may be provided in a form stored in a storage medium such as a CD-ROM, DVD-ROM, Blu-ray disc, USB memory, or the like. Also, the program may be downloaded from an external device via a network.
  • robot control system 10 motion path generation device 12 CPU 14 memory 16 storage device 18 input device 20 output device 22 storage medium reading device 24 communication I/F 26 bus 32 acquisition unit 34 simulation unit 36 generation unit 38 display control unit 40 robot control device 42 robot 44 robot arm 46 hand unit 50 sensor group 60 first presentation screen 70 second presentation screen 90A main object 90B sub object

Landscapes

  • Engineering & Computer Science (AREA)
  • Physics & Mathematics (AREA)
  • Geometry (AREA)
  • Theoretical Computer Science (AREA)
  • General Physics & Mathematics (AREA)
  • Robotics (AREA)
  • Mechanical Engineering (AREA)
  • Mathematical Optimization (AREA)
  • Mathematical Analysis (AREA)
  • Pure & Applied Mathematics (AREA)
  • Computer Hardware Design (AREA)
  • Evolutionary Computation (AREA)
  • General Engineering & Computer Science (AREA)
  • Computational Mathematics (AREA)
  • Manipulator (AREA)
  • Numerical Control (AREA)

Abstract

対象物同士の組立作業について存在する経路のうち、探り動作なしで高速に実行可能な動作経路を生成する。　取得部３２が、ロボット４２の把持部により把持される主対象物９０Ａと、組立対象の副対象物９０Ｂとの組立動作における、把持部の主対象物９０Ａとの相対的な開始位置姿勢及び終了位置姿勢と、環境情報とを取得し、シミュレーション部３４が、取得された情報に基づいて、接触状態から非接触状態になるまでの主対象物９０Ａと副対象物９０Ｂとの接触状態の遷移をシミュレーションし、生成部３６が、取得された情報と、シミュレーションされた接触状態の遷移とに基づいて、初期状態からの接触状態の遷移に含まれるいずれかの接触状態に至り、いずれかの接触状態を含む接触状態の遷移を辿って目標状態に至るロボットの動作経路を生成する。

Description

動作経路生成装置、方法、及びプログラム
　本開示は、動作経路生成装置、動作経路生成方法、及び動作経路生成プログラムに関する。
　従来、ロボットにより把持された主対象物と、組立対象である副対象物とを組み立てる作業を実行するためのロボットの動作経路を生成する技術が提案されている。例えば、物体間のクリアランスが小さい組立プロセスにおける高速挿入を確実にするための多指ハンドが提案されている。この多指ハンドは、６自由度可逆駆動性（バックドライバビリティ）及びオブジェクト姿勢可観測性の特徴を有し、小さな物体同士の高速挿入を達成するために、２つの物体が互いに接触するときに、衝撃低減と位置誤差補償との両方を実現する（国際公開第２０２０／１９４３９３号）。
　また、例えば、開始状態から目標状態へのロボットの動作経路を自動生成する経路出力方法が提案されている。この経路出力方法は、外部インタフェース、記憶部、及び演算部を備える演算装置を用いて、複数の可動軸を有するロボットが、対象物を第１の位置から第２の位置に搬送する際の経路を出力する方法である。演算部が、外部インタフェースを用いて第１の位置及び第２の位置を取得し、ロボットが対象物を第１の位置から第２の位置に搬送するときの搬送条件を取得する。そして、ロボットが対象物を第１の位置から第２の位置に搬送する経路であって、当該経路の全体にわたり、搬送条件を満たす経路を生成する（特許６８６０８６３号公報）。
　しかしながら、特許文献１に記載の技術では、人が動作経路を生成するため、組立作業をロボットにティーチングする際に、専門的な知識を有する人を必要としていた。また、対象物の位置誤差を調整しながら目標の組立状態に到達するような、いわゆる探り動作が発生する動作経路を立てた場合、探る時間が長く、かつ、ばらつくため、タスクの実行が遅く、そのタスクに要する時間も見積もれない、という問題がある。また、特許文献２に記載の技術では、周辺環境との接触を回避した経路を生成するため、対象物同士の組立を対象とした作業の動作経路の生成に適用することはできない。
　本開示は、上記の点に鑑みてなされたものであり、対象物同士の組立作業について存在する経路のうち、探り動作なしで高速に実行可能な動作経路を生成することを目的とする。
　上記目的を達成するために、本開示に係る動作経路生成装置は、ロボットの把持部により把持される主対象物と、組立対象の副対象物との組立動作における、前記把持部の前記主対象物との相対的な開始位置及び姿勢と終了位置及び姿勢と、前記主対象物及び前記副対象物を含む作業環境の形状情報とを取得する取得部と、前記取得部により取得された情報に基づいて、前記主対象物と前記副対象物との接触状態から、前記主対象物と前記副対象物とが非接触状態になるまでの前記主対象物と前記副対象物との接触状態の遷移をシミュレーションするシミュレーション部と、前記取得部により取得された情報と、前記シミュレーション部によりシミュレーションされた前記接触状態の遷移とに基づいて、前記把持部が前記開始位置及び姿勢である初期状態からの前記接触状態の遷移に含まれるいずれかの接触状態に至り、前記いずれかの接触状態を含む前記接触状態の遷移を辿って目標状態に至る前記ロボットの動作経路を生成する生成部と、を含んで構成される。
　また、本開示に係る動作経路生成方法は、取得部が、ロボットの把持部により把持される主対象物と、組立対象の副対象物との組立動作における、前記把持部の前記主対象物との相対的な開始位置及び姿勢と終了位置及び姿勢と、前記主対象物及び前記副対象物を含む作業環境の形状情報とを取得し、シミュレーション部が、前記取得部により取得された情報に基づいて、前記主対象物と前記副対象物との接触状態から、前記主対象物と前記副対象物とが非接触状態になるまでの前記主対象物と前記副対象物との接触状態の遷移をシミュレーションし、生成部が、前記取得部により取得された情報と、前記シミュレーション部によりシミュレーションされた前記接触状態の遷移とに基づいて、前記把持部が前記開始位置及び姿勢である初期状態からの前記接触状態の遷移に含まれるいずれかの接触状態に至り、前記いずれかの接触状態を含む前記接触状態の遷移を辿って目標状態に至る前記ロボットの動作経路を生成する方法である。
　また、本開示に係る動作経路生成プログラムは、コンピュータを、ロボットの把持部により把持される主対象物と、組立対象の副対象物との組立動作における、前記把持部の前記主対象物との相対的な開始位置及び姿勢と終了位置及び姿勢と、前記主対象物及び前記副対象物を含む作業環境の形状情報とを取得する取得部、前記取得部により取得された情報に基づいて、前記主対象物と前記副対象物との接触状態から、前記主対象物と前記副対象物とが非接触状態になるまでの前記主対象物と前記副対象物との接触状態の遷移をシミュレーションするシミュレーション部、及び、前記取得部により取得された情報と、前記シミュレーション部によりシミュレーションされた前記接触状態の遷移とに基づいて、前記把持部が前記開始位置及び姿勢である初期状態からの前記接触状態の遷移に含まれるいずれかの接触状態に至り、前記いずれかの接触状態を含む前記接触状態の遷移を辿って目標状態に至る前記ロボットの動作経路を生成する生成部として機能させるためのプログラムである。
　本開示に係る動作経路生成装置、方法、及びプログラムによれば、対象物同士の組立作業について存在する経路のうち、探り動作なしで高速に実行可能な動作経路を生成することができる。
ロボット制御システム１の概略構成図である。 本実施形態における動作経路の一例を説明するための図である。 ３次元表現のコンフィギュレーション空間の一例を示す図である。 動作経路生成装置のハードウェア構成を示すブロック図である。 動作経路生成装置の機能構成の例を示すブロック図である。 面接触の一例を示す図である。 面接触の一例を示す図である。 面接触の一例を示す図である。 辺接触の一例を示す図である。 辺接触の一例を示す図である。 辺接触の一例を示す図である。 辺接触の一例を示す図である。 辺接触の一例を示す図である。 辺接触の一例を示す図である。 維持変位及び離脱変位を説明するための図である。 第１提示画面の一例を示す図である。 第２提示画面の一例を示す図である。 動作経路生成処理の一例を示すフローチャートである。 分岐木生成処理の一例を示すフローチャートである。 接触状態の維持範囲の限定を説明するための図である。 分岐木生成処理の具体例を説明するための図である。 分岐木生成処理の具体例を説明するための図である。 分岐木生成処理の具体例を説明するための図である。 分岐木を説明するための図である。 維持範囲と分解能との関係を説明するための図である。 維持範囲と分解能との関係を説明するための図である。 環境との接触を考慮しないロボット制御を説明するための図である。 本開示を実装する対象としての力制御系の実現例の一例としてのアドミタンス制御を説明するための図である。 本開示を実装する対象としての力制御系の実現例の一例としてのインピーダンス制御を説明するための図である。
　以下、本開示の実施形態の一例を、図面を参照しつつ説明する。なお、各図面において同一又は等価な構成要素及び部分には同一の参照符号を付与している。また、図面の寸法及び比率は、説明の都合上誇張されており、実際の比率とは異なる場合がある。
　図１に示すように、本実施形態に係るロボット制御システム１は、動作経路生成装置１０と、ロボット制御装置４０と、ロボット４２と、センサ群５０とを含んで構成される。
　ロボット４２は、ロボットアーム４４とハンド部４６とを含む。ハンド部４６は本開示の「把持部」の一例である。ロボットアーム４４は、リンクと、リンク間を接続すると共に、モータの駆動により回転又は直動伸縮するジョイントとを含んで構成される。ロボットアーム４４は、ロボット制御装置４０から出力された指令値にしたがってモータが駆動されて、ジョイントの回転角度又は伸縮状態が変更される。これにより、ハンド部４６が３次元空間において指定された位置、及び指定された姿勢となるように制御される。なお、本実施形態では、水平面内の直交する２方向をＸ軸及びＹ軸、鉛直方向をＺ軸、Ｘ軸周りの回転をφ、Ｙ軸周りの回転をθ、Ｚ軸周りの回転をψとして、位置及び姿勢が表されるものとする。
　ハンド部４６は、ロボットアーム４４の先端に設けられ、主対象物９０Ａを把持可能なツールである。ハンド部４６は、例えば、多関節多指型ロボットハンド、グリッパー型ロボットハンド、吸着パッド等としてよい。
　また、ロボット４２は、好適には、接触による外部環境からの反力によりジョイントやリンクが押し負け、外部環境になじむ振る舞いをするソフトロボットや、そのような性質を備えたハンド部４６を備えたソフトロボットとしてよい。
　センサ群５０は、複数種類のセンサを含み、各センサで取得されたセンサデータは、時系列データとしてロボット制御装置４０へ出力される。なお、図１では、センサ群５０を、ハンド部４６付近に１つのブロックで概念的に表現しているが、センサ群５０に含まれる各センサは、センサの種類及び機能に応じた位置にそれぞれ設けられる。
　各センサとしては、ロボット４２で行う作業に応じて必要なものを備えればよい。ただし、本実施形態では、接触を利用した主対象物９０Ａと副対象物９０Ｂとの組立作業の動作経路に基づいて、ロボット４２の動作が制御される。そのため、主対象物９０Ａと環境との接触時の外力を検知可能なセンサ、例えば力覚センサ、パッドの変形量を計測可能なセンサ（パッドセンサ）等がセンサ群５０には含まれる。また、モータのエンコーダも外力を検知可能なセンサとして利用可能である。この場合、モータのエンコーダの出力値と
モータへの指令値との差分が外力として検知される。センサ群５０には、その他のセンサとして、制御目標に対する主対象物９０Ａの位置及び姿勢に関する相対誤差（以下、「位置姿勢誤差」という）を認識可能なセンサ、例えばビジョンセンサ、近接センサ等が含まれる。なお、パッドセンサは、位置姿勢誤差を認識可能なセンサとしても利用可能である。
　ロボット制御装置４０は、動作経路生成装置１０で生成された動作経路に基づく指令値を生成し、ロボット４２へ出力する。
　ここで、図２を参照して、本実施形態における動作経路の一例について説明する。図２の例は、主対象物９０Ａである円柱状のシャフトを、副対象物９０Ｂである円柱状のリングの穴（挿入位置）に挿入する作業についての動作経路である。図２では、各動作について、主対象物９０Ａと副対象物９０Ｂとの位置関係を示す概略側面図と、その動作時のコンフィギュレーション空間とを並べて図示している。また、図２には、各動作の開始時における主対象物９０Ａであるシャフトの底面右側の位置及び姿勢を表すコンフィギュレーション空間での座標を丸印で示している。
　まず、ハンド部４６により把持された主対象物９０Ａの傾き（θ）が０°で、かつ、主対象物９０Ａと副対象物９０Ｂとが非接触の状態を初期状態とする。動作（１）は、初期状態から、主対象物９０Ａを副対象物９０Ｂの挿入位置の上方へ平行移動させる動作である。動作（２）は、主対象物９０Ａを所定角度（図２の例では、４５°）傾ける動作である。動作（３）は、主対象物９０Ａと副対象物９０Ｂとが接触するまで、主対象物９０Ａを挿入位置に向けて下降させる動作である。動作（３）のコンフィギュレーション空間は、主対象物９０Ａを傾けたことにより、主対象物９０Ａが副対象物９０Ｂの穴の最下部まで到達不可となることを表している。
　動作（４）は、主対象物９０Ａの傾き、及び主対象物９０Ａと副対象物９０Ｂとの接触を維持したまま、主対象物９０Ａの他の箇所が副対象物９０Ｂと接触するまで、主対象物９０Ａを挿入位置方向へ移動させる動作である。動作（５）は、主対象物９０Ａと副対象物９０Ｂとのアライメントを利用して、主対象物９０Ａの傾きを、主対象物９０Ａが副対象物９０Ｂの挿入位置（穴）の内周に沿って下降可能な角度（図２の例では、１０°）まで戻す動作である。動作（６）は、主対象物９０Ａを副対象物９０Ｂの挿入位置（穴）の内周に沿って下方へ移動させる動作である。動作（７）は、主対象物９０Ａを目標の角度ε°（図２の例では、０°）にする動作である。このように、本実施形態では、主対象物９０Ａと環境の１つである副対象物９０Ｂとの接触を利用した動作経路を前提とする。
　なお、図２では、コンフィギュレーション空間を２次元で表しているが、図２におけるコンフィギュレーション空間は、紙面に垂直な方向にθの軸も有している。図３に、３次元表現のコンフィギュレーション空間の一例を示す。図３には、図２の各動作（１）～（７）の開始時における主対象物９０Ａであるシャフトの底面右側の位置及び姿勢を表すコンフィギュレーション空間での座標を丸印で示している。
　動作経路生成装置１０は、シミュレーションにより動作経路を生成し、ロボット制御装置４０へ出力する。図４は、本実施形態に係る動作経路生成装置１０のハードウェア構成を示すブロック図である。図４に示すように、動作経路生成装置１０は、ＣＰＵ（Ｃｅｎｔｒａｌ　Ｐｒｏｃｅｓｓｉｎｇ　Ｕｎｉｔ）１２、メモリ１４、記憶装置１６、入出力Ｉ／Ｆ（Ｉｎｔｅｒｆａｃｅ）１８、入出力装置２０、記憶媒体読取装置２２、及び通信Ｉ／Ｆ２４を有する。各構成は、バス２６を介して相互に通信可能に接続されている。
　記憶装置１６には、後述する動作経路生成処理を実行するための動作経路生成プログラ
ムが格納されている。ＣＰＵ１２は、中央演算処理ユニットであり、各種プログラムを実行したり、各構成を制御したりする。すなわち、ＣＰＵ１２は、記憶装置１６からプログラムを読み出し、メモリ１４を作業領域としてプログラムを実行する。ＣＰＵ１２は、記憶装置１６に記憶されているプログラムに従って、上記各構成の制御及び各種の演算処理を行う。
　メモリ１４は、ＲＡＭ（Ｒａｎｄｏｍ　Ａｃｃｅｓｓ　Ｍｅｍｏｒｙ）により構成され、作業領域として一時的にプログラム及びデータを記憶する。記憶装置１６は、ＲＯＭ（Ｒｅａｄ　Ｏｎｌｙ　Ｍｅｍｏｒｙ）、及びＨＤＤ（Ｈａｒｄ　Ｄｉｓｋ　Ｄｒｉｖｅ）、ＳＳＤ（Ｓｏｌｉｄ　Ｓｔａｔｅ　Ｄｒｉｖｅ）等により構成され、オペレーティングシステムを含む各種プログラム、及び各種データを格納する。
　入出力Ｉ／Ｆ１８は、動作経路生成装置１０とロボット制御装置４０とを接続するためのインタフェースである。動作経路生成装置１０で生成された動作経路は、入出力Ｉ／Ｆ１８を介して、ロボット制御装置４０へ出力される。入出力装置２０は、例えば、キーボードやマウス等の、各種の入力を行うための入力装置、ディスプレイ等の表示装置やプリンタ等の、各種の情報を出力するための出力装置である。出力装置として、タッチパネルディスプレイを採用することにより、入力装置として機能させてもよい。
　記憶媒体読取装置２２は、ＣＤ（Ｃｏｍｐａｃｔ　Ｄｉｓｃ）－ＲＯＭ、ＤＶＤ（Ｄｉｇｉｔａｌ　Ｖｅｒｓａｔｉｌｅ　Ｄｉｓｃ）－ＲＯＭ、ブルーレイディスク、ＵＳＢ（Ｕｎｉｖｅｒｓａｌ　Ｓｅｒｉａｌ　Ｂｕｓ）メモリ等の各種記憶媒体に記憶されたデータの読み込みや、記憶媒体に対するデータの書き込み等を行う。通信Ｉ／Ｆ２４は、他の機器と通信するためのインタフェースであり、例えば、イーサネット（登録商標）、ＦＤＤＩ、Ｗｉ－Ｆｉ（登録商標）等の規格が用いられる。
　次に、本実施形態に係る動作経路生成装置１０の機能構成について説明する。
　図５は、動作経路生成装置１０の機能構成の例を示すブロック図である。動作経路生成装置１０には、開始位置におけるハンド部４６の主対象物９０Ａとの相対的な位置及び姿勢の情報（以下、「開始位置姿勢」という）、及び終了位置におけるハンド部４６の主対象物９０Ａとの相対的な位置及び姿勢の情報（以下、「終了位置姿勢」という）が入力される。また、動作経路生成装置１０には、主対象物９０Ａ及び副対象物９０Ｂを含む作業環境の形状及び配置を示す環境情報が入力される。例えば、主対象物９０Ａ及び副対象物９０Ｂの各々のＣＡＤデータ、障害物が配置された作業環境を表すコンフィギュレーション空間等が環境情報に含まれる。また、動作経路生成装置１０には、ロボット４２の動作の分解能を特定するための情報（以下、「分解能特定情報」という）が入力される。分解能特定情報には、センサ群５０に含まれるセンサの分解能、及びロボット４２を駆動するためのアクチュエータの制御精度の少なくとも一方が含まれる。
　図５に示すように、動作経路生成装置１０は、機能構成として、取得部３２と、シミュレーション部３４と、生成部３６と、表示制御部３８とを含む。各機能構成は、ＣＰＵ１２が記憶装置１６に記憶された動作経路生成プログラムを読み出し、メモリ１４に展開して実行することにより実現される。
　取得部３２は、動作経路生成装置１０に入力された開始位置姿勢、終了位置姿勢、及び環境情報を取得し、シミュレーション部３４へ受け渡す。また、取得部３２は、動作経路生成装置１０に入力された分解能特定情報を取得し、生成部３６へ受け渡す。
　シミュレーション部３４は、取得部３２から受け渡された情報に基づいて、主対象物９
０Ａと副対象物９０Ｂとの接触状態から、主対象物９０Ａと副対象物９０Ｂとが非接触状態になるまでの主対象物９０Ａと副対象物９０Ｂとの接触状態の遷移をシミュレーションする。
　ここで、主対象物９０Ａと副対象物９０Ｂとの接触状態について定義する。主対象物９０Ａの面と副対象物９０Ｂの面とが接触する接触状態を面接触という。また、主対象物９０Ａ及び副対象物９０Ｂの少なくとも一方の辺（又は点）と、主対象物９０Ａ及び副対象物９０Ｂの他方とが接触する接触状態を辺（又は点）接触という。以下では、辺（又は点）の「（又は点）」を省略し、単に「辺」という。また、以下では、Ｘ軸に直交する面を「Ｘ面」、Ｙ軸に直交する面を「Ｙ面」、Ｚ軸に直交する面を「Ｚ面」という。さらに、以下の各図では、面接触の接触箇所を太線で表し、辺接触の接触箇所を黒丸で表す。
　図６Ａ～図６Ｃに、面接触の一例を示す。図６Ａでは、主対象物９０Ａと副対象物９０ＢとがＺ面同士で接触しており、図６Ｂでは、主対象物９０Ａと副対象物９０ＢとがＸ面同士で接触している。このように面接触であっても、接触面が異なる場合、図６Ａと図６Ｂとはそれぞれ異なる接触状態の面接触として扱う。また、図６Ａでは、主対象物９０ＡのＺ面全体が副対象物９０Ｂと接触しているのに対し、図６Ｃでは、Ｚ面の一部のみが接触している。このような場合、図６Ａと図６Ｃとはそれぞれ異なる接触状態の面接触として扱う。
　図７Ａ～図７Ｆに、辺接触の一例を示す。図７Ａ及び図７Ｂは、主対象物９０Ａと副対象物９０Ｂとが１辺で接触している辺接触である。このように、１辺で接触している辺接触を、以下では「１辺接触」という。この例において、図７Ａでは、主対象物９０Ａの下側Ｚ面右（＝右側Ｘ面下）の１辺が、副対象物９０Ｂの上側Ｚ面左（＝左側Ｘ面上）の１辺と接触している。図７Ｂでは、主対象物９０Ａの下側Ｚ面右（＝右側Ｘ面下）の１辺が副対象物９０Ｂの上側Ｚ面と接触している。これらは、それぞれ異なる接触状態の１辺接触として扱う。
　また、図７Ｃ、図７Ｄ、及び図７Ｅは、主対象物９０Ａと副対象物９０Ｂとが２辺で接触している辺接触である。このように、２辺で接触している辺接触を、以下では「２辺接触」という。この例において、図７Ｃでは、主対象物９０Ａの下側Ｚ面左（＝左側Ｘ面下）の１辺が左の副対象物９０Ｂの右側Ｘ面に接触し、右の副対象物９０Ｂの上側Ｚ面左（＝左側Ｘ面上）の１辺が主対象物９０Ａの右側Ｘ面と接触している。また、図７Ｄでは、左の副対象物９０Ｂの上側Ｚ面右（＝右側Ｘ面上）の１辺が主対象物９０Ａの下側Ｚ面に接触し、右の副対象物９０Ｂの上側Ｚ面左（＝左側Ｘ面上）の１辺が主対象物９０Ａの右側Ｘ面と接触している。また、図７Ｅでは、主対象物９０Ａの下側Ｚ面左（＝左側Ｘ面下）の１辺が左の副対象物９０Ｂの上側Ｚ面右（＝右側Ｘ面上）の１辺に接触し、右の副対象物９０Ｂの上側Ｚ面左（＝左側Ｘ面上）の１辺が主対象物９０Ａの右側Ｘ面と接触している。このように、接触している辺の少なくとも１箇所が異なる場合には、それぞれ異なる接触状態の２辺接触として取り扱う。なお、図７Ｆは、主対象物９０Ａと副対象物９０Ｂとが３辺で接触している３辺接触の例である。
　シミュレーション部３４は、目標状態から開始して、主対象物９０Ａと副対象物９０Ｂとの接触状態が、ある方向に力をかけても動かない状態（２辺接触～面接触状態）に達すると、違う方向に力をかけ続け、それにより動かない状態に達した場合、その状態からまた違う方向に力をかけ続ける、ということを繰り返すことで、接触状態の遷移をシミュレーションする。このような接触状態の遷移を探索する場合において考慮すべき、副対象物９０Ｂに対する主対象物９０Ａの変位には、図８に示すような維持変位及び離脱変位がある。維持変位は、同一の接触状態を維持したまま主対象物９０Ａを変位させることである。図８に示すように、維持変位は、面接触のまま主対象物９０Ａを平行移動させる場合、及び１辺接触又は２辺接触を維持したまま主対象物９０Ａを回転させる場合を含む。離脱
変位は、接触状態の自由度が高くなる方向へ主対象物９０Ａを変位させることである。図８に示すように、離脱変位は、面接触している副対象物９０Ｂとの接触面と交差する方向かつ、副対象物９０Ｂと離れる方向へ主対象物９０Ａを平行移動させる場合を含む。また、離脱変位は、面接触又は２辺接触から１辺接触になるように主対象物９０Ａを回転させる場合を含む。
　シミュレーション部３４は、具体的には、上記のように接触状態の遷移を探索しながら、目標状態を根ノードとし、それぞれ異なる接触状態に対応するノードを有する分岐木を生成する。より具体的には、シミュレーション部３４は、２辺接触、３辺接触、若しくは面接触状態のいずれかの状態から、離脱変位の移動を行った直後の状態、又は、その離脱変位の移動を行った方向に対する維持変位を実行し続けて、新しい接触状態に至るか否かをシミュレーションする。また、シミュレーション部３４は、新しい接触状態に至った場合において、新しい接触状態が２辺接触、３辺接触、又は面接触の場合、その接触状態に対して、同じように探索（分岐木の生成）を実行する。また、シミュレーション部３４は、離脱変位の移動を行った直後の接触状態が１辺接触で、その離脱変位の移動を行った方向に対する維持変位を実行し続けて至る新しい状態が何にも接触していない状態になる場合は１辺接触状態だけを分岐木に登録する。シミュレーション部３４は、分岐木のノードの各々に、そのノードに対応する接触状態において、主対象物９０Ａが維持変位可能な範囲（以下、「維持範囲」）を求め、そのノードにさらに対応付ける。シミュレーション部３４は、維持範囲を、平行移動の方向（Ｘ，Ｙ，Ｚ）、及び回転の方向（φ，θ，ψ）の各方向について求める。
　生成部３６は、取得部３２により取得された情報と、シミュレーション部３４によりシミュレーションされた接触状態の遷移とに基づいて、主対象物９０Ａが開始位置姿勢である初期状態からの接触状態の遷移に含まれるいずれかの接触状態に至り、いずれかの接触状態を含む接触状態の遷移を辿って目標状態に至るロボットの動作経路を生成する。具体的には、生成部３６は、分岐木において、ロボット４２の動作の分解能よりも大きな維持範囲が対応付けられたノードを特定する。生成部３６は、複数のノードが特定される場合、分岐木の根ノードから最も近いノードを特定する。そして、生成部３６は、分岐木において、特定したノードから根ノードまでに対応する接触状態の遷移を特定し、特定した接触状態の遷移に対応するロボット４２の経路を生成する。また、生成部３６は、初期状態の位置から、特定したノードに対応する接触状態までの経路であって、周辺の障害物を回避した経路を生成する。そして、生成部３６は、初期状態から、特定したノードに対応する接触状態までの経路と、特定したノードに対応する接触状態から目標状態までの経路とを結合した経路を動作経路として表示制御部３８へ受け渡す。なお、経路とは、ロボット４２のハンド部４６の所定部分（例えば、手先）の位置及び姿勢の系列である。
　また、生成部３６は、表示制御部３８から、後述するユーザ登録位置姿勢を受け渡され、動作経路の再生成を指示された場合、生成済みの動作経路にユーザ登録位置姿勢を反映して、動作経路を再生成する。詳細は後述するが、ユーザ登録位置姿勢は、動作経路内の特定の時間における主対象物９０Ａの位置及び姿勢の修正が登録されたものである。生成部３６は、生成済みの動作経路が、指定された位置及び姿勢を経由するように修正することにより、動作経路を再生成してよい。また、生成部３６は、ユーザ登録位置姿勢に対応する接触状態が対応付けられたノードを分岐木から探索し、探索されたノードに対応する接触状態を経由する接触状態の遷移に基づいて、動作経路を再生成してもよい。
　表示制御部３８は、生成部３６により生成された動作経路を表示装置に表示する。表示制御部３８は、例えば図９に示すような第１提示画面６０を表示装置に表示する。図９の例では、第１提示画面６０には、３次元ビュー６１が含まれる。３次元ビュー６１では、初期状態から目標状態に至る主対象物９０Ａ及び副対象物９０Ｂの３次元位置（Ｘ，Ｙ，
Ｚ）及び姿勢（φ，θ，ψ）が、３次元のアニメーションでプレビューされる。主対象物９０Ａ及び副対象物９０Ｂ以外の環境の３次元画像も合わせて表示してもよい。また、３次元ビュー６１において、３次元ＣＡＤソフトのように、主対象物９０Ａ及び副対象物９０Ｂの位置及び姿勢をマウスで動かせるようにしてよい。
　また、第１提示画面６０は、３次元ビュー６１で表示されるアニメーションの再生時間が表示される時間表示６２を含む。すなわち、３次元ビュー６１で表示されるアニメーションの各コマに対応付けられた時間のうち、３次元ビュー６１に表示されているコマに対応付けられた時間が時間表示６２に表示される。また、時間表示６２に任意の時間を入力し、その時間に対応するコマを３次元ビュー６１に表示するようにしてもよい。
　また、第１提示画面６０は、フェーズ表示６３を含む。フェーズ表示６３は、動作経路が示す動作の開始から終了までの時間経過を示す帯状の表示等の１軸の表示であり、動作経路において主対象物９０Ａと副対象物９０Ｂとの接触状態が変わる時間に対応する位置に第１マーク（図９のフェーズ表示６３における破線）が表示される。図９の例では、接触状態が変わる時間をそれぞれ「フェーズｉ（ｉ＝１，２，・・・）」としている。また、フェーズ表示６３には、時間表示６２に表示されている時間に対応する位置に第２マーク（図９のフェーズ表示６３における実線及び黒三角）が表示される。第２マークをフェーズ表示６３上で移動させることにより、時間表示６２に表示される時間を指定可能である。また、フェーズ表示６３には、後述するユーザ登録位置姿勢を示す第３マーク（図９のフェーズ表示６３における点線）が、ユーザ登録位置姿勢の対象の時間に対応する位置に表示される。
　また、第１提示画面６０は、登録ボタン６４を含む。ユーザは、特定の時間を指定し、その時間における主対象物９０Ａ及び副対象物９０Ｂの位置及び姿勢を３次元ビュー６１に表示し、マウス操作等により主対象物９０Ａの位置及び姿勢を修正したうえで、登録ボタン６４を選択する。これにより、動作経路内の指定された時間に対して、ユーザにより修正された主対象物９０Ａの位置及び姿勢が登録される。この登録を「ユーザ登録位置姿勢」という。また、第１提示画面６０は、再生成ボタン６５を含む。動作経路に対して位置姿勢のユーザ登録が行われて、ユーザにより再生成ボタン６５が選択された場合、表示制御部３８は、ユーザ登録位置姿勢を受け付け、受け付けたユーザ登録位置姿勢を生成部３６へ受け渡し、動作経路の再生成を指示する。
　また、表示制御部３８は、例えば図１０に示すような第２提示画面７０を表示装置に表示する。図１０の例では、第２提示画面７０には、第１提示画面６０と同様に、時間表示６２、フェーズ表示６３、登録ボタン６４、及び再生成ボタン６５が含まれる。また、第２提示画面７０には、３次元ビュー７１が含まれる。３次元ビュー７１には、位置の３自由度（Ｘ，Ｙ，Ｚ）と姿勢の３自由度（φ，θ，ψ）から選択された３変数を３軸とするコンフィギュレーション空間が表示される。第１提示画面６０の３次元ビュー６１と同様に、３次元ビュー７１においてもユーザによる位置姿勢の登録が可能である。ユーザは、３次元ビュー７１に表示されたコンフィギュレーション空間上で、座標点（図１０の３次元ビュー７１内の黒点）を移動させることにより、主対象物９０Ａの位置及び姿勢を修正可能である。また、第２提示画面７０には、３次元ビュー７１に表示するコンフィギュレーション空間の軸に対応させる３変数を選択するための選択領域７２が含まれる。
　表示制御部３８は、表示装置において、第１提示画面６０及び第２提示画面７０をそれぞれ別ウィンドウで同時に表示してよい。また、表示制御部３８は、選択する３変数がそれぞれ異なる第２提示画面７０を、それぞれ別ウィンドウで複数同時に表示してもよい。表示制御部３８は、第１提示画面６０及び第２提示画面７０を複数ウィンドウで表示した場合、各ウィンドウに対応する第１提示画面６０及び第２提示画面７０の時間表示６２を
同期させ、各ウィンドウの表示を連動させてもよい。
　次に、本実施形態に係るロボット制御システム１の作用について説明する。
　図１１は、動作経路生成装置１０のＣＰＵ１２により実行される動作経路生成処理の流れを示すフローチャートである。ＣＰＵ１２が記憶装置１６から動作経路生成プログラムを読み出して、メモリ１４に展開して実行することにより、ＣＰＵ１２が動作経路生成装置１０の各機能構成として機能し、図１１に示す動作経路生成処理が実行される。
　ステップＳ１０で、取得部３２が、動作経路生成装置１０に入力された開始位置姿勢、終了位置姿勢、環境情報、及び分解能特定情報を取得する。
　次に、ステップＳ１２で、シミュレーション部３４が、上記ステップＳ１０で取得された終了位置姿勢及び環境情報に基づいて、主対象物９０Ａと副対象物９０Ｂとが組み立てられた目標状態を特定する。具体的には、シミュレーション部３４は、終了位置姿勢において主対象物９０Ａと副対象物９０Ｂとが組み立てられた状態のうち、面接触している面の数が最も多い状態を目標状態として特定する。なお、シミュレーション部３４は、面接触している面が１つもない場合、辺接触の数が最も多い状態を目標状態として特定する。目標状態の候補が複数ある場合は、どの状態を目標状態として特定してもよい。
　次に、ステップＳ１４で、シミュレーション部３４が、分岐木生成処理を実行する。ここで、図１２を参照して、分岐木生成処理について説明する。
　ステップＳ１４１で、シミュレーション部３４が、主対象物９０Ａが副対象物９０Ｂに接触しているか否かを判定する。接触している場合には、ループ処理Ｌ１へ移行し、接触していない場合には、分岐木生成処理を終了し、リターンする。
　ループ処理Ｌ１は、ステップＬ１Ｓで開始し、現在の接触状態から、面接触又は辺接触を保ったまま、かつ離脱変位となる方向（回転を含む）への接触状態の遷移（以下、「Ｌ１の遷移」という）の各々について、ステップＳ１４２～Ｓ１４５及びループ処理Ｌ２が実行される。そして、ループ処理Ｌ１は、現在の接触状態について過去に探索した方向を除いて遷移可能な方向がなくなった場合に、ステップＬ１Ｅで終了する。
　ステップＳ１４２では、シミュレーション部３４が、Ｌ１の遷移後の接触状態が分岐木に登録済みか否かを判定する。登録済みの場合には、ステップＳ１４６へ移行し、登録されていない場合には、ステップＳ１４３へ移行する。ステップＳ１４３では、シミュレーション部３４が、Ｌ１の遷移後の接触状態の維持範囲を求める。そして、シミュレーション部３４が、分岐木における、Ｌ１の遷移前の接触状態に対応するノードの下流に接続するノードに、Ｌ１の遷移後の接触状態及び維持範囲を対応付けて登録する。
　Ｌ１の遷移後の接触状態の維持範囲を求める際、シミュレーション部３４は、Ｌ１の遷移を戻す方向の遷移を実行した際に、Ｌ１の遷移前と同じ接触状態に戻ることが可能な範囲に維持範囲を限定する。例えば、図１３の（ａ）に示すように、Ｌ１の遷移前の接触状態が、主対象物９０Ａの下側Ｚ面の一部が副対象物９０Ｂに接触している面接触であるとする。その状態から、（ｂ）に示すように、主対象物９０Ａの下側Ｚ面右（＝右側Ｘ面下）と、副対象物９０Ｂの上側Ｚ面との１辺接触を保って主対象物９０Ａを回転させることにより、接触状態を遷移させたとする。このＬ１の遷移後の接触状態から、維持変位として、（ｃ）に示すようにＸ軸方向に主対象物９０Ａを平行移動させるとする。この場合、維持変位ではあるが、（ｃ）の状態からＬ１の遷移である回転を戻した場合、主対象物９０Ａの下側Ｚ面の全面が副対象物９０Ｂに接触する面接触となり、（ａ）とは異なる接触
状態となる。したがって、この場合、（ｃ）の状態は（ｂ）の維持範囲には含まれない。
　次に、ステップＳ１４４で、シミュレーション部３４が、Ｌ１の遷移後の接触状態は１辺接触か否かを判定する。１辺接触の場合にはループ処理Ｌ２へ移行し、１辺接触ではない場合、すなわち２辺以上の辺接触又は面接触の場合には、ステップＳ１４５へ移行する。ステップＳ１４５では、シミュレーション部３４が、Ｌ１の遷移後の接触状態を分岐木生成処理の開始時の接触状態として、分岐木生成処理を再帰的に実行する。
　ステップＳ１４６では、シミュレーション部３４が、Ｌ１の遷移後の接触状態が１辺接触以外の場合、分岐木に既に登録されている、Ｌ１の遷移後の接触状態に対応するノードと、今回登録するノードとで、分岐木の根ノードに近い方のノードに、根ノードから遠い方のノードから葉ノードまでの部分木を付け替える。そして、ループ処理Ｌ２へ移行する。なお、Ｌ１の遷移後の接触状態が１辺接触の場合、本ステップの処理はスキップする。
　ループ処理Ｌ２は、ステップＬ２Ｓで開始し、同じ方向への遷移で新しい面接触又は２辺以上の辺接触となる接触状態の遷移（以下、「Ｌ２の遷移」という）の各々について、ステップＳ１４７～Ｓ１５０が実行される。そして、ループ処理Ｌ２は、新しい面接触又は２辺以上の辺接触に至らなくなった場合に、ステップＬ２Ｅで終了する。
　ステップＳ１４７では、シミュレーション部３４が、Ｌ２の遷移前の接触状態について、上記ステップＳ１４３を実行しているか否かを判定する。実行している場合には、ステップＳ１４９へ移行し、実行していない場合には、ステップＳ１４８へ移行する。ステップＳ１４８では、上記ステップＳ１４３の処理と同様に、Ｌ２の遷移前の接触状態及び維持範囲を分岐木に登録し、ステップＳ１４９へ移行する。ステップＳ１４９では、シミュレーション部３４が、上記ステップＳ１４３の処理と同様に、Ｌ２の遷移後の接触状態及び維持範囲を分岐木に登録する。次に、ステップＳ１５０で、シミュレーション部３４が、Ｌ２の遷移後の接触状態を分岐木生成処理の開始時の接触状態として、分岐木生成処理を再帰的に実行する。
　図１４～図１６に示す具体例を用いて、上記の分岐木生成処理をより詳細に説明する。なお、図１４～図１６では、Ｘ、Ｚ、及びθの方向への遷移を中心に、説明に必要な接触状態のみを図示しており、他の方向への遷移を考慮した接触状態の図示は省略している。
　まず、図１４の（ａ）に示すように、シミュレーション部３４が、主対象物９０Ａの左側Ｘ面及び下側Ｚ面の２面が副対象物９０Ｂと接触している面接触の状態を目標状態として特定し、分岐木生成処理を開始するものとする。次に、シミュレーション部３４は、Ｌ１の遷移として、例えば、主対象物９０Ａを、下側Ｚ面での面接触を保ったままＸ軸方向へ平行移動させる遷移を実行する。この場合におけるＬ１の遷移後の接触状態を（ｂ）に示す。シミュレーション部３４は、目標状態に対応する根ノードの下流に、接触状態（ｂ）及びその維持範囲を対応付けたノードを接続することにより、接触状態（ｂ）を分岐木に登録する（ステップＳ１４３）。接触状態（ｂ）は１辺接触ではないため（ステップＳ１４４で否定判定）、シミュレーション部３４は、接触状態（ｂ）からのＬ１の遷移を実行する（ステップＳ１４５中のループ処理Ｌ１）。
　シミュレーション部３４は、接触状態（ｂ）からのＬ１の遷移として、例えば、主対象物９０Ａを、下側Ｚ面右（右側Ｘ面下）での１辺接触を保ったまま回転させる遷移を実行する。この場合におけるＬ１の遷移後の接触状態を（ｃ）に示す。シミュレーション部３４は、接触状態（ｂ）に対応するノードの下流のノードに、接触状態（ｃ）を登録する（ステップＳ１４３）。接触状態（ｃ）は１辺接触であるため（ステップＳ１４４で肯定判定）、ループ処理Ｌ２が実行される。（ｂ）から（ｃ）への遷移と同じ方向へのＬ２の遷
移を継続すると、（ｄ）に示すように、２辺接触の状態が出現する。Ｌ２の遷移前の接触状態（ｃ）については分岐木へ登録済みであるため（ステップＳ１４７で肯定判定）、シミュレーション部３４は、接触状態（ｃ）に対応するノードの下流のノードに、接触状態（ｄ）を登録する（ステップＳ１４９）。次に、シミュレーション部３４は、接触状態（ｄ）からのＬ１の遷移を実行する（ステップＳ１５０中のループ処理Ｌ１）。
　接触状態（ｄ）から、主対象物９０Ａの右側Ｘ面での１辺接触を保ったままＺ軸方向上方へ平行移動させる遷移をＬ１の遷移として実行した後の接触状態を（ｅ）に示す。接触状態（ｅ）は１辺接触であるため（ステップＳ１４４で肯定判定）、ループ処理Ｌ２へ移行するが、（ｆ）に示すように、Ｌ１の遷移と同じ方向に維持変位させても、新たな面接触又は２辺接触は出現しない。したがって、シミュレーション部３４は、ループ処理Ｌ２を終了し、接触状態（ｄ）を初期の接触状態とする分岐生成処理のループ処理Ｌ１に戻り、（ｄ）から（ｅ）への遷移とは異なる方向へのＬ１の遷移を実行する。
　接触状態（ｄ）から下側Ｚ面右（＝右側Ｘ面下）での１辺接触を保ったまま主対象物９０Ａを回転させる遷移をＬ１の遷移として実行した後の接触状態を（ｇ）に示す。接触状態（ｇ）は接触状態（ｃ）と同様であるため、既に分岐木に登録されており（ステップＳ１４２で肯定判定）、また、１辺接触であるため、ステップＳ１４６がスキップされ、ループ処理Ｌ２へ移行する。そして、Ｌ１の遷移と同じ方向へＬ２の遷移を継続した場合、（ｈ）に示すような２辺接触が出現する。Ｌ２の遷移前の接触状態（ｇ）はステップＳ１４３を経由していないため、分岐木に登録されていない（ステップＳ１４７で否定判定）。そこで、シミュレーション部３４は、接触状態（ｇ）を分岐木に登録したうえで（ステップＳ１４８）、接触状態（ｈ）を分岐木に登録する（ステップＳ１４９）。接触状態（ｈ）について、以下、上記と同様に分岐木生成処理を実行する（ステップＳ１５０中のステップＳ１５０）。
　（ａ）から（ｂ）へのＬ１の遷移について、ステップＳ１４５の分岐木生成処理の再帰的な実行が終了すると、シミュレーション部３４は、（ａ）から（ｂ）へのＬ１の遷移と同じ方向へのＬ２の遷移を実行する。この場合、図１５の（ｉ）に示すように、下側Ｚ面及び右側Ｘ面の２面が接触した新たな面接触が出現する。シミュレーション部３４は、接触状態（ｉ）を分岐木に登録し（ステップＳ１４９）、接触状態（ｉ）を初期の接触状態とする分岐木生成処理を再帰的に実行する（ステップＳ１５０）。
　接触状態（ｉ）から、主対象物９０Ａの右側Ｘ面での面接触を保ったままＺ軸上方へ移動させる遷移をＬ１の遷移として実行した後の接触状態を（ｊ）に示す。シミュレーション部３４は、接触状態（ｊ）及びその維持範囲を対応付けたノードを接続することにより、接触状態（ｊ）を分岐木に登録する（ステップＳ１４３）。接触状態（ｊ）は１辺接触ではないため（ステップＳ１４４で否定判定）、シミュレーション部３４は、接触状態（ｊ）を初期の接触状態とする分岐木生成処理を再帰的に実行する（ステップＳ１４５）。
　接触状態（ｊ）から、主対象物９０Ａの右側Ｘ面での辺接触を保ったまま回転させる遷移をＬ１の遷移として実行した後の接触状態を（ｋ）に示す。接触状態（ｋ）は接触状態（ｅ）と同様であるため、既に分岐木に登録されており（ステップＳ１４２で肯定判定）、また、１辺接触であるため、ステップＳ１４６がスキップされ、ループ処理Ｌ２へ移行する。そして、Ｌ１の遷移と同じ方向へＬ２の遷移を継続した場合、（ｌ）に示すような２辺接触が出現する。Ｌ２の遷移前の接触状態（ｋ）はステップＳ１４３を経由していないため、分岐木に登録されていない（ステップＳ１４７で否定判定）。そこで、シミュレーション部３４は、接触状態（ｋ）を分岐木に登録したうえで（ステップＳ１４８）、接触状態（ｌ）を分岐木に登録する（ステップＳ１４９）。接触状態（ｋ）について、以下、上記と同様に分岐木生成処理を実行する（ステップＳ１５０中のステップＳ１５０）。
　接触状態（ｌ）から、主対象物９０Ａの右側Ｘ面の１辺接触を保ったままＺ軸方向上方かつＸ軸右方向へ平行移動させる遷移をＬ１の遷移として実行した後の接触状態を（ｍ）に示す。接触状態（ｍ）は接触状態（ｅ）と同じであり、Ｌ２の遷移でも新たな面接触又は辺接触は出現しないため、接触状態（ｍ）は分岐木に登録されない。
　また、接触状態（ｌ）から、主対象物９０Ａの左側Ｘ面の１辺接触を保ったまま回転させる遷移をＬ１の遷移として実行した後の接触状態を（ｎ）に示す。シミュレーション部３４は、接触状態（ｎ）を分岐木に登録し（ステップＳ１４３）、ループ処理Ｌ２へ移行する。そして、Ｌ１の遷移と同じ方向へＬ２の遷移を継続した場合、（ｏ）に示すような新たな面接触が出現する。シミュレーション部３４は、接触状態（ｏ）を分岐木に登録し（ステップＳ１４９）、接触状態（ｏ）を初期の接触状態とする分岐木生成処理を再帰的に実行する（ステップＳ１５０）。
　接触状態（ｏ）から、主対象物９０Ａの左側Ｘ面の１辺接触を保ったまま回転させる遷移をＬ１の遷移として実行した後の接触状態を（ｐ）に示す。シミュレーション部３４は、接触状態（ｐ）を分岐木に登録し（ステップＳ１４３）、ループ処理Ｌ２へ移行する。そして、Ｌ１の遷移と同じ方向へＬ２の遷移を継続した場合、図１６の（ｑ）に示すような新たな２辺接触が出現する。シミュレーション部３４は、接触状態（ｑ）を分岐木に登録し（ステップＳ１４９）、接触状態（ｑ）を初期の接触状態とする分岐木生成処理を再帰的に実行する（ステップＳ１５０）。
　接触状態（ｑ）から、主対象物９０Ａの右側Ｘ面の１辺接触を保ったままＺ軸方向上方へ平行移動させる遷移をＬ１の遷移として実行した後の接触状態を（ｒ）に示す。シミュレーション部３４は、接触状態（ｒ）を分岐木に登録し（ステップＳ１４３）、ループ処理Ｌ２へ移行する。（ｓ）に示すように、接触状態（ｒ）からＬ２の遷移では新たな面接触又は辺接触は出現しないため、分岐木へは登録されない。
　また、接触状態（ｑ）から、主対象物９０Ａの左側Ｘ面の１辺接触を保ったままＸ軸左側かつＺ軸上方へ平行移動させる遷移をＬ１の遷移として実行した後の接触状態を（ｔ）に示す。接触状態（ｔ）は接触状態（ｐ）と同様であるため、すでに分岐木に登録されており（ステップＳ１４２で肯定判定）、また、１辺接触であるため、ステップＳ１４６がスキップされ、ループ処理Ｌ２へ移行する。そして、Ｌ１の遷移と同じ方向へＬ２の遷移を継続した場合、新たな面接触又は２辺接触は出現せず、接触なし状態になる。したがって、シミュレーション部３４は、ループ処理Ｌ２を終了し、接触状態（ｑ）を初期の接触状態とする分岐生成処理のループ処理Ｌ１に戻り、（ｑ）から（ｔ）への遷移とは異なる方向へのＬ１の遷移を実行する。接触状態（ｐ）から、Ｘ面左側とＸ面右側（Ｚ面右側）の２辺接触を保ったまま回転させる遷移をＬ１の遷移として実行した後の接触状態を（ｕ）に示す。シミュレーション部３４は、接触状態（ｐ）を分岐木に登録し（ステップＳ１４３）、シミュレーション部３４は、接触状態（ｕ）を初期の接触状態とする分岐木生成処理を再帰的に実行する（ステップＳ１４５）。
　接触状態（ｂ）以降の分岐木生成処理の再帰的な実行が全て終了し、接触状態（ａ）、すなわち目標状態からの分岐木生成処理に戻ると、シミュレーション部３４は、左側Ｘ面の面接触を保ったまま、主対象物９０ＡをＺ軸上方へ移行移動させる遷移をＬ１の遷移として実行する。この場合におけるＬ１の遷移後の接触状態を図１４の（ｗ）に示す。接触状態（ｗ）は、図１５の接触状態（ｏ）と同じである。そこで、シミュレーション部３４は、分岐木の根ノード（接触状態（ａ））に近い方のノードとなる接触状態（ｗ）のノードに、接触状態（ｏ）に対応するノードから葉ノードまでの部分木を、接触状態（ｗ）のノードに付け替える（ステップＳ１４６）。
　上記のような処理により、図１４～図１６に示すような、各接触状態に対応するノードが接続された分岐木が生成される。
　図１７を参照して、上記分岐木生成処理により生成される分岐木について説明する。図１７において、丸は各接触状態に対応するノードを表し、矢印は、上記Ｌ１の遷移又はＬ２の遷移を表す。また、ノードのうち、網掛の丸で表されたノードは、探索により見つかる接触状態（図１４～図１６の（ａ）～（ｗ）の各状態に相当）のうち、１辺接触の状態に対応するノードである。また、白丸のノードは、探索により見つかる接触状態のうち、２辺接触、３辺接触、又は面接触の状態に対応するノードである。
　図１７に示すように、１辺接触の状態から遷移する状態は、必ず１つで、枝分かれしない。すなわち、枝分かれするのは、図１７において白丸のノードに対応する状態のみである。ただし、白丸のノードに対応する状態であっても、状態が枝分かれしない場合もある。一般には、葉ノード（それ以上の遷移がない状態、末端）は、１辺接触の状態になる。ただし、白丸のノードに対応する状態の先にある、網掛のノードに対応する状態が既に分岐木に登録されていた場合に限り、白丸のノードが葉ノードになる。１辺接触の状態から、１辺接触の状態に遷移することはあり得る。また、２辺以上の接触状態（白丸のノードに対応する状態）から２辺以上の接触状態に遷移することはあり得る。
　動作経路生成処理（図１１）に戻って、次のステップＳ１６で、生成部３６が、上記ステップＳ１０で取得された分解能特定情報に基づいて、ロボットの動作の分解能を決定する。具体的には、生成部３６は、センサ群５０に含まれるセンサのうち、主対象物９０Ａと副対象物９０Ｂとが非接触状態の場合において、主対象物９０Ａと副対象物９０Ｂとの位置及び姿勢の誤差を認識可能なセンサの各方向（ｄ＝Ｘ，Ｙ，Ｚ，φ，θ，ψ）の分解能をεｓｄｉとする。ｉは各センサに付与した識別番号である。生成部３６は、例えば、識別番号ｖのビジョンセンサの分解能εｓｄｖを、ロボット制御にフィードバック可能な大きさから決定する。より具体的には、生成部３６は、εｓｄｖ＝（主対象物９０Ａの長辺の長さ）×２／（ビジョンセンサの画角の短辺の画素数）（ｄ＝Ｘ，Ｙ）として決定してよい。これは、主対象物９０Ａの長辺がビジョンセンサの画角の短辺の半分以下になるようにビジョンセンサを利用する場合にビジョンセンサが認識可能な長さとするものである。生成部３６は、そのセンサで認識できない方向の分解能は、取り得る最大の値とし、６次元のベクトルとしてεｓｄｉを求める。そして、生成部３６は、εｓｄ＝ｍｉｎ（εｓｄｉ）をロボットの分解能として決定する。
　次に、ステップＳ１８で、生成部３６が、分岐木において維持範囲が分解能より大きい接触状態を特定する。例えば、生成部３６は、１辺をεｓｄとする領域に内包可能な維持範囲が対応付けられたノードを分岐木から特定する。該当のノードが複数存在する場合には、分岐木の根ノードから最も近いノードを特定する。分岐木の根ノードから最も近いノードは、例えば、根ノードから辿るノードの個数が少ないノードとしてもよいし、物理的な距離を測定して最も近いノードとしてもよい。前者の場合は、制御のロバスト性が高く、後者の場合は、実行時間が短くなるという利点がある。本実施形態では前者を採用するものとする。そして、生成部３６は、特定したノードに対応付けられた接触状態を特定する。
　維持範囲より分解能の方が大きい場合には、非接触状態から、特定した接触状態に到達するために探り動作が必要になる場合がある。例えば、分解能の中心位置にハンド部４６で把持された主対象物９０Ａを移動させるようにロボット４２を制御する場合、図１８に示すように、特定した接触状態の維持範囲に到達できない。一方、図１９に示すように、維持範囲より分解能の方が小さければ、センサにより認識可能な位置及び姿勢の誤差に基
づいて、確実に非接触状態から、特定した接触状態の維持範囲に到達することができる。
　次に、ステップＳ２０で、生成部３６が、特定した接触状態から目標状態までの接触状態の遷移に基づく動作経路を生成する。具体的には、生成部３６は、分岐木において、上記ステップＳ１８で特定した接触状態に対応するノードから根ノードに至るまでの各ノードに対応する接触状態からハンド部４６の位置及び姿勢を特定して並べることで、特定した接触状態から目標状態までの軌跡を生成する。
　次に、ステップＳ２２で、生成部３６が、上記ステップＳ１０で取得された開始位置姿勢から、上記ステップＳ１８で特定した接触状態に対応する位置及び姿勢までの経路であって、障害物を回避する経路を生成する。そして、生成部３６が、上記ステップＳ２０で生成した経路と、本ステップで生成した経路とを結合し、各位置及び姿勢に接触状態を対応付けて動作経路として生成する。
　次に、ステップＳ２４で、表示制御部３８が、上記ステップＳ２２で生成された動作経路に基づいて、第１提示画面６０及び第２提示画面の少なくとも一方を表示装置に表示する。そして、表示制御部３８は、ユーザによる主対象物９０Ａの位置及び姿勢の修正であるユーザ登録位置姿勢を受け付ける。
　次に、ステップＳ２６で、生成部３６が、上記ステップＳ２４でユーザ登録位置姿勢が受け付けられた場合には、上記ステップＳ２４で生成した動作経路に、受け付けられたユーザ登録位置姿勢を反映して動作経路を再生成する。そして、生成部３６が、最終的に生成した動作経路をロボット制御装置４０へ出力し、動作経路生成処理は終了する。
　ロボット制御装置４０は、動作経路生成装置１０から入力された動作経路を実現する力制御のための指令値を生成し、ロボット４２に出力する。
　主対象物９０Ａと環境との接触を考慮しない従来のロボット制御では、図２０に示すように、動作経路に基づく目標位置θｒｅｆから、その目標位置θｒｅｆを実現するためのモータトルクを位置制御器により算出し、ロボットを制御する。そして、制御されたロボットから（バネ性を有する）減速機出力後の関節角度θａｃｔを取得し、位置制御器へフィードバックする。
　図２１に、本開示を実装する対象としての力制御系の実現例の一例としてアドミタンス制御の場合を示す。この場合、動作経路に応じた力目標τｒｅｆから、環境から受ける外部トルクτｅを差し引いた値を力制御器で補正した補正値に、ユーザが指定する目標位置θｒｅｆを加え、そこからモータの現在位置θｍを差し引くことで位置目標値を算出する。そして、その位置目標値を実現するためのモータトルクを位置制御器により算出し、ロボットを制御する。そして、制御されたロボットからモータの現在位置θｍを取得し、位置制御器へフィードバックする。また、ロボットと環境との接触に基づいて（バネ性を有する）減速機出力後の関節角度θａｃｔを取得し、θｍからθａｃｔを差し引いた値を、ロボットのギア及びトルクセンサのバネ性に基づいて、環境から受ける外部トルクτｅに変換し、力制御器へフィードバックする。
　また、図２２に、本開示を実装する対象としての力制御系の実現例の一例としてインピーダンス制御の場合を示す。この場合、動作経路に応じた目標位置θｒｅｆから（バネ性を有する）減速機出力後の関節角度θａｃｔを差し引いた値を位置制御器で補正した補正値に、ユーザが指定する力目標τｒｅｆを加え、そこから環境から受ける外部トルクτｅを差し引くことで力目標値を算出する。そして、その力目標値を実現するためのモータトルクを力制御器により算出し、ロボットを制御する。そして、制御されたロボットからモ
ータの現在位置θｍを取得し、θｍからθａｃｔを差し引いた値を、ロボットのギア及びトルクセンサのバネ性に基づいて、環境から受ける外部トルクτｅに変換し、力制御器へフィードバックする。また、（バネ性を有する）減速機出力後の関節角度θａｃｔを位置制御器へフィードバックする。
　以上説明したように、本実施形態に係るロボット制御システムでは、動作経路生成装置が、ロボットの把持部により把持される主対象物と、組立対象の副対象物との組立動作における、把持部の主対象物との相対的な開始位置及び姿勢と終了位置及び姿勢と、主対象物及び副対象物を含む作業環境の形状情報とを取得する。また、動作経路生成装置が、取得された情報に基づいて、主対象物と副対象物との接触状態から、主対象物と副対象物とが非接触状態になるまでの主対象物と副対象物との接触状態の遷移をシミュレーションする。そして、取得された情報と、シミュレーションされた接触状態の遷移とに基づいて、主対象物が開始位置及び姿勢である初期状態からの接触状態の遷移に含まれるいずれかの接触状態に至り、いずれかの接触状態を含む接触状態の遷移を辿って目標状態に至るロボットの動作経路を生成する。これにより、対象物同士の組立作業について存在する経路のうち、探り動作なしで高速に実行可能な動作経路を生成することができる。
　これにより、ロボットによる作業を高速に実行できる。また、自動で自律的な組立動作をロボットが実行できるようになるため、ティーチング工数が激減する。また、実際の位置姿勢と制御目標との間に位置姿勢誤差があっても、ロボットが自律的に位置及び姿勢を補正して、作業を完遂できるようになる。
　なお、上記実施形態では、終了位置姿勢に対応する状態を目標状態とする場合について説明したが、分岐木を生成する際の目標状態として、中間点の位置姿勢を設定してもよい。この場合、中間点から接触状態を辿って終了位置姿勢に辿り着くことができる場合は、終了位置姿勢の状態が含まれる接触状態の遷移を表す分岐木が生成される。なお、生成された分岐木に終了位置姿勢が含まれない場合は、指定した中間点から、終了位置姿勢に到達できないことを表す。
　なお、上記実施形態では、動作経路生成装置とロボット制御装置とを別々の装置とする場合について説明したが、ロボット制御装置内に動作経路生成装置を組み込む形態としてもよい。また、動作経路生成装置の各機能構成を、それぞれ異なる装置で実現し、上記の動作経路生成処理を分散処理により実現してもよい。
　また、上記実施形態でＣＰＵがソフトウェア（プログラム）を読み込んで実行した動作経路生成処理を、ＣＰＵ以外の各種のプロセッサが実行してもよい。この場合のプロセッサとしては、ＦＰＧＡ（Ｆｉｅｌｄ－Ｐｒｏｇｒａｍｍａｂｌｅ　Ｇａｔｅ　Ａｒｒａｙ）等の製造後に回路構成を変更可能なＰＬＤ（Ｐｒｏｇｒａｍｍａｂｌｅ　Ｌｏｇｉｃ　Ｄｅｖｉｃｅ）、及びＡＳＩＣ（Ａｐｐｌｉｃａｔｉｏｎ　Ｓｐｅｃｉｆｉｃ　Ｉｎｔｅｇｒａｔｅｄ　Ｃｉｒｃｕｉｔ）等の特定の処理を実行させるために専用に設計された回路構成を有するプロセッサである専用電気回路等が例示される。また、動作経路生成処理を、これらの各種のプロセッサのうちの１つで実行してもよいし、同種又は異種の２つ以上のプロセッサの組み合わせ（例えば、複数のＦＰＧＡ、及びＣＰＵとＦＰＧＡとの組み合わせ等）で実行してもよい。また、これらの各種のプロセッサのハードウェア的な構造は、より具体的には、半導体素子等の回路素子を組み合わせた電気回路である。
　また、上記実施形態では、動作経路生成プログラムが記憶装置に予め記憶（インストール）されている態様を説明したが、これに限定されない。プログラムは、ＣＤ－ＲＯＭ、ＤＶＤ－ＲＯＭ、ブルーレイディスク、ＵＳＢメモリ等の記憶媒体に記憶された形態で提供されてもよい。また、プログラムは、ネットワークを介して外部装置からダウンロード
される形態としてもよい。
１ ロボット制御システム
１０ 動作経路生成装置
１２ ＣＰＵ
１４ メモリ
１６ 記憶装置
１８ 入力装置
２０ 出力装置
２２ 記憶媒体読取装置
２４ 通信Ｉ／Ｆ
２６ バス
３２ 取得部
３４ シミュレーション部
３６ 生成部
３８ 表示制御部
４０ ロボット制御装置
４２ ロボット
４４ ロボットアーム
４６ ハンド部
５０ センサ群
６０ 第１提示画面
７０ 第２提示画面
９０Ａ 主対象物
９０Ｂ 副対象物

Claims (12)

  1. 　ロボットの把持部により把持される主対象物と、組立対象の副対象物との組立動作における、前記把持部の前記主対象物との相対的な開始位置及び姿勢と終了位置及び姿勢と、前記主対象物及び前記副対象物を含む作業環境の形状情報とを取得する取得部と、
    　前記取得部により取得された情報に基づいて、前記主対象物と前記副対象物との接触状態から、前記主対象物と前記副対象物とが非接触状態になるまでの前記主対象物と前記副対象物との接触状態の遷移をシミュレーションするシミュレーション部と、
    　前記取得部により取得された情報と、前記シミュレーション部によりシミュレーションされた前記接触状態の遷移とに基づいて、前記把持部が前記開始位置及び姿勢である初期状態からの前記接触状態の遷移に含まれるいずれかの接触状態に至り、前記いずれかの接触状態を含む前記接触状態の遷移を辿って目標状態に至る前記ロボットの動作経路を生成する生成部と、
    　を含む動作経路生成装置。
  2. 　前記シミュレーション部は、前記目標状態から、前記主対象物と前記副対象物との接触状態が、第１の方向に力をかけても動かない状態に達すると、前記第１の方向とは異なる第２の方向に力をかけ続けて動かない状態に達した場合、前記動かない状態から前記第１の方向及び前記第２の方向とは異なる方向に力をかけ続けるというシミュレーションを繰り返すことで、前記接触状態の遷移を探索することを含む請求項１に記載の動作経路生成装置。
  3. 　前記シミュレーション部は、前記接触状態の遷移を順次探索しながら、前記目標状態を根ノードとし、それぞれ異なる接触状態をノードとする分岐木を生成する請求項２に記載の動作経路生成装置。
  4. 　前記シミュレーション部は、前記分岐木のノードの各々に、前記ノードに対応付けられた接触状態を維持したまま前記主対象物が変位可能な維持範囲を対応付ける請求項３に記載の動作経路生成装置。
  5. 　前記取得部は、前記ロボットの動作の分解能を特定するための情報を取得し、
    　前記生成部は、前記分岐木において、前記分解能よりも大きな前記維持範囲が対応付けられたノードに対応付けられた接触状態を、前記いずれかの接触状態として特定する
    　請求項４に記載の動作経路生成装置。
  6. 　前記生成部は、前記いずれかの接触状態が複数特定される場合、前記分岐木の根ノードから最も近いノードに対応けられた接触状態を前記いずれかの接触状態として特定する請求項５に記載の動作経路生成装置。
  7. 　前記ロボットの動作の分解能を特定するための情報は、前記作業環境を検知するセンサの分解能、及び前記ロボットを駆動するためのアクチュエータの制御精度の少なくとも一方を含む請求項５に記載の動作経路生成装置。
  8. 　前記生成部により生成された前記動作経路を表示装置に表示する表示制御部を含む請求項１～請求項７のいずれか１項に記載の動作経路生成装置。
  9. 　前記表示制御部は、表示した前記動作経路に対する修正をユーザから受け付け、
    　前記生成部は、受け付けられた前記修正に基づいて、前記動作経路を再生成する
    　請求項８に記載の動作経路生成装置。
  10. 　前記ロボットは、接触による外部環境からの反力に押し負ける柔軟構造を有するソフトロボットである請求項１～請求項７のいずれか１項に記載の動作経路生成装置。
  11. 　取得部が、ロボットの把持部により把持される主対象物と、組立対象の副対象物との組立動作における、前記把持部の前記主対象物との相対的な開始位置及び姿勢と終了位置及び姿勢と、前記主対象物及び前記副対象物を含む作業環境の形状情報とを取得し、
    　シミュレーション部が、前記取得部により取得された情報に基づいて、前記主対象物と前記副対象物との接触状態から、前記主対象物と前記副対象物とが非接触状態になるまでの前記主対象物と前記副対象物との接触状態の遷移をシミュレーションし、
    　生成部が、前記取得部により取得された情報と、前記シミュレーション部によりシミュレーションされた前記接触状態の遷移とに基づいて、前記把持部が前記開始位置及び姿勢である初期状態からの前記接触状態の遷移に含まれるいずれかの接触状態に至り、前記いずれかの接触状態を含む前記接触状態の遷移を辿って目標状態に至る前記ロボットの動作経路を生成する
    　動作経路生成方法。
  12. 　コンピュータを、
    　ロボットの把持部により把持される主対象物と、組立対象の副対象物との組立動作における、前記把持部の前記主対象物との相対的な開始位置及び姿勢と終了位置及び姿勢と、前記主対象物及び前記副対象物を含む作業環境の形状情報とを取得する取得部、
    　前記取得部により取得された情報に基づいて、前記主対象物と前記副対象物との接触状態から、前記主対象物と前記副対象物とが非接触状態になるまでの前記主対象物と前記副対象物との接触状態の遷移をシミュレーションするシミュレーション部、及び、
    　前記取得部により取得された情報と、前記シミュレーション部によりシミュレーションされた前記接触状態の遷移とに基づいて、前記把持部が前記開始位置及び姿勢である初期状態からの前記接触状態の遷移に含まれるいずれかの接触状態に至り、前記いずれかの接触状態を含む前記接触状態の遷移を辿って目標状態に至る前記ロボットの動作経路を生成する生成部
    　として機能させるための動作経路生成プログラム。
PCT/JP2022/046970 2022-01-28 2022-12-20 動作経路生成装置、方法、及びプログラム Ceased WO2023145309A1 (ja)

Priority Applications (3)

Application Number Priority Date Filing Date Title
CN202280086529.2A CN118450969A (zh) 2022-01-28 2022-12-20 动作路径生成装置、动作路径生成方法和动作路径生成程序
US18/724,598 US20250086341A1 (en) 2022-01-28 2022-12-20 Operation path generation device, method, and program
EP22924158.3A EP4470730A4 (en) 2022-01-28 2022-12-20 DEVICE, METHOD AND PROGRAM FOR GENERING OPERATING PATHS

Applications Claiming Priority (2)

Application Number Priority Date Filing Date Title
JP2022-012154 2022-01-28
JP2022012154A JP7815802B2 (ja) 2022-01-28 2022-01-28 動作経路生成装置、方法、及びプログラム

Publications (1)

Publication Number Publication Date
WO2023145309A1 true WO2023145309A1 (ja) 2023-08-03

Family

ID=87471612

Family Applications (1)

Application Number Title Priority Date Filing Date
PCT/JP2022/046970 Ceased WO2023145309A1 (ja) 2022-01-28 2022-12-20 動作経路生成装置、方法、及びプログラム

Country Status (5)

Country Link
US (1) US20250086341A1 (ja)
EP (1) EP4470730A4 (ja)
JP (1) JP7815802B2 (ja)
CN (1) CN118450969A (ja)
WO (1) WO2023145309A1 (ja)

Families Citing this family (3)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US11580627B2 (en) 2020-01-06 2023-02-14 Assurant, Inc. Systems and methods for automatically grading pre-owned electronic devices
GB2627511B (en) * 2023-02-24 2025-05-07 Airbus Operations Ltd Self aligning system
US20250216835A1 (en) * 2023-12-27 2025-07-03 Assurant, Inc. Methods, systems, apparatuses, and computer program products for following an edge of an object

Citations (6)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
JPH09198121A (ja) * 1996-01-19 1997-07-31 Nippon Telegr & Teleph Corp <Ntt> ロボット教示方法
WO2020194393A1 (ja) 2019-03-22 2020-10-01 オムロン株式会社 ロボットハンド、ロボットハンド制御方法及びプログラム
WO2021033471A1 (ja) * 2019-08-22 2021-02-25 オムロン株式会社 制御装置、制御方法、及び制御プログラム
JP6860863B2 (ja) 2017-07-05 2021-04-21 オムロン株式会社 経路出力方法
JP2021082222A (ja) * 2019-11-22 2021-05-27 キヤノン株式会社 情報処理方法、ロボットシステム、物品の製造方法、および情報処理装置
JP2021169149A (ja) * 2020-04-16 2021-10-28 ファナック株式会社 分解ベースのアセンブリ計画

Family Cites Families (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
JP6879464B2 (ja) * 2017-08-02 2021-06-02 オムロン株式会社 干渉判定方法、干渉判定システム及びコンピュータプログラム
EP3530418A1 (de) * 2018-02-21 2019-08-28 Siemens Aktiengesellschaft Verfahren und vorrichtung zum bestimmen eines optimierten bewegungsablaufs einer robotereinrichtung

Patent Citations (6)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
JPH09198121A (ja) * 1996-01-19 1997-07-31 Nippon Telegr & Teleph Corp <Ntt> ロボット教示方法
JP6860863B2 (ja) 2017-07-05 2021-04-21 オムロン株式会社 経路出力方法
WO2020194393A1 (ja) 2019-03-22 2020-10-01 オムロン株式会社 ロボットハンド、ロボットハンド制御方法及びプログラム
WO2021033471A1 (ja) * 2019-08-22 2021-02-25 オムロン株式会社 制御装置、制御方法、及び制御プログラム
JP2021082222A (ja) * 2019-11-22 2021-05-27 キヤノン株式会社 情報処理方法、ロボットシステム、物品の製造方法、および情報処理装置
JP2021169149A (ja) * 2020-04-16 2021-10-28 ファナック株式会社 分解ベースのアセンブリ計画

Non-Patent Citations (1)

* Cited by examiner, † Cited by third party
Title
See also references of EP4470730A4

Also Published As

Publication number Publication date
CN118450969A (zh) 2024-08-06
JP2023110601A (ja) 2023-08-09
EP4470730A4 (en) 2025-12-31
EP4470730A1 (en) 2024-12-04
JP7815802B2 (ja) 2026-02-18
US20250086341A1 (en) 2025-03-13

Similar Documents

Publication Publication Date Title
WO2023145309A1 (ja) 動作経路生成装置、方法、及びプログラム
US20200338730A1 (en) Trajectory planning device, trajectory planning method and program
Palmer et al. Real-time method for tip following navigation of continuum snake arm robots
US9186792B2 (en) Teaching system, teaching method and robot system
JP4413891B2 (ja) シミュレーション装置およびシミュレーション方法並びにシミュレーションプログラム
US20240351205A1 (en) Command value generating device, method, and program
CN112584990A (zh) 控制装置、控制方法以及控制程序
JP2017094406A (ja) シミュレーション装置、シミュレーション方法、およびシミュレーションプログラム
JP6915441B2 (ja) 情報処理装置、情報処理方法、および情報処理プログラム
JP7052250B2 (ja) 情報処理装置、情報処理方法、および情報処理プログラム
JP6972873B2 (ja) 情報処理装置、情報処理方法、および情報処理プログラム
Wang et al. Learning robotic insertion tasks from human demonstration
JP7331616B2 (ja) 回避軌道生成装置、方法、及びプログラム
JP2018176333A (ja) 接触センサによるワーク位置検出プログラム生成機能を備えたオフラインプログラミング装置及び方法
US20210129331A1 (en) Control method, control apparatus, robot apparatus, method of manufacturing an article, motion program creation method, motion program creation apparatus, display apparatus, and control program recording medium
WO2021250923A1 (ja) ロボットシステム、制御装置、及び制御方法
JPWO2017175340A1 (ja) 最適化装置及びそれを備えた垂直型多関節ロボット
WO2018180298A1 (ja) ロボット教示装置、ロボット教示装置の制御方法及びロボット教示プログラム
JP7335096B2 (ja) ロボットシステムの障害物探索装置
JP7424122B2 (ja) シミュレーション装置およびプログラム
JP2011140085A (ja) 制御装置、ロボット、ロボットシステム及びロボットの追従制御方法
KR102745540B1 (ko) 로봇용 주행 대차 위치 결정 장치, 해당 방법 및 해당 프로그램
JP4595042B2 (ja) ３次元測定方法およびシステム並びにマニピュレータの制御方法および装置
CN114728411B (zh) 控制装置、机器人系统和控制方法
CN113302026B (zh) 两个机器人机械手的路径的协调

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: 22924158

Country of ref document: EP

Kind code of ref document: A1

WWE Wipo information: entry into national phase

Ref document number: 202280086529.2

Country of ref document: CN

NENP Non-entry into the national phase

Ref country code: DE

ENP Entry into the national phase

Ref document number: 2022924158

Country of ref document: EP

Effective date: 20240828

WWP Wipo information: published in national office

Ref document number: 18724598

Country of ref document: US