CN103429397A - 多关节型机器人的控制装置、控制方法以及控制程序 - Google Patents

Abstract

本发明提供多关节型机器人的控制装置及方法，旨在维持多关节型机器人的作业部的位置以及移动速度的同时使第1关节驱动系统的驱动轴各自的角度可靠地到达作业结束点的角度。在例外条件未满足的情况下，基于步骤(S5)中所计算的插补点来驱动第1以及第2关节驱动系统的驱动轴的每一个轴(S64)。在例外条件满足后(S61的“是”侧)，至作业部到达作业结束点为止的期间(S12的“否”侧)，计算用于以作业结束点的角度为目标使第1关节驱动系统的驱动轴各自线性地变化的该第1关节驱动系统的驱动轴各自的角度(S8)，并基于该计算出的第1关节驱动系统的驱动轴各自的角度和步骤(S5)中计算出的插补点处的作业部的位置来计算第2关节驱动系统的驱动轴各自的角度(S9)，且遵照该计算结果来使第1以及第2关节驱动系统的驱动轴进行驱动(S11)。

Description

多关节型机器人的控制装置、控制方法以及控制程序

技术领域

本发明涉及具备第1关节驱动系统和第2关节驱动系统而成的多关节型机器人的控制技术，第1关节驱动系统具有使设置于前端的作业部的姿势进行变化的3个驱动轴，第2关节驱动系统具有使所述第1关节驱动系统的位置进行变化的至少3个驱动轴。

背景技术

作为图23所示的多关节型机器人的一种的6轴机械手具备：第1关节驱动系统，其具有使设置于前端的末端执行器(作业部)的姿势进行变化的J4轴、J5轴、J6轴；以及第2关节驱动系统，其具有使该第1关节驱动系统的位置进行变化的J1轴、J2轴、J3轴。而且，6轴机械手的控制装置基于预先设定的作业开始点以及作业结束点各自的位置以及姿势来控制J1轴～J6轴，从而使末端执行器的位置以及姿势进行变化。

更具体而言，计算对连结作业开始点与作业结束点的示教路径进行插补的多个插补点，并遵照该插补点使J1轴～J6轴驱动。此外，在给出了末端执行器的位置座标X、Y、Z以及姿势角α、β、γ时，通过求取逆运动学问题的解，能求取J1轴～Ｊ6轴的各关节角θ1～θ6。但此时作为逆运动学问题的解而得到的关节角θ1～θ6存在两组解。具体地，两组解中的J4轴的角度θ4，θ′4、J5轴的角度θ5，θ′5、J6轴的角度θ6，θ′6处于θ4-θ′4＝±180度，θ5+θ′5＝0度，θ6-θ′6＝±180度的关系。

而若在6轴机械手处J5轴的角度变为0度，则将取J4轴、J6轴的速度急剧变化的所谓的奇异姿势(奇异点)。故而，一般通过以与作业开始点处的解相同的符号直接使J5轴的角度移转，从而避开了奇异姿势。但在此情况下，在作业开始点以及作业结束点处的J5轴的角度的符号为不同符号时，不能使该J5轴的角度到达作业结束点处的角度。

与此相对，例如在专利文献1中公开了：在检测到处于奇异姿势附近的情况下，转移到逆运动学问题的另一解。根据这样的构成，即使在作业开始点以及作业结束点处的J5轴的角度的符号为不同符号的情况下，也能使该J5轴的角度到达作业结束点处的角度。此外，奇异姿势的检测一般是以J5轴的角度到达0度附近或J4轴以及J6轴的角度急剧变化为条件来进行的。

先行技术文献

专利文献

专利文献1：日本特开平02-218569号公报

发明要解决的课题

然而，在所述专利文献1所涉及的构成中，由于到达了奇异姿势附近是向逆运动学问题的另一解的转移条件，因此在未接近至奇异点姿势而进行动作的情况下，不能使驱动轴到达作业结束点的正确的角度。

具体地，图24示出了尽管作业开始点以及作业结束点处的J5轴的角度的符号是不同符号、但J5轴的角度未到达0度附近的前提下J4轴和J6轴的角度均未急剧变化而进行动作的情况下的轨迹。在此情况下，不能检测接近至奇异姿势附近。故而，J5轴的角度以作业开始点处的符号直接移动，因此不能使J4轴～J6轴的角度到达作业结束点的正确的角度。故而，例如有在朝着作业结束点的下一示教点进行动作的情况下J4轴～J6轴的角度会到达工作范围外的风险。

发明内容

为此，本发明鉴于上述事实而提出，其目的在于，提供能在维持作业部的位置以及移动速度的同时使第1关节驱动系统的驱动轴各自的角度可靠地到达作业结束点的角度的多关节型机器人的控制装置、控制方法以及控制程序。

用于解决课题的手段

为了达成上述目的，本发明应用于对多关节型机器人进行控制的多关节型机器人的控制装置，并构成为以具备以下的(1)～(4)为特征，其中，该多关节型机器人具备：第1关节驱动系统，其具有使设置于前端的作业部的姿势进行变化的3个驱动轴；以及第2关节驱动系统，其具有使所述第1关节驱动系统的位置进行变化的至少3个驱动轴。

(1)插补点计算单元，其计算多个插补点，所述插补点用于在对预先设定的作业开始点以及作业结束点各自的位置以及姿势进行连结的示教路径上使所述作业部的位置以预先规定的速度进行移动。

(2)驱动控制单元，其遵照由所述插补点计算单元计算出的所述插补点来使所述第1关节驱动系统以及所述第2关节驱动系统进行驱动。

(3)例外条件判定单元，其在由所述驱动控制单元对所述第1关节驱动系统以及所述第2关节驱动系统进行了驱动时，判定与所述第1关节驱动系统的奇异姿势的检测条件不同的预先设定的例外条件是否满足。

(4)例外动作单元，其在由所述例外条件判定单元判定为所述例外条件满足后，至所述作业部到达所述作业结束点为止的期间，计算用于以所述作业结束点的角度为目标使所述第1关节驱动系统的驱动轴各自线性地变化的该第1关节驱动系统的驱动轴各自的角度，并基于该计算出的第1关节驱动系统的驱动轴各自的角度和由所述插补点计算单元计算出的所述插补点处的所述作业部的位置来计算所述第2关节驱动系统的驱动轴各自的角度，且遵照该计算结果来使所述第1关节驱动系统以及所述第2关节驱动系统的驱动轴各自进行驱动。

由此，能与所述作业开始点以及所述作业结束点处的所述第1关节驱动系统的中央的驱动轴的符号的异同无关地，在维持由所述插补点计算单元计算出的所述插补点处的所述作业部的位置以及移动速度的同时，使所述第1关节驱动系统的驱动轴各自的角度可靠地到达所述作业结束点的角度。

例如，所述例外条件是：在从下一所述插补点起的所述作业部的剩余的移动时间或移动距离的期间使所述第1关节驱动系统的驱动轴各自的角度到达所述作业结束点处的角度的情况下，所述第1关节驱动系统的至少一个驱动轴的速度超过预先设定的容许范围。由此，能在将所述第1关节驱动系统的驱动轴各自的速度抑制到所述容许范围内的同时使该第1关节驱动系统的驱动轴各自的角度可靠地到达所述作业结束点的角度。另外，所述例外条件是：所述作业部的剩余的移动时间或移动距离达到预先设定的给定值以下。

进而，所述例外条件是：下一所述插补点处的所述第1关节驱动系统的至少一个驱动轴的角度偏离基于所述作业开始点和/或所述作业结束点而确定的基准角度预先设定的给定角度以上。由此，例如能预防所述第1关节驱动系统的驱动轴各自的角度过分偏离所述基准角度从而到达所述作业结束点的角度的速度变得迅急的情形，从而使该第1关节驱动系统的驱动轴各自可靠地到达所述作业结束点。

另外，所述例外条件可以是：下一所述插补点处的所述第1关节驱动系统的至少一个驱动轴的角度超过预先设定的工作范围。由此，能在不超过所述工作范围的范围内使所述第1关节驱动系统的驱动轴各自进行驱动而到达所述作业结束点的角度。

通过具备检测所述作业部与加工对象物的加工线的偏差量的偏差量检测单元、以及基于由所述偏差量检测单元检测出的偏差量使所述作业部仿形所述加工线进行移动的仿形控制单元在内的构成，所述作业结束点的位置会变化。

于是，考虑还具备在所述作业结束点加上由所述偏差量检测单元检测出的偏差量来更新该作业结束点的作业结束点更新单元，且所述例外条件判定单元或所述例外动作单元基于由所述作业结束点更新单元更新后的所述作业结束点来执行处理。由此，即使在由所述仿形控制单元执行仿形控制的情况下，所述例外动作单元也能使所述驱动轴各自到达由所述作业结束点更新单元更新后的作业结束点。所述例外条件判定单元所执行的所述例外条件的判定也能以由所述作业结束点更新单元更新后的作业结束点为基准来适当地进行。

进而，还考虑所述例外动作单元具备：角度候补计算单元，其在较之于以所述作业结束点的角度为目标使所述第1关节驱动系统的驱动轴各自进行线性动作的情况下离该目标更近的角度的范围内，将该驱动轴各自的角度的候补计算多个；以及角度选择单元，其从由所述角度候补计算单元计算出的角度的候补之中，选择对在采用了各该候补时的下一所述插补点的从所述作业部观察的作业坐标系下的3个姿势角度的分量当中预先设定的一个或二个特定分量的变动而言抑制效果最好的角度的候补。

