基于逆运动学的轨迹控制建模
这个Sim金宝appulink示例演示了如何逆运动学Block可以沿着指定的轨迹驱动机械手。所期望的轨迹被指定为机械手末端执行器的一系列紧密间隔的姿态。轨迹生成和路径点定义代表了许多机器人应用,如拾取和放置操作,从空间加速度和速度剖面计算轨迹,甚至使用摄像机和计算机视觉模拟关键帧的外部观察。一旦轨迹生成,逆运动学Block用于将其转换为关节空间轨迹,然后可用于模拟机械手和控制器的动力学。
模型概述
加载模型以查看它是如何构造的。
open_system (“IKTrajectoryControlExample.slx”);
该模型由四个主要操作组成:
目标姿态生成
逆运动学
机械手动力学
姿态测量
目标姿态生成
这个状态流图选择哪个路径点是操作器的当前目标。一旦操纵器到达当前目标的公差范围内,图表将目标调整到下一个航路点。图还将航路点的组件转换并组装为齐次转换eul2tform
函数。一旦没有更多的路径点可供选择,图表将终止模拟。
逆运动学
逆运动学计算了一组关节角度,以产生末端执行器所需的姿态。使用逆运动学与一个rigidBodyTree
将末端效应的目标位姿建模并指定为齐次变换。为解的位置和方向的相对公差约束指定一系列权重,并给出关节位置的初步估计。该块输出一个关节位置向量,该关节位置向量产生所需的姿态rigidBodyTree
块参数中指定的模型。为了保证解的平滑连续性,求解器的起始位置采用前面的配置解。金宝搏官方网站这也减少了计算的冗余,如果目标姿态没有更新自最后一个仿真时间步骤。
机械手动力学
机械手的动力学由两个部分组成,一个是产生力矩信号的控制器,另一个是给定力矩信号的机械手动力学模型。本例中的控制器使用通过机械手逆动力学计算的前馈组件和反馈PD控制器来纠正错误。机械手的模型采用前进动力的BlockrigidBodyTree
对象。对于更复杂的动力学和可视化技术,可以考虑使用Control Systems Toolbox™块集和Simscape Multibody™中的工具来替代Forward dynamics块。
姿态测量
位姿测量从机械臂模型中获取关节角度读数,并将其转换为齐次变换矩阵,作为控制系统的反馈路径选择部分。
汇率操纵国的定义
本例中使用的机械手是Rethink Sawyer™机器人机械手。的rigidBodyTree
对象从URDF(统一机器人描述格式)文件中导入importrobot
.
将操作器导入为rigidBodyTree对象Sawyer = importrobot(“sawyer.urdf”);索耶。DataFormat =“列”;定义末端执行器主体名称eeName =“right_hand”;定义机械手的关节数量num关节= 8;%可视化操作器显示(索耶);Xlim ([-1.00 1.50]) ylim([-1.00 1.50]);zlim ([-1.02 - 0.98]);视图([128.88 - 10.45]);
航点代
在这个例子中,机械手的目标是能够追踪出图像中检测到的硬币的边界,coins.png
.首先,对图像进行处理,找出硬币的边界。
I = imread(“coins.png”);bwBoundaries = imread(“coinBoundaries.png”);图subplot(1,2,1)“边界”,“紧”)标题(原始图像的) subplot(1,2,2) imshow(bwBoundaries,“边界”,“紧”)标题(“带边界检测的处理图像”)
经过图像处理后,将硬币的边缘提取为像素位置。数据是从mat文件中加载的,boundaryData
.边界
是一个单元格数组,其中每个单元格包含一个数组,描述单个检测到的边界的像素坐标。关于如何生成此数据的更全面的视图可以在示例“图像中的边界跟踪”(需要图像处理工具箱)中找到。
负载boundaryData.mat边界谁边界
名称大小字节类属性边界10x1 25376单元格
为了将该数据映射到世界帧,我们需要定义图像的位置以及像素坐标和空间坐标之间的缩放。
%图像原点坐标imageOrigin = [0.4,0.2, 08];将像素转换为物理距离的比例因子刻度= 0.0015;
还必须定义每个点上所需的末端执行器方向的欧拉角。
eeOrientation = [0, pi, 0];
在这个例子中,方向的选择使得末端执行器总是垂直于图像的平面。
一旦定义了这些信息,每组所需的坐标和欧拉角就可以编译成一个路点。每个路径点表示为一个六元素向量,其中前三个元素对应于所需的路径点xyz -机械手在世界坐标系中的位置。后三个元素对应所期望方向的ZYX欧拉角。
这些路径点被连接起来形成一个n-by-6数组,其中n是轨迹中姿态的总数。数组中的每一行都对应于轨迹中的一个路径点。
清除之前的路径点并开始构建路径点数组清晰的路点%从图像原点上方开始waypt0 = [imageOrigin + [0 0 .2],eeOrientation];触摸图像的原点waypt1 = [imageOrigin,eeOrientation];插值每个元素以平滑运动到图像的原点为I = 1:6 interp = linspace(waypt0(I),waypt1(I),100);wayPoints(:,i) = interp';结束
总共有10枚硬币。为了简单和快速,可以通过限制传递给路径点的总数量来跟踪较小的硬币子集。下面,numTraces = 3个硬币在图像中被跟踪。
定义要追踪的硬币数量numTraces = 3;组装用于边界跟踪的路点为i = 1:min(numtrace, size(边界,1))选择边界并映射到物理大小Segment = boundaries{i}*scale;在边界之间填充进近路点和升力路点的数据分段=[分段(1,:);段(:,);段(,)):;用于在边界之间移动的z偏移量段(1,3)= .02;段(end,3) = .02;%翻译到图像的原点cartesianCoord = imageOrigin + segment;%重复所需的方向,以匹配正在添加的路径点的数量eulerAngles = repmat(eeOrientation,size(segment,1),1);将数据追加到前一个路径点的末尾wayPoints = [wayPoints;cartesianCoord eulerAngles);结束
这个数组是模型的主要输入。
模型设置
在运行模型之前,必须初始化几个参数。
初始化尺寸q0, t=0时机器人关节构型。这将%稍后将被第一个路径点替换。q0 = 0 (num关节,1);定义模拟的采样率。Ts = .01;在方向和上定义一个[1x6]相对权重向量%位置误差为逆运动学求解器。Weights = ones(1,6);将第一个路径点转换为齐次变换矩阵进行初始化initTargetPose = eul2tform(wayPoints(1,4:6));initTargetPose(1:3,end) = wayPoints(1,1:3)';求解q0,使操纵符从第一个路径点开始ik =逆运动学(“RigidBodyTree”索耶);[q0,solInfo] = ik(eeName,initTargetPose,weights,q0);
模拟机械手运动
要模拟模型,请使用sim卡
命令。模型生成输出数据集,jointData
并在两个图中显示了进展:
的X - Y图显示机械手跟踪运动的自顶向下视图。当操纵器从一个硬币轮廓过渡到下一个硬币轮廓时,圆之间的线条出现。
的路径跟踪图以3D方式显示进度。绿色圆点表示目标位置。红点表示末端执行器使用反馈控制实现的实际末端执行器位置。
关闭当前打开的数字关闭所有打开并模拟模型open_system (“IKTrajectoryControlExample.slx”);sim卡(“IKTrajectoryControlExample.slx”);
可视化结果
该模型输出两个数据集,可用于仿真后的可视化。所述接头配置为jointData
.机器人末端执行器的姿态输出为poseData
.
删除不必要的网格,以更快的可视化clearMeshes(索耶);%映射图像的数据[m,n] = size(I);[X,Y] = meshgrid(0:m,0:n);X = imageOrigin(1) + X*scale;Y = imageOrigin(2) + Y*scale;Z = 0(大小(X));Z = Z + imageOrigin(3);关闭所有打开的数字关闭所有初始化一个新的图形窗口图;集(gcf,“可见”,“上”);绘制机器人初始位置显示(索耶,jointData (: 1) ');持有在初始化末端执行器绘图位置P = plot3(0,0,0,“。”);经(X, Y, Z,我');%改变视角和轴视图(65,45)轴([-。25 1 -。25 .75 0 0.75])以10个样本的间隔迭代输出以可视化结果。为j = 1:10:length(jointData)显示机械手型号显示(索耶,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)];%更新数字drawnow结束