当前位置:   article > 正文

机械臂正运动学-DH参数-Python快速实现_机械臂 dh python

机械臂 dh python

机械臂正运动学-DH参数-Python快速实现

前言:


更新:是我自己憨批了,说明书上有现成的计算方式,我没细看…

最近在玩一个非常弱智的机械臂,好多功能都没有,连个配套的仿真环境都没, 虚拟边界和碰撞检测的功能都非常难用。
没办法,我只能自己实现一个简陋的虚拟边界功能,这必须要在已知关节角的情况下,提前计算出每个关节的三维坐标。
这里的问题凝结为输入输出就是:
已知: 机械臂的关节长度,关节构型
在这里插入图片描述
输入: 机械臂的关节角度;
输出: 机械臂的关节坐标。
全网好像没有搜到一个简单可用、基于DH参数的Python的正运动学代码(github有一个不能用)。
为了防止以后忘记,以及方便大家学习借鉴。先抛出来,供大家参考

我为了实现这个功能 ,来来回回看了三天的资料和课程,但是感觉核心步骤也就几个公式和对应关系,所以我就把这几个核心的东西单拎出来了。

大家如果时间充分,可以详细的看看课程和教材,如果时间不够,就可以看我这个,如果我有哪些细节没有描述清楚的话,可以在评论区留言~

整体思路流程:

搜集机械臂相关配置资料:关节长度、构型、官方设定的坐标系;
通过两个对应关系,找到机械臂的DH参数表;
找到了之后,代入转换矩阵T中;
连乘所有关节的T;
获取关节三维坐标。

学习资料

要想得到上面的输出,需要的基础知识比较多。
有:

  1. 刚体的坐标变换;
  2. DH参数;
  3. 基本上就是上面两个了。

为了弄明白这个过程, 我请教了几位大佬,大佬说可以看看b站台大的机器人学,和《机器人学导论》-斯坦福的那本,自己去网上搜PDF版就好了
然后直接基本概念,大家可以去看看:
台大机器人学之运动学——林沛群(含课件+书籍)

核心概念:

我们将机械臂的每一个关节轴,都建立一个坐标系,那么从关节1到关节0的变化,其实就是做了一次刚体的坐标变换。
而关节7的末端点,则是串着做了好多次的坐标变换。

DH参数的理解。

先挂一个参考链接,这里面的介绍的更详细:
https://blog.csdn.net/aic1999/article/details/82490615
上节说到本质是坐标变换,那么我们如何根据已知信息,确定好坐标变换的基本信息?
这里面就得用到一个神奇的DH参数(两位大佬名字的缩写)
来看看课本里的这张经典图。
在这里插入图片描述
我们需要知道,决定四个轴的相对位置关系,我们可以用四个变量来描述(虽然可能不唯一,但是够了)。
那么我们需要知道的第一个对应关系:

DH参数的定义:

沿着轴方向,逆时针为正。
在这里插入图片描述
这里面我们还差一个东西,如何定义坐标系?

建立坐标系

其实一般如果是靠谱的机器人,这个坐标系应该是给的。
在这里插入图片描述
把我这次用的机器人的拿过来,作为例子,有例子,大家理解起来就方便了。
在这里插入图片描述
可以看出来一个很有意思的事情, 01关节是放在一起看了,即12坐标系原点重合,这里是将1杆的长度看作0了。下面去计算DH参数的时候也需要注意的。而且我们计算的时候,是无法计算出1轴的坐标。

坐标系和参数对应关系来了,我们就能填好DH表了:
如何填写?
对着坐标轴的图和连杆参数定义,一个一个填。
在这里插入图片描述
我们以第一个轴为例,第一行有四个值: α \alpha α1-0 , a a a1-0 , d d d1-0 θ \theta θ1-0
这里面的 α \alpha α1-0 α \alpha α0是绕X0正方向, 从Z0旋转到Z1的角度。从图中可以看出来,没旋转,即为0.
a a a0沿着X0,从Z0移动到Z1的距离,因为两个Z重合,不存在移动距离,即距离为0;
d1的话,不一样,沿着Z1轴,从X0移动到X1的距离,可以看出来,移动了0.31米,虽然方向不一样,但是确实是得移动这么多,才能重合。
θ \theta θ的话,就是关节转动的角度了,后面几个关节,可能初始角度得加一个180°才行。
这个DH参数值拿到了之后,就得想办法拿到转换矩阵了。
好在《机器人学导论》这本书里直接给了计算公式:

矩阵变换公式:

在这里插入图片描述

这玩意儿还得配套几个公式才行。
在这里插入图片描述
在这里插入图片描述

总之拿到了DH参数,就把表里的值代入到T中, 然后连乘,就能计算出末端位姿了。

最后直接上代码吧:

from mpl_toolkits.mplot3d import Axes3D
import matplotlib.pyplot as plt
from matplotlib import cm
import numpy as np
from math import radians, sin, cos


