От cmd_vel к скоростям колёс: дифференциальный привод в ROS 2

Введение

Всё, что находится выше колёс в роботе ROS 2 — телеуправление, навигация, ваш собственный регулятор стыковки, — говорит на одном языке: сообщение geometry_msgs/msg/Twist в /cmd_vel, смысл которого «ехать вперёд с такой скоростью и поворачивать с такой». Всё, что находится ниже колёс, говорит на языке рабочего цикла ШИМ (PWM).

Это руководство посвящено промежуточному слою, и он настолько короткий, что кажется тривиальным. Две строки алгебры преобразуют одно в другое. Другие 90% работы узла привода — это то, что эти две строки делают неправильно, когда робот просят о движении, которое он не может обеспечить.

Что на самом деле означает Twist

Для робота с дифференциальным приводом важны только два из шести чисел:

• linear.x — скорость вперёд в метрах в секунду вдоль направления робота.
• angular.z — скорость рыскания в радианах в секунду, положительная против часовой стрелки при взгляде сверху.

Остальные четыре описывают движение, которое двухколёсный робот выполнить не может. linear.y — движение вбок, доступное только роботу с омни-приводом; игнорируйте его, а не пытайтесь приблизить. Замените две строки ниже на четырёхстрочную матрицу mecanum — и linear.y начнёт что-то означать. Оба значения выражены в собственной системе координат робота base_link и заданы в единицах SI (СИ): робот, принимающий linear.x за процент, будет выглядеть исправным до встречи с настоящей навигационной системой.

Две строки кинематики

Пусть b — расстояние между колёсами, а v, ω — заданные линейная и угловая скорости. Скорость каждого колеса относительно земли равна скорости робота вперёд плюс или минус вклад поворота:

v_left  = v − ω · b / 2
v_right = v + ω · b / 2

Переведите каждое значение в угловую скорость колеса, разделив на радиус колеса r:

ω_wheel = v_wheel / r        [rad/s]

Это всё преобразование. Вращение на месте задаётся v = 0, что даёт две равные и противоположные скорости колёс; движение прямо задаётся ω = 0, что даёт две одинаковые скорости. Отношение между ними задаёт радиус поворота, R = v / ω, и сохранение этого отношения — то, чему посвящено остальное руководство.

Измерьте b движением, а не линейкой

Расстояние между колёсами — параметр, который наследуют все остальные, и расстояние между их центрами не является им.

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

Откалибруйте его напрямую:

1. Задайте чистое вращение, v = 0, ω = 1.0 rad/s, ровно на 10 секунд.
2. Робот должен повернуться на 10 радиан — полный оборот и примерно 213°. Отметьте начальный курс и измерьте, где он фактически остановился.
3. Масштаб: b_new = b_old × (turned / commanded).

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

Обрабатывайте насыщение, не меняя поворот

У двигателя есть максимальная скорость. Запросите v = 0.5 m/s с резким поворотом на роботе, колёса которого максимум разгоняются до 0.55 m/s, и заданная скорость одного колеса окажется выше возможного.

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

Вместо этого масштабируйте оба колеса одним и тем же коэффициентом. Отношение сохранится, а значит, сохранится и дуга; робот просто проходит её медленнее:

peak = max(abs(left), abs(right))
if peak > MAX_WHEEL_SPEED:
    scale = MAX_WHEEL_SPEED / peak
    left *= scale
    right *= scale

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

Знайте реальный рабочий диапазон

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

Задайте v_left или v_right равной максимальной скорости колеса и решите уравнение. Наибольшая доступная скорость рыскания при заданной скорости вперёд равна:

w_max(v) = 2 * (V_max - |v|) / b

Для робота в приведённом ниже коде — b = 0.152 m, колёса разгоняются максимум до 0.55 m/s:

Полезна именно форма: скорость и скорость поворота меняются линейно, а последние 10% скорости вперёд съедают почти всё управление поворотом. У робота, которому задана скорость 0.5 m/s, остаётся 12% запаса управления по рысканию, который был при неподвижном роботе, — поэтому регулятор пути обычно насыщается именно на повороте.

