多关节型机器人的控制装置、控制方法以及控制程序的制作方法
多关节型机器人的控制装置、控制方法以及控制程序的制作方法
【专利摘要】在速度抑制处理中，维持手腕前端的位置及手腕前端的移动速度与轴的允许速度，并且抑制目的坐标系中的姿势角度的特定成分的变动。在计算提示路径上的插值点的位置、姿势、以及各轴的角度并进行运动的多关节机器人中，判别手腕轴的速度是否超过允许限度，在超过允许限度的情况下，计算使速度为允许限度内的该手腕轴的角度的多个候选，从多个该候选中，选择焊接线坐标系的姿势角的特定成分的变动得到最大抑制的候选，作为该手腕轴的角度，重新计算与其对应的其他轴的角度并作为下一插值点的角度，驱动多关节机器人。
【专利说明】多关节型机器人的控制装置、控制方法以及控制程序
【技术领域】
[0001]本发明涉及多关节型机器人的控制装置、控制方法、以及控制程序，尤其涉及在手腕轴的各驱动轴的速度超过允许范围的情况下，维持手腕的姿势并将各驱动轴的速度维持在允许范围内的技术。
【背景技术】
[0002]这里，在图23所示的作为一种多关节型机器人的6轴机械手中，控制安装于基座的胳膊轴的关节轴从基座起依次为Jl轴、J2轴、J3轴，控制手腕轴的关节轴从胳膊部分到手腕前端依次为J4轴、J5轴、J6轴。在此情况下,在给出Jl轴?J6轴的各关节角Q1'Θ 6时，通过求出正运动学问题的解，能够求出正交坐标系中的末端执行器(安装于手指的前端部分的焊接器等效果器)的位置的XYZ坐标、和末端执行器的姿势角α、β、Y (另外，姿势角α、β、Y用欧拉角或横摆(Roll).纵倾(Pitch).横倾(Yaw)角等表示)。另外相反，在给出末端执行器的位置的XYZ坐标和姿势角α、β、Y时，通过求出逆运动学问题的解，能够求出Jl轴?J6轴的各关节角Θ i?Θ 6。
[0003]另外，如图24所示，在J5轴的关节角Θ 5在O度附近时使末端执行器以一定速度运动的情况下，J4轴的关节角Θ 4与J6轴的关节角Θ 6的角度急剧变化，因此必须使J4轴与J6轴高速转动。但是，机械手的各关节轴无法超过转动速度的上限值，在勉强超过上限值动作的情况下，产生末端执行器的路径偏离预定路径，或者末端执行器振动的问题。这样，在多关节型机械手中，有时稍微移动末端执行器的位置或姿势角，就产生要求具有极大转动速度的关节轴，出现无法动作的情况。在发生了这种无法动作的情况下，难以兼顾各关节轴的速度和末端执行器的移动路径的精度，会出现为了确保移动路径的精度各关节轴的转动速度低于要求的速度，或者安全装置起作用从而停止机械手的动作的问题。
[0004]对此，专利文献I?3中公开了在某个关节轴的速度超过允许范围的情况下，使各手腕轴不发生急剧变化并保持限制内的速度动作的方法。
[0005]现有技术文献
[0006]专利文献
[0007]专利文献I JP特开昭62-162109号公报
[0008]专利文献2 JP特开平6-324730号公报
[0009]专利文献3 JP特开2003-300183号公报
【发明内容】
[0010]发明要解决的问题
[0011]但是，一般已知，在某个关节轴的速度超过允许范围的情况下，使各手腕轴不发生急剧变化并保持限制内的速度动作，通过预定路径的情况下，无法同时满足对末端执行器要求的(I)位置(2)移动速度(3)姿势角度这三个要素。因此，在一般的机械手中，进行使机械手动作的控制，从而在(I)?(3)要素中仅满足一个或两个要素。[0012]例如，在专利文献I及专利文献2中，记载了维持末端执行器的(I)位置和(3)姿势角度的方法，但是由于无法维持(2)移动速度，移动速度变慢，因此在末端执行器具有焊接器的情况下，花费比原本要求的时间更长的时间通过作业线，比其他部分的焊接时间长，存在强度降低等问题。此外，移动速度变慢、焊接时间变长的地方过度隆起，有可能对下面的步骤(例如，第二层的焊接步骤等)产生影响。
[0013]另外，专利文献3中记载了维持末端执行器的移动速度或位置中的任一者的方法，但在维持移动速度的情况下无法维持位置，在末端执行器具有焊接器或涂镀器的情况下，存在偏离原本预定的作业线，效果无法到达需要焊接或涂镀的位置的问题。
[0014]因此，本发明鉴于上述情况而作，其目的在于提供一种多关节型机器人的控制装置、控制方法以及控制程序，在需要使手腕轴超过指定允许量急剧变化的情况下，维持焊接或涂镀作业中尤为必需的手腕前端的位置及手腕前端的移动速度和手腕轴的允许速度，并且抑制与目的相应的、作业坐标系中的姿势角度的至少一个特定成分的变动。
[0015]用于解决问题的方案
[0016]为了实现上述目的，本发明适用于控制多关节型机器人的多关节型机器人的控制装置，该多关节型机器人包括:第一关节驱动系统，在前端具有按照作业对象的作业线移动的作业部，包括与该作业部连接并使所述作业部的姿势发生变化的三个驱动轴；以及第二关节驱动系统，与所述第一关节驱动系统连接，包括使该第一关节驱动系统的位置发生变化的至少三个驱动轴，该多关节型机器人的控制装置的结构特征是包括下述的(11)?(16)。
[0017](11)插值数据计算单元，在所述多关节型机器人的基础坐标系中，计算表示对提示路径进行插值的多个插值点的所述作业部的位置及当时的姿势的数据，该提示路径连接预先提示的所述作业部的作业开始位置及当时的姿势和作业结束位置及当时的姿势。
[0018](12)角度计算单元，根据由所述插值数据计算单元计算的数据求出逆运动学问题的解，据此计算表示所述插值点的所述作业部的位置及当时的姿势的、包含所述第一关节驱动系统及所述第二关节驱动系统的所有驱动轴的角度。
[0019](13)速度计算单元，基于由所述角度计算单元计算的下一个所述插值点与当前的所述插值点的所述第一关节驱动系统的所述各驱动轴的角度的差，计算使所述作业部的姿势移动到下一个所述插值点时的所述第一关节驱动系统的两端的两个驱动轴的速度。
[0020](14)姿势数据变换单元，将由所述插值数据计算单元计算的数据变换为姿势数据，该姿势数据表示包含所述作业部的移动方向的轴、垂直于该轴的方向的轴、以及垂直于这两个轴的轴的作业坐标系中的所述作业部的姿势。
[0021](15)重新计算单元，在由所述速度计算单元计算的所述第一关节驱动系统的两端的两个驱动轴的一者或两者的速度在预先确定的允许范围外的情况下，不变更所述作业部的移动速度，并且对于由所述姿势数据变换单元变换的下一个所述插值点的所述作业部的姿势数据，抑制一个或两个特定成分的变动，同时重新计算所述第一关节驱动系统的两端的两个驱动轴的速度在所述允许范围内的所述第一关节驱动系统的各个驱动轴的角度，基于该计算的所述第一关节驱动系统的各个驱动轴的角度和由所述插值数据计算单元计算的所述作业部的位置，重新计算所述第二关节驱动系统的各个驱动轴的角度。
[0022](16)驱动指示单元，在由所述速度计算单元计算的所述第一关节驱动系统的两端的两个驱动轴的两者的速度在所述允许范围内的情况下，基于由所述角度计算单元计算的所述各个驱动轴的角度，驱动所述多关节型机器人，在由所述速度计算单元计算的所述第一关节驱动系统的两端的两个驱动轴的一者或两者的速度在所述允许范围外的情况下，基于由所述重新计算单元计算的所述各个驱动轴的角度，驱动所述多关节型机器人。
[0023]根据本发明，在需要使所述第一关节驱动系统的驱动轴超过所述允许范围急剧变化的情况下，能够维持焊接或涂镀作业中尤为必需的所述作业部的位置及手腕前端的移动速度和所述第一关节驱动系统的驱动轴的允许速度，并且抑制与目的相应的、作业坐标系中的姿势角度的至少一个特定成分的变动。另外，在计算从所述作业开始位置到所述作业结束位置之间存在的特异点(奇异点)区域或特异点避开路径等的情况下，需要设置能够应对事先计算时的过大负载的运算装置(CPU)，因而成本提高，但根据本发明，根据当前状态和下一状态逐步控制驱动即可，因此减少对运算装置瞬间要求的处理负载，能够实现低成本化。
[0024]例如，考虑所述重新计算单元包括:角度候选计算单元，基于当前的所述插值点的所述第一关节驱动系统的各个驱动轴的角度，计算多个该各个驱动轴的角度作为参照追随由所述角度计算单元计算的下一个所述插值点的角度，并且所述第一关节驱动系统的两端的两个驱动轴的下一个所述插值点的速度位于所述允许范围内的该各个驱动轴的角度候选；姿势数据计算单元，分别计算用所述作业坐标系表示采用由所述角度候选计算单元计算的各个角度候选时下一个所述插值点的所述作业部的姿势的姿势数据；以及角度选择单元，在由所述姿势数据计算单元计算的多个姿势数据中，选择对于由所述姿势数据变换单元变换的下一个所述插值点的所述作业部的姿势数据，所述特定成分的变动得到最大抑制的姿势数据，选择与该姿势数据对应的角度作为所述各个驱动轴的角度。
[0025]据此，在使所述作业部的位置?姿势变化时，能够抑制与所述作业部的作业目的相应的适当的特定成分的变动。
[0026]另外，在以±180°表示所述第一关节驱动系统的驱动轴的角度时，所述角度计算单元例如计算所述第一关节驱动系`统的位于中央的驱动轴的下一个所述插值点的角度与所述作业开始位置的该驱动轴的角度为相同符号的逆运动学问题的解。据此，控制使得所述第一关节驱动系统的位于中央的驱动轴的角度不经过0°，能够避开特异点。
[0027]另外，所述角度计算单元可以在由所述速度计算单元计算的所述第一关节驱动系统的两端的两个驱动轴的两者的速度在所述允许范围内的情况下，计算所述第一关节驱动系统的位于中央的驱动轴的下一个所述插值点的角度与所述作业开始位置的该驱动轴的角度为相同符号的逆运动学问题的解，在由所述速度计算单元计算的所述第一关节驱动系统的两端的两个驱动轴的一者或两者的速度在所述允许范围外的情况下，计算所述第一关节驱动系统的位于中央的驱动轴的下一个所述插值点的角度与所述作业开始位置的该驱动轴的角度为不同符号的逆运动学问题的解。
[0028]进而，还考虑对它们进行组合而得到的结构。即，较为理想的是，所述角度计算单元在所述作业开始位置以及所述作业结束位置的所述第一关节驱动系统的中央的驱动轴的角度为相同符号的情况下，计算所述第一关节驱动系统的位于中央的驱动轴的下一个所述插值点的角度与所述作业开始位置的该驱动轴的角度为相同符号的逆运动学问题的解，在所述作业开始位置以及所述作业结束位置的所述第一关节驱动系统的中央的驱动轴的角度为不同符号的情况下，在由所述速度计算单元计算的所述第一关节驱动系统的两端的两个驱动轴的两者的速度在所述允许范围内的情况下，计算所述第一关节驱动系统的位于中央的驱动轴的下一个所述插值点的角度与所述作业开始位置的该驱动轴的角度为相同符号的逆运动学问题的解，在由所述速度计算单元计算的所述第一关节驱动系统的两端的两个驱动轴的一者或两者的速度在所述允许范围外的情况下，计算所述第一关节驱动系统的位于中央的驱动轴的下一个所述插值点的角度与所述作业开始位置的该驱动轴的角度为不同符号的逆运动学问题的解。据此，能够根据提示到达所述作业结束位置的所述第一关节驱动系统的中央驱动轴的角度。
[0029]另外，考虑所述重新计算单元重新计算所述驱动轴的角度，使得由所述姿势数据变换单元变换后的所述作业部的姿势数据中，从预先确定的加权较大的所述特定成分起，变动的抑制量依次增大。据此，通过适当设定所述加权，能够适当调整所述特定成分的变动的抑制量。
[0030]作为本发明申请的更为具体的结构，考虑所述作业部为焊枪，所述姿势数据以作业坐标系表示，该作业坐标系中，以所述作业部的移动方向的轴为X轴，以由所述X轴与重力方向的外积表示的轴为Y轴，以由所述X轴与所述Y轴的外积表示的方向的轴为Z轴，以所述X轴的绕轴的转动角为焊枪倾斜角，以所述Y轴的绕轴的转动角为焊枪前进角，以所述Z轴的绕轴的转动角为焊枪转动角，所述特定成分为所述焊枪倾斜角、所述焊枪前进角、所述焊枪转动角中的任意一个或两个。如图25所示，对于所述手腕前端部具有的所述作业部的姿势，根据正交坐标系Σ base上表示的手腕前端位置XYZ与手腕前端姿势α β Y,在以所述作业部移动的方向的轴为X轴的焊接线坐标系Σ line(作业坐标系)上进行表示，由此，能够如图26?图28所示,作为相对于各轴的倾角成分来表示所述作业部的姿势。变换后的焊接线坐标系Σ line的绕坐标轴的转动角如图26、图27所示，绕Hine轴的转动角以右旋方向为正向作为所述焊枪倾斜角Rx，如图26、图28所示，绕Yline轴的转动角以右旋方向为正向作为所述焊枪前进角Ry，绕Zline轴的转动角以右旋方向为正向作为所述焊枪转动角Rz。
[0031]此时，在所述作业部是前端具有对所述作业对象产生作业上的效果的一个作业点的单头焊枪的情况下，认为所述特定成分是所述焊枪倾斜角和所述焊枪前进角。据此，能够抑制对于有一个作业点的单头焊枪的作业而言很重要的所述焊枪倾斜角和所述焊枪前进角的变动。
[0032]另一方面，在所述作业部是前端具有对所述作业对象产生作业上的效果的两个作业点的双头焊枪的情况下，认为所述特定成分是所述焊枪倾斜角和所述焊枪转动角。据此，能够抑制对于有两个作业点的双头焊枪的作业而言很重要的所述焊枪倾斜角和所述焊枪转动角的变动。
[0033]此外，考虑所述驱动指示单元在由所述速度允许判别单元判别为存在所述允许范围外的驱动轴之后，基于由所述重新计算单元计算的所述各个驱动轴的角度，驱动所述多关节型机器人，随后，以由所述角度计算单元计算的所述第一关节驱动系统的角度与由所述重新计算单元计算的所述第一关节驱动系统的角度的差达到指定值以下为条件，代替由所述重新计算单元计算的所述各个驱动轴的角度，基于由所述角度计算单元计算的所述各个驱动轴的角度，驱动所述多关节型机器人。[0034]此时，考虑所述驱动指示单元在从基于由所述重新计算单元计算的所述各个驱动轴的角度的所述多关节机器人的驱动控制切换为基于由所述角度计算单元计算的所述各个驱动轴的角度的所述多关节机器人的驱动控制时，使该各个驱动轴的角度在指定的变动范围内逐渐变动。据此，能够防止沿着所述作业对象的作业线移动的所述作业部发生振动从而不沿着作业线移动。例如，考虑为由所述重新计算单元计算的角度与根据所述多关节机器人的驱动控制由所述角度计算单元计算的角度的中间角度。
[0035]另外，所述速度计算单元可以代替速度而计算加速度，所述重新计算单元以及所述驱动指示单元可以代替速度，基于加速度进行判别。
[0036]此外，本发明可以理解为执行由所述多关节型机器人的控制装置执行的各步骤的多关节型机器人的控制方法的发明，或者可以理解为使计算机执行上述各步骤的多关节型机器人的控制程序的发明。
[0037]发明效果
[0038]根据本发明，在使手腕前端稍微移动，手腕轴就超过指定允许量急剧变化的情况下，也能够维持焊接或涂镀作业中尤为必需的手腕前端的位置及手腕前端的移动速度和手腕轴的允许速度，并且抑制与目的相应的、作业坐标系中的姿势角度的至少一个特定成分的变动。另外，在计算从所述作业开始位置到所述作业结束位置之间存在的特异点区域或特异点避开路径等的情况下，需要设置能够应对事先计算时的过大负载的运算装置(CPU)，因而成本提高，但根据本发明，根据当前状态和下一状态逐步控制驱动即可，因此减少对运算装置瞬间要求的处理负载，能够实现低成本化。
【专利附图】
【附图说明】
[0039]图1是表示由本发明实施方式的多关节型机器人的控制方法的一例控制的多关节型机器人X的概略结构的方框图。
[0040]图2是表示多关节型机器人的控制方法的控制整体处理的一例的流程图。
[0041]图3是表示插值点的各转动轴的速度.角度计算处理的一例的流程图。
[0042]图4是表示速度抑制必要性判别处理的一例的流程图。
[0043]图5是表不速度抑制处理的一例的流程图。
[0044]图6是表示从速度抑制处理恢复的处理的一例的流程图。
[0045]图7是表不插值点的路径的一例的图。
[0046]图8 (A)是表示原来路径与例外路径的J6轴的角度变化的一例的图，(B)是表示原来路径的插值点与例外路径的插值点的一例的图。
[0047]图9是表示避开特异点的路径的第一关节驱动轴的角度变化的一例的图表。
[0048]图10是表示J4轴的速度变化的一例的图。
[0049]图11是表示等速动作时焊接线坐标系的各姿势角的变动的一例的图。
[0050]图12是表示本发明的控制方法的焊接线坐标系的各姿势角的变动的一例的图。
[0051]图13是表示第二关节驱动轴的其他例的图。
[0052]图14是表示多关节型机器人的控制方法的控制整体处理的其他例的流程图。
[0053]图15是表示经由特异点的路径的第一关节驱动轴的角度变化的一例的图表。
[0054]图16是表示采用了本实施例1的控制方法的情况下的第一关节驱动轴的角度变化的一例的图表。
[0055]图17是表示等速动作时焊接线坐标系的各姿势角的变动的一例的图。
[0056]图18是表示采用了本实施例1的控制方法的情况下的焊接线坐标系的各姿势角的变动的一例的图。
[0057]图19是表示多关节型机器人的控制方法的控制整体处理的其他例的流程图。
[0058]图20是表不实施例3的具有两个作业点的末端执行器的一例的图。
[0059]图21是表示具有两个作业点的末端执行器的多关节型机器人的避开特异点的路径的焊接线坐标系的各姿势角的变动的一例的图。
[0060]图22是表示由实施例4的多关节型机器人的控制方法的一例控制的多关节型机器人X2的概略结构的方框图。
[0061]图23是表示普通的6轴机械手的概略结构的图。
[0062]图24是表示普通的6轴机械手经由特异点的路径的第一关节驱动轴的角度变化的一例的图表。
[0063]图25是表示将表示普通的6轴机械手的末端执行器的位置和姿势的坐标系变换为焊接线坐标系的情况下的一例的图。
[0064]图26是表示焊接线坐标系的各轴的倾角成分的一例的图。
[0065]图27是表示焊接线坐标系的焊枪倾斜角的一例的图。
[0066]图28是表示焊接线坐标系的焊枪前进角和焊枪转动角的一例的图。
[0067]图29是表示实施例5的计算机系统Y的概略结构的方框图。
[0068]图30是用于说明通知正在避开特异点这一情况的装置的图。
【具体实施方式】
[0069]下面参照【专利附图】
【附图说明】本发明的实施方式，以便理解本发明。另外，下面的实施方式是使本发明具体化的一例，不具有限定本发明的技术范围的性质。
[0070]首先，参照图1所示的概略结构图，说明本发明实施方式的多关节型机器人X的结构。
[0071]多关节型机器人X包括控制部10、操作部21、机械手(manipulator)主体30等。另外，除了图1所示的结构要素以外，多关节型机器人X还包括一般的多关节型机器人所包括的其他结构要素，这里省略记载。控制部10包括主控制部11、存储部12、以及驱动指示部13，是本发明的多关节型机器人的控制装置的一例。
[0072]机械手主体30包括:第二关节驱动系统，具有相当于人类的胳膊的Jl轴31、J2轴32、以及J3轴33 ;第一关节驱动系统，具有相当于人类的手腕的J4轴34、J5轴35、以及J6轴36 ;以及末端执行器39，对作业对象发挥效果，机械手主体30是按照所操作的内容进行驱动，与人类的上臂进行相同作业的机器人。另外，上述机械手主体30是多关节型机器人的一例。
[0073]Jl轴31?J6轴36由电动机等构成，根据来自驱动指示部13的指示在正负方向上旋转驱动，多个轴的旋转驱动相互关联，由此实现人类的手腕或胳膊的动作。尤其是，上述第二关节驱动系统与上述第一关节驱动系统的驱动轴的动作无关，可以自由地确定上述多关节型机器人的基础坐标系Σ base中的上述末端执行器39的位置(X，Y，Z)。[0074]末端执行器39是安装于机械手主体30前端(J6轴36的前端)的效果器，是利用一个作业点进行作业的焊接器(焊枪)、涂镀装置、工具、捕获器、传感器等。另外，上述末端执行器39是作业部的一例。
[0075]另外，具有一个作业点的焊枪称为单头焊枪。
[0076]操作部21由用户操作的薄片按键或操作按钮、操作杆等构成，是接收用户的操作输入的输入接口。例如，操作部21接收上述末端执行器39的作业开始位置及姿势、作业结束位置及姿势、以及对其进行连接的路径的作业路径或作业时间、用于抑制与目的相应的、焊接线坐标系Σ line (作业坐标系)中末端执行器39的姿势角度的各成分的变动的加权信息、以及记载了这些内容的程序等输入，并输出到控制部10。即，操作部21是用于提示末端执行器39的作业开始位置及姿势、作业结束位置及姿势的单元，是用于设定上述加权信息的单元。
[0077]存储部12例如是由硬盘或DRAM等构成的易失性存储器，存储末端执行器39的作业开始位置及姿势、作业结束位置及姿势、以及对其进行连接的路径的作业路径或作业时间，用于抑制末端执行器39的姿势角度的各成分的变动的加权信息，由后述的主控制部11计算的插值点(经由特异点的路径和避开特异点的路径的双方)的上述Jl轴31?J6轴36的角度和速度(根据情况不同有时还有加速度)等。
[0078]驱动指示部13按照来自主控制部11的控制指示，在每个采样周期中，读出上述Jl轴31?J6轴36的角度或速度的信息以使末端执行器39移动到存储部12中存储的下一个插值点(经由特异点的路径或避开特异点的路径中的一者)，并将上述信息输出到机械手主体30。另外，主控制部11和驱动指示部13是用于执行驱动指示步骤的驱动指示单元的一例。
[0079]主控制部11包括C.PU、R0M等，CPU是进行该多关节型机器人X的各种控制及运算的运算单元，ROM是存储由CPU执行的控制程序或运算程序、CPU执行这些程序时参照的数据等的存储器。
[0080]接着，参照图2?图6的流程图，说明以多关节型机器人X的主控制部11为中心，使机械手主体30动作的处理过程的一例。另外，以下所示的S11、S12、……表示处理过程(步骤)的识别标号。
[0081]首先，参照图2，说明一般的机械手主体30的控制与本发明的一实施方式的多关节型机器人X的控制的处理过程的差异。
[0082]一般的机械手主体30的控制实施如后所示对主控制部11提示动作轨迹的提示步骤(步骤Sll)，还实施为了以所提示的动作轨迹上的插值点的位置.姿势动作而计算并存储各驱动轴的角度的步骤(步骤S12)，读出所计算的角度并指示动作的步骤(步骤S14)，以及判别是否已到达作业结束位置?姿势的步骤(步骤S15)，以使机械手主体30从当前插值点的位置?姿势向下一插值点的位置?姿势运动，随后，在指定的每个采样期间中反复执行步骤S12、S14、S15，据此使机械手主体30沿着动作轨迹从作业开始位置.姿势运动到作业结束位置?姿势。另外，主控制部11使机械手主体30的末端执行器39以预先设定的一定速度移动，据此执行该末端执行器39的作业。但是，在以一定速度移动的情况下，当第一关节驱动系统的J5轴35经过0°附近时(特异点)，J4轴34、J6轴36的速度有可能超过预先确定的允许范围。[0083]对此，本实施方式的多关节型机器人X的控制除了上述一般的机械手主体30的控制所需的步骤以外，如后所述，还包含:判别是否由于第一关节驱动系统的J4轴34、J6轴36的至少一者的速度超过允许范围而需要执行速度抑制处理的步骤(步骤S13)，以及在需要执行速度抑制处理的情况下(步骤S13:是)，为了移动至避开特异点的动作轨迹上的插值点而重新计算并存储各驱动轴的角度的速度抑制步骤(步骤S20)，避开特异点，使机械手主体30从当前插值点的位置.姿势移动至重新计算的下一个插值点的位置.姿势。
[0084]接着，参照图2说明由主控制部11执行的多关节型机器人X的控制处理整体，各处理的细节在后面描述。
[0085]最初，利用者利用操作部21点到点地提示机器人的动作轨迹。即，通过操作部21移动到各提示点，将上述末端执行器39的作业位置以及姿势存储到控制部10的存储部12 (步骤SI I)。另外，步骤Sll是提示步骤的一例。
[0086]主控制部11在上述提示的点之间按照指定的每个采样周期(例如，1/16秒、1/32秒等)进行分割.插值，计算用于使末端执行器39移动到下一个插值点的Jl轴31?J6轴36的角度和第一关节驱动系统的J4轴34?J6轴36的速度(根据情况不同有时还计算加速度)，并存储到存储部12 (步骤S12)。这里，Jl轴31?J6轴36的角度用土 180°表示。当然，角度的指定方法并不限于±180°，例如还可以是0°?360°等。另外，该步骤S12的细节在后面进行描述。
[0087]然后，主控制部11判别上述步骤S12计算的用于使末端执行器39移动到下一插值点的第一关节驱动系统的J4轴34及J6轴36的一者或两者的速度或加速度位于允许范围内还是允许范围外(以后称为“需要.不需要速度抑制处理”)，或者是否处于已判别为存在允许范围外的驱动轴之后的避开处理中(以后称为“处于速度抑制处理中”)(步骤S13)。另外，该步骤S13的细节在后面进行描述。
[0088]这里，在判别为需要速度抑制处理或者处于速度抑制处理中时(步骤S13:是)，主控制部11进行速度抑制处理(步骤S20)，该处理不变更末端执行器39的移动速度，并且对上述步骤S12计算的下一个插值点的末端执行器39的姿势数据，抑制一个或两个特定成分的变动，重新计算该J4轴34及J6轴36的一者或两者的角度以使速度或加速度处于上述允许范围内，重新计算与此对应的其他轴的角度，并存储到存储部12。另外，在该步骤S20的上述说明中，作为判别“需要.不需要速度抑制处理”的标准，以各轴的速度或加速度为问题，但在该步骤S20的具体例的后述说明中，举出对速度进行判别的情况为例进行说明。
[0089]另一方面，在判别为不需要速度抑制处理，不处于速度抑制处理中(步骤S13:否)，或者进行了速度抑制处理(步骤S20)后，基于主控制部11的指示，驱动指示部13从存储部12中读出用于使末端执行器39移动到下一个插值点的Jl轴31?J6轴36的角度，并输出到机械手主体30的各驱动轴的致动器(步骤S14)。此时，在判别为不需要速度抑制处理，不处于速度抑制处理中的情况下，基于上述步骤S12计算的用于移动到下一插值点的Jl轴31?J6轴36的角度，控制机械手主体30，在J4轴34、J6轴36的速度位于允许范围外并执行了速度抑制处理的情况下，基于上述步骤S20计算的用于移动到下一插值点的Jl轴31?J6轴36的角度，控制机械手主体30。这里，相关步骤S14是驱动指示步骤的一例，执行相关处理时的主控制部11及驱动支持部13相当于驱动指示单元。
[0090]接着，主控制部11判别移动后的末端执行器39的位置及姿势是否已到达作业部结束位置及姿势(步骤S15)。在未到达作业部结束位置及姿势的情况下(步骤S15:否)，在每个指定的采样周期中，重新实施上述步骤S12之后的步骤，在到达作业部结束位置及姿势的情况下(步骤S15:是)，结束处理。
[0091]这样，主控制部11在每个指定的采样周期中，计算用于使末端执行器39从当前位置及姿势移动到下一个插值点的Jl轴31?J6轴36的角度，并使多关节型机器人X动作。
[0092]接着，参照图3，说明对应于图2的步骤S12的、计算用于从多关节型机器人X的当前插值点移动到下一个插值点的Jl轴31?J6轴36的角度与第一关节驱动系统的J4轴34?J6轴36的速度的处理过程的一例。
[0093]首先，主控制部11计算在上述步骤Sll中预先提示的作业开始位置及姿势PO与作业结束位置及姿势Pn之间用η(基于每个采样周期的任意值)个点进行插值而得到的末端执行器39的位置及姿势。在此情况下，每一个采样周期的移动量可用下式表示:Δ P= (Pn-P0) / η= { Δ X= (Xn-X0) / η, Δ Y= (Yn-Y0) / η, Δ Z= (Zn-Z0) / η, Δ α = ( α η-α 0) /η, Δ β=(βη-β0) / η, Δ y = (yn-y0) / η}。此外，上述各个插值点由上述多关节型机器人X的基础坐标系Σ base表不。
[0094]如图7所示，主控制部11在当前位置的插值点为Pi的情况下，通过Pw=P0+ΔPX (1+1)、？1+1=?1+八？计算下一个插值点？1+1的位置及姿势(步骤S121)。另外，该步骤S121是插值数据计算步骤的一例，执行相关处理时的主控制部11相当于插值数据
计算单元。
[0095]接着，主控制部11根据下一个插值点Pi+1的位置及姿势(Xi+1，Yi+1, Zi+1，a i+1，β i+1，i+1)求出逆运动学问题的解，由此计算各Jl轴31?J6轴36的角度(θ1； Θ 2，Θ 3，Θ 4，Θ 5，Θ 6)，并存储到存储部12 (步骤S122)。此时，关于J4轴34?J6轴36得到逆运动学问题的两个解 ，这里，主控制部11采用这两个解中第一关节驱动系统的J5轴35的角度与提示的上述末端执行器39的作业开始位置的J5轴35的角度符号相同的逆运动学问题的解。据此，J5轴35的角度不经过0°而进行推移，因此能够避免经过特异点。
[0096]另外，主控制部11在执行后述的速度抑制处理的情况下，不仅计算经由特异点的路径的插值点的各Jl轴31?J6轴36的角度，还分别计算避开特异点的路径的插值点的各Jl轴31?J6轴36的角度并存储到存储部12，作为进行后述的恢复处理(图5的步骤S310以及图6的流程图)时的比较对象。
[0097]这里，步骤S122是角度计算步骤的一例，执行相关处理时的主控制部11相当于角度计算单元。
[0098]接着,主控制部11从计算的下一插值点Pi+1的各J4轴34?J6轴36的角度中，减去存储部12中存储的当前插值点Pi的各J4轴34?J6轴36的角度，计算第一关节驱动系统的J4轴34?J6轴36的速度并存储到存储部12(步骤S123)。另外，如后所述，在执行速度抑制处理(步骤S20)的情况下，计算经由特异点的路径的插值点的第一关节驱动系统的J4轴34?J6轴36的速度并存储到存储部12，用于在后述的步骤S131中计算速度的允许范围，或者在步骤S211等中计算下一插值点的候选速度(根据需要还计算加速度)。
[0099]这里，步骤S123是速度计算步骤的一例，执行相关处理时的主控制部11相当于速度计算单元。
[0100]接着，参照图4说明相当于图12的步骤S13的速度抑制必要性判别处理过程的一例。
[0101]首先，主控制部11计算使末端执行器39的位置及姿势从当前插值点Pi移动到下一插值点Pi+Ι时的第一关节驱动系统的J4轴34、J6轴36的速度的允许范围(步骤S131)。例如，将当前插值点Pi的速度值加减指定值或指定比例得到的值作为上限.下限，计算速度的允许范围。另外，该各轴的速度的允许范围也可以根据末端执行器39的动作条件(例如焊接时的温度、涂镀时的涂镀膜的厚度等条件)、多关节型机器人动作时不引起振动等异常动作的条件等，在对作业对象的质量不产生影响的范围内预先设定。此外，也可以对J4轴34、J6轴36分别设定各自的允许范围。
[0102]接着，主控制部11判别第一关节驱动系统的下一个插值点的J4轴34以及J6轴36的一者或两者的速度是否超过上述允许范围，或者是否设立了表示处于速度抑制处理中的标志(步骤S132)。
[0103]这里，在第一关节驱动系统的下一个插值点的J4轴34以及J6轴36的两者位于上述允许范围内，并且不处于速度抑制处理中的情况下(步骤S132:否)，基于主控制部11的指示，驱动指示部13从存储部12读出上述步骤S12计算的各Jl轴31?J6轴36的角度并输出到机械手主体30 (步骤S14)。另一方面，在存在允许范围外的轴，或者处于速度抑制处理中的情况下(步骤S132:是)，主控制部11执行以下所述的速度抑制处理(步骤S20)。
[0104]下面，参照图5说明相当于图2的步骤S20的该多关节型机器人X的速度抑制处理的具体例。
[0105]另外，下面作为例子举出图24所示的J5轴35处于O度附近，J4轴34、J6轴36的速度超过允许范围时执行该速度抑制处理的情况进行说明，由于其他理由，J4轴34、J6轴36的速度超过允许范围的情况也同样。因此，该速度抑制处理在第一关节驱动系统的J4轴34、J6轴36中任意一个或者两个驱动轴的速度超过允许范围时执行。另外，最初发现下一个插值点有允许范围外的轴时，当前插值点的速度处于允许范围内。
[0106]首先，主控制部11判别关于下一个插值点，速度处于上述允许范围外的驱动轴是J4轴34及J6轴36中的哪一者或两者，或者在“处于速度抑制处理中”的标志已经设立的情况下，该速度抑制处理是对J4轴34及J6轴36中的哪一者或两者执行的，并对处理进行分支(步骤S201)。如前所述，通过速度是否位于允许范围的上限.下限间，来判别是在允许范围内还是允许范围外。这里，若J4轴34及J6轴36的两者都不在速度的允许范围外，并且“处于速度抑制处理中”的标志并未设立(步骤S201:其他)，则主控制部11结束该速度抑制处理。
[0107]另一方面，在J6轴36的速度位于允许范围外的情况下处理转至步骤S211，在J4轴34的速度位于允许范围外的情况下处理转至步骤S221，在J4轴34及J6轴36的两者的速度位于允许范围外的情况下处理转至步骤S231。
[0108](J6轴36的速度抑制处理步骤S211?S217)
[0109]在J6轴36的速度位于允许范围外，或者设立了表示处于J6轴36的速度抑制处理中的标志的情况下(步骤S201:J6轴为范围外)，主控制部11基于当前插值点的J6轴36的角度，计算多个该J6轴36的角度追随上述步骤S12计算的下一个插值点的角度，并且J6轴36的下一个插值点的速度位于上述允许范围内的角度候选(步骤S211)。[0110]例如，主控制部11利用下面的步骤(21)?(23)，计算J6轴36的速度候选。
[0111](21)首先，利用下式(21a)计算抑制基准速度V6b。这里，是下一个插值点的J6轴36的角度，Θ old是当前插值点的J6轴36的角度，E是预先设定的系数(例如0.1)。
[0112]V6b=E(9now-0old) (21a)
[0113](22)接着，判断抑制基准速度V6b是否在预先设定的速度限值Vmax以上，在判断为该速度限值Vmax以上的情况下，将抑制基准速度V6b设定为上次的速度Θ。1(1。上述速度限值Vmax例如是上述允许范围内的速度的最大值。
[0114](23)然后，根据抑制基准速度V6b和指定值D (已知保证事先确定的允许范围内的加速度的速度变化绝对值等)，计算三个速度候选(V Θ 6a=V6b-D, V Θ 6b=V6b, V Θ 6c=V6b+D)。上述指定值D例如是上述速度限值Vmax的10%左右的值。另外，在V 06a大于上述速度限值Vmax的情况下，令ve6a=vmax。
[0115]随后，主控制部11根据当前插值点的J6轴36的角度Θ 6，计算与上述速度的各候选(V θ 6a，V θ 6b> V θ 6c)对应的下一插值点的三个J6轴36的角度候选(θ 6a, 0 6b, 0 6c)。即，作为下一插值点，至少计算继续保持了不超过允许范围的抑制基准速度V6b或当前插值点的速度的角度(当然在允许范围内)、以及与其仅相差微小量(土D(与速度变化绝对值对应))的角度。这里，相关步骤S211的处理是角度候选计算步骤的一例，执行相关处理时的主控制部11相当于角度候选计算单元。
[0116]接着，主控制部11计算与所计算的下一插值点的J6轴36的各角度候选对应的其他驱动轴Jl轴31?J5轴35的角度(步骤S212)。例如，如以下的(I)?(3)所示，对于J6轴36的每个角度候选(Θ 6a，Θ 6b, Θ 6c)，使用上述步骤S122中计算的下一插值点Pi+1的末端执行器39的位置(Xi+1，Yi+1，Zi+1)和第一关节驱动系统的J4轴34、J5轴35的角度(Θ 4，Θ 5)，重新计算第一关节驱动系统及第二关节驱动系统的Jl轴31?J6轴36的角度
(0 1， θ2， θ3， θ4， θ5， θ6)。
[0117](I)对于候选 Θ 6a，根据[xi+1，Yi+1，zi+1, Θ 4，θ 5，θ 6J 计算[θ la，Θ 2a, Θ 3a, Θ 4，
θ 5，θ 6J。
[0118]⑵对于候选Θ 6b，根据[Xi+1，Yi+1，Zi+1, Θ 4，θ 5，θ 6b]计算[θ lb，Θ 2b, Θ 3b, Θ 4，Θ 5，Θ 6b]。
[0119]⑶对于候选Θ 6c，根据[Xi+1，Yi+1，Zi+1, Θ 4，θ 5，θ 6J 计算[θ lc，Θ 2c, Θ 3c, Θ 4，
θ 5，θ 6J。
[0120]由此，如后面说明的图8所示，计算速度不超过允许范围的下一插值点的候选P’ i+1，将该下一插值点的候选P’ i+1视为下一插值点，据此能够得到J6轴36的速度不超过允许范围，并且避开的原来的插值点Pi+1 (特异点)的例外路径(参照图8 (B))。
[0121]以此方式得到将J6轴36的速度维持在允许范围内的下一插值点的三个角度候选(06a, 06b, θ6。)后，进行应选择哪个候选作为下一插值点的判断。在迄今为止的计算中，将第一关节驱动系统的J4轴34?J6轴36的角度(Θ 4，Θ 5，Θ 6)作为问题，但最终必须结合根据作业种类认为重要的末端执行器39的姿势，来选择候选。因此，需要从上述Jl轴31?J6轴36的角度变换为表示末端执行器39的姿势的数据。
[0122]并且，为了进行应选择哪个候选作为下一插值点的判断，主控制部11对于计算的每个候选，分别计算表示下一插值点的焊接线坐标系Σ line的末端执行器39的姿势的数据的各成分(焊枪倾斜角Rx，焊枪前进角Ry，焊枪转动角Rz)(步骤S213)。另外，该步骤S123是姿势数据计算步骤的一例，执行相关处理时的主控制部11相当于姿势数据计算单元。这里，如图25所示，上述焊接线坐标系Σ line是以末端执行器39移动的方向(行进方向)的轴为X轴，以由该X轴与重力方向的外积(X轴X重力方向)表示的轴为Y轴，以由该X轴与该Y轴的外积(X轴XY轴)表示的方向的轴为Z轴的作业坐标系，如图26?图28所示,该X轴的绕轴的转动角由焊枪倾斜角Rx表示，该Y轴的绕轴的转动角由焊枪前进角Ry表示，该Z轴的绕轴的转动角由焊枪转动角Rz表示。
[0123]例如，对于上述⑴?(3)的候选06a?θ6。，利用以下的(I’)?(3’)计算表示末端执行器39的姿势的数据的各成分。
[0124](Γ )根据[Θ la, Θ 2a，Θ 3a，Θ 4，Θ 5，Θ J 计算[Rxa，Rya, RJ。
[0125](2，)根据[Θ lb，Θ 2b，θ 3b，θ 4，θ 5，θ J 计算[Rxb，Ryb, RJ。
[0126](3，)根据[Θ lc, Θ 2c, Θ 3c, Θ 4，Θ 5，Θ J 计算[Rxc, Ryc, RJ。
[0127]接着，主控制部11对于上述步骤S121中计算的原来路径上的下一插值点Pi+Ι的末端执行器39的位置及姿势，为了由以末端执行器39移动的方向的轴为X轴的焊接线坐标系Σ line (焊枪倾斜角Rx,焊枪前进角Ry，焊枪转动角Rz)表示由正交坐标系Σ base表示的(Xi+1，Yi+1, Zi+1，a i+1，β i+1，y i+1)，进行数据变换(步骤S214)。另外，该步骤S214是姿势数据变换步骤的一例，执行相关处理时的主控制部11相当于姿势数据变换单元。
[0128]另外，下面的(数式I)中表示从正交坐标系Σ base变换为焊接线坐标系Σ line的式子。
[0129][数式I]
【权利要求】
1.一种多关节型机器人的控制装置，其特征在于，对多关节型机器人进行控制，所述多关节型机器人包括:第一关节驱动系统，在前端具有按照作业对象的作业线移动的作业部，并且包括与该作业部连接并使所述作业部的姿势发生变化的三个驱动轴；以及第二关节驱动系统，与所述第一关节驱动系统连接，包括使该第一关节驱动系统的位置发生变化的至少三个驱动轴， 所述多关节型机器人的控制装置包括: 插值数据计算单元，在所述多关节型机器人的基础坐标系中，计算表示对提示路径进行插值的多个插值点的所述作业部的位置及当时的姿势的数据，该提示路径连接预先提示的所述作业部的作业开始位置及当时的姿势和作业结束位置及当时的姿势； 角度计算单元，根据由所述插值数据计算单元计算出的数据求出逆运动学问题的解，据此计算表示所述插值点的所述作业部的位置及当时的姿势的、包含所述第一关节驱动系统及所述第二关节驱动系统的所有驱动轴的角度； 速度计算 单元，基于由所述角度计算单元计算出的下一个所述插值点与当前的所述插值点的所述第一关节驱动系统的所述各驱动轴的角度的差，计算使所述作业部的姿势移动到下一个所述插值点时的所述第一关节驱动系统的两端的两个驱动轴的速度； 姿势数据变换单元，将由所述插值数据计算单元计算出的数据变换为姿势数据，该姿势数据表示包含所述作业部的移动方向的轴、垂直于该轴的方向的轴、以及垂直于这两个轴的轴的作业坐标系 中的所述作业部的姿势； 重新计算单元，在由所述速度计算单元计算出的所述第一关节驱动系统的两端的两个驱动轴的一者或两者的速度在预先确定的允许范围外的情况下，不变更所述作业部的移动速度，并且对于由所述姿势数据变换单元变换的下一个所述插值点的所述作业部的姿势数据，抑制一个或两个特定成分的变动，同时重新计算所述第一关节驱动系统的两端的两个驱动轴的速度成为所述允许范围内的所述第一关节驱动系统的各个驱动轴的角度，基于该计算出的所述第一关节驱动系统的各个驱动轴的角度和由所述插值数据计算单元计算出的所述作业部的位置，重新计算所述第二关节驱动系统的各个驱动轴的角度；以及 驱动指示单元，在由所述速度计算单元计算出的所述第一关节驱动系统的两端的两个驱动轴的两者的速度在所述允许范围内的情况下，基于由所述角度计算单元计算出的所述各个驱动轴的角度，驱动所述多关节型机器人，在由所述速度计算单元计算出的所述第一关节驱动系统的两端的两个驱动轴的一者或两者的速度在所述允许范围外的情况下，基于由所述重新计算单元计算出的所述各个驱动轴的角度，驱动所述多关节型机器人。
2.根据权利要求1所述的多关节型机器人的控制装置，其特征在于: 所述重新计算单元包括: 角度候选计算单元，基于当前的所述插值点的所述第一关节驱动系统的各个驱动轴的角度，计算多个该各个驱动轴的角度将由所述角度计算单元计算出的下一个所述插值点的角度作为参照进行追随，并且所述第一关节驱动系统的两端的两个驱动轴的下一个所述插值点的速度成为所述允许范围内的该各个驱动轴的角度的候选； 姿势数据计算单元，分别计算用所述作业坐标系表示采用由所述角度候选计算单元计算出的各个角度候选时下一个所述插值点的所述作业部的姿势的姿势数据；以及 角度选择单元，在由所述姿势数据计算单元计算出的多个姿势数据中，选择对于由所述姿势数据变换单元变换的下一个所述插值点的所述作业部的姿势数据，所述特定成分的变动得到最大抑制的姿势数据，选择与该姿势数据对应的角度作为所述各个驱动轴的角度。
3.根据权利要求2所述的多关节型机器人的控制装置，其特征在于: 在以±180°表示所述第一关节驱动系统的驱动轴的角度时， 所述角度计算单元计算所述第一关节驱动系统的位于中央的驱动轴的下一个所述插值点的角度与所述作业开始位置的该驱动轴的角度成为相同符号的逆运动学问题的解。
4.根据权利要求2所述的多关节型机器人的控制装置，其特征在于: 在以±180°表示所述第一关节驱动系统的驱动轴的角度时， 所述角度计算单元在由所述速度计算单元计算出的所述第一关节驱动系统的两端的两个驱动轴的两者的速度在所述允许范围内的情况下，计算所述第一关节驱动系统的位于中央的驱动轴的下一个所述插值点的角度与所述作业开始位置的该驱动轴的角度成为相同符号的逆运动学问题的解， 在由所述速度计算单元计算出的所述第一关节驱动系统的两端的两个驱动轴的一者或两者的速度在所述允许范围外的情况下，计算所述第一关节驱动系统的位于中央的驱动轴的下一个所述插值点的角度与所述作业开始位置的该驱动轴的角度成为不同符号的逆运动学问题的解。
5.根据权利要求2所述的多关节型机器人的控制装置，其特征在于: 在以±180°表示所述第 一关节驱动系统的驱动轴的角度时， 所述角度计算单元， 在所述作业开始位置以及所述作业结束位置的所述第一关节驱动系统的中央的驱动轴的角度为相同符号的情况下，计算所述第一关节驱动系统的位于中央的驱动轴的下一个所述插值点的角度与所述作业开始位置的该驱动轴的角度成为相同符号的逆运动学问题的解， 在所述作业开始位置以及所述作业结束位置的所述第一关节驱动系统的中央的驱动轴的角度为不同符号的情况下，在由所述速度计算单元计算出的所述第一关节驱动系统的两端的两个驱动轴的两者的速度在所述允许范围内的情况下，计算所述第一关节驱动系统的位于中央的驱动轴的下一个所述插值点的角度与所述作业开始位置的该驱动轴的角度成为相同符号的逆运动学问题的解，在由所述速度计算单元计算出的所述第一关节驱动系统的两端的两个驱动轴的一者或两者的速度在所述允许范围外的情况下，计算所述第一关节驱动系统的位于中央的驱动轴的下一个所述插值点的角度与所述作业开始位置的该驱动轴的角度成为不同符号的逆运动学问题的解。
6.根据权利要求1至5中任一项所述的多关节型机器人的控制装置，其特征在于: 所述重新计算单元重新计算所述驱动轴的角度，使得由所述姿势数据变换单元变换后的所述作业部的姿势数据中，从预先确定的加权较大的所述特定成分起，变动的抑制量依次增大。
7.根据权利要求1至6中任一项所述的多关节型机器人的控制装置，其特征在于: 所述作业部为焊枪，利用以所述作业部的移动方向的轴为X轴，以由所述X轴与重力方向的外积表示的轴为Y轴，以由所述X轴与所述Y轴的外积表示的方向的轴为Z轴，以所述X轴的绕轴的转动角为焊枪倾斜角，以所述Y轴的绕轴的转动角为焊枪前进角，以所述Z轴的绕轴的转动角为焊枪转动角的作业坐标系表示所述姿势数据， 所述特定成分是所述焊枪倾斜角、所述焊枪前进角、所述焊枪转动角中的任意一个或两个。
8.根据权利要求7所述的多关节型机器人的控制装置，其特征在于: 所述作业部是在前端具有对所述作业对象产生作业上的效果的一个作业点的单头焊枪， 所述特定成分是所述焊枪倾斜角以及所述焊枪前进角。
9.根据权利要求7所述的多关节型机器人的控制装置，其特征在于: 所述作业部是在前端具有对所述作业对象产生作业上的效果的两个作业点的双头焊枪， 所述特定成分是所述焊枪倾斜角以及所述焊枪转动角。
10.根据权利要求1至9中任一项所述的多关节型机器人的控制装置，其特征在于: 所述驱动指示单元在由所述速度允许判别单元判别为存在所述允许范围外的驱动轴之后，基于由所述重新计算单元计算出的所述各个驱动轴的角度，驱动所述多关节型机器人，随后，以由所述角度计算单元计算出的所述第一关节驱动系统的角度与由所述重新计算单元计算出的所述第一关节驱动系统的角度的差成为指定值以下为条件，代替由所述重新计算单元计算出的所述各 个驱动轴的角度，基于由所述角度计算单元计算出的所述各个驱动轴的角度，驱动所述多关节型机器人。
11.根据权利要求10所述的多关节型机器人的控制装置，其特征在于: 所述驱动指示单元在从基于由所述重新计算单元计算出的所述各个驱动轴的角度的所述多关节机器人的驱动控制切换为基于由所述角度计算单元计算出的所述各个驱动轴的角度的所述多关节机器人的驱动控制时，使该各个驱动轴的角度在指定的变动范围内逐渐变动。
12.根据权利要求1至11中任一项所述的多关节型机器人的控制装置，其特征在于: 所述速度计算单元计算加速度来代替速度，所述重新计算单元以及所述驱动指示单元基于加速度来代替速度进行判别。
13.一种多关节型机器人的控制方法，其特征在于: 该多关节型机器人包括:第一关节驱动系统，在前端具有按照作业对象的作业线移动的作业部，包括与该作业部连接并使所述作业部的姿势发生变化的三个驱动轴；以及第二关节驱动系统，与所述第一关节驱动系统连接，包括使该第一关节驱动系统的位置发生变化的至少三个驱动轴， 该多关节型机器人的控制方法包括: 插值数据计算步骤，在所述多关节型机器人的基础坐标系中，计算表示对提示路径进行插值的多个插值点的所述作业部的位置及当时的姿势的数据，该提示路径连接预先提示的所述作业部的作业开始位置及当时的姿势和作业结束位置及当时的姿势； 角度计算步骤，根据由所述插值数据计算步骤计算出的数据求出逆运动学问题的解，据此计算表示所述插值点的所述作业部的位置及当时的姿势的、包含所述第一关节驱动系统及所述第二关节驱动系统的所有驱动轴的角度； 速度计算步骤，基于由所述角度计算步骤计算出的下一个所述插值点与当前的所述插值点的所述第一关节驱动系统的所述各驱动轴的角度的差，计算使所述作业部的姿势移动到下一个所述插值点时的所述第一关节驱动系统的两端的两个驱动轴的速度； 姿势数据变换步骤，将由所述插值数据计算步骤计算出的数据变换为姿势数据，该姿势数据表示包含所述作业部的移动方向的轴、垂直于该轴的方向的轴、以及垂直于这两个轴的轴的作业坐标系中的所述作业部的姿势； 重新计算步骤，在由所述速度计算步骤计算出的所述第一关节驱动系统的两端的两个驱动轴的一者或两者的速度在预先确定的允许范围外的情况下，不变更所述作业部的移动速度，并且对于由所述姿势数据变换步骤变换的下一个所述插值点的所述作业部的姿势数据，抑制一个或两个特定成分的变动，同时重新计算所述第一关节驱动系统的两端的两个驱动轴的速度成为所述允许范围内的所述第一关节驱动系统的各个驱动轴的角度，基于该计算出的所述第一关节驱动系统的各个驱动轴的角度和由所述插值数据计算步骤计算出的所述作业部的位置，重新计算所述第二关节驱动系统的各个驱动轴的角度；以及 驱动指示步骤，在由所述速度计算步骤计算出的所述第一关节驱动系统的两端的两个驱动轴的两者的速度在所述允许范围内的情况下，基于由所述角度计算步骤计算出的所述各个驱动轴的角度，驱动所述多关节型机器人，在由所述速度计算步骤计算出的所述第一关节驱动系统的两端的两个驱动轴的一者或两者的速度在所述允许范围外的情况下，基于由所述重新计算步骤计算出的所述各个驱动轴的角度，驱动所述多关节型机器人。
14.一种多关节型机器人的控制程序，其特征在于: 该控制程序由控制所述多关节型机器人的计算机执行，所述多关节型机器人包括:第一关节驱动系统，在前端具有按照作业对象的作业线移动的作业部，包括与该作业部连接并使所述作业部的姿势发生变化的三个驱动轴；以及第二关节驱动系统，与所述第一关节驱动系统连接，包括使该第一关节`驱动系统的位置发生变化的至少三个驱动轴， 该多关节型机器人的控制程序使计算机执行如下步骤: 插值数据计算步骤，在所述多关节型机器人的基础坐标系中，计算表示对提示路径进行插值的多个插值点的所述作业部的位置及当时的姿势的数据，该提示路径连接预先提示的所述作业部的作业开始位置及当时的姿势和作业结束位置及当时的姿势； 角度计算步骤，根据由所述插值数据计算步骤计算出的数据求出逆运动学问题的解，据此计算表示所述插值点的所述作业部的位置及当时的姿势的、包含所述第一关节驱动系统及所述第二关节驱动系统的所有驱动轴的角度； 速度计算步骤，基于由所述角度计算步骤计算出的下一个所述插值点与当前的所述插值点的所述第一关节驱动系统的所述各驱动轴的角度的差，计算使所述作业部的姿势移动到下一个所述插值点时的所述第一关节驱动系统的两端的两个驱动轴的速度； 姿势数据变换步骤，将由所述插值数据计算步骤计算出的数据变换为姿势数据，该姿势数据表示包含所述作业部的移动方向的轴、垂直于该轴的方向的轴、以及垂直于这两个轴的轴的作业坐标系中的所述作业部的姿势； 重新计算步骤，在由所述速度计算步骤计算出的所述第一关节驱动系统的两端的两个驱动轴的一者或两者的速度在预先确定的允许范围外的情况下，不变更所述作业部的移动速度，并且对于由所述姿势数据变换步骤变换的下一个所述插值点的所述作业部的姿势数据，抑制一个或两个特定成分的变动，同时重新计算所述第一关节驱动系统的两端的两个驱动轴的速度成为所述允许范围内的所述第一关节驱动系统的各个驱动轴的角度，基于该计算出的所述第一关节驱动系统的各个驱动轴的角度和由所述插值数据计算步骤计算出的所述作业部的位置，重新计算所述第二关节驱动系统的各个驱动轴的角度；以及 驱动指示步骤，在由所述速度计算步骤计算出的所述第一关节驱动系统的两端的两个驱动轴的两者的速度在所述允许范围内的情况下，基于由所述角度计算步骤计算出的所述各个驱动轴的角度，驱动所述多关节型机器人，在由所述速度计算步骤计算出的所述第一关节驱动系统的两端的两个驱动轴的一者或两者的速度在所述允许范围外的情况下，基于由所述重新计算步骤计算 出的所述各个驱动轴的角度，驱动所述多关节型机器人。
【文档编号】B23K9/12GK103429398SQ201280012110
【公开日】2013年12月4日 申请日期:2012年3月2日 优先权日:2011年3月8日
【发明者】大根努, 西村利彦 申请人:株式会社神户制钢所
【专利摘要】在速度抑制处理中，维持手腕前端的位置及手腕前端的移动速度与轴的允许速度，并且抑制目的坐标系中的姿势角度的特定成分的变动。在计算提示路径上的插值点的位置、姿势、以及各轴的角度并进行运动的多关节机器人中，判别手腕轴的速度是否超过允许限度，在超过允许限度的情况下，计算使速度为允许限度内的该手腕轴的角度的多个候选，从多个该候选中，选择焊接线坐标系的姿势角的特定成分的变动得到最大抑制的候选，作为该手腕轴的角度，重新计算与其对应的其他轴的角度并作为下一插值点的角度，驱动多关节机器人。
【专利说明】多关节型机器人的控制装置、控制方法以及控制程序
【技术领域】
[0001]本发明涉及多关节型机器人的控制装置、控制方法、以及控制程序，尤其涉及在手腕轴的各驱动轴的速度超过允许范围的情况下，维持手腕的姿势并将各驱动轴的速度维持在允许范围内的技术。
【背景技术】
[0002]这里，在图23所示的作为一种多关节型机器人的6轴机械手中，控制安装于基座的胳膊轴的关节轴从基座起依次为Jl轴、J2轴、J3轴，控制手腕轴的关节轴从胳膊部分到手腕前端依次为J4轴、J5轴、J6轴。在此情况下,在给出Jl轴?J6轴的各关节角Q1'Θ 6时，通过求出正运动学问题的解，能够求出正交坐标系中的末端执行器(安装于手指的前端部分的焊接器等效果器)的位置的XYZ坐标、和末端执行器的姿势角α、β、Y (另外，姿势角α、β、Y用欧拉角或横摆(Roll).纵倾(Pitch).横倾(Yaw)角等表示)。另外相反，在给出末端执行器的位置的XYZ坐标和姿势角α、β、Y时，通过求出逆运动学问题的解，能够求出Jl轴?J6轴的各关节角Θ i?Θ 6。
[0003]另外，如图24所示，在J5轴的关节角Θ 5在O度附近时使末端执行器以一定速度运动的情况下，J4轴的关节角Θ 4与J6轴的关节角Θ 6的角度急剧变化，因此必须使J4轴与J6轴高速转动。但是，机械手的各关节轴无法超过转动速度的上限值，在勉强超过上限值动作的情况下，产生末端执行器的路径偏离预定路径，或者末端执行器振动的问题。这样，在多关节型机械手中，有时稍微移动末端执行器的位置或姿势角，就产生要求具有极大转动速度的关节轴，出现无法动作的情况。在发生了这种无法动作的情况下，难以兼顾各关节轴的速度和末端执行器的移动路径的精度，会出现为了确保移动路径的精度各关节轴的转动速度低于要求的速度，或者安全装置起作用从而停止机械手的动作的问题。
[0004]对此，专利文献I?3中公开了在某个关节轴的速度超过允许范围的情况下，使各手腕轴不发生急剧变化并保持限制内的速度动作的方法。
[0005]现有技术文献
[0006]专利文献
[0007]专利文献I JP特开昭62-162109号公报
[0008]专利文献2 JP特开平6-324730号公报
[0009]专利文献3 JP特开2003-300183号公报
【发明内容】
[0010]发明要解决的问题
[0011]但是，一般已知，在某个关节轴的速度超过允许范围的情况下，使各手腕轴不发生急剧变化并保持限制内的速度动作，通过预定路径的情况下，无法同时满足对末端执行器要求的(I)位置(2)移动速度(3)姿势角度这三个要素。因此，在一般的机械手中，进行使机械手动作的控制，从而在(I)?(3)要素中仅满足一个或两个要素。[0012]例如，在专利文献I及专利文献2中，记载了维持末端执行器的(I)位置和(3)姿势角度的方法，但是由于无法维持(2)移动速度，移动速度变慢，因此在末端执行器具有焊接器的情况下，花费比原本要求的时间更长的时间通过作业线，比其他部分的焊接时间长，存在强度降低等问题。此外，移动速度变慢、焊接时间变长的地方过度隆起，有可能对下面的步骤(例如，第二层的焊接步骤等)产生影响。
[0013]另外，专利文献3中记载了维持末端执行器的移动速度或位置中的任一者的方法，但在维持移动速度的情况下无法维持位置，在末端执行器具有焊接器或涂镀器的情况下，存在偏离原本预定的作业线，效果无法到达需要焊接或涂镀的位置的问题。
[0014]因此，本发明鉴于上述情况而作，其目的在于提供一种多关节型机器人的控制装置、控制方法以及控制程序，在需要使手腕轴超过指定允许量急剧变化的情况下，维持焊接或涂镀作业中尤为必需的手腕前端的位置及手腕前端的移动速度和手腕轴的允许速度，并且抑制与目的相应的、作业坐标系中的姿势角度的至少一个特定成分的变动。
[0015]用于解决问题的方案
[0016]为了实现上述目的，本发明适用于控制多关节型机器人的多关节型机器人的控制装置，该多关节型机器人包括:第一关节驱动系统，在前端具有按照作业对象的作业线移动的作业部，包括与该作业部连接并使所述作业部的姿势发生变化的三个驱动轴；以及第二关节驱动系统，与所述第一关节驱动系统连接，包括使该第一关节驱动系统的位置发生变化的至少三个驱动轴，该多关节型机器人的控制装置的结构特征是包括下述的(11)?(16)。
[0017](11)插值数据计算单元，在所述多关节型机器人的基础坐标系中，计算表示对提示路径进行插值的多个插值点的所述作业部的位置及当时的姿势的数据，该提示路径连接预先提示的所述作业部的作业开始位置及当时的姿势和作业结束位置及当时的姿势。
[0018](12)角度计算单元，根据由所述插值数据计算单元计算的数据求出逆运动学问题的解，据此计算表示所述插值点的所述作业部的位置及当时的姿势的、包含所述第一关节驱动系统及所述第二关节驱动系统的所有驱动轴的角度。
[0019](13)速度计算单元，基于由所述角度计算单元计算的下一个所述插值点与当前的所述插值点的所述第一关节驱动系统的所述各驱动轴的角度的差，计算使所述作业部的姿势移动到下一个所述插值点时的所述第一关节驱动系统的两端的两个驱动轴的速度。
[0020](14)姿势数据变换单元，将由所述插值数据计算单元计算的数据变换为姿势数据，该姿势数据表示包含所述作业部的移动方向的轴、垂直于该轴的方向的轴、以及垂直于这两个轴的轴的作业坐标系中的所述作业部的姿势。
[0021](15)重新计算单元，在由所述速度计算单元计算的所述第一关节驱动系统的两端的两个驱动轴的一者或两者的速度在预先确定的允许范围外的情况下，不变更所述作业部的移动速度，并且对于由所述姿势数据变换单元变换的下一个所述插值点的所述作业部的姿势数据，抑制一个或两个特定成分的变动，同时重新计算所述第一关节驱动系统的两端的两个驱动轴的速度在所述允许范围内的所述第一关节驱动系统的各个驱动轴的角度，基于该计算的所述第一关节驱动系统的各个驱动轴的角度和由所述插值数据计算单元计算的所述作业部的位置，重新计算所述第二关节驱动系统的各个驱动轴的角度。
[0022](16)驱动指示单元，在由所述速度计算单元计算的所述第一关节驱动系统的两端的两个驱动轴的两者的速度在所述允许范围内的情况下，基于由所述角度计算单元计算的所述各个驱动轴的角度，驱动所述多关节型机器人，在由所述速度计算单元计算的所述第一关节驱动系统的两端的两个驱动轴的一者或两者的速度在所述允许范围外的情况下，基于由所述重新计算单元计算的所述各个驱动轴的角度，驱动所述多关节型机器人。
[0023]根据本发明，在需要使所述第一关节驱动系统的驱动轴超过所述允许范围急剧变化的情况下，能够维持焊接或涂镀作业中尤为必需的所述作业部的位置及手腕前端的移动速度和所述第一关节驱动系统的驱动轴的允许速度，并且抑制与目的相应的、作业坐标系中的姿势角度的至少一个特定成分的变动。另外，在计算从所述作业开始位置到所述作业结束位置之间存在的特异点(奇异点)区域或特异点避开路径等的情况下，需要设置能够应对事先计算时的过大负载的运算装置(CPU)，因而成本提高，但根据本发明，根据当前状态和下一状态逐步控制驱动即可，因此减少对运算装置瞬间要求的处理负载，能够实现低成本化。
[0024]例如，考虑所述重新计算单元包括:角度候选计算单元，基于当前的所述插值点的所述第一关节驱动系统的各个驱动轴的角度，计算多个该各个驱动轴的角度作为参照追随由所述角度计算单元计算的下一个所述插值点的角度，并且所述第一关节驱动系统的两端的两个驱动轴的下一个所述插值点的速度位于所述允许范围内的该各个驱动轴的角度候选；姿势数据计算单元，分别计算用所述作业坐标系表示采用由所述角度候选计算单元计算的各个角度候选时下一个所述插值点的所述作业部的姿势的姿势数据；以及角度选择单元，在由所述姿势数据计算单元计算的多个姿势数据中，选择对于由所述姿势数据变换单元变换的下一个所述插值点的所述作业部的姿势数据，所述特定成分的变动得到最大抑制的姿势数据，选择与该姿势数据对应的角度作为所述各个驱动轴的角度。
[0025]据此，在使所述作业部的位置?姿势变化时，能够抑制与所述作业部的作业目的相应的适当的特定成分的变动。
[0026]另外，在以±180°表示所述第一关节驱动系统的驱动轴的角度时，所述角度计算单元例如计算所述第一关节驱动系`统的位于中央的驱动轴的下一个所述插值点的角度与所述作业开始位置的该驱动轴的角度为相同符号的逆运动学问题的解。据此，控制使得所述第一关节驱动系统的位于中央的驱动轴的角度不经过0°，能够避开特异点。
[0027]另外，所述角度计算单元可以在由所述速度计算单元计算的所述第一关节驱动系统的两端的两个驱动轴的两者的速度在所述允许范围内的情况下，计算所述第一关节驱动系统的位于中央的驱动轴的下一个所述插值点的角度与所述作业开始位置的该驱动轴的角度为相同符号的逆运动学问题的解，在由所述速度计算单元计算的所述第一关节驱动系统的两端的两个驱动轴的一者或两者的速度在所述允许范围外的情况下，计算所述第一关节驱动系统的位于中央的驱动轴的下一个所述插值点的角度与所述作业开始位置的该驱动轴的角度为不同符号的逆运动学问题的解。
[0028]进而，还考虑对它们进行组合而得到的结构。即，较为理想的是，所述角度计算单元在所述作业开始位置以及所述作业结束位置的所述第一关节驱动系统的中央的驱动轴的角度为相同符号的情况下，计算所述第一关节驱动系统的位于中央的驱动轴的下一个所述插值点的角度与所述作业开始位置的该驱动轴的角度为相同符号的逆运动学问题的解，在所述作业开始位置以及所述作业结束位置的所述第一关节驱动系统的中央的驱动轴的角度为不同符号的情况下，在由所述速度计算单元计算的所述第一关节驱动系统的两端的两个驱动轴的两者的速度在所述允许范围内的情况下，计算所述第一关节驱动系统的位于中央的驱动轴的下一个所述插值点的角度与所述作业开始位置的该驱动轴的角度为相同符号的逆运动学问题的解，在由所述速度计算单元计算的所述第一关节驱动系统的两端的两个驱动轴的一者或两者的速度在所述允许范围外的情况下，计算所述第一关节驱动系统的位于中央的驱动轴的下一个所述插值点的角度与所述作业开始位置的该驱动轴的角度为不同符号的逆运动学问题的解。据此，能够根据提示到达所述作业结束位置的所述第一关节驱动系统的中央驱动轴的角度。
[0029]另外，考虑所述重新计算单元重新计算所述驱动轴的角度，使得由所述姿势数据变换单元变换后的所述作业部的姿势数据中，从预先确定的加权较大的所述特定成分起，变动的抑制量依次增大。据此，通过适当设定所述加权，能够适当调整所述特定成分的变动的抑制量。
[0030]作为本发明申请的更为具体的结构，考虑所述作业部为焊枪，所述姿势数据以作业坐标系表示，该作业坐标系中，以所述作业部的移动方向的轴为X轴，以由所述X轴与重力方向的外积表示的轴为Y轴，以由所述X轴与所述Y轴的外积表示的方向的轴为Z轴，以所述X轴的绕轴的转动角为焊枪倾斜角，以所述Y轴的绕轴的转动角为焊枪前进角，以所述Z轴的绕轴的转动角为焊枪转动角，所述特定成分为所述焊枪倾斜角、所述焊枪前进角、所述焊枪转动角中的任意一个或两个。如图25所示，对于所述手腕前端部具有的所述作业部的姿势，根据正交坐标系Σ base上表示的手腕前端位置XYZ与手腕前端姿势α β Y,在以所述作业部移动的方向的轴为X轴的焊接线坐标系Σ line(作业坐标系)上进行表示，由此，能够如图26?图28所示,作为相对于各轴的倾角成分来表示所述作业部的姿势。变换后的焊接线坐标系Σ line的绕坐标轴的转动角如图26、图27所示，绕Hine轴的转动角以右旋方向为正向作为所述焊枪倾斜角Rx，如图26、图28所示，绕Yline轴的转动角以右旋方向为正向作为所述焊枪前进角Ry，绕Zline轴的转动角以右旋方向为正向作为所述焊枪转动角Rz。
[0031]此时，在所述作业部是前端具有对所述作业对象产生作业上的效果的一个作业点的单头焊枪的情况下，认为所述特定成分是所述焊枪倾斜角和所述焊枪前进角。据此，能够抑制对于有一个作业点的单头焊枪的作业而言很重要的所述焊枪倾斜角和所述焊枪前进角的变动。
[0032]另一方面，在所述作业部是前端具有对所述作业对象产生作业上的效果的两个作业点的双头焊枪的情况下，认为所述特定成分是所述焊枪倾斜角和所述焊枪转动角。据此，能够抑制对于有两个作业点的双头焊枪的作业而言很重要的所述焊枪倾斜角和所述焊枪转动角的变动。
[0033]此外，考虑所述驱动指示单元在由所述速度允许判别单元判别为存在所述允许范围外的驱动轴之后，基于由所述重新计算单元计算的所述各个驱动轴的角度，驱动所述多关节型机器人，随后，以由所述角度计算单元计算的所述第一关节驱动系统的角度与由所述重新计算单元计算的所述第一关节驱动系统的角度的差达到指定值以下为条件，代替由所述重新计算单元计算的所述各个驱动轴的角度，基于由所述角度计算单元计算的所述各个驱动轴的角度，驱动所述多关节型机器人。[0034]此时，考虑所述驱动指示单元在从基于由所述重新计算单元计算的所述各个驱动轴的角度的所述多关节机器人的驱动控制切换为基于由所述角度计算单元计算的所述各个驱动轴的角度的所述多关节机器人的驱动控制时，使该各个驱动轴的角度在指定的变动范围内逐渐变动。据此，能够防止沿着所述作业对象的作业线移动的所述作业部发生振动从而不沿着作业线移动。例如，考虑为由所述重新计算单元计算的角度与根据所述多关节机器人的驱动控制由所述角度计算单元计算的角度的中间角度。
[0035]另外，所述速度计算单元可以代替速度而计算加速度，所述重新计算单元以及所述驱动指示单元可以代替速度，基于加速度进行判别。
[0036]此外，本发明可以理解为执行由所述多关节型机器人的控制装置执行的各步骤的多关节型机器人的控制方法的发明，或者可以理解为使计算机执行上述各步骤的多关节型机器人的控制程序的发明。
[0037]发明效果
[0038]根据本发明，在使手腕前端稍微移动，手腕轴就超过指定允许量急剧变化的情况下，也能够维持焊接或涂镀作业中尤为必需的手腕前端的位置及手腕前端的移动速度和手腕轴的允许速度，并且抑制与目的相应的、作业坐标系中的姿势角度的至少一个特定成分的变动。另外，在计算从所述作业开始位置到所述作业结束位置之间存在的特异点区域或特异点避开路径等的情况下，需要设置能够应对事先计算时的过大负载的运算装置(CPU)，因而成本提高，但根据本发明，根据当前状态和下一状态逐步控制驱动即可，因此减少对运算装置瞬间要求的处理负载，能够实现低成本化。
【专利附图】
【附图说明】
[0039]图1是表示由本发明实施方式的多关节型机器人的控制方法的一例控制的多关节型机器人X的概略结构的方框图。
[0040]图2是表示多关节型机器人的控制方法的控制整体处理的一例的流程图。
[0041]图3是表示插值点的各转动轴的速度.角度计算处理的一例的流程图。
[0042]图4是表示速度抑制必要性判别处理的一例的流程图。
[0043]图5是表不速度抑制处理的一例的流程图。
[0044]图6是表示从速度抑制处理恢复的处理的一例的流程图。
[0045]图7是表不插值点的路径的一例的图。
[0046]图8 (A)是表示原来路径与例外路径的J6轴的角度变化的一例的图，(B)是表示原来路径的插值点与例外路径的插值点的一例的图。
[0047]图9是表示避开特异点的路径的第一关节驱动轴的角度变化的一例的图表。
[0048]图10是表示J4轴的速度变化的一例的图。
[0049]图11是表示等速动作时焊接线坐标系的各姿势角的变动的一例的图。
[0050]图12是表示本发明的控制方法的焊接线坐标系的各姿势角的变动的一例的图。
[0051]图13是表示第二关节驱动轴的其他例的图。
[0052]图14是表示多关节型机器人的控制方法的控制整体处理的其他例的流程图。
[0053]图15是表示经由特异点的路径的第一关节驱动轴的角度变化的一例的图表。
[0054]图16是表示采用了本实施例1的控制方法的情况下的第一关节驱动轴的角度变化的一例的图表。
[0055]图17是表示等速动作时焊接线坐标系的各姿势角的变动的一例的图。
[0056]图18是表示采用了本实施例1的控制方法的情况下的焊接线坐标系的各姿势角的变动的一例的图。
[0057]图19是表示多关节型机器人的控制方法的控制整体处理的其他例的流程图。
[0058]图20是表不实施例3的具有两个作业点的末端执行器的一例的图。
[0059]图21是表示具有两个作业点的末端执行器的多关节型机器人的避开特异点的路径的焊接线坐标系的各姿势角的变动的一例的图。
[0060]图22是表示由实施例4的多关节型机器人的控制方法的一例控制的多关节型机器人X2的概略结构的方框图。
[0061]图23是表示普通的6轴机械手的概略结构的图。
[0062]图24是表示普通的6轴机械手经由特异点的路径的第一关节驱动轴的角度变化的一例的图表。
[0063]图25是表示将表示普通的6轴机械手的末端执行器的位置和姿势的坐标系变换为焊接线坐标系的情况下的一例的图。
[0064]图26是表示焊接线坐标系的各轴的倾角成分的一例的图。
[0065]图27是表示焊接线坐标系的焊枪倾斜角的一例的图。
[0066]图28是表示焊接线坐标系的焊枪前进角和焊枪转动角的一例的图。
[0067]图29是表示实施例5的计算机系统Y的概略结构的方框图。
[0068]图30是用于说明通知正在避开特异点这一情况的装置的图。
【具体实施方式】
[0069]下面参照【专利附图】
【附图说明】本发明的实施方式，以便理解本发明。另外，下面的实施方式是使本发明具体化的一例，不具有限定本发明的技术范围的性质。
[0070]首先，参照图1所示的概略结构图，说明本发明实施方式的多关节型机器人X的结构。
[0071]多关节型机器人X包括控制部10、操作部21、机械手(manipulator)主体30等。另外，除了图1所示的结构要素以外，多关节型机器人X还包括一般的多关节型机器人所包括的其他结构要素，这里省略记载。控制部10包括主控制部11、存储部12、以及驱动指示部13，是本发明的多关节型机器人的控制装置的一例。
[0072]机械手主体30包括:第二关节驱动系统，具有相当于人类的胳膊的Jl轴31、J2轴32、以及J3轴33 ;第一关节驱动系统，具有相当于人类的手腕的J4轴34、J5轴35、以及J6轴36 ;以及末端执行器39，对作业对象发挥效果，机械手主体30是按照所操作的内容进行驱动，与人类的上臂进行相同作业的机器人。另外，上述机械手主体30是多关节型机器人的一例。
[0073]Jl轴31?J6轴36由电动机等构成，根据来自驱动指示部13的指示在正负方向上旋转驱动，多个轴的旋转驱动相互关联，由此实现人类的手腕或胳膊的动作。尤其是，上述第二关节驱动系统与上述第一关节驱动系统的驱动轴的动作无关，可以自由地确定上述多关节型机器人的基础坐标系Σ base中的上述末端执行器39的位置(X，Y，Z)。[0074]末端执行器39是安装于机械手主体30前端(J6轴36的前端)的效果器，是利用一个作业点进行作业的焊接器(焊枪)、涂镀装置、工具、捕获器、传感器等。另外，上述末端执行器39是作业部的一例。
[0075]另外，具有一个作业点的焊枪称为单头焊枪。
[0076]操作部21由用户操作的薄片按键或操作按钮、操作杆等构成，是接收用户的操作输入的输入接口。例如，操作部21接收上述末端执行器39的作业开始位置及姿势、作业结束位置及姿势、以及对其进行连接的路径的作业路径或作业时间、用于抑制与目的相应的、焊接线坐标系Σ line (作业坐标系)中末端执行器39的姿势角度的各成分的变动的加权信息、以及记载了这些内容的程序等输入，并输出到控制部10。即，操作部21是用于提示末端执行器39的作业开始位置及姿势、作业结束位置及姿势的单元，是用于设定上述加权信息的单元。
[0077]存储部12例如是由硬盘或DRAM等构成的易失性存储器，存储末端执行器39的作业开始位置及姿势、作业结束位置及姿势、以及对其进行连接的路径的作业路径或作业时间，用于抑制末端执行器39的姿势角度的各成分的变动的加权信息，由后述的主控制部11计算的插值点(经由特异点的路径和避开特异点的路径的双方)的上述Jl轴31?J6轴36的角度和速度(根据情况不同有时还有加速度)等。
[0078]驱动指示部13按照来自主控制部11的控制指示，在每个采样周期中，读出上述Jl轴31?J6轴36的角度或速度的信息以使末端执行器39移动到存储部12中存储的下一个插值点(经由特异点的路径或避开特异点的路径中的一者)，并将上述信息输出到机械手主体30。另外，主控制部11和驱动指示部13是用于执行驱动指示步骤的驱动指示单元的一例。
[0079]主控制部11包括C.PU、R0M等，CPU是进行该多关节型机器人X的各种控制及运算的运算单元，ROM是存储由CPU执行的控制程序或运算程序、CPU执行这些程序时参照的数据等的存储器。
[0080]接着，参照图2?图6的流程图，说明以多关节型机器人X的主控制部11为中心，使机械手主体30动作的处理过程的一例。另外，以下所示的S11、S12、……表示处理过程(步骤)的识别标号。
[0081]首先，参照图2，说明一般的机械手主体30的控制与本发明的一实施方式的多关节型机器人X的控制的处理过程的差异。
[0082]一般的机械手主体30的控制实施如后所示对主控制部11提示动作轨迹的提示步骤(步骤Sll)，还实施为了以所提示的动作轨迹上的插值点的位置.姿势动作而计算并存储各驱动轴的角度的步骤(步骤S12)，读出所计算的角度并指示动作的步骤(步骤S14)，以及判别是否已到达作业结束位置?姿势的步骤(步骤S15)，以使机械手主体30从当前插值点的位置?姿势向下一插值点的位置?姿势运动，随后，在指定的每个采样期间中反复执行步骤S12、S14、S15，据此使机械手主体30沿着动作轨迹从作业开始位置.姿势运动到作业结束位置?姿势。另外，主控制部11使机械手主体30的末端执行器39以预先设定的一定速度移动，据此执行该末端执行器39的作业。但是，在以一定速度移动的情况下，当第一关节驱动系统的J5轴35经过0°附近时(特异点)，J4轴34、J6轴36的速度有可能超过预先确定的允许范围。[0083]对此，本实施方式的多关节型机器人X的控制除了上述一般的机械手主体30的控制所需的步骤以外，如后所述，还包含:判别是否由于第一关节驱动系统的J4轴34、J6轴36的至少一者的速度超过允许范围而需要执行速度抑制处理的步骤(步骤S13)，以及在需要执行速度抑制处理的情况下(步骤S13:是)，为了移动至避开特异点的动作轨迹上的插值点而重新计算并存储各驱动轴的角度的速度抑制步骤(步骤S20)，避开特异点，使机械手主体30从当前插值点的位置.姿势移动至重新计算的下一个插值点的位置.姿势。
[0084]接着，参照图2说明由主控制部11执行的多关节型机器人X的控制处理整体，各处理的细节在后面描述。
[0085]最初，利用者利用操作部21点到点地提示机器人的动作轨迹。即，通过操作部21移动到各提示点，将上述末端执行器39的作业位置以及姿势存储到控制部10的存储部12 (步骤SI I)。另外，步骤Sll是提示步骤的一例。
[0086]主控制部11在上述提示的点之间按照指定的每个采样周期(例如，1/16秒、1/32秒等)进行分割.插值，计算用于使末端执行器39移动到下一个插值点的Jl轴31?J6轴36的角度和第一关节驱动系统的J4轴34?J6轴36的速度(根据情况不同有时还计算加速度)，并存储到存储部12 (步骤S12)。这里，Jl轴31?J6轴36的角度用土 180°表示。当然，角度的指定方法并不限于±180°，例如还可以是0°?360°等。另外，该步骤S12的细节在后面进行描述。
[0087]然后，主控制部11判别上述步骤S12计算的用于使末端执行器39移动到下一插值点的第一关节驱动系统的J4轴34及J6轴36的一者或两者的速度或加速度位于允许范围内还是允许范围外(以后称为“需要.不需要速度抑制处理”)，或者是否处于已判别为存在允许范围外的驱动轴之后的避开处理中(以后称为“处于速度抑制处理中”)(步骤S13)。另外，该步骤S13的细节在后面进行描述。
[0088]这里，在判别为需要速度抑制处理或者处于速度抑制处理中时(步骤S13:是)，主控制部11进行速度抑制处理(步骤S20)，该处理不变更末端执行器39的移动速度，并且对上述步骤S12计算的下一个插值点的末端执行器39的姿势数据，抑制一个或两个特定成分的变动，重新计算该J4轴34及J6轴36的一者或两者的角度以使速度或加速度处于上述允许范围内，重新计算与此对应的其他轴的角度，并存储到存储部12。另外，在该步骤S20的上述说明中，作为判别“需要.不需要速度抑制处理”的标准，以各轴的速度或加速度为问题，但在该步骤S20的具体例的后述说明中，举出对速度进行判别的情况为例进行说明。
[0089]另一方面，在判别为不需要速度抑制处理，不处于速度抑制处理中(步骤S13:否)，或者进行了速度抑制处理(步骤S20)后，基于主控制部11的指示，驱动指示部13从存储部12中读出用于使末端执行器39移动到下一个插值点的Jl轴31?J6轴36的角度，并输出到机械手主体30的各驱动轴的致动器(步骤S14)。此时，在判别为不需要速度抑制处理，不处于速度抑制处理中的情况下，基于上述步骤S12计算的用于移动到下一插值点的Jl轴31?J6轴36的角度，控制机械手主体30，在J4轴34、J6轴36的速度位于允许范围外并执行了速度抑制处理的情况下，基于上述步骤S20计算的用于移动到下一插值点的Jl轴31?J6轴36的角度，控制机械手主体30。这里，相关步骤S14是驱动指示步骤的一例，执行相关处理时的主控制部11及驱动支持部13相当于驱动指示单元。
[0090]接着，主控制部11判别移动后的末端执行器39的位置及姿势是否已到达作业部结束位置及姿势(步骤S15)。在未到达作业部结束位置及姿势的情况下(步骤S15:否)，在每个指定的采样周期中，重新实施上述步骤S12之后的步骤，在到达作业部结束位置及姿势的情况下(步骤S15:是)，结束处理。
[0091]这样，主控制部11在每个指定的采样周期中，计算用于使末端执行器39从当前位置及姿势移动到下一个插值点的Jl轴31?J6轴36的角度，并使多关节型机器人X动作。
[0092]接着，参照图3，说明对应于图2的步骤S12的、计算用于从多关节型机器人X的当前插值点移动到下一个插值点的Jl轴31?J6轴36的角度与第一关节驱动系统的J4轴34?J6轴36的速度的处理过程的一例。
[0093]首先，主控制部11计算在上述步骤Sll中预先提示的作业开始位置及姿势PO与作业结束位置及姿势Pn之间用η(基于每个采样周期的任意值)个点进行插值而得到的末端执行器39的位置及姿势。在此情况下，每一个采样周期的移动量可用下式表示:Δ P= (Pn-P0) / η= { Δ X= (Xn-X0) / η, Δ Y= (Yn-Y0) / η, Δ Z= (Zn-Z0) / η, Δ α = ( α η-α 0) /η, Δ β=(βη-β0) / η, Δ y = (yn-y0) / η}。此外，上述各个插值点由上述多关节型机器人X的基础坐标系Σ base表不。
[0094]如图7所示，主控制部11在当前位置的插值点为Pi的情况下，通过Pw=P0+ΔPX (1+1)、？1+1=?1+八？计算下一个插值点？1+1的位置及姿势(步骤S121)。另外，该步骤S121是插值数据计算步骤的一例，执行相关处理时的主控制部11相当于插值数据
计算单元。
[0095]接着，主控制部11根据下一个插值点Pi+1的位置及姿势(Xi+1，Yi+1, Zi+1，a i+1，β i+1，i+1)求出逆运动学问题的解，由此计算各Jl轴31?J6轴36的角度(θ1； Θ 2，Θ 3，Θ 4，Θ 5，Θ 6)，并存储到存储部12 (步骤S122)。此时，关于J4轴34?J6轴36得到逆运动学问题的两个解 ，这里，主控制部11采用这两个解中第一关节驱动系统的J5轴35的角度与提示的上述末端执行器39的作业开始位置的J5轴35的角度符号相同的逆运动学问题的解。据此，J5轴35的角度不经过0°而进行推移，因此能够避免经过特异点。
[0096]另外，主控制部11在执行后述的速度抑制处理的情况下，不仅计算经由特异点的路径的插值点的各Jl轴31?J6轴36的角度，还分别计算避开特异点的路径的插值点的各Jl轴31?J6轴36的角度并存储到存储部12，作为进行后述的恢复处理(图5的步骤S310以及图6的流程图)时的比较对象。
[0097]这里，步骤S122是角度计算步骤的一例，执行相关处理时的主控制部11相当于角度计算单元。
[0098]接着,主控制部11从计算的下一插值点Pi+1的各J4轴34?J6轴36的角度中，减去存储部12中存储的当前插值点Pi的各J4轴34?J6轴36的角度，计算第一关节驱动系统的J4轴34?J6轴36的速度并存储到存储部12(步骤S123)。另外，如后所述，在执行速度抑制处理(步骤S20)的情况下，计算经由特异点的路径的插值点的第一关节驱动系统的J4轴34?J6轴36的速度并存储到存储部12，用于在后述的步骤S131中计算速度的允许范围，或者在步骤S211等中计算下一插值点的候选速度(根据需要还计算加速度)。
[0099]这里，步骤S123是速度计算步骤的一例，执行相关处理时的主控制部11相当于速度计算单元。
[0100]接着，参照图4说明相当于图12的步骤S13的速度抑制必要性判别处理过程的一例。
[0101]首先，主控制部11计算使末端执行器39的位置及姿势从当前插值点Pi移动到下一插值点Pi+Ι时的第一关节驱动系统的J4轴34、J6轴36的速度的允许范围(步骤S131)。例如，将当前插值点Pi的速度值加减指定值或指定比例得到的值作为上限.下限，计算速度的允许范围。另外，该各轴的速度的允许范围也可以根据末端执行器39的动作条件(例如焊接时的温度、涂镀时的涂镀膜的厚度等条件)、多关节型机器人动作时不引起振动等异常动作的条件等，在对作业对象的质量不产生影响的范围内预先设定。此外，也可以对J4轴34、J6轴36分别设定各自的允许范围。
[0102]接着，主控制部11判别第一关节驱动系统的下一个插值点的J4轴34以及J6轴36的一者或两者的速度是否超过上述允许范围，或者是否设立了表示处于速度抑制处理中的标志(步骤S132)。
[0103]这里，在第一关节驱动系统的下一个插值点的J4轴34以及J6轴36的两者位于上述允许范围内，并且不处于速度抑制处理中的情况下(步骤S132:否)，基于主控制部11的指示，驱动指示部13从存储部12读出上述步骤S12计算的各Jl轴31?J6轴36的角度并输出到机械手主体30 (步骤S14)。另一方面，在存在允许范围外的轴，或者处于速度抑制处理中的情况下(步骤S132:是)，主控制部11执行以下所述的速度抑制处理(步骤S20)。
[0104]下面，参照图5说明相当于图2的步骤S20的该多关节型机器人X的速度抑制处理的具体例。
[0105]另外，下面作为例子举出图24所示的J5轴35处于O度附近，J4轴34、J6轴36的速度超过允许范围时执行该速度抑制处理的情况进行说明，由于其他理由，J4轴34、J6轴36的速度超过允许范围的情况也同样。因此，该速度抑制处理在第一关节驱动系统的J4轴34、J6轴36中任意一个或者两个驱动轴的速度超过允许范围时执行。另外，最初发现下一个插值点有允许范围外的轴时，当前插值点的速度处于允许范围内。
[0106]首先，主控制部11判别关于下一个插值点，速度处于上述允许范围外的驱动轴是J4轴34及J6轴36中的哪一者或两者，或者在“处于速度抑制处理中”的标志已经设立的情况下，该速度抑制处理是对J4轴34及J6轴36中的哪一者或两者执行的，并对处理进行分支(步骤S201)。如前所述，通过速度是否位于允许范围的上限.下限间，来判别是在允许范围内还是允许范围外。这里，若J4轴34及J6轴36的两者都不在速度的允许范围外，并且“处于速度抑制处理中”的标志并未设立(步骤S201:其他)，则主控制部11结束该速度抑制处理。
[0107]另一方面，在J6轴36的速度位于允许范围外的情况下处理转至步骤S211，在J4轴34的速度位于允许范围外的情况下处理转至步骤S221，在J4轴34及J6轴36的两者的速度位于允许范围外的情况下处理转至步骤S231。
[0108](J6轴36的速度抑制处理步骤S211?S217)
[0109]在J6轴36的速度位于允许范围外，或者设立了表示处于J6轴36的速度抑制处理中的标志的情况下(步骤S201:J6轴为范围外)，主控制部11基于当前插值点的J6轴36的角度，计算多个该J6轴36的角度追随上述步骤S12计算的下一个插值点的角度，并且J6轴36的下一个插值点的速度位于上述允许范围内的角度候选(步骤S211)。[0110]例如，主控制部11利用下面的步骤(21)?(23)，计算J6轴36的速度候选。
[0111](21)首先，利用下式(21a)计算抑制基准速度V6b。这里，是下一个插值点的J6轴36的角度，Θ old是当前插值点的J6轴36的角度，E是预先设定的系数(例如0.1)。
[0112]V6b=E(9now-0old) (21a)
[0113](22)接着，判断抑制基准速度V6b是否在预先设定的速度限值Vmax以上，在判断为该速度限值Vmax以上的情况下，将抑制基准速度V6b设定为上次的速度Θ。1(1。上述速度限值Vmax例如是上述允许范围内的速度的最大值。
[0114](23)然后，根据抑制基准速度V6b和指定值D (已知保证事先确定的允许范围内的加速度的速度变化绝对值等)，计算三个速度候选(V Θ 6a=V6b-D, V Θ 6b=V6b, V Θ 6c=V6b+D)。上述指定值D例如是上述速度限值Vmax的10%左右的值。另外，在V 06a大于上述速度限值Vmax的情况下，令ve6a=vmax。
[0115]随后，主控制部11根据当前插值点的J6轴36的角度Θ 6，计算与上述速度的各候选(V θ 6a，V θ 6b> V θ 6c)对应的下一插值点的三个J6轴36的角度候选(θ 6a, 0 6b, 0 6c)。即，作为下一插值点，至少计算继续保持了不超过允许范围的抑制基准速度V6b或当前插值点的速度的角度(当然在允许范围内)、以及与其仅相差微小量(土D(与速度变化绝对值对应))的角度。这里，相关步骤S211的处理是角度候选计算步骤的一例，执行相关处理时的主控制部11相当于角度候选计算单元。
[0116]接着，主控制部11计算与所计算的下一插值点的J6轴36的各角度候选对应的其他驱动轴Jl轴31?J5轴35的角度(步骤S212)。例如，如以下的(I)?(3)所示，对于J6轴36的每个角度候选(Θ 6a，Θ 6b, Θ 6c)，使用上述步骤S122中计算的下一插值点Pi+1的末端执行器39的位置(Xi+1，Yi+1，Zi+1)和第一关节驱动系统的J4轴34、J5轴35的角度(Θ 4，Θ 5)，重新计算第一关节驱动系统及第二关节驱动系统的Jl轴31?J6轴36的角度
(0 1， θ2， θ3， θ4， θ5， θ6)。
[0117](I)对于候选 Θ 6a，根据[xi+1，Yi+1，zi+1, Θ 4，θ 5，θ 6J 计算[θ la，Θ 2a, Θ 3a, Θ 4，
θ 5，θ 6J。
[0118]⑵对于候选Θ 6b，根据[Xi+1，Yi+1，Zi+1, Θ 4，θ 5，θ 6b]计算[θ lb，Θ 2b, Θ 3b, Θ 4，Θ 5，Θ 6b]。
[0119]⑶对于候选Θ 6c，根据[Xi+1，Yi+1，Zi+1, Θ 4，θ 5，θ 6J 计算[θ lc，Θ 2c, Θ 3c, Θ 4，
θ 5，θ 6J。
[0120]由此，如后面说明的图8所示，计算速度不超过允许范围的下一插值点的候选P’ i+1，将该下一插值点的候选P’ i+1视为下一插值点，据此能够得到J6轴36的速度不超过允许范围，并且避开的原来的插值点Pi+1 (特异点)的例外路径(参照图8 (B))。
[0121]以此方式得到将J6轴36的速度维持在允许范围内的下一插值点的三个角度候选(06a, 06b, θ6。)后，进行应选择哪个候选作为下一插值点的判断。在迄今为止的计算中，将第一关节驱动系统的J4轴34?J6轴36的角度(Θ 4，Θ 5，Θ 6)作为问题，但最终必须结合根据作业种类认为重要的末端执行器39的姿势，来选择候选。因此，需要从上述Jl轴31?J6轴36的角度变换为表示末端执行器39的姿势的数据。
[0122]并且，为了进行应选择哪个候选作为下一插值点的判断，主控制部11对于计算的每个候选，分别计算表示下一插值点的焊接线坐标系Σ line的末端执行器39的姿势的数据的各成分(焊枪倾斜角Rx，焊枪前进角Ry，焊枪转动角Rz)(步骤S213)。另外，该步骤S123是姿势数据计算步骤的一例，执行相关处理时的主控制部11相当于姿势数据计算单元。这里，如图25所示，上述焊接线坐标系Σ line是以末端执行器39移动的方向(行进方向)的轴为X轴，以由该X轴与重力方向的外积(X轴X重力方向)表示的轴为Y轴，以由该X轴与该Y轴的外积(X轴XY轴)表示的方向的轴为Z轴的作业坐标系，如图26?图28所示,该X轴的绕轴的转动角由焊枪倾斜角Rx表示，该Y轴的绕轴的转动角由焊枪前进角Ry表示，该Z轴的绕轴的转动角由焊枪转动角Rz表示。
[0123]例如，对于上述⑴?(3)的候选06a?θ6。，利用以下的(I’)?(3’)计算表示末端执行器39的姿势的数据的各成分。
[0124](Γ )根据[Θ la, Θ 2a，Θ 3a，Θ 4，Θ 5，Θ J 计算[Rxa，Rya, RJ。
[0125](2，)根据[Θ lb，Θ 2b，θ 3b，θ 4，θ 5，θ J 计算[Rxb，Ryb, RJ。
[0126](3，)根据[Θ lc, Θ 2c, Θ 3c, Θ 4，Θ 5，Θ J 计算[Rxc, Ryc, RJ。
[0127]接着，主控制部11对于上述步骤S121中计算的原来路径上的下一插值点Pi+Ι的末端执行器39的位置及姿势，为了由以末端执行器39移动的方向的轴为X轴的焊接线坐标系Σ line (焊枪倾斜角Rx,焊枪前进角Ry，焊枪转动角Rz)表示由正交坐标系Σ base表示的(Xi+1，Yi+1, Zi+1，a i+1，β i+1，y i+1)，进行数据变换(步骤S214)。另外，该步骤S214是姿势数据变换步骤的一例，执行相关处理时的主控制部11相当于姿势数据变换单元。
[0128]另外，下面的(数式I)中表示从正交坐标系Σ base变换为焊接线坐标系Σ line的式子。
[0129][数式I]
【权利要求】
1.一种多关节型机器人的控制装置，其特征在于，对多关节型机器人进行控制，所述多关节型机器人包括:第一关节驱动系统，在前端具有按照作业对象的作业线移动的作业部，并且包括与该作业部连接并使所述作业部的姿势发生变化的三个驱动轴；以及第二关节驱动系统，与所述第一关节驱动系统连接，包括使该第一关节驱动系统的位置发生变化的至少三个驱动轴， 所述多关节型机器人的控制装置包括: 插值数据计算单元，在所述多关节型机器人的基础坐标系中，计算表示对提示路径进行插值的多个插值点的所述作业部的位置及当时的姿势的数据，该提示路径连接预先提示的所述作业部的作业开始位置及当时的姿势和作业结束位置及当时的姿势； 角度计算单元，根据由所述插值数据计算单元计算出的数据求出逆运动学问题的解，据此计算表示所述插值点的所述作业部的位置及当时的姿势的、包含所述第一关节驱动系统及所述第二关节驱动系统的所有驱动轴的角度； 速度计算 单元，基于由所述角度计算单元计算出的下一个所述插值点与当前的所述插值点的所述第一关节驱动系统的所述各驱动轴的角度的差，计算使所述作业部的姿势移动到下一个所述插值点时的所述第一关节驱动系统的两端的两个驱动轴的速度； 姿势数据变换单元，将由所述插值数据计算单元计算出的数据变换为姿势数据，该姿势数据表示包含所述作业部的移动方向的轴、垂直于该轴的方向的轴、以及垂直于这两个轴的轴的作业坐标系 中的所述作业部的姿势； 重新计算单元，在由所述速度计算单元计算出的所述第一关节驱动系统的两端的两个驱动轴的一者或两者的速度在预先确定的允许范围外的情况下，不变更所述作业部的移动速度，并且对于由所述姿势数据变换单元变换的下一个所述插值点的所述作业部的姿势数据，抑制一个或两个特定成分的变动，同时重新计算所述第一关节驱动系统的两端的两个驱动轴的速度成为所述允许范围内的所述第一关节驱动系统的各个驱动轴的角度，基于该计算出的所述第一关节驱动系统的各个驱动轴的角度和由所述插值数据计算单元计算出的所述作业部的位置，重新计算所述第二关节驱动系统的各个驱动轴的角度；以及 驱动指示单元，在由所述速度计算单元计算出的所述第一关节驱动系统的两端的两个驱动轴的两者的速度在所述允许范围内的情况下，基于由所述角度计算单元计算出的所述各个驱动轴的角度，驱动所述多关节型机器人，在由所述速度计算单元计算出的所述第一关节驱动系统的两端的两个驱动轴的一者或两者的速度在所述允许范围外的情况下，基于由所述重新计算单元计算出的所述各个驱动轴的角度，驱动所述多关节型机器人。
2.根据权利要求1所述的多关节型机器人的控制装置，其特征在于: 所述重新计算单元包括: 角度候选计算单元，基于当前的所述插值点的所述第一关节驱动系统的各个驱动轴的角度，计算多个该各个驱动轴的角度将由所述角度计算单元计算出的下一个所述插值点的角度作为参照进行追随，并且所述第一关节驱动系统的两端的两个驱动轴的下一个所述插值点的速度成为所述允许范围内的该各个驱动轴的角度的候选； 姿势数据计算单元，分别计算用所述作业坐标系表示采用由所述角度候选计算单元计算出的各个角度候选时下一个所述插值点的所述作业部的姿势的姿势数据；以及 角度选择单元，在由所述姿势数据计算单元计算出的多个姿势数据中，选择对于由所述姿势数据变换单元变换的下一个所述插值点的所述作业部的姿势数据，所述特定成分的变动得到最大抑制的姿势数据，选择与该姿势数据对应的角度作为所述各个驱动轴的角度。
3.根据权利要求2所述的多关节型机器人的控制装置，其特征在于: 在以±180°表示所述第一关节驱动系统的驱动轴的角度时， 所述角度计算单元计算所述第一关节驱动系统的位于中央的驱动轴的下一个所述插值点的角度与所述作业开始位置的该驱动轴的角度成为相同符号的逆运动学问题的解。
4.根据权利要求2所述的多关节型机器人的控制装置，其特征在于: 在以±180°表示所述第一关节驱动系统的驱动轴的角度时， 所述角度计算单元在由所述速度计算单元计算出的所述第一关节驱动系统的两端的两个驱动轴的两者的速度在所述允许范围内的情况下，计算所述第一关节驱动系统的位于中央的驱动轴的下一个所述插值点的角度与所述作业开始位置的该驱动轴的角度成为相同符号的逆运动学问题的解， 在由所述速度计算单元计算出的所述第一关节驱动系统的两端的两个驱动轴的一者或两者的速度在所述允许范围外的情况下，计算所述第一关节驱动系统的位于中央的驱动轴的下一个所述插值点的角度与所述作业开始位置的该驱动轴的角度成为不同符号的逆运动学问题的解。
5.根据权利要求2所述的多关节型机器人的控制装置，其特征在于: 在以±180°表示所述第 一关节驱动系统的驱动轴的角度时， 所述角度计算单元， 在所述作业开始位置以及所述作业结束位置的所述第一关节驱动系统的中央的驱动轴的角度为相同符号的情况下，计算所述第一关节驱动系统的位于中央的驱动轴的下一个所述插值点的角度与所述作业开始位置的该驱动轴的角度成为相同符号的逆运动学问题的解， 在所述作业开始位置以及所述作业结束位置的所述第一关节驱动系统的中央的驱动轴的角度为不同符号的情况下，在由所述速度计算单元计算出的所述第一关节驱动系统的两端的两个驱动轴的两者的速度在所述允许范围内的情况下，计算所述第一关节驱动系统的位于中央的驱动轴的下一个所述插值点的角度与所述作业开始位置的该驱动轴的角度成为相同符号的逆运动学问题的解，在由所述速度计算单元计算出的所述第一关节驱动系统的两端的两个驱动轴的一者或两者的速度在所述允许范围外的情况下，计算所述第一关节驱动系统的位于中央的驱动轴的下一个所述插值点的角度与所述作业开始位置的该驱动轴的角度成为不同符号的逆运动学问题的解。
6.根据权利要求1至5中任一项所述的多关节型机器人的控制装置，其特征在于: 所述重新计算单元重新计算所述驱动轴的角度，使得由所述姿势数据变换单元变换后的所述作业部的姿势数据中，从预先确定的加权较大的所述特定成分起，变动的抑制量依次增大。
7.根据权利要求1至6中任一项所述的多关节型机器人的控制装置，其特征在于: 所述作业部为焊枪，利用以所述作业部的移动方向的轴为X轴，以由所述X轴与重力方向的外积表示的轴为Y轴，以由所述X轴与所述Y轴的外积表示的方向的轴为Z轴，以所述X轴的绕轴的转动角为焊枪倾斜角，以所述Y轴的绕轴的转动角为焊枪前进角，以所述Z轴的绕轴的转动角为焊枪转动角的作业坐标系表示所述姿势数据， 所述特定成分是所述焊枪倾斜角、所述焊枪前进角、所述焊枪转动角中的任意一个或两个。
8.根据权利要求7所述的多关节型机器人的控制装置，其特征在于: 所述作业部是在前端具有对所述作业对象产生作业上的效果的一个作业点的单头焊枪， 所述特定成分是所述焊枪倾斜角以及所述焊枪前进角。
9.根据权利要求7所述的多关节型机器人的控制装置，其特征在于: 所述作业部是在前端具有对所述作业对象产生作业上的效果的两个作业点的双头焊枪， 所述特定成分是所述焊枪倾斜角以及所述焊枪转动角。
10.根据权利要求1至9中任一项所述的多关节型机器人的控制装置，其特征在于: 所述驱动指示单元在由所述速度允许判别单元判别为存在所述允许范围外的驱动轴之后，基于由所述重新计算单元计算出的所述各个驱动轴的角度，驱动所述多关节型机器人，随后，以由所述角度计算单元计算出的所述第一关节驱动系统的角度与由所述重新计算单元计算出的所述第一关节驱动系统的角度的差成为指定值以下为条件，代替由所述重新计算单元计算出的所述各 个驱动轴的角度，基于由所述角度计算单元计算出的所述各个驱动轴的角度，驱动所述多关节型机器人。
11.根据权利要求10所述的多关节型机器人的控制装置，其特征在于: 所述驱动指示单元在从基于由所述重新计算单元计算出的所述各个驱动轴的角度的所述多关节机器人的驱动控制切换为基于由所述角度计算单元计算出的所述各个驱动轴的角度的所述多关节机器人的驱动控制时，使该各个驱动轴的角度在指定的变动范围内逐渐变动。
12.根据权利要求1至11中任一项所述的多关节型机器人的控制装置，其特征在于: 所述速度计算单元计算加速度来代替速度，所述重新计算单元以及所述驱动指示单元基于加速度来代替速度进行判别。
13.一种多关节型机器人的控制方法，其特征在于: 该多关节型机器人包括:第一关节驱动系统，在前端具有按照作业对象的作业线移动的作业部，包括与该作业部连接并使所述作业部的姿势发生变化的三个驱动轴；以及第二关节驱动系统，与所述第一关节驱动系统连接，包括使该第一关节驱动系统的位置发生变化的至少三个驱动轴， 该多关节型机器人的控制方法包括: 插值数据计算步骤，在所述多关节型机器人的基础坐标系中，计算表示对提示路径进行插值的多个插值点的所述作业部的位置及当时的姿势的数据，该提示路径连接预先提示的所述作业部的作业开始位置及当时的姿势和作业结束位置及当时的姿势； 角度计算步骤，根据由所述插值数据计算步骤计算出的数据求出逆运动学问题的解，据此计算表示所述插值点的所述作业部的位置及当时的姿势的、包含所述第一关节驱动系统及所述第二关节驱动系统的所有驱动轴的角度； 速度计算步骤，基于由所述角度计算步骤计算出的下一个所述插值点与当前的所述插值点的所述第一关节驱动系统的所述各驱动轴的角度的差，计算使所述作业部的姿势移动到下一个所述插值点时的所述第一关节驱动系统的两端的两个驱动轴的速度； 姿势数据变换步骤，将由所述插值数据计算步骤计算出的数据变换为姿势数据，该姿势数据表示包含所述作业部的移动方向的轴、垂直于该轴的方向的轴、以及垂直于这两个轴的轴的作业坐标系中的所述作业部的姿势； 重新计算步骤，在由所述速度计算步骤计算出的所述第一关节驱动系统的两端的两个驱动轴的一者或两者的速度在预先确定的允许范围外的情况下，不变更所述作业部的移动速度，并且对于由所述姿势数据变换步骤变换的下一个所述插值点的所述作业部的姿势数据，抑制一个或两个特定成分的变动，同时重新计算所述第一关节驱动系统的两端的两个驱动轴的速度成为所述允许范围内的所述第一关节驱动系统的各个驱动轴的角度，基于该计算出的所述第一关节驱动系统的各个驱动轴的角度和由所述插值数据计算步骤计算出的所述作业部的位置，重新计算所述第二关节驱动系统的各个驱动轴的角度；以及 驱动指示步骤，在由所述速度计算步骤计算出的所述第一关节驱动系统的两端的两个驱动轴的两者的速度在所述允许范围内的情况下，基于由所述角度计算步骤计算出的所述各个驱动轴的角度，驱动所述多关节型机器人，在由所述速度计算步骤计算出的所述第一关节驱动系统的两端的两个驱动轴的一者或两者的速度在所述允许范围外的情况下，基于由所述重新计算步骤计算出的所述各个驱动轴的角度，驱动所述多关节型机器人。
14.一种多关节型机器人的控制程序，其特征在于: 该控制程序由控制所述多关节型机器人的计算机执行，所述多关节型机器人包括:第一关节驱动系统，在前端具有按照作业对象的作业线移动的作业部，包括与该作业部连接并使所述作业部的姿势发生变化的三个驱动轴；以及第二关节驱动系统，与所述第一关节驱动系统连接，包括使该第一关节`驱动系统的位置发生变化的至少三个驱动轴， 该多关节型机器人的控制程序使计算机执行如下步骤: 插值数据计算步骤，在所述多关节型机器人的基础坐标系中，计算表示对提示路径进行插值的多个插值点的所述作业部的位置及当时的姿势的数据，该提示路径连接预先提示的所述作业部的作业开始位置及当时的姿势和作业结束位置及当时的姿势； 角度计算步骤，根据由所述插值数据计算步骤计算出的数据求出逆运动学问题的解，据此计算表示所述插值点的所述作业部的位置及当时的姿势的、包含所述第一关节驱动系统及所述第二关节驱动系统的所有驱动轴的角度； 速度计算步骤，基于由所述角度计算步骤计算出的下一个所述插值点与当前的所述插值点的所述第一关节驱动系统的所述各驱动轴的角度的差，计算使所述作业部的姿势移动到下一个所述插值点时的所述第一关节驱动系统的两端的两个驱动轴的速度； 姿势数据变换步骤，将由所述插值数据计算步骤计算出的数据变换为姿势数据，该姿势数据表示包含所述作业部的移动方向的轴、垂直于该轴的方向的轴、以及垂直于这两个轴的轴的作业坐标系中的所述作业部的姿势； 重新计算步骤，在由所述速度计算步骤计算出的所述第一关节驱动系统的两端的两个驱动轴的一者或两者的速度在预先确定的允许范围外的情况下，不变更所述作业部的移动速度，并且对于由所述姿势数据变换步骤变换的下一个所述插值点的所述作业部的姿势数据，抑制一个或两个特定成分的变动，同时重新计算所述第一关节驱动系统的两端的两个驱动轴的速度成为所述允许范围内的所述第一关节驱动系统的各个驱动轴的角度，基于该计算出的所述第一关节驱动系统的各个驱动轴的角度和由所述插值数据计算步骤计算出的所述作业部的位置，重新计算所述第二关节驱动系统的各个驱动轴的角度；以及 驱动指示步骤，在由所述速度计算步骤计算出的所述第一关节驱动系统的两端的两个驱动轴的两者的速度在所述允许范围内的情况下，基于由所述角度计算步骤计算出的所述各个驱动轴的角度，驱动所述多关节型机器人，在由所述速度计算步骤计算出的所述第一关节驱动系统的两端的两个驱动轴的一者或两者的速度在所述允许范围外的情况下，基于由所述重新计算步骤计算 出的所述各个驱动轴的角度，驱动所述多关节型机器人。
【文档编号】B23K9/12GK103429398SQ201280012110
【公开日】2013年12月4日 申请日期:2012年3月2日 优先权日:2011年3月8日
【发明者】大根努, 西村利彦 申请人:株式会社神户制钢所
相关技术
网友询问留言
已有0条留言
- 还没有人留言评论。精彩留言会获得点赞！
1