Статья написана для только входящих в сам ROS и имеющих необходимость в постройке реального робота для обучения или выполнения реальных задач.



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

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

Покупка готовых платформ


На рынке не так много готовых платформ, их список приведен на сайте ROS.

При этом стоимость их в России достаточно высока для начала работ, самый дешевый вариант это Turtlebot Burger 3 — 549 долларов. У реселлеров непосредственно в России мы нашли за сумму около 90000 т.р.



В Китае можно купить за 45-50 т.р… В любом случае, это будут платформы малого размера и грузоподъемности, не способные выполнять реальные задачи.

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

  • Лидар класса Slamtech A1-А2
  • Дешевые колеса на базе коллекторных двигателей с редукторами
  • Управление драйверами на базе разных пакетов, которые обычно включают связку PWM управления с управляющего компьютера, либо через Ардуино с управлением через UART порт + rosserial.
  • Single Board Computer — Raspberry, Jetson Nano
  • 2 колеса
  • связка gmapping + amcl/rosbot_ekf + move_base



Коллекторные двигатели нам не понравились потому что шумные, имеют высокое потребление и необходим редуктор. Тут надо сказать, что рассматриваются платформы достаточно тяжелые — т.е. сблокированные редукторы + двигатели что-то вроде двигателя с фото не подойдут.

Соответственно, как движители были выбраны BLDC мотор-колеса. Самый доступный на текущий момент вариант — это гироскутерные колеса 6.5. Осталось только ими управлять из ROS.

Но тут всплыла одна проблема: еще при работе с гольф роботами была замечена существенная проблема с поворотом такой платформы на низкой скорости — робот достаточно дерганно разворачивался.

Причины такого поведения просты: в развороте существенную роль сопротивлению движения работают силы трения скольжения. А у них есть особенность — сопротивление зависит от массы и характера самих поверхностей — именно поэтому маленький болтик первыми 2-3 витками держит большие конструкции.

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


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



И в итоге робот весом в 10-15 килограмм уже начинает разворачиваться с ощутимыми рывками.

Вторую часть проблемы добавляет пакет ROS diff_drive в сочетании с достаточно слабыми SBC платами.

Вот список того, что удалось понять


Управление ведется с использованием real-time реализации, но никак не учитывается, что прочие пакеты должны ожидать данный подход при работе с управлением.

Тут надо вспомнить, что real-time не мгновенная реакция на событие, а гарантия того, что обработка события пройдет в гарантированные интервалы физического времени. Т.е. условная ГЭС, регулируемая каждую минуту (обязательно каждую) — это тоже real-time система.
Появляется шанс того, что сообщениями будут забиты топики планировщика маршрута, а для подсистем позиционирования и планирования маршрута подобное поведение обычно приводит либо к останову и рестарту обработки потока, либо к попыткам отрегулировать на ходу процесс.



В итоге на слабых SBC возникает ситуация с беспрерывным пересчетом пути, либо поиск локализации отъедает достаточно большое процессорное время — и чем больше делается попыток успеть за RT компонентой — тем больше накапливается разбег по времени. В чистом виде положительная обратная связь, которая в данном случае совсем не нужна.

Дополнительно к этому gmapping при резких изменениях положения платформы и слабом лидаре тоже начинает работать совсем не быстро — наблюдались паузы до полсекунды, что тоже сильно озадачивает планировщик маршрута.

Управление двигателями идет раздельно, поэтому при использовании SBC и наивной реализации драйверов колеса будут управляться с некоторой задержкой.

Тут проблема еще проще — нам нужно управлять 2-4 колесами через канал связи, который. Соответственно, сигналы управления выстраиваются в очередь и начинают передаваться друг за другом — левое колесо, правое колесо, левое колесо, правое колесо.

В итоге сами колеса, а хуже того — и положения колес с энкодеров начинают вносить свою лепту в образование ПОС между планированием пути и управлением моторами

Встроенная одометрия реализована весьма упрощенно


Как мы не искали простого способа синхронизировать подачу команды и отдачу одометрии — не нашелся. Синхронизация всегда связана с ожиданием каких-то команд на входе, а они не регулярны во времени и в итоге одометрия начинает задерживаться на выходе. Возможно читатели подскажут простой способ — тут мы не считаем себя гуру в ROS.

Добиться того, чтобы подобная сборка вела себя хорошо, нам не удалось. Точнее удалось занизив ускорения перемещения до 0,05-0,1 м/c^2 и угловой скорости до 0,03 рад/с.

Подумалось что хорошо бы иметь :

  • Дешевая платформа
  • Синхронное управление колесами
  • Расчет одометрии с учетом поведения колес и управления
  • Режимы поворот-движение с разным управлением
  • Разные ограничения в разных режимах

Что в итоге вышло


Был взят контроллер с гироскутерных плат и написано управление колесами таким образом, чтобы при старте движения и поворотах использовалось управление по моменту. А если тележка едет прямо и разогналась до определенной скорости — то включается режим управления именно по скорости. В обоих режимах данные с энкодеров обрабатываются внутри контроллера с учетом скорости и режима и наружу выдаются данные с некоторым предсказанием вперед на 10-15 мс.

Энкодеры же читаются в контроллере всегда, скапливаются в буфере в 2-3 элемента и передаются с задержкой. Суть в том, чтобы при получении управление отклик пошел только после того как команда будет выполнена — т.е. выполняется блокировка буфера до того как придут изменившиеся значения энкодеров. Но выдача одометрии идет, просто использует последнее уже переданное значение.

