项目三：2D/3D 机械臂运动学工具箱

1. 项目概述

构建一个完整的机械臂运动学工具箱，支持：

2. 难度：★★☆☆☆ (2/5)

3. 预估时间：2-3周

---

4. 1. 项目结构

arm_toolbox/
├── arm_kinematics/
│   ├── __init__.py
│   ├── dh_params.py        # DH参数库（UR5, Franka, KUKA）
│   ├── forward_kinematics.py # 正运动学
│   ├── inverse_kinematics.py # 逆运动学（多方法）
│   ├── jacobian.py          # 雅可比矩阵 & 速度映射
│   ├── trajectory.py        # 轨迹规划
│   ├── collision.py         # 基本碰撞检测
│   └── visualization.py     # 可视化
├── examples/
│   ├── 01_fk_demo.py
│   ├── 02_ik_demo.py
│   ├── 03_trajectory_demo.py
│   ├── 04_workspace_plot.py
│   └── 05_pick_and_place.py
├── tests/
│   ├── test_fk.py
│   ├── test_ik.py
│   └── test_jacobian.py
├── requirements.txt
└── README.md

---

5. 2. 核心代码

2.1 DH参数正运动学

"kw">class="kw">import numpy "kw">as np

"kw">class DHKinematics:
    """通用DH参数法正运动学"""

    "kw">def __init__("kw">self, dh_params):
        """
        dh_params: List of [a, alpha, d, theta_offset] per joint
        """
        "kw">self.dh_params = np.array(dh_params)
        "kw">self.n_joints = "kw">len(dh_params)

    "kw">def dh_transform("kw">self, a, alpha, d, theta):
        """单个DH变换矩阵"""
        ct, st = np.cos(theta), np.sin(theta)
        ca, sa = np.cos(alpha), np.sin(alpha)
        "kw">class="kw">return np.array([
            [ct, -st*ca,  st*sa, a*ct],
            [st,  ct*ca, -ct*sa, a*st],
            [0,   sa,     ca,    d],
            [0,   0,      0,     1]
        ])

    "kw">def forward("kw">self, joint_angles):
        """
        正运动学：关节角度 → 末端位姿
        返回：[T_01, T_02, ..., T_0n] 每个关节的齐次变换
        """
        T = np.eye(4)
        transforms = [T.copy()]

        "kw">for i, theta "kw">in enumerate(joint_angles):
            a, alpha, d, offset = "kw">self.dh_params[i]
            Ti = "kw">self.dh_transform(a, alpha, d, theta + offset)
            T = T @ Ti
            transforms.append(T.copy())

        "kw">class="kw">return transforms

    "kw">def get_ee_pose("kw">self, joint_angles):
        """获取末端执行器位姿"""
        transforms = "kw">self.forward(joint_angles)
        T_ee = transforms[-1]
        "kw">class="kw">return T_ee[:3, 3], T_ee[:3, :3]

    "kw">def get_all_joint_positions("kw">self, joint_angles):
        """获取所有关节位置（用于可视化）"""
        transforms = "kw">self.forward(joint_angles)
        "kw">class="kw">return [T[:3, 3] "kw">for T "kw">in transforms]

# ===== 预定义机器人 =====
UR5_DH = [
    [0,      np.pi/2,  0.089159, 0],
    [-0.425, 0,        0,        0],
    [-0.39225, 0,       0,        0],
    [0,      np.pi/2,  0.10915,  0],
    [0,     -np.pi/2,  0.09465,  0],
    [0,      0,        0.0823,   0],
]

FRANKA_DH = [
    [0,      0,        0.333,    0],
    [0,     -np.pi/2,  0,        0],
    [0,      np.pi/2,  0.316,    0],
    [0.0825, np.pi/2,  0,        0],
    [-0.0825,-np.pi/2, 0.384,    0],
    [0,      np.pi/2,  0,        0],
    [0.088,  np.pi/2,  0.107,    0],
]

