Trajectory planning device, trajectory planning method, and trajectory planning program

Abstract

To achieve a trajectory plan even for a mechanism in which a plurality of robots is connected to each other at real time.SOLUTION: A trajectory planning device for planning a trajectory for controlling a first robot, and a second robot connected to the first robot so as to be movable by operation of the first robot holds information indicating a configuration of an arm of the second robot, and the device holds information indicating a position and an attitude of a fingertip at a plurality of through points where the fingertip of the second robot sequentially passes; calculates singular point curve surfaces at each of the through points on the basis of the configuration of the arm of the second robot, and the position and the attitude of the fingertip at each of the through points; determines a target position of the first robot on the basis of a singular point curve surface of the second robot obtained by calculating each of the through points, and a movable range of the second robot; plans a trajectory to a target position of the first robot; and plans a trajectory where the fingertip of the second robot passes through the plurality of through points at the target position of the first robot.SELECTED DRAWING: Figure 2

Description

[Technical Field]

[0001] The present invention relates to a trajectory planning device, a trajectory planning method, and a trajectory planning program for generating a trajectory. [Background technology]

[0002] Japanese Patent Laid-Open Publication No. 11-198071 (Patent Document 1) is a background art in this technical field. Patent Document 1 states, "However, when the redundant axis 2 of the redundant robot 1 is fixed, problems specific to two-joint robots may arise in the remaining axes other than the redundant axis 2. That is, in the two-joint robot 11, there is a point called a singular point 5 at the position where the arms overlap, and when a point on the arm is moved in a straight line, if the straight line trajectory and the singular point 5 are misaligned, the arm may not be able to pass through the singular point 5, resulting in inoperability. <Omitted> Therefore, the object of the present invention is to provide a method for determining the position of a redundant axis of a redundant robot, which is capable of determining the appropriate position of the redundant axis and interpolating linear movement when the arm moves. <Omitted> In order to achieve this object, the redundant axis position determination method of the invention described in claim 1 is The document states that "In a method for determining the position of a redundant axis in a robot, the robot has two rotatable arms connected in sequence via joints to a moving member that constitutes the redundant axis, and when a predetermined position on the arm is to be moved in a straight line, the arm has a redundant degree of freedom in a certain plane. In this method, the redundant robot determines the trajectory of a singular point, which is a predetermined position of the robot arm when the two connected arms overlap, by moving the redundant axis, while defining the direction of movement of the predetermined position with respect to a given target position as an access direction, and determines the position of the redundant axis based on the intersection formed by the singular point trajectory and a straight line that passes through the target position and is parallel to the access direction." [Prior art documents] [Patent documents]

[0003] [Patent Document 1] Japanese Patent Application Publication No. 11-198071 Summary of the Invention [Problem to be solved by the invention]

[0004] Patent Document 1 describes a method for determining the position of a redundant axis using an access direction to avoid singular points in a SCARA robot. However, with the technique in Patent Document 1, the method for determining the position of a redundant axis is limited to SCARA robots, and because it is necessary to set an access direction for each way point, it is not possible to appropriately plan the position of a single-axis robot or a multi-axis robot for a structure in which a six-axis robot is mounted on a single-axis robot or a multi-axis robot.

[0005] Therefore, this invention provides a trajectory planning device that calculates the target position of the other robot based on the singularity surface consisting of the movable range of the six-axis robot and the posture of each way point, and then plans the trajectory of the other robot so that it passes through each point within the way points, thereby quickly calculating a feasible trajectory across multiple robots. [Means for solving the problem]

[0006] In order to solve the above problem, for example, the configuration described in the claims is adopted. The present application includes a plurality of means for solving the above problem, and one example thereof is a trajectory planning device that plans a trajectory for controlling a first robot and a second robot connected to the first robot so as to be movable by the operation of the first robot, the trajectory planning device having a processing device and a storage device, the storage device holding information indicating the configuration of an arm of the second robot and information indicating the positions and postures of the hand of the second robot at a plurality of waypoints through which the hand of the second robot passes in sequence, and the processing device dividing the plurality of waypoints into a plurality of subsets, each subset including one or more of the waypoints; calculating a singularity surface, which is a set of singularities of the second robot at each of the via points, based on a configuration of the arm of the second robot and the position and posture of the hand at each of the via points; For each of the subsets, Based on the singularity surface of the second robot calculated for each of the via points and the movable range of the second robot, so that all of the one or more way points included in the subset are included in the intersection of the range of the second robot's movement range and the range outside the singularity surface of each of the way points, determining a target position for the first robot; For each target position of the first robot determined for each subset, Planning a trajectory for the first robot to a target position; For each of the subsets, The hand of the second robot at the target position of the first robot is One or more of the above included in the subset Plan a trajectory via waypoints If the end effector of the second robot fails to plan a trajectory that passes through a plurality of waypoints, the target position of the first robot is changed, and the end effector of the second robot at the changed target position of the first robot plans a trajectory that passes through the plurality of waypoints. It is characterized by: [Effects of the Invention]

