动力学¶

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

一、动力学的两大问题¶

问题	输入	输出	用途
正动力学	关节力矩 \(\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)\)	重力项	克服重力所需力矩

直觉理解

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

1.1 工程实操：动力学分析先准备什么¶

以 7 自由度仿人机械臂为例，动力学分析不是从公式开始，而是先把模型参数整理干净：

数据	来源	用途
连杆质量 \(m_i\)	CAD / 称重	计算惯性项和重力项
质心位置 \(p_{c_i}\)	CAD 质量属性	决定重力力矩和惯性耦合
惯性张量 \(I_i\)	CAD / 辨识	决定高速运动时的力矩需求
关节轴与坐标系	URDF / DH / 标定	保证运动学和动力学坐标一致
电机减速比、效率	电机与减速器手册	把关节力矩换算成电机侧力矩
摩擦参数	实验辨识	低速跟踪和静止保持时很关键
负载参数	末端工具、夹爪、物体	抓取后必须更新动力学模型

CAD 参数不是最终答案

CAD 给出的质量、质心和惯量通常是第一版模型。装配线束、外壳、螺丝、夹爪和实际负载都会改变参数。做高精度控制时，需要通过实验做参数辨识或至少做重力补偿标定。

二、7 自由度机械臂的动力学项目流程¶

2.1 逆动力学：从轨迹算电机需要多大力矩¶

给定一条关节轨迹 \(q(t),\dot{q}(t),\ddot{q}(t)\)，逆动力学直接计算：

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

实际项目中常用它做三件事：

电机选型：看 \(\tau_{\max}\) 和持续力矩是否超过电机/减速器能力
轨迹验算：如果某段力矩过大，就降低速度、降低加速度或换路径
前馈控制：把 \(\tau_{\text{ff}}\) 加到关节控制器中，减小 PID 负担

典型流程：

graph LR
    A[URDF/CAD 参数] --> B[建立刚体动力学模型]
    C[规划轨迹 q qdot qddot] --> D[逆动力学 RNEA]
    B --> D
    D --> E[关节力矩 tau]
    E --> F[检查电机/减速器/温升]
    E --> G[作为控制前馈]

其中 RNEA（Recursive Newton-Euler Algorithm）就是牛顿-欧拉递推的工程实现，复杂度 \(O(n)\)，适合 7 自由度机械臂实时计算。

2.2 正动力学：从力矩预测机器人怎么动¶

正动力学解的是：

\[ \ddot{q} = M^{-1}(q)\left[\tau - C(q,\dot{q})\dot{q} - G(q)\right] \]

它主要用于仿真和控制器验证。例如给定控制器输出的 \(\tau\)，积分得到下一时刻：

\[ \dot{q}_{k+1} = \dot{q}_k + \ddot{q}_k \Delta t \]

\[ q_{k+1} = q_k + \dot{q}_{k+1}\Delta t \]

如果仿真里都跟不住轨迹，实机通常更难跟住；如果仿真力矩长期贴近上限，实机还会受到摩擦、温升、电源电流限制的影响。

2.3 动力学模型的最小可用版本¶

项目早期可以按复杂度逐步加模型：

阶段	模型	适用
1	只做关节 PID，不加动力学	低速调通电机和编码器
2	加重力补偿 \(G(q)\)	机械臂静止、慢速运动明显变稳
3	加逆动力学前馈 \(M\ddot{q}+C\dot{q}+G\)	中高速轨迹跟踪
4	加摩擦、负载和参数辨识	高精度、接触操作、抓取负载变化

对仿人 7 自由度手臂来说，最值得尽早做的是重力补偿：让机械臂在不同姿态下不“掉臂”，也能显著降低位置环的积分压力。

2.4 重力补偿的调试方法¶

固定每个关节的低速位置环，先保证编码器方向和力矩方向正确
在多个静态姿态记录维持姿态所需力矩
用模型计算 \(G(q)\)，比较实测力矩和模型力矩
如果整体比例不对，检查减速比、力矩常数、单位
如果某些姿态误差大，检查质心、连杆坐标系和负载参数

方向检查

如果加了重力补偿后机械臂反而更想往下掉，优先检查重力向量方向、关节正方向、电机力矩正方向。这类错误比公式推导错误更常见。

三、预备知识¶

3.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\) 上的外力和外力矩。

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

3.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)\)

3.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）¶

4.1 基本思想¶

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

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

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

4.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）¶

5.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 \]

5.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)\) 就是质量矩阵（惯性矩阵）——对称正定。

5.3 势能计算¶

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

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

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

5.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\) 是连杆惯性参数向量。这个线性性使得参数辨识和自适应控制成为可能。