MOTION PLANNING FOR ROBOTIC SYSTEMS

CROSS-REFERENCES TO RELATED APPLICATIONS

[0001] This application is an International Application and claims priority to U.S.

Application Serial No. 62/293,843, filed February 11, 2016, titled "METHOD FOR

PLANNING MOTIONS FOR AUTONOMOUS VEHICLES", the disclosure of which is incorporated herein by reference in its entirety.

STATEMENT AS TO RIGHTS TO INVENTIONS MADE UNDER FEDERALLY SPONSORED RESEARCH AND DEVELOPMENT

[0002] This invention was made with government support under Contract # HR0011-15-C- 0110 awarded by Defense Advanced Research Projects Agency, Grant Reference 6933325. Title of Funded Project: Fast and Light Autonomy. The government has certain rights in the invention.

FIELD OF THE INVENTION

[0003] Embodiments provide a motion planner which constructs action histories executable by a robotic system which results in a trajectory satisfying a motion specification when such an action history exists. More specifically, embodiments construct an action history with approximately minimum cost between a starting state and one of a plurality of destination states.

BACKGROUND OF THE INVENTION [0004] Motion planning is a challenging and fundamental problem in robotics research with numerous algorithms fine-tuned to variations of the problem. For example, the probabilistic roadmap planner (PRM) algorithm is an algorithm for planning in relatively high dimensions, but requiring a point-to-point local planning subroutine or steering function to connect pairs of randomly sampled states. A variation, PRM*, converges to an optimal solution with respect to some objective provided the steering function is optimal. The expansive-spaces tree (EST) and rapidly exploring random tree (RRT) algorithms were developed to address suboptimal planning with differential constraints where a steering function is unavailable. The steering function in these algorithms is replaced by a subroutine which forward integrates the system dynamics with a selected action. Optimal planning under differential constraints can be addressed by the kinodynamic variant of the RRT* algorithm, but again requires an optimal steering function like the PRM* algorithm. The effectiveness of RRT* has motivated several general approaches for optimal steering. However, the evaluation of these steering solutions can cause unacceptable delays in the construction of a trajectory meeting the motion planning specification.

[0005] Moreover, existing algorithms cannot provide optimal planning under differential constraints without the use of a steering function.

[0006] Embodiments of the invention address this and other problems, individually and collectively.

BRIEF SUMMARY OF THE INVENTION

[0007] Embodiments generally provide a finite approximation, in the form of a graph, of the set of action histories executable by the actuators of the robot together with a

generalization of the label correcting technique for graph search problems. Vertices of the graph are associated with terminal states of traj ectories resulting from each of the finite set of action histories. Existing label correcting techniques compare the relative obj ective cost of action histories resulting in trajectories terminating at the same state and, among those being compared, discards any action history with non-minimal cost. This pruning of the set of action histories is effective when there is a plurality of action histories resulting in traj ectories terminating at a particular state. Without a steering function to construct an action history resulting in a trajectory originating from one pre-specified state and terminating at another pre-specified state, the presence of any action histories, within the approximation, resulting in trajectories comparable by the label correcting technique becomes uncertain. The

generalization of this technique is to make cost comparisons between action histories resulting in trajectories terminating close enough to one another. Embodiments provide a procedure for constructing, in a finite amount of time, approximately minimum cost action histories resulting in traj ectories meeting a motion planning specification. [0008] Embodiments allow generating a trajectory, formed of trajectory segments, that connect a first state (e.g. in state space) to one of multiple destination states (e.g. in the state space). The trajectory approximates a globally optimal trajectory in the sense that the bound on sub-optimality of the constructed trajectory converges to zero as the method's resolution is increased.

[0009] Embodiments provide systems, methods and an apparatus for motion planning for a robotic system proceeding from a starting state to one of a plurality of destination states. A first intermediate region may be identified between the starting state and one of the plurality of destination states. A first traj ectory segment is identified between the starting state and the first intermediate region. In some embodiments, the first trajectory segment is determined to satisfy previously determined feasibility constraints. A first cost value associated with the first trajectory segment is calculated. Embodiments determine whether the first trajectory segment is included in a final trajectory based on the first cost value and a predetermined threshold amount. The predetermined threshold depends upon at least a size of the first intermediate region. The final trajectory connecting the starting state to the one of the plurality of destination states is generated.

[0010] According to some embodiments, determining whether the first trajectory segment is included in the final trajectory includes determining a reference cost value assigned to the first intermediate region. The first cost value is compared to the reference cost value. The first traj ectory segment is discarded when the first cost value is greater than the reference cost value by at least the predetermined threshold amount. Alternatively, the first trajectory segment is included in the final traj ectory and assigning the first cost value as a new reference cost value to the first intermediate region when the reference cost value is greater than the first cost value by at least the predetermined threshold amount.

[0011] In some embodiments, determining whether the first trajectory segment is included in the final trajectory includes determining that the first trajectory segment is a first trajectory terminating in the first intermediate region. According to some embodiments, determining that the first trajectory segment is the first trajectory terminating in the first intermediate region is based on determining that a predetermined reference cost value assigned to the first intermediate region is infinity. The first cost value is assigned to the first intermediate region as a reference cost value, and the first traj ectory segment is incorporated in the final trajectory. [0012] In some embodiments, a second trajectory segment between the starting state and the first intermediate region is identified. A second cost value associated with the second trajectory segment is calculated and compared to the reference cost value. When the second cost value is greater than the reference cost value by at least the predetermined threshold amount, the second trajectory segment is discarded. On the other hand, when the reference cost value is greater than the second cost value by at least the predetermined threshold amount, the second trajectory segment is incorporated in the final trajectory and the first trajectory segment is discarded. The second cost value is assigned to the first intermediate region as a new reference cost value. According to various embodiments, the first trajectory segment is between the starting state and a first state, the second trajectory segment is between the starting state and a second state, such that the first state and the second state are within the first intermediate region. [0013] According to various embodiments, the first trajectory segment is incorporated in the final traj ectory based on the first cost value and the predetermined threshold amount. A subsequent trajectory segment is determined between the first intermediate region and the one of the plurality of destination states. The final trajectory between the starting state and the one of the plurality of destination states is generated by joining the first trajectory segment and the subsequent trajectory segment.

[0014] In some embodiments, a second intermediate region between the first intermediate region and the one of the plurality of destination states is identified. In addition, a second trajectory segment between the first intermediate region and the second intermediate region is identified. A second cost value associated with the second trajectory segment is calculated. The second traj ectory segment may be incorporated in the final trajectory or discarded based on a sum of the first cost value and the second cost value, and the predetermined threshold amount. For example a reference cost value assigned to the second intermediate region is determined. The sum of the first cost value and the second cost value is compared to the reference cost value. When the sum of the first cost value and the second cost value is greater than the reference cost value by at least the predetermined threshold amount, the second trajectory segment is discarded. On the other hand, when the reference cost value is greater than the sum of the first cost value and the second cost value by at least the predetermined threshold amount, the second trajectory segment is incorporated in the final trajectory and the sum of the first cost value and the second cost value is assigned to the second intermediate region as a new reference cost value.

[0015] These and other embodiments of the invention are described in further detail below. DEFINITIONS

[0016] A "robot" may generally refer to a mechanical system which can be actuated to perform tasks. The means of actuation may be by a device which alters or imparts movement of the robot's mechanical components. [0017] A conventional automobile may include an embodiment of a robot in that it is actuated by an engine and steering system to transport passengers or cargo.

[0018] The "state" of a robot may generally refer to the present data relevant to determining the cost of future action histories and whether or not the motion resulting from the execution of an action history satisfies a motion specification. The data may be viewed as a vector space with dimension equal to the number of parameters in the dataset. A notion of dissimilarity or distance between two states is measured by the square root of the sum of the squared differences between coordinates of two configurations. For example, an automobile configuration may include lateral and longitudinal coordinates to locate a position of the vehicle, an angular coordinate to describe the heading of automobile, and an angular coordinate to describe the angle of the steering wheel. These four parameters are then viewed as a vector in a four dimensional space.

[0019] An "action" taken by a robot may generally refer to the activation of any combination of its actuators to impart motion in the robot's mechanical components. For example, an action may refer to a control input. Each possible action may be encoded as a vector space with dimension equal to the number of actuators; the effort of each actuator composing the coordinates of an action. Since the effort of an actuator is bounded by design limitations, the subset of said vector space occupied by actions realized within the design limitations of the actuators is bounded. This vector space will be referred to as the action space. A notion of dissimilarity or distance between two actions is measured by the square root of the sum of the squared differences between efforts of each individual actuator. For a conventional automobile, one example of an action may include a steering wheel angle together with a torque applied to the drive-train by an internal combustion engine resulting in motion of the automobile.

