Motion Planning and Control for Robots in a Shared Workspace Using Look-Ahead Planning

Abstract

The structures and algorithms described herein employ look-ahead motion planning, in which motion planning for at least two targets is performed and the ability to transition between a preceding one of the motion plans and a subsequent (e.g., following) one of the motion plans is assessed before the robot executes the resulting motion plan. Thus, the system can determine if the robot is trapped (e.g., blocked by another robot) at the end of a first motion plan and prevent, limit, or delay execution of the second motion plan. Detection of such a condition can cause one or more corrective actions to be taken, for example, generating a new, modified, or replaced first motion plan. Other corrective actions can be moving another robot, performing motion planning for another robot to alleviate the blocking condition, and / or determining a new order for the targets.

Description

[Technical field]

[0001] CROSS-REFERENCE TO RELATED APPLICATIONS This patent application claims priority to U.S. Patent Application No. 63 / 327,917, filed April 6, 2022, the entire disclosure of which is incorporated herein by reference for all purposes.

[0002] The present disclosure relates generally to robotic motion planning and control, and more particularly to systems and methods that implement collision detection via processor circuitry to generate motion plans for efficiently driving robots and the like within a shared workspace. [Background technology]

[0003] Motion planning is a fundamental problem in robot control and robotics. A motion plan specifies a path that a robot can follow to transition from a start or current state to a goal state, typically to complete a task without colliding with any obstacles in the operating environment or with a reduced likelihood of colliding with any obstacles in the operating environment. Challenges to motion planning include the ability to perform motion planning very quickly, even as characteristics of the environment change. For example, one or more characteristics, such as the position and / or orientation of one or more obstacles in the environment, may change over time during the runtime of the robot(s). Challenges also include generating a motion plan that allows a robot to complete a task(s) efficiently, for example, in terms of duration to complete the task and / or in terms of energy consumed to complete the task(s). Challenges further include performing motion planning using relatively low-cost equipment, with relatively low energy consumption, and with a limited amount of storage (e.g., memory circuits on a processor chip circuit, for example).

[0004] One problem in robotics is the operation of two or more robots in a shared workspace (a workspace is commonly referred to as a workcell or environment), where, for example, the robots or robotic appendages of the robots may interfere with each other while performing a task.

[0005] One approach to operating multiple robots in a common workspace may be referred to as a task-level approach. The task-level approach may employ teach-and-repeat training. Engineers may ensure that the robots do not collide by defining a shared portion of the workspace and programming the individual robots such that only one robot is in the shared workspace at any given time. For example, when a first robot begins to move into the workspace, the first robot sets a flag. A controller (e.g., a microprocessor, microcontroller, programmed logic controller (PLC)) reads the flag and prevents other robots from moving into the shared workspace until the first robot deasserts the flag upon exiting the workspace. This approach is intuitive and easy to understand, implement, and troubleshoot. However, this approach necessarily has low work throughput because the use of task-level collision avoidance typically leads to at least one of the robots being idle for a significant portion of time, even if an idle robot is technically capable of performing useful work in the shared workspace.

[0006] Another approach for operating multiple robots in a common workspace employs offline planning (e.g., during configuration time prior to runtime) to achieve higher work throughput than the task-level collision avoidance-based approach mentioned above. To do so, the system may attempt to solve the planning problem in the combined joint space of all robots or robot appendages. For example, if two 6-degree-of-freedom (DOF) appendages are in the workspace, a 12-DOF planning problem must be solved. This approach allows for higher performance, but planning can be very time-consuming. The 12-DOF problem appears to be too large for conventional motion planning algorithms to solve using currently available architectures. Summary of the Invention [Problem to be solved by the invention]

[0007] One strategy to address these issues is to optimize the motion of a first robot / robot appendage and then manually optimize the motion of a second robot / robot appendage. This may involve iteratively simulating the motion to ensure that the robots / robot appendages do not collide with each other, which can require many hours of computation time. Additionally, if modifications to the workspace result in a change in the trajectory of one of the robots / robot appendages, the entire workflow must be revalidated.

[0008] Motion planning is performed to determine or generate motion plans that can be executed by a given robot to perform one or more tasks. A motion plan can specify a set of poses for a given robot to transition into, for example, to perform a task or as part of a task to achieve a goal. Each pose can be specified, for example, by a set of joint angles that define a physical configuration of one or more parts of the robot (e.g., an appendage, an end effector, or a link at the end of an arm tool) and can be mapped to a real-world space. A motion plan can specify poses that position at least a portion of the given robot (e.g., an end effector or an end of an arm tool) at a goal to perform or execute an assigned task. Motion planning for a given robot can take into account the positions, trajectories, and / or predicted trajectories of one or more other robots operating within a shared workspace. [Means for solving the problem]

[0009] The structures and algorithms described herein employ look-ahead motion planning, in which motion planning for at least two successive targets is performed before the robot executes the resulting motion plan, and the ability to transition between a preceding one of the motion plans and a subsequent (e.g., following or next) one of the motion plans is assessed. Thus, the system can determine whether the robot is trapped (e.g., blocked by another robot) or even deadlocked at the end of execution of a first motion plan and can prevent, limit, or delay execution of a second motion plan. In response to detection of such a condition, the structures and algorithms described herein can cause one or more corrective actions to be taken, such as generating a new, modified, or replaced first motion plan. Other corrective actions can include moving another robot and / or performing motion planning for another robot, mitigating a blockage or potential blockage condition, and / or determining a new order for the set of targets, the set of targets including the first target and at least a second target.

[0010] The structures and algorithms described herein advantageously implement motion planning to: i) determine a first motion plan for moving a given robot from a pose to a first end pose, the first end pose positioning at least a portion of the first robot at a first target; and ii) determine at least a second motion plan for moving the given robot from the first end pose to a second end pose, the second motion plan being determined, for example, before moving the first robot according to the first motion plan or a modified first motion plan. Such may, for example, enable motion planning for two or more targets to be performed before a given robot executes a motion plan. Such may, for example, enable the system to consider how one or more subsequent motion plans for each target affect one or more prior motion plans (e.g., a subsequent motion plan is a next motion plan that is a motion plan executed most immediately after execution of a prior motion plan). In response to determining that another robot (e.g., a second robot) blocks or is likely to block movement of a given robot (e.g., a first robot), various corrective actions can be taken. For example, if execution of a first motion plan places the given robot in a posture that makes execution of a subsequent motion plan impossible or difficult or further delayed (e.g., likely to be captured, deadlocked, blocked, or blocked by another robot), one or more corrective actions can be taken.

[0011] The corrective action may include, for example, generating a new or modified or replaced motion plan for transitioning to the first goal with the first robot in an ending pose different from the ending pose of the previously generated first motion plan. The new or modified or replaced motion plan may, for example, result in a pose at the first goal that better configures or orients the given robot to achieve a next or subsequent goal (e.g., a second goal). The new or modified or replaced motion plan may, for example, result in a pose at the first goal that improves the ability or likelihood of executing a next motion plan to achieve a next or subsequent goal by the given robot. The new or modified or replaced motion plan may, for example, result in a pose at the first goal that enables a transition between movements specified by the first motion plan and movements specified by the next motion plan of the new or modified or replaced motion.

[0012] Corrective action may include, for example, moving another robot that is blocking or potentially blocking a given robot so that it does not block or no longer blocks the trajectory of the given robot when executing a next or subsequent motion plan.

[0013] Corrective action may include, for example, generating a motion or modified motion plan for another robot that specifies a trajectory for the other robot that does not block, or is more likely to not block, the given robot as the given robot moves along the trajectory specified by its motion plan, or otherwise reduces the probability of a collision occurring between the given robot and the other robot.

[0014] The corrective action may include, for example, determining or generating a new order for the set of goals, where the set of goals includes the first goal and at least the second goal.

[0015] The various techniques described herein can advantageously facilitate the operation of two or more robots operating within a shared workspace or workcell, and operate to efficiently move each of the one or more robots to one or more targets to perform respective tasks within the shared workspace, while preventing or at least reducing the risk of the robots or robotic appendages of the robots colliding with one another.

[0016] The structures and algorithms described herein enable high degree of freedom robots to avoid collisions and continue to work efficiently in a changing shared environment. The efficient planning methods can generate collision-free motion plans in milliseconds, accelerated with or without hardware acceleration. Ultra-fast "real-time" motion planning allows robot paths to be determined at run-time during task execution, without the need for training or time-intensive path optimization. This can advantageously enable the coordination of multiple robots in a shared workspace in an efficient manner.

[0017] The structures and algorithms described herein, at least in some implementations, can result in efficient, collision-free robot movement for a robot in a workspace shared by multiple robots. Collision-free motion can occur for all parts of the robot (e.g., base, robot appendages, end-of-arm tools, end effectors) even when operating at high speeds.

[0018] The structures and algorithms described herein may advantageously reduce the programming effort for multi-robot workspaces, for example, by implementing autonomous planning during runtime of one or more robots. In at least some implementations, an operator does not need to program any safety zones, time synchronization, or joint-space trajectories. Input may be limited to a description of the task(s) to be performed, or a target pose or goal and kinematic model of the robot. Input may further include representations of fixed objects, or objects (e.g., people) with optionally unpredictable trajectories.

[0019] The structures and algorithms described herein can advantageously dynamically perform motion planning, including considering the effect of a motion plan for a next target on the motion plan for a preceding target, efficiently moving the robot through various poses to perform one or more tasks with no or low probability of collision, and taking corrective action when warranted to achieve successive targets.

[0020] The structures and algorithms described herein, at least in some implementations, may enable robots to share information, for example, over a non-proprietary communication channel (e.g., an Ethernet connection), which may advantageously facilitate the integration of robots in a shared workspace, even robots from different manufacturers.

[0021] The structures and algorithms described herein, at least in some implementations, can operate without cameras or other perception sensors. In at least some implementations, coordination between the robots relies on kinematic models of the robots, the ability of the robots to communicate their respective motion plans, and a geometric model of the shared workspace. In other implementations, vision or other perception may optionally be employed, for example, to avoid humans or other dynamic obstacles, such as other robots, that may enter or occupy portions of the shared workspace.

[0022] A wide variety of algorithms are used to solve the motion planning problem. Each of these algorithms typically needs to be able to determine whether a given pose of the robot or a motion from one pose to another will result in a collision, either with the robot itself or with obstacles in the environment. The collision assessment or check may be performed "in software" using a processor executing processor-executable instructions from a stored set of processor-executable instructions to execute the algorithm. The collision assessment or check may be performed "in hardware" using a set of dedicated hardware circuits (e.g., collision checking circuits implemented in a field programmable gate array (FPGA), application specific integrated circuit (ASIC)). Such a circuit may, for example, represent a volume swept by the robot / robot appendage or a part thereof during each motion or transition between two states (i.e., a swept volume). The circuit may, for example, generate a Boolean evaluation indicating whether the motion will collide with any obstacles, at least some of which represent a volume swept in performing a motion or transition by other robots operating in a shared workspace.

[0023] In the drawings, identical reference numbers identify similar elements or acts. The sizes and relative positions of elements in the drawings are not necessarily drawn to scale. For example, the shapes and angles of various elements are not drawn to scale, and some of these elements have been arbitrarily enlarged and positioned to improve legibility of the drawings. Furthermore, the particular shapes of the depicted elements are not intended to convey any information regarding the actual shape of the particular elements, but have been selected merely for ease of recognition in the drawings. [Brief description of the drawings]

[0024] [Figure 1] FIG. 1 is a schematic diagram of a robotic system according to at least one illustrated implementation, including multiple robots operating in a shared workspace to perform tasks, including a motion planner that dynamically generates motion plans for the robots that take into account planned motions of the other robots, and optionally including a perception subsystem. [Diagram 2] A functional block diagram of an environment in which a first robot, according to at least one illustrated implementation, is controlled via a robot control system that includes a motion planner, optionally provides motion plans to other motion planners of other robots, and further includes a source of planning graphs that may be separate and distinct from the motion planner. [Figure 3A] FIG. 1 is an example motion planning graph for a robot operating in a shared workspace with a path between a current node representing a current pose and a first goal, and a corresponding first pose that positions the robot or a portion thereof at the first goal, according to at least one illustrated implementation. [Figure 3B] FIG. 1 is an exemplary motion planning graph for a robot operating in a shared workspace with a path between a first goal node and a second goal and a corresponding second pose that positions the robot or a portion thereof at the second goal, according to at least one illustrated implementation. [Figure 3C]FIG. 1 is an example motion planning graph for a robot operating in a shared workspace with a new, modified, or replacement path between a current node representing a current pose and a first goal, and a corresponding alternative first pose that positions the robot or a portion thereof at the first goal, according to at least one illustrated implementation. [Figure 3D] FIG. 1 is an example motion planning graph for a robot operating in a shared workspace with another new, modified, or replacement path between a current node representing a current pose and a first target, and a corresponding alternative first pose that positions the robot or a portion thereof at the first target, according to at least one illustrated implementation. [Figure 4] FIG. 1 is a flow diagram illustrating a method of operation in a processor-based system for generating a motion planning graph and swept volume in accordance with at least one illustrated implementation. [Diagram 5] A flow diagram illustrating a method of operation of a processor-based system for controlling one or more robots, e.g., during runtime of the robots, according to at least one illustrated implementation. [Figure 6A] FIG. 6 is a high-level flow diagram illustrating a method of operation of a processor-based system for performing motion planning for one or more robots, e.g., during runtime of the robots, according to at least one illustrated implementation that can be executed as part of performing the method of FIG. [Figure 6B] FIG. 6B is a low-level flow diagram illustrating a method of operation of a processor-based system for performing motion planning for one or more robots, e.g., during runtime of the robots, according to at least one illustrated implementation that can be executed as part of performing the methods of FIGS. 5 and 6A. [Figure 7]FIG. 5 is a flow diagram illustrating a method of operation in a processor-based system for controlling the operation of one or more robots in a multi-robot environment, according to at least one illustrated implementation, that can be executed as part of performing the method of FIG. 5, FIG. 6A, or FIG. 6B. DETAILED DESCRIPTION OF THE PREFERRED EMBODIMENTS

[0025] In the following description, certain specific details are set forth to provide a thorough understanding of various disclosed embodiments. However, those skilled in the art will recognize that the embodiments may be practiced without one or more of these specific details, or with other methods, components, materials, etc. In other examples, well-known structures related to computer systems, robots, actuator systems, and / or communication networks have not been shown or described in detail to avoid unnecessarily obscuring the description of the embodiments. In other examples, well-known computer vision methods and techniques for generating sensory data and volumetric representations of one or more objects, etc. have not been described in detail to avoid unnecessarily obscuring the description of the embodiments.

[0026] Unless the context requires otherwise, throughout the following specification and claims, the word "comprise" and variations thereof, such as "comprises" and "comprising," are to be interpreted in their open and inclusive sense, i.e., "including but not limited to."

[0027] Throughout this specification, a reference to "one implementation" or "implementation" or "one embodiment" or "embodiment" means that a particular feature, structure, or characteristic described in connection with an embodiment is included in at least one implementation or at least one embodiment. Thus, the appearances of the phrases "one implementation" or "implementation" or "in one embodiment" or "in an embodiment" in various places throughout this specification do not necessarily all refer to the same implementation or embodiment. Furthermore, particular features, structures, or characteristics may be combined in any suitable manner in one or more implementations or embodiments.

[0028] As used in this specification and the appended claims, the singular forms "a," "an," and "the" include plural referents unless the content clearly dictates otherwise. Also, it should be noted that the term "or" is generally used in its sense to include "and / or" unless the content clearly dictates otherwise.

[0029] As used in this specification and the appended claims, when used in the context of whether a collision will occur or result, the terms "determine," "determining," and "determined" mean that an assessment or prediction is made as to whether a given pose, or a movement between two poses through several intermediate poses, will result or may result in a collision between a portion of the robot and some object (e.g., another portion of the robot, a portion of another robot, a permanent obstacle, a temporary obstacle, such as a person).

[0030] As used herein and in the appended claims, the term robot refers to a machine capable of performing a complex series of actions either autonomously or semi-autonomously. A robot may take the form of a robot having a base, a robotic appendage movably coupled to the base, and an end effector or end-of-arm instrument carried by the robotic appendage along with one or more actuators for driving its movement. Such a robot may or may not resemble a human. A robot may alternatively or additionally take the form of a vehicle (e.g., an autonomous or semi-autonomous vehicle).

[0031] As used herein and in the appended claims, terms such as "first," "second," and "third" are used in a relative rather than absolute sense. Thus, the term "first motion plan" refers to a motion plan executed by a given robot before a "second motion plan," even though the "first motion plan" may not be the absolute first motion plan executed by the given robot. Similarly, the terms "first target" and "first end pose" refer to a target or end pose that occurs before a next or subsequent target or next or subsequent end pose, e.g., before a "second target" or a "second end pose," respectively. Also, a "subsequent motion plan" refers to a motion plan executed by a robot following a "previous motion plan." When used in reference to robots, the terms "first robot," "second robot," and "third robot" are simply used to distinguish between two or more different robots operating within a shared workspace, with the "first robot" in various examples typically being the robot that is the object of a particular instance of motion planning. Similarly, the term "given robot" is used to refer to the robot for which motion planning is being performed (the object of a particular instance of motion planning), while the term "other robot" or "other robots" is used to refer to robots in the shared workspace other than the "given robot".

[0032] As used herein and in the appended claims, the term "feasible path" refers to a complete path from a start or current node to a goal node, where the nodes of each successive pair of nodes in the path are joined by a respective edge that represents a valid transition between the two configurations or poses represented by the respective nodes. As used herein and in the appended claims, the term "preferred path" refers to one or more feasible paths that satisfy one or more conditions or constraints, e.g., have an associated cost or cost function that is below a threshold cost or acceptable cost. As used herein and in the appended claims, the term "selected path" refers to a feasible path selected (e.g., selected based on a cost or cost function, e.g., a low cost or low cost function) from a set of feasible paths or a set of preferred paths based on one or more criteria. As used herein and in the appended claims, the term "minimum cost path" refers to a feasible path that has the minimum cost among the feasible paths in a set of feasible paths or a set of preferred paths. It should be noted that the cost or cost function can represent an assessment of the risk or probability of collision, the severity of the collision, the expenditure or consumption of energy, and / or the time or latency associated with the feasible path.

[0033] As used herein and in the appended claims, the term "blocked" or "likely to be blocked" means that a given robot is prevented or at least delayed, or is likely to be prevented or at least delayed, in executing a motion plan (e.g., a subsequent or second motion plan) by an obstacle, e.g., by another robot. In such cases, the given robot may be trapped, i.e., prevented or at least delayed, from executing a subsequent motion plan until a new, modified, or replaced motion plan is generated for the given robot and / or until the obstacle (e.g., another robot) is moved such that it no longer blocks or is no longer likely to block the given robot when moving along a trajectory defined by the motion plan (e.g., a subsequent or second motion plan). In some cases, the given robot may even be deadlocked and unable to move or transition from a particular pose along any path or trajectory, at least until the obstacle (e.g., another robot) is moved such that it no longer blocks or is no longer likely to block the given robot. The delay can be any delay (e.g., any amount of delay other than no delay in the ability to execute a subsequent or second motion plan when commanded to execute such), or the delay can be a delay that is greater than a specified or threshold duration of the delay (e.g., a specified non-zero amount of delay in the ability to execute a subsequent or second motion plan when commanded to execute such).

[0034] The headings provided herein and the Abstract of this Disclosure are for convenience only and do not interpret the scope or meaning of the embodiments.

[0035] 1 shows a robotic system 100 including multiple robots 102a, 102b, 102c (collectively robots 102) that, according to one illustrated implementation, operate within a shared workspace 104 to perform tasks that employ look-ahead motion planning to better position the robots when their subsequent trajectories or paths are blocked or potentially blocked, e.g., by another robot, and / or if a suitable unblocked subsequent trajectory or path cannot be found. Such can improve robot operation in various ways, e.g., reducing overall time to complete a task, enabling completion of a task that otherwise could not be completed, reducing overall energy usage, reducing the occurrence of collisions, and / or reducing consumption of computational resources.

[0036] The robot 102 may take any of a wide variety of forms. Typically, a robot takes or has the form of one or more robotic appendages. The robot 102 may include one or more linkages having one or more joints and an actuator (e.g., an electric motor, a stepper motor, a solenoid, a pneumatic actuator, or a hydraulic actuator) coupled to the linkage and operable to move the linkage in response to a control or drive signal. A pneumatic actuator may include, for example, one or more pistons, cylinders, valves, a reservoir of gas, and / or a pressure source (e.g., a compressor, a blower). A hydraulic actuator may include, for example, one or more pistons, cylinders, valves, a reservoir of fluid (e.g., a low compressibility hydraulic fluid), and / or a pressure source (e.g., a compressor, a blower). The robotic system 100 may employ other forms of robots 102, for example, autonomous or semi-autonomous vehicles.

[0037] The shared workspace 104 typically represents a three-dimensional space in which the robots 102a-102c can operate and move, although in certain limited implementations the shared workspace 104 can represent a two-dimensional space. The shared workspace 104 is a volume or area in which at least portions of the robots 102 overlap in space and time or may collide if motion is not controlled to avoid collisions. Note that the workspace 104 is distinct from the "configuration space" or "C-space" of each of the robots 102a-102c, e.g., described below with reference to Figures 3A-3D.

[0038] As described herein, the robot 102a or a portion thereof may constitute an obstacle when considered from the perspective of another robot 102b (i.e., during motion planning for another robot 102b). The shared workspace 104 may further include other obstacles, such as parts of machinery (e.g., a conveyor 106), posts, pillars, walls, ceilings, floors, tables, humans, and / or animals. The shared workspace 104 may further include one or more work items or workpieces 108, such as one or more parcels, packaging, fasteners, tools, items, or other objects, that the robot 102 manipulates as part of performing a task.

