ROBOT CONTROL DEVICE, ROBOT CONTROL METHOD, AND ROBOT CONTROL PROGRAM

Abstract

To provide a device, a method and a program for controlling a robot so that a manipulation object is manipulated with good accuracy.SOLUTION: In this controller, an observation update unit 152 of an execution unit 132 preferentially increases a weight of a particle, over other particles, that represents a position-attribute that are close to a position-attitude of a manipulation object indicated by observation results and preferentially increases the weight of a particle in correspondence to closeness to a state of being in contact with the manipulating object. A state estimation unit 154 calculates an estimated state being the position-attitude of the manipulating object having been estimated on the basis of an aggregate of particles, the weight of each of which has been adjusted. An action planning unit 144 plans an action for causing the manipulating object to move from the estimated state to a target state for current movement, and a command conversion unit 146 directs a robot to execute the planned action. An iteration assessment unit 156 causes setting the target state, acquiring observation results, setting a particle aggregate, adjusting the particle aggregate, calculating the estimated state, planning an action, and executing the action to be repeated until the estimated state matches a completion state within a prescribed error.SELECTED DRAWING: Figure 7

Description

[Technical field]

[0001] The present disclosure relates to a robot control device, a robot control method, and a robot control program. [Background technology]

[0002] Conventionally, when manipulating the position and orientation of an object using a robot, the conditions were that the relative position and orientation between the end effector and the object being held was known and did not change during manipulation, and that the value of the target position and orientation of the object in the robot coordinate system was known. In robot control under the above conditions, the robot does not pay attention to the position and orientation of the object being held during control, and brings the object's position and orientation to the target by performing a set operation.

[0003] In addition, there is a conventional method for controlling a robot based on the position and orientation of an object, which is recognized by visual observation using a camera. In this case, it is also possible to recognize the value of the target position and orientation in the robot coordinate system by recognizing the position and orientation of the object to be assembled.

[0004] There is also technology related to generating motions for robots performing assembly work. For example, a method of using machine learning to make a robot learn the work it will perform is being researched (see Non-Patent Document 1). This technology proposes a method of making a robot learn the motions to grasp an object by reinforcement learning based on image data obtained from a camera. This method makes it possible to automate at least a part of the series of processes for teaching a robot the motions to grasp an object. [Prior art documents] [Non-patent literature]

[0005] [Non-Patent Document 1] Dmitry Kalashnikov, et al. "QT-Opt: Scalable Deep Reinforcement Learning for Vision-Based Robotic Manipulation" arXiv preprint arXiv:1806.10293, 2018. Summary of the Invention [Problem to be solved by the invention]

[0006] However, in reality, the relative position and orientation between the end effector and the grasped object is uncertain and may change during the operation. Moreover, the value of the target position and orientation of the object in the robot coordinate system is uncertain. In particular, when the positions and orientations of the object to be grasped and the object to be assembled are not determined with high accuracy by a jig, these uncertainties become large. Therefore, in the conventional technology, even if the robot itself operates according to a determined operation, the position and orientation of the grasped object does not reach the target position and orientation, and there is a problem that the operation of grasping an object and assembling it to another object may fail.

[0007] Furthermore, in camera-based recognition methods, errors occur in the sensor values ​​due to factors such as imager quantization, fluctuations in the number of photons incident on one pixel, and lighting changes, so the recognized position and orientation also contain errors.As a result, even if a robot is controlled based on the recognized position and orientation, it is difficult to control the robot so that the position and orientation of an object reaches the target state.

[0008] The present disclosure has been made in consideration of the above circumstances, and aims to provide a robot control device, a robot control method, and a robot control program for controlling a robot to operate an object with precision. [Means for solving the problem]

[0009] A robot control device according to a first aspect of the present disclosure is a robot control device that controls a robot that manipulates a manipulation object to transition from a state in which the manipulation object, which is an object to be manipulated, is away from a target object located in an environment to a completed state in which the manipulation object is in contact with the target object in a specific manner, the robot control device including a goal state setting unit that sets an intermediate goal state that is a goal state on the way of the movement of the manipulation object until the completed state is reached, or the completed state, as a goal state of a current movement; an observation unit that acquires observation results by a sensor regarding the position and orientation of the manipulation object and the presence or absence of contact between the manipulation object and the target object; a particle set setting unit that sets a particle set representing uncertainty of the position and orientation of the manipulation object, wherein each particle included in the particle set represents one of the possible positions and orientations of the manipulation object; and a particle set setting unit that sets a particle set that is closer to the position and orientation of the manipulation object represented by the observation results, the particle set being weighted more heavily. and when the observation results indicate that contact has occurred, the robot is configured to increase the weight of the corresponding particle the closer the state is to a state in which the target object and the operating object arranged in the position and attitude represented by each of the particles in a virtual space in which the shapes and relative positional relationship of the target object and the operating object are represented, a state estimation unit calculates an estimated state, which is the position and attitude of the operating object estimated based on the particle set in which the weight of each of the particles has been adjusted, an action planning unit plans an action for moving the operating object from the estimated state to a target state of the current movement, an action unit commands the robot to execute the planned action, and a processing control unit repeats the steps of setting the target state, obtaining the observation results, setting the particle set, adjusting the particle set, calculating the estimated state, planning the action, and executing the action until the estimated state matches the completed state within a predetermined error.

[0010] A robot control method according to a second aspect of the present disclosure is a robot control method for controlling a robot that manipulates a manipulation object to transition from a state in which the manipulation object, which is an object to be manipulated, is away from a target object located in an environment to a completed state in which the manipulation object is in contact with the target object in a specific manner, the method comprising the steps of: setting an intermediate target state that is a target state on the way of the movement of the manipulation object to the completed state, or the completed state, as a target state of a current movement; acquiring observation results by a sensor regarding a position and orientation of the manipulation object and the presence or absence of contact between the manipulation object and the target object; setting a set of particles that represent uncertainty of the position and orientation of the manipulation object, each of the particles included in the set of particles representing one of the possible positions and orientations of the manipulation object; and weighting the particles so that the particles that represent a position and orientation closer to the position and orientation of the manipulation object represented by the observation results are weighted accordingly. and, if the observation results indicate that contact has occurred, increasing the weight of the corresponding particle the closer the state is to a state in which the target object and the manipulable object arranged at the position and attitude represented by each of the particles in a virtual space in which the shapes and relative positional relationship of the target object and the manipulable object are represented, calculating an estimated state which is the position and attitude of the manipulable object estimated based on the set of particles with the weight of each of the particles adjusted, planning an action for moving the manipulable object from the estimated state to a target state of the current movement, instructing the robot to execute the planned action, and causing a computer to execute a process of repeating the steps of setting the target state, obtaining the observation results, setting the set of particles, adjusting the set of particles, calculating the estimated state, planning the action, and executing the action until the estimated state matches the completed state within a specified error.

