CN112654469A - 机器人的直接示教装置及直接示教方法 - Google Patents

Abstract

本发明的直接示教装置(1)具备检测外力的外力检测部(102)；算出跟随外力的臂部(2)的动作的从动控制运算部(103)；算出臂部(2)的位置姿势的位置姿势运算部(104)；判定臂部(2)的位置姿势是否接近约束目标的接近判定部(105)；判定是否进行了解除操作的解除操作判定部(106)；判定是否满足了恢复条件的恢复判定部(107)；算出约束控制的目标值的目标值算出部(109)；算出臂部(2)向目标值的动作的约束控制运算部(110)；根据判定部(105)～(107)的判定结果切换约束控制运算部(110)的动作的切换部(108)；具备将从动控制运算部(103)的算出结果和约束控制运算部(110)的运算结果合成的合成部(111)；以及基于合成结果驱动臂部(2)的驱动控制部(112)。

Description

机器人的直接示教装置及直接示教方法

技术领域

本发明涉及一种进行机器人的直接示教的直接示教装置以及直接示教方法。

背景技术

在工业用机器人中，为了使机器人进行作业，预先实施被称为示教(teaching)的作业。作为进行该机器人的示教的方法之一，有被称为直接示教(direct testing)的方法。

例如在专利文献1中公开了使用力传感器的机器人的直接示教方法。另外，例如在专利文献2中公开了使用转矩检测单元的机器人的直接示教方法。图23示出在这些专利文献中公开的直接示教装置的概略构成。

在图23所示的直接示教装置11中，首先，外力检测部1101使用力传感器或转矩传感器等检测由操作者对机器人所具有的臂部2施加的外力。接着，从动控制运算部1102算出跟随由外力检测部1101检测到的外力的臂部的动作(从动控制运算)。另外，在从动控制运算中，从动控制运算部1102有时也使用与由位置姿势计测部(未图示)计测出的臂部的位置姿势相关的参数。另外，臂部的位置姿势是指臂部的位置和臂部的姿势中的至少一方。另外，作为所述参数，可以举出臂部的位置、臂部的姿势、或者臂部的关节角等。然后，从动控制运算部1102将基于从动控制运算的结果而更新的从动控制指令值通知给驱动控制部1103。接着，驱动控制部1103按照由动控制运算部1102通知的从动控制指令值驱动臂部2。

通过这一系列的动作，图23所示的直接示教装置11能够控制成臂部2按照由操作者施加的外力而动作。然后，直接示教装置11在通过控制使臂部2的位置以及姿势成为操作者意图的状态的情况下，将此时的所述参数作为示教点进行记录。由该直接示教装置11记录的示教点在机器人作业时使用。

这样，由于操作者直接操作臂部来示教位置以及姿势，因此机器人的直接示教具有对于操作者来说直观且容易理解的优点。另一方面，臂部随着操作者施加的外力而直接移动的特征不是只有优点。

例如，考虑操作者对机器人示教了某点P的位置之后，示教其正下方向的其他点Q的位置这样的例子。在该例子中，操作者使臂部向正下方向(Z轴方向)移动。但是，操作者难以对臂部准确地向正下方向施加外力，臂部的X轴坐标及Y轴坐标偏移的情况较多。尽管稍后能够使用示教器等来仅修改臂部的X轴坐标和Y轴坐标，但这降低了直接示教的优点。

另外，例如，考虑操作者在使设置在臂部的顶端的末端执行器朝向正下方向的状态下进行示教这样的例子。在该例子中，操作者也难以在维持相对于臂部准确地朝向正下方向的状态的情况下直接操作臂部，臂部的姿势偏移的情况较多。

如上所述，直接示教虽然直观且容易理解，但也存在操作困难的情况。

对此，例如在专利文献3、4中公开了解决上述课题的技术。

在专利文献3中，公开了能够通过模式切换开关将动作模式切换为位置方向移动模式、位置移动模式或方向移动模式的直接示教装置。在位置移动模式中，能够在维持臂部的顶端的姿势的状态下仅移动位置。另外，在方向移动模式中，能够在维持臂部的顶端的位置的状态下仅变更姿势。通过该直接示教装置，能够在某种程度上缓和上述那样的直接示教中的操作的难度。另外，通常的直接示教可以在位置方向移动模式下实现。

另外，在专利文献4中公开了能够通过输入装置将动作模式切换为约束模式或全方向移动模式的直接示教装置。在约束模式中，末端执行器的顶端可以沿着特定的约束轴或约束面移动。另外，在全方向移动模式中，能够实施通常的直接示教。该直接示教装置通过在直接示教的中途切换为约束模式，能够使臂部的顶端准确地在上下方向上移动，减轻直接示教中的操作难度。

现有技术文献

专利文献

专利文献1:日本专利特开平05-204441号公报

专利文献2:日本专利特开平05-250029号公报

专利文献3:日本专利特开平05-285870号公报

专利文献4:日本专利特开平05-303425号公报

发明内容

发明要解决的问题

如上所述，专利文献3、4所公开的直接示教装置切换进行不对臂部的操作施加约束的通常的直接示教的模式和进行对臂部的操作施加约束的带约束直接示教的模式。并且，在专利文献3中通过模式切换开关进行模式的切换，在专利文献4中通过输入装置进行模式的切换，操作者通过用手操作开关等就能够进行切换。

但是，在直接示教中，由于操作者用手直接操作臂部，所以用手操作模式的切换是不方便的。例如，在操作者用双手操作臂部的情况下，在操作模式的切换时，暂时停止臂部的操作，用单手保持臂部。如果用单手保持在通常情况下用双手保持的臂部，则臂部的位置姿势也会偏离。其结果是，可能会降低带约束直接示教的效果。

本发明是为了解决所述课题而完成的，其目的在于提供一种能够在操作者操作臂部的状态下切换通常的直接示教和带约束直接示教的直接示教装置。

解决问题的技术手段

本发明的直接示教装置，包括：外力检测部，其检测施加在机器人所具有的臂部上的外力；从动控制运算部，其算出跟随由外力检测部检测出的外力的臂部的动作；位置姿势运算部，其算出作为臂部的位置或姿势中的至少一方的位置姿势；接近判定部，其基于位置姿势运算部的算出结果，判定臂部的位置姿势是否接近作为该臂部的位置姿势的约束目的地的约束目标；解除操作判定部，其判定操作者是否进行了用于解除带约束直接示教的解除操作；恢复判定部，其判定是否满足向以下状态恢复的恢复条件，该状态是能够向带约束直接示教切换的状态；目标值算出部，其基于约束目标以及位置姿势运算部的算出结果，算出带约束直接示教中的约束控制的目标值；约束控制运算部，其基于位置姿势运算部的算出结果，算出移动到由目标值算出部算出的目标值的臂部的运动；切换部，在本机处于第一状态的情况下，在由接近判定部判定为接近约束目标的情况下，所述切换部将本机切换到第二状态，在本机处于第二状态的情况下，在由解除操作判定部判定为操作者进行了解除操作的情况下，所述切换部将本机切换到第三状态，在本机处于第三状态的情况下，在由恢复判定部判定为满足恢复条件的情况下，所述切换部将本机切换到第一状态，在本机处于第二状态的情况下，将约束控制运算部的处理设为有效，在本机处于第一状态或第三状态的情况下，将约束控制运算部的处理设为无效；合成部，其将从动控制运算部的算出结果和约束控制运算部的算出结果合成；以及驱动控制部，基于合成部的合成结果来驱动臂部。

发明的效果

根据本发明，由于如上述那样构成，所以能够在操作者操作臂部的状态下切换通常的直接示教和带约束直接示教。

附图说明

图1是表示本发明的实施方式1的直接示教装置的构成例的图。

图2A～图2D是表示本发明的实施方式1的直接示教装置的通常的直接示教和带约束直接示教(位置约束的情况)的切换的一例的图。

图3A～图3D是表示本发明的实施方式1的直接示教装置的通常的直接示教和带约束直接示教(姿势约束的情况)的切换的一例的图。

图4是表示本发明的实施方式1的直接示教装置的动作例的流程图。

图5是说明本发明的实施方式1中的接近判定部的动作例的图(姿势约束的情况)。

图6是说明本发明的实施方式1中的解除操作判定部的动作例的图(使用外力的情况)。

图7是说明本发明的实施方式1中的解除操作判定部的动作例的图(使用臂部的位置姿势的情况)。

图8是说明本发明的实施方式1中的切换部的动作例的图。

图9是说明本发明的实施方式1中的目标值算出部的动作例的图(姿势约束的情况)。

图10是说明本发明的实施方式1中的目标值算出部的动作例的图(姿势约束的情况)。

图11是表示本发明的实施方式2的直接示教装置的动作例的流程图。

图12A、图12B是表示本发明的实施方式2中的从动控制运算部的动作例的图。

图13是表示本发明的实施方式2中的接近判定部的动作例的流程图。

图14A、14B、14C是说明本发明的实施方式2中的接近判定部的动作例的图。

图15A、图15B是说明本发明的实施方式2中的目标值算出部的动作例的图，是表示旋转轴的算出例的图。

图16A、图16B是说明本发明的实施方式2中的目标值算出部的动作例的图，是表示旋转方向的算出例的图。

图17是说明本发明的实施方式2中的目标值算出部的动作例的图。

图18是说明本发明的实施方式3的直接示教装置的轴约束的图。

图19是表示本发明的实施方式3的直接示教装置的动作例的流程图。