[0020] An "action history", also referred to as a control signal or input signal, may generally refer to an ordered collection of actions taken at each instant of time over some time interval. [0021] A "string" of action histories may generally refer to the sequential concatenation of two or more action histories to form a new action history. Any sequential number of action histories within a string of action histories is called a substring of the string of action histories. [0022] Atomic action histories may generally refer to, nominally simple, action histories designed to be basic elements for the formation of more complex strings of action histories. An atomic action history denoted NULL may include the action history with zero duration. The NULL action history has the property that its concatenation with "a" is equal to "a", where "a" denotes an action history. [0023] A "prefix" of a string of action histories may generally refer to a substring of said string beginning with the first action history in the string, (same for suffix)

[0024] A robot's "dynamic model" may generally refer to a differential equation derived from physical laws of the form x'(t)=f(x(t),u(t)) where u(t) is the action taken by the actuators at time t. The function u represents the entire action history. Similarly, x(t) is the robot's state at the instant in time t so that the function x describes a motion of the robot. The notation x'(t) denotes the differential of the motion x with respect to time at the instant in time t. The motion generated from the initial state xO by an action history u is the solution x, with x(0)=x0, satisfying the dynamic model of the robot.

[0025] An "action history approximation" may generally refer to a subset of the set of all possible action histories that can be executed by a robot. The number of action histories that can be executed by a robot is infinite whenever there are at least two actions that can be chosen at any instant. Usually, each actuator of a robot affords an infinite number of actions. For example, the steering wheel of a car can take any of the infinite number of angles between its minimum and maximum steering angle. A preferred embodiment of an action history approximation may be an action history approximation that has a finite number of elements so that any type of evaluation of the approximation, requiring a nonzero amount of time allotted for each action history, can be accomplished in finite time.

[0026] An "autonomous robot" may generally refer to a robot equipped with a device that automatically selects actions to accomplish a pre-specified useful task. One embodiment of an autonomous robot is a robot whose actions are determined by a computer equipped with automation software. [0027] A "motion specification" for a robot may consist of an initial robot configuration, a set of admissible configurations, and a set of terminal configurations. An action history is said to "satisfy a motion specification" if the motion generated by the action history from the initial configuration remains in the set of admissible configurations throughout the execution of the action history, and terminates at a configuration in the set of terminal configurations.

[0028] A "running cost" may assign a rate of accumulation of cost to each action taken at each robot configuration. The "cost" of an action history is the integrated running cost of executing an action history and the resulting motion. Examples of cost include but are not limited to the duration of the executed action history, the electrical energy required to execute the motion, or a measure of discomfort to passengers in an automobile.

[0029] A "motion planner" for an autonomous robot may generally refer to a device which receives a motion specification and produces an action history resulting in a motion that moves the robot from an initial configuration to one of the terminal configurations while remaining within the set of admissible configurations. The preferred embodiment of a motion planner may include as a digital computer together with an appropriate procedure for calculating an action history which, when executed, generates a motion meeting a motion specification. An example of a measure of merit is the time required to execute the motion. A motion executed more quickly can be given preference over one with a longer execution time. [0030] X may denote a set equipped with a notion of dissimilarity or distance between two elements (a metric for one skilled in the art). S may denote a subset of X. For an arbitrary element of X, the distance to the nearest point in S may be nonzero since said element of X may not be contained in S. The distance between an element x of X and the nearest element in S to x may be referred as n(x). The "dispersion" of S in X is the least upper bound of n(x); alternatively, the greatest value that can be achieved by n(x) over all values of x in X. Thus, dispersion may provide a precise measure of how well S approximates X.

[0031] A "partition" of a set S may refer to a set of disjoint subsets of S whose union is equal to S. A partition P of a set S, equipped with a notion of dissimilarity between its elements, is said to have radius r if for every element p of P (p is a subset of S) the dissimilarity between any two elements of p is no greater than r.

[0032] D and R may refer to sets, each equipped with a notion of dissimilarity. A function with D as its domain and R as its range is said to have a Lipschitz constant L if the dissimilarity between any two elements dl and d2 of D is no greater than the dissimilarity between f(dl) and f(d2) multiplied by L.

BRIEF DESCRIPTION OF THE DRAWINGS

[0033] FIGs. 1A and IB show exemplary trajectories for motion planning between a starting state and a plurality of destination states.

[0034] FIG. 2 shows a plurality of intermediary regions provided between a starting state and a plurality of destination states for use in a trajectory elimination process.

[0035] FIG. 3 shows a flowchart of motion planning for a robotic system with

corresponding trajectory diagrams. [0036] FIG. 4 shows a flowchart of motion planning that generates a final trajectory between a starting state and one of a plurality of destination states for a robotic system.

[0037] FIG. 5 shows a flowchart for determining whether a trajectory segment will be included in a final trajectory between a starting state and one of a plurality of destination states for a robotic system. [0038] FIG. 6 shows a block diagram of an exemplary motion planning and control system.

[0039] FIG. 7A shows an approximation of an exemplary action history to a robotic system.

[0040] FIG. 7B shows exemplary trajectories resulting from two distinct action histories similar to the one illustrated in FIG. 7A. [0041] FIG. 7C shows an exemplary partition in the state space containing the end points of the trajectories illustrated in FIG. 7B.

[0042] FIG. 8 shows an exemplary motion planning for shortest path problem using embodiments of the present invention.

[0043] FIG. 9 shows an exemplary motion planning for the torque limited pendulum swing- up problem using embodiments of the present invention.

[0044] FIG. 10 shows an exemplary motion planning for an acrobot using embodiments of the present invention. [0045] FIG. 11 shows an exemplary motion planning for an acceleration limited 3D point robot (e.g. a quadcopter with high bandwidth attitude control) using embodiments of the present invention.

[0046] FIG. 12 shows an exemplary motion planning for a nonholonomic wheeled robot using embodiments of the present invention.

DETAILED DESCRIPTION

[0047] Embodiments provide a motion planner which, with sufficiently high

approximation resolution, constructs action histories satisfying a motion specification when such an action history exists. Further, embodiments may construct action histories with approximately the minimum cost in the sense that with increasing approximation resolution, the constructed action history converges to the globally optimal cost. According to various embodiments, the constructed action history may be formed of a string of atomic action histories. Embodiments provide that even though one or more of the plurality of atomic action histories may not be associated with an optimal cost, the resulting constructed action history converges to the globally optimal cost.

[0048] Embodiments may construct an action history approximation from a finite subset of primitive actions within the set of available actions. Such a finite subset may be provided for each resolution r such that the dispersion of these subsets converges to zero as the resolution is increased. For example, each time the resolution is increased, one action selected at random from the set of actions may be added to the prior set. The primitive actions for each resolution may form the basis for a set of atomic action histories. One atomic action history is constructed by executing one primitive action for a duration inversely proportional to the resolution of the approximation. The action history approximation is then the set of all strings of said atomic actions composed of no more than h(R) atomic action histories where h is a function encoding the limit on the number of atomic action histories within an admissible string of action histories at resolution r. The function h can be any function which is asymptotically greater than any function proportional to the resolution. A choice of h satisfying this property counteracts the effect of the decreasing duration of atomic action histories resulting in action history approximations of arbitrarily duration with sufficiently large resolution. For example, the selection of h for each resolution r is to assign h(R) equal to the resolution multiplied by the logarithm of the resolution. [0049] According to various embodiments, any partition P of the configuration space with radius r|(R), depending on the resolution, such that the expression

R . ( ^■ _e (L _f h(R)/R) _

L _f T)(R)

converges to zero as the resolution tends to infinity. The specific selection of r|(R) and means of generating it can be made by a practitioner with ordinary skill in the art. [0050] Embodiments further provide a mechanism of examining a string of action histories and determining if it and all action histories for which it is a prefix can be omitted from the search without affecting the properties of the motion planner. This pruning of a, possibly large, subset of the strings over the atomic action histories is made by comparing the cost of two action histories generating motions terminating in the same element of the partition of the configuration space.

[0051] FIG. 1A illustrates exemplary trajectory segments 112, 114, 116, 1 18 for motion planning between an initial state (e.g. starting state) 100 and a plurality of destination states 106, 108. The plurality of destination states 106, 108 are part of a set of destination goal states 110. The trajectory segments 112, 114 terminate within the same region in the partition of the state space (e.g. an intermediate region between the starting state and the destination state) 105 at intermediate states 102 and 104, respectively. The secondary trajectory segments 1 16 start at intermediate state 102 and terminate at regions in the partition of the state space 107 and 109, respectively. The secondary trajectory segments 118 start at intermediate state 104 and terminate at regions in the partition of the state space 107 and 109, respectively.

