JP2015157352A - ロボット、ロボットの制御装置及び制御方法、並びに、ロボット用制御プログラム - Google Patents

ロボット、ロボットの制御装置及び制御方法、並びに、ロボット用制御プログラム Download PDF

Info

Publication number
JP2015157352A
JP2015157352A JP2015012725A JP2015012725A JP2015157352A JP 2015157352 A JP2015157352 A JP 2015157352A JP 2015012725 A JP2015012725 A JP 2015012725A JP 2015012725 A JP2015012725 A JP 2015012725A JP 2015157352 A JP2015157352 A JP 2015157352A
Authority
JP
Japan
Prior art keywords
unit
force
robot
restriction
joint
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.)
Granted
Application number
JP2015012725A
Other languages
English (en)
Other versions
JP6238021B2 (ja
Inventor
真弓 小松
Mayumi Komatsu
真弓 小松
岡▲崎▼　安直
Yasunao Okazaki
安直 岡▲崎▼
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.)
Panasonic Intellectual Property Management Co Ltd
Original Assignee
Panasonic Intellectual Property Management Co Ltd
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 Panasonic Intellectual Property Management Co Ltd filed Critical Panasonic Intellectual Property Management Co Ltd
Priority to JP2015012725A priority Critical patent/JP6238021B2/ja
Publication of JP2015157352A publication Critical patent/JP2015157352A/ja
Application granted granted Critical
Publication of JP6238021B2 publication Critical patent/JP6238021B2/ja
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/1674—Programme controls characterised by safety, monitoring, diagnostic
    • B25J9/1676—Avoiding collision or forbidden zones
    • 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
    • G05B2219/00—Program-control systems
    • G05B2219/30—Nc systems
    • G05B2219/39—Robotics, robotics to robotics hand
    • G05B2219/39082—Collision, real time collision avoidance
    • 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
    • G05B2219/00—Program-control systems
    • G05B2219/30—Nc systems
    • G05B2219/39—Robotics, robotics to robotics hand
    • G05B2219/39209—Switch over from free space motion to constraint motion
    • 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
    • G05B2219/00—Program-control systems
    • G05B2219/30—Nc systems
    • G05B2219/40—Robotics, robotics mapping to robotics vision
    • G05B2219/40202—Human robot coexistence
    • Y—GENERAL TAGGING OF NEW TECHNOLOGICAL DEVELOPMENTS; GENERAL TAGGING OF CROSS-SECTIONAL TECHNOLOGIES SPANNING OVER SEVERAL SECTIONS OF THE IPC; TECHNICAL SUBJECTS COVERED BY FORMER USPC CROSS-REFERENCE ART COLLECTIONS [XRACs] AND DIGESTS
    • Y10—TECHNICAL SUBJECTS COVERED BY FORMER USPC
    • Y10S—TECHNICAL SUBJECTS COVERED BY FORMER USPC CROSS-REFERENCE ART COLLECTIONS [XRACs] AND DIGESTS
    • Y10S901/00—Robots
    • Y10S901/02—Arm motion controller
    • Y—GENERAL TAGGING OF NEW TECHNOLOGICAL DEVELOPMENTS; GENERAL TAGGING OF CROSS-SECTIONAL TECHNOLOGIES SPANNING OVER SEVERAL SECTIONS OF THE IPC; TECHNICAL SUBJECTS COVERED BY FORMER USPC CROSS-REFERENCE ART COLLECTIONS [XRACs] AND DIGESTS
    • Y10—TECHNICAL SUBJECTS COVERED BY FORMER USPC
    • Y10S—TECHNICAL SUBJECTS COVERED BY FORMER USPC CROSS-REFERENCE ART COLLECTIONS [XRACs] AND DIGESTS
    • Y10S901/00—Robots
    • Y10S901/46—Sensing device
    • Y—GENERAL TAGGING OF NEW TECHNOLOGICAL DEVELOPMENTS; GENERAL TAGGING OF CROSS-SECTIONAL TECHNOLOGIES SPANNING OVER SEVERAL SECTIONS OF THE IPC; TECHNICAL SUBJECTS COVERED BY FORMER USPC CROSS-REFERENCE ART COLLECTIONS [XRACs] AND DIGESTS
    • Y10—TECHNICAL SUBJECTS COVERED BY FORMER USPC
    • Y10S—TECHNICAL SUBJECTS COVERED BY FORMER USPC CROSS-REFERENCE ART COLLECTIONS [XRACs] AND DIGESTS
    • Y10S901/00—Robots
    • Y10S901/49—Protective device

Landscapes

  • Engineering & Computer Science (AREA)
  • Robotics (AREA)
  • Mechanical Engineering (AREA)
  • Manipulator (AREA)

Abstract

【課題】従来の産業用ロボットなどとは異なり、さらなる安全性に対応可能な、人と協働して作業を行うパワーアシスト型のロボット、ロボットの制御装置及び制御方法を提供する。
【解決手段】挟み込み判別部１５によって人９１若しくは物体が挟み込まれたことを判別後、制限時間設定部５１で設定された制限時間内は、領域分類部５２，５５が、先端部が、領域分類部で決定された制限をかける領域に位置すると判断するときには、制限をかける関節部の動作は停止しかつ制限をかけない関節部の少なくとも１つを力制御部１４，１６で動作させ、制限時間後は、すべての関節部を力制御部で動作制御可能とする。
【選択図】図１

Description