[0007] According to one aspect of the present invention, by calculating the target position of the first robot based on the range of motion and singularity surface of the second robot, it is possible to realize real-time trajectory planning even for mechanisms in which multiple robots are connected.

[0008] Problems, configurations, and effects other than those described above will become apparent from the following description of the embodiments. [Brief explanation of the drawings]

[0009] [Figure 1] 1 is a block diagram showing an example of a configuration of a system including a trajectory planning device and peripheral devices according to an embodiment of the present invention. [Figure 2] 10 is a flowchart illustrating an example of a trajectory planning process performed by an integrated trajectory planning unit of the trajectory planning device according to the embodiment of the present invention. [Figure 3] FIG. 2 is an explanatory diagram showing an example of a table configuration of robot arm configuration data stored in a robot arm configuration storage unit in an embodiment of the present invention. [Figure 4] FIG. 4 is an explanatory diagram showing an example of a table configuration of interfering object configuration data stored in an interfering object configuration storage unit in the embodiment of the present invention. [Figure 5] FIG. 3 is an explanatory diagram showing an example of a table configuration of waypoint data stored in a waypoint storage unit in the embodiment of the present invention. [Figure 6] FIG. 2 is an explanatory diagram showing an example of a table configuration of trajectory data stored in a trajectory storage unit in an embodiment of the present invention. [Figure 7] FIG. 2 is an explanatory diagram illustrating an example of a singularity surface of a robot according to an embodiment of the present invention. [Figure 8] FIG. 2 is an explanatory diagram illustrating an example of a singularity surface of a robot according to an embodiment of the present invention. [Figure 9] FIG. 10 is an explanatory diagram illustrating an example of a via point of a robot in an embodiment of the present invention. [Figure 10] FIG. 10 is an explanatory diagram showing an example of a method for dividing waypoints of a robot in an embodiment of the present invention. [Figure 11] 10A and 10B are explanatory diagrams showing examples of singularity surfaces at each via point of a robot in an embodiment of the present invention. [Figure 12] FIG. 10 is an explanatory diagram illustrating an example of an intersection of singularity surfaces of a robot according to an embodiment of the present invention. [Figure 13] FIG. 2 is an explanatory diagram showing an example of a trajectory of a robot according to an embodiment of the present invention. [Figure 14] FIG. 2 is an explanatory diagram showing an example of a trajectory of a robot according to an embodiment of the present invention. [Figure 15] FIG. 2 is an explanatory diagram showing an example of an output screen output by the trajectory planning device according to the embodiment of the present invention. DETAILED DESCRIPTION OF THE INVENTION

[0010] Hereinafter, the embodiments will be described with reference to the drawings. In all the drawings for explaining the embodiments, the same parts are generally designated by the same reference numerals, and the repeated description thereof will be omitted.

[0011] In this embodiment, an example of a trajectory planning device that is a basic embodiment of the present invention will be described.

[0012] [System Configuration] FIG. 1 is a block diagram showing an example of the configuration of a system including a trajectory planning device 100 and peripheral devices according to an embodiment of the present invention.

[0013] The entire system includes a trajectory planning device 100, an input / output device 140, an integrated control device 150, a single-axis robot 160 (hereinafter also referred to as "robot 1" in this embodiment), and a six-axis robot 170 (hereinafter also referred to as "robot 2" in this embodiment). A user utilizes the functions of the trajectory planning device 100 by operating the input / output device 140. The trajectory planning device 100 can be configured with a general computer (PC, server, etc.), and realizes characteristic processing functions (each processing unit of the processing device 110) by, for example, software program processing.

[0014] The trajectory planning device 100 includes a processing device 110, a storage device 120, an input / output I / F (interface) device 130, and the like.

[0015] The input / output device 140 is an input device that inputs measurement data and the like through user operation, and an output device that outputs reference shape allocation results and the like, and may be, for example, a keyboard, mouse, display, printer, smartphone, tablet PC, etc.

