上一篇逆解了三轴机械臂的三个重要偏转角度θ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轴负方向的区域,手爪反而向着目标的负方向移动了。
检查一下,当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的博客
加入自己的瞎琢磨,比较粗陋。且由于数学模型与实际模型的坐标差异,解决办法也许不是最优解,希望以后有空再改进。