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

Description

本発明は、工場内、又は、家庭内、又は、介護現場等でパワーアシスト等の作業支援を行うロボットアームを備えるロボット等、人と協働するロボット、ロボットの制御装置、及び制御方法に関する。

近年、工場等で人と協調して動作し、パワーアシスト等の作業支援を行う人協調型のロボットが注目されている。こうした人協調型のロボットは人の操作により動作するため、高度な自律機能がなくても実用的な作業が可能であり、また人の制御の下に動作するため安全性が高く、将来は工場内に留まらず、家庭内の生活支援又は介護への応用が期待されている。

一方、人協調型のロボットは、事前プログラミング通りに動作する従来のロボットと異なり、人が操作する故に動作範囲を越えてしまうなど、事前に想定した以上の動作を人が行い、制御が不安定になったり、ロボットを故障させてしまう可能性がある。

こうした課題に対し、従来技術としては、作動範囲を規制するリミッタを設けると共に、搬送物がリミッタを越えた場合には、作業者に所定の反力を伝えるアシスト搬送装置を開示している（特許文献１）。

特開２００５−１４１３３号公報

しかしながら、前記従来技術では、ロボットアームの手先の作業範囲を規制する固定のリミッタを設ける方法であるため、ロボットアームの各関節の動作範囲の規制など、複数の関節の関係によって手先の可動範囲が複雑に変化する場合には、適用が困難である。

また、前記従来技術は、リミッタを越える動作に対しては、バネ定数などの制御パラメータを変化させて反力を発生させることで、動作の制限を行うが、制御パラメータを可変とするために、制御系の構成が複雑になる可能性がある。また、制御パラメータを変化させることで制御系が不安定になり、ロボットの動作が不安定になる可能性がある。

本発明の目的は、前記従来の制御装置の課題を解決し、ロボットアームの関節の動作範囲の規制など、関節の動作状況に応じたロボットアームの動作制限を、簡潔な制御系構造で実現し、制御系の不安定さを低減するロボット、ロボットの制御装置、及び制御方法を提供する。

前記目的を達成するために、本発明は以下のように構成する。

本発明の第１態様によれば、多関節ロボットアームと、
前記多関節ロボットアームに配設され、かつ、外力を検出する外力検出手段と、
前記多関節ロボットアームの関節が可動範囲のいずれかに位置して可動状態を計算する関節可動状態計算手段と、
前記関節可動状態計算手段により計算された前記関節の前記可動状態に基づき、前記外力検出手段が検出した前記外力を、前記外力が前記ロボットアームに加えられたときに前記ロボットアームが動作可能な方向成分を抽出可能な変換外力に変換する外力変換手段と、
前記外力変換手段により変換された前記変換外力に基づいてインピーダンス制御を行うことにより、前記多関節ロボットアームの前記関節の前記可動状態に応じた動作制限が行われるように前記多関節ロボットアームを制御する制御手段とを有するロボットを提供する。

本発明の第９態様によれば、多関節ロボットアームを備えるロボットの制御装置であって、
前記多関節ロボットアームの関節が可動範囲のいずれかに位置して可動状態を計算する関節可動状態計算手段と、
前記関節可動状態計算手段により計算された前記関節の前記可動状態に基づき、前記多関節ロボットアームに配設されて外力を検出する外力検出手段が検出した前記外力を、前記外力が前記ロボットアームに加えられたときに前記ロボットアームが動作可能な方向成分を抽出可能な変換外力に変換する外力変換手段と、
前記外力変換手段により変換された前記変換外力に基づいてインピーダンス制御を行うことにより、前記多関節ロボットアームの前記関節の前記可動状態に応じた動作制限が行われるように前記多関節ロボットアームを制御する制御手段とを有することを特徴とするロボットの制御装置を提供する。

本発明の第１０態様によれば、多関節ロボットアームを備えるロボットの制御方法であって、
前記多関節ロボットアームの関節が可動範囲のいずれかに位置して可動状態を関節可動状態計算手段で計算し、
前記計算された前記関節の前記可動状態に基づき、前記多関節ロボットアームに配設されて外力を検出する外力検出手段が検出した前記外力を、前記外力が前記ロボットアームに加えられたときに前記ロボットアームが動作可能な方向成分を抽出可能な変換外力に外力変換手段で変換し、
前記変換された変換外力に基づいてインピーダンス制御を行うことにより、前記多関節ロボットアームの前記関節の前記可動状態に応じた動作制限が行われるように前記多関節ロボットアームを制御手段で制御することを特徴とするロボットの制御方法を提供する。

本発明の第１１態様によれば、多関節ロボットアームを備えるロボットの制御プログラムであって、
コンピュータを、
前記多関節ロボットアームの関節が可動範囲のいずれかに位置して可動状態を計算する関節可動状態計算手段と、
前記関節可動状態計算手段により計算された前記関節の前記可動状態に基づき、前記多関節ロボットアームに配設されて外力を検出する外力検出手段が検出した前記外力を、前記外力が前記ロボットアームに加えられたときに前記ロボットアームが動作可能な方向成分を抽出可能な変換外力に変換する外力変換手段と、
前記外力変換手段により変換された前記変換外力に基づいてインピーダンス制御を行うことにより、前記多関節ロボットアームの前記関節の前記可動状態に応じた動作制限が行われるように前記多関節ロボットアームを制御する制御手段として機能させるためのロボットの制御プログラムを提供する。

本発明のロボット、ロボットの制御装置、及び制御方法によれば、関節可動状態計算手段及び力変換手段を有することにより、インピーダンス制御による動作時に人が関節の可動領域を越えるような操作を行っても、関節可動状態計算手段において関節が可動範囲のいずれかに位置して可動状態であるかどうか可動状態を判定し、外力変換手段により変換された変換外力をインピーダンス計算手段への入力として使うことで、ロボットアームの動作制限が可能となり、関節の可動領域を越えることを防ぐことができる。

本発明のこれらと他の目的と特徴は、添付された図面についての好ましい実施形態に関連した次の記述から明らかになる。この図面においては、

図１は、本発明の第１実施形態におけるロボットの全体構成を示す図であり、 図２は、本発明の前記第１実施形態における前記ロボットのインピーダンス制御手段の構成を示すブロック図であり、 図３は、本発明の前記第１実施形態における前記ロボットによる人との協調搬送作業を説明する図であり、 図４は、関節角度ｑ_ｉと可動状態の関係を示す図であり、 図５は、本発明の前記第１実施形態における前記ロボットにおけるインピーダンス制御手段の全体的な動作ステップを表すフローチャートであり、 図６は、本発明の前記第１実施形態における前記ロボットにおける拘束曲面及び力の変換を説明する図であり、 図７は、本発明の第２実施形態におけるロボットのインピーダンス制御手段の構成を示すブロック図であり、 図８は、本発明の前記第２実施形態における前記ロボットの全体構成を示す図であり、 図９は、本発明の第３実施形態におけるロボットのインピーダンス制御手段の構成を示すブロック図であり、 図１０は、本発明の前記第３実施形態における前記ロボットによる人との協調搬送作業を説明する図であり、 図１１は、本発明の前記第３実施形態における前記ロボットの動作の具体例を示す図であり、 図１２は、式（１０３）に現れる並進外力ｆｈ、ｆの関係を示す図である。

以下に、本発明にかかる実施の形態を図面に基づいて詳細に説明する。

以下、図面を参照して本発明における実施形態を詳細に説明する前に、本発明の種々の態様について説明する。

本発明の第１態様によれば、多関節ロボットアームと、
前記多関節ロボットアームに配設され、かつ、外力を検出する外力検出手段と、
前記多関節ロボットアームの関節が可動範囲のいずれかに位置して可動状態を計算する関節可動状態計算手段と、
前記関節可動状態計算手段により計算された前記関節の前記可動状態に基づき、前記外力検出手段が検出した前記外力を、前記外力が前記ロボットアームに加えられたときに前記ロボットアームが動作可能な方向成分を抽出可能な変換外力に変換する外力変換手段と、
前記外力変換手段により変換された前記変換外力に基づいてインピーダンス制御を行うことにより、前記多関節ロボットアームの前記関節の前記可動状態に応じた動作制限が行われるように前記多関節ロボットアームを制御する制御手段とを有するロボットを提供する。

本発明の第２態様によれば、前記外力変換手段は、前記外力検出手段に検出された前記外力を、前記多関節ロボットアームの前記関節が固定されたときに前記多関節ロボットアームの手先が動くことのできる拘束曲面の接平面への正射影として変換する第１の態様に記載のロボットを提供する。

本発明の第３態様によれば、さらに、前記多関節ロボットアームにかかる前記外力により各関節に発生する外力トルクを計算する外力トルク計算手段を有し、
前記関節可動状態計算手段は、前記外力トルク計算手段の計算結果に基づき前記外力トルクが前記多関節ロボットアームの各関節の動作を可動領域外より可動領域内に戻そうとする力であるかどうかを判定し、
前記関節可動状態計算手段が、前記外力トルクが前記多関節ロボットアームの各関節の動作を前記可動領域外より前記可動領域内に戻そうとする力であると判定する場合には、前記制御手段は前記動作制限を行わない一方、
前記関節可動状態計算手段が、前記外力トルクが前記多関節ロボットアームの各関節の動作を前記可動領域外より前記可動領域内に戻そうとする力ではないと判定する場合には、前記制御手段は前記動作制限を行う第１又は２の態様に記載のロボットを提供する。

