CN101226386B - 并联机构机床的控制方法及控制装置 - Google Patents

Abstract

本发明提供一种并联机构机床的控制方法及控制装置，进行考虑了全部从动轴的旋转阻力的影响的补偿，由此可以高精度地确定末端执行器的位置和姿态。首先在第1步骤(S1)，求出针对末端执行器的位置指令值和姿态指令值的各致动器指令值。然后在第2步骤(S2)，获取第1和第2万向接头的旋转阻力值，在第3步骤(S3)，使用旋转阻力值计算作用于各第2万向接头的力和力矩，在第4步骤，根据这些力和力矩求出作用于末端执行器的合力和合力矩。然后在第5步骤，使用这些合力和合力矩计算机构的弹性变形量，使用该值计算致动器指令值的补偿量。然后在第6步骤，对于在第1步骤求出的致动器指令值，加算在第5步骤求出的补偿量以进行更新。

Description

并联机构机床的控制方法及控制装置

技术领域

本发明涉及并联机构机床(パラレルメカニズム機械)的控制方法及可以执行该方法的控制装置，可以利用由致动器驱动的多个支柱，使通过接头与这些支柱连接的末端执行器移动。

背景技术

作为以往的并联机构机床的控制方法，已公知有下述专利文献1记载的方法。在该方法中，在并联机构机床中，以求出加工时的切削阻力为目的，除了由致动器主动驱动的主动轴的输送阻力的影响外，还把在未由致动器主动驱动的从动轴上产生的旋转阻力和惯性项及重力项作为参数，将干扰模型化，使用干扰观测器同定各参数，由此提高加工精度。在此，该方法不将全部从动轴作为对象，只对一部分进行模型化。

【专利文献1】日本特开2006-11752号公报

并且，作为以往的作业机床的控制装置，已公知有下述专利文献2记载的装置。该装置不进行注重于并联机构机床的控制，而进行使用一般作业机床的致动器的负荷来诊断的控制。在该控制中，在作业机床正常时，关于致动器的负荷可以预先获得正常时的测定数据，通过比较该正常时的测定数据和诊断时的测定数据来进行诊断。

【专利文献2】日本特开平5-285788号公报

在专利文献1那样的现有方法中，除了主动轴的输送阻力外，还考虑了从动轴的旋转阻力，从这一点讲在某种程度上精度提高了，但是由于没有考虑未成为对象的从动轴的影响，所以根据条件，误差相应增大。而且在前述现有方法中，利用与前述主要因素的组合来同定旋转阻力，因此来自前述主要因素的影响和从动轴的影响混合在一起，误差根据条件而增大，即使想要进行使用了从动轴的旋转阻力的实测值的补偿，由于产生与前述主要因素的干涉，所以也有可能不能获得高精度的补偿。

发明内容

因此，本发明第一方面、第五方面所述发明的课题是提供一种并联机构机床的控制方法、控制装置，可以将全部从动轴选择为考虑对象，通过构建独立于惯性和重力等影响之外的模型，进行只单纯地抽取旋转阻力的影响的补偿，由此可以更加高精度地确定末端执行器的位置和姿态。

并且，本发明第九方面、第十方面所述发明的课题是提供一种并联机构机床的控制方法、控制装置，可以去除因旋转阻力造成的机构要素的变形误差的影响，提高机构参数的推测精度。

另一方面，即使将专利文献2那样的现有装置简单地应用于并联机构机床，在现有装置中也只能在预先获取的正常时测定数据的测定条件下进行诊断，因此诊断时的测定条件受并联机构机床的状态的限制，而且在该测定条件与正常时测定数据的测定条件不同的情况下，难以进行正确诊断，在进行把指定的支柱或接头作为对象的具体诊断时，未明确获得哪种测定条件下的正常时测定数据都可以，也难以实施针对指定支柱或接头的正确诊断。并且，即使能够比较正常时的数据来进行正常/异常的判别，也难以指定产生了异常的支柱或接头。

并且，在诊断并联机构机床时，从指定异常部位的观点考虑，需要控制装置掌握各接头的旋转阻力的增大，但为了获得接头的旋转阻力值，只能在将接头分解后实测。

因此，本发明第十一、十二方面、第十六、十七方面所述发明的课题是提供一种并联机构机床的控制方法、控制装置，可以容易且高精度地获取各接头的旋转阻力值。

并且，本发明第十五方面、第二十方面所述发明的课题是提供一种并联机构机床的诊断控制方法、诊断控制装置，通过在诊断时使用所得到的旋转阻力值，可以更加准确地指定异常部位。

为了解决上述课题，本发明第一方面提供一种并联机构机床的控制方法，所述并联机构机床具有：固定在外部的基座；通过第1万向接头与基座连接的多个支柱；驱动各支柱的致动器；通过第2万向接头与各支柱连接的末端执行器；提供致动器指令值并控制各致动器的控制单元；和旋转阻力值存储单元，其设于控制单元中，可以存储第1万向接头和/或第2万向接头的旋转阻力值，其特征在于，该控制方法包括：第1步骤，根据并联机构机床的预定的机构参数，求出针对末端执行器的位置指令值和姿态指令值的各致动器指令值；第2步骤，从旋转阻力值存储单元获取第1万向接头和/或第2万向接头的旋转阻力值；第3步骤，使用旋转阻力值计算作用于各第2万向接头的力和力矩；第4步骤，根据作用于各第2万向接头的力和力矩，求出作用于末端执行器的合力和合力矩；第5步骤，使用作用于末端执行器的合力和合力矩计算机构的弹性变形量，使用该值计算致动器指令值的补偿量；以及第6步骤，对于在第1步骤求出的致动器指令值，加算在第5步骤求出的补偿量以进行更新。

本发明第二方面所述的发明除上述课题外，为了解决提供一种以少的计算量进行高精度的有效补偿的控制方法的课题，其特征在于，在第3步骤中，将由支柱、第1万向接头、第2万向接头构成的连杆视为分别独立的串联连杆机构，计算通过各个串联连杆机构的万向接头的旋转阻力作用于支柱与末端执行器的连接部的力和力矩。

本发明第三方面所述的发明除上述课题外，为了解决提供一种在第1万向接头或第2万向接头反转时也进行高精度补偿的控制方法的课题，其特征在于，在第3步骤中，利用线性函数使第1万向接头或第2万向接头反转时的旋转阻力值的变化近似。

本发明第四方面所述的发明除上述课题外，为了解决提供一种在第1万向接头或第2万向接头反转时也以更少的计算量有效进行高精度补偿的控制方法的课题，其特征在于，在第3步骤中，对在第1万向接头或第2万向接头反转时反转的旋转轴的旋转阻力值进行移动平均处理。

为了解决上述课题，本发明第五方面所述的发明提供一种并联机构机床的控制装置，所述并联机构机床具有：固定在外部的基座；通过第1万向接头与基座连接的多个支柱；驱动各支柱的致动器；通过第2万向接头与各支柱连接的末端执行器；提供致动器指令值并控制各致动器的控制单元；和旋转阻力值存储单元，其设于控制单元中，可以存储第1万向接头和/或第2万向接头的旋转阻力值，其特征在于，所述控制装置具有：致动器指令值计算单元，其根据并联机构机床的预定的机构参数，求出针对末端执行器的位置指令值和姿态指令值的各致动器指令值；合力运算单元，其使用从旋转阻力值存储单元获取的旋转阻力值计算作用于各第2万向接头的力和力矩，根据作用于各第2万向接头的力和力矩，求出作用于末端执行器的合力和合力矩；补偿量运算单元，其使用作用于末端执行器的合力和合力矩来计算机构的弹性变形量，使用该值计算致动器指令值的补偿量；以及所述控制单元，其对于通过致动器指令值计算单元求出的致动器指令值，加算通过补偿量运算单元求出的补偿量以进行更新。

本发明第六方面所述的发明除上述课题外，为了解决提供一种以少的计算量进行高精度的有效补偿的控制装置的课题，其特征在于，合力运算单元将由支柱、第1万向接头、第2万向接头构成的连杆视为分别独立的串联连杆机构，计算通过各个串联连杆机构的万向接头的旋转阻力作用于支柱与末端执行器的连接部的力和力矩。

