[发明专利]一种基于粗插补的混联机器人误差在线补偿系统及方法

权利要求书

1.一种基于粗插补的混联机器人误差在线补偿系统，其特征在于，包括混联机械臂、第一转动支架、第二转动支架、检测系统及控制系统；其中：

混联机械臂包括动平台；动平台周侧铰接有三个由伺服电机驱动伸缩的主动臂，依次为第一主动臂、第二主动臂、第三主动臂；动平台后端固接有从动支撑臂；动平台前端连接有由伺服电机驱动的A/C轴双摆角头，其中双摆角头的C轴与动平台转动连接；第一主动臂通过第一转轴与第一转动支架转动连接；第一转动支架通过第二转轴与固定的轴承座转动连接；第二主动臂、第三主动臂、从动支撑臂，对应通过第三转轴、第四转轴、第五转轴与第二转动支架转动连接；第二转动支架通过第六转轴与固定的轴承座转动连接；第一转轴与第二转轴的轴线垂直；第三转轴、第四转轴、第五转轴的轴线相互平行且与第六转轴的轴线垂直；第二转动支架的中心位于第五转轴的轴线与第六转轴的轴线交点上；

检测系统包括：用于检测第六转轴转动角度的第一角度传感器；用于检测第五转轴转动角度的第二角度传感器；用于检测C轴转动角度的第三角度传感器；用于检测A轴转动角度的第四角度传感器；用于检测从动支撑臂轴向位移的第一位移传感器；

控制系统包括多轴运动控制器；该多轴运动控制器，其接收来自第一至第四角度传感器以及第一位移传感器的检测信号，其将检测值换算为对应第一至第三主动臂的伸缩位移值以及A、C轴的转动角度值，并与对应的给定值进行相减得到偏差，其基于偏差给出控制信号，控制驱动第一至第三主动臂以及A、C轴的伺服电机的工作。

2.根据权利要求1所述的基于粗插补的混联机器人误差在线补偿系统，其特征在于，第五转轴的轴线与第三转轴的轴线及第四转轴的轴线距离相等。

3.根据权利要求1所述的基于粗插补的混联机器人误差在线补偿系统，其特征在于，所述第一至第四角度传感器为圆光栅；第一位移传感器为直线光栅。

4.根据权利要求1所述的基于粗插补的混联机器人误差在线补偿系统，其特征在于，多轴运动控制器采用欧姆龙CK3M型多轴运动控制器。

5.根据权利要求1所述的基于粗插补的混联机器人误差在线补偿系统，其特征在于，第一转动支架和第二转动支架上下设置。

6.根据权利要求1所述的基于粗插补的混联机器人误差在线补偿系统，其特征在于，第一主动臂与动平台的铰接中心到第二主动臂、第三主动臂与动平台的铰接中心的距离相等。

7.根据权利要求1所述的基于粗插补的混联机器人误差在线补偿系统，其特征在于，第一主动臂、第二主动臂及第三主动臂与动平台球铰式连接。

8.一种利用权利要求1至7任一所述的基于粗插补的混联机器人误差在线补偿系统的基于粗插补的混联机器人误差在线补偿方法，其特征在于，包括如下步骤：

步骤A，在多轴运动控制器内，设置五个全局补偿变量存储器，用于对应存储第一至第三主动臂的轴向位移补偿变量以及A、C轴的转动角度补偿变量，并初始化变量存储器；

步骤B，多轴运动控制器读入G代码，将刀尖加工轨迹转化为若干连续微小线段，得到微小线段两端点的刀尖姿态数据；

步骤C，多轴运动控制器根据混联机器人逆运动学算法，并结合补偿变量存储器内的补偿值，输出对应第一至第三主动臂的轴向位移给定值的伺服驱动指令，以及对应A、C轴转动角度给定值的伺服驱动指令；

步骤D，多轴运动控制器接收当前第一至第四角度传感器以及第一位移传感器的反馈信号，其根据空间几何算法，得到对应第一至第三主动臂的轴向位移给定值以及A、C轴的转动角度给定值的偏差，将偏差作为新的补偿值，变量存储器对应更新为新的补偿值；

步骤E，重复步骤C至步骤D,直至插补结束。

9.根据权利要求8所述的基于粗插补的混联机器人误差在线补偿方法，其特征在于，刀尖姿态数据包括：刀尖点的x轴、y轴、z轴坐标，以及刀尖点绕x轴、y轴的转动角度。

10.根据权利要求8所述的基于粗插补的混联机器人误差在线补偿方法，其特征在于，空间几何算法如下：

设第一角度传感器检测值为θ₁，设第二角度传感器检测值为θ₂，设第一位移传感器检测值为L₁，设第一至第三主动臂与动平台相对转动围绕的中心点依次为A₁、A₂、A₃，设第一主动臂与第一转动支架的转动副中心为B₁；设第二、第三主动臂与第二转动支架的转动副中心分别为B₂、B₃；设A₂、A₃两点的连线中点为A点，设第二转动支架的中心为B点，设基座标系为B-xyz；设x_A、y_A、z_A对应为A点在基座标系下x轴、y轴、z轴坐标；设第一至第三主动臂由伺服电机转动带动滚珠丝杠进行轴向位移；

由空间几何关系，得到A点在基座标系下的坐标值如下：

x_A＝L₁sinθ₂

y_A＝L₁cosθ₂sinθ₁

z_A＝L₁cosθ₂cosθ₁

根据并联机构逆运动学，由以下各式求解得到第一至第三主动臂的伺服电机实际转动角度：

r_A＝[x_A y_A z_A]^T；

q₄＝||r_A||；

w₄＝r_A/q₄＝(s_3x s_3y s_3z)^T；

a_i0＝(a_icosγ_i a_isinγ_i 0)^T；

C_i＝Ra_i0；

D_i＝(b_icosγ_i b_isinγ_i 0)^T；

式中：

r_A为点A在坐标系B-xyz下的位置矢量；

q₄为点A到点B距离；

w₄为点A到点B的单位矢量；

s_3x为坐标系A-x_Ay_Az_A中的Z轴上单位矢量对应的点投影在坐标系B-xyz中x轴上的坐标值；

s_3y为坐标系A-x_Ay_Az_A中的Z轴上单位矢量对应的点投影在坐标系B-xyz中y轴上的坐标值；

s_3z为坐标系A-x_Ay_Az_A中的Z轴上单位矢量对应的点投影在坐标系B-xyz中z轴上的坐标值；

ψ为坐标系A-x_Ay_Az_A的姿态相对坐标系B-xyz绕x轴的旋转角度；

θ为坐标系A-x_Ay_Az_A相对坐标系B-xyz绕x轴旋转角度ψ后，坐标系A-x_Ay_Az_A的姿态相对坐标系B-xyz绕y_A轴的旋转角度；

R为坐标系A-x_Ay_Az_A相对坐标系B-xyz的姿态矩阵；

γ_i为中间变量；

C_i为点A_i在坐标系B-xyz下的位置矢量；i＝1,2,3；

a_i为点A_i到点A的距离；i＝1,2,3；

D_i为点B_i在坐标系B-xyz下的位置矢量；i＝1,2,3；

b_i为点B_i到点B的距离；i＝1,2,3；

a_i0为点A_i在坐标系A-x_Ay_Az_A下的位置矢量；i＝1,2,3；

p_i为第i个主动臂的滚珠丝杠的导程；i＝1,2,3；

θ_ia为第i个主动臂的伺服电机的实际转角；i＝1,2,3。