CN113165185A - 连杆动作装置的控制装置 - Google Patents

Abstract

连杆动作装置(51)包括：平行连杆机构(1)，在该连杆机构(1)中，相对基端侧连杆枢毂(2)，前端侧连杆枢毂(3)经由三组以上的连杆机构(4)，以姿势可变更的方式连接；姿势变更用的促动器(53)。教导机构(56)具有变换机构(61)，该变换机构(61)根据端部连杆部件(5)的旋转角βn(n＝1，2，··)，对作为前端侧连杆枢毂(3)的通过正交坐标而表示的前端侧连杆枢毂的中心的坐标Wt(＝Xt，Yt，Zt)进行运算。在该变换机构(61)中，使用对平面和球面的方程式应用法线矢量而整理后的式。

Description

连杆动作装置的控制装置

相关申请

本申请要求申请日为2018年11月29日、申请号为JP特愿2018-223789的申请的优先权，通过参照其整体，将其作为构成本申请的一部分的内容而进行引用。

技术领域

本发明涉及一种连杆动作装置的控制装置，其对用于产业设备或医疗设备等需要精密且广泛的动作范围的设备的平行连杆机构型的连杆动作装置进行教导及控制。

背景技术

使用平行连杆机构的连杆动作装置是具有二自由度的装置，其可以用极坐标系而表示。作为该前端侧连杆枢毂的球面中心的前端侧连杆枢毂的球面中心与基端侧连杆枢毂的球面中心的关系式如专利文献1所示，从前端侧连杆枢毂的球面中心即前端侧连杆中心的坐标，朝使基端侧端部连杆部件转动的马达的旋转角是通过逆变换而求出的。与此同样地，正向变换表明通过基于牛顿-拉夫逊法的近似解而进行根据基端侧2轴的旋转角的计算。

现有技术文献

专利文献

专利文献1：JP特许5864322号公报

发明内容

发明要解决的课题

作为对连杆动作装置的教导方法之一，有通过手动方式而使前端侧连杆枢毂向目标位置动作，根据此时的基端侧的端部连杆部件的旋转角，求出以正交坐标而表示的前端侧连杆中心的坐标的方法。

但是，在专利文献1中，作为根据基端侧2轴的旋转角而求出前端侧连杆中心的坐标的方法，使用牛顿-沃尔逊法的近似解，进行通过反复运算而求出近似解的计算，因此存在花费时间的问题。在上述那样的二自由度的平行连杆机构中，求取基端侧2轴的前端部坐标的关系式除了该近似解以外没有其它方法，不得不使用该近似解，教导的计算花费时间。另外，由于基于近似解，所以前端部坐标的位置的精度差。因此，在使用已教导的前端部坐标位置而使连杆动作装置动作的情况下，特别是在多点高速移动的情况下，存在不能充分得到抑制由机构间隙引起的晃动的转矩附加控制的精度，并且，不能充分得到定位精度和轨道精度的问题。

本发明是为了解决上述课题而提出的，本发明的目的在于，提供一种连杆动作装置的控制装置，其对于具有二自由度的平行连杆机构的连杆动作装置，可以不依赖近似解而进行根据端部连杆部件的旋转角，求出通过由正交坐标而表示的前端侧连杆中心的坐标的计算，从而缩短计算时间，并且可以高精度地求出。

本发明的另一目的在于，提供一种连杆动作装置的控制装置，其对于具有二自由度的平行连杆机构的连杆动作装置的控制，可在短时间内高精度地根据由正交坐标而表示的前端侧连杆中心的坐标，进行求出端部连杆部件的旋转角的计算，可提高定位精度和轨道精度。

用于解决课题的技术方案

针对本发明的连杆动作装置的控制装置，标注实施方式中使用的标号而进行说明。

本发明的第1连杆动作装置的控制装置控制连杆动作装置中的各促动器，在该连杆动作装置中，相对基端侧连杆枢毂，前端侧连杆枢毂经由三组以上的连杆机构，以姿势可变更的方式连接，上述各连杆机构包括基端侧及前端侧的端部连杆部件与中央连杆部件，在该基端侧及前端侧的端部连杆部件中，其一端分别以可旋转的方式与上述基端侧连杆枢毂和上述前端侧连杆枢毂连接，在该中央连杆部件中，其两端分别以可旋转的方式与上述基端侧和前端侧的端部连杆部件的另一端连接，上述各连杆机构包括平行连杆机构和促动器，该平行连杆机构为通过直线表示该连杆机构的几何学模型中的相对于上述中央连杆部件的中央部的基端侧部分和前端侧部分对称的形状，该促动器使上述三组以上的连杆机构中的两组以上的连杆机构的上述基端侧的端部连杆部件转动；

该连杆动作装置的控制装置具有教导上述前端侧连杆枢毂的姿势的教导机构56。

该教导机构56具有变换机构61，该变换机构61根据上述各促动器53所连接的至少两个上述端部连杆部件5的旋转角βn(n＝1，2，··)，对作为上述前端侧连杆枢毂3的上述姿势且作为通过正交坐标而表示的上述前端侧连杆枢毂3的中心的前端侧连杆中心的坐标Wt(＝Xt，Yt，Zt)(B)进行运算。

上述正交坐标为以上述基端侧连杆枢毂2的中心轴QA和与该中心轴正交的二维正交坐标系的交点为原点O，以作为上述前端侧连杆枢毂3的中心轴QB相对于上述基端侧连杆枢毂2的中心轴QA倾斜的垂直度的折角θ为0°的上述前端侧连杆枢毂3的中心轴QB从上述基端侧连杆枢毂2离开的方向为正的坐标轴为Z轴，相对于该Z轴，用右手定则构成XY轴的坐标。