本発明の第４態様によれば、前記関節可動状態計算手段は、前記外力を関節トルクに変換することにより、前記関節トルクに変換された前記変換外力が、前記多関節ロボットアームの各関節の動作を可動領域外より可動領域内に戻そうとする力であるかどうかを判定し、
前記関節可動状態計算手段が、前記変換外力が、前記多関節ロボットアームの各関節の動作を前記可動領域外より前記可動領域内に戻そうとする力であると判定する場合には、前記制御手段は前記動作制限を行わない一方、
前記関節可動状態計算手段が、前記変換外力が、前記多関節ロボットアームの各関節の動作を前記可動領域外より前記可動領域内に戻そうとする力ではないと判定する場合には、前記制御手段は前記動作制限を行う第１又は２の態様に記載のロボットを提供する。

本発明の第５態様によれば、前記多関節ロボットアームの各関節に対し、可動あるいは固定の動作モードを設定する動作モード設定部と、
前記動作モード設定部で設定された前記動作モードの指令が入力されて、前記動作モードと前記関節可動状態計算手段により計算された前記関節の前記可動状態とに基づき前記可動状態の計算を行う関節動作モード設定手段とをさらに備えて、
前記外力変換手段は、前記関節動作モード設定手段により計算された前記関節の前記可動状態に基づき、前記外力検出手段が検出した前記外力を、前記外力が前記ロボットアームに加えられたときに前記ロボットアームが動作可能な方向成分を抽出可能な変換外力に変換し、
前記制御手段は、前記変換外力に基づいてインピーダンス制御を行うことにより、前記多関節ロボットアームの前記関節の前記可動状態に応じた動作制限が行われる第１〜４のいずれか１つの態様に記載のロボットを提供する。

本発明の第６態様によれば、前記関節可動状態計算手段は、前記関節の可動領域の両端の境界線から一定範囲だけ内側に緩衝領域をそれぞれ設け、前記各緩衝領域では前記関節の前記可動状態を徐々に変化させる第３又は４の態様に記載のロボットを提供する。

本発明の第７態様によれば、前記多関節ロボットアームの前記関節の可動領域の両端の内側にそれぞれ設けられた緩衝領域と、前記緩衝領域を除く前記可動領域の残りの領域である完全可動領域ＰＭＲと、可動領域外の領域とに分割し、
前記関節可動状態計算手段は、各領域毎に、前記関節の前記可動状態の値を異ならせるように計算される第１の態様に記載のロボットを提供する。

本発明の第８態様によれば、前記関節可動状態計算手段と前記力変換手段とが第２制御手段として、前記多関節アームの前記制御手段とは別に備えられ、
前記力変換手段が変換した外力が前記第２制御手段より通信を通じて前記制御手段に入力される第１の態様に記載のロボットを提供する。

本発明の第９態様によれば、多関節ロボットアームを備えるロボットの制御装置であって、
前記多関節ロボットアームの関節が可動範囲のいずれかに位置して可動状態を計算する関節可動状態計算手段と、
前記関節可動状態計算手段により計算された前記関節の前記可動状態に基づき、前記多関節ロボットアームに配設されて外力を検出する外力検出手段が検出した前記外力を、前記外力が前記ロボットアームに加えられたときに前記ロボットアームが動作可能な方向成分を抽出可能な変換外力に変換する外力変換手段と、
前記外力変換手段により変換された前記変換外力に基づいてインピーダンス制御を行うことにより、前記多関節ロボットアームの前記関節の前記可動状態に応じた動作制限が行われるように前記多関節ロボットアームを制御する制御手段とを有することを特徴とするロボットの制御装置を提供する。

本発明の第１０態様によれば、多関節ロボットアームを備えるロボットの制御方法であって、
前記多関節ロボットアームの関節が可動範囲のいずれかに位置して可動状態を関節可動状態計算手段で計算し、
前記計算された前記関節の前記可動状態に基づき、前記多関節ロボットアームに配設されて外力を検出する外力検出手段が検出した前記外力を、前記外力が前記ロボットアームに加えられたときに前記ロボットアームが動作可能な方向成分を抽出可能な変換外力に外力変換手段で変換し、
前記変換された変換外力に基づいてインピーダンス制御を行うことにより、前記多関節ロボットアームの前記関節の前記可動状態に応じた動作制限が行われるように前記多関節ロボットアームを制御手段で制御することを特徴とするロボットの制御方法を提供する。

本発明の第１１態様によれば、多関節ロボットアームを備えるロボットの制御プログラムであって、
コンピュータを、
前記多関節ロボットアームの関節が可動範囲のいずれかに位置して可動状態を計算する関節可動状態計算手段と、
前記関節可動状態計算手段により計算された前記関節の前記可動状態に基づき、前記多関節ロボットアームに配設されて外力を検出する外力検出手段が検出した前記外力を、前記外力が前記ロボットアームに加えられたときに前記ロボットアームが動作可能な方向成分を抽出可能な変換外力に変換する外力変換手段と、
前記外力変換手段により変換された前記変換外力に基づいてインピーダンス制御を行うことにより、前記多関節ロボットアームの前記関節の前記可動状態に応じた動作制限が行われるように前記多関節ロボットアームを制御する制御手段として機能させるためのロボットの制御プログラムを提供する。

以下に、本発明にかかる実施の形態を図面に基づいて詳細に説明する。

（第１実施形態）
図１は本発明の第１実施形態のロボット９０の構成を示す図である。ロボット９０は、多関節ロボットアーム５と、多関節ロボットアーム５の動作を制御する制御装置１とを備えて構成されている。

制御装置１は、ハードウェア的には一般的なパーソナルコンピュータにより構成されている。そして、インピーダンス制御手段（インピーダンス制御部）４の入出力ＩＦ１９を除く部分は、パーソナルコンピュータで実行される制御プログラム１７としてソフトウェア的に実現される。

入出力ＩＦ１９は、パーソナルコンピュータのＰＣＩバスなどの拡張スロットルに接続された、Ｄ／Ａボード２０と、Ａ／Ｄボード２１と、カウンタボード２２とにより構成される。

ロボット９０の多関節ロボットアーム５の動作を制御するための制御プログラム１７が実行されることにより、制御装置１が機能する。ロボットアーム５の各関節１１，１２，１３，１４，１５，１６のそれぞれのエンコーダ４２より出力される関節角度情報がカウンタボード２２を通じて制御装置１に取り込まれ、制御装置１によって各関節１１，１２，１３，１４，１５，１６の回転動作での制御指令値が算出される。算出された各制御指令値は、Ｄ／Ａボード２０を通じてモータードライバ１８に与えられ、モータードライバ１８から送られた各制御指令値に従って、ロボットアーム５の各関節１１，１２，１３，１４，１５，１６のモータ４１が駆動される。

ロボットアーム５は、６自由度の多リンクマニピュレータであり、手先の一例として機能するハンド６と、ハンド６が取付けられる手首部７を有する前腕リンク８と、前腕リンク８が回転可能に連結される上腕リンク９と、上腕リンク９が回転可能に連結支持される台部１０とを有している。手首部７は、第４関節１４と、第５関節１５と、第６関節１６との３つの回転軸を有しており、上腕リンク９に対するハンド６の相対的な姿勢（向き）を変化させることができる。前腕リンク８の他端は、上腕リンク９の一端に対して第３関節１３周りに回転可能とし、上腕リンク９の他端は、台部１０に対して第２関節１２周りに回転可能とする。そして台部の上側可動部１０ａは台部の下側固定部１０ｂに対して第１関節１１周りに回転可能として、合計６個の軸周りに回転可能として前記６自由度の多リンクマニピュレータを構成している。

各軸の回転部分を構成する各関節１１，１２，１３，１４，１５，１６は、各関節１１，１２，１３，１４，１５，１６の一方の部材に設けられた回転駆動装置（本第１実施形態ではモータ４１）と、モータ４１の回転軸の回転位相角（すなわち関節角）を検出するエンコーダ４２とを備える。そして、モータ４１の回転軸が関節の他方の部材に連結されて前記回転軸を正逆回転させることにより、他方の部材を一方の部材に対して各軸周りに回転可能とする。回転駆動装置は、後述するモータードライバ１８により駆動制御される。本第１実施形態では、回転駆動装置の一例としてのモータ４１とエンコーダ４２とは、ロボットアーム５の各関節１１，１２，１３，１４，１５，１６の内部に配設されている。

３５は台部１０の下側固定部１０ｂに対して相対的な位置関係が固定された絶対座標系であり、３６はハンド６に対して相対的な位置関係が固定された手先座標系である。絶対座標系３５から見た手先座標系３６の原点位置Ｏ_ｅ（ｘ、ｙ、ｚ）をロボットアーム５の手先位置とし、絶対座標系３５から見た手先座標系３６の姿勢をロール・ピッチ・ヨー角で表現した（φ、θ、ψ）をロボットアーム５の手先姿勢とし、手先位置・姿勢ベクトルをｒ＝［ｘ、ｙ、ｚ、φ、θ、］^Ｔと定義する。ロボットアーム５の手先位置・姿勢を制御する場合には、手先位置・姿勢ベクトルｒを目標手先位置・姿勢ベクトルｒ_ｄに追従させることになる。