[0039] The robotic system 100 can include one or more robot control systems 109a, 109b, 109c (three shown, collectively robot control systems 109), which include one or more motion planners, e.g., respective motion planners 110a, 110b, 110c (three shown, collectively motion planners 110) for each of the robots 102a, 102b, 102c. In at least some implementations, a single motion planner 110 may be employed to generate motion plans for two, more, or all of the robots 102. The motion planners 110 are communicatively coupled to control respective ones of the robots 102. The motion planner 110 is also communicatively coupled to receive various types of inputs including, for example, robot kinematic models 112a, 112b, 112c (collectively, kinematic models 112) and / or task messages 114a, 114b, 114c. The motion planner 110 is optionally communicatively coupled to receive motion plans or other representations of motion 116a, 116b, 116c (collectively 116) for other robots 102 operating within the shared workspace 104 (not shown in FIG. 1 ). The robot kinematic model 112 defines the geometry of a given robot 102, for example, in terms of joints, degrees of freedom, dimensions (e.g., linkage lengths), and / or in terms of the robot's 102's respective C-space. (As shown in FIG. 2, conversion of the robot kinematic model 112 to a motion planning graph may occur during configuration time, which occurs before runtime of the robot or before task execution by the robot, and is performed, for example, by a processor-based system that is distinct from and separate from the robotic system 100, using any of a variety of techniques.) The task messages 114a-114c specify the task to be performed, for example, in terms of goals (e.g., target positions, target poses or final poses, target configurations or states, and / or final configurations or states). Task performance may also involve transitioning or moving through several intermediate poses, configurations, or states of each robot 102.The pose, configuration, or state may be defined, for example, in the robot's configuration space (C-space), for example, in terms of the joint positions and joint angles / rotations (e.g., joint poses, joint coordinates) of each robot 102. The goal positions may be specified in real world coordinates of the workspace (e.g., 2-space or 2D-space or 3-space or 3D-space) or may be specified in terms of C-space. The term one or more goals typically refers to a location or region or area where a robot or a part thereof is positioned to perform or execute a task and from where the task may be performed or executed by the robot. In this regard, it is noted that the location where a robot may perform a task is not necessarily limited to a single point in a space or region, but rather may encompass a relatively large volume of space or region, and may not be uniform in dimension along any given axis in a given coordinate system. Thus, the terms goal(s), goal position(s) are not necessarily limited to a single point in a space or region, but may encompass a volume, region, or area, for example, a volume, region, or area where a robot may perform a defined task.

[0040] The motion planner 110 is optionally communicatively coupled to receive static object data 118a, 118b, 118c as input. The static object data 118a, 118b, 118c represent static objects 118 in the workspace 104 (e.g., size, shape, location, occupied space), which may be known in advance, for example. The static objects 118 may include, for example, fixed structures in the workspace, such as posts, pillars, walls, ceilings, floors, and / or conveyors 106. Because the robots 102 are operating in a shared workspace, the static objects 118 are typically identical for each robot. Thus, at least in some implementations, the static object data 118a, 118b, 118c provided to the motion planner 110 will be identical. In other implementations, the static object data 118a, 118b, 118c provided to the motion planner 110 may vary for each robot, for example, based on the position or orientation of the robot 102 within the environment or the environmental perspective of the robot 102. Additionally, as noted above, in some implementations, a single motion planner 110 may generate motion plans for more than one robot 102.

[0041] The motion planner 110 is optionally communicatively coupled to receive as input sensory data 120, for example provided by a perception subsystem 124. The sensory data 120 represents static and / or dynamic objects within the workspace 104 that are not known a priori. The sensory data 120 may be raw data, such as sensed via one or more sensors (e.g., cameras 122a, 122b) and / or converted by the perception subsystem 124 into digital representations of obstacles.

[0042] The optional perception subsystem 124 may include one or more processors capable of executing one or more machine-readable instructions that cause the perception subsystem 124 to generate respective discretizations of representations of the environment in which the robot 102 operates to perform tasks for a variety of different scenarios.

[0043] Optional sensors (e.g., cameras 122a, 122b) provide raw sensory information (e.g., point clouds) to the perception subsystem 124. The optional perception subsystem 124 can process the raw sensory information, and the resulting sensory data can be provided as a point cloud, an occupancy grid, a box (e.g., bounding box) or other geometric object, or a stream of voxels (i.e., a "voxel" is equivalent to a 3D or volumetric pixel) that represent obstacles present in the environment. Representations of obstacles can optionally be stored in on-chip memory. The sensory data 120 can represent which voxels or sub-volumes (e.g., boxes) are occupied in the environment at the current time (e.g., during runtime). In some implementations, when representing either a robot or another obstacle in the environment, the surface of each of the robot or obstacle (e.g., including other robots) can be represented as either a mesh of voxels or polygons (often triangles). In some cases, it is advantageous to represent objects as boxes (rectangular prisms, bounding boxes, hierarchical data structures such as octrees or sphere trees) or other geometric objects. Due to the fact that objects are not randomly shaped, there may be a significant amount of structure in the way the voxels are organized, and many voxels in an object are immediately next to each other in 3D space. Thus, representing an object as a box may require many fewer bits (i.e., it may require only the x, y, z Cartesian coordinates for two opposite corners of the box). Also, performing intersection tests on boxes is comparable in complexity to performing intersection tests on voxels.

[0044] At least some implementations can combine the output of multiple sensors, which can provide a very fine grained voxelization. However, for the motion planner to efficiently perform motion planning, coarser voxels (i.e., "processor voxels") can be used to represent the environment and volumes in the 3D space swept by the robot 102 or parts thereof as it transitions between various states, configurations, or poses. Thus, the optional perception subsystem 124 can transform the output of the sensors (e.g., cameras 122a, 122b) accordingly. At runtime, if the robot control system 200 determines that any of the sensor voxels within the processor voxel are occupied, the robot control system 200 considers the processor voxel to be occupied and generates an occupancy grid accordingly.

[0045] Various communication paths are illustrated as arrows in FIG. 1. The communication paths may take the form of, for example, one or more wired communication paths (e.g., electrical conductors, signal buses, or optical fibers) and / or one or more wireless communication paths (e.g., via RF or microwave radio and antennas, infrared transceivers). In particular, each of the motion planners 110a-110c may be communicatively coupled to one another, either directly or indirectly, to provide a motion plan for a respective one of the robots 102a-102c to the other of the motion planners 110a-110c. For example, the motion planners 110a-110c may be communicatively coupled to one another via a network infrastructure, such as a non-proprietary network infrastructure (e.g., an Ethernet network infrastructure) 126. This may advantageously enable operation of robots from different manufacturers in a shared workspace.

[0046] The term "environment" is used to refer to the robot's current workspace, which is a shared workspace where two or more robots operate in the same workspace. The environment may include obstacles and / or workpieces (i.e., items that the robot interacts or acts on or with). The term "task" is used to refer to a robot task in which the robot transitions from a pose A to a target pose B, preferably without colliding with obstacles in its environment. A target pose is a pose for the robot to perform a specified task, typically on an object, at a target location or location called the target. A task may possibly include grasping or ungrasping an item, moving or dropping an item, rotating an item, or retrieving or placing an item. The transition from pose A to target pose B may optionally include transitions between one or more intermediate poses. The term "scenario" is used to refer to a class of environment / task pairs. For example, a scenario may be "a pick and place task between x obstacles and y obstacles with sizes and shapes within a given range in an environment with a 3-foot table or conveyor." Depending on the location of the goal and the size and shape of the obstacles, there may be many different task / environment pairs that meet such criteria.

[0047] The motion planner 110 is operable to dynamically generate the motion plan 116 to cause the robot 102 to perform a task in an environment while taking into account the planned motions of the other of the robots 102 (e.g., as represented by their respective motion plans 116 or resulting swept volumes). The motion planner 110 may optionally take into account the representations of the a priori static objects 118 represented by the static object data 118a, 118b, 118c and / or the sensory data 120 when generating the motion plan 116. The motion planner 110 may advantageously take into account the known or predicted locations, poses, and / or motion states of the other robots 102 at a given time, e.g., whether another robot 102 has completed a given motion or task, and may enable recalculation of the motion plan based on the completion of a motion or task of one of the other robots, e.g., making a previously excluded path or trajectory available for selection therefrom.

[0048] As described herein, the motion planner 110 employs look-ahead motion planning to facilitate efficient operation of two or more robots operating within a shared workspace or workcell, and to prevent or at least reduce the risk that the robots or robotic appendages of the robots will collide with one another while efficiently moving one or more of the robots to one or more targets to perform their respective tasks within the shared workspace. The motion planner 110 can, for example, identify (e.g., select, generate) a first motion plan for the first robot that specifies a plurality of poses for transitioning the first robot from one pose to a first target and a corresponding first end pose, the first end pose positioning at least a portion of the first robot at the first target, and can identify (e.g., select, generate) a second motion plan for the first robot that specifies a plurality of poses for transitioning the first robot from the first end pose to a second target and a corresponding second end pose, the second end pose positioning at least a portion of the first robot at the second target. The second motion plan may be determined before the first robot executes the first motion plan. The motion planner 110 may, for example, consider how the first motion plan affects the ability of the first robot to execute a subsequent second motion plan (e.g., the second motion plan). For example, the motion planner 110 may determine that the first motion plan will place the first robot in a first end pose in which it is impossible or difficult to execute the subsequent motion plan. For example, another robot (e.g., the second robot) may or may be able to block the first robot from moving from the first end pose according to the subsequent motion plan, e.g., trapping, deadlocking, or at least delaying the first robot in executing the second motion plan.The one or more corrective actions may result from, for example, the generation of a new or modified or replaced first motion plan to position the first robot or a portion thereof at the first goal, but at a better ending pose than the first ending pose in the first motion plan, as described elsewhere herein. Such may be performed, for example, during runtime of the robot(s). The new or modified or replaced first motion plan may be identified using various heuristics to efficiently position or configure the robot to complete its task. The system may monitor other robots and move the robot toward the goal in response to a preferred path becoming unblocked, cleared, or otherwise discovered.

[0049] Optionally, the motion planner 110 can take into account the motion conditions of the robot 102, such as the occurrence or detection of a blocked trajectory or path or a predicted blocked trajectory or path, and / or the occurrence or detection of a situation in which a suitable, unblocked trajectory or path cannot be found.

[0050] FIG. 2 shows an environment, according to one illustrated implementation, in which a first robot control system 200 generates a first motion plan 206a for controlling the movement of a first robot 202, and optionally provides the first motion plan 206a and / or a representation of the motion as obstacles to another motion planner 204b of another robot control system 200b via at least one communication channel (indicated by proximity arrows, e.g., transmitter, receiver, transceiver, wireless, router, Ethernet) for controlling the other robot (not shown in FIG. 2).

[0051] Similarly, the other motion planner 204b of the other robot control system 200b generates another motion plan 206b to control the movement of the other robot (not shown in FIG. 2) and optionally provides the other motion plan 206b to the first motion planner 204a and other of the other motion planners 204b of the other robot control system 200b. The motion planners 204a, 204b may also optionally receive motion completion messages 209 indicating when motions of the various robots 202 are completed. This may enable the motion planners 204a, 204b to generate new or updated motion plans based on a current or updated status of the environment. For example, a portion of the shared workspace may be unblocked or otherwise made available for a second robot to perform a task after the first robot 202 completes a motion that is part or all of a set of motions formed as part of completing a task by the first robot. Additionally or alternatively, the motion planner 204a can receive information (e.g., images, occupancy grid, joint positions, and joint angles / rotations) collected by various sensors or generated by the other motion planner 204b that indicates when a portion of the shared workspace can be unblocked or otherwise made available for a second robot to perform a task after the first robot 202 completes a motion that is part or all of a set of motions formed as part of completing a task by the first robot.

[0052] The robot control system(s) 200 may, for example, be communicatively coupled via at least one communication channel (indicated by adjacent arrows, e.g., transmitter, receiver, transceiver, wireless, router, Ethernet) to receive the motion planning graph 208 and / or the swept volumetric representation 211 from one or more sources 212 of the motion planning graph 208 and / or the swept volumetric representation 211. The source(s) 212 of the motion planning graph 208 and / or the swept volumetric representation 211 may be separate and distinct from the motion planners 204a, 204b, according to one illustrated implementation. The source(s) 212 of the motion planning graph 208 and / or the swept volumetric representation 211 may be one or more processor-based computing systems (e.g., server computers), which may be operated or controlled, for example, by the respective manufacturer of the robot 202 or by some other entity. Each of the motion planning graphs 208 may include a set of nodes 214 (only two are called out in FIG. 2 ) that represent a state, configuration, or posture of the respective robot, and a set of edges 216 (only two are called out in FIG. 2 ) that connect the nodes 214 of each pair of the nodes 214 and represent legal or valid transitions between the states, configurations, or postures. The states, configurations, or postures may represent, for example, a set of joint positions, orientations, postures, or coordinates for each of the joints of the respective robot 202. Thus, each node 214 may represent a posture of the robot 202 or a portion thereof, as fully defined by the postures of the joints that make up the robot 202. The motion planning graph 208 may be determined, set, or defined prior to runtime, for example, pre-runtime or during configuration time (i.e., defined prior to performing a task). The swept volume representation 211 represents a respective volume occupied by the robot 202 or a portion thereof when performing a motion or transition corresponding to the respective edge 216 of the motion planning graph 208. The swept volume representation 211 may be represented in any of a variety of forms, for example, as voxels, a Euclidean distance field, or a hierarchy of geometric objects.This advantageously allows some of the most computationally intensive work to be performed before runtime, when responsiveness is not a particular concern.

[0053] Each robot 202 may optionally include a base (not shown in FIG. 2). The base may be fixed in the environment or may be mobile therein (e.g., an autonomous or semi-autonomous vehicle). Each robot 202 may optionally include a set of links, joints, end-of-arm tools or end effectors, and / or actuators 218a, 218b, 218c (three shown, collectively 218) operable to move the links about the joints. The set of links, joints, end-of-arm tools, or end effectors typically comprise one or more appendages of the robot, which may be movably coupled to the base of the robot. Each robot 202 may optionally include one or more motion controllers (e.g., motor controllers) 220 (only one shown) that receive control signals, e.g., in the form of a motion plan 206a, and provide drive signals to drive the actuators 218. Alternatively, the motion controllers 220 may be separate from the robot 202 and communicatively coupled to the robot 202.

[0054] There may be a respective robot control system 200 for each robot 202, or alternatively, one robot control system 200 may perform motion planning for two or more robots 202. One robot control system 200 is described in detail for illustrative purposes. Those skilled in the art will recognize that this description may be applied to similar or even identical additional examples of other robot control systems 200.

[0055] The robotic control system 200 may include one or more processor(s) 222 and one or more associated non-transitory computer or processor-readable storage media, such as a system memory 224a, a disk drive(s) 224b, and / or memory or registers (not shown) of the processor(s) 222. The non-transitory computer or processor-readable storage media (e.g., system memory 224a, drive(s) 224b) are communicatively coupled to the processor(s) 222 via one or more communication channels, such as a system bus 234. The system bus 234 may employ any known bus structure or architecture, including a memory bus with a memory controller, a peripheral bus, and / or a local bus. One or more of such components may also, or instead, communicate with each other via one or more other communication channels, such as one or more parallel cables, serial cables, or wireless network channels capable of high-speed communication, such as Universal Serial Bus ("USB") 3.0, Peripheral Component Interconnect Express (PCIe), or via Thunderbolt.

[0056] The robotic control system 200 may also be communicatively coupled to one or more remote computer systems, such as a server computer (e.g., source of motion planning graph 212), desktop computer, laptop computer, ultra-portable computer, tablet computer, smartphone, wearable computer, and / or sensors (not shown in FIG. 2 ), that are directly or indirectly communicatively coupled to various components of the robotic control system 200, for example, via a network interface 227. The remote computing systems (e.g., server computers (e.g., source of motion planning graph 212)) may be used to program, configure, control, or otherwise interface with or input data (e.g., motion planning graph 208, swept volume representation 211, task specification 215) to the robotic control system 200 and various components within the robotic control system 200. Such connections may be through one or more communication channels, such as one or more wide area networks (WANs), such as Ethernet, or the Internet using the Internet Protocol. As described above, pre-runtime calculations may be performed by a system separate from the robot control system 200 or the robot 202, and runtime calculations may be performed by the processor(s) 222 of the robot control system 200, or in some implementations, may be on-board the robot 202.

[0057] As mentioned, the robotic control system 200 may include one or more processor(s) 222 (i.e., circuitry), non-transitory storage media (e.g., system memory 224a, drive(s) 224b), and a system bus 234 coupling various system components. The processor 222 may be any logical processing unit, such as one or more central processing units (CPUs), digital signal processors (DSPs), graphics processing units (GPUs), field programmable gate arrays (FPGAs), application specific integrated circuits (ASICs), programmable logic controllers (PLCs), and the like. The system memory 224a may include read only memory ("ROM") 226, random access memory ("RAM") 228, flash memory 230, and EEPROM (not shown). A basic input / output system ("BIOS") 232, which may form part of the ROM 226, contains the basic routines that help transfer information between elements within the robotic control system 200, such as during start-up.

[0058] The drives 224b may be, for example, hard disk drives for reading from and writing to magnetic disks, solid state (e.g., flash memory) drives for reading from and writing to solid state memory, and / or optical disk drives for reading from and writing to removable optical disks. The robotic control system 200 may also include any combination of such drives in various different embodiments. The drives 224b may communicate with the processor(s) 222 via a system bus 234. The drives(s) 224b may include interfaces or controllers (not shown) coupled between such drives and the system bus 234 as known to those skilled in the art. The drives 224b and their associated computer-readable media provide non-volatile storage of computer or processor-readable and / or executable instructions, data structures, program modules, and other data for the robotic control system 200. Those skilled in the art will appreciate that other types of computer-readable media capable of storing computer-accessible data may be used, such as WORM drives, RAID drives, magnetic cassettes, digital video disks ("DVDs"), Bernoulli cartridges, RAM, ROM, smart cards, and the like.

[0059] Executable instructions and data, such as an operating system 236, one or more application programs 238, other programs or modules 240, and program data 242, may be stored in the system memory 224a. The application programs 238 may include processor-executable instructions that cause the processor(s) 222 to do one or more of: generate a discretized representation of an environment in which the robot 202 will operate, including obstacles and / or target objects or workpieces in the environment in which the planned motion of other robots may be represented as obstacles; request or otherwise obtain results of collision assessments; generate a motion plan, including setting cost values ​​for edges in the motion planning graph, and evaluating available paths in the motion planning graph; identify (e.g., select, generate) feasible paths and / or motion plans; optionally store the determined motion plan; and / or provide instructions to one or more robots to move according to the motion plan. Motion planning and motion plan construction (e.g., collision detection or assessment, updating costs of edges in a motion planning graph based on collision detection or assessment, and path searching or assessment including identifying feasible paths, assessing the feasible paths for a preferred path, and optionally selecting a selected path from the feasible or preferred paths) may be performed as described herein (e.g., with reference to FIGS. 4, 5, 6A, 6B, and 7) and in the references incorporated herein by reference. Collision detection or assessment may be performed using various structures and techniques described elsewhere herein. The application program 238 may also include one or more machine-readable and machine-executable instructions that cause the processor(s) 222 to monitor the robot in an environment to determine when a trajectory or path (e.g., a feasible path) is unblocked or cleared, and, in response to the trajectory or path being unblocked or cleared, move the robot toward a goal.The application programs 238 may further include one or more machine-readable and machine-executable instructions that cause the processor(s) 222 to perform other operations, for example, optionally processing sensory data (captured via sensors). The application programs 238 may further include one or more machine-executable instructions that cause the processor(s) 222 to perform various other methods described herein and in the references incorporated herein by reference.

[0060] In various embodiments, one or more of the operations described above may be performed by one or more remote processing devices or computers linked through a communications network (e.g., network 210) via network interface 227.

[0061] While shown in FIG. 2 as stored in system memory 224a, operating system 236, application programs 238, other programs / modules 240, and program data 242 may be stored in other non-transitory computer- or processor-readable media, such as drive(s) 224b.

[0062] The motion planner 204 of the robotic control system 200 may include dedicated motion planner hardware, or may be implemented in whole or in part via the processor(s) 222 and processor-executable instructions stored in the system memory 224a and / or drive 224b.

[0063] The motion planner 204 may include or implement a motion converter 250, a collision detector 252, a cost setter 253, a path finder 254, a path analyzer 255, a look-ahead evaluator 256, and, optionally, a multi-path analyzer 257. Each of these may be implemented via one or more processors (e.g., circuits) executing logic (e.g., executable software or firmware instructions, or hardwired logic). The motion converter 250 converts the motion of the other robot into a representation of an obstacle. The motion converter 250 receives a motion plan 206b or other representation of motion from the other motion planner 204b.

[0064] The motion converter 250 may include a trajectory predictor 251 for predicting the trajectory of a transient object (e.g., other robots, e.g., other objects, including people), for example, when the trajectory of the transient object is not known (e.g., when the object is another robot but a motion plan for the other robot has not been received, when the object is not another robot, e.g., a human). The trajectory predictor 251 may, for example, assume that the object will continue its existing motion without change, both in direction, speed, and acceleration. The trajectory predictor 251 may, for example, take into account predicted changes in the motion or path of the object, such as when the path of the object will result in a collision and therefore the object may be predicted to stop or change direction, or when the object's goal is known and therefore the object may be predicted to stop upon reaching the goal. The trajectory predictor 251 may employ a learned behavior model of the object to predict the trajectory of the object, at least in some cases.