由此，能在抑制所述特定分量的变动的同时使所述第1关节驱动系统的驱动轴各自可靠地到达所述作业结束点处的角度。

另外，本发明还能采用为多关节型机器人的控制方法或控制程序的发明。

即，还能体现为具备如下工序的多关节型机器人的控制方法或使处理器执行如下工序的控制程序，所述工序包括：插补点计算工序，计算多个插补点，所述插补点用于在对预先设定的作业开始点以及作业结束点各自的位置以及姿势进行连结的示教路径上使所述作业部的位置以预先规定的速度进行移动；驱动控制工序，遵照由所述插补点计算工序计算出的所述插补点来使所述第1关节驱动系统以及所述第2关节驱动系统进行驱动；例外条件判定工序，在由所述驱动控制工序对所述第1关节驱动系统以及所述第2关节驱动系统进行了驱动时，根据与所述第1关节驱动系统的奇异姿势的检测条件不同的预先设定的例外条件，来判定至所述第1关节驱动系统的驱动轴各自到达所述作业结束点的角度为止的期间该第1关节驱动系统的至少一个驱动轴的速度或角度是否超过预先设定的给定范围；以及例外动作工序，在由所述例外条件判定工序判定为所述例外条件满足后，取代所述驱动控制工序，而使所述第1关节驱动系统以及所述第2关节驱动系统进行驱动，使得所述第1关节驱动系统的驱动轴各自以所述作业结束点的角度为目标线性地变化，且所述作业部的位置以预先规定的速度进行移动。

发明效果

根据本发明，能与所述作业开始点以及所述作业结束点处的所述第1关节驱动系统的中央的驱动轴的符号的异同无关地，在维持由所述插补点计算单元计算出的所述插补点处的所述作业部的位置以及移动速度的同时，使所述第1关节驱动系统的驱动轴各自的角度可靠地到达所述作业结束点的角度。

附图说明

图1是表征本发明的实施方式所涉及的多关节型机器人的控制装置1以及多关节型机器人2的概略构成的框图。

图2是表示多关节型机器人2的关节驱动系统的模型图。

图3是表示由本发明的实施方式所涉及的多关节型机器人的控制装置1执行的机器人控制处理的过程的一例的流程图。

图4是表示现有的驱动轴34～36的动作例的图。

图5是表示在执行了实施方式所涉及的机器人控制处理时的驱动轴34～36的动作例的图。

图6是表示现有的驱动轴34～36的动作例的图。

图7是表示在执行了实施例1所涉及的机器人控制处理时的驱动轴34～36的动作例的图。

图8是用于说明机器人控制处理中的线性动作的变形例的图。

图9是表示实施例2所涉及的机器人控制处理的过程的一例的流程图。

图10是表示在执行了实施例2所涉及的机器人控制处理时的驱动轴34～36的动作例的图。

图11是表示在一并执行仿形控制以及机器人控制处理时的末端执行器2的动作例的图。

图12是表示特定分量抑制处理的过程的一例的流程图。

图13是用于说明焊接线座标系∑line的图。

图14是用于说明焊接线座标系∑line的图。

图15是用于说明焊接线座标系∑line的图。

图16是用于说明焊接线座标系∑line的图。

图17是用于说明特定分量抑制处理的执行结果的图。

图18是用于说明特定分量抑制处理的执行结果的图。

图19是用于说明特定分量抑制处理的执行结果的图。

图20是用于说明特定分量抑制处理的执行结果的图。

图21是用于说明特定分量抑制处理的执行结果的图。

图22是用于说明特定分量抑制处理的执行结果的图。

图23是表示一般的多关节型机器人的关节驱动系统的模型图。

图24是表示在未接近奇异姿势的情况下的现有的驱动轴J4轴～J6轴的动作例的图。

图25是用于说明对使奇异姿势处于避开中进行报知的装置的图。

具体实施方式

以下，参照附图来说明本发明的实施方式，以帮助理解本发明。此外，以下的实施方式只是使本发明具体化的一例，并不是为了限定本发明的技术范围。

如图1所示，本发明的实施方式所涉及的多关节型机器人的控制装置1(以下简称为“控制装置1”)对前端设置有末端执行器2的多关节型机器人3的动作进行控制，具备：运算部11、存储部12、以及操作部13等。所述末端执行器2是对加工对象物施加例如焊接或涂装等作用的焊接器(焊炬)、涂装器、工具、捕捉器等。

所述存储部12具有：将给定的控制程序或各种处理中所参照的参数信息等预先进行存储的ROM或HDD等、以及作为由所述运算部11执行各种处理时的作业区域(临时存储区域)进行利用的EEPROM等的易失性存储器。

所述操作部13是具有板键或操作按钮、操作杆等的用户界面。所述操作部13将与用户操作对应的操作信号输入至所述运算部11。具体地，通过由用户操作所述操作部13，来设定所述多关节型机器人3的作业开始点、作业结束点、作业时间、以及移动速度等各种加工参数。

所述运算部11是遵照所述存储部12中所存储的所述控制程序来执行后述的机器人控制处理(参照图3)等各种处理的CPU等的处理器。此外，本发明可以体现为用于使所述运算部11执行后述的机器人控制处理的控制程序的发明。

在此，图2是表示所述多关节型机器人3的关节驱动系统的模型图。

如图2所示，所述多关节型机器人3具备：第1关节驱动系统，其具有使所述末端执行器2的姿势进行变化的3个驱动轴34～36；以及第2关节驱动系统，其具有与该第1关节驱动系统的后端的驱动轴34连结来使该第1关节驱动系统的位置进行变化的3个驱动轴31～33。所述驱动轴31～36各自由电动机等驱动单元例如在±180度的范围内单独地旋转驱动。此外，所述第2关节驱动系统可以是具有4轴以上的驱动轴的构成。

所述多关节型机器人3中的所述末端执行器2的位置·姿势通过该多关节型机器人3的基本座标系∑base下的位置座标XYZ和姿势角αβγ来表征。所述姿势角αβγ以欧拉角或辊距偏航角等来表征。在此，所述第2关节驱动系统能与所述第1关节驱动系统的驱动轴34～36的动作无关地，自由地生成所述多关节型机器人3的基本座标系∑base下的所述末端执行器2的位置座标XYZ。

此外，所述多关节型机器人3中的所述驱动轴31～36各自的角度θ1～θ6能通过从所述多关节型机器人3的基本座标系∑base下的位置座标XYZ、姿势角αβγ求取逆运动学问题的解来进行计算。

以下，遵照图3的流程图，来说明由所述控制装置1执行的机器人控制处理的过程的一例。在此，图3所示的S1，S2，…表示处理过程(步骤)编号。此外，本发明可以体现为执行以下的处理过程的各工序的多关节型机器人的控制方法的发明。

所述控制装置1通过执行该机器人控制处理，来基于由所述操作部13预先设定的作业开始点以及作业结束点对所述多关节型机器人3进行控制。在此，设通过所述操作部13的操作来预先设定了作业开始点Q1的位置·姿势{Xs，Ys，Zs，αs，βs，γs}、以及作业结束点Q2的位置·姿势{Xe，Ye，Ze，αe，βe，γe}。

(步骤S1)

首先，在步骤S1中，所述控制装置1求取路径方程式P(t)＝Q1+ΔP·t，该路径方程式用于计算插补对所述作业开始点Q1以及所述作业结束点Q2各自的位置以及姿势进行连结的示教路径的多个插补点。在此，ΔP表示每单位时间(1msec)的所述末端执行器2的位置的移动量，t表示从所述作业开始点Q1起计时的控制时间[msec]。此外，以所述路径方程式P(t)所计算的插补点通过所述多关节型机器人3的基本座标系∑base下的位置·姿势XYZαβγ(t)来表征。

具体地，所述路径方程式P(t)计算用于使所述末端执行器2的位置以预先规定的速度从所述作业开始点Q1起移动到所述作业结束点Q2的多个插补点。具体地，若将所述末端执行器2的位置从所述作业开始点Q1起移动到所述作业结束点Q2时的总控制时间设为T[msec]，将所述末端执行器2的位置·姿势XYZαβγ的每单位时间(1msec)的变化量设为ΔX、ΔY、ΔZ、Δα、Δβ、Δγ，则ΔP通过(Q2-Q1)／T＝{ΔX＝(Xe-Xs)／T，ΔY＝(Ye-Ys)／T，ΔZ＝(Ze-Zs)／T，Δα＝(αe-αs)／T，Δβ＝(βe-βs)／T，Δγ＝(γe-γs)／T}来表征。当然，不限于设定所述总控制时间T的情况，还可以将所述末端执行器2的移动速度设定为预先规定的速度。

(步骤S2～S3)

接着，在步骤S2中，所述控制装置1通过求取所述作业结束点Q2处的所述末端执行器2的位置·姿势{Xe，Ye，Ze，αe，βe，γe}的逆运动学问题的解，来计算该作业结束点Q2处的所述驱动轴31～36的角度{θ1e，θ2e，θ3e，θ4e，θ5e，θ6e}。此外，在基于所述操作部13的示教时可以直接设定所述作业结束点Q2处的所述驱动轴31～36的角度θ1e～θ6e。

另外，在步骤S3中，所述控制装置1将控制时间t以及控制开始标志F初始设定为0。此外，所述控制开始标志F是表示后述的例外条件(S61)是否已满足的信息，在后述的步骤S62中被设定为1。

