具体实施方式

<1.第1实施方式>

<1-1.机器人控制系统的概略结构>

下面，参照附图对第1实施方式的机器人控制系统进行说明。本实施方式的例子所示的机器人控制系统例如是对执行拾取和放置等工件的移送作业等的并联连杆机器人进行控制的系统。在图1中，机器人控制系统1具有上位控制装置2、机器人控制装置3和并联连杆机器人4。另外，下面，为了便于说明并联连杆机器人等的结构，有时适当使用上下左右前后等方向，但并不限定并联连杆机器人4等的各结构的位置关系。

上位控制装置2例如确定用户的输入操作、经由没有特别图示的通信线路使并联连杆机器人4执行的作业的种类，将对应的作业指令输出到后述的机器人控制装置3。

机器人控制装置3按照与从上述的上位控制装置2输入的作业指令对应的规定的作业顺序，对并联连杆机器人4具有的多个电机44分别供给电机驱动电力，由此，对该并联连杆机器人4的动作进行控制。此外，如后所述，在并联连杆机器人4的上述多个电机44中设置有检测各自的转子的旋转位置作为电机检测位置的编码器部14，机器人控制装置3参照各电机检测位置对各电机驱动电力的供给进行控制，以实现基于上述作业顺序的动作。另外，该机器人控制装置3的内部结构和处理内容在后面详细叙述(参照后述的图3)。

在图示的例子中，并联连杆机器人4是如下的机械系统：具有3个电机44，能够通过各电机44的轴驱动进行使末端执行器(未特别图示)向其动作空间中的任意坐标位置移动等动作控制。

<1-2.并联连杆机器人的结构>

如图1所示，该例子的并联连杆机器人4具有基础部41、可动部42、3个连杆机构部43a、43b、43c和3个电机44a、44b、44c。

3个连杆机构部43a、43b、43c沿着绕并联连杆机器人4的中心轴AX的圆周方向配置，连结基础部41和可动部42。3个电机44a、44b、44c配置于基础部41，分别驱动连杆机构部43a、43b、43c。在该例中，基础部41形成为圆盘状，在其内部固定并收纳有3个电机44a～44c。在该例中，可动部42形成为圆盘状，在下端具有安装部件45。在安装部件45安装有例如机器人手等未图示的末端执行器。另外，各电机44a～44c的结构在后面详细叙述(参照后述的图2)。

3个连杆机构部43a～43c均具有相同的结构。连杆机构部43a具有与电机44a的输出轴连结的驱动连杆46a、以及与驱动连杆46a和可动部42连结的2个被动连杆47a。2个被动连杆47a分别经由球面轴承48a与驱动连杆46a连结，分别经由球面轴承49a与可动部42连结。连杆机构部43b具有与电机44b的输出轴连结的驱动连杆46b、以及与驱动连杆46b和可动部42连结的2个被动连杆47b。2个被动连杆47b分别经由球面轴承48b与驱动连杆46b连结，分别经由球面轴承49b与可动部42连结。连杆机构部43c具有与电机44c的输出轴连结的驱动连杆46c、以及与驱动连杆46c和可动部42连结的2个被动连杆47c。2个被动连杆47c分别经由球面轴承48c与驱动连杆46c连结，分别经由球面轴承49c与可动部42连结。驱动连杆46a、46b、46c是直线状的部件，在以中心轴AX为中心的半径方向上延伸设置。在本实施方式中，将上述的3个连杆机构部43a、43b、43c、可动部42、安装部件45和末端执行器(未特别图示)统称为机构部50。

另外，在并联连杆机器人4中，在末端执行器的动作空间中设定Z轴方向对应于铅垂方向的XYZ轴直角坐标的机器人坐标系。此外，并联连杆机器人4具有的电机44和与其对应的连杆机构部43不限于图示的例子的3组，也可以设为具有4组以上的结构(即4轴以上的多轴驱动型的结构)(未特别图示)。

图2对各电机44a～44c的概略结构的一例进行说明。3个电机44a～44c具有相同的结构。如图2所示，电机44(44a～44c)具有电机主体部12、制动部13、编码器部14和减速器15。另外，该结构的电机44a～44c相当于各权利要求记载的驱动轴的一例。

电机主体部12是如下的旋转型(Rotary type)的电机：具有定子和转子(省略图示)，通过被供给电机驱动电力，转子相对于定子旋转。

制动部13通过被输入制动信号而对转子的旋转进行制动。

编码器部14检测转子的位置(也称为“旋转位置”或“旋转角度”等)，输出该位置作为电机检测位置。

减速器15将转子的旋转轴(省略图示)作为输入轴，经由设置于内部的齿轮减速机构对该轴输出进行减速转换(位置转换、扭矩转换)而输出到轴SH。上述的驱动连杆46(46a～46c)固定于该轴SH而进行摆动驱动。

<1-3.机器人控制装置的详细结构>

图3示出机器人控制装置3的内部结构和在其周边收发的各种信息。在该图3中，机器人控制装置3具有驱动控制部31和异常判定部32。

驱动控制部31对并联连杆机器人4具有的3个电机44a～44c进行控制。驱动控制部31具有作业控制部33、运动控制部34、以及与各电机44a～44c对应的3个伺服器35a、35b、35c。

