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

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

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

09.09.2025 07:37

ZeBrains_team 0 283 Источник

Привет, Хабр! Меня зовут Александр, и я хочу рассказать историю разработки системы управления роботом-манипулятором, который умеет захватывать движущиеся объекты на конвейере. Это была командная работа, в которой участвовала наша команда инженеров, но я выступлю голосом проекта и поделюсь деталями. Звучит просто, но на деле пришлось столкнуться с кучей интересных проблем — от математики обратной кинематики до ограничений готовых решений.

Всё началось с задачи запрограммировать робота, который мог бы в режиме реального времени отслеживать объекты, классифицировать их и захватывать нужные. Казалось бы, бери готовую библиотеку MoveIt2, настраивай параметры — и всё работает. Но жизнь, как всегда, оказалась сложнее.

Постановка задачи

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

1. Получают информацию о проклассифицированных объектах из предыдущих стадий

2. Планируют взаимодействие — рассчитывают, когда и где лучше захватить объект

3. Управляют роботом— строят траектории и выполняют движения

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

Архитектура системы

Система построена на ROS 2. Вся магия происходит в том, что объекты движутся, а робот должен предсказать, где будет объект в момент захвата, учитывая время своего движения.

Модуль сбора целей

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

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

\newpage

```c++
timeData timeToReachMaxRadius(double x, double y, double speedX, double speedY, double timestamp,
                              worldData worldDataStorage)
{
    double timeFirst = timestamp;
    double timeSecond = timestamp;

    double xTmp, yTmp, speedXTmp, speedYTmp;

    if (worldDataStorage.swap) {
        xTmp = (y * worldDataStorage.kX) / worldDataStorage.divideX;
        speedXTmp = (speedY * worldDataStorage.kX) / worldDataStorage.divideX;
        yTmp = (x * worldDataStorage.kY) / worldDataStorage.divideY;
        speedYTmp = (speedX * worldDataStorage.kY) / worldDataStorage.divideY;
    } else {
        xTmp = (x * worldDataStorage.kX) / worldDataStorage.divideX;
        speedXTmp = (speedX * worldDataStorage.kX) / worldDataStorage.divideX;
        yTmp = (y * worldDataStorage.kY) / worldDataStorage.divideY;
        speedYTmp = (speedY * worldDataStorage.kY) / worldDataStorage.divideY;
    }

    if (std::abs(speedXTmp) > 1e-6) {
        double xFirstTouch =
                -sqrt(pow(worldDataStorage.maxRadius, 2) - pow(yTmp - worldDataStorage.robotY, 2))
                + worldDataStorage.robotX;
        double xSecondTouch =
                sqrt(pow(worldDataStorage.maxRadius, 2) - pow(yTmp - worldDataStorage.robotY, 2))
                + worldDataStorage.robotX;

        timeFirst = timestamp + (xFirstTouch - xTmp) / speedXTmp;
        timeSecond = timestamp + (xSecondTouch - xTmp) / speedXTmp;
    } else if (std::abs(speedYTmp) > 1e-6) {
        double yFirstTouch =
                -sqrt(pow(worldDataStorage.maxRadius, 2) - pow(xTmp - worldDataStorage.robotX, 2))
                + worldDataStorage.robotY;
        double ySecondTouch =
                sqrt(pow(worldDataStorage.maxRadius, 2) - pow(xTmp - worldDataStorage.robotX, 2))
                + worldDataStorage.robotY;

        timeFirst = timestamp + (yFirstTouch - yTmp) / speedYTmp;
        timeSecond = timestamp + (ySecondTouch - yTmp) / speedYTmp;
    } else {
        timeFirst = 0.0;
        timeSecond = -1;
    }

    timeData result;
    result.timeFirst = timeFirst;
    result.timeSecond = timeSecond;
    return result;
}
```

\newpage

Эта функция учитывает, что робот и система координат мира могут не совпадать (параметр swap), и пересчитывает координаты между системами.

Планировщик взаимодействия

Второй модуль — настоящий мозг системы. Он получает информацию о цели и должен рассчитать:

Где будет объект в момент захвата
Куда его нужно сбросить (в зависимости от класса)
Успеет ли робот добраться до цели

Самая интересная часть — предсказание положения объекта:

