基于RRT的机械臂运动规划

此示例使用：

打开直播脚本

为一个掌握动作Kinova Jaco辅助机器人手臂使用快速探索随机树（RRT）算法。这个例子使用了一个plannerRRTStar对象来采样状态并规划机器人的运动。提供的示例助手演示了如何为运动规划应用程序定义自定义状态空间和状态验证。

从机器人库加载Kinova Jaco模型。这个特殊的型号包括三指夹持器。

亲属= loadrobot ('kinovajacoj2s7s300'）;

创建环境

使用碰撞物体基元，添加楼层，两张桌子和圆筒。指定这些对象的大小和姿势。提供的图像显示了创建的环境。

楼层= CollisionBox（1,1,0.01）;桌面1 = CollisionBox（0.4,1,0.02）;桌面1.Posse = trvec2tform（[0.3,0,0.6]）;桌面= CollisionBox（0.6,0.2,0.02）;桌面= trvec2tform（[ -  0.2,0.4,0.5]）;CAN = COLLISIONCYLIDER（0.03,0.16）;can.paits = trvec2tform（[0.3,0.0,0.7]）;

定制机械手的状态空间

Kinova手臂有十个自由度(自由度)，最后三个自由度对应于手指。只使用前七个自由度的规划和保持手指在零配置(开放宽)。一个ExampleHelperRigidBodyTreeStateSpace状态空间是用来表示配置空间(关节空间)的。ExampleHelperRigidBodyTreeStateSpace对机器人手臂的可行状态进行采样。的Samemiform.状态空间函数在以下两种等概率抽样策略之间交替:

均匀随机采样的末端执行器位姿工作空间的目标区域围绕参考目标姿态，然后通过逆运动学将其映射到关节空间。尊重联合限制。
联合空间中的均匀随机样品。尊重联合限制。

第一种采样策略有助于将RRT规划器引导到任务空间中的目标区域，使RRT更快地收敛到一个解，而不是迷失在七自由度关节空间中。

使用工作空间的目标区域(WGR)而不是单一目标姿态增加了通过将样本偏置到目标区域找到解决方案的机会。WGR定义了特定任务可接受的末端执行器姿态的连续体。例如，机器人可以从多个方向靠近，从侧面抓取一杯水，只要它不与环境发生碰撞。WGR的概念最早是由Dmitry Berenson等人在2009年提出的。这个算法后来发展成任务空间区域[2]。WGR由三部分组成：

Twgr_0- WGR在世界({0})坐标中的参考变换
Te_w-从WGR中采样{w}坐标中{w}的末端执行器偏移变换
界限- WGR参考坐标的一个6乘2的边界矩阵。前三行界限沿X，Y和Z轴（以米为单位）设置允许的平移，最后三个设置围绕X，Y和Z轴（在Radians中）的允许旋转的允许旋转。注意，辊隙横摆（rpy）欧拉角在可以直观地指定。

您可以在一个规划问题中定义和连接多个WGRs。本例中只允许一个WGR。

％创建状态空间和设置工作区目标区域（WGR）党卫军= ExampleHelperRigidBodyTreeStateSpace(亲属);ss.EndEffector =“j2s7s300_end_effector”；％定义工作区目标区域（WGR）%这个WGR告诉规划者可以从哪里掌握%侧面与实际抓握高度可摆动最多1厘米。％这是抓住姿势和罐框架中的末端执行器之间的方向偏移r = [0 0 1;1 0 0;0 1 0];tw_0 = can.pose;Te_w = rotm2tform (R);边界= [0 0;％ X0 0;％Y.0 0.01;％z.0 0;％r.0 0;％P.π-π);％Y.setWorkspaceGoalRegion(党卫军,Tw_0 Te_w,范围);

自定义状态验证器

自定义状态验证器，ExampleHelperValidatorRigidBodyTree，提供机器人与环境之间的刚体碰撞检测。这个验证器检查取样的配置，规划器应该丢弃无效的状态。

sv = ExampleHelperValidatorRigidBodyTree (ss);％增加了环境中的障碍tabletop1 addFixedObstacle (sv,“tabletop1”， [71 161 214]/256);addfixedobstacle（SV，桌面2，“tabletop2”， [71 161 214]/256);addFixedObstacle (sv,可以,“可以”，“r”）;addfixedobstacle（SV，楼层，“地板”, 1, 0.5, 0);为了性能，跳过某些物体的碰撞检查skipCollisionCheck (sv),“根”）;永远不会碰到任何障碍skipCollisionCheck (sv),“j2s7s300_link_base”）;％基地永远不会触及任何障碍skipCollisionCheck (sv),“j2s7s300_end_effector”）;％这是一个虚拟帧%设置验证距离sv.validationDistance = 0.01;

