CN1433352A - 机器人的控制装置 - Google Patents

Abstract

本发明提供一种机器人控制装置，该机器人控制装置通过简单的处理可将作业坐标系的工具姿势保持一定、进行平移的柔软动作。在用具有位置和速度的状态反馈回路的控制回路来控制驱动关节用的马达的机器人控制装置上，具有以下机构：计测机器人的关节角度的机构；存储安装在机器人手指尖前端上的工具的初期姿势的机构(11)；将特定的关节轴的位置增益或速度增益设定得比其它关节轴的位置增益或速度增益小的机构(10)；根据上述关节角度和工具的初期姿势，运算对位置或速度指令的修正量以使工具姿势相对于机器人底座保持一定的机构(12)。

Description

机器人的控制装置

技术领域

本发明涉及机器人等的控制方式，涉及可边使安装在机器人手指尖前端的工具姿势保持一定、边进行平移的柔软动作的机器人控制装置及可在作业座标系上另外附加柔软特性的机器人控制装置。

技术背景

现有的机器人用图13所示的位置速度控制系统进行控制。即，采用这样的控制系统，位置指令与用位置检测器检测的位置反馈信号的偏差乘位置增益K_P后得到的速度指令，与对位置反馈信号进行微分而得到的速度反馈信号的偏差乘速度增益K_V，进行比例积分，用放大器放大，再驱动关节马达。

用这种控制系统，进行伴随有与工件接触的作业时，当工件的位置有错位等时，由于刚性高，故因设定得较大的增益K_P、K_V和积分器的作用，产生较大的扭矩，难以进行作业。

作为例子，对于用机器人的工具把持压铸成型品即工件而从压铸设备中取出的作业进行说明。

现有的机器人中，用油压缸将工件强制性地从压铸设备中推出时，在高增益的控制系统的机器人上产生很大的扭矩，故不能在把持工件的状态下取出。进行这种作业的机器人必须作有适当的柔软性的顺从动作。

另外，进行这种压铸操作作业的机器人必须在作业座标系上进行适当的平移的柔软动作，这主要是因为外部机器的外力直线地作用，接触的对象物的形状由直线和平面构成的原因。

对于这样的问题，例如特开昭58-45891号公报所揭示的以往技术中，采用这样的方法，即为了吸收作用力，利用专用的顺从性工具。

另外，特开昭63-139678号公报所揭示的以往技术中，采用这样的方法，即使用力传感器检测作用力，在作用力的方向上进行力控制。

在机器人上不附加特别的装置、进行柔软控制的方式，在特开平6-332538号公报中所揭示的以往技术中，揭示了可在关节座标系上设定柔软度的控制方式。

近年来，作为在作业座标系上进行柔软性控制的方式，在特开平8-227320号公报中所揭示的以往技术中，公开了在作业座标系上可进行柔软性设定的控制方式。

特开平8-155868号公报所揭示的以往技术中，揭示了将在工具座标系上指定的向量成分的现在位置逐步变换为指令值的方式。

但是，上述以往技术存在以下问题。

特开昭58-45891号公报所记载的方式中，必须与各种作业相一致地制造专用工具，机器人的可搬运重量的减少量与专用工具的重量相当。

特开昭63-139678号公报所记载的方式中，必须设力传感器，在力传感器上作用载荷，故系统复杂。另外，力传感器的机械强度与机器人相比较低，当外力的作用超过力传感器的允许扭矩时，会引起力传感器的破损。

特开平6-332538号公报所记载的方式揭示了控制关节座标系的柔软性的方式，但由于通过降低增益，各关节轴独立地柔软地动作，故机器人的指教作业者难以明白综合的柔软动作的方向和柔软度。另外，机器人被外部机器推压等姿势产生了变化的情况下，作业座标系的平移的柔软动作的方向也产生变化，故不能进行将工具姿势保持一定的动作。因此，扭转等没有预期到的力作用于工件上，存在压铸成型品等复杂的形状，低刚性的工件会产生破损等问题。并且，由于每个关节轴制成圆弧状使其变得柔软，故即使将这些柔软动作合成，也不能完全进行作业座标系上的平移的柔软动作。

特开平8-227320号公报所记载的方式中，揭示了控制作业座标系的柔软性的方式，必须通过使关节座标系的变位和作业座标系的变位相对应求增益，故运算关系式非常烦杂，CPU的运算负荷大，不能对于机器人的姿势变化连续地求增益。特别是在特异点附近等、关节角和作业座标间，变位的对应关系的变化率大的机器人姿势，使CPU的运算负荷增大。另外，对于机器人的姿势变化，不能进行实时的运算，难以进行连续性的增益运算，故存在着机器人的柔软度因机器人的姿势不同而有大幅度差异等问题。

特开平8-155868号公报所记载的方式中，实时取入因外力作用位置变化后的各轴的角度，运算指定的工具座标系上的位置/姿势的现在位置(顺变换)，只将指定的向量成分的现在位置变换成指令值(逆变换)。但是，关于机器人全部轴的顺变换和更新的指令值的逆变换必须是实时的，故运算量多，处理时间大幅度增加。

发明的公开

因此，本发明的课题在于提供一种机器人的控制装置，该机器人控制装置通过简单的处理使作业座标系的工具姿势保持一定、进行平移的柔软动作。

为解决上述课题的第1种方法是在用具有位置和速度的状态反馈回路的控制回路控制驱动关节用的马达的机器人控制装置上，具有以下机构：

计测机器人关节角度的机构，存储安装在机器人手指尖前端的工具的初期姿势的机构，