本開示は、人と協働して作業をアシストするパワーアシスト型のロボット、ロボットの動作を制御するロボットの制御装置及び制御方法、並びに、ロボット用制御プログラムに関する。
近年、介護ロボット又は家事支援ロボットなどの家庭用ロボットが盛んに開発されるようになってきた。また、産業用ロボットでも、セル生産工場の広がりなどから、人と協働するロボットの開発が盛んに行われている（特許文献１及び２参照）。
特開平１１−２７７４８３号公報 国際公開第２００７／０８０７３３号
このような人と協働するアシストロボットは、従来のように人間の居るエリアとロボット用の作業エリアとを区切って動作するロボットとは異なり、人間と共生する必要があるため、従来の産業用ロボットなどとは異なり、さらなる安全性が求められている。
そこで、本開示は、従来の産業用ロボットなどとは異なり、さらなる安全性に対応可能な、人と協働するパワーアシスト型のロボットを提供する。
本開示の一態様に係るロボットは、複数の関節部を有するロボットアームの先端部の位置情報を取得する位置情報取得部と、人が前記ロボットアームにかけている力を検出する第１の力検出部と、前記第１の力検出部で検出した力を基に、前記ロボットアームと外界環境との間に前記人若しくは物体が挟まれたか否かを判別する挟み込み判別部と、前記先端部が動作する領域を、制限領域と非制限領域に分類する領域分類部と、前記ロボットアームの動作に制限をかける時間である制限期間（制限時間）を設定する制限時間設定部を備え、前記挟み込み発生検出後の期間が、前記制限期間内であり、かつ、前記先端部が前記制限領域に位置する場合は、前記複数の関節部に含まれる一以上の関節部に動作制限が課され、前記複数の関節部のうち前記一以上の関節部以外の少なくとも１つの関節部には前記動作制限が課されず、前記動作制限は前記一以上の関節部の動作停止であり、前記挟み込み検出後の期間が、前記制限期間以降である場合は、前記動作制限が解除され、前記挟み込み検出後の期間が、前記制限期間内であり、かつ、前記先端部が前記非制限領域に位置する場合は、前記複数の関節部に動作制限が課されない。
これらの概括的かつ特定の態様は、装置、システム、方法、コンピュータプログラム並びに装置、システム、方法及びコンピュータプログラムの任意の組み合わせにより実現してもよい。
本開示のロボットは、人と協働して作業を行うパワーアシストロボットにおいて、周辺環境への影響を配慮しつつ挟まれ状態から安全に脱出する動作を行うことができる。
本開示の第１実施形態におけるロボットシステムの概要を示す図。 本開示の第１実施形態におけるロボットシステムの構成を示す図。 本開示の第１実施形態におけるロボットの制御装置及び制御対象であるロボットの一部を示すブロック図。 本開示の第１実施形態における人物体判別部のアルゴリズムの一例を説明するための説明図。 本開示の第１実施形態における人物体判別部のアルゴリズムの一例を説明するための説明図。 本開示の第１実施形態における方向別領域分類部の動作を説明するための作業の一例を示すロボットアーム手先経路の断面図。 本開示の第１実施形態における方向別領域分類部が記憶する制限方向の一例を示す図。 本開示の第１実施形態における方向別領域分類部が記憶する制限方向の一例を示す図。 本開示の第２実施形態におけるロボットの制御装置及び制御対象であるロボットの一部を示すブロック図。 本開示の第２実施形態における領域分類部の動作を説明するための作業の一例を示すロボットアーム手先経路の断面図。 本開示の第２実施形態における領域分類部の動作の一例を示す図。 本開示の第２実施形態における人物体判別部のアルゴリズムの一例を説明するための説明図。 本開示の第２実施形態における人物体判別部のアルゴリズムの一例を説明するための説明図。 本開示の第１実施形態において弾性係数を求める式を説明するための図。 本開示の知見において説明した、人の挟み込み状態の一例を示す図。 本開示の第１実施形態における制限時間設定部のアルゴリズムの一例を示す図。
（基礎となった知見）
従来、例えば、物体搬送において、人と協働して作業を行うパワーアシストロボットは、簡単な物体の組み付け作業にも応用が可能である。その場合、搬送物体は、周辺に何もない安全な場所だけを移動するわけではなく、周辺に組み付け先となる他の物体がある可能性が高い。そして、同時にロボットの安全性を高めても、人が介在する以上、人がある程度危険な状況になる可能性は、無視できない。想定できる危険状況の中に、人がロボット若しくはロボットの搬送物体と周辺環境との間に挟まれる状況が考えられる。例えば図１２で示すように、ロボットアーム２１で保持した搬送物体９２を周辺物体９５の中に組み込むような作業をパワーアシストロボットで実現する場合に、最終組み付けで、人が搬送物体９２を少し位置合わせをしようと、ロボットアーム２１に取り付けたハンドルから手を離し、自らの手で搬送物体９２を触って搬送物体９２の位置合わせをするような状況において、搬送物体９２と周辺物体９５との間に手を挟まれてしまうようなケースが考えられる。
しかしながら、特許文献１の技術では、手首軸のみしか動かないため、挟まれた状態から脱出できない場合があり、人への安全性に問題がある。また、手首軸のみならず基本軸が可動な状態でも、基本軸と手首軸を含むロボットの可動範囲が小さいため、人が、例えば手を挟まれた状態から素早く脱出できない場合もある。また、特許文献２の技術では、狭い空間では、人が、例えば、手を挟まれたとき、動く空間が制限されてしまい、人が脱出出来ない場合がある。また、ロボットアームが動ける方向と予め決められた方向とだけに動いても、人は挟まれた状態から脱出できない場合もある。その場合、ロボット制御装置は人への安全性から、周辺環境への影響以上に人の脱出を優先すべき場合もあるが、ロボット制御装置は対応出来ない。
本開示は、従来の産業用ロボットなどとは異なり、さらなる安全性に対応可能な、より具体的には、周辺環境への影響を配慮しつつ挟まれた状態から人を出来る限り素早く安全状態に脱出させるための動作を行うことができる、人と協働するパワーアシスト型のロボット、ロボットの制御装置及び制御方法、並びに、ロボット用制御プログラムを提供する。
以下に、本開示にかかる実施の形態を図面に基づいて詳細に説明する。
以下、図面を参照して本開示における実施形態を詳細に説明する前に、本開示の種々の態様について説明する。
第１の態様によれば、複数の関節部を有するロボットアームの先端部の位置情報を取得する位置情報取得部と、人が前記ロボットアームにかけている力を検出する第１の力検出部と、前記第１の力検出部で検出した力を基に、前記ロボットアームと外界環境との間に前記人若しくは物体が挟まれたか否かを判別する挟み込み判別部と、前記先端部が動作する領域を、制限領域と非制限領域に分類する領域分類部と、前記ロボットアームの動作に制限をかける時間である制限期間（制限時間）を設定する制限時間設定部を備え、前記挟み込み発生検出後の期間が、前記制限期間内であり、かつ、前記先端部が前記制限領域に位置する場合は、前記複数の関節部に含まれる一以上の関節部に動作制限が課され、前記複数の関節部のうち前記一以上の関節部以外の少なくとも１つの関節部には前記動作制限が課されず、前記動作制限は前記一以上の関節部の動作停止であり、前記挟み込み検出後の期間が、前記制限期間以降である場合は、前記動作制限が解除され、前記挟み込み検出後の期間が、前記制限期間内であり、かつ、前記先端部が前記非制限領域に位置する場合は、前記複数の関節部に動作制限が課されない、ロボットを提供する。
前記第１の態様によれば、人と協働して作業を行うパワーアシスト型のロボットにおいて、周辺環境への影響を配慮しつつ挟まれ状態から安全に脱出する動作を行うことができるロボットが可能となる。
第２の態様によれば、外部から前記ロボットアームが受けている外力を検出する第２の力検出部をさらに備えて、
前記挟み込み判別部は、前記第１の力検出部で検出した力と、前記第２の力検出部で検出した、前記外部から前記ロボットアームが受けている外力とに基づき、前記ロボットアームと前記外界環境との間に前記人若しくは物体が挟まれたことを判別し、
前記挟み込み判別部で挟み込みを判別したとき、前記第２の力検出部で検出した、前記外部から前記ロボットアームが受けている外力に基づき、前記ロボットアームを動作制御させる、
第１の態様に記載のロボットを提供する。
前記第２の態様によれば、第１の力検出部では力を検出出来ない場所で挟み込みが発生した場合にも、確実に挟み込みを検出しロボットを動作させることが出来るため、挟まれ状態から安全に脱出する動作を行うことが出来るロボットが可能となる。
第３の態様によれば、前記領域分類部は、前記制限領域を有する作業領域のマップを事前に保持している第１又は２の態様に記載のロボットを提供する。
前記第３の態様によれば、マップにより制限領域が判断できるため、制限領域では、周辺環境への影響を配慮しつつ挟まれ状態から安全に脱出する動作を行うことができるロボットが可能となる。
第４の態様によれば、前記領域分類部は、前記先端部の動作速度を求め、前記先端部の動作速度が速度の閾値より遅い領域を、前記制限領域とする第１又は２の態様に記載のロボットを提供する。
前記第４の態様によれば、前記領域分類部の働きにより、制限領域を予め決定していなくても、先端部の動作速度により、制限領域か、非制限領域かを決定でき、制限領域では、動作させる方向に制限をかけ、かつ制限時間設定部の働きにより、一定の制限時間を超えた後は全ての方向に動作するロボットアームの制御が可能となる。このように、領域分類部が先端部の動作速度から周辺環境への影響を推定し、制限をかける範囲（領域）か制限をかけない範囲（領域）かを決定することで、事前に作業領域のマップを持たない場合でも制御を実現することが可能となる。
第５の態様によれば、前記制限領域は複数の領域を含み、前記領域分類部は、前記複数の領域ごとに、前記複数の関節部の動作の制限をかける方向の情報を有している方向別領域分類部である、第３の態様に記載のロボットを提供する。
前記第５の態様によれば、複数の領域ごとにおいて限定された方向のみ制限をかけることが可能となるため、周辺環境への影響を配慮しつつ挟まれ状態からより安全に脱出する動作を行うことができるロボットが可能となる。
第６の態様によれば、前記方向別領域分類部の前記複数の領域の情報と前記関節部の動作の制限をかける方向の情報に基づいて、前記動作の制限をかける関節部が決定される第５の態様に記載のロボットを提供する。
前記第６の態様によれば、制限をかける領域において制限をかける関節のみ制限をかけることが可能となるため、周辺環境への影響を配慮しつつ挟まれ状態からより安全に脱出する動作を行うことができるロボットが可能となる。
第７の態様によれば、前記複数の関節部のうち、前記先端部に近い関節部から少なくとも１個の関節部は、制限を課されない第１の態様に記載のロボットを提供する。
前記第７の態様によれば、制限をかける領域において限定された関節のみ制限をかけることが可能となるため、周辺環境への影響を配慮しつつ、同時に、先端部に近い関節部から少なくとも１個の関節部は制限がかからないため、挟まれ状態から安全に脱出する動作を行うことができるロボットが可能となる。
第８の態様によれば、前記制限時間設定部は、前記第２の力検出部が検出した前記外力が大きいほど制限時間を短くする第２の態様に記載のロボットを提供する。
前記第８の態様によれば、外力が大きいときはより強く脱出する必要があるととらえ、そのようなときは、早く制限が解除されるため挟まれ状態からより早く脱出出来ることが可能となるため、挟まれ状態から安全に脱出動作を行うことが出来るロボットが可能となる。
第９の態様によれば、さらに、人物体判別部を備え、
前記人物体判別部は、前記位置情報取得部で取得した位置情報と、前記第２の力検出部で検出した、前記外部から前記ロボットアームが受けている外力と、前記挟み込み判別部で判別された情報とを基に、前記ロボットアームの前記先端部と前記外界環境との間に挟み込まれた物質が人か物体かを判別し、前記人であると判別した場合は、前記制限時間設定部が前記制限時間を、前記物体であると判別した場合よりも小さくする第２の態様に記載のロボットを提供する。
前記第９の態様によれば、人物体判別部の働きにより、人が挟まれたと判別したときには制限時間を短く、人以外の物体が挟まれた判別したときには制限時間を長くする動作が可能となる。この動作により、挟み込みが発生したときには、まず、先端部で保持する搬送物体が周辺環境等に衝突しない範囲で力制御を実現することで、挟み込まれた人若しくは物体が挟み込み状態から脱出出来る方向に動作することが出来る。また、人への安全をより高めたロボット動作が可能となる。
第１０の態様によれば、前記第２の力検出部は、前記関節部の駆動装置の電流値と前記位置情報取得部で取得した位置情報とから推定関節トルクを推定し、
前記人物体判別部は、前記位置情報取得部で取得した位置情報と前記第２の力検出部で推定した前記推定関節トルクとを基に、弾性係数を求め、求められた弾性係数が人判別用第１閾値より小さい物質を挟み込んだと判断した場合は、挟み込まれた物質は人であると判別し、前記弾性係数が前記人判別用第１閾値以上である物質を挟み込んだと判断した場合は、挟み込まれた物質は物体であると判別する、第９の態様に記載のロボットを提供する。
前記第１０の態様によれば、人物体判別部の働きにより、人が挟まれたと判別したときには制限時間を短く、人以外の物体が挟まれた判別したときには制限時間を長くする動作が可能となる。この動作により、挟み込みが発生したときには、まず、先端部で保持する搬送物体が周辺環境等に衝突しない範囲で力制御を実現することで、挟み込まれた人若しくは物体が挟み込み状態から脱出出来る方向に動作することが出来る。また、人への安全をより高めたロボット動作が可能となる。
第１１の態様によれば、前記人物体判別部は、前記第２の力検出部で検出した、前記外部から前記ロボットアームが受ける外力の最大値と最小値との差が、人判別用第２閾値以上に大きい場合は、挟み込まれた物質を人であると判別する第９の態様に記載のロボットを提供する。
前記第１１の態様によれば、人物体判別部の働きにより、人が挟まれたと判別したときには制限時間を短く、人以外の物体が挟まれた判別したときには制限時間を長くする動作が可能となる。この動作により、挟み込みが発生したときには、まず、先端部で保持する搬送物体が周辺環境等に衝突しない範囲で力制御を実現することで、挟み込まれた人若しくは物体が挟み込み状態から脱出出来る方向に動作することが出来る。また、人への安全をより高めたロボット動作が可能となる。
第１２の態様によれば、前記挟み込み判別部は、前記第１の力検出部からの入力が０であり前記第２の力検出部からの入力が一定値以上の値であるときに、前記挟み込み判別部は挟み込みがあると判別する第８の態様に記載のロボットを提供する。
前記第１２の態様によれば、第１の力検出部では力を検出出来ない場所で挟み込みが発生した場合に確実に挟み込みを検出しロボットを動作させることが出来、かつ第１の力検出部で力を検出出来る場合は、通常動作において、挟まれ状態から安全に脱出する動作を行うことが出来るロボットが可能となるため、周辺環境への影響を配慮しつつ挟まれ状態からより安全に脱出する動作を行うことができるロボットの動作が可能となる。
第１３の態様によれば、複数の関節部を有するロボットアームの先端部の位置情報を取得する位置情報取得部と、人が前記ロボットアームにかけている力を検出する第１の力検出部と、前記第１の力検出部で検出した力を基に、前記ロボットアームと外界環境との間に前記人若しくは物体が挟まれたか否かを判別する挟み込み判別部と、前記先端部が動作する領域を、制限領域と非制限領域に分類する領域分類部と、前記ロボットアームの動作に制限をかける時間である制限期間を設定する制限時間設定部を備え、前記挟み込み発生検出後の期間が、前記制限期間内であり、かつ、前記先端部が前記制限領域に位置する場合は、前記複数の関節部に含まれる一以上の関節部に動作制限が課され、前記複数の関節部のうち前記一以上の関節部以外の少なくとも１つの関節部には前記動作制限が課されず、前記動作制限は前記一以上の関節部の動作停止であり、前記挟み込み検出後の期間が、前記制限期間以降である場合は、前記動作制限が解除され、前記挟み込み検出後の期間が、前記制限期間内であり、かつ、前記先端部が前記非制限領域に位置する場合は、前記複数の関節部に動作制限が課されない、ロボットの制御装置を提供する。
前記第１３の態様によれば、人と協働して作業を行うパワーアシスト型のロボットの制御装置において、周辺環境への影響を配慮しつつ挟まれ状態から安全に脱出する動作を行うことができるロボットの制御装置が可能となる。
第１４の態様によれば、複数の関節部を有するロボットアームの先端部の位置情報を取得するステップと、人が前記ロボットアームにかけている力を検出するステップと、前記検出した力を基に、前記ロボットアームと外界環境との間に前記人若しくは物体が挟まれたか否かを判別するステップと、前記先端部が動作する領域を、制限領域と非制限領域に分類するステップと、前記ロボットアームの動作に制限をかける時間である制限期間を設定するステップを含み、前記挟み込み発生検出後の期間が、前記制限期間内であり、かつ、前記先端部が前記制限領域に位置する場合は、前記複数の関節部に含まれる一以上の関節部に動作制限が課され、前記複数の関節部のうち前記一以上の関節部以外の少なくとも１つの関節部には前記動作制限が課されず、前記動作制限は前記一以上の関節部の動作停止であり、前記挟み込み検出後の期間が、前記制限期間以降である場合は、前記動作制限が解除され、前記挟み込み検出後の期間が、前記制限期間内であり、かつ、前記先端部が前記非制限領域に位置する場合は、前記複数の関節部に動作制限が課されない、ロボットの制御方法を提供する。
前記第１４の態様によれば、人と協働して作業を行うパワーアシスト型のロボットの制御方法において、周辺環境への影響を配慮しつつ挟まれ状態から安全に脱出する動作を行うことができるロボットの制御方法が可能となる。
第１５の態様によれば、第１４の態様のロボットの制御方法をコンピュータに実行させるためのロボット用制御プログラムを提供する。
前記第１５の態様によれば、人と協働して作業を行うパワーアシスト型のロボットの制御プログラムにおいて、周辺環境への影響を配慮しつつ挟まれ状態から安全に脱出する動作を行うことができるロボットのロボット用制御プログラムが可能となる。
以下に、本開示にかかる実施の形態を図面に基づいて詳細に説明する。
（第１実施形態）
図１は、本開示の第１実施形態にかかるロボットシステム１００の概要を示す図である。
ロボットシステム１００のロボット２０は、ロボットアーム２１を備えるとともに、人９１がロボットアーム２１に力を伝達するためのハンドル２２をロボットアーム２１の手先に備えている。また、物体９２を保持するための吸着パッド９３をロボットアーム２１の手先（先端）に備えている。人９１がハンドル２２に力をかけると、人９１がハンドル２２にかけている力を第１力検出部２３で検出し、検出した力に応じてロボットアーム２１が動作し、物体搬送等の作業支援パワーアシストアームとして動作することができる。ロボットアーム２１の先端には、吸着パッド９３を備えており、吸着パッド９３により物体９２を保持することで物体搬送が可能となる。本明細書では、ロボットアーム２１の先端と吸着パッド９３とを含めてロボットアーム２１の先端部と称する。
図２は、本開示の第１実施形態にかかるロボットシステム１００の構成を示す図である。
ロボットシステム１００は、制御装置５０と、制御装置５０の制御対象であるロボット２０とにより構成されている。
制御装置５０は、この第１実施形態では、一例として、一般的なパーソナルコンピュータにより構成されており、制御装置５０は、制御プログラム４０と、入出力ＩＦ４１とで構成されている。
入出力ＩＦ（インターフェース）４１は、パーソナルコンピュータのＰＣＩバスなどの拡張スロットに接続された、例えば、Ｄ／Ａボードと、Ａ／Ｄボードと、カウンタボードとなどを備えるように構成されている。
制御装置５０は、入力部の一例としての入出力ＩＦ４１を介して、ロボット機構部６の各リンクマニピュレータを駆動するモータドライバ４２と接続され、そのモータドライバ４２に制御装置５０から制御信号を送る。
ロボット２０は、前記したように、ロボットアーム２１を備えている。ロボットアーム２１の構造については後述する。
ロボットアーム２１の動作を制御する制御装置５０の制御動作が実行されることにより、ロボットアーム２１の各関節部の後述するエンコーダ７より出力される各関節角度情報（回転位相角など）を検出し、検出した各関節角度情報が入出力ＩＦ４１のカウンタボードを通じて制御装置５０に取り込まれ、取り込まれた各関節角度情報に基づき制御装置５０によって各関節部の回転動作での制御指令値が算出される。算出された各制御指令値は、入出力ＩＦ４１のＤ／Ａボードを通じて、ロボットアーム２１の各関節部を駆動制御するためのモータドライバ４２に与えられ、モータドライバ４２から送られた各制御指令値に従って、ロボットアーム２１の各関節部の駆動装置の一例としてのモータ１９がそれぞれ独立して駆動される。このモータドライバ４２とモータ１９とで駆動部の一例として機能する。また、エンコーダ７は、角度情報（回転位相角など）を出力する角度検出部の一例として機能する。言い換えれば、エンコーダ７は、ロボットアーム２１の手先（先端の一例）の位置情報を取得する位置情報取得部として機能する。モータドライバ４２には、モータ１９に流れている電流の大きさを検出する電流検出部８が内蔵されている。電流検出部８は、後述するように、負荷トルク推定部１３と協働して第２の力情報（外部からロボットアーム２１が受けている外力の力情報）を検出する第２の力検出部としても機能する。第１の力検出部と第２の力検出部は、共に、力検出部の一例として機能する。
ロボットアーム２１は、一例として、台部３４も含めて６自由度の多リンクマニピュレータであり、先端（手先）の手首部３１には、力センサ２３を介してハンドル２２と吸着パッド９３との取り付けが可能である。ロボットアーム２１は、手首部３１と、手首部３１を先端に有する第２リンク３２と、第２リンク３２の基端を先端に回転可能に連結する第１リンク３３と、第１リンク３３の基端が回転可能に連結支持された第２関節支柱３５と、第２関節支柱３５が取り付けられかつ床９０に固定される台部３４とを備えている。さらに、人９１が手でハンドル２２に加えた力を計測するために、手首部３１の先端に、第１力検出部の一例である力センサ２３を介してハンドル２２は取り付けられている。また、同じく、手首部３１の先端には、物体９２を保持するための吸着パッド９３を備えている。
手首部３１は、第４関節部２７と、第５関節部２８と、第６関節部２９との３つの回転軸を有しており、第２リンク３２に対するハンドル２２の相対的な姿勢（向き）を変化させることができる。第２リンク３２の基端は、第１リンク３３の先端に対して第３関節部２６周りに回転可能とする。第１リンク３３の基端は、第２関節支柱３５に対して第２関節部２５周りに回転可能とする。台部３４の上側可動部３４ａは、台部３４の下側固定部３４ｂに対して第１関節部２４周りに回転可能とする。
この結果、ロボットアーム２１は、第１関節部２４から第６関節部２９の合計６個の回転軸周りにそれぞれ独立して回転可能として、前記６自由度の多リンクマニピュレータを構成している。
各回転軸の回転部分を構成する各関節部には、各関節部を構成する一対の部材（例えば、回動側部材と、該回動側部材を支持する支持側部材）のうちの一方の部材に備えられ、かつ後述するモータドライバ４２により駆動制御される回転駆動装置の一例としてのモータ１９（実際には、ロボットアーム２１の各関節部の内部に配設されている）と、モータ１９の回転軸の回転位相角（すなわち、関節角）を検出するエンコーダ７（実際には、ロボットアーム２１の各関節部の内部に配設されている）とを備える。よって、各関節部を構成する一方の部材に備えられたモータ１９の回転軸が、各関節部の他方の部材に連結されて前記回転軸を正逆回転させることにより、他方の部材を一方の部材に対して各回転軸周りに回転可能とする。
図３は、本開示の第１実施形態にかかるロボット２０の制御装置５０及び制御対象であるロボット２０の一部を示すブロック図である。
ロボット２０の制御装置５０は、目標軌道生成部１と、目標角加速度計算部２と、目標関節トルク計算部３と、逆運動学計算部４と、角度誤差補償部５と、角度誤差計算部９と、修正角加速度計算部１０と、目標軌道加算部１１と、挟み込み検知部１２と、第１力制御部１４と、第２力制御部１６と、人物体判別部４９と、制限時間設定部５１と、領域分類部の一例としての方向別領域分類部（第１領域分類部）５２と、挟み込み時目標値加算部５３と、関節制限部５４とを備えて構成している。第１力制御部１４と第２力制御部１６とは、力制御部の一例として機能する。
挟み込み検知部１２は、負荷トルク推定部１３と、挟み込み判別部１５とを備えて構成している。
また、ロボット２０は、ロボットアーム２１で構成されるロボット機構部６と、エンコーダ７と、電流検出部８と、第１力検出部の一例である力センサ２３とを備えて構成している。また、制御装置５０に対しては、エンコーダ７で計測されたロボット２０からの関節角度情報と、力センサ２３で計測された第１の力情報（力ベクトルＦ）とが入力される。この第１の力情報は、人９１がハンドル２２を介してロボットアーム２１にかけた力である。
目標軌道生成部１は、ロボットに実現させる作業の目標軌道に沿ったロボット２０の動作を実現するための目標位置ベクトルｒｄを目標軌道加算部１１に出力する。目標とするロボット２０の動作は、目的とする作業動作に応じて、事前に、それぞれの時間ｔ（ｔ＝０、ｔ＝ｔ１、ｔ＝ｔ２、・・・）でのポイントごとの目標位置ｒｄｔ＝［ｒｄｔ１，ｒｄｔ２，ｒｄｔ３，ｒｄｔ４，ｒｄｔ５，ｒｄｔ６］Ｔ（ｒｄ０、ｒｄ１、ｒｄ２、・・・）が目標軌道生成部１内の記憶部に記録されている。そして、目標位置生成部１は、それぞれの時間（ｔ＝０、ｔ＝ｔ１、ｔ＝ｔ２、・・・）でのポイントごとの位置（ｒｄ０、ｒｄ１、ｒｄ２、・・・）の情報を基に多項式補間を使用し、各ポイント間の軌道を補完し、目標位置ベクトルｒｄ＝［ｒｄ１，ｒｄ２，ｒｄ３，ｒｄ４，ｒｄ５，ｒｄ６］Ｔを生成する。
第１力制御部１４は、ロボット２０の力センサ２３の出力である第１の力情報（力ベクトルＦ）を基に、修正目標位置ベクトルｒｄFを算出して、目標軌道加算部１１に出力す
る。第１力制御部１４は、インピーダンス制御法又はコンプライアンス制御法等の力制御手法により、目標位置ベクトルｒｄFを算出する。人９１がロボットアーム２１に取り付
けられたハンドル２２にかけた力を、第１力検出部の一例である力センサ２３で検出し、検出した力を基に第１力制御部１４が修正目標位置ベクトルｒｄFを算出して出力する。
このような方法で、人９１がかけた力の方向にロボットアーム２１が動作する、人協調動作を実現することができる。
目標軌道加算部１１は、目標軌道生成部１から出力された目標位置ベクトルｒｄと第１力制御部１４から出力された修正目標位置ベクトルｒｄFを加算して修正目標位置ベクト
ルｒｄｍを求め、修正目標位置ベクトルｒｄｍを逆運動学計算部４に出力する。
逆運動学計算部４は、目標軌道加算部１１から出力された修正目標位置ベクトルｒｄｍが入力され、目標角度ベクトルｑｄを計算で求めて、挟み込み時目標値加算部５３に出力する。目標角度ベクトルｑｄは、ロボット２０の幾何学情報から計算される。
負荷トルク推定部１３は、電流検出部８の出力である各モータ１９の電流値とエンコーダ７から出力された各関節角度情報とから、各モータ１９によって発生しているトルク（推定関節トルク）を推定し、それにより、ロボットアーム２１が外部から受けている力を推定（検出）する。推定した力は、第２の力情報として、挟み込み判別部１５と、第２力制御部１６と人物体判別部４９と第２力制御部１６とに出力する。
各モータ１９の発生トルクτｍは、電流検出部８において検出した電流値を
Figure 2015157352
とし、各回転軸のトルク係数を
Figure 2015157352
とすると、一般的に
Figure 2015157352
によって求めることが出来る。なお、τｍＮとは、Ｎ番目の関節部の回転軸のモータ１９が発生しているトルクを意味する。
次に、各回転軸のモータ１９が発生しているトルクτｍからロボットアーム２１にかかっている重力を減算して、ロボットアーム２１に外部から受けているトルクτｈを外部から受けていると負荷トルク推定部１３で推定する。
Figure 2015157352
 ここで、τｍは各回転軸のモータ１９が発生しているトルクであり、
