Patent Description:
Motion planning is a fundamental problem in robot control and robotics. A motion plan specifies a path that a robot can follow from a starting state to a goal state, typically to complete a task without colliding with any obstacles in an operational environment or with a reduced possibility of colliding with any obstacles in the operational environment. Challenges to motion planning involve the ability to perform motion planning at very fast speeds even as characteristics of the environment change. For example, characteristics such as location or orientation of one or more obstacles in the environment may change over time. Challenges further include performing motion planning using relatively low cost equipment, at relative low energy consumption, and with limited amounts of storage (e.g., memory circuits, for instance on processor chip circuitry).

<NPL> discloses an online method for obtaining jerk-bounded trajectories. The method described uses a concatenation of fifth-order polynomials to provide a trajectory between two way points. The trajectory approximates a linear segment with parabolic blends trajectory. A sine wave template is used to calculate the end conditions (control points) for ramps from zero acceleration to nonzero acceleration. The control points are joined with quintic polynomials. The method requires computation of the quintic control points, up to a maximum of eight points per trajectory way point. A method for blending these straight-line trajectories over a series of way points is also discussed.

<NPL> discloses an approach to scheduling or varying feedrate that takes into consideration the geometry of the contour that a machine is expected to follow and the physical capabilities of the machine (i.e., its maximum velocity, acceleration and jerk constraints). The approach introduces additional constraints on the permissible jerk (rate of change of acceleration) on the machine's axis.

It is typical for a robot or portion thereof to move along a path or trajectory, from a start pose or configuration to an end pose or configuration with one or more intermediary poses or configurations therebetween. One problem in robotics is maximizing a velocity of the robot or portion thereof along the path, while maintaining limits on acceleration and minimizing any jerking of the robot or portion thereof resulting from the motion. Such can be posed as an optimization problem, that is to optimize velocity along a geometric path without violating any constraints. The constraints in this context include constraints on velocity, acceleration, and jerk (i.e., the derivative of acceleration with respect to time).

Optimizing velocity while observing a limit on jerking (i.e., a jerk limit) is typically a computationally difficult problem. Conventional approaches employ non-linear optimization methods. These non-linear optimization methods are typically slow to solve, and are prone to getting stuck in non-optimal local minima, resulting in faulty solutions.

Faster, less computational intense, and more robust techniques to optimize velocity of robots or portions thereof without violating constraints on acceleration and jerk are commercially desirable.

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

In the following description, certain specific details are set forth in order to provide a thorough understanding of various disclosed embodiments. However, one skilled in the relevant art will recognize that 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, robots, robotic appendages, actuator systems, and/or communications networks have not been shown or described in detail to avoid unnecessarily obscuring descriptions of the embodiments.

Unless the context requires otherwise, throughout the specification and claims which follow, the word "comprise" and variations thereof, such as, "comprises" and "comprising" are to be construed in an open, inclusive sense, that is as "including, but not limited to.

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

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

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

As previously noted, an important problem in robotics is called "jerk minimization" which may be implemented by determining velocities between various configurations while maintaining limits on acceleration and on jerk (i.e., the derivative of acceleration with respect to time).

