JP2008200763A - 作業用マニピュレータの制御装置 - Google Patents

Abstract

【課題】特異点に接近もしくは通過するような目標軌道を与えられた場合でも、計算上の破綻なく連続してマニピュレータを動作させて作業を続行可能とする。
【解決手段】作業用マニピュレータの制御装置は、目標値生成手段１０１、感度パラメータ計算手段１０３、対応感度計算手段１０５、複数の制御手段１０９〜１１３、制御方式切替手段１０７、及び外力検出手段を備える。感度パラメータ計算手段１０３はマニピュレータ機構からの関節変位応答を用いて感度パラメータを計算し、対応感度計算手段１０５は感度パラメータを用いて対応感度を計算し、制御方式切替手段１０７は対応感度に基づいて制御手段１０９〜１１３の切り替えを行う。各制御手段１０９〜１１３は、目標値、関節変位応答、外力検出手段で検出された外力、及び感度パラメータを用いて制御処理を行う。
【選択図】図１

Description

本発明は、作業用マニピュレータの制御装置に係り、例えば複数の関節を有するリンク機構からなり主に接触作業を目的としたマニピュレータの制御装置に好適なものである。

接触作業を目的としたマニピュレータの制御装置の例として、例えば特開平７−１３６４２号公報（特許文献１）に示されるマニピュレータのコンプライアンス制御装置がある。ここでは、マニピュレータ手先点が特異点領域に近付けば近付くほど大きさが大きくなり、マニピュレータ手先点が特異点領域から遠ざかる向きを持つ仮想外力ベクトルを与えることにより、特異点領域から外れる方向に動作させる制御装置が示されている。

特開平７−１３６４２号公報

一般に、作業用マニピュレータで接触作業を行う場合、先端における目標位置を与え、慣性項・粘性項・剛性項の和からなる運動方程式に基づいて先端外力との関係付けを行うことで、安定な特性を実現することを目的としたコンプライアンス制御という制御方式がある。

通常、外界に働きかける制御はマニピュレータ先端の並進と回転を表す作業空間（位置３自由度、姿勢３自由度からなる６次元の空間）を基準に記述されているが、最終的には各関節変位が張る関節空間（通常６次元）に位置や力の情報を変換して制御を行う。このとき、作業空間と関節空間とを対応付ける感度パラメータ（微小変位間の対応係数）が必要となる。そのようなパラメータは行列の形で整理され、速度や微小変位、或いは力の変換に用いられる。これはマニピュレータのヤコビアン行列（もしくはヤコビ行列と略記されることもある）と呼ばれる。該ヤコビアン行列は関節の微小変位を作業空間での位置姿勢の微小変位に変換するものであるが、コンプライアンス制御ではその逆変換すなわち逆行列が必要になる部分がある。

しかしながら、当該行列の要素の値はマニピュレータの姿勢、言い換えれば各関節変位に応じて変化し、中には逆行列を求められなくなる要素値を与える関節変位の組（関節空間上の点）がある。そのような点を特異点、或いはそのような状態を退化状態と呼ぶ。上記の理由により、特異点に接近またはそれを通過するような軌道上では、かりに関節の可動範囲に関して余裕がある状態でもコンプライアンス制御を含め上記逆行列を利用する全ての制御方式に支障が生じていた。そのため従来は、この特異点に近付かないようにする様々な制御方法・装置によって回避せざるを得なかった。

本発明の目的は、特異点に接近もしくは通過するような目標軌道を与えられた場合でも、計算上の破綻なく連続してマニピュレータを動作させて作業を続行することができる作業用マニピュレータの制御装置を提供することにある。

前述の目的を達成するために、本発明は、作業空間基準もしくは関節空間基準の目標変位およびその微係数・目標発生力を指令値として生成する目標値生成手段と、関節空間−作業空間対応の感度パラメータを計算する感度パラメータ計算手段と、対応感度を計算する対応感度計算手段と、複数の制御手段と、前記複数の制御手段を切り替える制御方式切替手段と、作業空間基準もしくは関節空間基準の外力検出手段とを備えた作業用マニピュレータの制御装置であって、前記感度パラメータ計算手段はマニピュレータ機構からの関節変位応答を用いて感度パラメータを計算し、前記対応感度計算手段は前記感度パラメータ計算手段で計算された感度パラメータを用いて対応感度を計算し、前記制御方式切替手段は前記対応感度計算手段で計算された対応感度に基づいて制御手段の切り替えを行い、前記各制御手段は、前記目標値生成手段で生成された目標変位および目標発生力と、前記マニピュレータ機構からの関節変位応答と、前記外力検出手段で検出された外力と、前記感度パラメータ計算手段で計算された感度パラメータとを用いて制御処理を行うことにある。