Figure 2015157352
はエンコーダ７から入力された各回転軸の角度情報であり、
Figure 2015157352
はロボットアーム２１にかかる重力項である。
出力する第２の力情報は、各関節部に外部からかかっているトルクで表現されるため、
Figure 2015157352
 となる。ただし、τｈｎのｎは関節部番号である。
各モータ１９は、ロボットアーム２１の動作時には、以下の式で表すトルクを出力している。
Figure 2015157352
Figure 2015157352
 は、それぞれ、搬送物体９２及びロボット２０のダイナミクスパラメータからなる係数行列であり、
Figure 2015157352
は角加速度である。負荷トルク推定部１３において、各関節部にかかっているトルクを推定するときは挟み込みが発生しているときなので、ロボットアーム２１はほぼ静止していると考えられる。静止状態で各モータ１９が出力しているトルクτは、重力項のみであり、以下の式で表される。
Figure 2015157352
よって、各回転軸のモータ１９が発生しているトルクτｍから重力項を減じることにより、モータ１９がロボットアーム２１を静止状態に保つためのトルク以上に出力しているトルクが負荷トルク推定部１３で算出でき、これが外部から受けているトルクと負荷トルク推定部１３で推定できる。
挟み込み判別部１５は、力センサ２３から入力される第１の力情報Ｆと負荷トルク推定部１３から入力される第２の力情報とから、ロボットアーム２１と外界環境との間に人９１を含む何らかの物体が挟み込まれているかどうかを判別する。挟み込みが発生したと挟み込み判別部１５で判別した場合は、挟み込み判別部１５から、挟み込み信号と挟み込みが発生している関節部番号とを第２力制御部１６と人物体判別部４９とに出力する。
ここでの挟み込みとは、人９１若しくは物体がロボットアーム２１と外界環境との間に挟み込まれ、かつ、人９１がその挟み込まれた状態を簡単に解除できない状態のことを指す。すなわち、何らかの挟み込みが発生していても、ハンドル２２に人９１が力をかけている場合、作業に必要な意図的な挟み込みである可能性があること、又は、仮に意図しない挟み込みであっても人９１がハンドル２２に適切な力をかけることによって挟み込み状態から人９１などが脱出可能であるため、ハンドル２２に力がかかっている場合は、挟み込みが発生したとは挟み込み判別部１５で判別しない。
挟み込み判別部１５は、まず、力センサ２３から入力されている力ベクトルＦが事前に定めた閾値以上のときは、ハンドル２２に人９１が力をかけている状態と判断し、挟み込みは発生していないと判別する。挟み込み判別部１５は、力センサ２３から入力されている力ベクトルＦが挟み込み判別用第１閾値よりも小さく（例えば、「０」であり）、かつ負荷トルク推定部１３から出力された第２の力情報の各要素の全てが、事前に定めた挟み込み判別用第２閾値よりも小さいときは、ロボットアーム２１に外部から力はかかっていないと判断し、挟み込みは発生していないと判別する。挟み込み判別部１５は、力センサ２３から入力されている力ベクトルＦが、挟み込み判別用第１閾値よりも小さく、かつ負荷トルク推定部１３から出力された第２の力情報の各要素のうち一つの要素でも、事前に定めた挟み込み判別用第２閾値以上のときは、挟み込みが発生していると判別する。挟み込みが発生していると挟み込み判別部１５で判別した場合は、どの関節部で発生しているかも、挟み込み判別部１５から第２力制御部１６と人物体判別部４９とに出力する。よって、挟み込み判別部１５は、力センサ２３からの入力が０であり（一定値未満の値であり）、かつ第２の力検出部の一部である負荷トルク推定部１３からの入力が一定値以上の値であるときに、挟み込み判別部１５は挟み込みがあると判別する。挟み込みが発生している関節部は、事前に定めた閾値以上の大きいトルクが発生している関節部のうち最も関節部番号が大きい関節部とする。これは、例えば図２において、第３関節部２６において挟み込みが発生している、すなわち、第２リンク３２に対して矢印ａの方向に力がかかっている状況において、外部からロボットアーム２１にかけられている力によって発生する各関節部の負荷トルクは、第３関節部２６と、第２関節部２５と、第１関節部２４との３つの関節部に発生し、第２リンク３２よりも手先に近い第４関節部２７と、第５関節部２８と、第６関節部２７とには発生しない。よって、閾値以上の大きな負荷トルクが発生している関節部のうち、最も手先に近い関節部で挟み込みが発生している、と挟み込み判別部１５で推定出来る。
人物体判別部４９は、エンコーダ７から入力された角度情報と負荷トルク推定部１３から入力された第２の力情報と挟み込み判別部１５から入力された挟み込み信号とを基に、挟み込まれた物が人か、人ではない物体か、を判別する。判別結果は、人物体判別部４９から制限時間設定部５１に出力する。
人物体判別部４９は、常に、予め事前に実験等で定めた時間分（例えば１秒）の角度情報及び第２の力情報を、人物体判別部４９の内部に有するメモリなどの記憶装置４９ｍに記憶していく。この結果、常に、１秒前から現在（制御を行う時点）までの角度情報及び第２の力情報が記憶されている状態になる。挟み込み判別部１５から挟み込み信号が入力された場合、１秒前からの情報、及び、挟み込み信号が入力されてからの所定の時間分の情報を基に、人か物体かの判別を行う。挟み込み信号が入力されてからの所定の時間も予め実験等で定めておき、例えば０．５秒とする。
これについて、図４Ａ及び図４Ｂを基に具体的に説明する。図４Ａの上段のグラフは時間と角度情報との関係を示し、中段のグラフは時間と第２の力情報との関係を示し、下段のグラフは時間と挟み込み信号との関係を示す。図４Ｂも、上段のグラフは時間と角度情報との関係を示し、中段のグラフは時間と第２の力情報との関係を示し、下段のグラフは時間と挟み込み信号との関係を示す。ここで、図４Ａは人と判別される例である。ｔ＝０の時刻において挟み込み信号（グラフでは「１」の値）が入力されるため、人物体判別部４９の記憶装置４９ｍには、ｔ＝−１の時刻からｔ＝０の時刻までの角度情報及び第２の力情報が記憶されている。また、挟み込み信号が入力されてから０．５秒後となるｔ＝０．５の時刻までの情報を使用する。図４Ａの第２の力情報及び関節情報は、挟み込み判別部１５から入力されかつ挟み込みが発生していると推定された関節部のトルク及び角度である。挟み込まれた物質（物体又は人）の弾性係数Ｋを人物体判別部４９で計算するため、以下の式を用いる。
Figure 2015157352
 ただし、ｎは挟み込みが発生している関節部番号、τｎ２はｔ＝０．５の時刻での推定関節トルク、τｎ１はｔ＝−１の時刻での推定関節トルク、ｑｎ２はｔ＝０．５の時刻でのエンコーダ７より出力される関節角度、ｑｎ１はｔ＝−１の時刻でのエンコーダ７より出力される各関節角度である。
