Trajectory generator

Abstract

To enhance the efficiency of time optimization of a trajectory for operating a moving body such as a robot; and to accurately verify the feasibility of the trajectory.SOLUTION: A trajectory generator comprises: a trajectory candidate calculation unit 13A that calculates and obtains multiple trajectory candidates that have different route states taken until reaching a target operating state; a time optimization unit 13B that performs optimization to obtain the shortest operating time that can be achieved by a drive mechanism for each of the multiple trajectory candidates; an evaluation unit 13C that evaluates each of the multiple optimized trajectory candidates using predetermined evaluation items; and a verification unit 13D that verifies feasibility of obstacle avoidance in a process of reaching the target operating state for the multiple evaluated trajectory candidates. One having been most highly evaluated by the evaluation unit, of the multiple trajectory candidates with the feasibility of obstacle avoidance being evaluated by the verification unit, is set as the trajectory to be taken until reaching the target operating state.SELECTED DRAWING: Figure 2

Description

[Technical Field]

[0001] The present invention relates to a device for generating a trajectory for a moving body, such as a robot, that moves or operates autonomously. [Background technology]

[0002] Patent Document 1 describes an example of this type of trajectory generation device. Briefly describing its configuration, the device generates a trajectory for a robot to travel from its current position to a target position, or a trajectory for an arm that grasps an object, and includes a control device that is mounted on the robot or installed externally to the robot. The control device is primarily a computer, and its functional configuration includes a calculation unit, an evaluation unit, a check unit, and an optimization unit.

[0003] The calculation unit generates multiple candidate movement paths (trajectories) from the current state (e.g., current position) to a target state (e.g., target position). The evaluation unit evaluates the generated candidate trajectories based on predetermined criteria. Evaluation criteria are set appropriately as needed, and include, for example, satisfaction of constraints such as distance and movement limits. The check unit is functionally configured to check (verify) the feasibility of interference avoidance, verifying each candidate trajectory based on three-dimensional (3D) or two-dimensional (2D) data prepared in advance, and sorting the multiple verified candidate trajectories in order of highest evaluation. The optimization unit is functionally configured to optimize operation time, and controls each part of the robot to operate in the optimal time (shortest time) along the trajectory with the highest evaluation after verification.

[0004] By repeatedly calculating, evaluating, checking, and time-optimizing the above trajectory candidates in a short period of time, the trajectory following the waypoint passed by in the current operation is determined, and the trajectory for the short section (between waypoints) thus determined is connected to determine the trajectory to the final target state (target position), and the robot is controlled (operates) according to this trajectory to reach the target state (target position). [Prior art documents] [Patent documents]

[0005] [Patent Document 1] Japanese Patent Publication No. 2020-093364 Summary of the Invention [Problem to be solved by the invention]

[0006] The device described in the aforementioned Patent Document 1 calculates multiple trajectory candidates and then evaluates them. The evaluation items include software conditions and the length of the trajectory. However, because time optimization is performed after checking (verification), the evaluation does not cover robot movements that change from moment to moment. Therefore, it is not possible to perform evaluations such as extracting trajectories with short robot movement times. Similarly, the trajectory candidates verified by the checking unit are trajectory candidates that do not include data on the robot's movements (or time factors), meaning that the exact movements are not known, and therefore the verification may be inaccurate. Furthermore, because the verification does not include movements that depend on time, it may be impossible to verify collisions with other moving objects in the robot's operating environment.

[0007] Furthermore, in the conventional technology described in Patent Document 1, time optimization of trajectory candidates is performed in the final stage after checking (verification), so the target of the time optimization is limited to the one trajectory candidate with the highest evaluation among the trajectory candidates that have passed the check section, which may reduce the probability of success of the time optimization. Furthermore, the trajectory candidate targeted in the above time optimization has been verified for feasibility (feasibility of interference avoidance) by setting detailed waypoints, and is therefore restricted by the need to pass through detailed waypoints (route states), which may reduce the success rate of the time optimization or lengthen the calculation time.

[0008] The present invention has been made in light of the above technical problems, and aims to provide a trajectory generation device that can improve the efficiency of time optimization of trajectories and accurately verify the feasibility of trajectories. [Means for solving the problem]