本発明の作業用マニピュレータの制御装置によれば、特異点に接近もしくは通過するような目標軌道を与えられた場合でも、計算上の破綻なく連続してマニピュレータを動作させて作業を続行することができる。

以下、本発明の一実施例を、図面を用いて説明する。

図１は本発明の一実施例に係る作業用マニピュレータの制御装置の構成を示すブロック図である。図１において、１０１はマニピュレータ動作目標値生成手段、１０２は伝達される各種動作目標値、１０３は関節空間−作業空間対応感度パラメータ計算手段、１０４は各種動作目標値１０２とそれに加えて感度パラメータ計算手段１０３で計算された感度パラメータ、１０５は対応感度計算手段、１０６は１０２を含んだ感度パラメータ１０４とそれに加えて対応感度計算手段１０５で計算された対応感度、１０７は制御方式切替手段、１０８はいずれかの制御方式に対応感度１０６と同じ情報を伝達することを示す矢印、１０９〜１１３は制御方式１〜ｎで制御される制御手段１〜ｎ、１１４〜１１８は制御手段１０９〜１１３から出力された制御量、１１９はマニピュレータ機構、１２０はマニピュレータに働く外力および動作応答の情報を示す。

目標値生成手段１０１では、選択された制御手段１０９〜１１３に応じてマニピュレータ先端位置・速度が生成される。すなわち、目標値生成手段１０１は、作業空間基準もしくは関節空間基準の目標変位およびその微係数・目標発生力を指令値として生成する手段である。指令として生成する変位微係数の階数の最大値を２としている。感度パラメータ計算手段１０３は動作応答１２０のうちの関節位置応答の情報を用いて１０４のうちの対応感度パラメータ行列の各要素を計算する手段である。対応感度計算手段１０５は１０４のうちの該パラメータ行列の要素を用いて対応感度１０６を計算する手段である。制御方式切替手段１０７は対応感度１０６に応じて制御手段１０９〜１１３を選択して切り替える手段である。各制御手段１０９〜１１３は前記目標値１０２および外力、動作応答を用いて制御量１１４〜１１８を計算し、マニピュレータ機構１１９に与える。

次に、制御手段１〜ｎの数がｎ＝２で、作業空間基準でのコンプライアンス制御とスティフネス制御とを切り替える場合の処理の流れを図２を用いて説明する。ここでは、簡単のために、対象とするマニピュレータは６自由度で全関節が回転関節であるものとし、関節に関し一般的な表現として位置・速度と表記していたものを角度・角速度とする。

まず、作業空間での先端目標位置姿勢Ｘｒおよび目標速度・角速度ｄＸｒ／ｄｔが生成される（ステップ２０１）。目標姿勢の表現方法は、３ｘ３の回転行列によるもの、オイラーパラメータ（四元数）によるもの、などの何通りか考えられるが、ここでは、Roll-Pitch-Yaw角α ＝［α β γ］Ｔ（以下、行列の右側に置かれたＴは転置を表す）で表現する。ここで、α、β、γはｚ、ｙ、ｘ各軸まわりの回転角である。また目標速度・角速度は［ｄｘ／ｄｔ ｄｙ／ｄｙ ｄｚ／ｄｔ ｄα／ｄｔ ｄβ／ｄｔ ｄγ／ｄｔ］を表す。

次に、現在の関節角ベクトルΘ＝［θ１ θ２ θ３ θ４ θ５ θ６］Ｔからヤコビアン行列Ｊ（Θ）を、ΘとＪからｄＸ／ｄｔを以下の手順(1)〜(4)で計算する（ステップ２０２）。なお、座標Σｋの設定やＡｋの導出やその他の詳細などは、例えばロボット・マニピュレータ、Richard.P.Paul著、吉川訳、コロナ社（１９８４）等に開示されている。
(1) 次に示す数式１から同次変換行列Ｔｋ（ｋ＝１．．６）を導く。数式１の中のＡｋはリンクに固定された座標Σｋ−１からΣｋへの変換行列である。

