Robotics-雅可比矩阵 - 从关节运动到末端速度、力与控制

机械臂的正运动学回答一个问题：给定各关节的位置，末端在哪里、朝向哪里？

但让机器人真正动起来，还需要回答另一组问题：每个关节以多快的速度运动，末端才能向指定方向移动？同样的关节速度，为什么在手臂伸直和弯曲时产生不同的末端速度？为什么有些姿态下，末端明明只需要缓慢移动，某个关节却要转得很快？

这些问题，都可以从雅可比矩阵开始理解。

雅可比矩阵描述的是：在当前关节配置下，关节的微小变化或瞬时速度，怎样影响所选任务坐标。

它不是一张固定的“换算表”，也不是相邻连杆齐次变换矩阵的另一种名字。它是正运动学在当前配置附近的局部线性关系。

本文以固定基座、刚性连杆、独立单自由度关节的串联机械臂为主要对象。公式采用列向量；角度求导默认使用弧度；没有明确写参考系时，同一公式中的向量均在同一坐标系下表达。

阅读路线与符号

建议分三遍阅读：第一遍看第 1—5 节，建立雅可比的直觉；第二遍看第 6—12 节，理解角速度、坐标表示和速度递推；第三遍看第 13—19 节，掌握逆解、奇异性、加速度和力映射。

符号	含义
\(\mathbf q\)	独立关节位置变量组成的向量
\(\dot{\mathbf q},\ddot{\mathbf q}\)	关节速度、关节加速度
\(\mathbf X\)	选定的任务坐标，可以只描述位置，也可以包含姿态参数
\(J_{\mathbf X}\)	将关节速度映射到任务坐标变化率的雅可比
\(J_g\)	将关节速度映射到末端线速度和角速度的几何雅可比
\(\mathbf p,\mathbf v,\boldsymbol\omega\)	位置、线速度、角速度向量
\({}^{A}R_B\)	将 B 坐标系表达的向量转换为 A 坐标系表达的旋转矩阵
\({}^{A}\mathbf v_{P/B}\)	P 相对 B 的速度，用 A 的坐标轴表达
\([\mathbf a]_\times\)	向量 \(\mathbf a\) 对应的叉乘矩阵

1. 从一个导数开始：雅可比究竟在描述什么？

先不考虑机器人。假设一个输入 \(q\) 决定一个输出 \(x\)：

\[x=f(q). \]

如果输入只改变一点点，那么输出变化可以近似为：

\[\Delta x\approx\frac{df}{dq}\Delta q. \]

导数告诉我们：在当前输入位置，输入变化一点，输出大约改变多少。

机械臂只不过有多个输入和多个输出。输入是关节变量，输出是末端任务坐标：

\[\mathbf X=f(\mathbf q),\qquad \mathbf q=\begin{bmatrix}q_1&\cdots&q_n\end{bmatrix}^T,\qquad \mathbf X=\begin{bmatrix}X_1&\cdots&X_m\end{bmatrix}^T. \]

对每个输出分别求各输入的偏导，把结果排成矩阵：

\[\boxed{ J_{\mathbf X}(\mathbf q) =\frac{\partial f}{\partial\mathbf q} =\begin{bmatrix} \dfrac{\partial X_1}{\partial q_1}&\cdots&\dfrac{\partial X_1}{\partial q_n}\\ \vdots&\ddots&\vdots\\ \dfrac{\partial X_m}{\partial q_1}&\cdots&\dfrac{\partial X_m}{\partial q_n} \end{bmatrix}} \]

于是：

\[\Delta\mathbf X\approx J_{\mathbf X}(\mathbf q)\Delta\mathbf q. \]

微分记作 \(d\mathbf X=J_{\mathbf X}d\mathbf q\)。课件中的 \(\delta\mathbf X=J\delta\mathbf q\) 也常用于表达这种一阶变化关系。对有限位移应保留近似号；关节变化很大时，不能一直用起点处的同一个雅可比去乘总变化量。

如果正运动学光滑且没有显式时间依赖，对时间使用链式求导就得到精确的瞬时速度关系：

\[\boxed{\dot{\mathbf X}=J_{\mathbf X}(\mathbf q)\dot{\mathbf q}}. \]

它不是说整条非线性运动轨迹变成了直线，而是说每一时刻的速度满足这个线性映射。

2. \(m\) 和 \(n\) 分别是什么？为什么雅可比不一定是 6×6？

在公式中：

\[\underbrace{\dot{\mathbf X}}_{m\times1} =\underbrace{J_{\mathbf X}}_{m\times n} \underbrace{\dot{\mathbf q}}_{n\times1}. \]

符号	由什么决定	具体含义
\(n\)	机构本身及独立坐标的选取	独立关节变量数
\(m\)	你选择的任务和表示方式	任务坐标向量的分量数

对于普通串联 R/P 机构，每个关节一个自由度，因此 \(n\) 就是关节数。对于存在闭环约束、耦合或多自由度关节的机构，则不能机械地数关节外壳，应该数独立变量。

例如 RPRR 机构：

\[\mathbf q=\begin{bmatrix}\theta_1&d_2&\theta_3&\theta_4\end{bmatrix}^T,\qquad n=4. \]

只研究末端三维位置，\(m=3\)，雅可比为 \(3\times4\)；研究完整线速度和角速度，几何雅可比为 \(6\times4\)。