Из этого следуют два практических вывода. Передайте реальные ограничения планировщику, а не заставляйте его обнаруживать их насыщением: в Nav2 это max_vel_x и max_vel_theta, а установка max_vel_x на полные 0.55 m/s гарантирует, что регулятор запросит дуги, которые робот не сможет проехать. А при настройке pure pursuit помните: слишком малое упреждение, требующее тесного радиуса, незаметно ограничит скорость.

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

Twist — это скачок команды. Навигация публикует 0.0, затем 0.35 m/s в последовательных сообщениях, и скачок команды требует от двигателей максимального ускорения, которое они могут создать, — на лёгком роботе это больше, чем шины способны передать.

Ломается не движение, а одометрия. Проскальзывающее колесо вращается, когда робот не движется, энкодеры всё равно считают это движение, и оценка позы получает ошибку, которую ни один последующий фильтр не отличит от настоящего движения. Страница о сцеплении задаёт измеренный предел: робот, у которого 55% веса приходится на приводную ось, развивает около 1.6 m/s2 на пыльной плитке до пробуксовки колёс.

Ограничьте ускорение ниже этого предела в таймере, где dt известен и постоянен:

MAX_ACCEL = 1.0        # m/s^2 at the wheel, comfortably under the traction limit

def slew(target, current, dt):
    step = MAX_ACCEL * dt
    return max(current - step, min(current + step, target))

# in tick(), before sending:
self.out_left = slew(self.left, self.out_left, 0.02)
self.out_right = slew(self.right, self.out_right, 0.02)

При 1.0 m/s2 старт из покоя достигает полной скорости за 0.55 s, добавляя 20 mm/s за тик 50 Hz. Это достаточно медленно, чтобы шины оставались сцепленными, и достаточно быстро, чтобы ни один человек не назвал робота вялым.

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

Полный узел привода

import rclpy
from rclpy.node import Node
from geometry_msgs.msg import Twist

WHEEL_RADIUS = 0.0325        # m
WHEEL_SEPARATION = 0.152     # m — calibrated by driving, see above
MAX_WHEEL_SPEED = 0.55       # m/s at full duty, measured
CMD_TIMEOUT = 0.4            # s


class DiffDrive(Node):
    def __init__(self):
        super().__init__('diff_drive')
        self.create_subscription(Twist, 'cmd_vel', self.on_cmd, 10)
        self.create_timer(0.02, self.tick)          # 50 Hz output
        self.left = self.right = 0.0
        self.last_cmd = self.get_clock().now()

    def on_cmd(self, msg):
        v, w = msg.linear.x, msg.angular.z
        left = v - w * WHEEL_SEPARATION / 2.0
        right = v + w * WHEEL_SEPARATION / 2.0

        peak = max(abs(left), abs(right))
        if peak > MAX_WHEEL_SPEED:                  # keep the arc, lose the speed
            left *= MAX_WHEEL_SPEED / peak
            right *= MAX_WHEEL_SPEED / peak

        self.left, self.right = left, right
        self.last_cmd = self.get_clock().now()

    def tick(self):
        age = (self.get_clock().now() - self.last_cmd).nanoseconds * 1e-9
        if age > CMD_TIMEOUT:                       # nobody is driving — stop
            self.left = self.right = 0.0
        send_wheel_targets(self.left / WHEEL_RADIUS,
                           self.right / WHEEL_RADIUS)   # rad/s

Стоит назвать два структурных решения.

Вывод по таймеру, а не в обратном вызове. Двигателям задаётся фиксированная частота 50 Hz независимо от того, как часто приходит cmd_vel. Публикатор, который сбоит из-за Wi-Fi или долгого цикла планировщика, больше не заставляет колёса сбоить.

Сторожевой таймер обязателен. Без него последняя команда перед потерей связи остаётся командой, которую робот продолжает выполнять, а последняя команда обычно означает «вперёд». Именно способность робота останавливаться при прекращении команд отличает наличие программной ошибки от последствий в виде дыры в стене. Учебники беспроводного управления повторяют тот же аргумент по той же причине.

