|
Homework
#7 Probabilistic RoadMap In this homework I have implemented a planner
based on a probabilistic roadmap. My implementation is for a two dimensional
workspace W and a six dimensional configuration
space Q. I use a mobile robot that
can translate in the plane and a four link manipulator that is mounted on the
robot and where each link is free to rotate. At first, the algorithm samples the
free configuration space using a uniform distribution. Then, it samples again
the regions near the obstacles using a normal distribution and tries to connect
each configuration with its neighbors using a local planner. After connecting
the start and the goal configurations with other connected nodes, the A* star
algorithm is used twice: once to find a first path and a second time to smooth
the calculated path. The GUI of the program and the workspace are depicted in
the image below.
Sampling The first sampling is done using a uniform
distribution for the coordinates x and y and for the four joint angles. In
order to check if a sample is in the free configuration space I have implemented
the Matlab function check = PRM_check(n_O, O, r_a, robot, W, grid_s,
y_max, design) which checks whether the mobile robot is in the free workspace
and if there is some collision between the links and the obstacles edges. If a
collision is found, a random motion is started to move the robot and to rotate
the joints until a collision free configuration is found. The position of these
“close to obstacle” configurations is marked for further aims. The initial
randomly sampled free configurations are shown as yellow squiggles in the image
below. The initial randomly sampled configurations in collision with some
obstacle are depicted as red squiggles (click on images for full size view)
.
Resampling Since the sampled configurations are often
not enough to find a path between a start and a goal configuration especially
when narrow corridors are in the workspace, a local resampling is needed. I
have implemented the Matlab function local = PRM_local_n(q, step) that
calculates the set local of neighbor configurations using
a (Gaussian) distribution centered in the “close to obstacle” configurations q found
in the previous step. Then, the new configurations are checked for collision. If
a collision is found a random motion is stared to find a free configuration. The
positions of the robot in the new configurations are depicted as black dots in
the image below (the link are not shown to avoid confused images).
Nearest neighbors and local planner To define an appropriate metric in the
configuration space I have implemented the Matlab function d =
PRM_dist(q1, q2) that computes the distance between the two
configurations q1 and q2. The distance is the
weighted sum of the Euclidean distance between the two robot positions and the joint
angles variations. Obviously, since the links are free to rotate the angles
difference is defined as the smallest magnitude between positive and negative
rotation. The two weights are the two most important parameters of the
algorithm. If the Euclidean distance is weighted more then smoother paths with wiggling
joints are calculated. On the other hand, if joint angles variations are weighted
more then more uneven paths with small joints motions are returned. The below
pictures show the two results of the planner with higher weight for Euclidean
distance (left) or higher weight for joint angle variation (right).
For each configuration the distance with all
the other configurations is calculated and the six closer configurations are
checked for connection using a local planner. I have implemented the Matlab function local =
PRM_local(q1, q2, step) that returns the set local of step
configurations between two configuration q1 and q2. If
all these intermediate configurations are free, the edge (q1, q2) is
added to the edge list. In a first implementation I have also saved the number
of the failed connections corresponding to a configuration for resampling purposes.
In fact, if a configuration has a high number of failed connections it is
likely to lie close to an obstacle and I was resampling in that region. However,
the new resampling procedure illustrated in the previous section seems to work
better. The blue lines in the picture below represent the failed connections.
Roadmap search and path
refinement The start and the goal configurations are
connected to the partially connected roadmap which is searched for the path.
An A* search is done to find an optimal path using the same distance function
illustrated above for the definition of the heuristic cost function. I
have implemented the Matlab function [path, error] = PRM_astar(edges,
robot, q_start, q_goal) that computes the A* search. The
calculated path is shown as red line in the picture below (left). As you can see, the result is
often uneven, especially when a metric is used that weights more the angles
contributes. Therefore, to get a smoother solution I compute again the
connection (local planner) step and the roadmap search step using as nodes just
the nodes of the first calculated path. The new obtained path is depicted as cyan
line in the picture below (right).
Homework files:
Click here to download all MATLAB files:
PRM.zip.
|