将特定的关节轴的位置增益或速度增益设定得比其它关节轴的位置增益或速度增益小的机构，

根据上述关节角度和工具的初期姿势，运算对位置或速度指令的修正量，以使工具姿势相对于机器人底座保持一定。

为解决上述课题的第2种方法是在用具有位置和速度状态反馈回路的控制回路控制驱动关节用的马达的机器人控制装置上，具有以下机构：

计测机器人的关节角度的机构，

存储安装在机器人手指尖前端上的工具的初期姿势的机构，根据上述关节角度运算座标系间的静力学对应关系的机构，

根据上述关节角度和机器人的作业座标系上的位置指令，进行与上述控制回路分开的第2位置、速度的状态反馈控制的机构，

将上述第2反馈控制系统的特定轴的位置增益或速度增益设定得比其它关节轴的位置增益或速度增益小的机构，

通过用上述静力学对应关系，将上述第2反馈控制系统的输出值变换成关节角扭矩值的机构，

将上述第2反馈控制系统的关节角扭矩值加在上述第1反馈控制系统的输出值上的机构，

根据上述关节角度和上述工具的初期姿势，运算对位置或速度指令的修正量，以使工具姿势相对于机器人底座保持一定。

为解决上述课题的第3种方法是在控制机器人的手腕位置和姿势的马达的控制装置上，

具有用作业座标的控制系统进行上述手腕位置控制的机构和用关节座标的控制系统进行上述手腕姿势控制的机构。

将该控制装置具体化的第4种方法是上述作业座标的控制系统具有以下机构：

计测机器人关节角度的机构，

根据上述关节角度运算座标系间的静力学对应关系的机构，根据上述关节角度和机器人的作业座标系上的位置指令在作业座标系上进行位置和速度状态反馈控制的机构，通过用上述静力学对应关系将上述反馈控制系统的输出值变换成关节座标系的关节角扭矩指令的机构。

第5种方法是上述作业座标的控制系统具有以下机构：

计测机器人的关节角度的机构，根据上述关节角度运算座标系间的微小变位的对应关系的机构，

根据上述关节角度和在机器人的作业座标系上的位置指令，在作业座标系上进行位置的状态反馈控制的机构，

通过用上述微小变位的对应关系，将上述位置的反馈控制系统的输出值变换成关节座标系的速度指令的机构，

根据上述速度指令和上述关节角度，在关节座标系上进行速度的状态反馈控制的机构。

附图的简单说明

图1是表示本发明第1种基本构成的框图，图2是表示本发明的具体实施例1的框图，图3是表示本发明的第1个作用的概图。

图4是表示本发明的第2种机构的基本构成的框图，图5是表示本发明的具体实施例2的框图，图6是表示本发明的第2个作用的概图。

图7是表示本发明的适用例的说明图，图8是表示本发明的第3种机构的基本构成的框图，图9是表示本发明的第3种基本构成的框图，图10是表示本发明的具体实施例3的框图。

图11是表示本发明的第4种机构的基本构成的框图，图12是表示本发明具体实施例4的框图。

图13是表示原来的控制方式的框图。

实施发明的最佳形式

以下，对本发明的实施形式进行说明。

本发明的实施形式1如图1所示，在通过具有位置和速度的状态反馈回路的控制回路40来控制驱动关节用的马达的机器人的控制装置上，用增益降低机构10将特定的关节轴J_m的位置增益或速度增益设定得比其它轴J₁、J₂、J₃......J_n的位置增益或速度增益小或进行扭矩限制。在机器人被外部机器推压而使其特定的关节轴柔软地动作时，进行逆变换，以保持预先在工具初期姿势存储机构11中所存储的工具的初期姿势，用修正量运算机构12运算修正量，并修正指令。

另外，如图4所示，根据对机器人的关节轴的角度指令和所计测的关节角度，与关节座标系的控制回路分开地构成作业座标系的第2位置、速度的状态反馈回路13。利用增益降低机构10，将该第2位置、速度控制系统的特定轴的位置增益或速度增益设定得比其它轴的位置增益或速度增益小或进行力限制和扭矩限制。根据所计测的关节角度，用静力学对应关系式运算机构14运算座标系间的静力学对应关系式，将第2控制系统的输出变换成关节角扭矩，加到第1控制系统中。在机器人被外部机器推压、沿着作业座标系在特定的轴向或围绕轴柔软地动作的情况下，进行逆变换，以保持预先所存储的工具的初期姿势，运算修正量并修正指令。

解决上述第2个课题的机构如图8所示，在控制机器人的手腕的位置和姿势的马达的控制装置上，将机器人的各轴的控制分为控制手腕的位置的作业座标的控制系统和控制手腕的姿势的关节座标的控制系统。作业座标系的构成机构有以下2种。

第1种机构，根据机器人的关节角度和在机器人的作业座标系上的位置指令，在作业座标系上构成位置和速度的状态反馈控制。用根据关节角度运算的座标系间的静力学对应关系，可以将该反馈控制系统的输出值变换成关节座标系的关节角扭矩指令。

第2种机构，根据机器人的关节角度和在机器人的作业座标系上的位置指令，在作业座标系上构成位置的状态反馈控制。用根据关节角度运算的座标系间的微小变位的对应关系，可以将该反馈控制系统的输出值变换成关节座标系的关节角速度指令。根据该关节角速度指令和关节角度，在关节座标系上构成速度的状态反馈控制。

在各作业座标系上的状态反馈控制中，通过降低增益或限制各控制系的输出，可实现在作业座标系上平移的柔软动作。

