机器人动力学

概述

动力学（Dynamics）研究力/力矩与运动之间的关系。与运动学仅关注几何关系不同，动力学需要考虑质量、惯量、重力等物理量。动力学是机器人控制的基础。

动力学有两大核心问题：

问题	输入	输出	应用
正动力学	关节力矩 \(\tau\)	关节加速度 \(\ddot{q}\)	仿真
逆动力学	期望轨迹 \(q, \dot{q}, \ddot{q}\)	所需力矩 \(\tau\)	控制

1 运动方程标准形式

机器人的运动方程可以统一写成：

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

各项含义：

符号	名称	维度	物理意义
\(M(q)\)	质量/惯量矩阵	\(n \times n\)	正定对称，表征惯性
\(C(q, \dot{q})\dot{q}\)	科氏力与离心力	\(n \times 1\)	速度耦合效应
\(g(q)\)	重力项	\(n \times 1\)	重力在关节空间的投影
\(\tau\)	关节力矩/力	\(n \times 1\)	驱动器输出

重要性质

\(M(q)\) 是 对称正定 的：\(M = M^T\)，\(x^T M x > 0, \; \forall x \neq 0\)
\(\dot{M} - 2C\) 是 反对称 的（选取 Christoffel 符号定义的 \(C\) 时）
该性质在自适应控制和被动性分析中至关重要

2 Newton-Euler 递推法

Newton-Euler 方法是计算逆动力学最高效的方法，计算复杂度为 \(O(n)\)。

2.1 算法流程

graph TB
    subgraph "前向递推：从基座到末端"
        A["连杆 1"] -->|"ω₁, α₁, a₁"| B["连杆 2"]
        B -->|"ω₂, α₂, a₂"| C["连杆 3"]
        C -->|"..."| D["连杆 n"]
    end
    subgraph "后向递推：从末端到基座"
        D -->|"f_n, τ_n"| C
        C -->|"f₂, τ₂"| B
        B -->|"f₁, τ₁"| A
    end
    style A fill:#e8f4fd,stroke:#2196F3
    style D fill:#fff3e0,stroke:#FF9800

2.2 前向递推（速度与加速度传播）

从基座 (\(i=0\)) 开始，依次向末端 (\(i=n\)) 计算每个连杆的角速度、角加速度和线加速度。

旋转关节 \(i\)：

角速度传播：

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

角加速度传播：

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

连杆原点线加速度：

\[ \dot{v}_i = \;^{i}R_{i-1} [\dot{\omega}_{i-1} \times p_{i-1,i} + \omega_{i-1} \times (\omega_{i-1} \times p_{i-1,i}) + \dot{v}_{i-1}] \]

质心线加速度：

\[ \dot{v}_{c_i} = \dot{\omega}_i \times s_i + \omega_i \times (\omega_i \times s_i) + \dot{v}_i \]

其中 \(s_i\) 是从连杆坐标系原点到质心的向量。

2.3 后向递推（力与力矩传播）

从末端 (\(i=n\)) 开始，依次向基座 (\(i=0\)) 计算每个关节所受的力和力矩。

作用在连杆 \(i\) 上的净力（Newton 方程）：

\[ F_i = m_i \dot{v}_{c_i} \]

作用在连杆 \(i\) 上的净力矩（Euler 方程）：

\[ N_i = I_i \dot{\omega}_i + \omega_i \times (I_i \omega_i) \]

向基座方向递推合力和合力矩：

\[ f_i = \;^{i}R_{i+1} \, f_{i+1} + F_i \]

\[ n_i = N_i + \;^{i}R_{i+1} \, n_{i+1} + s_i \times F_i + p_{i,i+1} \times (^{i}R_{i+1} \, f_{i+1}) \]

最终关节力矩：

\[ \tau_i = n_i^T \hat{z}_i \]

3 Lagrangian 方法

3.1 基本原理

