TW202415506A - Robust motion planning and/or control for multi-robot environments - Google Patents

Abstract

Motion planning produces motion plans for robots operating in a shared workspace, the motion plans including nominal trajectories ( e.g., trajectories as specified). Acceptable lag times are determined for the nominal trajectories, where compliance with the acceptable lag times during actual operation ensures self-collision free operation. Actual operation ( e.g., actual trajectories) of the robots is monitored for compliance with respect to the respective acceptable lag times. Optionally, remedial action is selected and/or taken when warranted ( e.g., actual lag time approaches or exceeds a margin or threshold for instance the respective acceptable lag time). Motion planning can employ determined acceptable lag times to select trajectories that provide for more robust operation of the robots.

Description

Robust motion planning and/or control for multi-robot environments

The present invention generally relates to motion planning and operation of robots operating in a shared workspace, and to the computationally efficient generation of robust motion plans executable by the robot in an efficient manner (e.g., reducing or even eliminating collisions and unplanned stops), as well as the monitoring of the actual execution of the motion plans and, where appropriate, taking remedial action when warranted.

Various applications employ two or more robots operating in a common workspace. For example, two or more robots may be used to perform tasks on one or more objects or workpieces in the common workspace, such as driving bolts into a chassis, where parts of the robots may overlap in their range of motion.

Planning usually involves mission planning and motion planning. For example, mission planning may determine that in order to perform a given mission, a robot needs to move between two poses (e.g., from a starting pose to an ending pose). In contrast, motion planning focuses on how to move a robot between two poses.

Motion planning is a fundamental problem in robot control and robotics. A motion plan specifies a trajectory that a robot can follow from a starting pose, configuration, or state to a target pose, configuration, or state, typically in order to complete a task without colliding with any obstacles in the shared workspace, or with a reduced probability of collision with any obstacles in the shared workspace. The challenge for motion planning involves the ability to perform motion planning at rapid speed (i.e., in real time) while potentially taking into account changes in the environment (e.g., changes in the location or orientation of obstacles in the shared workspace). Challenges further include performing motion planning using relatively low-cost equipment, with relatively low energy consumption, and with limited computing power and/or storage (e.g., memory circuits, such as "processor-on-a-chip" circuit systems).

The operation of two or more robots in a shared workspace (also called a work cell or a multi-robot operating environment) presents a special class of problems. For example, motion planning should account for and avoid situations in which robots or their robotic appendages may interfere with each other during the performance of a task.

One approach to operating multiple robots in a shared workspace may be referred to as a task-level approach. An engineer may manually ensure that the robots are collision-free by defining portions of the shared workspace where the robots may collide with each other (i.e., interference zones) and programming individual robots so that only one robot is in an interference zone of the shared workspace at any given point in time. For example, when a first robot begins to move into an interference zone of the shared workspace, the first robot sets a flag. A controller (e.g., a programmable logic controller (PLC)) reads the flag and prevents other robots from moving into the interference zone of the shared workspace until the first robot unasserts the flag when it exits the interference zone. This approach is intuitive and easy to understand, but is often difficult and time-consuming to implement and may not produce once-optimized results. This approach necessarily has a lower work throughput because the use of deconfliction often results in at least one of the robots being idle for a significant period of time, even though the idle robot could technically be performing useful work in the shared workspace.

In the practice approach, a team of engineers typically breaks down a problem and optimizes the resulting smaller sub-problems independently of each other (e.g., assigning tasks to robots, sequencing the tasks assigned to each robot, and performing motion planning for each robot). This may involve iteratively simulating motion to ensure that the robots/robot appendages do not collide with each other, which may take many hours of computing time and may not produce an optimized solution. In addition, if a modification to the common workspace causes a change in the actual trajectory of one of the robots/robot appendages, the entire workflow must be revalidated. Such approaches are certainly not optimal and typically require experts to go through a slow iterative process to try to find a combination of solutions that, when combined together, produces a good result. In any case, such approaches generally fail to ensure collision-free operation when the motion planning is performed by a robot operating under real-world conditions.

Various methods and apparatus are described herein for generating motion plans for robots operating in a shared workspace, the motion plans comprising nominal trajectories (e.g., as specified) having associated acceptable hysteresis times, wherein compliance with the acceptable hysteresis times during actual operation ensures self-collision-free operation (i.e., collisions of a robot with itself and collisions of a robot with other robots of a robot system). For example, the nominal trajectories may represent respective collision-free paths. Various methods and apparatus are described herein for monitoring the actual operation (e.g., actual trajectory) of the robots relative to the acceptable lag times and taking one or more remedial actions as appropriate when warranted (e.g., the actual lag time approaches or exceeds a critical value, such as the respective acceptable lag time).

A particularly advantageous method is described in international patent application PCT/US2021/013610, published as WO2021150439A1, which describes a system for generating optimized multi-robot motion plans. More specifically, the motion plans are optimized because they minimize or attempt to minimize certain cost functions related to system performance (e.g., the probability or likelihood of collision). Each motion plan is also time-based because the motion plan specifies a nominal trajectory for the robotic system. The nominal trajectories are a specified ordered sequence of poses, configurations or states of each movable part of one or more robots of a robotic system, and are parameterized by time (e.g., at each time unit within a complete trajectory). For example, the nominal trajectories may represent respective collision-free paths. The motion plan may be "multi-robot" in that the robotic system may include multiple individual robots.

The motion plan can be represented by a set of discrete robot trajectories: Trj(t) = {trj_r(t), with r ∈ {1…N}, t∈{0, ΔT, 2ΔT, …, T} }, where N represents the total number of robots of the robot system, ΔT represents a sample time step, and T represents a total duration of the motion plan. For sufficiently small values of ΔT, Trj(t) is guaranteed to be collision-free with respect to all the robots of the robot system. In other words, the motion specified by a set of trajectories Trj(t) of a robot system with two or more robots will not result in self-collisions (i.e., collisions of a robot with itself and collisions of a robot with other robots of the robot system). The set of trajectories Trj(t) of a robotic system may not be collision-free with respect to other objects or obstacles in the environment, although a collision assessment with respect to other objects or obstacles in the shared workspace may be performed. This no-self-collision condition can be easily checked or verified by simulation; and once checked or verified, it can be considered safe to perform the movements specified by the movement plan.

However, in practice, when a physical robotic system including two or more robots is actually executing a motion plan, many factors may adversely affect a synchronization between the robots. For example, these factors may include: signal communication, control delays, sensor noise, numerical approximation errors, etc. This may cause the actual physical robotic system to deviate from the trajectory Trj(t) specified by the motion plan. In addition to this "reality gap" between the simulated environment of one of the motion plans and the actual operation of the real physical robotic system used to generate and review the motion plan(s), many other factors may also cause the robotic system to deviate from a motion plan that provides a set of nominal trajectories. For example, in some instances, the performance of a mission may require more time to complete than expected, and therefore, a robot may need to pause or remain at a particular location (e.g., a target location, and therefore in a particular pose, configuration, or state) for a longer time than specified by the trajectory in the motion plan. Deviations from a specified motion plan that has been reviewed and deemed safe may be dangerous and may compromise the safety of the robotic system.

The set of trajectories Trj(t) specified by the motion plan is named herein as a set of nominal trajectories to indicate that such trajectories are trajectories as specified. A single trajectory specified by a motion plan is called herein as a nominal trajectory. The set of trajectories performed by the actual physical motion of the robots is named herein as a set of actual trajectories to distinguish such trajectories from as specified or nominal trajectories. A single trajectory performed by the actual physical motion of the robots is named herein as an actual trajectory.

The methods described herein advantageously extend a safe operating range beyond the safe operating range associated with a set of nominal trajectories or nominal trajectories of the motion plans, such as by computing a neighborhood of the nominal trajectories or nominal trajectories of the motion plans within which the robotic system can operate without causing a self-collision as described above. For each robot to be operated in a common workspace, a maximum acceptable lag time (e.g., time delay or "lag") is calculated for the nominal trajectories or nominal trajectories that the robot will potentially execute. During online execution (i.e., run time), an actual lag time for each robot is monitored. If the actual lag time is within the defined margin or threshold, continued execution of the motion plan is considered safe because self-collision-free movement among the robots is ensured. If the actual lag time of any of the robots exceeds the respective margin or threshold, safety is considered compromised because self-collision-free operation is no longer ensured. Optionally, in such an event, a processor-based system may select and/or take one or more remedial actions.

CROSS-REFERENCE TO RELATED APPLICATIONS This patent application claims priority to U.S. Patent Application No. 63/358,422 filed on July 5, 2022, the entire disclosure of which is hereby incorporated by reference into this document for all purposes.

In the following description, certain specific details are set forth in order to provide a thorough understanding of one of the various disclosed embodiments. However, one 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 instances, well-known structures associated with computer systems, actuator systems, robots, and/or communication networks have not been shown or described in detail to avoid unnecessarily obscuring the description of the embodiments. In other instances, well-known computer vision methods and techniques for generating sensory data and volume representations of one or more objects and the like have not been described in detail to avoid unnecessarily obscuring the description of the embodiments.

Unless the context requires otherwise, throughout the following specification and claims, the word "comprise" and variations thereof (e.g., "comprises" and "comprising") should be construed in an open, inclusive sense, i.e., as "including but not limited to."

Throughout this specification, references to "one embodiment" or "an embodiment" or to "an embodiment" or "an embodiment" mean that a particular feature, structure, or characteristic described in conjunction with that embodiment or embodiment is included in at least one embodiment or at least one embodiment. Therefore, the phrases "one embodiment" or "an embodiment" or "in an embodiment" or "in an embodiment" appearing in various places throughout this specification do not necessarily all refer to the same embodiment or embodiment. Furthermore, particular features, structures, or characteristics may be combined in any suitable manner in one or more embodiments or embodiments.

As used in this specification and the appended claims, the singular forms "a", "an", and "the" include plural referents unless the context clearly dictates otherwise. It should also be noted that the term "or" is generally used in its sense including "and/or" unless the context clearly dictates otherwise.

As used in this specification and the appended claims, the terms "optimizing," "optimize," and "optimized" mean that an improved result is being prepared, generated, or produced, or has been prepared, generated, or produced. Such terms are used in their relative sense and do not necessarily mean that an absolute optimum has been prepared, generated, or produced.

As used in this specification and the appended claims, the term "workspace" or "shared workspace" is used to refer to an operating environment in which two or more robots operate, one or more portions of the shared workspace being the volume in which the robots may potentially collide with each other, and thus may be termed interference zones. The operating environment may include obstacles (i.e., items that the robots are to avoid colliding with) and/or workpieces (i.e., items that the robots are to interact with or act on or with).

As used in this specification and the appended claims, the term "mission" is used to refer to a robot task in which a robot transitions from a position A to a position B, preferably without colliding with obstacles in its environment. The task may involve grasping or releasing an object, moving or unloading an object, rotating an object, or picking up or placing an object. The transition from position A to position B may optionally include transitioning between one or more intermediate positions.

As used in this specification and the appended claims, the term "trajectory" or "trajectories" is used to refer to an ordered sequence of positions or configurations or states of one or more robots, parameterized by time, through which the robot or at least a portion thereof can move, for example to perform a task. Trajectories are preferably represented in a configuration space (also called C-space) of the individual robots, but may alternatively be represented in a real space or real-world space of a common workspace. Trajectories may include pauses at one or more positions, changes of direction, or even reversals, and are not necessarily smooth in direction, time, velocity, or acceleration.

As used in this specification and the appended claims, the term "nominal trajectory" or "nominal trajectories" is used to refer to one or more trajectories "as specified." As used in this specification and the appended claims, the term "actual trajectory" or "actual trajectories" is used to refer to one or more trajectories actually achieved by the physical movement of a physical robot, as distinguished from "nominal trajectory" or "nominal trajectories". In some instances, "actual trajectory" or "actual trajectories" may match "nominal trajectory" or "nominal trajectories", although in many instances there may be differences between the two, such as due to the "actual trajectory" or "actual trajectories" lagging behind the "nominal trajectory" or "nominal trajectories".

As used in this specification and the attached claims, the terms "self-collision" and "self-collisions" when used in the context of a robot system including two or more robots or referring to the robot system include both: i) a collision between a robot and itself; and ii) a collision between a robot and other robots in the robot system. As used in this specification and the attached claims, the terms "self-collision" and "self-collisions" when used in the context of a single robot or referring to the single robot include collisions between a robot and itself.

The titles and abstracts of the invention provided herein are for convenience only and do not interpret the scope or meaning of the embodiments.

The methods described herein advantageously extend a safety of robotic operation beyond that ensured by the nominal trajectory of a motion plan by determining an acceptable lag time within which a physical robot of a robotic system may operate while still ensuring that such operation of the robotic system is free of self-collision. In at least some embodiments, for each robot of a robotic system to be operated in a common workspace, a maximum acceptable lag time (e.g., time delay or "hysteresis") is calculated for each of one or more nominal trajectories. Advantageously, this can be calculated during a configuration time before the robot executes the motion plan. During online execution (i.e., run time, following configuration time), the actual motion of each robot (including an actual lag time of each robot along its actual trajectory) is monitored. If the actual lag time value is within a defined margin or threshold value (e.g., acceptable lag time), continued execution of the motion plan is considered safe because self-collision-free moves are ensured. If an actual lag time of a robot exceeds a corresponding margin or threshold value, safety is compromised because self-collision-free operation is no longer ensured. For example, the safety of all robots may be considered to be adversely affected even if only one robot is experiencing an actual lag time that exceeds a respective margin or threshold. Optionally, in such an event, a processor-based system may select and/or take one or more remedial actions.

FIG1 shows a robotic system 100 including a plurality of robots 102a, 102b, 102c (collectively 102) operating in a common workspace 104 (also referred to as a multi-robot environment) to perform tasks according to an illustrated embodiment. In the robotic system 100 of FIG1 , acceptable hysteresis is determined as part of an optimization and provided to one or more robot control systems along with optimized motion plans that include nominal trajectories for the robots.

A number of robots may be configured to perform a set of tasks. Tasks may be specified as a mission plan. A mission plan may specify a number T of tasks to be performed by a number N of robots. A mission plan may be modeled as a vector per robot, where the vector is an ordered list of tasks to be performed by the respective robot (e.g., {task 7, task 2, task 9}). Optionally, a mission vector may also include a dwell duration, which specifies a duration that the robot or a portion thereof should remain at a given configuration or target. Mission vectors may also specify a home pose and/or other "functional poses" that are not directly related to solving the task (e.g., a "get out of the way" or storage pose). Poses may be specified in the robot's C-space.

The robot 102 may take any of a variety of forms. Typically, the robot 102 will take the form of or have one or more robot appendages 103 (only one of which is labeled) and a base 105 (only one of which is labeled) from which the robot appendage 103 extends. The robot 102 may include one or more linkage mechanisms 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 and operable to move the linkage mechanism in response to a control or drive signal. For example, a pneumatic actuator may include one or more pistons, drums, valves, gas reservoirs, and/or pressure sources (e.g., compressors, blowers). For example, a hydraulic actuator may include one or more pistons, drums, valves, fluid reservoirs (e.g., low-compressible hydraulic fluids) and/or pressure sources (e.g., compressors, pumps, blowers). The robotic system 100 may take the form of other robots 102, such as autonomous vehicles.

In general, the common workspace 104 represents a three-dimensional space in which the robots 102 can operate and move, although in certain limited implementations, the common workspace 104 can represent a two-dimensional space. The common workspace 104 is a volume or area in which at least a portion of the robots 102 can overlap in space and time or otherwise collide if movement is not controlled to avoid collision. It should be noted that the common workspace 104 is a physical space or volume, wherein a position or orientation of the physical space or volume can be conveniently represented, for example, by Cartesian coordinates relative to a reference coordinate system, for example, a reference coordinate system represented by orthogonal axes X, Y, and Z as illustrated in FIG. 1. It should also be noted that the reference coordinate system of the common workspace 104 is different from a respective “configuration space” or “C-space” of any of the robots 102, which is typically represented by a set of joint positions, orientations, or configurations in a respective reference coordinate system of any of the robots 102.

As explained herein, a robot 102a or a portion thereof may constitute an obstacle when considered from the perspective of another robot 102b (i.e., when performing motion planning for the other robot 102b). Additionally, the shared workspace 104 may include other obstacles, such as machinery (e.g., conveyor 106), columns, posts, walls, ceilings, floors, tables, humans, and/or animals. Additionally, the shared workspace 104 may include one or more work items or workpieces, such as one or more packages, packaging materials, fasteners, tools, articles, or other objects, that the robot 102 manipulates as part of performing a task.

Optionally, the robotic system 100 includes one or more processor-based multi-robot configuration optimization systems 108 (one is shown in FIG. 1 ). The multi-robot configuration optimization system 108 is selected to receive a set of inputs 109 and generate as output 111 one or more solutions that specify a configuration of the robots 102, the configuration including a work cell layout (e.g., a respective base position and orientation of each robot 102), mission plans (e.g., a respective mission plan for each of the robots 102), and optionally one or more motion plans that specify or include one or more nominal trajectories for the robots 102a-102c (e.g., a respective nominal trajectory for each of the robots 102) along with an acceptable lag time for each of the nominal trajectories. One or more components of the output 111 may be optimized to at least some extent.

The optional multi-robot configuration optimization system 108 may include a population generator 110 , a multi-robot environment simulator 112 , a multi-robot optimization engine 114 , and an acceptable lag time evaluator 115 .

The swarm generator 110 generates a set of candidate solutions 116 based on the provided input 109. The candidate solutions 116 represent possible solutions to a configuration problem (i.e., how to configure the robots 102 in the shared workspace 104 to achieve a set of tasks). In practice, any given candidate solution 116 may be feasible or infeasible. That is, an initial candidate may be invalid (e.g., the robot is in an impossible place, has an infeasible mission plan, has an unreachable goal, or will cause a collision). In some embodiments, the swarm generator may attempt to find a better candidate solution.

The multi-robot environment simulator 112 models the workspace or multi-robot environment based on each candidate solution to determine certain properties, such as an amount of time required to complete the task, a probability or rate of collisions when completing the task, and the feasibility or infeasibility of a particular configuration as specified by the candidate solution. The multi-robot environment simulator 112 can reflect this in terms of cost, for example, which is generated via one or more cost functions.

For example, a cost or cost function may represent a probability or likelihood of a collision. Depending on the circumstances, the cost or cost function may represent one or more of the following: an acceptable hysteresis or "robustness", a severity of a collision, an expenditure or consumption of energy and/or time or delay to perform or complete a movement corresponding to a nominal trajectory. In some embodiments, a cost or cost function represents a determined acceptable hysteresis for a given nominal trajectory of a given robot. The determined acceptable hysteresis represents a maximum or approximately maximum or optimized hysteresis that may be incurred when executing respective nominal trajectories while still maintaining, ensuring or even guaranteeing at least self-collision-free movement relative to itself and relative to other robots operating in a shared workspace or work cell. Thus, hysteresis may represent an amount of delay that may be introduced into the actual execution of a nominal trajectory, or otherwise incurred when actually executing a nominal trajectory, without giving up a safety factor (e.g., self-collision-free operation), thereby enhancing a robustness of a corresponding motion plan. For example, the safety factor may be checked or reviewed by simulating robot operation using the nominal trajectory. Thus, if an actual trajectory of a robot lags behind the nominal trajectory by more than a specified margin or threshold (e.g., acceptable hysteresis), collision-free operation may no longer be ensured.

The multi-robot optimization engine 114 evaluates candidate solutions based at least in part on the associated costs 119 and advantageously co-optimizes across a set of two or more non-homogeneous parameters, such as across two or more of the following: respective base positions and orientations of the robots, one assignment of tasks to respective ones of the robots, respective goal sequences for the robots, and/or respective trajectories or paths between consecutive goals (e.g., collision-free trajectories or paths). Straight-line trajectories between consecutive goals may be used to simplify the explanation, but the trajectories need not be straight-line trajectories.

