Кинематика дельта-робота

Дельта-робот


В далёком 2009 году я загорелся идеей постройки собственного промышленного робота, который мог бы делать что-то полезное (а именно — сортировать мелкие детали на конвейере). Сразу скажу, что робота я построил (результат вы видите на заглавном фото), а заодно, в качестве побочного продукта, написал небольшую статью о кинематике дельта-роботов на форуме TrossenRobotics — американского продавца наборов из деталей для роботов. Они как раз проводили в то время какой-то конкурс для авторов. Конкурс я, разумеется, не выиграл, но статья на английском осталась. Несколько раз я порывался перевести её на родной язык, однако завершить начатое удалось только сейчас.

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

Что такое дельта-робот


Дельта-робота (wiki) придумал в начале 1980-х швейцарский учёный Реймонд Клавель, ниже приведена иллюстрация из оригинального патента US4976582 на «Устройство для перемещения и позиционирования элемента в пространстве»:

Конструкция дельта-робота


Робот состоит из двух платформ: неподвижно закреплённого верхнего основания (1) и небольшой подвижной платформы (8), соединённых тремя рычагами. Каждый рычаг состоит из двух частей: верхнее плечо (4) жёстко соединено с двигателем (3), расположенным на верхнем основании, а нижнее представляет собой параллелограмм (5), в углах которого установлены т.н. универсальные шарниры (6, 7) (wiki), которые позволяют углам изменяться. Каждый параллелограмм соединён с верхним рычагом шарниром (16) таким образом, чтобы его верхняя сторона всегда оставалась перпендикулярной своему рычагу и параллельной плоскости верхнего основания. Благодаря этому подвижная платформа робота, прикреплённая к нижним сторонам параллелограммов также будет всегда параллельной верхнему основанию. Управлять положением платформы мы сможем, изменяя угол поворота верхних рычагов относительно основания робота при помощи двигателей.

В центре нижней платформы (8) крепится т.н. рабочий орган (в английском языке употребляют термин end effector) робота (9). Это может быть манипулятор, захватывающее устройство или, например, экструдер в случае 3D принтера. Дополнительно может использоваться ещё один двигатель (11), который обеспечивает вращение рабочего органа через штангу (14).

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

Формулировка задачи


Чтобы построить своего собственного дельта-робота, необходимо научиться решать две задачи. В первой ситуации нам известна позиция, в которую мы хотим переместить манипулятор нашего робота (например, мы хотим схватить печенье, которое находится на конвейере в точке с координатами (x, y, z). Для этого нам требуется определить величины углов, на которые мы должны повернуть двигатели, связанные с рычагами робота, чтобы установить его в правильное положение для захвата. Процедура определения этих углов называется обратной (в некоторых русскоязычных источниках употребляется слово «инверсной») кинематической задачей.
Во второй ситуации нам известны углы, на которые повёрнуты управляющие моторы робота (если мы используем сервомоторы, то углы легко можно узнать, считав показания с датчиков углов поворота), и мы хотим найти положение платформы робота в пространстве (например, чтобы скорректировать его позицию). Это — прямая кинематическая задача.

Формализуем обе задачи. И неподвижное основание робота, и его движущуюся платформу можно представить в виде равносторонних треугольников: на схеме ниже они закрашены зелёным и розовым цветами соответственно. Углы поворота рычагов робота относительно плоскости основания (они же — углы поворота моторов) обозначены как Ѳ1, Ѳ2 и Ѳ3, а координаты точки Е0, расположенной в центре подвижной платформы и в которой в реальной жизни будет закреплён манипулятор нашего робота — как (x0, y0, z0).

Схема дельта-робота


Получается, что мы должны придумать две функции:

  • finverse(x0, y0, z0) → (Ѳ1, Ѳ2, Ѳ3) для решения обратной кинематической задачи и
  • fforward1, Ѳ2, Ѳ3) → (x0, y0, z0) для решения прямой кинематической задачи.

Обратная кинематика


Зададим несколько ключевых параметров, которые определяются геометрическими размерами нашего робота:

Обозначения размеров дельта-робота


Обозначим длину стороны верхнего основания f, сторону нижней платформы e, длину верхнего плеча рычага rf и длину нижнего плеча (длинной стороны параллелограмма) re. Для вычислений выберем систему координат с точкой отсчёта, совпадающей с геометрическим центром верхнего треугольника. Ось Z направим вверх, таким образом, z-координата подвижной платформы всегда будет отрицательной.