作业控制部33按照与从上述的上位控制装置2输入的作业指令对应的作业顺序，将并联连杆机器人4的末端执行器的移动目的地位置即坐标位置指令输出到运动控制部34。该坐标位置指令是上述机器人坐标系上的三维坐标位置，作业控制部33始终逐次持续输出坐标位置指令作为末端执行器下次应该移动到的移动目的地位置(通过持续输出相同的坐标位置而停止移动)。

此外，作业控制部33能够根据从后述的异常判定部32输入的异常判定信息，分别根据需要对全部伺服器35输出指示电机驱动电力的供给停止的伺服关闭信号和指示输出扭矩的限制的扭矩限制信号。此外，作业控制部33同样能够根据异常判定信息，分别根据需要对全部电机44的制动部13输出对电机44的旋转进行制动的制动信号。另外，关于安装于并联连杆机器人4的安装部件45的末端执行器自身的动作，该作业控制部33也按照上述作业顺序进行控制，下面，为了便于说明，省略与末端执行器的控制有关的图示说明。此外，该作业控制部33中的处理内容在后面详细叙述。

运动控制部34通过所谓的逆运动学运算，计算末端执行器针对从上述作业控制部33输入的坐标位置指令进行移动所需的各电机44的目标位置，分别作为电机位置指令逐次输出到对应的伺服器35。

伺服器35进行如下的驱动电力的供给控制：参照从对应的电机44的编码器部14检测到的电机检测位置，并且根据从上述运动控制部34输入的电机位置指令，对该电机44进行驱动控制(该情况下为位置控制)。此外，各伺服器35将各自的内部生成的电机扭矩指令、从对应的编码器部14检测到的电机检测位置和电机检测速度一并作为电机状态数据逐次输出到后述的异常判定部32。另外，该伺服器35中的控制内容在后面详细叙述(参照后述的图4)。

异常判定部32根据从各伺服器35输入的电机状态数据(该例的电机扭矩指令、电机检测位置和电机检测速度)，判定在并联连杆机器人4的机构部50中是否发生了后述的碰撞或脱臼等异常状态，将包含该判定结果和与其相关的信息的异常判定信息输出到作业控制部33。另外，该异常判定部32中的处理内容也在后面详细叙述。

另外，上述的驱动控制部31(作业控制部33、运动控制部34和伺服器35)、异常判定部32等中的处理等不限于这些处理的分担的例子，例如，也可以由更少数量的处理部(例如1个处理部)处理，此外，还可以由更加细分化的处理部处理。此外，机器人控制装置3可以通过后述的CPU901(运算装置：参照图15)执行的程序以软件方式进行安装，其一部分或全部也可以通过ASIC、FPGA、其他电路等实际装置以硬件方式进行安装。

<1-4.伺服器中的控制处理结构>

图4示出在上述伺服器35内处理的反馈环。该图4所示的反馈环利用传递函数形式表现在伺服器35中执行的控制处理。在本实施方式的例子中，设为伺服器35根据运动控制部34输出的电机位置指令进行位置控制，与其对应地执行图示的这种位置控制反馈环和速度控制反馈环的双环处理。

在该双环处理中，求出从运动控制部34输入的电机位置指令与从编码器部14检测到的电机检测位置之间的偏差作为位置偏差，位置控制部61根据该位置偏差生成速度指令。进而，求出该速度指令与从编码器部14检测到的电机检测速度之间的偏差作为速度偏差，速度控制部62根据该速度偏差生成电机扭矩指令。另外，如图所示，通过微分运算器65对电机检测位置进行1阶时间微分来计算电机检测速度即可。然后，PWM控制部64根据经由扭矩限幅器63的电机扭矩指令供给驱动电力，由此驱动电机44。

这里，上述扭矩限幅器63在通常时，以原本的值输出被输入的电机扭矩指令，但是，在从作业控制部33输入了扭矩限制信号的期间内，利用预先设定的上限值(或下限值)对电机扭矩指令的值进行限制并输出。此外，上述PWM控制部64在通常时，通过基于电机扭矩指令的PWM控制来供给电机驱动电力，但是，在从作业控制部33输入了伺服关闭信号的期间内，停止电机驱动电力的供给本身，使电机44的动作自由(以下称为伺服关闭)。

<1-5.本实施方式的特征>

如上述结构那样，并联连杆机器人4一般具有并列连结多个连杆机构部43而成的闭合连杆机构的机构部50，驱动控制部31协作控制分别单独驱动各连杆机构部43的多个电机(驱动轴)，由此，能够控制上述的机构部50的整体进行任意的动作。

这样，在通过多个电机的复合的协作控制来驱动闭合连杆机构的结构的情况下，根据机构部50的配置和姿态，为了使其状态稳定，有时多个电机对各连杆机构施加彼此不同的扭矩而使其抵消，这些扭矩之间的关系复杂。因此，很难简单地仅通过任意一个电机的举动或状态数据来判定机构部50中的碰撞或脱臼等动作异常的发生。

与此相对，在本实施方式中，具有异常判定部32，该异常判定部32根据多个电机的状态数据(本实施方式的例子的电机扭矩指令、电机检测位置和电机检测速度)，判定并联连杆机器人4的机构部50的碰撞和脱臼的至少任意一方。

