Типы траектории для манипуляторов

При использовании траекторий с манипуляторами цель состоит в том, чтобы обеспечить выполнимый результат, удовлетворяющий определенным ограничениям. Например, вы часто хотите траекторию со сглаженными и непрерывными производными, такими как та, которая не требует мгновенной скорости или ускорения. Траектория может также подвергнуться положению, скорости или ускоряющим границам. Существует два общих способа использовать траектории на объединенном пробеле и на пробеле задачи.

Объединенные пространственные траектории

Объединенная пространственная траектория обычно содержит набор waypoints нескольких настроек робота, соединенных с помощью сглаженного движения. Этот пример генерирует траекторию с помощью трапециевидного скоростного профиля, который прогрессивно ускоряет каждое соединение к максимальной скорости и замедляет его, когда оно приближается к следующему waypoint. Для получения дополнительной информации смотрите Трапециевидную Траекторию Профиля Скорости.

Сгенерируйте трапециевидную траекторию профиля скорости, чтобы соединить waypoints для робота Emika Panda™ Франки. Во-первых, задайте временной вектор для траекторий и загрузите модель робота.

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.

Задайте объединенную пространственную траекторию. Для этой траектории waypoints являются домашней настройкой для модели и двумя случайными настройками.

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 функция помощника, чтобы построить объединенную пространственную траекторию и ее waypoints.

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

Пространственные траектории задачи

Пространственная траектория задачи содержит waypoints, которые представляют движение исполнительного элемента конца в трехмерном пространстве. Сгенерируйте траекторию минимального толчка, чтобы соединить waypoints в свободном пространстве. Цель этого профиля траектории состоит в том, чтобы создать сглаженную траекторию с минимальным судорожным движением. Для получения дополнительной информации смотрите Траекторию Минимального Толчка. Затем сгенерируйте объединенные настройки робота Panda Emika Франки с помощью инверсной кинематики.

Во-первых, создайте набор waypoints, и затем создайте траекторию минимального толчка с помощью minjerkpolytraj.

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

Используйте inverseKinematics функция, чтобы создать решатель инверсной кинематики и решить для настроек, которые достигают желаемых положений исполнительного элемента конца по траектории.

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

Warning: The provided robot configuration violates the predefined joint limits.

Покажите результаты с помощью модели робота.

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 функция помощника, чтобы построить пространственную траекторию задачи и ее waypoints.

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

Сравните различные профили траектории

Траектории Gemerate с помощью различных инструментов, и затем сравнивают их использующий и задачу - и визуализацию объединенного пробела.

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

Траектория минимального толчка

minjerkpolytraj функционируйте соединяет waypoints использование сглаженного, непрерывного движения. С граничными условиями по умолчанию траектория имеет нулевую начальную и итоговую скорость, но проходит через все промежуточное звено waypoints с непрерывной скоростью.

Траектории минимального толчка называют как таковыми, потому что они минимизируют толчок, третью производную времени движения, приводящего к сглаженному профилю, который удобен для механических систем. Основная траектория минимального толчка является аналитическим решением, которое поражает waypoints в точках требуемого времени.

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

Трапециевидная траектория профиля скорости

Трапециевидный скоростной профиль останавливается в каждом waypoint и гарантирует сглаженное движение "точка-точка". Название профиля происходит от трех фаз каждого сегмента, который соединяет два waypoints:

Это приводит к скоростному профилю, который является трапецоидом по каждому сегменту. Каждый сегмент характеризуется временем окончания, пиковой скоростью, пиковым ускорением и ускоряющими параметрами времени, но specifiying любые два достаточен, чтобы полностью задать движение. Для получения дополнительной информации смотрите trapveltraj.

Можно использовать основной трапециевидный профиль, когда цель состоит в том, чтобы соединить набор waypoints, останавливающегося в каждом по пути. Например, этот код соединяет waypoints использование 1 второго сегмента.

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

Трапециевидные профили часто стремятся удовлетворить определенным ограничениям, таким как ускоряющие границы или скорость. Поскольку трапециевидный профиль является точной спецификацией, используйте функцию помощника, чтобы перевести ограничительные границы в точные уточнения профиля. helperProfileForMaxVel функция помощника принимает скоростные границы. Для получения дополнительной информации об использовании trapveltraj функционируйте, чтобы спроектировать скоростные профили, видеть Траекторию Проекта со Скоростными Пределами Используя Трапециевидную Скорость Profileexample.

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

Кубические и полиномиальные траектории Quintic

