16-311 Lab 5: Path Planning
Lab 5: Path Planning

Lead TA: Shucheng Chao (shuchenc@andrew.cmu.edu), Adriel Luo (aluo@andrew.cmu.edu)

Due Date: Tuesday, February 16, 2016

Grading Sheet


Lab Purpose:
This lab will help you understand fundamentals of motion planning for mobile robots.


Navigate around a known map from a user-provided start position to a user-provided end position without touching any obstacles. All obstacles will be either rectangles or diamonds (on a plane) and leaving the boundaries of the map counts as touching an obstacle.

Demo Procedure:

  1. Place the robot in a position and orientation on the map specified by a TA in inches and degrees respectively. You must specify a point on the robot to take measurements from. You will use this point to line up the robot, and it will be used to determine how far you are from the goal position.
  2. Input a desired goal position for the robot as specified by a TA given in inches.
  3. Start the robot so that it moves from its initial configuration to the goal position where it should come to a halt.
  4. If the robot hits an obstacle, the run is over and you will be scored from that point.
  5. Grading will be based on how close the robot comes to the desired goal position.
  6. The orientation of the robot at the goal position does not matter.
  7. Three tries will be given in order to improve your score if needed. All tries are independent of each other. Progressively easier start and goal positions can be requested, each for a 10 point penalty and only once per try. A request cannot be made on the first try. On the third try, assuming you've requested an easier course twice, we can assume that there's a 20 point penalty. - 10 point penalty each time you request it.
  8. The robot must come to a complete stop at the goal.
  9. The start and end coordinates will never be within 6 inches of an obstacle or edge.

Map and Obstacle List:
All measurements are in inches, 6 inch square grid displayed below. The image represents exactly (as accurately as possible) the placement of the obstacles during the demo. Each rectangle is 6 x 24 inches.

Obstacle Coordinates
Rectangle 1 Diagonal Point 1 : (0,24); Point 2 : (24,20)
Rectangle 2 Diagonal Point 1 : (33,18); Point 2 : (55,28)
Rectangle 3 Diagonal Point 1 : (78,24); Point 2 : (84,48)
Diamond 1 Center: (30,30); Length of side: 6 inches
Diamond 2 Center: (60,12); Length of side: 6 inches
Diamond 3 Center : (66,39); Length of side: 6 inches

How To:
The method described below is a waypoint based planner. However, feel free to implement any other planner for this lab.

  1. Write a structure that stores all obstacles on the map and a corresponding function that is capable of testing if a line in world space passes through any one of them.
  2. Pick points on the map such that at least one waypoint is visible from another (a straight line connecting two waypoints does not pass through any obstacle) and by following the waypoints any point on the map can be eventually reached. Remember that the robot has a non zero base area and that the obstacles must be expanded by the "radius" of the robot before picking the waypoints and doing collision detection.
  3. Now, given a start and goal position all that is necessary to plan a valid path between them is to do a graph search with the waypoints as nodes. Things that may help are hard coding all waypoints that are visible from each waypoint and having a cost function that picks a point that is both not too far away from the current location and takes you closer to the goal. Note that the start and the goal positions must be included in the search.
  4. Having constructed a path composed of straight lines from the start to the goal, add some basic timing information and use the trajectory planner from the previous lab to follow the path. The final orientation of the robot will be in the direction of the vector joining the two points.
  5. It is not necessary to use straight lines everywhere and it is possible to hardcode splines that minimize the time taken for the robot to travel between two waypoints (for example - racing turns) and use straight lines for paths that must be computed in real time (start to waypoint and waypoint to goal).

Sample Execution:

A map with hardcoded waypoints marked in blue and the start and goal positions.

Note how the shorter but more dangerous path between R1 and R2 by following Start -> WP4 -> Goal is avoided by picking a cost function that weights waypoints closer to the current position more than waypoints closer to the goal.

Other Tips:

  1. Pick waypoints that are far enough aways from the obstacles such that both physical dimensions of the robot and error due to dead reckoning are taken into account.
  2. Instead of the marker, use a different reference point like the front or back of the robot for the trajectory planner in this lab.
  3. It is possible to rotate towards the desired end orientation and then go straight between waypoints without using the trajectory planner.
  4. Save the planner. It will be used in future labs.

Grading: (75 points)
Points for the lab will be assigned based on your goal-seeking performance and analysis of your planner.

  • Robot Demo (max 50 points)
    • Final distance to goal (<=1 inches, <=3 inches, <=6 inches) - (25, 15, 5) points
    • Time to goal (<=30 seconds, <=60 seconds, <=90 seconds) - (25, 15, 5) points
  • Planner Analysis (25)
    • Explain your planner components in detail - 5 points
    • Explain the parameters you had to tune for your planner. Generate images of plans showing the effect of your parameters. - 10 points
    • Explain how your planner would change if you had to plan for a different group's robot - 5 points
    • How well would your planner adapt to a different map? What changes could you make to your planner to make it more flexible? - 5 points


Last updated 02/13/2016 by Adriel Luo
(c) 1999-2015: Howie Choset, Carnegie Mellon