CN105690410A - 干涉避免方法、控制装置以及程序 - Google Patents

Abstract

本发明涉及干涉避免方法、控制装置以及程序。本发明所涉及的干涉避免方法避免机械臂(120)与障碍物干涉，该机械臂(120)通过上臂部(122)与前臂部(124)经由肘部(134)连接而构成。计算机械臂(120)的两端的位置被固定的状态下的、上臂部(122)以及前臂部(124)的可动区域。计算包含障碍物的障碍物区域的边界面上的第一线与可动区域的交点。根据所计算出的交点，决定在可动区域中机械臂(120)不与障碍物区域干涉的干涉避免范围。

Description

干涉避免方法、控制装置以及程序

技术领域

本发明涉及干涉避免方法、控制装置以及程序，特别是涉及避免机械臂的干涉的干涉避免方法、控制装置以及程序。

背景技术

作为控制具有多个关节的机械臂的动作从而实时地操作机械臂的方法，例如有主从式方法。在主从式方法中，主动侧装置(控制装置)指定控制值，从动侧装置(机械臂)根据该被指定的控制值动作。在作为所指定的控制值而指定机械臂的手部位置的情况下，使用运动学逆运算计算各关节的关节角度。

这里，在机器人的周围存在机器人的躯体等障碍物。因此，在指定手部位置而控制机械臂的情况下，不仅需要计算各关节的关节角度，还需要计算使得机械臂不与躯体等障碍物干涉的解(干涉避免解)。而且，在实时地操作机械臂的情况下，需要实时地进行上述关节角度的计算和干涉避免解的计算。

与上述技术相关联，日本特开2011－093015号公报中公开了实现考虑了机械臂的手部的手眼传感器的存在的、与周边的干涉防止的手眼式料箱拣配(HAND－EYEBINPICKING)机器人的控制装置。日本特开2011－093015号公报所涉及的控制装置具有：存储干涉图案表的表存储构件、判定上述手眼传感器与上述机械臂是否干涉的移动目的地干涉判定构件、以及姿势再决定构件。

这里，干涉图案表定义：机械臂的各自由轴的旋转角度中的、上述手眼传感器与上述机械臂干涉的组合图案。另外，移动目的地干涉判定构件基于以使得末端执行器在工件的位置处成为所决定的姿势的方式驱动上述机械臂的情况下的、上述各自由轴的旋转角度的组合图案与在上述干涉图案表中定义的组合图案之间的对照，判定手眼传感器与上述机械臂是否干涉。在判定为干涉的情况下，姿势再决定构件重新决定适于上述工件的拣配的上述末端执行器的姿势。

在日本特开2011－093015号公报所记载的技术中，需要事先准备干涉图案表。存在为了准备该图案表而花费大量时间的顾虑。因此，即便使用日本特开2011－093015号公报所记载的技术，也难以实时地计算干涉避免解。

发明内容

本发明正是为了解决这样的课题而完成的，提供能够高速地进行用于避免机械臂的干涉的计算的干涉避免方法、控制装置以及程序。

本发明所涉及的干涉避免方法是避免机械臂与障碍物干涉的机械臂的干涉避免方法，该机械臂通过第一臂与第二臂经由关节部连接而构成，其中，计算上述机械臂的两端的位置被固定的状态下的、上述第一臂以及上述第二臂的可动区域；计算包含上述障碍物的障碍物区域的边界面上的第一线与上述可动区域的交点；基于上述计算出的交点，决定在上述可动区域中上述机械臂不与上述障碍物区域干涉的干涉避免范围。

另外，本发明所涉及的控制装置是控制第一臂和第二臂经由关节部连接而构成的机械臂的控制装置，其中，上述控制装置具有：可动区域计算构件，计算上述机械臂的两端的位置被固定的状态下的、上述第一臂和上述第二臂的可动区域；交点计算构件，计算包含障碍物的障碍物区域的边界面上的第一线与上述可动区域的交点；以及干涉避免范围决定构件，基于上述计算出的交点，决定在上述可动区域中上述机械臂不与上述障碍物区域干涉的干涉避免范围。

另外，本发明所涉及的程序实现避免机械臂与障碍物干涉的机械臂的干涉避免方法，该机械臂通过第一臂和第二臂经由关节部连接而构成，其中，上述程序使计算机执行以下步骤：计算上述机械臂的两端的位置被固定的状态下的、上述第一臂和上述第二臂的可动区域的步骤；计算包含上述障碍物的障碍物区域的边界面上的第一线与上述可动区域的交点的步骤；以及基于上述计算出的交点，决定在上述可动区域中上述机械臂不与上述障碍物区域干涉的干涉避免范围的步骤。

本发明以上述方式构成，由此，能够通过简单的几何学方法决定机械臂的干涉避免范围。因此，本发明能够高速地进行用于避免机械臂的干涉的计算。

另外，优选形成为：上述可动区域由以在上述机械臂的两端的位置被固定的状态下上述关节部旋转而得的轨迹所形成的圆作为底面、以上述第一臂以及上述第二臂分别旋转而得的轨迹所形成的旋转面作为圆锥侧面的圆锥形成，在由上述第二臂的轨迹形成的第一圆锥和由上述第一臂的轨迹形成的第二圆锥，分别计算上述交点。

通过分别进行计算，能够简单地应用计算圆锥与线的交点之类的简单的计算方法。因此，本发明能够更高速地决定机械臂的干涉避免范围。

另外，优选形成为：在与上述第一线的交点存在于上述底面上的情况下，将基于上述第一圆锥判定出的第一范围与基于上述第二圆锥判定出的第二范围重复的范围，决定为上述机械臂的上述干涉避免范围。

通过形成为上述结构，即使当在干涉避免范围的一方的边界第二臂与障碍物干涉，而在另一方的边界第一臂与障碍物干涉的情况下，也能够通过简单的几何学方法决定干涉避免范围。因此，本发明能够更高速地决定机械臂的干涉避免范围。

另外，优选形成为：基于与上述第一圆锥相关的上述圆锥侧面上的交点和上述底面上的交点判定上述第一范围，基于与上述第二圆锥相关的上述圆锥侧面上的交点和上述底面上的交点判定上述第二范围。

通过以上述方式构成，能够通过简单的几何学方法，针对第一圆锥以及第二圆锥分别判定第一范围以及第二范围。因此，本发明能够更高速地决定机械臂的干涉避免范围。

另外，优选形成为：上述障碍物区域的上述边界面的至少一部分由曲面形成。

若边界面的方向不连续地形成，则担心机械臂的动作变得剧烈。在这样的情况下，担心产生机械臂的超速或者超载。另一方面，本发明通过形成为上述结构，机械臂的动作的变化得以缓和。因此，能够抑制机械臂的超速或者超载。

另外，优选形成为：将通过上述机械臂的手部的位置并包含与上述曲面相切的切线的切点的线，决定为上述曲面上的上述第一线。

通过形成为上述结构，即便在上述边界面的至少一部分由曲面形成的情况下，也能够通过简单的几何学方法容易地决定干涉避免范围。因此，对于本发明，即便在上述边界面的至少一部分由曲面形成的情况下，也能够高速地决定机械臂的干涉避免范围。

根据本发明，能够提供可高速地进行用于避免机械臂的干涉的计算的干涉避免方法、控制装置以及程序。

通过参照作为并非用于限制本发明的例子的附图并阅读以下的详细说明，能够进一步清楚本发明的其它特征、目的、优点。

附图说明

图1是示出实施方式1所涉及的机器人***的简图。

图2是用于说明实施方式1所涉及的臂角度的图。

图3是示出实施方式1所涉及的控制装置的结构的功能框图。

图4是示出实施方式1所涉及的控制装置的处理的流程图。

图5是用于说明实施方式1所涉及的可动区域的计算方法的图。

图6是用于定义由实施方式1所涉及的可动区域计算部计算出的圆锥的各尺寸的图。

图7A是示出由实施方式1所涉及的可动区域计算部计算出的圆锥的图。

图7B是示出由实施方式1所涉及的可动区域计算部计算出的圆锥的图。

图8A是示出实施方式1所涉及的机器人的躯体与机械臂之间的关系的图。

图8B是示出实施方式1所涉及的机器人的躯体与机械臂之间的关系的图。

图8C是示出实施方式1所涉及的机器人的躯体与机械臂之间的关系的图。

图9是例示图7A所示的圆锥的侧面与干涉判定线段之间的交点存在两个的情况的图。

图10是用于说明图9的例子的臂角度的存在范围的判定方法的图。

图11是例示图7A所示的圆锥的底面与干涉判定线段之间的交点存在一个的情况的图。

图12是用于说明图11的例子的臂角度φ的存在范围的判定方法的图。

图13是例示图7A所示的圆锥的侧面与干涉判定线段之间的交点存在一个情况的图。

图14是用于说明使用运动学逆运算的关节角度的计算方法的图。

图15是示出实施方式1所涉及的、目标手部位置在躯体的角部附近的情况下的机械臂的动作的俯视图。

图16是示出实施方式2所涉及的、目标手部位置在躯体的角部附近的情况下的机械臂的动作的俯视图。

图17是示出实施方式2所涉及的控制装置的结构的功能框图。

图18是示出实施方式2所涉及的控制装置的处理的流程图。

图19是用于说明决定实施方式2的圆锥CL(前臂部的轨迹)的干涉判定线段的方法的图。

图20是用于说明决定实施方式2所涉及的切点的方法的图。

图21是用于说明决定实施方式2所涉及的圆锥CU(上臂部的轨迹)的干涉判定线段的方法的图。

图22是用于说明决定实施方式2所涉及的切点的其它方法的图。

图23是示出使采用了本实施方式的机器人的动作再现的仿真结果的图。

