Window Approach for Motion Planning Exercise 6: Dijkstra’s ......Window Approach for Motion Planning Daniel Dugas, Lukas Schmid 27.03.2019 | 2 Exercise 6: Overview Task 6.1 Task

27.03.2019 | 1

Autonomous Mobile Robots

Exercise 6: Dijkstra’s Algorithm and the Dynamic Window Approach for Motion PlanningDaniel Dugas, Lukas Schmid

27.03.2019 | 2

Exercise 6: Overview

Task 6.1 Task 6.2 Task 6.3 Task 6.4

In this exercise, we will design a simple path planning framework for a ground robot. To build understanding, the tasks first focus on local planning, then global planning, and finally how to combine the two, as well as verify that the planner works in simulation.

27.03.2019 | 3

The Dynamic Window Approach - Local Planning

Input: Obstacle Map, Current State, Goal PoseOutput: Next Control InputAlgorithm:1. Sample feasible inputs2. For all feasible inputs:

a. Simulate trajectory over horizonb. Score trajectory

3. Pick input with best trajectory score

27.03.2019 | 4

The Dynamic Window Approach - Local Planning

27.03.2019 | 5

The Dynamic Window Approach

Local: Heading towards the goal Global: Heading towards gradient

27.03.2019 | 6

Global Planning with Dijkstra

Dijkstra’s algorithm solves the problem of finding the shortest paths between nodes in a graph.

Inputs: Graph with transition costs between nodes, Target nodeOutput: Cost of shortest-path-to-target at each node

Algorithm:

Modelling a 2D grid as a graph (8-connectivity)

27.03.2019 | 7

Global Planning with Dijkstra

While queue is not empty and not at goal...

27.03.2019 | 8

Global Planning with Dijkstra

- Pop front node from queue- Expand and add new nodes to queue- Resolve double insertions

27.03.2019 | 9

Global Planning with Dijkstra

Repeat...

27.03.2019 | 10

Global Planning with Dijkstra

Repeat...

27.03.2019 | 11

Global Planning with Dijkstra

- Optimal solution as long as all edge costs are positive- O(|V| log(|V|) + |E|)- Speed up with heuristic (A*)

27.03.2019 | 12

The Dynamic Window Approach

Local: Heading towards the goal Global: Heading towards gradient

27.03.2019 | 13

The Dynamic Window Approach

Local and Global planning can be combined to solve more complex navigation problems.

27.03.2019 | 14

Rapidly-Exploring Random Trees (RRT)

Input: Obstacle Map, Current State, Goal PoseOutput: Feasible Robot PathAlgorithm:1. Sample random pose2. Link random pose to nearest pose

in graph, if there is no obstacle between the two poses

3. Repeat until goal pose is in graph

Extra - Not exam material

27.03.2019 | 15

Local and Global Planning - Kinodynamic RRT

By adding kinodynamic constraints (e.g. turning radius) to the RRT algorithm, it can be used without a separate local planner.

Extra - Not exam material