本发明第七方面所述的发明除上述课题外，为了解决提供一种在第1万向接头或第2万向接头反转时也进行高精度补偿的控制方法的课题，其特征在于，合力运算单元利用线性函数使第1万向接头或第2万向接头反转时的旋转阻力值的变化近似。

本发明第八方面所述的发明除上述课题外，为了解决提供一种在第1万向接头或第2万向接头反转时也以更少的计算量有效进行高精度补偿的控制方法的课题，其特征在于，合力运算单元对在第1万向接头或第2万向接头反转时反转的旋转轴的旋转阻力值进行移动平均处理。

为了解决上述课题，本发明第九方面所述的发明提供一种并联机构机床的控制方法，所述并联机构机床具有：通过第2万向接头与支柱连接的末端执行器；驱动支柱的致动器；提供致动器指令值并控制各致动器的控制单元；和旋转阻力值存储单元，其设于控制单元中，可以存储第1万向接头和/或第2万向接头的旋转阻力值，所述控制方法在将末端执行器定位于多个位置和姿态时，进行位置和/或姿态的测定或者距固定点的距离测定，以该测定值为基础，推测并校准并联机构机床的机构参数，其特征在于，在利用控制单元推测计算机构参数时，加算由第1万向接头和/或第2万向接头的旋转阻力引起的变形误差。

为了解决上述课题，本发明第十方面所述的发明提供一种并联机构机床的控制装置，所述并联机构机床具有：通过第2万向接头与支柱连接的末端执行器；驱动支柱的致动器；提供致动器指令值并控制各致动器的控制单元；和旋转阻力值存储单元，其设于控制单元中，可以存储第1万向接头和/或第2万向接头的旋转阻力值，其特征在于，控制单元在将末端执行器定位于多个位置和姿态时，进行位置和/或姿态的测定或者距固定点的距离测定，以该测定值为基础，推测并联机构机床的机构参数，由此进行校准，在推测计算该校准涉及的机构参数时，加算由第1万向接头和/或第2万向接头的旋转阻力引起的变形误差。

为了解决上述课题，本发明第十一方面所述的发明提供一种并联机构机床的控制方法，所述并联机构机床具有：固定在外部的基座；分别通过第1万向接头与基座连接的多个支柱；驱动各支柱的各个致动器；分别通过第2万向接头与各支柱连接的末端执行器；和检测各致动器的负荷的负荷检测器，其特征在于，根据由负荷检测器检测的负荷，推测第1万向接头和/或第2万向接头的旋转阻力值。

为了解决上述课题，本发明第十二方面所述的发明提供一种并联机构机床的控制方法，所述并联机构机床具有：固定在外部的基座；分别通过第1万向接头与基座连接的多个支柱；驱动各支柱的各个致动器；分别通过第2万向接头与各支柱连接的末端执行器；和检测各致动器的负荷的负荷检测器，其特征在于，关于通过第1万向接头和/或第2万向接头的旋转阻力作用于末端执行器的内力，将包括各第1万向接头、对应的支柱和对应的第2万向接头的各个脚轴系统，视为串联连杆机构，由此使用各个脚轴系统涉及的雅可比矩阵以及第1万向接头和/或第2万向接头的旋转阻力向量表示的算式，与根据并联连杆机构的静力学关系使用并联连杆机构的雅可比矩阵以及各致动器的负荷向量表示的算式相等，据此，根据由负荷检测器检测的负荷构成的负荷向量，求出第1万向接头和/或第2万向接头的旋转阻力向量，由此推测作为该旋转阻力向量的构成要素的第1万向接头和/或第2万向接头的旋转阻力值。

为了解决上述课题，本发明第十三方面所述的发明提供一种上述补偿涉及的控制方法，使用上述推测涉及的旋转阻力值。

本发明第十四方面所述的发明除上述课题外，为了实现与并联机构机床的状态对应的补偿参数的自动更新，其特征在于，控制单元对旋转阻力值存储单元中的推测的旋转阻力值的存储进行更新。

为了解决上述课题，本发明第十五方面所述的发明的特征在于，关于所推测的第1万向接头和/或第2万向接头的各旋转阻力值，在超过预定阈值时，判定为该旋转阻力值涉及的第1万向接头和/或第2万向接头产生异常。

为了解决上述课题，本发明第十六方面所述的发明提供一种并联机构机床的控制装置，所述并联机构机床具有：固定在外部的基座；分别通过第1万向接头与基座连接的多个支柱；驱动各支柱的各个致动器；分别通过第2万向接头与各个支柱连接的末端执行器；和检测各致动器的负荷的负荷检测器，所述控制装置的特征在于，根据由负荷检测器检测的负荷，推测第1万向接头和/或第2万向接头的旋转阻力值。

为了解决上述课题，本发明第十七方面所述的发明提供一种控制并联机构机床的控制装置，所述并联机构机床具有：固定在外部的基座；分别通过第1万向接头与基座连接的多个支柱；驱动各支柱的各个致动器；分别通过第2万向接头与各支柱连接的末端执行器；和检测各致动器的负荷的负荷检测器，所述控制装置的特征在于，关于通过第1万向接头和/或第2万向接头的旋转阻力作用于末端执行器的内力，将包括各第1万向接头、对应的支柱和对应的第2万向接头的各个脚轴系统，视为串联连杆机构，由此使用各个脚轴系统涉及的雅可比矩阵以及第1万向接头和/或第2万向接头的旋转阻力向量表示的算式，与根据并联连杆机构的静力学关系使用并联连杆机构的雅可比矩阵以及各致动器的负荷向量表示的算式相等，据此，根据由负荷检测器检测的负荷构成的负荷向量，求出第1万向接头和/或第2万向接头的旋转阻力向量，由此推测作为该旋转阻力向量的构成要素的第1万向接头和/或第2万向接头的旋转阻力值。

为了解决上述课题，本发明第十八方面所述的发明提供一种上述补偿涉及的控制装置，使用上述推测涉及的旋转阻力值。

本发明第十九方面所述的发明除上述课题外，为了实现一种与并联机构机床的状态对应的补偿参数的自动更新，其特征在于，该控制装置对旋转阻力值存储单元中的推测的旋转阻力值的存储进行更新。

为了解决上述课题，本发明第二十方面所述的发明的特征在于，关于所推测的第1万向接头和/或第2万向接头的各旋转阻力值，在超过预定阈值时，判定为该旋转阻力值涉及的第1万向接头和/或第2万向接头产生异常。

根据本发明发挥以下效果，可以提供一种并联机构机床的控制方法及控制装置，进行选择性地考虑了全部从动轴的旋转阻力的影响的补偿，构建独立于惯性和重力等的影响之外的模型，进行只单纯地抽取由旋转阻力引起的影响的补偿，由此可以更加高精度地确定末端执行器的位置和姿态。

并且，根据本发明，在推测计算校准涉及的机构参数时，加算由第1万向接头和/或第2万向接头的旋转阻力引起的变形误差，因此能够去除由旋转阻力引起的机构要素的变形误差的影响，进一步提高机构参数的推测精度。

附图说明

图1是利用本发明的控制方法控制的并联机构机床的局部透视立体图。

图2是表示图1中的脚轴的说明图。

图3是本发明中致动器指令值的补偿所涉及的控制装置的方框图。

图4是与本发明中致动器指令值的补偿所涉及的控制方法相关的实施方式的流程图。

图5是图1中的第2万向接头的旋转轴至万向接头轴的说明图。

图6是图1中的第1及第2万向接头的旋转中心位移与支柱轴向量的关系的说明图。

图7是在图1所示的并联机构机床中，(a)不应用本发明的情况、(b)应用了本发明的情况的、基于DBB法的圆度测定结果的曲线图。

图8是表示不进行线性近似时的第2万向接头的旋转阻力的转变的一例的曲线图。

图9是表示由第2万向接头的旋转阻力造成的滚珠丝杠的变形的示意图。

图10是表示进行线性近似时的第1万向接头或第2万向接头的旋转阻力的转变的一例的曲线图。

图11是(a)不进行线性近似时、(b)进行线性近似时基于DBB法的推测误差值的曲线图。

图12是(a)不进行线性近似时、(b)进行线性近似时基于DBB法的伺服电动机负荷的曲线图。

图13是根据第1和/或第2万向接头的旋转阻力而呈现于DBB轨迹中的变形误差量的一例的曲线图。

