干货 | “逆运动学”——从操作空间到关节空间(上篇)

2272
0
2020年4月8日 10时51分

本文转载自微信公众号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),怎么求12)

 

很明显我们有两个未知数、两个方程,解解方程就行了。当然,这明显是一个非线性方程组,怎么解?我是把两个等式都两边平方然后相加,可以求出θ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),是把问题转化为一个优化问题求数值解。用数学语言说,就是把求

干货 | “逆运动学”——从操作空间到关节空间(上篇)插图(1)

 

的问题,转化为

 

“求关节位置q,使实际end effector位置x与正运动学算出的end effector位置f(q)之间的差最小”的问题:

干货 | “逆运动学”——从操作空间到关节空间(上篇)插图(2)

 

如何求解上面这个式子(比如用梯度下降大法Gradient Descent)就是一个数学问题了,所以本文也不再详述。

 

·   迭代解 – 雅可比矩阵求逆法(Iterative Method – Jacobian Inverse),是把问题“微分化”,利用瞬时运动学的逆运算无限逼近

干货 | “逆运动学”——从操作空间到关节空间(上篇)插图(3)

 

·   雅可比矩阵转置法(Jacobian Transpose),是从下面式子出发,用雅可比矩阵的转置代替困难的求逆运算

干货 | “逆运动学”——从操作空间到关节空间(上篇)插图(4)

 

由于在逆运动学求解中,我们不关心系统的动力学特性,上面这个式子也可以写成

干货 | “逆运动学”——从操作空间到关节空间(上篇)插图(5)

 

上面这两个方法相对比较通用(是否工业上常用?这个很难说。可能工业上对特定机械臂用解析解的也不少……),所以这篇文章主要还是给我们的老朋友雅可比矩阵解锁新功能,本文将着重介绍雅可比矩阵求逆法。

 

迭代解 – 雅可比矩阵求逆(Jacobian Inverse)

 

雅可比矩阵求逆的方法有什么优点呢?我们还是拿那个两自由度机械臂举例:

干货 | “逆运动学”——从操作空间到关节空间(上篇)插图(6)

 

假设link的长度都是1,那我们可以求出它的解析解为(希望上面那个方程组你自己动手解了一下):

干货 | “逆运动学”——从操作空间到关节空间(上篇)插图(7)

 

现在我们需要end effector从图中(xe, ye)沿紫色直线移动到(x’e, y’e),但你需要控制关节位置实现这一点。现在你可以有几种思路——

 

第一,只求起点和终点对应的关节位置,直接对这两处关节位置进行线性插值求得关节运动轨迹——这样虽然省了很多计算量,但end effector不大可能沿着直线走

 

第二,把这条直线插入很多很多中间点,每一个点都求出对应的关节位置,然后控制每一个关节按着这一系列的关节位置走(就是我们前面说的解析/优化解法);

 

第三,还是把这条直线插入很多很多中间点,但如果点与点的间隔足够小、运动时间足够短,我们就可以在每一点用雅可比矩阵求逆来求得当前关节位置的变化——换个角度讲,我们也可以设定end effector沿这条直线的运动速度,用雅可比矩阵求逆求得关节速度,直接控制关节的运动速度而不是位置

 

我们把第三个方法画成控制框图:

干货 | “逆运动学”——从操作空间到关节空间(上篇)插图(8)

 

图中xd下标d表示“desired”,即你希望x到达的位置和朝向;以后还有下标c表示“current”,即当前x的位置/朝向;我们会经常看到这样的下标。

 

你可能不懂控制框图,没关系,首先上面一行Joint control(关节控制)之前,从左到右说的是:

干货 | “逆运动学”——从操作空间到关节空间(上篇)插图(9)

 

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是通过求解下面这个问题得到的:

干货 | “逆运动学”——从操作空间到关节空间(上篇)插图(10)

 

在机器人中,我们会希望每一次迭代的关节运动量尽可能小(从多种可能的运动方式中选出运动最少的一种),所以上面说的是求一个尽可能小的能满足方程的dq。

 

拉格朗日乘数法(Lagrange Multiplier),这个问题可以变成:

干货 | “逆运动学”——从操作空间到关节空间(上篇)插图(11)

 

(乘1/2是为了后面求导后不要有个讨厌的2——不影响结果)

 

拉格朗日乘数法是一个求约束条件下极值的方法,非常有用,用起来也比较简单,而且我们以后还会用到——如果你不是很了解,我强烈建议你去自学一下。

 

要求极值,必先求导——拉格朗日乘数法是分别对dq和lamda求导,导数为0时可求得极值(我们就不论证这里是极小值了):

干货 | “逆运动学”——从操作空间到关节空间(上篇)插图(12)

 

解上面两个方程,即可求出

干货 | “逆运动学”——从操作空间到关节空间(上篇)插图(13)

 

不难验证JJ+=I。用这个J+求解出的dq即为满足条件的最小关节运动速度。最后简单地提一下null space,我们可以验证:

干货 | “逆运动学”——从操作空间到关节空间(上篇)插图(14)

 

这说明矩阵I-J+J可以把任意关节速度投影到“零空间”内,投影后的关节速度将不引起任何end effector的运动。利用这个特性,我们可以在满足了end effector的xd之后,再利用零空间实现其它任务(比如避开障碍物)。

发表评论

后才能评论