図１１を用いて、弾性係数Ｋを求める式を説明する。推定関節トルクでは、その先のリンク６０のどこで挟み込みが発生しているかはわからないので、仮にリンク６０の関節軸（回転軸）６１の軸中心から長さｌの部分で挟み込みが発生したとする。ここで、挟み込みが発生する直前にエンコーダ７より出力される関節角度がｑｎ1に相当し、挟み込みが
発生して物体６２に力がかかっている状態でエンコーダ７より出力される関節角度がｑｎ２に相当する。また、挟み込みが発生する直前の推定関節トルクがτｎ1に相当し、挟み
込みが発生して物体６２に力がかかっている状態の推定関節トルクがτｎ２に相当する。挟み込み時の関節角度の変化は小さいと考えられるため、挟み込み前後で物体６２とリンク６０との接触点の位置はほとんど変わらず、どちらも関節軸６１から長さｌの部分だとすると、挟み込みによってリンク６０から物体６２にかけられた力は、
Figure 2015157352
で表すことが出来る。
また、挟み込みによって物体６２が縮んだ量は、リンク６０の長さｌの部分の変位量に相当するため、
Figure 2015157352
で表すことが出来る。ここで、挟み込み時の関節角度の変化は小さいため、
Figure 2015157352
と近似できる。
よって、物体６２の弾性係数Ｋは
Figure 2015157352
で求めることが出来る。
前記式で計算した弾性係数Ｋが、事前に定めた人判別用第１閾値よりも小さいときに、人物体判別部４９で人であると判別する。人物体判別部４９により人であると判別したときは、人判別信号を人物体判別部４９から制限時間設定部５１に出力する。
図４Ｂは物体と判別される例である。ｔ＝０の時刻において挟み込み信号（グラフでは「１」の値）が入力されるため、人物体判別部４９の記憶装置４９ｍには、ｔ＝−１からｔ＝０までの角度情報及び第２の力情報が記憶されている。また、挟み込み信号が入力されてから０．５秒後となるｔ＝０．５までの情報を使用する。図４Ａと同様に、弾性係数Ｋを人物体判別部４９で計算すると、閾値以上に大きくなり、人よりも堅いため、物体であると人物体判別部４９で判別する。人物体判別部４９により物体であると判別したときは、物体判別信号を人物体判別部４９から制限時間設定部５１に出力する。また、人であると判別したか物体であると判別したかに関わらず、挟み込みが発生して物体に力がかかっている状態での力、すなわち図１１における、力がかかっている状態の推定関節トルクτｎ２も、人物体判別部４９から制限時間設定部５１に出力する。
制限時間設定部５１は、人物体判別部４９から入力された人か物体かの判別信号と、挟み込みによって発生した力とを基に、予め決められた制限時間（制限期間）を関節制限部５４に出力する。
図１３は、制限時間設定部５１のアルゴリズムの一例を説明する説明図である。制限時間設定部５１は、挟み込まれた物が物体か人か、また、挟み込みによって発生した力が予め決められた閾値以上か未満かによって、制限時間をそれぞれ記憶している。この制限時間は、関節制限部５４が、挟み込みが発生後の力制御を行う際に動作する関節部に制限（動作制限）をかける時間である。よって、人が挟まれたと人物体判別部４９で判別した場合は、人への安全性を重視して、制限時間設定部５１により、制限時間を短く設定してある。逆に、物体が挟まれたと人物体判別部４９で判別した場合は、制限時間設定部５１により、周辺環境への影響も考えて人が挟まれたと判別した場合よりも、制限時間を長く設定する。挟み込みによって発生した力が大きいときは、それによって起こるロボット２０への影響及び挟み込まれた物体への影響が大きいと考え、力が小さいときよりも、制限時間設定部５１により、制限時間を短く時間設定する。
第２力制御部１６は、負荷トルク推定部１３の出力である各関節部に外部からかかっているトルクを基に、目標角度ベクトルを算出して、関節制限部５４に出力する。第２力制御部１６は、インピーダンス制御法又はコンプライアンス制御法等の力制御手法により、目標角度ベクトルを算出する。挟み込み等により、人９１がロボットアーム２１に取り付けられたハンドル２２にかけた力以外にロボットアーム２１に外部からかけられている力を、各関節部にかかっているトルク（推定関節トルク）として負荷トルク推定部１３で推定し、推定した力を基に第２力制御部１６が目標角度ベクトルを算出して、関節制限部５４に出力する。このような方法で、ロボットアーム２１にかけられている負荷トルクの方向にロボットアーム２１が動作し、挟み込み動作が発生したときに外部からかけられた力の方向に動作する力制御が実現する。力制御を行うことにより、ロボットアーム２１は、外部からかけられている力の方向に動作し、挟み込みが発生している場合は、挟み込まれた人若しくは物体から受ける力によって、挟み込みが解消される方向に動作するため、挟み込まれた人若しくは物体は挟み込み状態から脱出することが出来る。
方向別領域分類部５２の内部の記憶部には、ロボットアーム２１の作業領域のマップを事前に保持し、そのマップ内において、制限をかける方向を予め定めて記憶しておく。方向別領域分類部５２は、エンコーダ７から入力された角度情報に基づき、現在位置（制御する時点での位置）の制限方向を関節制限部５４に出力する。図５は、方向別領域分類部５２で分類した作業領域の一例である。この例では、ｚ方向（紙面と垂直な方向）への移動は小さいものとして、所定のｚ座標値におけるｘｙ平面での断面図で説明を行う。図５の外側の円Ａと内側の円Ｂとに挟まれた円環状の範囲がロボットアーム２１の吸着パッド９３の可動範囲である。そして、作業例として、楕円形状の領域Ｃで搬送物体９２をピックアップし、楕円形状の領域Ｄにある作業台９４に搬送物体９２を移設させる作業をロボットアーム２１で行うとする。ただし、領域Ｄの背後（図５では領域Ｄの下側）に、外界環境の一例として壁Ｅが存在するとする。この場合、仮に領域Ｄにある作業台９４と搬送物体９２との間に、何らかの物体若しくは人の一部が挟まれる可能性がある。その場合でも、周辺環境である壁Ｅへの影響を少なくするため、壁Ｅと搬送物体９２とをできる限り衝突させたくない。（搬送物体９２が壁Ｅに衝突しないことが、周辺環境への影響を最小限にすることに相当する。）そこで、方向別領域分類部５２は、領域Ｄを含む領域ＦＲにおいて壁Ｅと搬送物体９２とを衝突させないために、第１関節部２４を動作禁止とする。領域ＦＲは次の式で表現できる。なお、Ｘ１は壁Ｅの表面沿いのｘ軸の座標でもある。
Figure 2015157352
図５において、外側の円Ａと内側の円Ｂとに挟まれた円環状の領域であって、領域ＦＲと重なる領域は第１関節部２４を動作禁止する制限領域（ロボットアームの動作に制限をかける領域）である。一方、外側の円Ａと内側の円Ｂとに挟まれた円環状の領域であって、領域ＦＲと重ならない領域は、制限時間か否にかかわらず、第１関節部２４の動作は禁止とされない非制限領域（ロボットアームの動作に制限をかけない領域）である。方向別領域分類部５２は、エンコーダ７から入力された角度情報に基づき、吸着パッド９３が領域ＦＲ内に位置すると判断するときには、図６Ａの情報を関節制限部５４に出力し、吸着パッド９３（ロボットアームの先端部）が領域ＦＲ以外の領域に位置すると判断するときには、図６Ｂの情報を関節制限部５４に出力する。ただし、図６Ａ及び図６Ｂにおいて、「１」は制限（動作制限）をかけることを意味し、「０」は制限（動作制限）をかけないことを意味する。
方向別領域分類部５２は制限領域を含む作業領域のマップを複数保持してもよい。方向別領域分類部５２は複数の制限領域を含む作業領域のマップの各々に対応する図６Ａまたは図６Ｂに示すような動作の制限をかける方向の情報を、吸着パッド９３（ロボットアームの先端部）の位置に基づいて関節制限部５４に出力してもよい。
ロボットアーム２１の先端部に近い関節部から少なくとも１個の関節部は、制限をかけないようにしてもよい。
関節制限部５４は、方向別領域分類部５２から出力された制限方向の情報を基に、第２力制御部１６の出力である目標角度ベクトルのうち、制限方向の要素を更新せず、制限のない方向の要素は更新し、制限後目標角度ベクトルｑｄｍとして、挟み込み時目標値加算部５３に出力する。ただし、関節制限部５４は、制限時間設定部５１から出力された制限時間を超えた場合は、目標角度ベクトルのすべての要素を更新し、制限後目標角度ベクトルｑｄｍとして、挟み込み時目標値加算部５３に出力する。
具体的な動作は、次のように実現する。
関節制限部５４は、内部の記憶部に１回前の計算周期において出力した制限後目標角度ベクトルｑｄｍを常に記憶しておく。関節制限部５４は、制限時間設定部５１から出力された制限時間内の場合、今回の計算周期において、制限のない方向のベクトル要素については、今回入力された目標角度ベクトルの要素をそのまま、挟み込み時目標値加算部５３に出力する。制限方向のベクトル要素については、記憶しておいた制限後目標角度ベクトルの同じ要素番号のベクトル要素の値に関節制限部５４で置き換えて、挟み込み時目標値加算部５３に出力する。最後に、関節制限部５４は、今回出力した制限後目標角度ベクトルを内部の記憶部に新しく記憶する。この関節制限部５４の出力する制限後目標角度ベクトルｑｄｍに基づき（正確には、後述するように、この制限後目標角度ベクトルｑｄｍを含む情報を基に）、ロボットアーム２１が動作することで、制限のかけられていない方向については、負荷トルクの方向にロボットアーム２１が動作し、制限のある方向については停止するという動作が実現する。関節制限部５４は、制限時間設定部５１から出力された制限時間外の場合、今回の計算周期において、入力された目標角度ベクトルをそのまま制限後目標角度ベクトルｑｄｍとして挟み込み時目標値加算部５３に出力し、出力した制限後目標角度ベクトルを内部の記憶部に新しく記憶する。この関節制限部５４の出力する制限後目標角度ベクトルｑｄｍに基づき（正確には、後述するように、この制限後目標角度ベクトルｑｄｍを含む情報を基に）、ロボットアーム２１が動作することで、全ての方向について負荷トルクの方向にロボットアーム２１が動作する。
挟み込み時目標値加算部５３は、関節制限部５４から出力された制限後目標角度ベクトルｑｄｍと逆運動学計算部４から出力された目標角度ベクトルｑｄとを加算して最終角度目標値ｑｄｔを求める。挟み込み時目標値加算部５３は、求められた最終角度目標値ｑｄｔを目標角加速度計算部２と角度誤差計算部９とに出力する。
角度誤差計算部９は、挟み込み時目標値加算部５３から出力された目標角度ベクトルｑｄｔとエンコーダ７の出力ｑとが入力され、角度誤差ベクトルｑｅ＝ｑｄｔ−ｑを計算し、出力誤差の一例として角度誤差ベクトルｑｅを角度誤差補償部５に出力する。
目標角加速度計算部２は、挟み込み時目標値加算部５３が出力した目標角度ベクトルｑｄｔが入力され、目標角加速度
Figure 2015157352
を求めて修正目標角加速度計算部１０に出力する。
角度誤差補償部５は、角度誤差計算部９により出力される角度誤差ベクトルｑｅが入力され、制御指令値の一例として角度誤差修正指令値ΔＰｑｅを求めて修正目標角加速度計算部１０に出力される。
修正目標角加速度計算部１０は、目標角加速度計算部２の出力である目標角加速度
Figure 2015157352
と出力誤差補償部５の出力である角度誤差修正指令値ΔＰｑｅとが入力され、制御指令値の一例として修正目標角加速度
Figure 2015157352
を目標関節トルク計算部３に出力する。
目標関節トルク計算部３は、修正目標角加速度計算部１０の出力である修正目標角加速度
Figure 2015157352
と目標関節トルク計算部３の記憶部に予め記憶しているダイナミクスパラメータとから、目標関節トルクτｄを演算し、目標関節トルクτｄをロボット機構部６のモータドライバ４２に出力する。目標関節トルクτｄの演算方法の一例として、以下の式で目標関節トルクτｄを算出する。
Figure 2015157352
 ここで、
