CN106003041A - 一种五自由度机械手控制方法 - Google Patents

Abstract

本发明公开了一种五自由度机械手控制方法，属于机械设备控制领域。现有技术的控制方法繁琐，控制精度不高，不能有效控制机械手的运动。本发明对五自由机械手，所能出现的所有工作情况，将反解所有出现的可能解一一分析，分类成八种情况，将其所得的解与其对应进行控制。本发明控制方法简单、控制精度高的五自由度机械手控制方法。

Description

一种五自由度机械手控制方法

技术领域

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

背景技术

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

发明内容

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

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

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

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)的关节角度1；

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

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

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

代入之前求解得到的关节1和关节3的转动角度，通过上式解得关节2和关节3的转动角度之和；

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

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

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

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

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

进一步地，

①左臂

关节1的角度值：

对于关节1角度的解，通过对以上θ1、θ1+π、θ1-π三个方程进行判断，如在左臂的运动范围0°～180°，就将此值赋给θ1；

②右臂

关节1的角度值：

对于关节1角度的解，通过对以上θ1、θ1+π、θ1-π三个方程进行判断，如在左臂的运动范围-180°～0°，就将此值赋给θ1；

③上肘

关节3的角度值：

对于关节3角度的解，通过对上述两个方程θ3、θ3+π的判断，如若解得在上肘范围小于90度，就将此值赋给θ3；

④下肘

关节3的角度值：

对于关节3角度的解，通过对上述两个方程θ3、θ3+π的判断，如若解得在上肘范围大于90度，就将此值赋给θ3；

⑤翻转

关节4，5的角度值：

对于关节4，5角度的解，通过分别对以上θ4、θ4+π、θ4-π和以上θ5、θ5+π、θ5-π三个方程的判断，如若解得在翻转的运动范围-180°～0°，就将此值赋给关节4、5；

⑥非翻转

关节4，5的角度值：

对于关节4，5角度的解，通过分别对以上θ4、θ4+π、θ4-π和以上θ5、θ5+π、θ5-π三个方程的判断，如若解得在翻转的运动范围0°～180°，就将此值赋给关节4、5。

进一步地，将各个关节的角度值发送至运动控制卡，利用DSP高效的数据处理能力生成直流伺服电机的驱动信号，控制直流伺服电机的速度和转动角度；传感器检测到各个关节转动的角度，反馈给运动控制卡，从而精确的控制运动轨迹。

与现有技术相比，本发明具有以下有益效果：

本发明对五自由机械手，所能出现的所有工作情况，将反解所有出现的可能解一一分析，分类成八种情况，将其所得的解与其对应进行控制。本发明控制方法简单、控制精度高的五自由度机械手控制方法。

附图说明

图1为本发明的五自由度机械手连杆参数；

图4为本发明的机械手各关节角度示意图；

图2为本发明的机械手控制步骤；

图3为本发明的机械手各关节控制方法。

具体实施方式

为了使本发明的目的、技术方案及优点更加清楚明白，以下结合附图及实施例，对本发明进行进一步详细说明；应当理解，此处所描述的具体实施例仅仅用以解释本发明，并不用于限定本发明；

相反，本发明涵盖任何由权利要求定义的在本发明的精髓和范围上做的替代、修改、等效方法以及方案；进一步，为了使公众对本发明有更好的了解，在下文对本发明的细节描述中，详尽描述了一些特定的细节部分；对本领域技术人员来说没有这些细节部分的描述也完全理解本发明；

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

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°；

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

左臂上肘翻转、左臂上肘非翻、左臂下肘翻转、左臂下肘非翻、右臂上肘翻转、右臂上肘非翻、右臂下肘翻转、右臂下肘非翻，每一组的情况都是通过坐标的旋转、平移得到的齐次变换矩阵：

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

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

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

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

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

代入之前求解得到的关节1和关节3的转动角度，通过上式解得关节2和关节3的转动角度之和；

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

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

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

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

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

①左臂

关节1的角度值：

对于关节1角度的解，通过对以上θ1、θ1+π、θ1-π三个方程进行判断，如在左臂的运动范围0°～180°， 就将此值赋给θ1；

②右臂

关节1的角度值：

