JP2960233B2 - ロボットの制御装置 - Google Patents

Description

【発明の詳細な説明】

【０００１】

【産業上の利用分野】本発明は、たとえば産業用ロボッ
トなどのアームを制御するための装置に関する。

【０００２】

【従来の技術】従来からロボットアーム手先の作業端の
力／トルクの方向と値を制御するいわゆる位置／力ハイ
ブリッド制御方式と、ロボットアーム手先の作業端の抵
抗を制御するいわゆるインピーダンス制御方式または仮
想コンプライアンス制御方式とがあるけれども、このよ
うな各先行技術では、作業空間座標系で組まれた制御系
では、その制御系を最初に組むときに選んだロボットア
ーム固有のベース座標系でしか記述されない。したがっ
て任意の座標系についての力制御ならびにインピーダン
スおよびコンプライアンス制御を行うことができない。

【０００３】

【発明が解決しようとする課題】本発明の目的は、制御
系を最初に組むときに選んだ座標系であるロボットアー
ム固有のベース座標系から、設定したい希望する座標系
であるワーク座標系への座標変換を容易に行うことが可
能なロボットの制御装置を提供することである。

【０００４】

【課題を解決するための手段】本発明は、アーム手先の
作業端３によって作業を行う複数軸ＪＴ１〜ＪＴ６のロ
ボット２と、各軸のロボットアーム固有のベース座標系
⁰Σでの関節角速度θ１を検出する関節角速度検出手段
１５と、関節角速度検出手段１５の出力に応答し、作業
端３の関心点のヤコビ行列Ｊの演算を行い、現在の速度
ｘ１の出力を導出する第１演算器１７と、ベース座標系
⁰Σの目標速度ｘ_d１と、ベース座標系⁰Σの目標の位置
ｘ_dと、ワーク６０の姿勢に応じて設定されるワーク座
標系^WΣの目標の力／トルクｆ_dと、ベース座標系⁰Σの
関節角速度の目標値θ_d１とを表す出力を導出する目標
値設定回路１９と、第１演算器１７の出力ｘ１と、目標
値設定回路１９からの目標速度ｘ_d１とに応答し、出力
（ｘ_d１−ｘ１）を導出する第１減算器１８と、第１減
算器１８の出力（ｘ_d１−ｘ１）を、ワーク座標系^WΣの
値に変換する変換行列Ｃ_W0を有する第１変換行列演算器
６６と、第１変換行列演算器６６の出力（ｘ_d１−ｘ
１）に応答し、目標粘性行列Ｄ_dの演算を行い、出力Ｄ_d
（ｘ_d１−ｘ１）を導出する第２演算器２２と、ベース
座標系⁰Σでの作業端３の現在位置ｘを検出する現在位
置検出手段２５と、目標値設定回路１９からの目標の位
置ｘ_dと、現在位置検出手段２５からの現在位置ｘとに
応答し、出力（ｘ_d−ｘ）を導出する第２減算器２７
と、第２減算器２７の出力（ｘ_d−ｘ）を、ワーク座標
系^WΣの値に変換する変換行列Ｃ_W0を有する第２変換行
列演算器６７と、第２減算器２７の出力（ｘ_d−ｘ）に
応答し、目標剛性行列Ｋ_dの演算を行い、出力Ｋ_d（ｘ_d
−ｘ）を導出する第３演算器２９と、ベース座標系⁰Σ
での作業端３の力／トルクｆを検出する力／トルク検出
手段３１と、力／トルク検出手段３１の出力ｆを、ワー
ク座標系^WΣの値に変換する変換行列Ｃ_W0を有する第３
変換行列演算器６５と、目標値設定回路１９からの目標
の力／トルクｆ_dと、第３変換行列演算器６５からの力
／トルクｆとに応答し、出力（ｆ−ｆ_d）を導出する第
３減算器３３と、第３減算器３３の出力に、力フィード
バックゲインＫ_fを掛け算して出力Ｋ_f（ｆ−ｆ_d）を導
出する力制御補償器３７と、第３変換行列演算器６５の
出力ｆと、力制御補償器３７の出力Ｋ_f（ｆ−ｆ_d）とに
応答し、出力｛Ｋ_f（ｆ−ｆ_d）−ｆ｝を導出する第４減
算器３４と、第４減算器３４の出力｛Ｋ_f（ｆ−ｆ_d）−
ｆ｝に応答し、Ｓを対角スイッチ行列とし、Ｅを単位行
列とするとき、出力（Ｅ−Ｓ）｛Ｋ_f（ｆ−ｆ_d）−ｆ｝
を導出する第１行列演算器７７と、第２演算器２２の出
力Ｄ_d（ｘ_d１−ｘ１）と、第３演算器２９の出力Ｋ
_d（ｘ_d−ｘ）と、第１行列演算器７７の出力（Ｅ−Ｓ）
｛Ｋ_f（ｆ−ｆ_d）−ｆ｝とを加算する第１加算器２４
と、第１加算器２４の出力［Ｄ_d（ｘ_d１−ｘ１）＋Ｋ_d
（ｘ_d−ｘ）＋（Ｅ−Ｓ）｛Ｋ_f（ｆ−ｆ_d）−ｆ｝］
を、ベース座標系⁰Σの値に変換する変換行列Ｃ_0Wを有
する第４変換行列演算器６８と、第４変換行列演算器６
８からの出力［Ｄ_d（ｘ_d１−ｘ１）＋Ｋ_d（ｘ_d−ｘ）＋
（Ｅ−Ｓ）｛Ｋ_f（ｆ−ｆ_d）−ｆ｝］に応答し、作業端
３の関心点の転置ヤコビ行列^tＪを求めて出力^tＪ［Ｄ_d
（ｘ_d１−ｘ１）＋Ｋ_d（ｘ_d−ｘ）＋（Ｅ−Ｓ）｛Ｋ
_f（ｆ−ｆ_d）−ｆ｝］を導出する第２行列演算器３８
と、第２行列演算器３８の出力を、各関節のモータのト
ルクに換算する係数Ｋｑを掛け算して出力する第４演算
器３９と、関節角速度検出手段１５の出力θ１と、目標
値設定回路１９からの関節角速度の目標値θ_d１とに応
答し、出力（θ_d１−θ１）を導出する第５減算器４２
と、第５減算器４２の出力に、関節速度フィードバック
ゲインＫｖを掛け算して出力Ｋｖ（θ_d１−θ１）を導
出する第５演算器４３と、重力補償項Ｇ（θ）の信号を
導出する信号発生器４４と、第４演算器３９の出力と、
第５演算器４３の出力と、信号発生器４４の出力とを加
算し、関節トルクτ、 τ＝ ^tＪ［Ｄ_d（ｘ_d１−ｘ１）＋Ｋ_d（ｘ_d−ｘ） ＋(Ｅ−Ｓ)｛Ｋ_f（ｆ−ｆ_d）−ｆ｝]＋Ｇ(θ)＋Ｋ_v（θ_d１−θ１） を表す信号を導出して各軸を駆動するモータ４９に与え
る第２加算器４０とを含むことを特徴とするロボットの
制御装置である。