[0065] The motion converter 250 then determines an area or volume corresponding to the motion(s). For example, the motion converter can convert the motion to a corresponding swept volume, which is a volume swept by the corresponding robot or portion thereof when moving or transitioning between poses as represented by the motion plan. Advantageously, the motion converter 250 may simply queue the obstacle (e.g., swept volume) and may not need to determine, track, or indicate a time for the corresponding motion or swept volume. Although described as the motion converter 250 for a given robot 202 converting the motion of the other robot 202b to an obstacle, in some implementations the other robot 202b may provide an obstacle representation (e.g., swept volume) of a particular motion to the given robot 202.

[0066] Collision detector 252 performs collision detection or analysis to determine whether a transition or motion of a given robot 202 or part thereof results in a collision with an obstacle. As described above, the motion of other robots may be advantageously represented as obstacles. Thus, collision detector 252 can determine whether motion of one robot results in a collision with another robot moving through the shared workspace.

[0067] In some implementations, the collision detector 252 implements software-based collision detection or assessment, for example, performing bounding box-bounding box collision assessment or assessment based on a hierarchy of geometric (e.g., spherical) representations of the volume swept by the robot 202, 202b or a portion thereof during movement. In some implementations, the collision detector 252 implements hardware-based collision detection or assessment, for example, employing a set of dedicated hardware logic circuits to represent obstacles and streaming a representation of the motion through the dedicated hardware logic circuits. In hardware-based collision detection or assessment, the collision detector can use an array of one or more configurable circuits, for example one or more FPGAs 258, and can optionally generate a Boolean collision assessment.

[0068] The cost setter 253 may set or adjust costs of edges in the motion planning graph based at least in part on the collision detection or assessment. For example, the cost setter 253 may set relatively high cost values ​​for edges representing transitions between states or motions between poses that result or are likely to result in a collision. Also, for example, the cost setter 253 may set relatively low cost values ​​for edges representing transitions between states or motions between poses that do not result or are likely to not result in a collision. Setting the costs may include setting cost values ​​that are logically associated with the corresponding edges via some data structure (e.g., a field, a pointer, a table).

[0069] The path identifier 254 can determine or identify one or more feasible paths from a start node to a goal node. For example, the path identifier 254 can determine or identify a set of nodes in the motion planning graph that provides a complete path from a start or current node to a goal node, i.e., a set of nodes where each pair of consecutive nodes in the complete path has a respective valid transition therebetween, as represented by the presence of an edge connecting the nodes of the pair of nodes. The path identifier 254 can use or execute any of a variety of pathfinding algorithms. In some implementations, the path identifier 254 can determine or identify feasible paths independent of cost, creating a set of feasible paths. In other examples, the path identifier 254 can take cost into account when determining or identifying feasible paths.

[0070] The optional path analyzer 255 can use the motion planning graph with the cost values ​​to determine or identify one or more preferred paths and / or select a single path (i.e., a selected path, e.g., an optimal or optimized path). The path analyzer 255 can, for example, identify one or more paths that meet some specified criteria (e.g., cost within a threshold limit) or even select a single path (e.g., select a lowest cost path) from a set of feasible paths determined or identified by the path identifier 254. The path analyzer 255 can, for example, configure a minimum cost path optimizer that determines a minimum or relatively low cost path between two states, configurations, or attitudes, where the states, configurations, or attitudes are represented by respective nodes in the motion planning graph. The path analyzer 255 can use or execute any of a variety of path finding algorithms, e.g., a lowest cost path finding algorithm, taking into account a cost value associated with each edge that represents the likelihood of a collision and, optionally, one or more of the severity of the collision, the expenditure or consumption of energy, and / or the time or latency to execute or complete.

[0071] Various algorithms and structures for determining a minimum cost path may be used, including those implementing the Bellman-Ford algorithm, but others may be used, including but not limited to any such process in which a minimum cost path is determined as a path between two nodes in the motion planning graph 208 such that the sum of the costs or weights of its constituent edges is minimized. This process improves the technique of motion planning for the robot 102, 202 by using a motion planning graph that represents the motion of other robots as obstacles and collision detection to increase efficiency and response time for finding the "best" path to perform a task without collision.

[0072] The look-ahead evaluator 256 can evaluate whether an ending posture (e.g., a first ending posture) that would result from execution of a prior motion plan (e.g., a first motion plan) would be detrimental to execution of a subsequent motion plan (e.g., a second motion plan) and can cause corrective action to be taken if the ending posture (e.g., a first ending posture) would be detrimental. The look-ahead evaluator 256 can, for example, determine whether execution of a prior motion plan (e.g., a first motion plan) by a given robot would result in the given robot being placed in a posture (e.g., a posture at a first destination) that would subsequently cause the given robot to become trapped or even deadlocked (e.g., blocked or potentially blocked by another robot) when executing or attempting to execute a subsequent motion plan (e.g., a second motion plan). The look-ahead evaluator 256 can, for example, determine whether execution of a leading motion plan (e.g., a first motion plan) by a given robot will likely cause the given robot to be blocked (e.g., blocked by another robot) later or with a probability above a certain defined threshold when executing or attempting to execute a subsequent motion plan (e.g., a second motion plan). The look-ahead evaluator 256 can, for example, determine whether execution of a leading motion plan (e.g., a first motion plan) by a given robot will cause difficulty in generating a subsequent motion plan or subsequent motion plan with an acceptable probability of no collision (e.g., a probability equal to or greater than a defined probability of no collision or a probability below a defined probability of experiencing a collision). The look-ahead evaluator 256 can, for example, determine whether execution of a leading motion plan (e.g., a first motion plan) by a given robot will cause delays or unacceptably long delays in the execution of the subsequent motion plan due to the given robot being blocked or likely to be blocked from a transition between the first motion plan and the subsequent motion plan.The look-ahead evaluator 256 may, for example, compare respective delays associated with two or more different motion plans and determine whether any of the motion plans are acceptable (e.g., have associated delays that are below a defined delay threshold) and / or determine whether additional motion plans are generated in an attempt to find a motion plan with an acceptably small amount of delay.

[0073] The look-ahead evaluator 256 can cause a corrective action to be taken in response to, for example, determining the presence or existence of a blocking condition or a similar blocking condition, or determining that a blocking position or a similar blocking position will occur if the given robot and / or the other robot moves along their respective trajectories. In at least some implementations, the look-ahead evaluator 256 can determine or select the type of corrective action to be taken, for example, selecting from a set of different types of corrective actions based on one or more criteria. One or more of the various types of corrective actions can be implemented as described herein. For example, a new, modified, or replaced first motion plan can be generated for moving the given robot to the first target based on an analysis of a second motion plan for moving the given robot from the first target. Also, for example, a new, modified, or replaced motion plan can be generated for another robot that is blocking or likely to block the given robot. Also, for example, a new order for the set of targets can be determined or generated, where the set of targets includes the first target and at least the second target, which can be determined or generated pseudo-randomly or based on one or more heuristic methods (e.g., always attempting to move the first target one position downstream relative to the order of the set of targets being revised).

[0074] The optional multi-path analyzer 257 can analyze the total or aggregate cost associated with two or more motion plans (e.g., the aggregate cost of a first motion plan and a second motion plan). Such can be used to identify a combination of motion plans with an overall lowest cost. The multi-path analyzer 257 can, for example, consider the total or aggregate cost of two or more options for a first motion plan in conjunction with a second motion plan. Such is not limited to only two levels of motion planning depth, but can consider additional levels of motion planning (e.g., a third motion plan for transitioning from a second target to a third target). The multi-path analyzer 257 can configure a minimum-cost path optimizer that determines the lowest or relatively low-cost combination of two or more paths between two states, configurations, poses, or goals (e.g., from a starting pose through a first target to a second target) represented by respective nodes in the motion planning graph. The multi-path analyzer 257 may use or execute any of a variety of path-finding algorithms, for example, a minimum-cost path-finding algorithm, taking into account a cost value associated with each edge, the cost value representing an associated collision probability and, optionally, one or more of a collision severity, an energy expense or consumption, and / or a time or latency to execute or complete associated with the transition represented by the respective edge. In at least some implementations, the multi-path analyzer 257 may form part of the look-ahead evaluator 256.

[0075] The motion planner 204a may optionally include a pruner 260. The pruner 260 may receive information representing the completion of a motion by another robot, which is referred to herein as a motion completion message 209. Alternatively, a flag may be set to indicate completion. In response, the pruner 260 may remove the obstacle or part of the obstacle representing the currently completed motion. This may enable the generation of a new motion plan for the given robot, which may be more efficient, or may allow the given robot to tackle performing a task that was otherwise previously prevented by the motion of another robot. This approach advantageously allows the motion converter 250 to ignore the timing of the motion when generating the obstacle representation for the motion, while still achieving better throughput than using other techniques. The motion planner 204a may additionally send signals, prompts, or triggers to cause the collision detector 252 to perform new collision detection or assessment given the obstacle modifications, generate an updated motion planning graph in which the edge weights or costs associated with the edges have been modified, and cause the cost setter 253, the path identifier 254, the path analyzer 255, the look-ahead evaluator 256, and optionally the multi-path analyzer 257 to update cost values ​​and determine new or modified motion plans as appropriate.

[0076] Optionally, the motion planner 204a may include an environment converter 263 that converts output (e.g., a digitized representation of the environment) from optional sensors 262 (e.g., a digital camera) into representations of obstacles. Thus, the motion planner 204a can perform motion planning that takes into account transient objects in the environment, e.g., people, animals, etc.

[0077] The processor(s) 222 and / or the motion planner 204a may be or include any logic processing unit, such as one or more central processing units (CPUs), digital signal processors (DSPs), graphic processing units (GPUs), application specific integrated circuits (ASICs), field programmable gate arrays (FPGAs), programmable logic controllers (PLCs), etc. Non-limiting examples of commercially available computer systems include, but are not limited to, the Celeron, Core, Core2, Itanium, and Xeon families of microprocessors offered by Intel Corporation, USA, the K8, K10, Bulldozer, and Bobcat series of microprocessors offered by Advanced Micro Devices, USA, the A5, A6, A7 series microprocessors manufactured by Apple Computer, Inc., USA, the Snapdragon series of microprocessors offered by Qualcomm, Inc., USA, and the SPARC series of microprocessors offered by Oracle Corp., USA.The construction and operation of the various structures illustrated in FIG. 2 may be implemented using a method and system that is similar to that described in International Patent Application No. PCT / US2017 / 036880, filed June 9, 2017, entitled "MOTION PLANNING FOR AUTONOMOUS VEHICLES AND RECONFIGURABLE MOTION PLANNING PROCESSORS," International Patent Application Publication No. WO2016 / 122840, filed January 5, 2016, entitled "SPECIALIZED ROBOT MOTION PLANNING HARDWARE AND METHODS OF MAKING AND USING SAME," and / or International Patent Application Publication No. WO2016 / 122840, filed January 12, 2018, entitled "APPARATUS, METHOD AND ARTICLE TO FACILITATE MOTION PLANNING OF AN AUTONOMOUS VEHICLE IN AN ENVIRONMENT HAVING DYNAMIC No. 62 / 616,783, entitled "METHOD FOR IMPROVING AN INTERFACE WITH A HIGH-SPEED SYSTEM AND METHOD ...

[0078] Although not required, many of the implementations will be described in the general context of computer-executable instructions, such as program application modules, objects, or macros, stored on a computer- or processor-readable medium and executed by one or more computers or processors that can perform obstacle representation, collision assessment, and other motion planning operations.

[0079] The motion planning operations may include, without limitation, generating or converting one, more, or all of the following into a digital form, e.g., a point cloud, a Euclidean distance field, a data structure format (e.g., hierarchical format, non-hierarchical format), and / or a curve (e.g., a polynomial or spline representation): a representation of the robot geometry based on the kinematic model 112 (FIG. 1), the tasks 114 (FIG. 1), and a representation of the volumes occupied by the robot in various states or poses and / or during movements between states or poses (e.g., swept volumes). The motion planning operations may optionally include, without limitation, generating or converting one, more, or all of the representations of static or permanent obstacles or static objects 118 (FIG. 1) and / or sensory data 120 representing the static or temporary obstacles 118 (FIG. 1) into a digital form, such as a point cloud, a Euclidean distance field, a data structure format (e.g., hierarchical format, non-hierarchical format), and / or a curve (e.g., a polynomial or spline representation).

[0080] Motion planning operations may include, but are not limited to, using various collision assessment techniques or algorithms (e.g., software-based, hardware-based) to determine, detect or predict collisions for various states or poses of the robot, or motion of the robot between states or poses.

[0081] In some implementations, the motion planning operations may include, without limitation, determining one or more motion planning graphs, motion plans, or road maps, storing the determined planning graph(s), motion plan(s), or road map(s), and / or providing the planning graph(s), motion plan(s), or road map(s) to control motion of the robot.

[0082] In one implementation, the collision detection or assessment is performed in response to a function call or similar process returning a Boolean value to it. The collision detector 252 can be implemented via one or more field programmable gate arrays (FPGAs) and / or one or more application specific integrated circuits (ASICs) to perform collision detection while achieving low latency, relatively low power consumption, and increasing the amount of information that can be processed.

[0083] In various implementations, such operations may be performed entirely in hardware circuitry or as software stored in memory storage, such as system memory 224a, by one or more hardware processors 222, such as one or more microprocessors, digital signal processors (DSPs), field programmable gate arrays (FPGAs), application specific integrated circuits (ASICs), graphics processing unit (GPU) processors, program logic controllers (PLCs), electrically programmable read only memories (EEPROMs), or a combination of hardware circuitry and software stored in memory storage.

[0084] Various aspects of perception, planning graph construction, collision detection, and pathfinding that may be employed in whole or in part are also described in International Patent Application No. PCT / US2017 / 036880, entitled "MOTION PLANNING FOR AUTONOMOUS VEHICLES AND RECONFIGURABLE MOTION PLANNING PROCESSORS," filed June 9, 2017; International Patent Application Publication No. WO2016 / 122840, entitled "SPECIALIZED ROBOT MOTION PLANNING HARDWARE AND METHODS OF MAKING AND USING SAME," filed January 5, 2016; International Patent Application Publication No. WO2016 / 122840, entitled "SPECIALIZED ROBOT MOTION PLANNING HARDWARE AND METHODS OF MAKING AND USING SAME," filed January 12, 2018; No. 62 / 616783, entitled "APPARATUS, METHODS AND ARTICLES TO FACILITATE MOTION PLANNING IN ENVIRONMENTS HAVING DYNAMIC OBSTABLES," filed June 3, 2019; U.S. Patent Application No. 62 / 856,548, entitled "APPARATUS, METHODS AND ARTICLES TO FACILITATE MOTION PLANNING IN ENVIRONMENTS HAVING DYNAMIC OBSTABLES," filed June 23, 2020; and International Patent Application No. PCT / US2020 / 039193, entitled "MOTION PLANNING FOR MULTIPLE ROBOTS IN SHARED WORKSPACE," published as WO2020 / 263861. Those skilled in the art will appreciate that the illustrated implementations, as well as other implementations, can be implemented using other system architectures and configurations and / or other computing system architectures and configurations, including those of robots, handheld devices, multiprocessor systems, microprocessor-based or programmable consumer electronics, personal computers ("PCs"), networked PCs, minicomputers, mainframe computers, and the like.Implementations or embodiments or parts thereof (e.g., during configuration and runtime) may be practiced in a distributed computing environment where tasks or modules are performed by remote processing devices linked through a communication network. In a distributed computing environment, program modules may be located in both local and remote memory storage devices or media. However, where and how certain types of information are stored is important to help improve motion planning.

[0085] For example, various motion planning solutions "bake in" a roadmap (i.e., a motion planning graph) into a processor (e.g., an FPGA), with each edge in the roadmap corresponding to non-reconfigurable Boolean circuitry in the processor. Designs in which the planning graph is "baked in" to the processor pose the problem of having limited processor circuitry to store multiple or large planning graphs, and are generally not reconfigurable for use with different robots.

[0086] One solution offers a reconfigurable design that places the planning graph information in memory storage. This approach stores the information in memory instead of being burned into the circuit. Another approach uses templated reconfigurable circuits instead of memory.

[0087] As mentioned above, some of the information (e.g., a geometric model of the robot) may be captured, received, input, or provided during configuration time, prior to run time. The received information may be processed during configuration time to produce processed information (e.g., a motion planning graph) that may accelerate motion or reduce computational complexity during run time.

[0088] During runtime, for any pose or movement between poses, collision detection can be performed with respect to the entire environment, including determining whether any part of the robot will collide or is predicted to collide with another part of the robot itself, other robots or parts thereof, permanent or static obstacles in the environment, or temporary obstacles in the environment with unknown trajectories (e.g., people).

[0089] 3A, 3B, 3C, and 3D show example motion planning graphs 300a, 300b, 300c, 300d for a robot 102 (FIG. 1), 202 (FIG. 2) when the goal of the robot 102, 202 is to perform a task while avoiding collisions with static and dynamic obstacles, which may include other robots operating within a shared workspace.

[0090] In particular, the motion planning graph 300a (FIG. 3A) illustrates a first feasible path 312a between a current node 308a, which represents a current pose, and a first goal. In this example, the first goal may correspond to a node 308i, which represents a respective goal pose that positions the robot or a part thereof at the first goal. In this example, the first feasible path 312a is sequentially composed of edges between nodes 308a, 308b, 308c, 308d, 308e, 308f, 308g, 308h, and 308i, and is illustrated in FIG. 3A with a thicker line thickness than other edges in the motion planning graph 300a. For ease of reference, the first feasible path 312a may be referred to as a previous or first motion plan.

[0091] The motion planning graph 300b (FIG. 3B) shows a feasible path 314 between a first goal and a second goal. In this example, the first goal may correspond to, for example, a node 308j representing a respective goal pose that positions the robot or a part thereof at the first goal. In particular, at least in some circumstances, there may be one, two, or even more poses that position the robot or a part thereof at a given goal. Thus, there may be one, two, or even more nodes that position the robot or a part thereof at a given goal. Some poses may be more suitable to avoid the robot being trapped or even deadlocked at the given goal. In this example, the second goal may correspond to a node 308m representing a respective pose that positions the robot or a part thereof at the second goal. In this example, the feasible path 314 is sequentially constructed from edges between the nodes 308j, 308k, 308l, 308m, which are shown in FIG. 3B with a thicker line thickness than the other edges in the motion planning graph 300b. For ease of reference, the feasible path 314 may be referred to as a subsequent or second motion plan, in that it is intended to be executed after a previous or first motion plan.

[0092] In particular, an edge directly connecting a node 308i at the end of the first feasible path 312a to a node 308j in the second feasible path 314 is associated with a relatively high risk of collision. Such may represent, for example, a risk that the robot will be trapped or even deadlocked if two feasible paths are selected for a motion plan. The look-ahead motion planning described herein may take such into account and may take corrective action. As noted above, in at least some motion planning situations, there may be two or more poses (e.g., two or more goal poses) that position the robot or a portion thereof at a given target and enable the robot to perform a task. Those poses (e.g., goal poses) are represented by respective nodes in the motion planning graph. In the current example, the pose associated with node 308i and the pose associated with node 308j both position the robot or a portion thereof at the first target. In this situation, one of the available corrective actions is to perform motion planning again to determine a new, modified, or replaced first motion plan that accommodates or takes into account the second motion plan. Two different new, modified, or replaced first motion plans are illustrated in Figures 3C and 3D, respectively.

[0093] The motion planning graph 300c (FIG. 3C) shows one alternative example for a new, modified, or replaced first feasible path 312b between a current node 308a representing a current pose and a node 308j that positions the robot or a part thereof to a first target. In this example, the new, modified, or replaced first feasible path 312b is sequentially composed of edges between nodes 308a, 308b, 308c, 308d, 308e, 308f, 308g, 308h, 308n, and 308j, and is shown in FIG. 3C with a thicker line thickness than the other edges in the motion planning graph 300c. For ease of reference, the new, modified, or replaced first feasible path 312b can be named as a new, modified, or replaced previous motion plan, and as such is intended to be executed before a subsequent or second motion plan. In particular, this motion plan may include a relatively large number of poses (e.g., in this example, nine distinct poses, although in many practical applications the number of distinct poses may be much greater), yet have a relatively low associated risk of collision (e.g., as indicated by the aggregate values ​​associated with its respective edges, which in this example sum to 1).

[0094] The motion planning graph 300d (FIG. 3D) illustrates another alternative example of a new, modified, or replaced first feasible path 312c between a current node 308a representing a current pose and a node 308j that positions the robot or a part thereof at a first target. In this example, the new, modified, or replaced first feasible path 312c is sequentially composed of edges between nodes 308a, 308o, 308p, 308n, and 308j, and is shown in FIG. 3D with a thicker line thickness than the other edges in the motion planning graph 300d. For ease of reference, the new, modified, or replaced first feasible path 312d may be referred to as a new, modified, or replaced previous motion plan in that such is intended to be executed before a subsequent or second motion plan. In particular, this motion plan may have a relatively higher associated risk of collision (e.g., as indicated by the aggregate values ​​associated with its respective edges, which in this example totals 7) compared to that of FIG. 3C, but includes a relatively smaller number of postures (e.g., in this example, 5 distinct postures, although in many practical applications the number of distinct postures may be much larger) compared to that of FIG. A smaller number of postures may, at least in some implementations, be associated with less latency and / or less energy consumption. Thus, in some implementations, the higher risk of collision may be considered a reasonable tradeoff for savings in latency and / or energy consumption.