图24是示出使采用了本实施方式的机器人的动作再现的仿真结果的图。

图25是示出使采用了本实施方式的机器人的动作再现的仿真结果的图。

图26是示出使采用了本实施方式的机器人的动作再现的仿真结果的图。

具体实施方式

(实施方式1)

以下，参照附图对本发明的实施方式进行说明。

图1是示出实施方式1所涉及的机器人***1的简图。机器人***1具有机器人100和控制机器人的动作的控制装置2。机器人100具有躯体110和机械臂120。躯体110具有躯体前表面112和躯体侧面114。

此外，实际的机器人100的主体(实际躯体)由以任意的多个曲面构成的形状形成，但在本实施方式中，示意性地将躯体110看作包络实际躯体的长方体Bd。另外，本实施方式中，以躯体110作为成为机械臂120的干涉对象的障碍物。因此，本实施方式中，躯体110是包含障碍物的障碍物区域(与上述“长方体Bd”对应)，躯体前表面112以及躯体侧面114是包含障碍物的障碍物区域的边界面。

机械臂120连接于躯体侧面114。此外，图1仅示出了与机器人100的右臂对应的机械臂120，但机器人100也可以具有与左臂对应的机械臂120。并且，机器人100也可以具有一个以上(例如两个)脚。以下，对与机器人100的右臂对应的机械臂120的控制方法(干涉避免方法)进行说明，但并不限定于此。本实施方式所涉及的控制方法也能够应用于与机器人100的左臂对应的机械臂以及机器人100的脚。即，“臂(机械臂)”包含与手臂相当的臂、与脚相当的臂、以及手臂型机器人的臂。

另外，机械臂120从接近躯体110一侧起依次由肩部132、上臂部122(第一臂)、肘部134(关节部)、前臂部124(第二臂)、手腕部136、手部126构成。在躯体110(躯体侧面114)连接有肩部132。另外，上臂部122与前臂部124经由肘部134连接。另外，前臂部124与手部126经由手腕部136连接。另外，将上臂部122的长度设为L₁，将前臂部124的长度设为L₂。此外，在后述的控制装置2的计算中，上臂部122以及前臂部124分别被假定为长度L₁以及L₂的线段。

另外，肩部132具有分别绕相互正交的三个轴旋转的三个关节(第一关节部141、第二关节部142、第三关节部143)。另外，肘部134具有绕一个轴旋转的一个关节(第四关节部144)。另外，手腕部136具有分别绕相互正交的三个轴旋转的三个关节(第五关节部145、第六关节部146、第七关节部147)。

另外，将第一关节部141的关节角度设为θ₁，第二关节部142的关节角度设为θ₂，第三关节部143的关节角度设为θ₃。同样，将第四关节部144的关节角度设为θ₄，第五关节部145的关节角度设为θ₅，第六关节部146的关节角度设为θ₆，第七关节部147的关节角度设为θ₇。

此外，本实施方式中，在后述的控制装置2的计算中，用一个点模拟肩部132。即，假定第一关节部141、第二关节部142、第三关节部143在一个点处交叉。同样，在后述的控制装置2的计算中，用一个点模拟手腕部136。即，假定第五关节部145、第六关节部146、第七关节部147在一个点处交叉。在该情况下，可以认为肩部132的三个关节(第一关节部141、第二关节部142、第三关节部143)以及手腕部136的三个关节(第五关节部145、第六关节部146、第七关节部147)分别构成假设球面构造。并且，本实施方式的机械臂120具有7个关节，因此是具有7自由度的机械手。

这里，定义以肩部132为原点O的坐标系。将通过原点O并以前侧(躯体前表面112侧)方向(从图1的纸面里侧朝近前侧的方向)为正方向的坐标轴设为x轴。另外，将通过原点O并以从躯体的右侧朝向左侧的方向(图1的从左向右的方向)为正方向的坐标轴设为y轴。另外，将通过原点O并以朝向上侧的方向(图1的上方向)为正方向的坐标轴设为z轴。

控制装置2具有例如作为计算机的功能。控制装置2可以搭载于机器人100的内部，也可以与机器人100以能够经由有线或者无线通信的方式连接。控制装置2控制机器人100的动作、特别是机械臂120的动作。即，在机器人***1中，控制装置2具有作为主动装置的功能，机器人100(机械臂120)具有作为从动装置的功能。这里，控制装置2决定机械臂120的各关节角度以避免机械臂120与躯体110等障碍物干涉。详细内容后述。

这里，对本实施方式中使用的“臂角度”这一指标进行说明。“臂角度”在文献“考虑了关节的可动范围的7自由度冗余机械手的解析逆运动学解法”(日本ロボット学会誌Vol.25No.4,pp.606～617,2007)中记载。以下，简单地进行说明。

一般地，空间内的机械手的手部的位置以及姿势可由6个参数唯一地表现。因此，在7自由度机械手中，存在一个多余的自由度。由于该冗余自由度，在7自由度机械手中，能够进行不影响手部的位置以及姿势的机构内部的1自由度的自运动。这里，如上所述，本实施方式所涉及的机械臂120具有7自由度。因此，本实施方式所涉及的机械臂120具有一个冗余自由度。在本实施方式中，使用“臂角度”这一指标作为表示该冗余自由度的参数。

图2是用于说明实施方式1所涉及的臂角度的图。如上所述，肩部132的三个关节以及手腕部136的三个关节分别构成假想的球面构造。由此，即便指定手部126的位置以及姿势，并固定机械臂120的两端即肩部132的位置(点O)和手部126的位置(点P)，由于冗余自由度，肘部134的位置(点e)也会变化。此外，以下，手部126的位置假想地用点表示。即，假定手腕部136以及手部126的位置彼此相同。

如图2所示，若指定手部126的位置(点P)，则能够确定以肩部132(点O)、肘部134(点e)、手部126(点P)为顶点、以上臂部122、前臂部124、线段OP为三边的三角形O－e－P。此时，三角形O－e－P能够以线段OP为中心如箭头A所示那样旋转。将像这样以线段OP为中心而使肘部134的位置(点e)旋转的情况下的肘部134的位置(点e)的旋转角度设为臂角度φ。另外，若在肘部134的位置位于预先决定的参照平面上的情况下将该位置设为点e₀，则三角形O－e₀－P的臂角度为φ＝0。而且，将从点P观察点O而使点e顺时针旋转的方向(箭头A的方向)设为臂角度的正方向。换言之，按照使大拇指沿从点P朝向点O的方向竖起的右手法则来定义臂角度的正方向。若使臂角度φ沿正方向旋转，则肘部134的位置e从φ＝0的点e₀起向e₁、e₂、e₃移动。

另外，在像这样以线段OP为中心而使肘部134的位置(点e)旋转的情况下，上臂部122以及前臂部124所描绘的轨迹分别是手部126的位置(点P)被指定的情况下的上臂部122以及前臂部124的可动区域。上臂部122以及前臂部124的可动区域由以使肘部134旋转而得的轨迹所形成的圆作为底面、以上臂部122以及前臂部124旋转而得的轨迹所形成的旋转面分别作为侧面的圆锥形成。

图3是示出实施方式1所涉及的控制装置2的结构的功能框图。控制装置2作为主要的硬件结构具有CPU(CentralProcessingUnit，中央处理单元)4、ROM(ReadOnlyMemory，只读存储器)6、RAM(RandomAccessMemory，随机访问存储器)8。CPU4具有作为进行控制处理以及运算处理等的运算装置的功能。ROM6具有用于存储由CPU4执行的控制程序以及运算程序等的功能。RAM8具有用于暂时存储处理数据等的功能。

另外，控制装置2具有信息存储部12、手部位置决定部14、可动区域计算部16、交点计算部18、干涉避免范围决定部20、关节角度计算部22以及机械臂控制部24(以下称为“各构成要素”)。信息存储部12、手部位置决定部14、可动区域计算部16、交点计算部18、干涉避免范围决定部20、关节角度计算部22以及机械臂控制部24分别具有作为信息存储构件、手部位置决定构件、可动区域计算构件、交点计算构件、干涉避免范围决定构件、关节角度计算构件以及机械臂控制构件的功能。各构成要素例如能够通过CPU4执行存储于ROM6的程序来实现。另外，可以将必要的程序预先记录于任意的非易失性记录介质(非暂存性计算机可读介质)，并根据需要来进行安装(install)。此外，各构成要素并不限定于如上所述通过软件实现，也可以通过某些电路元件等硬件实现。使用图4来说明各构成要素的功能。

图4是示出实施方式1所涉及的控制装置2的处理的流程图。首先，在步骤S102中，控制装置2将机械臂120的控制所需要的信息存储于信息存储部12，由此来进行事先设定。信息存储部12存储上臂部122的长度L₁以及前臂部124的长度L₂。另外，信息存储部12存储与躯体110(障碍物区域)有关的位置信息。具体而言，信息存储部12存储以肩部132作为原点O的xyz坐标系中的、躯体110的位置信息(坐标数据)。更具体地说，信息存储部12存储表示躯体前表面112的位置信息(坐标数据)以及表示躯体侧面114的位置信息(坐标数据)。并且，信息存储部12存储表示图1所示的线段AB的位置信息(坐标数据)。该线段AB如后所述作为用于判定干涉的避免的干涉判定线段Ld(第一线)使用。

此外，存储于信息存储部12的躯体110的坐标数据示出考虑了实际的机械臂120的粗细的、为了检测干涉而使用的与躯体110对应的障碍物的三维形状。另外，躯体110的坐标数据示出包络实际的躯体110的障碍物区域的边界面的坐标。即，与躯体110对应的障碍物区域的坐标数据包含与机械臂120的粗细对应的余量(margin)。