(2) 次に示す数式２から基礎ヤコビアン行列ＪＢ（Θ）を導く。数式２の中のｚ０は、ベース座標Σ０のｚ方向単位ベクトルであり、ｚ０＝［０ ０ １］Ｔ、ｐ０は、Σ０の原点位置であり、ｐ０＝［０ ０ ０］Ｔである。ｚ１〜ｚ５、ｐ１〜ｐ５は、Σ１〜Σ５のｚ方向単位ベクトルおよび原点位置をΣ０基準で表現したものである。ｚｋ、ｐｋは、Ｔｋの（１、３）（２、３）（３、３）要素および（１、４）（２、４）（３、４）要素からなるベクトルである。ｐは、先端座標系の原点をΣ０基準で表現したものであり、Ｔ６の（１、４）（２、４）（３、４）要素からなる。

ＪＢ（Θ）は先端座標Σ６の、Σ０に対する速度・角速度の関係を示している。すなわち、この速度・角速度をそれぞれｖ＝［ｖｘ ｖｙ ｖｚ］Ｔ、ω＝［ωｘ ωｙ ωｚ］Ｔとし、ν＝［ｖＴ ωＴ］Ｔとすると、ν＝ＪＢ（Θ）ｄΘ／ｄｔの関係がある。
(3) 姿勢角の表現方法に応じてωとｄα ／ｄｔを変換する。ここでは、Roll-Pitch-Yaw角表現の場合を示す。ωとｄα ／ｄｔの関係は次の数式３のように書ける。

このときＫzyxは数式４のように書ける。式中Ｓα、Ｓβ、Ｃα、Ｃβはｓｉｎα、ｓｉｎβ、ｃｏｓα、ｃｏｓβを示している。これ以降も同様である。

なお、数式３、数式４の導出に関しては、例えばロボットモーション、内山、中村著、岩波書店（２００４）等に開示されている。

よって、前記のνとの関係は、次の数式５のようになる。

(4) 次に、ＪＢとＪの関係を導く。

数式５のようにＫ（α ）を定義すると、νとＪＢ、Ｊの関係は、次に示す数式６、数式７のようになる。

以上よりＪ（Θ）が導かれるので、ｄＸ／ｄｔ＝Ｊ（Θ）ｄΘ／ｄｔとして先端の速度・角速度を求める。ｄΘ／ｄｔは実装上はΘの差分に代えるか、オブザーバ等の演算で求めるかなど、方法は特に限定しない。

次に、対応感度を計算する（ステップ２０３）。対応感度Ｓとしてヤコビアン行列の行列式を用いるとすると、detＪは次に示す数式８のようになる。この値が小さいほどＪは非正則な状態、言い換えれば逆行列を求める際に誤差が非常に大きくなる状態に近付いている。

ここで、Ｋ（α ）の逆行列は次に示す数式９のようであるから、その行列式は数式１０のようになる。

結局、Ｊ（Θ）の行列式は次に示す数式１１のようになる。

ただし、数式１１の中のＣβ＝ｃｏｓβは姿勢角の表現に依存したものであり、表現形式が変わればこの項も変わる。例えば、姿勢角の表現をオイラー角（ｚ−ｙ−ｚ回転）にすれば、この部分はｓｉｎβになる。これらは機構の特異点に直接関係はない。結局、機構特異点への接近度はdetＪＢで評価すればよい。

ステップ２０４では、ステップ２０３で計算された対応感度Ｓと設定された閾値Ｓｃとを比較し、対応感度Ｓが設定された閾値Ｓｃを超えない場合は、Ｊの逆行列を用いず、Ｊとその転置ＪＴを用いて作業空間での位置姿勢Ｘｒ、およびその時間変化率ｄＸｒ／ｄｔを指令とするスティフネス制御を行う（ステップ２０５）。逆にＳ≧Ｓｃの場合は、Ｊから転置行列と逆行列を導き、目標位置姿勢Ｘｒ、外力Ｆ等を用いてコンプライアンス制御を行う（ステップ２０６）。