Конструкция робота подразумевает, что рычаг F1J1 (см. рисунок ниже) может вращаться лишь в плоскости YZ, описывая при этом окружность радиусом rf с центром в точке F1 (именно в этом месте он прикреплён к двигателю). В отличие от F1, в узлах J1 и E1 используются универсальные шарниры, благодаря которым плечо E1J1 может свободно вращаться относительно E1, описывая сферу радиусом re с центром в точке E1.



Пересечением этой сферы и плоскости YZ является окружность с центром в точке E’1 радиусом E’1J1, где точка E’1 находится как проекция точки E1 на плоскость YZ. Тогда точка J1 будет находиться на пересечении двух окружностей с центрами в точках E’1 и F1, причём радиусы этих окружностей мы можем определить. Тут есть небольшая тонкость: окружности пересекаются в двух точках, но нас интересует только одна из них — с меньшим значением координаты y, поскольку мы хотим, чтобы рычаги робота всегда торчали «локтями» наружу. Определив таким образом координаты точки J1, мы легко сможем найти угол интересующий нас угол Ѳ1.

Для удобства восприятия ниже показана проекция нашей трёхмерной картинки на плоскость YZ:



Нижняя платформа является равносторонним треугольником, центром которого является точка E0(x0, y0, z0). Значит, расстояние

E_0E_1={e\over2}\tan(30^o)={e\over2\sqrt3},

что даёт нам следующие координаты точки E1 и её проекции E’1 на плоскость YZ:

E_1(x_0, y_0-{e\over2\sqrt3}, z_0) \Rightarrow E'_1(0, y_0-{e\over2\sqrt3}, z_0)

Расстояние E1E’1=x0, тогда, согласно теореме Пифагора,

E'_1J_1=\sqrt{E_1J_1^2-E_1E'_1^2}=\sqrt{r_e^2-x_0^2}


Поскольку верхняя платформа также является равносторонним треугольником, то координаты у точки F1 будут

F_1(0, {-f\over{2\sqrt3}}, 0)

Чтобы найти координаты точки J1, являющейся пересечением двух окружностей, надо решить систему уравнений:

\begin{equation*}
 \begin{cases}
   (y_{J_1}-y_{F_1})^2+(z_{J_1}-z_{F_1})^2 = r_f^2
   \\
   (y_{J_1}-y_{E'_1})^2+(z_{J_1}-z_{E'_1})^2 = r_e^2-x_0
 \end{cases}
\end{equation*}


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

\begin{equation*}
 \begin{cases}
   (y_{J_1}+{f\over{2\sqrt3}})^2+z_{J_1}^2 = r_f^2
   \\
   (y_{J_1}-y_0+{e\over{2\sqrt3}})^2+(z_{J_1}-z_0)^2 = r_e^2-x_0
 \end{cases}
\end{equation*}


Если раскрыть скобки и вычесть одно уравнение из другого, можно линейным образом выразить z-координату точки J1 через y-координату, после чего, подставив её во второе уравнение, получим обычное квадратное уравнение относительно y, из двух решений которого выберем наименьшее (об этом мы говорили выше). А получив таким образом координаты точки J1, найдём и угол

\theta_1=\arctan\left({z_{J_1}\over{y_{F_1}-y_{J_1}}}\right)


Все выражения получились достаточно простыми благодаря удачному выбору системы координат: плечо рычага F1J1 всегда движется в плоскости YZ, поэтому мы можем просто не учитывать координату x. Чтобы сохранить это преимущество при нахождении двух оставшихся углов Ѳ2 и Ѳ3, воспользуемся симметрией конструкции дельта-робота. Сначала повернём нашу систему координат на 120° против часовой стрелки в плоскости XY вокруг оси Z:



Мы получили новую систему координат X’Y’Z’, и в этой новой системе мы можем воспользоваться нашими готовыми формулами для нахождения угла Ѳ2. Единственная тонкость заключается в том, что мы должны предварительно пересчитать координаты точки E0 в новой системе отсчёта. Это легко сделать при помощи известной формулы (преобразования при повороте системы вокруг начала координат), показанной на рисунке выше. Для нахождения угла Ѳ3 необходимо будет также повернуть исходную систему отсчёта, но теперь уже по часовой стрелке. Этот приём очень удобно реализуется в виде программы: достаточно написать функцию для вычисления угла Ѳ в плоскости YZ, а затем вызвать её три раза для каждого из углов и систем отсчёта.