上述变换机构将从上述基端侧连杆枢毂的中心即基端侧连杆中心，朝向上述前端侧连杆中心的轨道中心的矢量即法线矢量，代入在实施方式中描述的平面和球面的方程式而使用。

上述法线矢量是由下述式而表示的法线矢量Nn(An，Bn，Cn)：

(数学公式1)

其中：

n：表示各连杆机构的***的编号(＝1，2，··)；

lo：基端侧连杆枢毂的中心C与前端侧连杆枢毂的中心B之间的距离(固定值)；

γ：中央连杆部件的轴角(中央连杆部件的两端的端部连杆部件之间的两根连接中心轴所形成的角度)；

βn：各端部连杆部件的基端的旋转角；

ηn：相位角；

An、Bn、Cn：正交坐标系的轴向成分；

按照该方案，对于具有二自由度的平行连杆机构1的连杆动作装置51，不用近似解，而进行根据基端侧的端部连杆部件5的旋转角βn求出由正交坐标所表示的前端侧连杆中心B的旋转角的计算。因此，不需要求近似解时那样的多次重复计算，能够缩短计算时间，并且能够高精度地求近似解。

在本发明中，上述平行连杆机构的上述对称还可为旋转对称。还有，在本发明中，上述平行连杆机构的上述对称也可为镜像对称。即使在平行连杆机构为旋转对称和镜像对称中的任意的对称结构的情况下，上述各式均成立。

在本发明的第2连杆动作装置的控制装置54中，在控制上述各促动器53的控制机构55的变换机构61A中，将从上述基端侧连杆枢毂的中心即基端侧连杆中心，朝向上述前端侧连杆中心的轨道中心的矢量即法线矢量，代入在实施方式中描述的平面和球面的方程式使用。

连杆动作装置的结构与针对第1连杆动作装置而描述的结构相同。

控制上述各促动器53的控制机构55包括：

变换机构61A，该变换机构61A根据作为上述前端侧连杆枢毂3的上述姿势且作为通过正交坐标而表示的上述前端侧连杆枢毂2的中心的前端侧连杆中心的坐标Wt(＝Xt，Yt，Zt)(B)，采用下述的数学公式对上述各促动器53所连接的至少两个上述端部连杆部件5的旋转角βn(n＝1，2，··)进行运算；

上述正交坐标和各数学公式与针对第1连杆动作装置的控制装置而说明的事项相同，但是再次对其予以记载。

上述正交坐标为以上述基端侧连杆枢毂的中心轴和与该中心轴正交的二维正交坐标系的交点为原点，以作为上述前端侧连杆枢毂的中心轴相对于上述基端侧连杆枢毂的中心轴倾斜的垂直度的折角为0°的上述前端侧连杆枢毂的中心轴从上述基端侧连杆枢毂离开的方向为正的坐标轴为Z轴，相对于该Z轴，用右手定则构成XY轴的坐标。

上述法线矢量是由下式表示的法线矢量Nn(An，Bn，Cn)：

(数学公式2)

其中：

n：表示各连杆机构的***的号码(＝1，2，··)；

lo：基端侧连杆枢毂的中心C与前端侧连杆枢毂的中心B之间的距离(固定值)；

γ：中央连杆部件的轴角(中央连杆部件的两端的与端部连杆部件之间的两根连接中心轴所形成的角度)；

βn：各端部连杆部件的基端的旋转角；

ηn：相位角；

An、Bn、Cn：正交坐标系的轴向成分；

在该方案的场合，对于具有二自由度的平行连杆机构的连杆动作装置，不用近似解，而进行根据正交坐标所表示的前端侧连杆中心求出端部连杆部件的旋转角的计算。因此，不需要求近似解时那样的多次重复计算，能够缩短计算时间，并且能够高精度地求近似解。因此，在进行多点移动的情况下，通过高速移动缩短设备生产节拍时间。此外，由于计算出正确的旋转角，因此抑制由机械间隙引起的抖动的恒定扭矩附加控制的精度提高，定位精度和轨道精度提高。

在权利要求书和/或说明书和/或附图中公开的至少两种构成的任何组合都包括在本发明中。特别是，权利要求书的各权利要求的两个以上的任意组合也包含在本发明中。

附图说明

本发明将从以下参考附图对优选实施方式的描述中更清楚地理解。然而，实施方式和附图仅是为了图示和说明，而不是为了限制本发明的范围。本发明的范围由权利要求书来确定。在附图中，多个附图中的相同的部件标号表示相同或相当的部分。

图1A为本发明的一个实施方式的连杆动作装置的平行连杆机构的立体图；

图1B为该连杆动作装置的控制装置的构思方案的方框图；

图2为表示该平行连杆机构的不同于图1的动作状态的立体图；

图3为该平行连杆机构中的一个连杆机构的立体图；

图4为省略了该连杆动作装置的一部分的主视图。

图5A为表示该平行连杆机构中的一个连杆机构的姿势的主视图；

图5B为表示该平行连杆机构中的一个连杆机构中的与图5A不同的姿势的主视图；

图6为通过直线而表示该平行连杆机构中的一个连杆机构的模型图；

图7为表示该连杆动作装置的基端侧连杆枢毂与端部连杆的连接部的剖视图；