[0095] As used herein, the term feasible path refers to a complete path from a starting or current node to a goal node. The feasible paths of a set of feasible paths may be assessed to identify one or more preferred or even optimal feasible paths based at least in part on an associated cost or cost function of each feasible path, and optionally based on a threshold or acceptable cost. As described herein, the cost or cost function may represent an assessment of the risk of collision, the severity of the collision, the expenditure or consumption of energy and / or time or latency associated with the feasible path.

[0096] Each of the motion planning graphs 300a, 300b, 300c, 300d comprises a number of nodes, represented in the figures as open circles connected by edges, represented in the figures as straight lines between pairs of nodes. A subset of the nodes is referred to as nodes 308a-308p, and a subset of the edges is referred to as edges 310a-310h. Each node implicitly or explicitly represents a time and variables that characterize a state of the robot 102, 202 in the configuration space of the robot 102, 202. The configuration space is often referred to as C-space, which is the space of states or configurations or poses of the robot 102, 202 represented in the motion planning graphs 300a, 300b, 300c, 300d. For example, each node can represent a state, configuration, or pose of the robot 102, 202, which may include, but is not limited to, a position, orientation, or pose (i.e., position and orientation). A state, configuration, or pose may be represented, for example, by a set of joint positions and joint angles / rotations (e.g., joint poses, joint coordinates) for the joints of the robot 102, 202.

[0097] The edges in the motion planning graphs 300a, 300b, 300c, 300d represent valid or permitted transitions between these states, configurations, or postures of the robot 102, 202. The edges in the motion planning graphs 300a, 300b, 300c, 300d do not represent actual movements in Cartesian coordinates (2-space or 3-space), but rather represent transitions between states, configurations, or postures in the C-space of the robot. Each edge in the motion planning graphs 300a, 300b, 300c, 300d represents a transition of the robot 102, 202 between a respective pair of nodes. For example, edge 310a represents a transition of the robot 102, 202 between two nodes. Specifically, edge 310a represents a transition between the state of the robot 102, 202 in a particular configuration associated with node 308b and the state of the robot 102, 202 in a particular configuration associated with node 308c. For example, the robot 102, 202 may currently be in a particular configuration associated with node 308a. Although the nodes are shown at various distances from each other, this is for illustrative purposes only and is unrelated to any physical distance. There is no limit on the number of nodes or edges in the motion planning graph 300a, 300b, 300c, 300d, however, the more nodes and edges used in the motion planning graph 300a, 300b, 300c, 300d, the more accurately and precisely the motion planner may be able to determine a preferred or even optimal path according to one or more states, configurations, or poses of the robot 102, 202 to perform a task, since there are more feasible paths from which to select a minimum cost path.

[0098] Each edge is assigned or associated with a cost value, which may represent a collision assessment for the motion represented by the corresponding edge, and may also represent other information, such as the energy consumption associated with the transition, or the duration to complete the associated movement or transition or task.

[0099] Typically, it is desirable for the robot 102, 202 to avoid certain obstacles, e.g., other robots in the shared workspace. In some situations, it may be desirable for the robot 102, 202 to contact or closely approach certain objects in the shared workspace, e.g., to grasp or move the object or workpiece. Figures 3A, 3C, and 3D show a pair of "first" motion planning graphs 300a and "new" or "modified" or "alternative" "first" motion planning graphs 300c, 300d, respectively, that are used by a motion planner to identify feasible paths 312a, 312b, 312c (indicated by bold lines) for the robot 102, 202 to move to the first target. The feasible paths 312a, 312b, 312c are generated or selected to move the robot 102, 202 to the first target by moving through several "intermediate poses" while avoiding collisions with one or more obstacles. 3B shows a "second" motion planning graph 300b that is used by the motion planner to identify feasible paths 314 (indicated by bold lines) for the robot 102, 202 to move from a first target to a second target. A target is typically a location where the robot 102, 202 performs one or more specified tasks (e.g., picking and placing an object), and a goal pose is a posture of the robot for performing the specified task. As mentioned above, two or more nodes can be associated with respective postures that place the robot or a part thereof at a given target (e.g., two or more postures each place the robot or a part thereof at a given physical location or within a given physical volume or area or space where a task may be performed, e.g., within a physical location, volume or area or space in real-world space). Thus, it may be possible to reach a given target via two or more paths, each having a different goal node and thus a different end or goal posture from each other.

[0100] Obstacles may be digitally represented, for example, as bounding boxes, oriented bounding boxes, curves (e.g., splines), Euclidean distance fields, or hierarchies of geometric entities, whichever digital representation is most appropriate for the type of obstacle and collision detection to be performed, which may itself depend on the particular hardware circuitry employed. In some implementations, the swept volume in the roadmap for the primary agent (e.g., robot 102) is pre-computed. Examples of collision assessment are described in International Patent Application No. PCT / US2017 / 036880, filed June 9, 2017, entitled "MOTION PLANNING FOR AUTONOMOUS VEHICLES AND RECONFIGURABLE MOTION PLANNING PROCESSORS," U.S. Patent Application No. 62 / 722,067, filed August 23, 2018, entitled "COLLISION DETECTION USEFUL IN MOTION PLANNING FOR ROBOTICS," and International Patent Application Publication No. WO2016 / 122840, filed January 5, 2016, entitled "SPECIALIZED ROBOT MOTION PLANNING HARDWARE AND METHODS OF MAKING AND USING SAME."

[0101] The motion planners 110a, 110b, 110c (FIG. 1), 204 (FIG. 2), or portions thereof (e.g., collision detector 252, FIG. 2) determine or assess the likelihood or probability that a pose (represented by a node) and / or a motion or transition (represented by an edge) will result in a collision with an obstacle. In some cases, the decision results in a Boolean value, while in other cases, the decision may be expressed as a probability.

[0102] For nodes in the motion planning graphs 300a, 300b, 300c, 300d where there is a probability that a direct transition between the nodes will cause a collision with an obstacle, the motion planner (e.g., the cost setter 253 of FIG. 2) assigns cost values ​​or weights to the edges (e.g., edges 310a, 310b, 310c, 310d, 310e, 310f, 310g, 310h) of the planning graphs 300a, 300b, 300c, 300d that transition between those nodes that indicate the probability of a collision with an obstacle.

[0103] For example, the motion planner may assign a cost value or weight with a value equal to or close to zero for each of several edges of the motion planning graph 300a, 300b, 300c, 300d that have a respective probability of collision with an obstacle below a defined collision threshold probability. In this example, the motion planner assigns a cost value or weight of zero to those edges in the planning graph 300a, 300b, 300c, 300d that represent transitions or motions of the robot 102, 202 that have no or little probability of collision with an obstacle. For each of several edges of the motion planning graph 300a, 300b, 300c, 300d that have a respective probability of collision with an obstacle in the environment that exceeds a defined collision threshold probability, the motion planner assigns a cost value or weight with a value substantially greater than zero. In this example, the motion planner assigns cost values ​​or weights greater than zero to those edges in the motion planning graphs 300a, 300b, 300c, 300d that have a relatively high probability of collision with an obstacle. The particular threshold used for the probability of collision can vary. For example, the threshold can be 40%, 50%, 60%, or a lower or higher probability of collision. Also, assigning cost values ​​or weights with values ​​greater than zero can include assigning weights with magnitudes greater than zero that correspond to the respective probabilities of collision. For example, as shown in the motion planning graphs 300a, 300b, 300c, 300d, the motion planner assigns relatively high cost values ​​or weights of 10 to some edges that have a higher probability of collision, but assigns relatively low cost values ​​or weights with magnitude 0 to other edges that the motion planner has determined have a much lower probability of collision. In other implementations, the cost values ​​or weights may present a binary choice between collision and no collision, where there are only two cost values ​​or weights to choose from when assigning cost values ​​or weights to edges.

[0104] The motion planner (e.g., cost setter 253 of FIG. 2) may assign, set, or adjust a cost value or weight for each edge based on factors or parameters (e.g., energy consumption, overall duration to complete the associated movement, transition, and / or task) in addition to based on the probability of an associated collision.

[0105] After the motion planner sets cost values ​​or weights representing the probability of the robot 102, 202 colliding with obstacles based at least in part on the collision assessment, the motion planner (e.g., path identifier 254, path analyzer 255, FIG. 2 ) identifies or generates feasible paths (e.g., feasible paths with valid transitions between a current or start node and a goal node), and optionally, identifies preferred or optimal feasible paths 312 a, 312 b, 312 c, 312 d, 312 e, 312 f, 312 i, 312 i, 312 d ... Optimization is performed to identify (e.g., determine, select) a suitable path (e.g., a feasible path that satisfies one or more conditions or thresholds) within the resulting motion planning graph 300a, 300b, 300c, 300d that provides a motion plan for the robot 102, 202 as specified by 312b, 312c, 312d, or even select a selected feasible path 312a, 312b, 312c, 312d (e.g., an optimal feasible path or a best feasible path or a best suitable feasible path).

[0106] In one implementation, once all edge costs of the motion planning graph 300a, 300b, 300c, 300d have been assigned or set, the motion planner (e.g., the path analyzer 255, FIG. 2) may perform calculations to determine a minimum cost path to or towards the goal represented by the goal node 308i. For example, the path analyzer 255 (FIG. 2) may perform a minimum cost path algorithm from the current state of the robot 102, 202 represented by the current node 308a in the motion planning graph 300a, 300b, 300c, 300d to a possible state, configuration, or pose. The minimum cost (closest to zero) path in the motion planning graph 300a, 300b, 300c, 300d is then selected by the motion planner. As explained above, the cost may reflect not only the probability of collision but also other factors or parameters (e.g., energy consumption, overall duration to complete the associated moves, transitions, and / or tasks). 3A and 3B, the current state, configuration, or pose of the robot 102, 202 in the motion planning graph 300a, 300b is at node 308a, and a first goal state, configuration, or pose of the robot 102, 202 that places at least a portion of the given robot (e.g., an end effector, end of an arm tool) at a target for performing a given task is at node 308i. In FIG. 3A, a feasible path between the current node 308a and the goal node 308i is depicted in the motion planning graph 300a as a feasible path 312a (a bold path with a segment extending continuously from node 308a through node 308i). In FIG. 3B, a feasible path (e.g., modified) between the first goal node 308j and the second goal node 308m is depicted as a feasible path 314 in the motion planning graph 300b (a bold path comprising a segment extending from node 308j through node 308m, with intermediate nodes 308k and 308l).

[0107] Although shown as feasible paths 312a, 314, 312b, 312c in the motion planning graphs 300a, 300b, 300c, and 300d, respectively, that involve many sharp turns, such turns do not represent corresponding physical turns in the route, but rather represent logical transitions between states, configurations, or poses of the robot 102, 202. For example, each edge in the identified feasible paths 312a, 314, 312b, 312c may represent a state change with respect to the physical configuration of the robot 102, 202 in the environment, but does not necessarily represent a change in orientation of the robot 102, 202 that corresponds to the angles of the feasible paths 312a, 314, 312b, 312c shown in Figures 3A, 3B, 3C, and 3D, respectively.

[0108] 4 illustrates a method 400 of operation in a processor-based system for generating a motion planning graph and swept volume according to at least one illustrated implementation. Method 400 may be performed prior to runtime, e.g., during configuration time. Method 400 may be performed by a separate, possibly remote, processor-based system (e.g., a server computer) separate from one or more robots and / or one or more robot control systems.

[0109] A motion planning graph takes significant time and resources to construct, but as described herein, it would be necessary to do so only once, e.g., during configuration time occurring prior to runtime. Once generated, the motion planning graph may be stored, e.g., stored in a planning graph edge information memory or other non-transitory computer or processor readable medium, and it is relatively quick and efficient for a processor to swap in and out motion planning graphs, or select which motion planning graph to use, based, for example, on the current characteristics of the robot (e.g., when the robot is gripping an object of a particular size, etc.).

[0110] As mentioned above, some pre-processing activities may be performed prior to runtime, and thus in some implementations, these operations may be performed by a remote processing device linked to the robot control system via a network interface through a communication network. For example, a pre-runtime programming or configuration phase allows for preparation of the robot for a problem of interest. In such implementations, extensive pre-processing is utilized to avoid runtime calculations. Pre-processing may include, for example, generating pre-computed data (i.e., calculated prior to the runtime of the robot) regarding volumes in the 3D space that are swept by the robot when making a transition in the motion planning graph from one state to another, the transitions being represented by edges in the motion planning graph. The pre-computed data may, for example, be stored in a memory (e.g., a planning graph edge information memory) and accessed by a suitable processor during runtime. The system may also build a family of pre-runtime motion planning graphs that correspond to different possible changing dimensional characteristics of the robot that may occur during runtime. The system then stores such planning graphs in memory.

[0111] At 402, at least one component of the processor-based system receives a kinematic model representing a robot for which motion planning is to be performed. The kinematic model represents, for example, a robot appendage having a number of links and a number of joints, a joint being between each pair of links. The kinematic model may be in any known or to be developed format. Optionally, at least one component of the processor-based system generates a data structure representation of the robot based at least in part on the kinematic model of the robot. The data structure representation includes a representation of the number of links and joints. A variety of suitable data structures and techniques can be used, for example, various hierarchical data structures (e.g., tree data structures) or non-hierarchical data structures (e.g., EDF data structures). Such can use, for example, any of the structures, methods, or techniques described in PCT / US2020 / 045270, published as WO2019 / 040979.

[0112] At 404, the processor-based system generates a motion planning graph for the robot based on the respective robot kinematic model. The motion planning graph represents each state, configuration, or posture of the robot as a respective node, and represents valid transitions between pairs of states, configurations, or postures as edges connecting corresponding pairs of nodes. Although described in terms of a graph, the motion planning graph need not necessarily be represented or stored as a traditional graph, but rather may be represented, for example, logically or within a memory circuit or computer processor using any of a variety of data structures (e.g., records and fields, tables, linked lists, pointers, trees).

[0113] At 406, the processor-based system optionally generates a swept volume for each edge of a motion planning graph of the robot for which motion planning is being performed. The swept volume represents the volume swept by the robot or a portion thereof in performing the motion or transition corresponding to the respective edge. The swept volume may be represented in any of a wide variety of forms, for example, as a hierarchy of voxels, Euclidean distance fields, spheres, or other geometric objects.

[0114] At 408, the processor-based system provides the motion planning graph and / or swept volume to a robot control system and / or a motion planner. The processor-based system may provide the motion planning graph and / or swept volume over a non-proprietary communication channel (e.g., Ethernet). In some implementations, various robots from different robot manufacturers can operate in a shared workspace. In some implementations, various robot manufacturers may operate proprietary processor-based systems (e.g., server computers) that generate motion planning graphs and / or swept volumes for various robots that the robot manufacturers produce. Each of the robot manufacturers can provide their own motion planning graphs and / or swept volumes for use by the robot controller or motion planner.

[0115] The processor-based system may use, for example, any one or more of the structures and methods described in PCT / US2020 / 045270, published as WO2019 / 040979, PCT / US2020 / 034551, published as WO2020 / 247207, and / or PCT / US2019 / 016700, published as WO2019 / 156984 to generate a motion planning graph for the robot(s) on which motion planning is to be performed and / or generate a swept volume representative of the robot(s) on which motion planning is to be performed.

[0116] 5 illustrates a method 500 of operation of a processor-based system for controlling one or more robots according to at least one illustrated implementation. Method 500 may be performed during runtime of the robot(s). Alternatively, portions of method 500 may be performed during configuration time prior to runtime of the robot(s) and portions of method 500 may be performed during runtime of the robot(s).

[0117] Method 500 may be performed for one, two, or more robots operating in a multiple robot operating environment or workspace. For example, method 500 may be performed sequentially for each of two or more robots (e.g., a first robot R1, a second robot R2, a third robot R3). For ease of discussion, method 500 is described with respect to control of a first robot R1, where a second robot R2 in the operating environment potentially represents an obstacle to the movement of the first robot R1. Those skilled in the art will recognize from this discussion that the method may be extrapolated to control one robot when two or more other robots are present in the operating environment, or to control more than one robot when two or more robots are present in the operating environment. Such may be performed, for example, by performing method 500 sequentially for each robot, and / or by successively and iteratively performing method 500 for a given robot with respect to each of the other robots that present potential obstacles to the movement of the given robot. Those skilled in the art will also recognize that method 500 may omit some of the illustrated operations, may include additional operations, and that many of the operations of method 500 may be performed or carried out in a different order than that illustrated, and / or may be performed or carried out concurrently with one another.

[0118] Method 500 may be performed at run-time, a period during which at least one of the robots is operating (e.g., moving, performing a task), typically a period during which two or more robots are operating, where run-time follows, for example, a configuration time. Method 500 may be performed by one or more processor-based systems taking the form of one or more robot control systems, although method 500 is described with respect to one processor-based system. The robot control systems may, for example, be co-located or located "on-board" each one of the robots.

[0119] The method 500 begins at 502, for example, in response to powering on the robot and / or robot control system, in response to a call or invocation from a calling routine, or in response to receiving a task to be performed by the robot.

[0120] At 504, the processor-based system optionally receives one or more robot motion planning graph(s). For example, the processor-based system may receive the motion planning graph(s) from another processor-based system that generated the motion planning graph(s) during configuration time, e.g., as described with respect to and illustrated in FIG. 4. The motion planning graph represents each state, configuration, or posture of the robot as a respective node, and represents valid transitions between pairs of states, configurations, or postures as edges connecting corresponding nodes. Although described with respect to graphs, each motion planning graph need not necessarily be represented or stored as a traditional graph, but rather may be represented, e.g., logically or within a memory circuit or computer processor, using any of a variety of data structures (e.g., records and fields, tables, linked lists, pointers, trees).

[0121] At 506, the processor-based system optionally receives a set of swept volumes of one or more robots, e.g., the robot R1 whose motion is being planned, and optionally other robots R2, R3 operating in the environment. For example, the processor-based system may receive the set of swept volumes from another processor-based system that generated the motion planning graph(s) during configuration time, e.g., as described with respect to and illustrated in FIG. 4. Alternatively, the processor-based system may generate the set of swept volumes itself, e.g., based on the motion planning graph. The swept volumes represent respective volumes swept by the robots or parts thereof when executing motions or translations corresponding to the respective edges. The swept volumes may be represented in any of a wide variety of forms, e.g., as a hierarchy of voxels, Euclidean distance fields, spheres, or other geometric objects.

[0122] At 508, the processor-based system receives a number of tasks, e.g., in the form of a task specification. The task specification specifies a robot task to be executed or performed by the robot. The task specification may specify, for example, a first task in which the robot moves from a first position and a first pose to a second position (e.g., a first target) and a second pose (e.g., a first target pose) and grasps an object at the second position, and a second task in which the robot moves the object to a third position (e.g., a second target) and a third pose (e.g., a second target pose) and releases the object at the third position. The task specification may take various forms, e.g., a high-level specification (e.g., prose and syntax) that requires parsing into lower level specifications. The task specification may take the form of, for example, a low-level specification that specifies a set of joint positions and joint angles / rotations (e.g., joint poses, joint coordinates).

[0123] At 510, the processor-based system optionally receives or generates a representation of the environment, e.g., a representation of the environment sensed by one or more sensors (e.g., camera, LIDAR). The representation can represent static or permanent objects in the environment and / or can represent dynamic or temporary objects in the environment. In some implementations, the one or more sensors capture and transmit sensory data to the one or more processors. The sensory data can be, for example, occupancy information representing respective positions of obstacles in the environment. The occupancy information is typically generated by one or more sensors positioned and oriented to sense the environment in which the robot operates or operates. The occupancy information can take the form of raw data or pre-processed data and can be formatted in any existing or later created format or schema. The sensory data can be, for example, a stream indicating which voxels or boxes are occupied by objects in the current environment. The receipt or generation of the representation of the environment can occur for a period of time, e.g., for the entire runtime period, and can occur periodically, aperiodically, continuously, and / or continuously.

[0124] For each of several iterations, the system receives or generates a respective discretization of a representation of the environment in which the robot 102 operates. Each discretization may, for example, include a respective set of voxels. The voxels of each discretization may, for example, be non-homogeneous in at least one of size and shape within the respective discretization. The respective distributions of non-homogeneity of the voxels of each discretization may, for example, differ from each other. Obstacles may, for example, be digitally represented as bounding boxes, oriented bounding boxes, or curves (e.g., splines), whichever digital representation is most appropriate for the type of obstacle and the type of collision detection to be performed, which may itself depend on the particular hardware circuitry or software algorithm employed.

[0125] The processor-based system may represent one or more static or permanent obstacles in the environment, including, for example, one or more other robots, and / or one or more dynamic or temporary obstacles in the environment.

[0126] For example, at least one component of the processor-based system may receive occupancy information representative of a number of permanent obstacles in the environment and / or a number of temporary obstacles in the environment. Permanent obstacles are obstacles that generally remain stationary or fixed and do not move during performance of one or more tasks by the robot at least during the runtime of the robot. Temporary obstacles are obstacles that appear / enter, disappear / leave, or are dynamic or moving at least during the runtime of the robot. Thus, temporary obstacles, if any, have a known position and orientation in the environment during at least a portion of the runtime and did not have a known fixed position or orientation in the environment at configuration time. The occupancy information may represent, for example, a respective volume occupied by each obstacle in the environment. The occupancy information may be in any known or later developed format. The occupancy information may be generated by one or more sensors or may be defined or specified via a computer or even by a human. The occupancy information of the temporary obstacles is typically received during the runtime of the robot. Permanent or static objects may be identified and planned during configuration time, whereas temporary or dynamic objects are typically identified and planned during runtime.

