Симулятор mecanum: боковое движение, скольжение, дрейф

Управление

Каждый элемент управления повторяет весь манёвр при изменении, поэтому траектория следует за ползунком во время перетаскивания. Нажимать запуск не нужно.

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

Теория

Колесо mecanum может передавать усилие в пол только вдоль осей роликов. В любом другом направлении ролик просто вращается, и ничего не передаётся. Эти оси расположены под 45° к колесу, поэтому сила одного колеса на полу закреплена на одной линии, и никакой момент двигателя её не сдвинет.

Четыре такие линии с чередующимся направлением в сумме дают что угодно. Запишите скорость обода колеса относительно вектора движения шасси ξ = (vx, vy, ω), и получите по одной строке на колесо:

vᵢ = vx + sᵢ·vy + (sᵢ·xᵢ − yᵢ)·ω        sᵢ = ±1, the roller hand

Сложите четыре строки в матрицу A, и весь привод будет v = A·ξ. Получаются три следствия, и лаборатория существует, чтобы сделать каждое видимым:

Четыре уравнения, три неизвестных. Жёсткое шасси, все колёса которого сцепляются с полом, не может создавать произвольные скорости обода — лишнее уравнение всегда говорит FL + FR = RL + RR. Невязка этой проверки — бесплатный детектор проскальзывания, который можно вычислить только по энкодерам. Он срабатывает при асимметричном проскальзывании — одно колесо оторвалось от пола, одно стоит на влажном пятне, одно находится не в том углу, — и полностью не замечает случаи, когда скользят все четыре, поэтому ошибка одометрии mecanum накапливается незаметно.

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

a_max = μ·g / √2

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

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

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

Алгоритм

На каждом шаге 5 ms, в таком порядке. Две интеграции позы в конце намеренно разделены — именно это разделение объясняет одометрию.

1. Считайте положение стика для текущей точки манёвра.
2. При системе координат, связанной с полем, поверните в систему координат робота: vx' = vx·cos θ + vy·sin θ, vy' = −vx·sin θ + vy·cos θ. При системе координат, связанной с роботом, этот шаг полностью пропускается.
3. Обратная кинематика — v = A·ξ, с матрицей, в которую верит прошивка, а она всегда правильная. О неправильно установленном колесе прошивка узнать не может.
4. Ограничьте — масштабируйте весь вектор по худшему колесу или ограничьте каждое колесо отдельно.
5. Запаздывание привода колёс — каждый обод стремится к своей цели с реакцией первого порядка 0.1 s, потому что контур скорости хорош, но не мгновенен.
6. То, что требуют колёса, — прямая кинематика методом наименьших квадратов по скоростям обода с матрицей робота, который фактически построен.
7. Решите силы, которые согласовали бы шасси, затем примените два предела по порядку: двигателя (момент всегда ограничен) и пола (μN/√2 на колесо).
8. Избыточная сила, которую пол не передаёт, вращает обод. Этот избыток ускоряет колесо относительно заданной скорости, то есть создаёт пробуксовку, и это единственный симптом проскальзывания, который энкодер вообще может сообщить.
9. Интегрируйте истину от действительно приложенных сил.
10. Интегрируйте одометрию по скоростям обода через матрицу прошивки, потому что энкодер прикреплён к колесу, а не к полу.

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

Управляемые эксперименты

Каждый эксперимент изолирует одну переменную. Запускайте их по порядку: они последовательно усложняются.

1. Ради чего вы купили колёса mecanum

Загрузите Квадрат, который сходится. Робот проезжает метр вперёд, метр вправо, назад и влево и возвращается на 17 mm от начальной точки, при этом показание курса не сходит с 0.0°. Затем переключите манёвр на Квадрат с поворотами на углах — тот же квадрат, но выполненный так, как приходится роботу с дифференциальным приводом. Он тоже сходится, до 4 mm. Запомните, что здесь работают оба варианта.

Что наблюдать: квадрат боком перемещается по двум осям и при этом совсем не меняет курс. Ни один привод на дифференциальном принципе не может сделать такое.

2. Заберите пол

Перетащите сцепление пола с 0.85 до 0.25, оставив квадрат боком. Затем сделайте то же самое для квадрата с поворотами.

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

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

3. Посмотрите, как врут энкодеры

Загрузите Тот же квадрат, скользкий пол и посмотрите на две линии левой панели. Пунктирная линия одометрии рисует аккуратный квадрат; сплошная — нет. Робот заканчивает в 225 mm от дома, а его собственные энкодеры помещают его в 133 mm в другое место, после того как под колёсами прошло 1.27 m пола, хотя робот вообще не сдвинулся.

Теперь посмотрите на показание Расхождение энкодеров. Всё время оно остаётся 0.000.

Что наблюдать: бесплатная остаточная проверка — FL + FR должно равняться RL + RR на любом жёстком шасси — замечает одно проскальзывающее колесо и полностью слепа к тому, что скользят все четыре. Эта слепота и есть причина, по которой одометрия mecanum тихо отказывает. Установите IMU или опорные колёса и перестаньте доверять энкодерам привода для определения положения.

4. Предел √2

Загрузите Диагональный рывок. Скорость стика 0.60 m/s, как и верхняя скорость колёс, поэтому команда должна быть выполнима. Она не выполнена: команда ограничивается 91% времени, а робот достигает максимума 0.424 m/s — это 0.60/√2 с точностью до трёх знаков после запятой, достигнутый ровно.

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

5. Масштабирование против ограничения

Переключите манёвр на движение с вращением, увеличьте вращение стика до 240 °/s и переключите Превышение предела между двумя настройками. Форма траектории изменится.

Что наблюдать: масштабирование снижает скорость по каждой оси, но сохраняет направление. Ограничение сохраняет скорость вперёд и теряет 27% вращения — робот поворачивается на 127° за запуск вместо 92° — поэтому закручивается иначе, чем ему предписали. Затем вернитесь к диагональному рывку и попробуйте тот же переключатель: ничего не происходит, потому что нерабочие колёса уже находились внутри предела. Именно так эта ошибка сохраняется после тестирования.

6. Установите колесо задом наперёд

Загрузите Одно колесо установлено задом наперёд или нажмите W на графиках. Код не изменился ни в чём.

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

7. Ориентация на поле с одного нажатия

Загрузите пресет Система координат поля с вращением, затем нажмите O, чтобы вернуться к системе координат робота, и ещё раз O, чтобы вернуть систему координат поля.

Что наблюдать: при системе координат, связанной с роботом, удержание стика вперёд во время вращения рисует розетку: робот проходит 2.34 m пути и оказывается в 0.30 m от начальной точки — 13% его усилий ушло в никуда. При системе координат, связанной с полем, тот же стик проходит 2.03 m и оказывается в 2.03 m: совершенно прямая линия, 100%, причём корпус всё время вращается. Шесть строк тригонометрии разделяют эти режимы, и это разница между роботом, которым может управлять человек, и роботом, которым не может управлять никто.

Что следует наблюдать

Если собрать семь выводов вместе:

• Движение вперёд бесплатно; боковое и диагональное — нет. Каждое колесо полностью участвует в движении вперёд. Для диагонали используется половина колёс.
• Два штрафа за корень из двух возникают из одного угла 45°. Это скорость на диагонали и сцепление в каждом направлении. Ни один нельзя купить более мощными двигателями.
• Масса исключается из формулы. Предел ускорения равен μg/√2 независимо от массы робота, что противоположно роботу, толкающему груз.
• Голономидный привод лучше дифференциального при плохом полу, потому что именно развороты дают ошибку.
• Одометрия — это цена. Не проблема настройки и не проблема более качественного энкодера: проскальзывание колёс — это способ работы привода.

Переход к реальному оборудованию

Лаборатория двадцать минут стоит в основном ради эксперимента 6. Установка колеса задом наперёд здесь ничего не стоит, а на стенде отнимает полдня, и это отказ, который вы наиболее вероятно построите сами.

Исходный код

Весь привод — это две функции. Геометрия находится ровно в одном месте.

const float LX = 0.075f;      // half the wheelbase, m (wheel centre to wheel centre / 2)
const float LY = 0.085f;      // half the track, m
const float K  = LX + LY;
const float R  = 0.0485f;     // wheel radius, m
const float V_MAX = 0.60f;    // rim speed the motors can actually hold, m/s

// Inverse kinematics + saturation. vx forward, vy LEFT, omega counter-clockwise.
void driveMecanum(float vx, float vy, float omega) {
  float w[4] = {
    vx - vy - K * omega,      // front left
    vx + vy + K * omega,      // front right
    vx + vy - K * omega,      // rear left
    vx - vy + K * omega,      // rear right
  };

  float peak = 0;
  for (int i = 0; i < 4; i++) peak = max(peak, fabsf(w[i]));
  if (peak > V_MAX) {                    // SCALE, never clip
    float s = V_MAX / peak;
    for (int i = 0; i < 4; i++) w[i] *= s;
  }

  for (int i = 0; i < 4; i++) wheelLoop[i].target = w[i];   // hand to the velocity loops
}

// Forward kinematics for odometry, and the free diagnostic.
float updateOdometry(const float rim[4], float dt) {
  float vx    = ( rim[0] + rim[1] + rim[2] + rim[3]) * 0.25f;
  float vy    = (-rim[0] + rim[1] + rim[2] - rim[3]) * 0.25f;
  float omega = (-rim[0] + rim[1] - rim[2] + rim[3]) * 0.25f / K;

  theta += omega * dt;
  poseX += (vx * cosf(theta) - vy * sinf(theta)) * dt;
  poseY += (vx * sinf(theta) + vy * cosf(theta)) * dt;

  // Four equations, three unknowns. On any rigid, gripping chassis this is zero.
  // Large and steady => a wheel is in the wrong corner. Bursty => something slipped.
  return (rim[0] + rim[1]) - (rim[2] + rim[3]);
}

Вывод этих четырёх строк и принцип работы остатка приведены в кинематике колеса mecanum. Смысл wheelLoop[i].target создаёт замкнутое управление скоростью колёс. Для сборки используйте робота с приводом mecanum.