五自由度机械臂的建模 (3) —— 逆运动学求解
逆运动学的解法
- 封闭解法：计算速度快、可以获得所有解，但是受机械臂结构限制较大
- 代数法
- 几何法
- 数值解法：通用，但计算速度慢
- 消元法：由机器人的非线性运动方程出发构造相关的多项式方程，通过计算该多项式方程求解
- 延拓法：通过跟踪解的路径，从一个逆运动学解已知的初始位姿出发计算目标位姿的解
- 迭代计算法：从一个给定的初始值出发。迭代计算逆运动学的精确解
解的存在性
- 解的存在性完全取决于机械臂的工作空间
- 工作空间：
- 灵活工作空间：机械臂末端能够以任意位姿到达的空间点集
- 可达工作空间：机械臂末端至少能够以一种位姿到达的空间点集
- 如果末端位姿在可达工作空间中，则逆运动学的解是存在的，否则不存在
- 解的个数取决于机械臂的关节数量，它也是连杆参数和关节运动范围的函数。通常，连杆的非零参数越多，则到达某一位姿的方式也越多，即逆运动学的解就越多
- 低于 6 自由度的机械臂在三维空间中不能到达全部位姿
- 通常选取最短行程解作为多解情况下的最优解：根据 “多移动小连杆，少移动大连杆” 的原则设定加权系数
Pieper 解法
Pieper 准则：机械臂三个相邻关节轴交于一点或互相平行。
符合 Pieper 准则则逆运动学可解。以之前建好运动学模型的 5 自由度机械臂为例，其逆运动学求解过程如下：
通过分离 \(\theta_{1}\) , \(\theta_{2}\) , \(\theta_{3}\) ，求解移动部分（关节 1、2 和 3）的转动⻆度
首先，回顾一下之前确定好的机械臂各个连杆坐标系的关系，如下图所示。
给定坐标系 \(\{4\}\) 的原点在基坐标系中的位姿 \(^{0}P_{4\ ORG} = \begin{bmatrix} x & y & z & 1 \end{bmatrix}^{T}\)，则有
\[ ^{0}P_{4\ ORG} = \begin{bmatrix} x \\ y \\ z \\ 1 \end{bmatrix} = {_{1}^{0}T} {_{2}^{1}T} {_{3}^{2}T} \cdot {^{3}P_{4\ ORG}} \tag{1} \]
其中，\(^{3}P_{4\ ORG}\) 为坐标系 \(\{4\}\) 的原点在坐标系 \(\{3\}\) 中的位姿，可用 \(_{4}^{3}T\) 的第 4 列表示，即
\[ ^{3}P_{4\ ORG} = \begin{bmatrix} a_{3} \\ -d_{4} \sin{\alpha_{3}} \\ d_{4} \cos{\alpha_{3}} \\ 1 \\ \end{bmatrix} \]
令 \({_{3}^{2}T} ^{3}P_{4\ ORG} = \begin{bmatrix} f_{1}(\theta_{3}) & f_{2}(\theta_{3}) & f_{3}(\theta_{3}) & 1 \end{bmatrix}^{T}\)，则
\[ \begin{aligned} f_{1}(\theta_{3}) & = a_{2} + a_{3} c_{3} \\ f_{2}(\theta_{3}) & = a_{3} s_{3} \\ f_{3}(\theta_{3}) & = 0 \end{aligned} \tag{2} \]
再令
\[ {_{2}^{1}T} \begin{bmatrix} f_{1}(\theta_{3}) \\ f_{2}(\theta_{3}) \\ f_{3}(\theta_{3}) \\ 1 \end{bmatrix} = \begin{bmatrix} g_{1}(\theta_{2}, \theta_{3}) \\ g_{2}(\theta_{2}, \theta_{3}) \\ g_{3}(\theta_{2}, \theta_{3}) \\ 1 \end{bmatrix} \]
将式 \((1)\) 代入，可得
\[ \begin{bmatrix} x \\ y \\ z \\ 1 \end{bmatrix} = {_{1}^{0}T} \begin{bmatrix} g_{1}(\theta_{2}, \theta_{3}) \\ g_{2}(\theta_{2}, \theta_{3}) \\ g_{3}(\theta_{2}, \theta_{3}) \\ 1 \end{bmatrix} = \begin{bmatrix} g_{1} c_{1} - g_{2} s_{1} \\ g_{1} s_{1} + g_{2} c_{1} \\ g_{3} \\ 1 \end{bmatrix} \tag{3} \]
其中，
\[ \begin{aligned} g_{1}(\theta_{2}, \theta_{3}) & = c_{2} f_{1} - s_{2} f_{2} + a_{1} \\ & = a_{1} + a_{2} c_{2} + a_{3} (c_{2} c_{3} - s_{2} s_{3}) \\ g_{2}(\theta_{2}, \theta_{3}) & = s_{2} f_{1} \cos{\alpha_{1}} + c_{2} f_{2} \cos{\alpha_{1}} - f_{3} \sin{\alpha_{1}} - d_{2} \sin{\alpha_{1}} \\ & = 0 \\ g_{3}(\theta_{2}, \theta_{3}) & = s_{2} f_{1} \sin{\alpha_{1}} + c_{2} f_{2} \sin{\alpha_{1}} + f_{3} \cos{\alpha_{1}} + d_{2} \cos{\alpha_{1}} \\ & = -a_{2} s_{2} - a_{3} (c_{2} s_{3} + c_{3} s_{2}) \end{aligned} \tag{4} \]
再令 \(r(\theta_{1}, \theta_{2}) = x^{2} + y^{2} + z^{2} = g_{1}^{2} + g_{2}^{2} + g_{3}^{2} = 2 a_{1} (k_{1} c_{2} + k_{2} s_{2}) + k_{3}\)，其中
\[ \begin{aligned} k_{1} (\theta_{3}) & = f_{1} \\ & = a_{2} + a_{3} c_{3} \\ k_{2} (\theta_{3}) & = -f_{2} \\ & = -a_{3} s_{3} \\ k_{3} (\theta_{3}) & = f_{1}^{2} + f_{2}^{2} + f_{3}^{2} + a_{1}^{2} + d_{2}^{2} + 2 d_{2} f_{3} \\ & = a_{1}^{2} + a_{2}^{2} + a_{3}^{2} + 2 a_{2} a_{3} c_{3} \end{aligned} \tag{5} \]
此外，由 \((4)\) 可得 \(z = g_{3} = (k_{1} s_{2} - k_{2} c_{2}) \sin{\alpha_{1}} + k_{4}\)，其中，\(k_{4} = f_{3} \cos{\alpha_{1}} + d_{2} \cos{\alpha_{1}} = 0\)。
综上所述，可得方程组
\[ \begin{cases} \begin{aligned} r & = 2 a_{1} (k_{1} c_{2} + k_{2} s_{2}) + k_{3} \\ z & = (k_{1} s_{2} - k_{2} c_{2}) \sin{\alpha_{1}} + k_{4} \end{aligned} \tag{6} \end{cases} \]
\(a_{1}\) 和 \(\sin{\alpha_{1}}\) 均不为零，消去 \(\theta_{2}\) 项可得
\[ \frac{(r - k_{3})^{2}}{4 a_{1}^{2}} + \frac{(z - k_{4})^{2}}{\sin^{2} \alpha_{1}} = k_{1}^{2} + k_{2}^{2} \tag{7} \]
上式只含有未知数 \(\theta_{3}\)，可利用万能公式 \(u = \tan{\frac{\theta_{3}}{2}}\) 求解。
将 \(k_{1}\)、\(k_{2}\) 和 \(k_{3}\) 代入式 \((7)\)，可得
\[ \begin{aligned} & \frac{((a_{2} + a_{3} c_{3})^{2} + a_{1}^{2} - x^{2} - y^{2} - z^{2} + a_{3}^{2} s_{3}^{2})^{2}}{(4 a_{1}^{2})} + z^{2} \\ & = (a_{2} + a_{3} c_{3})^{2} + a_{3}^{2} s_{3}^{2} \end{aligned} \tag{8} \]
代入万能公式
\[ \begin{aligned} \sin{\theta_{3}} & = \frac{2 u}{1 + u^{2}} \\ \cos{\theta_{3}} & = \frac{1 - u^{2}}{1 + u^{2}} \end{aligned} \]
解得 \(\theta_{3} = 2 \tan^{-1}{u}\)，共有 4 个解。
再代入 \(r = 2 a_{1} (k_{1} c_{2} + k_{2} s_{2}) + k_{3}\) 可解得 \(\theta_{2}\)，有 2 个解；最后代入 \(x = g_{1} c_{1} - g_{2} s_{1}\) 可解得 \(\theta_{1}\)，也有 2 个解。
综上，可解出关节 1、2 和 3 的角度，共有 \(4 \times 2 \times 2 = 16\) 组解。
求解 \(\theta_{4}\) 和 \(\theta_{5}\)
现在已知关节 1、2、3 的角度 \(\theta_{1}\)、\(\theta_{2}\) 和 \(\theta_{3}\)，上一篇求得了机械臂的正运动学方程
\[ _{5}^{0}T = \begin{bmatrix} 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{bmatrix} \tag{9} \]
将上式等号两边同时左乘 \(_{3}^{0}T^{-1}\) 可得
\[ {_{3}^{0}T^{-1}} \cdot {_{5}^{0}T} = {_{4}^{3}T} {_{5}^{4}T} = \begin{bmatrix} c_{4} c_{5} & -c_{4} s_{5} & -s_{4} & a3 \\ c_{5} s_{4} & -s_{4} s_{5} & c_{4} & 0 \\ -s_{5} & -c_{5} & 0 & 0 \\ 0 & 0 & 0 & 1 \end{bmatrix} \tag{10} \]
等式左右两边元素 \((1, 3)\) 和 \((2, 3)\) 对应相等，可解得 \(\theta_{4} = \tan^{-1}{\frac{s_{4}}{c_{4}}}\)。
同理，用 \(_{4}^{0}T^{-1}\) 左乘等式 \((9)\) 的两边，可得
\[ {_{4}^{0}T^{-1}} \cdot {_{5}^{0}T} = {_{5}^{4}T} = \begin{bmatrix} c_{5} & -s_{5} & 0 & 0 \\ 0 & 0 & 1 & 0 \\ -s_{5} & -c_{5} & 0 & 0 \\ 0 & 0 & 0 & 1 \end{bmatrix} \tag{11} \]
等式左右两边元素 \((1, 1)\) 和 \((1, 2)\) 对应相等，可解得 \(\theta_{5} = \tan^{-1}{\frac{s_{5}}{c_{5}}}\)。
世界坐标系以及末端夹子坐标系的变换
通过前面两个步骤，已经可以通过给定坐标系 \(\{5\}\) 在机械臂基坐标系 \(\{0\}\) 中的位姿来计算出机械臂各个关节的角度。
但是，再回顾机械臂各连杆坐标系的建立过程，我们会发现，为了简化问题、方便计算，我们通常让机械臂的基坐标系 \(\{0\}\) 和坐标系 \(\{1\}\) 重合，而让机械臂末端坐标系 \(\{5\}\) 的原点与坐标系 \(\{4\}\) 的原点重合。而实际情况下，机械臂往往放置在一个具有一定高度的底座上，且末端带有一个具有一定长度的工具来完成夹持等的功能，因此，机械臂基坐标系与世界坐标系、末端执行器坐标系与机械臂末端坐标系之前还存在相应的变换关系。
我们将机械臂所在空间的世界坐标系 \(\{W\}\) 放置在机械臂底座的底部，将末端执行器的坐标系 \(\{T\}\) 放置在机械臂末端夹子的末端，如下图所示。
则可以得到这两个坐标系的齐次变换矩阵
\[ _{0}^{W}T = \begin{bmatrix} 1 & 0 & 0 & 0 \\ 0 & 1 & 0 & 0 \\ 0 & 0 & 1 & 72 \\ 0 & 0 & 0 & 1 \end{bmatrix} \ \ \ \ _{T}^{5}T = \begin{bmatrix} 0 & 0 & 1 & 0 \\ 0 & -1 & 0 & 0 \\ 1 & 0 & 0 & 120 \\ 0 & 0 & 0 & 1 \end{bmatrix} \]
通常，机械臂通过外部传感器可以获取目标物体在世界坐标系中的位姿 \(_{G}^{W}T\)，则可以得到如下关系
\[ _{G}^{W}T = {_{T}^{W}T} = {_{0}^{W}T} \cdot {_{5}^{0}T} \cdot {_{T}^{5}T} \tag{12} \]
通过分别左乘 \(_{0}^{W}T^{-1}\) 和右乘 \(_{T}^{5}T^{-1}\)，就可以得到机械臂坐标系 \(\{5\}\) 的原点在机械臂基坐标系 \(\{0\}\) 中的位姿 \(_{5}^{0}T = {_{0}^{W}T^{-1}} \cdot {_{G}^{W}T} \cdot {_{T}^{5}T^{-1}}\)，再代入前两步即可求解机械臂的各关节的角度。