MATLAB - 选择机械臂路径规划、轨迹规划方式（上）+https://developer.aliyun.com/article/1585255
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)。
时间缩放是一组离散的值 
，对定义在区间 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")
虽然空间运动遵循相同的路径，但第二组曲线图表明，速度在时间上是平滑和可跟踪的，从而形成了一个机械手或其他机械系统可以遵循的轨迹。