图14是针对图1所示并联机构机床的校准的实施方式的流程图。

图15是(a)不进行图14所示校准的情况、(b)进行图14所示校准的情况的、基于DBB法的圆度测定结果的曲线图。

图16是表示图1中的第1万向接头的说明图。

图17是表示图1中的第2万向接头的说明图。

图18是本发明中旋转阻力值的推测所涉及的控制装置和控制对象的一部分的方框图。

图19是与本发明中旋转阻力值的推测所涉及的控制方法相关的实施方式的流程图。

图20是与本发明中旋转阻力值的推测所涉及的控制方法相关的实施方式的流程图。

图21是表示图1所示并联机构机床中的某个脚轴系统的旋转阻力值的测定值与本发明涉及的推测值的曲线图。

图22(a)～(f)是表示对应的伺服电动机的负荷的实测值与推测值的曲线图。

图23是表示DBB轨迹的实测值与根据本发明涉及的推测值求出的DBB轨迹的曲线图。

符号说明

1框架；2第1万向接头；3伺服电动机(致动器)；4滚珠丝杠(支柱)；5第2万向接头；6末端执行器；7工作台。

具体实施方式

以下，适当根据附图说明本发明的实施方式涉及的并联机构机床。

图1是六自由度斯图沃特平台式的并联机构机床的说明图，该并联机构机床具有：固定在地面上的作为基座的框架1；固定在框架1上的6个第1万向接头2a～2f；连接在各第1万向接头2a～2f上的作为致动器的伺服电动机3a～3f；由各伺服电动机3a～3f驱动的作为支柱的滚珠丝杠4a～4f；连接在各滚珠丝杠4a～4f的下端的第2万向接头5a～5f；具有第2万向接头5a～5f的1个末端执行器6；以及在末端执行器6的对置位置固定在框架1上的工作台7。

末端执行器6在下表面具有刀具安装部和刀具旋转机构。工作台7具有固定加工物的机构。另外，在各伺服电动机3a～3f中安装有未图示的编码器，该编码器用于输出与自基准状态的旋转角度或转速对应的数值，编码器输出的信息反馈给控制单元16(图3)。

并且，并联机构机床1具有由第1万向接头2a、伺服电动机3a、滚珠丝杠4a、第2万向接头5a构成的脚轴系统的结构，并且具有合计6组这种相同的脚轴系统的结构。以下，在各脚轴相同时，主要利用省略了符号中的a～f所表示的代表示例进行说明。另外，图2表示1组脚轴。

第1万向接头2形成为使用了两旋转自由度的三重环的万向接头结构。最外侧的环固定在框架1上，并将支撑放射方向的轴的轴承包于内侧。并且，具有轴的中间的环通过将该轴支撑在最外侧环的轴承上，从而被安装为接头。另外，与最外侧和中间的环相同，中间与最内侧的环被安装为接头。但是，最内侧的环的轴的方向与中间的环的轴的方向正交。这样，利用两个接头的组合，形成两自由度的第1万向接头2。

第2万向接头5包括三旋转自由度的凸缘、两根轴和杆。凸缘31(图5)垂直地固定在与末端执行器6的下表面正交的侧面上。轴32(图5)通过轴承安装在凸缘31上，围绕与末端执行器6的侧面垂直的旋转轴旋转。杆33(图5)通过轴承安装在轴32上，围绕与所述旋转轴正交的旋转轴旋转。另外，杆33安装在轴32上，内包有围绕自身的轴旋转的轴承，在该轴承上连接着滚珠丝杠4。这样，利用3个轴承(接头)的组合，形成三自由度的第2万向接头5。

并且，并联机构机床1的脚轴系统形成闭环，另外，各脚轴系统如下面所示，可以视为具有1个主动轴和5个从动轴的六自由度串联连杆(serial link)机构。即，各脚轴系统具有：通过伺服电动机3成为主动轴的滚珠丝杠4；作为两旋转自由度的从动轴的第1万向接头2；和作为三旋转自由度的从动轴的第2万向接头5，整体上具有1个主动轴和5个从动轴。

图3是并联机构机床的控制装置的方框图。控制装置具有：CPU(未图示)；临时存储CPU为了进行控制而执行的程序的各个步骤等的存储器(未图示)；显示各种信息的显示部(未图示)；接受各种输入的输入部(未图示)；存储程序等的存储单元(未图示)；上述各部分与CPU的接口(未图示)；各伺服电动机3(致动器)与CPU的接口17；每个伺服电动机3的驱动单元(未图示)。在存储单元中存储着作为程序的一部分的接头旋转轴角度运算单元10、合力运算单元11、旋转阻力值存储单元12、末端执行器指令值计算单元13、补偿量运算单元14、致动器指令值计算单元15。

图4表示这种并联机构机床1的控制装置执行的控制方法所涉及的流程图。首先，在作为第1步骤的步骤S1中，控制装置的致动器指令值计算单元15根据末端执行器6的位置和姿态的指令值X_ref，计算各伺服电动机3的指令值l_i。并联机构机床1的机构参数包括第1万向接头2的旋转中心位置向量Q_i(i：1～6，依次对应于符号a～f表示的部件，以后相同)、末端执行器6的坐标系中的各第2万向接头5的旋转中心位置向量R_i、支柱基准长度l_Bi。致动器指令值计算单元15根据汇总了末端执行器6的位置指令值(x、y、z)和姿态指令值(a、b、c)的指令值X_ref(x、y、z、a、b、c)，利用下述[算式1]求出支柱长度指令值l_i。这里，E是将与X_ref相关的平行移动和旋转移动合成后的矩阵。

【算式1】

l_i＝|Q_i-E^T(R_i)|

另外，接头旋转轴角度运算单元10运算各脚轴系统的第1、第2万向接头2、5的旋转轴角度θ_i。旋转轴角度θ_i也表示滚珠丝杠4的长度。

然后，在作为第2步骤的步骤S2中，获取存储在从动轴的旋转阻力值存储单元12中的各从动轴的旋转阻力测定值即旋转阻力f_i。在将任意接头从考虑对象中去掉时，把相应f_i的分量设为0。由此，可以任意选择要考虑的接头。

然后，为了分析并联机构机床1的各脚轴系统的从动轴的影响，假设各脚轴系统是前面叙述的六自由度串联连杆机构，根据下述[算式2]，使用各从动轴的旋转阻力值，计算作用于第2万向接头5的力和力矩(作为第3步骤的步骤S3)。

【算式2】

$F_i={\left({{\mathrm{Js}}_i}^T\right)}^{-1}\·f_i$

f_i：从动轴的旋转阻力  J_s：串联连杆的雅可比矩阵

F_i：作用于第2万向接头5(连杆前端)的力

另外，在作为第4步骤的步骤S4中，控制装置的合力运算单元11根据在步骤S3求出的作用于连杆前端的力和力矩，按照下面的[算式3]求出作用于末端执行器6的内力的合力和合力矩(F)。这里，T_i表示从第2万向接头5到末端执行器6的变换矩阵，利用下述[算式4]定义。

【算式3】

$F=\underset{i}{\mathrm{\Σ}}{T}_i{F}_i$

【算式4】

$T_i=\left[\begin{array}{cc}{R}_i& 0\\ {\tilde{P}}_i\·R_i& R_i\end{array}\right]$

P_i：从第2万向接头5到末端执行器6的位置向量

R_i：从第2万向接头5到末端执行器6的旋转矩阵

：P_i的反对称矩阵(skew-symmetric matrix)

并且，控制装置的补偿量运算单元14使用在步骤S4求出的作用于末端执行器6的合力、合力矩F，利用本案申请人的在先申请即日本特开2005-186210中记载的方法，计算各伺服电动机3的控制量的校正值，进行末端执行器6的位移量的校正(步骤S5、S6)。更具体的情况如下所述。

即，在作为第5步骤的步骤S5中，根据并联机构机床1的机构参数、末端执行器6的位置和姿态的指令值按照公知方式确定的6行6列的雅可比矩阵(Jacobian matrix)J，与由各支柱轴方向的荷重w_i构成的荷重向量W和上述合力F，具有下述[算式5]的关系，因此如果求出雅可比矩阵J的逆矩阵J^-1，则成为下述[算式6]，可以求出荷重向量W，并求出各支柱轴方向的荷重w_i。另外，荷重向量W利用下述[算式7]表示。

