CN113580140A - 一种七轴机械臂的控制方法及终端 - Google Patents

Abstract

本发明提供了一种七轴机械臂的控制方法和计算终端，包括步骤：以七轴机械臂的行动轴的某一点为原点，以行动轴的移动方向为X轴，以平行于水平面且垂直于行动轴的方向为Y轴，以垂直于水平面的方向为Z轴，建立空间直角坐标系；获取抓取位置在空间直角坐标系的三维坐标值(X_抓,Y_抓,Z_抓)；依次计算T轴抓取平台平行于水平面且高度为Z_抓时的所有垂直转动轴关节的旋转角度、位于Y_抓时的水平转动轴关节的旋转角度、位于X_抓时的行动轴的X轴位置X_行；最后计算T轴关节的旋转角度，并根据每一个轴关节的旋转角度计算得到七轴机械臂的移动路径。本发明极大简化了各轴关节运算确定唯一值的算法，实现以快速精准地控制七轴机械臂移动至抓取位置。

Description

一种七轴机械臂的控制方法及终端

技术领域

本发明涉及机械臂控制技术领域，特别涉及一种七轴机械臂的控制方法及终端。

背景技术

工业机械臂往往通过示教方式设置机械臂的移动路径，但因为机械臂关节数量多，示教不灵活往往定位不准，需来回调试。如果通过上位机直接计算各个轴的脉冲(角度)数据，再把数据直接生成对应运行文件，可以精确控制机械臂的移动路径，但是七轴机械臂指定位置点后再计算各轴数据，其算法复杂而且同一个位置点往往有多个解，每一个解对应有不同的运动姿势，有些解所得到的姿势还会产生干涉碰撞，因此，这种由机器人通过给定坐标点自动生成的关节往往因为姿势不合理不能满足实际需求；同时又因为存在多个解，因此不同厂家的机械臂会采用不同的控制策略，且在设计相关上位机软件时也无法只通过计算和实际运行的机械臂关节姿势吻合。

发明内容

本发明所要解决的技术问题是：提供一种七轴机械臂的控制方法及终端，实现以快速和精准控制七轴机械臂移动至抓取位置。

为了解决上述技术问题，本发明采用的技术方案为：

一种七轴机械臂的控制方法，包括步骤：

S1、以七轴机械臂的行动轴的某一点为原点，以所述行动轴的移动方向为X轴，以平行于水平面且垂直于所述行动轴的方向为Y轴，以垂直于水平面的方向为Z轴，建立空间直角坐标系；

S2、获取抓取位置在所述空间直角坐标系的三维坐标值(X_抓,Y_抓,Z_抓)；

S3、计算以得到位于所述七轴机械臂的T轴的抓取平台平行于水平面且高度为Z_抓时的所有垂直转动轴关节的旋转角度；

S4、在所述垂直转动轴关节位于对应的所述旋转角度的基础上，计算以得到所述抓取平台的中心位于Y_抓时的水平转动轴关节的旋转角度；

S5、在所述垂直转动轴关节和所述水平转动轴关节位于对应的所述旋转角度的基础上，计算以得到所述抓取平台的中心位于X_抓时的所述行动轴的X轴位置X_行；

S6、计算以得到所述T轴关节的旋转角度，并根据每一个轴关节的旋转角度计算得到所述七轴机械臂的移动路径，控制所述七轴机械臂沿着所述移动路径移动至所述抓取平台的抓取位置。

为了解决上述技术问题，本发明采用的另一个技术方案为：

一种七轴机械臂的控制终端，包括存储器、处理器以及存储在存储器上并可在处理器上运行的计算机程序，所述处理器执行所述计算机程序时实现以下步骤：

S1、以七轴机械臂的行动轴的某一点为原点，以所述行动轴的移动方向为X轴，以平行于水平面且垂直于所述行动轴的方向为Y轴，以垂直于水平面的方向为Z轴，建立空间直角坐标系；

S2、获取抓取位置在所述空间直角坐标系的三维坐标值(X_抓,Y_抓,Z_抓)；

S3、计算以得到位于所述七轴机械臂的T轴的抓取平台平行于水平面且高度为Z_抓时的所有垂直转动轴关节的旋转角度；

S4、在所述垂直转动轴关节位于对应的所述旋转角度的基础上，计算以得到所述抓取平台的中心位于Y_抓时的水平转动轴关节的旋转角度；

S5、在所述垂直转动轴关节和所述水平转动轴关节位于对应的所述旋转角度的基础上，计算以得到所述抓取平台的中心位于X_抓时的所述行动轴的X轴位置X_行；

S6、计算以得到所述T轴关节的旋转角度，并根据每一个轴关节的旋转角度计算得到所述七轴机械臂的移动路径，控制所述七轴机械臂沿着所述移动路径移动至所述抓取平台的抓取位置。

本发明的有益效果在于：本发明提供一种七轴机械臂的控制方法及终端，根据七轴机械臂各轴关节的旋转特点，通过建立三维直角坐标系获取抓取位置的坐标后，首先求得对Z轴位置产生影响的垂直转动轴关节在抓取平台位于Z_抓下时的旋转角度，再求得对Y轴位置产生影响的水平转动轴关节在抓取平台位于Y_抓下的旋转角度，然后再进一步求得对X轴位置产生影响的行动轴在抓取平台位于X_抓下的行动长度X_行，最后计算T轴关节的旋转角度以便与抓取物料的抓取方向对应。由此，在将七轴机械臂各轴关节进行分类之后，每次计算同一转动方向的轴关节的旋转角度，以快速得到每个轴关节的旋转角度，从而快速计算得到七轴机械臂的移动路径，极大简化了各轴关节运算确定唯一值的算法，实现以快速精准地控制七轴机械臂移动至抓取位置。

