用RRT在杂乱的房间里移动家具

打开生活的脚本

这个例子展示了如何规划一个路径来移动笨重的家具在一个狭窄的空间避免杆。这个例子展示了一个“Piano Mover’s Problem”的工作流程，它用于测试带有约束状态空间的路径规划算法。本示例使用plannerRRTStar对象实现自定义优化快速探索树(RRT*)算法。提供的示例助手演示了如何为任何运动规划应用程序定义自定义状态空间和状态验证。

模型的场景

为了帮助可视化和解决这个路径规划问题，提供了两个助手类，ExampleHelperFurniture和ExampleHelperRoom．的ExampleHelperFurniture类通过将三个矩形碰撞框组合在一起来组装家具。家具设置如下:

的ExampleHelperRoom定义了房间的尺寸，并提供了将家具插入房间的功能，以及检查家具是否与墙壁或柱子发生碰撞。这两个类用于规划器的状态验证器。

用给定的长度和宽度展示房间。以给定的姿势添加一件家具。

len = 6;wid = 2;房间= ExampleHelperRoom(len, wid);椅子= ExampleHelperFurniture;addFurniture(房间、椅子、trvec2tform ([0 0 3.5]));表演(房间,gca)轴平等的

配置状态空间

创建一个stateSpaceSE2家具的对象。根据房间尺寸设置边界。状态空间对状态空间中的随机状态进行抽样。在这个例子中，家具状态是一个3元素向量，(x yθ),xy-坐标和旋转角度，以弧度为单位。

Bounds = [-0.8 0.8;[1 - 5];[-ππ]];党卫军= stateSpaceSE2(范围);ss.WeightTheta = 2;

创建一个自定义状态验证器

规划器需要一个定制的状态验证器来启用家具和房间内固定装置之间的冲突检查。提供的类,ExampleHelperFurnitureInRoomValidator，基于成对凸多边形碰撞检测函数检测家具状态的有效性。这个类会自动创建一个房间，并在建造时将形状怪异的家具放入其中。

设定家具的初始姿势initPose = trvec2tform([0 3.5 0]);创建自定义状态验证器sv = examplehelperfurnureinroomvalidator (ss, initPose);减少验证距离验证距离决定插补粒度%检查连接两种状态的运动。sv。ValidationDistance = 0.1;

配置路径规划器

使用plannerRRTStar作为规划器，并指定自定义状态空间和状态验证器。为规划器指定其他参数。的GoalReached示例助手函数返回真正的当一条可行路径在一个阈值内足够接近目标时。这就退出了计划者。

%制定计划rrt = plannerRRTStar(ss, sv); / /%设置搜索附近邻居的球半径rrt。BallRadiusConstant = 1000;找到路径后立即退出rrt。ContinueAfterGoalReached = false;两个家具姿势之间的运动长度应小于0.4米rrt。MaxConnectionDistance = 0.4;增加最大迭代次数rrt。MaxIterations = 20000;%使用定制的目标函数rrt。GoalReachedFcn = @exampleHelperGoalFunc;

计划,此举

设定家具的起始和结束姿势。这个例子从一个杆子移动到另一个杆子，并旋转椅子π弧度。规划姿势之间的路径。可视化搜索树和最终路径。

%设置初始和目标姿势Start = [0 3.5 0];目标= [0 -0.2 pi];%为重复性设置随机数种子rng (0,“旋风”）;[path, solnInfo] = plan(rrt,start,goal);持有在%搜索树情节(solnInfo.TreeData (: 1) solnInfo.TreeData (:, 2),“。”）;%插入路径和绘图点插入(路径,300)情节(path.States (: 1), path.States (:, 2),的r -，“线宽”, 2)从

可视化运动

提供了一个示例助手，用于在可能的情况下通过切割路径的拐角来平滑路径。使家具从开始到目标的运动变成动画。当家具导航到目标位置时，动画情节显示中间状态。

f =图;铺平道路，尽可能抄近道。路径= exampleHelperSmoothPath(path, sv);插入(pathSm, 100);animateFurnitureMotion (sv.Room 1 pathSm。轴(f))显示家具的痕迹跳过= 6;州= pathSm。州([1:跳过:结束,pathSm。NumStates):);exampleHelperShowFurnitureTrace (sv.Room。FurnituresInRoom{1}、州);