以下，对本发明的实施例加以说明。

图2是表示具体实施例1的图，表示在通常的关节座标系的位置速度控制系统中采用本发明控制的控制框图。用关节座标系的位置速度控制的内部回路进行通常比例积分控制，但恒定地作用的力通过静态补偿要素进行补偿。

在通常的位置速度控制状态下，由于高增益的位置控制回路和速度控制回路的作用，各关节轴在外部作用力的作用下难以柔软地变位。在这里，通过降低位置回路增益和速度回路增益，各关节轴在外力作用下可柔软地变位。但是，机器人的各关节分别不规则地进行柔软动作时，不能任意地选择柔软的方向。因此，难以使安装在机器人手指尖前端上的工具的姿势保持一定进行平移的柔软动作。

因此，本发明的控制将各关节的控制系统分为原来的高刚性的位置速度回路和低刚性的位置速度回路，为了使工具相对于机器人底座的姿势保持一定，根据以低刚性的关节轴所计测的关节角度和工具的初期姿势，实时运算对高刚性的关节轴的控制系统的角度修正量。即，根据柔软的轴的变位量，使其它的轴变位，这样，在保持工具初期姿势的状态下，可进行平移的柔软动作。以下，阐述各关节的位置速度控制回路的构成。

机器人的关节自由度有n个，将第m个轴J_m(1≤m≤n)的控制系统设定成低刚性。在此，即使设定2个以上成为低刚性的轴也没有任何问题。成为低刚性的机构，是通过增益降低机构10使J_m轴的控制系统的位置回路增益和速度回路增益比其它轴的位置增益或速度增益更降低。或者，在控制系统的后端设扭矩极限，较小地减小扭矩指令本身即可。这样，将控制系统设成低刚性的情况下，重力等静态负荷另外进行补偿。

另外，将机器人实际作业时使用的在作业座标系上的工具姿势角度R_X、R_Y、R_Z存储在工具初期姿势存储机构11中或进行设定。这实际上是将在进行柔软动作之前的工具姿势角度作为工具初期姿势进行存储，或在预先知道作业内容的情况下也可以将其姿势角度进行数值输入。

在实际作业中，当外力作用在安装在机器人手指尖前端上的工具上时，只有柔软的轴，即J_m轴按照外力进行变位。其它的轴由于高的回路增益，控制系统是高刚性的，故不进行变位。在这里，用关节角度计测机构，计测J_m轴的角度FB的值θfb_m(假设FB为反馈值)。用所计测的J_m轴的角度FB值θfb_m、进行作业时存储或设定的工具姿势角度R_X、R_Y、R_Z及J₁～J_n轴的角度指令θref₁～θrefn_n(除了θfb_m以外)，求对J_m轴以外的J₁～J_n轴的角度指令修正量Δθref₁～Δθrefn_n。这时，J₁～J_n轴(除J_m以外)是高刚性的，对于角度指令的角度FB取几乎相同的值，故也可以用角度FBθfb₁～θfb_n取代角度指令。

作为用修正量运算机构12求角度指令修正量Δθref₁～Δθref_n的具体方法，一般采用称为逆变换或逆运动学的变换式。这通常是根据机器人手指尖前端的工具位置和姿势求各关节的角度的变换式。关于该变换式，在很多文献(例如“机器人控制基础论”：吉川恒夫著)中已有记载，故在此不作论述了。

以下，按顺序表示求角度指令修正量Δθref₁～Δθref_n的方法。

(1)根据角度指令θref₁～θref_n和角度FBθfb_m，一般用称为顺变换或顺运动学的变换式，求作业座标系上的手腕轴的中心点即点(以下称为P点)的位置。

(2)根据所求出的P点的位置T_X、T_Y、T_Z和进行作业前所存储的工具姿势角度R_X、R_Y、R_Z，用逆变换式求J₁～J_n轴的新的角度指令θref₁～θref_n。

(3)通过与现在的角度指令θref₁～θref_n进行差分，求角度指令修正量Δθref₁～Δθref_n，将角度指令修正量加入J₁～J_n轴的角度指令中。

通过实时求该运算，J_m轴即使由于外力作用产生移动，J_m轴以外的轴也动作，以相对于机器人底座保持初期的工具姿势。

图3表示在6个自由度的机器人上采用本控制方法的例子，进行说明。设定为柔软的轴，假设为J₃轴，其它的J₁、J₂、J₄、J₅、J₆轴设成高的回路增益，将控制系统设成高刚性的。假设来自外部的力垂直向下地作用于安装在机器人手指尖前端的工具上。

当垂直向下的力施加在工具上时，唯一柔软的J₃轴向下方(在纸面上沿顺时针方向)变位。由于控制系统是高刚性的，故除此以外的轴不产生变位。因此，工具取得以虚线所描绘的姿势，结果，工具的姿势向下变化。

于是，通过进行以下的运算，修正J₅轴的姿势，可以在使工具姿势保持一定的状态下几乎进行平移的柔软动作。

(1)根据位置指令θref₁～θref_n和位置FBθfb₃，运算关于手腕轴中心的P点的顺变换式，求作业座标系上的P点的位置T_X、T_Y、T_Z。

(2)根据所求出的P点的位置T_X、T_Y、T_Z和进行作业前存储的工具姿势角度R_X、R_Y、R_Z，用逆变换式求J5轴的新的角度指令θref₅。首先，根据工具角度姿势R_X、R_Y、R_Z求(3，3)的回转行列ROT。若用XYZ固定角等表示，则可以表示如下。