Input 109 may include one or more static environment models representing or characterizing the operating environment or shared workspace 104, such as representing a floor, wall, ceiling, columns, other obstacles, etc. The operating environment or shared workspace 104 may be represented by one or more models, such as a geometric model (e.g., point cloud) representing a floor, wall, ceiling, obstacles, and other objects in the operating environment. For example, this may be represented using Cartesian coordinates.

The input 109 may include one or more robot models representing or characterizing each of the robots 102, such as specifying geometry and dynamics, such as size or length, number of linkages, number of joints, type of joints, range of motion, velocity limits, acceleration or jerk limits. The robots 102 may be represented by one or more robot geometry models that define the geometry of a given one of the robots 102a-102c, such as in terms of joints, degrees of freedom, dimensions (e.g., linkage lengths), and/or in terms of the respective C-spaces of the robots 102a-102c.

The input 109 may include one or more sets of tasks to be performed, for example, expressed as goals (e.g., poses, configurations, states, or positions or locations). For example, the tasks may be expressed in terms of end poses, end configurations, or end states and/or intermediate poses, intermediate configurations, or intermediate states of the respective robots 102a-102c. For example, poses, configurations, or states may be defined in terms of joint positions and joint angles/rotations (e.g., joint poses, joint coordinates) of the respective robots 102a-102c. Optionally, input 109 may include one or more dwell durations that specify a nominal amount of time that a robot or a portion thereof should remain at a given destination in order to complete a task (e.g., tightening a screw or nut, picking and placing objects, sorting a pile of objects into two or more distinct piles of objects of different types by two or more robots operating in a common workspace).

In some embodiments, the multi-robot configuration optimization system 108 may generate nominal trajectories for each robot to perform one or more tasks. Nominal trajectories are "as specified" trajectories, where each nominal trajectory includes a time-parameterized ordered set or sequence of poses, configurations, or states of a robot between an initial or starting pose, configuration, or state and a final or ending pose, configuration, or state of the nominal trajectory, where each pose, configuration, or state has a respective timing. Preferably, the poses or configurations are represented in a configuration space (also referred to as C-space) for each robot, or alternatively in a real space or real-world space of a workspace. Timing may be specified in relative terms (e.g., timing defined by a relative offset from a timing of a previous pose) or absolute terms (e.g., timing defined by a duration since the start of, for example, execution of a trajectory, and, for example, relative to a common clock). In at least some instances, a nominal trajectory may specify or include one or more pauses in a motion or path of a robot or a portion thereof, and/or may specify a reversal of a direction of motion or path of a robot or a portion thereof, and may otherwise not be a smooth motion in direction or time. Applicants note that while any given trajectory may correspond to a smooth motion of the robot, the term trajectory as used herein is not so limited, and will generally specify a motion that is neither smooth nor defines a straight path of the robot or a portion thereof. For example, a nominal trajectory may specify a time-parameterized ordered set or sequence of poses through which a robot or a portion thereof moves to accomplish a task or a portion of a task. One nominal trajectory or more than one nominal trajectory may be employed to perform any given task. As explained herein, an actual motion or actual trajectory of a robot or a portion thereof may deviate from a respective nominal trajectory, for example due to undesirable delays in transitioning between poses (e.g., due to a need to dwell or stay at (e.g., over) a target object for a longer time than otherwise expected).

The acceptable hysteresis time evaluator 115 evaluates various candidate hysteresis times for a given nominal trajectory to determine an acceptable hysteresis time that ensures self-collision-free operation even if the actual trajectory of a corresponding robot lags behind the nominal trajectory by no more than the acceptable hysteresis time. In a preferred approach, the acceptable hysteresis time evaluator 115 takes into account not only the effect of a hysteresis in the nominal trajectory of a given robot, but also the effect or hysteresis in the respective nominal trajectories of other robots operating in a common workspace. Thus, the acceptable hysteresis time evaluator 115 can identify an acceptable hysteresis time, or in other words, time hysteresis, for each nominal trajectory that assumes that all actual trajectories of the robots operating in the common workspace are experiencing a worst case of their own respective acceptable hysteresis times. Thus, the acceptable hysteresis time evaluator 115 can, for example, determine a maximum acceptable hysteresis time for each robot that ensures self-collision-free operation even assuming that all robots are experiencing their own respective maximum acceptable hysteresis times. Several methods of determining acceptable hysteresis times are described herein.

The input 109 visual condition includes a limit on the number of robots that can be configured in the shared workspace 104. The input 109 visual condition includes a limit on the number of tasks or goals (referred to herein as a task capacity) that can be assigned to a given robot 102a-102c that can be configured in the shared workspace 104, for example to limit the complexity of the configuration problem to ensure that the configuration problem is solvable, or can be solved within an acceptable time period using available computing resources, or to preemptively eliminate certain solutions that are presumed to be too slow in the event that one of the tasks or goals of a given robot 102a-102c is significantly over-allocated. The input 109 visual condition includes one or more boundaries or constraints on variables or other parameters. The input 109 may optionally include a total number of iteration cycles or a time limit for iterations, which may be used to refine candidate solutions, for example to ensure that the configuration problem is solvable or can be solved within an acceptable time period using available computing resources.

The robotic system 100 may optionally include one or more robot control systems 118 (only one is shown in FIG. 1 ) that are communicatively coupled to control the robot 102. The robot control system 118 may, for example, provide control signals (e.g., drive signals) to various actuators to cause the robot 102 to move between various configurations to various specified targets in order to perform specified tasks.

The robotic system 100 may optionally include one or more motion planners 120 (only one is shown in FIG. 1 ) communicatively coupled to control the robot 102. The motion planner 120 produces, generates, selects, or improves a motion plan for the robot 102, for example to account for small deviations in timing relative to a motion plan provided by the multi-robot optimization engine 114, or to account for the unexpected presence of obstacles (e.g., a human entering an operating environment or shared workspace 104), as described elsewhere herein. The motion planner 120 may be operable to dynamically generate motion plans to cause the robot 102 to perform tasks in an operating environment. The motion planner 120 and other structures and/or operations may be those described in U.S. patent application serial number 62/865,431 filed on June 24, 2019.

Where a motion planner 120 is included, the motion planner 120 is optionally communicatively coupled to receive as input sensory data, such as provided by a sensory subsystem (not shown). The sensory data represents a priori unknown static and/or dynamic objects in the shared workspace 104. The sensory data may be raw data sensed via one or more sensors (e.g., camera, stereo camera, time-of-flight camera, LIDAR) and/or raw data converted into digital representations of obstacles by the sensory subsystem, which may generate a respective discretization of a representation of an environment in which the robot 102 will operate to perform tasks in a variety of different scenarios.

Various communication paths are illustrated in FIG. 1 as lines between various structures, with arrows indicating, in some cases, the direction of inputs 109 and outputs 111. The communication paths may, for example, take the form of one or more wired communication paths (e.g., conductors, signal buses, or optical fibers) and/or one or more wireless communication paths (e.g., via RF or microwave radios and antennas, infrared transceivers). The communication channels may include, for example, one or more transmitters, receivers, transceivers, radios, routers, wired ports, such as Ethernet ports, etc. The general operation of the robot system 100, and in particular the general operation of the multi-robot configuration optimization system 108, is illustrated and described in the international patent application PCT/US2021/013610 with publication number WO 2021/150439, and for the sake of brevity, it is not repeated here. Only some of the more significant differences in operation are described herein, for example: in an embodiment in which the multi-robot configuration optimization system 108 executes the various methods described herein to determine an acceptable lag time, the motion plan is generated or selected based at least in part on the determined acceptable lag time; and/or in an embodiment in which the robot control system 118 executes the various methods described herein to determine an acceptable lag time; The method monitors the actual lag time of a robot executing a motion plan, compares the actual lag time with a margin or threshold value (e.g., an acceptable lag time); and/or controls the robot accordingly, for example by inducing one or more remedial actions when one or more actual lag times exceed one or more acceptable lag times or associated threshold values.

FIG. 2 shows a robotic system according to an illustrated embodiment, wherein a first robot control system 200a includes a first motion planner 204a that generates a first motion plan 206a to control the operation of a first robot 202, and the first motion planner 204a provides the first motion plan 206a and/or a motion representation of an obstacle to other motion planners 204b of other robot control systems 200b to control other robots (not illustrated in FIG. 2 ) via at least one communication channel (indicated by adjacent arrows, e.g., transmitter, receiver, transceiver, radio, router, Ethernet), as appropriate. In the robot control systems 200a, 200b of FIG2, acceptable lag time is determined by a motion planner in conjunction with the motion plan 206a, 206b, which includes one or more nominal trajectories to be executed by the robot 202. Compared to the robot control system 100 (FIG. 1), the robot control systems 200a, 200b of FIG2 do not necessarily perform optimization of work cell layout or mission planning. In addition, as explained herein, the robot control systems 200a, 200b of FIG2 monitor actual lag time and, if warranted, select and/or take remedial actions as appropriate.

Similarly, the other motion planners 204b of the other robot control systems 200b generate other motion plans 206b to control the operation of other robots (not illustrated in FIG. 2 ) and provide the other motion plans 206b to the first motion planner 204a and other of the other motion planners 204b of the other robot control systems 200b as appropriate. The motion planners 204a, 204b may also receive motion completion messages 209 indicating when the motion of each robot 202 has been completed as appropriate. This may allow the motion planners 204a, 204b to generate new or updated motion plans based on a current or updated state of one of the shared workspaces. For example, a portion of a shared workspace may be blocked, unblocked, or otherwise made available for a second robot to perform a task therein after a first robot 202 has completed a motion that is part or all of a set of motions formed as part of a task completed by the first robot 202. Additionally or alternatively, the motion planners 204a, 204b may receive information (e.g., images, occupancy grids, joint positions, and joint angles/rotations) collected by various sensors or generated by other motion planners 204b indicating when a portion of a shared workspace may be blocked, unblocked, or otherwise made available for a second robot to perform a task therein after a first robot 202 has completed a motion that is part or all of a set of motions formed as part of a task completed by the first robot 202.

As described herein, motion plans 206a, 206b specify a nominal trajectory for each robot, e.g., to perform one or more tasks. As previously explained, a nominal trajectory is an "as specified" trajectory, wherein each nominal trajectory comprises a time-parameterized ordered set or sequence of poses, configurations, or states of a robot, wherein each pose, configuration, or state has a respective timing. The poses, configurations, or states are preferably represented in a configuration space (also referred to as C-space) for the respective robot.

The robot control systems 200a, 200b may be communicatively coupled, such as via at least one communication channel (represented by adjacent arrows, such as a transmitter, receiver, transceiver, radio, router, Ethernet), to receive the motion planning map 208 and/or the swept volume representation 211 from one or more sources 212 of the motion planning map 208 and/or the swept volume representation 211, as the case may be. According to one illustrated embodiment, the sources 212 of the motion planning map 208 and/or the swept volume representation 211 may be separate and distinct from the motion planners 204a, 204b. The source 212 of the motion planning graph 208 and/or the swept volume representation 211 may be, for example, one or more processor-based computer systems (e.g., server computers) that may be operated or controlled by the respective manufacturer of the robot 202 or by some other entity. The motion planning graph 208 may each include a set of nodes 214 (only two are labeled in FIG. 2 ) representing the pose, configuration, or state of the respective robot and a set of edges 216 (only two are labeled in FIG. 2 ) coupling the nodes 214 in the respective plurality of pairs of nodes 214 and representing legal or valid transitions between the poses, configurations, or states. The poses, configurations, or states may be defined, for example, in a respective configuration space (C-space) of the robot, thereby representing multiple sets of joint positions, orientations, poses, or coordinates of each of the joints of the respective robot 202. Thus, each node 214 may represent a pose, configuration, or state of a robot 202 or a portion thereof that is completely defined by the poses, configurations, or states of the joints comprising the robot 202. The motion planning graph 208 may be determined, set, or defined prior to a run time (e.g., during a pre-run time or configuration time) (i.e., defined prior to executing a task). The optional swept volume representation 211 represents the respective volumes that a robot 202 or a portion thereof will occupy when executing a movement or transition between poses corresponding to a respective edge 216 of the motion planning graph 208. The optional swept volume representation 211 may be represented in any of a variety of forms such as voxels, a Euclidean distance field, or a geometric object level. This advantageously allows some of the most computationally intensive work to be performed before runtime, when responsiveness is not a particular concern. Although swept volumes are used herein, this is exemplary and any of a variety of other methods of collision assessment may be employed.

Each robot 202 visibly includes a base (not shown in FIG. 2 ). The base may be fixed in the environment or may be movable in the environment (e.g., an autonomous or semi-autonomous vehicle). Each robot 202 visibly includes a set of linkages, joints, end-of-arm tools or end effectors and/or actuators 218 a, 218 b, 218 c (three are shown, collectively referred to as 218 ) that are operable to move the linkages around the joints. The set of linkages, joints, end-of-arm tools or end effectors typically include one or more appendages of the robot, which may be movably connected to the base of the robot. Each robot 202 may include one or more motion controllers (e.g., motor controllers) 220 (only one is shown) that receive control signals, such as in the form of motion plans 206a, and provide drive signals to drive actuators 218. Alternatively, the motion controllers 220 may be separate from the robots 202 and communicatively coupled to the robots 202. Each robot 202 may be positioned and oriented in a shared workspace based on an optimized work cell layout, such as described in International Patent Application PCT/US2021/013610, published as WO 2021/150439.

Each robot 202 may have a separate robot control system 200a, 200b (only one robot is illustrated in FIG. 2 ), or alternatively one robot control system 200a may perform motion planning for two or more robots 202. For purposes of illustration, a first robot control system 200a will be described in detail. Those skilled in the art will recognize that the description may apply to similar or even identical additional instances of other robot control systems 200b.

The first robot control system 200a may include one or more processors 222, and one or more associated non-transitory computer or processor readable storage media, such as system memory 224a, disk drive 224b, and/or memory or register (not shown) of the processor 222. The non-transitory computer or processor readable storage media (e.g., system memory 224a, disk drive 224b) is communicatively coupled to the processor 222a via one or more communication channels (e.g., system bus 234). The system bus 234 may adopt 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 alternatively communicate with each other via one or more other communication channels or via ^{Thunderbolt®} , such as one or more parallel cables, serial cables, or a wireless network channel capable of high-speed communication, such as Universal Serial Bus (“USB”) 3.0, Peripheral Component Interconnect Express (PCIe).

The first robot control system 200a may also be communicatively coupled to one or more remote computer systems, such as a server computer (e.g., source 212), a desktop computer, a laptop computer, an ultraportable computer, a tablet computer, a smart phone, a wearable computer, and/or sensors (not illustrated in FIG. 2 ), for example, directly communicatively coupled via interface 227 or indirectly communicatively coupled to various components of the first robot control system 200a. A remote computing system such as a server computer (e.g., source 212) may be used to program, configure, control, or otherwise interact with or input data (e.g., motion planning map 208, sweep volume representation 211, mission specification 215, or even candidate paths or nominal trajectories) to the first robot control system 200a and various components within the first robot control system 200a. Such a connection may be through one or more communication channels 210, such as one or more wide area networks (WANs), such as Ethernet, or the Internet using Internet protocols. As described above, the estimated runtime calculation may be performed by a system separate from the first robot control system 200a or the first robot 202, and the runtime calculation may be performed by the processor 222 of the first robot control system 200a while one or more robots are performing a task. In some embodiments, one or more of the robot control systems 200a, 200b may be onboard a respective robot (e.g., the first robot 202).

As described above, the first robot control system 200a may include one or more processors 222 (i.e., circuit systems), non-temporary storage media (e.g., system memory 224a, disk drive 224b), and a system bus 234 coupling various system components. The processor 222 may be any logic processing unit, such as one or more central processing units (CPUs), digital signal processors (DSPs), graphic processing units (GPUs), field programmable gate arrays (FPGAs), application specific integrated circuits (ASICs), programmable logic controllers (PLCs), etc. The system memory 224a may include read-only memory ("ROM") 226, random access memory ("RAM") 228, flash memory 230, EEPROM (not shown), or any combination thereof. A basic input/output system ("BIOS") 232, which may form part of the ROM 226, contains basic routines that help transfer information between components within the first robot control system 200a, such as during startup.

The disk drive 224b can be, for example, a hard disk drive for reading from and writing to a disk, a solid-state (e.g., flash memory) hard disk for reading from and writing to a solid-state memory, and/or an optical disk drive for reading from and writing to a removable optical disk. In various different embodiments, the first robot control system 200a may also include any combination of such disk drives. The disk drive 224b can communicate with the processor 222 via the system bus 234. As known to those skilled in the art, the disk drive 224b may include an interface or controller (not shown) coupled between such a disk drive and the system bus 234. The disk drive 224b and its 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 first robot control system 200a. Those skilled in the art will appreciate that other types of computer-readable media capable of storing data accessible by a computer may be used, such as WORM disk drives, RAID disk drives, tape cartridges, digital video disks ("DVDs"), Bernoulli boxes, RAM, ROM, smart cards, etc.

Executable instructions and data may be stored in system memory 224a, such as an operating system 236, one or more applications 238, other programs or modules 240, and program data 242. Applications 238 may include processor-executable instructions that cause processor 222 to perform one or more of the following.

The application 238 may include processor executable instructions that cause the processor 222 to receive or generate a discretized representation of the common workspace in which the robot 202 will operate, the discretized representation including obstacles and/or target objects or workpieces in the common workspace, where the planned movements of other robots may be represented as obstacles.

The application 238 may include processor executable instructions that cause the processor 222 to generate motion plans 206a, 206b that specify nominal trajectories. Each of the nominal trajectories generally defines a respective ordered sequence of poses, configurations, or states of a robot or a portion thereof, parameterized by time.

The application 238 may include processor executable instructions that cause the processor 222 to determine respective acceptable lag times for nominal trajectories. An acceptable lag time is generally an amount of time that the actual motion of a robot 202 (e.g., actual trajectory) may lag behind or differ from a nominal time specified by the corresponding nominal trajectory while still ensuring at least self-collision-free operation relative to one or more other robots operating in a common workspace. In some embodiments, self-collision-free operation is premised on the other robots not experiencing any lag time in their execution, although preferably, self-collision-free operation is premised on all robots operating within their respective acceptable lag times in their respective nominal trajectories, thereby taking into account the lag time experienced by each robot in the execution of its respective nominal trajectory.

In order to generate a motion plan, generate a nominal trajectory and/or determine acceptable lag time, the application 238 may include processor executable instructions that cause the processor 222 to call or otherwise perform a collision assessment. Collision assessment is often referred to as "collision detection" or "collision checking", even though this usually determines a probability or likelihood of a collision and usually occurs before the actual movement of the robot, and does not refer to detecting an actual physical collision of the robot during the physical movement of the robot. Collision assessment may be interchangeably referred to as "collision detection" or "collision checking" or "collision analysis" herein.

The application 238 may include processor-executable instructions that cause the processor 222 to set cost values or cost functions for edges in a motion planning graph, e.g., to reflect a determined probability or likelihood of experiencing a collision and other parameters as appropriate. The application 238 may include processor-executable instructions that cause the processor 222 to set cost values or cost functions for nominal trajectories, e.g., to reflect respective acceptable lag times and, alternatively, to additionally reflect a determined probability or likelihood of experiencing a collision and other parameters as appropriate.

The application 238 may include processor-executable instructions that cause the processor 222 to: evaluate available nominal trajectories generated from a motion planning map; identify (e.g., select, determine, generate) a nominal trajectory based on, for example, a cost or a cost function; and/or identify or generate a motion plan executable by a robot to cause the robot to perform motion (e.g., further perform one or more tasks by the robot). The application 238 may include processor-executable instructions that cause the processor 222 to store the determined motion plan and/or provide instructions that cause one or more robots to execute or otherwise move in accordance with the motion plan, as appropriate. Movement planning and movement plan construction (e.g., for example, performing collision assessment or detection, setting, updating, or adjusting costs or cost functions based at least in part on collision assessment or detection and, as appropriate, based in part on a determined acceptable hysteresis time) and nominal trajectory generation, analysis, or evaluation of candidate nominal trajectories (e.g., selecting between two nominal trajectories based at least in part on respective acceptable hysteresis times) may be performed as described herein and as described in the references incorporated herein by reference (e.g., the methods of reference FIGS. 3 , 4 , 5 , 6 , 7 , and 8 ). Collision assessment or detection may employ any of the types of structures, techniques, and algorithms described herein as well as suitable structures, techniques, and/or algorithms described elsewhere.

