雅可比矩阵（Jacobian Matrix）脱胎于多变量微积分，是描述向量值函数一阶导数的核心数学工具，其命名源自19世纪德国数学家卡尔·古斯塔夫·雅可比（Carl Gustav Jacob Jacobi）。
作为非线性系统局部线性化的标准手段，雅可比矩阵的应用横跨多个学科领域：

• 控制科学：非线性系统线性化、扩展卡尔曼滤波状态估计
• 机器人学：微分运动学、静力学映射、逆运动学求解、奇异分析
• 数值优化：非线性最小二乘、高斯-牛顿法、LM算法
• 机器学习：自动微分、反向传播、归一化流概率密度变换
• 几何计算：坐标变换、误差协方差传播

---

一、数学基础：

1.1 数学定义

设存在多输入多输出的向量值函数：
$$\mathbf{y}=\mathbf{f}(\mathbf{x})$$
其中输入向量 $\mathbf{x}\in {\mathbb{R}}^n$，输出向量 $\mathbf{y}\in {\mathbb{R}}^m$，即映射关系为 $\mathbf{f}:{\mathbb{R}}^n\to {\mathbb{R}}^m$。

函数 $\mathbf{f}$ 在点 $\mathbf{x}$ 处的雅可比矩阵定义为所有一阶偏导数构成的矩阵：
$$J_{\mathbf{f}}(\mathbf{x})=\frac{\partial \mathbf{f}}{\partial \mathbf{x}}=\left[\begin{array}{cccc}\frac{\partial f_1}{\partial x_1}& \frac{\partial f_1}{\partial x_2}& \cdots & \frac{\partial f_1}{\partial x_n}\\ \frac{\partial f_2}{\partial x_1}& \frac{\partial f_2}{\partial x_2}& \cdots & \frac{\partial f_2}{\partial x_n}\\ ⋮& ⋮& \ddots & ⋮\\ \frac{\partial f_m}{\partial x_1}& \frac{\partial f_m}{\partial x_2}& \cdots & \frac{\partial f_m}{\partial x_n}\end{array}\right]$$

雅可比矩阵是一个 $m\times n$ 的矩阵，满足核心维度约定：

• 行数 = 输出空间维数
• 列数 = 输入空间维数

这一约定为数学与工程界的通用标准，SciPy官方文档同样采用该定义：对于映射 $f:{\mathbb{R}}^n\to {\mathbb{R}}^m$，雅可比矩阵形状为 $m\times n$，元素 $J_{ij}$ 表示第 $i$ 个输出分量对第 $j$ 个输入变量的偏导数。

1.2 本质：非线性函数的局部线性化

雅可比矩阵形式上是“将偏导数排列成矩阵”，而核心意义是在指定工作点附近对非线性函数进行一阶泰勒近似，实现局部线性化。

对于一元函数 $y=f(x)$，在 $x$ 附近的增量满足：
$$f(x+\Delta x)\approx f(x)+f^{\prime }(x)\Delta x$$

推广至向量值函数，增量形式为：
$$\mathbf{f}(\mathbf{x}+\Delta \mathbf{x})=\mathbf{f}(\mathbf{x})+J_{\mathbf{f}}(\mathbf{x})\Delta \mathbf{x}+o(|\Delta \mathbf{x}|)$$

忽略高阶无穷小项后，得到最核心的线性近似关系：
$$\Delta \mathbf{y}\approx J(\mathbf{x})\Delta \mathbf{x}$$

该式的物理含义十分直观：当输入在当前点 $\mathbf{x}$ 附近发生微小变化 $\Delta \mathbf{x}$ 时，输出的变化量近似等于雅可比矩阵与输入增量的乘积。

机器人学中的微分运动学，本质上就是对正运动学这一非线性函数应用该线性近似，从而建立关节速度与末端速度的映射关系。

局部性的直观解释

雅可比的线性近似仅在工作点的小邻域内有效。以函数 $y=x^2$ 为例，在 $x=2$ 处导数为4，即 $\Delta y\approx 4\Delta x$：

• 当 $\Delta x=0.01$ 时，近似值 $\Delta y=0.04$，真实值为 $2.01^2-2^2=0.0401$，误差极小；
• 当 $\Delta x=10$ 时，近似值 $\Delta y=40$，真实值为 $12^2-2^2=140$，误差极大。

这充分说明：雅可比矩阵描述的是当前工作点附近的小范围变化关系，而非全局映射规律。

1.3 行与列的物理意义

对于雅可比矩阵 $J\in {\mathbb{R}}^{m\times n}$，其行与列分别对应不同的物理含义：

每一行：单个输出的梯度

第 $i$ 行向量 $J_{i,:}$ 为：
$$\left[\begin{array}{cccc}\frac{\partial f_i}{\partial x_1}& \frac{\partial f_i}{\partial x_2}& \cdots & \frac{\partial f_i}{\partial x_n}\end{array}\right]$$
它描述了第 $i$ 个输出分量对所有输入变量的敏感程度，本质上是标量函数 $f_i$ 的梯度转置：
$$J_{i,:}=\nabla f_i^T$$

每一列：单个输入的影响

第 $j$ 列向量 $J_{:,j}$ 为：
$$J_{:,j}=\left[\begin{array}{c}\frac{\partial f_1}{\partial x_j}\\ \frac{\partial f_2}{\partial x_j}\\ ⋮\\ \frac{\partial f_m}{\partial x_j}\end{array}\right]$$
它描述了仅第 $j$ 个输入发生单位微小变化时，所有输出分量的变化方向与幅度。

在机器人学中这一解释尤为关键：机器人雅可比的第 $j$ 列，对应第 $j$ 个关节以单位速度运动、其余关节静止时，末端执行器产生的运动速度，这也是螺旋理论构造雅可比的核心依据。

1.4 方向导数：JVP与VJP

在工程实践中，完整计算大规模雅可比矩阵往往计算代价极高，多数场景仅需计算雅可比与向量的乘积，无需显式构造完整矩阵。

雅可比-向量乘积（JVP）

给定方向向量 $\mathbf{v}\in {\mathbb{R}}^n$，雅可比-向量乘积定义为：
$$J(\mathbf{x})\mathbf{v}={\frac{d}{dϵ}\mathbf{f}(\mathbf{x}+ϵ\mathbf{v})|}_{ϵ=0}$$
它表示函数沿方向 $\mathbf{v}$ 的方向导数。

机器人中的正微分运动学 $\dot{\mathbf{x}}=J\dot{\mathbf{q}}$ 就是典型的JVP运算：无需逐元素求导构造完整雅可比，仅需通过运动学链传播关节速度，即可直接得到末端速度。JVP对应自动微分中的前向模式（Forward Mode）。

向量-雅可比乘积（VJP）

给定向量 $\mathbf{u}\in {\mathbb{R}}^m$，向量-雅可比乘积定义为：
$$J^T\mathbf{u}={\nabla }_{\mathbf{x}}\left({\mathbf{u}}^T\mathbf{f}(\mathbf{x})\right)$$
它表示将输出空间的向量反向传播至输入空间，对应自动微分中的反向模式（Reverse Mode）。

机器人中的静力学映射 $\mathbf{\tau }=J^T\mathbf{W}$、深度学习中的反向传播算法，本质上都属于VJP运算。JAX官方文档明确将jacfwd（前向模式）与JVP对应、jacrev（反向模式）与VJP对应，为不同维度的雅可比计算提供最优方案。

1.5 梯度、雅可比与Hessian的辨析

