Robotics-读懂显式雅可比，从公式求导走向几何直觉

机械臂的每个关节都在运动，末端也随之移动和旋转。那么，末端此刻究竟向哪个方向运动？移动多快？绕哪个方向转动？

回答这些问题，需要雅可比矩阵。

第一次学习雅可比时，我们往往从“对正运动学求偏导”开始：写出末端位置表达式，再对各个关节变量求导。对于平面二连杆，这样做很直接；但当机械臂变成六个关节、三维空间运动时，越来越长的三角函数表达式很容易掩盖公式背后的物理意义。

显式几何法提供了另一条思路：先不展开整套位置公式，而是观察每一个关节——如果只让它运动，它会给末端带来什么速度？

理解了这一点，雅可比矩阵的每一列就不再是一串需要背诵的公式，而是一种可以想象出来的机械运动。

一、先理清概念：显式雅可比与雅可比是什么关系？

“显式雅可比”并不是与“雅可比”并列的另一种数学对象。这里的“显式”，指的是将每个关节对末端速度的贡献直接写出来，进而构造出完整的几何雅可比矩阵。

为了区分这些名称，可以先看两个问题：

• 矩阵输出什么？ 输出实际线速度和角速度，还是选定任务坐标的变化率？
• 矩阵怎样求出来？ 对函数求导、逐级递推，还是根据几何关系逐列构造？

前者区分雅可比的输出类型，后者区分计算方法。

名称	它描述的是什么？
雅可比矩阵	输入变化与输出变化之间的局部线性关系
几何雅可比	将关节速度映射到末端的实际线速度和角速度
解析雅可比	将关节速度映射到选定位置、姿态参数的变化率
几何显式法	根据关节轴和末端位置，直接构造几何雅可比的各列
直接求导法	对正运动学表达式求偏导，得到相应的雅可比
速度递推法	沿机械臂逐级传播速度，再整理出关节速度的系数

不同资料也会用“显式形式”泛指已经展开的矩阵表达式。本文讨论的是其中常见的一种：用关节轴的几何关系，直接构造串联机械臂的几何雅可比。

以下假设机械臂基座固定，每个关节只有一个独立运动变量；所有空间向量统一在基座坐标系中表达。

二、从一个简单问题开始：二连杆末端怎样运动？

考虑一个平面二连杆机械臂。两根连杆长度分别为 \(l_1\)、\(l_2\)，第一关节角为 \(\theta_1\)，第二关节角 \(\theta_2\) 是第二杆相对第一杆的角度。

它的末端位置为：

\[\mathbf p_E= \begin{bmatrix} l_1\cos\theta_1+l_2\cos(\theta_1+\theta_2)\\ l_1\sin\theta_1+l_2\sin(\theta_1+\theta_2)\\ 0 \end{bmatrix}. \]

角度决定位置，角速度则决定位置如何随时间变化。根据链式法则：

\[\mathbf v_E =\frac{\partial\mathbf p_E}{\partial\theta_1}\dot\theta_1 +\frac{\partial\mathbf p_E}{\partial\theta_2}\dot\theta_2. \]

将两个偏导向量并排放在一起：

\[\mathbf v_E= \underbrace{ \begin{bmatrix} \dfrac{\partial\mathbf p_E}{\partial\theta_1}& \dfrac{\partial\mathbf p_E}{\partial\theta_2} \end{bmatrix}}_{\mathbf J_v} \begin{bmatrix}\dot\theta_1\\\dot\theta_2\end{bmatrix}. \]

这就是线速度雅可比。它的第一列描述第一个关节的贡献，第二列描述第二个关节的贡献。

如果暂时锁住第二个关节，只让第一个关节转动，第一列乘上 \(\dot\theta_1\) 就是末端速度；反过来，锁住第一个关节，第二列乘上 \(\dot\theta_2\) 就是末端速度。

雅可比矩阵乘以关节速度，本质上是在叠加各关节对末端瞬时速度的贡献。

显式几何法从这里出发：既然每列都是一种运动贡献，能否直接根据机械结构写出它，而不先展开末端位置公式？

三、只看一个关节，就能写出一列

先定义三个几何量：

