[发明专利]一种多关节机器人位姿误差自补偿的码垛方法及其码垛系统

权利要求书

1.一种多关节机器人位姿误差自补偿的码垛方法，其特征在于，包括如下步骤：

S1、建立机器人基座坐标系；

S2、根据起始点和结束点位置和运动矩阵关系，计算合运动矢量；

S3、通过激光追踪仪检测执行末端的实际位姿；

S4、计算合运动矢量作用下的理论位姿与实际位姿；

S5、重新修正、并输出的合运动矢量。

2.根据权利要求1所述多关节机器人位姿误差自补偿的码垛方法，其特征在于，所述S2步骤中建立机器人基座坐标系具体方法为：以机器人基座中心旋转轴为Z轴，机器人中心旋转轴与所述执行目标的连线为X轴，根据右手定则，建立笛卡尔坐标系。

3.根据权利要求1所述多关节机器人位姿误差自补偿的码垛方法，其特征在于，所述S2步骤中计算合运动矢量的具体算法为：

S21、以关节连接处的销轴为坐标系的基准，以销轴的旋转轴线分别建立旋转关节坐标系的Z轴，具体为Z_i-2，Z_i-1，Z_i；

S22、以Z_i-1轴和Z_i轴的公共法线为方向，作为X_i轴，根据右手定则，建立笛卡尔坐标系T_i；

S23、得到从i-1销轴到i销轴时的运动学矩阵关系A_i

其中，为公法线长度；为坐标系原点与坐标系原点沿着公法线的距离；为Z_i-1到Z_i之间的夹角，为从坐标系T_i-1到T_i的变化过程中所旋转的角度；

S24、同样的，分别建立其它各个旋转轴关节坐标系，分别为、、……，得到执行末端坐标系在机器人基座坐标系的位姿变化为

其中，所述为初始变化时，执行末端坐标系在机器人的基座坐标系的D-H参数的位姿。

4.根据权利要求3所述多关节机器人位姿误差自补偿的码垛方法，其特征在于，所述执行末端坐标系在机器人的基座坐标系的D-H参数的位姿：

其中，，，，，，，，，为执行末端坐标系在基座坐标系的姿态变换分量，，，为执行末端坐标系在基座坐标系的理论位置变换分量。

5.根据权利要求1所述多关节机器人位姿误差自补偿的码垛方法，其特征在于，所述S3步骤中通过激光追踪仪检测执行末端的实际位姿具体算法为：

S31、在执行末端上安装靶球，激光追踪仪主机发射激光束到达执行末端后，通过靶球上的反射镜将激光束重新返回至激光跟踪仪主机，形成光学回路；

S32、根据激光跟踪仪主机中发射点的位置和激光发射方向，计算出执行末端的位置变换分量为：

=

其中，X，Y，Z为执行末端中某一点在基座坐标系的实际位置变换分量，为激光追踪仪主机到执行末端的距离，(X₁，Y₁，Z₁)为激光发射原点，（m，n，s）为激光发射方向；

S34、根据在基座坐标系的实际位置变换分量，以及其相互之间的实际距离关系，通过解三角形，求出某一点在执行末端坐标系中的实际位置变换分量；

S35、至少选取执行末端6个已知实际位置的点，将其在执行平台坐标系的位置变换分量和在基座坐标系中的位置变换分量代入

中，求解未知变量执行末端坐标系在基座坐标系中的，和执行末端坐标系依次绕基座坐标系的X，Y，Z轴旋转的卡尔丹角；

S36、最后，得出执行末端的实际位姿为。

6.根据权利要求1所述多关节机器人位姿误差自补偿的码垛方法，其特征在于，所述S5步骤和S6步骤的具体方法为，执行多段合运动矢量，通过对比理论位置和实际位置之间的偏差，修正合运动矢量，并输出修正后的合运动矢量。

7.根据权利要求1所述多关节机器人位姿误差自补偿的码垛方法，其特征在于，不仅限于码垛，还可以应用于基座固定的多关节机器人计算运动合成和位姿误差修正。