[0011] A robot control program according to a third aspect of the present disclosure is a robot control program for controlling a robot that manipulates a manipulation object to transition from a state in which the manipulation object, which is an object to be manipulated, is away from a target object located in an environment to a completed state in which the manipulation object is in contact with the target object in a specific manner, the robot control program sets an intermediate target state that is a target state on the way of the movement of the manipulation object to reach the completed state, or the completed state, as a target state of a current movement, obtains observation results by a sensor regarding a position and orientation of the manipulation object and the presence or absence of contact between the manipulation object and the target object, sets a set of particles that represent uncertainty of the position and orientation of the manipulation object, each of the particles included in the set of particles representing one of the possible positions and orientations of the manipulation object, and assigns a greater weight to particles that represent a position and orientation closer to the position and orientation of the manipulation object indicated by the observation results. and when the observation results indicate that contact has occurred, increasing the weight of the corresponding particle the closer the state is to a state in which the target object and the manipulated object arranged at the position and attitude represented by each of the particles in a virtual space in which the shapes and relative positional relationship of the target object and the manipulated object are represented, calculating an estimated state which is the position and attitude of the manipulated object estimated based on the set of particles in which the weight of each of the particles has been adjusted, planning an action for moving the manipulated object from the estimated state to a target state for the current movement, instructing the robot to execute the planned action, and repeating the steps of setting the target state, obtaining the observation results, setting the set of particles, adjusting the set of particles, calculating the estimated state, planning the action, and executing the action until the estimated state matches the completed state within a specified error. Effect of the Invention

[0012] According to the robot control device, the robot control method, and the robot control program of the present invention, it is possible to control a robot so as to operate an object with high precision. [Brief description of the drawings]

[0013] [Figure 1] FIG. 2 is a diagram showing the configuration of a control system for controlling a robot according to the present embodiment. [Diagram 2] FIG. 1 is a diagram showing a schematic configuration of a robot as an example of an object to be controlled. [Diagram 3] FIG. 2 is a block diagram showing the hardware configuration of the motion planning device and a control device. [Figure 4] 2 is a block diagram showing an example of a functional configuration of a motion planning apparatus and a control apparatus. FIG. [Diagram 5] FIG. 1 is a diagram showing an example of an assembly procedure using parts A to D. [Figure 6] FIG. 11 is a diagram illustrating an example of a relative position of gripping data. [Figure 7] 2 is a block diagram showing an example of a functional configuration of an execution unit of the control device. FIG. [Figure 8] FIG. 13 is a diagram for explaining a method for generating a guide. [Figure 9] FIG. 2 is a sequence diagram showing a processing flow of the control system according to the present embodiment. [Figure 10] 4 is a flowchart showing a flow of control processing of the control device. DETAILED DESCRIPTION OF THE PREFERRED EMBODIMENTS

[0014] Hereinafter, an example of an embodiment of the present disclosure will be described with reference to the drawings. In each drawing, the same or equivalent components and parts are given the same reference numerals. In addition, the dimensional ratios of the drawings are exaggerated for the convenience of explanation and may differ from the actual ratios.

[0015] Fig. 1 is a diagram showing the configuration of a control system for controlling a robot according to this embodiment. As shown in Fig. 1, the control system 1 includes a robot 10, a state observation sensor 14, a contact observation sensor 16, a motion planning device 20, a state transition control unit 25, and a control device 30. The state transition control unit 25 may be a part of the motion planning device 20 or a part of the control device 30, or may be configured as a device independent of both the motion planning device 20 and the control device 30 as in this embodiment.

[0016] (robot) 2 and 3 are diagrams showing a schematic configuration of a robot 10. The robot 10 in this embodiment is a six-axis vertical articulated robot, and an end effector 12 is provided at the tip 11a of an arm 11 via a flexible part 13. The robot 10 grasps a part with the end effector 12 to perform an assembly operation. In the example of FIG. 3, the end effector 12 is configured to have a pair of hands of a clamping part 12a, but the end effector 12 may be a suction pad to suck a part. In the following description, gripping a part includes suction of the part. Holding a part may be rephrased as gripping and suctioning a part.

[0017] As shown in FIG. 2, the robot 10 has an arm 11 with six degrees of freedom having joints J1 to J6. Each of the joints J1 to J6 connects links to each other so that they can rotate in the directions of the arrows C1 to C6 by a motor (not shown). A gripper is connected to the tip of the arm 11 as an end effector 12. Here, a vertical multi-joint robot is taken as an example, but a horizontal multi-joint robot (SCARA robot) may also be used. Also, a six-axis robot is taken as an example, but a multi-joint robot with other degrees of freedom such as a five-axis or seven-axis robot may also be used, or a parallel link robot may also be used.

[0018] (Status Observation Sensor) The state observation sensor 14 observes the state of the robot 10 and outputs the observed data as state observation data. For example, an encoder for the joints of the robot 10, a visual sensor (camera), motion capture, a force-related sensor, etc. are used as the state observation sensor 14. The position and posture of the tip 11a of the arm 11 can be identified from the angle of each joint as the state of the robot 10, and the posture of the part (work object) can be estimated from the visual sensor and / or the force-related sensor. When a marker for motion capture is attached to the end effector 12, the position and posture of the end effector 12 can be identified as the state of the robot 10, and the posture of the part (work object) can be estimated from the position and posture of the end effector 12.

[0019] The force-related sensor is a general term for a force sensor and a torque sensor, and also includes a tactile sensor when the sensor is provided at a portion that comes into contact with a part. The force-related sensor may be provided on the surface of the part where the end effector 12 grips a part or on a joint part within the end effector 12 so as to detect the force that the end effector of the robot 10 receives from the part. The force-related sensor is, for example, a sensor that detects a one-element or multiple-element force of one axis, three axes, or six axes as the state of the robot 10. By using the force-related sensor, it is possible to grasp more accurately how the end effector 12 grips the part, i.e., the attitude of the part, and to perform appropriate control.

[0020] In addition, the position and posture of the end effector 12 itself and the part being gripped by the end effector 12 can also be detected as the state of the robot 10 by a visual sensor.

[0021] In this way, the states of the end effector 12 and the gripped part can be detected by the various sensors that are the state observation sensor 14. In addition, the detection results of the various sensors can be obtained as state observation data.

[0022] (Contact Observation Sensor) The contact observation sensor 16 is a pressure sensor, a force sensor, or a tactile sensor. The tactile sensor is a sensor that detects pressure distribution, or a sensor that can detect forces in three orthogonal axial directions and moments around three orthogonal axes. The pressure sensor or tactile sensor is provided, for example, in a portion of the fingers of the end effector 12 that contacts an object to be grasped. The force sensor is provided, for example, in a wrist portion between the arm 11 and the end effector 12 of the robot 10.

[0023] (Motion planning device / control device) Next, the configurations of the motion planning device 20 and the control device 30 will be described.

[0024] FIG. 3 is a block diagram showing the hardware configuration of the motion planning device 20 and the control device 30 according to this embodiment. The motion planning device 20 and the control device 30 can be realized with the same hardware configuration. The motion planning device 20 has a CPU (Central Processing Unit) 20A, a ROM (Read Only Memory) 20B, a RAM (Random Access Memory) 20C, a storage 20D, an input unit 20E, a display unit 20F, and a communication interface (I / F) 20G. Each component is connected to be able to communicate with each other via a bus 20H. The control device 30 has a CPU 30A, a ROM 30B, a RAM 30C, a storage 30D, an input unit 30E, a display unit 30F, and a communication I / F 30G. Each component is connected to be able to communicate with each other via a bus 30H. The case of the motion planning device 20 will be described below.

