CN111745623B - 五自由度混联机器人末端位姿误差检测补偿系统及方法 - Google Patents

Abstract

本发明公开了一种五自由度混联机器人末端位姿误差检测补偿系统，包括混联机器人本体以及安装在混联机器人本体末端的搅拌摩擦焊搅拌头，混联机器人本体包括过约束三自由度的并联机构及A/C轴双摆角头，还包括视觉定位系统及激光测距仪；视觉定位系统用于测量搅拌头主轴末端的三维坐标值，其包括至少一组双目相机；激光测距仪用于采集其发射端到工件表面的距离；在搅拌头主轴的轴肩上设有环形转动盘；环形转动盘在驱动装置驱动下围绕搅拌头主轴轴线匀速转动，激光测距仪固定在环形转动盘上。本发明还公开了一种五自由度混联机器人末端位姿误差检测补偿方法，其可适用于搅拌摩擦焊混联机器人的误差精确动态检测及补偿。

Description

五自由度混联机器人末端位姿误差检测补偿系统及方法

技术领域

本发明涉及一种机器人末端位姿误差检测补偿系统及方法，特别涉及一种五自由度混联机器人末端位姿误差检测补偿系统及方法。

背景技术

目前，机器人发展迅速，并且广泛应用于各行各业。现代自动化领域高精技术的快速发展，对机器人的性能要求越来越高。混联机器人由于刚度大，性能好的特点，在工业自动化加工领域应用颇多。评价机器人的性能指标众多，其中较为重要的是绝对定位精度和加工轨迹的重复定位精度。其中位姿的准确性综合反映了机器人的控制精度以及整个机电系统的性能。因此补偿机器人在加工过程中的位姿误差尤为重要。

公开号为CN108297101A的中国专利文献公开了一种多关节臂串联机器人末端位姿误差检测和动态补偿方法，该方法利用倾角传感器模块获得机器人末端位姿误差。但是由于串联关节轴的实际参数值跟理论值存在误差，所以获得的位姿误差不够精确。

发明内容

本发明为解决公知技术中存在的技术问题而提供一种精确的五自由度混联机器人末端位姿误差检测补偿系统及方法。

本发明为解决公知技术中存在的技术问题所采取的技术方案是：一种五自由度混联机器人末端位姿误差检测补偿系统，包括混联机器人本体以及安装在混联机器人本体末端的搅拌摩擦焊搅拌头，混联机器人本体包括过约束三自由度的并联机构及A/C轴双摆角头，其中，并联机构包括动平台及与动平台铰接且沿轴向运动的第一至第三主动臂；双摆角头的C轴与动平台转动连接；双摆角头的A轴与搅拌头主轴的轴承座固接，且A轴轴线与搅拌头主轴轴线重合，还包括视觉定位系统及激光测距仪；视觉定位系统用于测量搅拌头主轴末端的三维坐标值，其包括至少一组双目相机；激光测距仪用于采集其发射端到工件表面的距离；在搅拌头主轴的轴肩上设有环形转动盘；环形转动盘在驱动装置驱动下围绕搅拌头主轴轴线匀速转动，激光测距仪固定在环形转动盘上。

进一步地，驱动装置包括电机和齿轮；环形转动盘的周侧面设有与齿轮啮合的齿；电机固定在搅拌头主轴的轴肩上，电机的输出轴与齿轮固接。

进一步地，在搅拌头主轴的轴肩上固接有环形滑轨，环形转动盘吊挂在环形滑轨上并与环形滑轨滑动连接。

进一步地，在混联机器人本体的固定基座上固接有支架，视觉定位系统的双目相机固定在支架上。

本发明还提供了一种利用上述的五自由度混联机器人末端位姿误差检测补偿系统的五自由度混联机器人末端位姿误差检测补偿方法，该方法为：

在基坐标系下，标定激光测距仪发射端初始坐标、搅拌头主轴末端的初始坐标以及A、C轴初始角度；

设位于工件表面上的激光照射点为检测点，设激光测距仪测量值第一次出现峰值的时刻为T₁,设T₁时刻环形转动盘绕搅拌头主轴轴线转动的角度为α；设第一次峰值对应的检测点为A点，设激光测距仪测量值第二次出现峰值的时刻为T₂,设T₂时刻环形转动盘绕搅拌头主轴轴线转动的角度为β；设第二次峰值对应的检测点为B点；

