主要内容

基于RRT的机械臂运动规划

这个示例使用:

打开生活的脚本

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

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

亲属= loadrobot (“kinovaJacoJ2S7S300”）;

创建环境

使用碰撞对象原语，添加一个地板，两个桌面和一个圆柱体。指定这些对象的大小和姿态。所提供的图像显示了创建的环境。

floor = collisionBox(1,1, 0.01); / /地板tabletop1 = collisionBox(0.4、1、0.02);tabletop1。构成= trvec2tform ([0.3, 0, 0.6]);tabletop2 = collisionBox (0.6, 0.2, 0.02);tabletop2。构成= trvec2tform ((-0.2, 0.4, 0.5));可以= collisionCylinder (0.03, 0.16);可以。构成= trvec2tform ((0.3, 0.0, 0.7));

定制机械手的状态空间

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

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

  • 关节空间中的均匀随机样本。尊重联合限制。

第一种采样策略有助于将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轴允许旋转的允许旋转(以弧度为单位)。请注意，Roll-Pitch-Yaw (RPY)欧拉角是可以直观指定的。

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

%创建状态空间并设置工作区目标区域(WGRs)党卫军= 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;% z0 0;R %0 0;% Pπ-π);Y %setWorkspaceGoalRegion(党卫军,Tw_0 Te_w,范围);

自定义状态验证器

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

sv = ExampleHelperValidatorRigidBodyTree (ss);在环境中添加障碍tabletop1 addFixedObstacle (sv,“tabletop1”， [71 161 214]/256);tabletop2 addFixedObstacle (sv,“tabletop2”， [71 161 214]/256);addFixedObstacle (sv,可以,“可以”，“r”）;addFixedObstacle (sv、地板、“地板”, 1, 0.5, 0);为了性能，跳过某些物体的碰撞检查skipCollisionCheck (sv),“根”）;永远不会碰到任何障碍skipCollisionCheck (sv),“j2s7s300_link_base”）;% base永远不会碰到任何障碍skipCollisionCheck (sv),“j2s7s300_end_effector”）;这是一个虚拟帧%设置验证距离sv。ValidationDistance = 0.01;

计划抓握动作

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

%为可重复的结果设置随机种子rng (0,“旋风”）% 0%计算参考目标配置。注意，这只适用于目标偏差大于0的情况。Te_0ref = Tw_0 * Te_w;参考末端执行器在世界坐标中的姿态，由WGR导出本土知识= inverseKinematics (“RigidBodyTree”、亲属);refGoalConfig =本土知识(ss.EndEffector、Te_0ref (1,6), homeConfiguration (ss.RigidBodyTree));计算初始配置(末端执行器初始在表下)T = Te_0ref;T(1、4)= 0.3;T(2、4)= 0.0;T(3、4)= 0.4;initConfig =本土知识(ss.EndEffector T (1,6), homeConfiguration (ss.RigidBodyTree));从先前创建的状态空间和状态验证器中创建计划器规划师= plannerRRT (ss、sv);%如果树中的一个节点落在WGR中，则认为找到了一条路径。计划。GoalReachedFcn = @exampleHelperIsStateInWorkspaceGoalRegion;%设置最大连接距离。计划。MaxConnectionDistance = 0.5;%使用WGR，不需要指定一个特定的目标配置(使用% initConfig保存位置)。因此，你可以将“目标偏差”设为零。计划。GoalBias = 0;[pthObj,solnInfo] = plan(planner,initConfig, initConfig);

想象抓握动作

找到的路径首先通过递归切角策略[3]进行平滑处理，然后运动才被动画化。

铺平道路。插入(pthObj, 100);newPathObj = exampleHelperPathSmoothing (pthObj, sv);插入(newPathObj, 200);figure states = newpathb . 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:length(states)“PreservePlot”假的,“帧”，“关闭”，“父”、ax);drawnow结束q =州(我:);抓住罐头。q = exampleHelperEndEffectorGrab (sv,“可以”q ax);

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

targetPos = (-0.15, 0.35, 0.51);exampleHelperDrawHorizontalCircle (targetPos, 0.02,“y”、ax);

图中包含一个轴对象。axis对象包含28个类型为patch, line的对象。这些对象代表世界,根、j2s7s300_link_base j2s7s300_link_1, j2s7s300_link_2, j2s7s300_link_3, j2s7s300_link_4, j2s7s300_link_5, j2s7s300_link_6, j2s7s300_link_7, j2s7s300_end_effector, j2s7s300_link_finger_1, j2s7s300_link_finger_tip_1, j2s7s300_link_finger_2, j2s7s300_link_finger_tip_2, j2s7s300_link_finger_3,j2s7s300_link_finger_tip_3,工件、root_mesh j2s7s300_link_base_mesh、j2s7s300_link_1_mesh j2s7s300_link_2_mesh, j2s7s300_link_3_mesh, j2s7s300_link_4_mesh, j2s7s300_link_5_mesh, j2s7s300_link_6_mesh, j2s7s300_link_7_mesh, j2s7s300_end_effector_mesh, j2s7s300_link_finger_1_mesh, j2s7s300_link_finger_tip_1_mesh, j2s7s300_link_finger_2_mesh,J2s7s300_link_finger_tip_2_mesh, j2s7s300_link_finger_tip_3_mesh, j2s7s300_link_finger_tip_3_mesh, workpiece_mesh。

制定行动计划

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

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

可视化运动。

州= pthObj2.States;视图(ax, 152年,45)为I = 2:length(states)“PreservePlot”假的,“帧”，“关闭”，“父”、ax);drawnow结束q =州(我:);放开罐头q = exampleHelperEndEffectorRelease (sv, q, ax);

图中包含一个轴对象。axis对象包含51个类型为patch, line的对象。这些对象代表世界,根、j2s7s300_link_base j2s7s300_link_1, j2s7s300_link_2, j2s7s300_link_3, j2s7s300_link_4, j2s7s300_link_5, j2s7s300_link_6, j2s7s300_link_7, j2s7s300_end_effector, j2s7s300_link_finger_1, j2s7s300_link_finger_tip_1, j2s7s300_link_finger_2, j2s7s300_link_finger_tip_2, j2s7s300_link_finger_3,j2s7s300_link_finger_tip_3,工件、root_mesh j2s7s300_link_base_mesh、j2s7s300_link_1_mesh j2s7s300_link_2_mesh, j2s7s300_link_3_mesh, j2s7s300_link_4_mesh, j2s7s300_link_5_mesh, j2s7s300_link_6_mesh, j2s7s300_link_7_mesh, j2s7s300_end_effector_mesh, j2s7s300_link_finger_1_mesh, j2s7s300_link_finger_tip_1_mesh, j2s7s300_link_finger_2_mesh,j2s7s300_link_finger_tip_2_mesh、j2s7s300_link_finger_3_mesh j2s7s300_link_finger_tip_3_mesh。

参考文献

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机器人与自动化学报，第14卷第3期(1998):190 -403

导航工具的文档

万博1manbetx