基准的差动驱动机器人路径规划仓库地图

打开生活的脚本

这个例子展示了如何选择最好的二维微分驱动机器人的路径规划仓库环境的可用路径规划。使用plannerBenchmark对象的基准路径规划plannerRRT,plannerRRTStar,plannerBiRRT,plannerPRM,plannerHybridAstar对仓库环境的随机选择启动和目标。比较路径规划的基础上找到一个有效的路径的能力,清除障碍,时间初始化一个规划师,时间找到一个路径,路径的长度,平滑的路径。选择一个合适的计划基于每条路径规划性能的上述指标。

设置环境

创建一个binaryOccupancyMap对象从一个仓库地图。在仓库,移动机器人移动从装载站到将货物卸载站。

地图=负载(“wareHouseMap.mat”).logicalMap;= binaryOccupancyMap地图(地图);

可视化的地图。

图显示(地图)标题(“仓库平面图”)%设置文本显示在地图上的位置。loadingStationTextLoc = 40 9 [0];unloadingStationTextLoc = 42 0] [7.5;持有在文本(loadingStationTextLoc (1) loadingStationTextLoc (2), 1,“装载站”);文本(unloadingStationTextLoc (1) unloadingStationTextLoc (2), 1,“卸货站”);持有从

定义基准测试的环境和规划者

指定的最小转弯半径差驱动机器人。

minTurningRadius = 2.2;%米

创建一个stateSpaceDubins对象的状态空间边界地图一样的限制。设置最小图灵半径。

stateSpace = stateSpaceDubins ([map.XWorldLimits;map.YWorldLimits;[-ππ]]);stateSpace。MinTurningRadius = MinTurningRadius;

创建一个validatorOccupancyMap验证器与杜宾状态空间使用地图。指定的距离插值和验证路径段。

验证器= validatorOccupancyMap (stateSpace地图=地图);验证器。ValidationDistance = 0.01 * (1 / map.Resolution);%米

定义的函数处理每个规划者的初始化函数。在这些初始化函数的更多信息,请参阅初始化函数,规划者。

rrtInit = @ (validator) plannerRRTWrapper (validator);rrtStarInit = @ (validator) plannerRRTStarWrapper (validator);birrtInit = @ (validator) plannerBiRRTWrapper (validator);haStarInit = @ (validator) plannerHybridAStarWrapper(验证器,minTurningRadius);prmInit = @ (validator) plannerPRM (validator.StateSpace,验证器);

定义的函数处理计划功能,这是常见的规划者。

planFcn = @ (initOutput、启动目标)计划(initOutput,开始,目标);

随机选择Start-Goal对仓库地图上

开始和目标位置是随机抽样的装卸站区域,分别。指定start-goal双的数量必须是随机生成的。在本例中只有三个start-goal两人选择减少这个例子的执行时间。增加start-goal对得到足够的地图覆盖。

%设置默认随机数字结果的可重复性。rng (“默认”)%选择start-goal双的数量。numStartGoalPairs = 3;

机器人是采样的开始位置的矩形区域标记为装载站和目标位置是采样的矩形区域标记为卸货站区域。机器人位置取样均匀的区域。矩形区域被指定为一个向量的形式[x y w h]。x和y指定矩形的左下角的坐标。w和h指定矩形的宽度和高度。

loadingArea = (51.5 11 5 10);startLocations = helperSampleSelectedAreaOnMap(验证器,loadingArea numStartGoalPairs);unloadingArea =(2 43.5 27日15);goalLocations = helperSampleSelectedAreaOnMap(验证器,unloadingArea numStartGoalPairs);

可视化映射的随机采样启动和目标位置。

显示(map)标题(“仓库平面图”)%表示地图上的装卸站。持有在文本(loadingStationTextLoc (1) loadingStationTextLoc (2), 1,“装载站”);矩形(位置= loadingArea)文本(unloadingStationTextLoc (1) unloadingStationTextLoc (2), 1,“卸货站”);矩形(位置= unloadingArea)%设置线的长度代表start-goal可视化的姿势。r = 2;%显示所有start-goal对在地图上。为i = 1: numStartGoalPairs开始= startLocations(我:);目标= goalLocations(我:);%定义在地图上显示的传奇startString = strcat (“开始位置”num2str(我));goalString = strcat (“目标位置”num2str(我));%显示启动和目标位置的机器人。p1 =情节(开始(1,1),开始(1、2),“o”DisplayName = startString);c1 = p1.Color;p2 =情节(目标(1,1)、目标(1、2),“o”DisplayName = goalString);c2 = p2.Color;%显示启动和目标标题。情节([开始(1)开始(1)+ r * cos(开始(3))],[开始(2)开始(2)+ r * sin(开始(3))),…“-”颜色= c1, HandleVisibility =“关闭”)情节([目标(1)目标(1)+ r * cos(目标(3))],[目标(2)目标(2)+ r * sin(目标(3))),…“-”颜色= c2, HandleVisibility =“关闭”)结束持有从传奇(位置=“northeastoutside”)

规划师基准对象创建和运行基准

创建一个plannerBenchmark对象为每一对start-goal并添加基准的规划者。计划执行两次在同一环境和start-goal通过设置runCount价值2。这样可以确保度量结果准确,因为基于抽样的规划者plannerRRT,plannerRRTStar,plannerBiRRT,plannerPRM生成不同的输出在同一组start-goal一对。在这个例子中runCount值设置为2减少执行时间的例子。增加了runCount价值得到更精确的基准测试结果。

%为每个start-goal存储生成的基准对象。benchmarkList =细胞(1、numStartGoalPairs);%指定每个计划要执行的次数相同%的start-goal一对。runCount = 2;为i = 1:尺寸(startLocations, 1)%得到每个开始从所有的取样位置和目标位置。开始= startLocations(我:);目标= goalLocations(我:);%设置默认随机数字结果的可重复性。rng (“默认”)%建立基准对象。基准= plannerBenchmark(验证器,开始,目标);%为基准添加规划者使用初始化和计划功能%处理。额外的可选输入NumPlanOutput定义的数量%输出函数返回的计划。addPlanner(基准,planFcn rrtInit PlannerName =“rrt”NumPlanOutput = 2) addPlanner(基准,planFcn、rrtStarInit PlannerName =“rrtStar”NumPlanOutput = 2) addPlanner(基准,planFcn、birrtInit PlannerName =“biRRT”NumPlanOutput = 2) addPlanner(基准,planFcn、prmInit PlannerName =“plannerPRM”NumPlanOutput = 2) addPlanner(基准,planFcn、haStarInit PlannerName =“hybridAstar”NumPlanOutput = 2)%运行基准。runCount runPlanner(基准)%存储基准进行进一步分析。benchmarkList{我}=基准;结束

初始化rrt……完成了。规划一条从一开始对目标造成1.2229(55.5736 - 20.1338)使用rrt 1.9757 (27.9063 - 56.2369)。执行运行1。执行运行2。初始化rrtStar……完成了。规划一条从一开始对目标造成1.2229(55.5736 - 20.1338)使用rrtStar 1.9757 (27.9063 - 56.2369)。执行运行1。执行运行2。 Initializing biRRT ... Done. Planning a path from the start pose (55.5736 20.1338 1.2229) to the goal pose (27.9063 56.2369 1.9757) using biRRT. Executing run 1. Executing run 2. Initializing plannerPRM ... Done. Planning a path from the start pose (55.5736 20.1338 1.2229) to the goal pose (27.9063 56.2369 1.9757) using plannerPRM. Executing run 1. Executing run 2. Initializing hybridAstar ... Done. Planning a path from the start pose (55.5736 20.1338 1.2229) to the goal pose (27.9063 56.2369 1.9757) using hybridAstar. Executing run 1. Executing run 2. Initializing rrt ... Done. Planning a path from the start pose (56.029 17.3236 1.6444) to the goal pose (19.705 57.5099 1.9527) using rrt. Executing run 1. Executing run 2. Initializing rrtStar ... Done. Planning a path from the start pose (56.029 17.3236 1.6444) to the goal pose (19.705 57.5099 1.9527) using rrtStar. Executing run 1. Executing run 2. Initializing biRRT ... Done. Planning a path from the start pose (56.029 17.3236 1.6444) to the goal pose (19.705 57.5099 1.9527) using biRRT. Executing run 1. Executing run 2. Initializing plannerPRM ... Done. Planning a path from the start pose (56.029 17.3236 1.6444) to the goal pose (19.705 57.5099 1.9527) using plannerPRM. Executing run 1. Executing run 2. Initializing hybridAstar ... Done. Planning a path from the start pose (56.029 17.3236 1.6444) to the goal pose (19.705 57.5099 1.9527) using hybridAstar. Executing run 1. Executing run 2. Initializing rrt ... Done. Planning a path from the start pose (52.1349 11.9754 2.2894) to the goal pose (19.6979 43.9775 0.93797) using rrt. Executing run 1. Executing run 2. Initializing rrtStar ... Done. Planning a path from the start pose (52.1349 11.9754 2.2894) to the goal pose (19.6979 43.9775 0.93797) using rrtStar. Executing run 1. Executing run 2. Initializing biRRT ... Done. Planning a path from the start pose (52.1349 11.9754 2.2894) to the goal pose (19.6979 43.9775 0.93797) using biRRT. Executing run 1. Executing run 2. Initializing plannerPRM ... Done. Planning a path from the start pose (52.1349 11.9754 2.2894) to the goal pose (19.6979 43.9775 0.93797) using plannerPRM. Executing run 1. Executing run 2. Initializing hybridAstar ... Done. Planning a path from the start pose (52.1349 11.9754 2.2894) to the goal pose (19.6979 43.9775 0.93797) using hybridAstar. Executing run 1. Executing run 2.

在所有Start-Goal对平均指标

所有的计划执行runCount为每个start-goal两次。还所有的规划者执行start-goal对。这意味着所有计划执行runCount * numStartGoalPairs次了。下面的图和表显示平均跨所有start-goal对度量值。表表示的意思是,中位数,和标准偏差在所有start-goal对每一个指标的平均。

的间隙指标代表的路径的最小距离障碍环境中。图显示plannerPRM间隙最高。

helperPlotAveragedMetrics (benchmarkList runCount,“清除”)

clearanceAverage = helperCalculateAverageMetricTable (benchmarkList,“清除”)

clearanceAverage =5×3表意味着平均stdDev交________ rrt rrtStar 0.097631 1.069 - 1 1 1 0 biRRT 1 1 0 plannerPRM hybridAstar 1 1 0 0.4714 1.3333 - 1

的isPathValid度量代表成功的每个计划用百分比表示。图显示所有有效路径产生的路径规划start-goal对。

helperPlotAveragedMetrics (benchmarkList runCount,“isPathValid”)

isPathValidAverage = helperCalculateAverageMetricTable (benchmarkList,“isPathValid”)

isPathValidAverage =5×3表意味着平均stdDev ____交rrt 100 100 0 rrtStar 100 100 0 biRRT 100 100 0 plannerPRM 100 100 0 hybridAstar 100 100 0

的executionTime计划指标代表的时间函数来执行。图显示plannerPRM花最少的时间,其次是plannerBiRRT。我们也可以注意到plannerRRTStar花较少时间比plannerRRT,这可能是由于小start-goal对用于基准测试的数量。

helperPlotAveragedMetrics (benchmarkList runCount,“executionTime”)

execTimeAverage = helperCalculateAverageMetricTable (benchmarkList,“executionTime”)

execTimeAverage =5×3表意味着平均stdDev ________说rrt 0.54401 0.63826 0.14408 rrtStar biRRT 0.27133 0.2217 0.072599 0.25001 0.1615 0.185 plannerPRM hybridAstar 0.5594 0.70358 0.20847 0.032383 0.030569 0.017871

的initializationTime指标显示的时间来执行每个计划的初始化函数。因此总执行时间计划的总和时间执行时间和初始化函数。plannerPRM有最长的初始化时间。因此plannerBiRRT至少总执行时间。

helperPlotAveragedMetrics (benchmarkList runCount,“initializationTime”)

initTimeAverage = helperCalculateAverageMetricTable (benchmarkList,“initializationTime”)

initTimeAverage =5×3表意味着平均stdDev说_____ rrt 0.016502 0.01375 0.0058899 rrtStar biRRT 0.01058 0.011134 0.0019155 0.033354 0.022513 0.022307 plannerPRM hybridAstar 0.030674 0.013199 0.025763 3.331 3.1915 0.29031

的路径长度量表示生成的路径的长度。plannerHybridAstar的最短路径,其次是吗plannerBiRRT。

helperPlotAveragedMetrics (benchmarkList runCount,“通路长度”)

pathLengthAverage = helperCalculateAverageMetricTable (benchmarkList,“通路长度”)

pathLengthAverage =5×3表意味着看上去stdDev长得一样rrt中位数65.992 69.037 7.3361 rrtStar biRRT 64.426 67.992 5.3652 59.68 63.763 8.4716 plannerPRM hybridAstar 52.575 50.487 5.1352 103.67 103.34 1.3802

的平滑度指标代表的平滑路径构成。plannerBiRRT产生最平滑路径(低平滑值表明平滑路径)紧随其后plannerPRM。观察到plannerRRTStar产生相当平滑路径相比plannerRRT。

helperPlotAveragedMetrics (benchmarkList runCount,“平滑”)

smoothnessAverage = helperCalculateAverageMetricTable (benchmarkList,“平滑”)

smoothnessAverage =5×3表意味着平均stdDev ________说rrt 0.32247 0.31304 0.054903 rrtStar biRRT 0.21445 0.23351 0.038925 0.064741 0.029512 0.069532 plannerPRM hybridAstar 1.5048 1.647 0.21204 0.086357 0.097025 0.025469

如果稍长一些的路径不是一个问题然后考虑使用plannerBiRRT仓库地图的路径规划设置如图所示在本例中,因为它最平滑路径和较小的执行时间。如果乘坐机器人的路径应该是最少的plannerHybridAstar可以考虑。

可以看到,在执行时间不一致,plannerRRTStar以较小的时间比plannerRRT,增加start-goal对和的数量runCount到一个相当大的值将产生更准确的度量结果和更好的选择判断的路径规划。

可视化为特定Start-Goal对度量结果

上面的表格和图形显示,平均所有start-goal对度量结果。plannerBiRRT产生平滑路径总路径长度略长于plannerHybridAstar,产生最少的路径长度。探针start-goal对看到生产的最长路径长度plannerBiRRT。

numStartGoalPairs pathLengthMean = 0 (1);%遍历所有的基准和商店的平均路径长度% start-goal一对。为i = 1: numStartGoalPairs基准= benchmarkList {};pathLengthTable = benchmark.metric (“通路长度”);pathLengthMean (i) = pathLengthTable.Mean (“biRRT”);结束%找到生产的基准指数最大的平均路径长度plannerBiRRT %值。[~,largestPathLengthIdx] = max (pathLengthMean);benchmarkLargestPathlength = benchmarkList {largestPathLengthIdx};显示(benchmarkLargestPathlength“通路长度”)

通路长度=度量(benchmarkLargestPathlength,“通路长度”)

通路长度=5×4表意味着平均StdDev sampleSize交_________ __________ rrt 2 biRRT 2 rrtStar 73.058 73.058 3.3079 68.444 68.444 0.49012 67.393 - 67.393 102.18 - 102.18 1.3109 - 2 plannerPRM 0 2 hybridAstar 59.643 - 59.643 0 2

所有针对特定Start-Goal规划者对可视化路径

可视化输出的路径的所有规划者start-goal生产的最长路径长度plannerBiRRT。

%检索开始从基准对象和目标位置。开始= benchmarkLargestPathlength.Start;目标= benchmarkLargestPathlength.Goal;%显示路径从开始位置到目标位置的路径%规划者runCount的运行。为运行= 1:runCount图显示(map)标题([“运行路径输出所有规划者”num2str(运行)])在%显示启动和目标位置的机器人。情节(开始(1,1),开始(1、2),“o”)情节(目标(1,1)、目标(1、2),“o”)%用于基准测试计划名称。plannerNames =字段名(benchmarkLargestPathlength.PlannerOutput);numPlanners =长度(plannerNames);runString = strcat (“运行”num2str(运行));%的指定迭代和情节路径规划运行。为i = 1: numPlanners plannerName = plannerNames {};plannerOutput = benchmarkLargestPathlength.PlannerOutput。(plannerName) .PlanOutput。(runString);pathObj = plannerOutput {1};情节(pathObj.States (: 1) pathObj.States (:, 2),“-”,“线宽”,2)结束%指定传说。标签= [{“开始位置”,“目标位置”}plannerNames ');传奇(标签、位置=“northeastoutside”)举行从结束

初始化函数,规划者

初始化函数plannerHybridAStar。

函数规划师= plannerHybridAStarWrapper(验证器,minTurningRadius)地图= validator.Map;党卫军= stateSpaceSE2;sv = validatorOccupancyMap(党卫军,地图=地图);sv。ValidationDistance = validator.ValidationDistance;MinTurningRadius = MinTurningRadius规划师= plannerHybridAStar (sv);结束

初始化函数plannerBiRRT。

函数规划师= plannerBiRRTWrapper (sv)规划师= plannerBiRRT (sv.StateSpace sv);计划。EnableConnectHeuristic = true;计划。MaxIterations = 5 e4;计划。MaxNumTreeNodes = 5 e4;计划。MaxConnectionDistance = 5;结束

初始化函数plannerRRTStar。

函数规划师= plannerRRTStarWrapper (sv)规划师= plannerRRTStar (sv.StateSpace sv);计划。MaxIterations = 5 e5;计划。MaxNumTreeNodes = 5 e5;计划。MaxConnectionDistance = 3.8;结束

初始化函数plannerRRT。

函数规划师= plannerRRTWrapper (sv)规划师= plannerRRT (sv.StateSpace sv);计划。MaxIterations = 5 e5;计划。MaxNumTreeNodes = 5 e5;计划。MaxConnectionDistance = 3.8;结束