ロボットアーム５の手首部７とハンド６の間には、外力検出手段（外力検出部又は外力検出装置）の一例として機能する力センサー３が配設されており、ハンド６にかかる外力を検出できる。

次に、図２を使い、インピーダンス制御手段４の詳細について説明する。図２において、５は図１に示したロボットアームである。ロボットアーム５からはそれぞれの関節軸のエンコーダ４２により計測された関節角の現在値（関節角度ベクトル）ｑ＝［ｑ_１，ｑ_２，ｑ_３，ｑ_４，ｑ_５，ｑ_６］^Ｔが出力され、カウンタボード２２によりインピーダンス制御手段４に取り込まれる。ただし、ｑ_１，ｑ_２，ｑ_３，ｑ_４，ｑ_５，ｑ_６は、それぞれ、第１関節１１、第２関節１２、第３関節１３、第４関節１４、第５関節１５、第６関節１６の関節角度である。

また、ロボットアーム５からは力センサー３により計測された外力Ｆ_Ｓ＝［ｆ_ｘ，ｆ_ｙ，ｆ_ｚ，ｎ_ｘ，ｎ_ｙ，ｎ_ｚ］^Ｔが出力され、Ａ／Ｄボード２１によりインピーダンス制御手段４に取り込まれる。

目標軌道生成手段（目標軌道生成部）２３は、目標とするロボットアーム５の動作を実現するための手先位置・姿勢目標ベクトルｒ_ｄを出力する。目標とするロボットアーム５の動作は、目的とする作業に応じて、事前に、ポイントごとの位置（ｒ_ｄ０、ｒ_ｄ１、ｒ_ｄ２、・・・）が時系列データとして与えられている。目標軌道生成手段２３は、多項式補間を使用し、各ポイント間の軌道を補完し、手先位置・姿勢目標ベクトルｒ_ｄを生成する。

インピーダンス計算手段（インピーダンス計算部）２５は、ロボットアーム５に機械インピーダンスを実現する機能を果たす部分であり、ロボットアーム５が目標軌道生成手段２３の生成する目標軌道に沿うよう位置制御により単独で動作する場合は０を出力する。一方、インピーダンス計算手段２５は、ロボットアーム５と人間が協働作業をする場合には、（ｉ）設定したインピーダンスパラメータＭ、Ｄ、Ｋ（慣性Ｍ、粘性Ｄ、剛性Ｋ）と、（ｉｉｉ）インピーダンス制御手段４（インピーダンス制御手段４の重力補償手段３０）へ入力される外力Ｆを変換した変換外力Ｆ_ｈとにより、ロボットアーム５に機械インピーダンスを実現するための手先位置・姿勢目標補正出力ｒ_ｄΔを以下の式（１）により計算し、出力する。手先位置・姿勢目標補正出力ｒ_ｄΔは、目標軌道生成手段２３の出力する手先位置・姿勢目標ｒ_ｄに第１演算部６１で加算され、手先位置・姿勢補正目標ベクトルｒ_ｄｍが生成される。

．．．．．（１）
ただし、

．．．．．（２）

．．．．．（３）

．．．．．（４）
であり、ｓはラプラス演算子である。

順運動学計算手段２６は、ロボットアーム５から出力された各関節角の現在値ｑである関節角度ベクトルｑが入力され、ロボットアーム５の関節角度ベクトルｑから手先位置・姿勢ベクトルｒへの変換の幾何学的計算を行う。

順運動学計算手段２６により計算される手先位置・姿勢ベクトルｒと、手先位置・姿勢補正目標ベクトルｒ_ｄｍとの誤差ｒ_ｅが第２演算部６２で求められる。

第２演算部６２で求められた誤差ｒ_ｅが位置誤差補償手段２７に入力され、位置誤差補償出力ｕ_ｒｐが位置誤差補償手段２７で求められて、近似逆運動学計算手段２８に出力される。

近似逆運動学計算手段２８は、近似式

を用いて、逆運動学の近似計算を行う。ただし、Ｊ_ｒ（ｑ）は

の関係を満たすヤコビ行列、ｕ_ｉｎは近似逆運動学計算手段２８への入力、ｕ_ｏｕｔは近似逆運動学計算手段２８からの出力である。ここで、

と近似して考えることができるので、ヤコビ行列の定義式

に代入すると、

が成り立つことが分かる。すなわち、Ｊ_ｒ（ｑ）^−１を近似逆運動学計算手段２８で乗算することで手先位置・姿勢に関する誤差値から関節角度に関する誤差値への変換となる。したがって、手先位置・姿勢誤差ｒ_ｅにゲインをかける等した値である位置誤差補償出力ｕ_ｒｅが近似逆運動学計算手段２８に入力されると、その出力として、関節角度誤差ｑ_ｅを補償するための関節角度誤差補償出力ｕ_ｑｅが近似逆運動学計算手段２８から出力されることになる。

関節角度誤差補償出力ｕ_ｑｅは、近似逆運動学計算手段２８からＤ／Ａボード２０を介してモータードライバ１８に電圧指令値として与えられ、各関節軸が正逆回転駆動されてロボットアーム５が動作する。

以上のように構成されるインピーダンス制御手段４の基本構成に関して動作の基本原理を説明する。

動作の基本は、位置誤差補償手段２７による手先位置・姿勢誤差ｒ_ｅのフィードバック制御（位置制御）であり、図２の点線で囲まれた部分が位置制御系２９になっている。位置誤差補償手段２７として、例えば、ＰＩＤ補償器を使用すれば、手先位置・姿勢誤差ｒ_ｅが０に収束するように制御が働き、目標とするロボットアーム５の動作が実現する。

インピーダンス制御を行う場合、前記説明した位置制御系２９に対し、インピーダンス計算手段２５により手先位置・姿勢目標補正出力ｒ_ｄΔが第１演算部６１で加算されて手先位置・姿勢の目標値の補正が行われる。このために、前記した位置制御系２９は、手先位置・姿勢の目標値が本来の値より微妙にずれることになり、結果的に機械インピーダンスが実現される。以上の図２の一点鎖線で囲まれた部分は、位置制御ベースのインピーダンス制御系４９と呼ばれる構成であり、このインピーダンス制御系４９により慣性Ｍ、粘性Ｄ、剛性Ｋの機械インピーダンスが実現される。インピーダンス制御手段４は制御手段（制御部）（又は動作制御手段、又は動作制御部）の一例として機能する。

以上のインピーダンス制御を利用すれば、図３に示すような人１００とロボット９０の物体３８の協調搬送のような協働作業が可能となる。人１００が物品３８を移動しようとして、物体３８に力をかけると、物体３８を通じて力がロボット９０のロボットアーム５に伝わり、前記力が、ロボットアーム５の力センサー３により外力Ｆ_ｓとして検出される。インピーダンス制御手段４への入力となる外力Ｆは、協調搬送する物体３８の重力を補償するため、重力補償手段３０において、力センサー３により検出された外力Ｆ_ｓを基に式（５）で計算を行い、算出されるＦを使用する。

．．．．．（５）
ただし、ｍは物体３８の質量、ｇは物体３８の重力加速度である。式（５）で計算される外力Ｆを入力にインピーダンス制御を行えば、ロボットアーム５は人１００がかけた力の方向に沿うように動作するため、協調搬送が実現する。なお、物体３８の質量ｍ、及び、物体３８の重力加速度ｇは、重力補償手段３０に予め記憶されている。

本発明の第１実施形態のロボットの制御装置１の特徴は、以上の基本的なインピーダンス制御手段４の構成に加え、関節可動状態計算手段２及び力変換手段２４を有するところにあり、以下、関節可動状態計算手段２及び力変換手段２４の詳細について説明する。

関節可動状態計算手段２は、ロボットアーム５の関節角の現在値（関節角度ベクトル）ｑが入力されることで各関節１１，１２，１３，１４，１５，１６の可動状態を判定し（具体的には、第１〜第６関節１１，１２，１３，１４，１５，１６のそれぞれの関節又は少なくとも１つの関節が可動範囲のいずれかに位置して可動状態を計算し）、その判定結果（計算結果）を力変換手段（力変換部、又は外力変換手段、又は外力変換部）２４へと出力する。

図４は関節角度ｑ_ｉ（以下、ｉ＝１，２，・・・，６）と可動状態の関係を示す図であり、紙面左回りが関節ｉの正方向の回転を表している。関節可動状態計算手段２は、各関節１１，１２，１３，１４，１５，１６の可動範囲の最小値ｑ_ｉＭＩＮ及び最大値ｑ_ｉＭＡＸ、及び緩衝領域幅ｄｑ_ｉにより、可動領域ＭＲと、可動領域ＭＲの両端の境界線から一定範囲だけ内側に配置された第１緩衝領域ＢＲ１と第２緩衝領域ＢＲ２とを定義する。第１緩衝領域ＢＲ１と第２緩衝領域ＢＲ２は、それぞれ、前記関節の前記可動状態を徐々に変化させる領域である。そして、関節可動状態計算手段２は、現在の関節角ｑ_ｉが属する領域に応じてパターン分けし、各関節１１，１２，１３，１４，１５，１６の可動状態Ｌ_ｉを０〜１の間の値として計算する。