ROT(R_X，R_Y，R_Z)＝ $\left[\begin{array}{c}[{\mathrm{cR}}_Y{\mathrm{cR}}_Z,{\mathrm{sR}}_X{\mathrm{sR}}_Y{\mathrm{cR}}_Z-{\mathrm{cR}}_Y{\mathrm{sR}}_Z,{\mathrm{cR}}_X{\mathrm{sR}}_Y{\mathrm{cR}}_Z+{\mathrm{sR}}_X{\mathrm{sR}}_X],\\ [{\mathrm{cR}}_Y{\mathrm{sR}}_Z,{\mathrm{sR}}_X{\mathrm{sR}}_Y{\mathrm{sR}}_Z+{\mathrm{cR}}_Y{\mathrm{cR}}_Z,{\mathrm{cR}}_X{\mathrm{sR}}_Y{\mathrm{sR}}_Z-{\mathrm{sR}}_X{\mathrm{cR}}_Z],\\ [-{\mathrm{sR}}_Y,{\mathrm{sR}}_X{\mathrm{cR}}_Y,{\mathrm{cR}}_X{\mathrm{cR}}_Y],\end{array}\right]$ $=\left[\begin{array}{ccc}{N}_X,& O_X,& A_X\\ N_Y,& O_Y,& A_Y\\ N_Z,& O_Z,& A_Z\end{array}\right]$

                                      .......(1)在此，CR＝cos R，SR＝sin R。

接着，用逆正切，求新的角度指令θref₅。θref₅＝tan^-1((-cθref₁sθαcθref₄+sθref₁sθref₄)A_X-(-sθref₁sθαcθref₄+cθref1sθref₄)A_Y+cθαcθref₄A_Z)/(cθα(cθref₁A_X+sθref₁A_Y)+sθαA₂)}               ...(2)

在此，θα是至J₃～J₅的联杆相对于水平面的角度，可以根据J₂和J₃求出。

cθref_n＝cosθref_n，sθref_n＝sinθref_n

(3)通过与现在的角度指令θref₅进行差分，可以求角度指令修正量Δθref₅，将该角度指令修正量加入J₅轴的角度指令。

在此，P点的变位量相对于从J₈至工具的外力作用点的距离来说非常小的情况下，可以考虑用平移的柔软控制，但在希望比较长的行程的情况下，采取以下所述的方法。

图4表示本发明的具体实施例2，进行说明。

控制方法与具体实施例1一样，目的是确实进行工具的平移柔软动作。为了进行工具的平移动作，手腕中心的P点进行平移动作即可。为了在作业座标系上进行P点的平移动作，采取的方法是除了用通常的关节座标系位置速度回路以外，还用作业座标系位置速度回路13。

在本实施例中，对于J₁～J_m轴，构成作业座标系位置速度回路13，通过用该作业座标系位置速度回路13进行柔软的动作，可以确实进行平移的柔软动作。对于J_m+1～J_m轴，用现在普通的关节座标系位置速度回路进行控制。

下面，对正交3轴和回转3轴的作业座标系位置速度回路13的构成方法进行说明。

将关节座标系的位置速度控制回路上的角度指令和角度FB的信息，一般用称为顺变换或顺运动学的关节变位和机器人的作业位置的关系式(方框15)，求作业座标系上的控制点的位置指令和位置FB。根据这些位置指令和位置FB，在作业座标系的XYZ轴方向和XYZ轴周向上构成位置速度控制回路13。在这里，该作业座标系的位置速度控制回路13的输出值是对作业座标系上的XYZ轴方向的力指令和XYZ轴周向的扭矩指令。作业座标系的位置速度控制回路13，通过增益降低机构10将XYZ轴的任一轴的控制系统的位置回路增益和速度回路增益降到比其它轴的位置增益或速度增益更低。或者，在控制系统的后端设限制，将力指令或扭矩指令本身减小得较小也可以。

因此，对于外部作用的力，在作业座标系上只在某特定的轴向上进行P点的平移柔软动作时，通过降低或限制该控制系统的增益对力进行限制。这样，可以设成在特定的轴向上允许位置偏差，在其它的轴向及轴周向上不允许位置偏差。

下面，方框14根据机器人现在的状态求一般称为函数行列式的行列式转置阵的关节座标系和作业座标系的静力学对应关系式，根据作业座标系的力指令和扭矩指令，可算出关节座标系的扭矩指令。

例如，3个自由度的机器人函数行列式的转置阵的计算式用下式表示。 $J^T={\left[\begin{array}{c}{s}_1\×\left(\mathrm{Pr}-P_1_0_0\right),s_2\×\left(\mathrm{Pr}-P_2_0_0\right),s_3\×\left(\mathrm{Pr}-P_3_0_0\right)_0_0_0\\ s_1,s_2,s_3_0_0_0\end{array}\right]}^T$

                                     ....(3)

式中：

J：函数行列式

Osi：第i关节座标的回转方向向量(以机器人的底座座标系作为基准)

Opi：第i关节位置向量(机器人的底座座标基准)

X：表示向量的外积

r：机器人的作业位置向量

T：表示行列的转置

因此，将作业座标系的输出值设为：

F＝[F_X，F_Y，F_Z]^T                        ...(4)

式中，

F：力向量

F_X，F_Y，F_Z：在作业座标系上的力

将变换成关节座标系的关节角扭矩值设为：

τ＝[τ₁，τ₂，τ₃]^T                    ...(5)

式中：

τ：在关节座标系上的扭矩向量

τ_i：第i轴的关节座标系的扭矩