符号	含义
\(\mathbf a_i\)	第 \(i\) 个关节轴的单位方向向量，正向与关节变量一致
\(\mathbf o_i\)	第 \(i\) 个关节轴线上任意一点的位置
\(\mathbf p_E\)	所选末端点的位置，例如工具尖端 TCP

从关节轴上参考点指向末端的向量为：

\[\mathbf r_{iE}=\mathbf p_E-\mathbf o_i. \]

注意，这个向量连接的是“第 \(i\) 个关节轴上的点”和“末端”，不一定只是某一根连杆。

移动关节：带着后面的结构一起平移

如果第 \(i\) 个关节是移动关节，它沿 \(\mathbf a_i\) 方向以 \(\dot d_i\) 的速度运动。其他关节暂时固定时，后面的结构一起平移，不产生额外转动。

因此：

\[\mathbf v_E^{(i)}=\mathbf a_i\dot d_i, \qquad \boldsymbol\omega_E^{(i)}=\mathbf0. \]

把关节速度提出来：

\[\begin{bmatrix}\mathbf v_E^{(i)}\\\boldsymbol\omega_E^{(i)}\end{bmatrix} = \underbrace{\begin{bmatrix}\mathbf a_i\\\mathbf0\end{bmatrix}}_{\mathbf J_i} \dot d_i. \]

这就是移动关节对应的雅可比列：上面放轴方向，下面放零。

这里的零只表示这个移动关节不直接贡献角速度。机械臂上的其他转动关节，仍然可以让末端旋转。

转动关节：让末端绕轴产生切向速度

如果第 \(i\) 个关节是转动关节，它自身产生的角速度为：

\[\boldsymbol\Omega_i=\mathbf a_i\dot\theta_i. \]

末端绕这根轴运动，根据刚体转动的速度关系：

\[\mathbf v_E^{(i)} =\boldsymbol\Omega_i\times\mathbf r_{iE} =\left[\mathbf a_i\times(\mathbf p_E-\mathbf o_i)\right]\dot\theta_i. \]

这个关节对末端角速度的贡献则直接为：

\[\boldsymbol\omega_E^{(i)}=\mathbf a_i\dot\theta_i. \]

于是，转动关节对应的雅可比列为：

\[\mathbf J_i= \begin{bmatrix} \mathbf a_i\times(\mathbf p_E-\mathbf o_i)\\ \mathbf a_i \end{bmatrix}. \]

例如，关节绕 \(z\) 轴旋转，末端位于轴旁 \(L\) 米处：

\[\mathbf a_i=\begin{bmatrix}0\\0\\1\end{bmatrix}, \qquad \mathbf r_{iE}=\begin{bmatrix}L\\0\\0\end{bmatrix}. \]

叉乘结果为：

\[\mathbf a_i\times\mathbf r_{iE} =\begin{bmatrix}0\\L\\0\end{bmatrix}. \]

因此正向转动时，末端瞬时沿 \(+y\) 方向运动，切向速率为 \(L|\dot\theta_i|\)。这与熟悉的圆周运动完全一致。

四、显式雅可比的完整公式，其实只有两种列

把两类关节放在一起：

\[\boxed{ \mathbf J_i= \begin{cases} \begin{bmatrix}\mathbf a_i\\\mathbf0\end{bmatrix},&\text{移动关节},\\[12pt] \begin{bmatrix}\mathbf a_i\times(\mathbf p_E-\mathbf o_i)\\\mathbf a_i\end{bmatrix},&\text{转动关节}. \end{cases}} \]

然后按关节顺序横向拼接：

\[\mathbf J_g= \begin{bmatrix}\mathbf J_1&\mathbf J_2&\cdots&\mathbf J_n\end{bmatrix}. \]

完整速度关系为：

\[\boxed{ \begin{bmatrix}\mathbf v_E\\\boldsymbol\omega_E\end{bmatrix} =\mathbf J_g(\mathbf q)\dot{\mathbf q}} \]

其中：

\[\mathbf J_g= \begin{bmatrix}\mathbf J_v\\\mathbf J_\omega\end{bmatrix} \in\mathbb R^{6\times n}. \]

前三行对应线速度，后三行对应角速度，每一列对应一个关节。可以记成一句话：

