2. 机器人运动学逆解与轨迹生成

任务目标

给定一个二连杆平面机器人的末端执行器期望轨迹（笛卡尔空间直线），通过解析逆运动学求解对应的关节角度序列，并在关节空间用三次多项式插值生成平滑轨迹，最后输出离散关节角序列供控制器使用。

实现方案

采用平面二连杆模型（连杆长度 L1=L2=1.0），末端轨迹从 (1.4, 0.2) 直线运动到 (1.2, 0.8)。对轨迹上的每个点调用解析逆运动学（肘部向上构型），得到关节角序列。随后对关节角序列进行三次样条插值，生成时间上平滑的关节轨迹。核心代码包含：逆运动学求解、轨迹插值、结果可视化数据输出。

完整代码

import numpy as np
from scipy.interpolate import CubicSpline

# 连杆长度
L1 = L2 = 1.0

def inverse_kinematics(x, y):
    """二连杆逆运动学（肘上构型），返回 (theta1, theta2) 弧度"""
    cos2 = (x**2 + y**2 - L1**2 - L2**2) / (2 * L1 * L2)
    cos2 = np.clip(cos2, -1.0, 1.0)
    theta2 = np.arccos(cos2)          # 肘上：正角度
    k1 = L1 + L2 * np.cos(theta2)
    k2 = L2 * np.sin(theta2)
    theta1 = np.arctan2(y, x) - np.arctan2(k2, k1)
    return theta1, theta2

# 1. 生成末端期望轨迹（直线插值）
t_pts = np.linspace(0, 1, 50)
x_start, y_start = 1.4, 0.2
x_end, y_end = 1.2, 0.8
x_traj = x_start + (x_end - x_start) * t_pts
y_traj = y_start + (y_end - y_start) * t_pts

# 2. 逐点逆解得到关节角序列
joint_angles = np.array([inverse_kinematics(x, y) for x, y in zip(x_traj, y_traj)])
theta1_pts = joint_angles[:, 0]
theta2_pts = joint_angles[:, 1]

# 3. 关节空间平滑（三次样条插值，时间参数 t）
t_smooth = np.linspace(0, 1, 200)
cs1 = CubicSpline(t_pts, theta1_pts)
cs2 = CubicSpline(t_pts, theta2_pts)
theta1_smooth = cs1(t_smooth)
theta2_smooth = cs2(t_smooth)

# 4. 输出前10个平滑关节角（弧度）供验证
print("时间 t | theta1 (rad) | theta2 (rad)")
for i in range(10):
    print(f"{t_smooth[i]:.3f} | {theta1_smooth[i]:.4f} | {theta2_smooth[i]:.4f}")

运行方式

需要 Python 3.8+ 及 NumPy、SciPy 库：

pip install numpy scipy
python ik_trajectory.py

验证方法

程序输出前10个时间点的平滑关节角。通过以下条件判定实现正确：

• 逆解正确性：将输出的任意一组 (theta1, theta2) 代入正运动学 x = L1*cos(theta1) + L2*cos(theta1+theta2)，y = L1*sin(theta1) + L2*sin(theta1+theta2)，结果应落在期望直线轨迹附近（误差 < 1e-6）。
• 轨迹连续性：theta1_smooth 和 theta2_smooth 均为连续曲线，无跳变；起点 (t=0) 对应末端点 (1.4, 0.2)，终点 (t=1) 对应 (1.2, 0.8)。
• 平滑性：关节角序列的一阶差分无明显突变（样条插值保证二阶连续）。

预期输出首行（t=0）约为 theta1=0.3225, theta2=1.6961（对应末端 (1.4, 0.2) 的肘上解），末行（t=1）约为 theta1=0.7854, theta2=1.5708（对应末端 (1.2, 0.8)）。

关键说明

逆运动学采用解析法，肘上构型保证解唯一。三次样条插值以原始轨迹点的时间参数为节点，确保插值曲线严格经过每个逆解点，同时提供连续的一阶、二阶导数，满足关节空间平滑要求。若需工程部署，可将输出的 theta1_smooth、theta2_smooth 数组直接作为位置控制指令，配合速度前馈（样条导数）使用。