[发明专利]一种五自由度机械手控制方法

说明书

技术领域

本发明涉及一种五自由度机械手控制方法，属于机械设备控制领域。

背景技术

机械手能模仿人手和臂的某些动作功能，可以按固定程序抓取、搬运物件或操作工具的自动操作装置。机械手是最早出现的工业机器人，也是最早出现的现代机器人，它可代替人的繁重劳动以实现生产的机械化和自动化，能在有害环境下操作以保护人身安全，因而广泛应用于机械制造、冶金、电子、轻工和原子能等部门。它可通过编程来完成各种预期的作业任务，在构造和性能上兼有人和机器各自的优点，尤其体现了人的智能和适应性。机械手作业的准确性和各种环境中完成作业的能力，在国民经济各领域有着广阔的发展前景。但是机械手的控制方法比较繁琐，需要预先获取机械臂末端运动轨迹曲线，根据轨迹曲线反算各个机械手关节的转动量，将上述得到的转动量下发至运动控制卡，通过伺服驱动系统驱动各臂转动，各连杆联动完成给定的运动轨迹。但是现有技术的控制方法繁琐，控制精度不高，不能有效控制机械手的运动。针对目前现有技术中存在的上述缺陷，实有必要进行研发，解决现有技术中存在的缺陷；

发明内容

针对现有技术的缺陷，本发明的目的在于提供一种控制方法简单、控制精度高的五自由度机械手控制方法。

为实现上述目的，本发明的技术方案为：

一种五自由度机械手控制方法，包括如下步骤：

S1.获取机械臂末端运动轨迹曲线，根据曲线特点以及要求选取N个末端位置坐标P(px，py，pz)和接近矢量a(ax，ay，az)，方位矢量o(ox，oy，oz)，法向矢量n(nx，ny，nz)；

S2.将获取的N个点进行运动学反解的计算，得到的每一个轨迹坐标点相对应的各个驱动关节所对应的一个角度值，获得机械手的运动轨迹与各关节驱动角度之间的一个关系，五个关节角度分别是θ₁，θ₂，θ₃，θ₄，θ₅；

S3.由于反解存在多解的情况，故而需要对反解得到的各个解进行分析和判断，进而选取合理的解，但是取解的情况需要考虑到实际的工作情况和人为的需求；

根据基座的运动范围，将机械手分为左臂与右臂，左臂的运动范围是：0°～180°，右臂的运动范围是：-180°～0°；其次，根据小臂的运动范围将机械臂分为上肘θ₃＜90°与下肘θ₃≥90°；最后，对于最后两个决定末端姿态的驱动关节将其分为翻转-180°～0°与非翻转0°～180°；

因此，可将反解的多解情况分为八组然后选择其合理的一组，分别是：左臂上肘翻转、左臂上肘非翻、左臂下肘翻转、左臂下肘非翻、右臂上肘翻转、右臂上肘非翻、右臂下肘翻转、右臂下肘非翻；

S4.将上述得到的转动角度下发至运动控制卡，通过伺服驱动系统驱动各臂转动，通过传感器检测并反馈转动信号，各连杆联动完成给定的运动轨迹。

进一步地，左臂上肘翻转、左臂上肘非翻、左臂下肘翻转、左臂下肘非翻、右臂上肘翻转、右臂上肘非翻、右臂下肘翻转、右臂下肘非翻，

每一组的情况都是通过坐标的旋转、平移得到的齐次变换矩阵：

进一步地，

a)首先解出θ₁，可用逆变换左乘方程两边来求出θ₁：

即可通过上式解出该位置坐标下P(px，py，pz)的关节角度θ₁；

b)再通过上面的转换矩阵，可得关节角度θ₃的求解方程：

其中l₂，d₄分别表示的是大臂和小臂的长度，px、py、pz表示的是在对应角度下的位置坐标；

c)通过在齐次变换矩阵方程两边左乘逆变换可求得：

代入之前求解得到的第一关节和第三关节的转动角度，通过上式解得第二关节和第三关节的转动角度之和；

d)根据θ₂＝θ₂₃-θ₃，得到相对应的坐标下的第二关节的转动角度值；

e)在步骤c得到的矩阵中，解得：

求得的在相应坐标下的第四关节的转动角度；

f)在齐次变换矩阵两边同时乘以得到第五关节的转动角度：

求得的在相应坐标下的第五关节的转动角度。