(步骤S4～S6)

接下来，在步骤S4中，所述控制装置1更新所述控制时间t(t＝t+Δt)。在此，Δt是所述控制装置1的所述多关节型机器人3的控制周期(采样周期)，例如是10[msec]或100[msec]。

然后，在步骤S5中，所述控制装置1基于所述路径方程式P(t)来计算下一插补点处的所述驱动轴31～36各自的角度θ1(t)～θ6(t)。在此，执行相应的计算处理时的所述控制装置1相当于插补点计算单元。

具体地，首先，所述控制装置1计算下一插补点的位置·姿势XYZαβγ(t)＝{Xs+ΔX·t，Ys+ΔY·t，Zs+ΔZ·t，αs+Δα·t，βs+Δβ·t，γs+Δγ·t}。然后，所述控制装置1通过求取所述位置·姿势XYZαβγ(t)的逆运动学问题的解，来计算下一插补点处的所述驱动轴31～36各自的角度θ1(t)、θ2(t)、θ3(t)、θ4(t)、θ5(t)、θ6(t)。此外，在将所述控制开始标志F设定为1后，不使用所述角度θ1(t)～θ6(t)，因此该步骤S5的处理可以省略。

其后，在步骤S6中，所述控制装置1判断所述控制开始标志F是否为1。在此，若判断为所述控制开始标志F是1(S6的“是”侧)，则处理转移至步骤S7，若判断为所述控制开始标志F是0(S6的“否”侧)，则处理转移至步骤S61。

(步骤S61)

在步骤S61中，所述控制装置1判定与所述第1关节驱动系统的奇异姿势的检测条件不同的预先设定的例外条件是否满足。在此，该步骤S61是在后述的步骤S64中所述第1关节驱动系统以及所述第2关节驱动系统的驱动轴31～36各自基于所述步骤S5中计算出的角度θ1(t)～θ6(t)被驱动时来执行的。在此，执行相应的判定处理时的所述控制装置1相当于例外条件判定单元。所述第1关节驱动系统的奇异姿势的检测条件是指，例如所述驱动轴35的角度接近0或所述驱动轴34、36的角度发生了急剧变化。

具体地，在本实施方式中，在从下一插补点起的所述末端执行器2的剩余的移动时间的期间使所述第1关节驱动系统的驱动轴34～36各自的角度到达所述作业结束点Q2处的角度θ4e～θ6e的情况下，将所述第1关节驱动系统的驱动轴34～36的至少一个驱动轴的速度超过预先设定的容许范围预先设定为所述例外条件。即，所述控制装置1在该步骤S61中，在所述末端执行器2的剩余的移动时间的期间，判定是否能在将所述驱动轴34～36各自的速度维持在所述容许范围内的同时到达所述作业结束点Q2。

例如，所述控制装置1在以下的(11)式～(13)式均成立的情况下，能判定为所述例外条件已满足。在此，T(t)是下一插补点处的剩余的移动时间，θ4d、θ5d、θ6d是所述驱动轴34～36的速度的容许范围的上限值。

|(θ4e-θ4(t))／T(t)|＞θ4d…(11)

|(θ5e-θ5(t))／T(t)|＞θ5d…(12)

|(θ6e-θ6(t))／T(t)|＞θ6d…(13)

此外，不限于所述末端执行器2的剩余的移动时间，还可以在该末端执行器2的剩余的移动距离(＝剩余的移动时间×移动速度)之间，判定是否能在将所述驱动轴34～36各自的速度维持在容许范围内的同时，使所述末端执行器2到达所述作业结束点Q2。另外，作为其他的实施例，还考虑仅将所述末端执行器2的剩余的移动时间或剩余的移动距离达到预先设定的给定值以下作为所述例外条件进行使用。

然后，在该步骤S61中若判断为所述例外条件未满足(S61的“否”侧)，则处理转移至步骤S64，若判断为所述例外条件已满足(S61的“是”侧)，则处理转移至步骤S62。

(步骤S64)

在步骤S64中，所述控制装置1基于所述步骤S5中计算出的角度θ1(t)、θ2(t)、θ3(t)、θ4(t)、θ5(t)、θ6(t)来使所述驱动轴31～36各自进行驱动，使处理转移至步骤S12。此外，该步骤S64中的处理与现有技术相同，故省略详细的说明。在此，执行相应的驱动处理时的所述控制装置1相当于驱动控制单元。

(步骤S62～S63)

另一方面，若所述例外条件满足，则在接下来的步骤S62中，所述控制装置1将所述控制开始标志F设定为1。由此，今后，在所述步骤S6中，若判断为所述控制开始标志F为1，则处理不经由所述步骤S61～S63而转移至后述的步骤S7。

接下来，在步骤S63中，所述控制装置1将所述驱动轴34～36各自的当前的角度设为θ4_OLD、θ5_OLD、θ6_OLD而存储至所述存储部12。即，θ4_OLD＝θ4(t-Δt)，θ5_OLD＝θ5(t-Δt)，θ6_OLD＝θ6(t-Δt)。

(步骤S7～S8)

然后，在步骤S7中，所述控制装置1在从当前的插补点起的剩余的移动时间T(t-Δt)的期间，计算使所述驱动轴34～36各自的角度到达所述作业结束点Q2的角度所需的所述驱动轴34～36各自的每单位时间(1msec)的速度Vθ4、Vθ5、Vθ6。在此，速度Vθ4、Vθ5、Vθ6是用于使当前的所述驱动轴34～36各自的角度θ4_OLD、θ5_OLD、θ6_OLD线性地变化至所述作业结束点Q2处的角度θ4e、θ5e、θ6e的速度。具体地，速度Vθ4、Vθ5、Vθ6能通过以下的(21)式～(23)式来进行计算。

Vθ4＝(θ4e-θ4_OLD)／T(t-Δt)…(21)

Vθ5＝(θ5e-θ5_OLD)／T(t-Δt)…(22)

Vθ6＝(θ6e-θ6_OLD)／T(t-Δt)…(23)

接下来，在步骤S8中，所述控制装置1基于所述步骤S7中计算出的速度Vθ4、Vθ5、Vθ6，来计算下一插补点的所述驱动轴34～36的角度θ4_NOW、θ5_NOW、θ6_NOW。在此，角度θ4_NOW、θ5_NOW、θ6_NOW能通过以下的(31)式～(33)式来计算。

θ4_NOW＝θ4_OLD+Vθ4·Δt…(31)

θ5_NOW＝θ5_OLD+Vθ5·Δt…(32)

θ6_NOW＝θ6_OLD+Vθ6·Δt…(33)

(步骤S9～S10)

然后，在步骤S9中，所述控制装置1基于所述步骤S8中计算出的下一插补点的角度θ4_NOW、θ5_NOW、θ6_NOW、以及所述步骤S5中计算出的下一插补点的位置座标XYZ(t)，来计算所述第2关节驱动系统的驱动轴31～33各自的下一插补点处的角度θ1_NOW、θ2_NOW、θ3_NOW。

在此所计算的所述驱动轴31～33的角度θ1_NOW、θ2_NOW、θ3_NOW维持了所述步骤S5中计算出的下一插补点的位置座标XYZ(t)，因此所述末端执行器2的位置以及移动速度维持了所述步骤S64的驱动控制时的所述末端执行器2的位置以及移动速度。

此外，所述控制装置1在步骤S10中为了将所述驱动轴34～36各自的角度θ4_NOW、θ5_NOW、θ6_NOW在下一控制周期中进行使用而作为θ4_OLD、θ5_OLD、θ6_OLD进行存储(θ4_OLD＝θ4_NOW，θ5_OLD＝θ5_NOW，θ6_OLD＝θ6_NOW)。

(步骤S11～S12)

其后，在步骤S11中，所述控制装置1基于所述步骤S8中计算出的下一插补点处的所述驱动轴34～36的角度θ4_NOW、θ5_NOW、θ6_NOW、以及所述步骤S9中计算出的下一插补点处的所述驱动轴31～33的角度θ1_NOW、θ2_NOW、θ3_NOW，来使所述驱动轴31～36驱动。如此，在该机器人控制处理中，在所述例外条件已满足后，取代所述步骤S64中的驱动控制，而执行所述步骤S11中的驱动控制。

此外，所述控制装置1在接下来的步骤S12中，判断所述末端执行器2是否已到达所述作业结束点Q2，并直到该末端执行器2到达所述作业结束点Q2为止(S12的“否”侧)，反复执行所述步骤S4以后的处理。另一方面，若所述末端执行器2到达所述作业结束点Q2(S12的“是”侧)，则结束该机器人控制处理。

如以上说明所述，本发明的实施方式所涉及的所述控制装置1在判定为所述例外条件已满足后，直到所述末端执行器2到达所述作业结束点Q2为止的期间，计算用于以所述作业结束点Q2的角度为目标使所述第1关节驱动系统的驱动轴34～36各自线性地变化的该第1关节驱动系统的驱动轴34～36各自的角度，并基于该计算出的第1关节驱动系统的驱动轴34～36各自的角度和通过所述步骤S5计算出的插补点处的所述末端执行器2的位置来计算所述第2关节驱动系统的驱动轴31～33各自的角度，且遵照该计算结果来使所述第1关节驱动系统以及所述第2关节驱动系统的驱动轴各自进行驱动。在此，执行相应的处理时的所述控制装置1相当于例外动作单元。