[0127] At least one component of the processor-based system can generate one or more data structure representations of one or more objects or obstacles in the environment in which the robot operates. The data structure representation can include representations of some obstacles that, at configuration time, have a known fixed position and orientation in the environment and are therefore referred to as persistent in that the objects are assumed to persist in the environment in known positions and orientations from configuration time through runtime. The data structure representation can include representations of some obstacles that, at configuration time, do not have a known fixed position and orientation in the environment and / or are expected to move during runtime and are therefore referred to as transient in that the objects are assumed to appear, disappear, or move in the environment in unknown positions and orientations during runtime. Various suitable data structures can be used, for example, various hierarchical (e.g., tree data structures) or non-hierarchical (e.g., EDF data structures) and techniques, as described, for example, in PCT / US2020 / 045270, published as WO2019 / 040979. Thus, for example, at least one component of the processor-based system receives or generates a representation of obstacles in the environment as any one or more of a Euclidean distance field, a hierarchy of bounding volumes, a tree of axis-aligned bounding boxes (AABBs), a tree of oriented (non-axis-aligned) bounding boxes, a tree of spheres, a hierarchy of bounding boxes with triangular meshes as leaf nodes, a hierarchy of spheres, a k-ary sphere tree, a hierarchy of axis-aligned bounding boxes (AABBs), a hierarchy of oriented bounding boxes, and / or an octree storing voxel occupancy information. In particular, any leaf of the tree-type data structure may be of a different shape than other nodes of the data structure, e.g., all nodes are AABBs except for the root node or leaves, which may take the form of a triangular mesh. Using spheres as bounding volumes facilitates fast comparisons (i.e., it is computationally easy to determine whether spheres overlap one another).

[0128] The processor-based system may represent obstacles in the environment using, for example, any one or more of the structures and methods described in PCT / US2020 / 045270, published as WO2019 / 040979.

[0129] Optionally, at 512, the processor-based system receives motion plans for one or more other robots, i.e., for robots in the environment other than the given robot for which the particular instance of motion planning is being executed. The processor-based system can advantageously use the motion plans for the one or more other robots to determine whether a trajectory of a given robot (e.g., a first robot R1) will result in a collision or has a non-zero probability of resulting in a collision with another robot (e.g., a second robot R2, a third robot R3), as described herein.

[0130] Optionally, at 514, the processor-based system predicts the motion of obstacles in the environment, such as the motion or trajectories of other robots in the environment (e.g., the second robot R2, the third robot R3). The processor-based system can extrapolate future trajectories, for example, from sampling of current trajectories and / or from knowledge or predictions of the other robots' targets, or predictions or expected collisions of the other robots. Such can be particularly useful when motion plans for one or more other robots are not known by the processor-based system.

[0131] At 516, the processor-based system represents the other robot and / or the motions of the other robot as obstacles to the robot on which the particular instance of motion planning is being performed. For example, the processor-based system may queue a swept volume corresponding to each motion as an obstacle in an obstacle queue. The swept volume may have been previously determined or calculated for each edge and may be logically associated with each edge in memory via a data structure (e.g., a pointer). As previously mentioned, the swept volume may be represented in any of a variety of forms, for example, as a hierarchy of voxels, Euclidean distance fields, spheres, or other geometric objects.

[0132] At 518, the processor-based system optionally initializes or sets a goal counter I to 1, which enables the processor-based system to iterate in a loop through each of the total number of goals N.

[0133] At 520, the processor-based system performs motion planning for a given robot (e.g., a first robot R1) to perform several tasks (e.g., one, two, or more tasks), where the motion planning is based on a number N of at least two or more consecutive targets. The motion planning is performed for at least two or more consecutive targets before a motion specified by the resulting two or more motion plans occurs. Thus, the processor-based system can consider a motion plan for a subsequent target (e.g., a second target) in assessing compatibility or appropriateness with a motion plan (e.g., a first motion plan) for a preceding target (e.g., a first target), and optionally implement corrective action if corrective action is deemed useful or warranted (as discussed below with respect to FIG. 6A). Such can advantageously avoid a robot being caught or even deadlocked, for example, if a transition from a pose associated with a first target specified by a preceding or first motion plan to a next or subsequent pose specified by a subsequent or second motion plan is blocked or potentially blocked, for example, by another robot in the shared workspace. At least one implementation of motion planning is illustrated and described below with reference to Figures 6A and 6B, and includes collision checking and assigning costs to edges representing transitions in a motion planning graph. The examples of Figures 6A and 6B are not intended to be limiting, and other implementations of motion planning may be employed.

[0134] At 522, the processor-based system optionally controls the operation of a given robot (e.g., the first robot R1) for which a motion plan was generated, causing the given robot to move according to the motion plan. For example, the processor-based system may send control or drive signals to one or more motion controllers (e.g., motor controllers) to cause one or more actuators to move one or more linkages according to the motion plan. Note that although edges typically define valid transitions between pairs of nodes representing respective poses, any edge may correspond to one, two, or even more movements of the robot or part thereof. Thus, an edge may correspond to a single motion of the robot from one pose to another in a pair of poses, or alternatively, an edge may correspond to multiple motions of the robot from one pose to another in a pair of poses.

[0135] At 524, the processor-based system monitors the planned trajectory of a given robot (e.g., the first robot R1) for which a motion plan was generated. Monitoring can include monitoring to determine whether the robot or the trajectory of the robot is, for example, blocked by another robot. Monitoring can include monitoring to determine whether the given robot or the trajectory of the given robot may be (e.g., potentially) blocked by known or predicted movements of another robot, optionally including assessing the probability that such blockage will occur. Monitoring can include monitoring to determine whether the given robot or the trajectory of the given robot will become unblocked or potentially unblocked due to movements of another robot.

[0136] Monitoring can include monitoring to determine whether a given robot has reached a goal (e.g., a final goal) or achieved a goal pose (e.g., a final goal pose) at which a task is completed. Monitoring can also include monitoring completion of a motion by the robot or other robots, which can advantageously enable the processor-based system to remove obstacles corresponding to the motion from consideration during subsequent collision detection or assessment. In some implementations, such can result in the generation of a motion completion message that can be used, for example, to prune obstacles, as discussed below with reference to FIG. 7. The processor-based system can rely on the coordinates of the corresponding robot(s). The coordinates may be based on information from a motion controller, actuators, and / or sensors (e.g., cameras including DOF cameras and LIDAR with or without structured lighting, rotational encoders, reed switches). In at least some implementations, monitoring for blockage or unblocking can include performing collision detection using any of a wide variety of collision detection techniques, such as collision detection using swept volumes that represent the volume swept by a given robot and / or the volume swept by other robots or other obstacles in the environment, or collision detection that employs an assessment of whether two spheres intersect.

[0137] At 526, the processor-based system determines or assesses whether a given robot (e.g., the first robot R1) or a trajectory of a given robot is blocked or potentially blocked, or optionally another trigger condition has occurred. For example, the processor-based system may perform a collision check of a path or trajectory of a given robot against one or more objects or obstacles in the operating environment. Although a wide variety of forms of collision checks may be used, such collision checks should be a relatively fast process since collision checks are typically performed during runtime of the robot(s). The collision check may employ, for example, a swept volume, which represents an area or volume swept by a given robot when transitioning from one pose to another. The collision check may employ, for example, a swept volume, which represents an area or volume swept by another robot when transitioning from one pose to another. The swept volume may be advantageously calculated during configuration time prior to runtime. The collision check may employ a determination of the intersection of spheres representing portions of the given robot and the obstacle(s).

[0138] If a given robot (e.g., the first robot R1) or a given robot's trajectory is blocked or potentially blocked (e.g., with a probability of being blocked or a probability of collision above a defined threshold), or if some other trigger condition occurs, control returns to 520. If a given robot or a given robot's trajectory is not blocked or potentially blocked (e.g., the probability of being blocked or the probability of collision is below a defined threshold), control passes to 528.

[0139] At 528, the processor-based system determines whether a given robot (e.g., the first robot R1) has reached a current target I or has achieved a target pose that positions the robot or a portion thereof at target I. The processor-based system may rely on coordinates of the given robot. The coordinates may be based on information from motion controllers, actuators, and / or sensors (e.g., structured lighting, rotational encoders, cameras including DOF cameras and LIDAR with or without reed switches).

[0140] If the given robot has not reached the current objective I or achieved a goal pose that positions the robot or a portion thereof at objective I, control returns to 522 or, optionally, 524, allowing the given robot to continue moving according to the motion plan toward the current objective I. If the given robot has reached the current objective I or achieved a goal pose, control passes to 530.

[0141] At 530, the processor-based system increments the target counter (I=I+1). Control then passes to 532.

[0142] At 532, the processor-based system determines whether the total number of goals N has been reached (I>N) based on the goal counter. If the total number of goals N has not been reached (I>N), control returns to 522. If the total number of goals N has been reached (I>N), control passes to 534.

[0143] At 534, method 500 ends, e.g., until called again, at 534. Alternatively, method 500 may repeat until affirmatively stopped, e.g., by a power down state or condition. In some implementations, method 500 may be executed as a multi-threaded process on one or more cores of one or more processors.

[0144] In particular, in many implementations, the method 500 repeats for additional tasks and / or additional robots. For example, the processor-based system can determine whether the end of the task queue and / or the motion planning request queue has been reached. For example, the processor-based system can determine whether any motion planning requests remain in the motion planning request queue and / or whether all tasks in the task queue have been completed. In at least some implementations, a set of tasks (e.g., a task queue) can be temporarily depleted, with the possibility that new or additional tasks will arrive later. In such implementations, the processor-based system can execute a wait loop, occasionally checking for new or additional tasks or waiting for a signal indicating that new or additional tasks are available to be processed and executed.

[0145] In some implementations, multiple instances of method 500 may run in parallel, e.g., in a multi-threaded process on one or more cores of one or more processors. Thus, method 500 may alternatively repeat until affirmatively stopped, e.g., by a power-down state or condition.

[0146] Although the method of operation 500 is described with respect to an ordered flow, various acts or operations are performed simultaneously or in parallel in many implementations. Often, motion planning of one robot to perform a task may be performed while one or more robots are performing the task. Thus, performance of a task by a robot may overlap or be simultaneous or parallel with performance of motion planning by one or more motion planners. Performance of a task by a robot may overlap or be simultaneous or parallel with performance of a task by other robots. In some implementations, at least some portions of motion planning for one robot may overlap or be simultaneous or parallel with at least some portions of motion planning for one or more other robots.

[0147] 6A illustrates a high level method 600 of operation of a processor-based system for performing motion planning for one or more robots, according to at least one illustrated implementation. Method 600 may be performed during runtime of the robot(s). Alternatively, portions of method 600 may be performed during configuration time prior to runtime of the robot(s) and portions of method 600 may be performed during runtime of the robot(s). Method 600 may be performed, for example, in implementing motion planning 520 of method 500 (FIG. 5), although motion planning 520 is not limited to that described and illustrated in method 600.

[0148] Method 600 can be performed for one, two, or more robots operating in a multiple robot operating environment. For example, method 600 can be performed sequentially for each robot (e.g., a first robot R1, a second robot R2, a third robot R3, or even more robots). For ease of explanation, method 600 is described with respect to control of a given robot (e.g., a first robot R1), and other robots (e.g., a second robot R2, a third robot R3) in the operating environment potentially represent obstacles to the movement of the given robot (e.g., a first robot R1). Those skilled in the art will recognize from the discussion herein that method 600 can be extrapolated to motion planning for one robot when two or more other robots are present in the operating environment, or motion planning for two or more robots when two or more other robots are present in the operating environment. Such may be performed, for example, by performing method 600 for each robot sequentially and / or by iteratively performing method 600 for a given robot in succession for each of the other robots that present potential obstacles to the movement of the given robot. Those skilled in the art will also recognize that method 600 may omit some of the illustrated operations and may include additional operations, and that many of the operations of method 600 may be performed or implemented in an order different from that illustrated and / or concurrently with one another.

[0149] Method 600 can be performed at runtime, which is a period during which at least one of the robots is operating (e.g., moving, performing a task), e.g., following a configuration time. Method 600 can be performed by one or more processor-based systems in the form of one or more robot control systems. The robot control systems may be, for example, co-located or "on-board" a respective one of the robots.

[0150] Method 600 begins (602), for example, in response to powering on the robot and / or robot control system, in response to a call or invocation from a calling routine, such as a call from a calling routine that executes method 500, in response to receiving a motion planning request to be performed for the robot or in a queue of motion planning requests, or in response to receiving a task to be performed by the robot.

[0151] At 604, the processor-based system performs collision detection or assessment for a given robot (e.g., the first robot R1). The processor-based system may employ any of the various structures and algorithms described herein or in the materials incorporated by reference herein or described elsewhere to perform the collision detection or assessment. The collision detection or assessment may include performing collision detection or assessment for each motion against each obstacle in the obstacle queue. Examples of collision assessment are disclosed in International Patent Application No. PCT / US2017 / 036880, filed June 9, 2017, entitled "MOTION PLANNING FOR AUTONOMOUS VEHICLES AND RECONFIGURABLE MOTION PLANNING PROCESSORS," U.S. Patent Application No. 62 / 722,067, filed August 23, 2018, entitled "COLLISION DETECTION USEFUL IN MOTION PLANNING FOR ROBOTICS," and U.S. Patent Application No. 62 / 722,067, filed August 23, 2018, entitled "SPECIALIZED ROBOT MOTION PLANNING HARDWARE AND METHODS OF MAKING AND USING A VEHICLE ... No. WO2016 / 122840, filed Jan. 5, 2016, entitled "SAME," and PCT / US2020 / 045270, published as WO2019 / 040979, to generate a motion planning graph, generate a swept volume, and / or perform collision checking (e.g., sphere intersection assessment). As described herein, the processor-based system can use the collision assessment to set cost values ​​or functions for various edges of the motion planning graph, or as part of setting cost values ​​or functions, that can be used in generating a motion plan that specifies a preferred path (e.g., an ordered set of poses) for the robot to accomplish a task.

[0152] At 606, the processor-based system sets cost values ​​or cost functions for edges in the motion planning graph based at least in part on the collision detection or assessment for a given robot (e.g., the first robot R1). The cost values ​​or cost functions may represent a collision assessment or risk of collision (e.g., probability or occurrence). The cost values ​​may further represent a severity of the collision (e.g., amount of resulting damage) if a collision occurs. For example, a collision with a human or other living being may be considered more serious and therefore more costly than a collision with a wall or table or other inanimate object. The cost values ​​or cost functions may further represent a resource usage or consumption associated with the transition corresponding to each edge, such as an amount of energy consumed or an amount of time incurred in executing the associated transition. The processor-based system may employ any of the various structures and algorithms described herein or in the materials incorporated herein by reference to perform the setting of cost values ​​or cost functions, typically setting or adjusting the cost of edges that have no or low risk of collision to a relatively low value (e.g., zero) and setting or adjusting the cost of edges that result in collisions or have a high risk of collision to a relatively high value (e.g., 100,000). The processor-based system may set the cost values ​​or cost functions of edges in the motion planning graph, for example, by logically associating each with an associated cost value or cost function via one or more data structures.

[0153] At 608, the processor-based system generates a motion plan (e.g., a first motion plan) that specifies a set of valid transitions for transitioning or moving the given robot from a start or current configuration to a first goal I. The first motion plan represents feasible paths, and in at least some cases, a preferred path (e.g., a complete path that meets a specified criteria, having a cost below a threshold cost) or even a selected path (e.g., a best path, e.g., a complete path with the lowest cost of all paths) between the start and the first goal I. Thus, the motion plan specifies trajectories for the given robot through various poses corresponding to the respective nodes to transition between the start and the goal. An exemplary implementation of the generation of the motion plan is illustrated and described below with reference to FIG. 6B.

[0154] At 610, the processor-based system generates a motion plan (e.g., a second motion plan) that specifies a set of valid transitions for transitioning or moving the given robot from the first objective I to the second objective I+1. The second motion plan represents a feasible path, and in at least some cases, a preferred path (e.g., a complete path having a cost below a threshold cost, e.g., meeting a specified criterion) or even a selected path (e.g., a best path, e.g., a complete path having a lowest cost of all paths) between the first objective I and the second objective I+1. Thus, the motion plan specifies a trajectory for the given robot to transition between the first objective and the second objective through various poses corresponding to the respective nodes. An exemplary implementation of the generation of the motion plan is illustrated and described below with reference to FIG. 6B.

[0155] It should be noted that in some implementations, the processor-based system may iterate through the generation of subsequent motion plans for any number of subsequent goals (e.g., I+2, I+3, I+4), allowing, for example, greater consideration or exploration of the impact that previous motion plans for a given robot will have on the ability of the given robot to achieve or reach one or more subsequent goals.

[0156] At 612, the processor-based system determines or assesses whether the other robot will or is likely (i.e., relatively high probability) to block the given robot or the movement of the given robot from a preceding target (e.g., first target I) to a following or next target (e.g., second target I+1) as the given robot moves along a trajectory as specified by the corresponding motion plan (e.g., second motion plan). For example, the processor-based system may determine whether the given robot is trapped, delayed, or even deadlocked in transitioning from a preceding motion plan (e.g., first motion plan) to a following (alternatively referred to as "next" or "following") motion plan (e.g., second motion plan). For example, the processor-based system may determine whether the given robot will or is likely to be blocked from transitioning from a pose that the given robot will be in when positioned at the first target according to the first motion plan, and a pose that the given robot will be in during at least an initial portion of execution of the second motion plan. For example, a processor-based system may determine whether a given robot will be or may be blocked from executing a subsequent motion plan (e.g., a next or subsequent motion plan) due to being blocked by another robot. Also, for example, a processor-based system may determine whether a given robot will be or may be delayed in executing a subsequent motion plan (e.g., a next or subsequent motion plan) due to being blocked by another robot. In at least some instances, the delay may be any amount of delay other than no delay, e.g., no delay in the ability to execute the subsequent or second motion plan upon being commanded to execute the subsequent or second motion plan.In other cases, the delay may be longer than a specified or threshold delay duration, e.g., a specified non-zero amount of delay in the ability to execute a subsequent or second motion plan once commanded to execute the subsequent or second motion plan.

[0157] The processor-based system may employ, for example, collision detection to determine or assess whether the other robot(s) will or may block the given robot or the movement of the given robot to a subsequent target (e.g., the second target I+1) as the given robot moves along a trajectory as specified by the corresponding motion plan (e.g., the second motion plan). For example, a probability of collision exceeding a threshold probability of collision, or a cost value or cost function exceeding a threshold cost or cost value, may indicate a blocking condition. Various collision detection techniques may be employed, for example, as described elsewhere herein. If another robot or robots will or may block the given robot or the movement of the given robot to the target, control passes to 614. If another robot or robots will not or may not block the given robot or the movement of the given robot to the target (i.e., relatively low probability), control passes to 618.

[0158] Optionally, at 614, the processor-based system selects one or more corrective actions to take in response to a determination that the given robot will be blocked, or likely to be blocked, in attempting to transition from a preceding objective (e.g., the first objective I) to a subsequent, following, or next objective (e.g., the second objective I+1) as the given robot moves along the trajectory as specified by the corresponding motion plan (e.g., the second motion plan).

[0159] Examples of corrective actions include generating a new, modified, or replaced motion plan for transitioning the given robot to a prior goal (e.g., generating a new, modified, or replaced first motion plan). The new, modified, or replaced motion plan may, for example, position the given robot or a portion thereof in a first goal location, volume, area, or space, but in a different target pose than the previously generated first motion plan would have placed the given robot in. The different target pose may make it easier for the given robot to transition to a subsequent goal (e.g., second goal I+1), thereby avoiding being trapped or even deadlocked. In at least some implementations, in generating a new or modified or replaced motion plan for a given robot (e.g., generating a new or modified or replaced first motion plan), the processor-based system may be biased or otherwise induced to generate a new or modified or replaced motion plan with a goal pose that differs from a goal pose of a previously generated motion plan (e.g., a first motion plan) for the given robot. Such may be accomplished, for example, by at least temporarily increasing a cost associated with an edge connected to a node that corresponds to an ending pose of the previously generated motion plan for the given robot.

[0160] The processor-based system can optionally select between the first motion plan and the at least one or more modified motion plans based, for example, at least in part, on a comparison of respective delay amounts associated with each of the first motion plan and the at least one or more modified motion plans. For example, the first motion plan may be associated with a first delay amount (e.g., 90 seconds), during which time the given robot is or may be blocked by another robot and thus will be stuck at the first target in a first ending pose before being able to execute the second motion plan. Also, for example, the first modified motion plan may be associated with a second delay amount (e.g., 60 seconds), during which time the given robot is or may be blocked by another robot and thus will be stuck at the first target in a second ending pose before being able to execute the second motion plan. In such an example, the processor-based system can select the first modified motion plan for execution by the first robot because it provides the smallest delay of the available options. As a further example, the second modified motion plan may be associated with a third delay amount (e.g., 15 seconds), during which time the given robot is or may be blocked by another robot and thus will be stuck on the first target at the third end pose before being able to execute the second motion plan. In such an example, the processor-based system may select the second modified motion plan for execution by the first robot because it will result in the least delay of the available options. Although the selection is shown based solely on delay, duration, or latency, the processor-based system may take into account other parameters, such as risk or probability of collision, severity of collision, and / or expenditure of energy.

[0161] Another example of a corrective action includes moving another robot if the other robot is blocking or likely to block a given robot. Such may include, for example, moving the other robot out of the way or moving differently so as not to block the given robot when moving along a trajectory specified by the generated motion plan(s) for the given robot (e.g., first motion plan, second motion plan).

[0162] Yet another example of a corrective action includes generating a new, modified, or replaced motion plan for another robot, e.g., having a trajectory or planned trajectory that would block, or be likely to block, the given robot, to move the other robot in a manner that would not, or would be likely to not, block the given robot's movement along the trajectory specified by the generated motion plan(s) for the given robot (e.g., first motion plan, second motion plan).