图8为该平行连杆机构的正交坐标系的立体图；

图9为该平行连杆机构的前端侧连杆中心轨道的说明图；

图10A为该平行连杆机构的旋转对称结构的立体图的说明图；

图10B为该平行连杆机构的镜像对称结构的立体图的说明图；

图10C为该平行连杆机构的旋转对称结构和镜像对称结构的立体图的说明图。

具体实施方式

下面参照附图，说明本发明的一个实施方式的连杆动作装置的控制装置。如图1A和图1B所示的那样，连杆动作装置51具有平行连杆机构1和使该平行连杆机构1动作的多个促动器53，通过控制装置54而进行控制和教导。控制装置54具有控制促动器53的控制机构55、教导机构56、手动操作机构57以及存储机构58。

＜平行连杆机构1＞

在平行连杆机构1中，将前端侧连杆枢毂3经由三组连杆机构4(4-1～4-3)而可改变姿势地连接于基端侧连杆枢毂2。基端侧连杆枢毂2如图4所示的那样，设置在基台52上。图1A、图1B、图2表示平行连杆机构1的不同的动作状态，图3～图5A、图5B表示平行连杆机构1的1组连杆机构4。

图5A、图5B是表示一组连杆机构4的不同状态的主视图。各连杆机构4由基端侧的端部连杆部件5、前端侧的端部连杆部件6及中央连杆部件7构成，形成由四个旋转组件构成的三节连锁的连杆机构。基端侧及前端侧的端部连杆部件5、6形成L字状(参照图1A、图1B～图3)，基端分别自由旋转地连接在基端侧连杆枢毂2及前端侧连杆枢毂3处。中央连杆部件7在两端分别自由旋转地连接有基端侧及前端侧的端部连杆部件5、6的前端。另外，在以下的说明中，前端侧的端部连杆部件6称为“基端侧臂”。

基端侧及前端侧的端部连杆部件5、6是球面连杆构造，三组连杆机构4中的基端侧及前端侧的球面连杆中心分别与基端侧连杆枢毂2的中心即基端侧连杆中心PA及前端侧连杆枢毂3的中心即前端侧连杆中心PB一致。因此，三组连杆机构4的球面连杆中心之间的距离为基端侧连杆中心PA与前端侧连杆中心PB之间的距离lo。端部连接部件5、6和中央连接部件7的各旋转组件的中心轴可以具有某交叉角，也可以平行。

即，三组连杆机构4在几何学上形成为相同形状。在几何上相同的形状是指，用直线表示各连杆部件5、6、7的几何模型，即用各旋转组件和连接这些旋转组件之间的直线表示的模型(参照图6)是相对于中央连杆部件7的中央部的基端侧部分和前端侧部分成为对称的形状。图6是用直线表示一组连杆机构4的图。

该实施方式的连杆机构4为旋转对称型，形成基端侧连杆枢毂2及基端侧的端部连杆部件5与前端侧连杆枢毂3及前端侧的端部连杆部件6的位置关系处于相对中央连杆部件7的中心线F为旋转对称的位置结构。图5A表示基端侧连杆枢毂2的中心轴QA与前端侧连杆枢毂3的中心轴QB位于同一直线上的状态，图5B表示前端侧连杆枢毂3的中心轴QB相对于基端侧连杆枢毂2的中心轴QA，形成规定的动作角的状态。即使各连杆机构4的姿态发生变化，基端侧与前端侧连杆的中心PA、PB间的距离lo仍不发生变化。

通过基端侧连杆枢毂2、前端侧连杆枢毂3、以及三组连杆机构4，构成前端侧连杆枢毂3相对于基端侧连杆枢毂2，沿正交的两轴向自由移动的二自由度机构。换言之，是相对于基端侧连杆枢毂2而使前端侧连杆枢毂3以二自由度自由旋转地改变姿势的机构。该2自由度机构在紧凑的同时型，可使前端侧连杆枢毂3相对于基端侧连杆枢毂2的可动范围较宽。例如，可使基端侧连杆枢毂2的中心轴QA与前端侧连杆枢毂3的中心轴QB的折角θ(图1A、图1B、图3)的最大值(最大折角)约为±90°。

此外，前端侧连杆毂3相对于基端侧连杆毂2的回转角φ可设定在0°～360°的范围内。折角θ是前端侧连杆枢毂3的中心轴QB相对于基端侧连杆枢毂2的中心轴QA倾斜的垂直角，回转角φ是前端侧连杆枢毂3的中心轴QB相对于基端侧连杆枢毂2的中心轴QA倾斜的水平角度。

在该平行连杆机构1中，如果满足以下的(1)～(3)的条件时，若相对于中央连杆部件7的对称面，使中央连杆部件7与端部连杆部件5、6的角度位置关系在基端侧和前端侧相同，则由于几何对称性，基端侧连杆枢毂2和基端侧的端部连杆部件5、前端侧连杆枢毂3和前端侧的端部连杆部件6同样地动作。

(1)各连杆机构4的端部连杆部件5、6的轴部件13(图7)的角度和长度相等。

(2)基端侧的端部连杆部件5和前端侧的端部连杆部件6的几何学形状相同。

(3)中央连杆部件7的形状也在基端侧和前端侧相同。

例如，在基端侧和前端侧的连杆枢毂2、3上分别与中心轴QA、QB同轴地设置旋转轴，从基端侧向前端侧进行旋转传递的情况下，形成基端侧和前端侧成为相同的旋转角，成为等速旋转的等速万向接头。将该等速旋转时的中央连杆部件7的对称面称为等速二等分面E(图3)。