[0025] In this embodiment, programs are stored in ROM 20B or storage 20D. CPU 20A is a central processing unit that executes various programs and controls each configuration. That is, CPU 20A reads out programs from ROM 20B or storage 20D and executes the programs using RAM 20C as a working area. CPU 20A controls each of the above configurations and performs various arithmetic processing according to the programs recorded in ROM 20B or storage 20D.

[0026] The ROM 20B stores various programs and various data. The RAM 20C temporarily stores programs or data as a working area. The storage 20D is configured with an HDD (Hard Disk Drive), SSD (Solid State Drive), or flash memory, and stores various programs including an operating system, and various data.

[0027] The input unit 20E includes a keyboard and a pointing device such as a mouse, and is used to perform various inputs. The display unit 20F is, for example, a liquid crystal display, and displays various information. The display unit 20F may adopt a touch panel system and function as the input unit 20E.

[0028] The communication interface (I / F) 20G is an interface for communicating with other devices, and uses standards such as Ethernet (registered trademark), FDDI, or Wi-Fi (registered trademark).

[0029] 4 is a block diagram showing an example of a functional configuration of the motion planning device 20 and the control device 30. The motion planning device 20 and the control device 30 may be configured as an integrated unit.

[0030] As shown in Fig. 4, the motion planning device 20 has, as its functional components, a transition creation unit 110 and a grip data identification unit 112. Each functional component of the motion planning device 20 is realized when the CPU 20A reads out a program stored in the ROM 20B or the storage 20D, expands it in the RAM 20C, and executes it. The control device 30 has, as its functional components, an acquisition unit 130 and an execution unit 132. Each functional component of the control device 30 is realized when the CPU 30A reads out a program stored in the ROM 30B or the storage 30D, expands it in the RAM 30C, and executes it.

[0031] In this embodiment, an assembly process performed by the robot 10 is represented by a state transition diagram. The entire state transition diagram is broken down into element units, that is, parts or assemblies, and data representing the transitions of the elements is represented as unit state transition data.

[0032] The motion planning device 20 creates each piece of unit state transition data as a motion plan, and accepts registration information required for creating the unit state transition data by input from the user. The registration information includes various information such as part information (part ID, type, etc.), CAD information for each type of end effector 12, assembly procedures (including disassembly procedures), and part grip positions (including postures). The assembly procedures include the relative trajectories of the parts recorded during assembly (or disassembly) in the simulation.

[0033] The transition creation unit 110 creates state transition data including each of the unit state transition data. Each of the unit state transition data is created from an assembly procedure in the registered information.

[0034] For example, the transition creation unit 110 creates unit state transition data representing a transition from a state in which a first element and a second element exist independently to a state in which a third element, which is an assembly of the first element and the second element, is assembled. The first element is a part or an assembly. The second element is also a part or an assembly.

[0035] Before describing the grip data specification unit 112, a specific example of an assembly procedure corresponding to the state transition data will be described. FIG. 5 is a diagram showing an example of an assembly procedure using parts A to D. When applied to state transitions, each of (1) to (3) corresponds to an initial state included in the unit state transition data. When the work process of assembling parts A and B in (1) is applied to the above-mentioned unit state transition data, parts A and B correspond to the first element and the second element, respectively, and the assembly (2) in which parts A and B are assembled corresponds to the third element. The third element is a target state included in the unit state transition data with (1) as the initial state. (2) and onwards can also be similarly applied to the unit state transition data. Below, the grip data specification unit 112 will be described using the assembly procedure of parts A to D as an example.

[0036] The grip data identifying unit 112 identifies grip data for each of the unit state transition data based on the grip position of the part in the registration information. The grip data identifying unit 112 identifies grip data when the end effector 12 of the robot 10 grips the first element or the second element, which is the grip target, for assembly. The grip data is a planned value of the relative position and posture of the end effector 12 and the grip target when the end effector 12 grips the grip target. The relative position and posture in the grip data will be described below. Here, the planned value of the relative position and posture is the planned value of the grip position and grip posture when the end effector 12 grips the element. Moreover, the other element with respect to the element set as the grip target becomes the target object. Moreover, the position and posture are expressed with six degrees of freedom.

[0037] Fig. 6 is a diagram showing an example of the relative position and orientation of the gripping data. Fig. 6(A) shows the relative position and orientation R1 of the end effector 12 with respect to part A as an arrow when the end effector 12 is an suction pad. Fig. 6(A) shows the relative position and orientation R1 of the end effector 12 with respect to part A when the end effector 12 is an suction pad. Fig. 6(B) shows the relative position and orientation R2 of the end effector 12 with respect to part B when the end effector 12 is a gripper (hand) equipped with a pair of clamping parts. Parts C and D are similarly shown.

[0038] The relative position and orientation can be calculated using the following formula:

[0039] (T obj→tcp ) -1 =T tcp→obj

[0040] The left side represents the part coordinate system, and the right side represents the relative position and orientation (position and orientation) from the end effector 12. The gripping data is calculated, for example, as planned values ​​of the relative position and orientation as a target for the gripping object and CAD data with an identification ID of the end effector 12 used during gripping. The planned values ​​of the relative position and orientation may be input by a user who is an administrator, by including them in the gripping position of the part in the registered information (the position on the surface of the part to be gripped), or may be automatically calculated by an existing method such as a gripping plan.

[0041] The motion planning device 20 outputs the created unit state transition data and grip data, which is control data, to the state transition control unit 25. One state transition represented by the unit state transition data is also called a task. Note that instead of outputting from the motion planning device 20 every time a task is updated, all unit state transition data and control data included in the state transition data may be output to the state transition control unit 25, and the state transition control unit 25 may manage which unit state transition data to output. Also, the grip data may be output directly to the control device 30 without going through the state transition control unit 25, and managed on the control device 30 side.

[0042] The state transition control unit 25 outputs unit state transition data corresponding to the task to be processed, out of the state transition data, to the execution unit 132 of the control device 30. The processing target is specified at the start of an assembly operation, and is updated every time a notification of task completion is received from the control device 30. The state transition control unit 25 may be included in the motion planning device 20, or may be included in the control device 30, or may be a device different from either of them. Also, the entire control system including the motion planning device 20, the control device 30, and the state transition control unit 25 may be a single device.

[0043] Hereinafter, each processing unit of the control device 30 will be described.

[0044] The acquisition unit 130 acquires unit state transition data and grip data to be processed from the state transition control unit 25. The acquisition unit 130 also acquires state observation data that observes the positions of the first element and the second element from the state observation sensor 14. Note that the positions of the first element and the second element include their orientations.

[0045] The acquisition unit 130 also acquires contact observation data, which is a sensor value obtained from the contact observation sensor 16 .

[0046] The execution unit 132 executes a task using the unit state transition data output from the state transition control unit 25. The task is, for example, a task of completing a state in which a third element is assembled in the unit state transition data. By executing the task, the execution unit 132 causes the end effector 12 to grasp a grasp object, which is one of the first element and the second element, based on the observation data and the grasping data, and moves the grasp object relatively to the other element along a target relative trajectory that includes a guide, which will be described later, in its final portion.

[0047] A specific method for executing a task by the execution unit 132 will be described below.

[0048] Here, the problem of manipulating the relative position and orientation of two objects by the motion of the robot 10 will be considered.

