В результате предыдущих расчётов мы выбрали размеры дельта-робота, построили его рабочую зону. Теперь настало время выбрать приводы. Привод, или устройство, которое вращает входные звенья (рычаги), имеет две главные характеристики – максимальную частоту вращения и максимальный крутящий момент. Если говорить более обстоятельно, то максимальный крутящий момент зависит ещё и от частоты вращения и нам стоит подбирать привод так, чтобы его момент был во всём диапазоне скоростей больше того, который может возникнуть в разрабатываемом роботе. Помимо этого, у привода есть параметры, характеризующие его точность. Основной упор сегодня будет на нахождение максимальной частоты вращения. Как всегда, дам все алгоритмы, реализованные в MATLAB.
Список обозначений
Теперь я не буду дублировать содержание этого списка из предыдущих статей, а занесу сюда лишь пояснения новых символов.
– угловые скорости первого, второго и третьего приводов (рычагов) соответственно;
– координаты вектора скорости рабочего органа;
– матрица Якоби;
– обратная матрица Якоби;
– вектор строка обратной матрицы Якоби.
Иначе, задача, в которой мы находим скорость вращения входных звеньев при известных скорости и положении выходного звена, называется задачей о скоростях. Иногда ещё полученные зависимости называют кинематическими характеристиками.
Поиск этих характеристик может понадобиться не только для правильного выбора приводов, но и для синтеза системы управления.
Намеченную задачу мы сегодня решим при помощи аж четырёх подходов.
Первым делом повторим ещё раз, что мы хотим. Мы хотим найти максимальную частоту вращения приводов, которая может возникнуть при движении выходного звена в пределах рабочей зоны с максимальной заданной линейной скоростью. При расчётах считаем, что нам известны текущие координаты подвижной платформы и вектор её скорости. Для меня было немного неожиданным, что скорость входного звена зависит не только от скорости выходного, но и от его положения, но так бывает, наверное, почти всегда, когда мы переходим с декартовой кинематики (портальные роботы), на какую-то иную сложную кинематику (дельта-робот).
Подход 1. Дифференцирование функций положения (ФП)
Ранее мы рассмотрели два варианта аналитического решения обратной кинематической задачи (это и есть функция положения). Воспользуемся вторым из них (формула (13)). Для поиска первой производной по времени запишем все подготовительные вычисления (формулы (9)-(12)) в итоговую формулу (13) и с помощью пакета прикладных программ MATLAB и встроенного в него модуля символьных вычислений найдём первую производную по времени. Ведь слева в этой формуле у нас , а если быть точнее то
. Производная по времени от этой функции
как раз и будет скоростью входного звена.
Скрипт для нахождения производной в символьном виде в MATLAB
%Объявляем символьные переменные
syms Tetta_1(t) X_V(t) Y_V(t) Z_V(t)
syms OQ VM R_l R_r F f X_0 Y_0 Z_0
syms omega_1(t) v_VX(t) v_VY(t) v_VZ(t)
%Записываем функцию порложения, как уравнение
NL = sqrt(R_r^2-X_V^2);
y_M = -VM + Y_V;
y_Q = -OQ;
const_1 = y_M - y_Q;
NQ = sqrt(const_1^2 + Z_V^2);
Eq1 = Tetta_1 == 2*pi - acos((R_l^2 + NQ^2 - NL^2)/(2*R_l*NQ)) - acos(const_1/NQ);
%Дифференцируем его
Eq2 = diff(Eq1, t);
%Подставляем вместо производных координат по времени скорости
Eq2 = subs(Eq2, [diff(Tetta_1(t), t), diff(X_V(t), t), diff(Y_V(t), t), diff(Z_V(t), t)], [omega_1 v_VX v_VY v_VZ ]);
%"Собираем" уравнение вокруг v_VX, v_VY и v_VZ
col2 = collect(Eq2, [v_VX v_VY v_VZ])Я рекомендую для запуска это кода использовать 
. Тогда после запуска вы сразу увидите это:
В последних версиях MATLAB появилась возможность даже копировать это в Word, что экономит кучу времени при наборе формул.
Как итог, получим формулу:
где 
– угловая скорость первого рычага;
– составляющие скорости точки V, направленные вдоль осей X, Y, Z соответственно;
– функции, зависящие от 
.
Функции 
определяют следующие формулы:
где
Код функции, для нахождения угловых скоростей дифференцированием ФП
%Находит решение задачи о скоростях методом дифференцирования функций пооложения
function [w_1, w_2, w_3] = OZK_v(X_V, Y_V, Z_V, v_VX, v_VY, v_VZ)
%Расчёт координат точки V в системах координат, повёрнутых на 120° и 240°
%по часовой стрелке относительно основной
global R_l R_r cos120 sin120 cos240 sin240 VM OQ
X_V_120 = X_V*cos120 - Y_V*sin120;
Y_V_120 = X_V*sin120 + Y_V*cos120;
X_V_240 = X_V*cos240 - Y_V*sin240;
Y_V_240 = X_V*sin240 + Y_V*cos240;
%Расчёт векторов скоростей точки V в системах координат, повёрнутых на 120° и 240°
%по часовой стрелке относительно основной
v_VX_120 = v_VX*cos120 - v_VY*sin120;
v_VY_120 = v_VX*sin120 + v_VY*cos120;
v_VX_240 = v_VX*cos240 - v_VY*sin240;
v_VY_240 = X_V*sin240 + v_VY*cos240;
Z_V_120 = Z_V;
Z_V_240 = Z_V;
v_VZ_120 = v_VZ;
v_VZ_240 = v_VZ;
%Расчёт углов поворота рычагов в соответствующих системах координат
w_1 = (v_VY/(Z_V^2 + (OQ - VM + Y_V)^2)^(1/2) - ((2*v_VY*(OQ - VM + Y_V) + 2*v_VZ*Z_V)*(OQ - VM + Y_V))/(2*(Z_V^2 + (OQ - VM + Y_V)^2)^(3/2)))/(1 - (OQ - VM + Y_V)^2/(Z_V^2 + (OQ - VM + Y_V)^2))^(1/2) + ((2*v_VY*(OQ - VM + Y_V) + 2*v_VX*X_V + 2*v_VZ*Z_V)/(2*R_l*(Z_V^2 + (OQ - VM + Y_V)^2)^(1/2)) - ((2*v_VY*(OQ - VM + Y_V) + 2*v_VZ*Z_V)*(X_V^2 + Z_V^2 + (OQ - VM + Y_V)^2 + R_l^2 - R_r^2))/(4*R_l*(Z_V^2 + (OQ - VM + Y_V)^2)^(3/2)))/(1 - (X_V^2 + Z_V^2 + (OQ - VM + Y_V)^2 + R_l^2 - R_r^2)^2/(4*R_l^2*(Z_V^2 + (OQ - VM + Y_V)^2)))^(1/2);
w_2 = (v_VY_120/(Z_V_120^2 + (OQ - VM + Y_V_120)^2)^(1/2) - ((2*v_VY_120*(OQ - VM + Y_V_120) + 2*v_VZ_120*Z_V_120)*(OQ - VM + Y_V_120))/(2*(Z_V_120^2 + (OQ - VM + Y_V_120)^2)^(3/2)))/(1 - (OQ - VM + Y_V_120)^2/(Z_V_120^2 + (OQ - VM + Y_V_120)^2))^(1/2) + ((2*v_VY_120*(OQ - VM + Y_V_120) + 2*v_VX_120*X_V_120 + 2*v_VZ_120*Z_V_120)/(2*R_l*(Z_V_120^2 + (OQ - VM + Y_V_120)^2)^(1/2)) - ((2*v_VY_120*(OQ - VM + Y_V_120) + 2*v_VZ_120*Z_V_120)*(X_V_120^2 + Z_V_120^2 + (OQ - VM + Y_V_120)^2 + R_l^2 - R_r^2))/(4*R_l*(Z_V_120^2 + (OQ - VM + Y_V_120)^2)^(3/2)))/(1 - (X_V_120^2 + Z_V_120^2 + (OQ - VM + Y_V_120)^2 + R_l^2 - R_r^2)^2/(4*R_l^2*(Z_V_120^2 + (OQ - VM + Y_V_120)^2)))^(1/2);
w_3 = (v_VY_240/(Z_V_240^2 + (OQ - VM + Y_V_240)^2)^(1/2) - ((2*v_VY_240*(OQ - VM + Y_V_240) + 2*v_VZ_240*Z_V_240)*(OQ - VM + Y_V_240))/(2*(Z_V_240^2 + (OQ - VM + Y_V_240)^2)^(3/2)))/(1 - (OQ - VM + Y_V_240)^2/(Z_V_240^2 + (OQ - VM + Y_V_240)^2))^(1/2) + ((2*v_VY_240*(OQ - VM + Y_V_240) + 2*v_VX_240*X_V_240 + 2*v_VZ_240*Z_V_240)/(2*R_l*(Z_V_240^2 + (OQ - VM + Y_V_240)^2)^(1/2)) - ((2*v_VY_240*(OQ - VM + Y_V_240) + 2*v_VZ_240*Z_V_240)*(X_V_240^2 + Z_V_240^2 + (OQ - VM + Y_V_240)^2 + R_l^2 - R_r^2))/(4*R_l*(Z_V_240^2 + (OQ - VM + Y_V_240)^2)^(3/2)))/(1 - (X_V_240^2 + Z_V_240^2 + (OQ - VM + Y_V_240)^2 + R_l^2 - R_r^2)^2/(4*R_l^2*(Z_V_240^2 + (OQ - VM + Y_V_240)^2)))^(1/2);
endГромоздко, не правда ли? На самом деле ещё нет. Вот если мы ещё раз продифференцируем, чтобы найти угловое ускорение, вот тогда будет по-настоящему громоздко. Но всё же, можно ли формулы покороче получить?
Подход 2. Метод планов скоростей
В умных книжках ???? по теории механизмов и машин всегда рассказывают о методе планов скоростей. По идее, это графический метод решения нужной нам задачи, но считать то мы будем всё равно в MATLAB-е. Поэтому я бы не сказал, что он менее точный.
Рассмотрим его. Вообще, с этим методом лучше знакомится на примере плоской задачи. Но я расскажу сразу на примере пространственного механизма – дельта-робота, подразумевая, что вы с ним заранее ознакомились. Начнём.
Пугаться этой картинки не стоит. Сейчас я поясню всё, что на ней изображено.
Первым шагом введём новую систему координат X'Y'Z' с началом координат .
– это полюс скоростей, если говорить "книжно". Систему координат можно располагать как угодно (например, как и основную), но мне показалась удобным именно так.
В этой системе координат необходимо построить вектор скорости подвижной платформы. Нам он известен по условию. Вектор найдем как сумму векторов его компонент по осям X, Y, и Z. Не забудем перевести эти компоненты в новую систему координат. Этот вектор будет совпадать с векторами скоростей точек V и M, так как это точки одного звена (платформы). Таким образом мы получим вектор 
.
Скорость точки L, будет направлена по касательной к окружности с центром в точке Q. То есть скорость точки L будет направлена перпендикулярно звену QL и, как и это звено, всегда будет лежать в плоскости . Направление этого вектора мы зададим прямой
, а саму прямую легко можем построить, так как знаем координаты точек L и Q.
Данный подход предполагает, что мы сначала решаем обратную задачу кинематики и, благодаря полученному решению, можем найти координаты точек Q, L и M.
Вектор 
начинается из полюса скоростей, то есть как бы из неподвижной точки. Вектор 
тоже будет начинаться как бы из неподвижной точки, то есть полюса скоростей. Вектор 
– это вектор скорости точки L относительно точки M. И его начало уже неподвижным считать нельзя. Он должен начинаться из конца вектора 
.
Мы знаем, что точка L относительно точки M будет двигаться в направлении перпендикулярном звену LM (по касательной к сфере с центром в точке M и радиусом LM). Тогда мы можем построить плоскость , которая будет перпендикулярна звену LM (или прямой
) и проходить через точку M' на плане скоростей. Пересечение этой плоскости с прямой
определит координаты точки L' на плане скоростей, что позволит найти вектор
. Зная координаты 
вектора 
мы можем найти его длину по формуле:
А зная длину этого вектора, то есть линейную скорость, легко найдём угловую скорость звена QL по формуле:
Таким образом, чтобы найти угловую скорость первого рычага, нужно найти координаты 
, как пресечение плоскости и прямой
.
Уравнение плоскости, перпендикулярной прямой (прямой в нашем случае) имеет вид:
где 
– координаты точки, через которую проходит плоскость;
– координаты направляющего вектора.
на плане скоростей – это и есть координаты вектора 
. Координаты направляющего вектора найдём путём вычитания координат точки L из координат точки M. Тогда уравнение плоскости в введённой нами системе координат примет вид:
Далее запишем уравнение прямой :
Помимо этого, мы знаем, что точка пересечения плоскости и прямой лежит в плоскости
:
Тогда решение системы из этих трёх уравнений (6, 7, 8) и будет искомой точкой, а и
будут координатами
и 
.
Я слишком стар, чтобы подставить второе и третье уравнение в первое и выразить самостоятельно. Поэтому даже здесь я заставил работать MATLAB:
syms X_Q1 Y_Q1 Z_Q1 X_L1 Y_L1 Z_L1 X_M1 Y_M1 Z_M1
syms x y z
syms v_VX v_VY v_VZ
y = (Y_Q1-Y_L1)/(Z_Q1-Z_L1)*x;
z = 0;
Eq3 = (-Y_L1+Y_M1)*(x+v_VY)+(Z_L1-Z_M1)*((Y_Q1-Y_L1)/(Z_Q1-Z_L1)*x-v_VZ)...
+(-X_L1+X_M1)*(0+v_VX) == 0
solve(Eq3, x) А он мне выдал:
Стоит отметить ещё необходимость проверки направления получившегося вектора, так как последнее влияет на знак угловой скорости . Благодаря тому, что мы заранее ограничиваем угол поворота рычага от 90° до 270°, условие проверки упрощается до проверки знака
. Если
, то знак у
-, иначе +.
Код функции, для нахождения угловых скоростей методом планов
%Находит решение задачи о скоростях методом планов
function [w_1, w_2, w_3] = OZK_v_pv(X_V, Y_V, Z_V, v_VX, v_VY, v_VZ)
global R_l VM OQ cos120 sin120 cos240 sin240 %Размеры и константы
%Расчёт углов поворота рычагов в соответствующих системах координат
[vectTheta] = OZK(X_V, Y_V, Z_V);
Theta1 = vectTheta(1); Theta2 = vectTheta(2); Theta3 = vectTheta(3);
%Вычисляем координаты точек в системах координат XOY,
%X120Y120Z120 и X240Y240Z240
Y_Q1 = -OQ;
Z_Q1 = 0;
X_L1 = 0;
Y_L1 = -(OQ + R_l*sind(Theta1-90));
Z_L1 = R_l*cosd(Theta1-90);
X_M1 = X_V;
Y_M1 = Y_V - VM;
Z_M1 = Z_V;
X_V_120 = X_V*cos120 - Y_V*sin120;
Y_V_120 = X_V*sin120 + Y_V*cos120;
Z_V_120 = Z_V;
Y_Q2_120 = -OQ;
Z_Q2_120 = 0;
X_L2_120 = 0;
Y_L2_120 = -(OQ + R_l*sind(Theta2-90));
Z_L2_120 = R_l*cosd(Theta2-90);
X_M2_120 = X_V_120;
Y_M2_120 = Y_V_120 - VM;
Z_M2_120 = Z_V_120;
X_V_240 = X_V*cos240 - Y_V*sin240;
Y_V_240 = X_V*sin240 + Y_V*cos240;
Z_V_240 = Z_V;
Y_Q3_240 = -OQ;
Z_Q3_240 = 0;
X_L3_240 = 0;
Y_L3_240 = -(OQ + R_l*sind(Theta3-90));
Z_L3_240 = R_l*cosd(Theta3-90);
X_M3_240 = X_V_240;
Y_M3_240 = Y_V_240 - VM;
Z_M3_240 = Z_V_240;
v_VX_120 = v_VX*cos120 - v_VY*sin120;
v_VY_120 = v_VX*sin120 + v_VY*cos120;
v_VZ_120 = v_VZ;
v_VX_240 = v_VX*cos240 - v_VY*sin240;
v_VY_240 = v_VX*sin240 + v_VY*cos240;
v_VZ_240 = v_VZ;
x = (v_VX*(X_L1 - X_M1) + v_VY*(Y_L1 - Y_M1) + v_VZ*(Z_L1 - Z_M1))/(Y_M1 - Y_L1 + ((Y_L1 - Y_Q1)*(Z_L1 - Z_M1))/(Z_L1 - Z_Q1));
y = (Y_Q1-Y_L1)/(Z_Q1-Z_L1)*x;
v_L1 = sqrt(x^2+y^2);
if y>0
znak = -1;
else
znak = 1;
end
w_1 = znak*v_L1/R_l;
x = (v_VX_120*(X_L2_120 - X_M2_120) + v_VY_120*(Y_L2_120 - Y_M2_120) + v_VZ_120*(Z_L2_120 - Z_M2_120))/(Y_M2_120 - Y_L2_120 + ((Y_L2_120 - Y_Q2_120)*(Z_L2_120 - Z_M2_120))/(Z_L2_120 - Z_Q2_120));
y = (Y_Q2_120-Y_L2_120)/(Z_Q2_120-Z_L2_120)*x;
v_L2_120 = sqrt(x^2+y^2);
if y>0
znak = -1;
else
znak = 1;
end
w_2 = znak*v_L2_120/R_l;
x = (v_VX_240*(X_L3_240 - X_M3_240) + v_VY_240*(Y_L3_240 - Y_M3_240) + v_VZ_240*(Z_L3_240 - Z_M3_240))/(Y_M3_240 - Y_L3_240 + ((Y_L3_240 - Y_Q3_240)*(Z_L3_240 - Z_M3_240))/(Z_L3_240 - Z_Q3_240));
y = (Y_Q3_240-Y_L3_240)/(Z_Q3_240-Z_L3_240)*x;
v_L3_240 = sqrt(x^2+y^2);
if y>0
znak = -1;
else
znak = 1;
end
w_3 = znak*v_L3_240/R_l;
endПодход 3. Матричный метод
А вот и настало время для зловещего матричного метода. В статьях, посвящённых дельта-роботу часто можно встретить матричное исчисление. Попробуем и мы применить его, а также скажем, чем этот метод хорош.
По ходу повествования я буду опираться на вот эти ????, ????, ???? источники. С автором первого источника я лично знаком и походу написания этого раздела активно закидывал его вопросами, чтобы совсем уж дичь тут вам не писать.
Первое, что следует сделать – следует записать уравнения связи. Уравнение связи (первое) для дельта-робота это не что иное, как функция положения (формула (13) этой работы), у которой после знака = мы оставим 0, а потом вместо 0 запишем :
Или, что тоже самое:
Заметим, что последнее уравнение записано не в явном виде, то есть 
не выражено, через координаты рабочего органа (если мы снова уберём ). Это первое преимущество метода – можно написать более простую неявную функцию.
Далее, используя эти уравнения связи можно составить матрицы:
Элементы матриц представляют собой не что иное, как частные производные функций положения по входным или выходным координатам.
Далее мы можем записать следующее матричное уравнение:
Решая его относительно вектора скоростей приводов, получим:
– это, так называемая, матрица Якоби (или Якобиан), то есть матрица, связывающая входные
и выходные
скорости.
Таким образом, знаю эту матрицу, можно легко найти скорости рычагов, зная скорость и положение выходного звена.
Второе преимущество такого подхода заключается в том, что мы можем быстренько переписать это матричное уравнение вот так:
То есть, зная Якобиан, можно решить и прямую задачу о скоростях.
Здесь сразу стоит отметить, что если мы составляем уравнение связи из явных функций, то можно сразу решать матричное уравнение и находить скорости входных звеньев. Если же мы составляем их из неявной функции, то сначала потребуется решить задачу о положениях, так как в матрице у нас будут присутствовать входные координаты, которые мы без решения ОЗК не знаем:
Если уж быть совсем честным, то в конечном счёте, при вычислении угловых скоростей матричным способом мы придём к уравнениям вида (1), о которых говорили в первом подходе. Та что все рассмотренные способы очень близки.
Ну и последним преимуществом матричного метода я бы назвал удобство последующего анализа. То есть математически формализуя таким образом решение, мы легко можем узнать, например, скорость входного звена при наихудшем направлении скорости выходного звена. Об этом будет сказано ниже. Помимо этого, можно достаточно легко найти, так называемые, особые положения и много чего ещё.
Чуть не забыл про алгоритмы.
Скрипт для нахождения обратной матрицы Якоби
%Задаём символьные переменные
syms Tetta_1 Tetta_2 Tetta_3 X_V Y_V Z_V
syms OQ VM R_l R_r F f X_0 Y_0 Z_0
syms omega_1(t) v_VX(t) v_VY(t) v_VZ(t)
syms epsilon_1 a_VX a_VY a_VZ
syms v_VX v_VY v_VZ
%Записываем решение обратной кинематической задачи в аналитическом виде
NL = sqrt(R_r^2-X_V^2);
y_M = -VM + Y_V;
y_Q = -OQ;
const_1 = y_M - y_Q;
NQ = sqrt(const_1^2 + Z_V^2);
Eq1 = Tetta_1 == 2*pi - acos((R_l^2 + NQ^2 - NL^2)/(2*R_l*NQ))...
- acos(const_1/NQ);
%Формируем уравнение связи
F_1 = 2*pi - acos((R_l^2 + NQ^2 - NL^2)/(2*R_l*NQ))...
- acos(const_1/NQ) - Tetta_1;
% F_1 = - (R_l^2 + NQ^2 - NL^2)/(2*R_l*NQ)...
% - const_1/NQ+cos(2*pi - Tetta_1);
%Проделываем те же операции для 2-го плеча
X_V_120 = X_V*cos(2*pi/3) - Y_V*sin(2*pi/3);
Y_V_120 = X_V*sin(2*pi/3) + Y_V*cos(2*pi/3);
Z_V_120 = Z_V;
NL = sqrt(R_r^2-X_V_120^2);
y_M = -VM + Y_V_120;
y_Q = -OQ;
const_1 = y_M - y_Q;
NQ = sqrt(const_1^2 + Z_V_120^2);
Eq2 = Tetta_2 == 2*pi - acos((R_l^2 + NQ^2 - NL^2)/(2*R_l*NQ))...
- acos(const_1/NQ);
F_2 = 2*pi - acos((R_l^2 + NQ^2 - NL^2)/(2*R_l*NQ))...
- acos(const_1/NQ) - Tetta_2;
% F_2 = - (R_l^2 + NQ^2 - NL^2)/(2*R_l*NQ)...
% - const_1/NQ + cos(2*pi - Tetta_2);
%Для 3-го плеча
X_V_240 = X_V*cos(4*pi/3) - Y_V*sin(4*pi/3);
Y_V_240 = X_V*sin(4*pi/3) + Y_V*cos(4*pi/3);
Z_V_240 = Z_V;
NL = sqrt(R_r^2-X_V_240^2);
y_M = -VM + Y_V_240;
y_Q = -OQ;
const_1 = y_M - y_Q;
NQ = sqrt(const_1^2 + Z_V_240^2);
Eq3 = Tetta_3 == 2*pi - acos((R_l^2 + NQ^2 - NL^2)/(2*R_l*NQ))...
- acos(const_1/NQ);
F_3 = 2*pi - acos((R_l^2 + NQ^2 - NL^2)/(2*R_l*NQ))...
- acos(const_1/NQ) - Tetta_3;
% F_3 = - (R_l^2 + NQ^2 - NL^2)/(2*R_l*NQ)...
% - const_1/NQ + cos(2*pi - Tetta_3);
%Составляем матрицы А и В
A = [diff(F_1, X_V), diff(F_1, Y_V), diff(F_1, Z_V);
diff(F_2, X_V), diff(F_2, Y_V), diff(F_2, Z_V);
diff(F_3, X_V), diff(F_3, Y_V), diff(F_3, Z_V)];
B = [diff(F_1, Tetta_1), 0, 0;
0, diff(F_2, Tetta_2), 0;
0, 0, diff(F_3, Tetta_3)];
J = -A*inv(B)
%Подставляем конкретные значения (для проверки)
J_ch = double(subs(J, [X_V, Y_V, Z_V, OQ, VM, R_l, R_r, F, f],...
[70.7107, 0, -325, 77.9423, 31.7543, 170, 320, 270, 110]));
%Вычисляем скорости в тестовой точке
v = [-0.4381; 139.3182; 787.7755];
w = J_ch*v
%Ответ [-3.1503; -3.3611; -4.4766]На эту матрицу в символьном виде смотреть, конечно, страшно:
Стрелочки указывают на полосы прокрутки. Можно представить сколько дополнительных констант MATLAB там нагенерировал.
Проверка. Движение по заданной траектории с максимальной скоростью. Подход 4. Графическое дифференцирование
После формирования алгоритма для нахождения скоростей входных звеньев следует убедиться в правильности работы полученных скриптов. Я решил проверить сразу всё.
Для проверки мы синтезируем траекторию (массив точек), по которым будет (виртуально) перемещаться подвижная платформа с максимальной скоростью. Я выбрал вот такую траекторию:
Далее для каждой точки траектории нужно найти вектор скорости. Направлен он будет к следующей точке.
В результате работы алгоритма хотелось бы получить четыре графика, на которых будут показаны зависимости скорости первого рычага от времени, рассчитываемые всеми четырьмя методами.
Тут появляется время. Вектор времени можно найти, зная, сколько должно занимать перемещение от одной точки до другой с максимальной скоростью. Между точками у нас расстояние постоянно меняется, поэтому тут придётся его считать для всей траектории.
Ну а потом, итоговый вектор времени можно получить используя функцию cumsum, которая создаёт новый вектор, суммируя для каждого значения все предшествующие значения исходного вектора.
В общем, не буду томить. Всё совпало с 101-го раза:
Последний 4-ый подход (на графике он первый) я использовал исключительно для проверки первых трёх.
Его суть заключается в следующем – мы уже умеем решать обратную задачу кинематики. Вот и решим её для каждой точки. То есть найдём зависимость 
.
А теперь просто можно найти производную численно, так как у нас есть вектора (здесь под векторами я подразумеваю массивы) и
:
И так для каждой точки.
Так как мы проверили ОЗК и, наверняка, правильно вычислили вектор значений , то и полученный график тоже должен быть правильным.
Вот рабочий скрипт:
Скрипт, который находит скорость первого рычага всеми методами
clear all
%Входные данные
BOX = [200, 100]; %Ширина и высота рабочей зоны (квадратный параллелепипед)
Z0BOX = -375; %Координата дна рабочей зоны
v_max = 800; %Максимальная требуемая линейная скорость [мм/с]
%Подгружаем константы и итоговые конструктивные размеры
InputConstant;
%Параметры дискретизации пространственных траекторий
Diskr = 1000; %Число точек, в которых вычизляем значение пространственной кривой
%Подготовительные вычисления
R_BOX = BOX(1)*sqrt(2)/2; % Радиус описанного цилиндра рабочей области
%Формируем траекторию для исследования скоростных характеристик
fi1 = linspace(0, 2*pi, Diskr);
fi2 = linspace(0, 8*2*pi, Diskr);
X_V = R_BOX*0.5*cos(fi1);
Y_V = R_BOX*0.5*sin(fi1);
Z_V = Z0BOX + BOX(2)/2 + BOX(2)/2*sin(fi2);
hold on;
plot3(X_V, Y_V, Z_V, 'LineWidth', 1.5);
hold off;
axis equal;
xlabel('x, мм');
ylabel('y, мм');
zlabel('z, мм');
title('Рассматриваемая траектория движения');
grid on;
view([-25, 20]);
%Вычисляем вектор скорости в каждой точке каждой траектории
dX = X_V([2:Diskr, 1])-X_V(1:Diskr);
dY = Y_V([2:Diskr, 1])-Y_V(1:Diskr);
dZ = Z_V([2:Diskr, 1])-Z_V(1:Diskr);
dL = sqrt(dX.^2 + dY.^2 + dZ.^2);
v_VX = dX*v_max./dL;
v_VY = dY*v_max./dL;
v_VZ = dZ*v_max./dL;
%v_vect=sqrt(v_VX.^2 + v_VY.^2 + v_VZ.^2)
%quiver3(X_V, Y_V, Z_V, v_VX*0.07, v_VY*0.07, v_VZ*0.07, 'AutoScale', 'off');
%Задаём вектор скорости рычага 1
w1 = [];
%Вычисляем скорость рычагов в каждой точке траектории
for k=1:Diskr
[w_1, w_2, w_3] = OZK_v(X_V(k), Y_V(k), Z_V(k), v_VX(k), v_VY(k), v_VZ(k));
w1 = [w1, w_1];
end
%Вычисляем вектор времени
dTime = dL/v_max;
Time = cumsum(dTime);
%Вычисляем скорости методом планов
for k=1:Diskr
[Tetta1, Tetta2, Tetta3] = OZK(X_V(k), Y_V(k), Z_V(k));
Tetta1_vect(k)=Tetta1;
Y_L1 = -(OQ + R_l*sind(Tetta1-90));
Z_L1 = R_l*cosd(Tetta1-90);
Y_M1 = Y_V(k) - VM;
Z_M1 = Z_V(k);
Y_Q1 = -OQ;
Z_Q1 = 0;
X_L1 = 0;
X_M1 = X_V(k);
x = (v_VX(k)*(X_L1 - X_M1) + v_VY(k)*(Y_L1 - Y_M1) + v_VZ(k)*(Z_L1 - Z_M1))/(Y_M1 - Y_L1 + ((Y_L1 - Y_Q1)*(Z_L1 - Z_M1))/(Z_L1 - Z_Q1));
y = (Y_Q1-Y_L1)/(Z_Q1-Z_L1)*x;
v_L1 = sqrt(x^2+y^2);
if y>0
znak = -1;
else
znak = 1;
end
w1_pv(k) = znak*v_L1/R_l;
end
%Вычисляем скорости матричным методом
for k=1:Diskr
J = [ X_V(k)/(R_l*(1 - ((OQ - VM + Y_V(k))^2 + R_l^2 - R_r^2 + X_V(k)^2 + Z_V(k)^2)^2/(4*R_l^2*((OQ - VM + Y_V(k))^2 + Z_V(k)^2)))^(1/2)*((OQ - VM + Y_V(k))^2 + Z_V(k)^2)^(1/2)), ((2*OQ - 2*VM + 2*Y_V(k))/(2*R_l*((OQ - VM + Y_V(k))^2 + Z_V(k)^2)^(1/2)) - ((2*OQ - 2*VM + 2*Y_V(k))*((OQ - VM + Y_V(k))^2 + R_l^2 - R_r^2 + X_V(k)^2 + Z_V(k)^2))/(4*R_l*((OQ - VM + Y_V(k))^2 + Z_V(k)^2)^(3/2)))/(1 - ((OQ - VM + Y_V(k))^2 + R_l^2 - R_r^2 + X_V(k)^2 + Z_V(k)^2)^2/(4*R_l^2*((OQ - VM + Y_V(k))^2 + Z_V(k)^2)))^(1/2) + (1/((OQ - VM + Y_V(k))^2 + Z_V(k)^2)^(1/2) - ((2*OQ - 2*VM + 2*Y_V(k))*(OQ - VM + Y_V(k)))/(2*((OQ - VM + Y_V(k))^2 + Z_V(k)^2)^(3/2)))/(1 - (OQ - VM + Y_V(k))^2/((OQ - VM + Y_V(k))^2 + Z_V(k)^2))^(1/2), (Z_V(k)/(R_l*((OQ - VM + Y_V(k))^2 + Z_V(k)^2)^(1/2)) - (Z_V(k)*((OQ - VM + Y_V(k))^2 + R_l^2 - R_r^2 + X_V(k)^2 + Z_V(k)^2))/(2*R_l*((OQ - VM + Y_V(k))^2 + Z_V(k)^2)^(3/2)))/(1 - ((OQ - VM + Y_V(k))^2 + R_l^2 - R_r^2 + X_V(k)^2 + Z_V(k)^2)^2/(4*R_l^2*((OQ - VM + Y_V(k))^2 + Z_V(k)^2)))^(1/2) - (Z_V(k)*(OQ - VM + Y_V(k)))/(((OQ - VM + Y_V(k))^2 + Z_V(k)^2)^(3/2)*(1 - (OQ - VM + Y_V(k))^2/((OQ - VM + Y_V(k))^2 + Z_V(k)^2))^(1/2));
(3^(1/2)/(2*((OQ - VM - Y_V(k)/2 + (3^(1/2)*X_V(k))/2)^2 + Z_V(k)^2)^(1/2)) - (3^(1/2)*(OQ - VM - Y_V(k)/2 + (3^(1/2)*X_V(k))/2)^2)/(2*((OQ - VM - Y_V(k)/2 + (3^(1/2)*X_V(k))/2)^2 + Z_V(k)^2)^(3/2)))/(1 - (OQ - VM - Y_V(k)/2 + (3^(1/2)*X_V(k))/2)^2/((OQ - VM - Y_V(k)/2 + (3^(1/2)*X_V(k))/2)^2 + Z_V(k)^2))^(1/2) + ((X_V(k)/2 + (3^(1/2)*Y_V(k))/2 + 3^(1/2)*(OQ - VM - Y_V(k)/2 + (3^(1/2)*X_V(k))/2))/(2*R_l*((OQ - VM - Y_V(k)/2 + (3^(1/2)*X_V(k))/2)^2 + Z_V(k)^2)^(1/2)) - (3^(1/2)*(OQ - VM - Y_V(k)/2 + (3^(1/2)*X_V(k))/2)*((X_V(k)/2 + (3^(1/2)*Y_V(k))/2)^2 + (OQ - VM - Y_V(k)/2 + (3^(1/2)*X_V(k))/2)^2 + R_l^2 - R_r^2 + Z_V(k)^2))/(4*R_l*((OQ - VM - Y_V(k)/2 + (3^(1/2)*X_V(k))/2)^2 + Z_V(k)^2)^(3/2)))/(1 - ((X_V(k)/2 + (3^(1/2)*Y_V(k))/2)^2 + (OQ - VM - Y_V(k)/2 + (3^(1/2)*X_V(k))/2)^2 + R_l^2 - R_r^2 + Z_V(k)^2)^2/(4*R_l^2*((OQ - VM - Y_V(k)/2 + (3^(1/2)*X_V(k))/2)^2 + Z_V(k)^2)))^(1/2), ((VM - OQ + Y_V(k)/2 - (3^(1/2)*X_V(k))/2 + 3^(1/2)*(X_V(k)/2 + (3^(1/2)*Y_V(k))/2))/(2*R_l*((OQ - VM - Y_V(k)/2 + (3^(1/2)*X_V(k))/2)^2 + Z_V(k)^2)^(1/2)) + ((OQ - VM - Y_V(k)/2 + (3^(1/2)*X_V(k))/2)*((X_V(k)/2 + (3^(1/2)*Y_V(k))/2)^2 + (OQ - VM - Y_V(k)/2 + (3^(1/2)*X_V(k))/2)^2 + R_l^2 - R_r^2 + Z_V(k)^2))/(4*R_l*((OQ - VM - Y_V(k)/2 + (3^(1/2)*X_V(k))/2)^2 + Z_V(k)^2)^(3/2)))/(1 - ((X_V(k)/2 + (3^(1/2)*Y_V(k))/2)^2 + (OQ - VM - Y_V(k)/2 + (3^(1/2)*X_V(k))/2)^2 + R_l^2 - R_r^2 + Z_V(k)^2)^2/(4*R_l^2*((OQ - VM - Y_V(k)/2 + (3^(1/2)*X_V(k))/2)^2 + Z_V(k)^2)))^(1/2) + ((OQ - VM - Y_V(k)/2 + (3^(1/2)*X_V(k))/2)^2/(2*((OQ - VM - Y_V(k)/2 + (3^(1/2)*X_V(k))/2)^2 + Z_V(k)^2)^(3/2)) - 1/(2*((OQ - VM - Y_V(k)/2 + (3^(1/2)*X_V(k))/2)^2 + Z_V(k)^2)^(1/2)))/(1 - (OQ - VM - Y_V(k)/2 + (3^(1/2)*X_V(k))/2)^2/((OQ - VM - Y_V(k)/2 + (3^(1/2)*X_V(k))/2)^2 + Z_V(k)^2))^(1/2), (Z_V(k)/(R_l*((OQ - VM - Y_V(k)/2 + (3^(1/2)*X_V(k))/2)^2 + Z_V(k)^2)^(1/2)) - (Z_V(k)*((X_V(k)/2 + (3^(1/2)*Y_V(k))/2)^2 + (OQ - VM - Y_V(k)/2 + (3^(1/2)*X_V(k))/2)^2 + R_l^2 - R_r^2 + Z_V(k)^2))/(2*R_l*((OQ - VM - Y_V(k)/2 + (3^(1/2)*X_V(k))/2)^2 + Z_V(k)^2)^(3/2)))/(1 - ((X_V(k)/2 + (3^(1/2)*Y_V(k))/2)^2 + (OQ - VM - Y_V(k)/2 + (3^(1/2)*X_V(k))/2)^2 + R_l^2 - R_r^2 + Z_V(k)^2)^2/(4*R_l^2*((OQ - VM - Y_V(k)/2 + (3^(1/2)*X_V(k))/2)^2 + Z_V(k)^2)))^(1/2) - (Z_V(k)*(OQ - VM - Y_V(k)/2 + (3^(1/2)*X_V(k))/2))/((1 - (OQ - VM - Y_V(k)/2 + (3^(1/2)*X_V(k))/2)^2/((OQ - VM - Y_V(k)/2 + (3^(1/2)*X_V(k))/2)^2 + Z_V(k)^2))^(1/2)*((OQ - VM - Y_V(k)/2 + (3^(1/2)*X_V(k))/2)^2 + Z_V(k)^2)^(3/2));
((X_V(k)/2 - (3^(1/2)*Y_V(k))/2 + 3^(1/2)*(VM - OQ + Y_V(k)/2 + (3^(1/2)*X_V(k))/2))/(2*R_l*((VM - OQ + Y_V(k)/2 + (3^(1/2)*X_V(k))/2)^2 + Z_V(k)^2)^(1/2)) - (3^(1/2)*(VM - OQ + Y_V(k)/2 + (3^(1/2)*X_V(k))/2)*((X_V(k)/2 - (3^(1/2)*Y_V(k))/2)^2 + (VM - OQ + Y_V(k)/2 + (3^(1/2)*X_V(k))/2)^2 + R_l^2 - R_r^2 + Z_V(k)^2))/(4*R_l*((VM - OQ + Y_V(k)/2 + (3^(1/2)*X_V(k))/2)^2 + Z_V(k)^2)^(3/2)))/(1 - ((X_V(k)/2 - (3^(1/2)*Y_V(k))/2)^2 + (VM - OQ + Y_V(k)/2 + (3^(1/2)*X_V(k))/2)^2 + R_l^2 - R_r^2 + Z_V(k)^2)^2/(4*R_l^2*((VM - OQ + Y_V(k)/2 + (3^(1/2)*X_V(k))/2)^2 + Z_V(k)^2)))^(1/2) - (3^(1/2)/(2*((VM - OQ + Y_V(k)/2 + (3^(1/2)*X_V(k))/2)^2 + Z_V(k)^2)^(1/2)) - (3^(1/2)*(VM - OQ + Y_V(k)/2 + (3^(1/2)*X_V(k))/2)^2)/(2*((VM - OQ + Y_V(k)/2 + (3^(1/2)*X_V(k))/2)^2 + Z_V(k)^2)^(3/2)))/(1 - (VM - OQ + Y_V(k)/2 + (3^(1/2)*X_V(k))/2)^2/((VM - OQ + Y_V(k)/2 + (3^(1/2)*X_V(k))/2)^2 + Z_V(k)^2))^(1/2), ((VM - OQ + Y_V(k)/2 + (3^(1/2)*X_V(k))/2 - 3^(1/2)*(X_V(k)/2 - (3^(1/2)*Y_V(k))/2))/(2*R_l*((VM - OQ + Y_V(k)/2 + (3^(1/2)*X_V(k))/2)^2 + Z_V(k)^2)^(1/2)) - ((VM - OQ + Y_V(k)/2 + (3^(1/2)*X_V(k))/2)*((X_V(k)/2 - (3^(1/2)*Y_V(k))/2)^2 + (VM - OQ + Y_V(k)/2 + (3^(1/2)*X_V(k))/2)^2 + R_l^2 - R_r^2 + Z_V(k)^2))/(4*R_l*((VM - OQ + Y_V(k)/2 + (3^(1/2)*X_V(k))/2)^2 + Z_V(k)^2)^(3/2)))/(1 - ((X_V(k)/2 - (3^(1/2)*Y_V(k))/2)^2 + (VM - OQ + Y_V(k)/2 + (3^(1/2)*X_V(k))/2)^2 + R_l^2 - R_r^2 + Z_V(k)^2)^2/(4*R_l^2*((VM - OQ + Y_V(k)/2 + (3^(1/2)*X_V(k))/2)^2 + Z_V(k)^2)))^(1/2) + ((VM - OQ + Y_V(k)/2 + (3^(1/2)*X_V(k))/2)^2/(2*((VM - OQ + Y_V(k)/2 + (3^(1/2)*X_V(k))/2)^2 + Z_V(k)^2)^(3/2)) - 1/(2*((VM - OQ + Y_V(k)/2 + (3^(1/2)*X_V(k))/2)^2 + Z_V(k)^2)^(1/2)))/(1 - (VM - OQ + Y_V(k)/2 + (3^(1/2)*X_V(k))/2)^2/((VM - OQ + Y_V(k)/2 + (3^(1/2)*X_V(k))/2)^2 + Z_V(k)^2))^(1/2), (Z_V(k)/(R_l*((VM - OQ + Y_V(k)/2 + (3^(1/2)*X_V(k))/2)^2 + Z_V(k)^2)^(1/2)) - (Z_V(k)*((X_V(k)/2 - (3^(1/2)*Y_V(k))/2)^2 + (VM - OQ + Y_V(k)/2 + (3^(1/2)*X_V(k))/2)^2 + R_l^2 - R_r^2 + Z_V(k)^2))/(2*R_l*((VM - OQ + Y_V(k)/2 + (3^(1/2)*X_V(k))/2)^2 + Z_V(k)^2)^(3/2)))/(1 - ((X_V(k)/2 - (3^(1/2)*Y_V(k))/2)^2 + (VM - OQ + Y_V(k)/2 + (3^(1/2)*X_V(k))/2)^2 + R_l^2 - R_r^2 + Z_V(k)^2)^2/(4*R_l^2*((VM - OQ + Y_V(k)/2 + (3^(1/2)*X_V(k))/2)^2 + Z_V(k)^2)))^(1/2) + (Z_V(k)*(VM - OQ + Y_V(k)/2 + (3^(1/2)*X_V(k))/2))/((1 - (VM - OQ + Y_V(k)/2 + (3^(1/2)*X_V(k))/2)^2/((VM - OQ + Y_V(k)/2 + (3^(1/2)*X_V(k))/2)^2 + Z_V(k)^2))^(1/2)*((VM - OQ + Y_V(k)/2 + (3^(1/2)*X_V(k))/2)^2 + Z_V(k)^2)^(3/2))];
v_V = [v_VX(k); v_VY(k); v_VZ(k)];
w = J*v_V;
w1_mat(k) = w(1);
end
%Находим производную Tetta1_vect графически
%Перевод в радианы
Tetta1_vect = Tetta1_vect*pi/180;
w1_der = diff([Tetta1_vect, Tetta1_vect(1)])./dTime;
%Отображаем скорость первого рычага на графиках
figure
hold on;
subplot(2, 2, 1);
plot(Time, w1_der*30/pi);
grid on;
xlabel('t, c');
ylabel('\omega, об/мин');
title('Грифическое дифференцирование');
axis([0, 2.1, -55, 55]);
subplot(2, 2, 2);
plot(Time, w1*30/pi);
grid on;
xlabel('t, c');
ylabel('\omega, об/мин');
title('Аналитический метод');
axis([0, 2.1, -55, 55]);
subplot(2, 2, 3);
plot(Time, w1_pv*30/pi);
grid on;
xlabel('t, c');
ylabel('\omega, об/мин');
title('Метод планов');
axis([0, 2.1, -55, 55]);
subplot(2, 2, 4);
plot(Time, w1_mat*30/pi);
grid on;
xlabel('t, c');
ylabel('\omega, об/мин');
title('Матричный мотод');
axis([0, 2.1, -55, 55]);
hold off;Тут используется функция, которую я демонстрировал в первом подходе, функция, решающая ОЗК, которую я давал ранее и код, в котором объявляются константы (InputConstant;):
%Задаём параметры механизма
global f F R_l R_r cos120 sin120 cos240 sin240 VM OQ
f = 110; %Длина стороны треугольника платформы
F = 270; %Длина стороны треугольника основания
R_l = 170; %Длина рычагов
R_r = 320; %Длина штанг
%Расчёт констант
cos120 = cosd(120);
sin120 = sind(120);
cos240 = cosd(240);
sin240 = sind(240);
VM = f*sqrt(3)/6;
OQ = F*sqrt(3)/6;Я это всё слепил из того, что было. Можно оформить это "красивше".
Добавив к коду выше вот это:
dim = [f F R_l R_r];
figure
for k = 1:Diskr
V = [X_V(k), Y_V(k), Z_V(k)];
[Theta1, Theta2, Theta3] = OZK(X_V(k), Y_V(k), Z_V(k));
Theta = [Theta1, Theta2, Theta3];
drawDelta(dim, V, Theta)%Это я накидывал бонусом ранее
axis([-250, 250, -250, 250, -390, 100]);
hold on;
plot3(X_V(1:k), Y_V(1:k), Z_V(1:k),'LineWidth', 1.5, 'Color', 'r');
hold off;
drawnow
pause(dTime(k)*20);
endМожно получить вот такую симуляцию работы (я замедлил отрисовку в 20 раз, а потом на видео ускорил в 65 раз, а то MATLAB на моём слабеньком ноуте не успевал считать):
Да, так быстро дельта-робот может и должен двигаться – 2.1 секунды на всю траекторию. Скорость платформы, кстати, 1 м/с.
Поиск максимальной скорости привода
Пришло время использовать то, что мы получили ранее. Для поиска максимальной скорости привода возьмём размеры робота, найденные в ходе предыдущей работы.
Первым делом сгенерируем массив точек для анализа внутри желаемой рабочей зоны:
Не забываем, что дельта-робот имеет симметрию и достаточно рассмотреть лишь 1/6 рабочей зоны.
Вот моя реализация этого алгоритма:
Функция genTP
%Генерирует массив точек для анализа
function [TestPointsX, TestPointsY, TestPointsZ] = genTP()
%Добавляем глобальные переменные входных и выходных данных
global WZ_D WZ_H WZ_Z WZ_d WZ_h %Рабочая зона
global addSeg %Переменная, показывающая необходимость дополнительного
%сегмента в рабочей зоне
global dotDensity %Плотность точек
%Для тестирования
dotDensity = 30;
WZ_D = 320;
WZ_d = 100;
WZ_d = 562;
WZ_H = 150;
WZ_Z = -390;
WZ_h = 50;
addSeg = 0;
%Задаём массив точек, заведомо покрывающий интересующий нас участок рабочей
%зоны
x = linspace(-WZ_D/2, 0, dotDensity);
y = linspace(-WZ_D/2, 0, dotDensity);
z = linspace(WZ_Z-WZ_h, WZ_Z+WZ_H, dotDensity);
[X, Y, Z] = meshgrid(x, y, z);
X = reshape(X, [1, size(X, 1)^3, 1]);
Y = reshape(Y, [1, size(Y, 1)^3, 1]);
Z = reshape(Z, [1, size(Z, 1)^3, 1]);
%Находим индексы точек, попадающих в цилиндрическую область
inCil = X.^2+Y.^2 <= (WZ_D/2)^2;
%И попадающих в сектр 60°
inSec = Y < tand(30)*X;
%Находим инексы точек попадающих
%1 Только в цилиндрическую РЗ
if addSeg == 0
inDopSeg = Z > WZ_Z;
end
%В цилиндрическую РЗ и усёчённый конус
if addSeg == 1
z0 = WZ_Z - WZ_h*(WZ_D/2)/((WZ_D-WZ_d)/2);
a2_c2 = ((WZ_D/2)/(WZ_Z-z0))^2;
inDopSeg = X.^2 + Y.^2 <= a2_c2*(Z-z0).^2;
end
%В цилиндрическую РЗ и в часть сферы
if addSeg == 2
inDopSeg = (X.^2 + Y.^2 + (Z-(WZ_Z-WZ_h+WZ_d/2)).^2 <= (WZ_d/2)^2)|(Z > WZ_Z);
end
%Формируем индексы точек, удовлетворяющих всем условиям
inWZ = inCil & inDopSeg & inSec;
TestPointsX = X(inWZ);
TestPointsY = Y(inWZ);
TestPointsZ = Z(inWZ);
%Тестовая отрисовка точек
plot3(X(inWZ), Y(inWZ), Z(inWZ), '.r');
xlabel('x, мм');
ylabel('y, мм');
zlabel('z, мм');
grid on;
end
Теперь в каждой точке нам нужно нужно найти угловые скорости рычагов (скорости приводов) при движении исполнительного звена в этой точке с максимальной требуемой скоростью.
Но ведь скорость входных звеньев зависит ещё и от направления движения выходного звена. Значит для каждой точки нужно рассмотреть ещё и массив направлений. Например точек для анализа у нас будет 1000. Направлений в каждой точке 100, тогда всего нам придётся запускать цикл поиска входных скоростей 100 000 раз. Многовато, не правда ли? Я так и сделал изначально. MATLAB считал секунд 40, но направлений я брал всего 22 в каждой точке:
Помимо того, что это долго, у нас ещё есть риск, что максимальная скорость будет чуть больше из-за того, что мы не проверили самое "наихудшее" направление.
Правильным подходом здесь является запуск алгоритма в каждой тестовой точке лишь с одним, но "наихудшим" направлением вектора скорости. Оказывается, можно это "наихудшее" направление заранее узнать.
Вот тут на помощь как раз приходит матричный метод. Скорость i-го привода мы вычисляем путём умножения вектора строки Якобиана на вектор столбец скорости рабочего органа:
Данное векторное произведение достигает своего максимума тогда, когда эти вектора являются коллинеарными. Если опустить все доказательства, то можно сказать, что для дельта-робота справедлива следующая формула:
Эта запись математически звучит так: "произведение длины вектора максимальной скорости платформы на евклидовой норму i-ой строки обратной матрицы Якоби". Звучит жутко, но записывается коротко, а считается быстро.
Посчитав для каждой точки для всех трёх приводов по этой формуле максимальную угловую скорость привода и найдя среди итих значений максимум, мы и получим максимальную скорость привода.
Вот коротенький код:
Итоговый код для определения максимальной скорости привода
%Задаём начальное значение
wmax = 0;
%Задаём входные данные
vmax = 1000;
R_l = 170;
R_r = 320;
OQ = 77.9423;
VM = 23.094;
cos120 = cosd(120);
sin120 = sind(120);
cos240 = cosd(240);
sin240 = sind(240);
[TestPointsX, TestPointsY, TestPointsZ] = genTP();
%Запускаем цикл проверки в кажой точке
for k = 1:size(TestPointsX, 2)
J = [ TestPointsX(k)/(R_l*(1 - ((OQ - VM + TestPointsY(k))^2 + R_l^2 - R_r^2 + TestPointsX(k)^2 + TestPointsZ(k)^2)^2/(4*R_l^2*((OQ - VM + TestPointsY(k))^2 + TestPointsZ(k)^2)))^(1/2)*((OQ - VM + TestPointsY(k))^2 + TestPointsZ(k)^2)^(1/2)), ((2*OQ - 2*VM + 2*TestPointsY(k))/(2*R_l*((OQ - VM + TestPointsY(k))^2 + TestPointsZ(k)^2)^(1/2)) - ((2*OQ - 2*VM + 2*TestPointsY(k))*((OQ - VM + TestPointsY(k))^2 + R_l^2 - R_r^2 + TestPointsX(k)^2 + TestPointsZ(k)^2))/(4*R_l*((OQ - VM + TestPointsY(k))^2 + TestPointsZ(k)^2)^(3/2)))/(1 - ((OQ - VM + TestPointsY(k))^2 + R_l^2 - R_r^2 + TestPointsX(k)^2 + TestPointsZ(k)^2)^2/(4*R_l^2*((OQ - VM + TestPointsY(k))^2 + TestPointsZ(k)^2)))^(1/2) + (1/((OQ - VM + TestPointsY(k))^2 + TestPointsZ(k)^2)^(1/2) - ((2*OQ - 2*VM + 2*TestPointsY(k))*(OQ - VM + TestPointsY(k)))/(2*((OQ - VM + TestPointsY(k))^2 + TestPointsZ(k)^2)^(3/2)))/(1 - (OQ - VM + TestPointsY(k))^2/((OQ - VM + TestPointsY(k))^2 + TestPointsZ(k)^2))^(1/2), (TestPointsZ(k)/(R_l*((OQ - VM + TestPointsY(k))^2 + TestPointsZ(k)^2)^(1/2)) - (TestPointsZ(k)*((OQ - VM + TestPointsY(k))^2 + R_l^2 - R_r^2 + TestPointsX(k)^2 + TestPointsZ(k)^2))/(2*R_l*((OQ - VM + TestPointsY(k))^2 + TestPointsZ(k)^2)^(3/2)))/(1 - ((OQ - VM + TestPointsY(k))^2 + R_l^2 - R_r^2 + TestPointsX(k)^2 + TestPointsZ(k)^2)^2/(4*R_l^2*((OQ - VM + TestPointsY(k))^2 + TestPointsZ(k)^2)))^(1/2) - (TestPointsZ(k)*(OQ - VM + TestPointsY(k)))/(((OQ - VM + TestPointsY(k))^2 + TestPointsZ(k)^2)^(3/2)*(1 - (OQ - VM + TestPointsY(k))^2/((OQ - VM + TestPointsY(k))^2 + TestPointsZ(k)^2))^(1/2));
(3^(1/2)/(2*((OQ - VM - TestPointsY(k)/2 + (3^(1/2)*TestPointsX(k))/2)^2 + TestPointsZ(k)^2)^(1/2)) - (3^(1/2)*(OQ - VM - TestPointsY(k)/2 + (3^(1/2)*TestPointsX(k))/2)^2)/(2*((OQ - VM - TestPointsY(k)/2 + (3^(1/2)*TestPointsX(k))/2)^2 + TestPointsZ(k)^2)^(3/2)))/(1 - (OQ - VM - TestPointsY(k)/2 + (3^(1/2)*TestPointsX(k))/2)^2/((OQ - VM - TestPointsY(k)/2 + (3^(1/2)*TestPointsX(k))/2)^2 + TestPointsZ(k)^2))^(1/2) + ((TestPointsX(k)/2 + (3^(1/2)*TestPointsY(k))/2 + 3^(1/2)*(OQ - VM - TestPointsY(k)/2 + (3^(1/2)*TestPointsX(k))/2))/(2*R_l*((OQ - VM - TestPointsY(k)/2 + (3^(1/2)*TestPointsX(k))/2)^2 + TestPointsZ(k)^2)^(1/2)) - (3^(1/2)*(OQ - VM - TestPointsY(k)/2 + (3^(1/2)*TestPointsX(k))/2)*((TestPointsX(k)/2 + (3^(1/2)*TestPointsY(k))/2)^2 + (OQ - VM - TestPointsY(k)/2 + (3^(1/2)*TestPointsX(k))/2)^2 + R_l^2 - R_r^2 + TestPointsZ(k)^2))/(4*R_l*((OQ - VM - TestPointsY(k)/2 + (3^(1/2)*TestPointsX(k))/2)^2 + TestPointsZ(k)^2)^(3/2)))/(1 - ((TestPointsX(k)/2 + (3^(1/2)*TestPointsY(k))/2)^2 + (OQ - VM - TestPointsY(k)/2 + (3^(1/2)*TestPointsX(k))/2)^2 + R_l^2 - R_r^2 + TestPointsZ(k)^2)^2/(4*R_l^2*((OQ - VM - TestPointsY(k)/2 + (3^(1/2)*TestPointsX(k))/2)^2 + TestPointsZ(k)^2)))^(1/2), ((VM - OQ + TestPointsY(k)/2 - (3^(1/2)*TestPointsX(k))/2 + 3^(1/2)*(TestPointsX(k)/2 + (3^(1/2)*TestPointsY(k))/2))/(2*R_l*((OQ - VM - TestPointsY(k)/2 + (3^(1/2)*TestPointsX(k))/2)^2 + TestPointsZ(k)^2)^(1/2)) + ((OQ - VM - TestPointsY(k)/2 + (3^(1/2)*TestPointsX(k))/2)*((TestPointsX(k)/2 + (3^(1/2)*TestPointsY(k))/2)^2 + (OQ - VM - TestPointsY(k)/2 + (3^(1/2)*TestPointsX(k))/2)^2 + R_l^2 - R_r^2 + TestPointsZ(k)^2))/(4*R_l*((OQ - VM - TestPointsY(k)/2 + (3^(1/2)*TestPointsX(k))/2)^2 + TestPointsZ(k)^2)^(3/2)))/(1 - ((TestPointsX(k)/2 + (3^(1/2)*TestPointsY(k))/2)^2 + (OQ - VM - TestPointsY(k)/2 + (3^(1/2)*TestPointsX(k))/2)^2 + R_l^2 - R_r^2 + TestPointsZ(k)^2)^2/(4*R_l^2*((OQ - VM - TestPointsY(k)/2 + (3^(1/2)*TestPointsX(k))/2)^2 + TestPointsZ(k)^2)))^(1/2) + ((OQ - VM - TestPointsY(k)/2 + (3^(1/2)*TestPointsX(k))/2)^2/(2*((OQ - VM - TestPointsY(k)/2 + (3^(1/2)*TestPointsX(k))/2)^2 + TestPointsZ(k)^2)^(3/2)) - 1/(2*((OQ - VM - TestPointsY(k)/2 + (3^(1/2)*TestPointsX(k))/2)^2 + TestPointsZ(k)^2)^(1/2)))/(1 - (OQ - VM - TestPointsY(k)/2 + (3^(1/2)*TestPointsX(k))/2)^2/((OQ - VM - TestPointsY(k)/2 + (3^(1/2)*TestPointsX(k))/2)^2 + TestPointsZ(k)^2))^(1/2), (TestPointsZ(k)/(R_l*((OQ - VM - TestPointsY(k)/2 + (3^(1/2)*TestPointsX(k))/2)^2 + TestPointsZ(k)^2)^(1/2)) - (TestPointsZ(k)*((TestPointsX(k)/2 + (3^(1/2)*TestPointsY(k))/2)^2 + (OQ - VM - TestPointsY(k)/2 + (3^(1/2)*TestPointsX(k))/2)^2 + R_l^2 - R_r^2 + TestPointsZ(k)^2))/(2*R_l*((OQ - VM - TestPointsY(k)/2 + (3^(1/2)*TestPointsX(k))/2)^2 + TestPointsZ(k)^2)^(3/2)))/(1 - ((TestPointsX(k)/2 + (3^(1/2)*TestPointsY(k))/2)^2 + (OQ - VM - TestPointsY(k)/2 + (3^(1/2)*TestPointsX(k))/2)^2 + R_l^2 - R_r^2 + TestPointsZ(k)^2)^2/(4*R_l^2*((OQ - VM - TestPointsY(k)/2 + (3^(1/2)*TestPointsX(k))/2)^2 + TestPointsZ(k)^2)))^(1/2) - (TestPointsZ(k)*(OQ - VM - TestPointsY(k)/2 + (3^(1/2)*TestPointsX(k))/2))/((1 - (OQ - VM - TestPointsY(k)/2 + (3^(1/2)*TestPointsX(k))/2)^2/((OQ - VM - TestPointsY(k)/2 + (3^(1/2)*TestPointsX(k))/2)^2 + TestPointsZ(k)^2))^(1/2)*((OQ - VM - TestPointsY(k)/2 + (3^(1/2)*TestPointsX(k))/2)^2 + TestPointsZ(k)^2)^(3/2));
((TestPointsX(k)/2 - (3^(1/2)*TestPointsY(k))/2 + 3^(1/2)*(VM - OQ + TestPointsY(k)/2 + (3^(1/2)*TestPointsX(k))/2))/(2*R_l*((VM - OQ + TestPointsY(k)/2 + (3^(1/2)*TestPointsX(k))/2)^2 + TestPointsZ(k)^2)^(1/2)) - (3^(1/2)*(VM - OQ + TestPointsY(k)/2 + (3^(1/2)*TestPointsX(k))/2)*((TestPointsX(k)/2 - (3^(1/2)*TestPointsY(k))/2)^2 + (VM - OQ + TestPointsY(k)/2 + (3^(1/2)*TestPointsX(k))/2)^2 + R_l^2 - R_r^2 + TestPointsZ(k)^2))/(4*R_l*((VM - OQ + TestPointsY(k)/2 + (3^(1/2)*TestPointsX(k))/2)^2 + TestPointsZ(k)^2)^(3/2)))/(1 - ((TestPointsX(k)/2 - (3^(1/2)*TestPointsY(k))/2)^2 + (VM - OQ + TestPointsY(k)/2 + (3^(1/2)*TestPointsX(k))/2)^2 + R_l^2 - R_r^2 + TestPointsZ(k)^2)^2/(4*R_l^2*((VM - OQ + TestPointsY(k)/2 + (3^(1/2)*TestPointsX(k))/2)^2 + TestPointsZ(k)^2)))^(1/2) - (3^(1/2)/(2*((VM - OQ + TestPointsY(k)/2 + (3^(1/2)*TestPointsX(k))/2)^2 + TestPointsZ(k)^2)^(1/2)) - (3^(1/2)*(VM - OQ + TestPointsY(k)/2 + (3^(1/2)*TestPointsX(k))/2)^2)/(2*((VM - OQ + TestPointsY(k)/2 + (3^(1/2)*TestPointsX(k))/2)^2 + TestPointsZ(k)^2)^(3/2)))/(1 - (VM - OQ + TestPointsY(k)/2 + (3^(1/2)*TestPointsX(k))/2)^2/((VM - OQ + TestPointsY(k)/2 + (3^(1/2)*TestPointsX(k))/2)^2 + TestPointsZ(k)^2))^(1/2), ((VM - OQ + TestPointsY(k)/2 + (3^(1/2)*TestPointsX(k))/2 - 3^(1/2)*(TestPointsX(k)/2 - (3^(1/2)*TestPointsY(k))/2))/(2*R_l*((VM - OQ + TestPointsY(k)/2 + (3^(1/2)*TestPointsX(k))/2)^2 + TestPointsZ(k)^2)^(1/2)) - ((VM - OQ + TestPointsY(k)/2 + (3^(1/2)*TestPointsX(k))/2)*((TestPointsX(k)/2 - (3^(1/2)*TestPointsY(k))/2)^2 + (VM - OQ + TestPointsY(k)/2 + (3^(1/2)*TestPointsX(k))/2)^2 + R_l^2 - R_r^2 + TestPointsZ(k)^2))/(4*R_l*((VM - OQ + TestPointsY(k)/2 + (3^(1/2)*TestPointsX(k))/2)^2 + TestPointsZ(k)^2)^(3/2)))/(1 - ((TestPointsX(k)/2 - (3^(1/2)*TestPointsY(k))/2)^2 + (VM - OQ + TestPointsY(k)/2 + (3^(1/2)*TestPointsX(k))/2)^2 + R_l^2 - R_r^2 + TestPointsZ(k)^2)^2/(4*R_l^2*((VM - OQ + TestPointsY(k)/2 + (3^(1/2)*TestPointsX(k))/2)^2 + TestPointsZ(k)^2)))^(1/2) + ((VM - OQ + TestPointsY(k)/2 + (3^(1/2)*TestPointsX(k))/2)^2/(2*((VM - OQ + TestPointsY(k)/2 + (3^(1/2)*TestPointsX(k))/2)^2 + TestPointsZ(k)^2)^(3/2)) - 1/(2*((VM - OQ + TestPointsY(k)/2 + (3^(1/2)*TestPointsX(k))/2)^2 + TestPointsZ(k)^2)^(1/2)))/(1 - (VM - OQ + TestPointsY(k)/2 + (3^(1/2)*TestPointsX(k))/2)^2/((VM - OQ + TestPointsY(k)/2 + (3^(1/2)*TestPointsX(k))/2)^2 + TestPointsZ(k)^2))^(1/2), (TestPointsZ(k)/(R_l*((VM - OQ + TestPointsY(k)/2 + (3^(1/2)*TestPointsX(k))/2)^2 + TestPointsZ(k)^2)^(1/2)) - (TestPointsZ(k)*((TestPointsX(k)/2 - (3^(1/2)*TestPointsY(k))/2)^2 + (VM - OQ + TestPointsY(k)/2 + (3^(1/2)*TestPointsX(k))/2)^2 + R_l^2 - R_r^2 + TestPointsZ(k)^2))/(2*R_l*((VM - OQ + TestPointsY(k)/2 + (3^(1/2)*TestPointsX(k))/2)^2 + TestPointsZ(k)^2)^(3/2)))/(1 - ((TestPointsX(k)/2 - (3^(1/2)*TestPointsY(k))/2)^2 + (VM - OQ + TestPointsY(k)/2 + (3^(1/2)*TestPointsX(k))/2)^2 + R_l^2 - R_r^2 + TestPointsZ(k)^2)^2/(4*R_l^2*((VM - OQ + TestPointsY(k)/2 + (3^(1/2)*TestPointsX(k))/2)^2 + TestPointsZ(k)^2)))^(1/2) + (TestPointsZ(k)*(VM - OQ + TestPointsY(k)/2 + (3^(1/2)*TestPointsX(k))/2))/((1 - (VM - OQ + TestPointsY(k)/2 + (3^(1/2)*TestPointsX(k))/2)^2/((VM - OQ + TestPointsY(k)/2 + (3^(1/2)*TestPointsX(k))/2)^2 + TestPointsZ(k)^2))^(1/2)*((VM - OQ + TestPointsY(k)/2 + (3^(1/2)*TestPointsX(k))/2)^2 + TestPointsZ(k)^2)^(3/2))];
w_max_TP = [norm(J(1, :))*vmax, norm(J(2, :))*vmax, norm(J(3, :))*vmax];
%Находим максимум скорости рычагов
wmax = max([abs(w_max_TP), wmax]);
end
n = 30*wmax/pi
Тут используем лишь одну пользовательскую функцию genTP(), которую я давал ранее.
В следующей статье найдём максимальны крутящий момент и порассуждаем, какие комплектующие для привода стоит выбирать. Затронем вопрос точности привода.
Комментарии (6)
SnakeSolid
26.10.2021 07:36+1А что с максимальным ускорением? Судя по графикам (t = ~0.05 и t = ~1.125) скорость привода должна достаточно быстро изменяться, что, при достаточной массе платформы, может привести к пропуску шагов и смещению траектории движения.
AVI-crak
Математическая жесть в чистом виде.
Мне всё таки хочется верить, что в реальном девайсе используются более короткие формулы.
vakhramov
У математиков не user-friendly названия переменных. Для девайса, поди, вообще экспорт в FPGA будет сделан, в который и фидбэк от энкодеров, и обратная связь по току для "привода", ну и названия переменных под это дело появятся.
А вот точность "привода" (автор не поясняет, какие части механизма он этим приводом (приводом треугольной площадки?) считает) - действительно сложная тема, которая может все эти формулы помножить на одно число, или поделить :) Я совершенно не представляю, чего в промышленных дельтах в хлипеньких узлах сопряжения "штанг" и "треугольника" используется, наработка на отказ этих узлов на мой дилетантский взгляд мне видится очень небольшой.
no1D
Используют шаровые шарниры, износ у них кстати не сказать что большой, смазывать надо вовремя, как и любую механику.
AVI-crak
Дельта лидирует по времени наработки на износ. Как только я пойму математику - обязательно соберу паучка.