[0016] The input / output I / F device 130 is a part that performs interface control (peripheral device control) processing such as data exchange with the input / output device 140. In this system, a graphical user interface (GUI) is configured on the screen of the input / output device 140 based on the processing of the processing device 110 and the processing of the input / output I / F device 130, and various types of information are displayed.

[0017] The integrated control device 150 is a device that controls the robot 1 and the robot 2 according to the trajectories output by the trajectory planning device 100.

[0018] The single-axis robot 160 is a robot that operates the six-axis robot 170. In this embodiment, the single-axis robot is described, but it can also be replaced by a robot with two or more degrees of translational freedom.

[0019] The six-axis robot 170 is a six-axis robot connected in series to the single-axis robot 160 (i.e., connected to the single-axis robot 160 so that it can move by the operation of the single-axis robot 160). In this embodiment, a vertically articulated six-axis robot will be described, but other six-axis robots with mechanisms other than the vertically articulated type, such as collaborative robots, can also be used instead.

[0020] The processing device 110 is composed of well-known elements such as a CPU (Central Processing Unit), RAM (Random Access Memory), and ROM (Read Only Memory). The processing device 110 is a part that performs processing to realize the characteristic functions of this embodiment, and includes an integrated trajectory planning unit 201, a robot 2 singularity surface calculation unit 202, a waypoint subset division unit 203, a robot 1 target position calculation unit 204, a robot 1 trajectory planning unit 205, and a robot 2 trajectory planning unit 206. These functions are realized by the CPU constituting the processing device 110 executing programs stored in the RAM or the like.

[0021] Although not shown, the trajectory planning device 100 has known elements such as an OS (Operating System), middleware, and applications, and in particular has existing processing functions for displaying a GUI screen on an input / output device 140 such as a display. The processing device 110 uses the above-mentioned existing processing functions to perform processing such as drawing and displaying a predetermined screen and processing data information input by a user on the screen.

[0022] The storage device 120 is composed of known elements such as an HDD (Hard Disk Drive) or an SSD (Solid State Drive), and has storage units or corresponding data information (e.g., databases or tables) including a robot arm configuration storage unit 301, an interference object configuration storage unit 302, a waypoint storage unit 303, and a trajectory storage unit 304.

[0023] The robot arm configuration storage unit 301 is a part that stores robot arm configuration data 410 used for trajectory planning, inverse kinematics calculation, dynamics calculation, and the like.

[0024] The interference object configuration storage unit 302 is a part that stores interference object configuration data 402 used for interference determination performed in trajectory planning.

[0025] The waypoint storage unit 303 is a part that stores waypoint data 403, which is a waypoint through which the hand passes in the trajectory plan.

[0026] The trajectory storage unit 304 is a part that stores the trajectory data 404 output by the integrated trajectory planning unit 201.

[0027] [flowchart] FIG. 2 is a flowchart showing an example of a trajectory planning process performed by the integrated trajectory planning unit 201 of the trajectory planning device 100 according to the embodiment of the present invention.

[0028] In step S101, the robot 2 singularity surface calculation unit 202 calculates the singularity surface of the robot 2 from the hand posture at each via point, based on the robot arm configuration data 401 stored in the robot arm configuration storage unit 301 and the via point data 403 stored in the via point storage unit 303. Here, the singularity surface is a set of singular points of the robot 2. This calculation method will be described with reference to FIGS. 7 and 8, taking as an example a case where a vertical articulated robot is used as the robot 2.

[0029] 7 and 8 are explanatory diagrams showing examples of singularity surfaces of a robot according to an embodiment of the present invention.

[0030] 7 and 8 respectively show the vertical articulated robot F101, the movable range F102 of the vertical articulated robot F101, and the singularity surface F103 of the vertical articulated robot F101. The movable range F102 of the vertical articulated robot F101 is a set of positions that point P can take, and is defined as within a spherical surface of radius (l1 + l2) centered on the position of the J2 axis. The singularity surface F103 is a curved surface calculated from the pitch angle θ of the hand, and is defined by the following mathematical formula in the cylindrical coordinate system (ρ, φ, z):

[0031]

number

[0032] For example, when the hand is directly downward, that is, when the pitch angle θ = π, the singularity surface F103 is a spherical surface obtained by rotating a circle whose center is on the axis center of the vertical articulated robot around the J1 axis, as shown in FIG. 7. On the other hand, when the hand is tilted 45 degrees, that is, when the pitch angle θ = π + π / 4, as shown in FIG. 8, the singularity surface F103 is a surface obtained by rotating a circle whose center position is shifted around the J1 axis. In other words, the singularity surface can be calculated based on the hand posture and the robot mechanism. In this example, a method for calculating the singularity surface was shown based on the mechanism of a vertical articulated robot, but for robots with six degrees of freedom, the singularity surface can be calculated from the hand posture based on a similar concept.

