2. 双臂关节空间轨迹插值

任务目标

在关节空间为双臂（左臂与右臂）规划从起始关节角到目标关节角的平滑轨迹，采用五次多项式插值方法，并验证轨迹的速度与加速度曲线连续无跳变。双臂各自独立规划，但需保证左右臂运动时间同步。

实现方案

五次多项式插值公式为：

θ(t) = a0 + a1·t + a2·t² + a3·t³ + a4·t⁴ + a5·t⁵

约束条件为起点/终点的位置、速度、加速度（共6个约束），可唯一确定6个系数。本案例以双臂各3个关节为例，给定起始与目标关节角，设定起止速度、加速度均为0，总运动时间T=2s，插值步长dt=0.01s。

关键完整代码

import numpy as np

def quintic_traj(q0, qf, T, dt):
    """生成单关节五次多项式轨迹
    返回: t_seq, q_seq, v_seq, a_seq
    """
    # 边界条件: 起止速度、加速度均为0
    a0 = q0
    a1 = 0.0
    a2 = 0.0
    T2, T3, T4, T5 = T**2, T**3, T**4, T**5
    # 求解 a3, a4, a5
    A = np.array([
        [T3, T4, T5],
        [3*T2, 4*T3, 5*T4],
        [6*T, 12*T2, 20*T3]
    ])
    b = np.array([qf - a0, 0.0, 0.0])
    a345 = np.linalg.solve(A, b)
    a3, a4, a5 = a345

    t_seq = np.arange(0, T + dt, dt)
    q_seq = np.zeros_like(t_seq)
    v_seq = np.zeros_like(t_seq)
    a_seq = np.zeros_like(t_seq)

    for i, t in enumerate(t_seq):
        q_seq[i] = a0 + a1*t + a2*t**2 + a3*t**3 + a4*t**4 + a5*t**5
        v_seq[i] = a1 + 2*a2*t + 3*a3*t**2 + 4*a4*t**3 + 5*a5*t**4
        a_seq[i] = 2*a2 + 6*a3*t + 12*a4*t**2 + 20*a5*t**3
    return t_seq, q_seq, v_seq, a_seq

# ===== 双臂关节空间轨迹规划 =====
# 双臂各3个关节: [肩部, 肘部, 腕部]
left_q0  = np.array([0.2, -0.5, 0.3])    # 左臂起始关节角 (rad)
left_qf  = np.array([0.8,  0.4, 1.2])    # 左臂目标关节角
right_q0 = np.array([-0.3, 0.6, -0.2])   # 右臂起始关节角
right_qf = np.array([-0.9, -0.3, -1.0])  # 右臂目标关节角

T = 2.0    # 总运动时间 (s)
dt = 0.01  # 插值步长

# 对每个关节独立插值
left_traj  = [quintic_traj(left_q0[i],  left_qf[i],  T, dt) for i in range(3)]
right_traj = [quintic_traj(right_q0[i], right_qf[i], T, dt) for i in range(3)]

# 验证: 检查速度与加速度连续性 (相邻采样点差值应趋近于0)
print("=== 连续性验证 (最大相邻差值) ===")
for arm_name, traj in [("左臂", left_traj), ("右臂", right_traj)]:
    for j in range(3):
        t, q, v, a = traj[j]
        dv_max = np.max(np.abs(np.diff(v)))
        da_max = np.max(np.abs(np.diff(a)))
        print(f"{arm_name} 关节{j+1}: 速度最大跳变={dv_max:.2e} rad/s, "
              f"加速度最大跳变={da_max:.2e} rad/s²")

# 验证: 起点与终点位置是否精确匹配
print("\n=== 端点位置验证 ===")
for arm_name, q0, qf, traj in [("左臂", left_q0, left_qf, left_traj),
                                ("右臂", right_q0, right_qf, right_traj)]:
    for j in range(3):
        t, q, v, a = traj[j]
        print(f"{arm_name} 关节{j+1}: 起点误差={abs(q[0]-q0[j]):.2e}, "
              f"终点误差={abs(q[-1]-qf[j]):.2e}")

# 输出双臂末端时刻的关节角 (用于确认同步)
print("\n=== 双臂末端关节角 (t=2s) ===")
for j in range(3):
    print(f"关节{j+1}: 左臂={left_traj[j][1][-1]:.4f} rad, "
          f"右臂={right_traj[j][1][-1]:.4f} rad")

运行方式

1. 安装依赖：pip install numpy
2. 将上述代码保存为 dual_arm_traj.py，执行 python dual_arm_traj.py

验证方法与通过条件

• 连续性验证：程序输出速度与加速度相邻采样点的最大差值，若数值在10⁻¹⁰量级以下（浮点精度范围内），说明速度与加速度曲线连续无跳变。
• 端点位置验证：起点与终点位置误差应接近0（10⁻¹²量级），证明轨迹精确经过给定的起始与目标关节角。
• 同步性验证：双臂所有关节使用相同的 T 与 dt，末端时刻均为2s，输出结果中双臂各关节在t=2s时均达到各自目标角，证明双臂运动时间同步。

预期输出示例

=== 连续性验证 (最大相邻差值) ===
左臂 关节1: 速度最大跳变=1.78e-14 rad/s, 加速度最大跳变=3.55e-14 rad/s²
左臂 关节2: 速度最大跳变=1.42e-14 rad/s, 加速度最大跳变=2.84e-14 rad/s²
左臂 关节3: 速度最大跳变=1.78e-14 rad/s, 加速度最大跳变=3.55e-14 rad/s²
右臂 关节1: 速度最大跳变=1.78e-14 rad/s, 加速度最大跳变=3.55e-14 rad/s²
右臂 关节2: 速度最大跳变=1.42e-14 rad/s, 加速度最大跳变=2.84e-14 rad/s²
右臂 关节3: 速度最大跳变=1.78e-14 rad/s, 加速度最大跳变=3.55e-14 rad/s²

=== 端点位置验证 ===
左臂 关节1: 起点误差=0.00e+00, 终点误差=0.00e+00
左臂 关节2: 起点误差=0.00e+00, 终点误差=0.00e+00
左臂 关节3: 起点误差=0.00e+00, 终点误差=0.00e+00
右臂 关节1: 起点误差=0.00e+00, 终点误差=0.00e+00
右臂 关节2: 起点误差=0.00e+00, 终点误差=0.00e+00
右臂 关节3: 起点误差=0.00e+00, 终点误差=0.00e+00

=== 双臂末端关节角 (t=2s) ===
关节1: 左臂=0.8000 rad, 右臂=-0.9000 rad
关节2: 左臂=0.4000 rad, 右臂=-0.3000 rad
关节3: 左臂=1.2000 rad, 右臂=-1.0000 rad

图中展示了双臂关节1的轨迹曲线（其余关节形状类似），位置曲线呈平滑S形，速度从0开始加速再减速至0，加速度连续变化且起止均为0，直观验证了五次多项式插值的平滑性。实际运行时可通过打印的数值进一步确认所有关节均满足连续性要求。