接下来，在步骤S104中，控制装置2决定手部126的位置(目标手部位置)。具体而言，控制装置2的手部位置决定部14接收操作者使用键盘、鼠标或者触感设备(hapticdevice)等的输入构件输入的目标手部位置的坐标数据。由此，手部126的位置以及姿势被指定。

接下来，在步骤S106中，控制装置2进行手部126的干涉的判定。这里，S106中的干涉的判定是手部126是否与躯体110干涉的判定。具体而言，手部位置决定部14根据所指定的目标手部位置判定手部126是否与躯体110干涉。例如，手部位置决定部14可以用多边形表示手部126以及躯体110，并使用OBB(OrientedBoundingBox：方向包围盒)判定手部126是否与躯体110干涉。

在产生手部126的干涉的情况下(S106为是)，在步骤S140中，控制装置2进行目标手部位置的修正处理。具体而言，控制装置2取消在S102的处理中被指定的目标手部位置。而且，控制装置2通知操作者由于在操作者所指定的目标手部位置产生干涉因此要再次指定目标手部位置。

另一方面，当未产生手部126的干涉的情况下(S106为否)，在步骤S108中，控制装置2计算机械臂120的可动区域。具体而言，控制装置2的可动区域计算部16使用上述的臂角度φ，计算上臂部122以及前臂部124的可动区域。另外，此时计算的“可动区域”并不考虑机械臂120与周围障碍物之间的干涉。以下，对可动区域计算部16中的计算方法进行说明。

图5是用于说明实施方式1所涉及的可动区域的计算方法的图。在S104中，假设手部126的位置P被指定为P₀[X，Y，Z]^T。可动区域计算部16如图5的(A)所示，在保持肩部132的位置(原点O)与手部126的位置P之间的距离不变的情况下，使手部126的位置P位于z轴的铅垂下方的位置(点P’)。此时，点P’的坐标为并且，可动区域计算部16使肘部134的位置e位于xz平面上的x＜0的区域的位置(e’)。由此，以肩部132(点O)、肘部134(点e)、手部126(点P)作为顶点的三角形O－e－P(O－e’－P’)存在于xz平面上。

而且，可动区域计算部16如图5的(B)所示，以使得手部126的位置P与在S104的处理中被指定的手部位置P₀一致的方式，使三角形O－e－P绕x轴以旋转角α进行旋转，绕y轴以旋转角β进行旋转。具体而言，可动区域计算部16使用以下的式1以及式2，将点P’以及点e’分别变换为点P₀以及点e₀。

(式1)

P₀＝R_βR_αP’

(式2)

e₀＝R_βR_αe’

这里，在式1以及式2中，P’、e’、P₀以及e₀分别是与点P’、点e’、点P₀以及点e₀有关的位置矢量。另外，R_α是表示旋转角α的旋转矩阵。另外，R_β是表示旋转角β的旋转矩阵。另外，机械臂120的连杆长度(L₁以及L₂)已预先决定，因此能够计算e₀以及e’，能够根据上述式1以及式2，唯一地计算α以及β。

可动区域计算部16将包含图5的(B)所示的旋转后的三角形O－e₀－P₀(用斜线表示)的平面设为作为臂角度的基准的参照平面。即，可动区域计算部16假设当肘部134的位置e为点e₀的情况下臂角度φ＝0。

而且，如图5的(C)所示，可动区域计算部16在将肩部132固定于原点O、将手部126固定于点P₀的状态下，计算使上臂部122以及前臂部124绕OP沿箭头A的方向旋转的轨迹。该轨迹是手部126的位置(点P)被指定为点P₀的情况下的上臂部122以及前臂部124的可动区域。而且，可动区域由以使肘部134旋转而得的轨迹所形成的圆作为底面、以由上臂部122以及前臂部124旋转而得的轨迹所形成的旋转面分别作为侧面的圆锥形成。换言之，机械臂120的可动区域由底面相互对接的两个圆锥形成。

图6是用于定义由实施方式1所涉及的可动区域计算部16计算出的圆锥的各尺寸的图。可动区域计算部16计算由前臂部124的轨迹形成的圆锥CL(第一圆锥)以及由上臂部122的轨迹形成的圆锥CU(第二圆锥)各自的尺寸。这里，将线段OP的长度设为L₀。另外，圆锥CU的底面与圆锥CL的底面相同，因此各自的底面即圆的半径r相同。这里，将圆锥CU的高度设为k₁，将圆锥CL的高度设为k₂。此时，k₁+k₂＝L₀。另外，此时，若将从肘部134的位置e到线段OP所引的垂线的垂足设为H，则k₁＝OH，k₂＝PH。此外，圆锥CU是使三角形O－e－H绕线段OH旋转而得的轨迹。同样，圆锥CL是使三角形P－e－H绕线段PH旋转而得的轨迹。

这里，如上所述，上臂部122的长度L₁以及前臂部124的长度L₂已预先决定。另外，通过将手部126的位置指定为点P₀，线段OP的长度也被确定。因此，三角形O－e－P与臂角度无关而被唯一地确定。因此，可动区域计算部16能够使用余弦定理计算r、k₁、k₂。

并且，对于前臂部124的轨迹即圆锥CL，可动区域计算部16以使得顶点(点P)位于z’轴上、底面位于x’y’平面上的方式，进行从坐标系(x，y，z)向坐标系(x’，y’，z’)的坐标变换。具体而言，可动区域计算部16使用以下的式3来进行针对圆锥CL的坐标变换。

(式3)

$\left[\begin{array}{c}{x}^{,}\\ y^{,}\\ z^{,}\end{array}\right]=\left[\begin{array}{ccc}1& 0& 0\\ 0& 1& 0\\ 0& 0& -1\end{array}\right]{R_{\mathrm{\α}}}^{-1}{R_{\mathrm{\β}}}^{-1}\left[\begin{array}{c}x\\ y\\ z\end{array}\right]+\left[\begin{array}{c}0\\ 0\\ -k_1\end{array}\right]$

在式3中，通过使用在上述式1以及式2中使用的旋转矩阵R_α以及旋转矩阵R_β，以使得线段PH位于z’轴上的方式变换坐标系。这相当于将图5的(A)所示的坐标系从(x，y，z)置换为(x’，y’，z’)并使前臂部124绕OP旋转而得的轨迹。另外，由于顶点(点P)位于z’轴的正向侧，因此坐标系的z轴(z’轴)的符号从正变为负。并且，为了使底面移动至x’y’平面(即z’＝0的平面)上，坐标系朝Z’轴方向偏移－k₁。由此，图6所示的点H与坐标系(x’，y’，z’)的原点O’一致。

并且，可动区域计算部16按照以下的式4计算变换后的圆锥底面的式，按照以下的式5计算变换后的圆锥面(侧面)的式。

(式4)

x’²+y’²≤r²,z’＝0

(式5)

k₂ ²(x’²+y’²)＝r²(z’-k₂)²,{z’|0≤z’≤k₂}

同样，针对所计算出的圆锥CU，可动区域计算部16以使得顶点(点O)位于z’轴上、底面位于x’y’平面上的方式，进行从坐标系(x，y，z)向坐标系(x’，y’，z’)的坐标变换。具体而言，可动区域计算部16使用以下的式6进行针对圆锥CU的坐标变换。

(式6)

$\left[\begin{array}{c}{x}^{,}\\ y^{,}\\ z^{,}\end{array}\right]=\left[\begin{array}{ccc}1& 0& 0\\ 0& 1& 0\\ 0& 0& 1\end{array}\right]{R_{\mathrm{\α}}}^{-1}{R_{\mathrm{\β}}}^{-1}\left[\begin{array}{c}x\\ y\\ z\end{array}\right]+\left[\begin{array}{c}0\\ 0\\ k_1\end{array}\right]$

式6中，与式3相同，使用在上述式1以及式2中使用的旋转矩阵R_α以及旋转矩阵R_β，以使得线段OP位于z’轴上的方式变换坐标系。另外，为了使底面移动至x’y’平面(即z’＝0的平面)上，坐标系朝z’轴方向偏移k₁。由此，图6所示的点H与坐标系(x’，y’，z’)的原点O’一致。

并且，可动区域计算部16按照上述式4计算变换后的圆锥底面的式，按照以下的式7计算变换后的圆锥面(侧面)的式。

(式7)

k₁ ²(x’²+y’²)＝r²(z’-k₁)²,{z’0≤z’≤k₁}

图7A以及图7B是示出由实施方式1所涉及的可动区域计算部16计算出的圆锥的图。如上，可动区域计算部16计算图7A所示的圆锥CL，计算图7B所示的圆锥CU。这里，若对肘部134的位置e位于参照平面的情况下(图5的(B))的位置e₀进行坐标变换，则变换后的位置e₀’的坐标变为[－r，0，0]。在该位置，臂角度为φ＝0。另外，臂角度的正方向对于图7B所示的圆锥CU而言，根据图2的箭头A，从z’轴的正方向观察为逆时针方向。另一方面，对于圆锥CL而言，伴随着在坐标变换时z轴(z’轴)的符号从正变为负，臂角度的正方向如图7A所示与圆锥CU相反，从z’轴的正方向观察为顺时针。

接下来，在步骤S110中，控制装置2计算可动区域与干涉判定线段Ld之间的交点。并且，在步骤S112～S120中，控制装置2使用所计算出的交点，决定机械臂120在可动区域不与躯体110干涉的范围(干涉避免范围)。以下，对本实施方式中的、使用了臂角度的决定干涉避免范围的机理进行说明。

