CN107708937B - 校准装置以及使用该校准装置的机器人系统 - Google Patents

Abstract

得到能够高精度地实施校准的校准装置。实施从力信息中减去偏移值以及指尖负荷的重力作用量而仅提取由于与作业对象的接触所产生的外力的校准处理的校准部具有：近似曲线生成部，其基于根据姿态指令值使工具部分旋转时的位置信息以及力信息，生成近似曲线；偏移值推定部，其基于近似曲线、位置信息以及力信息，对力信息的偏移值进行推定；质量/重心位置推定部，其从力信息中将偏移值去除，使用去除了偏移值的力信息，计算出指尖负荷的质量以及重心位置矢量；以及外力分量运算部，其使用推定出的偏移值、指尖负荷的质量以及重心位置矢量，从力信息中减去偏移值以及指尖负荷的重力作用量。

Description

校准装置以及使用该校准装置的机器人系统

技术领域

本发明涉及一种在例如进行力控制的机器人中，不依赖于机器人指尖的姿态以及动作而仅提取由于外部作用所产生的外力的校准装置以及使用该校准装置的机器人系统。

背景技术

校准装置实施针对自动加工装置、自动组装装置或机器人之类的机械装置的校准操作，实施针对作用于所配备的作业对象的指尖效果器等具有质量的作用装置所承受的指尖负荷的重力补偿。

例如，对于在指尖负荷和机械臂之间具有负载传感器或者力觉传感器的机器人，以重力补偿以及惯性力的补偿为目的，在作业前对指尖负荷的重量以及重心位置进行测量或推定，基于该值而对与指尖负荷的姿态或加减速动作相应的重力或惯性力进行运算，从力传感器的值中减去，由此提取通过外界作用而产生的外力。

这里，提出有一种校准装置，即，在未作用有指尖负荷以外的外力的条件下，对机器人的指尖凸缘或机器人指尖的位置姿态进行测量，并且在该位置姿态下，通过力觉传感器对由3轴力以及3矢量构成的6自由度的力信息进行测量，根据由于指尖负荷而从某个姿态变化为其他姿态时发挥作用的力传感器的输出之差，利用最小二乘法而推定偏差电压(例如，参照专利文献1)。

另外，提出有一种校准装置，即，考虑由于安装位置的问题引起的重力方向的误差，实施根据在多个姿态下得到的力信息所预先设定的计算，由此，通过最小二乘法求出重力方向矢量、指尖负荷重量、指尖负荷的重心位置矢量、力觉传感器的偏移值等在提取外力时应当减去的值(例如，参照专利文献2)。

专利文献1：日本特开平7-205075号公报

专利文献2：日本特开2012-40634号公报

发明内容

如专利文献1、2所示，对于校准装置，将在安装传感器时产生的偏差输出以及偏移输出高精度地去除，需要以高精度地对指尖负荷的重量以及重心位置进行推定为基础。

这里，在专利文献1、2所示的校准装置中，假定在所获取的数据未作用多余的外力。然而，在实际所使用的机器人中，在安装于力觉传感器目标的指尖负荷的周边，作为外力作用大多包含安装于工具部分的通信线缆以及信号线之类的配线，无法进行准确的校准。

特别是，在利用使用最小二乘法等根据在任意的多个姿态下获取的数据而计算出各参数的方法的情况下，包含由线缆等所引起的外力产生的分量的数据和除此以外的数据混在一起，因此，特别是在指尖负荷小的情况下，无法准确地对偏移值分量进行推定。其结果，存在指尖负荷质量以及重心位置矢量的推定不准确而无法高精度地实施校准的问题。

本发明就是为了解决如上述的课题而提出的，其目的在于得到一种校准装置，该校准装置考虑由外力产生的影响而将偏移分量去除，高精度地计算出指尖负荷质量以及重心位置矢量，由此能够高精度地实施校准。

本发明所涉及的校准装置在进行安装于前端且针对作业对象起作用的工具部分的力控制的机械装置中，仅提取由于与作业对象的接触而在工具部分产生的外力，该校准装置具有：位置信息获取部，其获取工具部分的位置信息；力信息获取部，其获取作用于工具部分的力信息；旋转轴指定部，其指定从传感器坐标系的原点通过的任意的旋转轴；姿态生成部，其生成使工具部分绕述旋转轴旋转的姿态指令值；以及校准部，其实施校准处理，该校准处理是从力信息中减去偏移值以及指尖负荷的重力作用量，仅提取由于与作业对象的接触而产生的外力，校准部具有：近似曲线生成部，其基于根据姿态指令值而使工具部分旋转时的位置信息以及力信息，生成近似曲线；偏移值推定部，其基于近似曲线、位置信息以及力信息，对力信息的偏移值进行推定；质量/重心位置推定部，其从力信息中将偏移值去除，使用去除了偏移值的力信息，计算出指尖负荷的质量以及重心位置矢量；以及外力分量运算部，其使用推定出的偏移值、指尖负荷的质量以及重心位置矢量，从力信息中减去偏移值以及指尖负荷的重力作用量。

根据本发明所涉及的校准装置，在进行安装于前端且针对作业对象起作用的工具部分的力控制的机械装置中，仅提取由于与作业对象的接触而在工具部分产生的外力，该校准装置具有：位置信息获取部，其获取工具部分的位置信息；力信息获取部，其获取作用于工具部分的力信息；旋转轴指定部，其指定从传感器坐标系的原点通过的任意的旋转轴；姿态生成部，其生成使工具部分绕述旋转轴旋转的姿态指令值；以及校准部，其实施校准处理，该校准处理是从力信息中减去偏移值以及指尖负荷的重力作用量，仅提取由于与作业对象的接触而产生的外力，校准部具有：近似曲线生成部，其基于根据姿态指令值而使工具部分旋转时的位置信息以及力信息，生成近似曲线；偏移值推定部，其基于近似曲线、位置信息以及力信息，对力信息的偏移值进行推定；质量/重心位置推定部，其从力信息中将偏移值去除，使用去除了偏移值的力信息，计算出指尖负荷的质量以及重心位置矢量；以及外力分量运算部，其使用推定出的偏移值、指尖负荷的质量以及重心位置矢量，从力信息中减去偏移值以及指尖负荷的重力作用量。

