13. 机器人正运动学---雅克比矩阵(1)

这篇文章主要介绍机器人雅克比矩阵的由来以及利用几何法求解雅克比矩阵

文章共4,658字 · 阅读需要大约16分钟

一键AI生成摘要，助你高效阅读

问答

hitgavin

23177人浏览 · 2020-09-20 10:57:02

hitgavin · 2020-09-20 10:57:02 发布

1. 引言

前面的一些文章我们一直对机器人进行静态分析，也就是给定一组关节角求机器人末端位姿。这篇文章我们来分析一下关节角的运动将怎样影响机器人末端的位置和姿态。这就是雅克比矩阵了。

2. 雅克比矩阵

还是先了解一下雅克比矩阵的由来吧。我们在高数中都学过函数以及函数的导数，设有一个关于时间 $t$ 的函数 $f(t)$ ，那么它的时间导数为 $\dot{f}(t)$ ，这个很简单吧。现在我们换一个提法，在时间 $t_{0}$ 处对应的函数值是 $f(t_{0})$ ，当时间 $t$ 在 $t_{0}$ 基础上发生一个微小变化 $d_t$ ，那么相应的 $f(t)$ 会在 $f(t_{0})$ 的基础上发生怎样的变化呢？这其实是一个微分的提法，我们把前面的导数写成微分的形式如下：

$d_f=\dot{f}(t_{0})d_t$

为了直观定义 $\dot{f}(t_0)=J_0$ 那么 $d_f=J_0\cdot d_t$ 。这里 $J_0$ 是一个标量。

扩展一下，设 $f(t)$ 是一个向量函数，如下：

$f(t)=\begin{bmatrix} f_1(t)\\ f_2(t)\\ ...\\ f_n(t) \end{bmatrix}$

还是以微分形式表达自变量 $t$ 的变化如何影响函数值，如下：

$d_f=\begin{bmatrix} d_{f1}\\ d_{f2}\\ ...\\ d_{fn} \end{bmatrix}=\begin{bmatrix} \dot{f_1}(t_{0})\\ \dot{f_2}(t_{0})\\ ...\\ \dot{f_n}(t_{0}) \end{bmatrix}d_t$

同样为了直观定义：

$J_0=\begin{bmatrix} \dot{f_1}(t_{0})\\ \dot{f_2}(t_{0})\\ ...\\ \dot{f_n}(t_{0}) \end{bmatrix}$

那么 $d_f=J_0\cdot d_t$ 。这里 $J_0$ 是一个nx1的向量。

接下来再扩展一下，自变量不是一维的时间 $t$ ，而是一个 $m$ 维的向量：

$x=\begin{bmatrix} x_0\\ x_1\\ ...\\ x_m \end{bmatrix}$

这个时候要怎么写这个微分表达式呢？其实也并不复杂，把所有的函数对所有的自变量都求一次导数然后按照一定的顺序排列一下就可以了，如下：

$d_f=\begin{bmatrix} d_{f1}\\ d_{f2}\\ \vdots \\ d_{fn} \end{bmatrix}=\begin{bmatrix} \frac{\partial f_{1}}{\partial x_{1}} & \frac{\partial f_{1}}{\partial x_{2}} & \cdots & \frac{\partial f_{1}}{\partial x_{m}}\\ \frac{\partial f_{2}}{\partial x_{1}} & \frac{\partial f_{2}}{\partial x_{2}} & \cdots & \frac{\partial f_{2}}{\partial x_{m}} \\ \vdots & \vdots & \ddots & \vdots \\ \frac{\partial f_{n}}{\partial x_{1}} & \frac{\partial f_{n}}{\partial x_{2}} & \cdots & \frac{\partial f_{n}}{\partial x_{m}} \end{bmatrix}\begin{bmatrix} d_{x1}\\ d_{x2}\\ \vdots \\ d_{xm} \end{bmatrix}$

同样，为了直观，我们定义：

$J_{0}=\begin{bmatrix} \frac{\partial f_{1}}{\partial x_{1}} & \frac{\partial f_{1}}{\partial x_{2}} & \cdots & \frac{\partial f_{1}}{\partial x_{m}}\\ \frac{\partial f_{2}}{\partial x_{1}} & \frac{\partial f_{2}}{\partial x_{2}} & \cdots & \frac{\partial f_{2}}{\partial x_{m}} \\ \vdots & \vdots & \ddots & \vdots \\ \frac{\partial f_{n}}{\partial x_{1}} & \frac{\partial f_{n}}{\partial x_{2}} & \cdots & \frac{\partial f_{n}}{\partial x_{m}} \end{bmatrix}$

那么 $d_f=J_0\cdot d_x$ 。这里 $J_0$ 是一个nxm的向量。

总结一下你会发现不管自变量和函数值是标量还是向量，他们之间微小量的变化总是存在一种固定的联系，那就是：

$d_f=J_0\cdot d_x$

其中 $d_x$ 代表自变量的一个微小变化， $d_f$ 代表函数值的微小变化。联系两者的一个纽带就是所谓雅可比矩阵 $J_0$ （其实就是函数向量对自变量向量的偏导数矩阵）。从前面的例子中类比得到当自变量和函数值都是标量时雅可比矩阵退化为一个标量，也就是导数值！

3. 机器人雅克比矩阵