さらに、インピーダンス制御手段４には、多関節ロボットアーム５にかかる外力Ｆにより各関節に発生する外力トルクτを計算する外力トルク計算手段５０を備えている。具体的には、外力トルク計算手段５０において、式（６）により外力Ｆによる各関節１１，１２，１３，１４，１５，１６に発生するトルクτ＝［τ_１，τ_２，τ_３，τ_４，τ_５，τ_６］^Ｔを計算する。そして、関節可動状態計算手段２において、各関節１１，１２，１３，１４，１５，１６が緩衝領域ＢＲ１，ＢＲ２内あるいは可動領域外にあるときに、前記外力トルク計算手段５０で計算されて求められたトルクτが、各関節を可動領域ＭＲ内へ戻そうとする方向のトルクであると関節可動状態計算手段２で判定される場合（図４において矢印Ｘ及びＹの方向、後述のパターンＢ２，Ｃ２，Ｄ２，Ｅ２の場合）、予め定めた一定のオフセット値ｄＬｉを可動状態Ｌ_ｉに関節可動状態計算手段２で加算する。また、関節可動状態計算手段２において、各関節が緩衝領域ＢＲ１，ＢＲ２内あるいは可動領域外にあるときに、前記計算されて求められたトルクが各関節を可動領域ＭＲ内へ戻そうとする方向のトルクではないと関節可動状態計算手段２で判定される場合には、オフセット値ｄＬ_ｉは可動状態Ｌ_ｉに関節可動状態計算手段２で加算しない。

．．．．．（６）
前記式（６）において、Ｊ_ｖは

の関係を満たすヤコビ行列であり、

で、

はロボットアーム５の手先の回転速度である。

オフセット値ｄＬ_ｉの具体的な数値は、操作のしやすさ、又は、制御の安定性を考慮して、実際にロボットアーム５を動作させて予め実験的に定めて、関節可動状態計算手段２内に記憶させることになる。

オフセット値ｄＬ_ｉは、後述するように、関節ｉを可動領域ＭＲ内へ戻そうとするトルクに対する関節ｉの反応を関節可動状態計算手段２で決定し、かつ、可動領域ＭＲの境界で発生するリミットサイクルのような現象の発生度合いを決める値となる。具体的には、オフセット値ｄＬ_ｉの値が大きくなるほど可動領域ＭＲへ戻そうとする力に対する反応が良くなり、操作が容易になる一方で、オフセット値ｄＬ_ｉの値が大きくなるほどリミットサイクルのような現象により不安定性が増大することになる。したがって、オフセット値ｄＬ_ｉの値の具体的な数値は、前記相反する反応の良さからくる操作性と制御の安定性とを考慮し、実際にロボットアーム５を動作させて予め実験的に定めて関節可動状態計算手段２内に記憶させることになる。

ここで言う「操作のしやすさ」とは、オフセット値ｄＬｉの値が小さいほど、大きな力をかけないと戻らなくなるので、操作性が悪いのに対し、オフセット値ｄＬｉの値が大きいほど、小さな力で容易に戻ることになるので、操作性が良いと呼んでいる。一方、オフセット値ｄＬｉの値が大きいほど、リミットサイクルのような現象が起こり、具体的には振動が発生し、制御が不安定になります。すなわち、ここで言う「制御の安定性」とは、制御が原因で振動が発生し運動が安定しないことを意味する。

以上を具体的に記述すると、以下のようにパターン分けして、関節ｉの可動状態Ｌ_ｉが関節可動状態計算手段２で計算される。

パターンＡ： （ｑ_ｉＭＩＮ＋ｄｑ_ｉ）≦ｑ_ｉ≦（ｑ_ｉＭＡＸ−ｄｑ_ｉ）：すなわち、（完全可動領域ＰＭＲ内）のとき：Ｌ_ｉ＝１
パターンＢ： （ｑ_ｉＭＩＮ）＜ｑ_ｉ＜（ｑ_ｉＭＩＮ＋ｄｑ_ｉ）：すなわち、（第１緩衝領域ＢＲ１内）のとき：
パターンＢ１： τ_ｉ≦０の場合： Ｌ_ｉ＝（ｑ_ｉ−ｑ_ｉＭＩＮ）／ｄｑ_ｉ
パターンＢ２： τ_ｉ＞０の場合： Ｌ_ｉ＝（ｑ_ｉ−ｑ_ｉＭＩＮ）／ｄｑ_ｉ＋ｄＬ_ｉ
（ただし、 Ｌ_ｉ≦１に制限）
パターンＣ： （ｑ_ｉＭＡＸ−ｄｑ_ｉ）＜ｑ_ｉ＜（ｑ_ｉＭＡＸ）：すなわち、（第２緩衝領域ＢＲ２内）のとき：
パターンＣ１： τ_ｉ≧０の場合： Ｌ_ｉ＝（ｑ_ｉＭＡＸ−ｑ_ｉ）／ｄｑ_ｉ
パターンＣ２： τ_ｉ＜０の場合： Ｌ_ｉ＝（ｑ_ｉＭＡＸ−ｑ_ｉ）／ｄｑ_ｉ＋ｄＬ_ｉ
（ただし、 Ｌ_ｉ≦１に制限）
パターンＤ： ｑ_ｉ≦ｑ_ｉＭＩＮ：すなわち、（可動領域外）のとき：
パターンＤ１： τ_ｉ≦０の場合： Ｌ_ｉ＝０
パターンＤ２： τ_ｉ＞０の場合： Ｌ_ｉ＝ｄＬ_ｉ
パターンＥ： ｑ_ｉＭＡＸ≦ｑ_ｉ：すなわち、（可動領域外）のとき：
パターンＥ１： τ_ｉ≧０の場合： Ｌ_ｉ＝０
パターンＥ２： τ_ｉ＜０の場合： Ｌ_ｉ＝ｄＬ_ｉ
力変換手段２４は、関節可動状態計算手段２の出力である可動状態Ｌ_ｉに基づき、外力Ｆを、前記外力Ｆがロボットアーム５に加えられたときにロボットアーム５が動作可能な方向成分を抽出可能な変換外力Ｆ_ｈに変換する。

変換外力Ｆ_ｈの変換方法に関しては、２つの方法を以下に示す。

第１の変換方法は、以下の式（７）〜（１３）式に基づき変換する方法である。

．．．．．（７）

．．．．．（８）

．．．．．（９）

．．．．．（１０）

．．．．．（１１）

．．．．．（１２）
ただし、ｊ_１，ｊ_２，ｊ_３は式（１３）を満たす３次元ベクトルであり、３×３行列［ｊ_１ ｊ_２ ｊ_３］は

の関係を満たすヤコビ行列Ｊ_ｒ（ｑ）の左上の３×３の部分行列に相当する。

．．．．．（１３）
第２の変換方法は、関節の可動状態のパターン分類である、パターンＡ〜Ｅに応じてパターン分けする方法である。

パターンα： 関節角度ｑ_１、ｑ_２、ｑ_３のすべてがパターンＡの場合、

．．．．．（１０１）
となる。

パターンβ： 関節角度ｑ_１、ｑ_２、ｑ_３のいずれか１つの関節角度ｑがパターンＢ〜Ｅのいずれかの場合（すなわち、関節角度ｑ_１、ｑ_２、ｑ_３のその他の２つの関節角度ｑがパターンＡの場合）、
例えば、関節角度ｑ_ｉがパターンＢ〜Ｅのいずれかであり、かつ、関節角度ｑ_ａ、ｑ_ｂがパターンＡとすると（ただし、この例では、ｑ_ｉは、ｑ_１、ｑ_２、ｑ_３のいずれか１つの関節角度を意味し、ｑ_ａ、ｑ_ｂは、関節角度ｑ_１、ｑ_２、ｑ_３のその他の２つの関節角度を意味する。）、

．．．．．（１０２）
となる。

パターンγ： 関節角度ｑ_１、ｑ_２、ｑ_３のいずれか２つの関節角度ｑがパターンＢ〜Ｅのいずれかの場合（すなわち、関節角度ｑ_１、ｑ_２、ｑ_３の残りの１つの関節角度ｑがパターンＡの場合）、
例えば、関節角度ｑ_ｉがパターンＡであり、かつ、関節角度ｑ_ａ、ｑ_ｂがパターンＢ〜Ｅのいずれかであるとすると（ただし、この例では、ｑ_ｉは、関節角度ｑ_１、ｑ_２、ｑ_３のいずれか１つの関節角度を意味し、ｑ_ａ、ｑ_ｂは、関節角度ｑ_１、ｑ_２、ｑ_３のその他の２つの関節角度を意味する。）、

．．．．．（１０３）
となる。

パターンδ： 関節角度ｑ_１、ｑ_２、ｑ_３のすべてがパターンＢ〜Ｅのいずれかの場合、

