WO2017139613A1 - Motion planning for robotic systems - Google Patents

Motion planning for robotic systems Download PDF

Info

Publication number
WO2017139613A1
WO2017139613A1 PCT/US2017/017420 US2017017420W WO2017139613A1 WO 2017139613 A1 WO2017139613 A1 WO 2017139613A1 US 2017017420 W US2017017420 W US 2017017420W WO 2017139613 A1 WO2017139613 A1 WO 2017139613A1
Authority
WO
WIPO (PCT)
Prior art keywords
cost value
trajectory
intermediate region
trajectory segment
segment
Prior art date
Application number
PCT/US2017/017420
Other languages
French (fr)
Inventor
Brian Alexander PADEN
Emilio FRAZZOLI
Dmytro S. YERSHOV
Original Assignee
Massachusetts Institute Of Technology
Priority date (The priority date is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the date listed.)
Filing date
Publication date
Application filed by Massachusetts Institute Of Technology filed Critical Massachusetts Institute Of Technology
Publication of WO2017139613A1 publication Critical patent/WO2017139613A1/en

Links

Classifications

    • GPHYSICS
    • G05CONTROLLING; REGULATING
    • G05DSYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
    • G05D1/00Control of position, course or altitude of land, water, air, or space vehicles, e.g. automatic pilot
    • G05D1/02Control of position or course in two dimensions
    • G05D1/021Control of position or course in two dimensions specially adapted to land vehicles
    • G05D1/0212Control of position or course in two dimensions specially adapted to land vehicles with means for defining a desired trajectory
    • G05D1/0217Control of position or course in two dimensions specially adapted to land vehicles with means for defining a desired trajectory in accordance with energy consumption, time reduction or distance reduction criteria
    • BPERFORMING OPERATIONS; TRANSPORTING
    • B25HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
    • B25JMANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
    • B25J9/00Programme-controlled manipulators
    • B25J9/16Programme controls
    • B25J9/1656Programme controls characterised by programming, planning systems for manipulators
    • B25J9/1664Programme controls characterised by programming, planning systems for manipulators characterised by motion, path, trajectory planning
    • GPHYSICS
    • G01MEASURING; TESTING
    • G01CMEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C21/00Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G01C21/26Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 specially adapted for navigation in a road network
    • G01C21/34Route searching; Route guidance
    • GPHYSICS
    • G05CONTROLLING; REGULATING
    • G05BCONTROL OR REGULATING SYSTEMS IN GENERAL; FUNCTIONAL ELEMENTS OF SUCH SYSTEMS; MONITORING OR TESTING ARRANGEMENTS FOR SUCH SYSTEMS OR ELEMENTS
    • G05B2219/00Program-control systems
    • G05B2219/30Nc systems
    • G05B2219/40Robotics, robotics mapping to robotics vision
    • G05B2219/40465Criteria is lowest cost function, minimum work path