前面介绍了雅克比矩阵的基本概念，很显然机器人雅克比矩阵描述的是关节转速（自变量）和末端笛卡尔速度和角速度（函数值）之间的关系。那么在机器人学中雅克比矩阵有什么用呢？总结来说：

DH参数可以把关节角映射为末端执行器的位置和姿态
雅克比矩阵可以把关节转速映射为笛卡尔空间的速度和角速度

DH参数和雅克比矩阵可以认为是机器人正运动学的基础。

4. 求解雅克比矩阵

怎么求解雅克比矩阵呢？其实大体上有两个思路，第一个思路是从几何意义去考虑：找出每个关节的微小变化如何影响笛卡尔空间中位置和姿态的微小变化，后面简称几何法。第二个思路是从数学意义的角度出发：推导出每个关节的微分与笛卡尔空间微分之间的关系，后面简称解析法。这篇文章主要介绍几何法，分析法涉及到矩阵求导等复杂的公式推导，下一篇文章单独介绍。

4.1 几何法

几何法求解雅克比矩阵相对来说简单直接。先从一个最简单的情况出发，设六轴机器人当前的关节角为 $q=\begin{bmatrix} q_1 & q_2 & q_3 & q_4 & q_5 & q_6 \end{bmatrix}$ ，除关节2以外其他关节均锁位（固定不动）。为了体现一般性，我们用 $i$ 来代表关节2。现在求解关节 $i$ 的转速与机器人末端执行器笛卡尔速度和角速度之间的关系。我们当前陈述的问题可以用下图表示。

解释一下图中符号的含义，提前声明，图中所有的点和向量都是在机器人的基坐标系下描述的，除关节2以外所有关节均锁位。 $E$ 代表末端法兰盘中心点坐标系， $v_e$ 和 $\omega_e$ 分别代表末端法兰盘中心点坐标系的速度和角速度矢量。 $O_{i-1}$ 代表连杆 $i-1$ 坐标系的原点（标准DH参数连杆 $i-1$ 坐标系建立在关节 $i$ 上！如果你还是存在疑惑请参考我的另外一篇博客：连杆坐标系与DH参数）， $\dot{q_i} \cdot z_{i-1}$ 代表关节 $i$ 角速度，其中 $\dot{q_i}$ 代表角速度的大小，是标量; $z_{i-1}$ 代表角速度的方向，也是连杆 $i-1$ 坐标系的 $z$ 轴，是矢量。

有了前面的这些阐述我们就可以求法关节 $i$ 转速和法兰盘中心点坐标系在笛卡尔空间的速度和角速度之间的关系了，根据速度的计算公式 $v=\omega \times r$ ：

$v_e=\dot{q_i}z_{i-1}\times \overrightarrow{O_{i-1}E}$

由于只有关节 $i$ 运动，法兰盘中心点坐标系角速度就是关节 $i$ 角速度，所以：

$\omega_e=\dot{q_i} \cdot z_{i-1}$

那么关节 $i$ 的转速和法兰盘中心点坐标系笛卡尔空间速度和角速度之间的关系可以描述为以下矩阵形式。

$\begin{bmatrix} v_e\\ \omega_e \end{bmatrix}=\begin{bmatrix} z_{i-1} \times \overrightarrow{O_{i-1}E}\\ z_{i-1} \end{bmatrix}\cdot \dot{q_i}=J^{i}\cdot\dot{q_i}$

在这里：

$J^i=\begin{bmatrix} z_{i-1} \times \overrightarrow{O_{i-1}E}\\ z_{i-1} \end{bmatrix}$

可见 $J^i$ 是一个列向量，它代表在 $q=\begin{bmatrix} q_1 & q_2 & q_3 & q_4 & q_5 & q_6 \end{bmatrix}$ 时，关节 $i$ 的转速与末端笛卡尔速度和角速度之间的关系！

对于移动关节就更简单了，它只能造成末端法兰盘沿着 $z_{i-1}$ 平移，不会产生旋转。因此这个时候：

$J^i=\begin{bmatrix} z_{i-1}\\ 0 \end{bmatrix}$

连杆 $i-1$ 坐标系 $z_{i-1}$ 轴矢量，原点 $O_{i-1}$ ，法兰盘中心点 $E$ 在基坐标系下的表示都可以根据DH参数和当前关节角轻松的求解出来。

高中物理中我们学过速度是满足叠加原理的，大学物理中扩展了这个描述，角速度也是满足叠加原理的。因此，机器人在某一个固定的位姿下，我们单独求解每一个关节的转速与末端笛卡尔速度和角速度的关系然后把它们加在一起就可以了！

对于以上的六关节机器人，它的雅克比矩阵即为：

$\begin{bmatrix} v_e\\ \omega_e \end{bmatrix}=\begin{bmatrix} J^1& J^2& J^3& J^4& J^5& J^6 \end{bmatrix}\begin{bmatrix} \dot{q_1}\\ \dot{q_2}\\ \dot{q_3}\\ \dot{q_4}\\ \dot{q_5}\\ \dot{q_6} \end{bmatrix}=J \cdot \dot{q}$

其中：

$J^i=\begin{cases} \begin{bmatrix} z_{i-1} \times \overrightarrow{O_{i-1}E}\\ z_{i-1} \end{bmatrix} & \text{ if } revolute \ joint \\ \begin{bmatrix} z_{i-1}\\ 0 \end{bmatrix}& \text{ if } prismatic \ joint \end{cases}$