因此，考虑从因外力产生的影响而将偏移分量去除，高精度地计算出指尖负荷质量以及重心位置矢量，由此能够高精度地实施校准。

附图说明

图1是表示应用了本发明的实施方式1所涉及的校准装置的机器人系统的结构图。

图2是表示本发明的实施方式1所涉及的校准装置的结构框图。

图3是例示出本发明的实施方式1所涉及的机器人系统的机器人坐标系、世界坐标系、重力坐标系、传感器坐标系的位置关系的说明图。

图4是例示出本发明的实施方式1所涉及的机器人系统的机械凸缘坐标系和传感器坐标系的关系的说明图。

图5是表示本发明的实施方式1所涉及的校准装置的校准部以及使用该校准装置的机器人系统的结构框图。

图6是详细地示出本发明的实施方式1所涉及的校准装置的校准部内的参数推定部的结构框图。

图7的(a)、(b)是例示出在本发明的实施方式1所涉及的校准装置中，基于由指定轴上的姿态生成部生成的姿态指令值的动作的说明图。

图8是表示在本发明的实施方式1所涉及的校准装置中，由参数推定部的偏移值推定部进行的轴力的偏移推定动作的说明图。

图9是表示在本发明的实施方式1所涉及的校准装置中，由参数推定部的偏移值推定部进行的力矩的偏移推定动作的说明图。

图10是表示在本发明的实施方式2所涉及的校准装置中，参数推定部的偏移值推定部判定偏移推定结果是否处于预先设定的误差水平的处理的流程图。

图11的(a)、(b)是例示出在本发明的实施方式2所涉及的校准装置中，参数推定部的偏移值推定部判断偏移推定结果是否处于预先设定的误差水平的情况的说明图。

图12是例示出在本发明的实施方式2所涉及的校准装置中，在输入至校准部之前由误差信息显示部显示给用户而选择错误数据的情况的说明图。

图13是示出在本发明的实施方式3所涉及的校准装置中，除了考虑与工具部相关的质量的影响之外，由处理校准部保存作业开始时的接触状态的偏差保存的具体例的说明图。

图14是表示本发明的实施方式3所涉及的校准装置的校准部的结构框图。

具体实施方式

下面，利用附图对本发明所涉及的校准装置以及使用该校准装置的机器人系统的优选实施方式进行说明，在各图中，对相同或相当的部分标注相同的标号而进行说明。

此外，在以下的各实施方式中，作为校准装置，具体以在利用了机器人的系统中进行校准处理的机器人系统为例进行说明。然而，本发明所涉及的校准装置即使在不运用机器人的情况下，也能够针对进行力控制的机械装置而以相同的结构实施校准，因此，不将应用范围限定于机器人系统。

具体而言，作为校准装置，能够举出针对自动加工装置、自动组装装置、机器人之类的实施自动加工或自动组装的机械装置，即进行力控制的机械装置而实施校准。

另外，本发明所涉及的校准装置根据绕从传感器坐标系的原点通过的任意轴而产生姿态变化时的机器人的指尖负荷位置信息、和作为力传感器的输出信息的力信息而生成近似曲线。另外，从近似曲线、位置信息以及力信息中，将由于校准处理所不需要的线缆、弹簧等引起的张力以及斥力而产生的外力分量去除，对偏移值进行推定，在从力觉传感器数据中将偏移值去除之后，对指尖负荷质量以及重心位置矢量进行推定，基于上述信息而进行指尖负荷的重力补偿，对作用于指尖负荷的外力分量进行运算。

由此，在根据将具有不要的外力分量的数据去除后的提取数据对偏移值进行推定之后，对指尖负荷质量以及重心位置矢量进行推定，由此，能够从获取数据之中将包含于在指尖负荷质量以及重心位置矢量的推定处理中使用的多个输入信息的外力的影响去除，能够高精度地进行重力补偿以及外力分量推定。

实施方式1.

图1是表示应用了本发明的实施方式1所涉及的校准装置的机器人系统的结构图。这里，首先，对校准装置的处理内容进行说明。在图1中，机器人系统作为基本的结构而具有机械臂1、机器人的控制器2以及安装于机械臂1的工具部4。

另外，在进行力控制的机器人系统中，在机械臂1与工具部4之间，作为获取力信息的力信息传感器而设置有力传感器3。通常，机器人在任意的坐标系上通过位置控制对机械臂1的前端位置或在机械臂1的前端安装的工具部4的前端位置进行控制，移动至由控制器2指定的希望的位置。

此外，如果导入使用了力传感器3的力控制，则不仅能够实现位置控制，还能够实现阻抗控制以及力控制。另外，阻抗控制以及力控制在后面叙述，是被动地或主动地对接触状态即工具部4与周边环境或周边物体之间的力的作用状态进行控制的控制方式。

通过运用该控制方式，从而使得能够实施通常使用机器人的研磨作业、去毛刺作业、连接器等的组装作业、具有锥形的轴和孔的嵌合作业等需要考虑接触状态的作业。

校准装置是在这种机器人系统中，在使用力信息而对机器人等的机械装置进行控制的情况下所需的装置。另外，在每次机器人进行作业时，在机械臂1的前端安装用在作业中的把手、刀具以及传感器，实施作业，将它们称作工具部分。此时，为了实施运用了力控制的作业，需要准确地知道指尖侧的工具部分和作业对象处于哪种接触状态。这里，接触状态能够由力以及力矩的大小以及方向矢量来表现。

机器人等机械装置在利用力信息而进行作业的情况下，能够实施例如去毛刺以及研磨之类的加工作业以及具有接触状态的组装作业之类的作业，但此时，为了使得机器人等机械装置准确地进行作业，优选一边将接触状态保存为目的状态一边进行作业。这里，目的状态是指为了进行加工以及作业而预先设计的接触状态，由作业者相应于作业目的而与作业速度以及所需的精度相匹配地进行调整。