．．．．．（１０４）
となる。

その他の式（７）、式（１１）、式（１２）は第１の変換方法と同様である。

上記第１の変換方法は、変換式のパターン分けをする必要が無く、簡潔なアルゴリズムとすることができるという利点を有するが、図１に示すロボットアーム５の第２関節１２，第３関節１３のように平行な関節軸を持つアーム構造の場合に、平行な関節軸の関節が同時に完全可動領域外になる場合に限り対応することはできない。

一方、第２の変換方法は、変換式のパターン分けをする必要があるが、アームの構造に関係なく、複数の関節が完全可動領域外になる場合も含め、あらゆる場合に対応可能である。

したがって、アームの構造又は各関節の可動範囲の大きさ等に応じて、上記第１の変換方法と第２の変換方法を選択するように、力変換手段２４において、予め設定しておけばよい。

図５は、上述した原理に基づく制御プログラムによるインピーダンス制御時の動作ステップを説明するフローチャートである。

ステップＳ１では、エンコーダ４２により計測された関節角度データ（関節角度ベクトルｑ）が制御装置１に取り込まれる。

ステップＳ２では、ロボットアーム５の運動学計算に必要なヤコビ行列Ｊ_ｒ等の計算が近似逆運動学計算手段２８により行われる。

ステップＳ３では、ロボットアーム５からの関節角度データ（関節角度ベクトルｑ）から、ロボットアーム５の現在の手先位置・姿勢ベクトルｒが順運動学計算手段２６により計算される（順運動学計算手段２６での処理）。

ステップＳ４では、関節可動状態計算手段２において、ロボットアーム５からの関節角度データ（関節角度ベクトルｑ）に基づいて、パターンＡ〜パターンＥのいずれになるかを判定し、関節可動状態Ｌ_ｉを算出する。

ステップＳ５では、力センサー３の計測値Ｆ_ｓが重力補償手段３０に取り込まれ、式（５）に基づき外力Ｆが重力補償手段３０で計算される。

ステップＳ６では、力変換手段２４において、式（７）〜式（１３）（第１の変換方法）、又は、式（７）、式（１１）、式（１２）、式（１０１）〜式（１０４）（第２の変換方法）に基づいて、重力補償手段３０で計算された外力Ｆが変換外力Ｆ_ｈに変換される。

ステップＳ７では、機械インピーダンスパラメータＭ、Ｄ、Ｋと、関節角度データ（関節角度ベクトルｑ）と、力変換手段２４において変換された変換外力Ｆ_ｈとから、手先位置・姿勢目標補正出力ｒ_ｄΔが、インピーダンス計算手段２５により計算される（インピーダンス計算手段２５での処理）。

ステップＳ８では、手先位置・姿勢目標ベクトルｒ_ｄと手先位置・姿勢目標補正出力ｒ_ｄΔの和が第１演算部６１で計算されて求められた手先位置・姿勢補正目標ベクトルｒ_ｄｍと、現在の手先位置・姿勢ベクトルｒとの差である手先位置・姿勢の誤差ｒ_ｅが、第２演算部６２で計算される（位置誤差補償手段２７での処理）。位置誤差補償手段２７の具体例としてはＰＩＤ補償器が考えられる。定数の対角行列である比例、微分、積分の３つのゲインを適切に調整することにより、位置誤差が０に収束するように制御が働く。

ステップＳ９では、ステップＳ２で計算したヤコビ行列Ｊ_ｒの逆行列を乗算することにより、位置誤差補償出力ｕ_ｒｅを手先位置・姿勢の誤差に関する値から、関節角度の誤差に関する値である関節角度誤差補償出力ｕ_ｑｅに、近似逆運動学計算手段２８により変換する（近似逆運動学計算手段２８での処理）。

ステップＳ１０では、関節角度誤差補償出力ｕ_ｑｅが、近似逆運動学計算手段２８からＤ／Ａボード２０を通じてモータードライバ１８に与えられ、各モータ４１を流れる電流量を変化させることによりロボットアーム５の各関節の関節軸の回転運動が発生する。

以上のステップＳ１〜ステップＳ１０が、制御の計算ループとして繰り返し実行されることにより、ロボットアーム５の動作の制御が実現する。なお、ステップＳ２〜ステップＳ６の動作の順番はパラレル処理でも可能であり、必ずしも、この順番通りでなくてもよい。

本発明の前記第１実施形態にかかるロボット９０では、関節可動状態計算手段２及び力変換手段２４を備えることにより、以下に詳述するような動作及び効果が実現する。以下では、人１００とロボット９０が協調搬送を行う場合の動作を第３関節１３を中心に説明を行う。

図６は、式（８）に現れる並進外力ｆ_ｈ、ｆ_ｈ２の関係を示す。図６においてはロボットアーム５の第１関節１１から第３関節１３までを模式的に表している。拘束曲面３１は、第３関節１３が可動領域ＭＲを越えて、それ以上動けずロックされた状態の場合に、その他の第１関節１１及び第２関節１２が動くことで、ロボットアーム５の手先のハンド６が動くことのできる曲面である。拘束曲面３１は、実際は球面であるが、図６では、ある一断面の円のみ描いている。すなわち、第３関節１３がロック状態になると、ロボットアーム５の手先は拘束曲面３１に拘束される。

この拘束曲面３１に接する平面であり、かつ、ロボットアーム５の手先を通る平面が、接平面３２である。また、接平面３２は、ベクトルｊ_１，ｊ_２を含む平面に平行な平面である。

この接平面３２への並進外力ｆ_ｈ２の正射影が、ｆ_ｈＣである。正射影ｆ_ｈＣは、式（８）において第３関節１３の可動状態Ｌ_３＝０のときの並進外力ｆ_ｈであり、式（１４）で表される。なお、式（１４）は式（８）において「｛」と「｝」とで囲まれた部分であり、式（８）を使って、変換外力Ｆ_ｈを計算することは、正射影への変換を使用していることになる。また、正射影への変換を、式（８）だけでなく、式（９）、式（１０）の「｛」と「｝」とで囲まれた部分も、それぞれ、同様に、正射影への変換を使用していることになる。また、正射影ｆ_ｈＣは変換外力Ｆ_ｈの一例である。すなわち、拘束曲面３１の接平面３２への正射影を考えれば、ロボットアーム５が動作可能な方向成分となる。

．．．．．（１４）
また、並進外力ｆ_ｈは、並進外力ｆ_ｈ２のベクトルの先端と、並進外力ｆ_ｈ２の正射影ｆ_ｈＣのベクトルの先端とを結んだ直線（点線）上に、先端が来る中間的な値であり、その位置は可動状態Ｌ_３により定義される（可動状態Ｌ_３＝０のときにはｆ_ｈ＝ｆ_ｈＣ、可動状態Ｌ_３＝１のときにはｆ_ｈ＝ｆ_ｈ２）。

第３関節１３の手先位置が、完全可動領域ＰＭＲ内にある緩衝領域境界３３の内側である点Ｐ１に位置するときは、パターンＡ（可動状態Ｌ_３＝１）となるので、ｆ_ｈ＝ｆ_ｈ２となり、人のかける力の方向にロボットアーム５の手先は自由に動作する。ここで、第１関節１１及び第２関節１２も完全可動領域ＰＭＲ内にある場合に限れば、ｆ_ｈ２＝ｆとなり、人のかける力の方向にロボットアーム５の手先は自由に動作する。

手先が緩衝領域境界３３を越え、外側の点Ｐ２に移動すると、パターンＣ１になり、可動状態Ｌ_３が変化すると共に変化する並進外力ｆ_ｈでインピーダンス制御が行われ、徐々に動きに拘束がかかるようになる。

手先が可動領域ＭＲの境界である拘束曲面３１上の点Ｐ３に到達すると、パターンＥ１になり、並進外力ｆ_ｈＣを入力にインピーダンス制御を行うことになり、ロボットアーム５の手先の動きは拘束曲面３１に沿うような動きとなる。すなわち、ｆ_Ｖのような成分を持つ力を人からロボットアーム５にかけても、式（８）による、並進外力ｆ_ｈへの変換の際に打ち消されるため、ロボットアーム５の手先を、拘束曲面３１を越えて外側に動くことはできず、第３関節１３の角度も可動領域ＭＲを越えることはない。

一方、点Ｐ３において、−ｆ_Ｖのような成分を持つ力を人からロボットアーム５にかけた場合、第３関節１３は可動領域ＭＲ内に戻る動きとなるため、パターンＥ２となり、可動状態Ｌ_３＝ｄＬ_３とオフセット値が加算される分、並進外力ｆ_ｈは復元力のように働き、関節が可動領域ＭＲ内に戻る動きが可能となる。

また、同様に点Ｐ２においても、−ｆ_Ｖのような成分を持つ力を人からロボットアーム５にかけた場合、第３関節１３は緩衝領域ＢＲ１から完全可動領域ＰＭＲ内に戻る動きとなるため、パターンＣ２となり、可動状態Ｌ_３＝（ｑ_３ＭＡＸ−ｑ_３）／ｄｑ_３＋ｄＬ_３とオフセット値が加算される分、並進外力ｆ_ｈは復元力のように働き、関節が完全可動領域ＰＭＲ内に戻る動きが可能となる。

