Skip to content

Motion Planning - Notes

Agenda - What's Planning? - Planning Architecture Pipeline - Applications - Tools and Frameworks - Motion Planning algorithms - Mission Planner - Behaviour Planner - Local Planner - Motion Planning Datasets \& Libraries - References

What's Planning?

planning

Src: @Waymo

The motion planning problem is the task of navigating the ego vehicle to its destination in a safe and comfortable manner while following the rules of the road.

Planning Architecture Pipeline

  • Mission planning : which street to take to achieve a mission goal.
  • Behavioral planning : when to change lanes and precedence at intersections and performs error recovery maneuvers.
  • Motion planning : selects actions to avoid obstacles while making progress toward local goals

Planning multiple dimensions

  • Location
  • Orientation
  • Direction of travel (DoT)

Applications

  • Self-driving vehicles
  • Robotics
  • Drones

Tools and Frameworks

State-of-the-art Motion Planning Approaches

- Global Planner
  |
  |- Long-Term Planner
  |
  |- Short-Term Planner
      |
      |- Local Planner
          |
          |- Control Stack
  • Global planner (Long-term planner) : map + static obstacles
  • Rule Based Planning (pipeline method)
  • Predictive planning
    • Imitation learning
    • Reinforcement learning
    • Parallel learning
  • Trajectory planning
  • Graph Based Planning
  • Probabilistic Graph Based Planning

  • Optimization Based Planning

    • Linear Programming
    • NonLinear Programming

  • Local planner (Short-term planner): dynamic obstacles

  • Reactive planning (trajectory roll-out planner)

Motion Planning Algorithms

Categories

  • Grid-based search
  • Interval-based search
  • Geometric algorithms
  • Artificial potential fields
  • Sampling-based algorithms
  • ...

A* (star) Search

OPEN<-{1}
past_cost[1]<-0, past_cost[node]<-infinity for node €{2,...,N} 
while OPEN is not empty do
    current   first node in OPEN, remove from OPEN
    add current to CLOSED
    if current is in the goal set then
        return SUCCESS and the path to current
    end if
    for each nbr of current not in CLOSED do
        tentative_past_cost   past_cost[current]+cost[current,nbr]
        if tentative past cost < past cost[nbr] then
            past_cost[nbr]   tentative_past_cost
            parent[nbr]   current
            put (or move) nbr in sorted list OPEN according to
                est_total_cost[nbr]   past_cost[nbr] + heuristic_cost_to_go(nbr)
        end if
    end for
end while
return FAILURE

Dijkstra

 1  function Dijkstra(Graph, source):
 2      
 3      for each vertex v in Graph.Vertices:
 4          dist[v] ← INFINITY
 5          prev[v] ← UNDEFINED
 6          add v to Q
 7      dist[source] ← 0
 8      
 9      while Q is not empty:
10          u ← vertex in Q with min dist[u]
11          remove u from Q
12          
13          for each neighbor v of u still in Q:
14              alt ← dist[u] + Graph.Edges(u, v)
15              if alt < dist[v]:
16                  dist[v] ← alt
17                  prev[v] ← u
18
19      return dist[], prev[]
- C-Implementation

Kruskal's algorithm ?

algorithm Kruskal(G) is
    F:= ∅
    for each v ∈ G.V do
        MAKE-SET(v)
    for each (u, v) in G.E ordered by weight(u, v), increasing do
        if FIND-SET(u) ≠ FIND-SET(v) then
            F:= F ∪ {(u, v)} ∪ {(v, u)}
            UNION(FIND-SET(u), FIND-SET(v))
    return F
- C implementation

Breadth First Search (BFS)

bfs

  • D* (D-star): @TODO
  • Depth-First Search (DFS) uses a last-in-first-out (LIFO) stack instead of a queue for the open set.
  • Suboptimal A* search
  • Rapidly-exploring random tree
  • Probabilistic roadmap

Motion Planning Datasets & Libraries

Hello World!

@TODO

References

Wikipedia:

Courses: - Motion Planning Course- Self-Driving Cars Specialization of University of Toronto

DARPA Challenge: - Boss Autonomous Driving

MathWorks - MATLAB - Motion Planning with MATLAB

Academia: