Кинематика колёс Mecanum: четыре колеса, три оси

Один ролик решает всё

Снимите одно колесо Mecanum с робота и подержите его. Оно вращается на своей оси, как любое колесо. По ободу расположены восемь или девять бочкообразных роликов, каждый на собственной маленькой оси, каждый свободно вращается и установлен под углом 45° к оси колеса.

Теперь спросите, какую силу это колесо может передать полу.

Оно не может толкать вдоль направления качения ролика — ролик просто вращается, и ничего не передаётся. Оно может толкать только вдоль оси ролика, то есть в единственном направлении, в котором ролик не катится. Поэтому сила колеса на полу привязана к одной линии под 45° к шасси, и никакой момент двигателя не меняет эту линию.

Вот и весь механизм. Всё остальное ниже — бухгалтерия.

Ограничение для каждого колеса

Задайте обычную систему координат робота: x вперёд, y влево от робота, ω против часовой стрелки. Это соглашение ROS, и использование любой другой системы заставит вас бороться с каждой библиотекой, к которой вы когда-либо обратитесь.

Поместите колесо i в позицию (xᵢ, yᵢ) в этой системе и задайте ему направление роликов sᵢ = ±1, указывающее, вдоль какой из двух линий под 45° расположены его ролики. Тогда скорость обода vᵢ (в метрах в секунду на шине, а не в радианах) связана с вектором twist шасси соотношением:

vᵢ = vx + sᵢ·vy + (sᵢ·xᵢ − yᵢ)·ω

Три слагаемых, и каждое заслуживает отдельного предложения:

• vx — каждое колесо полностью участвует в движении вперёд независимо от направления роликов. Поэтому движение вперёд на базе Mecanum ощущается совершенно обычным.
• sᵢ·vy — движение вбок вращает половину колёс в одну сторону, а половину — в другую. Направление роликов решает, какая половина куда движется.
• (sᵢ·xᵢ − yᵢ)·ω — плечо момента. Обратите внимание, что это не просто ширина колёсной базы; направление роликов входит внутрь, поэтому колесо, установленное не в том углу шасси, портит не только боковое движение, но и вращение.

Чтобы перейти от скорости обода к команде, которую действительно получает двигатель, разделите на радиус колеса: ωwheel = vᵢ / r, в rad/s.

Четыре колеса: матрица

Сложите четыре строки, и получится матрица 4 × 3 A, где вектор twist шасси ξ = (vx, vy, ω):

v = A · ξ

Для стандартной компоновки — переднее левое и заднее правое с одним направлением роликов, переднее правое и заднее левое с другим — запишите k = lx + ly, где lx — половина колёсной базы (от передней оси к задней), а ly — половина ширины колёс (от левой к правой). Тогда:

Вот четыре строки, которые пишут все:

float fl = vx - vy - K * omega;
float fr = vx + vy + K * omega;
float rl = vx + vy - K * omega;
float rr = vx - vy + K * omega;

Проверьте результат по четырём вещам, которые робот на Mecanum уже умеет:

Разобранный пример

У базы с lx = 75 mm, ly = 85 mm и колёсами 97 mm (r = 48.5 mm) задана команда vx = 0.30 m/s, vy = 0.20 m/s, ω = 0.50 rad/s. Поэтому k = 0.160 m.

Двадцатидевятикратная разница в скорости одного колеса относительно другого возникла из команды, которая не выглядит экстремальной. Привод Mecanum большую часть времени заставляет четыре двигателя делать совершенно разные вещи, поэтому «подать всем одинаковый PWM, и робот поедет прямо» здесь не является проверкой, имеющей смысл.

Насыщение: масштабировать, а не обрезать

Запросите больше, чем могут двигатели, и придётся отказаться от чего-то. Есть ровно два варианта, и они неэквивалентны.

float peak = max(max(fabs(fl), fabs(fr)), max(fabs(rl), fabs(rr)));
if (peak > V_MAX) {            // SCALE: keep the direction, lose the speed
  float s = V_MAX / peak;
  fl *= s; fr *= s; rl *= s; rr *= s;
}

В отличие от того, что произойдёт, если просто написать constrain(fl, -255, 255) для каждого двигателя: это обрезание. Обрезание сохраняет скорость и выбрасывает направление. Запросим движение вперёд 0.60 m/s, вбок — 0.15 и вращение 0.8 rad/s на роботе, максимальная скорость колёс которого равна 0.50 m/s:

Самое неприятное здесь в том, что обрезание выглядит нормально на командах, которые обычно проверяют. Чистая диагональная команда запрашивает 2 для двух колёс и 0 для двух других; нули уже находятся в пределах, поэтому обрезание и масштабирование дают одинаковые числа, и ошибка скрывается. Позже она проявляется при работе системы следования по пути: робот выходит из поворотов без видимой причины. Посмотрите на оба ограничителя в одном прогоне, и разница станет видимой кривой.

Движение назад: одометрия

Прямая кинематика — это та же матрица, прочитанная наоборот. Но A имеет размер 4 × 3 — четыре измерения и три неизвестных, поэтому обратной матрицы нет. Есть решение методом наименьших квадратов:

ξ = (Aᵀ A)⁻¹ Aᵀ v

Для стандартной компоновки AᵀA получается диагональной, и всё выражение сворачивается в три строки, которые можно поместить на Arduino:

vx    = (fl + fr + rl + rr) / 4.0;
vy    = (-fl + fr + rl - rr) / 4.0;
omega = (-fl + fr - rl + rr) / (4.0 * K);

Интегрируйте это через курс робота на каждом тике, и получите счисление координат:

float dt = (now - last) * 1e-6f;
theta += omega * dt;
x += (vx * cosf(theta) - vy * sinf(theta)) * dt;
y += (vx * sinf(theta) + vy * cosf(theta)) * dt;

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

Четвёртое уравнение, которым никто не пользуется

Четыре уравнения, три неизвестных. Лишнее уравнение — не отходы, а единственная бесплатная диагностика всего привода.

Если робот жёсткий и каждое колесо имеет сцепление, четыре скорости обода не могут быть произвольными. Сложите первые две строки матрицы и последние две строки, и обе суммы дадут 2·vx, поэтому:

FL + FR = RL + RR

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

float residual = (fl + fr) - (rl + rr);   // should be ~0 forever

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

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

Два следствия угла 45°

Оба следуют из матрицы, и ни одно нельзя устранить покупкой чего-либо.

Диагональное движение ограничено значением 1/√2. Команда под 45° задаёт vx + vy двум колёсам и 0 двум другим, поэтому нагруженная пара достигает предела скорости, когда робот движется только со скоростью V_MAX / √2 ≈ 71% своей максимальной скорости по прямой. Более быстрые двигатели поднимают предел, но не устраняют его.

Сцепление также ограничено значением 1/√2, но по другой причине. Полезная сила привода двигателя F направлена вдоль направления качения, но фактическая сила на полу равна F·√2 вдоль оси ролика, и трение ограничивает именно её. Поэтому на каждое колесо полезная сила ограничена значением μN/√2, а предел ускорения робота равен:

a_max = μ·g / √2

При μ = 0.85 это 5.9 m/s², тогда как тот же робот на обычных колёсах имел бы 8.3. Масса робота сокращается в обоих выражениях, поэтому балласт не помогает — урок, прямо противоположный роботу сумо, где вес является всей стратегией.

В прошивке

Разместите геометрию в одном месте и никогда не вводите её дважды.

struct Mecanum {
  float lx, ly, r;                 // half wheelbase, half track, wheel radius (m)
  float k() const { return lx + ly; }

  // twist -> four wheel angular velocities, rad/s
  void inverse(float vx, float vy, float w, float out[4]) const {
    out[0] = (vx - vy - k() * w) / r;   // FL
    out[1] = (vx + vy + k() * w) / r;   // FR
    out[2] = (vx + vy - k() * w) / r;   // RL
    out[3] = (vx - vy + k() * w) / r;   // RR
  }

  // four measured wheel angular velocities -> twist, plus the residual
  float forward(const float in[4], float &vx, float &vy, float &w) const {
    float fl = in[0] * r, fr = in[1] * r, rl = in[2] * r, rr = in[3] * r;
    vx = (fl + fr + rl + rr) * 0.25f;
    vy = (-fl + fr + rl - rr) * 0.25f;
    w  = (-fl + fr - rl + rr) * 0.25f / k();
    return (fl + fr) - (rl + rr);       // the free diagnostic
  }
};

Измеряйте lx и ly от центра колеса до центра колеса, а затем делите пополам, а не по платформе шасси, которая обычно шире. Неправильное k не остановит движение робота; оно смешает вращение и поступательное движение, поэтому команда чистого вращения будет ползти, а чистое боковое движение — заворачиваться. Если робот ползёт при вращении на месте, сначала подозревайте k, а не код.

Диагностика

Далее

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