机器人运动学：标准 DH 与改进 DH 参数对比与实现

在机器人运动学建模中，准确区分标准 D-H（SDH）与改进 D-H（MDH）参数至关重要。许多初学者容易混淆两者，导致教科书公式推导与现有代码库不一致。本文重点解析两者的建系差异、变换顺序及适用场景，并提供基于改进 D-H 参数的 MATLAB 几何雅可比矩阵计算示例。

改进 D-H 法建系规则

改进 D-H 参数将连杆坐标系 $i$ 建立在连杆 $i$ 的近端关节处（即靠近基座的一端）。具体步骤如下：

坐标系定义在连杆的前端关节，遵循以下原则：

1. $Z_i$ 轴沿关节 $i+1$ 的运动方向。
2. $X_i$ 轴沿 $Z_i$ 与 $Z_{i-1}$ 的公垂线方向。
3. 原点位于 $Z_i$ 与 $X_i$ 的交点。

标准 DH 与改进 DH 的核心区别

简单来说，一个连杆有远端和近端。SDH 将坐标系 $i$ 建在连杆 $i$ 的远端，而 MDH 建在近端。

1. 标号规则

连杆编号从基座开始为 0, 1, ..., $i$。关节编号对应连杆近端为 $i$，远端为 $i+1$。记住：连杆近端关节的标号和连杆标号是一致的。

2. 变换顺序不同

这是最容易出错的地方。

• SDH: 变换顺序为 $d \to \theta \to a \to \alpha$。
• MDH: 变换顺序为 $\alpha \to a \to \theta \to d$。 注意 MDH 的顺序正好与 SDH 相反，这直接影响齐次变换矩阵的构建逻辑。

3. 为何推荐使用 MDH

对于树形结构或闭链机构，SDH 方法可能产生歧义。因为 SDH 把连杆 $i$ 的坐标系放在远端，可能导致连杆 0 上同时出现两个坐标系（如图 3a）。MDH 将坐标系固定在每个连杆的近端，避免了重合问题，适应性更强。

常见误区与代码实现

部分经典教材使用 SDH，但在现代机器人工具箱（如 ROS 或 MATLAB Robotics System Toolbox）中，MDH 更为普遍。如果你直接套用旧教材公式到 MDH 代码中，会导致雅可比矩阵计算错误。

例如，在计算几何雅可比时，SDH 通常使用 $z_{i-1}$ 和 $p_{i-1}$，而 MDH 应使用 $z_i$ 和 $p_i$。这是因为坐标系的索引偏移了。

下面是基于改进 D-H 参数计算几何雅可比矩阵的 MATLAB 代码示例（以 Franka Panda 为例）：

function J_geo = geometric_jacobian_from_q_corrected(q)
    % 根据关节角度计算几何雅可比矩阵，匹配 MATLAB 工具箱的顺序
    % 输入：q - 7x1 关节角向量 (弧度)
    % 输出：J_geo - 6x7 几何雅可比矩阵
    
    % Franka Panda 的 DH 参数 (Modified DH)
    % 格式：[alpha, a, d, theta]
    MDH = [0, 0, 0.333, q(1); 
           -pi/2, 0, 0, q(2); 
           pi/2, 0, 0.316, q(3); 
           pi/2, 0.0825, 0, q(4); 
           -pi/2, -0.0825, 0.384, q(5); 
           pi/2, 0, 0, q(6); 
           pi/2, 0.088, 0.107, q(7)];
    
    n = 7;
    T = eye(4);
    T_all = cell(n, 1);
    p_all = zeros(3, n);
    z_all = zeros(3, n);
    
    % 正运动学累积
    for i = 1:n
        alpha = MDH(i, 1);
        a = MDH(i, 2);
        d = MDH(i, 3);
        theta = MDH(i, 4);
        
        % 改进 DH 变换矩阵 Ti
        Ti = [cos(theta), -sin(theta), 0, a;
              sin(theta)*cos(alpha), cos(theta)*cos(alpha), -sin(alpha), -d*sin(alpha);
              sin(theta)*sin(alpha), cos(theta)*sin(alpha), cos(alpha), d*cos(alpha);
              0, 0, 0, 1];
        
        T = T * Ti;
        T_all{i} = T;
        p_all(:, i) = T(1:3, 4);
        z_all(:, i) = T(1:3, 3);
    end
    
    % 末端执行器位置
    p_ee = p_all(:, end);
    
    % 计算几何雅可比
    J_geo = zeros(6, n);
    for i = 1:n
        if i == 1
            % 关节 1：使用基坐标系
            p_i = [0; 0; 0];
            z_i = [0; 0; 1];
        else
            % 改进 DH：使用当前连杆 i 的坐标系信息
            p_i = p_all(:, i);
            z_i = z_all(:, i);
        end
        
        % MATLAB 顺序：线速度在前，角速度在后
        J_geo(4:6, i) = z_i;
        J_geo(1:3, i) = cross(z_i, p_ee - p_i);
    end
end

function T = forward_kinematics_mdh(q)
    % 改进 DH 正运动学辅助函数
    MDH = [0, 0, 0.333, q(1); 
           -pi/2, 0, 0, q(2); 
           pi/2, 0, 0.316, q(3); 
           pi/2, 0.0825, 0, q(4); 
           -pi/2, -0.0825, 0.384, q(5); 
           pi/2, 0, 0, q(6); 
           pi/2, 0.088, 0.107, q(7)];
           
    T = eye(4);
    for i = 1:7
        alpha = MDH(i, 1);
        a = MDH(i, 2);
        d = MDH(i, 3);
        theta = MDH(i, 4);
        
        Ti = [cos(theta), -sin(theta), 0, a;
              sin(theta)*cos(alpha), cos(theta)*cos(alpha), -sin(alpha), -d*sin(alpha);
              sin(theta)*sin(alpha), cos(theta)*sin(alpha), cos(alpha), d*cos(alpha);
              0, 0, 0, 1];
        T = T * Ti;
    end
end

这段代码的关键在于 z_all 和 p_all 的索引。在 MDH 框架下，第 $i$ 个关节的旋转轴 $z_i$ 实际上对应的是连杆 $i$ 的局部坐标系 Z 轴，这与 SDH 中的 $z_{i-1}$ 概念不同。理解这一点能避免很多调试时的维度不匹配问题。

总结

1. SDH 适合开链结构，但易产生歧义。
2. MDH 对开链、树状、闭链结构均适用，推荐优先使用。
3. 编写代码时务必确认所用参数体系，混用会导致雅可比矩阵错误。