Figure 2015157352
 は、それぞれ、搬送物体９２及びロボット２０のダイナミクスパラメータからなる係数行列であり、
Figure 2015157352
は搬送物体９２及びロボット２０の質量にかかる重力項である。
目標関節トルクτｄは、目標関節トルク計算部３からＤ／Ａボードなどの入出力ＩＦ４１を介してモータドライバ４２にトルク目標値として入力され、モータドライバ４２により、各関節軸に備え付けられたモータ１９がそれぞれ独立して正逆回転駆動されて、ロボット機構部６のロボットアーム２１が動作する。
ロボット機構部６のロボットアーム２１が動作した結果、ロボットアーム２１の関節角度は変化し、それらの関節角度ｑを検出するエンコーダ７から検出した結果が、入出力ＩＦ４１を介して角度誤差計算部９に入力される。
以上のように、前記第１実施形態の前記制御装置５０は、目標軌道生成部１と、目標角加速度計算部２と、目標関節トルク計算部３と、逆運動学計算部４と、角度誤差補償部５と、角度誤差計算部９と、修正角加速度計算部１０と、目標軌道加算部１１と、挟み込み検知部１２と、第１力制御部１４と、第２力制御部１６と、人物体判別部４９と、制限時間設定部５１と、方向別領域分類部５２と、挟み込み時目標値加算部５３と、関節制限部５４とを備えて構成している。挟み込み検知部１２は、負荷トルク推定部１３と、挟み込み判別部１５とを備えて構成している。また、ロボット２０は、ロボットアーム２１で構成されるロボット機構部６と、エンコーダ７と、第１力検出部の一例としての力センサ２３と、電流検出部８とを備え、モータ１９のトルク制御を行うロボット２０を構成する。挟み込み検知部１２の働きにより、挟み込みが発生しているかどうか及び、挟み込みが発生しているときの外部からロボットアーム２１に働いている力を検出し、第２力制御部１６の働きによりロボットアーム２１に働いている力に基づいてロボットアーム２１を動作させる力制御を実現する。その際に、方向別領域分類部５２の働きにより、吸着パッド９３の位置する作業領域により動作させる方向に制限をかけ、かつ制限時間設定部５１の働きにより、一定の制限時間を超えた後は、全ての方向に動作するロボット２０の制御が可能となる。すなわち、挟み込み判別部１５が挟み込みを判別した場合、制限時間設定部５１で設定された動作制限をかける時間内であるときは、方向別領域分類部５２が、エンコーダ７で取得したロボットアーム２１の先端部の位置情報を基に、ロボットアーム２１の先端部が、方向別領域分類部５２で決定された制限をかける領域に位置すると判断するときには、制限をかける関節部の動作は停止して、制限をかけない関節部の少なくとも１つを第１力制御部１４によって動作制御させる。よって、ロボットアーム２１の先端部に近い関節部から少なくとも１個の関節部は、制限をかけないようにしている。そして、制限時設定部５１が設定された動作制限をかける時間が経過した後、制限をかける関節部の制限を解除する一方、ロボットアーム２１の先端部が、方向別領域分類部５２で決定された制限をかけない領域に位置すると判断するときには、いずれの関節部にも制限をかけることなく関節部を第１力制御部１４によって動作制御させることができる。また、人物体判別部４９の働きにより、人９１が挟まれたと判別したときには制限時間を短く、人以外の物体が挟まれた判別したときには制限時間を長くする動作が可能となる。この動作により、挟み込みが発生したときには、まず、吸着パッド９３で保持する搬送物体９２が周辺環境等に衝突しない範囲で力制御を実現することで、挟み込まれた人若しくは物体が挟み込み状態から脱出出来る方向に動作することが出来る。ただし、搬送物体９２が周辺環境等に衝突しない作業範囲でのロボットアーム２１の動作では挟み込み状態から脱出出来ない場合は、制限時間以降は全ての関節部が動作しての力制御が実現されるので、挟み込まれた人若しくは物体からロボットアーム２１にかかる力の方向にロボットアーム２１は動作し、挟み込み状態から脱出することが可能となる。また、人への安全性を考え、人が挟み込まれたと判別した場合は、制限時間を短くすることで、物体が挟み込まれたときよりも、より短い時間で人が確実に挟み込み状態から脱出することが可能となる。例えば、制限時間設定部５１は、第２の力検出部が検出した外力（第２の力情報）が大きいほど、制限時間を短くすることもできる。
（第２実施形態）
図７は、本開示の第２実施形態にかかるロボット２０の制御装置５０及び制御対象であるロボット２０の一部を示すブロック図である。第１実施形態は、ロボットアーム２１の動作に制限をかける領域か又は動作に制限をかけない領域かの情報（制限情報）を事前に記憶しているのに対して、第２実施形態は、そのような情報を事前に持たずに、動作制御時に取得した情報から制限情報を生成するものである。
ロボット２０の制御装置５０は、目標軌道生成部１と、目標角加速度計算部２と、目標関節トルク計算部３と、逆運動学計算部４と、角度誤差補償部５と、角度誤差計算部９と、修正角加速度計算部１０と、目標軌道加算部１１と、挟み込み検知部１２と、第１力制御部１４と、第２力制御部１６と、人物体判別部４９と、制限時間設定部５１と、領域分類部の別の例としての第２領域分類部５５と、挟み込み時目標値加算部５３と、関節制限部５４とを備えて構成している。
挟み込み検知部１２は、負荷トルク推定部１３と、挟み込み判別部１５とを備えて構成している。
また、ロボット２０は、ロボットアーム２１で構成されるロボット機構部６と、エンコーダ７と、電流検出部８と、第１力検出部の一例である力センサ２３とを備えて構成している。また、制御装置５０に対しては、エンコーダ７で計測されたロボット２０からの関節角度情報と、力センサ２３で計測された第１の力情報（力ベクトルＦ）とが入力される。この第１の力情報は、人９１がハンドル２２を介してロボットアーム２１にかけた力である。
人物体判別部４９は、挟み込み信号が入力されてから予め事前に実験等で定めた時間後（例えば０．５秒後）から、さらに一定時間後（例えば１秒後）までの第２の力情報の最大値と最小値との差が、事前に決めた閾値以上に大きいときは、人９１が挟み込まれていると判別し、前記差が、閾値よりも小さいときは、人ではない物体が挟み込まれていると判別する。これは、人が挟み込まれた場合、ロボットアーム２１の関節部が、ある程度の範囲内で動作するため、ロボットアーム２１にかかる力は変動するが、物体が挟み込まれた場合は、ロボットアーム２１にかかる力は変動しないと考えられるためである。
図１０Ａ及び図１０Ｂを基に、具体的に説明する。図１０Ａは、人が挟み込まれていると判別される例である。ｔ＝０の時刻において挟み込み信号が入力されるため、人物体判別部４９は、ｔ＝０．５からｔ＝１．５までの第２の力情報のうち、最大値τｎ２と最小値τｎ１との差を求める。この差が、事前に決めた人判別用第２閾値以上であるため、人が挟み込まれていると人物体判別部４９で判別する。人が挟み込まれていると判別したときは、人物体判別部４９から人判別信号を制限時間設定部５１に出力する。
図１０Ｂは、物体が挟み込まれていると判別される例である。ｔ＝０の時刻において挟み込み信号が入力されるため、人物体判別部４９は、ｔ＝０．５の時刻からｔ＝１．５の時刻までの第２の力情報のうち、最大値τｎ２と最小値τｎ１との差を求める。この差が事前に決めた閾値よりも小さいため、物体が挟み込まれていると判別する。物体が挟み込まれていると人物体判別部４９で判別したときは、人物体判別部４９から物体判別信号を制限時間設定部５１に出力する。なお、図１０Ａ及び図１０Ｂにおいて、図４Ａ及び図４Ｂと同様に、ｎは挟み込みが発生している関節部番号、τｎ２はｔ＝０．５の時刻での推定関節トルク、τｎ１はｔ＝−１の時刻での推定関節トルク、ｑｎ２はｔ＝０．５の時刻でエンコーダ７より出力される関節角度、ｑｎ１はｔ＝−１の時刻でエンコーダ７より出力される関節角度である。
第２領域分類部５５は、エンコーダ７から入力される角度情報に基づき、ロボットアーム２１の手先速度（先端部の動作速度）を算出し、算出した手先速度に応じて、制限をかける領域か又は制限をかけない領域かを分類し、関節制限部５４に出力する。手先速度の算出は、エンコーダ７から第２領域分類部５５に入力される角度情報とロボットアーム２１の幾何学情報とに基づき、手先（吸着パッド９３）の位置を第２領域分類部５５で算出し、算出した位置情報を第２領域分類部５５で微分することで得られる。
図８を用いて、第２領域分類部５５の動作について説明する。図８において、この例では、ｚ方向への移動は小さいものとして、動作の中心となるｚの値でのｘｙ平面での断面図で説明を行う。図８の外側の円Ａと内側の円Ｂとに挟まれた円環状の範囲が、ロボットアーム２１の手先にある吸着パッド９３の可動範囲である。そして、作業例として、楕円形状の領域Ｃで搬送物体９２をピックアップし、楕円形状の領域Ｄにある作業台９４に搬送物体９２を移設させる作業をロボットアーム２１で行うとする。
ただし、領域Ｄの左右及び背後（図５では領域Ｄの左右両側及び下側）に、外界環境として壁Ｅが存在するとする。ある作業において、ロボットアーム２１の手先位置が点ａから点ｂに移動すると仮定し、このときの手先位置の経路を実線で示す。人９１は、点ａにおいて搬送物体９２をピックアップして搬送物体９２の搬送を開始するが、点ａから点ｄまでの移動経路の周辺に特に障害物はないため、ロボットアーム２１の手先は比較的早い速度で点ａから点ｄに向けて移動し、点ｄに近づく。点ｄに近づいた位置で、搬送物体９２と壁Ｅとが衝突することなく、点ｂに搬送物体９２を近づけるためにロボットアーム２１の手先は速度を落とし、点ｂまで移動する。この作業の間、第２領域分類部５５は、エンコーダ７から入力される角度情報から、ロボットアーム２１の手先位置及びその位置を通過するときの手先速度を計算する。第２領域分類部５５は、計算して求めた手先速度が予め事前に決められた速度の閾値以上に早いときは、その位置の制限情報を「０」（制限をかけない）と分類し、計算して求めた手先速度が速度の閾値よりも遅いときは、その位置の制限情報を「１」（制限をかける）と分類する。図９は、第２領域分類部５５の、より具体的な分類の一例である。時刻ｔ＝ｔｎにおいてロボットアーム２１の手先は図８の点ｄを通過し、人９１はロボットアーム２１の操作速度を遅くした結果、時刻ｔ＝ｔｎまでの速度では制限がかからず、制限情報としては「０」を第２領域分類部５５から関節制限部５４に出力する。また、時刻ｔｎ＋１以降では、ロボットアーム２１の手先の速度が早いため、制限情報としては「１」を出力する。
関節制限部５４は、制限をかける関節部（動作を停止させる関節部）の情報と制限をかけない関節部（動作させる関節部）の情報とを内部の記憶部に事前に記憶しておき、制限時間設定部５１から出力された制限時間内の場合、第２力制御部１６の出力である目標角度ベクトルのうち、制限方向の要素を更新せず、制限のない方向の要素は更新し、制限後目標角度ベクトルｑｄｍとして挟み込み時目標値加算部５３に出力する。ただし、関節制限部５４は、制限時間設定部５１から出力された制限時間を超えた場合は、目標角度ベクトルのすべての要素を更新し、制限後目標角度ベクトルｑｄｍとして挟み込み時目標値加算部５３に出力する。また、関節制限部５４は、第２力制御部１６から目標角度ベクトルが入力され始めた時刻、すなわち、挟み込み判別部１５によって挟み込みが発生したと判別された時刻において、第２領域分類部５５から入力された制限情報が「０」の場合は、制限時間設定部５１から出力された制限時間にかかわらず、目標角度ベクトルのすべての要素を更新し、制限後目標角度ベクトルｑｄｍとして挟み込み時目標値加算部５３に出力する。関節制限部５４は、制限情報が「１」の場合は、関節部に制限をかける領域を意味するため、制限時間内は、制限をかける関節部の要素は更新せず、制限のない方向の要素は更新し、制限後目標角度ベクトルｑｄｍとして挟み込み時目標値加算部５３に出力する。
制限をかける具体的な動作は、第１実施形態と同様である。
制限をかける関節部と制限をかけない関節部とは、以下のように、関節制限部５４において事前に決めておく。関節制限部５４において、制限をかける関節部は、その関節部が動作することでロボットアーム２１の手先の移動距離が大きいものから順に、数個を、制限のかかる関節部群とし、残りの関節部を制限のかからない関節部群とする。例えば、図２に示したロボットアーム２１の場合、同じ１０度回転動作を行うと、第１関節部２４が最も手先の移動距離が大きくなり、順に、第２関節部２５、第３関節部２６、第４関節部２７と続く。そこで、関節制限部５４は、第１関節部２４、第２関節部２５、第３関節部２６を制限のかかる関節部に分類し、それ以外の関節部を制限のかからない関節部に分類して、内部の記憶部に記憶する。逆に言えば、ロボットアーム２１の先端部に近い関節部から少なくとも１個の関節部は、制限をかけないようにしている。
以上のように、前記第２実施形態の前記制御装置５０は、目標軌道生成部１と、目標角加速度計算部２と、目標関節トルク計算部３と、逆運動学計算部４と、角度誤差補償部５と、角度誤差計算部９と、修正角加速度計算部１０と、目標軌道加算部１１と、挟み込み検知部１２と、第１力制御部１４と、第２力制御部１６と、人物体判別部４９と、制限時間設定部５１と、第２領域分類部５５と、挟み込み時目標値加算部５３と、関節制限部５４とを備えて構成している。挟み込み検知部１２は、負荷トルク推定部１３と、挟み込み判別部１５とを備えて構成している。また、ロボット２０は、ロボットアーム２１で構成されるロボット機構部６と、エンコーダ７と、第１力検出部の一例としての力センサ２３と、電流検出部８とを備え、モータ１９のトルク制御を行うロボット２０を構成する。挟み込み検知部１２の働きにより、挟み込みが発生しているかどうか及び、挟み込みが発生しているときの外部からロボットアーム２１に働いている力を検出し、第２力制御部１６の働きによりロボットアーム２１に働いている力に基づいてロボットアーム２１を動作させる力制御を実現する。その際に、領域分類部５２の働きにより、制限をかける領域を予め決定していなくても、直前の作業速度（例えば、ロボットアーム２１の手先の速度）により、制限をかける領域か、制限をかけない領域かを第２領域分類部５５により決定でき、制限をかける領域では、動作させる方向に制限をかけ、かつ制限時間設定部５１の働きにより、一定の制限時間を超えた後は全ての方向に動作するロボットアーム２１の制御が可能となる。また、人物体判別部４９の働きにより、人が挟まれたときには制限時間を短く、人以外の物が挟まれたときには制限時間を長くする動作が可能となる。この動作により、挟み込みが発生したときには、まず、吸着パッド９３で保持する搬送物体９２が周辺環境等に衝突しない範囲で力制御を実現することで、挟み込まれた人若しくは物体が挟み込み状態から脱出出来る方向に動作することが出来る。ただし、搬送物体９２が周辺環境等に衝突しない作業範囲でのロボットアーム２１の動作では挟み込み状態から脱出出来ない場合は、制限時間以降は全ての関節部が動作しての力制御が実現されるので、挟み込まれた人若しくは物体からロボットアーム２１にかかる力の方向にロボットアーム２１は動作し、挟み込み状態から脱出することが可能となる。また、人への安全性を考え、人が挟み込まれたと判別した場合は、制限時間を短くすることで、物体が挟み込まれたときよりも、より短い時間で人が確実に挟み込み状態から脱出することが可能となる。第２領域分類部５５が直前の作業状態から周辺環境への影響を推定し、制限をかける範囲（領域）か制限をかけない範囲（領域）かを決定することで、事前に作業領域のマップを持たない場合でも前記制御を実現することが可能となる。
前記第１実施形態及び第２実施形態では、一例として６軸のロボットアーム２１を例に説明を行ったが、これに限られるわけではない。
前記第１実施形態及び第２実施形態では、一例として第２の力検知手段をモータ電流からの推定を例に説明を行ったが、これに限られるわけではなく、例えば各関節軸にトルクセンサを備えてもよいし、その他の推定手段を用いてもよい。
作業例としては、搬送作業以外に、手先に工具を取り付けて、組立作業又は加工作業などを行う場合も、本開示を適用することができる。
なお、本開示を第１実施形態及び第２実施形態及び変形例に基づいて説明してきたが、本開示は、前記の第１実施形態及び第２実施形態及び変形例に限定されないのはもちろんである。以下のような場合も本開示に含まれる。
前記制御装置５０の一部又は全部は、具体的には、マイクロプロセッサ、ＲＯＭ、ＲＡＭ、ハードディスクユニット、ディスプレイユニット、キーボード、マウスなどから構成されるコンピュータシステムである。前記ＲＡＭ又はハードディスクユニットには、コンピュータプログラムが記憶されている。前記マイクロプロセッサが、前記コンピュータプログラムにしたがって動作することにより、各部は、その機能を達成する。ここでコンピュータプログラムは、所定の機能を達成するために、コンピュータに対する指令を示す命令コードが複数個組み合わされて構成されたものである。
例えば、ハードディスク又は半導体メモリ等の記録媒体に記録されたソフトウェア・プログラムをＣＰＵ等のプログラム実行部が読み出して実行することによって、各構成要素が実現され得る。
なお、前記実施形態又は変形例における制御装置を構成する要素の一部又は全部を実現するソフトウェアは、以下のようなプログラムである。つまり、このプログラムは、コンピュータに、
複数の関節部を有するロボットアームの前記先端部に人が直接力を掛けて、位置情報取得部で取得した前記ロボットアームの先端部の位置情報に基づき前記ロボットアームを動作制御させる力制御部と、
前記人が前記ロボットアームにかけている力を検出する第１の力検出部と、
前記第１の力検出部で検出した力を基に、前記ロボットアームと外界環境との間に前記人若しくは物体が挟まれたことを判別する挟み込み判別部と、
前記第１の力検出部で力を検出したときに、動作の制限をかけない関節部と、動作の制限をかける関節部とを決定する関節制限部と、
前記ロボットアームの前記先端部の動作する領域を、前記ロボットアームの動作に制限をかける領域と、制限をかけない領域とに分類する領域分類部と、
前記動作の制限をかける時間を設定する制限時間設定部と、
として機能させるとともに、
前記挟み込み判別部が挟み込みを判別した場合、前記制限時間設定部で設定された前記動作制限をかける時間内であるときは、前記領域分類部が、前記位置情報取得部で取得した前記ロボットアームの前記先端部の前記位置情報を基に、前記ロボットアームの前記先端部が、前記領域分類部で決定された前記制限をかける領域に位置すると判断するときには、前記制限をかける関節部の動作は停止して、前記制限をかけない関節部の少なくとも１つを前記力制御部によって動作制御させ、前記制限時間設定部が設定された前記動作制限をかける時間が経過した後、前記制限をかける関節部の制限を解除する一方、前記ロボットアームの前記先端部が、前記領域制限部で決定された前記制限をかけない領域に位置すると判断するときには、いずれの関節部にも制限をかけることなく前記関節部を前記力制御部によって動作制御させるように機能させる、多関節ロボットの制御プログラムである。
また、このプログラムは、サーバなどからダウンロードされることによって実行されてもよく、所定の記録媒体（例えば、ＣＤ−ＲＯＭなどの光ディスク、磁気ディスク、又は、半導体メモリなど）に記録されたプログラムが読み出されることによって実行されてもよい。
また、このプログラムを実行するコンピュータは、単数であってもよく、複数であってもよい。すなわち、集中処理を行ってもよく、あるいは分散処理を行ってもよい。
なお、前記様々な実施形態又は変形例のうちの任意の実施形態又は変形例を適宜組み合わせることにより、それぞれの有する効果を奏するようにすることができる。
本開示のロボット、ロボットの制御装置及び制御方法、並びに、ロボット用制御プログラムは、人と協働して作業をアシストするパワーアシスト型のロボット、ロボットの制御装置及び制御方法、並びに、ロボット用制御プログラムとして有用である。
１ 目標軌道生成部
２ 目標角加速度計算部
３ 目標関節トルク計算部
４ 逆運動学計算部
５ 角度誤差補償部
６ ロボット機構部
７ エンコーダ
８ 電流検出部
９ 角度誤差計算部
１０ 修正目標角加速度計算部
１１ 目標軌道加算部
１２ 挟み込み検知部
１３ 負荷トルク推定部
１４ 第１力制御部
１５ 挟み込み判別部
１６ 第２力制御部
１９ モータ
２０ ロボット
２１ ロボットアーム
２２ ハンドル
２３ 第１力検出部
２４ 第１関節部
２５ 第２関節部
２６ 第３関節部
２７ 第４関節部
２８ 第５関節部
２９ 第６関節部
３１ 手首部
３２ 第２リンク
３３ 第１リンク
３４、３４ａ、３４ｂ 台部
３５ 第２関節支柱
４０ 制御プログラム
４１ 入出力ＩＦ
４２ モータドライバ
４９ 人物体判別部
４９ｍ 記憶装置
５０ 人物体判別部
５１ 制限時間設定部
５２ 方向別領域分類部（第１領域分類部）
５３ 挟み込み時目標値加算部
５４ 関節制限部
５５ 第２領域分類部
６０ リンク
６１ 関節軸（回転中心）
６２ 挟み込み物体
９０ 床
９１ 人
９２ 搬送物体
９３ 吸着パッド
９４ 作業台
９５ 周辺物体
１００ ロボットシステム
Ａ 外側の円
Ｂ 内側の円
Ｃ 楕円形状の領域
Ｄ 楕円形状の領域
Ｅ 外界環境の一例としての壁
ＦＲ 領域

