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 为 $q\; \in \; R\; n\; \backslash mathbf\; q\; \backslash in\; \backslash mathbb\; R^n$q∈Rn，end effector 当前 pose 记为 $x\; (\; q\; )\; \backslash mathbf\; x(\backslash mathbf\; q)$x(q)，目标 pose 记为 $x\; d\; \backslash mathbf\; x\_d$xd。要做的并不是一次性"解出答案"，而是每一帧根据当前位置和目标位置之间的误差，算出一小步 joint increment $\Delta \; q\; \backslash Delta\; \backslash mathbf\; q$Δq，让机械臂逐步逼近目标。

如果只看微分关系，这件事可以写成：
$$\Delta \; x\; \approx \; J\; (\; q\; )\; \hspace{0.17em}\; \Delta \; q\; \backslash Delta\; \backslash mathbf\; x\; \backslash approx\; \backslash mathbf\; J(\backslash mathbf\; q)\backslash ,\backslash Delta\; \backslash mathbf\; q$$Δx≈J(q)Δq

其中 $J\; (\; q\; )\; \backslash mathbf\; J(\backslash mathbf\; q)$J(q) 是 Jacobian matrix，它描述了"每个关节动一点点，end effector 的 pose 会怎样变化"。于是 inverse kinematics 的核心就变成了：已知末端误差 $e\; \backslash mathbf\; e$e，求一个合适的 $\Delta \; q\; \backslash Delta\; \backslash mathbf\; q$Δq，使得
$$J\; (\; q\; )\; \hspace{0.17em}\; \Delta \; q\; \approx \; e\; \backslash mathbf\; J(\backslash mathbf\; q)\backslash ,\backslash Delta\; \backslash mathbf\; q\; \backslash approx\; \backslash mathbf\; e$$J(q)Δq≈e

这里的 error vector 通常拆成 position error 和 orientation error 两部分：
$$e\; =$$ e p e r ∈ R 6 \mathbf e = \begin{bmatrix} \mathbf e_p \\ \mathbf e_r \end{bmatrix} \in \mathbb R^6 e=eper∈R6

其中 position error 可以直接写成
$$e\; p\; =\; p\; d\; -\; p\; \backslash mathbf\; e\_p\; =\; \backslash mathbf\; p\_d\; -\; \backslash mathbf\; p$$ep=pd−p

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

如果 $J\; \backslash mathbf\; J$J 恰好是方阵且可逆，形式上可以写成
$$\Delta \; q\; =\; J\; -\; 1\; e\; \backslash Delta\; \backslash mathbf\; q\; =\; \backslash mathbf\; J^\{-1\}\backslash mathbf\; e$$Δq=J−1e

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

• 关节数和任务维度不一致， $J\; \backslash mathbf\; J$J 不是方阵
• 机械臂接近奇异位形， $J\; \backslash mathbf\; J$J 虽然看起来能逆，但数值会很不稳定
• 有些目标本来就不可能被当前结构精确到达，只能求一个最接近的解

所以工程里更常见的是用 pseudoinverse 来做 least-squares 意义下的求解：
$$\Delta \; q\; =\; J\; +\; e\; \backslash Delta\; \backslash mathbf\; q\; =\; \backslash mathbf\; J^+\; \backslash mathbf\; e$$Δq=J+e

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

整个求解过程可以理解成一个迭代循环：
$$q\; k\; +\; 1\; =\; q\; k\; +\; \alpha \; \hspace{0.17em}\; \Delta \; q\; k\; \backslash mathbf\; q\_\{k+1\}\; =\; \backslash mathbf\; q\_k\; +\; \backslash alpha\; \backslash ,\backslash Delta\; \backslash mathbf\; q\_k$$qk+1=qk+αΔqk

阶段实现

Finite Difference 验证

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