把末端写成六维量，不代表四关节机构能够独立控制全部六个方向。 可实现的瞬时运动维数由雅可比的秩决定：

\[\operatorname{rank}(J)\leq\min(m,n). \]

还有一种容易混淆的情况：使用位置加四元数时，任务向量可以有 7 个分量；使用位置加旋转矩阵的九个元素时，可以有 12 个分量。但其中存在约束，空间刚体位姿仍只有 6 个独立自由度。

3. 机器人模型确定后，雅可比是固定的还是变化的？

通常是公式固定、数值变化。

连杆长度、关节类型、工具安装关系等确定后，我们可以推导出 \(J(\mathbf q)\) 的表达式。机器人运动过程中，当前关节角和伸缩量改变，代入表达式得到的矩阵数值通常也改变。

内容	是否随运动变化
刚性连杆长度、固定安装关系	不变
给定任务和参考系下的雅可比表达式	不变
当前关节配置 \(\mathbf q\)	变化
当前矩阵数值 \(J(\mathbf q)\)	通常变化
末端速度 \(J\dot{\mathbf q}\)	受配置和关节速度共同影响

因此，雅可比不直接依赖关节速度或加速度；它主要由当前配置和模型、任务定义决定。它的时间导数 \(\dot J\) 则会涉及关节速度，后面讨论加速度时会用到。

这里说“关节配置”比“末端姿态”更准确。同一个末端位姿可能对应肘上、肘下等不同关节配置，各自的雅可比可能不同。

也有常数雅可比。例如，理想 XYZ 直线平台的三个关节方向固定且彼此正交，若关节位移直接等于末端的三个位置坐标，那么 \(J=I_3\)。

4. 怎样读懂雅可比的一行、一列和一个元素？

将矩阵按列写成：

\[J=\begin{bmatrix}\mathbf j_1&\mathbf j_2&\cdots&\mathbf j_n\end{bmatrix}. \]

则：

\[\dot{\mathbf X} =\mathbf j_1\dot q_1+\mathbf j_2\dot q_2+\cdots+\mathbf j_n\dot q_n. \]

• 一个元素 \(J_{ij}\)：第 \(j\) 个关节变量对第 \(i\) 个任务坐标的局部影响。
• 一行：所有关节分别如何影响同一个任务坐标。
• 一列：只让某个关节运动时，它对全部任务坐标变化率的贡献系数。

例如，把第 2 关节速度设为单位速度，其他关节速度设为零，输出恰好就是第 2 列。所谓“单位速度”，对于 R 关节是 \(1\,\mathrm{rad/s}\)，对于 P 关节是 \(1\,\mathrm{m/s}\)，应根据实际单位解释。

每一列都在描述同一个目标点或同一个末端任务，只是贡献来自不同关节。

5. 第一个完整例子：平面 2R 的直接求导法

设两根连杆长为 \(l_1,l_2\)；\(\theta_1\) 是第一杆相对基准轴的角度，\(\theta_2\) 是第二杆相对第一杆的角度。第二杆的绝对朝向为 \(\theta_1+\theta_2\)。

末端位置：

\[\begin{aligned} x&=l_1\cos\theta_1+l_2\cos(\theta_1+\theta_2),\\ y&=l_1\sin\theta_1+l_2\sin(\theta_1+\theta_2). \end{aligned} \]

为简化书写，记 \(s_1=\sin\theta_1\)、\(c_1=\cos\theta_1\)、\(s_{12}=\sin(\theta_1+\theta_2)\)、\(c_{12}=\cos(\theta_1+\theta_2)\)。

分别对两个角求偏导：

\[\boxed{ J_p= \begin{bmatrix} -l_1s_1-l_2s_{12}&-l_2s_{12}\\ l_1c_1+l_2c_{12}&l_2c_{12} \end{bmatrix}} \]

因此：

\[\begin{bmatrix}\dot x\\\dot y\end{bmatrix} =J_p\begin{bmatrix}\dot\theta_1\\\dot\theta_2\end{bmatrix}. \]

如果还关心末端的平面朝向 \(\phi=\theta_1+\theta_2\)，就增加一行：

\[\begin{bmatrix}\dot x\\\dot y\\\dot\phi\end{bmatrix} = \begin{bmatrix} -l_1s_1-l_2s_{12}&-l_2s_{12}\\ l_1c_1+l_2c_{12}&l_2c_{12}\\ 1&1 \end{bmatrix} \begin{bmatrix}\dot\theta_1\\\dot\theta_2\end{bmatrix}. \]

这个 \(3\times2\) 矩阵可以描述三个输出变化率，但只有两个独立输入，所以不能任意指定三个输出。

代入一组数字

设 \(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}. \]

代入时角度按弧度参与计算，得到：

\[J_p=\begin{bmatrix}-1.5&-1\\\sqrt3/2&0\end{bmatrix},\qquad \begin{bmatrix}\dot x\\\dot y\end{bmatrix} =\begin{bmatrix}-0.55\\\sqrt3/4\end{bmatrix} \approx\begin{bmatrix}-0.55\\0.433\end{bmatrix}\mathrm{m/s}. \]

同时 \(\dot\phi=0.5-0.2=0.3\,\mathrm{rad/s}\)。这就是把关节速度换算为末端运动的完整过程。

6. 必须区分：任务坐标变化率与真实角速度

到目前为止，雅可比是对选定坐标直接求导得到的。但是，三维姿态有一个特殊之处：三个欧拉角的变化率，一般不等于角速度向量的三个分量。