图20是说明本发明的实施方式3中的接近判定部的动作例的图。

图21是说明本发明的实施方式4的直接示教装置的面约束的图。

图22是表示本发明的实施方式4的直接示教装置的动作例的流程图。

图23是表示现有的直接示教装置的构成例的图。

具体实施方式

以下，参照附图详细说明本发明的实施方式。

实施方式1

图1是表示本发明的实施方式1的直接示教装置1的构成例的图。

直接示教装置1进行机器人的直接示教。该直接示教装置1作为直接示教方法，能够切换对机器人所具有的臂部2的位置姿势不施加约束的通常的直接示教、和将臂部2的位置姿势约束于约束目标的带约束直接示教。

另外，臂部2的位置姿势是指臂部2的位置和臂部2的姿势中的至少一方。另外，臂部2的位置是指设置在臂部2的顶端的末端执行器201的位置501，臂部2的姿势是指末端执行器201的朝向502。另外，约束目标是臂部2的位置姿势的约束目标。作为对臂部2的位置的约束目标,有约束面503和约束轴504。作为约束面503的类型,有平面和曲面。作为约束轴504的类型,有直线和曲线。作为对臂部2的姿势的约束目标，有约束方向505。另外，在直接示教装置1中使用的约束目标不限于一个，也可以是多个。另外，对机器人设定作为基准的机器人坐标系，对末端执行器201设定工具坐标系(参照图9等)。

如图1所示，该直接示教装置1具备位置姿势计测部101、外力检测部102、从动控制运算部103、位置姿势运算部104、接近判定部105、解除操作判定部106、恢复判定部107、切换部108、目标值算出部109、约束控制运算部110、合成部111以及驱动控制部112。另外，从动控制运算部103、位置姿势运算部104、接近判定部105、解除操作判定部106、恢复判定部107、切换部108、目标值算出部109、约束控制运算部110、合成部111以及驱动控制部112通过系统LSI(Large-Scale Integration：大规模集成电路)等处理电路、或者执行存储在存储器等中的程序的CPU(Central Processing Unit：中央处理单元)等来实现。

位置姿势计测部101计测与臂部2的位置姿势相关的参数。另外，作为与臂部2的位置姿势相关的参数，可以举出臂部2的位置、臂部2的姿势、或者臂部2的关节角θ等。

外力检测部102检测由操作者施加在臂部2上的外力。例如，外力检测部102也可以使用安装在臂部2的顶端的力传感器，将由该力传感器计测到的力F作为上述外力进行检测。另外，例如，外力检测部102也可以使用安装在臂部2的电动机驱动轴上的扭矩传感器，将由该扭矩传感器计测到的扭矩τ作为上述外力进行检测。另外，外力检测部102也可以不是如上上述使用传感器直接计测外力，而是使用根据臂部2所具有的电动机的电流或臂部2的关节角θ的计测值间接地算出外力的外力观测器来检测上述外力。

从动控制运算部103根据位置姿势计测部101的计测结果，算出跟随由外力检测部102检测出的外力的臂部2的动作(从动控制指令值)。

另外，位置姿势计测部101、外力检测部102以及从动控制运算部103与在没有约束的通常的直接示教中使用的构成相同，是公知技术。

位置姿势运算部104根据从动控制运算部103的算出结果，算出臂部2的位置姿势。

另外，在图1中，从动控制运算部103使用位置姿势计测部101的计测结果，位置姿势运算部104使用从动控制运算部103的算出结果。

但是，不限于此，从动控制运算部103也可以不使用位置姿势计测部101的计测结果，而运算跟随由外力检测部102检测到的外力的臂部2的动作。在这种情况下，位置姿势运算部104根据位置姿势计测部101的计测结果算出臂部2的位置姿势。

另外，位置姿势运算部104也可以使用从动控制运算部103的算出结果和位置姿势计测部101的计测结果这两者来算出臂部2的位置姿势。

接近判定部105根据位置姿势运算部104的算出结果，判定臂部2的位置姿势是否接近约束目标。

解除操作判定部106判定操作者是否进行了用于解除带约束直接示教的解除操作。此时，例如，解除操作判定部106根据外力检测部102的检测结果(外力的大小、外力的朝向或外力的模式等)或位置姿势运算部104的算出结果(位置姿势或位置姿势的模式等)，判定操作者是否进行了解除操作。

恢复判定部107判定是否满足用于向能够向带约束直接示教切换的状态(后述的第一状态)恢复的规定的恢复条件。此时，例如在操作者进行了解除操作后经过了一定时间的情况下，或者由位置姿势运算部104算出的臂部2的位置姿势未接近约束目标的情况下，恢复判定部107判定为满足了恢复条件。

切换部108根据接近判定部105的判定结果、解除操作判定部106的判定结果以及恢复判定部107的判定结果，切换约束控制运算部110的动作。即，在直接示教装置1为第一状态(接近判定中)的情况下，在通过接近判定部105判定为臂部2的位置姿势接近约束目标的情况下，切换部108将直接示教装置1切换为第二状态(解除操作判定中)。另外，在直接示教装置1为第二状态的情况下，在通过解除操作判定部106判定为操作者进行了解除操作的情况下，切换部108将直接示教装置1切换为第三状态(恢复判定中)。另外，在直接示教装置1为第三状态的情况下，在由恢复判定部107判定为满足恢复条件的情况下，切换部108将直接示教装置1切换为第一状态。在此基础上，切换部108在直接示教装置1为第二状态的情况下使约束控制运算部110的处理有效，在直接示教装置1为除此以外的状态的情况下使约束控制运算部110的处理无效。

另外，在存在多个约束目标的情况下，切换部108也可以按照接近判定部105的判定结果，实施目标值算出部109的动作(作为对象的约束目标)的切换。

目标值算出部109基于约束目标以及位置姿势运算部104的算出结果，算出带约束直接示教中的约束控制的目标值。另外，约束控制的目标值是指臂部2的目标位置及臂部2的目标姿势中的至少一方。

约束控制运算部110根据位置姿势运算部104的算出结果，算出移动到由目标值算出部109算出的目标值的臂部2的动作(约束控制指令值)。

合成部111将从动控制运算部103的算出结果(从动控制指令值)和约束控制运算部110的算出结果(约束控制指令值)合成为一个指令值。

驱动控制部112根据合成部111的合成结果来驱动臂部2。

在实施方式1的直接示教装置1中，能够通过操作者对臂部2的操作来执行通常的直接示教和带约束直接示教的切换。该切换方法通过以下所示的3个机制来实现。

第一个是在臂部2的位置姿势接近约束目标时，直接示教装置1自动地从通常的直接示教切换为带约束直接示教的机制。例如，如图2A、2B所示，考虑直接示教装置1使臂部2的位置约束在规定的平面506(约束面503)上的情况。在这种情况下，直接示教装置1在进行通常的直接示教的状态(第一状态)下，在从平面506到臂部2的位置的距离例如为±50[mm]以下的情况下，进行向带约束直接示教的切换，在除此以外的情况下保持通常的直接示教。另外，例如图3A、3B所示，考虑直接示教装置1使臂部2的姿势约束在正下方向507(约束方向505)的情况。在这种情况下，直接示教装置1在进行通常的直接示教的状态下，在臂部2的姿势距正下方向507例如为±0.2[rad]以下的情况下，进行向带约束直接示教的切换，在除此以外的情况下，保持通常的直接示教。通过该第一个机制，直接示教装置1仅通过操作者对臂部2的操作，就能够实现从通常的直接示教向带约束直接示教的切换。

第二个是通过直接示教装置1检测操作者的解除操作来解除带约束直接示教的机制。例如，直接示教装置1在进行带约束直接示教的状态(第二状态)下，在检测到操作者对臂部2施加5.0[N]以上的力的情况下，判定为有解除操作，中止带约束直接示教，在没有检测到5.0[N]以上的力的情况下，继续带约束直接示教。或者，直接示教装置1在进行带约束直接示教的状态下，在检测到操作者对臂部2顺时针施加了1.0[Nm]以上的旋转转矩的情况下，判定为有解除操作，中止带约束直接示教，在没有检测到1.0[Nm]以上的顺时针的旋转转矩的情况下，继续带约束直接示教。

另外，直接示教装置1在判定为有解除操作的情况下，不返回到最初的状态(第一状态)，而转移到恢复判定中的第三状态。直接示教装置1在恢复判定中执行通常的直接示教，但即使臂部2的位置姿势接近约束目标，也不执行向带约束直接示教的切换。这是因为，直接示教装置1在刚解除带约束直接示教之后，臂部2的位置姿势与约束目标一致，如果在该状态下切换为带约束直接示教，则无法切换为通常的直接示教。因此，直接示教装置1在检测到操作者的解除操作的情况下，暂时停止向带约束直接示教的切换。这是需要第三状态的原因。

并且，操作者在恢复判定中的期间，如图2C、3C所示，能够使臂部2远离约束目标。通过该第二个机制，直接示教装置1能够解除带约束直接示教。

第三是直接示教装置1在恢复判定中满足规定的恢复条件的情况下，恢复到能够向带约束直接示教切换的状态(第一状态)的机制。通过第二个机制，直接示教装置1能够解除带约束直接示教，但仅此不能再次进行带约束直接示教。因此，例如，若臂部2的位置姿势充分远离约束目标，则直接示教装置1恢复到第一状态。或者，例如，直接示教装置1在臂部2的约束被解除后经过一定时间以上时，恢复到第一状态。通过该第三个机制，直接示教装置1不仅解除带约束直接示教，而且如图2D、3D所示，成为能够再次切换为带约束直接示教的状态。