附图说明

图1为本发明实施例的一种七轴机械臂的控制方法的流程图；

图2为本发明实施例的一种七轴机械臂的结构示意图；

图3为本发明实施例的一种七轴机械臂的XZ轴投影面示意图；

图4为本发明实施例的一种七轴机械臂的XY轴投影面示意图；

图5为本发明实施例的一种七轴机械臂的控制终端的结构示意图。

标号说明：

1、行动轴；2、S轴关节；3、L轴关节；4、U轴关节；5、R轴关节；6、B轴关节；7、T轴关节；8、抓取平台。

具体实施方式

为详细说明本发明的技术内容、所实现目的及效果，以下结合实施方式并配合附图予以说明。

请参照图1至图4，一种七轴机械臂的控制方法，包括步骤：

S1、以七轴机械臂的行动轴的某一点为原点，以所述行动轴的移动方向为X轴，以平行于水平面且垂直于所述行动轴的方向为Y轴，以垂直于水平面的方向为Z轴，建立空间直角坐标系；

S2、获取抓取位置在所述空间直角坐标系的三维坐标值(X_抓,Y_抓,Z_抓)；

S3、计算以得到位于所述七轴机械臂的T轴的抓取平台平行于水平面且高度为Z_抓时的所有垂直转动轴关节的旋转角度；

S4、在所述垂直转动轴关节位于对应的所述旋转角度的基础上，计算以得到所述抓取平台的中心位于Y_抓时的水平转动轴关节的旋转角度；

S5、在所述垂直转动轴关节和所述水平转动轴关节位于对应的所述旋转角度的基础上，计算以得到所述抓取平台的中心位于X_抓时的所述行动轴的X轴位置X_行；

S6、计算以得到所述T轴关节的旋转角度，并根据每一个轴关节的旋转角度计算得到所述七轴机械臂的移动路径，控制所述七轴机械臂沿着所述移动路径移动至所述抓取平台的抓取位置。

由上述描述可知，本发明的有益效果在于：根据七轴机械臂各轴关节的旋转特点，通过建立三维直角坐标系获取抓取位置的坐标后，首先求得对Z轴位置产生影响的垂直转动轴关节在抓取平台位于Z_抓下时的旋转角度，再求得对Y轴位置产生影响的水平转动轴关节在抓取平台位于Y_抓下的旋转角度，然后再进一步求得对X轴位置产生影响的行动轴在抓取平台位于X_抓下的行动长度X_行，最后计算T轴关节的旋转角度以便与抓取物料的抓取方向对应。由此，在将七轴机械臂各轴关节进行分类之后，每次计算同一转动方向的轴关节的旋转角度，以快速得到每个轴关节的旋转角度，从而快速计算得到七轴机械臂的移动路径，极大简化了各轴关节运算确定唯一值的算法，实现以快速精准地控制七轴机械臂移动至抓取位置。

进一步地，所述垂直转动轴关节包括L轴关节、U轴关节、R轴关节和B轴关节，所述水平转动轴关节包括S轴关节，所述行动轴、所述S轴关节、所述L轴关节、所述U轴关节、所述R轴关节、所述B轴关节和所述T轴关节依次连接；

所述S3具体为：

设L₂为所述S轴关节到所述L轴关节的垂直距离，L₃为所述L轴关节到所述U轴关节的距离，L₄为所述U轴关节与所述R轴关节的垂直距离，L₅为所述U轴关节与所述B轴关节的水平距离，L₆为所述B轴关节到所述T轴关节的距离，L₇为所述T轴关节到所述抓取平台的距离，L₈为L₄与L₅的斜线长，θ₁为所述L轴关节的旋转角度，θ₂为所述U轴关节的旋转角度，θ₃为所述B轴关节的旋转角度，θ₄为L₅与L₈的夹角，θ₅为L₈与L₆的夹角，θ₆为L₈与垂直于L₃的线之间的夹角，θ₇为L₈与水平面的夹角；

其中，L₂、L₃、L₄、L₅、L₆和L₇均为固定值，由所述七轴机械臂的规格而定，则有θ₄＝arccos(L₅/L₈)，

若所述抓取平台为向下抓取物料时，则预设所述R轴关节的旋转角度为0°，有第一公式：

Z_抓＝L₂-L₃sinθ₁+L₈sinθ₇-L₆-L₇；

若所述抓取平台为向上抓举物料时，则预设所述R轴关节旋转角度为±180°，有第二公式：

Z_抓＝L₂-L₃sinθ₁+L₈sinθ₇+L₆+L₇；

预设θ₁+θ₂为预设值K，通过逐次逼近法求出Z_抓高度下θ₁的接近值，得到所述L轴关节的旋转角度θ₁，则所述U轴关节的旋转角度θ₂＝K-θ₁；

由三角函数求得第三公式：

θ₁＝90°+θ₆+θ₇；

第四公式：

