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

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

Начнем с особенностей робота:

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

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

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

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

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

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

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

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

Принимаем, что положение второго джоинта и цели, куда робот должен дотянуться лежат на одной прямой (), а звено () параллельно этой прямой. Угол поворота прямой ) — ∠γ считается отдельно и влияет на угол поворота 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

Найдем координаты точки . Длина отрезка () равна длине джоинта 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.

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

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

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