则关节座标系上的扭矩可以根据以下关系求出。

τ＝J^TF                                     ...(6)

因此，通过对于机器人的姿势变化，进行(3)，(6)式的运算，关节控制系统的扭矩指令加上(5)式所示的扭矩指令，便可以构成可在机器人的全部动作区域进行作业座标上的平移的柔软动作的机器人柔软控制系统。

图5表示应用于实际机器人的控制系统的构成，对各要素的作用加以说明。机器人关节的自由度设为6个，分为使J₁～J₃轴和J₄～J₆轴动作的控制方法。在控制作业座标系的手腕轴中心的P点位置的J₁～J₃轴上构成作业座标系的位置速度回路13。在控制工具相对于作业座标系的手腕轴中心的P点的姿势的J₄～J₆轴上构成关节座标系的位置速度回路。这样，将位置和姿势的控制分开，便可以通过位置的控制进行平移的柔软动作，通过姿势的控制进行工具的姿势变化的修正。

(1)由于构成作业座标系的位置速度回路，故用J₁～J₃轴的角度指令θref₁～θref₃和角度FBθfb₁～θfb₃，分别用顺变换式15，求对于作业座标系上的P点的位置指令和位置FB。

(2)根据作业座标系上的位置指令和位置FB，在作业座标系的XYZ轴方向上构成位置速度控制回路。在这里，作业座标系的位置速度控制回路的输出值成为对作业座标系上的XYZ轴方向的力指令。作业座标系的位置速度控制回路，通过增益降低机构10，将XYZ任一轴的控制系统的位置回路增益和速度回路增益降低得比其它轴的位置增益或速度增益低。或者，也可以在控制系统的后端设限制，将力指令本身减小得较小。

在这里，假设在作业座标系的X轴方向上进行P点的平移柔软动作。用增益降低机构10将作业座标系的X轴控制系统的位置回路增益和速度回路增益降低得比其它轴的位置增益或速度增益更低。这样，因外力作用P点位置可以在X轴方向上柔软地动作，但由于在YZ轴方向上的作业座标系的位置速度回路是高刚性的，故产生力而限制动作，以不产生偏差。

(3)方框14根据机器人的角度FBθfb₁～θfb₃求关节座标系和作业座标系的静力学对应关系式，通过用其转置行列式，根据作业座标系的力指令算出关节座标系的扭矩指令。

(4)将该扭矩指令加到J₁～J₃轴的关节座标系的扭矩指令中，这样，作业座标系上的P点可在X轴方向上进行平移的柔软动作和在YZ轴方向上进行高刚性的动作。这时，将J₁～J₃轴的关节座标系的位置速度回路13发出的扭矩指令设为零，可防止扭矩指令的干扰。重力等静负荷另外进行补偿。

以下表示实际上外力作用在机器人手指尖前端的工具上时的运算方法。P点位置由于在X轴方向上进行柔软动作，J₁～J₃轴的角度各自产生了变化。用关系角度计测机构计测这些轴的角度FBθfb₁～θfb₃和手腕轴J₄～J₆轴的角度FBθfb₄～θfb₆。

根据外力作用前所存储的工具的初期姿势角度R_X、R_Y、R_Z和角度FBθfb₁～θfb₃，可以用逆变换式重新求使工具姿势保持初期姿势的手腕轴的角度指令θref₄～θref₆。根据该新的角度指令θref₄～θref₆和现在的手腕角度FBθfb₄～θfb₆(或角度指令θref₄～θref₆)，可以通过取差分求角度指令修正量Δθref₄～Δθref₆。将该角度指令修正量Δθref₄～Δθref₆加到角度指令θref₄～θref₆中。

关于J5轴的角度指令θref₅，可以用(1)式求出。关于J₄轴和J₆轴的角度指令θref₄和θref₆可以用以下的式子求出。θref₄＝tan^-1{(sθref₁A_X-cθref₁A_Y)/(cθαA₂-sθα(cθref₁A_X+sθref₁A_Y))}            ...(7)θref₆＝tan^-1{((cθref₁sθαsθref₄+sθref₁cθref₄)N_X+(sθref₁sθαsθref₄-cθref₁cθref₄)N_y-cθαsθref₄N₂)/((cθref₁sθref₄+sθref₁cθref₄)O_X+(sθref₁sθαsθref₄-cθref₁cθref₄)O_Y-cθαsθref₄O₂)}                ...(8)

以上的运算量非常少，故可以实时解明。

图6表示在6个自由度的机器人上采用本控制方法的动作例子，进行说明。

设定成柔软的轴作为作业座标系的Z轴，其他的XY轴和手腕轴即J₄～J₆轴设成高回路增益，将控制系统设成高刚性。假设外部的力垂直向下地作用于安装在机器人手指前端的工具上。

手腕轴的中心即P点，由于作业座标系的位置速度回路的作用，在外力作用下只在垂直方向(Z轴方向)上柔软地动作，故沿着图中的虚线移动。P点向下方移动，手腕轴的角度(在这里是J₅轴)变成如虚线所示那样。但是，与P点的移动相一致地运算ΔJ₅，通过使J₅轴变化，工具相对于机器人底座保持初期姿势状态，J₅轴变成实线所示的状态，工具在垂直方向上平行移动。即，工具也沿着图中的虚线移动。这时，关于手腕轴，由于只指教了初期的工具姿势，故可以与P点的平移动作相一致地使机器人动作，以使工具资料保持一定。