Lagrangian 方法基于能量观点，对建模更直观，但计算复杂度为 \(O(n^3)\) 或 \(O(n^4)\)。

定义 Lagrangian 函数：

\[ L = K - P \]

其中 \(K\) 为系统总动能，\(P\) 为系统总势能。

Euler-Lagrange 方程：

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

3.2 动能与势能

连杆 \(i\) 的动能：

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

系统总动能可以写成二次型：

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

其中 \(M(q)\) 就是质量矩阵，其元素为：

\[ M_{ij}(q) = \sum_{k=\max(i,j)}^{n} \left[ m_k \left(\frac{\partial v_{c_k}}{\partial \dot{q}_i}\right)^T \frac{\partial v_{c_k}}{\partial \dot{q}_j} + \left(\frac{\partial \omega_k}{\partial \dot{q}_i}\right)^T I_k \frac{\partial \omega_k}{\partial \dot{q}_j} \right] \]

连杆 \(i\) 的势能：

\[ P_i = m_i g^T p_{c_i} \]

系统总势能：

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

3.3 推导运动方程各项

由 Euler-Lagrange 方程展开可得：

\[ \tau_i = \sum_{j=1}^{n} M_{ij}(q) \ddot{q}_j + \sum_{j=1}^{n} \sum_{k=1}^{n} c_{ijk} \dot{q}_j \dot{q}_k + g_i(q) \]

其中 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) \]

4 示例：2-link 平面机械臂完整推导

4.1 系统描述

连杆 1：质量 \(m_1\)，长度 \(l_1\)，质心在 \(l_{c1}\)，惯量 \(I_1\)
连杆 2：质量 \(m_2\)，长度 \(l_2\)，质心在 \(l_{c2}\)，惯量 \(I_2\)
重力沿 \(-y\) 方向

4.2 位置与速度

连杆 1 质心位置：

\[ x_{c1} = l_{c1} \cos\theta_1, \quad y_{c1} = l_{c1} \sin\theta_1 \]

连杆 2 质心位置：

\[ x_{c2} = l_1 \cos\theta_1 + l_{c2} \cos(\theta_1 + \theta_2) \]

\[ y_{c2} = l_1 \sin\theta_1 + l_{c2} \sin(\theta_1 + \theta_2) \]

4.3 动能

\[ K_1 = \frac{1}{2}(m_1 l_{c1}^2 + I_1)\dot{\theta}_1^2 \]

\[ K_2 = \frac{1}{2}m_2\left[l_1^2 \dot{\theta}_1^2 + l_{c2}^2(\dot{\theta}_1 + \dot{\theta}_2)^2 + 2l_1 l_{c2} \cos\theta_2 \, \dot{\theta}_1(\dot{\theta}_1 + \dot{\theta}_2)\right] + \frac{1}{2}I_2(\dot{\theta}_1 + \dot{\theta}_2)^2 \]

4.4 势能

\[ P = m_1 g l_{c1} \sin\theta_1 + m_2 g [l_1 \sin\theta_1 + l_{c2} \sin(\theta_1 + \theta_2)] \]

4.5 质量矩阵

\[ M(q) = \begin{bmatrix} m_1 l_{c1}^2 + m_2(l_1^2 + l_{c2}^2 + 2l_1 l_{c2}\cos\theta_2) + I_1 + I_2 & m_2(l_{c2}^2 + l_1 l_{c2}\cos\theta_2) + I_2 \\ m_2(l_{c2}^2 + l_1 l_{c2}\cos\theta_2) + I_2 & m_2 l_{c2}^2 + I_2 \end{bmatrix} \]

4.6 科氏力与离心力矩阵

令 \(h = m_2 l_1 l_{c2} \sin\theta_2\)，则：

\[ C(q, \dot{q}) = \begin{bmatrix} -h\dot{\theta}_2 & -h(\dot{\theta}_1 + \dot{\theta}_2) \\ h\dot{\theta}_1 & 0 \end{bmatrix} \]

