Конечный автомат захвата и перемещения, устойчивый к отказам

Почему демонстрация работает, а система — нет

Первая попытка захвата и перемещения у каждого — это список вызовов delay(). Сюда, закрыть захват, туда, открыть. На видео это работает, и на стенде примерно в восьми случаях из ста, что кажется почти завершённым результатом.

Это далеко не завершённый результат, потому что двадцать отказов проходят безмолвно. Захватный механизм закрылся в пустоте, и манипулятор перенёс воздух над столом, открывшись над контейнером. Деталь стояла под небольшим углом и была сбита вместо захвата. Сервопривод потерял питание в середине подъёма, и контроллер перезагрузился в неизвестную позу с закрытыми губами.

То, что превращает катастрофы в восстановление, состоит из одной вещи: шага проверки между захватом и подъёмом и конечного автомата, у которого есть переход на случай отказа. Вся эта страница посвящена именно этому.

Состояния

Цикл захвата и перемещения состоит из девяти состояний, и стоит перечислить все девять, а не сливать.

Два проектных решения здесь выполняют большую часть работы.

Позы подхода и отхода — отдельные состояния над объектом. Захватный механизм, который ныряет прямо к детали из произвольного направления, сбивает её, а захватный механизм, который поднимает её по кривой, сначала тащит деталь вбок, прежде чем она освободится. Всегда подходите строго сверху (или непосредственно вдоль оси вставки) коротким прямым участком и выходите так же.

VERIFY — это состояние, а не строка кода. Это единственное место в цикле, где машина узнаёт, совпала ли реальность с планом, и здесь должно быть место, куда она может перейти при неудаче.

Проверка захвата

Четыре способа, от самого дешёвого к самому дорогому. Нужен хотя бы один.

Проверка положения губ (бесплатно)

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

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

Ограничения: сообщает, что что-то есть, но не что это именно нужный объект и что он хорошо удерживается.

Микровыключатель на пальце (около 50p)

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

Это лучший вариант по соотношению цены и пользы. Он даёт прямой цифровой ответ на вопрос «есть ли что-то между губами», выдерживает любые воздействия и требует для устранения дребезга три строки программного обеспечения. Разместите его на податливой части пальца, чтобы он надёжно срабатывал при повторяемом усилии.

Измерение тока (около £2)

Шунт или датчик Холла в цепи питания сервопривода. Сервопривод, закрывающийся в воздухе, снижает ток до холостого; сервопривод, закрывающийся на объекте, сохраняет повышенный ток.

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

Луч, пересекающий раскрытие губ (около £1)

ИК-излучатель (IR) и фототранзистор по краям раскрытия. Разрыв луча означает, что в промежутке есть объект.

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

Как это написать

Шаблон — обычный конечный автомат с неблокирующим таймером, чтобы ничего в цикле не останавливалось и аварийный останов оставался отзывчивым:

enum State { IDLE, APPROACH, DESCEND, GRASP, VERIFY, LIFT, TRANSFER, PLACE, RECOVER };

State state = IDLE;
uint32_t stateEnteredAt = 0;
uint8_t  attempts = 0;

const uint8_t  MAX_ATTEMPTS = 3;
const uint16_t GRASP_DWELL_MS = 350;   // let the compliance settle before you trust it
const uint16_t SETTLE_MS      = 250;

void enter(State next) { state = next; stateEnteredAt = millis(); }
bool elapsed(uint16_t ms) { return millis() - stateEnteredAt >= ms; }

// Debounced: one reading is a glitch, three in a row is a fact.
bool objectHeld() {
  static uint8_t streak = 0;
  streak = digitalRead(GRIP_SWITCH) == LOW ? min(streak + 1, 3) : 0;
  return streak >= 3;
}

