unity制作机械臂

硬盘坏了，项目没了/(ㄒoㄒ)/~~幸好记录了一些步骤

一、前置知识

1.1 变换矩阵

  位置是相对的，必须先确定参考系，才能描述物体的位置。我们将定义的第一个坐标系称为参考坐标系，其他的坐标系都是相对于这个参考坐标系来描述的。从参考坐标系转换到其他坐标系上的转换关系矩阵叫变换矩阵，变换矩阵是连接两个坐标系的桥梁。具体来说，变换矩阵T是由两部分组成：旋转矩阵R和平移矩阵t。旋转矩阵R负责处理坐标系之间的旋转关系，而平移矩阵t则负责处理坐标系之间的平移关系。通过变换矩阵，我们能够准确地将物体从一个坐标系转换到另一个坐标系。
  下面举出一个例子，定义蓝色的坐标系为参考坐标系，绿色的坐标系为目标坐标系，红色的点表示空间中的一个物体。首先确定旋转矩阵R，z轴绕负方向旋转了90度，其余轴都没有进行旋转，则$R=\left[\begin{array}{cccc}0& -1& 0& 0\\ 1& 0& 0& 0\\ 0& 0& 1& 0\\ 0& 0& 0& 1\end{array}\right]$，再确定平移矩阵t，x轴正向平移1单位，y轴正向平移6单位，z轴正向平移2单位，$t=\left[\begin{array}{cccc}1& 0& 0& 1\\ 0& 1& 0& 6\\ 0& 0& 1& 2\\ 0& 0& 0& 1\end{array}\right]$，则从参考坐标系转换到目标坐标系的变换矩阵$T=\left[\begin{array}{cccc}0& -1& 0& 6\\ 1& 0& 0& -1\\ 0& 0& 1& -2\\ 0& 0& 0& 1\end{array}\right]$。物体在参考坐标系的坐标位置是$P=(3,2,4)$，如果要变换到目标坐标系则左乘变换矩阵T，将得到物体在目标坐标系的位置$P=(4,2,2)$。

  参考：旋转矩阵R、平移向量t以及变换矩阵T的定义及其下标的含义。

1.2 旋转矩阵

  向量的旋转一共有三种表示方法：旋转矩阵、欧拉角和四元数。因此可以用计算旋转（旋转矩阵、欧拉角、四元数、旋转向量）+平移（平移矩阵、平移向量）的8种排列组合来表示变换矩阵，旋转矩阵+平移矩阵是表示变换矩阵最直接的，但其中的数值无法直观的体现出来，因此需要借助欧拉角表示旋转矩阵。欧拉角的计算要区分左右手坐标系以及旋转顺序，在unity中使用的是左手坐标系yxz的顺序。unity中物体的旋转用的是transform.Rotate函数，传入参数的是角度，因此欧拉角转旋转矩阵的代码如下：

Matrix4x4 eulerAnglesToRotationMatrix(Vector3 eulerAngles)
{
    double rx = eulerAngles.x / 180.0 * Math.PI;
    double ry = eulerAngles.y / 180.0 * Math.PI;
    double rz = eulerAngles.z / 180.0 * Math.PI;
    Matrix4x4 Rx = new Matrix4x4(new Vector4(1f, 0f, 0f, 0f), new Vector4(0f, (float)Math.Cos(rx), (float)Math.Sin(rx), 0f), new Vector4(0f, -(float)Math.Sin(rx), (float)Math.Cos(rx), 0f), new Vector4(0f, 0f, 0f, 1f));
    Matrix4x4 Ry = new Matrix4x4(new Vector4((float)Math.Cos(ry), 0f, -(float)Math.Sin(ry), 0f), new Vector4(0f, 1f, 0f, 0f), new Vector4((float)Math.Sin(ry), 0f, (float)Math.Cos(ry), 0f), new Vector4(0f, 0f, 0f, 1f));
    Matrix4x4 Rz = new Matrix4x4(new Vector4((float)Math.Cos(rz), (float)Math.Sin(rz), 0f, 0f), new Vector4(-(float)Math.Sin(rz), (float)Math.Cos(rz), 0f, 0f), new Vector4(0f, 0f, 1f, 0f), new Vector4(0f, 0f, 0f, 1f));
    return Ry * Rx * Rz;
}

四元数转旋转矩阵的代码如下：

Matrix4x4 quaternionToRotationMatrix(Quaternion quaternion)
{
    Matrix4x4 result = new Matrix4x4();
    double w = quaternion.w;
    double rx = quaternion.x;
    double ry = quaternion.y;
    double rz = quaternion.z;
    result.m00 = (float)(1 - 2 * ry * ry - 2 * rz * rz);
    result.m01 = (float)(2 * rx * ry - 2 * w * rz);
    result.m02 = (float)(2 * rx * rz + 2 * w * ry);
    result.m10 = (float)(2 * rx * ry + 2 * w * rz);
    result.m11 = (float)(1 - 2 * rx * rx - 2 * rz * rz);
    result.m12 = (float)(2 * ry * rz - 2 * w * rx);
    result.m20 = (float)(2 * rx * rz - 2 * w * ry);
    result.m21 = (float)(2 * ry * rz + 2 * w * rx);
    result.m22 = (float)(1 - 2 * rx * rx - 2 * ry * ry);
    result.m33 = 1;
    return result;
}