由此，能够根据对并联连杆机器人4的机构部50进行驱动的多个电机44a～44c全部的状态数据，高精度地判定机构部50中的与外部的碰撞、各球面轴承48、49中的脱臼的发生。关于实现以上功能所需的方法，下面依次进行说明。

<1-6.关于碰撞判定的基本方法>

首先，参照图5所示的单轴驱动机构作为模型例，对本实施方式中的碰撞判定的基本方法进行说明。图5所示的单轴驱动机构70构成为，利用1个电机72使从地面垂直竖立设置的滚珠丝杠71旋转，使连杆73升降移动。作为在该单轴驱动机构70中应对碰撞的控制顺序，执行如下的控制顺序：在如图5(a)所示使连杆73下降移动的中途，当如图5(b)所示意外的异物74碰撞连杆73时使连杆73的移动停止，然后，如图5(c)所示使连杆73上升移动以避开异物74。图6示出这种控制顺序中的碰撞前后的位置指令速度、电机扭矩指令、碰撞扭矩的时间变化。另外，碰撞扭矩相当于将施加给碰撞异物74的连杆73的作为阻力的接触外力换算为外界干扰扭矩而得到的值。

如图6所示，在连杆73的下降移动中稳定的电机扭矩指令在刚刚与异物74接触而碰撞后大幅降低，伴随于此，碰撞扭矩也大幅变动。这是在对电机72进行控制的伺服器的反馈环中、要实现在碰撞后输入的位置指令或速度指令的结果引起的变动。在本控制顺序中，检测这种电机扭矩指令和碰撞扭矩的变动来判定碰撞。此外，在本控制顺序中，特别地，在该碰撞判定后，立即朝向碰撞方向限制电机扭矩指令(扭矩限制)，并且停止下降移动，由此，能够抑制与异物74的碰撞的影响，接着，使连杆73上升移动而进行退避。另外，针对该退避的上升移动中的拉回方向，不需要进行扭矩限制。

这里，如图5(b)所示，在连杆73刚刚接触异物74后受到的阻力F1和异物74中的影响较小，但是，当这样持续下降移动时，受到的阻力F2增大，异物74中的影响也增大。因此，期望在连杆73实际接触异物74后尽可能迅速地进行碰撞判定，停止其下降移动。为此，可考虑通过电机扭矩指令与绝对值尽可能小的阈值的比较来进行碰撞判定的方法，但是，该情况下，当考虑通常时的连杆73的升降移动的稳定性时，很难设定该阈值。

即，在连杆73自身的质量非常大的情况下或使其进行急剧的升降动作的情况下，在其升降移动的加速期间和减速期间，分别需要较大的电机扭矩，无法更低地设定阈值。此外，例如在应用于移送工件的多轴驱动的多关节机器人等的情况下，根据把持工件的质量的增减、连杆部分的配置和姿态，所需的电机扭矩复杂地变化。特别地，在对本实施方式的并联连杆机器人4这样的闭合连杆机构的动作进行控制的情况下，根据各连杆机构部43的配置和姿态，为了使其状态稳定，有时多个电机44对各连杆机构部43施加彼此不同的扭矩并使其抵消，这些扭矩之间的关系复杂。如上所述，仅简单地进行电机扭矩指令与固定地设定的1个阈值的比较，很难进行电机驱动机构中的碰撞的判定。

与此相对，本实施方式的异常判定部32估计在碰撞时从异物74作用于机构部50的作为阻力的接触外力F自身，通过该接触外力F与非常低地设定的阈值的比较，进行迅速且高精度的碰撞判定。通过基于多个电机44的电机状态数据(本实施方式的例子的电机扭矩指令、电机检测位置和电机检测速度)的运算处理，估计该接触外力F。

<1-7.关于接触外力的估计运算处理>

下面，详细说明用于估计作用于并联连杆机器人4的机构部50的接触外力F的运算处理内容。在本实施方式的例子中，根据由图7所示的并联连杆机器人4的机构模型定义的规格参数导出拉格朗日运动方程式，根据该运动方程式计算作用于机构部50的接触外力F。

首先，末端执行器的动能能够由下式描述。

同样，驱动连杆46和被动连杆47的动能能够由下式描述。

此外，末端执行器的势能能够由下式描述。

U_p＝m_pg_cP_z(t)…(式4)

同样，驱动连杆46和被动连杆47的势能能够由下式描述。

另外，在上述(式1)～(式6)中，图7中未记载的规格参数如下所述。

m_p：末端执行器的质量

m_a：驱动连杆的质量

J_a：电机的转子和齿轮的惯性力矩

J_b(t，θ_b)：被动连杆的绕相对于XYZ平面的重心的惯性张量矩阵

g_c：重力加速度

这里，

J_b＝Rot(δx)Rot(δy)J′_bRot(δy)^TRot(δx)^T

其中，

δx、δy是被动连杆47的绕X轴、Y轴的旋转角度。此外，

m_b：被动连杆的质量

L₁：被动连杆的长度

而且，根据上述(式1)～(式6)，能够得到下式这样的拉格朗日函数。

L＝(T_p+T_a+T_b)-(U_p+U_a+U_b)…(式7)

这里，将广义坐标设为

q＝[P_x P_y P_z θ_α1 θ_α2 θ_α3]

将施加给机构部50的广义力设为

