Роботизированная рука 4-DOF на PCA9685: сервоприводы и кинематика

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

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

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

Перечень компонентов

Итого: примерно $40–60.

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

Измерьте руку до написания кода

Обратная кинематика — это арифметика по длинам звеньев. Эти четыре числа составляют весь входной набор, и получение их из карточки товара вместо измерения линейкой — самая частая причина того, что рука уверенно тянется не туда.

Последняя строка выполняет больше работы, чем кажется. Фактическая точка захвата схвата перемещается при смыкании губок, и выбор размера схвата — отдельная задача, отличная от его позиционирования, со своими ограничениями, арифметикой и режимами отказа. Если рука достаёт до детали, а затем роняет или раздавливает её, ответ не на этой странице: начните с симулятора схвата. | h | Поверхность стола | Ось сустава плеча | 50–70 mm | | d | Ось вращения основания | Горизонтально до оси плеча | 0–15 mm |

Измеряйте между осями суставов, а не по краям кронштейнов. Ось — центр выходного шлица сервопривода, часто на 5–10 mm внутри видимого кронштейна. На звене 95 mm ошибка 10 mm превышает 10% и создаёт руку, которая систематически не достаёт до цели и не исправляется никаким коэффициентом.

Эти две длины сразу дают рабочее пространство:

maximum reach = L1 + L2          e.g. 100 + 90 = 190 mm
minimum reach = |L1 - L2|        e.g. |100 - 90| = 10 mm

Рабочее пространство — кольцевая область, а не диск. В центре есть область, до которой рука физически не достаёт, и если цель рядом с основанием, ответ «недостижимо» корректен, а не является ошибкой.

Сборка по этапам

Порядок сборки содержит один шаг, который почти невозможно отменить, — это этап 2.

Восстановиться после ошибки на этапе 2 невозможно. Рычаг сервопривода шлицевой — примерно на 20 зубьев, — поэтому устанавливается только в дискретных положениях с шагом около 18°. Если сначала собрать механизм, а затем калибровать, может оказаться, что рабочий диапазон сустава смещён на 40° и исправить это механически невозможно. Подайте питание на каждый сервопривод, задайте 90°, затем устанавливайте рычаг.

Всегда перемещайте руку плавно

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

// Interpolate every joint together, so the tool takes a sensible path and
// no single servo carries the whole transition.
void moveTo(float base, float shoulder, float elbow, int steps, int stepMs) {
  static float cur[3] = {0, 0, 0};
  const float target[3] = {base, shoulder, elbow};

  for (int s = 1; s <= steps; s++) {
    const float t = (float)s / steps;
    for (int j = 0; j < 3; j++) {
      const float a = cur[j] + (target[j] - cur[j]) * t;
      pwm.setPWM(JOINT[j].channel, 0, countsFor(JOINT[j], a));
    }
    delay(stepMs);
  }
  memcpy(cur, target, sizeof(cur));
}

Интерполяция всех суставов одновременно важна не меньше самого сглаживания. Перемещение суставов по одному отправляет рабочий орган по странной дуге через рабочее пространство — медленнее и с гораздо большей вероятностью столкнуться с чем-либо по пути.

Как выглядит хороший результат

Разница между двумя строками о повторяемости и есть люфт, и это главный источник ошибки в любительской манипуляционной системе. Люфт в четыре градуса на плече при рабочем диапазоне 190 mm даёт более 13 mm у схвата. Практическое решение заключается не в лучшей математике, а в том, чтобы всегда подходить к цели с одного и того же направления: это полностью устраняет смену знака и превращает ошибку ±13 mm в повторяемое смещение, которое можно откалибровать.

Диагностика неисправностей

Куда двигаться дальше

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

Добавьте камеру и замкните контур. Всё здесь работает без обратной связи: рука перемещается туда, куда ей указали, и ничего не проверяет. Камера, считывающая маркер ArUco на объекте, позволяет руке компенсировать ошибку от люфта и прогиба, и так недорогой манипулятор становится точным.

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