03机器人动力学.md

1. 简介

动力学研究机器人运动与产生该运动所需的力/力矩之间的关系。它分为：

• 正动力学: 给定力矩，计算加速度/运动轨迹。
• 逆动力学: 给定运动轨迹（位置、速度、加速度），计算所需关节力矩（用于控制）。

最常用的建模方法是 拉格朗日法 (Euler-Lagrange)，它基于能量守恒。

2. 核心公式

拉格朗日函数 $L$ 定义为系统总动能 $K$ 与总势能 $P$ 之差：

$$L(q, \dot{q}) = K(q, \dot{q}) - P(q)$$

动力学方程（欧拉-拉格朗日方程）：

$$\frac{d}{dt} \left( \frac{\partial L}{\partial \dot{q}_i} \right) - \frac{\partial L}{\partial q_i} = \tau_i$$

最终通常整理为标准的机器人在动力学形式：

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

• $M(q)$: 惯性矩阵 (Mass Matrix)
• $C(q, \dot{q})$: 科里奥利力和离心力项
• $G(q)$: 重力项

3. 架构流程 (Mermaid)

graph TD
    Start[定义系统参数: 质量m, 长度l, 惯量I] --> Energy[计算能量]
    Energy --> K[动能 Kinetic Energy]
    Energy --> P[势能 Potential Energy]
    K & P --> L[构造拉格朗日函数 L = K - P]
    L --> EL_Eq[应用欧拉-拉格朗日方程]
    EL_Eq --> Derive[符号推导/微分]
    Derive --> Torque[获得力矩方程 Tau]
    Torque --> Matrix[整理为 M, C, G 矩阵形式]

4. Python 代码验证 (SymPy 符号推导)

动力学推导非常繁琐，使用 Python 的符号计算库 sympy 可以完美演示这一过程。以下演示推导一个简单单摆的动力学方程。

import sympy as sp

# 1. 定义符号
t = sp.symbols('t')
m, l, g = sp.symbols('m l g', real=True, positive=True)
theta = sp.Function('theta')(t)  # 关节角度随时间变化

# 定义速度 (theta_dot) 和 加速度 (theta_ddot)
theta_dot = theta.diff(t)
theta_ddot = theta_dot.diff(t)

# 2. 计算能量
# 动能 K = 1/2 * m * v^2。对于单摆，v = l * angular_velocity
v = l * theta_dot
K = sp.Rational(1, 2) * m * v**2

# 势能 P = m * g * h。假设最低点势能为0，则 h = l * (1 - cos(theta))
# 或者简单定义坐标原点在转轴，y轴向上，则 h = -l * cos(theta)
h = -l * sp.cos(theta)
P = m * g * h

# 3. 拉格朗日函数 L
L = K - P

# 4. 欧拉-拉格朗日方程
# d/dt (dL/d_theta_dot)
dL_dthetadot = sp.diff(L, theta_dot)
term1 = sp.diff(dL_dthetadot, t)

# dL/d_theta
term2 = sp.diff(L, theta)

# 力矩 tau = term1 - term2
tau = term1 - term2

# 5. 简化并打印结果
equation = sp.simplify(tau)
print("--- 自动推导的动力学方程 (Tau = ...) ---")
sp.pprint(equation)

print("\n--- 提取各项 ---")
# 对于单摆: M*q_ddot + G = 0 (无阻尼情况)
# 我们期望看到 M = m*l^2, G = m*g*l*sin(theta)
print(f"惯性项 (对应 theta_ddot): {equation.coeff(theta_ddot)}")
# 注意：sympy提取非线性项比较复杂，这里人工观察打印结果即可验证
# 理论结果应为: m * l**2 * theta_ddot + m * g * l * sin(theta)