[go: up one dir, main page]
More Web Proxy on the site http://driver.im/

CN112118940B - 机械手的直接教示装置以及直接教示方法 - Google Patents

机械手的直接教示装置以及直接教示方法 Download PDF

Info

Publication number
CN112118940B
CN112118940B CN201980032159.2A CN201980032159A CN112118940B CN 112118940 B CN112118940 B CN 112118940B CN 201980032159 A CN201980032159 A CN 201980032159A CN 112118940 B CN112118940 B CN 112118940B
Authority
CN
China
Prior art keywords
constraint
unit
arm
orientation
calculation unit
Prior art date
Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
Active
Application number
CN201980032159.2A
Other languages
English (en)
Other versions
CN112118940A (zh
Inventor
田原铁也
岩下纯久
野村琢磨
杉本广大
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Azbil Corp
Original Assignee
Azbil Corp
Priority date (The priority date is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the date listed.)
Filing date
Publication date
Application filed by Azbil Corp filed Critical Azbil Corp
Publication of CN112118940A publication Critical patent/CN112118940A/zh
Application granted granted Critical
Publication of CN112118940B publication Critical patent/CN112118940B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

Classifications

    • B—PERFORMING OPERATIONS; TRANSPORTING
    • B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
    • B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
    • B25J9/00—Programme-controlled manipulators
    • B25J9/16—Programme controls
    • B25J9/1628—Programme controls characterised by the control loop
    • B25J9/1653—Programme controls characterised by the control loop parameters identification, estimation, stiffness, accuracy, error analysis
    • B—PERFORMING OPERATIONS; TRANSPORTING
    • B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
    • B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
    • B25J9/00—Programme-controlled manipulators
    • B25J9/16—Programme controls
    • B25J9/1656—Programme controls characterised by programming, planning systems for manipulators
    • B25J9/1664—Programme controls characterised by programming, planning systems for manipulators characterised by motion, path, trajectory planning
    • G—PHYSICS
    • G05—CONTROLLING; REGULATING
    • G05B—CONTROL OR REGULATING SYSTEMS IN GENERAL; FUNCTIONAL ELEMENTS OF SUCH SYSTEMS; MONITORING OR TESTING ARRANGEMENTS FOR SUCH SYSTEMS OR ELEMENTS
    • G05B19/00—Programme-control systems
    • G05B19/02—Programme-control systems electric
    • G05B19/18—Numerical control [NC], i.e. automatically operating machines, in particular machine tools, e.g. in a manufacturing environment, so as to execute positioning, movement or co-ordinated operations by means of programme data in numerical form
    • G05B19/19—Numerical control [NC], i.e. automatically operating machines, in particular machine tools, e.g. in a manufacturing environment, so as to execute positioning, movement or co-ordinated operations by means of programme data in numerical form characterised by positioning or contouring control systems, e.g. to control position from one programmed point to another or to control movement along a programmed continuous path
    • G—PHYSICS
    • G05—CONTROLLING; REGULATING
    • G05B—CONTROL OR REGULATING SYSTEMS IN GENERAL; FUNCTIONAL ELEMENTS OF SUCH SYSTEMS; MONITORING OR TESTING ARRANGEMENTS FOR SUCH SYSTEMS OR ELEMENTS
    • G05B19/00—Programme-control systems
    • G05B19/02—Programme-control systems electric
    • G05B19/42—Recording and playback systems, i.e. in which the programme is recorded from a cycle of operations, e.g. the cycle of operations being manually controlled, after which this record is played back on the same machine

Landscapes

  • Engineering & Computer Science (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Automation & Control Theory (AREA)
  • Robotics (AREA)
  • Mechanical Engineering (AREA)
  • Human Computer Interaction (AREA)
  • Manufacturing & Machinery (AREA)
  • Numerical Control (AREA)
  • Manipulator (AREA)

Abstract

本发明涉及进行机械手的直接教示的直接教示装置以及直接教示方法。具备：外力检测部(102)，其检测外力；从动控制运算部(103)，其计算依照外力的臂部(2)的活动；位置姿势运算部(104)，其计算臂部(2)的位置姿势；接近判定部(105)，其判定臂部(2)的位置姿势是否接近约束目标；目标值计算部(107)，其计算约束控制的目标值；约束控制运算部(108)，其计算移动至目标值的臂部(2)的活动；约束控制限制部(109)，其限制约束控制运算部(108)的计算结果；切换部(106)，其在被判定为接近约束目标的情况下将约束控制运算部(108)设为有效，在被判定为未接近约束目标的情况下将约束控制运算部(108)设为无效；合成部(110)，其将从动控制运算部(103)的计算结果与约束控制限制部(109)的限制结果进行合成；以及驱动控制部(111)，其基于合成结果来驱动臂部(2)。

Description

机械手的直接教示装置以及直接教示方法
技术领域
本发明涉及进行机械手的直接教示的直接教示装置以及直接教示方法。
背景技术
在工业用的机械手中，为了使机械手进行作业，事先实施被称为教示(Teaching)的作业。作为进行该机械手的教示的方法之一，有被称为直接教示(Direct teaching)的方法。
例如在专利文献1中，公开了使用力传感器的机械手的直接教示方法。另外，例如在专利文献2中，公开了使用转矩检测单元的机械手的直接教示方法。图20示出在这些专利文献中公开的直接教示装置的概略构成。
在图20所示的直接教示装置11中，首先，外力检测部1101使用力传感器或者转矩传感器等，检测由操作者对机械手所具有的臂部2施加的外力。接着，从动控制运算部1102计算(从动控制运算)依照由外力检测部1101检测到的外力的臂部的活动。此外，从动控制运算部1102还存在在从动控制运算中使用与由位置姿势测量部(未图示)测量出的臂部的位置姿势有关的参数的情况。此外，臂部的位置姿势意味着臂部的位置以及臂部的姿势中的至少一方。另外，作为上述参数，可举出臂部的位置、臂部的姿势或者臂部的关节角等。然后，从动控制运算部1102将基于从动控制运算的结果而更新的从动控制指令值通知给驱动控制部1103。接着，驱动控制部1103依照由从动控制运算部1102通知的从动控制指令值而驱动臂部2。
通过该一连串的动作，图20所示的直接教示装置11能够以依照由操作者施加的外力使臂部2活动的方式进行控制。而且，在通过控制而臂部2的位置以及姿势成为操作者想要的状态的情况下，直接教示装置11作为教示点记录此时的上述参数。由该直接教示装置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的直接教示装置的构成例的图。
图2的图2A～图2D为表示由本发明的实施方式1的直接教示装置进行的通常的直接教示和附带约束的直接教示(位置约束的情况)的切换的一个例子的图。
图3的图3A～图3D为表示由本发明的实施方式1的直接教示装置进行的通常的直接教示和附带约束的直接教示(姿势约束的情况)的切换的一个例子的图。
图4为表示本发明的实施方式1的直接教示装置的动作例的流程图。
图5为对本发明的实施方式1中的接近判定部的动作例进行说明的图(姿势约束的情况)。
图6为对本发明的实施方式1中的目标值计算部的动作例进行说明的图(姿势约束的情况)。
图7为对本发明的实施方式1中的目标值计算部的动作例进行说明的图(姿势约束的情况)。
图8为表示本发明的实施方式2的直接教示装置的动作例的流程图。
图9的图9A、图9B为表示本发明的实施方式2中的从动控制运算部的动作例的图。
图10为表示本发明的实施方式2中的接近判定部的动作例的流程图。
图11的图11A、图11B、图11C为对本发明的实施方式2中的接近判定部的动作例进行说明的图。
图12的图12A、图12B为对本发明的实施方式2中的目标值计算部的动作例进行说明的图，为表示旋转轴的计算例的图。
图13的图13A、图13B为对本发明的实施方式2中的目标值计算部的动作例进行说明的图，为表示旋转方向的计算例的图。
图14为对本发明的实施方式2中的目标值计算部的动作例进行说明的图。
图15为对由本发明的实施方式3的直接教示装置进行的轴约束进行说明的图。
图16为表示本发明的实施方式3的直接教示装置的动作例的流程图。
图17为对本发明的实施方式3中的接近判定部的动作例进行说明的图。
图18为对由本发明的实施方式4的直接教示装置进行的面约束进行说明的图。
图19为表示本发明的实施方式4的直接教示装置的动作例的流程图。
图20为表示以往的直接教示装置的构成例的图。
具体实施方式
以下，参考附图，对本发明的实施方式详细地进行说明。
实施方式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中使用的约束目标不限于1个，也可以为多个。另外，针对机械手而设定作为基准的机械手坐标系，针对末端执行器201而设定工具坐标系(参考图6等)。
如图1所示，该直接教示装置1具备位置姿势测量部101、外力检测部102、从动控制运算部103、位置姿势运算部104、接近判定部105、切换部106、目标值计算部107、约束控制运算部108、约束控制限制部109、合成部110以及驱动控制部111。此外，从动控制运算部103、位置姿势运算部104、接近判定部105、切换部106、目标值计算部107、约束控制运算部108、约束控制限制部109、合成部110以及驱动控制部111由系统LSI(Large-ScaleIntegration，大规模集成电路)等处理电路、或者执行存储于存储器等的程序的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依照接近判定部105的判定结果，切换约束控制运算部108的动作。即，在由接近判定部105判定为臂部2的位置姿势接近约束目标的情况下，切换部106将由约束控制运算部108进行的处理设为有效。另一方面，在由接近判定部105判定为臂部2的位置姿势未接近约束目标的情况下，切换部106将由约束控制运算部108进行的处理设为无效。
另外，切换部106也可以在存在多个约束目标的情况下，依照接近判定部105的判定结果，实施由目标值计算部107进行的动作(作为对象的约束目标)的切换。
目标值计算部107基于约束目标以及位置姿势运算部104的计算结果，计算约束控制的目标值。此外，约束控制的目标值意味着臂部2的目标位置以及臂部2的目标姿势中的至少一方。
约束控制运算部108基于位置姿势运算部104的计算结果，计算移动至由目标值计算部107计算出的目标值的臂部2的活动(约束控制指令值)。
约束控制限制部109限制约束控制运算部108的计算结果。即，约束控制限制部109以使从动控制运算部103的计算结果比约束控制运算部108的计算结果优先的方式对约束控制运算部108的计算结果施加限制。
此外，在图1中，将约束控制运算部108和约束控制限制部109作为分开的构成要素而表示，但还能够作为一体而安装。
合成部110将从动控制运算部103的计算结果(从动控制指令值)和约束控制限制部109的限制结果(约束控制指令值)合成为一个指令值。
驱动控制部111基于合成部110的合成结果，驱动臂部2。
在实施方式1的直接教示装置1中，能够通过由操作者进行的臂部2的操作来执行通常的直接教示和附带约束的直接教示的切换。该切换方法通过以下所示的两个方法来实现。
第1个为在臂部2的位置姿势接近约束目标时、直接教示装置1自动地从通常的直接教示切换至附带约束的直接教示的方法。例如如图2A、2B所示，在想要使臂部2的位置约束于规定的平面506(约束面503)的情况下，直接教示装置1在从该平面506至臂部2的位置为止的距离例如为±50[mm]以下的情况下进行附带约束的直接教示，在除此以外的情况下进行通常的直接教示。另外，例如如图3A、3B所示，在想要使臂部2的姿势约束于正下方向507(约束方向505)的情况下，直接教示装置1在臂部2的姿势从正下方向507起例如±0.2[rad]以下的情况下进行附带约束的直接教示，在除此以外的情况下进行通常的直接教示。利用该方法，直接教示装置1仅凭由操作者进行的臂部2的操作，就能够实现从通常的直接教示向附带约束的直接教示的切换。
第2个为直接教示装置1强行抑制在附带约束的直接教示时约束的力、从而操作者能够克服约束控制而操作臂部2的方法。例如，直接教示装置1将约束控制中的关节角速度θ(Dot)例如抑制为0.1[rad/s]以下，操作者通过施加其以上的外力而能够操作臂部2。由此，操作者能够使臂部2从被约束的状态稍微脱离，最终能够从附带约束的直接教示的执行条件脱离。在普通的以往的直接教示装置中，在附带约束的直接教示中将臂部2强力地约束于约束面等，从而保持臂部2的位置姿势。相对于此，在实施方式1的直接教示装置1中，强行减弱约束的力，从而如图2C、2D、3C、3D所示，仅凭由操作者进行的臂部2的操作，就能够实现从附带约束的直接教示向通常的直接教示的切换。
将以上的两个方法进行组合，从而实施方式1的直接教示装置1能够在操作者操作臂部2的状态下，切换通常的直接教示和附带约束的直接教示。以下，参考图4，对图1所示的直接教示装置1的动作例进行说明。
在图1所示的直接教示装置1的动作例中，如图4所示，首先，位置姿势测量部101测量与臂部2的位置姿势有关的参数(步骤ST401)。
另外，外力检测部102检测由操作者对臂部2施加的外力(步骤ST402)。此外，由外力检测部102检测的外力根据传感器的构造等，有时不仅有由操作者对臂部2施加的外力，还重叠有重力所引起的分量。因而，在这样的情况下，外力检测部102计算重力所引起的分量，从检测到的外力减去该重力所引起的分量，从而仅计算外力分量即可。这为被称为重力补偿的公知技术，例如在专利文献5等中被公开。
专利文献5：日本特开平01-066715号公报
接着，从动控制运算部103基于位置姿势测量部101的测量结果，计算依照由外力检测部102检测到的外力的臂部2的活动(从动控制指令值)(步骤ST403)。此时，从动控制运算部103首先根据由外力检测部102检测到的外力的朝向以及大小，判断操作者将以何种方式使臂部2挪动。然后，从动控制运算部103基于其判断结果以及由位置姿势测量部101测量出的臂部2的位置姿势，计算臂部2的移动量(关节角θ等的差分)或者关节角速度θ(Dot)等。此外，从动控制运算部103的从动控制运算方法例如在专利文献2等中被公开，开发有各种方式，所以省略其详细的记述。
接着，位置姿势运算部104基于从动控制运算部103的计算结果，计算臂部2的位置姿势(步骤ST404)。
在此，在直接教示装置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的位置姿势是否接近约束目标(步骤ST405)。
例如，约束控制的目的在于将末端执行器201的顶端的Z轴坐标固定于0.1[m]的位置，另外，设定接近判定的阈值为0.05[m]。在该情况下，若由位置姿势运算部104计算出的末端执行器201的顶端的Z轴坐标为0.05～0.15[m]的范围内，则接近判定部105判定为臂部2的位置接近约束目标，在除此以外的情况下判定为臂部2的位置未接近约束目标。
另外，约束控制的目的在于使末端执行器201的朝向502(Z轴)朝向正下方，另外，设定接近判定的阈值为0.1[rad]。在该情况下，如图5所示，若正上方向(基准方向)508与末端执行器201的朝向502所成的角度θ为(π－0.1)[rad]以上、即正下方向507与末端执行器201的朝向502所成的角度小于0.1[rad]，则接近判定部105判定为臂部2的姿势接近约束目标，在除此以外的情况下判定为臂部2的姿势未接近约束目标。
另外，在如存在多个约束面503的情况等那样存在多个约束目标的情况下，接近判定部105针对每个约束目标而进行上述判定，只要在判定为臂部2的位置姿势接近各约束目标中的1个约束目标的情况下，臂部2的位置姿势都判定为接近约束目标。此时，进而，接近判定部105也可以评价臂部2与各约束目标之间的距离，与上述判定的结果一起将表示最接近臂部2的约束目标的信息通知给切换部106。
接着，切换部106依照接近判定部105的判定结果，切换约束控制运算部108的动作(步骤ST406)。即，在由接近判定部105判定为臂部2的位置姿势接近约束目标的情况下，切换部106将由约束控制运算部108进行的处理设为有效。另一方面，在由接近判定部105判定为臂部2的位置姿势未接近约束目标的情况下，切换部106以使由约束控制运算部108进行的运算停止或者使计算结果不被反映至臂部2的控制的方式，将由约束控制运算部108进行的处理设为无效。
这样，当由操作者使臂部2的位置姿势接近约束目标时，直接教示装置1自动地开始约束控制。另外，当由操作者使臂部2的位置姿势远离约束目标时，直接教示装置1利用与后述约束控制限制部109的组合来自动地结束约束控制。由此，直接教示装置1能够在由操作者保持臂部2的状态下切换约束控制的开启关断。
此外，切换部106在约束目标有多个，从接近判定部105通知表示最接近臂部2的约束目标的信息的情况下，切换部106也可以将该信息输出至后述目标值计算部107，切换该目标值计算部107的动作。
接着，目标值计算部107基于约束目标以及位置姿势运算部104的计算结果，计算约束控制的目标值(步骤ST407)。
例如，约束控制的目的在于将末端执行器201的顶端的Z轴坐标固定于0.1[m]的位置，设定由位置姿势运算部104计算出的末端执行器201的顶端的当前值为[XP，YP，ZP]。在该情况下，目标值计算部107将约束控制的目标值设为[XP，YP，0.1]。
另外，约束控制的目的在于如图6下层所示在将末端执行器201的朝向502中的Z轴设为正下方向507的基础上将X轴与机械手坐标系的X轴相匹配。在该情况下，通过旋转矩阵表达来表示作为约束控制的目标值的目标姿势的公式成为如下式(1)那样。此外，基于旋转矩阵的姿势的表达例如在非专利文献1等中被公开，省略其详细内容。
Figure BDA0002776031800000111
非专利文献1：John J.Craig(三浦·下山訳):ロボティクス，共立出版(1991)(John J.Craig(三浦·下山翻译)：机械手技术，共立出版(1991))
此外，目标值计算部107既可以仅计算1个目标值作为约束控制的目标值，也可以计算多个目标值并从其中选择1个目标值。例如，在约束控制的目的在于将末端执行器201的顶端的Z轴坐标以0.1[m]刻度固定于最接近的位置的情况下，目标值计算部107选择在[XP，YP，0.1n](n为整数)之中最接近当前的位置的值作为约束控制的目标值。
另外，在约束控制的目的在于如图7所示以将末端执行器201的朝向502中的Z轴设为正下方向507并且将该Z轴设为旋转轴而旋转至π/2刻度(90度刻度)的姿势进行固定的情况下，目标值计算部107选择最接近当前的姿势的值作为约束控制的目标值。此时，通过旋转矩阵表达来表示作为约束控制的目标值的目标姿势的公式成为如下式(2)那样。
Figure BDA0002776031800000121
关于该式(2)，如图7所示，根据n而目标姿势改变为4种，但目标值计算部107从其中选择1个最接近当前的姿势的目标姿势。目标值计算部107能够根据从当前的姿势旋转至各目标姿势的情况下的旋转角的大小等来判断是否接近当前的姿势。另外，在接近判定部105通知表示最接近臂部2的约束目标的信息的情况下，目标值计算部107也可以根据该信息选择约束控制的目标值。
接着，约束控制运算部108基于位置姿势运算部104的计算结果，计算移动至由目标值计算部107计算出的目标值的臂部2的活动(约束控制指令值)(步骤ST408)。此时，约束控制运算部108根据由目标值计算部107计算出的目标值，计算为了使臂部2从当前的位置姿势移动至目标值而所需的臂部2的移动量或者作为每单位时间的移动量的关节角速度θ(Dot)等。另外，约束控制运算部108也可以不对关节角速度θ(Dot)进行运算，而对每一个控制周期的关节角变化量Δθ进行运算。
接着，约束控制限制部109限制约束控制运算部108的计算结果(步骤ST409)。即，约束控制限制部109以使从动控制运算部103的计算结果比约束控制运算部108的计算结果优先的方式对约束控制运算部108的计算结果施加限制。
该约束控制限制部109的作用在于操作者从直接教示装置1进行约束控制的状态(附带约束的直接教示中的状态)切换至未进行约束控制的状态(通常的直接教示的状态)。例如，若直接教示装置1增强约束的力，使如从约束脱离那样的操作者的操作始终完全抵消，则一旦约束控制开始，就始终约束于约束目标，无法结束约束控制。因而，为了能够结束约束控制，利用约束控制限制部109来限制约束控制的强度，只要操作者施加强的外力，就能够从约束目标脱离。
例如，在直接教示装置1将臂部2的位置约束于规定的约束面503的情况下，约束控制限制部109将约束控制的速度例如限制为0.05[m/s]以下。由此，若操作者以比0.05[m/s]快的速度移动臂部2，则臂部2能够离开约束面503，直到最终接近判定部105判定为未接近约束面503。另一方面，若操作者以比0.05[m/s]慢的速度移动臂部2，则臂部2继续成为约束于约束面503的状态。
另外，在直接教示装置1将臂部2的位置约束于规定的约束面503的情况下，约束控制限制部109将约束控制的关节角速度θ(Dot)例如限制为0.1[rad/s]以下。由此，若操作者以比0.1[rad/s]快的速度移动臂部2，则臂部2能够离开约束面503，直到最终接近判定部105判定为未接近约束面503。另一方面，若操作者以比0.1[rad/s]慢的速度移动臂部2，则臂部2继续成为约束于约束面503的状态。
除了上述之外，例如，对于直接教示装置1使臂部2的姿势朝向正下方向507的情况，也是约束控制限制部109对使姿势变化的速度或者关节角速度θ(Dot)施加限制，从而操作者能够进行解除约束状态的操作。
此外，非常优选的是，在约束控制限制部109限制约束控制的强度时，在任意的情况下，都通过约束控制来维持使臂部2挪动的朝向。
例如，在约束控制限制部109限制关节角速度θ(Dot)的情况下，在某个关节角速度θ(Dot)不是0.5倍而超过限制的情况下，应使所有的关节角速度θ(Dot)都成为0.5倍，不应仅修正超过限制的关节角速度θ(Dot)。在约束控制限制部109不对所有的关节角速度θ(Dot)同样地进行限制的情况下，将会从约束控制运算部108要挪动的朝向发生改变，约束控制的精度劣化。
接着，合成部110将从从动控制运算部103输出的从动控制指令值和从约束控制限制部109输出的约束控制指令值合成为一个指令值(步骤ST410)。
例如，在从动控制运算部103作为从动控制指令值而输出关节角θD，约束控制限制部109作为约束控制指令值而输出关节角变化量ΔθC的情况下，合成部110作为关节角指令值而输出θD+ΔθC。
另外，例如，在从动控制运算部103作为从动控制指令值而输出位置坐标[xD，yD，zD]以及姿势TD(旋转矩阵表达)，约束控制限制部109作为约束控制指令值而输出位置变化量[ΔxC，ΔyC，ΔzC]以及姿势变化量ΔTC(旋转矩阵表达)的情况下，合成部110作为位置指令值而输出[xD+ΔxC，yD+ΔyC，zD+ΔzC]，作为姿势指令值而输出ΔTCTD(旋转矩阵表达)(旋转矩阵表达的姿势的合成为矩阵之积)。
另外，例如，在从动控制运算部103作为从动控制指令值而输出转矩τD，约束控制限制部109作为约束控制指令值而输出转矩变化量ΔτC的情况下，合成部110作为转矩指令值而输出τD+ΔτC。
接着，驱动控制部111利用合成部110合成结果，驱动臂部2(步骤ST411)。由该驱动控制部111进行的臂部2的驱动为通常的臂部2的控制，所以省略其详细内容。
如以上那样，根据该实施方式1，直接教示装置1具备：外力检测部102，其检测被施加至机械手所具有的臂部2的外力；从动控制运算部103，其计算依照由外力检测部102检测到的外力的臂部2的活动；位置姿势运算部104，其计算作为臂部2的位置或者姿势中的至少一方的位置姿势；接近判定部105，其基于位置姿势运算部104计算结果，判定臂部2的位置姿势是否接近作为该臂部2的位置姿势的约束目的地的约束目标；目标值计算部107，其基于约束目标以及位置姿势运算部104的计算结果，计算约束控制的目标值；约束控制运算部108，其基于位置姿势运算部104的计算结果，计算移动至由目标值计算部107计算出的目标值的臂部2的活动；约束控制限制部109，其限制约束控制运算部108的计算结果；切换部106，其在由接近判定部105判定为接近约束目标的情况下将约束控制运算部108的计算结果设为有效，在由接近判定部105判定为未接近约束目标的情况下将约束控制运算部108的计算结果设为无效；合成部110，其将从动控制运算部103的计算结果与约束控制限制部109的限制结果进行合成；以及驱动控制部111，其基于合成部110的合成结果来驱动臂部2。由此，实施方式1的直接教示装置1能够在操作者操作臂部2的状态下切换通常的直接教示和附带约束的直接教示。
实施方式2.
在实施方式2中，表示臂部2为垂直多关节型机械手臂部，基于直接教示装置1的约束方法仅限定于姿势约束的情况。此外，实施方式2的直接教示装置1的构成例与实施方式1的直接教示装置1的构成例相同，以下，使用图1所示的构成例来进行说明。
此外，实施方式2中的位置姿势运算部104计算臂部2的姿势。
另外，实施方式2中的接近判定部105基于位置姿势运算部104的计算结果，判定臂部2的姿势是否接近约束方向505。
接下来，参考图8，对实施方式2的直接教示装置1的动作例进行说明。
以下，直接教示装置1进行如将末端执行器201的朝向502(Z轴)约束于正下方向507或者正横向509那样的姿势约束。另外，用eX、eY、eZ表示机械手坐标系的各轴的单位矢量。另外，臂部2设为6轴。另外，切换部106切换目标值计算部107以及约束控制运算部108的动作。另外，从动控制运算部103作为从动控制指令值而计算臂部2的各关节角θ，约束控制运算部108作为约束控制指令值而计算臂部2的各关节角速度θ(Dot)。
在实施方式2的直接教示装置1的动作例中，如图8所示，首先，位置姿势测量部101测量臂部2的各关节角θ(步骤ST801)。例如，位置姿势测量部101使用安装于臂部2所具有的马达的编码器等，针对臂部2的每个关节而测量关节角θ。
接着，外力检测部102检测由操作者对臂部2施加的外力(步骤ST802)。由该外力检测部102进行的处理与实施方式1相同。此外，外力检测部102进行重力补偿。
接着，从动控制运算部103基于位置姿势测量部101的测量结果，计算依照由外力检测部102检测到的外力的臂部2的各关节角θ(从动控制指令值)(步骤ST803)。参考图9，对该从动控制运算部103的动作例进行说明。
从动控制运算部103例如能够利用图9A所示的方法，根据由外力检测部102检测到的转矩τ来计算臂部2的各关节角θ。
在图9A中，从动控制运算部103首先将增益KT与由外力检测部102检测到的转矩τ相乘，从而针对臂部2的每个关节而计算作为目标的关节角速度θ(Dot)。此外，增益KT针对臂部2的每个关节既可以为相同也可以为不同。此外，如图9A所示，从动控制运算部103也可以将限制器应用于作为目标的关节角速度θ(Dot)，限制从动控制的速度。
接下来，从动控制运算部103将控制周期TS与作为目标的关节角速度θ(Dot)相乘，从而针对臂部2的每个关节而计算每一个控制周期的关节角变化量Δθ。
接下来，从动控制运算部103针对臂部2的每个关节而对当前时刻k的关节角θ(k)加上关节角变化量Δθ，计算下一时刻k+1的关节角θ(k+1)。
另外，从动控制运算部103例如能够利用图9B所示的方法，根据由外力检测部102检测到的力F来对臂部2的各关节角θ进行运算。
在图9B的例子中，从动控制运算部103首先将增益KF与由外力检测部102检测到的力F相乘，从而针对臂部2的每个位置以及每个姿势而计算移动速度。此外，增益KF针对臂部2的位置以及姿势的各分量既可以为相同也可以为不同。
接下来，从动控制运算部103将雅可比矩阵J的逆矩阵与移动速度相乘，从而针对臂部2的每个关节而计算作为目标的关节角速度θ(Dot)。此外，从动控制运算部103在不存在雅可比矩阵J的逆矩阵的情况下乘以伪逆矩阵。此外，如图9B所示，从动控制运算部103也可以将限制器应用于作为目标的关节角速度θ(Dot)，限制从动控制的速度。
接下来，从动控制运算部103将控制周期TS与作为目标的关节角速度θ(Dot)相乘，从而针对臂部2的每个关节而计算每一个控制周期的关节角变化量Δθ。
接下来，从动控制运算部103针对臂部2的每个关节而对当前时刻k的关节角θ(k)加上关节角变化量Δθ，计算下一时刻k+1的关节角θ(k+1)。
接着，位置姿势运算部104基于从动控制运算部103的计算结果，计算臂部2的姿势(步骤ST804)。位置姿势运算部104能够通过正向运动学来执行上述计算。以下，通过旋转矩阵而用T表示由位置姿势运算部104计算的臂部2的姿势。另外，如下式(3)所示，将T分解为列矢量、要素的公式。
Figure BDA0002776031800000161
接着，接近判定部105基于位置姿势运算部104的计算结果，判定臂部2的姿势是否接近约束方向505(步骤ST805)。在此，约束方向505为正下方向507或者正横向509。另外，接近判定的阈值为0.1[rad]。参考图10、11，对接近判定部105的动作例进行说明。
在接近判定部105的动作例中，如图10所示，接近判定部105首先计算正上方向(基准方向)508与末端执行器201的朝向502(Z轴)所成的角度θ(步骤ST1001)。即，接近判定部105如下式(4)那样，根据eZ与tZ的内积来计算角度θ。
Θ＝cos-1(tz·eZ)＝cos-1(lZz) (4)
然后，接近判定部105判定角度θ是否为(π/2－0.1)[rad]至(π/2+0.1)[rad]的范围内(步骤ST1002)。
在该步骤ST1002中，接近判定部105在判定为角度θ在(π/2－0.1)[rad]至(π/2+0.1)[rad]的范围内的情况(例如图11A的情况)下，判定为臂部2的姿势接近正横向509(步骤ST1003)。
另一方面，在步骤ST1002中，接近判定部105在判定为角度θ不在(π/2－0.1)[rad]至(π/2+0.1)[rad]的范围内的情况下，判定角度θ是否在(π－0.1)[rad]以上(步骤ST1004)。
在该步骤ST1004中，接近判定部105在判定为角度θ在(π－0.1)[rad]以上的情况(例如图11B的情况)下，判定为臂部2的姿势接近正下方向507(步骤ST1005)。
另一方面，在步骤ST1004中，接近判定部105在判定为角度θ不在(π－0.1)[rad]以上的情况(例如图11C的情况)下，判定为臂部2的姿势与哪个约束方向505都未接近(步骤ST1006)。
接着，切换部106依照接近判定部105的判定结果，切换目标值计算部107以及约束控制运算部108的动作(步骤ST806)。
此时，在由接近判定部105判定为臂部2的姿势接近正横向509的情况下，切换部106使目标值计算部107计算向正横向509的目标姿势作为目标值，将由约束控制运算部108进行的处理设为有效。
另外，在由接近判定部105判定为臂部2的姿势接近正下方向507的情况下，切换部106使目标值计算部107计算向正下方向507的目标姿势作为目标值，将由约束控制运算部108进行的处理设为有效。
另外，在由接近判定部105判定为臂部2的姿势与哪个约束方向505都未接近的情况下，切换部106不需要目标值计算部107的计算，将由约束控制运算部108进行的处理设为无效。
接着，目标值计算部107基于约束目标以及位置姿势运算部104的计算结果，计算目标姿势(步骤ST807)。例如，目标值计算部107计算为正横向509或者正下方向507、并且从臂部2的当前的姿势以最小的旋转角度θR能够到达的目标姿势。以下表示基于目标值计算部107的目标姿势的计算方法。
首先，目标值计算部107针对每个作为对象的目标姿势，计算与eZ相对的目标角度θ*。在此，在目标姿势为正横向509的情况下，目标角度θ*为π/2，在目标姿势为正下方向507的情况下，目标角度θ*为π。
接下来，目标值计算部107基于臂部2的姿势，计算旋转角度θR最小的旋转轴510。如图12所示，该旋转轴510为处于机械手坐标系的XY平面上并且与tZ正交的轴，即为与eZ以及tZ这两方正交的轴。因而，目标值计算部107求出1个如与eZ以及tZ这两方的内积成为0那样的单位矢量即r，将其设为旋转轴510。此外，在eZ与tZ平行的情况下，目标值计算部107无法唯一地求出r。然而，其为臂部2的姿势朝向正上方向508或者正下方向507的情况，目标值计算部107计算XY平面上的任意的矢量作为旋转轴510。
接下来，目标值计算部107基于计算出的目标角度θ*以及旋转轴510，计算旋转角度θR作为目标姿势。在此，在末端执行器201的朝向502从正上方向(基准方向)508起为角度θ的情况下，旋转角度θR的大小为θ*－θ，但需要如图13所示考虑旋转的朝向(正负)。这是因为在实施方式2的直接教示装置1中，在求出角度θ时使用了内积，所以丧失末端执行器201的朝向502的信息。若考虑旋转的朝向，则旋转角度θR成为如下式(5)那样。即，旋转的朝向在eZ×tZ与r为相同的朝向的情况下不需要符号的相反，在eZ×tZ与r为相反的朝向的情况下，需要符号的相反。
Figure BDA0002776031800000181
在目标值计算部107中，若能够得到表示旋转轴510的r＝[rx，ry，rz]T(T表示转置)和旋转角度θR，则会得到从臂部2的姿势向目标姿势的旋转，所以事实上得到约束目标。在需要目标姿势的具体的表达的情况下，目标值计算部107能够通过将前述旋转应用于臂部2的姿势而得到。
在上述中，对目标值计算部107计算如末端执行器201的朝向502(Z轴)朝向正下方向507或者正横向509那样的目标姿势的方法进行了叙述。目标值计算部107还能够从此处进一步加以旋转而成为所期望的姿势，将其设为目标姿势。
例如，目标值计算部107从臂部2朝向正下方向507的状态绕Z轴旋转，从而还能够将如图7所示的姿势设为目标姿势。另外，目标值计算部107还能够如图14所示从臂部2朝向正横向509的状态进一步绕Z轴旋转，将tX或者tY与机械手坐标系的Z轴平行的姿势设为目标姿势。
接着，约束控制运算部108基于位置姿势运算部104的计算结果，计算移动至由目标值计算部107计算出的目标姿势的臂部2的活动(约束控制指令值)(步骤ST808)。此外，目标值计算部107在计算目标姿势时已经计算出如使臂部2从当前的姿势挪动至目标姿势那样的旋转，所以约束控制运算部108以如上方式计算使臂部2旋转的活动。只是，约束控制运算部108的输出需要为关节角速度θ(Dot)，所以约束控制运算部108需要根据由目标值计算部107计算出的旋转轴510即r与旋转角度θR的组来计算关节角速度θ(Dot)。以下，表示该计算的一个例子。
约束控制运算部108首先决定旋转的角速度的大小|ω|。例如，若以若不受到限制则1秒到达至目标姿势的方式进行控制，则设为|ω|＝θR。
接下来，约束控制运算部108计算臂部2的当前的位置姿势下的雅可比矩阵J。关于雅可比矩阵J，下式(6)成立。
Figure BDA0002776031800000191
在此，vX、vY、vZ为末端执行器201的顶端的X、Y、Z轴方向的速度，ωX、ωY、ωZ为臂部2的姿势相对X轴、Y轴、Z轴而旋转的角速度，θ1(Dot)～θ6(Dot)为各关节角速度。
接下来，约束控制运算部108根据旋转轴510即r和转速的大小|ω|来计算与姿势有关的旋转矢量ω＝[ωX，ωY，ωZ]T。
接下来，约束控制运算部108计算如满足下式(7)那样的臂部2的每个关节的关节角速度θ1(Dot)～θ6(Dot)。此外，当在雅可比矩阵J中不存在逆矩阵的情况下，约束控制运算部108使用伪逆矩阵等来计算关节角速度θ1(Dot)～θ6(Dot)。
Figure BDA0002776031800000201
接着，约束控制限制部109限制约束控制运算部108的计算结果(步骤ST809)。
首先，约束控制限制部109决定针对由约束控制运算部108计算出的臂部2的每个关节的关节角速度θ(Dot)的上限θMAX(Dot)。上限θMAX(Dot)针对每个关节既可以为相同也可以为不同。另外，当在从动控制运算部103中应用了限制器的情况下，约束控制限制部109需要使上限θMAX(Dot)比该限制器的上限小。若不使上限θMAX(Dot)比限制器的上限小，则由操作者进行的操作与约束控制相比更加被限制，无法从附带约束的直接教示脱离。实际上，约束控制限制部109优选使上限θMAX(Dot)与限制器的上限相比成为一半以下。若限制至该程度，则比较容易通过由操作者进行的操作来脱离约束。
接下来，约束控制限制部109以使所有的关节角速度θ(Dot)都不超过上限θMAX(Dot)的方式进行限制。此时，优选以不改变旋转的方向的方式进行限制。以下表示进行这样的运算的次序的一个例子。此外，用θi，MAX(Dot)表示第i个关节角速度θi(Dot)的上限。
约束控制限制部109首先针对臂部2的每个关节，通过|θi(Dot)|/θi，MAX(Dot)来计算关节角速度θi(Dot)的大小与上限θi，MAX(Dot)之比ci。
接下来，约束控制限制部109计算该比ci的最大值c。
接下来，约束控制限制部109针对臂部2的每个关节，将关节角速度θi(Dot)除以最大值c，计算限制后的关节角速度θi*(Dot)。即，限制后的关节角速度θi*(Dot)为θi(Dot)/c。
接着，合成部110将从从动控制运算部103输出的从动控制指令值(关节角θ)与从约束控制限制部109输出的约束控制指令值(关节角速度θ(Dot))进行合成(步骤ST810)。
例如，在从动控制运算部103输出关节角θ＝[θ1，…，θm]T，约束控制限制部109输出关节角速度θ(Dot)＝[θ1(Dot)，…，θm(Dot)]T的情况下，根据控制周期TS，作为合成部110的输出的关节角指令值成为θ+TSθ(Dot)。其中，m为驱动控制部111控制的臂部2的关节数。
接着，驱动控制部111基于合成部110的合成结果(关节角指令值)，驱动臂部2(步骤ST811)。
此外，在上述中一部分以6轴(关节数为6个)的臂部2为前提而进行了说明以及例示，但即使为除了6轴以外的臂部2，也能够同样地实施。
实施方式3.
在实施方式2的直接教示装置1中，表示约束方法仅限定于姿势约束的情况。相对于此，在实施方式3的直接教示装置1中，表示约束方法仅限定于轴约束的情况。轴约束意味着同时进行将臂部2的位置约束于规定的约束轴504的位置约束和将臂部2的姿势约束于规定的约束方向505的姿势约束。即，实施方式3的直接教示装置1除了进行实施方式2所示的姿势约束之外，还进行位置约束。以下，表示直接教示装置1进行如图15所示的向与机械手坐标系的Z轴平行的直线511的位置约束以及向正下方向507的姿势约束的情况，约束轴504以及约束方向505能够任意地设定。
此外，实施方式3的直接教示装置1的构成例与实施方式2的直接教示装置1的构成例相同，以下，使用图1所示的构成例来进行说明。
此外，实施方式3中的位置姿势运算部104计算臂部2的位置以及姿势。
另外，实施方式3中的接近判定部105基于位置姿势运算部104的计算结果，判定臂部2的位置是否接近作为约束目标的约束轴504并且判定臂部2的姿势是否接近作为约束目标的约束方向505，从而判定臂部2的位置姿势是否接近约束目标。
图16为表示实施方式3的直接教示装置1的动作例的流程图。在图16所示的实施方式3的直接教示装置1中，动作与图8所示的实施方式2的直接教示装置1不同的是位置姿势运算部104、接近判定部105、目标值计算部107以及约束控制运算部108，仅对这些构成的动作例进行说明。
首先，位置姿势运算部104基于从动控制运算部103的计算结果，计算臂部2的位置以及姿势(步骤ST1604)。在实施方式3中，为了对臂部2的位置以及姿势这两方进行约束，位置姿势运算部104需要计算臂部2的位置以及姿势这两方。位置姿势运算部104能够通过正向运动学来执行上述计算。
另外，接近判定部105基于位置姿势运算部104的计算结果，分别判定臂部2的位置是否接近约束轴504以及臂部2的姿势是否接近约束方向505，根据它们的判定结果来综合地判定臂部2的位置姿势是否接近约束目标(步骤ST1605)。例如，接近判定部105在判定为臂部2的位置接近约束轴504并且臂部2的姿势接近约束方向505的情况下，判定为臂部2的位置姿势接近约束目标。关于由接近判定部105进行的针对约束方向505的判定，在实施方式2中进行了说明，所以省略，以下，对由接近判定部105进行的针对约束轴504的判定进行说明。
此时，接近判定部105例如如图17所示从末端执行器201的位置501相对于约束轴504做垂线512，若该垂线512的长度比规定的值小，则判定为臂部2的位置接近约束轴504。例如将臂部2的位置的坐标用[x，y，z]表示，将直线511的X坐标和Y坐标分别用x*，y*表示。在该情况下，垂线512的长度成为如下式(8)那样。
Figure BDA0002776031800000221
然后，若式(8)所示的垂线512的长度比规定的值小，则接近判定部105判定为臂部2的位置接近约束轴504，在除此以外的情况下，判定为臂部2的位置未接近约束轴504。此外，即使约束轴504与机械手坐标系的Z轴平行的情况以外，接近判定部105也能够同样地根据相对于约束轴504做的垂线的长度来进行接近判定。
另外，目标值计算部107除了进行在实施方式2中说明的目标姿势的计算之外，还进行使臂部2的位置移动至约束轴504的情况下的目标位置的计算(步骤ST1607)。例如，目标值计算部107将从末端执行器201的位置501如图17所示向约束轴504做垂线512的位置513设为目标位置。在此，目标值计算部107若使臂部2的位置约束于与机械手坐标系的Z轴平行的约束轴504，则目标位置成为[x*，y*，z]。
另外，约束控制运算部108与实施方式2同样地，使用雅可比矩阵J，针对臂部2的每个关节而计算关节角速度θ(Dot)(步骤ST1608)。但是，在实施方式3中的约束控制运算部108中，除了姿势约束之外，还进行位置约束，所以相对于实施方式2，运算次序以如下方式改变。
首先，约束控制运算部108决定旋转的角速度的大小|ω|。
接下来，约束控制运算部108计算臂部2的当前的位置姿势下的雅可比矩阵J。
接下来，约束控制运算部108根据旋转轴510即r和转速的大小|ω|，计算与姿势有关的旋转矢量ω＝[ωX，ωY，ωZ]T。至此为止的动作与实施方式2相同。
接下来，约束控制运算部108决定使臂部2的位置移动至约束轴504的速度|v|。例如，若以不受到限制则1秒到达目标位置的方式进行控制，则约束控制运算部108设为|
Figure BDA0002776031800000231
另外，约束控制运算部108也可以决定某个常数v*，设为|v|＝v*。
接下来，约束控制运算部108计算与臂部2的位置有关的速度矢量v＝[vX，vY，0]T。约束控制运算部108在约束于与机械手坐标系的Z轴平行的约束轴504的情况下，能够根据
Figure BDA0002776031800000232
来计算。
接下来，约束控制运算部108针对臂部2的每个关节而根据下式(9)来计算关节角速度θ(Dot)。
Figure BDA0002776031800000233
其它动作与实施方式2相同。
此外，在实施方式3的直接教示装置1中，在省略与姿势约束有关的运算的情况下，不进行与臂部2的姿势有关的约束，而以约束于仅指定了臂部2的位置的轴的方式进行控制。
实施方式4.
在实施方式2的直接教示装置1中，表示约束方法仅限定于姿势约束的情况。相对于此，在实施方式4的直接教示装置1中，表示约束方法仅限定于面约束的情况。面约束意味着同时进行将臂部2的位置约束于规定的约束面503的位置约束和将臂部2的姿势约束于规定的约束方向505的姿势约束。即，实施方式4的直接教示装置1除了进行实施方式2所示的姿势约束之外，还进行位置约束。以下，表示直接教示装置1进行如图18所示的向用X轴坐标以及Y轴坐标的函数z＝f(x，y)表示Z轴坐标的曲面513的位置约束以及向正下方向507的姿势约束的情况，约束面503以及约束方向505能够任意地设定，例如关于用函数x＝f(y，z)或者y＝f(z，x)表示约束面503的情况等，也能够通过同样的方法来实施。
此外，实施方式4的直接教示装置1的构成例与实施方式2的直接教示装置1的构成例相同，以下，使用图1所示的构成例来进行说明。
此外，实施方式4中的位置姿势运算部104计算臂部2的位置以及姿势。
另外，实施方式4中的接近判定部105基于位置姿势运算部104的计算结果，判定臂部2的位置是否接近作为约束目标的约束面503并且判定臂部2的姿势是否接近作为约束目标的约束方向505，从而判定臂部2的位置姿势是否接近约束目标。
图19为表示实施方式4的直接教示装置1的动作例的流程图。在图19所示的实施方式4的直接教示装置1中，动作与图16所示的实施方式3的直接教示装置1不同的是接近判定部105、目标值计算部107以及约束控制运算部108，仅对这些构成的动作例进行说明。
首先，接近判定部105基于位置姿势运算部104的计算结果，分别判定臂部2的位置是否接近约束面503以及臂部2的姿势是否接近约束方向505，根据它们的判定结果来综合地判定臂部2的位置姿势是否接近约束目标(步骤ST1905)。例如，接近判定部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，在除此以外的情况下判定为臂部2的位置未接近约束面503。
另外，目标值计算部107除了进行在实施方式2中说明的目标姿势的计算之外，还进行使臂部2的位置移动至约束面503的情况下的目标位置的计算(步骤ST1907)。例如，在臂部2的位置的坐标为[x，y，z]的情况下，目标值计算部107将仅将Z轴坐标置换为曲面上的坐标而得到的[x，y，f(x，y)]设为目标位置。
另外，约束控制运算部108与实施方式2同样地，使用雅可比矩阵J，针对臂部2的每个关节而计算关节角速度θ(Dot)(步骤ST1908)。但是，在实施方式4中的约束控制运算部108中，除了进行姿势约束之外，还进行位置约束，所以相对于实施方式2，运算次序以如下方式改变。
首先，约束控制运算部108决定旋转的角速度的大小|ω|。
接下来，约束控制运算部108计算臂部2的当前的位置姿势下的雅可比矩阵J。
接下来，约束控制运算部108根据旋转轴510即r和转速的大小|ω|来计算与姿势有关的旋转矢量ω＝[ωX，ωY，ωZ]T。至此为止的动作与实施方式2相同。
接下来，约束控制运算部108决定使臂部2的位置移动至约束轴504的速度|v|。例如，若以不受到限制则1秒到达目标位置的方式进行控制，则约束控制运算部108设为|v|＝|z－f(x，y)|。另外，约束控制运算部108也可以将速度|v|设为某个常数值。
接下来，约束控制运算部108根据v＝[0，0，|v|]来计算与臂部2的位置有关的速度矢量v＝[0，0，vZ]T。
接下来，约束控制运算部108针对臂部2的每个关节，根据下式(10)来计算关节角速度θ(Dot)。
Figure BDA0002776031800000261
其它动作与实施方式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 驱动控制部
201 末端执行器。

Claims (7)

1.一种直接教示装置，其特征在于，具备：
外力检测部，其检测被施加至机械手所具有的臂部的外力；
从动控制运算部，其计算依照由所述外力检测部检测到的外力的所述臂部的活动；
位置姿势运算部，其计算作为所述臂部的位置或者姿势中的至少一方的位置姿势；
接近判定部，其基于所述位置姿势运算部的计算结果，判定所述臂部的位置姿势是否接近作为该臂部的位置姿势的约束目的地的约束目标；
目标值计算部，其基于约束目标以及所述位置姿势运算部的计算结果，计算约束控制的目标值；
约束控制运算部，其基于所述位置姿势运算部的计算结果，计算移动至由所述目标值计算部计算出的目标值的所述臂部的活动；
约束控制限制部，其限制所述约束控制运算部的计算结果；
切换部，其在由所述接近判定部判定为接近约束目标的情况下将所述约束控制运算部的计算结果设为有效，在由所述接近判定部判定为未接近约束目标的情况下将所述约束控制运算部的计算结果设为无效；
合成部，其将所述从动控制运算部的计算结果与所述约束控制限制部的限制结果进行合成；以及
驱动控制部，其基于所述合成部的合成结果来驱动所述臂部。
2.根据权利要求1所述的直接教示装置，其特征在于，
所述直接教示装置具备位置姿势测量部，其测量与所述臂部的位置姿势有关的参数，
所述从动控制运算部基于所述位置姿势测量部的测量结果，计算依照由所述外力检测部检测到的外力的所述臂部的活动，
所述位置姿势运算部基于所述从动控制运算部的计算结果，计算所述臂部的位置姿势。
3.根据权利要求1所述的直接教示装置，其特征在于，
所述直接教示装置具备位置姿势测量部，其测量与所述臂部的位置姿势有关的参数，
所述位置姿势运算部基于所述位置姿势测量部的测量结果，计算所述臂部的位置姿势。
4.根据权利要求1至3中的任意一项所述的直接教示装置，其特征在于，
所述臂部为垂直多关节型机械手臂部，
所述位置姿势运算部计算所述臂部的姿势，
所述接近判定部基于所述位置姿势运算部的计算结果，判定所述臂部的姿势是否接近作为约束目标的约束方向。
5.根据权利要求1至3中的任意一项所述的直接教示装置，其特征在于，
所述臂部为垂直多关节型机械手臂部，
所述位置姿势运算部计算所述臂部的位置以及姿势，
所述接近判定部基于所述位置姿势运算部的计算结果，判定所述臂部的位置是否接近作为约束目标的约束轴，并且判定该臂部的姿势是否接近作为约束目标的约束方向，从而判定该臂部的位置姿势是否接近约束目标。
6.根据权利要求1至3中的任意一项所述的直接教示装置，其特征在于，
所述臂部为垂直多关节型机械手臂部，
所述位置姿势运算部计算所述臂部的位置以及姿势，
所述接近判定部基于所述位置姿势运算部的计算结果，判定所述臂部的位置是否接近作为约束目标的约束面，并且判定该臂部的姿势是否接近作为约束目标的约束方向，从而判定该臂部的位置姿势是否接近约束目标。
7.一种直接教示方法，其特征在于，具有：
外力检测部检测被施加至机械手所具有的臂部的外力的步骤；
从动控制运算部计算依照由所述外力检测部检测到的外力的所述臂部的活动的步骤；
位置姿势运算部计算作为所述臂部的位置或者姿势中的至少一方的位置姿势的步骤；
接近判定部基于所述位置姿势运算部的计算结果，判定所述臂部的位置姿势是否接近作为该臂部的位置姿势的约束目的地的约束目标的步骤；
目标值计算部基于约束目标以及所述位置姿势运算部的计算结果，计算约束控制的目标值的步骤；
约束控制运算部基于所述位置姿势运算部的计算结果，计算移动至由所述目标值计算部计算出的目标值的所述臂部的活动的步骤；
约束控制限制部限制所述约束控制运算部的计算结果的步骤；
切换部在由所述接近判定部判定为接近约束目标的情况下将所述约束控制运算部的计算结果设为有效，在由所述接近判定部判定为未接近约束目标的情况下将所述约束控制运算部的计算结果设为无效的步骤；
合成部将所述从动控制运算部的计算结果与所述约束控制限制部的限制结果进行合成的步骤；以及
驱动控制部基于所述合成部的合成结果来驱动所述臂部的步骤。
CN201980032159.2A 2018-05-23 2019-05-10 机械手的直接教示装置以及直接教示方法 Active CN112118940B (zh)

Applications Claiming Priority (3)

Application Number Priority Date Filing Date Title
JP2018098891A JP7048176B2 (ja) 2018-05-23 2018-05-23 ロボットの直接教示装置及び直接教示方法
JP2018-098891 2018-05-23
PCT/JP2019/018766 WO2019225358A1 (ja) 2018-05-23 2019-05-10 ロボットの直接教示装置及び直接教示方法

Publications (2)

Publication Number Publication Date
CN112118940A CN112118940A (zh) 2020-12-22
CN112118940B true CN112118940B (zh) 2023-05-30

Family

ID=68616049

Family Applications (1)

Application Number Title Priority Date Filing Date
CN201980032159.2A Active CN112118940B (zh) 2018-05-23 2019-05-10 机械手的直接教示装置以及直接教示方法

Country Status (4)

Country Link
JP (1) JP7048176B2 (zh)
KR (1) KR102381594B1 (zh)
CN (1) CN112118940B (zh)
WO (1) WO2019225358A1 (zh)

Families Citing this family (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
JP7131087B2 (ja) * 2018-05-31 2022-09-06 セイコーエプソン株式会社 ロボットシステムの制御方法およびロボットシステム
JP2022062859A (ja) * 2020-10-09 2022-04-21 川崎重工業株式会社 手術支援システム、手術支援システムの制御装置および手術支援システムの制御方法

Citations (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
JPH05303425A (ja) * 1992-04-27 1993-11-16 Nippon Telegr & Teleph Corp <Ntt> 直接教示方式ロボット
JPH09141580A (ja) * 1995-11-22 1997-06-03 Yaskawa Electric Corp 直接教示ロボットの動作範囲制限装置
CN1200691A (zh) * 1995-09-14 1998-12-02 株式会社安川电机 机器人的教学装置
JP2017094440A (ja) * 2015-11-24 2017-06-01 川崎重工業株式会社 ロボットのダイレクト教示方法

Family Cites Families (10)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
JPH0337701A (ja) * 1989-07-05 1991-02-19 Toshiba Corp ロボットの制御方法及び装置
JP3207231B2 (ja) 1992-01-24 2001-09-10 松下電器産業株式会社 ロボットの直接教示方法
JPH05250029A (ja) 1992-03-04 1993-09-28 Kobe Steel Ltd 産業用ロボット
JPH05285870A (ja) 1992-04-03 1993-11-02 Matsushita Electric Ind Co Ltd 多関節ロボットの直接教示装置
US6385508B1 (en) 2000-10-31 2002-05-07 Fanuc Robotics North America, Inc. Lead-through teach handle assembly and method of teaching a robot assembly
JP3923053B2 (ja) 2004-03-31 2007-05-30 ファナック株式会社 ロボット教示装置
JP5636119B2 (ja) 2011-11-30 2014-12-03 パナソニック株式会社 ロボットの教示装置、ロボット装置、ロボットの教示装置の制御方法、ロボットの教示装置の制御プログラム
JP5893666B2 (ja) 2014-04-14 2016-03-23 ファナック株式会社 力に応じて動かすロボットのロボット制御装置およびロボットシステム
US9804593B1 (en) 2014-12-12 2017-10-31 X Development Llc Methods and systems for teaching positions to components of devices
US11364630B2 (en) 2017-02-17 2022-06-21 Abb Schweiz Ag Method for controlling an industrial robot during lead-through programming of the robot and an industrial robot

Patent Citations (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
JPH05303425A (ja) * 1992-04-27 1993-11-16 Nippon Telegr & Teleph Corp <Ntt> 直接教示方式ロボット
CN1200691A (zh) * 1995-09-14 1998-12-02 株式会社安川电机 机器人的教学装置
JPH09141580A (ja) * 1995-11-22 1997-06-03 Yaskawa Electric Corp 直接教示ロボットの動作範囲制限装置
JP2017094440A (ja) * 2015-11-24 2017-06-01 川崎重工業株式会社 ロボットのダイレクト教示方法

Also Published As

Publication number Publication date
KR102381594B1 (ko) 2022-04-04
CN112118940A (zh) 2020-12-22
KR20200139217A (ko) 2020-12-11
WO2019225358A1 (ja) 2019-11-28
JP7048176B2 (ja) 2022-04-05
JP2019202383A (ja) 2019-11-28

Similar Documents

Publication Publication Date Title
Park et al. Compliance-based robotic peg-in-hole assembly strategy without force feedback
JP5637883B2 (ja) 力センサの出力に基づいてロボットのダイレクトティーチをおこなうロボット教示装置
JP6924145B2 (ja) ロボット教示方法及びロボットアーム制御装置
US10065311B1 (en) Singularity handling for robot jogging
CN106493711B (zh) 控制装置、机器人以及机器人系统
JP5893666B2 (ja) 力に応じて動かすロボットのロボット制御装置およびロボットシステム
JP5946859B2 (ja) 力に応じて動かすロボットのロボット制御装置およびロボットシステム
US12011825B2 (en) Device, method and program for estimating weight and position of gravity center of load by using robot
CN107363830B (zh) 机器人控制装置、机器人以及机器人系统
US20200030992A1 (en) Robot System
JP2010149267A (ja) ロボットのキャリブレーション方法および装置
JP7187765B2 (ja) ロボット制御装置
CN109129525B (zh) 机器人的负载重心位置推定装置及负载重心位置推定方法
CN112118940B (zh) 机械手的直接教示装置以及直接教示方法
EP1795315A1 (en) Hand-held control device for an industrial robot
JP2018202588A (ja) 制御装置、ロボットおよびロボットシステム
JP2020171989A (ja) ロボット教示システム
Kobayashi et al. Motion capture with inertial measurement units for hand/arm robot teleoperation
JP2017077600A (ja) マニピュレータ装置
CN112654469A (zh) 机器人的直接示教装置及直接示教方法
Kobayashi et al. Human motion caption with vision and inertial sensors for hand/arm robot teleoperation
Shirato et al. Assist control for dual arm cooperative manipulation by remote controlled robot
JP2000079582A (ja) 多関節ロボットアームの制御方法
JP2005219205A (ja) 直交座標系上で柔らかさが調節可能なサーボ系

Legal Events

Date Code Title Description
PB01 Publication
PB01 Publication
SE01 Entry into force of request for substantive examination
SE01 Entry into force of request for substantive examination
GR01 Patent grant
GR01 Patent grant