三者均为多元函数的导数概念，但适用场景与维度完全不同，是极易混淆的基础概念：

梯度（Gradient）

针对标量函数 $f:{\mathbb{R}}^n\to \mathbb{R}$，梯度是 $n$ 维列向量：
$$\nabla f=\left[\begin{array}{c}\frac{\partial f}{\partial x_1}\\ ⋮\\ \frac{\partial f}{\partial x_n}\end{array}\right]$$
在“输出为行、输入为列”的雅可比约定下，标量函数的雅可比是其梯度的转置：
$$J_f=\nabla f^T$$

雅可比（Jacobian）

针对向量值函数 $\mathbf{f}:{\mathbb{R}}^n\to {\mathbb{R}}^m$，一阶导数为 $m\times n$ 的雅可比矩阵。

Hessian矩阵

针对标量函数 $f:{\mathbb{R}}^n\to \mathbb{R}$，Hessian是二阶偏导数构成的 $n\times n$ 方阵：
$$H_f=\left[\begin{array}{ccc}\frac{{\partial }^{2}f}{\partial x_1^2}& \cdots & \frac{{\partial }^{2}f}{\partial x_1\partial x_n}\\ ⋮& \ddots & ⋮\\ \frac{{\partial }^{2}f}{\partial x_n\partial x_1}& \cdots & \frac{{\partial }^{2}f}{\partial x_n^2}\end{array}\right]$$
Hessian可以理解为“梯度函数的雅可比”，即 $H_f=J_{\nabla f}$。

对于向量值函数，每个输出分量都对应一个Hessian矩阵，因此二阶导数本质上是 $m\times n\times n$ 的三阶张量。JAX官方文档同样明确了该维度约定：向量函数一阶导数形状为 $m\times n$，二阶导数形状为 $m\times n\times n$。

1.6 链式法则

设存在两层复合映射：
$$\mathbf{y}=\mathbf{f}(\mathbf{x}),\mathbf{z}=\mathbf{g}(\mathbf{y})$$
即复合函数为 $\mathbf{z}=\mathbf{g}(\mathbf{f}(\mathbf{x}))$，则复合函数的雅可比满足：
$$J_{\mathbf{g}\circ \mathbf{f}}=J_{\mathbf{g}}(\mathbf{f}(\mathbf{x}))\cdot J_{\mathbf{f}}(\mathbf{x})$$

矩阵乘法顺序不可颠倒，维度匹配关系为：若 $\mathbf{x}\in {\mathbb{R}}^n$、$\mathbf{y}\in {\mathbb{R}}^m$、$\mathbf{z}\in {\mathbb{R}}^p$，则 $J_f\in {\mathbb{R}}^{m\times n}$、$J_g\in {\mathbb{R}}^{p\times m}$，最终复合雅可比 $J_g{J}_f\in {\mathbb{R}}^{p\times n}$。

机器人正运动学本质上是多级刚体变换的复合，因此机械臂雅可比既可以通过对正运动学表达式求导得到，也可以通过链式递推或螺旋理论逐列构造。

1.7 雅可比行列式：面积/体积缩放与可逆性

只有当输入与输出维数相等（$n=m$）时，雅可比矩阵为方阵，才可计算行列式 $\det J$。

局部几何缩放

在坐标变换中，雅可比行列式的绝对值 $|\det J|$ 表示局部面积/体积的缩放倍数：

• 二维场景：微小面积元 $dxdy$ 经过变换后，面积近似乘以 $|\det J|$；
• 三维及更高维：微小体积元经过变换后，体积近似乘以 $|\det J|$。

MIT多变量微积分课程的坐标变换章节，正是以雅可比行列式为核心工具描述面积/体积元的映射关系。

行列式的符号与几何意义

• $\det J>0$：变换保持局部空间方向；
• $\det J<0$：变换发生局部方向翻转；
• $\det J=0$：局部空间被压缩至更低维度，面积/体积变为0。

局部可逆性

根据逆函数定理，若在点 ${\mathbf{x}}_0$ 处满足 $\det J({\mathbf{x}}_0)\ne 0$，则函数在该点的邻域内局部可逆。

需要特别注意：局部可逆 ≠ 全局可逆。例如函数 $y=x^2$ 在 $x=1$ 处雅可比行列式不为零，局部可逆，但在整个实数域上并非一一映射。

机器人正运动学同理：某一位姿下雅可比满秩仅说明可进行局部逆运动学求解，不代表整个工作空间内该位姿对应唯一关节解。

延伸：概率密度变换

雅可比行列式是概率统计中随机变量变换的核心工具。若随机变量 $\mathbf{X}$ 服从概率密度 $p_X(\mathbf{x})$，经过可逆变换 $\mathbf{y}=\mathbf{f}(\mathbf{x})$ 得到随机变量 $\mathbf{Y}$，则 $\mathbf{Y}$ 的概率密度满足：
$$p_Y(\mathbf{y})=p_X({\mathbf{f}}^{-1}(\mathbf{y}))\cdot \left|\det J_{{\mathbf{f}}^{-1}}(\mathbf{y})\right|$$
这一性质是归一化流（Normalizing Flow）等深度生成模型的理论基础，广泛应用于生成式AI领域。

---

二、机器人学：微分运动学与静力学

2.1 机器人雅可比的基本定义

设机器人关节变量向量为 $\mathbf{q}={\left[\begin{array}{cccc}{q}_1& q_2& \cdots & q_n\end{array}\right]}^T$，末端任务变量（位姿）为 $\mathbf{x}=\mathbf{f}(\mathbf{q})$，其中 $\mathbf{f}$ 为正运动学函数。

对时间求导可得速度映射关系：
$$\dot{\mathbf{x}}=\frac{\partial \mathbf{f}}{\partial \mathbf{q}}\dot{\mathbf{q}}$$

定义机器人雅可比矩阵为正运动学函数对关节变量的偏导：
$$J(\mathbf{q})=\frac{\partial \mathbf{f}}{\partial \mathbf{q}}$$

由此得到微分运动学核心方程：
$$\dot{\mathbf{x}}=J(\mathbf{q})\dot{\mathbf{q}}$$

雅可比矩阵是连接关节空间与笛卡尔任务空间的核心桥梁，它不仅将关节速度映射为末端速度，还可通过转置将末端广义力（wrench）映射为关节广义力矩。

机器人雅可比重在回答以下核心问题：

1. 单个关节微小运动时，末端的运动方向与幅度；
2. 给定关节速度组合，对应的末端运动速度；
3. 期望末端沿指定方向运动时，各关节的速度配合关系；
4. 末端受到外力/力矩时，各关节需要承担的驱动力矩；
5. 当前位姿是否接近奇异位形；
6. 当前位姿下，末端在哪些方向更易运动或施力。

2.2 案例：平面二连杆机械臂推导

以最经典的二自由度平面机械臂为例，完整推导其位置雅可比。设连杆长度为 $l_1,l_2$，关节角为 $q_1,q_2$，末端坐标为 $(x,y)$。