因此，通过在圆周上配置多个共有基端侧连杆枢毂2及前端侧连杆枢毂3的相同几何形状的连杆机构4，多个连杆机构4作为可无矛盾地移动的位置而仅限定为中央连杆部件7仅在等速二等分面E上的移动。由此，即使基端侧和前端侧取任意的动作角，基端侧和前端侧也等速旋转。

基端侧连杆枢毂2及前端侧连杆枢毂3在该实施方式中，为在其中心部沿轴向形成有贯通孔10，外形呈球面状的环形。贯通孔10的中心与连杆枢毂2、3的中心轴QA、QB一致。基端侧的端部连杆部件5及前端侧的端部连杆部件6分别自由旋转地连接在这些基端侧连杆枢毂2及前端侧连杆枢毂3的外周面的圆周方向的等间隔的位置。

图7是表示基端侧连杆枢毂2与基端侧的端部连杆部件5的旋转组件、以及基端侧的端部连杆部件5与中央连杆部件7的旋转组件的剖视图。在基端侧连杆枢毂2上，在圆周方向的3个部位形成有连通轴向的贯通孔10和外周侧的半径方向的连通孔11，通过设在各连通孔11内的两个轴承12，分别自由旋转地支承轴部件13。轴部件13的外侧端部从基端侧连杆枢毂2突出，基端侧的端部连杆部件5与其突出螺纹部13a连接，由螺母14紧固固定。

轴承12例如是深槽球轴承等的滚动轴承，其外圈(在图中没有示出)嵌合在上述连通孔11的内周，其内圈(在图中没有示出)嵌合在轴部件13的外周。外圈通过挡圈15而防止脱落。此外，在内圈和基端侧的端部连接部件5之间夹有垫片16，螺母14的紧固力通过基端侧的端部连接部件5和垫片16传递到内圈，对轴承12施加规定的预压。

在基端侧的端部连杆部件5和中央连杆部件7的旋转组件中，在形成于中央连杆部件7的两端的连通孔18中设置有两个轴承19，通过这些轴承19，基端侧的端部连杆部件5的前端的轴部20自由旋转地支承。轴承19经由衬垫21，通过螺母22紧固固定。

轴承19例如是深槽球轴承等的滚动轴承，其外圈(在图中没有示出)嵌合于连通孔18的内周，其内圈(在图中没有示出)嵌合于轴部20的外周。外圈通过挡圈23防止脱落。螺合在轴部20的前端螺纹部20a上的螺母22的紧固力经由垫圈21传递到内圈，对轴承19施加预定的预压。

以上，对基端侧连杆枢毂2与基端侧的端部连杆部件5的旋转组件、以及基端侧的端部连杆部件5与中央连杆部件7的旋转组件进行了说明，但前端侧连杆枢毂3与前端侧的端部连杆部件6的旋转组件、以及前端侧的端部连杆部件6与中央连杆部件7的旋转组件也是与基端侧相同的结构(省略图示)。

像这样，各连杆机构4中的四个旋转组件，即基端侧连杆枢毂2与基端侧的端部连杆部件5的旋转组件、前端侧连杆枢毂3与前端侧的端部连杆部件6的旋转组件、基端侧的端部连杆部件5与中央连杆部件7的旋转组件、以及前端侧的端部连杆部件6与中央连杆部件7的旋转组件为设置有轴承12、19的结构。由此，可抑制各旋转组件的摩擦阻力而谋求旋转阻力的减小，可确保顺畅的动力传递，并且提高耐久性。

在设置有该轴承12、19的结构中，通过对轴承12、19施加预压，可消除径向间隙和推力间隙，抑制旋转组件的晃动。由此，基端侧连杆枢毂2侧与前端侧连杆枢毂3侧之间的旋转相位差消失，可维持等速性，并且可抑制振动和异常声音的产生。特别是，通过使轴承12、19的轴承间隙为负间隙，可减少在输入输出间产生的齿隙。

<促动器53>

在图4中，平行连杆机构1的基端侧连杆毂2固定至基部52的上表面。在基台52的上部的外周设有凸缘状的促动器安装台55，在该促动器安装台55上以垂下状态安装有促动器53。促动器53的数量是与连杆机构4相同数量的3个，但也可以是2个。促动器53由旋转促动器构成，安装在其输出轴上的锥齿轮56与安装在基端侧的端部连杆部件5的轴部件13(图7)上的扇形的锥齿轮57啮合。促动器53也可以是电动马达。

在各促动器53中设置有旋转角度检测机构59(参照图1B)。旋转角度检测机构59直接检测促动器53的输出轴的旋转角度。但是，在本实施方式中，在控制装置54侧，例如在教导机构56或控制机构55中，也可以通过将输出轴与基端侧的端部连杆部件5的轴部件13的转速比(锥齿轮56、57的齿数比)相乘来检测基端侧的端部连杆部件5的旋转角βn(β1、β2、··)(参照图1A)。

＜连杆动作装置51的动作的概要＞

该连杆动作装置51按照控制装置54的控制机构55的控制或手动操作机构57的操作，旋转驱动各促动器53，由此使平行连杆机构1动作。具体来说，当驱动器53旋转驱动时，其旋转通过一对锥齿轮56、57而传递到轴部件13，基端侧的端部连杆部件5相对于基端侧连杆枢毂2的旋转角度βn发生变更。由此，前端侧连杆枢毂3相对于基端侧连杆枢毂2的位置(以下称为“前端位置”)确定。在这里，使用锥齿轮56、57来改变基端侧的端部连接部件5的角度，但也可以是其他机构(例如，正齿轮或蜗杆机构)。