线速度 \(\mathbf v\) 描述指定点的瞬时运动；角速度 \(\boldsymbol\omega\) 描述刚体绕哪根瞬时轴、以多快的速率旋转。

定义几何雅可比：

\[\boxed{ \begin{bmatrix}\mathbf v_E\\\boldsymbol\omega_E\end{bmatrix} =J_g(\mathbf q)\dot{\mathbf q},\qquad J_g=\begin{bmatrix}J_v\\J_\omega\end{bmatrix}} \]

其中 \(J_v,J_\omega\) 都是 \(3\times n\)，完整的 \(J_g\) 是 \(6\times n\)。即使机构只能做平面运动，也可以写成六行形式，其中部分行恒为零；也可以只保留所需的非零运动分量。

比较项	几何雅可比	对任务坐标求导的雅可比
输出	指定末端点的线速度与刚体角速度	所选位置、姿态参数的变化率
姿态部分	三维角速度向量	欧拉角、四元数等参数的变化率
行数	完整形式为 6	取决于任务表示
主要依赖	配置、几何、参考系和目标点	还依赖坐标参数化方式

本文把后一类统一记为 \(J_{\mathbf X}\)；采用位置加三个姿态参数时，通常称为解析雅可比。几何雅可比与解析雅可比的区分也见 Robotics Toolbox 文档。

7. 两种雅可比如何连接？\(J_{\mathbf X}=EJ_g\)

设任务坐标分为位置表示 \(\mathbf x_p\) 与姿态表示 \(\mathbf x_R\)：

\[\mathbf X=\begin{bmatrix}\mathbf x_p\\\mathbf x_R\end{bmatrix}. \]

按本文章和课件的转换方向定义：

\[\dot{\mathbf x}_p=E_p\mathbf v_E,\qquad \dot{\mathbf x}_R=E_R\boldsymbol\omega_E. \]

代入几何雅可比，得到：

\[\boxed{ J_{\mathbf X} =\begin{bmatrix}E_p&0\\0&E_R\end{bmatrix}J_g =\begin{bmatrix}E_pJ_v\\E_RJ_\omega\end{bmatrix}} \]

这就是“关节速度 → 线速度与角速度 → 任务坐标变化率”的两级映射。对应课件的矩阵关系可见 Stanford Jacobian 讲义第 4 页。

位置取同一固定参考系下的笛卡尔坐标时，\(E_p=I_3\)。姿态转换则必须明确参数顺序、旋转约定和角速度表达坐标系。

任务表示	\(E_p\)	\(E_R\) 尺寸	\(J_{\mathbf X}\) 尺寸
笛卡尔位置 + 三个欧拉角	\(I_3\)	\(3\times3\)	\(6\times n\)
笛卡尔位置 + 单位四元数	\(I_3\)	\(4\times3\)	\(7\times n\)
笛卡尔位置 + 旋转矩阵九元素	\(I_3\)	\(9\times3\)	\(12\times n\)

四元数与旋转矩阵形式的输出包含约束，不能把所有分量的目标变化率任意独立指定。三个欧拉角的转换则需要避开表示奇异点。

有的教材先定义 \(\boldsymbol\omega=A(\mathbf x_R)\dot{\mathbf x}_R\)。对于三个姿态角，在 \(A\) 可逆的区域，本文章的 \(E_R=A^{-1}\)。因此，看到不同写法时要先看映射方向，不能只凭矩阵名称判断。

单杆圆周运动：同一个速度，不同的坐标变化率

设平面单杆长 \(l=2\,\mathrm m\)，当前 \(q=30^\circ\)，\(\dot q=0.5\,\mathrm{rad/s}\)。末端的平面几何速度为：

\[\begin{bmatrix}v_x\\v_y\\\omega_z\end{bmatrix} =\begin{bmatrix}-l\sin q\\l\cos q\\1\end{bmatrix}\dot q =\begin{bmatrix}-0.5\\\sqrt3/2\\0.5\end{bmatrix}. \]

若任务坐标为 \([x,y,\phi]^T\)，则这三个分量直接对应 \([\dot x,\dot y,\dot\phi]^T\)。这里 \(\dot\phi=\omega_z\) 是平面单轴转动的性质。

如果位置改用极坐标，任务坐标取 \([r,\beta,\phi]^T\)，那么在 \(r>0\) 时：

\[\begin{bmatrix}\dot r\\\dot\beta\\\dot\phi\end{bmatrix} = \begin{bmatrix} \cos\beta&\sin\beta&0\\ -\sin\beta/r&\cos\beta/r&0\\ 0&0&1 \end{bmatrix} \begin{bmatrix}v_x\\v_y\\\omega_z\end{bmatrix} =\begin{bmatrix}0\\0.5\\0.5\end{bmatrix}. \]

半径不变，位置方位角与杆的朝向角都在变化。物理运动完全相同，只是表示方式改变了。

8. 构造几何雅可比的基础：\(\mathbf v=\boldsymbol\omega\times\mathbf r\)

设刚体绕固定点 O 旋转，P 是固连在刚体上的点，\(\mathbf r_{OP}\) 为从 O 指向 P 的向量，则：

\[\boxed{\mathbf v_P=\boldsymbol\omega\times\mathbf r_{OP}}. \]