[0009] In order to achieve the above-mentioned object, the present invention provides a trajectory generation device that generates a trajectory to reach a target operating state of a moving body that performs a predetermined operation by a drive mechanism, the trajectory generation device comprising: a trajectory candidate calculation unit that calculates and obtains a plurality of trajectory candidates that have different route states to reach the target operating state; a time optimization unit that performs optimization to find the shortest operating time that can be achieved by the drive mechanism for each of the plurality of trajectory candidates; an evaluation unit that evaluates each of the optimized plurality of trajectory candidates using predetermined evaluation items; and a verification unit that verifies the feasibility of obstacle avoidance in the process of reaching the target operating state for the evaluated plurality of trajectory candidates, and is configured to select the trajectory candidate that has the highest evaluation by the evaluation unit as the trajectory to reach the target operating state.

[0010] In the present invention, the evaluation unit may be configured to order the plurality of time-optimized trajectory candidates in order of the optimized operation time.

[0011] In the present invention, the obstacle may include an object that moves within the environment in which the moving body exists. [Effects of the Invention]

[0012] According to the present invention, multiple trajectory candidates calculated by the trajectory candidate calculation unit are time-optimized before evaluation by the evaluation unit. This makes it easier to achieve successful optimization compared to when only one trajectory candidate is used for time optimization, and therefore reduces the calculation time. Furthermore, since the trajectory candidates targeted by the evaluation unit have known operation times (information about operation times), it is possible to rank the trajectory candidates by operation time, which is a factor in ultimately selecting a trajectory, and thus facilitates the selection of a trajectory from multiple candidates. Furthermore, because the trajectory candidates have operation times set by the time optimization unit, the feasibility of obstacle avoidance can be verified, even if the obstacle is a moving object, to determine whether or not there is interference. In other words, it is possible to verify the feasibility of avoiding collisions or contact with dynamic obstacles. [Brief explanation of the drawings]

[0013] [Figure 1] 1 is a front view schematically showing a robot as an example of a moving body according to the present invention; [Figure 2] FIG. 2 is a block diagram illustrating the functional configuration of a controller. [Figure 3] FIG. 10 is a diagram illustrating the feasibility of avoiding contact or collision between a trajectory candidate and a moving obstacle. DETAILED DESCRIPTION OF THE INVENTION

[0014] Next, an embodiment of the present invention will be described with reference to the accompanying drawings. Note that the embodiment described below is merely an example of how the present invention can be implemented, and is not intended to limit the present invention.

[0015] An example of a mobile object in an embodiment of the present invention is a robot that can move autonomously and has a moving part such as an arm. An example of such a robot is shown schematically in FIG. 1. The robot 1 shown here may have a configuration similar to that of the robot described in the aforementioned Patent Document 1, or may be a robot with some modifications. The robot 1 is provided with a carriage 3 for self-propelled movement attached to the bottom of a main body (or trunk) 2. The carriage 3 is configured to drive wheels 4 using a motor (or drive mechanism) (not shown) and to change the direction of the wheels 4 so that the robot 1 can move in an appropriate direction within the environment in which it is placed. Note that the carriage 3 may be configured to walk on two legs instead of moving using wheels 4.

[0016] An arm 5, modeled after a human body, is attached to the torso 2. The arm 5 is a so-called operating unit for performing some task and may have a suitable configuration, such as a configuration simulating a human arm or a two-stage folding configuration. In the example shown in FIG. 1 , a first arm 7 is connected to the torso 2 via a first joint 6, and a second arm 9 is connected to the distal end of the first arm 7 via a second joint 8. Furthermore, a gripper 11 is connected to the distal end of the second arm 9 via a wrist joint 10. Each of the joints 6, 8, and 10 is a so-called joint that can rotate within a predetermined angular range around two or three orthogonal axes. It may be configured to incorporate a rotation sensor such as a servomotor or resolver. In other words, the arm 5 is driven by each of the joints 6, 8, and 10 to move up and down and rotate as a whole. The gripper 11 is configured to grip and release objects by opening and closing a pair of gripping pieces corresponding to fingers.

[0017] The robot 1 further has a camera (not shown) as a sensor for acquiring information about the environment. The camera may be a two-dimensional camera or a three-dimensional camera, and is provided at a location corresponding to the front of the torso 2, or at the so-called head 12 provided at the top of the torso 2, or at the gripping part 11. Note that the robot 1 may have one camera or multiple cameras.

[0018] The robot 1 is provided with a controller (control unit) 13 that controls the movement (travel) of the robot 1, the movement of the arm unit 5, and the gripping and releasing of the gripper unit 11. The controller 13 is mainly composed of a computer including a processing element (CPU), memory elements (RAM, ROM), and various interfaces. In the example shown in FIG. 1, the controller 13 is mounted on the torso 2, but it may alternatively be mounted on the head 12, or may be installed outside the robot 1 and configured to communicate data wirelessly with the robot 1. The aforementioned sensors and cameras are configured to be able to communicate data with the controller 13.

