一种多关节机器人位姿误差自补偿的码垛方法及其码垛系统技术方案

本发明专利技术公开了一种多关节机器人位姿误差自补偿的码垛方法及其码垛系统，属于自动化领域。包括如下步骤：建立机器人基座坐标系；根据起始点和结束点位置和运动矩阵关系，计算合运动矢量；通过激光追踪仪检测执行末端的实际位姿；计算合运动矢量作用下的理论位姿与实际位姿；重新修正、并输出的合运动矢量。本发明专利技术通过在执行末端安装多个靶球，测量出至少6个已知实际位置，根据执行末端坐标系与基座坐标系之间的关系，确定执行末端的实际位姿信息，与理论位姿信息进行对比，对合成运动矢量进行修正并输出，完成码垛任务。解决了多关节机器人的执行末端实际位姿与理论位姿不匹配问题。

A stacking method and stacking system for self compensation of pose error of multi joint robot

The invention discloses a stacking method and a stacking system for self compensation of pose error of a multi joint robot, belonging to the field of automation. It includes the following steps: establish the robot base coordinate system; calculate the combined motion vector according to the relationship between the starting point and the ending point position and the motion matrix; detect the actual position and attitude of the end through the laser tracker; calculate the theoretical position and the actual position and attitude under the action of the combined motion vector; re correct and output the combined motion vector. By installing a plurality of target balls at the execution end, the invention measures at least 6 known actual positions, determines the actual position and posture information of the execution end according to the relationship between the execution end coordinate system and the base coordinate system, compares with the theoretical position and posture information, corrects and outputs the resultant motion vector, and completes the stacking task. The problem of mismatch between the actual position and the theoretical position of the multi joint robot is solved.