[0052] According to various embodiments, if the trajectory segment 114 has cost exceeding the cost of trajectory segment 1 12 by a predetermined cost threshold, the trajectory segment 114 may be discarded. As such, all continuations of the traj ectory segment 1 14 need not be evaluated which saves time in the trajectory construction process. The logical basis for this is that any sequence of actions taken to produce continuations of the trajectory segment 1 14, illustrated with secondary trajectory segments 118, can be applied to produce a continuation of the traj ectory segment 1 12. The secondary trajectory segments 116 and 118 result from the same sequence of actions from similar initial states 102, 104. The states 102 and 104 are considered to be similar because they are within the same region in the partition of the state space (e.g. an intermediate region) 105 in the state space. Therefore, the secondary trajectory segments 1 16 and 1 18 will have similar cost and terminate at similar terminal states 106 and 108.

[0053] A bound on the difference in cost of secondary trajectory segments 1 16 and 1 18 can be determined from the separation of the intermediary states 102 and 104, and mathematical properties of the dynamical model for the robotic system. If the cost of the trajectory segment 112 exceeds the cost of the trajectory segment 1 14 by the bound on the difference in cost, then the cost of the trajectory segment 1 14 together with the continuation with any secondary trajectory segment 118 will necessarily exceed the cost of the trajectory segment 112 together with the cost of any secondary trajectory segment 116. This logic holds for any continuation of trajectory segments 1 12 and 1 14 so that all continuations of secondary trajectory segment 118 can be discarded without evaluation.

[0054] FIG. IB illustrates exemplary trajectory segments 1 12, 114 for motion planning between an initial state (e.g. starting state) 100 and intermediary states 102 and 104 provided between the initial state 100 and the goal destination states 1 10. As discussed in connection with FIG. 1 A, if the trajectory segment 114 has cost exceeding the cost of trajectory segment 112 by a predetermined cost threshold, the trajectory segment 114 may be discarded. As such, all continuations of the trajectory segment 1 14 need not be evaluated which saves time in the trajectory construction process. Since both intermediary states 102 and 104 are within the same hyper-rectangular partition (e.g. an intermediate region) 105 in the state space, any potential secondary segment 122 (with optimal or near-optimal cost) connecting the intermediary state 104 to one 108 of the goal destination states 110 may be substantially replicated in a secondary trajectory segment 120 connecting the intermediary state 102 to one 106 of the goal destination states 1 10, such that the destination state 106 and the destination state 108 are within the same destination region in the partition of the state space (e.g. a destination region) 1 15 in the state space.

[0055] The secondary trajectory segment 120 is deemed to be sufficiently similar to the secondary trajectory segment 122. In some embodiments, the secondary trajectory segment 120 may not be the same as or have the identical cost with the secondary trajectory segment 122. Thus, a final trajectory from the initial state 100 to the goal destination states 1 10 may be formed by the trajectory segment 112 and the trajectory segment 120.

[0056] FIG. 2 shows a plurality of intermediary regions 200, 202, 204 provided between a starting state 100 and a plurality of destination states 1 10 for use in a trajectory elimination process. An approximately minimum cost trajectory between the initial state 100 the one of the plurality of destination states 1 10 may be formed of a plurality of trajectory segments 208, 210, 212, and 214. The traj ectory segments may connect the initial state to a state at an intermediary region, a state in an intermediary region to a state in an intermediary region, or a state in an intermediary region and a state in the one of the plurality of destination states 110.

[0057] The exemplary embodiment illustrated in FIG. 2 illustrates two trajectories terminating at states 216 and 218 in a second intermediary region 202. The first being composed of trajectory segments 208 and 210, and the second being composed of trajectory segments 220 and 222. The cost of these two traj ectories is determined by the sum of the cost of the trajectory segments making up these traj ectories. By terminating in the same intermediary region the cost of these two trajectories can be compared and if the difference in cost exceeds a predetermined cost threshold, then the greater cost traj ectory can be removed from consideration for extension. In the embodiment illustrated in FIG. 2 the trajectory composed of trajectory segments 220 and 222 was determined to have sufficiently greater cost than the trajectory composed of trajectory segments 208 and 210 and was not considered for further extension while the trajectory composed of trajectory segments 208 and 210 was extended with segments 212 and 214 resulting in a trajectory satisfying the motion specification.

[0058] FIG. 3 shows a flowchart 300 of motion planning for a robotic system with corresponding trajectory diagrams. Motion planning may start by constructing a collection of atomic action histories from a finite set of actions. Each of the primitive actions may generate associated motions 302 of a robotic system. For example, the robotic system may include a driverless vehicle 350 and a first primitive action (e.g. a first action history) may generate three potential motions (e.g. trajectories) 310 for the driverless vehicle 350. The goal may be to guide the driverless vehicle 350 from its current state to a goal destination state 318.

[0059] At 304, all strings of primitive actions whose length is within the limit determined by the current resolution may be sequentially evaluated in order of their cost. For example, any subsequent primitive action following the first primitive action may result in additional trajectories 312 as illustrated in connection with step 304. In order to analyze all trajectories 312, the trajectories 312 may be set on a state space 352 (e.g. a configuration space) having a plurality of partitions. The size of the partitions may depend on the selected resolution. For example, a higher resolution may result in smaller sized partitions but may require additional processing resources (e.g. may be expensive to implement). Once the resolution and, consequently, the size of the partitions are determined, the trajectories 312 may be sequentially evaluated. The trajectories resulting in the same partition may be compared against each other. [0060] At 306, costs of trajectories terminating in the same partition may be compared to determine if the difference in cost exceeds a predetermined cost threshold. The

predetermined cost threshold may depend upon the selected resolution. The predetermined cost threshold is discussed below in greater detail. Upon comparing two trajectories, a trajectory among the two with greatest cost may be discarded and omitted from consideration for further extension if its cost exceeds a least cost trajectory among the two by a

predetermined cost threshold. For example, the trajectory 316 exceeds the cost of another trajectory terminating within its associated partition by the predetermined cost threshold. As a result, trajectory 316 is discarded and not considered for further extension. In contrast, trajectory 314 does not exceed the cost of the second trajectory terminating within its associated partition by the predetermined cost threshold. Thus, it remains as a candidate for further extension.

[0061] At 308, the search for a trajectory ends when a trajectory terminating in the goal partition is found, and the trajectory is provided to the robotic system. For example, when the trajectory 320 that terminates in the goal destination partition 318 is determined, the algorithm stops. The trajectory 320 is the globally optimal trajectory for arriving to the destination state (e.g. partition 318) from the starting state.

[0062] Embodiments may provide a method for motion planning for a robotic system proceeding from a starting state to one of a plurality of destination states. FIG. 4 shows a flowchart 400 of motion planning that generates a final trajectory between a starting state and one of a plurality of destination states for a robotic system. The following discussion of FIG. 4 also refers to FIGs. IB and 2 for ease of description.

[0063] At 402, a first intermediate region between the starting state and one of the plurality of destination states is identified. Exemplary intermediary states are illustrated in FIG. 2. In some embodiments, the trajectory segments may be evaluated using the state space. The state space may be partitioned into regions whose size depend on the selected resolution for the state space. An intermediate region may correspond to a partition in the state space. For example, the rectangular region 105 illustrated in FIG. IB may represent an intermediary region between the starting state 100 and the plurality of destination states 1 10.

[0064] At 404, a first trajectory segment between the starting state and the first intermediate region may be identified. For example, in FIG. IB, the trajectory 1 12 between the starting state 100 and the partition 105 may be identified. At 406, a first cost value associated with the first traj ectory segment may be calculated. Exemplary cost functions that may be used to determine the cost of the first trajectory segment is discussed below in greater detail. In some embodiments, prior to determining the cost function for the first trajectory segment, it may be determined that the first trajectory segment satisfies all feasibility constraints defined for the particular motion planning scenario. This is to ensure that trajectory segments that violate feasibility constraints will not be evaluated as they cannot be a part of the final trajectory even if they are associated with an optimal or otherwise desirable cost.

[0065] At 408, a determination is made as to whether the first trajectory segment may be included in a final trajectory based on the first cost value and a predetermined threshold amount. The predetermined threshold depends upon at least a size of the first intermediate region. Referring back to FIG. IB, the motion planner determines a final trajectory for a robotics system that proceeds from the starting state to at least one of the destination states.

This is done by determining trajectory segments that will form the final trajectory. For trajectory segment 112, the system determines whether the cost of the trajectory segment 112 allows for the trajectory segment to be a part of the final trajectory. This determination is discussed in greater detail in connection with FIG. 5.