Q＝[F_x F_y F_z Trq₁ Trq₂ Trq₃]

运动方程式能够通过运算下式导出。

(其中，j＝1～6)

根据上述(式8)，能够得到基于由XYZ轴方向的向量表示的接触外力F(Fx、Fy、Fz)和各电机44的轴扭矩(Trq₁、Trq₂、Trq₃)的运动方程式。另外，在机构部50未接触异物的通常状态下，接触外力F为0，因此，Fx＝Fy＝Fz＝0。此外，各轴扭矩(Trq₁、Trq₂、Trq₃)是各电机44的减速器15的轴SH输出而对驱动连杆46进行摆动驱动的扭矩，是与设上述减速器15的减速比与减速效率之积为η的情况下的电机扭矩指令×η相当的值。而且，在本实施方式的例子中，能够在对上述(式8)的运动方程式进行变形而得到的微分方程式中，代入各电机44的电机状态数据(电机扭矩指令、电机检测位置、电机检测速度)以及末端执行器的位置(Px、Py、Pz)、速度、加速度，求出接触外力F。另外，异常判定部32根据由图7的机构模型等所示的机械规格和从各伺服器35输入的电机状态数据，通过正向运动学运算来计算上述末端执行器的位置、速度、加速度即可。

<1-8.关于针对碰撞的各轴方向的应对控制内容>

一般而言，使多个并联连杆机器人4执行的拾取和放置的动作在其控制上被区分为水平方向(XY轴方向)上的移动动作和铅垂方向(Z轴方向)上的移动动作，多数情况下，根据需要组合它们而进行动作。而且，在进行上述拾取和放置动作的作业环境中，在机构部50在水平方向上与工件以外的异物74等碰撞的情况下和在铅垂方向上与工件以外的异物74等碰撞的情况下分别需要的应对控制的内容有时不同。因此，在本实施方式中，驱动控制部31的作业控制部33以使得在机构部50发生碰撞时在水平方向(XY轴方向)和铅垂方向(Z轴方向)上分别进行不同的应对动作的方式对多个电机44进行协作控制。

<1-8-1.针对XY轴方向上的碰撞的应对控制内容>

图8(a)从侧面示出并联连杆机器人4的可动部42主要在XY轴方向(水平方向)上与异物74碰撞的状态，图8(b)示出作业控制部33与这种XY轴方向的碰撞对应地进行控制以使可动部42退避移动的状态。如上所述，异常判定部32能够在根据运动方程式得到的规定的计算式中代入各电机44的电机状态数据(电机扭矩指令、电机检测位置、电机检测速度)和末端执行器的位置、速度、加速度，由此，逐次计算该时刻施加给可动部42的接触外力F，作为异常判定信息输出到作业控制部33。该接触外力F利用机器人坐标系的X轴方向、Y轴方向、Z轴方向各自的分力Fx、Fy、Fz来计算。

因此，能够利用Fxy＝(Fx²+Fy²)^1/2求出对接触外力F的X轴方向和Y轴方向的分力Fx、Fy进行合成后的合成分力Fxy的绝对值。此外，根据各分力Fx、Fy各自的符号，还能够求出合成分力Fxy的方向。然后，异常判定部32在计算出的合成分力Fxy的绝对值超过规定阈值的情况下，判定为发生了XY轴方向上的碰撞。此外，在接触外力F的Z轴方向的分力Fz的绝对值超过另外的阈值的情况下，能够简单地判定为发生了Z轴方向上的碰撞。在以上的2个碰撞判定中，分别与阈值进行比较的对象仅仅是施加给机构部50的接触外力，因此，即使非常小地设定该阈值，也能够进行可靠度和灵敏度高的碰撞判定。

在图8(a)所示的例子中，计算出的接触外力F的分力Fz与对应的阈值相比非常小，另一方面，合成分力Fxy与对应的阈值相比非常大，因此，异常判定部32判定为在机构部50(可动部42)中仅发生了XY轴方向上的碰撞。在一般的并联连杆机器人4的作业现场，在异物74这样地在水平方向上与机构部50碰撞的情况下，仅通过使该机构部50以在水平方向沿着碰撞方向远离该碰撞点P的方式移动，就能在机构部50和异物74双方中使该碰撞的影响较轻。因此，在本实施方式中，在异常判定部32输出发生了XY轴方向的碰撞这样的判定结果和合成分力Fxy作为异常判定信息的情况下，如图8(b)所示，接收到该信息的作业控制部33输出作业指令，以使可动部42退避移动到在合成分力Fxy的方向上水平地分开预先设定的规定分开距离De的位置(规定位置)。

另外，虽然没有特别图示，但也可以不是将上述的合成分力Fxy与阈值进行比较，而是将X轴方向的分力Fx、Y轴方向的分力Fy分别单独地与阈值进行比较，按照轴方向判定碰撞，与各碰撞判定对应地进行应对。

<1-8-2.针对Z轴方向上的碰撞的应对控制内容>

