[Из песочницы] Кинематика дельта-робота11.02.2016 16:37

В далёком 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) для решения обратной кинематической задачи и
fforward (Ѳ1, Ѳ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». Честно признаюсь, что моей математической подготовки не хватило, чтобы понять её до конца, поэтому многое пришлось выводить самому.

Заключение

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