因此，能在维持所述步骤S5中计算出的插补点处的所述末端执行器2的位置以及移动速度的同时，使所述第1关节驱动系统的驱动轴34～36各自的角度可靠地到达所述作业结束点Q2的角度。由此，例如能防止在从所述作业结束点Q2起朝着下一示教点进行动作的情况下所述驱动轴34～36超过工作范围的情形。

所述控制装置1在所述作业开始点Q1以及所述作业结束点Q2的所述驱动轴35的角度的符号为不同符号、且若不经历所述第1关节驱动系统的奇异姿势(驱动轴35的角度为0度)则不能使所述驱动轴35的角度到达所述作业结束点Q2的角度的情况下特别适用。以下，参照图4以及图5来说明该点。在此，图4以及图5示出在所述作业开始点Q1以及所述作业结束点Q2处的所述驱动轴35的符号为不同符号的情况下的动作例，图4示出现有例，图5示出所述机器人控制处理(图3)的执行结果。具体地，所述作业开始点Q1处的所述驱动轴35的角度θ5s为+65度，所述作业结束点Q2处的所述驱动轴35的角度θ5e为-65度。

如图4所示，通常，在所述作业开始点Q1以及所述作业结束点Q2处的所述驱动轴35的角度θ5s、θ5e的符号为不同符号的情况下，所述末端执行器2以维持了所述作业开始点Q1处的所述驱动轴35的符号的解直接进行移动，因此所述驱动轴34～36各自不到达所述作业结束点Q2处的角度。故而，例如在从所述作业结束点Q2起朝着下一示教点进行动作的情况下，存在所述驱动轴34～36脱离工作范围的风险。另外，即使是以所述驱动轴35接近0度或所述驱动轴34、36的速度成为给定值以上为条件来检测所述第1关节驱动系统的奇异姿势并转移至不同的解的构成，如图4所示，在所述驱动轴35未接近0度、所述驱动轴34、36的速度未成为给定值以上的情况下，也不能检测所述第1关节驱动系统的奇异姿势。

与此相对，如图5所示，在执行所述机器人控制处理(图3)的情况下，若判定为所述例外条件已满足，则从该时间点L1起驱动所述驱动轴31～36以使所述驱动轴34～36朝着所述作业结束点Q2的角度线性地变化，因此能使该驱动轴34～36各自可靠地到达所述作业开始点Q2处的角度。

当然，即使在所述作业开始点Q1以及所述作业结束点Q2处的所述驱动轴35的符号为相同符号的情况下，也同样地，从所述例外条件已满足的时间点起，所述驱动轴34～36朝着所述作业结束点Q2处的角度线性地变化，因此能使该驱动轴34～36各自可靠地到达所述作业开始点Q2处的角度。

【实施例1】

在本实施例1中，针对所述机器人控制处理(参照图3)中的所述例外条件的其他的例子进行说明。

具体地，在本实施例1中，在所述步骤S61中，在所述驱动轴34～36的至少一个驱动轴的下一插补点处的角度超过预先设定的工作范围的情况下，所述控制装置1判定为所述例外条件已满足。例如，在预先设定的所述驱动轴34～36各自的正侧的工作范围为θ4+L、θ5+L、θ6+L、且所述驱动轴34～36各自的负侧的工作范围为θ4-L、θ5-L、θ6-L时，所述控制装置1在以下的(41)～(46)的至少一个成立了的情况下判定为所述例外条件已满足。此外，所述驱动轴34～36各自的工作范围例如指所述驱动轴34～36的驱动电动机的容许动作范围。

θ4(t)＞θ4+_L…(41)

θ4(t)＜θ4-_L…(42)

θ5(t)＞θ5+_L…(43)

θ5(t)＜θ5-_L…(44)

θ6(t)＞θ6+_L…(45)

θ6(t)＜θ6-_L…(46)

在此，图6以及图7示出在所述作业开始点Q1以及所述作业结束点Q2处的所述驱动轴35的符号为不同符号的情况下的动作例，图6示出现有例，图7示出本实施例1所涉及的机器人控制处理的执行结果。具体地，所述作业开始点Q1处的所述驱动轴35的角度θ5s为-10度，所述作业结束点Q2处的所述驱动轴35的角度θ5e为+10度。此外，所述驱动轴34～36的角度的工作范围设为-180度～+180度。

如图6所示，若与所述作业开始点Q1以及所述作业结束点Q2处的所述驱动轴35的符号是否为不同符号无关地，所述末端执行器2以维持了所述作业开始点Q1处的所述驱动轴35的符号的解直接进行移动，则存在所述驱动轴34、36的角度超过该驱动轴34，36的工作范围(±180度)从而异常停止的风险。

与此相对，如图7所示，在执行本实施例1所涉及的机器人控制处理的情况下，若判定为所述驱动轴34～36的至少一个驱动轴的下一插补点处的角度超过预先设定的工作范围这样的所述例外条件已满足，则驱动所述驱动轴31～36以使从其时间点L2起所述驱动轴34～36朝着所述作业结束点Q2的角度线性地变化。然后，一般在所述控制装置1中能将所述作业结束点Q2设定为仅在所述驱动轴34～36的角度的工作范围内。故而，能在使所述驱动轴34～36各自在容许范围内进行驱动的同时使所述作业开始点Q2的角度可靠地到达。当然，即使在所述作业开始点Q1以及所述作业结束点Q2处的所述驱动轴35的符号为相同符号的情况下也同样。

而在图7所示的动作例中，所述驱动轴34～36各自的角度在比所述作业结束点Q2稍微提前的时间点到达该作业结束点Q2的附近，其后，朝着所述作业结束点Q2处的角度以缓慢的速度进行动作。如此，若一点点地降下所述驱动轴34～36各自的速度来使得缓慢地停止至所述作业结束点Q2，则能抑制停止时的振动等。

特别地，此时期望所述控制装置1对所述驱动轴34～36各自的速度进行调节来使该驱动轴34～36各自同时到达所述作业结束位置Q2处的角度而停止。由此，较之于单独停止所述驱动轴34～36各自的驱动电动机的情况，能抑制在该驱动电动机的停止时产生的振动等。进而，还考虑校正该驱动轴34～36的角度以使所述例外条件满足的时间点L2上的所述驱动轴34～36各自的线性动作开始时的速度变缓。例如图8示出了在所述例外条件满足的前后的所述驱动轴36的角度的变化例。如图8中实线所示，若在所述驱动轴36的角度增加到180度后突然朝着负方向变化，则在该驱动轴36产生急剧的负方向的加速度，从而成为振动等的原因。于是，如图8中虚线所示，考虑事先估计到达所述驱动轴36的工作范围的上限值180，并校正该驱动轴36的角度以使从该时间点起所述驱动轴36的角度缓慢地朝着负方向变化。由此，在所述例外条件满足后不会立刻产生所述驱动轴36的急剧的加速度，因此能防止振动等。当然，关于所述驱动轴35、36也同样。

另外，如此用于防止所述驱动轴34～36的急剧的停止或急剧的加速度的控制手法还能同样适用于所述实施方式或后述的实施例2等的使用其他的例外条件的情况。

【实施例2】

在本实施例2中，说明所述机器人控制处理(参照图3)中的所述例外条件的又一例。

在此，图9是表示本实施例2所涉及的机器人控制处理的过程的一例的流程图。此外，针对与所述机器人控制处理(参照图3)同样的处理过程赋予相同的符号并省略其说明。

具体地，如图9所示，在本实施例2所涉及的机器人控制处理中，取代所述步骤S2～S4、S61(参照图3)而执行后述的步骤S21、S31、S41、S611。

(步骤S21、S31、S41)

在步骤S21中，所述控制装置1一并计算所述作业结束点Q2处的所述驱动轴34～36的角度θ4e、θ5e、θ6e、以及所述作业开始点Q1处的所述驱动轴34～36的角度θ4s、θ5s、θ6s。

另外，所述控制装置1在该步骤S21中还计算在使所述驱动轴34～36从所述作业开始点Q1起到所述作业结束点Q2为止以一定速度线性地动作时的每单位时间(lmsec)的所述驱动轴34～36的角度变动Δθ4、Δθ5、Δθ6。此时，若将所述作业开始点Q1处的所述末端执行器2的剩余的移动时间设为T(s)，则角度变动Δθ4、Δθ5、Δθ6通过以下的(51)～(53)来求取。

Δθ4＝(θ4e-θ4s)/T(s)…(51)

Δθ5＝(θ5e-θ5s)/T(s)…(52)

Δθ6＝(θ6e-θ6s)/T(s)…(53)

接下来，在步骤S31中，所述控制装置1不仅将控制时间t以及控制开始标志F初始设定为0，还进行与所述驱动轴34～36对应的基准角度θk(t)＝{θ4k，θ5k，θ6k}的初始设定。所述基准角度θk(t)在后述的步骤S611中在例外条件的判断中被参照，其初始值θk(0)被设定为{θ4k，θ5k，θ6k}＝{θ4s，θ5s，θ6s}。

另外，关于所述控制装置1，在步骤S41中，所述控制装置1更新所述控制时间t(t＝t+Δt)，并更新所述基准角度θk(t)。具体地，基准角度θk(t)成为：θ4k＝θ4k+Δθ4·Δt，θ5k＝θ5k+Δθ5·Δt，θ6k＝θ6k+Δθ6·Δt。

