正运动学求解：从DH参数到工作空间分析

正运动学，说白了就是：给定关节角度，算出末端位姿。这是机器人学里最基础、也最绕不开的一块。我刚开始学的时候，总觉得公式推导太枯燥，直到第一次在真机上看到末端执行器按我算出的轨迹走起来——那种感觉，嗯，确实不一样。

DH参数：机器人建模的通用语言

DH参数法（Denavit-Hartenberg）是描述相邻连杆之间位姿关系的标准方法。每个关节对应一组四个参数：

正运动学推导：一步步来

有了DH参数，相邻连杆的变换矩阵就是标准公式：

T = [cosθ    -sinθ·cosα    sinθ·sinα    a·cosθ
     sinθ     cosθ·cosα   -cosθ·sinα    a·sinθ
       0        sinα          cosα         d
       0          0            0           1   ]

把每个关节的T矩阵连乘，就得到末端相对于基座的位姿：

T₀ⁿ = T₀¹ · T₁² · ... · T_n-1ⁿ

举个六轴工业机器人的例子。我常用的一个模型是：

• 关节1：绕基座旋转（θ₁）
• 关节2：肩关节俯仰（θ₂）
• 关节3：肘关节俯仰（θ₃）
• 关节4：前臂旋转（θ₄）
• 关节5：腕关节俯仰（θ₅）
• 关节6：末端旋转（θ₆）

你想想看，六个矩阵连乘，符号展开后能有几十项。手算？不现实。所以我们用计算机代劳。

MATLAB仿真实现

我个人偏爱用MATLAB的Robotics Toolbox，它把DH建模封装得很干净：

% 定义六轴机器人DH参数（标准DH）
L(1) = Link([0    0      0.3    0      0], 'standard');
L(2) = Link([0    0.15   0      0.5    0], 'standard');
L(3) = Link([0    0      0      0.4    0], 'standard');
L(4) = Link([0    0.4    0      0      0], 'standard');
L(5) = Link([0    0      0      0.3    0], 'standard');
L(6) = Link([0    0      0      0      0], 'standard');

% 创建机器人对象
robot = SerialLink(L, 'name', '六轴机械臂');

% 给定关节角度（单位：弧度）
q = [0.5, -0.3, 0.8, 0, 0.2, 0];

% 正运动学求解
T = robot.fkine(q);

% 显示末端位姿
disp('末端位姿矩阵：');
disp(T);

Python仿真实现

Python这边，我推荐用roboticstoolbox-python库。语法和MATLAB版本很像，但更灵活：

import roboticstoolbox as rtb
import numpy as np

# 定义DH参数（修正DH）
robot = rtb.DHRobot([
    rtb.RevoluteDH(d=0.3, a=0, alpha=0),
    rtb.RevoluteDH(d=0, a=0.15, alpha=np.pi/2),
    rtb.RevoluteDH(d=0, a=0.4, alpha=0),
    rtb.RevoluteDH(d=0.4, a=0, alpha=-np.pi/2),
    rtb.RevoluteDH(d=0, a=0, alpha=np.pi/2),
    rtb.RevoluteDH(d=0, a=0, alpha=-np.pi/2)
], name='六轴机械臂')

# 给定关节角度
q = [0.5, -0.3, 0.8, 0, 0.2, 0]

# 正运动学求解
T = robot.fkine(q)

print("末端位姿矩阵：")
print(T)

工作空间分析

工作空间，就是机器人末端能到达的所有点的集合。分析它，说白了就是回答一个问题：我的机器人能碰到那个位置吗？

常用的方法是蒙特卡洛法——随机采样关节角度，计算末端位置，然后画点云。我习惯这样做：

import numpy as np
import matplotlib.pyplot as plt

# 随机采样10000组关节角度
num_samples = 10000
q_samples = np.random.uniform(
    low=[-np.pi, -np.pi/2, -np.pi, -np.pi, -np.pi/2, -np.pi],
    high=[np.pi, np.pi/2, np.pi, np.pi, np.pi/2, np.pi],
    size=(num_samples, 6)
)

# 计算末端位置
positions = []
for q in q_samples:
    T = robot.fkine(q)
    positions.append(T.t)  # 提取平移向量

positions = np.array(positions)

# 绘制工作空间（3D散点图）
fig = plt.figure(figsize=(10, 8))
ax = fig.add_subplot(111, projection='3d')
ax.scatter(positions[:,0], positions[:,1], positions[:,2], 
           s=1, c='blue', alpha=0.3)
ax.set_xlabel('X (m)')
ax.set_ylabel('Y (m)')
ax.set_zlabel('Z (m)')
ax.set_title('六轴机械臂工作空间（蒙特卡洛法）')
plt.show()

为什么会用蒙特卡洛法？因为解析法求工作空间边界太复杂了，尤其是六轴机器人。随机采样虽然粗糙，但足够工程使用。

核心逻辑：一张图说清楚

下面这张图概括了正运动学求解的完整流程：

工作空间的工程意义

工作空间分析不是纸上谈兵。我在做产线布局时，经常用它来回答三个问题：

1. 可达性：工件放在这个位置，机器人能碰到吗？
2. 灵巧性：在某个区域，机器人有多少种姿态可以到达？
3. 奇异性：工作空间内部有没有“死点”？

举个例子。有一次我在调试一个焊接机器人，发现末端在某个位置总是抖动。后来一查工作空间，那个点正好在奇异位形附近——关节速度瞬间变得极大。嗯，从那以后，我每次做轨迹规划前，都会先跑一遍工作空间分析。