【０００５】

【作用】本発明に従えば、ロボットアーム固有のベース
座標系における現在位置ｘ、現在速度ｘ１および現在の
力／トルクｆを、ワークに適したワーク座標系の各値に
変換し、そのワーク座標系の少なくとも目標の力／トル
クｆ_dを用いて、ワーク座標系で演算し、その演算結果
をベース座標系に変換して力制御ならびにインピーダン
スおよびコンプライアンス制御などを行うことができる
ようになり、特にこのことは浮遊状態でロボットのベー
スが移動するときにおいても、時々刻々、座標変換を変
化させることによって作業空間でのたとえば希望するイ
ンピーダンスを保持することが可能となる。さらに本発
明に従えば、たとえば産業用ロボットなどのアームの各
関節に関節速度サーボ系を有し、所望の直交座標系にお
いて、ロボットアームの手先である作業端３の関心点の
方向剛性と方向粘性を設定することができ、さらに、所
望の方向の力・モーメントをも制御することが可能にな
る。このように本発明に従えば、ロボットのアーム手先
の作業端３で純粋に力を制御する方向と、抵抗を制御す
る方向とに分離して作業を行わせることが可能になる。
これによってロボットアームの取扱うことができる接触
作業範囲が大きく拡がる。

【０００６】

【実施例】図１は、本発明の一実施例の全体のブロック
図である。演算回路１は、図２に示される産業用ロボッ
ト２の各軸を制御し、そのロボットアーム手先の作業端
３によって、たとえば図３に示されるようにワークであ
るボルト４を把んで、物体５のねじ孔６に嵌め込む作業
を行う。このボルト４をねじ孔６に螺合する際には、ね
じ孔６の方向７には、低剛性の並進剛性を設定し、ボル
ト４を矢符８のように回転させる方向には、必要な目標
締付トルクを指令し、これによってボルト４をねじ孔７
に嵌め込む作業を達成する。ねじ孔６の方向７は、ｚ軸
に平行であり、力制御が行われ、回転方向８およびｚ軸
方向は、コンプライアンス制御が行われ、後述の目標剛
性Ｋ_dｘを低く設定し、これによって反力を小さくし、
ねじ孔６へのボルト４の挿入を容易にする。このように
力制御とコンプライアンス制御とを複合し、これによっ
て作業に応じた柔軟な対応を可能とする。