[0033] In step S102, the way point subset dividing unit 203 divides the way points into the specified division number N based on the way point data 403 in the way point storage unit 303. This will be explained using FIGS.

[0034] FIG. 9 is an explanatory diagram showing an example of a waypoint of the robot in the embodiment of the present invention.

[0035] FIG. 10 is an explanatory diagram showing an example of a method for dividing waypoints of a robot in an embodiment of the present invention.

[0036] Specifically, FIG. 9 shows an example of waypoints when a robot having a vertical articulated robot 2 mounted on a single-axis robot 1 passes through waypoints P1 to P5. The division process is a process of dividing such a sequence of waypoints P1 to P5 into a plurality of waypoint subsets, each containing one or more waypoints, by dividing the sequence. If the number of waypoint subsets generated by the division (i.e., the number of divisions) is N, the waypoint subset division unit 203 inserts (N-1) delimiter positions. For example, if the number of divisions N is 2, one delimiter position is inserted. This delimiter position is inserted at a location where the positional difference between the center of gravity V of the subset and the waypoint is minimized after division. In other words, the waypoint subset division unit 203 inserts delimiter positions so that the following mathematical expression is minimized:

[0037]

number

[0038] Note that n indicates the number of each route point, and V k indicates the center of gravity of each subset k (i.e., the center of gravity of the positions of one or more way points belonging to each subset k). For example, when dividing the example in FIG. 9 into two parts, the division point is set between P3 and P4.

[0039] In step S103, the integrated trajectory planning unit 201 repeatedly executes the processes from S104 to S109 for each subset calculated in S102.

[0040] In step S104, the robot 1 target position calculation unit 204 calculates the intersection of the range within the movable range of robot 2 and outside the singularity surface based on the singularity surface and the movable range calculated from the posture at the via point calculated in step S101. This calculation will be described with reference to FIGS.

[0041] FIG. 11 is an explanatory diagram showing an example of a singularity surface at each via point of a robot in an embodiment of the present invention.

[0042] FIG. 12 is an explanatory diagram showing an example of the intersection of singularity surfaces of a robot according to an embodiment of the present invention.

[0043] Here, as an example, when a first subset including via points P1, P2, and P3 and a second subset including via points P4 and P5 are obtained as shown in FIG. 10, the intersection of the singularity surface for the first subset will be described. For example, when a movable range F102 and a singularity surface F103 are obtained from the hand posture at via points P1, P2, and P3 as shown in FIG. 11, the intersection of the range between the singularity surface and the movable range is calculated as shown in FIG. 12. Specifically, the range indicated by hatching in FIG. 11 is the range inside the movable range F102 at via points P1, P2, and P3 and outside the singularity surface F103. The intersection of these ranges is indicated by hatching in FIG. 12.

[0044] In step S105, the robot 1 target position calculation unit 204 calculates the target position of the robot 1 based on the intersection of the singularity surfaces calculated in step S104 so that all via points are included in the intersection, as shown in Fig. 13. There are various possible methods for this calculation, but for example, the calculation can be performed by moving the robot 1 in various ways within its movable range and determining whether all via points are included in the intersection.

[0045] FIG. 13 is an explanatory diagram showing an example of a trajectory of the robot 1 in the embodiment of the present invention.

[0046] In the example of FIG. 13, the target position of robot 1 is calculated so that the current position of the hand of robot 2 and the waypoints P1, P2, and P3 are all included in the intersection calculated for the first subset in step S104.

[0047] In step S106, the robot 1 trajectory planning unit 205 plans a trajectory for moving the robot 1, i.e., the single-axis robot 160, so as not to interfere with the interfering object configuration data 402 contained in the interfering object configuration storage unit 302, and stores the planned trajectory in the trajectory storage unit 304. Various methods for calculating this trajectory are possible, but any method (including publicly known methods) can be used, such as the RRT (Rapidly-Exploring Random Trees) method or a simple method that simply divides the path from the current position to the target position into equal intervals and determines whether there is interference. For example, the RRT method is described in Steven M. LaValle, "Rapidly-Exploring Random Trees: A New Tool for Path Planning," http: / / msl.cs.illinois.edu / ~lavalle / papers / Lav98c.pdf.