# 使用
ur5 = DHKinematics(UR5_DH)
q = np.array([0.5, -0.3, 0.2, 0.1, -0.4, 0.3])
pos, rot = ur5.get_ee_pose(q)
"kw">print(f"UR5末端: {pos.round(4)}")

2.2 数值逆运动学

"kw">from scipy.spatial.transform "kw">class="kw">import Rotation "kw">as R

"kw">class NumericalIK:
    """数值逆运动学求解器"""

    "kw">def __init__("kw">self, robot: DHKinematics):
        "kw">self.robot = robot

    "kw">def compute_jacobian("kw">self, joint_angles):
        """计算几何雅可比 J = [Jv; Jω] (6×n)"""
        transforms = "kw">self.robot.forward(joint_angles)
        T_ee = transforms[-1]
        pe = T_ee[:3, 3]
        n = "kw">self.robot.n_joints

        J = np.zeros((6, n))
        "kw">for i "kw">in "kw">range(n):
            Ti = transforms[i]
            zi = Ti[:3, 2]
            pi = Ti[:3, 3]

            J[:3, i] = np.cross(zi, pe - pi)   # 平动雅可比
            J[3:, i] = zi                       # 转动雅可比

        "kw">class="kw">return J

    "kw">def pseudo_inverse_ik("kw">self, target_pos, target_rot="kw">None,
                          q_init="kw">None, max_iters=500, tol=1e-4):
        """雅可比伪逆法IK"""
        q = np.zeros("kw">self.robot.n_joints) "kw">if q_init "kw">is "kw">None "kw">else q_init.copy()

        "kw">for i "kw">in "kw">range(max_iters):
            pos_curr, rot_curr = "kw">self.robot.get_ee_pose(q)
            pos_err = target_pos - pos_curr

            "kw">if target_rot "kw">is "kw">not "kw">None:
                R_err = target_rot @ rot_curr.T
                rotvec_err = R.from_matrix(R_err).as_rotvec()
            "kw">else:
                rotvec_err = np.zeros(3)

            error = np.concatenate([pos_err, rotvec_err])

            "kw">if np.linalg.norm(error) < tol:
                "kw">class="kw">return q, "kw">True, i+1

            J = "kw">self.compute_jacobian(q)
            damping = 0.01 * (1 + np.exp(-i/20))  # 自适应阻尼
            J_pinv = J.T @ np.linalg.inv(
                J @ J.T + damping * np.eye(6)
            )

            dq = J_pinv @ error
            q += np.clip(dq, -0.5, 0.5)  # 限制每步最大变化

        "kw">class="kw">return q, "kw">False, max_iters

    "kw">def dls_ik("kw">self, target_pos, target_rot="kw">None,
               q_init="kw">None, max_iters=500, tol=1e-4, lambda_max=0.1):
        """Damped Least Squares IK（更稳定，适合奇异位姿附近）"""
        q = np.zeros("kw">self.robot.n_joints) "kw">if q_init "kw">is "kw">None "kw">else q_init.copy()

        "kw">for i "kw">in "kw">range(max_iters):
            pos_curr, rot_curr = "kw">self.robot.get_ee_pose(q)
            pos_err = target_pos - pos_curr

            "kw">if target_rot "kw">is "kw">not "kw">None:
                R_err = target_rot @ rot_curr.T
                rotvec_err = R.from_matrix(R_err).as_rotvec()
            "kw">else:
                rotvec_err = np.zeros(3)

            error = np.concatenate([pos_err, rotvec_err])

            "kw">if np.linalg.norm(error) < tol:
                "kw">class="kw">return q, "kw">True, i+1

            J = "kw">self.compute_jacobian(q)
            JtJ = J.T @ J

            # 自适应阻尼：误差大→阻尼小（激进），误差小→阻尼大（保守）
            e_norm = np.linalg.norm(error)
            lam = lambda_max * e_norm / (e_norm + 0.1)

            dq = np.linalg.solve(JtJ + lam * np.eye("kw">self.robot.n_joints),
                                 J.T @ error)
            q += dq

        "kw">class="kw">return q, "kw">False, max_iters

