中正平和的机器人学笔记——2. 机械臂逆运动学（附MATAB代码）

0. 预备知识（别跳过呀）

上一篇我们讲了正运动学，也就是通过齐次变换矩阵，知晓关节角后求得机械臂末端的位姿，并表示在基座标系下。这一篇我们就来讲一讲逆运动学问题——已知工具坐标系{T}相对于固定坐标系{S}的期望位姿，如何计算一系列满足期望要求的关节角？

0.1 求解的难度

这是一个难度更大的问题，因为求解操作臂运动学方程是一个非线性问题。譬如六自由度的机械臂Puma560，回顾最后求得的运动学方程，

这里我们一共有12个方程，6个未知数。旋转部分3×3的矩阵由于每一列都是单位矢量，两两正交，因此这9个方程只有3个是独立的。再加上第四列前三行位置矢量的三个方程，6个方程对应6个未知数，应当是有解的。但是其中含有sin和cos，是非线性超越方程，求解难度很大，应到考虑到解是否存在，多解以及求解方法的问题。

0.2 解的存在性

这里引入三个概念，
工作空间（workspace）：操作臂末端的执行器所能达到的范围。
灵巧工作空间（dexterous workspace）：机器人末端执行器能从各个方向达到的空间。
可达工作空间（reachable workspace）：机器人至少从一个方向上可以道德的空间。

从上述概念可以分析得到，如果要求解存在，那么期望点一定在工作空间内才行。
我们以两连杆机械臂为例，如果l1 = l2 = r，且关节旋转角能达到360°，那么其可达工作空间为半径为2r的圆，灵巧工作空间为原点；如果l1 ≠ l2，那么可达工作空间为外径l1 + l2，内径为|l1 - l2|的圆环，不存在灵巧工作空间。
一般的，当机械臂自由度小于6时，其不能到达三维空间内一般的目标位姿。

0.3 多解问题

多解问题也很好理解，例如Puma560机械臂，对已确定目标可有8个不同解，但往往由于关节运动范围的限制，这8个解中的某些解不能实现。如下图所示：

0.4 解法

由于非线性方程组没有通用的求解算法，针对解法问题，最好对一直操作臂的解的构成形式加以定义。
我们把操作臂的全部求解方法分为两类：封闭解（closed solutions）和数值解（numerical solutions）。
本篇主要讲述封闭解形式，也可分为两类，代数法和几何法。
同时我们给出一个结论：所有包含转动关节和移动关节的串联型6自由度机械臂均是可解的。 当然一般是数值解，只有在特殊情况下才存在封闭解（解析解），而且这种情况往往具有一些特性（存在几个正交关节轴或者有多个α_i为0或者90°）。根据这一特性，机械臂设计者们在设计时也往往会考虑到这一点，尽可能将工业机械臂设计得足够简单，从而能够得到封闭解。

1. 代数解法

我们以三连杆平面机械臂为例，看的小伙伴可以一起拿出纸笔算一下，首先按照上一篇正运动学的方法求解出三连杆的运动学方程：

因为是平面机械臂，确定期望的目标位姿需要三个参数（x,y,Φ）

将上面两个矩阵联立，可得如下方程：

我们把第三个和第四个式子求平方再相加，利用三角函数公式可以求出cosθ₂


假定目标位置在工作空间内，那么sinθ₂的表达式为

应用双变量反正切公式¹ 就可以计算出θ₂

接下来我们求θ₁，我们将原来x和y的式子改写成如下形式
式中k₁和k₂为

这种变换在求解逆运动学时是很常用的形式，大家可以留意。
假设

且

则有

那么上面改写后的x和y的式子就可以写成如下形式

上面就是很明显的正余弦公式了，整理可得

那么我们再用双变量反正切公式就可以求出
求出θ₁和θ₂后，我们也可以由下式求出θ₃了。

2. 几何解法

以下图的平面机械臂为例

由余弦定理可得

整理后可以求出cosθ₂

应用双变量反正切公式就可以求出θ₂了。
接下来再由几何分析可以得到，


我们就可以求出θ₁

最后由如下公式求得θ₃

3.逆运动学实例——Puma560机械臂

写起来太麻烦啦，贴上自己计算手稿又太羞耻，字也不好看。csdn上有现成的求解步骤，大家去看吧！
如果嫌麻烦不想找就看这里我贴的书中的求解步骤吧！





基本步骤和代数法是差不多的，所以大家可以感受到，逆运动学的求解实在是太枯燥啦（呕）！

5. 逆运动学MATLAB的实现

mdl_puma560
%载入工具箱提供的example，建立好的puma560机械臂模型

qn = [0	0.7854	3.1416	0	0.7854	0]  %期望的关节角

T = transl(0.5, 0.5, 0.7) * rpy2tr(0, 3*pi/4, 0)
T = p560.fkine(qn)
%上面两句都是求出正运动学的位姿，也可以直接自己给出一个期望的位姿T

qi = p560.ikine(T)
qi = p560.ikine6s(T)
%这两句都是求解逆运动学，但是第二句适用于6自由度机械臂

%由于存在多解情况，我们可以人为的限定期望的位型解
qi = p560.ikine6s(T, 'ru')
%'l','r'   左手/右手
%'u','d'   肘部在上/肘部在下
%'f','n'    手腕翻转/手腕不翻转

p560.plot(qi)

这只是最简单的一些代码，还有很多函数的使用方式大家可以参考《Robotics, Vision and Control》这本书。

6. 总结

以上就是机械臂的逆运动学部分讲解，实际上如果你也跟着每一个例子计算以后会发现如果手算逆运动学会很麻烦（我都懒得敲公式直接贴图了能不麻烦吗Or2），但是得益于计算机的发展，得益于大佬们工具包做的好，我们可以看到逆运动学用matlab求解非常简单（这里希望大家能够试着不用工具箱自己写一下逆运动学的求解），但是我们不能满足于躺在前人的成就上洋洋自得，几行代码写起来简单，但是对于我们而言，为了真正学好机器人学，理解代码背后的原理才是最为重要的，各个学科都是如此。所以也希望大家不论是学什么，都不要眼高手低，拿出纸笔自己多写写画画，譬如BP神经网络的原理，直接看一坨公式自然不容易理解，那么画一个简单的网络结构，自己按照公式推导一下慢慢理解就好了。

---

1. http://blog.sina.com.cn/s/blog_131fa47b20102wftq.html ↩︎