2. 双足机器人动力学建模

任务目标

为双足机器人建立可用于控制设计的动力学模型。本案例采用拉格朗日方程方法，以五连杆双足机器人（躯干+双腿，共5个刚体）为对象，推导其动力学方程，并给出可直接用于仿真的矩阵形式：

M(q)q̈ + C(q,q̇)q̇ + G(q) = τ

其中 q = [q1, q2, q3, q4, q5]ᵀ 为五个关节角，τ 为对应关节力矩。该模型是步态规划、平衡控制与仿真验证的基础。

模型定义与假设

  • 五连杆结构：躯干1个 + 左右大腿2个 + 左右小腿2个，共5个刚体，5个旋转关节（髋×2、膝×2、躯干与髋连线虚拟关节×1）。
  • 所有质量集中在各连杆质心，质心位于连杆几何中心。
  • 运动限制在矢状面（二维平面）内，忽略脚掌与地面的滚动，支撑脚与地面为点接触且无滑动（单支撑相）。
  • 各连杆参数：质量 m_i、长度 l_i、质心到关节距离 r_i、转动惯量 I_i。

关键完整代码（Python + SymPy）

以下代码自动推导五连杆双足机器人的拉格朗日动力学方程，输出质量矩阵、科氏力/离心力项和重力项，并生成可被数值仿真直接调用的Python函数。

import sympy as sp
import numpy as np
from sympy.physics.mechanics import dynamicsymbols, ReferenceFrame, Point, Particle, Lagrangian, LagrangesMethod

# ========== 1. 定义符号参数 ==========
# 关节角（广义坐标）
q1, q2, q3, q4, q5 = dynamicsymbols('q1 q2 q3 q4 q5')
# 关节角速度
q1d, q2d, q3d, q4d, q5d = dynamicsymbols('q1 q2 q3 q4 q5', 1)
# 连杆参数（符号）
m1, m2, m3, m4, m5 = sp.symbols('m1 m2 m3 m4 m5')   # 质量
l1, l2, l3, l4, l5 = sp.symbols('l1 l2 l3 l4 l5')   # 杆长
r1, r2, r3, r4, r5 = sp.symbols('r1 r2 r3 r4 r5')   # 质心到近端关节距离
I1, I2, I3, I4, I5 = sp.symbols('I1 I2 I3 I4 I5')   # 绕质心转动惯量
g = sp.symbols('g')                                  # 重力加速度

# ========== 2. 建立参考系与坐标 ==========
N = ReferenceFrame('N')          # 惯性系（地面）
# 支撑脚踝位于原点，各连杆方向用旋转矩阵表示
# 定义各连杆坐标系（相对前一杆的转角）
theta1 = q1   # 支撑腿大腿相对竖直方向
theta2 = q1 + q2   # 支撑腿小腿相对竖直方向
theta3 = q1 + q2 + q3   # 躯干相对竖直方向
theta4 = q1 + q2 + q3 + q4   # 摆动腿大腿
theta5 = q1 + q2 + q3 + q4 + q5   # 摆动腿小腿

# 各连杆质心位置（从支撑脚踝出发，沿矢状面）
# 支撑腿大腿质心
p1 = r1 * sp.sin(theta1) * N.x - r1 * sp.cos(theta1) * N.y
# 支撑腿小腿质心（髋关节 + 小腿质心偏移）
hip = l1 * sp.sin(theta1) * N.x - l1 * sp.cos(theta1) * N.y
p2 = hip + r2 * sp.sin(theta2) * N.x - r2 * sp.cos(theta2) * N.y
# 躯干质心（髋关节 + 躯干质心偏移）
torso_base = hip
p3 = torso_base + r3 * sp.sin(theta3) * N.x - r3 * sp.cos(theta3) * N.y
# 摆动腿大腿质心（髋关节 + 大腿偏移）
p4 = torso_base + l3 * sp.sin(theta3) * N.x - l3 * sp.cos(theta3) * N.y + r4 * sp.sin(theta4) * N.x - r4 * sp.cos(theta4) * N.y
# 摆动腿小腿质心
knee_swing = torso_base + l3 * sp.sin(theta3) * N.x - l3 * sp.cos(theta3) * N.y + l4 * sp.sin(theta4) * N.x - l4 * sp.cos(theta4) * N.y
p5 = knee_swing + r5 * sp.sin(theta5) * N.x - r5 * sp.cos(theta5) * N.y