[0066] At 410, the final trajectory connecting the starting state to the one of the plurality of destination states is determined and provided to the robotics system. [0067] Referring now to FIG. 5, the trajectory selection process will be described. FIG. 5 shows a flowchart 500 for determining whether a trajectory segment will be included in a final trajectory between a starting state and one of a plurality of destination states for a robotic system. At 502, a reference cost value assigned to the first intermediate region is determined. For example, the reference cost value of the first intermediate region may be the cost value of another trajectory segment. In some embodiments, the reference cost value of the first intermediate region may be infinity. For example, reference cost values of all partitions in the state space representation may be set to infinity. When a trajectory segment terminates in a given partition, the cost of the trajectory segment is compared to the reference cost of the partition, and if the cost of the trajectory segment is greater than the reference cost of the partition by the predetermined threshold amount, then the cost of the trajectory segment is assigned to the partition as the new reference cost. [0068] At 504, the first cost value of the first trajectory is compared to the reference cost value. At 506, the first trajectory segment is discarded when the first cost value is greater than the reference cost value by at least the predetermined threshold amount. This means that another trajectory segment terminating at the same intermediate region had a more desirable cost (e.g. lower than the cost of the first traj ectory segment by the predetermined threshold amount) than the first traj ectory segment. Thus, the first trajectory segment is discarded in favor of the trajectory segment with lower cost. For example, in FIG. IB, if the trajectory segment under evaluation (e.g. "the first trajectory segment) was trajectory segment 1 14, and the trajectory segment 112 previously terminated at partition 105 giving the partition 105 its cost value as the reference cost, the cost value of the trajectory segment 1 14 will be greater (the trajectory segment 1 14 takes a longer path with multiple turns) than the reference cost of the partition 105. Therefore, the traj ectory segment 1 14 will be discarded.

[0069] On the other hand, at 506, when the reference cost value is greater than the first cost value by at least the predetermined threshold amount, the first trajectory segment is incorporated in the final trajectory and the first cost value is assigned as a new reference cost value to the first intermediate region. Referring back to FIG. IB, if the trajectory segment under evaluation (e.g. "the first trajectory segment) was trajectory segment 112, and the trajectory segment 1 14 previously terminated at partition 105 giving the partition 105 its cost value as the reference cost, the cost value of the trajectory segment 112 will be less than the reference cost of the partition 105. Therefore, the trajectory segment 112 will be included in the final traj ectory, the trajectory segment 1 14 will be discarded, and the cost value of the trajectory segment 112 will be assigned as the new reference cost value to the partition 105. Alternatively, if the trajectory segment under evaluation (e.g. "the first trajectory segment) was trajectory segment 1 12, and the partition 105 has not been previously visited by another trajectory segment and therefore the partition 105 has infinity as the reference cost, the cost value of the trajectory segment 1 12 will be less than the reference cost of the partition 105. Therefore, the traj ectory segment 1 12 will be included in the final trajectory, and the cost value of the trajectory segment 1 12 will be assigned as the new reference cost value to the partition 105. [0070] Embodiments provide a system and/or an apparatus for implementing motion planning as discussed in connection with FIGs. 1-5. FIG. 6 shows a block diagram of an exemplary motion planning and control system 600. The motion planning and control system 600 includes a motion planning computer 602 and a motion control system 604. The motion planning computer 602 may include a data processor 610, a network interface 612 and a computer-readable medium 614 storing various modules for generating a final trajectory for a robotic system connecting a starting state to one of a plurality of destination states. The generated trajectory may be stored at a storage unit 606. The storage unit 606 may be internal to or external and accessible to the motion planning computer 602. [0071] According to various embodiments, the computer-readable medium 614 may store an action history approximation module 616 for approximating, using the data processor 610, an action history space TI by a finite subset 1i _R where R E N is a resolution parameter. To construct 1i _R , a family of finite subsets Ω _κ c Ω of the input space is determined, such that the dispersion in Ω converges to zero as i?→∞. A family of such subsets exists and may be obtained with regular grids or random sampling for a given Ω. The approximated action history space 1i _R consists of piecewise constant action histories on time intervals.

[0072] The computer-readable medium 614 may store a state space partitioning module 618 for partitioning, using the data processor 610, the state space into regions whose size depend on the selected resolution for the state space. The partition has radius r if the partition elements are each contained in a neighborhood of radius r. Like the discretization of TI, the radius of the partition is parametrized by the resolution R.

[0073] The computer-readable medium 614 may store a comparable action elimination module 619 for eliminating, using the data processor 610, actions within the approximated action history that are deemed to be comparable. According to various embodiments, two actions may be comparable if the actions generate similar trajectories. For example, similar trajectories may terminate at the same partition of the state space and may also have similar costs.

[0074] The computer-readable medium 614 may store a cost per state space partition determination module 620 for determining, using the data processor 610, a cost for each partition (as determined by the state space partitioning module 618) in the state space. As discussed above, the cost per state space partition determination module 620 may assign infinity as a reference cost to all partitions of the state space. When a first trajectory terminates in a given partition, the cost per state space partition determination module 620 may assign the cost of the first trajectory to that partition. When a second trajectory, with a cost lower that the cost of the first trajectory by a predetermined threshold amount, the cost per state space partition determination module 620 may assign the cost of the second trajectory to the partition.

[0075] The computer-readable medium 614 may store a motion specification checking module 622 for determining, using the data processor 610, whether a trajectory satisfies predetermined motion specifications. For example, if a trajectory requires lateral movement which is not permitted, the motion specification checking module 622 may identify the trajectory and remove the traj ectory from trajectories to be evaluated by the traj ectory evaluation and final trajectory generation module 624.

[0076] The computer-readable medium 614 may store a trajectory evaluation and final trajectory generation module 624 for evaluating, using the data processor 610, a plurality of trajectories, eliminating a set of traj ectories, and identifying trajectories that form a final trajectory. According to various embodiments, the traj ectory evaluation and final trajectory generation module 624 may compare a cost of a trajectory to a reference cost of a state space partition in which the traj ectory terminates. If the cost of the traj ectory is lower than the reference cost of the partition by a predetermined threshold amount, then the trajectory evaluation and final trajectory generation module 624 may include the trajectory in the final trajectory. The trajectory evaluation and final trajectory generation module 624 may generate, using the data processor 610, a final trajectory for a robotic system proceeding from a starting state to one of a plurality of destination states and store the final trajectory at the storage unit 606.

[0077] Other modules and submodules may also reside on the computer readable medium 614. Examples of additional modules may include a trajectory cost calculating module for calculating the cost for each evaluated trajectory. The trajectory cost calculating module may work in connection with the motion specification checking module 622 so as not to calculate cost for a trajectory that violates motion specifications. Each module in the motion planning computer 602 may be combined with any of the additional modules as appropriate. Each module in the motion planning computer 602 may comprise one or more submodules, where each submodule may comprise one or more functions implemented by computer instructions, executable by the data processor 610. [0078] The data processor 610 (e.g., a microprocessor) may process functions of the motion planning computer 602. The data processor 610 may include hardware that can carry out instructions embodied as computer instructions (e.g. code) in a computer-readable medium. The data processor 610 may be a central processing unit (CPU). As used herein, a processor can include a single-core processor, a plurality of single-core processors, a multi- core processor, a plurality of multi-core processors, or any other suitable combination of hardware configured to perform arithmetical, logical, and/or input/output operations of a computing device.

[0079] The network interface 612 may be any suitable combination of hardware and software that enables data to be transferred to and from the motion planning computer 622. The network interface 612 may enable the motion planning computer 622 to communicate data to and from another device (e.g. the motion control system 604). Some examples of the network interface 612 may include a modem, a physical network interface (such as an Ethernet card or other Network Interface Card (NIC)), a virtual network interface, a communications port, a Personal Computer Memory Card International Association

(PCMCIA) slot and card, or the like.

[0080] Details of the motion planning method as discussed in connection with FIGs. 1-5 is discussed next. The following sections provide detailed explanations on how trajectory segments are evaluated and how the final trajectory is generated. Use cases illustrating the implementation of the motion planning method and/or algorithm are discussed in connection with FIGs. 8-12.

[0081] Shortest path algorithms are methods of searching over paths of a graph for a minimum-cost path between an origin or root vertex to a set of goal vertices. In a

conventional label correcting method, the algorithm maintains a best known path terminating at each vertex of the graph. This path labels that vertex. At a particular iteration, if a path under consideration does not have lower cost than the path labeling the terminal vertex, the path under consideration is discarded. As a consequence, the subtree of paths originating from the discarded path will not be evaluated.

[0082] The label of a vertex in conventional label correcting algorithms is in fact a label for the paths terminating at that vertex. Then each vertex identifies an equivalence class of paths. Paths within each equivalence class are ordered by their cost, and the efficiency of label correcting methods comes from narrowing the search to minimum cost paths in their associated equivalence class. In contrast, embodiments identify paths associated to trajectories terminating in the same region of the state space instead of the same state. The difference in cost of two paths must exceed a predetermined threshold.

[0083] The configuration and relevant quantities of a system may be described by a state in M. ⁿ . According to various embodiments, the decision variable may be an action history or continuous history of actions u from an action history space ίί affecting a state trajectory x in a trajectory space X _XQ . The action history space is constructed from a set of admissible actions Ω c M. ^m bounded by u _max . The action history space is defined as: [0084] The action history and trajectory are related through a model of the system dynamics described by a differential constraint, x(t) = f(x(t), u(t)), x(0) (2)

[0085] A system map φ _Χο : ίί→ X _Xg is defined to relate action histories to associated trajectories (e.g. the solution to equation (2)) with domain equal to the domain of the action history. The initial state x ₀ parametrizes the map and trajectory space. To simplify notation, τ(χ) for a function xwith domain [t ₁₍ t ₂ ] denotes the maximum of the domain, t ₂ .

[0086] In addition to the differential constraint, feasible trajectories for a particular problem satisfy point-wise constraints defined by a subset Xf _ree i I and a specified initial state x _ic . The subset of feasible trajectories X^ _eas are defined

'= { e : .·:( * > e X ^ Vi G ; . (.<' ?] ) . (3)

