MAE 4180/5180, CS 4758/5758, ECE 4180/5772 Autonomous Mobile Robots Homework 6

6. Play with the tuning parameters (catt , crep , Q) and find a combination of parameters that creates at least one local minimum and plot the potential field.

7. Based on the potential field without the local minima, plot the trajectory of a holonomic robot starting at (80,55). To plot a trajectory, choose a time step and at each time step your velocity vector should be the gradient of the potential function.

Test function Potential Field (5 points)

1. Edit the function TestSphereWorldPot.m to output a plot of the potential field.

Controlling the Create using potential functions (25 points)

Since the Create simulator does not support circles, you will be over approximating the obstacles with spheres. Assume the robot goal is at [0,0] for this section.

1. Load the map hw6aMap.txt into the Create simulator.

2. Define a sphere world that would be appropriate for the map. Plot the sphere world and explain your choice. Use the definition of your sphere world to calculate the potential functions in the following.

3. Write a control program potentialPlanner.m to call your function potentialPoint.m (tuned to avoid local minima) and calculate the appropriate control input at the robot’s current location (as given by truthPose).  Test that the program works with the Create simulator.  You should be able to place the robot at any  starting location on the field and have it navigate to the goal without hitting the obstacles. Plot the trajectory of the robot starting at

(a)  [ −4, 4]

(b)  [ −4, −4]

4. Are there any starting locations that cause the robot to get “stuck” (even though there are no local minima)? Why?

Part B - Sampling Based Motion Planning (140 points)

The file hw6b.txt contains the description of obstacles within a map.   The boundary of the workspace is a rectangle whose bottom left corner is at (0,0) and its top right corner is at (100,100).  Each line in the file contains the vertices of one polygonal obstacle:  v1x, v1y, v2x, v2y, etc.  The vertices are given in counterclockwise order.  To make it easy to import in MATLAB, each line contains 16 entries correspond-

ing to (up to) 8 vertices. If an obstacle has fewer vertices, unused entries in the line will contain the value zero. The zip file plotting.zip contains plotting functions you may find useful.

Cell decomposition (15 points)

1. Plot the workspace defined in hw6b.txt. Create a convex cell decomposition of this environment and plot that as well. The decomposition may be done by hand. Make sure to distinguish in the plot which polygons are obstacles and which are the decomposition of the free space.

2. Draw a graph representing the decomposed workspace. This may be drawn by hand.

3. What are the nodes? What are the Edges?

Roadmap (25 points)

1. Write a function createRoadmap.m that given a polygonal environment (i.e the vertices of obstacles) returns a roadmap covering Qfree. You may use any method including using the cell decomposition of the previous section (if it was not done manually).  Explain your method and the data structure you use to represent the roadmap.

2. Plot the workspace defined in hw6b.txt and the roadmap that was generated.

3. What are the nodes? What are the Edges?

Paths in the discrete abstraction (25 points)

1. Write a function findPath.m that given a polygonal environment, a roadmap, and initial and goal points, returns the shortest path connecting the initial and goal points. Note that you will first need to connect the initial and goal points to the roadmap.  You may either implement a shortest path algorithm or use MATLAB file exchange to get a function implementing Dijkstra’s algorithm or any other shortest path algorithm. Make sure to mention which algorithm is being used and cite the source.

2. For the following pairs of initial and goal positions, using the roadmap you created in the previous section and the environment defined in hw6b.txt, find and plot the shortest path connecting initial and goal positions.

(a) Init: (22,65) Goal: (90,10)

(b) Init: (80,95) Goal: (10,50)

3. How did you define the costs?

Rapidly-exploring Random Trees - point robot (40 points)

1. Write a function buildRRTpoint.m that takes in a map (i.e a polygonal workspace where each row contains the vertices of an obstacle), workspace boundary, (x,y) start location and (x,y) goal location, and output a series of waypoints defining a collision-free path from start to goal generated using a rapidly-exploring random tree.  (You may find it useful to plot the tree as it grows, to make sure it’s doing what you expect.)  Note:  do not forget to check whether the goal is visible from the current node.

Submit this function in the autograded assignment Homework  6 buildRRTpoint.m on Canvas. Course staff will use this function to test your code with different inputs. We are using the autograder as a way to ensure that your buildRRTpoint.m function runs and all the consecutive waypoints are connected using a path that is collision free. We are NOT testing for correctness of the algorithm. Make sure the zip file you submit on Canvas contains this function and all of the functions necessary to run it.

2. For the environment in hw6b.txt, start location (20,65) and goal location (90,10) plot the map walls, the full search tree, and the path from start to goal.

3. Repeat  (2) to obtain a new solution.   What do you observe?   Comment on the efficiency of your algorithm, as well as the quality of the solution paths.

4. Qualitatively looking at paths obtained from the potential function, comment on the differences be- tween the resulting motions.

Rapidly-exploring Random Trees - circular robot (25 points)

1. Write a function buildRRT.m that takes in a map (i.e a polygonal workspace where each row contains the vertices of an obstacle), workspace boundary, (x,y) start location, (x,y) goal location and robot radius, and output a series of waypoints defining a collision-free path from start to goal generated using a rapidly-exploring random tree.  (You may find it useful to plot the tree as it grows, to make sure it’s doing what you expect.)

2. How did you take into account that the robot is no longer a point robot?

3. Write a control program rrtPlanner.m that loads cornerMap.mat, has a goal position at [2, 2.5] and uses the initial position of the robot (position when calling the function) as the initial position. Assume the robot radius is 0.2m and build an RRT using buildRRT.m.  Once a collision-free path has been found, have the robot follow the path using visitWaypoints.m.

4. Run the Create simulator (SimulatorGUI.m), load the map cornerMap and place the robot near [2, −2]. Press ‘Start’ and choose the control program rrtPlanner.m.

5. Plot the map walls, the full search tree, the resulting path, and the robot trajectory from part (4).

Test Function (10 points)

Edit the function HW6btest.m so that it creates the following plot:

1. The workspace with an RRT created for connecting the start and the goal.   Use the robot radius provided as input.

Part C - Navigation Function (60 Points Extra Credit)

Navigation function - Sphere world (30 Points)

Using the sphere map defined in Part A ( hw6sphere.txt) and assuming the goal is at (30,40),

1. Create a function spherePoint.m to take in a map, goal point, k , λ and an (x,y) location, and output the navigation function value at that location.

2. Write a function navigationPlot.m that calls spherePoint.m and plots the navigation function. Its inputs should include the map, the goal, k , λ, and a list of (x,y) points over which the navigation function will be evaluated.  This function and  spherePoint.m should be general enough that you can easily run it on a different sphere map  (different size and locations of spheres)!

3. 8 points - Plot the navigation function for different values of K. How does changing K affect the potential?

4. 8 points - Plot the navigation function for different values of λ.  How does changing λ affect the potential?

5. 8 points - Choose K large enough so that there are no local minima.  Use gradient.m to find the numerical gradient of the navigation function and plot the vector field over the map. Save the matrices defining the gradient to the file navGrad.mat.

6. 6 points - Plot the trajectory of a holonomic robot starting at (80,55).

Test function Navigation Function (5 points)

1. Edit the function TestSphereWorldNav.m to output a plot of the Navigation function.

Controlling the Create using navigation functions (25 points)

Since the Create simulator does not support circles, you will be over approximating the obstacles with spheres. Assume the robot goal is at [0,0] for this section.

1. Load the map hw6aMap.txt into the Create simulator.