[0163] Yet a further example of a corrective action includes determining or generating a new ordering for the set of goals, where the set of goals includes a first goal and at least a second goal, such that what was the first goal can become the second goal and what was the second goal can become the first goal.

[0164] Optionally, at 616, the processor-based system causes one or more of the corrective actions to be taken. Such may include, for example, the processor-based system invoking another iteration of motion planning for the given robot. Such may include, for example, alternatively or additionally, the processor-based system invoking another iteration of motion planning for one or more other robots that are blocking or likely to block movement of the given robot. Such may include, for example, alternatively or additionally, the processor-based system sending or transmitting control or drive signals to one or more motion controllers (e.g., motor controllers) to cause one or more actuators to move one or more linkages of the one or more other robots to move the other robot away from blocking or likely to block the given robot. Such may include, for example, alternatively or additionally, determining or generating a new order of the set of goals, the set of goals including a first goal and at least a second goal.

[0165] At 618, the processor-based system provides a motion plan that implements the selected path (e.g., selected path, selected feasible path, selected preferred path, selected preferred feasible path) to control the movement of the given robot (e.g., the first robot R1) for which the motion plan was generated. For example, the processor-based system may send control or drive signals to one or more motion controllers (e.g., motor controllers) to cause one or more actuators to move one or more linkages to move the given robot or a portion thereof (e.g., an appendage, end effector, end of arm tool) along a trajectory specified by the motion plan.

[0166] Method 600 may terminate at 620, for example, until called again. Alternatively, method 600 may repeat until affirmatively stopped, for example, by a power down state or condition. In some implementations, method 600 may be executed as a multi-threaded process on one or more cores of one or more processors.

[0167] Although the method of operation 600 is described with respect to an ordered flow, various acts or operations are performed simultaneously or in parallel in many implementations. Often, motion planning of one robot to perform a task may be performed while one or more robots are performing the task. Thus, performance of a task by a robot may overlap or be simultaneous or parallel with performance of motion planning by one or more motion planners. Performance of a task by a robot may overlap or be simultaneous or parallel with performance of a task by other robots. In some implementations, at least some portions of motion planning for one robot may overlap or be simultaneous or parallel with at least some portions of motion planning for one or more other robots.

[0168] 6B illustrates a low-level method 630 of operation of a processor-based system for performing motion planning for one or more robots, according to at least one illustrated implementation. Method 630 may be performed during runtime of the robot(s). Alternatively, portions of method 630 may be performed during configuration time prior to runtime of the robot(s) and portions of method 630 may be performed during runtime of the robot(s). Method 630 may be performed, for example, in performing motion planning 520 of method 500 (FIG. 5), although motion planning 520 is not limited to what is described and illustrated in method 630.

[0169] Method 630 can be performed for one, two, or more robots operating in a multiple robot operating environment. For example, method 630 can be performed sequentially for each robot (e.g., a first robot R1, a second robot R2, a third robot R3, or even more robots). For ease of explanation, method 630 is described with respect to control of a given robot (e.g., a first robot R1), with other robots in the operating environment (e.g., a second robot R2, a third robot R3) potentially representing obstacles to the movement of the first robot. Those skilled in the art will recognize from the discussion herein that method 630 can be extrapolated to motion planning for one robot when two or more other robots are present in the operating environment, or motion planning for each of two or more robots when one, two, or more other robots are present in the operating environment. Such may be performed, for example, by performing method 630 for each robot sequentially and / or by iteratively performing method 630 for a given robot in succession for each of the other robots that present potential obstacles to the movement of the given robot. Those skilled in the art will also recognize that method 630 may omit some of the illustrated operations and may include additional operations, and that many of the operations of method 630 may be performed or implemented in a different order than that illustrated and / or concurrently with one another.

[0170] Method 630 can be performed at runtime, which is a period during which at least one of the robots is operating (e.g., moving, performing a task), e.g., following a configuration time. Method 630 can be performed by one or more processor-based systems in the form of one or more robot control systems. The robot control systems may be, for example, co-located or "on-board" a respective one of the robots.

[0171] Method 630 begins at 632, for example, in response to powering on the robot and / or robot control system, in response to a call or invocation from a calling routine such as a routine performing method 500 or a routine performing method 600, in response to receiving a motion planning request to be performed for the robot or a motion planning request in a queue of motion planning requests, or in response to receiving a task to be performed by the robot.

[0172] At 634, the processor-based system uses the motion planning graph to generate one or more feasible paths. A feasible path is a complete path that extends from either the start node or the current node through the goal node via valid transitions. A feasible path specifies possible trajectories for a given robot through various poses corresponding to each node.

[0173] It should be noted that one or more of the generated feasible paths may not be suitable feasible paths, for example because the cost or cost function of the transitions or edges along the path, and even the accumulated cost or accumulated cost function along the path, is too high (e.g., exceeds a threshold cost or value or magnitude). The processor-based system may ultimately reject those feasible paths as not suitable for performing a particular task by a given robot, even if performance of that task via the transitions defined by that path is feasible and therefore feasible. Thus, as described below with reference to 636, the processor-based system may optionally identify one or more suitable feasible paths from a set of one or more feasible paths, for example based on a cost threshold. Furthermore, as described below with reference to 638, the processor-based system may select one of the feasible paths, or may even select one of the suitable feasible paths when a suitable feasible path is identified, for example via a minimum-cost analysis to identify a selected path (alternatively referred to as a selected feasible path).

[0174] Optionally, at 636, the processor-based system identifies one or more preferred paths (also interchangeably referred to as preferred feasible paths or identified preferred feasible paths) from the one or more feasible paths. The processor-based system may, for example, identify a set of feasible paths, each having an associated cumulative cost over the respective feasible path that is less than or equal to a threshold cost. Thus, the set of preferred feasible paths may include feasible paths that satisfy some defined cost condition or constraint. As noted elsewhere, the cost may reflect various parameters, including, for example, risk or probability of collision, severity of collision, energy consumption, duration or latency of execution or completion of a path or assigned task. The processor-based system may implement other conditions or constraints in addition to or instead of the cost condition in identifying the preferred feasible path.

[0175] Optionally, at 638, the processor-based system selects a path, referred to as a selected path. The processor-based system may, for example, select a path from one or more feasible paths (and thus interchangeably referred to as a selected feasible path). The processor-based system may, for example, optionally select a path from one or more identified preferred paths (and thus interchangeably referred to as a selected preferred path or a selected preferred feasible path). The processor-based system may, for example, perform a minimum-cost analysis on the set of feasible paths or the set of preferred paths to find a selected path having either a minimum cost of all of the feasible paths or a relatively low cost of all of the feasible paths. The cost or cost function at least partially represents a collision check, so that selecting a path is at least partially based on a collision detection or assessment (e.g., risk or probability of collision). The cost value or cost function may further represent other criteria and / or parameters in performing the corresponding motion, such as, for example, severity of the collision if a collision occurs, consumption of energy, and / or consumption of time, etc. The processor-based system may employ any of the various structures and algorithms described herein or in the materials incorporated herein by reference to select a path (e.g., selected path, selected feasible path, selected preferred path, selected preferred feasible path), for example, via performing a least-cost analysis on feasible or preferred paths generated from a motion planning graph with associated cost values.

[0176] Optionally, at 640, the processor-based system selects a multi-path combination or multi-motion plan combination, e.g., a combination of a preceding path or preceding motion plan (e.g., a first path or first motion plan) and a following path or following motion plan (e.g., a second path or second motion plan), based on one or more parameters (e.g., an aggregate or total cost over the combinations).

[0177] The processor-based system may, for example, generate a first set of candidate paths or motion plans, each specifying a complete path between a current or start and a first target. Additionally or alternatively, the processor-based system may, for example, generate a second set of candidate paths or motion plans, each specifying a complete path between the first target and a subsequent, trailing, or second target. The processor-based system may, for example, select one path or motion plan from the first set and / or select one path or motion plan from the second set based on one or more criteria.

[0178] The processor-based system can, for example, perform a minimum-cost analysis on various combinations, each combination including a first path or a first motion from a first set of candidates and a second path or a second motion plan from a second set of candidates, to find a combination that has a minimum total aggregate cost from a current or starting pose to an Nth target, or has a relatively low total aggregate cost, where N is an integer equal to or greater than 1. The cost or cost function at least partially represents a collision check, so that selecting the combination is based at least in part on a collision detection or assessment (e.g., risk or probability of collision). The cost value or cost function can further represent other criteria and / or parameters in performing the corresponding motion, such as, for example, severity of the collision if a collision occurs, energy consumption, and / or time consumption, etc. The processor-based system may employ any of the various structures and algorithms described herein or in the materials incorporated herein by reference to select a path (e.g., selected path, selected feasible path, selected preferred path, selected preferred feasible path), for example, via performing a least-cost analysis on feasible or preferred paths generated from a motion planning graph with associated cost values.

[0179] Although not explicitly illustrated, combinations may be identified and selected to minimize the aggregate number of postures or transitions experienced in reaching two or more targets. In some instances, this may advantageously minimize latency and / or energy consumption. Note that the duration of time and / or energy consumption to transition between any given pair of postures may not necessarily be equal to that of some other pair of postures, and thus minimizing the aggregate number of postures or transitions may not necessarily limit or reduce latency and / or energy consumption.

[0180] Method 630 may terminate at 642, for example, until called again. Alternatively, method 630 may repeat until affirmatively stopped, for example, by a power down state or condition. In some implementations, method 630 may be executed as a multi-threaded process on one or more cores of one or more processors.

[0181] Although the method of operation 630 is described with respect to an ordered flow, various acts or operations are performed simultaneously or in parallel in many implementations. Often, motion planning of one robot to perform a task may be performed while one or more robots are performing the task. Thus, performance of a task by a robot may overlap or be simultaneous or parallel with performance of motion planning by one or more motion planners. Performance of a task by a robot may overlap or be simultaneous or parallel with performance of a task by other robots. In some implementations, at least some portions of motion planning for one robot may overlap or be simultaneous or parallel with at least some portions of motion planning for one or more other robots.

[0182] 7 illustrates an optional method 700 of operation in a processor-based system for controlling the movement of one or more robots in a multi-robot environment, according to at least one example implementation. Method 700 may be performed, for example, as part of the implementation of method 500 (FIG. 5) and / or the implementation of method 600 (FIGS. 6A-6B) during runtime of one or more robots. Method 700 may be performed by one or more processor-based systems in the form of one or more robot control systems. The robot control systems may be, for example, co-located with or "on-board" respective ones of the robots.

[0183] Method 700 begins at 702, for example, in response to powering on the robot and / or robot control system, in response to a call or invocation from a calling routine (e.g., method 500), or in response to receiving a set or list or queue of tasks.

[0184] Optionally, at 704, the processor-based system optionally determines whether the corresponding motion is complete. Monitoring the completion of a motion may advantageously enable the processor-based system to remove obstacles corresponding to the motion from consideration during subsequent collision detection or assessment. The processor-based system may rely on the coordinates of the corresponding robot. The coordinates may be based on information from a motion controller, actuators, and / or sensors (e.g., cameras, rotational encoders, reed switches) to determine whether a given motion is complete.

[0185] Optionally, the processor-based system generates or transmits a motion complete message in response to determining that a given motion is complete, at 706. Alternatively, the processor-based system may set and reset a flag.

[0186] At 708, a processor-based system (e.g., obstacle pruner 260 of FIG. 2) prune one or more obstacles corresponding to the given motion in response to determining that a motion complete message was generated or received or a flag was set. For example, the processor-based system may remove the corresponding obstacle from the obstacle queue. This advantageously allows collision detection or assessment to proceed in an environment where swept volumes or other representations (e.g., spheres, bounding boxes) that no longer exist as obstacles have been removed. Also, for example, the processor-based system may remove swept volumes that correspond to edges that represent completed movements of a given robot. This also advantageously allows motions and corresponding swept volumes to be tracked without having to track the timing of the motions or the corresponding swept volumes.

[0187] Method 700 may terminate at 710, for example, until called again. Alternatively, method 700 may repeat until affirmatively stopped, for example, by a power down state or condition. In some implementations, method 700 may be executed as a multi-threaded process on one or more cores of one or more processors. [Example] EXAMPLES

[0188] 1. A method of operating a processor-based system for controlling one or more robots in a multi-robot environment, the method comprising: For a first robot of the one or more robots, implementing, by at least one processor, motion planning for the first robot to determine a first motion plan for the first robot for a first target, the motion planning taking into account at least a second robot of the one or more robots, the first motion plan specifying a plurality of poses for transitioning the first robot from one pose to a first ending pose, the first ending pose positioning at least a portion of the first robot at the first target; implementing, by at least one processor, motion planning for the first robot to attempt to determine a second motion plan for the first robot with respect to a second target, the motion planning taking into account at least the second robot of the one or more robots, the second motion plan specifying a plurality of poses for transitioning the first robot from the first ending pose to a second ending pose, the second ending pose positioning at least a portion of the first robot at the second target; and moving the first robot after performing motion planning for the first robot to attempt to determine the second motion plan for the first robot. EXAMPLES

[0189] 2. The method of example 1, wherein performing motion planning for the first robot includes determining whether the first robot moving along a trajectory will result in a collision with the second robot or has a probability of resulting in a collision with the second robot that exceeds a threshold probability. EXAMPLES

[0190] 3. The method of claim 2, further comprising: moving the second robot from the path of the first robot in response to determining that the trajectory of the first robot will result in a collision with the second robot or has a probability of resulting in a collision with the second robot that exceeds a threshold probability. EXAMPLES

[0191] 3. The method of claim 2, further comprising, in response to determining that a trajectory of the first robot will result in a collision with the second robot or has a probability of resulting in a collision with the second robot that exceeds a threshold probability, moving the second robot from a path of the first robot as specified by at least one of the first motion plan for the first robot or the second motion plan for the first robot. EXAMPLES

[0192] The method of example 2, further comprising: in response to determining that a trajectory of the first robot will result in a collision with the second robot or has a probability of resulting in a collision with the second robot that exceeds a threshold probability, performing, by at least one processor, motion planning for the first robot to determine a modified motion plan for the first robot, the motion planning taking into account at least the second robot of the one or more robots, the modified motion plan specifying a plurality of poses for transitioning the first robot from one pose to the first ending pose while avoiding or at least reducing a probability of a collision with the second robot, the first ending pose positioning at least a portion of the first robot at the first target. EXAMPLES

[0193] 6. The method of example 5, further comprising moving the first robot according to the modified motion plan for the first robot. EXAMPLES

[0194] 2. The method of example 1, wherein performing motion planning for the first robot includes determining whether the first robot moving along a first trajectory will result in a collision with the second robot moving along a second trajectory or has a probability of resulting in a collision with the second robot moving along a second trajectory that exceeds a threshold probability. EXAMPLES

[0195] in response to determining that the first robot moving along a first trajectory will result in a collision with the second robot moving along a second trajectory or has a probability of resulting in a collision with the second robot moving along a second trajectory that exceeds a threshold probability, performing, by at least one processor, motion planning for the second robot to determine a motion plan for the second robot, the motion plan for the second robot specifying at least one pose for transitioning the second robot out of a path of the first robot; 8. The method of example 7, further comprising causing the second robot to move according to the second trajectory for the second robot. EXAMPLES

[0196] 8. The method of claim 7, further comprising: in response to determining that the first robot moving along a first trajectory will result in a collision with the second robot moving along a second trajectory or has a probability of resulting in a collision with the second robot moving along a second trajectory that exceeds a threshold probability, implementing, by at least one processor, motion planning for the first robot to determine a modified motion plan for the first robot, the motion planning taking into account at least the second trajectory of the second robot of the one or more robots, the modified motion plan specifying a plurality of poses for transitioning the first robot from one pose to the first ending pose while avoiding or at least reducing a probability of a collision with the second robot as the second robot moves along the second trajectory of the second robot, the first ending pose positions at least a portion of the first robot at the first target. EXAMPLES

[0197] 10. The method of example 9, further comprising moving the first robot according to the modified motion plan for the first robot. EXAMPLES

[0198] The method according to any one of Examples 1 to 10, wherein moving the first robot includes moving the first robot to the first target after performing motion planning for the first robot to attempt to determine the second motion plan for the first robot. EXAMPLES

[0199] The method according to any one of Examples 1 to 10, wherein moving the first robot includes moving the first robot to the first target and then moving the first robot to the second target. EXAMPLES

[0200] For a third goal, implementing, by at least one processor, motion planning for the first robot to attempt to determine a third motion plan for the first robot, the motion planning taking into account at least the second robot of the one or more robots, the third motion plan specifying a plurality of poses for transitioning the first robot from the second ending pose to a third ending pose, the third ending pose positioning at least a portion of the first robot at the third goal; The method of any one of Examples 1 to 10, wherein the moving of the first robot occurs after the performing of motion planning for the first robot to attempt to determine the third motion plan for the first robot. EXAMPLES

[0201] Performing motion planning for the first robot includes: representing at least the second robot as at least one obstacle; A method according to any one of claims 1 to 10, comprising: performing, by at least one processor, collision detection on at least one motion of at least a portion of the first robot relative to the representation of the at least one obstacle. EXAMPLES

[0202] Performing motion planning for the first robot includes: representing a plurality of motions of at least the second robot as at least one obstacle; A method according to any one of claims 1 to 10, comprising: performing, by at least one processor, collision detection on at least one motion of at least a portion of the first robot relative to the representation of the at least one obstacle. EXAMPLES

[0203] 16. The method of example 15, wherein representing a plurality of motions of at least the second robot as an obstacle includes using a set of swept volumes, each of the swept volumes representing a respective volume swept by at least a portion of the second robot as the at least a portion of the second robot moves along a trajectory represented by the respective motions. EXAMPLES

[0204] 17. The method of example 16, further comprising receiving, by at least one processor, the set of swept volumes previously calculated prior to runtime, each of the swept volumes representing a respective volume swept by at least the portion of the second robot as the portion of the second robot moves along a trajectory represented by the respective motion. EXAMPLES

[0205] 16. The method of claim 15, wherein representing the multiple motions of the second robot as obstacles includes representing the motions of the robot as at least one of an occupancy grid, a hierarchical tree, or a Euclidean distance field. EXAMPLES

[0206] The method according to any one of Examples 1 to 10, further comprising generating, by at least one processor, for each of at least the first robot and the second robot, a respective motion planning graph, each motion planning graph comprising a plurality of nodes and edges, the nodes representing respective states of the respective first and second robots, and the edges representing valid transitions between respective states represented by respective ones of respective pairs of nodes connected by the respective edges. EXAMPLES

[0207] A method according to any one of Examples 1 to 10, wherein both performing motion planning for the first robot to determine a first motion plan for the first robot and performing motion planning for the first robot to attempt to determine a second motion plan for the first robot occur during runtime of at least one of the one or more robots. EXAMPLES

[0208] The method of any one of Examples 1 to 10, further comprising taking corrective action by the at least one processor in response to determining that a trajectory of the first robot will result in a collision with the second robot or has a probability of resulting in a collision with the second robot that exceeds a threshold probability. EXAMPLES

[0209] 22. The method of example 21, further comprising autonomously selecting, by the at least one processor, the corrective action from a set of two or more defined corrective actions. EXAMPLES

[0210] Taking corrective action by the at least one processor includes: i) implementing, by the at least one processor, motion planning for the first robot to determine a revised motion plan for the first robot, the motion planning taking into account at least the second robot of the one or more robots, the revised motion plan specifying a plurality of poses for transitioning the first robot from one pose to the first ending pose while avoiding or at least reducing a probability of collision with the second robot, the first ending pose positioning at least a portion of the first robot at the first target; and ii) implementing, by the at least one processor, a first motion plan for the first robot or a previous motion plan for the first robot to determine a revised motion plan for the first robot, the motion planning taking into account at least the second robot of the one or more robots, the revised motion plan specifying a plurality of poses for transitioning the first robot from one pose to the first ending pose while avoiding or at least reducing a probability of collision with the second robot, the first ending pose positioning at least a portion of the first robot at the first target; 23. The method of claim 21 or 22, further comprising taking at least one of the corrective actions, including: moving the second robot from the path of the first robot as specified by at least one of the second motion plans for the first robot; iii) implementing motion planning for the second robot by the at least one processor to determine a modified motion plan for the second robot, the modified motion plan specifying a plurality of poses for transitioning the second robot from the path of the first robot; and iv) determining a new order of a set of goals, the set of goals including the first goal and at least the second goal. EXAMPLES

[0211] The method of any one of Examples 1 to 10, further comprising autonomously selecting, by the at least one processor, a motion plan from a set of two or more candidate motion plans based at least in part on a total cost of the two or more motion plans across respective ones of the two or more targets. EXAMPLES

[0212] 1. A processor-based system for controlling one or more robots, the system comprising: At least one processor; and at least one non-transitory processor-readable medium communicatively coupled to the at least one processor and storing processor-executable instructions that, when executed by the at least one processor, cause the at least one processor to: A processor-based system configured to carry out the method according to any one of Examples 1 to 24. EXAMPLES

[0213] 1. A processor-based system for controlling one or more robots, the system comprising: a first robot in a robot environment; at least a second robot in the robot environment; At least one processor; and at least one non-transitory processor-readable medium communicatively coupled to the at least one processor and storing processor-executable instructions that, when executed by the at least one processor, cause the at least one processor to: For a first robot of the one or more robots, performing motion planning for the first robot to determine a first motion plan for the first robot with respect to a first target, the motion planning taking into account at least a second robot of the one or more robots, the first motion plan specifying a plurality of poses for transitioning the first robot from one pose to a first ending pose, the first ending pose positioning at least a portion of the first robot at the first target; performing motion planning for the first robot to attempt to determine a second motion plan for the first robot with respect to a second target, the motion planning taking into account at least the second robot of the one or more robots, the second motion plan specifying a number of poses for transitioning the first robot from the first ending pose to a second ending pose, the second ending pose positioning at least a portion of the first robot at the second target; and, after performing motion planning for the first robot, moving the first robot to attempt to determine the second motion plan for the first robot. EXAMPLES