对于关节1角度的解，通过对以上θ1、θ1+π、θ1-π三个方程进行判断，如在左臂的运动范围-180°～0°，就将此值赋给θ1；

③上肘

关节3的角度值：

对于关节3角度的解，通过对上述两个方程θ3、θ3+π的判断，如若解得在上肘范围小于90度，就将此值赋给θ3；

④下肘

关节3的角度值：

对于关节3角度的解，通过对上述两个方程θ3、θ3+π的判断，如若解得在上肘范围大于90度，就将此值赋给θ3；

⑤翻转

关节4，5的角度值：

对于关节4，5角度的解，通过分别对以上θ4、θ4+π、θ4-π和以上θ5、θ5+π、θ5-π三个方程的判断，如若解得在翻转的运动范围-180°～0°，就将此值赋给关节4、5；

⑥非翻转

关节4，5的角度值：

对于关节4，5角度的解，通过分别对以上θ4、θ4+π、θ4-π和以上θ5、θ5+π、θ5-π三个方程的判 断，如若解得在翻转的运动范围0°～180°，就将此值赋给关节4、5。

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

将各个关节的角度值发送至运动控制卡，利用DSP高效的数据处理能力生成直流伺服电机的驱动信号，控制直流伺服电机的速度和转动角度；传感器检测到各个关节转动的角度，反馈给运动控制卡，从而精确的控制运动轨迹。

以上所述仅为本发明的较佳实施例而已，并不用以限制本发明，凡在本发明的精神和原则之内所作的任何修改、等同替换和改进等，均应包含在本发明的保护范围之内。

Claims (5)

1.一种五自由度机械手控制方法，其特征在于，包括如下步骤：

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

2.如权利要求1所述的一种五自由度机械手控制方法，其特征在于，左臂上肘翻转、左臂上肘非翻、左臂下肘翻转、左臂下肘非翻、右臂上肘翻转、右臂上肘非翻、右臂下肘翻转、右臂下肘非翻，

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

$T_5^0=T_1^{0}T_5^1=\left[\begin{array}{cccc}{n}_x& o_x& a_x& p_x\\ n_y& o_y& a_y& p_y\\ n_z& o_z& a_z& p_z\\ 0& 0& 0& 1\end{array}\right].$

3.如权利要求2所述的一种五自由度机械手控制方法，其特征在于，

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

${\mathrm{\θ}}_1=arctan\left(\frac{p_y}{p_x}\right)$

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

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

${\mathrm{\θ}}_3=arctan\frac{{p_x}^2+{p_y}^2+{p_z}^2-{d_4}^2-{l_2}^2}{2l_d{d}_4\sqrt{1-{\left(\frac{{p_x}^2+{p_y}^2+{p_z}^2-{d_4}^2-{l_2}^2}{2l_2{d}_4}\right)}^2}}$

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

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

${\mathrm{\θ}}_{23}=arctan\frac{l_2{c}_3{p}_z+(c_1{p}_x+s_1{p}_y)(l_2{s}_3+d_4)}{l_2{c}_3(c_1{p}_x+s_1{p}_y)-p_z(l_2{s}_3+d_4)}$

代入之前求解得到的关节1和关节3的转动角度，通过上式解得关节2和关节3的转动角度之和；

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

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

${\mathrm{\θ}}_4=arctan\frac{c_1{c}_{23}{a}_x+s_1{c}_{23}{a}_y+s_{23}{a}_z}{s_1{a}_x-c_1{a}_y}$

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

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

${\mathrm{\θ}}_5=arctan\frac{(c_4{c}_1{c}_{23}+s_1{s}_4)o_x+(c_4{s}_1{c}_{23}-c_1{s}_4)o_y+c_4{s}_{23}{o}_z}{c_1{s}_{23}{o}_x+s_1{s}_{23}{o}_y-c_{23}{o}_z}$

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

4.如权利要求3所述的一种五自由度机械手控制方法，其特征在于，

①左臂

关节1的角度值：

${\mathrm{\θ}}_1=arctan\left(\frac{p_y}{p_x}\right),arctan\left(\frac{p_y}{p_x}\right)\±\mathrm{\π}$