θ₆＝90°-θ₂-θ₄＝90°-K-θ₁-θ₄；

将所述第四公式代入所述第三公式，得到θ₇为：

θ₇＝-θ₁+θ₄+K-θ₁＝-2θ₁+θ₄+K；

则所述B轴关节的旋转角度θ₃为：

θ₃＝θ₄+θ₅＝θ₄+90°-θ₇＝2θ₁+90°-K。

由上述描述可知，根据七轴机械臂的特点可知，由于R轴的旋转会导致抓取平台的倾斜，为了保证抓取平台平行大地，实现更好地抓取物料，则R轴的旋转角度只能为0°或者180°，可根据抓取物料的方式为向下抓取还是向上抓举而定；因此在R轴旋转角度固定的前提下，对Z轴高度会产生影响的就剩下L轴、U轴和B轴了，因此通过如图3所示在XZ轴投影面上模拟L轴关节、U轴关节和B轴关节可能旋转的角度，使最终的T轴上的抓取平台能够位于抓取位置所在的Z_抓高度下，由于七轴机械臂的规格决定了其中一些的边线长度为固定值，在此基础上，将L轴关节和U轴关节旋转角度之和预设为一个固定的预设值，并通过逐次逼近法求得L轴关节的旋转角度后结合数学方法求得U轴、B轴的旋转角度，极大简化了计算的复杂度，即快速得到了L轴、U轴、R轴和B轴关节的旋转角度。

进一步地，所述S4具体为：

设L轴、U轴、R轴和B轴在XY投影平面上的伸展长度为L_伸，则有第五公式：

L_伸＝L₁+L₃cosθ₁+L₈cosθ₇＝L₁+L_3COSθ₁+L₈cos(-2θ₁+θ₄+K)；

其中L₁为所述S轴关节到所述L轴关节的水平偏移距离，则所述S轴关节的旋转角度θ_S为：

θ_S＝arcsin(Y_抓/L_伸)。

由上述描述可知，由七轴机械臂的特点可知，七轴机械臂上除去行动轴为水平移动之外，其他每个轴关节无论如何转动，其关节始终在一条水平线上，在此基础上，由于L轴、U轴、R轴和B轴关节的旋转角度已经确定，因此可以求得这四个轴在XY轴投影面上的伸展长度，即如图4所示；同时，剩下的行动轴、S轴和T轴中，只有S轴的转动会影响Y轴的变化，即只有S轴关节的旋转能使抓取平台位于Y_抓下，因此结合已经求得的伸展长度再次通过数学方法即可快速求得S轴关节的旋转角度。

进一步地，所述S5具体为：

设L_伸在X轴上的投影为L_X，则：

L_X＝L_伸sinθ_S；

X_行＝X_抓-L_X。

由上述描述可知，由于S轴、L轴、U轴、R轴和B轴关节的旋转角度均以确定，此时只有行动轴的移动能使抓取平台位于X_抓下，而由于L轴、U轴、R轴和B轴关节在XY轴投影面上的伸展长度已经求出，S轴关节的旋转角度也已经确定，即可以快速求得L轴、U轴、R轴和B轴这四轴在X轴上的投影长度，而行动轴需要移动的长度即为抓取位置所在的X轴坐标距离原点的长度减去L轴、U轴、R轴和B轴这四轴在X轴上的投影长度。

进一步地，所述S6中求得所述T轴关节的旋转角度具体为：

设所述T轴关节的初始角度为θ_z，则所述T轴关节的旋转角度θ_T＝θ_z-θ_S。

由上述描述可知，当S轴关节旋转时会带动T轴偏转到与原来抓取位置偏离的角度，因此需要将T轴关节转回S轴关节旋转的角度，确保抓取平台与抓取位置的抓取角度保持不变，以便抓取物料。

请参照图5，一种七轴机械臂的控制终端，包括存储器、处理器以及存储在存储器上并可在处理器上运行的计算机程序，所述处理器执行所述计算机程序时实现以下步骤：

S1、以七轴机械臂的行动轴的某一点为原点，以所述行动轴的移动方向为X轴，以平行于水平面且垂直于所述行动轴的方向为Y轴，以垂直于水平面的方向为Z轴，建立空间直角坐标系；

S2、获取抓取位置在所述空间直角坐标系的三维坐标值(X_抓,Y_抓,Z_抓)；

S3、计算以得到位于所述七轴机械臂的T轴的抓取平台平行于水平面且高度为Z_抓时的所有垂直转动轴关节的旋转角度；

S4、在所述垂直转动轴关节位于对应的所述旋转角度的基础上，计算以得到所述抓取平台的中心位于Y_抓时的水平转动轴关节的旋转角度；

S5、在所述垂直转动轴关节和所述水平转动轴关节位于对应的所述旋转角度的基础上，计算以得到所述抓取平台的中心位于X_抓时的所述行动轴的X轴位置X_行；

S6、计算以得到所述T轴关节的旋转角度，并根据每一个轴关节的旋转角度计算得到所述七轴机械臂的移动路径，控制所述七轴机械臂沿着所述移动路径移动至所述抓取平台的抓取位置。

