说到逆运动学（IK），其中最重要的一部分就是利用雅克比矩阵表示目标状态和变量组之间的关系。具体文献参考“Introduction to Inverse Kinematics with Jacobian Transpose, Pseudoinverse and Damped Least Squares methods”。我们今天主要介绍雅克比矩阵在正运动学中的推导和应用。可能会用到一些我以前写的一篇关于坐标变换的相关知识，有兴趣的可以翻阅一下。
现在先假设我们已经知道一些先验知识，比如机械臂是由一些关节（Joint）连接的，一直从基座（base）连接到末端执行器（End-Effector），关节一般只有一个自由度（要么是旋转，要么是平移），末端执行器有六个自由度。如图1所示，我们已经将机械臂的结构描述的很清楚了。我们现在就是要解决，
如何通过每一个关节的自由度求出末端执行器的角速度和线速度
。这里在引入一个瞬时动力学的概念，就是任意关节运动，末端执行器也会跟着运动，如果时间足够小的话，我们可以看作关节和末端执行器都是瞬时变化的。现在我们问题也就转变成找
ΔQ
（关节的变化）和
ΔX
（末端执行器的变化）之间的关系。
图1
我们先确定一些变量，我们对于
旋转关节
（revolute）一般讨论的是
角度
的变化，对于
平移关节
（prismatic）一般讨论的是
距离
的变化。这里为了统一表示，我们将角度和距离统称为通道量，记为
q
。同时，我们将目标状态（就是指末端执行器的角速度和线速度）记为向量组
X
={
x
1
，
x
2
，
x
3
，
x
4
，
x
5
，
x
6
}。好了，准备工作差不多了，现在我们来推导公式了。我们假设
X
中的每一项都是由
q
求出来的（事实上也是如此），记为
x=f(q)
。这样我们可以得到一组等式：
图2
根据上面等式，我们可以进行扩展。我们对
X
中的每一项进行全微分，等式的右边对所有的
q
进行偏导。具体展开式如下 , 另外我们可以将图3左边的展开式转化成右边的矩阵形式：
图3
可以发现一些结论，比如矩阵的行等于
X
中的项数m，也就是末端执行器的自由度个数，而矩阵的列等于
Q
中的项目数，也就是关节的个数。我们现在很自然的将上文中提到的
ΔQ
和
ΔX
联系起来了。这里得到的这个矩阵我们一般称为雅克比矩阵（Jacobian Matrix），记为
J
。上面的等式可以简写成：
图4
这里，雅克比矩阵的推导就完成了。雅克比主要联系的是
ΔQ
和
ΔX
，是瞬时运动学的重要组成部分。在机器人学中，雅克比矩阵主要是用来求末端执行器的角速度和线速度，
ΔQ
和
ΔX
在时间片很小的情况下是可以用来表示速度的。在逆运动学中，我们通常利用末端执行器的自由度反算每个节点的自由度，这是我们只要将
ΔX
乘以
雅克比矩阵的逆
就可以得到
Q
的变化了。
以前我也说过，人体运动主要计算的主要是位置信息和速度信息的变化。所以对于机械臂来说，我们最终想得到的也就是末端执行器的位置和速度。这里重点说一下怎么利用正运动学求末端执行器的线速度和角速度。我们知道关节分为旋转关节和平移关节，那么这两种关节分别对线速度和角速度的影响如图5所示。
图5
我们逐项分析一下。平移关节对末端执行器的角速度是没有影响的；平移关节沿自身的
Z
轴移动，对末端执行器的线速度提供一个Z轴方向的分速度。旋转关节对末端执行器的角速度也是提供一个分量，角速度分量的方向也是沿
Z
轴的。旋转关节对末端执行器的线速度影响比较复杂，我们想象一下圆盘，现在圆盘随着圆心旋转，越远离圆心，
P
点的速度越快。我们结合图5和图6，
Ω
i
和
P
in
的叉乘的结果等于
v
p
。这也就是旋转关节对于末端执行器线速度的影响。下面进行公式推导。
图6
首先，我们来看一下
V
i
和
Ω
i
的表示，其实这两个量就是通道量乘上其方向向量，所以可以得到图7的两个公式：
图7
根据
V
i
和
Ω
i
，我们可以写出末端执行器的角速度和线速度的公式：
图8
这里，
ε
有两个值0和1，当
ε
表示的是平移矩阵的时候值为1，表示的是旋转矩阵的时候值为0。这样就得到线速度和角速度的公式，我们扩展一下公式：
图9
这里，我们就得到了线速度
v
和角速度
ω
跟各关节通道
q
之间的关系。另外，可以发现，在这两个公式是蕴含了雅克比矩阵的，我已经用红线标出来了。这就可以得到整个的雅克比矩阵：
图10
我们再对雅克比矩阵进一步分析。对于线速度而言，当前的公式比较难以计算，我们换一种思路。我们一般可以比较容易的得到末端执行器的位置变化
x
p
，所以我们可以根据对
x
p
的微分求出线速度。改进的公式如下：
图11
写到这里，我们就完完全全的得到了通道q和线速度v以及角速度ω之间的关系。这也是机器人学
中
求解机械臂的
最常用
方法。
下面举个例子，加深一下对雅可比矩阵的认识。如图12所示，这是一个二维的机械臂，各个关节的通道已经给出了，机械臂的长度也已经给出，下面就是如何求解末端执行器的角速度和线速度。
图12
我们先来看线速度。
P
1
的速度肯定是为0的，后面节点的速度其实就是一个累加过程，各个节点的线速度如下：
图12
s
1
表示的是
sinθ
1
，
c
1
表示的是
cosθ
1
，
s
2
表示的是
sinθ
2
，
c
2
表示的是
cosθ
2
，
s
12
表示的是
sin(θ
1
+θ
2
)
，
c
12
表示的是
cos(θ
1
+θ
2
)
。这里牵扯到一些向量叉乘的计算，可以翻阅相关的数学书籍，就不在详述了。我们下面接着看角速度的求解， 这个就比较简单了，具体公式如下：
图13
接下来，我们根据
V
P3
和
ω
3
的公式就可以得到雅克比矩阵，如下所示：
图14
转载于:https://www.cnblogs.com/caster99/p/4725914.html