Прямая кинематика


Попробуем решить обратную задачу: теперь нам известны углы Ѳ1, Ѳ2 и Ѳ3, и мы хотим найти координаты (x0, y0, z0) точки E0, расположенной в центре подвижной платформы нашего робота. Зная углы, мы легко можем найти координаты точек J1, J2 и J3 (см. рисунок ниже). Плечи рычагов J1E1, J2E2 и J3E3 могут свободно вращаться вокруг точек J1, J2 и J3 соответственно, образуя в пространстве три сферы с радиусами re.



Воспользуемся хитрым приёмом: сместим центры каждой из этих сфер из точек J1, J2, J3 в плоскости XY в направлении оси Z, используя вектора смещения E1E0, E2E0 и E3E0 соответственно (на рисунке они показаны в виде красных стрелочек). После этого преобразования окажется, что все три сферы пересекаются в точке E0, как показано на рисунке ниже:



Получается, что для определения координат (x0, y0, z0) точки E0 мы должны найти точку пересечения трёх сфер, радиусы и координаты центров которых нам известны. Иными словами, нам надо решить систему из трёх уравнений, описывающих трёхмерные сферы:

(x-x_i)^2 + (y-y_i)^2 + (z-z_i)^2 = r_e^2,

где (xi, yi, zi) — координаты центров сфер J’1, J’2 и J’3, которые можно найти следующим образом:



Ниже для сокращения записи я буду использовать в качестве координат точек J’1, J’2 и J’3 обозначения (x1, y1, z1), (x2, y2, z2) и (x3, y3, z3) соответственно. Также хочу обратить внимание, что x1=0 (поскольку точка J’1 находится в плоскости YZ). Получаем следующую систему уравнений:

\begin{equation*}
 \begin{cases}
   x^2+(y-y_1)^2+(z-z_1)^2 = r_e^2
   \\
   (x-x_2)^2+(y-y_2)^2+(z-z_2)^2 = r_e^2
   \\
   (x-x_3)^2+(y-y_3)^2+(z-z_3)^2 = r_e^2
 \end{cases}
\end{equation*}
\Rightarrow
\begin{equation*}
 \begin{cases}
   x^2+y^2+z^2-2y_1y-2z_1z=r_e^2-y_1^2-z_1^2
   \\
   x^2+y^2+z^2-2x_2x-2y_2y-2z_2z=r_e^2-x_2^2-y_2^2-z_2^2
   \\
   x^2+y^2+z^2-2x_3x-2y_3y-2z_3z=r_e^2-x_3^2-y_3^2-z_3^2
 \end{cases}
\end{equation*}


Введем обозначение

w_i=x_i^2+y_i^2+z_i^2


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

\begin{equation*}
 \begin{cases}
   x_2x+(y_1-y_2)y+(z_1-z_2)z=(w_1-w_2)/2
   \\
   x_3x+(y_1-y_3)y+(z_1-z_3)z=(w_1-w_3)/2
   \\
   (x_2-x_3)x+(y_2-y_3)y+(z_2-z_3)z=(w_2-w_3)/2
 \end{cases}
\end{equation*}


Вычитая из первого уравнения второе (сократив, таким образом, y) и из второго третье (сократив x), сможем выразить x и y через z:

x=a_1z+b_1\qquad y=a_2z+b_2


a_1={1\over{d}}\left[(z_2-z_1)(y_3-y_1)-(z_3-z_1)(y_2-y_1)\right] \qquad a_2=-{1\over{d}}\left[(z_2-z_1)x_3-(z_3-z_1)x_2\right]


b_1=-{1\over{2d}}\left[(w_2-w_1)(y_3-y_1)-(w_3-w_1)(y_2-y_1)\right] \qquad b_2={1\over{2d}}\left[(w_2-w_1)x_3-(w_3-w_1)x_2\right]


d=(y_2-y_1)x_3-(y_3-y_1)x_2