首先，假设第 $i\; i$i 列 Jacobian 通过 forward finite difference 来近似，那么它通常写成
$$J\; i\; \approx \; p\; (\; q\; +\; \epsilon \; e\; i\; )\; -\; p\; (\; q\; )\; \epsilon \; \backslash mathbf\; J\_i\; \backslash approx\; \backslash frac\{\backslash mathbf\; p(\backslash mathbf\; q\; +\; \backslash varepsilon\; \backslash mathbf\; e\_i)\; -\; \backslash mathbf\; p(\backslash mathbf\; q)\}\{\backslash varepsilon\}$$Ji≈εp(q+εei)−p(q)

其中 $e\; i\; \backslash mathbf\; e\_i$ei 是第 $i\; i$i 个关节对应的 basis vector。这个式子看起来只是一列一个小公式，但真正落到代码里，含义其实是：

• 先拿当前关节姿态 $q\; \backslash mathbf\; q$q
• 只扰动第 $i\; i$i 个关节一个很小的量 $\epsilon \; \backslash varepsilon$ε
• 重新做一次 forward kinematics，得到新的末端位置
• 和原来的末端位置做差
• 最后除以 $\epsilon \; \backslash varepsilon$ε

问题在于，Jacobian 不是只有一列，而是每个关节都要来一次。如果机械臂有 $n\; n$n 个关节，那么一轮迭代里大致要做：

• 1 次 forward kinematics，用来求当前误差
• $n\; n$n 次扰动后的 forward kinematics，用来拼出整个 Jacobian

也就是说，一轮迭代的成本大约就是
$$1\; +\; n\; 1\; +\; n$$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 还有一个精度和稳定性上的两难。 $\epsilon \; \backslash varepsilon$ε 取大了，导数近似会变粗； $\epsilon \; \backslash varepsilon$ε 取小了，又容易受到浮点误差和姿态表示方式的影响，尤其是在旋转问题里更明显。它既慢，又不够稳，这也是我后来必须继续往 analytic Jacobian 方向走的原因。

Analytic Jacobian 梯度下降

对第 $i\; i$i 个转动关节，记：

• joint position 为 $p\; i\; \backslash mathbf\; p\_i$pi
• joint axis direction 为 $a\; i\; \backslash mathbf\; a\_i$ai
• 末端位置为 $p\; \backslash mathbf\; p$p

则 Jacobian 的 linear velocity 部分为：
$$J\; v\; ,\; i\; =\; a\; i\; \times \; (\; p\; -\; p\; i\; )\; \backslash mathbf\; J\_\{v,i\}\; =\; \backslash mathbf\; a\_i\; \backslash times\; (\backslash mathbf\; p\; -\; \backslash mathbf\; p\_i)$$Jv,i=ai×(p−pi)

angular velocity 部分为：
$$J\; \omega \; ,\; i\; =\; a\; i\; \backslash mathbf\; J\_\{\backslash omega,i\}\; =\; \backslash mathbf\; a\_i$$Jω,i=ai

所以第 $i\; i$i 列 Jacobian 写成：
$$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} 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 $q\; \backslash mathbf\; q$q 沿着本轮算出来的 increment $\Delta \; q\; \backslash Delta\; \backslash mathbf\; q$Δq 推进一步：
$$q\; k\; +\; 1\; =\; q\; k\; +\; \alpha \; \Delta \; q\; k\; \backslash mathbf\; q\_\{k+1\}\; =\; \backslash mathbf\; q\_k\; +\; \backslash alpha\; \backslash Delta\; \backslash mathbf\; q\_k$$qk+1=qk+αΔqk

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

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

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

数值差分：
$$1\; +\; n\; 次\; FK\; 1\; +\; n\; \backslash text\{\; 次\; FK\}$$1+n 次 FK

analytic Jacobian：
$$1\; 次\; FK\; +\; O\; (\; n\; )\; 个向量运算\; 1\; \backslash text\{\; 次\; FK\}\; +\; O(n)\; \backslash text\{\; 个向量运算\}$$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 协作也是一样，前提仍然是自己对问题边界、模块关系和验证方式有基本判断。