[0049] Here, an object located in the environment is called the target object, and an object that the robot 10 manipulates with the end effector 12 is called the manipulated object. As a prerequisite, CAD models of both objects are given, both objects are rigid bodies, and there is no error between the CAD models and the actual object shapes. It is also prerequisite that the position and orientation of the target object do not change due to any external force. The relative position and orientation, which is the completed state of the target object and the manipulated object, is given as unit state transition data of the task.

[0050] In the case of a task of assembling a grasped part to another part, the target object is the part to be attached to, and the manipulation object is the grasped part. The completion state is the relative position and orientation of the grasped part when it is attached to the destination.

[0051] In the case of a task of moving a grasped part to another position in the environment, the target object is a landmark in the environment, the manipulation object is the grasped part, and the completion state is the relative position and orientation of the desired destination position from the landmark.

[0052] The relative position and orientation of the target object and the operation object are recognized by visual observation using the state observation sensor 14. For example, the position and orientation of the target object and the operation object in the respective camera coordinate systems are recognized by CAD matching using an image obtained from the camera, which is the state observation sensor 14, and the respective CAD models. Using the two positions and orientations, the position and orientation of the CAD origin of the operation object as viewed from the CAD origin of the target object is set as the relative position and orientation. As described above, since the positions and orientations of the respective objects observed in the camera coordinate system have errors, the obtained relative position and orientation also contains errors. The deviation between the true value and the observed value of the relative position and orientation is called the observation noise of the system. Since there is no means of knowing the true value of the relative position and orientation, the observation noise is also conceptual and its value cannot be obtained. The observed value of the relative position and orientation of the target object and the operation object may be obtained by processing an image captured by a camera, or may be obtained by calculation based on the angles of each joint of the robot 10 measured by an encoder after the robot 10 grasps the operation object if calibration has been performed with the observed value by the camera.

[0053] Furthermore, contact between the target object and the operation object is recognized by a pressure sensor, force sensor, or tactile sensor, which is the contact observation sensor 16. In this embodiment, it is assumed that the contact observation value of the contact observation sensor 16 is free of errors and that it is possible to accurately observe whether or not contact has occurred.

[0054] When the end effector 12 grasps an object, and the robot 10 is controlled to change the relative position and orientation by moving the object by a commanded amount of movement, the change in the relative position and orientation, which is the amount by which the object actually moves, differs from the commanded amount of movement. For example, an error in the mounting position of the robot 10 or an error in the calculation of forward kinematics may cause an error in the amount of movement of the TCP (tool center point). In actual situations where the robot 10 is used, a certain amount of pre-calibration is performed, so the magnitude of these errors is usually small. Nevertheless, even a slight error can cause a failure in an operation that requires high precision, such as an insertion operation. Alternatively, if the position and orientation of the object relative to the end effector 12 is observed immediately after grasping and not observed again, the position and orientation of the object relative to the end effector 12 may change between the observation and the movement of the object to the guide start point due to insufficient gripping force. Furthermore, if an external force is applied to the object while it is being moved along a guide (described later), for example if the object comes into contact with a target object, the position and orientation of the object relative to the end effector 12 may change. In any case, if there are such errors, it is not possible to move the object by the commanded movement amount. The deviation of the actual movement amount from the commanded movement amount is called system noise.

[0055] Here, the relative position and orientation of the manipulation object from the target object is defined as state s, and the state s at discrete times t = 0, 1, . . . , M is t The coordinate system of state s is the object coordinate system, which is a relative coordinate system with the target object as the reference. The position and orientation of the TCP in the robot coordinate system at time t is x t Let x t ands t Let T be the homogeneous transformation matrix of the position and orientation. t Let us assume that.

[0056] The transformation of the position and orientation is shown in formula (1). t , T t , x t is a homogeneous coordinate system.

[0057] JPEG0007622455000001.jpg1254

[0058] As is clear from equation (1), the homogeneous transformation matrix T t contains not only the transformation of the TCP's position and orientation from the robot coordinate system to the object coordinate system, but also the transformation from the TCP's relative position and orientation to the relative position and orientation of the object. That is, x t is the TCP position and orientation, while x t T t The state s that is the result of the transformation by t is the relative position and orientation of the object, not the TCP.

[0059] In addition, state s t and TCP position and orientation x t is a 4-by-4 matrix, with the top left 3-row and 3-column elements representing the rotation matrix, and the 1st to 3rd rows of the 4th column can be treated as a position vector. The 4th row of the 4th column is the element for handling infinity. By its very nature, a rotation matrix only has 3-dimensional information, so these matrices can be treated as being 6-dimensional in nature. Homogeneous transformation matrix T t is also a 4-by-4 matrix. The product of two matrices in a homogeneous coordinate system is equivalent to the addition of the positions and orientations expressed as vectors.

[0060] Now, consider that the robot 10 is gripping an object to be manipulated by the end effector 12 .

[0061] At a certain time t, the TCP position and orientation x t Control amount u t By applying the above, the position and orientation of TCP at time t+1 becomes x t+1 At this time, since the operation object is held by the end effector 12, the state transitions to s t From state s t+1 The control amount u t The amount of movement of the relative position and orientation in the object coordinate system changed by a t However, the gripping posture of the object during movement is affected by noise v t can change only by a t ν tThe amount of movement influenced by is considered to be the net movement of the manipulated object.

[0062] ν t is the system noise of the system, and the system noise ν t Let us assume that Pν follows a certain probability distribution. t is also a homogeneous coordinate system.

[0063] Also, the gripping posture changes during the operation. t By changing only s t and x t homogeneous transformation matrix T t is ν t The homogeneous transformation matrix is ​​changed by JPEG0007622455000002.jpg1152 It is.

[0064] The state equation of the system is written in matrix operation as Equation (2). ν t Tou t and a t is a homogeneous coordinate system.

[0065] JPEG0007622455000003.jpg3169

[0066] From equations (1) and (2), ν t a t Tou t As the relationship, we obtain equation (4).

[0067] JPEG0007622455000004.jpg26100

[0068] Next, consider the observation actions by the state observation sensor 14 and the contact observation sensor 16. t The observed value y t is the observed value of state s t ' and contact observation c t ' is composed of.

[0069] y t =(s t ',ct ')

[0070] s t ' is the observed value of the relative position and orientation in the object coordinate system. However, due to the observation noise ω, s t ' is the true value s t There is a margin of error from c t ' is the contact observation value between the target object and the operation object. t There is no error in the observation of c '. If the target object and the operation object are not in contact, t '=0 and in contact, c t '=1.

[0071] ω t Let be the observation noise of the system, and the observation noise ω t Let us assume that Pω follows a certain probability distribution. t is also a homogeneous coordinate system.

[0072] The observation equation is written in the form of a homogeneous coordinate system as Equation (5).

[0073] JPEG0007622455000005.jpg2262

[0074] Based on the above, the state space model is defined as follows.

[0075] JPEG0007622455000006.jpg2572

[0076] The state space model is described as a probabilistic model in equation (8).

[0077] JPEG0007622455000007.jpg2285

[0078] When time has passed to a certain time t, the set of observations JPEG0007622455000008.jpg11104 is given. 0:t Given s t The probability of obtaining is expressed by the following equation (9).

