一、建立DH坐标系
1.确定Zi轴：
关节轴Zi 坐标系 {i} 的Z轴（转动轴或移动轴）
2.确定原点Oi:
公垂线ai与关节轴i的交点
示例： 如下图所示，坐标系{i-1}的原点是zi-1与zi的公垂线与zi-1的交点
3.确定Xi轴：
Xi沿公垂线 ai 由关节 i 指向关节i+1
示例：如下图所示，Xi-1的方向是沿着公垂线ai-1由关节i-1指向关节i
二、DH坐标系建立实例
1.找Z轴
以下所有关节全部为转动关节，则对应的Z轴就是各关节的转动轴
2.找原点：涵盖了两个z轴相交、平行、异面等情况的原点确定方法
Zi轴与Zi+1相交，则交点为原点Oi
Zi轴与Zi+1平行，则原点Oi一般使得di=0
异面时，原点Oi 为公垂线 ai 与关节轴 Zi 的交点
3.找X轴：涵盖了两个z轴相交、平行、异面等情况的X轴确定方法
Zi与Zi+1相交，则Xi为Zi与Zi+1的叉乘
Zi与Zi+1平行或异面，Xi的方向为沿公垂线ai由Zi指向Zi+1
三、确定DH参数
1.DH参数
•α(i-1) ( 连杆转角 ) ：绕X(i-1) 轴 ，从Z(i-1) 旋转 到Zi 的角度。
•a(i-1) ( 连杆长度 ): 沿X(i-1) 轴 ，从Z(i-1) 移动到Zi 的距离。
•di ( 关节偏移 ): 沿Zi 轴，从X(i-1) 移动到Xi 的距离。
•θi ( 关节角度 ): 绕Zi 轴 ，从X(i-1) 旋转 到Xi 的角度。
注：以下图为例，变换顺序如下：
先从坐标系{i-1}——变换α(i-1)得到坐标系{R}——变换a（i-1）得到坐标系{Q}——变换θi得到坐标系{P}——变换di得到坐标系{i}
2.齐次变换矩阵
3.示例的DH参数
三、python代码实现（6自由度机械臂）
1.建立一个六自由度机械臂的类函数
class RobotClass:def __init__(self, name, num_joints, count_type, joint_limits=None):self.name = nameself.num_joints = num_joints# 关节限位，格式[(最小值，最大值)]self.joint_limits = joint_limits if joint_limits is not None else [(0,0)] * num_jointsself.joint_angles = [0] * num_jointsself.a = [0, 0, 220, 90, 0, 0] self.alpha = [0, np.pi/2, 0, np.pi/2, -np.pi/2, np.pi/2]self.d = [0, 0, 0, 174, 0, 0]def self_joint_angles(self, angles):if len(angles) != self.num_joints:print(f"需要提供{self.num_joints}个关节角度, 但提供了{len(angles)}个。" ) # 关节数目过少报错for i, angle in enumerate(angles):if i == 2:self.joint_angles[i] = angle+np.pi/2else:if i == 3:self.joint_angles[i] = angle-np.pi/2else:self.joint_angles[i] = angleif angle < self.joint_limits[i][0] or angle > self.joint_limits[i][1]:print(f"关节{i+1}的角度{angle}超出了范围{self.joint_limits[i]}。" )def get_joint_angles(self):return self.joint_anglesdef dh_matrix(self, theta, d, a, alpha):# 根据dh参数计算单个关节的变换矩阵return np.array([[np.cos(theta), -np.sin(theta), 0, a],[np.sin(theta)*np.cos(alpha), np.cos(theta)*np.cos(alpha), -np.sin(alpha), -d*np.sin(alpha)],[np.sin(theta)*np.sin(alpha), np.cos(theta)*np.sin(alpha), np.cos(alpha), d*np.cos(alpha)],[0, 0, 0, 1]])def _ste_(self):return (f"机器人名称:{self.name}\n"f"关节数量：{self.num_joints}\n"f"关节角度：{self.joint_gangles}\n"f"关节范围：{self.joint_limits}")2.正运动学函数
# 正运动学
def forward_kinematics(self, angles):T = np.eye(4)TList = []self.self_joint_angles(angles)for i in range(self.num_joints):theta = self.joint_angles[i]a = self.a[i]d = self.d[i] alpha = self.alpha[i]T = np.dot(T, self.dh_matrix(theta, d, a, alpha)) # 矩阵乘法，计算各坐标系对基座的齐次变换矩阵TList.append(T)return TList3. 主函数调用相应的类函数及正运动学函数
import numpy as np
from robot_class import RobotClass # 导入类
from kinematic_function import forward_kinematics # 导入正运动学方程
if __name__ == '__main__':joint_limits = [(-2*np.pi, 2*np.pi), (-np.pi, 0), (0, np.pi), (-2*np.pi, 2*np.pi), (-np.pi/2, np.pi/2), (-2*np.pi, 2*np.pi)]robot = RobotClass(name="SunShine robot", num_joints=6, count_type=1, joint_limits=joint_limits)joint_angles = [0, -np.pi/2, 0, np.pi, 0, 0]T_matrix = forward_kinematics(robot, joint_angles)