The application 238 may also include one or more machine-readable and machine-executable instructions that cause the processor 222 to monitor robot motion (e.g., actual trajectories) and evaluate actual delays or actual lag times of such actual trajectories compared to margins or thresholds (e.g., acceptable lag times or based on acceptable lag times) of corresponding nominal trajectories. The application 238 may also include one or more machine-readable and machine-executable instructions, as appropriate, that cause the processor 222 to select and/or take remedial action (e.g., slowing down one or more robots, stopping one or more robots, and/or speeding up one or more robots) if warranted (e.g., if the actual lag time approaches or exceeds an acceptable lag time).

The application 238 may also include one or more machine-readable and machine-executable instructions that cause the processor 222 to, as appropriate: monitor the robot in the environment; determine when a path along an actual trajectory becomes unblocked or clear; and cause the robot to move to a target in response to the path along the actual trajectory becoming unblocked or clear.

The application 238 may further include one or more machine-readable and machine-executable instructions that cause the processor 222 to perform other operations, such as processing sensory data (captured via sensors) as appropriate. The application 238 may further include one or more machine-executable instructions that cause the processor 222 to perform various other methods described herein and in the references incorporated herein by reference.

In various embodiments, one or more of the operations described above may be performed by one or more remote processing devices or computers that are linked via an interface 227 through a communication channel 210 (e.g., a network).

Although shown in FIG. 2 as being stored in system memory 224a, operating system 236, applications 238, other programs/modules 240, and program data 242 may be stored on other non-transitory computer or processor-readable media such as disk drive 224b.

The motion planner 204a of the first robot control system 200a may include dedicated motion planner hardware, or may be implemented in whole or in part via the processor 222 and processor executable instructions stored in the system memory 224a and/or the disk drive 224b.

The motion planner 204a may include or implement a motion converter 250, a path generator 252, a collision evaluator 253, a cost setter 254, optionally a path analyzer 255, a trajectory generator 256, an acceptable lag time evaluator 257, and optionally a nominal trajectory analyzer 258. Each of these devices may be implemented via one or more processors (e.g., circuit systems) that execute logic (e.g., executable software instructions, firmware instructions, hardwired logic, or any combination thereof).

The motion converter 250 converts the motion of an object (e.g., other robots, people) into a representation of an obstacle. The motion converter 250 receives a motion plan 206b or other motion representation from other motion planners 204b.

