一个经得起失败的拾取与放置状态机

为什么演示行、系统不行

人人的第一版取放都是一串 delay() 调用。走到这，合爪，走到那，张爪。拍视频行，台架上一百次成八十次，感觉接近完成了。

离完成还远，因为二十次失败全是静默的：夹爪合了个空，手臂抱着空气过桌子，在料盒上空张开。零件稍歪，没夹起来反而撞倒。抬到一半舵机欠压复位，控制器重启进未知位形还合着爪。

把这些灾难变成可恢复的只有一样：抓取和抬起之间的验证步骤，以及验证失败时有处可去的状态机。这就是本文的全部主题。

状态

取放循环九个状态，值得全命名，不合并。

两个设计点干了大部分活。

接近和撤退位形是独立状态，在物体上方。 夹爪从任意方向直扑零件会撞倒它，沿曲线抬起会在脱离前横向拖拽。永远从正上方（或沿插入轴）走一小段直线进出。

VERIFY 是状态，不是一行代码。 它是循环里机器唯一发现现实与计划是否相符的地方，失败要有处可去。

验证抓取

四种办法，最便宜先行，至少要一种。

爪位检查（免费）

夹爪有串联柔顺时，夹到东西的爪停在比空合更开的位置。能读爪位，对比空合位置，就知道指间有东西。

普通业余级舵机读不到位置——开环。但把舵机电位器抽头引到 ADC 脚，一根线的代价就有这个，改装十分钟。

局限： 只知道有东西，不知道是想要的东西，也不知道夹得牢不牢。

指上微动开关（约 50 便士）

一指内侧装杠杆微动开关，接上拉输入脚。垫块碰到东西即闭合。

这是性价比断层第一，直接用数字回答“指间有没有东西”，皮实耐造，软件三行消抖。装在手指柔顺段，保证以可重复的力可靠触发。

电流检测（约 2 英镑）

舵机供电串分流电阻或霍尔传感器。空合的舵机电流回落到待机，夹到东西的舵机维持高电流。

局限： 噪声大，阈值要标定，电池一下垂就错。想检压坏而不只检夹住时值——电流远超预期意味着零件比预期硬，或打偏夹了夹具。

爪间对射（约 1 英镑）

爪口两侧红外发射加光敏接收，光束断了说明间隙里有东西。

局限： 没夹也看得见，验的是存在而非抓牢，配别的方法用。在视觉搞不定的透明或深色零件机器上是真好用。

写法

套路是普通有限状态机加非阻塞定时，循环里什么都不卡，急停一直灵敏：

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. 位形来自感知而非常量，这是从演示到机器的一步。

下一步

• 机器人行为的有限状态机——本文是该通用模式的一个实例
• 舵机夹爪力、堵转与柔顺——停留为什么存在、爪为什么不应常合
• 机器人夹爪类型与夹持力——时序依赖的抓取如何配
• 夹爪模拟器——按自己的数找到抓取松开的瞬间