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

打开实时脚本

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

建模场景

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

的ExampleHelperRoom定义房间的尺寸，并提供将家具插入房间，检查家具是否与墙壁或电线杆碰撞的功能。这两个类用于计划器的状态验证器。

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

Len = 6;Wid = 2;room = examplehelproom (len, wid);椅子= examplehelper家具;addFurniture(房间，椅子，trvec2tform([0 3.5 0]));表演(房间,gca)轴平等的

配置状态空间

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

Bounds = [-0.8 0.8;[1 - 5];[-ππ]];ss = stateSpaceSE2(bounds);ss.WeightTheta = 2;

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

该计划需要一个定制的状态验证器，以支持家具和房间中的固定装置之间的碰撞检查。提供的类，ExampleHelperFurnitureInRoomValidator，基于成对凸多边形碰撞检验函数对家具状态进行有效性检验。课程会自动创建一个房间，并在建造时把形状怪异的家具放进去。

设置家具的初始姿势initPose = trvec2tform([0 3.5 0]);创建一个自定义状态验证器sv = examplehelperfurniture ureinroomvalidator (ss, initPose);缩短验证距离时，%验证距离决定插值的粒度检查连接两个状态的运动。sv。ValidationDistance = 0.1;

配置路径规划器

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

%创建计划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),“。”）;插值路径和绘图点插值(path,300) plot(path. states (:，1)， path. states (:，2)，的r -，“线宽”2) hold从

想象运动

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

F =数字;把这条路弄平，尽可能地抄近路。pathSm = exampleHelperSmoothPath(path, sv);插入(pathSm, 100);animateFurnitureMotion (sv.Room 1 pathSm。轴(f))显示家具的痕迹Skip = 6;states = pathSm。州([1:跳过:结束,pathSm。NumStates):);exampleHelperShowFurnitureTrace (sv.Room。FurnituresInRoom{1}、州);