[0079] JPEG0007622455000009.jpg71163 however, JPEG0007622455000010.jpg1055

[0080] The state is controlled based on the state space model of formula (8) and the probability of obtaining the state when the observation of formula (9) is obtained. That is, the relative position and posture between the two objects is manipulated by the robot 10.

[0081] According to the above-described method for executing a task, the execution unit 132 controls the robot 10 to operate the operation object so that the operation object to be operated for the task transitions from an initial state in which the operation object is separated from a target object located in the environment to a completed state in which the operation object is in contact with the target object in a specific manner. The execution unit 132 controls the robot 10 for each task.

[0082] Here, the task is a task of assembling the grasped part to another part, a task of transferring the grasped part to another position in the environment, or a task of grasping a part, which will be described later.

[0083] 7, the execution unit 132 includes a guide setting unit 140, an initial setting unit 142, an action planning unit 144, a command conversion unit 146, an observation unit 148, a time update unit 150, an observation update unit 152, a state estimation unit 154, an iteration determination unit 156, and a goal setting unit 158. The goal setting unit 158 ​​is an example of a goal state setting unit, the time update unit 150 is an example of a particle set setting unit, the observation update unit 152 is an example of a particle set adjustment unit, the command conversion unit 146 is an example of a behavior unit, and the iteration determination unit 156 is an example of a processing control unit.

[0084] The guide setting unit 140 sets a guide. The guide is a trajectory that is the final part of the relative trajectory of the operation target object with respect to the attention object, and is a common target relative trajectory regardless of the initial relative position of the operation target object with respect to the attention object. The guide includes a completion state and a series of intermediate target states leading to the completion state. Here, the completion state and the intermediate target states are represented by an object coordinate system.

[0085] For example, the guide setting unit 140 creates a guide for the unit state transition data based on an assembly procedure in the registration information. Here, the guide is a series of discrete relative positions and orientations leading to a completed state. The completed state is the end of the guide.

[0086] The guide setting unit 140 may be provided in the motion planning device 20. In that case, the guide is also acquired by the control device 30 in the same manner as the unit state transition data.

[0087] An example of a method for creating a guide will be described below.

[0088] 8 shows an example of the trajectory of the position and orientation of part D when part D is moved from a state where part D is attached to an assembly of parts A to C in a direction in which part D can move without interfering with the assembly of parts A to C. A trajectory that starts from the end point of that trajectory and reverses the direction of movement is created as guide G1. The intervals between the states that make up the guide do not need to be uniform; for example, the states may be set densely in an area where contact is expected.

[0089] The initial setting unit 142 sets the intermediate goal state during the guide that is the furthest from the completed state as the goal state for the current movement.

[0090] Moreover, the initial setting unit 142 initializes a particle set that represents uncertainty in the position and orientation of the manipulation object, and each particle included in the particle set represents one of the possible positions and orientations of the manipulation object. Here, the position and orientation of the manipulation object represented by the particles is represented in the object coordinate system.

[0091] The action planning unit 144 plans an action for moving the manipulation object from an initial state or an estimated state (to be described later) to a goal state of the current movement. Here, the estimated state and the action are represented in the object coordinate system.

[0092] The command conversion unit 146 converts the planned behavior into a command that the robot 10 can execute, outputs the command, and issues the command to the robot 10.

[0093] The observation unit 148 acquires an observation result regarding the relative position and orientation of the target object and the operation object from the state observation data acquired by the acquisition unit 130. Here, the observation unit 148 acquires the relative position and orientation of the target object and the operation object as an observation result represented by an object coordinate system.

[0094] Moreover, the observation unit 148 acquires an observation result on the presence or absence of contact between the operation object and the target object from the contact observation data acquired by the acquisition unit 130. Specifically, the contact observation data is converted into a binary value corresponding to contact or non-contact, and recognized as a contact observation value. The method of converting the sensor value into a binary value is not limited, and an existing method may be used.

[0095] The time update unit 150 updates the set of particles to the next step.

[0096] The observation update unit 152 updates the weight of each particle included in the particle set. At this time, the weight of the particle is increased as the particle expresses a position and orientation closer to the position and orientation of the manipulation object indicated by the observation result, and when the observation result indicates that contact has occurred, the weight of the corresponding particle is increased as the particle is closer to a state in which the target object and the manipulation object arranged at the position and orientation indicated by each particle are in contact in a virtual space in which the shapes and relative positional relationship of the target object and the manipulation object are expressed. Here, the virtual space is a space described by an object coordinate system.

[0097] State estimation unit 154 calculates an estimated state, which is the position and orientation of the manipulation object estimated based on a set of particles in which the weight of each particle has been adjusted.

[0098] The repetition determination unit 156 determines whether the calculated estimated state coincides with the completed state within a predetermined error. The repetition determination unit 156 also repeats the processes of the goal setting unit 158, the action planning unit 144, the command conversion unit 146, the observation unit 148, the time update unit 150, the observation update unit 152, and the state estimation unit 154 until the estimated state coincides with the completed state within a predetermined error.

[0099] The goal setting unit 158 ​​sequentially sets intermediate goal states during the guide as goal states for the current movement, from the farthest to the closer to the completed state, and sets the completed state as the final goal state for the current movement.

[0100] Next, the operation of the control system 1 will be described.

[0101] 9 is a sequence diagram showing the flow of processing in the control system 1 of this embodiment. The CPU 20A functions as each part of the motion planning device 20 to perform motion planning processing, and the CPU 30A functions as each part of the control device 30 to perform control processing.

[0102] In step S100, the motion planning apparatus 20 creates state transition data including each of the unit state transition data and control data. The control data is each of the grip data corresponding to each of the created unit state transition data.

[0103] In step S102, the motion planning device 20 outputs the state transition data and the control data to the state transition control unit 25.

[0104] In step S104, the state transition control unit 25 receives a series of task start instructions and starts the task to be processed. When the task start instruction is received, the first task to be processed is started, and the tasks are updated sequentially according to the progress of the tasks. The task start instruction may be received by the control device 30.

[0105] In step S108, state transition control unit 25 outputs unit state transition data corresponding to the task to be processed and control data corresponding to the unit state transition data to control device 30. State transition control unit 25 first outputs unit state transition data corresponding to the first task in the sequence of repeated processing, and outputs unit state transition data corresponding to the next task according to the progress of the task.

[0106] In step S114, the acquisition unit 130 of the control device 30 acquires the state observation data and the contact observation data observed by the state observation sensor 14 and the contact observation sensor 16, and the execution unit 132 executes the task to be processed. As a result, the robot 10 is controlled so that the task transitions from an initial state in which the manipulation object to be manipulated is separated from a target object located in the environment to a completed state in which the manipulation object is in contact with the target object in a specific manner.

[0107] In step S118, the execution unit 132 of the control device 30 notifies the state transition control unit 25 of the completion of the task.

[0108] In step S120, state transition control unit 25 determines whether or not the process has been completed up to the final state of the state transition data. If it is determined that the process has been completed up to the final state, the process proceeds to step S124. If it is determined that the process has not been completed up to the final state, the process proceeds to step S122.

[0109] In step S122, state transition control unit 25 updates the task to be processed, and returns to step S108. Task update is a process of updating the task to be processed to the next task. In step S108 after task update, unit state transition data and control data (grasping data) corresponding to the task to be processed are output to control device 30, and the subsequent processes are repeated.