如上所述，在指定了手部126的位置的情况下，机械臂120的可动区域用使臂角度φ旋转而得的轨迹即两个圆锥CU以及CL表示。这里，为了避免躯体110与机械臂120之间的干涉，只要机械臂120存在于躯体110的外部即可。换言之，只要圆锥CU的侧面上的连结肩部132与肘部134的线段(圆锥CU的母线)、以及圆锥CL的侧面上的连结肘部134与手部126的线段(圆锥CL的母线)不进入模拟躯体110的长方体Bd的内部即可。即，只要决定使得圆锥CU的母线以及圆锥CL的母线不进入长方体Bd的内部的臂角度的范围即可。

图8A、图8B以及图8C是示出实施方式1所涉及的机器人100的躯体110与机械臂120之间的关系的图。在图8A、图8B以及图8C中，示出与手部126的位置对应的、可动区域(圆锥CU以及CL)和躯体110(长方体Bd)之间的位置关系。在手部126的位置被指定为图8A所示的位置的情况下，可动区域(圆锥CU以及CL)的一部分进入长方体Bd的内部。因此，在该情况下，只要决定可动区域(圆锥CU以及CL)中的、没有进入长方体Bd的内部的臂角度的范围(干涉避免范围)即可。

这里，在图8A所示那样的情况下，可动区域(圆锥CU以及CL)与躯体侧面114以及躯体前表面112双方相交。由此，可动区域(圆锥CU以及CL)与线段AB(干涉判定线段Ld)相交。因此，本实施方式中，使用该可动区域(圆锥CU以及CL)与线段AB(干涉判定线段Ld)之间的交点来决定干涉避免范围。

另外，在手部126的位置被指定为图8B所示的位置的情况下，除了肩部132附近以及手部126的附近之外，可动区域(圆锥CU以及CL)全部进入长方体Bd的内部。在该情况下，无法避免躯体110与机械臂120之间的干涉。因此，认定为不存在干涉避免范围。此外，在图8B所示那样的情况下，可动区域(圆锥CU以及CL)不与线段AB(干涉判定线段Ld)相交。

另外，在手部126的位置被指定为图8C所示那样的位置的情况下，可动区域(圆锥CU以及CL)也不与线段AB(干涉判定线段Ld)相交。另一方面，在图8C的情况下，可动区域(圆锥CU以及CL)全部存在于长方体Bd的外侧、或者可动区域(圆锥CU以及CL)仅与躯体侧面114相交。因此，在该情况下，根据可动区域(圆锥CU以及CL)与躯体侧面114之间的关系决定干涉避免范围。

对步骤S110进行说明。控制装置2的交点计算部18计算图7A以及图7B所示的圆锥与干涉判定线段Ld(图1的线段AB)之间的交点。交点计算部18计算圆锥CL与干涉判定线段Ld之间的交点，计算圆锥CU与干涉判定线段Ld之间的交点。换言之，交点计算部18针对圆锥CL和圆锥CU分别计算交点。以下，对圆锥CL和圆锥CU分开进行说明。

具体而言，交点计算部18在计算圆锥CL与干涉判定线段Ld之间的交点时，使用上述式3，将点A的坐标以及点B的坐标从坐标系(x，y，z)变换为坐标系(x’，y’，z’)。由此，交点计算部18得到对点A进行变换而得的点A’的坐标以及对点B进行变换而得的点B’的坐标。并且，交点计算部18按照以下的式8计算表示干涉判定线段Ld(线段A’B’)上的点P_Ld的式。

(式8)

P_Ld＝(B’-A’)t+A’,{t|0≤t≤1}

这里，在式8中，t为变量。另外，在式8中，P_Ld为点P_Ld的位置矢量[x’，y’，z’]^T。同样，在式8中，A’以及B’分别是点A’以及点B’的位置矢量。交点计算部18将式8代入式4以及式5，计算所得到的2次方程式中的t的解。当存在满足式4的t的实数的情况下，交点计算部18判定为存在圆锥CL的底面与干涉判定线段Ld之间的交点。另外，当存在满足式5的t的实数的情况下，交点计算部18判定为存在圆锥CL的侧面(圆锥面)与干涉判定线段Ld之间的交点。然后，交点计算部18根据所得到的t的值来计算交点。

同样，交点计算部18在计算圆锥CU与干涉判定线段Ld之间的交点时，使用上述式6，将点A的坐标以及点B的坐标从坐标系(x，y，z)变换为坐标系(x’，y’，z’)。由此，交点计算部18得到对点A进行变换而得的点A’的坐标以及对点B进行变换而得的点B’的坐标。并且，与圆锥CL的情况同样，交点计算部18按照上述式8，计算表示干涉判定线段Ld(线段A’B’)上的点PLd的式。而且，与圆锥CL的情况同样，交点计算部18将式8代入式4以及式7，计算t的解。当存在满足式4的t的实数的情况下，交点计算部18判定为存在圆锥CU的底面与干涉判定线段Ld之间的交点。另外，当存在满足式7的t的实数的情况下，交点计算部18判定为存在圆锥CU的侧面(圆锥面)与干涉判定线段Ld之间的交点。而且，交点计算部18根据所得到的t的值来计算交点。

在步骤S112中，控制装置2的交点计算部18判定是否存在可动区域(圆锥CU以及CL)与线段A’B’(干涉判定线段Ld)之间的交点。当判定为不存在交点的情况下(S112为否)，处理进入步骤S142。另一方面，当判定为存在交点的情况下(S112为是)，处理进入步骤S114。

在步骤S142中，控制装置2判定是否存在干涉避免范围。具体而言，控制装置2的干涉避免范围决定部20判定可动区域(圆锥CU以及CL)与躯体110之间的位置关系是图8C所示的关系还是图8B所示的关系。例如，当连结肩部132和手部126的线段(OP)与躯体前表面112相交的情况下，干涉避免范围决定部20可以判定为是图8B所示的位置关系。另一方面，当线段OP不与躯体前表面112相交的情况下，干涉避免范围决定部20可以判定为是图8C所示的位置关系。

在判定为不存在干涉避免范围的情况下(S142为否)，即判定为是图8B的状态的情况下，进入处理S140。另一方面，当判定为存在干涉避免范围的情况下(S142为是)、即判定为是图8C的状态的情况下，在步骤S144中，干涉避免范围决定部20根据可动区域(圆锥CU以及CL)与躯体侧面114之间的关系，决定能够避免干涉的臂角度的范围(干涉避免范围φs)。在该情况下，干涉避免范围决定部20根据可动区域(圆锥CU以及CL)与躯体侧面114之间的交点，将位于躯体侧面114的外侧的区域的臂角度的范围决定为干涉避免范围。此外，当不存在可动区域(圆锥CU以及CL)与躯体侧面114之间的交点的情况下，干涉避免范围决定部20将臂角度的整周决定为干涉避免范围φs。

在步骤S114中，干涉避免范围决定部20判定前臂部124的臂角度的存在范围(机械臂120不与躯体110干涉的臂角度φ的范围)。具体而言，干涉避免范围决定部20判定是否存在圆锥CL的底面与干涉判定线段Ld(线段A’B’)的交点。并且，干涉避免范围决定部20判定圆锥CL的侧面(圆锥面)与干涉判定线段Ld之间的交点有几个。

图9是例示图7A所示的圆锥CL的侧面与干涉判定线段Ld之间的交点存在两个的情况的图。在将式8代入式5而得到的t的2次方程式具有两个实数解的情况下，如图9所示，圆锥CL的侧面与干涉判定线段Ld(线段A’B’)之间的交点存在两个。此外，在该情况下，底面与干涉判定线段Ld之间的交点不存在。

干涉避免范围决定部20取得由交点计算部18计算出的交点a’以及b’的坐标。而且，干涉避免范围决定部20使用交点a’的坐标，计算将交点a’向圆锥CL的底面投影(正射影)而得的映像即点a”。此外，圆锥CL为正圆锥、且底面位于x’y’平面上，因此，当交点a’的坐标为[x_a’，y_a’，z_a’]的情况下，点a”的坐标为[x_a’，y_a’，0]。同样，干涉避免范围决定部20使用交点b’的坐标，计算将交点b’向圆锥CL的底面投影(正射影)而得的映像即点b”。

并且，干涉避免范围决定部20计算：在圆锥CL的底面中，以x’轴的负方向(通过点e₀’[－r，0，0]的半径)为基准的、通过点a”的半径Ra的角度φ1L。另外，同样，干涉避免范围决定部20计算：在圆锥CL的底面中，以x’轴的负方向为基准的、通过点b”的半径Rb的角度φ2L。这里，如使用图7A上述的那样，臂角度φ的基准点即φ＝0的点为点e₀’。因此，所计算出的角度φ1L以及φ2L成为臂角度φ的极限角度(边界角度)。此外，如上所述，在圆锥CL中，臂角度的正方向从z’轴正方向观察为顺时针。因此，在点a’以及点b’位于图9那样的位置的情况下，若－π≤φ≤π，则φ1L＞0，φ2L＜0。

接下来，干涉避免范围决定部20使用所计算出的角度φ1L以及φ2L，判定前臂部124的臂角度的存在范围φL(第一范围)。换言之，干涉避免范围决定部20决定机械臂120的前臂部124不与躯体110干涉的臂角度φ的存在范围。这里，角度φ1L以及φ2L是臂角度的存在范围φL的边界角度。因此，圆锥CL的臂角度的存在范围φL是从φ2L顺时针转到φ1L的范围(包含点e₀’侧)，或者是从φ1L顺时针转到φ2L的范围(不含点e₀’侧)任一范围。以下，对其判定方法进行说明。

图10是用于说明图9的例子中的臂角度的存在范围φL的判定方法的图。在图10中，使用式3对躯体前表面112以及躯体侧面114分别进行坐标变换而得的平面F’以及平面S’与圆锥CL重叠。这里，臂角度的存在范围φL是指不与躯体110干涉的范围，因此将圆锥CL没有嵌入躯体的一侧作为臂角度的存在范围φL即可。

