Управление движением: cmd_vel и PID

Команда «ехать» превращается в обороты колёс, а удерживать скорость помогает PID-регулятор.

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

От cmd_vel к колёсам

Планировщик публикует в /cmd_vel сообщение Twist: линейная скорость v (вперёд) и угловая w (поворот). Контроллер базы переводит их в скорости левого и правого колеса по формулам дифференциального привода. Это чистая математика:

WHEEL_BASE = 0.32     # между колёсами, м
WHEEL_RADIUS = 0.05   # радиус колеса, м

v = 0.2    # м/с вперёд
w = 0.5    # рад/с поворот

# скорости колёс (м/с)
v_left  = v - w * WHEEL_BASE / 2
v_right = v + w * WHEEL_BASE / 2

# в угловые скорости колёс (рад/с)
omega_left  = v_left  / WHEEL_RADIUS
omega_right = v_right / WHEEL_RADIUS

print(f'левое колесо:  {omega_left:.2f} рад/с')
print(f'правое колесо: {omega_right:.2f} рад/с')

Вывод:

левое колесо:  2.40 рад/с
правое колесо: 5.60 рад/с

Правое колесо крутится быстрее — робот едет вперёд и поворачивает влево. Так абстрактная команда стала конкретными оборотами.

Зачем нужен PID

Сказать мотору «крутись на 5.2 рад/с» мало: под нагрузкой, на подъёме, при разряженной батарее реальная скорость отличается от заданной. Нужна обратная связь: измеряем фактическую скорость колеса (энкодером) и подстраиваем мощность, пока она не совпадёт с желаемой. Этим занимается PID-регулятор.

Три составляющие PID

P (пропорциональная): чем больше ошибка, тем сильнее воздействие. Быстро реагирует, но может не дотягивать.
I (интегральная): копит ошибку во времени и устраняет постоянное отклонение (например, из-за трения).
D (дифференциальная): реагирует на скорость изменения ошибки, гасит колебания и перерегулирование.

PID на Python

Смоделируем разгон колеса до целевой скорости с помощью PID. Код самодостаточный, его можно запустить:

Kp, Ki, Kd = 1.2, 0.4, 0.1
target = 5.0      # желаемая скорость, рад/с
current = 0.0     # текущая
integral = 0.0
prev_error = 0.0
dt = 0.1

for step in range(1, 9):
    error = target - current
    integral += error * dt
    derivative = (error - prev_error) / dt
    output = Kp*error + Ki*integral + Kd*derivative
    # упрощённая модель колеса: воздействие меняет скорость
    current += output * dt * 0.5
    prev_error = error
    print(f'шаг {step}: скорость = {current:.2f} рад/с')

Вывод:

шаг 1: скорость = 0.56 рад/с
шаг 2: скорость = 0.82 рад/с
шаг 3: скорость = 1.08 рад/с
шаг 4: скорость = 1.34 рад/с
шаг 5: скорость = 1.59 рад/с
шаг 6: скорость = 1.83 рад/с
шаг 7: скорость = 2.06 рад/с
шаг 8: скорость = 2.29 рад/с

Колесо плавно и без рывков разгоняется в сторону цели 5.0 рад/с: за восемь шагов оно прошло часть пути, а ошибка постепенно уменьшается. Чем больше ошибка, тем сильнее воздействие — по мере приближения к цели разгон замедляется. Подбор Kp, Ki, Kd под конкретный мотор называют настройкой регулятора.

Как работает под капотом

В реальном роботе PID крутится с высокой частотой (сотни герц) на нижнем уровне — часто прямо в микроконтроллере мотора, а не в ROS. ROS публикует желаемую скорость в /cmd_vel, контроллер базы переводит её в задание для колёс, а локальный PID каждого мотора держит эту скорость, опираясь на энкодер. Так разделяются уровни: ROS думает «куда ехать», PID — «как точно крутить колесо».

Частые ошибки

Только P без I. Робот стабильно недотягивает до цели из-за трения — нужна интегральная часть.
Слишком большой Kp. Система раскачивается и идёт вразнос вместо плавного выхода на цель.
Путать уровни. /cmd_vel — это желание системы; PID мотора — отдельный быстрый контур обратной связи.

Итоги

cmd_vel (v, w) переводится в обороты колёс формулами дифференциального привода.
PID удерживает заданную скорость по обратной связи от энкодера.
P реагирует на ошибку, I убирает постоянное отклонение, D гасит колебания.
PID мотора работает быстро на нижнем уровне, ROS задаёт цель сверху.