[0110] In step S124, the state transition control unit 25 ends the task, and the processing of the control system 1 ends.

[0111] FIG. 10 is a flowchart showing the flow of the control process of the control device 30 in step S114.

[0112] In step S130, the guide setting unit 140 generates and sets a guide for the unit state transition data based on the assembly procedure in the registration information. * , end state s M * Define a set of M+1 states JPEG0007622455000011.jpg10105 is determined as a guide.

[0113] Here, the state set s 0:M * is a guide, and the guide is generated using CAD models of the target object and the operation object. For example, as described with reference to Fig. 8, starting from a state in which the operation object is attached to the target object, which is the end state, in the CAD model, the operation object is moved little by little over M steps in a direction in which the operation object can move without interfering with the target object, a trajectory of the position and orientation of the operation object is created, and a trajectory is generated as a guide by starting from the end point of the trajectory and reversing the movement direction. The intervals between the states constituting the guide do not need to be uniform, and for example, the states may be set densely in an area where contact is expected.

[0114] In step S132, the initial setting unit 142 sets the initial value of the target state to the state s0 * to.

[0115] In step S134, the observation unit 148 acquires the relative position and orientation of the operation object from the state observation data obtained by the state observation sensor 14.

[0116] In step S136, the initial setting unit 142 sets a particle set, which is a set of any N states at the initial time t=0. JPEG0007622455000012.jpg1297 Determine the initial value of s0 by using some prior knowledge. (n) For example, the initial value s0 may be set so as to be uniformly distributed within the range of the maximum error expected for the observed value of the relative position and orientation of the manipulation object. (n) may be determined.

[0117] Here, the particle set represents the uncertainty of the relative position and orientation of the object, with each particle representing one of the possible relative positions of the object.

[0118] State estimate ^s t is the relative position and orientation that represents the particle set.

[0119] Also, a set of N particle weights corresponding to the N particles JPEG0007622455000013.jpg10101 The initial value of is set as w (n) = 1 / N.

[0120] Finally, we use the probability distribution P φ Determine.

[0121] JPEG0007622455000014.jpg847

[0122] The particle dispersion variable φ corresponds to a hypothetical system noise.

[0123] In step S138, the action planning unit 144 calculates the element s m * Among them, the current element is s t * , the element for the next step is s t+1 * Then, s t+1 * and the current state estimate ^s t from the displacement a in the object coordinate system t Determine.

[0124] JPEG0007622455000015.jpg1166

[0125] In addition, the superscript ^ (hat) of X in the formula is represented as ^X.

[0126] Status t Estimate of ^s t is calculated in step S148, which will be described later. t The initial value of is the relative position and orientation acquired in step S134. Note that the amount of movement may be determined by performing route planning using CAD each time. When a guide is not used, the current state is used as the starting point, and the state s is calculated as shown in formula (1). t and position x t s t =T t x t Based on this relationship, the estimated state ^s t and the position and orientation of the TCP in the robot coordinate system x t A homogeneous transformation matrix ^T that transforms t ^T t is T t corresponds to the estimated value of

[0127] JPEG0007622455000016.jpg2769

[0128] Determined movement amount a t When the robot 10 moves the object, the estimated ^T t Using the movement amount a t The amount of control over TCP^u t By converting it to ^u t The robot 10 is controlled by

[0129] JPEG0007622455000017.jpg1479

[0130] However, in practice, the system noise ν t Therefore, the operation object is ν t a tThe net relative position and orientation movement ν t a t The actual control amount u is calculated by converting the amount of movement of the TCP in the robot coordinate system. t However, ν t is unknown for u t Since it is not possible to obtain t This is done using.

[0131] In step S140, the command conversion unit 146 converts the movement amount ^u of the TCP in the robot coordinate system into t These are converted into rotation command amounts for each joint of the robot 10 using inverse kinematics, and the robot 10 is commanded to act.

[0132] In step S142, the observation unit 148 calculates, from the state observation data and the contact observation data obtained from the state observation sensor 14 and the contact observation sensor 16, an observation value y t =(s t ',c t ').

[0133] In addition, using the measured values ​​of the angles of each joint of the robot 10 by the encoder, the position and orientation of the TCP in the robot coordinate system is calculated by forward kinematics as x t get.

[0134] In step S144, the time update unit 150 updates the particle aggregation after the action in step S140. JPEG0007622455000018.jpg1514 A t and the particle scattering variable φ t By JPEG0007622455000019.jpg14104 Also, normalize the particle weights to 1 / N.

[0135] JPEG0007622455000020.jpg2975

[0136] In step S146, the observation update unit 152 updates the weight of each particle included in the particle set.

[0137] Specifically, interference detection is first performed as follows. An object of interest depicted using CAD shape data is placed in a simulation space (virtual space) for interference detection. Furthermore, for each particle included in the particle set, a control object with a position and orientation represented by that particle is placed in the simulation space using CAD shape data. If a control object placed based on a certain particle overlaps with the control object of interest, it is determined that there is interference, and if the control object and the control object are separated and there is no overlap, it is determined that there is no interference. In this way, the presence or absence of interference is determined for each particle. In addition, the interference distance, which is the shortest distance between the surface of the control object and the surface of the control object in the simulation, is also calculated.

[0138] In this way, particle assemblies JPEG0007622455000021.jpg1084 Using this, we perform interference detection using CAD in the simulation, and the set of interference detection results is JPEG0007622455000022.jpg1065 Let the set of interference distances be JPEG0007622455000023.jpg1163 Let us assume that.

[0139] Collision Detection t (n) is the presence or absence of interference between the target object and the manipulated object of the nth particle in the simulation, and c t (n) = 0, and in the absence of interference, c t (n) =1.

[0140] Interference distance d t (n) is the shortest distance between the surface of the target object and the surface of the manipulated object for the nth particle in the simulation, and d t(n) <0, and when there is no interference, d t (n) >0.

[0141] Probability p(y t |s t (n) ) corresponding to the likelihood L(y t |s t (n) ) is defined as equation (16). t (n) |s t ') is the distance between the particle's state and the observed state. δ(·) is the delta function, and τ and σ are hyperparameters.

[0142] JPEG0007622455000024.jpg49155

