几种冗余机械臂逆向运动学方法（带公式推导）_七自由度冗余机械臂梯度投影逆运动学

此篇博客转自本作者在古月居的博客：几种冗余机械臂逆向运动学方法（带公式推导） - 古月居

前言：

之前展哥（导师）喊我在入学前做一个具有冗余腿的四足机器人用于部署XXX算法，验证XXX的ideal。最终由于有其他事情，没有投入太多的精力，以失败告终。但是过程中腿部末端需要求逆向运动学运用轨迹跟踪，然而由于腿部设计为冗余的，单单使用带阻尼的伪逆方法可以在一开始跟随轨迹，但是在一段时间后，部分关节开始趋向于极限，轨迹跟踪最后得不到保证。设计的关节运动范围是完全满足这段轨迹规划的，但是在运动过程中，不好的规划导致系统取向于极端状态，最终导致末端运动空间受限的问题。下图是我在寒假中，在Coppeliasim平台做的冗余腿（5自由度）四足机器人模型。

本文主要总结下现在常用的及其简单的求解逆向运动学的方法。最后，给出我之前遇到问题的解决方法（说实话这方法也比较基础）。

文中将涉及的理论推导将用我手写的笔记图片来展示（因为我懒，懒得打Math Latex了）。文中所阐述冗余机械臂逆向运动学问题可以简化为一个简单的案例，具体示意图如下：

这是一个二维空间中末端点到期望点的执行任务，中间可能预先设定好了一段运动轨迹，现在需要求解关节角度使末端可以沿设定轨迹运行（逆向运动学问题）。

最简单的就是雅可比转置方法。可以很直觉的设定最小二次型目标代价函数为

然后代价函数对theta求导为0，可以得到以下的推导

仔细观察最后的式子，可以发现这非常像末端力空间通过转置的雅可比映射到扭矩空间。这里的末端力 在此变成了末端位置差，

前面多乘了一个很小的α缩小迭代步长。如此看来该方法的式子具有直观的物理意义。但是该方法并不能确定约束关节位置，这依旧不能解决之前所提到的问题。

  还有就是伪逆方法。伪逆方法还有他添加阻尼的形式，具体这个链接（干货，逆运动学）。

不同于雅可比转置方法，这里的代价函数是期望关节最小的速度运行。换句话说，每一次朝期望目标移动的时候，综合全部关节要求变化最小

（要求在角度空间中的最短路径），这似乎非常容易理解。后面的关节速度都转化为微小位移来表示。具体的代价函数可以表示为

通过拉格朗日乘子法求解，可以得到以下式子

 求偏导为0，可以得到以下式子

经过一些比较基础的带入，得到以下式子

通过以上的基本操作后，可以的到熟悉的式子，其中Δx前面的部分就是雅可比的伪逆形式。除此之外，可能还要求对某些关节的变化更小，

因此还可以有加权形式的表达，可以在代价函数中Δθ前再乘一项正定的斜对称矩阵Q（这里是一个3X3的矩阵），这里不在做详细的推导。

一般使用该方法，右式前还需要添加一个系数α，作为迭代的步长（这如同上述的转置方法一样）。

最后，介绍可以让关节朝着某个期望状态的变化，又同时满足逆解的方法。其实就是在最小关节角度路径的同时,

又满足最小化与期望关节状态之间的差异。可以得到同伪逆方法相似的二次型代价函数如下

其中多了一项S，表述为最小化与期望关节状态的差异，而代价函数需要加入这个最小化的目标。

同样使用拉格朗日乘子法，可以得到清晰的式子如下

接下来，经过比较常规操作，求偏导可以得到以下式子

同样的，经过一番简单的操作后可以得到以下式子

可以发现，最后的结果（框选出来的）相比之前最简单的伪逆方法额外的多了一项，简单来说，就是最后这一项起到了在关节空间中协调的作用，使满足末端位置运动的同时协调关节之间关系可以最小化与期望角度的差异。然后，我们可以很轻松的发现，结果的右式第二项的S前面是雅可比对应的零空间（null-space）投影矩阵。

零空间矩阵的基本特点如下

使用零空间方法可以对冗余机械臂进行额外的关节空间规划以满足某些约束条件，但是又不影响任务空间执行结果。

上述的零空间投影矩阵是一种较为简单的定义方法，并不是固定的。

当然，有时候我们希望不同关节的运行幅度不一样，这可能取决于任务和机械臂关节物理结构和属性。

这个时候我们同样可以采用加权矩阵来做为惩罚项。具体可以表现为如下图

其中Q可以是一个半正定的对角矩阵（这里是一个3X3的矩阵）。

另外，一般使用该方法，右式第一项前还需要添加一个系数α，作为迭代的步长（这如同上述的两种方法一样）。

以上是个人在做四足机器人期间的一些总结，如有误，还望指出。