【０００７】ロボット２は、図２に示されるように床に
固定されたベース９と、このベース９に第１軸ＪＴ１ま
わりに角変位されるアーム１０、さらに第２および第３
の各軸ＪＴ２，ＪＴ３まわりに角変位されるアーム１
１，１２が設けられ、このアーム１２は第４軸ＪＴ４の
まわりに角変位可能であり、さらにアーム１３は第５軸
ＪＴ５まわりに角変位し、またこのアーム１３に固定さ
れている作業端３は第６軸ＪＴ６まわりに角変位可能で
ある。

【０００８】このようなロボットの応答制御は、数１を
実現することによって達成される。

【０００９】

【数１】 Ｍ_x・ｘ２＋Ｄ_d（ｘ１−ｘ_d１）＋Ｓ・Ｋ_d（ｘ−ｘ_d） ＝Ｓ・ｆ＋（Ｅ−Ｓ）Ｋ_f（ｆ−ｆ_d） ここでｘはロボットアーム手先作業端３である関心点の
位置、ｘ１は手先関心点の速度、ｘ２は手先関心点の加
速度である。

【００１０】Ｍ_x は作業座標系での実慣性行列、ｘ_d は
目標の位置、ｘ_d１は目標の速度、ｆ_dは目標の力／トル
クである。

【００１１】さらにＤ_d は目標粘性行列、Ｋ_d は目標剛
性行列、Ｋ_f は力フィードバックゲインである。

【００１２】ｆは外力である。

【００１３】Ｓは対角スイッチ行列、Ｅは単位行列であ
る。

【００１４】Ｓ＝Ｅのときには全方向にコンプライアン
ス制御が行われる。Ｓ＝１が設定される成分方向にコン
プライアンス制御が行われ、Ｓ＝０が設定される成分方
向には力制御が行われる。

【００１５】マニピュレータ、すなわち作業端３の作業
座標系でのダイナミクスは数２で示される。

【００１６】

【数２】Ｍ_x・ｘ２＋Ｈ_x（θ１，θ）＝ ^tＪ^-1τ＋ｆ ここでθは現在の関節角、θ１は現在の関節角速度、Ｈ
_x（θ１，θ）は遠心力、コリオリ力、重力および摩擦
力などの外乱を含む非線形項、Ｊはヤコビ行列であり、
^tＪ^-1はその転置逆行列であり、τは関節トルク入力で
ある。

【００１７】数１および数２から、Ｍ_x・ｘ２を消去し
て整理すると、数３が成立する。

【００１８】

【数３】 τ＝ ^tＪ［Ｄ_d （ｘ_d１−ｘ１）＋ ＳＫ_d（ｘ_d −ｘ） ＋（Ｅ−Ｓ）｛Ｋ_f（ｆ−ｆ_d）−ｆ｝＋Ｈ_x（θ１，θ）］ この数３は、ハイブリッド・コンプライアンス／力制御
の基礎式である。

【００１９】前述の外乱を含む非線形項の中で、本発明
の一実施例では、重力項Ｇ（θ）だけを補償する。また
応答改善のために各軸の速度サーボ系を追加し、Ｋ_v を
各軸速度ゲインとすると、数４が成立する。

【００２０】

