AI Как написать собственную кинематику для робота-манипулятора и заставить его ловить объекты на лету

[AI]

Привет, Хабр! Одна из задач при управлении роботами-манипуляторами – расчет обратной кинематики. Данный вид кинематики позволяет вычислить углы наклона суставов робота (joints) таким образом, чтобы захват (grip) робота пришел в заданные трехмерные координаты с правильным углом наклона. Для многих роботов уже есть алгоритмы и формулы вычисления обратной кинематики, мы (команда Zebrains) столкнулись с отсутствием готового решения для робота xArm 2.0.

В статье мы подробно опишем с какими сложностями столкнулись при управлении данным роботом, как получили формулы для расчета двух видов кинематики для данного робота и поделимся кодом на C++. В проекте использовался ROS2, ноды которого были написаны на C++.

Внешний вид робота xArm 2.0
Начнем с особенностей робота:


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

Внешний вид робота xArm 2.0

Сначала мы пытались использовать различные готовые алгоритмы вычисления обратной кинематики, представленные в MoveIt2. Но местные алгоритмы предназначены для 6-ти осевых роботов (без учета поворота захвата), да при том, с несколько иным строением суставов (см. робот Panda на картинке ниже).

Внешний вид робота Panda

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

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

Демонстрация движения робота в rviz
Решение задачи кинематики робота-манипулятора (кинематика №1)


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

Геометрическая схема расчетов 1-ой кинематики

При вычислении задачи обратной кинематики использовались следующие условия:

За поворот в сторону цели отвечает нулевой джоинт, поэтому для него угол поворота считаем отдельно, а дальше работаем в двухмерном пространстве;

Принимаем, что положение второго джоинта и цели, куда робот должен дотянуться лежат на одной прямой (

), а звено (

) параллельно этой прямой. Угол поворота прямой

) — ∠γ считается отдельно и влияет на угол поворота 2 джоинта.

Угол поворота нулевого джоинта вычисляется по формуле:

где

и

— координаты точки назначения по x и y

и

— координаты робота назначения по x и y

Поскольку робот может иметь свое отклонение по данному джоинту, то дополнительно добавим коэффициент смещения угла

—

, а также коэффициент направления —

Итоговая формула будет иметь следующий вид:

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

Где

— длина нулевого джоинта,

— координата точки назначения по оси z,

— координата робота по оси z.

Расстояние dist рассчитывается по формуле:

Примем новую систему координат, в которой dist лежит на оси x, а перпендикуляры к этой прямой на оси y.

Тогда, точки

и

лежат на одной прямой и имеют высоту h=y. Эта высота присутствует в треугольниках △

и △

. Через эти же треугольники можно найти углы