Definitions

  • 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.
  • 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.
  • 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.
  • Embodiments of the invention address this and other problems, individually and collectively.
  • 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
  • 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.
  • 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.
  • 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.
  • 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.
  • 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.
  • 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.
  • 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.
  • 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.
  • the second trajectory segment is discarded.
  • 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.
  • 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.
  • 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.
  • a second intermediate region between the first intermediate region and the one of the plurality of destination states is identified.
  • 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.
  • 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.
  • 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.
  • 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.
  • 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.
  • 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.
  • 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.
  • 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.
  • 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.
  • 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.
  • 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.
  • 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.
  • 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)
  • the function u represents the entire action history.
  • 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.
  • 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.
  • each actuator of a robot affords an infinite number of actions.
  • 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.
  • 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.
  • 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.
  • 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.
  • 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.
  • 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.
  • 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.
  • 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.
  • 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.
  • FIGs. 1A and IB show exemplary trajectories for motion planning between a starting state and a plurality of destination states.
  • 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.
  • FIG. 3 shows a flowchart of motion planning for a robotic system
  • 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.
  • 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.
  • FIG. 6 shows a block diagram of an exemplary motion planning and control system.
  • FIG. 7A shows an approximation of an exemplary action history to a robotic system.
  • FIG. 7B shows exemplary trajectories resulting from two distinct action histories similar to the one illustrated in FIG. 7A.
  • FIG. 7C shows an exemplary partition in the state space containing the end points of the trajectories illustrated in FIG. 7B.
  • FIG. 8 shows an exemplary motion planning for shortest path problem using embodiments of the present invention.
  • FIG. 9 shows an exemplary motion planning for the torque limited pendulum swing- up problem using embodiments of the present invention.
  • FIG. 10 shows an exemplary motion planning for an acrobot using embodiments of the present invention.
  • 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.
  • an acceleration limited 3D point robot e.g. a quadcopter with high bandwidth attitude control
  • FIG. 12 shows an exemplary motion planning for a nonholonomic wheeled robot using embodiments of the present invention.
  • Embodiments provide a motion planner which, with sufficiently high
  • 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.
  • 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.
  • 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
  • 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.
  • the selection of h for each resolution r is to assign h(R) equal to the resolution multiplied by the logarithm of the resolution.
  • 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.
  • 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.
  • 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.
  • 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.
  • 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.
  • an initial state e.g. starting state
  • intermediary states 102 and 104 provided between the initial state 100 and the goal destination states 1 10.
  • the trajectory segment 114 may be discarded.
  • 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.
  • 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.
  • 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.
  • 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.
  • 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
  • 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.
  • 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.
  • 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.
  • 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.
  • 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.
  • 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).
  • the trajectories 312 may be sequentially evaluated.
  • the trajectories resulting in the same partition may be compared against each other.
  • costs of trajectories terminating in the same partition may be compared to determine if the difference in cost exceeds a predetermined cost threshold.
  • 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
  • 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.
  • 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.
  • 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.
  • 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.
  • 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.
  • 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.
  • 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.
  • 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.
  • 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.
  • the predetermined threshold depends upon at least a size of the first intermediate region.
  • 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.
  • 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.
  • a reference cost value assigned to the first intermediate region is determined.
  • the reference cost value of the first intermediate region may be the cost value of another trajectory segment.
  • the reference cost value of the first intermediate region may be infinity.
  • reference cost values of all partitions in the state space representation may be set to infinity.
  • 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.
  • the first cost value of the first trajectory is compared to the reference cost value.
  • 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.
  • the first trajectory segment is discarded in favor of the trajectory segment with lower cost.
  • the trajectory segment under evaluation e.g. "the first trajectory segment
  • 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.
  • 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.
  • the trajectory segment under evaluation e.g. "the first trajectory segment
  • 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.
  • 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.
  • the trajectory segment under evaluation e.g. "the first trajectory segment
  • 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.
  • 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.
  • 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.
  • R E N is a resolution parameter.
  • 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.
  • 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.
  • 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.
  • 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.
  • 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.
  • 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.
  • 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.
  • 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.
  • Additional modules may also reside on the computer readable medium 614.
  • 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.
  • the data processor 610 e.g., a microprocessor
  • 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).
  • 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.
  • 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
  • NIC Network Interface Card
  • PCMCIA PCMCIA slot and card, or the like.
  • 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.
  • 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.
  • 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.
  • the configuration and relevant quantities of a system may be described by a state in M. n .
  • 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]
  • 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 0 parametrizes the map and trajectory space.
  • ⁇ ( ⁇ ) for a function xwith domain [t 1( t 2 ] denotes the maximum of the domain, t 2 .
  • 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
  • the subset 3 ⁇ 4 oai of M. n 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.
  • action histories mapping to trajectories in Xf eas /goai are defined by the inverse relation * * «»» .. ⁇ - s «» s -fa**/
  • 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 0 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 0 at time t.
  • the sets X ree and X goa i are open with respect to the standard topology on M n .
  • the reachable set is not required to have a nonempty interior as in the kinodynamic variant of RRT* and SST.
  • the action history space TI is approximated by a finite subset 1i R where R E M is a resolution parameter.
  • R E M is a resolution parameter.
  • 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 ⁇ .
  • the approximated action history space 1i R consists of piecewise constant action 1 )
  • the constant values of the action history are actions from ⁇ .
  • N ⁇ N defines a horizon limit and can be any function satisfying
  • 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.
  • w is
  • 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.
  • ld u has no parent, but is the parent of action histories with domain [0, c/R] .
  • u is a descendant of w.
  • the depth of an action in t ⁇ is the number of ancestors of that action.
  • 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.
  • the radius of the partition is parametrized by the resolution R.
  • hypercube partitions may be considered.
  • a user specified function ⁇ : N ⁇ M >0 controls the side length of the hypercube.
  • 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.
  • 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 2 , 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 n into ⁇ ⁇ 720 to identify that the terminal states are elements of the same region of predetermined size.
  • An action history is called minimal if there is no u 2 E 1i R such that u 2 ⁇ 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
  • 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.
  • equation (8) and [condition 3] simplify in some cases.
  • h j 0.
  • L j 1 in equation (4)
  • L g 0.
  • [condition 3] simplifies to / Xic ( ) ⁇ J Xlc (u 2 ).
  • 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
  • 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.
  • 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
  • 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
  • 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.
  • 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.
  • 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.
  • 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.
  • 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.
  • the free space 801 and goal 806 are illustrated in FIG. 8.
  • FIG. 9 shows an exemplary motion planning for the torque limited pendulum swing- up problem using embodiments of the present invention.
  • 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.
  • 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 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
  • the inventive method arrives at a solution with less construction time 1002 than the prior art 1004.
  • FIG. 11 shows an exemplary motion planning problem for an acceleration limited 3D point robot model using embodiments of the present invention.
  • 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 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
  • 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 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 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.
  • the running time for the inventive method to produce a (visually) good quality trajectory is roughly equal to the execution time.
  • 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.
  • 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.
  • the map (3 ⁇ 4- 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 ⁇ ⁇
  • Lemma 1 J Xo '-U ⁇ IK.is continuous for any x 0 EK n .
  • Lemma 2 For any u E 1Lf eas and x 0 , z 0 £ IR n , ") - o ⁇ H*o -Z0II2 - ⁇ (e 1 ⁇ ) - 1) (17)
  • 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) ⁇ ⁇ .
  • 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.
  • Jx lc (UiW) /x ic Oi) +/[3 ⁇ 4 ic (u i )](T(u i ))( )
  • the first step applies the conditions for u t ⁇ R Uj.
  • the second step combines Lemma
  • 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.
  • 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.
  • I/O controller which can be a processor or other suitable controller
  • serial port 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.
  • 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.
  • RAM random access memory
  • ROM read only memory
  • magnetic medium such as a hard-drive or a floppy disk
  • 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.

Abstract

Disclosed is 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 trajectories resulting from each of the finite set of action histories. Embodiments provide a procedure for constructing, in a finite amount of time, approximately minimum cost action histories resulting in trajectories meeting a motion planning specification.