【技术实现步骤摘要】
一种多关节机器人位姿误差自补偿的码垛方法及其码垛系统
本专利技术属于自动化领域，尤其是一种多关节机器人位姿误差自补偿的码垛方法及其码垛系统。
技术介绍
码垛机器人是机械与计算机程序有机结合的产物。为现代生产提供了更高的生产效率。在物流运输行业有着广泛的应用，不仅节省了劳动力，节省空间，而且提高了运输效率、降低了运输成本。其中，现有的码垛机器人多大采用串联多关节机器人，具有很高的自由度，几乎适合于任何轨迹或角度的工作，可以自由编程，完成全自动化的码垛工作，提高生产效率。机器人根据离线加工程序给定的理论位姿来完成码垛定位，但是由于串联多关节机器人安装姿态、关节运动误差、理论加工模型与实际加工模型等原因的存在。在多次累积运动、工作后，机器人在加工时通常无法保证机器人末端执行器的运动方向与目标执行位置表面的法向一致性，会存在一定的姿态角度偏差，即机器人的执行末端实际位姿与理论位姿不匹配。当垛堆存在一定偏差时，在运输和后期固定过程中，很容易造成垛堆坍塌，形成安全隐患。
技术实现思路
专利技术目的：提供一种多关节机器人位姿误差自补偿的码垛方法及其码垛系统，以解决上述
技术介绍
中所涉及的问题。技术方案：一种多关节机器人位姿误差自补偿的码垛方法，包括如下步骤：S1、建立机器人基座坐标系；S2、根据起始点和结束点位置和运动矩阵关系，计算合运动矢量；S3、通过激光追踪仪检测执行末端的实际位姿；S4、计算合运动矢量作用下的理论位姿与实际位姿；S5、重新修正、并输出的合运动矢量。在进一步实施过程中，所述S2步骤中建立机器人基座坐标系具体方法为：以机器人基座中心旋转轴为Z轴，机器人中心旋转轴与所述执行目标的连线为X轴，根据右手定则，建立笛卡尔坐标系。在进一步实施过程中，所述S2步骤中计算合运动矢量的具体算法为：S21、以关节连接处的销轴为坐标系的基准，以销轴的旋转轴线分别建立旋转关节坐标系的Z轴，具体为Zi-2，Zi-1，Zi；S22、以Zi-1轴和Zi轴的公共法线为方向，作为Xi轴，根据右手定则，建立笛卡尔坐标系Ti；S23、得到从i-1销轴到i销轴时的运动学矩阵关系Ai其中，为公法线长度；为坐标系原点与坐标系原点沿着公法线的距离；为Zi-1到Zi之间的夹角，为从坐标系Ti-1到Ti的变化过程中所旋转的角度；S24、同样的，分别建立其它各个旋转轴关节坐标系，分别为、、……，得到执行末端坐标系在机器人基座坐标系的位姿变化为其中，所述为初始变化时，执行末端坐标系在机器人的基座坐标系的D-H参数的位姿。在进一步实施过程中，所述执行末端坐标系在机器人的基座坐标系的D-H参数的位姿：其中，，，，，，，，，为执行末端坐标系在基座坐标系的姿态变换分量，，，为执行末端坐标系在基座坐标系的理论位置变换分量。在进一步实施过程中，所述S3步骤中通过激光追踪仪检测执行末端的实际位姿具体算法为：S31、在执行末端上安装靶球，激光追踪仪主机发射激光束到达执行末端后，通过靶球上的反射镜将激光束重新返回至激光跟踪仪主机，形成光学回路；S32、根据激光跟踪仪主机中发射点的位置和激光发射方向，计算出执行末端的位置变换分量为：=其中，X，Y，Z为执行末端中某一点在基座坐标系的实际位置变换分量，为激光追踪仪主机到执行末端的距离，(X1，Y1，Z1)为激光发射原点，（m，n，s）为激光发射方向；S34、根据在基座坐标系的实际位置变换分量，以及其相互之间的实际距离关系，通过解三角形，求出某一点在执行末端坐标系中的实际位置变换分量；S35、至少选取执行末端6个已知实际位置的点，将其在执行平台坐标系的位置变换分量和在基座坐标系中的位置变换分量代入中，求解未知变量执行末端坐标系在基座坐标系中的位置变换分量，和执行末端坐标系依次绕基座坐标系的X，Y，Z轴旋转的卡尔丹角；S36、最后，得出执行末端的实际位姿为。在进一步实施过程中，所述S5步骤和S6步骤的具体方法为，执行多段合运动矢量，通过对比理论位置和实际位置之间的偏差，修正合运动矢量，并输出修正后的合运动矢量。在进一步实施过程中，不仅限于码垛，还可以应用于基座固定的多关节机器人计算运动合成和位姿误差修正。所述机器人至少应用于码垛、搬运、制孔、焊接、涂胶领域其中之一。一种多关节机器人位姿误差自补偿的码垛系统，包括如下模块：用于建立机器人基座坐标系的第一模块；用于根据起始点和结束点位置和运动矩阵关系，计算合运动矢量的第二模块；用于通过激光追踪仪检测执行末端的实际位姿的第三模块；用于计算合运动矢量作用下的理论位姿与实际位姿的第四模块；用于重新修正、并输出的合运动矢量第五模块。在进一步实施过程中，所述第一模块以机器人基座中心旋转轴为Z轴，机器人中心旋转轴与所述执行目标的连线为X轴，根据右手定则，建立笛卡尔坐标系。在进一步实施过程中，所述第二模块以关节连接处的销轴为坐标系的基准，以销轴的旋转轴线分别建立旋转关节坐标系的Z轴，具体为Zi-2，Zi-1，Zi；以Zi-1轴和Zi轴的公共法线为方向，作为Xi轴，根据右手定则，建立笛卡尔坐标系Ti；得到从i-1销轴到i销轴时的运动学矩阵关系其中，为公法线长度；为坐标系原点与坐标系原点沿着公法线的距离；为Zi-1到Zi之间的夹角，为从坐标系Ti-1到Ti的变化过程中所旋转的角度；同样的，分别建立其它各个旋转轴关节坐标系，分别为、、……，得到执行末端坐标系在机器人基座坐标系的位姿变化，即理论位姿为其中，所述为初始变化时，执行末端坐标系在机器人的基座坐标系的D-H参数的位姿：其中，，，，，，，，，为执行末端坐标系在基座坐标系的姿态变换分量，，，为执行末端坐标系在基座坐标系的位置变换分量。在进一步实施过程中，所述第三模块在执行末端上安装靶球，激光追踪仪主机发射激光束到达执行末端后，通过靶球上的反射镜将激光束重新返回至激光跟踪仪主机，形成光学回路；根据激光跟踪仪主机中发射点的位置和激光发射方向，计算出执行末端的位置变换分量为：=其中，X，Y，Z为执行末端中某一点在基座坐标系的实际位置变换分量，为激光追踪仪主机到执行末端的距离，(X1，Y1，Z1)为激光发射原点，（m，n，s）为激光发射方向；根据在基座坐标系的实际位置变换分量，以及其相互之间的实际距离关系，通过解三角形，求出某一点在执行末端坐标系中的实际位置变换分量；至少选取执行末端6个已知实际位置的点，将其在执行平台坐标系的位置变换分量和在基座坐标系中的位置变换分量代入中，求解未知变量执行末端坐标系在基座坐标系中的位置变换分量，和执行末端坐标系依次绕基座坐标系的X，Y，Z轴旋转的卡尔丹角；最后，得出执行末端的实际位姿为。在进一步实施过程中，所述第四模块和第五模块执行多段合运动矢量，通过对比理论位置和实际位置之间的偏差，修本文档来自技高网...