即，所述基准角度θk(t)是基于所述作业开始点Q1以及所述作业结束点Q2而确定的，被随时更新成对所述作业开始点Q1以及所述作业结束点Q2进行连结的直线上的值。此外，作为其他的实施例，还考虑所述基准角度θk(t)为所述作业开始点Q1或所述作业结束点Q2处的所述驱动轴34～36的角度(步骤S611)。

然后，在本实施例2所涉及的机器人控制处理中，在步骤S611的例外条件的判定处理中，所述控制装置1判断下一插补点处的所述第1关节驱动系统的驱动轴34～36的角度θ4(t)、θ5(t)、θ6(t)的至少一个与所述基准角度θ4k、θ5k、θ6k之间的偏离角度是否超过预先设定的给定角度θ4w、θ5w、θ6w。为了对所述驱动轴34～36的角度θ4(t)、θ5(t)、θ6(t)过分偏离所述基准角度θ4k、θ5k、θ6k从而为了使该驱动轴34～36到达所述作业结束点Q2会伴随该驱动轴34～36的角度的急剧的变化的可能性的情形进行事先估计，预先设定所述给定角度θ4w、θ5w、θ6w。例如，考虑将所述给定角度θ4w、θ5w、θ6w各自设定为90度、180度、90度。

例如，所述控制装置1在下述的(61)式～(63)式的任一者成立的情况下判定为所述例外条件已满足。

|θ4(t)-θ4k|＞θ4w…(61)

|θ5(t)-θ5k|＞θ5w…(62)

|θ6(t)-θ6k|＞θ6w…(63)

在此，图10示出在所述作业开始点Q1以及所述作业结束点Q2处的所述驱动轴35的符号为不同符号的情况下的动作例，示出本实施例2所涉及的所述机器人控制处理的执行结果。具体地，所述作业开始点Q1处的所述驱动轴35的角度θ5s为+10度，所述作业结束点Q2处的所述驱动轴35的角度θ5e为-10度。另外，在此以将所述给定角度θ6w设定为90度、且所述驱动轴36的角度θ6(t)偏离所述基准角度θ6k(t)90度的情况为例进行说明。当然，关于所述驱动轴34、35也同样。

如图10所示，在执行本实施例2所涉及的机器人控制处理的情况下，从判断为所述驱动轴36的下一插补点的角度θ6(t)偏离所述基准角度θ6k(t)90度以上的时间点L3起，驱动所述驱动轴31～36以使所述驱动轴34～36朝着所述作业结束点Q2的角度线性地变化，因此能使该驱动轴34～36各自可靠地到达所述作业开始点Q2处的角度。特别地，根据本实施例2所涉及的机器人控制处理，在所述驱动轴34～36各自的角度过分偏离所述基准角度θk(t)前的早期的阶段，能使该驱动轴34～36朝着所述作业结束点Q2线性动作，从而能将所述驱动轴34～36的急剧的角度变动防范于未然。

尽管所述控制装置1只要判断所述实施方式、所述实施例1、本实施例2中说明的多个所述例外条件的至少一个满足的有无即可，但在同时监视多个所述例外条件的满足的有无、且任一个例外条件已满足的情况下，还考虑执行所述步骤S7起以后的线性动作处理。由此，能具体实现同时起到所述实施方式、所述实施例1、本实施例2所得到的效果的构成。

【实施例3】

在本实施例3中，考虑所述控制装置1执行对所述多关节型机器人3进行控制的仿形控制以使所述末端执行器2仿形加工对象物的加工线进行移动的构成。在此情况下，在所述多关节型机器人3，设置对所述末端执行器2与所述加工线的偏差量ΔXYZαβγ(t)进行检测的传感器(相当于偏差量检测单元)。

在此，在各时间点由所述传感器检测的所述偏差量ΔXYZαβγ(t)是以通过所述路径方程式P(t)计算出的该时间点上的插补点为基准而表征的值。然后，所述控制装置1基于所述传感器的检测结果来执行使所述末端执行器2仿形所述加工线进行移动的所述仿形控制。在此，执行相应的仿形控制时的所述控制装置1相当于仿形控制单元。

具体地，所述控制装置1在所述机器人控制处理(参照图3)的步骤S5中计算下一插补点时，将由当前所述传感器检测出的偏差量ΔXYZαβγ(t)相加到所述路径方程式P(t)。另外，所述控制装置1通过根据该相加后的插补点的位置·姿势X′Y′Z′α′β′γ′(t)求取逆运动学问题的解，来计算下一插补点处的所述驱动轴31～36各自的角度θ1′(t)～θ6′(t)。然后，在所述例外条件未满足的情况下，所述控制装置1在所述步骤S64中基于以所述偏差量ΔXYZαβγ(t)修正后的所述角度θ1′(t)～θ6′(t)来驱动所述驱动轴31～36。

另外，所述控制装置1在所述步骤S5中将所述作业结束点Q2更新成对该作业结束点Q2相加了所述偏差量ΔXYZαβγ(t)后的值。此时，将更新后的所述作业结束点Q2处的所述驱动轴34～36各自的角度设为θ4e(t)、θ5e(t)、θ6e(t)。在此，执行相应的处理时的所述控制装置1相当于作业结束点更新单元。

由此，在所述例外条件的判断处理中，基于所述更新后的所述作业结束点Q2来判断所述例外条件的满足的有无。例如，作为所述实施方式中的所述步骤S61的例外条件的判定指标的上述(11)式～(13)式成为以下的(11′)式～(13′)式。

|(θ4e(t)-θ4(t))/T(t)|＞θ4d…(11′)

|(θ5e(t)-θ5(t))/T(t)|＞θ5d…(12′)

|(θ6e(t)-θ6(t))/T(t)|＞θ6d…(13′)

另外，在所述步骤S7中用于计算所述速度Vθ4、Vθ5、Vθ6的上述(21)式～(23)式成为以下的(21′)式～(23′)式。即，在所述步骤S11中的所述驱动处理中，以所述更新后的所述作业结束点Q2为目标来进行所述驱动轴34～36的线性动作处理。

Vθ4＝(θ4e(t)-θ4_OLD)/T(t-Δt)…(21′)

Vθ5＝(θ5e(t)-θ5_OLD)/T(t-Δt)…(22′)

Vθ6＝(θ6e(t)-θ6_OLD)/T(t-Δt)…(23′)

由此，即使在基于由所述传感器检测的偏差量来执行所述仿形控制的情况下，也能使所述驱动轴34～36各自的角度可靠地到达以所述仿形控制进行了修正后的作业结束点Q2。

具体地，如图11(a)～(c)所示，所述末端执行器2从所述作业开始点Q1起朝着所述作业结束点Q2移动，在通过所述仿形控制对所述偏差量ΔXYZαβγ(t)进行修正时，所述作业结束点Q2相加所述偏差量ΔXYZαβγ(t)而被更新成作业结束点Q2(t)。由此，能使所述驱动轴34～36各自的角度到达所述作业结束点Q2(t)。

【实施例4】

而在所述机器人控制处理(参照图3)中，不希望完全无视所述末端执行器2的姿势。于是，在本实施例4中，针对能抑制所述末端执行器2的姿势当中预先规定的特定分量的变动的构成进行说明。

具体地，本实施例4中，所述控制装置1在所述机器人控制处理(参照图3)的所述步骤S8与所述步骤S9之间执行后述的特定分量抑制处理(参照图12)。此外，关于所述机器人控制处理，不限于在所述实施方式中说明的处理，采用所述实施例1～3的任一者所记载的内容即可。

以下，遵照图12的流程图，来说明由所述控制装置1执行的特定分量抑制处理的过程的一例。

(步骤S81)

首先，在步骤S81中，所述控制装置1针对下一插补点，如图13所示，求取将所述末端执行器2移动的方向(行进方向)的轴作为X轴、将通过该X轴与重力方向的外积(X轴×重力方向)所表征的轴作为Y轴、且将通过该X轴与该Y轴的外积(X轴×Y轴)所表征的方向的轴作为Z轴的作业座标系(以下，称为“焊接线座标系∑line”)下的姿势角度。在此，所述焊接线座标系∑line下的所述末端执行器2的姿势角度如图14～图16所示，通过所述X轴的绕轴的旋转角即焊炬倾斜角Rx、所述Y轴的绕轴的旋转角即焊炬前进角Ry、以及所述Z轴的绕轴的旋转角即焊炬旋转角Rz来表征。

具体地，所述控制装置1将所述步骤S5中计算出的下一插补点的所述基本座标系∑base下的位置·姿势XYZαβγ(t)变换成所述焊接线座标系∑line下的姿势角度(焊炬倾斜角Rx，焊炬前进角Ry，焊炬旋转角Rz)。

例如，从所述基本座标系∑base到所述焊接线座标系∑line的变换遵照以下的(101)式～(107)式来进行。此外，将所述焊接线座标系∑line下的X轴、Y轴、Z轴的单位矢量设为Xm、Ym、Zm。

【数式1】

$\mathrm{line}{R}_{\mathrm{tool}}=\left[\begin{array}{ccc}1& 0& 0\\ 0& \cos \mathrm{Rx}& -\sin \mathrm{Rx}\\ 0& \sin \mathrm{Rx}& \cos \mathrm{Rx}\end{array}\right]\left[\begin{array}{ccc}\cos \mathrm{Ry}& 0& \sin \mathrm{Ry}\\ 0& 1& 0\\ -\sin \mathrm{Ry}& 0& \cos \mathrm{Ry}\end{array}\right]\left[\begin{array}{ccc}\cos \mathrm{Rz}& -\sin \mathrm{Rz}& 0\\ \sin \mathrm{Rz}& \cos \mathrm{Rz}& 0\\ 0& 0& 1\end{array}\right]=$

