05综合实战与仿真.md

1. 实战练习：UR5 机器人 DH 参数表

UR5 是工业界最经典的 6 自由度协作机器人。以下是其标准 DH 参数（注意：不同厂商或文献可能使用改进型 DH，即 MDH，这里展示标准 DH 以符合本文档 2 的定义）。

单位： $d, a$ (毫米 mm), $\alpha, \theta$ (弧度 rad)

关节 Joint (i)	αi (Twist)	ai (Link Length)	di (Link Offset)	θi (Range)
1 (Base)	$\pi/2$	0	89.16	$q_1$
2 (Shoulder)	0	-425.00	0	$q_2$
3 (Elbow)	0	-392.25	0	$q_3$
4 (Wrist 1)	$\pi/2$	0	109.15	$q_4$
5 (Wrist 2)	$-\pi/2$	0	94.65	$q_5$
6 (Wrist 3)	0	0	82.30	$q_6$

练习题：请尝试使用文档 2 提供的 forward_kinematics 代码，输入上述参数，计算 UR5 处于零位 ($q=[0,0,0,0,0,0]$) 时的末端坐标。

2. 动力学仿真：单摆的相图与时域响应

我们在文档 3 中推导出了单摆的动力学方程。现在，我们使用 Python 的科学计算库 scipy 对其进行数值积分，模拟真实的物理运动。

微分方程形式：

我们将二阶微分方程 $\ddot{\theta} = -\frac{g}{L}\sin(\theta) - \frac{b}{m L^2}\dot{\theta}$ (加入阻尼项 $b$) 转换为一阶状态空间方程组：

令状态向量 $y = [\theta, \omega]^T$，则：

$$\frac{dy}{dt} = \begin{bmatrix} \dot{\theta} \ \dot{\omega} \end{bmatrix} = \begin{bmatrix} \omega \ -\frac{g}{L}\sin(\theta) - \frac{b}{m L^2}\omega \end{bmatrix}$$

3. Python 仿真代码 (scipy.integrate.odeint)

import numpy as np
from scipy.integrate import odeint
import matplotlib.pyplot as plt

# 1. 定义系统动力学方程 (导数函数)
def pendulum_dynamics(y, t, g, L, m, b):
    """
    y: 状态向量 [theta, omega]
    t: 时间点
    g, L, m, b: 物理参数
    """
    theta, omega = y
    
    # 状态方程:
    # d(theta)/dt = omega
    # d(omega)/dt = -(g/L)sin(theta) - (b/(mL^2))omega
    dydt = [omega, -(g/L)*np.sin(theta) - (b/(m*L**2))*omega]
    
    return dydt

# 2. 设置参数
g = 9.81   # 重力加速度
L = 1.0    # 杆长
m = 1.0    # 质量
b = 0.5    # 阻尼系数 (空气阻力等)

# 3. 初始条件
theta_0 = np.radians(179) # 初始角度 (接近顶点，测试非线性)
omega_0 = 0.0             # 初始角速度
y0 = [theta_0, omega_0]

# 4. 时间跨度
t = np.linspace(0, 10, 200) # 模拟 10 秒

# 5. 解微分方程
# odeint 自动使用数值积分方法求解
solution = odeint(pendulum_dynamics, y0, t, args=(g, L, m, b))

# 6. 绘图结果
theta_traj = solution[:, 0]
omega_traj = solution[:, 1]

plt.figure(figsize=(10, 6))

# 子图 1: 角度随时间变化
plt.subplot(2, 1, 1)
plt.plot(t, theta_traj, 'b-', label='Theta (rad)')
plt.title('Pendulum Dynamics Simulation (Damped)')
plt.ylabel('Angle (rad)')
plt.grid(True)
plt.legend()

# 子图 2: 相图 (Phase Portrait) - 速度 vs 角度
plt.subplot(2, 1, 2)
plt.plot(theta_traj, omega_traj, 'r-')
plt.title('Phase Portrait (State Space)')
plt.xlabel('Angle (rad)')
plt.ylabel('Angular Velocity (rad/s)')
plt.grid(True)
plt.tight_layout()

print("仿真完成，请查看绘图窗口。")
# 实际运行时请确保环境支持 plt.show()
# plt.show()

4. 结果分析

• 时域图 (上图): 你会看到单摆从高处落下，震荡几次后，由于阻尼 $b$ 的存在，振幅逐渐减小，最终趋于 0（垂直向下）。
• 相图 (下图): 展示了 $\theta$ 和 $\omega$ 的轨迹。它应该是一个向原点收缩的螺旋线，这直观地展示了系统的稳定性。