用Python实战推导二连杆机械臂的拉格朗日动力学方程

在机器人动力学领域，拉格朗日方程提供了一种基于能量的优雅解法。但对于初学者来说，纯数学推导往往令人望而生畏。本文将带你用Python代码一步步实现二连杆机械臂的动力学建模，让抽象的理论变得触手可及。

1. 准备工作与环境搭建

在开始推导前，我们需要准备合适的工具链。推荐使用Python 3.8+环境，配合以下核心库：

import sympy as sp
import numpy as np
import matplotlib.pyplot as plt
from matplotlib.animation import FuncAnimation

SymPy 是本次推导的核心工具，它能进行符号计算，自动处理微分和代数运算。安装这些库只需运行：

pip install sympy numpy matplotlib

定义机械臂的基本参数：

# 定义符号变量
t = sp.symbols('t')  # 时间变量
l1, l2 = sp.symbols('l1 l2')  # 连杆长度
m1, m2 = sp.symbols('m1 m2')  # 连杆质量
g = sp.symbols('g')  # 重力加速度
theta1 = sp.Function('theta1')(t)  # 关节1角度
theta2 = sp.Function('theta2')(t)  # 关节2角度

2. 运动学分析与动能计算

动能计算需要先确定各连杆末端的速度。我们首先建立坐标系：

• 基座固定在关节1处
• 连杆1长度为l1，质量m1集中在末端
• 连杆2长度为l2，质量m2集中在末端

位置分析：

# 连杆1末端位置
x1 = l1 * sp.cos(theta1)
y1 = l1 * sp.sin(theta1)

# 连杆2末端位置
x2 = x1 + l2 * sp.cos(theta1 + theta2)
y2 = y1 + l2 * sp.sin(theta1 + theta2)

速度计算：

通过对位置求时间导数得到速度：

# 计算速度
v1_sq = sp.diff(x1, t)**2 + sp.diff(y1, t)**2
v2_sq = sp.diff(x2, t)**2 + sp.diff(y2, t)**2

# 动能表达式
T = (m1 * v1_sq + m2 * v2_sq) / 2

小技巧：使用SymPy的simplify()函数可以简化复杂的动能表达式：

T_simplified = sp.simplify(T)
print("简化后的动能表达式:")
sp.pprint(T_simplified)

3. 势能计算与拉格朗日量

势能计算相对简单，主要考虑重力势能：

# 势能计算
U = m1 * g * y1 + m2 * g * y2

# 拉格朗日量
L = T - U

为了更直观理解，我们可以将势能表达式展开：

U = g*(l1*m1*sin(θ₁(t)) + l2*m2*sin(θ₁(t) + θ₂(t)) + l1*m2*sin(θ₁(t)))

4. 拉格朗日方程推导

拉格朗日方程的一般形式为：

$$ \frac{d}{dt}\left(\frac{\partial L}{\partial \dot{q_i}}\right) - \frac{\partial L}{\partial q_i} = \tau_i $$

其中$q_i$是广义坐标（这里即θ₁和θ₂），$\tau_i$是对应的关节力矩。

对θ₁的推导：

# 对θ₁的偏导数
dL_dtheta1 = sp.diff(L, theta1)
dL_dtheta1_dot = sp.diff(L, sp.diff(theta1, t))

# 时间导数
dt_dL_dtheta1_dot = sp.diff(dL_dtheta1_dot, t)

# θ₁的力矩方程
tau1 = dt_dL_dtheta1_dot - dL_dtheta1
tau1 = sp.simplify(tau1)

对θ₂的推导：

# 对θ₂的偏导数
dL_dtheta2 = sp.diff(L, theta2)
dL_dtheta2_dot = sp.diff(L, sp.diff(theta2, t))

# 时间导数
dt_dL_dtheta2_dot = sp.diff(dL_dtheta2_dot, t)