[0143] That is, there is no actual contact between the target object and the operation object detected by the contact observation sensor 16 (c t If c ′=0, the upper part of equation (16) is used, and there is actual contact (c t '=1), the lower formula is used. The delta function in the formula for the absence of real contact returns a value of 1 for particles that do not interfere in the simulation, and a value of 0 for particles that do interfere.

[0144] The exponential function part in formula (16) is a Gaussian function. The function system may not be a Gaussian function, but may use a function in which the likelihood is high if the distance is short and low if the distance is long. For example, exponential decay may be used.

[0145] The particle weight W is updated using the likelihood value obtained from the likelihood function by Equation (17).

[0146] JPEG0007622455000025.jpg1584

[0147] In other words, when there is no actual contact between the target object and the manipulated object, the likelihood function sets the weight of the particle with interference in the simulation (particle that contradicts the observation result of no contact) to 0, and for the particle with no interference in the simulation, the state of the particle s t (n) is the observed state s of the manipulation object. t The closer it is to ', the greater the weight.

[0148] On the other hand, when there is real contact between the target object and the manipulated object, the likelihood function is t (n) is the observed state s of the manipulation object. t and the weight of a particle is increased as the surface of the target object and the surface of the manipulated object are smaller. As a result, when there is actual contact, the weight of a particle that represents a state close to a state where the surfaces of the target object and the manipulated object are in contact with each other in the simulation is updated to be larger, and the weight of a particle that represents a state where the manipulated object is far away from the target object or deeply penetrates into the target object is updated to be smaller.

[0149] In step S148, state estimation unit 154 calculates an estimated state, which is the position and orientation of the manipulation object estimated based on a set of particles in which the weights of each particle have been adjusted. At this time, the estimated value ^s of the state of the manipulation object is calculated as described above. t is modified so as to approach the observed position and orientation of the object to be operated, and when actual contact is observed by the contact observation sensor 16, is also modified so as to approach a state in which the surfaces of the object to be operated and the object of interest are in contact with each other in the simulation.

[0150] Specifically, the estimated state at time t, ^s t is calculated as the expected value for the particle ensemble from Equation (18). In Equation (18), log means the logarithm of the matrix, and exp means the exponent of the matrix.

[0151] JPEG0007622455000026.jpg2684

[0152] Updated particle weights JPEG0007622455000027.jpg1179 Particles assemble with a probability proportional to JPEG0007622455000028.jpg1079 Randomly select a particle from JPEG0007622455000029.jpg1313 Resampling is performed.

[0153] In step S150, the iteration determination unit 156 calculates the state estimate ^s t is within a given error range and the final guide state s M * Determine whether the estimated state value ^s t is within a given error range and the final guide state s M * If they match, the control process ends, otherwise the process proceeds to S152.

[0154] In step S152, the target setting unit 158 ​​sets the target state to the next state during the guide. Also, the time is advanced by one step, and the previous t+1 is set as the new t. Then, the process returns to step S138.

[0155] 10 may be changed as appropriate as long as the purpose is achieved. For example, the observation in step S142 may be performed after step S144.

[0156] Furthermore, the robot 10 may be controlled as follows to move the manipulation object from the gripped position to the guide start point.

[0157] First, the path of movement from the location where the operation object is held to the guide start point is created by connecting the path with a straight line or by using a conventional motion planning method. The movement of the operation object from the location where it is held to the guide start point is performed, for example, as follows.

[0158] First, the position and orientation of the target object in the camera coordinate system is observed from the state observation data obtained by the camera of the state observation sensor 14, and a virtual space having an object coordinate system, which is a coordinate system based on the target object, is developed. This makes it possible to convert between the camera coordinate system and the object coordinate system.

[0159] Next, a guide is placed in the virtual space, where the guide represents the trajectory of the manipulation object rather than the end effector 12.

[0160] Next, the position and orientation of the object before gripping in the camera coordinate system is observed from the state observation data obtained by the camera, and the object is placed in the virtual space by converting it into the position and orientation in the object coordinate system. Then, the position and orientation of the end effector 12 in the robot coordinate system is obtained from the state observation data obtained by the encoder.

[0161] Next, the position and orientation of the end effector 12 in the camera coordinate system is observed from the state observation data obtained by the camera, and converted into the position and orientation of the object coordinate system to place the end effector 12 in the virtual space. This makes it possible to convert between the robot coordinate system and the object coordinate system. If the correspondence between the camera coordinate system and the robot coordinate system has been calibrated in advance, the observation of the end effector 12 by the camera can be omitted.

[0162] Next, the end effector 12 grasps the object to be manipulated.

[0163] After grasping, the position and orientation of the object in the camera coordinate system are observed from state observation data obtained by the camera, and the position and orientation of the object in the object coordinate system are corrected, thereby eliminating the grasping error.

[0164] Then, in the virtual space, a movement path of the operation object from the initial position and posture when it is grasped to the position and posture at the guide start point is planned.

[0165] Next, the path plan in the virtual space is transformed into a path plan in the robot coordinate system space, and the object is moved according to the path plan. Note that errors are not eliminated during the movement.

[0166] Then, the positions and orientations of the object and end effector 12 after movement are obtained from state observation data obtained by the camera, and the object is moved so as to match the position and orientation at the guide start point. The conversion relationships between the camera coordinate system, object coordinate system, and robot coordinate system are corrected. This eliminates the position and orientation error of the object at the guide start point. Also, the conversion error between the coordinate systems is reduced.

[0167] As described above, according to the control system 1 of the present embodiment, a particle set for the relative position and orientation of two objects is assumed, the likelihood of each particle is obtained from the observation information and the likelihood function, and the relative position and orientation are estimated. The amount of movement of the object is determined from the estimated relative position and orientation and the target relative position and orientation at the next time. A homogeneous transformation matrix that converts the estimated relative position and orientation into the position and orientation of the TCP in the robot coordinate system is obtained, and the amount of movement of the object is converted into the amount of movement of the TCP from the transformation matrix to control the robot 10, and the relative position and orientation of the two objects are brought to the target relative position and orientation. At this time, using information that the manipulated object and the target object have come into contact, the relative position and orientation of the two objects is estimated by the contact detection result between the objects and the interference determination simulation using CAD, and the amount of movement of the manipulated object is determined from the estimation result. As a result, it is possible to manipulate an object using the robot 10 in an uncertain environment where noise is added to observation and behavior, and bring the relative position and orientation of the two objects to the target state with high accuracy. Furthermore, even in an environment in which the relative position and orientation between the end effector 12 and the object being operated is unknown and changes during control, making control uncertain, or even in an environment in which the recognition of the relative position and orientation by the state observation sensor 14 contains errors, making it possible to bring the relative position and orientation between the two objects to a target state.

[0168] In the above embodiment, the operation planning process or the control process executed by the CPU by reading the software (program) may be executed by various processors other than the CPU. In this case, examples of the processor include a PLD (Programmable Logic Device) such as an FPGA (Field-Programmable Gate Array) whose circuit configuration can be changed after manufacture, and a dedicated electric circuit such as an ASIC (Application Specific Integrated Circuit) which is a processor having a circuit configuration designed exclusively for executing a specific process. In addition, the operation planning process or the control process may be executed by one of these various processors, or may be executed by a combination of two or more processors of the same or different types (for example, a plurality of FPGAs, and a combination of a CPU and an FPGA). In addition, the hardware structure of these various processors is, more specifically, an electric circuit that combines circuit elements such as semiconductor elements.

[0169] In the above embodiment, the program is pre-stored (installed) in the ROM 20B (30B) or the storage 20D (30D), but the present invention is not limited to this. The program may be provided in a form recorded on a recording medium such as a CD-ROM (Compact Disk Read Only Memory), a DVD-ROM (Digital Versatile Disk Read Only Memory), or a USB (Universal Serial Bus) memory. The program may be downloaded from an external device via a network.

[0170] In the above embodiment, the end effector 12 is a gripper having the clamping portion 12a or a suction pad, but is not limited thereto. Any other configuration that can hold an object may be used. For example, the end effector 12 may be a vacuum suction chuck, a magnetic suction chuck, a spatula for scooping up an object, or the like.

[0171] In addition, although the guide is generated from the completed state of the unit state transition data, the present invention is not limited to this. The unit state transition data may be not only the completed state, but also a series of discrete relative positions and orientations leading to the completed state.

[0172] Although the example has been described in which a series of relative positions and orientations is automatically generated as a guide, it is also possible not to generate a guide. In this case, it may be considered as a special case in the above embodiment in which the guide is configured in a single state.

[0173] The guide may also be a sequence of discrete relative positions and orientations leading to a non-complete state.

[0174] The task may also be a task of gripping a part that is a target object. In the above embodiment, an object located in the environment is the target object, and an object gripped by the end effector 12 is the manipulated object. However, in the task of gripping a part, an object placed in the environment to be manipulated (grasped) by the end effector 12 is the target object, and the end effector 12 itself is the manipulated object. Since the end effector 12 is manipulated by a robot arm, the end effector 12 can also be considered as an object manipulated by a robot. In the task of gripping a part, the completion state is a state in which the end effector 12, which is the manipulated object, grips the target object placed in the environment but has not yet moved the target object. [Explanation of symbols]

[0175] 1. Control System 10. Robot 11 Arm 12 End effector 14 Condition Observation Sensor 16 Contact Observation Sensor 20 Motion planning device 30 Control device 130 Acquisition Department 132 Executive Department 140 Guide setting section 142 Initial setting section 144 Action Planning Department 146 Command conversion unit 148 Observation Department 150 hours update section 152 Observation and Update Division 154 State Estimation Unit 156 Repetition Judgment Section 158 Goal Setting Department

Claims

1. A robot control device for controlling a robot that manipulates a manipulation object so that the manipulation object, which is an object to be manipulated, transitions from a state in which the manipulation object is separated from a target object located in an environment to a completed state in which the manipulation object is in contact with the target object in a specific manner, a goal state setting unit that sets an intermediate goal state, which is a goal state on the way of the movement of the operation object until the movement reaches the completion state, or the completion state as a goal state of a current movement; an observation unit that acquires an observation result by a sensor regarding a position and orientation of the manipulation object and whether or not the manipulation object is in contact with the target object; a particle set setting unit that sets a particle set representing uncertainty of a position and orientation of the manipulation object, each of the particles included in the particle set representing one of possible positions and orientations of the manipulation object; and a particle set adjustment unit that increases a weight of a particle that represents a position and orientation closer to the position and orientation of the manipulation object indicated by the observation result, and increases a weight of a corresponding particle that, when the observation result indicates that the contact is occurring, is closer to a state in which the target object and the manipulation object arranged at the position and orientation represented by each of the particles are in contact with each other in a virtual space in which the shapes and relative positional relationship of the target object and the manipulation object are represented; a state estimation unit that calculates an estimated state, which is a position and orientation of the manipulation object estimated based on a set of the particles in which the weights of the particles have been adjusted; a behavior planning unit that plans a behavior for moving the operation object from the estimated state to a goal state of the current movement; a behavior unit for commanding the robot to execute the planned behavior; a processing control unit that repeats the setting of the target state, the acquisition of the observation results, the setting of the particle set, the adjustment of the particle set, the calculation of the estimated state, the planning of the action, and the execution of the action until the estimated state coincides with the completed state within a predetermined error; A robot control device comprising:

2. Further, a guide setting unit is provided that sets a guide including the completion state and a series of intermediate goal states leading to the completion state, The goal state setting unit sequentially sets the intermediate goal states in the guide as goal states of the current movement from the farthest to the closest to the completed state, and sets the completed state as the final goal state of the current movement. The robot control device according to claim 1 .

3. the completed state, the intermediate target state, the position and orientation of the manipulation object represented by the particles, the estimated state, and the action are represented by an object coordinate system which is a relative coordinate system with respect to the target object; the virtual space is a space described by the object coordinate system, the observation unit provides a position and orientation of the operation object detected by the sensor as an observation result represented by the object coordinate system; The behavior unit converts the behavior into a command that can be executed by the robot and outputs the command. The robot control device according to claim 1 or 2.

4. The manipulation object is an object that is manipulated by an end effector of the robot. The robot control device according to any one of claims 1 to 3.

5. The manipulation object is an end effector attached to an arm of the robot, and the target object is a holding target object that is to be held by the end effector in the completed state. The robot control device according to any one of claims 1 to 3.

6. A robot control method for controlling a robot that manipulates a manipulation object so that the manipulation object, which is an object to be manipulated, transitions from a state in which the manipulation object is separated from a target object located in an environment to a completed state in which the manipulation object is in contact with the target object in a specific manner, comprising: an intermediate target state, which is a target state on the way of the movement of the operation object until the completion state is reached, or the completion state is set as a target state of the current movement; Obtaining a position and orientation of the manipulation object and an observation result of the manipulation object and the presence or absence of contact between the manipulation object and the target object by a sensor; setting a set of particles representing uncertainty of a position and orientation of the manipulation object, each of the particles representing one of possible positions and orientations of the manipulation object; a weight of the particle is increased as the particle's position and orientation is closer to the position and orientation of the manipulation object indicated by the observation result, and when the observation result indicates that the contact is occurring, a weight of the corresponding particle is increased as the particle is closer to a state in which the target object and the manipulation object arranged at the position and orientation indicated by each of the particles are in contact with each other in a virtual space in which the shapes and relative positional relationship of the target object and the manipulation object are represented, calculating an estimated state which is a position and orientation of the manipulation object estimated based on the set of particles in which the weights of the particles have been adjusted; planning an action for moving the manipulation object from the estimated state to a target state of the current movement; commanding the robot to execute the planned action; The steps of setting the target state, obtaining the observation results, setting the set of particles, adjusting the set of particles, calculating the estimated state, planning the action, and executing the action are repeated until the estimated state coincides with the completed state within a predetermined error. A robot control method for causing a computer to execute processing.

7. A robot control program for controlling a robot that manipulates a manipulation object so as to transition from a state in which the manipulation object is separated from a target object located in an environment to a completed state in which the manipulation object is in contact with the target object in a specific manner, the program comprising: an intermediate target state, which is a target state on the way of the movement of the operation object until the completion state is reached, or the completion state is set as a target state of the current movement; Obtaining a position and orientation of the manipulation object and an observation result of the manipulation object and the presence or absence of contact between the manipulation object and the target object by a sensor; setting a set of particles representing uncertainty of a position and orientation of the manipulation object, each of the particles representing one of possible positions and orientations of the manipulation object; a weight of the particle is increased as the particle's position and orientation is closer to the position and orientation of the manipulation object indicated by the observation result, and when the observation result indicates that the contact is occurring, a weight of the corresponding particle is increased as the particle is closer to a state in which the target object and the manipulation object arranged at the position and orientation indicated by each of the particles are in contact with each other in a virtual space in which the shapes and relative positional relationship of the target object and the manipulation object are represented, calculating an estimated state which is a position and orientation of the manipulation object estimated based on the set of particles in which the weights of the particles have been adjusted; planning an action for moving the manipulation object from the estimated state to a target state of the current movement; commanding the robot to execute the planned action; The steps of setting the target state, obtaining the observation results, setting the set of particles, adjusting the set of particles, calculating the estimated state, planning the action, and executing the action are repeated until the estimated state coincides with the completed state within a predetermined error. A robot control program that causes a computer to execute a process.

Images