4.7 重力项

\[ g(q) = \begin{bmatrix} (m_1 l_{c1} + m_2 l_1)g\cos\theta_1 + m_2 l_{c2} g \cos(\theta_1 + \theta_2) \\ m_2 l_{c2} g \cos(\theta_1 + \theta_2) \end{bmatrix} \]

4.8 完整运动方程

\[ \begin{bmatrix} \tau_1 \\ \tau_2 \end{bmatrix} = M(q)\begin{bmatrix} \ddot{\theta}_1 \\ \ddot{\theta}_2 \end{bmatrix} + C(q, \dot{q})\begin{bmatrix} \dot{\theta}_1 \\ \dot{\theta}_2 \end{bmatrix} + g(q) \]

5 动力学性质与应用

5.1 关键性质

性质 1：\(M(q)\) 对称正定

保证了系统动能总是正的，也保证了运动方程的解存在且唯一。

性质 2：\(\dot{M} - 2C\) 反对称（Christoffel 定义下）

\[ x^T(\dot{M} - 2C)x = 0, \quad \forall x \in \mathbb{R}^n \]

该性质意味着系统的被动性（passivity），在自适应控制中起关键作用。

性质 3：线性参数化

运动方程可以写成：

\[ M(q)\ddot{q} + C(q, \dot{q})\dot{q} + g(q) = Y(q, \dot{q}, \ddot{q}) \, \pi \]

其中 \(Y\) 是回归矩阵，\(\pi\) 是动力学参数向量。这一性质是自适应控制的基础。

5.2 正动力学求解

正动力学用于仿真，给定 \(\tau\)，求 \(\ddot{q}\)：

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

结合数值积分（如 Runge-Kutta）即可仿真机器人运动。

5.3 逆动力学求解

逆动力学用于控制，给定期望轨迹 \(q_d(t)\)，计算前馈力矩：

\[ \tau_{\text{ff}} = M(q_d)\ddot{q}_d + C(q_d, \dot{q}_d)\dot{q}_d + g(q_d) \]

6 计算效率比较

方法	复杂度	适用场景
Newton-Euler 递推	\(O(n)\)	实时逆动力学计算、控制
Lagrangian	\(O(n^3) \sim O(n^4)\)	推导符号表达式、理论分析
Articulated-Body Algorithm	\(O(n)\)	正动力学仿真
Composite-Rigid-Body	\(O(n^2)\)	计算 \(M(q)\)（配合 \(O(n)\) 逆动力学）

7 扩展话题

7.1 接触动力学

当机器人与环境发生接触时，需要引入接触力约束：

\[ M(q)\ddot{q} + C(q,\dot{q})\dot{q} + g(q) = \tau + J_c^T F_c \]

其中 \(J_c\) 为接触雅可比，\(F_c\) 为接触力。接触力需满足摩擦锥约束等。

7.2 柔性关节动力学

当关节存在弹性（如谐波减速器）时，需要扩展模型：

\[ M(q)\ddot{q} + C\dot{q} + g = K(\theta_m - q) \]

\[ B\ddot{\theta}_m + K(\theta_m - q) = \tau_m \]

其中 \(K\) 为关节刚度矩阵，\(B\) 为电机惯量，\(\theta_m\) 为电机侧角度。

7.3 常用仿真工具

MuJoCo：高效的接触动力学仿真器，广泛用于机器人学习
PyBullet：开源物理引擎，Python 接口
Drake：MIT 开发，支持多体动力学与优化控制
Pinocchio：高效的刚体动力学库，支持解析导数

参考文献

Featherstone, R. (2008). Rigid Body Dynamics Algorithms. Springer.
Siciliano, B. et al. (2009). Robotics: Modelling, Planning and Control. Springer.
Craig, J. J. (2005). Introduction to Robotics: Mechanics and Control. Pearson.
Spong, M. W. et al. (2006). Robot Modeling and Control. Wiley.