为什么是叉乘？点到旋转轴的垂直距离为 \(\rho=\|\mathbf r_{OP}\|\sin\phi\)，圆周运动速率为 \(\|\boldsymbol\omega\|\rho\)，恰好等于叉乘的模。叉乘方向又满足右手定则，沿圆周切线。

例如 \(\boldsymbol\omega=[0,0,2]^T\,\mathrm{rad/s}\)，\(\mathbf r_{OP}=[1,0,0]^T\,\mathrm m\)，则 \(\mathbf v_P=[0,2,0]^T\,\mathrm{m/s}\)。

叉乘顺序不能颠倒：

\[\boldsymbol\omega\times\mathbf r=-\mathbf r\times\boldsymbol\omega. \]

把叉乘写成矩阵乘法

对任意三维向量 \(\mathbf a=[a_x,a_y,a_z]^T\)，定义：

\[[\mathbf a]_\times= \begin{bmatrix} 0&-a_z&a_y\\ a_z&0&-a_x\\ -a_y&a_x&0 \end{bmatrix}. \]

于是：

\[\mathbf a\times\mathbf b=[\mathbf a]_\times\mathbf b, \qquad [\mathbf a]_\times^T=-[\mathbf a]_\times. \]

课件把它写成 \(\widehat{\mathbf a}\)。这里的“帽子”表示反对称矩阵，不表示单位向量。它也不是旋转矩阵；一个描述叉乘运算，另一个描述有限旋转。

记忆方法是：叉乘左边的向量变成矩阵，右边保持为列向量。

9. 同时平移与旋转：三个速度贡献从哪里来？

前面的公式默认 O 固定。如果移动坐标系 B 相对 A 既平移又旋转，同时 P 还能相对 B 自己移动，那么：

\[\boxed{ {}^A\mathbf v_{P/A} ={}^A\mathbf v_{B/A} +{}^AR_B\,{}^B\mathbf v_{P/B} +{}^A\boldsymbol\omega_{B/A}\times \left({}^AR_B\,{}^B\mathbf r_{BP}\right)} \]

项	物理来源	小车与乘客的类比
\({}^A\mathbf v_{B/A}\)	B 原点的平移	小车参考点前进
\({}^AR_B\,{}^B\mathbf v_{P/B}\)	P 相对 B 自己移动	乘客在车上行走
角速度叉乘位置向量	B 旋转带动 P	小车转弯使偏离参考点的位置获得速度

这条式子可以由位置关系直接推导。令 B 原点在 A 中的位置为 \({}^A\mathbf p_B\)：

\[{}^A\mathbf p_P={}^A\mathbf p_B+{}^AR_B\,{}^B\mathbf r_{BP}. \]

求导时有三项：原点位置变化、相对位置变化、旋转矩阵变化。结合：

\[{}^A\dot R_B=[{}^A\boldsymbol\omega_{B/A}]_\times\,{}^AR_B, \]

就得到上面的速度公式。

如果 P 固定在 B 上，相对速度项为零。若 B 原点也固定，便进一步回到纯旋转公式。旋转矩阵在这里用于统一向量的表达坐标系；真正的旋转速度贡献由叉乘项给出，二者不能混为一谈。

10. 速度传播：从基座一路递推到末端

对机械臂的每一级，都可以把上一节中的 A、B、P 换成相邻连杆与原点。

先看适用于一般原点选择的关系。所有量统一用基座表达：

\[\begin{aligned} \boldsymbol\omega_{i+1}&=\boldsymbol\omega_i+\boldsymbol\omega_{\mathrm{rel},i+1},\\ \mathbf v_{i+1}&=\mathbf v_i+\boldsymbol\omega_i\times\mathbf r_{i,i+1}+\mathbf v_{\mathrm{rel},i+1}. \end{aligned} \]

如果按课件的方式建系，第 \(i+1\) 个关节轴为 \(z_{i+1}\)，原点 \(O_{i+1}\) 在该轴上，便可写成：

\[\begin{aligned} \boldsymbol\omega_{i+1}&=\boldsymbol\omega_i+\dot\theta_{i+1}\mathbf z_{i+1},\\ \mathbf v_{i+1}&=\mathbf v_i+\boldsymbol\omega_i\times\mathbf P_{i+1}+\dot d_{i+1}\mathbf z_{i+1}. \end{aligned} \]

这里 \(\mathbf P_{i+1}\) 是从 \(O_i\) 指向 \(O_{i+1}\) 的位移，而不是从基座指向下一原点的位置。

对于转动关节，\(\dot d_{i+1}=0\)；对于移动关节，\(\dot\theta_{i+1}=0\)。转动关节不直接移动它轴上的原点，但会改变下一连杆的角速度，从而在后续的叉乘项中影响离轴的点。

这个简式有建系前提，不能只记公式、不看原点位置。 如果原点偏离当前关节轴，当前转动也可能产生相对线速度项。

每一级都用自己的坐标系表达

工程计算中常保存 \({}^{i}\mathbf v_i\)、\({}^{i}\boldsymbol\omega_i\)，即该连杆相对固定基座的速度，但以自身轴向给出分量。递推时需要转换坐标系：

\[\boxed{ {}^{i+1}\boldsymbol\omega_{i+1} ={} ^{i+1}R_i\,{}^i\boldsymbol\omega_i +\dot\theta_{i+1}\mathbf e_z} \]

