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

1041
0
2020年4月8日 11时01分

本文转载自微信公众号ROBOTICS

 

逆运动学,就是从操作空间的end effector position and orientation,求关节空间的joint position的问题。在上一篇文章中,我们简单提到求逆运动学解的解析解法和优化解法,详细讲解了用逆瞬时(或说微分)运动学即雅可比矩阵法迭代求解逆运动学的方法。这篇文章我们继续讲雅可比矩阵求逆法存在的问题、用以对付Singularity问题的阻尼最小平方法,并详细地讲讲雅可比矩阵转置法。

 

雅可比矩阵求逆法复习

 

雅可比矩阵求逆法,就是利用下面式子所示的关节速度与end effector速度的关系,迭代求解逆运动学问题的方法。

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

 

我们可以用一个控制框图来表示这个求解过程:

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

 

这个方法把复杂的逆运动学运算(解析法和优化法)转化为求解线性方程的运算,具有很好的通用性;然而这个方法也存在一些问题。

 

雅可比矩阵求逆法存在的问题

 

从原理上讲,使用这个方法最明显的一点要求是——dx不能过大。因为Jacobian是随着关节位置变化不断在变化的,一旦关节位置变化很大,算出来的Jacobian Inverse就不再准确了。这个问题通常我们可以用轨迹线性插值(linear interpolation)或限制dx的大小(clamping)来避免。

 

这个方法的第二个难点是雅可比矩阵求逆运算。矩阵求逆是一个非常消耗计算资源的运算(在写程序时能避免就绝对不要求逆!)当然,我们总是可以使用各种各样的解线性方程的方法来避开求逆运算,比如LU分解、Chelosky分解、QR分解、SVD(Singular Value Decomposition)等等——这个以后也可以考虑用来填几篇干货了……

 

这个方法最大的问题还是在于它无法很好地对付机器人Singularity或接近Singularity的情况。从线性方程的角度看,当机器人接近Singularity时,雅可比矩阵也越来越“病态”(ill-conditioned),很小的dx可能求得很大的dq,方程对数值误差也更加敏感;而当机器人处于Singularity时,线性方程可能无解、也可能有无数多个解。

 

下面的动图展示了控制机械臂运动轨迹通过两种常见的Singularity Configuration的情况,可以帮助你理解什么叫“接近Singularity时,很小的dx可能得到很大的dq”。(图中end effector走直线轨迹,它停顿的那一点就是机械臂处于singularity的点,可以看到在那个点关节突然快速运动)

 

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

 

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

 

DLS(Damped Least Square,阻尼最小平方法)

 

为了避免利用雅可比矩阵求逆法控制机械臂时由于接近Singularity而产生非常大的关节速度,一个自然的想法就是在求解过程中限制关节速度——既要尽可能地满足方程条件、也要尽可能地让关节速度不要太大

 

对于前者,我们可以用最小平方法求解方程,这时问题可以这样表述:

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

 

即求一个dq,使Jdq – dx的norm(向量范数,可以理解为衡量向量距离的一种指标)的平方最小;理想情况下等式左右相等,这个norm则为0。

 

对于后者,即是希望||dq||尽可能小(但显然通常情况下不能为0),此时我们可以把上面的式子后面再加一个“阻尼”项,变成这样:

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

 

即求一个dq,使Jdq-dx的norm的平方,加上乘以一个系数的dq的norm的平方,它们的和最小。这个时候,λ的大小决定了你更“看重”哪一个条件:如果λ很大,那可能你求得一个很小的关节运动速度,但这个速度却不能准确地让end effector按照你所希望的轨迹走;如果λ很小,小到接近于0,那这个方法跟之前的最基础的Jacobian Inverse算法也没有什么两样了。在实际使用,λ的大小往往需要仔细选择。

 

求解上面那个式子的最小值,还是用我们伟大的导数求极值法

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

 

(这么详细的求导过程就算看不懂也要记住啊!极其重要,随处可见)

 

于是我们得到一个等效方程:

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

 

数学好的人能够证明左边的系数矩阵是可逆的,于是我们就有了解:

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

 

数学好的人还能看出显然以下式子也是相等的:

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

 

知道这个有什么用呢?因为左边需要求逆的矩阵大小为n×n,n为关节数量,要多大有多大;右边需要求逆的矩阵大小为m×m,m为操作空间的自由度大小,最大肯定不超过六。这个小小的转换,就限制了需要求逆运算的矩阵大小,提高了总体的运算速度。

 

雅可比矩阵转置法

 

上次我们简单地讲到说,我们可以用雅可比矩阵的转置代替求逆运算来求逆运动学问题:

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

 

今天我们详细地讲下这个看起来很随便的方法是怎么推导出来的。

 

首先明确求解逆运动学问题,我们的终极目标是控制机械臂的各个关节,让机械臂的end effector运动到我们想要它到达的位置及朝向。我们把这个“理想位置”计为xd,我们的目标就是要让当前end effector位置xc与xd的“距离”最短。于是我们又有了一个数学问题:

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

 

注意上面式子中我们要求的是q,而不是dq。乘上1/2是为了求导后不要有讨厌的系数2。

 

用正运动学的表达式可以把xc用f(q)表示:

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

 

(也许你已经发现了这个就是把逆运动学转化为优化问题的优化法)。

 

由于正运动学的表达式f(q)通常比较复杂,导数极值法在这里不太好用。所以我们祭出求极值的另一个伟大工具:梯度下降法(Gradient Descent)

 

梯度下降法说,每一步我都要沿着下降最快的那个方向走,而这个方向可以由梯度的反方向求得,所以我们有:

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

 

这个推导证明了如果我们用梯度下降大法,每一步都用J的转置乘dx求出dq,那么机械臂迭代几步以后end effector将无限趋近指定的xd。它的迭代框图其实与雅可比矩阵求逆法是一样的,只是J-1换成了JT。

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

 

上面的α也是梯度下降法的一个系数,叫“步长”(step size)或“学习速率”(learning rate)。这个值太小,则迭代速度可能太慢;这个值太大,则可能“走过头”或要多绕一些弯路,最终还是使迭代速度变慢(甚至无法收敛到最小值)。

 

采用雅可比矩阵转置而不是求逆的方法,最大的好处当然是避免了求逆运算;不过与求逆相比,它的迭代收敛速率反而慢些。另外,用这个方法控制的机械臂,离end effector较远的关节常常需要输出更大的扭矩。

 

关于逆运动学的这两篇文章,可以说讲了机器人学家们与逆运动学问题斗智斗勇的过程;我们简单提到解析法和优化法,讲了雅可比矩阵求逆迭代法、阻尼最小平方法、以及实质上也是优化和迭代方法的雅可比矩阵转置法。然而这只是逆运动学问题求解方法中比较有代表性的一小部分。

 

最后我想说,我们用到的向量求导、导数极值法、拉格朗日乘数法、梯度下降法等,都是非常重要非常有用的数学工具。相信通过对逆运动学问题解法的学习研究,你的数学水平也得到了提高……

发表评论

后才能评论