这里，从力觉传感器或负载传感器之类的传感器作为电信号而获取力信息。另外，用于获取力信息的传感器安装于机器人的指尖侧的凸缘位置。此外，传感器利用构造体的应变而输出电信号，因此，由于在将传感器构造体安装于机器人时所产生的构造体的应变、由于碰撞等而产生的构造体的应变以及传感器本身的电路的零点设定，有时产生偏移。

这里，零点设定是指为了将所产生的偏移分量抵消，针对输出值施加偏差量而进行调整的设定，通常在机器人动作前进行。另外，偏移是指输出至传感器的稳定的偏差量，如果仅观测到传感器输出，则表现为不依赖于姿态而作用有恒定量的大小的外力。即，即使在针对工具没有产生因作业对象引起的作用力的无负荷状态的情况下，也测量为产生了恒定的作用力。因此，偏移分量必须被去除。

如上所述，在使用了力信息的机器人等机械装置的控制中，进行阻抗控制以及力控制。此外，阻抗控制是如下控制方式，即，在赋予位置指令值而进行控制的情况下，针对外力作用时的工具部前端的动作而设定临时的刚性以及粘性，对作业对象和工具前端的位置关系进行控制。另一方面，力控制是如下控制方式，即，赋予力目标值，以追随该力目标值的方式进行控制。

此时，通过以各个控制方式，将力或位置关系作为目标状态而适当地进行指令值设定或阻抗/参数设定，从而能够应用于组装输送或去毛刺研磨之类的作业。这里，指令值是指力控制的力目标值、阻抗控制的位置控制的位置姿态目标值或位置指令执行中的指尖速度指令值。另外，阻抗/参数表现为，例如通常的阻抗控制中的刚度矩阵、阻尼矩阵以及惯性矩阵中的各要素值。

此外，在实施上述控制的情况下，如果在力信息中产生偏移，则与从外部作用的实际的作用力不同的矢量值的力信息被输入至控制系统。因此，与机器人工具部相关的阻抗控制或力控制基于与实际的作用力不同的力而进行，因此，变得不能达到用户已设计的目的位置以及状态，或是变得机器人工具部无法追随由控制器指定的目标作用力。因此，通常大多进行减弱偏移的影响的操作。

这里，如上述专利文献1的例如第0031段记载所述，在将在特定的姿态下获取的力信息作为偏移量而与输出值相抵消的方法中，在垂直多关节型的机器人等通过姿态变更而能够变更重力方向以及工具前端的朝向的情况下，与重力影响相应的补偿量发生变化，因此所运算的外力的大小变得不准确。

另外，为了将因重力以及指尖加速度产生的惯性力的影响去除而求出准确的外力，针对从工具部分即传感器观察到的指尖负荷而中准确地对质量以及重心位置进行同定，同时对偏移量进行推定，由此，通过从力信息预先减去偏移量以及指尖负荷的重力作用量，从而能够准确地计算实际的外力。

另外，在上述专利文献1、2的方法中，在任意N(N＝1，2，…)个姿态下，分别获取力信息而进行偏移量推定以及作为其他参数的工具质量、工具重心位置、重力方向等的推定，但无论任何方法，均是以能够在理想地从获取数据将外力分量去除的状态下获取位置信息以及力信息为前提。

另外，作为作用力的误差的外力分量，假设为通过由低通滤波器或移动平均滤波器所示的滤波处理能够减弱影响的白噪声，不考虑依赖于特定的方向而产生的误差数据。

然而，实际上在机器人系统以及机械系统中，如图1所示，作为根据目的而安装于工具前端的线缆，设置有力传感器线缆5、触觉传感器以及视觉传感器的传感器线缆6、气缆7之类的线缆类，并且，设置有未图示的电源线之类的配线，因此，在线缆类以及配线的作用下会产生外力。另外，通过上述线缆以及配线的安装方法，有时力信息中会包含大于或等于传感器噪声的外力分量。

其结果，如果基于上述信息而应用上述专利文献1、2的方法，则存在如下课题，即，难以判断各个数据受到何种程度的影响，例如应用了最小二乘法等得到的解会受到误差的影响。此外，该影响在进行高精度的控制时无法忽视。

因此，本发明为了解决上述课题，作为获取位置信息以及力信息的姿态，使用实施了绕传感器坐标系的任意轴旋转的姿态变更的数据，由此将因线缆以及配线产生的外力作用量显著的数据去除，对于具有一定倾向的数据，通过进行校正而高精度地对偏移进行推定，将偏移推定处理后的力信息用作输入，由此，能够高精度地对工具部分的质量、即指尖负荷的质量以及重心位置进行推定。

下面，参照图2，对本发明的实施方式1所涉及的校准装置的校准步骤详细进行说明。图2是表示本发明的实施方式1所涉及的校准装置的结构框图。图2所示的校准装置从力传感器的信息中，仅提取作用于与作业相关的机器人的指尖部分以及工具前端的外力作用量，针对力传感器的信息而实施校准处理。

在将机器人设为某个姿态R_k(k＝1，2，…，M)时，将姿态R_k时的力传感器信息设为F(k)。这里，姿态由旋转矩阵R_k表现。另外，旋转矩阵是由3×3矩阵表现出在从某个基准坐标系观察时，当前所关注的坐标系正在进行哪样的姿态变化的旋转矩阵。

力传感器信息F(k)包含偏移F_bis(k)、与指尖负荷相应的作用力F_mas(k)、外部作用力F_ext(k)以及作为电信号的噪声分量F_nos(k)，由下式(1)表达。

F(k)＝F_bis(k)+F_mas(k)

+F_ext(k)+F_nos(k)...(1)

另外，通过从力传感器信息F(k)中去除F_bis(k)、F_mas(k)、F_nos(k)，从而求出希望求出的外力分量F_ext(k)。将式(1)变形而由下式(2)表达外力分量F_ext(k)。

F_ext(k)＝F(k)-F_bis(k)

-F_mas(k)-F_nos(k)...(2)

这里，作为对外力分量F_ext(k)进行运算的方法，通过低通等滤波处理将噪声分量F_nos(k)去除，对偏移值以及基于工具部的负荷推定的指尖负荷影响进行运算，将偏移值及指尖负荷影响减去，由此求出外力分量F_ext(k)。此外，偏移分量设为在工具部被固定时发挥作用的恒定力、具体为轴力以及力矩，不依赖于工具的姿态。

即，F_bis(k)成为不依赖于姿态k而针对全部姿态k共用的值：F_bis(k)＝[F_bis_x，F_bis_y，F_bis_z，M_bis_x，M_bis_y，M_bis_z]。另外，对于F_mas(k)，能够通过求出质量、传感器坐标系、重心位置的关联性而进行运算。

通常，在不施加外力的状态下取多个姿态R_k，基于此时的位置信息和力信息而进行校准处理。在本发明中，对于在校准时所处理的数据包含F_ext(k)不为0的力信息而进行处理的情况下，针对获取姿态而赋予约束条件，从而实施高精度的校准处理，基于此对因外力分量产生的作用力F_ext(k)进行计算。下面对该方法进行说明。

图3是例示出本发明的实施方式1所涉及的机器人系统的机器人坐标系、世界坐标系、重力坐标系、传感器坐标系的位置关系的说明图。如图3所示，将定义为固定于系统内的机器人主体的基准坐标系的机器人坐标系设为Σrob，将定义为在同一系统内的仪器所共用的坐标系的世界坐标系设为Σwld，将以重力加速度方向为-Z方向的重力坐标系设为Σgrv。

通常，由于大多使用水平仪以使得与安装机器人的台的上表面的法线方向即机器人坐标系Σrob的Z方向、重力坐标系Σgrv的Z轴方向即重力方向正交的方式高精度地进行调平，因此，在自动化系统中，对于机器人坐标系Σrob以及重力坐标系Σgrv，Z轴大致一致，误差的影响较少。

另外，为了简单，将作为重力坐标系的定义的Σgrv的X轴方向以及Y轴方向设为与机器人坐标系Σrob一致。另外，在基于设计图而构建机器人系统的情况下，在附图所记载的精度下位置姿态关系可以是已知的，因此，世界坐标系Σwld以及机器人坐标系Σrob的作为相对关系的齐次变换矩阵^wldT_rob也可以是已知的。即，Σwld、Σrob、Σgrv的相对关系的初始值设为大致推定的值而进行说明。

这里，将在机械臂的末端能够安装传感器以及工具的部分称作机器人机械凸缘。机器人机械凸缘的坐标系能够通过从机器人坐标系观察到的指尖位置指令值进行运算。

下面，说明在将传感器安装于机器人机械凸缘的情况下对传感器坐标系Σsen进行定义的方法。在本发明中，假设所输出的信息为3轴或6轴的情况，但传感器坐标系Σsen相对于框体而事先定义有X轴、Y轴、Z轴方向，因此按照该方式进行。

图4是例示本发明的实施方式1所涉及的机器人系统的机械凸缘坐标系和传感器坐标系的关系的说明图。如图4所示，如果将相对于机器人坐标系的机械凸缘坐标系设为Σmec，则对这里所定义的机械凸缘的坐标系和传感器坐标系Σsen进行比较，从而能够求出齐次变换矩阵。此外，严格来讲，包含有误差，但能够将初始值作为已知的值而处理。

这里，齐次变换矩阵是由旋转矩阵R(3×3)以及表示在参照坐标系中所定义的位置关系的位置矢量P构成的4×4的矩阵。例如，如果将作为基准的坐标系取作世界坐标系Σwld，将所关注的坐标系设为机器人坐标系Σrob而表现齐次变换矩阵^wldT_rob，则旋转矩阵R、位置矢量P以及齐次变换矩阵^wldT_rob由下式(3)～(5)表示。

【算式1】

【算式2】

【算式3】

在上述位置关系成立的情况下，能够进一步准确地对偏移进行推定，如果已知工具部分的质量以及重心位置，则能够对外力分量F_ext(k)进行运算。无法准确地对它们进行运算的原因在于存在如下情况：各坐标系的位置关系不准确且在重力方向以及重力加速度存在偏差、偏移分量不准确、或者工具部分的质量以及重心位置不准确。

与此相对，本发明的特征在于，无需通过最佳计算而计算出将误差最小化的参数，而是通过进行固有的动作而实施作为误差要因的数据的去除以及个别的推定处理，由此提高偏移、物体质量以及重心位置的推定精度。

因此，首先，获取机器人的校准处理所需的位置信息以及力信息。作为位置信息而获取工具部分的位置信息，位置信息是指相对于在直交坐标系中表现出的X轴、Y轴、Z轴的平移3自由度即[X，Y，Z]以及相对于欧拉表示的X轴、Y轴、Z轴的旋转3自由度即[A，B，C]。此时，为了获取校准用的位置信息以及力信息的目的，通过图2的旋转轴指定部100而指定从传感器坐标系的原点通过的任意的旋转轴Vec_rot。

这里，在以传感器坐标系的定义作为基准的校准中，在绕由旋转轴指定部100指定的旋转轴Vec_rot旋转后的姿态下获取位置信息以及力信息。因此，图2的指定轴上的姿态生成部101基于旋转轴Vec_rot而决定用于获取位置信息以及力信息的姿态，并作为姿态指令值而输出。

此外，由于如果相对于旋转轴Vec_rot从作为基准的位置姿态起增大旋转量则误差变小，因此，各姿态能够决定为，例如相对于旋转轴Vec_rot的正旋转方向以45度间隔而获取。另一方面，如后所述，在本发明中，即使相对于旋转轴Vec_rot的旋转量小也能够进行推定处理，因此，在由于与周边物体的干涉等理由而针对姿态变化的制约较大的情况下，也能够进行信息获取。

另外，关于旋转轴Vec_rot，可以指定传感器坐标系的主轴即传感器坐标系的X轴、Y轴、Z轴。此外，在后述的近似曲线生成部中，需要定义在与所指定的旋转轴正交的面上相正交的主轴，因此如果事先指定传感器坐标系的主轴，则变得不需要变化处理。另一方面，能够将主轴以外的轴指定为旋转轴，因此，可以由用户考虑与周边环境的干涉而决定，还可以由制造商事先决定旋转轴Vec_rot以及旋转角度量θ。

针对以该方式指定的旋转轴Vec_rot，基于姿态指令值而进行姿态变更，获取在校准中使用的数据。这里，作为安装于机器人102的力信息获取部103，例如使用负载传感器或力传感器，根据当前的机器人状态获取力信息。

另外，位置信息获取部104使用例如来自在机器人的各轴安装的编码器的信息而对机器人的指尖位置进行运算，并且，获取传感器坐标系的用于获取位置姿态的位置信息。此外，位置信息获取部104通过在机器人工具部分粘贴标记，由视觉传感器对标记进行测量，从而还能够从机器人外部对机器人姿态进行测量或推定。

这里，在使用外部传感器的情况下，可以不考虑机器人固有的机构误差即齿隙以及挠曲等误差的影响。此外，在安装外部传感器的情况下，对于传感器安装位置从机器人坐标系Σrob或世界坐标系Σwld即基准坐标系观察安装于哪个位置，需要使用夹具准确地进行定位、或者由外部传感器参照基准标记等而推定安装位置。

需要以上述方式获取用于旋转机器人工具前端的校准的位置信息以及力信息，需要最少针对旋转轴Vec_rot1获取大于或等于3个点。这是因为，该校准装置应用了利用圆的近似。此外，基本上优选以大于或等于4个点考虑误差的影响。

这样，一边绕旋转轴Vec_rot使姿态变化，一边获取位置信息以及力信息，存储至数据存储部105。接着，利用所存储的数据在校准部106中进行校准处理。

图5是表示本发明的实施方式1所涉及的校准装置的校准部以及使用该校准装置的机器人系统的结构框图。在每次使用校准装置时，首先，在求出参数的参数推定部201中，进行参数推定处理。然后，在实际使用机器人而实施作业时(通常动作时)，使用所推定的参数，进行从力信息中减去偏移值以及重力影响量的处理。下面，对其进行说明。

在图5中，在该校准装置中，在参数推定部201中，求出参数即包含工具部分的指尖负荷的质量、包含工具部分的指尖负荷的重心位置以及偏移。关于与以上的参数推定部相关的说明，利用图6在后面叙述。

然后，通常动作时的机器人102根据由指令值生成部203生成的姿态指令值而动作，根据当前的机器人状态而由力信息获取部103获取力信息。此时，外力分量运算部207基于存储于参数存储部202的参数，对由位置信息获取部104得到的、机器人的当前位置姿态R_k中的因指尖负荷质量产生的力的作用量F_mas(k)进行运算。

另外，如上述式(2)所示，外力分量运算部207从传感器数据F(k)中减去因指尖负荷质量产生的力的作用量F_mas(k)以及因偏移产生的作用量F_bis，由此求出因接触产生的外力分量F_ext(k)。另外，外力分量运算部207将该外力分量作为作用外力运算值而反馈至指令值生成部203，由此进行校准处理。

如上所述，校准部106由下述部件构成：参数推定部201，其针对机器人102，基于存储于数据存储部105的位置信息以及力信息而进行求出参数的参数推定处理；参数存储部202，其对参数推定部201中的推定结果进行存储；以及外力分量运算部207，其使用所存储的参数，将因偏移产生的作用量以及因指尖负荷质量产生的力的作用量去除，求出因接触产生的外力分量。本发明将校准部106应用于由指令值生成部203、机器人102、力信息获取部103以及位置信息获取部104构成的机器人系统。

下面，进一步对作为本发明的特征的参数推定部201进行详细说明。图6是详细地示出本发明的实施方式1所涉及的校准装置的校准部内的参数推定部的结构框图。

如图6所示，在绕由旋转轴指定部100指定的旋转轴Vec_rot旋转的姿态下获取多个数据，将所获取的数据储存至数据存储部105，由参数推定部201首先求出偏移值以及临时的质量。另外，参数推定部201进一步基于所求出的偏移值以及临时的质量，实施质量以及重心位置的推定。推定出的质量以及重心位置信息输出至参数存储部202。

另外，由参数推定部201所记载的偏移值推定部21以及近似曲线生成部20进行的偏移值的具体的求解方法在下面说明。首先，作为旋转轴Vec_rot，考虑在与重力方向垂直的面内，使传感器坐标系的任意轴之1绕传感器坐标系的主轴而旋转的情况。这里，将传感器-Z轴方向相对于重力方向一致的方向设为基准姿态R_k0。另外，作为旋转轴Vec_rot，选择传感器坐标系主轴的Y轴。

接着，参照图7～9，对轴力的偏移值F_bis_x、F_bis_y、F_bis_z的计算方法进行说明。图7(a)、(b)是例示在本发明的实施方式1所涉及的校准装置中，基于由指定轴上的姿态生成部生成的姿态指令值的动作的说明图。如图7所示，获取大于或等于3点从基准姿态R_k0起绕Y轴旋转后的姿态。将这里获取的轴力数据Fx、Fz记述为在纵轴为Fz、在横轴为Fx。

此外，在垂直于重力方向的面内选择旋转轴的情况下，在关注进行了旋转运动时的作用力的矢量变化时，在该状态下针对与旋转轴正交的轴、这里为X轴以及Z轴的力信息的绘制如图8所示，显现为描绘圆轨道的特征。利用该特征，在相对于所获取的力信息而对在各位置姿态下获取的数据进行绘制时，生成呈圆形的近似曲线。

