Homework 6 Autonomous Mobile Robots Solution

A document containing the answers and gures should be uploaded to Gradescope as HW6writeup.pdf. All the code and the les needed to run the code are to be uploaded to Canvas as a zip le named HW6code.zip. Speci c functions, as indicated below, should be uploaded to their corresponding assignments on Canvas.

Notes about autograded assignments:

    • We highly recommend you develop and debug the autograded functions in Matlab. The error messages that the autograder provides are not as explicit as in Matlab.

    • You may submit as many times as you like until the deadline

    • Make sure to also include the functions in the zip  le you upload to Canvas

    • Reusing code: We encourage you to reuse your prior functions in the new functions. For the auto-graded assignments, if you want to use a previous function you have written (for example robot2global), simply copy-paste it below the function you are writing.

Part A - Potential Field (70 Points)

The le hw6Sphere.txt contains the de nition for a sphere world. Column 1 and 2 contain the X and Y coordinates of the centers of the spheres, and column 3 contain the radius. The rst line describes the boundary of the workspace while all other lines describe the obstacles.

Potential function (40 Points)

Assuming the goal is at (30,40),

    1. Plot the sphere world. The \useful functions" we provide on Canvas will come in handy for this.

    2. Write a function potentialPoint.m to take in a map, goal point, the scaling factors catt (for the attractive force) and crep (for the obstacles), in uence range of the obstacles Q, and a single (x,y) location, and output the potential eld and gradient at that location.

Submit this function in the autograded assignment Homework 6 potentialPoint.m on Canvas. Course sta will use this function to test your code with di erent inputs, and we are using the autograder as a way to ensure that your potentialPoint.m runs and gives correct output for simple scenarios. We are NOT testing for correctness of the algorithm. Make sure the zip le you submit on Canvas contains this function and all of the functions necessary to run it.

    3. Write a function potentialPlot.m that calls potentialPoint.m and plots the potential eld. Its inputs should include the map, the goal, the scaling factors catt and crep, the in uence range of the obstacles Q, and a list of (x,y) points over which the potential will be evaluated. This function and potentialPoint.m should be general enough that you can easily run it on a di erent sphere map (di erent size and locations of spheres)!

    4. Play with the tuning parameters (catt, crep, Q) and observe how they a ect the potential eld. Try to nd a combination of parameters that avoids local minima and plot the potential eld (you can use mesh.m, contour.m or surf.m) and the vector eld (quiver.m) over the entire map.

    5. For part 4, what is the Q used for each of the obstacles? Why?
    6. Play with the tuning parameters (catt, crep, Q) and nd a combination of parameters that creates at least one local minimum and plot the potential eld.

    7. Based on the potential eld 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    eld.

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. De ne a sphere world that would be appropriate for the map. Plot the sphere world and explain your choice. Use the de nition 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 eld 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 le 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 le 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    le plotting.zip contains plotting functions you may    nd useful.


Cell decomposition (15 points)

    1. Plot the workspace de ned 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?







2
Roadmap (25 points)

    1. Write a function createRoadmap.m that given a polygonal environment (i.e the vertices of obstacles) returns a roadmap covering Qf ree. 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 de ned 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 rst need to connect the initial and goal points to the roadmap. You may either implement a shortest path algorithm or use MATLAB le 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 de ned in hw6b.txt, nd and plot the shortest path connecting initial and goal.

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

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

    3. How did you de ne 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 de ning a collision-free path from start to goal generated using a rapidly-exploring random tree. (You may nd 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 sta will use this function to test your code with di erent 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 le 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 e ciency 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 di erences 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 de ning a collision-free path from start to goal generated using a rapidly-exploring random tree. (You may nd it useful to plot the tree as it grows, to make sure it’s doing what you expect.)


3
    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 de ned 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 di erent sphere map (di erent size and locations of spheres)!

    3. 8 points - Plot the navigation function for di erent values of K. How does changing K a ect the potential?

    4. 8 points - Plot the navigation function for di erent values of . How does changing a ect the potential?


    5. 8 points - Choose K large enough so that there are no local minima. Use gradient.m to nd the numerical gradient of the navigation function and plot the vector eld over the map. Save the matrices de ning the gradient to the le 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.







4
    2. Run your function navigationPlot.m with the sphere world you de ned in Part A from hw6aMap.txt. Save the matrices de ning the gradient to the le navGrad2.mat. Write a control program navigationPlanner.m that loads navGrad2.mat and calculates the appropriate control input at the robot’s current location (as given by truthPose). Use interp2.m to nd the gradient at the robot’s current location by interpolating the gradient matrices. Test that the program works with the Create simulator. You should be able to place the robot at any starting location on the eld 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]

Part D - Probabilistic Roadmaps (65 points Extra Credit)

Probablistic Roadmaps (55 points)

For this section you may assume a holonomic point robot.

    1. Write a function buildPRM.m that takes as input a polygonal workspace, the number of vertices in the roadmap n and a sampling function and returns a PRM.

Submit this function in the autograded assignment Homework 6 buildPRM.m on Canvas. Course sta will use this function to test your code with di erent inputs. We are using the autograder as a way to ensure that your buildPRM.m runs and that all the edges are in Qf ree. We are NOT testing for correctness of the algorithm. Make sure the zip le you submit on Canvas contains this function and all of the functions necessary to run it.

    2. 8 points - Create and plot a PRM for the environment de ned in hw6b.txt and n = 10 using uniform random sampling.

    3. 8 points - Create and plot a PRM for the environment de ned in hw6b.txt and n = 10 using a deterministic, low-dispersion sampling.

    4. 8 points - Create and plot a PRM for the environment de ned in hw6b.txt and n = 10 using a deterministic, low-discrepancy sampling.

    5. 5 points - How did you detect collisions?

    6. 5 points - For parts 2 and 4, does there exist a path between (20,65) and (90,10)?

    7. 5 points - Choose n large enough so that for parts 2-4 a path exists between (20,65) and (90,10). Plot the PRMs and the paths. Indicate what n is.

    8. 16 points - Create a visibility based PRM. How many samples did you need?

Test Function (10 points)

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

    1. The workspace with a generated PRM and the path between the start and the goal. You may use any sampling method you wish. Assume the robot is a point robot. If the algorithm cannot nd a path, plot the PRM only.












5