例如，干涉避免范围决定部20计算以线段a’b’上的任意点(例如中点g’)为起点的、躯体前表面112上的任意长度的矢量Vf。而且，干涉避免范围决定部20将矢量Vf向圆锥CL的底面(即x’y’平面)投影(正射影)。而且，干涉避免范围决定部20将不与矢量Vf的映像相交的一侧的范围决定为臂角度的存在范围φL。在图9以及图10的例子中，臂角度的存在范围φL是从φ2L顺时针转到φ1L的范围(φ2L≤φ≤φ1L)。另外，例如，干涉避免范围决定部20可以将包含φ＝0的一侧(包含点e₀’的一侧)作为臂角度的存在范围φL。

图11是例示图7A所示的圆锥CL的底面与干涉判定线段Ld之间的交点存在一个的情况下的图。当满足式4的式8的t存在一个的情况下，如图11所示，圆锥CL的底面与干涉判定线段Ld(线段A’B’)之间的交点(a’)存在一个。并且，在该情况下，圆锥CL的侧面与干涉判定线段Ld之间的交点(b’)存在一个。即，将式8代入式5而得到的t的2次方程式具有一个实数解。

与图9的情况同样，干涉避免范围决定部20取得由交点计算部18计算出的交点a’以及b’的坐标。在图11中，点a’位于底面上，点b’位于侧面上。在该情况下，干涉避免范围决定部20使用交点b’的坐标，计算将交点b’向圆锥CL的底面投影(正射影)而得的映像即点b”。另外，干涉避免范围决定部20计算：在圆锥CL的底面中，以x’轴的负方向为基准的、通过点b”的半径Rb的角度φ2L。该角度φ2L成为臂角度φ的边界角度之一。以下，说明另一个边界角度φ1L的计算方法。

图12是用于说明图11的例子中的臂角度的存在范围φL的判定方法的图。如上所述，臂角度φ的边界角度之一为φ2L。这里，躯体前表面112向x’y’z’坐标系进行坐标变换后的平面F’通过点a’。此时，干涉避免范围决定部20计算平面F’与形成圆锥CL的底面的圆周之间的交点D’。并且，干涉避免范围决定部20计算：在圆锥CL的底面中，以x’轴的负方向为基准的、通过交点D’的半径Rd的角度。干涉避免范围决定部20将该角度决定为边界角度φ1L。并且，干涉避免范围决定部20将包含以x’轴的负方向为基准的、通过交点a’的半径Ra的角度φa的一侧的范围即从φ2L顺时针转到φ1L的范围(φ2L≤φ≤φ1L)决定为臂角度的存在范围φL。

图13是例示图7A所示的圆锥CL的侧面与干涉判定线段Ld之间的交点存在一个的情况的图。在将式8代入式5而得的t的2次方程式具有一个重根的情况下，如图13所示，圆锥CL的侧面与干涉判定线段Ld(线段A’B’)之间的交点存在一个。换言之，干涉判定线段Ld与圆锥CL的侧面相切。该干涉判定线段Ld与圆锥CL侧面之间的切点成为交点。此外，在该情况下，并不存在底面与干涉判定线段Ld之间的交点。

在该情况下，干涉避免范围决定部20判定连结肩部132与手部126的线段(OP)是否从比干涉判定线段Ld更靠躯体110的内侧的位置通过。该判定可以在坐标变换后的坐标系x’y’z’中进行，也可以在坐标变换前的坐标系xyz中进行。当线段(OP)从比干涉判定线段Ld更靠躯体110的内侧的位置通过的情况下，在全部的臂角度φ的范围中均无法避免与躯体110之间的干涉。因此，干涉避免范围决定部20决定为不存在臂角度的存在范围φL。此外，在该情况下，如果是允许机械臂120与躯体110相切的条件，则干涉避免范围决定部20可以与图9的例子同样根据交点(切点)计算臂角度φ，仅将该臂角度φ决定为臂角度的存在范围φL。

另一方面，当线段(OP)从比干涉判定线段Ld更靠躯体110的外侧的位置通过的情况下，在全部的臂角度φ的范围中均不与躯体110干涉。因此，干涉避免范围决定部20将臂角度的整周决定为臂角度的存在范围φL。此外，在该情况下，如果是不允许机械臂120与躯体110相切的条件，则干涉避免范围决定部20可以与图9的例子同样根据交点(切点)计算臂角度φ，将仅除去该臂角度φ的范围决定为臂角度的存在范围φL。

接下来，在步骤S116中，干涉避免范围决定部20判定上臂部122的臂角度的存在范围φU(第二范围)。具体而言，与S114的处理相同，干涉避免范围决定部20判定是否存在圆锥CU的底面与干涉判定线段Ld(线段A’B’)之间的交点。并且，与S114的处理同样，干涉避免范围决定部20判定圆锥CU的侧面(圆锥面)与干涉判定线段Ld之间的交点存在几个。

S116的具体处理与S114的处理同样，因此省略说明。但是，在圆锥CU中，臂角度φ的正方向的方向与圆锥CL不同。另外，上臂部122存在于躯体侧面114侧，因此，S114的处理中的“躯体前表面112”被置换为“躯体侧面114”。

具体而言，在S116中，使用图10说明了的“矢量Vf”被置换为躯体侧面114上的任意长度的“矢量Vs”。即，干涉避免范围决定部20计算：以线段a’b’上的任意点(例如中点g’)为起点的、躯体侧面114上的任意长度的矢量Vs。而且，干涉避免范围决定部20使用该矢量Vs，决定臂角度的存在范围φU。

另外，在图12中，干涉避免范围决定部20计算对躯体前表面112进行坐标变换而得的平面F’与形成圆锥CL的底面的圆周之间的交点D’。另一方面，在S116中，干涉避免范围决定部20计算对躯体侧面114进行坐标变换而得的平面S’与形成圆锥CU的底面的圆周之间的交点D’，并进行同样的处理。

此外，在S114中，当在与前臂部124相关的圆锥CL中处于图9或者图13那样的状态的情况下，即仅干涉判定线段Ld与圆锥CL的侧面具有交点、而圆锥CL的底面与干涉判定线段Ld之间不存在交点的情况下，在与上臂部122相关的圆锥CU中，不存在与干涉判定线段Ld之间的交点。在该情况下，在S116的处理中，干涉避免范围决定部20将臂角度φ的整周决定为圆锥CU中的臂角度的存在范围φU。同样，当在S116中仅干涉判定线段Ld与圆锥CU的侧面具有交点、而圆锥CU的底面与干涉判定线段Ld之间不存在交点的情况下，在S114的处理中，干涉避免范围决定部20也将臂角度φ的整周决定为圆锥CL中的臂角度的存在范围φL。

接下来，在步骤S120中，干涉避免范围决定部20使用所判定出的φL和φU，决定臂角度φ的干涉避免范围φs。具体而言，干涉避免范围决定部20将φL与φU重复的范围决定为干涉避免范围φs。

此外，如上所述，当在圆锥CL中仅干涉判定线段Ld与圆锥CL的侧面具有交点、而圆锥CL的底面与干涉判定线段Ld之间不存在交点的情况下，圆锥CU中的臂角度的存在范围φU是臂角度φ的整周。因此，干涉避免范围决定部20将圆锥CL中的臂角度的存在范围φL决定为干涉避免范围φs。同样，当在圆锥CU中仅干涉判定线段Ld与圆锥CU的侧面具有交点、而圆锥CU的底面与干涉判定线段Ld之间不存在交点的情况下，圆锥CL中的臂角度的存在范围φL是臂角度φ的整周。因此，干涉避免范围决定部20将圆锥CU中的臂角度的存在范围φU决定为干涉避免范围φs。

另一方面，当在圆锥CL中干涉判定线段Ld与圆锥CL的侧面以及底面具有交点的情况下，在圆锥CU中，干涉判定线段Ld与圆锥CU的侧面以及底面也具有交点。在这样的情况下，φL的一部分与φU的一部分相互重复。因此，干涉避免范围决定部20将该φL与φU重复的范围决定为干涉避免范围φs。在这样的情况下，在臂角度的干涉避免范围φs的一方的边界角度，前臂部124与躯体110(躯体110上的干涉判定线段Ld)干涉，在另一方的边界角度，上臂部122与躯体110(躯体110上的干涉判定线段Ld)干涉。

接下来，在步骤S130中，干涉避免范围决定部20决定目标臂角度φt。具体而言，干涉避免范围决定部20将满足干涉避免范围φs的臂角度φ作为目标臂角度φt。例如，干涉避免范围决定部20可以将干涉避免范围φs的中央值作为目标臂角度φt。

另外，当在S102的处理中进行事先设定时、或者在S104的处理中决定目标手部位置时，干涉避免范围决定部20可以根据机械臂120的各关节的允许关节角度，决定规定机械臂120的基准姿势的最佳臂角度φ₀。而且，干涉避免范围决定部20可以使用该最佳臂角度φ₀决定目标臂角度φt。例如，可以使用允许关节角度的中间值来设定最佳臂角度φ₀。这里，在最佳臂角度φ₀包含于干涉避免范围φs的情况下，干涉避免范围决定部20可以将最佳臂角度φ₀决定为目标臂角度φt。另一方面，在最佳臂角度φ₀并未包含于干涉避免范围φs的情况下，干涉避免范围决定部20可以将接近最佳臂角度φ₀的一侧的干涉避免范围φs的边界角度决定为目标臂角度φt。

接下来，在步骤S132中，控制装置2计算机械臂120的各关节的关节角度。具体而言，控制装置2的关节角度计算部22使用所决定的臂角度φ的目标臂角度φt，通过运动学逆运算，计算各关节(第一关节部141～第七关节部147)的关节角度θ₁～θ₇。详细地说，关节角度计算部22通过以下所示的方法，计算各关节角度θ₁～θ₇。