对于关节1角度的解，通过对以上θ1、θ1+π、θ1-π三个方程进行判断，如在左臂的运动范围0°～180°，就将此值赋给θ1；

②右臂

关节1的角度值：

${\mathrm{\θ}}_1=arctan\left(\frac{p_y}{p_x}\right),arctan\left(\frac{p_y}{p_x}\right)\±\mathrm{\π}$

对于关节1角度的解，通过对以上θ1、θ1+π、θ1-π三个方程进行判断，如在左臂的运动范围-180°～0°，就将此值赋给θ1；

③上肘

关节3的角度值：

${\mathrm{\θ}}_3=arctan\frac{{p_x}^2+{p_y}^2+{p_z}^2-{d_4}^2-{l_2}^2}{2l_2{d}_4\sqrt{1-{\left(\frac{{p_x}^2+{p_y}^2+{p_z}^2-{d_4}^2-{l_2}^2}{2l_2{d}_4}\right)}^2}},{\mathrm{\θ}}_3+\mathrm{\π}$

对于关节3角度的解，通过对上述两个方程θ3、θ3+π的判断，如若解得在上肘范围小于90度，就将此值赋给θ3；

④下肘

关节3的角度值：

${\mathrm{\θ}}_3=arctan\frac{{p_x}^2+{p_y}^2+{p_z}^2-{d_4}^2-{l_2}^2}{2l_2{d}_4\sqrt{1-{\left(\frac{{p_x}^2+{p_y}^2+{p_z}^2-{d_4}^2-{l_2}^2}{2l_2{d}_4}\right)}^2}},{\mathrm{\θ}}_3+\mathrm{\π}$

对于关节3角度的解，通过对上述两个方程θ3、θ3+π的判断，如若解得在上肘范围大于90度，就将此值赋给θ3；

⑤翻转

关节4，5的角度值：

$\begin{array}{c}{\mathrm{\θ}}_4=arctan\frac{c_1{c}_{23}{a}_x+s_1{c}_{23}{a}_y+s_{23}{a}_z}{s_1{a}_x-c_1{a}_y},{\mathrm{\θ}}_4+\mathrm{\π}\\ {\mathrm{\θ}}_5=\arctan \frac{(c_4{c}_1{c}_{23}+s_1{s}_4)o_x+(c_4{s}_1{c}_{23}-c_1{s}_4)o_y+c_4{s}_{23}{o}_z}{c_1{s}_{23}{o}_x+s_1{s}_{23}{o}_y-c_{23}{o}_z},{\mathrm{\θ}}_5\±\mathrm{\π}\end{array},$

对于关节4，5角度的解，通过分别对以上θ4、θ4+π、θ4-π和以上θ5、θ5+π、θ5-π三个方程的判折，如若解得在翻转的运动范围-180°～0°，就将此值赋给关节4、5；

⑥非翻转

关节4，5的角度值：

$\begin{array}{c}{\mathrm{\θ}}_4=arctan\frac{c_1{c}_{23}{a}_x+s_1{c}_{23}{a}_y+s_{23}{a}_z}{s_1{a}_x-c_1{a}_y},{\mathrm{\θ}}_4\±\mathrm{\π}\\ {\mathrm{\θ}}_5=arctan\frac{(c_4{c}_1{c}_{23}+s_1{s}_4)o_x+(c_4{s}_1{c}_{23}-c_1{s}_4)o_y+c_4{s}_{23}{o}_z}{c_1{s}_{23}{o}_x+s_1{s}_{23}{o}_y-c_{23}{o}_z},{\mathrm{\θ}}_5\±\mathrm{\π}\end{array},$

对于关节4，5角度的解，通过分别对以上θ4、θ4+π、θ4-π和以上θ5、θ5+π、θ5-π三个方程的判断，如若解得在翻转的运动范围0°～180°，就将此值赋给关节4、5。

5.如权利要求1-4任一所述的一种五自由度机械手控制方法，其特征在于，将各个关节的角度值发送至运动控制卡，利用DSP高效的数据处理能力生成直流伺服电机的驱动信号，控制直流伺服电机的速度和转动角度；传感器检测到各个关节转动的角度，反馈给运动控制卡，从而精确的控制运动轨迹。