锐客网

  1. 首页 > it技术 > >

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

?# 机械臂正运动学-DH参数-Python快速实现 @[toc]
前言：
最近在玩一个非常弱智的机械臂，好多功能都没有，连个配套的仿真环境都没， 虚拟边界和碰撞检测的功能都非常难用。 没办法，我只能自己实现一个简陋的虚拟边界功能，这必须要在已知关节角的情况下，提前计算出每个关节的三维坐标。 这里的问题凝结为输入输出就是： 已知： 机械臂的关节长度，关节构型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参数(两位大佬名字的缩写) 来看看课本里的这张经典图。python机器人运动学_机械臂正运动学-DH参数-Python快速实现
文章图片

我们需要知道，决定四个轴的相对位置关系，我们可以用四个变量来描述(虽然可能不唯一，但是够了)。 那么我们需要知道的第一个对应关系：
DH参数的定义：
沿着轴方向，逆时针为正。python机器人运动学_机械臂正运动学-DH参数-Python快速实现
文章图片

这里面我们还差一个东西，如何定义坐标系？
建立坐标系
其实一般如果是靠谱的机器人，这个坐标系应该是给的。python机器人运动学_机械臂正运动学-DH参数-Python快速实现
文章图片

【python机器人运动学_机械臂正运动学-DH参数-Python快速实现】把我这次用的机器人的拿过来，作为例子，有例子，大家理解起来就方便了。python机器人运动学_机械臂正运动学-DH参数-Python快速实现
文章图片

可以看出来一个很有意思的事情， 01关节是放在一起看了，即12坐标系原点重合，这里是将1杆的长度看作0了。下面去计算DH参数的时候也需要注意的。而且我们计算的时候，是无法计算出1轴的坐标。
坐标系和参数对应关系来了，我们就能填好DH表了： 如何填写？ 对着坐标轴的图和连杆参数定义，一个一个填。python机器人运动学_机械臂正运动学-DH参数-Python快速实现
文章图片

我们以第一个轴为例，第一行有四个值：$\alpha$~1-0~ , $a$~1-0~ , $d$~1-0~ $\theta$~1-0~ 这里面的$\alpha$~1-0~即$\alpha$~0~是绕X~0~正方向, 从Z~0~旋转到Z~1~的角度。从图中可以看出来，没旋转，即为0. $a$~0~沿着X~0~,从Z~0~移动到Z~1~的距离，因为两个Z重合，不存在移动距离，即距离为0； d~1~的话，不一样，沿着Z~1~轴，从X~0~移动到X~1~的距离，可以看出来，移动了0.31米，虽然方向不一样，但是确实是得移动这么多，才能重合。 $\theta$的话，就是关节转动的角度了，后面几个关节，可能初始角度得加一个180°才行。 这个DH参数值拿到了之后，就得想办法拿到转换矩阵了。 好在《机器人学导论》这本书里直接给了计算公式：
矩阵变换公式：
python机器人运动学_机械臂正运动学-DH参数-Python快速实现
文章图片

这玩意儿还得配套几个公式才行。python机器人运动学_机械臂正运动学-DH参数-Python快速实现
文章图片
python机器人运动学_机械臂正运动学-DH参数-Python快速实现
文章图片

总之拿到了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'sax.set_aspect('equal') and ax.axis('equal') not working for 3D.Inputax: 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()

    推荐阅读


    上一篇：python|python 机器人运动仿真_ROS仿真揭秘——七自由度机器人(五)

    下一篇：python|python 视频ai换脸_Python实现AI换脸--Faceswap