参考：彻底搞懂“旋转矩阵/欧拉角/四元数”，让你体会三维旋转之美。

1.3 欧拉角

旋转矩阵转欧拉角的代码如下：

Vector3 rotationMatrixToEulerAngles(Matrix4x4 T)
{
    float rx = (float)(Math.Asin(-T.m12));
    float ry = (float)(Math.Atan2(T.m02, T.m22));
    float rz = (float)(Math.Atan2(T.m10, T.m11));
    return new Vector3(rx, ry, rz);
}

四元数转欧拉角的代码如下：

var eulerAngles = rotation.eulerAngles;

1.4 四元数

旋转矩阵转四元数的代码如下：

var rotation = Quaternion.Euler(rotationMatrixToEulerAngles(T));

欧拉角转四元数的代码如下：

var rotation = Quaternion.Euler(eulerAngles);

二、unity搭建机械臂

2.1 下载机械臂模型

  unity导入的模型需要fbx格式，下载下来的是step格式，需要使用软件(如3dsmax)进行转换。将模型导入后看到模型部件的名称是乱码，且有层级关系，我们修改对应的名称并且不需要这种层级关系。创建14个空物体分别命名为机械臂、组件1-7、关节1-6，所有空物体的初始位置要在(0,0,0)，并把模型部件移动到对应的组件下，如图所示。

参考：资料下载。

2.2 确定用于动画的旋转坐标系

  把关节的初始位置设置在关节的圆形切面的中心位置，如图所示。

参考：unity制作机械臂。

2.3 关节旋转动画的设计与实现

  将第i+1个组件放入第i个关节中，如图所示。

  此时，改变inspector界面中的rotation就可以实现关节的旋转了，如果动画实现错误，将组件、关节拖动到一个层级上，检查组件的初始位置是否为(0,0,0)，关节的初始位置是否在关节圆形切面的中心位置。

  接下来，为每个关节实现旋转的代码。

public class Joint : MonoBehaviour
{
    public TextMeshProUGUI text;		//用于可视化界面显示角度的文本框
    public Vector3 rotationAxis;		//xyz哪个轴进行旋转才能实现动画效果
    public Vector2 rotationLimit;		//对关节的角度进行限制
    float angle = 0f;					//用于存储当前关节的角度
    float rotationAnglePerSencond = 45f;//每秒旋转多少角度
    int isRotation = 0;					//按钮是否被按下，有-1，0，1三个值，分别表示逆向旋转，暂停，正向旋转
    // 如果按钮被按下，则按照按钮对应的值进行旋转
    void Update(){if (isRotation != 0) jointRotation(rotationAnglePerSencond * Time.deltaTime * isRotation);}
    //获取当前关节的角度
    public float getAngle(){return angle;}
    //检查关节能否继续旋转△角度
    public bool jointCheck(float deltaAngle)
    {
        if (angle + deltaAngle < rotationLimit.x)
            return false;
        if (angle + deltaAngle > rotationLimit.y)
            return false;
        return true;
    }
    //检查关节是否可以旋转，若可以更新角度和UI
    public void jointRotation(float deltaAngle)
    {
        if (jointCheck(deltaAngle))
        {
            angle += deltaAngle;
            transform.Rotate(deltaAngle * rotationAxis);
            text.SetText(text.text.Substring(0, 12) + String.Format("{0:F2}", angle).PadLeft(7, ' '));
        }
    }
    //绑定按钮+按下事件
    public void rotationDirectionAdd(){isRotation = 1;}
    //绑定按钮-按下事件
    public void rotationDirectionSub(){isRotation = -1;}
    //绑定按钮+-弹起事件
    public void rotationDirectionStop(){isRotation = 0;}
}

  将脚本拖动到每个关节中，设置脚本中的初始值，并将UI界面的按钮事件与脚本绑定，最终就可以实现如3.1 正移动学中的动画效果了。

参考：基于 Unity 的5自由度机械臂运动仿真。

2.4 确定用于建模的变换坐标系

