(57)【要約】  多関節ロボットアーム（５）と、アームに配設され外力を検出する外力検出手段（３）と、アームの関節の可動状態を計算する関節可動状態計算手段（２）と、関節可動状態計算手段により計算された可動状態に基づき外力検出手段が検出した外力を変換外力に変換する外力変換手段（２４）と、変換外力に基づくことでアーム（５）の動作が制限されるようアーム（５）を制御する制御手段（２５、２９）とをロボットが有する。
2011080856.tif 000002
【特許請求の範囲】
【請求項１】
多関節ロボットアームと、
前記多関節ロボットアームに配設され、かつ、外力を検出する外力検出手段と、
前記多関節ロボットアームの関節が可動範囲のいずれかに位置して可動状態を計算する関節可動状態計算手段と、
前記関節可動状態計算手段により計算された前記関節の前記可動状態に基づき、前記外力検出手段が検出した前記外力を、前記外力が前記ロボットアームに加えられたときに前記ロボットアームが動作可能な方向成分を抽出可能な変換外力に変換する外力変換手段と、
前記外力変換手段により変換された前記変換外力に基づいてインピーダンス制御を行うことにより、前記多関節ロボットアームの前記関節の前記可動状態に応じた動作制限が行われるように前記多関節ロボットアームを制御する制御手段とを有するロボット。
【請求項２】
前記外力変換手段は、前記外力検出手段に検出された前記外力を、前記多関節ロボットアームの前記関節が固定されたときに前記多関節ロボットアームの手先が動くことのできる拘束曲面の接平面への正射影として変換する請求項１に記載のロボット。
【請求項３】
さらに、前記多関節ロボットアームにかかる前記外力により各関節に発生する外力トルクを計算する外力トルク計算手段を有し、
前記関節可動状態計算手段は、前記外力トルク計算手段の計算結果に基づき前記外力トルクが前記多関節ロボットアームの各関節の動作を可動領域外より可動領域内に戻そうとする力であるかどうかを判定し、
前記関節可動状態計算手段が、前記外力トルクが前記多関節ロボットアームの各関節の動作を前記可動領域外より前記可動領域内に戻そうとする力であると判定する場合には、前記制御手段は前記動作制限を行わない一方、
前記関節可動状態計算手段が、前記外力トルクが前記多関節ロボットアームの各関節の動作を前記可動領域外より前記可動領域内に戻そうとする力ではないと判定する場合には、前記制御手段は前記動作制限を行う請求項１又は２に記載のロボット。
【請求項４】
前記関節可動状態計算手段は、前記外力を関節トルクに変換することにより、前記関節トルクに変換された前記変換外力が、前記多関節ロボットアームの各関節の動作を可動領域外より可動領域内に戻そうとする力であるかどうかを判定し、
前記関節可動状態計算手段が、前記変換外力が、前記多関節ロボットアームの各関節の動作を前記可動領域外より前記可動領域内に戻そうとする力であると判定する場合には、前記制御手段は前記動作制限を行わない一方、
前記関節可動状態計算手段が、前記変換外力が、前記多関節ロボットアームの各関節の動作を前記可動領域外より前記可動領域内に戻そうとする力ではないと判定する場合には、前記制御手段は前記動作制限を行う請求項１又は２に記載のロボット。
【請求項５】
前記多関節ロボットアームの各関節に対し、可動あるいは固定の動作モードを設定する動作モード設定部と、
前記動作モード設定部で設定された前記動作モードの指令が入力されて、前記動作モードと前記関節可動状態計算手段により計算された前記関節の前記可動状態とに基づき前記可動状態の計算を行う関節動作モード設定手段とをさらに備えて、
前記外力変換手段は、前記関節動作モード設定手段により計算された前記関節の前記可動状態に基づき、前記外力検出手段が検出した前記外力を、前記外力が前記ロボットアームに加えられたときに前記ロボットアームが動作可能な方向成分を抽出可能な変換外力に変換し、
前記制御手段は、前記変換外力に基づいてインピーダンス制御を行うことにより、前記多関節ロボットアームの前記関節の前記可動状態に応じた動作制限が行われる請求項１又は２に記載のロボット。
【請求項６】
前記関節可動状態計算手段は、前記関節の可動領域の両端の境界線から一定範囲だけ内側に緩衝領域をそれぞれ設け、前記各緩衝領域では前記関節の前記可動状態を徐々に変化させる請求項３に記載のロボット。
【請求項７】
前記多関節ロボットアームの前記関節の可動領域の両端の内側にそれぞれ設けられた緩衝領域と、前記緩衝領域を除く前記可動領域の残りの領域である完全可動領域ＰＭＲと、可動領域外の領域とに分割し、
前記関節可動状態計算手段は、各領域毎に、前記関節の前記可動状態の値を異ならせるように計算される請求項１に記載のロボット。
【請求項８】
前記関節可動状態計算手段と前記力変換手段とが第２制御手段として、前記多関節アームの前記制御手段とは別に備えられ、
前記力変換手段が変換した外力が前記第２制御手段より通信を通じて前記制御手段に入力される請求項１に記載のロボット。
【請求項９】
多関節ロボットアームを備えるロボットの制御装置であって、
前記多関節ロボットアームの関節が可動範囲のいずれかに位置して可動状態を計算する関節可動状態計算手段と、
前記関節可動状態計算手段により計算された前記関節の前記可動状態に基づき、前記多関節ロボットアームに配設されて外力を検出する外力検出手段が検出した前記外力を、前記外力が前記ロボットアームに加えられたときに前記ロボットアームが動作可能な方向成分を抽出可能な変換外力に変換する外力変換手段と、
前記外力変換手段により変換された前記変換外力に基づいてインピーダンス制御を行うことにより、前記多関節ロボットアームの前記関節の前記可動状態に応じた動作制限が行われるように前記多関節ロボットアームを制御する制御手段とを有するロボットの制御装置。
【請求項１０】
多関節ロボットアームを備えるロボットの制御方法であって、
前記多関節ロボットアームの関節が可動範囲のいずれかに位置して可動状態を関節可動状態計算手段で計算し、
前記計算された前記関節の前記可動状態に基づき、前記多関節ロボットアームに配設されて外力を検出する外力検出手段が検出した前記外力を、前記外力が前記ロボットアームに加えられたときに前記ロボットアームが動作可能な方向成分を抽出可能な変換外力に外力変換手段で変換し、
前記変換された変換外力に基づいてインピーダンス制御を行うことにより、前記多関節ロボットアームの前記関節の前記可動状態に応じた動作制限が行われるように前記多関節ロボットアームを制御手段で制御するロボットの制御方法。
【請求項１１】
多関節ロボットアームを備えるロボットの制御プログラムであって、
コンピュータを、
前記多関節ロボットアームの関節が可動範囲のいずれかに位置して可動状態を計算する関節可動状態計算手段と、
前記関節可動状態計算手段により計算された前記関節の前記可動状態に基づき、前記多関節ロボットアームに配設されて外力を検出する外力検出手段が検出した前記外力を、前記外力が前記ロボットアームに加えられたときに前記ロボットアームが動作可能な方向成分を抽出可能な変換外力に変換する外力変換手段と、
前記外力変換手段により変換された前記変換外力に基づいてインピーダンス制御を行うことにより、前記多関節ロボットアームの前記関節の前記可動状態に応じた動作制限が行われるように前記多関節ロボットアームを制御する制御手段として機能させるためのロボットの制御プログラム。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は、工場内、又は、家庭内、又は、介護現場等でパワーアシスト等の作業支援を行うロボットアームを備えるロボット等、人と協働するロボット、ロボットの制御装置、及び制御方法に関する。
【背景技術】
【０００２】
近年、工場等で人と協調して動作し、パワーアシスト等の作業支援を行う人協調型のロボットが注目されている。こうした人協調型のロボットは人の操作により動作するため、高度な自律機能がなくても実用的な作業が可能であり、また人の制御の下に動作するため安全性が高く、将来は工場内に留まらず、家庭内の生活支援又は介護への応用が期待されている。【０００３】
一方、人協調型のロボットは、事前プログラミング通りに動作する従来のロボットと異なり、人が操作する故に動作範囲を越えてしまうなど、事前に想定した以上の動作を人が行い、制御が不安定になったり、ロボットを故障させてしまう可能性がある。【０００４】
こうした課題に対し、従来技術としては、作動範囲を規制するリミッタを設けると共に、搬送物がリミッタを越えた場合には、作業者に所定の反力を伝えるアシスト搬送装置を開示している（特許文献１）。
【先行技術文献】
【特許文献】
【０００５】
【特許文献１】特開２００５−１４１３３号公報
【発明の概要】
【発明が解決しようとする課題】
【０００６】
しかしながら、前記従来技術では、ロボットアームの手先の作業範囲を規制する固定のリミッタを設ける方法であるため、ロボットアームの各関節の動作範囲の規制など、複数の関節の関係によって手先の可動範囲が複雑に変化する場合には、適用が困難である。【０００７】
また、前記従来技術は、リミッタを越える動作に対しては、バネ定数などの制御パラメータを変化させて反力を発生させることで、動作の制限を行うが、制御パラメータを可変とするために、制御系の構成が複雑になる可能性がある。また、制御パラメータを変化させることで制御系が不安定になり、ロボットの動作が不安定になる可能性がある。【０００８】
本発明の目的は、前記従来の制御装置の課題を解決し、ロボットアームの関節の動作範囲の規制など、関節の動作状況に応じたロボットアームの動作制限を、簡潔な制御系構造で実現し、制御系の不安定さを低減するロボット、ロボットの制御装置、及び制御方法を提供する。
【課題を解決するための手段】
【０００９】
前記目的を達成するために、本発明は以下のように構成する。【００１０】
本発明の第１態様によれば、多関節ロボットアームと、
前記多関節ロボットアームに配設され、かつ、外力を検出する外力検出手段と、
前記多関節ロボットアームの関節が可動範囲のいずれかに位置して可動状態を計算する関節可動状態計算手段と、
前記関節可動状態計算手段により計算された前記関節の前記可動状態に基づき、前記外力検出手段が検出した前記外力を、前記外力が前記ロボットアームに加えられたときに前記ロボットアームが動作可能な方向成分を抽出可能な変換外力に変換する外力変換手段と、
前記外力変換手段により変換された前記変換外力に基づいてインピーダンス制御を行うことにより、前記多関節ロボットアームの前記関節の前記可動状態に応じた動作制限が行われるように前記多関節ロボットアームを制御する制御手段とを有するロボットを提供する。【００１１】
本発明の第９態様によれば、多関節ロボットアームを備えるロボットの制御装置であって、
前記多関節ロボットアームの関節が可動範囲のいずれかに位置して可動状態を計算する関節可動状態計算手段と、
前記関節可動状態計算手段により計算された前記関節の前記可動状態に基づき、前記多関節ロボットアームに配設されて外力を検出する外力検出手段が検出した前記外力を、前記外力が前記ロボットアームに加えられたときに前記ロボットアームが動作可能な方向成分を抽出可能な変換外力に変換する外力変換手段と、
前記外力変換手段により変換された前記変換外力に基づいてインピーダンス制御を行うことにより、前記多関節ロボットアームの前記関節の前記可動状態に応じた動作制限が行われるように前記多関節ロボットアームを制御する制御手段とを有することを特徴とするロボットの制御装置を提供する。【００１２】
本発明の第１０態様によれば、多関節ロボットアームを備えるロボットの制御方法であって、
前記多関節ロボットアームの関節が可動範囲のいずれかに位置して可動状態を関節可動状態計算手段で計算し、
前記計算された前記関節の前記可動状態に基づき、前記多関節ロボットアームに配設されて外力を検出する外力検出手段が検出した前記外力を、前記外力が前記ロボットアームに加えられたときに前記ロボットアームが動作可能な方向成分を抽出可能な変換外力に外力変換手段で変換し、
前記変換された変換外力に基づいてインピーダンス制御を行うことにより、前記多関節ロボットアームの前記関節の前記可動状態に応じた動作制限が行われるように前記多関節ロボットアームを制御手段で制御することを特徴とするロボットの制御方法を提供する。【００１３】
本発明の第１１態様によれば、多関節ロボットアームを備えるロボットの制御プログラムであって、
コンピュータを、
前記多関節ロボットアームの関節が可動範囲のいずれかに位置して可動状態を計算する関節可動状態計算手段と、
前記関節可動状態計算手段により計算された前記関節の前記可動状態に基づき、前記多関節ロボットアームに配設されて外力を検出する外力検出手段が検出した前記外力を、前記外力が前記ロボットアームに加えられたときに前記ロボットアームが動作可能な方向成分を抽出可能な変換外力に変換する外力変換手段と、
前記外力変換手段により変換された前記変換外力に基づいてインピーダンス制御を行うことにより、前記多関節ロボットアームの前記関節の前記可動状態に応じた動作制限が行われるように前記多関節ロボットアームを制御する制御手段として機能させるためのロボットの制御プログラムを提供する。
【発明の効果】
【００１４】
本発明のロボット、ロボットの制御装置、及び制御方法によれば、関節可動状態計算手段及び力変換手段を有することにより、インピーダンス制御による動作時に人が関節の可動領域を越えるような操作を行っても、関節可動状態計算手段において関節が可動範囲のいずれかに位置して可動状態であるかどうか可動状態を判定し、外力変換手段により変換された変換外力をインピーダンス計算手段への入力として使うことで、ロボットアームの動作制限が可能となり、関節の可動領域を越えることを防ぐことができる。
【図面の簡単な説明】
【００１５】
本発明のこれらと他の目的と特徴は、添付された図面についての好ましい実施形態に関連した次の記述から明らかになる。この図面においては、【００１６】
【図１】図１は、本発明の第１実施形態におけるロボットの全体構成を示す図であり、
【図２】図２は、本発明の前記第１実施形態における前記ロボットのインピーダンス制御手段の構成を示すブロック図であり、
【図３】図３は、本発明の前記第１実施形態における前記ロボットによる人との協調搬送作業を説明する図であり、
【図４】図４は、関節角度ｑｉと可動状態の関係を示す図であり、
【図５】図５は、本発明の前記第１実施形態における前記ロボットにおけるインピーダンス制御手段の全体的な動作ステップを表すフローチャートであり、
【図６】図６は、本発明の前記第１実施形態における前記ロボットにおける拘束曲面及び力の変換を説明する図であり、
【図７】図７は、本発明の第２実施形態におけるロボットのインピーダンス制御手段の構成を示すブロック図であり、
【図８】図８は、本発明の前記第２実施形態における前記ロボットの全体構成を示す図であり、
【図９】図９は、本発明の第３実施形態におけるロボットのインピーダンス制御手段の構成を示すブロック図であり、
【図１０】図１０は、本発明の前記第３実施形態における前記ロボットによる人との協調搬送作業を説明する図であり、
【図１１】図１１は、本発明の前記第３実施形態における前記ロボットの動作の具体例を示す図であり、
【図１２】図１２は、式（１０３）に現れる並進外力ｆｈ、ｆの関係を示す図である。
【発明を実施するための形態】
【００１７】
以下に、本発明にかかる実施の形態を図面に基づいて詳細に説明する。
【００１８】
以下、図面を参照して本発明における実施形態を詳細に説明する前に、本発明の種々の態様について説明する。
【００１９】
本発明の第１態様によれば、多関節ロボットアームと、
前記多関節ロボットアームに配設され、かつ、外力を検出する外力検出手段と、
前記多関節ロボットアームの関節が可動範囲のいずれかに位置して可動状態を計算する関節可動状態計算手段と、
前記関節可動状態計算手段により計算された前記関節の前記可動状態に基づき、前記外力検出手段が検出した前記外力を、前記外力が前記ロボットアームに加えられたときに前記ロボットアームが動作可能な方向成分を抽出可能な変換外力に変換する外力変換手段と、
前記外力変換手段により変換された前記変換外力に基づいてインピーダンス制御を行うことにより、前記多関節ロボットアームの前記関節の前記可動状態に応じた動作制限が行われるように前記多関節ロボットアームを制御する制御手段とを有するロボットを提供する。
【００２０】
本発明の第２態様によれば、前記外力変換手段は、前記外力検出手段に検出された前記外力を、前記多関節ロボットアームの前記関節が固定されたときに前記多関節ロボットアームの手先が動くことのできる拘束曲面の接平面への正射影として変換する第１の態様に記載のロボットを提供する。
【００２１】
本発明の第３態様によれば、さらに、前記多関節ロボットアームにかかる前記外力により各関節に発生する外力トルクを計算する外力トルク計算手段を有し、
前記関節可動状態計算手段は、前記外力トルク計算手段の計算結果に基づき前記外力トルクが前記多関節ロボットアームの各関節の動作を可動領域外より可動領域内に戻そうとする力であるかどうかを判定し、
前記関節可動状態計算手段が、前記外力トルクが前記多関節ロボットアームの各関節の動作を前記可動領域外より前記可動領域内に戻そうとする力であると判定する場合には、前記制御手段は前記動作制限を行わない一方、
前記関節可動状態計算手段が、前記外力トルクが前記多関節ロボットアームの各関節の動作を前記可動領域外より前記可動領域内に戻そうとする力ではないと判定する場合には、前記制御手段は前記動作制限を行う第１又は２の態様に記載のロボットを提供する。
【００２２】
本発明の第４態様によれば、前記関節可動状態計算手段は、前記外力を関節トルクに変換することにより、前記関節トルクに変換された前記変換外力が、前記多関節ロボットアームの各関節の動作を可動領域外より可動領域内に戻そうとする力であるかどうかを判定し、
前記関節可動状態計算手段が、前記変換外力が、前記多関節ロボットアームの各関節の動作を前記可動領域外より前記可動領域内に戻そうとする力であると判定する場合には、前記制御手段は前記動作制限を行わない一方、
前記関節可動状態計算手段が、前記変換外力が、前記多関節ロボットアームの各関節の動作を前記可動領域外より前記可動領域内に戻そうとする力ではないと判定する場合には、前記制御手段は前記動作制限を行う第１又は２の態様に記載のロボットを提供する。
【００２３】
本発明の第５態様によれば、前記多関節ロボットアームの各関節に対し、可動あるいは固定の動作モードを設定する動作モード設定部と、
前記動作モード設定部で設定された前記動作モードの指令が入力されて、前記動作モードと前記関節可動状態計算手段により計算された前記関節の前記可動状態とに基づき前記可動状態の計算を行う関節動作モード設定手段とをさらに備えて、
前記外力変換手段は、前記関節動作モード設定手段により計算された前記関節の前記可動状態に基づき、前記外力検出手段が検出した前記外力を、前記外力が前記ロボットアームに加えられたときに前記ロボットアームが動作可能な方向成分を抽出可能な変換外力に変換し、
前記制御手段は、前記変換外力に基づいてインピーダンス制御を行うことにより、前記多関節ロボットアームの前記関節の前記可動状態に応じた動作制限が行われる第１〜４のいずれか１つの態様に記載のロボットを提供する。
【００２４】
本発明の第６態様によれば、前記関節可動状態計算手段は、前記関節の可動領域の両端の境界線から一定範囲だけ内側に緩衝領域をそれぞれ設け、前記各緩衝領域では前記関節の前記可動状態を徐々に変化させる第３又は４の態様に記載のロボットを提供する。
【００２５】
本発明の第７態様によれば、前記多関節ロボットアームの前記関節の可動領域の両端の内側にそれぞれ設けられた緩衝領域と、前記緩衝領域を除く前記可動領域の残りの領域である完全可動領域ＰＭＲと、可動領域外の領域とに分割し、
前記関節可動状態計算手段は、各領域毎に、前記関節の前記可動状態の値を異ならせるように計算される第１の態様に記載のロボットを提供する。
【００２６】
本発明の第８態様によれば、前記関節可動状態計算手段と前記力変換手段とが第２制御手段として、前記多関節アームの前記制御手段とは別に備えられ、
前記力変換手段が変換した外力が前記第２制御手段より通信を通じて前記制御手段に入力される第１の態様に記載のロボットを提供する。
【００２７】
本発明の第９態様によれば、多関節ロボットアームを備えるロボットの制御装置であって、
前記多関節ロボットアームの関節が可動範囲のいずれかに位置して可動状態を計算する関節可動状態計算手段と、
前記関節可動状態計算手段により計算された前記関節の前記可動状態に基づき、前記多関節ロボットアームに配設されて外力を検出する外力検出手段が検出した前記外力を、前記外力が前記ロボットアームに加えられたときに前記ロボットアームが動作可能な方向成分を抽出可能な変換外力に変換する外力変換手段と、
前記外力変換手段により変換された前記変換外力に基づいてインピーダンス制御を行うことにより、前記多関節ロボットアームの前記関節の前記可動状態に応じた動作制限が行われるように前記多関節ロボットアームを制御する制御手段とを有することを特徴とするロボットの制御装置を提供する。
【００２８】
本発明の第１０態様によれば、多関節ロボットアームを備えるロボットの制御方法であって、
前記多関節ロボットアームの関節が可動範囲のいずれかに位置して可動状態を関節可動状態計算手段で計算し、
前記計算された前記関節の前記可動状態に基づき、前記多関節ロボットアームに配設されて外力を検出する外力検出手段が検出した前記外力を、前記外力が前記ロボットアームに加えられたときに前記ロボットアームが動作可能な方向成分を抽出可能な変換外力に外力変換手段で変換し、
前記変換された変換外力に基づいてインピーダンス制御を行うことにより、前記多関節ロボットアームの前記関節の前記可動状態に応じた動作制限が行われるように前記多関節ロボットアームを制御手段で制御することを特徴とするロボットの制御方法を提供する。
【００２９】
本発明の第１１態様によれば、多関節ロボットアームを備えるロボットの制御プログラムであって、
コンピュータを、
前記多関節ロボットアームの関節が可動範囲のいずれかに位置して可動状態を計算する関節可動状態計算手段と、
前記関節可動状態計算手段により計算された前記関節の前記可動状態に基づき、前記多関節ロボットアームに配設されて外力を検出する外力検出手段が検出した前記外力を、前記外力が前記ロボットアームに加えられたときに前記ロボットアームが動作可能な方向成分を抽出可能な変換外力に変換する外力変換手段と、
前記外力変換手段により変換された前記変換外力に基づいてインピーダンス制御を行うことにより、前記多関節ロボットアームの前記関節の前記可動状態に応じた動作制限が行われるように前記多関節ロボットアームを制御する制御手段として機能させるためのロボットの制御プログラムを提供する。
【００３０】
以下に、本発明にかかる実施の形態を図面に基づいて詳細に説明する。
【００３１】
（第１実施形態）
図１は本発明の第１実施形態のロボット９０の構成を示す図である。ロボット９０は、多関節ロボットアーム５と、多関節ロボットアーム５の動作を制御する制御装置１とを備えて構成されている。
【００３２】
制御装置１は、ハードウェア的には一般的なパーソナルコンピュータにより構成されている。そして、インピーダンス制御手段（インピーダンス制御部）４の入出力ＩＦ１９を除く部分は、パーソナルコンピュータで実行される制御プログラム１７としてソフトウェア的に実現される。
【００３３】
入出力ＩＦ１９は、パーソナルコンピュータのＰＣＩバスなどの拡張スロットルに接続された、Ｄ／Ａボード２０と、Ａ／Ｄボード２１と、カウンタボード２２とにより構成される。
【００３４】
ロボット９０の多関節ロボットアーム５の動作を制御するための制御プログラム１７が実行されることにより、制御装置１が機能する。ロボットアーム５の各関節１１，１２，１３，１４，１５，１６のそれぞれのエンコーダ４２より出力される関節角度情報がカウンタボード２２を通じて制御装置１に取り込まれ、制御装置１によって各関節１１，１２，１３，１４，１５，１６の回転動作での制御指令値が算出される。算出された各制御指令値は、Ｄ／Ａボード２０を通じてモータードライバ１８に与えられ、モータードライバ１８から送られた各制御指令値に従って、ロボットアーム５の各関節１１，１２，１３，１４，１５，１６のモータ４１が駆動される。
【００３５】
ロボットアーム５は、６自由度の多リンクマニピュレータであり、手先の一例として機能するハンド６と、ハンド６が取付けられる手首部７を有する前腕リンク８と、前腕リンク８が回転可能に連結される上腕リンク９と、上腕リンク９が回転可能に連結支持される台部１０とを有している。手首部７は、第４関節１４と、第５関節１５と、第６関節１６との３つの回転軸を有しており、上腕リンク９に対するハンド６の相対的な姿勢（向き）を変化させることができる。前腕リンク８の他端は、上腕リンク９の一端に対して第３関節１３周りに回転可能とし、上腕リンク９の他端は、台部１０に対して第２関節１２周りに回転可能とする。そして台部の上側可動部１０ａは台部の下側固定部１０ｂに対して第１関節１１周りに回転可能として、合計６個の軸周りに回転可能として前記６自由度の多リンクマニピュレータを構成している。
【００３６】
各軸の回転部分を構成する各関節１１，１２，１３，１４，１５，１６は、各関節１１，１２，１３，１４，１５，１６の一方の部材に設けられた回転駆動装置（本第１実施形態ではモータ４１）と、モータ４１の回転軸の回転位相角（すなわち関節角）を検出するエンコーダ４２とを備える。そして、モータ４１の回転軸が関節の他方の部材に連結されて前記回転軸を正逆回転させることにより、他方の部材を一方の部材に対して各軸周りに回転可能とする。回転駆動装置は、後述するモータードライバ１８により駆動制御される。本第１実施形態では、回転駆動装置の一例としてのモータ４１とエンコーダ４２とは、ロボットアーム５の各関節１１，１２，１３，１４，１５，１６の内部に配設されている。
【００３７】
３５は台部１０の下側固定部１０ｂに対して相対的な位置関係が固定された絶対座標系であり、３６はハンド６に対して相対的な位置関係が固定された手先座標系である。絶対座標系３５から見た手先座標系３６の原点位置Ｏｅ（ｘ、ｙ、ｚ）をロボットアーム５の手先位置とし、絶対座標系３５から見た手先座標系３６の姿勢をロール・ピッチ・ヨー角で表現した（φ、θ、ψ）をロボットアーム５の手先姿勢とし、手先位置・姿勢ベクトルをｒ＝［ｘ、ｙ、ｚ、φ、θ、］Ｔと定義する。ロボットアーム５の手先位置・姿勢を制御する場合には、手先位置・姿勢ベクトルｒを目標手先位置・姿勢ベクトルｒｄに追従させることになる。
【００３８】
ロボットアーム５の手首部７とハンド６の間には、外力検出手段（外力検出部又は外力検出装置）の一例として機能する力センサー３が配設されており、ハンド６にかかる外力を検出できる。
【００３９】
次に、図２を使い、インピーダンス制御手段４の詳細について説明する。図２において、５は図１に示したロボットアームである。ロボットアーム５からはそれぞれの関節軸のエンコーダ４２により計測された関節角の現在値（関節角度ベクトル）ｑ＝［ｑ１，ｑ２，ｑ３，ｑ４，ｑ５，ｑ６］Ｔが出力され、カウンタボード２２によりインピーダンス制御手段４に取り込まれる。ただし、ｑ１，ｑ２，ｑ３，ｑ４，ｑ５，ｑ６は、それぞれ、第１関節１１、第２関節１２、第３関節１３、第４関節１４、第５関節１５、第６関節１６の関節角度である。
【００４０】
また、ロボットアーム５からは力センサー３により計測された外力ＦＳ＝［ｆｘ，ｆｙ，ｆｚ，ｎｘ，ｎｙ，ｎｚ］Ｔが出力され、Ａ／Ｄボード２１によりインピーダンス制御手段４に取り込まれる。
【００４１】
目標軌道生成手段（目標軌道生成部）２３は、目標とするロボットアーム５の動作を実現するための手先位置・姿勢目標ベクトルｒｄを出力する。目標とするロボットアーム５の動作は、目的とする作業に応じて、事前に、ポイントごとの位置（ｒｄ０、ｒｄ１、ｒｄ２、・・・）が時系列データとして与えられている。目標軌道生成手段２３は、多項式補間を使用し、各ポイント間の軌道を補完し、手先位置・姿勢目標ベクトルｒｄを生成する。
【００４２】
インピーダンス計算手段（インピーダンス計算部）２５は、ロボットアーム５に機械インピーダンスを実現する機能を果たす部分であり、ロボットアーム５が目標軌道生成手段２３の生成する目標軌道に沿うよう位置制御により単独で動作する場合は０を出力する。一方、インピーダンス計算手段２５は、ロボットアーム５と人間が協働作業をする場合には、（ｉ）設定したインピーダンスパラメータＭ、Ｄ、Ｋ（慣性Ｍ、粘性Ｄ、剛性Ｋ）と、（ｉｉｉ）インピーダンス制御手段４（インピーダンス制御手段４の重力補償手段３０）へ入力される外力Ｆを変換した変換外力Ｆｈとにより、ロボットアーム５に機械インピーダンスを実現するための手先位置・姿勢目標補正出力ｒｄΔを以下の式（１）により計算し、出力する。手先位置・姿勢目標補正出力ｒｄΔは、目標軌道生成手段２３の出力する手先位置・姿勢目標ｒｄに第１演算部６１で加算され、手先位置・姿勢補正目標ベクトルｒｄｍが生成される。
【００４３】
【数１】
2011080856.tif 000003
．．．．．（１）
ただし、
【００４４】
【数２】
2011080856.tif 000004
．．．．．（２）
【００４５】
【数３】
2011080856.tif 000005
．．．．．（３）
【００４６】
【数４】
2011080856.tif 000006
．．．．．（４）
であり、ｓはラプラス演算子である。
【００４７】
順運動学計算手段２６は、ロボットアーム５から出力された各関節角の現在値ｑである関節角度ベクトルｑが入力され、ロボットアーム５の関節角度ベクトルｑから手先位置・姿勢ベクトルｒへの変換の幾何学的計算を行う。
【００４８】
順運動学計算手段２６により計算される手先位置・姿勢ベクトルｒと、手先位置・姿勢補正目標ベクトルｒｄｍとの誤差ｒｅが第２演算部６２で求められる。
【００４９】
第２演算部６２で求められた誤差ｒｅが位置誤差補償手段２７に入力され、位置誤差補償出力ｕｒｐが位置誤差補償手段２７で求められて、近似逆運動学計算手段２８に出力される。
【００５０】
近似逆運動学計算手段２８は、近似式
【数５】
2011080856.tif 000007
を用いて、逆運動学の近似計算を行う。ただし、Ｊｒ（ｑ）は
【数６】
2011080856.tif 000008
の関係を満たすヤコビ行列、ｕｉｎは近似逆運動学計算手段２８への入力、ｕｏｕｔは近似逆運動学計算手段２８からの出力である。ここで、
【数７】
2011080856.tif 000009
と近似して考えることができるので、ヤコビ行列の定義式
【数８】
2011080856.tif 000010
に代入すると、
【数９】
2011080856.tif 000011
が成り立つことが分かる。すなわち、Ｊｒ（ｑ）−１を近似逆運動学計算手段２８で乗算することで手先位置・姿勢に関する誤差値から関節角度に関する誤差値への変換となる。したがって、手先位置・姿勢誤差ｒｅにゲインをかける等した値である位置誤差補償出力ｕｒｅが近似逆運動学計算手段２８に入力されると、その出力として、関節角度誤差ｑｅを補償するための関節角度誤差補償出力ｕｑｅが近似逆運動学計算手段２８から出力されることになる。
【００５１】
関節角度誤差補償出力ｕｑｅは、近似逆運動学計算手段２８からＤ／Ａボード２０を介してモータードライバ１８に電圧指令値として与えられ、各関節軸が正逆回転駆動されてロボットアーム５が動作する。
【００５２】
以上のように構成されるインピーダンス制御手段４の基本構成に関して動作の基本原理を説明する。
【００５３】
動作の基本は、位置誤差補償手段２７による手先位置・姿勢誤差ｒｅのフィードバック制御（位置制御）であり、図２の点線で囲まれた部分が位置制御系２９になっている。位置誤差補償手段２７として、例えば、ＰＩＤ補償器を使用すれば、手先位置・姿勢誤差ｒｅが０に収束するように制御が働き、目標とするロボットアーム５の動作が実現する。
【００５４】
インピーダンス制御を行う場合、前記説明した位置制御系２９に対し、インピーダンス計算手段２５により手先位置・姿勢目標補正出力ｒｄΔが第１演算部６１で加算されて手先位置・姿勢の目標値の補正が行われる。このために、前記した位置制御系２９は、手先位置・姿勢の目標値が本来の値より微妙にずれることになり、結果的に機械インピーダンスが実現される。以上の図２の一点鎖線で囲まれた部分は、位置制御ベースのインピーダンス制御系４９と呼ばれる構成であり、このインピーダンス制御系４９により慣性Ｍ、粘性Ｄ、剛性Ｋの機械インピーダンスが実現される。インピーダンス制御手段４は制御手段（制御部）（又は動作制御手段、又は動作制御部）の一例として機能する。
【００５５】
以上のインピーダンス制御を利用すれば、図３に示すような人１００とロボット９０の物体３８の協調搬送のような協働作業が可能となる。人１００が物品３８を移動しようとして、物体３８に力をかけると、物体３８を通じて力がロボット９０のロボットアーム５に伝わり、前記力が、ロボットアーム５の力センサー３により外力Ｆｓとして検出される。インピーダンス制御手段４への入力となる外力Ｆは、協調搬送する物体３８の重力を補償するため、重力補償手段３０において、力センサー３により検出された外力Ｆｓを基に式（５）で計算を行い、算出されるＦを使用する。
【００５６】
【数１０】
2011080856.tif 000012
．．．．．（５）
ただし、ｍは物体３８の質量、ｇは物体３８の重力加速度である。式（５）で計算される外力Ｆを入力にインピーダンス制御を行えば、ロボットアーム５は人１００がかけた力の方向に沿うように動作するため、協調搬送が実現する。なお、物体３８の質量ｍ、及び、物体３８の重力加速度ｇは、重力補償手段３０に予め記憶されている。
【００５７】
本発明の第１実施形態のロボットの制御装置１の特徴は、以上の基本的なインピーダンス制御手段４の構成に加え、関節可動状態計算手段２及び力変換手段２４を有するところにあり、以下、関節可動状態計算手段２及び力変換手段２４の詳細について説明する。
【００５８】
関節可動状態計算手段２は、ロボットアーム５の関節角の現在値（関節角度ベクトル）ｑが入力されることで各関節１１，１２，１３，１４，１５，１６の可動状態を判定し（具体的には、第１〜第６関節１１，１２，１３，１４，１５，１６のそれぞれの関節又は少なくとも１つの関節が可動範囲のいずれかに位置して可動状態を計算し）、その判定結果（計算結果）を力変換手段（力変換部、又は外力変換手段、又は外力変換部）２４へと出力する。
【００５９】
図４は関節角度ｑｉ（以下、ｉ＝１，２，・・・，６）と可動状態の関係を示す図であり、紙面左回りが関節ｉの正方向の回転を表している。関節可動状態計算手段２は、各関節１１，１２，１３，１４，１５，１６の可動範囲の最小値ｑｉＭＩＮ及び最大値ｑｉＭＡＸ、及び緩衝領域幅ｄｑｉにより、可動領域ＭＲと、可動領域ＭＲの両端の境界線から一定範囲だけ内側に配置された第１緩衝領域ＢＲ１と第２緩衝領域ＢＲ２とを定義する。第１緩衝領域ＢＲ１と第２緩衝領域ＢＲ２は、それぞれ、前記関節の前記可動状態を徐々に変化させる領域である。そして、関節可動状態計算手段２は、現在の関節角ｑｉが属する領域に応じてパターン分けし、各関節１１，１２，１３，１４，１５，１６の可動状態Ｌｉを０〜１の間の値として計算する。
【００６０】
さらに、インピーダンス制御手段４には、多関節ロボットアーム５にかかる外力Ｆにより各関節に発生する外力トルクτを計算する外力トルク計算手段５０を備えている。具体的には、外力トルク計算手段５０において、式（６）により外力Ｆによる各関節１１，１２，１３，１４，１５，１６に発生するトルクτ＝［τ１，τ２，τ３，τ４，τ５，τ６］Ｔを計算する。そして、関節可動状態計算手段２において、各関節１１，１２，１３，１４，１５，１６が緩衝領域ＢＲ１，ＢＲ２内あるいは可動領域外にあるときに、前記外力トルク計算手段５０で計算されて求められたトルクτが、各関節を可動領域ＭＲ内へ戻そうとする方向のトルクであると関節可動状態計算手段２で判定される場合（図４において矢印Ｘ及びＹの方向、後述のパターンＢ２，Ｃ２，Ｄ２，Ｅ２の場合）、予め定めた一定のオフセット値ｄＬｉを可動状態Ｌｉに関節可動状態計算手段２で加算する。また、関節可動状態計算手段２において、各関節が緩衝領域ＢＲ１，ＢＲ２内あるいは可動領域外にあるときに、前記計算されて求められたトルクが各関節を可動領域ＭＲ内へ戻そうとする方向のトルクではないと関節可動状態計算手段２で判定される場合には、オフセット値ｄＬｉは可動状態Ｌｉに関節可動状態計算手段２で加算しない。
【００６１】
【数１１】
2011080856.tif 000013
．．．．．（６）
前記式（６）において、Ｊｖは
【数１２】
2011080856.tif 000014
の関係を満たすヤコビ行列であり、
【数１３】
2011080856.tif 000015
で、
【数１４】
2011080856.tif 000016
はロボットアーム５の手先の回転速度である。
【００６２】
オフセット値ｄＬｉの具体的な数値は、操作のしやすさ、又は、制御の安定性を考慮して、実際にロボットアーム５を動作させて予め実験的に定めて、関節可動状態計算手段２内に記憶させることになる。
【００６３】
オフセット値ｄＬｉは、後述するように、関節ｉを可動領域ＭＲ内へ戻そうとするトルクに対する関節ｉの反応を関節可動状態計算手段２で決定し、かつ、可動領域ＭＲの境界で発生するリミットサイクルのような現象の発生度合いを決める値となる。具体的には、オフセット値ｄＬｉの値が大きくなるほど可動領域ＭＲへ戻そうとする力に対する反応が良くなり、操作が容易になる一方で、オフセット値ｄＬｉの値が大きくなるほどリミットサイクルのような現象により不安定性が増大することになる。したがって、オフセット値ｄＬｉの値の具体的な数値は、前記相反する反応の良さからくる操作性と制御の安定性とを考慮し、実際にロボットアーム５を動作させて予め実験的に定めて関節可動状態計算手段２内に記憶させることになる。
【００６４】
ここで言う「操作のしやすさ」とは、オフセット値ｄＬｉの値が小さいほど、大きな力をかけないと戻らなくなるので、操作性が悪いのに対し、オフセット値ｄＬｉの値が大きいほど、小さな力で容易に戻ることになるので、操作性が良いと呼んでいる。一方、オフセット値ｄＬｉの値が大きいほど、リミットサイクルのような現象が起こり、具体的には振動が発生し、制御が不安定になります。すなわち、ここで言う「制御の安定性」とは、制御が原因で振動が発生し運動が安定しないことを意味する。
【００６５】
以上を具体的に記述すると、以下のようにパターン分けして、関節ｉの可動状態Ｌｉが関節可動状態計算手段２で計算される。
【００６６】
パターンＡ：  （ｑｉＭＩＮ＋ｄｑｉ）≦ｑｉ≦（ｑｉＭＡＸ−ｄｑｉ）：すなわち、（完全可動領域ＰＭＲ内）のとき：Ｌｉ＝１
パターンＢ：  （ｑｉＭＩＮ）＜ｑｉ＜（ｑｉＭＩＮ＋ｄｑｉ）：すなわち、（第１緩衝領域ＢＲ１内）のとき：
パターンＢ１：  τｉ≦０の場合：  Ｌｉ＝（ｑｉ−ｑｉＭＩＮ）／ｄｑｉ
パターンＢ２：  τｉ＞０の場合：  Ｌｉ＝（ｑｉ−ｑｉＭＩＮ）／ｄｑｉ＋ｄＬｉ
（ただし、  Ｌｉ≦１に制限）
パターンＣ：  （ｑｉＭＡＸ−ｄｑｉ）＜ｑｉ＜（ｑｉＭＡＸ）：すなわち、（第２緩衝領域ＢＲ２内）のとき：
パターンＣ１：  τｉ≧０の場合：  Ｌｉ＝（ｑｉＭＡＸ−ｑｉ）／ｄｑｉ
パターンＣ２：  τｉ＜０の場合：  Ｌｉ＝（ｑｉＭＡＸ−ｑｉ）／ｄｑｉ＋ｄＬｉ
（ただし、  Ｌｉ≦１に制限）
パターンＤ：  ｑｉ≦ｑｉＭＩＮ：すなわち、（可動領域外）のとき：
パターンＤ１：  τｉ≦０の場合：  Ｌｉ＝０
パターンＤ２：  τｉ＞０の場合：  Ｌｉ＝ｄＬｉ
パターンＥ：  ｑｉＭＡＸ≦ｑｉ：すなわち、（可動領域外）のとき：
パターンＥ１：  τｉ≧０の場合：  Ｌｉ＝０
パターンＥ２：  τｉ＜０の場合：  Ｌｉ＝ｄＬｉ
力変換手段２４は、関節可動状態計算手段２の出力である可動状態Ｌｉに基づき、外力Ｆを、前記外力Ｆがロボットアーム５に加えられたときにロボットアーム５が動作可能な方向成分を抽出可能な変換外力Ｆｈに変換する。
【００６７】
変換外力Ｆｈの変換方法に関しては、２つの方法を以下に示す。
【００６８】
第１の変換方法は、以下の式（７）〜（１３）式に基づき変換する方法である。
【００６９】
【数１５】
2011080856.tif 000017
．．．．．（７）
【数１６】
2011080856.tif 000018
．．．．．（８）
【数１７】
2011080856.tif 000019
．．．．．（９）
【数１８】
2011080856.tif 000020
．．．．．（１０）
【数１９】
2011080856.tif 000021
．．．．．（１１）
【数２０】
2011080856.tif 000022
．．．．．（１２）
ただし、ｊ１，ｊ２，ｊ３は式（１３）を満たす３次元ベクトルであり、３×３行列［ｊ１  ｊ２  ｊ３］は
【数２１】
2011080856.tif 000023
の関係を満たすヤコビ行列Ｊｒ（ｑ）の左上の３×３の部分行列に相当する。
【００７０】
【数２２】
2011080856.tif 000024
．．．．．（１３）
第２の変換方法は、関節の可動状態のパターン分類である、パターンＡ〜Ｅに応じてパターン分けする方法である。
【００７１】
パターンα：  関節角度ｑ１、ｑ２、ｑ３のすべてがパターンＡの場合、
【数２３】
2011080856.tif 000025
．．．．．（１０１）
となる。
【００７２】
パターンβ：  関節角度ｑ１、ｑ２、ｑ３のいずれか１つの関節角度ｑがパターンＢ〜Ｅのいずれかの場合（すなわち、関節角度ｑ１、ｑ２、ｑ３のその他の２つの関節角度ｑがパターンＡの場合）、
例えば、関節角度ｑｉがパターンＢ〜Ｅのいずれかであり、かつ、関節角度ｑａ、ｑｂがパターンＡとすると（ただし、この例では、ｑｉは、ｑ１、ｑ２、ｑ３のいずれか１つの関節角度を意味し、ｑａ、ｑｂは、関節角度ｑ１、ｑ２、ｑ３のその他の２つの関節角度を意味する。）、
【００７３】
【数２４】
2011080856.tif 000026
．．．．．（１０２）
となる。
【００７４】
パターンγ：  関節角度ｑ１、ｑ２、ｑ３のいずれか２つの関節角度ｑがパターンＢ〜Ｅのいずれかの場合（すなわち、関節角度ｑ１、ｑ２、ｑ３の残りの１つの関節角度ｑがパターンＡの場合）、
例えば、関節角度ｑｉがパターンＡであり、かつ、関節角度ｑａ、ｑｂがパターンＢ〜Ｅのいずれかであるとすると（ただし、この例では、ｑｉは、関節角度ｑ１、ｑ２、ｑ３のいずれか１つの関節角度を意味し、ｑａ、ｑｂは、関節角度ｑ１、ｑ２、ｑ３のその他の２つの関節角度を意味する。）、
【数２５】
2011080856.tif 000027
．．．．．（１０３）
となる。
【００７５】
パターンδ：  関節角度ｑ１、ｑ２、ｑ３のすべてがパターンＢ〜Ｅのいずれかの場合、
【数２６】
2011080856.tif 000028
．．．．．（１０４）
となる。
【００７６】
その他の式（７）、式（１１）、式（１２）は第１の変換方法と同様である。
【００７７】
上記第１の変換方法は、変換式のパターン分けをする必要が無く、簡潔なアルゴリズムとすることができるという利点を有するが、図１に示すロボットアーム５の第２関節１２，第３関節１３のように平行な関節軸を持つアーム構造の場合に、平行な関節軸の関節が同時に完全可動領域外になる場合に限り対応することはできない。
【００７８】
一方、第２の変換方法は、変換式のパターン分けをする必要があるが、アームの構造に関係なく、複数の関節が完全可動領域外になる場合も含め、あらゆる場合に対応可能である。
【００７９】
したがって、アームの構造又は各関節の可動範囲の大きさ等に応じて、上記第１の変換方法と第２の変換方法を選択するように、力変換手段２４において、予め設定しておけばよい。
【００８０】
図５は、上述した原理に基づく制御プログラムによるインピーダンス制御時の動作ステップを説明するフローチャートである。
【００８１】
ステップＳ１では、エンコーダ４２により計測された関節角度データ（関節角度ベクトルｑ）が制御装置１に取り込まれる。
【００８２】
ステップＳ２では、ロボットアーム５の運動学計算に必要なヤコビ行列Ｊｒ等の計算が近似逆運動学計算手段２８により行われる。
【００８３】
ステップＳ３では、ロボットアーム５からの関節角度データ（関節角度ベクトルｑ）から、ロボットアーム５の現在の手先位置・姿勢ベクトルｒが順運動学計算手段２６により計算される（順運動学計算手段２６での処理）。
【００８４】
ステップＳ４では、関節可動状態計算手段２において、ロボットアーム５からの関節角度データ（関節角度ベクトルｑ）に基づいて、パターンＡ〜パターンＥのいずれになるかを判定し、関節可動状態Ｌｉを算出する。
【００８５】
ステップＳ５では、力センサー３の計測値Ｆｓが重力補償手段３０に取り込まれ、式（５）に基づき外力Ｆが重力補償手段３０で計算される。
【００８６】
ステップＳ６では、力変換手段２４において、式（７）〜式（１３）（第１の変換方法）、又は、式（７）、式（１１）、式（１２）、式（１０１）〜式（１０４）（第２の変換方法）に基づいて、重力補償手段３０で計算された外力Ｆが変換外力Ｆｈに変換される。
【００８７】
ステップＳ７では、機械インピーダンスパラメータＭ、Ｄ、Ｋと、関節角度データ（関節角度ベクトルｑ）と、力変換手段２４において変換された変換外力Ｆｈとから、手先位置・姿勢目標補正出力ｒｄΔが、インピーダンス計算手段２５により計算される（インピーダンス計算手段２５での処理）。
【００８８】
ステップＳ８では、手先位置・姿勢目標ベクトルｒｄと手先位置・姿勢目標補正出力ｒｄΔの和が第１演算部６１で計算されて求められた手先位置・姿勢補正目標ベクトルｒｄｍと、現在の手先位置・姿勢ベクトルｒとの差である手先位置・姿勢の誤差ｒｅが、第２演算部６２で計算される（位置誤差補償手段２７での処理）。位置誤差補償手段２７の具体例としてはＰＩＤ補償器が考えられる。定数の対角行列である比例、微分、積分の３つのゲインを適切に調整することにより、位置誤差が０に収束するように制御が働く。
【００８９】
ステップＳ９では、ステップＳ２で計算したヤコビ行列Ｊｒの逆行列を乗算することにより、位置誤差補償出力ｕｒｅを手先位置・姿勢の誤差に関する値から、関節角度の誤差に関する値である関節角度誤差補償出力ｕｑｅに、近似逆運動学計算手段２８により変換する（近似逆運動学計算手段２８での処理）。
【００９０】
ステップＳ１０では、関節角度誤差補償出力ｕｑｅが、近似逆運動学計算手段２８からＤ／Ａボード２０を通じてモータードライバ１８に与えられ、各モータ４１を流れる電流量を変化させることによりロボットアーム５の各関節の関節軸の回転運動が発生する。
【００９１】
以上のステップＳ１〜ステップＳ１０が、制御の計算ループとして繰り返し実行されることにより、ロボットアーム５の動作の制御が実現する。なお、ステップＳ２〜ステップＳ６の動作の順番はパラレル処理でも可能であり、必ずしも、この順番通りでなくてもよい。
【００９２】
本発明の前記第１実施形態にかかるロボット９０では、関節可動状態計算手段２及び力変換手段２４を備えることにより、以下に詳述するような動作及び効果が実現する。以下では、人１００とロボット９０が協調搬送を行う場合の動作を第３関節１３を中心に説明を行う。
【００９３】
図６は、式（８）に現れる並進外力ｆｈ、ｆｈ２の関係を示す。図６においてはロボットアーム５の第１関節１１から第３関節１３までを模式的に表している。拘束曲面３１は、第３関節１３が可動領域ＭＲを越えて、それ以上動けずロックされた状態の場合に、その他の第１関節１１及び第２関節１２が動くことで、ロボットアーム５の手先のハンド６が動くことのできる曲面である。拘束曲面３１は、実際は球面であるが、図６では、ある一断面の円のみ描いている。すなわち、第３関節１３がロック状態になると、ロボットアーム５の手先は拘束曲面３１に拘束される。
【００９４】
この拘束曲面３１に接する平面であり、かつ、ロボットアーム５の手先を通る平面が、接平面３２である。また、接平面３２は、ベクトルｊ１，ｊ２を含む平面に平行な平面である。
【００９５】
この接平面３２への並進外力ｆｈ２の正射影が、ｆｈＣである。正射影ｆｈＣは、式（８）において第３関節１３の可動状態Ｌ３＝０のときの並進外力ｆｈであり、式（１４）で表される。なお、式（１４）は式（８）において「｛」と「｝」とで囲まれた部分であり、式（８）を使って、変換外力Ｆｈを計算することは、正射影への変換を使用していることになる。また、正射影への変換を、式（８）だけでなく、式（９）、式（１０）の「｛」と「｝」とで囲まれた部分も、それぞれ、同様に、正射影への変換を使用していることになる。また、正射影ｆｈＣは変換外力Ｆｈの一例である。すなわち、拘束曲面３１の接平面３２への正射影を考えれば、ロボットアーム５が動作可能な方向成分となる。
【００９６】
【数２７】
2011080856.tif 000029
．．．．．（１４）
また、並進外力ｆｈは、並進外力ｆｈ２のベクトルの先端と、並進外力ｆｈ２の正射影ｆｈＣのベクトルの先端とを結んだ直線（点線）上に、先端が来る中間的な値であり、その位置は可動状態Ｌ３により定義される（可動状態Ｌ３＝０のときにはｆｈ＝ｆｈＣ、可動状態Ｌ３＝１のときにはｆｈ＝ｆｈ２）。
【００９７】
第３関節１３の手先位置が、完全可動領域ＰＭＲ内にある緩衝領域境界３３の内側である点Ｐ１に位置するときは、パターンＡ（可動状態Ｌ３＝１）となるので、ｆｈ＝ｆｈ２となり、人のかける力の方向にロボットアーム５の手先は自由に動作する。ここで、第１関節１１及び第２関節１２も完全可動領域ＰＭＲ内にある場合に限れば、ｆｈ２＝ｆとなり、人のかける力の方向にロボットアーム５の手先は自由に動作する。
【００９８】
手先が緩衝領域境界３３を越え、外側の点Ｐ２に移動すると、パターンＣ１になり、可動状態Ｌ３が変化すると共に変化する並進外力ｆｈでインピーダンス制御が行われ、徐々に動きに拘束がかかるようになる。
【００９９】
手先が可動領域ＭＲの境界である拘束曲面３１上の点Ｐ３に到達すると、パターンＥ１になり、並進外力ｆｈＣを入力にインピーダンス制御を行うことになり、ロボットアーム５の手先の動きは拘束曲面３１に沿うような動きとなる。すなわち、ｆＶのような成分を持つ力を人からロボットアーム５にかけても、式（８）による、並進外力ｆｈへの変換の際に打ち消されるため、ロボットアーム５の手先を、拘束曲面３１を越えて外側に動くことはできず、第３関節１３の角度も可動領域ＭＲを越えることはない。
【０１００】
一方、点Ｐ３において、−ｆＶのような成分を持つ力を人からロボットアーム５にかけた場合、第３関節１３は可動領域ＭＲ内に戻る動きとなるため、パターンＥ２となり、可動状態Ｌ３＝ｄＬ３とオフセット値が加算される分、並進外力ｆｈは復元力のように働き、関節が可動領域ＭＲ内に戻る動きが可能となる。
【０１０１】
また、同様に点Ｐ２においても、−ｆＶのような成分を持つ力を人からロボットアーム５にかけた場合、第３関節１３は緩衝領域ＢＲ１から完全可動領域ＰＭＲ内に戻る動きとなるため、パターンＣ２となり、可動状態Ｌ３＝（ｑ３ＭＡＸ−ｑ３）／ｄｑ３＋ｄＬ３とオフセット値が加算される分、並進外力ｆｈは復元力のように働き、関節が完全可動領域ＰＭＲ内に戻る動きが可能となる。
【０１０２】
前記した動作制限において、緩衝領域ＢＲ１，ＢＲ２では、徐々に動作に拘束をかけることで、動作を安定させる役割を担っている。緩衝領域ＢＲ１，ＢＲ２を設けない場合、手先が拘束曲面３１上に到達すると、インピーダンス計算手段２５への入力が並進外力ｆｈ２から並進外力ｆｈＣへステップ的に変化することになるので、制御系、すなわち、インピーダンス制御手段４が不安定になりやすい。
【０１０３】
また、パターンＤ２又はパターンＥ２のように可動領域ＭＲ内へ戻る力を受け付ける場合に、拘束曲面３１の近傍でリミットサイクルのような現象が起こり、制御系、すなわち、インピーダンス制御手段４がさらに不安定となる可能性がある。これに対して、緩衝領域ＢＲ１，ＢＲ２を設け、オフセット値ｄＬｉを導入することで、リミットサイクルを起こさず、スムーズに動作の拘束・復帰を行うことが可能となる。
【０１０４】
なお、パターンＢ（パターンＢ１，パターンＢ２）は、第１緩衝領域ＢＲ１に関するものであり、パターンＡの第１緩衝領域ＢＲ１とは反対側の領域である第２緩衝領域ＢＲ２のパターンＣ（パターンＣ１，パターンＣ２）と同様であるため、説明を省略する。パターンＤ（パターンＤ１，パターンＤ２）は、第１緩衝領域ＢＲ１の可動領域外に関するものであり、パターンＡの第１緩衝領域ＢＲ１とは反対側の領域である第２緩衝領域ＢＲ２の可動領域外のパターンＥ（パターンＥ１，パターンＥ２）と同様であるため、説明を省略する。
【０１０５】
以上は、第３関節１３の場合を中心に説明を行ったが、第１関節１１及び第２関節１２の場合も同様であり、第１の変換方法の場合には、それぞれ、式（９）及び式（１０）に基づく変換を行うことにより、また、第２の変換方法の場合には、パターンβの変換を行うことにより、各関節の可動領域ＭＲに応じたロボットアーム５の動作の拘束が可能となる。そして、第１の変換方法の場合、一連の式（７）〜式（１１）を使うことにより、第１関節１１と、第２関節１２と、第３関節１３とに対して、平行な関節軸の関節が同時に完全可動領域外になる場合を除き、同時に動作制限を行うことが可能となる。
【０１０６】
第２の変換方法の場合、パターンα〜パターンδのパターン分けにより、複数の関節が同時にロックする場合も厳密に扱うことができる。
【０１０７】
図１２は、式（１０３）に現れる並進外力ｆｈ、ｆの関係を示す。図１２においては、ロボットアーム５の第１関節１１から第３関節１３までを模式的に表している。拘束曲線２０１は、第２関節１２と第３関節１３が可動領域ＭＲを越えて、それ以上動けずロックされた状態の場合に、残りの第１関節１１が動くことで、ロボットアーム５の手先のハンド６が動くことのできる曲線である。すなわち、第２関節１２と第３関節１３がロック状態になると、ロボットアーム５の手先は拘束曲線２０１に拘束される。
【０１０８】
この拘束曲線２０１の接線であり、かつ、ロボットアーム５の手先を通る直線が、接線２０２である。また、接線２０２は、ベクトルｊ１に平行な直線である。
【０１０９】
この接線２０２への並進外力ｆの正射影が、ｆｈｃである。正射影ｆｈｃは、式（１０３）において第２関節１２の可動状態Ｌ２＝０かつ第３関節１３の可動状態Ｌ３＝０のときの並進外力ｆｈであり、式（１０５）で表される。なお、式（１０５）はＬ２＝０の場合の式（１０３）であり、式（１０３）を使って、変換外力Ｆｈを計算することは、正射影への変換を使用していることになる。すなわち、拘束曲線２０１の接線２０２への正射影を考えれば、ロボットアーム５が動作可能な方向成分となる。
【０１１０】
【数２８】
2011080856.tif 000030
．．．．．（１０５）
また、並進外力ｆｈは、並進外力ｆのベクトルの先端と、並進外力ｆの正射影ｆｈｃのベクトルの先端とを結んだ直線（点線）上に、先端が来る中間的な値であり、その位置は可動状態Ｌ２Ｌ３により定義される（可動状態Ｌ２Ｌ３＝０のときにはｆｈ＝ｆｈＣ、可動状態Ｌ３＝１のときにはｆｈ＝ｆ２）。
【０１１１】
以上のように式（１０５）に基づく変換によれば、第２関節１２と第３関節１３の２つの関節がロックする場合の動作も扱うことが可能となる。
【０１１２】
ロボットアーム５の手先の姿勢（φ、θ、ψ）を決める第４関節１４、第５関節１５、第６関節１６に関しては、式（１２）に基づくことで各関節の動作制限が実現する。式（１２）において、Ｊｖ（ｑ）ＴＦの部分は式（６）右辺と同様であり、外力Ｆにより各関節１１，１２，１３，１４，１５，１６に発生するトルクτ＝［τ１，τ２，τ３，τ４，τ５，τ６］Ｔに相当する。このトルクベクトルの第３行，第４行，第５行に、それぞれ第４関節１４、第５関節１５、第６関節１６の関節可動状態Ｌ４，Ｌ５，Ｌ６をかけたベクトル［τ１，τ２，τ３，Ｌ４τ４，Ｌ５τ５，Ｌ６τ６］Ｔを、式Ｊｖ（ｑ）−Ｔに用いて手先外力に逆変換し、モーメント成分のみを抽出したのがｎｈである。したがって、例えば、Ｌ４＝０とすると、τ４＝０となるので、モーメント成分ｎｈを入力としてインピーダンス制御すると、第４関節１４は動作せず固定されることになる。
【０１１３】
第５関節１５及び第６関節１６に関しても同様であり、以上の原理により、関節可動状態Ｌ４，Ｌ５，Ｌ６に基づき、ロボットアーム５の手先の姿勢（φ、θ、ψ）を決める手首部７である第４関節１４と第５関節１５と第６関節１６との動作制限が実現する。
【０１１４】
以上、本第１実施形態によれば、関節可動状態計算手段２及び力変換手段２４を有することにより、インピーダンス制御による動作時に人がロボットアーム５の関節の可動領域ＭＲを越えるような操作を行っても、関節可動状態計算手段２において当該関節の可動状態を判定し、力変換手段２４により変換された変換外力Ｆｈをインピーダンス計算手段２５への入力として使うことで、ロボットアーム５の動作制限が可能となり、関節の可動領域ＭＲを越えるのを防ぐことができる。
【０１１５】
また、本第１実施形態によれば、インピーダンス計算手段２５への入力に対し力変換手段２４により変換をかけることにより動作制限を行う、すなわち、入力を外力Ｆから変換外力Ｆｈへと制限することで動作制限を行うため、ロボットアーム５の機構的な可動領域ＭＲを越えてロボットアーム５が動けない状態でも、インピーダンス計算手段２５に入力が入り続け、後段にあるインピーダンス計算手段２５又は位置制御系２９に誤差が蓄積してしまうことがなく、安定な制御系とすることができるという格別の効果を発揮する。
【０１１６】
さらに、力変換手段２４を使わずに、位置制御系２９において関節の動作制限を行う場合が（公知技術として）考えられるが、その場合は、後段にあるインピーダンス計算手段２５又は位置制御系２９に誤差蓄積に対する対策を施す必要があり、複雑な制御系構造となってしまう。これに対して、力変換手段２４を使う本第１実施形態では、誤差蓄積に対する対策を施す必要はなく、簡潔な制御系構造とすることができる。
【０１１７】
また、前記第１実施形態では、インピーダンス制御のインピーダンスパラメータを変化させることで動作制限を行う方式ではないため、パラメータ可変のための仕組みが不要であり、制御系の構造を簡潔にすることができ、さらに、パラメーター変化により制御系が不安定になることを防ぐことができる。すなわち、前記第１実施形態では、力変換手段２４により入力制限を行うこと（言い換えれば、力変換手段２４により外力Ｆが変換された変換外力Ｆｈをインピーダンス計算手段２５への入力として使うこと）で、前記優れた効果を発揮させることができるので、インピーダンスパラメータを変化させる必要はない。
【０１１８】
さらに、本第１実施形態によれば、各関節の動作制限は独立に行われるため、仮にある関節が動作制限されても、動作可能な他の関節は動作制限されず動き続けることが可能であるので、すぐに動作制限がかかり停止するようなことのない、操作性の良いロボットとすることができる。
【０１１９】
（第２実施形態）
本発明の第２実施形態におけるロボットの基本的な構成は、図１及び図２に示した第１実施例の場合と同様であるので、共通部分の説明は省略し、異なる部分についてのみ以下、詳細に説明する。
【０１２０】
図７に示すように、第２実施形態では、通常のインピーダンス制御手段４３（すなわち、インピーダンス計算手段２５と目標軌道生成手段２３と順運動学計算手段２６と位置誤差補償手段２７と近似逆運動学計算手段２８と第１演算部６１と第２演算部６２）とは独立した力入力制御手段３４に、関節可動状態計算手段２と力変換手段２４とを配設し、既存の制御装置（第１制御装置）１Ｂと通信する構成としている。
【０１２１】
図８は、第２実施形態のロボット９０Ｂの全体構成を示しており、制御装置の具体的な構成を示している。第２実施形態の制御装置では、第１制御装置１Ｂとは別に第２制御装置４６が追加され、２つの独立した制御装置１Ｂ，４６で構成される。
【０１２２】
第２制御装置４６は、ハードウェア的には一般的なパーソナルコンピュータにより構成されている。そして、入出力ＩＦ（Ｄ／Ａボード４５とＡ／Ｄボード４７とカウンタボード４８）を除く部分は、パーソナルコンピュータで実行される制御プログラム４４により力入力制御手段３４を実現する。力入力制御手段３４は、Ａ／Ｄボード４７を通じて力センサー３より外力Ｆｓが入力され、力変換手段２４による変換結果である変換外力Ｆｈが、Ｄ／Ａボード４５を通じて第１制御装置１Ｂの力センサー入力であるＡ／Ｄボード２１に出力され、Ａ／Ｄボード２１を通じて第１制御装置１Ｂに取り込まれる。このように、Ｄ／Ａボード４５とＡ／Ｄボード２１とを接続することで、電圧値により変換外力Ｆｈが伝えられる通信手段が構成されている。
【０１２３】
また力入力制御手段３４は、カウンタボード４８を通じてロボットアーム５の各関節部のエンコーダ４２より出力される関節角度情報を取得し、関節可動状態計算手段２による、関節１１，１２，１３，１４，１５，１６の可動状態の判定のための情報として使用される。
【０１２４】
本第２実施形態は、前記通常のインピーダンス制御手段４３に、力入力制御手段３４を別途追加した構成であるため、図７のような、２つに分離独立した構成が容易に実現できる。したがって第１実施形態は、図１に示すように一つの制御装置（ＰＣ）１上で動作する１つの制御プログラム１７によりインピーダンス制御手段４を実現したが、第２実施形態は、図８に示すように第１制御装置１Ｂに第２制御装置４６を加え、既存の第１制御装置１Ｂと通信する構成が可能となる。
【０１２５】
この構成により、例えば、市販のロボット制御装置の力センサーからの入力信号を処理するために第２制御装置４６を追加するだけで、ロボットアーム５の動作制限を行なうことができる。
【０１２６】
（第３実施形態）
本発明の第３実施形態における制御装置の基本的な構成は、図１及び図２に示した第１実施例の場合と同様であるので、共通部分の説明は省略し、異なる部分についてのみ以下、詳細に説明する。
【０１２７】
図９は、本発明の第３実施形態におけるロボット９０Ｃのインピーダンス制御手段の構成を示すブロック線図であり、図１０はロボット９０Ｃによる人１００との協調搬送作業を説明する図である。
【０１２８】
図１０に示すように、第３実施形態のロボット９０Ｃは、手首部７に操作ハンドル４０が力センサー３を介して固定されおり、人１００は操作ハンドル４０を直接握り、力をかけることでロボット９０Ｃを操作することができる。操作ハンドル４０とロボットアーム５の間には力センサー３が配設されており、人１００が操作するときの力を力センサー３で検出することができる。
【０１２９】
また、操作ハンドル４０には、動作モード設定部の一例としての動作モード設定スイッチ３９が設けられており、人１００は、動作モード入力部の一例として機能する動作モード設定スイッチ３９を操作することにより、動作モード（可動又は固定のモード）の設定及び切換を行うことができる。
【０１３０】
図９に示すように、動作モード設定スイッチ３９から動作モード指令が関節動作モード設定手段３７に入力される。さらに、関節可動状態計算手段２から関節可動状態Ｌｉが関節動作モード設定手段３７に入力され、力変換手段２４に拡張関節可動状態Ｌｉｍが出力される。
【０１３１】
図１０に示すような人１００とロボット９０Ｃとの協調作業形態の場合、物体３８の負荷をすべてロボットアーム５が受け持つため、重力補償手段３０は不要である。この場合には、力センサー３の計測値Ｆｓが力変換手段２４に外力Ｆとして入力されて、変換外力Ｆｈに変換される。
【０１３２】
動作モード設定スイッチ３９では、ロボットアーム５の６つの関節１１，１２，１３，１４，１５，１６のそれぞれの可動又は固定のモード設定を行うことができる。
【０１３３】
インピーダンス制御手段４内に備えられた関節動作モード設定手段３７は、動作モード設定スイッチ３９で設定した各関節の可動又は固定のモード設定に応じて、可動のモード設定の場合はそのまま可動状態Ｌｉを拡張関節可動状態Ｌｉｍとして出力し、固定のモード設定の場合は拡張関節可動状態Ｌｉｍ＝０を出力する。これらの拡張関節可動状態Ｌｉｍは、前記第１実施形態で説明した可動状態Ｌｉと同様に取り扱い、変換手段２４に入力されて使用される。これにより、固定のモード設定された関節は、第１実施形態で説明した原理により、その動作が制限されて固定される。
【０１３４】
以上のように、本第３実施形態によれば、関節動作モード設定手段３７を有することにより、特定の関節を固定した動作、又は、特定の関節のみを回転させるような動作が容易に可能となり、より操作性の良いロボットとすることができる。
【０１３５】
第３実施形態にかかるロボットの動作の具体例としては、図１１に示すように、第１関節１１のみ回転させ、ロボットアーム５を台部１０に対して旋回させるような動作をさせるときには、動作モード設定スイッチ３９で第１関節１１を可動と設定し、第２〜第６関節１２〜１６を固定に設定すれば、人１００が操作ハンドル４０に力をかけるだけでロボットアーム５の第１関節１１のみを容易に回転させる旋回動作を実現することができる。
【０１３６】
なお、前記様々な実施形態のうちの任意の実施形態を適宜組み合わせることにより、それぞれの有する効果を奏するようにすることができる。
【産業上の利用可能性】
【０１３７】
本発明のロボット、ロボットの制御装置、及び制御方法は、ロボットアームの関節の動作範囲の規制など、関節の動作状況に応じたロボットアームの動作制限を、簡潔な制御系構造で実現し、制御系の不安定さを低減することができて、工場又は家庭内、介護現場等でパワーアシスト等の作業支援を行うロボット等、人と協働するロボット、ロボットの制御装置、及び制御方法として有用である。【０１３８】
本発明は、添付図面を参照しながら好ましい実施形態に関連して充分に記載されているが、この技術の熟練した人々にとっては種々の変形又は修正は明白である。そのような変形又は修正は、添付した請求の範囲による本発明の範囲から外れない限りにおいて、その中に含まれると理解されるべきである。
【図１】
2011080856.tif 000031
【図２】
2011080856.tif 000032
【図３】
2011080856.tif 000033
【図４】
2011080856.tif 000034
【図５】
2011080856.tif 000035
【図６】
2011080856.tif 000036
【図７】
2011080856.tif 000037
【図８】
2011080856.tif 000038
【図９】
2011080856.tif 000039
【図１０】
2011080856.tif 000040
【図１１】
2011080856.tif 000041
【図１２】
2011080856.tif 000042
【手続補正書】
【提出日】平成23年9月28日(2011.9.28)
【手続補正１】
【補正対象書類名】明細書
【補正対象項目名】全文
【補正方法】変更
【補正の内容】
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は、工場内、又は、家庭内、又は、介護現場等でパワーアシスト等の作業支援を行うロボットアームを備えるロボット等、人と協働するロボット、ロボットの制御装置、及び制御方法に関する。
【背景技術】
【０００２】
近年、工場等で人と協調して動作し、パワーアシスト等の作業支援を行う人協調型のロボットが注目されている。こうした人協調型のロボットは人の操作により動作するため、高度な自律機能がなくても実用的な作業が可能であり、また人の制御の下に動作するため安全性が高く、将来は工場内に留まらず、家庭内の生活支援又は介護への応用が期待されている。
【０００３】
一方、人協調型のロボットは、事前プログラミング通りに動作する従来のロボットと異なり、人が操作する故に動作範囲を越えてしまうなど、事前に想定した以上の動作を人が行い、制御が不安定になったり、ロボットを故障させてしまう可能性がある。
【０００４】
こうした課題に対し、従来技術としては、作動範囲を規制するリミッタを設けると共に、搬送物がリミッタを越えた場合には、作業者に所定の反力を伝えるアシスト搬送装置を開示している（特許文献１）。
【先行技術文献】
【特許文献】
【０００５】
【特許文献１】特開２００５−１４１３３号公報
【発明の概要】
【発明が解決しようとする課題】
【０００６】
しかしながら、前記従来技術では、ロボットアームの手先の作業範囲を規制する固定のリミッタを設ける方法であるため、ロボットアームの各関節の動作範囲の規制など、複数の関節の関係によって手先の可動範囲が複雑に変化する場合には、適用が困難である。
【０００７】
また、前記従来技術は、リミッタを越える動作に対しては、バネ定数などの制御パラメータを変化させて反力を発生させることで、動作の制限を行うが、制御パラメータを可変とするために、制御系の構成が複雑になる可能性がある。また、制御パラメータを変化させることで制御系が不安定になり、ロボットの動作が不安定になる可能性がある。
【０００８】
本発明の目的は、前記従来の制御装置の課題を解決し、ロボットアームの関節の動作範囲の規制など、関節の動作状況に応じたロボットアームの動作制限を、簡潔な制御系構造で実現し、制御系の不安定さを低減するロボット、ロボットの制御装置、及び制御方法を提供する。
【課題を解決するための手段】
【０００９】
前記目的を達成するために、本発明は以下のように構成する。
【００１０】
本発明の第１態様によれば、多関節ロボットアームと、
前記多関節ロボットアームに配設され、かつ、外力を検出する外力検出手段と、
前記多関節ロボットアームの関節が可動範囲のいずれかに位置して可動状態を計算する関節可動状態計算手段と、
前記関節可動状態計算手段により計算された前記関節の前記可動状態に基づき、前記外力検出手段が検出した前記外力を、前記外力が前記ロボットアームに加えられたときに前記ロボットアームが動作可能な方向成分を抽出可能な変換外力に変換する外力変換手段と、
前記外力変換手段により変換された前記変換外力に基づいてインピーダンス制御を行うことにより、前記多関節ロボットアームの前記関節の前記可動状態に応じた動作制限が行われるように前記多関節ロボットアームを制御する制御手段とを有するロボットを提供する。
【００１１】
本発明の第９態様によれば、多関節ロボットアームを備えるロボットの制御装置であって、
前記多関節ロボットアームの関節が可動範囲のいずれかに位置して可動状態を計算する関節可動状態計算手段と、
前記関節可動状態計算手段により計算された前記関節の前記可動状態に基づき、前記多関節ロボットアームに配設されて外力を検出する外力検出手段が検出した前記外力を、前記外力が前記ロボットアームに加えられたときに前記ロボットアームが動作可能な方向成分を抽出可能な変換外力に変換する外力変換手段と、
前記外力変換手段により変換された前記変換外力に基づいてインピーダンス制御を行うことにより、前記多関節ロボットアームの前記関節の前記可動状態に応じた動作制限が行われるように前記多関節ロボットアームを制御する制御手段とを有することを特徴とするロボットの制御装置を提供する。
【００１２】
本発明の第１０態様によれば、多関節ロボットアームを備えるロボットの制御方法であって、
前記多関節ロボットアームの関節が可動範囲のいずれかに位置して可動状態を関節可動状態計算手段で計算し、
前記計算された前記関節の前記可動状態に基づき、前記多関節ロボットアームに配設されて外力を検出する外力検出手段が検出した前記外力を、前記外力が前記ロボットアームに加えられたときに前記ロボットアームが動作可能な方向成分を抽出可能な変換外力に外力変換手段で変換し、
前記変換された変換外力に基づいてインピーダンス制御を行うことにより、前記多関節ロボットアームの前記関節の前記可動状態に応じた動作制限が行われるように前記多関節ロボットアームを制御手段で制御することを特徴とするロボットの制御方法を提供する。
【００１３】
本発明の第１１態様によれば、多関節ロボットアームを備えるロボットの制御プログラムであって、
コンピュータを、
前記多関節ロボットアームの関節が可動範囲のいずれかに位置して可動状態を計算する関節可動状態計算手段と、
前記関節可動状態計算手段により計算された前記関節の前記可動状態に基づき、前記多関節ロボットアームに配設されて外力を検出する外力検出手段が検出した前記外力を、前記外力が前記ロボットアームに加えられたときに前記ロボットアームが動作可能な方向成分を抽出可能な変換外力に変換する外力変換手段と、
前記外力変換手段により変換された前記変換外力に基づいてインピーダンス制御を行うことにより、前記多関節ロボットアームの前記関節の前記可動状態に応じた動作制限が行われるように前記多関節ロボットアームを制御する制御手段として機能させるためのロボットの制御プログラムを提供する。
【発明の効果】
【００１４】
本発明のロボット、ロボットの制御装置、及び制御方法によれば、関節可動状態計算手段及び力変換手段を有することにより、インピーダンス制御による動作時に人が関節の可動領域を越えるような操作を行っても、関節可動状態計算手段において関節が可動範囲のいずれかに位置して可動状態であるかどうか可動状態を判定し、外力変換手段により変換された変換外力をインピーダンス計算手段への入力として使うことで、ロボットアームの動作制限が可能となり、関節の可動領域を越えることを防ぐことができる。
【図面の簡単な説明】
【００１５】
本発明のこれらと他の目的と特徴は、添付された図面についての好ましい実施形態に関連した次の記述から明らかになる。この図面においては、
【００１６】
【図１】図１は、本発明の第１実施形態におけるロボットの全体構成を示す図であり、
【図２】図２は、本発明の前記第１実施形態における前記ロボットのインピーダンス制御手段の構成を示すブロック図であり、
【図３】図３は、本発明の前記第１実施形態における前記ロボットによる人との協調搬送作業を説明する図であり、
【図４】図４は、関節角度ｑｉと可動状態の関係を示す図であり、
【図５】図５は、本発明の前記第１実施形態における前記ロボットにおけるインピーダンス制御手段の全体的な動作ステップを表すフローチャートであり、
【図６】図６は、本発明の前記第１実施形態における前記ロボットにおける拘束曲面及び力の変換を説明する図であり、
【図７】図７は、本発明の第２実施形態におけるロボットのインピーダンス制御手段の構成を示すブロック図であり、
【図８】図８は、本発明の前記第２実施形態における前記ロボットの全体構成を示す図であり、
【図９】図９は、本発明の第３実施形態におけるロボットのインピーダンス制御手段の構成を示すブロック図であり、
【図１０】図１０は、本発明の前記第３実施形態における前記ロボットによる人との協調搬送作業を説明する図であり、
【図１１】図１１は、本発明の前記第３実施形態における前記ロボットの動作の具体例を示す図であり、
【図１２】図１２は、式（１０３）に現れる並進外力ｆｈ、ｆの関係を示す図である。
【発明を実施するための形態】
【００１７】
以下に、本発明にかかる実施の形態を図面に基づいて詳細に説明する。
【００１８】
以下、図面を参照して本発明における実施形態を詳細に説明する前に、本発明の種々の態様について説明する。
【００１９】
本発明の第１態様によれば、多関節ロボットアームと、
前記多関節ロボットアームに配設され、かつ、外力を検出する外力検出手段と、
前記多関節ロボットアームの関節が可動範囲のいずれかに位置して可動状態を計算する関節可動状態計算手段と、
前記関節可動状態計算手段により計算された前記関節の前記可動状態に基づき、前記外力検出手段が検出した前記外力を、前記外力が前記ロボットアームに加えられたときに前記ロボットアームが動作可能な方向成分を抽出可能な変換外力に変換する外力変換手段と、
前記外力変換手段により変換された前記変換外力に基づいてインピーダンス制御を行うことにより、前記多関節ロボットアームの前記関節の前記可動状態に応じた動作制限が行われるように前記多関節ロボットアームを制御する制御手段とを有するロボットを提供する。
【００２０】
本発明の第２態様によれば、前記外力変換手段は、前記外力検出手段に検出された前記外力を、前記多関節ロボットアームの前記関節が固定されたときに前記多関節ロボットアームの手先が動くことのできる拘束曲面の接平面への正射影として変換する第１の態様に記載のロボットを提供する。
【００２１】
本発明の第３態様によれば、さらに、前記多関節ロボットアームにかかる前記外力により各関節に発生する外力トルクを計算する外力トルク計算手段を有し、
前記関節可動状態計算手段は、前記外力トルク計算手段の計算結果に基づき前記外力トルクが前記多関節ロボットアームの各関節の動作を可動領域外より可動領域内に戻そうとする力であるかどうかを判定し、
前記関節可動状態計算手段が、前記外力トルクが前記多関節ロボットアームの各関節の動作を前記可動領域外より前記可動領域内に戻そうとする力であると判定する場合には、前記制御手段は前記動作制限を行わない一方、
前記関節可動状態計算手段が、前記外力トルクが前記多関節ロボットアームの各関節の動作を前記可動領域外より前記可動領域内に戻そうとする力ではないと判定する場合には、前記制御手段は前記動作制限を行う第１又は２の態様に記載のロボットを提供する。
【００２２】
本発明の第４態様によれば、前記関節可動状態計算手段は、前記外力を関節トルクに変換することにより、前記関節トルクに変換された前記変換外力が、前記多関節ロボットアームの各関節の動作を可動領域外より可動領域内に戻そうとする力であるかどうかを判定し、
前記関節可動状態計算手段が、前記変換外力が、前記多関節ロボットアームの各関節の動作を前記可動領域外より前記可動領域内に戻そうとする力であると判定する場合には、前記制御手段は前記動作制限を行わない一方、
前記関節可動状態計算手段が、前記変換外力が、前記多関節ロボットアームの各関節の動作を前記可動領域外より前記可動領域内に戻そうとする力ではないと判定する場合には、前記制御手段は前記動作制限を行う第１又は２の態様に記載のロボットを提供する。
【００２３】
本発明の第５態様によれば、前記多関節ロボットアームの各関節に対し、可動あるいは固定の動作モードを設定する動作モード設定部と、
前記動作モード設定部で設定された前記動作モードの指令が入力されて、前記動作モードと前記関節可動状態計算手段により計算された前記関節の前記可動状態とに基づき前記可動状態の計算を行う関節動作モード設定手段とをさらに備えて、
前記外力変換手段は、前記関節動作モード設定手段により計算された前記関節の前記可動状態に基づき、前記外力検出手段が検出した前記外力を、前記外力が前記ロボットアームに加えられたときに前記ロボットアームが動作可能な方向成分を抽出可能な変換外力に変換し、
前記制御手段は、前記変換外力に基づいてインピーダンス制御を行うことにより、前記多関節ロボットアームの前記関節の前記可動状態に応じた動作制限が行われる第１〜４のいずれか１つの態様に記載のロボットを提供する。
【００２４】
本発明の第６態様によれば、前記関節可動状態計算手段は、前記関節の可動領域の両端の境界線から一定範囲だけ内側に緩衝領域をそれぞれ設け、前記各緩衝領域では前記関節の前記可動状態を徐々に変化させる第３又は４の態様に記載のロボットを提供する。
【００２５】
本発明の第７態様によれば、前記多関節ロボットアームの前記関節の可動領域の両端の内側にそれぞれ設けられた緩衝領域と、前記緩衝領域を除く前記可動領域の残りの領域である完全可動領域ＰＭＲと、可動領域外の領域とに分割し、
前記関節可動状態計算手段は、各領域毎に、前記関節の前記可動状態の値を異ならせるように計算される第１の態様に記載のロボットを提供する。
【００２６】
本発明の第８態様によれば、前記関節可動状態計算手段と前記力変換手段とが第２制御手段として、前記多関節アームの前記制御手段とは別に備えられ、
前記力変換手段が変換した外力が前記第２制御手段より通信を通じて前記制御手段に入力される第１の態様に記載のロボットを提供する。
【００２７】
本発明の第９態様によれば、多関節ロボットアームを備えるロボットの制御装置であって、
前記多関節ロボットアームの関節が可動範囲のいずれかに位置して可動状態を計算する関節可動状態計算手段と、
前記関節可動状態計算手段により計算された前記関節の前記可動状態に基づき、前記多関節ロボットアームに配設されて外力を検出する外力検出手段が検出した前記外力を、前記外力が前記ロボットアームに加えられたときに前記ロボットアームが動作可能な方向成分を抽出可能な変換外力に変換する外力変換手段と、
前記外力変換手段により変換された前記変換外力に基づいてインピーダンス制御を行うことにより、前記多関節ロボットアームの前記関節の前記可動状態に応じた動作制限が行われるように前記多関節ロボットアームを制御する制御手段とを有することを特徴とするロボットの制御装置を提供する。
【００２８】
本発明の第１０態様によれば、多関節ロボットアームを備えるロボットの制御方法であって、
前記多関節ロボットアームの関節が可動範囲のいずれかに位置して可動状態を関節可動状態計算手段で計算し、
前記計算された前記関節の前記可動状態に基づき、前記多関節ロボットアームに配設されて外力を検出する外力検出手段が検出した前記外力を、前記外力が前記ロボットアームに加えられたときに前記ロボットアームが動作可能な方向成分を抽出可能な変換外力に外力変換手段で変換し、
前記変換された変換外力に基づいてインピーダンス制御を行うことにより、前記多関節ロボットアームの前記関節の前記可動状態に応じた動作制限が行われるように前記多関節ロボットアームを制御手段で制御することを特徴とするロボットの制御方法を提供する。
【００２９】
本発明の第１１態様によれば、多関節ロボットアームを備えるロボットの制御プログラムであって、
コンピュータを、
前記多関節ロボットアームの関節が可動範囲のいずれかに位置して可動状態を計算する関節可動状態計算手段と、
前記関節可動状態計算手段により計算された前記関節の前記可動状態に基づき、前記多関節ロボットアームに配設されて外力を検出する外力検出手段が検出した前記外力を、前記外力が前記ロボットアームに加えられたときに前記ロボットアームが動作可能な方向成分を抽出可能な変換外力に変換する外力変換手段と、
前記外力変換手段により変換された前記変換外力に基づいてインピーダンス制御を行うことにより、前記多関節ロボットアームの前記関節の前記可動状態に応じた動作制限が行われるように前記多関節ロボットアームを制御する制御手段として機能させるためのロボットの制御プログラムを提供する。
【００３０】
以下に、本発明にかかる実施の形態を図面に基づいて詳細に説明する。
【００３１】
（第１実施形態）
図１は本発明の第１実施形態のロボット９０の構成を示す図である。ロボット９０は、多関節ロボットアーム５と、多関節ロボットアーム５の動作を制御する制御装置１とを備えて構成されている。
【００３２】
制御装置１は、ハードウェア的には一般的なパーソナルコンピュータにより構成されている。そして、インピーダンス制御手段（インピーダンス制御部）４の入出力ＩＦ１９を除く部分は、パーソナルコンピュータで実行される制御プログラム１７としてソフトウェア的に実現される。
【００３３】
入出力ＩＦ１９は、パーソナルコンピュータのＰＣＩバスなどの拡張スロットルに接続された、Ｄ／Ａボード２０と、Ａ／Ｄボード２１と、カウンタボード２２とにより構成される。
【００３４】
ロボット９０の多関節ロボットアーム５の動作を制御するための制御プログラム１７が実行されることにより、制御装置１が機能する。ロボットアーム５の各関節１１，１２，１３，１４，１５，１６のそれぞれのエンコーダ４２より出力される関節角度情報がカウンタボード２２を通じて制御装置１に取り込まれ、制御装置１によって各関節１１，１２，１３，１４，１５，１６の回転動作での制御指令値が算出される。算出された各制御指令値は、Ｄ／Ａボード２０を通じてモータードライバ１８に与えられ、モータードライバ１８から送られた各制御指令値に従って、ロボットアーム５の各関節１１，１２，１３，１４，１５，１６のモータ４１が駆動される。
【００３５】
ロボットアーム５は、６自由度の多リンクマニピュレータであり、手先の一例として機能するハンド６と、ハンド６が取付けられる手首部７を有する前腕リンク８と、前腕リンク８が回転可能に連結される上腕リンク９と、上腕リンク９が回転可能に連結支持される台部１０とを有している。手首部７は、第４関節１４と、第５関節１５と、第６関節１６との３つの回転軸を有しており、上腕リンク９に対するハンド６の相対的な姿勢（向き）を変化させることができる。前腕リンク８の他端は、上腕リンク９の一端に対して第３関節１３周りに回転可能とし、上腕リンク９の他端は、台部１０に対して第２関節１２周りに回転可能とする。そして台部の上側可動部１０ａは台部の下側固定部１０ｂに対して第１関節１１周りに回転可能として、合計６個の軸周りに回転可能として前記６自由度の多リンクマニピュレータを構成している。
【００３６】
各軸の回転部分を構成する各関節１１，１２，１３，１４，１５，１６は、各関節１１，１２，１３，１４，１５，１６の一方の部材に設けられた回転駆動装置（本第１実施形態ではモータ４１）と、モータ４１の回転軸の回転位相角（すなわち関節角）を検出するエンコーダ４２とを備える。そして、モータ４１の回転軸が関節の他方の部材に連結されて前記回転軸を正逆回転させることにより、他方の部材を一方の部材に対して各軸周りに回転可能とする。回転駆動装置は、後述するモータードライバ１８により駆動制御される。本第１実施形態では、回転駆動装置の一例としてのモータ４１とエンコーダ４２とは、ロボットアーム５の各関節１１，１２，１３，１４，１５，１６の内部に配設されている。
【００３７】
３５は台部１０の下側固定部１０ｂに対して相対的な位置関係が固定された絶対座標系であり、３６はハンド６に対して相対的な位置関係が固定された手先座標系である。絶対座標系３５から見た手先座標系３６の原点位置Ｏｅ（ｘ、ｙ、ｚ）をロボットアーム５の手先位置とし、絶対座標系３５から見た手先座標系３６の姿勢をロール・ピッチ・ヨー角で表現した（φ、θ、ψ）をロボットアーム５の手先姿勢とし、手先位置・姿勢ベクトルをｒ＝［ｘ、ｙ、ｚ、φ、θ、］Ｔと定義する。ロボットアーム５の手先位置・姿勢を制御する場合には、手先位置・姿勢ベクトルｒを目標手先位置・姿勢ベクトルｒｄに追従させることになる。
【００３８】
ロボットアーム５の手首部７とハンド６の間には、外力検出手段（外力検出部又は外力検出装置）の一例として機能する力センサー３が配設されており、ハンド６にかかる外力を検出できる。
【００３９】
次に、図２を使い、インピーダンス制御手段４の詳細について説明する。図２において、５は図１に示したロボットアームである。ロボットアーム５からはそれぞれの関節軸のエンコーダ４２により計測された関節角の現在値（関節角度ベクトル）ｑ＝［ｑ１，ｑ２，ｑ３，ｑ４，ｑ５，ｑ６］Ｔが出力され、カウンタボード２２によりインピーダンス制御手段４に取り込まれる。ただし、ｑ１，ｑ２，ｑ３，ｑ４，ｑ５，ｑ６は、それぞれ、第１関節１１、第２関節１２、第３関節１３、第４関節１４、第５関節１５、第６関節１６の関節角度である。
【００４０】
また、ロボットアーム５からは力センサー３により計測された外力ＦＳ＝［ｆｘ，ｆｙ，ｆｚ，ｎｘ，ｎｙ，ｎｚ］Ｔが出力され、Ａ／Ｄボード２１によりインピーダンス制御手段４に取り込まれる。
【００４１】
目標軌道生成手段（目標軌道生成部）２３は、目標とするロボットアーム５の動作を実現するための手先位置・姿勢目標ベクトルｒｄを出力する。目標とするロボットアーム５の動作は、目的とする作業に応じて、事前に、ポイントごとの位置（ｒｄ０、ｒｄ１、ｒｄ２、・・・）が時系列データとして与えられている。目標軌道生成手段２３は、多項式補間を使用し、各ポイント間の軌道を補完し、手先位置・姿勢目標ベクトルｒｄを生成する。
【００４２】
インピーダンス計算手段（インピーダンス計算部）２５は、ロボットアーム５に機械インピーダンスを実現する機能を果たす部分であり、ロボットアーム５が目標軌道生成手段２３の生成する目標軌道に沿うよう位置制御により単独で動作する場合は０を出力する。一方、インピーダンス計算手段２５は、ロボットアーム５と人間が協働作業をする場合には、（ｉ）設定したインピーダンスパラメータＭ、Ｄ、Ｋ（慣性Ｍ、粘性Ｄ、剛性Ｋ）と、（ｉｉｉ）インピーダンス制御手段４（インピーダンス制御手段４の重力補償手段３０）へ入力される外力Ｆを変換した変換外力Ｆｈとにより、ロボットアーム５に機械インピーダンスを実現するための手先位置・姿勢目標補正出力ｒｄΔを以下の式（１）により計算し、出力する。手先位置・姿勢目標補正出力ｒｄΔは、目標軌道生成手段２３の出力する手先位置・姿勢目標ｒｄに第１演算部６１で加算され、手先位置・姿勢補正目標ベクトルｒｄｍが生成される。
【００４３】
【数１】
2011080856.tif 000043
．．．．．（１）
ただし、
【００４４】
【数２】
2011080856.tif 000044
．．．．．（２）
【００４５】
【数３】
2011080856.tif 000045
．．．．．（３）
【００４６】
【数４】
2011080856.tif 000046
．．．．．（４）
であり、ｓはラプラス演算子である。
【００４７】
順運動学計算手段２６は、ロボットアーム５から出力された各関節角の現在値ｑである関節角度ベクトルｑが入力され、ロボットアーム５の関節角度ベクトルｑから手先位置・姿勢ベクトルｒへの変換の幾何学的計算を行う。
【００４８】
順運動学計算手段２６により計算される手先位置・姿勢ベクトルｒと、手先位置・姿勢補正目標ベクトルｒｄｍとの誤差ｒｅが第２演算部６２で求められる。
【００４９】
第２演算部６２で求められた誤差ｒｅが位置誤差補償手段２７に入力され、位置誤差補償出力ｕｒｐが位置誤差補償手段２７で求められて、近似逆運動学計算手段２８に出力される。
【００５０】
近似逆運動学計算手段２８は、近似式
【数５】
2011080856.tif 000047
を用いて、逆運動学の近似計算を行う。ただし、Ｊｒ（ｑ）は
【数６】
2011080856.tif 000048
の関係を満たすヤコビ行列、ｕｉｎは近似逆運動学計算手段２８への入力、ｕｏｕｔは近似逆運動学計算手段２８からの出力である。ここで、
【数７】
2011080856.tif 000049
と近似して考えることができるので、ヤコビ行列の定義式
【数８】
2011080856.tif 000050
に代入すると、
【数９】
2011080856.tif 000051
が成り立つことが分かる。すなわち、Ｊｒ（ｑ）−１を近似逆運動学計算手段２８で乗算することで手先位置・姿勢に関する誤差値から関節角度に関する誤差値への変換となる。したがって、手先位置・姿勢誤差ｒｅにゲインをかける等した値である位置誤差補償出力ｕｒｅが近似逆運動学計算手段２８に入力されると、その出力として、関節角度誤差ｑｅを補償するための関節角度誤差補償出力ｕｑｅが近似逆運動学計算手段２８から出力されることになる。
【００５１】
関節角度誤差補償出力ｕｑｅは、近似逆運動学計算手段２８からＤ／Ａボード２０を介してモータードライバ１８に電圧指令値として与えられ、各関節軸が正逆回転駆動されてロボットアーム５が動作する。
【００５２】
以上のように構成されるインピーダンス制御手段４の基本構成に関して動作の基本原理を説明する。
【００５３】
動作の基本は、位置誤差補償手段２７による手先位置・姿勢誤差ｒｅのフィードバック制御（位置制御）であり、図２の点線で囲まれた部分が位置制御系２９になっている。位置誤差補償手段２７として、例えば、ＰＩＤ補償器を使用すれば、手先位置・姿勢誤差ｒｅが０に収束するように制御が働き、目標とするロボットアーム５の動作が実現する。
【００５４】
インピーダンス制御を行う場合、前記説明した位置制御系２９に対し、インピーダンス計算手段２５により手先位置・姿勢目標補正出力ｒｄΔが第１演算部６１で加算されて手先位置・姿勢の目標値の補正が行われる。このために、前記した位置制御系２９は、手先位置・姿勢の目標値が本来の値より微妙にずれることになり、結果的に機械インピーダンスが実現される。以上の図２の一点鎖線で囲まれた部分は、位置制御ベースのインピーダンス制御系４９と呼ばれる構成であり、このインピーダンス制御系４９により慣性Ｍ、粘性Ｄ、剛性Ｋの機械インピーダンスが実現される。インピーダンス制御手段４は制御手段（制御部）（又は動作制御手段、又は動作制御部）の一例として機能する。
【００５５】
以上のインピーダンス制御を利用すれば、図３に示すような人１００とロボット９０の物体３８の協調搬送のような協働作業が可能となる。人１００が物品３８を移動しようとして、物体３８に力をかけると、物体３８を通じて力がロボット９０のロボットアーム５に伝わり、前記力が、ロボットアーム５の力センサー３により外力Ｆｓとして検出される。インピーダンス制御手段４への入力となる外力Ｆは、協調搬送する物体３８の重力を補償するため、重力補償手段３０において、力センサー３により検出された外力Ｆｓを基に式（５）で計算を行い、算出されるＦを使用する。
【００５６】
【数１０】
2011080856.tif 000052
．．．．．（５）
ただし、ｍは物体３８の質量、ｇは物体３８の重力加速度である。式（５）で計算される外力Ｆを入力にインピーダンス制御を行えば、ロボットアーム５は人１００がかけた力の方向に沿うように動作するため、協調搬送が実現する。なお、物体３８の質量ｍ、及び、物体３８の重力加速度ｇは、重力補償手段３０に予め記憶されている。
【００５７】
本発明の第１実施形態のロボットの制御装置１の特徴は、以上の基本的なインピーダンス制御手段４の構成に加え、関節可動状態計算手段２及び力変換手段２４を有するところにあり、以下、関節可動状態計算手段２及び力変換手段２４の詳細について説明する。
【００５８】
関節可動状態計算手段２は、ロボットアーム５の関節角の現在値（関節角度ベクトル）ｑが入力されることで各関節１１，１２，１３，１４，１５，１６の可動状態を判定し（具体的には、第１〜第６関節１１，１２，１３，１４，１５，１６のそれぞれの関節又は少なくとも１つの関節が可動範囲のいずれかに位置して可動状態を計算し）、その判定結果（計算結果）を力変換手段（力変換部、又は外力変換手段、又は外力変換部）２４へと出力する。
【００５９】
図４は関節角度ｑｉ（以下、ｉ＝１，２，・・・，６）と可動状態の関係を示す図であり、紙面左回りが関節ｉの正方向の回転を表している。関節可動状態計算手段２は、各関節１１，１２，１３，１４，１５，１６の可動範囲の最小値ｑｉＭＩＮ及び最大値ｑｉＭＡＸ、及び緩衝領域幅ｄｑｉにより、可動領域ＭＲと、可動領域ＭＲの両端の境界線から一定範囲だけ内側に配置された第１緩衝領域ＢＲ１と第２緩衝領域ＢＲ２とを定義する。第１緩衝領域ＢＲ１と第２緩衝領域ＢＲ２は、それぞれ、前記関節の前記可動状態を徐々に変化させる領域である。そして、関節可動状態計算手段２は、現在の関節角ｑｉが属する領域に応じてパターン分けし、各関節１１，１２，１３，１４，１５，１６の可動状態Ｌｉを０〜１の間の値として計算する。
【００６０】
さらに、インピーダンス制御手段４には、多関節ロボットアーム５にかかる外力Ｆにより各関節に発生する外力トルクτを計算する外力トルク計算手段５０を備えている。具体的には、外力トルク計算手段５０において、式（６）により外力Ｆによる各関節１１，１２，１３，１４，１５，１６に発生するトルクτ＝［τ１，τ２，τ３，τ４，τ５，τ６］Ｔを計算する。そして、関節可動状態計算手段２において、各関節１１，１２，１３，１４，１５，１６が緩衝領域ＢＲ１，ＢＲ２内あるいは可動領域外にあるときに、前記外力トルク計算手段５０で計算されて求められたトルクτが、各関節を可動領域ＭＲ内へ戻そうとする方向のトルクであると関節可動状態計算手段２で判定される場合（図４において矢印Ｘ及びＹの方向、後述のパターンＢ２，Ｃ２，Ｄ２，Ｅ２の場合）、予め定めた一定のオフセット値ｄＬｉを可動状態Ｌｉに関節可動状態計算手段２で加算する。また、関節可動状態計算手段２において、各関節が緩衝領域ＢＲ１，ＢＲ２内あるいは可動領域外にあるときに、前記計算されて求められたトルクが各関節を可動領域ＭＲ内へ戻そうとする方向のトルクではないと関節可動状態計算手段２で判定される場合には、オフセット値ｄＬｉは可動状態Ｌｉに関節可動状態計算手段２で加算しない。
【００６１】
【数１１】
2011080856.tif 000053
．．．．．（６）
前記式（６）において、Ｊｖは
【数１２】
2011080856.tif 000054
の関係を満たすヤコビ行列であり、
【数１３】
2011080856.tif 000055
で、
【数１４】
2011080856.tif 000056
はロボットアーム５の手先の回転速度である。
【００６２】
オフセット値ｄＬｉの具体的な数値は、操作のしやすさ、又は、制御の安定性を考慮して、実際にロボットアーム５を動作させて予め実験的に定めて、関節可動状態計算手段２内に記憶させることになる。
【００６３】
オフセット値ｄＬｉは、後述するように、関節ｉを可動領域ＭＲ内へ戻そうとするトルクに対する関節ｉの反応を関節可動状態計算手段２で決定し、かつ、可動領域ＭＲの境界で発生するリミットサイクルのような現象の発生度合いを決める値となる。具体的には、オフセット値ｄＬｉの値が大きくなるほど可動領域ＭＲへ戻そうとする力に対する反応が良くなり、操作が容易になる一方で、オフセット値ｄＬｉの値が大きくなるほどリミットサイクルのような現象により不安定性が増大することになる。したがって、オフセット値ｄＬｉの値の具体的な数値は、前記相反する反応の良さからくる操作性と制御の安定性とを考慮し、実際にロボットアーム５を動作させて予め実験的に定めて関節可動状態計算手段２内に記憶させることになる。
【００６４】
ここで言う「操作のしやすさ」とは、オフセット値ｄＬｉの値が小さいほど、大きな力をかけないと戻らなくなるので、操作性が悪いのに対し、オフセット値ｄＬｉの値が大きいほど、小さな力で容易に戻ることになるので、操作性が良いと呼んでいる。一方、オフセット値ｄＬｉの値が大きいほど、リミットサイクルのような現象が起こり、具体的には振動が発生し、制御が不安定になります。すなわち、ここで言う「制御の安定性」とは、制御が原因で振動が発生し運動が安定しないことを意味する。
【００６５】
以上を具体的に記述すると、以下のようにパターン分けして、関節ｉの可動状態Ｌｉが関節可動状態計算手段２で計算される。
【００６６】
パターンＡ：  （ｑｉＭＩＮ＋ｄｑｉ）≦ｑｉ≦（ｑｉＭＡＸ−ｄｑｉ）：すなわち、（完全可動領域ＰＭＲ内）のとき：Ｌｉ＝１
パターンＢ：  （ｑｉＭＩＮ）＜ｑｉ＜（ｑｉＭＩＮ＋ｄｑｉ）：すなわち、（第１緩衝領域ＢＲ１内）のとき：
パターンＢ１：  τｉ≦０の場合：  Ｌｉ＝（ｑｉ−ｑｉＭＩＮ）／ｄｑｉ
パターンＢ２：  τｉ＞０の場合：  Ｌｉ＝（ｑｉ−ｑｉＭＩＮ）／ｄｑｉ＋ｄＬｉ
（ただし、  Ｌｉ≦１に制限）
パターンＣ：  （ｑｉＭＡＸ−ｄｑｉ）＜ｑｉ＜（ｑｉＭＡＸ）：すなわち、（第２緩衝領域ＢＲ２内）のとき：
パターンＣ１：  τｉ≧０の場合：  Ｌｉ＝（ｑｉＭＡＸ−ｑｉ）／ｄｑｉ
パターンＣ２：  τｉ＜０の場合：  Ｌｉ＝（ｑｉＭＡＸ−ｑｉ）／ｄｑｉ＋ｄＬｉ
（ただし、  Ｌｉ≦１に制限）
パターンＤ：  ｑｉ≦ｑｉＭＩＮ：すなわち、（可動領域外）のとき：
パターンＤ１：  τｉ≦０の場合：  Ｌｉ＝０
パターンＤ２：  τｉ＞０の場合：  Ｌｉ＝ｄＬｉ
パターンＥ：  ｑｉＭＡＸ≦ｑｉ：すなわち、（可動領域外）のとき：
パターンＥ１：  τｉ≧０の場合：  Ｌｉ＝０
パターンＥ２：  τｉ＜０の場合：  Ｌｉ＝ｄＬｉ
力変換手段２４は、関節可動状態計算手段２の出力である可動状態Ｌｉに基づき、外力Ｆを、前記外力Ｆがロボットアーム５に加えられたときにロボットアーム５が動作可能な方向成分を抽出可能な変換外力Ｆｈに変換する。
【００６７】
変換外力Ｆｈの変換方法に関しては、２つの方法を以下に示す。
【００６８】
第１の変換方法は、以下の式（７）〜（１３）式に基づき変換する方法である。
【００６９】
【数１５】
2011080856.tif 000057
．．．．．（７）
【数１６】
2011080856.tif 000058
．．．．．（８）
【数１７】
2011080856.tif 000059
．．．．．（９）
【数１８】
2011080856.tif 000060
．．．．．（１０）
【数１９】
2011080856.tif 000061
．．．．．（１１）
【数２０】
2011080856.tif 000062
．．．．．（１２）
ただし、ｊ１，ｊ２，ｊ３は式（１３）を満たす３次元ベクトルであり、３×３行列［ｊ１  ｊ２  ｊ３］は
【数２１】
2011080856.tif 000063
の関係を満たすヤコビ行列Ｊｒ（ｑ）の左上の３×３の部分行列に相当する。
【００７０】
【数２２】
2011080856.tif 000064
．．．．．（１３）
第２の変換方法は、関節の可動状態のパターン分類である、パターンＡ〜Ｅに応じてパターン分けする方法である。
【００７１】
パターンα：  関節角度ｑ１、ｑ２、ｑ３のすべてがパターンＡの場合、
【数２３】
2011080856.tif 000065
．．．．．（１０１）
となる。
【００７２】
パターンβ：  関節角度ｑ１、ｑ２、ｑ３のいずれか１つの関節角度ｑがパターンＢ〜Ｅのいずれかの場合（すなわち、関節角度ｑ１、ｑ２、ｑ３のその他の２つの関節角度ｑがパターンＡの場合）、
例えば、関節角度ｑｉがパターンＢ〜Ｅのいずれかであり、かつ、関節角度ｑａ、ｑｂがパターンＡとすると（ただし、この例では、ｑｉは、ｑ１、ｑ２、ｑ３のいずれか１つの関節角度を意味し、ｑａ、ｑｂは、関節角度ｑ１、ｑ２、ｑ３のその他の２つの関節角度を意味する。）、
【００７３】
【数２４】
2011080856.tif 000066
．．．．．（１０２）
となる。
【００７４】
パターンγ：  関節角度ｑ１、ｑ２、ｑ３のいずれか２つの関節角度ｑがパターンＢ〜Ｅのいずれかの場合（すなわち、関節角度ｑ１、ｑ２、ｑ３の残りの１つの関節角度ｑがパターンＡの場合）、
例えば、関節角度ｑｉがパターンＡであり、かつ、関節角度ｑａ、ｑｂがパターンＢ〜Ｅのいずれかであるとすると（ただし、この例では、ｑｉは、関節角度ｑ１、ｑ２、ｑ３のいずれか１つの関節角度を意味し、ｑａ、ｑｂは、関節角度ｑ１、ｑ２、ｑ３のその他の２つの関節角度を意味する。）、
【数２５】
2011080856.tif 000067
．．．．．（１０３）
となる。
【００７５】
パターンδ：  関節角度ｑ１、ｑ２、ｑ３のすべてがパターンＢ〜Ｅのいずれかの場合、
【数２６】
2011080856.tif 000068
．．．．．（１０４）
となる。
【００７６】
その他の式（７）、式（１１）、式（１２）は第１の変換方法と同様である。
【００７７】
上記第１の変換方法は、変換式のパターン分けをする必要が無く、簡潔なアルゴリズムとすることができるという利点を有するが、図１に示すロボットアーム５の第２関節１２，第３関節１３のように平行な関節軸を持つアーム構造の場合に、平行な関節軸の関節が同時に完全可動領域外になる場合に限り対応することはできない。
【００７８】
一方、第２の変換方法は、変換式のパターン分けをする必要があるが、アームの構造に関係なく、複数の関節が完全可動領域外になる場合も含め、あらゆる場合に対応可能である。
【００７９】
したがって、アームの構造又は各関節の可動範囲の大きさ等に応じて、上記第１の変換方法と第２の変換方法を選択するように、力変換手段２４において、予め設定しておけばよい。
【００８０】
図５は、上述した原理に基づく制御プログラムによるインピーダンス制御時の動作ステップを説明するフローチャートである。
【００８１】
ステップＳ１では、エンコーダ４２により計測された関節角度データ（関節角度ベクトルｑ）が制御装置１に取り込まれる。
【００８２】
ステップＳ２では、ロボットアーム５の運動学計算に必要なヤコビ行列Ｊｒ等の計算が近似逆運動学計算手段２８により行われる。
【００８３】
ステップＳ３では、ロボットアーム５からの関節角度データ（関節角度ベクトルｑ）から、ロボットアーム５の現在の手先位置・姿勢ベクトルｒが順運動学計算手段２６により計算される（順運動学計算手段２６での処理）。
【００８４】
ステップＳ４では、関節可動状態計算手段２において、ロボットアーム５からの関節角度データ（関節角度ベクトルｑ）に基づいて、パターンＡ〜パターンＥのいずれになるかを判定し、関節可動状態Ｌｉを算出する。
【００８５】
ステップＳ５では、力センサー３の計測値Ｆｓが重力補償手段３０に取り込まれ、式（５）に基づき外力Ｆが重力補償手段３０で計算される。
【００８６】
ステップＳ６では、力変換手段２４において、式（７）〜式（１３）（第１の変換方法）、又は、式（７）、式（１１）、式（１２）、式（１０１）〜式（１０４）（第２の変換方法）に基づいて、重力補償手段３０で計算された外力Ｆが変換外力Ｆｈに変換される。
【００８７】
ステップＳ７では、機械インピーダンスパラメータＭ、Ｄ、Ｋと、関節角度データ（関節角度ベクトルｑ）と、力変換手段２４において変換された変換外力Ｆｈとから、手先位置・姿勢目標補正出力ｒｄΔが、インピーダンス計算手段２５により計算される（インピーダンス計算手段２５での処理）。
【００８８】
ステップＳ８では、手先位置・姿勢目標ベクトルｒｄと手先位置・姿勢目標補正出力ｒｄΔの和が第１演算部６１で計算されて求められた手先位置・姿勢補正目標ベクトルｒｄｍと、現在の手先位置・姿勢ベクトルｒとの差である手先位置・姿勢の誤差ｒｅが、第２演算部６２で計算される（位置誤差補償手段２７での処理）。位置誤差補償手段２７の具体例としてはＰＩＤ補償器が考えられる。定数の対角行列である比例、微分、積分の３つのゲインを適切に調整することにより、位置誤差が０に収束するように制御が働く。
【００８９】
ステップＳ９では、ステップＳ２で計算したヤコビ行列Ｊｒの逆行列を乗算することにより、位置誤差補償出力ｕｒｅを手先位置・姿勢の誤差に関する値から、関節角度の誤差に関する値である関節角度誤差補償出力ｕｑｅに、近似逆運動学計算手段２８により変換する（近似逆運動学計算手段２８での処理）。
【００９０】
ステップＳ１０では、関節角度誤差補償出力ｕｑｅが、近似逆運動学計算手段２８からＤ／Ａボード２０を通じてモータードライバ１８に与えられ、各モータ４１を流れる電流量を変化させることによりロボットアーム５の各関節の関節軸の回転運動が発生する。
【００９１】
以上のステップＳ１〜ステップＳ１０が、制御の計算ループとして繰り返し実行されることにより、ロボットアーム５の動作の制御が実現する。なお、ステップＳ２〜ステップＳ６の動作の順番はパラレル処理でも可能であり、必ずしも、この順番通りでなくてもよい。
【００９２】
本発明の前記第１実施形態にかかるロボット９０では、関節可動状態計算手段２及び力変換手段２４を備えることにより、以下に詳述するような動作及び効果が実現する。以下では、人１００とロボット９０が協調搬送を行う場合の動作を第３関節１３を中心に説明を行う。
【００９３】
図６は、式（８）に現れる並進外力ｆｈ、ｆｈ２の関係を示す。図６においてはロボットアーム５の第１関節１１から第３関節１３までを模式的に表している。拘束曲面３１は、第３関節１３が可動領域ＭＲを越えて、それ以上動けずロックされた状態の場合に、その他の第１関節１１及び第２関節１２が動くことで、ロボットアーム５の手先のハンド６が動くことのできる曲面である。拘束曲面３１は、実際は球面であるが、図６では、ある一断面の円のみ描いている。すなわち、第３関節１３がロック状態になると、ロボットアーム５の手先は拘束曲面３１に拘束される。
【００９４】
この拘束曲面３１に接する平面であり、かつ、ロボットアーム５の手先を通る平面が、接平面３２である。また、接平面３２は、ベクトルｊ１，ｊ２を含む平面に平行な平面である。
【００９５】
この接平面３２への並進外力ｆｈ２の正射影が、ｆｈＣである。正射影ｆｈＣは、式（８）において第３関節１３の可動状態Ｌ３＝０のときの並進外力ｆｈであり、式（１４）で表される。なお、式（１４）は式（８）において「｛」と「｝」とで囲まれた部分であり、式（８）を使って、変換外力Ｆｈを計算することは、正射影への変換を使用していることになる。また、正射影への変換を、式（８）だけでなく、式（９）、式（１０）の「｛」と「｝」とで囲まれた部分も、それぞれ、同様に、正射影への変換を使用していることになる。また、正射影ｆｈＣは変換外力Ｆｈの一例である。すなわち、拘束曲面３１の接平面３２への正射影を考えれば、ロボットアーム５が動作可能な方向成分となる。
【００９６】
【数２７】
2011080856.tif 000069
．．．．．（１４）
また、並進外力ｆｈは、並進外力ｆｈ２のベクトルの先端と、並進外力ｆｈ２の正射影ｆｈＣのベクトルの先端とを結んだ直線（点線）上に、先端が来る中間的な値であり、その位置は可動状態Ｌ３により定義される（可動状態Ｌ３＝０のときにはｆｈ＝ｆｈＣ、可動状態Ｌ３＝１のときにはｆｈ＝ｆｈ２）。
【００９７】
第３関節１３の手先位置が、完全可動領域ＰＭＲ内にある緩衝領域境界３３の内側である点Ｐ１に位置するときは、パターンＡ（可動状態Ｌ３＝１）となるので、ｆｈ＝ｆｈ２となり、人のかける力の方向にロボットアーム５の手先は自由に動作する。ここで、第１関節１１及び第２関節１２も完全可動領域ＰＭＲ内にある場合に限れば、ｆｈ２＝ｆとなり、人のかける力の方向にロボットアーム５の手先は自由に動作する。
【００９８】
手先が緩衝領域境界３３を越え、外側の点Ｐ２に移動すると、パターンＣ１になり、可動状態Ｌ３が変化すると共に変化する並進外力ｆｈでインピーダンス制御が行われ、徐々に動きに拘束がかかるようになる。
【００９９】
手先が可動領域ＭＲの境界である拘束曲面３１上の点Ｐ３に到達すると、パターンＥ１になり、並進外力ｆｈＣを入力にインピーダンス制御を行うことになり、ロボットアーム５の手先の動きは拘束曲面３１に沿うような動きとなる。すなわち、ｆＶのような成分を持つ力を人からロボットアーム５にかけても、式（８）による、並進外力ｆｈへの変換の際に打ち消されるため、ロボットアーム５の手先を、拘束曲面３１を越えて外側に動くことはできず、第３関節１３の角度も可動領域ＭＲを越えることはない。
【０１００】
一方、点Ｐ３において、−ｆＶのような成分を持つ力を人からロボットアーム５にかけた場合、第３関節１３は可動領域ＭＲ内に戻る動きとなるため、パターンＥ２となり、可動状態Ｌ３＝ｄＬ３とオフセット値が加算される分、並進外力ｆｈは復元力のように働き、関節が可動領域ＭＲ内に戻る動きが可能となる。
【０１０１】
また、同様に点Ｐ２においても、−ｆＶのような成分を持つ力を人からロボットアーム５にかけた場合、第３関節１３は緩衝領域ＢＲ１から完全可動領域ＰＭＲ内に戻る動きとなるため、パターンＣ２となり、可動状態Ｌ３＝（ｑ３ＭＡＸ−ｑ３）／ｄｑ３＋ｄＬ３とオフセット値が加算される分、並進外力ｆｈは復元力のように働き、関節が完全可動領域ＰＭＲ内に戻る動きが可能となる。
【０１０２】
前記した動作制限において、緩衝領域ＢＲ１，ＢＲ２では、徐々に動作に拘束をかけることで、動作を安定させる役割を担っている。緩衝領域ＢＲ１，ＢＲ２を設けない場合、手先が拘束曲面３１上に到達すると、インピーダンス計算手段２５への入力が並進外力ｆｈ２から並進外力ｆｈＣへステップ的に変化することになるので、制御系、すなわち、インピーダンス制御手段４が不安定になりやすい。
【０１０３】
また、パターンＤ２又はパターンＥ２のように可動領域ＭＲ内へ戻る力を受け付ける場合に、拘束曲面３１の近傍でリミットサイクルのような現象が起こり、制御系、すなわち、インピーダンス制御手段４がさらに不安定となる可能性がある。これに対して、緩衝領域ＢＲ１，ＢＲ２を設け、オフセット値ｄＬｉを導入することで、リミットサイクルを起こさず、スムーズに動作の拘束・復帰を行うことが可能となる。
【０１０４】
なお、パターンＢ（パターンＢ１，パターンＢ２）は、第１緩衝領域ＢＲ１に関するものであり、パターンＡの第１緩衝領域ＢＲ１とは反対側の領域である第２緩衝領域ＢＲ２のパターンＣ（パターンＣ１，パターンＣ２）と同様であるため、説明を省略する。パターンＤ（パターンＤ１，パターンＤ２）は、第１緩衝領域ＢＲ１の可動領域外に関するものであり、パターンＡの第１緩衝領域ＢＲ１とは反対側の領域である第２緩衝領域ＢＲ２の可動領域外のパターンＥ（パターンＥ１，パターンＥ２）と同様であるため、説明を省略する。
【０１０５】
以上は、第３関節１３の場合を中心に説明を行ったが、第１関節１１及び第２関節１２の場合も同様であり、第１の変換方法の場合には、それぞれ、式（９）及び式（１０）に基づく変換を行うことにより、また、第２の変換方法の場合には、パターンβの変換を行うことにより、各関節の可動領域ＭＲに応じたロボットアーム５の動作の拘束が可能となる。そして、第１の変換方法の場合、一連の式（７）〜式（１１）を使うことにより、第１関節１１と、第２関節１２と、第３関節１３とに対して、平行な関節軸の関節が同時に完全可動領域外になる場合を除き、同時に動作制限を行うことが可能となる。
【０１０６】
第２の変換方法の場合、パターンα〜パターンδのパターン分けにより、複数の関節が同時にロックする場合も厳密に扱うことができる。
【０１０７】
図１２は、式（１０３）に現れる並進外力ｆｈ、ｆの関係を示す。図１２においては、ロボットアーム５の第１関節１１から第３関節１３までを模式的に表している。拘束曲線２０１は、第２関節１２と第３関節１３が可動領域ＭＲを越えて、それ以上動けずロックされた状態の場合に、残りの第１関節１１が動くことで、ロボットアーム５の手先のハンド６が動くことのできる曲線である。すなわち、第２関節１２と第３関節１３がロック状態になると、ロボットアーム５の手先は拘束曲線２０１に拘束される。
【０１０８】
この拘束曲線２０１の接線であり、かつ、ロボットアーム５の手先を通る直線が、接線２０２である。また、接線２０２は、ベクトルｊ１に平行な直線である。
【０１０９】
この接線２０２への並進外力ｆの正射影が、ｆｈｃである。正射影ｆｈｃは、式（１０３）において第２関節１２の可動状態Ｌ２＝０かつ第３関節１３の可動状態Ｌ３＝０のときの並進外力ｆｈであり、式（１０５）で表される。なお、式（１０５）はＬ２＝０の場合の式（１０３）であり、式（１０３）を使って、変換外力Ｆｈを計算することは、正射影への変換を使用していることになる。すなわち、拘束曲線２０１の接線２０２への正射影を考えれば、ロボットアーム５が動作可能な方向成分となる。
【０１１０】
【数２８】
2011080856.tif 000070
．．．．．（１０５）
また、並進外力ｆｈは、並進外力ｆのベクトルの先端と、並進外力ｆの正射影ｆｈｃのベクトルの先端とを結んだ直線（点線）上に、先端が来る中間的な値であり、その位置は可動状態Ｌ２Ｌ３により定義される（可動状態Ｌ２Ｌ３＝０のときにはｆｈ＝ｆｈＣ、可動状態Ｌ３＝１のときにはｆｈ＝ｆ２）。
【０１１１】
以上のように式（１０５）に基づく変換によれば、第２関節１２と第３関節１３の２つの関節がロックする場合の動作も扱うことが可能となる。
【０１１２】
ロボットアーム５の手先の姿勢（φ、θ、ψ）を決める第４関節１４、第５関節１５、第６関節１６に関しては、式（１２）に基づくことで各関節の動作制限が実現する。式（１２）において、Ｊｖ（ｑ）ＴＦの部分は式（６）右辺と同様であり、外力Ｆにより各関節１１，１２，１３，１４，１５，１６に発生するトルクτ＝［τ１，τ２，τ３，τ４，τ５，τ６］Ｔに相当する。このトルクベクトルの第３行，第４行，第５行に、それぞれ第４関節１４、第５関節１５、第６関節１６の関節可動状態Ｌ４，Ｌ５，Ｌ６をかけたベクトル［τ１，τ２，τ３，Ｌ４τ４，Ｌ５τ５，Ｌ６τ６］Ｔを、式Ｊｖ（ｑ）−Ｔに用いて手先外力に逆変換し、モーメント成分のみを抽出したのがｎｈである。したがって、例えば、Ｌ４＝０とすると、τ４＝０となるので、モーメント成分ｎｈを入力としてインピーダンス制御すると、第４関節１４は動作せず固定されることになる。
【０１１３】
第５関節１５及び第６関節１６に関しても同様であり、以上の原理により、関節可動状態Ｌ４，Ｌ５，Ｌ６に基づき、ロボットアーム５の手先の姿勢（φ、θ、ψ）を決める手首部７である第４関節１４と第５関節１５と第６関節１６との動作制限が実現する。
【０１１４】
以上、本第１実施形態によれば、関節可動状態計算手段２及び力変換手段２４を有することにより、インピーダンス制御による動作時に人がロボットアーム５の関節の可動領域ＭＲを越えるような操作を行っても、関節可動状態計算手段２において当該関節の可動状態を判定し、力変換手段２４により変換された変換外力Ｆｈをインピーダンス計算手段２５への入力として使うことで、ロボットアーム５の動作制限が可能となり、関節の可動領域ＭＲを越えるのを防ぐことができる。
【０１１５】
また、本第１実施形態によれば、インピーダンス計算手段２５への入力に対し力変換手段２４により変換をかけることにより動作制限を行う、すなわち、入力を外力Ｆから変換外力Ｆｈへと制限することで動作制限を行うため、ロボットアーム５の機構的な可動領域ＭＲを越えてロボットアーム５が動けない状態でも、インピーダンス計算手段２５に入力が入り続け、後段にあるインピーダンス計算手段２５又は位置制御系２９に誤差が蓄積してしまうことがなく、安定な制御系とすることができるという格別の効果を発揮する。
【０１１６】
さらに、力変換手段２４を使わずに、位置制御系２９において関節の動作制限を行う場合が（公知技術として）考えられるが、その場合は、後段にあるインピーダンス計算手段２５又は位置制御系２９に誤差蓄積に対する対策を施す必要があり、複雑な制御系構造となってしまう。これに対して、力変換手段２４を使う本第１実施形態では、誤差蓄積に対する対策を施す必要はなく、簡潔な制御系構造とすることができる。
【０１１７】
また、前記第１実施形態では、インピーダンス制御のインピーダンスパラメータを変化させることで動作制限を行う方式ではないため、パラメータ可変のための仕組みが不要であり、制御系の構造を簡潔にすることができ、さらに、パラメーター変化により制御系が不安定になることを防ぐことができる。すなわち、前記第１実施形態では、力変換手段２４により入力制限を行うこと（言い換えれば、力変換手段２４により外力Ｆが変換された変換外力Ｆｈをインピーダンス計算手段２５への入力として使うこと）で、前記優れた効果を発揮させることができるので、インピーダンスパラメータを変化させる必要はない。
【０１１８】
さらに、本第１実施形態によれば、各関節の動作制限は独立に行われるため、仮にある関節が動作制限されても、動作可能な他の関節は動作制限されず動き続けることが可能であるので、すぐに動作制限がかかり停止するようなことのない、操作性の良いロボットとすることができる。
【０１１９】
（第２実施形態）
本発明の第２実施形態におけるロボットの基本的な構成は、図１及び図２に示した第１実施例の場合と同様であるので、共通部分の説明は省略し、異なる部分についてのみ以下、詳細に説明する。
【０１２０】
図７に示すように、第２実施形態では、通常のインピーダンス制御手段４３（すなわち、インピーダンス計算手段２５と目標軌道生成手段２３と順運動学計算手段２６と位置誤差補償手段２７と近似逆運動学計算手段２８と第１演算部６１と第２演算部６２）とは独立した力入力制御手段３４に、関節可動状態計算手段２と力変換手段２４とを配設し、既存の制御装置（第１制御装置）１Ｂと通信する構成としている。
【０１２１】
図８は、第２実施形態のロボット９０Ｂの全体構成を示しており、制御装置の具体的な構成を示している。第２実施形態の制御装置では、第１制御装置１Ｂとは別に第２制御装置４６が追加され、２つの独立した制御装置１Ｂ，４６で構成される。
【０１２２】
第２制御装置４６は、ハードウェア的には一般的なパーソナルコンピュータにより構成されている。そして、入出力ＩＦ（Ｄ／Ａボード４５とＡ／Ｄボード４７とカウンタボード４８）を除く部分は、パーソナルコンピュータで実行される制御プログラム４４により力入力制御手段３４を実現する。力入力制御手段３４は、Ａ／Ｄボード４７を通じて力センサー３より外力Ｆｓが入力され、力変換手段２４による変換結果である変換外力Ｆｈが、Ｄ／Ａボード４５を通じて第１制御装置１Ｂの力センサー入力であるＡ／Ｄボード２１に出力され、Ａ／Ｄボード２１を通じて第１制御装置１Ｂに取り込まれる。このように、Ｄ／Ａボード４５とＡ／Ｄボード２１とを接続することで、電圧値により変換外力Ｆｈが伝えられる通信手段が構成されている。
【０１２３】
また力入力制御手段３４は、カウンタボード４８を通じてロボットアーム５の各関節部のエンコーダ４２より出力される関節角度情報を取得し、関節可動状態計算手段２による、関節１１，１２，１３，１４，１５，１６の可動状態の判定のための情報として使用される。
【０１２４】
本第２実施形態は、前記通常のインピーダンス制御手段４３に、力入力制御手段３４を別途追加した構成であるため、図７のような、２つに分離独立した構成が容易に実現できる。したがって第１実施形態は、図１に示すように一つの制御装置（ＰＣ）１上で動作する１つの制御プログラム１７によりインピーダンス制御手段４を実現したが、第２実施形態は、図８に示すように第１制御装置１Ｂに第２制御装置４６を加え、既存の第１制御装置１Ｂと通信する構成が可能となる。
【０１２５】
この構成により、例えば、市販のロボット制御装置の力センサーからの入力信号を処理するために第２制御装置４６を追加するだけで、ロボットアーム５の動作制限を行なうことができる。
【０１２６】
（第３実施形態）
本発明の第３実施形態における制御装置の基本的な構成は、図１及び図２に示した第１実施例の場合と同様であるので、共通部分の説明は省略し、異なる部分についてのみ以下、詳細に説明する。
【０１２７】
図９は、本発明の第３実施形態におけるロボット９０Ｃのインピーダンス制御手段の構成を示すブロック線図であり、図１０はロボット９０Ｃによる人１００との協調搬送作業を説明する図である。
【０１２８】
図１０に示すように、第３実施形態のロボット９０Ｃは、手首部７に操作ハンドル４０が力センサー３を介して固定されおり、人１００は操作ハンドル４０を直接握り、力をかけることでロボット９０Ｃを操作することができる。操作ハンドル４０とロボットアーム５の間には力センサー３が配設されており、人１００が操作するときの力を力センサー３で検出することができる。
【０１２９】
また、操作ハンドル４０には、動作モード設定部の一例としての動作モード設定スイッチ３９が設けられており、人１００は、動作モード入力部の一例として機能する動作モード設定スイッチ３９を操作することにより、動作モード（可動又は固定のモード）の設定及び切換を行うことができる。
【０１３０】
図９に示すように、動作モード設定スイッチ３９から動作モード指令が関節動作モード設定手段３７に入力される。さらに、関節可動状態計算手段２から関節可動状態Ｌｉが関節動作モード設定手段３７に入力され、力変換手段２４に拡張関節可動状態Ｌｉｍが出力される。
【０１３１】
図１０に示すような人１００とロボット９０Ｃとの協調作業形態の場合、物体３８の負荷をすべてロボットアーム５が受け持つため、重力補償手段３０は不要である。この場合には、力センサー３の計測値Ｆｓが力変換手段２４に外力Ｆとして入力されて、変換外力Ｆｈに変換される。
【０１３２】
動作モード設定スイッチ３９では、ロボットアーム５の６つの関節１１，１２，１３，１４，１５，１６のそれぞれの可動又は固定のモード設定を行うことができる。
【０１３３】
インピーダンス制御手段４内に備えられた関節動作モード設定手段３７は、動作モード設定スイッチ３９で設定した各関節の可動又は固定のモード設定に応じて、可動のモード設定の場合はそのまま可動状態Ｌｉを拡張関節可動状態Ｌｉｍとして出力し、固定のモード設定の場合は拡張関節可動状態Ｌｉｍ＝０を出力する。これらの拡張関節可動状態Ｌｉｍは、前記第１実施形態で説明した可動状態Ｌｉと同様に取り扱い、変換手段２４に入力されて使用される。これにより、固定のモード設定された関節は、第１実施形態で説明した原理により、その動作が制限されて固定される。
【０１３４】
以上のように、本第３実施形態によれば、関節動作モード設定手段３７を有することにより、特定の関節を固定した動作、又は、特定の関節のみを回転させるような動作が容易に可能となり、より操作性の良いロボットとすることができる。
【０１３５】
第３実施形態にかかるロボットの動作の具体例としては、図１１に示すように、第１関節１１のみ回転させ、ロボットアーム５を台部１０に対して旋回させるような動作をさせるときには、動作モード設定スイッチ３９で第１関節１１を可動と設定し、第２〜第６関節１２〜１６を固定に設定すれば、人１００が操作ハンドル４０に力をかけるだけでロボットアーム５の第１関節１１のみを容易に回転させる旋回動作を実現することができる。
【０１３６】
なお、前記様々な実施形態のうちの任意の実施形態を適宜組み合わせることにより、それぞれの有する効果を奏するようにすることができる。
【産業上の利用可能性】
【０１３７】
本発明のロボット、ロボットの制御装置、及び制御方法は、ロボットアームの関節の動作範囲の規制など、関節の動作状況に応じたロボットアームの動作制限を、簡潔な制御系構造で実現し、制御系の不安定さを低減することができて、工場又は家庭内、介護現場等でパワーアシスト等の作業支援を行うロボット等、人と協働するロボット、ロボットの制御装置、及び制御方法として有用である。
【０１３８】
本発明は、添付図面を参照しながら好ましい実施形態に関連して充分に記載されているが、この技術の熟練した人々にとっては種々の変形又は修正は明白である。そのような変形又は修正は、添付した請求の範囲による本発明の範囲から外れない限りにおいて、その中に含まれると理解されるべきである。
【手続補正２】
【補正対象書類名】特許請求の範囲
【補正対象項目名】全文
【補正方法】変更
【補正の内容】
【特許請求の範囲】
【請求項１】
多関節ロボットアームと、
前記多関節ロボットアームに配設され、かつ、外力を検出する外力検出手段と、
前記多関節ロボットアームの関節が可動範囲のいずれかに位置して可動状態を計算する関節可動状態計算手段と、
前記関節可動状態計算手段により計算された前記関節の前記可動状態に基づき、前記外力検出手段が検出した前記外力を、前記外力が前記ロボットアームに加えられたときに前記ロボットアームが動作可能な方向成分を抽出可能な変換外力に変換する外力変換手段と、
前記外力変換手段により変換された前記変換外力に基づいてインピーダンス制御を行うことにより、前記多関節ロボットアームの前記関節の前記可動状態に応じた動作制限が行われるように前記多関節ロボットアームを制御する制御手段とを有し、
前記外力変換手段は、前記外力検出手段に検出された前記外力を、前記多関節ロボットアームの前記関節が固定されたときに前記多関節ロボットアームの手先が動くことのできる拘束曲面の接平面への正射影として変換するロボット。
【請求項２】
多関節ロボットアームと、
前記多関節ロボットアームに配設され、かつ、外力を検出する外力検出手段と、
前記多関節ロボットアームの関節が可動範囲のいずれかに位置して可動状態を計算する関節可動状態計算手段と、
前記関節可動状態計算手段により計算された前記関節の前記可動状態に基づき、前記外力検出手段が検出した前記外力を、前記外力が前記ロボットアームに加えられたときに前記ロボットアームが動作可能な方向成分を抽出可能な変換外力に変換する外力変換手段と、
前記外力変換手段により変換された前記変換外力に基づいてインピーダンス制御を行うことにより、前記多関節ロボットアームの前記関節の前記可動状態に応じた動作制限が行われるように前記多関節ロボットアームを制御する制御手段とを有し、
さらに、前記多関節ロボットアームにかかる前記外力により各関節に発生する外力トルクを計算する外力トルク計算手段を有し、
前記関節可動状態計算手段は、前記外力トルク計算手段の計算結果に基づき前記外力トルクが前記多関節ロボットアームの各関節の動作を可動領域外より可動領域内に戻そうとする力であるかどうかを判定し、
前記関節可動状態計算手段が、前記外力トルクが前記多関節ロボットアームの各関節の動作を前記可動領域外より前記可動領域内に戻そうとする力であると判定する場合には、前記制御手段は前記動作制限を行わない一方、
前記関節可動状態計算手段が、前記外力トルクが前記多関節ロボットアームの各関節の動作を前記可動領域外より前記可動領域内に戻そうとする力ではないと判定する場合には、前記制御手段は前記動作制限を行うロボット。
【請求項３】
多関節ロボットアームと、
前記多関節ロボットアームに配設され、かつ、外力を検出する外力検出手段と、
前記多関節ロボットアームの関節が可動範囲のいずれかに位置して可動状態を計算する関節可動状態計算手段と、
前記関節可動状態計算手段により計算された前記関節の前記可動状態に基づき、前記外力検出手段が検出した前記外力を、前記外力が前記ロボットアームに加えられたときに前記ロボットアームが動作可能な方向成分を抽出可能な変換外力に変換する外力変換手段と、
前記外力変換手段により変換された前記変換外力に基づいてインピーダンス制御を行うことにより、前記多関節ロボットアームの前記関節の前記可動状態に応じた動作制限が行われるように前記多関節ロボットアームを制御する制御手段とを有し、
前記関節可動状態計算手段は、前記外力を関節トルクに変換することにより、前記関節トルクに変換された前記変換外力が、前記多関節ロボットアームの各関節の動作を可動領域外より可動領域内に戻そうとする力であるかどうかを判定し、
前記関節可動状態計算手段が、前記変換外力が、前記多関節ロボットアームの各関節の動作を前記可動領域外より前記可動領域内に戻そうとする力であると判定する場合には、前記制御手段は前記動作制限を行わない一方、
前記関節可動状態計算手段が、前記変換外力が、前記多関節ロボットアームの各関節の動作を前記可動領域外より前記可動領域内に戻そうとする力ではないと判定する場合には、前記制御手段は前記動作制限を行うロボット。
【請求項４】
多関節ロボットアームと、
前記多関節ロボットアームに配設され、かつ、外力を検出する外力検出手段と、
前記多関節ロボットアームの関節が可動範囲のいずれかに位置して可動状態を計算する関節可動状態計算手段と、
前記関節可動状態計算手段により計算された前記関節の前記可動状態に基づき、前記外力検出手段が検出した前記外力を、前記外力が前記ロボットアームに加えられたときに前記ロボットアームが動作可能な方向成分を抽出可能な変換外力に変換する外力変換手段と、
前記外力変換手段により変換された前記変換外力に基づいてインピーダンス制御を行うことにより、前記多関節ロボットアームの前記関節の前記可動状態に応じた動作制限が行われるように前記多関節ロボットアームを制御する制御手段とを有し、
前記多関節ロボットアームの各関節に対し、可動あるいは固定の動作モードを設定する動作モード設定部と、
前記動作モード設定部で設定された前記動作モードの指令が入力されて、前記動作モードと前記関節可動状態計算手段により計算された前記関節の前記可動状態とに基づき前記可動状態の計算を行う関節動作モード設定手段とをさらに備えて、
前記外力変換手段は、前記関節動作モード設定手段により計算された前記関節の前記可動状態に基づき、前記外力検出手段が検出した前記外力を、前記外力が前記ロボットアームに加えられたときに前記ロボットアームが動作可能な方向成分を抽出可能な変換外力に変換し、
前記制御手段は、前記変換外力に基づいてインピーダンス制御を行うことにより、前記多関節ロボットアームの前記関節の前記可動状態に応じた動作制限が行われるロボット。
【請求項５】
多関節ロボットアームと、
前記多関節ロボットアームに配設され、かつ、外力を検出する外力検出手段と、
前記多関節ロボットアームの関節が可動範囲のいずれかに位置して可動状態を計算する関節可動状態計算手段と、
前記関節可動状態計算手段により計算された前記関節の前記可動状態に基づき、前記外力検出手段が検出した前記外力を、前記外力が前記ロボットアームに加えられたときに前記ロボットアームが動作可能な方向成分を抽出可能な変換外力に変換する外力変換手段と、
前記外力変換手段により変換された前記変換外力に基づいてインピーダンス制御を行うことにより、前記多関節ロボットアームの前記関節の前記可動状態に応じた動作制限が行われるように前記多関節ロボットアームを制御する制御手段とを有し、
さらに、前記多関節ロボットアームにかかる前記外力により各関節に発生する外力トルクを計算する外力トルク計算手段を有し、
前記関節可動状態計算手段は、前記外力トルク計算手段の計算結果に基づき前記外力トルクが前記多関節ロボットアームの各関節の動作を可動領域外より可動領域内に戻そうとする力であるかどうかを判定し、
前記関節可動状態計算手段が、前記外力トルクが前記多関節ロボットアームの各関節の動作を前記可動領域外より前記可動領域内に戻そうとする力であると判定する場合には、前記制御手段は前記動作制限を行わない一方、
前記関節可動状態計算手段が、前記外力トルクが前記多関節ロボットアームの各関節の動作を前記可動領域外より前記可動領域内に戻そうとする力ではないと判定する場合には、前記制御手段は前記動作制限を行い、
前記関節可動状態計算手段は、前記関節の可動領域の両端の境界線から一定範囲だけ内側に緩衝領域をそれぞれ設け、前記各緩衝領域では前記関節の前記可動状態を徐々に変化させるロボット。
【請求項６】
多関節ロボットアームと、
前記多関節ロボットアームに配設され、かつ、外力を検出する外力検出手段と、
前記多関節ロボットアームの関節が可動範囲のいずれかに位置して可動状態を計算する関節可動状態計算手段と、
前記関節可動状態計算手段により計算された前記関節の前記可動状態に基づき、前記外力検出手段が検出した前記外力を、前記外力が前記ロボットアームに加えられたときに前記ロボットアームが動作可能な方向成分を抽出可能な変換外力に変換する外力変換手段と、
前記外力変換手段により変換された前記変換外力に基づいてインピーダンス制御を行うことにより、前記多関節ロボットアームの前記関節の前記可動状態に応じた動作制限が行われるように前記多関節ロボットアームを制御する制御手段とを有し、
前記多関節ロボットアームの前記関節の可動領域の両端の内側にそれぞれ設けられた緩衝領域と、前記緩衝領域を除く前記可動領域の残りの領域である完全可動領域ＰＭＲと、可動領域外の領域とに分割し、
前記関節可動状態計算手段は、各領域毎に、前記関節の前記可動状態の値を異ならせるように計算されるロボット。
【請求項７】
多関節ロボットアームを備えるロボットの制御装置であって、
前記多関節ロボットアームの関節が可動範囲のいずれかに位置して可動状態を計算する関節可動状態計算手段と、
前記関節可動状態計算手段により計算された前記関節の前記可動状態に基づき、前記多関節ロボットアームに配設されて外力を検出する外力検出手段が検出した前記外力を、前記外力が前記ロボットアームに加えられたときに前記ロボットアームが動作可能な方向成分を抽出可能な変換外力に変換する外力変換手段と、
前記外力変換手段により変換された前記変換外力に基づいてインピーダンス制御を行うことにより、前記多関節ロボットアームの前記関節の前記可動状態に応じた動作制限が行われるように前記多関節ロボットアームを制御する制御手段とを有するとともに、
前記外力変換手段は、前記外力検出手段に検出された前記外力を、前記多関節ロボットアームの前記関節が固定されたときに前記多関節ロボットアームの手先が動くことのできる拘束曲面の接平面への正射影として変換するロボットの制御装置。
【請求項８】
多関節ロボットアームを備えるロボットの制御方法であって、
前記多関節ロボットアームの関節が可動範囲のいずれかに位置して可動状態を関節可動状態計算手段で計算し、
前記計算された前記関節の前記可動状態に基づき、前記多関節ロボットアームに配設されて外力を検出する外力検出手段が検出した前記外力を、前記外力が前記ロボットアームに加えられたときに前記ロボットアームが動作可能な方向成分を抽出可能な変換外力に外力変換手段で変換することにより、前記外力検出手段に検出された前記外力を、前記多関節ロボットアームの前記関節が固定されたときに前記多関節ロボットアームの手先が動くことのできる拘束曲面の接平面への正射影として前記外力変換手段で変換し、
前記変換された変換外力に基づいてインピーダンス制御を行うことにより、前記多関節ロボットアームの前記関節の前記可動状態に応じた動作制限が行われるように前記多関節ロボットアームを制御手段で制御するロボットの制御方法。
【請求項９】
多関節ロボットアームを備えるロボットの制御プログラムであって、
コンピュータを、
前記多関節ロボットアームの関節が可動範囲のいずれかに位置して可動状態を計算する関節可動状態計算手段と、
前記関節可動状態計算手段により計算された前記関節の前記可動状態に基づき、前記多関節ロボットアームに配設されて外力を検出する外力検出手段が検出した前記外力を、前記外力が前記ロボットアームに加えられたときに前記ロボットアームが動作可能な方向成分を抽出可能な変換外力に変換することにより、前記外力検出手段に検出された前記外力を、前記多関節ロボットアームの前記関節が固定されたときに前記多関節ロボットアームの手先が動くことのできる拘束曲面の接平面への正射影として変換する外力変換手段と、
前記外力変換手段により変換された前記変換外力に基づいてインピーダンス制御を行うことにより、前記多関節ロボットアームの前記関節の前記可動状態に応じた動作制限が行われるように前記多関節ロボットアームを制御する制御手段として機能させるためのロボットの制御プログラム。
【手続補正３】
【補正対象書類名】図面
【補正対象項目名】全図
【補正方法】変更
【補正の内容】
【図１】
2011080856.tif 000071
【図２】
2011080856.tif 000072
【図３】
2011080856.tif 000073
【図４】
2011080856.tif 000074
【図５】
2011080856.tif 000075
【図６】
2011080856.tif 000076
【図７】
2011080856.tif 000077
【図８】
2011080856.tif 000078
【図９】
2011080856.tif 000079
【図１０】
2011080856.tif 000080
【図１１】
2011080856.tif 000081
【図１２】
2011080856.tif 000082
【国際調査報告】
2011080856.tif 000083
2011080856.tif 000084
2011080856.tif 000085