该示例示出了如何使用快速-随机树(RRT)算法通过已知的地图来规划车辆的路径。特种车辆的限制也适用与自定义状态空间。你可以调整自己的规划师与任何导航应用自定义状态空间和路径验证的对象。
加载小型办公空间的占用现有的地图。绘制在地图上的顶级车辆的起点和目标姿势。
加载(“office_area_gridmap.mat”,“occGrid”)显示(occGrid)%设置开始和目标姿势开始= [-1.0,0.0,-pi];目标= [14,-2.25,0];%显示机器人的起点和终点位置保持上图(开始(1),启动(2),'RO')情节(目标(1),目标(2),“莫”)%显示起点和目标标题R = 0.5;图([开始(1),启动(1)+ R * COS(START(3))],[启动(2),启动(2)+ R * SIN(启动(3))],'R-')情节([目标(1),目标(1)+ R * COS(目标(3))],[目标(2),目标(2)+ R * SIN(目标(3))],“间”)保持离
指定车辆使用的状态空间stateSpaceDubins
对象,并指定状态边界。该目的限制了取样状态,以可行的Dubins曲线的状态范围内转向的车辆。为0.4m转弯半径允许在这个小环境急转弯。
边界= [occGrid.XWorldLimits;occGrid.YWorldLimits;[-pi PI]];SS = stateSpaceDubins(边界);ss.MinTurningRadius = 0.4;
要规划路径,状态空间和尝试内RRT运算法则样本随机状态连接的路径。这些状态和连接需要进行验证或基于所述映射约束排除。车辆不能在地图上确定障碍物碰撞。
创建一个validatorOccupancyMap
与指定的状态空间对象。设置地图
属性所加载的occupancyMap
宾语。集ValdiationDistance
0.05微米。这个距离在离散化基于此地图上的道路连接,并检查障碍。
stateValidator = validatorOccupancyMap(SS);stateValidator.Map = occGrid;stateValidator.ValidationDistance = 0.05;
创建路径规划,并增加最大连接距离连接更多的状态。设置迭代的最大数量为采样状态。
规划人员= plannerRRT(SS,stateValidator);planner.MaxConnectionDistance = 2.0;planner.MaxIterations = 30000;
自定义GoalReached
功能。本实施例中的辅助功能检查的可行路径达到设定的阈值内的目标。该函数返回真正
当目标已经达成,并且计划停止。
planner.GoalReachedFcn = @exampleHelperCheckIfGoal;
功能isReached = exampleHelperCheckIfGoal(规划师,goalState,newState)isReached = FALSE;阈值= 0.1;如果planner.StateSpace.distance(newState,goalState)<阈isReached = 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),'RO')情节(目标(1),目标(2),“莫”)保持离
要指定自定义车辆的限制,自定义状态空间对象。本例使用ExampleHelperStateSpaceOneSidedDubins
,这是基于stateSpaceDubins
类。这个辅助类限制了转动方向到右边或左边基于布尔属性,往左走
。此属性实质上是禁用了路径类型dubinsConnection
对象使用(见dubinsConnection.DisabledPathTypes
)。
创建使用例如辅助状态空间物体。指定相同的状态边界和给予新的布尔参数作为真正
(仅左转弯)。
%只让左转弯goLeft = TRUE;%创建的状态空间ssCustom = ExampleHelperStateSpaceOneSidedDubins(边界,goLeft);ssCustom.MinTurningRadius = 0.4;
创建自定义的Dubins约束的新规划师的对象,并根据这些约束的验证。指定相同GoalReached
功能。
stateValidator2 = validatorOccupancyMap(ssCustom);stateValidator2.Map = occGrid;stateValidator2.ValidationDistance = 0.05;规划人员= plannerRRT(ssCustom,stateValidator2);planner.MaxConnectionDistance = 2.0;planner.MaxIterations = 30000;planner.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),'RO')情节(目标(1),目标(2),“莫”)保持离