此外，如图9(a)所示的例子那样，在计算出的接触外力F的合成分力Fxy与对应的阈值相比非常小、另一方面、分力Fz与对应的阈值相比非常大的情况下，判定为在机构部50(可动部42)中仅发生了Z轴方向上的碰撞。在一般的并联连杆机器人4的作业现场，在与机构部50发生了铅垂方向上的碰撞的情况下，有时机构部50例如在未图示的带式输送机等与地面之间夹着异物74并向下方按压。因此，在本实施方式中，在异常判定部32输出发生了Z轴方向的碰撞这样的判定结果和分力Fz作为异常判定信息的情况下，接收到该信息的作业控制部33如图9(b)所示输出作业指令，以使可动部42退避移动到在与分力Fz相同的方向(上方、下方)分开预先设定的规定分开距离Ue的位置(规定位置)。此外，此时，作业控制部33向全部伺服器35输出扭矩限制信号，限制各电机44的输出扭矩，由此，能够迅速且可靠地减轻机构部50和异物74双方的影响。

另外，根据机构部50和异物74的接触方向，有时判定为在上述XY轴方向和Z轴方向双方发生了碰撞，该情况下，可以同时执行双方的应对控制(退避移动)(未特别图示)。

<1-9.关于针对脱臼的应对控制内容>

例如，在机构部50急剧地与异物74碰撞的情况下，如图10(a)所示，有时成为球形接头从该机构部50具有的任意一个球面轴承48、49脱离的脱臼状态。此时，各电机44对机构部50的驱动力大幅变化，因此，异常判定部32计算的接触外力F也大幅变化。在本实施方式的例子中，异常判定部32通过判别碰撞的情况与脱臼的情况之间的接触外力F(或其时间变化等)的差异，能够区分地进行碰撞判定和脱臼判定。

在发生了这种脱臼状态的情况下，不对机构部50的各连杆机构部43施加不必要的激振力或约束力，如图10(b)所示，优选使机构部50整体直接通过利用自重的自由动作而向下方下降。因此，在本实施方式中，在异常判定部32输出脱臼判定作为异常判定信息的情况下，接收到该信息的作业控制部33向全部伺服器35输出伺服关闭信号，停止对各电机44供给驱动电力。

<1-10.控制流程>

参照图11、图12的流程图，说明为了实现以上说明的异常判定部32和作业控制部33的功能而在机器人控制装置3的CPU901中以软件方式执行的异常判定处理和作业控制处理的控制步骤。图11示出以软件方式安装了异常判定部32的情况下的流程图，在开始并联连杆机器人4的控制时开始执行该流程。

首先，在步骤S105中，机器人控制装置3的CPU901取得由各伺服器35生成的电机扭矩指令、以及检测到的电机检测位置和电机检测速度作为电机状态数据。

接着，转移到步骤S110，机器人控制装置3的CPU901通过基于上述步骤S105中取得的电机状态数据的正向运动学运算，计算该时刻的末端执行器(在图中简记为“EE”)的位置、速度、加速度。

接着，转移到步骤S115，机器人控制装置3的CPU901根据上述步骤S105中取得的电机状态数据、以及上述步骤S110中计算出的末端执行器的位置、速度、加速度，计算接触外力F(Fx、Fy、Fz)。关于本实施方式中的该计算方法，通过使用上述(式8)的运动方程式的运算处理进行即可。

接着，转移到步骤S120，机器人控制装置3的CPU901根据上述步骤S115中计算出的接触外力F的XY轴方向的分力Fx、Fy，计算合成分力Fxy。

接着，转移到步骤S125，机器人控制装置3的CPU901判定上述步骤S120中计算出的合成分力Fxy是否为对应的阈值以上、换言之是否发生了XY轴方向的碰撞。在合成分力Fxy为阈值以上的情况下，满足判定(S125：是)，转移到步骤S130。

在步骤S130中，机器人控制装置3的CPU901将在XY轴方向上发生了碰撞这样的判定结果和合成分力Fxy作为异常判定信息输出到作业控制部33。然后，转移到步骤S135。

另一方面，在上述步骤S125的判定中合成分力Fxy小于阈值的情况下，不满足判定(S125：否)，转移到步骤S135。

在步骤S135中，机器人控制装置3的CPU901判定上述步骤S115中计算出的接触外力F的Z轴方向的分力Fz是否为对应的阈值以上、换言之是否发生了Z轴方向上的碰撞。在分力Fz为阈值以上的情况下，满足判定(S135：是)，转移到步骤S140。

在步骤S140中，机器人控制装置3的CPU901将在Z轴方向上发生了碰撞这样的判定结果和分力Fz作为异常判定信息输出到作业控制部33。然后，转移到步骤S145。

另一方面，在上述步骤S135的判定中分力Fz小于阈值的情况下，不满足判定(S135：否)，转移到步骤S145。

在步骤S145中，机器人控制装置3的CPU901根据上述步骤S115中计算出的接触外力F(或其时间变化等)，判定是否发生了脱臼。在发生了脱臼的情况下，满足判定(S145：是)，转移到步骤S150。

在步骤S150中，机器人控制装置3的CPU901将发生了脱臼这样的判定结果作为异常判定信息输出到作业控制部33。然后，返回步骤S105，重复同样的步骤。

另一方面，在上述步骤S145的判定中未发生脱臼的情况下，不满足判定(S145：否)，返回步骤S105，重复同样的步骤。

通过重复以上的步骤，异常判定部32在并联连杆机器人4的机构部50发生了碰撞或脱臼时，将与其判定结果有关的信息作为异常判定信息逐次输出到作业控制部33。