这一部分需要理解参考中的方法才能实现，如果机械臂的大致位置朝向和本文类似，可以试着跟着做。本文用的是SDH法，具体流程如下：
1、创建7个空物体分别命名为参考系1-7，所有空物体的初始位置要在(0,0,0)。
2、移动7个空物体到其相应的位置，精确位置的数值可以参考2.2旋转坐标系的位置。
具体操作：
①往上（绿色y轴正方向）移动得到②
②往上（绿色y轴正方向）移动得到③
③往上（绿色y轴正方向）移动得到④
④往右（蓝色z轴正方向）移动得到⑤
⑤往上（绿色y轴正方向）移动得到⑥
⑥往右（蓝色z轴正方向）移动得到⑦
3、旋转7个空物体到其相应的朝向，朝向的规则为：绿色y轴与其对应的关节圆形切面垂直；红色x轴与屏幕平面垂直。
具体操作：
②红色x轴正向旋转90度
③红色x轴正向旋转90度
④红色x轴正向旋转90度
⑥红色x轴正向旋转90度
⑦红色x轴正向旋转90度
4、确定SDH法中的参数，可以描述为：参考系i的红色x轴(xoz平面)绕其绿色y轴offset度得到参考系i+1的红色x轴(xoz平面)相同朝向；再将参考系i的原点沿绿色y轴平移d单位得到参考系i+1的原点相同高度；再将参考系i的绿色y轴(yoz平面)绕其红色x轴alpha度得到参考系i+1的绿色y轴(yoz平面)相同朝向；再将参考系i的原点沿蓝色z轴平移a单位得到参考系i+1的原点相同位置。
具体操作：
①依次通过rotation.y转动0度，position.y平移1.22单位，rotation.x转动90度，position.z平移0单位得到②
②依次通过rotation.y转动0度，position.y平移0单位，rotation.x转动0度，position.z平移-4.07单位得到③
③依次通过rotation.y转动0度，position.y平移0单位，rotation.x转动0度，position.z平移-3.77单位得到④
④依次通过rotation.y转动0度，position.y平移1.21单位，rotation.x转动-90度，position.z平移0单位得到⑤
⑤依次通过rotation.y转动0度，position.y平移1.03单位，rotation.x转动90度，position.z平移0单位得到⑥
⑥依次通过rotation.y转动0度，position.y平移0.95单位，rotation.x转动0度，position.z平移0单位得到⑦

参考：机器人工程师进阶之路（二）6轴机械臂D-H法建模。

2.5 正逆运动学的设计与实现

2.5.1 正运动学

正运动学的推导如4.2描述，因此正运动学的代码如下：

Matrix4x4 forward(double[] joint_theta)
{
    Matrix4x4 T = Matrix4x4.identity;
    // 公式T=T1T2T3T4T5T6
    for (int i = 0; i < 6; i++) T *= SDH(new Vector3((float)alpha[i], (float)(joint_theta[i]+offset[i]), 0), new Vector3(0, (float)d[i], (float)a[i]));
    return T;
}

其中SDH函数的代码如下：

Matrix4x4 SDH(Vector3 eulerAngles, Vector3 position = new Vector3())
{
    double rx = eulerAngles.x / 180.0 * Math.PI;
    double ry = eulerAngles.y / 180.0 * Math.PI;
    double rz = eulerAngles.z / 180.0 * Math.PI;
    double x = position.x;
    double y = position.y;
    double z = position.z;
    Matrix4x4 Rx = new Matrix4x4(new Vector4(1f, 0f, 0f, 0f), new Vector4(0f, (float)Math.Cos(rx), (float)Math.Sin(rx), 0f), new Vector4(0f, -(float)Math.Sin(rx), (float)Math.Cos(rx), 0f), new Vector4(0f, 0f, 0f, 1f));
    Matrix4x4 Ry = new Matrix4x4(new Vector4((float)Math.Cos(ry), 0f, -(float)Math.Sin(ry), 0f), new Vector4(0f, 1f, 0f, 0f), new Vector4((float)Math.Sin(ry), 0f, (float)Math.Cos(ry), 0f), new Vector4(0f, 0f, 0f, 1f));
    Matrix4x4 Rz = new Matrix4x4(new Vector4((float)Math.Cos(rz), (float)Math.Sin(rz), 0f, 0f), new Vector4(-(float)Math.Sin(rz), (float)Math.Cos(rz), 0f, 0f), new Vector4(0f, 0f, 1f, 0f), new Vector4(0f, 0f, 0f, 1f));
    Matrix4x4 Tx = new Matrix4x4(new Vector4(1f, 0f, 0f, 0f), new Vector4(0f, 1f, 0f, 0f), new Vector4(0f, 0f, 1f, 0f), new Vector4((float)x, 0f, 0f, 1f));
    Matrix4x4 Ty = new Matrix4x4(new Vector4(1f, 0f, 0f, 0f), new Vector4(0f, 1f, 0f, 0f), new Vector4(0f, 0f, 1f, 0f), new Vector4(0f, (float)y, 0f, 1f));
    Matrix4x4 Tz = new Matrix4x4(new Vector4(1f, 0f, 0f, 0f), new Vector4(0f, 1f, 0f, 0f), new Vector4(0f, 0f, 1f, 0f), new Vector4(0f, 0f, (float)z, 1f));
    return Ry * Ty * Rx * Tx * Rz * Tz;
}

返回的T变换矩阵包含了旋转和位置的信息，可以通过1.3 变换矩阵转欧拉角来获取旋转信息，T.m03，T.m13，T.m23则是位置信息。

2.5.2 逆运动学

逆运动学的推导如4.3描述，因此逆运动学的代码如下：

