PRM Path Planning algorithm

As the foundation of the robot to accomplish various tasks, path planning has been the hotspot of research. The researchers put forward many planning methods, such as artificial potential field method, unit decomposition method, stochastic road Map (PRM) method, Fast search tree (RRT) method, etc. The traditional artificial potential field and the unit decomposition method need to accurately model the obstacles in the space, and when the obstacles in the environment are more complicated, the computational amount of the planning algorithm will be larger. The PRM method based on random sampling technology can effectively solve the problem of path planning in high dimensional space and complex constraints.

PRM is a method based on Graph search, which transforms continuous space into discrete space, then uses a * search algorithm to find paths on the road map to improve search efficiency. This method can be used to find a solution with a relatively small number of random sampling points, for most problems, a relatively small sample is sufficient to cover most of the feasible space, and the probability of finding the path is 1 (as the sample count increases, P (find a path) index tends to 1). Obviously, when the sampling point is too little, or the distribution is unreasonable, the PRM algorithm is not complete, but with the increase of the use point, can also achieve complete. So the PRM is a complete and suboptimal probability.

The PRM Path Planner constructs a roadmap in the free space of a given map using randomly sampled nodes in the FR EE space and connecting them with each other. Once the roadmap have been constructed, you can query for a path from a given start location to a given end location on the Map.

　　The Probabilistic Roadmap planner consists of the phases:a construction and a query phase. learning stage: Randomly scatter points (number of customizations) in the free space of a given graph to construct a path network diagram. Query phase: Queries the path from one starting point to one end point.

? Roadmap is a graph G (V, E) (No network graph G, where V represents a random point set and E represents all possible sets of paths between two points)

? Robot configuration Q→q_free is a vertex (each point ensures that the robot does not collide with the obstacle)

? Edge (Q1, Q2) implies Collision-free path between these robot configurations

? A metric is needed for D (Q1,Q2) (e.g. Euclidean distance) (Dist function calculates the distance between the midpoint of the configuration space and the point, judging whether it is the same point)

? Uses Coarse sampling of the nodes, and fine sampling of the edges

? RESULT:A Roadmap in Q_free

Step 1, learning the Map

? Initially empty Graph G

? A configuration q is randomly chosen

? If Q→q_free then added to G (collision detection needed here)

? Repeat until N vertices chosen

? For each q, select K closest Neighbors,

? Local Planner connects Q to neighbor Q '