接着，参照图12的流程图对作业控制处理的控制步骤进行说明。在开始并联连杆机器人4的控制时，与上述图11的异常判定处理并行地执行该作业控制处理。

首先，在步骤S205中，机器人控制装置3的CPU901取得从上位控制装置2输入的作业指令。

接着，转移到步骤S210，机器人控制装置3的CPU901按照与上述步骤S205中取得的作业指令对应的控制顺序，计算末端执行器的下一个移动目的地位置作为坐标位置指令，将其输出到运动控制部34。

接着，转移到步骤S220，机器人控制装置3的CPU901取得从异常判定部32输入的异常判定信息。

接着，转移到步骤S225，机器人控制装置3的CPU901判定在上述步骤S220中取得的异常判定信息中是否包含某些异常判定结果、换言之是否包含发生了碰撞或脱臼这样的判定结果。在未包含异常判定结果的情况下，不满足判定(S225：否)，转移到步骤S230。

在步骤S230中，机器人控制装置3的CPU901判定该时刻执行的作业顺序是否结束。在作业顺序未结束而处于执行中的情况下，不满足判定(S230：否)，返回步骤S210，重复同样的步骤。

另一方面，在作业顺序结束的情况下，满足判定(S230：是)，返回步骤S205，重复同样的步骤。

此外，另一方面，在上述步骤S225的判定中在异常判定信息中包含异常判定结果的情况下，满足判定(S225：是)，转移到步骤S235。另外，此时，也可以暂时固定坐标位置指令，使各电机44的输出位置停止。

在步骤S235中，机器人控制装置3的CPU901判定是否判定为发生XY轴方向的碰撞、换言之在异常判定信息中是否包含发生XY轴方向的碰撞的意思的判定结果。在判定为发生了XY轴方向的碰撞的情况下，满足判定(S234：是)，转移到步骤S240。

在步骤S240中，机器人控制装置3的CPU901根据异常判定信息中包含的合成分力Fxy，计算使可动部42退避移动的移动目的地位置，将其作为坐标位置指令输出到运动控制部34。然后，转移到步骤S245。

另一方面，在上述步骤S235的判定中未判定为发生XY轴方向的碰撞的情况下，不满足判定(S235：否)，转移到步骤S245。

在步骤S245中，机器人控制装置3的CPU901判定是否判定为发生Z轴方向的碰撞、换言之在异常判定信息中是否包含发生Z轴方向的碰撞的意思的判定结果。在判定为发生Z轴方向的碰撞的情况下，满足判定(S245：是)，转移到步骤S250。

在步骤S250中，机器人控制装置3的CPU901向各伺服器35输出扭矩限制信号，并且，根据异常判定信息中包含的分力Fz计算使可动部42上升退避移动的移动目的地位置，将其作为坐标位置指令输出到运动控制部34。然后，转移到步骤S255。

另一方面，在上述步骤S245的判定中未判定为发生Z轴方向的碰撞的情况下，不满足判定(S245：否)，转移到步骤S255。

在步骤S255中，机器人控制装置3的CPU901判定是否判定为发生脱臼、换言之在异常判定信息中是否包含发生脱臼的意思的判定结果。在判定为发生脱臼的情况下，满足判定(S255：是)，转移到步骤S260。

在步骤S260中，机器人控制装置3的CPU901向各伺服器35输出伺服关闭信号。然后，转移到步骤S265。

另一方面，在上述步骤S255的判定中未判定为发生脱臼的情况下，不满足判定(S255：否)，转移到步骤S265。

在步骤S265中，机器人控制装置3的CPU901经由未特别图示的显示部等向用户报知在并联连杆机器人4的机构部50中发生了碰撞或脱臼的异常的意思。然后，结束该流程。

<1-11.本实施方式的效果>

如以上说明的那样，本实施方式的机器人控制装置3具有：驱动控制部31，其对并联连杆机器人4的多个电机44进行控制；以及异常判定部32，其根据多个电机44的状态数据(电机扭矩指令、电机检测位置、电机检测速度)，判定并联连杆机器人4的机构部50的碰撞和脱臼中的至少任意一方。由此，能够根据对并联连杆机器人4的机构部50进行驱动的全部多个电机44的状态数据，高精度地判定机构部50中的与外部的碰撞、各球面轴承48、49中的脱臼的发生。

另外，本实施方式的例子的机器人控制系统1使用通过协作驱动多个旋转型电机44a～44c来进行控制的并联连杆机器人4，但还能够应用于使用通过对多个直动型(直线型)的电机进行协作驱动而进行控制的并联连杆机器人(未特别图示)的情况。该情况下，上述编码器部14被置换为能够检测可动件的移动位置(移动速度)的线性标度，此外，不需要上述减速器15。此外，在上述(式8)的运动方程式的导出中，根据使用直动型电机的并联连杆机器人的机构模型进行导出即可。

此外，在本实施方式中，特别地，在异常判定部32判定了碰撞的情况下，驱动控制部31以使得在XYZ轴直角坐标系的动作空间中的XY轴方向上的碰撞和Z轴方向上的碰撞中机构部50的动作不同的方式对多个电机44进行控制，其中，XYZ轴直角坐标系的Z轴方向对应于铅垂方向。由此，在组合地执行水平方向(XY轴方向)上的移动动作和铅垂方向(Z轴方向)上的移动动作的并联连杆机器人4的拾取和放置的动作中，能够进行发生了碰撞的情况下的功能性应对。