```c++

if (targetCollectorRes->success) {

    robotControlServiceRequest->id = targetCollectorRes->class_id;

    auto it = std::find(classIDs.begin(), classIDs.end(), targetCollectorRes->class_id);

    int index = std::distance(classIDs.begin(), it);

    if (index == -1) {

        RCLCPP_INFO(this->get_logger(), "Skip class: %d", targetCollectorRes->class_id);

        continue;

    }

    double timeVar = targetCollectorRes->last_detection_time.stamp.sec

            + (targetCollectorRes->last_detection_time.stamp.nanosec * 1e-9);

    robotControlServiceRequest->touch_time =

            this->get_clock()->now().seconds() + robotMotionTime + robotTouchTime;

    RCLCPP_INFO(this->get_logger(), "Time: %f %f", robotControlServiceRequest->touch_time,

                targetCollectorRes->robot_zone_in_time);

    if (robotControlServiceRequest->touch_time < targetCollectorRes->robot_zone_in_time) {

        robotControlServiceRequest->touch_time =

                targetCollectorRes->robot_zone_in_time + robotTouchTime;

    }

    robotControlServiceRequest->position.center.x = targetCollectorRes->position.center.x

            + (robotControlServiceRequest->touch_time - timeVar)

                    * targetCollectorRes->speed.x;

    robotControlServiceRequest->position.center.y = targetCollectorRes->position.center.y

            + (robotControlServiceRequest->touch_time - timeVar)

                    * targetCollectorRes->speed.y;

    robotControlServiceRequest->drop_position.center.x = boxXs[index];

    robotControlServiceRequest->drop_position.center.y = boxYs[index];

}

```

Здесь происходит экстраполяция — система предсказывает, где будет объект через определённое время, учитывая его текущую скорость.

Проблемы с готовыми решениями

Изначально мы планировали использовать стандартную кинематику из MoveIt2. Но тут нас ждал сюрприз. Дело в том, что мы использовали робот-манипулятор FANUC LR Mate 200iD/7L — компактный 5-осевой робот, оптимизированный для высокоскоростных операций в ограниченном пространстве. Однако алгоритмы в MoveIt2 рассчитаны на 6-осевых роботов, что привело к проблемам:

Результат был печальным:

Робот не мог дотянуться до многих точек
Алгоритмы планирования постоянно зависали
Когда что-то получалось, робот выгибался в непредсказуемые позы

Пришлось писать собственную кинематику.

Кинематика №1: простое решение

Первая кинематика решает задачу движения в точку с ограничением — второй джоинт и цель должны лежать на одной прямой.

Математика получилась довольно красивая:

Угол поворота нулевого джоинта:

$\angle{0} = \text{angle}_{0~\text{direction}} \cdot \arctan\left(\frac{\text{goal}_y - \text{robot}_y}{\text{goal}_x - \text{robot}_x}\right) + \text{angle}_{0~\text{add}}$

$$\angle{0} = \text{angle}_{0~\text{direction}} \cdot \arctan\left(\frac{\text{goal}_y - \text{robot}_y}{\text{goal}_x - \text{robot}_x}\right) + \text{angle}_{0~\text{add}}$$

Дальше работаем в 2D пространстве и находим катеты через уравнения окружностей:

$b_1 = \frac{a_3^2 - a_1^2 - (a_2 - \text{dist})^2}{2 \cdot (a_2 - \text{dist})}.$ $b_2 = \text{dist} - b_1 - a_2$

А затем вычисляем углы:

$\angle{1} = \text{angle}_{1~\text{direction}} \cdot \left(\arcsin\left(\frac{b_1}{a_1}\right) - \angle{\gamma}\right)$ $\angle{2} = \text{angle}_{2~\text{direction}} \cdot \left(90^\circ - \arcsin\left(\frac{b_1}{a_1}\right)\right)$ $\angle{3} = \text{angle}_{3~\text{direction}} \cdot \left(\arccos\left(\frac{b_2}{a_3}\right)\right)$

Реализация:

```c++

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: с контролем угла захвата

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

Задача усложняется: теперь нужно найти положение точки P₄, зная угол β:

$P_4(x) = P_5(x) - \cos(\angle{\beta}) \cdot a_3$ $P_4(y) = \sin(\angle{\beta}) \cdot a_3 + P_5(y)$

Дальше решаем треугольник через теорему косинусов и получаем углы джоинтов.

Тестирование в Unreal Engine

Перед переносом на реального робота я решил протестировать кинематику в Unreal Engine 4. Почему именно там? Blueprint'ы позволяют быстро прототипировать, есть поддержка Python, и можно визуально контролировать, что происходит.

Создал сцену с несколькими роботами и проверил алгоритмы. Нашёл несколько багов с направлениями и смещениями углов, которые на бумаге были незаметны.

Результаты

В итоге получилась система, которая:

Работает в реальном времени
Учитывает физические ограничения робота
Предсказывает движение объектов
Имеет собственную, оптимизированную кинематику
Легко настраивается под разные роботы и окружения

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

Что дальше?

Система получилась модульной, поэтому её легко дорабатывать. В планах:

Добавить компенсацию вибраций конвейера
Интегрировать машинное обучение для предсказания траекторий

Заключение

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

Если у вас есть вопросы или хотите обсудить детали реализации — задавайте в комментариях!