【数４】 τ＝ ^tＪ［Ｄ_d（ｘ_d１−ｘ１）＋Ｋ_d（ｘ_d−ｘ） ＋(Ｅ−Ｓ)｛Ｋ_f（ｆ−ｆ_d）−ｆ｝]＋Ｇ(θ)＋Ｋ_v（θ_d１−θ１） 数４は、前述の図１に示される実施例によって実現され
る。

【００２１】産業用ロボット２の演算回路１では、産業
用ロボット２のロボットアーム固有のベース座標系⁰Σ
で構成されている。これに対して図２のワーク６０はた
とえばその平坦な表面６１に沿ってｘｙ平面を形成して
ワーク座標系^wΣを設定する。このようなワーク座標系^w
Σは、ワーク６０の姿勢に応じて適切に設定される。こ
のワーク６０には、その表面６１に垂直な軸線を有する
ねじ孔または凹所６２が形成されており、この部分６２
に、産業用ロボット２によってボルトまたはピンなどが
着脱される。演算回路１では、ロボットアーム固有のベ
ース座標系における後述の各値をライン１６，２６，３
２から導出し、またライン４６からの信号に応答してベ
ース座標系における各軸の演算動作を制御する。目標値
設定回路１９では、ライン２０，２８，４１には、ロボ
ットアーム固有のベース座標系の目標速度ｘ_d１，目標
位置ｘ_dおよび目標角速度θ_d１をそれぞれ導出し、また
ライン３５には、ワーク座標系の目標の力／トルクｆ_d
を導出する。そこで本件実施例では、後述のようにライ
ン３２に変換行列演算器６５を設け、また同様にライン
２１，５２に変換行列演算器６６，６７を設ける。これ
らの変換行列演算器６５，６６，６７は、変換行列Ｃ_w0
を有し、入力されてくるロボットアーム固有のベース座
標系⁰Σの各信号を、ワークに適したワーク座標系^wΣの
各値に変換する変換行列を有する。さらにまたライン５
３には変換行列演算器６８が設けられる。この変換行列
演算器６８は、変換行列Ｃ_0wを有し、ワーク座標系^wΣ
の各値をベース座標系⁰Σに変換する働きをする。各軸
のロボットアーム固有のベース座標系⁰Σでの関節角速
度は検出手段１５によって検出され、その出力はライン
１６を介して演算器１７に与えられる。この演算器１７
は、作業座標系におけるアーム手先の関心点のヤコビ行
列Ｊの演算を行い、これによってライン１７ａには、現
在の速度ｘ１が導出されて減算器１８に与えられる。目
標値設定回路１９からライン２０には、ベース座標系⁰
Σでの目標速度ｘｄ１が導出されて減算器１８に与えら
れる。減算器１８の出力（ｘ_d１−ｘ１）はライン２１
から変換行列演算器６６によってワーク座標系^wΣの値
に変換され、演算器２２に与えられ、目標粘性行列Ｄ_d
の演算が行われ、その出力Ｄ_d（ｘ_d１−ｘ１）はライン
２３から加算回路２４に与えられる。

【００２２】検出手段２５はライン２６にアーム手先で
ある作業端３の現在位置ｘを表す信号を導出して減算器
２７に与える。目標値設定回路１９からライン２８に
は、目標の位置ｘ_dを表す信号が導出され、これによっ
て減算器２７の出力（ｘ_d −ｘ）はライン５２から変換
行列変換器６７によってワーク座標系^wΣの値に変換さ
れ、演算器２９に与えられる。演算器２９は、減算器２
７の出力と目標剛性行列Ｋ_dの積を求め、ライン３０か
ら加算器２４に、Ｋ_d（ｘ_d−ｘ）を導出する。

【００２３】検出手段３１は、ライン３２にアーム手先
である作業端３の検出された力／トルクｆを表す信号を
導出し、変換行列演算器６５によってワーク座標系^wΣ
の値に変換され、減算器３３，３４にそれぞれ与えられ
る。目標値設定回路１９は、ライン３５からワーク座標
系^wΣにおける目標の力／トルクｆ_dを表す信号を導出し
て減算器３３に与える。減算器３３はライン３６に、
（ｆ−ｆ_d）を表す信号を導出し、力制御補償器３７
は、力フィードバックゲインＫ_fを掛け算して減算器３
４に与える。減算器３４は、Ｋ_f（ｆ−ｆ_d）−ｆを演算
し、行列演算器３７に与える。行列演算器３７は、（Ｅ
−Ｓ）を減算器３４の出力に掛け算し、その出力を加算
器２４に与える。加算器２４の出力はライン５３を介し
て変換行列変換器６８によってベース座標系⁰Σの値に
変換され、その後、行列演算器３８に与えられる。この
行列演算器３８は、加算器２４の出力に応答し、作業座
標系におけるアーム手先である作業端３の関心点の転置
ヤコビ行列^tＪを求め、その出力を演算器３９に与え
る。演算器３９は、行列演算器３８の出力を、各関節の
モータの電流すなわちトルクに換算する係数Ｋｑを掛け
算し、その出力を加算器４０に与える。