此外，在本实施方式中，特别地，在异常判定部32判定了XY轴方向上的碰撞的情况下，驱动控制部31以使机构部50具有的末端执行器移动到在XY轴方向上与碰撞点P分开规定距离的相对位置的方式对多个电机44进行控制。例如，即使在水平方向上与机构部50发生碰撞，在多数情况下，仅通过使该机构部50以在水平方向上从该碰撞方向分开的方式移动，就能够在机构部50和异物74双方中使该碰撞的影响较轻。这样，针对水平方向的碰撞，驱动控制部31以使得仅向规定位置移动的方式对多个电机44进行协作控制，就能迅速且可靠地进行适当的应对。另外，碰撞判定时的移动目的地位置不限于上述这种从碰撞点P起的相对位置，也可以移动到在机器人坐标系中预先设定的所谓的原始位置等绝对位置。

此外，针对XY轴方向上的碰撞，也可以使XY轴方向上的末端执行器的移动立即停止。即，在异常判定部32判定了XY轴方向上的碰撞的情况下，驱动控制部31也可以以使机构部50具有的末端执行器的移动停止的方式对多个电机44进行控制。例如，在水平方向上与机构部50发生了碰撞的情况下，为了在机构部50和异物74双方中不使该碰撞的影响增大，最低限度需要使XY轴方向的移动停止。这样，针对水平方向的碰撞，驱动控制部31以使XY轴方向的移动停止的方式对多个电机44进行协作控制，由此，与向规定位置移动相比，能够省去该驱动控制部31中的运算处理的负担，能够更迅速且可靠地进行适当的应对。另外，关于此时的移动停止控制，作业控制部33可以持续输出相同的坐标位置指令而使末端执行器的位置固定。或者，作业控制部33也可以进行输出伺服关闭信号并停止向各电机44供给电机驱动电力的伺服关闭，并且输出制动信号对各电机44的位置进行制动。

此外，在本实施方式中，特别地，在异常判定部32判定了Z轴方向上的碰撞的情况下，驱动控制部31限制多个电机44的输出扭矩，并且以使机构部50具有的末端执行器向在Z轴方向上与碰撞点P分开规定距离的相对位置移动的方式对多个电机44进行控制。例如，在铅垂方向上与机构部50发生了碰撞的情况下，有时机构部50例如在带式输送机等与地面之间夹着异物74并向下方按压。与此相对，不仅使机构部50从该碰撞方向向铅垂方向(上升方向、下降方向)移动，还限制各电机44的输出扭矩，由此，能够迅速且可靠地减轻机构部50和异物74双方的影响。另外，碰撞判定时的移动目的地位置不限于上述这种从碰撞点P起的相对位置，也可以移动到在机器人坐标系中预先设定的所谓的原始位置等绝对位置。

此外，在本实施方式中，特别地，在异常判定部32判定了脱臼的情况下，驱动控制部31停止多个电机44的控制。由此，例如，在球面轴承48、49中发生脱臼而使机构部50分解的情况下，作业控制部33也通过输出伺服关闭信号并停止向各电机44供给电机驱动电力的伺服关闭来停止控制，由此，不对各连杆机构施加不必要的激振力和约束力，能够允许自由动作，因此，能够减轻对周围的干扰。

<1-12.变形例>

另外，以上说明的实施方式能够在不脱离其主旨和技术思想的范围内进行各种变形。

在上述实施方式中，异常判定部32根据各电机44的状态数据(电机扭矩指令、电机检测位置、电机检测速度)，计算被估计为对机构部50施加的接触外力F，根据该接触外力F直接判定机构部50的碰撞和脱臼的发生，但不限于此。除此以外，也可以根据计算出的接触外力F进一步计算各电机44的外界干扰扭矩，根据这些干扰扭矩判定碰撞和脱臼。该情况下，上述的外界干扰扭矩相当于作为针对接触外力F的阻力量而包含在各电机44的电机扭矩指令中的潜在扭矩值。为了估计该外界干扰扭矩，通过考虑上述图7所示的机构模型而按照各电机44适当设计的外界干扰观测器，将接触外力F转换为各电机44的估计外界干扰即可。该外界干扰观测器根据上述(式8)中的广义力Q的接触外力F和轴扭矩Trq的关系来设计即可(未特别图示)。

这样估计出的各电机44的外界干扰扭矩是简单地相当于对机构部50施加的接触外力F的扭矩值，因此，通过与以非常低的值设定的阈值进行比较，能够进行可靠度和灵敏度高的碰撞判定。此外，通过根据该外界干扰扭矩进行判定，还能够进行精度高的脱臼判定。此外，为了更高精度地估计上述的接触外力F和外界干扰扭矩，电机44的编码器部14也可以不检测电机44的转子，而检测减速器15的输出轴即轴SH的旋转位置作为电机检测位置，参照对应的电机扭矩指令、电机检测速度估计编码器的位置、速度、加速度、接触外力F和外界干扰扭矩。

<2.第2实施方式>