[0087] Similarly, the subset ¾ _oai of M. ⁿ is used to encode a terminal constraint. The subset of Xf _eas consisting of trajectories x with ^χ (τ( ^χ )) £ X _goai defines X _goa i- The decision variable for the problem is the action history u which is chosen such that the trajectory (p _Xic (u) is in Xgoai. Naturally, action histories mapping to trajectories in Xf _eas /goai ^are defined by the inverse relation * * ^«»» .. ^<- s ^«» s -fa**/

[0088] A general cost functional which integrates a running-cost g of the state and action at each instant along a trajectory is used to compare the merit of one action history over another. Restricted to solutions of equation (2), the cost functional depends only on the action history and initial state,

Jx ₀ M = J _[0>T(u)] fl ^f ([^o( ^u ]( ^t ,"( )dM(t)- (4) The notation [(p _Xo (u)](t) denotes the evaluation of the trajectory satisfying equation (2) with the action history u and initial state x ₀ at time t.

[0089] The domain of / is extended with the object NULL such that ] _Xq (NULL) =∞ for all x ₀ E M. ⁷¹ . Further, equation (4) may not admit a minimum, accordingly, the following relaxed optimal kinodynamic motion planning problem may be defined and solved:

Problem i. F d a sequence {»#} C U SULL stseh tat

Jim -s- _y Juft)— m ^" i ,/* _K («) ^; = -c ^* . (5) [0090] With the convention that inf _UE0 J _x . _c (u) =∞, a solution sequence exists. An algorithm parameterized by a resolution R E M whose output for each informs a sequence solving this problem is called resolution complete.

[0091] The problem data are assumed to satisfy the following:

1. The sets X _ree and X _goa i are open with respect to the standard topology on M ⁿ .

2. There are known constants Lj > 0 and M≥ 0 such that u)— f(x ₂ , ) || ₂ <

^L f\\( ^x i ^{~ x} 2 \\2, and || (xi,u)|| ₂ < for all x ₁ ,x ₂ E X _free mdu E Ω.

3. There is a known constant L _g ≥ 0 such that )— 5 ^, ( ₂ , ₂ ) || ₂ <

¾ll( ^x i ^{— x} 2 _> ^u i ^{~ M} 2)ll ₂ f° ^r all X ,x ₂ E Xf _ree d u ₁ ,u ₂ E Ω.

4· J _Xlc ) > Oforallu E U.

[0092] The reachable set is not required to have a nonempty interior as in the kinodynamic variant of RRT* and SST.

Approximation of U

[0093] According to various embodiments, the action history space TI is approximated by a finite subset 1i _R where R E M is a resolution parameter. To construct 1i _R , a family of finite subsets Ω _κ c Ω of the input space is determined, such that the dispersion in Ω converges to zero as ?→∞. A family of such subsets exists and may be obtained with regular grids or random sampling for a given Ω.

[0094] The approximated action history space 1i _R consists of piecewise constant action 1 )

histories on time intervals [— -— ,— ) for c > 0, ί £ (1, . . . , d], and all values d less than a user specified function h(R). The constant values of the action history are actions from Ω .

[0095] The function h: N→ N defines a horizon limit and can be any function satisfying

\im R/h(R) = 0. (6) i?→co

[0096] For example, h(R) = Rlog(R) is acceptable. This ensures that the horizon limit is unbounded in Rso that any finite time domain can be approximated for sufficiently large R.

A parent of an action history w £ t^with domain [0,— ] is defined as the action history u £ 1i _R with domain [0, ^— such that w(t) = u(t)for all t £ [0, ^ ^). In this case, w is

R R

a child of u. A tree (graph) is defined with l _R as the vertex set, and edges defined by ordered pairs of action histories (u, w) such that u is the parent of w. To serve as the root of the tree, 1i _R is augmented with the special action history ld _u defined such that ] _Xo ( d _u ) = 0 and [φ _Χο (Ι du)] (0) = x ₀ . ld _u has no parent, but is the parent of action histories with domain [0, c/R] . The action history wis an ancestor of u if τ(νν) < τ(μ) and w(t) = u(t) for all t £ [0, T(W)). In this case u is a descendant of w. The depth of an action in t^is the number of ancestors of that action.

Partitioning Xf _rPP and the GLC Conditions

[0097] A partition of Xf _ree is used to define comparable action histories. The partition has radius r if the partition elements are each contained in a neighborhood of radius r. Like the discretization of TI, the radius of the partition is parametrized by the resolution R. [0098] According to various embodiments, hypercube partitions may be considered. A user specified function η : N→ M _>0 controls the side length of the hypercube. For states p ₁₍ p ₂ £ Ii ⁿ we write p _x ~ p ₂ if where [·] is the coordinate-wise floor map (e.g. [(2.9,3.2) J = (2,3)). Then the equivalence classes of the ~ relation define a simple hypercube partition of radius /η/η(Κ). We extend this relation to actions by comparing the terminal state of the resulting trajectory. For u _lt u ₂ £ U _R we write ~ u ₂ ii the resulting trajectories terminate in the same hypercube. That is,

¾ R

_Ul ~ u ₂ <=> [<¾, ( )] (τ( )) ~ W _XI U ₂ )] {T{U ₂ )). (7) [0099] The following descriptions refer to FIGs. 7A-7C. FIG. 7A shows an exemplary action history composed of a string of atomic action histories. FIG. 7B shows exemplary trajectories resulting from two distinct action histories similar to the one illustrated in FIG. 7A. FIG. 7C shows an exemplary region in state space containing the end points of the trajectories illustrated in FIG. 7B.

[0100] More specifically, FIG. 7A illustrates an action history 700 from the approximated space of action histories 1i _R , FIG. 7B illustrates the mapping, determined by the dynamic model, of two equivalent action histories u _x ~ u ₂ , into the trajectory space X _Xic resulting in trajectories 710 and 712. FIG. 7C illustrates the mapping from terminal states 706 and 708 of trajectories 710 and 712 respectively in M ⁿ into Έ ^η 720 to identify that the terminal states are elements of the same region of predetermined size.

[0101] To compare action histories, embodiments provide < _R u ₂ if the GLCconditions are satisfied. The GLC conditions are:

1. [condition 1] ~ u ₂ ,

2. [condition 2] τ( ) < τ(μ ₂ ),

3. [condition 3] + ( _e — - 1) < / _¾ (u ₂ )

[0102] An action history is called minimal if there is no u ₂ E 1i _R such that u ₂ < _R u _t . Such an action history can be thought of as being a good candidate for later expansion during the search. Otherwise, it can be discarded. In order for the claimed method to be a resolution complete algorithm, the scaling parameter η must satisfy

[0103] Equation (8) implies the cost threshold in [condition 3] is in 0(1/R) and converges to zero. A condition yielding the same theoretical results, but asymptotically requiring more action histories to be evaluated would be to replace the threshold with an arbitrarily small positive constant.

[0104] Additionally, equation (8) and [condition 3] simplify in some cases. For kinematic problems h _j = 0. Taking the limit L _j → 0, the constraint (8) becomes ϊι(β)/η(Κ)→ 0. The second special case is minimum-time problems where g(x, u) = 1 in equation (4) so that L _g = 0. Then [condition 3] simplifies to / _Xic ( )≤ J _Xlc (u ₂ ).

Algorithm Description

[0105] Pseudocode for the motion planning method according to various embodiments is described in the algorithm below. A set Q serves as a priority queue of candidate action histories. A set∑ contains action histories representing labels of ~ equivalence classes. The method expand (u) returns the set of all children of u. The method pop(Q) deletes from Q, and returns an action history u such that

[0106] The addition of an admissible heuristic in equation (9) can be used to guide the search without affecting the solution accuracy.

[0107] The method find u,∑) returns w E∑ such that u ~ w or NULL if no such w is present in∑. Problem specific collision and goal checking subroutines are used to evaluate u E V-feas and u E Ugoai. The method depth(u) returns the number of ancestors of u.

Algorithm I Generalized Label Correcting (GL€> eifKxl

Q { Idu ), £ <- . S *~ %

• MS« Q 0

•u popiQ)

S <··· ϊ;.:ϊ' ¾:« ^' ί.ί:ί(ΐί)

If V.} £ ¾ _CMl! ) V (ie≠hiw) > (R})

