臂型机械手的障碍物自动回避方法及控制装置与流程

本发明涉及一种臂型机械手的障碍物自动回避方法及控制装置。

背景技术：

先前以来，已知有使已停止的机械手自动地回避与障碍物的干扰而到达规定姿势的方法。

专利文献1中揭示有沿路径逆向返回至作业原点的方法。具体而言，是借由依序执行包含移动指令而记述的控制程序，而使沿所需的路径进行一系列动作的机械手自停止位置返回至作业原点的控制方法，当依序逆执行已执行的控制程序时，使用其前一个移动指令的位置参数来执行各移动指令。

专利文献2中揭示有一种机械手；即便机械手异常停止于动作路径以外时亦能返回至原点。具体而言，具备：将至少包含机械手进行动作的动作区域的区域映射图（areamap）划分为各个具有规定区域的区域块的区域块作成工序、及针对每个所划分的区域块设定机械手的返回方向的方向设定工序。借此，能回避与障碍物的干扰。

现有技术文献

专利文献

专利文献1：日本专利特开平7-28520号公报

专利文献2：日本专利特开2009-90383号公报。

技术实现要素：

发明所要解决的问题

专利文献1所述的方法中，当机械手异常停止于动作路径以外时，无法返回至作业原点。另一方面，专利文献2的方法中，当机械手异常停止于动作路径以外时，能够返回至作业原点。然而，专利文献2所述的方法中，须预先针对每个区域块设定机械手的返回方向，从而，存在作业者因设定作业而令作业量增大的问题。而且，返回方向的决定亦须作业者根据经验法则进行判断，从而存在作业者难以确保返回方向的问题。此外，针对每个区域块设定有机械手的返回方向的数据会导致数据量庞大的问题。换言之，存在须针对机械手的返回方向进行复杂设定的问题。

本发明鉴于上述课题而完成，其目的在于提供不论机械手停止于哪个位置均能以简单的设定使其安全地到达规定姿势的机械手的障碍物自动回避方法及控制装置。

解决问题的技术手段

为了解决所述问题，本发明的一形态的机械手的障碍物自动回避方法是以如下形式结构：该臂型机械手具备利用关节连结有一个以上的连杆且于梢端部设有手部的机械手臂，该臂型机械手的障碍物自动回避方法包括如下步骤：对于所述机械手以使其具有几何形状的形式建模，从而表现为几何模型的几何模型表现步骤；设定所述几何模型不得进入的禁止进入区域、和由该禁止进入区域规定而所述几何模型进行动作的动作区域的区域设定步骤；决定所述机械手的最终姿势的最终姿势决定步骤；决定当所述机械手自当前姿势向所述最终姿势变化时所述手部的初始轨道的初始轨道决定步骤；算出与所述初始轨道上的规定的点对应的所述机械手的假想姿势的假想姿势算出步骤；判定所述假想姿势下的所述几何模型是否干扰所述禁止进入区域的干扰判定步骤；当所述干扰判定步骤中判定为不干扰时，将所述假想姿势决定为经过姿势，而当判定为产生干扰时，假想产生使所述禁止进入区域的干扰部分与所述几何模型的干扰部分相对排斥的斥力，算出因假想斥力而将所述几何模型的干扰部分自所述禁止进入区域推出至所述动作区域的状态下的姿势，将该算出的姿势决定为经过姿势的经过姿势决定步骤；将自所述当前姿势经由所述经过姿势而变化为所述最终姿势时所述手部的轨道决定为更新轨道的更新轨道决定步骤；和，将最新的经过姿势决定步骤中所决定的经过姿势假设为所述初始轨道决定步骤中的所述当前姿势，并反复执行所述初始轨道决定步骤、所述假想姿势算出步骤、所述干扰判定步骤、所述经过姿势决定步骤、及所述更新轨道决定步骤的步骤。

根据上述结构，算出当机械手自当前姿势向最终姿势变化时手部的初始轨道上的规定的点所对应的机械手的假想姿势，判定假想姿势下的机械手的几何模型是否干扰禁止进入区域（障碍物）。并且，当判定为不干扰时，将假想姿势决定为当自当前姿势向最终姿势变化时经过的经过姿势，而当判定为产生干扰时，算出因假想斥力而将几何模型的干扰部分自禁止进入区域推出至动作区域的状态下的姿势，将该姿势决定为经过姿势。因此，当机械手的几何模型干扰禁止进入区域时，赋予不干扰禁止进入区域的经过姿势，故而，可回避与障碍物的冲突。然而，此时，因偏离原来的初始轨道，故而无法保证接近最终姿势。另一方面，当机械手的几何模型不干扰禁止进入区域时，赋予与初始轨道上的点对应的经过姿势，故而接近最终姿势。并且，根据上述结构，将新赋予的经过姿势作为当前姿势且反复实施该程序，故而，每当机械手的几何模型不干扰禁止进入区域的程序出现时，赋予与该程序中的初始轨道上的点对应的经过姿势且接近最终姿势。因此，借由反复进行试行错误，最终，机械手可以回避与障碍物的冲突同时获得当自当前姿势变化为最终姿势时手部的更新轨道。

结果，借由机械手的几何模型、禁止进入区域及动作区域的设定这样的简单设定，从而，不论机械手停止于哪个位置均能自动回避与障碍物的干扰，使机械手安全地到达规定姿势（例如为退避姿势）。

