机器人正运动学DH参数详解

import math import numpy as np import matplotlib.pyplot as plt class DHParameter: def __init__(self, theta, d, a, alpha): self.theta = theta # 绕z轴的旋转角度（关节变量） self.d = d # 沿z轴的距离 self.a = a # 沿x轴的距离 self.alpha = alpha # 绕x轴的旋转角度 def transformation_matrix(self, alpha, a, d, theta): alpha = alpha / 180 * np.pi theta = theta / 180 * np.pi # 替换theta为关节变量（如果theta是关节变量） c_theta = math.cos(theta) s_theta = math.sin(theta) c_alpha = math.cos(alpha) s_alpha = math.sin(alpha) # 4x4齐次变换矩阵 # MDH T = [[c_theta, -s_theta, 0, a], [s_theta * c_alpha, c_theta * c_alpha, -s_alpha, -s_alpha * d], [s_theta * s_alpha, c_theta * s_alpha, c_alpha, c_alpha * d], [0, 0, 0, 1]] return T # 示例：使用D-H参数法描述一个简单的机器人连杆 # 假设我们有一个连杆，其D-H参数为：theta=0, d=0, a=1, alpha=0（只是一个平移连杆） dh_param = DHParameter(0, 0, 1, 0) # 如果你有多个连杆，并且需要计算整个机器人的变换矩阵， # 你需要按照连杆的顺序依次相乘它们的变换矩阵。 joint_num = 6 # --- Robotic Arm construction --- # DH参数表，分别用一个列表来表示每个关节的东西。 joints_alpha = [0, 90, 0, 0, -90, 90] # 0, -90, 0, -90, 90, -90 joints_a = [0, 0, 425, 395, 0, 0] joints_d = [152, 0.0, 0.0, 102,102, 100] joints_theta = [90, 90, -90, 0, 90, 180] joints_angle = [10, 10,-10, 0, 0, 0] # DH参数转转换矩阵T--------------------- joint_hm = [] for i in range(joint_num): joint_hm.append( dh_param.transformation_matrix(joints_alpha[i], joints_a[i], joints_d[i], joints_theta[i] + joints_angle[i])) # -----------连乘计算---------------------- for i in range(joint_num - 1): joint_hm[i + 1] = np.dot(joint_hm[i], joint_hm[i + 1]) print(np.round(joint_hm[i+1])) # Prepare the coordinates for plotting # # 获取坐标值 X = [hm[0, 3] for hm in joint_hm] Y = [hm[1, 3] for hm in joint_hm] Z = [hm[2, 3] for hm in joint_hm] ax = plt.axes(projection='3d') ax.set_aspect('equal') ax.plot3D(X, Y, Z)