由上述描述可知，本发明的有益效果在于：基于同一技术构思，配合上述的一种七轴机械臂的控制方法，提供一种七轴机械臂移动路径的计算终端，根据七轴机械臂各轴关节的旋转特点，通过建立三维直角坐标系获取抓取位置的坐标后，首先求得对Z轴位置产生影响的垂直转动轴关节在抓取平台位于Z_抓下时的旋转角度，再求得对Y轴位置产生影响的水平转动轴关节在抓取平台位于Y_抓下的旋转角度，然后再进一步求得对X轴位置产生影响的行动轴在抓取平台位于X_抓下的行动长度X_行，最后计算T轴关节的旋转角度以便与抓取物料的抓取方向对应。由此，在将七轴机械臂各轴关节进行分类之后，每次计算同一转动方向的轴关节的旋转角度，以快速得到每个轴关节的旋转角度，从而快速计算得到七轴机械臂的移动路径，极大简化了各轴关节运算确定唯一值的算法，实现以快速精准地控制七轴机械臂移动至抓取位置。

进一步地，所述垂直转动轴关节包括L轴关节、U轴关节、R轴关节和B轴关节，所述水平转动轴关节包括S轴关节，所述行动轴、所述S轴关节、所述L轴关节、所述U轴关节、所述R轴关节、所述B轴关节和所述T轴关节依次连接；

所述S3具体为：

设L₂为所述S轴关节到所述L轴关节的垂直距离，L₃为所述L轴关节到所述U轴关节的距离，L₄为所述U轴关节与所述R轴关节的垂直距离，L₅为所述U轴关节与所述B轴关节的水平距离，L₆为所述B轴关节到所述T轴关节的距离，L₇为所述T轴关节到所述抓取平台的距离，L₈为L₄与L₅的斜线长，θ₁为所述L轴关节的旋转角度，θ₂为所述U轴关节的旋转角度，θ₃为所述B轴关节的旋转角度，θ₄为L₅与L₈的夹角，θ₅为L₈与L₆的夹角，θ₆为L₈与垂直于L₃的线之间的夹角，θ₇为L₈与水平面的夹角；

其中，L₂、L₃、L₄、L₅、L₆和L₇均为固定值，由所述七轴机械臂的规格而定，则有θ₄＝arccos(L₅/L₈)，

若所述抓取平台为向下抓取物料时，则预设所述R轴关节的旋转角度为0°，有第一公式：

Z_抓＝L₂-L₃sinθ₁+L₈sinθ₇-L₆-L₇；

若所述抓取平台为向上抓举物料时，则预设所述R轴关节旋转角度为±180°，有第二公式：

Z_抓＝L₂-L₃sinθ₁+L₈sinθ₇+L₆+L₇；

预设θ₁+θ₂为预设值K，通过逐次逼近法求出Z_抓高度下θ₁的接近值，得到所述L轴关节的旋转角度θ₁，则所述U轴关节的旋转角度θ₂＝K-θ₁；

由三角函数求得第三公式：

θ₁＝90°+θ₆+θ₇；

第四公式：

θ₆＝90°-θ₂-θ₄＝90°-K-θ₁-θ₄；

将所述第四公式代入所述第三公式，得到θ₇为：

θ₇＝-θ₁+θ₄+K-θ₁＝-2θ₁+θ₄+K；

则所述B轴关节的旋转角度θ₃为：

θ₃＝θ₄+θ₅＝θ₄+90°-θ₇＝2θ₁+90°-K。

由上述描述可知，根据七轴机械臂的特点可知，由于R轴的旋转会导致抓取平台的倾斜，为了保证抓取平台平行大地，实现更好地抓取物料，则R轴的旋转角度只能为0°或者180°，可根据抓取物料的方式为向下抓取还是向上抓举而定；因此在R轴旋转角度固定的前提下，对Z轴高度会产生影响的就剩下L轴、U轴和B轴了，因此通过如图3所示在XZ轴投影面上模拟L轴关节、U轴关节和B轴关节可能旋转的角度，使最终的T轴上的抓取平台能够位于抓取位置所在的Z_抓高度下，由于七轴机械臂的规格决定了其中一些的边线长度为固定值，在此基础上，将L轴关节和U轴关节旋转角度之和预设为一个固定的预设值，并通过逐次逼近法求得L轴关节的旋转角度后结合数学方法求得U轴、B轴的旋转角度，极大简化了计算的复杂度，即快速得到了L轴、U轴、R轴和B轴关节的旋转角度。

进一步地，所述S4具体为：

设L轴、U轴、R轴和B轴在XY投影平面上的伸展长度为L_伸，则有第五公式：

L_伸＝L₁+L₃cosθ₁+L₈cosθ₇＝L₁+L_3COSθ₁+L₈cos(-2θ₁+θ₄+K)；

其中L₁为所述S轴关节到所述L轴关节的水平偏移距离，则所述S轴关节的旋转角度θ_S为：

θ_S＝arcsin(Y_抓/L_伸)。

由上述描述可知，由七轴机械臂的特点可知，七轴机械臂上除去行动轴为水平移动之外，其他每个轴关节无论如何转动，其关节始终在一条水平线上，在此基础上，由于L轴、U轴、R轴和B轴关节的旋转角度已经确定，因此可以求得这四个轴在XY轴投影面上的伸展长度，即如图4所示；同时，剩下的行动轴、S轴和T轴中，只有S轴的转动会影响Y轴的变化，即只有S轴关节的旋转能使抓取平台位于Y_抓下，因此结合已经求得的伸展长度再次通过数学方法即可快速求得S轴关节的旋转角度。