在上述第1实施方式中，根据基于各电机44的状态数据的运动方程式计算碰撞和脱臼的判定中使用的接触外力F，但不限于此。除此以外，也可以根据设置于机构部50的传感器的检测值与电机状态数据的比较来求出接触外力F，下面，对这种结构的第2实施方式进行说明。另外，省略与上述第1实施方式相同的结构和处理的图示和说明。

与上述图1对应的图13示出本实施方式中的机器人控制系统1A的概略结构。在该图13中，并联连杆机器人4在可动部42的上表面具有三维加速度传感器16。该三维加速度传感器16检测与机器人坐标系的X轴方向、Y轴方向、Z轴方向分别对应的可动部42的加速度作为坐标检测加速度，将其输出到机器人控制装置3。与上述图3对应的图14示出本实施方式中的机器人控制装置3的内部结构和在其周边收发的各种信息。在该图14中，上述三维加速度传感器16检测到的坐标检测加速度被输入到机器人控制装置3的异常判定部32。

在以上结构的机器人控制系统1A中，异常判定部32能够根据从各伺服器35输入的电机状态数据计算末端执行器的加速度(和位置、速度)，根据这些计算值和从三维加速度传感器16输入的坐标检测加速度，估计作用于机构部50的接触外力F。例如，可认为在XYZ轴的各轴方向上，末端执行器的计算加速度与坐标检测加速度之间的差分相对于接触外力F的分力Fx、Fy、Fz成为比例关系(F＝a·m)。根据该研究，考虑上述图7所示的机构模型的各规格参数，能够按照XYZ轴的各轴方向求出接触外力F的分力Fx、Fy、Fz。此外，还能够根据这样求出的接触外力F进一步估计各电机44的外界干扰扭矩。

由此，在该第2实施方式中，与利用运动方程式的上述第1实施方式相比，能够通过负担更少的运算处理简单地计算作用于机构部50的各轴方向的接触外力F。进而，还能够利用根据上述(式8)设计的外界干扰观测器，参照从各伺服器35输入的电机状态数据计算外界干扰扭矩。

<3.机器人控制装置的硬件结构例>

接着，参照图15，对实现通过上述说明的CPU901执行的程序以软件方式安装的驱动控制部31(作业控制部33、运动控制部34、伺服器35)、异常判定部32等进行的处理的机器人控制装置3的硬件结构例进行说明。

如图15所示，机器人控制装置3例如具有CPU901、ROM903、RAM905、ASIC或FPGA等面向特定用途构建的专用集成电路907、输入装置913、输出装置915、记录装置917、驱动器919、连接端口921和通信装置923。这些结构经由总线909、输入输出接口911以相互能够传递信号的方式连接。

程序例如能够预先记录在ROM903、RAM905、记录装置917等中。

此外，程序例如还能够暂时或永久地预先记录在软盘等磁盘、各种CD、MO盘、DVD等光盘、半导体存储器等可移动的记录介质925中。这种记录介质925还能够作为所谓的软件包来提供。该情况下，这些记录介质925中记录的程序也可以由驱动器919读出，经由输入输出接口911、总线909等记录在上述记录装置917中。

此外，程序例如还能够预先记录在下载站点、其他计算机、其他记录装置等(未图示)中。该情况下，程序经由LAN、互联网等网络NW传输，通信装置923接收该程序。而且，通信装置923接收到的程序也可以经由输入输出接口911、总线909等记录在上述记录装置917中。

此外，程序例如还能够预先记录在适当的外部连接设备927中。该情况下，程序也可以经由适当的连接端口921传输，经由输入输出接口911、总线909等记录在上述记录装置917中。

而且，CPU901按照上述记录装置917记录的程序执行各种处理，由此实现上述驱动控制部31(作业控制部33、运动控制部34、伺服器35)、异常判定部32等进行的处理。此时，CPU901例如可以从上述记录装置917直接读出程序并执行，也可以暂时载入到RAM905后执行。进而，CPU901例如在经由通信装置923、驱动器919、连接端口921接收程序的情况下，也可以直接执行接收到的程序而不记录在记录装置917中。

此外，CPU901例如也可以按照需要，根据从鼠标、键盘、麦克风(未图示)等输入装置913输入的信号和信息进行各种处理。

而且，CPU901例如也可以从显示装置或声音输出装置等输出装置915输出执行上述处理后的结果，进而，CPU901也可以根据需要经由通信装置923或连接端口921发送该处理结果，还可以记录在上述记录装置917、记录介质925中。

另外，在以上的说明中，在存在“垂直”、“平行”、“平面”等记载的情况下，该记载不是严格的意思。即，这些“垂直”、“平行”、“平面”容许设计上、制造上的公差、误差，是“实质上垂直”、“实质上平行”、“实质上平面”这样的意思。

此外，在以上的说明中，在存在外观上的尺寸、大小、形状、位置等“同一”、“相同”、“相等”、“不同”等记载的情况下，该记载不是严格的意思。即，这些“同一”、“相等”、“不同”容许设计上、制造上的公差、误差，是“实质上同一”、“实质上相同”、“实质上相等”、“实质上不同”这样的意思。

此外，除了以上已经叙述的内容以外，也可以适当组合利用上述实施方式和各变形例的方法。除此之外，虽然没有一一例示，但是，上述实施方式和各变形例能够在不脱离其主旨的范围内施加各种变更来实施。