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

打开生活的脚本

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

负载占用地图

载入一个小型办公空间的现有占用地图。在地图顶部绘制车辆的起始和目标姿态。

加载（“Office_area_gridmap.mat”那“converdiv”）展示（Conngrid）％设置开始和目标姿势。开始=(-1.0,0.0,-π);目标= (14,-2.25,0);％显示机器人的开始和目标位置。抓住在情节（开始（1），开始（2），'ro')情节(目标(1)、目标(2),'莫'的）％显示开始和目标标题。r = 0.5;绘图（[开始（1），开始（1）+ R * cos（开始（3））]，[开始（2），开始（2）+ R * SIN（开始（3））]，'r-'）情节（[目标（1），目标（1）+ R * cos（目标（3））]，[目标（2），目标（2）+ R * SIN（目标（3））]，'m-')举行离开

定义状态空间

使用a指定车辆的状态空间标准尼封储对象并指定状态界限。该物体将采样状态限制为可行的Dubins曲线，用于在状态范围内转向车辆。0.4米的转弯半径允许在这个小环境中紧张。

边界= [occGrid.XWorldLimits;occGrid.YWorldLimits;[-ππ]];党卫军= stateSpaceDubins(范围);ss.MinTurningRadius = 0.4;

计划的路径

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

创建一个Validatoroccupancimap.具有指定状态空间的对象。设定地图属性到加载占领子目的。设置A.瓦尔迪瓦斯坦0.05 m。这个验证距离将离散路径连接并基于此检查地图中的障碍。

StateValidator = ValidatoroccupAccyMap（SS）;statevalidator.map = convergrid;StateValidator.ValidationDistance = 0.05;

创建路径规划器，增加最大连接距离，以连接更多的状态。设置采样状态的最大迭代次数。

规划师= plannerRRT (ss, stateValidator);计划。MaxConnectionDistance = 2.0;计划。MaxIterations = 30000;

自定义守门员功能。此示例辅助功能检查可行的路径是否达到设置阈值内的目标。函数返回真正的当达到目标时，策划者停止。

计划。GoalReachedFcn = @exampleHelperCheckIfGoal;

函数ISREACHED = examplehelpercheckifgoal（Planner，valegstate，Newstate）isReached = false;阈值= 0.1;如果planner.StateSpace。distance(newState, goalState) < threshold isReached = true;结尾结尾

计划开始和目标之间的路径。重置随机数发生器以进行可重复的结果。

rng默认[Pthobj，Solninfo] =计划（计划者，开始，目标）;

绘制路径

显示占用地图。绘制来自的搜索树solninfo.。插值并覆盖最终路径。

显示(occGrid)在％绘制整个搜索树。plot（solninfo.tredata（：，1），solninfo.tredata（:,2），'.-'）;％插值和绘图路径。插入(pthObj 300)情节(pthObj.States (: 1), pthObj.States (:, 2),'r-'那'行宽'2）在网格地图中显示开始和目标。情节（开始（1），开始（2），'ro')情节(目标(1)、目标(2),'莫')举行离开

自定义Dubins车辆限制

要指定自定义车辆约束，请自定义状态空间对象。这个示例使用ExampleHelperStateSpaceOneSidedDubins，这是基于标准尼封储班级。此辅助类基于布尔属性将转向或左侧限制为右侧或左侧，向左走。此属性基本上禁用路径类型dubinsConnection目的。

使用示例助手创建状态空间对象。指定相同的状态边界并给出新的布尔参数真正的（仅左转）。

％只做左转goleft = true;％创建状态空间scustom = examplehelperstatespaceonesidedubins（界限，goleft）;sscustom.minturningradius = 0.4;

计划路径

使用自定义Dubins约束和基于这些约束创建一个新的Planner对象。指定相同守门员功能。

stateValidator2 = validatorOccupancyMap (ssCustom);stateValidator2。地图= occGrid;stateValidator2。ValidationDistance = 0.05;规划师= plannerRRT (ssCustom stateValidator2);计划。MaxConnectionDistance = 2.0;计划。MaxIterations = 30000;计划。GoalReachedFcn = @exampleHelperCheckIfGoal;

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

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

绘图路径

在地图上绘制新路径。路径仅执行左转以达到目标。

图显示(occGrid)在％显示搜索树。plot（solninfo.tredata（：，1），solninfo.tredata（:,2），'.-'）;％插值和绘图路径。pthObj2.interpolate(300)情节(pthObj2.States (: 1), pthObj2.States (:, 2),'r-'那'行宽'2）在网格地图中显示开始和目标。情节（开始（1），开始（2），'ro'）情节（目标（1），目标（2），'莫')举行离开