所述最终姿势决定步骤可将设定于已知轨道上的所述机械手的多个最终候补姿势中的、所述机械手的当前姿势与所述最终候补姿势的所述各关节的角度的位移的绝对值的总和为最小值的最终候补姿势选择并决定为所述最终姿势。

根据此结构，可选择接近机械手的当前姿势的姿势作为最终姿势。借此，能缩短直至到达最终姿势为止所需的时间。

所述经过姿势决定步骤可算出因所述假想斥力而某假想姿势下的所述机械手的所述各关节围绕转动轴产生的转矩，进而以所述假想姿势为起点，反复进行算出因该转矩的影响而变化的所述机械手经过规定时间后的假想姿势的运算，借此算出所述机械手的假想姿势的经时变化，且根据所述机械手的假想姿势的经时变化收敛时的所述机械手的假想姿势决定所述经过姿势。

根据此结构，能适当地赋予回避机械手与禁止进入区域的干扰的经过姿势。

所述假想斥力可以与所述几何模型进入至禁止进入区域的距离成比例地变大的形式构成。

根据此结构，即便机械手的几何模型深入至禁止进入区域，亦能迅速地将其推出至禁止进入区域外。

本发明的机械手的障碍物自动回避方法可具有如下步骤：判断所述机械手的姿势是否未以接近所述最终姿势的形式经时变化而是陷入停留状态的停留状态判定步骤；当所述停留状态判定步骤中判定为陷入停留状态时，生成所述机械手的因所述假想斥力而被推出的状态下的姿势以外的经过候补姿势的经过候补姿势生成步骤；判定所述经过候补姿势是否为接近所述最终姿势的姿势的第一判定步骤；和，当所述第一判定步骤中判定为所述经过候补姿势为向接近所述最终姿势的方向变化的姿势时，根据该经过候补姿势而决定所述经过姿势的第一决定步骤。

根据此结构，例如，当因斥力而回推的方向与初始轨道的方向一致时，被回推至动作区域的姿势对应于初始轨道上的点，故而，几何模型反复对禁止进入区域产生干扰而陷入停留状态，机械手不接近最终姿势。然而，根据上述结构，可获得与非初始轨道上的点对应的经过姿势，故而，即便陷入这样的停留状态，亦可经由接近最终姿势的其他经过姿势而到达最终姿势。

本发明的机械手的障碍物自动回避方法可具有如下步骤：当所述第一判定步骤中判定为所述经过候补姿势为向远离所述最终姿势的方向变化的姿势时，根据概率值判定是否选择所述经过候补姿势的第二判定步骤；和，当所述第二判定步骤中判定为选择时，根据该经过候补姿势决定所述经过姿势的第二决定步骤。

根据此结构，即便无法生成接近最终姿势的其他姿势，亦可尝试是否可经由其他经过姿势而到达最终姿势。借此，可适当地脱离当前的无法到达最终姿势的状态。

为了解决所述问题，本发明的一形态的臂型机械手的控制装置以如下形式构成，该臂型机械手具备利用关节连结有一个以上的连杆且于梢端部设有手部的机械手臂，该臂型机械手的控制装置具备：对于所述机械手以使其具有几何形状的形式建模，从而表现为几何模型的几何模型表现部；设定所述几何模型不得进入的禁止进入区域、和由该禁止进入区域规定而所述几何模型进行动作的动作区域的区域设定部；决定所述机械手的最终姿势的最终姿势决定部；决定所述机械手自当前姿势向所述最终姿势变化时所述手部的初始轨道的初始轨道决定部；算出与所述初始轨道上的规定的点对应的所述机械手的假想姿势的假想姿势算出部；判定所述假想姿势下的所述几何模型是否干扰所述禁止进入区域的干扰判定部；决定经过姿势的经过姿势决定部；和更新轨道决定部；所述经过姿势决定部当所述干扰判定部判定为不干扰时，将所述假想姿势决定为经过姿势，而当判定为产生干扰时，假想产生使所述禁止进入区域的干扰部分与所述几何模型的干扰部分相对排斥的斥力，算出因假想斥力而将所述几何模型的干扰部分自所述禁止进入区域推出至所述动作区域的状态下的姿势，将该算出的姿势决定为经过姿势；所述更新轨道决定部将自所述当前姿势经由所述经过姿势而变化为所述最终姿势时所述手部的轨道决定为更新轨道；将所述经过姿势决定部所决定的最新的经过姿势假设为所述初始轨道决定部的当前姿势，且所述初始轨道决定部、所述假想姿势算出部、所述干扰判定部、所述经过姿势决定部、及所述更新轨道决定部反复进行处理。

根据上述结构，算出当机械手自当前姿势向最终姿势变化时手部的初始轨道上的规定的点所对应的机械手的假想姿势，判定假想姿势下的机械手的几何模型是否干扰禁止进入区域（障碍物）。并且，当判定为不干扰时，将假想姿势决定为当自当前姿势向最终姿势变化时经过的经过姿势，而当判定为产生干扰时，算出因假想斥力而将几何模型的干扰部分自禁止进入区域推出至动作区域的状态下的姿势，将该姿势决定为经过姿势。因此，当机械手的几何模型干扰禁止进入区域时，赋予不干扰禁止进入区域的经过姿势，故而，可回避与障碍物的冲突。然而，此时，因偏离原来的初始轨道，故而无法保证接近最终姿势。另一方面，当机械手的几何模型不干扰禁止进入区域时，赋予与初始轨道上的点对应的经过姿势，故而接近最终姿势。并且，根据上述结构，将新赋予的经过姿势作为当前姿势且反复实施该程序，故而，每当机械手的几何模型不干扰禁止进入区域的程序出现时，赋予与该程序中的初始轨道上的点对应的经过姿势且接近最终姿势。因此，借由反复进行试行错误，最终，机械手可以回避与障碍物的冲突同时获得当自当前姿势变化为最终姿势时手部的更新轨道。

