本文转载自微信公众号ROBOTICS
复习
ROBOTICS的机器人学干货系列已经写到第八篇了，都讲了些什么呢？让我啰嗦地给大家整理一下：
· 入门：如果你看到R关节和P关节不知道我在说什么，请去看第一篇《从RP入门机器人学》
· 重要背景知识：如果我说end effector的位置w.r.t frame{0}、旋转矩阵、齐次坐标变换，你搞不清我在说什么，请看第二篇《位置角度旋转平移，“乱七八糟”的坐标变换》
· DH参数是啥？从关节空间的关节位置（关节位置又是啥！说不出来要面壁了）如何求出操作空间的end effector position/orientation？不记得的请看《机械臂的坐标系与数学模型：传说中的DH参数》《“正运动学”——从关节空间到操作空间》
· 从关节空间的关节速度怎么求出操作空间的end effector速度？雅可比矩阵是啥？三篇文章分别为《雅可比矩阵上篇》、《雅可比矩阵中篇》、《雅可比矩阵下篇》，非常重要，值得好好理解。
我们讨论过的问题真的只有这么多……
逆运动学（Inverse Kinematics）
有了前面的铺垫，理解逆运动学所研究的问题就变得非常简单了——我要求我的end effector运动到某一个位置朝向某一个方向（比如我要控制我的手去拿一杯水），此时我的手臂各个关节应该运动到哪个关节位置呢？这个问题显然是机器人学需要研究的一个根本问题——因为对于任意机械臂，你想控制的量在操作空间，而你能控制的量却在关节空间。想要控制end effector在操作空间完成特定操作，就一定避不开逆运动学这个问题。
逆运动学最基本的思路，是从正运动学反过来，比如说来看我们的史上出镜率最高机械臂：
它的逆运动学问题是：已知(xe, ye)，怎么求(θ1,θ2)？
很明显我们有两个未知数、两个方程，解解方程就行了。当然，这明显是一个非线性方程组，怎么解？我是把两个等式都两边平方然后相加，可以求出θ2，然后再求出θ1。
如果是一个六自由度的机械臂，我们用包含关节位置的Transformation Matrix（齐次坐标变换矩阵）来表示end effector的位置和朝向，这个矩阵包含了end effector position w.r.t {0}（一个3×1的向量），以及end effector的旋转矩阵w.r.t {0}（一个3×3的矩阵），一共12个未知数，这时候逆运动学又怎么求呢？
如果是一个七自由度的机械臂，我们说这时候机械臂存在冗余自由度，end effector固定不动的时候关节仍然可以运动，这时候逆运动学又怎么求呢？
由于逆运动学非线性、解不一定存在或不是唯一等特性，比起正运动学直截了当的写矩阵，求解逆运动学就要困难得多，也因此有很多不同的方法：
· 解析法（Analytical Solution），顾名思义是用代数或几何大法直接求解，如上面的例子所示。通常随着自由度上升，求得解析解的难度也越来越大，对于冗余机械臂还需要从几个解中选择合适的解。这种方法通常用在特定几何结构的机械臂，并且有特定的关节位置求解顺序。下面这个教学演示视频展示了一个六自由度Kuka KR 60机械臂的解析解求解方法。由于解析解法不具有通用性，我们后面不再详述。
· 优化法（Optimization-based Solution），是把问题转化为一个优化问题求数值解。用数学语言说，就是把求
的问题，转化为
“求关节位置q，使实际end effector位置x与正运动学算出的end effector位置f(q)之间的差最小”的问题：
如何求解上面这个式子（比如用梯度下降大法Gradient Descent）就是一个数学问题了，所以本文也不再详述。
· 迭代解 – 雅可比矩阵求逆法（Iterative Method – Jacobian Inverse），是把问题“微分化”，利用瞬时运动学的逆运算无限逼近
· 雅可比矩阵转置法（Jacobian Transpose），是从下面式子出发，用雅可比矩阵的转置代替困难的求逆运算
由于在逆运动学求解中，我们不关心系统的动力学特性，上面这个式子也可以写成
上面这两个方法相对比较通用（是否工业上常用？这个很难说。可能工业上对特定机械臂用解析解的也不少……），所以这篇文章主要还是给我们的老朋友雅可比矩阵解锁新功能，本文将着重介绍雅可比矩阵求逆法。
迭代解 – 雅可比矩阵求逆（Jacobian Inverse）
雅可比矩阵求逆的方法有什么优点呢？我们还是拿那个两自由度机械臂举例：
假设link的长度都是1，那我们可以求出它的解析解为（希望上面那个方程组你自己动手解了一下）：
现在我们需要end effector从图中(xe, ye)沿紫色直线移动到(x’e, y’e)，但你需要控制关节位置实现这一点。现在你可以有几种思路——
第一，只求起点和终点对应的关节位置，直接对这两处关节位置进行线性插值求得关节运动轨迹——这样虽然省了很多计算量，但end effector不大可能沿着直线走；
第二，把这条直线插入很多很多中间点，每一个点都求出对应的关节位置，然后控制每一个关节按着这一系列的关节位置走（就是我们前面说的解析/优化解法）；
第三，还是把这条直线插入很多很多中间点，但如果点与点的间隔足够小、运动时间足够短，我们就可以在每一点用雅可比矩阵求逆来求得当前关节位置的变化——换个角度讲，我们也可以设定end effector沿这条直线的运动速度，用雅可比矩阵求逆求得关节速度，直接控制关节的运动速度而不是位置。
我们把第三个方法画成控制框图：
图中xd的下标d表示“desired”，即你希望x到达的位置和朝向；以后还有下标c表示“current”，即当前x的位置/朝向；我们会经常看到这样的下标。
你可能不懂控制框图，没关系，首先上面一行Joint control（关节控制）之前，从左到右说的是：
Joint control则是你把你计算出来的qd送到每一个关节的控制器中（比如最简单的舵机）；这些控制器最终把各个关节控制到位置q（好的控制器，大部分情况q和qd应当非常接近）。
这个图的下边一行说的是，我们又从这个关节位置q用正运动学求出了此时的end effector位置x，反馈到前面再给上面第一个式子运算求dx。
通常你设置一个xd，控制器需要走几遍这个控制回路才能将dx降到接近0（让x逼近xd），因此这个方法也被称为迭代法。
冗余雅可比矩阵求逆
*前方高能预警：以下部分数学较难，属扩展内容
你一定想问，当雅可比矩阵不可求逆的时候怎么办呢？不可求逆的情况有很多种，我们这里先讨论一种：即当雅可比矩阵是矮胖型，机械臂具有冗余自由度的情况。
这个时候要用到一个数学概念，叫伪逆矩阵（pseudoinverse）。伪逆矩阵也有很多种，在机器人逆运动学中用得较多的是右伪逆矩阵（right-inverse）。如果把伪逆矩阵记为A+，left-inverse是说(A+)A = I；而right-inverse就是说A(A+) = I。
Right-inverse是通过求解下面这个问题得到的：
在机器人中，我们会希望每一次迭代的关节运动量尽可能小（从多种可能的运动方式中选出运动最少的一种），所以上面说的是求一个尽可能小的能满足方程的dq。
用拉格朗日乘数法（Lagrange Multiplier）,这个问题可以变成：
（乘1/2是为了后面求导后不要有个讨厌的2——不影响结果）
拉格朗日乘数法是一个求约束条件下极值的方法，非常有用，用起来也比较简单，而且我们以后还会用到——如果你不是很了解，我强烈建议你去自学一下。
要求极值，必先求导——拉格朗日乘数法是分别对dq和lamda求导，导数为0时可求得极值（我们就不论证这里是极小值了）：
解上面两个方程，即可求出
不难验证JJ+=I。用这个J+求解出的dq即为满足条件的最小关节运动速度。最后简单地提一下null space，我们可以验证：
这说明矩阵I-J+J可以把任意关节速度投影到“零空间”内，投影后的关节速度将不引起任何end effector的运动。利用这个特性，我们可以在满足了end effector的xd之后，再利用零空间实现其它任务（比如避开障碍物）。
