MATLAB - 为机械臂路径选择轨迹

系列文章目录

前言

本示例概述了机器人系统工具箱™中可用的轨迹类型。在机械手运动、规划和控制应用中，必须为机器人选择轨迹。本示例有三个主要部分。第一部分展示了机械手使用的轨迹类型，第二部分演示了生成轨迹的功能，最后一部分则展示了更多轨迹规划工具。

一、机械手的轨迹类型

在机械手中使用轨迹时，目标是在某些约束条件下提供可行的结果。例如，您通常希望轨迹具有平滑和连续的导数，如不需要瞬时速度或加速度的轨迹。轨迹还可能受位置、速度或加速度约束。在关节空间和任务空间中使用轨迹一般有两种方法。

1.1 关节空间轨迹

关节空间轨迹通常包含一组通过平滑运动连接的多个机器人配置的航点。本示例使用梯形速度曲线生成轨迹，每个关节逐渐加速到最大速度，并在接近下一个航点时减速。更多信息，请参阅梯形速度曲线轨迹。

为 Franka Emika Panda™ 机器人生成连接航点的梯形速度曲线轨迹。首先，定义轨迹的时间矢量并加载机器人模型。

tpts = 0:4;
sampleRate = 20;
tvec = tpts(1):1/sampleRate:tpts(end);
numSamples = length(tvec);robot = loadrobot("frankaEmikaPanda",DataFormat="column");

定义关节空间轨迹。对于该轨迹，航点是模型的原点配置和两个随机配置。

rng default
frankaWaypoints = [robot.homeConfiguration robot.randomConfiguration robot.randomConfiguration];
frankaTimepoints = linspace(tvec(1),tvec(end),3);
[q,qd] = trapveltraj(frankaWaypoints,numSamples);

通过迭代生成的轨迹点 q，可视化机器人执行轨迹。

figure
set(gcf,"Visible","on");
rc = rateControl(sampleRate);
for i = 1:numSamplesshow(robot,q(:,i),FastUpdate=true,PreservePlot=false);waitfor(rc);
end

要检查不同的关节位置和速度，请绘制所有维度与时间的对比图。使用 helperPlotJointSpaceTraj 辅助函数绘制关节空间轨迹及其航点。

helperPlotJointSpaceTraj("Joint-Space Trajectory and Waypoints", ...tvec,q,qd,frankaWaypoints,frankaTimepoints);

1.2 任务空间轨迹

任务空间轨迹包含代表三维空间中末端执行器运动的航点。生成最小抖动轨迹来连接自由空间中的航点。该轨迹轮廓的目的是创建一个平滑的轨迹，尽量减少生涩的运动。更多信息，请参阅 "最小抖动轨迹"。然后使用逆运动学生成弗兰卡-艾米卡熊猫机器人的关节配置。