发明效果

本发明可利用简单的设定而发挥如下效果，即，不论机械手停止于哪个位置均能自动回避与障碍物的干扰而使机械手安全地到达规定姿势。

附图说明

图1是表示具备本发明的实施形态1的机械手的设备的结构例的俯视图；

图2是概略性地表示图1的机械手的控制系统的结构例的框图；

图3是表示图1的机械手的最终姿势的选择方法的说明图；

图4是表示图1的机械手的经过姿势的算出方法的流程图；

图5a是表示图1的机械手的经过姿势的算出方法的说明图；

图5b是表示图1的机械手的经过姿势的算出方法的说明图；

图5c是表示图1的机械手的经过姿势的算出方法的说明图；

图6是表示图1的机械手的动作例的流程图；

图7a是表示图1的机械手的动作例的图；

图7b是表示图1的机械手的动作例的图；

图7c是表示图1的机械手的动作例的图；

图7d是表示图1的机械手的动作例的图；

图7e是表示图1的机械手的动作例的图；

图7f是表示图1的机械手的动作例的图；

图8是表示本发明的实施形态2的机械手的动作例的流程图；

图9是表示图8的机械手的处理中使用的转移概率的坐标图；

图10a是表示图8的机械手的模拟条件的图；

图10b是表示图8的机械手的模拟条件的图；

图10c是表示图8的机械手的模拟结果的图。

具体实施方式

以下，参照附图对本发明的实施形态进行说明。再者，本发明并不受本实施形态的限制。而且，以下的所有图中，对于相同或同等的要素标注相同的参照符号，且省略其重复说明。

（实施形态1）

图1是表示具备本发明的实施形态1的机械手100的设备110的结构例的俯视图。

设备110例如为进行基板处理的设备，且内部设置有机械手100，且例如具有包围腔室113的壁111、及于腔室内部自壁111向内侧突出的壁112。这些壁111及壁112结构机械手100动作中的障碍物。

[机械手的结构]

机械手100例如为搬送半导体晶圆、玻璃晶圆等基板的机械手。

如图1所示，机械手100是具备利用关节连结有一个以上的连杆且于梢端部设有手部的机械手臂的臂型机械手。更具体而言，机械手100是水平多关节机械手，且具备基台1、第一臂2、第二臂3及手部4，它们以直链状连结。本实施形态中，第一臂2、第二臂3、手部4构成机械手臂。而且，机械手100具备控制机械手100的动作的控制器5（参照图2）。

基台1例如为中空的圆筒状构件。于基台1的内部，配设有包含伺服马达的第一臂驱动部12（参照图2）。第一臂驱动部12以使后述的第一关节轴2a转动的形式构成。

第一臂2例如为中空的板状构件，当俯视时大致形成为条状。于第一臂2，以自第一臂2的基端部的底面向下方突出的形式形成有第一关节轴2a。并且，第一关节轴2a能以沿z方向延伸的转动轴线l1为中心转动地安装于基台1，该部分构成第一关节。因此，第一臂2以于xy平面转动的形式构成。再者，本实施形态中，转动轴线l1构成xy平面上的基准点o。

于第一臂2的内部配设有包含伺服马达的第二臂驱动部13（参照图2）。第二臂驱动部13以使后述的第二关节轴3a转动的形式构成。

并且，借由第一臂驱动部12的伺服马达的编码器，检测第一臂2相对于基台1的围绕转动轴线l1的相对角度位置q1（参照图3）。该编码器构成第一臂角度位置检测部15（参照图2）。

再者，第一臂2有时称为第一连杆，而且有时将第一关节轴2a称为一轴。

第二臂3例如为中空的板状构件，且当俯视时大致形成为条状。于第二臂3，以自第二臂3的基端部的底面向下方突出的形式设有第二关节轴3a。并且，第二关节轴3a能以与转动轴线l1平行地延伸的转动轴线l2为中心转动地安装于第一臂2，该部分构成第二关节。因此，第二臂3以于xy平面上转动的形式构成。

于第二臂3的内部，配设有包含伺服马达的手部驱动部14（参照图2）。手部驱动部14以使后述的第三关节轴4a转动的形式构成。

并且，借由第二臂驱动部13的伺服马达的编码器，检测第二臂3相对于第一臂2的围绕转动轴线l2的相对角度位置q2（参照图3）。该编码器构成第二臂角度位置检测部16（参照图2）。

再者，第二臂3有时称为第二连杆，而且第二关节轴3a有时称为二轴。

手部4是保持基板者，且形成为平板状。于手部4，具有以自其基端部的底面向下方突出的形式形成的第三关节轴4a。并且，第三关节轴4a能以与转动轴线l1、l2平行地延伸的转动轴线l3为中心转动地安装于第二臂3，该部分构成第三关节。因此，手部4以于xy平面转动的形式构成。

并且，借由手部驱动部14的伺服马达的编码器，检测手部4相对于第二臂3的围绕转动轴线l3的相对角度位置q3（参照图3）。该编码器构成手部角度位置检测部17（参照图2）。

