目录
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;
}
或者在旋转时加入球形插值，让它转得丝滑一些。文章来源：https://www.toymoban.com/news/detail-690303.html
文章来源地址https://www.toymoban.com/news/detail-690303.html
到了这里，关于UnityVR--机械臂场景8-三自由度逆向解算3-应用在ABB机械臂的文章就介绍完了。如果您还想了解更多内容，请在右上角搜索TOY模板网以前的文章或继续浏览下面的相关文章，希望大家以后多多支持TOY模板网！