【算式5】

F＝J·W

【算式6】

W＝J^-1·F

【算式7】

W＝[w₁，w₂，w₃，w₄，w₅，w₆]^T

另外，补偿量运算单元14进行将按照上面所述算出的支柱轴方向的荷重在两个万向接头2、5中的各接头的轴方向(万向接头轴的方向)分解的运算。图5是第2万向接头5的模型，与第2万向接头5的旋转轴P_a、P_b、U_i区别来定义被固定在末端执行器6上的万向接头轴P_I、P_J、P_K。其中，U_i表示支柱轴方向(滚珠丝杠4的方向)的单位向量。万向接头轴P_I～P_K在同一正交点相互正交而规定了正交坐标系，该正交点与旋转轴P_a～P_c的正交点相同，但在附图中为了便于理解而错开表示。并且，将旋转轴P_a与万向接头轴P_K取为相同方向，使万向接头轴P_J与末端执行器6的下表面平行，使万向接头轴P_I与末端执行器6的下表面垂直。

算出的各支柱轴方向的荷重作用于第2万向接头5。控制装置将该荷重在安装有第2万向接头5的末端执行器6上固定的万向接头轴P_I～P_K的各方向上分解。该分解根据下述[算式8]进行。另外，万向接头轴P_I、P_J、P_K根据末端执行器6的位置和姿态而变化，也因第2万向接头5在末端执行器6上的每个安装面(每个安装角度)而不同。

【算式8】

