Introduction to Robotics Lab 5: Path Planning Solution

Description

The goals of this lab are

• Implement a shortest-path finding algorithm (Dijkstra’s algorithm)

• Use shortest path information from Dijkstra to generate an actual route to follow

You need

• SparkiDuino IDE

• 2D grid data structure and helper functions from Lab 4 to turn the 2D grid into a graph and calculate cost between nodes

Dijkstra’s algorithm takes a graph with N nodes as an input and returns the cost of every single node to a user-specified source node. Have a look

at https://en.wikipedia.org/wiki/Dijkstra%27s_algorithm#Pseudocode for example code. For path planning purposes, treat every cell of your 2D map as a node of a graph with edge costs that are inferred from the map geometry and presence of obstacles. Provided with a “source node”, Dijkstra’s algorithm will provide you with the distance of every single node to that source.

Part 1: Path Planning

1. Implement a version of Dijkstra’s algorithm on Sparki. In particular, replace the cost matrix with your distance function from Lab 4 Part 3-2c. Use a simple 4×4 map to validate that Dijkstra’s algorithm returns the correct distances.

2. Write code that returns a sequence of vertices from a given location on the map to the source. Make sure your solution gracefully handles the case where there is no valid path between source and goal vertices!

Part 2: Lab Report

1. Briefly explain how your algorithm works.

2. What is an example of a heuristic that would help with implementing an informed search algorithm for this problem?

3. Show sample output from step 1 (a 4×4 cost matrix given a start and goal state)

4. Show sample output from step 2 for your example used in Q2.

5. What are the names of everyone in your lab group?

6. Roughly how much time did you spend programming this lab?

7. (Optional, confidential, not for credit) A brief description of any problems (either technical or collaborative) encountered while performing this lab (e.g., issues with the clarity of instructions, clarity of documentation, lab colleague’s behavior, etc.)

Please have at least one member per group submit their implementation (.ino)