这个Homework是用Matlab建立能见度图以计算从起点到目标位置的最佳路径以及使用PRM进行实验并评估PRM近似最佳路径的程度

HW 5 – Roadmaps
Due: 2018.11.13 End of day
G. Fainekos – Intro to Robotics – Fall 2019
This homework assignment has two components:
1. Build a visibility graph to compute the optimal path from a start to a goal position
2. Run experiments using PRM to assess how well PRM can approximate the optimal path
Part I (70pt)
You will construct a roadmap given a set of polygonal obstacles, a start and a goal position. For example,
given the environment of Fig. 1 (left), the resulting graph should be the one in Fig. 1 (right). In Fig. 1
(right), you can also see the shortest path computed using Dijkstra’s algorithm (red path).
Figure 1 Left: Simple environment with 2 polygonal obstacles and the start and goal positions for the robot. The black boundary
represents the boundaries of the workspace of the robot. The boundary is always the first polygon in the polygon vector. Right:
The visibility graph (roadmap) and the shortest path (red path). The axis dimensions are in meters (m).
Assumptions
1. A path from start to goal always exists, i.e., the visibility graph will be connected.
2. The obstacles will have width of at least 2 meters (no obstacles of zero width – informally, every
convex region of each obstacle can enclose a circle of radius 1)
3. Each vertex has distance at least 1 m from every other vertex and no different obstacles
intersect with each other.
Part II (30pt)
We have discussed in class that as the complexity of the workspace and the robot dynamics increases,
solving the motion planning problem becomes a much harder problem. Thus, we introduced probabilistic
roadmap methods which can provide good solutions to difficult motion and path planning problems. For
this homework problem, you will design a small-scale experimental analysis of the PRM algorithm to
assess the approximation error introduced. You will perform the following experiments:
1. Compute the success rate of finding a path and the statistics of the additional distance that needs
to be travelled. The experiment is setup within the template hw5_PRM_Exp.m.
2. A graph plot and data on how the number of sampled nodes for the PRM construction affects the
performance of the PRM in terms of identifying the optimal path. Run tests on scenario 1 for the
following number of samples for the nodes: n_samp_vec = [50 100 150 200 250 300 350 400].
Store the resulting approximation error of the distance in a vector err_vec. For this part, you will
have to use hw5_PRM_Exp.m. Observe how the error reduces as the number of nodes increases.
Files and templates provided
The following files are provided:
1. hw5_init_script.m : It initializes the polygonal environment and it calls the templates for
the visibility graph computation and the experimental analysis. See Figure 2 for the expected
outputs of running the script for the first time.
2. hw5_VisGraph.m : The template m-function for creating your solution. The current file
creates the dummy graph in Fig. 2.
3. hw5_PRM_Exp.m : The template m-function for running PRM experiments and collecting the
data.
Auxiliary files you do not need to modify:
1. Polygon.m : A class that implements basic operations on polygons.
2. tb_optparse.m : an m-function which parses inputs to plotting functions.
Grading
Part I: 70pt; Out of 100% for the 70pt:
 10% Coding style
 Graph missing vertices and/or edges: -2% penalty for each missing vertex or edge (up to -30%
total penalty)
 Graph contains unnecessary or wrong vertices and/or edges, e.g., edges that collide with
obstacles: -3% penalty for each unnecessary vertex or edge (up to -30% total penalty)
Additional penalties:
 The computed graph does not contain the optimal path: -5%
Figure 2 Left: The toy graph returned by the template file hw5_init_script.m . The graph contains only the nodes
corresponding to start and goal positions. No collision checking with the obstacles is performed. Right: Running PRM on the same
environment. Notice that the resulting path is sub-optimal and that the occupancy grid is an approximation of the actual
environment.
 If edge weights do not represent distances between vertices: -5%
Part I: 30pt; Out of 100% for the 30pt:
 10% Coding style
 70% for the coding part
 20% for the plot part
Notes
You will be using the following classes:
 graph : constructs graphs which are topologically embedded. The latter means that the nodes
have topological information regarding their coordinates and the edges are labeled with weights
representing distances between nodes.
 Polygon : construct and manipulate polygons. Remark: not all methods are supported for
arrays of polygons.
Read the help files carefully. You may have to look into the code to get a better insight.
Remarks
 You do not need to check whether start and goal are not inside an obstacle
 Self-loops MUST NOT be added on the graphs. Self-loops are not relevant for applications where
we need to compute shortest paths.
 Directed vs undirected graphs: The class “graph” creates undirected graphs. Hence, when you
add an edge (i,j), the edge (j,i) is added as well. The graph vertices (nodes) are not topologically
grounded; hence, you need to store the x and y coordinates in the properties xPos and yPos of
the Nodes property of the graph.
 Intersection of line segment and polygon: You can use the method “intersect_line” or you can
sample points along the line.
o You can read more on the intersection between lines and polytopes in [de Berg et al
2000] Ch 2 or [Choset et al] App F
Deliverable
 Submit a zip file called hw5_ASUID.zip with the following:
o hw5_VisGraph.txt (just change .m to .txt)
o hw5_PRM_Exp.txt (just change .m to .txt)
o hw5_data.mat (a matlab data file containing n_samp_vec and err_vec)
o hw5_graph.fig (save and submit the Matlab figure generated by
plot(n_samp_vec,err_vec) )