UnityVR--机械臂场景7-三自由度逆向解算2

上一篇逆解了三轴机械臂的三个重要偏转角度θT、θ1和θ2，现在我在场景中将它实现出来。

1. 在场景中建立三个空节点作为旋转轴，可以使用不同颜色的Icon标记一下。为了计算简单，将三个旋转轴之间的距离设置为1，即L1=L2=1，三者关系如下图，并在最大的手臂之上建立一个空节点Base，位置与Arm1相同，脚本就挂在Base上

2. 为了计算方便，先将机械臂的姿态摆放至与前文分析中的初始姿态一致。旋转轴分别为Arm0，Arm1。另外在场景中建立一个目标物体Target，可以放置在距离Base点小于（L1+L2）的范围内。（图就不放了）

3. 按照前一篇的解算过程，写一个逆解函数，得到末端坐标（Target），解算出3个偏转角度。这个还比较容易，套公式就行：

    float[] I_K(float[] cartesion,float[] f_scara)
    {
        float sita_1, sita_2, sita_3, sita_T, X, Y, Z, W, A;
        X = cartesion[x];
        Y = cartesion[y];
        Z = cartesion[z];
        W = Mathf.Sqrt(X * X + Z * Z);
        A = Mathf.Sqrt(W * W + Y * Y);
        sita_T = Mathf.Acos(W / A);
        sita_1=Mathf.Acos((L1*L1+W*W+Y*Y-L2*L2)/(2*L1*A))+sita_T;
        sita_2 = Mathf.Acos((W * W + Y * Y - L1 * L1 - L2 * L2) / (2 * L1 * L2));
        sita_3 = Mathf.Acos(X/ W);
        sita_1 *= Mathf.Rad2Deg;//弧度转角度
        sita_2 *= Mathf.Rad2Deg;
        sita_3 *= Mathf.Rad2Deg;
        f_scara[0] = sita_1;
        f_scara[1] = sita_2;
        f_scara[2] = sita_3;
        return f_scara;
    }

4. 然后在Update中实现三个角度的偏转：

        Vector3 Euler0 = Arm0.transform.localEulerAngles;
        Euler0.z = f_scara[0];
        Arm0.transform.localEulerAngles = Euler0;

        Vector3 Euler1 = Arm1.transform.localEulerAngles;
        Euler1.z =f_scara[1];
        Arm1.transform.localEulerAngles = Euler1;

        Vector3 Euler2 =transform.localEulerAngles;//这个是Base的偏转
        Euler2.y = f_scara[2];
        base. transform.localEulerAngles = Euler2;

5.测试：

将脚本挂在Base节点上，并拖入相应的节点，运行程序。

一开始还好，手爪Hand（黄球）还能够随着目标（Target）移动，后来就尴尬了，在Z轴正方向和Y轴负方向的区域，手爪反而向着目标的负方向移动了。

扫描二维码关注公众号，回复： 15197591 查看本文章

检查一下，当Target位置为（1，1，1）时，输出sita_1，sita_2，sita_3的实际角度发现，sita_1，sita_2与计算值相符，而sita_3应该是绕Y轴旋转-45°（左手坐标）。

要解决这个问题，我的设想是：以机械臂基座（也就是Base）为坐标原点的零时坐标系中，在1、2象限中需要将sita_3的偏转角度反向，而3、4象限不需要。

因此，sita_3的偏移角度的代码修改：

        Vector3 euler2 = Base.transform.localEulerAngles;
        euler2.y = F_scara[2]*Mathf.Sign(Base.position.z-Target.position.z);
        //Target和Base的Z向差，取它的正负号
        Base.localEulerAngles = euler2;

运行没有问题，如果不动Target，拖动Base节点也是一样的。

5. 全文如下：

using System.Collections;
using System.Collections.Generic;
using UnityEngine;

public class Robot_Cartesion : MonoBehaviour
{
    public Transform Base,Arm0,Arm1,Hand,Target;   //拖入机械臂的4个主要节点
    public Transform Target; //拖入目标物体

    private float L1,L2; 
    private float[] cartesion = new float[3], f_scara = new float[3];
    int x = 0，y = 1，z = 2;

    void Start()
    {
        L1 = Vector3.Distance(Arm0.position, Arm1.position);
        L2 = Vector3.Distance(Arm1.position, Hand.position);
    }

    void Update()
    {
        //基座到目标的直线距离
        float length2 = Vector3.Distance(transform.position, Target.position);
        if(length2>L1+L2||length2<L1-L2)
            Debug.Log("超过抓取范围");
        else
        {
            cartesion[x] = Target.position.x - Base.position.x;
            cartesion[y] = Target.position.y - Base.position.y;
            cartesion[z] = Target.position.z - Base.position.z;
            F_scara=I_K(cartesion, F_scara);
        }

        Vector3 euler0 = Arm0.transform.localEulerAngles;
        euler0.z = F_scara[0];
        Arm0.localEulerAngles = euler0;
        Vector3 euler1 = Arm1.transform.localEulerAngles;
        euler1.z = -F_scara[1];
        Arm1.localEulerAngles = euler1;
        Vector3 euler2 = Base.transform.localEulerAngles;
        euler2.y = F_scara[2]*Mathf.Sign(Base.position.z-Target.position.z);
       //Target和Base的Z向差，取它的正负号
        Base.localEulerAngles = euler2;
    }

    float[] I_K(float[] cartesion,float[] f_scara)
    {
        float sita_1, sita_2, sita_3, sita_T, X, Y, Z, W, A;
        X = cartesion[x];
        Y = cartesion[y];
        Z = cartesion[z];
        W = Mathf.Sqrt(X * X + Z * Z);
        A = Mathf.Sqrt(W * W + Y * Y);
        sita_T = Mathf.Acos(W / A);
        sita_1=Mathf.Acos((L1*L1+W*W+Y*Y-L2*L2)/(2*L1*A))+sita_T;
        sita_2 = Mathf.Acos((W * W + Y * Y - L1 * L1 - L2 * L2) / (2 * L1 * L2));
        sita_3 = Mathf.Acos(X/ W);
        sita_1 *= Mathf.Rad2Deg;
        sita_2 *= Mathf.Rad2Deg;
        sita_3 *= Mathf.Rad2Deg;
        f_scara[0] = sita_1;
        f_scara[1] = sita_2;
        f_scara[2] = sita_3;
        return f_scara;
    }
}

本文中的方法，参照了grbl控制3轴机械臂原理实现（二）之3D机械臂模拟及实现_ourkix的博客

加入自己的瞎琢磨，比较粗陋。且由于数学模型与实际模型的坐标差异，解决办法也许不是最优解，希望以后有空再改进。

UnityVR--机械臂场景7-三自由度逆向解算2

猜你喜欢