《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)↓步骤1sympy推导 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 时才是真正的科氏力jk 时是离心力。重力项∂V/∂qᵢ翻译关节i的重力矩 势能对该关节坐标的偏导数。物理意义是把这个关节转动一小下重力势能变化多少。4. OOP代码实现技术栈严格限定numpy, scipy, matplotlib, sympy符号推导lambdify仅用于建模阶段requirements.txtnumpy1.24.0scipy1.10.0matplotlib3.7.0sympy1.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 dataclassfrom typing import Listdataclassclass Link:单连杆刚体参数2-DOF平面臂name: strmass: float # kglength: float # mcom_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}])dataclassclass RobotParams:2-DOF平面装配机械臂完整参数links: List[Link]payload_mass: float 1.5 # kg装配场景负载轻gravity: float 9.81 # m/s²dt: float 0.01 # st_end: float 2.0 # s2秒节拍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 npimport sympy as spfrom dynamics.models import RobotParamsclass LagrangianDynamics:2-DOF平面机械臂动力学建模与分解def __init__(self, params: RobotParams):self.params paramsself._build_symbolic()self._lambdify()def _build_symbolic(self):符号推导T, V → M, C, Gq1, q2 sp.symbols(q1 q2, realTrue)dq1, dq2 sp.symbols(dq1 dq2, realTrue)ddq1, ddq2 sp.symbols(ddq1 ddq2, realTrue)g self.params.gravityl1 self.params.links[0].lengthl2 self.params.links[1].lengthm1 self.params.links[0].massm2 self.params.links[1].masslc1 self.params.links[0].com_offsetlc2 self.params.links[1].com_offsetI1 self.params.links[0].inertiaI2 self.params.links[1].inertiamp self.params.payload_mass# 动能 TT1 0.5 * m1 * (lc1 * dq1)**2 0.5 * I1 * dq1**2T2 (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# 势能 VV (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/∂qG1 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 0for 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 Gddq sp.Matrix([ddq1, ddq2])tau M * ddq C Gself._q1, self._q2 q1, q2self._dq1, self._dq2 dq1, dq2self._ddq1, self._ddq2 ddq1, ddq2self._M, self._C, self._G, self._tau M, C, G, tauself._T_expr, self._V_expr T, Vdef _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²修复签名只接受 qM 只依赖位置不依赖速度return np.array(self._M_func(q[0], q[1], 0.0, 0.0, 0.0, 0.0), dtypefloat)def get_C(self, q, dq):科氏力/离心力项 C(q,dq), 形状 (2,), 单位 N·mreturn np.array(self._C_func(q[0], q[1], dq[0], dq[1], 0.0, 0.0),dtypefloat).reshape(2)def get_G(self, q):重力项 G(q), 形状 (2,), 单位 N·mreturn np.array(self._G_func(q[0], q[1]), dtypefloat).reshape(2)def get_inertia_term(self, q, dq, ddq):惯性项 M(q)·ddq, 形状 (2,), 单位 N·mM self.get_M(q)return M np.asarray(ddq, dtypefloat).reshape(2)def inverse_dynamics(self, q, dq, ddq):总力矩 τ M·ddq C G, 形状 (2,), 单位 N·mtau self._tau_func(q[0], q[1], dq[0], dq[1], ddq[0], ddq[1])return np.array(tau, dtypefloat).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, dtypefloat).reshape(2) - C - Gtry:ddq np.linalg.solve(M, rhs)except np.linalg.LinAlgError:ddq np.linalg.pinv(M) rhsreturn ddqdef compute_energy(self, q, dq):返回 (动能, 势能, 总能量) 单位 JT 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 Vdynamics/simulator.py — 仿真计算类仿真引擎轨迹生成 动力学三项分解 正向积分验证import numpy as npfrom scipy.integrate import odeintfrom dynamics.models import RobotParamsfrom dynamics.lagrangian import LagrangianDynamicsclass Simulator:轨迹生成 沿轨迹分解动力学方程def __init__(self, params: RobotParams, dynamics: LagrangianDynamics):self.params paramsself.dynamics dynamicsself.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.0q1 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, energiesdef simulate_forward(self, tau_func, q0, dq0):正向动力学积分验证def rhs(state, t):q1, q2, dq1, dq2 stateq np.array([q1, q2])dq np.array([dq1, dq2])tau np.asarray(tau_func(t), dtypefloat).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, rtol1e-10, atol1e-12)dynamics/visualize.py — 可视化模块四张核心图表 三项独立曲线import numpy as npimport matplotlib.pyplot as pltfrom dynamics.models import RobotParamsplt.rcParams[font.sans-serif] [SimHei, DejaVu Sans]plt.rcParams[axes.unicode_minus] Falseclass Visualizer:def __init__(self, params: RobotParams, results_dirresults):self.params paramsself.results_dir results_dirself.N int(params.t_end / params.dt) 1import osos.makedirs(results_dir, exist_okTrue)def plot_torque_curves(self, t, total):① 各关节总力矩 vs 时间fig, (ax1, ax2) plt.subplots(2, 1, figsize(10, 7), sharexTrue)ax1.plot(t, total[:, 0], b-, linewidth2)ax1.axhline(ynp.max(np.abs(total[:, 0])), colorr,linestyle--, alpha0.5,labelfPeak {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, alpha0.3)ax2.plot(t, total[:, 1], r-, linewidth2)ax2.axhline(ynp.max(np.abs(total[:, 1])), colorb,linestyle--, alpha0.5,labelfPeak {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, alpha0.3)plt.tight_layout()path f{self.results_dir}/01_torque_vs_time.pngplt.savefig(path, dpi150)plt.close()return pathdef plot_decomposition(self, t, inertia, coriolis, gravity, total):② 力矩分量堆叠面积图核心三笔账拆开看fig, (ax1, ax2) plt.subplots(2, 1, figsize(10, 7), sharexTrue)ax1.fill_between(t, 0, gravity[:, 0], alpha0.6,colorgreen, labelGravity)ax1.fill_between(t, gravity[:, 0], gravity[:, 0] coriolis[:, 0],alpha0.6, colororange, labelCoriolis/Centrifugal)ax1.fill_between(t, gravity[:, 0] coriolis[:, 0],gravity[:, 0] coriolis[:, 0] inertia[:, 0],alpha0.6, colorpurple, labelInertia)ax1.plot(t, total[:, 0], k-, linewidth1.5)ax1.set_title(Joint 1 Torque Decomposition: G C M·ddq)ax1.legend(fontsize8)ax1.grid(True, alpha0.3)ax2.fill_between(t, 0, gravity[:, 1], alpha0.6,colorgreen, labelGravity)ax2.fill_between(t, gravity[:, 1], gravity[:, 1] coriolis[:, 1],alpha0.6, colororange, labelCoriolis/Centrifugal)ax2.fill_between(t, gravity[:, 1] coriolis[:, 1],gravity[:, 1] coriolis[:, 1] inertia[:, 1],alpha0.6, colorpurple, labelInertia)ax2.plot(t, total[:, 1], k-, linewidth1.5)ax2.set_xlabel(Time (s))ax2.set_title(Joint 2 Torque Decomposition: G C M·ddq)ax2.legend(fontsize8)ax2.grid(True, alpha0.3)plt.tight_layout()path f{self.results_dir}/02_torque_decomposition.pngplt.savefig(path, dpi150)plt.close()return pathdef plot_individual_terms(self, t, inertia, coriolis, gravity):②-b 三项独立曲线对比看清每项波形和量级fig, axes plt.subplots(2, 3, figsize(15, 7), sharexTrue)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], colorcolor, linewidth2)axes[0, col].set_title(fJoint 1 — {title})axes[0, col].grid(True, alpha0.3)axes[0, col].axhline(y0, colork, linewidth0.5)axes[1, col].plot(t, data[:, 1], colorcolor, linewidth2)axes[1, col].set_title(fJoint 2 — {title})axes[1, col].set_xlabel(Time (s))axes[1, col].grid(True, alpha0.3)axes[1, col].axhline(y0, colork, linewidth0.5)plt.tight_layout()path f{self.results_dir}/02b_individual_terms.pngplt.savefig(path, dpi150)plt.close()return pathdef plot_arm_keyframes(self, q_traj):③ 机械臂运动关键帧idxs [0, self.N // 4, self.N // 2, 3 * self.N // 4, -1]l1 self.params.links[0].lengthl2 self.params.links[1].lengthfig, 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-, linewidth4)ax.plot([x1, x2], [y1, y2], r-, linewidth4)ax.plot(x1, y1, go, markersize6)ax.plot(x2, y2, ro, markersize6)ax.set_xlim(-1.2, 1.2)ax.set_ylim(-1.2, 1.2)ax.set_aspect(equal)ax.set_title(fFrame {i})ax.grid(True, alpha0.3)plt.tight_layout()path f{self.results_dir}/03_arm_keyframes.pngplt.savefig(path, dpi150)plt.close()return pathdef plot_energy(self, t, energies):④ 能量变化曲线fig, ax plt.subplots(figsize(10, 5))ax.plot(t, energies[:, 0], b-, labelKinetic T (J))ax.plot(t, energies[:, 1], r-, labelPotential V (J))ax.plot(t, energies[:, 2], k--, linewidth2, labelTotal E (J))ax.set_xlabel(Time (s))ax.set_ylabel(Energy (J))ax.set_title(Energy Variation (Driven Trajectory))ax.legend()ax.grid(True, alpha0.3)plt.tight_layout()path f{self.results_dir}/04_energy_curves.pngplt.savefig(path, dpi150)plt.close()return path利用AI解决实际问题如果你觉得这个工具好用欢迎关注长安牧笛