逆运动学轨迹控制模型

此示例使用：

此Simu金宝applink示例演示了逆运动学块可以沿指定的轨迹驱动机械手。所需的轨迹被指定为机械手末端执行器的一系列紧密间隔的姿势。轨迹生成和航点定义代表了许多机器人应用程序，如挑选和放置操作，从空间加速度和速度配置文件计算轨迹，甚至使用摄像机和计算机视觉模仿关键帧的外部观察。一旦产生轨迹，逆运动学块用于将其转换为关节空间轨迹，然后可以用于模拟操纵器和控制器的动态。

模型概述

加载模型以查看它是如何构建的。

open_system ('iktrajectorycontrolexample.slx'）;

该模型由四个主要操作组成：

目标姿势生成
逆运动学
操纵器动力学
姿态测量

目标姿势生成

这个状态流程图选择哪个航路点是操纵器的当前目标。一旦操纵器到达当前目标的容忍范围内，图表将目标调整到下一个航路点。图表还将路径点的组件转换和组装为齐次转换Eul2tform.功能。一旦没有更多的航点来选择，图表终止了模拟。

逆运动学

逆运动学计算一组关节角，以产生所需的姿态末端执行器。使用逆运动学与A.rigidBodyTree模型并指定终止效应的目标姿势作为均匀转化。为解决方案的位置和方向指定相对公差约束的一系列权重，并初始估计接头位置。该块输出具有从中产生所需姿势的接合位置的向量rigidBodyTree在块参数中指定的模型。为保证解的平滑连续性，将前面的配置解作为求解器的起始位置。金宝搏官方网站这也减少了计算的冗余，如果目标姿态没有更新自最后一个模拟时间步长。

操纵器动力学

操纵器动力学由两个组件组成，控制器产生扭矩信号和动力学模型，以考虑到这些扭矩信号的操纵器的动态。该示例中的控制器使用通过操纵器的逆动态计算的前馈组件和反馈PD控制器来纠正错误。操纵器的模型使用前进动力与rigidBodyTree目的。有关更复杂的动态和可视化技术，请考虑利用来自Control Systembox™SlockSet和Simscape Multibody™的工具来替换前向动态块。

姿态测量

姿势测量从操纵器模型中采用关节角度读数，并将它们转换成均匀的变换矩阵以用作反馈航点选择部分。

操纵器定义

用于此示例的机械手是Rethink Sawyer™机器人机械手。这rigidBodyTree描述操纵器的对象是从URDF（Unified Robot描述格式）文件导入的ImportRobot.．

％将机械手导入刚性小组特对象Sawyer = ImportRobot（“sawyer.urdf”）;索耶。DataFormat ='柱子';％定义终端效应器正文名称eeName ='右手';％定义了操纵器中的关节数numJoints = 8;%可视化机械手展示（Sawyer）;XLIM（[ -  1.00 1.00]）ylim（[ -  1.00 1.00]）;Zlim（[ -  1.02 0.98]）;查看（[128.88 10.45]）;

图中包含一个轴。坐标轴包含53个patch、line类型的对象。这些对象代表基座，控制器盒，基座，基座，right_arm_base_link, right_l0，头部，屏幕，头部相机，right_l1, right_l2, right_l3, right_l4, right_arm_itb, right_l5, right_hand_camera, right_l6，右手，右手腕，right_torsoitb，躯干，基座网，right_arm_base_link_mesh, right_l0_mesh，头部网，丝网网，right_l1_mesh, right_l2_mesh, right_l3_mesh, right_l4_mesh, right_l5_mesh, right_l6_mesh, torso_mesh。

航点一代

在这个例子中，机械手的目标是能够追踪出图像中检测到的硬币的边界，coins.png．首先，对图像进行处理，找到硬币的边界。

我= imread（'coins.png'）;bwboundaries = imread（'coinboundaries.png'）;图次要情节(1、2、1)imshow(我“边界”那'紧的'） 标题（'原始图像'）子图（1,2,2）imshow（Bwboundaries，“边界”那'紧的'） 标题（“具有边界检测的处理图像”）

图包含2个轴。具有标题原始图像的轴1包含类型图像的对象。带有边界检测的标题处理图像的轴2包含类型图像的对象。

图像处理后，提取硬币的边缘作为像素位置。数据是从mat文件加载进来的，边界数据．界限是一个小区阵列，其中每个单元包含描述单个检测边界的像素坐标的阵列。在示例中可以找到如何生成此数据的更全面的观点，“图像中的边界跟踪”（需要图像处理工具箱）。

加载naugulardata.mat.界限谁是界限

名称大小字节类属性边界10x1 25376 cell

要将此数据映射到世界框架，我们需要定义图像所在的位置以及像素坐标和空间坐标之间的缩放。

%图像原点坐标imageOrigin = (0.4, 0.2, 0.08);从像素转换为物理距离的％比例因子Scale = 0.0015;

每个点上期望的末端执行器方向的欧拉角也必须定义。

eeorientation = [0，pi，0];

在该示例中，选择方向，使得末端执行器总是垂直于图像的平面。

一旦定义了该信息，就可以将每组期望的坐标和欧拉角度编译成航点。每个航点表示为六元素向量，其前三个元素对应于所需xyz -操纵器在世界框架中的位置。最后三个元素对应于所需方向的Zyx欧拉角。

$路标 = [\begin{matrix} X & y & Z. & ϕ_{Z.} & ϕ_{y} & ϕ_{X} \end{matrix}]$

航点被连接到形成一个N-My-6阵列，在哪里N是轨迹中的总数。阵列中的每一行对应于轨迹中的航点。

％清除前一个航点并开始构建航点数组清晰的航点％刚刚开始于图像源waypt0 = [imageorigin + [0 0.2]，eeorientation];触摸图像的原点waypt1 = [imageorigin，eeorientation];％插入每个元素，以便平滑地移动到图像的原点为了i = 1：6 Interp = linspace（waypt0（i），waypt1（i），100）;航点（：，i）= interp';结尾

总共有10个硬币。对于像素性和速度，可以通过限制传递到航点的总数来跟踪较小的硬币子集。下面，NumTraces = 3个硬币在图像中追踪。

％定义痕迹的硬币数量numtraces = 3;%组装路线点以进行边界跟踪为了i = 1:min(numTraces, size(bounds,1))％选择边界并映射到物理大小段=边界{i} *比例;接近航点的％垫数据和边界之间的升力航点段=[段(1:);段(:,);段(,)):;% z轴偏移用于在边界之间移动段（1,3）= .02;段（结束，3）= .02;％转换为图像的起源cartesianCoord = imageOrigin +分段;％重复期望的方向以匹配添加的航点数eulerAngles = repmat (eeOrientation、大小(段,1),1);％附加数据到前一行机的结尾waypoints = [waypoints;Cartesiancoord，Eulerangles];结尾

此数组是模型的主要输入。

模型设置

在运行模型之前，必须初始化几个参数。

％初始化Q0大小，T = 0的机器人联合配置。这会％以后被第一航点代替。q0 = zeros（numjoints，1）;％定义模拟的采样率。ts = .01;％定义了在方向上的相对权重的[1x6]向量逆运动学求解器的％位置误差。重量= =（1,6）;％将第一个航点转换为均匀变换矩阵进行初始化inittargetpose = Eul2tform（航点（1,4：6））;inittargetpose（1：3，结束）=航点（1,1：3）';％求解Q0，使得操纵器从第一航点开始本土知识= inverseKinematics ('刚性小组特雷'索耶);本土知识[q0 solInfo] = (eeName initTargetPose,权重q0);

模拟操纵器运动

要模拟模型，请使用SIM命令。模型生成输出数据集，教育部并显示两个地块中的进展：

这x y plot.显示机械手的跟踪运动的自上而下视图。当操纵器从一个硬币轮廓过渡到下一个轮廓时，圆圈之间的线就会出现。
这路径跟踪plot以3D方式显示进程。绿色的点表示目标位置。红点表示末端执行器使用反馈控制实现的实际末端执行器位置。

％关闭目前开放的数字关闭所有％打开并模拟模型open_system ('iktrajectorycontrolexample.slx'）;sim卡('iktrajectorycontrolexample.slx'）;

图2D Plot包含一个轴。标题为xy Plot的轴包含一个类型的对象。

图包含2个轴。带有标题航点跟踪的轴1是空的。轴2包含3个类型的线。

可视化结果

该模型输出两个可以用于仿真后可视化的数据集。提供联合配置教育部．机器人末端执行器姿态输出为poseData．

％删除不必要的网格以获得更快的可视化clearmeshes（Sawyer）;用于映射图像的％数据[m，n] =尺寸（i）;[x，y] = meshgrid（0：m，0：n）;x = imageorigin（1）+ x *比例;y = imageorigin（2）+ y * scale;z = zeros（尺寸（x））;z = z + imageorigin（3）;％关闭所有开放的数字关闭所有％初始化一个新的数字窗口数字;设置（GCF，“可见”那'上'）;％绘制初始机器人位置展示（Sawyer，Thepdata（1，:)'）;抓住在％初始化末端执行器绘图位置p = plot3（0,0,0，'。'）;经纱（x，y，z，i'）;％变化视图角度和轴视图（65,45）轴（[ - 。 - 。25 1 -.25 .75 0 0.75]）％迭代输出以10样本间隔，以可视化结果为了J = 1：10：长度（讲台）%显示机械手模型显示(索耶,jointData (j:) ','框架'那'离开'那“PreservePlot”， 错误的）;％从同性恋变换输出中获得最终效应位置pos = poseData (1:3 4 j);％更新末端执行器位置p.XData = [p。XData pos(1)]; p.YData = [p.YData pos(2)]; p.ZData = [p.ZData pos(3)];％更新图drawn结尾

图中包含一个轴。轴包含13个类型的线，表面，贴片物体。这些对象代表基础，控制器_Box，PedeStal_Link，PeiteStal，Right_arm_Base_Link，Right_L0，Head，Screen，Head_Camera，Right_L1，Right_L4，Right_Arm_ItB，Right_L5，Right_hand_Camera，Right_L6，Right_hand，Right_Wrist，Right_Torso_itb，Torso，Right_Arm_Base_Link_mesh，Right_L0_Mesh，Head_Mesh，screen_mesh，right_l1_mesh，right_l2_mesh，right_l3_mesh，right_l4_mesh，right_l5_mesh，right_l6_mesh，torso_mesh。