图14是用于说明使用了运动学逆运算的关节角度的计算方法的图。此外，假定手部126的位置以及姿势与手腕部136的位置以及姿势分别相同，因此，以下将手部位置作为手腕位置进行说明，将手部姿势作为手腕姿势进行说明。假设在S104中，手部位置决定部14决定手腕部136的目标位置⁰x₇和手腕部136的目标姿势⁰R₇。这里，“ⁱx_k”表示从第i号关节观察的第k号关节的位置。另外，“ⁱR_k”表示从第i号关节观察的第k号关节的姿势。这里，i＝0的情况表示从肩部132的位置即原点O观察的情况。

这里，如图14的(A)所示，关节角度计算部22以使得肘部134位于xz平面上的x＜0的区域、手部126(手腕部136)的位置位于z轴上的方式，假想地设定机械臂120的位置。此时，关节角度计算部22根据余弦定理并通过以下的式9计算肘部134的关节即第四关节部144的关节角度θ₄(参照图6)。

(式9)

θ₄＝cos^-1{(L₁ ²+L₂ ²-L₀ ²)/2L₁L₂}-π

∵L₀＝|⁰x₇|

另外，关节角度计算部22根据余弦定理并通过以下的式10计算上臂部122与z轴之间的角度θp、即上臂部122同连结肩部132与手腕部136的线段之间的角度θp。

(式10)

θ_p＝cos^-1{(L₀ ²+L₁ ²-L₂ ²)/2L₀L₁}

此时，与肘部134的位置对应的位置矢量e₀以及与手腕部136的位置对应的位置矢量w₀由以下的式11表示。

(式11)

e₀＝[-L₁sin(θ_P)0-L₁cos(θ_P)]^T

w₀＝[00-L₀]^T

关节角度计算部22使图14的(A)所示的假想的机械臂120(假想机械臂O－e₀－w₀)如图14的(B)所示绕z轴旋转目标臂角度φt。并且，关节角度计算部22使图14的(B)所示的假想机械臂O－e₁－w₀如图14的(C)所示绕x轴旋转角度α。并且，关节角度计算部22使图14的(C)所示的假想机械臂O－e₂－w₂如图14的(D)所示绕y轴旋转角度β。这里，α以及β通过上述式1以及式2得到。若这样使假想机械臂旋转，则w₃与目标手部位置⁰x₇相等。并且，e₃成为目标臂角度φt下的肘部134的位置。

这里，将与关节角度θ₁～θ₇对应的旋转矩阵分别设为R₁～R₇。另外，关节角度计算部22使用以下的式12计算与使各关节角度为0时的从肩部132观察的肘部134的位置对应的位置矢量³I_se、以及与从肘部134观察的手腕部136的位置对应的位置矢量⁴I_ew。

(式12)

³Ⅰ_se＝[00-L₁]^T

⁴Ⅰ_ew＝[00-L₂]^T

另外，手腕部136的位置的决定与手腕部136的各关节角度无关，因此，关节角度计算部22通过以下的式13计算与手腕部136的位置对应的位置矢量。

(式13)

⁰x₇＝w₃＝⁰R₃(³Ⅰ_se+³R₄ ⁴Ⅰ_ew)

另外，关于手腕部136的姿势，分为肩部132、肘部134以及手腕部136，通过以下的式14表示。

(式14)

⁰R₇＝⁰R₃ ³R₄ ⁴R₇

这里，关节角度计算部22使用上述的α、β、φt以及θp，根据式14计算以下的式15。

(式15)

⁰R₇＝R_βR_αR_φtR_θp ³R₄ ⁴R₇

另外，可以使用仅由θ₁以及θ₂构成的⁰R₂来计算位置矢量e₃。因此，关节角度计算部22通过以下的式16计算肘部134的位置矢量e₃。关节角度计算部22将α、β、φt以及θp代入式16并求解，由此来计算θ₁以及θ₂。

(式16)

$\begin{array}{c}{e}_3=R_{\mathrm{\β}}{R}_{\mathrm{\α}}{R}_{\mathrm{\φ}t}{R_{\mathrm{\θ}p}}^3{I}_{se}={R_0}_2{I_3}_{se}\\ =\left[\begin{array}{ccc}{\mathrm{cos\θ}}_1& 0& {\mathrm{sin\θ}}_1\\ 0& 1& 0\\ -{\mathrm{sin\θ}}_1& 0& {\mathrm{cos\θ}}_1\end{array}\right]\left[\begin{array}{ccc}1& 0& 0\\ 0& {\mathrm{cos\θ}}_2& -{\mathrm{sin\θ}}_2\\ 0& {\mathrm{sin\θ}}_2& {\mathrm{cos\θ}}_2\end{array}\right]{I_3}_{se}\end{array}$

另外，若考虑肩部132的基于3自由度的姿势，则如以下的式17所示，第三关节部143的姿势(⁰R₃)与基于α、β、φt以及θp的姿势一致。因此，关节角度计算部22使用式17来计算θ₃。

(式17)

$\begin{array}{c}{R_0}_3={R_0}_2{R_2}_3\\ ={R_0}_2=\left[\begin{array}{ccc}{\mathrm{cos\θ}}_3& -{\mathrm{sin\θ}}_3& 0\\ {\mathrm{sin\θ}}_3& {\mathrm{cos\θ}}_3& 0\\ 0& 0& 1\end{array}\right]\\ =R_{\mathrm{\β}}{R}_{\mathrm{\α}}{R}_{\mathrm{\φ}t}{R}_{\mathrm{\θ}p}\end{array}$

并且，若考虑手腕部136的基于3自由度的姿势，则如以下的式18那样来定义⁴R₇。关节角度计算部22使用式18来计算θ₅～θ₇。

(式18)

$\begin{array}{c}{R_4}_7=\left[\begin{array}{ccc}{\mathrm{cos\θ}}_5& -{\mathrm{sin\θ}}_5& 0\\ {\mathrm{sin\θ}}_5& {\mathrm{cos\θ}}_5& 0\\ 0& 0& 1\end{array}\right]\left[\begin{array}{ccc}{\mathrm{cos\θ}}_6& 0& {\mathrm{sin\θ}}_6\\ 0& 1& 0\\ -{\mathrm{sin\θ}}_6& 0& {\mathrm{cos\θ}}_6\end{array}\right]\left[\begin{array}{ccc}1& 0& 0\\ 0& {\mathrm{cos\θ}}_7& -{\mathrm{sin\θ}}_7\\ 0& {\mathrm{sin\θ}}_7& {\mathrm{cos\θ}}_7\end{array}\right]\\ ={\left({R_0}_3{R_3}_4\right)}^{-1}\·{R_0}_7\end{array}$

在步骤S134中，控制装置2使机械臂120动作。具体而言，控制装置2的机械臂控制部24控制机械臂120的各关节以使其成为在S132中计算出的各关节角度。此外，此时，机械臂控制部24可以进行机械臂120的各关节的所被允许的动作区域以及速度区域的检查。而且，在不满足动作区域或者速度区域的情况下，控制装置2可以再次重新决定目标臂角度φt。

本实施方式中，用圆锥来模拟机械臂120的可动区域，根据该圆锥(圆锥CL以及CU)与位于躯体110的表面上的干涉判定线段Ld之间的交点来决定干涉避免范围。这里，圆锥与线段之间的交点可通过简单的几何学方法而容易地计算。因此，本实施方式中，能够高速地决定机械臂120的干涉避免范围。

作为避免机械臂的干涉的其它方法，例如有如下的方法：关于一系列的手臂轨道，事先设定位场，并使用该位场来探索可避免干涉的路径。然而，在这样的数值解析方法中，需要进行反复计算，存在直至解收敛至探索到最终的路径为止的时间变得很长的顾虑。另外，还有预先存储表示能够避免干涉的各关节角度的表、并随时参照该表的方法。然而，在该方法中，需要事先设定表，而并非每次计算能够避免干涉的解。

另一方面，本实施方式中，如上所述，能够通过简单的几何学方法决定机械臂120的干涉避免范围。因此，本实施方式中，能够高速地进行用于避免机械臂的干涉的计算。换言之，能够实时地决定机械臂的干涉避免解。并且，本实施方式中，能够高速地(实时地)决定干涉避免解，因此能够根据机器人的每一控制周期决定干涉避免解。并且，由于能够高速地决定干涉避免解，因此能够缩短机器人的控制周期，由此，能够提高力控制的响应性能。即，本实施方式中，能够考虑机械臂120与躯体110之间的干涉避免而高速地求解各关节角度，由此，能够以高周期进行力控制，因此能够实现可进行高响应的力相互作用的机器人。

另外，本实施方式中，在上臂部122的可动区域的轨迹(圆锥CU)、和前臂部124的可动区域的轨迹(圆锥CL)分别进行计算。通过像这样分别进行计算，能够简单地应用计算圆锥与线(线段)之间的交点这样的简单的计算方法。因此，能够更高速地决定机械臂120的干涉避免范围。

另外，本实施方式中，在圆锥的底面x’y’平面上，以使得顶点位于z’轴上的方式进行坐标变换。由此，能够使用于避免机械臂120的干涉的计算更加简单。因此，能够更高速地决定机械臂120的干涉避免范围。

