动力学¶

运动学只关心"位置和速度"，动力学则关心"力和力矩"——要让机器人按期望方式运动，每个关节需要施加多大的驱动力？

一、动力学的两大问题¶

问题	输入	输出	用途
正动力学	关节力矩 \(\tau\)	关节加速度 \(\ddot{q}\)	仿真
逆动力学	\(q, \dot{q}, \ddot{q}\)（轨迹）	关节力矩 \(\tau\)	控制（计算力矩法）

核心方程——机器人动力学方程：

\[ \tau = M(q)\ddot{q} + C(q, \dot{q})\dot{q} + G(q) \]

项	含义	物理解释
\(M(q)\ddot{q}\)	惯性项	加速运动所需力矩（类比 \(F = ma\)）
\(C(q, \dot{q})\dot{q}\)	科氏力与离心力项	连杆运动间的耦合效应
\(G(q)\)	重力项	克服重力所需力矩

直觉理解

想象你挥动一根长棍——加速时需要更大力（惯性项），旋转时手臂感到被甩开（离心力），静止举着也需要力矩抵抗重力（重力项）。

二、预备知识¶

2.1 连杆间静力的传递¶

从末端到基座逐级传递力和力矩。对于连杆 \(i\)：

\[ {}^i f_i = {}^i_{i+1}R \; {}^{i+1}f_{i+1} + {}^i F_i \]

\[ {}^i n_i = {}^i_{i+1}R \; {}^{i+1}n_{i+1} + {}^i p_{i+1} \times ({}^i_{i+1}R \; {}^{i+1}f_{i+1}) + {}^i N_i \]

其中 \(f_i\) 是连杆 \(i-1\) 对连杆 \(i\) 施加的力，\(n_i\) 是力矩，\(F_i\) 和 \(N_i\) 分别是作用在连杆 \(i\) 上的外力和外力矩。

2.2 力雅可比矩阵¶

静力平衡时，末端施加的力/力矩与关节力矩的关系：

\[ \tau = J^T(q) \mathcal{F} \]

其中 \(\mathcal{F} = [f_x, f_y, f_z, n_x, n_y, n_z]^T\) 是末端的广义力。

对偶关系

速度映射用 \(J\)，力映射用 \(J^T\)——这就是虚功原理的体现：

\[\tau^T \dot{q} = \mathcal{F}^T \mathbf{v} \implies \tau^T \dot{q} = \mathcal{F}^T J \dot{q} \implies \tau = J^T \mathcal{F}\]

2.3 转动惯量与惯性张量¶

刚体的旋转惯性不像平动那么简单——它取决于旋转轴的方向。惯性张量完整描述刚体绕任意轴的旋转惯性：

\[ I = \begin{bmatrix} I_{xx} & -I_{xy} & -I_{xz} \\ -I_{xy} & I_{yy} & -I_{yz} \\ -I_{xz} & -I_{yz} & I_{zz} \end{bmatrix} \]

记号	含义
\(I_{xx}\)	绕 \(x\) 轴的转动惯量：\(I_{xx} = \int (y^2 + z^2) \, dm\)
\(I_{xy}\)	惯性积：\(I_{xy} = \int xy \, dm\)

惯性张量的性质

对称正定矩阵
在主轴坐标系中，惯性积为零，矩阵为对角阵
坐标变换：\({}^A I = R \; {}^B I \; R^T\)（相似变换）
平行轴定理：\(I_P = I_C + m(d^T d \cdot E_3 - d \cdot d^T)\)

2.4 连杆加速度的传递¶

角加速度从基座向末端传递（外推）：

对于转动关节 \(i\)：

\[ {}^{i}\omega_i = {}^{i}_{i-1}R \; {}^{i-1}\omega_{i-1} + \dot{\theta}_i \hat{z}_i \]

\[ {}^{i}\dot{\omega}_i = {}^{i}_{i-1}R \; {}^{i-1}\dot{\omega}_{i-1} + {}^{i}_{i-1}R \; {}^{i-1}\omega_{i-1} \times \dot{\theta}_i \hat{z}_i + \ddot{\theta}_i \hat{z}_i \]

线加速度：

\[ {}^{i}\dot{v}_i = {}^{i}_{i-1}R({}^{i-1}\dot{v}_{i-1} + {}^{i-1}\dot{\omega}_{i-1} \times {}^{i-1}p_i + {}^{i-1}\omega_{i-1} \times ({}^{i-1}\omega_{i-1} \times {}^{i-1}p_i)) \]

三、牛顿-欧拉法（Newton-Euler）¶

3.1 基本思想¶

逐个连杆应用牛顿第二定律和欧拉方程，采用递推方式：

外推（从基座到末端）：逐级计算每个连杆的速度和加速度
内推（从末端到基座）：逐级计算每个连杆上的力和力矩

graph LR
    A[基座] -->|外推: ω, α, a| B[连杆 1]
    B --> C[连杆 2]
    C --> D[... 连杆 n]
    D -->|内推: f, τ| C
    C --> B
    B --> A

3.2 算法流程¶

第一步：外推（\(i = 1 \to n\)）

计算连杆 \(i\) 的角速度 \(\omega_i\)
计算连杆 \(i\) 的角加速度 \(\dot{\omega}_i\)
计算连杆 \(i\) 原点的线加速度 \(\dot{v}_i\)
计算连杆 \(i\) 质心的线加速度 \(\dot{v}_{c_i}\)
计算作用在质心上的惯性力 \(F_i = m_i \dot{v}_{c_i}\) 和惯性力矩 \(N_i = I_i \dot{\omega}_i + \omega_i \times I_i \omega_i\)

