介紹
此示例演示如何使用概率路線圖（PRM）路徑規划器來計算給定地圖上兩個位置之間的無障礙路徑。PRM路徑規划器使用自由空間中的隨機采樣節點並將它們相互連接，從而在給定地圖的自由空間中構建路線圖。路線圖構建完成后，您可以查詢從給定開始位置到地圖上給定結束位置的路徑。
在這個例子中，地圖被表示為使用導入數據的占用網格地圖。在對地圖的空閑空間中的節點進行采樣時，PRM使用此二進制占用網格表示來推斷空閑空間。此外，PRM在計算地圖上的無障礙路徑時沒有考慮機器人尺寸。因此，您應該根據機器人的尺寸對地圖進行充氣，以便計算無障礙路徑，從而解決機器人的尺寸問題，並確保避免實際機器人的碰撞。定義地圖上的開始和結束位置，以便PRM路徑規划器查找無障礙路徑。
導入用於規划路徑的示例地圖
filePath = fullfile(fileparts(which('PathPlanningExample')),'data','exampleMaps.mat');
load(filePath)
導入的地圖是：simpleMap，complexMap和ternaryMap。這將在變量名稱中搜索包含字符串'Map'的變量。
whos *Map*
名稱大小字節類屬性 complexMap 41x52 2132邏輯 simpleMap 26x27 702邏輯 三元圖501x501 2008008 double
使用導入的simpleMap數據並使用robotics.BinaryOccupancyGrid
map = robotics.BinaryOccupancyGrid(simpleMap, 2)
map = 具有屬性的 BinaryOccupancyGrid ：GridSize：[26 27] 分辨率：2 XWorldLimits：[0 13.5000] YWorldLimits：[0 13] GridLocationInWorld：[0 0]
使用對象show上的函數顯示地圖robotics.BinaryOccupancyGrid
show(map)
定義機器人尺寸並填充地圖
為確保機器人不與任何障礙物碰撞，應在將機器人提供給PRM路徑規划器之前，按照機器人的尺寸對地圖充氣。
這里機器人的尺寸可以假定為半徑為0.2米的圓。然后，您可以使用該inflate函數通過此維度膨脹地圖。
robotRadius = 0.2;
如前所述，PRM不考慮機器人的尺寸，因此為PRM提供膨脹的地圖考慮了機器人尺寸。在使用該inflate功能保留原始地圖之前，創建地圖的副本。
mapInflated = copy(map); inflate(mapInflated,robotRadius);
顯示膨脹的地圖
show(mapInflated)
構建PRM並設置參數
現在你需要定義一個路徑規划器。創建一個robotics.PRM對象並定義關聯的屬性。
prm = robotics.PRM
prm = 具有屬性的PRM： 映射：[0x1 robotics.BinaryOccupancyGrid] NumNodes：50 ConnectionDistance：Inf
將充氣地圖分配給PRM對象
prm.Map = mapInflated;
定義在PRM構建過程中要使用的PRM節點的數量。PRM使用給定地圖上給定數量的節點構建路線圖。根據輸入地圖的維度和復雜性，這是要在地圖上的兩個點之間獲得解決方案的主要屬性之一。大量節點創建密集的路線圖並增加找到路徑的可能性。但是，擁有更多節點會增加創建路線圖和尋找解決方案的計算時間。
prm.NumNodes = 50;
定義地圖上兩個連接節點之間允許的最大距離。PRM連接地圖上以此距離（或更少）分隔的所有節點。這是調整較大和/或復雜輸入地圖的另一個屬性。較大的連接距離會增加節點之間的連通性，從而更容易找到路徑，但可以增加路線圖創建的計算時間。
prm.ConnectionDistance = 5;
找到構建PRM的可行路徑
在地圖上定義路徑規划器要使用的開始和結束位置。
startLocation = [2 1]; endLocation = [12 10];
使用該findpathpath由於PRM算法的概率性質，這將會有所不同。
path = findpath(prm, startLocation, endLocation)
路徑= 2.0000 1.0000 1.9569 1.0546 1.8369 2.3856 3.2389 6.6106 7.8260 8.1330 11.4632 10.5857 12.0000 10.0000
顯示PRM解決方案。
show(prm)
將PRM用於大型和復雜的地圖
使用導入的complexMap數據代表一個龐大而復雜的平面圖，並以給定的分辨率構建二進制占用網格表示（每米1個單元）
map = robotics.BinaryOccupancyGrid(complexMap, 1)
map = 具有屬性的 BinaryOccupancyGrid ：GridSize：[41 52] 解析：1 XWorldLimits：[0 52] YWorldLimits：[0 41] GridLocationInWorld：[0 0]
顯示地圖
show(map)
基於機器人尺寸填充地圖
復制並膨脹地圖以考慮機器人的尺寸以避開障礙物
mapInflated = copy(map); inflate(mapInflated, robotRadius);
顯示膨脹的地圖
show(mapInflated)
將現有的PRM對象與新的映射關聯並設置參數
用新膨脹的地圖更新PRM對象並定義其他屬性。
prm.Map = mapInflated;
設置NumNodes和ConnectionDistance屬性
prm.NumNodes = 20; prm.ConnectionDistance = 15;
顯示PRM圖
show(prm)
找到構建PRM的可行路徑
定義地圖上的開始和結束位置以查找無障礙路徑。
startLocation = [3 3]; endLocation = [45 35];
搜索開始和結束位置之間的解決方案。對於復雜的地圖，給定數量的節點可能沒有可行路徑（返回空路徑）。
path = findpath(prm, startLocation, endLocation)
path = []
由於您正在規划大型復雜地圖上的路徑，因此可能需要大量節點。但是，通常不清楚有多少節點就足夠了。調整節點的數量以確保開始和結束位置之間有可行的路徑。
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
% Display path
path
路徑= 3.0000 3.0000 4.2287 4.2628 7.7686 5.6520 6.8570 8.2389 19.5613 8.4030 33.1838 8.7614 31.3248 16.3874 41.3317 17.5090 48.3017 25.8527 49.4926 36.8804 44.3936 34.8592 45.0000 35.0000
顯示PRM解決方案
show(prm)
使用PRM與概率占用網格
robotics.OccupancyGrid使用導入的ternaryMap數據構造一個對象。的ternaryMap使用概率，其中占用的自由空間中的概率是0，對於占用空間是1和用於未知空間為0.5表示的環境。這里使用每米20個單元的分辨率。
map = robotics.OccupancyGrid(ternaryMap, 20)
map = OccupancyGrid屬性：占有 閾值：0.6500 FreeThreshold：0.2000 概率飽和度：[0.0010 0.9990] GridSize：[501 501] 分辨率：20 XWorldLimits：[0 25.0500] YWorldLimits：[0 25.0500] GridLocationInWorld：[0 0]
顯示地圖
show(map)
基於機器人尺寸填充地圖
復制並膨脹地圖以考慮機器人的尺寸以避開障礙物
mapInflated = copy(map); inflate(mapInflated, robotRadius);
顯示膨脹的地圖
show(mapInflated)
將現有的PRM對象與新的映射關聯並設置參數
用新膨脹的地圖更新PRM對象並定義其他屬性。PRM使用FreeThresholdOccupancyGrid對象來確定無障礙空間並計算該無障礙空間內的路徑。未知單元格的值ternaryMap是0.5，而FreeThresholdOccupancyGrid對象上的默認值mapInflated是0.2。因此，PRM不會在未知地區規划一條路徑。
prm.Map = mapInflated;
設置NumNodes和ConnectionDistance屬性
prm.NumNodes = 60; prm.ConnectionDistance = 5; % Display PRM graph show(prm)
找到構建PRM的可行路徑
定義地圖上的開始和結束位置以查找無障礙路徑。
startLocation = [7 22];
endLocation = [15 5];
% Search for a solution between start and end location.
path = findpath(prm, startLocation, endLocation);
while isempty(path)
prm.NumNodes = prm.NumNodes + 10;
update(prm);
path = findpath(prm, startLocation, endLocation);
end
% Display path
path
路徑= 7.0000 22.0000 7.9059 22.1722 10.6881 22.3734 11.7508 19.3716 13.7982 17.1659 17.5826 14.6769 16.8227 9.9964 15.1329 8.3100 14.9489 5.7648 15.0000 5.0000
顯示PRM解決方案
show(prm)
