使用RRT规划移动机器人路径
这个例子展示了如何使用快速探索随机树(RRT)算法来规划通过已知地图的车辆路径。特殊的车辆约束也应用于自定义状态空间。您可以为任何导航应用程序使用自定义状态空间和路径验证对象来优化您自己的规划器。
负载入住率地图
加载一个小办公空间的现有占用地图。在地图顶部绘制车辆的开始和目标姿势。
负载(“office_area_gridmap.mat”，“occGrid”)显示(occGrid)设定开始和目标姿势。开始=[-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米的转弯半径允许在这个小环境中进行急转弯。
边界=[occGrid.XWorldLimits；occGrid.YWorldLimits；[-pi]]；ss=stateSpaceDubins（边界）；ss.MinTurningRadius=0.4；
规划道路
为了规划一条路径，RRT算法在状态空间中抽样随机状态，并尝试连接一条路径。这些状态和连接需要根据映射约束进行验证或排除。车辆不能与地图上规定的障碍物相撞。
创建一个职业地图具有指定状态空间的对象。设定地图属性设置为occupancyMap对象。设置一个ValdiationDistance0.05 m。此验证距离离散路径连接，并基于此检查地图中的障碍物。
stateValidator = validatorOccupancyMap (ss);stateValidator。地图= occGrid;stateValidator。ValidationDistance = 0.05;
创建路径规划器并增加最大连接距离以连接更多状态。设置采样状态的最大迭代次数。
planner=PlannerRT（ss，stateValidator）；planner.MaxConnectionDistance=2.0；planner.MaxIterations=30000；
定制GoalReached函数。这个示例助手函数检查一个可行路径是否在设定的阈值内达到目标。函数返回符合事实的当目标已经达到，计划者就停止了。
planner.GoalReachedFcn=@examplehelopercheckifgoal；
作用isReached = exampleHelperCheckIfGoal(planner, goalState, newState) isReached = false;阈值= 0.1;如果planner.StateSpace.distance（newState，goalState）结束结束
在开始和目标之间规划路径。重置随机数生成器以获得可重复的结果。
rng默认的[pthObj，solnInfo]=计划（计划者，开始，目标）；
图的路径
出示入住地图。绘制搜索树solnInfo．插入并覆盖最终路径。
显示（occGrid）保持在…上%绘制整个搜索树。情节(solnInfo.TreeData (: 1) solnInfo.TreeData (:, 2),“。”)；%插入和绘制路径。插值（pthObj，300）绘图（pthObj.状态（：，1），pthObj.状态（：，2），的r -，“线宽”,2)%在栅格地图中显示开始和目标。情节(开始(1),(2)开始,“罗”)绘图（目标（1）、目标（2），“莫”)持有关
自定义杜宾车辆约束
要指定自定义车辆约束，请自定义状态空间对象示例HelperStateSpaceOneSidedDubins，这是基于stateSpaceDubins类。这个helper类基于布尔属性将转向方向限制为右或左，GoLeft．属性的路径类型杜宾斯连接对象。
使用示例帮助器创建状态空间对象。指定相同的状态边界，并将新的布尔参数指定为符合事实的(左)。
只向左转弯goLeft = true;%创建状态空间goLeft ssCustom = ExampleHelperStateSpaceOneSidedDubins(范围);ssCustom。MinTurningRadius = 0.4;
规划路径
使用自定义Dubins约束和基于这些约束的验证器创建一个新的planner对象。指定相同的GoalReached函数。
stateValidator2=ValidatorOccinecyMap（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] =计划(计划,开始,目标);
图的路径
在地图上画出新的路径。路径应该只执行左转以达到目标。
图2显示（occGrid）保持在…上%显示搜索树。情节(solnInfo.TreeData (: 1) solnInfo.TreeData (:, 2),“。”)；%插入和绘制路径。pthObj2.插值（300）绘图（pthObj2.状态（：，1），pthObj2.状态（：，2），的r -，“线宽”,2)%在栅格地图中显示开始和目标。情节(开始(1),(2)开始,“罗”)情节(目标(1)、目标(2),“莫”)持有关
您还可以从以下列表中选择网站：