四轴机械手正运动学求解

随着机械行业的发展，机械手在工业生产用的应用愈加广泛，尤其是在自动化生产中。

为了更好的理解机械手的工作方式，以便在工作中得以更好的应用，从前段时间开始学习《机器人学》。

在我当前工作中，用的较多的是四轴机械手，在对节拍要求不高的流水线中使用。如下图所示：

其包含三个旋转轴、一个Z向平移轴。建立坐标系如下图所示：

由坐标系可以得出D-H参数表，如下所示：

在本次运动求解中，应用python、numpy、sympy来进行求解。首先将numpy、sympy导入。

import numpy as np
import sympy

将涉及到的变量符号化：

o1,o2,o3,l1,l2,l4 = sympy.symbols("o1,o2,o3,l1,l2,l4")

不同关节坐标之间的转换，A1~A4：

A1 = np.array([ [(暂不可见)s(o1), -sympy.sin(o1), 0, l1*(暂不可见)s(o1)], [sympy.sin(o1), (暂不可见)s(o1) , 0 , l1*sympy.sin(o1)], [0,0,1,0], [0,0,0,1]])
A2 = np.array([ [(暂不可见)s(o2), -sympy.sin(o2), 0, l2*(暂不可见)s(o2)], [sympy.sin(o2), (暂不可见)s(o2) , 0 , l2*sympy.sin(o2)], [0,0,1,0], [0,0,0,1]])
A3 = ([ [(暂不可见)s(o3), -sympy.sin(o3),0,0], [sympy.sin(o3), (暂不可见)s(o3) ,0,0], [0,0,1,0], [0,0,0,1]])
A4 = np.array([ [1,0,0,0], [0,1,0,0], [0,0,1,l4], [0,0,0,1]])

求解总的变换矩阵：

A = np.dot(A1,A2)
A = np.dot(A,A3)
A = np.dot(A,A4)

可以得到结果如下：

[[(-sin(o1)*sin(o2) + cos(o1)*cos(o2))*cos(o3) + (-sin(o1)*cos(o2) - sin(o2)*cos(o1))*sin(o3) 
-(-sin(o1)*sin(o2) + cos(o1)*cos(o2))*sin(o3) + (-sin(o1)*cos(o2) - sin(o2)*cos(o1))*cos(o3)
0 l1*cos(o1) - l2*sin(o1)*sin(o2) + l2*cos(o1)*cos(o2)] 
[(-sin(o1)*sin(o2) + cos(o1)*cos(o2))*sin(o3) + (sin(o1)*cos(o2) + sin(o2)*cos(o1))*cos(o3) 
(-sin(o1)*sin(o2) + cos(o1)*cos(o2))*cos(o3) - (sin(o1)*cos(o2) + sin(o2)*cos(o1))*sin(o3) 0 l1*sin(o1) + l2*sin(o1)*cos(o2) + l2*sin(o2)*cos(o1)] 
[0 0 1 l4] 
[0 0 0 1]]

应用sympy对结果进行简化：

A = sympy.simplify(A)

简化后的结果如下：

[[cos(o1 + o2 + o3), -sin(o1 + o2 + o3), 0, l1*cos(o1) + l2*cos(o1 + o2)], 
[sin(o1 + o2 + o3), cos(o1 + o2 + o3), 0, l1*sin(o1) + l2*sin(o1 + o2)], 
[0, 0, 1, l4],
[0, 0, 0, 1]]

至此求解完成。

（ 北国风123 DIY电子制作与控制）

声明：本网站所收集的部分公开资料来源于互联网，转载的目的在于传递更多信息及用于网络分享，并不代表本站赞同其观点和对其真实性负责，也不构成任何其他建议。本站部分作品是由网友自主投稿和发布、编辑整理上传，对此类作品本站仅提供交流平台，不为其版权负责。如果您发现网站上所用视频、图片、文字如涉及作品版权问题，请第一时间告知，我们将根据您提供的证明材料确认版权并按国家标准支付稿酬或立即删除内容，以保证您的权益！联系电话：010-58612588 或 Email:editor@mmsonline.com.cn。