Claims (15)

  1. 複数の関節部を有するロボットアームの先端部の位置情報を取得する位置情報取得部と、
    人が前記ロボットアームにかけている力を検出する第１の力検出部と、
    前記第１の力検出部で検出した力を基に、前記ロボットアームと外界環境との間に前記人若しくは物体が挟まれたか否かを判別する挟み込み判別部と、
    前記先端部が動作する領域を、制限領域と非制限領域に分類する領域分類部と、
    前記ロボットアームの動作に制限をかける時間である制限期間を設定する制限時間設定部を備え、
    前記挟み込み発生検出後の期間が、前記制限期間内であり、かつ、前記先端部が前記制限領域に位置する場合は、前記複数の関節部に含まれる一以上の関節部に動作制限が課され、前記複数の関節部のうち前記一以上の関節部以外の少なくとも１つの関節部には前記動作制限が課されず、
    前記動作制限は前記一以上の関節部の動作停止であり、
    前記挟み込み検出後の期間が、前記制限期間以降である場合は、前記動作制限が解除され、
    前記挟み込み検出後の期間が、前記制限期間内であり、かつ、前記先端部が前記非制限領域に位置する場合は、前記複数の関節部に動作制限が課されない、
    ロボット。
  2. 外部から前記ロボットアームが受けている外力を検出する第２の力検出部をさらに備えて、
    前記挟み込み判別部は、前記第１の力検出部で検出した力と、前記第２の力検出部で検出した、前記外部から前記ロボットアームが受けている外力とに基づき、前記ロボットアームと前記外界環境との間に前記人若しくは前記物体が挟まれたことを判別し、
    前記挟み込み判別部で挟み込みを判別したとき、前記第２の力検出部で検出した、前記外部から前記ロボットアームが受けている外力に基づき、前記ロボットアームを動作制御させる、
    請求項１に記載のロボット。
  3. 前記領域分類部は、前記制限領域を有する作業領域のマップを事前に保持している請求項１又は２に記載のロボット。
  4. 前記領域分類部は、前記先端部の動作速度を求め、前記先端部の動作速度が速度の閾値より遅い領域を、前記制限領域とする請求項１又は２に記載のロボット。
  5. 前記制限領域は複数の領域を含み、
    前記領域分類部は、前記複数の領域ごとに、前記複数の関節部の動作の制限をかける方向の情報を有している方向別領域分類部である、請求項３に記載のロボット。
  6. 前記方向別領域分類部の前記複数の領域の情報と前記関節部の動作の制限をかける方向の情報に基づいて、前記動作の制限をかける関節部が決定される請求項５に記載のロボット。
  7. 前記複数の関節部のうち、前記先端部に近い関節部から少なくとも１個の関節部は、制限を課されない請求項１に記載のロボット。
  8. 前記制限時間設定部は、前記第２の力検出部が検出した前記外力が大きいほど制限時間を短くする請求項２に記載のロボット。
  9. さらに、人物体判別部を備え、
    前記人物体判別部は、前記位置情報取得部で取得した位置情報と、前記第２の力検出部で検出した、前記外部から前記ロボットアームが受けている外力と、前記挟み込み判別部で判別された情報とを基に、前記ロボットアームの前記先端部と前記外界環境との間に挟み込まれた物質が前記人か前記物体かを判別し、前記人であると判別した場合は、前記制限時間設定部が前記制限時間を、前記物体であると判別した場合よりも小さくする請求項２に記載のロボット。
  10. 前記第２の力検出部は、前記関節部の駆動装置の電流値と前記位置情報取得部で取得した位置情報とから推定関節トルクを推定し、
    前記人物体判別部は、前記位置情報取得部で取得した位置情報と前記第２の力検出部で推定した前記推定関節トルクとを基に、弾性係数を求め、求められた弾性係数が人判別用第１閾値より小さい物質を挟み込んだと判断した場合は、挟み込まれた物質は人であると判別し、前記弾性係数が前記人判別用第１閾値以上である物質を挟み込んだと判断した場合は、挟み込まれた物質は物体であると判別する、請求項９に記載のロボット。
  11. 前記人物体判別部は、前記第２の力検出部で検出した、前記外部から前記ロボットアームが受ける外力の最大値と最小値との差が、人判別用第２閾値以上の場合は、挟み込まれた物質を人であると判別する請求項９に記載のロボット。
  12. 前記挟み込み判別部は、前記第１の力検出部からの入力が０であり前記第２の力検出部からの入力が一定値以上の値であるときに、前記挟み込み判別部は挟み込みがあると判別する請求項８に記載のロボット。
  13. 複数の関節部を有するロボットアームの先端部の位置情報を取得する位置情報取得部と、
    人が前記ロボットアームにかけている力を検出する第１の力検出部と、
    前記第１の力検出部で検出した力を基に、前記ロボットアームと外界環境との間に前記
    人若しくは物体が挟まれたか否かを判別する挟み込み判別部と、
    前記先端部が動作する領域を、制限領域と非制限領域に分類する領域分類部と、
    前記ロボットアームの動作に制限をかける時間である制限期間を設定する制限時間設定部を備え、
    前記挟み込み発生検出後の期間が、前記制限期間内であり、かつ、前記先端部が前記制限領域に位置する場合は、前記複数の関節部に含まれる一以上の関節部に動作制限が課され、前記複数の関節部のうち前記一以上の関節部以外の少なくとも１つの関節部には前記動作制限が課されず、
    前記動作制限は前記一以上の関節部の動作停止であり、
    前記挟み込み検出後の期間が、前記制限期間以降である場合は、前記動作制限が解除され、
    前記挟み込み検出後の期間が、前記制限期間内であり、かつ、前記先端部が前記非制限領域に位置する場合は、前記複数の関節部に動作制限が課されない、
    ロボットの制御装置。
  14. 複数の関節部を有するロボットアームの先端部の位置情報を取得するステップと、
    人が前記ロボットアームにかけている力を検出するステップと、
    前記検出した力を基に、前記ロボットアームと外界環境との間に前記人若しくは物体が挟まれたか否かを判別するステップと、
    前記先端部が動作する領域を、制限領域と非制限領域に分類するステップと、
    前記ロボットアームの動作に制限をかける時間である制限期間を設定するステップを含み、
    前記挟み込み発生検出後の期間が、前記制限期間内であり、かつ、前記先端部が前記制限領域に位置する場合は、前記複数の関節部に含まれる一以上の関節部に動作制限が課され、前記複数の関節部のうち前記一以上の関節部以外の少なくとも１つの関節部には前記動作制限が課されず、
    前記動作制限は前記一以上の関節部の動作停止であり、
    前記挟み込み検出後の期間が、前記制限期間以降である場合は、前記動作制限が解除され、
    前記挟み込み検出後の期間が、前記制限期間内であり、かつ、前記先端部が前記非制限領域に位置する場合は、前記複数の関節部に動作制限が課されない、
    ロボットの制御方法。
  15. 請求項１４に記載のロボットの制御方法をコンピュータに実行させるためのロボット用制御プログラム。