The motion converter 250 may include a trajectory predictor 251 for predicting the trajectory of a transient object (e.g., other robots, other objects including, for example, humans), for example, when the trajectory of the transient object is unknown (e.g., when the object is another robot but has not yet received a motion plan from the other robot, when the object is not another robot (e.g., a human). The trajectory predictor 251 may, for example, assume that an object will continue an existing motion in one of the two aspects of direction, velocity, and acceleration if there is no change. The trajectory predictor 251 may, for example, take into account an expected change in a motion or path of an object, for example, where the object's path will result in a collision, so the object may be expected to stop or change direction, or where a destination of the object is known, so the object may be expected to stop shortly after reaching the destination. In at least some instances, the trajectory predictor 251 may employ a learned behavior model of the object to predict the trajectory of the object. The trajectory predictor 251 may, for example, extrapolate known motions and expected changes to produce a predicted trajectory of the instantaneous object.

The motion converter 250 then determines an area or volume corresponding to the known and/or extrapolated motion of the object, as appropriate. For example, the motion converter can convert the motion into a corresponding swept volume, i.e., a volume swept by the corresponding robot or part thereof as it moves or transitions between poses as represented by the motion plan, such as by generating a volume representation of the robot or part thereof and projecting the volume representation along a path defined by the trajectory of the robot or part thereof. For another example, the motion converter may convert motion into a corresponding swept volume, for example, by generating a volume representation of an object (e.g., a non-robotic object, such as a human) and projecting the volume representation of the object along a path defined by a known and/or extrapolated trajectory of the object. Advantageously, the motion planner 204a may simply queue obstacles (e.g., swept volumes) and may not need to determine, track, or indicate a time corresponding to the motion or swept volume. Although generally described as a motion converter 250 of the first robot 202 that can convert the motion of other robots (not illustrated in Figure 2) into obstacles, in some embodiments, other robot control systems 200b of other robots operating in a shared workspace can provide an obstacle representation of a particular motion (e.g., a swept volume) to the motion planner 204a of the first robot 202.

The path generator 252 generates a path from one pose, configuration, or state (e.g., a starting pose, configuration, or state) to another pose, configuration, or state (e.g., an ending pose, configuration, or state; or a target pose, configuration, or state). The path generator 252 may determine or identify one or more feasible paths from a start (e.g., a starting node or a starting pose or a starting state) to a target (e.g., a target node or a target pose or a target state; an ending node or an ending pose or an ending state). For example, the path generator 252 may determine or identify an ordered sequence of nodes in a motion planning graph that provide a complete path from a start or current node to a target or end node (i.e., an ordered set of nodes for which each of a pair of consecutive nodes in the complete path has a respective valid transition therebetween, as indicated by the presence of an edge coupling the nodes in the pair of nodes). As described above, each node may correspond to a respective pose, configuration, or state of a respective robot. The path generator 252 may use or execute any type of path finding methods, techniques, and/or algorithms. For example, the path generator 252 may employ various methods, techniques and/or algorithms for randomly or pseudo-randomly generating paths to select a plurality of sequences of nodes in a motion planning graph, wherein a node is connected to the next node in the sequence via an edge (i.e., an edge representing a valid transition between the poses represented by the connected nodes). The path generator 252 may generate a relatively large number of candidate paths between a starting node and an ending node, and thus between a starting pose, configuration or state and an ending pose, configuration or state. In some implementations, the path generator 252 may determine or identify feasible paths or trajectories independently of cost (e.g., cost representing a probability or likelihood of experiencing a collision along the path), thereby establishing a set of feasible paths or candidate paths that may later be evaluated based at least in part on the probability or likelihood of experiencing a collision. In other examples, the path generator 252 may take cost (e.g., cost representing a probability or likelihood of experiencing a collision along the path) into consideration when determining or identifying feasible paths or trajectories.

The collision evaluator 253 performs collision evaluation, also known as collision detection or collision analysis. Specifically, the collision evaluator 253 performs collision evaluation as part of determining whether a candidate path representing a transition or movement of a given robot 202 or a portion thereof as specified by a nominal trajectory will result in or may result in a collision with an obstacle. As described above, the movement of other robots can advantageously be represented as obstacles. Therefore, the collision evaluator 253 can determine whether a movement of a robot will result in or may result in a collision with another robot moving through a shared workspace.

As explained herein, collision assessment, detection, or analysis may be performed not only for candidate paths, but may additionally or alternatively be performed for nominal trajectories (e.g., trajectories as specified) and particularly for nominal trajectories in which various hysteresis times are introduced (e.g., simulating an actual trajectory that may lag behind the nominal trajectory). In at least some embodiments, collision assessment, detection, or analysis of nominal trajectories in which various hysteresis times are introduced may be performed for each of two or more robots (e.g., multiple pairs of robots). Collision assessment, detection, or analysis may be performed for respective sets of one or more nominal trajectories for each of the robots in multiple pairs of robots. Collision assessment, detection, or analysis may be performed for each of the trajectories with a hysteresis equal to zero and with various (e.g., one, two, or more) non-zero hysteresis times introduced into their respective nominal trajectories, such as evaluating the pair of trajectories for each permutation of their hysteresis times. As explained herein, collision assessment, detection, or analysis does not necessarily result in a binary result, but may result in a non-binary value, such as thereby representing a probability or likelihood of a collision between a pair of robots resulting from the path or trajectory.

In some embodiments, the collision evaluator 253 implements software-based collision evaluation, detection, or analysis, such as to perform a bounding box-bounding box collision evaluation, detection, or analysis based on a level of geometric (e.g., sphere) representation of a volume swept by a robot (e.g., the first robot 202) or a portion thereof during movement. In some embodiments, the collision evaluator 253 implements hardware-based collision evaluation, detection, or analysis, such as to employ a set of dedicated hardware logic circuits to represent obstacles and stream representations of motion through the dedicated hardware logic circuits. In hardware-based collision assessment, detection or analysis, the collision detector may employ one or more configurable circuit arrays, such as one or more FPGAs 259, and may optionally generate Boolean collision assessments.

The cost setter 254 may set, update and/or adjust a cost or cost function associated with a transition (e.g., an edge in a motion planning graph) or a movement (e.g., a trajectory of a motion planning). The cost setter 254 may set, update and/or adjust a cost or cost function, for example, based at least in part on collision assessment, detection or analysis. For example, the cost setter 254 may set a relatively high cost value for an edge or trajectory that indicates that a transition between nodes or motions will or may result in a collision. For another example, the cost setter 254 may set a relatively low cost value for an edge or trajectory that indicates that a transition between nodes or motions will not or may not result in a collision. Setting, updating and/or adjusting a cost or cost function may include setting, updating or adjusting a cost or cost function that is logically associated with a corresponding edge or track via some data structure (e.g., a field in a record, a pointer in a list, a table).

In some implementations, the cost setter 254 may, for example, set, update, or adjust a cost or cost function, as appropriate, to at least partially represent a determined acceptable hysteresis time for a respective nominal trajectory. This may advantageously allow the first robot control system 200a to select a nominal trajectory from a set of available or candidate nominal trajectories for a robot to perform a given task, such as thereby selecting the nominal trajectory with the longest or largest determined acceptable hysteresis time for use in generating a motion plan that results in more robust operation when the robot is experiencing real-world conditions while performing the task.

In some embodiments, the cost setter 254 may additionally or alternatively set, update, or adjust a cost or cost function to at least partially represent one or more other parameters, such as one or more of: a severity of the collision, an expenditure or consumption of energy and/or time or delay to perform or complete.

The alternative path analyzer 255 may use the motion planning graph 208 along with the cost or cost function to determine or identify one or more suitable paths and/or select a single path (i.e., the selected path, e.g., an optimal or optimized path). The alternative path analyzer 255 may, for example, identify one or more paths that meet certain specified criteria (e.g., a cost within a critical upper limit), or even select a single path from a set of candidate feasible paths determined or identified by the free path generator 252 (e.g., select a single minimum cost path). The identified paths may be named suitable paths because they represent options with an acceptably low probability or likelihood of collision. The alternative path analyzer 255 may, for example, be configured as a minimum cost path optimizer that determines a minimum or relatively low cost path between two nodes (and therefore, between two poses, configurations, or states represented by respective nodes in the motion planning graph). The alternative path analyzer 255 may use or execute any type of path finding algorithm, such as a minimum cost path finding algorithm, thereby taking into account the cost value associated with each edge, the cost value representing a probability or likelihood of a collision and, as appropriate, one or more other parameters (e.g., a severity of the collision, an expenditure or consumption of energy and/or time or delay to execute or complete). In certain embodiments, cost-based optimization may alternatively or additionally be applied to the nominal trajectory to advantageously allow acceptable lag times to be expressed in cost values or cost functions, as described herein, for example in addition to a probability or likelihood of a collision, and optionally in addition to or in lieu of one or more of: a severity of the collision, an expenditure or consumption of energy and/or time or delay to execute or complete.

The nominal trajectory generator 256 can generate a nominal trajectory that a robot can follow, for example, to complete a task. As explained herein, a trajectory includes an ordered sequence of poses or configurations or states parameterized by time through which a robot or at least a portion thereof can move, for example, to complete a task. The nominal trajectory generator 256 can generate trajectories for all of the generated paths, or if the optional path analyzer 255 is used, it can generate trajectories for only selected paths or only one selected path.

The acceptable hysteresis time evaluator 257 evaluates various candidate hysteresis times for a given nominal trajectory to determine an acceptable hysteresis time that ensures self-collision-free operation even if the actual trajectory of a corresponding robot lags behind the nominal trajectory by no more than the acceptable hysteresis time. In a preferred approach, the acceptable hysteresis time evaluator 257 takes into account not only the effect of a hysteresis in the nominal trajectory of a given robot, but also the effect or hysteresis in the respective nominal trajectories of other robots operating in a common workspace. Thus, the acceptable hysteresis time evaluator 257 identifies an acceptable hysteresis time for each nominal trajectory that assumes that the actual trajectories of all of the robots operating in the common workspace are experiencing a worst case scenario of their own respective acceptable hysteresis times. Thus, the acceptable hysteresis time evaluator 257 can, for example, determine an optimized (e.g., maximum) hysteresis time for each robot that still ensures self-collision-free operation. Several methods of determining acceptable hysteresis times are described herein, for example, with respect to a method 300 (FIG. 3), a method 400 (FIG. 4), a method 500 (FIG. 5), a method 600 (FIG. 6), and/or a method 700 (FIG. 7).

The nominal trajectory analyzer 258 may use a cost value or cost value function or some other objective function to determine, identify or select one or more suitable trajectories and/or determine, identify or select a single trajectory (i.e., a selected trajectory, e.g., an optimal or optimized trajectory). The nominal trajectory analyzer 258 may, for example, determine, identify or select one or more trajectories that meet some specified criteria (e.g., a cost within a critical upper limit), or even determine, identify or select a single trajectory (e.g., a trajectory with the lowest cost) from a set of feasible or candidate trajectories generated by the free trajectory generator 256 (e.g., determine, identify or select a lowest cost trajectory). The nominal trajectory analyzer 258 may, for example, constitute a minimum cost trajectory optimizer that determines a minimum or relatively low cost trajectory between two poses, configurations, or states represented by respective nodes in the motion planning graph. The nominal trajectory analyzer 258 may, for example, select a set of trajectories for all robots that optimizes robust operation by maximizing a sum of all lag times across all robots in a given time period, a given task, or a given set of tasks to be performed by the robots. The nominal trajectory analyzer 258 may use or execute any type of algorithm, such as a minimum cost trajectory finding algorithm, that takes into account a cost associated with each trajectory, where the cost or cost function represents a probability or likelihood of a collision and an acceptable lag time, and optionally represents one or more of the following: a severity of the collision, an expenditure or consumption of energy and/or time or delay to execute or complete.

Various algorithms and structures may be used to determine minimum cost paths and/or minimum cost trajectories, including those implementing the Bellman-Ford algorithm, but other algorithms and structures may also be used, including but not limited to any such procedures in which a minimum cost path or minimum cost trajectory is determined to be a path between two nodes in the motion planning graph 208 or a trajectory such that the sum of the costs or weights of its constituent edges or motions is minimized as specified by an ordered sequence of time-parameterized poses. This process improves the motion planning of a robot 102 (Figure 1), 202 (Figure 2) by determining an acceptable lag time, selecting a trajectory based on the determined acceptable lag time, monitoring the actual trajectory to evaluate whether the actual lag time approaches or exceeds a margin or threshold value (e.g., the acceptable lag time), and selecting and/or taking remedial actions as appropriate if warranted.

Although not illustrated, the motion planner 204a may include a forward-looking evaluator that may, for example, cause an improvement action to be taken in response to a determination that a blocking or possible blocking condition exists or occurs, or a determination that an blocking or possible blocking position will occur if a given robot and/or other robots move along a respective trajectory. In at least some embodiments, the forward-looking evaluator may determine or select a type of improvement action to be taken, for example, by selecting from a set of different types of improvement actions based on one or more criteria. As described in U.S. Patent Application No. 63/327,917 filed on April 6, 2022, one or more different types of improvement actions (referred to as remedial actions) may be implemented. For example, a new, revised or replacement first motion plan may be generated to move a given robot to a first target based on an analysis of a second motion plan to move the given robot from the first target. For another example, a new, revised or replacement motion plan may be generated for another robot that is blocking or may block the given robot. For another example, a new order of a set of targets including a first target and at least a second target may be determined or generated. This may be determined or generated pseudo-randomly or based on one or more heuristics (e.g., always trying to move the first target one position downstream relative to the order of the set of targets being modified).

Although not illustrated, the motion planner 204a may include an optional multi-path analyzer that can analyze a total cost or aggregate cost associated with the trajectories of two or more motion plans (e.g., the aggregate cost of a first motion plan and a second motion plan), such as described in U.S. Patent Application No. 63/327,917 filed on April 6, 2022. This can be used to identify the combination of motion plans that has the lowest overall cost. The multi-path analyzer may, for example, use or execute any type of minimum cost finding algorithm, such as to consider the total cost or aggregate cost of two or more options for a first motion plan in combination with a second motion plan, thereby taking into account cost values representing an associated likelihood of collision and, as appropriate, one or more of: an acceptable lag time, a severity of the collision, an expenditure or consumption of energy and/or time or delay associated with executing or completing the transition represented by the respective edges.

The motion planner 204a visually includes a trimmer 260. The trimmer 260 can receive information indicating the completion of a motion by another robot, which information is named herein as a motion completion message 209. Another option is that a flag can be set to indicate completion. In response, the trimmer 260 can remove an obstacle or a portion of an obstacle that represents the now completed motion. This can allow a new motion plan to be generated for a given robot, which may be more efficient or allow the given robot to focus on performing a task that was previously blocked by the motion of another robot. This method advantageously allows the motion converter 250 to ignore the timing of the motion when generating an obstacle representation of the motion while still achieving a better throughput than using other techniques. The motion planner 204a may additionally send a signal, prompt or trigger to cause the collision evaluator 253 to perform a new collision detection or evaluation given the modification of the obstacle to produce an updated motion planning graph in which the edge weights or costs associated with the edges have been modified, and cause the cost setter 254, the optional path analyzer 255 and the nominal trajectory analyzer 258 to update the cost values and accordingly determine a new or revised path, trajectory and/or motion plan.

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

The processor 222 and/or the motion planner 204a may be or may include any logic processing unit, such as one or more central processing units (CPUs), digital signal processors (DSPs), graphics 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, Celeron, Core, Core 2, Itanium, and Xeon families of microprocessors provided by ^Intel® Corporation; K8, K10, Bulldozer, and Bobcat series of microprocessors provided by Advanced Micro Devices; A5, A6, and A7 series of microprocessors provided by Apple Computer; Snapdragon series of microprocessors provided by Qualcomm; and SPARC series of microprocessors provided by Oracle Corporation. The structures and operations of the various structures shown in FIG. 2 may implement or employ structures, techniques, and algorithms described in or similar to those described in the following patents: International Patent Application No. PCT/US2017/036880 filed on June 9, 2017; International Patent Application Publication No. WO 2016/122840 filed on January 5, 2016; U.S. Patent Application No. 62/616,783 filed on January 12, 2018; International Patent Application PCT/US2021/013610 with Publication No. WO 2021/150439; and/or U.S. Patent Application No. 63/327,917 filed on April 6, 2022.

Although not required, many embodiments 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.

Motion planning operations may include, but are not limited to, generating or transforming one, more or all of the following: a representation of the robot's geometry based on a geometric model, the mission specification 215, and optionally a representation of the volume (e.g., a swept volume) occupied by the robot in various states or postures and/or during movement between states or postures, a representation in digital form, such as a point cloud, a Euclidean distance field, a data structure format (e.g., a hierarchical format, a non-hierarchical format), and/or a curve (e.g., a polynomial or spline representation). Motion planning operations may include, but are not limited to, generating or transforming one, more, or all of the following: a representation of a static or persistent obstacle and/or sensory data representing a static or transient obstacle, such as a point cloud, a Euclidean distance field, a data structure format (e.g., a hierarchical format, a non-hierarchical format), and/or a curve (e.g., a polynomial or spline representation).

Motion planning operations may include, but are not limited to, using various collision assessment techniques or algorithms (e.g., software-based, hardware-based) to assess, detect, determine, or predict collisions between various poses, configurations, or state transitions of the robot, or the movement of the robot between states or poses along respective trajectories.

In certain embodiments, motion planning operations may include, but are not limited to: determining one or more motion planning maps, motion plans, or roadmaps having a nominal trajectory; an acceptable lag time for the nominal trajectory, storing the determined planning maps, motion plans, or roadmaps or the acceptable lag time; and/or providing planning maps, motion plans, or roadmaps or the acceptable lag time to control the operation of one or more robots, and monitoring the operation of the robots as appropriate, and selecting and/or taking remedial actions if warranted.

In one embodiment, collision detection or estimation is performed in response to a function call or similar process and a Boolean value is returned thereto. The collision evaluator 253 may be implemented via one or more field programmable gate arrays (FPGAs) 259 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.

In various implementations, such operations may be performed entirely in hardware circuitry, or as software stored in a memory such as system memory 224a, and executed by one or more hardware processors 222a, 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, programmable logic controllers (PLCs), electrically programmable read-only memories (EEPROMs), or as a combination of hardware circuitry and software stored in a memory storage device.

Various aspects of perception, map construction, collision detection, and path finding that may be used in whole or in part are also described in the following patents: International Patent Application No. PCT/US2017/036880 filed on June 9, 2017; International Patent Application Publication No. WO 2016/122840 filed on January 5, 2016; U.S. Patent Application No. 62/616,783 filed on January 12, 2018; U.S. Patent Application No. 62/856,548 filed on June 3, 2019; International Patent Application No. PCT/US2020/039193 filed on June 23, 2020 and its publication number is WO 2020/263861; International Patent Application PCT/US2021/013610, Publication No. WO 2021/150439; and/or U.S. Patent Application No. 63/327,917, filed on April 6, 2022. Those skilled in the art will appreciate that the illustrated embodiments and other embodiments may be practiced with other system structures and configurations and/or other computing system structures and configurations, including robots, handheld devices, multiprocessor systems, microprocessor-based or programmable consumer electronics, personal computers ("PCs"), network-connected PCs, minicomputers, mainframe computers, and the like. Embodiments or embodiments or portions thereof (e.g., at configuration time and run time) may be practiced in a distributed computing environment where tasks or modules are executed by remote processing devices linked via a communications 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 are important to help improve campaign planning.

For example, various motion planning solutions "bake" a roadmap (i.e., a motion plan) into a processor (e.g., FPGA 259), and each edge in the roadmap corresponds to a non-reconfigurable Boolean circuit of the processor. Designs that "bake" the plan into the processor present a problem in that finite processor circuitry cannot store multiple or large plans and generally cannot be reconfigured for use with different robots.

One solution provides a reconfigurable design that places the schematic information in a memory storage device. This approach stores the information in memory rather than baking it into a circuit. Another approach uses a templated reconfigurable circuit to replace the memory.

As described above, some of the information (e.g., robot geometry) may be captured, received, input, or provided during a configuration time (i.e., prior to run time). The received information may be processed during configuration time to produce processed information (e.g., motion plan 208) to speed up operations or reduce computational complexity during run time.

During runtime, collision detection may be performed for the entire environment (i.e., the shared workspace), including determining, for any pose or motion between poses, whether any part of the robot will collide or is predicted to collide with another part of the robot itself, with other robots or parts thereof, with persistent or static obstacles in the environment, or with transient obstacles in the environment with unknown trajectories (e.g., humans).

The first robot control system 200a or some other processor-based system may include a lag monitor 264 (interchangeably referred to as a lag time monitor 264) and/or a remedial action selector 266. The lag time monitor 264 and/or a remedial action selector 266 operate during an operating time (e.g., during operation of one or more robots to complete one or more tasks) to monitor the operation of the robots and select and take remedial actions as appropriate when warranted (e.g., if the actual lag time approaches or exceeds a margin or threshold value, such as a corresponding acceptable lag time, then slowing down or stopping one or more robots).

The hysteresis monitor 264 monitors the actual hysteresis of the actual trajectory executed by the robot and compares the actual hysteresis to a margin or threshold value corresponding to the nominal trajectory (e.g., representing or even equal to a margin or threshold value corresponding to an acceptable hysteresis). As previously explained, while in some instances the actual trajectory may match the nominal trajectory, in many instances the actual trajectory will not match the nominal trajectory, typically with a timing of at least some of the execution of the pose, configuration, state, or motion of the actual trajectory lagging behind the timing of those poses, configurations, states, or motions as specified by the nominal trajectory. The lag time monitor 264 determines whether the actual lag time and the monitored amount of the actual lag time of the actual trajectory of a robot approach or exceed a margin or threshold value for a respective robot (e.g., a respective determined acceptable lag time for a respective nominal trajectory). If the actual lag time of any of the robots operating in the shared workspace exceeds a respective threshold value (e.g., approaches or exceeds a corresponding acceptable lag time), the lag time monitor 264 can, for example, provide an indication (e.g., set a flag, send a message and/or call the remedial action selector 266). In some embodiments, the margin or threshold value may be set to include a desired safety factor, such as a set amount or defined percentage less than the determined acceptable hysteresis time for the respective nominal trajectory. Thus, in some embodiments, the margin or threshold value may be set equal to the corresponding acceptable hysteresis time, while in other embodiments, the margin or threshold value may be less than (e.g., 10% less) the corresponding acceptable hysteresis time and thereby allow detection and remedial action to be taken before a self-collision may occur.

The remedial action selector 266 selects one or more appropriate remedial actions and/or causes the selected remedial actions to be taken in response to a determination that the monitored amount of lag time for a respective trajectory of any robot exceeds a respective threshold value (e.g., a determined acceptable lag time). A determination that the actual lag time exceeds a respective threshold value (e.g., a determined acceptable lag time) means that self-collision-free motion is no longer ensured, and therefore taking one or more remedial actions may be warranted.

The remedial action may include one or more of the following: stopping a movement of one or more robots; slowing down a movement of one or more robots; and/or speeding up a movement of one or more robots. For example, the movement of one, two, more, or even all robots may be stopped. For another example, the movement of one, two, more, or even all robots may be slowed down, such as by different amounts relative to each other. For another example, the movement of one, two, more, or even all robots may be accelerated, such as by different amounts relative to each other. For another example, the movement of one or more robots may be stopped, while the movement of one or more robots may be slowed down. For another example, the movement of one or more robots may be stopped, while the movement of one or more robots may be accelerated or maintained constant. For another example, the movement of one or more robots may be decelerated, while the movement of one or more robots may be accelerated or maintained constant. For another example, the movement of one or more robots may be stopped, while the movement of one or more robots may be decelerated and the movement of one or more other robots may be accelerated or maintained constant. To take at least one remedial action, the processor-based system may send instructions to one or more actuators (e.g., via actuators 218a to 218c of motion controller 220) to cause the robot's movement to stop, slow down, speed up, or even advance along a respective trajectory as specified by the respective nominal trajectory. This may be followed, for example, by a restart of the robot's movement, such as restarting the movement from where it was stopped or slowed down, although in some instances may include returning to the beginning of a respective nominal trajectory and restarting from that beginning.

3 shows a method 300 of operation of a processor-based system that performs motion planning to control robots to be operated in a common workspace according to at least one illustrated embodiment. The processor-based system includes one or more processors that execute instructions to determine an acceptable lag time for each robot based on a nominal trajectory of the other robots without considering an effect of the lag time in the nominal trajectory of the other robots, and to generate or select motion plans that specify nominal trajectories that are associated with respective acceptable lag times according to at least one illustrated embodiment. For example, the method 300 may be executed during a configuration or "pre-run" time (i.e., a time when some or all motion planning occurs), such as to generate nominal trajectories and/or motion plans for each of the robots. The configuration or "pre-run" time may occur prior to movement of the robots as dictated by the respective robots' nominal trajectories, for example, prior to a run time, where a run time is a time when one or more robots are executing respective motion plans. This advantageously permits some of the most computationally intensive work to be performed prior to run time, when responsiveness is not a particular concern. In at least some embodiments, method 300 or a portion thereof is performed during a run time (i.e., a time when one or more robots are performing a task) after a configuration time or a pre-run time. In still other embodiments, method 300 is performed during a run time (i.e., a time when one or more robots are performing a task) after a configuration time or a pre-run time.

Method 300 begins at 302, for example, in response to a system or component thereof being started or powered on, receipt of information or data, or being called or invoked by a calling routine or program.

Although not illustrated in FIG. 3 , method 300 may: generate paths; perform collision assessment or detection or analysis on those paths; set, update or adjust costs of edges of the paths; and optionally perform analysis to identify or select a path (e.g., a minimum cost path) based on cost. This is explained above with respect to FIG. 2 , and at least some aspects of at least this are explained in the references cited herein. For the sake of brevity, this is omitted from method 300 .

Optionally, at 304, at least one processor of the processor-based system generates or accesses (e.g., receives, captures) multiple sets of nominal trajectories for each robot 1. As previously described, nominal trajectories are "as specified" trajectories, wherein each trajectory includes an ordered set or sequence of poses, configurations, or states of a robot, the ordered set or sequence extending between two poses, configurations, or states (e.g., a starting pose, configuration, or state; an ending pose, configuration, or state), including a separate timing for each pose or configuration. Timing can be specified in relative terms (timing defined by a relative offset from a previous pose) or absolute terms (timing defined by a relative offset from the start of execution of the trajectory, such as relative to a common clock). Applicants note that while any given trajectory may correspond to a smooth motion of a robot, the term trajectory as used herein is not so limited and will generally specify a motion that is neither smooth nor defines a straight path of a robot or a portion thereof. In at least some instances, a nominal trajectory may specify or include one or more pauses in a motion of a robot or a portion thereof, and/or may specify changes in direction or even reversals in direction, changes in velocity, changes in speed, changes in acceleration in a direction of motion or path of a robot or a portion thereof, and may not be a smooth motion in direction, speed, velocity, or time. For example, a nominal trajectory may specify an ordered set or sequence of poses parameterized in time through which a robot or a portion thereof moves to perform a task or a portion of a task. Any given task may be performed using one nominal trajectory or more than one nominal trajectory. As described herein, the actual motion or actual trajectory of a robot or a portion thereof may deviate from a respective nominal trajectory, for example due to unexpected delays in transitioning between poses (e.g., due to a need to linger or stay at (e.g., over) a target object longer than expected, for example).

Nominal trajectories may be generated using any of a variety of techniques and/or algorithms, such as a sample-based motion planner (SBMP), such as a probabilistic roadmap (PRM) or rapidly exploring random trees (RRT, RTT*), stable sparse RRT* (SST*), and/or fast matching trees (FMT). In at least one embodiment, the nominal trajectory may be generated by the systems, methods, and techniques described in international patent application PCT/US2021/013610, having publication number WO2021150439A1. The teachings of this patent application are not limited to a specific form of nominal trajectory generation. Advantageously, in any combination or arrangement of such aspects, the teachings herein include: computationally efficiently using nominal trajectories to generate a motion plan having an associated acceptable hysteresis time; monitoring deviations of actual motion from the nominal trajectory based on the hysteresis time; taking remedial action for deviations exceeding respective acceptable hysteresis times, as appropriate; and/or using the determined hysteresis times to select a set of nominal trajectories for the motion plan to enhance robustness of the operation, as appropriate.

At 306, at least one processor of the processor-based system initializes a robot counter I, e.g., setting the robot counter equal to an integer value of 1. At 308, at least one processor of the processor-based system executes an outer iteration loop, thereby performing an iteration for each of two or more robots to be operated in a common workspace, or until a stop condition is reached (e.g., determining a maximum lag time that ensures no self-collision motion).

At 310, at least one processor of the processor-based system initializes a nominal trajectory counter J, for example, setting the nominal trajectory counter equal to an integer value of 1.

At 312, at least one processor of the processor-based system executes an inner iterative loop, performing one iteration for each of the nominal trajectories of a given robot, at least until a stop condition is reached.

At 314, at least one processor of the processor-based system determines a respective acceptable hysteresis time (i.e., current in an outer iterative loop) of a current nominal trajectory (i.e., current in an inner iterative loop) of the current robot. The respective acceptable hysteresis time reflects a maximum acceptable delay or hysteresis in the current nominal trajectory J that, when the current robot executes the nominal trajectory, is introduced into the nominal trajectory of the current robot while still ensuring self-collision-free motion of the current robot relative to other robots, each of which moves according to the respective nominal trajectory. The acceptable hysteresis time of a given robot I guarantees self-collision freedom as long as all other robots are operating on their respective nominal trajectories (i.e., with hysteresis time = 0). In other words, as long as a respective current actual hysteresis time of a given robot is less than or equal to (i.e., not greater than) the respective acceptable hysteresis time of that robot and the other robots are operating according to their nominal trajectories, the robots will not collide with each other (i.e., self-collision freedom between the robots is guaranteed).

At 316, at least one processor of the processor-based system determines whether each of the nominal trajectories for the given robot has been considered, for example, thereby determining whether the nominal trajectory counter J is equal to the total number of nominal trajectories for the given robot (e.g., J=M?). If each of the nominal trajectories for the given robot has not been considered, control transfers to 318, where the nominal trajectory counter is incremented (e.g., J=J+1), and then control returns to 312 to consider the next nominal trajectory for the given robot. If each of the nominal trajectories for the given robot has been considered (e.g., J=M), control transfers directly to 320.

Optionally, at 320, at least one processor of the processor-based system selects a maximum value of a set of determined acceptable lag times determined for a given robot.

At 322, at least one processor of the processor-based system provides one or more determined acceptable hysteresis times (e.g., provides a selected maximum of the determined acceptable hysteresis times) for a given robot for use in determining a motion plan for at least the given robot and/or controlling operation of at least the given robot. For example, this may include providing the determined acceptable hysteresis times to a different processor, or transmitting the determined acceptable hysteresis times to a different register of the processor.

At 324, at least one processor of the processor-based system determines whether each of the robots has been considered, e.g., thereby determining whether the robot counter I is equal to the total number of robots (e.g., I=N?). If each of the robots has not been considered (e.g., I<N), control passes to 326, where the at least one processor of the processor-based system iterates the robot counter (e.g., I=I+1), and then control returns to 308 to consider the next robot. If each of the robots has been processed (e.g., I=N), control passes directly to 328.

At 328, at least one processor of the processor-based system generates or selects a separate motion plan for each robot. The motion plan may be or may represent a nominal trajectory for the robot associated with a maximum value in acceptable lag time. The motion plan may be or may represent the nominal trajectory for one or more robots. This method may advantageously enhance the robustness of the motion plan generated by the motion planner and thereby improve the operation of the robot executing the resulting motion plan.

At 330, at least one processor of the processor-based system provides the respective motion plan to each robot or a motion controller so that the robots move according to the respective motion plan.

At 332, method 300 may terminate, for example, until called again. Although method 300 is described according to an ordered flow, in many implementations, various actions or operations may be performed simultaneously or in parallel, and/or may include additional actions and/or omit certain actions.

4 shows a method 400 of operation of a processor-based system that performs motion planning to control robots to be operated in a common workspace according to at least one illustrated embodiment. The processor-based system includes one or more processors that execute processor-executable instructions to determine an acceptable lag time for each robot based on a nominal trajectory of the other robots, while always considering an effect of the lag time in the nominal trajectory of the other robots, and to generate or select motion plans that specify nominal trajectories that are associated with respective acceptable lag times according to at least one illustrated embodiment. For example, the method 400 may be executed during a configuration or "pre-run" time (i.e., a time when some or all motion planning occurs), such as to generate nominal trajectories and/or motion plans for each of the robots. The configuration or "pre-run" time may occur prior to movement of the robots as dictated by the respective robots' nominal trajectories, for example, prior to a run time, where a run time is a time when one or more robots are executing respective motion plans. This advantageously permits some of the most computationally intensive work to be performed prior to run time, when responsiveness is not a particular concern. In at least some embodiments, method 400 or a portion thereof is performed during a run time (i.e., a time when one or more robots are performing a task) after a configuration time or a pre-run time. In still other embodiments, method 400 is performed during a run time (i.e., a time when one or more robots are performing a task) after a configuration time or a pre-run time.

Method 400 begins at 402, for example, in response to a system or component thereof being started or powered on, receipt of information or data, or being called or invoked by a calling routine or program.

Although not illustrated in FIG. 4 , method 400 may: generate paths; perform collision assessment or detection or analysis on those paths; set, update or adjust costs of edges of the paths; and optionally perform analysis to identify or select a path (e.g., a minimum cost path) based on cost. This is explained above with reference to FIG. 2 , and at least some aspects of this are explained in the references cited herein. For the sake of brevity, this is omitted from method 400 .

Optionally, at 404, at least one processor of the processor-based system generates or accesses (e.g., receives, captures) multiple sets of nominal trajectories for each robot (i). As previously described, nominal trajectories are "as specified" trajectories, where each trajectory includes an ordered set or sequence of poses, configurations, or states of a robot between two poses, configurations, or states, together with a respective timing for each pose or configuration. The timing can be specified in relative terms (timing defined by a relative offset from a previous pose) or absolute terms (timing defined by a relative offset from the start of execution of the trajectory, such as relative to a common clock). Applicants note that while any given trajectory may correspond to a smooth motion of a robot, the term trajectory as used herein is not so limited and generally specifies a motion that is neither smooth nor defines a straight path of a robot or a portion thereof. In at least some instances, a nominal trajectory may specify or include one or more pauses in a motion of a robot or a portion thereof, and/or may specify changes in direction or even reversals in direction, changes in velocity, changes in speed, changes in acceleration in a direction of motion or path of a robot or a portion thereof, and may not be a smooth motion in direction, speed, velocity, or time. For example, a nominal trajectory may specify an ordered set or sequence of poses parameterized in time through which a robot or a portion thereof moves to perform a task or a portion of a task. Any given task may be performed using one nominal trajectory or more than one nominal trajectory. As described herein, the actual motion or actual trajectory of a robot or a portion thereof may deviate from a respective nominal trajectory, for example due to undesirable delays in transitioning between poses (e.g., due to a need to linger or stay longer than expected at (e.g., over) a target object).

Nominal trajectories may be generated using any of a variety of techniques and/or algorithms, such as a sample-based motion planner (SBMP), such as a probabilistic roadmap (PRM) or rapidly exploring random trees (RRT, RTT*), stable sparse RRT* (SST*), and/or fast matching trees (FMT). In at least one embodiment, the nominal trajectory may be generated by the systems, methods, and techniques described in international patent application PCT/US2021/013610, having publication number WO2021150439A1. The teachings of this patent application are not limited to a specific form of nominal trajectory generation. Advantageously, in any combination or arrangement of such aspects, the teachings herein include: computationally efficiently using nominal trajectories to generate a motion plan having an associated acceptable hysteresis time; monitoring deviations of actual motion from the nominal trajectory based on the hysteresis time; taking remedial action for a deviation that exceeds a respective acceptable hysteresis time, as appropriate; and/or using the determined hysteresis time to select a set of nominal trajectories for the motion plan to enhance the robustness of the operation, as appropriate.

At 406, at least one processor of the processor-based system initializes a robot counter I, e.g., setting the robot counter equal to an integer value of 1. At 408, at least one processor of the processor-based system executes an outer iteration loop, thereby performing an iteration for each of two or more robots to be operated in a common workspace, or until a stopping condition is reached (e.g., determining a maximum lag time that ensures no self-collision motion among the robots).

At 410, at least one processor of the processor-based system initializes a nominal trajectory counter J, for example, setting the nominal trajectory counter equal to an integer value of 1. At 412, at least one processor of the processor-based system executes an inner iteration loop, performing one iteration for each of the nominal trajectories of a given robot, at least until a stop condition is reached.

At 414, at least one processor of the processor-based system determines a respective acceptable hysteresis time of a current nominal trajectory (i.e., current in an inner iterative loop) of a current robot (i.e., current in an outer iterative loop). The respective acceptable hysteresis time reflects a maximum acceptable delay or hysteresis in the current nominal trajectory J that, when the current robot executes the nominal trajectory, still ensures self-collision-free motion of the current robot relative to other robots when a corresponding acceptable hysteresis time is introduced into the nominal trajectory of the current robot, wherein each robot moves according to the respective nominal trajectory with the respective acceptable hysteresis time introduced. The acceptable hysteresis time of a given robot I is guaranteed to be self-collision free as long as the current actual hysteresis time of all other robots is less than their own respective acceptable hysteresis time. That is, the other robots do not have to operate on their respective nominal trajectories (that is, where hysteresis time = 0). In other words, as long as one of the respective current actual hysteresis times of each of the robots among all robots operating in the common workspace is less than or equal to (that is, not greater than) the robot's respective acceptable hysteresis time, the robots will not collide with each other or with themselves (that is, self-collision freedom between robots is guaranteed).

At 416, at least one processor of the processor-based system determines whether each of the nominal trajectories for the given robot has been considered, e.g., thereby determining whether the nominal trajectory counter J is equal to the total number of nominal trajectories for the given robot (e.g., J=M?). If each of the nominal trajectories for the given robot has not been considered, control passes to 418, where the nominal trajectory counter is incremented (e.g., J=J+1), and then control returns to 412 to consider the next nominal trajectory for the given robot. If each of the nominal trajectories for the given robot has been considered (e.g., J=M), control passes directly to 420.

Optionally, at 420, at least one processor of the processor-based system selects a maximum value of a set of determined acceptable lag times determined for a given robot. At 422, at least one processor of the processor-based system provides one or more determined acceptable lag times (e.g., the selected maximum value of the determined acceptable lag times) for the given robot for use in determining a motion plan for at least the given robot and/or controlling operation of at least the given robot. For example, this may include providing the determined acceptable lag times to a different processor, or transmitting the determined acceptable lag times to a different register of the processor.

At 424, at least one processor of the processor-based system determines whether each of the robots has been considered, e.g., thereby determining whether the robot counter I is equal to the total number of robots (e.g., I=N?). If each of the robots has not been considered (e.g., I<N), control passes to 426, where the at least one processor of the processor-based system iterates the robot counter (e.g., I=I+1), and then control returns to 408 to consider the next robot. If each of the robots has been processed (e.g., I=N), control passes directly to 428.

At 428, at least one processor of the processor-based system generates or selects a separate motion plan for each robot. The motion plan can be or can represent a nominal trajectory of the robot associated with a maximum value in acceptable lag time. The motion plan can be or can represent the nominal trajectory of one or more robots. This method can advantageously enhance the robustness of the motion plan generated by performing motion planning, and thereby improve the operation of the robot executing the resulting motion plan.

At 430, at least one processor of the processor-based system provides the respective motion plan to each robot or a motion controller so that the robots move according to the respective motion plan.

At 432, method 400 may terminate, for example, until called again. Although method 400 is described according to an ordered flow, in many implementations, various actions or operations may be performed simultaneously or in parallel, and/or may include additional actions and/or omit certain actions.

As described herein, to determine acceptable hysteresis times, a processor-based system may perform collision assessment to determine which of a set of candidate hysteresis times will result in or may result in a collision. One method of collision assessment is to perform collision assessment using a sweep volume of a robot. A sweep volume represents a volume swept by a robot or a portion thereof while moving along a given trajectory. The sweep volume is defined by the geometry and kinematics of a given robot and a start and end position defined by a given trajectory. Notably, the sweep volume itself is not affected by the specific time at which the motion defined by the trajectory is performed. Thus, the swept volume can be advantageously determined during a configuration or pre-runtime period before the robot executes or performs a task. Thus, a processor-based system can generate a swept volume for each of a number (e.g., a plurality) of paths from which the robot's trajectory will be generated for each of the robots to be operated in a common workspace. A processor-based system can use the swept volume to perform collision assessment. For example, for each robot of two or more robots, a processor-based system may perform a collision assessment between: i) at least a portion of a respective sample trajectory representing a respective nominal trajectory of the robot, wherein at least one respective hysteresis time is introduced; and ii) at least a portion of a respective sample of each of the respective trajectories of each of the other robots of the two or more robots, wherein at least one respective hysteresis time is introduced in the respective trajectory of each of the other robots of the two or more robots. In response to determining that a collision will occur or will likely occur (e.g., expressed as a probability or likelihood) between the robot and at least one of the other robots of the two or more robots, the processor-based system may identify as respective acceptable hysteresis times a respective hysteresis time for one or more nominal trajectories of the robot to be less than a respective hysteresis time that would result in a determination that a collision will occur. The acceptable hysteresis time may represent a maximum acceptable delay in the timing of the poses from the nominal trajectory that still ensures that the movement of the robot through the multiple sequences of poses specified by the nominal trajectory will remain collision-free (no self-collision of the robot system) relative to the movement of the other robots of the two or more robots. One non-limiting method of using the scan volume to determine acceptable lag time is described below in FIGS. 5 and 6 .

FIG5 shows a method 500 of operation of a processor-based system to generate a swept volume for one or more trajectories for each of a number of robots to be operated in a common workspace in accordance with at least one illustrated embodiment. According to at least one illustrated embodiment, the processor-based system includes one or more processors, whose execution processors can execute instructions to generate the swept volume. For example, method 500 can be executed during a configuration or "pre-run" time (i.e., a time when some or all of motion planning can occur), such as generating nominal trajectories and/or motion plans for each of the robots. For example, configuration or "pre-run" time may be prior to run time, where run time is a time when one or more robots are executing respective motion plans. This advantageously permits some of the most computationally intensive work to be performed prior to run time, when responsiveness is not a particular concern. In at least some embodiments, the robot configuration method 500 or a portion thereof is performed during a run time (i.e., a time when one or more robots are executing tasks) after a configuration time or pre-run time. Optionally, configuration-time operations may be performed via a processor-based system that is different from a processor-based system that performs run-time operations. Method 500 may, for example, be used to perform the methods of FIGS. 3 , 4 , and/or 6 , as appropriate, such as as part of performing collision detection (see method 600 , FIG. 6 ) when determining respective acceptable hysteresis times 314 ( FIG. 3 ) and 414 ( FIG. 4 ).

Method 500 of generating a swept volume begins at 502, for example, in response to a system or component thereof being started or powered on, receipt of information or data, or by a call or invocation of a call routine or program.

At 504, at least one processor of the processor-based system initializes a robot counter I (e.g., I=1). At 506, at least one processor of the processor-based system executes an external robot processing loop. The external robot processing loop allows at least one processor of the processor-based system to perform sweep volume generation for each of the robots to be operated in a common workspace.

At 508, at least one processor of the processor-based system initializes a trajectory counter J (e.g., J=1). At 510, at least one processor of the processor-based system executes an inner nominal trajectory processing loop. The inner nominal trajectory processing loop is nested within the outer robot processing loop. The inner nominal trajectory processing loop allows the at least one processor of the processor-based system to generate a respective sweep volume for each of one or more nominal trajectories for a given robot.

At 512, at least one processor of the processor-based system generates a swept volume representation representing a volume swept by at least a portion of a given robot I moving between poses in a set or sequence of poses defined by a given nominal trajectory J. Thus, for each of two or more robots, and for each of one or more nominal trajectories, the processor-based system may generate a swept volume representation representing a volume swept by at least a portion of the robot while moving between a set or sequence of poses defined by the nominal trajectory from at least one time in the nominal trajectory to another time in the nominal trajectory. The swept volume may be generated using any of a variety of techniques, including digitally representing a robot or a portion thereof using one or more (e.g., hierarchical) data structures or point clouds, and projecting the digital representation along a path or trajectory to generate a digital representation of the swept volume (e.g., a set of voxels).

At 514, at least one processor of the processor-based system determines whether all trajectories for a given robot have been processed (e.g., J=M?). If all trajectories for a given robot have not been processed, control passes to 516, where a trajectory counter is incremented (e.g., J=J+1), and control then returns to the top of the inner nominal trajectory processing loop 510. If all nominal trajectories have been processed (e.g., J=M), control passes directly to 518.

At 518, at least one processor of the processor-based system determines whether all robots have been processed (e.g., I=N?). If all robots have not been processed, control passes to 520, where the robot counter is incremented (e.g., I=I+1), and then control returns to the top of the outer robot processing loop 506. If all robots have been processed (e.g., I=N), control passes directly to 522.

At 522, method 500 may terminate, for example, until called again. Although method 500 is described according to an ordered flow, in many implementations, various actions or operations may be performed simultaneously or in parallel, and/or may include additional actions and/or may omit certain actions.

FIG6 shows a method 600 of operation of a processor-based system according to at least one illustrated embodiment, the processor-based system performing collision assessment for a number of robots and for a number of nominal trajectories for each of the robots. The method 600 may be used to perform collision assessment to identify feasible paths between a start node and an end node in a motion planning graph. Additionally or alternatively, the method 600 may be used to perform collision assessment to determine respective acceptable lag times for one or more nominal trajectories of robots operating in a common workspace. The processor-based system includes one or more processors that execute instructions to perform collision assessment, such as using a sweep volume, according to at least one illustrated embodiment. A swept volume represents or models a volume swept by a robot or a portion thereof, for example, while transitioning between various poses, configurations, or states, while transitioning between a start node and an end node along a path represented in a motion planning graph, or, for example, while moving along a nominal trajectory between a start pose, configuration, or state to an end pose, configuration, or state. Method 600 may, for example, advantageously use a swept volume generated via method 500. For example, method 600 may be executed during a configuration or "pre-run" time (i.e., a time when some or all of motion planning may occur), such as to generate nominal trajectories and/or motion plans for each of the robots. For example, the configuration or "pre-run" time may be before a run time, where run time is a time when one or more robots are executing respective motion plans. This advantageously allows some of the most computationally intensive work to be performed before run time, when responsiveness is not a particular concern. In at least some embodiments, the robot configuration method 600 or a portion thereof is executed during a run time (i.e., a time when one or more robots are executing a task) after a configuration time or pre-run time.

Method 600 of determining acceptable lag time begins at 602, for example, in response to a system or component thereof being started or powered on, receipt of information or data, or being called or invoked by a calling routine or program.

At 604, at least one processor of the processor-based system starts an external robot pair collision evaluation loop. The external robot pair collision evaluation loop allows at least one processor of the processor-based system to evaluate or assess a probability or likelihood of a collision between robots in each pair of robots that will operate in a common workspace. The external robot pair collision evaluation loop can process each combination or arrangement of multiple pairs of robots for potential collision.

At 606, at least one processor of the processor-based system initializes a trajectory counter J (e.g., J=1). At 608, at least one processor of the processor-based system executes an inner nominal trajectory collision evaluation loop. The inner nominal trajectory collision evaluation loop is nested within the outer robot collision evaluation loop. The inner nominal trajectory collision evaluation loop allows the at least one processor of the processor-based system to evaluate or assess a probability or likelihood of a collision for each of one or more nominal trajectories of a given pair of robots.

At 610, at least one processor of the processor-based system initializes a hysteresis counter K (e.g., K=1). At 612, at least one processor of the processor-based system executes a nested inner hysteresis collision evaluation loop. The nested inner hysteresis collision evaluation loop is nested within the inner nominal trajectory collision evaluation loop. The nested inner hysteresis collision evaluation loop allows at least one processor of the processor-based system to evaluate or assess an effect that each of a number of possible hysteresis times (i.e., candidate hysteresis times from a set of candidate hysteresis times) has when introduced into a given nominal trajectory. Thus, for example, a nominal trajectory may be evaluated with a hysteresis equal to zero and with a number (one, two or more) of non-zero hysteresis times introduced to determine the probability or likelihood of a collision occurring if a trajectory corresponding to the nominal trajectory and with the respective hysteresis times introduced is executed. Hysteresis times of increasing duration may be evaluated.

At 614, at least one processor of the processor-based system performs collision assessment using the resulting swept volumes corresponding to the trajectory when hysteresis is introduced (e.g., for zero hysteresis and non-zero hysteresis). The at least one processor of the processor-based system can, for example, determine whether respective swept volumes intersect, wherein the respective swept volumes correspond to respective volumes swept by a robot of a pair of robots when executing the nominal trajectory when a hysteresis is equal to zero and/or when each of a number of non-zero hysteresis times is introduced. For example, for each robot of two or more robots, a processor-based system may perform a collision assessment between: i) at least a portion of a respective sample trajectory representing a respective nominal trajectory of the robot, in which at least one respective hysteresis time is introduced (e.g., a zero hysteresis time, and at least one non-zero hysteresis time); and ii) at least a portion of a respective sample of each of the respective trajectories of each of the other robots of the two or more robots, in which at least one respective hysteresis time is introduced in the respective trajectory of each of the other robots of the two or more robots (e.g., a zero hysteresis time, and at least one non-zero hysteresis time). Thus, all permutations of "candidate" hysteresis times for each of the nominal trajectories and each of the robots may be evaluated, for example to identify when a probability or likelihood of a collision equals or exceeds a certain collision detection threshold.

