间隙

路径最小净空

在页面中全部折叠

语法

间隙(pathMetricsObj)

间隙(pathMetricsObj、“类型”、“州”)

描述

例子

间隙(pathMetricsObj）返回路径的最小间隙。间隙是指指定地图环境中路径上状态的网格单元中心与障碍物之间的最小距离。

请注意

计算出的间隙精确到√6 (2)乘以网格地图单元格大小。

间隙(pathMetricsObj，“类型”，“州”）返回路径的每个状态的最小距离集，形式为n1的向量,n是状态数。

例子

全部折叠

计算路径度量

打开生活的脚本

基于一组姿态和相关的映射环境，计算规划路径的平滑度、间隙和有效性。

加载和分配映射到状态验证器

从示例地图创建占用地图并设置地图分辨率。

负载exampleMaps.mat；% simpleMapmapResolution = 1;%细胞/计地图= occupancyMap (simpleMap mapResolution);

创建Dubins状态空间。

statespace = stateSpaceDubins;

创建一个基于占用映射的状态验证器，将参数和状态存储在Dubins状态空间中。

statevalidator = validatorOccupancyMap (statespace);

将映射分配给验证器。

statevalidator。地图=地图;

为验证器设置验证距离。

statevalidator。ValidationDistance = 0.01;

更新状态空间边界，使其与映射限制相同。

statespace。StateBounds = [map.XWorldLimits;map.YWorldLimits;[-pi pi]];

计划路径

创建一个RRT*路径规划器，并允许进一步优化。

规划师= plannerRRTStar (statespace statevalidator);计划。ContinueAfterGoalReached = true;

减少最大迭代次数并增加最大连接距离。

计划。MaxIterations = 2500;计划。MaxConnectionDistance = 0.3;

将路径规划器的开始和目标状态定义为［x, y,θ］向量。x而且y是笛卡尔坐标，和θ是方向角。

Start = [2.5, 2.5, 0];%[米，米，弧度]目标= [22.5,8.75,0];

计划从开始状态到目标状态的路径。plan函数返回anavPath对象。

rng (100“旋风”）%可重复的结果[路径,solutionInfo] =计划(计划,开始,目标);

计算和可视化路径度量

创建一个路径度量对象。

pathMetricsObj = pathmetrics(路径,statevalidator);

检查路径的有效性。结果是1(真正的)如果规划的路径没有障碍。0(假)无效的路径。

isPathValid (pathMetricsObj)

ans =逻辑1

计算路径的最小间隙。

间隙(pathMetricsObj)

ans = 1.4142

评估路径的平滑度。值接近0指示一条更平坦的路径。直线路径返回值为0．

平滑(pathMetricsObj)

ans = 1.7318

想象路径的最小净空。

显示(pathMetricsObj)传说(“计划路径”，的最小间隙）

二维成本图环境下的车辆路径规划与度量计算

这个示例使用:

打开生活的脚本

使用RRT*算法规划通过停车场的车辆路径。计算并可视化规划路径的平滑度、间隙和有效性。

加载和分配映射到状态验证器

加载一个停车场的成本图。绘制成本图，看到停车场和充气区域，车辆应该避免。

负载parkingLotCostmap.mat；costmap = parkingLotCostmap;情节(costmap)包含(“X(米)) ylabel (“Y”(米)）

创建一个stateSpaceDubins对象，并将最小图灵图半径增加到4米。

statespace = stateSpaceDubins;statespace。MinTurningRadius = 4;%米

创建一个validatorVehicleCostmap对象使用已创建的状态空间创建。

statevalidator = validatorVehicleCostmap (statespace);

将停车场成本图分配给状态验证器对象。

statevalidator。地图= costmap;

计划路径

定义车辆的起始和目标姿态为[x，y，Θ)向量。的世界单位(x，y)的位置以米为单位。世界单位为方向角Θ在度。

startPose = [5,5,90];%[米，米，度]goalPose = [40,38,180];%[米，米，度]

使用一个pathPlannerRRT(自动驾驶工具箱)对象和计划(自动驾驶工具箱)函数用于规划车辆从起始姿势到目标姿势的路径。

规划师= pathPlannerRRT (costmap);refPath =计划(计划、startPose goalPose);

沿着路径每隔一米插值一次。将方向角从角度转换为弧度。

提出了= 0(大小(refPath.PathSegments, 2) + 1, 3);:提出了(1)= refPath.StartPose;为i = 1:size(refPath.PathSegments,2) pose (i+1，:) = refPath.PathSegments(i).GoalPose;结束提出了函数(:,3)=(姿势(:,3));

创建一个navPath对象使用Dubins状态空间对象和指定的状态提出了．

路径= navPath (statespace,姿势);

计算和可视化路径度量

创建一个pathmetrics对象。

pathMetricsObj = pathmetrics(路径,statevalidator);

检查路径的有效性。结果是1（真正的)，如果所规划的道路没有障碍。0（假)表示无效路径。

isPathValid (pathMetricsObj)

ans =逻辑1

计算并可视化最小值间隙的路径。

间隙(pathMetricsObj)

ans = 0.5000

显示(pathMetricsObj)传说(“膨胀的领域”，“计划路径”，的最小间隙)包含(“X(米)) ylabel (“Y”(米)）

计算和可视化平滑度的路径。值接近0指示一条更平坦的路径。直线路径返回值为0．

平滑(pathMetricsObj)

ans = 0.0842

显示(pathMetricsObj“指标”，{“平滑”})(传说“膨胀的领域”，路径平滑的)包含(“X(米)) ylabel (“Y”(米)）

可视化路径的每个状态的间隙。

显示(pathMetricsObj“指标”，{“StatesClearance”})(传说“膨胀的领域”，“计划路径”，“清除路径状态”)包含(“X(米)) ylabel (“Y”(米)）

三维环境下的路径规划与度量计算

打开生活的脚本

使用RRT算法规划一条穿过城市街区的路径。计算并可视化规划路径的平滑度、间隙和有效性。

加载和分配映射到状态验证器

在工作区中加载一个城市街区的3-D占用地图。指定将单元格视为无障碍的阈值。

mapData =负载(“dMapCityBlock.mat”）;omap = mapData.omap;的核心。FreeThreshold = 0.5;

膨胀占用地图，以增加一个缓冲区的安全操作周围的障碍。

充气(omap, 1)

创建具有状态变量边界的SE(3)状态空间对象。

ss = stateSpaceSE3([0 220;0 220;0 100;inf inf;inf inf;

使用创建的状态空间创建一个3d占用地图状态验证器。将占用映射分配给状态验证器对象。指定采样距离间隔。

sv = validatorOccupancyMap3D(党卫军,.．.地图=的核心,.．.ValidationDistance = 0.1);

计划路径

创建RRT路径规划器，增加最大连接距离，减少最大迭代次数。指定一个自定义目标函数，该函数确定如果到目标的欧氏距离低于1米的阈值，则路径达到目标。

规划师= plannerRRT (ss、sv、.．.MaxConnectionDistance = 50,.．.MaxIterations = 1000,.．.GoalReachedFcn = @ (~ s, g)(规范(s - g (1:3)) (1:3) < 1),.．.GoalBias = 0.1);

指定起始姿势和目标姿势。

Start = [50 180 25 0.7 0.2 0 0.1];目标= [150 33 35 0.3 0 0.1 0.6];

配置随机数生成器以获得可重复的结果。

rng (1,“旋风”）

计划的路径。

[pthObj, solnInfo] =计划(计划,开始,目标);

计算和可视化路径度量

创建一个路径度量对象。

pathMetricsObj = pathmetrics (pthObj, sv);

检查路径的有效性。结果是1(真正的)如果规划的路径没有障碍。0(假)无效的路径。

isPathValid (pathMetricsObj)

ans =逻辑1

计算路径的最小间隙。

间隙(pathMetricsObj)

ans = 10

评估路径的平滑度。值接近0指示一条更平坦的路径。直线路径返回值为0．

平滑(pathMetricsObj)

ans = 0.0011

想象路径的最小净空。

显示(pathMetricsObj)轴平等的视图(75 [100])在%开始状态scatter3(开始(1,1),开始(1、2),开始(1、3)“g”，“填充”）%的目标状态scatter3(目标(1,1)、目标(1、2)、目标(1、3)“r”，“填充”）%的路径plot3 (pthObj.States (: 1) pthObj.States (:, 2), pthObj.States (:, 3),.．.“r -”线宽= 2)