[发明专利]一种机器人自动补偿运动控制方法

权利要求书

1.一种机器人自动补偿运动控制方法，包括机械臂和安装于所述机械臂上的执行元件，所述机械臂包括夹持所述执行元件的夹持装置和驱动所述夹持装置摆动的驱动臂；所述驱动臂包括第一关节和与所述第一关节连接的第二关节，所述第一关节驱动所述夹持装置摆动，所述第二关节驱动所述第一关节摆动；所述第一关节包括第一线性电机和第二线性电机；所述第二关节包括第三线性电机和第四线性电机；所述夹持装置设置有驱动所述执行元件直线运动的第五线性电机和驱动所述执行元件的活塞运动的第六线性电机；其特征在于，包括如下步骤：

步骤一：控制全部关节线性电机进行位置标定，完成运动初始化；

步骤二：选定执行元件末端初始位置，并控制机械臂运动至指定末端初始位置；

步骤三：设定机器人各个关节的运动路径，即设定各线性电机的位移量序列；

步骤四：控制机械臂驱动执行元件沿着设定的运动路径运动，当机械臂末端执行元件进行旋转运动时，进入自动补偿运动控制模式，综合设定位移量与自动补偿位移量，获得最终执行的实际线性位移量；

步骤五：根据步骤四中计算获得的实际线性位移量，驱动机械臂执行运动。

2.根据权利要求1所述的一种机器人自动补偿运动控制方法，其特征在于，在所述步骤四中，执行元件末端的自动补偿运动中的实际线性位移量计算如下：

S4.1：获取机械臂内部硬件参数dm1,dm2,h1,h2,Ltool；其中，dm1为第三线性电机到第四线性电机的水平轴的垂直距离；dm2为第一线性电机到第二线性电机的水平轴的垂直距离；h1为第三线性电机和第四线性电机的水平中点到执行元件的垂直距离；h2为第一线性电机和第二线性电机的水平中点到执行元件的垂直距离；Ltool为执行元件的长度；

S4.2：计算执行元件末端的角度，具体为：

式中，θ1为第一关节与垂直面形成的角度；dL1为第一线性电机的当前位置；dL2为第二线性电机的当前位置；θ2为第二关节与垂直面形成的角度；dL3为第三线性电机的当前位置；dL4为第四线性电机的当前位置；

S4.3：计算执行元件相对于机器人坐标系的当前位置，以执行元件末端位置为自动补偿点坐标，具体如下：

z＝((Ltool+dL5)*cos(θ1)+h1*sin(θ1))*cos(θ2)+h2*sin(θ2)

式中，x为执行元件在机器人空间坐标系下的x轴坐标位置；y为执行元件在机器人空间坐标系下的y轴坐标位置；z为执行元件在机器人空间坐标系下的z轴坐标位置；dL5为第五线性电机的当前位置；

S4.4：计算执行元件末端的自动补偿线性位移量，具体如下：

式中，ΔL1、ΔL2、ΔL3、ΔL4、ΔL5分别为第一线性电机、第二线性电机、第三线性电机、第四线性电机和第五线性电机的目标补偿位移量；M为补偿矩阵；

式中补偿矩阵具体为：

M＝[M₁ M₂ M₃ M₄ M₅]

式中，θ2为第二关节与垂直面形成的角度；L1为第一线性电机的设定线性位移量；L2为第二线性电机的设定线性位移量；dm1为第三线性电机到第一线性电机或第二线性电机的水平轴的垂直距离；L3为第三线性电机的设定线性位移量；L4为第四线性电机的设定线性位移量；L5为第五线性电机的设定线性位移量；dm2为第五线性电机到第三线性电机或第四线性电机的水平轴的垂直距离；

S4.5：由设定位移量与自动补偿位移量，计算获得最终执行的实际线性位移量；具体如下：

L1'＝L1+ΔL1

L2'＝L2+ΔL2

L3'＝L3+ΔL3

L4'＝L4+ΔL4

L5'＝L5+ΔL5

式中，L1、L2、L3、L4、L5为第一线性电机、第二线性电机、第三线性电机、第四线性电机和第五线性电机原本的设定线性位移量，L1’、L2’、L3’、L4’、L5’为第一线性电机、第二线性电机、第三线性电机、第四线性电机和第五线性电机补偿后的实际线性位移量。