02 Robot Kinematics And Dh Parameters · Mr.Robota 具身智能学习手册

1. Introduction

Forward Kinematics solves the problem of determining the pose of an end effector when all joint angles are known. The Denavit-Hartenberg (DH) parameter method is the standard approach for establishing linkage coordinate systems.

2. Detailed Explanation of DH Parameters (Standard Type)

Describe the relationship between the two adjacent links $i-1$ and $i$ , requiring only 4 parameters:

Parameter	Symbol	Physical Meaning	Measurement Reference
Link Length	$a_i$	The distance along the $x_i$ axis, from $z_{i-1}$ to $z_i$	Structure of the link itself
Link Torsion Angle	$\alpha_i$	The angle rotated around the $x_i$ axis, from $z_{i-1}$ to $z_i$	Structure of the link itself
Joint Offset	$d_i$	The distance along the $z_{i-1}$ axis, from $x_{i-1}$ to $x_i$	Joint variable (translational joint)
Joint Angle	$\theta_i$	The angle rotated around the $z_{i-1}$ axis, from $x_{i-1}$ to $x_i$	Joint variable (rotational joint)

The formula for the transformation matrix $T_{i-1}^{i}$ of adjacent links is:

T_{i-1}^{i} = \text{Rot}_{z}(\theta_i) \cdot \text{Trans}_{z}(d_i) \cdot \text{Trans}_{x}(a_i) \cdot \text{Rot}_{x}(\alpha_i)

3. Architecture Flow (Mermaid)

Mermaidgraph TD
    Input[输入: 关节角度 q1, q2, ..., qn] --> Step1[建立DH参数表]
    Step1 --> Step2[计算每个连杆的变换矩阵 T_01, T_12...]
    Step2 --> Step3[链式乘法 T_total = T_01 * T_12 * ...]
    Step3 --> Output[输出: 末端位姿 T_0n]

4. Python code verification

The following code implements a general DH matrix generator and validates a simple 2-link planar robotic arm.

Pythonimport numpy as np

def dh_matrix(theta, d, a, alpha):
    """
    根据标准DH参数计算变换矩阵 T_{i-1, i}
    单位: 角度使用弧度, 长度使用米
    """
    ct, st = np.cos(theta), np.sin(theta)
    ca, sa = np.cos(alpha), np.sin(alpha)

    return np.array([
        [ct, -st*ca,  st*sa, a*ct],
        [st,  ct*ca, -ct*sa, a*st],
        [0,   sa,     ca,    d   ],
        [0,   0,      0,     1   ]
    ])

def forward_kinematics_2link(joint_angles, link_lengths):
    """
    计算2连杆平面机械臂的末端位置
    """
    theta1, theta2 = joint_angles
    L1, L2 = link_lengths

    # 连杆 1: frame 0 -> frame 1
    # theta=q1, d=0, a=L1, alpha=0
    T_01 = dh_matrix(theta1, 0, L1, 0)

    # 连杆 2: frame 1 -> frame 2
    # theta=q2, d=0, a=L2, alpha=0
    T_12 = dh_matrix(theta2, 0, L2, 0)

    # 总变换矩阵
    T_02 = T_01 @ T_12

    return T_02

# --- 验证 ---
# 设定: L1=1m, L2=1m
# 情况 1: q1=0, q2=0 -> 手臂伸直横在X轴 -> 末端应为 (2, 0, 0)
# 情况 2: q1=90度, q2=0 -> 手臂垂直指天 -> 末端应为 (0, 2, 0)
# 情况 3: q1=0, q2=90度 -> 第一段横着，第二段向上 -> 末端应为 (1, 1, 0)

L_params = [1.0, 1.0]
q_case3 = [0, np.radians(90)]

T_final = forward_kinematics_2link(q_case3, L_params)
pos_final = T_final[:3, 3]

print(f"关节角: {np.degrees(q_case3)}")
print(f"计算出的末端位置 (x, y, z): {np.round(pos_final, 4)}")
# 预期输出: x=1.0, y=1.0, z=0.0