再者，手部4有时称为第三连杆，而且第三关节轴4a有时称为三轴。

[控制部]

图2是概略性地表示机械手100的控制系统的结构例的框图。

机械手100所具备的控制器5例如具备：具有cpu等运算器的控制部21、及具有rom及ram等存储器的存储部22。控制器5既可由进行集中控制的单独的控制器构成，亦可由彼此协作地进行分散控制的多个控制器构成。控制部21包含第一臂控制部31、第二臂控制部32、手部控制部33、最终姿势决定部34、初始轨道决定部35、假想姿势算出部36、几何模型表现部37、区域设定部40、干扰判定部38、经过姿势决定部39及更新轨道决定部41。这些功能部31～41是借由控制部21执行存储部22中储存的规定的控制程序而实现的功能块（功能模块）。

第一臂控制部31控制第一臂驱动部12，使第一臂2于xy平面围绕转动轴线l1转动。

第二臂控制部32控制第二臂驱动部13，使第二臂3于xy平面围绕转动轴线l2转动。

手部控制部33控制手部驱动部14，使手部4于xy平面围绕转动轴线l3转动。

再者，本实施形态中，例如，控制部21的主控制部（未图示）根据机械手100的第一臂角度位置检测部15、第二臂角度位置检测部16及手部角度位置检测部17各自检测出的角度位置、及最终姿势，分别向第一臂控制部31、第二臂控制部32及手部控制部33输出最终角度位置。第一臂控制部31、第二臂控制部32及手部控制部33分别以使对应的控制对象物（第一关节轴2a、第二关节轴3a及第三关节轴4a）的角度位置成为与最终姿势对应的最终角度位置的形式，根据对应的角度位置检测部15、16、17检测出的转动轴的角度位置，对于对应的第一臂驱动部12、第二臂驱动部13及手部驱动部14分别进行反馈控制。

几何模型表现部37对于机械手100以使其具有几何形状的形式建模，从而表示为几何模型m。

本实施形态中，几何模型表现部37利用几何模型m1、m2及m3表现第一臂2、第二臂3及手部4。

如图1所示，第一臂2的几何模型m1例如为长圆形的模型。几何模型m1表现为，于将第一臂2投影至xy平面所得的图形的外侧，与该图形相离。

第二臂3的几何模型m2例如为长圆形的模型。几何模型m2表现为，于将第二臂3投影至xy平面所得的图形的外侧，与该图形相离。

手部4的几何模型m3例如为水滴型的模型。几何模型m3规定（划定）为，于将保持基板的手部4投影至xy平面所得的图形的外侧，与该图形相离。

如图1及2所示，区域设定部40设定几何模型m1～m3不得进入的禁止进入区域s1、及由禁止进入区域s1规定（划定）且几何模型m1～m3进行动作的动作区域s2。

本实施形态中，禁止进入区域s1及动作区域s2储存于存储部22，且借由区域设定部40自存储部22读取禁止进入区域s1及动作区域s2而设定禁止进入区域s1及动作区域s2。

而且，禁止进入区域s1以还包含比设备110的腔室113的内壁面（壁111及壁112的壁面）靠内侧的区域的形式设定。而且，沿禁止进入区域s1与动作区域s2的边界延伸的假想线构成边界线b。

最终姿势决定部34决定机械手100的最终姿势pt。

本实施形态中，最终姿势决定部34自存储部22中存储的多个最终候补姿势ptc中选择并决定一个最终姿势pt。

最终候补姿势ptc是设定于不干扰禁止进入区域s1的已知轨道上的姿势。借此，机械手100只要可到达任一姿势，则之后可沿已知轨道进行动作而不会干扰禁止进入区域s1。

图3是表示机械手100的最终姿势pt的选择方法的说明图。

如图3所示，当最终姿势决定部34自多个最终候补姿势ptc（图3中表示其中的一）中选择一个最终姿势pt时，首先，根据以下式子，算出当前姿势ps与最终候补姿势ptc下各关节轴的关节角度的位移的绝对值的总和e。

[数1]

其中，

n是轴数，

qi是i轴的目标姿势下的关节角度，

qdi是i轴的目标候补姿势下的关节角度。

继而，选择e为最小值的最终候补姿势ptc，且将其决定为最终姿势pt。

初始轨道决定部35是决定当机械手100自当前姿势ps向最终姿势决定部34所决定的最终姿势pt变化时手部4的初始轨道ta。本实施形态中，初始轨道ta是借由赋予当前姿势ps及最终姿势pt而唯一性地决定的轨道。

假想姿势算出部36算出初始轨道ta上的规定的点上的机械手100的假想姿势pp。

干扰判定部38分别判定假想姿势pp下的机械手100的几何模型m1～m3是否干扰禁止进入区域s1。

经过姿势决定部39算出当自当前姿势ps向最终姿势pt变化时经过的经过姿势pv。

此外，当干扰判定部38判定为机械手100的几何模型m1～m3与禁止进入区域s1完全无干扰时，经过姿势决定部39将假想姿势pp决定为经过姿势pv。

另一方面，当判定为几何模型m1～m3中的至少一个几何模型与禁止进入区域s1产生干扰时，经过姿势决定部39假想产生使禁止进入区域s1的干扰部分与几何模型的干扰部分相对排斥的斥力fi，算出因假想斥力fi将几何模型的干扰部分自禁止进入区域s1推出至动作区域s2的状态下的姿势，且将该算出的姿势决定为经过姿势pv。

