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

如果只看微分关系，这件事可以写成：

\Delta \mathbf x \approx \mathbf J(\mathbf q)\,\Delta \mathbf q

其中 $\mathbf J(\mathbf q)$ 是 Jacobian matrix，它描述了“每个关节动一点点，end effector 的 pose 会怎样变化”。于是 inverse kinematics 的核心就变成了：已知末端误差 $\mathbf e$ ，求一个合适的 $\Delta \mathbf q$ ，使得

\mathbf J(\mathbf q)\,\Delta \mathbf q \approx \mathbf e

这里的 error vector 通常拆成 position error 和 orientation error 两部分：

\mathbf e = \begin{bmatrix} \mathbf e_p \\ \mathbf e_r \end{bmatrix} \in \mathbb R^6

其中 position error 可以直接写成

\mathbf e_p = \mathbf p_d - \mathbf p

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

如果 $\mathbf J$ 恰好是方阵且可逆，形式上可以写成

\Delta \mathbf q = \mathbf J^{-1}\mathbf e

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

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

所以工程里更常见的是用 pseudoinverse 来做 least-squares 意义下的求解：

\Delta \mathbf q = \mathbf J^+ \mathbf e

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

整个求解过程可以理解成一个迭代循环：

\mathbf q_{k+1} = \mathbf q_k + \alpha \,\Delta \mathbf q_k

阶段实现

Finite Difference 验证

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

首先，假设第 $i$ 列 Jacobian 通过 forward finite difference 来近似，那么它通常写成

\mathbf J_i \approx \frac{\mathbf p(\mathbf q + \varepsilon \mathbf e_i) - \mathbf p(\mathbf q)}{\varepsilon}

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

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

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

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

也就是说，一轮迭代的成本大约就是

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

Analytic Jacobian 梯度下降

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

joint position 为 $\mathbf p_i$
joint axis direction 为 $\mathbf a_i$
末端位置为 $\mathbf p$

则 Jacobian 的 linear velocity 部分为：

\mathbf J_{v,i} = \mathbf a_i \times (\mathbf p - \mathbf p_i)

angular velocity 部分为：

\mathbf J_{\omega,i} = \mathbf a_i

所以第 $i$ 列 Jacobian 写成：

\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}

写成伪代码，会更容易看出为什么它比 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 $\mathbf q$ 沿着本轮算出来的 increment $\Delta \mathbf q$ 推进一步：

\mathbf q_{k+1} = \mathbf q_k + \alpha \Delta \mathbf q_k

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

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

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

数值差分：

1 + n \text{ 次 FK}

analytic Jacobian：

1 \text{ 次 FK} + O(n) \text{ 个向量运算}

前者把大部分时间花在重复刷新整条链的 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 协作也是一样，前提仍然是自己对问题边界、模块关系和验证方式有基本判断。