【００２４】目標値設定回路１９からライン４１には、
関節角速度の目標値θ_d１を表す信号を導出して減算器
４２に与える。減算器４２は、（θ_d１−θ１）を演算
し、演算器４３に与える。演算器４３は、関節速度フィ
ードバックゲインＫ_vを掛け算し、その出力Ｋ_v（θ_d１
−θ１）を導出して加算器４０に与える。この加算器４
０には、重力補償項Ｇ（θ）の信号を導出する信号発生
器４４からの信号がライン４５を介して与えられる。こ
うして加算器４０からライン４６には、前述の数４で示
される関節トルク入力τを表す信号が与えられる。

【００２５】演算回路１において、ライン４６の関節ト
ルク入力τを表す信号は減算器４７に与えられ、増幅回
路４８から各軸を駆動するモータ４９に与えられる。モ
ータ４９によって駆動される各軸の位置／速度はエンコ
ーダ５０によって検出され、検出手段１５と検出手段２
５とに与えられる。このようにしてモータ４９によって
各軸のトルクが前記値τに設定される。

【００２６】このような実施例によれば、たとえば組立
て作業としてガイドピンを有する物体を、そのピンを台
盤などのワーク６０の凹所６２にならわせることによっ
て着脱する作業を行い、この場合の設定は、ワーク座標
系^wΣのｘｙ平面へは力制御、ｚ方向および回転方向は
コンプライアンス制御を行い、この挿入直前にミスアラ
イメントがあるとき、そのたとえばｘ方向に約１５ｍｍ
のミスアライメント偏差があっても、本件発明者の実験
結果によれば、エンドエフェクタ、すなわち作業端３に
柔軟なコンプライアンスを設定することによって、安定
な装着および取外し作業を容易に実現することができる
ことが確認された。したがって組立て作業に先立って適
当な位置決めさえできていれば、多少の誤差があって
も、その作業が可能であることが判る。これによって作
業用ロボットのティーチング作業を簡素化することがで
き、また作業端位置絶対精度を得にくい長尺アームや浮
遊ロボットアームの制御などに本発明が有効に実施され
る。

【００２７】目標粘性行列Ｄ_dおよび目標剛性行列Ｋ
_dは、ワーク座標系において可変である。

【００２８】本発明は産業用ロボットだけでなく、予め
定める特定の用途にのみ実施されるロボットなどに関連
して実施することもまた可能である。

【００２９】

【発明の効果】以上のように本発明によれば、制御系を
最初に組むときに選んだ座標系であるロボットアーム固
有のベース座標系から、位置／力ハイブリッド制御なら
びにインピーダンス制御およびコンプライアンス制御な
どの設定したい座標系であるワーク座標系への座標変換
を行うことによって、そのワーク座標系に適した座標系
を選んで最適な制御を行うことが容易に可能になる。ま
た本発明によれば、産業用ロボットなどのアーム手先作
業端で、純粋に力を制御する方向と、抵抗を制御する方
向とに分離して作業を行わせることが可能になり、これ
によってロボットアームの取扱うことができる接触作業
範囲が大きく拡がることになる。特に本発明では、現在
の各軸の角速度θ１を検出して目標角速度θ_d１となる
ように負帰還し、これによって各軸の速度サーボ系の追
加による応答改善を行うことができ、また各軸を目標速
度θ_dに保つための重力補償項Ｇ（θ）を加算するよう
にし、重力による誤差をなくす。こうして本発明によれ
ば、位置／力ハイブリッド制御とインピーダンス／コン
プライアンス制御とを組合せて柔軟な作業を可能とする
ロボットの制御装置を実現することができる。こうして
作業対象物体であるワークに対してアーム手先の作業端
３から加える力／トルクを、アーム手先の作業端３の変
位または速度に応じて可変としてインピーダンス／コン
プライアンス制御を行い、しかも手先座標軸方向別にそ
の特性を任意に選べる構成としてハイブリッド制御を行
うことを可能にし、前述の数４で示してある重力補償項
Ｇ（θ）を含む特性が改良された動作が実現される。イ
ンピーダンス／コンプライアンス制御は、単に位置、速
度、加速度、さらには力を単独に制御するのではなく、
それらの制御を組合わせてさらに発展させたものであ
り、作業端３に発生される力／トルクを作業端３の位置
ずれまたは速度の関数として指定するものであり、柔軟
な作業のために必要である。