移动关节上轴下零，转动关节上叉下轴。

这就是显式几何法的核心。它不是把相邻关节的雅可比连乘，而是把各关节贡献的列向量拼起来。

五、回到二连杆：显式法与直接求导法是否一致？

记：

\[c_1=\cos\theta_1,\quad s_1=\sin\theta_1,\quad c_{12}=\cos(\theta_1+\theta_2),\quad s_{12}=\sin(\theta_1+\theta_2). \]

对末端位置直接求导，得到：

\[\mathbf J_v= \begin{bmatrix} -l_1s_1-l_2s_{12}&-l_2s_{12}\\ l_1c_1+l_2c_{12}&l_2c_{12}\\ 0&0 \end{bmatrix}. \]

现在改用几何显式法。

两个关节轴均沿 \(z\) 方向：

\[\mathbf a_1=\mathbf a_2=\begin{bmatrix}0\\0\\1\end{bmatrix}. \]

两个关节中心分别为：

\[\mathbf o_1=\begin{bmatrix}0\\0\\0\end{bmatrix}, \qquad \mathbf o_2=\begin{bmatrix}l_1c_1\\l_1s_1\\0\end{bmatrix}. \]

第一列的线速度部分为：

\[\mathbf a_1\times(\mathbf p_E-\mathbf o_1) =\begin{bmatrix}-l_1s_1-l_2s_{12}\\l_1c_1+l_2c_{12}\\0\end{bmatrix}. \]

第二列的线速度部分为：

\[\mathbf a_2\times(\mathbf p_E-\mathbf o_2) =\begin{bmatrix}-l_2s_{12}\\l_2c_{12}\\0\end{bmatrix}. \]

两种方法得到相同结果。原因并不神秘：它们描述的都是“只改变某个关节时，末端位置怎样变化”。一个从函数求导出发，另一个从绕轴运动出发。

补上角速度部分，完整几何雅可比为：

\[\mathbf J_g= \begin{bmatrix} -l_1s_1-l_2s_{12}&-l_2s_{12}\\ l_1c_1+l_2c_{12}&l_2c_{12}\\ 0&0\\ 0&0\\ 0&0\\ 1&1 \end{bmatrix}. \]

最后一行表示两个关节都贡献绕 \(z\) 轴的角速度，因此末端角速度的 \(z\) 分量为 \(\dot\theta_1+\dot\theta_2\)。

取 \(l_1=l_2=1\,\mathrm m\)，\(\theta_1=30^\circ\)，\(\theta_2=60^\circ\)，以及：

\[\dot{\mathbf q}=\begin{bmatrix}0.5\\-0.2\end{bmatrix}\,\mathrm{rad/s}. \]

相乘后得到：

\[\mathbf v_E=\begin{bmatrix}-0.55\\0.4330127\\0\end{bmatrix}\,\mathrm{m/s}, \qquad \boldsymbol\omega_E=\begin{bmatrix}0\\0\\0.3\end{bmatrix}\,\mathrm{rad/s}. \]

这就是两个关节共同运动产生的末端瞬时速度。

六、与速度递推法横向比较：一个逐列算，一个逐级传

速度递推法从基座开始，逐级计算下一根连杆的运动。

对于同一刚体上两个点，速度关系为：

\[\mathbf v_B=\mathbf v_A+\boldsymbol\omega\times\mathbf r_{AB}. \]

在二连杆例子中，第一个关节固定在基座上。第二个关节中心的速度为：

\[\mathbf v_{O_2}=\dot\theta_1\mathbf e_z\times\mathbf r_{12}. \]

第二根连杆的总角速度是：

\[\boldsymbol\omega_2=(\dot\theta_1+\dot\theta_2)\mathbf e_z. \]

于是末端速度为：

\[\mathbf v_E=\mathbf v_{O_2} +(\dot\theta_1+\dot\theta_2)\mathbf e_z\times\mathbf r_{2E}. \]

按 \(\dot\theta_1\) 和 \(\dot\theta_2\) 整理：

\[\mathbf v_E= \left[\mathbf e_z\times(\mathbf r_{12}+\mathbf r_{2E})\right]\dot\theta_1 +(\mathbf e_z\times\mathbf r_{2E})\dot\theta_2. \]