图4是表示机械手100的经过姿势pv的算出方法的流程图。

图5a～5c是表示机械手100的经过姿势pv的算出方法的说明图。

以下，关于经过姿势决定部39为了算出经过姿势pv而进行的处理进行详细说明。再者，本实施形态中，机械手100是具有3个连杆的机械手，以下，表示使具有n个连杆的机械手进行一般化的见解。

首先，决定机械手100的几何模型进入至禁止进入区域s1的距离di（步骤s71）。距离di是根据假想姿势pp下各几何模型与禁止进入区域s1的干扰形态而算出。

例如，于图5a所示的情况下，首先，于几何模型mi的干扰部分（图5a中淡墨色的部分），算出与边界线b的距离达到最大的几何模型mi上的点p（x，y）。并且，于边界线b的法线方向，将点p与边界线b的距离设定为距离di。

而且，于图5b所示的情况下，首先，于几何模型mi的干扰部分（图5b中淡墨色的部分），算出与边界线b的距离达到最大的几何模型mi上的点p（x，y）。并且，将点p与边界线b的顶点q的距离设定为距离di。

此外，于图5c所示的情况下，首先，于几何模型mi的干扰部分（图5c中淡墨色的部分），算出位于自边界线b的顶点q朝几何模型与边界线b的一个交点r之侧的、与边界线b的距离达到最大的几何模型mi上的点p1（x，y）。并且，于线段qr的法线方向上，将点p1与边界线b的距离设定为距离di1。

而且，于几何模型mi的干扰部分，算出位于自边界线b的顶点q朝几何模型与边界线b的另一交点r’之侧的、与边界线b的距离达到最大的几何模型上的点p2（x，y）。并且，于线段qr’的法线方向上，将点p2与边界线b的距离设定为距离di2。

如此，距离di的算出形成为根据假想姿势pp下几何模型与禁止进入区域s1的干扰形态而进行的结构，但并不限于上述结构。

继而，根据所决定的距离di，依据以下式子（2）针对各个连杆算出假想斥力fi（步骤s72）。

[数2]

其中，

i是连杆编号，

k是任意假想刚性系数，

di是相对于禁止进入边界线b的进入距离。

再者，于图5c所示的情况下，根据所决定的距离di1及di2，依据式子（2）分别算出假想斥力fi1及fi2，之后将假想斥力fi1及fi2合成，算出假想斥力fi。

即，对于判定为干扰禁止进入区域s1的第i连杆，假想产生与几何模型进入至禁止区域s2的距离成比例的假想斥力fi。借此，当几何模型深入禁止进入区域s1时，会强力地推出。因此，可缩短直至几何模型被推出为止所需的时间。

另一方面，判定为不干扰禁止进入区域s1的第i连杆以不会产生假想斥力fi的形式构成。

继而，算出因假想斥力fi而产生的围绕机械手100的各轴而产生的假想转矩τ（步骤s73）。

[数3]

其中，

τi是第i轴上产生的转矩，

li是第i（1≤i≤n）连杆的长度，

wn是以越远离第i连杆则第i＜n假想斥力fi的影响越小的形式设定的加权系数，

ψi是禁止进入边界线b的法线方向与x轴于xy平面上所成的角度，

φi是第i（1≤i≤n）连杆与x轴于xy平面上所成的角度。

即，判定为干扰边界线b的第i连杆上产生的假想斥力fi以不仅影响第i连杆而且亦影响第i＜n连杆的形式构成。借此，即便于第i连杆上产生假想斥力fi的点为该连杆的死点时，该假想斥力fi亦会影响其他连杆且产生转矩。借此，可利用该转矩使其他连杆转动，从而生成与假想姿势pp不同的经过姿势pv。

而且，角度ψi是根据各几何模型与禁止进入区域s1的干扰形态而算出。

例如，于图5a所示的情形时，角度ψi是边界线b的法线方向与x轴于xy平面上所成的角度。

而且，于图5b所示的情形时，角度ψi是线段pq与x轴所成的角度。

此外，于图5c所示的情形时，角度ψi是上述假想斥力的合成向量与x轴所成的角度。

继而，算出因上述假想转矩τi的影响而产生的、机械手100的假想姿势的经时变化。

[数4]

其中，

q[0]是当前姿势，

[0]=0，

τ[0]由式子（3）算出，

α、β是以使机械手进行预期的举动的形式设定的常数。

此处，q（·）设为于q上方标注一个符号·（点）而得的记号。

并且，借由反复计算式子（2）与式子（3），算出当τ[k]与q（·）[k]收敛为0时的q[k]。该q[k]是第一臂2、第二臂3、及手部4自禁止进入区域s1推出至动作区域s2的姿势。

并且，根据q[k]设定假想姿势pp（步骤s74）。

回避轨道决定部41将当自当前姿势ps经由经过姿势pv而变化为最终姿势pt时手部4的轨道决定为更新轨道tb。

[动作例]

继而，说明机械手100的动作例。

图6是表示本发明的实施形态的机械手100的动作例的流程图。图7a～图7f是表示本发明的实施形态的机械手100的动作例的图。

首先，如图7a所示，控制部21的最终姿势决定部34决定机械手100的最终姿势pt（最终姿势决定步骤）（步骤s10）。本实施形态中，控制部21的最终姿势决定部34针对4个最终候补姿势ptc1～ptc4求出e值，且将e值最小的最终候补姿势ptc4选择为最终姿势pt。如此，以将接近当前姿势ps的姿势选择为最终姿势pt的形式构成，故而，能减少自当前姿势ps至到达最终姿势pt为止所需的各轴的位移，从而能缩短直至到达为止所需的平均时间。