$=\left[\begin{array}{ccc}\cos \mathrm{Ry}\cos \mathrm{Rz}& -\cos \mathrm{Ry}\sin \mathrm{Rz}& \sin \mathrm{Ry}\\ \sin \mathrm{Rx}\sin \mathrm{Ry}\cos \mathrm{Rz}+\cos \mathrm{Rx}\sin \mathrm{Rz}& -\sin \mathrm{Rx}\sin \mathrm{Ry}\sin \mathrm{Rz}+\cos \mathrm{Rx}\cos \mathrm{Rz}& -\sin \mathrm{Rx}\cos \mathrm{Ry}\\ -\cos \mathrm{Rx}\sin \mathrm{Ry}\cos \mathrm{Rz}+\sin \mathrm{Rx}\sin \mathrm{Rz}& \cos \mathrm{Rx}\sin \mathrm{Ry}\sin \mathrm{Rz}+\sin \mathrm{Rx}\cos \mathrm{Rz}& \cos \mathrm{Rx}\cos \mathrm{Ry}\end{array}\right]\·\·\·\left(101\right)$

【数式2】

$\mathrm{base}{R}_{\mathrm{tool}}=\left[\begin{array}{ccc}\cos \mathrm{\α}& -\sin \mathrm{\α}& 0\\ \sin \mathrm{\α}& \cos \mathrm{\α}& 0\\ 0& 0& 1\end{array}\right]\left[\begin{array}{ccc}\cos \mathrm{\β}& 0& \sin \mathrm{\β}\\ 0& 1& 0\\ -\sin \mathrm{\β}& 0& \cos \mathrm{\β}\end{array}\right]\left[\begin{array}{ccc}1& 0& 0\\ 0& \cos \mathrm{\γ}& -\sin \mathrm{\γ}\\ 0& \sin \mathrm{\γ}& \cos \mathrm{\γ}\end{array}\right]\·\·\·\left(102\right)$

$\mathrm{base}{R}_{\mathrm{line}}=\left[\begin{array}{ccc}\mathrm{Xm}\left[x\right]& \mathrm{Ym}\left[x\right]& \mathrm{Zm}\left[x\right]\\ \mathrm{Xm}\left[y\right]& \mathrm{Ym}\left[y\right]& \mathrm{Zm}\left[y\right]\\ \mathrm{Xm}\left[z\right]& \mathrm{Ym}\left[z\right]& \mathrm{Zm}\left[z\right]\end{array}\right]\·\·\·\left(103\right)$

$\mathrm{line}{R}_{\mathrm{tool}}=\mathrm{line}{R}_{\mathrm{base}}\·\mathrm{base}{R}_{\mathrm{tool}}=\mathrm{base}{R}_{\mathrm{line}}^T\·\mathrm{base}{R}_{\mathrm{tool}}=\left[\begin{array}{ccc}\mathrm{Rm}11& \mathrm{Rm}12& \mathrm{Rm}13\\ \mathrm{Rm}21& \mathrm{Rm}22& \mathrm{Rm}23\\ \mathrm{Rm}31& \mathrm{Rm}32& \mathrm{Rm}33\end{array}\right]\·\·\·\left(104\right)$

Rx＝atan(Rm23/Rm33)…(105)

Ry＝asin(Rm13)…(106)

Rz＝atan(Rm12/Rm11)…(107)

(步骤S82)

接下来，在步骤S82中，所述控制装置1以前次的控制周期中的所述驱动轴34～36的速度为基准，将该驱动轴34～36各自的下一插补点处的速度的候补生成多个。在此，执行相应的处理时的所述控制装置1相当于角度候补计算单元。

例如，所述控制装置1在将前次的控制周期中的所述驱动轴34～36的速度设为了Vθ4_OLD、Vθ5_OLD、Vθ6_OLD时，生成Vθ4m＝Vθ4_OLD-F，Vθ4n＝Vθ4_OLD、Vθ4p＝Vθ4_OLD+F来作为所述驱动轴34的速度的候补。另外，所述控制装置1同样地，生成Vθ5m＝Vθ5_OLD-G、Vθ5n＝Vθ5_OLD、Vθ5p＝Vθ5_OLD+G来作为所述驱动轴35的速度的候补，且生成Vθ6m＝Vθ6_OLD-H、Vθ6n＝Vθ6_OLD、Vθ6p＝Vθ6_OLD+H来作为所述驱动轴36的速度的候补。在此，所述常数F、G、H各自以每1采样周期的加速度的限度值或振动等不发生的范围的值预先设定。

若如此针对所述驱动轴34～36各自生成3组速度的候补，则产生以下(201)～(227)的总共27组不同的组合。

(201)：Vθ4m，Vθ5m，Vθ6m

(202)：Vθ4m，Vθ5m，Vθ6n

(203)：Vθ4m，Vθ5m，Vθ6p

(204)：Vθ4m，Vθ5n，Vθ6m

(205)：Vθ4m，Vθ5n，Vθ6n

(206)：Vθ4m，Vθ5n，Vθ6p

(207)：Vθ4m，Vθ5p，Vθ6m

(208)：Vθ4m，Vθ5p，Vθ6n

(209)：Vθ4m，Vθ5p，Vθ6p

(210)：Vθ4n，Vθ5m，Vθ6m

(211)：Vθ4n，Vθ5m，Vθ6n

(212)：Vθ4n，Vθ5m，Vθ6p

(213)：Vθ4n，Vθ5n，Vθ6m

(214)：Vθ4n，Vθ5n，Vθ6n

(215)：Vθ4n，Vθ5n，Vθ6p

(216)：Vθ4n，Vθ5p，Vθ6m

(217)：Vθ4n，Vθ5p，Vθ6n

(218)：Vθ4n，Vθ5p，Vθ6p

(219)：Vθ4p，Vθ5m，Vθ6m

(220)：Vθ4p，Vθ5m，Vθ6n

(221)：Vθ4p，Vθ5m，Vθ6p

(222)：Vθ4p，Vθ5n，Vθ6m

(223)：Vθ4p，Vθ5n，Vθ6n

(224)：Vθ4p，Vθ5n，Vθ6p

(225)：Vθ4p，Vθ5p，Vθ6m

(226)：Vθ4p，Vθ5p，Vθ6n

(227)：Vθ4p，Vθ5p，Vθ6p

(步骤S83～S84)

接下来，在步骤S83中，所述控制装置1从所述组合(201)～(227)当中，随机地或基于预先设定的规则来选择3个组合。将此时所选择的3个速度的候补设为Vθa(t)、Vθb(t)、Vθc(t)。

接着，在步骤S84中，所述控制装置1生成在采用了3个速度的候补Vθa(t)、Vθb(t)、Vθc(t)各自的情况下的下一插补点处的所述驱动轴34～36的角度的候补θa(t)、θb(t)、θc(t)。此外，尽管在此为了方便说明而生成了3个速度的候补以及角度的候补，但该候补的数目根据所述控制装置1的运算性能而在满足所述多关节型机器人3所要求的动作速度的范围任意地设定即可。

例如，在最初的采样周期中，如以下的(301)～(303)所示，若选择所述组合(201)、(202)、(203)来作为所述速度的候补Vθa(t)、Vθb(t)、Vθc(t)，则如以下的(304)～(306)所示生成所述角度的候补θa(t)、θb(t)、θc(t)。

Vθa(t)＝Vθ4a(t)，Vθ5a(t)，Vθ6a(t)＝Vθ4m，Vθ5m，Vθ6m…(301)

Vθb(t)＝Vθ4b(t)，Vθ5b(t)，Vθ6b(t)＝Vθ4m，Vθ5m，Vθ6n…(302)

Vθc(t)＝Vθ4c(t)，Vθ5c(t)，Vθ6c(t)＝Vθ4m，Vθ5m，Vθ6p…(303)

θa(t)＝θ4a(t)，θ5a(t)，θ6a(t)＝θ4_OLD+Vθ4a(t)·Δt，θ5_OLD+Vθ5a(t)·Δt，θ6_OLD+Vθ6a(t)·Δt…(304)

θb(t)＝θ4b(t)，θ5b(t)，θ6b(t)＝θ4_OLD+Vθ4b(t)·Δt，θ5_OLD+Vθ5b(t)·Δt，θ6_OLD+Vθ6b(t)·Δt…(305)

θc(t)＝θ4c(t)，θ5c(t)，θ6c(t)＝θ4_OLD+Vθ4c(t)·Δt，θ5_OLD+Vθ5c(t)·Δt，θ6_OLD+Vθ6c(t)·Δt…(306)

另外，在下一采样周期也同样地，如以下的(311)～(313)所示，在选择了所述组合(203)、(206)、(209)作为所述速度的候补Vθa(t)、Vθb(t)、Vθc(t)的情况下，也生成所述角度的候补θa(t)、θb(t)、θc(t)。

Vθa(t)＝Vθ4a(t)，Vθ5a(t)，Vθ6a(t)＝Vθ4m，Vθ5m，Vθ6p…(311)

Vθb(t)＝Vθ4b(t)，Vθ5b(t)，Vθ6b(t)＝Vθ4m，Vθ5n，Vθ6p…(312)

