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

用Python+SymPy实战推导平面2R机器人动力学方程

在机器人学领域，动力学方程的推导往往是理论学习中最令人头疼的环节。传统教材中密密麻麻的偏微分符号和冗长的代数运算，让许多初学者望而却步。本文将带你用Python的SymPy符号计算库，从零开始完整推导平面2R机器人的动力学方程，让抽象的数学推导变成可执行、可验证的代码实践。

1. 准备工作与环境搭建

1.1 为什么选择SymPy进行符号计算

SymPy是Python的一个纯符号计算库，它能够像人类一样处理数学符号而非数值。与数值计算库如NumPy不同，SymPy可以保留变量符号并进行精确的代数运算，这正是推导动力学方程所需要的核心能力。安装SymPy非常简单：

pip install sympy

1.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)**2

2.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 - V

4. 拉格朗日方程推导

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 $$

我们可以从推导结果中提取出质量矩阵M：

from 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机器人为例，但这种方法可以推广到更复杂的机器人结构：

1. 增加更多连杆只需扩展运动学分析部分
2. 考虑分布式质量需要修改动能计算方式
3. 加入摩擦等非保守力需要在拉格朗日方程右侧添加相应项

# 示例：添加粘滞摩擦项 b1, b2 = symbols('b1 b2') # 摩擦系数 friction = Matrix([-b1*diff(theta1, t), -b2*diff(theta2, t)]) dynamics_eq_with_friction = dynamics_eq + friction

通过这种符号计算方法，我们不仅能够验证教科书上的结论，还可以灵活地探索各种变体和扩展情况。这种"可执行的数学"方法极大地增强了学习过程的互动性和直观性。