PUMA560的运动学建模

最新推荐文章于 2024-04-22 22:18:13 发布

KK_Man_42

最新推荐文章于 2024-04-22 22:18:13 发布

阅读量4.5k

点赞数 13

分类专栏：机器人文章标签： matlab

本文链接：https://blog.csdn.net/KK_Man_42/article/details/128343754

版权

机器人专栏收录该内容

1 篇文章 0 订阅

订阅专栏

转载请注明出处

文章目录

引言
一、连杆坐标系
- 1.1 连杆和关节
- 1.2 建立连杆坐标系
二、运动学
三、MATLAB仿真
- 3.1 资源
- 3.2 进阶
参考资料

引言

PUMA的历史：

1962年，由乔治·德沃尔（George Devol）和约瑟夫·恩格伯格（Joseph Engelberger）共同创建的美国万能自动化（Unimation）公司的第一台机器人Unimate，在美国通用汽车（General Motors，GM）公司投入使用，标志着第一代机器人的诞生。

机器人Unimate
1969年，维克多·沙因曼（Victor Scheinman）发明了Standford Arm。

Standform Arm
1972年，Scheinman在MIT的人工智能实验室研发了MIT Arm，是PUMA的前身。

MIT Arm
1977年，Scheinman将他之前的设计卖给了Unimation，在GM的帮助下深入发展并设计出PUMA (Programmable Universal Machine for Assembly / Programmable Universal Manipulation Arm)，下图为1983年设计出的PUMA 500。

PUMA 500

一、连杆坐标系

1.1 连杆和关节

在这里插入图片描述

图1-1 PUMA560的3D模型

PUMA560的各个连杆：

连杆0（基座）：
- 连杆0虽然有一个具体的形状，但是在建立连杆坐标系时，由于坐标系{0}和坐标系{1}重合，所以其在运动学模型中长度设为0，用一个点或者坐标系进行表示。
连杆1：
- 关节1贯穿连杆1（上端）和连杆0（下端）
连杆2：
- 关节2贯穿连杆2（上端）和连杆1（下端）
连杆3：
- 关节3贯穿连杆3（上端）和连杆2（下端）
连杆4：
- 关节4贯穿连杆4（上端）和连杆3（下端）
连杆5：
- 关节5贯穿连杆5（上端）和连杆4（下端）
- 连杆5可以看成一个“擀面杖”
连杆6（末杆）：
- 关节6贯穿连杆6（上端）和连杆5（下端）
- 连杆6可以看成一个“印章”或“国际象棋”，底座用于固连末端执行器或工具。虽然此处连杆6有具体形状，但是在连杆坐标系中将其长度设为0（跟连杆0一样）。

在这里插入图片描述

图1-2 PUMA560的连杆

1.2 建立连杆坐标系

第一步：确定Z轴¹

找到六个转动关节，关节i和关节i+1之间为连杆i（i=1, 2, … , 5），连杆6用于连接末端执行器。根据关节轴确定Z轴，由于Z轴正方向有两个，所以选取不同的方向，会影响连杆坐标系的建立，也即D-H参数列表会不同。

在这里插入图片描述

图1-3 确定Z轴

第二步：找公垂线或交点

找到两相邻Z轴的公垂线或交点。 ${Z_2}$ 、 ${Z_3}$ 的公垂线有无数条，这也是导致D-H参数列表不同的一个原因。

在这里插入图片描述

图1-4 找公垂线或交点

第三步：确定坐标系{i}的原点

坐标系 $\{ i\} (i = 1,2, \cdots ,5)$ 的原点为：

${Z_i}$ 轴和 ${Z_{i+1}}$ 的交点
${Z_i}$ 轴和公垂线的交点

在这里插入图片描述

图1-5 确定坐标系{i}的原点

第四步：确定坐标系{i}

确定X轴的规则：