通过组合以上的3个机制，实施方式1的直接示教装置1能够在操作者操作臂部2的状态下，切换通常的直接示教和带约束直接示教。以下，参照图4对图1所示的直接示教装置1的动作例进行说明。

在图1所示的直接示教装置1的动作例中，如图4所示，首先，位置姿势计测部101计测与臂部2的位置姿势有关的参数(步骤ST 401)。

另外，外力检测部102检测由操作者施加在臂部2上的外力(步骤ST 402)。另外，根据传感器的结构等，由外力检测部102检测的外力不仅是由操作者施加在臂部2上的外力，有时还重叠了由重力引起的成分。因此，在这种情况下，外力检测部102可以算出由重力引起的分量，并且可以通过从检测到的外力中减去由重力引起的分量来仅算出外力分量。这是被称为重力补偿的公知技术，例如在专利文献5等中公开。

专利文献5:日本专利特开平01-066715号公报。

接着，从动控制运算部103根据位置姿势计测部101的计测结果，算出跟随由外力检测部102检测出的外力的臂部2的动作(从动控制指令值)(步骤ST 403)。此时，从动控制运算部103首先根据由外力检测部102检测到的外力的方向和大小，判定操作者如何使臂部2运动。然后，从动控制运算部103根据该判定结果和由位置姿势计测部101计测的臂部2的位置姿势，算出臂部2的移动量(关节角θ等的差分)或关节角速度θ(Dot)等。另外，基于从动控制运算部103的从动控制运算方法例如在专利文献2等中公开，由于开发了各种方式，所以省略其详细的记述。

接着，位置姿势运算部104根据从动控制运算部103的算出结果，算出臂部2的位置姿势(步骤ST 404)。

在此，在直接示教装置1进行位置约束的情况下(即直接示教装置1将臂部2的位置约束在规定的约束面503或约束轴504上的情况下)，位置姿势运算部104至少需要算出臂部2的位置。

另外，在直接示教装置1进行姿势约束的情况下(即直接示教装置1将臂部2的姿势约束在规定的约束方向505上的情况下)，位置姿势运算部104至少需要算出臂部2的姿势。

另外，在直接示教装置1进行位置约束及姿势约束的情况下(即直接示教装置1将臂部2的位置及姿势约束在规定的约束面503或约束轴504且规定的约束方向505上的情况下)，位置姿势运算部104需要算出臂部2的位置及姿势。

另外，最优选位置姿势运算部104根据从动控制运算部103的算出结果(最新的从动控制指令值)进行上述计算。这是因为，在直接示教装置1进行带约束直接示教的情况下，能够同时抵消操作者对臂部2赋予的移动量中的从约束面503等脱离的成分，约束控制最有效地发挥作用。

另外，位置姿势运算部104也可以不使用从动控制运算部103的算出结果，而根据位置姿势计测部101的计测结果进行上述计算。另一方面，在该情况下，由于在计测到从约束面503等脱离之后开始约束控制，所以约束控制的效果变差。

另外，位置姿势运算部104也可以根据从动控制运算部103的算出结果和位置姿势计测部101的计测结果这两者进行上述计算。

另外，在由位置姿势计测部101和从动控制运算部103仅得到关节角θ而不能直接得到臂部2的位置姿势的情况下，位置姿势运算部104能够通过正向运动学运算来算出臂部2的位置姿势。

接着，接近判定部105根据位置姿势运算部104的算出结果，判定臂部2的位置姿势是否接近约束目标(步骤ST 405)。

例如，约束控制的目的是将末端执行器201的顶端的Z轴坐标固定在0.1[m]的位置，另外，接近判定的阈值为0.05[m]。在这种情况下，如果由位置姿势运算部104算出的末端执行器201的顶端的Z轴坐标在0.05～0.15[m]的范围内，则接近判定部105判定为臂部2的位置接近约束目标，在除此以外的情况下，接近判定部105判定为臂部2的位置未接近约束目标。

另外，约束控制的目的是使末端执行器201的朝向502(Z轴)朝向正下方向，另外，接近判定的阈值为0.1[rad]。在这种情况下，接近判定部105如图5所示，如果正上方向(基准方向)508与末端执行器201的朝向502所成的角度Θ为(π-0.1)[rad]以上(即正下方向507与末端执行器201的朝向502所成的角度小于0.1[rad])，则判定为臂部2的姿势接近约束目标，在除此以外的情况下判定为臂部2的姿势未接近约束目标。

另外，在存在多个约束面503的情况等这样存在多个约束目标的情况下，接近判定部105对每个约束目标进行上述判定，在判定为臂部2的位置姿势接近各约束目标中的至少一个约束目标的情况下，判定为臂部2的位置姿势接近约束目标。此时，接近判定部105还可以评价臂部2与各约束目标之间的距离，与上述判定的结果一起将表示最接近臂部2的约束目标的信息通知给切换部108。

另外，解除操作判定部106判定操作者是否进行了解除操作(步骤ST 406)。

基于解除操作判定部106的最优选的判定方法是使用外力的判定方法。解除操作判定部106的使用外力的最简单的判定方法的一例是在外力为规定的力(例如5.0[N])以上的情况下判定为操作者进行了解除操作这样的、基于外力的大小的判定方法。另外，解除操作判定部106也可以除了外力的大小之外还考虑时间，在外力为规定的力以上且持续一定时间以上的情况下，判定为操作者进行了解除操作。

图6表示使用了解除操作判定部106的外力的更复杂的判定方法的一例。

如图6A所示，解除操作判定部106也可以在外力为远离约束面503的方向且为规定的力以上的情况下，判定为操作者进行了解除操作。该判定方法可以通过在外力检测部102中对外力中的与约束面503正交的成分的大小进行检测来实现。这样，作为解除操作判定部106的判定方法，能够实施不仅考虑外力的大小还考虑外力的朝向的判定方法。

另外，如图6B所示，解除操作判定部106也可以在外力是以末端执行器201的轴为中心的旋转力且在规定的力以上的情况下，判定为操作者进行了解除操作。该判定方法可以通过在外力检测部102中对以末端执行器201的轴为中心的旋转力进行检测来实现。

另外，如图6C所示，解除操作判定部106也可以在外力是在左右方向上交替施加的力且在规定的力以上的情况下，判定为操作者进行了解除操作。在该判定方法中，解除操作判定部106不仅考虑外力的大小和方向，还考虑时间序列的模式，因此能够期待通过非本意的操作偶然判定为解除操作的可能性(即误判定的可能性)变低。作为解除操作判定部106判定是否是想要解除操作的模式的方法，可以利用公知的模式识别、机器学习或神经网络等各种方法。

另外，解除操作判定部106不限于基于外力的判定，也可以使用臂部2的位置姿势来判定操作者是否进行了解除操作。但是，直接示教装置1在进行带约束直接示教的期间，约束臂部2的位置姿势，所以只能在该约束范围内移动臂部2。因此，解除操作判定部106的使用臂部2的位置姿势的判定方法在某种程度上受到限制。图7表示解除操作判定部106的使用臂部2的位置姿势的判定方法的一例。

如图7A～7C所示，考虑直接示教装置1将臂部2约束在Z坐标恒定的平面506(约束面503)上的情况。

在该情况下，如图7A所示，解除操作判定部106也可以在臂部2的X坐标或Y坐标比平面506靠外(例如±0.5[m]的范围外)的情况下，判定为操作者进行了解除操作。另外，解除操作判定部106也可以在从工件到末端执行器201的顶端的距离例如大于0.2[m]的情况下，判定为操作者进行了解除操作。

另外，如图7B所示，解除操作判定部106也可以在臂部2在约束面503上移动为规定的形状(在图7B中为三角形)的情况下判定为操作者进行了解除操作等，基于末端执行器201的位置的时间序列的模式进行判定。作为解除操作判定部106判定是否是想要解除操作的模式的方法，可以利用公知的模式识别、机器学习或神经网络等各种方法。

另外，如图7C所示，解除操作判定部106也可以在臂部2在约束面503上旋转一周的情况下判定为操作者进行了解除操作等，基于末端执行器201的姿势的时间序列的模式进行判定。但是，该判定方法有时在直接示教装置1进行带约束直接示教的状态下不能实施。作为解除操作判定部106判定是否是想要解除操作的模式的方法，可以利用公知的模式识别、机器学习或神经网络等各种方法。

另外，如图7D、图7E所示，考虑直接示教装置1将臂部2约束在与Z轴平行的轴(约束轴504)上的情况。

此时，如图7D所示，解除操作判定部106也可以在末端执行器201的顶端的Z坐标大于规定的位置(例如0.3[m])的情况下，判定为操作者进行了解除操作。另外，解除操作判定部106也可以在从工件到末端执行器201的顶端的距离大的情况下(即臂部2远离工件的情况下)，判定为操作者进行了解除操作。

另外，如图7E所示，解除操作判定部106也可以在臂部2在约束轴504上以比规定的周期短的周期上下往复的情况下，判定为操作者进行了解除操作。

另外，解除操作判定部106也可以代替由位置姿势运算部104算出的臂部2的位置姿势，而使用与由位置姿势计测部101计测出的臂部2的位置姿势相关的参数。

另外，在直接示教装置1的实施环境是能够取得操作者的声音的环境的情况下，直接示教装置1也可以使用麦克风等取得操作者的声音，解除操作判定部106通过声音识别来判定操作者是否进行了解除操作。在该判定方法中，操作者能够不将手从臂部2离开地实施解除操作。