? If Connect successful (i.e. collision free Local Path), add Edge (q, Q ')

　　

Refer to the MATLAB code here, enter a map of 500x500, according to roadmap construction algorithm set up a network diagram, and then use a * algorithm to search for a shortest path.

The PRM.M file is as follows, note that the following code does not pick the K nearest point to connect (or limit the connection distance), but instead joins the entire node.

Percent PRM PARAMETERSMAP=IM2BW (imread (' map1.bmp ')); % input map read from a BMP file. For new maps write the file name heresource=[10 10]; % source position in Y, X formatgoal=[490 490]; % goal position in Y, X formatk=50; % of points in the prmdisplay=true; % display processing of nodesif ~feasiblepoint (SOURCE,MAP), error (' Source lies on an obstacle or outside map '); endif ~feasiblepoint (Goal,map), error (' Goal lies on an obstacle or outside map '); Endimshow (map); Rectangle (' position ', [1 1 size (map)-1], ' edgecolor ', ' K ') vertex=[source;goal]; % source and goal taken as additional vertices in the path planning to ease planning of the robotif display, rectangle (' Po Sition ', [vertex] -5,vertex ( -5,10,10], ' curvature ', [[], ' Facecolor ', ' R '); End% Draw Circleif display, rectangle (' Position ', [Vertex (2,2) -5,vertex (2,1) -5,10,10], ' curvature ', [all], ' Facecolor ' , ' R '); endtic; % tic-toc:functions for Elapsed time%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% Step 1, constructs the Map%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% iteratively Add verticeswhile Length (vertex) <k+2 x = Double (Int32 (Rand ( ). * Size (map));         % using randomly sampled nodes (convert to pixel unit) if Feasiblepoint (X,map), vertex=[vertex;x]; If display, rectangle (' Position ', [x (2) -5,x (1) -5,10,10], ' curvature ', [all], ' Facecolor ', ' R '); End ENDENDIF Display disp (' click/press any key ');% blocks the caller ' s execution stream until the function detects  That the user have pressed a mouse button or a key while the figure window is active waitforbuttonpress;  end%% attempts to connect all pairs of randomly selected Verticesedges = cell (k+2,1);            % edges to be stored as a adjacency listfor i=1:k+2 for j=i+1:k+2 if Checkpath (vertex (i,:), Vertex (j,:), map); EDGES{I}=[EDGES{I};J];            Edges{j}=[edges{j};i]; If display, line ([Vertex (i,2); vertex (j,2)],[vertex (i,1); vertex (j,1)]);    End End ENDENDIF Display disp (' click/press any key '); Waitforbuttonpress; end%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% Step 2, finding a Path using a *%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%%%%%%%%%%%%%%%% structure of a node is taken as: [Index of node in vertex, historic cost, heuristic cost, total cos  T, parent index in closed list ( -1 for source)]q=[1 0 heuristic (vertex (1,:), goal) 0+heuristic (vertex (1,:), goal)-1];% the Processing queue of A * algorihtm, open listclosed=[]; % The closed list taken as a listpathfound=false;while size (q,1) > 0 while open-list are not empty [a, I] = min (     q,[],1);% find the minimum value of each column current = Q (I (4),:);% select smallest total cost element to process Q=[q (1:I (4)-1,:); Q (I (4) +1:end,:)]; % delete element under processing if current (1) ==2% Index of node in vertex==2 (goal) Pathfound=true;bre     Ak End for MV = 1:length (edges{current (1)})% iterate through all edges from the node newvertex=edges{current (1) } (MV); % move to neighbor NOde if length (closed) ==0 | | Length (Find (Closed (:, 1) ==newvertex)) ==0% not in closed (Ignore the neighbor which is already evaluated) Histor             Iccost = Current (2) + historic (vertex (current (1),:), vertex (Newvertex,:));% the distance from start to a neighbor             Heuristiccost = heuristic (vertex (Newvertex,:), goal);              TotalCost = Historiccost + heuristiccost; Add = true; % not already in queue with better cost if length (Find (Q (:, 1) ==newvertex) >=1 I = Find (q (:,                 1) ==newvertex); If TotalCost > Q (i,4), Add=false; % not a better path else q=[q (1:i-1,:); Q (I+1:end,:);];                  Add=true; End End If add Q=[q;newvertex historiccost heuristiccost totalcost size (closed,1) +1]; % Add new nodes in queue end end end Closed=[closed;current]; % Update closed LISTSENDIF ~pathfound error (' No path found ') ENDFPrintf (' processing time=%d \npath length=%d \ n ', TOC, current (4)); Path=[vertex (current (1),:)];% retrieve path from parent Informationprev = current (5), while prev > 0 path = [Vertex (    Closed (prev,1),:);p Ath]; prev = Closed (prev, 5); endimshow (map); Rectangle (' position ', [1 1 size (map)-1], ' edgecolor ', ' K ') Line (Path (:, 2), Path (:, 1), ' Color ', ' r ');

View Code

Other M files:

% checkpath.mfunction Feasible=checkpath (n,newpos,map) feasible = True;dir=atan2 (Newpos (1)-N (1), NewPos (2)-N (2 )); for R=0:0.5:sqrt (SUM ((n-newpos). ^2)) Poscheck = n + r.*[sin (dir) cos (dir)];if ~ (Feasiblepoint (Ceil (Poscheck), map) && Feasiblepoint (Poscheck), map && feasiblepoint ([Ceil (Poscheck (1)) Floor (Poscheck (2))], MAP) && Feasiblepoint ([Floor (Poscheck (1)) Ceil (Poscheck (2))],map)) feasible = False;break;endif ~ Feasiblepoint (newpos,map), feasible = false; endendend%% feasiblepoint.mfunction feasible=feasiblepoint (point,map) feasible=true;% check if Collission-free spot And inside maps% 0:obstacle 1:free spaceif ~ (Point (1) >=1 && Point (1) <=size (map,1) && Point (2) ; =1 && Point (2) <=size (map,2) && Map (Point (1), point (2)) ==1) feasible=false;endend%% Heuristic.mfunction h=heuristic (x,goal) h = sqrt (sum ((x-goal). ^2)); end%% historic.mfunction H=historic (A, b) H = sqrt ( Sum ((a). ^2)); end 

View Code

MATLAB Robotics System Toolbox

The Robotics System Toolbox in MATLAB provides the PRM path planning method, which makes it easy to create a probabilistic roadmap path planner for path planning. The following points need to be noted when using:

• Tune the number of Nodes (numbers of tuning nodes)

Increasing the number of nodes can increase the efficiency of the path by giving more feasible paths . However, the increased complexity increases computation time . To get good coverages of the map, you might need a large number of nodes. Due to the random placement of nodes, some areas of the "the" map may not has enough nodes to connect to the rest of the map. In this example, you create a large and small number of nodes in a roadmap.

% Create an occupancy grid 2 ); % Create A simple roadmap with nodes.  - ); show (Prmsimple)

% Create A dense roadmap with nodes. = Robotics. PRM (map,+); show (Prmcomplex)

• tune the Connection Distance (adjust connection distance)

use the  connectiondistance  property on the  PRM  object to tune the algorithm.  connectiondistance  is A upper threshold for points That is connected in the roadmap. Each of the node is connected to all nodes within this connection distance the does not has obstacles between them. by lowering the connection distance, you can limit the number of connections to R Educe the computation time and simplify the map . However, a lowered distance limits the number of available paths from which to find a complete obstacle-free path. When working and simple maps, you can use a higher connection distance with a small number of nodes to increase Efficienc Y. for complex maps with lots of obstacles, a higher number of nodes with a lower Ed connection distance increases the chance of finding a solution .

% Save The random number generation settings using the RNG function. The saved settings enable you to reproduce the same points ConnectionDistance  and see the effect of changing.
Rngstate =rng;
% Create A roadmap with nodes and calculate the path. The default connectiondistance is set To  INF. PRM= Robotics. PRM (Map, -);
Startlocation= [2 1];endlocation= [ A Ten];p Ath=Findpath (prm,startlocation,endlocation);
Show (PRM)

% Reload the random number generation settings to has the PRM use the  same nodesrng (rngstate);
% Lower connectiondistance to 2 m 2  = Findpath (PRM, startlocation, endlocation); show (PRM)

• Create or Update PRM

This roadmap changes only if your call is update or change the properties in the PRM object. When properties change, any method ( update , findpath , or show ) called on the object triggers the roadmap points and Connectio NS to is recalculated. Because recalculating the map can be computationally intensive, you can reuse the same roadmap by calling with findpath Diffe Rent starting and ending locations. That is, when updating with the update function or changing the properties of the PRM object, calling Findpath, show, and so on will cause recalculation.

The PRM algorithm recalculates the node placement and generates a new network of nodes

• Inflate the Map Based on Robot Dimension

　　PRM does not take to account the robot dimension and computing an obstacle free path on a map. Hence, should inflate the mapby the dimension of the robot, in order to allow computation of an obstacle fre E path that accounts for the robot ' s size and ensures collision avoidance for the actual robot. This inflation was used to add a factor of safety on obstacles and create buffer zones between the robot and obstacle in th E environment.  The inflate method of an occupancy grid object converts the specified to the number of radius cells rounded resolution*radius the Value.

0.2= copy (map); inflate (Mapinflated,robotradius); Show (Mapinflat

• Find A feasible Path on the constructed PRM

Since You is planning a path on a large and complicated map, larger number of nodes could be required. However, often it is isn't clear how many nodes would be sufficient. Tune the number of nodes to make sure there are a feasible path between the start and endlocation.

Path = Findpath (PRM, startlocation, endlocation)

 while IsEmpty (path)     %  No feasible path found yet, increase the number of nodes     Ten ;       attribute    Update (PRM);     % Search for  A feasible path with the updated PRM     = Findpath (PRM, startlocation, endlocation); end

Reference:

Probabilistic roadmaps (PRM)

Occupancy Grids

Path planning in environments of Different complexity

Code for Robot Path planning using Probabilistic Roadmap

Roadmap Methods for multiple Queries

Probabilistic Roadmap Path Planning

V-rep Study notes: Robot Path planning

Path Planning Method-Stochastic Path graph method (PRM)

A * algorithm

PRM Path Planning algorithm