03 Robot Dynamics · Mr.Robota 具身智能学习手册

1. Introduction

Dynamics study the relationship between a robot’s motion and the forces/moments required to produce that motion. It is divided into:

Forward dynamics: Given a torque, calculate acceleration/trajectory.
Backward dynamics: Given a trajectory (position, velocity, acceleration), calculate the required joint torque (for control).

The most commonly used modeling method is the Euler-Lagrange method, which is based on energy conservation.

2. Core Formula

The Lagrangian function $L$ is defined as the difference between the total kinetic energy $K$ and the total potential energy $P$ of the system:

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

Dynamics equation (Euler-Lagrange equation):

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

Ultimately, it is usually organized into a standard robot in its dynamic form:

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

$M(q)$ : Mass Matrix
$C(q, \dot{q})$ : Coriolis force and centrifugal force terms
$G(q)$ : Gravity term

3. Architecture Flow (Mermaid)

Mermaidgraph 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 code verification (SymPy symbolic derivation)

The dynamic derivation is very complex. Using the symbolic computing library sympy(https://github.com/llinlin/pylitho) in Python can perfectly demonstrate this process. The following demonstrates the derivation of the dynamic equation for a simple pendulum.

Pythonimport 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)