List<np.array> inverseKinematics(Matrix4x4 T)
{
    double d1 = d[0], a2 = a[1], a3 = a[2], d4 = d[3], d5 = d[4], d6 = d[5];
    double nx = T.m00, ox = T.m01, ax = T.m02, px = T.m03;
    double ny = T.m10, oy = T.m11, ay = T.m12, py = T.m13;
    double nz = T.m20, oz = T.m21, az = T.m22, pz = T.m23;
    var ans = new np.array[6];
    for (int i = 0; i < ans.Length; i++) ans[i] = new np.array();
    double m1 = pz - d6 * oz;
    double n1 = d6 * ox - px;
    ans[0][0, 4] = new np.array(np.arctan2(m1, n1) - np.arctan2(d4, np.pow(np.pow(m1, 2) + np.pow(n1, 2) - np.pow(d4, 2), 0.5)), 4);
    ans[0][4, 8] = new np.array(np.arctan2(m1, n1) - np.arctan2(d4, -np.pow(np.pow(m1, 2) + np.pow(n1, 2) - np.pow(d4, 2), 0.5)), 4);
    var s1 = np.sin(ans[0]);
    var c1 = np.cos(ans[0]);
    ans[4][0, 2] = np.arccos(s1[0, 2] * ox + oz * c1[0, 2]);
    ans[4][2, 4] = -np.arccos(s1[2, 4] * ox + oz * c1[2, 4]);
    ans[4][4, 6] = np.arccos(s1[4, 6] * ox + oz * c1[4, 6]);
    ans[4][6, 8] = -np.arccos(s1[6, 8] * ox + oz * c1[6, 8]);
    var s5 = np.sin(ans[4]);
    var c5 = np.cos(ans[4]);
    var m2 = s1 * ax + az * c1;
    var n2 = s1 * nx + nz * c1;
    ans[5] = np.arctan2(m2, n2) + np.arctan2(0, -s5);
    var s6 = np.sin(ans[5]);
    var c6 = np.cos(ans[5]);
    var m3 = d5 * (-(c1 * nx - nz * s1) * s6 + c6 * (c1 * ax - az * s1)) - d6 * (c1 * ox - oz * s1) + (c1 * px - pz * s1);
    var n3 = d5 * (ny * s6 - ay * c6) + oy * d6 - py + d1;
    ans[2][0, 8, 2] = np.arccos(np.clip((np.pow(m3[0, 8, 2], 2) + np.pow(n3[0, 8, 2], 2) - np.pow(a2, 2) - np.pow(a3, 2)) / (2 * a2 * a3), -1, 1));
    ans[2][1, 8, 2] = -np.arccos(np.clip((np.pow(m3[1, 8, 2], 2) + np.pow(n3[1, 8, 2], 2) - np.pow(a2, 2) - np.pow(a3, 2)) / (2 * a2 * a3), -1, 1));
    var s3 = np.sin(ans[2]);
    var c3 = np.cos(ans[2]);
    var s2 = ((a3 * c3 + a2) * m3 - a3 * s3 * n3) / (np.pow(a2, 2) + np.pow(a3, 2) + 2 * a2 * a3 * c3);
    var c2 = (n3 + a3 * s3 * s2) / (a3 * c3 + a2);
    ans[1] = np.arctan2(s2, c2);
    var m4 = (-ny * c6 - ay * s6) * (-c5) + s5 * oy;
    var n4 = (ny * s6 - ay * c6);
    ans[3] = np.arctan2(m4, n4) - ans[1] - ans[2];
    return ans;
}

其中np类是对一些计算的封装，array类是对double[]的一个封装，方便用于计算，给出部分代码。

class np
{
    public class array
    {
        public double[] data;
        public int Length;
		public array(double[] data){this.data = data;this.Length = data.Length;}
		public static array operator +(array a, array b)
		{
		    array ans = new array(new double[a.Length]);
		    for (int i = 0; i < a.Length; i++){ans[i] = a[i] + b[i];}
		    return ans;
		}
		public array this[int begin, int end, int sep]
		{
		    get{array ans = new array(new double[(end - begin + 1) / sep]);for (int i = 0; i < ans.Length; i++){ans[i] = this[begin + i * sep];}return ans;}
		    set{for (int i = begin; i < end; i += sep){this[i] = value[(i - begin) / sep];}}
		}
	}
	public static array arctan2(array a, array b)
	{
	    array ans = new array(new double[a.Length]);
	    for (int i = 0; i < a.Length; i++){ans[i] = Math.Atan2(a[i], b[i]);}
	    return ans;
	}
}

参考：UR机械臂正逆运动学求解。

2.6 机械臂示教器的设计与实现

2.6.1 回顾之前的工作

我们在第一章实现了旋转矩阵、欧拉角和四元数的互相转换。在2.5节实现了正逆移动学的代码，可以将(6个关节的角度)和(末端位置的旋转和平移)进行互相转换。在2.3节实现了单个关节的旋转动画。因此在这一节将实现机械臂示教器的模拟。

2.6.2 UI界面的制作

从下幅图片中可以看出，对机械臂的控制按钮有24个，分别是6个轴、3个姿态、3个位置的正向和反向变化。

创建一个UI相机；创建一个UI图层，包括显示机械臂所在环境的image，显示当前帧和下一帧的变化矩阵的text，6个轴、3个姿态、3个位置的正向和反向button及text。

2.6.3 机械臂脚本的制作

机械臂脚本的核心代码如下，在Start函数中限制帧率并初始化关节位置，在Update函数中的步骤如下：
1、获取每个关节的角度
2、正运动学求解获取x、y、z、rx、ry、rz
3、更新示教器UI
4、有无示教器操作，若无返回1
5、逆运动学求解获取j1、j2、j3、j4、j5、j6
6、选择最优解
7、移动每个关节到相应角度
8、更新示教器UI