[0214] 27. The processor-based system of example 26, wherein to perform motion planning for the first robot, the at least one processor determines whether the first robot moving along a trajectory will result in a collision with the second robot or has a probability of resulting in a collision with the second robot that exceeds a threshold probability. EXAMPLES

[0215] The processor-executable instructions, when executed, cause the at least one processor to: 28. The processor-based system of Example 27, further comprising: moving the second robot out of the path of the first robot in response to determining that the trajectory of the first robot will result in a collision with the second robot or has a probability of resulting in a collision with the second robot that exceeds a threshold probability. EXAMPLES

[0216] The processor-executable instructions, when executed, cause the at least one processor to: 28. The processor-based system of example 27, further comprising: in response to determining that the trajectory of the first robot will result in a collision with the second robot or has a probability of resulting in a collision with the second robot that exceeds a threshold probability, moving the second robot from a path of the first robot as specified by at least one of the first motion plan for the first robot or the second motion plan for the first robot. EXAMPLES

[0217] The processor-executable instructions, when executed, cause the at least one processor to: 28. The processor-based system of example 27, further comprising: performing motion planning for the first robot to determine a modified motion plan for the first robot in response to determining that a trajectory of the first robot will result in a collision with the second robot or has a probability of resulting in a collision with the second robot that exceeds a threshold probability, the motion planning taking into account at least the second robot of the one or more robots, the modified motion plan specifying a plurality of poses for transitioning the first robot from one pose to the first ending pose while avoiding a collision with the second robot or at least reducing the probability of a collision, the first ending pose positioning at least a portion of the first robot at the first target. EXAMPLES

[0218] The processor-executable instructions, when executed, cause the at least one processor to: 31. The processor-based system of example embodiment 30, further comprising moving the first robot according to the modified motion plan for the first robot. EXAMPLES

[0219] 27. The processor-based system of Example 26, wherein to perform motion planning for the first robot, the at least one processor determines whether the first robot moving along a first trajectory will result in a collision with the second robot moving along a second trajectory or has a probability of resulting in a collision with the second robot moving along a second trajectory that exceeds a threshold probability. EXAMPLES

[0220] The processor-executable instructions, when executed, cause the at least one processor to: responsive to determining that the first robot moving along a first trajectory will result in a collision with the second robot moving along a second trajectory or has a probability of resulting in a collision with the second robot moving along a second trajectory that exceeds a threshold probability, performing motion planning for the second robot to determine a motion plan for the second robot, the motion plan for the second robot specifying at least one pose for transitioning the second robot out of a path of the first robot; 33. The processor-based system of example embodiment 32, further causing the second robot to move according to the second trajectory for the second robot. EXAMPLES

[0221] The processor-executable instructions, when executed, cause the at least one processor to: 33. The processor-based system of example 32, further comprising: performing motion planning for the first robot to determine a modified motion plan for the first robot in response to determining that the first robot moving along a first trajectory will result in a collision with the second robot moving along a second trajectory or has a probability of resulting in a collision with the second robot moving along a second trajectory that exceeds a threshold probability, the motion planning taking into account at least the second trajectory of the second robot of the one or more robots, the modified motion plan specifying a plurality of poses for transitioning the first robot from one pose to the first ending pose while avoiding or at least reducing the probability of a collision with the second robot as the second robot moves along the second trajectory of the second robot, the first ending pose positioning at least a portion of the first robot at the first target. EXAMPLES

[0222] The processor-executable instructions, when executed, cause the at least one processor to: 35. The processor-based system of example embodiment 34, further comprising moving the first robot according to the modified motion plan for the first robot. EXAMPLES

[0223] A processor-based system as described in any one of Examples 26 to 35, wherein to move the first robot, the at least one processor moves the first robot to the first target after performing motion planning for the first robot to attempt to determine the second motion plan for the first robot. EXAMPLES

[0224] A processor-based system described in any one of Examples 26 to 35, wherein, to move the first robot, the at least one processor moves the first robot to the first target and then moves the first robot to the second target. EXAMPLES

[0225] The processor-executable instructions, when executed, cause the at least one processor to: further performing motion planning for the first robot to attempt to determine a third motion plan for the first robot with respect to a third target, the motion planning taking into account at least the second robot of the one or more robots, the third motion plan specifying a plurality of poses for transitioning the first robot from the second ending pose to a third ending pose, the third ending pose positioning at least a portion of the first robot at the third target; The processor-based system of any one of Examples 26 to 35, wherein moving the first robot occurs after performing motion planning for the first robot to attempt to determine the third motion plan for the first robot. EXAMPLES

[0226] To implement motion planning for the first robot, the at least one processor: representing at least the second robot as at least one obstacle; A processor-based system described in any one of Examples 26 to 35, which performs collision detection for at least one motion of at least a portion of the first robot relative to the representation of the at least one obstacle. EXAMPLES

[0227] To implement motion planning for the first robot, the at least one processor: Representing a plurality of motions of at least the second robot as at least one obstacle; A processor-based system described in any one of Examples 26 to 35, which performs collision detection for at least one motion of at least a portion of the first robot relative to the representation of the at least one obstacle. EXAMPLES

[0228] 41. The processor-based system of example 40, wherein to represent multiple motions of at least the second robot as obstacles, the at least one processor uses a set of swept volumes, each of the swept volumes representing a respective volume swept out by at least the portion of the second robot as the portion of the second robot moves along a trajectory represented by the respective motions. EXAMPLES

[0229] The processor-executable instructions, when executed, cause the at least one processor to: 42. The processor-based system of example 41, further receiving a set of swept volumes previously calculated prior to runtime, each of the swept volumes representing a respective volume swept by at least the portion of the second robot as the portion of the second robot moves along a trajectory represented by the respective motion. EXAMPLES

[0230] The processor-based system of example 40, wherein to represent multiple motions of the second robot as obstacles, the at least one processor represents the motions of the robot as at least one of an occupancy grid, a hierarchical tree, or a Euclidean distance field. EXAMPLES

[0231] The processor-executable instructions, when executed, cause the at least one processor to: The processor-based system of any one of Examples 26 to 35, further generating a respective motion planning graph for at least each of the first robot and the second robot, each motion planning graph including a plurality of nodes and edges, the nodes representing respective states of the respective first and second robots, and the edges representing valid transitions between respective states represented by respective ones of respective pairs of nodes connected by the edges. EXAMPLES

[0232] The processor-based system of any one of Examples 26 to 35, wherein both of the performing of motion planning for the first robot to determine a first motion plan for the first robot and the performing of motion planning for the first robot to attempt to determine a second motion plan for the first robot occur during runtime of at least one of the one or more robots. EXAMPLES

[0233] The processor-executable instructions, when executed, cause the at least one processor to: The processor-based system of any one of Examples 26 to 36, further causing corrective action to be taken in response to determining that the trajectory of the first robot will result in a collision with the second robot or has a probability of resulting in a collision with the second robot that exceeds a threshold probability. EXAMPLES

[0234] The processor-executable instructions, when executed, cause the at least one processor to: 47. The processor-based system of example 46, further comprising: an autonomous selection of the corrective action from a set of two or more defined corrective actions. EXAMPLES

[0235] To take corrective action, the at least one processor includes: i) implementing, by the at least one processor, motion planning for the first robot to determine a revised motion plan for the first robot, the motion planning taking into account at least the second robot of the one or more robots, the revised motion plan specifying a plurality of poses for transitioning the first robot from one pose to the first ending pose while avoiding or at least reducing a probability of collision with the second robot, the first ending pose positioning at least a portion of the first robot at the first target; and ii) implementing the first motion plan for the first robot. or the processor-based system of example 46 or 47, which causes at least one of the corrective actions to include: iii) moving the second robot from the path of the first robot as specified by at least one of the second motion plans for the first robot; iii) causing implementation of motion planning for the second robot to determine a modified motion plan for the second robot, the modified motion plan specifying a plurality of poses for transitioning the second robot from the path of the first robot; and iv) determining a new order of a set of goals, the set of goals including the first goal and at least the second goal. EXAMPLES

[0236] The processor-executable instructions, when executed, cause the at least one processor to: The processor-based system of any one of Examples 26 to 36, further autonomously selecting a motion plan from a set of two or more candidate motion plans based at least in part on a total cost of the two or more motion plans across respective ones of the two or more targets. EXAMPLES

[0237] 1. A method of operating a processor-based system for controlling one or more robots in a multi-robot environment, the method comprising: implementing, by at least one processor, motion planning for a first robot of the one or more robots to determine a first motion plan for the first robot, the motion planning taking into account at least a second robot of the one or more robots, the first motion plan specifying a plurality of poses for transitioning the first robot from a pose to a first ending pose, the first ending pose positioning at least a portion of the first robot at a first target; implementing, by at least one processor, motion planning for the first robot to attempt to determine a second motion plan for the first robot, the motion planning taking into account at least the second robot of the one or more robots, the second motion plan specifying a plurality of poses for transitioning the first robot from the first ending pose to a second ending pose, the second ending pose positioning at least a portion of the first robot at a second target; determining whether the first robot is at least one of trapped, delayed, or deadlocked in transitioning from the first motion plan to the second motion plan; and in response to determining that the first robot is at least one of trapped, delayed, or deadlocked in transitioning from the first motion plan to the second motion plan, causing execution of at least one corrective action, by the at least one processor. EXAMPLES

[0238] autonomously selecting, by the at least one processor, the corrective action from the set of corrective actions. The method of example 50, further comprising: EXAMPLES

[0239] Causing the execution of the corrective action by the at least one processor includes: i) implementing, by the at least one processor, motion planning for the first robot to determine a modified motion plan for the first robot, the motion planning taking into account at least the second robot of the one or more robots, the modified motion plan specifying a plurality of poses for transitioning the first robot from one pose to the first ending pose while avoiding or at least reducing a probability of collision with the second robot, the first ending pose positioning at least a portion of the first robot at the first target; and ii) implementing the first motion plan for the first robot or The method of any one of claims 50 to 51, further comprising: causing at least one of the corrective actions to include: moving the second robot from the path of the first robot as specified by at least one of the second motion plans for the first robot; iii) implementing motion planning for the second robot by the at least one processor to determine a modified motion plan for the second robot, the modified motion plan specifying a plurality of poses for transitioning the second robot from the path of the first robot; and iv) determining a new order of a set of goals, the set of goals including the first goal and at least the second goal. EXAMPLES

[0240] The method of example 50 or 51, wherein causing the execution of the corrective action by the at least one processor includes causing further motion planning for the first robot by the at least one processor to determine a modified motion plan for the first robot, the motion planning taking into account at least the second robot of the one or more robots, the modified motion plan specifying a number of postures for transitioning the first robot from one posture to the first ending posture while avoiding or at least reducing the probability of collision with the second robot, and the first ending posture positions at least a portion of the first robot at the first target. EXAMPLES

[0241] 54. The method of claim 53, further comprising selecting between the first motion plan and at least the modified motion plan based at least in part on a comparison of respective delay amounts associated with each of the first motion plan and at least the modified motion plan. EXAMPLES

[0242] A method according to any one of Examples 50 to 54, further comprising, after performing motion planning for the first robot, moving the first robot to attempt to determine the second motion plan for the first robot. EXAMPLES

[0243] 1. A processor-based system for controlling one or more robots, the processor-based system comprising: At least one processor; and at least one non-transitory processor-readable medium communicatively coupled to the at least one processor and storing processor-executable instructions that, when executed by the at least one processor, cause the at least one processor to: A processor-based system configured to carry out the method according to any one of Examples 50 to 55. EXAMPLES

[0244] 1. A processor-based system for controlling one or more robots, comprising: At least one processor; and at least one non-transitory processor-readable medium communicatively coupled to the at least one processor and storing processor-executable instructions that, when executed by the at least one processor, cause the at least one processor to: performing motion planning by a first robot of the one or more robots to determine a first motion plan for the first robot, the motion planning taking into account at least a second robot of the one or more robots, the first motion plan specifying a plurality of poses for transitioning the first robot from a pose to a first ending pose, the first ending pose positioning at least a portion of the first robot at a first target; performing motion planning for the first robot to attempt to determine a second motion plan for the first robot, the motion planning taking into account at least the second robot of the one or more robots, the second motion plan specifying a plurality of poses for transitioning the first robot from the first ending pose to a second ending pose, the second ending pose positioning at least a portion of the first robot at a second target; determining whether the first robot is at least one of trapped, delayed, or deadlocked in transitioning from the first motion plan to the second motion plan; and in response to determining that the first robot is at least one of trapped, delayed, or deadlocked in transitioning from the first motion plan to the second motion plan, causing execution of at least one corrective action. EXAMPLES

[0245] The processor-executable instructions, when executed by the at least one processor, cause the at least one processor to: 58. The processor-based system of example 57, further comprising: an autonomous selection of the corrective action from the set of corrective actions. EXAMPLES

[0246] To cause execution of at least one corrective action, the at least one processor includes: i) further motion planning to determine a modified motion plan for the first robot, the motion planning considering at least the second robot of the one or more robots, the modified motion plan specifying a plurality of poses for transitioning the first robot from one pose to the first ending pose while avoiding or at least reducing a probability of collision with the second robot, the first ending pose positioning at least a portion of the first robot at the first target; and ii) further motion planning to determine a modified motion plan for the first robot or the first robot. The processor-based system of Example 57 or 58, which causes execution of at least one of the corrective actions, including: iii) implementing a motion plan for the second robot to determine a modified motion plan for the second robot, the modified motion plan specifying a plurality of poses for transitioning the second robot from the path of the first robot as specified by at least one of the second motion plans for the second robot; and iv) determining a new order of a set of goals, the set of goals including the first goal and at least the second goal. EXAMPLES

[0247] The processor-based system of Example 57 or 58, wherein to cause execution of a corrective action, the at least one processor causes further motion planning for the first robot to determine a modified motion plan for the first robot, the motion planning taking into account at least the second robot of the one or more robots, the modified motion plan specifying a number of poses for transitioning the first robot from one pose to the first ending pose while avoiding or at least reducing the probability of collision with the second robot, and the first ending pose positions at least a portion of the first robot at the first target. EXAMPLES

[0248] The processor-executable instructions, when executed by the at least one processor, cause the at least one processor to: The processor-based system of Example 60, further selecting between the first motion plan and at least the modified motion plan based at least in part on a comparison of respective delay amounts associated with each of the first motion plan and at least the modified motion plan. EXAMPLES

[0249] The processor-executable instructions, when executed by the at least one processor, cause the at least one processor to: A processor-based system described in any one of Examples 57 to 61, further comprising, after performing motion planning for the first robot, moving the first robot to attempt to determine the second motion plan for the first robot.

[0250] The foregoing detailed description describes various embodiments of devices and / or processes through the use of block diagrams, schematics, and examples. To the extent that such block diagrams, schematics, and examples include one or more functions and / or operations, those skilled in the art will appreciate that each function and / or operation within such block diagrams, flow charts, or examples may be individually and / or collectively implemented by a wide range of hardware, software, firmware, or virtually any combination thereof. In one embodiment, the subject matter may be implemented via Boolean circuits, application specific integrated circuits (ASICs), and / or FPGAs. However, those skilled in the art will recognize that the embodiments disclosed herein may be implemented, in whole or in part, as one or more computer programs running on one or more computers (e.g., as one or more programs running on one or more computer systems), as one or more programs running on one or more controllers (e.g., microcontrollers), as one or more programs running on one or more processors (e.g., microprocessors), as firmware, or as virtually any combination thereof, in a variety of different implementations in standard integrated circuits, and that designing circuitry and / or writing code for the software and / or firmware is well within the skills of those skilled in the art in light of this disclosure.

[0251] Those skilled in the art will recognize that many of the methods or algorithms described herein may employ additional operations, omit certain operations, and / or perform operations in an order other than that specified.

[0252] Additionally, those skilled in the art will appreciate that the mechanisms taught herein can be implemented in hardware, for example, in one or more FPGAs or ASICs.

[0253] The various embodiments described above can be combined to provide further embodiments. No. PCT / US2017 / 036880, entitled "MOTION PLANNING FOR AUTONOMOUS VEHICLES AND RECONFIGURABLE MOTION PLANNING PROCESSORS," filed June 9, 2017; International Patent Application Publication No. WO2016 / 122840, entitled "SPECIALIZED ROBOT MOTION PLANNING HARDWARE AND METHODS OF MAKING AND USING SAME," filed January 5, 2016; U.S. Patent Application No. 62 / 616783, entitled "APPARATUS, METHOD AND ARTICLE TO FACILITATE MOTION PLANNING OF AN AUTONOMOUS VEHICLE IN AN ENVIRONMENT HAVING DYNAMIC OBJECTS," filed January 12, 2018; U.S. Patent Application No. 62 / 616783, entitled "APPARATUS, METHOD AND ARTICLE TO FACILITATE MOTION PLANNING OF AN AUTONOMOUS VEHICLE IN AN ENVIRONMENT HAVING DYNAMIC OBJECTS," filed February 6, 2018; U.S. Patent Application No. 62 / 626,939, entitled “STORING A DISCRETIZED ENVIRONMENT ON ONE OR MORE PROCESSORS AND IMPROVED OPERATION OF SAME,” filed June 3, 2019;No. 62 / 856548, entitled "METHODS AND ARTICLES TO FACILITATE MOTION PLANNING IN ENVIRONMENTS HAVING DYNAMIC OBSTABLES," filed June 24, 2019; U.S. Patent Application No. 62 / 865431, entitled "MOTION PLANNING FOR MULTIPLE ROBOTS IN SHARED WORKSPACE," filed February 5, 2019; International Patent Application No. PCT / US2019 / 016700, entitled "MOTION PLANNING OF A ROBOT STORING A DISCRETIZED ENVIRONMENT ON ONE OR MORE PROCESSORS AND IMPROVED OPERATION OF SAME," published as WO2019 / 156984; and International Patent Application No. PCT / US2019 / 016700, filed May 26, 2020;No. PCT / US2020 / 034551, filed on June 23, 2020, entitled “METHODS AND ARTICLES TO FACILITATE MOTION PLANNING ENVIRONMENTS HAVING DYNAMIC OBSTACLE” and published as WO2020 / 247207; No. PCT / US2020 / 039193, filed on August 21, 2020, entitled “MOTION PLANNING FOR MULTIPLE ROBOTS IN SHARED WORKSPACE” and published as WO2020 / 263861; No. PCT / US2020 / 039193, filed on August 21, 2020, entitled “MOTION PLANNING FOR ROBOTS TO OPTIMIZE VELOCITY WHILE MAINTAINING LIMITS ON ACCELERATION AND All commonly-owned U.S. patent application publications, U.S. patent applications, foreign patents, and foreign patent applications referenced herein and / or listed in the Application Data Sheet, including, but not limited to, International Patent Application No. PCT / US2021041223 / 047429, entitled "CONFIGURATION OF ROBOTS IN MULTI-ROBOT OPERATIONAL ENVIRONMENT," filed on January 20, 2021, published as US2021 / 0220994, and U.S. patent application Ser. No. 63 / 318,933, entitled "MOTION PLANNING AND CONTROL FOR ROBOTS IN SHARED WORKSPACE EMPLOYING STAGING POSES," filed on March 11, 2022, are hereby incorporated by reference in their entireties. These and other changes can be made to the embodiments in light of the above detailed description. In general, in the following claims, the terms used should not be construed to limit the claims to the specific embodiments disclosed in the specification and the claims, but rather to include all possible embodiments along with the full scope of equivalents to which such claims are entitled. Accordingly, the claims are not limited by this disclosure.

Claims

1. 1. A method of operating a processor-based system for controlling one or more robots in a multi-robot environment, the method comprising: For a first robot of the one or more robots: implementing, by at least one processor, motion planning for the first robot to determine a first motion plan for the first robot with respect to a first target, the motion planning considering at least a second robot of the one or more robots, the first motion plan specifying a plurality of poses for transitioning the first robot from one pose to a first ending pose, the first ending pose positioning at least a portion of the first robot at the first target; implementing, by at least one processor, motion planning for the first robot for attempting to determine a second motion plan for the first robot with respect to a second target, the motion planning considering at least the second robot of the one or more robots, the second motion plan specifying a plurality of poses for transitioning the first robot from the first end pose to a second end pose, the second end pose positioning at least a portion of the first robot at the second target; and moving the first robot after performing motion planning for the first robot to attempt to determine the second motion plan for the first robot.

2. 2. The method of claim 1, wherein performing motion planning for the first robot includes determining whether the first robot moving along a trajectory will result in a collision with the second robot or has a probability of resulting in a collision with the second robot that exceeds a threshold probability.

3. moving the second robot from the path of the first robot in response to determining that the trajectory of the first robot will result in a collision with the second robot or has a probability of resulting in a collision with the second robot that exceeds a threshold probability; The method of claim 2 further comprising:

4. In response to determining that the trajectory of the first robot will result in a collision with the second robot or has a probability of resulting in a collision with the second robot that exceeds a threshold probability, moving the second robot from a path of the first robot as specified by at least one of the first motion plan for the first robot or the second motion plan for the first robot. The method of claim 2 further comprising:

5. and performing, by at least one processor, motion planning for the first robot to determine a revised motion plan for the first robot in response to determining that a trajectory of the first robot will result in a collision with the second robot or has a probability of resulting in a collision with the second robot that exceeds a threshold probability. Further comprising:

