别再死记公式了！用Python+SymPy从零推导两连杆机械臂动力学方程（保姆级教程）
用Python+SymPy从零推导两连杆机械臂动力学方程：让数学推导自动化
在机器人动力学建模领域，两连杆机械臂是最基础的模型之一，也是理解更复杂机械系统的重要起点。传统教材中，推导其动力学方程往往需要大量繁琐的手工计算，从位置、速度到动能、势能，再到拉格朗日方程的展开，每一步都可能因为符号运算的复杂性而引入错误。而现代符号计算工具如SymPy，可以让我们摆脱这些重复性劳动，专注于物理本质和工程应用。
1. 环境准备与基础概念
1.1 安装必要的Python库
开始之前，确保你的Python环境已安装以下库：
pip install sympy numpy matplotlibSymPy是核心的符号计算库，NumPy用于后续可能的数值计算，Matplotlib则可用于可视化结果。
1.2 机械臂模型的基本假设
我们考虑一个典型的平面两连杆机械臂模型，做出以下简化假设：
- 连杆为刚性杆件，质量集中在末端（点质量模型）
- 关节处无摩擦，运动完全由驱动力矩控制
- 所有运动发生在二维平面内
模型参数定义：
| 符号 | 含义 |
|---|---|
| l₁ | 第一连杆长度 |
| l₂ | 第二连杆长度 |
| m₁ | 第一连杆末端质量 |
| m₂ | 第二连杆末端质量 |
| q₁ | 第一关节角度 |
| q₂ | 第二关节相对角度 |
| g | 重力加速度 |
2. 建立符号系统与运动学关系
2.1 初始化符号变量
首先导入SymPy并定义所有需要的符号变量：
from sympy import symbols, Function, Matrix, sin, cos, diff, simplify # 定义常数和符号变量 l1, l2, m1, m2, g, t = symbols('l1 l2 m1 m2 g t') q1 = Function('q1')(t) # 关节角度是时间的函数 q2 = Function('q2')(t) # 角速度定义 q1_dot = diff(q1, t) q2_dot = diff(q2, t)2.2 计算末端位置坐标
根据几何关系，可以写出两个连杆末端的位置坐标：
# 第一连杆末端位置 x1 = l1 * cos(q1) y1 = l1 * sin(q1) # 第二连杆末端位置 x2 = x1 + l2 * cos(q1 + q2) y2 = y1 + l2 * sin(q1 + q2)注意：这里q₂是第二连杆相对于第一连杆的角度，因此总角度为q₁+q₂
2.3 计算末端速度
通过对位置求时间导数得到速度：
# 第一连杆末端速度 v1x = diff(x1, t) v1y = diff(y1, t) # 第二连杆末端速度 v2x = diff(x2, t) v2y = diff(y2, t) # 速度平方（用于动能计算） v1_sq = v1x**2 + v1y**2 v2_sq = v2x**2 + v2y**23. 能量计算与拉格朗日函数
3.1 动能计算
根据物理学定义，动能T=½mv²：
# 动能计算 T1 = m1 * v1_sq / 2 T2 = m2 * v2_sq / 2 T = T1 + T2 # 系统总动能3.2 势能计算
以基座为势能零点，势能V=mgh：
# 势能计算 V1 = m1 * g * y1 V2 = m2 * g * y2 V = V1 + V2 # 系统总势能3.3 构建拉格朗日函数
拉格朗日函数L定义为动能减去势能：
L = T - V4. 推导动力学方程
4.1 拉格朗日方程形式
对于每个广义坐标qᵢ，拉格朗日方程为：
d/dt(∂L/∂q̇ᵢ) - ∂L/∂qᵢ = τᵢ其中τᵢ是对应关节的广义力（力矩）。
4.2 自动求导实现
使用SymPy自动计算各项导数：
# 对第一个关节的方程 tau1 = symbols('tau1') # 第一个关节的力矩 # 计算各项导数 dL_dq1dot = diff(L, q1_dot) term1 = diff(dL_dq1dot, t) term2 = diff(L, q1) eq1 = simplify(term1 - term2 - tau1) # 对第二个关节的方程 tau2 = symbols('tau2') # 第二个关节的力矩 dL_dq2dot = diff(L, q2_dot) term3 = diff(dL_dq2dot, t) term4 = diff(L, q2) eq2 = simplify(term3 - term4 - tau2)4.3 整理矩阵形式
机械臂动力学通常表示为矩阵形式：
M(q)q̈ + C(q,q̇)q̇ + G(q) = τ我们可以将前面得到的方程整理成这种形式：
# 定义角加速度符号 q1_ddot = symbols('q1_ddot') q2_ddot = symbols('q2_ddot') # 提取惯性矩阵M M = Matrix([[eq1.coeff(q1_ddot), eq1.coeff(q2_ddot)], [eq2.coeff(q1_ddot), eq2.coeff(q2_ddot)]]) # 提取科氏力和离心力项 C = Matrix([[simplify(eq1 - M[0,0]*q1_ddot - M[0,1]*q2_ddot).subs({q1_dot:0, q2_dot:0})], [simplify(eq2 - M[1,0]*q1_ddot - M[1,1]*q2_ddot).subs({q1_dot:0, q2_dot:0})]]) # 提取重力项 G = Matrix([[simplify(eq1 - M[0,0]*q1_ddot - M[0,1]*q2_ddot - C[0]).subs({q1_dot:0, q2_dot:0, q1_ddot:0, q2_ddot:0})], [simplify(eq2 - M[1,0]*q1_ddot - M[1,1]*q2_ddot - C[1]).subs({q1_dot:0, q2_dot:0, q1_ddot:0, q2_ddot:0})]])5. 结果验证与应用
5.1 与理论结果对比
将SymPy推导结果与经典教材中的两连杆机械臂动力学方程对比：
- 惯性矩阵M应具有对称性
- 当q₂=0时，系统应退化为单摆
- 当g=0时，重力项G应为零
5.2 数值模拟准备
为了验证推导的正确性，可以将符号表达式转换为数值计算函数：
from sympy.utilities.lambdify import lambdify import numpy as np # 创建数值计算函数 params = {l1: 1.0, l2: 0.8, m1: 1.0, m2: 0.5, g: 9.81} M_func = lambdify((q1, q2), M.subs(params), 'numpy') C_func = lambdify((q1, q2, q1_dot, q2_dot), C.subs(params), 'numpy') G_func = lambdify((q1, q2), G.subs(params), 'numpy')5.3 控制仿真中的应用
这些动力学方程可直接用于：
- 计算前馈控制力矩
- 设计基于模型的控制器
- 系统动力学特性分析
在实际项目中，我发现将符号推导结果转换为C代码可以显著提高实时控制性能。SymPy提供了ccode函数，可以直接生成C代码：
from sympy.printing import ccode # 生成M矩阵的C代码 print("// 惯性矩阵M") for i in range(2): for j in range(2): print(f"double M{i}{j} = {ccode(M[i,j].subs(params))};")