控制装置54是计算机的数值控制式，根据存储在存储装置58中的程序及前端侧连杆枢毂3的中心位置即前端侧连杆中心B(以下有时称为“前端位置”)的数据，由控制装置55而进行控制。前端位置的数据可以由后述的正交坐标Wt(＝Xt，Yt，Zt)、极坐标(θ，φ)、各基端侧的端部连杆部件5的旋转角βn中的任一个而指定。

在用极坐标指定了前端位置的情况下，如下述那样而进行姿势控制。首先，对应于所指示的前端位置，求出各基端侧的端部连杆部件5的旋转角βn(n＝1，2，··)(图1A、图1B、图3)。在此，“旋转角βn”是指与所指示的前端位置相对应的各基端侧的端部连杆部件5的旋转角(相对水平面的角度)。

旋转角βn例如，通过对下述式1进行逆变换而求出。所谓逆变换是指根据基端侧连杆枢毂2的中心轴QA与前端侧连杆枢毂3的中心轴QB的折角θ(图1A、图1B、图3)以及输出侧的连杆枢毂3相对于基端侧连杆枢毂2的回转角φ而计算基端侧的端部连杆部件5的旋转角βn的变换。折角θ及回转角φ与旋转角βn具有相互关系，可从一个值而导出另一个值。

cos(θ/2)sinβn-sin(θ/2)sin(φ+ηn)cosβn+sin(γ/2)＝0；(n＝1，2，3)…(式1)

在这里：

轴角γ(图1A、图1B、图3)是自由旋转地连接在基端侧的端部连杆部件5上的中央连杆部件7的连接中心轴与自由旋转地连接在前端侧的端部连杆部件6上的中央连杆部件7的连接中心轴所形成的角度。

Ηn(图1A中的η1、η2、η3)是各基端侧的端部连杆部件5相对于成为基准的基端侧的端部连杆部件5在圆周方向的分离角，即相位角。

如图1A所示的那样，正交坐标是与基端侧连杆枢毂2的中心轴QA的延长轴正交，原点O确定在延长轴上的任意位置的XY正交坐标。作为目标的前端姿势，由前端侧连杆枢毂3的中心轴QB与XY正交坐标系相交的点的坐标即目标坐标t(Xt，Yt)表示。在这里，以基端侧连杆中心C为原点O为例而进行说明。

＜手动操作机构57＞

手动操作机构57按照来自手动输入装置(在图中没有示出)的输入而旋转驱动各促动器53，使平行连杆机构1进行姿势变更。手动输入装置例如，对设置于控制装置54的液晶显示装置的画面所显示进给按钮，每当进行触摸时，提供使对应的促动器53以微小角度动作的指令。此外，手动输入装置也可以是在设置于液晶显示装置的画面的输入框中以数值输入端部连接部件5的旋转角βn等的结构。

＜教导机构56＞

教导机构56根据基端侧的各端部连杆部件5的旋转角βn(n＝1，2或者n＝1，2，3)而求出使平行连杆机构1的前端位置位于任意位置时的前端侧连杆中心的坐标Wt(＝Xt，Yt，Zt)。使平行连杆机构1位于任意位置的动作可以通过手动操作装置57以手动输入进行，或者也可以在使各促动器53处于自由旋转的状态下用手而移动平行连杆机构1来进行。

该教导机构56具有变换机构61。变换机构61根据与各促动器53相连接的至少两个端部连杆部件5的旋转角βn(n＝1，2，··)，使用以下的法矢量、数学式来运算作为前端侧连杆枢毂3的姿势的用正交坐标表示的前端侧连杆枢毂3的中心即前端侧连杆中心Wt(＝Xt，Yt，Zt)(B)。

＜正交坐标＞

正交坐标是(1)以基端侧连杆枢毂2的中心轴QA和与该中心轴QA正交的二维正交坐标系的交点C为原点，(2)以上述折角θ为0°的前端侧连杆枢毂3的中心轴QB的远离基端侧连杆枢毂2的方向为正的坐标轴为Z轴；(3)以右手定则构成XY轴的坐标。

＜数学公式＞

上述数学式(表示法线矢量Nn(An、Bn、Cn)的式子)如下所述。此外，关于法线矢量Nn(An、Bn、Cn)，将在后面进行说明。

[数学公式3]

其中：

n：表示各连杆机构的***的号码(＝1，2，··)；

lo：基端侧连杆枢毂的中心C与前端侧连杆枢毂的中心B之间的距离(固定值)；

γ：中央连杆部件的轴角(中央连杆部件的两端与端部连杆部件的连接中心轴所形成的角度)；

βn：各端部连杆部件的基端的旋转角；

ηn：相位角；

法线矢量Nn(An，Bn，Cn)与作为前端侧连杆枢毂中心的前端侧连杆中心Wt(＝Xt，Yt，Zt)(B)的关系式可根据上述关系而适当获得。

按照该方案，对于具有2自由度的平行连杆机构1的连杆动作装置，不用近似解而进行根据端部连杆部件5的旋转角βn而求出以正交坐标表示的前端侧连杆中心B的计算。因此，不需要求近似解时那样的多次重复计算，可缩短计算时间，并且可高精度地求近似解。

＜上述式成立的理由＞