void loop() {
  arm.update();          // non-blocking motion, every pass
  gripper.update();

  switch (state) {
    case IDLE:
      if (jobPending()) { attempts = 0; arm.moveTo(approachPose); enter(APPROACH); }
      break;

    case APPROACH:
      gripper.open();
      if (arm.arrived() && elapsed(SETTLE_MS)) { arm.moveTo(graspPose); enter(DESCEND); }
      break;

    case DESCEND:
      if (arm.arrived()) { gripper.close(); enter(GRASP); }
      break;

    case GRASP:
      // The dwell is not politeness. A compliant finger rings after closing,
      // and reading the switch during the ring gives you a coin flip.
      if (elapsed(GRASP_DWELL_MS)) enter(VERIFY);
      break;

    case VERIFY:
      if (objectHeld()) { arm.moveTo(approachPose); enter(LIFT); }
      else              { enter(RECOVER); }
      break;

    case LIFT:
      if (arm.arrived()) { arm.moveTo(placeApproach); enter(TRANSFER); }
      break;

    case TRANSFER:
      if (arm.arrived()) { arm.moveTo(placePose); enter(PLACE); }
      break;

    case PLACE:
      if (arm.arrived() && elapsed(SETTLE_MS)) {
        gripper.open();
        // Open BEFORE retreating, and retreat straight up, or you drag the part.
        if (elapsed(SETTLE_MS + 200)) { arm.moveTo(placeApproach); enter(IDLE); }
      }
      break;

    case RECOVER:
      gripper.open();
      if (elapsed(SETTLE_MS)) {
        if (++attempts < MAX_ATTEMPTS) { arm.moveTo(approachPose); enter(APPROACH); }
        else { raiseFault("grasp failed after 3 attempts"); enter(IDLE); }
      }
      break;
  }
}

Три детали здесь оправдывают своё место.

Выдержка после захвата. Податливый палец колеблется несколько сотен миллисекунд после закрытия. Проверка во время колебаний даёт результат, похожий на подбрасывание монеты. Обычно достаточно 300–400 ms; измерьте своё значение, записывая состояние выключателя в течение секунды после закрытия.

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

Ограниченное число повторов. Бесконечные повторы — это способ, которым робот проводит всю ночь, перетирая деталь, упавшую на пол час назад. Три попытки, затем неисправность, которую должен устранить человек.

Что делать, если повтор не помогает

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

1. Сместите подход. При каждом повторе смещайте позу захвата на пару миллиметров по небольшой спирали. Это стоит трёх строк и восстанавливает удивительную долю неудач.
2. Повторите измерение. Если есть камера или датчик расстояния, снимите новое показание, не полагаясь на заданную позу.
3. Увеличьте раскрытие. Раскрытие губ, точно рассчитанное на номинальную деталь, не сработает, если деталь повёрнута. При повторе открывайте шире.
4. Откажитесь громко. Неисправность, которая останавливает машину и сообщает, в каком состоянии она находилась, лучше машины, которая продолжает работать и молча наполняет контейнер отходов.

Отказы, в которых виноват не захватный механизм

Когда захват и перемещение работают ненадёжно, причина часто не в захвате:

Четвёртая строка заслуживает отдельного внимания, потому что именно она больше всего похожа на загадку. Захват, который идеально удерживает деталь на стенде, может отпустить её через 40 ms после начала подъёма — ещё до того, как манипулятор заметно сдвинется, — потому что преобладает составляющая, обусловленная ускорением. Симулятор захватного механизма покажет точный момент для ваших чисел, а исправление обычно состоит в замедлении профиля, а не в более сильном сжатии.

Что добавить дальше

Когда цикл станет надёжным, следующими по ценности дополнениями будут:

1. Подсчёт циклов и журнал неисправностей. Нельзя улучшить показатель надёжности, который не измеряется.
2. Полноценный профиль движения для переноса, чтобы ускорение было ограниченным и не приходилось делать захват слишком сильным ради пережидания резких движений.
3. Позу из данных датчиков, а не из констант — это шаг от демонстрации к машине.

Далее

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