一种基于粗插补的混联机器人误差在线补偿系统及方法
技术领域
本发明涉及一种机器人加工领域，特别涉及一种基于粗插补的混联机器人误差在线补偿系统及方法。
背景技术
为适应加工大型结构件的大范围移动、局部高速精密加工的服役环境，应对大型结构件具有外形尺寸大、几何形状复杂、精度要求高等特点，以混联构型装备为核心功能部件的单机制造单元或多机制造系统正逐渐成为不可或缺的重要工具。然而，在使用过程中混联机器人由于末端执行器负载和机械臂自身重力作用会发生一定程度的形变，使机器人末端位置发生偏移，及机器人零件在加工制造过程中实际尺寸与理论尺寸不完全相符，在装配过程中会造成驱动支链长度不准确，各驱动关节因减速器带来的传动误差，摆角头因减速器间隙、摩擦、零位偏差引起的关节误差，会造成运动控制器中用于计算的理论模型参数与机器人实际参数不一致，直接影响末端的定位精度。
提高机器人精度的方法主要有误差预防技术和误差补偿技术。目前误差预防技术一直受到零件制造技术和经济效益的制约，而误差补偿技术可以在较低的硬件投入下使机器人精度得到有效的改善。误差补偿技术主要有两类：一类是离线补偿，在机器人使用之前通过标定或者建立误差补偿映射模型提高精度，这类方法复杂繁琐且现场使用时无法改变；另一类是在线补偿，在机器人使用过程中检测误差，实时对误差进行补偿。目前的在线补偿方法，监测数据较多，运算量大，占用计算机的存储容量和运行速度，设备的运行速度相应受到影响。
发明内容
本发明为解决公知技术中存在的技术问题而提供一种高效准确的基于粗插补的混联机器人误差在线补偿系统及方法。
本发明为解决公知技术中存在的技术问题所采取的技术方案是：一种基于粗插补的混联机器人误差在线补偿系统，包括混联机械臂、第一转动支架、第二转动支架、检测系统及控制系统；其中：
混联机械臂包括动平台；动平台周侧铰接有三个由伺服电机驱动伸缩的主动臂，依次为第一主动臂、第二主动臂、第三主动臂；动平台后端固接有从动支撑臂；动平台前端连接有由伺服电机驱动的A/C轴双摆角头，其中双摆角头的C轴与动平台转动连接；第一主动臂通过第一转轴与第一转动支架转动连接；第一转动支架通过第二转轴与固定的轴承座转动连接；第二主动臂、第三主动臂、从动支撑臂，对应通过第三转轴、第四转轴、第五转轴与第二转动支架转动连接；第二转动支架通过第六转轴与固定的轴承座转动连接；第一转轴与第二转轴的轴线垂直；第三转轴、第四转轴、第五转轴的轴线相互平行且与第六转轴的轴线垂直；第二转动支架的中心位于第五转轴的轴线与第六转轴的轴线交点上；
检测系统包括：用于检测第六转轴转动角度的第一角度传感器；用于检测第五转轴转动角度的第二角度传感器；用于检测C轴转动角度的第三角度传感器；用于检测A轴转动角度的第四角度传感器；用于检测从动支撑臂轴向位移的第一位移传感器；
控制系统包括多轴运动控制器；该多轴运动控制器，其接收来自第一至第四角度传感器以及第一位移传感器的检测信号，其将检测值换算为对应第一至第三主动臂的伸缩位移值以及A、C轴的转动角度值，并与对应的给定值进行相减得到偏差，其基于偏差给出控制信号，控制驱动第一至第三主动臂以及A、C轴的伺服电机的工作。
进一步地，第五转轴的轴线与第三转轴的轴线及第四转轴的轴线距离相等。
进一步地，所述第一至第四角度传感器为圆光栅；第一位移传感器为直线光栅。
进一步地，多轴运动控制器采用欧姆龙CK3M型多轴运动控制器。
进一步地，第一转动支架和第二转动支架上下设置。
进一步地，第一主动臂与动平台的铰接中心到第二主动臂、第三主动臂与动平台的铰接中心的距离相等。
进一步地，第一主动臂、第二主动臂及第三主动臂与动平台球铰式连接。
本发明还提供了一种采用上述的基于粗插补的混联机器人误差在线补偿系统的基于粗插补的混联机器人误差在线补偿方法，该方法包括如下步骤：
步骤A，在多轴控制器内，设置五个全局补偿变量存储器，用于对应存储第一至第三主动臂的轴向位移补偿变量以及A、C轴的转动角度补偿变量，并初始化变量存储器；
步骤B，多轴运动控制器读入G代码，将刀尖加工轨迹转化为若干连续微小线段，得到微小线段两端点的刀尖姿态数据；
步骤C，多轴运动控制器根据混联机器人逆运动学算法，并结合补偿变量存储器内的补偿值，输出对应第一至第三主动臂的轴向位移给定值的伺服驱动指令，以及对应A、C轴转动角度给定值的伺服驱动指令；
步骤D，多轴运动控制器接收当前第一至第四角度传感器以及第一位移传感器的反馈信号，其根据空间几何算法，得到对应第一至第三主动臂的轴向位移给定值以及A、C轴的转动角度给定值的偏差，将偏差作为新的补偿值，变量存储器对应更新为新的补偿值；
步骤E，重复步骤C至步骤D,直至插补结束。
进一步地，刀尖姿态数据包括：刀尖点的x轴、y轴、z轴坐标，以及刀尖点绕x轴、y轴的转动角度。
进一步地，空间几何算法如下：
设第一角度传感器检测值为θ1，设第二角度传感器检测值为θ2，设第一位移传感器检测值为L1，设第一至第三主动臂与动平台相对转动围绕的中心点依次为A1、A2、A3，设第一主动臂与第一转动支架的转动副中心为B1；设第二、第三主动臂与第二转动支架的转动副中心分别为B2、B3；设A2、A3两点的连线中点为A点，设第二转动支架的中心为B点，设基座标系为B-xyz；设xA、yA、zA对应为A点在基座标系下x轴、y轴、z轴坐标；设第一至第三主动臂由伺服电机转动带动滚珠丝杠进行轴向位移；
由空间几何关系，得到A点在基座标系下的坐标值如下：
xA＝L1sinθ2
yA＝L1cosθ2sinθ1
zA＝L1cosθ2cosθ1
根据并联机构逆运动学，由以下各式求解得到第一至第三主动臂的伺服电机实际转动角度：
rA＝[xA yA zA]T；
q4＝||rA||；
w4＝rA/q4＝(s3x s3y s3z)T；
ai0＝(ai cosγi ai sinγi 0)T；
Ci＝Rai0；
Di＝(bi cosγi bisinγi 0)T；
式中：
rA为点A在坐标系B-xyz下的位置矢量；
q4为点A到点B距离；
w4为点A到点B的单位矢量；
s3x为坐标系A-xAyAzA中的Z轴上单位矢量对应的点投影在坐标系B-xyz中x轴上的坐标值；
s3y为坐标系A-xAyAzA中的Z轴上单位矢量对应的点投影在坐标系B-xyz中y轴上的坐标值；
s3z为坐标系A-xAyAzA中的Z轴上单位矢量对应的点投影在坐标系B-xyz中z轴上的坐标值；
ψ为坐标系A-xAyAzA的姿态相对坐标系B-xyz绕x轴的旋转角度；
θ为坐标系A-xAyAzA相对坐标系B-xyz绕x轴旋转角度ψ后，坐标系A-xAyAzA的姿态相对坐标系B-xyz绕yA轴的旋转角度；
R为坐标系A-xAyAzA相对坐标系B-xyz的姿态矩阵；
γi为中间变量；
Ci为点Ai在坐标系B-xyz下的位置矢量；i＝1,2,3；
ai为点Ai到点A的距离；i＝1,2,3；
Di为点Bi在坐标系B-xyz下的位置矢量；i＝1,2,3；
bi为点Bi到点B的距离；i＝1,2,3；
ai0为点Ai在坐标系A-xAyAzA下的位置矢量；i＝1,2,3；
pi为第i个主动臂的滚珠丝杠的导程；i＝1,2,3；
θia为第i个主动臂的伺服电机的实际转角；i＝1,2,3。
本发明具有的优点和积极效果是：可在线实时补偿混联机器人在加工过程中由于机械臂自重和末端负载导致的末端位置偏移、零部件在加工装配过程中产生的尺寸误差、各驱动关节的传动误差、摆角头因减速器间隙、摩擦、零位偏差引起的关节误差，能够提高混联机器人的定位精度，相对于传统运动学标定方法和离线补偿方法更加简单，且该方法在粗插补过程中完成相邻插补点的误差补偿，求补偿值算法简单，在线补偿控制容易，补偿效果满足机器人高速高精度运动控制的需求。
附图说明
图1是本发明的一种基于粗插补的混联机器人误差在线补偿系统结构示意图。
图2是本发明的一种基于粗插补的混联机器人误差在线补偿方法工作流程图。
图3是本发明的空间几何算法的几何原理图。
图中：1、第三伺服电机；2、从动支撑臂；3、第二伺服电机；4、第二角度传感器；5、第五转轴；6、第二转动支架；7、动平台；8、第三角度传感器；9、A/C轴双摆角头；10、A轴；11、第四角度传感器；12、第一位移传感器；13、第六转轴；14、第一角度传感器；15、第一伺服电机；16、第一转动支架。
具体实施方式
为能进一步了解本发明的发明内容、特点及功效，兹列举以下实施例，并配合附图详细说明如下：
请参见图1至图3，一种基于粗插补的混联机器人误差在线补偿系统，包括混联机械臂、第一转动支架16、第二转动支架6、检测系统及控制系统；其中：
混联机械臂包括动平台7；动平台7周侧铰接有三个由伺服电机驱动伸缩的主动臂，依次为第一主动臂、第二主动臂、第三主动臂；动平台7后端固接有从动支撑臂2；动平台7前端连接有由伺服电机驱动的A/C轴双摆角头9，其中A/C轴双摆角头9的C轴与动平台7转动连接；第一主动臂通过第一转轴与第一转动支架16转动连接；第一转动支架16通过第二转轴与固定的轴承座转动连接；第二主动臂、第三主动臂、从动支撑臂2，对应通过第三转轴、第四转轴、第五转轴5与第二转动支架6转动连接；第二转动支架6通过第六转轴13与固定的轴承座转动连接；第一转轴与第二转轴的轴线垂直；第三转轴、第四转轴、第五转轴5的轴线相互平行且与第六转轴13的轴线垂直；第二转动支架6的中心位于第五转轴5的轴线与第六转轴13的轴线交点上；
第一主动臂、第二主动臂、第三主动臂构成混联机器人的并联机构；A/C轴双摆角头的A轴10和A/C轴双摆角头9的C轴构成混联机器人串联机构。从动支撑臂2用于固定支撑动平台7并对动平台7的运动进行约束。
其中第一主动臂的驱动伺服电机称为第一伺服电机15、第二主动臂的驱动伺服电机称为第二伺服电机3、第三主动臂的驱动伺服电机称为第三伺服电机1，A/C轴双摆角头9的C轴驱动伺服电机称为第四伺服电机，A/C轴双摆角头的A轴10驱动伺服电机称为第五伺服电机。第一至第三主动臂的伺服电机可采用输出转矩的伺服电机，这种伺服电机的输出轴可与将旋转运动变为直线运动的传动机构连接，优选与滚动丝杠传动机构连接，将伺服电机的转动驱动转变为直线驱动，第一至第三主动臂的伺服电机也可采用直线伺服电机，来实现第一主动臂、第二主动臂、第三主动臂直线伸缩。
检测系统包括：用于检测第六转轴13转动角度的第一角度传感器14；用于检测第五转轴5转动角度的第二角度传感器4；用于检测C轴转动角度的第三角度传感器8；用于检测A轴10转动角度的第四角度传感器11；用于检测从动支撑臂2轴向位移的第一位移传感器12；
控制系统包括多轴运动控制器；该多轴运动控制器，其接收来自第一角度传感器14、第二角度传感器4、第三角度传感器8、第四角度传感器11以及第一位移传感器12的检测信号，其将检测值换算为对应第一主动臂、第二主动臂、第三主动臂的伸缩位移值以及A轴10、C轴的转动角度值，并与对应的给定值进行相减得到偏差，其基于偏差给出控制信号，控制驱动第一主动臂、第二主动臂、第三主动臂以及A轴10、C轴的伺服电机的工作。
将检测值换算获得的第一主动臂、第二主动臂、第三主动臂的伸缩位移值以及A轴10、C轴的转动角度值作为反馈的实际值，对应与第一主动臂、第二主动臂、第三主动臂的伸缩位移给定值以及A轴10、C轴的转动角度给定值相减，得到偏差，即第一主动臂的伸缩位移实际值与其伸缩位移给定值相减，第二主动臂的伸缩位移实际值与其伸缩位移给定值相减，第三主动臂的伸缩位移实际值与其伸缩位移给定值相减，A/C轴双摆角头的C轴转动角度实际值与其转动角度给定值相减，A/C轴双摆角头的A轴10转动角度实际值与其转动角度给定值相减，分别得到相应的偏差，将对应的偏差作为补偿值，多轴运动控制器基于偏差给出控制信号，控制驱动第一至第三主动臂以及A、C轴的伺服电机的工作，使A/C轴双摆角头9上安装的加工刀具沿加工路径移动。
如果驱动第一主动臂、第二主动臂、第三主动臂伸缩位移的伺服电机，通过其输出轴与将旋转运动变为直线运动的传动机构连接，将旋转运动变为直线运动的传动机构可为带传动机构、链轮传动机构、齿轮齿条传动机构、蜗轮蜗杆传动机构、丝杠传动机构等，则将第一主动臂、第二主动臂、第三主动臂伸缩位移给定值和实际值，换算为对应的伺服电机转动角度给定值和实际值。
优选地，第五转轴5的轴线可与第三转轴的轴线及第四转轴的轴线距离相等。
第一角度传感器14、第二角度传感器4、第三角度传感器8及第四角度传感器11可采用现有技术中用于测量转动角度的角度传感器，比如可选用圆光栅；第一位移传感器12可采用现有技术中用于测量直线位移的位移传感器，比如可选用直线光栅。
运动控制器用于实现机械运动精确的位置控制、速度控制、加速度控制、转矩或力的控制，多轴运动控制器可同时控制多个伺服驱动器，多轴运动控制器发送运动控制指令到伺服驱动器，由伺服驱动器驱动伺服电机运行，再通过伺服电机上的角度传感器如编码器等的检测伺服电机的实际旋转角度，将检测信号作为反馈信号反馈至至多轴运动控制器，实现系统的闭环控制。多轴运动控制器可采用现有技术中的适用控制5轴以上伺服电机的运动控制器，比如OMRON公司生产的CK3M型可编程多轴运动控制器。
优选地，第一转动支架16和第二转动支架6可上下设置。
优选地，第一主动臂与动平台7的铰接中心到第二主动臂、第三主动臂与动平台7的铰接中心的距离可相等。
优选地，第一主动臂、第二主动臂及第三主动臂可与动平台7球铰式连接。
本发明还提供了一种采用上述的基于粗插补的混联机器人误差在线补偿系统的基于粗插补的混联机器人误差在线补偿方法，该方法包括如下步骤：
步骤A，在多轴控制器内，设置五个全局补偿变量存储器，用于对应存储第一至第三主动臂的轴向位移补偿变量以及A、C轴的转动角度补偿变量，并初始化变量存储器。
步骤B，多轴运动控制器读入G代码，将刀尖加工轨迹转化为若干连续微小线段，得到微小线段两端点的刀尖姿态数据。
步骤C，多轴运动控制器根据混联机器人逆运动学算法，并结合补偿变量存储器内的补偿值，输出对应第一至第三主动臂的轴向位移给定值的伺服驱动指令，以及对应A、C轴转动角度给定值的伺服驱动指令。
步骤D，多轴运动控制器接收当前第一角度传感器14、第二角度传感器4、第三角度传感器8、第四角度传感器11以及第一位移传感器12的反馈信号，其根据空间几何算法，得到对应第一至第三主动臂的轴向位移给定值以及A、C轴的转动角度给定值的偏差，将偏差作为新的补偿值，变量存储器对应更新为新的补偿值。
步骤E，重复步骤C至步骤D,直至插补结束。
其中，刀尖姿态数据可包括：刀尖点的x轴、y轴、z轴坐标，以及刀尖点绕x轴、y轴的转动角度等。
优选地，上述的空间几何算法可如下：
可设第一角度传感器14检测值为θ1，可设第二角度传感器4检测值为θ2，可设第一位移传感器12检测值为L1，可设第一至第三主动臂与动平台相对转动围绕的中心点依次为A1、A2、A3，即第一主动臂与动平台围绕点A1相对转动，第二主动臂与动平台围绕点A2相对转动，第三主动臂与动平台围绕点A3相对转动；可设第一主动臂与第一转动支架16的转动副中心为B1；可设第二、第三主动臂与第二转动支架6的转动副中心分别为B2、B3；可设A2、A3两点的连线中点为A点，设第二转动支架6的中心为B点，可设基座标系为B-xyz；可设xA、yA、zA对应为A点在基座标系下x轴、y轴、z轴坐标；可设第一至第三主动臂由伺服电机转动带动滚珠丝杠进行轴向位移；
可由空间几何关系，得到A点在基座标系下的坐标值如下：
xA＝L1sinθ2
yA＝L1cosθ2sinθ1
zA＝L1cosθ2cosθ1
可根据并联机构逆运动学，由以下各式求解得到第一至第三主动臂的伺服电机实际转动角度：
rA＝[xA yA zA]T；
q4＝||rA||；
w4＝rA/q4＝(s3x s3y s3z)T；
ai0＝(ai cosγi ai sinγi 0)T；
Ci＝Rai0；
Di＝(bi cosγi bisinγi 0)T；
式中：
rA为点A在坐标系B-xyz下的位置矢量；
q4为点A到点B距离；
w4为点A到点B的单位矢量；
s3x为坐标系A-xAyAzA中的Z轴上单位矢量对应的点投影在坐标系B-xyz中x轴上的坐标值；
s3y为坐标系A-xAyAzA中的Z轴上单位矢量对应的点投影在坐标系B-xyz中y轴上的坐标值；
s3z为坐标系A-xAyAzA中的Z轴上单位矢量对应的点投影在坐标系B-xyz中z轴上的坐标值；
ψ为坐标系A-xAyAzA的姿态相对坐标系B-xyz绕x轴的旋转角度；
θ为坐标系A-xAyAzA相对坐标系B-xyz绕x轴旋转角度ψ后，坐标系A-xAyAzA的姿态相对坐标系B-xyz绕yA轴的旋转角度；
R为坐标系A-xAyAzA相对坐标系B-xyz的姿态矩阵；
γi为中间变量；
Ci为点Ai在坐标系B-xyz下的位置矢量；i＝1,2,3；
ai为点Ai到点A的距离；i＝1,2,3；
Di为点Bi在坐标系B-xyz下的位置矢量；i＝1,2,3；
bi为点Bi到点B的距离；i＝1,2,3；
ai0为点Ai在坐标系A-xAyAzA下的位置矢量；i＝1,2,3；
pi为第i个主动臂的滚珠丝杠的导程；i＝1,2,3；
θia为第i个主动臂的伺服电机的实际转角；i＝1,2,3。
下面以本发明的一种基于粗插补的混联机器人误差在线补偿方法的优选实施例，来进一步说明本发明的工作原理：
一种基于粗插补的混联机器人误差在线补偿方法，该方法采用前述的混联机器人误差在线补偿系统，其中第一主动臂的驱动伺服电机为第一伺服电机15、第二主动臂的驱动伺服电机为第二伺服电机3、第三主动臂的驱动伺服电机为第三伺服电机1，A/C轴双摆角头9的C轴驱动伺服电机为第四伺服电机，A/C轴双摆角头的A轴10驱动伺服电机为第五伺服电机。设多轴运动控制器在第N个粗插补周期内，对应第一至第五伺服电机，给定输出对应为：θ1d(n)、θ2d(n)、θ3d(n)、θ4d(n)、θ5d(n)。
该方法包括如下具体步骤：
步骤1，待加工零件图导入Unigraphics应用系统中(简称UG)，经UG处理生成数控机床加工的G代码；
步骤2，多轴运动控制器采用OMRON公司生产的CK3M型可编程多轴运动控制器，在多轴运动控制器中对第一角度传感器14、第二角度传感器4、第三角度传感器8、第四角度传感器11以及第一位移传感器12进行标零初始化；
步骤3，将待加工零件G代码写入混联机器人的上位机操作系统中，经上位机编译后下载到下位机多轴运动控制器中；
步骤4，实时检测机器人运动过程中动平台7参考点位置误差和摆角头转角误差并计算补偿量，实现基于粗插补的相邻点误差在线补偿控制，步骤如下：
(a)机器人运动控制器读入G代码，根据相应的运动指令(直线指令、圆弧指令、点到点指令等)进行粗插补，得到刀尖加工微小轨迹两端点姿态信息；
(b)多轴运动控制器内预设五个虚轴全局变量存储器并初始化，变量存储器用于存储各轴补偿量数据；
(c)针对多轴运动控制器内实轴伺服电机指令输出值，由粗插过程中微小轨迹端点姿态信息rc，调用运动控制器系统内位置在“LanguageRealtime Routinesusrcode.c”项目文件中编写的混联机器人逆运动学算法，求解实轴伺服电机指令值；
(d)当机器人运行后，利用并联机构中第一角度传感器14，实时检测第六转轴13的实际转角θ1，第二角度传感器4实时检测第五转轴5的实际转角θ2，第一位移传感器12实时检测从动支撑臂2沿轴向的位移L1，结合空间几何关系得到动平台7参考点A的位置信息xA、yA、zA，然后调用多轴运动控制器内usrcode.c中编写的误差算法程序，计算出粗插补中第N-1粗插补周期的第一至第三主动臂的伺服电机的实际转角θ1a(n-1)、θ2a(n-1)、θ3a(n-1)，并将实际值与转角给定值θ1d(n-1)、θ2d(n-1)、θ3d(n-1)的进行相减，得到补偿值Δθ1(n-1)、Δθ2(n-1)、Δθ3(n-1)；同时利用串联A/C轴双摆角头9的第三角度传感器8实时检测C轴的实际转角θ3和第四角度传感器11实时检测A轴10的实际转角θ4，经关节求逆得到粗插补中第N-1粗插补周期的串联A/C轴双摆角头9伺服电机的实际转角θ4a(n-1)、θ5a(n-1)，并将其与指令转角θ4d(n-1)、θ5d(n-1)相减，得到补偿值Δθ4(n-1)、Δθ5(n-1)，将得到的补偿值分别放到预先设定的五个虚轴全局变量中，五个虚轴全局变量存储更新为新的补偿值；
(e)将补偿值加到粗插补中的第N粗插补周期的第一至第三主动臂和A/C轴双摆角头9的伺服电机指令输出值θ1d(n)、θ2d(n)、θ3d(n)、θ4d(n)、θ5d(n)中完成粗插补过程中相邻点误差在线补偿。
以上所述的实施例仅用于说明本发明的技术思想及特点，其目的在于使本领域内的技术人员能够理解本发明的内容并据以实施，不能仅以本实施例来限定本发明的专利范围，即凡本发明所揭示的精神所作的同等变化或修饰，仍落在本发明的专利范围内。
