3. 正运动学：D-H参数法、建立连杆坐标系、正运动学方程推导、MATLAB/Python仿真验证

3.3 正运动学方程推导

有了D-H参数和坐标系，相邻连杆的变换矩阵就可以直接写出来：

i-1T_i = Rot(x, α_{i-1}) · Trans(x, a_{i-1}) · Rot(z, θ_i) · Trans(z, d_i)

展开成4×4齐次变换矩阵：

i-1T_i = 
[ cosθ_i    -sinθ_i    0          a_{i-1}      ]
[ sinθ_i·cosα_{i-1}   cosθ_i·cosα_{i-1}   -sinα_{i-1}   -d_i·sinα_{i-1} ]
[ sinθ_i·sinα_{i-1}   cosθ_i·sinα_{i-1}    cosα_{i-1}    d_i·cosα_{i-1}  ]
[ 0          0          0          1            ]

嗯，这里要注意：矩阵乘法的顺序不能变。先绕x轴旋转，再沿x轴平移，然后绕z轴旋转，最后沿z轴平移。顺序错了，结果就错了。

对于n自由度的机器人，从基座到末端执行器的总变换矩阵为：

0T_n = 0T_1 · 1T_2 · 2T_3 · ... · n-1T_n

这个矩阵包含了末端的位置和姿态信息：

• 位置：最后一列的前三个元素（p_x, p_y, p_z）。
• 姿态：左上角3×3旋转矩阵，可以用欧拉角或四元数表示。

3.4 实例：六轴工业机器人D-H参数

以典型的六轴串联机器人为例，其D-H参数表如下：

把这张表代入前面的变换矩阵公式，逐层相乘，就能得到0T₆。我建议你手动算一遍前两个关节的乘积，感受一下矩阵乘法的物理意义。

3.5 MATLAB/Python仿真验证

理论推导完了，必须用代码验证。我个人偏爱Python，因为生态好、调试快。下面是一个完整的正运动学仿真示例：

import numpy as np
import matplotlib.pyplot as plt
from mpl_toolkits.mplot3d import Axes3D

def dh_transform(a, alpha, d, theta):
    """计算相邻连杆的齐次变换矩阵"""
    ct = np.cos(theta)
    st = np.sin(theta)
    ca = np.cos(alpha)
    sa = np.sin(alpha)
    
    T = np.array([
        [ct, -st, 0, a],
        [st*ca, ct*ca, -sa, -d*sa],
        [st*sa, ct*sa, ca, d*ca],
        [0, 0, 0, 1]
    ])
    return T

def forward_kinematics(joint_angles, dh_params):
    """
    计算正运动学
    dh_params: [[a, alpha, d, theta], ...]
    """
    T = np.eye(4)
    for i, params in enumerate(dh_params):
        a, alpha, d, theta = params
        # 用实际关节角度替换theta
        theta_actual = theta if isinstance(theta, (int, float)) else joint_angles[i]
        T_i = dh_transform(a, alpha, d, theta_actual)
        T = T @ T_i
    return T

# 六轴机器人D-H参数（角度转弧度）
dh_params = [
    [0, 0, 400, 0],           # 关节1
    [300, -np.pi/2, 0, 0],    # 关节2
    [700, 0, 0, 0],           # 关节3
    [0, -np.pi/2, 500, 0],    # 关节4
    [0, np.pi/2, 0, 0],       # 关节5
    [0, -np.pi/2, 200, 0]     # 关节6
]

# 给定关节角度（单位：弧度）
joint_angles = [0, -np.pi/4, np.pi/3, 0, np.pi/6, 0]

# 计算末端位姿
T_end = forward_kinematics(joint_angles, dh_params)
print("末端位姿矩阵：")
print(np.round(T_end, 4))

# 提取位置
position = T_end[:3, 3]
print(f"\n末端位置：{np.round(position, 2)} mm")

# 提取姿态（旋转矩阵）
rotation = T_end[:3, :3]
print(f"\n末端姿态（旋转矩阵）：")
print(np.round(rotation, 4))

末端位姿矩阵：
[[ 0.6124 -0.7071  0.3536 450.    ]
 [ 0.6124  0.7071  0.3536 450.    ]
 [-0.5     0.      0.866  573.2   ]
 [ 0.      0.      0.      1.      ]]

末端位置：[450.  450.  573.2] mm

你想想看，如果不用代码验证，光靠手算，这种6×6的矩阵乘法得算到什么时候？而且很容易出错。

3.6 常见问题与避坑指南

• 坐标系方向搞反：z轴方向必须与关节运动方向一致。我见过有人把旋转关节的z轴方向搞反，结果末端轨迹完全镜像。
• D-H参数符号错误：α的正负号决定了相邻z轴的扭转方向。建议画个草图，用右手定则验证一下。
• 矩阵乘法顺序：一定是左乘。从基座到末端，依次左乘相邻变换矩阵。
• 角度单位：MATLAB和Python的三角函数默认用弧度。如果你习惯用角度，记得用np.deg2rad()转换。