前記した動作制限において、緩衝領域ＢＲ１，ＢＲ２では、徐々に動作に拘束をかけることで、動作を安定させる役割を担っている。緩衝領域ＢＲ１，ＢＲ２を設けない場合、手先が拘束曲面３１上に到達すると、インピーダンス計算手段２５への入力が並進外力ｆ_ｈ２から並進外力ｆ_ｈＣへステップ的に変化することになるので、制御系、すなわち、インピーダンス制御手段４が不安定になりやすい。

また、パターンＤ２又はパターンＥ２のように可動領域ＭＲ内へ戻る力を受け付ける場合に、拘束曲面３１の近傍でリミットサイクルのような現象が起こり、制御系、すなわち、インピーダンス制御手段４がさらに不安定となる可能性がある。これに対して、緩衝領域ＢＲ１，ＢＲ２を設け、オフセット値ｄＬ_ｉを導入することで、リミットサイクルを起こさず、スムーズに動作の拘束・復帰を行うことが可能となる。

なお、パターンＢ（パターンＢ１，パターンＢ２）は、第１緩衝領域ＢＲ１に関するものであり、パターンＡの第１緩衝領域ＢＲ１とは反対側の領域である第２緩衝領域ＢＲ２のパターンＣ（パターンＣ１，パターンＣ２）と同様であるため、説明を省略する。パターンＤ（パターンＤ１，パターンＤ２）は、第１緩衝領域ＢＲ１の可動領域外に関するものであり、パターンＡの第１緩衝領域ＢＲ１とは反対側の領域である第２緩衝領域ＢＲ２の可動領域外のパターンＥ（パターンＥ１，パターンＥ２）と同様であるため、説明を省略する。

以上は、第３関節１３の場合を中心に説明を行ったが、第１関節１１及び第２関節１２の場合も同様であり、第１の変換方法の場合には、それぞれ、式（９）及び式（１０）に基づく変換を行うことにより、また、第２の変換方法の場合には、パターンβの変換を行うことにより、各関節の可動領域ＭＲに応じたロボットアーム５の動作の拘束が可能となる。そして、第１の変換方法の場合、一連の式（７）〜式（１１）を使うことにより、第１関節１１と、第２関節１２と、第３関節１３とに対して、平行な関節軸の関節が同時に完全可動領域外になる場合を除き、同時に動作制限を行うことが可能となる。

第２の変換方法の場合、パターンα〜パターンδのパターン分けにより、複数の関節が同時にロックする場合も厳密に扱うことができる。

図１２は、式（１０３）に現れる並進外力ｆ_ｈ、ｆの関係を示す。図１２においては、ロボットアーム５の第１関節１１から第３関節１３までを模式的に表している。拘束曲線２０１は、第２関節１２と第３関節１３が可動領域ＭＲを越えて、それ以上動けずロックされた状態の場合に、残りの第１関節１１が動くことで、ロボットアーム５の手先のハンド６が動くことのできる曲線である。すなわち、第２関節１２と第３関節１３がロック状態になると、ロボットアーム５の手先は拘束曲線２０１に拘束される。

この拘束曲線２０１の接線であり、かつ、ロボットアーム５の手先を通る直線が、接線２０２である。また、接線２０２は、ベクトルｊ_１に平行な直線である。

この接線２０２への並進外力ｆの正射影が、ｆ_ｈｃである。正射影ｆ_ｈｃは、式（１０３）において第２関節１２の可動状態Ｌ_２＝０かつ第３関節１３の可動状態Ｌ_３＝０のときの並進外力ｆ_ｈであり、式（１０５）で表される。なお、式（１０５）はＬ_２＝０の場合の式（１０３）であり、式（１０３）を使って、変換外力Ｆ_ｈを計算することは、正射影への変換を使用していることになる。すなわち、拘束曲線２０１の接線２０２への正射影を考えれば、ロボットアーム５が動作可能な方向成分となる。

．．．．．（１０５）
また、並進外力ｆ_ｈは、並進外力ｆのベクトルの先端と、並進外力ｆの正射影ｆ_ｈｃのベクトルの先端とを結んだ直線（点線）上に、先端が来る中間的な値であり、その位置は可動状態Ｌ_２Ｌ_３により定義される（可動状態Ｌ_２Ｌ_３＝０のときにはｆ_ｈ＝ｆ_ｈＣ、可動状態Ｌ_３＝１のときにはｆ_ｈ＝ｆ_２）。

以上のように式（１０５）に基づく変換によれば、第２関節１２と第３関節１３の２つの関節がロックする場合の動作も扱うことが可能となる。

ロボットアーム５の手先の姿勢（φ、θ、ψ）を決める第４関節１４、第５関節１５、第６関節１６に関しては、式（１２）に基づくことで各関節の動作制限が実現する。式（１２）において、Ｊ_ｖ（ｑ）^ＴＦの部分は式（６）右辺と同様であり、外力Ｆにより各関節１１，１２，１３，１４，１５，１６に発生するトルクτ＝［τ_１，τ_２，τ_３，τ_４，τ_５，τ_６］^Ｔに相当する。このトルクベクトルの第３行，第４行，第５行に、それぞれ第４関節１４、第５関節１５、第６関節１６の関節可動状態Ｌ_４，Ｌ_５，Ｌ_６をかけたベクトル［τ_１，τ_２，τ_３，Ｌ_４τ_４，Ｌ_５τ_５，Ｌ_６τ_６］^Ｔを、式Ｊ_ｖ（ｑ）^−Ｔに用いて手先外力に逆変換し、モーメント成分のみを抽出したのがｎ_ｈである。したがって、例えば、Ｌ_４＝０とすると、τ_４＝０となるので、モーメント成分ｎ_ｈを入力としてインピーダンス制御すると、第４関節１４は動作せず固定されることになる。

第５関節１５及び第６関節１６に関しても同様であり、以上の原理により、関節可動状態Ｌ_４，Ｌ_５，Ｌ_６に基づき、ロボットアーム５の手先の姿勢（φ、θ、ψ）を決める手首部７である第４関節１４と第５関節１５と第６関節１６との動作制限が実現する。

以上、本第１実施形態によれば、関節可動状態計算手段２及び力変換手段２４を有することにより、インピーダンス制御による動作時に人がロボットアーム５の関節の可動領域ＭＲを越えるような操作を行っても、関節可動状態計算手段２において当該関節の可動状態を判定し、力変換手段２４により変換された変換外力Ｆ_ｈをインピーダンス計算手段２５への入力として使うことで、ロボットアーム５の動作制限が可能となり、関節の可動領域ＭＲを越えるのを防ぐことができる。

また、本第１実施形態によれば、インピーダンス計算手段２５への入力に対し力変換手段２４により変換をかけることにより動作制限を行う、すなわち、入力を外力Ｆから変換外力Ｆ_ｈへと制限することで動作制限を行うため、ロボットアーム５の機構的な可動領域ＭＲを越えてロボットアーム５が動けない状態でも、インピーダンス計算手段２５に入力が入り続け、後段にあるインピーダンス計算手段２５又は位置制御系２９に誤差が蓄積してしまうことがなく、安定な制御系とすることができるという格別の効果を発揮する。

さらに、力変換手段２４を使わずに、位置制御系２９において関節の動作制限を行う場合が（公知技術として）考えられるが、その場合は、後段にあるインピーダンス計算手段２５又は位置制御系２９に誤差蓄積に対する対策を施す必要があり、複雑な制御系構造となってしまう。これに対して、力変換手段２４を使う本第１実施形態では、誤差蓄積に対する対策を施す必要はなく、簡潔な制御系構造とすることができる。

また、前記第１実施形態では、インピーダンス制御のインピーダンスパラメータを変化させることで動作制限を行う方式ではないため、パラメータ可変のための仕組みが不要であり、制御系の構造を簡潔にすることができ、さらに、パラメーター変化により制御系が不安定になることを防ぐことができる。すなわち、前記第１実施形態では、力変換手段２４により入力制限を行うこと（言い換えれば、力変換手段２４により外力Ｆが変換された変換外力Ｆ_ｈをインピーダンス計算手段２５への入力として使うこと）で、前記優れた効果を発揮させることができるので、インピーダンスパラメータを変化させる必要はない。

さらに、本第１実施形態によれば、各関節の動作制限は独立に行われるため、仮にある関節が動作制限されても、動作可能な他の関節は動作制限されず動き続けることが可能であるので、すぐに動作制限がかかり停止するようなことのない、操作性の良いロボットとすることができる。

（第２実施形態）
本発明の第２実施形態におけるロボットの基本的な構成は、図１及び図２に示した第１実施例の場合と同様であるので、共通部分の説明は省略し、異なる部分についてのみ以下、詳細に説明する。

図７に示すように、第２実施形態では、通常のインピーダンス制御手段４３（すなわち、インピーダンス計算手段２５と目標軌道生成手段２３と順運動学計算手段２６と位置誤差補償手段２７と近似逆運動学計算手段２８と第１演算部６１と第２演算部６２）とは独立した力入力制御手段３４に、関節可動状態計算手段２と力変換手段２４とを配設し、既存の制御装置（第１制御装置）１Ｂと通信する構成としている。

