为机械手路径选择轨迹
本例概述了机器人系统工具箱™中可用的轨迹类型。对于机械手的运动、规划和控制应用,必须为机器人选择一个轨迹。这个示例有三个主要部分。第一部分展示了机械手使用的轨迹类型,第二部分展示了生成轨迹的函数,最后一部分展示了更多用于轨迹规划的工具。
机械手的轨迹类型
当使用带有操纵器的轨迹时,目标是在某些约束条件下提供一个可行的结果。例如,您通常希望轨迹具有平滑和连续的导数,例如不需要瞬时速度或加速度的轨迹。轨迹也可能受位置、速度或加速度的限制。在关节空间和任务空间中有两种使用轨迹的方法.
关节空间轨迹
一个关节空间轨迹通常包含一组使用平滑运动连接的多个机器人配置的路点。这个示例使用梯形速度剖面生成一个轨迹,它逐步加速每个关节到最大速度,并在接近下一个路径点时减慢速度。有关更多信息,请参见梯形速度剖面轨迹.
为Franka Emika Panda机器人生成梯形速度剖面轨迹以连接路径点。首先,定义轨迹的时间向量并加载机器人模型。
TPTS = 0:4;sampleRate = 20;tvec = tpts(1):1/sampleRate:tpts(end);numSamples = length(tvec)机器人=装载机器人(“frankaEmikaPanda”DataFormat =“列”);
定义关节空间轨迹。对于该轨迹,路径点是模型的主配置和两个随机配置。
rng默认的机器人。homeConfiguration机器人。randomConfiguration robot.randomConfiguration];frankaTimepoints = linspace(tvec(1),tvec(end),3);[q,qd] = trapveltraj(frankaWaypoints,numSamples);
通过迭代生成的轨迹点,可视化机器人执行轨迹问
.
图组(gcf,“可见”,“上”);rc = rateControl(sampleRate);为i = 1:numSamples show(robot,q(:,i),FastUpdate=true,PreservePlot=false);等待(rc);结束
为了检查不同的关节位置和速度,画出所有的尺寸随时间的变化。使用helperPlotJointSpaceTraj
绘制关节空间轨迹及其路径点的辅助函数。
helperPlotJointSpaceTraj (“关节空间轨迹和路点”,...tvec q qd、frankaWaypoints frankaTimepoints);
任务空间轨迹
一个任务空间轨迹包含在三维空间中表示末端执行器运动的路径点。生成最小颠簸轨迹以连接自由空间中的路径点。这种弹道轮廓的目的是创建一个平滑的弹道与最小的颠簸运动。有关更多信息,请参见Minimum-Jerk轨迹.然后利用逆运动学方法生成Franka Emika Panda机器人的关节构型。
首先,创建一组路径点,然后使用创建最小颠簸轨迹minjerkpolytraj
.
frankaSpaceWaypoints = [0.5 0.25 0.25;0.75 0 0.35;0.5 -0.25 0.25;0.5 0.25 0.25]';frankaTimepoints = linspace(tvec(1),tvec(end),4);[pos,vel] = minjerkpolytraj(frankaSpaceWaypoints,frankaTimepoints,numSamples);
使用inverseKinematics
函数来创建逆运动学求解器,并求解在轨迹上达到所需的末端执行器位置的配置。
rng (0)%种子RNG,使逆运动学解是一致的ik = inverseKinematics(RigidBodyTree=robot);ik.SolverParameters.AllowRandomRestart = false;q = 0 (9,numSamples);权重= [0.2 0.2 0.2 1 1 1];优先考虑职位而不是方向initialGuess =[0, 0, 0, -π/ 2,0,0,0,0.01,0.01)';在机器人关节范围内选择一个初始值。为i = 1:size(pos,2) targetPose = trvec2tform(pos(:,i)')*eul2tform([0,0, pi]);Q (:,i) = ik(“panda_hand”,重量targetPose initialGuess);initialGuess = q(:,i);使用最后的结果作为下一个初始猜测结束
用机器人模型展示结果。
图组(gcf,“可见”,“上”)(机器人);
rc = rateControl(sampleRate);为i = 1:numSamples show(robot, q(:,i),FastUpdate=true,PreservePlot=false);等待(rc);结束
为了检查不同的关节位置和速度,通过绘制所有尺寸与时间的关系来可视化这些结果。使用helperPlotTaskSpaceTraj
绘制任务空间轨迹及其路径点的辅助函数。
helperPlotTaskSpaceTraj (“任务-空间轨迹和路径点”,...tvec、pos、韦尔,frankaSpaceWaypoints frankaTimepoints);
比较各种弹道轮廓
使用不同的工具生成轨迹,然后使用任务和关节空间可视化对它们进行比较。
WPTS = [0 45 15 90 45;90 45 -45 15 90];TPTS = 0:(size(wpts,2)-1);导出的量。sampleRate = 20;tvec = tpts(1):1/sampleRate:tpts(end);numSamples = length(tvec)
Minimum-Jerk轨迹
的minjerkpolytraj
功能连接路径点使用平滑,连续的运动。在默认边界条件下,轨迹的初始速度和最终速度为零,但通过所有中间路径点的速度为连续速度。
最小颠簸轨迹之所以如此命名,是因为它们最小化了运动的第三次时间导数,从而产生了一个平滑的轮廓,这对机械系统来说很方便。基本的最小颠簸轨迹是在指定时间点命中路径点的解析解。
[q,qd,~,~,~,~,tvec] = minjerkpolytraj(wpts,tpts,numSamples);helperPlotTaskSpaceTraj (“Minimum-Jerk轨迹”,问tvec qd、wpt tpt);
梯形速度剖面轨迹
一个梯形速度剖面停止在每个航路点,并确保平稳的点对点的运动。配置文件名称来自于连接两个路径点的每个段的三个阶段:
从零速度到峰值速度的加速度
峰值速度恒定
减速到零速度
这导致速度剖面是一个梯形在每一段。每个部分都由结束时间、峰值速度、峰值加速度和加速时间参数来表征,但指定任何两个都足以完全定义运动。有关更多信息,请参见trapveltraj
.
当目标是连接一组路径点时,您可以使用基本的梯形轮廓,并在每个路径点上停下来。例如,这段代码使用1秒段连接路径点。
[q,qd,~,t] = trapveltraj(wpts,100);helperPlotTaskSpaceTraj (梯形剖面,最大速度= 0.5, t, q, qd、wpt);
梯形轮廓经常寻求满足某些约束条件,如速度或加速度的界限。因为梯形轮廓是一个精确的规范,所以使用一个辅助函数将约束边界转换为精确的轮廓规范。的helperProfileForMaxVel
辅助函数接受速度边界。的详细信息trapveltraj
函数来设计速度剖面,请参见利用梯形速度剖面设计具有速度限制的弹道的例子。
[endTimes,peakVels] = helperProfileForMaxVel(wpts, 0.5);[q,qd,~,t] = trapveltraj(wpts,100,EndTime=endTimes,PeakVelocity=peakVels);路径点被命中的时间是累积和的向量结束次数的%trapVelTrajTime = [0 cumsum(endTimes(1,:))];helperPlotTaskSpaceTraj (梯形剖面,最大速度= 0.5wpt,, t, q, qd trapVelTrajTime);
三次和五次多项式轨迹
有些用例可能需要更一般的多项式轨迹。的cubicpolytraj
而且quinticpolytraj
函数是创建分段多项式插值的常用工具。像前面的轨迹工具一样,它们返回位置、速度和加速度,以及分段多项式对象。
在默认情况下,这些函数使用零值边界条件,导致轨迹在每个路径点都停止。
[q,qd] = cubicpolytraj(wpts,tpts,tvec);helperPlotTaskSpaceTraj (“基本三次多项式”,问tvec qd、wpt tpt);
您可以使用这些函数来设计具有自定义边界行为的插值多项式。例如,通过使用这些函数和其他分段多项式工具,比如样条
,pchip
,或makima
,您可以在路径点上创建具有理想运动的平滑轮廓。
该示例代码从MATLAB中内置的分段多项式函数之一导出边界条件。选择一个多项式函数,看看它如何影响三次多项式速度剖面。代码使用一个助手文件来计算速度。
smoothPP =样条(wpt tpt);smoothVelPP = mkpp(smoothPP.breaks, robots .core. inner . polycoeffsderivative (smoothPP.coefs),size(wpts,1));smoothVelPoly = ppval(smoothVelPP,tpts);
用导出的速度作为内路点的边界条件。保持外部边界条件为零速度,以确保弹道的起始和结束速度均为零
boundaryVel = 0(大小(smoothVelPoly));boundaryVel(:,2:end-1) = smoothVelPoly(:,2:end-1);[q,qd] = cubicpolytraj(wpts,tpts,tvec,速度边界条件=边界vel);helperPlotTaskSpaceTraj (自定义速度bc的三次多项式,问tvec qd、wpt tpt);
相反,使用五次多项式可以确保平滑的速度剖面。
[q,qd] = quinticpolytraj(wpts,tpts,tvec,VelocityBoundaryCondition=boundaryVel);helperPlotTaskSpaceTraj (“自定义速度bc的五次多项式”,问tvec qd、wpt tpt);
b样条轨迹
b样条多项式产生平滑连续的运动,主要用于任务空间应用。可以通过使用bsplinepolytraj
函数。与其他轨迹不同的是,b样条是由一组控制点定义的。得到的轨迹只命中初始控制点和最终控制点,但落在完整控制点集的凸包内。
应用之前使用的路径点作为控制点。
[q,qd] = bsplinepolytraj(wpts,tpts([1 end]),tvec);helperPlotTaskSpaceTraj (Non-interpolating b样条的,问tvec qd、wpt);
您可以通过从原始路径点推导出一组新的控制点来创建插值b样条,b样条多项式会对原始路径点进行插值。与标准b样条一样,将此多项式主要用于任务空间应用。
使用helperCreateControlPointsFromWaypoints
辅助函数来派生新的控制点。
cpts = helperCreateControlPointsFromWaypoints(wpts);[q,qd] = bsplinepolytraj(cpts,tpts([1 end]),tvec);helperPlotTaskSpaceTraj (插值b样条的,问tvec qd、wpt);
更多的任务空间轨迹工具
一些任务空间应用不仅受益于计算插值位置的轨迹,还受益于计算机械手的完整姿态。在这些情况下,考虑使用rottraj
而且transformtraj
功能。的rottraj
函数在两次旋转之间创建轨迹,而transformtraj
函数对两个4 × 4齐次变换矩阵做同样的运算。这些函数除了输出位置导数外,还输出角加速度和速度。
这个例子连接了两个姿势,T1
而且T2
,其中包含位置和方向数据。
T1 = eul2tform([pi/4 0 pi/3]);T2 = trvec2tform([5 -2 1]);tInterval = [0 1];Tvec = 0:0.01:1;
用插值两个变换矩阵transformtraj
为了创建一个完整的轨迹,插值的姿势,表示为变换矩阵。
[tfInterp,v1,a1] = transformtraj(T1,T2,tInterval,tvec);
在三维空间中绘制轨迹plotTransforms
.该函数既需要四元数形式的旋转数据,也需要插值变换矩阵轨迹的平移数据。使用tform2quat
求四元数和tform2trvec
从轨迹中找到平动数据。
图旋转= tform2quat(tfInterp);翻译= tform2trvec(tfInterp);plotTransforms(平移、旋转)标题(“内插变换轨迹”)包含(“X”) ylabel (“Y”) zlabel (“Z”)
三维图显示旋转和运动是线性的。可行的机械手轨迹应该是平滑连续的。由于插值是线性的,因此轨迹不能保证得到平滑的运动。当你分别画出位置和速度时,这就更清楚了。
图位置=重塑(tfInterp(1:3,4,:),3,大小(tfInterp,3));次要情节(1,1);情节(tvec职位)标题(“XYZ时间位置”) ylim (“填充”)次要情节(3、1、2);情节(tvec v1(1:3,:))标题(“时间速度”) ylim (“填充”)次要情节(3、1,3);情节(tvec v1 (4:6 -:));标题(“时间角速度”) ylim (“填充”)
速度开始和结束于非零值,这不是一个可行的轨迹,并将引起突然和急剧的运动。
您可以使用时间尺度
的名称-值参数transformtraj
作为一种变通办法。这个参数使用中间参数化定义了轨迹时间,
,以致于transformtraj
定义为
随着时间的。在本例中使用的默认情况下,时间缩放是均匀的,因此
.结果是每个姿势之间的线性运动。相反,使用由最小颠簸轨迹定义的时间缩放:
minjerkpolytraj
.
时间缩放是一组离散的值, ,对函数进行抽样 ,定义在区间上 .
%时间缩放是一个离散的值集合[s;ds / dt;^ 2 s d / dt ^ 2]%是函数s(t)的样本,定义在区间s = [0,1][s,sd,sdd] = minjerkpolytraj([0 1],tInterval,numel(tvec));[tfInterp,v1,a1] = transformtraj(T1,T2,tInterval,tvec,TimeScaling=[s;sd;sdd]);
再次绘制插值的变换轨迹,与之前的图进行比较。
图旋转= tform2quat(tfInterp);翻译= tform2trvec(tfInterp);plotTransforms(平移、旋转)标题(“内插变换轨迹”)包含(“X”) ylabel (“Y”) zlabel (“Z”)
图位置=重塑(tfInterp(1:3,4,:),3,大小(tfInterp,3));次要情节(1,1);情节(tvec职位)标题(“XYZ时间位置”)次要情节(3、1、2);情节(tvec v1(1:3,:))标题(“时间速度”)次要情节(3、1,3);情节(tvec v1(4:6 -:))标题(“时间角速度”)
当空间中的运动遵循相同的路径时,第二组图阐明了速度在时间上是平滑的和可跟踪的,从而得到了一个对机械手或其他机械系统可行的轨迹。