[0048] In step S107, the robot 2 trajectory planning unit 206 plans a trajectory for moving the robot 2, i.e., the six-axis robot 170, so as to avoid interference with the interfering object configuration data 402 contained in the interference object configuration storage unit 302, and stores the planned trajectory in the trajectory storage unit 304. Various methods are conceivable for calculating this trajectory, and any method can be adopted, including well-known methods such as the RRT method described above or a simple method that simply divides the distance between the current position and the target position into equal intervals and determines whether there is interference. This makes it possible to plan a trajectory that passes through P1, P2, and P3 from the current position, as shown in Figure 14.

[0049] FIG. 14 is an explanatory diagram showing an example of a trajectory of the robot 2 in the embodiment of the present invention.

[0050] Figure 14 shows an example of a trajectory planned so that, at the target position of robot 1 calculated in step S105, the current position of robot 2's hand and all of way points P1, P2, and P3 are included in the intersection set calculated for the first subset in step S104.

[0051] In step S108, the integrated trajectory planning unit 201 determines whether a trajectory was planned in step S107, and branches the process depending on whether the plan was successful. For example, reasons for failure include when no interference-free trajectory was found, or when the trajectory passed through a singular point midway and the joint angle moved significantly in response to a change in the hand posture, and if none of these reasons for failure apply, it is determined that the trajectory was planned successfully, but if any of these reasons apply, it is determined that the trajectory planning failed.

[0052] If it is determined in step S108 that the trajectory planning of the robot 2 has failed (step S108: F), in step S109, the robot 1 target position calculation unit 204 determines whether the target position of the robot 1 has been changed a specified number of times or more. If the target position has already been changed a specified number of times or more (step S109: T), the process exits from the loop processing of step S103 and proceeds to step S111.

[0053] If it is determined in step S109 that the target position of the robot 1 has not been changed the specified number of times or more (step S109: F), in step S110, the robot 1 target position calculation unit 204 changes the target position of the robot 1. This change in target position can also be calculated by moving the robot 1 in various ways within the movable range and determining whether all of the via points are included in the intersection set, similar to the method described in step S105.

[0054] In step S111, the integrated trajectory planning unit 201 determines whether the division number N of the waypoints is already the same as the number of waypoints. If the division number N is the same as the number of waypoints (step S111: T), the integrated trajectory planning unit 201 cannot divide the trajectory any further, and therefore proceeds to step S114, where it determines that the planning has failed.

[0055] In step S112, the integrated trajectory planning unit 201 increases the division number N of the waypoint to N+1, and performs the process from step S102 again.

[0056] If it is determined in step S108 that the trajectory planning of the robot 2 has been successful (step S108: T), in step S113, the integrated trajectory planning unit 201 controls the robot 1 and the robot 2 according to the planned trajectories.

[0057] In step S114, the integrated trajectory planning unit 201 determines that the trajectory planning has failed, notifies the user of the failure of the trajectory planning, and ends the processing.

[0058] The initial value of the division number N of the way points may be 1. In that case, when step S102 is executed for the first time, the way point subset division unit 203 does not divide the way point set, and when step S102 is executed for the second time after step S112, the division number N becomes 2, and thereafter, the division number N increases each time the process is repeated.

[0059] Furthermore, when the division number N increases, the waypoint subset division unit 203 may increase the division number by further dividing the subsets that have already been generated, or may increase the division number by some other method.

[0060] [Robot arm configuration data] FIG. 3 is an explanatory diagram showing an example of a table configuration of the robot arm configuration data 401 stored in the robot arm configuration storage unit 301 in the embodiment of the present invention.

[0061] The table of robot arm configuration data 401 is made up of categories of joint information and link information.

[0062] Joint information is information about each joint that makes up the robot arm, such as the joint name, joint type, joint position, joint orientation, joint lower limit, joint upper limit, maximum acceleration, and maximum speed.

[0063] Link information is information that represents the configuration of the links that make up the robot arm, and includes, for example, link names, parent joint names, child joint names, and link shapes. Link shapes are the actual shapes of the links, and are, for example, solid data saved in a format such as STEP (Standard for the Exchange of Product model data) or polygon data saved in a format such as STL (STereoLithography).

[0064] [Interference object configuration data] FIG. 4 is an explanatory diagram showing an example of a table configuration of the interfering object configuration data 402 stored in the interfering object configuration storage unit 302 in the embodiment of the present invention.