# ===== 测试 =====
ur5 = DHKinematics(UR5_DH)
ik = NumericalIK(ur5)

target = np.array([0.5, 0.2, 0.3])
q_sol, success, iters = ik.pseudo_inverse_ik(target)
"kw">print(f"IK {'成功' "kw">if success "kw">else '失败'} | 迭代次数: {iters}")
"kw">print(f"关节角: {q_sol.round(4)}")

pos_final, _ = ur5.get_ee_pose(q_sol)
"kw">print(f"目标: {target}")
"kw">print(f"到达: {pos_final.round(4)}")
"kw">print(f"误差: {np.linalg.norm(pos_final-target):.6f} m")

# 对比DLS
q_sol_dls, success_dls, iters_dls = ik.dls_ik(target)
"kw">print(f"\nDLS IK {&#x27;成功' "kw">if success_dls "kw">else '失败'} | 迭代次数: {iters_dls}")

2.3 轨迹规划

"kw">class TrajectoryPlanner:
    """轨迹规划器"""

    @staticmethod
    "kw">def cubic_polynomial(q0, qf, v0, vf, tf, dt=0.01):
        """三次多项式轨迹"""
        a0 = q0
        a1 = v0
        a2 = (3*(qf-q0)/tf - 2*v0 - vf) / tf
        a3 = (-2*(qf-q0)/tf + v0 + vf) / tf**2

        t = np.arange(0, tf + dt, dt)
        q = a0 + a1*t + a2*t**2 + a3*t**3
        qd = a1 + 2*a2*t + 3*a3*t**2
        qdd = 2*a2 + 6*a3*t
        "kw">class="kw">return t, q, qd, qdd

    @staticmethod
    "kw">def trapezoidal_velocity(q0, qf, v_max, a_max, dt=0.01):
        """梯形速度轨迹"""
        dq = abs(qf - q0)
        sign = np.sign(qf - q0)

        # 检查是否能达到最大速度
        t_acc = v_max / a_max
        d_acc = 0.5 * a_max * t_acc**2

        "kw">if 2 * d_acc > dq:
            # 三角形速度曲线（达不到v_max）
            t_acc = np.sqrt(dq / a_max)
            t_const = 0
            v_peak = a_max * t_acc
        "kw">else:
            t_const = (dq - 2*d_acc) / v_max
            v_peak = v_max

        t_total = 2*t_acc + t_const
        t = np.arange(0, t_total, dt)

        q = np.zeros_like(t)
        qd = np.zeros_like(t)

        "kw">for i, ti "kw">in enumerate(t):
            "kw">if ti < t_acc:
                qd[i] = a_max * ti
                q[i] = 0.5 * a_max * ti**2
            "kw">elif ti < t_acc + t_const:
                qd[i] = v_peak
                q[i] = d_acc + v_peak * (ti - t_acc)
            "kw">else:
                t_dec = ti - t_acc - t_const
                qd[i] = v_peak - a_max * t_dec
                q[i] = d_acc + v_peak*t_const + v_peak*t_dec - 0.5*a_max*t_dec**2

        "kw">class="kw">return t, sign*q + q0, sign*qd, np.zeros_like(t)

    @staticmethod
    "kw">def linear_cartesian(start_pose, end_pose, duration, dt=0.01):
        """笛卡尔空间直线轨迹"""
        t = np.arange(0, duration + dt, dt)
        n = "kw">len(t)

        # 位置线性插值
        pos = np.array([
            np.linspace(start_pose[0], end_pose[0], n),
            np.linspace(start_pose[1], end_pose[1], n),
            np.linspace(start_pose[2], end_pose[2], n),
        ]).T

        # 姿态SLERP插值
        "kw">from scipy.spatial.transform "kw">class="kw">import Rotation, Slerp
        key_times = [0, duration]
        key_rots = Rotation.from_matrix([start_pose[3:].reshape(3,3),
                                          end_pose[3:].reshape(3,3)])
        slerp = Slerp(key_times, key_rots)
        rots = slerp(t)

        "kw">class="kw">return t, pos, rots