14: return { &,BULL}

[0108] The algorithm begins by adding the root Id^to the queue (line 1), and then enters a loop which recursively removes and expands the top of the queue (line 3) adding children to S (line 4). If the queue is empty the algorithm terminates (line 2) returning NULL (line 14). Each action history in S (line 5) is checked for membership in 1i _q0 ai ^m which case the algorithm terminates returning a feasible solution with approximately the optimal cost. Otherwise, the action histories are checked for infeasibility or suboptimality by the

GLCconditions (line 9). Next, a relabeling condition for the associated equivalence classes (e.g. grid partitions) of remaining action histories is checked (line 11). Finally, remaining action histories in S are added to the queue (line 13).

[0109] Accordingly, embodiments provide a motion planning method that is a resolution complete algorithm for the optimal kinodynamic motion planning problem. If w _R is the action history returned by the motion planning method, for resolution R, then

\im _R→∞ J _Xlc (w _R ) = c ^* . This conclusion is independent of the order in which children of the current node are examined in line 5 as well as the relabeling condition in line 11.

EXAMPLES

[0110] The motion planning method (e.g. Algorithm 1) discussed herein was tested on five problems and compared, when applicable, to the implementation of RRT ^* and SST techniques. The goal is to examine the performance of the inventive method on a wide variety of problems. The examples include under-actuated nonlinear systems, multiple cost objectives, and environments with/without obstacles. Adding obstacles effectively speeds up the inventive method since it reduces the size of the search tree. Another focus of the examples is on real-time application. In each example the running time for the inventive method to produce a (visually) acceptable traj ectory is comparable to the execution time. [0111] According to various embodiments, the inventive method was implemented in C++ and run with a 3.70GHz Intel Xeon CPU. The set Q was implemented with an STL priority queue so that the pop(Q) method and insertion operations have logarithmic complexity. The set∑ was implemented with an STL set which uses a binary search tree so that find(w,∑) has logarithmic complexity as well. Sets Xf _ree and X _goa i are described by algebraic inequalities. The approximation of the input space Ω is constructed by uniform deterministic sampling of i? ^m controls from Ω, where m is the dimension of the input space.

[0112] Evaluation of u E V-f _eas and u E V- _goa i is approximated by first numerically approximating cp _Xic (u) with Euler integration (except for i?i?T ^* which uniformly samples along the local planning solution). The number of time-steps is given by N = T(M)/41 with duration τ(μ)/Ν. Maximum time-steps A are 0.005 for the first problem, 0.1 for the second through fourth problem, and 0.02 for the last problem. Feasibility is then approximated by collision checking at each time-step along the trajectory.

[0113] In the examples illustrates in FIGs. 8-12, the running times are based on 10 trial average for randomized planners and only reported if a solution was found at a particular time in all 10 trials. Shortest path problem:

[0114] FIG. 8 shows an exemplary motion planning for shortest path problem using embodiments of the present invention. Exemplary motion planning problem with the objective of finding a shortest path from the initial state to one of the plurality of goal states may be considered as a basic form of the motion planning problem which is adequately addressed by prior art.

[0115] The exact solution to this problem is known so that the relative convergence rates of the inventive method (GLC method) 802 can be compared with prior art methods SST method 803 and the RRT* method 805. The opaque paths 812, 814 and solid paths 808, 810 illustrate first and last solutions obtained respectively. The 812 is a trajectory planned by the inventive method with a low resolution. Similarly 814 is a trajectory planned by the RRT* method with a low resolution. The trajectory 808 is high resolution trajectory generated by the inventive method while trajectory 810 is a high resolution trajectory generated by the RRT* method. "

[0116] A shortest path problem can be represented by the dynamics f (x, u) = uwith the running-cost g (x, u) = 1 and input space Ω = {u E M. ² : \\u \\ ₂ = 1}. The free space 801 and goal 806 are illustrated in FIG. 8. The parameters for the inventive method are c = 10,

77 (Λ) = 300i? ² , and h(R) = 100i?log(i?) with resolutions R E {20,25, . . . ,200}. The exact solution to this problem is known that the relative convergence rates of the inventive method (GLC method) 802, SST 803, and RRT ^* 805 can be compared. The opaque paths 807 and solid paths 808 illustrate first and last solutions obtained respectively.

Torque limited pendulum swing-up:

[0117] FIG. 9 shows an exemplary motion planning for the torque limited pendulum swing- up problem using embodiments of the present invention.

[0118] The trajectory 900 constructed by the inventive method for a resolution of R = 7 is shown as traced out in the state space 901. Markers 902 illustrate regions with finite associated reference cost with the map indicating the cost of the associate region.

Computation time and solution cost for the inventive method at various resolutions 904 is compared to that of the SST method 906. The nontrivial dynamics of the pendulum make it so that the RRT* method cannot be adapted to this example by one with ordinary skill in the art.

[0119] The trajectory 900 illustrates the GLC solution for R = 7 with the markers 904 indicating the cost of grid labels. The system dynamics are /(0, ω, u) = (ω, u—

sin(0)) ^T with the running-cost g(6, ω, w) = 1 and input space Ω = [—0.2,0.2]. The free space is modeled as Xf _ree = . ² . The initial state is x _ic = (0,0) and the goal region is

Xgoai ⁼ { ^{χ e} 11( ± it, ω || ₂ ≤ 0.1}. The parameters for the GLC method are c = 6, η Κ) = 16R ² - ⁵ , and h(R) = 100Mog(i?) with resolutions R E {4,5,6,7,8}. The optimal solution is unknown so only the running time vs. cost can be plotted. Without a local planning solution, the RRT ^* technique is not applicable. The same is true for the remaining examples. Torque limited acrobot swing-up:

[0120] FIG. 10 shows an exemplary motion planning problem for a robot model known as the acrobot and motion plans constructed by the inventive method. The acrobot is a double link pendulum actuated by a motor at the second link. The state vector space has four dimensions; two dimensions encode the configuration of the mechanical elements, and two encode the rate of movement of the mechanical elements. The minimum time objective is reflected with the running-cost g(x,u)=l and the input space Ω =[-4,4]. The parameters for the inventive method are c=6, η^ = 16R ² , and h(R) = 100i?log(i?) with resolutions

RG {4,5,...,10}. The graphic 1000 illustrates the configuration of the robot at uniformly spaced time instances during execution of the trajectory constructed by the inventive method. The inventive method and the SST method are suitable methods of constructing

approximately minimum cost trajectories satisfying the motion specification. However, in a given amount of time the inventive method arrives at a solution with less construction time 1002 than the prior art 1004.

Acceleration limited 3D point robot: [0121] FIG. 11 shows an exemplary motion planning problem for an acceleration limited 3D point robot model using embodiments of the present invention. Such a robot model emulates the mobility of an unmanned aerial vehicle such as a quadcopter with a high bandwidth attitude controller for redirecting thrust. The dynamic model is f(x, v, u) = (y, 5.0M— 0.1i7 ||i7|| ₂ ) ^T where x, v, and ware each elements of W ³ ; there are a total of six states and three actuators. A minimum time objective is reflected with the running-cost g(x, v, u) = 1. The set of available actions is Ω = {u E M. ³ : \\u \\ ₂ ≤ 1}. The free space and goal are illustrated in 1104. The parameters for the inventive method are c = 6, η (R) = 16R ² , and h(R) = 100Mog(i?). Embodiments withi? E (4,5, . . . ,10} were used to construct approximately minimum cost trajectories from the initial state to one of the plurality of goal states while satisfying the motion specification. Time required to construct the trajectory and cost of the approximate solution are shown in the plot 1100 demonstrating convergence of the approximately minimum cost trajectory to the minimum cost trajectory with increasing resolution. To demonstrate that the inventive method may be used for real-time motion planning, the curve 1102 identifies the boundary where execution time exceeds the time required to construct a trajectory satisfying the motion specification. The trajectory negotiating the environment in 1104 was constructed in less time than the duration of the executed trajectory demonstrating the feasibility of real-time planning in a receding horizon manner.

Nonholonomic wheeled robot:

[0122] FIG. 12 shows an exemplary motion planning problem where a state trajectory must be constructed through a gauntlet of obstacles 1208 shown in grey. The system dynamics emulating the mobility of a wheeled robot are f(x, y, Θ, u) = (cos(6>), sin(6>), u) ^T . The wheeled robot is used to reflect the limited maneuverability of an automobile. Embodiments with resolutions R E (4,5, . . . ,9} were used to construct approximately minimum cost trajectories from the initial state 1204 to one of the plurality of goal states while satisfying the motion specification. In this example, two objectives were considered. The first is a minimum time objective reflected by the running cost g(x, y, 9, u) = 1, resulting in a trajectory 1202 with aggressive turns around obstacles. The second is a passenger comfort objective reflected by the running cost g(x, y, Q, u) = 1 + 2u ² , resulting in a trajectory 1200 with gradual turns around obstacles. The time required to construct the trajectory and cost of the approximate solution are shown in the plot 1210 demonstrating convergence of the approximately minimum cost trajectory to the minimum cost trajectory with increasing resolution.