继而，如图7b所示，控制部21的初始轨道决定部35决定机械手100自当前姿势ps向最终姿势pt变化的初始轨道ta（初始轨道决定步骤）（步骤s20）。

继而，如图7c所示，控制部21的假想姿势算出部36算出初始轨道ta上的当前姿势ps附近的，自当前姿势ps朝最终姿势pt之侧的规定的点上的机械手100的假想姿势pp（假想姿势算出步骤）（步骤s30）。此处，所谓附近是指相对于当前姿势而对各轴施以微小的变更所得的姿势。

继而，控制部21的区域设定部40自存储部22读取禁止进入区域s1及动作区域s2且进行设定（区域设定步骤）（步骤s35）。

继而，控制部21的几何模型表现部37对于处于假想姿势pp的机械手100的第一臂2、第二臂3及手部4建模，从而分别以几何模型m1、m2及m3表现（几何模型表现步骤）（步骤s40）。

几何模型m1～m3是使机械手100的第一臂2、第二臂3及手部4简化后呈现的模型，故而，能削减后述的干扰判定所需的计算量，从而能迅速进行干扰判定。

而且，如上所述，由几何模型m1至m3的假想线描绘出的图形的外周以如下形式规定（划定）：于将保持基板的手部4投影至xy平面所得的图形的外侧，与该图形相离。即，几何模型表现为比第一臂2、第二臂3及手部4粗（壁厚），故而，即便后述的步骤s50中控制部21的干扰判定部38判定为几何模型与禁止进入区域s1产生干扰，亦能使机械手100与设备110不产生干扰，从而，能防止机械手100或者机械手100保持的基板实际上接触于设备110。

而且，禁止进入区域s1以包含比设备110的腔室113的内壁面靠内侧的区域的形式设定，故而，能更确实地防止机械手100或者机械手100保持的基板实际上接触于设备110。

继而，控制部21的干扰判定部38分别判定假想姿势pp下的几何模型m1～m3是否干扰禁止进入区域s1（干扰判定步骤）（步骤s50）。即，所谓几何模型与禁止进入区域s1产生干扰是指，几何模型的边界线与边界线b交叉的状态。

再者，本实施形态中，几何模型表现部37于紧靠干扰判定部38的干扰判定之前的步骤中生成几何模型，但并不限于此，亦可于干扰判定部38的干扰判定之前的任意步骤中执行。同样，区域设定部40的禁止进入区域s1及禁止进入区域s2的设定亦可于干扰判定部38的干扰判定之前的任意步骤中执行。

并且，当判定为丝毫不干扰时（步骤s50中为否），控制部21的经过姿势决定部39将假想姿势pp决定为经过姿势pv（经过姿势决定步骤）（步骤s60）。

因此，当机械手100的几何模型不干扰禁止进入区域s1时，赋予与初始轨道ta上的点对应的经过姿势，故而，接近最终姿势pt。

另一方面，如图7d所示，步骤s50中，当控制部21的干扰判定部38判定为几何模型与边界线b产生干扰时（步骤s50中为是），控制部21的经过姿势决定部39如图7e所示，算出几何模型的干扰部分自禁止进入区域s1推出至动作区域s2的状态下的姿势，将该算出的姿势决定为经过姿势pv（经过姿势决定步骤）（步骤s70）。

因此，当机械手100的几何模型干扰禁止进入区域s1时，赋予不干扰禁止进入区域s1的经过姿势pv，故而可回避与障碍物的冲突。

再者，当将上述假想姿势决定为经过姿势pv时，亦可进而判定自当前姿势至上述推出状态的姿势的轨道是否干扰禁止进入区域s1。

于步骤s60或者步骤s70中决定经过姿势pv后，继而，控制部21的更新轨道决定部41将当自当前姿势ps经由经过姿势pv而变化为最终姿势pt时手部4的轨道决定为更新轨道tb（更新轨道决定步骤）（步骤s80）。

继而，控制部21将经过姿势pv决定（暂定）为新的当前姿势ps，且将更新轨道tb决定（暂定）为新的初始轨道ta（步骤s90）。

继而，控制部21的假想姿势算出部36算出初始轨道ta上的当前姿势ps附近的、自当前姿势ps朝最终姿势pt之侧的规定的点上的新的假想姿势pp（步骤s100）。

继而，判定步骤s100中算出的假想姿势pp与最终姿势pt是否一致。并且，当判定为不一致时（步骤s110中为否），反复执行上述步骤s40～步骤s110。即，步骤s40～步骤s110执行至当前姿势ps与最终姿势pt一致为止。

并且，若步骤s100中算出的假想姿势pp与最终姿势pt一致（步骤s110中为是），则结束处理。借此，如图7f所示，生成已回避与禁止进入区域s1的干扰的最终的更新轨道tb。

再者，本实施形态中，机械手100以如下形式构成：当上述处理结束之后，使机械手100实际上沿最终的更新轨道tb自当前姿势ps向最终姿势pt活动。即，以如下形式构成：当步骤s90中将经过姿势pv再设定为新的当前姿势ps时，控制部21并不使第一臂2、第二臂3、手部4向该当前姿势ps活动，而是将所决定的更新轨道tb的历程存储至存储部22。然而，并不限于此，亦可每当步骤s90中将经过姿势pv决定为新的当前姿势ps时，由控制部21使第一臂2、第二臂3及手部4实际上向该当前姿势ps活动。