进一步地，所述S5具体为：

设L_伸在X轴上的投影为L_X，则：

L_X＝L_伸sinθ_S；

X_行＝X_抓-L_X。

由上述描述可知，由于S轴、L轴、U轴、R轴和B轴关节的旋转角度均以确定，此时只有行动轴的移动能使抓取平台位于X_抓下，而由于L轴、U轴、R轴和B轴关节在XY轴投影面上的伸展长度已经求出，S轴关节的旋转角度也已经确定，即可以快速求得L轴、U轴、R轴和B轴这四轴在X轴上的投影长度，而行动轴需要移动的长度即为抓取位置所在的X轴坐标距离原点的长度减去L轴、U轴、R轴和B轴这四轴在X轴上的投影长度。

进一步地，所述S6中求得所述T轴关节的旋转角度具体为：

设所述T轴关节的初始角度为θ_z，则所述T轴关节的旋转角度θ_T＝θ_z-θ_S。

由上述描述可知，当S轴关节旋转时会带动T轴偏转到与原来抓取位置偏离的角度，因此需要将T轴关节转回S轴关节旋转的角度，确保抓取平台与抓取位置的抓取角度保持不变，以便抓取物料。

请参照图1，本发明的实施例一为：

一种七轴机械臂的控制方法，如图1所示，包括步骤：

S1、以七轴机械臂的行动轴的某一点为原点，以行动轴的移动方向为X轴，以平行于水平面且垂直于行动轴的方向为Y轴，以垂直于水平面的方向为Z轴，建立空间直角坐标系；

S2、获取抓取位置在空间直角坐标系的三维坐标值(X_抓,Y_抓,Z_抓)；

S3、计算以得到位于七轴机械臂的T轴的抓取平台平行于水平面且高度为Z_抓时的所有垂直转动轴关节的旋转角度；

S4、在垂直转动轴关节位于对应的旋转角度的基础上，计算以得到抓取平台的中心位于Y_抓时的水平转动轴关节的旋转角度；

S5、在垂直转动轴关节和水平转动轴关节位于对应的旋转角度的基础上，计算以得到抓取平台的中心位于X_抓时的行动轴的X轴位置X_行；

S6、计算以得到T轴关节的旋转角度，并根据每一个轴关节的旋转角度计算得到七轴机械臂的移动路径，控制七轴机械臂沿着移动路径移动至抓取平台的抓取位置。

即在本实施例中，根据七轴机械臂各轴关节的旋转特点，通过建立三维直角坐标系获取抓取位置的坐标后，首先求得对Z轴位置产生影响的垂直转动轴关节在抓取平台位于Z_抓下时的旋转角度，再求得对Y轴位置产生影响的水平转动轴关节在抓取平台位于Y_抓下的旋转角度，然后再进一步求得对X轴位置产生影响的行动轴在抓取平台位于X_抓下的行动长度X_行，最后计算T轴关节的旋转角度以便与抓取物料的抓取方向对应。由此，在将七轴机械臂各轴关节进行分类之后，每次计算同一转动方向的轴关节的旋转角度，以快速得到每个轴关节的旋转角度，从而快速计算得到七轴机械臂的移动路径，极大简化了各轴关节运算确定唯一值的算法，实现以快速精准地控制七轴机械臂移动至抓取位置。

请参照图2至图4，本发明的实施例二为：

在实施例一的基础上，在本实施例中，七轴机械臂的结构如图2所示，其中垂直转动轴关节包括L轴关节3、U轴关节4、R轴关节5和B轴关节6，水平转动轴关节包括S轴关节2，以及图2中未标出的可在X轴上直线移动的行动轴1和可旋转抓取角度的T轴关节7，其中T轴关节7上安装有抓取平台8。在本实施例中，行动轴1、S轴关节2、L轴关节3、U轴关节4、R轴关节5、B轴关节6和T轴关节7依次连接，组成完整的一个七轴机械臂。

在此基础上，在本实施例中，根据七轴机械臂的特点，由于R轴关节5的旋转会导致抓取平台8的倾斜，为了保证抓取平台8平行大地，实现更好地抓取物料，则R轴的旋转角度只能为0°或者180°。因此，可以首先根据实际应用中对抓取物料时通过向下抓取的方式还是向上抓举的方式预设R轴为0°或者180°，即R轴的旋转角度首先得以确定。然后，通过先将对Z轴的高度产生影响的L轴、U轴和B轴在XZ轴投影面上模拟画出可能旋转的角度，使最终的T轴上的抓取平台8中能够位于抓取位置所在的Z_抓高度下，即如图3所示，则在步骤S3中：

设L₂为S轴关节2到L轴关节3的垂直距离，L₃为L轴关节3到U轴关节4的距离，L₄为U轴关节4与R轴关节5的垂直距离，L₅为U轴关节4与B轴关节6的水平距离，L₆为B轴关节6到T轴关节7的距离，L₇为T轴关节7到抓取平台8的距离，L₈为L₄与L₅的斜线长，θ₁为L轴关节3的旋转角度，θ₂为U轴关节4的旋转角度，θ₃为B轴关节6的旋转角度，θ₄为L₅与L₈的夹角，θ₅为L₈与L₆的夹角，θ₆为L₈与垂直于L₃的线之间的夹角，θ₇为L₈与水平面的夹角。