下面说明根据上述公式从各端部连杆部件的旋转角βn(n＝1，2或n＝1，2，3)而求出前端侧连杆中心的坐标Wt(＝Xt，Yt，Zt)(B)的理由。

图1A表示平行连杆机构1的结构，图3表示接头的姿态。在图3中，以基端侧连杆中心线QA与前端侧连杆中心线QB的交点作为坐标系基准，以θ·φ来表示前端侧连杆中心B。在变换机构61(图1B)中，在第1连杆机构4－1、第2连杆机构4－2和第3连杆机构4－3的3个连杆***中，根据2个***的关系式，以基端侧连杆中心C为坐标系基准，导出前端侧连杆中心B的位置关系式。

首先，考虑一个连杆***(图3)。该连杆机构4理论上可以用长度相同的点划线的直线的线段a、b这两条线及其交点A表示。线段a、b是在成为中央连杆部件7的两端的连接中心轴的直线中的从交点A到基端侧连杆中心C及前端侧连杆中心B的部分。以线段a为半径、以基端侧链节中心C为中心的基端部球面SC与以线段b为半径、以前端侧连杆中心B为中心的前端部球面SB的交点位于A点的轨道圆CI上。将包含该A点轨道圆CI的平面表示为等速二等分面E。平行连杆机构1的前端部侧部分与基端部侧部分相对于该等速二等分面E处于面对称的关系。

由于轴角γ在平行连接机构1中是恒定的，因此与线段a和b的交点A相反的端点之间的距离是恒定的。因此，基端侧连杆中心C与前端侧连杆中心B之间的距离lo是固定的。因此，前端侧连杆中心B描绘出以基端部侧的线段a(AC)为旋转中心、半径为基端侧连杆中心C与前端侧连杆中心B间的距离lo的圆轨道TB(参照图9)。在该平行连杆机构1中，各***的连杆机构4的圆形轨道TB在前端侧连杆中心B处相交。利用该关系，导出平行连杆机构1的前端位置B(前端侧连杆中心Wt(＝Xt，Yt，Zt)(B))。

图8表示该关系式所示的世界坐标系。坐标系的原点O是基端侧连杆中心C。

图9表示前端侧连杆中心轨道TB(前端侧连杆中心B的轨道)。图9的前端侧连杆中心轨道TB是针对第1连杆机构41而表示的。在3个连杆机构4的结构中，各连杆机构4的前端侧轨道TB的交点为前端中心位置(前端侧连杆中心B)。另外，由于某两个***的连杆机构4的连杆交点(轨道圆的交点(换言之，前端侧轨道TB的交点))与另一个***的连杆机构4的轨道圆相交，因此，可求出两个***的连杆机构4的连杆交点，即，可导出二自由度的前端侧连杆中心B。

在三维空间内的各连杆机构4的前端侧连杆中心轨道TB是满足下述二式与条件的轨道。

平面：An(X－An)+Bn(Y－Bn)+Cn(Z－Cn)＝0

球面：(X－An)²+(Y－Bn)²+(Z－Cn)²＝r²

矢量Nn设为法线矢量(An，Bn，··)

n：表示各连杆机构的***的号码(＝1，2，··)

＜法线矢量＞

法线矢量Nn(An、Bn、Cn)是在图9中从原点(基端侧连杆中心C)朝向前端侧连杆中心轨道TB的中心的矢量。另外，上述法线矢量Nn的An、Bn、Cn分别表示正交坐标系的轴向成分。

法线矢量如下所示。

[数学公式4]

其中；

n：表示各连杆机构的***的号码(＝1，2，··)；

lo：基端侧连杆枢毂的中心C与前端侧连杆枢毂的中心B之间的距离(固定值)

γ：中央连杆部件的轴角(中央连杆部件的两端的端部连杆部件连接的两根连接中心轴所形成的角度)；

βn：各端部连杆部件的基端的旋转角；

ηn：相位角；

An、Bn、Cn：正交坐标系的轴向分量；

前端轨道半径如下所示。

[数学公式5]

将法线矢量n1、n2应用于上述平面和球面的方程式而进行整理。将前端中心坐标(前端侧连杆中心的坐标)设为Wt(＝Xt，Yt，Zt)(B)。

上述是将基端部连杆中心作为世界坐标系原点时的前端侧连杆中心坐标。另外，在将该连杆动作装置为在前端侧连杆枢毂3上安装作业机(在图中没有示出)，并对工件进行作业的结构，在工件的作业点指定或控制前端侧连杆枢毂3的位置的实际机构中，使用前端侧连杆中心的坐标与工件坐标等的关系式。

为了表示上述关系式的正当性，将现有控制式表示如下：

[数学公式6]

G＝cos(γ/2)cosεn{-sin(θ/2)cos(φ-ηn)+sin(φ-ηn)cos(φ-ηn)(cos(θ/2)-1)}

H＝cos(γ/2)sinεn{-sin(θ/2)sin(φ-ηn)+cos(θ/2)sin²(φ-ηn)+cos²(φ-ηn)}

I＝sin(γ/2){cos(θ/2)+sin(θ/2)sin(φ-ηn)}

其中：θ：折角；

Ф：回旋角；

γ：中央连杆的轴角；

ηn：基端侧臂(基端侧的端部连杆部件)的分离角；

α：臂角；

εn：基端侧臂和中央连杆的旋转组件部的相位；

n：连杆***的号码；

