第3章 机器人机构学：运动学与动力学
原课程: Robots That Learn, UC Berkeley CS294-291, Spring 2026 本章基于 Lecture 2B: Robot Mechanisms — Kinematics and Dynamics 参考: Kevin M. Lynch & Frank C. Park, Modern Robotics: Mechanics, Planning, and Control (Cambridge, 2017); Richard M. Murray, Zexiang Li & S. Shankar Sastry, A Mathematical Introduction to Robotic Manipulation (CRC, 1994)
一、本章概要
1.1 动机与位置
上一章我们研究了生物运动力学，从骨骼肌的 Hill 模型到 CPG（中枢模式发生器）驱动的节律步态，建立了生物体如何动的底层原理。本章转入同一问题的工程侧：给定一个由刚性连杆和关节组成的机器人，如何用数学精确描述它的运动？
这一转换回答一个根本问题：从生物到机器人，运动控制失去了什么？又得到了什么？
生物体拥有可变的肌肉刚度、冗余驱动、弹性肌腱和分布式传感。机器人则拥有精确的关节编码器、可控力矩电机和确定性运动学链。要弥合两者之间的鸿沟，首先必须在数学上精确刻画机器人的几何 (kinematics)与力学 (dynamics)。
1.2 核心问题
本章围绕三个核心问题展开：
- 刚体运动如何用数学表示？ —— 从旋转矩阵到指数坐标，建立 SO(3)/SE(3) 李群-李代数对应关系。
- 串联机器人的前向运动学如何高效计算？ —— Product-of-Exponentials (PoE) 公式与 DH 参数的对比。
- 机器人的运动方程如何建立？ —— 拉格朗日动力学与牛顿-欧拉逆动力学的统一视角。
1.3 与前后章节的逻辑关系
Ch02 生物运动力学 ──→ Ch03 机器人机构学 ──→ Ch04 扩散模型入门
(生物如何动) (机器如何动) (如何生成动作)
本章的数学工具（李群、指数映射、雅可比矩阵）是后续所有章节的共同语言：
- Ch05 (灵巧手): 多指手运动学是本章串联运动学的自然推广
- Ch08 (模型预测控制): 逆动力学是 MPC 的内层优化核心
- Ch11 (强化学习): 运动学约束作为策略输出的映射层
二、核心概念与数学基础：李群与刚体运动
2.1 刚体运动表示法全景
描述三维空间中刚体的位姿 (pose)，有三种等价但计算特性各异的表示法：
| 表示法 | 数学对象 | 参数数量 | 奇异性 | 组合运算 |
|---|---|---|---|---|
| 旋转矩阵 + 位置向量 | \((R, p) \in SO(3) \times \mathbb{R}^3\) | \(9+3=12\) (约束后 6 DOF) | 无 | 矩阵乘法 |
| 齐次变换矩阵 | \(T \in SE(3) \subset \mathbb{R}^{4\times4}\) | 16 (约束后 6 DOF) | 无 | 矩阵乘法 |
| 指数坐标 | \(\mathcal{S}\theta \in \mathbb{R}^6\) | 6 | 无 (局部) | Baker-Campbell-Hausdorff |
| 欧拉角 + 位置 | \((\alpha,\beta,\gamma,x,y,z)\) | 6 | 万向节死锁 | 非线性 |
| 单位四元数 + 位置 | \((q_w,q_x,q_y,q_z,x,y,z)\) | 7 | 无 | 四元数乘法 |
| 旋量 (screw) | \((\omega,v) \in \mathbb{R}^6\) | 6 | 无 | 伴随变换 |
本章重点关注前三种——它们构成了现代机器人运动学的理论框架。旋量理论 (screw theory) 是连接所有表示的统一语言。
2.2 旋转运动：SO(3) 李群与 so(3) 李代数
2.2.1 SO(3) 的定义与性质
三维旋转群 SO(3) （Special Orthogonal group）定义为全体行列式为 \(+1\) 的正交矩阵的集合：
\[\boxed{SO(3) = \left\{ R \in \mathbb{R}^{3\times 3} \;\middle\vert \; R^T R = I,\; \det(R) = 1 \right\}}\]核心性质：
- 封闭性: \(R_1, R_2 \in SO(3) \Rightarrow R_1R_2 \in SO(3)\)
- 逆元: \(R^{-1} = R^T \in SO(3)\)
- 非交换: 一般情况下 \(R_1R_2 \neq R_2R_1\)
- 维数: \(\dim(SO(3)) = 3\) — 三维光滑流形，嵌入在 \(\mathbb{R}^{3\times3} \cong \mathbb{R}^9\) 中
- 紧致且连通: 任何两个旋转矩阵之间都存在一条连续路径
2.2.2 李代数 so(3) — 无穷小旋转的线性空间
SO(3) 在单位元 \(I\) 处的切空间构成其李代数 (Lie algebra)，记为 so(3)。考虑一条经过单位元的曲线 \(R(t) \in SO(3)\)，满足 \(R(0)=I\)。对 \(R(t)^T R(t)=I\) 两边在 \(t=0\) 处求导：
\[\dot{R}(0)^T I + I^T \dot{R}(0) = 0 \;\Rightarrow\; \dot{R}(0)^T = -\dot{R}(0)\]因此 so(3) 是全体 \(3\times3\) 反对称矩阵构成的线性空间：
\[\boxed{so(3) = \left\{ [\omega] \in \mathbb{R}^{3\times 3} \;\middle\vert \; [\omega]^T = -[\omega] \right\}}\]其中反对称矩阵算子 \([\cdot]: \mathbb{R}^3 \to so(3)\) 定义为：
\[[\omega] = \begin{bmatrix} 0 & -\omega_3 & \omega_2 \\ \omega_3 & 0 & -\omega_1 \\ -\omega_2 & \omega_1 & 0 \end{bmatrix}\]\(\dim(so(3)) = 3\)，与 SO(3) 的维数一致——这正是李群-李代数对应的核心特征。
两个关键恒等式：
- 叉积恒等式: \([\omega] v = \omega \times v,\quad \forall v \in \mathbb{R}^3\)
- 李括号: \(\big[ [\omega_1], [\omega_2] \big] \triangleq [\omega_1][\omega_2] - [\omega_2][\omega_1] = [\omega_1 \times \omega_2]\)
李括号衡量了 so(3) 的非交换性。特别地，so(3) 的李括号等价于向量空间的叉积，这使 \(\mathbb{R}^3\) 本身构成一个李代数（同构于 so(3)）。
反对称矩阵的幂次性质（对单位向量 \(\hat{\omega}\)，\(\|\hat{\omega}\|=1\)）：
\[[\hat{\omega}]^2 = \hat{\omega}\hat{\omega}^T - I,\qquad [\hat{\omega}]^3 = -[\hat{\omega}]\]第二式是化简指数映射的关键。推导如下：
\[[\hat{\omega}]^3 = [\hat{\omega}] \cdot [\hat{\omega}]^2 = [\hat{\omega}](\hat{\omega}\hat{\omega}^T - I) = [\hat{\omega}]\hat{\omega}\hat{\omega}^T - [\hat{\omega}]\]由于 \([\hat{\omega}]\hat{\omega} = \hat{\omega} \times \hat{\omega} = 0\)，故 \([\hat{\omega}]^3 = -[\hat{\omega}]\)。
该关系的几何含义是：绕固定轴旋转 \(\pi/2\) 角度的三次方等于绕同轴反向旋转 \(\pi/2\) 角度。
2.2.3 指数映射：从 so(3) 到 SO(3)
指数映射 \(\exp: so(3) \to SO(3)\) 是天桥，将线性空间 so(3) 中的元素映射到非线性流形 SO(3) 上。其定义为矩阵指数：
\[\boxed{\exp([\hat{\omega}]\theta) = e^{[\hat{\omega}]\theta} = \sum_{k=0}^{\infty} \frac{([\hat{\omega}]\theta)^k}{k!}}\]这里 \(\theta\) 是旋转角度，\(\hat{\omega}\) 是单位旋转轴（\(\|\hat{\omega}\|=1\)）。参数 \(\omega = \hat{\omega}\theta \in \mathbb{R}^3\) 称为 指数坐标 (exponential coordinates)，三维向量同时编码了旋转轴和旋转角。
解析求和 — Rodrigues 公式的推导：
利用 \([\hat{\omega}]^3 = -[\hat{\omega}]\)，所有高阶项可归约为 \(I\), \([\hat{\omega}]\), \([\hat{\omega}]^2\) 的线性组合：
\[\begin{aligned} e^{[\hat{\omega}]\theta} &= I + \theta[\hat{\omega}] + \frac{\theta^2}{2!}[\hat{\omega}]^2 + \frac{\theta^3}{3!}[\hat{\omega}]^3 + \frac{\theta^4}{4!}[\hat{\omega}]^4 + \frac{\theta^5}{5!}[\hat{\omega}]^5 + \frac{\theta^6}{6!}[\hat{\omega}]^6 + \cdots \\ &= I + \theta[\hat{\omega}] + \frac{\theta^2}{2!}[\hat{\omega}]^2 - \frac{\theta^3}{3!}[\hat{\omega}] - \frac{\theta^4}{4!}[\hat{\omega}]^2 + \frac{\theta^5}{5!}[\hat{\omega}] + \frac{\theta^6}{6!}[\hat{\omega}]^2 - \cdots \\ &= I + \underbrace{\left(\theta - \frac{\theta^3}{3!} + \frac{\theta^5}{5!} - \cdots\right)}_{\sin\theta}\,[\hat{\omega}] \;+\; \underbrace{\left(\frac{\theta^2}{2!} - \frac{\theta^4}{4!} + \frac{\theta^6}{6!} - \cdots\right)}_{1-\cos\theta}\,[\hat{\omega}]^2 \end{aligned}\]得到Rodrigues 公式的闭合形式：
\[\boxed{R = e^{[\hat{\omega}]\theta} = I + \sin\theta\,[\hat{\omega}] + (1-\cos\theta)\,[\hat{\omega}]^2}\]这一结果说明：指数映射的无穷级数可精确解析求和 — 这正是 Rodrigues 公式的代数本质。
几何推导（物理直觉验证）：
任意向量 \(v \in \mathbb{R}^3\) 绕轴 \(\hat{\omega}\) 旋转 \(\theta\) 角：
\[v_{\text{rot}} = \underbrace{\hat{\omega}\hat{\omega}^T v}_{v_{\parallel}} + \cos\theta\,\underbrace{(I - \hat{\omega}\hat{\omega}^T)v}_{v_{\perp}} + \sin\theta\,(\hat{\omega} \times v_{\perp})\]代入 \([\hat{\omega}]v = \hat{\omega} \times v\) 和 \([\hat{\omega}]^2 = \hat{\omega}\hat{\omega}^T - I\)，整理后 \(v_{\text{rot}} = (I + \sin\theta[\hat{\omega}] + (1-\cos\theta)[\hat{\omega}]^2)v\)，与矩阵形式一致。
2.2.4 对数映射：从 SO(3) 到 so(3)
给定 \(R \in SO(3)\)，对数映射 \(\log: SO(3) \to so(3)\) 恢复其指数坐标：
旋转角：\(\displaystyle \theta = \cos^{-1}\left(\frac{\operatorname{tr}(R) - 1}{2}\right)\)
旋转轴：当 \(\theta \neq 0\) 时，
\[[\hat{\omega}] = \frac{1}{2\sin\theta}(R - R^T)\]由此 \(\hat{\omega} = \operatorname{unskew}([\hat{\omega}])\)，其中 \(\operatorname{unskew}\) 为反对称矩阵算子的逆： \(\operatorname{unskew}([a]) = (a_1, a_2, a_3)\) （从 \([\omega]_{3,2}, [\omega]_{1,3}, [\omega]_{2,1}\) 中提取）。
边界情况：
- \(\theta = 0\)：\(R = I\)，\(\hat{\omega}\) 任意（取 \(\mathbf{0}\)）
- \(\theta = \pi\)：\(R\) 对称，\(R-R^T = 0\)，需特殊处理。此时 \(R = I + 2[\hat{\omega}]^2 = 2\hat{\omega}\hat{\omega}^T - I\)，因此 \(\hat{\omega}\) 可从 \(R+I\) 的非零列中归一化提取：对任意非零列 \(v\)，\(\hat{\omega} = v/\|v\|\)
- \(\theta \approx \pi\)：使用 \(\theta = \pi\) 的逻辑以避免 \(\sin\theta \approx 0\) 导致的数值不稳定
2.3 刚体变换：SE(3) 李群与 se(3) 李代数
2.3.1 SE(3) 的定义
将旋转与平移统一为齐次变换矩阵，得到特殊欧几里得群 SE(3)：
\[\boxed{SE(3) = \left\{ T = \begin{bmatrix} R & p \\ 0_{1\times 3} & 1 \end{bmatrix} \;\middle\vert \; R \in SO(3),\; p \in \mathbb{R}^3 \right\}}\]群运算：
- 乘法：\(T_1 T_2 = \begin{bmatrix} R_1R_2 & R_1p_2 + p_1 \\ 0 & 1 \end{bmatrix}\)
- 逆元：\(T^{-1} = \begin{bmatrix} R^T & -R^T p \\ 0 & 1 \end{bmatrix}\)
- 维数：\(\dim(SE(3)) = 3 + 3 = 6\)（SO(3) 的 3 个旋转 + \(\mathbb{R}^3\) 的 3 个平移自由度）
2.3.2 运动旋量 (Twist) — se(3) 的元素
刚体的瞬时速度由运动旋量 (twist) \(\mathcal{V} = (\omega, v) \in \mathbb{R}^6\) 描述：
- \(\omega \in \mathbb{R}^3\): 角速度向量（旋转轴方向 \(\times\) 角速率大小）
- \(v \in \mathbb{R}^3\): 线速度向量
se(3) 的 \(4\times4\) 矩阵表示为：
\[[\mathcal{V}] = \begin{bmatrix} [\omega] & v \\ 0_{1\times 3} & 0 \end{bmatrix} \in se(3)\]它是 SE(3) 在单位元 \(I_4\) 处的切空间：对任意曲线 \(T(t) \in SE(3)\) 且 \(T(0)=I\)，有 \(\dot{T}(0) = [\mathcal{V}] \in se(3)\)。
2.3.3 空间旋量与物体旋量 — 伴随变换的推导
这是本章最关键的细节之一。同一刚体速度在不同坐标系中有不同表示：
- 空间旋量 \(\mathcal{V}_s\): 在固定坐标系 \(\{s\}\) 中观察到的刚体瞬时速度
- 物体旋量 \(\mathcal{V}_b\): 在随体坐标系 \(\{b\}\) 中观察到的刚体瞬时速度
推导：设 \(T_{sb}(t) \in SE(3)\) 为 \(\{b\}\) 系相对于 \(\{s\}\) 系的位姿。空间速度旋量定义为：
\[[\mathcal{V}_s] = \dot{T}_{sb} T_{sb}^{-1}\]物体速度旋量定义为：
\[[\mathcal{V}_b] = T_{sb}^{-1} \dot{T}_{sb}\]两者关系的推导：由 \(\dot{T}_{sb} = [\mathcal{V}_s] T_{sb} = T_{sb} [\mathcal{V}_b]\)，得 \([\mathcal{V}_b] = T_{sb}^{-1} [\mathcal{V}_s] T_{sb}\)。将 \(T_{sb} = \begin{bmatrix} R & p \\ 0 & 1 \end{bmatrix}\) 代入矩阵乘法：
\[\begin{aligned} \begin{bmatrix} [\omega_b] & v_b \\ 0 & 0 \end{bmatrix} &= \begin{bmatrix} R^T & -R^T p \\ 0 & 1 \end{bmatrix} \begin{bmatrix} [\omega_s] & v_s \\ 0 & 0 \end{bmatrix} \begin{bmatrix} R & p \\ 0 & 1 \end{bmatrix} \\ &= \begin{bmatrix} R^T[\omega_s]R & R^T([\omega_s]p + v_s) \\ 0 & 0 \end{bmatrix} \end{aligned}\]利用 \(R^T[\omega_s]R = [R^T\omega_s]\)（旋转对叉积的共轭性质），得到 \(6\times6\) 的伴随矩阵 (adjoint matrix)：
\[\boxed{\text{Ad}_T = \begin{bmatrix} R & 0_{3\times3} \\ [p]R & R \end{bmatrix} \in \mathbb{R}^{6\times6}}\]使得 \(\mathcal{V}_s = \text{Ad}_{T_{sb}}(\mathcal{V}_b)\) 和 \(\mathcal{V}_b = \text{Ad}_{T_{sb}^{-1}}(\mathcal{V}_s)\)。注意 \(\text{Ad}_{T^{-1}} = \text{Ad}_T^{-1}\)。
伴随矩阵的核心性质：
- 群同态：\(\text{Ad}_{T_1 T_2} = \text{Ad}_{T_1} \text{Ad}_{T_2}\)
- 行列式：\(\det(\text{Ad}_T) = 1\)
- 逆矩阵：\(\text{Ad}_T^{-1} = \text{Ad}_{T^{-1}} = \begin{bmatrix} R^T & 0 \\ -R^T[p] & R^T \end{bmatrix}\)
2.3.4 力旋量 (Wrench) 与对偶
与运动旋量对偶的是力旋量 (wrench) \(\mathcal{F} = (m, f) \in \mathbb{R}^6\):
- \(m \in \mathbb{R}^3\): 力矩 (moment)
- \(f \in \mathbb{R}^3\): 力 (force)
力和速度的对偶关系通过虚功率原理建立：
\[\text{power} = \mathcal{V}^T \mathcal{F} = \omega^T m + v^T f\]力旋量在不同坐标系间的变换满足伴随变换的转置逆：
\[\mathcal{F}_b = \text{Ad}_T^T \mathcal{F}_s\]这保证了功率在不同坐标系下的不变性：
\[\mathcal{V}_b^T \mathcal{F}_b = (\text{Ad}_T^{-1}\mathcal{V}_s)^T (\text{Ad}_T^T \mathcal{F}_s) = \mathcal{V}_s^T \text{Ad}_T^{-T} \text{Ad}_T^T \mathcal{F}_s = \mathcal{V}_s^T \mathcal{F}_s \;\checkmark\]这一对偶结构 (速度 \(\to\) \(\text{Ad}_T\), 力 \(\to\) \(\text{Ad}_T^T\)) 贯穿整个机器人动力学。
2.3.5 SE(3) 的指数映射
se(3) 中由旋量轴 (screw axis) \(\mathcal{S} = (\omega, v) \in \mathbb{R}^6\) 参数化。这里 \(\omega\) 定义旋转轴（未必单位化），\(v\) 定义平移分量。SE(3) 的指数映射为：
\[\boxed{T = e^{[\mathcal{S}]\theta}}\]其闭合形式分两种情况。令 \(\|\omega\|\) 为 \(\omega\) 的范数，\(\hat{\omega} = \omega/\|\omega\|\)。
情况 1 (\(\|\omega\| \neq 0\)，即旋量包含旋转分量):
将 \([\mathcal{S}]\) 分块，利用分块矩阵的幂次公式：
\[e^{[\mathcal{S}]\theta} = \begin{bmatrix} e^{[\omega]\theta} & G(\theta) v \\ 0_{1\times 3} & 1 \end{bmatrix}\]其中
\[G(\theta) = I\theta + (1-\cos\theta)[\hat{\omega}] + (\theta - \sin\theta)[\hat{\omega}]^2\]等效地，也可以将平移部分写为：
\[e^{[\mathcal{S}]\theta} = \begin{bmatrix} e^{[\omega]\theta} & (I - e^{[\omega]\theta})(\omega \times v)/\|\omega\|^2 + \omega\omega^T v\,\theta/\|\omega\|^2 \\ 0 & 1 \end{bmatrix}\]（Lynch-Park Eq 3.76, 3.88）。
情况 2 (\(\|\omega\| = 0\)，纯平移):
\[e^{[\mathcal{S}]\theta} = \begin{bmatrix} I & v\theta \\ 0 & 1 \end{bmatrix}\]这里 \(v = \hat{v}\) 是平移方向的单位向量，\(\theta\) 是平移距离。
2.4 前向运动学：Product-of-Exponentials (PoE)
2.4.1 问题描述
\(n\) 自由度串联机器人的前向运动学 (forward kinematics)：给定关节变量 \(\boldsymbol{\theta} = (\theta_1, \ldots, \theta_n)\)，求末端执行器坐标系 \(\{b\}\) 相对于固定坐标系 \(\{s\}\) 的位姿 \(T_{sb}(\boldsymbol{\theta})\)。
2.4.2 旋量轴的几何构造 — PoE 的物理基础
这是 PoE 方法的核心优势：只需在零位形时测量每个关节的轴线，无需逐帧定义坐标系。
-
转动关节 (revolute): 给定关节轴的单位方向 \(\hat{\omega}_i\) 和轴上任意一点 \(q_i\)（均在 \(\{s\}\) 系中表示）
\[\boxed{\mathcal{S}_i = \begin{bmatrix} \hat{\omega}_i \\ v_i \end{bmatrix} = \begin{bmatrix} \hat{\omega}_i \\ -\hat{\omega}_i \times q_i \end{bmatrix}}\]\(v_i\) 的物理含义：关节以单位角速度转动时，原点处观察到的线速度。若转轴通过原点 (\(q_i = 0\))，则 \(v_i = 0\)。
-
移动关节 (prismatic): \(\mathcal{S}_i = \begin{bmatrix} 0_{3\times 1} \\ \hat{v}_i \end{bmatrix}\)，其中 \(\hat{v}_i\) 为移动方向的单位向量
2.4.3 PoE 公式 — 空间形式
在零位形 (\(\boldsymbol{\theta} = 0\)) 时，末端位姿为 \(M \in SE(3)\)。在零位形处确定每个关节的空间旋量轴 \(\mathcal{S}_i\)（在 \(\{s\}\) 系中表示）。
\[\boxed{T_{sb}(\boldsymbol{\theta}) = e^{[\mathcal{S}_1]\theta_1}\, e^{[\mathcal{S}_2]\theta_2}\, \cdots\, e^{[\mathcal{S}_n]\theta_n}\, M}\]物理含义：从右向左读 — 先将末端从零位形通过 \(M\) 移动到空间，再依次应用各关节运动。先运动的关节（最靠近基座）写在最左边，后运动的关节写在右边。
2.4.4 PoE 公式 — 物体形式
若使用物体旋量轴 \(\mathcal{B}_i\)（在 \(\{b\}\) 系中表示），则：
\[\boxed{T_{sb}(\boldsymbol{\theta}) = M\, e^{[\mathcal{B}_1]\theta_1}\, e^{[\mathcal{B}_2]\theta_2}\, \cdots\, e^{[\mathcal{B}_n]\theta_n}}\]从左向右读，各关节依次作用于基座侧的坐标系。
两种形式的等价性由伴随变换保证：\(\mathcal{S}_i = \text{Ad}_M(\mathcal{B}_i)\)。选择空间形式还是物体形式取决于应用场景——空间形式在固定系中表达自然，物体形式在末端系中表达自然。
2.4.5 PoE 与 DH 参数的对比
DH (Denavit-Hartenberg) 参数法自 1955 年起一直是机器人学教学的标准工具。现代教学已逐渐转向 PoE：
| 维度 | DH 参数 | PoE (指数积) |
|---|---|---|
| 参数数量 | \(4n\) (每条连杆 4 个参数) | \(6n\) (每个关节 1 个 6D 旋量) |
| 坐标系定义 | 每条连杆需精确定义坐标系原点与方向 | 仅需固定系 \(\{s\}\) 和末端系 \(\{b\}\) |
| 闭链/树形/并联结构 | 难以统一处理 | 自然推广（通过约束方程） |
| 奇异性 | 平行关节轴附近出现参数跳变 | 无参数奇异性 |
| 物理直觉 | 弱（\(a, \alpha, d, \theta\) 缺乏直接几何意义） | 强（旋量轴 = 关节的实际运动轴） |
| 运动学校准 | 困难（参数高度耦合，辨识过程非线性） | 相对容易（旋量轴可独立辨识） |
| 微分运动学 | 坐标变换的链式法则 | 伴随映射的自然代数结构 |
| 现代工具支持 | MuJoCo/Isaac 仍用 DH 作为输入格式 | Drake/Pinocchio 原生支持旋量表示 |
教学建议: 现代机器人学教材（如 Lynch & Park 2017）以 PoE 为主线，DH 仅作为历史注记。本教程遵循这一范式。
2.5 速度运动学与雅可比矩阵
2.5.1 空间雅可比 (Space Jacobian)
末端执行器的空间速度旋量 \(\mathcal{V}_s\) 与关节速度 \(\dot{\boldsymbol{\theta}}\) 的关系：
\[\boxed{\mathcal{V}_s = J_s(\boldsymbol{\theta})\,\dot{\boldsymbol{\theta}}}\]其中 \(J_s(\boldsymbol{\theta}) \in \mathbb{R}^{6\times n}\) 的第 \(i\) 列由伴随变换递推给出：
\[\boxed{J_{s,i}(\boldsymbol{\theta}) = \text{Ad}_{\;e^{[\mathcal{S}_1]\theta_1} \cdots e^{[\mathcal{S}_{i-1}]\theta_{i-1}}}(\mathcal{S}_i)}\]推导：对 PoE 空间形式求导 \(\dot{T}_{sb} T_{sb}^{-1} = [\mathcal{V}_s]\)，利用乘积求导法则逐项展开，每项对应一个关节对末端速度的贡献——而该贡献需要通过前 \(i-1\) 个关节的刚体变换将第 \(i\) 个关节的旋量”传送”到当前构型下。
物理含义：第 \(i\) 列是将第 \(i\) 个关节的旋量轴经过前 \(i-1\) 个关节的刚体变换后，在当前构型下的空间表示。即 \(\mathcal{V}_s = \sum_{i=1}^n J_{s,i}\,\dot{\theta}_i\)。
2.5.2 物体雅可比 (Body Jacobian)
物体速度旋量 \(\mathcal{V}_b\) 与关节速度的关系：
\[\boxed{\mathcal{V}_b = J_b(\boldsymbol{\theta})\,\dot{\boldsymbol{\theta}}}\] \[J_{b,i}(\boldsymbol{\theta}) = \text{Ad}_{\;e^{-[\mathcal{B}_n]\theta_n} \cdots e^{-[\mathcal{B}_{i+1}]\theta_{i+1}}}(\mathcal{B}_i)\]空间雅可比与物体雅可比的关系：\(J_s(\boldsymbol{\theta}) = \text{Ad}_{T_{sb}(\boldsymbol{\theta})}\,J_b(\boldsymbol{\theta})\)
2.5.3 雅可比矩阵的几何直觉与奇异位形
雅可比矩阵的每一列回答：若以单位速度驱动关节 \(i\)，末端会产生何种空间速度？
考虑一个 6 自由度工业机器人，其雅可比矩阵的奇异位形（\(\det(J) \to 0\) 或 \(\operatorname{rank}(J) < n\)）对应于：
- 工作空间边界奇异: 手臂完全伸展，至少失去一个平动自由度
- 内部奇异: 腕关节轴线重合（如肩-肘-腕共线），失去至少一个转动自由度
在这些位形附近，很小的末端速度要求极大的关节速度——这是阻抗控制和运动规划中必须规避的状态。在实际工程中，可操作度 (manipulability) \(\mu = \sqrt{\det(JJ^T)}\) 是量化当前构型离奇异位形距离的常用指标。
2.6 拉格朗日动力学
2.6.1 连杆动能与质量矩阵的完整构造
对于 \(n\) 自由度串联机器人，第 \(i\) 个连杆的动能由平动动能和转动动能组成：
\[K_i = \frac{1}{2}m_i v_{c_i}^T v_{c_i} + \frac{1}{2}\omega_i^T \mathcal{I}_{c_i} \omega_i\]其中 \(v_{c_i}\) 是连杆质心的线速度，\(\omega_i\) 是连杆的角速度，\(\mathcal{I}_{c_i}\) 是绕质心的惯性张量（在 \(\{s\}\) 系中表示）。
利用速度运动学，质心速度可表示为雅可比矩阵与关节速度的乘积：
\[v_{c_i} = J_{v_i}(\boldsymbol{\theta})\dot{\boldsymbol{\theta}},\quad \omega_i = J_{\omega_i}(\boldsymbol{\theta})\dot{\boldsymbol{\theta}}\]其中 \(J_{v_i}, J_{\omega_i} \in \mathbb{R}^{3\times n}\) 是连杆 \(i\) 质心速度的子雅可比（可由空间雅可比的对应行提取）。
代入动能表达式：
\[K_i = \frac{1}{2}\dot{\boldsymbol{\theta}}^T \underbrace{\left(m_i J_{v_i}^T J_{v_i} + J_{\omega_i}^T \mathcal{I}_{c_i} J_{\omega_i}\right)}_{M_i(\boldsymbol{\theta})} \dot{\boldsymbol{\theta}}\]系统的总质量矩阵是各连杆贡献的叠加：
\[\boxed{M(\boldsymbol{\theta}) = \sum_{i=1}^n \left(m_i J_{v_i}^T J_{v_i} + J_{\omega_i}^T \mathcal{I}_{c_i} J_{\omega_i}\right)}\]总动能：\(K = \frac{1}{2}\dot{\boldsymbol{\theta}}^T M(\boldsymbol{\theta})\dot{\boldsymbol{\theta}}\)。\(M(\boldsymbol{\theta})\) 是对称正定的 \(n\times n\) 矩阵。
2.6.2 势能
连杆 \(i\) 的重力势能为 \(P_i = m_i g^T h_{c_i}(\boldsymbol{\theta})\)，其中 \(h_{c_i}\) 是质心在重力方向上的高度。总势能 \(P(\boldsymbol{\theta}) = \sum_i P_i\)。
重力矩向量定义为：
\[g(\boldsymbol{\theta}) = \frac{\partial P}{\partial \boldsymbol{\theta}} \in \mathbb{R}^n\]2.6.3 拉格朗日方程的标准形式
拉格朗日量 \(\mathcal{L} = K - P\)。拉格朗日运动方程：
\[\frac{d}{dt}\frac{\partial \mathcal{L}}{\partial \dot{\theta}_i} - \frac{\partial \mathcal{L}}{\partial \theta_i} = \tau_i,\quad i = 1,\ldots,n\]第一项 \(\frac{d}{dt}\frac{\partial K}{\partial \dot{\theta}_i}\) 产生 \(M(\boldsymbol{\theta})\ddot{\boldsymbol{\theta}} + \dot{M}(\boldsymbol{\theta})\dot{\boldsymbol{\theta}}\)。第二项 \(-\frac{\partial K}{\partial \theta_i} + \frac{\partial P}{\partial \theta_i}\) 产生科里奥利力/离心力 + 重力项。整理为：
\[\boxed{M(\boldsymbol{\theta})\ddot{\boldsymbol{\theta}} + C(\boldsymbol{\theta}, \dot{\boldsymbol{\theta}})\dot{\boldsymbol{\theta}} + g(\boldsymbol{\theta}) = \boldsymbol{\tau}}\]每一项的物理含义：
- \(M(\boldsymbol{\theta})\ddot{\boldsymbol{\theta}}\) — 惯性力 (inertial coupling between joints)
- \(C(\boldsymbol{\theta}, \dot{\boldsymbol{\theta}})\dot{\boldsymbol{\theta}}\) — 科里奥利力与离心力，来自质量矩阵对构型的依赖
- \(g(\boldsymbol{\theta})\) — 重力项 (gravity loading)
- \(\boldsymbol{\tau}\) — 广义力 (joint torques/forces)
2.6.4 科里奥利矩阵的 Christoffel 符号构造
\(C(\boldsymbol{\theta}, \dot{\boldsymbol{\theta}})\) 的第 \((i,j)\) 元素为：
\[C_{ij} = \sum_{k=1}^{n} \Gamma_{ijk}\,\dot{\theta}_k\]其中第一类 Christoffel 符号：
\[\boxed{\Gamma_{ijk} = \frac{1}{2}\left(\frac{\partial M_{ij}}{\partial \theta_k} + \frac{\partial M_{ik}}{\partial \theta_j} - \frac{\partial M_{jk}}{\partial \theta_i}\right)}\]这一构造保证了 \(\dot{M} - 2C\) 的反对称性质：
\[\boxed{(\dot{M} - 2C)^T = -(\dot{M} - 2C)}\]等价于 \(x^T(\dot{M} - 2C)x = 0\) 对所有 \(x \in \mathbb{R}^n\) 成立。物理含义：外力做功的功率 \(\dot{\boldsymbol{\theta}}^T\boldsymbol{\tau}\) 等于动能变化率 \(\frac{d}{dt}K\) 加上势能变化率加上摩擦耗散——即能量守恒。
这一结构性质在无源控制 (passivity-based control) 中至关重要：如果令控制律为 \(\boldsymbol{\tau} = g(\boldsymbol{\theta}) - K_p\tilde{\boldsymbol{\theta}} - K_d\dot{\boldsymbol{\theta}}\)（重力补偿 + PD），则闭环系统满足无源性，Lyapunov 函数 \(V = \frac{1}{2}\dot{\boldsymbol{\theta}}^T M\dot{\boldsymbol{\theta}} + \frac{1}{2}\tilde{\boldsymbol{\theta}}^T K_p \tilde{\boldsymbol{\theta}}\) 的导数 \(\dot{V} = -\dot{\boldsymbol{\theta}}^T K_d \dot{\boldsymbol{\theta}} \leq 0\)，保证了全局渐近稳定性。
2.6.5 单连杆完整推导（拉格朗日方法）
考虑质量为 \(m\)、长度为 \(l\)、质心在距轴 \(l_c\) 处的单摆：
- 动能: \(K = \frac{1}{2}I\dot{\theta}^2\)，\(I = ml_c^2 + I_{\text{cm}}\)
- 势能: \(P = m g l_c(1 - \cos\theta)\)
- 拉格朗日量: \(\mathcal{L} = \frac{1}{2}I\dot{\theta}^2 - m g l_c(1 - \cos\theta)\)
- \(\frac{d}{dt}\frac{\partial\mathcal{L}}{\partial\dot{\theta}} = I\ddot{\theta}\)，\(\frac{\partial\mathcal{L}}{\partial\theta} = -m g l_c\sin\theta\)
- 运动方程: \(I\ddot{\theta} + m g l_c\sin\theta = \tau\)
2.6.6 二连杆平面机器人完整推导（拉格朗日方法）
设两连杆长度 \(l_1, l_2\)，质量 \(m_1, m_2\)，质心 \(l_{c_1}, l_{c_2}\)（从近端测量），转动惯量 \(I_1, I_2\)。关节角 \(\theta_1, \theta_2\) 从水平方向测量。
质心位置： \(\begin{aligned} x_{c_1} &= l_{c_1}\cos\theta_1,\quad y_{c_1} = l_{c_1}\sin\theta_1 \\ x_{c_2} &= l_1\cos\theta_1 + l_{c_2}\cos(\theta_1+\theta_2),\quad y_{c_2} = l_1\sin\theta_1 + l_{c_2}\sin(\theta_1+\theta_2) \end{aligned}\)
速度： \(\begin{aligned} \dot{x}_{c_1} &= -l_{c_1}\dot{\theta}_1\sin\theta_1,\quad \dot{y}_{c_1} = l_{c_1}\dot{\theta}_1\cos\theta_1 \\ \dot{x}_{c_2} &= -l_1\dot{\theta}_1\sin\theta_1 - l_{c_2}(\dot{\theta}_1+\dot{\theta}_2)\sin(\theta_1+\theta_2) \\ \dot{y}_{c_2} &= l_1\dot{\theta}_1\cos\theta_1 + l_{c_2}(\dot{\theta}_1+\dot{\theta}_2)\cos(\theta_1+\theta_2) \end{aligned}\)
质量矩阵： \(M(\boldsymbol{\theta}) = \begin{bmatrix} M_{11} & M_{12} \\ M_{21} & M_{22} \end{bmatrix}\)
其中： \(\begin{aligned} M_{11} &= I_1 + I_2 + m_1 l_{c_1}^2 + m_2(l_1^2 + l_{c_2}^2 + 2l_1 l_{c_2}\cos\theta_2) \\ M_{12} = M_{21} &= I_2 + m_2(l_{c_2}^2 + l_1 l_{c_2}\cos\theta_2) \\ M_{22} &= I_2 + m_2 l_{c_2}^2 \end{aligned}\)
\(M_{11}\) 中 \(2m_2 l_1 l_{c_2}\cos\theta_2\) 项体现了连杆 2 的位置对连杆 1 转动惯量的动态贡献。这是串联机器人动力学的核心特征：构型相关的惯性耦合。
科里奥利/离心力项： \(C(\boldsymbol{\theta}, \dot{\boldsymbol{\theta}}) = \begin{bmatrix} -2m_2 l_1 l_{c_2}\dot{\theta}_2\sin\theta_2 & -m_2 l_1 l_{c_2}\dot{\theta}_2\sin\theta_2 \\ m_2 l_1 l_{c_2}\dot{\theta}_1\sin\theta_2 & 0 \end{bmatrix}\)
重力项： \(g(\boldsymbol{\theta}) = \begin{bmatrix} (m_1 l_{c_1} + m_2 l_1)g\cos\theta_1 + m_2 l_{c_2}g\cos(\theta_1+\theta_2) \\ m_2 l_{c_2}g\cos(\theta_1+\theta_2) \end{bmatrix}\)
2.7 牛顿-欧拉逆动力学 (RNEA)
2.7.1 问题表述
逆动力学 (inverse dynamics): 给定 \((\boldsymbol{\theta}, \dot{\boldsymbol{\theta}}, \ddot{\boldsymbol{\theta}})\)，计算产生该运动所需的关节力矩 \(\boldsymbol{\tau}\)。
递归牛顿-欧拉算法 (Recursive Newton-Euler Algorithm, RNEA) 利用链式结构的优势，在 O(\(n\)) 时间内完成计算，无需显式构造 \(M(\boldsymbol{\theta})\)。
2.7.2 前向递推 (Forward Recursion): 计算连杆速度与加速度
从基座 \((i=0)\) 向末端 \((i=n)\) 递推。对于每个连杆 \(i\)：
转动关节的速度/加速度递推（Lynch-Park §8.3.1）：
\[\begin{aligned} \omega_i &= \omega_{i-1} + \dot{\theta}_i\,\hat{z}_{i-1} \\[2pt] v_i &= v_{i-1} + \omega_i \times r_{i-1,i} \end{aligned}\]其中 \(r_{i-1,i}\) 是从连杆 \(i-1\) 的原点到连杆 \(i\) 的原点的向量，\(\hat{z}_{i-1}\) 是关节 \(i\) 的转轴。
加速度递推（考虑科里奥利项和离心项）：
\[\begin{aligned} \dot{\omega}_i &= \dot{\omega}_{i-1} + \ddot{\theta}_i\,\hat{z}_{i-1} + \dot{\theta}_i\,\omega_{i-1} \times \hat{z}_{i-1} \\[2pt] \dot{v}_i &= \dot{v}_{i-1} + \dot{\omega}_i \times r_{i-1,i} + \omega_i \times (\omega_i \times r_{i-1,i}) \end{aligned}\]质心线加速度（用于后续牛顿方程）：
\[\boxed{\dot{v}_{c_i} = \dot{v}_i + \dot{\omega}_i \times r_{i,c_i} + \omega_i \times (\omega_i \times r_{i,c_i})}\]其中 \(r_{i,c_i}\) 是从连杆 \(i\) 原点到其质心的向量。
物理直觉：旋转关节在串联链中产生的角速度是累积的（\(\omega_i = \omega_{i-1} + \dot{\theta}_i\hat{z}\)），而连杆间的相对运动通过刚性连接传递，产生科里奥利和离心效应（\(\omega_i \times (\omega_i \times r)\) 项）。
移动关节的速度递推（无角速度贡献，平移速度经旋转传递）：
\[v_i = v_{i-1} + \dot{\theta}_i\,\hat{z}_{i-1} + \omega_i \times r_{i-1,i}\]2.7.3 后向递推 (Backward Recursion): 计算力与力矩
从末端 \((i=n)\) 向基座 \((i=1)\) 递推。对每个连杆 \(i\)：
牛顿方程（平动）： \(F_i = m_i\,\dot{v}_{c_i}\)
欧拉方程（转动，在质心处）： \(\boxed{N_i = \mathcal{I}_{c_i}\,\dot{\omega}_i + \omega_i \times (\mathcal{I}_{c_i}\,\omega_i)}\)
其中 \(\mathcal{I}_{c_i}\) 是连杆 \(i\) 绕其质心的惯性张量（在基坐标系 \(\{s\}\) 中表示）。
力/力矩平衡与关节力矩提取：
设 \(f_i\) 为连杆 \(i-1\) 通过关节 \(i\) 施加于连杆 \(i\) 的力，\(n_i\) 为对应的力矩。力/力矩平衡给出：
\[\begin{aligned} f_i &= F_i + f_{i+1} \\[2pt] n_i &= N_i + n_{i+1} + r_{i,c_i} \times F_i + r_{i,i+1} \times f_{i+1} \end{aligned}\]关节力矩 \(\tau_i\) 是将 \(n_i\) 投影到关节轴方向的分量。对于转动关节：
\[\boxed{\tau_i = n_i^T\,\hat{z}_{i-1}}\]对于移动关节：\(\tau_i = f_i^T\,\hat{z}_{i-1}\)（力沿关节轴方向的分量）。
2.7.4 RNEA 的计算复杂度与算法选择
RNEA 的复杂度为 O(\(n\))，远优于拉格朗日方法的 O(\(n^3\))。下表总结了三者关键差异：
| 维度 | 拉格朗日 | 牛顿-欧拉 (RNEA) | 复合刚体算法 (CRBA) |
|---|---|---|---|
| 构造 \(M(\boldsymbol{\theta})\) | O(\(n^3\)) | 不构造 | O(\(n^2\)) (递归) |
| 求逆动力学 \(\boldsymbol{\tau}\) | 先求 M 再乘 \(\ddot{\boldsymbol{\theta}}\) | O(\(n\)) 直接计算 | 需要 RNEA 作为子程序 |
| 求前向动力学 \(\ddot{\boldsymbol{\theta}}\) | \(M^{-1}(\boldsymbol{\tau} - C\dot{\boldsymbol{\theta}} - g)\) | 不适用 | O(\(n\)) (利用 M 的稀疏结构) |
| 实时控制适用性 | 仅 n ≤ 3 | 任意 n | 任意 n |
所有现代机器人仿真引擎（MuJoCo, Isaac Gym, Drake, Pinocchio）的核心都使用 RNEA + CRBA 的组合。
RNEA 与拉格朗日方法的一致性：对于同一 \((\boldsymbol{\theta}, \dot{\boldsymbol{\theta}}, \ddot{\boldsymbol{\theta}})\)，RNEA 的输出严格等于 \(M(\boldsymbol{\theta})\ddot{\boldsymbol{\theta}} + C(\boldsymbol{\theta},\dot{\boldsymbol{\theta}})\dot{\boldsymbol{\theta}} + g(\boldsymbol{\theta})\)——两者是同一物理问题（刚体动力学）的两种计算方法，而非不同模型。
三、算法详解
3.1 算法 1: SE(3) 指数映射
输入: 旋量轴 \(\mathcal{S} = (\omega, v) \in \mathbb{R}^6\)，关节位移 \(\theta \in \mathbb{R}\) 输出: 齐次变换矩阵 \(T \in SE(3)\)
步骤:
- 若 \(\|\omega\| < \epsilon\)（纯平移）:
- \(T = \begin{bmatrix} I & v\theta \\ 0 & 1 \end{bmatrix}\)，返回
- 归一化: \(\|\omega\| = \sqrt{\omega^T\omega}\), \(\hat{\omega} = \omega / \|\omega\|\), \(\theta' = \|\omega\|\theta\)
- 计算 \(R = I + \sin\theta'[\hat{\omega}] + (1-\cos\theta')[\hat{\omega}]^2\)
- 计算 \(G = I\theta' + (1-\cos\theta')[\hat{\omega}] + (\theta'-\sin\theta')[\hat{\omega}]^2\)
- \[T = \begin{bmatrix} R & G v/\|\omega\| \\ 0 & 1 \end{bmatrix}\]
3.2 算法 2: PoE 前向运动学（空间形式）
输入: 空间旋量轴列表 \(\{\mathcal{S}_1, \ldots, \mathcal{S}_n\}\)，零位形 \(M\)，关节角度 \(\{\theta_1, \ldots, \theta_n\}\) 输出: 末端位姿 \(T_{sb}(\boldsymbol{\theta})\)
步骤:
- \[T \leftarrow I_4\]
- for \(i = 1\) to \(n\):
- \(T \leftarrow T \cdot e^{[\mathcal{S}_i]\theta_i}\) （调用算法 3.1）
- \[T_{sb} \leftarrow T \cdot M\]
3.3 算法 3: 空间雅可比计算
输入: 空间旋量轴列表 \(\{\mathcal{S}_i\}\)，关节角度 \(\{\theta_i\}\)，末端位姿 \(T_{sb}\) 输出: 空间雅可比 \(J_s \in \mathbb{R}^{6\times n}\)，物体雅可比 \(J_b \in \mathbb{R}^{6\times n}\)
步骤:
- \(T \leftarrow I_4\), \(J_s \leftarrow 0_{6\times n}\)
- for \(i = 1\) to \(n\):
- \[J_s[:,i] \leftarrow \text{Ad}_T(\mathcal{S}_i)\]
- \[T \leftarrow T \cdot e^{[\mathcal{S}_i]\theta_i}\]
- \[J_b \leftarrow \text{Ad}_{T_{sb}^{-1}} \cdot J_s\]
3.4 算法 4: RNEA 逆动力学
输入: \(\boldsymbol{\theta}, \dot{\boldsymbol{\theta}}, \ddot{\boldsymbol{\theta}}\)，连杆参数 \(\{m_i, c_i, \mathcal{I}_i, \hat{z}_{i-1}, r_{i-1,i}\}\) 输出: 关节力矩 \(\boldsymbol{\tau} \in \mathbb{R}^n\)
阶段 1 — 前向递推 (\(\omega_0=0, \dot{\omega}_0=0, v_0=0, \dot{v}_0=-g\))： for \(i = 0\) to \(n-1\):
- \(\omega_{i+1} \leftarrow \omega_i + \delta_{\text{rev}}\dot{\theta}_{i+1}\hat{z}_i\)（\(\delta_{\text{rev}}=1\) 若转动，\(0\) 若移动）
- \[\dot{\omega}_{i+1} \leftarrow \dot{\omega}_i + \delta_{\text{rev}}(\ddot{\theta}_{i+1}\hat{z}_i + \dot{\theta}_{i+1}\omega_i\times\hat{z}_i)\]
- \[v_{i+1} \leftarrow v_i + \delta_{\text{pris}}\dot{\theta}_{i+1}\hat{z}_i + \omega_{i+1}\times r_{i,i+1}\]
- \[\dot{v}_{i+1} \leftarrow \dot{v}_i + \delta_{\text{pris}}(\ddot{\theta}_{i+1}\hat{z}_i + 2\dot{\theta}_{i+1}\omega_{i+1}\times\hat{z}_i) + \dot{\omega}_{i+1}\times r_{i,i+1} + \omega_{i+1}\times(\omega_{i+1}\times r_{i,i+1})\]
- \[\dot{v}_{c_{i+1}} \leftarrow \dot{v}_{i+1} + \dot{\omega}_{i+1}\times r_{i+1,c_{i+1}} + \omega_{i+1}\times(\omega_{i+1}\times r_{i+1,c_{i+1}})\]
阶段 2 — 后向递推 (\(f_{n+1}=0\), \(n_{n+1}=0\))： for \(i = n\) down to \(1\):
- \[F_i \leftarrow m_i \dot{v}_{c_i}\]
- \[N_i \leftarrow \mathcal{I}_{c_i}\dot{\omega}_i + \omega_i\times(\mathcal{I}_{c_i}\omega_i)\]
- \[f_i \leftarrow F_i + f_{i+1}\]
- \[n_i \leftarrow N_i + n_{i+1} + r_{i,c_i}\times F_i + r_{i,i+1}\times f_{i+1}\]
- \[\tau_i \leftarrow \delta_{\text{rev}}(n_i^T\hat{z}_{i-1}) + \delta_{\text{pris}}(f_i^T\hat{z}_{i-1})\]
3.5 逆动力学的三种应用场景
| 场景 | 输入 | 输出 | 核心公式 |
|---|---|---|---|
| 前向仿真 | \(\boldsymbol{\tau}\) (外力) | \(\ddot{\boldsymbol{\theta}}\) | \(\ddot{\boldsymbol{\theta}} = M^{-1}(\boldsymbol{\tau} - C\dot{\boldsymbol{\theta}} - g)\) |
| 计算力矩控制 | \(\boldsymbol{\theta}_d\) (期望轨迹) | \(\boldsymbol{\tau}\) | \(\boldsymbol{\tau} = M(\ddot{\boldsymbol{\theta}}_d + K_d\dot{e} + K_p e) + C\dot{\boldsymbol{\theta}} + g\) |
| 逆动力学 | \(\boldsymbol{\theta}, \dot{\boldsymbol{\theta}}, \ddot{\boldsymbol{\theta}}\) | \(\boldsymbol{\tau}\) | \(\texttt{RNEA}(\boldsymbol{\theta}, \dot{\boldsymbol{\theta}}, \ddot{\boldsymbol{\theta}})\) |
四、算法与代码实现
以下代码基于 NumPy 实现，完整呈现本章核心数学结构。每个函数标注了对应的公式章节。
4.1 反对称矩阵与李代数基础
import numpy as np
from numpy.linalg import norm
def skew(w):
"""构造反对称矩阵 [w] ∈ so(3)
对应公式 2.2.2: [ω] = [[0, -ω₃, ω₂],
[ω₃, 0, -ω₁],
[-ω₂, ω₁, 0]]
Args:
w: (3,) 向量
Returns:
(3,3) 反对称矩阵
"""
return np.array([
[ 0, -w[2], w[1]],
[ w[2], 0, -w[0]],
[-w[1], w[0], 0]
])
def unskew(W):
"""从反对称矩阵恢复向量 (skew 的逆运算)
Args:
W: (3,3) 反对称矩阵
Returns:
(3,) 向量
"""
return np.array([W[2, 1], W[0, 2], W[1, 0]])
def adjoint(T):
"""计算 SE(3) 变换 T 的伴随表示 Ad_T ∈ R^{6×6}
对应 2.3.3 节伴随矩阵推导:
Ad_T = [[R, 0],
[[p]R, R]]
Args:
T: (4,4) SE(3) 齐次变换矩阵
Returns:
(6,6) 伴随矩阵
"""
R = T[:3, :3]
p = T[:3, 3]
Ad = np.zeros((6, 6))
Ad[:3, :3] = R
Ad[3:, 3:] = R
Ad[3:, :3] = skew(p) @ R
return Ad
4.2 SO(3) 与 SE(3) 的指数/对数映射
def so3_exp(omega_theta):
"""so(3) → SO(3) 指数映射 (Rodrigues 公式)
对应 2.2.3 节公式:
R = I + sin(θ)[ω̂] + (1-cos(θ))[ω̂]²
Args:
omega_theta: (3,) 指数坐标 ω̂θ (旋转轴 × 角度)
Returns:
R: (3,3) 旋转矩阵 ∈ SO(3)
"""
theta = norm(omega_theta)
if theta < 1e-12:
return np.eye(3)
omega_hat = omega_theta / theta
W = skew(omega_hat)
R = np.eye(3) + np.sin(theta) * W + (1.0 - np.cos(theta)) * (W @ W)
return R
def so3_log(R):
"""SO(3) → so(3) 对数映射
对应 2.2.4 节公式:
θ = arccos((tr(R)-1)/2)
[ω̂] = (R-R^T) / (2 sin θ), θ ≠ 0, π
Args:
R: (3,3) 旋转矩阵 ∈ SO(3)
Returns:
omega_theta: (3,) 指数坐标 ω̂θ
"""
# 数值稳定性: 夹紧 arccos 参数到 [-1, 1]
cos_theta = np.clip((np.trace(R) - 1.0) / 2.0, -1.0, 1.0)
theta = np.arccos(cos_theta)
# 情况 1: θ ≈ 0 (R ≈ I)
if theta < 1e-12:
return np.zeros(3)
# 情况 2: θ ≈ π (R 对称, R-R^T ≈ 0, 公式奇异)
if np.abs(theta - np.pi) < 1e-10:
# R = 2 ω̂ω̂^T - I, 故 R+I 的非零列平行于 ω̂
R_plus_I = R + np.eye(3)
# 选择范数最大的列以避免数值退化
col_norms = np.sum(R_plus_I ** 2, axis=0)
best_col = np.argmax(col_norms)
v = R_plus_I[:, best_col]
v_norm = norm(v)
if v_norm < 1e-10:
# 退化情况: 绕多个轴均为 π
return np.zeros(3)
omega_hat = v / v_norm
return omega_hat * theta
# 情况 3: 一般情况 (0 < θ < π 且 θ 不接近 π)
omega_hat = unskew((R - R.T) / (2.0 * np.sin(theta)))
return omega_hat * theta
def se3_exp(S_theta):
"""se(3) → SE(3) 指数映射
对应 2.3.5 节闭合公式。
情况 1 (含旋转, ||ω|| ≠ 0):
T = [[R, G(θ)v],
[0_{1×3}, 1 ]]
情况 2 (纯平移, ||ω|| = 0):
T = [[I, vθ],
[0, 1 ]]
Args:
S_theta: (6,) 旋量 × 角度 [ωθ; vθ]
Returns:
T: (4,4) SE(3) 齐次变换矩阵
"""
omega_theta = S_theta[:3]
v_theta = S_theta[3:]
theta_omega = norm(omega_theta)
T = np.eye(4)
# 情况 2: 纯平移
if theta_omega < 1e-12:
T[:3, 3] = v_theta
return T
# 情况 1: 包含旋转分量
omega_hat = omega_theta / theta_omega
W = skew(omega_hat)
# Rodrigues: R ∈ SO(3)
R = np.eye(3) + np.sin(theta_omega) * W + (1.0 - np.cos(theta_omega)) * (W @ W)
# G(θ) = Iθ + (1-cosθ)[ω̂] + (θ-sinθ)[ω̂]² (Lynch-Park Eq 3.74)
G = (np.eye(3) * theta_omega
+ (1.0 - np.cos(theta_omega)) * W
+ (theta_omega - np.sin(theta_omega)) * (W @ W))
T[:3, :3] = R
# v = vθ / θ 是为了将 vθ = v·θ 还原为旋量轴的 v 分量
T[:3, 3] = G @ (v_theta / theta_omega)
return T
def se3_log(T):
"""SE(3) → se(3) 对数映射
Args:
T: (4,4) SE(3) 齐次变换矩阵
Returns:
S_theta: (6,) 旋量 × 角度
"""
R = T[:3, :3]
p = T[:3, 3]
omega_theta = so3_log(R)
theta = norm(omega_theta)
# 纯平移
if theta < 1e-12:
return np.concatenate([np.zeros(3), p])
omega_hat = omega_theta / theta
W = skew(omega_hat)
# G^{-1}(θ) = I/θ - [ω̂]/2 + (1/θ - cot(θ/2)/2)[ω̂]²
# 对应 Lynch-Park Eq 3.85
cot_half = 1.0 / np.tan(theta / 2.0)
G_inv = (np.eye(3) / theta
- 0.5 * W
+ (1.0 / theta - 0.5 * cot_half) * (W @ W))
v_theta = G_inv @ p * theta
return np.concatenate([omega_theta, v_theta])
4.3 Product-of-Exponentials 前向运动学与雅可比
class PoEKinematics:
"""Product-of-Exponentials 前向运动学模型
对应 2.4.3 节 PoE 空间形式:
T_sb(θ) = e^{[S₁]θ₁} e^{[S₂]θ₂} ... e^{[Sₙ]θₙ} M
使用示例:
>>> poe = PoEKinematics(S_list, M)
>>> T = poe.forward_kinematics(theta)
>>> J_s = poe.space_jacobian(theta)
>>> V_s = poe.end_effector_velocity(theta, theta_dot)
"""
def __init__(self, S_list, M):
"""
Args:
S_list: List[(6,)], 各关节在零位形处的空间旋量轴
M: (4,4), 零位形时末端在固定系中的位姿
"""
self.S_list = [np.asarray(s, dtype=float) for s in S_list]
self.M = np.asarray(M, dtype=float)
self.n = len(S_list)
self._validate()
def _validate(self):
"""验证输入维度"""
assert self.M.shape == (4, 4), f"M must be 4x4, got {self.M.shape}"
for i, s in enumerate(self.S_list):
assert s.shape == (6,), f"S[{i}] must be (6,), got {s.shape}"
def forward_kinematics(self, theta):
"""前向运动学: 关节角度 → 末端位姿
对应算法 3.2。
Args:
theta: (n,) 关节角度
Returns:
T_sb: (4,4) 末端位姿矩阵
"""
theta = np.asarray(theta, dtype=float)
T = np.eye(4)
for S, th in zip(self.S_list, theta):
T = T @ se3_exp(S * th)
return T @ self.M
def space_jacobian(self, theta):
"""计算空间雅可比 J_s(θ)
对应 2.5.1 节公式与算法 3.3:
J_{s,i} = Ad_{∏_{j=1}^{i-1} e^{[Sⱼ]θⱼ}} (S_i)
Args:
theta: (n,) 关节角度
Returns:
J_s: (6, n) 空间雅可比矩阵
"""
theta = np.asarray(theta, dtype=float)
J = np.zeros((6, self.n))
T = np.eye(4)
for i in range(self.n):
S_i = self.S_list[i]
J[:, i] = adjoint(T) @ S_i
T = T @ se3_exp(S_i * theta[i])
return J
def body_jacobian(self, theta):
"""计算物体雅可比 J_b(θ)
利用关系 J_b = Ad_{T_sb^{-1}} J_s (2.5.2 节)
Args:
theta: (n,) 关节角度
Returns:
J_b: (6, n) 物体雅可比矩阵
"""
T_sb = self.forward_kinematics(theta)
J_s = self.space_jacobian(theta)
# Ad_{T^{-1}} = (Ad_T)^{-1}
Ad_inv = adjoint(np.linalg.inv(T_sb))
return Ad_inv @ J_s
def manip_ellipsoid(self, theta):
"""计算速度可操作度椭球 (velocity manipulability ellipsoid)
可操作度 μ = sqrt(det(JJ^T)) 量化当前构型离奇异位形的距离。
Args:
theta: (n,) 关节角度
Returns:
mu: 可操作度标量
axes: 椭球主轴方向 (SVD 的 U 矩阵)
lengths: 各主轴长度 (奇异值)
"""
J_s = self.space_jacobian(theta)
U, s, Vt = np.linalg.svd(J_s, full_matrices=False)
mu = np.prod(s)
return mu, U, s
def end_effector_velocity(self, theta, theta_dot):
"""计算末端执行器的空间速度旋量
Args:
theta: (n,) 关节角度
theta_dot: (n,) 关节速度
Returns:
V_s: (6,) 空间速度旋量 [ω; v]
"""
J_s = self.space_jacobian(theta)
return J_s @ np.asarray(theta_dot)
4.4 递归牛顿-欧拉逆动力学 (RNEA)
class RNEA:
"""递归牛顿-欧拉逆动力学 (Recursive Newton-Euler Algorithm)
对应算法 3.4 与 2.7 节理论。O(n) 时间复杂度。
"""
def __init__(self, links, gravity=np.array([0.0, 0.0, -9.81])):
"""
Args:
links: List[dict], 每条连杆包含:
- 'mass' (float): 质量 m_i
- 'com' (3,): 质心在连杆坐标系中的位置
- 'inertia' (3,3): 绕质心的惯性张量 (连杆坐标系中)
- 'R_parent_to_link' (3,3): 从父连杆系到该连杆系的旋转矩阵
- 'p_parent_to_link' (3,): 从父连杆原点到该连杆原点的位移
- 'joint_axis' (3,): 关节轴在父坐标系中的方向
- 'joint_type' (str): 'revolute' 或 'prismatic'
gravity: (3,) 重力加速度向量 (基坐标系中)
"""
self.links = links
self.n = len(links)
self.gravity = np.asarray(gravity, dtype=float)
def compute(self, theta, theta_dot, theta_ddot):
"""计算逆动力学
Args:
theta: (n,) 关节位置
theta_dot: (n,) 关节速度
theta_ddot: (n,) 关节加速度
Returns:
tau: (n,) 关节力矩
"""
theta = np.asarray(theta, dtype=float)
theta_dot = np.asarray(theta_dot, dtype=float)
theta_ddot = np.asarray(theta_ddot, dtype=float)
n = self.n
# --- 阶段 1: 前向递推 (Forward Recursion, 2.7.2 节) ---
# 存储中间结果: v[0] 对应基座, v[k] 对应连杆 k
omega = [np.zeros(3) for _ in range(n + 1)]
alpha = [np.zeros(3) for _ in range(n + 1)]
a_lin = [np.zeros(3) for _ in range(n + 1)]
a_com = [np.zeros(3) for _ in range(n)]
# 基座加速度: 等效原理 — 基座以 -g "加速"等价于重力场
a_lin[0] = -self.gravity
for i in range(n):
link = self.links[i]
R = link['R_parent_to_link']
p = link['p_parent_to_link']
z = link['joint_axis']
is_rev = (link['joint_type'] == 'revolute')
# 角速度递推
omega[i+1] = R @ omega[i]
if is_rev:
omega[i+1] = omega[i+1] + theta_dot[i] * z
# 角加速度递推
alpha[i+1] = R @ alpha[i]
if is_rev:
alpha[i+1] = (alpha[i+1]
+ theta_ddot[i] * z
+ theta_dot[i] * np.cross(R @ omega[i], z))
# 线加速度递推 (连杆原点处)
a_lin[i+1] = (R @ a_lin[i]
+ np.cross(alpha[i+1], p)
+ np.cross(omega[i+1], np.cross(omega[i+1], p)))
if not is_rev:
# 移动关节: 额外的牵连加速度项
a_lin[i+1] = (a_lin[i+1]
+ theta_ddot[i] * z
+ 2.0 * theta_dot[i] * np.cross(omega[i+1], z))
# 质心线加速度
com = link['com']
a_com[i] = (a_lin[i+1]
+ np.cross(alpha[i+1], com)
+ np.cross(omega[i+1], np.cross(omega[i+1], com)))
# --- 阶段 2: 后向递推 (Backward Recursion, 2.7.3 节) ---
F_next = np.zeros(3) # f_{i+1}
N_next = np.zeros(3) # n_{i+1}
tau = np.zeros(n)
for i in range(n - 1, -1, -1):
link = self.links[i]
com = link['com']
I_com = link['inertia']
# 末端执行器对最后一根连杆的作用力/力矩 (默认 0)
# 牛顿: F_i = m_i a_{c_i} + f_{i+1}
F_i = link['mass'] * a_com[i] + F_next
# 欧拉: N_i = I_c α + ω × (I_c ω) + n_{i+1}
# + r_{c_i} × (m_i a_{c_i}) + r_{i,i+1} × f_{i+1}
N_i = (I_com @ alpha[i+1]
+ np.cross(omega[i+1], I_com @ omega[i+1])
+ N_next
+ np.cross(com, link['mass'] * a_com[i])
+ np.cross(link['p_parent_to_link'], F_next))
# 投影到关节轴获取关节力矩
z = link['joint_axis']
if link['joint_type'] == 'revolute':
tau[i] = np.dot(N_i, z)
else:
tau[i] = np.dot(F_i, z)
F_next = F_i
N_next = N_i
return tau
4.5 完整示例：二连杆平面机器人验证
def two_link_planar_validation():
"""二连杆平面机器人: PoE + RNEA 交叉验证
机器人参数 (SI 单位):
- 连杆 1: 长度 1.0m, 质量 2.0kg, 质心在中点
- 连杆 2: 长度 1.0m, 质量 1.0kg, 质心在中点
验证项目:
1. PoE 前向运动学与显式指数积的一致性
2. 悬垂静止状态 RNEA 输出与重力补偿力矩的匹配
3. 运动状态下 RNEA 输出与拉格朗日解析公式的一致性
"""
L1, L2 = 1.0, 1.0
m1, m2 = 2.0, 1.0
g_val = 9.81
# ---- PoE 旋量轴 (在固定系中) ----
# 关节 1: 绕 z 轴旋转，轴线通过原点
S1 = np.array([0.0, 0.0, 1.0, 0.0, 0.0, 0.0])
# 关节 2: 绕 z 轴旋转，在零位形 (θ₁=0, θ₂=0) 时
# 轴线通过 q₂ = (l₁, 0, 0)
# v₂ = -ω₂ × q₂ = -(0,0,1) × (1,0,0) = (0,-1,0)
S2 = np.array([0.0, 0.0, 1.0, 0.0, -1.0, 0.0])
# 零位形: 连杆完全伸展，末端在 (2, 0, 0)
M = np.eye(4)
M[:3, 3] = [2.0, 0.0, 0.0]
poe = PoEKinematics([S1, S2], M)
# ---- RNEA 连杆定义 ----
links = [
{
'mass': m1,
'com': np.array([L1 / 2.0, 0.0, 0.0]),
'inertia': np.diag([0.01, m1 * L1**2 / 12.0, m1 * L1**2 / 12.0]),
'R_parent_to_link': np.eye(3),
'p_parent_to_link': np.array([0.0, 0.0, 0.0]),
'joint_axis': np.array([0.0, 0.0, 1.0]),
'joint_type': 'revolute',
},
{
'mass': m2,
'com': np.array([L2 / 2.0, 0.0, 0.0]),
'inertia': np.diag([0.01, m2 * L2**2 / 12.0, m2 * L2**2 / 12.0]),
'R_parent_to_link': np.eye(3),
'p_parent_to_link': np.array([L1, 0.0, 0.0]),
'joint_axis': np.array([0.0, 0.0, 1.0]),
'joint_type': 'revolute',
},
]
rnea = RNEA(links, gravity=np.array([0.0, 0.0, -g_val]))
# ---- 验证 1: PoE 前向运动学 ----
theta_test = np.array([0.5, -0.8])
T_poe = poe.forward_kinematics(theta_test)
T_explicit = np.eye(4)
for S, th in zip([S1, S2], theta_test):
T_explicit = T_explicit @ se3_exp(S * th)
T_explicit = T_explicit @ M
assert np.allclose(T_poe, T_explicit, atol=1e-10), \
"PoE forward kinematics inconsistent with explicit computation"
print(f"[PASS] PoE FK: |T_poe - T_explicit| = {np.max(np.abs(T_poe - T_explicit)):.2e}")
# ---- 验证 2: 悬垂静止 (仅重力) ----
# θ₁ = π/2 (连杆1水平), θ₂ = 0 (连杆2竖直下垂)
theta_static = np.array([np.pi / 2.0, 0.0])
zeros = np.zeros(2)
tau_static = rnea.compute(theta_static, zeros, zeros)
# 理论值: 连杆1质心在 (0, l_c1) 即 (0, 0.5), 连杆2质心在 (0, l1 + l_c2) = (0, 1.5)
# τ₁ = m₁ g l_c1 + m₂ g (l1 + l_c2) = 2·9.81·0.5 + 1·9.81·1.5 = 24.525
# τ₂ = m₂ g l_c2 = 1·9.81·0.5 = 4.905
tau1_theory = m1 * g_val * L1 / 2.0 + m2 * g_val * (L1 + L2 / 2.0)
tau2_theory = m2 * g_val * L2 / 2.0
print(f"[PASS] Static gravity: τ₁ = {tau_static[0]:.3f} vs theory {tau1_theory:.3f} N·m")
print(f" τ₂ = {tau_static[1]:.3f} vs theory {tau2_theory:.3f} N·m")
assert np.abs(tau_static[0] - tau1_theory) < 0.01
assert np.abs(tau_static[1] - tau2_theory) < 0.01
# ---- 验证 3: 运动状态 ----
theta_dyn = np.array([0.3, 1.2])
theta_dot_dyn = np.array([0.5, -0.3])
theta_ddot_dyn = np.array([2.0, -1.0])
tau_dyn = rnea.compute(theta_dyn, theta_dot_dyn, theta_ddot_dyn)
print(f"Dynamic: τ₁ = {tau_dyn[0]:.3f}, τ₂ = {tau_dyn[1]:.3f} N·m")
# ---- 验证 4: 雅可比与奇异位形 ----
theta_sing = np.array([0.0, np.pi]) # 连杆折叠 (内部奇异)
J_s_sing = poe.space_jacobian(theta_sing)
mu, U, s = poe.manip_ellipsoid(theta_sing)
print(f"\nSingular config (θ={theta_sing}): μ = {mu:.2e}, singular values = {s}")
print(f" Rank(J_s) = {np.linalg.matrix_rank(J_s_sing)} (expected < 3 for planar 2-DOF)")
theta_nonsing = np.array([0.5, 0.5])
mu2, _, s2 = poe.manip_ellipsoid(theta_nonsing)
print(f"Nonsingular config (θ={theta_nonsing}): μ = {mu2:.4f}, σ = {s2}")
print("\n=== All validations passed ===")
if __name__ == "__main__":
two_link_planar_validation()
4.6 与第 2 章的连接：从生物关节到机器人关节的运动学类比
def biological_robot_kinematics_comparison():
"""生物关节与机器人关节的运动学类比
人体髋关节 ≈ 球窝关节 (3 DOF) ≈ 3 个正交转动关节的串联
在 PoE 框架下, 髋关节表示为 3 个旋量轴的积:
关键差异:
- 生物: 肌肉共收缩可瞬时改变关节刚度
- 机器: 刚度由控制器参数 (Kp/Kd) 决定
- 生物: 拮抗肌对提供冗余驱动, 天生具备力控能力
- 机器: 单电机驱动, 力控依赖力矩传感器反馈
"""
# 髋关节旋量轴 (屈伸/内收外展/内外旋)
hip_flexion = np.array([0.0, 1.0, 0.0, 0.0, 0.0, 0.0]) # 绕 y
hip_adduction = np.array([1.0, 0.0, 0.0, 0.0, 0.0, 0.0]) # 绕 x
hip_rotation = np.array([0.0, 0.0, 1.0, 0.0, 0.0, 0.0]) # 绕 z
M_hip = np.eye(4)
hip_poe = PoEKinematics([hip_flexion, hip_adduction, hip_rotation], M_hip)
theta_hip = np.array([0.3, -0.1, 0.2]) # 屈伸30°, 外展-10°, 外旋20°
T_hip = hip_poe.forward_kinematics(theta_hip)
J_hip = hip_poe.space_jacobian(theta_hip)
print("机器人髋关节 (3-DOF 等效球窝):")
print(f" 位姿矩阵:\n{T_hip}")
print(f" 雅可比矩阵 (6x3):\n{J_hip}")
print("\n生物-机器差异总结:")
print(" [刚度调制] 生物可瞬时改变, 机器需控制器参数调整")
print(" [冗余驱动] 生物天然具备, 机器需额外电机/肌腱")
print(" [力控能力] 生物通过长度/张力反馈闭环, 机器依赖传感器")
if __name__ == "__main__":
biological_robot_kinematics_comparison()
五、核心 Takeaway 与延伸阅读
5.1 五个核心洞察
-
指数坐标 = 旋量理论的代数化。SO(3)/SE(3) 的指数映射不是抽象数学，而是 Rodrigues 公式的自然推广。\(e^{[\mathcal{S}]\theta}\) 统一了转动和平移——每当你写下一个旋量轴 \(\mathcal{S} = (\omega, v)\)，你同时在定义关节的运动方向、运动类型（\(\omega \neq 0\): 转动; \(\omega = 0\): 移动）、以及它在空间中的位置（\(v = -\omega \times q\) 编码了轴线位置）。
-
PoE 优于 DH 的本质原因：PoE 将坐标系定义从”每个连杆一个”削减到”全局仅需两个”（固定系和末端系）。这不仅简化了建模，更重要的是避免了 DH 的符号歧义和参数奇异性。在工程中，PoE 的旋量轴可以直接从关节编码器数据和 CAD 模型中辨识——这是现代机器人运动学标定的理论基础。
-
RNEA 是仿真与控制的基础设施。O(\(n\)) 的递推牛顿-欧拉算法使得复杂机器人的实时动力学仿真成为可能。从 MuJoCo 到 Isaac Gym，所有现代物理引擎的核心都是 RNEA + CRBA 的组合。理解 RNEA 的前向-后向递推结构，是理解基于模型的强化学习中”如何将策略输出的力矩转化为运动”的关键。
-
运动学与动力学通过伴随变换互为表里。速度运动学的雅可比 \(J(\boldsymbol{\theta})\) 不仅用于前向运动学的微分，更是静力传递和动力学方程的核心组件：力从任务空间经 \(J^T\) 映射回关节空间，速度从关节空间经 \(J\) 映射到任务空间——这是一个优美的对偶结构，是虚功原理在串联机器人上的直接体现。
-
从生物到机器的鸿沟从力学层面就已开始。生物关节的变刚度、冗余驱动和非线性弹性是机器人领域尚未完全复现的特性。本章的刚体运动学描述了”理想化的机器”，而真实世界的接触、柔顺和摩擦（将在 Ch06 触觉感知和 Ch08 控制中展开）使问题远比本章的数学描述复杂。然而，正是本章建立的精确数学模型，为后续的基于学习的控制方法提供了不可或缺的先验结构 (inductive bias)。
5.2 开放问题与研究前沿
- 软体机器人运动学: 常曲率假设（PCC）下的指数映射推广到非均匀曲率场，\(se(3)\) 的无穷维扩展是活跃的研究领域。
- 可微物理 (Differentiable Physics): 如何在 PyTorch/JAX 中高效实现 SE(3) 运算和 RNEA 的可微版本？这是基于梯度的轨迹优化和 Sim-to-Real 迁移的核心需求。
- 数据驱动的运动学标定: 使用神经网络学习旋量轴的残差修正，融合视觉和关节编码器的多模态标定。
- 接触丰富的动力学: RNEA 假设开链——如何处理多接触（如四足机器人同时有四足着地）？这需要引入约束动力学（Udwadia-Kalaba 方程或 LCP 互补约束）。
5.3 推荐延伸阅读
| 优先级 | 文献 | 内容 |
|---|---|---|
| ★★★★★ | Lynch & Park (2017), Modern Robotics, Ch 3-8 | 本章的主要参考，覆盖 SO(3)/SE(3) 到动力学 |
| ★★★★☆ | Murray, Li & Sastry (1994), A Mathematical Introduction to Robotic Manipulation, Ch 2-4 | 旋量理论的经典教材，数学深度更深 |
| ★★★☆☆ | Featherstone (2008), Rigid Body Dynamics Algorithms | RNEA 及其变体（CRBA、ABA）的权威来源 |
| ★★★☆☆ | Spong, Hutchinson & Vidyasagar (2006), Robot Modeling and Control | 经典的控制导向机器人学教材 |
| ★★★☆☆ | Siciliano et al. (2009), Robotics: Modelling, Planning and Control | 百科全书式的机器人学参考 |
| ★★★☆☆ | Sola, Deray, Atchuthan (2018), “A micro Lie theory for state estimation in robotics” | so(3)/se(3) Jacobian 与扰动模型的工程实践指南 |
六、练习与思考
6.1 基础练习
练习 3.1 (反对称矩阵的幂次) 证明：对于任意 \(\omega \in \mathbb{R}^3\)，有 \([\omega]^3 = -\|\omega\|^2[\omega]\)，并由此推导 Rodrigues 公式的闭合形式。提示：利用 \([\hat{\omega}]^2 = \hat{\omega}\hat{\omega}^T - I\) 以及 \([\hat{\omega}]^3\) 的代换关系。
练习 3.2 (PoE 公式) 考虑一个平面三连杆机器人（所有关节轴平行于 \(z\) 轴，连杆长度均为 \(l\)）。写出其空间旋量轴 \(\mathcal{S}_1, \mathcal{S}_2, \mathcal{S}_3\)，并用 PoE 公式计算末端在 \(\boldsymbol{\theta} = (\pi/4, -\pi/6, \pi/3)\) 下的位姿。用显式三角函数验证 PoE 给出的 \(x, y\) 坐标是否与几何推导一致。
练习 3.3 (RNEA 手工推导) 手工完成二连杆平面机器人的 RNEA 前向递推和后向递推全过程，包括：
- 写出每条连杆的 \((\omega_i, v_i, \dot{\omega}_i, \dot{v}_{c_i})\) 的完整解析表达式
- 从末端反向递推得到 \((\tau_1, \tau_2)\)
- 与 2.6.6 节拉格朗日解析公式进行逐项比对，验证 \(\dot{M} - 2C\) 的反对称性在 RNEA 中自动满足
练习 3.4 (伴随变换的链式性质) 设 \(T_1, T_2 \in SE(3)\)，验证 \(\text{Ad}_{T_1 T_2} = \text{Ad}_{T_1} \text{Ad}_{T_2}\)。进而证明：对于 PoE 空间形式 \(T_{sb}(\boldsymbol{\theta}) = e^{[\mathcal{S}_1]\theta_1} \cdots e^{[\mathcal{S}_n]\theta_n} M\)，有 \(\text{Ad}_{T_{sb}(\boldsymbol{\theta})} = \text{Ad}_{e^{[\mathcal{S}_1]\theta_1}} \cdots \text{Ad}_{e^{[\mathcal{S}_n]\theta_n}} \text{Ad}_{M}\)。
6.2 编程练习
练习 3.5 (数值逆运动学) 扩展 PoEKinematics 类，添加基于 Newton-Raphson 方法的数值逆运动学求解器。利用空间雅可比构造迭代步：\(\Delta\boldsymbol{\theta} = J_s^\dagger(\boldsymbol{\theta}_k) \mathcal{V}_s\)，其中 \(\mathcal{V}_s = \text{se3\_log}(T_{des} \cdot T_{sb}(\boldsymbol{\theta}_k)^{-1})\) 为位姿误差的旋量表示。测试：给定目标位姿 \(T_{des}\)，验证迭代收敛且 \(\|T_{sb}(\boldsymbol{\theta}^*) - T_{des}\| < 10^{-6}\)。
练习 3.6 (可微 SE(3)) 使用 PyTorch 实现 se3_exp 的可微版本（确保 sin, cos, norm 均可自动微分）。验证：对于纯平移情况，\(\partial T / \partial v\) 的自动微分结果是否与解析 Jacobian 一致。
练习 3.7 (RNEA 前向动力学) 利用 RNEA 构造前向动力学求解器：通过逐列计算 \(M(\boldsymbol{\theta})\)（对每个单位基向量 \(e_i\) 调用 RNEA），然后求解 \(M\ddot{\boldsymbol{\theta}} = \boldsymbol{\tau} - C\dot{\boldsymbol{\theta}} - g\)。与 O(\(n^3\)) 的拉格朗日矩阵求逆法对比运行时间（\(n = 2, 4, 8, 16\)）。
6.3 思考题
思考 3.1 (生物-机器运动学鸿沟) 参照第 2 章中人类步态周期的关节角度轨迹，设计一个简化的双足机器人运动学模型（至少 6 DOF），用 PoE 方法描述其前向运动学。讨论：生物关节的”旋量轴”是时变的（如膝关节的瞬时旋转中心在运动过程中滑动），这对机器人的建模和控制意味着什么？是否存在工程上可行的近似方法？
思考 3.2 (学习 vs 建模) 如果可以用神经网络直接从关节角度回归末端位姿，为什么我们还需要 PoE 或 DH 参数？在什么场景下显式运动学模型仍然不可或缺？给出至少三个具体应用场景并论证。
思考 3.3 (逆动力学在前向仿真中的角色) RNEA 的 O(\(n\)) 复杂度使实时逆动力学成为可能。在基于物理的强化学习仿真中（如 Ch11 将涵盖的 SAC），引擎需要求解前向动力学（\(\boldsymbol{\tau} \to \ddot{\boldsymbol{\theta}}\)）。列出前向动力学求解的关键数值挑战（如质量矩阵的条件数、关节摩擦的建模、接触约束的处理），并讨论 RNEA/CRBA 如何应对这些挑战。
思考 3.4 (SO(3) 流形上的优化) 在线性空间中，\(\Delta x = x_{des} - x_k\) 自然地定义为目标与当前的差。但在 SO(3) 流形上，”差”必须是切空间中的元素。讨论：为什么 \(\Delta R = R_{des} - R_k\) 在 SO(3) 上是错误的（它将脱离流形），以及为什么 \(\log(R_{des} R_k^T)\) 是正确的切空间误差表示。
下一章预告: 第 4 章《扩散模型入门》将由 Angjoo Kanazawa 的 Guest Lecture 引入，从 Normalizing Flows 到 Flow Matching，建立生成模型这一贯穿本课程”从演示中学习”范式的核心工具。