Arsenii Iarmolinskii

Как ездить роботом по траекториям

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

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

Каким образом мы можем проехать по этой траектории?

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

Возможное решениe

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

Как разрешить этот парадокс?

Необходимо следить не за требуемой скоростью в начале траектории (она всегда будет равна скорсти робота), а за требуемым ускорением!

  1. В начальный момент мы сохраняем текущую скорость робота в вектор v, и считаем требуемое ускорение в этой точке как a.
  2. После этого, интегрируем по времени вектор v используя рассчитанное ускорение a: v(next) = v(old) + a * ts, где ts - время шага. Таким образом мы получаем новое значение скорости v, которое мы посылаем на робота.
  3. Не сбрасывая накопленное значение v, мы снова измеряем требуемое ускорение робота a и повторяем шаг 2.

Пример псевдокода


def movetraj():
    # INIT
    v = robot.vel
    a = traj.recalculate(robot, target, a_max)

    # MAIN LOOP
    while True:
        # CALCULATE NEXT VELOCITY
        v = v + a * ts
        
        # MOVE ROBOT
        robot.move(v)
        
        # CALCULATE NEXT TARGET
        a = traj.recalculate(robot, target, a_max)

Небольшие заметки

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

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

Для компенсации эффектов реального мира мы можем применить следующие подходы:

  1. Добавить ограничение на максимальное ускорение робота. Поможет при больших задержках и следующих из этого колебаниях и перерегулировании.
  2. Добавить коэффициент масштабирования ускорения (т.е. интегрировать с коэффициентом: v = v + a * ts * k). Может помочь при долгом реагировании робота на команды. Хотя скорее всего сделает только хуже, надо проверять.
  3. Добавить ограничение на максимальную разницу между измеренной скоростью робота и подаваемой как сигнал управления как функцию от требуемого ускорения. Примерно так:
def movetraj():
    # .....
    while True:
        v = v + a * ts
        v = constrain(v, robot.vel - a * k, robot.vel + a * k) # Ограничение на максимальную разницу между измеренной скоростью робота и командой управления
        robot.move(v)
        a = traj.recalculate(...)