越野车规划与数字高程模型
这个例子显示了如何处理和存储2.5 - d信息,并提出各种技术为使用它一场越野道路计划。
移动机器人规划往往涉及为轮式机器人找到最短路径的障碍。规划者们通常认为规划空间二维笛卡尔平面,与某些区域标记为限制由于障碍的存在。在路边车辆,环境也可以包含高程的变化,把这变成一个三维的问题。规划在高维空间的同时,再规划时期,因此一个有效的妥协可以计划在2.5 - d空间利用数字高程模型(民主党)。这个例子显示了创建和测试规划启发式的过程模拟,然后展示如何应用这些启发式达到2.5 - d路径规划一场越野规划师与真实的数据。
定义规划空间<年代pan id="mw_rtc_OffroadPlanningOnDigitalElevationMapsExample_H_620CB364" class="anchor_target">
创建计划表面<年代pan id="mw_rtc_OffroadPlanningOnDigitalElevationMapsExample_H_37A60AFC" class="anchor_target">
大多数环境模型创建从传感器安装在机器人,或从数据库检索基于估计构成的平台。创建一个表面使用山峰作为环境模型来测试不同的成本函数的影响计划。
%添加辅助文件夹路径目录<年代pan style="color:#A020F0">“分析”,<年代pan style="color:#A020F0">“数据”,<年代pan style="color:#A020F0">“启发式”,<年代pan style="color:#A020F0">“MapProcessing”);<年代pan style="color:#228B22">%建立表面(X, Y) = meshgrid (100:100);Z =山峰(max(大小(X))) * 1.5;<年代pan style="color:#228B22">%单位米X = X + 100;<年代pan style="color:#228B22">% X起源从中心移到角落里Y = Y + 100;<年代pan style="color:#228B22">% X起源从中心移到角落里Z (Z < 0) = 0;<年代pan style="color:#228B22">%丢弃山谷%可视化表面甘氨胆酸图ax =;冲浪(ax, X, Y, Z, EdgeColor =<年代pan style="color:#A020F0">“没有”)标题(<年代pan style="color:#A020F0">“测试地形”)包含(<年代pan style="color:#A020F0">“X”(m))ylabel (<年代pan style="color:#A020F0">“Y (m)”)zlabel (<年代pan style="color:#A020F0">“Z”(m))colormap (<年代pan style="color:#A020F0">“涡轮”斧头。PlotBoxAspectRatio = (1 1 0.15);视图([-29.59 - 21.30])
为2.5 - d空间定义启发式<年代pan id="mw_rtc_OffroadPlanningOnDigitalElevationMapsExample_H_284F7240" class="anchor_target">
接下来,定义启发式。这些启发式利用高程数据以不同的方式创建一个成本,<年代pan class="inlineequation">
,这可能导致改进与路径规划在只有两个维度:
这个成本函数增加计划状态(XY)前的表面高度(Z)计算距离。这将导致最短的路线通过2.5 - d歧管,平衡高程的变化对两点之间距离找到最直接的路径:
,在那里<年代pan class="inlineequation">
,
exampleHelperGradientHeuristic
这个成本函数添加一个成本与斜坡陡度的尺度<年代pan class="emphasis">相同的方向。与Z启发式,这惩罚不了距离,而且应该因此偏见规划师对路线,减少直接高程的变化,潜在节能:
exampleHelperRolloverHeuristic
这个成本函数类似exampleHelperGradientHeuristic,而是由翻转的X和Y值空间梯度,它惩罚运动的方向<年代pan class="emphasis">垂直的斜率。换句话说,这个函数应该最小化风险低卷角的展期通过寻求路线:
每个成本函数包含一个权重变量尺度相应的启发式。你可以调整重量优先最快的二维路由(重量= 0)或者可能更节能的道路(重量= 4)。
离散化和环境信息存储在映射层<年代pan id="mw_rtc_OffroadPlanningOnDigitalElevationMapsExample_H_8959BAB3" class="anchor_target">
成本函数的定义,我们知道从环境中获取的信息。离散三维表面,并将结果存储在示例mapLayer对象。这些提供笛卡尔坐标之间的转换和功能用于查询或修改数据,类似binaryOccupancyMap和occupancyMap对象。
构建Z-Layer
你可以直接从表面高度层和存储它mapLayer对象,有效地担任数字高程模型。创建一个层映射到包含表面的高度与地面杂波和障碍移除。
%查询和存储Z-heightzLayer = mapLayer (flipud (Z), LayerName =<年代pan style="color:#A020F0">“Z”);
计算梯度和转换成本
计算从Z-layer梯度和成本信息。首先,计算X和Y的梯度表面,并使用轮廓函数可视化梯度等值线图。
(gx, gy) =梯度(Z);图轮廓(X, Y, Z)标题(<年代pan style="color:#A020F0">“地形坡”)举行<年代pan style="color:#A020F0">在箭袋(X, Y, gx gy)轴<年代pan style="color:#A020F0">平等的持有<年代pan style="color:#A020F0">从
存储的X和Y梯度mapLayer对象。
dzdx = mapLayer (flipud (gx) LayerName =<年代pan style="color:#A020F0">“dzdx”);dzdy = mapLayer (flipud (gy) LayerName =<年代pan style="color:#A020F0">“dzdy”);
定义的函数exampleHelperGradientToCost,梯度值映射到一个成本。使其更直观应用cost-weighting变量,使用斜坡来确定加权成本。斜率值从0到规模最大斜率线性区间[0,1],和值大于最大坡度呈指数级增长:
在哪里<年代pan class="inlineequation">
是斜坡。定义的最大坡度角15度,并将其转化为最大斜率。
maxinclineangle = 15;<年代pan style="color:#228B22">%度maxSlope =罐内(maxinclineangle);<年代pan style="color:#228B22">%马克斯首选斜率车辆
未加权的形象化slope-to-cost函数。
slope2cost = @ (x) exampleHelperGradientToCost (maxSlope flipud (x));斜率= linspace (2 * maxSlope, 2 * maxSlope, 100);情节(斜率,slope2cost(斜率))<年代pan style="color:#A020F0">在参照线(maxSlope * [1],<年代pan style="color:#A020F0">r:“)yline (1,<年代pan style="color:#A020F0">”乙:“)标题(<年代pan style="color:#A020F0">“成本与坡”)包含(<年代pan style="color:#A020F0">“坡”)ylabel (<年代pan style="color:#A020F0">“成本”)轴<年代pan style="color:#A020F0">广场
用斜率计算成本函数,并将它们存储在映射层。
xCost = mapLayer (slope2cost (gx) LayerName =<年代pan style="color:#A020F0">“xCost”);yCost = mapLayer (slope2cost (gy) LayerName =<年代pan style="color:#A020F0">“yCost”);diagCost = mapLayer (slope2cost (sqrt (gx ^ 2 + gy。^ 2)), LayerName =<年代pan style="color:#A020F0">“diagCost”);
地形的障碍<年代pan id="mw_rtc_OffroadPlanningOnDigitalElevationMapsExample_H_DD9C433D" class="anchor_target">
除了海拔和坡度信息,您必须代表offlimits地区,由于高斜率或障碍。使用binaryOccupancyMap或occupancyMap对象来表示这些地区plannerAStarGrid规划师。
生成一个随机矩阵,然后所有元素值高于指定的阈值标记为障碍的位置。代表障碍的概率测试,随机概率相加值设置障碍。
rng (4)<年代pan style="color:#228B22">%设置RNG种子可重复的结果obstacleprob =兰德(max(大小(X)));
创建一个binaryOccupancyMap包含的障碍和膨胀的地形。
obstacleMap = binaryOccupancyMap (flipud (obstacleprob > 0.998), LayerName =<年代pan style="color:#A020F0">“障碍”);terrainObstacles = binaryOccupancyMap (obstacleMap LayerName =<年代pan style="color:#A020F0">“terrainObstacles”);充气(terrainObstacles 2);
检查每个单元对三个斜率约束,阻止细胞违反所有三个斜率约束。
obstaclesAndTerrain =双(getMapData (terrainObstacles));mInvalidSlope = getMapData (xCost) > 1 & getMapData (yCost) > 1 & getMapData (diagCost) > 1;obstaclesAndTerrain (mInvalidSlope) = true;setMapData (terrainObstacles obstaclesAndTerrain);
显示一个地图包含原始的障碍和地图包含的障碍和无效的地形。
图显示(obstacleMap父母=次要情节(1、2、1))显示(terrainObstacles、家长=次要情节(1、2、2)标题(<年代pan style="color:#A020F0">“障碍和Gradient-Restricted地区”)
结合个人地图层<年代pan id="mw_rtc_OffroadPlanningOnDigitalElevationMapsExample_H_3B9E54F1" class="anchor_target">
组织数据同步和保持你的层,添加图层multiLayerMap。这将确保更新共享一个图层的属性也更新,所以,数据总是代表了同一地区的笛卡尔空间。
costMap = multiLayerMap ({zLayer dzdx dzdy terrainObstacles obstacleMap xCost yCost diagCost});
使自我中心通过更新地图GridOriginInLocalX成本价值地图,地图,然后恢复到原来的设置。
xCost。GridOriginInLocal = -xCost.GridSize / xCost.Resolution / 2
xCost = mapLayer属性:DataSize mapLayer属性:(201 201)数据类型:“双”DefaultValue: 0 GridSize: 201年[201]LayerName:“xCost”GridLocationInWorld: [-100.5000 - -100.5000] GridOriginInLocal: [-100.5000 - -100.5000] LocalOriginInWorld:[0 0]分辨率:1 XLocalLimits: [-100.5000 - 100.5000] YLocalLimits: [-100.5000 - 100.5000] XWorldLimits: [-100.5000 - 100.5000] YWorldLimits: [-100.5000 - 100.5000] GetTransformFcn: [] SetTransformFcn: []
%恢复映射到原始设置costMap。GridOriginInLocal = [0 0];
可视化障碍的三层基于成本。在这些图表,注意斜率阈值如何影响成本:
灰色-自由空间
蓝色系的颜色,不同的成本低于软阈值(接近灰色,降低成本)
红色的阴影——不同成本高于软阈值(亮红色,高成本)
黑色——障碍和细胞违反斜率阈值在所有的方向
图(5)obstaclesAndTerrain (obstaclesAndTerrain < 1) =南;<年代pan style="color:#228B22">% 0转向nan costmaps策划显示(visualizationHelper xCost,次要情节(1、3、1),持有=<年代pan style="color:#A020F0">“上”、标题=<年代pan style="color:#A020F0">“X坡”阈值= 1);表面(flipud (obstaclesAndTerrain) FaceColor =[0 0 0])显示(visualizationHelper yCost,次要情节(1、3、2),持有=<年代pan style="color:#A020F0">“上”、标题=<年代pan style="color:#A020F0">“Y坡”阈值= 1);表面(flipud (obstaclesAndTerrain) FaceColor =[0 0 0])显示(visualizationHelper diagCost,次要情节(1,3,3)=<年代pan style="color:#A020F0">“上”、标题=<年代pan style="color:#A020F0">“斜坡”阈值= 1);表面(flipud (obstaclesAndTerrain) FaceColor = [0 0 0]) sgtitle (<年代pan style="color:#A020F0">“未加权的成本障碍”)
计划使用2.5 - d启发式路径<年代pan id="mw_rtc_OffroadPlanningOnDigitalElevationMapsExample_H_C6EFEAA7" class="anchor_target">
创建计划<年代pan id="mw_rtc_OffroadPlanningOnDigitalElevationMapsExample_H_FFE1A0FB" class="anchor_target">
创建一组规划者,每使用一个已定义的启发式,并比较他们的计划结果。
定义启动和目标位置并将他们转换成网格坐标。
起动位置= [54 131];goalposition = (143 - 104);开始= world2grid (costMap、起动位置);目标= world2grid (costMap goalposition);
分配一个G和H重量,并创建为每个定制启发式规划师。
gWeight = 1;hWeight = 1;defaultPlanner = plannerAStarGrid (getLayer (costMap<年代pan style="color:#A020F0">“terrainObstacles”));heightAwarePlanner = plannerAStarGrid (getLayer (costMap<年代pan style="color:#A020F0">“terrainObstacles”),<年代pan style="color:#0000FF">…GCostFcn = @ (s1, s2) exampleHelperZHeuristic (costMap gWeight, s1, s2),<年代pan style="color:#0000FF">…HCostFcn = @ (s1, s2) exampleHelperZHeuristic (costMap hWeight, s1, s2));gradientAwarePlanner = plannerAStarGrid (getLayer (costMap<年代pan style="color:#A020F0">“terrainObstacles”),<年代pan style="color:#0000FF">…GCostFcn = @ (s1, s2) exampleHelperGradientHeuristic (costMap gWeight, s1, s2),<年代pan style="color:#0000FF">…HCostFcn = @ (s1, s2) exampleHelperGradientHeuristic (costMap hWeight, s1, s2));rolloverAwarePlanner = plannerAStarGrid (getLayer (costMap<年代pan style="color:#A020F0">“terrainObstacles”),<年代pan style="color:#0000FF">…GCostFcn = @ (s1, s2) exampleHelperRolloverHeuristic (costMap gWeight, s1, s2),<年代pan style="color:#0000FF">…HCostFcn = @ (s1, s2) exampleHelperRolloverHeuristic (costMap hWeight, s1, s2));
计划路径<年代pan id="mw_rtc_OffroadPlanningOnDigitalElevationMapsExample_H_437C824A" class="anchor_target">
计划从一开始就与每个启发式路径的目标并将它们存储在一个单元阵列。使用抽搐和toc测量的时间为每一个启发式规划计划他们的路径。
%规划者返回一系列IJ细胞路径=细胞(4,1);路径名= [<年代pan style="color:#A020F0">“默认”,<年代pan style="color:#A020F0">“Elevation-Aware路径”,<年代pan style="color:#A020F0">“Gradient-Aware路径”,<年代pan style="color:#A020F0">“Rollover-Aware路径”];planningTime =南(4,1);抽搐;路径{1}=计划(defaultPlanner,开始,目标);planningTime (1) = toc;路径{2}=计划(heightAwarePlanner,开始,目标);planningTime (2) = toc-planningTime (1);路径{3}=计划(gradientAwarePlanner,开始,目标);planningTime (3) = toc-planningTime (2);路径{4}=计划(rolloverAwarePlanner,开始,目标); planningTime(4) = toc-planningTime(3);
比较和分析结果<年代pan id="mw_rtc_OffroadPlanningOnDigitalElevationMapsExample_H_90CD3B27" class="anchor_target">
比较每个规划师通过定义一组的性能指标和运动沿着每条路径的可视化。这个示例模型平台作为一个简单的地面车辆,并使一些简化假设相关的操作所需要的能量的机器人。编辑这些参数如何规划结果的影响。
参数定义机器人和场景<年代pan id="mw_rtc_OffroadPlanningOnDigitalElevationMapsExample_H_3C7318F9" class="anchor_target">
%定义仿真参数scenarioParams。重力= 9.81;<年代pan style="color:#228B22">% m / s ^ 2scenarioParams。UpdateRate = 1/60;<年代pan style="color:#228B22">%赫兹%定义车辆参数估计功耗scenarioParams.Robot。质量=<年代pan class="live_control_container">70年 ;<年代pan style="color:#228B22">%公斤,没有有效载荷scenarioParams.Robot。速度=<年代pan class="live_control_container">0.6 ;<年代pan style="color:#228B22">% m / sscenarioParams.Robot。AmpHour =<年代pan class="live_control_container">24 ;<年代pan style="color:#228B22">%关系scenarioParams.Robot。Vnom = 24;<年代pan style="color:#228B22">% V电压scenarioParams.Robot。RegenerativeBrakeEfficiency =<年代pan class="live_control_container">0 ;
定义性能指标<年代pan id="mw_rtc_OffroadPlanningOnDigitalElevationMapsExample_H_E35DC0BC" class="anchor_target">
分析,使用三维路径距离、旅行时间、规划时间和能量消耗的一个粗略的测量路径的效率。还定义的最小、最大和平均俯仰和滚车辆的角度,比较每条路径的相对质量。有关更多信息,请参见exampleHelperCalculateMetrics。
%显示结果图animateResults = true;statTable = exampleHelperCalculateMetrics (planningTime costMap scenarioParams,路径,路径名)
statTable =<年代pan class="emphasis">4×8表距离(米)权力(W)旅行时间(s)计划时间(s)马克斯音高(度)平均间距(度)马克斯辊(度)平均辊(度)_______ _____售予_________________售予_____________默认___________________ * * * 101.2 226.85 2.8112 0.044133 13.744 6.4108 18.797 7.4844 Elevation-Aware路径100.7 218.13 2.7972 2.9829 12.274 4.1055 14.542 3.9993 Gradient-Aware路径109.97 214.76 3.0547 10.855 12.395 3.4126 19.916 9.0152 Rollover-Aware路径103.19 218.03 2.8665 - 7.5138 14.751 - 4.5569 4.4032 - 0.72387
可视化结果使用的路径exampleHelperVisualizeResults。这个helper函数项目前面生成的路径上的所有三维表面。
exampleHelperVisualizeResults (gca、statTable scenarioParams costMap,路径,路径名,~ animateResults);
可视化路径结果<年代pan id="mw_rtc_OffroadPlanningOnDigitalElevationMapsExample_H_0B38072B" class="anchor_target">
可视化后的机器人每个计划的路径。
图exampleHelperVisualizeResults (gca, statTable、scenarioParams costMap,路径,路径名,animateResults);
分析结果,elevation-aware规划者提供了最好的结果最低3 d距离和时间,并保持能耗相对较低,但elevation-aware规划师并未引导机器人远离地区陡坡或高卷角。默认的规划师也需要一个简短的路线,即使高度包含在距离度量,但它消耗额外的能量升序和降序elevation-aware规划师避免斜坡上,导致了更高的能耗。
基于的梯度规划者贸易路线安全距离和时间,消耗更少的能量相比其他规划者。gradient-aware规划师最小化时间向上攀升,山,而rollover-aware规划师寻求路径穿越地形时保持水平。这些结果反映在俯仰和滚指标,gradient-aware规划师显示最低的平均间距,和rollover-aware规划师显示最大和平均辊角度远低于其他规划者。
计划在实际数字高程模型数据
现在您已经测试了启发式的人工表面,测试它们使用真实的高程数据。
现实世界的数据导入<年代pan id="mw_rtc_OffroadPlanningOnDigitalElevationMapsExample_H_1AB306E5" class="anchor_target">
可以导入数字高程数据从不同的来源。这个示例使用TIF文件下载美国地质调查局国家土地覆盖数据库马纳萨斯国家战场遗址公园包含高程数据,在美国维吉尼亚州。如果你想下载这个文件,遵循这些步骤:
数据库中的地址栏、搜索“弗吉尼亚州马纳萨斯国家战场遗址公园”。
放大到感兴趣的地区。
在<年代trong class="emphasis bold">数据集选项卡,在<年代trong class="emphasis bold">数据中,选择<年代trong class="emphasis bold">提升产品(3-DEP下载188bet金宝搏)>1米民主党,然后单击<年代trong class="emphasis bold">搜索产品下载188bet金宝搏。
下载你想要的文件覆盖该地区的高程数据。
下载后,您可以提取使用的气管无名动脉瘘管的高程数据文件readgeoraster的函数映射工具箱™:
注意:这段代码需要映射的工具箱
%的地形数据加载(海拔、信息)= readgeoraster (<年代pan style="color:#A020F0">“USGS_1m_x28y430_VA_Fairfax_County_2018.tif”OutputType =<年代pan style="color:#A020F0">“替身”);res = 2.5;<年代pan style="color:#228B22">%选择地形数据的一部分Zinit = flipud(海拔(700:1100 800:1200));[Xinit, Yinit] = meshgrid(1:长度(Zinit));<年代pan style="color:#228B22">%更好的匹配与图像分辨率进行插值[Xreal, Yreal] = meshgrid (1: (1 / res):长度(Zinit));Zreal = interp2 (Xinit Yinit、Zinit Xreal, Yreal);
对于这个示例,已经处理的数据并存储在一个垫子文件。为感兴趣的次区域加载进行后期处理数据。
%保存表面数据加载和解析负载(<年代pan style="color:#A020F0">“manassasData.mat”);<年代pan style="color:#228B22">%可视化表面冲浪(Xreal Yreal、Zreal EdgeColor =<年代pan style="color:#A020F0">“没有”)formatAxes (visualizationHelper标题=<年代pan style="color:#A020F0">“马纳萨斯战场地形”colormap =<年代pan style="color:#A020F0">“喷气机”)pbaspect ([1 1 0.2])
负载相同的位置的卫星图像
img = imread (<年代pan style="color:#A020F0">“visitorcenter_satellitesq.jpg”);imgResized = imresize (img, 1001年[1001]);imshow (img)
创建数据层<年代pan id="mw_rtc_OffroadPlanningOnDigitalElevationMapsExample_H_185E500F" class="anchor_target">
重复相同的过程计算梯度和转换成本部分创建成本梯度和梯度层。
图(gxReal gyReal] =梯度(Zreal);轮廓(Xreal Yreal Zreal) formatAxes (visualizationHelper、标题=<年代pan style="color:#A020F0">“地形坡”,=<年代pan style="color:#A020F0">“上”colormap =<年代pan style="color:#A020F0">“喷气机”)箭袋(Xreal Yreal、gxReal gyReal)
卫星图像转换成terrain-obstacle地图。
%建立数据层zLayerReal = mapLayer (flipud (Zreal),分辨率= res, LayerName =<年代pan style="color:#A020F0">“Z”);dzdxReal = mapLayer (flipud (gxReal),分辨率= res, LayerName =<年代pan style="color:#A020F0">“dzdx”);dzdyReal = mapLayer (flipud (gyReal),分辨率= res, LayerName =<年代pan style="color:#A020F0">“dzdy”);xCostReal = mapLayer (slope2cost (flipud (gxReal)),分辨率= res, LayerName =<年代pan style="color:#A020F0">“xCost”);yCostReal = mapLayer (slope2cost (flipud (gyReal)),分辨率= res, LayerName =<年代pan style="color:#A020F0">“yCost”);diagCostReal = mapLayer (slope2cost (flipud (√gxReal。^ 2 + gyReal。^ 2))),分辨率= res, LayerName =<年代pan style="color:#A020F0">“diagCost”);
因为头顶的卫星图像是可用的,你可以尝试申请的成本或无效区域地图基于缺乏或植被的存在。使用三个过滤器:一个突出颜色光谱强度较低(道路),另一个重点是绿色(田野),和第三个突出棕色和黄色(高草)。
%利用阈值分割图像[BWroads, roadRGB] = exampleHelperHighlightRoad (imgResized);[BWfields, fieldRGB] = exampleHelperHighlightField (imgResized);[BWbrush, brushRGB] = exampleHelperHighlightBrush (imgResized);图imshow (roadRGB父母=次要情节(1、3、1)标题(<年代pan style="color:#A020F0">“停机坪”)imshow (fieldRGB、家长=次要情节(1、3、2)标题(<年代pan style="color:#A020F0">“低草”)imshow (brushRGB、家长=次要情节(1,3,3)标题(<年代pan style="color:#A020F0">“刷”)
结合这三个地图创建地形努力成本以下重量为每个类型的地形:
路或停机坪上,0
低草-1
高的刷子,3
数一数这些类别的每个单元格合格,并计算每个单元的成本。
数=双(BWroads) +双(BWfields) +双(BWbrush);terrainCost =(双(BWfields) +双(BWbrush) * 3) /计数;
取代南值与值插值从邻居。
[ii, jj] = meshgrid(1:尺寸(terrainCost, 1), 1:尺寸(terrainCost, 2));一转眼= isnan (terrainCost);terrainCost(女人)= max (griddata (ii(~阿齐兹·伊南),jj(~阿齐兹·伊南),terrainCost(~阿齐兹·伊南),二(女人),jj(女人),<年代pan style="color:#A020F0">“线性”),0);
为简单起见,这个例子是任何地形斜率或表面成本超过给定的阈值作为占领,但这些成本可以被纳入现有的或新的成本函数。
地形成本插入一层,定义一个最大地形成本。改变地形的最大成本会影响地图地形二进制入住率眼中一个障碍。
terrainLayer = mapLayer (terrainCost分辨率= res LayerName =<年代pan style="color:#A020F0">“surfaceCost”);maxTerrainCost =<年代pan class="live_control_container">2.5 ;
地形成本转换成一个binaryOccupancyMap对象。
terrainObstaclesReal = binaryOccupancyMap (terrainLayer。getMapData > = maxTerrainCost分辨率= res, LayerName =<年代pan style="color:#A020F0">“terrainObstacles”);
膨胀slope-blocked细胞并设置障碍。
充气(terrainObstaclesReal .25);terrainData = getMapData (terrainObstaclesReal);mInvalidSlope = getMapData (xCostReal) > 1 & getMapData (yCostReal) > 1 & getMapData (diagCostReal) > 1;terrainData (mInvalidSlope) = true;setMapData (terrainObstaclesReal terrainData);
地图显示地形成本和占用并排。
图显示(visualizationHelper terrainLayer,次要情节(1、2、1),标题=<年代pan style="color:#A020F0">“地形表面成本”阈值= maxTerrainCost)显示(terrainObstaclesReal父母=次要情节(1、2、2))
结合层映射到成本multiLayerMap对象。
costMapReal = multiLayerMap ({zLayerReal dzdxReal dzdyReal terrainObstaclesReal xCostReal yCostReal diagCostReal});
创建一个计划配置通过配置的g值和h-cost功能。探索不同的配置如何影响计划的路径。
配置g值函数
函数设置为GCostFcn的属性plannerAStarGrid计算两个节点之间的距离(或成本)的计划图。规划师然后添加根节点之间的成本,成本和当前节点形成了g值:
gCostType =<年代pan class="live_control_container">“gDefault” ;gFcnChoices = exampleHelperFcnOptions (<年代pan style="color:#A020F0">“函数式”,gCostType);gWeightRange = exampleHelperFcnOptions (<年代pan style="color:#A020F0">“体重”gCostType,<年代pan style="color:#A020F0">“g”);gWLow = gWeightRange (1);gWHigh = gWeightRange (2);gStep = gWeightRange (3);gWeight =<年代pan class="live_control_container">1 ;vArgs = exampleHelperFcnOptions (<年代pan style="color:#A020F0">“成本函数输入”,gCostType);gCostSetting = exampleHelperFcnOptions (<年代pan style="color:#A020F0">“功能选择”,<年代pan class="live_control_container">gFcnChoices (1) ,vArgs {:});
配置H-Cost函数
的hCostFcn规划师的计算<年代pan class="emphasis">cost-to-go估计成本,在当前节点和目标节点之间。h-cost作为启发式指导规划师在探索。为了满足最优性的保证,它还应该被低估了cost-to-go:
hCostType =<年代pan class="live_control_container">“hDefault” ;hFcnChoices = exampleHelperFcnOptions (<年代pan style="color:#A020F0">“函数式”,hCostType);hWeightRange = exampleHelperFcnOptions (<年代pan style="color:#A020F0">“体重”,hCostType);hWLow = hWeightRange (1);hWHigh = hWeightRange (2);hStep = hWeightRange (3);hWeight =<年代pan class="live_control_container">1 ;vArgs = exampleHelperFcnOptions (<年代pan style="color:#A020F0">“成本函数输入”,hCostType);hCostSetting = exampleHelperFcnOptions (<年代pan style="color:#A020F0">“功能选择”,<年代pan class="live_control_container">hFcnChoices (1) ,vArgs {:});
一起加入g值和h-cost计算f值。*规划师使用f值优先探索下一个节点图:
构建*规划师与选定的成本函数。
aStarPlanner = plannerAStarGrid (getLayer (costMapReal<年代pan style="color:#A020F0">“terrainObstacles”),hCostSetting {:}, gCostSetting {:});
计划路径
定义启动和目标位置并将它们转换为网格坐标。然后,开始计划之间的路径和目标的位置。
起动位置= (290 - 300);goalposition = (95 - 165);开始= world2grid (costMapReal、起动位置);目标= world2grid (costMapReal goalposition);抽搐;pathReal =计划(aStarPlanner,开始,目标);planningTimeReal = toc;
审查结果<年代pan id="mw_rtc_OffroadPlanningOnDigitalElevationMapsExample_H_DA0D639F" class="anchor_target">
计划路径的计算和查看结果。
statTableReal = exampleHelperCalculateMetrics (costMapReal scenarioParams、pathReal planningTimeReal,<年代pan style="color:#A020F0">“用户配置计划”)
statTableReal =<年代pan class="emphasis">表1×8距离(米)权力(W)旅行时间(s)计划时间(s)马克斯音高(度)平均间距(度)马克斯辊(度)平均辊(度)_______ _____售予_________________售予_____________ ___________________ * * *用户配置计划264.16 197.87 7.3377 - 1.2672 2.7563 - 0.7005 1.8954 - 0.77303
显示计划路径图的区域。
图exampleHelperVisualizeResults (gca, statTableReal、scenarioParams costMapReal, {pathReal},<年代pan style="color:#A020F0">“用户配置计划”~ animateResults)
这个示例介绍了越野车的概念规划为移动机器人。您了解了如何应用二维规划者3 d搜索通过提供定制的成本函数,利用2.5 - d信息。这通常2.5 - d信息数字高程模型的形式,可以从数据库的采购美国地质调查局国家土地覆盖数据库,或者从机载传感器实时构造。最后,这个例子向您展示了如何将这些信息存储在地图,如何构造不同的成本函数,以及如何分析这些功能的影响路径效率和质量。
这个例子显示了如何处理和存储2.5 - d信息,并提出各种技术为使用它一场越野道路计划。 移动机器人规划往往涉及为轮式机器人找到最短路径的障碍。规划者们通常认为规划空间二维笛卡尔平面,与某些区域标记为限制由于障碍的存在。在路边车辆,环境也可以包含高程的变化,把这变成一个三维的问题。规划在高维空间的同时,再规划时期,因此一个有效的妥协可以计划在2.5 - d空间利用数字高程模型(民主党)。这个例子显示了创建和测试规划启发式的过程模拟,然后展示如何应用这些启发式达到2.5 - d路径规划一场越野规划师与真实的数据。 大多数环境模型创建从传感器安装在机器人,或从数据库检索基于估计构成的平台。创建一个表面使用 接下来,定义启发式。这些启发式利用高程数据以不同的方式创建一个成本,<年代pan class="inlineequation">
,这可能导致改进与路径规划在只有两个维度: 这个成本函数增加计划状态(XY)前的表面高度(Z)计算距离。这将导致最短的路线通过2.5 - d歧管,平衡高程的变化对两点之间距离找到最直接的路径:
,在那里<年代pan class="inlineequation">
, 这个成本函数添加一个成本与斜坡陡度的尺度<年代pan class="emphasis">相同
这个成本函数类似
每个成本函数包含一个权重变量尺度相应的启发式。你可以调整重量优先最快的二维路由(重量= 0)或者可能更节能的道路(重量= 4)。 成本函数的定义,我们知道从环境中获取的信息。离散三维表面,并将结果存储在示例 你可以直接从表面高度层和存储它 计算从Z-layer梯度和成本信息。首先,计算X和Y的梯度表面,并使用 存储的X和Y梯度 定义的函数
在哪里<年代pan class="inlineequation">
是斜坡。定义的最大坡度角 未加权的形象化slope-to-cost函数。 用斜率计算成本函数,并将它们存储在映射层。 除了海拔和坡度信息,您必须代表offlimits地区,由于高斜率或障碍。使用 生成一个随机矩阵,然后所有元素值高于指定的阈值标记为障碍的位置。代表障碍的概率测试,随机概率相加值设置障碍。 创建一个 检查每个单元对三个斜率约束,阻止细胞违反所有三个斜率约束。 显示一个地图包含原始的障碍和地图包含的障碍和无效的地形。 组织数据同步和保持你的层,添加图层 使自我中心通过更新地图 可视化障碍的三层基于成本。在这些图表,注意斜率阈值如何影响成本: 灰色-自由空间 蓝色系的颜色,不同的成本低于软阈值(接近灰色,降低成本) 红色的阴影——不同成本高于软阈值(亮红色,高成本) 黑色——障碍和细胞违反斜率阈值在所有的方向 创建一组规划者,每使用一个已定义的启发式,并比较他们的计划结果。 定义启动和目标位置并将他们转换成网格坐标。 分配一个G和H重量,并创建为每个定制启发式规划师。 计划从一开始就与每个启发式路径的目标并将它们存储在一个单元阵列。使用 比较每个规划师通过定义一组的性能指标和运动沿着每条路径的可视化。这个示例模型平台作为一个简单的地面车辆,并使一些简化假设相关的操作所需要的能量的机器人。编辑这些参数如何规划结果的影响。 分析,使用三维路径距离、旅行时间、规划时间和能量消耗的一个粗略的测量路径的效率。还定义的最小、最大和平均俯仰和滚车辆的角度,比较每条路径的相对质量。有关更多信息,请参见 可视化结果使用的路径 可视化后的机器人每个计划的路径。 分析结果,elevation-aware规划者提供了最好的结果最低3 d距离和时间,并保持能耗相对较低,但elevation-aware规划师并未引导机器人远离地区陡坡或高卷角。默认的规划师也需要一个简短的路线,即使高度包含在距离度量,但它消耗额外的能量升序和降序elevation-aware规划师避免斜坡上,导致了更高的能耗。 基于的梯度规划者贸易路线安全距离和时间,消耗更少的能量相比其他规划者。gradient-aware规划师最小化时间向上攀升,山,而rollover-aware规划师寻求路径穿越地形时保持水平。这些结果反映在俯仰和滚指标,gradient-aware规划师显示最低的平均间距,和rollover-aware规划师显示最大和平均辊角度远低于其他规划者。 现在您已经测试了启发式的人工表面,测试它们使用真实的高程数据。 可以导入数字高程数据从不同的来源。这个示例使用TIF文件下载 数据库中的地址栏、搜索“弗吉尼亚州马纳萨斯国家战场遗址公园”。 放大到感兴趣的地区。 在<年代trong class="emphasis bold">数据集 下载你想要的文件覆盖该地区的高程数据。 下载后,您可以提取使用的气管无名动脉瘘管的高程数据文件 注意:这段代码需要映射的工具箱 对于这个示例,已经处理的数据并存储在一个垫子文件。为感兴趣的次区域加载进行后期处理数据。 负载相同的位置的卫星图像 重复相同的过程 卫星图像转换成terrain-obstacle地图。 因为头顶的卫星图像是可用的,你可以尝试申请的成本或无效区域地图基于缺乏或植被的存在。使用三个过滤器:一个突出颜色光谱强度较低(道路),另一个重点是绿色(田野),和第三个突出棕色和黄色(高草)。 结合这三个地图创建地形努力成本以下重量为每个类型的地形: 路或停机坪上, 低草- 高的刷子, 数一数这些类别的每个单元格合格,并计算每个单元的成本。 取代 为简单起见,这个例子是任何地形斜率或表面成本超过给定的阈值作为占领,但这些成本可以被纳入现有的或新的成本函数。 地形成本插入一层,定义一个最大地形成本。改变地形的最大成本会影响地图地形二进制入住率眼中一个障碍。 地形成本转换成一个 膨胀slope-blocked细胞并设置障碍。 地图显示地形成本和占用并排。 结合层映射到成本 创建一个计划配置通过配置的g值和h-cost功能。探索不同的配置如何影响计划的路径。 配置g值函数 函数设置为
配置H-Cost函数 的
一起加入g值和h-cost计算f值。*规划师使用f值优先探索下一个节点图:
构建*规划师与选定的成本函数。 定义启动和目标位置并将它们转换为网格坐标。然后,开始计划之间的路径和目标的位置。 计划路径的计算和查看结果。 显示计划路径图的区域。 这个示例介绍了越野车的概念规划为移动机器人。您了解了如何应用二维规划者3 d搜索通过提供定制的成本函数,利用2.5 - d信息。这通常2.5 - d信息数字高程模型的形式,可以从数据库的采购定义规划空间<年代pan id="mw_rtc_OffroadPlanningOnDigitalElevationMapsExample_H_620CB364" class="anchor_target">
创建计划表面<年代pan id="mw_rtc_OffroadPlanningOnDigitalElevationMapsExample_H_37A60AFC" class="anchor_target">
山峰
%添加辅助文件夹路径
为2.5 - d空间定义启发式<年代pan id="mw_rtc_OffroadPlanningOnDigitalElevationMapsExample_H_284F7240" class="anchor_target">
exampleHelperGradientHeuristic
exampleHelperRolloverHeuristic
离散化和环境信息存储在映射层<年代pan id="mw_rtc_OffroadPlanningOnDigitalElevationMapsExample_H_8959BAB3" class="anchor_target">
mapLayer
binaryOccupancyMap
occupancyMap
构建Z-Layer
%查询和存储Z-height
计算梯度和转换成本
轮廓
(gx, gy) =梯度(Z);图轮廓(X, Y, Z)标题(<年代pan style="color:#A020F0">“地形坡”
dzdx = mapLayer (flipud (gx) LayerName =<年代pan style="color:#A020F0">“dzdx”
exampleHelperGradientToCost
maxinclineangle = 15;<年代pan style="color:#228B22">%度
slope2cost = @ (x) exampleHelperGradientToCost (maxSlope flipud (x));斜率= linspace (2 * maxSlope, 2 * maxSlope, 100);情节(斜率,slope2cost(斜率))<年代pan style="color:#A020F0">在
xCost = mapLayer (slope2cost (gx) LayerName =<年代pan style="color:#A020F0">“xCost”
地形的障碍<年代pan id="mw_rtc_OffroadPlanningOnDigitalElevationMapsExample_H_DD9C433D" class="anchor_target">
plannerAStarGrid
rng (4)<年代pan style="color:#228B22">%设置RNG种子可重复的结果
obstacleMap = binaryOccupancyMap (flipud (obstacleprob > 0.998), LayerName =<年代pan style="color:#A020F0">“障碍”
obstaclesAndTerrain =双(getMapData (terrainObstacles));mInvalidSlope = getMapData (xCost) > 1 & getMapData (yCost) > 1 & getMapData (diagCost) > 1;obstaclesAndTerrain (mInvalidSlope) = true;setMapData (terrainObstacles obstaclesAndTerrain);
图显示(obstacleMap父母=次要情节(1、2、1))显示(terrainObstacles、家长=次要情节(1、2、2)标题(<年代pan style="color:#A020F0">“障碍和Gradient-Restricted地区”
结合个人地图层<年代pan id="mw_rtc_OffroadPlanningOnDigitalElevationMapsExample_H_3B9E54F1" class="anchor_target">
multiLayerMap
costMap = multiLayerMap ({zLayer dzdx dzdy terrainObstacles obstacleMap xCost yCost diagCost});
xCost。GridOriginInLocal = -xCost.GridSize / xCost.Resolution / 2
xCost = mapLayer属性:DataSize mapLayer属性:(201 201)数据类型:“双”DefaultValue: 0 GridSize: 201年[201]LayerName:“xCost”GridLocationInWorld: [-100.5000 - -100.5000] GridOriginInLocal: [-100.5000 - -100.5000] LocalOriginInWorld:[0 0]分辨率:1 XLocalLimits: [-100.5000 - 100.5000] YLocalLimits: [-100.5000 - 100.5000] XWorldLimits: [-100.5000 - 100.5000] YWorldLimits: [-100.5000 - 100.5000] GetTransformFcn: [] SetTransformFcn: []
%恢复映射到原始设置
图(5)obstaclesAndTerrain (obstaclesAndTerrain < 1) =南;<年代pan style="color:#228B22">% 0转向nan costmaps策划
计划使用2.5 - d启发式路径<年代pan id="mw_rtc_OffroadPlanningOnDigitalElevationMapsExample_H_C6EFEAA7" class="anchor_target">
创建计划<年代pan id="mw_rtc_OffroadPlanningOnDigitalElevationMapsExample_H_FFE1A0FB" class="anchor_target">
起动位置= [54 131];goalposition = (143 - 104);开始= world2grid (costMap、起动位置);目标= world2grid (costMap goalposition);
gWeight = 1;hWeight = 1;defaultPlanner = plannerAStarGrid (getLayer (costMap<年代pan style="color:#A020F0">“terrainObstacles”
计划路径<年代pan id="mw_rtc_OffroadPlanningOnDigitalElevationMapsExample_H_437C824A" class="anchor_target">
抽搐
toc
%规划者返回一系列IJ细胞
比较和分析结果<年代pan id="mw_rtc_OffroadPlanningOnDigitalElevationMapsExample_H_90CD3B27" class="anchor_target">
参数定义机器人和场景<年代pan id="mw_rtc_OffroadPlanningOnDigitalElevationMapsExample_H_3C7318F9" class="anchor_target">
%定义仿真参数
定义性能指标<年代pan id="mw_rtc_OffroadPlanningOnDigitalElevationMapsExample_H_E35DC0BC" class="anchor_target">
exampleHelperCalculateMetrics
%显示结果
statTable =<年代pan class="emphasis">4×8表
exampleHelperVisualizeResults
exampleHelperVisualizeResults (gca、statTable scenarioParams costMap,路径,路径名,~ animateResults);
可视化路径结果<年代pan id="mw_rtc_OffroadPlanningOnDigitalElevationMapsExample_H_0B38072B" class="anchor_target">
图exampleHelperVisualizeResults (gca, statTable、scenarioParams costMap,路径,路径名,animateResults);
计划在实际数字高程模型数据
现实世界的数据导入<年代pan id="mw_rtc_OffroadPlanningOnDigitalElevationMapsExample_H_1AB306E5" class="anchor_target">
%的地形数据加载
%保存表面数据加载和解析
img = imread (<年代pan style="color:#A020F0">“visitorcenter_satellitesq.jpg”
创建数据层<年代pan id="mw_rtc_OffroadPlanningOnDigitalElevationMapsExample_H_185E500F" class="anchor_target">
图(gxReal gyReal] =梯度(Zreal);轮廓(Xreal Yreal Zreal) formatAxes (visualizationHelper、标题=<年代pan style="color:#A020F0">“地形坡”
%建立数据层
%利用阈值分割图像
数=双(BWroads) +双(BWfields) +双(BWbrush);terrainCost =(双(BWfields) +双(BWbrush) * 3) /计数;
[ii, jj] = meshgrid(1:尺寸(terrainCost, 1), 1:尺寸(terrainCost, 2));一转眼= isnan (terrainCost);terrainCost(女人)= max (griddata (ii(~阿齐兹·伊南),jj(~阿齐兹·伊南),terrainCost(~阿齐兹·伊南),二(女人),jj(女人),<年代pan style="color:#A020F0">“线性”
terrainLayer = mapLayer (terrainCost分辨率= res LayerName =<年代pan style="color:#A020F0">“surfaceCost”
terrainObstaclesReal = binaryOccupancyMap (terrainLayer。getMapData > = maxTerrainCost分辨率= res, LayerName =<年代pan style="color:#A020F0">“terrainObstacles”
充气(terrainObstaclesReal .25);terrainData = getMapData (terrainObstaclesReal);mInvalidSlope = getMapData (xCostReal) > 1 & getMapData (yCostReal) > 1 & getMapData (diagCostReal) > 1;terrainData (mInvalidSlope) = true;setMapData (terrainObstaclesReal terrainData);
图显示(visualizationHelper terrainLayer,次要情节(1、2、1),标题=<年代pan style="color:#A020F0">“地形表面成本”
costMapReal = multiLayerMap ({zLayerReal dzdxReal dzdyReal terrainObstaclesReal xCostReal yCostReal diagCostReal});
gCostType =<年代pan class="live_control_container">“gDefault”
hCostType =<年代pan class="live_control_container">“hDefault”
aStarPlanner = plannerAStarGrid (getLayer (costMapReal<年代pan style="color:#A020F0">“terrainObstacles”
计划路径
起动位置= (290 - 300);goalposition = (95 - 165);开始= world2grid (costMapReal、起动位置);目标= world2grid (costMapReal goalposition);抽搐;pathReal =计划(aStarPlanner,开始,目标);planningTimeReal = toc;
审查结果<年代pan id="mw_rtc_OffroadPlanningOnDigitalElevationMapsExample_H_DA0D639F" class="anchor_target">
statTableReal = exampleHelperCalculateMetrics (costMapReal scenarioParams、pathReal planningTimeReal,<年代pan style="color:#A020F0">“用户配置计划”
statTableReal =<年代pan class="emphasis">表1×8
图exampleHelperVisualizeResults (gca, statTableReal、scenarioParams costMapReal, {pathReal},<年代pan style="color:#A020F0">“用户配置计划”