Т.к. все вышеприведенные проблемы сводились к тому, что надо в любом случае синхронизировать управление на синхронной приеме-передаче с UART порта, то вводить принудительный синхронизатор в пакет diff_drive мы сочли неразумным, поэтому написан свой пакет управления.

Находится здесь github.com/Shadowru/hoverboard_driver и на текущий момент активно дорабатывается:

Пакет включается в launch файл как :

<node name="hoverboard_driver" pkg="hoverboard_driver" type="node" output="screen">
        <param name="uart" value="{имя порта}"/>
        <param name="baudrate" value="115200"/>
        <param name="wheel_radius" value="0.116"/>
        <param name="base_width”  value="0.43"/>
        <param name="odom_frame”  value="odom"/>
        <param name="base_frame”  value="base_link"/>
    </node>

Сам uart порт должен быть с соответствующими правами доступа, на некоторых платформах он не всегда имеет доступ для пользователя. Как пример — для Jetson Nano в каталоге hoverboard_driver/scripts/jetson_nano_serial.sh приложен скрипт проставляющий права при старте ОС.

В самом пакете есть чтение потока данных с контроллера и выдача в топики соответствующей информации:

hoverboard_driver/hoverboard_msg с пакетом типа hoverboard_msg

Структура сообщения такова :

int16 state1 — внутренняя информация по колесу 1
int16 state2 — внутренняя информация по колесу 2
int16 speed1 — мгновенная скорость колеса 1
int16 speed2 — мгновенная скорость колеса 2
int16 batVoltage — напряжение батареи
int16 boardTemp — температура контроллера
int16 error1 — ошибка колеса 1
int16 error2 — ошибка колеса 2
int32 pulseCount1 — счетчик энкодера колеса 1
int32 pulseCount2 — счетчик энкодера колеса 2

Кроме этого в топик hoverboard_driver/odometry выдается типовое сообщение nav_msgs::Odometry

Рассчитывается положение так :

Исходя из параметров wheel_radius — радиус колеса в метрах, base_width — расстояние между центрами колес мы рассчитываем насколько каждое колесо сдвинулось за время между предыдущим положением и и считанным.

double curr_wheel_L_ang_pos = getAngularPos((double) feedback.pulseCount1);
double curr_wheel_R_ang_pos = getAngularPos((double) feedback.pulseCount2);

double dtime = (current_time - last_time).toSec();

double delta_L_ang_pos = curr_wheel_L_ang_pos - raw_wheel_L_ang_pos;
double delta_R_ang_pos = -1.0 * (curr_wheel_R_ang_pos - raw_wheel_R_ang_pos);

После чего рассчитываем ускорение каждого из колес

wheel_L_ang_vel = delta_L_ang_pos / (dtime);
wheel_R_ang_vel = delta_R_ang_pos / (dtime);

Далее считаем линейное ускорение робота по каждой из осей

robot_angular_vel = (((wheel_R_ang_pos - wheel_L_ang_pos) * wheel_radius / base_width) - robot_angular_pos) / dtime;
robot_angular_pos = (wheel_R_ang_pos - wheel_L_ang_pos) * wheel_radius / base_width;

robot_x_vel = ((wheel_L_ang_vel * wheel_radius + robot_angular_vel * (base_width / 2.0)) * cos(robot_angular_pos));
robot_y_vel = ((wheel_L_ang_vel * wheel_radius + robot_angular_vel * (base_width / 2.0)) * sin(robot_angular_pos));

В итоге получается полный набор — линейное ускорение, угловое ускорение и перемножив их на время — смещение позиции робота.

robot_x_pos = robot_x_pos + robot_x_vel * dtime;
robot_y_pos = robot_y_pos + robot_y_vel * dtime;

После чего на отправляется сообщение одометрии плюс tf трансляция между фреймом одометрии и базовым.

Контроллер на входе управляется одним пакетом с 2 параметрами — скорость и поворот, пакет транслируется из twist почти нативно — скорость делится на длину окружности и получаются обороты.

double v = vel.linear.x;
double rps = v / rpm_per_meter;
double rpm = rps * 60;
int16_t speed = static_cast(rpm);

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

double w = vel.angular.z;
int16_t steer = static_cast<int>(-1 * w * 30);

В итоге удалось добиться весьма стабильного управления тележки на дешевых компонентах.



Мы создаем роботов, как и подавляющее большинство из Вас. У нас есть наш основной продукт, который мы активно развиваем. Провели пилотные испытания и готовы к производству. Это робот для сбора мячей для гольфа.

Мы разрабатываем роботов и производные решения на заказ. Иногда это работа от ТЗ и скетча до готового продукта, иногда часть работ.



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

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



Что касается контроллера чаще всего это модельные esc, vesc, odrive, BLD-300B или самодельные решения.

Для легкого входа в создание реальных роботов и снятия проклятия Gazebo большинству разработчиков требуется кит для легкого входа. Многое в реальном мире происходит не так как в идеальном.

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


Мы предлагаем то, что помогло бы нам сэкономить нервы в свое время, это кит для сборки корпуса(параллелепипед и цилиндр), два мотор-колеса, батарея, зарядное устройство и перепрошитый контроллер, который выдает одометрию и работает с нашим пакетом ROS из коробки за 19.000 руб. Это дешевле чем фрезерная резка, стоимость композитного материала, крепежа и поворотного колеса с амортизацией.



Позвоните нам или оформите заявку на платформу для ROS online. Давайте делать роботов вместе.

Подробнее о платформе мы расскажем на митапе, посвященный Robot Operating System 5 декабря. Регистрация для участников уже открыта.

> Регистрация для зрителей