13.10.2021 13:00
multiengineer 4 1300 Источник

В прошлых статьях мы познакомились с механизмом, узнали его историю, решили, пожалуй, главную для дельта-робота задачу – задачу о положениях, но так и не получили никаких данных, необходимых для проектирования этого манипулятора. Сегодня, используя ранее полученные зависимости, мы начнём это делать. Построим рабочую зону этого механизма, порассуждаем о том, что влияет на её размер и форму, а также обсудим вопрос выбора длин звеньев этого робота.

Как и в предыдущей статье, если какой-то символ в формулах означает непонятно что, поднимаемся сюда и нажимаем на спойлер.

Список обозначений

n– число подвижных звеньев;

p_1– число одноподвижных кинематических пар;

p_2– число двухподвижных кинематических пар;

p_3– число трёхподвижных кинематических пар;

p_4– число четырёхподвижных кинематических пар;

p_5– число пятиподвижных кинематических пар;

z_L– координата точки L по оси Z;

y_Q– координата точки Q по оси y;

и т. д. по той же логике здесь и далее я буду обозначать координаты точек;

F– длина стороны треугольника основания;

f– длина стороны треугольника платформы;

R_l– длина рычага;

R_r– длина штанги;

\theta_1, \theta_2и \theta_3– углы поворота рычагов, отсчитываемые от плоскости основания;

QG – вынос основания;

φ_{max}– максимальный угол в шаровом шарнире;

ϑ – угол давления.

Рабочая зона

Согласно  ГОСТ 25686-85 рабочей зоной (РЗ) называю пространство, в котором может находиться рабочий орган при функционировании манипулятора (или промышленного робота).

Для роботов с декартовой кинематикой, например, портальных манипуляторов вопрос с размерами и формой рабочей зоны решается довольно быстро. Это параллелепипед с размерами, определяемыми конструктивом механизма.

Определение размеров и формы рабочей зоны механизмов параллельной структуры задача не самая простая. Здесь можно выделить два подхода.

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

Попробуем применить его для нахождения РЗ дельта-робота

Сначала рисунок:

Звено QL, или рычаг, может вращаться в плоскости YOZ, то есть его конец описывает часть окружности. На этой окружности располагается трёхподвижный шарнир L, к которому прикреплено звено LM, или штанга, которая своим концом описывает уже сферу. Таким образом, вращая сферу по окружности получаем часть тора. На картинке выше я ограничил минимальный угол между штангой и рычагом 180-ю градусами, поэтому части торов заканчиваются сферическими поверхностями.

Далее, получив части торов для всех трёх плеч робота, первые необходимо сместить в плоскости XOY к оси Z, чтобы механизм принял такой вид, при котором все три штанги соединяются в одном шарнире. Пересечение этих частей торов и будет рабочей зоной дельта-робота.

Я уже говорил, что ограничил минимальный угол между штангой и рычагом. А ведь, по-хорошему, ограничений должно быть больше. Есть максимальные углы в шаровых шарнирах, конструктивные ограничения по перемещению, ограничение на максимальный угол давления ϑ_{max}(обо всём этом написано ниже).

Добавляя лишь одно ограничение на угол давления, получаем задачу трудноформализуемую с математической точки зрения. Штанга своим концом теперь описывает не сферу, а её сегмент, ограниченный максимальным углом давления между штангой и рычагом ϑ_{max}. Вращая этот сегмент вокруг точки Q, то есть вместе с рычагом, получим некое тело вращения. Пересечение этих тел (смещённых у центру платформы) для трёх рычагов и будет, в данном случае, рабочей зоной. Дальнейшие ограничения накладывать не будем в связи с возникшими трудностями уже на этом этапе и перейдём к дискретному методу.

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

Ограничения

Существование решений обратной или прямой задач кинематики для конкретной точки совсем не означает того, что исполнительное звено механизма может оказаться в этом месте. На движение механизма накладывается ряд ограничений. Рассмотрим их.

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

Ограничение движения конструктивными особенностями будем оценивать исходя из предполагаемого значения длины отрезка QG и минимально возможного угла θ_{min}:

Расстояние QG представляет собой сумму радиуса штанги и радиуса окружности (трубы), в которую можно вписать все приводные элементы (двигатель, редуктор, крепёж).

Угол θ_{min} потребуется примерно оценить по предполагаемой конструкции основания и элементов крепления приводов. Этот угол в алгоритме построения учесть легко – просто начнём перебор углов рычагов с этого значения.

Не приблизилась ли штанга к приводу можно оценить по углу γ. С одной стороны этот угол не должен быть меньше некоторого минимально значения γ_{min}зависящего от QG. С другой стороны, этот угол не может быть больше 180°.

Решая прямую задачу кинематики, мы определим координаты точек Q, L, M и легко можем узнать координаты точки N – проекции точки M на плоскость вращения рычага. Тогда искомый угол γ найдем по формуле:

(1)
(1)

где Y_{N }=Y_{M }