读取T₁至T₂时间段的视觉定位系统测得的搅拌头主轴末端在基坐标系下的实际坐标值，由搅拌头主轴末端与激光测距仪发射端两者初始时刻的相对坐标、环形转动盘的转速，计算得到T₁至T₂时间段的激光测距仪发射端在基坐标系下的实际坐标值；由激光测距仪在T₁至T₂时间段测得的其发射端到工件表面检测点的距离，进一步得到T₁至T₂时间段的检测点在基坐标系下的坐标；

采样T₁至T₂时间段且不含A、B点的检测点，由采样的检测点在基坐标系下的坐标值拟合工件平面；从而得到工件平面法向量，由工件平面法向量得到工件平面与搅拌头主轴的夹角，设该夹角为

以搅拌头末端为原点，焊缝方向为X轴，工件平面法向量为Z轴建立坐标系O₂；令γ＝α-β，由

和γ，计算得到搅拌头主轴轴线在坐标系O₂中的坐标，利用坐标系O₂和基坐标系之间的坐标转换，计算得到搅拌头主轴轴线在基坐标系下的姿态角，姿态角包括主轴轴线绕基坐标系的X轴转动的转角及绕基坐标系的Y轴转动的转角；

设T₁时刻搅拌头主轴垂直于工件表面，由T₂时刻视觉定位系统测得的搅拌头主轴末端在基坐标系下的坐标实际值以及搅拌头主轴轴线在基坐标系下的姿态角，根据五自由度混联机器人逆运动学方程，得到T₂时刻第一至第三主动臂的轴向位移实际值及A、C轴转角实际值；由位姿指令得到对应T₂时刻第一至第三主动臂的轴向位移给定值以及A、C轴转角给定值，进一步获得T₂时刻第一至第三主动臂的轴向位移误差值以及A、C轴转角误差值；将得到的误差值作为补偿值。

本发明具有的优点和积极效果是：本发明基于激光测距仪和视觉定位系统，将混联加工机器人加工过程中由于各种因素导致的末端位姿误差进行检测和动态补偿，实时修正机器人末端位姿，简化了工业机器人末端位姿测量方式，大大提高了加工精度，有效弥补了机器人半闭环控制的缺陷，优化了机器人的实时性能，显著提高了智能化水平，该方法可适用于搅拌摩擦焊混联机器人的误差检测及补偿。

附图说明

图1是本发明的一种五自由度混联机器人末端位姿误差检测补偿系统结构示意图。

图2是本发明的一种五自由度混联机器人末端位姿误差检测补偿方法工作流程图。

图中：1、搅拌头主轴；2、第一相机；3、第二相机；4、工件；5、工作台；6、激光测距仪；7、混联机器人本体。

具体实施方式

为能进一步了解本发明的发明内容、特点及功效，兹列举以下实施例，并配合附图详细说明如下：

请参见图1至图2，一种五自由度混联机器人末端位姿误差检测补偿系统，包括混联机器人本体7以及安装在混联机器人本体7末端的搅拌摩擦焊搅拌头，混联机器人本体7包括过约束三自由度的并联机构及A/C轴双摆角头，其中，并联机构包括动平台及与动平台铰接且沿轴向运动的第一至第三主动臂；双摆角头的C轴与动平台转动连接；双摆角头的A轴与搅拌头主轴1的轴承座固接，且A轴轴线与搅拌头主轴1的轴线重合，还包括视觉定位系统及激光测距仪6；视觉定位系统用于测量搅拌头主轴1末端的三维坐标值，其包括至少一组双目相机；激光测距仪6用于采集其发射端到工件4表面的距离；在搅拌头主轴1的轴肩上设有环形转动盘；环形转动盘在驱动装置驱动下围绕搅拌头主轴1的轴线匀速转动，激光测距仪6固定在环形转动盘上。

五自由度混联机器人由三自由度过约束空间并联机构与一两自由度A/C轴双摆角头串接而成，其中并联机构的第一至第三主动臂形成三条主动支链，第一至第三主动臂的一端可通过虎克铰链铰接在静平台上，另一端可通过球铰链铰接在动平台上，动平台后端可设有从动支撑臂；动平台前端连接A/C轴双摆角头，其中双摆角头的C轴与动平台转动连接。