図８は、第２実施形態のロボット９０Ｂの全体構成を示しており、制御装置の具体的な構成を示している。第２実施形態の制御装置では、第１制御装置１Ｂとは別に第２制御装置４６が追加され、２つの独立した制御装置１Ｂ，４６で構成される。

第２制御装置４６は、ハードウェア的には一般的なパーソナルコンピュータにより構成されている。そして、入出力ＩＦ（Ｄ／Ａボード４５とＡ／Ｄボード４７とカウンタボード４８）を除く部分は、パーソナルコンピュータで実行される制御プログラム４４により力入力制御手段３４を実現する。力入力制御手段３４は、Ａ／Ｄボード４７を通じて力センサー３より外力Ｆ_ｓが入力され、力変換手段２４による変換結果である変換外力Ｆ_ｈが、Ｄ／Ａボード４５を通じて第１制御装置１Ｂの力センサー入力であるＡ／Ｄボード２１に出力され、Ａ／Ｄボード２１を通じて第１制御装置１Ｂに取り込まれる。このように、Ｄ／Ａボード４５とＡ／Ｄボード２１とを接続することで、電圧値により変換外力Ｆ_ｈが伝えられる通信手段が構成されている。

また力入力制御手段３４は、カウンタボード４８を通じてロボットアーム５の各関節部のエンコーダ４２より出力される関節角度情報を取得し、関節可動状態計算手段２による、関節１１，１２，１３，１４，１５，１６の可動状態の判定のための情報として使用される。

本第２実施形態は、前記通常のインピーダンス制御手段４３に、力入力制御手段３４を別途追加した構成であるため、図７のような、２つに分離独立した構成が容易に実現できる。したがって第１実施形態は、図１に示すように一つの制御装置（ＰＣ）１上で動作する１つの制御プログラム１７によりインピーダンス制御手段４を実現したが、第２実施形態は、図８に示すように第１制御装置１Ｂに第２制御装置４６を加え、既存の第１制御装置１Ｂと通信する構成が可能となる。

この構成により、例えば、市販のロボット制御装置の力センサーからの入力信号を処理するために第２制御装置４６を追加するだけで、ロボットアーム５の動作制限を行なうことができる。

（第３実施形態）
本発明の第３実施形態における制御装置の基本的な構成は、図１及び図２に示した第１実施例の場合と同様であるので、共通部分の説明は省略し、異なる部分についてのみ以下、詳細に説明する。

図９は、本発明の第３実施形態におけるロボット９０Ｃのインピーダンス制御手段の構成を示すブロック線図であり、図１０はロボット９０Ｃによる人１００との協調搬送作業を説明する図である。

図１０に示すように、第３実施形態のロボット９０Ｃは、手首部７に操作ハンドル４０が力センサー３を介して固定されおり、人１００は操作ハンドル４０を直接握り、力をかけることでロボット９０Ｃを操作することができる。操作ハンドル４０とロボットアーム５の間には力センサー３が配設されており、人１００が操作するときの力を力センサー３で検出することができる。

また、操作ハンドル４０には、動作モード設定部の一例としての動作モード設定スイッチ３９が設けられており、人１００は、動作モード入力部の一例として機能する動作モード設定スイッチ３９を操作することにより、動作モード（可動又は固定のモード）の設定及び切換を行うことができる。

図９に示すように、動作モード設定スイッチ３９から動作モード指令が関節動作モード設定手段３７に入力される。さらに、関節可動状態計算手段２から関節可動状態Ｌ_ｉが関節動作モード設定手段３７に入力され、力変換手段２４に拡張関節可動状態Ｌ_ｉｍが出力される。

図１０に示すような人１００とロボット９０Ｃとの協調作業形態の場合、物体３８の負荷をすべてロボットアーム５が受け持つため、重力補償手段３０は不要である。この場合には、力センサー３の計測値Ｆ_ｓが力変換手段２４に外力Ｆとして入力されて、変換外力Ｆ_ｈに変換される。

動作モード設定スイッチ３９では、ロボットアーム５の６つの関節１１，１２，１３，１４，１５，１６のそれぞれの可動又は固定のモード設定を行うことができる。

インピーダンス制御手段４内に備えられた関節動作モード設定手段３７は、動作モード設定スイッチ３９で設定した各関節の可動又は固定のモード設定に応じて、可動のモード設定の場合はそのまま可動状態Ｌ_ｉを拡張関節可動状態Ｌ_ｉｍとして出力し、固定のモード設定の場合は拡張関節可動状態Ｌ_ｉｍ＝０を出力する。これらの拡張関節可動状態Ｌ_ｉｍは、前記第１実施形態で説明した可動状態Ｌ_ｉと同様に取り扱い、変換手段２４に入力されて使用される。これにより、固定のモード設定された関節は、第１実施形態で説明した原理により、その動作が制限されて固定される。

以上のように、本第３実施形態によれば、関節動作モード設定手段３７を有することにより、特定の関節を固定した動作、又は、特定の関節のみを回転させるような動作が容易に可能となり、より操作性の良いロボットとすることができる。

第３実施形態にかかるロボットの動作の具体例としては、図１１に示すように、第１関節１１のみ回転させ、ロボットアーム５を台部１０に対して旋回させるような動作をさせるときには、動作モード設定スイッチ３９で第１関節１１を可動と設定し、第２〜第６関節１２〜１６を固定に設定すれば、人１００が操作ハンドル４０に力をかけるだけでロボットアーム５の第１関節１１のみを容易に回転させる旋回動作を実現することができる。

なお、前記様々な実施形態のうちの任意の実施形態を適宜組み合わせることにより、それぞれの有する効果を奏するようにすることができる。

本発明のロボット、ロボットの制御装置、及び制御方法は、ロボットアームの関節の動作範囲の規制など、関節の動作状況に応じたロボットアームの動作制限を、簡潔な制御系構造で実現し、制御系の不安定さを低減することができて、工場又は家庭内、介護現場等でパワーアシスト等の作業支援を行うロボット等、人と協働するロボット、ロボットの制御装置、及び制御方法として有用である。

本発明は、添付図面を参照しながら好ましい実施形態に関連して充分に記載されているが、この技術の熟練した人々にとっては種々の変形又は修正は明白である。そのような変形又は修正は、添付した請求の範囲による本発明の範囲から外れない限りにおいて、その中に含まれると理解されるべきである。

Claims (9)

1. 多関節ロボットアームと、
   前記多関節ロボットアームに配設され、かつ、外力を検出する外力検出手段と、
   前記多関節ロボットアームの関節が可動範囲のいずれかに位置して可動状態を計算する関節可動状態計算手段と、
   前記関節可動状態計算手段により計算された前記関節の前記可動状態に基づき、前記外力検出手段が検出した前記外力を、前記外力が前記ロボットアームに加えられたときに前記ロボットアームが動作可能な方向成分を抽出可能な変換外力に変換する外力変換手段と、
   前記外力変換手段により変換された前記変換外力に基づいてインピーダンス制御を行うことにより、前記多関節ロボットアームの前記関節の前記可動状態に応じた動作制限が行われるように前記多関節ロボットアームを制御する制御手段とを有し、
   前記外力変換手段は、前記外力検出手段に検出された前記外力を、前記多関節ロボットアームの前記関節が固定されたときに前記多関節ロボットアームの手先が動くことのできる拘束曲面の接平面への正射影として変換するロボット。
2. 多関節ロボットアームと、
   前記多関節ロボットアームに配設され、かつ、外力を検出する外力検出手段と、
   前記多関節ロボットアームの関節が可動範囲のいずれかに位置して可動状態を計算する関節可動状態計算手段と、
   前記関節可動状態計算手段により計算された前記関節の前記可動状態に基づき、前記外力検出手段が検出した前記外力を、前記外力が前記ロボットアームに加えられたときに前記ロボットアームが動作可能な方向成分を抽出可能な変換外力に変換する外力変換手段と、
   前記外力変換手段により変換された前記変換外力に基づいてインピーダンス制御を行うことにより、前記多関節ロボットアームの前記関節の前記可動状態に応じた動作制限が行われるように前記多関節ロボットアームを制御する制御手段とを有し、
   さらに、前記多関節ロボットアームにかかる前記外力により各関節に発生する外力トルクを計算する外力トルク計算手段を有し、
   前記関節可動状態計算手段は、前記外力トルク計算手段の計算結果に基づき前記外力トルクが前記多関節ロボットアームの各関節の動作を可動領域外より可動領域内に戻そうとする力であるかどうかを判定し、
   前記関節可動状態計算手段が、前記外力トルクが前記多関節ロボットアームの各関節の動作を前記可動領域外より前記可動領域内に戻そうとする力であると判定する場合には、前記制御手段は前記動作制限を行わない一方、
   前記関節可動状態計算手段が、前記外力トルクが前記多関節ロボットアームの各関節の動作を前記可動領域外より前記可動領域内に戻そうとする力ではないと判定する場合には、前記制御手段は前記動作制限を行うロボット。
