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

常见误区与代码实现

部分经典教材使用 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. 编写代码时务必确认所用参数体系，混用会导致雅可比矩阵错误。