使用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 -”)举行从

定义状态空间

属性指定车辆的状态空间stateSpaceDubins对象，并指定状态边界。该对象将采样状态限制为可行的杜宾斯曲线，以便在状态范围内转向车辆。0.4米的转弯半径允许在这种小环境中进行急转弯。

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

规划好路径

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

创建一个validatorOccupancyMap对象使用指定的状态空间。设置地图属性设置为occupancyMap对象。设置一个ValdiationDistance0.05米。这个验证距离离散路径连接，并在此基础上检查地图中的障碍物。

stateValidator = validatorOccupancyMap(ss);stateValidator。Map = occGrid;stateValidator。ValidationDistance = 0.05;

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

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

自定义GoalReached函数。这个示例助手函数检查可行路径是否在设置的阈值内达到目标。函数返回真正的当目标达到时，计划者就会停止。

计划。GoalReachedFcn = @exampleHelperCheckIfGoal;

函数isReached = exampleHelperCheckIfGoal(planner, goalState, newState) isReached = false;阈值= 0.1;如果planner.StateSpace。距离(newState, goalState) < threshold isReached = true;结束结束

计划好从开始到目标的路径。重置随机数发生器可重现的结果。

rng默认的[pthObj, solnInfo] = plan(计划，开始，目标);

规划路径

显示占用地图。对象绘制搜索树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类。这个helper类根据布尔属性将转向方向限制为右或左，GoLeft．属性的路径类型dubinsConnection对象。

使用示例帮助器创建状态空间对象。指定相同的状态边界，并给出新的布尔参数为真正的(仅限左转)。

只做左转弯goLeft = true;创建状态空间ssCustom = ExampleHelperStateSpaceOneSidedDubins(bounds,goLeft);ssCustom。MinTurningRadius = 0.4;

计划路径

使用自定义Dubins约束和基于这些约束的验证器创建一个新的计划器对象。指定相同的GoalReached函数。

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

计划好从开始到目标的路径。重新设置随机数发生器。

rng默认的[pthObj2,solnInfo] = plan(计划，开始，目标);

图的路径

在地图上画出新的路径。路径应该只执行左转弯到达目标。

figure show(occGrid) hold住在显示搜索树。情节(solnInfo.TreeData (: 1) solnInfo.TreeData (:, 2),“。”）;插值并绘制路径。pthObj2.interpolate(300) plot(pthObj2.States(:，1)， pthObj2.States(:，2)，的r -，“线宽”, 2)在网格地图中显示开始和目标。情节(开始(1),(2)开始,“罗”图(目标(1)，目标(2)，“莫”)举行从