3. 多関節ロボットアームと、
   前記多関節ロボットアームに配設され、かつ、外力を検出する外力検出手段と、
   前記多関節ロボットアームの関節が可動範囲のいずれかに位置して可動状態を計算する関節可動状態計算手段と、
   前記関節可動状態計算手段により計算された前記関節の前記可動状態に基づき、前記外力検出手段が検出した前記外力を、前記外力が前記ロボットアームに加えられたときに前記ロボットアームが動作可能な方向成分を抽出可能な変換外力に変換する外力変換手段と、
   前記外力変換手段により変換された前記変換外力に基づいてインピーダンス制御を行うことにより、前記多関節ロボットアームの前記関節の前記可動状態に応じた動作制限が行われるように前記多関節ロボットアームを制御する制御手段とを有し、
   前記関節可動状態計算手段は、前記外力を関節トルクに変換することにより、前記関節トルクに変換された前記変換外力が、前記多関節ロボットアームの各関節の動作を可動領域外より可動領域内に戻そうとする力であるかどうかを判定し、
   前記関節可動状態計算手段が、前記変換外力が、前記多関節ロボットアームの各関節の動作を前記可動領域外より前記可動領域内に戻そうとする力であると判定する場合には、前記制御手段は前記動作制限を行わない一方、
   前記関節可動状態計算手段が、前記変換外力が、前記多関節ロボットアームの各関節の動作を前記可動領域外より前記可動領域内に戻そうとする力ではないと判定する場合には、前記制御手段は前記動作制限を行うロボット。
4. 多関節ロボットアームと、
   前記多関節ロボットアームに配設され、かつ、外力を検出する外力検出手段と、
   前記多関節ロボットアームの関節が可動範囲のいずれかに位置して可動状態を計算する関節可動状態計算手段と、
   前記関節可動状態計算手段により計算された前記関節の前記可動状態に基づき、前記外力検出手段が検出した前記外力を、前記外力が前記ロボットアームに加えられたときに前記ロボットアームが動作可能な方向成分を抽出可能な変換外力に変換する外力変換手段と、
   前記外力変換手段により変換された前記変換外力に基づいてインピーダンス制御を行うことにより、前記多関節ロボットアームの前記関節の前記可動状態に応じた動作制限が行われるように前記多関節ロボットアームを制御する制御手段とを有し、
   前記多関節ロボットアームの各関節に対し、可動あるいは固定の動作モードを設定する動作モード設定部と、
   前記動作モード設定部で設定された前記動作モードの指令が入力されて、前記動作モードと前記関節可動状態計算手段により計算された前記関節の前記可動状態とに基づき前記可動状態の計算を行う関節動作モード設定手段とをさらに備えて、
   前記外力変換手段は、前記関節動作モード設定手段により計算された前記関節の前記可動状態に基づき、前記外力検出手段が検出した前記外力を、前記外力が前記ロボットアームに加えられたときに前記ロボットアームが動作可能な方向成分を抽出可能な変換外力に変換し、
   前記制御手段は、前記変換外力に基づいてインピーダンス制御を行うことにより、前記多関節ロボットアームの前記関節の前記可動状態に応じた動作制限が行われるロボット。
5. 多関節ロボットアームと、
   前記多関節ロボットアームに配設され、かつ、外力を検出する外力検出手段と、
   前記多関節ロボットアームの関節が可動範囲のいずれかに位置して可動状態を計算する関節可動状態計算手段と、
   前記関節可動状態計算手段により計算された前記関節の前記可動状態に基づき、前記外力検出手段が検出した前記外力を、前記外力が前記ロボットアームに加えられたときに前記ロボットアームが動作可能な方向成分を抽出可能な変換外力に変換する外力変換手段と、
   前記外力変換手段により変換された前記変換外力に基づいてインピーダンス制御を行うことにより、前記多関節ロボットアームの前記関節の前記可動状態に応じた動作制限が行われるように前記多関節ロボットアームを制御する制御手段とを有し、
   さらに、前記多関節ロボットアームにかかる前記外力により各関節に発生する外力トルクを計算する外力トルク計算手段を有し、
   前記関節可動状態計算手段は、前記外力トルク計算手段の計算結果に基づき前記外力トルクが前記多関節ロボットアームの各関節の動作を可動領域外より可動領域内に戻そうとする力であるかどうかを判定し、
   前記関節可動状態計算手段が、前記外力トルクが前記多関節ロボットアームの各関節の動作を前記可動領域外より前記可動領域内に戻そうとする力であると判定する場合には、前記制御手段は前記動作制限を行わない一方、
   前記関節可動状態計算手段が、前記外力トルクが前記多関節ロボットアームの各関節の動作を前記可動領域外より前記可動領域内に戻そうとする力ではないと判定する場合には、前記制御手段は前記動作制限を行い、
   前記関節可動状態計算手段は、前記関節の可動領域の両端の境界線から一定範囲だけ内側に緩衝領域をそれぞれ設け、前記各緩衝領域では前記関節の前記可動状態を徐々に変化させるロボット。
6. 多関節ロボットアームと、
   前記多関節ロボットアームに配設され、かつ、外力を検出する外力検出手段と、
   前記多関節ロボットアームの関節が可動範囲のいずれかに位置して可動状態を計算する関節可動状態計算手段と、
   前記関節可動状態計算手段により計算された前記関節の前記可動状態に基づき、前記外力検出手段が検出した前記外力を、前記外力が前記ロボットアームに加えられたときに前記ロボットアームが動作可能な方向成分を抽出可能な変換外力に変換する外力変換手段と、
   前記外力変換手段により変換された前記変換外力に基づいてインピーダンス制御を行うことにより、前記多関節ロボットアームの前記関節の前記可動状態に応じた動作制限が行われるように前記多関節ロボットアームを制御する制御手段とを有し、
   前記多関節ロボットアームの前記関節の可動領域の両端の内側にそれぞれ設けられた緩衝領域と、前記緩衝領域を除く前記可動領域の残りの領域である完全可動領域ＰＭＲと、可動領域外の領域とに分割し、
   前記関節可動状態計算手段は、各領域毎に、前記関節の前記可動状態の値を異ならせるように計算されるロボット。
7. 多関節ロボットアームを備えるロボットの制御装置であって、
   前記多関節ロボットアームの関節が可動範囲のいずれかに位置して可動状態を計算する関節可動状態計算手段と、
   前記関節可動状態計算手段により計算された前記関節の前記可動状態に基づき、前記多関節ロボットアームに配設されて外力を検出する外力検出手段が検出した前記外力を、前記外力が前記ロボットアームに加えられたときに前記ロボットアームが動作可能な方向成分を抽出可能な変換外力に変換する外力変換手段と、
   前記外力変換手段により変換された前記変換外力に基づいてインピーダンス制御を行うことにより、前記多関節ロボットアームの前記関節の前記可動状態に応じた動作制限が行われるように前記多関節ロボットアームを制御する制御手段とを有するとともに、
   前記外力変換手段は、前記外力検出手段に検出された前記外力を、前記多関節ロボットアームの前記関節が固定されたときに前記多関節ロボットアームの手先が動くことのできる拘束曲面の接平面への正射影として変換するロボットの制御装置。
8. 多関節ロボットアームを備えるロボットの制御方法であって、
   前記多関節ロボットアームの関節が可動範囲のいずれかに位置して可動状態を関節可動状態計算手段で計算し、
   前記計算された前記関節の前記可動状態に基づき、前記多関節ロボットアームに配設されて外力を検出する外力検出手段が検出した前記外力を、前記外力が前記ロボットアームに加えられたときに前記ロボットアームが動作可能な方向成分を抽出可能な変換外力に外力変換手段で変換することにより、前記外力検出手段に検出された前記外力を、前記多関節ロボットアームの前記関節が固定されたときに前記多関節ロボットアームの手先が動くことのできる拘束曲面の接平面への正射影として前記外力変換手段で変換し、
   前記変換された変換外力に基づいてインピーダンス制御を行うことにより、前記多関節ロボットアームの前記関節の前記可動状態に応じた動作制限が行われるように前記多関節ロボットアームを制御手段で制御するロボットの制御方法。
9. 多関節ロボットアームを備えるロボットの制御プログラムであって、
   コンピュータを、
   前記多関節ロボットアームの関節が可動範囲のいずれかに位置して可動状態を計算する関節可動状態計算手段と、
   前記関節可動状態計算手段により計算された前記関節の前記可動状態に基づき、前記多関節ロボットアームに配設されて外力を検出する外力検出手段が検出した前記外力を、前記外力が前記ロボットアームに加えられたときに前記ロボットアームが動作可能な方向成分を抽出可能な変換外力に変換することにより、前記外力検出手段に検出された前記外力を、前記多関節ロボットアームの前記関節が固定されたときに前記多関節ロボットアームの手先が動くことのできる拘束曲面の接平面への正射影として変換する外力変換手段と、
   前記外力変換手段により変換された前記変換外力に基づいてインピーダンス制御を行うことにより、前記多関節ロボットアームの前記関節の前記可動状態に応じた動作制限が行われるように前記多関節ロボットアームを制御する制御手段として機能させるためのロボットの制御プログラム。

Images