另外，在直接示教装置1的实施环境是能够将操作者的图像作为动态图像取得的环境的情况下，直接示教装置1也可以使用照相机等取得操作者的嘴唇的图像，解除操作判定部106通过从该图像中识别操作者发出的词语来判定操作者是否进行了解除操作。在该方法中，操作者也能够不将手从臂部2离开地实施解除操作。

另外，恢复判定部107判定是否满足了用于向能够向带约束直接示教切换的状态恢复的规定的恢复条件(步骤ST 407)。如上所述，直接示教装置1如果在由操作者进行解除操作之后立即转移到第一状态，则即使进行解除操作也会立即返回到带约束直接示教，因此需要不这样。恢复判定部107是为了消除这样的不良情况而存在的。

恢复判定部107的最优选的判定方法之一是等待接近判定部105不再判定为臂部2的位置姿势接近约束目标而判定为满足恢复条件的方法。例如，恢复判定部107也可以在图2中臂部2从约束面503离开±50[mm]以上的情况下，在图3中正下方向507与臂部2的姿势所成的角度离开±0.2[rad]以上的情况下，判定为满足恢复条件。由此，直接示教装置1能够抑制在由操作者进行了解除操作之后违反操作者的意图而使带约束直接示教再次有效的情况。

另外，恢复判定部107也可以不直接使用接近判定的阈值，而使用与接近判定的阈值相比多少具有富余的阈值来进行判定，认为成为更稳定的动作。例如，恢复判定部107也可以在图2中臂部2从约束面503离开±55[mm]以上的情况下，在图3中正下方向507与臂部2的姿势所成的角度离开±0.22[rad]以上的情况下，判定为满足恢复条件。

另外，恢复判定部107也可以在操作者进行了解除操作后经过了一定时间的情况下，判定为满足了恢复条件。例如，在操作者进行了解除操作后经过了10秒的情况下，恢复判定部107也可以判定为满足了恢复条件。在这种情况下，操作者在想要返回通常的直接示教的情况下，只要在10秒以内向不满足接近判定条件的区域操作臂部2即可。另外，操作者在想要重新开始带约束直接示教的情况下，只要将臂部2保持原样而等待时间经过即可。

另外，恢复判定部107也可以组合上述两个方法，在臂部2的位置姿势从约束目标离开后经过了一定时间的情况下，判定为满足恢复条件。

另外，无论在恢复判定部107使用从约束目标的距离或从解除操作开始的时间中的哪一个的情况下，操作者都能够不将手从臂部2离开地进行恢复操作。

接着，切换部108根据接近判定部105的判定结果、解除操作判定部106的判定结果以及恢复判定部107的判定结果，切换约束控制运算部110的动作(步骤ST 408)。

如图8所示，切换部108将直接示教装置1分为三个状态(第一状态～第三状态)进行管理。然后，切换部108基于接近判定部105、解除操作判定部106以及恢复判定部107的各自的判定结果，切换直接示教装置1的状态。第一状态是仅接近判定部105的判定有效的接近判定中的状态，直接示教装置1从该状态开始动作。第二状态是仅解除操作判定部106的判定有效的解除操作判定中的状态。第三状态是仅恢复判定部107的判定有效的恢复判定中的状态。直接示教装置1成为这3个状态中的某一个状态。

在直接示教装置1为第一状态的情况下，切换部108仅参照接近判定部105的判定结果。然后，在由接近判定部105判定为臂部2的位置姿势接近约束目标的情况下，切换部108将直接示教装置1切换为第二状态。

在直接示教装置1为第二状态的情况下，切换部108仅参照解除操作判定部106的判定结果。然后，在通过解除操作判定部106判定为操作者进行了解除操作的情况下，切换部108将直接示教装置1切换为第三状态。

在直接示教装置1为第三状态的情况下，切换部108仅参照恢复判定部107的判定结果。然后，在由恢复判定部107判定为满足恢复条件的情况下，切换部108将直接示教装置1切换为第一状态。

然后，根据直接示教装置1的状态，切换部108将约束控制运算部110的处理切换为有效或无效。即，在直接示教装置1为第二状态的情况下，切换部108使约束控制运算部110的处理有效。另外，在直接示教装置1为第一状态或第三状态的情况下，切换部108停止约束控制运算部110的运算，或者使算出结果不反映在臂部2的控制中，使约束控制运算部110的处理无效。

另外，如图8所示，切换部108也可以在直接示教装置1为第二状态的情况下执行目标值算出部109的目标值的算出，在直接示教装置1为第一状态或第三状态的情况下停止目标值算出部109的目标值的算出。

这样，当操作者使臂部2的位置姿势接近约束目标时，直接示教装置1自动地开始约束控制。另外，直接示教装置1在由操作者进行带约束直接示教的解除操作时，自动结束约束控制。另外，直接示教装置1在满足恢复条件后再次由操作者使臂部2的位置姿势接近约束目标时，自动地开始约束控制。由此，直接示教装置1能够在由操作者保持臂部2的状态下切换约束控制的开和关。

另外，在存在多个约束目标且从接近判定部105通知了表示最接近臂部2的约束目标的信息的情况下，切换部108也可以将该信息输出到后述的目标值算出部109来切换该目标值算出部109的动作。

另外，接近判定部105、解除操作判定部106以及恢复判定部107也可以在切换部108不需要该判定结果时停止处理。例如，在直接示教装置1为第二状态的情况下，由于切换部108不使用接近判定部105的判定结果，所以接近判定部105即使停止处理也没有障碍。另外，在直接示教装置1为第一状态或第三状态的情况下，由于切换部108不使用解除操作判定部106的判定结果，所以解除操作判定部106即使停止处理也没有障碍。另外，在直接示教装置1为第一状态或第二状态的情况下，由于切换部108不使用恢复判定部107的判定结果，所以恢复判定部107即使停止处理也没有障碍。另外，在恢复判定部107为了恢复判定而使用接近判定部105的判定结果的情况下，接近判定部105在直接示教装置1为第三状态的情况下也需要实施处理。

接着，目标值算出部109根据约束目标和位置姿势运算部104的算出结果，算出约束控制的目标值(步骤ST 409)。

例如，约束控制的目的是将末端执行器201的顶端的Z轴坐标固定在0.1[m]的位置，由位置姿势运算部104算出的末端执行器201的顶端的当前值为[X_P、Y_P、Z_P]。在该情况下，目标值算出部109将约束控制的目标值设为[X_P、Y_P、0.1]。

另外，如图9下段所示，约束控制的目的是使末端执行器201的朝向502中的Z轴为正下方向507，然后使X轴与机器人坐标系的X轴一致。这种情况下，用旋转矩阵表现表示约束控制的目标值即目标姿势的公式为下式(1)。另外，例如在非专利文献1等中公开了基于旋转矩阵的姿势的表现，省略其详细说明。

非专利文献1：John J.Craig(三浦下山译)：机器人，共立出版(1991)(JohnJ.Craig(三浦·下山訳)：ロボティクス，共立出版(1991))。

另外，目标值算出部109可以仅算出一个目标值作为约束控制的目标值，也可以算出多个目标值并从其中选择一个目标值。例如，在约束控制的目的是将末端执行器201的顶端的Z轴坐标以0.1[m]刻度固定在最接近的位置的情况下，目标值算出部109选择[X_P、Y_P、0.1n](n为整数)中最接近当前位置的值作为约束控制的目标值。

另外，如图10所示，在约束控制的目的是将末端执行器201的朝向502中的Z轴设为正下方向507，并且以该Z轴为旋转轴以π/2刻度(90度刻度)旋转的姿势进行固定的情况下，目标值算出部109选择最接近当前姿势的目标值作为约束控制的目标值。此时，用旋转矩阵表现来表示作为约束控制的目标值的目标姿势的情况如下式(2)所示。

如图10所示，该式(2)中目标姿势根据n而变化为4种，目标值算出部109从其中选择与当前姿势最接近的一个。目标值算出部109能够根据从当前的姿势向各目标姿势旋转时的旋转角的大小等来判定是否接近当前的姿势。另外，在接近判定部105通知了表示最接近臂部2的约束目标的信息的情况下，目标值算出部109也可以根据该信息选择约束控制的目标值。

接着，约束控制运算部110根据位置姿势运算部104的算出结果，算出移动到由目标值算出部109算出的目标值的臂部2的动作(约束控制指令值)(步骤ST 410)。此时，约束控制运算部110根据由目标值算出部109算出的目标值，算出使臂部2从当前的位置姿势移动到目标值所需要的臂部2的移动量或每单位时间的移动量即关节角速度θ(Dot)等。另外，约束控制运算部110也可以代替关节角速度θ(Dot)而运算每一个控制周期的关节角变化量Δθ。

接着，合成部111将从从动控制运算部103输出的从动控制指令值和从约束控制运算部110输出的约束控制指令值合成为一个指令值(步骤ST 411)。

例如,在从动控制运算部103输出关节角θ_D作为从动控制指令值、约束控制运算部110输出关节角变化量Δθ_C作为约束控制指令值的情况下,合成部111输出θ_D+Δθ_C作为关节角指令值。