[0019] The controller 13 is configured to perform calculations in accordance with a pre-stored program using data input from each sensor and camera, pre-stored data, and data acquired from outside via communication, and to output the results of the calculations as control command signals to the drive mechanisms of each motor in the robot 1. The control by the controller 13 is mainly movement (travel) to a target position and gripping and releasing of an object by the gripper 11. As a prerequisite for controlling such so-called operations, the controller 13 controls to set a trajectory (travel path, movement path of the arm 5 or the gripper 11, etc.) from a current state (position, posture, etc.) to a target state (position, posture, etc.). In other words, the controller 13 has a function of generating a trajectory. Therefore, the controller 13 corresponds to a trajectory generation device in an embodiment of the present invention.

[0020] The functional configuration of the controller 13 for trajectory generation is shown in a block diagram in FIG. 2. The trajectory generation function of the controller 13 generates trajectory candidates, time-optimizes the trajectory candidates, evaluates the trajectory candidates, and verifies them repeatedly within a short period of time to obtain a final trajectory to be output as a control command signal. Therefore, the controller 13 includes a trajectory candidate calculation unit 13A for obtaining trajectory candidates. The trajectory candidate calculation unit 13A may be the same as the calculation unit described in the aforementioned Patent Document 1. Essentially, the trajectory candidate calculation unit 13A generates multiple trajectories for changing the robot 1 or its gripping unit 11 from a current operating state (or position or posture) to a target operating state (or position or posture). In the following description, the target state is assumed to be the target position of the robot 1 or the gripping unit 11. For example, the target state is not limited to the final position but may be a position of a predetermined extent including the final position. In this case, it is preferable to gradually narrow the extent of the target position as the current position and the target position approach each other. In addition, the candidate trajectory is a trajectory that passes through a minimum number of intermediate states (or waypoints) that are randomly set between the current position and the target position, and is a trajectory that is set so as to satisfy constraint conditions that are set according to the actions that the robot 1 should perform.

[0021] The controller 13 is provided with a time optimization unit 13B that performs time optimization on the multiple trajectory candidates calculated by the trajectory candidate calculation unit 13A. The time optimization performed by this time optimization unit 13B may be similar to the control or calculation performed by the optimization unit described in Patent Document 1, or may be performed using the method proposed in Japanese Patent Application Laid-Open No. 2023-110178. That is, for each of the multiple trajectory candidates, the shortest time achievable by the fastest possible operation of each drive unit in the robot 1 is calculated as the optimal time. In this case, optimization may fail (the optimal time may not be obtained) for one of the trajectory candidates. However, since optimization is performed on multiple trajectory candidates, there is usually a trajectory candidate that can be optimized, resulting in a high success rate of optimization. Moreover, since there are no or few unnecessary waypoints, the optimization process time for each trajectory candidate can be shortened.

[0022] The controller 13 is provided with an evaluation unit 13C that evaluates a plurality of time-optimized trajectory candidates. The evaluation is a process of evaluating a plurality of trajectory candidates using predetermined evaluation criteria, for example, to rank the trajectory candidates. The evaluation criteria may be the movement (movement) distance or various constraint conditions, or may further be the time obtained by time optimization. The candidate with the shortest optimization time is given the highest rating, and the candidates can be ranked in descending order of optimization time. This simplifies the evaluation process.

[0023] Verification of trajectory candidates is a function of verifying whether they are valid as trajectories for actually operating the robot 1. The controller 13 is provided with a verification unit 13D that performs the verification. The verification unit 13D primarily verifies whether the robot 1 can avoid interfering objects or obstacles in the environment in which it exists. Therefore, the controller 13 pre-stores three-dimensional position data related to the environment, or four-dimensional data obtained by adding time to the three-dimensional position data. An example of verification will be described with reference to FIG. 3. From a starting point P0, a robot travels along a trajectory R0 to a current point P1. At this point, trajectory candidates R1 and R2 to a destination position G are obtained as the results of the evaluation. Both of these trajectory candidates R1 and R2 bypass an obstacle Ob present in the environment. Furthermore, the path distance of one trajectory candidate R1 is shorter than that of the other trajectory candidate R2. Accordingly, the optimized time is shorter for one trajectory candidate R1 and longer for the other trajectory candidate R2. Therefore, the evaluation result is higher for one trajectory candidate R1 and lower for the other trajectory candidate R2.