另外，本实施方式中，当在圆锥CL中干涉判定线段Ld与圆锥CL的侧面以及底面具有交点、在圆锥CU中干涉判定线段Ld与圆锥CU的侧面以及底面具有交点的情况下，将由干涉避免范围决定部20判定出的臂角度的存在范围φL与φU重复的范围决定为干涉避免范围φs。通过形成为这样的结构，即便在臂角度的干涉避免范围φs的一方的边界角度前臂部124与躯体110(躯体110上的干涉判定线段Ld)干涉，在另一方的边界角度上臂部122与躯体110(躯体110上的干涉判定线段Ld)干涉的情况下，也能够通过简单的几何学方法决定干涉避免范围。因此，能够更高速地决定机械臂120的干涉避免范围。

另外，在这样的情况下，使用圆锥CL的底面与干涉判定线段Ld之间的交点来计算与前臂部124相关的臂角度的存在范围φL的边界角度φ1L。同样，使用圆锥CU的底面与干涉判定线段Ld之间的交点来计算与上臂部122相关的臂角度的存在范围φU的边界角度φ1U。通过以这种方式构成，能够通过简单的几何学方法，针对各圆锥CL以及CU分别判定臂角度的存在范围φL以及φU。因此，能够更高速地决定机械臂120的干涉避免范围。

另外，本实施方式中，可以事先决定最佳臂角度φ₀，并使用该最佳臂角度φ₀决定目标臂角度φt。由此，能够控制机械臂以避免机械臂的干涉并且使其接近机械臂的基准姿势。由此，能够以避免机械臂的干涉并且使机械臂的动作更加自然的方式进行控制。

(实施方式2)

接下来，对实施方式2进行说明。在实施方式1中，将躯体110示意性地设为长方体。另一方面，关于实施方式2，将躯体110的角部模拟为曲面形状，这点与实施方式1不同。以下，使用附图对实施方式1与实施方式2的比较进行说明。

图15是示出实施方式1所涉及的、目标手部位置在躯体110的角部110a附近的情况下的机械臂120的动作的俯视图。另外，图16是示出实施方式2所涉及的、目标手部位置在躯体110的角部110b附近的情况下的机械臂120的动作的俯视图。在图15以及图16中，示出机器人100的上侧即从z轴的正方向观察的机器人100。

如上所述，在实施方式1中，将躯体110模拟为长方体，因此角部110a例如由大致呈90度交叉的两个平面形成。在这样的情况下，若在角部110a附近使手部126沿箭头A方向朝躯体110中央侧稍稍移动，则机械臂120为了避免与躯体110干涉而如箭头B所示大幅移动。换言之，在实施方式1中，存在手部126仅稍稍移动而机械臂120却急剧移动的可能性。在像这样使机械臂120急剧移动情况下，机械臂120的各关节也急剧动作。结果，存在在各关节的马达(未图示)中产生超速以及超载，各关节恶化的可能性。

另一方面，在实施方式2中，躯体110的角部110b由曲面形成。因此，如图16所示，在使手部126沿箭头A方向以与图15相同的变化量移动的情况下，如箭头C所示，机械臂120的动作变化得以缓和。因此，能够抑制机械臂120的各关节的马达中的超速以及超载。

图17是示出实施方式2所涉及的控制装置2的结构的功能框图。实施方式2所涉及的控制装置2的硬件构成与实施方式1所涉及的控制装置2的硬件构成实际相同。另一方面，实施方式2所涉及的控制装置2除了具有实施方式1所涉及的控制装置2的各构成要素之外，还具有线段决定部30，这点与实施方式1不同。

线段决定部30决定干涉判定线段Ld。即，线段决定部30具有作为决定干涉判定线段Ld的线段决定构件的功能。在实施方式1中，干涉判定线段Ld始终是线段AB。换言之，在实施方式1中，无论手部126的位置如何，干涉判定线段Ld是不变的。另一方面，在实施方式2中，如后所述，干涉判定线段Ld根据手部126的位置而变化。线段决定部30根据手部126的位置(例如每当手部126的位置被指定时)，决定干涉判定线段Ld。通过像这样根据手部126的位置决定干涉判定线段Ld，即便在利用曲面形成角部110b的情况下，也能够用与实施方式1相同的方法，通过简单的几何学方法容易地决定干涉避免范围。

图18是示出实施方式2所涉及的控制装置2的处理的流程图。在图18中，除了图4的各处理之外，还追加了步骤S209。如图18所示，在实施方式2中，控制装置2进行与图4所示的S102～S108分别实质上相同的处理S202～S208。因此，省略S202～S208的说明。

在步骤S209中，控制装置2决定干涉判定线段Ld。具体而言，控制装置2的线段决定部30根据手部126的位置(目标手部位置)，针对圆锥CL(前臂部124的轨迹)以及圆锥CU(上臂部122的轨迹)，分别决定干涉判定线段Ld。针对圆锥CL，线段决定部30将通过手部126的位置、且包含与角部110b的曲面相切的切线的切点的线，决定为干涉判定线段Ld_L。另外，针对圆锥CU，线段决定部30将通过肩部132的位置、且包含与角部110b的曲面相切的切线的切点的线，决定为干涉判定线段Ld_U。以下具体地进行说明。

图19是用于说明决定实施方式2所涉及的圆锥CL(前臂部124的轨迹)的干涉判定线段Ld_L的方法的图。在图19中，假设角部110b为由半径R的圆弧形成的曲面。在手部126位于位置A的情况下，线段决定部30将通过从位置A朝角部110b的曲面所引的切线的切点a、且与z轴平行的线，决定为干涉判定线段LdL。同样，在手部126位于位置B(或者位置C)的情况下，线段决定部30将通过从位置B(或者位置C)朝角部110b的曲面所引的切线的切点b(或者切点c)、且与z轴平行的线，决定为干涉判定线段Ld_L。此外，在手部126位于位置D的情况下，无法从位置D向角部110b的曲面引切线。在这样的情况下，线段决定部30将通过角部110b的曲面的端点d、且与z轴平行的线，决定为干涉判定线段Ld_L。

图20是用于说明实施方式2所涉及的决定切点的方法的图。这里，在决定切点时，忽略z轴分量。另外，以形成角部110b的曲面的圆弧的中心作为原点，以躯体110的前方向(图20的纸面上方向)作为x轴正方向，以躯体110的左方向(图20的纸面左方向)作为y轴正方向。在这样的坐标系中，假设手部126的位置被决定为(h_x，h_y)。此时，线段决定部30按照如下方式决定切点(c_x，c_y)。

切点(c_x，c_y)位于半径R的圆周上，因此，线段决定部30能够得到以下的式19。

(式19)

c_x ²+c_y ²＝R²

另外，线段决定部30如以下的式20所示计算表示切线的方程式。

(式20)

c_xx+c_yy＝R²

这里，切线通过手部位置(h_x，h_y)，因此，线段决定部30将(h_x，h_y)代入式20，从而得到以下的式21。

(式21)

c_xh_x+c_yh_y＝R²

∴ $c_y=\frac{R^2-c_x{h}_x}{h_y}$

并且，线段决定部30通过对将利用式21得到的c_y代入式19而得的c_x的2次方程式进行求解来计算c_x。并且，线段决定部30将所计算出的c_x代入式20来计算c_y。由此，线段决定部30决定切点(c_x，c_y)。此外，虽然求出多个切点，但线段决定部30将位于图20的粗线部分(x≥0、y≤0)的切点作为决定对象。此外，实际上xyz坐标系的原点是肩部132的位置，因此线段决定部30针对所计算出的切点(c_x，c_y)进行用于变换原点位置的坐标变换。

图21是用于说明实施方式2所涉及的决定圆锥CU(上臂部122的轨迹)上的干涉判定线段Ld_U的方法的图。线段决定部30将通过从肩部132朝角部110b所引的曲面的切线的切点e、且与z轴平行的线决定为干涉判定线段Ld_U。这里，无论手部126的位置如何，肩部132的位置是恒定的。因此，线段决定部30可以在手部126的位置被指定前预先决定干涉判定线段Ld_U。

在步骤S210中，控制装置2的交点计算部18与图4所示的S110同样，计算图7A以及图7B所示的圆锥与干涉判定线段Ld之间的交点。此时，交点计算部18计算由线段决定部30计算出的干涉判定线段Ld_L与圆锥CL之间的交点。同样，交点计算部18计算由线段决定部30计算出的干涉判定线段Ld_U与圆锥CU之间的交点。除此以外的处理与S110同样，因此省略说明。

另外，如图18所示，在实施方式2中，控制装置2也进行与图4所示的S112～S134、S140～S144分别实质上相同的处理S212～S234、S240～S244。因此，省略对S212～S234、S240～S244的说明。

此外，在S214中，对于使用图10说明了的臂角度的存在范围φL的决定方法，代替平面F’，使用对与躯体前表面112平行且通过切点的平面进行坐标变换而得的平面。同样，代替平面S’，使用对与躯体侧面114平行且通过切点的平面进行坐标变换而得的平面。例如，在切点被决定为图19中的切点b的情况下，代替平面F’，使用对与躯体前表面112平行且通过切点b的平面112b进行坐标变换而得的平面。同样，代替平面S’，使用对与躯体侧面114平行且通过切点b的平面114b进行坐标变换而得的平面。

另外，在S214中，对于使用图12说明了的臂角度φ的决定方法，代替平面F’，使用对与躯体前表面112平行且通过切点的平面进行坐标变换而得的平面。例如，在切点被决定为图19中的切点b的情况下，代替平面F’，使用对与躯体前表面112平行且通过切点b的平面112b进行坐标变换而得的平面。

另外，在S216中，对于使用图10说明了的臂角度的存在范围φL的决定方法，代替平面F’，使用对与躯体前表面112平行且通过切点e的平面进行坐标变换而得的平面。同样，代替平面S’，使用对与躯体侧面114平行且通过切点e的平面进行坐标变换而得的平面。另外，在S216中，对于使用图12说明了的臂角度φ的决定方法，也代替平面S’，使用对与躯体侧面114平行且通过切点e的平面进行坐标变换而得的平面。