Формулу необходимо применять в системе координат, в которой рычаг вращается в плоскости YOZ. Минимальный угол γ_{min} определим из прямоугольного треугольника QL'G, в котором сторона L'G является касательной к окружности с центром в точке Q и радиусом QG. Зная также длину рычага R_l легко найдем γ_{min} по формуле:

(2)
(2)

Следующие ограничение связано с трёхподвижными шаровыми шарнирами. Все они имеют предельный угол поворота как минимум вокруг двух осей. При использовании шаровых магнитных шарниров, зачастую, можно считать, что прочие используемые ограничения, не позволят шаровому шарниру дойти до крайнего положения. В случае использования других шарниров угол между штангой и рычагом будет ограничен максимальным углом поворота шара в обойме. Тогда, чтобы убедиться, что это ограничение не нарушается, нужно найти угол φ между штангой и плоскостью вращения рычага и проверить, чтобы он не был больше, чем φ_{max} для каждого плеча. Это легко сделать из треугольника LMN (рисунок ниже).

Сторона LM известна и равна длине штанги R_r, а сторона MN равна координате точки V по оси X. Тогда угол φ можно найти по формуле:

(3)
(3)

Модуль в формуле обусловлен тем фактом, что центр каретки может отклоняться от плоскости ZOY как вправо, так и влево. Заметим также, что данные вычисления для каждого плеча, как и ранее, нужно проводить в системах координат, повёрнутых на 120° и 240°.

Последним ограничением выделим допустимое значение угла давления, превышение которого может привести к заклиниванию и самоторможению ????. В исследуемом механизме можно выделить несколько углов давления, но мы остановимся на наложении ограничений на один из них – угол между касательной к траектории движения конца рычага, вдоль которой направлена скорость, и штангой, вдоль которой направлена сила. Наглядно угол давления ϑ показан на рисунке:

Для его определения найдём координаты двух векторов и . Если координаты первого вектора легко найти через координаты точек M и L, то для поиска второго вектора сначала нужно найти координаты точки Q', повернув отрезок QL на 90° относительно точки L, чтобы получить касательную. В итоге, координаты векторов и найдём по формулам:

(4)
(4)

Теперь угол давления найдём как угол между векторами и  по формуле:

(5)
(5)

Забегая вперёд, следует отметить, что введённые ограничения делают форму рабочей области иной, чем во многих работах (????, ????, ???? и пр.). Эта особенность обусловлена тем, что очень часто авторы не накладывают никаких ограничений, а просто при построении варьируют углы рычагов в некотором диапазоне. А между тем, разница в форме и размерах получается существенная:

Реальная и требуемая рабочие зоны

Реальная рабочая зона дельта-робота имеет сложную форму, которую не ясно, как описать. Пример такой зоны представлен на рисунке ниже. Не менее затруднительно понять, подходит ли данная рабочая зона под конкретную задачу.

В связи с этим форму рабочей зоны, которая будет фигурировать в документации на робота, выбирают более простой (желаемая или требуемая РЗ). Имея дело с рабочей зоной в виде кругового цилиндра или параллелепипеда, легко понять, подходит ли робот с такими параметрами под конкретную задачу. Довольно часто с целью минимизации участков неиспользованной реальной рабочей зоны желаемую рабочую зону представляют в виде совокупности простых фигур: цилиндра и усечённого конуса или цилиндра и части сферы.

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

Размерный синтез

Одним из ключевых вопросов при проектировании дельта-робота является вопрос выбора длин звеньев. Иногда его называют задачей метрического синтеза или размерного синтеза.

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

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

Но этот подход не имеет никакой ценности, так как все остальные характеристики (динамическое, характеристики точности и пр.) дельта-робота не рассматривались.

Поэтому здесь я расскажу о более простом методе выбора длин звеньев – методе подбора.

Мы будим сами задавать размеры звеньев и запускать алгоритм, который будет проверять, подходит ли данная конфигурация для обслуживания дельта-роботом желаемой рабочей зоны. Визуально можно будет оценить, насколько реальная рабочая зона больше требуемой, алгоритм также будет выдавать и итоговые габариты получившегося устройства. Найдя подходящую конфигурацию, можно будет оценить, насколько робот отличается по размерным пропорциям от предложенных на рынке. А потом, в следующих статьях, мы ещё и напишем алгоритм силового расчёта, который позволит оценить нагрузки на приводы и равномерность динамических характеристик по всему объёму рабочей зоны. Если нас что-то не устраивает – задаём другие размеры, запускаем алгоритм заново. Так повторяем, пока нас всё не устроит.

Итак, по приведённому выше алгоритму можно построить реальную рабочую зону. Но как определить, помещается ли в неё желаемая зона?

Для начала стоит решить, какую рабочую зону мы хотим видеть (выше я привёл три варианта). Далее нужно будет задать размеры этой зоны (они тоже отмечены на рисунках). Помимо этого, нужно указать координату дна цилиндра по оси Z.

Тогда, имея все входные данные, можно построить желаемую зону и распределить по ней точки для анализа:

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

Можно заметить, что точки распределены не по всей поверхности, а по 1/6 её части. Это связанно с симметрией робота. При любом анализе можно рассматривать лишь 1/6 часть, отсчитываемую от плоскости YOZ в любую сторону на 60°.

