移动机器人使用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函数。这个例子helper函数检查是否可行路径到达目标设置的阈值。函数返回真正的当目标已经达到,计划停止。
计划。GoalReachedFcn = @exampleHelperCheckIfGoal;
函数=,除非这个细胞exampleHelperCheckIfGoal(规划师,goalState newState) = false,除非这个细胞;阈值= 0.1;如果planner.StateSpace。阈值距离(newState goalState) < = true,除非这个细胞;结束结束
计划开始和目标之间的路径。由于随机抽样,本例中设置rng种子一致的结果。
rng (0,“旋风”)[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类。这个助手类限制转动方向右或左一个布尔属性的基础上,GoLeft。这个属性本质上禁用路径类型的dubinsConnection对象使用(见dubinsConnection.DisabledPathTypes)。
使用辅助的例子创建状态空间对象。指定相同的国家界限,给新布尔参数真正的(左)。
%只有左转弯goLeft = true;%建立状态空间goLeft ssCustom = ExampleHelperStateSpaceOneSidedDubins(范围);ssCustom。MinTurningRadius = 0.4;
计划的路径
创建一个新的规划对象的自定义杜宾约束和基于这些约束的验证器。指定相同的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] =计划(计划,开始,目标);
图的路径
画地图上的新路径。应该只执行左转弯路径到达目标。这个例子展示了如何定制你的约束和仍然计划使用通用RRT路径算法。
图显示(occGrid)在%显示搜索树情节(solnInfo.TreeData (: 1) solnInfo.TreeData (:, 2),“。”);%树扩张%画出路径(路径插值后)pthObj2.interpolate(300)情节(pthObj2.States (: 1), pthObj2.States (:, 2),的r -,“线宽”,2)%显示网格地图的启动和目标情节(开始(1),(2)开始,“罗”)情节(目标(1)、目标(2),“莫”)举行从