Теперь, подставив x и y, выраженные через z, в уравнение для первой окружности (с центром в точке J’1), получим:

(a_1^2+a_2^2+1)z^2+2(a_1+a_2(b_2-y_1)-z_1)z+(b_1^2+(b_2-y_1)^2+z_1^2-r_e^2)=0


Осталось решить это квадратное уравнение (стандартным образом, через дискриминант), чтобы найти z (мы помним, что надо выбирать наименьший из двух z!), а через него x и y.

Примеры исходного кода


Ниже приведены примеры функций для расчёта кинематики дельта-робота на языке С. Названия переменных соответствуют обозначениям, используемым в статье, углы theta1, theta2 и theta3 указываются в градусах.

// размеры робота
// (обозначения см. на схеме)
const float e = 115.0;     // сторона подвижной платформы
const float f = 457.3;     // сторона неподвижного основания
const float re = 232.0;
const float rf = 112.0;

// тригонометрические константы
const float sqrt3 = sqrt(3.0);
const float pi = 3.141592653;    // PI
const float sin120 = sqrt3/2.0;
const float cos120 = -0.5;
const float tan60 = sqrt3;
const float sin30 = 0.5;
const float tan30 = 1/sqrt3;

// прямая кинематика: (theta1, theta2, theta3) -> (x0, y0, z0)
// возвращаемый статус: 0=OK, -1=несуществующая позиция
int delta_calcForward(float theta1, float theta2, float theta3, float &x0, float &y0, float &z0) {
    float t = (f-e)*tan30/2;
    float dtr = pi/(float)180.0;

    theta1 *= dtr;
    theta2 *= dtr;
    theta3 *= dtr;

    float y1 = -(t + rf*cos(theta1));
    float z1 = -rf*sin(theta1);

    float y2 = (t + rf*cos(theta2))*sin30;
    float x2 = y2*tan60;
    float z2 = -rf*sin(theta2);

    float y3 = (t + rf*cos(theta3))*sin30;
    float x3 = -y3*tan60;
    float z3 = -rf*sin(theta3);

    float dnm = (y2-y1)*x3-(y3-y1)*x2;

    float w1 = y1*y1 + z1*z1;
    float w2 = x2*x2 + y2*y2 + z2*z2;
    float w3 = x3*x3 + y3*y3 + z3*z3;

    // x = (a1*z + b1)/dnm
    float a1 = (z2-z1)*(y3-y1)-(z3-z1)*(y2-y1);
    float b1 = -((w2-w1)*(y3-y1)-(w3-w1)*(y2-y1))/2.0;

    // y = (a2*z + b2)/dnm;
    float a2 = -(z2-z1)*x3+(z3-z1)*x2;
    float b2 = ((w2-w1)*x3 - (w3-w1)*x2)/2.0;

    // a*z^2 + b*z + c = 0
    float a = a1*a1 + a2*a2 + dnm*dnm;
    float b = 2*(a1*b1 + a2*(b2-y1*dnm) - z1*dnm*dnm);
    float c = (b2-y1*dnm)*(b2-y1*dnm) + b1*b1 + dnm*dnm*(z1*z1 - re*re);

    // дискриминант
    float d = b*b - (float)4.0*a*c;
    if (d < 0) return -1; // несуществующая позиция

    z0 = -(float)0.5*(b+sqrt(d))/a;
    x0 = (a1*z0 + b1)/dnm;
    y0 = (a2*z0 + b2)/dnm;
    return 0;
}

// обратная кинематика
// вспомогательная функция, расчет угла theta1 (в плоскости YZ)
int delta_calcAngleYZ(float x0, float y0, float z0, float &theta) {
    float y1 = -0.5 * 0.57735 * f; // f/2 * tg 30
    y0 -= 0.5 * 0.57735 * e;       // сдвигаем центр к краю
    // z = a + b*y
    float a = (x0*x0 + y0*y0 + z0*z0 +rf*rf - re*re - y1*y1)/(2*z0);
    float b = (y1-y0)/z0;
    // дискриминант
    float d = -(a+b*y1)*(a+b*y1)+rf*(b*b*rf+rf);
    if (d < 0) return -1; // несуществующая точка
    float yj = (y1 - a*b - sqrt(d))/(b*b + 1); // выбираем внешнюю точку
    float zj = a + b*yj;
    theta = 180.0*atan(-zj/(y1 - yj))/pi + ((yj>y1)?180.0:0.0);
    return 0;
}