[0123] In the examples discussed in connection with FIGs. 8-12, the i?i?r ^* algorithm was only tested on the first example since a steering function is unavailable for the remaining four examples. The SST algorithm was tested in three examples illustrated in FIGs. 8-11 since neither the analysis nor the implementation supports objectives other than minimum-time (i.e. the analysis or the implementation does not support the nonholonomic wheeled robot example illustrated in FIG. 12). The run-time vs. objective-cost curves provided for the inventive method (i.e. GLC method) is several orders of magnitude faster than the conventional SST algorithm.

[0124] In the 3D point robot example, the running time for the inventive method to produce a (visually) good quality trajectory is roughly equal to the execution time. In the wheeled robot example a minimum time cost function was compared to a cost function which also penalized lateral acceleration. The minimum time solution results in a path with abrupt changes in angular rate making it unsuitable for autonomous driving applications where passenger comfort is a consideration. Penalizing angular velocity resulted in a solution with more gradual changes in angular velocity.

[0125] Each data point in FIGs. 8-12 represents the running time and solution cost of a complete evaluation of the inventive GLC method while conventional RRT ^* and SST methods are incremental methods running until interrupted. Each run of Algorithm 1 operates on l _R for a fixed R. Since 1i _R <£ U _R+1 , it is possible that an optimal action history in l _R may be better than any action history in 1i _R+1 for some R which explains the nonmonotonic convergence observed.

Analysis of the GLC Condition

[0126] The action history space Uwas defined in equation (1). A metric on this space is given by The family of trajectory spaces are more precisely defined as

where M is that of assumption A-2. This set is equipped with the metric

d _x (x ₁ , x ₂ )≡ _† . _n . max {||*i (t) - x ₂ (t) ll} + M \ T{X ) - τ(χ ₂ ) | . [0127] Several known continuity properties of <¾ _o are reviewed here. The distance between solutions to equation (2) with initial conditions x ₀ and z ₀ is bounded by ll _*0 (")](t) - z ₀ (")](t)ll ₂ ≤ ΙΙ ^χ ο -zolbe^.