如上述关系式所示，在本方法中，由于在计算步骤中不需要臂角α，因此在本方法中不需要考虑。即，各轴旋转角与臂角α无关。

如上所述，该实施方式中的使用对上述的平面和球面的方程式应用了法线矢量的数学式成立，通过将该数学式用于教导，可不依赖近似解地进行根据端部连杆部件5的旋转角βn求出通过正交坐标表示的前端侧连杆中心B的计算。因此，不需要求近似解时那样的多次重复计算，可缩短计算时间，并且可高精度地求近似解。

另外，在上述实施方式中，对平行连杆机构1为旋转对称型的场合进行了说明，但也可以适用于镜像对称型的情况。图10A和图10B表示平行连杆机构的结构存在两种。图10A示出旋转对称类型，图10B示出镜像对称类型。如图10C的那样，这两者可以从基端侧连杆中心C到前端侧连杆中心B进行理论上的表现。线段H表示直线模型(参照图6)。在图10A和图10B中，也存在连杆机构4相对于中心线相反的旋转***，但这也与上述同样，可如图10C那样表示。

＜控制机构55＞

上述说明是对将上述数学式应用于教导机构56的情况进行的说明，但也可以将上述数学式应用于控制机构55。

在该场合，控制机构55具有变换机构61A。变换机构61A根据作为前端侧连杆枢毂3的姿势而用正交坐标表示的前端侧连杆枢毂3的中心即前端侧连杆中心Wt(＝Xt、Yt、Zt)(B)，使用以下的法线矢量、数学式来运算与各促动器53相连接的至少两个端部连杆部件5的旋转角βn(n＝1、2、·)。正交坐标及各算式与对示范机构56说明的事项相同，但再次进行记载。

＜正交坐标＞

正交坐标是如下坐标：(1)以基端侧连杆枢毂的中心轴和与该中心轴正交的二维的正交坐标系的交点为原点，(2)以上所述折角为0°的前端侧连杆枢毂的中心轴远离基端侧连杆枢毂的方向作为正的坐标轴为Z轴，(3)相对于该Z轴以右手定则构成XY轴。

(数学公式)

上述数学公式(表示法线矢量Nn(An，Bn，Cn)的数学公式)如下所述2：

[数学公式7]

其中：

n：表示各连杆机构的***的号码(＝1，2，··)；

lo：基端侧连杆枢毂的中心C与前端侧连杆枢毂的中心B之间的距离(固定值)；

γ：中央连杆部件的轴角(中央连杆部件的两端的端部连杆部件的2个连接中心轴所成的角度)；

βn：各端部连杆部件的基端的旋转角；

ηn：相位角；

An、Bn、Cn：正交坐标系的轴向成分法线矢量；

法线矢量Nn(An，Bn，Cn)与作为前端侧连杆中心的前端侧连杆中心Wt(＝Xt，Yt，Zt)的关系式根据上述关系适当得到。

在该方案的场合，对于具有二自由度的平行连杆机构的连杆动作装置，不依赖于近似解而进行根据正交坐标表示的前端侧连杆中心，求出端部连杆部件的旋转角的计算。因此，不需要求近似解时那样的多次重复计算，可缩短计算时间，并且可高精度地求近似解。由此，在进行多点移动的情况下，通过高速移动而缩短设备生产节拍时间。此外，由于计算出正确的旋转角，因此抑制由机械间隙引起的抖动的恒定扭矩附加控制的精度提高，定位精度和轨道精度提高。

如上所述，参照附图而说明了优选的实施方式，但本发明不限于以上的实施方式，在不脱离本发明的主旨的范围内，可进行各种追加、变更或删除。因此，这样的结构也包含在本发明的范围内。

标号的说明：

标号1表示平行连杆机构；

标号2表示基端侧连杆枢毂；

标号3表示前端侧连杆枢毂；

标号4(4－1～4－3)表示连杆机构；

标号5表示基端侧的端部连杆部件；

标号6表示前端侧的端部连杆部件；

标号7表示中央连杆部件；

标号51表示连杆动作装置；

标号53表示促动器；

标号54表示控制装置；

标号55表示控制机构；

标号56表示教导机构；

标号57表示手动操作机构；

标号59表示旋转角检测机构；

标号61、61A表示变换机构；

符号A表示交点；

符号B表示前端侧连杆中心；

符号C表示基端侧连杆中心；

符号PA表示基端侧连杆中心；

符号PB表示前端侧连杆中心；

符号QA表示基端侧连杆枢毂的中心轴；

符号QB表示前端侧连杆枢毂的中心轴。

Claims (4)

1.一种连杆动作装置的控制装置，该连杆动作装置的控制装置控制连杆动作装置中的各促动器，在该连杆动作装置中，相对基端侧连杆枢毂，前端侧连杆枢毂经由三组以上的连杆机构，以姿势能变更的方式连接，上述各连杆机构包括基端侧及前端侧的端部连杆部件与中央连杆部件，在该基端侧及前端侧的端部连杆部件中，其一端分别以能旋转的方式与上述基端侧连杆枢毂和上述前端侧连杆枢毂连接，在该中央连杆部件中，其两端分别以能旋转的方式与上述基端侧和前端侧的端部连杆部件的另一端连接，上述各连杆机构包括平行连杆机构和上述促动器，该平行连杆机构是通过直线表示该连杆机构的几何学模型中的相对于上述中央连杆部件的中央部的基端侧部分和前端侧部分对称的形状，该促动器使上述三组以上的连杆机构中的两组以上的连杆机构的上述基端侧的端部连杆部件转动；