ヤコビアン行列の行列式を計算する方法は特異点への接近を確実に検出できるが、特定のマニピュレータ機構においてはより処理量の少ない方法で特異点への接近を知ることができる。一例として、図３に示すような機構構成の６自由度マニピュレータを考える。図３において、３０１はリンク０（土台リンク）、３０２は関節１、３０３はリンク１、３０４は関節２、３０５はリンク２、３０６は関節３、３０７はリンク３、３０８は関節４、３０９はリンク４、３１０は関節５、３１１はリンク５、３１２は関節６、３１３はリンク６（先端リンク）、３１４は接触センサ、３１５は力センサである。接触センサ３１４は作業対象との接触状態を検出する手段であり、力センサ３１５は作業対象からの外力を検出する手段ある。なお、外力検出手段としては、作業空間基準もしくは関節空間基準の外力検出手段が考えられる。本実施例のマニピュレータは、複数の関節を有するリンク機構からなり、主に接触作業を目的とした作業をするのに好適なものである。

ここで、糸巻き形の記号で示される関節１、３、５はリンク１、３、５の長手方向を軸として捻るように回る。○に黒点で示される関節２、４、６は黒点を中心にリンク２、４、６を振るように回る。関節ｎ（ｎ＝１．．６）の回転角をθｎとする。また、基準姿勢は、腕を真横に伸ばし、かつ関節２、４、６の軸が紙面に垂直な状態とし、そのときの角度を０とする。各リンクはオフセットなしで軸に取り付けられており、基準姿勢においては関節１、３、５の回転軸が一直線になるものとする。

このような機構のマニピュレータにおいては、少なくとも図４や図５のような特異点姿勢が存在する。図４はθ２＝０、図５はθ４＝０の場合である。他にはθ２＝±π、θ４＝±πなども特異点姿勢であり、それらは腕を折り曲げた姿勢になる。いずれの場合も、関節１、３、５のうちの２つ以上の回転軸が一直線上に並び、自由度の縮退が起こるため、先端位置姿勢６自由度全てを決めることができなくなる。よって、ステップ２０３において、detＪを計算する前にθ２≒０、θ４≒０か否かをチェックすれば、計算時間を減らすことができる。

一般のマニピュレータにおいては、関節空間を小空間に区切り、動作中にはdetＪによって特異点への接近判定を行うとともに、常に関節空間中のどの小空間にあるかを監視する。特異点に接近したと判断された場合は、その関節角度の組が存在する小空間にフラグを立てる。こうすることで、次にその小空間に関節角度の組が入ったときには、detＪを計算することなく特異点への接近を判断できる。

小空間の数は表現によるが、例えば、まず各関節の可動範囲ｚｉをｎ分割し、ｎの６乗個の小空間に分け、動作中に特異点接近を検出したらθｉをｚｉ／ｎで除した商をもってインデックスとし、６個のインデックスで指定されるその小空間にフラグを立てる。１つの小空間を１バイトで表現し分割数ｎを１０程度に取ると、必要なマップのサイズは１０の６乗＝１［ＭＢ］であり、大した分量ではない。ｎを大きく取ればこれだけでも十分使用に耐えるが、マップの大部分は使われないので、メモリ節約のためにはｎを小さく取り、その代わりフラグの立った小空間に関してのみ再度分割を行い、これを再帰的に繰り返すことで、必要な領域においてのみ局所的に分解能を上げつつメモリの節約を果たすこともできる。分割数をｎ＝２、４、８にすれば二分木、四分木、八分木構造で管理できる。これらの構造であれば、探索等に関してさまざまな効率的手法が研究されており、それらを利用可能であるという利点がある。

次に、切り替える制御手段の例を図６〜図９を使って説明する。

図６に作業空間基準のコンプライアンス制御系の一例を示す。この制御系は特異点に接近していないときに適用する。図６において、６０１は外力・モーメント、６０２は目標位置姿勢、６０３は位置姿勢応答、６０４は関節角応答、６０５は関節トルクもしくは推力、６０６は関節角速度指令、６０７は先端速度・角速度指令、６０８は先端加速度・角加速度指令、６０９は仮想慣性の逆行列、６１０は仮想粘性、６１１は仮想剛性、６１２は関節角から先端位置姿勢への変換部、６１３はヤコビアン行列の逆行列、６１４は積分要素、６１５はロボット制御系、６１６はロボット機構をそれぞれ表す。６０１〜６０８は６次元ベクトル、６０９〜６１１は６ｘ６行列である。