# ========== 3. 定义刚体（质点 + 转动惯量） ==========
# 各连杆角速度（二维平面内，绕Z轴）
omega1 = q1d * N.z
omega2 = (q1d + q2d) * N.z
omega3 = (q1d + q2d + q3d) * N.z
omega4 = (q1d + q2d + q3d + q4d) * N.z
omega5 = (q1d + q2d + q3d + q4d + q5d) * N.z

# 创建质点（平动动能）与刚体（转动动能）
def make_body(name, mass, pos, omega, inertia):
    pt = Point(name + '_pt')
    pt.set_pos(Point('origin'), pos)
    pt.set_vel(N, pt.pos_from(Point('origin')).diff(N))
    return Particle(name, pt, mass), inertia * omega.dot(omega) / 2

# 简化：用Particle表示平动，转动动能单独加入拉格朗日量
# 各连杆平动动能
KE_trans = 0
for m_i, p_i in [(m1, p1), (m2, p2), (m3, p3), (m4, p4), (m5, p5)]:
    v_i = sp.diff(p_i, dynamicsymbols._t)
    KE_trans += sp.Rational(1, 2) * m_i * v_i.dot(v_i)

# 转动动能
KE_rot = sp.Rational(1, 2) * (I1 * omega1.dot(omega1) + I2 * omega2.dot(omega2) +
                               I3 * omega3.dot(omega3) + I4 * omega4.dot(omega4) +
                               I5 * omega5.dot(omega5))
KE = KE_trans + KE_rot

# 势能（重力，y轴向上为负）
PE = m1 * g * p1.dot(N.y) + m2 * g * p2.dot(N.y) + m3 * g * p3.dot(N.y) + \
     m4 * g * p4.dot(N.y) + m5 * g * p5.dot(N.y)

# ========== 4. 拉格朗日方程 ==========
L = KE - PE
q = [q1, q2, q3, q4, q5]
qd = [q1d, q2d, q3d, q4d, q5d]

# 计算动力学方程：d/dt(dL/dq̇) - dL/dq = τ
lagrange_eqs = []
for qi, qdi in zip(q, qd):
    dL_dqdi = sp.diff(L, qdi)
    ddt_dL_dqdi = sp.diff(dL_dqdi, dynamicsymbols._t)
    dL_dqi = sp.diff(L, qi)
    lagrange_eqs.append(sp.simplify(ddt_dL_dqdi - dL_dqi))

# ========== 5. 提取 M, C, G 矩阵 ==========
# 将方程线性化为 M(q)q̈ + C(q,q̇)q̇ + G(q) = τ
qdd = [dynamicsymbols(f'q{i}_dd') for i in range(1, 6)]
# 用符号替换二阶导数
subs_dict = {sp.diff(qdi, dynamicsymbols._t): qdd[i] for i, qdi in enumerate(qd)}
eqs_linear = [sp.expand(eq.subs(subs_dict)) for eq in lagrange_eqs]

# 提取质量矩阵 M（q̈ 的系数）
M = sp.zeros(5, 5)
for i in range(5):
    for j in range(5):
        M[i, j] = sp.simplify(sp.diff(eqs_linear[i], qdd[j]))

# 计算 C(q,q̇)q̇ + G(q) = τ - M q̈
# 将非M项分离
Cq_G = []
for i in range(5):
    expr = sp.expand(eqs_linear[i] - sum(M[i, j] * qdd[j] for j in range(5)))
    Cq_G.append(sp.simplify(expr))

