使用RRT计划移动机器人路径

Open Live Script

此示例显示了如何使用快速探索的随机树（RRT）算法来计划通过已知地图的车辆的路径。特殊的车辆约束也适用于自定义状态空间。您可以为任何导航应用程序使用自定义状态空间和路径验证对象调整自己的计划师。

负载占用图

Load an existing occupancy map of a small office space. Plot the start and goal poses of the vehicle on top of the map.

加载（“ Office_area_gridmap.mat”，，，，“叶酸”）show（occGrid）百分比设定的开始和目标姿势。start = [-1.0,0.0,-pi]; goal = [14,-2.25,0];％显示机器人的开始和目标位置。抓住on绘图（start（1），start（2），'ro'）plot(goal(1),goal(2),'Mo'）％显示开始和目标标题。r = 0.5;绘图（[start（1），start（1） + r*cos（start（3））]，[start（2），start（2） + r*sin（start（start（3））]，'r-'）情节（[目标（1），目标（1） + r*cos（目标（3））]，[目标（2），目标（2） + r*sin（目标（3））]，'M-'）抓住离开

定义状态空间

使用A指定车辆的状态空间状态序列对象并指定状态边界。该对象将采样状态限制为可行的Dubins曲线，以在状态范围内转向车辆。在这个小的环境中，半径为0.4米，可以紧紧转弯。

bounds = [occGrid.XWorldLimits; occGrid.YWorldLimits; [-pi pi]]; ss = stateSpaceDubins(bounds); ss.MinTurningRadius = 0.4;

Plan The Path

为了计划路径，RRT算法样本在状态空间内进行随机状态，并试图连接路径。这些状态和连接需要根据地图约束进行验证或排除。车辆不得与地图中定义的障碍物碰撞。

创建一个验证库cupancymap具有指定状态空间的对象。设置地图已加载的属性占领目的。设置阀门of 0.05 m. This validation distance discretizes the path connections and checks obstacles in the map based on this.

stateValidator = validatoroccupancymap（ss）;stateValidator.map = occGrid;nateValidator.ValidationDistance = 0.05;

Create the path planner and increase the max connection distance to connect more states. Set the maximum number of iterations for sampling states.

planner = plannerRRT(ss,stateValidator); planner.MaxConnectionDistance = 2.0; planner.MaxIterations = 30000;

Customize the进球功能。此示例助手功能检查可行的路径是否达到设定阈值之内的目标。功能返回true当达到目标时，计划者就会停止。

planner.GoalReachedFcn = @exampleHelperCheckIfGoal;

functionISREACHED = exampleHelperCheckifGoal（计划者，Goarsstate，Newstate）Isreached = false;阈值= 0.1;如果planner.StateSpace.distance(newState, goalState) < threshold isReached = true;结尾结尾

计划起始和目标之间的路径。重置随机数生成器以获得可再现的结果。

rng默认[pthobj，solninfo] = plan（计划者，开始，目标）;

绘制路径

显示占用图。从Solninfo。插值和覆盖最终路径。

show(occGrid) holdon％绘制整个搜索树。情节（solninfo.treedata（：，1），solninfo.treedata（：，2），'.-'）；％插值和情节路径。interpolate(pthObj,300) plot(pthObj.States(:,1),pthObj.States(:,2),'r-'，，，，'行宽'，2）百分比在网格图中显示开始和目标。绘图（start（1），start（2），'ro'）plot(goal(1),goal(2),'Mo'）抓住离开

自定义Dubins车辆限制

To specify custom vehicle constraints, customize the state space object. This example usesExampleHelperStateSpaceOneSidedDubins，这是基于状态序列班级。该辅助类别将转弯方向限制为基于布尔属性的右或左侧，向左走。此属性本质上禁用了路径类型dubinsConnection目的。

使用示例助手创建状态空间对象。指定相同的状态界限，并将新的布尔参数作为true（仅左转）。

％仅左转goleft = true;％创建状态空间sscustom = examplehelperstatespaceonessidedubins（边界，goleft）;sscustom.minturningradius = 0.4;

Plan Path

使用自定义Dubins约束创建新的计划对象，并根据这些约束来创建验证器。指定相同进球功能。

stateValidator2 = validatorOccupancyMap(ssCustom); stateValidator2.Map = occGrid; stateValidator2.ValidationDistance = 0.05; planner = plannerRRT(ssCustom,stateValidator2); planner.MaxConnectionDistance = 2.0; planner.MaxIterations = 30000; planner.GoalReachedFcn = @exampleHelperCheckIfGoal;

计划起始和目标之间的路径。再次重置随机数生成器。

rng默认[pthobj2，solninfo] = plan（计划者，开始，目标）;

情节路径

在地图上绘制新路径。路径只能执行左转弯才能达到目标。

figure show(occGrid) holdon％显示搜索树。情节（solninfo.treedata（：，1），solninfo.treedata（：，2），'.-'）；％插值和情节路径。pthObj2.interpolate(300) plot(pthObj2.States(:,1), pthObj2.States(:,2),'r-'，，，，'行宽'，2）百分比在网格图中显示开始和目标。绘图（start（1），start（2），'ro'）情节（目标（1），目标（2），'Mo'）抓住离开