该连杆动作装置的控制装置具有教导上述前端侧连杆枢毂的姿势的教导机构；

该教导机构具有变换机构，该变换机构根据上述各促动器所连接的至少两个上述端部连杆部件的旋转角βn(n＝1，2，··)，对作为上述前端侧连杆枢毂的上述姿势且作为通过正交坐标表示的上述前端侧连杆枢毂的中心的前端侧连杆中心的坐标Wt(＝Xt，Yt，Zt)进行运算；

上述正交坐标为以上述基端侧连杆枢毂的中心轴和与该中心轴正交的二维正交坐标系的交点为原点，以作为上述前端侧连杆枢毂的中心轴相对于上述基端侧连杆枢毂的中心轴倾斜的垂直度的折角为0°的上述前端侧连杆枢毂的中心轴从上述基端侧连杆枢毂离开的方向为正的坐标轴为Z轴，相对于该Z轴，用右手定则构成XY轴的坐标；

上述变换机构将从上述基端侧连杆枢毂的中心即基端侧连杆中心朝向上述前端侧连杆中心的轨道中心的矢量即法线矢量，代入下述平面和球面的方程式而使用，

上述法线矢量是由下述式而表示的法线矢量Nn(An，Bn，Cn)：

其中：

n：表示各连杆机构的***的号码(＝1，2，··)；

lo：基端侧连杆枢毂的中心C与前端侧连杆枢毂的中心B之间的距离(固定值)；

γ：中央连杆部件的轴角(中央连杆部件的两端的端部连杆部件之间的两根连接中心轴所形成的角度)；

βn：各端部连杆部件的基端的旋转角；

ηn：相位角；

An、Bn、Cn：正交坐标系的轴向成分；

在这里，在上述正交坐标系的三维空间内的上述各连杆机构的前端侧连杆中心轨道中，以下述的条件，满足下述的平面与球面的方程式，

平面：An(X-An)+Bn(Y-Bn)+Cn(Z-Cn)＝0；

球面：(X-An)²+(Y-Bn)²+(Z-Cn)²＝r²；

r：上述前端侧连杆中心的轨道半径，其由下述式确定：

2.根据权利要求1所述的连杆动作装置的控制装置，其特征在于，上述平行连杆机构的上述对称为旋转对称。

3.根据权利要求1或2所述的连杆动作装置的控制装置，其特征在于，上述平行连杆机构的上述对称为镜像对称。

4.一种连杆动作装置的控制装置，该连杆动作装置的控制装置控制连杆动作装置中的各促动器，在该连杆动作装置中，相对基端侧连杆枢毂，前端侧连杆枢毂经由三组以上的连杆机构，以姿势能变更的方式连接，上述各连杆机构包括基端侧及前端侧的端部连杆部件与中央连杆部件，在该基端侧及前端侧的端部连杆部件中，其一端分别以能旋转的方式与上述基端侧连杆枢毂和上述前端侧连杆枢毂连接，在该中央连杆部件中，其两端分别以能旋转的方式与上述基端侧和前端侧的端部连杆部件的另一端连接，上述各连杆机构包括平行连杆机构和上述促动器，该平行连杆机构为通过直线表示该连杆机构的几何学模型中的相对于上述中央连杆部件的中央部的基端侧部分和前端侧部分对称的形状，该促动器使上述三组以上的连杆机构中的两组以上的连杆机构的上述基端侧的端部连杆部件转动；

控制上述各促动器的控制机构包括：

变换机构，该变换机构根据作为上述前端侧连杆枢毂的上述姿势且作为通过正交坐标表示的上述前端侧连杆枢毂的中心的前端侧连杆中心的坐标Wt(＝Xt，Yt，Zt)，对上述各促动器所连接的至少两个上述端部连杆部件的旋转角βn(n＝1，2，··)进行运算；

上述正交坐标为以上述基端侧连杆枢毂的中心轴和与该中心轴正交的二维正交坐标系的交点为原点，以作为上述前端侧连杆枢毂的中心轴相对于上述基端侧连杆枢毂的中心轴倾斜的垂直度的折角为0°的上述前端侧连杆枢毂的中心轴从上述基端侧连杆枢毂离开的方向为正的坐标轴为Z轴，相对于该Z轴，用右手定则构成XY轴的坐标；

上述变换机构将从上述基端侧连杆枢毂的中心即基端侧连杆中心朝向上述前端侧连杆中心的轨道中心的矢量即法线矢量，代入下述平面和球面的方程式而使用，

上述法线矢量是由下式表示的法线矢量Nn(An，Bn，Cn)：

其中：

n：表示各连杆机构的***的号码(＝1，2，··)；

lo：基端侧连杆枢毂的中心C与前端侧连杆枢毂的中心B之间的距离(固定值)；

γ：中央连杆部件的轴角(中央连杆部件的两端的端部连杆部件之间的两根连接中心轴所形成的角度)；

βn：各端部连杆部件的基端的旋转角；

ηn：相位角；

An、Bn、Cn：正交坐标系的轴向成分；

在这里，在上述正交坐标系的三维空间内的上述各连杆机构的前端侧连杆中心轨道中，按照下述的条件，满足下述的平面与球面的方程式：

平面：An(X-An)+Bn(Y-Bn)+Cn(Z-Cn)＝0；

球面：(X-An)²+(Y-Bn)²+(Z-Cn)²＝r²；

r：上前述前端侧连杆中心的轨道半径，其由下式确定：

Images