整理出来的两个系数，仍然是显式雅可比的两列。

三种方法的区别，可以放在同一张表里理解：

比较项	直接求导法	速度递推法	几何显式法
从哪里开始	正运动学表达式	基座及相邻连杆运动关系	各关节轴和末端位置
核心动作	对关节变量求偏导	沿连杆链逐级传播速度	分别构造每个关节的贡献列
直观含义	输出对输入的变化率	上游运动怎样传给下游	单个关节怎样推动或转动末端
常见优势	适合简单表达式、自动微分与验证	适合同时计算各连杆速度	适合构造串联机构末端几何雅可比
常见错误	误把姿态参数导数当角速度	混用坐标系或漏掉叉乘项	轴下标、叉乘方向、末端点选择错误

只要构型、末端点、表达坐标系和输出定义相同，正确的计算结果就应一致。选择哪种方法取决于已有信息和计算目标，显式法并不保证在所有程序中都更快。

七、几何雅可比与解析雅可比，真正的区别在输出

平面机构中，末端方向可以用一个角度表示，它的导数就是垂直平面的角速度分量。这很容易让人以为，三维欧拉角的导数也直接等于角速度。

一般情况下，这并不成立。

设任务坐标采用位置加欧拉角：

\[\mathbf X=\begin{bmatrix}\mathbf p_E\\\boldsymbol\eta\end{bmatrix}. \]

直接求导得到：

\[\dot{\mathbf X} =\begin{bmatrix}\dot{\mathbf p}_E\\\dot{\boldsymbol\eta}\end{bmatrix} =\mathbf J_X\dot{\mathbf q}. \]

而几何雅可比给出：

\[\begin{bmatrix}\mathbf v_E\\\boldsymbol\omega_E\end{bmatrix} =\mathbf J_g\dot{\mathbf q}. \]

对于固定基座系中的笛卡尔位置，\(\dot{\mathbf p}_E=\mathbf v_E\)；但姿态部分通常满足：

\[\boldsymbol\omega_E=\mathbf A(\boldsymbol\eta)\dot{\boldsymbol\eta}. \]

因此，在 \(\mathbf A\) 可逆的范围内：

\[\mathbf J_X= \begin{bmatrix}\mathbf I&\mathbf0\\\mathbf0&\mathbf A^{-1}(\boldsymbol\eta)\end{bmatrix} \mathbf J_g. \]

\(\mathbf A\) 的具体形式取决于欧拉角顺序和角速度表达坐标系。欧拉角表示奇异时，这个逆不存在，但机械臂本身未必处于运动学奇异构型。

比较项	几何雅可比	采用欧拉角的解析雅可比
输出	实际线速度、角速度	位置坐标变化率、欧拉角变化率
姿态部分	\(\boldsymbol\omega_E\)	\(\dot{\boldsymbol\eta}\)
是否依赖欧拉角顺序	不需要选择欧拉角	需要明确角度顺序
可能的奇异问题	机器人任务运动能力退化	还可能遇到姿态参数化奇异

这也解释了为什么“显式雅可比”和“解析雅可比”不宜当作两个互斥选项：前者在本文中强调构造方法，后者强调输出参数。

八、工程上怎样获得关节轴方向？

当正运动学已经通过 D-H 参数建立，关节轴方向通常可以直接从累计齐次变换中提取。

\[{}^0\mathbf T_k= \begin{bmatrix} {}^0\mathbf R_k&{}^0\mathbf p_k\\ \mathbf0^T&1 \end{bmatrix}. \]

旋转矩阵的三列分别是局部坐标系的三个轴在基座系中的方向：

\[{}^0\mathbf R_k= \begin{bmatrix}{}^0\mathbf x_k&{}^0\mathbf y_k&{}^0\mathbf z_k\end{bmatrix}. \]

所以：

\[{}^0\mathbf z_k={}^0\mathbf R_k\begin{bmatrix}0\\0\\1\end{bmatrix}. \]

换句话说，取旋转矩阵第三列即可。若直接看 \(4\times4\) 齐次矩阵，只取第三列的前三项，底部的零不属于三维方向向量。