Although collision assessment is described in terms of sweep volume, the teachings herein are not necessarily limited thereto, and the methods and systems may employ a variety of other collision assessment or detection techniques or algorithms.

At 616, at least one processor of the processor-based system determines whether a given pair of robots will or is likely to cause a collision based on the collision evaluation. In at least some embodiments, the collision evaluation can produce a non-binary value representing a chance or probability of a collision. The determination of whether a collision will or is likely to cause a collision can be based on the non-binary value being above a defined collision detection threshold (e.g., collision probability >50%; >10% >5%). In response to a collision being detected or possibly occurring (e.g., a collision probability equal to or greater than a collision detection threshold), at 617, the processor-based system may identify and/or store previous hysteresis values that did not result in a potential collision and, if appropriate, exit the nested inner hysteresis collision evaluation loop 612, thereby returning control to the top of the inner nominal trajectory collision evaluation loop 608. Thus, for example, in response to a determination that a collision will or may occur between the robot and at least one of the other robots of the two or more robots, the processor-based system identifies, for one or more nominal trajectories of at least one of the other robots of the two or more robots in which it has been determined that a collision will occur, a respective hysteresis time that is less than a respective hysteresis time that would result in a determination that a collision will occur as a respective acceptable hysteresis time, wherein the acceptable hysteresis time reflects a maximum acceptable delay in timing relative to a pose of the nominal trajectory that still ensures that movement of the robot through the sequence of poses specified by the respective nominal trajectory will remain collision-free relative to movement of the other robots of the two or more robots. In response to a determination that a collision has been detected or may have occurred (eg, the collision probability is equal to or greater than the collision detection threshold), control transfers directly to 618 .

At 618, at least one processor of the processor-based system determines whether all hysteresis times for the current trajectory of the current pair of robots have been considered (e.g., K=P?). If all hysteresis times have not been considered, then at 620, a hysteresis time counter K is incremented (e.g., K=K+1) to select the next candidate hysteresis time for evaluation from a set of candidate hysteresis times, and control returns to the top of the nested inner hysteresis time collision evaluation loop 612. If all hysteresis times have been considered, control passes directly to 622.

At 622, at least one processor of the processor-based system determines whether all trajectories for a given pair of robots have been processed (e.g., J=M?). If all trajectories for a given pair of robots have not been processed, then at 624, a trajectory counter is incremented (e.g., J=J+1) and control returns to the top of the inner nominal trajectory collision evaluation loop 608. If all nominal trajectories have been processed (e.g., J=M), then control passes to 626.

At 626, at least one processor of the processor-based system determines whether additional pairs of robots remain to be processed. The method 600 may process each permutation of pairs of robots, each permutation of trajectories for those pairs of robots, and each permutation of candidate lag times, at least until a stopping condition is reached (e.g., an unacceptably high probability of collision is found). Thus, the determined acceptable lag time (e.g., maximum lag time) for any given robot may be a function of the respective selected trajectories of each of the other robots operating in a common workspace and the determined acceptable lag times associated with those selected trajectories. In this sense, the term maximum hysteresis does not necessarily mean an absolute maximum hysteresis for a given robot in isolation, but rather may mean a maximum hysteresis for a given robot in view of the trajectories and associated hysteresis of other robots operating in the shared workspace. If additional pairs of robots remain to be processed, control passes to 628 where a new pair of robots is selected and control then passes to the top of the external robot pair collision evaluation loop. If additional pairs of robots do not remain to be processed, control passes directly to 630.

At 630, at least one processor of the processor-based system returns respective acceptable lags for one or more nominal trajectories. The acceptable lags may be stored, for example, in a non-transitory processor-readable medium, such as in one or more data structures associated with the nominal trajectories. Control then passes to 632.

At 632, method 600 may terminate, for example, until called again. Although method 600 is described according to an ordered flow, in many implementations, various actions or operations may be performed simultaneously or in parallel, and/or may include additional actions and/or omit certain actions.

To determine a respective acceptable hysteresis time for the nominal trajectory, the processor-based system may, for example: iterate through each of a plurality of candidate hysteresis times in a sequence from a relatively small candidate hysteresis time to a relatively large candidate hysteresis time, at least until a stopping condition is met; iterate through each of a plurality of times covered by the nominal trajectory; and check for a collision between one of the two or more robots and at least one other of the two or more robots based on a current one of the candidate hysteresis times. In response to a determination that a collision will or may occur between one of the two or more robots and at least one of the other robots of the two or more robots, the processor-based system may set respective acceptable hysteresis times of nominal trajectories of the robot for which a collision is determined to occur and at least one of the other robots of the two or more robots to a closest previous candidate hysteresis time (e.g., a hysteresis time for which a probability of collision is less than a collision detection threshold indicating an acceptable risk of collision). To check for a collision, the processor-based system may, for example, determine whether a swept volume of one of the robots in a pair of robots intersects a swept volume of the other of the robots in the pair of robots. For example, a processor-based system may generate an indication of a collision in response to a determination that a swept volume of one of a pair of robots intersects a swept volume of the other of the pair of robots and an identification of the robots determined to be in collision.

An exemplary method for determining and applying acceptable hysteresis time to provide a safety margin is described as follows.

First, a motion plan including a set of nominal trajectories (Trj(t)) is generated, for example using the optimization described in international patent application PCT/US2021/013610 with publication number WO2021150439A1. Therefore, for each mobile robot r in a motion plan, an acceptable lag time relative to the motion plan (e.g., a maximum safe lag value (max_lag_r)) is calculated. It is important to note the following: (i) for each robot r, the acceptable lag time value (max_lag) may be different (e.g., max_lag_r1 ≠ max_lag_r2); (ii) the acceptable lag time value (max_lag) is strictly dependent on a set of nominal trajectories Trj(t) and the relative position of the robot r; and (iii) static obstacles and static robots (if any) do not affect the acceptable lag time value (max_lag). In this example, a set of nominal trajectories Trj(t) and the acceptable lag time value (max_lag_r) are calculated sequentially, and preferably offline during configuration time before run time; however, this is not intended to limit the methods described herein. During execution (e.g., on-line; during run-time) a central clock (t) is implemented which is common to all robots operating in a common workspace. For each robot r, the lag (lag_r) relative to the nominal motion plan is calculated at a high frequency, i.e., monitoring of the actual lag time relative to a movement speed of the robots is performed at a sufficiently high frequency to avoid undesired actual collisions.

In the ideal case, each robot r is preferably synchronized with the motion plan and the actual lag of the robot is zero (lag_r=0). However, in the general case, one or more robots may fall behind the schedule, where a current robot state is given by the trajectory trj_r(t-nΔT) and the actual lag lag_r=nΔT.

The processor-based system monitors the actual hysteresis values to ensure the safety of the robotic system. If all actual hysteresis values are within the safety margin specified by the acceptable hysteresis time (e.g., lag_r ≤ max_lag_r), it is considered safe to continue execution because the no-self-collision condition is ensured. Otherwise, the no-self-collision condition is not ensured, and therefore the safety of the system is considered to be compromised.

In order to keep the robot always in safe operation, the system can monitor the actual hysteresis value and simply stop the movement of the entire robot system when a safety margin is crossed (e.g., a margin or threshold value is exceeded). To safely restart the operation, the robot can move to any (safe) state of a set of nominal trajectories Trj(t*) and the central clock is reset to t*.