视觉定位系统的工作原理为：先用双目相机进行图像信息采集，将采集的图像信息进行目标特征提取处理，基于双目测距原理，将采集到的目标图像信息和目标的实际位置联系起来，完成目标的三维坐标位置测量。

视觉定位系统，其包括至少一组双目相机，其中一组双目相机包括第一相机2和第二相机3，可在第一相机2建立坐标系P₁，在第二相机3建立坐标系P₂，经过图片矫正和双目标定后，可对坐标系P₁、坐标系P₂相对机器人基坐标系进行标定。获得搅拌头末端在坐标系P₁及坐标系P₂中的坐标，通过坐标转换，可获得搅拌头末端在基坐标系下的坐标。

视觉定位系统可采用现有技术中的视觉定位系统，如基恩士XG-X系列视觉定位系统等。可采用常规技术手段对视觉定位系统的双目相机进行内外参数标定。

将工件4放在工作台5上，激光测距仪6围绕搅拌头主轴1的轴线旋转，其发出激光束，可检测搅拌头的轴肩等相对工件4表面的高度。激光测距仪6可采用无线激光测距仪6，其将检测信号通过无线方式发送给混联机器人的控制系统。

视觉定位系统和激光测距仪6将检测的数据作为反馈数据发送至混联机器人的数控系统中，混联机器人的数控系统对接收的反馈数据进行处理，得到搅拌头主轴1末端的位姿实际数据，其与位姿给定数据进行相减，得到搅拌头主轴1末端的位姿偏差数据，其将位姿偏差数据作为补偿数据，进行补偿控制。

进一步地，驱动环形转动盘绕搅拌头主轴1的轴线旋转的驱动装置可包括电机和齿轮；环形转动盘的周侧面设有与齿轮啮合的齿；电机固定在搅拌头主轴1的轴肩上，电机的输出轴与齿轮固接。驱动装置也可以采用现有技术中的其他驱动装置、驱动结构和传动方法。

进一步地，在搅拌头主轴1的轴肩上可固接有环形滑轨，环形转动盘可吊挂在环形滑轨上并与环形滑轨滑动连接。

进一步地，在混联机器人本体7的固定基座上可固接有支架，视觉定位系统的双目相机固定在支架上。

本发明还提供了一种利用上述的五自由度混联机器人末端位姿误差检测补偿系统的五自由度混联机器人末端位姿误差检测补偿方法实施例，该方法为：

在基坐标系下，标定激光测距仪6发射端初始坐标、搅拌头主轴1末端的初始坐标以及A、C轴初始角度。

激光测距仪6的发射端在工件4平面的投影方向即为搅拌头Z向前进方向，激光测距仪6的发射端距离焊缝高度，大于激光测距仪6的发射端到工件4表面其他部位高度，即环形转动盘在旋转一圈的时间段内，激光测距仪6测得的数据中会出现一个峰值。

设位于工件4表面上的激光照射点为检测点，设激光测距仪6测量值第一次出现峰值的时刻为T₁,设T₁时刻环形转动盘绕搅拌头主轴1的轴线转动的角度为α；设第一次峰值对应的检测点为A点，设激光测距仪6测量值第二次出现峰值的时刻为T₂,设T₂时刻环形转动盘绕搅拌头主轴1的轴线转动的角度为β；设第二次峰值对应的检测点为B点；转动角度α和转动角度β皆为相对于初始时刻绕搅拌头主轴1的轴线旋转的角度值。

两个相邻峰值出现时刻之间的时间段，相当于激光测距仪6的一个测量周期，由这两个时刻之间的搅拌头主轴1末端的三维坐标值可推导出工件4平面上的多个点的坐标值。当搅拌头前进方向出现偏差时，第二次峰值对应的检测点B与第一次峰值对应的检测点A的连线与焊缝的中线不重合。