真正需要留心的是编号：

建系约定	第 \(i\) 个关节对应的轴方向	常用轴上参考点
标准 D-H	\({}^0\mathbf z_{i-1}\)	\({}^0\mathbf p_{i-1}\)
Craig 改进 D-H	\({}^0\mathbf z_i\)	\({}^0\mathbf p_i\)

这里的变换必须是相对基座的累计变换。不同关节在各自局部坐标系中都可能写成 \([0,0,1]^T\)，但它们在基座系中的方向通常不同，不能直接混用。

九、为什么有些机械臂的雅可比会出现整块零？

以 RRPRRR 型 Stanford 机械臂为例，关节变量为：

\[\mathbf q=[\theta_1,\theta_2,d_3,\theta_4,\theta_5,\theta_6]^T. \]

如果后三根腕部转轴交于同一个中心 \(W\)，并且选择这个中心作为位置参考点，那么雅可比具有如下结构：

\[\mathbf J_W= \begin{bmatrix} \dfrac{\partial\mathbf p_W}{\partial\theta_1}& \dfrac{\partial\mathbf p_W}{\partial\theta_2}& \dfrac{\partial\mathbf p_W}{\partial d_3}& \mathbf0&\mathbf0&\mathbf0\\[6pt] \mathbf a_1&\mathbf a_2&\mathbf0&\mathbf a_4&\mathbf a_5&\mathbf a_6 \end{bmatrix}. \]

第三列下半部分为零，因为第三关节只移动，不直接产生角速度。

最后三列上半部分为零，因为后三个关节绕经过腕部中心的轴旋转，不移动这个中心点。

两类零来自不同原因。也不能因此认为“前三个关节只影响位置”：第一、第二个转动关节仍然会影响末端姿态。

如果把参考点改成偏离腕部中心的工具尖端 \(E\)，腕部转动就可能移动工具尖端：

\[\mathbf v_E=\mathbf v_W+\boldsymbol\omega_E\times\mathbf r_{WE}, \qquad \mathbf r_{WE}=\mathbf p_E-\mathbf p_W. \]

因此最后三列的线速度部分一般不再为零。雅可比不仅与机械臂有关，也与所选速度参考点有关。

十、真正使用之前，还要记住三件事

公式固定，不代表矩阵数值固定

关节运动时，轴方向、轴上点和末端位置都可能改变，因此通常需要根据当前关节构型更新 \(\mathbf J_g(\mathbf q)\)。

在模型、末端点和表达约定固定时，雅可比不直接依赖关节速度和加速度。但它的时间导数 \(\dot{\mathbf J}_g\) 与关节速度有关。

同一个末端位姿也可能对应不同关节构型，所以“知道末端姿态”通常不足以唯一确定雅可比。

瞬时线性关系，不是全局线性关系

瞬时速度关系是：

\[\begin{bmatrix}\mathbf v_E\\\boldsymbol\omega_E\end{bmatrix} =\mathbf J_g(\mathbf q)\dot{\mathbf q}. \]

但有限位移通常只能局部近似，例如：

\[\Delta\mathbf p_E\approx\mathbf J_v(\mathbf q)\Delta\mathbf q. \]

关节变化较大时，雅可比本身也在变化，需要重新计算正运动学或沿路径更新，不能用起点的矩阵一乘到底。

得到雅可比，不意味着可以直接求逆

只有方阵且满秩时，才能使用普通矩阵逆。非方阵、奇异构型或接近奇异的情况，需要结合任务可实现性，考虑伪逆、阻尼最小二乘等方法。

显式法只是构造矩阵的方法，不能消除机器人本身的奇异性。

写在最后

学习显式雅可比，最有价值的收获不是多记住一条矩阵公式，而是能够读懂矩阵里的每一列。

移动关节的一列，描述它怎样推动后续结构；转动关节的一列，描述末端怎样绕该关节轴产生切向运动，以及如何增加角速度。

直接求导法让我们看到位置函数的变化率，速度递推法让我们看到运动沿连杆传播的过程，显式几何法让我们看到每个关节对末端的贡献。把三种视角对应起来，雅可比就从抽象的偏导矩阵，变成了可以用机械运动解释的计算工具。