// обратная кинематика: (x0, y0, z0) -> (theta1, theta2, theta3)
// возвращаемый статус: 0=OK, -1=несуществующая позиция
int delta_calcInverse(float x0, float y0, float z0, float &theta1, float &theta2, float &theta3) {
    theta1 = theta2 = theta3 = 0;
    int status = delta_calcAngleYZ(x0, y0, z0, theta1);
    if (status == 0) status = delta_calcAngleYZ(x0*cos120 + y0*sin120, y0*cos120-x0*sin120, z0, theta2);  // rotate coords to +120 deg
    if (status == 0) status = delta_calcAngleYZ(x0*cos120 - y0*sin120, y0*cos120+x0*sin120, z0, theta3);  // rotate coords to -120 deg
    return status;
}

Используемая литература


Все ключевые идеи о кинематике дельта-робота я взял из работы проф. Paul Zsombor-Murray «Descriptive Geometric Kinematic Analysis of Clavel’s «Delta» Robot». Честно признаюсь, что моей математической подготовки не хватило, чтобы понять её до конца, поэтому многое пришлось выводить самому.

Заключение


Спасибо всем, кто прочитал эту статью до конца. Надеюсь, для кого-то она окажется полезной и вдохновит на создание своих вариантов дельта-робота.
  • +18
  • 18,9k
  • 9
Поделиться публикацией
Комментарии 9
    0
    А что же будет когда возникнет деление на 0?
      0
      Такие ситуации могут возникать в отдельных «неудачных» положениях робота, которые называют точками сингулярности (singularities). В своей реальной модели я с ними не столкнулся, поэтому особо не углублялся в этот вопрос и таких проверок в программу не добавлял.

      Почитать про сингулярности дельта-робота можно, например, вот в этой статье, в разделе 4.
        0
        Ну известно, что будет.
          +3
          Нашел на YouTube пару примеров того, что бывает в таких случаях: первый, второй.
        0
        «Промышленный робот» и Lego, это как?
          +3
          Слово «промышленный» я употребил в том смысле, что передо мной стояла вполне конкретная задача по сортировке деталей на конвейере. А то, что у меня получилось, является, конечно, прототипом, а не настоящим промышленным роботом.
          0
          Спасибо, хорошая статья. Я давно интересовался этой темой — как же работают триподы и гексаподы, хотелось построить своего (или хотя бы понять, как оно внутри крутится). Да, с точки зрения механики построение трипода во многом проще робота с прямоугольной системой координат, но теперь понятно, дело в мощной математике. Видел у нас в Томске станок Metrom P1000, вот он как раз работает с параллельной кинематикой, штука навороченная и дорогая, зато интересная — жуть! 5 координат, залезет шпинделем туда, куда обычный ОЦ не сможет. И всё это быстро-быстро. Жаль, сломали. Как говорили, у нас вообще в области всего 5 человек умеет работать на таком навороченном станке.
          А ведь делают умельцы и 3Д-принтеры на параллельной кинематике, и в продаже есть готовые.

          Немножко не в тему: вот ведь как наш мозг может такие сложнейшие вещи обсчитывать? У людей ведь каждый сустав — полярная координата, связанная с цепью других. Поразительно. Когда видишь, как такое решается в числовом виде, поневоле задумываешься — это какой же контроллер нужен для обсчёта? С другой стороны, тактовая частота нашего мозга невелика, значит берёт параллельностью. А раз так — то, возможно, parallax propeller, или подобные решения в будущем найдут свою нишу.
            0
            Мне кажется, рассматривать человеческий мозг, как цифровую эвм — это допущение, по вольности выходящее за рамки здравого смысла.
            Сводя задачи позиционирования к дифференцированию, интегрированию, интегральным уравнениям — и отнеся их решение с заданной точностью на аналоговые части вычислителя, управление в такой системе будет требовать минимальной цифровой логики. В аналоговой ЭВМ решение появляется «сразу» — не требуется тактов, итераций и шагов алгоритмов.

            Автору — большое спасибо. Отличная статья, из тех что идут «в закладки» как справочник.
            0
            Большое спасибо за статью, ради такого и захожу на geektimes

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

            Самое читаемое