Превращение скорости колеса в рабочий цикл

Узел выше передаёт rad/s для каждого колеса. Кто-то должен реализовать это, и есть два честных способа.

Разомкнутое управление. Линейно преобразуйте скорость в рабочее значение ШИМ и примите ошибку. У двигателей есть мёртвая зона: примерно ниже 15–20% рабочего цикла ШИМ они гудят, но не вращаются, поэтому отображение должно начинаться выше неё:

int dutyFor(float radPerSec) {
  if (fabsf(radPerSec) < 0.05f) return 0;                 // genuinely stopped
  float u = fabsf(radPerSec) / MAX_RAD_PER_SEC;           // 0..1
  int duty = DEADBAND + (int)((255 - DEADBAND) * u);
  return (radPerSec > 0) ? duty : -duty;
}

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

Замкнутое управление. Установите квадратурные энкодеры на колёса и запустите контур ПИД (PID) для каждого колеса с rad/s в качестве уставки. Теперь слой привода выдаёт заданную скорость, а мёртвая зона, нагрузка и аккумулятор становятся задачей регулятора, а не вашей.

На роботе с Pi обычное разделение такое: узел ROS работает на Pi, а ПИД-регулятор скорости каждого колеса — на Arduino через последовательный порт с частотой в несколько сотен герц. Linux не место для контура, который не должен дрожать.

Проверьте до того, как робот сможет навредить себе

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

# Straight ahead at 0.2 m/s, republished at 10 Hz
ros2 topic pub /cmd_vel geometry_msgs/msg/Twist \
  '{linear: {x: 0.2}, angular: {z: 0.0}}' -r 10

# Pure rotation, counter-clockwise at 1 rad/s
ros2 topic pub /cmd_vel geometry_msgs/msg/Twist \
  '{linear: {x: 0.0}, angular: {z: 1.0}}' -r 10

# Who is on the topic, and with which message type
ros2 topic info /cmd_vel --verbose

Четыре проверки по порядку:

1. Прямая команда, оба колеса в одну сторону и с одинаковой скоростью. Если они отличаются, до всякой математики неверно подключение или соглашение о знаках.
2. Положительный angular.z, левое колесо назад. REP 103 говорит, что положительное рыскание против часовой стрелки при взгляде сверху, поэтому левое колесо должно вращаться наоборот. Ошибка заставляет любую навигационную систему уходить от цели.
3. Завершите публикатор через Ctrl-C. Колёса должны остановиться за CMD_TIMEOUT. Засеките время: сторожевой таймер, который ни разу не проверили, следует считать неисправным.
4. Задайте команду за пределами диапазона, например linear.x: 0.5 с angular.z: 3.0, и убедитесь, что масштабируются оба колеса, а не ограничивается одно. Отношение скоростей колёс должно совпадать с ненасыщенной командой.

Только после этого поставьте робота на пол, а калибровку вращения из предыдущего раздела используйте как первый реальный тест.

Twist или TwistStamped?

Стоит знать до того, как это обойдётся целого вечера: новые части экосистемы ROS 2 — включая diff_drive_controller — перешли на geometry_msgs/msg/TwistStamped, которое оборачивает ту же команду вращения в заголовок с меткой времени и идентификатором системы координат.

Сбой тихий. Узел телеоперации, публикующий обычный Twist, и регулятор, подписанный на TwistStamped, с точки зрения DDS используют два разных топика, которые случайно имеют одинаковое имя. Ошибка не возникает; робот просто никогда не движется. Проверьте через ros2 topic info /cmd_vel --verbose, который печатает тип на каждой стороне.

Когда всё идёт не так

Когда слой привода честен, всё выше него становится проще: один и тот же интерфейс /cmd_vel обслуживает телеоперацию, регулятор стыковки и полноценную навигационную систему, и никто из них не знает, какие у вас двигатели. Регулятор следования маршруту — именно такой источник: он превращает маршрут в линейную и угловую скорость и передаёт их через тот же интерфейс.