Unity中如何实现带关节角度约束的FABRIK逆运动学算法？

你已经完成的无约束FABRIK是基础版本，角度约束不需要改动核心的前向/反向迭代逻辑，只需要在每次更新节点位置后，对超出角度阈值的关节做位置校正即可，校正时保持臂段原有长度不变。

前置准备 #

• 为每个关节配置独立的角度阈值参数，每个关节存储minAngle、maxAngle两个值，统一使用弧度或角度单位，和你调用的向量夹角计算接口单位匹配即可。
• 明确每个关节的合法旋转轴：2D机械臂的旋转轴固定为垂直于运动平面的轴；3D机械臂需要为每个关节单独配置允许旋转的轴，避免出现非法扭转。

约束校正逻辑 #

约束校正需要分别嵌入到反向、前向迭代的过程中，每更新完一个节点位置就立刻做相邻关节的角度校验，不要等整轮迭代跑完再统一校正，否则会大幅降低收敛速度，甚至出现位置抖动。
校正逻辑以**前向迭代（从根节点往末端走）**为例：

1. 前向迭代时，根节点侧的节点位置已经固定，当你算出第i+1个节点的初始位置后，取当前关节i前后两个臂段的方向
2. 计算两个臂段的实际夹角，如果夹角落在当前关节配置的[minAngle, maxAngle]区间内，无需校正，继续迭代下一个节点
3. 如果夹角超出阈值，将当前臂段的方向绕关节旋转轴旋转到合法的角度边界，用校正后的方向重新计算第i+1个节点的位置，保证臂长和预设值完全一致

反向迭代（从末端往根节点走）的校正逻辑完全对称，只是固定端换成了末端侧的节点，校正根节点侧的节点位置即可。

带约束的实现代码参考 #

// 提前为每个关节配置好角度限制、旋转轴、臂长参数
for (int j = 0; j < iterationCount; j++) {
    // 反向传播：从末端到根
    nodePositions[nodeCount - 1] = targetPos;
    for (int i = nodeCount - 2; i >= 0; i--) {
        Vector3 dir = (nodePositions[i] - nodePositions[i + 1]).normalized;
        nodePositions[i] = nodePositions[i + 1] + dir * arms[i].Length;

        // 反向传播时的关节约束：i不是根节点时才需要校验
        if (i > 0) {
            // 固定i+1和i的位置，校正i-1的位置
            Vector3 nextArmDir = (nodePositions[i] - nodePositions[i + 1]).normalized;
            Vector3 currArmDir = (nodePositions[i - 1] - nodePositions[i]).normalized;
            float currentAngle = Vector3.Angle(nextArmDir, currArmDir);
            
            Joint currentJoint = arms[i-1].ConnectedJoint;
            if (currentAngle > currentJoint.maxAngle || currentAngle < currentJoint.minAngle) {
                // 把当前臂方向旋转到合法角度边界
                float clampAngle = Mathf.Clamp(currentAngle, currentJoint.minAngle, currentJoint.maxAngle);
                Quaternion rot = Quaternion.FromToRotation(currArmDir, nextArmDir) * Quaternion.AngleAxis(-clampAngle, currentJoint.rotateAxis);
                Vector3 legalDir = rot * currArmDir;
                nodePositions[i - 1] = nodePositions[i] + legalDir * arms[i-1].Length;
            }
        }
    }

    // 前向传播：从根到末端
    nodePositions[0] = rootNodePos;
    for (int i = 1; i < nodeCount; i++) {
        Vector3 dir = (nodePositions[i] - nodePositions[i - 1]).normalized;
        nodePositions[i] = nodePositions[i - 1] + dir * arms[i - 1].Length;

        // 前向传播时的关节约束：i不是末端节点时才需要校验
        if (i < nodeCount - 1) {
            // 固定i-1和i的位置，校正i+1的位置
            Vector3 prevArmDir = (nodePositions[i] - nodePositions[i - 1]).normalized;
            Vector3 currArmDir = (nodePositions[i + 1] - nodePositions[i]).normalized;
            float currentAngle = Vector3.Angle(prevArmDir, currArmDir);

            Joint currentJoint = arms[i-1].ConnectedJoint;
            if (currentAngle > currentJoint.maxAngle || currentAngle < currentJoint.minAngle) {
                // 把当前臂方向旋转到合法角度边界
                float clampAngle = Mathf.Clamp(currentAngle, currentJoint.minAngle, currentJoint.maxAngle);
                Quaternion rot = Quaternion.FromToRotation(currArmDir, prevArmDir) * Quaternion.AngleAxis(clampAngle, currentJoint.rotateAxis);
                Vector3 legalDir = rot * currArmDir;
                nodePositions[i + 1] = nodePositions[i] + legalDir * arms[i].Length;
            }
        }
    }
}

调优注意点 #

• 带约束的FABRIK收敛速度比无约束版本稍慢，可以把迭代次数设置为无约束版本的1.5~2倍，保证最终位置误差足够小。
• 如果是3D机械臂，除了臂段之间的夹角限制，还可以用同样的逻辑添加绕臂段轴线的扭转角限制，只需要额外计算扭转角后做方向校正即可。
• 角度阈值不要设置成互斥区间（比如minAngle大于maxAngle），否则会出现算法无法收敛、关节持续抖动的问题。
• 2D场景下使用时，把夹角计算替换为带符号的角度计算，区分关节左右旋转的方向即可。

内容的提问来源于stack exchange，提问作者Munkhuu G