一种五自由度混联机器人的双闭环控制方法
技术领域
本发明涉及机器人的双闭环控制方法,尤其涉及一种五自由度混联机器人的双闭环控制方法。
背景技术
工业机器人普遍采用半闭环控制策略,即利用编码器通过检测各关节伺服电机的实际转角构成位置和速度半闭环控制。这种控制策略调试维修方便,稳定性好,已得到广泛应用。但半闭环控制无法补偿机械系统传动误差及由构件弹性变形所造成的末端执行器位姿误差。如可直接检测到机器人机构输出构件的位姿信息,则可实施全闭环反馈控制,进而有利于提高机器人末端执行器的运动精度。
专利US006301525B1公开了对一种五自由度混联机器人中一平动两转动并联机构的控制方法。该方法在从动长度调节装置与机架相连的虎克铰两个轴线上安装两个圆光栅,沿从动长度调节装置轴线安装一个直线光栅。利用这些传感器检测到的转角和长度信息,首先计算出并联机构动平台参考点的位置误差,然后再经误差逆解模型得到三个主动长度调节装置伺服电机的转角误差,进而用这些转角误差于修改理想控制指令。然后再利用伺服电机的编码器检测信息实现位置和速度环调节。这种方法必须经过两步运算,计算时间长,难于以伺服更新速率实现。此外,在从动长度调节装置与机架相连的虎克铰上安装的圆光栅开放性不好,不便于维修和更换。
发明内容
本发明的目的在于克服现有技术的不足,提供一种可实时补偿其中并联机构三个主动长度调节装置因丝杠传动误差、热误差及构件弹性变形引起的杆长误差,有利于提高五自由度混联机器人的运动控制精度的五自由度混联机器人的双闭环控制方法。
本发明的一种含转动支架的五自由度混联机器人的双闭环控制方法,它包括以下步骤:
(1)安装第一圆光栅、第二圆光栅以及直线光栅;
其中在转动支架与机架连接的第一转动副的转轴上安装第一圆光栅的尺身,在第一固定轴座上安装第一圆光栅的读数头;在转动支架与从动长度调节装置连接的第四转动副的转轴上安装第二圆光栅的尺身,在转动支架上安装第二圆光栅的读数头;在从动长度调节装置轴线移动方向安装直线光栅的尺身,在从动长度调节装置与转动支架所连接的第四转动副的内圈内侧安装直线光栅读数头;
(2)利用第一圆光栅、第二圆光栅实时检测第一转动副和第四转动副的轴线的实际转角,利用直线光栅实时检测从动长度调节装置沿移动副方向的移动距离,从而得到从动长度调节装置实际杆长,将所述的实际转角和实际杆长信息通过算法转换为驱动三个主动长度调节装置的伺服电机的实际转角,并将其与指令值比较,经位置环控制器PID调节转换为速度指令,进而实现位置环反馈控制;通过驱动三个主动长度调节装置的伺服电机的电机轴上安装的绝对式角度编码器,实时检测三个伺服电机轴的实际转角并分别作微分处理得到三个伺服电机的实际电机转速,分别与相应的所述的速度指令进行比较,进而通过伺服放大器PI调节实现三个伺服电机的速度环反馈控制;
所述的算法为:
式中:
p——主动长度调节装置中滚珠丝杠的导程
q4——通过直线光栅检测的移动距离加上从动长度调节装置初始位置杆长而得到的从动长度调节装置杆长
ψ——第一圆光栅检测到的第一转动副的轴线的实际转角
θ——第二圆光栅检测到的第四转动副的轴线的实际转角
a1——由第一球链中心到从动调节装置轴线的理论垂距
a2——由第二、三球链中心到从动调节装置轴线的理论垂距
b1——连接第一主动长度调节装置与第二固定轴座的虎克铰的近架转轴轴线到转动支架与机架相连转轴轴线间的理论距离
b2——连接第二主动长度调节装置与转动支架的第二转动副的转轴轴线到连接从动长度调节装置与转动支架的第四转动副的转轴轴线间的理论距离或者连接第三主动长度调节装置与转动支架的第三转动副的转轴轴线到连接从动长度调节装置与转动支架的第四转动副的转轴轴线间的理论距离
θ1a、θ2a、θ3a——由第一圆光栅、第二圆光栅和直线光栅检测信息计算得到的驱动三个主动长度调节装置的伺服电机的实际转角。
本发明方法与现有技术相比,可在线实时补偿并联机构中三个主动长度调节装置因丝杠传动误差、热伸长和因轴向弹性变形造成的杆长误差,弥补了以往传统半闭环控制控制精度不高的局限性,且三个主动长度调节装置伺服电机转角与转动支架相对机架的转角、从动长度调节装置相对转动支架的转角,以及从动长度调节装置杆长的数学关系非常简单,故可以每个伺服周期更新一次的速率完成位置闭环控制,可满足这种机器人高速高精度运动控制的需求。此外,在转动支架上安装的两个圆光栅开放性好,便于维修和更换。
附图说明
图1是五自由度混联机器人的一个方向的结构示意图;
图2是五自由度混联机器人的另一个方向的结构示意图;
图3是圆光栅和直光栅安装在图1所示的机器人中的结构示意图;
图4是本发明的一种五自由度混联机器人的双闭环控制方法的控制原理图;
图5是五自由度混联机器人的尺度参数示意图。
具体实施方式
下面结合附图和具体实施例对本发明加以详细说明。
本发明是对公开号为CN104985596A的中国专利公开的一种含转动支架的五自由度混联机器人的双闭环控制方法。
下面结合附图简要说明CN104985596A中公开的五自由度混联机器人的结构,该机器人包括具有一个平动和二个转动的三自由度并联机构和与之串接的具有两个转动自由度的转头组成,转头6与动平台5串接。所述的三自由度并联机构包括转动支架12,所述的转动支架12的两侧各自通过第一转动副55与安装在机架上的两个第一固定轴座11转动连接。第二主动长度调节装置3以及第三主动长度调节装置4上部分别通过第二转动副35、第三转动副45对称的转动连接在所述的转动支架12两侧并且底端分别通过第二球铰34、第三球铰44与所述的动平台5转动连接,从动长度调节装置1与转动支架12通过第四转动副15转动相连并且从动长度调节装置1底部与动平台固定相连,第一主动长度调节装置2上部通过虎克铰25与安装在机架上的两个第二固定轴座21转动连接并且底端通过第一球铰24与动平台5转动连接;
所述的第一主动长度调节装置2、第二主动长度调节装置3以及第三主动长度调节装置4分别由第一伺服电机23、第二伺服电机33、第三伺服电机43驱动以使三个主动长度调节装置2、3、4可以伸长和缩短,使得所述动平台5可绕第一转动副55的轴线以及第四转动副15的轴线转动,及可沿从动长度调节装置1的轴线移动,进而形成一个平动和两个转动自由度。第二转动副35的转轴轴线、第三转动副转轴轴线45以及第四转动副15转轴轴线彼此平行并且与第一转动副55的转轴轴线正交。
本发明的一种含转动支架的五自由度混联机器人的双闭环控制方法,它包括以下步骤:
(1)安装第一圆光栅8、第二圆光栅9以及直线光栅7;
其中在转动支架12与机架连接的第一转动副55的转轴上安装第一圆光栅8的尺身,在第一固定轴座11上安装第一圆光栅的读数头;在转动支架12与从动长度调节装置1连接的第四转动副15的转轴上安装第二圆光栅9的尺身,在转动支架12上安装第二圆光栅的读数头;在从动长度调节装置1轴线移动方向安装直线光栅7的尺身,在从动长度调节装置与转动支架所连接的第四转动副15的内圈内侧安装直线光栅读数头;
(2)利用第一圆光栅8、第二圆光栅9实时检测第一转动副55和第四转动副15的轴线的实际转角ψ、θ,利用直线光栅7实时检测从动长度调节装置1沿移动副方向的移动距离,从而得到从动长度调节装置1实际杆长q4,将所述的实际转角和实际杆长信息通过算法转换为驱动三个主动长度调节装置的伺服电机的实际转角θ1a、θ2a、θ3a,并将其与指令值θ1d、θ2d、θ3d比较,经位置环控制器PID调节转换为速度指令进而实现位置环反馈控制;通过驱动三个主动长度调节装置的伺服电机的电机轴上安装的绝对式角度编码器,实时检测三个伺服电机轴的实际转角并分别作微分处理得到三个伺服电机的实际电机转速 分别与相应的所述的速度指令进行比较,进而通过伺服放大器PI调节实现三个伺服电机的速度环反馈控制;
所述的算法为:
式中:
p——主动长度调节装置中滚珠丝杠的导程
q4——通过直线光栅7检测的移动距离加上从动长度调节装置初始位置杆长而得到的从动长度调节装置1杆长
ψ——第一圆光栅8检测到的第一转动副55的轴线的实际转角
θ——第二圆光栅9检测到的第四转动副15的轴线的实际转角
a1——由第一球链24中心到从动调节装置1轴线的理论垂距
a2——由第二、三球链34、44中心到从动调节装置1轴线的理论垂距
b1——连接第一主动长度调节装置2与第二固定轴座21的虎克铰的近架转轴轴线到转动支架12与机架相连转轴轴线间的理论距离
b2——连接第二主动长度调节装置3与转动支架的第二转动副35的转轴轴线到连接从动长度调节装置1与转动支架的第四转动副15的转轴轴线间的理论距离或者连接第三主动长度调节装置4与转动支架的第三转动副45的转轴轴线到连接从动长度调节装置1与转动支架的第四转动副15的转轴轴线间的理论距离
θ1a、θ2a、θ3a——由第一圆光栅8、第二圆光栅9和直线光栅7检测信息计算得到的驱动三个主动长度调节装置的伺服电机的实际转角。
本发明提出控制方法的有益之处在于,可在线实时补偿并联机构中三个主动长度调节装置因丝杠传动误差、热伸长和因轴向弹性变形造成的杆长误差,弥补了以往传统半闭环控制控制精度不高的局限性,且三个主动长度调节装置伺服电机转角与转动支架相对机架的转角、从动长度调节装置相对转动支架的转角,以及动长度调节装置杆长的数学关系非常简单,故可以每个伺服周期更新一次的速率完成位置闭环控制,可满足这种机器人高速高精度运动控制的需求。此外,在转动支架上安装的两个圆光栅开放性好,便于维修和更换。