例如，图7表示本发明应用于压铸操作的情况，进行说明，压铸操作是用机器人21的工具22把持压铸成型品即工件23，从压铸设备中取出的作业。在机器人21用手24把持工件23的状态，将用活塞25从铸模26中推压的力F设定在沿作业座标系的Y轴的方向a上，机器人21必须沿着方向a移动。这里，若稍向方向b移动，则扭转力便作用于工件23上，在铸模26中产生变形或破损。原来，在手24部分上装上滑块等机械性机构来吸收力，故机器人的可搬重量减少，必须将外力的方向正确地指教给滑块的动作方向等，很费事。

在本实施例中，沿着方向a构成作业座标系的位置速度回路，将Y轴方向设定成柔软的方式(降低增益或限制力)，这样，P点只在方向a上柔软地动作。机器人21的P点被活塞25推出，但为了使作业座标系的手24的姿势保持一定而运算对手腕部分的修正量，故工件23从铸模23中笔直地被推出。这时，不用关节轴单位，可以在作业座标系上设定机器人的柔软方向和柔软度，故指教作业可非常简单地进行。

图9表示本发明的具体实施例3，进行说明。

图9表示在通常的关节座标系上的位置速度控制系统中采用本发明的控制方法的控制框图。

在上述实施例中，P点位置的控制系统和作业座标系的位置速度回路及关节座标系的位置速度回路并用。

因此，本实施例的控制，将机器人的各关节的控制系统完全地分为控制手腕轴的中心点的位置(以下称为P点)用的正交座标系的位置速度控制系统30和控制工具的姿势用的关节座标系的位置速度控制系统40，将正交座标系的位置速度控制的特定轴设成柔软的方式，这样，可进行作业座标系的平移的柔软动作。由于只集中到除回转以外的平移，仅用正交座标系构成位置速度回路，故可使运算量减半。

以下，对本实施例的各关节的位置速度控制回路的构成加以说明。将机器人关节的自由度设为n个。其中，用正交座标系的位置速度回路构成从机器人的底座至第m个轴(以下称为基本轴)的控制系统。另外，用关节座标系的位置速度回路41构成第m+1～第n个轴(以下称为手腕轴)的控制系统。正交座标系的构成机构，用基本轴的各关节的角度检测值，并且用一般称为顺变换或顺运动学的关节变位和机器人的作业位置的关系式，求关于在作业座标系上的手腕中心点(以下称为P点)的位置FB。根据该位置FB和在作业座标系上的P点的位置指令，在作业座标系的XYZ轴方向上构成位置速度控制回路。在这里，作业座标系的位置速度控制回路的输出值成为对作业座标系上的XYZ轴方向的力指令。作业座标系的位置速度控制回路，通过增益降低机构32将XYZ轴的任一控制系统的位置回路增益和速度回路增益降低得比其它轴的位置增益或速度增益要低。或者，虽未图示，在速度控制系统的后端设限制，将力指令减小得较小即可。因此，在将控制系统设为低刚性的情况下，重力等静负荷另外进行补偿。

下面，根据机器人现在的状态，求一般称为函数行列式的行列转置阵的关节座标系和作业座标系的静力学对应关系式，根据作业座标系的力指令可算出关节座标系的扭矩指令。并且，用上述(3)式～(6)式，针对机器人的姿势变化，进行(3)、(6)式的运算，将(5)式所示的扭矩指令加到关节控制系统的扭矩指令中，这样可以构成在机器人的全部动作区域可进行作业座标上的平移的柔软动作的机器人柔软控制系统。

手腕轴的关节座标系的位置控制回路中，用通常的位置回路增益、速度回路增益按高刚性进行位置控制，或将位置回路增益、速度回路增益降低得比其它轴的位置增益或速度增益更小，按低刚性进行柔软控制。另外，作为低刚性的机构，虽未进行图示，但也可以在速度控制器的后段设限制代替降低增益。

图10表示应用于实际机器人上的控制系统的构成，对各要素的作用加以说明。

机器人关节的自由度设为6个，分为使J₁～J₃轴和J₄～J₆轴动作的控制方法。在控制作业座标系的P点的位置的J₁～J₃轴上构成作业座标系30的位置速度回路31。在控制作业座标系的工具姿势的J₄～J₆上构成关节座标系40的位置速度回路41。这样，由于分为位置和姿势的控制，可以用位置的控制进行平移的柔软动作，用姿势的控制修正工具的姿势变化。

(1)由于构成作业座标系的位置速度回路41，故根据J₁～J₃轴的角度检测值θfb₁～θfb₃，用顺变换式求作业座标系上的P点的位置(角度)FB(检测值)。

(2)根据作业座标系上的位置指令和位置FB，在作业座标系的XYZ轴方向上构成位置速度控制回路31。在此，作业座标系的位置速度控制回路31的输出值成为向作业座标系上的XYZ轴方向的力指令[F_X、F_Y、F_Z]。作业座标系的位置速度控制回路31通过增益降低机构32将XYZ任一轴的控制系统的位置回路增益及速度回路增益降低得比其它轴的位置增益或速度增益更低。或者，在速度控制系统的后端设限制，减小力指令自身。

(3)然后，根据机器人的角度FBθfb₁～θfb₃，求关节座标系和作业座标系的静力学对应关系式，用其转置阵式，根据XYZ轴的作业座标系的力指令，算出J₁～J₃轴的关节座标系的扭矩指令。

(4)通过用该扭矩指令控制各关节的马达，可实现作业座标系的P点的平移柔软动作。这时，重力等静负荷另外进行补偿。

下面，利用上述的图6，对将本控制方法应用于6个自由度的机器人上的动作例进行说明。

