别再死记硬背公式了！用Python+SymPy手把手推导平面2R机器人动力学方程

用PythonSymPy实战推导平面2R机器人动力学方程在机器人学领域动力学方程的推导往往是理论学习中最令人头疼的环节。传统教材中密密麻麻的偏微分符号和冗长的代数运算让许多初学者望而却步。本文将带你用Python的SymPy符号计算库从零开始完整推导平面2R机器人的动力学方程让抽象的数学推导变成可执行、可验证的代码实践。1. 准备工作与环境搭建1.1 为什么选择SymPy进行符号计算SymPy是Python的一个纯符号计算库它能够像人类一样处理数学符号而非数值。与数值计算库如NumPy不同SymPy可以保留变量符号并进行精确的代数运算这正是推导动力学方程所需要的核心能力。安装SymPy非常简单pip install sympy1.2 定义符号变量我们首先需要定义所有涉及的符号变量。对于平面2R机器人系统需要以下变量from sympy import symbols, Function, Matrix # 定义符号变量 theta1, theta2 symbols(theta1 theta2) # 关节角度 dtheta1, dtheta2 symbols(dtheta1 dtheta2) # 关节角速度 ddtheta1, ddtheta2 symbols(ddtheta1 ddtheta2) # 关节角加速度 m1, m2 symbols(m1 m2) # 连杆质量 l1, l2 symbols(l1 l2) # 连杆长度 g symbols(g) # 重力加速度 t symbols(t) # 时间变量 # 定义角度随时间变化的函数 theta1 Function(theta1)(t) theta2 Function(theta2)(t)2. 运动学分析与动能计算2.1 连杆末端位置与速度平面2R机器人由两个连杆组成我们需要先计算每个连杆末端的位置坐标from sympy import cos, sin, diff # 第一连杆末端位置 x1 l1 * cos(theta1) y1 l1 * sin(theta1) # 第二连杆末端位置(相对于第一连杆) x2 l2 * cos(theta1 theta2) y2 l2 * sin(theta1 theta2) # 绝对位置 x2_abs x1 x2 y2_abs y1 y2计算速度需要对位置求时间导数# 计算速度 v1_squared diff(x1, t)**2 diff(y1, t)**2 v2_squared diff(x2_abs, t)**2 diff(y2_abs, t)**22.2 系统动能表达式假设质量集中在连杆末端系统的总动能为from sympy import simplify # 动能表达式 T (m1 * v1_squared m2 * v2_squared) / 2 T simplify(T)提示这里使用了simplify函数对表达式进行化简这在后续复杂表达式中尤为重要。3. 势能分析与拉格朗日函数3.1 系统势能计算重力方向为-y方向系统的势能为# 势能计算 V m1 * g * y1 m2 * g * y2_abs V simplify(V)3.2 构造拉格朗日函数拉格朗日函数L定义为动能减去势能# 拉格朗日函数 L T - V4. 拉格朗日方程推导4.1 计算各项偏导数拉格朗日方程的形式为$$ \frac{d}{dt}\left(\frac{\partial L}{\partial \dot{q}_i}\right) - \frac{\partial L}{\partial q_i} \tau_i $$我们需要分别计算对角度和角速度的偏导数from sympy import Derivative # 对theta1的偏导 partial_L_partial_theta1 L.diff(theta1) # 对theta1角速度的偏导 partial_L_partial_dtheta1 L.diff(diff(theta1, t)) # 对theta1角速度偏导的时间导数 d_partial_L_partial_dtheta1 diff(partial_L_partial_dtheta1, t) # 计算tau1 tau1 simplify(d_partial_L_partial_dtheta1 - partial_L_partial_theta1)4.2 完整动力学方程重复上述过程对theta2进行计算最终可以得到完整的动力学方程# 对theta2进行同样计算 partial_L_partial_theta2 L.diff(theta2) partial_L_partial_dtheta2 L.diff(diff(theta2, t)) d_partial_L_partial_dtheta2 diff(partial_L_partial_dtheta2, t) tau2 simplify(d_partial_L_partial_dtheta2 - partial_L_partial_theta2) # 将结果整理为矩阵形式 dynamics_eq Matrix([tau1, tau2])5. 方程化简与标准形式5.1 提取质量矩阵机器人动力学方程通常表示为$$ M(q)\ddot{q} C(q,\dot{q}) G(q) \tau $$我们可以从推导结果中提取出质量矩阵Mfrom sympy import collect # 提取角加速度项得到质量矩阵 M11 collect(tau1, diff(theta1, t, t)).coeff(diff(theta1, t, t)) M12 collect(tau1, diff(theta2, t, t)).coeff(diff(theta2, t, t)) M21 collect(tau2, diff(theta1, t, t)).coeff(diff(theta1, t, t)) M22 collect(tau2, diff(theta2, t, t)).coeff(diff(theta2, t, t)) M Matrix([[M11, M12], [M21, M22]])5.2 科里奥利力和向心力项提取包含角速度乘积的项# 提取科里奥利力和向心力项 C Matrix([tau1, tau2]) - M * Matrix([diff(theta1, t, t), diff(theta2, t, t)]) C simplify(C)5.3 重力项提取仅与角度相关的项# 提取重力项 G simplify(C.subs({diff(theta1, t): 0, diff(theta2, t): 0}))6. 结果验证与可视化6.1 与传统推导结果对比我们可以将SymPy推导的结果与教材中的经典形式进行对比验证# 验证质量矩阵对称性 assert M[0,1] M[1,0] # 验证特定条件下的重力项 test_case {theta1: pi/2, theta2: 0} assert simplify(G[0].subs(test_case)) simplify((m1 m2)*g*l1*cos(pi/2))6.2 使用SymPy的打印功能SymPy提供了多种美观的数学表达式打印方式from sympy import pprint print(质量矩阵M:) pprint(M) print(\n科里奥利力和向心力项C:) pprint(C) print(\n重力项G:) pprint(G)7. 实际应用与扩展7.1 参数代入与数值计算推导出符号形式的动力学方程后我们可以代入具体参数进行数值计算import numpy as np from sympy import lambdify # 定义参数值 params { m1: 1.0, # kg m2: 0.5, # kg l1: 0.3, # m l2: 0.25, # m g: 9.81 # m/s^2 } # 创建数值计算函数 numerical_tau lambdify((theta1, theta2, dtheta1, dtheta2, ddtheta1, ddtheta2), [tau1.subs(params), tau2.subs(params)]) # 示例计算 current_pos [np.pi/4, np.pi/6] # rad current_vel [0.1, -0.2] # rad/s current_acc [0.5, 0.3] # rad/s^2 torques numerical_tau(*current_pos, *current_vel, *current_acc) print(f所需关节力矩: τ1{torques[0]:.2f} Nm, τ2{torques[1]:.2f} Nm)7.2 扩展到更复杂机器人结构虽然本文以平面2R机器人为例但这种方法可以推广到更复杂的机器人结构增加更多连杆只需扩展运动学分析部分考虑分布式质量需要修改动能计算方式加入摩擦等非保守力需要在拉格朗日方程右侧添加相应项# 示例添加粘滞摩擦项 b1, b2 symbols(b1 b2) # 摩擦系数 friction Matrix([-b1*diff(theta1, t), -b2*diff(theta2, t)]) dynamics_eq_with_friction dynamics_eq friction通过这种符号计算方法我们不仅能够验证教科书上的结论还可以灵活地探索各种变体和扩展情况。这种可执行的数学方法极大地增强了学习过程的互动性和直观性。