A given geometric path or trajectory that a robot or robotic appendage is to follow may, for instance be specified as a configuration vector, that is a vector of points in a configuration or C-space of the robot (e.g., {<NUM>st posn of joints, <NUM>nd posn of joints, , nth posn of joints}. For each geometric path, a time vector is generated with times at which the robot or portion thereof reaches those respective configurations or "positions" (e.g., {t<NUM>, t<NUM>, , tn}) The time vector may have a same length as the configuration vector. A performance optimization problem is presented, that is to go as fast as possible along the geometric path without violating any constraints (e.g., constraints on velocity, acceleration, and jerk (the derivative of acceleration).

Optimizing speed while observing the jerk limit is computationally difficult. Conventional approaches typically employ non-linear optimization methods that are slow and can get stuck in non-optimal local minima. The approach described herein makes optimization with respect to jerk a quasi-convex optimization problem, significantly reducing the computational complexity. A linear optimization approach to optimize speed along a geometric path while staying within an acceleration limit is described herein, first in terms of theoretical basis for simplifying the problem, then in terms of practical implementation.

The geometric path or trajectory has some number of "waypoints" (positions on the path) between a start and an end. Waypoints are denoted as si, as in {s<NUM>, s<NUM>, , sn}. The goal is to find the times (e.g., vector of times or time vector) that correspond to those waypoints {t<NUM>, t<NUM>, , tn}. A configuration of the joints is denoted qi, as in the robot is in a configuration qi at waypoint si. Velocity is denoted as v, acceleration denoted as a, and jerk is the derivative of acceleration with respect to time (da/dt).

Two types of derivatives are used. Derivatives with respect to time (e.g., dZ/dt) are denoted with a dot or multiple dots) on top the variable (e.g., Ż). Derivatives with respect to position on the geometric path (e.g., dZ/ds) are denoted with a prime or multiple primes (e.g., Z').

Start with Equation <NUM> below, where the derivative dq/ds=q' is a change in a configuration as a position on a geometric path changes, and the derivative ds/dt= ṡ is velocity.

Next, take the derivative of q̇ with respect to s in Equation <NUM>, to get: <MAT>.

Substituting a = s̈, and x = ṡ<NUM> into Equation <NUM>,leaves <MAT>.

The optimization goal is to maximize an absolute value of velocity, or more conveniently velocity squared (x)), under the following three constraints:.

The technique advantageously takes advantage of transforming this non-linear optimization problem into a linear optimization problem. To do so, it is assumed that at the end of the geometric path the velocity (ṡ) and acceleration (a) will each equal zero. Velocity is zero once the end of the geometric path is reached, and acceleration can arbitrarily be ensured to also be zero. This means that at an end waypoint sn, v=a=<NUM>.

In implementation, to solve for an estimated maximum velocity while maintaining a limit on acceleration, first a range of feasible velocities at each waypoint can be found, subject to a bound on acceleration (a) or subject to an acceleration limit. This can be accomplished working backwards along the geometric path or trajectory, determining the feasible range at each waypoint along the geometric path successively from at least sn-<NUM> to s<NUM>. The range of feasible velocities may be continuous or may be discrete. Notably, at a waypoint sn-<NUM> (the waypoint just proceeding the final waypoint sn), the acceleration (a) will be something between zero and a small negative number as the robot or portion thereof decelerates towards the final waypoint sn. Since it is known that acceleration (a) is a small negative number, the velocity (v) can be bounded. Now, working forwards along the geometric path or trajectory successively from s<NUM> to sn-<NUM>, a value of (a) is selected at each waypoint si that allows an absolute value of velocity or more conveniently velocity squared (x) to be at least approximately maximized such that the respective value of acceleration is within the corresponding acceleration limit and the corresponding velocity is in the respective range of feasible velocities.

The approach described above is a linear approach for at least approximately maximizing or optimizing velocity while staying under an acceleration limit. The correct choice of variables makes the problem linear. In the acceleration limited case, the constraints and the objective function are intentionally constructed from decision variables that make the problem linear. A linear solver can be employed to solve the problem.

A linear approach is now described for at least approximately maximizing or optimizing velocity while not exceeding a constraint on jerk.

Substituting: x = ṡ<NUM>, a = s̈, <MAT>, leaves: <MAT>.

Notably, an estimate of a maximized velocity = ṡ has already been solved for as described above, by optimizing velocity while staying within the acceleration limit. Due to the linearization of the problem of solving for velocity while staying with the acceleration limit, the resulting estimated velocity is not bounded by jerk or jerk limited, but advantageously serves as a good starting point for optimizing velocity while staying within the jerk limit. Notably, there is a different value of ṡ (velocity) at each waypoint si. The previously determined value of ṡ at the acceleration limit (denoted as ṡa_limit) can advantageously be used for the linearized problem of solving for velocity while staying with the jerk limit, as described below.

A value denoted as "approximated-jerk" is defined as the jerk equation with velocity ṡ replaced by the previously determined value of ṡ at the acceleration limit, that is ṡa_limit. This approximation helps, even though it means that the optimization might not be as optimal as if optimizing for a "true" jerk limit. The equation for approximated-jerk, including the substitutions mentioned above is given as: <MAT>.

Now we optimize by maximizing an absolute value of velocity, or more conveniently velocity squared (i.e., x + 2Δa + Δ<NUM>j ) under three new constraints, as well as constraints <NUM> and <NUM> above:.

The goal is to transform a non-linear optimization problem into a linear one. The choice of the three decision variables ( x = ṡ<NUM>, a = s̈, and <MAT>) almost makes the approximated jerk limit equation linear with respect to the decision variables, except that there is an additional ṡ. In this case, the additional ṡ is held constant by substituting in a previously determined value of ṡ at the acceleration limit (denoted as ṡalimit). This linearizes the problem of solving for velocity while staying with the jerk limit, at the cost of optimality.

The sub-optimality of the problem is directly related to how poor of an approximation ṡa_limit is of ṡ. Notably, the jerk limit is a special form of nonlinearity, known as quasi-convex. This form of non-convexity can easily be handled by a binary search for the value ṡ. This provides two options, depending on whether optimality or planner speed is preferred. In the case where optimality is paramount, for the cost of three (<NUM>) or four (<NUM>) additional iterations of the linear solver, the solution can get within <NUM>-<NUM>% of the optimal solution. The determined solution could be made arbitrarily close to the optimal solution by increasing the accuracy of the ṡ estimate. In the case where planner speed is of utmost importance, the approximated-jerk solution can be used directly, without successive iterations or refinements.

In implementation, to solve for an estimated maximum velocity while maintaining a limit on jerk, first a range of feasible velocities and a range of feasible accelerations can be found, subject to a bound on jerk or subject to a jerk limit. This can be accomplished working backwards along the geometric path or trajectory, determining the feasible ranges at each waypoint along the geometric path successively from at least sn-<NUM> to s<NUM>. The range of feasible velocities and/or the range of feasible accelerations may be continuous or may be discrete. Now, working forwards along the geometric path or trajectory successively from s<NUM> to sn-<NUM>, a value of jerk is selected at each waypoint si that allows an absolute value of velocity or more conveniently velocity squared (x) to be at least approximately maximized or optimized such that the respective value of jerk is within the corresponding jerk limit, the corresponding velocity is in the respective range of feasible velocities, and the corresponding acceleration is in the respective range of feasible accelerations.

<FIG> show a robot <NUM> with a base <NUM> and a movable robotic appendage <NUM>, illustrated respectively at a plurality of sequential configurations or poses along a geometric path or trajectory that the robot or a portion thereof follows, according to one illustrated implementation. In particular, <FIG> shows the robotic appendage <NUM> at a series of sequential configurations or poses, moving from a first or initial configuration or pose (<FIG>) to a final or end configuration or pose (<FIG>), with a number of intermediary configurations or poses (<FIG>) along the geometric path or trajectory. The robot <NUM> is operable to carry out tasks, during which the robot or robotic appendage <NUM> may traverse one or more geometric paths or trajectories.

The base <NUM> of the robot <NUM> may, for example be fixed to a floor or other support, or alternatively may include wheels or treads. As illustrated, the robotic appendage <NUM> may be comprised a number of links 106a, 106b, 106c (three shown, collectively <NUM>), joints 108a, 108b, 108c, 108d (four shown, collectively <NUM>), and optionally an end of arm tool or end effector <NUM>. A first joint 108a may rotatably couple a first link 106a to the base <NUM>, for rotation around a central axis of the base. A second joint 108b may rotatably couple a second link 106b to the first link 106a for rotation about a corresponding axis of rotation. A third joint 108c may rotatably couple a third link 106c to the second link 106b for rotation about a corresponding axis of rotation. A fourth joint 108d may rotatably couple the end of arm tool or end effector <NUM> to the third link 106c for rotation about a corresponding axis of rotation. While not visible in <FIG>, the robot <NUM> typically includes one or more actuators and optionally transmissions, coupled to move the links with respect to the base and/or one another.

Each configuration or pose of the robot <NUM> or robotic appendage <NUM> may be defined by a respective set of configurations or poses of the collection of the joints 108a-108d, for example represented in the configuration space (C-space) of the robot <NUM>. It is noted that the "configuration space" or "C-space" of the robot <NUM> is different than a workspace (i.e., two- or three-dimensional environment) in which the robot <NUM> operates. The workspace may include one or more work items or work pieces (not illustrated) which the robot <NUM> manipulates as part of performing tasks, for example one or more parcels, packaging, fasteners, tools, items or other objects. A geometric path or trajectory may be represented as a sequence of configurations or poses of the robot <NUM> or robotic appendage <NUM>, each configuration or pose in the sequence represented as a respective set of a plurality of configurations or poses of the collection of the joints 108a-108d. The collection of configurations or poses of the robot <NUM> or robotic appendage <NUM> may be provided, received, or represented as a configuration vector.

<FIG> shows an environment in which a robot control system <NUM> is communicatively coupled to control the motion of a robot <NUM> or portion thereof (e.g., robotic appendage <NUM>), according to one illustrated implementation.

The robot control system <NUM> includes a motion planner <NUM>, that generates motion plans which specify geometric paths or trajectories, and also includes an acceleration limited optimizer or solver <NUM> and a jerk limited optimizer or solver <NUM> which are operable to computationally efficiently determine at least approximately optimal trajectories (e.g., robot configurations and timing) to cause a robot <NUM> or robotic appendage <NUM> to move along a geometric path or trajectory at approximately optimal velocities while complying with limits on acceleration and jerk. The geometric path or trajectory may allow the robot <NUM> to perform one or more tasks <NUM>.

The robot control system <NUM> is communicatively coupled to control operation of the robot <NUM> via at least one communications channel (indicated by proximate arrows, e.g., transmitter, receiver, transceiver, radio, router, Ethernet). The robot control system <NUM> may be communicatively coupled to one or more motion controllers <NUM> (e.g., motor controller), which provide drive signals to one or more actuators 220a, 220b, 220c, 220d of the robot <NUM>. The motion controllers <NUM> may be part of the robot <NUM>, or distinct therefrom.

The robot <NUM> can take any of a large variety of forms. Typically, the robots <NUM> will take the form of, or have, one or more robotic appendages <NUM>. The robot <NUM> may include one or more linkages 106a, 106b, 106c (<FIG>) with one or more joints (108a, 108b, 108c, 108d, 108e (<FIG>), and actuators 220a, 220b, 220c, 220d (e.g., electric motors, stepper motors, solenoids, pneumatic actuators or hydraulic actuators) coupled and operable to move the linkages 106a, 106b, 106c in response to control or drive signals. Pneumatic actuators may, for example, include one or more pistons, cylinders, valves, reservoirs of gas, and/or pressure sources (e.g., compressor, blower). Hydraulic actuators may, for example, include one or more pistons, cylinders, valves, reservoirs of fluid (e.g., low compressibility hydraulic fluid), and/or pressure sources (e.g., compressor, blower). The robot <NUM> may employ or take the form of other forms of robots, for example autonomous vehicles.

The robot control system <NUM> may be communicatively coupled, for example via at least one communications channel (indicated by proximate arrows, e.g., transmitter, receiver, transceiver, radio, router, Ethernet), to optionally receive motion planning graphs (not shown) and/or swept volume representations (not shown) from one or more sources <NUM> of motion planning graphs and/or swept volume representations. The source(s) <NUM> of motion planning graphs and/or swept volumes may be separate and distinct from the motion planner according to one illustrated implementation. The source(s) <NUM> of motion planning graphs and/or swept volumes may, for example, be one or more processor-based computing systems (e.g., server computers), which may be operated or controlled by respective manufacturers of the robots <NUM> or by some other entity. The motion planning graphs may each include a set of nodes (not shown) which represent states, configurations or poses of the robot <NUM>, and a set of edges (not shown) which couple nodes of respective pairs of nodes, and which represent legal or valid transitions between the states, configurations or poses. States, configurations or poses may, for example, represent sets of joint positions, orientations, poses, or coordinates for each of the joints of the respective robot <NUM>. Thus, each node may represent a pose of a robot <NUM> or portion thereof as completely defined by the poses of the joints comprising the robot <NUM>. The motion planning graphs may be determined, set up, or defined prior to a runtime (i.e., defined prior to performance of tasks), for example during a pre-runtime or configuration time. The swept volumes represent respective volumes that a robot <NUM> or portion thereof would occupy when executing a motion or transition that corresponds to a respective edge of the motion planning graph. The swept volumes may be represented in any of a variety of forms, for example as voxels, a Euclidean distance field, a hierarchy of geometric objects. This advantageously permits some of the most computationally intensive work to be performed before runtime, when responsiveness is not a particular concern.

The robot control system <NUM> may comprise one or more processor(s) <NUM>, and one or more associated nontransitory computer- or processor-readable storage media for example system memory 224a, disk drives 224b, and/or memory or registers (not shown) of the processors <NUM>. The nontransitory computer- or processor-readable storage media 224a, 224b are communicatively coupled to the processor(s) 222a via one or more communications channels, such as system bus <NUM>. The system bus <NUM> can employ any known bus structures or architectures, including a memory bus with memory controller, a peripheral bus, and/or a local bus. One or more of such components may also, or instead, be in communication with each other via one or more other communications channels, for example, one or more parallel cables, serial cables, or wireless network channels capable of high speed communications, for instance, Universal Serial Bus ("USB") <NUM>, Peripheral Component Interconnect Express (PCIe) or via Thunderbolt®.

As noted above, the robot control system <NUM> may optionally be communicably coupled to one or more remote computer systems, e.g., server computer (e.g. source of motion planning graphs <NUM>), desktop computer, laptop computer, ultraportable computer, tablet computer, smartphone, wearable computer, that are directly communicably coupled or indirectly communicably coupled to the various components of the robot control system <NUM>, for example via a network interface <NUM>. Remote computing systems (e.g., server computer) may be used to program, configure, control or otherwise interface with or input data (e.g., motion planning graphs, swept volumes, task specifications <NUM>) to the robot control system <NUM> and various components within the robot control system <NUM>. Such a connection may be through one or more communications channels, for example, one or more wide area networks (WANs), for instance, Ethernet, or the Internet, using Internet protocols. As noted above, pre-runtime calculations (e.g., generation of the family of motion planning graphs) may be performed by a system that is separate from the robot control system <NUM> or robot <NUM>, while runtime calculations may be performed by the processor(s) <NUM> of the robot control system <NUM>, which in some implementations may be on-board the robot <NUM>.

The robot control system <NUM> may optionally be communicably coupled to one or more sensors, for example one or more cameras <NUM>, motion sensors, rotational encoders, accelerometer, etc..

As noted, the robot control system <NUM> may include one or more processor(s) <NUM>, (i.e., circuitry), nontransitory storage media 224a, 224b, and system bus <NUM> that couples various system components. The processors <NUM> may be any logic processing unit, such as one or more central processing units (CPUs), digital signal processors (DSPs), graphics processing units (GPUs), field programmable gate arrays (FPGAs), application-specific integrated circuits (ASICs), programmable logic controllers (PLCs), etc. The system memory 224a may include read-only memory ("ROM") <NUM>, random access memory ("RAM") <NUM> FLASH memory <NUM>, EEPROM (not shown). A basic input/output system ("BIOS") <NUM>, which can form part of the ROM <NUM>, contains basic routines that help transfer information between elements within the robot control system <NUM>, such as during start-up.

The drive 224b may be, for example, a hard disk drive for reading from and writing to a magnetic disk, a solid state (e.g., flash memory) drive for reading from and writing to solid state memory, and/or an optical disk drive for reading from and writing to removable optical disks. The robot control system 200a may also include any combination of such drives in various different embodiments. The drive 224b may communicate with the processor(s) <NUM> via the system bus <NUM>. The drive(s) 224b may include interfaces or controllers (not shown) coupled between such drives and the system bus <NUM>, as is known by those skilled in the relevant art. The drive 224b and its associated computer-readable media provide nonvolatile storage of computer- or processor readable and/or executable instructions, data structures, program modules and other data for the robot control system <NUM>. Those skilled in the relevant art will appreciate that other types of computer-readable media that can store data accessible by a computer may be employed, such as WORM drives, RAID drives, magnetic cassettes, digital video disks ("DVD"), Bernoulli cartridges, RAMs, ROMs, smart cards, etc..

Executable instructions and data can be stored in the system memory 224a, for example an operating system <NUM>, one or more application programs <NUM>, other programs or modules <NUM> and program data <NUM>. Application programs <NUM> may include processor-executable instructions that cause the processor(s) <NUM> to perform one or more of: generating discretized representations of the environment in which the robot <NUM> will operate, including obstacles and/or target objects or work pieces in the environment where planned motions of other robots may be represented as obstacles; generating motion plans or road maps including calling for or otherwise obtaining results of a collision assessment, setting cost values for edges in a motion planning graph, and evaluating available paths in the motion planning graph; optionally storing the determined plurality of motion plans or road maps, and/or performing optimizations (e.g., linear optimizers). The motion plan construction (e.g., collision detection or assessment, updating costs of edges in motion planning graphs based on collision detection or assessment, and path search or evaluation) can be executed as described in the references.

The collision detection or assessment may perform collision detection or assessment using various structures and techniques described in the references.

Application programs <NUM> may additionally include one or more machine-readable and machine-executable instructions that cause the processor(s) <NUM> to perform other operations, for instance optionally handling perception data (captured via sensors) and/or optimizations. Application programs <NUM> may additionally include one or more machine-executable instructions that cause the processor(s) <NUM> to perform various other methods described herein and in the references.

In various embodiments, one or more of the operations described above may be performed by one or more remote processing devices or computers, which are linked through a communications network (e.g., network <NUM>) via network interface <NUM>.

While shown in <FIG> as being stored in the system memory 224a, the operating system <NUM>, application programs <NUM>, other programs/modules <NUM>, and program data <NUM> can be stored on other nontransitory computer- or processor-readable media, for example drive(s) 224b.

The structure and/or operation of the motion planner <NUM> of the robot control system <NUM> may be as illustrated and described in commonly assigned <CIT>.

The processor(s) <NUM> and/or the motion planner <NUM> may be, or may include, any logic processing units, 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, the Celeron, Core, Core <NUM>, Itanium, and Xeon families of microprocessors offered by Intel® Corporation, U. ; the K8, K10, Bulldozer, and Bobcat series microprocessors offered by Advanced Micro Devices, U. ; the A5, A6, and A7 series microprocessors offered by Apple Computer, U. ; the Snapdragon series microprocessors offered by Qualcomm, Inc. ; and the SPARC series microprocessors offered by Oracle Corp. The construction and operation of the various structure shown in <FIG> may implement or employ structures, techniques and algorithms described in or similar to those described in International Patent Application No. <CIT> entitled "MOTION PLANNING FOR AUTONOMOUS VEHICLES AND RECONFIGURABLE MOTION PLANNING PROCESSORS"; International Patent Application Publication No. <CIT>, entitled "SPECIALIZED ROBOT MOTION PLANNING HARDWARE AND METHODS OF MAKING AND USING SAME"; <CIT>, entitled, "APPARATUS, METHOD AND ARTICLE TO FACILITATE MOTION PLANNING OF AN AUTONOMOUS VEHICLE IN AN ENVIRONMENT HAVING DYNAMIC OBJECTS"; <CIT>, entitled "APPARATUS, METHODS AND ARTICLES TO FACILITATE MOTION PLANNING IN ENVIRONMENTS HAVING DYNAMIC OBSTACLES"; and/or <CIT>, entitled "MOTION PLANNING FOR MULTIPLE ROBOTS IN SHARED WORKSPACE".

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

The motion planner <NUM> may determine a geometric path or trajectory (e.g., optimal or optimized) that specifies a sequence of configurations of the robot <NUM> or robotic appendage <NUM>, for example specified as a configuration vector in the c-space of the robot <NUM>.

The acceleration limited optimizer or solver <NUM> determines an at least approximately optimized velocity for each transition between waypoints si of the geometric path or trajectory while respecting an acceleration limit. As previously noted, converting the optimization problem into a linear form advantageously provides significant increases in computational efficiency and reduces or even eliminates occurrences of getting stuck in local minima. The determined velocity values may advantageously be provided to the jerk limited optimizer or solver <NUM>.

The jerk limited optimizer or solver <NUM> determines an at least approximately optimized velocity for each transition between waypoints si of the geometric path or trajectory while respecting a jerk limit. As previously noted, converting the optimization problem into a linear form advantageously provides significant increases in computational efficiency and reduces or even eliminates occurrences of getting stuck in local minima. The jerk limited optimizer or solver <NUM> may refine the determined velocity values to reach any defined level of optimization.

The processor-based system or a portion thereof may determine times corresponding to each waypoint si at which the robot or portion thereof should be in the corresponding configuration or pose. The processor-based system or a portion thereof may provide the configurations (e.g., as configuration vector) 214a and times (e.g., as time vector) 214b to drive the robot or portion thereof to move along the geometric path or trajectory in an at least approximately optimized fashion.

<FIG> shows a method of operation <NUM> in a processor-based system to computationally efficiently generate at least approximately optimized trajectory information to control a robot or portion thereof, according to at least one illustrated implementation. The method <NUM> may, for example, be executed during a runtime. The method <NUM> may be executed by a processor-based system that is part of a robot and/or robot control system. Alternatively, the method <NUM> may be executed by a processor-based system (e.g., server computer) that is separate and distinct, and possibly remote, from the robot.

As an overview, initially an acceleration limited optimizer produces an initial optimized velocity estimate. That initial optimized velocity estimate is provided to the jerk limited optimizer as an initial estimate or seed for determining a jerk limited velocity estimate. That initial estimate of acceleration limited velocity is not limited by a constraint on jerk. If desired, the subsequently determined jerk limited velocity estimate may be employed after a single iteration. Alternatively, multiple iterations may be performed to refine the jerk limited velocity estimate; each iteration stepping closer to an optimal jerk limited velocity. The optimal value is, of course, not known until reached. In many practical applications an estimated or approximated optimal jerk limited velocity comes close to, but may in fact fall short of, a theoretically optimal value since precise accuracy is generally not required, and may be traded off for how much time the processing takes.

At <NUM>, a processor-based system or portion thereof generates or receives a discretized geometric path for the robot or portion thereof to follow. The discretized geometric path may, for example, take the form of a configuration vector, specifying for each position or waypoint along the geometric path a respective set of joint positions or joint coordinates that place the robot or portion thereof in a corresponding configuration or pose. As illustrated, an acceleration limited optimizer or solver <NUM> (<FIG>) may receive the configuration vector.

At <NUM>, the processor-based system or portion thereof (e.g., acceleration limited optimizer or solver <NUM> (<FIG>)) determines, for each of the waypoints, a range of feasible velocities based on an acceleration limit. Determination of the range of feasible velocities corresponds to the backward pass along the geometric path or trajectory that is performed as part of determining an optimized acceleration limited velocity, described above. The range of feasible velocities may be continuous, or alternatively may be a set of discrete velocities.

At <NUM>, the processor-based system or portion thereof, for each of the waypoints along the geometric path or trajectory, selects an acceleration that maximizes velocity. The selection of the acceleration to maximize velocity corresponds to the forward pass along the geometric path or trajectory that is performed as part of determining an optimized acceleration limited velocity, described above. That is, for example, a value of acceleration may be selected that approximately maximizes a velocity squared such that the respective value of acceleration is within the corresponding acceleration limit and the corresponding velocity is in the respective range of feasible velocities.

At <NUM>, the maximum feasible velocity values are provided to a jerk limited optimizer or solver <NUM> (<FIG>). For example, an acceleration limited velocity solver passes the velocity estimate to an approximated-jerk limited subproblem solver, which uses the velocity estimate as a starting point. The velocity estimates are typically higher or faster than what the corresponding jerk limited velocity estimates will end up being.

At <NUM>, the processor-based system or portion thereof (e.g., jerk limited optimizer or solver <NUM> (<FIG>)) determines, for each waypoint, a range of feasible velocities and a range of feasible accelerations based on a jerk limit. Determination of the range of feasible velocities and the range of feasible accelerations corresponds to the backward pass along the geometric path or trajectory that is performed as part of determining an optimized jerk limited velocity, described above. The range of feasible velocities and/or the range of feasible accelerations may be continuous, or alternatively may be a set of discrete velocities or discrete accelerations.

At <NUM>, the processor-based system or portion thereof selects a maximum feasible velocity for each of the waypoints along the geometric path from the range of feasible velocities. The selection of the maximum feasible velocity corresponds to the forward pass along the geometric path or trajectory that is performed as part of determining an optimized jerk limited velocity, described above. That is, for example, a value of jerk may be selected that approximately maximizes a velocity squared such that the respective value of jerk is within the corresponding jerk limit, the corresponding velocity is in the respective range of feasible velocities, and the corresponding acceleration is in the respective range of feasible accelerations.

At <NUM>, the processor-based system determines whether an exit condition has been reached, for example as described below.

At <NUM>, in response to a determination that the exit condition has not occurred, the processor-based system refines the velocity estimate, for example as described below. Once the exit condition has occurred for each waypoint, the processor-based system determines and outputs a time optimal constraint limited trajectory at <NUM>. Thus, the velocity estimate may be refined through several iterations, as described below, until the exit condition occurs.

The exit condition evaluation at <NUM> may be a determination that an estimated jerk limited velocity is sufficient relative to a defined criteria, and/or may be the performance of a defined number of iterations of refining the jerk limited velocity estimate, e.g. converging to a suitable jerk limited velocity estimate. For example, to determine whether a jerk limited velocity estimate is sufficient, the processor-based system may determine a difference between a jerk limited velocity or velocity squared estimate for a current iteration and a jerk limited velocity or velocity square estimate for a most recent previous iteration. The processor-based system can then determine whether the difference is within a defined acceptable difference threshold. The difference threshold may be selected to reflect the fact that gradual improvements in the jerk limited velocity estimate will result in the difference becoming smaller with each iteration by the jerk limited optimizer. The specific difference threshold will be application specific, and will depend on balancing a desire for optimized jerk limited velocity of a robot or robotic appendage versus the speed of calculating the optimized jerk limited velocity. The number of iterations can be set to any desired integer value.

Stated differently, the processor-based system may, for example, iterate until an end condition is reached. During a first iteration, an acceleration limited velocity is employed as an input to the jerk limited optimization. The first iteration results in a first jerk limited velocity estimate. The first jerk limited velocity estimate is likely lower or slower than optimal, but can be used if, for example, processing time is considered more valuable than optimal velocity of the robot or portion thereof. If the exit condition is not satisfied, during a second iteration the input from the first iteration minus an epsilon value is used as the new input to the jerk limited optimization. The second iteration results in a second jerk limited velocity estimate. The second jerk limited velocity estimate may be lower or slower than optimal, but can be used if, for example, processing time is considered more valuable than optimal velocity of the robot or portion thereof. If the exit condition is not satisfied, during a third iteration the input from the most recent previous iteration (e.g., second iteration) minus an epsilon value is used as the new input to the jerk limited optimization. The third iteration results in a third jerk limited velocity estimate. The third jerk limited velocity estimate may be lower or slower than optimal, but can be used if, for example, processing time is considered more valuable than optimal velocity of the robot or portion thereof. In each iteration, the jerk limited velocity estimates converge toward an optimal jerk limited velocity. The algorithm may be repeated until the exit condition is satisfied.

In performing each iteration of the jerk limited optimization, the jerk limited optimizer may perform the backward pass and forward pass generally described above. In some implementations, the epsilon value is a constant that is held constant across the number of iterations. In some implementations, the epsilon value is a variable that varies across the number of iterations. For instance, the epsilon value may take into account an assessment of convergence between two or more previous iterations.

The foregoing detailed description has set forth various embodiments of the devices and/or processes via the use of block diagrams, schematics, and examples. Insofar as such block diagrams, schematics, and examples contain one or more functions and/or operations, it will be understood by those skilled in the art that each function and/or operation within such block diagrams, flowcharts, or examples can be implemented, individually and/or collectively, by a wide range of hardware, software, firmware, or virtually any combination thereof. In one embodiment, the present subject matter may be implemented via Boolean circuits, Application Specific Integrated Circuits (ASICs) and/or FPGAs. However, those skilled in the art will recognize that the embodiments disclosed herein, in whole or in part, can be implemented in various different implementations 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 virtually any combination thereof, and that designing the circuitry and/or writing the code for the software and or firmware would be well within the skill of one of ordinary skill in the art in light of this disclosure.

Those of skill in the art will recognize that many of the methods or algorithms set out herein may employ additional acts, may omit some acts, and/or may execute acts in a different order than specified.

In addition, those skilled in the art will appreciate that the mechanisms taught herein are capable of being implemented in hardware, for example in one or more FPGAs or ASICs.

Claim 1:
A method (<NUM>) of operation in a processor-based system to control motion of a robot (<NUM>), the processor-based system (<NUM>) including at least one processor (<NUM>), the method comprising:
for each of a plurality of waypoints si from s<NUM> to at least sn-<NUM> along a geometric path, there being a corresponding robot configuration qi for each waypoint si, linearly determining a first estimate of a maximized velocity along the path while applying an acceleration limit to movement represented by the transitions between adjacent ones of the waypoints si;
following the linearly determining the first estimate using the first estimate as a starting point to linearly determine a second estimate of a maximized velocity along the path while applying a jerk limit to movement represented by the transitions between adjacent ones of the waypoints si;
for each of the waypoints si from at least s<NUM> to sn along the geometric path, determining a respective time at which the respective waypoint is to be reached based on respective ones of the determined second estimate of the maximized velocity; and providing at least the determined respective times at which the respective waypoints are to be reached to control motion of the robot.