另外，例如，在从动控制运算部103输出位置坐标[x_D，y_D，z_D]以及姿势T_D(旋转矩阵表现)作为从动控制指令值，约束控制运算部110输出位置变化量[Δx_C，Δy_C，Δz_C]以及姿势变化量ΔT_C(旋转矩阵表现)作为约束控制指令值的情况下，合成部111将[x_D+Δx_C，y_D+Δy_C，z_D+Δz_C]作为位置指令值，将ΔT_CT_D(旋转矩阵表现)作为姿势指令值输出(旋转矩阵表现的姿势的合成为矩阵的积)。

另外,例如,在从动控制运算部103输出转矩τ_D作为从动控制指令值、约束控制运算部110输出转矩变化量Δτ_C作为约束控制指令值的情况下,合成部111输出τ_D+Δτ_C作为转矩指令值。

接着，驱动控制部112根据合成部111的合成结果，驱动臂部2(步骤ST 412)。该驱动控制部112对臂部2的驱动是通常的臂部2的控制，因此省略其详细说明。

如上所述，根据本实施方式1，直接示教装置1具有：外力检测部102，其检测施加在机器人所具有的臂部2上的外力；从动控制运算部103，其算出跟随由外力检测部102检测出的外力的臂部2的动作；位置姿势运算部104，其算出臂部2的位置或姿势中的至少一方即位置姿势；接近判定部105，其根据位置姿势运算部104的算出结果，判定臂部2的位置姿势是否接近作为该臂部2的位置姿势的约束目的地的约束目标；解除操作判定部106，其判定操作者是否进行了用于解除带约束直接示教的解除操作；恢复判定部107，其判定是否满足能够切换为带约束直接示教的状态的恢复条件；目标值算出部109，其基于约束目标以及位置姿势运算部104的算出结果，算出带约束直接示教中的约束控制的目标值；约束控制运算部110，其基于位置姿势运算部104的算出结果，算出移动到由目标值算出部109算出的目标值的臂部2的动作；切换部108，其在本机处于第一状态的情况下，在由接近判定部105判定为接近约束目标的情况下，将本机切换到第二状态，在本机处于第二状态的情况下，在由解除操作判定部106判定为操作者进行了解除操作的情况下，将本机切换到第三状态，在本机处于第三状态的情况下，在由恢复判定部107判定为满足恢复条件的情况下，将本机切换到第一状态，在本机处于第二状态时使约束控制运算部110的处理有效，在本机处于第一状态或第三状态时使约束控制运算部110的处理无效；合成部111，其合成从动控制运算部103的算出结果和约束控制运算部110的算出结果；以及驱动控制部112，其根据合成部111的合成结果来驱动臂部2。由此，实施方式1的直接示教装置1能够在操作者操作臂部2的状态下切换通常的直接示教和带约束直接示教。

实施方式2

在实施方式2中，表示臂部2是垂直多关节型机器人臂，由直接示教装置1进行的约束方法仅限定于姿势约束的情况。另外，实施方式2的直接示教装置1的构成例与实施方式1的直接示教装置1的构成例相同，以下使用图1所示的构成例进行说明。

另外，实施方式2中的位置姿势运算部104算出臂部2的姿势。

另外，实施方式2中的接近判定部105根据位置姿势运算部104的算出结果，判定臂部2的姿势是否接近约束方向505。

接着，参照图11说明实施方式2的直接示教装置1的动作例。

以下，直接示教装置1进行将末端执行器201的朝向502(Z轴)约束在正下方向507或正横方向509上的姿势约束。另外，用e_X、e_Y、e_Z表示机器人坐标系的各轴的单位矢量。另外，臂部2为6轴。另外，切换部108切换目标值算出部109和约束控制运算部110的动作。另外，从动控制运算部103算出臂部2的各关节角θ作为从动控制指令值，约束控制运算部110算出臂部2的各关节角速度θ(Dot)作为约束控制指令值。

在实施方式2的直接示教装置1的动作例中，如图11所示，首先，位置姿势计测部101计测臂部2的各关节角θ(步骤ST 1101)。例如，位置姿势计测部101使用安装在臂部2所具有的电动机上的编码器等，针对臂部2的每个关节计测关节角θ。

接着，外力检测部102检测由操作者施加在臂部2上的外力(步骤ST 1102)。该外力检测部102的处理与实施方式1相同。另外，外力检测部102进行重力补偿。

接着，从动控制运算部103根据位置姿势计测部101的计测结果，算出跟随由外力检测部102检测到的外力的臂部2的各关节角θ(从动控制指令值)(步骤ST 1103)。参照图12说明该从动控制运算部103的动作例。

从动控制运算部103例如通过图12A所示的方法，能够根据由外力检测部102检测出的转矩τ算出臂部2的各关节角θ。

在图12A中，从动控制运算部103首先对由外力检测部102检测出的转矩τ乘以增益K_T，由此对臂部2的每个关节算出作为目标的关节角速度θ(Dot)。另外，增益K_T在臂部2的每个关节可以相同也可以不同。另外，如图12A所示，从动控制运算部103也可以对作为目标的关节角速度θ(Dot)应用限制器，限制从动控制的速度。

接着，从动控制运算部103通过对作为目标的关节角速度θ(Dot)乘以控制周期T_s，对臂部2的每个关节算出每1个控制周期的关节角变化量Δθ。

接着，从动控制运算部103针对臂部2的每个关节，在当前时刻k的关节角θ(k)上加上关节角变化量Δθ，算出下一个时刻k+1的关节角θ(k+1)。

另外，从动控制运算部103例如通过图12B所示的方法，能够根据由外力检测部102检测出的力F来运算臂部2的各关节角θ。

在图12B的例子中，从动控制运算部103首先对由外力检测部102检测出的力F乘以增益K_F，由此针对臂部2的每个位置和姿势算出移动速度。另外，增益K_F在臂部2的位置以及姿势的各成分中可以相同，也可以不同。

接着，从动控制运算部103通过将移动速度乘以雅可比矩阵J的逆矩阵，对臂部2的每个关节算出作为目标的关节角速度θ(Dot)。另外，从动控制运算部103在不存在雅可比矩阵J的逆矩阵的情况下乘以伪逆矩阵。另外，如图12B所示，从动控制运算部103也可以对作为目标的关节角速度θ(Dot)应用限制器，限制从动控制的速度。

接着，从动控制运算部103通过对作为目标的关节角速度θ(Dot)乘以控制周期T_s，对臂部2的每个关节算出每1个控制周期的关节角变化量Δθ。

接着，从动控制运算部103针对臂部2的每个关节，在当前时刻k的关节角θ(k)上加上关节角变化量Δθ，算出下一个时刻k+1的关节角θ(k+1)。

接着，位置姿势运算部104根据从动控制运算部103的算出结果，算出臂部2的姿势(步骤ST 1104)。位置姿势运算部104能够通过正向运动学执行上述计算。以下，通过旋转矩阵用T表示由位置姿势运算部104算出的臂部2的姿势。另外，如下式(3)那样表示将T分解为列矢量或要素。

接着，接近判定部105根据位置姿势运算部104的算出结果，判定臂部2的姿势是否接近约束方向505(步骤ST 1105)。这里，约束方向505是正下方向507或正横方向509。另外，设接近判定的阈值为0.1[rad]。参照图13说明接近判定部105的动作例。

在接近判定部105的动作例中，如图13所示，接近判定部105首先算出正上方向(基准方向)508与末端执行器201的朝向502(Z轴)所成的角度Θ(步骤ST 1301)。即，接近判定部105如下式(4)那样，根据e_Z与t_Z的内积算出角度Θ。

Θ＝cos^-1(t_Z·e_Z)＝cos^-1(t_Zz) (4)

然后，接近判定部105判定角度Θ是否在从(π/2-0.1)[rad]到(π/2+0.1)[rad]的范围内(步骤ST 1302)。

在该步骤ST 1302中，接近判定部105在判定为角度Θ在从(π/2-0.1)[rad]到(π/2+0.1)[rad]的范围内的情况下(例如图14A的情况)，判定为臂部2的姿势接近正横方向509(步骤ST 1303)。

另一方面，在步骤ST 1302中，接近判定部105在判定为角度Θ不在从(π/2-0.1)[rad]到(π/2+0.1)[rad]的范围内的情况下，判定角度Θ是否在(π-0.1)[rad]以上(步骤ST1304)。

在该步骤ST 1304中，接近判定部105在判定为角度Θ为(π-0.1)[rad]以上的情况下(例如图14B的情况)，判定为臂部2的姿势接近正下方向507(步骤ST 1305)。

另一方面，在步骤ST 1304中，接近判定部105在判定为角度Θ不是(π-0.1)[rad]以上的情况下(例如图14C的情况)，判定为臂部2的姿势不接近任何约束方向505(步骤ST1306)。

另外，解除操作判定部106判定操作者是否进行了解除操作(步骤ST 1106)。这里，在解除操作判定部106进行使用外力的判定的情况下，可以应用与实施方式1同样的方法。另一方面，解除操作判定部106在进行使用了臂部2的位置姿势的判定的情况下，由于在实施方式2中直接示教装置1进行臂部2的姿势约束，因此能够应用的判定方法限定为能够与姿势约束并存的判定方法。例如，实施方式2中的解除操作判定部106不能应用如图7C所示那样使用臂部2的姿势的模式的判定方法。

另外，恢复判定部107判定是否满足向能够向带约束直接示教切换的状态恢复的恢复条件(步骤ST 1107)。在实施方式2中，由于直接示教装置1进行臂部2的姿势约束，因此恢复判定部107在基于臂部2的位置姿势进行判定的情况下，能够基于角度Θ的值进行判定。例如，在角度Θ不在从(π/2-0.1)[rad]到(π/2+0.1)[rad]的范围内，并且小于(π-0.1)[rad]的情况下，恢复判定部107能够判定为臂部2的姿势从约束姿势离开，满足了恢复条件。

