Unity中手动实现逆运动学（IK）的问题排查

我在Unity中用C#实现3自由度机械臂的逆运动学（IK），运行时发现移动目标点（紫色点）关节虽会移动，但始终无法正确抵达目标位置。公式逻辑应该没问题，推测问题出在弧度与角度转换、四元数旋转处理或坐标系匹配上。

参考的IK计算公式示意图
实际运行的IK结果示意图

原代码 #

public class IK : MonoBehaviour
{
    public double A1, A2, A3;
    
    public Transform J1, J2, J3;
    public Transform target;

    void Update()
    {
        InverseKinematic();
        //DoInverseKinematic();
    }

    void InverseKinematic()
    {
        double x = target.position.x;
        double y = target.position.y;
        double z = target.position.z;

        double T1 = Math.Atan(y / x);

        double R1 = Math.Sqrt(Math.Pow(x, 2) + Math.Pow(y, 2));
        double R2 = z - A1;

        double P2 = Math.Atan(R2 / R1) * Mathf.Rad2Deg;

        double R3 = Math.Sqrt(Math.Pow(R1, 2) + Math.Pow(R2, 2));

        double P1 = Math.Acos((Math.Pow(A3, 2) - Math.Pow(A2, 2) - Math.Pow(R3, 2)) / (-2*A2*R3));

        double T2 = P2 - P1;

        double P3 = Math.Acos((Math.Pow(R3, 2) - Math.Pow(A2, 2) - Math.Pow(A3, 2)) / (-2 * A2 * A3));

        double T3 = 180 - P3;
        
        Debug.Log(T1 + " - " + T2 + " - " + T3);

        J1.rotation = Quaternion.AngleAxis((float)T1 * Mathf.Rad2Deg, Vector3.up);
        J2.rotation = Quaternion.AngleAxis((float)T2 * Mathf.Rad2Deg, Vector3.forward);
        J3.rotation = Quaternion.AngleAxis((float)T3 * Mathf.Rad2Deg, Vector3.forward);

    }
}

问题分析与修正方案 #

• 弧度/角度单位混乱：原代码中P2提前将弧度转成角度，但P1、P3仍为弧度，导致T2、T3计算时单位不统一，完全错误。修正为统一用弧度计算所有角度，最后再转成角度给Quaternion使用。
• Atan的象限问题：Math.Atan(y/x)无法处理x为负数的场景，会返回错误象限的角度，改用Math.Atan2(y, x)可正确计算所有象限的旋转角。
• 旋转空间错误：直接给rotation赋值是设置世界空间旋转，机械臂关节为父子层级结构，需用localRotation设置局部空间旋转，否则会脱离父关节的姿态约束。
• T3计算的单位错误：180 - P3中180是角度、P3是弧度，需改成Math.PI - P3（弧度制的180度）后再转角度。

修正后的代码 #

public class IK : MonoBehaviour
{
    public double A1, A2, A3;
    
    public Transform J1, J2, J3;
    public Transform target;

    void Update()
    {
        InverseKinematic();
    }

    void InverseKinematic()
    {
        double x = target.position.x;
        double y = target.position.y;
        double z = target.position.z;

        // 用Atan2处理所有象限，返回弧度
        double T1 = Math.Atan2(y, x);

        double R1 = Math.Sqrt(Math.Pow(x, 2) + Math.Pow(y, 2));
        double R2 = z - A1;

        // 保持弧度计算，不提前转角度
        double P2 = Math.Atan2(R2, R1);

        double R3 = Math.Sqrt(Math.Pow(R1, 2) + Math.Pow(R2, 2));

        // 余弦定理计算角度（弧度）
        double P1 = Math.Acos((Math.Pow(A3, 2) - Math.Pow(A2, 2) - Math.Pow(R3, 2)) / (-2 * A2 * R3));
        double T2 = P2 - P1;

        double P3 = Math.Acos((Math.Pow(R3, 2) - Math.Pow(A2, 2) - Math.Pow(A3, 2)) / (-2 * A2 * A3));
        // 弧度制的180度减去P3
        double T3 = Math.PI - P3;
        
        // 转成角度输出日志
        Debug.Log($"{T1 * Mathf.Rad2Deg:F2}° - {T2 * Mathf.Rad2Deg:F2}° - {T3 * Mathf.Rad2Deg:F2}°");

        // 设置局部旋转，确保基于父关节的姿态
        J1.localRotation = Quaternion.AngleAxis((float)(T1 * Mathf.Rad2Deg), Vector3.up);
        J2.localRotation = Quaternion.AngleAxis((float)(T2 * Mathf.Rad2Deg), Vector3.forward);
        J3.localRotation = Quaternion.AngleAxis((float)(T3 * Mathf.Rad2Deg), Vector3.forward);
    }
}

额外注意事项 #

• 确保机械臂层级结构正确：J1是基座子物体，J2是J1子物体，J3是J2子物体，这样局部旋转才能正确传递姿态。
• 检查A1、A2、A3数值是否和机械臂实际长度一致，单位需统一（如都用米）。
• 当目标点超出机械臂工作范围时（R3 > A2+A3 或 R3 < Math.Abs(A2-A3)），Math.Acos会返回NaN，需添加边界判断避免报错。

内容的提问来源于stack exchange，提问作者Olav Ausland