在近似曲线生成部20中，将使重力方向与传感器坐标系-Z轴匹配、且绕Y轴旋转的情况下得到的X轴方向的轴力设为Fx，将Z轴方向的轴力设为Fz。然后，将横轴设为Fx、纵轴设为Fz而求出针对各点(Fx，Fz)的近似圆。对于从圆的中心位置的原点起的偏差量，如果在对X轴力进行绘制的横轴方向上设为Fx_b，在对Z轴力进行绘制的纵轴方向上设为Fz_b，则作为以Fx_b、Fz_b、圆的半径R为变量的圆的公式，能够定义为下式(6)。

(Fx-Fx_b)^2+(Fz-Fz_b)^2＝R^2...(6)

这里，为了求出上述的多项式的最小二乘近似解，能够将以Fx_b、Fz_b、R为变量的函数f(Fx_b，Fz_b，R)定义为下式(7)，能够应用最小二乘近似，该最小二乘近似用于求出使得进行平方并偏微分后的结果成为0的解。

f(Fx_b，Fz_b，R)

＝(Fx-Fx_b)^2+(Fz-Fz_b)^2-R^2...(7)

以上述方式得到的Fx_b、Fz_b是X轴力的偏移值F_bis_x、F_bis_z。此外，这里得到的R是与质量相应的外力Mg’。将从该外力Mg’得到的质量设为临时的质量M_tmp。另外，将旋转主轴变更为X轴而实施同样的处理，由此还得到F_bis_y。这样，能够对与轴力相关的偏移值进行计算。

另外，在近似曲线生成部20中，以下述方式求出与力矩相关的偏移值。首先，关于力矩，具有下述特征，即，在实施了旋转运动的情况下，如果将旋转轴Vec_rot选为Y轴，针对绕旋转轴Vec_rot的姿态变化Rk如图9所示，能够近似为对余弦曲线施加了相位差和偏差得到的曲线。

即，作为数值模型，在将绕Y轴的力矩的偏移分量设为M_y_b，将从基准姿态Rk0起的绕Y轴的旋转角度设为θ时，在横轴上定义θ，在纵轴上定义取为力矩时的相位差φ。另外，将余弦曲线的振幅设为Am。此时，如果将获取的力矩数据设为M_y，则下式(8)成立。

M_y＝M_y_b+Am*COS(θ+φ)...(8)

此外，针对旋转角度θ的周期性的特征在于，成为正好以360度作为1周期的频率。针对该模型，将设为f(M_y_b，Am，φ)的函数定义为如下式(9)，以牛顿拉普森法反复地实施运算，由此能够得到与M_y_b、Am、φ相关的近似解。

f(M_y_b，Am，φ)

＝M_y-M_y_b-Am*COS(θ+φ)...(9)

这样得到的M_y_b成为所求出的绕Y轴的力矩的偏移值M_bis_y。此外，在解的收敛性差而得不到近似解的情况下，也可以选择以360的因数除360度得到的角度，例如如果为2，则作为θ而选择0度和180度，例如如果为4，则作为θ而选择0度、90度、180度和270度，取各个M_y的平均值。

另外，同样地，作为旋转轴Vec_rot而选定X轴，由此能够得到M_bis_x。另外，对于M_bis_z，与针对Y轴、X轴实施的处理相同地，能够实施与在垂直于重力方向的面上将使Z轴一致的姿态R_k1作为基准位置、作为旋转轴Vec_rot而选定Z轴并求出M_bis_y时相同的处理。

此外，作为其他方法，也可以单纯地根据R_k0的姿态而将旋转轴Vec_rot选定为Z轴，对于以360的因数除360度得到的角度，例如如果为2，则作为θ而选定0度和180度，例如如果为4，则作为θ而选择0度、90度、180度和270度，取各个M_z的平均值。

通过以上示出的方法，在近似曲线生成部20中，根据近似得到的曲线的式子而进行临时偏移值的推定，求出偏移值F_bis以及临时的质量M_tmp。

此外，此时，在混入了线缆发生了钩挂的数据的情况下，存在图8的错误数据13这样的点。本发明的特征在于，在校准处理中将该点从作为近似曲线生成部20的结果的近似曲线、力信息和位置信息中去除。

具体而言，在如图8所示的错误数据为几个点的情况下，在全部在N个的姿态R_k下获取了数据的情况下，分为每个为N-M数据的组，分别对最大误差进行比较，由此能够提取错误数据。其中，M以及N为满足M＜N的正的整数。

另外，对于工具部分的指尖负荷质量以及重心位置的计算，由图6的质量/重心位置推定部22实施。这里，进行力控制用的轴设定，在校准后的状态的X轴、Y轴、Z轴的坐标轴设定中，将置于传感器坐标系Σsen传感器中心位置，以与机械凸缘坐标系Σmec相同的姿态将重心坐标系Σ_L定义在指尖负荷的重心位置。

这里，将传感器坐标系与指尖负荷的重心坐标系的位置关系(xq，yq，zq，Aq，Bq，Cq)以及质量m定义为未知变量q。即，定义为q＝(xq，yq，zq，Aq，Bq，Cq，m)。

这里，如果将模型与传感器输出的误差定义为从模型推定的力F_mdl与由传感器实际获取的前述的数据^SF_i的差，则模型与传感器输出的误差由下式(10)表示。此外，作为初始值，设为m＝M_tmp。

【算式4】

即，由于原本的目的就是寻找误差最小的q，因此，能够归结到通过反复运算而求出使得f(q_i)分别接近0的q_i的问题。此外，i是迭代。

另外，根据牛顿拉普森法，在求出用于渐进地接近0的变量q_i的情况下，由下式(11)表示的更新法则成立。

【算式5】

这里，如果在通过反复计算而近似地逼近时以每次计算的差分的形式重新表达，则得到下式(12)。

【算式6】

q_i+l＝q_i+dq_i...(12)

此外，如果具体地将式子展开，则求解的形式成为下式(13)的运算。

【算式7】

式(13)成为将式(11)和式(12)针对dq进行了整理的表达。对式(13)进行说明，对于右边进行偏微分的变量，通常全部由与q相关的变量来定义。这里，对于指尖负荷的质量/重心位置的推定方法，以还考虑了重力方向的一般形式来表达，以将想要推定的变量设为重心位置＝(Xs，Ys，Zs，As，Bs，Cs)、质量m，将从世界坐标系Σwld观察的重力矢量的斜率以与绕X轴以及Y轴的相关的旋转量将Aw、Bw设为变量的形式来表达。