另外在上述中，恢复判定部107使用(π/2-0.1)[rad]、(π/2+0.1)[rad]和(π-0.1)[rad]作为恢复判定的阈值。但是，不限于此，恢复判定部107也可以如(π/2-0.12)[rad]、(π/2+0.12)[rad]和(π-0.12)[rad]那样，使用比上述阈值稍宽的值作为恢复判定的阈值。

另一方面，恢复判定部107不能使用比接近判定部105使用的阈值窄的值作为恢复判定的阈值。这是因为，若恢复判定部107使用上述的窄的值，则在恢复判定部107判定为满足恢复条件之后，接近判定部105判定为臂部2的姿势接近约束方向505，直接示教装置1不能从带约束直接示教脱离。

另外，在恢复判定部107在判定中不使用臂部2的位置姿势而仅使用时间的情况下，能够应用与实施方式1同样的方法。

接着，切换部108切换目标值算出部109的动作，并且切换约束控制运算部110的动作(步骤ST 1108)。即，切换部108按照接近判定部105的判定结果，切换目标值算出部109的动作。另外，切换部108根据接近判定部105的判定结果、解除操作判定部106的判定结果以及恢复判定部107的判定结果，切换约束控制运算部110的动作。

此时，在接近判定部105判定为臂部2的姿势接近正横方向509的情况下，切换部108使目标值算出部109算出向正横方向509的目标姿势作为目标值。

另外，在接近判定部105判定为臂部2的姿势接近正下方向507的情况下，切换部108使目标值算出部109算出向正下方向507的目标姿势作为目标值。

另外，在由接近判定部105判定为臂部2的姿势没有接近任何约束方向505的情况下，切换部108不需要由目标值算出部109进行算出。

另外，切换部108对约束控制运算部110的动作的切换与实施方式1相同。

接着，目标值算出部109基于约束目标及位置姿势运算部104的算出结果，算出目标姿势(步骤ST 1109)。例如，目标值算出部109算出为正横方向509或正下方向507、且能够从臂部2的当前姿势以最小的旋转角度Θ_R到达的目标姿势。以下示出目标值算出部109的目标姿势的算出方法。

首先，目标值算出部109对成为对象的每个目标姿势，算出相对于e_Z的目标角度Θ_*。在此，在目标姿势为正横方向509的情况下，目标角度Θ_*为π/2，在目标姿势为正下方向507的情况下，目标角度Θ_*为π。

接着，目标值算出部109基于臂部2的姿势，算出旋转角度Θ_R最小的旋转轴510。如图15所示，该旋转轴510是位于机器人坐标系的XY平面上且与t_Z正交的轴(即与e_Z和t_Z两者正交的轴)。因此，目标值算出部109求出一个与e_Z和t_Z双方内积为0的单位矢量即r，将其作为旋转轴510。另外，目标值算出部109在e_Z与t_Z平行的情况下无法唯一地求出r。但是，这是臂部2的姿势朝向正上方向508或正下方向507的情况，目标值算出部109算出XY平面上的任意矢量作为旋转轴510。

接着，目标值算出部109基于算出的目标角度Θ_*以及旋转轴510，算出旋转角度Θ_R作为目标姿势。这里，在末端执行器201的朝向502与正上方向(基准方向)508成角度Θ的情况下，旋转角度Θ_R的大小为Θ_*-Θ，但如图16所示，需要考虑旋转的朝向(正负)。这是因为，在实施方式2的直接示教装置1中，由于在求出角度Θ时使用内积，所以末端执行器201的朝向502的信息丢失。如果考虑旋转的方向，则旋转角度Θ_R如下式(5)所示。即，关于旋转的方向，在e_Z×t_Z和r为相同方向的情况下，不需要符号的反转，在e_Z×t_Z和r为相反方向的情况下，需要符号的反转。

在目标值算出部109中，如果得到表示旋转轴510的r＝[r_x，r_y，r_z]^T(T表示转置)和旋转角度Θ_R，则得到从臂部2的姿势向目标姿势的旋转，所以事实上得到约束目标。在需要目标姿势的具体表现的情况下，目标值算出部109通过对臂部2的姿势应用上述的旋转而得到。

在上述说明中，描述了目标值算出部109算出使末端执行器201的朝向502(Z轴)朝向正下方向507或正横方向509的目标姿势的方法。目标值算出部109也可以从此进一步施加旋转而成为所希望的姿势，将其作为目标姿势。

例如，目标值算出部109通过从臂部2朝向正下方向507的状态绕Z轴施加旋转，也能够将图10所示的姿势作为目标姿势。另外，如图17所示，目标值算出部109也可以从臂部2朝向正横方向509的状态进一步绕Z轴施加旋转，将t_X或t_Y与机器人坐标系的Z轴平行的姿势作为目标姿势。

接着，约束控制运算部110根据位置姿势运算部104的算出结果，算出移动到由目标值算出部109算出的目标姿势的臂部2的动作(约束控制指令值)(步骤ST 1110)。另外，由于目标值算出部109在算出目标姿势时已经算出使臂部2从当前的姿势向目标姿势移动的旋转，因此约束控制运算部110算出使臂部2这样旋转的动作。但是，约束控制运算部110的输出需要为关节角速度θ(Dot)，因此约束控制运算部110需要根据由目标值算出部109算出的旋转轴510即r与旋转角度Θ_R的组来算出关节角速度θ(Dot)。以下表示其计算的一例。

约束控制运算部110首先决定旋转的角速度的大小|ω|。例如，如果没有受到限制，以1秒到达目标姿势的方式进行控制，则|ω|＝θ_R。如果不希望角速度过大，则约束控制运算部110也可以适当限制角速度。

接着，约束控制运算部110算出臂部2的当前的位置姿势下的雅可比矩阵J。对于雅可比矩阵J，下式(6)成立。

在此，v_X、v_Y、v_Z是末端执行器201的顶端的X、Y、Z轴方向的速度，ω_X、ω_Y、ω_Z是臂部2的姿势绕X轴、Y轴、Z轴旋转的角速度，θ₁(Dot)～θ₆(Dot)是各关节角速度。

接着，约束控制运算部110根据作为旋转轴510的r和旋转速度的大小|ω|，算出与姿势有关的旋转矢量ω＝[ω_X，ω_Y，ω_Z]^T。

接着，约束控制运算部110算出满足下式(7)的臂部2的每个关节的关节角速度θ₁(Dot)～θ₆(Dot)。另外，约束控制运算部110在雅可比矩阵J中不存在逆矩阵的情况下，使用伪逆矩阵等算出关节角速度θ₁(Dot)～θ₆(Dot)。

另外，约束控制运算部110为了避免所得到的关节角速度过大，也可以应用限制器。在这种情况下，约束控制运算部110优选对全部关节角乘以相同常数(0至1之间的值)来进行限制，以不改变旋转的方向。

接着，合成部111合成从从动控制运算部103输出的从动控制指令值(关节角θ)和从约束控制运算部110输出的约束控制指令值(关节角速度θ(Dot))(步骤ST 1111)。

例如，在从动控制运算部103输出关节角θ＝[θ₁，…，θ_m]^T，约束控制运算部110输出关节角速度θ(Dot)＝[θ₁(Dot)，…，θ_m(Dot)]^T的情况下，根据控制周期T_S，作为合成部111的输出的关节角指令值为θ+T_Sθ(Dot)。其中，m是驱动控制部112控制的臂部2的关节数。

接着，驱动控制部112基于合成部111的合成结果(关节角指令值)，驱动臂部2(步骤ST 1112)。

另外，在上述中，一部分以6轴(关节数为6个)的臂部2为前提进行了说明和例示，但即使是6轴以外的臂部2也同样能够实施。

实施方式3

在实施方式2的直接示教装置1中，示出了约束方法仅限于姿势约束的情况。与此相对，在实施方式3的直接示教装置1中，表示约束方法仅限于轴约束的情况。所谓轴约束是指同时进行将臂部2的位置约束在规定的约束轴504上的位置约束和将臂部2的姿势约束在规定的约束方向505上的姿势约束。即，实施方式3的直接示教装置1除了实施方式2所示的姿势约束之外，还进行位置约束。以下，表示直接示教装置1进行向与图18所示的机器人坐标系的Z轴平行的直线511的位置约束以及向正下方向507的姿势约束的情况，但约束轴504以及约束方向505可以任意设定。

另外，实施方式3的直接示教装置1的构成例与实施方式2的直接示教装置1的构成例相同，以下使用图1所示的构成例进行说明。

另外，实施方式3中的位置姿势运算部104算出臂部2的位置以及姿势。

另外，实施方式3中的接近判定部105基于位置姿势运算部104的算出结果，判定臂部2的位置是否接近作为约束目标的约束轴504，并且判定臂部2的姿势是否接近作为约束目标的约束方向505，由此判定臂部2的位置姿势是否接近约束目标。

图19是表示实施方式3的直接示教装置1的动作例的流程图。在图19所示的实施方式3的直接示教装置1中，动作与图11所示的实施方式2的直接示教装置1不同的是位置姿势运算部104、接近判定部105、解除操作判定部106、恢复判定部107、目标值算出部109以及约束控制运算部110，仅对这些构成的动作例进行说明。