# 提取重力项 G(q)（不含速度的项）
G_vec = sp.zeros(5, 1)
for i in range(5):
    G_vec[i] = sp.simplify(Cq_G[i].subs({q1d: 0, q2d: 0, q3d: 0, q4d: 0, q5d: 0}))

# 科氏力/离心力项 C(q,q̇)q̇ = Cq_G - G
Cq_vec = sp.zeros(5, 1)
for i in range(5):
    Cq_vec[i] = sp.simplify(Cq_G[i] - G_vec[i])

# ========== 6. 输出结果 ==========
print("=== 质量矩阵 M(q) ===")
print(M)
print("\n=== 重力项 G(q) ===")
print(G_vec)
print("\n=== 科氏力/离心力项 C(q,q̇)q̇ ===")
print(Cq_vec)

# ========== 7. 生成数值仿真函数 ==========
# 将符号表达式转换为数值函数（需指定具体参数）
# 示例参数（单位：kg, m, kg·m²）
params = {m1: 3.0, m2: 1.5, m3: 20.0, m4: 3.0, m5: 1.5,
          l1: 0.4, l2: 0.4, l3: 0.5, l4: 0.4, l5: 0.4,
          r1: 0.2, r2: 0.2, r3: 0.25, r4: 0.2, r5: 0.2,
          I1: 0.05, I2: 0.02, I3: 0.5, I4: 0.05, I5: 0.02,
          g: 9.81}

# 生成可调用的数值函数（示例：计算给定状态下的 M, C, G）
def compute_dynamics(q_vals, qd_vals):
    """输入：q_vals, qd_vals 为5维数组，输出：M, Cq, G"""
    subs = {q[i]: q_vals[i] for i in range(5)}
    subs.update({qd[i]: qd_vals[i] for i in range(5)})
    subs.update(params)
    M_num = np.array([[float(M[i, j].subs(subs)) for j in range(5)] for i in range(5)])
    Cq_num = np.array([float(Cq_vec[i].subs(subs)) for i in range(5)])
    G_num = np.array([float(G_vec[i].subs(subs)) for i in range(5)])
    return M_num, Cq_num, G_num

# 测试调用
q_test = np.array([0.1, -0.2, 0.05, 0.3, -0.4])   # 关节角（rad）
qd_test = np.array([0.0, 0.0, 0.0, 0.0, 0.0])     # 静止状态
M_num, Cq_num, G_num = compute_dynamics(q_test, qd_test)
print("\n=== 数值测试（静止状态） ===")
print("M 矩阵维度:", M_num.shape)
print("G 向量:", G_num)
print("Cq 向量（应为0）:", Cq_num)

运行与验证

  1. 依赖安装：pip install sympy numpy
  2. 运行方式：将上述代码保存为 biped_dynamics.py，执行 python biped_dynamics.py
  3. 验证方法：
    • 程序输出5×5质量矩阵 M，应是对称正定矩阵（对角元素为正）。
    • 静止状态（qd=0）下，科氏力项 Cq 应为零向量。
    • 重力项 G 在 q=0（直立姿态）时，各关节重力矩应满足静力平衡关系（支撑腿关节力矩不为零，摆动腿关节力矩为零）。
    • 可通过 compute_dynamics 函数在任意状态调用，检查输出维度与数值连续性。

模型使用说明

得到的 M(q), C(q,q̇), G(q) 可直接用于：

  • 前馈控制：计算期望力矩 τ = M(q)q̈_des + C(q,q̇)q̇ + G(q)
  • 仿真：正向动力学 q̈ = M⁻¹(τ - Cq̇ - G)，使用数值积分器（如RK4）递推状态。
  • 控制设计：计算动力学参数用于LQR、计算力矩控制或步态优化。

本模型忽略脚掌与地面弹性、关节摩擦和电机动力学，适用于单支撑相步态规划与控制验证。如需双支撑相，需额外引入接触约束。

← 上一章 📖 返回目录 下一章 →