如以上说明所述，本发明的机械手100算出当机械手100自当前姿势ps向最终姿势pt变化时与手部4的初始轨道ta上的规定的点对应的机械手100的假想姿势pp，且判定假想姿势pp下的机械手100的几何模型m1～m3是否干扰对应于障碍物而规定（划定）的禁止进入区域s1。并且，当判定为不干扰时，将假想姿势pp决定为当自当前姿势ps向最终姿势pt变化时经过的经过姿势pv，而当判定为产生干扰时，算出因假想斥力fi使几何模型的干扰部分自禁止进入区域s1推出至动作区域s2的状态下的姿势，且将该姿势决定为经过姿势pv。因此，当机械手100的几何模型干扰禁止进入区域s1时，赋予不干扰禁止进入区域s1的经过姿势pv，故而，可回避与障碍物的冲突。

另一方面，当机械手100的几何模型不干扰禁止进入区域s1时，赋予与初始轨道ta上的点对应的经过姿势pv，故而，接近最终姿势pt。并且，根据上述结构，将新赋予的经过姿势pv作为当前姿势ps且反复实施该程序，故而，每当机械手100的几何模型不干扰禁止进入区域s1的程序出现时，均赋予与该程序的初始轨道ta上的点对应的经过姿势pv，且接近最终姿势pt。因此，借由反复进行试行错误，最终，机械手100可一面回避与障碍物的冲突一面获得当自当前姿势ps变化为最终姿势pt时手部4的更新轨道tb。

结果，借由机械手的几何模型、禁止进入区域及动作区域的设定这样的简单设定，从而，不论机械手停止于哪个位置均能自动回避与障碍物的干扰，使机械手安全地到达规定姿势。

（实施形态2）

以下，关于实施形态2的结构、动作，以与实施形态1的不同点为中心进行叙述。

上述实施形态1中，初始轨道ta是借由赋予当前姿势ps及最终姿势pt而唯一性地决定的轨道。而且，步骤s70中决定的经过姿势pv是于假想姿势pp下的各几何模型与边界线b的位置关系下唯一性地决定的姿势。借此，实施形态1中，例如，当因斥力而回推的方向与初始轨道ta的方向一致时，根据被假想斥力fi推出的状态下的姿势而决定的更新轨道tb有时不接近最终姿势pt。

本实施形态是考虑到更新轨道tb不接近最终姿势pt的情况的实施形态。

图8是表示本发明的实施形态2的机械手100的动作例的流程图。

本实施形态中，步骤s70的经过姿势的决定以如下形式执行。

首先，判定是否陷入停留状态（停留状态判定步骤）（步骤s271）。本实施形态中，判定是否陷入停留状态，是根据以下式子（5）算出假想姿势pp与最终姿势pt下各关节轴的关节角度的位移的绝对值的总和e。

[数5]

其中，

n是轴数，

qri是i轴的假想姿势pp下的关节角度，

qgi是i轴的目标姿势pt下的关节角度。

并且，若e值减小，则判定为接近最终姿势。另一方面，若e值未变化或者增加，则判定为不接近最终姿势pt、或者远离最终姿势pt。并且，若e值未变化或者增加的状态以规定的时间间隔持续，则判定为陷入停留状态。

并且，若步骤s271中控制部21判定为并非陷入停留状态，则执行步骤s71～s74。步骤s71～s74与上述实施形态1中的步骤s71～s74相同，故而，省略其说明。

另一方面，若步骤s271中控制部21判定为陷入停留状态，则控制部21的经过姿势决定部39算出当前姿势ps附近的、被假想斥力fi推出的状态下的姿势以外的经过候补姿势pvc（经过候补姿势生成步骤）（步骤s272）。本实施形态中，经过候补姿势pvc是随机生成的姿势。然而，并不限于此。亦可取而代之而例如根据当前姿势ps周围的状况生成。

继而，判定经过候补姿势pvc是否为比当前姿势ps接近最终姿势pt的姿势（第一判定步骤）（步骤s273）。本实施形态中，判定经过候补姿势pvc是否为接近最终姿势pt的姿势，是借由将根据以下式子（6）算出的e值与根据以下式子（7）算出的e值进行比较而进行。

[数6]

其中，

n是轴数，

qgi是i轴的目标姿势pt下的关节角度，

qri是i轴的经过候补姿势pvc下的关节角度，

qi是i轴的当前姿势ps下的关节角度。

即，上述式子（6）求出经过候补姿势pvc与最终姿势pt下各关节轴的关节角度的位移的绝对值的总和e（qr）。而且，上述式子（7）求出当前姿势ps与最终姿势pt下各关节轴的关节角度的位移的绝对值的总和e（q）。并且，上述式子（8）是将e（qr）与e（q）的值进行比较的式子。

并且，例如，上述式子（8）中，若δ≤0，即，若经过候补姿势pvc的e（qr）值等于或小于当前姿势ps的e（q）值，则判定为是接近最终姿势pt的姿势。另一方面，若δ＞0，即，若经过候补姿势pvc的e（qr）值大于当前姿势ps的e（q）值，则判定为并非接近最终姿势pt的姿势。

并且，若控制部21的经过姿势决定部39判定为是接近最终姿势pt的姿势（步骤s273中为是），则根据经过候补姿势pvc决定经过姿势pv（第一决定步骤）（步骤s274）。进而，判定自当前姿势至该经过姿势pv的轨道是否干扰禁止进入区域s1。