【図面の簡単な説明】

【図１】本発明の一実施例の全体のブロック図である。

【図２】産業用ロボット２の簡略化した断面図である。

【図３】図１および図２に示される実施例によって物体
５のねじ孔６にボルト４を嵌め込む作業を示す断面図で
ある。

【符号の説明】

１ 演算回路 ２ 産業用ロボット ３ アーム手先作業端 ４ ボルト ５ 物体 １９ 目標値設定回路 ６５，６６，６７ ロボットアーム固有のベース座標系
からワークに適したワーク座標系への変換行列Ｃ_w0を行
う変換行列演算器 ６８ ワークに適したワーク座標系からロボットアーム
固有のベース座標系に変換する変換行列Ｃ_0wの変換行列
演算器

フロントページの続き (56)参考文献 特開 平３−184785（ＪＰ，Ａ) 特開 平２−167685（ＪＰ，Ａ) 特開 平５−143152（ＪＰ，Ａ) 特開 平５−96482（ＪＰ，Ａ) 特開 平５−6209（ＪＰ，Ａ) 特開 平４−159059（ＪＰ，Ａ) 特開 平４−609（ＪＰ，Ａ) 特開 昭62−63304（ＪＰ，Ａ) 特開 平４−343690（ＪＰ，Ａ) (58)調査した分野(Int.Cl.⁶，ＤＢ名) G05D 3/00 - 3/20 B25J 9/10 B25J 13/08 G05B 19/18 - 19/40 G05B 19/403 - 19/415 G05B 19/417 - 19/46

Claims (1)