Некоторые варианты использования могут потребовать более общей полиномиальной траектории. cubicpolytraj и quinticpolytraj функции являются общими средствами для создания кусочных полиномов интерполяции. Как предыдущие инструменты траектории, они возвращают положение, скорость, и ускорение, а также объект кусочного полинома.

В случае по умолчанию эти функции используют граничные условия с нулевым знаком, приводящие к траекториям, которые останавливаются в каждом waypoint.

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

Можно использовать эти функции, чтобы спроектировать интерполяционный многочлен с пользовательским поведением на границе. Например, при помощи этих функций вместе с другими инструментами кусочного полинома как splinepchip, или makima, можно создать сглаженные профили с желательным движением в waypoints.

Этот пример кода выводит граничные условия из одной из встроенных функций кусочного полинома в MATLAB. Выберите полиномиальную функцию, чтобы видеть, как она влияет на скоростной профиль кубического полинома. Код использует файл помощника, чтобы вычислить скорость.

smoothPP = spline(tpts, wpts);
smoothVelPP = mkpp (smoothPP.breaks, robotics.core.internal.polyCoeffsDerivative (smoothPP.coefs), размер (wpts, 1));
smoothVelPoly = ppval (smoothVelPP, tpts);

Используйте выведенную скорость в качестве граничного условия для внутреннего waypoints. Оставьте внешние граничные условия как нулевую скорость, чтобы гарантировать начала и концы траектории при нулевой скорости

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

Используя quintic полином вместо этого гарантирует сглаженный скоростной профиль.

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

Траектория B-сплайна

Полином B-сплайна приводит к сглаженному, непрерывному движению и преимущественно используется для применения космической техники задачи. Можно создать эту траекторию при помощи bsplinepolytraj функция. В отличие от других траекторий, которые интерполируют waypoints, которые задают их, B-сплайн задан набором контрольных точек. Получившаяся траектория поражает только начальные и итоговые контрольные точки, но падает в выпуклой оболочке полного набора контрольных точек.

Примените ранее используемый waypoints как контрольные точки.

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

Можно создать B-сплайн интерполяции путем получения нового набора контрольных точек от исходного waypoints, который полином B-сплайна интерполирует с исходным waypoints. Как стандартный B-сплайн, используйте этот полином, в основном, для применения космической техники задачи.

Используйте 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')

3-D график показывает, что вращения и движение линейны. Вспомните, что выполнимые траектории манипулятора должны явиться гладкими и быть непрерывными. Поскольку интерполяция линейна, траектория, как гарантируют, не приведет к сглаженному движению. Это становится более ясным, когда вы строите положение и скорости отдельно.

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

Скоростное начало и конец в ненулевых значениях, которое не является выполнимой траекторией для манипулятора и вызвало бы внезапное и судорожное движение.

Можно использовать TimeScaling аргумент значения имени transformtraj как обходное решение. Этот аргумент задает время траектории с помощью промежуточной параметризации, $\mathit{s}$, таким образом, что transformtraj задан с помощью $\mathit{s}\left(\mathit{t}\right)$ как время. В случае по умолчанию, используемом в этом примере, время, масштабируясь универсально, таким образом, $\mathit{s}\left(\mathit{t}\right)=\mathit{t}$. Результатом является линейное движение между каждым положением. Вместо этого используйте масштабирование времени, заданное траекторией минимального толчка: $\mathit{s}\left(\mathit{t}\right)=$ minjerkpolytraj$\left(\mathit{t}\right)$.

Время масштабируясь является дискретным набором значений,$\left[\mathit{s};\frac{d}{\mathrm{d}\mathit{t}}\mathit{s};\frac{d^2}{\mathrm{d}{\mathit{t}}^2}\mathit{s}\right]$, которые производят функцию $\mathit{s}\left(\mathit{t}\right)$, заданный на интервале $\mathit{s}=\left[0,1\right]$.

% 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
positions = reshape(tfInterp(1:3,4,:),3,size(tfInterp,3));
subplot(3,1,1); plot(tvec,positions)
title('XYZ Position in Time')
subplot(3,1,2); plot(tvec,v1(1:3,:))
title('Velocity in Time')
subplot(3,1,3); plot(tvec,v1(4:6,:))
title('Angular Velocity in Time')

В то время как движение на пробеле следует за тем же путем, второй набор графиков разъясняет, что скорости являются гладкими и followable вовремя, приводя к траектории, которая выполнима для манипулятора или другой механической системы следовать.