一种五自由度混联机器人的双闭环控制方法
【专利摘要】本发明公开了一种含转动支架的五自由度混联机器人的双闭环控制方法，步骤为：利用第一、第二圆光栅实时检测第一、第四转动副的轴线的实际转角，利用直线光栅实时检测从动长度调节装置沿移动副方向的移动距离，将实际转角和实际杆长信息通过算法转换为驱动三个伺服电机的实际转角，并将其与指令值比较，经PID调节转换为速度指令，进而实现位置环反馈控制；通过伺服电机的绝对式角度编码器，实时检测三个伺服电机轴的实际转角并分别作微分处理得到电机的实际转速，分别与相应的速度指令进行比较，进而通过PI调节实现电机的速度环反馈控制。本发明可在线实时补偿主动长度调节装置因丝杠传动误差、热伸长和因轴向弹性变形造成的杆长误差。
【专利说明】
一种五自由度混联机器人的双闭环控制方法
技术领域
[0001 ]本发明涉及机器人的双闭环控制方法，尤其涉及一种五自由度混联机器人的双闭 环控制方法。
【背景技术】
[0002] 工业机器人普遍采用半闭环控制策略，即利用编码器通过检测各关节伺服电机的 实际转角构成位置和速度半闭环控制。这种控制策略调试维修方便，稳定性好，已得到广泛 应用。但半闭环控制无法补偿机械系统传动误差及由构件弹性变形所造成的末端执行器位 姿误差。如可直接检测到机器人机构输出构件的位姿信息，则可实施全闭环反馈控制，进而 有利于提尚机器人末端执彳丁器的运动精度。
[0003] 专利US006301525B1公开了对一种五自由度混联机器人中一平动两转动并联机构 的控制方法。该方法在从动长度调节装置与机架相连的虎克铰两个轴线上安装两个圆光 栅，沿从动长度调节装置轴线安装一个直线光栅。利用这些传感器检测到的转角和长度信 息，首先计算出并联机构动平台参考点的位置误差，然后再经误差逆解模型得到三个主动 长度调节装置伺服电机的转角误差，进而用这些转角误差于修改理想控制指令。然后再利 用伺服电机的编码器检测信息实现位置和速度环调节。这种方法必须经过两步运算，计算 时间长，难于以伺服更新速率实现。此外，在从动长度调节装置与机架相连的虎克铰上安装 的圆光栅开放性不好，不便于维修和更换。
【发明内容】
[0004] 本发明的目的在于克服现有技术的不足，提供一种可实时补偿其中并联机构三个 主动长度调节装置因丝杠传动误差、热误差及构件弹性变形引起的杆长误差，有利于提高 五自由度混联机器人的运动控制精度的五自由度混联机器人的双闭环控制方法。
[0005] 本发明的一种含转动支架的五自由度混联机器人的双闭环控制方法，它包括以下 步骤：
[0006] (1)安装第一圆光栅、第二圆光栅以及直线光栅；
[0007] 其中在转动支架与机架连接的第一转动副的转轴上安装第一圆光栅的尺身，在第 一固定轴座上安装第一圆光栅的读数头;在转动支架与从动长度调节装置连接的第四转动 副的转轴上安装第二圆光栅的尺身，在转动支架上安装第二圆光栅的读数头;在从动长度 调节装置轴线移动方向安装直线光栅的尺身，在从动长度调节装置与转动支架所连接的第 四转动副的内圈内侧安装直线光栅读数头；
[0008] (2)利用第一圆光栅、第二圆光栅实时检测第一转动副和第四转动副的轴线的实 际转角，利用直线光栅实时检测从动长度调节装置沿移动副方向的移动距离，从而得到从 动长度调节装置实际杆长，将所述的实际转角和实际杆长信息通过算法转换为驱动三个主 动长度调节装置的伺服电机的实际转角，并将其与指令值比较，经位置环控制器PID调节转 换为速度指令，进而实现位置环反馈控制;通过驱动三个主动长度调节装置的伺服电机的 电机轴上安装的绝对式角度编码器，实时检测三个伺服电机轴的实际转角并分别作微分处 理得到三个伺服电机的实际电机转速，分别与相应的所述的速度指令进行比较，进而通过 伺服放大器PI调节实现三个伺服电机的速度环反馈控制；
[0009] 所述的算法为：
[0010]
[0011]
[0012] 式中：
[0013] p一一主动长度调节装置中滚珠丝杠的导程
[0014] q4一一通过直线光栅检测的移动距离加上从动长度调节装置初始位置杆长而得 到的从动长度调节装置杆长
[0015] φ一一第一圆光栅检测到的第一转动副的轴线的实际转角
[0016] Θ一一第二圆光栅检测到的第四转动副的轴线的实际转角
[0017] ai 一一由第一球链中心到从动调节装置轴线的理论垂距
[0018] a2一一由第二、三球链中心到从动调节装置轴线的理论垂距
[0019] bi一一连接第一主动长度调节装置与第二固定轴座的虎克铰的近架转轴轴线到 转动支架与机架相连转轴轴线间的理论距离
[0020] b2一一连接第二主动长度调节装置与转动支架的第二转动副的转轴轴线到连接 从动长度调节装置与转动支架的第四转动副的转轴轴线间的理论距离或者连接第三主动 长度调节装置与转动支架的第三转动副的转轴轴线到连接从动长度调节装置与转动支架 的第四转动副的转轴轴线间的理论距离
[0021] 0la、02a、03a--由第一圆光栅、第二圆光栅和直线光栅检测信息计算得到的驱动 三个主动长度调节装置的伺服电机的实际转角。
[0022] 本发明方法与现有技术相比，可在线实时补偿并联机构中三个主动长度调节装置 因丝杠传动误差、热伸长和因轴向弹性变形造成的杆长误差，弥补了以往传统半闭环控制 控制精度不高的局限性，且三个主动长度调节装置伺服电机转角与转动支架相对机架的转 角、从动长度调节装置相对转动支架的转角，以及从动长度调节装置杆长的数学关系非常 简单，故可以每个伺服周期更新一次的速率完成位置闭环控制，可满足这种机器人高速高 精度运动控制的需求。此外，在转动支架上安装的两个圆光栅开放性好，便于维修和更换。
【附图说明】
[0023] 图1是五自由度混联机器人的一个方向的结构示意图；
[0024] 图2是五自由度混联机器人的另一个方向的结构示意图；
[0025] 图3是圆光栅和直光栅安装在图1所示的机器人中的结构示意图；
[0026]图4是本发明的一种五自由度混联机器人的双闭环控制方法的控制原理图；
[0027]图5是五自由度混联机器人的尺度参数示意图。
【具体实施方式】
[0028]下面结合附图和具体实施例对本发明加以详细说明。
[0029] 本发明是对公开号为CN104985596A的中国专利公开的一种含转动支架的五自由 度混联机器人的双闭环控制方法。
[0030] 下面结合附图简要说明CN104985596A中公开的五自由度混联机器人的结构，该机 器人包括具有一个平动和二个转动的三自由度并联机构和与之串接的具有两个转动自由 度的转头组成，转头6与动平台5串接。所述的三自由度并联机构包括转动支架12,所述的转 动支架12的两侧各自通过第一转动副55与安装在机架上的两个第一固定轴座11转动连接。 第二主动长度调节装置3以及第三主动长度调节装置4上部分别通过第二转动副35、第三转 动副45对称的转动连接在所述的转动支架12两侧并且底端分别通过第二球铰34、第三球铰 44与所述的动平台5转动连接，从动长度调节装置1与转动支架12通过第四转动副15转动相 连并且从动长度调节装置1底部与动平台固定相连，第一主动长度调节装置2上部通过虎克 铰25与安装在机架上的两个第二固定轴座21转动连接并且底端通过第一球铰24与动平台5 转动连接；
[0031] 所述的第一主动长度调节装置2、第二主动长度调节装置3以及第三主动长度调节 装置4分别由第一伺服电机23、第二伺服电机33、第三伺服电机43驱动以使三个主动长度调 节装置2、3、4可以伸长和缩短，使得所述动平台5可绕第一转动副55的轴线以及第四转动副 15的轴线转动，及可沿从动长度调节装置1的轴线移动，进而形成一个平动和两个转动自由 度。第二转动副35的转轴轴线、第三转动副转轴轴线45以及第四转动副15转轴轴线彼此平 行并且与第一转动副55的转轴轴线正交。
[0032] 本发明的一种含转动支架的五自由度混联机器人的双闭环控制方法，它包括以下 步骤：
[0033] (1)安装第一圆光栅8、第二圆光栅9以及直线光栅7;
[0034]其中在转动支架12与机架连接的第一转动副55的转轴上安装第一圆光栅8的尺 身，在第一固定轴座11上安装第一圆光栅的读数头;在转动支架12与从动长度调节装置1连 接的第四转动副15的转轴上安装第二圆光栅9的尺身，在转动支架12上安装第二圆光栅的 读数头;在从动长度调节装置1轴线移动方向安装直线光栅7的尺身，在从动长度调节装置 与转动支架所连接的第四转动副15的内圈内侧安装直线光栅读数头；
[0035] (2)利用第一圆光栅8、第二圆光栅9实时检测第一转动副55和第四转动副15的轴 线的实际转角Φ、θ，利用直线光栅7实时检测从动长度调节装置1沿移动副方向的移动距离， 从而得到从动长度调节装置1实际杆长q4,将所述的实际转角和实际杆长信息通过算法转 换为驱动三个主动长度调节装置的伺服电机的实际转角9 la、02a、03a，并将其与指令值0ld、 0 2d、03d比较，经位置环控制器PID调节转换为速度指令九.,、么(/、进而实现位置环反 馈控制；通过驱动三个主动长度调节装置的伺服电机的电机轴上安装的绝对式角度编码 器，实时检测三个伺服电机轴的实际转角并分别作微分处理得到三个伺服电机的实际电机 转速<,、4,,、4,,，分别与相应的所述的速度指令进行比较，进而通过伺服放大器PI调节 实现三个伺服电机的速度环反馈控制；
[0036]所述的算法为：
[0037]
[0038]
[0039] 式中：
[0040] p一一主动长度调节装置中滚珠丝杠的导程
[0041] Q4一一通过直线光栅7检测的移动距离加上从动长度调节装置初始位置杆长而得 到的从动长度调节装置1杆长
[0042] Φ一一第一圆光栅8检测到的第一转动副55的轴线的实际转角
[0043] Θ一一第二圆光栅9检测到的第四转动副15的轴线的实际转角
[0044] ai一一由第一球链24中心到从动调节装置1轴线的理论垂距
[0045] a2一一由第二、三球链34、44中心到从动调节装置1轴线的理论垂距
[0046] bi一一连接第一主动长度调节装置2与第二固定轴座21的虎克铰的近架转轴轴线 到转动支架12与机架相连转轴轴线间的理论距离
[0047] b2一一连接第二主动长度调节装置3与转动支架的第二转动副35的转轴轴线到连 接从动长度调节装置1与转动支架的第四转动副15的转轴轴线间的理论距离或者连接第三 主动长度调节装置4与转动支架的第三转动副45的转轴轴线到连接从动长度调节装置1与 转动支架的第四转动副15的转轴轴线间的理论距离
[0048] 0la、02a、03a-一由第一圆光栅8、第二圆光栅9和直线光栅7检测信息计算得到的驱 动三个主动长度调节装置的伺服电机的实际转角。
[0049] 本发明提出控制方法的有益之处在于，可在线实时补偿并联机构中三个主动长度 调节装置因丝杠传动误差、热伸长和因轴向弹性变形造成的杆长误差，弥补了以往传统半 闭环控制控制精度不高的局限性，且三个主动长度调节装置伺服电机转角与转动支架相对 机架的转角、从动长度调节装置相对转动支架的转角，以及动长度调节装置杆长的数学关 系非常简单，故可以每个伺服周期更新一次的速率完成位置闭环控制，可满足这种机器人 高速高精度运动控制的需求。此外，在转动支架上安装的两个圆光栅开放性好，便于维修和 更换。
【主权项】
1.一种含转动支架的五自由度混联机器人的双闭环控制方法，其特征在于它包括以下 步骤： (1) 安装第一圆光栅、第二圆光栅以及直线光栅； 其中在转动支架与机架连接的第一转动副的转轴上安装第一圆光栅的尺身，在第一固 定轴座上安装第一圆光栅的读数头;在转动支架与从动长度调节装置连接的第四转动副的 转轴上安装第二圆光栅的尺身，在转动支架上安装第二圆光栅的读数头;在从动长度调节 装置轴线移动方向安装直线光栅的尺身，在从动长度调节装置与转动支架所连接的第四转 动副的内圈内侧安装直线光栅读数头； (2) 利用第一圆光栅、第二圆光栅实时检测第一转动副和第四转动副的轴线的实际转 角，利用直线光栅实时检测从动长度调节装置沿移动副方向的移动距离，从而得到从动长 度调节装置实际杆长，将所述的实际转角和实际杆长信息通过算法转换为驱动三个主动长 度调节装置的伺服电机的实际转角，并将其与指令值比较，经位置环控制器PID调节转换为 速度指令，进而实现位置环反馈控制;通过驱动三个主动长度调节装置的伺服电机的电机 轴上安装的绝对式角度编码器，实时检测三个伺服电机轴的实际转角并分别作微分处理得 到三个伺服电机的实际电机转速，分别与相应的所述的速度指令进行比较，进而通过伺服 放大器PI调节实现三个伺服电机的速度环反馈控制； 所述的算法为： 式中：P一一主动长度调节装置中滚珠丝杠的导程 Q4一一通过直线光栅检测的移动距离加上从动长度调节装置初始位置杆长而得到的从 动长度调节装置杆长 Φ一一第一圆光栅检测到的第一转动副的轴线的实际转角 Θ一一第二圆光栅检测到的第四转动副的轴线的实际转角 ai一一由第一球链中心到从动调节装置轴线的理论垂距 a2一一由第二、三球链中心到从动调节装置轴线的理论垂距 bi一一连接第一主动长度调节装置与第二固定轴座的虎克铰的近架转轴轴线到转动支 架与机架相连转轴轴线间的理论距离 b2一一连接第二主动长度调节装置与转动支架的第二转动副的转轴轴线到连接从动长 度调节装置与转动支架的第四转动副的转轴轴线间的理论距离或者连接第三主动长度调 节装置与转动支架的第三转动副的转轴轴线到连接从动长度调节装置与转动支架的第四 转动副的转轴轴线间的理论距离 0la、02a、03a-一由第一圆光栅、第二圆光栅和直线光栅检测信息计算得到的驱动三个 主动长度调节装置的伺服电机的实际转角。
【文档编号】B25J9/16GK105945954SQ201610465824
【公开日】2016年9月21日
【申请日】2016年6月21日
【发明人】黄田, 刘祺, 董成林, 肖聚亮, 刘海涛, 梅江平, 田文杰, 丁雅斌
【申请人】天津大学