其中，在本实施例中，L₂、L₃、L₄、L₅、L₆和L₇均为固定值，由七轴机械臂的规格而定，则根据三角函数可以得到θ₄＝arccos(L₅/L₈)，

若抓取平台8为向下抓取物料时，则预设R轴关节5的旋转角度为0°，有第一公式：

Z_抓＝L₂-L₃sinθ₁+L₈sinθ₇-L₆-L₇；

若抓取平台8为向上抓举物料时，则预设R轴关节5旋转角度为±180°，有第二公式：

Z_抓＝L₂-L₃sinθ₁+L₈sinθ₇+L₆+L₇。

为了进一步简化运算，预设θ₁+θ₂为预设值K，K值会影响机械臂的移动范围，在实际应用中可根据实际情况进行调整。

在本实施例中，预设K值为90°，即θ₁+θ₂＝则90°，然后通过采用逐次逼近法求出Z_抓高度下θ₁的接近值，得到L轴关节3的旋转角度θ₁，则U轴关节4的旋转角度θ₂＝90°-θ₁。

接着，由三角函数求得第三公式：

θ₁＝90°+θ₆+θ₇；

第四公式：

θ₆＝90°-θ₂-θ₄＝90°-K-θ₁-θ₄＝-θ₁-θ₄；

将第四公式代入第三公式，得到θ₇为：

θ₇＝-θ₁+θ₄+K-θ₁＝-2θ₁+θ₄+K＝-2θ₁+θ₄+90°；

则B轴关节6的旋转角度θ₃为：

θ₃＝θ₄+θ₅＝θ₄+90°-θ₇＝2θ₁+90°-K＝2θ₁。

则此时L轴关节3旋转角度θ₁、U轴关节4旋转角度θ₂和B轴关节6旋转角度θ₃均以求出。

在确定了L轴、U轴、R轴和B轴的旋转角度后，根据七轴机械臂每个轴关节无论如何转动始终处在一条水平线上的特点，如图4所示，可以求得L轴、U轴、R轴和B轴在XY轴投影面上的伸展长度L_伸，即步骤S4具体为：

设L轴、U轴、R轴和B轴在XY投影平面上的伸展长度为L_伸，则有第五公式：

L_伸＝L₁+L₃cosθ₁+L₈cosθ₇＝L₁+L_3COSθ₁+L₈cos(-2θ₁+θ₄+90°)；

其中L₁为S轴关节2到L轴关节3的水平偏移距离，则S轴关节2的旋转角度θ_S为：

θ_S＝arcsin(Y_抓/L_伸)。

即在确定了L轴、U轴、R轴和B轴的旋转角度后，剩下的行动轴1、S轴和T轴中，只有S轴的转动会影响Y轴的变化，即只有S轴关节2的旋转能使抓取平台8位于Y_抓下，因此结合已经求得的伸展长度再次通过三角函数即可快速求得S轴关节2的旋转角度。

在确定了S轴关节2的旋转角度后，则还需要算出行动轴1需要移动的长度X_行，由于L轴、U轴、R轴和B轴关节6在XY轴投影面上的伸展长度已经求出，S轴关节2的旋转角度也已经确定，即可以快速求得L轴、U轴、R轴和B轴这四轴在X轴上的投影长度，而行动轴1需要移动的长度即为抓取位置所在的X轴坐标距离原点的长度减去L轴、U轴、R轴和B轴这四轴在X轴上的投影长度。即步骤S5具体为：

设L_伸在X轴上的投影为L_X，则：

L_X＝L_伸sinθ_S；

X_行＝X_抓-L_X。

最后，还剩下T轴关节7的旋转角度未确定。在本实施例中，当S轴关节2旋转时会带动T轴偏转到与原来抓取位置偏离的角度，因此需要将T轴关节7转回S轴关节2旋转的角度，确保抓取平台8与抓取位置的抓取角度保持不变，以便抓取物料，即步骤S6中求得T轴关节7的旋转角度具体为：

设T轴关节7的初始角度为θ_z，则T轴关节7的旋转角度θ_T＝θ_z-θ_S。

请参照图5，本发明的实施例三为：

一种七轴机械臂的控制终端10，包括存储器20、处理器30以及存储在存储器20上并可在处理器30上运行的计算机程序，处理器30执行计算机程序时实现上述实施例一或实施例二中任一实施例的一种七轴机械臂的控制方法的步骤。

综上所述，本发明提供的一种七轴机械臂的控制方法及终端，根据七轴机械臂各轴关节的旋转特点，通过建立三维直角坐标系获取抓取位置的坐标后，首先求得对Z轴位置产生影响的垂直转动轴关节在抓取平台位于Z_抓下时的旋转角度，其中，由于R轴的旋转会导致抓取平台的倾斜，为了保证抓取平台平行大地，实现更好地抓取物料，则R轴的旋转角度只能为0°或者180°，可根据抓取物料的方式为向下抓取还是向上抓举而定；因此在R轴旋转角度固定的前提下，对Z轴高度会产生影响的垂直转动轴关节就剩下L轴、U轴和B轴，因此，此时只需求出L轴、U轴和B轴关节的旋转角度，再求得对Y轴位置产生影响的水平转动轴关节，即S轴关节在抓取平台位于Y_抓下的旋转角度，然后再进一步求得对X轴位置产生影响的行动轴在抓取平台位于X_抓下的行动长度X_行，最后计算T轴关节的旋转角度，确保抓取平台与抓取位置的抓取角度保持不变，以便抓取物料。即在将七轴机械臂各轴关节进行分类之后，每次计算同一转动方向的轴关节的旋转角度，以快速得到每个轴关节的旋转角度，从而快速计算得到七轴机械臂的移动路径，极大简化了各轴关节运算确定唯一值的算法，实现以快速精准地控制七轴机械臂移动至抓取位置。