首先，位置姿势运算部104根据从动控制运算部103的算出结果，算出臂部2的位置和姿势(步骤ST 1904)。在实施方式3中，为了约束臂部2的位置和姿势这两者，位置姿势运算部104需要算出臂部2的位置和姿势这两者。位置姿势运算部104能够通过正向运动学执行上述算出。

另外，接近判定部105根据位置姿势运算部104的算出结果，分别判定臂部2的位置是否接近约束轴504以及臂部2的姿势是否接近约束方向505，根据这些判定结果综合判定臂部2的位置姿势是否接近约束目标(步骤ST 1905)。例如，接近判定部105在判定为臂部2的位置接近约束轴504且臂部2的姿势接近约束方向505的情况下，判定为臂部2的位置姿势接近约束目标。关于接近判定部105对约束方向505的判定，在实施方式2中进行了说明，因此省略，以下，对接近判定部105对约束轴504的判定进行说明。

此时，接近判定部105例如如图20所示，从末端执行器201的位置501向约束轴504引出垂线512，如果该垂线512的长度小于规定值，则判定为臂部2的位置接近约束轴504。例如，用[x，y，z]表示臂部2的位置的坐标，分别用x^*，y^*表示直线511的X坐标和Y坐标。此时，垂线512的长度如下式(8)所示。

然后，如果式(8)所示的垂线512的长度小于规定值，则接近判定部105判定为臂部2的位置接近约束轴504，在除此以外的情况下，接近判定部105判定为臂部2的位置没有接近约束轴504。另外，在约束轴504与机器人坐标系的Z轴平行的情况以外，接近判定部105也同样能够根据相对于约束轴504引出的垂线的长度进行接近判定。

接着，解除操作判定部106判定操作者是否进行了解除操作(步骤ST 1906)。这里，在解除操作判定部106进行使用外力的判定的情况下，可以应用与实施方式1同样的方法。另一方面，在解除操作判定部106进行使用了臂部2的位置姿势的判定的情况下，在实施方式3中，直接示教装置1进行臂部2的轴约束，因此可应用的判定方法限定为能够与轴约束并存的判定方法。例如，在图7的例子中，实施方式3中的解除操作判定部106只能应用图7D、7E所示的方法。另一方面，实施方式3中的解除操作判定部106不能应用图7A～图7C所示的方法，因为在臂部2被轴约束的状态下不能实施。

接着，恢复判定部107判定是否满足向能够向带约束直接示教切换的状态恢复的恢复条件(步骤ST 1907)。在实施方式3中，由于直接示教装置1进行臂部2的轴约束，所以恢复判定部107在基于臂部2的位置姿势进行判定的情况下，能够基于臂部2的位置与约束轴504之间的距离以及臂部2的姿势与约束方向505之间的关系进行判定。关于恢复判定部107对约束方向505的判定，在实施方式2中进行了说明，因此省略，以下，对恢复判定部107对约束轴504的判定进行说明。

在恢复判定部107对约束轴504的判定中，与接近判定部105同样，根据垂线512的长度进行。即，恢复判定部107在垂线512的长度大于规定值的情况下，判定为满足恢复条件。另外，上述规定的值需要与接近判定部105使用的阈值相同或更大。这是因为，如果恢复判定部107使用比上述阈值窄的值，则在恢复判定部107判定为满足了恢复条件之后，接近判定部105判定为臂部2的位置姿势接近约束目标，直接示教装置1不能从带约束直接示教脱离。

另外，在恢复判定部107在判定中不使用臂部2的位置姿势而仅使用时间的情况下，能够应用与实施方式1同样的方法。

另外，目标值算出部109除了在实施方式2中说明的目标姿势的算出之外，还进行使臂部2的位置向约束轴504移动时的目标位置的算出(步骤ST 1909)。例如，目标值算出部109将从末端执行器201的位置501如图20所示地向约束轴504引出垂线512的位置513作为目标位置。在此，如果目标值算出部109使臂部2的位置约束在与机器人坐标系的Z轴平行的约束轴504上，则目标位置为[x^*，y^*，z]。

另外，约束控制运算部110与实施方式2同样，使用雅可比矩阵J对臂部2的每个关节算出关节角速度θ(Dot)(步骤ST 1910)。但是，在实施方式3中的约束控制运算部110中，除了姿势约束之外还进行位置约束，因此运算步骤相对于实施方式2进行如下改变。

首先，约束控制运算部110决定旋转的角速度的大小|ω|。

接着，约束控制运算部110算出臂部2的当前的位置姿势下的雅可比矩阵J。

接着，约束控制运算部110根据作为旋转轴510的r和旋转速度的大小|ω|，算出与姿势有关的旋转矢量ω＝[ω_X，ω_Y，ω_Z]^T。到此为止的动作与实施方式2相同。

接着，约束控制运算部110决定使臂部2的位置向约束轴504移动的速度|v|。例如，如果没有受到限制，以1秒到达目标位置的方式进行控制，则约束控制运算部110设为|v|＝√{(x-x^*)²+(y-y^*)²}。另外，约束控制运算部110也可以决定某常数v^*，从而设为|v|＝v^*。

接着，约束控制运算部110算出与臂部2的位置有关的速度矢量v＝[v_X，v_Y，0]^T。在约束在与机器人坐标系的Z轴平行的约束轴504上的情况下，约束控制运算部110能够通过v＝|v|[x^*-x，y^*-y，0]/√{(x-x^*)²+(y-y^*)²}算出。

接着，约束控制运算部110针对臂部2的每个关节，通过下式(9)算出关节角速度θ(Dot)。另外，约束控制运算部110也可以对得到的关节角速度应用限制器。

其他动作与实施方式2相同。

另外，在实施方式3的直接示教装置1中，在省略了与姿势约束相关的运算的情况下，不进行与臂部2的姿势相关的约束，成为仅将臂部2的位置约束在指定的轴上的控制。

实施方式4

在实施方式2的直接示教装置1中，示出了约束方法仅限于姿势约束的情况。与此相对，在实施方式4的直接示教装置1中，表示约束方法仅限于面约束的情况。面约束是指同时进行将臂部2的位置约束于规定的约束面503的位置约束和将臂部2的姿势约束于规定的约束方向505的姿势约束。即，实施方式4的直接示教装置1除了实施方式2所示的姿势约束之外，还进行位置约束。以下，表示直接示教装置1进行如图21所示的Z轴坐标由X轴坐标和Y轴坐标的函数z＝f(x，y)表示的向曲面513的位置约束和向正下方向507的姿势约束的情况，但约束面503和约束方向505可以任意设定，例如约束面503由函数x＝f(y，z)或y＝f(z，x)表示的情况等也可以用同样的方法实施。

另外，实施方式4的直接示教装置1的构成例与实施方式2的直接示教装置1的构成例相同，以下使用图1所示的构成例进行说明。

另外，实施方式4中的位置姿势运算部104算出臂部2的位置以及姿势。

另外，实施方式4中的接近判定部105基于位置姿势运算部104的算出结果，判定臂部2的位置是否接近作为约束目标的约束面503，并且判定臂部2的姿势是否接近作为约束目标的约束方向505，由此判定臂部2的位置姿势是否接近约束目标。

图22是表示实施方式4的直接示教装置1的动作例的流程图。在图22所示的实施方式4的直接示教装置1中，动作与图19所示的实施方式3的直接示教装置1不同的是接近判定部105、解除操作判定部106、恢复判定部107、目标值算出部109以及约束控制运算部110，仅对这些构成的动作例进行说明。

首先，接近判定部105根据位置姿势运算部104的算出结果，分别判定臂部2的位置是否接近约束面503以及臂部2的姿势是否接近约束方向505，根据这些判定结果综合判定臂部2的位置姿势是否接近约束目标(步骤ST 2205)。例如，接近判定部105在判定为臂部2的位置接近约束面503且臂部2的姿势接近约束方向505的情况下，判定为臂部2的位置姿势接近约束目标。关于接近判定部105对约束方向505的判定，由于在实施方式2中进行了说明，因此省略，以下，对接近判定部105对约束面503的判定进行说明。

此时，接近判定部105从末端执行器201的位置501对约束面503划出与机器人坐标系的Z轴平行的直线，如果该直线的长度小于规定值，则判定为臂部2的位置接近约束面503。例如，在臂部2的位置的坐标为[x，y，z]的情况下，上述直线的长度为{z-f(x，y)}。并且，如果该长度小于规定值，则接近判定部105判定为臂部2的位置接近约束面503，在除此以外的情况下，接近判定部105判定为臂部2的位置没有接近约束面503。

接着，解除操作判定部106判定操作者是否进行了解除操作(步骤ST 2206)。这里，在解除操作判定部106进行使用外力的判定的情况下，可以应用与实施方式1同样的方法。另一方面，在解除操作判定部106进行使用了臂部2的位置姿势的判定的情况下，在实施方式4中，直接示教装置1进行臂部2的曲面约束，因此能够应用的判定方法限定为能够与曲面约束并存的判定方法。例如，实施方式4中的解除操作判定部106在图7中只能应用图7A、图7B所示的方法。另一方面，实施方式4中的解除操作判定部106不能应用图7C所示的方法，因为不能自由变更臂部2的姿势。另外，实施方式4中的解除操作判定部106不能应用图7D、7E所示的方法，因为臂部2不能从曲面513离开。