【技术保护点】
1.一种多关节机器人位姿误差自补偿的码垛方法，其特征在于，包括如下步骤：/nS1、建立机器人基座坐标系；/nS2、根据起始点和结束点位置和运动矩阵关系，计算合运动矢量；/nS3、通过激光追踪仪检测执行末端的实际位姿；/nS4、计算合运动矢量作用下的理论位姿与实际位姿；/nS5、重新修正、并输出的合运动矢量。/n

【技术特征摘要】
1.一种多关节机器人位姿误差自补偿的码垛方法，其特征在于，包括如下步骤：
S1、建立机器人基座坐标系；
S2、根据起始点和结束点位置和运动矩阵关系，计算合运动矢量；
S3、通过激光追踪仪检测执行末端的实际位姿；
S4、计算合运动矢量作用下的理论位姿与实际位姿；
S5、重新修正、并输出的合运动矢量。


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


3.根据权利要求1所述多关节机器人位姿误差自补偿的码垛方法，其特征在于，所述S2步骤中计算合运动矢量的具体算法为：
S21、以关节连接处的销轴为坐标系的基准，以销轴的旋转轴线分别建立旋转关节坐标系的Z轴，具体为Zi-2，Zi-1，Zi；
S22、以Zi-1轴和Zi轴的公共法线为方向，作为Xi轴，根据右手定则，建立笛卡尔坐标系Ti；
S23、得到从i-1销轴到i销轴时的运动学矩阵关系Ai



其中，为公法线长度；为坐标系原点与坐标系原点沿着公法线的距离；为Zi-1到Zi之间的夹角，为从坐标系Ti-1到Ti的变化过程中所旋转的角度；
S24、同样的，分别建立其它各个旋转轴关节坐标系，分别为、、……，得到执行末端坐标系在机器人基座坐标系的位姿变化为



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


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



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


5.根据权利要求1所述多关节机器人位姿误差自补偿的码垛方法，其特征在于，所述S3步骤中通过激光追踪仪检测执行末端的实际位姿具体算法为：
S31、在执行末端上安装靶球，激光追踪仪主机发射激光束到达执行末端后，通过靶球上的反射镜将激光束重新返回至激光跟踪仪主机，形成光学回路；
S32、根据激光跟踪仪主机中发射点的位置和激光发射方向，计算出执行末端的位置变换分量为：

=
其中，X，Y，Z为执行末端中某一点在基座坐标系的实际位置变换分量，为激光追踪仪主机到执行末端的距离，(X1，Y1，Z1)为激光发射原点，（m，n，s）为激光发射方向；
S34、根据在基座坐标系的实际位置变换分量，以及其相互之间的实际距离关系，通过解三角形，求出某一点在执行末端坐标系中的实际位置变换分量；
S35、至少选取执行末端6个已知实际位置的点，将其在执行平台坐标系的位置变换分量和在基座坐标系中的位置变换分量代入



中，求解未知变量执行末端坐标系在基座坐标系中的，和执行末端坐标系依次绕基座坐标系的X，Y，Z轴旋转的卡尔丹角；
S36、最后，得出执行末端的实际位姿为。


6.根据权利要求1所述多关节机器人位姿误差自补偿的码垛方法，其特征在于，...

【专利技术属性】
技术研发人员：吴超，陈玉振，张洁，
申请(专利权)人：南京博约智能科技有限公司，
类型：发明
国别省市：江苏;32