Также в процессе расчёта полезно будет найти максимальный и минимальный углы θ, на который поворачивается рычаг. Это позволит нам при проектировании, возможно, смоделировать какие-то детали более массивными, где-то сэкономить, а также выбрать место для концевых выключателей, срабатывание которых будет давать системе управления информацию о текущем положении входных звеньев.

Реализация алгоритмов

Я реализовал алгоритм построения рабочей области в MATLAB. Алгоритм состоит из двух файлов. В первом вводятся все исходные данные и запускается функция построения и расчётов.

Файл, где задаются входные данные и запускается алгоритм
%Объявляем глобальные переменные характерных размеров робота и констант
global R_l R_r VM OQ cos120 sin120 cos240 sin240 %Размеры и константы
global minTheta QG phiMax varthetaMax %Ограничения
global WZ_D WZ_H WZ_Z WZ_a WZ_d WZ_h %Рабочая зона
global stepTheta %Шаг изменения углов
global minTheta_real maxTheta_real %Максимальный и минимальный углы рычагов
%при движении в реальной рабочей области
global minTheta_wish maxTheta_wish %Максимальный и минимальный углы рычагов
%при движении в желаемой рабочей области
global addSeg %Переменная, показывающая необходимость дополнительного
%сегмента в рабочей зоне
global status %Флаг свидетельствующий об удовлетворении условия нахождения
%желаемой рабочей зоны в реальной рабочей зоне
%поворота рычагов, которые могут быть с учёном ограничений
global Dg Hg hg %Габаритные размеры почившегося робота

%Вычисляем константы
cos120 = cosd(120);
sin120 = sind(120);
cos240 = cosd(240);
sin240 = sind(240);

%Задаём размеры робота [мм]
F = 270; %Длина стороны треугольника основания
f = 80; %Длина стороны треугольника платформы
R_l = 170;  %Длина рычагов (по осям)
R_r = 320;  %Длина штанг (по осям)

%Вычисляем радиусы вписанных окружностей
OQ = F*sqrt(3)/6; %Радиус окружности осей шарниров
VM = f*sqrt(3)/6; %Радиус окружности осей рычагов
%Задаём ограничения
minTheta = 135; %Минимальный угол поворота рычага
QG = 50; %Вынос основания
phiMax = 30; %Максимальный шарнирный угол
varthetaMax = 45; %Максимальный угол давления

%Задаём характеристики желаемой рабочей зоны
addSeg = 0; %Добавляемый к рабочей зоне сегмент. 0 - ничего; 1 - усечённый
%конус; %2 - часть сферы
WZ_D = 320; %Диаметр цилиндра рабочей зоны
WZ_H = 150; %Высота цилиндра рабочей зоны
WZ_Z = -390; %Координаты дна цилиндра рабочей зоны
WZ_a = WZ_D*sqrt(2)/2 %Сторона вписанного в основание цилиндра квадрата
WZ_d = 100; %Диаметр нижней окружности усечённого конуса
WZ_h = 50; %Высота добавляемой к РЗ части