读取T₁至T₂时间段的视觉定位系统测得的搅拌头主轴1末端在基坐标系下的实际坐标值，由搅拌头主轴1末端与激光测距仪6发射端两者初始时刻的相对坐标、环形转动盘的转速，计算得到T₁至T₂时间段的激光测距仪6发射端在基坐标系下的实际坐标值；由激光测距仪6在T₁至T₂时间段测得的其发射端到工件4表面检测点的距离，进一步得到T₁至T₂时间段的检测点在基坐标系下的坐标。

采样T₁至T₂时间段且不含A、B点的检测点，由采样的检测点在基坐标系下的坐标值拟合工件4平面；从而得到工件4平面法向量，由工件4平面法向量得到工件4平面与搅拌头主轴1的夹角，设该夹角为

拟合工件4平面可采用如下拟合工件4平面方程进行拟合：ax+by+cz+d＝0，拟合后可得到平面法向量(a，b，c)，工件4平面法向量与Z轴夹角即为搅拌头倾角。

以搅拌头末端为原点，焊缝方向为X轴，工件4平面法向量为Z轴建立坐标系O₂；令γ＝α-β，由

和γ，由空间几何关系，可计算得到搅拌头主轴1的轴线在坐标系O₂中的坐标，利用坐标系O₂和基坐标系之间的坐标转换，计算得到搅拌头主轴1的轴线在基坐标系下的姿态角，姿态角包括主轴轴线绕基坐标系的X轴转动的转角及绕基坐标系的Y轴转动的转角。

设T₁时刻搅拌头主轴1垂直于工件4表面，由T₂时刻视觉定位系统测得的搅拌头主轴1末端在基坐标系下的坐标实际值以及搅拌头主轴1的轴线在基坐标系下的姿态角，根据五自由度混联机器人逆运动学方程，得到T₂时刻第一至第三主动臂的轴向位移实际值及A、C轴转角实际值；由位姿指令得到对应T₂时刻第一至第三主动臂的轴向位移给定值以及A、C轴转角给定值，进一步获得T₂时刻第一至第三主动臂的轴向位移误差值以及A、C轴转角误差值；将得到的误差值作为补偿值。五自由度混联机器人逆运动学方程由混联机器人的结构推导得出。

下面以本发明的详细工作流程来进一步说明本发明的工作步骤及工作原理：

步骤一：设定机器人本体运动学参数，包括各关节杆长，零点位置，标定视觉定位系统的双目相机及激光测距仪6。

步骤二：在数控系统人机交互界面中，设定机器人运动轨迹，编辑一条焊缝轨迹，从而生成对应的加工G代码，即给定搅拌头主轴1末端位姿T＝[x，y，z，A，B]。根据机器人运动学逆解求取并联机构三个主动支链的进给长度q₁，q₂，q₃，以及串联机构A/C双摆角头的两个转角θ₄，θ₅,令θ＝[q₁，q₂，q₃，θ₄，θ₅]，并在数控系统底层运动学模块设定初始补偿值为Δθ＝[0，0，0，0，0]。

步骤三：在激光测距仪6上建立坐标系O₁-XYZ，并与机器人基坐标系标定。激光测距仪6围绕搅拌头主轴1的轴线旋转，其发出激光束，可检测搅拌头的轴肩等相对工件4表面的高度。设位于工件4表面上的激光照射点为检测点，由视觉定位系统测得的搅拌头主轴1末端在基坐标系下的坐标实际值，得到一系列检测点在基坐标系下的坐标值，由检测点在基坐标系下的坐标值拟合工件4平面；从而得到工件4的平面法向量，由工件4的平面法向量得到工件4的平面与搅拌头主轴1的夹角，令该夹角为

即搅拌头主轴1相对工件4平面的倾角为

以搅拌头末端为原点，焊缝方向为X轴，工件4平面法向量为Z轴建立坐标系O₂-XYZ。

设激光测距仪6测量值第一次出现峰值的时刻为T₁,设T₁时刻环形转动盘绕搅拌头主轴1的轴线转动的角度为α；设第一次峰值对应的检测点为A点，设激光测距仪6测量值第二次出现峰值的时刻为T₂,设T₂时刻环形转动盘绕搅拌头主轴1的轴线转动的角度为β；设第二次峰值对应的检测点为B点；转动角度α和转动角度β皆为相对于初始时刻绕搅拌头主轴1的轴线旋转的角度值。

令γ＝α-β，利用倾角

