UnityVR--机械臂场景8-三自由度逆向解算3-应用在ABB机械臂_三自由度机械臂

目录

1. ABB机械臂形态

2. ABB机械臂数学模型分析

3. 初步程序实现

4. 误差分析

5. 最终实现

  前文已经在Unity中，将3自由度机械臂的数学模型实现出来了，现在我们将它实际应用于机械臂中。选用的机械臂3D模型是ABB IRB 4600工业机器人，来自AssetStore。

（图片来自ABB官网）

1. ABB机械臂形态

  模型中总共有六个旋转轴，为了继承上一章内容和分析方法，仅使用其中的3个自由度。

2. ABB机械臂数学模型分析

  但是与上一章的数学模型不同的是，这里的base和Arm0两个节点坐标不同，因此在上一章的基础上，需要分两步来考虑：

  第一步：以base为原点来计算θ3，这一步与上一章相同；

或者θ3=arctan(x/z)

  第二步：以Arm0为原点计算θ1和θ2。

  这里用二维图作示意，将坐标轴偏移一个距离，这时候Target的坐标变成了与Arm0节点的差值（x',y',z'）

按照上一章的计算方式，重新计算W‘、A'、θT、θ2、θ1：

3. 初步程序实现

  1. 准备工作：首先要拿到上述计算需要用到的变量：

    public Transform Base,Arm0,Arm1,Hand,Target;
    float X, Y, Z; //记录目标Target的位置，相对于Base
    float x_plus, y_plus, z_plus; //Target的位置，相对于Arm0
    private float L1,L2;
    private float sita_1, sita_2, sita_3, sita_T, W, A;

  2. 准备工作：计算出L1、L2的值，这是固定不变的

        L1 = Vector3.Distance(Arm0.position, Arm1.position);
        L2 = Vector3.Distance(Arm1.position, Hand.position);

  3. 先在以base为原点的坐标系中计算出θ3，套公式：

        X = Target.position.x - Base.transform.position.x;
        Y = Target.position.y - Base.transform.position.y;
        Z = Target.position.z - Base.transform.position.z;
        sita_3 = Mathf.Atan2(X, Z);
        sita_3 = Mathf.Rad2Deg * sita_3;

  4. 再以Arm0为原点，计算x',y',z',W‘、A'、θT、θ2、θ1，套公式：

        x_plus = Target.position.x - Arm0.position.x;
        y_plus = Target.position.y - Arm0.position.y;
        z_plus = Target.position.z - Arm0.position.z;
        W = Mathf.Sqrt(square(x_plus) + square(z_plus));
        A = Mathf.Sqrt(square(W) + square(y_plus));
        sita_T = Mathf.Acos(W / A); 
        sita_1 = Mathf.Acos((square(L1) + square(W) + square(y_plus) - square(L2)) / (2 * L1 * A)) + sita_T;
        sita_2 = Mathf.Acos((square(W) + square(y_plus) - square(L1) - square(L2)) / (2 * L1 * L2));
        sita_1 *= Mathf.Rad2Deg;
        sita_2 *= Mathf.Rad2Deg;

  好了，运行试试看：

   结果中可以看到，虽然机械臂随着Target移动，但是却有一定的误差：Target和手爪之间错位了一段距离。这是为什么呢？下面分析一下误差产生的原因。

4. 误差分析

  将这几个旋转节点单独拿出来观察，从XY平面来看，在机械臂的初始状态，Arm0到Arm1，以及Arm1到Hand，并不是完全地与Y轴平行的，XZ平面也是如此。这一点从Arm0和Arm1的本地坐标上也能看出来。但上述数学模型是简化了的，没有考虑到这些问题。

   因此，在一开始还需要计算出初始角度，并在之后实际旋转时减去。sita_L1和sita_L2分别是两条手臂L1和L2的初始偏移角度。

        sita_L1 =(Mathf.Asin(Arm1.localPosition.x / L1))*Mathf.Rad2Deg;
        var temp = Hand.position.x - Arm1.position.x;
        sita_L2=(Mathf.Asin(temp/ L2)) * Mathf.Rad2Deg;

5. 最终实现

 完整代码如下：

    public Transform Base,Arm0,Arm1,Hand,Target;
    private float L1,L2;
    private float sita_L1, sita_L2;//L1、L2初始位置的偏移角
    private float[] moveAngle;    
 
    void Start()
    {   //在ABB机械臂中，L1改成（S_Axis_2）到Arm1节点（U_Axis_3）的距离
        L1 = Vector3.Distance(Arm0.position, Arm1.position);
        L2 = Vector3.Distance(Arm1.position, Hand.position);
 
        sita_L1 =(Mathf.Asin(Arm1.localPosition.x / L1))*Mathf.Rad2Deg;
        var temp = Hand.position.x - Arm1.position.x;
        sita_L2=(Mathf.Asin(temp/ L2)) * Mathf.Rad2Deg;
    }
 
    void Update()
    {
        moveAngle=IKCaculator(Target,moveAngle);
        
        Vector3 euler0 = Base.transform.localEulerAngles;
        euler0.y = moveAngle[0];
        //插值旋转举例
        Base.localRotation = Quaternion.Slerp(Base.localRotation, Quaternion.Euler(Euler0), 0.9f * Time.deltaTime);
        if (Quaternion.Angle(Base.localRotation, Quaternion.Euler(Euler0)) < 0.5f)
            Base.transform.localEulerAngles = Euler0;
 
        Vector3 euler1 = Arm0.transform.localEulerAngles;
        euler1.z = moveAngle[1];
        Arm0.localEulerAngles = euler1;
 
        Vector3 euler2 = Arm1.transform.localEulerAngles;
        euler2.z =moveAngle[2];
        Arm1.localEulerAngles = euler2;
 
    }
 
    float[] IKCaculator(Transform target,float[] Scara)
    {
        float sita_1, sita_2, sita_3, sita_T, X, Y, Z, W, A,x_plus,y_plus,z_plus;
        //计算目标Target和第一个关节Base距离的三个分量X、Y、Z
        X = target.position.x - Base.transform.position.x;
        Y = target.position.y - Base.transform.position.y;
        Z = target.position.z - Base.transform.position.z;
        //计算目标Target和Arm0距离的三个分量x_plus、y_plus、z_plus
        x_plus = target.position.x - Arm0.position.x;
        y_plus = target.position.y - Arm0.position.y;
        z_plus = target.position.z - Arm0.position.z;
        //计算W和A，在图中为W'和A'
        W = Mathf.Sqrt(square(x_plus) + square(z_plus));
        A = Mathf.Sqrt(square(W) + square(y_plus));  //A的长度=根号（W平方+Y平方）
        sita_T = Mathf.Acos(W / A);  //辅助角T
        sita_1 = Mathf.Acos((square(L1) + square(W) + square(y_plus) - square(L2)) / (2 * L1 * A)) + sita_T;
        sita_2 = Mathf.Acos((square(W) + square(y_plus) - square(L1) - square(L2)) / (2 * L1 * L2));
        sita_3 = Mathf.Atan2(X, Z);
 
        sita_1 *= Mathf.Rad2Deg;
        sita_2 *= Mathf.Rad2Deg;
        sita_3 *= Mathf.Rad2Deg;
 
        Scara[0] = sita_3-90;    //ABB机械臂的误差排除
        Scara[1] =-(90- sita_1+sita_L1);
        Scara[2] = -(sita_2-sita_L2);
        return Scara;
    }
 
    static float square(float f)
    {
        return f * f;
    }

或者在旋转时加入球形插值，让它转得丝滑一些。