%Шаг изменения угла при расчётах (рекомендуется от 1 до 8
stepTheta = 5;

%Вызываем функцию построения и расчётов
zoneBuilder;

%Выводим рассчитанные перемепенные
status %Если = 1, то желаемая РЗ вписывается в реальную
minTheta_real %Минимальный угол рычага в реальной РЗ
maxTheta_real %Максимальный угол рычага в реальной РЗ
if status
minTheta_wish %Минимальный угол рычага в желаемой РЗ
maxTheta_wish %Максимальный угол рычага в желаемой РЗ
Dg %Диаметр габаритного цилиндра
Hg %Высота габаритного цилиндра
end
hg %Растояние от основания до верха габаритного цилиндра

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

Алгоритм расчётов и построения
%Функция построения реальной и желаемой рабочих областей
%дельта-робота
function [] = zoneBuilder()
global R_l R_r VM OQ cos120 sin120 cos240 sin240 %Размеры и константы
global minTheta QG phiMax varthetaMax %Ограничения
global WZ_D WZ_H WZ_Z WZ_a WZ_d WZ_h %Рабочая зона
global stepTheta %Шаг изменения углов
global minTheta_real maxTheta_real %Максимальный и минимальный углы рычагов
%при движении в реальной рабочей области
global minTheta_wish maxTheta_wish %Максимальный и минимальный углы рычагов
%при движении в желаемой рабочей области
global addSeg %Переменная, показывающая необходимость дополнительного
%сегмента в рабочей зоне
global status %Флаг свидетельствующий об удовлетворении условия нахождения
%желаемой рабочей зоны в реальной рабочей зоне
%поворота рычагов, которые могут быть с учёном ограничений
global Dg Hg hg %Габаритные размеры почившегося робота
maxTheta = 270; %Максимальный угол для перебора

%Задаём переменные максимальных и минимальных углов поворота рычагов,
%которые могут быть с учёном ограничений
minTheta_real = maxTheta;
maxTheta_real = minTheta;

minTheta_wish = maxTheta;
maxTheta_wish = minTheta;


%Подготовительные вычисления
gammaMin = asind(QG/R_l);
gammaMax = 180;
%Создаём вектор с одной нулевой точкой (потом её удалим)
V_RZ = [0, 0, 0];

%Создаём цикл перебора всех возможных комбинаций углов поворота рычагов
for Theta1 = minTheta:stepTheta:maxTheta
    for Theta2 = minTheta:stepTheta:maxTheta
        for Theta3 = minTheta:stepTheta:maxTheta
            %Вызываем функцию, решающую ПЗК
            [V] = PZK(Theta1, Theta2, Theta3);
            X_V = V(1); Y_V = V(2); Z_V = V(3);
            %Вычисляем координаты точек в системах координат XOY
            %X120Y120Z120 и X240Y240Z240
            X_Q1 = 0;
            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;
                X_Q2_120 = 0;
                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;
                    X_Q3_240 = 0;
                    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;
            %Собираем это в вектора
            Q1 = [X_Q1, Y_Q1, Z_Q1];
            L1 = [X_L1, Y_L1, Z_L1];
            M1 = [X_M1, Y_M1, Z_M1];
            V_120 = [X_V_120, Y_V_120, Z_V_120];
            Q2_120 = [X_Q2_120, Y_Q2_120, Z_Q2_120];
            L2_120 = [X_L2_120, Y_L2_120, Z_L2_120];
            M2_120 = [X_M2_120, Y_M2_120, Z_M2_120];
            V_240 = [X_V_240, Y_V_240, Z_V_240];
            Q3_240 = [X_Q3_240, Y_Q3_240, Z_Q3_240];
            L3_240 = [X_L3_240, Y_L3_240, Z_L3_240];
            M3_240 = [X_M3_240, Y_M3_240, Z_M3_240];
            %Вычисляем координаты точек N1 N2 N3
            N1 = [L1(1); M1(2); V(3)];
            N2_120 = [L2_120(1); M2_120(2); V_120(3)];
            N3_240 = [L3_240(1); M3_240(2); V_240(3)];
            %------Проверяем ограничения------%
            % 1 Проверяем отсутствие пересечений
            gamma1 = 180-acosd(-L1(3)/R_l)-asind((N1(2)-L1(2))/R_r);
            gamma2 = 180-acosd(-L2_120(3)/R_l)-asind((N2_120(2)-L2_120(2))/R_r);
            gamma3 = 180-acosd(-L3_240(3)/R_l)-asind((N3_240(2)-L3_240(2))/R_r);
            if (gamma1 < gammaMax) && (gamma1 > gammaMin) && (gamma2 < gammaMax) && (gamma2 > gammaMin) && (gamma3 < gammaMax) && (gamma3 > gammaMin)
                % 2 Проверяем, могут ли трёхподвижные шарниры позволить механизму принять
                % такое положение
                varphi1 = asind(abs(V(1))/R_r);
                varphi2 = asind(abs(V_120(1))/R_r);
                varphi3 = asind(abs(V_240(1))/R_r);
                if (varphi1<phiMax) && (varphi2<phiMax) && (varphi3<phiMax)
                    % 3 Проверяем, не превышают ли углы давления заданное
                    % максимальное значение
                    p1 = [M1(1)-L1(1); M1(2)-L1(2); M1(3)-L1(3)];
                    q1 = [0; L1(3)-Q1(3); Q1(2)-L1(2)];
                    vartheta1 = acosd(abs(p1(1)*q1(1)+p1(2)*q1(2)+p1(3)*q1(3))/(sqrt(p1(1)^2+p1(2)^2+p1(3)^2)*sqrt(q1(1)^2+q1(2)^2+q1(3)^2)));
                    p2 = [M2_120(1)-L2_120(1); M2_120(2)-L2_120(2); M2_120(3)-L2_120(3)];
                    q2 = [0; L2_120(3)-Q2_120(3); Q2_120(2)-L2_120(2)];
                    vartheta2 = acosd(abs(p2(1)*q2(1)+p2(2)*q2(2)+p2(3)*q2(3))/(sqrt(p2(1)^2+p2(2)^2+p2(3)^2)*sqrt(q2(1)^2+q2(2)^2+q2(3)^2)));
                    p3 = [M3_240(1)-L3_240(1); M3_240(2)-L3_240(2); M3_240(3)-L3_240(3)];
                    q3 = [0; L3_240(3)-Q3_240(3); Q3_240(2)-L3_240(2)];
                    vartheta3 = acosd(abs(p3(1)*q3(1)+p3(2)*q3(2)+p3(3)*q3(3))/(sqrt(p3(1)^2+p3(2)^2+p3(3)^2)*sqrt(q3(1)^2+q3(2)^2+q3(3)^2)));
                    if (vartheta1<varthetaMax) && (vartheta2<varthetaMax) && (vartheta3<varthetaMax)
                        %Если все условия выполнены - добавляем точку в
                        %вектор облака точек реальной РЗ
                        V_RZ = cat(1, V_RZ, V);
                        %Находим максимальный и минимальный углы поворота рычагов,
                        %которые могут быть с учёном ограничений
                        minTheta_real = min([minTheta_real, Theta1, Theta2, Theta3]);
                        maxTheta_real = max([maxTheta_real, Theta1, Theta2, Theta3]);
                    end
                end
            end
        end
    end
end

%Удаляем первую нулевую точку
V_RZ(1,:) = [];
%Находим какие точки в нашем облаке являются граничными и создаём
%группы по 3 точки, которые будем соединять треугольниками (для
%образования поверхности)
[K] = boundary(V_RZ);

%Находим точки для построения цилиндра
%Крышка
t = linspace(0, 2*pi, 50);
Xo1 = (WZ_D/2) * cos(t);
Yo1 = (WZ_D/2) * sin(t);
Zo1 = ones(1, 50)*(WZ_Z+WZ_H);
Zo2 = ones(1, 50)*WZ_Z;
%Цилиндрическая поверхность
[Xc,Yc,Zc] = cylinder((WZ_D/2), 50);
Zc = Zc*WZ_H+WZ_Z;

%Находим точки для построения параллелепипеда
Xp = [0 WZ_a WZ_a 0 0 WZ_a WZ_a 0];
Yp = [0 0 WZ_a WZ_a 0 0 WZ_a WZ_a] ;
Zp = Xp'*Xp*Yp'*Yp/WZ_a^2/WZ_a^2*WZ_H/2 + WZ_Z;
Xp = Xp - WZ_a/2;
Yp = Yp - WZ_a/2;

%Отрисовка рабочих зон

%Отображаем реальную РЗ
p1 = trisurf(K,V_RZ(:,1),V_RZ(:,2),V_RZ(:,3),'Facecolor','red', 'FaceAlpha',0.2);
hold('on');
%Отображаем крышку и боковую поверхность цилиндра
p2 = patch(Xo1, Yo1, Zo1, 'c', 'FaceAlpha',0.5);
p3 = surf(Xc,Yc,Zc, 'FaceColor', 'c',  'FaceAlpha',0.5);
%Отображаем параллелепипед
p4 = surf(Xp, Yp, Zp, 'FaceColor', 'g', 'FaceAlpha', 0.2);
p4.Visible = 0; %Скрываем
%Отображаем дополнительные сегменты, если нужно
if addSeg == 0
    %Дно цилиндра
    p5 = patch(Xo1, Yo1, Zo2, 'c', 'FaceAlpha',0.5);
end
if addSeg == 1
    %Дно усечённого конуса
    t = linspace(0, 2*pi, 50);
    Xo2 = (WZ_d/2) * cos(t);
    Yo2 = (WZ_d/2) * sin(t);
    p6 = patch(Xo2, Yo2, Zo2-WZ_h, 'c', 'FaceAlpha',0.5);
    %Коническая поверхность
    [Xco,Yco,Zco] = cylinder([(WZ_d/2), (WZ_D/2)], 50);
    Zco = Zco*WZ_h+WZ_Z-WZ_h;
    p7 = surf(Xco,Yco,Zco, 'FaceColor', 'c',  'FaceAlpha',0.5);
end
if addSeg == 2
    %Сферическая поверхность
    thetta = linspace(pi-asin((WZ_D/2)/(WZ_d/2)), pi, 50);
    phi = linspace(0, 2*pi, 50);
    [THETTA, PHI] = meshgrid(thetta, phi);
    Xsph = (WZ_d/2)*sin(THETTA).*cos(PHI);
    Ysph = (WZ_d/2)*sin(THETTA).*sin(PHI);
    Zsph = (WZ_Z+(WZ_d/2-WZ_h))+(WZ_d/2)*cos(THETTA);
    p8 = surf(Xsph,Ysph,Zsph, 'FaceColor', 'c',  'FaceAlpha',0.5);
end
%Настраиваем отображение
hold('off');
axis('equal');
xlabel('x, мм');
ylabel('y, мм');
zlabel('z, мм');
title('Рабочие зоны механизма');
%legend('Реальная РЗ', 'Требуемая РЗ');
view([-30 , 20]);
grid('on');

%Проверяем, подходит ли данная конфигурация робота под желаемую рабочую
%зону
%Формируем массив точек для проверки
%Точки крышки цилиндра
[t, r] = meshgrid(linspace(-pi/2, -pi/2-pi/3, 10), linspace(0, WZ_D/2,10));
[Xtc, Ytc] = pol2cart(t,r);
Ztc = ones(10, 10)*(WZ_Z+WZ_H);
%Точки боковой поверхности цилиндра
[t,r] = meshgrid(linspace(-pi/2, -pi/2-pi/3, 10), WZ_D/2);
[Xmv, Ymv] = pol2cart(t,r);
mv = linspace(WZ_Z, WZ_Z+WZ_H, 10);
for k = 2:9
    Xss((k-1)*10-9:(k-1)*10) = Xmv;
    Yss((k-1)*10-9:(k-1)*10) = Ymv;
    Zss((k-1)*10-9:(k-1)*10) = ones(1, 10)*mv(k);
end
if addSeg == 0
    %Точки дна цилиндра
    [t, r] = meshgrid(linspace(-pi/2, -pi/2-pi/3, 10), linspace(0, WZ_D/2,10));
    [Xbc, Ybc] = pol2cart(t,r);
    Zbc = ones(10, 10)*(WZ_Z);
    %Формируем итоговый вектор точек
    PointsToBeChecked_X = [reshape(Xtc',1,[]), reshape(Xss',1,[]), reshape(Xbc',1,[])];
    PointsToBeChecked_Y = [reshape(Ytc',1,[]), reshape(Yss',1,[]), reshape(Ybc',1,[])];
    PointsToBeChecked_Z = [reshape(Ztc',1,[]), reshape(Zss',1,[]), reshape(Zbc',1,[])];
end
if addSeg == 1
    %Точки дна конуса
    [t, r] = meshgrid(linspace(-pi/2, -pi/2-pi/3, 10), linspace(0, WZ_d/2,10));
    [Xbco, Ybco] = pol2cart(t,r);
    Zbco = ones(10, 10)*(WZ_Z-WZ_h);
    %Точки боковой поверхности конуса
    mv = linspace(WZ_Z-WZ_h, WZ_Z, 10);
    mvD = linspace(WZ_d, WZ_D, 10);
    for k = 2:10
        [t,r] = meshgrid(linspace(-pi/2, -pi/2-pi/3, 10), mvD(k)/2);
        [Xmv, Ymv] = pol2cart(t,r);
        Xssco((k-1)*10-9:(k-1)*10) = Xmv;
        Yssco((k-1)*10-9:(k-1)*10) = Ymv;
        Zssco((k-1)*10-9:(k-1)*10) = ones(1, 10)*mv(k);
    end
    %Формируем итоговый вектор точек
    PointsToBeChecked_X = [reshape(Xtc',1,[]), reshape(Xss',1,[]), reshape(Xbco',1,[]), reshape(Xssco',1,[])];
    PointsToBeChecked_Y = [reshape(Ytc',1,[]), reshape(Yss',1,[]), reshape(Ybco',1,[]), reshape(Yssco',1,[])];
    PointsToBeChecked_Z = [reshape(Ztc',1,[]), reshape(Zss',1,[]), reshape(Zbco',1,[]), reshape(Zssco',1,[])];
end
if addSeg == 2
    %Точки сферы
    thetta = linspace(pi-asin((WZ_D/2)/(WZ_d/2)), pi, 10);
    phi = linspace(-pi/2, -pi/2-pi/3, 10);
    [THETTA, PHI] = meshgrid(thetta, phi);
    Xsph = (WZ_d/2)*sin(THETTA).*cos(PHI);
    Ysph = (WZ_d/2)*sin(THETTA).*sin(PHI);
    Zsph = (WZ_Z+(WZ_d/2-WZ_h))+(WZ_d/2)*cos(THETTA);
    %Формируем итоговый вектор точек
    PointsToBeChecked_X = [reshape(Xtc',1,[]), reshape(Xss',1,[]), reshape(Xsph',1,[])];
    PointsToBeChecked_Y = [reshape(Ytc',1,[]), reshape(Yss',1,[]), reshape(Ysph',1,[])];
    PointsToBeChecked_Z = [reshape(Ztc',1,[]), reshape(Zss',1,[]), reshape(Zsph',1,[])];
end

% %Показываем точки для анализа
% hold('on');
% plot3(PointsToBeChecked_X,PointsToBeChecked_Y, PointsToBeChecked_Z, '.r');
% hold('off');
    
%Проверяем каждую точку на условие нахождения её в реальной рабочей зоне
%Задаём начальный вектор статуса (ни одна точка не подходит)
stt(1:size(PointsToBeChecked_X, 2)) = 0;
for k=1:size(PointsToBeChecked_X, 2)
    %Вызываем функцию, решающую ПЗК
    [vectTheta] = OZK(PointsToBeChecked_X(k), PointsToBeChecked_Y(k), PointsToBeChecked_Z(k));
    Theta1 = vectTheta(1); Theta2 = vectTheta(2); Theta3 = vectTheta(3);
    X_V = PointsToBeChecked_X(k); Y_V = PointsToBeChecked_Y(k); Z_V = PointsToBeChecked_Z(k);
    if imag(vectTheta) == [0, 0, 0]
        %Вычисляем координаты точек в системах координат XOY
        %X120Y120Z120 и X240Y240Z240
        X_Q1 = 0;
        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;
        X_Q2_120 = 0;
        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;
        X_Q3_240 = 0;
        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;
        %Собираем это в вектора
        Q1 = [X_Q1, Y_Q1, Z_Q1];
        L1 = [X_L1, Y_L1, Z_L1];
        M1 = [X_M1, Y_M1, Z_M1];
        V_120 = [X_V_120, Y_V_120, Z_V_120];
        Q2_120 = [X_Q2_120, Y_Q2_120, Z_Q2_120];
        L2_120 = [X_L2_120, Y_L2_120, Z_L2_120];
        M2_120 = [X_M2_120, Y_M2_120, Z_M2_120];
        V_240 = [X_V_240, Y_V_240, Z_V_240];
        Q3_240 = [X_Q3_240, Y_Q3_240, Z_Q3_240];
        L3_240 = [X_L3_240, Y_L3_240, Z_L3_240];
        M3_240 = [X_M3_240, Y_M3_240, Z_M3_240];
        %Вычисляем координаты точек N1 N2 N3
        N1 = [L1(1); M1(2); V(3)];
        N2_120 = [L2_120(1); M2_120(2); V_120(3)];
        N3_240 = [L3_240(1); M3_240(2); V_240(3)];
        %------Проверяем ограничения------%
        % 1 Проверяем отсутствие пересечений
        gamma1 = 180-acosd(-L1(3)/R_l)-asind((N1(2)-L1(2))/R_r);
        gamma2 = 180-acosd(-L2_120(3)/R_l)-asind((N2_120(2)-L2_120(2))/R_r);
        gamma3 = 180-acosd(-L3_240(3)/R_l)-asind((N3_240(2)-L3_240(2))/R_r);
        if (gamma1 < gammaMax) && (gamma1 > gammaMin) && (gamma2 < gammaMax) && (gamma2 > gammaMin) && (gamma3 < gammaMax) && (gamma3 > gammaMin)
            % 2 Проверяем, могут ли трёхподвижные шарниры позволить механизму принять
            % такое положение
            varphi1 = asind(abs(V(1))/R_r);
            varphi2 = asind(abs(V_120(1))/R_r);
            varphi3 = asind(abs(V_240(1))/R_r);
            if (varphi1<phiMax) && (varphi2<phiMax) && (varphi3<phiMax)
                % 3 Проверяем, не превышают ли углы давления заданное
                % максимальное значение
                p1 = [M1(1)-L1(1); M1(2)-L1(2); M1(3)-L1(3)];
                q1 = [0; L1(3)-Q1(3); Q1(2)-L1(2)];
                vartheta1 = acosd(abs(p1(1)*q1(1)+p1(2)*q1(2)+p1(3)*q1(3))/(sqrt(p1(1)^2+p1(2)^2+p1(3)^2)*sqrt(q1(1)^2+q1(2)^2+q1(3)^2)));
                p2 = [M2_120(1)-L2_120(1); M2_120(2)-L2_120(2); M2_120(3)-L2_120(3)];
                q2 = [0; L2_120(3)-Q2_120(3); Q2_120(2)-L2_120(2)];
                vartheta2 = acosd(abs(p2(1)*q2(1)+p2(2)*q2(2)+p2(3)*q2(3))/(sqrt(p2(1)^2+p2(2)^2+p2(3)^2)*sqrt(q2(1)^2+q2(2)^2+q2(3)^2)));
                p3 = [M3_240(1)-L3_240(1); M3_240(2)-L3_240(2); M3_240(3)-L3_240(3)];
                q3 = [0; L3_240(3)-Q3_240(3); Q3_240(2)-L3_240(2)];
                vartheta3 = acosd(abs(p3(1)*q3(1)+p3(2)*q3(2)+p3(3)*q3(3))/(sqrt(p3(1)^2+p3(2)^2+p3(3)^2)*sqrt(q3(1)^2+q3(2)^2+q3(3)^2)));
                if (vartheta1<varthetaMax) && (vartheta2<varthetaMax) && (vartheta3<varthetaMax)
                    %Если все условия выполнены - добавляем точку в
                    %вектор облака точек реальной РЗ
                    stt(k) = 1;
                    %Находим максимальный и минимальный углы поворота рычагов,
                    %которые могут быть с учёном ограничений
                    minTheta_wish = min([minTheta_wish, Theta1, Theta2, Theta3]);
                    maxTheta_wish = max([maxTheta_wish, Theta1, Theta2, Theta3]);
                end
            end
        end
    end
end
if min(stt) == 1
    status = 1;
    Dg = (OQ+R_l)*2;
    hg = R_l*sind(180-minTheta_wish);
    if hg < 0
        hg = 0;
    end
    Hg = hg - WZ_Z;
else
    status = 0;
end
end

Показать максимально наглядно рабочую зону помогает встроенная функция «boundary», которая среди предложенных точек выберет только те, которые являются граничными для трёхмерной области. Достоинством данной функции является возможность определять не только выпуклые, но и вогнутые участки граничной поверхности, что в рамках решаемой задачи необходимо.

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

Бонусом накинем алгоритм для построения робота:

Вот он
%Строит схаматично дельта-робота на графике
function [] = drawDelta(dim, V, Theta)
f = dim(1);    %Длина стороны треугольника платформы
F = dim(2);    %Длина стороны треугольника основания
R_l = dim(3);  %Длина рычагов (по осям)
R_r = dim(4);  %Длина штанг (по осям)
X_V = V(1); Y_V = V(2); Z_V = V(3); %Координаты точки V
Theta1 = Theta(1); Theta2 = Theta(2); Theta3 = Theta(3); %Углы поворота рычагов

%Находим все точки для построения механизма
%Подготовительные действия
global cos120 sin120 cos240 sin240
VM = f*sqrt(3)/6;
OQ = F*sqrt(3)/6;
%Находим крайние точки рычагов и плеч
X_Q1 = 0;
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_Q2 = -Y_Q1*sin120;
    Y_Q2 = Y_Q1*cos120;
    Z_Q2 = 0;
    X_L2 = (OQ + R_l*sind(Theta2-90))*sin120;
    Y_L2 = -(OQ + R_l*sind(Theta2-90))*cos120;
    Z_L2 =  R_l*cosd(Theta2-90);
    X_M2 = X_V - (-VM)*sin120;
    Y_M2 = Y_V + (-VM)*cos120;
    Z_M2 = Z_V;
        X_Q3 = -Y_Q1*sin240;
        Y_Q3 = Y_Q1*cos240;
        Z_Q3 = 0;
        X_L3 = (OQ + R_l*sind(Theta3-90))*sin240;
        Y_L3 = -(OQ + R_l*sind(Theta3-90))*cos240;
        Z_L3 =  R_l*cosd(Theta3-90);
        X_M3 = X_V - (-VM)*sin240;
        Y_M3 = Y_V + (-VM)*cos240;
        Z_M3 = Z_V;
%Находим точки треугольника основания
A = [-F/2, -OQ, 0];
B = [F/2, A(2), 0];
C = [0, F/sqrt(3), 0];
%Находим точки треугольника каретки
An = [X_V-(f/2), Y_V-VM, Z_V];
Bn = [X_V+(f/2), Y_V-VM, Z_V];
Cn = [X_V, Y_V+f/sqrt(3), Z_V];
%Формирование выходных векторов для построения
BaseTri = [A(1) B(1) C(1); A(2) B(2) C(2); A(3) B(3) C(3)];
CarTri = [An(1) Bn(1) Cn(1); An(2) Bn(2) Cn(2); An(3) Bn(3) Cn(3)];
Pl1 = [X_Q1 X_L1 X_M1; Y_Q1 Y_L1 Y_M1; Z_Q1 Z_L1 Z_M1];
Pl2 = [X_Q2 X_L2 X_M2; Y_Q2 Y_L2 Y_M2; Z_Q2 Z_L2 Z_M2];
Pl3 = [X_Q3 X_L3 X_M3; Y_Q3 Y_L3 Y_M3; Z_Q3 Z_L3 Z_M3];

%Построение робота
cla;
patch(BaseTri(1, :), BaseTri(2, :), BaseTri(3, :), 'c', 'FaceAlpha',0.7);
hold('on');
patch(CarTri(1, :), CarTri(2, :), CarTri(3, :), 'm', 'FaceAlpha',0.7);
plot3(Pl1(1, :), Pl1(2, :), Pl1(3, :), 'LineWidth', 1.5, 'Color', 'b');
plot3(Pl2(1, :), Pl2(2, :), Pl2(3, :), 'LineWidth', 1.5, 'Color', 'b');
plot3(Pl3(1, :), Pl3(2, :), Pl3(3, :), 'LineWidth', 1.5, 'Color', 'b');
xlabel('x, мм');
ylabel('y, мм');
zlabel('z, мм');
title('Вид робота');
view([ -30 , 20 ]);
axis('equal');
grid('on');
hold('off');
end

Ему на вход нужна дать размеры работа, координаты центра исполнительно звена и углы, соответствующие этому положению (решение обратной задачи кинематики).

Можно, конечно, обернуть всё это дело в приложение с графическим интерфейсом:

Но его я пока дать не готов.

Итог

Я постарался изложить свой взгляд на построение рабочей зоны дельта-робота, свой взгляд на выбор длин звеньев этого механизма. Жалующие легко соберут (но небольшой навык работы в MATLAB всё же нужен) все алгоритмы, которые я дал, в комплект файлов для расчётов и построений.

В следующей статье будем разбирать вопрос выбора приводов. Будет не очень приятная математика, но я постараюсь сгладить это своими пояснениями. До новых встреч.

Комментарии (4)


  1. LuggerMan
    13.10.2021 13:12
    #23586600

    Приложухой заинтриговал, стервец xD! Когда?


    1. multiengineer Автор
      13.10.2021 13:14
      #23586614

      Увы, сравнительно не скоро, через 3-6 месяцев. Мне самому отладить нужно и ряд вопросов решить. Раньше никак, но всё будет в лучшем виде.


  1. SnakeSolid
    13.10.2021 20:51
    #23588478

    Возможно глупый вопрос - рассматривали ли вы передачу с линейными приводами, что-то вроде принтера "Anet A4"? Интуитивно, кажется более логичным поставить три линейных привода в углах основания и перемещать ими платформу. Расчет в этом случае упрощается до определения расстояния от угла основания до угла платформы. И, теоретически, это позволит передавать на платформу большее усилие, чем описанный вариант.


    1. multiengineer Автор
      13.10.2021 20:55
      #23588492

      Нет, не рассматривал. Это уже другая кинематика, другой механизм. Линейные дельты, как их ещё называют, тоже интересный параллельный механизм, но я решил поработать над классическим. В целом, у линейной дельты много своих достоинств.