[0024] In FIG. 3, symbol M indicates the path of the moving obstacle. One trajectory candidate R1 intersects with the moving path M at intersection C1, and the other trajectory candidate R2 intersects with the moving path M at intersection C2. For one trajectory candidate R1, the time it takes for the robot 1 to move (or operate) from the current point P1 to intersection C1 is known, and therefore can be calculated based on the optimized time. In addition, the moving obstacle moving along the moving path M is stored in advance as environmental data or detected by a sensor such as a camera, so the time it takes for the moving obstacle to reach intersection C1 can be calculated. If the time at which the robot 1 moves (or operates) to intersection C1 and the time at which the moving obstacle reaches intersection C1 match, the two will come into contact or collide. In other words, avoidance of contact or collision is no longer possible, and as a result of verification, one trajectory candidate R1 cannot be used as a trajectory for moving (operating) the robot 1.

[0025] On the other hand, for the other trajectory candidate R2, the time it takes for the robot 1 to move (or operate) from the current point P1 to the intersection C2 and the time it takes for the moving obstacle to reach the intersection C2 can be calculated in the same manner as described above. The other trajectory candidate R2 has a longer path distance and a longer optimized time than the one trajectory candidate R1, so the robot 1 will reach the intersection C2 at a later time than the time it takes for the robot 1 to reach the intersection C1 via the one trajectory candidate R1. In this case, the moving obstacle has already passed the intersection C2 and is out of the way, so the robot 1 will not come into contact with or collide with the moving obstacle. In other words, contact or collision is avoided, and the verification result shows that the other trajectory candidate R2 can be used as a trajectory for moving (operating) the robot 1.

[0026] As a result of the verification, the candidate trajectory that avoids collisions or contacts is time-optimized, satisfies the appropriately set constraint conditions, and is given a high rating in evaluation items such as short distance or operation time.Furthermore, the candidate trajectory that avoids contact or collisions with equipment in the environment, including moving obstacles, is adopted as the trajectory that will actually operate (or move) the robot 1, and each driving unit is controlled based on it.

[0027] As described above, in the embodiment of the present invention, time optimization is performed for each of a plurality of trajectory candidates, and then evaluation and verification are performed. Therefore, the evaluation and verification can be performed including data related to time. As a result, it becomes possible to perform rigorous verification that takes into account not only fixed obstacles, but also moving obstacles or environmental facilities.

[0028] As already mentioned, the present invention is not limited to the above-described embodiment, and the present invention can be implemented by appropriately modifying the above-described embodiment within the scope of the present invention. For example, trajectories can be generated not only for movements on a plane, but also for movements or movements in three-dimensional space. In short, these modified forms also fall within the scope of the present invention. Such modifications also include modifications that are considered technically equivalent. [Explanation of symbols]

[0029] 1. Robot 2. Torso 3 Bogie section 4 wheels 5 Arm section 6 First joint 7 First arm 8 Second joint 9 Second arm 10 Wrist joint 11 Gripping part 12 Head 13 Controller 13A Trajectory candidate calculation section 13B Time Optimization Section 13C Evaluation Department 13D Verification Department C1,C2 intersection G Target position M Travel Route Obstacle P0 Starting point P1 Current point R0 orbit R1,R2 trajectory candidates

Claims

1. A trajectory generation device that generates a trajectory to reach a target operating state of a moving body that performs a predetermined operation by a drive mechanism, a trajectory candidate calculation unit that calculates and obtains a plurality of trajectory candidates that have different route states to reach the target operating state; a time optimization unit that performs optimization to obtain the shortest operation time that can be achieved by the drive mechanism for each of the plurality of trajectory candidates; an evaluation unit that evaluates each of the plurality of optimized trajectory candidates based on predetermined evaluation items; a verification unit that verifies whether or not the plurality of evaluated trajectory candidates can avoid obstacles during the process of reaching the target operating state, Among the plurality of trajectory candidates whose success in avoiding the obstacle has been verified by the verification unit, the trajectory candidate with the highest evaluation by the evaluation unit is set as the trajectory leading to the target operating state. A trajectory generation device characterized by:

2. 2. The trajectory generation device according to claim 1, The evaluation unit is configured to order the plurality of time-optimized trajectory candidates by the optimized motion time. A trajectory generation device characterized by:

3. 3. The trajectory generation device according to claim 1 or 2, The obstacle includes an object that moves within the environment in which the moving body exists. A trajectory generation device characterized by:

Images