机器人学入门:用Python手把手推导SDH与MDH变换矩阵(附代码)
机器人学实战:从零推导SDH与MDH变换矩阵的Python实现
刚接触机器人运动学时,DH参数法总是让人又爱又恨——概念清晰但矩阵推导繁琐,理论易懂可代码实现时总出错。今天我们就用Python的SymPy和NumPy库,以三自由度平面机械臂为例,手把手带您推导两种DH参数法的变换矩阵,并解释为何工业机器人领域更偏爱MDH方法。
1. 准备工作:理解坐标系差异
在开始写代码前,先明确两种方法的本质区别。SDH(标准DH)和MDH(改进DH)最核心的不同在于坐标系附着规则:
- SDH坐标系:固定在连杆的远端(即远离基座的一端)
- MDH坐标系:固定在连杆的近端(即靠近基座的一端)
这种差异导致参数定义和变换顺序完全不同。来看一个直观对比:
# 坐标系定义示例(伪代码)
class SDH_Frame:
z_axis = previous_joint_axis
x_axis = along_link_to_next_joint
class MDH_Frame:
z_axis = current_joint_axis
x_axis = along_link_from_previous_joint
提示:在串联机器人中,MDH通常能减少参数耦合,这就是UR、KUKA等厂商采用它的原因。
2. 建立三自由度机械臂模型
我们以经典的平面RRR机械臂为例,其DH参数如下:
2.1 MDH参数表
| 关节i | αᵢ₋₁ | aᵢ₋₁ | dᵢ | θᵢ |
|---|---|---|---|---|
| 1 | 0 | 0 | 0 | θ₁ |
| 2 | 0 | L₁ | 0 | θ₂ |
| 3 | 0 | L₂ | 0 | θ₃ |
2.2 SDH参数表
| 关节i | θᵢ | dᵢ | aᵢ | αᵢ |
|---|---|---|---|---|
| 1 | θ₁ | 0 | L₁ | 0 |
| 2 | θ₂ | 0 | L₂ | 0 |
| 3 | θ₃ | 0 | L₃ | 0 |
用Python定义这些参数:
import sympy as sp
import numpy as np
# 定义符号变量
theta1, theta2, theta3 = sp.symbols('theta1 theta2 theta3')
L1, L2, L3 = sp.symbols('L1 L2 L3')
# MDH参数
mdh_params = [
{'alpha': 0, 'a': 0, 'd': 0, 'theta': theta1},
{'alpha': 0, 'a': L1, 'd': 0, 'theta': theta2},
{'alpha': 0, 'a': L2, 'd': 0, 'theta': theta3}
]
# SDH参数
sdh_params = [
{'theta': theta1, 'd': 0, 'a': L1, 'alpha': 0},
{'theta': theta2, 'd': 0, 'a': L2, 'alpha': 0},
{'theta': theta3, 'd': 0, 'a': L3, 'alpha': 0}
]
3. 推导MDH变换矩阵
MDH的变换顺序为:RotX(α) → TransX(a) → RotZ(θ) → TransZ(d)
3.1 单关节变换实现
def mdh_transform(alpha, a, d, theta):
"""计算MDH单关节变换矩阵"""
# 绕X轴旋转α
Rx = sp.Matrix([
[1, 0, 0, 0],
[0, sp.cos(alpha), -sp.sin(alpha), 0],
[0, sp.sin(alpha), sp.cos(alpha), 0],
[0, 0, 0, 1]
])
# 沿X轴平移a
Tx = sp.Matrix([
[1, 0, 0, a],
[0, 1, 0, 0],
[0, 0, 1, 0],
[0, 0, 0, 1]
])
# 绕Z轴旋转θ
Rz = sp.Matrix([
[sp.cos(theta), -sp.sin(theta), 0, 0],
[sp.sin(theta), sp.cos(theta), 0, 0],
[0, 0, 1, 0],
[0, 0, 0, 1]
])
# 沿Z轴平移d
Tz = sp.Matrix([
[1, 0, 0, 0],
[0, 1, 0, 0],
[0, 0, 1, d],
[0, 0, 0, 1]
])
return Rx * Tx * Rz * Tz
3.2 验证三关节变换
计算从基座到末端执行器的总变换:
# 计算各关节MDH变换
T_mdh = []
for params in mdh_params:
T_mdh.append(mdh_transform(**params))
# 总变换矩阵
T_total_mdh = T_mdh[0] * T_mdh[1] * T_mdh[2]
# 打印简化后的结果
print("MDH总变换矩阵:")
sp.simplify(T_total_mdh)
输出结果将显示一个4×4的符号矩阵,包含θ₁、θ₂、θ₃和L₁、L₂的组合表达式。
4. 推导SDH变换矩阵
SDH的变换顺序相反:RotZ(θ) → TransZ(d) → TransX(a) → RotX(α)
4.1 单关节变换实现
def sdh_transform(theta, d, a, alpha):
"""计算SDH单关节变换矩阵"""
# 绕Z轴旋转θ
Rz = sp.Matrix([
[sp.cos(theta), -sp.sin(theta), 0, 0],
[sp.sin(theta), sp.cos(theta), 0, 0],
[0, 0, 1, 0],
[0, 0, 0, 1]
])
# 沿Z轴平移d
Tz = sp.Matrix([
[1, 0, 0, 0],
[0, 1, 0, 0],
[0, 0, 1, d],
[0, 0, 0, 1]
])
# 沿X轴平移a
Tx = sp.Matrix([
[1, 0, 0, a],
[0, 1, 0, 0],
[0, 0, 1, 0],
[0, 0, 0, 1]
])
# 绕X轴旋转α
Rx = sp.Matrix([
[1, 0, 0, 0],
[0, sp.cos(alpha), -sp.sin(alpha), 0],
[0, sp.sin(alpha), sp.cos(alpha), 0],
[0, 0, 0, 1]
])
return Rz * Tz * Tx * Rx
4.2 验证三关节变换
# 计算各关节SDH变换
T_sdh = []
for params in sdh_params:
T_sdh.append(sdh_transform(**params))
# 总变换矩阵
T_total_sdh = T_sdh[0] * T_sdh[1] * T_sdh[2]
# 打印简化后的结果
print("SDH总变换矩阵:")
sp.simplify(T_total_sdh)
5. 两种方法的数值验证
为了验证我们的推导正确,用具体数值进行测试:
# 设置具体参数值
test_values = {
L1: 1.0, L2: 0.8, L3: 0.6,
theta1: np.pi/4, theta2: np.pi/3, theta3: np.pi/6
}
# 数值化MDH矩阵
T_mdh_num = T_total_mdh.subs(test_values)
np_mdh = np.array(T_mdh_num).astype(np.float64)
# 数值化SDH矩阵
T_sdh_num = T_total_sdh.subs(test_values)
np_sdh = np.array(T_sdh_num).astype(np.float64)
print("MDH末端位置:", np_mdh[:3, 3])
print("SDH末端位置:", np_sdh[:3, 3])
注意:虽然两种方法的中间坐标系不同,但末端执行器的最终位姿应该一致——这是验证实现正确性的关键。
6. 实际应用中的选择建议
在真实项目中该如何选择?考虑以下因素:
-
MDH优势:
- 坐标系与关节自然对应
- 参数物理意义更直观
- 工业机器人常用(如UR机器人)
-
SDH适用场景:
- 学术文献和历史代码中常见
- 某些特定构型机械臂简化计算
# 实际项目中的推荐做法
def create_dh_parameters(robot_type):
if robot_type in ['UR', 'KUKA', 'ABB']:
return mdh_params # 工业机器人用MDH
elif robot_type == 'academic':
return sdh_params # 学术研究可能用SDH
else:
raise ValueError("Unknown robot type")
7. 常见错误排查
在实现过程中容易遇到的坑:
-
变换顺序错误:
- MDH必须是X旋转→X平移→Z旋转→Z平移
- SDH必须是Z旋转→Z平移→X平移→X旋转
-
参数混淆:
# 错误示例:把MDH的a参数当成SDH的a使用 wrong_transform = sdh_transform(theta1, 0, L1, 0) # 这个L1在SDH中实际对应a₁ -
符号计算陷阱:
- SymPy中角度默认是弧度
- 记得调用
simplify()或trigsimp()化简矩阵
-
数值验证失败:
- 检查末端位置是否相同
- 确认旋转矩阵行列式为1(有效的旋转矩阵)
# 检查旋转矩阵有效性的方法
def is_valid_rotation(R):
det = np.linalg.det(R[:3, :3])
return np.isclose(det, 1.0) and np.allclose(R @ R.T, np.eye(3))
8. 性能优化技巧
当需要高频计算时,可以:
-
预编译符号表达式:
from sympy.utilities.lambdify import lambdify # 将符号表达式转为数值函数 mdh_func = lambdify((theta1, theta2, theta3, L1, L2), T_total_mdh, 'numpy') sdh_func = lambdify((theta1, theta2, theta3, L1, L2, L3), T_total_sdh, 'numpy') -
使用NumPy向量化计算:
# 批量计算多个位形 thetas = np.random.rand(100, 3) * 2*np.pi # 100个随机位形 results = np.array([mdh_func(*t, 1.0, 0.8, 0.6) for t in thetas]) -
缓存常用变换:
from functools import lru_cache @lru_cache(maxsize=100) def cached_mdh(theta1, theta2, theta3): return mdh_func(theta1, theta2, theta3, 1.0, 0.8)
在机械臂控制项目中,我习惯先用SymPy推导验证公式,再转为NumPy函数集成到实时控制系统中。当遇到奇怪的位姿问题时,回头检查DH参数定义总能发现大多数错误——特别是新手容易混淆a和d参数的定义方向。
更多推荐

所有评论(0)