${X_i}$ 轴的正方向沿着公垂线由 ${Z_i}$ 轴指向 ${Z_{i+1}}$ 轴
当 ${X_i}$ 轴和 ${Z_{i+1}}$ 轴相交时， ${X_i}$ 轴的正方向决定了 ${\alpha _i}$ 的符号：
- 当从 ${Z_i}$ 轴转向 ${Z_{i+1}}$ 轴时，选取大拇指指向的方向为 ${X_i}$ 轴，则 ${\alpha _i}$ 的符号为正
- 反之则 ${\alpha _i}$ 的符号为负

确定了Z轴和X轴的正方向之后，根据右手定则，确定Y轴的正方向。

在这里插入图片描述

图1-6 确定坐标系{i}

第五步：确定坐标系{0}和{6}

选取坐标系{0}和{6}的原点，确定坐标系{0}和{6}。通常将坐标系{0}和坐标系{1}重合，坐标系{6}和坐标系{5}重合。

本文中坐标系{0}相对于坐标系{1}绕 ${Z_i}$ 轴正方向旋转了 ${\rm{ - }}{90^ \circ }$ ，将坐标系{6}和坐标系{5}重合。

在这里插入图片描述

图1-7 确定坐标系{0}和{6}

第六步：得到D-H参数列表

在已建立的坐标系上，添加机械臂的几何参数，得到连杆坐标系的完整图形，如下图所示。图中，0.6604m表示坐标系{0}的原点和底面之间的高度，也即基座的高度；0.05625m表示末端执行器的尖端点到坐标系{6}原点的距离，也即「工具坐标系（Tool Coordinate System，TCP）」的原点与坐标系{6}原点之间的距离²。

在这里插入图片描述

图1-8 PUMA560的连杆坐标系

最终得到M-DH参数列表，如表1-1所示：

表1-1 PUMA560的MD-H列表

在这里插入图片描述

二、运动学

2.1 正运动学

MD-H参数法对应的齐次变换矩阵，如下式：
$\begin{array}{l}{}_i^{i - 1}T = Rot({z_{i{\rm{ - }}1}},{\alpha _{i - 1}}) \cdot Trans({x_{i{\rm{ - }}1}},{a_{i{\rm{ - }}1}}) \cdot Rot({z_i},{\theta _i}) \cdot Trans({z_i},{d_i})\\{\rm{ = }}\left[ {\begin{array}{ccccccccccccccc}{\cos {\theta _i}}&{ - \sin {\theta _i}}&0&{{a_{i - 1}}}\\{\sin {\theta _i}\cos {\alpha _{i - 1}}}&{\cos {\theta _i}\cos {\alpha _{i - 1}}}&{ - \sin {\alpha _{i - 1}}}&{ - {d_i}\sin {\alpha _{i - 1}}}\\{\sin {\theta _i}\sin {\alpha _{i - 1}}}&{\cos {\theta _i}\sin {\alpha _{i - 1}}}&{\cos {\alpha _{i - 1}}}&{{d_i}\cos {\alpha _{i - 1}}}\\0&0&0&1\end{array}} \right]\end{array} \tag{2-1}$