第二步：内推（\(i = n \to 1\)）

计算连杆 \(i-1\) 对连杆 \(i\) 的力 \(f_i\) 和力矩 \(n_i\)
提取关节力矩：
- 转动关节：\(\tau_i = n_i^T \hat{z}_i\)
- 移动关节：\(\tau_i = f_i^T \hat{z}_i\)

牛顿-欧拉法的优势

计算效率高：\(O(n)\) 复杂度（\(n\) 为关节数）
非常适合实时控制中的逆动力学计算

四、拉格朗日法（Lagrangian）¶

4.1 基本思想¶

从能量角度出发，不需要分析内部力的传递。使用拉格朗日函数：

\[ \mathcal{L}(q, \dot{q}) = K(q, \dot{q}) - P(q) \]

其中 \(K\) 是总动能，\(P\) 是总势能。

拉格朗日方程：

\[ \tau_i = \frac{d}{dt}\frac{\partial \mathcal{L}}{\partial \dot{q}_i} - \frac{\partial \mathcal{L}}{\partial q_i}, \quad i = 1, 2, \ldots, n \]

4.2 动能计算¶

连杆 \(i\) 的动能（平动 + 转动）：

\[ K_i = \frac{1}{2} m_i v_{c_i}^T v_{c_i} + \frac{1}{2} \omega_i^T {}^0 I_i \; \omega_i \]

总动能：

\[ K = \sum_{i=1}^{n} K_i = \frac{1}{2} \dot{q}^T M(q) \dot{q} \]

其中 \(M(q)\) 就是质量矩阵（惯性矩阵）——对称正定。

4.3 势能计算¶

\[ P = \sum_{i=1}^{n} m_i g^T p_{c_i} \]

其中 \(p_{c_i}\) 是连杆 \(i\) 质心的位置，\(g\) 是重力加速度向量。

4.4 推导动力学方程¶

将 \(K\) 和 \(P\) 代入拉格朗日方程，经过推导可以得到标准形式：

\[ \tau = M(q)\ddot{q} + C(q, \dot{q})\dot{q} + G(q) \]

各项的具体表达式：

质量矩阵 \(M(q)\)：

\[ M_{ij}(q) = \sum_{k=\max(i,j)}^{n} \left[ m_k J_{v_{c_k,i}}^T J_{v_{c_k,j}} + J_{\omega_{k,i}}^T {}^0 I_k J_{\omega_{k,j}} \right] \]

科氏力/离心力项使用 Christoffel 符号：

\[ c_{ijk} = \frac{1}{2}\left(\frac{\partial M_{ij}}{\partial q_k} + \frac{\partial M_{ik}}{\partial q_j} - \frac{\partial M_{jk}}{\partial q_i}\right) \]

\[ C_{ij} = \sum_{k=1}^{n} c_{ijk} \dot{q}_k \]

重力项：

\[ G_i(q) = \frac{\partial P}{\partial q_i} = -\sum_{k=i}^{n} m_k g^T J_{v_{c_k,i}} \]

4.5 实例：平面 2R 机械臂¶

假设两个连杆均匀分布，质心在各连杆中点：

质量矩阵：

\[ M(q) = \begin{bmatrix} M_{11} & M_{12} \\ M_{12} & M_{22} \end{bmatrix} \]

其中：

\[ M_{11} = (m_1 + m_2)L_1^2 + m_2 L_2^2 + 2m_2 L_1 L_2 \cos\theta_2 + I_1 + I_2 \]

\[ M_{12} = m_2 L_2^2 + m_2 L_1 L_2 \cos\theta_2 + I_2 \]

\[ M_{22} = m_2 L_2^2 + I_2 \]

注意 \(M_{12}\) 项

质量矩阵通常是非对角的——这意味着关节之间存在耦合：加速其中一个关节，另一个也会受到力的影响。

五、两种方法对比¶

特性	牛顿-欧拉法	拉格朗日法
出发点	力与力矩的递推	能量方程
推导难度	公式多，需要逐级推导	数学推导较繁琐
计算效率	\(O(n)\)，适合实时控制	\(O(n^4)\) 或更高，适合离线分析
物理直觉	每个力有明确物理意义	只关注能量，不需要分析内力
最终结果	相同的动力学方程	相同的动力学方程
主要用途	实时逆动力学计算	理论分析、控制器设计

实际中的选择

实时控制：用牛顿-欧拉递推算法（CTC 控制器的核心）
理论研究/建模：用拉格朗日法推导封闭形式的动力学方程
仿真软件：通常两种方法都有实现

六、动力学方程的性质¶

理解这些性质对控制器设计非常重要：

\(M(q)\) 是对称正定矩阵：意味着加速总是需要正的能量输入
\(\dot{M} - 2C\) 是反对称矩阵：\(\dot{q}^T(\dot{M} - 2C)\dot{q} = 0\)，这个性质在自适应控制中大量使用
方程关于惯性参数是线性的：

\[ \tau = Y(q, \dot{q}, \ddot{q}) \; \pi \]

其中 \(Y\) 是回归矩阵，\(\pi\) 是连杆惯性参数向量。这个线性性使得参数辨识和自适应控制成为可能。