《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解决实际问题，如果你觉得这个工具好用，欢迎关注长安牧笛！