接着，恢复判定部107判定是否满足向能够向带约束直接示教切换的状态恢复的恢复条件(步骤ST 2207)。在实施方式4中，由于直接示教装置1进行臂部2的曲面约束，所以恢复判定部107在基于臂部2的位置姿势进行判定的情况下，能够基于臂部2的位置与约束面503之间的距离以及臂部2的姿势与约束方向505之间的关系进行判定。关于恢复判定部107对约束方向505的判定，在实施方式2中进行了说明，因此省略，以下，对恢复判定部107对约束面503的判定进行说明。

恢复判定部107在对约束面503的判定中，与接近判定部105同样，从末端执行器201的位置501对约束面503划出与机器人坐标系的Z轴平行的直线，根据该直线的长度进行判定。即，恢复判定部107在该长度大于规定值的情况下，判定为满足恢复条件。另外，上述规定的值需要与接近判定部105使用的规定的值相同或更大。其理由与实施方式3相同。

另外，恢复判定部107在判定中不使用臂部2的位置姿势而仅使用时间的情况下，能够应用与实施方式1同样的方法。

另外，目标值算出部109除了在实施方式2中说明的目标姿势的算出之外，还进行使臂部2的位置向约束面503移动时的目标位置的算出(步骤ST 2209)。例如，目标值算出部109在臂部2的位置的坐标为[x，y，z]的情况下，将仅将Z轴坐标置换为曲面上的坐标的[x，y，f(x，y)]作为目标位置。

另外，约束控制运算部110与实施方式2同样，使用雅可比矩阵J对臂部2的每个关节算出关节角速度θ(Dot)(步骤ST 2210)。但是，在实施方式4中的约束控制运算部110中，除了姿势约束之外还进行位置约束，因此运算步骤相对于实施方式2进行如下改变。

首先，约束控制运算部110决定旋转的角速度的大小|ω|。

接着，约束控制运算部110算出臂部2的当前的位置姿势下的雅可比矩阵J。

接着，约束控制运算部110根据作为旋转轴510的r和旋转速度的大小|ω|，算出与姿势有关的旋转矢量ω＝[ω_X，ω_Y，ω_Z]^T。到此为止的动作与实施方式2相同。

接着，约束控制运算部110决定使臂部2的位置向约束轴504移动的速度|v|。例如，如果没有受到限制，以1秒到达目标位置的方式进行控制，则约束控制运算部110设为|v|＝|z-f(x，y)|。另外，约束控制运算部110也可以将速度|v|设为某常数值。

接着，约束控制运算部110根据v＝[0，0，|v|]算出与臂部2的位置有关的速度矢量v＝[0，0，v_Z]^T。

接着，约束控制运算部110针对臂部2的每个关节，通过下式(10)算出关节角速度θ(Dot)。另外，约束控制运算部110也可以对得到的关节角速度应用限制器。

其他动作与实施方式2、3相同。

另外，在实施方式4的直接示教装置1中，在省略了与姿势约束相关的运算的情况下，不进行与臂部2的姿势相关的约束，成为仅将臂部2的位置约束在指定的面上的控制。另外，在实施方式4的直接示教装置1中，在将从臂部2的位置向约束面503做垂线的方向作为目标姿势的情况下，成为末端执行器201相对于约束面503始终垂直的姿势约束。

另外，本申请发明在其发明的范围内，可以进行各实施方式的自由组合、或各实施方式的任意构成要素的变形、或各实施方式中任意构成要素的省略。

产业上的可利用性

本发明的机器人的直接示教装置，能够在操作者操作臂部的状态下切换通常的直接示教和带约束直接示教，适用于进行机器人的直接示教的直接示教装置等。

符号说明

1 直接示教装置

2 臂部

101 位置姿势计测部

102 外力检测部

103 从动控制运算部

104 位置姿势运算部

105 接近判定部

106 解除操作判定部

107 恢复判定部

108 切换部

109 目标值算出部

110 约束控制运算部

111 合成部

112 驱动控制部

201 末端执行器。

Claims (9)

1.一种直接示教装置，其特征在于，具备：

外力检测部，其检测施加在机器人所具有的臂部上的外力；

从动控制运算部，其算出跟随由所述外力检测部检测到的外力的所述臂部的动作；

位置姿势运算部，其算出作为所述臂部的位置或姿势中的至少一方的位置姿势；

接近判定部，其基于所述位置姿势运算部的算出结果，判定所述臂部的位置姿势是否接近作为该臂部的位置姿势的约束目的地的约束目标；

解除操作判定部，其判定操作者是否进行了用于解除带约束直接示教的解除操作；

恢复判定部，其判定是否满足向以下状态恢复的恢复条件，该状态是能够向带约束直接示教切换的状态；

目标值算出部，其基于约束目标和所述位置姿势运算部的算出结果，算出带约束直接示教中的约束控制的目标值；

约束控制运算部，其基于所述位置姿势运算部的算出结果，算出移动到由所述目标值算出部算出的目标值的所述臂部的动作；

切换部，在本机处于第一状态的情况下，在由所述接近判定部判定为接近约束目标的情况下，所述切换部将本机切换到第二状态，在本机处于第二状态的情况下，在由所述解除操作判定部判定为操作者进行了解除操作的情况下，所述切换部将本机切换到第三状态，在本机处于第三状态的情况下，在由所述恢复判定部判定为满足恢复条件的情况下，所述切换部将本机切换到第一状态，在本机处于第二状态的情况下，使所述约束控制运算部的处理有效，在本机处于第一状态或第三状态的情况下，使所述约束控制运算部的处理无效；

合成部，其合成所述从动控制运算部的算出结果和所述约束控制运算部的算出结果；以及

驱动控制部，其基于所述合成部的合成结果来驱动所述臂部。

2.根据权利要求1所述的直接示教装置，其特征在于，

具备位置姿势计测部，其计测与所述臂部的位置姿势相关的参数，

所述从动控制运算部基于位置姿势计测部的计测结果，算出跟随由所述外力检测部检测到的外力的所述臂部的动作，

所述位置姿势运算部基于所述从动控制运算部的算出结果，算出所述臂部的位置姿势。

3.根据权利要求1所述的直接示教装置，其特征在于，

具备位置姿势计测部，其计测与所述臂部的位置姿势相关的参数，

所述位置姿势运算部基于所述位置姿势计测部的计测结果，算出所述臂部的位置姿势。

4.根据权利要求1所述的直接示教装置，其特征在于，

所述解除操作判定部基于所述外力检测部的检测结果或所述位置姿势运算部的算出结果，判定操作者是否进行了解除操作。

5.根据权利要求1所述的直接示教装置，其特征在于，

在操作者进行了解除操作后经过了一定时间的情况下，或者由所述位置姿势运算部算出的所述臂部的位置姿势没有接近约束目标的情况下，所述恢复判定部判定为满足了恢复条件。

6.根据权利要求1-5中的任一项所述的直接示教装置，其特征在于，

所述臂部是垂直多关节型机械臂，

所述位置姿势运算部算出所述臂部的姿势，

所述接近判定部基于所述位置姿势运算部的算出结果，判定所述臂部的姿势是否接近作为约束目标的约束方向。

7.根据权利要求1-5中的任一项所述的直接示教装置，其特征在于，

所述臂部是垂直多关节型机械臂，

所述位置姿势运算部算出所述臂部的位置及姿势，

所述接近判定部基于所述位置姿势运算部的算出结果，判定所述臂部的位置是否接近作为约束目标的约束轴，并且判定该臂部的姿势是否接近作为约束目标的约束方向，由此判定该臂部的位置姿势是否接近约束目标。

8.根据权利要求1-5中的任一项所述的直接示教装置，其特征在于，

所述臂部是垂直多关节型机械臂，

所述位置姿势运算部算出所述臂部的位置及姿势，

所述接近判定部基于所述位置姿势运算部的算出结果，判定所述臂部的位置是否接近作为约束目标的约束面，并且判定该臂部的姿势是否接近作为约束目标的约束方向，由此判定该臂部的位置姿势是否接近约束目标。

9.一种直接示教方法，其特征在于，具有以下步骤：

外力检测部检测施加在机器人所具有的臂部上的外力；

从动控制运算部算出跟随由所述外力检测部检测到的外力的所述臂部的动作；

位置姿势运算部算出作为所述臂部的位置或姿势中的至少一方的位置姿势；

接近判定部基于所述位置姿势运算部的算出结果，判定所述臂部的位置姿势是否接近作为该臂部的位置姿势的约束目的地的约束目标；

解除操作判定部判定操作者是否进行了用于解除带约束直接示教的解除操作；

恢复判定部判定是否满足向以下状态恢复的恢复条件，该状态是能够向带约束直接示教切换的状态；

目标值算出部基于约束目标和所述位置姿势运算部的算出结果，算出带约束直接示教中的约束控制的目标值；

约束控制运算部基于所述位置姿势运算部的算出结果，算出移动到由所述目标值算出部算出的目标值的所述臂部的动作；

在本机处于第一状态的情况下，在由所述接近判定部判定为接近约束目标的情况下，切换部将本机切换到第二状态，在本机处于第二状态的情况下，在由所述解除操作判定部判定为操作者进行了解除操作的情况下，所述切换部将本机切换到第三状态，在本机处于第三状态的情况下，在由所述恢复判定部判定为满足恢复条件的情况下，所述切换部将本机切换到第一状态，在本机处于第二状态的情况下，使所述约束控制运算部的处理有效，在本机处于第一状态或第三状态的情况下，使所述约束控制运算部的处理无效；

合成部合成所述从动控制运算部的算出结果和所述约束控制运算部的算出结果；以及

驱动控制部基于所述合成部的合成结果驱动所述臂部。

Images