[0128] In addition to continuous dependence on the initial condition parameter, the map (¾- _o is also continuous from ίί into X _Xq . Xf _eas is open when assumption A-l is satisfied since it follows directly from the definition of continuity that Uf _eas and U _goa i are open subsets of 11. They are defined as the preimage of Xf _eas and X _goa i under φ _χ

[0129] Similar observations for the cost functional are developed below.

Lemma 1: J _Xo '-U ^→ IK.is continuous for any x ₀ EK ⁿ .

Let , u ₂ Είί and without loss of generality, τ( ) < T( ₂ ). Trajectories φ _Χο ( ) and <¾ ₀ (w ₂ ) are denoted by x-^ d x ₂ , respectively. The associated difference in cost is

ΙΛ ₀ ( ) = I / _[OT(Ui)] #( i(t), ( ) - 9(.X2(.t >u ₂ (t dp(t

Using the Lipschitz constant of g , the definition of d _u in equation (10), and the definition of d _x equation (12) the difference is bounded as follows:

[/*„("-.) -A _o ("2)1 ≤ I J _[0>T(Ui)] ½lki(t)-¾(t)ll2+½ll"i(t) - "2(t)ll2dM(t)

+ J[T( _UL ),T(« ₂ )] 9( ^χ 2 "2 ( )dM(t) I

<

+ ).T(« ₂ )] "2 (0)1 dM(t)

< L ₅ T(u ₁ )<i _x (x ₁ ,x ₂ ) +Lgd _u u ₁ ,u ₂ )

+ ).T(¾)] i0(*2(t o)i <*M(t)- [0130] Since x ₂ is continuous, w ₂ is bounded, and g is continuous, there exists a bound G on g(x ₂ (t), u ₂ (t))ior t £ [τ( ), T(U ₂ )]. Thus, the difference in cost is further bounded by l/x ₀ ( ^M i) (16)

From the continuity of φ _Χο , for u ₂ ) sufficiently small the resulting trajectories will satisfy L _Q T u ₁ )d _x x ₁ , x ₂ ) < ε/3. For , w ₂ additionally satisfying d ₁ (u ₁ ,u ₂ ) <— 3 G ¹ and

"

<— we have G T U ₂ )— τ( )) < ε/3 and L _g d _u (u ₁ ,u ₂ ) < ε/3. For such a selection of u _t ,u ₂ , \J _Xo (u _t )—J _Xg (u ₂ )\ < ε. Thus, / _Xo is continuous.

Lemma 2: For any u E 1Lf _eas and x ₀ , z ₀ £ IR ⁿ , ") - _o ≤ H*o -Z0II2 -^(e ¹ ^ ⁾ - 1) (17)

[0131] The difference is bounded using the Lipschitz continuity of g. This is further bounded using equation (13). Denoting x(t) = φ _Χο (μ) and z(t) = φ _Ζο (μ),

Ι/*ο(") - ^ζ ₀ (")Ι = I /[O,T(U)]

^{≤ "} [o,T(«)] l ( ^x ( ^f )' "(0) - #0( , 1<*μ(

^≤ J _[0>T(ji)] ¾ll*( -z(t)ll2dM( (is)

ll½ -¾ll ₂ ¾(^ ^T(u) - i). Properties of the Approximation of Ii by U _R

[0132] Lemma 3 is not a statement about the dispersion of l _R m ίί which does not actually converge. For numerical approximations of function spaces the weaker statement that lim sup _R→∞ l _R is dense in ίί will be sufficient. Equivalently,

Lemma 3: For each u £ Hand ε > 0, there exists 7? ^* > 0 such that for any R > R ^* there exists w E l _R such that d _u (u, w) < ε.

By Lusin's Theorem, there exists a continuous u: [0, T(M)] → Ω such that

({t £ [0, z(u)] : u(t)≠ u(t)}) < (19)

[0133] The domain of υ is compact so υ is also uniformly continuous. Its modulus of continuity is denoted by 5(e),

\σ-γ\ <δ(ε)→ \\υ(σ)-υ(γ)\\ ₂ <ε. (20)

To construct an approximation of uby w £ l _R choose R sufficiently large so that (i) h(R)/ R > T(U), (ii) there exists an integer rsuch that 0 < τ(μ)— r/R < 1/R < <5(^~^), and (iii) the dispersion of J¾in His less than

6T(U) ^'

Then tor t€ i - l)/R _> i/R) i = there exists € OR uch that

Select w i½ which is e ual to v.; 00 each Interval. Combinin (i>- uii). d _u (v, w) - ω(ί) \\ _2ι άμ(ί) + u _max \r/R - τ(ω)

<

[0134] Thus, by the triangle inequality

d _u (u, w) < du(u, v) + d(v, w) < ε. (22) cZ(-) and int(-) denote the closure and interior of subsets of 11.

Lemma 4: For any w £ cl(int( l _goa i)) and ε > 0 there exists R ^* > 0 such that for any ? > R ^*

ω E cl(int( lg _0a i)) implies that for all δ > 0, Β _δ/2 (ω) Π int(V, _goal )≠ 0. Then each v E int(TIg _0a i) has a neighborhood B _p (v) c int(TI _goa i) with p < Take u £ β _δ/2 (ω) Π int(lL _goal y By Lemma 3, for sufficiently large R, there exists u £ t^such that u) < 5/2 which implies u £ B _p (v) c int(TI _goa i). Then u £ 1^ _oai and £½ (ω, w) < £½(ω, u) + u) < 5. Now by the continuity of J _Xlc ior (^sufficiently small, <¾(ω, υ) < δ implies ~ Jx _lc ( ^w ) I < ^ε f ^rom which the result follows.

[0135] A sufficient condition for every ω E 1i _goa i to be contained in the closure of the interior of V- _goa i is that ^goai ^De open which is the case when Assumption A-1 is satisfied.

Pruning U _p with the GLC Condition

[0136] To describe trajectories remaining on the ε-interior of Xf _ree at each instant and which terminate on the ε-interior of ¾ _oai the following sets are defined,

and similarly for c _R , <fp '■= . (25)

Since U _goa i ^c ^goai ^WQ have that 0 < c _R < c _R .

€(R

Lemma 5: If \im _R→00 e(R) = Othen lim _j?→∞ c _fi = c ^* .

[0137] By the definition of c ^* in equation (5) , for any ε > 0 there exists ω E V- _goa i such that J _X1C (CL> — ε/2 < c ^* . Since V- _goa i is °P ^en and ψ _Χί( . is continuous there exists > 0 and p > 0 such that B _f (ω) c 1^ _oai and <p _Xic ( _f (ω)) c β _ρ ((? _¾ (ω)). Thus, ω £ 1^ _oai .

Similarly, there exists a positive r < such that (p _Xlc (B _r (a>)) c Β _ρ/2 (φ _Χΐ(: (ω)) and Β _Γ (ω) c

Ugoai- From the continuity of / in Lemma 1 there also exists a positive δ < r such that for any action history υ with d _u (v, ω) < δ we have |/ _Χί(; (ω)—J _Xt (v) \ < ε/2.

[0138] Next, choose 7? ^* to be sufficiently large such that R > R ^* implies e(R) < p/2 and Β _δ (ω) n l _R ≠ 0. Such a resolution R ^* exists by Lemma 4 and the assumption

1ίιη _β→∞ ε( ?) = 0. Now choose u E Β _δ (ω) Π l _R . Then \J _Xic (u)— J _x (ω) \ < ε/2 and

« ^e ¾ ⁷ ₍ ^c ¾o r T en by definition of u £ implies c ⁰ < Finally, by triangle inequality,

\] _x . _c (u) - c ^* \ < \] _Xic (u) + - c ^* | < ε. (26)

Rearranging the expression yields J _Xlc (u) < c ^* + eand thus, c _R ^ < c ^* + ε. The result follows since the choice of ε is arbitrary.

[0139] To simplify the notation in what follows, a concatenation operation on elements of [0140] The concatenation operation will be useful together with the following equalities which are readily verified,

Jx ₀ ( ^u l ^u 2) ⁼ / _¾ ( ^u l) + /[¾ ₀ (u ₁ )](r( ₁ )) (¾) ( ²⁸ ) <¾ ₀ Ol"2) = <Ρχ ₀ ( )<Ρ[<ρ _¾0 ( _Μι )](τ(Μ)) (Μ2)- (29)

[0141] The concatenation operation on X _Xg m equation (29) is defined in the same way as in equation (27). H ^L r ^h(R)

Lemma 6: Let <5 ₀ =—— -e R . For ε≥ 5 ₀ and W;, u,- £ 1L _R Π Uf _ea s, if ^u i ^< R ^u i, ^men f° ^r each descendant of w ₇ in cost ^c i ≤ j.

There is a w £ Ή _β such that Ujw £ ZL _R is a descendant of u _; -and M _; W £ 1i _g ^£ _oal . Since Uj ~ _; -,

Then, by equation (13), for all t £ [0, T(W)] C [0, h(R)/R], (31)

Then for δ ₀ = e R we have UjW £ ΐ^ _οα ° .As for the cost, from equation (28)

Jx _lc (UiW) = /x _ic Oi) +/[¾ _ic (u _i )](T(u _i ))( )

< Jx _ic (. ^u j + J[<p _Xlc (Uj)](T(Uj))(w)

< Jx _lc i ^U i ^W )

[0142] The first step applies the conditions for u _t < _R Uj. The second step combines Lemma

2 and u ~ w,-

[0143] In the above Lemma, the quantity δ ₀ was constructed based on the radius of the partition of _ree and the sensitivity of solutions to initial conditions. Let 8 _k be defined as a finite sum of related quantities, ^{0,i *} The following bound will be useful to state our main result in the next Theorem,

This is derived as follows

(e R - 1).

LfV(R) Theorem 2: Let e(R) = fi — 1). Algorithm 1 terminates in finite time and

returns a solution with cost less than or equal to c _R .

[0144] The queue is a subset of 1i _R U {Idy and at line 3 in each iteration a lowest cost action history wis removed from the queue. In line 13, only children of the current action history u are added to the queue. Since 1i _R is organized as a tree and has no cycles, any action history u will enter the queue at most once. Therefore the queue must be empty after a finite number of iterations so the algorithm terminates.

[0145] Next, as a point of contradiction consider the hypothesis that the output has cost greater than c _R . Then c _R <∞ and by the definition of c _R in equation (25), 11 _R Π ^u foai ^is non-empty.

[0146] Action histories u ₀ £ Zl _R Π U _g0 ai with ^c °st Jx _lc ( ^u o) ^{= C} R ^are descendants of ld _u which is initially present in the queue. The hypothesis prohibits these action histories from entering the queue. Otherwise, by equation (9) they would be evaluated before any

€(R

action history of cost greater than c _R . To prevent these action histories from entering the queue an action history with dept i( ) > 1 must at some iteration be present in∑ with ^u i ^< R ^a o f° ^{r m} ancestor ₀ of each u ₀ . By line 10, having present in∑ allows action histories in the sub-tree rooted at ₀ containing each u ₀ to be prevented from entering the queue. From Lemma 6, each u _t has a descendant in l _R of the form W-L CZ-L SUCII that u _t d _t £

^goai ^{5l an} d /x _jC ( di)≤ ^C R ^{< R} ^ ^5L - To be present in∑, an action history w-j must at some point be present in the queue as well by line 10. The hypothesis implies that u _t d _t does not enter the queue which is only possible by the same mechanism as for u ₀ . Suppose that repeating the process k < h(R) times reveals that an action history u _k with depth(u _k )≥ k must at some iteration be present in the queue and that there is a descendant u _k d _k £ l _R n ¾^ ^"¾ with cost J _Xlc u _k d _k )≤ c _R ^(R) . Note, S _k ≤ e(R) for any k≤ h(R) by equation (33).

[0147] The hypothesis implies that an ancestor _fc of u d must be prevented from entering the queue in lines 9-10 by evaluating u _k+1 < _R a _k for some u _k+1 £∑ with depth(u _k+1 ) < k + 1. Again, by Lemma 6, a descendant of u _k+1 of the form u _k+1 d _k+1 will be in V-^ai ^5k+1 with cost Jx _{lc k+1} d _k+1 )≤ c _R ^ ^5k+1 so that some ancestor of _k+1 d _k+1 must be prevented from entering the queue. [0148] By induction on k, this procedure can continue up to k = h(R) at which point there exist action histories for the descendant of in ^{5h R)~1} . By Lemma 6 u _h ^ < _R ¾ _(β) -ι<ί _/ ι _(β) -ι implies ι½ _(β) has a descendant in U _goa i. However, u _h ^ has no descendants; a contradiction.

[0149] The choice of e(R)in Theorem 2 converges to zero by equation (8). Then by Lemma 5 we have c _R ^→ c ^* .

[0150] The inventive method does not require a point-to-point local planning subroutine. Moreover, numerical experiments demonstrate that the inventive method is considerably faster the conventional SST algorithm, and is more broadly applicable than the conventional i?i?r ^* algorithm. In addition, the cost of the feasible solutions returned by the inventive method converge to the optimal cost for the problem. [0151] Some entities or components described herein may be associated with or operate one or more computer apparatuses to facilitate the functions described herein. Some of the entities or components described herein, including any server or database, may use any suitable number of subsystems to facilitate the functions. [0152] Examples of such subsystems or components can be interconnected via a system bus. Additional subsystems such as a printer, keyboard, fixed disk (or other memory comprising computer readable media), monitor, which is coupled to display adapter, and others are shown. Peripherals and input output (I/O) devices, which couple to I/O controller (which can be a processor or other suitable controller), can be connected to the computer system by any number of means known in the art, such as serial port. For example, serial port or external interface can be used to connect the computer apparatus to a wide area network such as the Internet, a mouse input device, or a scanner. The interconnection via system bus allows the central processor to communicate with each subsystem and to control the execution of instructions from system memory or the fixed disk, as well as the exchange of information between subsystems. The system memory and/or the fixed disk may embody a computer readable medium.

[0153] It should be understood that the present invention as described above can be implemented in the form of control logic using computer software (stored in a tangible physical medium) in a modular or integrated manner. Based on the disclosure and teachings provided herein, a person of ordinary skill in the art will know and appreciate other ways and/or methods to implement the present invention using hardware and a combination of hardware and software.

[0154] Any of the software components or functions described in this application, may be implemented as software code to be executed by a processor using any suitable computer language using, for example, conventional or object-oriented techniques. The software code may be stored as a series of instructions, or commands on a computer readable medium, such as a random access memory (RAM), a read only memory (ROM), a magnetic medium such as a hard-drive or a floppy disk, or an optical medium such as a CD-ROM. Any such computer readable medium may reside on or within a single computational apparatus, and may be present on or within different computational apparatuses within a system or network.

[0155] The above description is illustrative and is not restrictive. Many variations of the invention will become apparent to those skilled in the art upon review of the disclosure. The scope of the invention should, therefore, be determined not with reference to the above description, but instead should be determined with reference to the pending claims along with their full scope or equivalents.

[0156] One or more features from any embodiment may be combined with one or more features of any other embodiment without departing from the scope of the invention.

[0157] A recitation of "a", "an" or "the" is intended to mean "one or more" unless specifically indicated to the contrary.

[0158] All patents, patent applications, publications, and descriptions mentioned above are herein incorporated by reference in their entirety for all purposes. None is admitted to be prior art.