(поскольку он равен углу ∠

, как углы, образованные общей прямой (

) и параллельными отрезками (

) и (

) и

(равен углу ∠

как накрест лежащие, образованные параллельными отрезками (

)и (

) и пересекающим их отрезком (

).

Для нахождения углов

и

необходимо знать значения катетов

=

и

=

,гипотенузы (

)=

и (

)=

уже известны — это длины звеньев робота (между 2 и 3 джоинтами, между 4 и 5 джоинтами). Также, через уравнения окружностей (приняв P2за начало координат):

Уравнение окружности с центром в начале координат имеет вид:

Где

,

— координаты точки

— координаты точки

а так как выше мы установили, что они лежат на одной прямой, то

Следовательно:

Поскольку

есть в обоих уравнениях, приравняем уравнения, выразив их через

Тогда:

Вычисления углов:

Где y - высота до цели относительно второго джоинта в мировых координатах.

Финальные формулы для вычисления углов поворота джоинтов с учетом коэффициента смещения имеют следующий вид:

Фрагмент кода, отвечающий за просчет кинематики №1:

bool calcIKSimple(float goalX, float goalY, float goalZ, const IkParameters &ikParams,
std::vector<double> &jointGroupPositions)
{
double localX = sqrt(pow((ikParams.robotX - goalX), 2) + pow((ikParams.robotY - goalY), 2));
double localY = goalZ - ikParams.d0 - ikParams.robotZ;
double distance = sqrt(pow((localX), 2) + pow((localY), 2));

jointGroupPositions[ikParams.joints[0]] =
ikParams.angleZeroDirection * atan2(goalY - ikParams.robotY, goalX - ikParams.robotX)
+ ikParams.angleZeroAdd;
jointGroupPositions[ikParams.joints[1]] = 0.0;
jointGroupPositions[ikParams.joints[2]] = 0.0;
jointGroupPositions[ikParams.joints[3]] = 0.0;
jointGroupPositions[ikParams.joints[4]] = 0.0;

if (distance > ikParams.d3 && distance < ikParams.d1 + ikParams.d2 + ikParams.d3) {
double b1 = (pow(ikParams.d1, 2) - pow(ikParams.d3, 2) + pow((distance - ikParams.d2), 2))
/ (2 * (distance - ikParams.d2));
double b3 = distance - b1 - ikParams.d2;

double gamma = std::asin(localY / distance);
jointGroupPositions[ikParams.joints[1]] = std::asin(b1 / ikParams.d1);
jointGroupPositions[ikParams.joints[2]] =
ikParams.angleTwoDirection * (M_PI_2 - jointGroupPositions[ikParams.joints[1]]);
jointGroupPositions[ikParams.joints[1]] =
ikParams.angleOneDirection * (jointGroupPositions[ikParams.joints[1]] - gamma);

jointGroupPositions[ikParams.joints[3]] =
ikParams.angleThreeDirection * std::acos(b3 / ikParams.d3);
return true;
} else {
return false;
}
}
Решение задачи кинематики робота-манипулятора (кинематика №2)


Назначение второго вида кинематики — расчет углов поворота джоинтов таким образом, чтобы робот пришел в заданную точку с соблюдением заданного угла последнего джоинта относительно горизонтальной линии.

Заданный угол — ∠β.

Геометрическая схема расчетов 2-ой кинематики представлена на рисунке 6

Рисунок 6 – Геометрическая схема расчетов 2-ой кинематики

Найдем координаты точки

. Длина отрезка (

) равна длине джоинта 3 —

. Точка

— цель, куда должен приехать робот. Ее координаты мы знаем. Рассмотрим треугольник △

. В нем угол △

известен и равен ∠β. Таким образом, координаты точки

можно получить по следующим формулам:

Рассмотрим треугольник △

. В нем нам известны длины всех трех сторон:

Чтобы найти угол отклонения первого джоинта ∠

необходимо найти углы ∠

и ∠

, так как:

По теореме косинусов [ [8], [9], [10] ] найдем угол ∠

:

Рассмотрим треугольник △

. В нем:

Таким образом, угол ∠

можно рассчитать по формуле:

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

Финальная формула для вычисления угла поворота первого джоинта с учетом коэффициента смещения имеет следующий вид:

Для вычисления угла отклонения второго джоинта ∠

потребуется вычислить угол ∠

, который является смежным углу ∠

, так как угол ∠

— развернутый и равен 180°, то угол ∠

можно найти по формуле:

Повторно рассмотрим треугольник ∠(

) . По теореме косинусов получаем:

Угол ∠

вычисляется по формуле:

Финальная формула для вычисления угла поворота второго джоинта с учетом коэффициента смещения имеет следующий вид:

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

Из точки

построим вертикаль. Поскольку отрезок (

) является продолжением отрезка (

), то угол ∠(

) равен углу ∠

Также построим вертикаль из точки

и продлим отрезок (

).

Таким образом угол ∠

будет равен сумме углов ∠

и ∠

.

Рассмотрим треугольник

. Углы ∠

и ∠

— вертикальные, а значит равны. Угол ∠

вычисляется по формуле:

Угол ∠

вычислим зная, что сумма углов треугольника равна 180°, а два других угла треугольника нам известны. Получим:

Тут важно обратить внимание, что используются углы ∠

и ∠

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

Фрагмент кода, отвечающий за просчет кинематики №2

bool calcIKFinalAngle(float goalX, float goalY, float goalZ, float angle,
const IkParameters &ikParams, std::vector<double> &jointGroupPositions)
{
double localX = sqrt(pow((ikParams.robotX - goalX), 2) + pow((ikParams.robotY - goalY), 2));
double localY = goalZ - ikParams.d0 - ikParams.robotZ;

jointGroupPositions[ikParams.joints[0]] =
ikParams.angleZeroDirection * atan2(goalY - ikParams.robotY, goalX - ikParams.robotX)
+ ikParams.angleZeroAdd;
jointGroupPositions[ikParams.joints[1]] = 0.0;
jointGroupPositions[ikParams.joints[2]] = 0.0;
jointGroupPositions[ikParams.joints[3]] = 0.0;
jointGroupPositions[ikParams.joints[4]] = 0.0;

double yP4 = std::sin(angle) * ikParams.d3 + localY;
double xP4 = localX - std::cos(angle) * ikParams.d3;
double distanceSmall = sqrt(pow((xP4), 2) + pow((yP4), 2));

if (distanceSmall < ikParams.d1 + ikParams.d2) {

jointGroupPositions[ikParams.joints[1]] = M_PI_2
- std::acos((pow(distanceSmall, 2) + pow(ikParams.d1, 2) - pow(ikParams.d2, 2))
/ (2 * distanceSmall * ikParams.d1))
- std::asin(yP4 / distanceSmall);
jointGroupPositions[ikParams.joints[2]] = M_PI
- std::acos((pow(ikParams.d1, 2) + pow(ikParams.d2, 2) - pow(distanceSmall, 2))
/ (2 * ikParams.d1 * ikParams.d2));
jointGroupPositions[ikParams.joints[3]] = ikParams.angleThreeDirection
* (M_PI - (M_PI_2 - angle)
- (jointGroupPositions[ikParams.joints[1]]
+ jointGroupPositions[ikParams.joints[2]]));

jointGroupPositions[ikParams.joints[1]] =
ikParams.angleOneDirection * jointGroupPositions[ikParams.joints[1]];
jointGroupPositions[ikParams.joints[2]] =
ikParams.angleTwoDirection * jointGroupPositions[ikParams.joints[2]];
return true;
} else {
return false;
}
}
Расчет рабочей области робота


Поскольку соблюдение заданного угла при перемещении в точку, накладывает на робота ограничение, следовательно, его рабочая зона сокращается с суммы длин его 2, 3 и 4 звеньев до меньшего значения, которое необходимо посчитать. Для этого, необходимо получить расстояние D =

. Точка

будет иметь наибольшее значение x при том же y и угле ∠β, если угол ∠

будет равен 0, т.е. звенья

и

будут лежать на одной прямой, а (

)=

. Тогда, в треугольнике △

неизвестен только нужный нам катет (

), который можно найти следующим образом:

Из расчетов ранее, известно, что:

Рассмотрим треугольник △

:

Тогда неизвестный катет находим по формуле:

Итоговая формула будет иметь следующий вид:

Результат разработки


Полученная кинематика была сначала протестирована в 3D игровом движке Unreal Engine 4, поскольку в нем при помощи системы Blueprints (визуальный язык программирования) можно быстро настроить управление объектами на сцене, также есть поддержка языка программирования Python.

Рисунок 7 – Сцена в среде 3D движка Unreal Engine 4 с манипуляторами для тестирования кинематики

После проверки и отладки кода для расчета кинематики код был адаптирован для языка программирования C++ и перенесен в отдельную библиотеку, которая подгружается модулем управления роботом-манипулятором.

Рисунок 8 – Стенд системы с конвейером и роботом для захвата предметов

В ходе работы над проектом команде Zebrains удалось разработать и реализовать два собственных алгоритма обратной кинематики для робота xArm 2.0, адаптированного под задачу захвата объектов с конвейера с помощью вакуумной присоски. Первый алгоритм обеспечивает перемещение захвата в заданную точку при условии параллельности определённых звеньев, а второй — с учётом фиксированного угла наклона конечного звена. Оба решения реализованы на C++ и интегрированы в ROS2-ноды, что позволило добиться стабильного и предсказуемого поведения манипулятора в реальных условиях.

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