机械臂PUMA560属于工业机器人（或称机器人操作臂）。从外形来看, 它和人的手臂相似, 是由一系列刚性连杆通过一系列柔性关节交替连接而成的开式链。PUMA560的基座、连杆一、连杆二、连杆三、连杆四到六分别类似于的盆骨、腰椎、大臂、小臂、腕手。操作臂的前端装有末端执行器或相应的工具, 也常称为手或手爪。手臂的动作幅度一般较大,通常实现宏操作。
PUMA560 由机器人本体(手臂)和计算机控制系统两大部分组成。PUMA560整臂重53kg，有六个自由度，驱动采用直流伺服电机并配有安全刹闸，手腕最大载荷为2 kg (包括手腕法兰盘)，最大抓紧力为60 N，重复精度为±0.1mm。工具在最大载荷下的自由运动速度速度为1.0m/s,直线运动速度为0.5m/s。操作范围是以肩部中心为球心0.92 m 为半径的空间半球。
PUMA560属于关节式的机器人，6个关节都是转动关节。前面3个关节确定手腕参考点的位置，后面3个关节确定手腕的方位。和多数工业机器人一样，后3个关节轴线交于一点。关节1 的轴线为铅直方向，关节1和关节2的轴线垂直相交,关节3和关节4的轴线垂直交错。
Unimation PUMA560是一个六自由度机器人，所有关节均为转动关节（即它是一个6R机构）。整个操作臂由基座、大连杆一、大连杆二、大连杆三、小连杆四、小连杆五、小连杆六共七个主要构件串联而成。其中基座相对世界坐标系{W}固定，三个大连杆关节坐标系之间的角度变量的大小决定了操作臂的末端点在世界坐标系的位置坐标，三个小连杆和三个大连杆关节坐标系之间的角度变量的大小共同决定了操作臂末端点坐标系（与连杆六固连的关节坐标系）的姿态，并且三个小连杆的关节轴线交于一点（末三轴相交）。
机器人的运动学包括：正运动学——已知各关节角计算末端的位置姿态；逆运动学——已知末端的位置姿态计算对应的全部关节角。正常情况下，正运动学的解是唯一的，且容易求得；逆运动学的解可能不存在、可能有唯一解、也可能有多个解，且计算复杂。机器人逆运动分析是运动规划与控制中的重要问题，逆运动问题复杂多样，往往无法建立通用的解析算法，但PUMA560结构特殊，存在解析逆解算法。
对PUMA560进行关节轴和关节坐标系标定（如上图），获得PUMA560的MDH参数表：
由于PUMA560是一个串联机器人，末端系与基坐标系之间的映射变换关系具有“传递性”，故两坐标系间的关系可用下式表示：
$$ {}^0T_6= {}^0T_1{}^1T_2{}^2T_3{}^3T_4{}^4T_5{}^5T_6 $$
其中，${}^{i-1}T_{i}$是{i-1}系变换至{i}系所需左乘的一个4×4位姿矩阵，一般表达式为：
$$ \begin{aligned} { }{i}^{i-1} T =\left(\begin{array}{cccc}c \theta{i} & -s \theta_{i} & 0 & a_{i-1} \\ s \theta_{i} c \alpha_{i-1} & c \theta_{i} c \alpha_{i-1} & -s \alpha_{i-1} & -s \alpha_{i-1} d_{i} \\ s \theta_{i} s \alpha_{i-1} & c \theta_{i} s \alpha_{i-1} & c \alpha_{i-1} & c \alpha_{i-1} d_{i} \\ 0 & 0 & 0 & 1\end{array}\right)
\end{aligned} $$
将PUMA560的MDH参数代入上式得到：
$$ \begin{aligned} {}_1^0T &=\left(\begin{array}{cccc}c \theta_1 & -s \theta_1& 0 & 0 \\ s \theta_1& c \theta_1 & 0 & 0\\0 & 0& 1 & 0\\ 0 & 0 & 0 & 1\end{array}\right)\\
{}_2^1T &=\left(\begin{array}{cccc}c \theta_2 & -s \theta_2 & 0 &0\\ 0&0& -1 &0\\ s \theta_2 & c \theta_2& 0&0 \\ 0 & 0 & 0 & 1\end{array}\right)\\
{}_3^2T &=\left(\begin{array}{cccc}c \theta_3 & -s \theta_3 & 0 & 0.4318 \\ s \theta_3& c \theta_3&0&0\\0&0& 1 & 0.15005 \\ 0 & 0 & 0 & 1\end{array}\right)\\
{}_4^3T &=\left(\begin{array}{cccc}c \theta_4 & -s \theta_4 & 0 & 0.0203 \\ 0&0&1&0.4318 \\ -s \theta_4& -c \theta_4&0&0\\ 0 & 0 & 0 & 1\end{array}\right)\\
{}_5^4T &=\left(\begin{array}{cccc}c \theta_5 & -s \theta_5 & 0 &0\\0&0& -1&0\\ s \theta_5& c \theta_5& 0&0\\ 0 & 0 & 0 & 1\end{array}\right)\\
{}_6^5T &=\left(\begin{array}{cccc}c \theta_6 & -s \theta_6 & 0 &0 \\0&0&1&0 \\ -s \theta_6& -c \theta_6 &0&0\\ 0 & 0 & 0 & 1\end{array}\right)\\
\end{aligned} $$
将PUMA560运行时的实际关节角位移代入上式便可得到机械臂的运动学正解。
PUMA560的末三轴相交于一点，因此逆运动学求解方法可采用pieper求法。求解思路为：先由期望的末端位置求出PUMA560前三个关节角位移，再结合期望的末端姿态得到后三个关节角位移。
由{}^0T_6= {}^0T_1{}^1T_2{}^2T_3{}^3T_4{}^4T_5{}^5T_6 可知PUMA560的关节角位移和末端笛卡尔位置满足
$$ \begin{aligned} p={}^0p_{4ORG}&={}^0_1T{}^1_2T{} \left(\begin{array}{c}f_1\\f_2\\f_3\\1\end{array}\right)={}^0_1T \left(\begin{array}{c}g_1\\g_2\\g_3\\1\end{array}\right)\\
&=\left(\begin{array}{c} g_1c\theta_1-g_2s\theta_1\\ g_1s\theta_1+g_2c\theta_1\\ g_3\\ 1 \end{array}\right) =\left(\begin{array}{c}x\\y\\z\\1\end{array}\right) \end{aligned} $$
式中，
$$ \begin{aligned} &f_1(\theta_3)=0.0203c\theta_3-0.4318s\theta_3+0.4318\\ &f_2(\theta_3)=0.0203s\theta_3+0.4318c\theta_3\\ &f_3(\theta_3)=0.15005\\ &g_1(\theta_2,f_1)=f_1c\theta_2-f_2s\theta_2\\ &g_2(\theta_2,f_2)=-f_3\\ &g_3(\theta_2,f_3)=f_1s\theta_2+f_2c\theta_2\\ \end{aligned} $$
对关系式最后一行的等式平方，得到
$$ g_1^2+g_2^2+g_3^2=x^2+y^2+z^2=r $$
将$g_1,g_2,g_3$的表达式代入上式，得到
$$ \begin{aligned} r&=f_1^2+f_2^2+f_3^2\\ &=0.01753c\theta_3-0.373s\theta_3+0.39583\\ &=0.01753\frac{1-t^2\frac{\theta_3}{2}}{1+t^2\frac{\theta_3}{2}}-0.373\frac{2t\frac{\theta_3}{2}}{1+t^2\frac{\theta_3}{2}}+0.39583 \end{aligned} $$
整理得
$$ (r-0.3783)t^2\frac{\theta_3}{2}+0.746t\frac{\theta_3}{2}+(r-0.41336)=0 $$
上式根判别式
$$
\Delta =-4r^2+3.16664r-0.069
$$
由上式可判断是否有解及解的个数，最后通过求根公式可解得$\theta_3$。
下面继续求解$\theta_2$。由
$$ z=g_3=f_1s\theta_2+f_2c\theta_2=f_1\frac{2t\frac{\theta_2}{2}}{1+t^2\frac{\theta_2}{2}}+f_2\frac{1-t^2\frac{\theta_2}{2}}{1+t^2\frac{\theta_2}{2}} $$
得
$$ (z+f_2)t^2\frac{\theta_2}{2}-2f_1t\frac{\theta_2}{2}+(z-f_2)=0 $$
$$ \Delta=4f_1^2-4(z^2-f_2^2) $$
同理，可求得$\theta_2$。
最后进行$\theta_1$的求解：
$$
\left(\begin{array} {c} x \\ y \end{array}\right)=
\left(\begin{array}{cc}c\theta_1 & -s\theta_1 \\ s\theta_1 & c\theta_1\end{array}\right)
\left(\begin{array}{c}g_1\\g_2\end{array}\right) $$
故
$$ \theta_1=Atan2(y，x)-Atan2(g_2,g_1) $$
经上述步骤，我们求得了$\theta_1,\theta_2,\theta_3$。接下来将下面将继续求解剩余的三个关节角$\theta_4,\theta_5,\theta_6$。
由{}^0T_6= {}^0T_1{}^1T_2{}^2T_3{}^3T_4{}^4T_5{}^5T_6 可知PUMA560末端的姿态矩阵为
$$ {}^0R_6={}^0R_1{}^1R_2{}^2R_3{}^3R_4{}^4R_5{}^5R_6 $$
故
$$ {}^{4ORG}R_6={}^0R_{4ORG}^{-1}{}^0R_6 $$
由Z-Y-Z欧拉角关系式
$$ { }^{4ORG}{6} R{Z^{\prime} Y^{\prime} Z^{\prime}(\alpha, \beta, \gamma)}=\left(\begin{array}{ccc}c \alpha c \beta c \gamma-s \alpha s \gamma & -c \alpha c \beta s \gamma-s \alpha c \gamma & c \alpha s \beta \\ s \alpha c \beta c \gamma+c \alpha s \gamma & -s \alpha c \beta s \gamma+c \alpha \gamma & s \alpha s \beta \\ -s \beta c \gamma & s \beta s \gamma & c \beta\end{array}\right) $$
其逆解为
$$ \beta=±\operatorname{Atan2}\left(\sqrt{r_{31}^2+r_{32}^2},r_{33}\right)\\
\alpha=\operatorname{Atan} 2\left(r_{23}/s\beta, r_{13}/s\beta\right)\\
\gamma=\operatorname{Atan} 2\left(r_{32}/s\beta,-r_{31}/s\beta\right) $$
可求出后三个关节角$(\theta_4,\theta_5,\theta_6)=(\alpha,-\beta,\gamma)$
在Solidworks中得到机械臂的三维装配体模型，通过Solidworks插件https://github.com/ros/solidworks_urdf_exporter 导出得到PUMA560网格可视化的URDF文件。具体步骤为：
首先将导出的URDF文件夹包含在MATLAB的搜索路径内，然后将文件导入到MATLAB-SIMULINK-SIMSCAPE中。在MATLAB命令行输入的命令为：
smimport('urdf_filename.urdf') %导入URDF文件：'Puma560Assembly.SLDASM.urdf'
片刻等待后便得到机械臂的SIMULINK可视化仿真模型，将模型关节模块的关节限制修改后运行仿真，出现机械臂做自由运动的仿真动画。
机械臂关节空间的轨迹规划解决机械臂从起始位姿到终止位姿去取放物体的问题。机械臂末端移动的过程并不重要，只要求运动是平滑的且没有碰撞产生。该方法算法简单、工具移动效率高、一般不会发生奇异性。基于关节空间轨迹规划实现机械臂运动的开发路线为
flowchart TD
A[机械臂末端在起始点和终止点的六维位姿]--逆运动学求解-->B[各关节在起始点和终止点的角位移]--关节空间内轨迹规划-->C[各关节在整个过程中的角位移序列]--关节运动控制-->D[末端在笛卡尔空间中的实际运动]
在关节空间内进行轨迹规划最常用的方法主要为三次多项式算法和五次多项式算法。其中三次多项式算法的表达式为
$$ \begin{array}{l}\theta(t)=a_{0}+a_{1} t+a_{2} t^{2}+a_{3} t^{3} \\ \dot{\theta}(t)=a_{1}+2 a_{2} t+3 a_{3} t^{2} \\ \ddot{\theta}(t)=2 a_{2}+6 a_{3} t\end{array} $$
约束条件为
$$ \begin{array}{l} \theta\left(t_{0}\right)=\theta_{0} \\ \theta\left(t_{f}\right)=\theta_{f} \\ \dot{\theta}\left(t_{0}\right)=\dot\theta_{0}\\ \dot{\theta}\left(t_{f}\right)=\dot\theta_{f} \end{array} $$
系数为
$$ \begin{array}{l}a_{0}=\theta_{0} \\ a_{1}=v_{0} \\ a_{2}=\frac{3}{t_{f}^{2}}\left(\theta_{f}-\theta_{0}\right)-\frac{1}{t_{f}}\left(2 \dot\theta_{0}+\dot\theta_{f}\right) \\ a_{3}=\frac{2}{t_{f}^{3}}\left(\theta_{0}-\theta_{f}\right)+\frac{1}{t_{f}^{2}}\left(\dot\theta_{0}+\dot\theta_{f}\right)\end{array} $$
五次多项式算法的表达式为
$$ \begin{aligned} \theta{(t)}&=a_{0}+a_{1} t+a_{2} t^{2}+a_{3} t^{3}+a_{4} t^{4}+a_{5} t^{5}\\ \dot{\theta}{(t)}&=a_{1}+2 a_{2} t+3 a_{3} t^{2}+4 a_{4} t^{3}+5 a_{5} t^{4}\\ \ddot{\theta}{(t)}&=2 a_{2}+6 a_{3} t+12 a_{4} t^{2}+20 a_{5} t^{3} \end{aligned} $$
约束条件为
$$ \begin{array}{l} \theta\left(t_{0}\right)=\theta_0 \\ \theta\left(t_{f}\right)=\theta_f \\ \dot{\theta}\left(t_{0}\right)=\dot\theta_0\\ \theta\left(t_{f}\right)=\dot\theta_f\\ \dot{\theta}\left(t_{0}\right)=\ddot\theta_0\\ \dot{\theta}\left(t_{f}\right)=\ddot\theta_f \end{array} $$
系数为
$$ \begin{aligned} a_0&=\theta_0\\ a_1&=\dot{\theta}0\\ a_2&=\frac{\ddot{\theta}0}{2}\\ a_3&=\frac{20 \theta_f-20 \theta{0}-\left(8\dot{\theta}{f}+12\dot{\theta}{0}\right) t{f}-\left(3 \ddot{\theta}{0}-\ddot{\theta}{f}\right) t_{f}^{2}}{2 t_{f}^{3}}\\ a_4&=\frac{30 \theta_{0}-30 \theta_{f}+\left(14 \dot{\theta}{f}+16 \dot{\theta}{0}\right) t_f+\left(3 \ddot{\theta}{0}-2 \ddot{\theta}{f}\right) t_{f}^{2}}{2 t_{f}^{4}}\\ a_5&=\frac{12\theta_f-12 \theta_{0}-\left(6 \dot{\theta}{f}+6 \dot{\theta}{0}\right) t_f+\left(\ddot{\theta}{0}-\ddot{\theta}{f}\right) t_{f}^{2}}{2 t_{f}^{5}} \end{aligned} $$
运用五次多项式插补算法进行轨迹规划，在RTB工具箱中仿真得到的结果如下
在Simulink中仿真得到的结果如下
PUMA560各关节角运动情况为
不同于关节空间内的轨迹规划，笛卡尔空间内的轨迹规划将在笛卡尔空间内通过插补算法得到末端轨迹点序列，然后经过逆运动学解法得到。基于笛卡尔空间轨迹规划实现机械臂运动的开发路线为：
flowchart TD
A[机械臂末端在起始点和终止点的六维位姿]--末端笛卡尔空间内轨迹规划-->B[末端在整个过程中的六维位姿序列]--逆运动学求解-->C[各关节在整个过程中的角位移序列]--关节运动控制-->D[末端在笛卡尔空间中的实际运动]
笛卡尔空间中的曲线均可分割为多个直线和弧线的组合，空间直线和空间弧线的轨迹规划是笛卡尔空间轨迹规划中最重要的两部分内容。直线的插补最为简单，
$$ X_i=X_0+\frac{X_{f}-X_{0} }{N+1} \cdot i $$
其中，$X_0,X_f,N$分别为起始点坐标、终止点坐标、插补点数。
笛卡尔空间中的圆弧轨迹规划将先采用旋转变换的方法将世界坐标系X-Y面变换至圆弧所在平面，然后在该系中进行弧线轨迹规划，最后再反变换至世界坐标系。设起始点、中间点、终止点分别$P_1,P_2,P_3$，则具体步骤为：
构造坐标系$P_1-UVW$。其中，$P_1$为坐标原点，$\vec{P_1P_2}$为U轴方向，平面法向量为W轴方向，UVW三轴满足右手系。
$$ \begin{aligned} \hat U&=\frac{P_{2}-P_{1}}{|P_{2}-P_{1}|}\\ \hat W&=\frac{(P_{3}-P_{1}) \times \hat U}{|(P_{3}-P_{1}) \times \hat U|}\\ \hat V&=\hat W \times \hat U \end{aligned} $$
求解圆心坐标和半径在坐标系$O−XYZ$的值。在$P_1-UVW$系中$P_1,P_2,P_3$三点的坐标为$A(0,0),B(b_x,0),C(c_x,c_y)$。由数学关系知，圆心必定处在AB的垂直平分线上，故设圆心为$(\frac{b_x}{2},h)$，此时有：
构造圆心坐标系$P_0-RST$。$P_0$为坐标原点，$\vec{P_0P_1}$为R轴方向，$\vec{P_0P_1}\times \vec{P_0P_2}$为T轴方向，$T\times R$为S轴方向。此时三个轴向量为
坐标系$P_0-RST$进行圆弧轨迹规划
最后，将$X_{P}$变换至$O−XYZ$系，即
在末端笛卡尔空间内进行圆弧轨迹规划，得到的RTB仿真如下
得到的Simulink仿真如下
各关节角的实际运动为
本文首先对PUMA560机械臂的基本结构、参数及其运动学特性进行了分析并给出了正逆运动学的完整算法。其次，建立了PUMA560的三维模型，制作了机器人的URDF网格可视化文件，并在Simulink中开发了机械臂的可视化仿真框架。最后，分别基于关节空间和笛卡尔空间对机械臂进行了轨迹规划并在Simulink中实现了典型轨迹的可视化仿真。
本项目完成了机器人的全开发周期，包括：机器人三维模型→URDF通用文件→运动学&动力学模型→轨迹规划算法→运动仿真模型。
https://github.com/Z-G-Zhou/PUMA560-Kinematics-trajectory_planning-and-simulation
Email：[email protected]