2.4 可视化

"kw">class="kw">import matplotlib.pyplot "kw">as plt
"kw">from matplotlib.animation "kw">class="kw">import FuncAnimation

"kw">def visualize_arm(arm, joint_angles, ax="kw">None):
    """可视化机械臂姿态"""
    "kw">if ax "kw">is "kw">None:
        fig = plt.figure(figsize=(8, 6))
        ax = fig.add_subplot(111, projection='3d')

    positions = arm.get_all_joint_positions(joint_angles)

    # 绘制连杆
    "kw">for i "kw">in "kw">range("kw">len(positions) - 1):
        ax.plot3D([positions[i][0], positions[i+1][0]],
                  [positions[i][1], positions[i+1][1]],
                  [positions[i][2], positions[i+1][2]],
                  'b-', linewidth=3)

    # 绘制关节点
    xs, ys, zs = zip(*positions)
    ax.scatter(xs, ys, zs, c='red', s=50)

    # 绘制基座
    ax.scatter(*positions[0], c='black', s=100, marker='s')

    # 绘制末端
    ax.scatter(*positions[-1], c='green', s=100, marker='*')

    ax.set_xlabel('X (m)')
    ax.set_ylabel('Y (m)')
    ax.set_zlabel('Z (m)')
    ax.set_title(f'UR5 机械臂姿态')
    ax.set_xlim([-1, 1])
    ax.set_ylim([-1, 1])
    ax.set_zlim([0, 1.5])

    "kw">class="kw">return ax

"kw">def animate_trajectory(arm, joint_trajectory, interval=50):
    """动画化关节轨迹"""
    fig = plt.figure(figsize=(8, 6))
    ax = fig.add_subplot(111, projection='3d')

    "kw">def update(frame):
        ax.clear()
        visualize_arm(arm, joint_trajectory[frame], ax)
        ax.set_title(f'Frame {frame}/{"kw">len(joint_trajectory)}')

    ani = FuncAnimation(fig, update, frames="kw">len(joint_trajectory),
                        interval=interval, repeat="kw">True)
    "kw">class="kw">return ani

2.5 工作空间分析

"kw">def analyze_workspace(arm, n_samples=10000):
    """蒙特卡洛采样分析工作空间"""
    # 各关节角度范围
    joint_limits = [
        (-np.pi, np.pi),     # joint 1
        (-np.pi, np.pi),     # joint 2
        (-np.pi, np.pi),
        (-np.pi, np.pi),
        (-np.pi, np.pi),
        (-np.pi, np.pi),
    ]

    positions = []
    "kw">for _ "kw">in "kw">range(n_samples):
        q = np.array([np.random.uniform(lo, hi)
                      "kw">for lo, hi "kw">in joint_limits])
        pos, _ = arm.get_ee_pose(q)
        positions.append(pos)

    positions = np.array(positions)

    # 统计
    "kw">print(f"X范围: [{positions[:,0].min():.2f}, {positions[:,0].max():.2f}] m")
    "kw">print(f"Y范围: [{positions[:,1].min():.2f}, {positions[:,1].max():.2f}] m")
    "kw">print(f"Z范围: [{positions[:,2].min():.2f}, {positions[:,2].max():.2f}] m")
    "kw">print(f"最大可达距离: {np.max(np.linalg.norm(positions, axis=1)):.2f} m")

    "kw">class="kw">return positions

---

6. 3. 验收标准

7. 4. 扩展方向

8. 参考