制御則は以下の式で表される。

数式１２において、各要素６０９〜６１１を指定することで所望の運動特性を実現される。実装は離散値系で近似的に行われることが多いので、数式１２の積分を後退差分法や双一次変換法などで置き換え、ｋ回目のサンプル時刻をｔｋで表し、遅延オペレータｚの次数だけ添字ｋをずらして整理する。後退差分法で整理した一例を数式１３に示す。この場合は左右のｄＸｒ／ｄｔの時刻を同じと考えて整理してある。

数式１３の中のＩは単位行列である。なお、広義のコンプライアンス制御には多くのバリエーションがあり、上記はその一例にすぎない。また、同じ制御則であっても、実装方法は一通りとは限らないのは言うまでもない。

次に、数式１３の制御則によって導かれた目標値ｄＸｒ／ｄｔにヤコビアン行列Ｊの逆行列６１３を乗じることで、各関節速度目標値ｄΘｒ／ｄｔが得られる。ロボット制御系６１５は、この目標値とロボット機構６１６から出力される関節角度応答Θをもとに、関節の速度制御系もしくは位置制御系を構成する。ここに用いられる制御則としては、位置または速度を追従させるものであればどのようなものでもよく、ここでは方法を特定しない。速度制御系であればロボット機構からの角度応答を微分して用いるか、もしくはタコジェネレータ等で位置とは別途速度を検出する方法を用いてもよい。演算により微分を近似するときは、使用領域より高いカットオフ周波数をもつ１次以上のフィルタを分母に加えて要素全体をプロパな系にして用いる。位置制御系ならば角度目標値６０６を積分して用いればよい。ロボット制御系６１５は最終的に各関節トルク６０５を出力し、これによってロボット機構６１６が動作する。

なお、外力・モーメント６０１はロボット機構の中に備えられた力センサ３１５からの情報でも、仮想的に設定された値でもよい。前者であれば通常のコンプライアンス制御、後者であれば例えばマスタ・スレーブ制御の際に、物理的障害物の存在しない作業空間の領域に適当な粘弾性特性を有した仮想障壁を設定することなどに利用できる。

図７〜図９はＪの逆行列を用いない制御の例である。すなわち、特異点に接近している場合に適用する。以下にこれらを説明する。

図７は各関節でコンプライアンス制御を行う場合のブロック図である。図７において、７０１は外乱トルク、７０２は目標関節角、７０３は関節角応答、７０４は関節出力トルクもしくは推力、７０５は関節角速度指令、７０６は関節角加速度指令、７０７は仮想慣性の逆数、７０８は仮想粘性、７０９は仮想剛性、７１０は積分要素、７１１はロボット制御系、７１２はロボット機構をそれぞれ表す。７０１〜７０９はいずれもスカラである。この場合の制御則は数式１４で示される。

これを一例として後退差分法で離散化すると数式１５のようになる。

図８は一般にスティフネス制御と称する制御方式の一例である。これもＪの逆行列を必要としないので、特異点近傍における逆行列演算誤差が問題になることはない。図８において、８０１は作業座標基準の目標位置・姿勢、８０２は位置・姿勢応答、８０３は関節角応答、８０４は作業座標基準の目標速度・角速度、８０５は速度・角速度応答、８０６は関節角の角速度応答、８０７は作業座標基準の発生力・トルク目標値、８０８は発生力・トルクの応答、８０９は各関節に与えるトルク、８１０は剛性係数、８１１は粘性係数、８１２は力フィードバック係数、８１３は関節角Θから先端位置姿勢Ｘへの変換記述、８１４はヤコビアン行列、８１５はヤコビアン行列の転置、８１６はロボット機構、８１７は該機構に装備されている力センサである。８０１〜８０９はベクトル、８１０〜８１２は行列である。

制御則は数式１６で記述される。

