用Python从零推导两连杆机械臂动力学：手把手带你复现拉格朗日方程（附完整代码）

用Python从零推导两连杆机械臂动力学手把手带你复现拉格朗日方程附完整代码机械臂动力学是机器人控制的核心基础但许多学习者在理解理论后往往卡在如何将数学公式转化为可执行代码的环节。本文将带你用Python一步步实现两连杆机械臂的完整动力学建模从符号运算到数值计算最终输出惯性矩阵、科氏力矩阵和重力向量。我们会用SymPy处理符号推导用NumPy进行数值验证并提供可直接运行的Jupyter Notebook代码。1. 环境准备与基础建模首先确保安装以下Python库推荐使用Anaconda环境pip install numpy sympy matplotlib定义机械臂的基本参数和符号变量import numpy as np import sympy as sp from sympy.physics.mechanics import dynamicsymbols # 定义符号变量 q1, q2 dynamicsymbols(q1 q2) # 关节角度 dq1, dq2 dynamicsymbols(q1 q2, 1) # 关节角速度 l1, l2 sp.symbols(l1 l2) # 连杆长度 m1, m2 sp.symbols(m1 m2) # 连杆质量 g sp.symbols(g) # 重力加速度 t sp.symbols(t) # 时间变量2. 运动学建模与能量计算2.1 位置与速度推导计算各连杆末端的位置坐标假设关节1固定在原点# 连杆1末端位置 x1 l1 * sp.cos(q1) y1 l1 * sp.sin(q1) # 连杆2末端位置 x2 x1 l2 * sp.cos(q1 q2) y2 y1 l2 * sp.sin(q1 q2)通过时间导数获得速度分量# 连杆1末端速度 v1x sp.diff(x1, t) v1y sp.diff(y1, t) # 连杆2末端速度 v2x sp.diff(x2, t) v2y sp.diff(y2, t)2.2 动能与势能计算根据经典物理公式计算各连杆能量# 动能计算假设质量集中在连杆末端 T1 m1 * (v1x**2 v1y**2) / 2 T2 m2 * (v2x**2 v2y**2) / 2 T_total T1 T2 # 系统总动能 # 势能计算y轴向上为正 V1 m1 * g * y1 V2 m2 * g * y2 V_total V1 V2 # 系统总势能3. 拉格朗日动力学推导3.1 构建拉格朗日函数L T_total - V_total3.2 推导动力学方程使用SymPy的LagrangesMethod自动推导from sympy.physics.mechanics import LagrangesMethod # 定义广义坐标和广义力 q [q1, q2] Q [0, 0] # 假设无外力 # 创建拉格朗日方法对象 LM LagrangesMethod(L, q) # 形成运动方程 LM.form_lagranges_equations() equations LM.eom3.3 提取标准动力学形式将方程整理为矩阵形式# 定义广义加速度 ddq1, ddq2 sp.symbols(ddq1 ddq2) # 提取惯性矩阵M M LM.mass_matrix # 提取科氏力/离心力向量C C LM.forcing # 提取重力向量G G [sp.diff(V_total, qi) for qi in q]4. 数值验证与可视化4.1 参数代入与函数生成# 定义具体参数值 params { l1: 1.0, # 连杆1长度1m l2: 0.8, # 连杆2长度0.8m m1: 1.5, # 连杆1质量1.5kg m2: 1.0, # 连杆2质量1kg g: 9.81 # 重力加速度 } # 将符号表达式转换为数值函数 M_func sp.lambdify((q1, q2), M.subs(params), numpy) C_func sp.lambdify((q1, q2, dq1, dq2), C.subs(params), numpy) G_func sp.lambdify((q1, q2), [g.subs(params) for g in G], numpy)4.2 动力学方程求解示例def compute_torques(q, dq, ddq): 计算所需关节力矩 q1, q2 q dq1, dq2 dq ddq1, ddq2 ddq M_val M_func(q1, q2) C_val C_func(q1, q2, dq1, dq2) G_val G_func(q1, q2) return M_val [ddq1, ddq2] C_val G_val # 测试在特定位形下的力矩 q_test [np.pi/4, np.pi/3] # 关节角度 dq_test [0.1, -0.2] # 关节角速度 ddq_test [0, 0] # 关节加速度 torques compute_torques(q_test, dq_test, ddq_test) print(f所需关节力矩: {torques})5. 常见问题与调试技巧符号运算速度慢简化表达式使用sp.simplify()或sp.trigsimp()提前计算子表达式数值计算异常# 检查惯性矩阵是否正定 def is_positive_definite(M): return np.all(np.linalg.eigvals(M) 0) print(is_positive_definite(M_func(0,0)))可视化验证运动学import matplotlib.pyplot as plt def plot_arm(q1, q2): x0, y0 0, 0 x1 params[l1] * np.cos(q1) y1 params[l1] * np.sin(q1) x2 x1 params[l2] * np.cos(q1q2) y2 y1 params[l2] * np.sin(q1q2) plt.plot([x0,x1], [y0,y1], b-o, linewidth3) plt.plot([x1,x2], [y1,y2], r-o, linewidth3) plt.axis(equal) plt.grid(True) plot_arm(np.pi/4, np.pi/3)完整代码已封装为可复用的Python类包含以下核心方法compute_M(): 返回当前位形下的惯性矩阵compute_C(): 返回科氏力/离心力向量compute_G(): 返回重力补偿向量forward_dynamics(): 给定力矩计算加速度inverse_dynamics(): 给定运动状态计算所需力矩实际项目中这些动力学计算可以集成到控制循环中实现基于模型的精确控制。例如在PID控制器中可以先计算重力补偿项显著提高控制性能。