def set_axes_equal(ax):
# 这一段是copy别人的。用处不是很大。
    '''Make axes of 3D plot have equal scale so that spheres appear as spheres,
    cubes as cubes, etc..  This is one possible solution to Matplotlib's
    ax.set_aspect('equal') and ax.axis('equal') not working for 3D.
    Input
      ax: a matplotlib axis, e.g., as output from plt.gca().
    '''

    x_limits = ax.get_xlim3d()
    y_limits = ax.get_ylim3d()
    z_limits = ax.get_zlim3d()

    x_range = abs(x_limits[1] - x_limits[0])
    x_middle = np.mean(x_limits)
    y_range = abs(y_limits[1] - y_limits[0])
    y_middle = np.mean(y_limits)
    z_range = abs(z_limits[1] - z_limits[0])
    z_middle = np.mean(z_limits)

    # The plot bounding box is a sphere in the sense of the infinity
    # norm, hence I call half the max range the plot radius.
    plot_radius = 0.5*max([x_range, y_range, z_range])

    ax.set_xlim3d([x_middle - plot_radius, x_middle + plot_radius])
    ax.set_ylim3d([y_middle - plot_radius, y_middle + plot_radius])
    ax.set_zlim3d([z_middle - plot_radius, z_middle + plot_radius])

def dh_matrix(alpha, a, d, theta):
# 传入四个DH参数,根据公式3-6,输出一个T矩阵。
    alpha = alpha / 180 * np.pi
    theta = theta / 180 * np.pi
    matrix = np.identity(4)
    matrix[0,0] = cos(theta)
    matrix[0,1] = -sin(theta)
    matrix[0,2] = 0
    matrix[0,3] = a
    matrix[1,0] = sin(theta)*cos(alpha)
    matrix[1,1] = cos(theta)*cos(alpha)
    matrix[1,2] = -sin(alpha)
    matrix[1,3] = -sin(alpha)*d
    matrix[2,0] = sin(theta)*sin(alpha)
    matrix[2,1] = cos(theta)*sin(alpha)
    matrix[2,2] = cos(alpha)
    matrix[2,3] = cos(alpha)*d
    matrix[3,0] = 0
    matrix[3,1] = 0
    matrix[3,2] = 0
    matrix[3,3] = 1
    return matrix、
    
joint_num = 7

# --- Robotic Arm construction ---
# DH参数表,分别用一个列表来表示每个关节的东西。
joints_alpha = [0, 90, 90, 90, 90, 90, 90]
joints_a = [0, 0, 0, 0, 0, 0, 0]
joints_d = [0.31, 0.0, 0.4, 0.0, 0.4, 0.0, 0.175]
joints_theta = [0, 180, 180, 180, 180, 180, 180]

#    Joint Angle variables
# joints_angle = [-0.001, -21.0, -0.001, -21.0, 0.0, 0.0, -0.0]
# 选定几个特定的关节角,看看算出来的值,和真实值是否一致,方向是否反了。
joints_angle = [0, -23.43, 0, 50, 0, 0, 0]
#    DH参数转转换矩阵T---------------------
joint_hm = []
for i in range(joint_num):        
    joint_hm.append(dh_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])    
# Prepare the coordinates for plotting
for i in range(joint_num):
    print(np.round(joint_hm[i][:3, 3], 5))
# 获取坐标值
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]
# Plot
ax = plt.axes(projection='3d')
# ax.set_aspect('equal')
ax.plot3D(X, Y, Z)

ax.set_xlabel('x')
ax.set_ylabel('y')
ax.set_zlabel('z')

set_axes_equal(ax)
plt.show()
  • 1
  • 2
  • 3
  • 4
  • 5
  • 6
  • 7
  • 8
  • 9
  • 10
  • 11
  • 12
  • 13
  • 14
  • 15
  • 16
  • 17
  • 18
  • 19
  • 20
  • 21
  • 22
  • 23
  • 24
  • 25
  • 26
  • 27
  • 28
  • 29
  • 30
  • 31
  • 32
  • 33
  • 34
  • 35
  • 36
  • 37
  • 38
  • 39
  • 40
  • 41
  • 42
  • 43
  • 44
  • 45
  • 46
  • 47
  • 48
  • 49
  • 50
  • 51
  • 52
  • 53
  • 54
  • 55
  • 56
  • 57
  • 58
  • 59
  • 60
  • 61
  • 62
  • 63
  • 64
  • 65
  • 66
  • 67
  • 68
  • 69
  • 70
  • 71
  • 72
  • 73
  • 74
  • 75
  • 76
  • 77
  • 78
  • 79
  • 80
  • 81
  • 82
  • 83
  • 84
  • 85
  • 86
  • 87
  • 88
  • 89
  • 90
  • 91
  • 92
  • 93
  • 94
  • 95
  • 96
  • 97
声明:本文内容由网友自发贡献,不代表【wpsshop博客】立场,版权归原作者所有,本站不承担相应法律责任。如您发现有侵权的内容,请联系我们。转载请注明出处:https://www.wpsshop.cn/w/Monodyee/article/detail/96713
推荐阅读
相关标签
  

闽ICP备14008679号