假设在作业座标系的Z轴方向上进行P点的平移柔软动作。利用增益降低机构将作业座标系的Z轴控制系的位置回路增益和速度回路增益降低得比其它轴的位置增益或速度增益更低。这样，通过外力作用P点位置可以沿着Z轴方向柔软地动作，但在XY轴方向上作业座标系的位置速度回路则高刚性地动作，故产生力限制动作，以不产生偏差。

手腕轴的中心即P点，由于作业座标系的位置速度回路的作用，通过外力作用只在垂直方向(Z轴方向)上柔软地动作，故在图中的虚线上移动。P点向下方移动，手腕轴的角度(在此为J₅轴)变成虚线所示那样。但是，通过与P点的移动速度相一致地指数ΔJ₅，在工具相对于机器人底座保持初期姿势的状态下，工具在垂直方向上平行地移动。即，工具也在图中的虚线上移动。

图11表示本发明具体实施例4，进行说明。

首先，对本发明的各关节的位置速度控制回路的构成加以说明。

机器人的关节自由度设为n个。其中，用正交座标系的位置回路51和关节座标系的速度回路52构成从机器人的底座至第m个轴J₁～J_m(以下称为基本轴)的控制系统50。另外，用关节座标系的位置速度回路61构成第m+1～第n个轴J_m+1～J_n(以下称为手腕轴)的控制系统60。

正交座标系的构成方法与具体实施例3一样，用基本轴的各关节的角度检测值求作业座标系上的P点的位置(角度)FB。根据该位置FB和作业座标系上的P点的位置指令，在作业座标系的XYZ轴方向上构成位置控制回路51。在这里，作业座标系的位置控制回路51的输出值成为向作业座标系上的XYZ轴方向的速度指令。作业座标系的位置控制回路51通过增益降低机构53，将XYZ轴的任一控制系统的位置回路增益降低得比其它轴的位置增益要低。或者，虽未进行图示，但也可以在位置控制系统的后端设限制，减小速度指令。这样，将控制系统设成低刚性的，重力等静负荷另外进行补偿。

然后，根据机器人的现在状态，求一般称为函数行列式的行列式逆行列的关节座标系和作业座标系的微小变位的对应关系式，根据作业座标系的速度指令可算出关节座标系的速度指令。

例如，在3个自由度的机器人上函数行列式的逆行列的计算式用下式表示。 $J^{-1}={\left[\begin{array}{c}{s}_1\×(\mathrm{Pr}_0-P_1_0),s_2\×(\mathrm{Pr}_0-P_2_0),s_3\×\left(\mathrm{Pr}-P_3_0_0\right)_0_0_0\\ s_1,s_2,s_3_0_0_0\end{array}\right]}^{-1}$

                                            ....(9)

作业座标系的速度指令用下式表示：

V＝[V_X，V_Y，V_Z]^T                            ...(10)

式中，

V：速度指令向量

V_X，V_Y，V_Z：作业座标系上的速度指令变换为关节座标系的关节角速度指令用下式表示：

v＝[V₁，V₂，V₃]^T                            ...(11)

式中，

V：关节座标系上的速度指令向量

Vi：第i轴的关节座标系的速度指令

则关节座标系上的速度指令可以根据以下关系求出。

v＝J^-1V                                     ...(12)

因此，对于机器人的姿势变化，进行(9)、(12)式运算，根据(11)式所示的关节控制系统的速度指令和由各关节的角度检测所求出的速度FB，进行关节座标系的速度反馈控制，求关节座标系上的扭矩指令。手腕轴的关节座标系的位置控制回路设成与具体实施例3一样。

图12表示应用于实际机器人上的控制系统的构成，对各要素的作用加以说明。

将机器人的关节自由度设为6个，分别使J₁～J₃轴和J₄～J₆轴动作的控制方法。在控制作业座标系的P点位置的J₁～J₃轴上构成作业座标系的位置回路51和关节座标系上的速度回路52。在控制作业座标系的工具姿势的J₄～J₆轴上，构成关节座标系的位置速度回路61。这样，由于分为位置和姿势的控制，可以用位置的控制进行平移的柔软动作，用姿势的控制进行工具姿势变化的修正。

(1)由于构成作业座标系的位置速度回路，故用顺变换式根据J₁～J₃轴的角度检测值θfb₁～θfb₃，求作业座标系上的P点的位置FB(检测值)。

(2)根据作业座标系上的位置指令和位置FB，在作业座标系的XYZ轴方向上构成位置控制回路。在此，作业座标系的位置控制回路的输出值成为向作业座标系的XYZ轴方向发出速度指令[V_X、V_Y、V_Z]。作业座标系的位置控制回路通过增益降低机构53，将XYZ任一轴的控制系统的位置回路增益降低得比其它轴的位置增益更低。或者，在位置控制系统的后端设限制，减小速度指令。

(3)然后，根据机器人的角度FBθfb₁～θfb₃求关节座标系和作业座标系的微小变位的对应关系式，用其逆行列式，根据XYZ轴的作业座标系的速度指令，算出J₁～J₃轴的关节座标系的速度指令。

(4)用该速度指令和角度检测值，构成关节座标系50上的速度控制回路52，运算扭矩指令。通过用该扭矩指令控制各关节的马达，于是，作业座标系的P点可实现平移的柔软动作。这时，重力等静负荷另外进行补偿。以上的运算，对于构成机器人的轴来说，关于手腕的位置构成作业座标系，进行平移的柔软动作，故运算量少，可实时解明。

如上所述，根据本发明，可获得下述效果。