图22是用于说明实施方式2所涉及的决定切点的其它方法的图。图22与图20不同，示出了角部110b的曲面为椭圆弧的情况。这里，在决定切点时，忽略z轴分量。另外，以形成角部110b的曲面的椭圆弧的中心作为原点，以躯体110的前方向作为x轴正方向，以躯体110的左方向作为y轴正方向。这里，设椭圆的x轴方向的轴为2a，y轴方向的轴为2b。在这样的坐标系中，手部126的位置被决定为(h_x，h_y)。此时，线段决定部30按照以下方式决定切点(c_x，c_y)。

切点(c_x，c_y)位于椭圆周上，因此线段决定部30能够得到以下的式22。

(式22)

$\frac{{c_x}^2}{a^2}+\frac{{c_y}^2}{b^2}=1$

另外，线段决定部30如以下的式23那样计算表示切线的方程式。

(式23)

$\frac{c_{x}x}{a^2}+\frac{c_{y}y}{b^2}=1$

这里，切线通过手部位置(h_x，h_y)，因此线段决定部30将(h_x，h_y)代入式23，得到以下的式24。

(式24)

$\frac{c_x{h}_x}{a^2}+\frac{c_y{h}_y}{b^2}=1$

∴ $c_y=\frac{b^2}{h_y}-\frac{c_x{h}_x{b}^2}{a^2{h}_y}$

并且，线段决定部30对通过将利用式24得到的c_y代入式22而得的c_x的2次方程式进行求解，由此来计算c_x。并且，线段决定部30通过将所计算出的c_x代入式23来计算c_y。由此，线段决定部30决定切点(c_x，c_y)。此外，虽然求出了多个切点，但线段决定部30将位于图22的粗线部分(x≥0、y≤0)的切点作为决定对象。此外，实际上xyz坐标系的原点位于肩部132的位置，因此线段决定部30对所计算出的切点(c_x，c_y)进行用于变换原点位置的坐标变换。

此外，在实施方式2中，假设仅角部110b为曲面，但不限于这样的结构。例如，也可以躯体110的整周由曲面形成。在这样的情况下，在实施方式2中只要去掉躯体前表面112以及躯体侧面114的平面部分即可。换言之，不仅是角部110b，将躯体前表面112以及躯体侧面114全部都用曲面表示即可。

图23～图26是示出使采用了本实施方式的机器人100的动作再现的仿真结果的图。首先，如图23所示，通过操作者的操作，手部126的位置被指定为躯体侧面114的外侧的位置A。接下来，如图24所示，通过操作者的操作，手部126的位置被指定为躯体前表面112的前侧的位置B。接下来，如图25所示，通过操作者的操作，手部126的位置被指定为比位置B更接近躯体前表面112的位置C。并且，如图26所示，通过操作者的操作，手部126的位置被指定为比位置C更接近躯体前表面112的位置D。在手部126的位置被指定为位置A～D中任一位置的情况下，上臂部122以及前臂部124均以不与躯体110干涉的方式动作。并且，机器人100的躯体110的角部110b由曲面形成，因此，在图23～图26所示的一系列动作中，机械臂120的动作(特别是肘部134的移动)变得平滑(即不急剧)。

(变形例)

此外，本发明不限于上述实施方式，能够在不脱离主旨的范围适当地变更。例如，在图4以及图18所示的流程图中，可以适当变更多个处理的顺序。例如，在图4中，可以省略S102～S106的处理以及S140～S144的处理。另外，S114的处理以及S116的处理的顺序也可以颠倒。另外，在图18中，S208的处理以及S209的处理也可以颠倒。

另外，在上述实施方式中，作为障碍物的例子假设是机器人100的躯体110，但障碍物也可以不是机器人100的躯体。例如，障碍物也可以是柱子或者墙壁等会与机械臂120发生干涉的任意物体。在该情况下，控制装置2可以具有取得表示各障碍物的位置的数据的构件。例如，可以在机器人100设置用于取得障碍物的位置数据(坐标数据)的传感器。另外，障碍物不限于一个，也可以为多个。在该情况下，可以针对多个障碍物分别进行上述的处理。此时，针对多个障碍物分别决定干涉避免范围，但也可以将所决定出的多个干涉避免范围相互重复的范围决定为机械臂不与多个障碍物干涉的干涉避免范围。

另外，第一关节部141、第二关节部142、第三关节部143的顺序以及第五关节部145、第六关节部146、第七关节部147的顺序不限于图1所示的顺序，也可以适当变更。此外，在变更各关节的顺序的情况下，适当变更关节角度计算部22的计算方法即可。

另外，在上述实施方式中，干涉判定线段Ld位于躯体110的角部110a或者角部110b，但不限于这样的结构。干涉判定线段Ld可以是躯体110(障碍物)的表面上(边界面上)的任意线段。然而，通过使干涉判定线段Ld是位于角部110a或者角部110b的线段，干涉避免范围的判定变得更加容易。

另外，在上述实施方式中，说明了机械臂120与障碍物(躯体等)的干涉，但机械臂120不限于机器人的手臂。例如，本实施方式所涉及的机械臂也能够应用于机器人的脚。在该情况下，上臂部可以置换为大腿部、前臂部可以置换为下腿(小腿)部，肘可以置换为膝。另外，本实施方式所涉及的机械臂也能够应用于任意的机械手。

另外，在上述实施方式中，机械臂120(以及机器人100)设置于在物理上为实体的机器人100，但不限于这样的结构。例如，机械臂120(以及机器人100)也可以是假想的物体。即，本实施方式例如也能够应用于虚拟现实。具体而言，本实施方式例如也能够应用在游戏中用于避免任意的假想的杆状物体与任意的假想障碍物之间的干涉的计算。另外，本实施方式也能够应用于模拟任意的杆状物体与障碍物之间有无干涉。

另外，在上述实施方式中，如图1所示那样定义xyz坐标系，但相对于机械臂120而如何定义xyz坐标系可以适当地变更。x’y’z’坐标系也如此。另外，在上述实施方式中，假设手部126的位置与手腕部136的位置相同而进行计算，但也可以是相互不同的位置。在该情况下，在手部位置被指定的情况下，只要针对该被指定了的手部位置而考虑手部126与手腕部136之间的偏移即可。另外，在上述实施方式中，将xyz坐标系的原点设定于肩部132，但原点位置是任意的。在该情况下，只要考虑肩部132与原点之间的偏移而进行计算即可。

由以上的本发明的说明可知，本发明能够以各种方式变形。这种变形并不应认为是脱离了本发明的思想以及范围的变形，另外，所有的对于本领域技术人员来说不言而喻的这种改进均包含在技术方案中。

Claims (8)

1.一种干涉避免方法，是避免机械臂与障碍物干涉的机械臂的干涉避免方法，该机械臂通过第一臂和第二臂经由关节部连接而构成，

其中，

计算所述机械臂的两端的位置被固定的状态下的、所述第一臂以及所述第二臂的可动区域；

计算包含所述障碍物的障碍物区域的边界面上的第一线与所述可动区域的交点；

基于所述计算出的交点，决定在所述可动区域中所述机械臂不与所述障碍物区域干涉的干涉避免范围。

2.根据权利要求1所述的干涉避免方法，其中，

所述可动区域由以在所述机械臂的两端的位置被固定的状态下所述关节部旋转而得的轨迹所形成的圆作为底面、以所述第一臂以及所述第二臂分别旋转而得的轨迹所形成的旋转面作为圆锥侧面的圆锥形成，

在由所述第二臂的轨迹形成的第一圆锥和由所述第一臂的轨迹形成的第二圆锥，分别计算所述交点。

3.根据权利要求2所述的干涉避免方法，其中，

在与所述第一线的交点存在于所述底面上的情况下，将基于所述第一圆锥判定出的第一范围与基于所述第二圆锥判定出的第二范围重复的范围，决定为所述机械臂的所述干涉避免范围。

4.根据权利要求3所述的干涉避免方法，其中，

基于与所述第一圆锥相关的所述圆锥侧面上的交点和所述底面上的交点判定所述第一范围，基于与所述第二圆锥相关的所述圆锥侧面上的交点和所述底面上的交点判定所述第二范围。

5.根据权利要求1～4中任一项所述的干涉避免方法，其中，

所述障碍物区域的所述边界面的至少一部分由曲面形成。

6.根据权利要求5所述的干涉避免方法，其中，

将通过所述机械臂的手部的位置并包含与所述曲面相切的切线的切点的线，决定为所述曲面上的所述第一线。

7.一种控制装置，是控制第一臂和第二臂经由关节部连接而构成的机械臂的控制装置，其中，所述控制装置具有：

可动区域计算构件，计算所述机械臂的两端的位置被固定的状态下的、所述第一臂和所述第二臂的可动区域；

交点计算构件，计算包含障碍物的障碍物区域的边界面上的第一线与所述可动区域的交点；以及

干涉避免范围决定构件，基于所述计算出的交点，决定在所述可动区域中所述机械臂不与所述障碍物区域干涉的干涉避免范围。

8.一种非暂存性计算机可读介质，存储有如下的程序，该程序实现避免机械臂与障碍物干涉的机械臂的干涉避免方法，该机械臂通过第一臂和第二臂经由关节部连接而构成，其中，

所述程序使计算机执行以下步骤：

计算所述机械臂的两端的位置被固定的状态下的、所述第一臂和所述第二臂的可动区域的步骤；

计算包含所述障碍物的障碍物区域的边界面上的第一线与所述可动区域的交点的步骤；以及

基于所述计算出的交点，决定在所述可动区域中所述机械臂不与所述障碍物区域干涉的干涉避免范围的步骤。