机器人轨迹生成

对于机械臂的运动、规划和控制应用，必须选择机器人要遵循的轨迹。

有三个主要部分：第一部分展示了机械手使用的轨迹类型，第二部分展示了生成轨迹的函数，最后一部分展示了更多的轨迹规划工具。

机械手的轨迹类型：

当使用机械手的轨迹时，目标是提供一个受一定约束的可行结果。例如，您经常想要一个具有平滑和连续导数的轨迹，例如一个不需要瞬时速度或加速度的轨迹。轨迹也可能受到位置、速度或加速度界限的影响。在关节空间和任务空间中有两种使用轨迹的方法。

关节空间轨迹：

关节空间轨迹通常包含一组使用平滑运动连接的多个机器人构型的路径点。本例使用梯形速度剖面生成一个轨迹，它逐渐加速每个关节，使其达到最大速度，并在它接近下一个路径点时使其减速。有关更多信息，请参见梯形速度剖面轨迹。

生成一个梯形速度剖面轨迹来连接机器人的路径点。首先，定义轨迹的时间矢量，并对机器人模型进行加载。

tpts = 0:4;
sampleRate = 20;
tvec = tpts(1):1/sampleRate:tpts(end);
numSamples = length(tvec);

robot = loadrobot('frankaEmikaPanda',DataFormat='column');

Warning: Inertia of all the rigid bodies in this model were missing in the source. Inertia property for each body is set to zero.

定义关节空间轨迹。对于这个轨迹，路径点是模型的home配置和两个随机配置。

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:numSamples
    show(robot,q(:,i),FastUpdate=true,PreservePlot=false);
    waitfor(rc);
end

 为了检查不同的关节位置和速度，绘制所有尺寸与时间的关系。

使用helperPlotJointSpaceTraj helper函数来绘制关节空间轨迹及其路径点。

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

 任务空间轨迹

一个任务空间轨迹包含了在三维空间中表示末端执行器运动的路径点。生成一个在自由空间中连接路点的最小加速度轨迹。这种轨迹轮廓的目的是创建一个平稳的轨迹与最小的突变运动。要了解更多信息，可参考最小加速度轨迹。然后利用逆运动学方法生成机器人的关节构型。

首先，创建一组路点，然后使用minjerkpolytraj创建最小jerk轨迹。

frankaSpaceWaypoints = [0 0.1 0.5; 0 -0.3 0.2; 0.5 -0.25 0; -0.3 0 0.2]';
frankaTimepoints = linspace(tvec(1),tvec(end),4);
[pos,vel] = minjerkpolytraj(frankaSpaceWaypoints,frankaTimepoints,numSamples);

使用inverse运动学函数来创建一个逆运动学求解器，并求解在轨迹上达到理想末端执行器位置的构型。

ik = inverseKinematics(RigidBodyTree=robot);
ik.SolverParameters.AllowRandomRestart = false;
q = zeros(9,numSamples);
weights = [0 0 0 1 1 1]; % Ignore rotation
initialGuess = robot.homeConfiguration;
for i = 1:size(pos,2)
    rng(0);
    q(:,i) = ik('panda_hand',trvec2tform(pos(:,i)'),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:numSamples
    show(robot, q(:,i),FastUpdate=true,PreservePlot=false);
    waitfor(rc);
end

 为了检查不同的关节位置和速度，通过绘制所有尺寸与时间的关系来可视化这些结果。使用helperPlotTaskSpaceTraj helper函数来绘制任务空间轨迹及其路径点。

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);

Minimum-Jerk轨迹

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

最小加速度轨迹之所以这样命名，是因为它们最小化了运动的三阶时间导数——加速度，从而得到了方便机械系统的光滑轮廓。基本的最小加速度轨迹是一个解析解，它在指定的时间点到达航路点。

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

 梯形速度剖面轨迹

梯形速度剖面在每个路径点停止，并确保平稳的点对点运动。概要文件的名称来自于连接两个路点的每个部分的三个阶段:

（1）从零速度到峰值速度的加速度

（2）以峰值速度匀速运动

（3）减速到零速度

这导致速度剖面是一个梯形的每个部分。每个段的特征是结束时间、峰值速度、峰值加速度和加速度时间参数，但指定任意两个就足以充分定义运动。可参考trapveltraj。

当目标是连接一组路点时，你可以使用一个基本的梯形轮廓线，在每个路点停留。