首先，创建一组航点，然后使用 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) % Seed the RNG so the inverse kinematics solution is consistent
ik = inverseKinematics(RigidBodyTree=robot);
ik.SolverParameters.AllowRandomRestart = false;
q = zeros(9,numSamples);
weights = [0.2 0.2 0.2 1 1 1]; % Prioritize position over orientation
initialGuess = [0, 0, 0, -pi/2, 0, 0, 0, 0.01, 0.01]'; % Choose an initial guess within the robot joint limits
for i = 1:size(pos,2)targetPose = trvec2tform(pos(:,i)')*eul2tform([0, 0, pi]);q(:,i) = ik("panda_hand",targetPose,weights,initialGuess);initialGuess = q(:,i); % Use the last result as the next initial guess
end

使用机器人模型展示结果。

figure
set(gcf,"Visible","on")
show(robot);

rc = rateControl(sampleRate);
for i = 1:numSamplesshow(robot, q(:,i),FastUpdate=true,PreservePlot=false);waitfor(rc);
end

要检查不同的关节位置和速度，可以通过绘制所有维度与时间的对比图来直观地显示这些结果。使用 helperPlotTaskSpaceTraj 辅助函数绘制任务空间轨迹及其航点。

helperPlotTaskSpaceTraj("Task-Space Trajectory and Waypoints", ...tvec,pos,vel,frankaSpaceWaypoints,frankaTimepoints);

二、比较各种轨迹剖面图

使用不同工具生成轨迹，然后使用任务空间和关节空间可视化对它们进行比较。

wpts = [0 45 15 90 45; 90 45 -45 15 90];
tpts = 0:(size(wpts,2)-1);% Derived quantities.
sampleRate = 20;
tvec = tpts(1):1/sampleRate:tpts(end);
numSamples = length(tvec);

2.1 最小运行轨迹

minjerkpolytraj 函数使用平滑、连续的运动来连接航点。在默认边界条件下，轨迹的初速度和最终速度均为零，但以连续的速度通过所有中间航点。

最小力矩轨迹之所以被命名为最小力矩轨迹，是因为它能最大限度地减小力矩（即运动的三次时间导数），从而产生平滑的轮廓，便于机械系统使用。基本的最小颠簸轨迹是一种在指定时间点到达航点的解析解。

[q,qd,~,~,~,~,tvec] = minjerkpolytraj(wpts,tpts,numSamples);
helperPlotTaskSpaceTraj("Minimum-Jerk Trajectory",tvec,q,qd,wpts,tpts);

2.2 梯形速度曲线轨迹

梯形速度曲线会在每个航点停止，并确保点到点的平滑运动。梯形速度曲线的名称来自于连接两个航点的每个线段的三个阶段：

从零速度加速到峰值速度
在峰值速度下保持恒速
减速至零速度

这样，每个航段的速度剖面都是一个梯形。每个分段都有结束时间、峰值速度、峰值加速度和加速时间参数，但指定任意两个参数就足以完全定义运动。更多信息，请参阅 trapveltraj。

当目标是连接一组航点时，可以使用基本梯形剖面，沿途在每个航点停留。例如，这段代码使用 1 秒钟的分段来连接航点。

[q,qd,~,t] = trapveltraj(wpts,100);
helperPlotTaskSpaceTraj("Trapezoidal Profile, Max Velocity = 0.5",t,q,qd,wpts);

梯形剖面通常需要满足某些约束条件，如速度或加速度边界。由于梯形剖面是一种精确的规范，因此需要使用辅助函数将约束边界转换为精确的剖面规范。helperProfileForMaxVel 辅助函数接受速度边界。有关使用 trapveltraj 函数设计速度剖面的更多信息，请参阅使用梯形速度剖面设计带速度限制的轨迹示例。

[endTimes,peakVels] = helperProfileForMaxVel(wpts, 0.5);
[q,qd,~,t] = trapveltraj(wpts,100,EndTime=endTimes,PeakVelocity=peakVels);% The time at which the waypoints are hit is the vector of cumulative sums
% of the end times
trapVelTrajTime = [0 cumsum(endTimes(1,:))];
helperPlotTaskSpaceTraj("Trapezoidal Profile, Max Velocity = 0.5",t,q,qd,wpts,trapVelTrajTime);

2.3 三次和五次多项式轨迹

某些使用情况可能需要更通用的多项式轨迹。三次多项式轨迹（cubicpolytraj）和五次多项式轨迹（quinticpolytraj）函数是创建插值分段多项式的通用工具。与之前的轨迹工具一样，它们也会返回位置、速度和加速度以及分段多项式对象。

在默认情况下，这些函数使用零值边界条件，因此轨迹在每个航点都会停止。

[q,qd] = cubicpolytraj(wpts,tpts,tvec);
helperPlotTaskSpaceTraj("Basic Cubic Polynomial",tvec,q,qd,wpts,tpts);

您可以使用这些函数设计具有自定义边界行为的内插多项式。例如，将这些函数与 spline、pchip 或 makima 等其他分段多项式工具一起使用，就可以在航点处创建具有理想运动的平滑剖面。

本示例代码使用 MATLAB 中的一个内置分段多项式函数导出边界条件。选择一个多项式函数，查看它对三次多项式速度剖面的影响。代码使用辅助文件计算速度。

smoothPP = spline(tpts,wpts);
smoothVelPP = mkpp(smoothPP.breaks,robotics.core.internal.polyCoeffsDerivative(smoothPP.coefs),size(wpts,1));
smoothVelPoly = ppval(smoothVelPP,tpts);

使用导出的速度作为内部航点的边界条件。将外部边界条件保留为零速度，以确保轨迹以零速度开始和结束

boundaryVel = zeros(size(smoothVelPoly));
boundaryVel(:,2:end-1) = smoothVelPoly(:,2:end-1);
[q,qd] = cubicpolytraj(wpts,tpts,tvec,VelocityBoundaryCondition=boundaryVel);
helperPlotTaskSpaceTraj("Cubic Polynomial with Custom Velocity BCs",tvec,q,qd,wpts,tpts);

使用五次多项式可以确保速度曲线的平滑性。

[q,qd] = quinticpolytraj(wpts,tpts,tvec,VelocityBoundaryCondition=boundaryVel);
helperPlotTaskSpaceTraj("Quintic Polynomial with Custom Velocity BCs",tvec,q,qd,wpts,tpts);

2.4 B 样条轨迹

B 样条多项式能产生平滑、连续的运动，主要用于任务空间应用。使用 bsplinepolytraj 函数可以创建这种轨迹。与其他轨迹不同的是，B-样条曲线是由一组控制点插值定义的。生成的轨迹只命中初始控制点和最终控制点，但会落在整组控制点的凸壳中。

应用之前使用过的航点作为控制点。

[q,qd] = bsplinepolytraj(wpts,tpts([1 end]),tvec);
helperPlotTaskSpaceTraj("Non-interpolating B-spline",tvec,q,qd,wpts);

您可以通过从原始航点推导出一组新的控制点来创建内插 B-样条曲线，B-样条曲线多项式将这些控制点与原始航点进行内插。与标准 B-spline 一样，该多项式主要用于任务空间应用。

使用 helperCreateControlPointsFromWaypoints 辅助函数生成新的控制点。

cpts = helperCreateControlPointsFromWaypoints(wpts);
[q,qd] = bsplinepolytraj(cpts,tpts([1 end]),tvec);
helperPlotTaskSpaceTraj("Interpolating B-spline",tvec,q,qd,wpts);

三、任务空间轨迹的更多工具

有些任务空间应用不仅需要计算插值位置的轨迹，还需要计算操纵器的完整姿态。在这种情况下，可以考虑使用 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 查找轨迹中的平移数据。

figure
rotations = tform2quat(tfInterp);
translations = tform2trvec(tfInterp);
plotTransforms(translations,rotations)
title("Interpolated Transformation Trajectory")
xlabel("X")
ylabel("Y")
zlabel("Z")

三维图显示，旋转和运动都是线性的。回想一下，可行的机械手运动轨迹应该是平滑且连续的。由于插值是线性的，因此并不能保证运动轨迹是平滑的。如果分别绘制位置和速度曲线，这一点就会更加清晰。

figure
tiles1 = tiledlayout(3,1)

tiles1 = TiledChartLayout with properties:TileArrangement: 'fixed'GridSize: [3 1]Padding: 'loose'TileSpacing: 'loose'Use GET to show all properties

positions = reshape(tfInterp(1:3,4,:),3,size(tfInterp,3));
nexttile
plot(tvec,positions)
title("XYZ Position in Time")
ylim("padded")
nexttile
plot(tvec,v1(1:3,:))
title("Velocity in Time")
ylim("padded")
nexttile
plot(tvec,v1(4:6,:)); 
title("Angular Velocity in Time")
ylim("padded")

速度以非零值开始和结束，这对机械手来说不是可行的轨迹，会导致运动突然而生涩。

你可以使用 transformtraj 的 TimeScaling 名-值参数来解决这个问题。该参数使用中间参数化 s 来定义轨迹时间，这样 transformtraj 就可以使用 s(t) 作为时间来定义。在本示例中使用的默认情况下，时间缩放是均匀的，因此 s(t)=t 。结果是每个姿势之间都是线性运动。取而代之的是使用最小跃迁轨迹定义的时间缩放：s(t)= minjerkpolytraj(t)。

时间缩放是一组离散的值 $\left[s;{\frac{d}{\mathrm{d}t}}s;{\frac{d^{2}}{\mathrm{d}t^{2}}}s\right]$ ，对定义在区间 s=[0,1] 上的函数 s(t) 进行采样。

% The time scaling is a discrete set of values [s; ds/dt; d^2s/dt^2] that
% are sample the function s(t), defined on the interval s = [0,1]
[s,sd,sdd] = minjerkpolytraj([0 1],tInterval,numel(tvec));
[tfInterp,v1,a1] = transformtraj(T1,T2,tInterval,tvec,TimeScaling=[s; sd; sdd]);

再次绘制插值变换轨迹图，与之前的图进行比较。

figure
rotations = tform2quat(tfInterp);
translations = tform2trvec(tfInterp);
plotTransforms(translations,rotations)
title("Interpolated Transformation Trajectory")
xlabel("X")
ylabel("Y")
zlabel("Z")

figure
tiles2 = tiledlayout(3,1);
positions = reshape(tfInterp(1:3,4,:),3,size(tfInterp,3));
nexttile
plot(tvec,positions)
title("XYZ Position in Time")
nexttile
plot(tvec,v1(1:3,:))
title("Velocity in Time")
nexttile
plot(tvec,v1(4:6,:))
title("Angular Velocity in Time")