3. The method of claim 2, wherein the motion planning considers at least the second robot of the one or more robots, and the modified motion plan specifies a plurality of poses for transitioning the first robot from one pose to a modified first ending pose while avoiding or at least reducing the probability of collision with the second robot, and the modified first ending pose positions at least a portion of the first robot at the first target.

6. moving the first robot according to the modified motion plan for the first robot; The method of claim 5 further comprising:

7. 2. The method of claim 1, wherein performing motion planning for the first robot includes determining whether the first robot moving along a first trajectory will result in a collision with the second robot moving along a second trajectory or has a probability of resulting in a collision with the second robot moving along a second trajectory that exceeds a threshold probability.

8. In response to determining that the first robot moving along a first trajectory will result in a collision with the second robot moving along a second trajectory or has a probability of resulting in a collision with the second robot moving along a second trajectory that exceeds a threshold probability, performing, by at least one processor, motion planning for the second robot to determine a motion plan for the second robot, the motion plan for the second robot specifying at least one pose for transitioning the second robot out of a path of the first robot; causing the second robot to move according to the second trajectory for the second robot; The method of claim 7 further comprising:

9. and in response to determining that the first robot moving along a first trajectory will result in a collision with the second robot moving along a second trajectory or has a probability of resulting in a collision with the second robot moving along a second trajectory that exceeds a threshold probability, performing, by at least one processor, motion planning for the first robot to determine a revised motion plan for the first robot. Further comprising:

8. The method of claim 7, wherein the motion planning considers at least the second trajectory of the second robot of the one or more robots, and the modified motion plan specifies a plurality of poses for transitioning the first robot from one pose to a modified first ending pose while avoiding or at least reducing the probability of collision with the second robot as the second robot moves along the second trajectory of the second robot, and the modified first ending pose positions at least a portion of the first robot at the first target.

10. moving the first robot according to the modified motion plan for the first robot; The method of claim 9 further comprising:

11. 11. The method of claim 1, wherein moving the first robot comprises moving the first robot to the first target after performing motion planning for the first robot to attempt to determine the second motion plan for the first robot.

12. 11. The method of claim 1, wherein moving the first robot comprises moving the first robot to the first target and then moving the first robot to the second target.

13. For a third goal, performing, by at least one processor, motion planning for the first robot to attempt to determine a third motion plan for the first robot. Further comprising: The motion planning considers at least the second robot of the one or more robots, the third motion plan specifies a plurality of poses for transitioning the first robot from the second end pose to a third end pose, the third end pose positions at least a portion of the first robot at the third target; the moving of the first robot occurs after the performing motion planning for the first robot to attempt to determine the third motion plan for the first robot.

11. The method according to any one of claims 1 to 10.

14. Performing motion planning for the first robot includes: representing at least the second robot as at least one obstacle; performing, by at least one processor, collision detection on at least one motion of at least a portion of the first robot relative to a representation of the at least one obstacle; 11. The method of claim 1, comprising:

15. Performing motion planning for the first robot includes: representing a plurality of motions of at least the second robot as at least one obstacle; performing, by at least one processor, collision detection on at least one motion of at least a portion of the first robot relative to a representation of the at least one obstacle; 11. The method of claim 1, comprising:

16. 16. The method of claim 15, wherein representing a plurality of motions of at least the second robot as obstacles includes using a set of swept volumes, each of the swept volumes representing a respective volume swept out by the at least a portion of the second robot as the portion of the second robot moves along a trajectory represented by a corresponding respective one of the plurality of motions.

17. receiving, by at least one processor, the set of swept volumes previously calculated prior to runtime; Further comprising:

17. The method of claim 16, wherein each of the swept volumes represents a respective volume swept out by at least a portion of the second robot as the portion moves along a trajectory represented by the respective motion.

18. 16. The method of claim 15, wherein representing the motions of the second robot as obstacles includes representing the motions of the robot as at least one of an occupancy grid, a hierarchical tree, or a Euclidean distance field.

19. generating, by at least one processor, a respective motion planning graph for at least the first robot and the second robot; Further comprising:

11. The method of claim 1, wherein each motion planning graph includes a plurality of nodes and edges, the nodes representing respective states of the first robot and the second robot, and the edges representing valid transitions between respective states represented by respective ones of respective pairs of nodes connected by respective edges.

20. 11. The method of claim 1, wherein the performing motion planning for the first robot to determine a first motion plan for the first robot and the performing motion planning for the first robot to attempt to determine a second motion plan for the first robot both occur during runtime of at least one of the one or more robots.

21. 11. The method of claim 1, further comprising taking corrective action by the at least one processor in response to determining that a trajectory of the first robot will result in a collision with the second robot or has a probability of resulting in a collision with the second robot that exceeds a threshold probability.

22. and autonomously selecting, by the at least one processor, the corrective action from a set of two or more defined corrective actions.

22. The method of claim 21 further comprising:

23. Taking corrective action by the at least one processor includes: i) performing, by the at least one processor, motion planning for the first robot to determine a modified motion plan for the first robot, the motion planning taking into account at least the second robot of the one or more robots, the modified motion plan specifying a plurality of poses for transitioning the first robot from one pose to the modified first ending pose while avoiding or at least reducing a probability of collision with the second robot, the first ending pose positioning at least a portion of the first robot at the first target; and ii) performing, by the at least one processor, motion planning for the first robot to determine a modified motion plan for the first robot, the motion planning taking into account at least the second robot of the one or more robots, the modified motion plan specifying a plurality of poses for transitioning the first robot from one pose to the modified first ending pose while avoiding or at least reducing a probability of collision with the second robot, the first ending pose positioning at least a portion of the first robot at the first target.

22. The method of claim 21, further comprising taking at least one of the corrective actions including: moving the second robot from the path of the first robot as specified by at least one of a run or the second motion plan for the first robot; iii) implementing, by the at least one processor, motion planning for the second robot to determine a modified motion plan for the second robot, the modified motion plan specifying a plurality of poses for transitioning the second robot from the path of the first robot; and iv) determining a new order of a set of goals, the set of goals including the first goal and at least the second goal.

24. autonomously selecting, by the at least one processor, a motion plan from a set of two or more candidate motion plans based at least in part on a total cost of the two or more motion plans across a respective one of the two or more targets. The method of claim 1 , further comprising:

25. 1. A processor-based system for controlling one or more robots, the system comprising: at least one processor; at least one non-transitory processor-readable medium communicatively coupled to the at least one processor and storing processor-executable instructions; the processor-executable instructions, when executed by the at least one processor, cause the at least one processor to: A processor-based system configured to perform the method of any one of claims 1 to 10.

26. 1. A processor-based system for controlling one or more robots, the system comprising: a first robot within a robotic environment; at least a second robot within the robot environment; at least one processor; and at least one non-transitory processor-readable medium communicatively coupled to the at least one processor and storing processor-executable instructions, the processor-executable instructions, when executed by the at least one processor, causing the at least one processor to: For a first robot of the one or more robots: performing motion planning for the first robot to determine a first motion plan for the first robot with respect to a first target, the motion planning considering at least a second robot of the one or more robots, the first motion plan specifying a plurality of poses for transitioning the first robot from one pose to a first ending pose, the first ending pose positioning at least a portion of the first robot at the first target; performing motion planning for the first robot to attempt to determine a second motion plan for the first robot with respect to a second target, the motion planning considering at least the second robot of the one or more robots, the second motion plan specifying a plurality of poses for transitioning the first robot from the first ending pose to a second ending pose, the second ending pose positioning at least a portion of the first robot at the second target; and moving the first robot after performing motion planning for the first robot to attempt to determine the second motion plan for the first robot.

27. 27. The processor-based system of claim 26, wherein to perform motion planning for the first robot, the at least one processor determines whether the first robot moving along a trajectory will result in a collision with the second robot or has a probability of resulting in a collision with the second robot that exceeds a threshold probability.

28. The processor-executable instructions, when executed, cause the at least one processor to:

28. The processor-based system of claim 27, further comprising: in response to determining that a trajectory of the first robot will result in a collision with the second robot or has a probability of resulting in a collision with the second robot that exceeds a threshold probability, moving the second robot out of the path of the first robot.

29. The processor-executable instructions, when executed, cause the at least one processor to:

28. The processor-based system of claim 27, further causing, in response to determining that a trajectory of the first robot will result in a collision with the second robot or has a probability of resulting in a collision with the second robot that exceeds a threshold probability, to move the second robot from a path of the first robot as specified by at least one of the first motion plan for the first robot or the second motion plan for the first robot.

30. The processor-executable instructions, when executed, cause the at least one processor to:

28. The processor-based system of claim 27, further comprising: performing motion planning for the first robot to determine a modified motion plan for the first robot in response to determining that a trajectory of the first robot will result in a collision with the second robot or has a probability of resulting in a collision with the second robot that exceeds a threshold probability, the motion planning taking into account at least the second robot of the one or more robots, the modified motion plan specifying a plurality of poses for transitioning the first robot from one pose to a modified first ending pose while avoiding or at least reducing a probability of a collision with the second robot, the modified first ending pose positioning at least a portion of the first robot at the first target.

31. The processor-executable instructions, when executed, cause the at least one processor to:

31. The processor-based system of claim 30, further comprising: moving the first robot according to the modified motion plan for the first robot.

32. 27. The processor-based system of claim 26, wherein to perform motion planning for the first robot, the at least one processor determines whether the first robot moving along a first trajectory will result in a collision with the second robot moving along a second trajectory or has a probability of resulting in a collision with the second robot moving along a second trajectory that exceeds a threshold probability.

33. The processor-executable instructions, when executed, cause the at least one processor to: responsive to determining that the first robot moving along a first trajectory will result in a collision with the second robot moving along a second trajectory or has a probability of resulting in a collision with the second robot moving along a second trajectory that exceeds a threshold probability, performing motion planning for the second robot to determine a motion plan for the second robot, the motion plan for the second robot specifying at least one pose for transitioning the second robot out of a path of the first robot; 33. The processor-based system of claim 32, further causing the second robot to move according to the second trajectory for the second robot.

34. The processor-executable instructions, when executed, cause the at least one processor to:

33. The processor-based system of claim 32, further performing motion planning for the first robot to determine a modified motion plan for the first robot in response to determining that the first robot moving along a first trajectory will result in a collision with the second robot moving along a second trajectory or has a probability of resulting in a collision with the second robot moving along a second trajectory that exceeds a threshold probability, the motion planning taking into account at least the second trajectory of the second robot of the one or more robots, the modified motion plan specifying a plurality of poses for transitioning the first robot from one pose to the modified first ending pose while avoiding or at least reducing a probability of a collision with the second robot as the second robot moves along the second trajectory of the second robot, the modified first ending pose positioning at least a portion of the first robot at the first target.

35. The processor-executable instructions, when executed, cause the at least one processor to:

35. The processor-based system of claim 34, further comprising: moving the first robot according to the modified motion plan for the first robot.

36. 36. The processor-based system of claim 26, wherein the at least one processor moves the first robot to the first target after performing motion planning for the first robot to attempt to determine the second motion plan for the first robot.

37. 35. The processor-based system of any one of claims 26 to 34, wherein to move the first robot, the at least one processor moves the first robot to the first target and then moves the first robot to the second target.

38. The processor-executable instructions, when executed, cause the at least one processor to: further performing motion planning for the first robot to attempt to determine a third motion plan for the first robot with respect to a third target, the motion planning considering at least the second robot of the one or more robots, the third motion plan specifying a plurality of poses for transitioning the first robot from the second ending pose to a third ending pose, the third ending pose positioning at least a portion of the first robot at the third target; 36. The processor-based system of claim 26, wherein moving the first robot occurs after performing motion planning for the first robot to attempt to determine the third motion plan for the first robot.

39. To perform motion planning for the first robot, the at least one processor: representing at least the second robot as at least one obstacle; 36. The processor-based system of any one of claims 26 to 35, wherein the system performs collision detection for at least one motion of at least a portion of the first robot relative to a representation of the at least one obstacle.

40. To perform motion planning for the first robot, the at least one processor: representing a plurality of motions of at least the second robot as at least one obstacle; 36. The processor-based system of any one of claims 26 to 35, wherein the system performs collision detection for at least one motion of at least a portion of the first robot relative to a representation of the at least one obstacle.

41. 41. The processor-based system of claim 40, wherein to represent a plurality of motions of at least the second robot as obstacles, the at least one processor uses a set of swept volumes, each of the swept volumes representing a respective volume swept out by at least the portion of the second robot as the portion moves along a trajectory represented by a corresponding respective one of the plurality of motions.

42. The processor-executable instructions, when executed, cause the at least one processor to:

42. The processor-based system of claim 41, further receiving a set of swept volumes previously calculated prior to runtime, each of the swept volumes representing a respective volume swept by at least a portion of the second robot as the portion moves along a trajectory represented by the respective motion.

43. 41. The processor-based system of claim 40, wherein to represent multiple motions of the second robot as obstacles, the at least one processor represents the motions of the robot as at least one of an occupancy grid, a hierarchical tree, or a Euclidean distance field.

44. The processor-executable instructions, when executed, cause the at least one processor to:

36. The processor-based system of claim 26, further generating a respective motion planning graph for at least each of the first robot and the second robot, each motion planning graph including a plurality of nodes and edges, the nodes representing respective states of the first robot and the second robot, and the edges representing valid transitions between respective states represented by respective ones of respective pairs of nodes connected by the edges.

45. 36. The processor-based system of claim 26, wherein the performing of motion planning for the first robot to determine a first motion plan for the first robot and the performing of motion planning for the first robot to attempt to determine a second motion plan for the first robot both occur during runtime of at least one of the one or more robots.

46. The processor-executable instructions, when executed, cause the at least one processor to:

36. The processor-based system of any one of claims 26 to 35, further causing corrective action to be taken in response to determining that the trajectory of the first robot will result in a collision with the second robot or has a probability of resulting in a collision with the second robot that exceeds a threshold probability.

47. The processor-executable instructions, when executed, cause the at least one processor to:

47. The processor-based system of claim 46, further adapted to autonomously select the corrective action from a set of two or more defined corrective actions.

48. To take corrective action, the at least one processor includes: i) implementing, by at least one processor, motion planning for the first robot to determine a modified motion plan for the first robot, the motion planning considering at least the second robot of the one or more robots, the modified motion plan specifying a plurality of poses for transitioning the first robot from one pose to a modified first ending pose while avoiding or at least reducing a probability of collision with the second robot, the modified first ending pose positioning at least a portion of the first robot at the first target; and ii) implementing the first motion planning for the first robot.

47. The processor-based system of claim 46, wherein the processor-based system causes at least one of the corrective actions to include: iii) moving the second robot from the path of the first robot as specified by at least one of the motion plan for the first robot or the second motion plan for the first robot; iii) causing execution of motion planning for the second robot to determine a modified motion plan for the second robot, the modified motion plan specifying a plurality of poses for transitioning the second robot from the path of the first robot; and iv) determining a new order of a set of goals, the set of goals including the first goal and at least the second goal.

49. The processor-executable instructions, when executed, cause the at least one processor to:

36. The processor-based system of claim 26, further comprising: an autonomous selection of a motion plan from a set of two or more candidate motion plans based at least in part on a total cost of the two or more motion plans across a respective one of the two or more targets.

50. 1. A method of operating a processor-based system for controlling one or more robots in a multi-robot environment, the method comprising: implementing, by at least one processor, motion planning for a first robot of the one or more robots to determine a first motion plan for the first robot, the motion planning taking into account at least a second robot of the one or more robots, the first motion plan specifying a plurality of poses for transitioning the first robot from one pose to a first end pose, the first end pose positioning at least a portion of the first robot at a first target; implementing, by at least one processor, motion planning for the first robot to attempt to determine a second motion plan for the first robot, the motion planning considering at least the second robot of the one or more robots, the second motion plan specifying a plurality of poses for transitioning the first robot from the first end pose to a second end pose, the second end pose positioning at least a portion of the first robot at a second target; determining whether the first robot is at least one of trapped, delayed, or deadlocked in transitioning from the first motion plan to the second motion plan; and causing, by the at least one processor, execution of at least one corrective action in response to determining that the first robot is at least one of trapped, delayed, or deadlocked in transitioning from the first motion plan to the second motion plan.

51. autonomously selecting, by the at least one processor, the corrective action from the set of corrective actions.

51. The method of claim 50, further comprising:

52. Causing the execution of the corrective action by the at least one processor includes: i) performing, by the at least one processor, motion planning for the first robot to determine a modified motion plan for the first robot, the motion planning taking into account at least the second robot of the one or more robots, the modified motion plan specifying a plurality of poses for transitioning the first robot from one pose to a modified first ending pose while avoiding or at least reducing a probability of collision with the second robot, the modified first ending pose positioning at least a portion of the first robot at the first target; and ii) performing, by the at least one processor, the first motion plan for the first robot.

52. The method of claim 50 or 51, comprising causing at least one of the corrective actions to include: iii) moving the second robot from the path of the first robot as specified by at least one of a plan or the second motion plan for the first robot; and iii) implementing, by the at least one processor, motion planning for the second robot to determine a modified motion plan for the second robot, the modified motion plan specifying a plurality of poses for transitioning the second robot from the path of the first robot; and iv) determining a new order of a set of goals, the set of goals including the first goal and at least the second goal.

53. 52. The method of claim 50 or 51, wherein causing the execution of the corrective action by the at least one processor includes causing further motion planning for the first robot to determine a modified motion plan for the first robot, the motion planning taking into account at least the second robot of the one or more robots, the modified motion plan specifying a plurality of poses for transitioning the first robot from one pose to the modified first ending pose while avoiding or at least reducing a probability of collision with the second robot, and the modified first ending pose positions at least a portion of the first robot at the first target.

54. selecting between the first motion plan and at least the modified motion plan based at least in part on a comparison of respective delay amounts associated with each of the first motion plan and at least the modified motion plan; 54. The method of claim 53, further comprising:

55. moving the first robot after performing motion planning for the first robot to attempt to determine the second motion plan for the first robot.

51. The method of claim 50, further comprising:

56. 1. A processor-based system for controlling one or more robots, the processor-based system comprising: at least one processor; at least one non-transitory processor-readable medium communicatively coupled to the at least one processor and storing processor-executable instructions; Equipped with The processor-executable instructions, when executed by the at least one processor, cause the at least one processor to:

51. A processor-based system configured to perform the method of claim 50.

57. 1. A processor-based system for controlling one or more robots, comprising: at least one processor; and at least one non-transitory processor-readable medium communicatively coupled to the at least one processor and storing processor-executable instructions, the processor-executable instructions, when executed by the at least one processor, causing the at least one processor to: performing motion planning by a first robot of the one or more robots to determine a first motion plan for the first robot, the motion planning taking into account at least a second robot of the one or more robots, the first motion plan specifying a plurality of poses for transitioning the first robot from one pose to a first end pose, the first end pose positioning at least a portion of the first robot at a first target; performing motion planning for the first robot to attempt to determine a second motion plan for the first robot, the motion planning considering at least the second robot of the one or more robots, the second motion plan specifying a plurality of poses for transitioning the first robot from the first ending pose to a second ending pose, the second ending pose positioning at least a portion of the first robot at a second target; determining whether the first robot is at least one of trapped, delayed, or deadlocked in transitioning from the first motion plan to the second motion plan; and causing execution of at least one corrective action in response to determining that the first robot is at least one of trapped, delayed, or deadlocked in transitioning from the first motion plan to the second motion plan.

58. The processor-executable instructions, when executed by the at least one processor, cause the at least one processor to:

58. The processor-based system of claim 57, further comprising autonomously selecting the corrective action from the set of corrective actions.

59. To cause execution of at least one corrective action, the at least one processor performs: i) further motion planning to determine a modified motion plan for the first robot, the motion planning considering at least the second robot of the one or more robots, the modified motion plan specifying a plurality of poses for transitioning the first robot from one pose to a modified first ending pose while avoiding or at least reducing a probability of collision with the second robot, the modified first ending pose positioning at least a portion of the first robot at the first target; and ii) further motion planning to determine a modified motion plan for the first robot, the motion planning considering at least the second robot of the one or more robots, the modified motion plan specifying a plurality of poses for transitioning the first robot from one pose to a modified first ending pose while avoiding or at least reducing a probability of collision with the second robot, the modified first ending pose positioning at least a portion of the first robot at the first target.

59. The processor-based system of claim 57 or 58, which causes execution of at least one of the corrective actions including: iii) moving the second robot from the path of the first robot as specified by at least one of the second motion plans for the first robot; and iv) performing motion planning for the second robot to determine a modified motion plan for the second robot, the modified motion plan specifying a plurality of poses for transitioning the second robot from the path of the first robot; and iv) determining a new order of a set of goals, the set of goals including the first goal and at least the second goal.

60. 59. The processor-based system of claim 57 or 58, wherein to cause execution of the corrective action, the at least one processor causes further motion planning for the first robot to determine a modified motion plan for the first robot, the motion planning taking into account at least the second robot of the one or more robots, the modified motion plan specifying a plurality of poses for transitioning the first robot from one pose to the modified first ending pose while avoiding or at least reducing the probability of collision with the second robot, and the modified first ending pose positions at least a portion of the first robot at the first target.

61. The processor-executable instructions, when executed by the at least one processor, cause the at least one processor to:

61. The processor-based system of claim 60, further selecting between the first motion plan and at least the modified motion plan based at least in part on a comparison of respective delay amounts associated with each of the first motion plan and at least the modified motion plan.

62. The processor-executable instructions, when executed, cause the at least one processor to:

59. The processor-based system of claim 57 or 58, further comprising, after performing motion planning for the first robot, moving the first robot to attempt to determine the second motion plan for the first robot.

Images