以上所述仅为本发明的实施例，并非因此限制本发明的专利范围，凡是利用本发明说明书及附图内容所作的等同变换，或直接或间接运用在相关的技术领域，均同理包括在本发明的专利保护范围内。

Claims (10)

1.一种七轴机械臂的控制方法，其特征在于，包括步骤：

S1、以七轴机械臂的行动轴的某一点为原点，以所述行动轴的移动方向为X轴，以平行于水平面且垂直于所述行动轴的方向为Y轴，以垂直于水平面的方向为Z轴，建立空间直角坐标系；

S2、获取抓取位置在所述空间直角坐标系的三维坐标值(X_抓,Y_抓,Z_抓)；

S3、计算以得到位于所述七轴机械臂的T轴的抓取平台平行于水平面且高度为Z_抓时的所有垂直转动轴关节的旋转角度；

S4、在所述垂直转动轴关节位于对应的所述旋转角度的基础上，计算以得到所述抓取平台的中心位于Y_抓时的水平转动轴关节的旋转角度；

S5、在所述垂直转动轴关节和所述水平转动轴关节位于对应的所述旋转角度的基础上，计算以得到所述抓取平台的中心位于X_抓时的所述行动轴的X轴位置X_行；

S6、计算以得到所述T轴关节的旋转角度，并根据每一个轴关节的旋转角度计算得到所述七轴机械臂的移动路径，控制所述七轴机械臂沿着所述移动路径移动至所述抓取平台的抓取位置。

2.根据权利要求1所述的一种七轴机械臂的控制方法，其特征在于，所述垂直转动轴关节包括L轴关节、U轴关节、R轴关节和B轴关节，所述水平转动轴关节包括S轴关节，所述行动轴、所述S轴关节、所述L轴关节、所述U轴关节、所述R轴关节、所述B轴关节和所述T轴关节依次连接；

所述S3具体为：

设L₂为所述S轴关节到所述L轴关节的垂直距离，L₃为所述L轴关节到所述U轴关节的距离，L₄为所述U轴关节与所述R轴关节的垂直距离，L₅为所述U轴关节与所述B轴关节的水平距离，L₆为所述B轴关节到所述T轴关节的距离，L₇为所述T轴关节到所述抓取平台的距离，L₈为L₄与L₅的斜线长，θ₁为所述L轴关节的旋转角度，θ₂为所述U轴关节的旋转角度，θ₃为所述B轴关节的旋转角度，θ₄为L₅与L₈的夹角，θ₅为L₈与L₆的夹角，θ₆为L₈与垂直于L₃的线之间的夹角，θ₇为L₈与水平面的夹角；

其中，L₂、L₃、L₄、L₅、L₆和L₇均为固定值，由所述七轴机械臂的规格而定，则有θ₄＝arccos(L₅/L₈)，

若所述抓取平台为向下抓取物料时，则预设所述R轴关节的旋转角度为0°，有第一公式：

Z_抓＝L₂-L₃sinθ₁+L₈sinθ₇-L₆-L₇；

若所述抓取平台为向上抓举物料时，则预设所述R轴关节旋转角度为±180°，有第二公式：

Z_抓＝L₂-L₃sinθ₁+L₈sinθ₇+L₆+L₇；

预设θ₁+θ₂为预设值K，通过逐次逼近法求出Z_抓高度下θ₁的接近值，得到所述L轴关节的旋转角度θ₁，则所述U轴关节的旋转角度θ₂＝K-θ₁；

由三角函数求得第三公式：

θ₁＝90°+θ₆+θ₇；

第四公式：

θ₆＝90°-θ₂-θ₄＝90°-K-θ₁-θ₄；

将所述第四公式代入所述第三公式，得到θ₇为：

θ₇＝-θ₁+θ₄+K-θ₁＝-2θ₁+θ₄+K；

则所述B轴关节的旋转角度θ₃为：

θ₃＝θ₄+θ₅＝θ₄+90°-θ₇＝2θ₁+90°-K。

3.根据权利要求2所述的一种七轴机械臂的控制方法，其特征在于，所述S4具体为：

设L轴、U轴、R轴和B轴在XY投影平面上的伸展长度为L_伸，则有第五公式：

L_伸＝L₁+L₃cosθ₁+L₈cosθ₇＝L₁+L_3COSθ₁+L₈cos(-2θ₁+θ₄+K)；

其中L₁为所述S轴关节到所述L轴关节的水平偏移距离，则所述S轴关节的旋转角度θ_S为：

θ_S＝arcsin(Y_抓/L_伸)。

4.根据权利要求3所述的一种七轴机械臂的控制方法，其特征在于，所述S5具体为：

设L_伸在X轴上的投影为L_X，则：