void Start()
{
    // 限制帧率，初始化关节位置
    Application.targetFrameRate = 60;
    joint1.GetComponent<Joint>().jointRotation(0f);
    joint2.GetComponent<Joint>().jointRotation(-10f);
    joint3.GetComponent<Joint>().jointRotation(-50f);
    joint4.GetComponent<Joint>().jointRotation(-30f);
    joint5.GetComponent<Joint>().jointRotation(90f);
    joint6.GetComponent<Joint>().jointRotation(90f);
}
void Update()
{
    theta = getTheta();                     //获取每个关节的角度
    T = forward(theta);                     //正运动学求解
    updateUI();                             //更新示教器UI
    if (teachingPendant())                  //有无示教器操作
    {
        var ans = inverseKinematics(nextT); //逆运动学求解
        if (ans.Count > 0)
        {
            var cnt = chooseBest(ans);      //选择最优解
            moveJ(ans[cnt].data);           //通过轴动实现运动
        }
    }
}

其中updateUI函数是读取变量并更新到UI界面上；teachingPendant函数是判断有无键鼠操作，有操作修改下一帧的T，无操作返回false；chooseBest函数是从8组解中选择与当前关节角度最接近的解。
参考：unity制作机械臂。

三、结果展示

3.1 正移动学

3.2 逆移动学

  初版代码有bug，导致有时关节直接进行转跳，后来采用double的高精度浮点型数据类型进行计算可以避免bug。

四、原理解释

不同的坐标系和参数算出来的结果都不一样，下面的符号只适用于本文的情况进行推导，其中的区别是unity用的是左手坐标系y轴朝上，教程是右手坐标系z轴朝上。
参考：[机器人工程师进阶之路（二）6轴机械臂D-H法建模；UR机械臂正逆运动学求解]。

4.1 符号解释

• $T_i$表示从第$i-1$个坐标系变换到第$i$个坐标系的变换矩阵。
• 定义所有的y轴与关节活动处的圆形平面垂直。
• ${\alpha }_i$表示从第$i-1$个坐标系的y轴（yoz平面）绕其x轴旋转多少度能得到第$i$个坐标系的y轴（yoz平面）。
• ${\theta }_i$表示第$i$个关节的转动角度。
• $offse{t}_i$表示对第$i$个关节的补偿角度。
• $d_i$表示从第$i-1$个坐标系的原点沿其y轴平移多少距离能得到第$i$个坐标系的原点。
• $a_i$表示从第$i-1$个坐标系的原点沿其z轴平移多少距离能得到第$i$个坐标系的原点。
• Rx,Ry,Rz表示旋转矩阵，tx,ty,tz表示平移矩阵。
• 将$sin({\theta }_1)$简化为s1，将$cos({\theta }_1)$简化为c1，其他下标依次类推。

4.2 正运动学

重新推导变换矩阵为：
$$T_i=Ry\ast ty\ast Rx\ast tz=\left[\begin{array}{cccc}cos(\theta )& sin(\theta )sin(\alpha )& sin(\theta )cos(\alpha )& z\ast sin(\theta )cos(\alpha )\\ 0& cos(\alpha )& -sin(\alpha )& -z\ast sin(\alpha )+y\\ -sin(\theta )& cos(\theta )sin(\alpha )& cos(\theta )cos(\alpha )& z\ast cos(\theta )cos(\alpha )\\ 0& 0& 0& 1\end{array}\right]$$
绘制参数表格：

i	$\alpha$(x轴)	$\theta$(y轴)	d(y轴)	a(z轴)	offset(y轴)
1	90	j1	1.22	0	0
2	0	j2	0	-4.07	0
3	0	j3	0	-3.77	0
4	-90	j4	1.21	0	0
5	90	j5	1.03	0	0
6	0	j6	0.95	0	0

将每个变量代入得：
$$T_1=\left[\begin{array}{cccc}c1& s1& 0& 0\\ 0& 0& -1& d1\\ -s1& c1& 0& 0\\ 0& 0& 0& 1\end{array}\right]，T_2=\left[\begin{array}{cccc}c2& 0& s2& a2\ast s2\\ 0& 1& 0& 0\\ -s2& 0& c2& a2\ast c2\\ 0& 0& 0& 1\end{array}\right]，T_3=\left[\begin{array}{cccc}c3& 0& s3& a3\ast s3\\ 0& 1& 0& 0\\ -s3& 0& c3& a3\ast c3\\ 0& 0& 0& 1\end{array}\right]，T_4=\left[\begin{array}{cccc}c4& -s4& 0& 0\\ 0& 0& 1& d4\\ -s4& -c4& 0& 0\\ 0& 0& 0& 1\end{array}\right]，T_5=\left[\begin{array}{cccc}c5& s5& 0& 0\\ 0& 0& -1& d5\\ -s5& c5& 0& 0\\ 0& 0& 0& 1\end{array}\right]，T_6=\left[\begin{array}{cccc}c6& 0& s6& 0\\ 0& 1& 0& d6\\ -s6& 0& c6& 0\\ 0& 0& 0& 1\end{array}\right]$$
设T为：
$$T=\left[\begin{array}{cccc}nx& ox& ax& px\\ ny& oy& ay& py\\ nz& oz& az& pz\\ 0& 0& 0& 1\end{array}\right]$$
正运动学公式为
$$T=T_1{T}_2{T}_3{T}_4{T}_5{T}_6$$