Less destructive solutions may include taking remedial or corrective action on robots while they are still within the safety margin but relatively close to the limit. For example, a movement of a robot approaching the limit may be accelerated (increased in speed) in order to reduce the value of its actual lag time. This action will directly increase the distance to the limit and improve the safety level. Alternatively, the other robots may be slowed down. This action may utilize a time dilation of the central clock to maintain consistency. Hybrid actions may also be very effective.

An exemplary processor-executable algorithm for determining the safety margin is described in the pseudo code below.

For a better understanding, the algorithm can be divided into 4 components: •  main_loop: It steps through candidate hysteresis values from 0 to L and iterates the nominal trajectory Trj(t). •  CheckLagCollision(): It evaluates the effect of candidate hysteresis values applied at a specific time t. Given a set of candidate hysteresis values for each of the robots, it determines if any robot is in collision at a given time t. In a world without hysteresis, a robot occupies a volume determined by its current pose. Now, with hysteresis, a robot can sweep a volume based on its trajectory at time t-lag up to time t-lag. •  ComputeSweptVolume_r(t1, t2): computes the volume swept by robot r while moving along one segment of the trajectory from time t1 to time t2 (trj_r(t1 … t2)). •  VolumesIntersect(): geometrically searches for the intersection between two volumes.

main_loop() and CheckLagCollision() are presented in detail below. ComputeSweptVolume_r() and VolumesIntersect() are not described in detail because there are many algorithms that can be used to implement such algorithms.

The input includes: trj[1…N]: a set of collision-free trajectories of N robots; ΔT: sample time; T: maximum duration of robot activity; ΔL: sample hysteresis time value for search; and L: maximum hysteresis value for search (L ＜= T).

The output includes: max_lag[1…N], the maximum lag value of each robot, which ensures that there is no self-collision movement among the robots.

Internal variables include: lag_candidate: the lag value evaluated in the current iteration; lag[1…N]: the lag value evaluated by each robot in the current iteration; lag_prev[1…N]: the lag value evaluated by each robot in the previous iteration; and found_max_lag[1…N]: the Boolean value at which each robot stops searching. main_loop(trj[], T, ΔT, L, ΔL) -> max_lag[1..N] 1.      lag[1…N] = 0 2.      lag_prev[1…N] = 0 3.      found_max_lag[1…N] = false 4.      FOR lag_candidate in [0, L]//for all possible lag candidates 1. (for r in [1,N])    IF found_max_lag[r] == false 1.  lag[r] = lag_candidate 2. t = 0 3. FOR t in [0, T]   // for all possible trajectory times 1.  IF CheckLagCollision(trj[], t, lag[]) == true 1.     FOR x in robots_in_collision[] 1.  found_max_lag[x] = true 2.  lag[x] = lag_prev[x] 2.     FOR r in [1,N])   IF found_max_lag[r] == true 1.  GOTO 4.4 2.    t = t + ΔT 4.  (for r in [1,N])   lag_prev[r] = lag[r] 5.  lag_candidate = lag_candidate + ΔL 5.      RETURN lag_prev[] CheckLagCollision(trj[],t,lag[])->[true/false,robots_in_collision[]] 1.      swept_volume[1…N] = none 2.      r = 1 3.      robots_in_collision[] = NULL 4.      bool any_collision = FALSE 5.      (for r in [1,N]) 1. t_min = max(0, t - lag[r]) 2. swept_volume[r] = ComputeSweptVolume_r(t_min, t) 6.      FOR r1 in [1,N], r2 in [1,N]  // r1 != r2 1. IF VolumesIntersect(swept_volume[r1],swept_volume[r2]) == true i.   robots_in_collision[].add(r1 and r2) 7.      RETURN [any_collision, robots_in_collision[]]

FIG. 7 shows a method 700 of operation of a processor-based system to generate or select motion plans for one or more robots to be operated in a common workspace based at least in part on acceptable lag time and, as appropriate, at least in part on other criteria, according to at least one illustrated embodiment. The lag time and, as appropriate, other criteria may be expressed as a cost or cost function. For example, the method 700 may be executed during a configuration or "pre-run" time (i.e., a time when some or all of the motion planning may occur), e.g., to generate nominal trajectories and/or motion plans for each of the robots. For example, a configuration or "pre-run" time may be prior to a run time, which is a time when one or more robots are executing respective motion plans. This advantageously permits some of the most computationally intensive work to be performed prior to the run time, when responsiveness is not a particular concern. In at least some embodiments, method 700 or a portion thereof is performed during a run time (i.e., a time when one or more robots are executing a task) after a configuration time or pre-run time.

Method 700 of determining acceptable lag time begins at 702, such as in response to a system or component thereof being started or powered on, receipt of information or data, or by a call or invocation of a routine or program.

Optionally, at 704, at least one processor of the processor-based system generates or accesses (e.g., receives, captures) a nominal trajectory for each robot. As previously explained, a nominal trajectory is an "as specified" trajectory, wherein each trajectory includes an ordered sequence of poses or configurations of a robot together with a respective timing for each pose or configuration. As also explained herein, actual motion or actual trajectory of a robot or a portion thereof may deviate from a respective nominal trajectory, such as due to undesirable delays in transitioning between poses (e.g., due to, for example, a longer stay or dwell at (e.g., over) a target object than would otherwise be expected).

As previously explained, any of a variety of techniques and/or algorithms may be employed to generate nominal trajectories, such as a sampling-based motion planner (SBMP), such as a probabilistic roadmap (PRM) or rapidly exploring random trees (RRT, RTT*), stable sparse RRT* (SST*), and/or fast matching trees (FMT). Advantageously, the teachings herein include computationally efficient use of determined hysteresis times to select a set of nominal trajectories for motion planning, which enhances robustness of operation.

At 706, at least one processor of the processor-based system initializes a robot counter I, e.g., setting the robot counter equal to an integer value of 1. At 708, at least one processor of the processor-based system executes an outer robot processing iteration loop, thereby performing an iteration for each of two or more robots to be operated in a common workspace, or until a stop condition is reached (e.g., determining a maximum lag time to ensure collision-free motion).

At 710, at least one processor of the processor-based system initializes a nominal trajectory counter J, for example, thereby setting the nominal trajectory counter equal to an integer value of 1. At 712, at least one processor of the processor-based system executes an inner nominal trajectory processing iterative loop, thereby performing one iteration for each of the nominal trajectories of a given robot. The inner nominal trajectory processing iterative loop 712 is nested in the outer robot processing iterative loop 708.

At 714, at least one processor of the processor-based system determines a respective acceptable hysteresis time of the current nominal trajectory J of the current robot I. The respective acceptable hysteresis time reflects a maximum acceptable delay or hysteresis in the current nominal trajectory J that still ensures self-collision-free movement of the current robot I relative to other robots that are each moving according to the respective nominal trajectory when at least the current robot I (the current robot of the external robot processing iteration loop) executes the current nominal trajectory J (the current nominal trajectory of the internal current nominal trajectory iteration loop), wherein the corresponding acceptable hysteresis time is introduced into the current nominal trajectory J of the current robot I. This may preferably be evaluated relative to the nominal trajectories of the other robots, where various candidate hysteresis times are introduced (e.g., zero hysteresis times and non-zero hysteresis times), or where even a respective acceptable hysteresis time is introduced if known. An acceptable hysteresis time for a given robot I guarantees self-collision freedom as long as all other robots are operating at or within their respective nominal trajectories (i.e., with hysteresis time = 0), or alternatively are operating at or having their respective nominal trajectories, where the respective hysteresis times are introduced into the nominal trajectories of the other robots. Therefore, in at least one embodiment, as long as one of the respective current actual lag times of a given robot is less than the respective acceptable lag time of that robot, and the other robots are operating according to their nominal trajectories, the robots will not collide with each other (ensuring self-collision freedom between robots). Therefore, in at least one preferred embodiment, as long as one of the respective current actual lag times of all robots is less than the respective acceptable lag time of each of the robots, the robots will not collide with each other (ensuring self-collision freedom between robots).

At 716, at least one processor of the processor-based system determines whether each of the nominal trajectories for the given robot has been considered, for example, thereby determining whether the nominal trajectory counter J is equal to the total number of nominal trajectories for the given robot (e.g., J=M?). If each of the nominal trajectories for the given robot has not been considered, control transfers to 718, where the nominal trajectory counter is incremented (e.g., J=J+1), and control then returns to the top of the inner nominal trajectory processing iteration loop 712 to consider the next nominal trajectory for the given robot I. If each of the nominal trajectories for the given robot I has been considered (e.g., J=M), control transfers directly to 720.

At 720, at least one processor of the processor-based system selects a maximum value of a set of determined acceptable lag times to be determined for a given robot 1. At 722, at least one processor of the processor-based system provides one or more determined acceptable lag times for the given robot (e.g., provides the selected maximum value of the determined acceptable lag times) for use in determining a motion plan for at least the given robot. For example, this may include providing the determined acceptable lag times to a different processor, or transmitting the determined acceptable lag times to a different register of the processor, or otherwise storing them in a non-transitory processor-readable medium.

At 724, at least one processor of the processor-based system determines whether each of the robots has been considered, for example, thereby determining whether the robot counter I is equal to the total number of robots (e.g., I=N?). If each of the robots has not been considered (e.g., I<N), control passes to 726, where the at least one processor of the processor-based system iterates the robot counter (e.g., I=I+1), and then control returns to the top of the outer iterative robot processing loop 708 to consider the next robot. If each of the robots has been processed (e.g., I=N), control passes directly to 728.

At 728, at least one processor of the processor-based system generates or selects a respective motion plan for each robot. The motion plan may be or may represent a respective nominal trajectory for the robot associated with a maximum value in acceptable lag time. This may advantageously enhance the robustness of the motion plan generated by the motion planner and thereby improve the operation of the robot executing the resulting motion plan.

When considering multiple trajectories per robot, the determined acceptable hysteresis time (e.g., maximum hysteresis time) for any given robot can be a function of the respective selected trajectories of each of the other robots operating in a common workspace and the determined acceptable hysteresis times associated with those selected trajectories. Thus, as described herein, in at least some embodiments, the system determines an acceptable hysteresis time for each robot and each candidate trajectory of that robot, given every other possible combination of candidate trajectories and candidate hysteresis times for all other robots. Once completed, the system (e.g., nominal trajectory analyzer 258 of FIG. 2 ) can select a set of trajectories for all robots operating in the common workspace. The system (e.g., nominal trajectory analyzer 258 of FIG. 2 ) may segment the set of trajectories by, for example, optimizing an objective function (e.g., the maximum sum of all hysteresis times across all robots).

At 730, at least one processor of the processor-based system provides a respective motion plan to each robot or each motion controller so that the robot moves according to the respective motion plan. For example, this may include providing the motion plan to a respective motion controller of each of the robots.

At 732, method 700 may terminate, for example, until called again. Although method 700 is described according to an ordered flow, in many implementations, various actions or operations may be performed simultaneously or in parallel, and/or may include additional actions and/or omit certain actions.

Thus, for example, for each of two or more robots, and for each of two or more nominal trajectories for a respective one of the two or more robots, a processor-based system may determine a respective acceptable lag time for the nominal trajectory, the acceptable lag time reflecting a maximum acceptable delay in timing relative to the poses of the nominal trajectory that still ensures that a movement of the robot through a sequence of poses specified by the nominal trajectory will remain collision-free relative to a movement of each of the other robots of the two or more robots through a respective sequence of poses specified by the respective nominal trajectory because it is itself delayed by a respective acceptable lag time for the nominal trajectory of each of the other robots of the two or more robots. The processor-based system may, for example: select between two or more nominal trajectories based at least in part on respective acceptable hysteresis times of the two or more nominal trajectories; and provide a motion plan for the respective robot to control operation of the respective robot of the two or more robots based at least in part on the selected one of the nominal trajectories. The processor-based system may, for example, select the nominal trajectory of the two or more nominal trajectories of the respective robot that has the largest one of the acceptable hysteresis times, thereby generating a more robust motion plan than would otherwise be generated. For example, a processor-based system may select a nominal trajectory based on a respective cost function representing acceptable lag time and, optionally, a risk or probability of collision, each cost function being associated with a respective one of two or more nominal trajectories for a respective robot. A processor-based system may, for example, select a nominal trajectory based on a respective cost function representing acceptable lag time, a risk or probability of collision, and, optionally, a severity of collision, each cost function being associated with a respective one of two or more nominal trajectories for a respective robot. A processor-based system may select a nominal trajectory, for example, based on a respective cost function representing an acceptable lag time, a risk or probability of a collision, a severity of a collision, and, as the case may be, at least one of a duration of completion or an expenditure of energy, each cost function being associated with a respective one of two or more nominal trajectories of a respective robot, wherein each variable in the cost function representing, respectively, at least one of the acceptable lag time, the risk or probability of a collision, the severity of a collision, and the duration of completion or the expenditure of energy is weighted in the respective cost function. A processor-based system may, for example, select a respective nominal trajectory for each of two or more robots that maximizes an acceptable lag time in an aggregate across the robots of the two or more robots. A processor-based system may, for example, select a respective nominal trajectory for each of two or more robots that optimizes a respective cost function for each of the robots in an aggregate across all of the two or more robots, wherein each cost function represents an acceptable lag time and at least one risk or probability of collision, each cost function being associated with a respective nominal trajectory of a respective one of the two or more nominal trajectories of the respective robot. A processor-based system may, for example, select a respective nominal trajectory for each of two or more robots that optimizes a respective cost function for each of the robots in an aggregate across all of the two or more robots, wherein each cost function represents an acceptable lag time, at least a risk or probability of a collision, and a severity of the collision, each cost function being associated with a respective nominal trajectory of a respective one of the two or more nominal trajectories of the respective robot. A processor-based system may, for example, select a respective nominal trajectory for each of two or more robots that optimizes a respective cost function for each of the robots in an aggregate across all of the two or more robots, wherein each cost function represents at least one of an acceptable lag time, at least a risk or probability of collision, a severity of collision, and a duration or energy expenditure to complete, and each cost function is associated with a respective nominal trajectory of a respective one of the two or more nominal trajectories of the respective robot, wherein each variable in the cost function that represents at least one of the acceptable lag time, the risk or probability of collision, the severity of collision, and the duration or energy expenditure to complete is weighted in the respective cost function.

A processor-based system may, for example, determine a respective acceptable hysteresis time for a nominal trajectory, the acceptable hysteresis time reflecting a maximum acceptable delay in timing relative to a pose of the nominal trajectory, the maximum acceptable delay still ensuring that self-collision-free movement between two or more robots in a common workspace through one of a sequence of poses specified by the nominal trajectory will remain, which may include performing a collision assessment on each of the two or more robots.

A processor-based system may, for example, perform collision assessment between: i) at least a portion of a respective sample trajectory representing a respective nominal trajectory of the robot, into which at least one respective hysteresis time is introduced; and ii) at least a portion of a respective sample of each of the respective trajectories of each of the other of two or more robots, into which at least one respective hysteresis time is introduced.

FIG8 shows a method 800 of operation of a processor-based system to control operation of a robot operating in a shared workspace based at least in part on acceptable lag time according to at least one illustrated embodiment. According to at least one illustrated embodiment, the processor-based system includes one or more processors that monitor actual lag time compared to acceptable lag time and take remedial action if the actual lag time is outside a critical value. Method 800 may, for example, follow execution of method 300 ( FIG3 ), method 400 ( FIG4 ), method 500 ( FIG5 ), method 600 ( FIG6 ), and/or method 700 ( FIG7 ). The method 800 may be executed, for example, during a run time, i.e., during a time when one or more robots are executing respective motion plans and/or are executing tasks. The run time may, for example, follow a configuration or "pre-run" time during which some or all of the motion planning may occur, such as to generate a nominal trajectory and/or motion plan for each of the robots.

Method 800 begins at 802, for example, in response to a system or component thereof being started or powered on, receipt of information or data, or by a call or invocation of a routine or program.

Optionally, at 804, at least one processor of the processor-based system accesses (e.g., receives, retrieves) a respective motion plan for each robot. The respective motion plans may be stored and accessed from one or more non-transitory processor-readable media.

At 806, at least one processor of the processor-based system instructs one or more robots to execute respective motion plans. For example, at least one processor may provide instructions to one or more motion controllers of one or more robots. The motion controller provides control signals to one or more actuators (e.g., electric motors, solenoids, valves, pumps) that are coupled to drive various linkage mechanisms of the robot, thereby causing a robot or a portion thereof to move.

At 808, at least one processor of the processor-based system monitors an amount of lag time (actual lag time) of a respective actual trajectory compared to a corresponding nominal trajectory as the actual trajectory is executed by a respective one of the robots. As previously explained, the actual trajectory is an actual sequence of poses and the timing of those poses as executed by the respective robot. While in some instances the actual trajectory may match the nominal trajectory, in many instances the actual trajectory will not match the nominal trajectory, typically where a timing of execution of at least some of the poses of the actual trajectory of one or more of the robots lags behind the timing of those poses as specified by the respective nominal trajectories of the respective robots. Monitoring may be performed in any of a variety of ways. For example, a processor-based system may use input sensed via one or more sensors positioned to sense the position and/or movement and/or pose or configuration or state of the robots in a shared workspace. The entire workspace may be monitored by sensors, such as in the form of cameras, video cameras, stereo cameras, motion detectors, etc., positioned and having a field of view that encapsulates all or at least a portion of the shared workspace. Additionally or alternatively, the position and/or movement and/or posture or configuration or state of a specific robot may be monitored by sensors, such as any one or more of the following: cameras, video cameras, motion detectors, position encoders or rotation encoders, Hall effect sensors and/or Reed switches associated with one or more joints, linkages and/or actuators of the robot. One or more processors may perform processing (e.g., machine vision processing) to monitor the actual trajectory. Additionally or alternatively, the processor-based system may use information from a robot control system and/or drive system (e.g., motor controller, pneumatic controller, hydraulic controller, etc.) representing control signals used to drive the robot or portions thereof (e.g., PWM motor control signals), and/or use feedback signals received from the robot or the robot's drive system (e.g., feedback EMF). One or more processors may perform processing (e.g., machine vision processing) to determine respective actual lag times of actual trajectories.

At 810, at least one processor of the processor-based system determines whether the amount of monitored lag time (actual lag time) exceeds a margin or threshold (e.g., a respective determined acceptable lag time; a percentage of a respective determined acceptable lag time) for a respective trajectory of any of the robots operating in a common workspace.

In response to a determination that a monitored lag time (i.e., actual lag time) exceeds a respective margin or threshold (e.g., a determined acceptable lag time; a percentage of the respective determined acceptable lag time) for a respective trajectory of a respective robot of any robot, at 812, at least one processor of the processor-based system selects and/or takes one or more remedial actions, as appropriate.

For example, to take at least one remedial action, when executed by at least one processor of the processor-based system, the processor may execute instructions that may cause the processor to perform one or more of the following: stop a movement of one or more robots; slow down a movement of one or more robots; and/or speed up a movement of one or more robots. For example, at least one processor may stop the movement of one, two, more, or even all robots. For another example, at least one processor may slow down the movement of one, two, more, or even all robots. For another example, at least one processor may speed up the movement of one, two, more, or even all robots. For another example, at least one processor may stop the movement of one or more robots while slowing down the movement of one or more robots. For another example, at least one processor may stop the movement of one or more robots while accelerating or keeping the movement of one or more robots constant. For another example, at least one processor may slow down the movement of one or more robots while accelerating or keeping the movement of one or more robots constant. For another example, at least one processor may slow down the movement of one or more robots while also stopping the movement of one or more robots while accelerating or keeping the movement of one or more other robots constant. The term "keeping the movement constant" means that the nominal trajectory of the robot does not change, but the rate of movement (i.e., speed and direction) of the robot may change, which is therefore specified by the nominal trajectory. For another example, to take at least one remedial action, when executed by at least one processor of the processor-based system, the processor may execute instructions that may cause the processor to perform one or more of the following: stop the movement of one or more robots; cause one or more robots to move along a respective trajectory as specified by the respective nominal trajectory; followed by a restart of one of the movements of the robots. Restarting a move will typically involve restarting the move from where it was stopped, but in some instances may include returning to the beginning of a respective trajectory and restarting from that beginning.