\[\boxed{ {}^{i+1}\mathbf v_{i+1} ={} ^{i+1}R_i \left({}^i\mathbf v_i+{}^i\boldsymbol\omega_i\times{}^i\mathbf P_{i+1}\right) +\dot d_{i+1}\mathbf e_z} \]

其中 \(\mathbf e_z=[0,0,1]^T\)；\({}^{i+1}R_i=({}^{i}R_{i+1})^T\)，方向不要用反。

对于固定基座，从 \(\mathbf v_0=\mathbf0\)、\(\boldsymbol\omega_0=\mathbf0\) 开始，便可一路计算到末端。移动基座则必须加入基座本身的初始运动。

这里的“传播”是计算上的递推，不是说前面关节的运动要经过一段时间才传到后面。

11. 平面 3R 完整推导：把每一级贡献整理成雅可比

现在把递推方法完整用于三连杆机构。设 \(P_1\) 为固定的第一关节点，\(P_2,P_3\) 为后两个关节点，E 为第三连杆末端，连杆长度分别为 \(l_1,l_2,l_3\)。

所有关节轴均沿基座 z 轴正方向，三个关节角都是相对前一连杆的角度。记：

\[\theta_{12}=\theta_1+\theta_2,\qquad \theta_{123}=\theta_1+\theta_2+\theta_3. \]

进一步记 \(s_{123}=\sin\theta_{123}\)、\(c_{123}=\cos\theta_{123}\)，其他简写类似。

第一步：第一关节点不动，第一连杆转动

\[\mathbf v_{P_1}=\mathbf0,\qquad \boldsymbol\omega_1=\dot\theta_1\mathbf e_z. \]

固定点的线速度为零，与连杆角速度非零并不矛盾。

第二步：求 \(P_2\) 的线速度

\[\mathbf r_{12}=\begin{bmatrix}l_1c_1\\l_1s_1\\0\end{bmatrix}, \qquad \mathbf v_{P_2}=\boldsymbol\omega_1\times\mathbf r_{12} =\begin{bmatrix}-l_1s_1\\l_1c_1\\0\end{bmatrix}\dot\theta_1. \]

关节 2 不会改变其自身轴上的 \(P_2\)，所以这里没有 \(\dot\theta_2\)。

第三步：求 \(P_3\) 的线速度

第二连杆既受到关节 1 带动，又受到关节 2 自身转动影响：

\[\boldsymbol\omega_2=(\dot\theta_1+\dot\theta_2)\mathbf e_z, \qquad \mathbf r_{23}=\begin{bmatrix}l_2c_{12}\\l_2s_{12}\\0\end{bmatrix}. \]

因此：

\[\mathbf v_{P_3} =\begin{bmatrix}-l_1s_1\\l_1c_1\\0\end{bmatrix}\dot\theta_1 +\begin{bmatrix}-l_2s_{12}\\l_2c_{12}\\0\end{bmatrix}(\dot\theta_1+\dot\theta_2). \]

这里的第二项必须乘总角速度 \(\dot\theta_1+\dot\theta_2\)，不能只乘 \(\dot\theta_2\)，否则会漏掉第一关节对第二杆的带动。

第四步：继续算到第三杆末端 E

\[\boldsymbol\omega_3=(\dot\theta_1+\dot\theta_2+\dot\theta_3)\mathbf e_z, \qquad \mathbf r_{3E}=\begin{bmatrix}l_3c_{123}\\l_3s_{123}\\0\end{bmatrix}. \]

所以：

\[\begin{aligned} \mathbf v_E={}& \begin{bmatrix}-l_1s_1\\l_1c_1\\0\end{bmatrix}\dot\theta_1\\ &+\begin{bmatrix}-l_2s_{12}\\l_2c_{12}\\0\end{bmatrix}(\dot\theta_1+\dot\theta_2)\\ &+\begin{bmatrix}-l_3s_{123}\\l_3c_{123}\\0\end{bmatrix}(\dot\theta_1+\dot\theta_2+\dot\theta_3). \end{aligned} \]

第五步：按三个关节速度收集系数

任务取 \(\mathbf X=[x_E,y_E,\phi_E]^T\)，其中 \(\phi_E=\theta_{123}\)，则：

\[\boxed{ J_{\mathrm{planar}}= \begin{bmatrix} -l_1s_1-l_2s_{12}-l_3s_{123}&-l_2s_{12}-l_3s_{123}&-l_3s_{123}\\ l_1c_1+l_2c_{12}+l_3c_{123}&l_2c_{12}+l_3c_{123}&l_3c_{123}\\ 1&1&1 \end{bmatrix}} \]

\[\begin{bmatrix}\dot x_E\\\dot y_E\\\dot\phi_E\end{bmatrix} =J_{\mathrm{planar}} \begin{bmatrix}\dot\theta_1\\\dot\theta_2\\\dot\theta_3\end{bmatrix}. \]

这个例子中 \(\dot\phi_E=\omega_z\)，因此任务雅可比与只保留 \(v_x,v_y,\omega_z\) 三行的几何雅可比相同。

列	包含哪些连杆长度	原因
第 1 列	\(l_1,l_2,l_3\)	关节 1 带动其后全部连杆
第 2 列	\(l_2,l_3\)	关节 2 带动第二、第三连杆
第 3 列	\(l_3\)	关节 3 只带动第三连杆

也可以对 \(x_E=l_1c_1+l_2c_{12}+l_3c_{123}\)、\(y_E=l_1s_1+l_2s_{12}+l_3s_{123}\) 和 \(\phi_E=\theta_{123}\) 直接求导，会得到完全相同的结果。

