路徑規劃作為機器人完成各種任務的基礎，一直是研究的熱點。研究人員提出了許多規劃方法：如人工勢場法、單元分解法、随機路标圖（PRM）法、快速搜尋樹（RRT）法等。傳統的人工勢場、單元分解法需要對空間中的障礙物進行精确模組化，當環境中的障礙物較為複雜時，将導緻規劃算法計算量較大。基于随機采樣技術的PRM法可以有效解決高維空間和複雜限制中的路徑規劃問題。
PRM是一種基于圖搜尋的方法，它将連續空間轉換成離散空間，再利用A*等搜尋算法在路線圖上尋找路徑，以提高搜尋效率。這種方法能用相對少的随機采樣點來找到一個解，對多數問題而言，相對少的樣本足以覆寫大部分可行的空間，并且找到路徑的機率為1（随着采樣數增加，P（找到一條路徑）指數的趨向于1）。顯然，當采樣點太少，或者分布不合理時，PRM算法是不完備的，但是随着采用點的增加，也可以達到完備。是以PRM是機率完備且不最優的。
The PRM path planner constructs a roadmap in the free space of a given map using randomly sampled nodes in the free space and connecting them with each other. Once the roadmap has 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 two phases: a construction and a query phase. 學習階段：在給定圖的自由空間裡随機撒點（自定義個數），建構一個路徑網絡圖。查詢階段：查詢從一個起點到一個終點的路徑。
• Roadmap is a graph G(V, E) （無向網絡圖G，其中V代表随機點集，E代表所有可能的兩點之間的路徑集）
• Robot configuration q→Q_free is a vertex （每個點都要確定機器人與障礙物無碰撞）
• 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計算Configuration Space中點與點之間的距離，判斷是否是同一個點）
• 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’)
參考這裡的MATLAB代碼，輸入一幅500×500的地圖，根據Roadmap Construction Algorithm建立網絡圖，然後使用A*算法搜尋出一條最短路徑。
PRM.m檔案如下，注意下面代碼中并沒有選取k個最近點進行連接配接（或是限定連接配接距離），而是連接配接了全部的節點。
%% PRM parameters
map=im2bw(imread('map1.bmp')); % input map read from a bmp file. for new maps write the file name here
source=[10 10]; % source position in Y, X format
goal=[490 490]; % goal position in Y, X format
k=50; % number of points in the PRM
display=true; % display processing of nodes
if ~feasiblePoint(source,map), error('source lies on an obstacle or outside map'); end
if ~feasiblePoint(goal,map), error('goal lies on an obstacle or outside map'); end
imshow(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 robot
if display, rectangle('Position',[vertex(1,2)-5,vertex(1,1)-5,10,10],'Curvature',[1,1],'FaceColor','r'); end % draw circle
if display, rectangle('Position',[vertex(2,2)-5,vertex(2,1)-5,10,10],'Curvature',[1,1],'FaceColor','r'); end
tic; % tic-toc: Functions for Elapsed Time
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% Step 1, Constructs the Map %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%% iteratively add vertices
while length(vertex)<k+2
x = double(int32(rand(1,2) .* 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',[1,1],'FaceColor','r'); end
end
end
if display
disp('click/press any key');
% blocks the caller's execution stream until the function detects that the user has pressed a mouse button or a key while the Figure window is active
waitforbuttonpress;
end
%% attempts to connect all pairs of randomly selected vertices
edges = cell(k+2,1); % edges to be stored as an adjacency list
for 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
end
end
if 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 cost, 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 list
closed=[]; % the closed list taken as a list
pathFound=false;
while size(Q,1) > 0 % while open-list is 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;break;
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)
historicCost = 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 lists
end
if ~pathFound
error('no path found')
end
fprintf('processing time=%d \nPath Length=%d \n\n', toc, current(4));
path=[vertex(current(1),:)]; % retrieve path from parent information
prev = current(5);
while prev > 0
path = [vertex(closed(prev,1),:);path];
prev = closed(prev, 5);
end
imshow(map);
rectangle('position',[1 1 size(map)-1],'edgecolor','k')
line(path(:,2),path(:,1),'color','r'); View Code
其它M檔案：
%% checkPath.m
function 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(floor(posCheck),map) && ...
feasiblePoint([ceil(posCheck(1)) floor(posCheck(2))],map) && feasiblePoint([floor(posCheck(1)) ceil(posCheck(2))],map))
feasible = false;break;
end
if ~feasiblePoint(newPos,map), feasible = false; end
end
end
%% feasiblePoint.m
function feasible=feasiblePoint(point,map)
feasible=true;
% check if collission-free spot and inside maps
% 0: obstacle 1: free space
if ~(point(1)>=1 && point(1)<=size(map,1) && point(2)>=1 && point(2)<=size(map,2) && map(point(1),point(2))==1)
feasible=false;
end
end
%% heuristic.m
function h=heuristic(X,goal)
h = sqrt(sum((X-goal).^2));
end
%% historic.m
function h=historic(a,b)
h = sqrt(sum((a-b).^2));
end MATLAB Robotics System Toolbox
MATLAB的robotics system toolbox中提供了PRM路徑規劃方法，可以很友善的建立一個probabilistic roadmap path planner來進行路徑規劃。使用時有下面幾點需要注意：
- Tune the Number of 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 coverage of the map, you might need a large number of nodes. Due to the random placement of nodes, some areas of the map may not have 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
map = robotics.OccupancyGrid(simpleMap, 2);
% Create a simple roadmap with 50 nodes.
prmSimple = robotics.PRM(map, 50);
show(prmSimple) % Create a dense roadmap with 250 nodes.
prmComplex = robotics.PRM(map,250);
show(prmComplex) - Tune the Connection Distance（調整連接配接距離）
Use the
ConnectionDistance
property on the
PRM
object to tune the algorithm.
ConnectionDistance
is an upper threshold for points that are connected in the roadmap. Each node is connected to all nodes within this connection distance that do not have obstacles between them. By lowering the connection distance, you can limit the number of connections to reduce 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 with simple maps, you can use a higher connection distance with a small number of nodes to increase efficiency. For complex maps with lots of obstacles, a higher number of nodes with a lowered 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 and see the effect of changing ConnectionDistance .
rngState = rng;
% Create a roadmap with 100 nodes and calculate the path. The default ConnectionDistance is set to inf.
prm = robotics.PRM(map, 100);
startLocation = [2 1];
endLocation = [12 10];
path = findpath(prm,startLocation,endLocation);
show(prm) % Reload the random number generation settings to have PRM use the same nodes
rng(rngState);
% Lower ConnectionDistance to 2 m
prm.ConnectionDistance = 2;
path = findpath(prm, startLocation, endLocation);
show(prm) - Create or Update PRM
This roadmap changes only if you call 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 connections to be recalculated. Because recalculating the map can be computationally intensive, you can reuse the same roadmap by calling
findpath
with different starting and ending locations. 即當使用update函數進行更新或者改變PRM對象屬性後，調用findpath、show等方法會引發重新計算。
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 into account the robot dimension while computing an obstacle free path on a map. Hence, you should inflate the map by the dimension of the robot, in order to allow computation of an obstacle free path that accounts for the robot's size and ensures collision avoidance for the actual robot. This inflation is used to add a factor of safety on obstacles and create buffer zones between the robot and obstacle in the environment. The
inflate
method of an occupancy grid object converts the specified
radius
to the number of cells rounded up from the
resolution*radius
value.
robotRadius = 0.2;
mapInflated = copy(map);
inflate(mapInflated,robotRadius);
show(mapInflated) - Find a Feasible Path on the Constructed PRM
Since you are planning a path on a large and complicated map, larger number of nodes may be required. However, often it is not clear how many nodes will be sufficient. Tune the number of nodes to make sure there is a feasible path between the start and end location.
path = findpath(prm, startLocation, endLocation) while isempty(path)
% No feasible path found yet, increase the number of nodes
prm.NumNodes = prm.NumNodes + 10;
% Use the |update| function to re-create the PRM roadmap with the changed attribute
update(prm);
% Search for a feasible path with the updated PRM
path = findpath(prm, startLocation, endLocation);
end 下面在VREP仿真軟體中搭建一個簡單的場景（修改practicalPathPlanningDemo.ttt）：在地圖正上方放置一個視覺傳感器采集地圖黑白圖像，綠色方塊代表起始位置，紅色方塊代表目标位置。然後使用MATLAB中的PRM進行路徑規劃。
MATLAB代碼如下：
function simpleTest()
disp('Program started');
vrep=remApi('remoteApi'); % using the prototype file (remoteApiProto.m)
vrep.simxFinish(-1); % just in case, close all opened connections
clientID=vrep.simxStart('127.0.0.1',19999,true,true,5000,5);
if (clientID>-1)
disp('Connected to remote API server');
[returnCode,sensorHandle] = vrep.simxGetObjectHandle(clientID,'Vision_sensor',vrep.simx_opmode_blocking);
[returnCode,objectHandle] = vrep.simxGetObjectHandle(clientID,'start',vrep.simx_opmode_blocking);
[returnCode,goalHandle] = vrep.simxGetObjectHandle(clientID,'goal',vrep.simx_opmode_blocking);
% Retrieves the image of a vision sensor as an image array(each image pixel is a byte (greyscale image))
[returnCode,resolution,image] = vrep.simxGetVisionSensorImage2(clientID, sensorHandle, 1, vrep.simx_opmode_blocking);
% Creates a BinaryOccupancyGrid object with resolution specified in cells per meter.
width = 5; height = 5; % Map width / height, specified as a double in meters.
grid = robotics.BinaryOccupancyGrid(image, resolution(1) / width);
grid.GridLocationInWorld = [-width/2, -height/2]; % world coordinates of the bottom-left corner of the grid
% Inflate the Map Based on Robot Dimension
inflate(grid, 0.1);
% Create a roadmap with 200 nodes and calculate the path
prm = robotics.PRM(grid, 200);
prm.ConnectionDistance = 1;
[returnCode,startLocation] = vrep.simxGetObjectPosition(clientID,objectHandle,-1,vrep.simx_opmode_blocking);
[returnCode,endLocation] = vrep.simxGetObjectPosition(clientID,goalHandle, -1,vrep.simx_opmode_blocking);
path = findpath(prm, double(startLocation(1:2)), double(endLocation(1:2)));
show(prm)
% Simply jump through the path points, no interpolation here:
for i=1 : size(path,1)
pos = [path(i,:), 0.05];
vrep.simxSetObjectPosition(clientID, objectHandle, -1, pos, vrep.simx_opmode_blocking);
pause(0.5);
end
% Now close the connection to V-REP:
vrep.simxFinish(clientID);
else
disp('Failed connecting to remote API server');
end
vrep.delete(); % call the destructor!
disp('Program ended');
end 下圖是MATLAB根據擷取到的黑白圖像建立的栅格圖，并進行路徑規劃的結果：
下圖可以看出方塊沿着生成的路徑移動到目标位置，隻是路徑還需要進一步優化。
參考：
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學習筆記：機器人路徑規劃
路徑規劃方法之-随機路徑圖法（PRM）
Motion Planning-UC Berkeley EECS
A*算法