Optionally, at 814, at least one processor of the processor-based system monitors an amount of lag time (actual lag time) of a respective actual trajectory compared to a corresponding nominal trajectory as the actual trajectory is executed by a respective one of the robots.

Optionally, at 816, for each of the robots operating in the shared workspace, at least one processor of the processor-based system determines whether the monitored lag time (i.e., actual lag time) amount no longer exceeds a respective margin or threshold value (e.g., determined acceptable lag time; percentage of the respective determined acceptable lag time) for the respective trajectory of the respective robot. The system may enter a waiting loop to continue monitoring the lag time (i.e., actual lag time) amount until it no longer exceeds the respective margin or threshold value position (e.g., determined acceptable lag time; percentage of the respective determined acceptable lag time) for all robots. Once this condition is met, control is transferred to 818.

Optionally, at 818, at least one processor of the processor-based system causes one or more of the robots to continue executing a respective motion plan (e.g., resume movement).

The method 800 terminates at 820, for example, until it is called again. Although the method 800 is described according to an ordered flow, in many implementations, various actions or operations will be performed simultaneously or in parallel, and/or may include additional actions and/or omit certain actions.

The foregoing detailed description has been presented by using block diagrams, schematics, and examples to describe various embodiments of devices and/or programs. As long as such block diagrams, schematics, and examples contain one or more functions and/or operations, those skilled in the art will understand that each function and/or operation within such block diagrams, flow charts, or examples may be implemented individually and/or collectively by a wide range of hardware, software, firmware, or any combination thereof. In one embodiment, the subject matter of the present invention may be implemented via a Boolean circuit, an application specific integrated circuit (ASIC), and/or an FPGA. However, those skilled in the art will recognize that, in various implementations, the embodiments disclosed herein may be implemented in whole or in part in standard integrated circuits 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 any combination thereof, and that in view of the present invention, designing circuit systems and/or writing code for the software and/or firmware will be well within the skills of those skilled in the art.

Those skilled in the art will recognize that many of the methods or algorithms described herein may utilize additional actions, may omit certain actions, and/or may perform the actions in a different order than that specified.

Additionally, those skilled in the art will appreciate that the mechanisms taught herein can be implemented in hardware, such as in one or more FPGAs or ASICs.

The various embodiments described above can be combined to provide further embodiments. All commonly assigned U.S. patent application publications, U.S. patent applications, foreign patents, and foreign patent applications mentioned in this specification and/or listed in the Application Data Sheet are incorporated herein by reference in their entirety, including but not limited to: International Patent Application No. PCT/US2017/036880 filed on June 9, 2017; International Patent Application Publication No. WO2016/122840 filed on January 5, 2016; U.S. Patent Application No. 62/616,783 filed on January 12, 2018; U.S. Patent Application No. 62/626,939 filed on June 3, 2019; U.S. Patent Application No. 62/856,548 filed on June 3, 2019; U.S. Patent Application No. 62/865,431 filed on June 24, 2019; U.S. Patent Application No. 62/964,405 filed on January 22, 2020; U.S. Patent Application No. 63/327,917 filed on April 6, 2022; International Patent Application No. PCT/US2021/013610 with Publication No. WO2021150439A1. These and other changes may be made to the embodiments in light of the description detailed above. 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 claims, but should be construed to include all possible embodiments together with the full range of equivalents granted by this claim. Therefore, the claims are not limited by the present disclosure.

100: robot system/robot control system 102a: robot/given robot 102b: robot/given robot 102c: robot/given robot 103: robot appendage 104: shared workspace 105: base 106: conveyor 108: multi-robot configuration optimization system/selected multi-robot configuration optimization system/processor-based multi-robot configuration optimization system 109: input/provided input 110: swarm generator 111: output 112: multi-robot environment simulator 114: multi-robot optimization engine 115: acceptable lag time estimator 116: candidate solution/given candidate solution 118: robot control system 120: motion planner/selected motion planner 200a: robot control system/first robot control system 200b: robot control system/other robot control systems 202: robot/first robot/given robot 204a: motion planner/first motion planner 204b: motion planner/other motion planners 206a: motion plan/first motion plan 206b: motion plan/other motion plan 208: motion plan graph 209: motion completion message 210: communication channel 211: swept volume representation/selected swept volume representation 212: source 214: Node 215: Task specification 216: Edge 218a: Actuator 218b: Actuator 218c: Actuator 220: Motion controller 222: Processor 224a: System memory 224b: Disk drive 226: Read-only memory 227: Interface 228: Random access memory 230: Flash memory 232: Basic input/output system 234: System bus 236: Operating system 238: Application 240: Program/module 242: Program data 250: Motion converter 251: Trajectory predictor 252: Path Generator 253: Collision Evaluator 254: Cost Setter 255: Path Analyzer/Optional Path Analyzer 256: Trajectory Generator/Nominal Trajectory Generator 257: Acceptable Lag Time Evaluator 258: Nominal Trajectory Analyzer/Optional Nominal Trajectory Analyzer 259: Field Programmable Gate Array 260: Trimmer 262: Optional Sensor 263: Environment Translator 264: Lag Monitor/Lag Time Monitor 266: Remedial Action Selector/Optional Remedial Action Selector 300: Method 302: Step 304: Step 306: step 308: step 310: step 312: step 314: step 316: step 318: step 320: step 322: step 324: step 326: step 328: step 330: step 332: step 400: method 402: step 404: step 406: step 408: step 410: step 412: step 414: step 416: step 418: step 420: step 422: Step 424: Step 426: Step 428: Step 430: Step 432: Step 500: Method/Robot Configuration Method 502: Step 504: Step 506: Step/External Robot Processing Loop 508: Step 510: Step/Internal Nominal Path Processing Loop 512: Step 514: Step 516: Step 518: Step 520: Step 522: Step 600: Method/Robot Configuration Method 602: Step 604: Step 606: step 608: step/internal nominal trajectory collision evaluation loop 610: step 612: step/nested internal hysteresis time collision evaluation loop 614: step 616: step 617: step 618: step 620: step 622: step 624: step 626: step 628: step 630: step 632: step 700: method 702: step 704: step 706: step 708: Step/External Iterative Robot Processing Loop/External Robot Processing Iterative Loop 710: Step 712: Step/Internal Nominal Trajectory Processing Iterative Loop 714: Step 716: Step 718: Step 720: Step 722: Step 724: Step 726: Step 728: Step 730: Step 732: Step 800: Method 802: Step 804: Step 806: Step 808: Step 810: Step 812: Step 814: Step 816: Step 818: Step 820: Step X: Orthogonal axis Y: Orthogonal axis Z: Orthogonal axis

In the drawings, the same reference numbers identify similar elements or actions. The size and relative position of the 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 are arbitrarily enlarged and positioned to improve the legibility of the drawings. In addition, the specific shapes of the elements as drawn are not intended to indicate any information about the actual shape of the specific elements, and are selected solely for the purpose of easy identification in the drawings. Figure 1 is a schematic diagram of a shared workspace in which multiple robots operate to perform tasks and a configuration optimization system that performs an optimization to configure the robots according to an illustrated embodiment. FIG. 2 is a functional block diagram of at least one first robot and a robot control system communicatively coupled to control operation of the first robot according to at least another illustrated embodiment, wherein the robot control system includes a motion planner that advantageously determines and employs acceptable lag times for respective nominal trajectories of the robots in motion planning and optionally monitors actual operation of at least the first robot, compares actual lag time to a margin or threshold value (e.g., acceptable lag time), and optionally takes remedial action when warranted. FIG. 3 illustrates a method for a processor-based system to perform motion planning to control the operation of robots to be operated in a common workspace according to at least one illustrated embodiment, wherein an acceptable lag time for each robot is determined based on a nominal trajectory of the other robots without having to consider an effect of lag time in the nominal trajectory of the other robots, and a motion plan is generated or selected that specifies nominal trajectories that are associated with respective acceptable lag times. FIG. 4 shows a method of a processor-based system performing motion planning to control the operation of robots to be operated in a common workspace according to at least one illustrated embodiment, wherein an acceptable lag time for each robot is determined based on the nominal trajectories of the other robots while considering an effect of lag time in the nominal trajectories of the other robots as necessary, and motion plans are generated or selected that specify nominal trajectories that are associated with respective acceptable lag times. FIG. 5 shows a method of operation of a processor-based system according to at least one illustrated embodiment to generate a swept volume for one or more trajectories for each of a number of robots operating in a common workspace, the processor-based system being operable to perform the methods of FIG. 3 , FIG. 4 , and/or FIG. 6 , as appropriate. FIG. 6 shows a method of operation of a processor-based system according to at least one illustrated embodiment to perform collision assessment for a number of robots and a number of trajectories for each of the robots, wherein movement of the robots or portions thereof along the trajectories is represented as swept volumes, and collision assessment can be used to determine acceptable lag times for one or more robots operating in a common workspace. FIG. 7 shows a method of operation of a processor-based system according to at least one illustrated embodiment to generate or select motion plans for one or more robots to be operated in a shared workspace based at least in part on acceptable lag time and, as appropriate, at least in part on other criteria represented, for example, as a cost or cost function. FIG. 8 shows a method of operation of a processor-based system according to at least one illustrated embodiment to control the operation of robots operating in a shared workspace based at least in part on acceptable lag time.

100:Robot system/Robot control system

102a:Robot/Given robot

102b:Robot/given robot

102c:Robot/given robot

103: Robotic Appendages

104: Shared workspace

105: Base

106: Conveyor machine

108: Multi-robot configuration optimization system/selection of multi-robot configuration optimization system/processor-based multi-robot configuration optimization system

109: Input/Input provided

110: Group Generator

111: Output

112:Multi-robot environment simulator

114:Multi-robot optimization engine

115: Acceptable lag time estimator

116: Candidate solution/Given candidate solution

118:Robot control system

120: Movement Planner/Select Movement Planner

X: orthogonal axis

Y: orthogonal axis

Z: orthogonal axis

Claims (60)

