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

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

引用

CSDN

1.

https://blog.csdn.net/doudou2weiwei/article/details/145939224

本文将详细介绍如何使用DH参数法建立6自由度机械臂的正运动学模型。通过本文，读者将了解DH坐标系的建立方法、DH参数的确定以及如何使用Python代码实现正运动学计算。

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