関節に発生すべきトルクと発生力との関係は数式１６の１段目のようになる。これに対して運動特性を規定するのが２段目の式である。これは作業座標基準での位置・姿勢偏差および速度・角速度偏差に対するフィードバックと等価である。これによって剛性と粘性を制御する。３段目の右辺第二項は力を正確に発生するためのフィードバック項である。各関節のトルクが正確に発生できる場合は係数行列Ｋｆは０（零行列）でよい。その場合、Ｆｓ＝Ｆとなり、当該制御系では位置のＰＤ制御を行っているのと同じになる。剛性係数８１０、粘性係数８１１は固定でもよいが、ロボットの状態や作業の内容に応じて適宜変更することで適応性が高まる。係数の設定変更に関してはここでは詳述しない。

図９は各関節でスティフネス制御を行う制御系の一例である。これは関節毎の制御であり、ヤコビアン行列自体を必要とせず、特異点という概念もない。図９において、９０１は関節角基準の目標角度、９０２は関節角応答、９０３は関節角基準の目標角速度、９０４は角速度応答、９０５は関節剛性・粘性を与えるトルク指令、９０６は関節の発生トルク、９０７はトルク目標値、９０８は剛性係数、９０９は粘性係数、９１０は力フィードバック係数、９１１はロボット関節機構、９１２は該機構に装備されている関節トルクセンサである。９０１〜９１０はスカラである。

制御則は数式１７で記述される。

関節の運動特性を規定するのが数式１７の１段目の式である。これは関節角基準での角度偏差および角速度偏差に対するフィードバックと等価である。これによって関節の剛性と粘性を制御する。２段目の右辺第二項はトルクを正確に発生するためのフィードバック項である。各関節のトルクが正確に発生できる場合、係数ｋｆは０でよい。その場合、τｓ＝τとなり、当該制御系では関節のＰＤ制御を行っているのと同じになる。剛性係数９０８、粘性係数９０９は固定でもよいが、ロボットの状態や作業の内容に応じて適宜変更することで適応性が高まる。係数の設定変更に関しては作業座標基準における制御と同様ここでは詳述しない。

以上に示した制御系はいずれも一例にすぎないが、特異点への接近度を評価して図６と図７〜図９との間で切り替えを行うことで、作業空間と関節空間との対応感度を表現した行列（ヤコビアン行列）の逆行列を制御に用いることに起因した問題を回避することができ、特異点へ接近または通過しながら関節の可動範囲全域を動作に用いることができる。

本発明の作業用マニピュレータの制御装置の構成図である。 図１の制御装置の処理手順を示すフローチャートである。 図１の作業用マニピュレータのマニピュレータ機構を示す図である。 マニピュレータ機構の自明な特異点姿勢の例を示す図である。 マニピュレータ機構の自明な特異点姿勢の他の例を示す図である。 図１の作業用マニピュレータの作業座標基準でのコンプライアンス制御系の例を示す図である。 図１の作業用マニピュレータの関節角度基準でのコンプライアンス制御系の例を示す図である。 図１の作業用マニピュレータの作業座標基準でのスティフネス制御系の例を示す図である。 図１の作業用マニピュレータの関節角度基準でのスティフネス制御系の例を示す図である。

符号の説明