A method operating in a processor-based system to cause a plurality of robots to operate in a multi-robot operating environment in which the plurality of robots are to operate, the method comprising: for each of two or more robots, for each of one or more nominal trajectories for the respective robot of the two or more robots, the nominal trajectories specifying a respective sequence of poses of the respective robot and the timing of the poses, Determining a respective acceptable hysteresis time for the nominal trajectory, the respective acceptable hysteresis time reflecting a maximum acceptable delay of the timing of the poses relative to the nominal trajectory, the maximum acceptable delay still ensuring that a movement of the robot through the sequences of poses specified by the nominal trajectory will remain collision-free relative to a movement of each of the other robots of the two or more robots through the respective sequences of poses specified by the respective nominal trajectory; and Providing the acceptable hysteresis time to at least one processor to control the operation of each of the two or more robots. The method of claim 1, wherein determining a respective acceptable lag time of the nominal trajectory comprises: For each of the two or more robots, For each of the one or more nominal trajectories, Generating a swept volume representation representing a volume swept by at least a portion of the robot while moving between a set of said sequences of poses specified by the nominal trajectory from at least one time in the nominal trajectory to another time in the nominal trajectory. The method of claim 2, wherein determining a respective acceptable lag time of the nominal trajectory further comprises: Using the generated sweep volumes to perform a collision assessment. The method of claim 3, wherein using the generated swept volumes to perform collision assessment comprises: for each of the two or more robots, performing collision assessment between: i) at least a portion of a respective sample trajectory representing the respective nominal trajectory of the robot, wherein at least one respective hysteresis time is introduced; and ii) at least a portion of a respective sample of each of the respective trajectories of each of the other robots in the two or more robots, wherein at least one respective hysteresis time is introduced in the respective trajectories of each of the other robots in the two or more robots, and In response to determining that a collision will occur between the robot and at least one of the other robots in the two or more robots, a respective hysteresis time that is less than a respective hysteresis time that would cause the determination that a collision will occur is identified as the respective acceptable hysteresis time for the one or more nominal trajectories of the robot, wherein the acceptable hysteresis time reflects the maximum acceptable delay of the timing relative to the poses of the nominal trajectory, the maximum acceptable delay still ensuring that the movement of the robot through the sequence of poses specified by the nominal trajectory will remain collision-free relative to the movement of the other robots in the two or more robots. As in the method of claim 4, determining an acceptable lag time of each of the nominal trajectories further comprises: In response to determining that a collision will occur between the robot and at least one of the other robots of the two or more robots, the one or more nominal trajectories for which it is determined that a collision will occur for the at least one of the other robots of the two or more robots are identified as the respective acceptable hysteresis times by respective hysteresis times that are less than respective hysteresis times that would result in the determination that a collision will occur, wherein the acceptable hysteresis times reflect the maximum acceptable delay of the timing of the poses relative to the nominal trajectories that still ensures that the movement of the robot through the sequences of poses specified by the respective nominal trajectories will remain collision-free relative to the movement of the other robots of the two or more robots. The method of claim 1, wherein determining a respective acceptable hysteresis time of the nominal trajectory further comprises: iterating through each of a plurality of candidate hysteresis times in a sequence from a relatively smaller candidate hysteresis time to a relatively larger candidate hysteresis time, at least until a stop condition is met, iterating through each of a plurality of times covered by the nominal trajectory, checking whether there is a collision between one of the two or more robots and at least one other of the two or more robots based on a current one of the candidate hysteresis times; and In response to determining that a collision will occur between one of the two or more robots and at least one other of the two or more robots, the respective acceptable lag times of the nominal trajectories of the robot and the at least one of the other of the two or more robots where it is determined that a collision will occur are set to a closest previous candidate lag time. The method of claim 6, wherein checking whether there is a collision between the robot and the other robots based on the current one of the waiting and selecting delay times comprises: For each of the two or more robots, performing a collision assessment on at least a portion of the respective nominal trajectory of the robot with the current one of the waiting and selecting delay times or another previously determined delay time relative to at least a portion of each of the respective trajectories of each of the other robots of the two or more robots with the current one of the waiting and selecting delay times. A method as claimed in claim 7, wherein performing a collision assessment on at least a portion of the respective nominal trajectories of the robot with the current preceding one of the waiting and selection delay time or another previously determined hysteresis time relative to at least a portion of each of the respective trajectories of each of the other robots of the two or more robots with the current preceding one of the waiting and selection delay time comprises: performing a collision assessment to determine whether at least a portion of the robot collides with at least a portion of any of the other robots of the two or more robots while the robot and the other robots move along at least that portion of the respective nominal trajectories as delayed by the waiting and selection delay time. The method of claim 6, wherein checking for a collision between the robot and the other robots based on the current one of the waiting delay times comprises: For each of the two or more robots, Generating a swept volume representation representing a volume swept by at least a portion of the robot while moving between a set of the sequences of positions defined by the respective nominal trajectories from at least one time in the respective nominal trajectories to another time in the respective nominal trajectories; and For each pair of the two or more robots, At least before an intersection is detected, determining, for the timing of the positions defined by the nominal trajectories without introducing any hysteresis time, and for the timing of the positions defined by the nominal trajectories when each of a plurality of candidate hysteresis times is introduced into the respective nominal trajectories, whether the swept volumes of one of the robots in the pair intersect with the swept volumes of the other of the robots in the pair, and In response to a determination that the swept volume of one of the robots in the pair intersects the swept volume of the other of the robots in the pair and an identification of the robots determined to be in collision, an indication of a collision is generated. The method of claim 1, further comprising: For each of the two or more robots, For each of one or more actual trajectories as executed by the respective robot, Monitoring a hysteresis between the nominal trajectory and the respective actual trajectory as executed by the respective robot; Determining whether the monitored hysteresis exceeds the respective determined acceptable hysteresis time of the respective actual trajectory of the respective robot; and Taking at least one remedial action in response to the monitored hysteresis exceeding the respective determined acceptable hysteresis time of the respective trajectory of the respective robot. A method as claimed in claim 10, wherein taking at least one remedial action comprises one or more of the following: stopping the movement of one or more of the robots; slowing down the movement of one or more of the robots; speeding up the movement of one or more of the robots. A method as claimed in claim 10, wherein taking at least one remedial action comprises: stopping the movement of the robots; moving one or more of the robots along a respective trajectory as specified by the respective nominal trajectory; and subsequently restarting the movement of the robots. A method as claimed in claim 10, wherein the determination of a respective acceptable lag time of the nominal trajectory occurs during a configuration time period prior to movement of the at least two robots as specified by the one or more nominal trajectories of the respective robots. A method as claimed in claim 13, wherein while the at least two robots are executing movements as specified by the one or more nominal trajectories of the respective robots, the lag between the monitored nominal trajectories and the respective actual trajectories as executed by the respective robots occurs during a run time that follows the configuration time. A method as in claim 10, wherein monitoring the lag between the nominal trajectory and the respective actual trajectory as executed by the respective robots includes using a central clock that is common to a monitor of each of the one or more robots. The method of any one of claims 1 to 15 and any combination thereof, further comprising: Receiving a respective motion plan for each of the robots, each of the motion plans specifying a respective one of the one or more nominal trajectories for the respective robot, the nominal trajectories representing respective collision-free paths. A processor-based system for facilitating the operation of a plurality of robots against a multi-robot operating environment in which the plurality of robots will operate, the processor-based system comprising: At least one processor; and At least one non-transitory processor-readable medium storing processor-executable instructions that, when executed by the at least one processor, cause the at least one processor to perform a method as claimed in any one of claims 1 to 16. A processor-based system for configuring a plurality of robots for a multi-robot operating environment in which the plurality of robots are to operate, the processor-based system comprising: At least one processor; and At least one non-transitory processor-readable medium storing at least one of data and processor-executable instructions, the processor-executable instructions, when executed by the at least one processor, causing the processor to: For each of two or more robots, For each of one or more nominal trajectories for the respective robots of the two or more robots, the nominal trajectories specifying a respective sequence of poses for the respective robots and the timing of the poses, Determining a respective acceptable hysteresis time for the nominal trajectory, the acceptable hysteresis time reflecting a maximum acceptable delay of the timing of the poses relative to the nominal trajectory, the maximum acceptable delay still ensuring that a movement of the robot through the sequences of poses specified by the nominal trajectory will remain collision-free relative to a movement of each of the other robots of the two or more robots through the respective sequences of poses specified by the respective nominal trajectory; and providing the acceptable hysteresis time to at least one processor to control the operation of each of the two or more robots. The processor-based system of claim 18, wherein determining a respective acceptable lag time of the nominal trajectory comprises: For each of the two or more robots, For each of the one or more nominal trajectories, Generating a swept volume representation representing a volume swept by at least a portion of the robot while moving between a set of said sequences of poses specified by the nominal trajectory from at least one time in the nominal trajectory to another time in the nominal trajectory. The processor-based system of claim 19, wherein determining a respective acceptable lag time of the nominal trajectory further comprises: Using the generated sweep volumes to perform collision assessment. A processor-based system as claimed in claim 20, wherein in order to perform collision assessment using the generated swept volumes, the processors may execute instructions that when executed by the at least one processor cause the at least one processor to: for each of the two or more robots, perform collision assessment between: i) at least a portion of a respective sample trajectory representing the respective nominal trajectory of the robot, wherein at least one respective hysteresis time is introduced; and ii) at least a portion of a respective sample of each of the respective trajectories of each of the other robots of the two or more robots, wherein at least one respective hysteresis time is introduced in the respective trajectories of each of the other robots of the two or more robots, and In response to a determination that a collision will occur between the robot and each of the other robots in the two or more robots, identifying, for the one or more nominal trajectories of the robot, a respective hysteresis time that is less than a respective hysteresis time that would cause the determination that a collision will occur as the respective acceptable hysteresis time, wherein the acceptable hysteresis time reflects the maximum acceptable delay of the timing relative to the poses of the nominal trajectory that still ensures that the movement of the robot through the sequences of poses specified by the nominal trajectory will remain collision-free relative to the movement of the other robots in the two or more robots. A processor-based system as claimed in claim 21, wherein in order to determine a respective acceptable lag time of the nominal trajectory, the processors may execute instructions which, when executed by the at least one processor, cause the at least one processor to further: In response to determining that a collision will occur between the robot and at least one of the other robots of the two or more robots, the one or more nominal trajectories for which it is determined that a collision will occur for at least one of the other robots of the two or more robots are identified as respective acceptable hysteresis times that are less than respective hysteresis times that would result in the determination that a collision will occur, wherein the acceptable hysteresis times reflect the maximum acceptable delay of the timing of the poses relative to the nominal trajectories that still ensures that the movement of the robot through the sequences of poses specified by the respective nominal trajectories will remain collision-free relative to the movement of the other robots of the two or more robots. A processor-based system as claimed in claim 18, wherein in order to determine a respective acceptable lag time for the nominal trajectory, the processors may execute instructions that when executed by the at least one processor cause the processor to further: iterate through each of a plurality of candidate lag times in a sequence from a relatively smaller candidate lag time to a relatively larger candidate lag time, at least until a stop condition is met, iterate through each of a plurality of times covered by the nominal trajectory, check for a collision between one of the two or more robots and at least one other of the two or more robots based on a current one of the candidate lag times; and In response to a determination that a collision will occur between one of the two or more robots and at least one of the other robots of the two or more robots, the respective acceptable lag times of the nominal trajectories of the robot and at least one of the other robots of the two or more robots where it is determined that a collision will occur are set to a closest previous candidate lag time. A processor-based system as claimed in claim 23, wherein in order to check for a collision between the robot and the other robots based on the current one of the waiting post-selection delay times, the processors may execute instructions that when executed by the at least one processor cause the processor to: For each of the two or more robots, Perform a collision assessment on at least a portion of the respective nominal trajectory of the robot with the previous or another previously determined delay time in the waiting post-selection delay time relative to at least a portion of each of the respective trajectories of each of the other robots of the two or more robots with the current previous one of the waiting post-selection delay time. The processor-based system of claim 24, wherein a collision detection is performed on at least a portion of the respective nominal trajectory of the robot with the current preceding one of the waiting and selection delay times or another previously determined delay time relative to at least a portion of the respective trajectories of each of the other robots of the two or more robots with the current preceding one of the waiting and selection delay times. Collision assessment, the processors may execute instructions that, when executed by the at least one processor, cause the processor to perform a collision assessment to determine whether at least a portion of the robot collides with at least a portion of any of the other robots while the robot and the other robots among the two or more robots move along at least the portion of the respective nominal trajectories delayed by the waiting delay time. A processor-based system as claimed in claim 23, wherein in order to check for a collision between the robot and the other robots based on the current one of the waiting delay times, the processors may execute instructions that when executed by the at least one processor cause the processor to: For each of the two or more robots, Generate a swept volume representation, the swept volume representation representing a volume swept by at least a portion of the robot while moving between a set of said sequences of poses specified by the respective nominal trajectory from at least one time in the respective nominal trajectory to another time, wherein the current one of the waiting delay times is introduced into the respective nominal trajectory; and For each pair of the two or more robots, At least before detecting an intersection, determining, for the timing of the positions defined by the nominal trajectories without introducing any hysteresis time and for the timing of the positions defined by the nominal trajectories with each of a plurality of candidate hysteresis times introduced into the respective nominal trajectories, whether the swept volumes of one of the robots in the pair intersect with the swept volumes of the other of the robots in the pair, and In response to a determination that the swept volume of one of the robots in the pair intersects with the swept volume of the other of the robots in the pair and an identification of the robots determined to be in collision, generating an indication of a collision. A processor-based system as claimed in claim 18, wherein the processors can execute instructions that when executed by the at least one processor cause the processor to further: For each of the two or more robots, For each of one or more actual trajectories as executed by the respective robot, Monitor a lag between the nominal trajectory and the respective actual trajectory as executed by the respective robot; Determine whether the monitored lag exceeds the respective determined acceptable lag time of the respective actual trajectory of the respective robot; and In response to the monitored lag exceeding the respective determined acceptable lag time of the respective trajectory of the respective robot, at least one remedial action occurs. A processor-based system as in claim 27, wherein, in order to take at least one remedial action, the processors may execute instructions that, when executed by the at least one processor, cause the processor to perform one or more of the following: stop the movement of one or more of the robots; slow down the movement of one or more of the robots; speed up the movement of one or more of the robots. A processor-based system as in claim 27, wherein, in order to take at least one remedial action, the processors may execute instructions that, when executed by the at least one processor, cause the processor to: stop the movement of the robots; cause one or more robots to move along a respective trajectory as specified by the respective nominal trajectory; and subsequently restart one of the movements of the robots. A processor-based system as in claim 27, wherein the determination of a respective acceptable lag time for the nominal trajectory occurs during a configuration time prior to movement of the at least two robots as specified by the one or more nominal trajectories of the respective robots. A processor-based system as in claim 30, wherein while the at least two robots are executing movements as specified by the one or more nominal trajectories of the respective robots, the monitoring of the hysteresis between the nominal trajectories and the respective actual trajectories as executed by the respective robots occurs during a run time that follows the configuration time. A processor-based system as in claim 27, wherein the monitoring of the amount of lag between the nominal trajectory and the respective actual trajectory as executed by the respective robots comprises using a central clock that is common to the monitor of each of the one or more robots. A processor-based system as claimed in any of claims 18 to 32 and any combination thereof, wherein the processors can execute instructions that when executed by the at least one processor cause the processor to further: Receive a respective motion plan for each of the robots, each of the motion plans specifying a respective one of the one or more nominal trajectories for the respective robot, the nominal trajectories representing respective collision-free paths. A method operating in a processor-based system to cause a plurality of robots to operate in a multi-robot operating environment in which the plurality of robots are to operate, the method comprising: for each of two or more robots, for each of two or more nominal trajectories for the respective robots of the two or more robots, the nominal trajectories specifying a respective sequence of poses of the respective robots and timing of the poses, Determining a respective acceptable hysteresis time for the nominal trajectory, the respective acceptable hysteresis time reflecting a maximum acceptable delay of the timing relative to the poses of the nominal trajectory, the maximum acceptable delay still ensuring that a movement of the robot through the sequences of poses specified by the nominal trajectory will remain collision-free relative to a movement of each of the other robots of the two or more robots through the respective sequences of poses as specified by the respective nominal trajectory, the respective nominal trajectory being delayed by a respective acceptable hysteresis time of the nominal trajectory of the at least one other robot of the two or more robots; Selecting between the two or more nominal trajectories based at least in part on the respective acceptable hysteresis times of the two or more nominal trajectories; and Providing a motion plan for the respective robot to control the operation of the respective robot of the two or more robots based at least in part on the selected one of the nominal trajectories. A method as in claim 34, wherein selecting between the two or more nominal trajectories based at least in part on the respective acceptable hysteresis times of the two or more nominal trajectories for each robot includes selecting the nominal trajectory among the two or more nominal trajectories of the respective robot having a maximum of the acceptable hysteresis times. A method as in claim 34, wherein selecting between the two or more nominal trajectories based at least in part on the respective acceptable lag times for the two or more nominal trajectories for each robot includes selecting the nominal trajectory based on a respective cost function representing the acceptable lag time and at least one risk or probability of collision, each cost function being associated with a respective one of the two or more nominal trajectories for the respective robot. A method as in claim 34, wherein selecting between the two or more nominal trajectories based at least in part on the respective acceptable lag times for the two or more nominal trajectories for each robot includes selecting the nominal trajectory based on a respective cost function representing the acceptable lag time, a risk or probability of a collision, and a severity of the collision, each cost function being associated with a respective one of the two or more nominal trajectories for the respective robot. A method as in claim 34, wherein selecting between the two or more nominal trajectories based at least in part on the respective acceptable lag times for the two or more nominal trajectories for each robot comprises selecting the nominal trajectory based on a respective cost function representing at least one of the acceptable lag time, the risk or probability of a collision, the severity of a collision, and the duration or energy expenditure to complete, each cost function being associated with the respective one of the two or more nominal trajectories for the respective robot, wherein each variable in the cost function representing at least one of the acceptable lag time, the risk or probability of a collision, the severity of a collision, and the duration or energy expenditure to complete is weighted in the respective cost function. A method as in claim 34, wherein selecting between the two or more nominal trajectories based at least in part on the respective acceptable hysteresis times for the two or more nominal trajectories includes selecting a respective nominal trajectory for each of the two or more robots that maximizes the acceptable hysteresis times in an aggregate across all of the two or more robots. A method as in claim 34, wherein selecting between the two or more nominal trajectories based at least in part on the respective acceptable lag times for the two or more nominal trajectories for each robot comprises selecting a respective nominal trajectory for each of the two or more robots, the respective nominal trajectory optimizing a respective cost function for each of the two or more robots in an aggregate across all of the robots, wherein each cost function represents the acceptable lag time and at least a risk or probability of collision, each cost function being associated with a respective nominal trajectory in a respective one of the two or more nominal trajectories of the respective robot. A method as in claim 34, wherein selecting between the two or more nominal trajectories based at least in part on the respective acceptable lag times for the two or more nominal trajectories for each robot comprises selecting a respective nominal trajectory for each of the two or more robots, the respective nominal trajectory optimizing a respective cost function for each of the two or more robots in an aggregate across all of the robots, wherein each cost function represents the acceptable lag time, at least a risk or probability of a collision, and a severity of a collision, each cost function being associated with the respective nominal trajectory in a respective one of the two or more nominal trajectories for the respective robot. The method of claim 34, wherein selecting between the two or more nominal trajectories based at least in part on the respective acceptable hysteresis times for the two or more nominal trajectories for each robot comprises selecting a respective nominal trajectory for each of the two or more robots that optimizes a respective cost function for each of the two or more robots in an aggregate across all of the robots, wherein each cost function represents the acceptable hysteresis time. The invention relates to a method for controlling a robot to perform a certain desired lag time, wherein each cost function is associated with a respective one of the two or more nominal trajectories of the respective robot, wherein each variable in the cost function representing at least one of the acceptable lag time, the risk or probability of collision, the severity of collision, and the duration or energy expenditure to complete is weighted in the respective cost function. A method as in claim 34, wherein determining a respective acceptable lag time for the nominal trajectory comprises performing a collision assessment for each of the two or more robots, the respective acceptable lag time reflecting a maximum acceptable delay of the timing of the poses relative to the nominal trajectory, the maximum acceptable delay still ensuring that a movement of the robot through the sequence of poses specified by the nominal trajectory will remain collision-free. The method of claim 43, wherein performing a collision assessment on each of the two or more robots comprises: Performing a collision assessment between: i) at least a portion of a respective sample trajectory representing the respective nominal trajectory of the robot, wherein at least one respective hysteresis time is introduced; and ii) at least a portion of a respective sample of each of the respective trajectories of each of the other robots in the two or more robots, wherein at least one respective hysteresis time is introduced in the respective trajectories of each of the other robots in the two or more robots. The method of claim 44, wherein determining a respective acceptable hysteresis time for the nominal trajectory further comprises: In response to determining that a collision will occur between the robot and at least one of the other robots in the two or more robots, identifying a respective hysteresis time for the one or more nominal trajectories of the robot that is less than a respective hysteresis time that would cause the determination that a collision will occur as the respective acceptable hysteresis time, wherein the acceptable hysteresis time reflects a maximum acceptable delay of the timing relative to the poses of the nominal trajectory, the maximum acceptable delay still ensuring that the movement of the robot through the sequence of poses specified by the nominal trajectory will remain collision-free relative to the movement of the other robots in the two or more robots. As in the method of claim 45, determining an acceptable lag time of each of the nominal trajectories further comprises: In response to determining that a collision will occur between the robot and at least one of the other robots of the two or more robots, the one or more nominal trajectories for which it is determined that a collision will occur for the at least one of the other robots of the two or more robots are identified as the respective acceptable hysteresis times by respective hysteresis times that are less than respective hysteresis times that would result in the determination that a collision will occur, wherein the acceptable hysteresis times reflect the maximum acceptable delay of the timing of the poses relative to the nominal trajectories that still ensures that the movement of the robot through the sequences of poses specified by the respective nominal trajectories will remain collision-free relative to the movement of the other robots of the two or more robots. A processor-based system for facilitating the operation of a plurality of robots against a multi-robot operating environment in which the plurality of robots will operate, the processor-based system comprising: At least one processor; and At least one non-transitory processor-readable medium storing processor-executable instructions that, when executed by the at least one processor, cause the at least one processor to perform a method as claimed in any one of claims 34 to 46. A processor-based system for configuring a plurality of robots for a multi-robot operating environment in which the plurality of robots are to operate, the processor-based system comprising: At least one processor; and At least one non-transitory processor-readable medium storing at least one of data and processor-executable instructions, the processor-executable instructions, when executed by the at least one processor, causing the processor to: For each of two or more robots, For each of the two or more nominal trajectories of the respective robots of the two or more robots, the nominal trajectories specifying a respective sequence of poses of the respective robots and the timing of the poses, Determining a respective acceptable hysteresis time for the nominal trajectory, the respective acceptable hysteresis time reflecting a maximum acceptable delay of the timing relative to the poses of the nominal trajectory, the maximum acceptable delay still ensuring that a movement of the robot through the sequences of poses specified by the nominal trajectory will remain collision-free relative to a movement of each of the other robots of the two or more robots through the respective sequences of poses specified by the respective nominal trajectory, the respective nominal trajectory being delayed by a respective acceptable hysteresis time of the nominal trajectory of the at least one other robot of the two or more robots; Selecting between the two or more nominal trajectories based at least in part on the respective acceptable lag times of the two or more nominal trajectories; and Providing a motion plan for the respective robot to control the operation of the respective robot of the two or more robots based at least in part on the selected one of the nominal trajectories. A processor-based system as in claim 48, wherein in order to select between the two or more nominal trajectories of each robot based at least in part on the respective acceptable lag times of the two or more nominal trajectories, the processors may execute instructions that, when executed, cause at least one of the processors to select the nominal trajectory of the two or more nominal trajectories of the respective robot having the largest of the acceptable lag times. A processor-based system as in claim 48, wherein in order to select between two or more nominal trajectories based at least in part on the respective acceptable lag times for the two or more nominal trajectories for each robot, the processors may execute instructions that, when executed, cause at least one processor to select the nominal trajectory based on a respective cost function representing acceptable lag times and at least one risk or probability of collision, each cost function being associated with a respective one of the two or more nominal trajectories for the respective robot. A processor-based system as in claim 48, wherein in order to select between the two or more nominal trajectories based at least in part on the respective acceptable lag times for the two or more nominal trajectories for each robot, the processors may execute instructions that, when executed, cause the at least one processor to select the nominal trajectory based on a respective cost function representing the acceptable lag time, a risk or probability of a collision, and a severity of the collision, each cost function being associated with the respective one of the two or more nominal trajectories for the respective robot. A processor-based system as in claim 48, wherein in order to select between two or more nominal trajectories based at least in part on the respective acceptable lag times for the two or more nominal trajectories for each robot, the processors may execute instructions that, when executed, cause at least one of the processors to select the nominal trajectory based on a respective cost function representing the acceptable lag time, a risk or probability of collision, a probability of collision, and a probability of collision. A cost function is associated with a respective one of the two or more nominal trajectories of the respective robot, wherein each variable in the cost function representing at least one of the acceptable lag time, the risk or probability of collision, the severity of collision, and the duration or energy expenditure to complete is weighted in the respective cost function. A processor-based system as in claim 48, wherein in order to select between the two or more nominal trajectories based at least in part on the respective acceptable lag times of the two or more nominal trajectories, the processors may execute instructions that, when executed, cause the at least one processor to select a respective nominal trajectory for each of the two or more robots that maximizes the acceptable lag times in an aggregate across all of the two or more robots. A processor-based system as in claim 48, wherein in order to select between two or more nominal trajectories based at least in part on the respective acceptable lag times for the two or more nominal trajectories of each robot, the processors may execute instructions that, when executed, cause the at least one processor to select a respective nominal trajectory for each of the two or more robots, the respective nominal trajectory optimizing a respective cost function for each of the two or more robots in an aggregate across all of the robots, wherein each cost function represents the acceptable lag time and at least a risk or probability of collision, each cost function being associated with the respective nominal trajectory in the respective one of the two or more nominal trajectories of the respective robot. A processor-based system as in claim 48, wherein in order to select between the two or more nominal trajectories based at least in part on the respective acceptable lag times of the two or more nominal trajectories for each robot, the processors may execute instructions that when executed cause the at least one processor to select a respective nominal trajectory for each of the two or more robots, the respective nominal trajectory A respective cost function for each of the two or more robots is optimized in an aggregate across all of the robots, wherein each cost function represents the acceptable lag time, at least a risk or probability of collision, and a severity of collision, each cost function being associated with the respective nominal trajectory of the respective one of the two or more nominal trajectories of the respective robot. A processor-based system as in claim 48, wherein in order to select between two or more nominal trajectories based at least in part on the respective acceptable lag times for the two or more nominal trajectories for each robot, the processors may execute instructions that, when executed, cause the at least one processor to select a respective nominal trajectory for each of the two or more robots that optimizes a respective cost function for each of the two or more robots in an aggregate across all of the robots, wherein Each cost function represents at least one of the acceptable lag time, at least one risk or probability of collision, a severity of collision, and at least one of the duration or energy expenditure to complete, and each cost function is associated with the respective nominal trajectory of the respective one of the two or more nominal trajectories of the respective robot, wherein each variable in the cost function that represents at least one of the acceptable lag time, the risk or probability of collision, the severity of collision, and the duration or energy expenditure to complete is weighted in the respective cost function. A processor-based system as in claim 48, wherein in order to determine a respective acceptable lag time for the nominal trajectory, the respective acceptable lag time reflecting a maximum acceptable delay of the timing of the poses relative to the nominal trajectory, the maximum acceptable delay still ensuring that a movement of the robot through the sequence of poses specified by the nominal trajectory will remain collision-free, the processors may execute instructions that, when executed, cause the at least one processor to perform a collision assessment on each of the two or more robots. A processor-based system as in claim 57, wherein in order to perform a collision assessment for each of the two or more robots, the processors may execute instructions that, when executed, cause the at least one processor to: Perform a collision assessment between: i) at least a portion of a respective sample trajectory representing the respective nominal trajectory of the robot, wherein at least one respective hysteresis time is introduced; and ii) at least a portion of a respective sample of each of the respective trajectories of each of the other robots in the two or more robots, wherein at least one respective hysteresis time is introduced in the respective trajectories of each of the other robots in the two or more robots. A processor-based system as claimed in claim 58, wherein in order to determine a respective acceptable lag time of the nominal trajectory, the processors may execute instructions which, when executed, cause the at least one processor to further: In response to a determination that a collision will occur between the robot and at least one of the other robots of the two or more robots, a respective hysteresis time that is less than a respective hysteresis time that would cause the determination that a collision will occur is identified as the respective acceptable hysteresis time for the one or more nominal trajectories of the robot, wherein the acceptable hysteresis time reflects the maximum acceptable delay of the timing relative to the poses of the nominal trajectory that still ensures that the movement of the robot through the sequence of poses specified by the nominal trajectory will remain collision-free relative to the movement of the other robots of the two or more robots. A processor-based system as claimed in claim 59, wherein in order to determine a respective acceptable lag time of the nominal trajectory, the processors may execute instructions which, when executed, cause the at least one processor to further: In response to a determination that a collision will occur between the robot and at least one of the other robots of the two or more robots, the one or more nominal trajectories for which it is determined that a collision will occur for at least one of the other robots of the two or more robots are identified as respective acceptable hysteresis times that are less than respective hysteresis times that would result in the determination that a collision will occur, wherein the acceptable hysteresis times reflect the maximum acceptable delay of the timing of the poses relative to the nominal trajectories that still ensures that the movement of the robot through the sequences of poses specified by the respective nominal trajectories will remain collision-free relative to the movement of the other robots of the two or more robots.