[发明专利]一种特定构型六轴工业机器人的运动控制方法

摘要

本发明公开了一种特定构型六轴工业机器人的运动控制方法，包括建立工业机器人的连杆坐标系，获得工业机器人的七根杆件和六个轴之间的几何关系；获取工业机器人对应的G指令文件后，读取工业机器人的目标运动位置；根据该目标运动位置以及工业机器人的几何关系，依次计算工业机器人的六个轴的关节角度；分别将计算出的六个关节角度转化成每个轴对应的运动脉冲数后，发送到工业机器人的驱动器；驱动器根据接收的每个轴对应的运动脉冲数，依次驱动对应的电机带动相应的轴进行运动。本发明基于几何关系，对工业机器人进行运动逆解，从而实现对工业机器人的运动控制，运算速度快，计算效率高且控制效率高，可广泛应用于工业机器人的控制领域中。

主权项

一种特定构型六轴工业机器人的运动控制方法，其特征在于，包括以下步骤：S1、建立工业机器人的连杆坐标系，获得工业机器人的七根杆件和六个轴之间的几何关系；S2、获取工业机器人对应的G指令文件后，读取工业机器人的目标运动位置；S3、根据该目标运动位置以及工业机器人的几何关系，依次计算工业机器人的六个轴的关节角度；S4、分别将计算出的六个关节角度转化成每个轴对应的运动脉冲数后，发送到工业机器人的驱动器；S5、驱动器根据接收的每个轴对应的运动脉冲数，依次驱动对应的电机带动相应的轴进行运动；所述步骤S1，其具体为：建立工业机器人的连杆坐标系，获得工业机器人的七根杆件和六个轴之间的几何关系，设七根杆件的长度分别为L1、L2、L3、L4、L5、L6和L7，六个轴的关节角度分别为θ1、θ2、θ3、θ4、θ5和θ6；所述步骤S3，包括S31～S36：S31、获取目标运动位置的姿态作为工业机器人的腕关节点的姿态[n o a]，同时获取目标运动位置的位置矢量作为工业机器人的腕关节点的坐标(Px,Py,Pz)，其中，n、o和a分别表示腕关节点的法向矢量、方位矢量和接近矢量，且n＝o×a，Px、Py和Pz分别表示腕关节点的X轴、Y轴和Z轴坐标值；S32、根据工业机器人的几何关系，采用下式，计算工业机器人的关节角度θ1、θ2、θ3：上式中，Lcf、Lcg、Ldf满足下式：Lcf=Lcg2+Lfg2Lcg=Px2+Py2-L2Ldf=L42+L52Lfg=Pz-L1]]>S32、判断工业机器人的第四个轴与第六个轴是否共线，若是，则执行步骤S33后结束，反之，直接执行步骤S34；S33、根据下式计算获得工业机器人的关节角度θ4、θ5、θ6：θ4=0θ5=0θ6=Angle(o,Normal)]]>上式中，Normal表示工业机器人的前五个杆件所在的工作平面的法向矢量，且Normal＝Vector(sinθ1,‑cosθ1,0)；S34、根据连杆变换公式构建代表工业机器人的第六轴的Z轴转轴方向的目标函数：上式中，0Z′5表示工业机器人的第六轴的Z轴转轴方向的目标函数，分别表示工业机器人的坐标系之间的变换矩阵；S35、将下式的条件因子代入目标函数的表达式中求解目标函数值，进而将求解获得的目标函数值与第六轴的目标值相等时的条件因子作为关节角度θ4、θ5的最优解：θ4=min(Angle(Normal,-Z05′),Angle(Normal,-Z05′′))θ5=Angle(Z04,Z06)]]>上式中，0Z′5和0Z″5分别表示工业机器人的第五轴的两个可能的Z轴转轴方向，0Z4表示工业机器人的第四轴的Z轴转轴方向，0Z6表示工业机器人的第六轴的Z轴转轴方向；S36、根据关节角度θ4和θ5的最优解，对腕关节点的法向矢量和方位矢量进行更新后，根据下式计算获得工业机器人的关节角度θ6：θ6＝Angle(o',Normal)上式中，o'表示更新后的腕关节点的方位矢量。