DH法建立6自由度机械臂正运动学模型

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

2.齐次变换矩阵

3.示例的DH参数

三、python代码实现（6自由度机械臂）

1.建立一个六自由度机械臂的类函数

class RobotClass:
    def __init__(self, name, num_joints, count_type, joint_limits=None):
        self.name = name
        self.num_joints = num_joints
        # 关节限位，格式[(最小值，最大值)]
        self.joint_limits = joint_limits if joint_limits is not None else [(0,0)] * num_joints
        self.joint_angles = [0] * num_joints
        self.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/2
            else:
                if i == 3:
                    self.joint_angles[i] = angle-np.pi/2
                else:
                    self.joint_angles[i] = angle
            if 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_angles
    def 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)