📚 课程目录第1章双足机器人运动学建模

1. 双足机器人运动学建模

任务目标

为双足机器人建立平面运动学模型：给定机器人髋关节位置及两条腿各关节角度，计算足端位置（正运动学）；给定期望足端位置，求解各关节角度（逆运动学）。该模型是后续步态规划中足端轨迹跟踪的基础。

模型定义

采用平面五连杆模型（髋关节为公共顶点，两条腿各含髋、膝、踝三个旋转关节）。每条腿的几何参数相同：大腿长度 L1、小腿长度 L2。关节角定义：θ1 为髋关节角（大腿相对竖直轴），θ2 为膝关节角（小腿相对大腿延长线），θ3 为踝关节角（足底相对小腿延长线）。所有角度以逆时针为正，竖直向上为0°基准。

正运动学实现

给定髋关节世界坐标 (x_h, y_h) 及某条腿的三个关节角，依次通过旋转和平移计算踝关节（足端）位置。核心递推关系：每段杆件末端位置 = 上一节点位置 + 杆长 × 方向向量，方向向量由累积旋转角决定。

逆运动学实现

给定期望足端位置 (x_f, y_f)，先由足端相对髋关节的极坐标求出髋关节到踝关节的距离 d，再通过余弦定理求解膝关节角 θ2（取屈膝为正的解），进而求出髋关节角 θ1，最后根据足底期望姿态角 φ 确定踝关节角 θ3 = φ − θ1 − θ2。

完整代码（Python）

import numpy as np

L1 = 0.4   # 大腿长度 (m)
L2 = 0.4   # 小腿长度 (m)

def forward_kinematics(x_h, y_h, theta1, theta2, theta3):
    """
    正运动学：由髋关节位置和关节角计算足端位置
    返回: (x_f, y_f) 足端世界坐标
    """
    # 累积旋转角（相对竖直轴）
    alpha1 = theta1
    alpha2 = theta1 + theta2
    alpha3 = theta1 + theta2 + theta3

    # 髋 -> 膝
    x_k = x_h + L1 * np.sin(alpha1)
    y_k = y_h - L1 * np.cos(alpha1)

    # 膝 -> 踝
    x_a = x_k + L2 * np.sin(alpha2)
    y_a = y_k - L2 * np.cos(alpha2)

    # 踝关节即足端（忽略足部厚度）
    x_f = x_a
    y_f = y_a
    return x_f, y_f

def inverse_kinematics(x_h, y_h, x_f, y_f, phi=0.0):
    """
    逆运动学：由期望足端位置和足底姿态角求解关节角
    返回: (theta1, theta2, theta3) 单位: 弧度
    """
    dx = x_f - x_h
    dy = y_f - y_h
    d = np.sqrt(dx**2 + dy**2)

    # 检查可达性
    if d > L1 + L2 or d < abs(L1 - L2):
        raise ValueError("目标位置超出工作空间")

    # 膝关节角（余弦定理，取屈膝正解）
    cos_theta2 = (L1**2 + L2**2 - d**2) / (2 * L1 * L2)
    theta2 = np.arccos(np.clip(cos_theta2, -1.0, 1.0))

    # 髋关节角
    alpha = np.arctan2(dx, dy)  # 髋到踝连线与竖直轴夹角
    beta = np.arccos(np.clip((L1**2 + d**2 - L2**2) / (2 * L1 * d), -1.0, 1.0))
    theta1 = alpha - beta

    # 踝关节角（保证足底姿态角为phi）
    theta3 = phi - theta1 - theta2
    return theta1, theta2, theta3

# ===== 验证示例 =====
if __name__ == "__main__":
    # 给定髋关节位置和一组关节角
    x_h, y_h = 0.0, 0.9
    th1, th2, th3 = np.radians([10, 30, -20])

    # 正运动学
    x_f, y_f = forward_kinematics(x_h, y_h, th1, th2, th3)
    print(f"正运动学足端位置: ({x_f:.4f}, {y_f:.4f})")

    # 逆运动学（用正运动学结果反算）
    th1_ik, th2_ik, th3_ik = inverse_kinematics(x_h, y_h, x_f, y_f, phi=th1+th2+th3)
    print(f"逆运动学关节角: ({np.degrees(th1_ik):.2f}, {np.degrees(th2_ik):.2f}, {np.degrees(th3_ik):.2f}) 度")

    # 验证：逆运动学结果代入正运动学应还原足端位置
    x_f_check, y_f_check = forward_kinematics(x_h, y_h, th1_ik, th2_ik, th3_ik)
    err = np.sqrt((x_f_check - x_f)**2 + (y_f_check - y_f)**2)
    print(f"正逆运动学互验误差: {err:.2e} m")
    assert err < 1e-10, "正逆运动学不一致"

运行与验证

运行方式：将代码保存为 kinematics.py，在终端执行 python kinematics.py（需安装 NumPy）。
预期输出：程序打印正运动学足端坐标、逆运动学反算的关节角，以及互验误差。由于代码逻辑确定，预期误差为 0（浮点精度内），且逆运动学角度与初始角度一致（本例中 phi 取正运动学的累积角，保证唯一解）。
通过条件：互验误差小于 1e-10，且程序无异常退出。若修改输入角度，正逆运动学互验仍应满足该精度。

关键说明

逆运动学中 phi 参数表示期望的足底姿态角（相对竖直轴）。步态规划中通常令 phi=0 保持足底水平，此时踝关节角自动补偿髋、膝关节的累积旋转。
本模型忽略足部厚度，将踝关节位置视为足端接触点。实际机器人若足部有结构高度，需在踝关节坐标基础上增加固定偏移。
工作空间为以髋关节为圆心、半径 [|L1−L2|, L1+L2] 的圆环区域。超出该范围时函数抛出 ValueError。

上图展示了双足机器人的平面五连杆模型。髋关节为两腿公共顶点，每条腿由大腿（L₁）和小腿（L₂）组成。正运动学从髋关节位置和关节角 (θ₁, θ₂, θ₃) 递推计算足端坐标；逆运动学则从期望足端位置反解关节角。该模型是步态规划中足端轨迹跟踪与稳定性控制的基础。