$\left\{\begin{array}{c}{w}_{\mathrm{PIi}}=w_i(U_i\·P_{\mathrm{Ii}})\\ w_{\mathrm{PJi}}=w_i(U_i\·P_{\mathrm{Ji}})\\ w_{\mathrm{PKi}}=w_i(U_i\·P_{\mathrm{Ki}})\end{array}\right.$

W_PIi、W_PJi、W_PKi：在万向接头轴P_I、P_J、P_K方向分解的荷重

P_Ii、P_Ji、P_Ki：万向接头轴P_I、P_J、P_K的单位向量

关于第1万向接头2，与第2万向接头5同样，可以在各万向接头轴方向分解各支柱轴方向荷重。但是，作为万向接头轴，使用固定在安装有第1万向接头2的框架1上的万向接头轴。施加给第1万向接头2的荷重w_i’利用下述[算式9]表示。

【算式9】

w_i′＝w_i+w_PCi+w_BSi

然后，补偿量运算单元14算出并联机构机床1的各要素的弹性变形量。控制装置首先推测各支柱轴方向(致动器轴方向)的各要素的弹性变形量。支柱轴方向的荷重w_i作用于各支柱轴方向的各要素。

并且，控制装置如图5所示，在考虑了施加的荷重因各要素的位置关系而不同的基础上，利用各要素的支柱轴方向的柔量(刚性的倒数)与各荷重，根据下述[算式10]可以求出作为第3弹性变形量的各支柱轴方向的弹性变形量Δl_i。这里，l_i’表示根据末端执行器6的位置和姿态的指令值确定的支柱轴长度中滚珠丝杠4占据的长度，c_LOi、c_LAi、c_LBi依次表示：根据支柱轴长度而变化的滚珠丝杠4的部件的柔量；滚珠丝杠4的部件的柔量；因滚珠丝杠4的自重引起的自身变形换算后的柔量，c_MOi、c_MAi、c_MBi依次表示：不依赖于支柱轴长度的凸缘51、杆52、各种轴承、滚珠丝杠4的螺母和第1万向接头2的环的柔量；各种轴承、滚珠丝杠4的螺母、第1万向接头2的环的柔量；各种轴承、滚珠丝杠4的螺母、环41、44的柔量(部分省略)。

【算式10】

Δl_i＝(c_LOil_i′+c_MOi)w_i+(c_LAil_i′+c_MAi)w_PCi+(c_LBil_i′+c_MBi)w_BSi

然后，控制装置推测第2万向接头5的弹性变形量。第2万向接头5的各要素的柔量一般分为关于要素中的各轴承的轴方向(轴向)及与其垂直的方向(径向)来分配。如图5所示，如果考虑万向接头轴P_I～P_K，则轴51所涉及的轴承的轴向为万向接头轴P_K方向，径向为万向接头轴P_I、P_J的合成方向。另外，关于轴52、杆53所涉及的轴承，不需要考虑支柱轴方向荷重，或者只有支柱轴方向荷重起作用，因此不进行用于算出第1或第2万向接头的弹性变形量(第1或第2弹性变形量)的支柱轴方向荷重的分解。

因此，对于在万向接头轴P_I～P_K方向分解有关各个轴承的柔量，容易从预定的柔量得到。并且，根据在万向接头轴P_I～P_K方向分解的柔量和万向接头轴P_I～P_K方向的荷重，求出第2万向接头5的弹性变形量。但是，由于第2万向接头5被固定在末端执行器6上，受到因作用于第2万向接头5的荷重引起的末端执行器6的变形的影响，所以对该影响作线性近似并加算。因此，根据下述[算式11]推测作为第2弹性变形量的第2万向接头5的弹性变形量ΔP_i。这里，c_PIi、c_PJi、c_PKi分别表示万向接头轴P_I、P_J、P_K方向的柔量，j表示第i个滚珠丝杠4等(支柱)的相邻的轴序号，k_Pj表示作为比例常数的相邻影响系数。

【算式11】

ΔP_i＝(c_PIiw_PIi)P_Ii+(c_PJiw_PJi)P_Ji+(c_PKiw_PKi+k_Pjc_PKjw_PKj)P_Ki

另一方面，第1万向接头2的弹性变形量也可以与第2万向接头5同样求出。但是，给邻接带来影响的是框架1。即，施加给第1万向接头2中一方的荷重，给框架1中相邻的第1万向接头2的安装部分带来影响而使其变形。此时，对固定在框架1上的第1万向接头轴Q_i～Q_k中第1万向接头轴Q_k方向的影响较大，其他方向可以忽视，因此在[算式11]中，只对万向接头轴Q_k方向适用相邻影响系数。

然后，控制装置把因两个万向接头进行弹性变形而引起的位移换算到致动器轴方向。即，捕捉按照上面所述求出的第1和第2万向接头2、5的弹性变形，作为各万向接头2、5的旋转中心的移动。图6表示第1、第2万向接头2、5的旋转中心位移的向量ΔP_i、ΔQ_i与支柱轴向量L_i的关系。这里，Q_i、P_i分别表示弹性变形前的第1、第2万向接头2、5的旋转中心，Q_i’、P_i’分别表示弹性变形后的第1、第2万向接头2、5的旋转中心，L_i’表示弹性变形后的支柱轴向量。根据这种关系，各万向接头旋转中心的位移向量与支柱轴向量的关系可以利用下述[算式12]表示。

【算式12】

L_i′＝L_i+ΔP_i-ΔQ_i

另外，由于两个万向接头2、5的位移微小，所以可以近似于支柱轴长度位移。即，从使旋转中心位移向量ΔP_i、ΔQ_i的支柱轴方向分量近似于支柱轴长度位移Δl_Ji的观点考虑，可以根据下述[算式13]将各万向接头2、5的位移变换为支柱轴长度位移Δl_Ji。此外，在并联机构机床1中，由于支柱轴方向(滚珠丝杠4的方向)与致动器轴方向(伺服电动机3的轴方向)为同一方向，所以能够以支柱轴长度位移Δl_Ji作为换算到致动器轴方向的位移。

【算式13】

Δl_Ji＝U_i·(ΔP_i-ΔQ_i)

并且，在作为第6步骤的步骤S6中，控制装置的控制单元16对致动器指令值进行更新。即，如下述[算式14]所示，从目标指令值l_i减去在步骤S5求出的各支柱长度的位移Δl_i、Δl_Ji，校正支柱长度指令值即致动器指令值，并发送给接口17。

【算式14】

l_i←l_i-Δl_i-Δl_Ji

通过以该补偿后的致动器指令值为基础控制伺服电动机3，从而并联机构机床1的弹性变形量相抵消。即，通过对最初的致动器指令值赋予补偿后的指令值，可以抵消与第1、第2万向接头2、5相关的弹性变形。

图7表示使用并联机构机床1测定基于Double Ball Bar(DBB：双球规)法的圆弧轨迹的圆度的结果。图7(a)是不使用任何补偿的情况，图7(b)是使用本发明涉及的补偿的情况。在没有补偿时，根据旋转方向的不同，可以看到较大的迟滞(hysteresis)，并且波形在各方向也大大变形。另一方面，在采用了本发明涉及的补偿时，迟滞被大幅改善，并且可以确认到各个方向的圆度大幅提高。

但是，更加具体地讲，在上述控制中使用预定的旋转阻力f_i，在使旋转方向反转时，只有旋转阻力f_i的正负变化并呈阶梯性转变(例如参照图8)，据此形成的变形误差推测值也呈阶梯性变化，结果，对应于动作方向的反转，在基于DBB法的圆弧轨迹的变形误差推测值中，在接头的反转相位附近呈现阶梯状的半径变化。并且，实际上旋转阻力f_i的值连续变化，在DBB法的轨迹中，在反转相位附近，推测值也连续变化，所以在接头反转相位附近，推测值偏离实测值。

因此，在利用控制装置算出变形误差推测值时，作为进一步提高精度的控制，存在通过调整动作方向反转时的旋转阻力f_i的值的阶梯性变化来进行的控制。并且，该调整基于在第1万向接头2和/或第2万向接头5的旋转轴周围作用于末端执行器6的力的关系。

在这些力的关系中，利用伺服电动机3的驱动力驱动末端执行器6时的绕旋转轴的转矩的存在很重要。在该转矩起作用时，根据并联机构的各要素的弹性变形力和第1万向接头2或第2万向接头5的旋转阻力的大小，第1万向接头2或第2万向接头5的动作发生变化。在此以绕第2万向接头5的动作为例进行说明。在该弹性变形力小于第2万向接头5的旋转阻力时(弹性变形力＜旋转阻力)，第2万向接头5不旋转，机构要素伴随末端执行器6的移动而变形(参照图9，箭头G表示末端执行器6的移动方向，箭头H指出其前方的由旋转阻力形成的滚珠丝杠的变形)。并且，在变形量积累、弹性变形力与旋转阻力相等时(弹性变形力＝旋转阻力)，第2万向接头5开始旋转，以后在弹性变形力和旋转阻力平衡的状态下转移，直到第2万向接头5停止。

因此，作为作用于末端执行器6的力，在第2万向接头5开始旋转前的区域中是与滚珠丝杠4的弹性变形量成比例的弹性力，在第2万向接头5开始旋转后是恒定值的旋转阻力。因此，作用于动作方向反转后的末端执行器6的转矩的推移，例如，如相对于图8所示的图10那样，反转后马上呈线性推移，然后成为恒定值。

因此，关于利用控制装置进行的旋转阻力f_i的值的变化的调整，可以列举利用线性函数使刚刚反转后的旋转阻力f_i的变化近似的情况。并且，关于该线性函数，把机构要素的弹性系数作为参数，作为具体示例，形成为具有对应于滚珠丝杠4的弹性系数的倾斜。

图11(a)表示使旋转阻力f_i的值按照图8所示那样呈阶梯状不连续地转变时的DBB轨迹的推测误差，图11(b)表示按照图10所示那样进行线性近似时通过控制装置算出的DBB轨迹的推测误差。由此可知，在线性近似的情况下，推测误差变小，实测值与推测值的一致程度较高。并且，图12(a)表示使旋转阻力f_i的值按照图8所示那样呈阶梯状不连续地转变时的伺服电动机3的负荷，图12(b)表示按照图10所示那样进行线性近似时的伺服电动机3的负荷。由此可知，在进行线性近似的情况下，特别是在反转动作附近，推测值更接近实测值。因此，在伴随动作方向反转的过渡区域中，也可确认到通过对旋转阻力f_i的变化进行线性近似，可以提高推测值的精度。

另外，有关旋转阻力f_i的变化向线性函数的近似，控制装置以反转相位为基准，根据旋转阻力f_i是从正变为负还是与其相反的变化，分情况切换线性函数，或者如果对图8所示状态进行移动平均处理，则可以获得与图10相同的波形，鉴于此可以代替机构要素的弹性系数，而把移动平均点数作为参数。尤其与前者相比，后者不需要分情况等特殊处理即可实现相当于图10的转变，所以能够保持精度并进一步降低处理量。

下面，举例说明接头的旋转阻力对并联机构的校准(calibration)的影响。根据以上所述可知，以第1万向接头2和/或第2万向接头5的旋转阻力f_i为起因，即使末端执行器6处于相同位置、相同姿态，例如图13所示，变形误差也有可能因动作方向而变化。因此，如果不考虑第1万向接头2和/或第2万向接头5的旋转阻力f_i即进行并联机构机床1的校准，则导致校准结果因校准时的动作方向而变化，在算出作为校准目标的机构参数的推测值时，精度出现偏差或者比较差。因此，按照图14所示，利用控制装置进行考虑第1万向接头2和/或第2万向接头5的旋转阻力f_i的校准，由此可以排除因该旋转阻力造成的干扰影响，提高校准精度。

即，控制装置首先进行与普通校准相同的校准用测定条件的设定(步骤S11)。例如，测定位置、姿态、动作方向(这里，形成为逆时针方向即CCW方向，但相反的情况也可以同以下那样考虑)。

然后，控制装置根据上述支柱轴长度位移Δl_i、Δl_Ji，求出第1万向接头2和/或第2万向接头的旋转阻力fi涉及的半径误差量Δr_fi(步骤S12)。即，控制装置按照校准的测定条件，求出各个测定点的支柱轴长度位移Δl_i、Δl_Ji，根据这些关系求出各个测定点的半径误差量Δr_fi。然后，控制装置实施基于上述测定条件的测定(步骤S13)。

并且，控制装置按照下面所述从测定结果中去除变形涉及的半径误差量Δr_fi，修正测定结果(步骤S14)。即，如果如下述[算式15]那样表示n个机构参数，则DBB轨迹的测定中的两个球的中心间距离r_i的关系一般如下述[算式16]所示，如果对其进行线性近似，则成为下述[算式17]。在该[算式17]中，由于未加算因第1万向接头2和/或第2万向接头5的旋转阻力引起的误差分量，所以如果设定将其加算后的等式，则如下述[算式18]所示，可以根据[算式18]表示更准确的半径误差Δr_i。因此，控制装置将在步骤S12中求出的半径误差量Δr_fi代入该[算式18]，由此可以得到去除了半径误差量Δr_fi后的测定结果。

【算式15】

P＝[p₁，…，p_n]^T

【算式16】

r_i＝g(P，X_i)

g：正运动学函数     X_i：第i个测定点的指令值

r_i：第i个测定点的测定值

【算式17】

${\mathrm{\Δr}}_i=\underset{j=1}{\overset{n}{\mathrm{\Σ}}}\frac{{\∂g}_i}{{\∂p}_j}{\mathrm{\Δp}}_j$

【算式18】

${\mathrm{\Δr}}_i=\underset{j=1}{\overset{n}{\mathrm{\Σ}}}\frac{{\∂g}_i}{{\∂p}_j}{\mathrm{\Δp}}_j+{\mathrm{\Δr}}_{\mathrm{fi}}$

Δr_fi：第i个测定点的基于旋转阻力的半径误差量

然后，控制装置继续关于机构参数Δp_j求解[算式18]，由此得到去除了基于旋转阻力f_i的半径误差量Δr_fi的影响的机构参数Δp_j，再据此进行不会受到旋转阻力f_i的影响的校准(步骤S15)。在DBB轨迹的测定点数为m点时，得到m个[算式18]的联立方程式，该联立方程式可以表述为下述[算式19]所示的矩阵。[算式19]可以按照下述[算式20]所示那样利用最小二乘法求解，由此控制装置可以得到去除了旋转阻力f_i的影响的机构参数误差ΔP。

【算式19】

ΔR＝A·ΔP

其中，

ΔP：机构参数误差

【算式20】

ΔP＝(A^TA)^-1A^T·ΔR

图15(a)表示没有去除基于旋转阻力f_i的半径误差量Δr_fi的影响而进行校准后的DBB轨迹的测定结果，图15(b)表示按照上面所述去除基于旋转阻力f_i的半径误差量Δr_fi的影响而进行校准后的DBB轨迹的测定结果。在图15(a)中，能够看到误差因测定方向不同从而产生偏差的情况，在图15(b)中，能够看得这种偏差较小，DBB轨迹的圆度提高。并且，在图15(a)中，在包含基于旋转阻力f_i的半径误差量Δr_fi的状态下进行校准，半径误差量Δr_fi也被同定为机构参数误差ΔP，因此总体上圆度误差较大，但在图15(b)中，圆度误差的绝对值也降低，可以确认到总体上圆度提高。

在此，将上述校准的至少一部分总结如下。即，一种并联机构机床的校准方法，所述并联机构机床具有：通过第2万向接头与支柱连接的末端执行器；驱动支柱的致动器；提供致动器指令值并控制各个致动器的控制单元；和旋转阻力值存储单元，其设于控制单元中，可以存储第1万向接头和/或第2万向接头的旋转阻力值，所述校准方法在将并联机构机床的末端执行器定位于多个位置和姿态时，进行位置和/或姿态的测定或者进行距固定点的距离测定，以该测定值为基础推测并联机构机床的机构参数并对其进行校准，所述校准方法的特征在于，在利用控制单元推测计算机构参数时，加算由第1万向接头和/或第2万向接头的旋转阻力引起的变形误差。

并且，提供一种并联机构机床的控制装置，所述并联机构机床具有：通过第2万向接头与支柱连接的末端执行器；驱动支柱的致动器；提供致动器指令值并控制各个致动器的控制单元；和旋转阻力值存储单元，其设于控制单元中，可以存储第1万向接头和/或第2万向接头的旋转阻力值，其特征在于，控制单元在将末端执行器定位于多个位置和姿态时，进行位置和/或姿态的测定或者进行距固定点的距离测定，以该测定值为基础推测并联机构机床的机构参数并对其进行校准，在推测计算该校准涉及的机构参数时，加算由第1万向接头和/或第2万向接头的旋转阻力引起的变形误差。

下面，说明适合用于与上述致动器指令值的补偿相关的控制的旋转阻力f_i的推测。

在此，虽然已在前面叙述了第1万向接头2和第2万向接头5，但要重新说明。图16是第1万向接头2的说明图，第1万向接头2是使用了双重环的万向接头结构。外侧的环41通过放射方向的轴42旋转自如地支撑在框架1上。并且，具有与轴42正交的轴43的内侧的环44将其轴支撑在外侧的环41的轴承45上，从而被安装成接头。这样，利用两个接头的组合，形成两自由度的第1万向接头2，第1万向接头2由相互正交的两个从动旋转轴60、61构成。

图17是第2万向接头5的说明图，第2万向接头5包括凸缘51和轴52。凸缘51被垂直地固定在与末端执行器6(在图17中没有示出，但在该图中配置在右后方)的下表面正交的侧面上，围绕从动旋转轴64旋转。轴52通过轴承53安装在凸缘51上，围绕与末端执行器6的侧面平行的从动旋转轴63旋转。另一方面，在轴52中的轴承53相反侧的端部，设有围绕与轴52的轴相同的从动旋转轴62旋转的轴承54，滚珠丝杠4(在图17中没有示出)装入该轴承54。这样，利用3个轴承(接头)的组合，形成三自由度的第2万向接头5，第2万向接头5由相互正交的两个从动旋转轴63、64、和围绕从动旋转轴63摆动的从动旋转轴62构成。

这样，并联机构机床的各脚轴系统具有5个从动旋转轴60～64，并且脚轴系统存在6组，因此并联机构机床总体上存在30个从动旋转轴。以下关于第1万向接头2和第2万向接头5的各接头，除特别提及时之外，不相互区分而都称为从动接头。

图18是表示并联机构机床的控制装置和作为控制对象的一部分的机床系统的指定要素的方框图。控制装置使用NC装置构成，包括轨道生成单元20、补偿参数存储单元21、控制量运算单元22、接头阻力值推测单元26、接头阻力值履历存储单元27和状态诊断单元28。另外，这些单元中的前四个包含于精度补偿部中，后两个包含于状态诊断部中。并且，在各个单元中使用NC装置的程序。

控制装置在轨道生成单元20中生成末端执行器6的运动轨迹。并且，控制装置在控制量运算单元22中通过求解并联机构的逆运动学计算，求出各个伺服电动机3的指令值。控制装置将这些指令值提供给属于机床系统的伺服控制器23，伺服控制器23根据指令值驱动对应的伺服电动机3。并且，伺服电动机3设有检测驱动时的负荷(驱动动力值)的负荷检测器25，这些负荷可以由控制装置的接头阻力值推测单元26掌握并存储。

另一方面，按照后面所述，通过接头阻力值推测单元26，以由轨道生成单元20生成的运动轨迹和由负荷检测器25测定的伺服电动机3的负荷测定值为基础，算出各从动接头的旋转阻力值。

这样得到的旋转阻力值由于用作补偿末端执行器6的动作误差时的新的参数，所以用于更新补偿参数存储单元21的内容。另外，关于把旋转阻力作为参数的误差推测方法或动作的补偿，可以适用上面说明的方法。

并且，旋转阻力值也被用于追加接头旋转阻力履历存储单元27的内容。控制装置在状态诊断单元28中参照接头旋转阻力履历存储单元27，掌握旋转阻力值及其变化，由此实施并联机构机床的状态诊断。

在控制装置的状态诊断单元28中预先存储有各从动接头的旋转阻力值的阈值、和旋转阻力值的变化量的阈值。作为这些阈值，设定表示正常状态(级别(level)1)与状态(级别2)的界限的阈值、以及表示该级别2与状态(级别3)的界限的阈值，状态(级别2)指虽然开始向从动接头施加大的负荷或者开始出现急剧变化，但通过利用控制装置补偿误差可以保持精度的状态，状态(级别3)指向从动接头施加过大负荷或者产生过大的负荷变化的状态。

图19和图20表示这种并联机构机床的控制装置执行的控制方法的流程图。首先，控制装置的接头阻力值推测单元26按照下面那样推测从动接头的旋转阻力值(图19的步骤S21)。

即，如上面所述，为了分析并联机构机床的各脚轴系统的从动轴的影响，把各脚轴系统假设为六自由度的串联连杆机构，按照前述[算式2]，可以使用各从动接头的旋转阻力值表示作用于第2万向接头5的力和力矩。

另一方面，作用于末端执行器6的内力的合力及合力矩(F)，可以使用作用于连杆前端的力或力矩按照前述[算式3]进行表述。这里，T_i表示从第2万向接头5到末端执行器6的变换矩阵，利用前述[算式4]定义。

并且，可以从[算式2]和[算式3]导出下面的[算式21]。另一方面，根据并联连杆机构的静力学关系，下面[算式22]的关系成立。因此，可以从[算式21]和[算式2]得到[算式23]的关系式。

【算式21】

$F=\underset{i}{\mathrm{\Σ}}{T}_i\·{\left({{\mathrm{Js}}_i}^T\right)}^{-1}\·f_i$

【算式22】

F＝Jp^T·τ

J_Pi：并联连杆机构的雅可比(Jacobi)矩阵

τ：各个伺服电动机3的负荷向量

【算式23】

${\mathrm{Jp}}^T\·\mathrm{\τ}=\underset{i}{\mathrm{\Σ}}{T}_i\·{\left({{\mathrm{Js}}_i}^T\right)}^{-1}\·f_i$

另外，若将[算式23]用矩阵表示，则得到下述[算式24]。并且，考虑到该[算式24]对各个测定点都成立，将这些关系式联立，可以表述为下述[算式25]。其中，n表示测定点数。把[算式25]右边的第1项设为J，使用J的伪逆矩阵J⁺，可以利用下述[算式26]以最小二乘近似的方式求出旋转阻力向量f。

【算式24】

$\left[\mathrm{\τ}\right]=[{\left({\mathrm{Jp}}^T\right)}^{-1}{T}_1{\left({{\mathrm{Js}}_1}^T\right)}^{-1}\·\·\·{\left({\mathrm{Jp}}^T\right)}^{-1}{T}_6{\left({{\mathrm{Js}}_6}^T\right)}^{-1}]\·\left[\begin{array}{c}{f}_1\\ \·\\ \·\\ \·\\ f_6\end{array}\right]$

【算式25】

【算式26】

f＝J⁺·τ(＝(J^T·J)^-1·J^T·τ)

实际上驱动并联机构机床测定的伺服电动机3的负荷中，除基于各从动接头的旋转阻力的分量外，还包含基于自重的分量、基于惯性力的分量等。因此，为了正确推测各从动接头的旋转阻力，需要从根据[算式26]得到的旋转阻力向量f仅抽出基于旋转阻力的分量。但是，通过在测定时使并联机构机床低速动作，可以使惯性力的影响非常微小，所以将基于惯性力的分量视为没有，不考虑去除。

另一方面，关于基于自重的分量，该分量只依赖于末端执行器6的位置和姿态，不依赖于动作方向。因此，无论使末端执行器6在CCW方向(绕逆时针的方向)移动，还是使其在CW方向(绕顺时针的方向)移动，如果在同一轨迹上的相同位置、姿态之间比较，则施加基于相同自重的负荷。因此，通过对这两个方向测定伺服电动机3的负荷，并对各个测定结果获取差分，可以消除基于自重的分量。即，假设对[算式25]考虑这种差分，则得到下述[算式27]，与[算式25]相同，对该[算式27]求解f，则得到[算式28]。根据该[算式28]，可以对各脚轴系统进行表示从动接头的旋转阻力的旋转阻力向量f的同定。在此，在将任意接头从同定对象中去除的情况下，从[算式27]右边的第2项中去除对应的分量，并且从[算式27]右边的第1项中去除对应的列向量，由此构成矩阵即可。这样，可以任意选择同定对象的接头。

【算式27】

【算式28】

f＝(J_CCW-J_CW)⁺·(τ_CCW-τ_CW)

控制装置的接头阻力值推测单元26通过以上处理，首先根据测定条件算出各测定点的矩阵J(图20的步骤S31)。然后，接头阻力值推测单元26测定CCW方向、CW方向的伺服电动机3(致动器)的负荷(步骤S32)。然后，接头阻力值推测单元26对这些测定结果获取差分(步骤S33)。并且，接头阻力值推测单元26根据预先存储为可以进行相当于[算式28]的计算的程序，使用该差分和矩阵J求解[算式28](步骤S34)。这样，在图19的步骤S21，接头阻力值推测单元26根据伺服电动机3的负荷推测从动接头的旋转阻力。

并且，控制装置在步骤S22，比较存储在状态诊断单元28中的与旋转阻力值相关的级别1、2的阈值(旋转阻力值的级别1的阈值)、和保存在接头旋转阻力履历存储单元27中的旋转阻力值。其结果是，如果旋转阻力值在该阈值以下(旋转阻力值≤旋转阻力值的级别1的阈值)，控制装置使处理转入步骤S26。另一方面，如果旋转阻力值超过该阈值(旋转阻力值＞旋转阻力值的级别1的阈值)，则控制装置判定为该旋转阻力值涉及的从动接头产生异常，使处理转入步骤S23。

控制装置在步骤S23，比较存储在状态诊断单元28中的与旋转阻力值相关的级别2、3的阈值(旋转阻力值的级别2的阈值)、和保存在接头旋转阻力履历存储单元27中的旋转阻力值。其结果是，如果旋转阻力值在该阈值以下(旋转阻力值≤旋转阻力值的级别2的阈值)，则控制装置使处理转入步骤S24，并显示旋转阻力值大于正常值的警告和产生这种旋转阻力值的从动接头或者其所属的脚轴系统的类别。另一方面，如果旋转阻力值超过该阈值(旋转阻力值＞旋转阻力值的级别2的阈值)，则控制装置使处理转入步骤S25，使并联机构机床的动作停止。另外，控制装置在该动作停止时进行该情况的显示。

另一方面，控制装置在步骤S26，比较存储在状态诊断单元28中的与旋转阻力值的变化(增加部分)相关的级别1、2的阈值(旋转阻力值增加部分的级别1的阈值)、和根据接头旋转阻力履历存储单元27中的旋转阻力值履历掌握的旋转阻力值增加部分。其结果是，如果旋转阻力值增加部分在该阈值以下(旋转阻力值增加部分≤旋转阻力值增加部分的级别1的阈值)，则控制装置使处理转入步骤S28，视为诊断结果无异常，继续正常动作。另一方面，如果旋转阻力值增加部分超过该阈值(旋转阻力值增加部分＞旋转阻力值增加部分的级别1的阈值)，则控制装置判定为该旋转阻力值增加部分涉及的从动接头产生异常，使处理转入步骤S27。

控制装置在步骤S27，比较存储在状态诊断单元28中的与旋转阻力值增加部分相关的级别2、3的阈值(旋转阻力值增加部分的级别2的阈值)、和参照接头旋转阻力履历存储单元27得到的旋转阻力值增加部分。其结果是，如果旋转阻力值增加部分在该阈值以下(旋转阻力值增加部分≤旋转阻力值增加部分的级别2的阈值)，则控制装置使处理转入步骤S29，并显示旋转阻力值增加部分大于正常值的警告、和产生这种旋转阻力值增加部分的从动接头或者其所属的脚轴系统的类别。另一方面，如果旋转阻力值增加部分超过该阈值(旋转阻力值增加部分＞旋转阻力值增加部分的级别2的阈值)，则控制装置使处理转入步骤S30，使并联机构机床的动作停止。另外，控制装置在该动作停止时进行该情况的显示。

在以上所述的控制装置或控制方法中，根据作为致动器的伺服电动机3涉及的负荷，利用[算式28]的关系求出从动接头的旋转阻力值，因此能够容易进行高精度的推测。

图21是表示对于一个脚轴系统(第1万向接头2a～第2万向接头5a)，比较各从动接头(图21中的11～15)的基于[算式28]的旋转阻力值的推测值(图21中的黑柱)、与旋转阻力值的实测值(图21中的白柱)的曲线图。由此，可以确认到基于[算式11]的精度良好的推测。

并且，图22(a)～(f)是表示对于各脚轴系统(第1万向接头2a～第2万向接头5a涉及的脚轴系统表示为(a)，其他依次相同)，比较各个方向的伺服电动机3的负荷的差分τ_i、CCW-τ_i、CW的波形(相对[算式11]的输入波形，图22中的浅色线)、和从根据[算式28]推测的各从动接头的旋转阻力值得到的负荷的推测值波形(图22中的实线)的曲线图。这里，各曲线图的纵轴表示对应的伺服电动机3的负荷，横轴表示DBB(Double Ball Bar)相位角(单位deg.、度)。由此得知输入波形与推测结果的一致程度较高。

并且，在以上叙述的控制装置或控制方法中，使用这样推测的旋转阻力值，判定该旋转阻力值是否超过阈值，在旋转阻力值超过阈值时，判定为该旋转阻力值涉及的从动接头异常，因此在判定异常时使并联机构机床的动作停止，可以防止在从动接头中由于产生过大的旋转阻力及变化而产生破损、变形和功能异常等，也可以在产生过大的旋转阻力或变化之前的异常判定时显示该情况，以提醒作业者注意。

并且，可以使用在以上叙述的控制装置或控制方法中推测的旋转阻力值，如上述那样推测并补偿机构变形误差。由于能够如上所述那样高精度地推测旋转阻力值，所以其补偿精度也良好，而且按照上面所述推测的旋转阻力值可以在计算量较小的状态下根据致动器负荷算出，因此容易执行该补偿。图23表示比较根据此时的旋转阻力值推测结果推测的DBB轨迹(CCW方向)、与DBB轨迹(CCW方向)的实测值的曲线图。从该曲线图中确认到变形误差补偿的良好精度。

另外，根据本发明控制的机床不限于空间六自由度的并联机构作业机床。其自由度不限于六自由度，可以是六自由度以上或以下的自由度。并且，万向接头不限于一旋转自由度接头的组合，也可以是球面接头等其他结构。另外，机床不限于具有刀具的作业机床，也可以是机器人、产业机械、娱乐设备、建筑机械等。

并且，在根据作用于末端执行器的合力求出致动器指令值的补偿量时，也可以不采用日本特开2005-186210记载的方法，而使用机构要素的柔量求出机构的弹性变形量，加算得到的弹性变形量进行逆机构变换，直接求出补偿后的致动器指令值。

Claims (16)

1.一种并联机构机床的控制方法，所述并联机构机床具有：固定在外部的基座；通过第1万向接头与基座连接的多个支柱；驱动各支柱的致动器；通过第2万向接头与各支柱连接的末端执行器；提供致动器指令值并控制各致动器的控制单元；和旋转阻力值存储单元，其设于控制单元中，可以存储第1万向接头和/或第2万向接头的旋转阻力值，其特征在于，

该控制方法包括：

第1步骤，根据并联机构机床的预定的机构参数，求出针对末端执行器的位置指令值和姿态指令值的各致动器指令值；

第2步骤，从旋转阻力值存储单元获取第1万向接头和/或第2万向接头的旋转阻力值；

第3步骤，使用旋转阻力值计算作用于各第2万向接头的力和力矩，该第3步骤将由支柱、第1万向接头、第2万向接头构成的连杆视为分别独立的串联连杆机构，计算通过各串联连杆机构的万向接头的旋转阻力作用于支柱与末端执行器的连接部的力和力矩；

第4步骤，根据作用于各第2万向接头的力和力矩，求出作用于末端执行器的合力和合力矩；

第5步骤，使用作用于末端执行器的合力和合力矩来计算机构的弹性变形量，使用该值计算致动器指令值的补偿量；

第6步骤，对于在第1步骤求出的致动器指令值，加算在第5步骤求出的补偿量以进行更新。

2.根据权利要求1所述的并联机构机床的控制方法，其特征在于，

在第3步骤中，利用线性函数使第1万向接头或第2万向接头反转时的旋转阻力值的变化近似。

3.根据权利要求1所述的并联机构机床的控制方法，其特征在于，

在第3步骤中，对在第1万向接头或第2万向接头反转时反转的旋转轴的旋转阻力值进行移动平均处理。

4.一种并联机构机床的控制装置，所述并联机构机床具有：固定在外部的基座；通过第1万向接头与基座连接的多个支柱；驱动各支柱的致动器；通过第2万向接头与各支柱连接的末端执行器；提供致动器指令值并控制各致动器的控制单元；和旋转阻力值存储单元，其设于控制单元中，可以存储第1万向接头和/或第2万向接头的旋转阻力值，其特征在于，

所述控制装置具有：

致动器指令值计算单元，其根据并联机构机床的预定的机构参数，求出针对末端执行器的位置指令值和姿态指令值的各致动器指令值；

合力运算单元，其使用从旋转阻力值存储单元获取的旋转阻力值计算作用于各第2万向接头的力和力矩，根据作用于各第2万向接头的力和力矩，求出作用于末端执行器的合力和合力矩，该合力运算单元将由支柱、第1万向接头、第2万向接头构成的连杆视为分别独立的串联连杆机构，计算通过各个串联连杆机构的万向接头的旋转阻力作用于支柱与末端执行器的连接部的力和力矩；

补偿量运算单元，其使用作用于末端执行器的合力和合力矩计算机构的弹性变形量，使用该值计算致动器指令值的补偿量；以及

所述控制单元，其对于通过致动器指令值计算单元求出的致动器指令值，加算通过补偿量计算单元求出的补偿量以进行更新。

5.根据权利要求4所述的并联机构机床的控制装置，其特征在于，

合力运算单元利用线性函数使第1万向接头或第2万向接头反转时的旋转阻力值的变化近似。

6.根据权利要求4所述的并联机构机床的控制装置，其特征在于，

合力运算单元对在第1万向接头或第2万向接头反转时反转的旋转轴的旋转阻力值进行移动平均处理。

7.一种并联机构机床的控制方法，所述并联机构机床具有：通过第2万向接头与支柱连接的末端执行器；驱动支柱的致动器；提供致动器指令值并控制各致动器的控制单元；和旋转阻力值存储单元，其设于控制单元中，可以存储第1万向接头和/或第2万向接头的旋转阻力值，

所述控制方法在将末端执行器定位于多个位置和姿态时，进行位置和/或姿态的测定或者距固定点的距离测定，以该测定值为基础，推测并校准并联机构机床的机构参数，其特征在于，

在利用控制单元推测计算机构参数时，加算由第1万向接头和/或第2万向接头的旋转阻力引起的变形误差。

8.一种并联机构机床的控制装置，所述并联机构机床具有：通过第2万向接头与支柱连接的末端执行器；驱动支柱的致动器；提供致动器指令值并控制各致动器的控制单元；和旋转阻力值存储单元，其设于控制单元中，可以存储第1万向接头和/或第2万向接头的旋转阻力值，其特征在于，

控制单元在将末端执行器定位于多个位置和姿态时，进行位置和/或姿态的测定或者距固定点的距离测定，

以该测定值为基础，推测并联机构机床的机构参数由此进行校准，

在推测计算该校准涉及的机构参数时，加算由第1万向接头和/或第2万向接头的旋转阻力引起的变形误差。

9.一种并联机构机床的控制方法，所述并联机构机床具有：固定在外部的基座；分别通过第1万向接头与基座连接的多个支柱；驱动各支柱的各个致动器；分别通过第2万向接头与各支柱连接的末端执行器；和检测各致动器的负荷的负荷检测器，其特征在于，

关于通过第1万向接头和/或第2万向接头的旋转阻力作用于末端执行器的内力，

将包括各第1万向接头、对应的支柱和对应的第2万向接头的各个脚轴系统，视为串联连杆机构，由此使用各脚轴系统涉及的雅可比矩阵以及第1万向接头和/或第2万向接头的旋转阻力向量表示的算式，与根据并联连杆机构的静力学关系使用并联连杆机构的雅可比矩阵以及各致动器的负荷向量表示的算式相等，

据此，根据由负荷检测器检测的负荷构成的负荷向量，求出第1万向接头和/或第2万向接头的旋转阻力向量，

由此推测作为该旋转阻力向量的构成要素的第1万向接头和/或第2万向接头的旋转阻力值。

10.一种并联机构机床的控制方法，其特征在于，

在权利要求1～3任一项所述的控制方法中，使用根据权利要求9的控制方法推测的旋转阻力值。

11.一种并联机构机床的控制方法，其特征在于，

在权利要求10所述的并联机构机床的控制方法中，控制单元对旋转阻力值存储单元中的推测的旋转阻力值的存储进行更新。

12.根据权利要求9～11中的任一项所述的并联机构机床的控制方法，其特征在于，

关于所推测的第1万向接头和/或第2万向接头的各旋转阻力值，在超过预定阈值时，判定为该旋转阻力值涉及的第1万向接头和/或第2万向接头产生异常。

13.一种并联机构机床的控制装置，所述控制装置对致动器进行控制，所述并联机构机床具有：固定在外部的基座；分别通过第1万向接头与基座连接的多个支柱；驱动各支柱的各个致动器；分别通过第2万向接头与各支柱连接的末端执行器；和检测各致动器的负荷的负荷检测器，其特征在于，

关于通过第1万向接头和/或第2万向接头的旋转阻力作用于末端执行器的内力，

将包括各第1万向接头、对应的支柱和对应的第2万向接头的各个脚轴系统，视为串联连杆机构，由此使用各个脚轴系统涉及的雅可比矩阵以及第1万向接头和/或第2万向接头的旋转阻力向量表示的算式，与根据并联连杆机构的静力学关系使用并联连杆机构的雅可比矩阵以及各致动器的负荷向量表示的算式相等，

据此，根据由负荷检测器检测的负荷构成的负荷向量，求出第1万向接头和/或第2万向接头的旋转阻力向量，

由此推测作为该旋转阻力向量的构成要素的第1万向接头和/或第2万向接头的旋转阻力值。

14.一种并联机构机床的控制装置，其特征在于，

在权利要求4～6任一项所述的控制装置中，使用根据权利要求13的控制装置推测的旋转阻力值。

15.一种并联机构机床的控制装置，其特征在于，

在权利要求14所述的并联机构机床的控制装置中，该控制装置对旋转阻力值存储单元中的推测的旋转阻力值的存储进行更新。

16.根据权利要求13～15中的任一项所述的并联机构机床的控制装置，其特征在于，

关于所推测的第1万向接头和/或第2万向接头的各旋转阻力值，在超过预定阈值时，判定该旋转阻力值涉及的第1万向接头和/或第2万向接头产生异常。

Images