１０１…マニピュレータ動作目標値生成手段、１０２…伝達される各種動作目標値、１０３…関節空間−作業空間対応の感度パラメータ計算手段、１０４…１０２とそれに加えて１０３で計算された感度パラメータ、１０５…対応感度計算手段、１０６…１０４とそれに加えて１０５で計算された対応感度、１０７…制御方式切替手段、１０８…いずれかの制御方式に１０６と同じ情報を伝達することを示す矢印、１０９、１１０、１１１、１１２、１１３…制御方式１〜制御方式ｎ、１１４、１１５、１１６、１１７、１１８…１０９〜１１３から出力された制御量、１１９…マニピュレータ機構、１２０…マニピュレータに働く外力および動作応答の情報、２０１…作業空間での先端目標位置姿勢および目標速度・角速度が生成されるステップ、２０２…現在の関節角ベクトルからヤコビアン行列Ｊと先端速度・角速度応答を計算するステップ、２０３…対応感度を計算するステップ、２０４…計算された対応感度と設定された閾値とを比較するステップ、２０５…Ｊとその転置ＪＴを用いて作業空間での位置姿勢および速度・角速度を指令とするスティフネス制御を行うステップ、２０６…Ｊから転置行列と逆行列を導き、目標位置姿勢、外力等を用いてコンプライアンス制御を行うステップ、３０１…リンク０（土台リンク）、３０２…関節１、３０３…リンク１、３０４…関節２、３０５…リンク２、３０６…関節３、３０７…リンク３、３０８…関節４、３０９…リンク４、３１０…関節５、３１１…リンク５、３１２…関節６、３１３…リンク６（先端リンク）、３１４…接触センサ、３１５…力センサ、６０１…外力・モーメント、６０２…目標位置姿勢、６０３…位置姿勢応答、６０４…関節角応答、６０５…関節トルクもしくは推力、６０６…関節角速度指令、６０７…先端速度・角速度指令、６０８…先端加速度・角加速度指令、６０９…仮想慣性の逆行列、６１０…仮想粘性、６１１…仮想剛性、６１２…関節角から先端位置姿勢への変換部、６１３…ヤコビアン行列の逆行列、６１４…積分要素、６１５…ロボット制御系、６１６…ロボット機構、７０１…外乱トルク、７０２…目標関節角、７０３…関節角応答、７０４…関節出力トルクもしく…推力、７０５…関節角速度指令、７０６…関節角加速度指令、７０７…仮想慣性の逆数、７０８…仮想粘性、７０９…仮想剛性、７１０…積分要素、７１１…ロボット制御系、７１２…ロボット機構、８０１…作業座標基準の目標位置・姿勢、８０２…位置・姿勢応答、８０３…関節角応答、８０４…作業座標基準の目標速度・角速度、８０５…速度・角速度応答、８０６…関節角の角速度応答、８０７…作業座標基準の発生力・トルク目標値、８０８…発生力・トルクの応答、８０９…各関節に与えるトルク、８１０…剛性係数、８１１…粘性係数、８１２…力フィードバック係数、８１３…関節角から先端位置姿勢への変換記述、８１４…ヤコビアン行列、８１５…ヤコビアン行列の転置、８１６…ロボット機構、８１７…力センサ、９０１…関節角基準の目標角度、９０２…関節角応答、９０３…関節角基準の目標角速度、９０４…角速度応答、９０５…関節剛性・粘性を与えるトルク指令、９０６…関節の発生トルク、９０７…トルク目標値、９０８…剛性係数、９０９…粘性係数、９１０…力フィードバック係数、９１１…ロボット関節機構、９１２…関節トルクセンサ。

Claims (4)

1. 作業空間基準もしくは関節空間基準の目標変位およびその変位微係数・目標発生力を指令値として生成する目標値生成手段と、
   関節空間−作業空間対応の感度パラメータを計算する感度パラメータ計算手段と、
   対応感度を計算する対応感度計算手段と、
   複数の制御手段と、
   前記複数の制御手段を切り替える制御方式切替手段と、
   作業空間基準もしくは関節空間基準の外力検出手段とを備えた作業用マニピュレータの制御装置であって、
   前記感度パラメータ計算手段は、マニピュレータ機構からの関節変位応答を用いて感度パラメータを計算し、前記対応感度計算手段は前記感度パラメータ計算手段で計算された感度パラメータを用いて対応感度を計算し、
   前記制御方式切替手段は前記対応感度計算手段で計算された対応感度に基づいて制御手段の切り替えを行い、
   前記各制御手段は、前記目標値生成手段で生成された目標変位および目標発生力と、前記マニピュレータ機構からの関節変位応答と、前記外力検出手段で検出された外力と、前記感度パラメータ計算手段で計算された感度パラメータとを用いて制御処理を行うこと、
   を特徴とする作業用マニピュレータの制御装置。
2. 請求項１において、前記指令として生成する変位微係数の階数の最大値を２とすることを特徴とする作業用マニピュレータの制御装置。
3. 請求項１において、前記感度パラメータをヤコビアン行列で表現することを特徴とする作業用マニピュレータの制御装置。
4. 請求項３において、前記感度パラメータを前記ヤコビアン行列の行列式、もしくは該ヤコビアン行列の転置行列と該ヤコビアン行列との積の行列式で表現することを特徴とする作業用マニピュレータの制御装置。

Images