和角度γ，可获得搅拌头矢量在坐标系O₂中的坐标，利用坐标系O₂和基坐标系之间的坐标转换，计算得到搅拌头主轴1的轴线在基坐标系下的姿态角，姿态角包括搅拌头主轴1的轴线绕基坐标系的X轴转动的转角A₁,及搅拌头主轴1的轴线绕基坐标系的Y轴转动的转角B₁。依此可得到末端姿态角A₁、B₁。将测得的数据实时传送到机器人数控系统。

步骤四：通过标定的坐标系O₂-XYZ与机器人基坐标系O-XYZ之间的转换关系，求解当前机器人执行器末端即搅拌头主轴1末端，在基坐标系O-XYZ下的位置坐标及姿态角，得到混联机器人末端实际位姿数据，用T_real表示，T_real＝[x₁，y₁，z₁，A₁，B₁]，并显示在数控系统位姿补偿模块界面中。

步骤五：通过数控系统底层运动学模块，将获得的机器人实际位姿数据和给定位姿数据进行实时比对，得到的位姿偏差，用ΔT表示，ΔT＝[Δx，Δy，Δz，ΔA，ΔB],可根据混联机器人的雅克比矩阵J，利用公式ΔT＝J*Δθ,可求解各关节角度补偿值，将该值输入数控系统位姿补偿模块中。数控系统将该补偿值补偿到下一个插补周期中。

以上所述的实施例仅用于说明本发明的技术思想及特点，其目的在于使本领域内的技术人员能够理解本发明的内容并据以实施，不能仅以本实施例来限定本发明的专利范围，即凡本发明所揭示的精神所作的同等变化或修饰，仍落在本发明的专利范围内。

Claims (5)

1.一种五自由度混联机器人末端位姿误差检测补偿系统，包括混联机器人本体以及安装在混联机器人本体末端的搅拌摩擦焊搅拌头，混联机器人本体包括过约束三自由度的并联机构及A/C轴双摆角头，其中，并联机构包括动平台及与动平台铰接且沿轴向运动的第一至第三主动臂；双摆角头的C轴与动平台转动连接；双摆角头的A轴与搅拌头主轴的轴承座固接，且A轴轴线与搅拌头主轴轴线重合，其特征在于：还包括视觉定位系统及激光测距仪；视觉定位系统用于测量搅拌头主轴末端的三维坐标值，其包括至少一组双目相机；激光测距仪用于采集其发射端到工件表面的距离；在搅拌头主轴的轴肩上设有环形转动盘；环形转动盘在驱动装置驱动下围绕搅拌头主轴轴线匀速转动，激光测距仪固定在环形转动盘上；

在基坐标系下，标定激光测距仪发射端初始坐标、搅拌头主轴末端的初始坐标以及A、C轴初始角度；

设位于工件表面上的激光照射点为检测点，设激光测距仪测量值第一次出现峰值的时刻为T₁,设T₁时刻环形转动盘绕搅拌头主轴轴线转动的角度为α；设第一次峰值对应的检测点为A点，设激光测距仪测量值第二次出现峰值的时刻为T₂,设T₂时刻环形转动盘绕搅拌头主轴轴线转动的角度为β；设第二次峰值对应的检测点为B点；

读取T₁至T₂时间段的视觉定位系统测得的搅拌头主轴末端在基坐标系下的实际坐标值，由搅拌头主轴末端与激光测距仪发射端两者初始时刻的相对坐标、环形转动盘的转速，计算得到T₁至T₂时间段的激光测距仪发射端在基坐标系下的实际坐标值；由激光测距仪在T₁至T₂时间段测得的其发射端到工件表面检测点的距离，进一步得到T₁至T₂时间段的检测点在基坐标系下的坐标；

采样T₁至T₂时间段且不含A、B点的检测点，由采样的检测点在基坐标系下的坐标值拟合工件平面；从而得到工件平面法向量，由工件平面法向量得到工件平面与搅拌头主轴的夹角，设该夹角为

以搅拌头末端为原点，焊缝方向为X轴，工件平面法向量为Z轴建立坐标系O₂；令γ＝α-β，由

