下面在前面的ur5机械臂的DH参数基础是对其正逆解进行求解，为了后面能在MATLAB中利用stl文件进行实际显示，这里以标准DH参数为例进行讲解。（修正DH参数在用plot3d函数是显示失败，不知道是不是这个函数只能显示标准dh参数的机械臂模型，有知道的网友可以在评论里告知一下，谢谢。）

机器人正运动学是在已知各连杆相对位置关系（关节角）的情况下，得到末端执行器的位姿。在标准DH参数下相邻坐标系之间的齐次变换矩阵为：

 则，正解代码如下：

这里对姿态的描述进行说明：在MATLAB中RPY欧拉角是世界坐标系下的XYZ欧拉角；

RPY角的定义如下：

 输入端位姿形式为：

MATLAB中对应的RPY旋转矩阵如下：

 这是个旋转矩阵，与齐次矩阵中的旋转矩阵等价，所以根据齐次矩阵中的旋转矩阵便可以得到末端的姿态RPY。

机器人逆运动学是已知机器人末端执行器的位姿，通过变换矩阵T得到机器人各关节的角度。求解逆运动学有解析法、几何法、迭代法。这里介绍解析法。如果机器人末端三轴的轴线始终交于一点则该机器人必有解析解。

1、得到齐次变换矩阵：

利用MATLAB求解其齐次变换矩阵：

利用三角函数的两角和差公式进行化简：

可以得到：

逆解输入的参数是末端的位姿 ，其中是绕z轴的旋转角度，是绕y轴的旋转角度，是绕x轴的旋转角度。这里还需要注意一个问题，MATLAB的.teach()函数输出的图形角度如下图所示：R表示的是世界坐标系到末端坐标系需要绕z轴转动的角度；P表示的是世界坐标系到末端坐标系需要绕y轴转动的角度；Y表示的是世界坐标系到末端坐标系需要绕x轴转动的角度；

 则R对应，P对应，Y对应；表示的是末端坐标系绕自身的xyz轴旋转对应的角度后会与基坐标系平行。这里利用XYZ欧拉角公式，

其对应的旋转矩阵为：

根据输入的Pos参数也能得到一个齐次矩阵：

简记为：

 而T06乘以得到下式：

由于不存在常数项（不包含角度θ的项）；所以需要进一步化简，由上式知如果d6=0则存在常数项。

所以，我们做如下处理，类比于增加一个新连杆，且该连杆不转动而是与连杆6固连，如下表所示：

 然后让d6=0，而d7=d6，新连杆的坐标系与连杆6完全重合，（这里需要注意变化前后总的齐次变换矩阵是不变的即原来的T06与现在的T07是一模一样的即）其齐次矩阵为：

 又，

 故：

 ​​​​​​​

然后便可以求出相应角度的表达式：

这里首先介绍个万能公式：

由上面的R34相等得：

 由R31、R32、R33相等得：

 ②、求关节2、3、4的角度：

由R13、R23相等得：

由R14、R24相等得：

 以上便是所有关节角度的求解公式。

MATLAB代码如下（有点长删掉了部分）：