Description

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 (Lfh(R)/R) _
LfT)(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 1iR where R E N is a resolution parameter. To construct 1iR, 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 1iR 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.n. 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 XXQ . The action history space is constructed from a set of admissible actions Ω c M.m bounded by umax. The action history space is defined as:
Figure imgf000021_0001
[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 φΧο : ίί→ XXg 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 x0 parametrizes the map and trajectory space. To simplify notation, τ(χ) for a function xwith domain [t1( t2] denotes the maximum of the domain, t2.
[0086] In addition to the differential constraint, feasible trajectories for a particular problem satisfy point-wise constraints defined by a subset Xfree i I and a specified initial state xic. The subset of feasible trajectories X^easare defined
'= { e : .·:( * > e X ^ Vi G ; . (.<' ?] ) . (3)
[0087] Similarly, the subset ¾oaiof M.n is used to encode a terminal constraint. The subset of Xfeas consisting of trajectories x with χ(τ(χ)) £ Xgoai defines Xgoai- The decision variable for the problem is the action history u which is chosen such that the trajectory (pXic(u) is in Xgoai. Naturally, action histories mapping to trajectories in Xfeas/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,
Jx0M = J[0>T(u)]flf([^o(u](t,"( )dM(t)- (4) The notation [(pXo(u)](t) denotes the evaluation of the trajectory satisfying equation (2) with the action history u and initial state x0 at time t.
[0089] The domain of / is extended with the object NULL such that ]Xq (NULL) =∞ for all x0 E M.71. 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-yJuft)— m " i ,/*K («) ;= -c*. (5) [0090] With the convention that infUE0Jx.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 Xree and Xgoai are open with respect to the standard topology on Mn.
2. There are known constants Lj > 0 and M≥ 0 such that u)— f(x2, ) ||2 <
Lf\\(xi ~ x2 \\2, and || (xi,u)||2 < for all x1,x2 E Xfreemdu E Ω.
3. There is a known constant Lg≥ 0 such that
Figure imgf000022_0001
)— 5,( 2, 2) ||2 <
¾ll(xi— x2> ui ~ M2)ll2r all X ,x2 E Xfree d u1,u2 E Ω.
4· JXlc ) > 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 1iR where R E M is a resolution parameter. To construct 1iR, 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 1iR 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 £ 1iR 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 lR 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, 1iR is augmented with the special action history ldu defined such that ]Xo ( du) = 0 and [φΧο (Ι du)] (0) = x0. ldu 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 XfrPPand the GLC Conditions
[0097] A partition of Xfree 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 p1( p2 £ Iinwe write px ~ p2 if
Figure imgf000023_0001
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 ult u2 £ URwe write ~ u2ii the resulting trajectories terminate in the same hypercube. That is,
¾ R
Ul ~ u2 <=> [<¾, ( )] (τ( )) ~ WXI U2)] {T{U2)). (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 1iR, FIG. 7B illustrates the mapping, determined by the dynamic model, of two equivalent action histories ux ~ u2, into the trajectory space XXic resulting in trajectories 710 and 712. FIG. 7C illustrates the mapping from terminal states 706 and 708 of trajectories 710 and 712 respectively in Mn 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 u2 if the GLCconditions are satisfied. The GLC conditions are:
1. [condition 1] ~ u2,
2. [condition 2] τ( ) < τ(μ2),
3. [condition 3] + (e— - 1) < /¾ (u2)
[0102] An action history is called minimal if there is no u2 E 1iR such that u2 <R ut. 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
Figure imgf000024_0001
[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 hj = 0. Taking the limit Lj→ 0, the constraint (8) becomes ϊι(β)/η(Κ)→ 0. The second special case is minimum-time problems where g(x, u) = 1 in equation (4) so that Lg = 0. Then [condition 3] simplifies to /Xic( )≤ JXlc(u2).
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})
Figure imgf000025_0001
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 1iq0ai 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 wR is the action history returned by the motion planning method, for resolution R, then
\imR→∞JXlc (wR) = 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 Xfree and Xgoai are described by algebraic inequalities. The approximation of the input space Ω is constructed by uniform deterministic sampling of i?mcontrols from Ω, where m is the dimension of the input space.
[0112] Evaluation of u E V-feas and u E V-goai is approximated by first numerically approximating cpXic (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.2 : \\u \\2 = 1}. The free space 801 and goal 806 are illustrated in FIG. 8. The parameters for the inventive method are c = 10,
77 (Λ) = 300i?2, 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))Twith the running-cost g(6, ω, w) = 1 and input space Ω = [—0.2,0.2]. The free space is modeled as Xfree = .2. The initial state is xic = (0,0) and the goal region is
Xgoai = {χ e 11( ± it, ω ||2≤ 0.1}. The parameters for the GLC method are c = 6, η Κ) = 16R2-5, 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, η^ = 16R2, 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||2)T where x, v, and ware each elements of W3; 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.3 : \\u \\2≤ 1}. The free space and goal are illustrated in 1104. The parameters for the inventive method are c = 6, η (R) = 16R2, 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 + 2u2, 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 lR for a fixed R. Since 1iR <£ UR+1, it is possible that an optimal action history in lR may be better than any action history in 1iR+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
Figure imgf000030_0001
The family of trajectory spaces are more precisely defined as
Figure imgf000030_0002
where M is that of assumption A-2. This set is equipped with the metric
dx(x1, x2)≡ .n . max {||*i (t) - x2 (t) ll} + M \ T{X ) - τ(χ2) | . [0127] Several known continuity properties of <¾oare reviewed here. The distance between solutions to equation (2) with initial conditions x0 and z0 is bounded by ll *0(")](t) - z0(")](t)ll2≤ ΙΙχο -zolbe^.
[0128] In addition to continuous dependence on the initial condition parameter, the map (¾-ois also continuous from ίί into XXq . Xfeas is open when assumption A-l is satisfied since it follows directly from the definition of continuity that Ufeas and Ugoai are open subsets of 11. They are defined as the preimage of Xfeas and Xgoai under φχ
[0129] Similar observations for the cost functional are developed below.
Lemma 1: JXo'-U IK.is continuous for any x0 EKn.
Let , u2 Είί and without loss of generality, τ( ) < T( 2). Trajectories φΧο ( ) and <¾0(w2) are denoted by x-^ d x2, respectively. The associated difference in cost is
ΙΛ0( )
Figure imgf000031_0001
= I /[OT(Ui)]#( i(t), ( ) - 9(.X2(.t >u2(t dp(t
Using the Lipschitz constant of g , the definition of du in equation (10), and the definition of dx equation (12) the difference is bounded as follows:
[/*„("-.) -Ao ("2)1 ≤ I J[0>T(Ui)]½lki(t)-¾(t)ll2+½ll"i(t) - "2(t)ll2dM(t)
+ J[T(UL),T(«2)] 9(χ2 "2 ( )dM(t) I
<
Figure imgf000031_0002
+ ).T(«2)] "2 (0)1 dM(t)
< L5T(u1)<ix(x1,x2) +Lgdu u1,u2)
+ ).T(¾)] i0(*2(t o)i <*M(t)- [0130] Since x2 is continuous, w2 is bounded, and g is continuous, there exists a bound G on g(x2(t), u2(t))ior t £ [τ( ), T(U2)]. Thus, the difference in cost is further bounded by l/x0(Mi)
Figure imgf000031_0003
(16)
From the continuity of φΧο, for u2) sufficiently small the resulting trajectories will satisfy LQT u1)dx x1, x2) < ε/3. For , w2 additionally satisfying d1(u1,u2) <— 3 G1 and
"
<— we have G T U2)— τ( )) < ε/3 and Lgdu(u1,u2) < ε/3. For such a selection of ut,u2, \JXo(ut)—JXg(u2)\ < ε. Thus, /Xois continuous.
Lemma 2: For any u E 1Lfeas and x0, z0 £ IRn, ") - o ≤ H*o -Z0II2 -^(e1 ^) - 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) = φΖο(μ),
Ι/*ο(") - ζ 0(")Ι = I /[O,T(U)]
Figure imgf000032_0001
≤ "[o,T(«)] l (x(f)' "(0) - #0( , 1<*μ(
J[0>T(ji)]¾ll*( -z(t)ll2dM( (is)
Figure imgf000032_0002
ll½ -¾ll2¾(^T(u) - i). Properties of the Approximation of Ii by UR
[0132] Lemma 3 is not a statement about the dispersion of lRm ίί which does not actually converge. For numerical approximations of function spaces the weaker statement that lim supR→∞ lR 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 lR such that du(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),
\σ-γ\ <δ(ε)→ \\υ(σ)-υ(γ)\\2 <ε. (20)
To construct an approximation of uby w £ lR 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). du(v, w) - ω(ί) \\ άμ(ί) + umax \r/R - τ(ω)
Figure imgf000033_0001
<
[0134] Thus, by the triangle inequality
du(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( lgoai)) and ε > 0 there exists R* > 0 such that for any ? > R*
Figure imgf000033_0002
ω E cl(int( lg0ai)) implies that for all δ > 0, Βδ/2 (ω) Π int(V,goal)≠ 0. Then each v E int(TIg0ai) has a neighborhood Bp(v) c int(TIgoai) with p < Take u £ βδ/2 (ω) Π int(lLgoaly By Lemma 3, for sufficiently large R, there exists u £ t^such that u) < 5/2 which implies u £ Bp(v) c int(TIgoai). Then u £ 1^oaiand £½ (ω, w) < £½(ω, u) + u) < 5. Now by the continuity of JXlcior (^sufficiently small, <¾(ω, υ) < δ implies ~ Jxlc (w) I < ε from which the result follows.
[0135] A sufficient condition for every ω E 1igoai to be contained in the closure of the interior of V-goai is that ^goai De open which is the case when Assumption A-1 is satisfied.
Pruning Up with the GLC Condition
[0136] To describe trajectories remaining on the ε-interior of Xfree at each instant and which terminate on the ε-interior of ¾oaithe following sets are defined,
and similarly for cR, <fp '■= . (25)
Figure imgf000034_0001
Since Ugoai c ^goaiWQ have that 0 < cR < cR.
€(R
Lemma 5: If \imR→00e(R) = Othen limj?→∞cfi = c*.
[0137] By the definition of c* in equation (5) , for any ε > 0 there exists ω E V-goai such that JX1C (CL> — ε/2 < c*. Since V-goai is °Pen and ψΧί(. is continuous there exists > 0 and p > 0 such that Bf (ω) c 1^oaiand <pXic ( f (ω)) c βρ((?¾(ω)). Thus, ω £ 1^oai.
Similarly, there exists a positive r < such that (pXlc(Br(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 du(v, ω) < δ we have |/Χί(;(ω)—JXt (v) \ < ε/2.
[0138] Next, choose 7?* to be sufficiently large such that R > R* implies e(R) < p/2 and Βδ(ω) n lR≠ 0. Such a resolution R* exists by Lemma 4 and the assumption
1ίιηβ→∞ε( ?) = 0. Now choose u E Βδ(ω) Π lR. Then \JXic(u)— Jx (ω) \ < ε/2 and
« e ¾7 ( c ¾o r T en by definition of u £ implies c 0 < Finally, by triangle inequality,
\]x.c(u) - c* \ < \]Xic (u) + - c* | < ε. (26)
Rearranging the expression yields JXlc(u) < c* + eand thus, cR^ < c* + ε. The result follows since the choice of ε is arbitrary.
[0139] To simplify the notation in what follows, a concatenation operation on elements of
Figure imgf000034_0002
[0140] The concatenation operation will be useful together with the following equalities which are readily verified,
Jx0 (ulu2) = /¾ (ul) + /[¾0(u1)](r( 1)) (¾) (28) <¾0Ol"2) = <Ρχ0 ( )<Ρ[<ρ¾0 (Μι)](τ(Μ)) (Μ2)- (29)
[0141] The concatenation operation on XXgm equation (29) is defined in the same way as in equation (27). H Lrh(R)
Lemma 6: Let <50 =—— -e R . For ε≥ 50and W;, u,- £ 1LR Π Ufeas, if ui <R ui, menr each descendant of w7in
Figure imgf000035_0001
cost ci ≤ j.
There is a w £ Ήβ such that Ujw £ ZLR is a descendant of u;-and M;W £ 1ig £ oal. Since Uj ~ ;-,
Figure imgf000035_0002
Then, by equation (13), for all t £ [0, T(W)] C [0, h(R)/R], (31)
Figure imgf000035_0003
Then for δ0 = e R we have UjW £ ΐ^οα° .As for the cost, from equation (28)
Jxlc(UiW) = /xicOi) +/[¾ic(ui)](T(ui))( )
Figure imgf000035_0004
< Jxic(.uj + J[<pXlc(Uj)](T(Uj))(w)
< JxlciUiW)
[0142] The first step applies the conditions for ut <R Uj. The second step combines Lemma
2 and u ~ w,-
[0143] In the above Lemma, the quantity δ0 was constructed based on the radius of the partition of ree and the sensitivity of solutions to initial conditions. Let 8k be defined as a finite sum of related quantities, 0,i *
Figure imgf000035_0005
The following bound will be useful to state our main result in the next Theorem,
Figure imgf000035_0006
This is derived as follows
Figure imgf000035_0007
(e R - 1).
LfV(R) Theorem 2: Let e(R) = fi — 1). Algorithm 1 terminates in finite time and
Figure imgf000036_0001
returns a solution with cost less than or equal to cR .
[0144] The queue is a subset of 1iR 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 1iR 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 cR . Then cR <∞ and by the definition of cR in equation (25), 11R Π ufoai is non-empty.
[0146] Action histories u0 £ ZlR Π Ug0ai with c°st Jxlc(uo) = CR are descendants of ldu 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 cR . To prevent these action histories from entering the queue an action history with dept i( ) > 1 must at some iteration be present in∑ with ui <R ao f°r m ancestor 0 of each u0. By line 10, having present in∑ allows action histories in the sub-tree rooted at 0 containing each u0 to be prevented from entering the queue. From Lemma 6, each ut has a descendant in lR of the form W-L CZ-L SUCII that utdt £
^goai 5l and /xjC ( di)≤ CR< 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 utdt does not enter the queue which is only possible by the same mechanism as for u0. Suppose that repeating the process k < h(R) times reveals that an action history uk with depth(uk)≥ k must at some iteration be present in the queue and that there is a descendant ukdk £ lR n ¾^with cost JXlc ukdk)≤ cR (R). Note, Sk≤ e(R) for any k≤ h(R) by equation (33).
[0147] The hypothesis implies that an ancestor fcof u d must be prevented from entering the queue in lines 9-10 by evaluating uk+1 <R ak for some uk+1 £∑ with depth(uk+1) < k + 1. Again, by Lemma 6, a descendant of uk+1 of the form uk+1dk+1 will be in V-^ai 5k+1 with cost Jxlc k+1dk+1)≤ cR^ 5k+1 so that some ancestor of k+1dk+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
Figure imgf000037_0001
for the descendant of in 5h R)~1. By Lemma 6 uh^ <R ¾(β)-ι<ί/ι(β)-ι implies ι½(β) has a descendant in Ugoai. However, uh^ 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 cR^→ 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.

Claims

WHAT IS CLAIMED IS:
1. A method for motion planning for a robotic system proceeding from a starting state to one of a plurality of destination states, the method comprising:
identifying a first intermediate region between the starting state and one of the plurality of destination states;
identifying a first trajectory segment between the starting state and the first intermediate region;
calculating a first cost value associated with the first trajectory segment; determining whether the first trajectory segment is included in a final trajectory based on the first cost value and a predetermined threshold amount, wherein the predetermined threshold depends upon at least a size of the first intermediate region; and generating the final trajectory connecting the starting state to the one of the plurality of destination states.
2. The method of claim 1 , further comprising:
determining a reference cost value assigned to the first intermediate region; comparing the first cost value to the reference cost value;
discarding the first trajectory segment when the first cost value is greater than the reference cost value by at least the predetermined threshold amount, or
incorporating the first trajectory segment in the final trajectory 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.
3. The method of claim 1 , further comprising:
determining that the first trajectory segment is a first trajectory evaluated for the first intermediate region;
assigning the first cost value as a reference cost value to the first intermediate region; and
incorporating the first trajectory segment in the final traj ectory.
4. The method of claim 3, wherein determining that the first trajectory segment is the first trajectory evaluated for the first intermediate region is based on determining that a predetermined reference cost value assigned to the first intermediate region is infinity.
5. The method of claim 3, further comprising:
identifying a second trajectory segment between the starting state and the first intermediate region;
calculating a second cost value associated with the second trajectory segment; comparing the second cost value to the reference cost value;
when the second cost value is greater than the reference cost value by at least the predetermined threshold amount:
discarding the second trajectory segment, or
when the reference cost value is greater than the second cost value by at least the predetermined threshold amount:
incorporating the second trajectory segment in the final trajectory, discarding the first trajectory segment, and
assigning the second cost value as a new reference cost value to the first intermediate region.
6. The method of claim 5, wherein
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, the first state and the second state are within the first intermediate region.
7. The method of claim 1 , further comprising, prior to determining the first cost value:
determining that the first traj ectory segment satisfies previously determined feasibility constraints.
8. The method of claim 1 , further comprising:
incorporating the first trajectory segment in the final traj ectory based on the first cost value and the predetermined threshold amount;
determining a subsequent trajectory segment between the first intermediate region and the one of the plurality of destination states; and generating the final trajectory between the starting state and the one of the plurality of destination states by joining the first trajectory segment and the subsequent trajectory segment.
9. The method of claim 1 , further comprising:
identifying a second intermediate region between the first intermediate region and the one of the plurality of destination states;
identifying a second trajectory segment between the first intermediate region and the second intermediate region;
calculating a second cost value associated with the second trajectory segment; determining whether the second trajectory segment is discarded based on a sum of the first cost value and the second cost value, and the predetermined threshold amount.
10. The method of claim 9 further comprising:
determining a reference cost value assigned to the second intermediate region; comparing the sum of the first cost value and the second cost value to the reference cost value;
discarding the second trajectory segment 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, or
incorporating the second trajectory segment in the final trajectory and assigning the sum of the first cost value and the second cost value as a new reference cost value to the second intermediate region 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.
11. A motion planning system comprising:
a processor; and
a computer readable medium storing instructions that, when executed by the processor, cause the processor to perform a method comprising:
identifying a first intermediate region between the starting state and one of the plurality of destination states;
identifying a first trajectory segment between the starting state and the first intermediate region; calculating a first cost value associated with the first trajectory segment;
determining whether the first trajectory segment is included in a final trajectory based on the first cost value and a predetermined threshold amount, wherein the predetermined threshold depends upon at least a size of the first intermediate region; and
generating the final trajectory connecting the starting state to the one of the plurality of destination states.
12. The system of claim 11 , wherein the method performed by the processor further comprises:
determining a reference cost value assigned to the first intermediate region; comparing the first cost value to the reference cost value;
discarding the first trajectory segment when the first cost value is greater than the reference cost value by at least the predetermined threshold amount, or
incorporating the first trajectory segment in the final trajectory 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.
13. The system of claim 1 1, wherein the method performed by the processor further comprises:
determining that the first trajectory segment is a first trajectory evaluated for the first intermediate region;
assigning the first cost value as a reference cost value to the first intermediate region; and
incorporating the first trajectory segment in the final trajectory.
14. The system of claim 13, wherein determining that the first trajectory segment is the first trajectory evaluated for the first intermediate region is based on determining that a predetermined reference cost value assigned to the first intermediate region is infinity.
15. The system of claim 13, wherein the method performed by the processor further comprises: identifying a second trajectory segment between the starting state and the first intermediate region;
calculating a second cost value associated with the second trajectory segment; comparing the second cost value to the reference cost value;
when the second cost value is greater than the reference cost value by at least the predetermined threshold amount:
discarding the second trajectory segment, or
when the reference cost value is greater than the second cost value by at least the predetermined threshold amount:
incorporating the second trajectory segment in the final trajectory, discarding the first trajectory segment, and
assigning the second cost value as a new reference cost value to the first intermediate region.
16. The system of claim 1 1, wherein the method performed by the processor further comprises, prior to determining the first cost value:
determining that the first traj ectory segment satisfies previously determined feasibility constraints.
17. The system of claim 1 1, wherein the method performed by the processor further comprises:
incorporating the first trajectory segment in the final trajectory based on the first cost value and the predetermined threshold amount;
determining a subsequent trajectory segment between the first intermediate region and the one of the plurality of destination states; and
generating the final trajectory between the starting state and the one of the plurality of destination states by joining the first trajectory segment and the subsequent trajectory segment.
18. The system of claim 1 1, wherein the method performed by the processor further comprises:
identifying a second intermediate region between the first intermediate region and the one of the plurality of destination states;
identifying a second trajectory segment between the first intermediate region and the second intermediate region; calculating a second cost value associated with the second trajectory segment; determining whether the second trajectory segment is discarded based on a sum of the first cost value and the second cost value, and the predetermined threshold amount.
19. The system of claim 18, wherein the method performed by the processor further comprises:
determining a reference cost value assigned to the second intermediate region; comparing the sum of the first cost value and the second cost value to the reference cost value;
discarding the second trajectory segment 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, or
incorporating the second trajectory segment in the final trajectory and assigning the sum of the first cost value and the second cost value as a new reference cost value to the second intermediate region 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.
20. A motion planning and control system comprising the motion planning system according to claim 11 and a motion control system for realizing the final trajectory determined by the motion planning system.
PCT/US2017/017420 2016-02-11 2017-02-10 Motion planning for robotic systems WO2017139613A1 (en)

Applications Claiming Priority (2)

Application Number Priority Date Filing Date Title
US201662293843P 2016-02-11 2016-02-11
US62/293,843 2016-02-11

Publications (1)

Publication Number Publication Date
WO2017139613A1 true WO2017139613A1 (en) 2017-08-17

Family

ID=59563542

Family Applications (1)

Application Number Title Priority Date Filing Date
PCT/US2017/017420 WO2017139613A1 (en) 2016-02-11 2017-02-10 Motion planning for robotic systems

Country Status (1)

Country Link
WO (1) WO2017139613A1 (en)

Cited By (17)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN109375626A (en) * 2018-11-20 2019-02-22 深圳市海柔创新科技有限公司 Alignment code is sticked method, apparatus, computer equipment and storage medium
CN109955250A (en) * 2019-01-21 2019-07-02 中国船舶重工集团公司第七一六研究所 Tracking applied to industrial robot reacts planning algorithm with Real Time Obstacle Avoiding
US20190244111A1 (en) * 2018-02-02 2019-08-08 University Of New Hampshire Avoiding dead ends in real-time heuristic search
US10480947B2 (en) * 2016-12-21 2019-11-19 X Development Llc Boolean satisfiability (SAT) reduction for geometry and kinematics agnostic multi-agent planning
WO2020005993A1 (en) * 2018-06-25 2020-01-02 X Development Llc Robot coordination in a shared workspace
CN110695994A (en) * 2019-10-08 2020-01-17 浙江科技学院 Finite time planning method for cooperative repetitive motion of double-arm manipulator
EP3648021A1 (en) * 2018-10-30 2020-05-06 Aptiv Technologies Limited Generation of optimal trajectories for navigation of vehicles
EP3666476A1 (en) * 2018-12-14 2020-06-17 Toyota Jidosha Kabushiki Kaisha Trajectory generation system and trajectory generating method
WO2020245769A1 (en) * 2019-06-04 2020-12-10 Motional Ad Llc Autonomous vehicle operation using linear temporal logic
CN112703147A (en) * 2018-09-25 2021-04-23 三菱电机株式会社 System and method for controlling movement of a vehicle
CN113255967A (en) * 2021-04-28 2021-08-13 北京理工大学 Task planning method and device based on end point backtracking under signal time sequence logic constraint
CN114274148A (en) * 2022-02-15 2022-04-05 中科新松有限公司 Trajectory planning method and device, electronic equipment and storage medium
CN114326767A (en) * 2021-12-14 2022-04-12 宁波天擎航天科技有限公司 Aircraft attitude control method, aircraft attitude control system and aircraft
US11526791B2 (en) 2019-11-11 2022-12-13 International Business Machines Corporation Methods and systems for diverse instance generation in artificial intelligence planning
US11796332B2 (en) 2018-10-30 2023-10-24 Motional Ad Llc Generation of optimal trajectories for navigation of vehicles
US11803184B2 (en) 2018-12-18 2023-10-31 Motional Ad Llc Methods for generating maps using hyper-graph data structures
WO2024045091A1 (en) * 2022-08-31 2024-03-07 西门子股份公司 Motion planning method, apparatus and system for actuator, and storage medium

Citations (12)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US4773025A (en) * 1986-11-20 1988-09-20 Unimation, Inc. Multiaxis robot control having curve fitted path control
US4860209A (en) * 1983-11-24 1989-08-22 Kabushiki Kaisha Toyota Chuo Kenkyusho Running command system for unmanned vehicle
US6493607B1 (en) * 1994-11-09 2002-12-10 Amada America, Inc. Method for planning/controlling robot motion
US6604005B1 (en) * 1987-11-20 2003-08-05 Koninklijke Philips Electronics N.V. Method and apparatus for path planning
US7389210B2 (en) * 2002-09-09 2008-06-17 The Maia Institute Movement of an autonomous entity through an environment
US20110190933A1 (en) * 2010-01-29 2011-08-04 Andrew Shein Robotic Vehicle
US8099214B2 (en) * 2009-02-09 2012-01-17 GM Global Technology Operations LLC Path planning for autonomous parking
US8442713B2 (en) * 2008-04-12 2013-05-14 Seoul National University Industry Foundation Steering method for vehicle and apparatus thereof
US8645016B2 (en) * 2006-01-18 2014-02-04 I-Guide Robotics, Inc. Robotic vehicle controller
US8706298B2 (en) * 2010-03-17 2014-04-22 Raytheon Company Temporal tracking robot control system
US8972057B1 (en) * 2013-01-09 2015-03-03 The Boeing Company Systems and methods for generating a robotic path plan in a confined configuration space
US9156159B2 (en) * 2012-03-08 2015-10-13 Disney Enterprises Inc. Robot cyclic locomotion using a dynamic object

Patent Citations (12)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US4860209A (en) * 1983-11-24 1989-08-22 Kabushiki Kaisha Toyota Chuo Kenkyusho Running command system for unmanned vehicle
US4773025A (en) * 1986-11-20 1988-09-20 Unimation, Inc. Multiaxis robot control having curve fitted path control
US6604005B1 (en) * 1987-11-20 2003-08-05 Koninklijke Philips Electronics N.V. Method and apparatus for path planning
US6493607B1 (en) * 1994-11-09 2002-12-10 Amada America, Inc. Method for planning/controlling robot motion
US7389210B2 (en) * 2002-09-09 2008-06-17 The Maia Institute Movement of an autonomous entity through an environment
US8645016B2 (en) * 2006-01-18 2014-02-04 I-Guide Robotics, Inc. Robotic vehicle controller
US8442713B2 (en) * 2008-04-12 2013-05-14 Seoul National University Industry Foundation Steering method for vehicle and apparatus thereof
US8099214B2 (en) * 2009-02-09 2012-01-17 GM Global Technology Operations LLC Path planning for autonomous parking
US20110190933A1 (en) * 2010-01-29 2011-08-04 Andrew Shein Robotic Vehicle
US8706298B2 (en) * 2010-03-17 2014-04-22 Raytheon Company Temporal tracking robot control system
US9156159B2 (en) * 2012-03-08 2015-10-13 Disney Enterprises Inc. Robot cyclic locomotion using a dynamic object
US8972057B1 (en) * 2013-01-09 2015-03-03 The Boeing Company Systems and methods for generating a robotic path plan in a confined configuration space

Non-Patent Citations (1)

* Cited by examiner, † Cited by third party
Title
SVENSTRUP ET AL.: "Trajectory planning for robots in dynamic human environments", TRAJECTORY PLANNING FOR ROBOTS IN DYNAMIC HUMAN ENVIRONMENTS, 2010, pages 4293 - 4298, XP031920666, Retrieved from the Internet <URL:http://vbn.aau.dk/ws/files/43408396/05651531.pdf> [retrieved on 20170321] *

Cited By (28)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US10480947B2 (en) * 2016-12-21 2019-11-19 X Development Llc Boolean satisfiability (SAT) reduction for geometry and kinematics agnostic multi-agent planning
US11262200B2 (en) 2016-12-21 2022-03-01 Intrinsic Innovation Llc Boolean satisfiability (SAT) reduction for geometry and kinematics agnostic multi-agent planning
US20190244111A1 (en) * 2018-02-02 2019-08-08 University Of New Hampshire Avoiding dead ends in real-time heuristic search
WO2020005993A1 (en) * 2018-06-25 2020-01-02 X Development Llc Robot coordination in a shared workspace
US11675369B2 (en) 2018-06-25 2023-06-13 Intrinsic Innovation Llc Robot coordination in a shared workspace
US11216009B2 (en) 2018-06-25 2022-01-04 Intrinsic Innovation Llc Robot coordination in a shared workspace
CN112703147B (en) * 2018-09-25 2023-04-28 三菱电机株式会社 System and method for controlling movement of a vehicle
CN112703147A (en) * 2018-09-25 2021-04-23 三菱电机株式会社 System and method for controlling movement of a vehicle
US11796332B2 (en) 2018-10-30 2023-10-24 Motional Ad Llc Generation of optimal trajectories for navigation of vehicles
EP3648021A1 (en) * 2018-10-30 2020-05-06 Aptiv Technologies Limited Generation of optimal trajectories for navigation of vehicles
CN109375626B (en) * 2018-11-20 2021-08-24 深圳市海柔创新科技有限公司 Positioning code pasting method and device, computer equipment and storage medium
CN109375626A (en) * 2018-11-20 2019-02-22 深圳市海柔创新科技有限公司 Alignment code is sticked method, apparatus, computer equipment and storage medium
EP3666476A1 (en) * 2018-12-14 2020-06-17 Toyota Jidosha Kabushiki Kaisha Trajectory generation system and trajectory generating method
US11433538B2 (en) 2018-12-14 2022-09-06 Toyota Jidosha Kabushiki Kaisha Trajectory generation system and trajectory generating method
US11803184B2 (en) 2018-12-18 2023-10-31 Motional Ad Llc Methods for generating maps using hyper-graph data structures
CN109955250A (en) * 2019-01-21 2019-07-02 中国船舶重工集团公司第七一六研究所 Tracking applied to industrial robot reacts planning algorithm with Real Time Obstacle Avoiding
WO2020245769A1 (en) * 2019-06-04 2020-12-10 Motional Ad Llc Autonomous vehicle operation using linear temporal logic
GB2589245B (en) * 2019-06-04 2023-01-25 Motional Ad Llc Autonomous vehicle operation using linear temporal logic
US11577754B2 (en) 2019-06-04 2023-02-14 Motional Ad Llc Autonomous vehicle operation using linear temporal logic
GB2611438A (en) * 2019-06-04 2023-04-05 Motional Ad Llc Autonomous vehicle operation using linear temporal logic
GB2589245A (en) * 2019-06-04 2021-05-26 Motional Ad Llc Autonomous vehicle operation using linear temporal logic
GB2611438B (en) * 2019-06-04 2023-11-01 Motional Ad Llc Autonomous vehicle operation using linear temporal logic
CN110695994A (en) * 2019-10-08 2020-01-17 浙江科技学院 Finite time planning method for cooperative repetitive motion of double-arm manipulator
US11526791B2 (en) 2019-11-11 2022-12-13 International Business Machines Corporation Methods and systems for diverse instance generation in artificial intelligence planning
CN113255967A (en) * 2021-04-28 2021-08-13 北京理工大学 Task planning method and device based on end point backtracking under signal time sequence logic constraint
CN114326767A (en) * 2021-12-14 2022-04-12 宁波天擎航天科技有限公司 Aircraft attitude control method, aircraft attitude control system and aircraft
CN114274148A (en) * 2022-02-15 2022-04-05 中科新松有限公司 Trajectory planning method and device, electronic equipment and storage medium
WO2024045091A1 (en) * 2022-08-31 2024-03-07 西门子股份公司 Motion planning method, apparatus and system for actuator, and storage medium

Similar Documents

Publication Publication Date Title
WO2017139613A1 (en) Motion planning for robotic systems
Garg et al. Despot-alpha: Online pomdp planning with large state and observation spaces.
US8849559B2 (en) Apparatus and method for generating and using a grid map path
EP1733287B1 (en) System and method for adaptive path planning
JP6711949B2 (en) Method and system for avoiding one or more obstacles and determining the path of an object moving from a starting state to a final state set
US11922574B2 (en) Method and device for determining plurality of layers of bounding boxes, collision detection method and device, and motion control method and device
Solis et al. Representation-optimal multi-robot motion planning using conflict-based search
Gammell et al. Bit*: Batch informed trees for optimal sampling-based planning via dynamic programming on implicit random geometric graphs
JP2018537745A (en) Method and system for recognizing objects
Wang et al. Alternating minimization based trajectory generation for quadrotor aggressive flight
Koschi et al. Interaction-aware occupancy prediction of road vehicles
Van Den Berg et al. Kinodynamic motion planning on roadmaps in dynamic environments
Martinsen et al. Optimal model-based trajectory planning with static polygonal constraints
Barták et al. On SAT-based approaches for multi-agent path finding with the sum-of-costs objective
Chen et al. On-line 3D active pose-graph SLAM based on key poses using graph topology and sub-maps
Rahman et al. An adaptive agent-specific sub-optimal bounding approach for multi-agent path finding
Ju et al. Path planning using a hybrid evolutionary algorithm based on tree structure encoding
Paden et al. A generalized label correcting method for optimal kinodynamic motion planning
US11680807B2 (en) Probabilistic decision engine
de Groot et al. Scenario-Based Motion Planning with Bounded Probability of Collision
Adolf et al. Rapid multi-query path planning for a vertical take-off and landing unmanned aerial vehicle
Cowlagi et al. Multi-resolution path planning: Theoretical analysis, efficient implementation, and extensions to dynamic environments
Hauer et al. Reduced complexity multi-scale path-planning on probabilistic maps
Titkov et al. Collision-aware formation assignment of quadrotors
Moon et al. Towards explanations of plan execution for human-robot teaming

Legal Events

Date Code Title Description
121 Ep: the epo has been informed by wipo that ep was designated in this application

Ref document number: 17750851

Country of ref document: EP

Kind code of ref document: A1

NENP Non-entry into the national phase

Ref country code: DE

122 Ep: pct application non-entry in european phase

Ref document number: 17750851

Country of ref document: EP

Kind code of ref document: A1