逆运动学轨迹控制模型

此示例使用：

打开直播脚本

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

模型概述

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

Open_System（'iktrajectorycontrolexample.slx'）;

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

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

目标姿势生成

此状态溢图选择了哪个航点是操纵器的当前目标。一旦操纵器进入当前目标的公差内，该图表将目标调整到下一个航点。图表还通过以下方式转换和组装航路点的组分成为均匀的转变Eul2tform.功能。一旦没有更多的航点来选择，图表终止了模拟。

逆运动学

逆运动学计算了一组关节角度以产生末端效应器的所需姿势。使用逆运动学与A.刚性小组细胞模型并指定终止效应的目标姿势作为均匀转化。为解决方案的位置和方向指定相对公差约束的一系列权重，并初始估计接头位置。该块输出具有从中产生所需姿势的接合位置的向量刚性小组细胞块参数中指定的模型。为确保解决方案的平稳连续性，先前的配置解决方案用作求解器的起始位置。金宝搏官方网站如果目标姿势由于自上次仿真时间步长以来未更新，则这也降低了计算的冗余。

操纵器动力学

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

姿态测量

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

操纵器定义

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

％将机械手导入刚性小组特对象Sawyer = ImportRobot（'sawyer.urdf'）;Sawyer.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个类型的贴片物体，线。这些对象代表基础，控制器_Box，PedeStal_Feet，PeiteTal，Right_arm_Base_Link，Right_L0，Head，Screen，Right_L2，Right_L3，Right_L4，Right_Arm_L3，Right_L5，Right_Hand_Camera，Right_L6，Right_Hand，Right_Wrist，Right_torso_itb，Torso，PedeStal_Mesh，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。

航点一代

在该示例中，操纵器的目标是能够挖掘图像中检测到的硬币的边界，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单元格

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

％图像来源坐标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（边界，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，使得操纵器从第一航点开始ik = underedkinematics（'刚性小组特雷'，锯割）;[Q0，Solinfo] = IK（eEname，inittargetpose，权重，q0）;

模拟操纵器运动

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

这x y plot.显示了操纵器的追踪运动的自上视图。圆圈之间的线路由于操纵器从一个硬币轮廓转换到下一个硬币。
这航点跟踪绘图可视化3D的进度。绿点表示目标位置。红色点表示通过反馈控制通过末端执行器实现的实际末端效应位置。

％关闭目前开放的数字关闭全部％打开并模拟模型Open_System（'iktrajectorycontrolexample.slx'）;SIM（'iktrajectorycontrolexample.slx'）;

图2D绘图包含轴。具有标题x y图的轴包含类型的对象。

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

可视化结果

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

％删除不必要的网格以获得更快的可视化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：长度（讲台）％显示操纵器模型展示（Sawyer，Thepdata（J，:)'，'框架'那'离开'那'preserveplot'， 错误的）;％从同性恋变换输出中获得最终效应位置pos = postata（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。