使用RRT计划移动机器人路径
这个例子展示了如何使用快速探索随机树(RRT)算法来规划车辆通过已知地图的路径。特殊的车辆约束也应用于自定义状态空间。您可以使用任何导航应用程序的自定义状态空间和路径验证对象来优化自己的规划器。
负载入住率地图
加载一个小型办公空间的现有占用图。在地图上标出车辆的起始姿势和目标姿势。
负载(“office_area_gridmap.mat”,“occGrid”)显示(occGrid)设置开始和目标姿势start = [-1.0, 0.0， -pi];目标= [14，-2.25,0];显示机器人的起始位置和目标位置持有在情节(开始(1),(2)开始,“罗”)情节(目标(1)、目标(2),“莫”)显示开始和目标标题r = 0.5;情节([开始(1)开始(1)+ r * cos(开始(3))],[开始(2),(2)+ r *罪(开始(3)),的r -)情节([目标(1),目标(1)+ r * cos(目标(3))],[目标(2)、目标(2)+ r *罪(目标(3))],“m -”)举行从
定义状态空间
使用a指定车辆的状态空间stateSpaceDubins对象并指定状态边界。该对象将采样状态限制为在状态范围内引导车辆的可行dubin曲线。0.4米的转弯半径允许在这个小环境中进行急转弯。
边界= [occGrid.XWorldLimits;occGrid.YWorldLimits;[-ππ]];党卫军= stateSpaceDubins(范围);ss.MinTurningRadius = 0.4;
计划的路径
为了规划路径，RRT算法对状态空间中的随机状态进行采样，并尝试连接路径。需要根据映射约束验证或排除这些状态和连接。车辆不得与地图上定义的障碍物碰撞。
创建一个validatorOccupancyMap对象，具有指定的状态空间。设置地图已装载货物的属性occupancyMap对象。设置一个ValdiationDistance0.05 m。这个距离离散路径连接，并在此基础上检查地图中的障碍。
stateValidator = validatorOccupancyMap (ss);stateValidator。地图= occGrid;stateValidator。ValidationDistance = 0.05;
创建路径规划器并增加最大连接距离以连接更多状态。设置采样状态的最大迭代次数。
规划师= plannerRRT(ss, stateValidator);计划。MaxConnectionDistance = 2.0;计划。MaxIterations = 30000;
自定义GoalReached函数。这个示例帮助器函数检查是否有可行路径在设置的阈值内达到目标。函数返回真正的当目标达到，计划者停止的时候。
计划。GoalReachedFcn = @exampleHelperCheckIfGoal;
函数isreach = exampleHelperCheckIfGoal(planner, goalState, newState) = false;阈值= 0.1;如果planner.StateSpace。距离(newState, goalState) <阈值isreach = true;结束结束
计划开始和目标之间的路径。由于是随机抽样，本例设置了rng为一致的结果播种。
rng (0,“旋风”) [pthObj, solnInfo] =计划(planner, start, goal);
图的路径
显示占用图。绘制搜索树solnInfo。插值和覆盖最后的路径。
显示(occGrid)在%搜索树情节(solnInfo.TreeData (: 1) solnInfo.TreeData (:, 2),“。”);插值和绘制路径插入(pthObj 300)情节(pthObj.States (: 1), pthObj.States (:, 2),的r -,“线宽”,2)在网格映射中显示开始和目标情节(开始(1),(2)开始,“罗”)情节(目标(1)、目标(2),“莫”)举行从
自定义Dubins车辆约束
要指定自定义车辆约束，请自定义状态空间对象。这个示例使用ExampleHelperStateSpaceOneSidedDubins，这是基于stateSpaceDubins类。这个助手类根据布尔属性将转弯方向限制在左或右，GoLeft。属性的路径类型dubinsConnection对象使用(见dubinsConnection.DisabledPathTypes)。
使用示例助手创建状态空间对象。指定相同的状态界限，并将新的布尔参数指定为真正的(左)。
只向左转goLeft = true;创建状态空间ssCustom = ExampleHelperStateSpaceOneSidedDubins(bounds, goLeft);ssCustom。MinTurningRadius = 0.4;
计划的路径
使用自定义dubin约束和基于这些约束的验证器创建一个新的planner对象。指定相同的GoalReached函数。
stateValidator2 = validatorOccupancyMap (ssCustom);stateValidator2。地图= occGrid;stateValidator2。ValidationDistance = 0.05;计划表= plannerRRT(ssCustom, stateValidator2);计划。MaxConnectionDistance = 2.0;计划。MaxIterations = 30000;计划。GoalReachedFcn = @exampleHelperCheckIfGoal;
计划开始和目标之间的路径。重置rng种子了。
rng (0,“旋风”) [pthObj2, solnInfo] =计划(planner, start, goal);
图的路径
在地图上画出新的路径。该路径应只执行左转弯，以达到目标。这个示例展示了如何使用通用RRT算法自定义约束和规划路径。
图显示(occGrid)在显示搜索树情节(solnInfo.TreeData (: 1) solnInfo.TreeData (:, 2),“。”);%树扩张%绘制路径(路径内插后)pthObj2.interpolate(300)情节(pthObj2.States (: 1), pthObj2.States (:, 2),的r -,“线宽”,2)在网格映射中显示开始和目标情节(开始(1),(2)开始,“罗”)情节(目标(1)、目标(2),“莫”)举行从