如本发明的实施方式1，在将重力方向的斜率设为已知而排除的情况下，能够实现如下应对，即，除了Aw、Bw，能够将想要推定的变量设为重心位置＝(Xq，Yq，Zq，Aq，Bq，Cq)、质量m。如果对应于前述的变量q，偏微分的变量成为Xq、Yq、Zq、Aq、Bq、Cq、m。

此时，从模型推定的力F_mdl能够以下述方式定义。其中，将重心坐标系Σ_L定义在与机械凸缘坐标系Σmec相同的轴方向上，将从重心坐标系Σ_L观察的轴力的3维矢量即因质量产生的外力矢量表达为^Lf，将从相同的重心坐标系Σ_L观察的因质量产生的力矩矢量表达为^Lm，将从重心坐标系观察的重力加速度矢量表达为^Lg。另外，从世界坐标系Σwld观察的针对重心坐标系Σ_L的姿态矩阵(3×3矩阵)由^WR_L表达。下面，下式(14)表示F_mdl的定义，下式(15)～(21)表示式(14)的要素的定义。

【算式8】

F_mdl＝(^sf_x，mdl ^sf_y，mdl ^sf_z，mdl ^sm_x，mdl ^sm_y，mdl ^sm_z，mdl)^T...(14)

【算式9】

^sf_x，mdl＝n·^Lf＝n·m^Lg...(15)

【算式10】

^sf_y，mdl＝o·^Lf＝o·m^Lg...(16)

【算式11】

^sf_z，mdl＝a·^Lf＝a·m^Lg...(17)

【算式12】

^sm_x，mdl＝n·(^Lf×^Lp_s+^Lm)...(18)

【算式13】

^sm_y，mdl＝o·(^Lf×^Lp_s+^Lm)...(19)

【算式14】

^sm_z，mdl＝a·(^Lf×^Lp_S+^Lm)...(20)

【算式15】

此外，传感器与负荷重心的位置关系、即从工具部分的重心位置观察的向传感器坐标系的齐次变换矩阵^LT_S而定义为下式(22)。

【算式16】

重心坐标系Σ_L向传感器坐标系Σsen进行姿态变更的情况下的欧拉角(A，B，C)而定义为下式(23)。

【算式17】

如果基于以上的定义而利用上述式(12)、式(13)不断进行更新，则渐进地使得上述式(10)接近于0。由此，作为质量m以及传感器坐标系与工具部分的重心位置的相对关系，能够得到(xq，yq，zq，Aq，Bq，Cq)。

机器人坐标系Σrob与传感器坐标系Σsen的相对关系在传感器安装时的校准中设为已知，如上述运用水平仪等而将重力方向相对于机器人坐标系Σrob设为已知，因此，其结果，能够根据从传感器坐标系Σsen观察的重力方向与质量m的关系，在传感器坐标系上对指尖负荷的重力或惯性力的影响部分进行计算。其中，重力方向的信息并不限定于通过水平仪而获取。

这样，根据上述的校准装置，能够将未去除的错误数据去除。另外，能够高精度地得到基于由此得到的准确的偏移值而计算出的质量以及工具部分的重心位置。因此，能够实现以往无法得到的使用包含错误数据的力信息的力信息校准的高精度化，因此，能期待力控制性能格外提高的效果。

如上所述，根据实施方式1，其在进行安装于前端且针对作业对象起作用的工具部分的力控制的机械装置中，仅提取由于与作业对象的接触而在工具部分产生的外力，该校准装置具有：位置信息获取部，其获取工具部分的位置信息；力信息获取部，其获取作用于工具部分的力信息；旋转轴指定部，其指定从传感器坐标系的原点通过的任意的旋转轴；姿态生成部，其生成使工具部分绕述旋转轴旋转的姿态指令值；以及校准部，其执行校准处理，该校准处理是从力信息中减去偏移值以及指尖负荷的重力作用量，仅提取由于与作业对象的接触而产生的外力，校准部具有：近似曲线生成部，其基于根据姿态指令值而使工具部分旋转时的位置信息以及力信息，生成近似曲线；偏移值推定部，其基于近似曲线、位置信息以及力信息，对力信息的偏移值进行推定；质量/重心位置推定部，其从力信息中将偏移值去除，使用去除了偏移值的力信息，计算出指尖负荷的质量以及重心位置矢量；以及外力分量运算部，其使用推定出的偏移值、指尖负荷的质量以及重心位置矢量，从力信息中减去偏移值以及指尖负荷的重力作用量。

因此，考虑从因外力产生的影响而将偏移分量去除，高精度地计算出指尖负荷质量以及重心位置矢量，由此能够高精度地实施校准。

另外，能够独立而高精度地推定偏移值，其结果，整体的校准精度提高，因此能够进行以往没有的高精度的力控制。

实施方式2.

在上述实施方式1所记载的校准装置中，采用了如下方法，即，为了提取因线缆、配线等而产生的错误数据，实施将数据分为几组的分组化，确定结果大幅偏移的数据，发现错误数据。然而，仅以上述方式，例如在如下情况下，有时无法将错误数据去除。

事例1：错误数据包含得多的情况。在该情况下，难以仅提取错误数据。

事例2：传感器坐标系的旋转轴与由机器人赋予的旋转轴偏移，无法将近似曲线拟合为圆形。

因此，在本发明的实施方式2中，为了解决上述问题，如图10所示，其特征在于，在偏移值推定部21中，判定由于安装于工具部分的线缆以及配线所引起的外力的作用而产生的力信息的增加量是否超过预先设定的误差水平即设定值，如果达到误差水平，则移动至质量/重心位置推定部22，在除此以外的情况下，进一步追加获取新的姿态。

如图11(a)所示，在圆形的情况下，新定义与拟合的圆相比半径变动了±X％后的圆，将其用作预先设定的误差水平的阈值。其中，X的值根据线缆以及配线的外力的变动以及由于噪声而受到的力的变动的大小而个别进行定义。