[0065] The table of interference object configuration data 402 has an interference object ID 421, an interference object shape 422, and an interference object position and orientation 423. The interference object shape 422 indicates the shape of the interference object, and is, for example, solid data saved in a format such as STEP, or polygon data saved in a format such as STL. The interference object position and orientation 423 is information indicating the position and orientation of the interference object in space, and is, for example, information indicating an AFFINE transformation matrix, or a position in three-dimensional space and the orientation at that time expressed by Roll-Pitch-Yaw, etc.

[0066] [Via point data] FIG. 5 is an explanatory diagram showing an example of a table configuration of the via point data 403 stored in the via point storage unit 303 in the embodiment of the present invention.

[0067] The table of via point data 403 consists of an orientation ID 431, a target link name 432, and position and orientation information 433. The target link name 432 corresponds to the link name in the link information included in the robot arm configuration data 401. The position and orientation information 433 is information indicating the position and orientation of the hand of the robot 2 at each via point, and is, for example, information indicating an AFFINE transformation matrix, or a position in three-dimensional space and the orientation at that time expressed by Roll-Pitch-Yaw, etc.

[0068] [Trajectory data] FIG. 6 is an explanatory diagram showing an example of a table configuration of the trajectory data 404 stored in the trajectory storage unit 304 in the embodiment of the present invention.

[0069] The trajectory data 404 corresponds to the output of the trajectory planning device 100 in this embodiment. The table of the trajectory data 404 consists of trajectory point IDs 441, controllers 442, joint angle information 443, and control times 444. The trajectory point IDs 441 identify points on the trajectory of each robot (i.e., trajectory points). For example, waypoints P1 to P5 in this embodiment are included in the trajectory points. The controller 442 indicates the robot to be controlled (robot 1 or robot 2 in this embodiment). The joint angle information 443 is a collection of joint angles taken by the robot to be controlled at a certain timing, and is a collection of joint names and their joint values. The control times 444 indicate the time to reach each trajectory point. For example, the control time 444 of a certain trajectory point (e.g., T102) is the time (e.g., 1.21) obtained by adding the time (e.g., 1.2) required to move from the previous trajectory point (e.g., T101) to the trajectory point (e.g., T102) (e.g., 0.01).

[0070] [Output screen] FIG. 15 is an explanatory diagram showing an example of an output screen output by the trajectory planning device 100 in the embodiment of the present invention.

[0071] When the user operates the button O101, input data entered via the input / output interface device 130 is read. When the user operates the button O102, the trajectory planning device 100 executes a trajectory planning calculation and generates a simulation screen, a table O103, a robot 1 position graph O104, and the like. Table O103 displays, for example, the number of divisions of waypoints and the operation time, and selecting a row in table O103 also changes the graph O104 showing the robot 1 position. In addition, when the user presses the operation playback button in table O103, the operations of robot 1 and robot 2 according to the created operation plan are displayed on the simulation screen (for example, an animation display of the robot's operation or a sequential display of the robot's position at each waypoint, etc.). This allows the user to confirm the operation based on the operation plan on the simulator.

[0072] [Effects, etc.] As described above, the trajectory planning device 100 of this embodiment makes it possible to realize real-time trajectory planning even for a mechanism in which multiple robots are connected.

[0073] Furthermore, the system according to the embodiment of the present invention may be configured as follows.

[0074] (1) A trajectory planning device (e.g., trajectory planning device 100) that plans a trajectory for controlling a first robot (e.g., robot 160) and a second robot (e.g., robot 170) connected to the first robot so as to be movable by the operation of the first robot, the trajectory planning device having a processing device (e.g., processing device 110) and a storage device (e.g., storage device 120), the storage device storing information indicating the configuration of the arm of the second robot (e.g., information in a robot arm configuration storage unit 301), and information indicating the positions and postures of the hand of the second robot at a plurality of waypoints through which the hand of the second robot passes in sequence (e.g., information in a waypoint storage unit 303). , and the processing device calculates a singularity surface, which is a collection of singularities of the second robot at each waypoint, based on the configuration of the arm of the second robot and the position and posture of the hand at each waypoint (e.g., step S104), determines a target position of the first robot based on the singularity surface of the second robot calculated for each waypoint and the movable range of the second robot (e.g., step S105), plans a trajectory to the target position of the first robot (e.g., step S106), and plans a trajectory for the hand of the second robot at the target position of the first robot that passes through multiple waypoints (e.g., step S107).

