# 第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 核心问题 本章围绕三个核心问题展开： 1. **刚体运动如何用数学表示？** —— 从旋转矩阵到指数坐标，建立 SO(3)/SE(3) 李群-李代数对应关系。 2. **串联机器人的前向运动学如何高效计算？** —— Product-of-Exponentials (PoE) 公式与 DH 参数的对比。 3. **机器人的运动方程如何建立？** —— 拉格朗日动力学与牛顿-欧拉逆动力学的统一视角。 ### 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) （**S**pecial **O**rthogonal 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) 的维数一致——这正是李群-李代数对应的核心特征。 **两个关键恒等式**： 1. **叉积恒等式**: $$[\omega] v = \omega \times v,\quad \forall v \in \mathbb{R}^3$$ 2. **李括号**: $$\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}$$。 伴随矩阵的核心性质： 1. **群同态**：$$\text{Ad}_{T_1 T_2} = \text{Ad}_{T_1} \text{Ad}_{T_2}$$ 2. **行列式**：$$\det(\text{Ad}_T) = 1$$ 3. **逆矩阵**：$$\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$$）对应于： 1. **工作空间边界奇异**: 手臂完全伸展，至少失去一个平动自由度 2. **内部奇异**: 腕关节轴线重合（如肩-肘-腕共线），失去至少一个转动自由度 在这些位形附近，很小的末端速度要求极大的关节速度——这是阻抗控制和运动规划中必须规避的状态。在实际工程中，**可操作度** (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)$$ **步骤**: 1. 若 $$\|\omega\| < \epsilon$$（纯平移）: - $$T = \begin{bmatrix} I & v\theta \\ 0 & 1 \end{bmatrix}$$，返回 2. 归一化: $$\|\omega\| = \sqrt{\omega^T\omega}$$, $$\hat{\omega} = \omega / \|\omega\|$$, $$\theta' = \|\omega\|\theta$$ 3. 计算 $$R = I + \sin\theta'[\hat{\omega}] + (1-\cos\theta')[\hat{\omega}]^2$$ 4. 计算 $$G = I\theta' + (1-\cos\theta')[\hat{\omega}] + (\theta'-\sin\theta')[\hat{\omega}]^2$$ 5. $$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})$$ **步骤**: 1. $$T \leftarrow I_4$$ 2. for $$i = 1$$ to $$n$$: - $$T \leftarrow T \cdot e^{[\mathcal{S}_i]\theta_i}$$ （调用算法 3.1） 3. $$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}$$ **步骤**: 1. $$T \leftarrow I_4$$, $$J_s \leftarrow 0_{6\times n}$$ 2. 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}$$ 3. $$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 反对称矩阵与李代数基础 ```python 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) 的指数/对数映射 ```python 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 前向运动学与雅可比 ```python 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) ```python 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 完整示例：二连杆平面机器人验证 ```python 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 章的连接：从生物关节到机器人关节的运动学类比 ```python 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 五个核心洞察 1. **指数坐标 = 旋量理论的代数化**。SO(3)/SE(3) 的指数映射不是抽象数学，而是 Rodrigues 公式的自然推广。$$e^{[\mathcal{S}]\theta}$$ 统一了转动和平移——每当你写下一个旋量轴 $$\mathcal{S} = (\omega, v)$$，你同时在定义关节的运动方向、运动类型（$$\omega \neq 0$$: 转动; $$\omega = 0$$: 移动）、以及它在空间中的位置（$$v = -\omega \times q$$ 编码了轴线位置）。 2. **PoE 优于 DH 的本质原因**：PoE 将坐标系定义从"每个连杆一个"削减到"全局仅需两个"（固定系和末端系）。这不仅简化了建模，更重要的是避免了 DH 的符号歧义和参数奇异性。在工程中，PoE 的旋量轴可以直接从关节编码器数据和 CAD 模型中辨识——这是现代机器人运动学标定的理论基础。 3. **RNEA 是仿真与控制的基础设施**。O($$n$$) 的递推牛顿-欧拉算法使得复杂机器人的实时动力学仿真成为可能。从 MuJoCo 到 Isaac Gym，所有现代物理引擎的核心都是 RNEA + CRBA 的组合。理解 RNEA 的前向-后向递推结构，是理解基于模型的强化学习中"如何将策略输出的力矩转化为运动"的关键。 4. **运动学与动力学通过伴随变换互为表里**。速度运动学的雅可比 $$J(\boldsymbol{\theta})$$ 不仅用于前向运动学的微分，更是静力传递和动力学方程的核心组件：力从任务空间经 $$J^T$$ 映射回关节空间，速度从关节空间经 $$J$$ 映射到任务空间——这是一个优美的对偶结构，是虚功原理在串联机器人上的直接体现。 5. **从生物到机器的鸿沟从力学层面就已开始**。生物关节的变刚度、冗余驱动和非线性弹性是机器人领域尚未完全复现的特性。本章的刚体运动学描述了"理想化的机器"，而真实世界的接触、柔顺和摩擦（将在 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，建立生成模型这一贯穿本课程"从演示中学习"范式的核心工具。 ---
← 第02章 生物运动力学：行走与奔跑
↑ 目录
第04章 扩散模型入门 →