[发明专利]一种七轴机械臂的控制方法及终端

权利要求书

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。