(1)由于将各关节的控制系统分为现有的高刚性的位置速度回路和低刚性的位置速度回路，根据因外力作用由低刚性的关节轴进行变位的关节角度和预先所存储的工具的初期姿势，实时运算对高刚性的关节轴的角度修正量，以使工具姿势相对于机器人底座保持一定，故不需要专用工具和力传感器，运算量也可以非常少，可以进行使作业座标系上的工具姿势保持一定的平移柔软控制。

(2)不需要以前必须具有的力传感器和滑块等机构性机构，不会使机器人的可搬重量减少，指教作业也容易，可非常简单地实现作业座标系上的平移的柔软控制。

(3)用关节座标系和作业座标系分别组成位置、速度的状态反馈回路，采用利用了关节角的位置信息的关节座标系和作业座标系的静力学对应关系式，使两个控制系统的输出组合起来，这样，对于外部所作用的力，可以只在作业座标系上某特定的方向上正确地限制前端的作业位置的动作，通过非常简单的运算，可以在保持工具相对于机器人底座为初期姿势的状态下，容易地实现平移的柔软控制。

(4)由于将机器人的各关节的控制系统分为控制手腕的位置的作业座标系上的反馈回路和控制手腕的姿势的关节座标系上的反馈回路，降低作业座标系的特定轴的增益或设限制，故不需要专用工具和力传感器，运算量也可以非常少，可以进行作业座标系上的平移的柔软控制。

产业上利用的可能性

本发明作为通过简单的处理使作业座标系的工具姿势保持一定、进行平移的柔软动作的机器人控制装置是有用的。

Claims (10)

1.一种机器人的控制装置，该机器人控制装置用具有位置和速度的状态反馈回路的控制回路来控制驱动关节用的马达，其特征在于，具有以下机构：

计测机器人的关节角度的机构，

存储安装在机器人手指尖前端的工具的初期姿势的机构，

将特定的关节轴的位置增益或速度增益设定得比其它关节轴的位置增益或速度增益小的机构，

根据上述关节角度和工具的初期姿势，运算对位置或速度指令的修正量的机构，以使工具姿势相对于机器人底座保持一定。

2.一种机器人的控制装置，该机器人控制装置用具有位置和速度的状态反馈回路的控制回路来控制驱动关节用的马达，其特征在于，具有以下机构：

计测机器人的关节角度的机构，

存储安装在机器人手指尖前端上的工具的初期姿势的机构，

根据上述关节角度运算座标系间的静力学对应关系的机构，

根据上述关节角度和机器人的作业座标系上的位置指令，进行与上述控制回路分开的第2位置、速度的状态反馈控制的机构，

将上述第2反馈控制系统的特定轴的位置增益或速度增益设定得比其它关节轴的位置增益或速度增益小的机构，

通过用上述静力学对应关系，将上述第2反馈控制系统的输出值变换成关节角扭矩值的机构，

将上述第2反馈控制系统的关节角扭矩值加在上述第1反馈控制系统的输出值上的机构，

根据上述关节角度和上述工具的初期姿势，运算对位置或速度指令的修正量，以使工具姿势相对于机器人底座保持一定。

3.根据权利要求1或2所记载的机器人控制装置，其特征在于，在速度控制器的后段具有设扭矩极限的机构，用来取代将上述特定关节轴的位置增益或速度增益设定得较小。

4.一种机器人的控制装置，其特征在于，在控制机器人的手腕位置和姿势的马达的控制装置上，具有下述机构：

用作业座标的控制系统进行上述手腕位置控制的机构，

用关节座标的控制系统进行上述手腕姿势控制的机构。

5.根据权利要求4所记载的机器人控制装置，其特征在于，上述作业座标的控制系统具有以下机构：

计测机器人的关节角度的机构，

根据上述关节角度运算座标系间的静力学对应关系的机构，

根据上述关节角度和机器人的作业座标系上的位置指令在作业座标系上进行位置和速度状态反馈控制的机构，

通过用上述静力学对应关系将上述反馈控制系统的输出值变换成关节座标系的关节角扭矩指令的机构。

6.根据权利要求5所记载的机器人控制装置，其特征在于，具有将上述作业座标系的位置、速度的状态反馈控制的特定轴的位置增益或速度增益设定得比其它轴的位置增益或速度增益小的机构。

7.根据权利要求5所记载的机器人控制装置，其特征在于，在速度控制器的后段具有设限制的机构，用来代替将在上述作业座标系的位置、速度的状态反馈控制中的特定轴的位置增益或速度增益设定得较小。

8.根据权利要求4所记载的机器人控制装置，其特征在于，上述作业座标的控制系统具有以下机构：

计测机器人的关节角度的机构，

根据上述关节角度运算座标系间的微小变化的对应关系的机构，

根据上述关节角度和在机器人的作业座标系上的位置指令，在作业座标系上进行位置的状态反馈控制的机构，

通过用上述微小变化的对应关系，将上述位置的反馈控制系统的输出值变换成关节座标系的速度指令的机构，

根据上述速度指令和上述关节角度，在关节座标系上进行速度的状态反馈控制的机构。

9.根据权利要求8所记载的机器人控制装置，其特征在于，具有将上述作业座标系的位置状态反馈控制的特定关节轴的位置增益设定得比其它轴的位置增益小的机构。

10.根据权利要求8所记载的机器人控制装置，其特征在于，在位置控制器的后段具有设限制的机构，用来代替将上述作业座标系的位置状态反馈控制的特定关节轴的位置增益设定得较小。

Images