二连杆机器人手臂的逆运动学求解

正运动学很容易：给定关节角，把连杆变换乘起来，你就知道工具在哪儿。逆运动学反着跑——给定一个目标位置，找出够到它的关节角——而有趣的问题都住在那个方向上。平面二连杆臂是把它们全部展示出来的最小例子。在机械臂模拟器里探索它。

可达工作空间

连杆长 L₁、L₂，工具只能够到环带（一个圆环）内的点。外半径是 L₁ + L₂（臂完全伸直），内半径是 |L₁ − L₂|（臂完全折叠）。环外太远，洞内太近。没有任何关节角能到那些点，所以正确的答案就是“到不了”。

两个解：肘上与肘下

对环内大多数目标点，肘部可以朝两个方向弯而仍然把工具放在同一个点上。这就是肘上和肘下两种配置。它们来自第二关节余弦定理角度里的那个正负号：

cos(θ₂) = (x² + y² − L₁² − L₂²) / (2·L₁·L₂)
θ₂ = ± acos(cos(θ₂))        # + is one elbow, − is the other
θ₁ = atan2(y, x) − atan2(L₂·sin θ₂, L₁ + L₂·cos θ₂)

因为有两个有效答案，逆运动学不是一个函数——它是一个选择。机器人会挑那个让关节远离限位、把移动量降到最低、或者避开障碍的配置。

奇异

在工作空间的边缘——臂完全伸直或完全折叠——两个解合并成一个。这些地方就是奇异，而在它们附近，工具的一点小动作可能要求关节的大幅动作。规划器会在奇异位姿附近放慢速度或绕开它，因为在那里臂会短暂地失去沿某个方向移动工具的能力。

什么时候无解？

目标落在可达环之外时无解，需要一个硬件禁止的关节角时无解，需要一个可用关节产生不了那个朝向时也无解。一个好的模拟器会诚实地把这几种情况区分开，而不是吸附到最近的位姿、假装自己成功了。

解析求解器与数值求解器

两三根连杆时几何有闭式（解析）解，所以你可以直接、精确、快速地算出关节角。这让解析逆解成为教学和测试的理想选择。冗余的或完全空间的手臂通常没有闭式解，需要迭代逼近解的数值求解器——更通用，但更慢，而且更难推理，因为它们可能漏解或者在奇异附近停滞。

把求解器写安全

上面那些方程是正确的，而在真实硬件上它们仍然会产出 NaN。下面是同样的数学，只是每一次失败都显式处理：

enum IkStatus { IK_OK, IK_TOO_FAR, IK_TOO_CLOSE, IK_JOINT_LIMIT, IK_SINGULAR };

struct IkResult {
  IkStatus status;
  float theta1, theta2;      // radians
};

IkResult solveIk(float x, float y, float L1, float L2, bool elbowUp) {
  IkResult r{};

  const float r2 = x * x + y * y;
  const float reach = sqrtf(r2);

  // 1. Reachability FIRST, before any trigonometry.
  //    A small epsilon keeps a target exactly on the boundary from
  //    producing an acos argument of 1.0000001 through rounding.
  const float eps = 1e-4f;
  if (reach > L1 + L2 - eps)          { r.status = IK_TOO_FAR;   return r; }
  if (reach < fabsf(L1 - L2) + eps)   { r.status = IK_TOO_CLOSE; return r; }

  // 2. Now acos is guaranteed to be in range.
  float c2 = (r2 - L1 * L1 - L2 * L2) / (2.0f * L1 * L2);
  c2 = constrain(c2, -1.0f, 1.0f);              // belt and braces
  const float s2 = sqrtf(1.0f - c2 * c2);

  r.theta2 = elbowUp ? atan2f(s2, c2) : atan2f(-s2, c2);

  // 3. atan2, never atan — this is what gets all four quadrants right.
  r.theta1 = atan2f(y, x) - atan2f(L2 * sinf(r.theta2), L1 + L2 * cosf(r.theta2));

  // 4. Warn near the boundary, where the arm is ill-conditioned.
  if (reach > (L1 + L2) * 0.95f) { r.status = IK_SINGULAR; return r; }

  // 5. Mechanical limits are part of the answer, not an afterthought.
  if (r.theta1 < T1_MIN || r.theta1 > T1_MAX ||
      r.theta2 < T2_MIN || r.theta2 > T2_MAX) { r.status = IK_JOINT_LIMIT; return r; }

  r.status = IK_OK;
  return r;
}

有五处细节让它区别于教科书版本，每一处都对应一次真实的故障：

**先判可达，再算三角。**目标在 L1 + L2 之外时，余弦定理会给出一个落在 −1 到 1 之外的参数，acos 返回 NaN。那个 NaN 接着就流进舵机指令，变成一个垃圾角度——这正是“够不到”的目标往往产生一次剧烈移动而不是一次拒绝移动的原因。

**那个极小余量。**一个算出来正好落在边界上的目标，经过浮点舍入后可能给出 c2 = 1.0000001。用一个小余量测可达性，能在 acos 之前抓住它。

**用 atan2f，不用 atan。**单参数的 atan 只取一个商，分不出前面的目标和它背后镜像的那个，于是臂在两个象限里正常、在另外两个象限里镜像。这是第一个逆解实现里最常见的缺陷。

**给奇异警告而不是拒绝。**接近全伸时解仍然有效，但病态——一点点工具动作就要求巨大的关节动作。标出来就能让调用方放慢或绕开。

**把关节限位当成一种状态。**一条大臂不能低于 −10°，于是就存在一些几何上可达、机械上不可达的目标。悄悄钳位会产出一台去了目标附近某个地方却报告成功的臂。

在两个解之间选

数学把两个分支都给你，却没法告诉你该用哪个。那是一个机器人决策，常见的规则有四条：

最小移动量是正确的默认值，而理由在于不这样做会发生什么：如果求解器在路径中途悄悄切换分支，臂就会翻过一次幅度大、速度快的、完全没被计划过的动作。在真实的臂上，那既吓人又可能造成破坏。

// Pick the branch closest to where the arm already is.
IkResult up   = solveIk(x, y, L1, L2, true);
IkResult down = solveIk(x, y, L1, L2, false);

if (up.status == IK_OK && down.status == IK_OK) {
  const float costUp   = fabsf(up.theta1   - cur1) + fabsf(up.theta2   - cur2);
  const float costDown = fabsf(down.theta1 - cur1) + fabsf(down.theta2 - cur2);
  return (costUp <= costDown) ? up : down;
}
return (up.status == IK_OK) ? up : down;

直线运动每一步都要逆解

在关节空间里在两个位姿之间插值——让每个关节从起始角线性走到终止角——很简单，而且工具会沿一条曲线跟随。很多任务这样就够。但任何必须沿直线走的场合，都要在笛卡尔空间里插值，并在每一步解一次逆解：

void moveStraight(float x0, float y0, float x1, float y1, int steps) {
  for (int i = 1; i <= steps; i++) {
    const float t = (float)i / steps;
    const float x = x0 + (x1 - x0) * t;
    const float y = y0 + (y1 - y0) * t;

    IkResult r = solveClosest(x, y);
    if (r.status != IK_OK) { abortMove(r.status); return; }   // do not skip the point
    commandJoints(r.theta1, r.theta2);
  }
}

第一次这么做就会有两件事变得明显。关节速度并不恒定，尽管工具速度是——在工作空间边缘，相同的工具动作要求关节动得快得多。而一条直线可能穿过臂够不到的区域，哪怕两个端点都没问题，这正是循环内部那次状态检查重要、而只查端点不够的原因。

从关节角到舵机计数

求解器返回的是一个数学坐标系下的关节角。舵机对那个坐标系一无所知：它的零位是舵盘装在哪里的地方，而且它可能朝相反方向转。

struct Joint {
  uint8_t channel;
  float   offsetDeg;   // servo angle when the joint is at mathematical zero
  int8_t  sign;        // +1 or -1
  float   minDeg, maxDeg;
};

int servoAngle(const Joint &j, float jointRad) {
  const float deg = jointRad * 57.2957795f;
  const float clamped = constrain(deg, j.minDeg, j.maxDeg);
  return (int)(j.offsetDeg + j.sign * clamped);
}

那个结构体就是运动学与硬件之间整座桥，而把它显式化，正是让你能把求解器缺陷和标定缺陷区分开的原因。一台运动平滑却落在离目标老远地方的臂，几乎总是这个原因，而不是数学。

故障排查

自己试

在机械臂模拟器里拖着目标走，看臂实时求解。切换肘部分支去看另一个解，把目标推到环外去看一个诚实的“到不了”，朝工作空间边缘移动去体会奇异。