至此，直接求导法与速度递推法就连在一起了：一个从位置方程出发，一个从刚体速度关系出发，最终描述的是同一个运动。

12. 不逐级展开，也能直接写出每一列

对于独立单自由度串联关节，在统一参考系下，设：

• \(\mathbf a_i\)：第 \(i\) 个关节的单位轴向；
• \(\mathbf o_i\)：该轴上的任一点；
• \(\mathbf p_E\)：所选末端点的位置。

则几何雅可比第 \(i\) 列为：

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

转动关节上半部分来自叉乘速度，下半部分来自绕轴转动；移动关节上半部分是沿轴的平移，下半部分为零。

为什么轴上的任一点都可以用？沿轴移动 \(\mathbf o_i\) 只会给位置差增加一个平行于 \(\mathbf a_i\) 的向量，而该向量与轴向的叉乘为零。

若用 D-H 数据提取关节轴，必须分清编号：

约定	第 \(i\) 个关节轴
标准 D-H	\(z_{i-1}\)
Craig 改进 D-H	\(z_i\)

为避免混乱，上面的通用公式使用 \(\mathbf a_i\) 表示“物理上的第 \(i\) 关节轴”，不绑定任何一种 D-H 下标。

所有列最后水平拼接：

\[J_g=\begin{bmatrix}\mathbf j_1&\cdots&\mathbf j_n\end{bmatrix}. \]

相邻连杆的变换能连乘，雅可比也能连乘吗？

通常不能这样做。齐次变换表示相邻坐标系的位姿关系，可以沿链连乘；末端雅可比的每一列描述各关节对同一个末端的贡献，要按列拼接，再乘关节速度相加。

当然，复合函数的雅可比可以按链式法则相乘，例如前面的 \(J_{\mathbf X}=EJ_g\)。但这不意味着可以任意把几个相邻连杆的雅可比相乘，得到机械臂末端雅可比。必须先确认它们的输入、输出是否真的构成函数复合。

13. 换表达坐标系，与换末端参考点，是两件事

同一个末端点，只换向量的表达坐标系

若递推结果在末端坐标系表达，换成基座坐标系时：

\[\begin{bmatrix}{}^0\mathbf v_E\\{}^0\boldsymbol\omega_E\end{bmatrix} = \begin{bmatrix}{}^0R_E&0\\0&{}^0R_E\end{bmatrix} \begin{bmatrix}{}^E\mathbf v_E\\{}^E\boldsymbol\omega_E\end{bmatrix}. \]

对应的几何雅可比也左乘同样的块对角矩阵。这里只换表达轴向，线速度仍属于同一个点 E，所以不加平移项。

从法兰原点换到工具尖端

若 F 与 E 固连在同一个末端刚体上，\(\mathbf r_{FE}\) 从法兰原点指向工具尖端，则：

\[\mathbf v_E=\mathbf v_F+\boldsymbol\omega\times\mathbf r_{FE} =\mathbf v_F-[\mathbf r_{FE}]_\times\boldsymbol\omega. \]

所以在相同表达坐标系下：

\[\boxed{ J_E= \begin{bmatrix}I&-[\mathbf r_{FE}]_\times\\0&I\end{bmatrix}J_F} \]

这说明“同一台机械臂的雅可比”还需要说明是针对哪个点。法兰原点与工具尖端的线速度雅可比通常不同。

进阶阅读时还会遇到空间旋量、体旋量及其伴随变换。某些教材采用 \([\boldsymbol\omega;\mathbf v]\) 的排列，而且空间旋量的线性部分不一定就是末端原点的线速度。使用别人的公式前，应先核对定义，不能只看它也是一个六维向量。

14. 能正算速度，也能反求速度，但反求不一定有唯一解

正向计算很直接：

\[\dot{\mathbf X}=J\dot{\mathbf q}. \]

反向则是在求解线性方程：

\[J\dot{\mathbf q}=\dot{\mathbf X}_{\mathrm{des}}. \]

情况	结果
\(J\) 是可逆方阵	任意目标输出速度有唯一关节速度解
目标速度不在 \(J\) 的列空间内	无精确解
有精确解且 \(J\) 有非零零空间	存在多个关节速度解

因此，不是“关节数比任务维数多就一定可行”，也不是“关节数比任务维数少就全部不可行”。要看目标方向是否落在列空间中，以及矩阵的秩。

常用伪逆给出：

\[\dot{\mathbf q}=J^\dagger\dot{\mathbf X}_{\mathrm{des}}. \]

它是在所采用的欧氏度量下，最小范数的最小二乘解。目标可实现时，能得到精确匹配；不可实现时，只能尽量逼近。

如果有冗余，还可以写成：

\[\dot{\mathbf q} =J^\dagger\dot{\mathbf X}_{\mathrm{des}} +(I-J^\dagger J)\mathbf u. \]

第二项处于零空间，不改变瞬时任务速度，可用于调整手肘、远离关节限位等。有限时间内仍需不断更新配置和雅可比，不能把“瞬时不影响末端”理解为一次大幅关节位移也完全不影响。

有速度限制、碰撞约束时，常需要带约束优化，而不只是求一个伪逆。

15. 奇异配置：哪些方向突然无法产生？

对于给定机构和任务，若雅可比的秩下降到低于它通常能够达到的最大值，就发生了运动学奇异。

