4、正运动学推导：基于DH参数的齐次变换矩阵连乘、正解函数实现

好，咱们进入正题。前面我们把DH参数表给撸出来了，六个关节，四列参数，整整齐齐。但光有参数表没用，得让它动起来。怎么动？靠的就是齐次变换矩阵连乘。

说白了，正运动学就是回答一个问题：你给我六个关节角度，我告诉你末端在哪儿、朝哪儿。这事儿在机器人学里属于“基本功”，但基本功不意味着简单。我当年刚入行时，就因为连乘顺序搞反了，调试时末端直接飞到天花板上去……嗯，从那以后我每次写正解都会先画个草图。

4.1 从DH参数到齐次变换矩阵

每个关节i，对应一个4x4的齐次变换矩阵 T_i。这个矩阵描述了从关节i的坐标系到关节i-1的坐标系的变换。公式长这样：

T_i = Rot(z, θ_i) * Trans(z, d_i) * Trans(x, a_i) * Rot(x, α_i)

展开成矩阵形式：

| cosθ_i   -sinθ_i*cosα_i    sinθ_i*sinα_i    a_i*cosθ_i |
| sinθ_i    cosθ_i*cosα_i   -cosθ_i*sinα_i    a_i*sinθ_i |
|    0          sinα_i           cosα_i           d_i     |
|    0            0                0               1      |

这个矩阵，我建议你直接背下来。不是死记硬背，而是理解它的结构：左上角3x3是旋转，右上角3x1是平移，最后一行是齐次坐标的“尾巴”。

4.2 连乘顺序：从基座到末端

有了每个关节的变换矩阵，接下来就是连乘。顺序是：

T_06 = T_01 * T_12 * T_23 * T_34 * T_45 * T_56

这里 T_01 是关节1相对于基座的变换，T_12 是关节2相对于关节1的变换，以此类推。最终 T_06 就是末端坐标系相对于基座坐标系的齐次变换矩阵。

为什么会是这个顺序？你想想看，机械臂从基座开始，先动关节1，再动关节2……每一步变换都是在前一步的基础上进行的。所以矩阵乘法必须从左到右，不能反过来。

4.3 正解函数实现

好了，理论讲完，上代码。我用Python写一个正解函数，输入是六个关节角度（弧度制），输出是末端位姿（位置+姿态）。

import numpy as np

def dh_transform(theta, d, a, alpha):
    """
    根据DH参数计算单个关节的齐次变换矩阵
    """
    ct = np.cos(theta)
    st = np.sin(theta)
    ca = np.cos(alpha)
    sa = np.sin(alpha)
    
    T = np.array([
        [ct, -st*ca,  st*sa, a*ct],
        [st,  ct*ca, -ct*sa, a*st],
        [0,      sa,     ca,    d],
        [0,       0,      0,    1]
    ])
    return T

def forward_kinematics(joint_angles, dh_params):
    """
    正运动学求解
    :param joint_angles: 6个关节角度，单位弧度
    :param dh_params: DH参数表，每行 [theta, d, a, alpha]
    :return: 末端位姿矩阵 T_06
    """
    T = np.eye(4)  # 初始化为单位矩阵
    for i in range(6):
        # 注意：DH参数表中的theta是变量，需要加上当前关节角度
        theta_i = dh_params[i, 0] + joint_angles[i]
        d_i = dh_params[i, 1]
        a_i = dh_params[i, 2]
        alpha_i = dh_params[i, 3]
        
        T_i = dh_transform(theta_i, d_i, a_i, alpha_i)
        T = T @ T_i  # 左乘，注意顺序
    
    return T

这段代码里，我用了 @ 运算符做矩阵乘法，Python 3.5+ 都支持。注意看循环里的 T = T @ T_i，这是从左到右连乘的关键。

4.4 验证：拿个简单例子试试

写完了函数，得验证一下对不对。我一般会用两个方法：

1. 零位验证： 所有关节角度为0时，末端应该在什么位置？根据机械臂的几何结构，你应该能大概估算出来。
2. 单关节运动： 只动一个关节，其他关节锁死，看末端轨迹是不是一个圆弧。

举个例子，假设我们的六轴臂在零位时，末端位置是 [x0, y0, z0]。如果只转动关节1（腰部旋转），末端应该在水平面上画圆。如果代码算出来z坐标变了，那肯定有问题。

4.5 从矩阵中提取位置和姿态

正解算出来的是4x4矩阵，但实际应用中我们更关心位置和姿态。提取方法很简单：

def extract_pose(T):
    """
    从齐次变换矩阵中提取位置和姿态
    :param T: 4x4齐次变换矩阵
    :return: position (3,), rotation_matrix (3,3)
    """
    position = T[:3, 3]  # 平移向量
    rotation_matrix = T[:3, :3]  # 旋转矩阵
    return position, rotation_matrix

位置就是矩阵的第四列前三行，姿态就是左上角3x3旋转矩阵。如果你需要欧拉角或者四元数，可以进一步转换，但那是后话了。

4.6 本章知识体系

我把这一章的核心逻辑画成了流程图，方便你理解整体脉络：

这张图把整个流程串起来了。你从DH参数表出发，每个关节生成一个变换矩阵，然后按顺序连乘，最后提取位姿。每一步都有对应的代码实现，环环相扣。

嗯，正运动学这部分其实不难，关键是细心。矩阵别写错，顺序别搞反，参数别漏掉。我当年第一次跑通正解时，看着末端在仿真里画出正确的轨迹，那种感觉还是挺爽的。

---