目录
前文已经在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;
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;
private float sita_L1, sita_L2;//L1、L2初始位置的偏移角
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;
Debug.Log(String.Format("{0:N6}",sita_L1));//保留六位小数
Debug.Log(String.Format("{0:N6}", sita_L2));
}
void Update()
{
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;
Vector3 Euler3 = transform.localEulerAngles;
Euler3.y = sita_3-90;//实际是从z轴正方向开始的旋转
Base.transform.localEulerAngles = Euler3;
//Sita_3旋转完成后，开始计算从Arm0为坐标原点的sita_1和sita_2
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 = 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_1 *= Mathf.Rad2Deg;
sita_2 *= Mathf.Rad2Deg;
Vector3 euler1 = Arm0.transform.localEulerAngles;
euler1.z = -(90-sita_1+sita_L1);
Arm0.localEulerAngles = euler1;
Vector3 euler2 = Arm1.transform.localEulerAngles;
euler2.z = -(sita_2- sita_L2);
Arm1.localEulerAngles = euler2;
Debug.Log("hand的位置:"+Hand.position);
}
static float square(float f)
{
return f * f;
}
或者在旋转时加入球形插值，让它转得丝滑一些。
