SymPy 机器人动力学符号推导

机器人学
1.7k 词

用 SymPy 进行机器人动力学的符号推导，避免手工计算出错。覆盖分析力学基础、拉格朗日方程和约束处理。

环境安装

1
pip install sympy

验证：

1
2
3
4
import sympy as sp
sp.init_printing()
x = sp.Symbol('x')
sp.diff(sp.sin(x), x)  # → cos(x)

从零学分析力学

广义坐标

描述系统位形所需的最少独立变量。n 个自由度 → n 个广义坐标 $q_1, q_2, …, q_n$。

约束

  • 完整约束：$f(q_1, …, q_n, t) = 0$（如连杆长度恒定）
  • 非完整约束：不可积分为完整约束的约束（如纯滚动条件）

虚功原理

系统平衡时，所有主动力在虚位移上所做的虚功之和为零：

$$\sum_i \mathbf{F}_i \cdot \delta \mathbf{r}_i = 0$$

达朗贝尔原理

将动力学问题转化为静力学问题：

$$\sum_i (\mathbf{F}_i - m_i \ddot{\mathbf{r}}_i) \cdot \delta \mathbf{r}_i = 0$$

拉格朗日方程

标准形式

$$\frac{d}{dt}\frac{\partial L}{\partial \dot{q}_i} - \frac{\partial L}{\partial q_i} = Q_i$$

其中 $L = T - V$（动能 - 势能），$Q_i$ 为非保守广义力。

带约束的拉格朗日方程

$$\frac{d}{dt}\frac{\partial L}{\partial \dot{q}_i} - \frac{\partial L}{\partial q_i} = Q_i + \sum_k \lambda_k \frac{\partial f_k}{\partial q_i}$$

其中 $\lambda_k$ 为拉格朗日乘子，$f_k$ 为约束方程。

SymPy 推导实战

定义符号变量

1
2
3
4
5
6
7
8
9
10
11
12
import sympy as sp

# 时间
t = sp.Symbol('t')

# 广义坐标
q1, q2 = sp.symbols('q1 q2', cls=sp.Function)
q1 = q1(t); q2 = q2(t)
dq1 = sp.diff(q1, t); dq2 = sp.diff(q2, t)

# 物理参数
m1, m2, l1, l2, g = sp.symbols('m1 m2 l1 l2 g')

位形与速度

1
2
3
4
5
6
7
8
9
10
# 连杆 1 质心位置
x1 = l1/2 * sp.sin(q1)
y1 = -l1/2 * sp.cos(q1)

# 速度
dx1 = sp.diff(x1, t)
dy1 = sp.diff(y1, t)

# 动能
T1 = m1/2 * (dx1**2 + dy1**2)

使用 LagrangesMethod

SymPy 提供了封装好的拉格朗日方法：

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
from sympy.physics.mechanics import *
from sympy.physics.mechanics import LagrangesMethod

# 定义参考系和广义坐标
N = ReferenceFrame('N')
q = dynamicsymbols('q1:3')
dq = dynamicsymbols('q1:3', 1)

# 定义质点
P1 = Point('P1')
P1.set_vel(N, l1/2 * dq[0] * N.y)

# 构建拉格朗日量
L = T - V
LM = LagrangesMethod(L, [q1, q2])
eom = LM.form_lagranges_equations()

建模要点

  1. 明确广义坐标：选择能完整描述系统位形的最小独立变量集
  2. 正确写出动能和势能：对复杂系统，用 SymPy 的 mechanics 模块更安全
  3. 验证约束：检查约束方程是否独立、是否可积
  4. 线性化：对平衡点附近线性化，得到 $M\ddot{q} + Kq = \tau$

手工推导 3 自由度以上的动力学方程极易出错，强烈建议使用 SymPy 或类似的符号计算工具。