4.3 逆运动学

4.3.1 求解156轴

这个公式成立：
$$T_1^{-1}T{T}_6^{-1}=T_2{T}_3{T}_4{T}_5$$
其中1和6的逆为：
$$T_1^{-1}=\left[\begin{array}{cccc}c1& 0& -s1& 0\\ s1& 0& c1& 0\\ 0& -1& 0& d1\\ 0& 0& 0& 1\end{array}\right]，T_6^{-1}=\left[\begin{array}{cccc}c6& 0& -s6& 0\\ 0& 1& 0& -d6\\ s6& 0& c6& 0\\ 0& 0& 0& 1\end{array}\right]$$

左边等于：
$$T_1^{-1}T{T}_6^{-1}=\left[\begin{array}{cccc}(c1\ast nx-nz\ast s1)\ast c6+s6\ast (c1\ast ax-az\ast s1)& (c1\ast ox-oz\ast s1)& -(c1\ast nx-nz\ast s1)\ast s6+c6\ast (c1\ast ax-az\ast s1)& -d6\ast (c1\ast ox-oz\ast s1)+(c1\ast px-pz\ast s1)\\ (s1\ast nx+nz\ast c1)\ast c6+s6\ast (s1\ast ax+az\ast c1)& (s1\ast ox+oz\ast c1)& -(s1\ast nx+nz\ast c1)\ast s6+c6\ast (s1\ast ax+az\ast c1)& -d6\ast (s1\ast ox+oz\ast c1)+(s1\ast px+pz\ast c1)\\ -ny\ast c6-ay\ast s6& -oy& ny\ast s6-ay\ast c6& oy\ast d6-py+d1\\ 0& 0& 0& 1\end{array}\right]$$
右边等于：
$$T_2{T}_3{T}_4{T}_5=\left[\begin{array}{cccc}c234\ast c5& c234\ast s5& s234& -s234\ast d5+a3\ast s23+a2\ast s2\\ -s5& c5& 0& d4\\ -s234\ast c5& -s234\ast s5& c234& -c234\ast d5+a3\ast c23+a2\ast c2\\ 0& 0& 0& 1\end{array}\right]$$
其中有3条方程为：
$$\{\begin{array}{c}-d6\ast (s1\ast ox+oz\ast c1)+(s1\ast px+pz\ast c1)=d4①\\ (s1\ast ox+oz\ast c1)=c5②\\ (s1\ast nx+nz\ast c1)\ast c6+s6\ast (s1\ast ax+az\ast c1)=-s5③\end{array}$$

---

根据①式解得：$${\theta }_1=arctan2(m_1,n_1)-arctan2(d4,\pm \sqrt{m_1^2+n_1^2-d_4^2})$$
其中 $m_1=pz-d6\ast oz，n_1=d6\ast ox-px$。
*${\theta }_1$有两个解，得到两组解。

---

根据②式解得：$${\theta }_5=\pm arccos(s1\ast ox+oz\ast c1)$$
*${\theta }_5$有两个解，得到四组解。

---

根据③式解得：$${\theta }_6=arctan2(m_2,n_2)+arctan2(0,-s5)$$
其中 $m_2=s1\ast ax+az\ast c1，n_2=s1\ast nx+nz\ast c1$。
*arctan2(0,-s5)根据第5个轴的正负使其相差180°。

---

4.3.2 求解234轴

这个公式成立：
$$T_1^{-1}T{T}_6^{-1}{T}_5^{-1}=T_2{T}_3{T}_4$$
其中5的逆为：
$$T_5^{-1}=\left[\begin{array}{cccc}c5& 0& -s5& 0\\ s5& 0& c5& 0\\ 0& -1& 0& d5\\ 0& 0& 0& 1\end{array}\right]$$

左边等于：
$$T_1^{-1}T{T}_6^{-1}{T}_5^{-1}=\left[\begin{array}{cccc}((c1\ast nx-nz\ast s1)\ast c6+s6\ast (c1\ast ax-az\ast s1))\ast c5+s5\ast (c1\ast ox-oz\ast s1)& (c1\ast nx-nz\ast s1)\ast s6-c6\ast (c1\ast ax-az\ast s1)& ((c1\ast nx-nz\ast s1)\ast c6+s6\ast (c1\ast ax-az\ast s1))\ast (-s5)+c5\ast (c1\ast ox-oz\ast s1)& d5\ast (-(c1\ast nx-nz\ast s1)\ast s6+c6\ast (c1\ast ax-az\ast s1))-d6\ast (c1\ast ox-oz\ast s1)+(c1\ast px-pz\ast s1)\\ ((s1\ast nx+nz\ast c1)\ast c6+s6\ast (s1\ast ax+az\ast c1))\ast c5+s5\ast (s1\ast ox+oz\ast c1)& (s1\ast nx+nz\ast c1)\ast s6-c6\ast (s1\ast ax+az\ast c1)& ((s1\ast nx+nz\ast c1)\ast c6+s6\ast (s1\ast ax+az\ast c1))\ast (-s5)+c5\ast (s1\ast ox+oz\ast c1)& d5\ast (-(s1\ast nx+nz\ast c1)\ast s6+c6\ast (s1\ast ax+az\ast c1))-d6\ast (s1\ast ox+oz\ast c1)+(s1\ast px+pz\ast c1)\\ (-ny\ast c6-ay\ast s6)\ast c5+s5\ast (-oy)& -(ny\ast s6-ay\ast c6)& (-ny\ast c6-ay\ast s6)\ast (-s5)+c5\ast (-oy)& d5\ast (ny\ast s6-ay\ast c6)+oy\ast d6-py+d1\\ 0& 0& 0& 1\end{array}\right]$$
右边等于：
$$T_2{T}_3{T}_4=\left[\begin{array}{cccc}c234& -s234& 0& a3\ast s23+a2\ast s2\\ 0& 0& 1& d4\\ -s234& -c234& 0& a3\ast c23+a2\ast c2\\ 0& 0& 0& 1\end{array}\right]$$
其中有4条方程为：
$$\{\begin{array}{c}d5\ast (-(c1\ast nx-nz\ast s1)\ast s6+c6\ast (c1\ast ax-az\ast s1))-d6\ast (c1\ast ox-oz\ast s1)+(c1\ast px-pz\ast s1)=a3\ast s23+a2\ast s2④\\ d5\ast (ny\ast s6-ay\ast c6)+oy\ast d6-py+d1=a3\ast c23+a2\ast c2⑤\\ -s234=(-ny\ast c6-ay\ast s6)\ast c5+s5\ast (-oy)⑥\\ -c234=-(ny\ast s6-ay\ast c6)⑦\end{array}$$

---

根据④⑤式解得：$${\theta }_3=\pm arccos(\frac{m_3^2+n_3^2-a{2}^2-a{3}^2}{2\ast a2\ast a3})$$
其中 $m_3=d5\ast (-(c1\ast nx-nz\ast s1)\ast s6+c6\ast (c1\ast ax-az\ast s1))-d6\ast (c1\ast ox-oz\ast s1)+(c1\ast px-pz\ast s1)，n_3=d5\ast (ny\ast s6-ay\ast c6)+oy\ast d6-py+d1$。
*${\theta }_3$有两个解，得到八组解。但$\frac{m_3^2+n_3^2-a{2}^2-a{3}^2}{2\ast a2\ast a3}>1$，使某几组解解不出？？？最终解为2组的概率为24.5%，为4组的概率为73.2%，为6组的概率为1.7%，为8组的概率为0.6%。

---

根据④⑤式解得：$${\theta }_2=arctan2(s2,c2)$$
其中 $s2=\frac{(a3\ast c3+a2)\ast m_3-a3\ast s3\ast n_3}{a{2}^2+a{3}^2+2\ast a2\ast a3\ast c3}，c2=\frac{n_3+a3\ast s3\ast s2}{a3\ast c3+a2}$。

---

根据⑥⑦式解得：$${\theta }_4=arctan2(m_4,n_4)-{\theta }_2-{\theta }_3$$
其中 $m_4=(-ny\ast c6-ay\ast s6)\ast (-c5)+s5\ast oy，n_4=(ny\ast s6-ay\ast c6)$。

---

4.4 用python进行验证

import numpy as np
def eulerAnglesToRotationMatrix(alpha,theta,beta,x,y,z):
    alpha = alpha/180.0*np.pi
    theta = theta/180.0*np.pi
    beta = beta/180.0*np.pi
    Rx=np.array([
       [1, 0,             0,              0],
       [0, np.cos(alpha), -np.sin(alpha), 0],
       [0, np.sin(alpha), np.cos(alpha),  0],
       [0, 0,             0,              1]
    ])
    Ry=np.array([
       [np.cos(theta),  0, np.sin(theta), 0],
       [0,              1,             0, 0],
       [-np.sin(theta), 0, np.cos(theta), 0],
       [0,              0,             0, 1]
    ])
    Rz=np.array([
       [np.cos(beta), -np.sin(beta), 0, 0],
       [np.sin(beta), np.cos(beta),  0, 0],
       [0,             0,              1, 0],
       [0,             0,              0, 1]
    ])
    Tx=np.array([
       [1, 0, 0, x],
       [0, 1, 0, 0],
       [0, 0, 1, 0],
       [0, 0, 0, 1]
    ])
    Ty=np.array([
       [1, 0, 0, 0],
       [0, 1, 0, y],
       [0, 0, 1, 0],
       [0, 0, 0, 1]
    ])
    Tz=np.array([
       [1, 0, 0, 0],
       [0, 1, 0, 0],
       [0, 0, 1, z],
       [0, 0, 0, 1]
    ])
    return Ry@Ty@Rx@Tx@Rz@Tz
pq = lambda x: 180 - (180 - x) % 360
pr = lambda x: print(np.round(x,4))
num_ans = []
for epoch in range(10):
    #======================init=======================
    a = [0,-4.07,-3.77,0,0,0]
    d = [1.22,0,0,1.21,1.03,0.95]
    d1,a2,a3,d4,d5,d6 = d[0],a[1],a[2],d[3],d[4],d[5]
    theta = (np.random.rand(6)*2-1)*180/np.pi
    #====================forward======================
    T = np.eye(4)
    T = T@eulerAnglesToRotationMatrix(90,theta[0],0,0,d[0],a[0])
    T = T@eulerAnglesToRotationMatrix(0,theta[1],0,0,d[1],a[1])
    T = T@eulerAnglesToRotationMatrix(0,theta[2],0,0,d[2],a[2])
    T = T@eulerAnglesToRotationMatrix(-90,theta[3],0,0,d[3],a[3])
    T = T@eulerAnglesToRotationMatrix(90,theta[4],0,0,d[4],a[4])
    T = T@eulerAnglesToRotationMatrix(0,theta[5],0,0,d[5],a[5])
    (nx,ox,ax,px),(ny,oy,ay,py),(nz,oz,az,pz),_= T
    print("6个关节旋转的角度:",np.round(theta,4))
    print("===>正运动学解:\n",np.round(T,4))
    #======================156轴=======================
    ans = np.zeros((8,6))
    m1 = pz-d6*oz
    n1 = d6*ox-px
    ans[0:4,0]=np.arctan2(m1,n1)-np.arctan2(d4,(m1**2+n1**2-d4**2)**0.5)
    ans[4:8,0]=np.arctan2(m1,n1)-np.arctan2(d4,-(m1**2+n1**2-d4**2)**0.5)
    s1 = np.sin(ans[:,0])
    c1 = np.cos(ans[:,0])
    # pr(-d6*(s1*ox+oz*c1)+(s1*px+pz*c1)-d4)
    ans[0:2,4]=np.arccos(s1[0:2]*ox+oz*c1[0:2])
    ans[2:4,4]=-np.arccos(s1[2:4]*ox+oz*c1[2:4])
    ans[4:6,4]=np.arccos(s1[4:6]*ox+oz*c1[4:6])
    ans[6:8,4]=-np.arccos(s1[6:8]*ox+oz*c1[6:8])
    s5 = np.sin(ans[:,4])
    c5 = np.cos(ans[:,4])
    # pr(s1*ox+oz*c1-c5)
    m2 = s1*ax+az*c1
    n2 = s1*nx+nz*c1
    ans[:,5]=np.arctan2(m2,n2)+np.arctan2(0,-s5)
    s6 = np.sin(ans[:,5])
    c6 = np.cos(ans[:,5])
    # pr(n*c6+s6*m+s5)
    #======================234轴=======================
    m3 = d5*(-(c1*nx-nz*s1)*s6+c6*(c1*ax-az*s1))-d6*(c1*ox-oz*s1)+(c1*px-pz*s1)
    n3 = d5*(ny*s6-ay*c6)+oy*d6-py+d1
    ans[0::2,2] = np.arccos(np.clip((m3[0::2]**2+n3[0::2]**2-a2**2-a3**2)/(2*a2*a3), -1, 1))
    ans[1::2,2] = -np.arccos(np.clip((m3[1::2]**2+n3[1::2]**2-a2**2-a3**2)/(2*a2*a3), -1, 1))
    s3 = np.sin(ans[:,2])
    c3 = np.cos(ans[:,2])
    s2 = ((a3*c3+a2)*m3-a3*s3*n3)/(a2**2+a3**2+2*a2*a3*c3)
    c2 = (n3+a3*s3*s2)/(a3*c3+a2)
    ans[:,1]=np.arctan2(s2,c2)
    m4=(-ny*c6-ay*s6)*(-c5)+s5*oy
    n4=(ny*s6-ay*c6)
    ans[:,3]=np.arctan2(m4,n4)-ans[:,1]-ans[:,2]
    #======================验证========================
    print("===>逆运动学解: ")
    cnt=0
    for theta_ans in ans:
        theta_ans = theta_ans*180.0/np.pi
        t1 = eulerAnglesToRotationMatrix(90,theta_ans[0],0,0,d[0],a[0])
        t2 = eulerAnglesToRotationMatrix(0,theta_ans[1],0,0,d[1],a[1])
        t3 = eulerAnglesToRotationMatrix(0,theta_ans[2],0,0,d[2],a[2])
        t4 = eulerAnglesToRotationMatrix(-90,theta_ans[3],0,0,d[3],a[3])
        t5 = eulerAnglesToRotationMatrix(90,theta_ans[4],0,0,d[4],a[4])
        t6 = eulerAnglesToRotationMatrix(0,theta_ans[5],0,0,d[5],a[5])
        if np.all(np.isclose(t1@t2@t3@t4@t5@t6,T)):
            pr(pq(theta_ans))
            cnt+=1
    print("="*64)
    num_ans.append(cnt)
print(np.unique(num_ans,return_counts=True))

代码实现了：随机给定一组6轴关节旋转角度，根据正运动学解得末端位置坐标系相对于参考坐标系的变换矩阵；给定变换矩阵，根据逆运动学解得6轴关节旋转角度。具体结果如下图所示：

*注意区分角度和弧度，以上结果均为角度。