不是所有矩阵“秩小于 6”都叫奇异。一个只研究二维位置的任务最多只有秩 2，这本身很正常。奇异性的核心是可实现的瞬时运动方向比通常少了。这与 Modern Robotics 对奇异性的说明一致。

用平面 2R 看清楚

对于前面的二维位置雅可比：

\[\det J_p=l_1l_2\sin\theta_2. \]

假设杆长非零，当 \(\theta_2=0\) 或 \(\pi\) 时，两杆共线，二维位置雅可比降秩。

例如两杆沿 x 正方向完全伸直：

\[J_p=\begin{bmatrix}0&0\\l_1+l_2&l_2\end{bmatrix}. \]

无论怎样组合瞬时关节速度，都有 \(\dot x=0\)。此刻只能产生 y 方向的一阶位置变化。

这不代表机械臂永远不能改变 x 坐标。它可以先弯曲，再产生不同方向的运动；从伸直位置开始的小角度弯曲也会产生二阶的 x 位移。雅可比说的是当前瞬间的一阶关系。

这个例子也提醒我们：奇异性与任务有关。如果把平面朝向也加入，2R 的 \(3\times2\) 雅可比在 \(l_1\ne0\) 时始终具有列秩 2。它本来就不能任意控制三个输出，不能把二维位置雅可比的降秩结论原封不动套过去。

机械臂奇异，与欧拉角表示奇异不同

• 机械臂奇异：几何映射的可实现方向减少。
• 表示奇异：例如欧拉角出现万向节锁，姿态参数本身无法正常描述某些变化，即使机械臂几何运动能力没有退化。

只看解析雅可比里的异常数值，可能混淆这两种问题。

为什么接近奇异点时，反求速度会变得很大？

奇异值分解 \(J=U\Sigma V^T\) 可以理解为把映射分解为若干方向上的伸缩。某个奇异值很小，意味着该方向上很大的关节运动只能产生很小的任务运动。反求时除以这个小数，就会放大关节速度和噪声。

常见的阻尼最小二乘形式是：

\[\dot{\mathbf q}=J^T(JJ^T+\lambda^2I)^{-1}\dot{\mathbf X}_{\mathrm{des}},\qquad\lambda>0. \]

阻尼通过允许一定跟踪误差来抑制放大，并不会创造原本不可实现的运动方向。

16. 加速度：为什么必须多出一项 \(\dot J\dot{\mathbf q}\)？

对速度关系求导：

\[\boxed{ \ddot{\mathbf X} =J(\mathbf q)\ddot{\mathbf q} +\dot J(\mathbf q,\dot{\mathbf q})\dot{\mathbf q}} \]

其中：

\[\dot J=\sum_{k=1}^{n}\frac{\partial J}{\partial q_k}\dot q_k. \]

第一项来自关节加速度，第二项来自机器人运动时，关节到末端的速度映射本身在变化。

问题	需要哪些已知量
求当前 \(J\)	配置 \(\mathbf q\) 和模型、任务定义
求当前 \(\dot J\)	还需要关节速度 \(\dot{\mathbf q}\)
求末端加速度	再加关节加速度 \(\ddot{\mathbf q}\)

关节匀速，末端也可以有加速度

长度为 \(l\) 的单杆匀速旋转：

\[\mathbf p=l\begin{bmatrix}\cos q\\\sin q\end{bmatrix},\qquad J_p=l\begin{bmatrix}-\sin q\\\cos q\end{bmatrix}. \]

即使 \(\ddot q=0\)，仍然有：

\[\ddot{\mathbf p}=-l\dot q^2\begin{bmatrix}\cos q\\\sin q\end{bmatrix}. \]

这正是朝向旋转中心的向心加速度。例如 \(l=2\,\mathrm m\)、\(\dot q=0.5\,\mathrm{rad/s}\)，加速度大小为 \(0.5\,\mathrm{m/s^2}\)。

反求关节加速度时，需要解：

\[J\ddot{\mathbf q}=\ddot{\mathbf X}_{\mathrm{des}}-\dot J\dot{\mathbf q}. \]

仍然可能出现无解或多解。如果 \(\mathbf X\) 包含欧拉角，这里的二阶导数是欧拉角二阶变化率，不应直接当成角加速度。

若使用基座表达的几何雅可比，则对其速度向量求导可得到 \([\mathbf a_E;\dot{\boldsymbol\omega}_E]\)。若用随动坐标系表达，向量分量的时间导数与物理加速度之间还要考虑坐标轴本身的转动。

17. 力与力矩：为什么会出现雅可比的转置？

速度关系为：

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

在同一作用点和参考系下，把末端力与力矩组成力旋量：

\[\mathbf w=\begin{bmatrix}\mathbf F\\\mathbf M\end{bmatrix}. \]

外力对末端运动做功的瞬时功率是：

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

因此该外载荷对应的关节广义力为：

\[\boxed{\boldsymbol\tau_{\mathrm{ext}}=J_g^T\mathbf w_{\mathrm{ext}}}. \]

R 关节的广义力是力矩，P 关节的广义力是轴向力。雅可比转置的静力映射可参考 Modern Robotics：开链机构静力学。

符号要看力的定义：如果 \(\mathbf w_{\mathrm{ext}}\) 是环境施加在机器人上的外载荷，忽略重力等其他影响时，静止保持需要驱动力矩 \(\boldsymbol\tau_{\mathrm{act}}=-J_g^T\mathbf w_{\mathrm{ext}}\)。如果讨论机器人向环境施加的输出力，则要按作用与反作用调整符号。

