使用RRT规划移动机器人路径
这个例子展示了如何使用快速探索随机树(RRT)算法来规划通过已知地图的车辆路径。特殊的车辆约束也应用于自定义状态空间。您可以为任何导航应用程序使用自定义状态空间和路径验证对象来优化您自己的规划器。
负载入住率地图
载入一个小型办公空间的现有占用地图。在地图顶部绘制车辆的起始和目标姿态。
负载(“office_area_gridmap.mat”，“occGrid”)显示(occGrid)设定开始和目标姿势。开始=(-1.0,0.0,-π);目标= (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米的转弯半径允许在这个小环境中进行急转弯。
边界= [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;
函数isReached = exampleHelperCheckIfGoal(planner, goalState, newState) isReached = false;阈值= 0.1;如果planner.StateSpace。distance(newState, goalState) < threshold isReached = true;结束结束
在开始和目标之间规划路径。重置随机数生成器以获得可重复的结果。
rng默认的[pthObj, solnInfo] =计划(计划，开始，目标);
图的路径
出示入住地图。绘制搜索树solnInfo．插入并覆盖最终路径。
显示(occGrid)在%绘制整个搜索树。情节(solnInfo.TreeData (: 1) solnInfo.TreeData (:, 2),“。”）;%插入和绘制路径。插入(pthObj 300)情节(pthObj.States (: 1), pthObj.States (:, 2),的r -，“线宽”, 2)在网格地图中显示开始和目标。情节(开始(1),(2)开始,“罗”)情节(目标(1)、目标(2),“莫”)举行从
自定义杜宾车辆约束
要指定自定义车辆约束，请自定义状态空间对象。这个示例使用ExampleHelperStateSpaceOneSidedDubins，这是基于stateSpaceDubins类。这个helper类基于布尔属性将转向方向限制为右或左，GoLeft．属性的路径类型dubinsConnection对象。
使用示例助手创建状态空间对象。指定相同的状态边界并给出新的布尔参数真正的(左)。
只向左转弯goLeft = true;%创建状态空间goLeft ssCustom = ExampleHelperStateSpaceOneSidedDubins(范围);ssCustom。MinTurningRadius = 0.4;
计划路径
使用自定义Dubins约束和基于这些约束的验证器创建一个新的planner对象。指定相同的GoalReached函数。
stateValidator2 = validatorOccupancyMap (ssCustom);stateValidator2。地图= occGrid;stateValidator2。ValidationDistance = 0.05;规划师= plannerRRT (ssCustom stateValidator2);计划。MaxConnectionDistance = 2.0;计划。MaxIterations = 30000;计划。GoalReachedFcn = @exampleHelperCheckIfGoal;
在开始和目标之间规划路径。再次重置随机数生成器。
rng默认的[pthObj2, solnInfo] =计划(计划,开始,目标);
图的路径
在地图上画出新的路径。路径应该只执行左转以达到目标。
图显示(occGrid)在%显示搜索树。情节(solnInfo.TreeData (: 1) solnInfo.TreeData (:, 2),“。”）;%插入和绘制路径。pthObj2.interpolate(300)情节(pthObj2.States (: 1), pthObj2.States (:, 2),的r -，“线宽”, 2)在网格地图中显示开始和目标。情节(开始(1),(2)开始,“罗”)情节(目标(1)、目标(2),“莫”)举行从
你也可以从以下列表中选择一个网站: