three 实现简单机械臂逆运动

很久没写博客了。原来那种事无巨细地铺开技术细节的写法，现在看性价比已经不高了，如何有结构地和 AI 协作反而更重要。这篇就从个人视角回顾一个具体案例。

开始

25 年底机缘巧合需要使用 Unity，我在接近零基础的情况下借助 Coding Agent，快速迁移和扩展了已有知识，把一个角色绕圆柱世界移动的需求拆成了运动合成问题，并通过大量试验做 trade-off，解决了视角抖动和偏移。虽然项目本身只是个半成品，但随着对话轮数增加，工程量还是涨得很快。整个过程在几周内完成，这样的迭代速度让我意识到可以去探索一些更有意思的东西，于是我开始花时间了解机器人的运动学。

机械臂建模

一开始最先遇到的问题是，机械臂该怎么定义。这个问题很快收敛到用 DH 参数和改进型 DH 参数（MDH）来描述机械结构。我原本以为这部分会很简单，无非是父子层级关系下的一套坐标系嵌套，但真正实现时还是踩了不少坑。后来通过几套结构反复试验、补上可视化 gizmo、再结合一些 AI 给出的改进方向，才把这部分逐渐理顺。这部分我单独整理到了 wwjll.github.io/three-chamb... ，希望对开始该领域学习的朋友有所帮助。

继续探索

这套东西本身已经很成熟了。我一开始就确定不想用 FABRIK、CCD 这类游戏里常见的巧解，也不想走基于几何关系推导的纯几何解析，因为一旦到了多轴机械臂，复杂度会迅速上升，而且不够通用。这样问题就明确了下来，需要引入 Jacobian matrix 来同时表达旋转和位移。问题到这里虽然变精确了，但也开始发散，因为我并不知道这些量在工程里到底一一对应什么。继续追问之后，AI 默认我已经有了相关背景，开始往工业解法和动力学方程上带。那时我的状态大概就是，"字都认识，意思不清楚"。

问题收敛

在继续扩展之前，我先把问题压到一个能实现的范围里。我想要的只是一个基于位移和旋转的简单可视化求解过程，用 JavaScript 跑在网页里，而不是把 Mujoco 一整套搬进来。于是我给自己定了一个边界：忽略电机参数、编码器噪声、动力学方程这些工业机械臂必须面对的内容，只保留 forward kinematics 和 inverse kinematics，用它们来定义 position error 和 orientation error。这样问题的复杂度就收敛到了一个我当时还能继续推进的层级。

问题定义

把问题压到最小以后，inverse kinematics 的目标就可以写得很直接。设机械臂当前 joint variables 为 <math xmlns="http://www.w3.org/1998/Math/MathML"> q ∈ R n \mathbf q \in \mathbb R^n </math>q∈Rn，end effector 当前 pose 记为 <math xmlns="http://www.w3.org/1998/Math/MathML"> x ( q ) \mathbf x(\mathbf q) </math>x(q)，目标 pose 记为 <math xmlns="http://www.w3.org/1998/Math/MathML"> x d \mathbf x_d </math>xd。要做的并不是一次性"解出答案"，而是每一帧根据当前位置和目标位置之间的误差，算出一小步 joint increment <math xmlns="http://www.w3.org/1998/Math/MathML"> Δ q \Delta \mathbf q </math>Δq，让机械臂逐步逼近目标。

如果只看微分关系，这件事可以写成：
<math xmlns="http://www.w3.org/1998/Math/MathML" display="block"> Δ x ≈ J ( q )   Δ q \Delta \mathbf x \approx \mathbf J(\mathbf q)\,\Delta \mathbf q </math>Δx≈J(q)Δq

其中 <math xmlns="http://www.w3.org/1998/Math/MathML"> J ( q ) \mathbf J(\mathbf q) </math>J(q) 是 Jacobian matrix，它描述了"每个关节动一点点，end effector 的 pose 会怎样变化"。于是 inverse kinematics 的核心就变成了：已知末端误差 <math xmlns="http://www.w3.org/1998/Math/MathML"> e \mathbf e </math>e，求一个合适的 <math xmlns="http://www.w3.org/1998/Math/MathML"> Δ q \Delta \mathbf q </math>Δq，使得
<math xmlns="http://www.w3.org/1998/Math/MathML" display="block"> J ( q )   Δ q ≈ e \mathbf J(\mathbf q)\,\Delta \mathbf q \approx \mathbf e </math>J(q)Δq≈e

这里的 error vector 通常拆成 position error 和 orientation error 两部分：
<math xmlns="http://www.w3.org/1998/Math/MathML" display="block"> e = [ e p e r ] ∈ R 6 \mathbf e = \begin{bmatrix} \mathbf e_p \\ \mathbf e_r \end{bmatrix} \in \mathbb R^6 </math>e=[eper]∈R6

其中 position error 可以直接写成
<math xmlns="http://www.w3.org/1998/Math/MathML" display="block"> e p = p d − p \mathbf e_p = \mathbf p_d - \mathbf p </math>ep=pd−p

orientation error 则不再直接拿 Euler angles 相减，而是更常见地转成 axis-angle 或 rotation vector 来表达。也就是说，我真正需要求解的不是"角度本身"，而是一个能让当前姿态朝目标姿态旋过去的微小旋转量。

如果 <math xmlns="http://www.w3.org/1998/Math/MathML"> J \mathbf J </math>J 恰好是方阵且可逆，形式上可以写成
<math xmlns="http://www.w3.org/1998/Math/MathML" display="block"> Δ q = J − 1 e \Delta \mathbf q = \mathbf J^{-1}\mathbf e </math>Δq=J−1e

但这在真实问题里几乎不是常态。机械臂经常会遇到下面几种情况：

• 关节数和任务维度不一致， <math xmlns="http://www.w3.org/1998/Math/MathML"> J \mathbf J </math>J 不是方阵
• 机械臂接近奇异位形， <math xmlns="http://www.w3.org/1998/Math/MathML"> J \mathbf J </math>J 虽然看起来能逆，但数值会很不稳定
• 有些目标本来就不可能被当前结构精确到达，只能求一个最接近的解

所以工程里更常见的是用 pseudoinverse 来做 least-squares 意义下的求解：
<math xmlns="http://www.w3.org/1998/Math/MathML" display="block"> Δ q = J + e \Delta \mathbf q = \mathbf J^+ \mathbf e </math>Δq=J+e

其中 <math xmlns="http://www.w3.org/1998/Math/MathML"> J + \mathbf J^+ </math>J+ 是 <math xmlns="http://www.w3.org/1998/Math/MathML"> J \mathbf J </math>J 的 Moore-Penrose pseudoinverse。可以把它理解成：在所有可能的 joint increment 里，找一个尽量减小末端误差、同时又不过分夸张的解。工程上通常不会直接停在普通 pseudoinverse 这一步，还会继续引入更稳的形式，但这个项目先走到这里。

整个求解过程可以理解成一个迭代循环：
<math xmlns="http://www.w3.org/1998/Math/MathML" display="block"> q k + 1 = q k + α   Δ q k \mathbf q_{k+1} = \mathbf q_k + \alpha \,\Delta \mathbf q_k </math>qk+1=qk+αΔqk

阶段实现

Finite Difference 验证

"每次沿一个很小的方向更新，然后重新计算误差"，顺着这个思路，我先让 Codex 搭了一个只处理位置误差的 demo。它每轮扰动角度 <math xmlns="http://www.w3.org/1998/Math/MathML"> ε \mathbf \varepsilon </math>ε，反复迭代求解。这个 demo 确实会慢慢向目标靠近，但速度很慢，收敛也不稳定。拆开来看，它做的是下面这件事：

首先，假设第 <math xmlns="http://www.w3.org/1998/Math/MathML"> i i </math>i 列 Jacobian 通过 forward finite difference 来近似，那么它通常写成
<math xmlns="http://www.w3.org/1998/Math/MathML" display="block"> J i ≈ p ( q + ε e i ) − p ( q ) ε \mathbf J_i \approx \frac{\mathbf p(\mathbf q + \varepsilon \mathbf e_i) - \mathbf p(\mathbf q)}{\varepsilon} </math>Ji≈εp(q+εei)−p(q)

其中 <math xmlns="http://www.w3.org/1998/Math/MathML"> e i \mathbf e_i </math>ei 是第 <math xmlns="http://www.w3.org/1998/Math/MathML"> i i </math>i 个关节对应的 basis vector。这个式子看起来只是一列一个小公式，但真正落到代码里，含义其实是：

• 先拿当前关节姿态 <math xmlns="http://www.w3.org/1998/Math/MathML"> q \mathbf q </math>q
• 只扰动第 <math xmlns="http://www.w3.org/1998/Math/MathML"> i i </math>i 个关节一个很小的量 <math xmlns="http://www.w3.org/1998/Math/MathML"> ε \varepsilon </math>ε
• 重新做一次 forward kinematics，得到新的末端位置
• 和原来的末端位置做差
• 最后除以 <math xmlns="http://www.w3.org/1998/Math/MathML"> ε \varepsilon </math>ε

问题在于，Jacobian 不是只有一列，而是每个关节都要来一次。如果机械臂有 <math xmlns="http://www.w3.org/1998/Math/MathML"> n n </math>n 个关节，那么一轮迭代里大致要做：

• 1 次 forward kinematics，用来求当前误差
• <math xmlns="http://www.w3.org/1998/Math/MathML"> n n </math>n 次扰动后的 forward kinematics，用来拼出整个 Jacobian

也就是说，一轮迭代的成本大约就是
<math xmlns="http://www.w3.org/1998/Math/MathML" display="block"> 1 + n 1 + n </math>1+n

次 FK 级别的工作。

写成伪代码：

function solveStep(q, targetPosition, epsilon):
    # 1. 当前姿态先做一次 FK，拿到当前位置和误差
    p0 = forwardKinematics(q)
    error = targetPosition - p0

    # 2. 用数值差分构造 Jacobian
    J = zeroMatrix(3, n)

    for i in 0 .. n-1:
        qPerturbed = copy(q)
        qPerturbed[i] += epsilon

        pi = forwardKinematics(qPerturbed)

        Ji = (pi - p0) / epsilon
        J.setColumn(i, Ji)

    # 3. 解一个关节增量
    dq = pseudoInverse(J) * error

    # 4. 更新关节
    qNext = q + alpha * dq
    return qNext

如果放到 three.js 这类基于节点树的实现里，这个"做一次 FK"往往不只是算几个三角函数，而是要把整条链条上的 local transform 重新传播到 world transform。换句话说，每求一列数值 Jacobian，基本都要重新触发一遍从关节到末端的 world matrix 更新。这也是为什么它在 demo 阶段还能跑，但一旦关节数增多、每帧迭代次数提高，性能就会很快掉下去。

如果这时再叠加一些工程上常见的策略，比如：

• 每轮失败后 retry
• 为了避免发散做 backtracking
• 对多个 candidate step 分别试算误差

那么每试一次候选步长，通常都还要重新评估一次误差，开销会继续往上叠。所以"差分法验证"很适合拿来确认思路，但并不适合作为后续稳定迭代的核心方案。

另外，finite difference 还有一个精度和稳定性上的两难。 <math xmlns="http://www.w3.org/1998/Math/MathML"> ε \varepsilon </math>ε 取大了，导数近似会变粗； <math xmlns="http://www.w3.org/1998/Math/MathML"> ε \varepsilon </math>ε 取小了，又容易受到浮点误差和姿态表示方式的影响，尤其是在旋转问题里更明显。它既慢，又不够稳，这也是我后来必须继续往 analytic Jacobian 方向走的原因。

Analytic Jacobian 梯度下降

对第 <math xmlns="http://www.w3.org/1998/Math/MathML"> i i </math>i 个转动关节，记：

• joint position 为 <math xmlns="http://www.w3.org/1998/Math/MathML"> p i \mathbf p_i </math>pi
• joint axis direction 为 <math xmlns="http://www.w3.org/1998/Math/MathML"> a i \mathbf a_i </math>ai
• 末端位置为 <math xmlns="http://www.w3.org/1998/Math/MathML"> p \mathbf p </math>p

则 Jacobian 的 linear velocity 部分为：
<math xmlns="http://www.w3.org/1998/Math/MathML" display="block"> J v , i = a i × ( p − p i ) \mathbf J_{v,i} = \mathbf a_i \times (\mathbf p - \mathbf p_i) </math>Jv,i=ai×(p−pi)

angular velocity 部分为：
<math xmlns="http://www.w3.org/1998/Math/MathML" display="block"> J ω , i = a i \mathbf J_{\omega,i} = \mathbf a_i </math>Jω,i=ai

所以第 <math xmlns="http://www.w3.org/1998/Math/MathML"> i i </math>i 列 Jacobian 写成：
<math xmlns="http://www.w3.org/1998/Math/MathML" display="block"> J i = [ J v , i J ω , i ] = [ a i × ( p − p i ) a i ] \mathbf J_i = \begin{bmatrix} \mathbf J_{v,i} \\ \mathbf J_{\omega,i} \end{bmatrix} = \begin{bmatrix} \mathbf a_i \times (\mathbf p - \mathbf p_i) \\ \mathbf a_i \end{bmatrix} </math>Ji=[Jv,iJω,i]=[ai×(p−pi)ai]

写成伪代码，会更容易看出为什么它比 finite difference 便宜：

function solveStepAnalytic(q, targetPosition):
    # 1. 先做一次 FK，拿到整条链的世界坐标信息
    updateWorldTransforms(q)

    pEnd = getEndEffectorPosition()
    error = targetPosition - pEnd

    # 2. 直接用关节轴和末端位置构造 Jacobian
    J = zeroMatrix(3, n)

    for i in 0 .. n-1:
        pJoint = getJointWorldPosition(i)
        axis = getJointWorldAxis(i)

        Ji = cross(axis, pEnd - pJoint)
        J.setColumn(i, Ji)

    # 3. 解一个关节增量
    dq = pseudoInverse(J) * error

    # 4. 更新关节
    qNext = q + alpha * dq
    return qNext

这里的 qNext 表示这一轮更新之后的新 joint state，也就是把当前 joint variables <math xmlns="http://www.w3.org/1998/Math/MathML"> q \mathbf q </math>q 沿着本轮算出来的 increment <math xmlns="http://www.w3.org/1998/Math/MathML"> Δ q \Delta \mathbf q </math>Δq 推进一步：
<math xmlns="http://www.w3.org/1998/Math/MathML" display="block"> q k + 1 = q k + α Δ q k \mathbf q_{k+1} = \mathbf q_k + \alpha \Delta \mathbf q_k </math>qk+1=qk+αΔqk

finite difference 和 analytic Jacobian 最大的区别，不在最后那一步更新公式，而在 Jacobian 的构造方式：

• finite difference 是"每扰动一个关节，就重新做一次 FK，再拿结果做差"
• analytic Jacobian 是"先做一次 FK，拿到所有关节的 world position 和 axis direction，再直接算出每一列"

所以每轮迭代的开销大致分别是：

数值差分：
<math xmlns="http://www.w3.org/1998/Math/MathML" display="block"> 1 + n 次 FK 1 + n \text{ 次 FK} </math>1+n 次 FK

analytic Jacobian：
<math xmlns="http://www.w3.org/1998/Math/MathML" display="block"> 1 次 FK + O ( n ) 个向量运算 1 \text{ 次 FK} + O(n) \text{ 个向量运算} </math>1 次 FK+O(n) 个向量运算

前者把大部分时间花在重复刷新整条链的 world transform，后者则把这部分工作压缩到一次，然后只做叉乘、减法和矩阵拼装。对于 three.js 这种场景，这个差距会很直接地反映在每帧迭代次数和交互流畅度上。

到这里，这个简单的算法已经能比较流畅地工作了。点击实验 wwjll.github.io/three-chamb...

加入 Physics Engine

后来发现官方在用 rapier 这个相对轻量的 physics engine，于是我也把它引了进来，设计了一个带滑轨的夹爪，并把各个部分做了一定程度的解耦。动画过程也被拆成几个阶段：夹爪下降、夹取、抬升、移动到目标位、释放。

为了保证渲染性能，我把整个仓库里的例子都改成了 lazy rendering，也就是只有显式触发 render request 时才会真正渲染，性能提升很明显。

但 physics engine 和 grasp flow 一旦加进来，复杂度也立刻上去了，state control 多了很多。为了让过程更可控，又补了不少设定：

• 预设了 end effector 垂直向下的 zero pose
• 抓取过程的 physics contact state 解除
• 增加了 joint limits，限位外的无法到达
• 增加了 axis sign 来方便控制不同关节旋转正负号
• position、quaternion pose 的插值
• 可视化调节 controls

这部分可以在 wwjll.github.io/three-chamb... 里实验，需要先点击 "spawn cubes" 生成拾取方块。

最后的感受是，复杂度上来之后，真正关键的不是多写几轮代码，而是先把整体结构想清楚。遇到未知步骤时要停下来，重新拆解和设计，而不是顺着惯性往下堆实现。和 AI 协作也是一样，前提仍然是自己对问题边界、模块关系和验证方式有基本判断。