正运动学方程为：
$$\{\begin{array}{l}x=l_1\cos q_1+l_2\cos (q_1+q_2)\\ y=l_1\sin q_1+l_2\sin (q_1+q_2)\end{array}$$

写成向量形式为 $\mathbf{p}=\left[\begin{array}{c}x\\ y\end{array}\right]=\mathbf{f}(q_1,q_2)$，雅可比矩阵为 $2\times 2$ 方阵：
$$J=\left[\begin{array}{cc}\frac{\partial x}{\partial q_1}& \frac{\partial x}{\partial q_2}\\ \frac{\partial y}{\partial q_1}& \frac{\partial y}{\partial q_2}\end{array}\right]$$

对各分量求偏导：
$$\begin{array}{rl}\frac{\partial x}{\partial q_1}& =-l_1\sin q_1-l_2\sin (q_1+q_2)\\ \frac{\partial x}{\partial q_2}& =-l_2\sin (q_1+q_2)\\ \frac{\partial y}{\partial q_1}& =l_1\cos q_1+l_2\cos (q_1+q_2)\\ \frac{\partial y}{\partial q_2}& =l_2\cos (q_1+q_2)\end{array}$$

最终得到二连杆平面机械臂的位置雅可比：
$$\boxed{J=\left[\begin{array}{cc}-l_1\sin q_1-l_2\sin (q_1+q_2)& -l_2\sin (q_1+q_2)\\ l_1\cos q_1+l_2\cos (q_1+q_2)& l_2\cos (q_1+q_2)\end{array}\right]}$$

速度映射关系为：
$$\left[\begin{array}{c}\dot{x}\\ \dot{y}\end{array}\right]=J\left[\begin{array}{c}{\dot{q}}_1\\ {\dot{q}}_2\end{array}\right]$$

也可按列分解为：
$$\dot{\mathbf{p}}=J_1{\dot{q}}_1+J_2{\dot{q}}_2$$
其中 $J_1$ 为关节1单位转速产生的末端速度，$J_2$ 为关节2单位转速产生的末端速度，与“雅可比列向量对应单关节单位速度”的物理解释完全一致。

2.3 关节类型与雅可比列向量的几何构造

对于串联机器人，无需展开正运动学求导，可直接通过关节轴的几何关系逐列构造六维几何雅可比。本文采用通用的运动旋量（twist）排列顺序：
$$\mathbf{V}=\left[\begin{array}{c}\mathbf{v}\\ \mathbf{\omega }\end{array}\right]$$
其中 $\mathbf{v}$ 为末端线速度，$\mathbf{\omega }$ 为末端角速度。

注意：部分教材与函数库采用 $[\mathbf{\omega };\mathbf{v}]$ 的排列顺序，工程使用前必须确认接口约定，避免线速度与角速度混淆。

旋转关节（Revolute Joint）

设第 $i$ 个旋转关节的关节轴方向单位向量为 ${\mathbf{z}}_{i-1}$，关节轴上任意一点的位置为 ${\mathbf{p}}_{i-1}$，末端执行器位置为 ${\mathbf{p}}_e$，则雅可比第 $i$ 列为：
$$\boxed{J_i=\left[\begin{array}{c}{\mathbf{z}}_{i-1}\times ({\mathbf{p}}_e-{\mathbf{p}}_{i-1})\\ {\mathbf{z}}_{i-1}\end{array}\right]}$$

上半部分线速度源自刚体定轴转动公式 $\mathbf{v}=\mathbf{\omega }\times \mathbf{r}$：关节以单位角速度旋转时，$\mathbf{\omega }={\mathbf{z}}_{i-1}$，关节轴到末端的径向向量为 $\mathbf{r}={\mathbf{p}}_e-{\mathbf{p}}_{i-1}$，因此线速度为 ${\mathbf{v}}_i={\mathbf{z}}_{i-1}\times ({\mathbf{p}}_e-{\mathbf{p}}_{i-1})$；下半部分角速度即为关节轴方向。

移动关节（Prismatic Joint）

移动关节仅产生平动，不直接带动末端转动，因此雅可比第 $i$ 列为：
$$\boxed{J_i=\left[\begin{array}{c}{\mathbf{z}}_{i-1}\\ 0\end{array}\right]}$$
其中 ${\mathbf{z}}_{i-1}$ 为移动关节的运动方向。

2.4 位置雅可比与几何雅可比

根据任务空间的维度，机器人雅可比可分为两类：

位置雅可比

若仅关注末端位置 $\mathbf{p}\in {\mathbb{R}}^3$，不考虑姿态，则使用位置雅可比 $J_v\in {\mathbb{R}}^{3\times n}$，满足：
$$\dot{\mathbf{p}}=J_v\dot{\mathbf{q}}$$

几何雅可比

若同时考虑末端位置与姿态，使用六维几何雅可比（Geometric Jacobian）：
$$\left[\begin{array}{c}\mathbf{v}\\ \mathbf{\omega }\end{array}\right]=\left[\begin{array}{c}{J}_v\\ J_{\omega }\end{array}\right]\dot{\mathbf{q}}$$
即完整几何雅可比 $J_g=\left[\begin{array}{c}{J}_v\\ J_{\omega }\end{array}\right]\in {\mathbb{R}}^{6\times n}$。

对于六自由度工业机械臂，几何雅可比为 $6\times 6$ 方阵，但这并不意味着其在所有位姿下都可逆，奇异位形下矩阵秩会下降。

2.5 空间雅可比与本体雅可比

末端运动旋量可以在不同参考坐标系中表达，由此衍生出两类雅可比：

空间雅可比（Space Jacobian）

末端运动旋量在固定基坐标系（空间坐标系）中表达：
$${\mathbf{V}}_s=J_s(\mathbf{q})\dot{\mathbf{q}}$$
空间雅可比形状为 $6\times n$，其第 $i$ 列的运动学意义仅受该关节之前的连杆位姿影响。

本体雅可比（Body Jacobian）

末端运动旋量在末端自身连杆坐标系（本体坐标系）中表达：
$${\mathbf{V}}_b=J_b(\mathbf{q})\dot{\mathbf{q}}$$
本体雅可比形状同样为 $6\times n$，其列向量在末端坐标系中描述，第 $i$ 列的运动学意义主要受该关节之后的连杆配置影响。

二者的转换关系

设末端坐标系相对于空间坐标系的位姿为 $T_{sb}\in SE(3)$，运动旋量可通过伴随矩阵（Adjoint）进行坐标变换：
$${\mathbf{V}}_s={\mathrm{Ad}}_{T_{sb}}{\mathbf{V}}_b$$

因此两类雅可比满足转换关系：
$$\boxed{J_s={\mathrm{Ad}}_{T_{sb}}{J}_b}$$

本质上，空间雅可比与本体雅可比并非两个不同的物理量，而是同一末端运动在不同参考坐标系下的数学表达。

2.6 几何雅可比与解析雅可比

几何雅可比

几何雅可比输出的是刚体真实的线速度与角速度，对应物理上可直接测量的运动：
$$\left[\begin{array}{c}\mathbf{v}\\ \mathbf{\omega }\end{array}\right]=J_g\dot{\mathbf{q}}$$
其中 $\mathbf{\omega }$ 是刚体瞬时角速度矢量。

解析雅可比

若末端姿态采用欧拉角、RPY角等最小参数化表示，设姿态角向量为 $\mathbf{\varphi }={\left[\begin{array}{ccc}r& p& y\end{array}\right]}^T$，定义任务坐标 $\mathbf{x}=\left[\begin{array}{c}\mathbf{p}\\ \mathbf{\varphi }\end{array}\right]$，则：
$$\dot{\mathbf{x}}=\left[\begin{array}{c}\dot{\mathbf{p}}\\ \dot{\mathbf{\varphi }}\end{array}\right]=J_a\dot{\mathbf{q}}$$
其中 $J_a$ 称为解析雅可比（Analytic Jacobian）。

核心区别：欧拉角变化率 ≠ 角速度

一般情况下，欧拉角的时间导数不等于刚体角速度，即：
$$\dot{\mathbf{\varphi }}\ne \mathbf{\omega }$$

二者之间存在姿态相关的转换矩阵 $E(\mathbf{\varphi })$：
$$\mathbf{\omega }=E(\mathbf{\varphi })\dot{\mathbf{\varphi }}$$

因此几何雅可比与解析雅可比满足：
$$J_g=\left[\begin{array}{cc}I& 0\\ 0& E(\mathbf{\varphi })\end{array}\right]J_a$$

Pinocchio官方文档单独提供了RPY角变化率到角速度的转换雅可比，明确指出二者不可直接等同，需通过额外的变换矩阵进行转换。

两类奇异性

• 机构运动学奇异性：几何雅可比降秩，由机器人机构本身的位形导致，是真实的运动能力丧失；
• 姿态表示奇异性：解析雅可比因欧拉角参数化本身出现奇异（如俯仰角接近 $\pm \pi /2$ 时的万向锁），属于参数化导致的数值奇异，并非机构本身失去运动能力。

两类奇异可能同时出现，也可能单独出现，工程分析中必须加以区分。

2.7 参考坐标系与参考点的工程影响

雅可比矩阵的数值不仅取决于机器人关节位形，还与三个工程约定强相关：

1. 速度/力所参考的坐标系（基坐标系、末端坐标系等）；
2. 雅可比对应末端连杆上的参考点（法兰中心、TCP尖端等）；
3. 运动旋量中线速度与角速度的排列顺序。

例如同一末端连杆，分别对法兰原点和法兰前方100mm的焊枪TCP尖端求雅可比，二者角速度部分完全相同，但线速度部分存在差异。这是因为刚体上两点的速度满足：
$${\mathbf{v}}_B={\mathbf{v}}_A+\mathbf{\omega }\times {\mathbf{r}}_{AB}$$

更换参考点仅影响雅可比的线速度部分 $J_v$，不影响角速度部分 $J_{\omega }$。

主流机器人库均对参考系与参考点做了明确区分：MoveIt的RobotState::getJacobian()接口要求指定关节组、目标连杆及链路上的参考点；Pinocchio则提供LOCAL、WORLD、LOCAL_WORLD_ALIGNED等多种参考坐标系选项。

2.8 加速度映射与雅可比时间导数

已知速度映射关系 $\dot{\mathbf{x}}=J(\mathbf{q})\dot{\mathbf{q}}$，对时间再次求导可得加速度映射：
$$\ddot{\mathbf{x}}=\frac{d}{dt}\left(J(\mathbf{q})\dot{\mathbf{q}}\right)$$

展开后得到：
$$\boxed{\ddot{\mathbf{x}}=J(\mathbf{q})\ddot{\mathbf{q}}+\dot{J}(\mathbf{q},\dot{\mathbf{q}})\dot{\mathbf{q}}}$$

其中雅可比的时间导数 $\dot{J}$ 满足：
$$\dot{J}=\sum _{i=1}^n\frac{\partial J}{\partial q_i}{\dot{q}}_i$$

两项的物理含义分别为：

• $J\ddot{\mathbf{q}}$：关节加速度直接产生的末端加速度；
• $\dot{J}\dot{\mathbf{q}}$：机器人位姿变化导致雅可比本身改变，进而产生的速度耦合项。

即使关节速度恒定（$\ddot{\mathbf{q}}=0$），末端加速度也可能不为零，正是因为耦合项的存在。

Pinocchio提供了frame Jacobian时间变化量的计算接口，Orocos KDL也提供ChainJntToJacDotSolver专门用于求解 $\dot{J}$。

2.9 静力学对偶：雅可比转置与力映射

雅可比矩阵不仅映射速度，其转置还可实现力/力矩的空间映射，这一关系可由功率守恒或虚功原理推导。

设末端广义力（wrench）为 $\mathbf{W}=\left[\begin{array}{c}\mathbf{f}\\ \mathbf{m}\end{array}\right]$，其中 $\mathbf{f}$ 为末端作用力，$\mathbf{m}$ 为末端作用力矩；关节广义驱动力为 $\mathbf{\tau }$。

根据功率守恒，关节空间功率等于笛卡尔空间功率：
$${\mathbf{\tau }}^T\dot{\mathbf{q}}={\mathbf{W}}^T\mathbf{V}$$

代入速度映射 $\mathbf{V}=J\dot{\mathbf{q}}$，可得：
$${\mathbf{\tau }}^T\dot{\mathbf{q}}={\mathbf{W}}^{T}J\dot{\mathbf{q}}$$

由于该式对任意 $\dot{\mathbf{q}}$ 成立，因此得到静力学核心方程：
$$\boxed{\mathbf{\tau }=J^T\mathbf{W}}$$

该结论同样可由虚功原理推导，是机器人力控制的核心基础。MIT Underactuated Robotics课程与《Modern Robotics》均采用该推导方式，建立笛卡尔力到关节力矩的映射关系。

注意：若 $\mathbf{W}$ 定义为“环境施加给机器人的外力”，而 $\mathbf{\tau }$ 定义为“电机平衡外力所需的力矩”，公式可能因受力方向约定带有负号，工程应用中需确认符号定义。

2.10 奇异位形分析

奇异的通用定义

雅可比奇异并非简单的“行列式为零”——因为机器人雅可比常为非方阵。更通用的定义是：
$$\mathrm{rank}(J)<\text{机器人正常位形下的最大秩}$$
即雅可比矩阵失去满秩，列空间维数下降。

列空间视角的解释

所有关节速度组合能产生的末端速度，都属于雅可比的列空间 $\mathrm{Col}(J)$。当发生奇异时，列空间维数降低，机械臂丧失部分瞬时运动方向。
《Modern Robotics》正是将奇异位姿定义为雅可比列空间维数从最大值下降的关节配置。

二连杆机械臂的奇异条件

对前述二连杆位置雅可比求行列式：
$$\det J=l_1{l}_2\sin q_2$$

因此当 $q_2=0$（完全展开）或 $q_2=\pi$（完全折叠）时，$\det J=0$，机械臂进入奇异位形。此时两根连杆共线，雅可比两列向量平行，机械臂失去沿连杆方向的瞬时运动能力。

延伸：六轴工业机器人的典型奇异

工业六轴机器人常见三类奇异位形：

1. 腕部奇异：腕部轴线与肩部轴线共线，腕部关节失去绕自身轴线的转动能力；
2. 肘部奇异：肘关节完全伸展或折叠，与二连杆奇异原理一致；
3. 肩部奇异：末端位置落在肩部关节轴线上，肩关节失去绕竖直轴的转动能力。

接近奇异的工程现象

若采用 $\dot{\mathbf{q}}=J^{-1}{\mathbf{V}}_d$ 进行逆运动学求解，当雅可比接近奇异时，逆矩阵元素数值会急剧增大，工程中表现为：

• 关节速度指令突增，超出电机限速；
• 路径跟踪误差显著增大；
• 机器人出现抖动与冲击；
• 控制指令不连续，数值稳定性恶化；
• 测量噪声被大幅放大；
• 迭代式逆运动学收敛困难。

2.11 SVD：雅可比分析的核心工具

奇异值分解（Singular Value Decomposition, SVD）是分析雅可比矩阵性质最强大的工具。对雅可比矩阵做SVD：
$$\boxed{J=U\Sigma V^T}$$
其中：

• $U\in {\mathbb{R}}^{m\times m}$：任务空间的正交基矩阵，列向量为左奇异向量；
• $V\in {\mathbb{R}}^{n\times n}$：关节空间的正交基矩阵，列向量为右奇异向量；
• $\Sigma \in {\mathbb{R}}^{m\times n}$：对角矩阵，对角元素为奇异值，按降序排列 ${\sigma }_1\ge {\sigma }_2\ge \cdots \ge {\sigma }_r>0$。

奇异值的物理意义

若关节速度沿第 $i$ 个右奇异向量方向，即 $\dot{\mathbf{q}}={\mathbf{v}}_i$，则：
$$J{\mathbf{v}}_i={\sigma }_i{\mathbf{u}}_i$$

这表示：

• 关节空间的运动方向 ${\mathbf{v}}_i$，对应末端任务空间的运动方向 ${\mathbf{u}}_i$；
• 速度的放大/缩小倍数为奇异值 ${\sigma }_i$。

简言之，奇异值描述了当前位姿下，关节速度沿特定方向转换为末端速度的增益。

最小奇异值

最小奇异值 ${\sigma }_{\min }$ 对应末端最难产生运动的方向。当 ${\sigma }_{\min }\to 0$ 时，机械臂沿该方向的运动能力趋近于零；当 ${\sigma }_{\min }=0$ 时，雅可比降秩，机器人进入奇异状态。

条件数

雅可比的条件数定义为最大奇异值与最小奇异值的比值：
$$\kappa (J)=\frac{{\sigma }_{\max }}{{\sigma }_{\min }}$$

• 当 $\kappa \approx 1$ 时，各方向运动能力均匀，机器人各向同性好；
• 当 $\kappa \gg 1$ 时，不同方向运动能力差异悬殊；
• 当 ${\sigma }_{\min }=0$ 时，条件数趋于无穷大。

条件数同时反映了逆运动学求解的数值稳定性，条件数越大，求解对噪声越敏感。

2.12 可操作度椭球与可操作度指标

可操作度椭球

假设关节速度受单位球约束：
$${\dot{\mathbf{q}}}^T\dot{\mathbf{q}}\le 1$$

经过雅可比映射到任务空间后，单位球会变为椭球，即可操作度椭球（Manipulability Ellipsoid）：

• 椭球主轴方向由左奇异向量矩阵 $U$ 决定；
• 各主轴长度由对应奇异值 ${\sigma }_i$ 决定。

其物理含义十分直观：

• 长轴方向：末端容易产生速度，运动灵活性高；
• 短轴方向：末端难以产生速度，运动灵活性低；
• 某一轴长度趋近于零：接近奇异位形；
• 椭球退化为平面或直线：发生奇异。

与之对偶的是力可操作度椭球：速度椭球的长轴对应力椭球的短轴，即运动灵活的方向施力能力弱，运动困难的方向施力能力强。

Yoshikawa可操作度指标

Yoshikawa在1985年的经典论文中提出了量化机器人操作灵活性的指标——可操作度。对于满行秩雅可比，可操作度定义为：
$$\boxed{w(\mathbf{q})=\sqrt{\det (J{J}^T)}}$$

由于 $\det (J{J}^T)={\prod }_i{\sigma }_i^2$，因此 $w={\prod }_i{\sigma }_i$，即可操作度与速度可操作度椭球的体积成正比。当机器人接近奇异点时，$w\to 0$。

指标的局限性

空间机械臂的几何雅可比同时包含线速度（单位m/s）和角速度（单位rad/s），二者量纲不同，直接计算条件数或可操作度会受长度单位选择的强烈影响。例如将单位从米改为毫米，平移部分数值扩大1000倍，可操作度数值会发生量级变化。

因此工程实践中通常采用以下优化方案：

• 分别计算平移可操作度与旋转可操作度；
• 引入特征长度进行无量纲化处理；
• 根据具体任务对平移与旋转分量加权；
• 仅分析任务真正关心的雅可比子矩阵。

《Modern Robotics》同样明确指出，角速度与线速度单位不一致，将雅可比拆分分析通常更合理。

---

三、逆运动学与控制：雅可比的工程落地

3.1 逆运动学求解：逆矩阵与伪逆

已知期望末端速度 ${\mathbf{V}}_d$，求解对应的关节速度 $\dot{\mathbf{q}}$，本质是解线性方程 $J\dot{\mathbf{q}}={\mathbf{V}}_d$。

普通逆矩阵

仅当雅可比为方阵且满秩时，可使用逆矩阵求解：
$$\dot{\mathbf{q}}=J^{-1}{\mathbf{V}}_d$$

工程中不推荐直接手动计算逆矩阵，原因包括：

• 雅可比常为非方阵（冗余机械臂、欠驱动系统）；
• 接近奇异点时逆矩阵数值不稳定；
• 显式求逆计算量大且数值精度差。

Moore–Penrose伪逆

通用场景下使用Moore–Penrose伪逆（广义逆）求解：
$$\boxed{\dot{\mathbf{q}}=J^{+}{\mathbf{V}}_d}$$

基于SVD分解，伪逆可表示为：
$$J^{+}=V{\Sigma }^{+}{U}^T$$
其中 ${\Sigma }^{+}$ 将非零奇异值取倒数，零奇异值保持为零。

Penrose在1955年的经典论文中证明，该广义逆对任意矩形矩阵或奇异矩阵都存在，且满足四个唯一性条件，是广义逆的标准定义。

行满秩情况（冗余机械臂）

当 $J\in {\mathbb{R}}^{m\times n}$ 且 $m<n$、满行秩时，伪逆可表示为：
$$J^{+}=J^T(J{J}^T{)}^{-1}$$

该场景常见于7自由度冗余机械臂：7个关节控制6维末端位姿。伪逆给出满足任务要求的最小二范数关节速度解。

列满秩情况

当 $m>n$ 且满列秩时，伪逆可表示为：
$$J^{+}=(J^{T}J{)}^{-1}{J}^T$$

此时任务约束多于关节自由度，无法精确满足所有输出要求，伪逆给出最小二乘意义下的最优解。

3.2 阻尼最小二乘：奇异鲁棒求解

普通伪逆在接近奇异点时，最小奇异值的倒数趋于无穷大，导致关节速度指令失控。工程中广泛使用阻尼最小二乘（Damped Least Squares, DLS），也称为阻尼伪逆：
$$\boxed{\dot{\mathbf{q}}=J^T{\left(J{J}^T+{\lambda }^{2}I\right)}^{-1}{\mathbf{V}}_d}$$

等价正则化形式为：
$$\dot{\mathbf{q}}={\left(J^{T}J+{\lambda }^{2}I\right)}^{-1}{J}^T{\mathbf{V}}_d$$

它等价于求解带L2正则项的最小二乘问题：
$$\underset{\dot{\mathbf{q}}}{\min }\left(|J\dot{\mathbf{q}}-{\mathbf{V}}_d{|}^2+{\lambda }^2|\dot{\mathbf{q}}{|}^2\right)$$
其中第一项保证末端速度跟踪精度，第二项抑制关节速度幅值，$\lambda$ 为阻尼系数。

Nakamura与Hanafusa在1986年的经典工作中提出了奇异鲁棒逆运动学思想，为阻尼伪逆方法奠定了理论基础。

阻尼的作用机制

• 当奇异值较大时，$\frac{{\sigma }_i}{{\sigma }_i^2+{\lambda }^2}\approx \frac{1}{{\sigma }_i}$，行为接近普通伪逆，跟踪精度高；
• 当奇异值很小时，$\frac{{\sigma }_i}{{\sigma }_i^2+{\lambda }^2}$ 不会无限增大，关节速度被限制在合理范围内。

阻尼方法的代价是牺牲部分末端跟踪精度，换取数值稳定性与速度有界性，是精度与稳定性的权衡。

3.3 冗余机械臂与零空间优化

当关节自由度多于任务维数（$n>m$）时，满足末端任务的关节速度解不唯一，通解可表示为：
$$\boxed{\dot{\mathbf{q}}=J^{+}{\mathbf{V}}_d+\left(I-J^{+}J\right)\mathbf{z}}$$

其中 $N=I-J^{+}J$ 称为零空间投影矩阵，满足 $JN=0$，因此第二项不会改变末端速度，仅在零空间内调整关节运动。

零空间的应用

通过设计向量 $\mathbf{z}$，可在不影响主任务的前提下实现次级优化目标，典型目标包括：

• 远离关节限位，避免超程；
• 提升可操作度，远离奇异位形；
• 规避障碍物与自碰撞；
• 优化关节力矩分布，降低能耗；
• 保持肘部、肩部等特定姿态；
• 维持传感器视野朝向。

例如以提升Yoshikawa可操作度为目标时，可设：
$$\mathbf{z}=k{\nabla }_{\mathbf{q}}w(\mathbf{q})$$
其中 $k$ 为增益系数，沿可操作度梯度方向调整关节位形。

工程注意事项

若使用阻尼伪逆 $J_{\lambda }^{\#}$ 替代精确伪逆，则 $I-J_{\lambda }^{\#}J$ 不再是严格的正交投影矩阵，次级任务会对主任务产生轻微干扰。高精度多任务控制通常需要采用分层任务优先级、加权伪逆、约束二次规划（Hierarchical QP）等更严格的方案。

3.4 加权伪逆与任务优先级

普通伪逆最小化关节速度的二范数 $|\dot{\mathbf{q}}{|}^2$，但实际工程中不同关节的运动代价、限速、负载存在差异，因此引入正定权重矩阵 $W\succ 0$，求解带权约束的最小化问题：
$$\underset{\dot{\mathbf{q}}}{\min }{\dot{\mathbf{q}}}^{T}W\dot{\mathbf{q}}\text{s.t.}J\dot{\mathbf{q}}={\mathbf{V}}_d$$

对应的加权伪逆为：
$$\boxed{J_W^{\#}=W^{-1}{J}^T{\left(J{W}^{-1}{J}^T\right)}^{-1}}$$

权值越大的关节，运动代价越高，求解结果中该关节的运动幅度会被尽可能压低。通过动态调整权重矩阵，可实现接近关节限位、接近奇异时的自适应降速。

3.5 数值迭代逆运动学

基于雅可比的迭代法是数值逆运动学的主流方案，目标是求解关节位形 ${\mathbf{q}}^{\ast }$，使得正运动学结果满足 $\mathbf{f}({\mathbf{q}}^{\ast })={\mathbf{x}}_d$。

算法流程如下：

1. 给定初始关节值 ${\mathbf{q}}_k$，计算当前位姿误差 ${\mathbf{e}}_k={\mathbf{x}}_d-\mathbf{f}({\mathbf{q}}_k)$；
2. 利用雅可比进行局部线性化，近似得到 $J({\mathbf{q}}_k)\Delta \mathbf{q}\approx {\mathbf{e}}_k$；
3. 使用伪逆求解关节增量 $\Delta \mathbf{q}=J^{+}{\mathbf{e}}_k$；
4. 更新关节值 ${\mathbf{q}}_{k+1}={\mathbf{q}}_k+\alpha \Delta \mathbf{q}$，其中 $\alpha$ 为步长系数；
5. 重复迭代直至误差收敛至阈值以内。

完整迭代公式为：
$$\boxed{{\mathbf{q}}_{k+1}={\mathbf{q}}_k+\alpha J^{+}({\mathbf{q}}_k){\mathbf{e}}_k}$$

Pinocchio官方文档提供了基于该原理的闭环逆运动学（CLIK）示例，支持末端位置与姿态的同步求解。

姿态误差的正确计算

位置误差可直接使用笛卡尔差 ${\mathbf{e}}_p={\mathbf{p}}_d-\mathbf{p}$，但旋转误差不能直接用旋转矩阵相减或四元数分量相减。

更严谨的做法是构造相对位姿 $T_e=T^{-1}{T}_d$，再通过SE(3)对数映射得到六维误差向量：
$$\mathbf{e}=\mathrm{Log}(T_e{)}^{\vee }$$
同时需确保误差向量的参考坐标系与所使用的雅可比一致。

3.6 分辨率速度控制

分辨率速度控制（Resolved-Rate Control）是基于雅可比的经典速度级控制框架，Whitney的经典工作奠定了该方法的理论基础。

其核心思想是：给定期望末端速度，通过雅可比逆映射直接得到关节速度指令。为实现闭环位姿修正，通常引入误差反馈项：
$${\mathbf{V}}_c={\mathbf{V}}_{\mathrm{ff}}+K\mathbf{e}$$
其中 ${\mathbf{V}}_{\mathrm{ff}}$ 为前馈期望速度，$K\mathbf{e}$ 为位姿误差修正项。

最终控制律为：
$$\boxed{\dot{\mathbf{q}}=J^{\#}\left({\mathbf{V}}_{\mathrm{ff}}+K\mathbf{e}\right)+N\mathbf{z}}$$
其中 $J^{\#}$ 为伪逆或阻尼伪逆，$N\mathbf{z}$ 为零空间次级任务。

3.7 阻抗控制与力反馈

雅可比转置是力控制系统的核心纽带。对于末端位姿误差 $\mathbf{e}={\mathbf{x}}_d-\mathbf{x}$，可设计期望笛卡尔力：
$${\mathbf{F}}^{\ast }=K\mathbf{e}-D\dot{\mathbf{x}}$$
其中 $K$ 为刚度矩阵，$D$ 为阻尼矩阵，模拟虚拟弹簧阻尼特性。

通过雅可比转置将笛卡尔力映射为关节力矩指令：
$$\boxed{\mathbf{\tau }=J^T{\mathbf{F}}^{\ast }}$$

若考虑机器人动力学补偿，完整控制律为：
$$\mathbf{\tau }=J^T{\mathbf{F}}^{\ast }+\mathbf{C}(\mathbf{q},\dot{\mathbf{q}})+\mathbf{g}(\mathbf{q})$$
其中 $\mathbf{C}$ 为科里奥利与离心力项，$\mathbf{g}$ 为重力项。

在力反馈设备与协作机器人系统中，典型的控制链路为：
$$\mathbf{q}\to \mathbf{x}\to {\mathbf{F}}_{\mathrm{virtual}}\to J^T\to \mathbf{\tau }$$

例如虚拟弹簧力 ${\mathbf{F}}_{\mathrm{virtual}}=-K(\mathbf{x}-{\mathbf{x}}_0)-B\dot{\mathbf{x}}$，对应的电机力矩即为 $\mathbf{\tau }=J^T\left[-K(\mathbf{x}-{\mathbf{x}}_0)-B\dot{\mathbf{x}}\right]$。

由此可见，雅可比矩阵是连接关节空间与笛卡尔空间、速度与力的核心桥梁。

---

四、跨领域延伸：控制、估计与优化

4.1 非线性控制系统线性化

对于非线性控制系统：
$$\dot{\mathbf{x}}=\mathbf{f}(\mathbf{x},\mathbf{u})$$

在工作点 $({\mathbf{x}}_0,{\mathbf{u}}_0)$ 附近定义扰动变量 $\delta \mathbf{x}=\mathbf{x}-{\mathbf{x}}_0$、$\delta \mathbf{u}=\mathbf{u}-{\mathbf{u}}_0$，进行一阶泰勒展开可得线性化模型：
$$\delta \dot{\mathbf{x}}=A\delta \mathbf{x}+B\delta \mathbf{u}$$

其中状态矩阵 $A$ 与输入矩阵 $B$ 本质上就是两个雅可比矩阵：
$$\boxed{A={\frac{\partial \mathbf{f}}{\partial \mathbf{x}}|}_{{\mathbf{x}}_0,{\mathbf{u}}_0},B={\frac{\partial \mathbf{f}}{\partial \mathbf{u}}|}_{{\mathbf{x}}_0,{\mathbf{u}}_0}}$$

对于输出方程 $\mathbf{y}=\mathbf{h}(\mathbf{x},\mathbf{u})$，同理可得：
$$C=\frac{\partial \mathbf{h}}{\partial \mathbf{x}},D=\frac{\partial \mathbf{h}}{\partial \mathbf{u}}$$

简言之：非线性系统通过雅可比实现局部线性化，进而可使用特征值分析、LQR控制、稳定性判据等线性系统理论工具。

4.2 扩展卡尔曼滤波中的雅可比

扩展卡尔曼滤波（EKF）是处理非线性状态估计的标准方法，其核心是在当前估计点附近对非线性模型进行线性化。

设非线性状态转移方程与观测方程：
$$\begin{array}{rl}{\mathbf{x}}_k& =\mathbf{f}({\mathbf{x}}_{k-1},{\mathbf{u}}_k)+{\mathbf{w}}_k\\ {\mathbf{z}}_k& =\mathbf{h}({\mathbf{x}}_k)+{\mathbf{v}}_k\end{array}$$
其中 ${\mathbf{w}}_k$ 为过程噪声，${\mathbf{v}}_k$ 为观测噪声。

EKF需要计算两个雅可比矩阵：

• 状态转移雅可比 $F_k={\frac{\partial \mathbf{f}}{\partial \mathbf{x}}|}_{{\hat{\mathbf{x}}}_{k-1}}$，用于协方差预测步；
• 观测雅可比 $H_k={\frac{\partial \mathbf{h}}{\partial \mathbf{x}}|}_{{\hat{\mathbf{x}}}_k}$，用于卡尔曼增益计算步。

ROS生态中的robot_localization包正是基于EKF/UKF实现多传感器融合，广泛应用于移动机器人里程计、IMU、GPS数据融合。

4.3 非线性最优化与最小二乘

考虑非线性最小二乘问题：
$$\underset{\mathbf{x}}{\min }\frac{1}{2}|\mathbf{r}(\mathbf{x}){|}^2$$
其中残差函数 $\mathbf{r}:{\mathbb{R}}^n\to {\mathbb{R}}^m$，其雅可比为 $J_r=\frac{\partial \mathbf{r}}{\partial \mathbf{x}}$。

目标函数的梯度可表示为：
$$\nabla F(\mathbf{x})=J_r^T\mathbf{r}$$

高斯-牛顿法

高斯-牛顿法用雅可比乘积近似Hessian矩阵：
$$H\approx J_r^T{J}_r$$

迭代更新量满足线性方程：
$$J_r^T{J}_r\Delta \mathbf{x}=-J_r^T\mathbf{r}$$

Levenberg–Marquardt算法

LM算法在高斯-牛顿基础上增加阻尼项，是最常用的非线性最小二乘算法：
$$\left(J_r^T{J}_r+\lambda I\right)\Delta \mathbf{x}=-J_r^T\mathbf{r}$$

其数学形式与机器人阻尼最小二乘逆运动学高度同源，本质都是带正则项的线性最小二乘。

SciPy官方的least_squares函数正是针对向量残差的非线性最小二乘求解，支持用户提供或自动估计残差雅可比。

4.4 误差与协方差传播

对于函数映射 $\mathbf{y}=\mathbf{f}(\mathbf{x})$，若输入存在微小误差 $\delta \mathbf{x}$，则输出误差近似为：
$$\delta \mathbf{y}\approx J\delta \mathbf{x}$$

若输入的协方差矩阵为 ${\Sigma }_x=E[\delta \mathbf{x}\delta {\mathbf{x}}^T]$，则输出协方差的一阶近似为：
$$\boxed{{\Sigma }_y\approx J{\Sigma }_x{J}^T}$$

这一误差传播公式在机器人领域应用极为广泛：

• 相机标定误差向三维点云传播；
• 手眼标定不确定度分析；
• 机器人TCP位姿精度评估；
• 多传感器融合中的协方差传递；
• 连杆加工误差向末端精度传播。

若函数非线性较强或输入误差较大，一阶雅可比传播精度不足，可采用二阶近似、无迹变换（Unscented Transform）、蒙特卡洛采样或粒子滤波等方法。

---

五、工程实践：计算、验证与避坑

5.1 雅可比的四种计算方法

方法一：手工解析求导

直接对正运动学表达式逐元素求偏导，得到解析形式的雅可比矩阵。

• 优点：数值精确、运行速度快，适合低自由度系统；
• 缺点：高自由度系统表达式繁琐易出错，机器人结构修改后需重新推导。

方法二：几何构造法

利用关节轴几何关系，逐列构造雅可比矩阵：

• 旋转关节：$J_i=\left[\begin{array}{c}{\mathbf{z}}_i\times ({\mathbf{p}}_e-{\mathbf{p}}_i)\\ {\mathbf{z}}_i\end{array}\right]$

• 移动关节：$J_i=\left[\begin{array}{c}{\mathbf{z}}_i\\ 0\end{array}\right]$

• 优点：物理直观，适合串联机器人，无需展开复杂三角函数；

• 补充：《Modern Robotics》中基于螺旋轴与伴随变换构造空间雅可比的方法，本质上也是几何构造法的延伸，无需显式求导。

方法三：自动微分

给定可微的正运动学程序，由自动微分（AD）系统计算精确到机器精度的一阶导数。JAX等框架提供了jacfwd和jacrev两种接口：

• 输入少、输出多：前向模式（jacfwd）更高效；
• 输入多、输出少：反向模式（jacrev）更高效；
• 接近方阵：需根据计算图结构实测对比。

自动微分是现代机器人仿真与优化框架的主流方案，兼顾精度与开发效率。

方法四：有限差分法

通过数值扰动近似偏导数，第 $j$ 列的前向差分近似为：
$$J_{:,j}\approx \frac{\mathbf{f}(\mathbf{x}+h{\mathbf{e}}_j)-\mathbf{f}(\mathbf{x})}{h}$$

中心差分精度更高：
$$J_{:,j}\approx \frac{\mathbf{f}(\mathbf{x}+h{\mathbf{e}}_j)-\mathbf{f}(\mathbf{x}-h{\mathbf{e}}_j)}{2h}$$

SciPy的approx_fprime函数即采用前向有限差分实现导数估计。

有限差分的核心难点是步长 $h$ 的选择：

• 步长过大：截断误差显著，近似精度低；
• 步长过小：浮点减法抵消误差占主导，精度恶化；
• 工程经验：通常取 $10^{-6}\sim 10^{-8}$，变量尺度差异大时需采用相对步长。

5.2 雅可比正确性验证方法

工程实践中强烈推荐使用方向导数测试验证雅可比正确性，比逐列检查更高效，且能同时验证列顺序、符号与整体映射关系。

方法如下：

1. 任选一个随机小向量 $\Delta \mathbf{q}$；
2. 计算解析雅可比乘积 $J(\mathbf{q})\Delta \mathbf{q}$；
3. 计算数值近似 $\frac{\mathbf{f}(\mathbf{q}+ϵ\Delta \mathbf{q})-\mathbf{f}(\mathbf{q})}{ϵ}$，其中 $ϵ$ 取足够小的正数（如 $10^{-6}$）；
4. 定义相对误差：
   $$e=\frac{\left|J\Delta \mathbf{q}-\frac{\mathbf{f}(\mathbf{q}+ϵ\Delta \mathbf{q})-\mathbf{f}(\mathbf{q})}{ϵ}\right|}{\max (1,|J\Delta \mathbf{q}|)}$$

正常情况下相对误差应在 $10^{-6}$ 量级。对于旋转分量，不要直接比较欧拉角差值，应通过旋转矩阵对数映射或四元数相对旋转计算等效角速度，再进行对比。

5.3 主流机器人库中的雅可比实现

MoveIt

MoveIt2的RobotState类可根据当前机器人状态计算指定关节组、指定末端连杆、指定参考点的雅可比矩阵。官方教程明确说明，RobotState维护关节位置、速度等状态量，并提供雅可比计算接口。

Pinocchio

Pinocchio是高性能刚体动力学库，支持多种参考坐标系下的frame Jacobian计算，包括LOCAL、WORLD、LOCAL_WORLD_ALIGNED。核心接口包括：

• computeFrameJacobian：计算指定连杆的雅可比；
• getFrameJacobianTimeVariation：计算雅可比的时间导数；
• 同时支持速度、加速度与导数的完整求解。

官方文档特别强调，使用前必须确认输出速度的参考坐标系约定。

Orocos KDL

KDL中的雅可比存储为动态列数的 $6\times n$ 矩阵，提供完整的求解器套件：

• 关节到雅可比求解器；
• 伪逆速度逆运动学；
• 加权阻尼最小二乘求解器；
• 雅可比时间导数 $\dot{J}$ 求解器。

5.4 工程常见错误与避坑指南

1. 参考坐标系不匹配

期望速度定义在世界坐标系，雅可比却输出末端局部坐标系速度，直接相乘会导致方向完全错误。必须保证任务空间量与雅可比的参考坐标系一致。

2. 运动旋量顺序混淆

不同库可能采用 $[\mathbf{v};\mathbf{\omega }]$ 或 $[\mathbf{\omega };\mathbf{v}]$ 的排列顺序，混用会导致线速度与角速度、力与力矩颠倒，引发严重控制事故。

3. 欧拉角速度与角速度混淆

错误认为 $\mathbf{\omega }=\left[\begin{array}{c}\dot{r}\\ \dot{p}\\ \dot{y}\end{array}\right]$，实际上二者需通过姿态相关的转换矩阵 $E(\mathbf{\varphi })$ 关联。该错误是解析雅可比应用中最常见的问题。

4. 奇异点附近直接求逆

禁止直接使用J.inverse()进行逆运动学求解，更合理的方案包括：SVD伪逆、阻尼最小二乘、QR分解求解线性方程组、速度/加速度限幅、奇异值在线监测。

5. 单位尺度不统一

例如机器人模型采用米为单位，目标速度却传入毫米每秒数值，会导致逆运动学输出异常大的关节速度。工程中必须统一单位制。

6. 忽略TCP偏置

雅可比计算基于法兰原点，但实际控制目标是焊枪、夹爪等工具的TCP点。TCP偏置会导致线速度映射错误，尤其在高速旋转时，即使法兰原点不动，TCP也会产生显著的线速度。

7. 忽略关节约束

数学上得到的最优关节速度，不一定在机器人执行能力范围内。必须考虑：

• 关节速度、加速度、力矩限制；
• 关节位置限位；
• 碰撞与自碰撞约束；
• 控制周期与通信延迟。

严格处理这些约束通常需要采用二次规划（QP）求解器，构建带约束的速度优化问题。

---

六、公式汇总

功能	公式
一阶局部线性化	$\Delta \mathbf{y}\approx J\Delta \mathbf{x}$
关节速度到末端速度	$\mathbf{V}=J\dot{\mathbf{q}}$
末端加速度映射	$\dot{\mathbf{V}}=J\ddot{\mathbf{q}}+\dot{J}\dot{\mathbf{q}}$
末端力到关节力矩	$\mathbf{\tau }=J^T\mathbf{W}$
方阵逆速度解	$\dot{\mathbf{q}}=J^{-1}{\mathbf{V}}_d$
Moore–Penrose伪逆解	$\dot{\mathbf{q}}=J^{+}{\mathbf{V}}_d$
阻尼最小二乘解	$\dot{\mathbf{q}}=J^T(J{J}^T+{\lambda }^{2}I{)}^{-1}{\mathbf{V}}_d$
冗余零空间通解	$\dot{\mathbf{q}}=J^{+}{\mathbf{V}}_d+(I-J^{+}J)\mathbf{z}$
条件数	$\kappa ={\sigma }_{\max }/{\sigma }_{\min }$
Yoshikawa可操作度	$w=\sqrt{\det (J{J}^T)}$
误差协方差传播	${\Sigma }_y\approx J{\Sigma }_x{J}^T$
非线性系统线性化	$A=\partial f/\partial x,B=\partial f/\partial u$
高斯-牛顿Hessian近似	$H\approx J^{T}J$

---

七、五层理解框架：从数学到工程

雅可比矩阵的认知可分为由浅入深的五个层次：

第一层：多变量导数的矩阵形式

$$J_{ij}=\frac{\partial y_i}{\partial x_j}$$
最基础的认知：雅可比是偏导数的有序排列，第j个输入对第i个输出的局部影响系数。

第二层：非线性系统的局部线性模型

$$\Delta \mathbf{y}\approx J\Delta \mathbf{x}$$
核心本质：将复杂的非线性映射，在当前工作点附近近似为简单的线性变换，是所有工程应用的理论根基。

第三层：机器人空间映射的桥梁

$$\mathbf{V}=J\dot{\mathbf{q}}$$
机器人领域的核心价值：连接关节空间与笛卡尔任务空间，实现速度的正向传递。

第四层：运动与力的对偶关系

$$\mathbf{\tau }=J^T\mathbf{W}$$
对偶特性：雅可比映射速度，雅可比转置映射力，构成微分运动学与静力学的完整对偶体系。

第五层：机器人能力的几何描述

雅可比矩阵蕴含了机器人当前位姿的全部运动学能力信息：

• 秩：独立运动方向的数量；
• 奇异值：不同方向的运动增益；
• 条件数：数值稳定性与各向同性；
• 零空间：不影响主任务的内部自由度；
• 可操作度椭球：全方向运动灵活性的几何表征。

最终可以给出两个高度凝练的结论：
从数学本质看：雅可比矩阵是非线性映射在当前状态附近的一阶线性化算子。
从机器人视角看：雅可比矩阵是关节空间与笛卡尔任务空间之间，传递速度、力与灵敏度的核心桥梁。