和γ，计算得到搅拌头主轴轴线在坐标系O₂中的坐标，利用坐标系O₂和基坐标系之间的坐标转换，计算得到搅拌头主轴轴线在基坐标系下的姿态角，姿态角包括主轴轴线绕基坐标系的X轴转动的转角及绕基坐标系的Y轴转动的转角；

设T₁时刻搅拌头主轴垂直于工件表面，由T₂时刻视觉定位系统测得的搅拌头主轴末端在基坐标系下的坐标实际值以及搅拌头主轴轴线在基坐标系下的姿态角，根据五自由度混联机器人逆运动学方程，得到T₂时刻第一至第三主动臂的轴向位移实际值及A、C轴转角实际值；由位姿指令得到对应T₂时刻第一至第三主动臂的轴向位移给定值以及A、C轴转角给定值，进一步获得T₂时刻第一至第三主动臂的轴向位移误差值以及A、C轴转角误差值；将得到的误差值作为补偿值。

2.根据权利要求1所述的五自由度混联机器人末端位姿误差检测补偿系统，其特征在于，驱动装置包括电机和齿轮；环形转动盘的周侧面设有与齿轮啮合的齿；电机固定在搅拌头主轴的轴肩上，电机的输出轴与齿轮固接。

3.根据权利要求1所述的五自由度混联机器人末端位姿误差检测补偿系统，其特征在于，在搅拌头主轴的轴肩上固接有环形滑轨，环形转动盘吊挂在环形滑轨上并与环形滑轨滑动连接。

4.根据权利要求1所述的五自由度混联机器人末端位姿误差检测补偿系统，其特征在于，在混联机器人本体的固定基座上固接有支架，视觉定位系统的双目相机固定在支架上。

5.一种利用权利要求1至4任一所述的五自由度混联机器人末端位姿误差检测补偿系统的五自由度混联机器人末端位姿误差检测补偿方法，其特征在于，该方法为：

在基坐标系下，标定激光测距仪发射端初始坐标、搅拌头主轴末端的初始坐标以及A、C轴初始角度；

设位于工件表面上的激光照射点为检测点，设激光测距仪测量值第一次出现峰值的时刻为T₁,设T₁时刻环形转动盘绕搅拌头主轴轴线转动的角度为α；设第一次峰值对应的检测点为A点，设激光测距仪测量值第二次出现峰值的时刻为T₂,设T₂时刻环形转动盘绕搅拌头主轴轴线转动的角度为β；设第二次峰值对应的检测点为B点；

读取T₁至T₂时间段的视觉定位系统测得的搅拌头主轴末端在基坐标系下的实际坐标值，由搅拌头主轴末端与激光测距仪发射端两者初始时刻的相对坐标、环形转动盘的转速，计算得到T₁至T₂时间段的激光测距仪发射端在基坐标系下的实际坐标值；由激光测距仪在T₁至T₂时间段测得的其发射端到工件表面检测点的距离，进一步得到T₁至T₂时间段的检测点在基坐标系下的坐标；

采样T₁至T₂时间段且不含A、B点的检测点，由采样的检测点在基坐标系下的坐标值拟合工件平面；从而得到工件平面法向量，由工件平面法向量得到工件平面与搅拌头主轴的夹角，设该夹角为

以搅拌头末端为原点，焊缝方向为X轴，工件平面法向量为Z轴建立坐标系O₂；令γ＝α-β，由

和γ，计算得到搅拌头主轴轴线在坐标系O₂中的坐标，利用坐标系O₂和基坐标系之间的坐标转换，计算得到搅拌头主轴轴线在基坐标系下的姿态角，姿态角包括主轴轴线绕基坐标系的X轴转动的转角及绕基坐标系的Y轴转动的转角；

设T₁时刻搅拌头主轴垂直于工件表面，由T₂时刻视觉定位系统测得的搅拌头主轴末端在基坐标系下的坐标实际值以及搅拌头主轴轴线在基坐标系下的姿态角，根据五自由度混联机器人逆运动学方程，得到T₂时刻第一至第三主动臂的轴向位移实际值及A、C轴转角实际值；由位姿指令得到对应T₂时刻第一至第三主动臂的轴向位移给定值以及A、C轴转角给定值，进一步获得T₂时刻第一至第三主动臂的轴向位移误差值以及A、C轴转角误差值；将得到的误差值作为补偿值。

Images