Vθc(t)＝Vθ4c(t)，Vθ5c(t)，Vθ6c(t)＝Vθ4m，Vθ5p，Vθ6p…(313)

其后，在再下一采样周期中也同样地，如以下的(321)～(323)所示，在选择了所述组合(209)、(218)、(227)作为所述速度的候补Vθa(t)、Vθb(t)、Vθc(t)的情况下，也生成所述角度的候补θa(t)、θb(t)、θc(t)。

Vθa(t)＝Vθ4a(t)，Vθ5a(t)，Vθ6a(t)＝Vθ4m，Vθ5p，Vθ6p…(321)

Vθb(t)＝Vθ4b(t)，Vθ5b(t)，Vθ6b(t)＝Vθ4n，Vθ5p，Vθ6p…(322)

Vθc(t)＝Vθ4c(t)，Vθ5c(t)，Vθ6c(t)＝Vθ4p，Vθ5p，Vθ6p…(323)

(步骤S85)

然后，在步骤S85中，所述控制装置1判断是否在较之于使所述第1关节驱动系统的驱动轴34～36各自以所述作业结束点Q2的角度为目标进行线性动作的情况下离该目标更近的角度的范围内。即，判断所述步骤S84中计算出的角度的候补θa(t)、θb(t)、θc(t)各自是否为所述步骤S8中计算出的角度θ4_NOW、θ5_NOW、θ6_NOW与所述步骤S2中计算出的角度θ4e、θ5e、θ6e之间的角度。

在此，在判断为所述角度的候补θa(t)、θb(t)、θc(t)各自是所述角度θ4_NOW、θ5_NOW、θ6_NOW与所述角度θ4e、θ5e、θ6e之间的角度的情况下(S85的“是”侧)，处理转移至步骤S86。但是，在判断为否的情况下(S85的“否”侧)，处理返回至所述步骤S83，选择不同的组合，接着在所述步骤S84中再次生成所述候补θa(t)、θb(t)、θc(t)。

如此，所述驱动轴34～36各自的角度的候补始终被限定在所述角度θ4_NOW、θ5_NOW、θ6_NOW与所述角度θ4e、θ5e、θ6e之间的角度的范围，从而能使该驱动轴34～36各自的角度可靠地到达所述作业结束位置Q2处的角度。此外，尽管还考虑在所述作业结束位置Q2之前的位置上所述驱动轴34～36各自的角度到处该作业结束位置Q2处的角度，但在此情况下，其后设为所述驱动轴34～36各自的角度θ4_NOW＝θ4e、θ5_NOW＝θ5e、θ6_NOW＝θ6e即可。在此，通过所述步骤S82～S85来计算多个角度的候补时的所述控制装置1相当于角度候补计算单元。

(步骤S86)

接下来，在步骤S86中，所述控制装置1针对所述步骤S84中所生成的角度的候补θa(t)、θb(t)、θc(t)的每一个，求取从所述焊接线座标系∑line观察的所述末端执行器2的姿势角度。

具体而言，考虑在通过针对所述角度的候补θa(t)、θb(t)、θc(t)来求取正运动学问题的解从而计算出与每个该候补对应的所述基本座标系∑base下的位置·姿势后，从该基本座标系∑base变换成焊接线座标系∑line。此外，从基本座标系∑base到焊接线座标系∑line的变换与所述步骤S81同样地进行即可，因此在此省略说明。

(步骤S87～S88)

然后，在步骤S87中，所述控制装置1从所述角度的候补θa(t)～θc(t)当中选择所述步骤S86中求出的姿势角度离所述步骤S81中求出的姿势角度最近的角度的候补。此时，所述控制装置1在抑制了从所述焊接线座标系∑line观察的所述末端执行器2的姿势角度的3个分量(焊炬倾斜角Rx，焊炬前进角Ry，焊炬旋转角Rz)当中预先规定的一个或二个特定分量的变动时，选择离所述步骤S81中求出的姿势角度最近的角度的候补。在此，执行相应的处理时的所述控制装置1相当于角度选择单元。此外，所述特定分量对应于所述操作部13所执行的操作或对应于所述多关节型机器人3的加工内容而由所述控制装置1预先设定。

具体地，所述控制装置1遵照下述的(331)～(333)来求取在分别采用了所述角度的候补θa(t)、θb(t)、θc(t)的情况下的姿势角度与所述步骤S81中求出的姿势角度的变动量Ua、Ub、Uc。在此，常数D、E、F是抑制所述末端执行器2的姿势角度的3个分量(焊炬倾斜角Rx，焊炬前进角Ry，焊炬旋转角Rz)的哪一个分量的变动而确定的数值。例如，在抑制焊炬倾斜角Rx、焊炬前进角Ry的变动的情况下，考虑D＝1，E＝1，F＝θ。

Ua＝D·(Rx-Rxa)²+E·(Ry-Rya)²+F·(Rz-Rza)²  (331)

Ub＝D·(Rx-Rxb)²+E·(Ry-Ryb)²+F·(Rz-Rzb)²  (332)

Uc＝D·(Rx-Rxc)²+E·(Ry-Ryc)²+F·(Rz-Rzc)²  (333)

然后，所述控制装置1选择所述变动量Ua、Ub、Uc当中值最小的变动量，即所述步骤S81中求出的姿势角度之间所述特定分量的变动量最小的变动量所对应的所述角度的候补θa(t)、θb(t)、θc(t)。

其后，在步骤S88中，所述控制装置1将所述步骤S8中计算出的θ4_NOW、θ5_NOW、θ6_NOW变更为所述步骤S87中选择出的所述角度的候补θa(t)、θb(t)、θc(t)的任一者。例如，若Ua＜Ub＜Uc，则选择与变动量Ua对应的所述角度的候补θa(t)，成为θ4_NOW＝θ4a(t)，θ5_NOW＝θ5a(t)，θ6_NOW＝θ6a(t)。

由此，能抑制通过其后的所述步骤S9～S11来驱动了所述驱动轴31～36时的所述末端执行器2的所述特定分量的变动量。

在此，图17示出了在所述实施方式中的所述机器人控制处理(图3)中执行了所述特定分量抑制处理的情况下的所述驱动轴34～36的驱动轨迹。

此外，图17中的虚线示出了在不执行所述特定分量抑制处理而使所述驱动轴34～36线性动作的情况下的驱动轨迹。

在此，设所述多关节型机器人3的加工内容是焊接，且抑制所述末端执行器2的所述焊接线座标系∑line下的姿势角度(焊炬倾斜角Rx，焊炬前进角Ry，焊炬旋转角Rz)当中的焊炬倾斜角Rx、焊炬前进角Ry的变动特别重要，并将该焊炬倾斜角Rx、焊炬前进角Ry预先设定为所述特定分量。

在此情况下，在通过执行所述特定分量抑制处理(参照图12)从而如图17所示所述例外条件满足后，所述驱动轴34～36以所述作业结束点Q2为目标进行大致线性动作，但为了抑制所述末端执行器2的所述焊接线座标系∑line下的焊炬倾斜角Rx、焊炬前进角Ry而对其路径进行微调整。

由此，如图18所示，能以对焊接作业而言不重要(加权小)焊炬旋转角Rz的变动作为牺牲，来抑制对焊接作业重要的(加权大)焊炬倾斜角Rx和焊炬前进角Ry的变动。此外，图18中的虚线示出了在不执行所述特定分量抑制处理而使所述驱动轴34～36线性动作的情况下的变动量。

另外，图19示出了在所述实施例2中的所述机器人控制处理(图9)中执行了所述特定分量抑制处理的情况下的所述驱动轴34～36的驱动轨迹。此外，图19中的虚线示出了在不执行所述特定分量抑制处理而使所述驱动轴34～36线性动作的情况下的驱动轨迹。

在此，也设所述多关节型机器人3的加工内容是焊接，并将所述焊炬倾斜角Rx、焊炬前进角Ry预先设定为所述特定分量。

在此情况下，在通过执行所述特定分量抑制处理(参照图12)从而如图19所示所述例外条件满足后，所述驱动轴34～36以所述作业结束点Q2为目标进行大致线性动作，但为了抑制所述末端执行器2的所述焊接线座标系∑line下的焊炬倾斜角Rx、焊炬前进角Ry而对其路径进行微调整。

由此，如图20～图22所示，能以对焊接作业而言不重要(加权小)焊炬旋转角Rz的变动作为牺牲，来抑制对焊接作业重要的(加权大)焊炬倾斜角Rx和焊炬前进角Ry的变动。此外，图20～22中的虚线示出了在不执行所述特定分量抑制处理而使所述驱动轴34～36线性动作的情况下的变动量。

【实施例5】

而在该步骤S64中，所述控制装置1检测所述第1关节驱动系统是否已接近奇异姿势(奇异点)，在已接近奇异姿势的情况下，可以执行用于使平滑地转移至所述步骤S5中所计算的逆运动学问题的解当中另一解的处理。

进而，在从所述逆运动学问题的一解转移到另一解时，像所述实施例4那样，在从多个候补之中选择一个候补使得抑制所述末端执行器2的所述焊接线座标系∑line下的姿势角度(焊炬倾斜角Rx，焊炬前进角Ry，焊炬旋转角Rz)当中预先设定的一个或二个特定分量的变动的同时，期望所述驱动轴34～36的角度从所述逆运动学问题的一个解平滑地转移到另一个解。