[0075] (2) In (1) above, the processing device divides the multiple waypoints into multiple subsets, each including one or more waypoints, and for each subset, determines a target position of the first robot based on the singularity surface of the second robot calculated for each waypoint and the moving range of the second robot so that all of the one or more waypoints included in the subset are included in the intersection of the range within the moving range of the second robot and the range outside the singularity surface of each waypoint, plans a trajectory of the first robot for each target position of the first robot determined for each subset, and plans a trajectory for the hand of the second robot at the target position of the first robot to pass through one or more waypoints included in the subset.

[0076] (3) In (2) above, if the processing device fails to plan a trajectory for the second robot's hand passing through multiple waypoints, it changes the target position of the first robot (e.g., step S110) and plans a trajectory for the second robot's hand at the changed target position of the first robot passing through multiple waypoints.

[0077] (4) In (3) above, if the processing device is unable to plan a trajectory within the moving range of the second robot that does not pass through the singularity surface of the second robot and does not interfere with other objects, it determines that the second robot's hand has failed to plan a trajectory that passes through multiple waypoints (e.g., step S108).

[0078] (5) In (3) above, if the processing device fails to plan a trajectory for the second robot's hand at the changed target position of the first robot that passes through multiple way points, the processing device divides the multiple way points into multiple subsets, each of which includes one or more of the way points (e.g., step S112).

[0079] (6) In (5) above, if the processing device fails to plan a trajectory for the second robot's hand at the target position of the first robot that passes through one or more waypoints included in any of the subsets, the processing device increases the number of divisions of the subset (e.g., step S112).

[0080] (7) In the above (1), the second robot is a robot with six degrees of freedom.

[0081] (8) In the above (1), the second robot is a six-axis vertical articulated robot.

[0082] With the above configuration, by calculating the target position of the first robot based on the range of motion and singularity surface of the second robot, it is possible to realize real-time trajectory planning even for mechanisms in which multiple robots are connected.

[0083] The present invention is not limited to the above-described embodiments, but includes various modifications. For example, the above-described embodiments have been described in detail to facilitate a better understanding of the present invention, and the present invention is not necessarily limited to those including all of the described configurations. Furthermore, it is possible to replace part of the configuration of one embodiment with the configuration of another embodiment, or to add the configuration of another embodiment to the configuration of one embodiment. Furthermore, it is possible to add, delete, or replace part of the configuration of each embodiment with other configurations.

[0084] Furthermore, the above-described configurations, functions, processing units, processing means, etc. may be partially or entirely implemented in hardware, for example, by designing them as integrated circuits. The above-described configurations, functions, etc. may also be implemented in software, with a processor interpreting and executing a program that implements each function. Information such as the programs, tables, and files that implement each function can be stored in storage devices such as nonvolatile semiconductor memory, hard disk drives, and solid-state drives (SSDs), or in computer-readable, non-transitory data storage media such as IC cards, SD cards, and DVDs.

[0085] In addition, the control lines and information lines shown are those that are considered necessary for the explanation, and not all control lines and information lines in the product are necessarily shown. In reality, it can be considered that almost all components are interconnected. [Explanation of symbols]

[0086] 100 Trajectory planning device 110 Processing device 120...Storage device 130 Input / output interface device 140 Input / output device 150 Integrated control device 160···Robot 1 170...Robot 2 201 Integrated Trajectory Planning Department 202...Robot 2 singularity surface calculation unit 203....Via point subset division part 204 Robot 1 target position calculation unit 205···Robot 1 Trajectory Planning Section 206···Robot 2 Trajectory Planning Unit 301: Robot arm configuration memory unit 302: Interference object configuration memory unit 303: Waypoint memory section 304...Trajectory storage section

Claims

1. 1. A trajectory planning device that plans a trajectory for controlling a first robot and a second robot connected to the first robot so as to be movable by an operation of the first robot, the device comprising: a processing unit and a storage unit, the storage device holds information indicating a configuration of an arm of the second robot and information indicating positions and orientations of the arm of the second robot at a plurality of waypoints through which the arm of the second robot passes in sequence; The processing device includes: dividing the plurality of waypoints into a plurality of subsets, each subset including one or more of the waypoints; calculating a singularity surface, which is a set of singularities of the second robot at each of the via points, based on a configuration of the arm of the second robot and the position and posture of the hand at each of the via points; determining a target position of the first robot for each of the subsets based on the singularity surface of the second robot calculated for each of the way points and a movable range of the second robot so that all of the one or more way points included in the subset are included in the intersection of the movable range of the second robot and the range outside the singularity surface of each of the way points; planning a trajectory of the first robot to each of the target positions of the first robot determined for each of the subsets; For each of the subsets, a trajectory is planned for the hand of the second robot at the target position of the first robot, passing through one or more of the way points included in the subset; If the end effector of the second robot fails to plan a trajectory that passes through a plurality of waypoints, changing the target position of the first robot; a trajectory planning device for planning a trajectory for the hand of the second robot at the changed target position of the first robot to pass through the plurality of waypoints;