# θ₂的力矩方程
tau2 = dt_dL_dtheta2_dot - dL_dtheta2
tau2 = sp.simplify(tau2)

5. 结果分析与可视化

推导完成后，我们可以将结果整理为标准的动力学方程形式：

$$ \tau = M(q)\ddot{q} + C(q,\dot{q}) + G(q) $$

其中：

• $M(q)$是质量矩阵
• $C(q,\dot{q})$包含离心力和哥氏力项
• $G(q)$是重力项

提取质量矩阵：

# 定义加速度符号
theta1_ddot = sp.symbols('theta1_ddot')
theta2_ddot = sp.symbols('theta2_ddot')

# 替换加速度项
tau1_acc = tau1.subs(sp.diff(theta1, t, t), theta1_ddot).subs(sp.diff(theta2, t, t), theta2_ddot)
tau2_acc = tau2.subs(sp.diff(theta1, t, t), theta1_ddot).subs(sp.diff(theta2, t, t), theta2_ddot)

# 提取质量矩阵系数
M11 = tau1_acc.coeff(theta1_ddot)
M12 = tau1_acc.coeff(theta2_ddot)
M21 = tau2_acc.coeff(theta1_ddot)
M22 = tau2_acc.coeff(theta2_ddot)

M = sp.Matrix([[M11, M12], [M21, M22]])
print("质量矩阵M:")
sp.pprint(M)

可视化机械臂运动：

为了更直观理解，我们可以用Matplotlib创建简单的动画：

def animate_arm(theta1_vals, theta2_vals, l1, l2):
    fig, ax = plt.subplots(figsize=(8, 6))
    ax.set_xlim(-(l1+l2)*1.2, (l1+l2)*1.2)
    ax.set_ylim(-(l1+l2)*1.2, (l1+l2)*1.2)
    line, = ax.plot([], [], 'o-', lw=2)
    
    def update(frame):
        x = [0, l1*np.cos(theta1_vals[frame]), 
             l1*np.cos(theta1_vals[frame]) + l2*np.cos(theta1_vals[frame]+theta2_vals[frame])]
        y = [0, l1*np.sin(theta1_vals[frame]), 
             l1*np.sin(theta1_vals[frame]) + l2*np.sin(theta1_vals[frame]+theta2_vals[frame])]
        line.set_data(x, y)
        return line,
    
    ani = FuncAnimation(fig, update, frames=len(theta1_vals), blit=True)
    plt.close()
    return ani

6. 数值验证与效率对比

为了验证推导的正确性，我们可以设定具体参数进行数值计算：

# 参数设定
params = {
    l1: 1.0,   # 连杆1长度1m
    l2: 0.8,   # 连杆2长度0.8m
    m1: 2.0,   # 连杆1质量2kg
    m2: 1.5,   # 连杆2质量1.5kg
    g: 9.81    # 重力加速度
}

# 定义运动状态
state = {
    theta1: np.pi/4,          # 关节1角度45度
    theta2: np.pi/6,          # 关节2角度30度
    sp.diff(theta1, t): 0.5,  # 关节1角速度0.5rad/s
    sp.diff(theta2, t): -0.3, # 关节2角速度-0.3rad/s
    sp.diff(theta1, t, t): 0, # 关节1角加速度0
    sp.diff(theta2, t, t): 0  # 关节2角加速度0
}

# 计算力矩
tau1_num = tau1.subs(params).subs(state)
tau2_num = tau2.subs(params).subs(state)
print(f"关节1力矩: {tau1_num.evalf():.3f} N·m")
print(f"关节2力矩: {tau2_num.evalf():.3f} N·m")

计算效率对比：

方法	时间复杂度	适用场景	实现难度
拉格朗日法	O(n³)	理论分析、控制器设计	中等
牛顿-欧拉法	O(n)	实时控制	较高

在实际项目中，拉格朗日法更适合用于理论分析和控制器设计，而实时控制通常采用计算效率更高的牛顿-欧拉法。