(57)【特許請求の範囲】
1. 【請求項１】 アーム手先の作業端３によって作業を行
   う複数軸ＪＴ１〜ＪＴ６のロボット２と、 各軸のロボットアーム固有のベース座標系⁰Σでの関節
   角速度θ１を検出する関節角速度検出手段１５と、 関節角速度検出手段１５の出力に応答し、作業端３の関
   心点のヤコビ行列Ｊの演算を行い、現在の速度ｘ１の出
   力を導出する第１演算器１７と、 ベース座標系⁰Σの目標速度ｘ_d１と、ベース座標系⁰Σ
   の目標の位置ｘ_dと、ワーク６０の姿勢に応じて設定さ
   れるワーク座標系^WΣの目標の力／トルクｆ_dと、ベース
   座標系⁰Σの関節角速度の目標値θ_d１とを表す出力を導
   出する目標値設定回路１９と、 第１演算器１７の出力ｘ１と、目標値設定回路１９から
   の目標速度ｘ_d１とに応答し、出力（ｘ_d１−ｘ１）を導
   出する第１減算器１８と、 第１減算器１８の出力（ｘ_d１−ｘ１）を、ワーク座標
   系^WΣの値に変換する変換行列Ｃ_W0を有する第１変換行
   列演算器６６と、 第１変換行列演算器６６の出力（ｘ_d１−ｘ１）に応答
   し、目標粘性行列Ｄ_dの演算を行い、出力Ｄ_d（ｘ_d１−
   ｘ１）を導出する第２演算器２２と、 ベース座標系⁰Σでの作業端３の現在位置ｘを検出する
   現在位置検出手段２５と、 目標値設定回路１９からの目標の位置ｘ_dと、現在位置
   検出手段２５からの現在位置ｘとに応答し、出力（ｘ_d
   −ｘ）を導出する第２減算器２７と、 第２減算器２７の出力（ｘ_d−ｘ）を、ワーク座標系^WΣ
   の値に変換する変換行列Ｃ_W0を有する第２変換行列演算
   器６７と、 第２減算器２７の出力（ｘ_d−ｘ）に応答し、目標剛性
   行列Ｋ_dの演算を行い、出力Ｋ_d（ｘ_d−ｘ）を導出する
   第３演算器２９と、 ベース座標系⁰Σでの作業端３の力／トルクｆを検出す
   る力／トルク検出手段３１と、 力／トルク検出手段３１の出力ｆを、ワーク座標系^WΣ
   の値に変換する変換行列Ｃ_W0を有する第３変換行列演算
   器６５と、 目標値設定回路１９からの目標の力／トルクｆ_dと、第
   ３変換行列演算器６５からの力／トルクｆとに応答し、
   出力（ｆ−ｆ_d）を導出する第３減算器３３と、 第３減算器３３の出力に、力フィードバックゲインＫ_f
   を掛け算して出力Ｋ_f（ｆ−ｆ_d）を導出する力制御補償
   器３７と、 第３変換行列演算器６５の出力ｆと、力制御補償器３７
   の出力Ｋ_f（ｆ−ｆ_d）とに応答し、出力｛Ｋ_f（ｆ−
   ｆ_d）−ｆ｝を導出する第４減算器３４と、 第４減算器３４の出力｛Ｋ_f（ｆ−ｆ_d）−ｆ｝に応答
   し、Ｓを対角スイッチ行列とし、Ｅを単位行列とすると
   き、出力（Ｅ−Ｓ）｛Ｋ_f（ｆ−ｆ_d）−ｆ｝を導出する
   第１行列演算器７７と、 第２演算器２２の出力Ｄ_d（ｘ_d１−ｘ１）と、第３演算
   器２９の出力Ｋ_d（ｘ_d−ｘ）と、第１行列演算器７７の
   出力（Ｅ−Ｓ）｛Ｋ_f（ｆ−ｆ_d）−ｆ｝とを加算する第
   １加算器２４と、 第１加算器２４の出力［Ｄ_d（ｘ_d１−ｘ１）＋Ｋ_d（ｘ_d
   −ｘ）＋（Ｅ−Ｓ）｛Ｋ_f（ｆ−ｆ_d）−ｆ｝］を、ベー
   ス座標系⁰Σの値に変換する変換行列Ｃ_0Wを有する第４
   変換行列演算器６８と、 第４変換行列演算器６８からの出力［Ｄ_d（ｘ_d１−ｘ
   １）＋Ｋ_d（ｘ_d−ｘ）＋（Ｅ−Ｓ）｛Ｋ_f（ｆ−ｆ_d）−
   ｆ｝］に応答し、作業端３の関心点の転置ヤコビ行列^t
   Ｊを求めて出力^tＪ［Ｄ_d（ｘ_d１−ｘ１）＋Ｋ_d（ｘ_d−
   ｘ）＋（Ｅ−Ｓ）｛Ｋ_f（ｆ−ｆ_d）−ｆ｝］を導出する
   第２行列演算器３８と、 第２行列演算器３８の出力を、各関節のモータのトルク
   に換算する係数Ｋｑを掛け算して出力する第４演算器３
   ９と、 関節角速度検出手段１５の出力θ１と、目標値設定回路
   １９からの関節角速度の目標値θ_d１とに応答し、出力
   （θ_d１−θ１）を導出する第５減算器４２と、第５減算
   器４２の出力に、関節速度フィードバックゲインＫｖを
   掛け算して出力Ｋｖ（θ_d１−θ１）を導出する第５演
   算器４３と、 重力補償項Ｇ（θ）の信号を導出する信号発生器４４
   と、 第４演算器３９の出力と、第５演算器４３の出力と、信
   号発生器４４の出力とを加算し、関節トルクτ、 τ＝ ^tＪ［Ｄ_d（ｘ_d１−ｘ１）＋Ｋ_d（ｘ_d−ｘ） ＋(Ｅ−Ｓ)｛Ｋ_f（ｆ−ｆ_d）−ｆ｝]＋Ｇ(θ)＋Ｋ_v（θ_d１−θ１） を表す信号を導出して各軸を駆動するモータ４９に与え
   る第２加算器４０とを含むことを特徴とするロボットの
   制御装置。