计划掌握运动

使用plannerRRT对象的自定义状态空间和状态验证器对象。使用inverseKinematics基于末端执行器位姿求解构型。指定GoalReachedFCN.使用exampleHelperIsStateInWorkspaceGoalRegion，检查路径是否到达目标区域。

%为可重复的结果设置随机种子rng (0,“旋风”）% 0%计算参考目标配置。注意，这只适用于目标偏差大于0的情况。te_0ref = tw_0 * te_w;世界坐标中的％参考末端效应器姿势，来自于WGRik = underedkinematics（“RigidBodyTree”，亲属）;RefggoalConfig = IK（SS.ENDEFFERCHER，TE_0REF，ONE（1,6），homeconfiguration（ss.rigidbodytree））;％计算初始配置（最终执行器最初在表格下）t = te_0ref;t（1,4）= 0.3;T（2,4）= 0.0;T（3,4）= 0.4;initconfig = ik（ss.endeffector，t，then（1,6），homeconfiguration（ss.rigidbodytree））;从先前创建的状态空间和状态验证器中创建计划器规划师= plannerRRT (ss、sv);%如果树中的一个节点落在WGR中，则认为找到了一条路径。planner.goalreachedfcn = @examplehelperisstateinworkspacegoalregion;%设置最大连接距离。计划。MaxConnectionDistance = 0.5;%使用WGR，不需要指定一个特定的目标配置(使用％initconfig持有该地点）。因此，你可以将“目标偏差”设为零。planner.goalbias = 0;[pthobj，solninfo] =计划（Planner，initconfig，initconfig）;

可视化掌握运动

在运动动画之前，首先通过递归角切割策略[3]平滑发现的路径。

铺平道路。插值（Pthobj，100）;newpateobj = examplehelperpathsmooth（pthobj，sv）;插值（NewPathobj，200）;图态= newpateobj.states;画机器人。ax =显示(亲属、州(1:));zlim (ax, [-0.03, 1.4]) xlim (ax, [1]) ylim (ax, [1])％渲染环境。持有在Showobstacles（SV，AX）;查看（146,33）Camzoom（1.5）显示动作。为i = 2：长度（状态）显示（亲属，州（我，:)，“PreservePlot”，错误的，“帧”，'离开'，'父母'、ax);drawnow结束q =州(我:);抓住罐头。q = examplehelperendeffectorgrab（sv，“可以”q ax);

在另一个桌面上为罐子设置一个目标溶液。

targetpos = [-0.15,0.35,0.51];examplehelperdrawhorizoNtalcircle（targetpos，0.02，'是'、ax);

制定行动计划

在移动过程中，保持气缸处于水平状态，避免泄漏。为RRT规划器指定临时机械手配置的附加约束。通过设置UseConstrainedSampling属性为真。

Tw_0 = trvec2tform (targetPos + [0, 0, 0.08]);Te_w = rotm2tform (R);边界= [0 0;％ X0 0;％Y.0 0;％z.0 0;％r.0 0;％P.π-π);％Y.setWorkspaceGoalRegion(党卫军,Tw_0 Te_w,范围);ss.UseConstrainedSampling = true;计划。MaxConnectionDistance = 0.05;[pthObj2 ~] =计划(规划师q q);

可视化运动。

= pthobj2.states;查看（斧头，152,45）为i = 2：长度（状态）显示（亲属，州（我，:)，“PreservePlot”，错误的，“帧”，'离开'，'父母'、ax);drawnow结束q =州(我:);％放开罐头q = exampleHelperEndEffectorRelease (sv, q, ax);

参考文献

D. Berenson, S. Srinivasa, D. Ferguson, A. Collet，和J. Kuffner，“工作空间目标区域的操纵规划”，在IEEE机器人和自动化国际会议的诉讼程序，2009年,pp.1397 - 1403

[2] D. Berenson, S. Srinivasa, J. Kuffner，“任务空间区域:姿态约束操作规划框架”，国际机器人研究杂志， Vol. 30, No. 12 (2011): 1435-1460

[3] P. Chen和Y. Hwang，“Sandros：运动规划动态图搜索算法”，IEEE交易机器人和自动化，Vol。14号3（1998）：390-403