这是因为，在由于噪声而受到的力的变动大的情况下，能够通过执行滤波而减弱噪声的影响，但如果将X过度减小，则会产生全部变成错误数据的情况。

根据本发明的实施方式2所涉及的校准装置，能够将脱离预先设定的误差水平的数据逐渐去除，因此能够不断接近不包含错误数据的数据。

另外，如图11(b)所示，在属于上述事例2的情况下，需要进行椭圆体拟合。在该情况下，能够通过将上述式(6)所示的圆的式子置换成楕圆的式子而进行定义来应对，由于在楕圆的状态下进行评价，因此能够以针对短径以及长径分别±X％的形式进行定义。

并且，作为其他方法，如图12所示，可以经由误差信息显示部301和数据选择部302通过使用户选择错误数据的处理而将错误数据去除，进行校准处理，该误差信息显示部301将存储于数据存储部105的位置信息以及力信息以图11所示的形式显示为曲线图，该数据选择部302通过鼠标、键盘或触摸面板之类的输入仪器而选择数据。

如上所述，根据实施方式2，在包含多个错误数据的情况下，也能够可靠地进行选择，能够实现高精度的校准，因此，其结果，能够进行以往没有的高精度的力控制。

实施方式3.

在上述实施方式1或实施方式2的校准装置中，能够进行考虑了因工具部分产生的质量的影响这样的校准。然而，在例如图13所示按照示教点12而移动的机器人中，在P2的地点处开始作业的情况下，有时针对在该时刻所作用的外力F_ext不想执行力控制。即，有时想要一边保持在机器人的周边物体11在P2的时刻所产生的接触的状态一边直至P3的位置为止进行作业。

在这种情况下，仅单纯应用工具部分的校准是不充分的，难以进行保持接触状态这样的控制。另一方面，在仅单纯地进行零点调整的系统中，不应对工具部分的姿态变更。因此，例如在将工具部分向不受机器人的周边物体11约束的方向例如绕Xsen旋转的方向进行了姿态变更的情况下，由于重力的轴方向相对于传感器坐标系发生变化，因此，无法考虑因工具部分的姿态变更产生的影响，会误判断为作用有外力。

因此，在本发明的实施方式3中，为了解决上述问题，如图14所示，在到达P2时，通过由用户对工具部分的位置姿态的指令值进行指定的偏差位置姿态指定部208而获取预先设定的位置姿态信息，将在工具部分移动至该位置姿态时所运算的作用外力的运算值存储于偏差保存部209。另外，偏差保存部209对在该瞬间由外力分量运算部207计算出的作用外力的运算值进行存储。

另外，对于存储了作业外力的运算值的下一个控制周期及其以后，从由外力分量运算部207计算出的作用外力的运算值中减去先前存储于偏差保存部209的作用外力的运算值并输出。

其中，在由外力分量运算部207计算出的作用外力的运算值之中，使以往的作用外力的运算值和将存储于偏差保存部209的作用外力的运算值减去后的偏差处理后的作用外力的运算值双方作为信息而流动。由此，在进一步由偏差位置姿态指定部208指定了姿态的情况下，能够仅将在该位置处产生的作用外力量作为偏差而进行运算。

如上所述，根据实施方式3，能够一边保持在特定的位置处产生的接触状态一边生成进一步对姿态进行变更这样复杂的动作，能够容易地进行以往没有的动作生成，因此，用户使用的便利性格外提高。

即，在进行考虑了因工具部分的校准以外的作用产生的外力量的力控制时，在某个动作中能够作为不影响力控制的恒定外力进行处理，因此进行伴随着与外部的接触的力控制的情况的设变得非常容易，能够提高用户使用的便利性。

此外，以上发明能够应用于工业用的机器人或能够进行位置控制的机械装置。

Claims (4)

1.一种校准装置，其在进行安装于前端且针对作业对象起作用的工具部分的力控制的机械装置中，仅提取由于与所述作业对象的接触而在所述工具部分产生的外力，

该校准装置具有：

位置信息获取部，其获取所述工具部分的位置信息；

力信息获取部，其获取作用于所述工具部分的力信息；

旋转轴指定部，其指定从获取所述力信息的传感器的传感器坐标系的原点通过的任意的旋转轴；

姿态生成部，其生成使所述工具部分绕述旋转轴旋转的姿态指令值；以及

校准部，其实施校准处理，该校准处理是从所述力信息中减去偏移值以及指尖负荷的重力作用量，仅提取由于与所述作业对象的接触而产生的外力，

所述校准部具有：

近似曲线生成部，其基于根据所述姿态指令值而使所述工具部分旋转时的所述位置信息以及所述力信息，生成近似曲线；

偏移值推定部，其基于所述近似曲线、所述位置信息以及所述力信息，对所述力信息的偏移值进行推定；

质量/重心位置推定部，其从所述力信息中将所述偏移值去除，使用去除了所述偏移值的所述力信息，计算出所述指尖负荷的质量以及重心位置矢量；以及

外力分量运算部，其使用推定出的所述偏移值、所述指尖负荷的质量以及重心位置矢量，从所述力信息中减去所述偏移值以及所述指尖负荷的重力作用量。

2.根据权利要求1所述的校准装置，其中，

所述偏移值推定部在由于安装于所述工具部分的线缆以及配线中的至少一者所引起的外力的作用而产生的所述力信息的增加量超过设定值的情况下，将该力信息去除。

3.根据权利要求1所述的校准装置，其中，

所述校准部具有：

偏差位置姿态指定部，其供用户指定所述工具部分的位置姿态的指令值；以及

偏差保存部，其存储在将所述工具部分移动至由所述用户指定的位置姿态时所运算的作用外力的运算值，

所述校准部还从所述偏差保存部进行了动作的下一个运算周期的校准中减去由所述偏差保存部保存的作用外力的运算值量。

4.一种机器人系统，其应用了权利要求1至3中任一项所述的校准装置。

Images