最便宜的网站建设国内10大搜索引擎

一、建立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 TList

3. 主函数调用相应的类函数及正运动学函数

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)