JP2015012725A 2014-01-27 2015-01-26 ロボット、ロボットの制御装置及び制御方法、並びに、ロボット用制御プログラム Active JP6238021B2 (ja)

Priority Applications (1)

Application Number Priority Date Filing Date Title
JP2015012725A JP6238021B2 (ja) 2014-01-27 2015-01-26 ロボット、ロボットの制御装置及び制御方法、並びに、ロボット用制御プログラム

Applications Claiming Priority (3)

Application Number Priority Date Filing Date Title
JP2014011945 2014-01-27
JP2014011945 2014-01-27
JP2015012725A JP6238021B2 (ja) 2014-01-27 2015-01-26 ロボット、ロボットの制御装置及び制御方法、並びに、ロボット用制御プログラム

Publications (2)

Publication Number Publication Date
JP2015157352A true JP2015157352A (ja) 2015-09-03
JP6238021B2 JP6238021B2 (ja) 2017-11-29

Family

ID=53678200

Family Applications (1)

Application Number Title Priority Date Filing Date
JP2015012725A Active JP6238021B2 (ja) 2014-01-27 2015-01-26 ロボット、ロボットの制御装置及び制御方法、並びに、ロボット用制御プログラム

Country Status (2)

Country Link
US (1) US9452532B2 (ja)
JP (1) JP6238021B2 (ja)