另一方面，若控制部21的经过姿势决定部39判定为并非接近最终姿势pt的姿势（步骤s273中为否），则进而决定是否将经过候补姿势pvc选择为经过姿势pv（第二判定步骤）（步骤s275）。

该选择使用概率值进行。本实施形态中，该选择是利用以下式子（9）算出与经过候补姿势pvc的评估值相应的转移概率p（δ，t），且根据该概率，决定是否将经过候补姿势pvc选择为经过姿势pv。而且，以图中的a所示的线来例示该转移概率的坐标图（graph）。

[数7]

其中，

t是参数，

δ由式子（8）算出。

借此，就转移概率p（δ，t）而言，δ越接近0，即，经过候补姿势pvc较之当前姿势ps、背离（远离）最终姿势pt的程度越小，则经过候补姿势pvc被选择为经过姿势pv的可能性越高（接近1），而经过候补姿势pvc较之当前姿势ps、背离最终姿势pt的程度越大，则经过候补姿势pvc被选择为经过姿势pv的可能性越低（接近0）。

并且，若经过候补姿势pvc被选择为经过姿势pv，则根据经过候补姿势pvc决定经过姿势pv（第二决定步骤）（步骤s276）。

并且，控制部21与上述实施形态相同地执行步骤s80之后的处理。

再者，于经过候补姿势pvc较之当前姿势ps、背离最终姿势pt的程度较小的姿势下，有时难以发现能脱离停留状态的姿势。此时，亦可如图9的坐标图中的b表示的线所示以如下形式进行调整：当陷入停留状态时，借由增大上述式子（9）的参数t的值，使得即便为较之当前姿势ps、背离最终姿势pt的程度相对较大的经过候补姿势pvc亦容易被选择。

而且，较佳为，随着当前姿势ps接近最终姿势pt，而降低选择背离最终姿势pt的姿势的概率。因此，也可以是，随着当前姿势ps接近最终姿势pt，如图9的坐标图中b表示的线所示，借由减小参数t的值，而降低选择背离最终姿势pt的姿势的概率。

如此，本实施形态中，就机械手100而言，例如当被斥力fi推出的方向与初始轨道ta的方向一致时，推出至动作区域s2的姿势与初始轨道ta上的点对应，故而，几何模型会反复干扰禁止进入区域s1而陷入停留状态，机械手100并不接近最终姿势pt（无法到达最终姿势）。在如这样的情况下，能获得与并非初始轨道ta上的点对应的经过姿势pv，且能根据该姿势决定更新轨道tb，且经由接近最终姿势pt的其他经过姿势pv而到达最终姿势pt。

而且，即便无法算出其他接近最终姿势pt的姿势，亦可尝试是否可经由别的姿势而到达最终姿势pt。借此，可适当地脱离当前的无法到达最终姿势pt的状态。并且，根据和最终姿势pt与经过候补姿势pvc的背离度成比例地下降的概率值，判定是否选择经过候补姿势，故而，与当前姿势ps的背离度小的经过候补姿势pvc被选择为经过姿势pv的可能性变大。借此，可更佳地适当脱离当前的无法到达最终姿势的状态。

图10a及图10b是表示本实施形态的机械手100的模拟条件的图。如图10a所示，设定机械手100的当前姿势ps、禁止进入区域s1、动作区域s2、几何模型m。而且，如图10b所示，设定机械手100的最终姿势pt。再者，最终姿势pt下的手部的教示点为[x，y]＝[0，450]。

图10c是表示机械手100的模拟结果的图，且是表示手部的轨迹的图。如图10c所示，可知，机械手100自当前姿势ps到达最终姿势pt。

＜变形例＞

上述实施形态中，由水平多关节机械手构成机械手100，于xy平面表现几何模型，未判定z轴方向上干扰的有无，但并不限于此。亦可取而代之，由垂直多关节构成机械手、立体地表现几何模型、于三维空间进行干扰的判定。

而且，上述实施形态中，假想姿势算出部36算出机械手100的假想姿势pp，但亦可取而代之而算出机械手100的几何模型的假想姿势pp。

根据上述说明可知，本领域技术人员了解本发明的多种改良或其他实施形态。因此，上述说明应仅作为例示进行解释，且是为了将执行本发明的最佳形态教示给本领域技术人员而提供。可于不脱离本发明的精神的情况下，实质性地变更其构造及/或者功能的细节。

产业应用性

本件发明可适用于产业用机械手。

符号说明

b边界线

l1转动轴线

l2转动轴线

l3转动轴线

m1几何模型

m2几何模型

m3几何模型

pp假想姿势

ps当前姿势

pt最终姿势

ptc最终候补姿势

pv经过姿势

pvc经过候补姿势

s1禁止进入区域

s2动作区域

ta初始轨道

tb更新轨道

1基台

2第一臂

2a第一关节轴

3第二臂

3a第二关节轴

4手部

4a第三关节轴

5控制器

12第一臂驱动部

13第二臂驱动部

14手部驱动部

15第一臂角度位置检测部

16第二臂角度位置检测部

17手部角度位置检测部

21控制部

22存储部

31第一臂控制部

32第二臂控制部

33手部控制部

34最终姿势决定部

35初始轨道决定部

36假想姿势算出部

37几何模型表现部

38干扰判定部

39经过姿势决定部

40区域设定部

41更新轨道决定部

100机械手

110设备。