L_X＝L_伸sinθ_S；

X_行＝X_抓-L_X。

5.根据权利要求4所述的一种七轴机械臂的控制方法，其特征在于，所述S6中求得所述T轴关节的旋转角度具体为：

设所述T轴关节的初始角度为θ_z，则所述T轴关节的旋转角度θ_T＝θ_z-θ_S。

6.一种七轴机械臂的控制终端，其特征在于，包括存储器、处理器以及存储在存储器上并可在处理器上运行的计算机程序，所述处理器执行所述计算机程序时实现以下步骤：

S1、以七轴机械臂的行动轴的某一点为原点，以所述行动轴的移动方向为X轴，以平行于水平面且垂直于所述行动轴的方向为Y轴，以垂直于水平面的方向为Z轴，建立空间直角坐标系；

S2、获取抓取位置在所述空间直角坐标系的三维坐标值(X_抓,Y_抓,Z_抓)；

S3、计算以得到位于所述七轴机械臂的T轴的抓取平台平行于水平面且高度为Z_抓时的所有垂直转动轴关节的旋转角度；

S4、在所述垂直转动轴关节位于对应的所述旋转角度的基础上，计算以得到所述抓取平台的中心位于Y_抓时的水平转动轴关节的旋转角度；

S5、在所述垂直转动轴关节和所述水平转动轴关节位于对应的所述旋转角度的基础上，计算以得到所述抓取平台的中心位于X_抓时的所述行动轴的X轴位置X_行；

S6、计算以得到所述T轴关节的旋转角度，并根据每一个轴关节的旋转角度计算得到所述七轴机械臂的移动路径，控制所述七轴机械臂沿着所述移动路径移动至所述抓取平台的抓取位置。

7.根据权利要求6所述的一种七轴机械臂的控制终端，其特征在于，所述垂直转动轴关节包括L轴关节、U轴关节、R轴关节和B轴关节，所述水平转动轴关节包括S轴关节，所述行动轴、所述S轴关节、所述L轴关节、所述U轴关节、所述R轴关节、所述B轴关节和所述T轴关节依次连接；

所述S3具体为：

设L₂为所述S轴关节到所述L轴关节的垂直距离，L₃为所述L轴关节到所述U轴关节的距离，L₄为所述U轴关节与所述R轴关节的垂直距离，L₅为所述U轴关节与所述B轴关节的水平距离，L₆为所述B轴关节到所述T轴关节的距离，L₇为所述T轴关节到所述抓取平台的距离，L₈为L₄与L₅的斜线长，θ₁为所述L轴关节的旋转角度，θ₂为所述U轴关节的旋转角度，θ₃为所述B轴关节的旋转角度，θ₄为L₅与L₈的夹角，θ₅为L₈与L₆的夹角，θ₆为L₈与垂直于L₃的线之间的夹角，θ₇为L₈与水平面的夹角；

其中，L₂、L₃、L₄、L₅、L₆和L₇均为固定值，由所述七轴机械臂的规格而定，则有θ₄＝arccos(L₅/L₈)，

若所述抓取平台为向下抓取物料时，则预设所述R轴关节的旋转角度为0°，有第一公式：

Z_抓＝L₂-L₃sinθ₁+L₈sinθ₇-L₆-L₇；

若所述抓取平台为向上抓举物料时，则预设所述R轴关节旋转角度为±180°，有第二公式：

Z_抓＝L₂-L₃sinθ₁+L₈sinθ₇+L₆+L₇；

预设θ₁+θ₂为预设值K，通过逐次逼近法求出Z_抓高度下θ₁的接近值，得到所述L轴关节的旋转角度θ₁，则所述U轴关节的旋转角度θ₂＝K-θ₁；

由三角函数求得第三公式：

θ₁＝90°+θ₆+θ₇；

第四公式：

θ₆＝90°-θ₂-θ₄＝90°-K-θ₁-θ₄；

将所述第四公式代入所述第三公式，得到θ₇为：

θ₇＝-θ₁+θ₄+K-θ₁＝-2θ₁+θ₄+K；

则所述B轴关节的旋转角度θ₃为：

θ₃＝θ₄+θ₅＝θ₄+90°-θ₇＝2θ₁+90°-K。

8.根据权利要求7所述的一种七轴机械臂的控制终端，其特征在于，所述S4具体为：

设L轴、U轴、R轴和B轴在XY投影平面上的伸展长度为L_伸，则有第五公式：

L_伸＝L₁+L₃cosθ₁+L₈cosθ₇＝L₁+L_3COSθ₁+L₈cos(-2θ₁+θ₄+K)；

其中L₁为所述S轴关节到所述L轴关节的水平偏移距离，则所述S轴关节的旋转角度θ_S为：

θ_S＝arcsin(Y_抓/L_伸)。

9.根据权利要求8所述的一种七轴机械臂的控制终端，其特征在于，所述S5具体为：

设L_伸在X轴上的投影为L_X，则：

L_X＝L_伸sinθ_S；

X_行＝X_抓-L_X。

10.根据权利要求9所述的一种七轴机械臂的控制终端，其特征在于，所述S6中求得所述T轴关节的旋转角度具体为：

设所述T轴关节的初始角度为θ_z，则所述T轴关节的旋转角度θ_T＝θ_z-θ_S。

Images