实际电机力矩还可能包含重力补偿、惯性、科氏与离心项、摩擦等。不能把 \(J_g^T\mathbf w\) 当作完整的动力学模型。

此外，物理力旋量不能不加区分地乘以欧拉角解析雅可比的转置。转置映射依赖功率配对；使用任务坐标变化率时，其对偶的广义任务力也要匹配转换。

18. 除了速度和力，雅可比还能做什么？

用途	基本思路	实际意义
数值逆运动学	用局部误差求关节修正量	迭代到达目标位置或姿态
冗余运动协调	利用零空间	调整手肘、远离关节限位
灵巧度分析	分析奇异值和可操作性椭球	判断当前配置朝哪个方向容易运动
误差传播	\(\Delta\mathbf X\approx J\Delta\mathbf q\)	估计关节误差引起的末端误差
协方差传播	\(\Sigma_X\approx J\Sigma_qJ^T\)	一阶估计定位不确定性
柔顺与力控制	将任务空间要求映射到关节空间	控制接触力或表现出指定的柔顺性

数值逆运动学常写作：

\[\mathbf q_{k+1}=\mathbf q_k+\eta J^\dagger(\mathbf q_k)\mathbf e_k. \]

这里的误差 \(\mathbf e_k\) 必须与雅可比输出匹配。只控制笛卡尔位置时可以使用目标位置减当前位置；对于姿态，通常需要合理的旋转误差定义，不能对四元数或旋转矩阵随意相减后直接套用。

分析奇异值、最小范数和灵巧度时，还要注意单位。一个任务同时包含米和弧度，或者关节同时包含 R 与 P 类型，直接使用未经归一化的欧氏范数会隐含权重选择。工程上常引入特征长度、速度尺度或权重矩阵。

19. 实际编程时，怎样避免“公式对了，结果却错了”？

下面的流程适用于从几何关系构造末端雅可比：

1. 明确目标点：是法兰原点、夹爪中心，还是工具尖端？
2. 明确输出：是位置变化率、位置加姿态参数变化率，还是线速度加角速度？
3. 明确表达坐标系和排列顺序，例如基座表达的 \([\mathbf v;\boldsymbol\omega]\)。
4. 读取当前关节配置，完成正运动学，取得各关节轴和目标点位置。
5. 将所有轴向、位置统一到同一坐标系。
6. 按 R/P 类型构造每列，水平拼接成 \(J_g\)。
7. 如有需要，再转换为目标坐标参数对应的 \(J_{\mathbf X}\)。
8. 反求时处理奇异性、权重、关节速度限制和其他约束。

用数值差分核对位置雅可比

正运动学能独立验证雅可比。对第 \(i\) 个关节作小的正负扰动：

\[J_{p,i}\approx \frac{\mathbf p(\mathbf q+h\mathbf e_i)-\mathbf p(\mathbf q-h\mathbf e_i)}{2h}. \]

这里的 \(\mathbf e_i\) 是关节空间第 \(i\) 个标准基向量，与前文的三维轴向 \(\mathbf e_z\) 不同。步长 \(h\) 按该关节的单位选取，过大会带来截断误差，过小会放大浮点误差。

对旋转不能直接把“旋转矩阵九元素的差分”当成三维角速度。若 \(R\) 把工具坐标转换到基座表达，则：

\[[{}^0\boldsymbol\omega]_\times=\dot R R^T, \qquad [{}^E\boldsymbol\omega]_\times=R^T\dot R. \]

可先通过差分得到 \(\dot R\) 的近似，再按所需表达坐标系恢复角速度。实际计算时可取矩阵的反对称部分来抑制数值误差。

20. 一张表回顾最容易混淆的地方

常见误解	更准确的理解
雅可比一定是 6×6	行数取决于任务表示，列数取决于独立关节变量数
模型建好后雅可比就是常数	表达式确定，数值通常随配置变化
雅可比取决于运动快慢	\(J\) 不直接依赖速度，\(\dot J\) 才涉及速度
同一末端位姿必然有同一个雅可比	不同逆解配置可能有不同雅可比
欧拉角导数就是角速度	三维情况下通常需要转换矩阵
相邻连杆雅可比像齐次矩阵一样连乘	末端雅可比通常由各关节对同一末端的贡献按列组成
转动关节自身原点一定有线速度	如果原点在该转轴上，本关节转动不直接移动这个原点
用哪个坐标系表达，就是相对哪个坐标系运动	表达坐标系与相对运动关系是两件事
反求关节速度总能用普通逆矩阵	可能无解、多解或接近奇异，需要伪逆、阻尼或约束优化
奇异时机器人完全不能动	通常是某些瞬时任务方向无法实现
末端加速度等于 \(J\ddot{\mathbf q}\)	还要加 \(\dot J\dot{\mathbf q}\)
\(J^T\mathbf w\) 就是实际电机力矩	它只给出匹配载荷的广义力贡献，还需考虑符号及动力学

最值得亲手练习的是本文的平面 3R 例子：先对正运动学求导，再用角速度与叉乘逐段递推，最后用每个关节轴到末端的几何关系直接构造列。三条路线得到同一个矩阵时，雅可比就不再只是需要背诵的公式，而成为能看见几何含义、也能用于计算的工具。

---