2. A trajectory planning device according to claim 1, the processing device determines that the hand of the second robot has failed to plan a trajectory that passes through the plurality of waypoints when it is unable to plan a trajectory that does not pass through the singularity surface of the second robot and does not interfere with other objects within the movable range of the second robot.

3. A trajectory planning device according to claim 1, The processing device is characterized in that, when the hand of the second robot at the changed target position of the first robot fails to plan a trajectory that passes through the plurality of waypoints, the processing device divides the plurality of waypoints into a plurality of subsets, each of which includes one or more of the waypoints.

4. A trajectory planning device according to claim 3, The processing device is characterized in that, in any of the subsets, when the hand of the second robot at the target position of the first robot fails to plan a trajectory that passes through one or more of the waypoints included in the subset, increases the number of divisions of the subset.

5. A trajectory planning device according to claim 1, A trajectory planning device, wherein the second robot is a robot having six degrees of freedom.

6. A trajectory planning device according to claim 1, The trajectory planning device is characterized in that the second robot is a six-axis vertical articulated robot.

7. A trajectory planning method in which a trajectory planning device plans a trajectory for controlling a first robot and a second robot connected to the first robot so as to be movable by the operation of the first robot, comprising: The trajectory planning device includes a processing device and a storage device, the storage device holds information indicating a configuration of an arm of the second robot and information indicating positions and orientations of the arm of the second robot at a plurality of waypoints through which the arm of the second robot passes in sequence; The trajectory planning method includes: The processing device divides the plurality of waypoints into a plurality of subsets, each subset including one or more of the waypoints; a step in which the processing device calculates a singularity surface, which is a set of singularities of the second robot at each of the waypoints, based on a configuration of the arm of the second robot and the position and posture of the hand at each of the waypoints; a step by the processing device of determining, for each of the subsets, a target position of the first robot based on the singularity surface of the second robot calculated for each of the way points and the movable range of the second robot, so that all of the one or more way points included in the subset are included in the intersection of the range within the movable range of the second robot and the range outside the singularity surface of each of the way points; a step in which the processing device plans a trajectory of the first robot to each target position of the first robot determined for each of the subsets; a step by the processing device of planning, for each of the subsets, a trajectory of the hand of the second robot at the target position of the first robot passing through one or more of the way points included in the subset; a step of changing a target position of the first robot when the processing device fails to plan a trajectory for the hand of the second robot passing through a plurality of waypoints; a procedure by the processing device of planning a trajectory for the hand of the second robot at the changed target position of the first robot to pass through the plurality of waypoints.

8. A trajectory planning program for controlling a trajectory planning device that plans a trajectory for controlling a first robot and a second robot connected to the first robot so as to be movable by the operation of the first robot, comprising: The trajectory planning device includes a processing device and a storage device, the storage device holds information indicating a configuration of an arm of the second robot and information indicating positions and orientations of the arm of the second robot at a plurality of waypoints through which the arm of the second robot passes in sequence; The trajectory planning program dividing the plurality of waypoints into a plurality of subsets, each subset including one or more of the waypoints; a step of calculating a singularity surface, which is a set of singularities of the second robot at each of the waypoints, based on a configuration of the arm of the second robot and the position and posture of the hand at each of the waypoints; a step of determining, for each of the subsets, a target position of the first robot based on the singularity surface of the second robot calculated for each of the way points and a movable range of the second robot, so that all of the one or more way points included in the subset are included in the intersection of the movable range of the second robot and the range outside the singularity surface of each of the way points; a step of planning a trajectory of the first robot to the target position for each of the target positions of the first robot determined for each of the subsets; a step of planning, for each of the subsets, a trajectory of the hand of the second robot at the target position of the first robot, passing through one or more of the way points included in the subset; a step of changing a target position of the first robot when the end effector of the second robot fails to plan a trajectory that passes through a plurality of waypoints; and a procedure for planning a trajectory for the hand of the second robot at the changed target position of the first robot to pass through the plurality of waypoints.

Images