将表1-1中的MD-H参数带入式（2-1），得到各相邻连杆之间的齐次变换矩阵如下：
$\begin{array}{l}{}_1^0T = \left[ {\begin{array}{ccccccccccccccc}{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}{ccccccccccccccc}{c{\theta _2}}&{ - s{\theta _2}}&0&0\\0&0&1&{{d_2}}\\{ - s{\theta _2}}&{ - c{\theta _2}}&0&0\\0&0&0&1\end{array}} \right]\\{}_3^2T = \left[ {\begin{array}{ccccccccccccccc}{c{\theta _3}}&{ - s{\theta _3}}&0&{{a_2}}\\{s{\theta _3}}&{c{\theta _3}}&0&0\\0&0&1&0\\0&0&0&1\end{array}} \right]\\{}_4^3T = \left[ {\begin{array}{ccccccccccccccc}{c{\theta _4}}&{ - s{\theta _4}}&0&{{a_3}}\\0&0&1&{{d_4}}\\{ - s{\theta _4}}&{ - c{\theta _4}}&0&0\\0&0&0&1\end{array}} \right]\\{}_5^4T = \left[ {\begin{array}{ccccccccccccccc}{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}{ccccccccccccccc}{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{array} \tag{2-2}$

通过依次右乘齐次变化矩阵，最终可以得到机械臂末端坐标系相对于基坐标系的齐次变化矩阵 ${}_6^0T$ ，如下式³：
${}_6^0T = {}_1^0T \cdot {}_2^1T \cdot {}_3^2T \cdot {}_4^3T \cdot {}_5^4T \cdot {}_6^5T = \left[ {\begin{array}{ccccccccccccccc}{{n_x}}&{{o_x}}&{{a_x}}&{{p_x}}\\{{n_y}}&{{o_y}}&{{a_y}}&{{p_y}}\\{{n_z}}&{{o_z}}&{{a_z}}&{{p_z}}\\0&0&0&1\end{array}} \right] \tag{2-3}$ 式中：
$\left\{ \begin{array}{l}{n_x} = {c_1}\left[ {{c_{23}}\left( {{c_4}{c_5}{c_6} - {s_4}{s_6}} \right) - {s_{23}}{s_5}{c_6}} \right] + {s_1}\left( {{s_4}{c_5}{c_6} + {c_4}{s_6}} \right)\\{n_y} = {s_1}\left[ {{c_{23}}\left( {{c_4}{c_5}{c_6} - {s_4}{s_6}} \right) - {s_{23}}{s_5}{c_6}} \right] - {c_1}\left( {{s_4}{c_5}{c_6} + {c_4}{s_6}} \right)\\{n_z} = - {s_{23}}\left( {{c_4}{c_5}{c_6} - {s_4}{s_6}} \right) - {c_{23}}{s_5}{c_6}\\\begin{array}{lllllllllllllll}{{o_x} = {c_1}\left[ {{c_{23}}\left( { - {c_4}{c_5}{s_6} - {s_4}{c_6}} \right) + {s_{23}}{s_5}{s_6}} \right] + {s_1}\left( {{c_4}{c_6} - {s_4}{c_5}{s_6}} \right)}\\{{o_y} = {s_1}\left[ {{c_{23}}\left( { - {c_4}{c_5}{s_6} - {s_4}{c_6}} \right) + {s_{23}}{s_5}{s_6}} \right] - {c_1}\left( {{c_4}{c_6} - {s_4}{c_5}{s_6}} \right)}\\{{o_z} = - {s_{23}}\left( { - {c_4}{c_5}{s_6} - {s_4}{c_6}} \right) + {c_{23}}{s_5}{s_6}}\end{array}\\{a_x} = - {c_1}\left( {{c_{23}}{c_4}{s_5} + {s_{23}}{c_5}} \right) - {s_1}{s_4}{s_5}\\{a_y} = - {s_1}\left( {{c_{23}}{c_4}{s_5} + {s_{23}}{c_5}} \right) + {c_1}{s_4}{s_5}\\{a_z} = {s_{23}}{c_4}{s_5} - {c_{23}}{c_5}\\{p_x} = {c_1}\left( {{a_2}{c_2} + {a_3}{c_{23}} - {d_4}{s_{23}}} \right) - {d_2}{s_1}\\{p_y} = {s_1}\left( {{a_2}{c_2} + {a_3}{c_{23}} - {d_4}{s_{23}}} \right) + {d_2}{c_1}\\{p_z} = - {a_3}{s_{23}} - {a_2}{s_2} - {d_4}{c_{23}}\end{array} \right.$ $\left\{ \begin{array}{l} {c_{23}} = {c_2}{c_3} - {s_2}{s_3}\\ {s_{23}} = {c_2}{s_3} + {s_2}{c_3} \end{array} \right.$

2.2 逆运动学

2.3.1 代数解

代数解的核心思想为：将式（2-3）依次左称 ${}_i^{i - 1}{T^{ - 1}}$ 使得等式右边不存在参数 ${\theta _i}$ ，从而将方程解耦，具体步骤如下³⁴：

第一步：求 ${\theta _1}$

将式（2-3）左乘 ${}_1^0{T^{ - 1}}$ ，则有下式：
$\begin{array}{l}{}_1^0{T^{ - 1}}({\theta _1}) \cdot {}_6^0T = {}_2^1T({\theta _2}) \cdot {}_3^2T({\theta _3}) \cdot {}_4^3T({\theta _4}) \cdot {}_5^4T({\theta _5}) \cdot {}_6^5T({\theta _6})\\\left[ {\begin{array}{ccccccccccccccc}{{c_1}}&{{s_1}}&0&0\\{ - {s_1}}&{{c_1}}&0&0\\0&0&1&0\\0&0&0&1\end{array}} \right]\left[ {\begin{array}{ccccccccccccccc}{{n_x}}&{{o_x}}&{{a_x}}&{{p_x}}\\{{n_y}}&{{o_y}}&{{a_y}}&{{p_y}}\\{{n_z}}&{{o_z}}&{{a_z}}&{{p_z}}\\0&0&0&1\end{array}} \right] = {}_6^1T(Left)\\{}_6^1T(Right) = \left[ {\begin{array}{ccccccccccccccc}{^1{n_x}}&{^1{o_x}}&{^1{a_x}}&{^1{p_x}}\\{^1{n_y}}&{^1{o_y}}&{^1{a_y}}&{^1{p_y}}\\{^1{n_z}}&{^1{o_x}}&{^1{a_z}}&{^1{p_z}}\\0&0&0&1\end{array}} \right]\end{array} \tag{2-4}$ 式中：
$\left\{ \begin{array}{l} ^1{n_x} = {c_{23}}\left( {{c_4}{c_5}{c_5} - {s_4}{s_6}} \right) - {s_{23}}{s_5}{c_6}\\ ^1{n_y} = - {s_4}{c_5}{c_6} - {c_4}{s_6}\\ ^1{n_z} = - {s_{23}}\left( {{c_4}{c_5}{c_6} - {s_4}{s_6}} \right) - {c_{23}}{s_5}{c_6}\\ ^1{o_x} = - {c_{23}}\left( {{c_4}{c_5}{s_6} + {s_4}{c_6}} \right) + {s_{23}}{s_5}{s_6}\\ ^1{o_y} = {s_4}{c_5}{s_6} - {c_4}{c_6}\\ ^1{o_z} = {s_{23}}\left( {{c_4}{c_5}{s_6} + {s_4}{c_6}} \right) + {c_{23}}{s_5}{s_6}\\ ^1{a_x} = - {c_{23}}{c_4}{s_5} - {s_{23}}{c_5}\\ ^1{a_y} = {s_4}{s_5}\\ ^1{a_z} = {s_{23}}{c_4}{s_5} - {c_{23}}{c_5}\\ ^1{p_x} = {a_2}{c_2} + {a_3}{c_{23}} - {d_4}{s_{23}}\\ {}^1{p_y} = {d_2}\\ ^1{p_z} = - {a_2}{s_{23}} - {a_2}{s_2} - {d_4}{c_{23}} \end{array} \right.$

式（2-4）左边只含有参数 ${\theta _1}$ 所以需要从等式右边找到一个只含有 ${\theta _1}$ 或者为常数的元素，可以找到该元素位于第二行第四列。令等式左边L(2, 4)与右边R(2,4)的对应元素相等，利用三角代换，则有：
$\left\{ {\begin{array}{lllllllllllllll}{L(2,4) = R(2,4)}\\{L(2,4) = - s{\theta _1} \cdot {p_x} + c{\theta _1} \cdot {p_y}}\\{R(2,4) = {d_2}}\end{array}} \right. \tag{2-5}$ 式中： ${p_x} = \rho \cos \phi$ ； ${p_y} = \rho \sin \phi$ ；且有 $\rho = \sqrt {p_x^2 + p_y^2}$ ， $\phi = a\tan 2\left( {{p_y},{p_x}} \right)$ 。

于是有：
$\left\{ \begin{array}{l} \sin \left( {\phi - {\theta _1}} \right) = {d_2}/\rho \\ \cos \left( {\phi - {\theta _1}} \right) = \pm \sqrt {1 - {{\left( {{d_2}/\rho } \right)}^2}} \\ \phi - {\theta _1} = {\mathop{\rm atan}\nolimits} 2\left[ {{d_2}/\rho , \pm \sqrt {1 - {{\left( {{d_2}/\rho } \right)}^2}} } \right] \end{array} \right. \tag{2-6}$ 可以求得 ${\theta _1}$ 的表达式如下：
${\theta _1} = {\mathop{\rm atan}\nolimits} 2\left( {{p_y},{p_x}} \right) - {\mathop{\rm atan}\nolimits} 2\left( {{d_2}, \pm \sqrt {p_x^2 + p_y^2 - d_2^2} } \right) \tag{2-7}$ 式中：正、负号对应 ${\theta _1}$ 的两个可能的解。

第二步：求 ${\theta _3}$

当选定 ${\theta _1}$ 的一个解之后，继续求解 ${\theta _2}$ 或 ${\theta _3}$ 。令式（2-4）两端的元素L(1, 4)和R(3, 4)分别对应相等，并结合式（2-5），则有：
$\left\{ \begin{array}{l} {c_1}{p_x} + {s_1}{p_y} = {a_2}{c_2} + {a_3}{c_{23}} - {d_4}{s_{23}}\\ {p_x} = - {a_2}{s_{23}} - {a_2}{s_2} - {d_4}{c_{23}}\\ {d_2} = - s{\theta _1}{p_x} + c{\theta _1}{p_y} \end{array} \right. \tag{2-8}$

将上式中的三个等式的左右两边分别进行平方后，再相加求和，得到：
${a_3}{c_3} - {d_4}{s_3} = k \tag{2-9}$ 式中： $\frac{{p_x^2 + p_y^2 + p_z^2 - a_2^2 - a_3^2 - d_2^2 - d_4^2}}{{2{a_2}}}$ 。

通过三角代换，求得：
${\theta _3} = {\mathop{\rm atan}\nolimits} 2\left( {{a_3},{d_4}} \right) - {\mathop{\rm atan}\nolimits} 2\left( {k, \pm \sqrt {a_3^2 + d_4^2 - {k^2}} } \right) \tag{2-10}$ 式中：正、负号对应 ${\theta _3}$ 的两种可能的解。

第三步：求 ${\theta _2}$

已经求出 ${\theta _1}$ 和 ${\theta _3}$ 之后，可以通过将式（2-4）左乘一个包含 ${\theta _1}$ 、 ${\theta _2}$ 和 ${\theta _3}$ 的逆变换 ${}_3^0{T^{ - 1}}({\theta _1},{\theta _2},{\theta _3})$ ，进而求解出 ${\theta _2}$ 。
$\begin{array}{l}{}_3^0{T^{ - 1}}({\theta _1},{\theta _2},{\theta _3}) \cdot {}_6^0T = {}_4^3T({\theta _4}) \cdot {}_5^4T({\theta _5}) \cdot {}_6^5T({\theta _6})\\\left[ {\begin{array}{ccccccccccccccc}{{c_1}{c_{23}}}&{{s_1}{c_{23}}}&{ - {s_{23}}}&{ - {a_2}{c_3}}\\{ - {c_1}{s_{23}}}&{ - {s_1}{s_{23}}}&{ - {c_{23}}}&{{a_2}{s_3}}\\{ - {s_1}}&{{c_1}}&0&{ - {d_2}}\\0&0&0&1\end{array}} \right]\left[ {\begin{array}{ccccccccccccccc}{{n_x}}&{{o_x}}&{{a_x}}&{{p_x}}\\{{n_y}}&{{o_y}}&{{a_y}}&{{p_y}}\\{{n_z}}&{{u_x}}&{{a_z}}&{{p_z}}\\0&0&0&1\end{array}} \right] = {}_6^3T(Left)\\{}_6^3T(Right) = \left[ {\begin{array}{ccccccccccccccc}{{c_4}{c_5}{c_6} - {s_4}{s_6}}&{ - {c_4}{c_5}{c_6} - {s_4}{c_6}}&{ - {c_4}{s_5}}&{{a_3}}\\{{s_5}{s_5}}&{ - {s_5}{s_6}}&{{c_5}}&{{d_4}}\\{ - {s_4}{s_5}{s_6} - {c_4}{s_6}}&{{s_4}{c_5}{s_6} - {c_4}{c_6}}&{{s_4}{s_5}}&0\\0&0&0&1\end{array}} \right]\end{array} \tag{2-11}$

从式（2-11）右边的矩阵可以看出只有R(1, 4)和R(2, 4)为常数，所以令左右两边(1, 4)和(2, 4)处的元素分别对应相等，得到下式：
$\left\{ \begin{array}{l}{c_1}{c_{23}}{p_x} + {s_1}{c_{23}}{p_y} - {s_{23}}{p_z} - {a_2}{c_3} = {a_3}\\ - {c_1}{s_{23}}{p_x} - {s_1}{s_{23}}{p_y} - {c_{23}}{p_z} + {a_2}{s_3} = {d_4}\end{array} \right. \tag{2-12}$

联立解得：
$\left\{ {\begin{array}{lllllllllllllll}{{s_{23}} = \frac{{\left( { - {a_3} - {a_2}{c_3}} \right){p_z} + \left( {{c_1}{p_x} + {s_1}{p_y}} \right)\left( {{a_2}{s_3} - {d_4}} \right)}}{{p_z^2 + {{\left( {{c_1}{p_x} + {s_1}{p_y}} \right)}^2}}}}\\{{c_{23}} = \frac{{\left( { - {d_4} + {a_2}{s_3}} \right){p_z} + \left( {{c_1}{p_x} + {s_1}{p_y}} \right)\left( {{a_2}{c_3} + {a_3}} \right)}}{{p_z^2 + {{\left( {{c_1}{p_x} + {s_1}{p_y}} \right)}^2}}}}\end{array}} \right. \tag{2-13}$

${s_{23}}$ 和 ${c_{23}}$ 表达式的分母相等，且为正，于是有：
${\theta _{23}} = {\theta _2} + {\theta _3} = {\mathop{\rm atan}\nolimits} 2\left[ \begin{array}{l} - \left( {{a_3} + {a_2}{c_3}} \right){p_z} + \left( {{c_1}{p_x} + {s_1}{p_y}} \right)\left( {{a_2}{s_3} - {d_4}} \right),\\\left( { - {d_4} + {a_2}{s_3}} \right){p_z} + \left( {{c_1}{p_x} + {s_1}{p_y}} \right)\left( {{a_2}{c_3} + {a_3}} \right)\end{array} \right] \tag{2-14}$

根据 ${\theta _1}$ 和 ${\theta _3}$ 解的四种可能组合，由式（2-14）可以得到对应的四种可能值 ${\theta _{23}}$ ，于是可以得到 ${\theta _2}$ 的四种可能的解：
${\theta _2} = {\theta _{23}} - {\theta _3} \tag{2-15}$ 式中： ${\theta _2}$ 取与 ${\theta _3}$ 相对应的值。

第四步：求 ${\theta_4}$

此时，式（2-11）左边均为已知，通过观察矩阵元素，可以通过(1, 3)和(3, 3)处两个元素的比值求出 ${\theta_4}$ ，所以令等式两边的元素(1, 3)和(3, 3)分别对应相等，则有：
$\left\{ {\begin{array}{lllllllllllllll}{{a_x}{c_1}{c_{23}} + {a_y}{s_1}{c_{23}} - {a_z}{s_{23}} = - {c_4}{s_5}}\\{ - {a_x}{s_1} + {a_y}{c_1} = {s_4}{s_5}}\end{array}} \right. \tag{2-16}$

分两种情况：

当 ${s_5} \ne 0$ 时，可以求出 ${\theta_4}$ 的表达式：
${\theta _4} = {\mathop{\rm atan}\nolimits} 2\left( { - {a_x}{s_1} + {a_y}{c_1}, - {a_x}{c_1}{c_{23}} - {a_y}{s_1}{c_{23}} + {a_z}{s_{23}}} \right) \tag{2-17}$
当 ${s_5} = 0$ 时，机械臂处于奇异位形。此时，关节轴4和6重合，只能求出 ${\theta_4}$ 与 ${\theta_6}$ 的和或差。处于奇异位形时，可以任意选取 ${\theta_4}$ 的值，再计算对应的 ${\theta_6}$ 。

第五步：求 ${\theta_5}$

根据求出的 ${\theta_4}$ ，可以进一步求出 ${\theta_5}$ ，将方程（2-3）两边左乘逆变换 ${}_4^0{T^{ - 1}}({\theta _1},{\theta _2},{\theta _3},{\theta _4})$ ，则有：
$\begin{array}{l}{}_4^0{T^{ - 1}}({\theta _1},{\theta _2},{\theta _3},{\theta _4}) \cdot {}_6^0T = {}_5^4T({\theta _5}) \cdot {}_6^5T({\theta _6})\\\left[ {\begin{array}{ccccccccccccccc}{{c_1}{c_{23}}{c_4} + {s_1}{s_4}}&{{s_1}{c_{23}}{c_4} - {c_1}{s_4}}&{ - {s_{23}}{c_4}}&{ - {a_2}{c_3}{c_4} + {d_2}{s_4} - {a_3}{c_4}}\\{ - {c_1}{c_{23}}{s_4} + {s_1}{c_4}}&{ - {s_1}{c_{23}}{s_4} - {c_1}{c_4}}&{{s_{23}}{s_4}}&{{a_2}{c_3}{s_4} + {d_2}{c_4} + {a_3}{s_4}}\\{ - {c_1}{s_{23}}}&{ - {s_1}{s_{23}}}&{ - {c_{23}}}&{{a_2}{s_3} - {d_4}}\\0&0&0&1\end{array}} \right]\left[ {\begin{array}{ccccccccccccccc}{{n_x}}&{{o_x}}&{{a_x}}&{{p_x}}\\{{n_y}}&{{o_y}}&{{a_y}}&{{p_y}}\\{{n_z}}&{{o_z}}&{{a_z}}&{{p_z}}\\0&0&0&1\end{array}} \right] = {}_6^4T(Left)\\{}_6^4T(Right) = \left[ {\begin{array}{ccccccccccccccc}{{c_5}{c_6}}&{ - {c_5}{s_6}}&{ - {s_5}}&0\\{{s_6}}&{{c_6}}&0&0\\{{s_5}{s_6}}&{ - {s_5}{c_6}}&{{c_5}}&0\\0&0&0&1\end{array}} \right]\end{array} \tag{2-18}$

式（2-18）左边的 ${\theta _1}$ 、 ${\theta _2}$ 、 ${\theta _3}$ 和 ${\theta _4}$ 都已经求出，可以观察到矩阵中仅仅包含 ${\theta_5}$ 的元素为L(1, 3)和L(3, 3)，令矩阵两边(1, 3)和(3, 3)处的元素分别对应相等，则有：
$\left\{ \begin{array}{l} {a_x}\left( {{c_1}{c_{23}}{c_4} + {s_1}{s_4}} \right) + {a_y}\left( {{s_1}{c_{23}}{c_4} - {c_1}{s_4}} \right) + {a_z}\left( { - {s_{23}}{c_4}} \right) = - {s_5}\\ {a_x}\left( { - {c_1}{s_{23}}} \right) + {a_y}\left( { - {s_1}{s_{23}}} \right) + {a_z}\left( { - {c_{23}}} \right) = {c_5} \end{array} \right. \tag{2-19}$

于是得到 ${\theta_5}$ 的表达式：
${\theta _5} = {\mathop{\rm atan}\nolimits} 2\left( {{s_5},{c_5}} \right) \tag{2-20}$

第六步：求 ${\theta_6}$

将方程（2-3）两边左乘逆变换 ${}_5^0{T^{ - 1}}({\theta _1},{\theta _2},{\theta _3},{\theta _4},{\theta _5})$ ，则有：
${}_5^0{T^{ - 1}}({\theta _1},{\theta _2},{\theta _3},{\theta _4},{\theta _5}) \cdot {}_6^0T = {}_6^5T({\theta _6}) \tag{2-21}$

令式（2-21）中(1, 1)和(3, 1)处的等式两边的元素对应相等，可得：
$\left\{ \begin{array}{l} - {n_x}\left( {{c_1}{c_{23}}{s_4} - {s_1}{c_4}} \right) - {n_y}\left( {{s_1}{c_{23}}{s_4} + {c_1}{c_4}} \right) + {n_z}\left( {{s_{23}}{s_4}} \right) = {s_6}\\{n_x}\left[ {\left( {{c_1}{c_{23}}{c_4} + {s_1}{s_4}} \right){c_5} - {c_1}{s_{23}}{s_5}} \right] + {n_y}\left[ {\left( {{s_1}{c_{23}}{c_4} - {c_1}{s_4}} \right){c_5} - {s_1}{s_{23}}{s_5}} \right]\\ - {n_z}\left( {{s_{23}}{c_4}{c_5} + {c_{23}}{s_5}} \right) = {c_6}\end{array} \right. \tag{2-22}$

进而可以求出 ${\theta_6}$ 的表达式：
${\theta _6} = {\mathop{\rm atan}\nolimits} 2\left( {{s_6},{c_6}} \right) \tag{2-23}$

2.3.2 多解性

机械臂末端的一组特定位姿，对应的逆解有8组。

由于三角函数产生的正负号导致 ${\theta _1}$ 和 ${\theta _3}$ 分别都有两个符号不同的解，因此两两组合就可以得到四组不同的解，其产生的解的组合如表2-1所示，对应的四种构型如图2-1所示。

表2-1 PUMA560的四组逆解

在这里插入图片描述

图2-1 四组逆解对应的机械臂构型

又由于PUMA560的腕部可以转动，即通过以下操作可以得到相同的末端位姿：
$\left\{ \begin{array}{l} {{\theta '}_4} = {\theta _4} + \pi \\ {{\theta '}_5} = - {\theta _5}\\ {{\theta '}_6} = {\theta _6} + \pi \end{array} \right. \tag{2-24}$

通过上述操作，可以得到另外不同的四组解，如表2-2所示。

表2-2 腕部可以转动导致的四组逆解

在这里插入图片描述

三、MATLAB仿真

3.1 资源

Github：

https://github.com/KK-Man-42/Kinematic-modeling-of-PUMA560

百度网盘：

链接：https://pan.baidu.com/s/1F7PWrCJjyp7dlQOk6a1qtQ
提取码：0xgf

3.2 进阶

一知半解|MATLAB机器人建模与仿真控制

参考资料

Puma560 机器人DH变换 ↩︎
臧庆凯,李春贵,闫向磊.基于MATLAB的PUMA560机器人运动仿真研究[J].广西科学院学报,2010,26(04):397-400.DOI:10.13657/j.cnki.gxkxyxb.2010.04.041. ↩︎
蔡自兴, 谢斌. 机器人学[M]. 清华大学出版社, 2000. ↩︎ ↩︎
陈晗,李林升.基于MATLAB的PUMA560机器人正逆解研究[J].制造业自动化,2018,40(12):34-36. ↩︎