《Python机器人基础原理》第二篇：装配产线仿真——分解动力学方程，提取惯性项/科氏力项/重力项数值曲线
1. 实际工业应用场景
周四下午2点，某3C产品装配产线。
一台2-DOF平面装配机械臂正在做节拍优化调试。连杆1长0.6 m、质量8 kg，连杆2长0.4 m、质量5 kg，末端夹持一个1.5 kg的手机中框。节拍要求2 s/循环，定位精度±0.5 mm。
工艺工程师小李盯着节拍曲线发愁。
"这条线理论节拍2秒，实际跑出来要2.4秒。瓶颈在关节1——加速段明显拖尾。"
旁边做仿真的小王调出力矩曲线："李工，你看这个。总力矩曲线里，重力项占了一大半，但真正影响加速拖尾的是惯性项和科氏力项。你把加速段拉长，惯性项会跟着降下来，但重力项不降——这是两笔不同的账。"
小李追问："那我怎么知道该优化哪一段？是减速？还是改轨迹？"
小王指着屏幕："把总力矩拆开看。重力项是'房贷'——只要关节在这个位置，它就固定存在，跟你怎么加速无关。惯性项是'临时开支'——你想加速多快，它就多大。科氏力项是'意外支出'——只有两个关节同时动的时候才出现。你把这三条曲线叠在一起看，瓶颈在哪个项上一目了然。"
这就是本篇要解决的问题。
2. 痛点分析
课程模块 本篇痛点 解决层级
第4章 机器人动力学 → 4.3 动力学方程各项物理意义 总力矩曲线是一条线，工程师看不出瓶颈在惯性还是重力 离线仿真校核
第4章 机器人动力学 → 4.2 拉格朗日方程建模方法 需要把 τ = M·q̈ + C + G 分解为三个独立项分别计算 离线仿真校核
第5章 轨迹规划 → 5.2 轨迹平滑性与加速度约束 加速度曲线决定惯性项大小，进而影响峰值力矩 离线仿真校核
本篇解决 L1 离线仿真校核层。目标是在控制代码上线之前，把动力学方程 τ = M(q)·q̈ + C(q,q̇) + G(q) 分解为三个独立分量，分别输出数值曲线，让工程师看清"力矩花在哪了"，从而针对性优化轨迹或调整机械结构。
不覆盖：逆动力学数值解法、牛顿-欧拉递推法、柔性动力学、关节柔性、控制算法、6-DOF空间机械臂、力控制、智能控制（本篇限定2-DOF平面构型讲清原理）。
3. 核心逻辑讲解（大白话）
物理直觉
动力学方程 τ = M·q̈ + C + G 就像一个三口之家的账单：
- 重力项 G(q) 是"房贷"——只要关节在这个位置，它就固定存在，跟你怎么动无关。臂抬得越高，这笔账越大。
- 惯性项 M(q)·q̈ 是"临时开支"——你想加速多快，它就多大。跟负载质量和臂长平方成正比。加速越猛，这笔账越高。
- 科氏力项 C(q,q̇) 是"意外支出"——只有两个关节同时动的时候才出现，单个关节匀速转的时候它为零。
大白话翻译："把总力矩拆成三笔账：固定的房贷（重力）、临时的加速费（惯性）、意外的交叉费（科氏力），看哪笔是瓶颈。"
业务逻辑 → 代码映射
输入：机械臂构型 + 轨迹(q, dq, ddq)
↓
步骤1：sympy推导 T, V → 解析提取 M(q), C(q,dq), G(q)
↓
步骤2：沿轨迹逐点计算 M, C, G
↓
步骤3：惯性项 = M(q) · ddq （矩阵乘向量）
↓
步骤4：科氏力项 = C(q,dq) （直接读取）
↓
步骤5：重力项 = G(q) （直接读取）
↓
步骤6：校验 τ = 惯性项 + 科氏力项 + 重力项（数值一致性检查）
↓
输出：四条独立曲线 + 堆叠面积图 + 各项峰值/均值/占比统计
为什么选拉格朗日不选牛顿-欧拉
方法 特点 适合场景
拉格朗日方程 系统级建模，代码简洁，推导一次处处可用 离线仿真、教学、参数分析 ← 本篇选这个
牛顿-欧拉递推 从基座递推到末端再反向递推，计算量小，速度快 实时控制、嵌入式部署 → 后续篇目
力矩构成拆解
- 重力项 G(q)：跟位置有关，跟速度、加速度无关。
- 惯性项 M(q)·q̈：跟加速度有关，加速越猛越大。
- 科氏力/离心力项 C(q,q̇)：跟速度交叉有关，单关节匀速时为零。
关键公式文字翻译
惯性项：M₁₁·q̈₁ + M₁₂·q̈₂
翻译：关节1的惯性力矩 = 惯性矩阵第一行点乘加速度向量。M₁₂不为零意味着"关节2加速也会给关节1带来惯性负载"——这就是耦合。
科氏力项：Σⱼₖ Γᵢⱼₖ · q̇ⱼ · q̇ₖ
翻译：关节i的科氏力 = 三个Christoffel符号分别乘以对应速度对的乘积再求和。只有当 j≠k 时才是真正的科氏力，j=k 时是离心力。
重力项：∂V/∂qᵢ
翻译：关节i的重力矩 = 势能对该关节坐标的偏导数。物理意义是"把这个关节转动一小下，重力势能变化多少"。
4. OOP代码实现
技术栈
严格限定：numpy, scipy, matplotlib, sympy（符号推导+lambdify，仅用于建模阶段）
"requirements.txt"
numpy>=1.24.0
scipy>=1.10.0
matplotlib>=3.7.0
sympy>=1.12.0
项目结构树
robot_dynamics_decomposition/
├── run_dynamics.py
├── requirements.txt
├── dynamics/
│ ├── __init__.py
│ ├── models.py
│ ├── lagrangian.py
│ ├── simulator.py
│ └── visualize.py
├── tests/
│ ├── __init__.py
│ └── test_dynamics.py
└── results/
核心源码
"dynamics/models.py" — dataclass数据模型
"""连杆参数与机器人构型定义"""
from dataclasses import dataclass
from typing import List
@dataclass
class Link:
"""单连杆刚体参数（2-DOF平面臂）"""
name: str
mass: float # kg
length: float # m
com_offset: float # m, 质心距近端关节距离
inertia: float # kg·m², 绕质心z轴转动惯量
def __post_init__(self):
if self.mass <= 0:
raise ValueError(
f"{self.name}: mass must be > 0, got {self.mass}")
if self.length <= 0:
raise ValueError(
f"{self.name}: length must be > 0, got {self.length}")
if not (0 <= self.com_offset <= self.length):
raise ValueError(
f"{self.name}: com_offset must be in [0, {self.length}]")
@dataclass
class RobotParams:
"""2-DOF平面装配机械臂完整参数"""
links: List[Link]
payload_mass: float = 1.5 # kg（装配场景负载轻）
gravity: float = 9.81 # m/s²
dt: float = 0.01 # s
t_end: float = 2.0 # s（2秒节拍）
def __post_init__(self):
if len(self.links) != 2:
raise ValueError(
f"本篇限定2-DOF平面构型, links数量必须为2, got {len(self.links)}")
if self.payload_mass < 0:
raise ValueError("payload_mass must be >= 0")
"dynamics/lagrangian.py" — 拉格朗日建模算法类
"""
拉格朗日动力学建模 + 方程分解
核心：τ = M(q)·q̈ + C(q,q̇) + G(q)，分别提取三项
"""
import numpy as np
import sympy as sp
from dynamics.models import RobotParams
class LagrangianDynamics:
"""2-DOF平面机械臂动力学建模与分解"""
def __init__(self, params: RobotParams):
self.params = params
self._build_symbolic()
self._lambdify()
def _build_symbolic(self):
"""符号推导：T, V → M, C, G"""
q1, q2 = sp.symbols('q1 q2', real=True)
dq1, dq2 = sp.symbols('dq1 dq2', real=True)
ddq1, ddq2 = sp.symbols('ddq1 ddq2', real=True)
g = self.params.gravity
l1 = self.params.links[0].length
l2 = self.params.links[1].length
m1 = self.params.links[0].mass
m2 = self.params.links[1].mass
lc1 = self.params.links[0].com_offset
lc2 = self.params.links[1].com_offset
I1 = self.params.links[0].inertia
I2 = self.params.links[1].inertia
mp = self.params.payload_mass
# 动能 T
T1 = 0.5 * m1 * (lc1 * dq1)**2 + 0.5 * I1 * dq1**2
T2 = (0.5 * m2 * (l1**2 * dq1**2
+ lc2**2 * (dq1 + dq2)**2
+ 2 * l1 * lc2 * dq1 * (dq1 + dq2) * sp.cos(q2))
+ 0.5 * I2 * (dq1 + dq2)**2)
Tp = 0.5 * mp * (l1**2 * dq1**2
+ l2**2 * (dq1 + dq2)**2
+ 2 * l1 * l2 * dq1 * (dq1 + dq2) * sp.cos(q2))
T = T1 + T2 + Tp
# 势能 V
V = (m1 * g * lc1 * sp.sin(q1)
+ m2 * g * (l1 * sp.sin(q1) + lc2 * sp.sin(q1 + q2))
+ mp * g * (l1 * sp.sin(q1) + l2 * sp.sin(q1 + q2)))
# 惯性矩阵 M(q) = ∂²T/∂dqᵢ∂dqⱼ
M11 = sp.diff(sp.diff(T, dq1), dq1)
M12 = sp.diff(sp.diff(T, dq1), dq2)
M21 = sp.diff(sp.diff(T, dq2), dq1)
M22 = sp.diff(sp.diff(T, dq2), dq2)
M = sp.Matrix([[M11, M12], [M21, M22]])
# 重力项 G(q) = ∂V/∂q
G1 = sp.diff(V, q1)
G2 = sp.diff(V, q2)
G = sp.Matrix([G1, G2])
# 科氏力项 C(q,dq) via Christoffel 符号
q = [q1, q2]
dq = [dq1, dq2]
C_exprs = []
for i in range(2):
C_i = 0
for j in range(2):
for k in range(2):
dMij_dqk = sp.diff(M[i, j], q[k])
dMik_dqj = sp.diff(M[i, k], q[j])
dMjk_dqi = sp.diff(M[j, k], q[i])
gamma = 0.5 * (dMij_dqk + dMik_dqj - dMjk_dqi)
C_i += gamma * dq[j] * dq[k]
C_exprs.append(C_i)
C = sp.Matrix(C_exprs)
# 总力矩 τ = M*ddq + C + G
ddq = sp.Matrix([ddq1, ddq2])
tau = M * ddq + C + G
self._q1, self._q2 = q1, q2
self._dq1, self._dq2 = dq1, dq2
self._ddq1, self._ddq2 = ddq1, ddq2
self._M, self._C, self._G, self._tau = M, C, G, tau
self._T_expr, self._V_expr = T, V
def _lambdify(self):
"""编译为numpy数值函数"""
vars_vec = (self._q1, self._q2, self._dq1, self._dq2,
self._ddq1, self._ddq2)
self._M_func = sp.lambdify(vars_vec, self._M, 'numpy')
self._C_func = sp.lambdify(vars_vec, self._C, 'numpy')
self._G_func = sp.lambdify((self._q1, self._q2), self._G, 'numpy')
self._tau_func = sp.lambdify(vars_vec, self._tau, 'numpy')
self._T_func = sp.lambdify(
(self._q1, self._q2, self._dq1, self._dq2), self._T_expr, 'numpy')
self._V_func = sp.lambdify(
(self._q1, self._q2), self._V_expr, 'numpy')
def get_M(self, q):
"""
惯性矩阵 M(q), 形状 (2,2), 单位 kg·m²
修复：签名只接受 q，M 只依赖位置不依赖速度
"""
return np.array(
self._M_func(q[0], q[1], 0.0, 0.0, 0.0, 0.0), dtype=float)
def get_C(self, q, dq):
"""科氏力/离心力项 C(q,dq), 形状 (2,), 单位 N·m"""
return np.array(
self._C_func(q[0], q[1], dq[0], dq[1], 0.0, 0.0),
dtype=float).reshape(2)
def get_G(self, q):
"""重力项 G(q), 形状 (2,), 单位 N·m"""
return np.array(self._G_func(q[0], q[1]), dtype=float).reshape(2)
def get_inertia_term(self, q, dq, ddq):
"""惯性项 M(q)·ddq, 形状 (2,), 单位 N·m"""
M = self.get_M(q)
return M @ np.asarray(ddq, dtype=float).reshape(2)
def inverse_dynamics(self, q, dq, ddq):
"""总力矩 τ = M·ddq + C + G, 形状 (2,), 单位 N·m"""
tau = self._tau_func(q[0], q[1], dq[0], dq[1], ddq[0], ddq[1])
return np.array(tau, dtype=float).flatten()
def forward_dynamics(self, q, dq, tau):
"""正向动力学：ddq = M⁻¹(τ - C - G)"""
M = self.get_M(q)
C = self.get_C(q, dq)
G = self.get_G(q)
rhs = np.asarray(tau, dtype=float).reshape(2) - C - G
try:
ddq = np.linalg.solve(M, rhs)
except np.linalg.LinAlgError:
ddq = np.linalg.pinv(M) @ rhs
return ddq
def compute_energy(self, q, dq):
"""返回 (动能, 势能, 总能量) 单位 J"""
T = float(self._T_func(q[0], q[1], dq[0], dq[1]))
V = float(self._V_func(q[0], q[1]))
return T, V, T + V
"dynamics/simulator.py" — 仿真计算类
"""
仿真引擎：轨迹生成 + 动力学三项分解 + 正向积分验证
"""
import numpy as np
from scipy.integrate import odeint
from dynamics.models import RobotParams
from dynamics.lagrangian import LagrangianDynamics
class Simulator:
"""轨迹生成 + 沿轨迹分解动力学方程"""
def __init__(self, params: RobotParams, dynamics: LagrangianDynamics):
self.params = params
self.dynamics = dynamics
self.t = np.arange(0, params.t_end + params.dt, params.dt)
self.N = len(self.t)
def generate_trajectory(self):
"""
生成平滑关节空间轨迹（2s周期装配节拍）
关节1：±0.6 rad, 峰值角加速度 ≈ 5.9 rad/s²
关节2：±0.4 rad, 峰值角加速度 ≈ 3.9 rad/s²
"""
period = 2.0
q1 = 0.6 * np.sin(2 * np.pi * self.t / period)
dq1 = 0.6 * (2 * np.pi / period) * np.cos(2 * np.pi * self.t / period)
ddq1 = -0.6 * (2 * np.pi / period)**2 * np.sin(2 * np.pi * self.t / period)
q2 = 0.4 * np.sin(2 * np.pi * self.t / period + np.pi / 3)
dq2 = 0.4 * (2 * np.pi / period) * np.cos(2 * np.pi * self.t / period + np.pi / 3)
ddq2 = -0.4 * (2 * np.pi / period)**2 * np.sin(2 * np.pi * self.t / period + np.pi / 3)
return (np.column_stack([q1, q2]),
np.column_stack([dq1, dq2]),
np.column_stack([ddq1, ddq2]))
def decompose_trajectory(self, q_traj, dq_traj, ddq_traj):
"""
沿轨迹逐点分解动力学方程为三项
返回：inertia (N,2), coriolis (N,2), gravity (N,2), total (N,2), energies (N,3)
"""
inertia = np.zeros((self.N, 2))
coriolis = np.zeros((self.N, 2))
gravity = np.zeros((self.N, 2))
total = np.zeros((self.N, 2))
energies = np.zeros((self.N, 3))
for i in range(self.N):
q, dq, ddq = q_traj[i], dq_traj[i], ddq_traj[i]
inertia[i] = self.dynamics.get_inertia_term(q, dq, ddq)
coriolis[i] = self.dynamics.get_C(q, dq)
gravity[i] = self.dynamics.get_G(q)
total[i] = self.dynamics.inverse_dynamics(q, dq, ddq)
energies[i] = self.dynamics.compute_energy(q, dq)
return inertia, coriolis, gravity, total, energies
def simulate_forward(self, tau_func, q0, dq0):
"""正向动力学积分验证"""
def rhs(state, t):
q1, q2, dq1, dq2 = state
q = np.array([q1, q2])
dq = np.array([dq1, dq2])
tau = np.asarray(tau_func(t), dtype=float).flatten()
ddq = self.dynamics.forward_dynamics(q, dq, tau)
return np.concatenate([dq, ddq])
state0 = np.concatenate([q0, dq0])
return odeint(rhs, state0, self.t, rtol=1e-10, atol=1e-12)
"dynamics/visualize.py" — 可视化模块
"""四张核心图表 + 三项独立曲线"""
import numpy as np
import matplotlib.pyplot as plt
from dynamics.models import RobotParams
plt.rcParams['font.sans-serif'] = ['SimHei', 'DejaVu Sans']
plt.rcParams['axes.unicode_minus'] = False
class Visualizer:
def __init__(self, params: RobotParams, results_dir="results"):
self.params = params
self.results_dir = results_dir
self.N = int(params.t_end / params.dt) + 1
import os
os.makedirs(results_dir, exist_ok=True)
def plot_torque_curves(self, t, total):
"""① 各关节总力矩 vs 时间"""
fig, (ax1, ax2) = plt.subplots(2, 1, figsize=(10, 7), sharex=True)
ax1.plot(t, total[:, 0], 'b-', linewidth=2)
ax1.axhline(y=np.max(np.abs(total[:, 0])), color='r',
linestyle='--', alpha=0.5,
label=f'Peak {np.max(np.abs(total[:, 0])):.2f} N·m')
ax1.set_ylabel('Torque tau1 (N·m)')
ax1.set_title('Joint 1 Total Torque vs Time')
ax1.legend()
ax1.grid(True, alpha=0.3)
ax2.plot(t, total[:, 1], 'r-', linewidth=2)
ax2.axhline(y=np.max(np.abs(total[:, 1])), color='b',
linestyle='--', alpha=0.5,
label=f'Peak {np.max(np.abs(total[:, 1])):.2f} N·m')
ax2.set_xlabel('Time (s)')
ax2.set_ylabel('Torque tau2 (N·m)')
ax2.set_title('Joint 2 Total Torque vs Time')
ax2.legend()
ax2.grid(True, alpha=0.3)
plt.tight_layout()
path = f"{self.results_dir}/01_torque_vs_time.png"
plt.savefig(path, dpi=150)
plt.close()
return path
def plot_decomposition(self, t, inertia, coriolis, gravity, total):
"""② 力矩分量堆叠面积图（核心：三笔账拆开看）"""
fig, (ax1, ax2) = plt.subplots(2, 1, figsize=(10, 7), sharex=True)
ax1.fill_between(t, 0, gravity[:, 0], alpha=0.6,
color='green', label='Gravity')
ax1.fill_between(t, gravity[:, 0], gravity[:, 0] + coriolis[:, 0],
alpha=0.6, color='orange', label='Coriolis/Centrifugal')
ax1.fill_between(t, gravity[:, 0] + coriolis[:, 0],
gravity[:, 0] + coriolis[:, 0] + inertia[:, 0],
alpha=0.6, color='purple', label='Inertia')
ax1.plot(t, total[:, 0], 'k-', linewidth=1.5)
ax1.set_title('Joint 1 Torque Decomposition: G + C + M·ddq')
ax1.legend(fontsize=8)
ax1.grid(True, alpha=0.3)
ax2.fill_between(t, 0, gravity[:, 1], alpha=0.6,
color='green', label='Gravity')
ax2.fill_between(t, gravity[:, 1], gravity[:, 1] + coriolis[:, 1],
alpha=0.6, color='orange', label='Coriolis/Centrifugal')
ax2.fill_between(t, gravity[:, 1] + coriolis[:, 1],
gravity[:, 1] + coriolis[:, 1] + inertia[:, 1],
alpha=0.6, color='purple', label='Inertia')
ax2.plot(t, total[:, 1], 'k-', linewidth=1.5)
ax2.set_xlabel('Time (s)')
ax2.set_title('Joint 2 Torque Decomposition: G + C + M·ddq')
ax2.legend(fontsize=8)
ax2.grid(True, alpha=0.3)
plt.tight_layout()
path = f"{self.results_dir}/02_torque_decomposition.png"
plt.savefig(path, dpi=150)
plt.close()
return path
def plot_individual_terms(self, t, inertia, coriolis, gravity):
"""②-b 三项独立曲线对比（看清每项波形和量级）"""
fig, axes = plt.subplots(2, 3, figsize=(15, 7), sharex=True)
terms = [('Inertia M·ddq', inertia, 'purple'),
('Coriolis/Centrifugal C', coriolis, 'orange'),
('Gravity G', gravity, 'green')]
for col, (title, data, color) in enumerate(terms):
axes[0, col].plot(t, data[:, 0], color=color, linewidth=2)
axes[0, col].set_title(f'Joint 1 — {title}')
axes[0, col].grid(True, alpha=0.3)
axes[0, col].axhline(y=0, color='k', linewidth=0.5)
axes[1, col].plot(t, data[:, 1], color=color, linewidth=2)
axes[1, col].set_title(f'Joint 2 — {title}')
axes[1, col].set_xlabel('Time (s)')
axes[1, col].grid(True, alpha=0.3)
axes[1, col].axhline(y=0, color='k', linewidth=0.5)
plt.tight_layout()
path = f"{self.results_dir}/02b_individual_terms.png"
plt.savefig(path, dpi=150)
plt.close()
return path
def plot_arm_keyframes(self, q_traj):
"""③ 机械臂运动关键帧"""
idxs = [0, self.N // 4, self.N // 2, 3 * self.N // 4, -1]
l1 = self.params.links[0].length
l2 = self.params.links[1].length
fig, axes = plt.subplots(1, 5, figsize=(20, 4))
for ax, i in zip(axes, idxs):
q1, q2 = q_traj[i, 0], q_traj[i, 1]
x1, y1 = l1 * np.cos(q1), l1 * np.sin(q1)
x2, y2 = x1 + l2 * np.cos(q1 + q2), y1 + l2 * np.sin(q1 + q2)
ax.plot([0, x1], [0, y1], 'b-', linewidth=4)
ax.plot([x1, x2], [y1, y2], 'r-', linewidth=4)
ax.plot(x1, y1, 'go', markersize=6)
ax.plot(x2, y2, 'ro', markersize=6)
ax.set_xlim(-1.2, 1.2)
ax.set_ylim(-1.2, 1.2)
ax.set_aspect('equal')
ax.set_title(f'Frame {i}')
ax.grid(True, alpha=0.3)
plt.tight_layout()
path = f"{self.results_dir}/03_arm_keyframes.png"
plt.savefig(path, dpi=150)
plt.close()
return path
def plot_energy(self, t, energies):
"""④ 能量变化曲线"""
fig, ax = plt.subplots(figsize=(10, 5))
ax.plot(t, energies[:, 0], 'b-', label='Kinetic T (J)')
ax.plot(t, energies[:, 1], 'r-', label='Potential V (J)')
ax.plot(t, energies[:, 2], 'k--', linewidth=2, label='Total E (J)')
ax.set_xlabel('Time (s)')
ax.set_ylabel('Energy (J)')
ax.set_title('Energy Variation (Driven Trajectory)')
ax.legend()
ax.grid(True, alpha=0.3)
plt.tight_layout()
path = f"{self.results_dir}/04_energy_curves.png"
plt.savefig(path, dpi=150)
plt.close()
return path
利用AI解决实际问题，如果你觉得这个工具好用，欢迎关注长安牧笛！