Cited By (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
JP2017074660A (ja) * 2015-10-16 2017-04-20 ファナック株式会社 人と協働して物体を運搬するロボットを制御するロボット制御装置、ロボットシステム、およびその方法
JP2019018340A (ja) * 2017-07-20 2019-02-07 キヤノン株式会社 作業用ロボット、作業用ロボットの制御方法
US11358276B2 (en) 2018-08-31 2022-06-14 Fanuc Corporation Robot and robot system
JP7098080B1 (ja) * 2021-09-08 2022-07-08 三菱電機株式会社 ロボット制御装置

Families Citing this family (25)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
JP5360254B2 (ja) * 2012-03-21 2013-12-04 トヨタ自動車株式会社 トルク検出方法及びアーム装置
JP5980877B2 (ja) * 2014-10-23 2016-08-31 ファナック株式会社 ロボットに掛かる荷重を検知するためのシステム、ロボット、およびロボットシステム
US9804593B1 (en) * 2014-12-12 2017-10-31 X Development Llc Methods and systems for teaching positions to components of devices
JP6034900B2 (ja) * 2015-03-06 2016-11-30 ファナック株式会社 動作プログラムの再開を判断するロボット制御装置
DE102015009048B3 (de) * 2015-07-13 2016-08-18 Kuka Roboter Gmbh Steuern eines nachgiebig geregelten Roboters
US9828094B2 (en) * 2015-07-26 2017-11-28 John B. McMillion Autonomous cleaning system
US10350766B2 (en) * 2015-09-21 2019-07-16 GM Global Technology Operations LLC Extended-reach assist device for performing assembly tasks
US9855664B2 (en) 2015-11-25 2018-01-02 Denso Wave Incorporated Robot safety system
JP6657859B2 (ja) * 2015-11-30 2020-03-04 株式会社デンソーウェーブ ロボット安全システム
WO2017127202A1 (en) * 2016-01-20 2017-07-27 Intuitive Surgical Operations, Inc. System and method for rapid halt and recovery of motion deviations in medical device repositionable arms
CN107427328B (zh) * 2016-01-22 2020-03-20 奥林巴斯株式会社 医疗用机械手***
US11045955B2 (en) * 2016-05-26 2021-06-29 Mitsubishi Electric Corporation Robot control device
JP6392910B2 (ja) * 2017-01-13 2018-09-19 ファナック株式会社 ロボットの安全確保動作機能を備えた人間協働ロボットシステム
JP6420388B2 (ja) * 2017-03-13 2018-11-07 ファナック株式会社 サーボモータ制御装置、及び、サーボモータ制御システム
JP2018176397A (ja) * 2017-04-21 2018-11-15 オムロン株式会社 ロボットシステム
CN110869174B (zh) * 2017-07-10 2023-12-05 海别得公司 用于生成材料处理机器人工具路径的计算机实现的方法和***
TWI642523B (zh) * 2017-09-21 2018-12-01 上銀科技股份有限公司 用於機械臂之負載估測重力補償的方法及其負載估測重力補償系統
JP6633587B2 (ja) * 2017-10-05 2020-01-22 ファナック株式会社 ロボットの構成部材の速度を制限する制御装置
EP3489641B1 (en) 2017-11-27 2024-04-17 Goodrich Actuation Systems Limited Improved system for detecting a mechanical fault in a rotating shaft
JP6687654B2 (ja) * 2018-03-14 2020-04-28 ファナック株式会社 協働ロボットの制御装置及び制御方法
EP3807058A1 (en) * 2018-06-15 2021-04-21 Universal Robots A/S Estimation of payload attached to a robot arm
JP7384602B2 (ja) * 2019-09-12 2023-11-21 ファナック株式会社 ロボット制御装置
JP7388870B2 (ja) * 2019-10-18 2023-11-29 ファナック株式会社 ロボットシステムおよび制御装置
US11801603B2 (en) * 2020-05-12 2023-10-31 Soremartec S.A. Operating device
CN111906787A (zh) * 2020-08-10 2020-11-10 珠海格力智能装备有限公司 机器人的控制方法及控制装置、机器人

Citations (8)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
JPH11277483A (ja) * 1998-03-30 1999-10-12 Matsushita Electric Ind Co Ltd ロボットの制御方法および装置
JP2000006065A (ja) * 1998-06-24 2000-01-11 Sony Corp 関節装置、ロボット装置及び関節制御方法
JP2000218589A (ja) * 1999-02-02 2000-08-08 Sony Corp アクチュエータ制御装置及びその方法
JP2001287190A (ja) * 2000-04-07 2001-10-16 Denso Corp ロボット
JP2004174644A (ja) * 2002-11-26 2004-06-24 Sony Corp 脚式移動ロボットの制御装置
JP2004223663A (ja) * 2003-01-24 2004-08-12 Doshisha インピーダンス制御装置、およびインピーダンス制御プログラム
US20100145515A1 (en) * 2008-12-10 2010-06-10 Kabushiki Kaisha Yaskawa Denki Robot system and control method
US20140163735A1 (en) * 2011-08-19 2014-06-12 Kabushiki Kaisha Yaskawa Denki Robot system, robot, and robot control device

Family Cites Families (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
JPS63123682A (ja) 1986-11-11 1988-05-27 オムロン株式会社 産業用ロボツトの動作領域制御装置
JPH01310889A (ja) 1988-06-10 1989-12-14 Toshiba Corp 産業用ロボットの制御装置
WO2007080733A1 (ja) 2006-01-13 2007-07-19 Matsushita Electric Industrial Co., Ltd. ロボットアームの制御装置及び制御方法、ロボット、及びプログラム
JP4319232B2 (ja) 2007-09-12 2009-08-26 トヨタ自動車株式会社 パワーアシスト装置およびその制御方法
CN102470531B (zh) * 2010-01-04 2016-01-20 松下知识产权经营株式会社 机器人、机器人的控制装置及控制方法

Patent Citations (8)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
JPH11277483A (ja) * 1998-03-30 1999-10-12 Matsushita Electric Ind Co Ltd ロボットの制御方法および装置
JP2000006065A (ja) * 1998-06-24 2000-01-11 Sony Corp 関節装置、ロボット装置及び関節制御方法
JP2000218589A (ja) * 1999-02-02 2000-08-08 Sony Corp アクチュエータ制御装置及びその方法
JP2001287190A (ja) * 2000-04-07 2001-10-16 Denso Corp ロボット
JP2004174644A (ja) * 2002-11-26 2004-06-24 Sony Corp 脚式移動ロボットの制御装置
JP2004223663A (ja) * 2003-01-24 2004-08-12 Doshisha インピーダンス制御装置、およびインピーダンス制御プログラム
US20100145515A1 (en) * 2008-12-10 2010-06-10 Kabushiki Kaisha Yaskawa Denki Robot system and control method
US20140163735A1 (en) * 2011-08-19 2014-06-12 Kabushiki Kaisha Yaskawa Denki Robot system, robot, and robot control device

Cited By (7)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
JP2017074660A (ja) * 2015-10-16 2017-04-20 ファナック株式会社 人と協働して物体を運搬するロボットを制御するロボット制御装置、ロボットシステム、およびその方法
US10232513B2 (en) 2015-10-16 2019-03-19 Fanuc Corporation Robot control device, robot system and method of controlling robot which carries object in cooperation with person
JP2019018340A (ja) * 2017-07-20 2019-02-07 キヤノン株式会社 作業用ロボット、作業用ロボットの制御方法
JP7427358B2 (ja) 2017-07-20 2024-02-05 キヤノン株式会社 ロボットシステム、物品の製造方法、制御方法、制御プログラム、および記録媒体
US11358276B2 (en) 2018-08-31 2022-06-14 Fanuc Corporation Robot and robot system
JP7098080B1 (ja) * 2021-09-08 2022-07-08 三菱電機株式会社 ロボット制御装置
US12017372B2 (en) 2021-09-08 2024-06-25 Mitsubishi Electric Corporation Robot control device

Also Published As

Publication number Publication date
US20150209961A1 (en) 2015-07-30
JP6238021B2 (ja) 2017-11-29
US9452532B2 (en) 2016-09-27

Similar Documents

Publication Publication Date Title
JP6238021B2 (ja) ロボット、ロボットの制御装置及び制御方法、並びに、ロボット用制御プログラム
Cherubini et al. Collaborative manufacturing with physical human–robot interaction
US9821459B2 (en) Multi-joint robot having function for repositioning arm
CN111479660B (zh) 包括确保停止时间和距离的安全***的机器人
US9579798B2 (en) Human collaborative robot system
JP6314426B2 (ja) ロボット制御装置およびロボット制御方法
JP4550849B2 (ja) アーム搭載移動ロボット
JP5942311B2 (ja) ロボット、ロボットの制御装置及び制御方法、並びに、ロボット用制御プログラム
KR102003216B1 (ko) 로봇을 위한 모터 제어 및/또는 조정
US10464210B2 (en) Method and device for open-loop/closed-loop control of a robot manipulator
KR101443502B1 (ko) 모터 구동 구조를 구비한 시스템, 프로그램을 기록한 컴퓨터 판독 가능한 기록 매체, 및 용접물 제조 방법
US11040449B2 (en) Robot control system and method of controlling a robot
JP2012051042A (ja) ロボットシステム及びロボット制御装置
JP7206638B2 (ja) ロボット、制御装置およびロボットの制御方法
US20200198133A1 (en) Method and apparatus for torque estimation
JP2008302496A (ja) ロボットアームの制御装置及び制御方法、ロボット、及びロボットアームの制御プログラム
US20140163735A1 (en) Robot system, robot, and robot control device
JP6831530B2 (ja) 外乱オブザーバ及びロボット制御装置
JP2015116663A (ja) ロボット装置の制御方法及びロボット装置
EP2426037A1 (en) Walking robot and control method thereof
JP5212754B2 (ja) 作業支援システム
JP2013169609A (ja) ロボットの衝突検出方法
JP2017177321A (ja) 制御装置及びロボットシステム
JP2017159427A (ja) 制御装置、ロボット、及びロボットシステム
JP7068133B2 (ja) 制御システム、制御方法、及び制御プログラム

Legal Events

Date Code Title Description
A621 Written request for application examination

Free format text: JAPANESE INTERMEDIATE CODE: A621

Effective date: 20160825

A977 Report on retrieval

Free format text: JAPANESE INTERMEDIATE CODE: A971007

Effective date: 20170622

A131 Notification of reasons for refusal

Free format text: JAPANESE INTERMEDIATE CODE: A131

Effective date: 20170627

A521 Request for written amendment filed

Free format text: JAPANESE INTERMEDIATE CODE: A523

Effective date: 20170824

TRDD Decision of grant or rejection written
A01 Written decision to grant a patent or to grant a registration (utility model)

Free format text: JAPANESE INTERMEDIATE CODE: A01

Effective date: 20171003

A61 First payment of annual fees (during grant procedure)

Free format text: JAPANESE INTERMEDIATE CODE: A61

Effective date: 20171017

R151 Written notification of patent or utility model registration

Ref document number: 6238021

Country of ref document: JP

Free format text: JAPANESE INTERMEDIATE CODE: R151