优选设置如下单元：在进行了上述的实施方式那样的避开奇异姿势的处理(避开处理)时，将表示进行了该避开处理，即，多关节型机器人3处于奇异姿势避开中的情况向外部进行报知。

具体而言，如图25所示，将表示处于奇异姿势避开中的“奇异点避开中”显示于操作部13(示教台)的显示画面M。此外，表示处于奇异姿势避开中的显示不限于“奇异点避开中”这样的表现，只要是能明白避开了奇异姿势的表现，则无论怎样的表现均可。

如此，通过显示处于奇异点避开中，操作者在焊接中能掌握多关节型机器人3是处于奇异点避开中的状态还是处于并非奇异点避开中的状态。故而，例如，在作业对象物(材料)的焊接后进行质量检查的情况下，能注视以奇异点避开中的状态进行了焊接的部位来进行检查。

另外，例如，操作者还能以目视等监视在奇异点避开中的状态下的多关节型机器人3的整体的运动，还能基于该监视，在因下一焊接作业等而再次使多关节型机器人3运动时，重估多关节型机器人3的整体的动作。

此外，除了“奇异点避开中”的显示之外，还可以将处理程序的内容(例如，再生中程序的编号)显示于显示画面M，或将执行了处理程序的时间(再生时间)、末端执行器的运动(例如，直线移动)显示于显示画面M，以使处理程序明白。进而，还可以将预先示教的焊炬的信息(例如，焊炬倾斜角，焊炬前进角，焊炬旋转角等)、以及当前的焊炬的信息显示于显示画面M。

由此，能掌握“奇异点避开中”时的多关节型机器人3的详细的状态。

尽管在上述的例子中是在显示画面M上显示进行了奇异点避开的处理，但也可以取而代之，通过扬声器等以声音来通知进行了奇异点避开，还可以通过灯等的点亮或闪烁等以光来进行通知。还可以将进行了奇异点避开这一情况显示于与多关节型机器人的示教台13分体的外部的显示装置。

另外，可以将“奇异点避开中”的时间作为日志等存储至控制部10的存储部12等。然后，例如，在焊接结束后，可以将奇异点避开中的时间履历显示于操作部13(示教台)的显示画面M，从而操作者等能确认奇异点避开中的时间履历。

符号说明

1：多关节型机器人的控制装置

11：运算部

12：存储部

13：操作部

2：末端执行器

3：多关节型机器人

31～36：驱动轴

Claims (9)

1.一种多关节型机器人的控制装置，对多关节型机器人进行控制，所述多关节型机器人具备：第1关节驱动系统，其具有使设置于前端的作业部的姿势进行变化的3个驱动轴；以及第2关节驱动系统，其具有使所述第1关节驱动系统的位置进行变化的至少3个驱动轴，

所述多关节型机器人的控制装置具备：

插补点计算单元，其计算多个插补点，所述插补点用于在对预先设定的作业开始点以及作业结束点各自的位置以及姿势进行连结的示教路径上使所述作业部的位置以预先规定的速度进行移动；

驱动控制单元，其遵照由所述插补点计算单元计算出的所述插补点来使所述第1关节驱动系统以及所述第2关节驱动系统进行驱动；

例外条件判定单元，其在由所述驱动控制单元对所述第1关节驱动系统以及所述第2关节驱动系统进行了驱动时，判定与所述第1关节驱动系统的奇异姿势的检测条件不同的预先设定的例外条件是否满足；以及

例外动作单元，其在由所述例外条件判定单元判定为所述例外条件满足后，至所述作业部到达所述作业结束点为止的期间，计算用于以所述作业结束点的角度为目标使所述第1关节驱动系统的驱动轴各自线性地变化的该第1关节驱动系统的驱动轴各自的角度，并基于该计算出的第1关节驱动系统的驱动轴各自的角度和由所述插补点计算单元计算出的所述插补点处的所述作业部的位置来计算所述第2关节驱动系统的驱动轴各自的角度，且遵照该计算结果来使所述第1关节驱动系统以及所述第2关节驱动系统的驱动轴各自进行驱动。

2.根据权利要求1所述的多关节型机器人的控制装置，其中，

所述例外条件是：在从下一所述插补点起的所述作业部的剩余的移动时间或移动距离间使所述第1关节驱动系统的驱动轴各自的角度到达所述作业结束点处的角度的情况下，所述第1关节驱动系统的至少一个驱动轴的速度超过预先设定的容许范围。

3.根据权利要求1或2所述的多关节型机器人的控制装置，其中，

所述例外条件是：所述作业部的剩余的移动时间或移动距离达到预先设定的给定值以下。

4.根据权利要求1～3中任一项所述的多关节型机器人的控制装置，其中，

所述例外条件是：下一所述插补点处的所述第1关节驱动系统的至少一个驱动轴的角度偏离基于所述作业开始点和/或所述作业结束点而确定的基准角度预先设定的给定角度以上。

5.根据权利要求1～4中任一项所述的多关节型机器人的控制装置，其中，

所述例外条件是：下一所述插补点处的所述第1关节驱动系统的至少一个驱动轴的角度超过预先设定的工作范围。

6.根据权利要求1～5中任一项所述的多关节型机器人的控制装置，其中，

所述多关节型机器人的控制装置还具备：偏差量检测单元，其检测所述作业部与加工对象物的加工线的偏差量；仿形控制单元，其基于由所述偏差量检测单元检测出的偏差量，使所述作业部仿形所述加工线进行移动；以及作业结束点更新单元，其在所述作业结束点加上由所述偏差量检测单元检测出的偏差量来更新该作业结束点，

所述例外条件判定单元和/或所述例外动作单元基于由所述作业结束点更新单元更新后的所述作业结束点来执行处理。

7.根据权利要求1～6中任一项所述的多关节型机器人的控制装置，其中，

所述例外动作单元具备：

角度候补计算单元，其在较之于以所述作业结束点的角度为目标使所述第1关节驱动系统的驱动轴各自进行线性动作的情况下离该目标更近的角度的范围内，计算多个该驱动轴各自的角度的候补；以及

角度选择单元，其从由所述角度候补计算单元计算出的角度的候补之中，选择将在采用了各该候补时的下一所述插补点的从所述作业部观察的作业坐标系下的3个姿势角度的分量当中预先设定的一个或两个特定分量的变动抑制得最大的角度的候补。

8.一种多关节型机器人的控制方法，对多关节型机器人进行控制，所述多关节型机器人具备：第1关节驱动系统，其具有使设置于前端的作业部的姿势进行变化的3个驱动轴；以及第2关节驱动系统，其具有使所述第1关节驱动系统的位置进行变化的至少3个驱动轴，

所述多关节型机器人的控制方法执行如下工序：

插补点计算工序，计算多个插补点，所述插补点用于在对预先设定的作业开始点以及作业结束点各自的位置以及姿势进行连结的示教路径上使所述作业部的位置以预先规定的速度进行移动；

驱动控制工序，遵照由所述插补点计算工序计算出的所述插补点来使所述第1关节驱动系统以及所述第2关节驱动系统进行驱动；

例外条件判定工序，在由所述驱动控制工序对所述第1关节驱动系统以及所述第2关节驱动系统进行了驱动时，判定与所述第1关节驱动系统的奇异姿势的检测条件不同的预先设定的例外条件是否满足；以及

例外动作工序，在由所述例外条件判定工序判定为所述例外条件满足后，至所述作业部到达所述作业结束点为止的期间，计算用于以所述作业结束点的角度为目标使所述第1关节驱动系统的驱动轴各自线性地变化的该第1关节驱动系统的驱动轴各自的角度，并基于该计算出的第1关节驱动系统的驱动轴各自的角度和由所述插补点计算工序计算出的所述插补点处的所述作业部的位置来计算所述第2关节驱动系统的驱动轴各自的角度，且遵照该计算结果来使所述第1关节驱动系统以及所述第2关节驱动系统的驱动轴各自进行驱动。

9.一种多关节型机器人的控制程序，对多关节型机器人进行控制，所述多关节型机器人具备：第1关节驱动系统，其具有使设置于前端的作业部的姿势进行变化的3个驱动轴；以及第2关节驱动系统，其具有使所述第1关节驱动系统的位置进行变化的至少3个驱动轴，

所述多关节型机器人的控制程序使处理器执行如下工序：

插补点计算工序，计算多个插补点，所述插补点用于在对预先设定的作业开始点以及作业结束点各自的位置以及姿势进行连结的示教路径上使所述作业部的位置以预先规定的速度进行移动；

驱动控制工序，遵照由所述插补点计算工序计算出的所述插补点来使所述第1关节驱动系统以及所述第2关节驱动系统进行驱动；

例外条件判定工序，在由所述驱动控制工序对所述第1关节驱动系统以及所述第2关节驱动系统进行了驱动时，判定与所述第1关节驱动系统的奇异姿势的检测条件不同的预先设定的例外条件是否满足；以及

例外动作工序，在由所述例外条件判定工序判定为所述例外条件满足后，至所述作业部到达所述作业结束点为止的期间，计算用于以所述作业结束点的角度为目标使所述第1关节驱动系统的驱动轴各自线性地变化的该第1关节驱动系统的驱动轴各自的角度，并基于该计算出的第1关节驱动系统的驱动轴各自的角度和由所述插补点计算工序计算出的所述插补点处的所述作业部的位置来计算所述第2关节驱动系统的驱动轴各自的角度，且遵照该计算结果来使所述第1关节驱动系统以及所述第2关节驱动系统的驱动轴各自进行驱动。

Images