JPS6075904A - 多関節ロボットの制御のための座標変換方法，演算方法，座標変換装置および演算装置 - Google Patents

Abstract

(57)【要約】本公報は電子出願前の出願データであるた
め要約のデータは記録されません。

Description

【発明の詳細な説明】 発明の分野 この発明は、多関節［１ホツトの制御方法おＪ：ひその
ための装置に関し、特に、位置情報についてだけではな
く、速度情報および加速度情報についても直角座標系と
関節座標系との間で座標変換を行ない、かつ、運動方程
式の逆問題を解くことによって予測制御をも行ない、ざ
らに力制御をも行なうことがでさる、多関節ロボットの
制御方法およびそのための装置に関する。

先行技術の説明 多関節ロボットは、据付面積が小さく大きな作業領域が
得られるという極めて優れた特徴を有りるものであるが
、その運動を制御づるにあたっては座標変換が必要であ
り、制御のＩζめの演梓時間、特に座標変換に要する時
間が長いという欠点がある。これをもう少し説明する。

まず座標系につき説明する。多関節１−１ポツトの制御
に用いる座標系には、表１に承りように、直角座標系（
互いに直角に交わった：、３本の軸に関づる成分で座標
が定まる）どそれに対する関′ｍ外標系（各関節の角度
によって外枠が定まる）とがあり、さらに、絶対座標系
（静止した１°］６標系）とそれに対するハンド座標系
（各ハン１−に固定されていてハンドの動きに従って動
く座標系〉とがある。

この明細書においては、直角庁拐（系ｔ↓、ＸＹＺ座標
系あるいはＸ座標系とも呼ばれている。また、関節座標
系は、角度座標系あるいはα座標系とも呼ばれている。

さらに、ハンド座標系は、局所座標系とも呼ばれている
。

表　１ 次に、座標変換の必要性につき説明する。たとえば産業
用ロボットで加工されるワークピースの加工線たとえば
アーク溶接の溶接線（Ｊ、一般に、互いに直角方向に延
ばされており、がっ、この加工線に治って連続して加工
づることが多い。多関節ロボッ１〜では、このようなワ
ークピースの加工線に沿ってたとえば溶接トーチなどの
被制御体（エンドエフェクタ）を移動させる際、このよ
うな直線を小区間に分け、それぞれの分割点の位置をめ
、その分割点の間を補間して、その位置へ被制御体が移
動するよう指令するのであるが、このような直線を直角
座標系で取扱うと１次式になるのに対し、関節座標系で
取扱うと非線形の複雑な式になってしまう。それゆえ、
被制御体の移動を指令するための位置情報の演算は、簡
単で有利な直角座標系で行なった後、その直角８標系で
の位置情報を関ｆｌｉ５座標系へ座標変換して各関節１
１１１を移動させるための指令情報とづる必要がある。

また、逆に、関節座標系にょるイ８′１間情報を直角座
標系の位置情報に変換り−ることし必要（・ある。要す
るに、多関節［１ホツトにＪ３いては、位置情報等を直
角座標系と関節座標系との間で相？ンに座標変換するこ
とが必要である。この座標変換、′１・１に、直角座標
系から関節座標系への座標変換には膨大な演算か必要で
あり、そのために多くの時間がかがる。

なお、この明細書にＪ５いて、関節とは回転あるいは旋
回する部分だけではイ１く、スライドする部分も含／υ
でいる。そして、そのようｂ関１１ｉ）は、輔あるいは
自由度と呼ぶ場合もある。

そこで、このような演算時間を短縮するために従来から
人別して２つの方式が提案されている。

その１つは、たとえば特開昭ξ５３−１００５６１号な
どに開示される、近似的座標変換制御方式である。この
方式では、厳密な座標変換式から成る近似式をめ、その
近似式に基づいて多関節ロボツ１−の各関節角を回動さ
せる。この方式によれば、たしかに演算時間の短縮は可
能である（ブれども、相当大きなエラー（たとえば７ｎ
ｖ）が生じ、厳密＜ｒ位置制御を必要とづ゛るものには
利用できないという欠点があった。他の１つは、たとえ
ば特開昭５３−１２１３６２号などに開示されているよ
うな、５ないし６自由度を有するものであってもすべて
の軸を同時に座標変換するのではなく、そのうらのたと
えば３自由度のみを厳密な座標変換式に基づいて同時に
制御するものである。この方式によれば、位置決め精度
は向上するものの、達成し得る機能ないし位置決めの態
様が当然少なくなるという問題がある。これらとは別に
、使用する演算素子をたとえばジョセフソン素子のよう
な高速のものにすることも考えられるが、実用化には程
遠くしかも高価になってしまう。それゆえ、第１（〔、
簡単でかつ高速度の演算が可能な座標変換方法およびそ
のた　、装置が望まれていた。

一方、従来からの位置Ｌｊ＋　Ｏｌｌはフィードバック
制御が基本であった。フィードバック制御では、目標イ
装置と現在位置とに差か生じて初め−（駆動力が発生ず
る。特に、ＩＩ＋ホットのアー１１自身の慣ｆｆＪツよ
びエンドエフェクタの慣性が大きいど、この−エンドエ
フェクタの「１標位置と現在位置との偏差が大きくなる
。この偏Ｚ：を最小とづべく、位置偏差に乗する荷重（
ノイードハックグイン）や駆動最大トルクを大きくとろ
うとじても、フィードバック制御の安定性の問題から限
界があった。これは特に２軸以上の制御のときに、エン
ド土フ１クタの径路のずれとなって現われ顕：８な問題
となっていた。これはアームの直線的運動に伴う慣ｆＪ
力ににるものであるが、この他にす、遠心力、コリオリ
の力など他の腕の回転的な動ぎにＪ、って６発」づる力
やアーム自身および］−ノドエフ１クタの質量によって
発生する重力によって［１ボツ１〜の実際の動ぎと所望
の動きとの間に差が発生し、特に高速かつ高精度が要求
される実施例においては重人な問題どなってきた。この
問題は、指令値と現在値との、つまり、制御入ノＪとし
ての目標位置と制御結果としての現在位ＩＰ！ｔとの因
果関係を運動方程式として前もって知り、所望の結果を
生むような制御パックを与えることによってのみ解決さ
れるもので、これをフィードフォワード制御、あるいは
予測制御あるいは最適制御と呼んでいる。それゆえ、第
２に、エンドエフェクタが所望のバスを移動するような
予測制御方法およびそのための装置が望まれていた。

さらに、位置のフィードバックによってのみ制御される
ロボッ１〜が何らかの障害物に対したときにロボットの
アームに発生する力は、そ゛の力の方向の位置偏差によ
って変わってしまうが、組立や面仕上げに類する応用に
おいては、成る方向には一定のツノで押、し付けそれに
垂直な方向には位置側９１１をする要求があった。また
、その方向は口１ポットの機構の設計上の都合で決まる
方向ではなく、被加工物によって決まる方向である。し
たがって、任意の被加工物で決まる座標系で位ｆｌｆｆ
ｆ制御と力制御の切換または荷重の選択を行な−）必要
がある。

それゆえに、第３に、そのための装置と被加工物で決ま
る座標系での力からロホット機構で決まる座標系での力
（１〜ルク）への変換の手段が望まれていた。さらに、
特に組立作）°毛にＪ３いて、挿入づる部品によって決
まる座標系での回転を含めたいくつかの方向のうちの成
る方向について、柔かい位置決め（偏芒が生しても反力
が小さいこと）をづることにより、部品の挿入作業が容
易になる現象があり、これを特別なエンド１フＪクタ上
のメツＪニズムで実現したものがＲＣＣ（Ｒｅｍｏｔｅ
　Ｃｅｎｔｅｒ　０ｆＣＯＩｎｐｌｉａｎＣｅ　）であ
る。３つの異なる形のＲＣＣ装置が、米国特許第４．、
ｏ９ｓ、ＯＯ，１号、第４，１５５，１６９号および１
９８０η４月１６日に出願された米国性Ｔ１出願第１４
０，７６８号中に開示されている。これらの米国特許お
にび特許出願は、ここに参考として組入れられている。

しかし、ＲＣＣを特別な］−ンド］フ１クタとしてでは
なく、アーム自身で実現する要求があった。

好ましい実施例の説明 この発明は、前述した第１ないし第３の要望を満たすこ
とを企図してＪ３す、そのために、この発明は、要約ず
れば、次のことを行なう方法およびそのための装置を含
む。

まず第１の要望に対しては、少なくとも専用のハードウ
ェアでヤコビアンＪ（α）をめ、かつ関節座標系でめ位
置テ！−タαがら直角座標系（・の位置データＸへの座
標変換の演算を行なう。ざらに好ましくは、専用のハー
ドウェアでヤコビアン−インバース［Ｊ（α）］−１を
め、かつ直角座標系での速度データｄＸから関節座標系
での速度データｄαへの座標変換の演算そのものをも行
なう。

次に第２の要望に対しては、運動方程式の逆問題を解き
、予測制御を行なう。運動方程式は関節座標系で書かれ
ているので、これを解くためには関節座標系での情報が
必要である。そのため、直角座標系での速度データｄマ
から関節座標系での速度データｄαへの変換および直角
座標系での加速度データｄ２父から関節座標系での加速
度データｄ２αへの変換を行なう。これらの変換Ｊ３よ
び運動方程式の逆問題を解くことも、専用のハードウェ
アで行なう。

さらに第３の要望に対しては、力のフィードバック制御
を行なう。そのため、直ｆｌ、１座標系でのツノデータ
Ｆから関節座標系でのトルクｊ゛−タτへの変換を行な
う。この変換も専用のハードウェアで行なう。

全体システムの」Ｌ 」−記処理のすべてを実施する全体システムの構成につ
き概略説明する。第１Ａ図および第１Ｂ図は、それぞれ
、多関節ロボットの制９１１装置の全体システムの構成
を概略的に示づブロック図である。

図において、一点鎖線より左側は直角座標系（−表わさ
れる■を取扱う部分であり、右側は関節座標系で表わさ
れる徂を取扱う部分である。

まず、第１Ａ図に示づシステムにつき説明づる。

ポストシステム４からは、時々刻々の直角座標系におけ
る指令値がマトリクスの形で出力される。

ホスＩ・システム４は、図示しないけれども、マイクロ
コンピュータを備える。ホストシステム４から出力され
る指令値には、大きく分（プて、パスの指令値と押イ」
【フカの指令値とがある。バスの指令値は、位置指令デ
ータＸ、速１良指令データｄＸＪ：ｉよび加速度指令デ
ータｄ２Ｘを含み、押付は力の指令値は、力指令データ
Ｆを含む。ホストシステム４からの位置指令データ×は
、後述の位置フィードバックデータが差引かれた後、ゲ
イン調整部１４でゲイン調整され、後述の速度指令デー
タに補正分として加算される。ポストシステム４からの
速度指令データｄＸは、ゲイン調整部１４からの補正分
と加算され、座標変換部６にＪ夕いて関節座標系に変換
され、運動方程式演算部９に与えられる。座標変換部６
からの速度指令データは、さらに、後述の速度フィード
バック信号が差引かれた後、ゲイン調整部１５でゲイン
１１１ｍされ、その結果が補正分として関節部１０に与
えられる。ポストシステム４からの加速度指令データｄ
２Ｘは、座標変換部７においてｒ３１１ｍｓ標系に変換
８れて運動方程式演算部９に与えられる。運動方程式演
算部９において（よ、与えられた速度データｄ５よび７
１０速度データに基づいて運動方程式の逆問題が解かれ
、各関節に与えられるべき１ヘルクｊ−−夕が出力され
、このデータが関節部１０１こ！ｊえられる。関節部１
０は、アンプおＪ：びモータを含む。関節部１０の位置
は、エンコータ１１によって検出され、■ンコータ１１
からは位置〕（−１〜ハツクデータが出力されこれが座
標変換部５にａ３いて直角座標系に変換され、前述した
とＪ３リホストシステム４からの位置指令データＸから
着引かれる。１同様に、関節部１０の速度はタコメータ
１２によって検出され、タコメータ１２はｊ士度フィー
ｉ〜ハックデータを出力し、これが前述したように座標
変換部６からの速度指令データに加算される。−力、ホ
ストシステム４からは、力指令データ［−が出力され、
これに後）ホするカフイー１〜パツクデータが差引かれ
て両者の偏差がめられ、ゲイン調整部１６においてゲイ
ン調整される。力の偏差ｄＦ−１はハンド座標系で表現
されているため、これが座標変換部１７にａ−３いて絶
対座標系に変換されてｄＦＸとなる。このｄＦｘがさら
に座標変換部８にＪ３いて関節座標系に変換されてｄτ
となり、補正分として関節部１０に与えられる。関節部
１０に生じる力はノコ検出器１３によって検出され、力
検出器１３は前述のカフィードバックデータを出力する
。

次に、第１Ｂ図に承りシステムにつき、第１Ａ図のシス
テムとの相違点を主に説明する。第１Ａ図のシステムに
おいては、位置フィードバックは、運動方程式演算部９
を介して関節部１０にかけられている。ところが、運動
方程式演算部℃）には複雑な要素が含まれているため、
この方式では、フィードバックの効き具合が明瞭でない
という欠点がある。これに対して、第１Ｂ図のシステム
にａ３いては、位置フィードバックは、運動方程式演算
部９を介さずに直接関節部１０にか【プられている。

この方式では、余分の座標変換部１８が必要になるもの
の、フィードバックの効き具合が明瞭になるという長所
がある。

第１Ａ図および第１Ｂ図に示したシステムの特徴は、簡
単に言えば次のとおりである。第１に、直角座標系と関
節座標系との間の座標変換を行なうＰ１〜標変換部５な
いし８を備λＣい＜）ことて−ある。

第２に、ホストシステム４から座標変換部６を経由して
運動方程式演算部９に至るフィードフォワードラインお
よびホス（へシステム４から座標変換部７を経由して運
動方程式演算部９に至るフィードフォワードラインを備
えていることである。第３に、位置のフィードバックが
直角Ｐ１８標系にＪ３いてかけられていることである。

従って、演算を行なうことが不可能に近い、直角座標系
て゛の位置データＸから関節座標系での位置データＩへ
の座標変換を行なう必要がない。さらに、速度のフィー
ドバックもか（プられている。第４に、ホス１゛システ
ム４から座標変換部８を経由して関節部１０に至る力の
フィードバック制御ラインを備えていることである。

以下、第１Ａ図および第１Ｂ図のうちで特徴のある各部
分を詳細に説明する。第２Δ図および第２Ｂ図は、全体
システムの構成を詳細に示フプ［１ツク図である。第２
Ａ図の右端は、第２１：３図の左端につながっている。

このブロック図は、第１Δ図８３よび第１Ｂ図に示した
座標変換部５ないし８Ｊ５　Ｊ：び運動方程式演算部９
に相当する。この第２Δ図および第２８図の説明をこれ
以降に４３いて詳細に行なう。

Ｌｌ」Ｊ丸立二」Ｕ組 まず、座標変換の一般論につき説明する。第３図は、多
関節１：ｌボッｌ−の模式図である。この第３図の例で
は、多関節ロボット（ユ６つの自由度あるいは関節角α
４．α２　、α。、α、　、α５　、α６を有する。絶
対座標系がＸＹＺで規定され、エンドエフェクタの局所
座標系くハンド座標系ンが×ｙ２で規定されている。

絶対座標系の位置ムク１〜ルＹとハンド座標系の位置ム
ク１ヘルｙとの関係（ま、その間の座標変換マトリクス
をＭで表わすと、次式（１）でちえられる。

Ｙ＝Ｍｘｙ・・・（１） 変換マトリクスＭは、次式（２）で与えられる。

（１１Ｌ、、Ｃ，＝ＣＯ３α、　、３．　＝Ｓｉｎα、
であり、立１９ｍ、およびｎ、ｌよ、各関節の座標系の
原点間の関係を示す。ここで、１＝１．・・・、６であ
る。

したがって、上記（１〉式は、次の（３）式で”与えら
れる。

→ Ｙ＝Ｍ＋　ＸＭｚ　ｘＭａ　ＸＭ４　ＸＭ５　ＸＭＧ　
Ｘ１ｌｌ・・・く３） 但し、Ｍ＋ないしＭｅｔま、台腕についての変換マトリ
クスを示す。

一方、ハンド座標系は、第４図に示すベタ１〜ルーテ ｒ−’　（Ｐｘ、ＰＹ　、Ｐ、）の点に位置している。

そして、絶対座標系の各軸の単位ハク１〜ルをｉ、ｊ。

１くとし、ハンド座標系の各軸の１１１侍ベク１ヘルを
１’−、ｊ’−、に−とすると、第４図におｔノるヘク
トリ　→　→　−２ Ａ　＝　Ｂ　＋　Ｐとづるとき、絶対座標系（Ｙ）とハ
ンド、ＰＩｉ標系（ｙ）の相互関係（よ、次式（５）で
与えられる。

・・・　（５） 但し、１　・Ｊ′は、ベクトルｉとベクトルｊ′との内
偵を意味する。

これらのことから、（１）式の座標変換マトリクスＭは
、次のように要約される。

（ｉ）Ｍは、それぞれの関節角α、　、α２　。

α０　、α２．α５およびα６の関数であるが、それぞ
れの腕についての変換マトリクスＭｔ　（ｒ＝１、・・
・、６）は、その軸の関節角α１だ（）の関数である。

（ｉｉ）　Ｐｘ　、ＰｙおよびＰ２は、ハンド座標系の
原点位置を示す。

（ｉｉｉ）Ｍのブロツクシトリクスｍ　（たとえば、（
５）式の点線で囲んだ部分）は、座標系の姿勢情報（方
向余弦）を持っていて、ｍ−１＝　ｍＴなる関係が成立
する。但し、右肩の添字−１Ｊ′３よび王は、それぞれ
逆マトリクスおよび転置マトリクスを示す。すなわち、
方向余弦に着目覆ると、座標系の回転を表わすブ１コツ
クマ１−リクスｎ１は、第５図に示す関係にあるので、
逆変換は、このブＤツクマトリクスを転置するだけでよ
いことがわかる（直交マトリクズ）。

エンドエフェクタの位置・姿勢を実現づるの９、艮各自
由度の回転角であるため、絶対座標系から各軸のα座標
系への変換が必要Ｃある。［１ボツ１〜のエンドエフェ
クタの姿勢・位置ベクトル（Ｘ）は、位置情報をＸ、Ｙ
、７とし、姿勢情報をｎＸ　＋ｎＹ、ｎｚとづると、 で表わづことができる。また、各軸の回転角度はま　Ｉ
こ で表わされる。したがって、前記位置情報および姿勢情
報は、それぞれ（７）式で表わされる。

この（７）式を要約すれば、（８）式が得られる。

Ｘ−Ｆ（α〉　・・・（８） このく８）式は、絶対座標系の姿勢位置ベクトルＸが各
関節角ベクトルαの関数であ゛ることを示している。

αからＸへの変 次に、−ベクトル了からベクトル又への座標変換の方法
およびそのための装置につき説明する。これは、第１Ａ
図および第１Ｂ図の座標変換部５に相当する。

（２）式かられかるように、Ｍ、は、αＩ　、９４１ｍ
、およびｎ、で表現されたントリクスである。

また、Ｍ２は、α２　、見２９ｍ２および１１□て表現
されてＪ５す、ＭｏないしＭ６ｂ同様である。したがっ
て、それらの積ＭＩ　ＸＭ２　ＸＭ３　ＸＭ４　ＸＭｓ
ＸＭ６、すなわちＭは、αｔ　、Ｃ，ｔ　、Ｉｌｌ、　
＋ｎ＋（ｉ＝１ないし６）を含んだマトリクスとなっ−
くいる。このうら、見＋＋ｍ１Ｊシよひｎｌは、各関節
間の距離、すなわち、台腕の長さに関係した１■であり
、ここでは、定数である。このため、Ｍの各要素は、変
数としてα＋（ｉ＝１ないし６）を含んだ１７）となっ
ている。このことがら、Ｍ　Ｉ；ｊ、Ｍ−「°汁（α霞
］　・・・（９） のように書くことができる。Ｍの、ｊｌ（要素ａ　ｃｋ
　（α１）は、具体的には（２）式のマトリクス乗算を
実行し、その結果として得られる第３行第に列の要素で
ある。ところで、このようにしてｉＥＦられるａ、４．
　（α、ンがどのような意味を持っているかを考える。

Ｍは、その定義（１）式から、空間中の任意の点のハン
ド座標系での位置ベクトルＶと、同じ点の絶対座標系で
の位置ベクトルＹとの関係を示づものである。そこで、
ハンド座標系での原点すなわらエンドエフェクタの先端
が絶対座標系でどのよ−うに表わされるかを貨える。こ
の点は、ハンド座標系ではＸ　＝Ｏ，Ｖ　＝Ｏ，ｚ　＝
Ｏであるｈ口ら、絶対座標系でのこの点の座ＬＪｊをＸ
。、ｙｏ、Ｚ。

とりると、（１）式より なる式が得られる。この式の右辺の乗算を実行すれば、 となる。したがって、 なる式か１１ノられる。すなわら、７１〜クスＭのａ１
４　く　α　１　）　＋ａｚ＋　（α　ｌ＋”０４　（
α　１　）　（まそれぞれエンドエフェクタの先端の絶
対月６標系における座標成分に対応していることになる
。そして、（１２）式の左辺は絶対座標系に４１５　Ｌ
ノる聞であり、右辺はα、（ｉ＝ｉないし６）によって
書かれていることから、α１を与えればＸ、、Ｙ。

、Ｚｏがわかることになる。エンドエフ１クタの姿勢情
報を表わすｍ　ｎＸ　Ｏ＋”１０　＋口ｌ、についても
同様にＭの中のブロックマトリクスｍに含まれる要素と
の関係が得られる。条間１１１）日ボ・ントの制御にお
いて重要な役割を演するのは、エンドエフ１クタの先端
の位置およびその姿勢であるから、（１２）式でＸ。、
Ｙ、、Ｚ、と沿え字Ｏを付けＣ占いたものを改めてｘ、
ｙ、ｚと書いて使用することとづば（’ｎｘｏ　Ｙｏ、
ｎ□。についても同、ｎ 様）、（１２）式等により なる関係があることになる。すなわら、（ｉ　）　α１
で書かれたＭｌの乗算を行なってＭをめ（このＭはα１
で書かれている）、（Ｉｉ）　Ｍの各要素のうらａ、。

（α、）、ａ４．（αｌ＋ａ１４　（αｌ＋”２Ｇ（α
１）。

・・・＋　ａ３４　（α１）を取出せば、これらはイれ
ぞれ、ｎＸ　＋ｎＹ　＊ｎＺ　＋・・・　、Ｚに対応し
でいるため、（１２）式等の関係式がＩ’ｌられ、 →　→ （ｉｉｉ）　（１２＞式等の関係式は、Ｘ　＜−２αの
関係を示づ式であるため、ｄとＸの相ケの変換式がＰＩ
られたことになる。づなわち、ｄからマへの変換を行な
うためには、前記（２）式の演緯、ずなわち、 ｌＶＩ＋　ＸＭｚ　ｘＭａ　ＸＭ４　ＸＭＳ、ＸＭ６’
＝Ｍ・・・（１７１，） なる演停を行なえばよい。ざらに操ｔ；ηれば、αから
Ｘへの変換は、ロボットの各自由度の動きを座標変換７
トクスＭ３として表現し、ぞの７１へりクスの自由度の
数（たとえば６自由度の場合、ｉ−１ないし６）だけの
積マトリクスＭをめればよい。

そこで、前記（１４）式の演紳を行なう装置につき説明
する。第６図は、１から又への座棺変換装置を説明する
ためのブロック図である。この図は、第２Ａ図に相当し
、点線′Ｃ囲んｌこ′部分が第１Δ図および第１Ｂ図に
示した座標変換部５である。

座標変換部５は、マトリクスシフトレジスタ２５と、そ
れに接続されたマトリクス乗算ｆｉ４１と、・でれに接
続されたマトリクスレジスタ４２と、それに接続された
マトリクスレジスタ４３およびバッファレジスタ４４と
を備える。マトリクスシフ（〜レジスタ２５は、自由度
の数１だりの、たとえば６自由度の場合６個のマトリク
スレジスタ２５１ないし２５６から構成されている。マ
トリクスレジスタ２５１ないし２５６は、それぞれ、各
自由度の動きを座標変換マトリクスとして保持づる。

マトリクス乗算器４１は、２つのマトリクスを入力とし
、その２つのマトリクスの積のマトリクスを出力する。

このマトリクス乗算器４１については後でざらに詳しく
説明する。マトリクスレジスタ４２は、マトリクス乗算
器４１での乗算結果を保持づる。

ここで、クロック信号につき説明する。座標変換部５に
タイミング回路（図示せず）から２種類のり１コック信
号φＡおよびφＢが与えられる。すなわち、マトリクス
レジスタ４２にはクロック信号ψＡが与えられ、マトリ
クスシフトレジスタ２５、マトリクス乗算器４１．７１
−リクスレジスタ７′Ｉ３およびバッフ７レジスタ４．
　／ｌに（Ｊり１］コック信φＢが与えられる。クロッ
ク信号φＡは、マトリクスの積をとるためのものであり
、細かく見れば６４個のパルスからなる。り［１ツク１
占号φＢは、レジスタをシフ１へさげるためのものであ
り、細かく見れば１６（固のパルスからイ蒙る。なお、
これＩうのクロックφΔおよびφＢは、以上と同様の考
え方で、第２Δ図および第２１３図に示したりへてのブ
ロックに人力される。Ｊなわら、ｖ／ｌ−リクスの積を
受【プるレジスタにはクロック信号φＡが入力され、そ
れ以外のブロックにはクロック信￥’′１ｄ）Ｂが入力
される。

次に、座標変換部５の動作につき説明する。ンイクロコ
ンピュータ（図示Ｕず）から、各マトリクスレジスタ２
５１ない１ノ２５６に変換ントリクスＭ、ないしＭＩｉ
がそれぞれ与えられる。図において、■は、マイクロコ
ンピュータからの入力を示している。つまり、マイクロ
コンピュータ内のり−ドオーンリメモリ内には、各自由
度の正弦および余弦をめるためのテーブルがあり、その
テーブルに基づいてソフトウェアによる補間が行なわれ
、これによって上述のようにマイクロコンビ−Ｌ−夕か
らマトリクスレジスタ２５１ないし２５６に変換マトリ
クスＭ、ないしＭ６が与えられる。

まず始めに、マトリクスレジスタ２５１内にはマトリク
スＭ、が保持されており、マトリクスレジスタ４．３は
単位マトリクスＥにイニシャライズされている。したが
って、マトリクス乗算器４１は、マトリクスＭ、と単位
マトリクス［との乗算を行ない、その結果であるマトリ
クスＭ、がマトリクスレジスタ４２内に保持される。第
６図は、この状態を示している。次に、前記クロック信
号によりマトリクスシフトレジスタ２５内の７１−リク
スは図において右の方向に１ブロツクだけシフトされ、
すなわち、マトリクスレジスタ２５１内にはマトリクス
Ｍ２がロードされ、さらにマトリクスレジスタ４３には
マトリクスレジスタ４２からのマトリクスＭ、がロード
され、そしてマトリクス乗算器４１においてマトリクス
レジスタ２５　ｌ　ｈｓらのマトリクスＭ２とマトリク
スレジスタ／１３からのマトリクスＭ、との乗算が行な
われ、その結果マトリクスＭＩＸＭ２がマトリクスレジ
スタ４２中にロードされる。以降バ］ｊ様の動作が繰返
され、所定の６クロツクタイムの後には、７１ヘリクス
レジスタ４２およびバッファレジスタ４４中には１ノト
リクスＭＩ　ＸＭ２　ＸＭ３　ＸＭ４　ＸＭＳ　ＸＭ６
が、すなわちマトリクスＭが［１−ドされる。こねによ
なお、バッファレジスタ４４（Ｊ必づ゛しし必要イｆも
のではなく、さらにマトリクスレジスタ４３の代りに最
小限１行（Ｊ、たけ１列）分のバラノア機能を７１〜リ
クスレジスタ４２中に臼めることあるいは７１〜リクス
乗算器４１に（＝Ｊ加することにより省略り゛ることが
できる。この場合、マトリクスレジスタ４２は、単位マ
トリクスＦにイーシトライスされるものとする。

以上のようなαからＸへの変換をンイクロコンピユータ
のソフトウェアのみで行なうとずれｌば。

その演算時間はたとえば５０〜１００ｍ５ｅｃ程度必要
とするが、同様の演算をこの実施例ように専用のハード
ウェアで実施すると非常に高速度で、たとえば１ｍ　ｓ
ｅｃ以下で行なうことができるという効果がある。

次に、前述したマトリクス乗算器４１の訂細につき説明
する。第７図は、マ］・リクス乗算器を説明するための
ブロック図である。このブロックの全体の運用でマトリ
クス乗算器の機能が達成される。第６図のマトリクス乗
算器４１は、そのような機能を達成づるものを便宜上１
つの７１−リクス乗算器４１として表わしたものである
。第２Δ図Ｊシよび第２Ｂ図に示し、後で説明する他の
マトリクス乗算器についても同様である。

４×４のマトリクスレジスタ８０および８１が乗算・加
算器８２の入力部に接続されており、東樟・加算器８２
の出力部には４×４のマトリクスレジスタ８３が接続さ
れている。このマトリクスレジスタ８０，８１おＪ：び
８３は、それぞれ、第６図のマトリクスレジスタ４３．
マトリクスジットレジスタ２５およびマトリクスレジス
タ４２に対応している。乗算・加算器８２は、乗算器８
２１と、それに接続された加算器８２２と、それに接続
されたバッフ７Ｉレジスタ８２３とを佑える。

マトリクスレジスタ８０からの乗算・加算器８２への人
ツノを×１とし、マトリクスレジスタ８１からの乗算・
加算器８２への入力をｙｌどし、乗算・加算器８２から
のマトリクスレジスタ８３　’＼の出ツノを７．とした
場合、乗算・加ぢ）器ε３２　ｆＪおい−Ｃは次の式に
示づ演緯が？ｊなわれる。

Ｚ、＝７．．．．＋Ｘ、Ｘｙ、・・・（’ｌ　５　）こ
こで、ｉは１乗算時間単（＜２のタイミンクを示し、１
−１ないし４である。このブ［ミンク図の装置は、２つ
の機能を有している。第１（よ、次の式に承りような７
１〜リクス′乗算の機能である。

［Ｘ］　Ｘ　［Ｙ］　＝　［２］ 第２は、マトリクスレジスタ８３の内容をマトリクスレ
ジスタ８０中に転送する機能である。

第８図は、マトリクスレジスタの一例を示すノ［ミンク
図である。この図は４×４のマトリクスレジスタについ
て示しているが、６×６のマトリクスレジスタについて
も同様である。ここでは、７１〜リクスレジスタ８ｏを
例にして説明する。４×４のシフトレジスタ８０２の人
力部および出力部に、それぞれ、ゲート８０１および８
０３が接続されている。さらに、ゲート８０’１おにび
８０３ならびにシフ１〜レジスタ８０２には、制御装置
８０４が接続されている。このマトリスレジスタ８０は
、次に示す４つの機能を有する。

（ｉ）　２つのモードを選択することが可能である。１
つは、シフトレジスタ８０２の各？？：累をシフトさけ
るシフトモードであり、このシフトは指定された１つの
行または列についてのみ行なわれる。伯の行または列は
巡回（［１−テート）する。

他の１つは、シフトレジスタ８０２の各要素を巡回さぼ
るローテーションモードである。このローテーションは
、全部の行または全部の列について行なわれる。

（＋１）　データを横方向にシフトまたは［１−デージ
ョンさＵる横シフトモードと、データを縦方向にシフト
またはローテーションざＵる縦シ“）Ｉ〜モードとを選
択づることができる。

（ｉｉｉ　）　シフ１へレジスタ８０２が外部からデー
タを受取る要素を指定することができる。すなわら、シ
フ１〜レジスタ８０２において、シフトする行または列
を指定することが７−さる（残りの部分は１」−デート
する）。

（ｉｖ）　シフトレジスタ８０２において外部へ）°−
夕を出力づる要素を指定づることがて−きる。

上記の４つの機能は、制υＩ装同８０４にょっ−Ｃ制御
される。

第９図は、制御装置とマトリクス乗算器との接続を示す
ブロック図である。マトリクスレジスタ８０．８１およ
び８３は、データラップ回路８４に接続されており、デ
ータラッチ回路８４は制御用リードオンリメモリ（ＲＯ
Ｍ＞８５に接続されている。制御用ＲＯＭ８５には、ア
ドレスカウンタ８６が接続されており、アドレスカウン
タ８６にはナントゲート８７が接続されている。

各マトリクスレジスタ８０．８１および８３において、
データラッチ回路８４からの信号により、前記４つの機
能の選択が行なわれる。制御用ＲＯＭ８５内には、各マ
トリクスレジスタ８０．８１＋１３よび８３を制御する
ための所定のデータが記憶されている。そのデータは、
アドレスカウンタ８６により指定されたアドレスから順
次読出される。

アドレスカウンタ８６の内容は、ホストシステム（図示
せず）からのクロック信号ＣＬ　Ｋにより更新される。

第１０図は、マトリクスレジスタの運用方法を説明７る
ための図である。この図と下に示す表２どを用いて、マ
トリクスレジスタがいかに運用され、それによってマ（
−リクス乗算が行なわれるかを説明する。

７Ｆ・リクスレジスタ８０．８１６よび８３のモードを
選択づる。りなわら、マトリクスレジスタ８０のモード
として、横モードおよびローテーションモードを選択す
る。マトリクスレジスタ８１のし一ドとして、縦モード
およびローテーションモードを選択Ｊる。マトリクスレ
ジスタ８３のモードとしで、横モードＪ３よびシフトモ
ードを選択する。

まず、マトリクスレジスタ８０の第４行の各要素と、マ
トリクスレジスタ８１の第４列の各要素との乗算が行な
われ、６積の和がマトリクスレジスタ８３の第４行第１
列に［１−ドされる。これは、前記（１５）式の演算を
行なうことにより達成される。次に、マトリクスレジス
タ８０の第４行の各要素とマトリクスレジスタ８１の第
３列の各要素との演算が前述と同様に行なわれる。その
演算結果は、マトリクスレジスタ８３の第４行第１列に
ロードされていた先のデータを第４行第２列にシフトす
るとともに、マトリクスレジスタ８３の第４行第１列に
ロードされる。以後同様にして、マトリクスレジスタ８
０の第４行の各要素とマトリクスレジスタ８１の第２列
あるいは第１列の各要素との演算が行なわれ、その演算
結果はマトリクスレジスタ８３の第４行にロードされる
。

次に、マトリクスレジスタ８０の第３９」の各君素とマ
トリクスレジスタ８１の各列の要素との演算が前述と同
様に行なわれ、その演算結果が？　ｌ−リクスレジスタ
８３の第３行にロードされる。以陪同様にしで、マトリ
クスレジスタ８０の第２行あるいは第１行の各要素どマ
トリクスレジスタ８１の各列との演算が行なわれ、その
演算結果がマトリクスレジスタ８３の第２行あるいは第
１行にロードされる。以上により次式のマトリクス乗算
がなされたことになる。

［Ｘ］　Ｘ　［Ｙ］　＝　［２］ Ｊ（ｆｆ＞のめ方 次に、ヤコどアンＪ（α）を専用ハードでめる方法およ
びそのための装ｙｔを説明づる。これは、第１Ａ図およ
び第１Ｂ図の座標変換部６の一部分に相当する。

前記く８）式は、αについての方程式であり、絶対座標
系の姿勢・位置ベクトル×と各関節角ベクトルαとの間
には、非線形の関係が存在する。

したがって、（８）式からＸ−）αの直接変換をめるこ
とは一般に複雑である。

そこで、この実施例では上記（８）式について→ 微小変化ｄＸおよびｄαに限定すれば、線形で取扱い１
りることに着目し、■コピアンＪ（α）を利用ザる。、
なＪ５、このヤコビアン（Ｊ　ａｃｏｂｉａｎ　）は、
たとえば、ＩＥＥＥ　ＴＲＡＮＳＡＣＴＩＯＮＳＯＮ　
ＭΔＮ−ＭΔＣ１−（ＩＮＥ　ＳＹＳＴｌ＝ＭＳ、ＶＯ
Ｌ、ＭＭＳ−１０，ＮＯ，２，ＪＵＮＥｌ　９６９　（
７）　Ｄ　ａｎｉｅｌ　Ｅ　、　Ｗ　ｈｉｔｎｅｙによ
る“Ｒｅ５ｏｌｖｅｄ　Ｍｏｔｉｏｎ　Ｒａｔｅ　Ｃｏ
ｎｔｒｏｌ　ｏｆ　Ｍａｎｉｐｕｌａｔｏｒｓ　ａｎｄ
　Ｈｕｍａｎ　ｐ　ｒｏｓｔｈｅｓｉｓ　”という記事
でよく知られている。ｄＸとｄαとの間の変換のヤコビ
アンは、（７）式の右辺を微小なｄαについてテーラ−
展開し、その最低次の項のみを残づことによって、次式
（１６）で表わされることがわかる。

・・・　（１６） ここで、 （ｌ　Ｘ＝Ｊ　（α）・ｄα　・・・（１７）であるか
ら、連立方程式を解けば、 ｄα−［Ｊ（α）］−１・ｄＸ　・・・（１８）が成立
する。この発明の実施例では、（１７）おにび（１８）
式を利用して、直角座標系からα系への座標変換を行な
う。上記２式は微小変化についてのものであり、したが
ってそのための演算は、少な（ともサーボレートで行な
う心数がある。

そこで、この実施例では、上記のようなアルゴリズムを
演算回路で実現せんとするものである。

前述の（３）式に戻って、ＹをαＩないしα６７ Ｂ４＝ＩＶＩ＋　ＸＶ２　’　ｘＭａ　ＸＶ４　ＸＶ５
　ＸＶ６　ＸＶ・・・（１９） 変換マトリクスを微分するということはその中の各要素
を微分することであり、したがって上記（１９）式のわ
Ｙ／、ｌα、は、（５）式を用いて次式（２０）式で表
わされる。

これを前述の（１６）式と対応させると、マタ、ａｆｑ
／ａａ＋　、２ｆｓ／ａｒｘ、およびＤｆ６１つα１は
、上記ブロックマ１〜リクスａｍ／δα１の中から適当
な３つを選択する。（１９）式％式％（２１） −１，２，・・・、６）を目算することによって１７コ
ビアンＪ（α）の各々の要素がまる。このようにしてめ
た要素を、前述の（１７）式に代入づることによって、
微小区間での座標変換が達成される。

この実施例では、前述の＼７コビアンマトリクスの各要
素をめるために、（１９）式は同じ演算過程の繰り返し
が多いことに着目して、マイクロコンピュータやマイク
ロプロセサとは別のｔなりち専用のハードウェアで演算
し、マイクロコンビコータないしマイクロプロセサはそ
のめた要素を代入して、最終的に座標変換を行なうよう
にする。したがって、マイクロプロセリないしマイクロ
コンビコータにａ５いて、座標変換のために必要な処理
が非常に少なくなる。したがって、高速の索子を用いる
必要がないばかりか、マイクロコンビコータやマイクロ
プロセサのＣＰＵに対して別の機能をイ１加することが
できる。従来のものでは、座標変換のＩＣめの演算時間
が非常に長かったので、ＣＩ）　（Ｊの処理速度が高速
でなｔ−１れば、あまり細かく内分補間することができ
なかった。また、他の機能を（＝ｌ加しようとすれば補
間点の演算時間を制限しなければならずそのためには補
間間隔を大きクツ゛ることが考えられたが、軌跡の正確
さに欠けるという欠点があった。これに対して、この実
施例のように座標変換のために必要な演算要素を別の演
算回路で行なうようにすれば、マイクロプロセサやマイ
クロコンピュータに高速のものが必要ではなくなり、あ
るいは他の機能を刊加することが簡単にできる。

さらに、このＪζうな座標変換の方法によれば、多関節
ロボットの構造の変更ｔｊ　にび自由度の増減に比較的
容易に対処できる。たとえはエンドエフェクタの種類に
よって要求される自由度は具なる場合があるが、このよ
うな回合でもこの実施例の装置によれば、マイクロコン
ピュータなどのソフトウェアを多少修正するだけでＪ：
いの（゛、汎用性が増強されることになる。

第１１図はこの実施例の概念を説明するためのブロック
図である。マイクロブｏ　ｔ　（ｊｌは演瞳回路２に接
続される。マイクロプロセリ”１から、演算回路２に、
前述の（１つ）式の台腕ごとの変換マトリクスＭ、〜Ｍ
ｒ、７３よびＭ１′〜Ｍ６’　が与えられ、演算回路２
では、この（１９）式に基づいて演算を行なう。演算回
路２は、バッファレジスタ８１〜Ｂ１．およびす、〜ｂ
＋１　を含み、乗ｎ器ＭＰ、〜ＭＰ、によって、これら
バッファレジスタにストアされたデータが乗算される。

乗算器ＭＰ１はバッファレジスタＢ５の内容とバッファ
レジスタＢ、の内容とを乗算し、その結果をバッファレ
ジスタＢ６に再びロードする。乗算器Ｍ　Ｐ　２はバッ
フルレジスタＢ６の内容とバッファレジスタｂ６の内容
とを乗算し、その結果をバッファレジスタｂ６にロード
する。乗算器ＭＰ３はバッファレジスタＢ７の内容とバ
ッファレジスタＢ８の内容とを乗算し、その結果を再び
バッファレジスタＢ７にロードする。そして乗算器ＭＰ
、はバッファレジスタＢ７の内容とバッフ７レジスタｂ
７の内容とを乗算し、その結果をバッファレジスタｂ６
にロードする。この演算回路２においては、２つの演算
過程を含み、その１つはデータの流れが第１１図におけ
る右方向であり、他の過程は逆方向のデータの流れを右
する。そして、それぞれの演算動作Ｊ５よび成るバッフ
ァレジスタから伯のバッファレジスタへのデータのシフ
トは、所定の１ユニツトタイムごとに行なわれる。バッ
ファレジスタＢ６およびＢ７は、バッファレジスタＢ５
からのデータを受け、バラフッ・レジスタ１３６はさら
にバッフ７レジスタＢ８からのデータを受ける。

バッファレジスタＢ、〜８１λとそれらに関連する乗算
器ＭＰ、およびＭＰ、とが（２０）式の変換マトリクス
Ｍ、〜Ｍ６の演算のために用いられ、バッファレジスタ
ｂ１〜Ｌｌ　ど乗算器Ｍ１〕２おＪ：びＭ　Ｐ　４　と
が（２０）式における微分された変換７１−リクスＭ＋
’〜Ｍ６′の演算のために利用される。

マイクロプロセリ１からのデータの入力が完了した状態
では、バッファレジスタＢ、、Ｂ２，３３１Ｂ角＋Ｂｓ
＋’ＢｌｊおよびＢ、に、それぞれ、変換マトリクスＭ
４１Ｍ２　、Ｍ５．Ｍ４．Ｍｓ　。

Ｍ６およびＭ６がロードされ、バッファレジスタｂ、、
ｂ２．ｂ、、ｂ４．ｂ９．ｂ６に、それぞれ、微分され
た変換マトリクスＭ　Ｉ　’　、　Ｍ２　’　＋Ｍ　ｓ
　’　＋　Ｍ４　’　＋　Ｍｓ　’　＋　Ｍｅ　’　が
［１−ドされている。次のユニットタイム間にバッフ）
ｌレジスタＢｓにロードされた変換マトリクスＭ５とバ
ッファレジスタＢ、にロードされた変換マトリクスＭ６
とが乗算器Ｍ　Ｐ　＋　ぐ乗算され、同時にバツファレ
ジスタｂ、にロードされている微分された変換７１−リ
クスＭ５’　とバッファレジスタＢ、にロードされでい
る変換マトリクスＭ６とが乗算器ＭＰ２によって乗算さ
れ、その結果Ｍ　ｓ　Ｘ　Ｍ　６およびＭ　ｓ　’　Ｘ
　Ｍ　６が、それぞれ、バラノアレジスタＢ６およびｂ
６にロードされる。また、この間に、各バッファレジス
タにロードされている変換マトリクス（微分されたもの
も含む）は、次の乗算に備えて、第１１図の右方向へ１
ブロツクシフトされている。そして、バラノアレジスタ
８７〜Ｂ１ｚにそれぞれ変換マトリクスＭ、〜Ｍ６がロ
ードされ、バラノアレジスタＢ６にはすべての変換マト
リクスの乗算結果すなわちＭＩ　ＸＭ２　ＸＭＳ　ＸＭ
４　Ｘ　Ｍ　５　ｘ　Ｍ　６がロードされ、バッフ７レ
ジスタｂ１１　・ｂＩＯｂ！３・ｂ８．ｂ、およびｂ６
に【よ、それぞれ、Ｍｅ　’　＊　Ｍｓ　’、Ｘ１ｙ４
６１　Ｍｓ　’　ＸＭｓ　ＸＭｅ　＋　Ｍ３　’　ＸＭ
４　ＸＭＳ　ＸＭＧ　＋　Ｍ２　’　ｘＭｓＸＭ４　Ｘ
ＭＳ　ＸＭＳおよびＭ１’　ＸＭ２　ｘＭａ　ＸＭ４　
ＸＭｓ　ｘＭ６がロードされている。そして、この時点
で第１の演算過程が終了する。

次の演算過程の流れは第１１図の左方向であり、乗算器
ＭＰ３およびＭＰ、によって処理される。

先の演算過程で第１１図の右側のバラノアレジスタに移
ったそれぞれのデータは、次の５５ユニットタイム間に
づ°べて、左側へ再びシフトされる。その結果、バッフ
ァレジスタ８１　、　ｂ’＋　、　ｂ２＋・・・ｂ６に
は、それぞれ、各マトリクスの積ずなわちＭＩ　ＸＭｚ
　ｘＭａ　ＸＭ４　ＸＭＳ　ＸＩＶＩ６．Ｍ１’　ＸＭ
２　ＸＭｓ　ＸＭ４　ＸＭＳ　ＸＭＧ　＊　ＭＩ　ＸＭ
２　’　ＸＭＳ　ＸＭ４　ＸＭＳ　ＸＭＳ　、・・・Ｍ
　＋　×Ｍ　２　Ｘ　Ｍ　ａ　ｘＭ４　ＸＭＳ　ＸＭＳ
　’がロードされることになる。

このようにして演算された（１９）式の要素はバッファ
レジスタｂ、からマイクロプロｔ７す１にこのマトリク
スは（２０）式にａ５いて［］で示したマトリクスであ
る。このマトリクスの及。

δＪ１ ６）式のヤコビアンＪ（α）の一部の要素に対応従って
上記ロードされた６つのマトリクス積のそれぞれから必
要な要素を取り出すことにより、（１６）式の１２コピ
アンＪ（α）のすべての要素→ が得られる。ヤコビアンＪ〈α）が得られれば、（１８
）式を用いて各関節角の変化量が得られる。

これらの処理がマイクロプロセッサ１によって行なわれ
る。

このように、ヤコビアンの各要素を計算するために別の
演算回路２を用いるようにすれば、演算回路２における
演算速度はマイクロプロセサ１のソフトウェア処理にに
る演尊速度に比べて非常に早いので、マイクロプロセ、
す１の負担ずべき演算時間が少なくて済み、ｄ７からｄ
ｌの変換が（ノーボレートで行なえる。また、このよう
に別のハードウェア構成による演算回路を設けるように
すれば、ロボットのｔ！６造の変更おＪ：び自由度の増
減に対して容易に対処できる。なぜならば、自由度の増
減に対処するにはハードウェア回路を増減するだけでよ
いからである。

なお、バッファレジスタＢ１から得られるデータＭＩ　
ＸＭ２　ＸＭＳ　ＸＭ４　ＸＭＳ　ＸＭＳは、前記〈１
）および（３）式の変換マトリクスであり、ＣＰＵはこ
のデータを用いて、ハンド座標系から絶対座標系への変
換も行なうことができる。

第１２図はこの実施例の概念を説明づ−るための伯のブ
ロック図である。この第１２図の回路は、第１１図の回
路よりも演算時間を短縮することができる。たとえばこ
の第１２図の回路によれば、同じようにヤコビアンマト
リクスの各要素を演算するについて、第１１図の回路に
比べて、回路規模を減少Ｊ゛ることがでさる。なＬ！’
　／、ｉらば、データ転送数を大幅に減らずことができ
るからである。

この第１２図に示ず演算回路２はバッファレジスタＢ、
〜Ｂ７およびす、〜ｂ６を含み、マイクロプロセサ１（
第１１図）はバッファレジスタＢ１とｂｌとに結合され
る。乗算回路Ｍ　Ｐ　＋は、バッファレジスタＢ５の内
容とバッファレジスタＢ６の内容とを乗算しその結果を
バッフ７レジスタＢＧに再びロードするとともに、バツ
フ７レジスタＢ、の内容とバッフ７レジスタＢ７の内容
とを乗算しその結果を再びバッファレジスタＢ７にロー
ドすることができる。同じように、乗算回路ＭＰ２は、
バッフ７レジスタＢ６の内容とバッファレジスタｂ５の
内容とを乗算してその結果をバッフ７レジスタｂ６にロ
ードするとともに、バッファレジスタＢ７の内容とバッ
ファレジスタｂ、の内容どを乗算し、その結果を同じ（
バッファレジスタｂ６にロードすることができる。

マイクロプロセサ（図示せず）からのデータ入力が完了
した状態では、バッフ７レジスタＢＩ。

８２　、Ｂ−、Ｂ−、Ｂｓ　、ＢＧおよびＢりには、そ
れぞれ、（１９）式の変換マトリクスＭ、　、　Ｍ２　
、　Ｍａ　、　Ｍ４　、　Ｍｓ　＋　Ｍａ　ａ３よびＭ
ａがロードされ、バッファレジスタｂ、〜ｂ６には、そ
れぞれ、微分された変換マトリクスＭ＋’〜ＭＧ’がロ
ードされる。次のユニットタイムでは、乗算回路ＭＰ、
によってバッファレジスタＢ！にロードされていたＭ５
とバッファレジスタＢ６にロードされていたＭａとが乗
算され、バッファレジスタ８ｃにはＩＶＩＳＸＭ６がロ
ードされる。同じように、乗算回路ＭＰ２によって、バ
ッファレジスタｂ６には、Ｍｓ’ＸＭ６がロードされる
。またこの間に、各ブロックのマトリクス（微分された
ものも含む）は次の乗算に備えて１ブロツクずつジノｉ
〜される。次のユニットタイムでは、バッファレジスタ
Ｂ６にＭ＜　ＸＭＳ　ＸＭ６がロードされ、バッファレ
ジスタｂ６にはＭ４　’　ＸＭａ　ｘＭｅがロードされ
る。このとき、バッファレジスタｂ１には、先にバッフ
ァレジスタｂ６にロー１〜されていたＭ５　’　ＸＭ　
６がロードされている。また、バッフルレジスタＢ７に
は最初にバッフフルレジスタＢ４にロードされていたＭ
４がロードされているであろう。以接このことが繰返さ
れて、最終的にはバッファレジスタ８電、Ｂ２．Ｂ３．
Ｂ、、ＢＳ、Ｂｒ　ｄ３よびＢ７には、それぞれ、Ｍ　
＋　Ｘ　Ｍ　２　Ｘ　Ｍ　ａＸＭ４　ＸＭＳ　Ｘ、Ｍａ
、Ｍ２　、Ｍｓ　、Ｍ＜　９Ｍｓ　。

ＭａおよびＭＩ　ＸＭ２　ＸＭ３　ＸＭ４　ＸＭＳ　Ｘ
Ｍ６がロードされる。一方、バッフアレシス、りす、。

ｂ２．ｂ、、ｂ、、ｂ、およびｂ６には、それぞれ、Ｍ
Ｉ　’　ＸＭ２　ＸＭ３　ＸＭ４　ＸＭｓ　ＸＭｓ　、
ＭＩ　ＸＭ２　’　ＸＭｓ　ＸＭ４　ＸＭＳ　ＸＭＳ　
、ＭＩ　ＸＭ２　ｘＭａ　’　ＸＭ４　ＸＭ５　ＸＭ６
　＋　ＭＩ　ＸＭ２　ＸＭｓ　ＸＭ４　’　ＸＭ５　Ｘ
Ｍ６　＋　ＭＩ　ＸＭ２　ＸＭａ　ＸＭ４　ＸＭｓ　’
　ｘＭｓおよびＭ　＋　Ｘ　Ｍ　２　Ｘ　Ｍ　ｓ　Ｘ　
Ｍ　４ＸＭｓ　ＸＭｒ　’がロードされる。そして、マ
イクロプロセサへはバッファレジスタｂ１がらデータが
転送される。

マイクロプロセサはこのようにしてめられたヤコビアン
Ｊ（α）のインバース［Ｊ（’＃）］−’をめ、ざらに
（１８）式を用いて各関節角の変化量をめる。

上記バッファレジスタＢ７に得られ１＝デ一タＭＩ　ｘ
ｌｙ１２ＸＭａ　ＸＭ４　ＸＭｓ　ＸＭ６を用いてハン
ド座標系から絶対座標系への変換を行ない得ることは前
述のとおりである。

好ましい実施例では、第１２図の回路に従ったハードウ
ェア回路をＣＰＵとは別の集積回路で構成づる。

第１３図はこの発明の実施例に用いられる集積回路の一
例を示すブ【コック図である。この集積回路３は、後に
詳細に説明するがその中には８ピツＩ〜レジスタが１６
個配置されたシフトレジスタである。そして、この集積
回路３はマイクロプロセリないしＣＩ）　ＬＪに接続さ
れ、入力バス［３ｉに接続された入力ポートＩｎ”・■
７をイｊし、出力バスＢＯに接続された出力ボートＯｏ
　”　０７を有する。

マイクロプロセサないしＣＰＬＪからのデータは入力バ
ス３ｉの入力ポートＩｏ〜１□によって与えられ、ＣＰ
Ｕへは出力バスＳｏの出力ボートＯ。

〜０７を通してデータがもえられる。他の集積回路から
のデータはデータバスＤ１またはＤ２によってそれぞれ
に対応づる入力ボートＤｌｏ〜Ｄ１、またはＤ　２ｏ　
”Ｄ　２　？に与えられる。伯の集積回路へのデータ出
力は［）ｏｕｔ　（Ｄｏ〜Ｄ７）から得られる。データ
の選択はＣＰ　ＬＪからちえられる信号Ｄ　＋　／　Ｄ
　２によって行なわれる。集積回路３は、さらに、その
中に含まれるシフトレジスタのためのシフトクロックＧ
Ｋおよびイの他の信号ＲＯ王、Ｏ８，Ｒ／Ｗ、ＳＴＢお
よびＣｌ−Ａを受ける。

第１４図はこの集積回路の詳細を示すブロック図である
。集積回路３は１６の８ビツトレジスタ３００〜３１５
を含み、各々のレジスタは８つのフリップフロップから
なる。たとえばレジスタ３００はフリップフロップ３０
０ａ〜３００　ｈがらなり、レジスタ３１５はフリップ
フロップ３１５ａ〜３１５ｈからなる。レジスタ３００
の各々の７リツプフロツプ３００ａ−３００ｈは、各々
のデータセレクタ３１６ａ〜３１６ｈがらのデータ出力
を受け、これらフリップフロップ３００ａ〜３００　ｂ
の出力は、ナントゲート３１７ａ　〜３１７１１のそれ
ぞれの１人力に与えられるとともに、次段のレジスタ３
０１を構成づるそれぞれのフリップフロップ３０１ａ〜
３０１ｈに与えられる。

そして、レジスタ３０１の内容は順次レジスタ３０２、
・・・３１４，３１５に送られる。このようにしてレジ
スタ３００〜３１５にＪ：ってシフトレジスタが構成さ
れる。レジスタ３１５の各々のフリップフロップ３１５
８〜３１５ｈの出力は、対応のデータセレクタ３１６８
〜３１６ハに与えられるとともに、データ出力ＤＯｕｔ
の各ピッ１−Ｄｏ〜Ｄ７となる。データセレクタ３１５
ａ　〜３１６ｈには、それぞれ、データ入ノ）Ｄ”ＩＪ
３よび１っ２づなわらＤ　１　ｏ　”□Ｄ　１７および
Ｄ２ｏ”Ｄ２７が与えられ、ざらに、入力バス３　ｉ　
ｉすゎらｌ　ｏ　’［７が与えられる。データセレクタ
３１６ａへ・３１６ｈには、さらに、コン１−〇−ラ３
１８を経た信号Ｄ１／ｌ）２および信号Ｒ／Ｗの反転と
信号６百の反転との論理積Ｒ／Ｗ−Ｃ８が与えられ、ざ
らに信号ＲＯＴが与えられる。ここで、信号１）　１’
　／　Ｄ２は、データ入力Ｄ　１　ｏ　”−Ｄ　Ｉ　？
　Ａ′３よびＤ２ｏ”−０２、のいづ゛れがを選択的に
取込むための指令信号であり、たとえばこの信号「１」
でデータ入力Ｄ　１０　”　Ｄ　１７が選択され、「ｏ
−１でＤ２ｏ〜・Ｄ２７が選択される。信号Ｒ／Ｗは、
レジスタへのう゛−タ書込みまたはレジスタからのデー
タ読出しの選択を行ない、「１」で読出し「ｏ」で書込
みとなる。信号ｃｓはチップレレク１〜（Ｆｉ号であり
、ｒＯＪでこの素子が選択される。信号ＲＯＴは、ロー
テーション信号であり、レジスタ３００〜３１５の内容
をそれぞれ次段のレジスタにシフトさせるためものであ
り、ｒＯＪでそのシフト動作が指令される。このように
シフトさ才る必要があるのは、データ書込みおにび読出
しの両方においてであり、レジスタ３００からしかデー
タ書込みやデータ読出しができない構成であるからであ
る。

レジスタ３００のフリップフロップ３００８〜３００ｈ
の出力を受けるナントゲート３１７８〜３１７ｂのそれ
ぞれの残余の２人力には、コントローラ３１８を通して
、信号Ｏ８の反転と信号Ｒ／Ｗとの論理積Ｃ８−Ｒ／Ｗ
、Ｂよび信号Ｓ　Ｔ　Ｂの反転ＳＴＢが与えられる。信
号ＳＴＢはストローブ信号である。また、レジスタ３０
０−３１５を構成り−るフリップフロップ３００８〜３
１５ｈには、信号ＣＬＡが与えられ、この信号ＣＬＡは
クリア信号である。レジスタ３０１〜３１５のフリップ
フロラ７３０１ａ〜３１５１１には、クロックＧＫが与
えられる。レジスタ３００のフリップ７０ツブ３００ａ
〜３００ｈには、クロックＧＫに代えて、］ンヒトロー
ラ３１を通して、タロツクＯＫとストローブ信号ＳＴＢ
の反転とチップセレクト信＠Ｏ８の反転との論理積′！
ｌなわらＣＫ・ＳＴＢ　−Ｏ８が与えられる。

ナントゲート３１７ａ−３１７ｂの出力は出力バスＢＯ
の各ビットＢｏ”−Ｂ７ど４ｆる。

第１４図に示す集積回路３では、レジスタ３゜０〜３１
５間の１６回のシフトづなわちローテーション動作が、
先の第１２図に示づバラツノ・レジスタＢｏ〜［３７Ｊ
５よびｂｏ〜１〕６間の１シフトに相当°リ−る。そし
て、信号Ｏ８のｌ”ＯＪで当該チップが能動化され、信
号Ｒ／ＷがｒＯＪのときデータセレクタ３１６ａ　”３
１６ｉ＋を；［］　Ｌ／　Ｔ、信号Ｄ１／Ｄ２に依って
、データ入力Ｄｉ（Ｄ１ｏ〜Ｄ１７）またはＤ　２　（
Ｄ　２ｏ　”Ｄ　２７　＞が、レジスタ３００に与えら
れる。また、信号Ｒ／Ｗが１−１ｊのとぎ、ナントゲー
ト３１７８〜３１７ｈが有効化され、レジスタ３００の
フリップフ［」ツブ３００ａ〜３００ｈから、出力バス
Ｂ　ｏ　′−Ｂ　？ヘデータが読出される。なお、信号
ＤＩ／Ｄ２は第１２図に示す１題念図におけるデータシ
フト方向を表わＪｏ レジスタ３００〜３１５間のデータのローテーションは
信号ＲＯＴに応答して行なわれ、信号ＯＬ八に応答して
づべでのレジスタ３００〜３］５がクリアされる。

第１５図は第１３図および第１４図に示づ一集積回路３
を用いて第１２図の回路を実現づるための構成の一例を
示づブロック図である。第１２図の回路と機能的に対応
づ−る部分には同じ参照符号を付している。第１５図に
おいて、バッファレジスタ８１〜Ｂ７およびす、、〜ｂ
６が、それぞれ、第１２図の対応の参照符号のものに相
当する。それぞれのバッフ７レジスタは第１４図に示す
集積回路３を２個ずつ並列に含み、したがって１６ビツ
１〜の１６段シフトレジスタとして構成されている。

乗算回路ＭＰ、は、データセレクタ４つおよび５１なら
びに乗算器６１を含み、乗算回路Ｍ　Ｐ　２はデータセ
レクタ４２および５２ならびに乗算器６２を含む。デー
タセレクタ４１には、バッファレジスタＢ５の出力がＸ
、としてまたレジスタＢ。

の出力がＸＬとして）ｊえられる。データセレクタ５１
には、レジスタＢＧの出力がＹ、としてまたレジスタＢ
１の出力がＹ、としてうえられる。乗算器６１は、指令
信号ＣＭに応各して、データせレクタ４１および５１か
らの入力を乗算し、その乗算結果は一時記憶しジスタＩ
Ｒ４にロードされる。

一方、乗算回路ＭＰ２に含まれるデータセレクタ４２に
はバッファレジスタｂ５の出力がＸ、としてまたバッフ
ァレジスタＢ７の出力が×１として与えられる。データ
セレクタ５２にはバスノアレジスタＢ６の出ノＪがＹＲ
としてまたバッファレジスタｂ、の出力がＹ、−として
与えられる。乗算器６２は、指令信号ＣＭに応答しＵ　
、　−Ｊ’−タレレクタ４２および５２からのデータを
乗算し、その結果を一時記憶レジスタＲ２にロードづ−
る。データセレクタ４１，５１，４２おにび５２には、
それぞれ、データ選択信号Ｄ　Ｉ　／Ｄ　２が与えられ
、この信号によって信号ＸＲ，ＹＲまたはｘＬ、Ｙ。

が選択される。なお、一時記憶レジスタＲ５およびＲ２
もまた他のバッファレジスタと同じように第１４図に示
す集積回路の組合わせからなり１６ビツ１−のシフ１〜
レジスタである。

シフトレジスタ８１〜Ｂ？　、　Ｉ）　＋〜ｂ６．Ｒ，
＋Ｊ５　Ｊ：びＲ２には、入力バス３ｉおよび出力バス
ＢＯが連結される。そして、レジスタＢ１およびｂ、に
は後述の第１６図回路からのシフ１〜タイミング信号■
が与えられ、レジスタＢ５およびｂ５には、シフ１〜タ
イミング信、ＣＩ　Ｕが与えられ、一時記憶レジスタす
なわらバッファレジスタＲ＋およびＲ２にはシフ１〜タ
イミング信号Ｗ　ｈ＜与えられ、他のレジスタ８２〜Ｂ
、およびｂ２〜ｂうには、クロック信号φが与えられる
。

第１６図は第１５図に示す演輝回路に接続されるべきマ
イクロプロセザに関連する回路を示すブロック図である
。第１６図において、発振器１０１は、クォーツ１０１
ａを有し、２相クロツクφ１　ａ３よびφ２を出力する
。〜方のクロックφ１はシフトタイミング発生回路１０
２（後述）に与えられ、クロックφ２は乗算指令発生回
路１０３に与えられる。シフトタイミング発生回路１０
２からは、シフトタイミング信号Ｕ、ＶおにびＷおＪ、
びクロックφが出ツノされるとともに各レジスタ１３１
〜Ｂ？　、　ｌ）　１へ・ｂ、、Ｒ，およびＲ２内にお
いて、一連のシフｌ−が行なわれたことを表わず信号Ｓ
が出力される。シフ１〜タイミング発４［回路１０２か
らの信号Ｓはシフト回数検出回路１０４．シフト方向検
出回路１０５およびフリシフＬ］ツブ１０７に与えられ
る。シフト回数検出回路１０４１．は１６進カウンタと
フリップフロップとを含み、シフトタイミング発生回路
１０２かうの信号Ｓを１６ノＪウントするごとに出力を
出１ノ。すなわら、この回ＶＢ１０４は第１４図に示す
レジスタ３００〜３１５間でデータが１循環したことを
検出りる。

シフト方向検出回路１０５は、５進カウンタを含み、第
１２図、第１５図に示づバスｊ・レジスタ間において左
方向または右方向に５回シフトｍＪ＋作が行なわれるご
とに出力を出り。このシフ１ル方向検出回路１０５の出
力はフリツプフＬ１ツブ１０６に与えられ、このフリッ
プフロップ１０６はたとえば１〜グルフリツプフロツプ
からなり、回路１０５からの信号が与えられるごとにそ
の出力Ｑを反転フ゛る。このフリップフロップ１’０６
の出力はデータ選択信＠Ｄ　１　／　Ｄ　２となる。フ
リップフロップ１．０７は、１ｃとえばＪ−にフリップ
フロップがらなり、シフト動作をさせるかまたは乗算動
作をさせるかを区別づるもので、出力ΦがｒＯＪのとき
バッフルレジスタ間のデータシフト動作を、出力Ｑが「
１」のとき乗算動作を行なわせる。

シフト回数検出回路１０４の出力は、フリップフロップ
１０６の出力とともにアンドゲート１゜８の入力に与え
られる。アンドゲート１０８の出力はフリップフロップ
１０９のリヒット入力に与えられる。フリップフロップ
１０９はＣＰＵ１がらのスタート信号５ＴＡＲＴによっ
てセットされ、その出力０がエンド信号ＥＮＤ　（また
は割込信号）としてＣＰＬＪｌに与えられる。

フリップ７０ツブ１０７の出力Φは４つのアンドグー）
−１１０，１１１，１１２および１１３のそれぞれの一
方入力として与えられ、これらアンドゲートの他方入力
にはそれぞれシフトタイミング発生回路１０２からの信
号ｕ、ｖ、ｗ、ｂよびφが与えられる。したがっＣフリ
ップ７０ツブ１０７の出力Ｑが「１」のとき各バッノ７
？レジスタ内のシフトレジスタがシフトされてそのため
に乗算動作されることになるのは前）本のとおりである
。

ＣＰＵ１はレジスタ呼出タイミング発生回路１１４にア
ドレスデコーダ（図示せｆ〉からの信号を与える。この
レジスタ呼出タイミング発生回路１１４は第１５図の各
レジスタ内のアドレスを指定するためのものであり、ア
ドレス入力に対応して初期値をロードすべきレジスタを
指定し、第１４図に示す各レジスタ間のデータのシフト
動作ずなわらローテーションを行なわぜるための信号π
ＯＴを出）〕する。この信号ＲＯＴがｒＯＪのとき各レ
ジスタがシフト動作され、「１」のとぎはシフト動作が
行なわれない。

ＣＰＵ１に含まれるアドレスデコーダからの指定信号は
、このレジスタ呼出タイミング発生回路１１４を通して
オアゲート１１５の一方入力に与えられ、このオアゲー
ト１１５の他方入力にはアンドゲート１１３の出力が与
えられる。オアゲート１１５の出力はまたオアゲート１
１６．１１７Ｊ３よび１１８の一方入力として与えられ
、これらオアゲートにはそれぞれアンドグー１−１１０
，１１１Ｊ３Ｊ：び１１２の出力が与えられる。このよ
うにして、オアグー１−１１５．１１６，１１７および
１１８の出力が、シフトタイミング信号φ、Ｕ。

Ｖ　Ｊ３よびＷとなる。

また、ＣＰＬＪｌからは、他＠σ否、σ■ム、５Ｔ　Ｂ
およびＲ／Ｗが出力される。

ここで、第１７図を参照してシフトタイミング発生回路
１０２について詳細に説明する。まず初期状態としてフ
リップフロップ１３０．１３３おＪζσ１３６はずべて
リセットされている。したがってそれぞれの出力Φは［
１］である。応じて、アンドゲート１２１，１２２およ
び１２３の入力がｒｌＪであり、アンドゲート１２４の
入力が１゛０」であり、アンドゲート１２５の入力が「
１」であり、アンドゲート１２６および１２７の入力が
「０」である。

アンドゲート１２１にクロックφ１が与えられると、ア
ンドゲート１２２および１２５を通して４進カウンタ１
２８にクロックφが与えられる。

なお、カウンタ１２８は、他のカウンタ１２９゜１３１
．１３２および１３４と同じように、与えられるカラン
１へ入力がそのまま出ツノされる端子と所定のカラン１
〜した後信号を出ツノするカウントアツプ出力端子とを
含む。クロックφがカウンタ１２８に１個人ると、した
がっ゛（そのとさＡアゲート１３８に出力が出る。この
Ａアゲート１３８の出力がシフトタイミング信号Ｖとな
る。そして、カウンタ１２８に４つのクロックφが与え
られるど、このカウンタ１２８がらカウントアツプ信号
が出力されてそれが４進カウンタ１２９に入力される。

したがって、４進カウンタ１２９からオアゲート１３７
を通して、シフトタイミング（ｇ号（］が出力される。

したがって、成る場合には、このシフトタイミング信号
Ｕは４つのシフ１−タイミング信号Ｖごどに１つ出力さ
れることになる。カウンタ１２９がカウントアツプする
とすなわちアンドゲート１２５から１６個のクロックφ
が出力されると、フリップフロップ１３０がセットされ
、その出ノ）ＱがｒｌＪとなる。このフリップフロップ
１３０の出力ＱがレジスタＲ，，Ｒ２（第１５図）への
データロード信号すなわちシフトタイミング信＠Ｗであ
る。

フリップフロップ１３０の出力ＱがＮＪとなり、出ノＪ
ＱがｒＯＪとなることによって、アンドグーｈ　１２６
１Ｊ３Ｊ：び１２７が開かれる。しかしながら、アンド
ゲート１２７には、フリップフロップ１３３の出力Φが
「１」であるので、このときはアンドゲート１２４から
の出力φは与えられない。他方アンドゲート１２５はこ
のフリップフロップ１３０の出力ＱのｒＯＪによって閉
じられる。

したがって、この状態ではアンドゲート１２１がらのり
０ツクφはアンドゲート１２３および１２６を介してカ
ウンタ１３１に与えられる。そして、’７”−ト１２６
からクロックφが与えられるごとにカウンタ１３１から
オアゲート１３７を通してシフトタイミング信号Ｕが出
力される。カウンタ１３１は１２進カウンタであり、１
２個のクロックφがシフトタイミング信号Ｕどして出力
され、このカウンタ１３１が１２クロツクをカウントす
ると、カウントアツプ出力をカウンタ１３２およびフリ
ップフロップ１３０のリセット入力に与える。

したがって、フリップフロップ１３０がリレットされ、
再びカウンタ１２Ｂおよσ１２９が能動化される。そし
て、前述の１６のクロックφのカウントを繰返し、再び
フリツノフロップ１３０がセラ１〜され、カウンタ１３
１が能動化される。カウンタ１３１がカウントアツプづ
るごとにこのことが繰返され、その都度カウンタ１３２
に信号が与えられる。この信号を３進カウンタ１３２に
につてカウントすると、このカウンタ１３２がノリツブ
フロップ１３３をレットする。なお、カウンタ１３２に
カウンタ１３１から信号が与えられるごとに、このカラ
タン１３２からオアゲート１３８を通してシフトタイミ
ング信号Ｖが出力される。

フリップフロップ１３３がリセットされることによって
、その出力口が「０」となり、アンドグー１〜１２２お
よび１２３が閉じられ、アンドゲート１２４のみが開く
。このとき、フリップフロップ１３０がリセットされて
いるので、アンドゲート１２７が聞いている。したがっ
て、アンドグー１−１２１からのクロックφがカウンタ
１３４に与えられる。カウンタ１３４にクロックが与え
られるごとに、オアゲート１３８を通してシフ１〜タイ
ミング信号Ｖが出力される。カウンタ１３４は１３進カ
ウンタであり、クロックφを１３カウントするごとにカ
ウントアツプ出力を導出する。このカウンタ１３４から
のカウントアツプ出力がカウンタ１３５に与えられると
ともに、フリップフロップ１３３のリセット入力に与え
られる。カウンタ１３５は４進カウンタであり、そのカ
ウントアツプ出力はフリップフロップ１３６のセット入
力に与えられる。このフリップフロップ１３６のリセッ
ト入力にはＣＰＵからのリセット信号が与えられる。フ
リップフロップ１３６の出力Ｑが、第１６図の信号Ｓで
ある。フリップ７０ツブ１３６がリセットされると、そ
の出力Φが「１」となり、アンドゲート１２１が再び開
かれて、先の動作が繰返される。

ここで、第１８図〜第２０図を参照して、シフトタイミ
ング発生回路１０２からのシフトタイミング信号Ｕ、■
およびＷが持つ意味についてやや詳しく説明する。一般
に第１８図に示りようなマトリクスの乗算は、下記の式
に従って行なわれる。

ｗ、　＝ｕ、、　−ｖ、＋　＋ｕ、ス　′　Ｖ、、）　
−ト　ｕ＋３　＋　ｖＩｔ　＋Ｕす４　°Ｖ４４ ｗ　＝ｕ　−ｖ　＋ｕ　−ｖ　−＋−ｕ、−ｖ、、＋４
３　すｌ　ｌ３４２　２３ Ｕ牛ｆ”　ＶＳ３ ”ＩＩ−〇Ｉｔ”ｖＩｔ−”ｌｚ−”Ｖ）ｌ＋ＬＩ＋３
”Ｖ７７−トＵ叫　°Ｖ斗１ ところ−で、第１５図に示１゛ような集積回路では、第
１８図および曲成に示すマトリクスの乗算を一斉にづ−
ることはできないので、たとえばまず’１４・ＶＳ２　
を行ない、次にｕ４３→Ｕ耕、ｕ４２→ｕ４．。

Ｊｆ　４４　、■ｉｙ　−ｖＪ＋　、”’のＪ、うにデ
ータをシフトして行なう。すなわち、この実施例では、
第１８図に示ずＵ　の位置と■４ｆ　の位置との乗算４
１ を行ないこのそれぞれの位置に必要なデータをシフトな
いしローデートさせるのである。

このようなマトリクスにおけるり、■およびＷのデータ
のローデートないしシフト動作は、シフ１へタイミング
発生回路（第・１６図）からのそれぞれ対応のシフ１〜
タイミング信号ごとに１回行なわれる。そして、上記１
つの演算が終わると、データは順次シフトされるが、そ
の順序は第１９図に示す。Ｕについては、１つのシフト
タイミング信号Ｕごとに一列ずつシフトする。したがっ
て、！〔とえば第１８図の例で、Ｕよ、の位置にあるデ
ータを１４．１の位置にシフ１−するためには、１つの
シフ１〜タイミング信号Ｕを与えるだけでよい。このと
き、Ｉ輸、は同時にＵうの位置に移されるので、次にＵ
のデータをＵｑの位置に移ずためには、再度１つのタイ
ミング信号Ｕを加えるだけでよい。ずなわら、Ｕについ
ては、それぞれのデータを０４４の位置にもたらすため
には、それぞれ１つのシフトタイミング信号Ｕを加える
だけでよい。

■については、４つのシフトタイミング信号Ｖが必要で
ある。たとえば、第１８図の例において、Ｖ３４　の位
置にあるデータをＶ、の位置にシフトづるためには、４
つのシフトタイミング信号Ｖが必要である。

また、Ｗについては、１つのジノ１〜タイミング信号で
１つずつシフトされる。したがって、第１８図の例でい
えば、ｗ４．の位置にあるデータをＷの位置に移すため
には、１つのシフトタイミング信号Ｗが加えられればよ
い。

簡単に言うと、上記式のＷ、については、シフトタイミ
ング信号Ｗが１回出力され、続いてシフトタイミング信
号Ｕが１回そしてＶが４回与えられると１つ項たとえば
Ｕイ、・ｖ４ｆが１１なわれるので、このＷ　＜、＋　
のずべでの項の演算のためにはこのことを４回繰返せば
よい。そして、次のｗ４．の演算のために、シフトタイ
ミング信号（〕を１２回出力づ−るとともにＶを１回シ
フトしてその１．の演算のための初期状態を設定する。

続いてＷ＋Ｊの演算のｔ＝めにＷ　＋＋の演算と同じよ
うにＷ＝１．Ｕ＝１　ｄ３よびＶ＝４を４回繰返せ−ば
よい。以下同様である。

このことが第２０図のタイジングチ１フートに表わされ
ている。

そして、第１７図のシフトタイミング発生回路は、第２
０図に従って、それぞれのタイミング信号Ｕ、■および
Ｗを出力されるように構成されている。ずなわち、カウ
ンタ１２８によってタイミング信号ｖを、ノノウンタ１
２９によってタイミング信号Ｕを、そしてフリップフロ
ップ１３０によってタイミング信号Ｗを出力り−る。そ
の後の１２個のタイミング信Ｑ　ＬＪはカウンタ１３１
がら出力される。

第２１図は第１５図および第１６図に示Ｊ実施例の動作
を説明づ“るためのフロー図である。ここで、第２１図
を参照して、この実施例の動作について説明する。まず
、ＣＰＵ１　（第１６図）は、動作の最初において、初
期設定を行なう。このとき、必要に応じて信号ＣＬＡを
出力し、第１５図に示す各レジスタをクリアする。そし
て、ステラ１２０１において、このＣＰＵ１に含まれる
カウンタ（図示せず）の値ｎを１にセットする。なお、
この値ｎはＯ〜１５をとり智る。続くステップ２０２に
おいて、ＣＰＵ１は第１４図に示づレジスタ３００〜３
１５を、カウンタの値１１に従って指定する。これはこ
のＣＰＵ　１からレジスタ呼出タイミング発生回路１１
４にレジスタ指定アドレスを出力することによって行な
われ得る。したがって、このステップ２０２て・は、レ
ジスタ呼出タイミング発生回路１１４（第１６図）７３
口ら、信号ＲＯＴが出力され、該当のレジスタが選択さ
れる。

ずなわら、第１５図に示す各レジスタ８１〜［３７およ
びす、〜ｂ６のそれぞれｌ）番目のレジスタが指定され
る。続くステップ２０３にａ３いて、前記（１９）式の
変換７１−リクスＭ、へ□ＩＶＩ６ｉ１＞よびＭ１′〜
Ｍ６’　をそれぞれの対応のレジスタ８１〜Ｂ７および
す、〜ｂ６　（第１５図）にス１ヘアする。

すなわち、まずそれぞれのレジスタのｎ番目のレジスタ
（第１４図）に、Ｍｋ　、　Ｍｋ　＋、（ｋ　＝　１へ
・６）の１つの要素をス］〜アする。したがって、この
とぎは、チップレレクト信号ｃｓによって第１５図のそ
れぞれのレジスタＢ、〜Ｂ６おＪ：びす。

〜ｂ６が順次指定される。このようにして、それぞれの
レジスタＢ、〜Ｂ７Ｊｊよびす、〜ｂ６のそれぞれの１
１番目のレジスタにそれぞれ対応の変換マトリクスの１
つの要素がストアされる。そして、続くステップ２０４
において、カウンタの値が１６（これは第１４図に示す
レジスタの数に対応づる）であるかどうかを判断する。

そして、ｎ＝１６でなければ、ステップ２０５においＣ
そのカウンタをインクリメントし、再び同じことを繰返
ず。

し／ｊがって、ステップ２０４にＪ３いで、１１−１６
であるということは、すべてのレジスタＢ、〜］３７Ｊ
：；Ｊ：びｂ１〜ｂ６のずべてのレジスタ３００〜３１
５（第１４図）にそれぞれの変換マトリクスＭ、〜Ｍ６
およびＭ、１〜Ｍ６’のすべて゛の要素がストアされた
ことを意味する。

続いて、ＣＰＵＩは演算スタート指令５ＴＡＲＴを出力
する。応じて、フリップフロップ１０９がセットされる
とともにフリップ７０ツブ１０７がリレットされ、この
フリップフロップ１０７の出力０が「１」となる。した
がって、アンドゲート１１０〜１１３が開き、シフトタ
イミング発生回路１０２からのシフトタイミング信号Ｕ
、■およびＷおよびクロックφが出力可能どなる。′？
Ｉなわち、この状態で乗算が行なわれる。

乗算は、シフトタイミング発生回路１０２から、第２０
図で示すようなタイミングで各々シフトタイミング信号
が発生し第１９図に従ってシフト〃Ｊ作が行なわれるこ
とによって、第１８図に従ってマトリクスの乗算が行な
われる。この間に、シフ（一方向検出回路］０５および
データ選択フリップフロップ１０６によって、第１５図
に示１回路にお（）るデータ転送（シフ１〜）方向が制
御される。

すなわち、第１２図の回路に示すデータシフト方向がこ
のシフト方向検出回路１０５およびフリップフロップ１
０６で制御ａｌｌされる。データシフ１〜を５回繰返ず
ことに応じてフリップフ［コツプ１０６の出力がデータ
選択信号Ｄ　１　／Ｄ　２どして導出されるが、このと
きシフト回数検出回路１０／ｌからはまだ信号が出され
ていないので、アンドゲート１０８は出力を出さない。

曲成のＷ４□、Ｗ、３．・・・のそれぞれの演算が終わ
るごとにシフトタイミング信号発生回路１０２からシフ
１へ回数検出回路１０４に信号が与えられる。

シフト回数検出回路１０４は、前述のように１６進カウ
ンタを含むので、シフトタイミング発生回路１０２から
１６個の信号が得られたとき、すなわち前（１９）式の
演算がすべて行なわれたとき、「１」の信号を出す。こ
のとぎフリップフロップ１０６の出力もまたｒＩＪであ
り、アンドゲート１０８から「１」の信号がノリツブ７
０ツブ１０９のリセツｌ−信号として与えられる。した
がって、このフリップフロップ１０９からの出力Ｑが「
１」となり、それがＣＰＵ　１に与えられる。ＣＰＵ１
では、このフリップフロップ１０９の出力０の状態に応
じて、ステップ２０７にａ３いて、演算終了がどうかを
１′り断する。

上述のようにして演算が終了するど、スミ−ツブ２０８
．２０９．．２１０．２１１および２１２において、先
のステップ２０１，２０２，２０３゜２０４および２０
５と同じようにして、第１５図に示す各々のレジスタ８
１〜’３ｔｊＪ３よびｂ１〜ｂ６のそれぞれのレジスタ
３００〜３１５（第１４図）からデータを読込む。この
とき、データの書込みと異なり、ＣＰＵ　１からのイＩ
；舅］で／ＷはｒＯＪとされるのはもちろんである。こ
のようにして、ＣＰＵは、ヤコビアンの各々の要素を演
算回路２から取込み、前記（１８）式に基づいて各関節
角の変化量をめる。このとき、ＣＰＵは第２２図に示づ
ような演算を行なう。

第２２図は、Ｒｅｓｏｌｖｅｄ　Ｍ　ｏｔｉｏｎ　１１
　ａｔｅ　Ｃｏｎｔｒｏｌにおける処理演緯ループを示
ず。ずなわ１５、ＣＰＵは第２２図に示ずように、直角
座標系での変化量ｄＸ、ヤ、＝　Ｘ　、ｔ、　−Ｘ　：
　をめ、次いで前述のようにしてめた”トコビアンＪ（
ｄｌ　）の逆マ［−リクス［Ｊ（α、）］−’とｄＸｔ
千１　とを用いて関節座標系の変化量ｄα、す１　を演
算づる１、最後に、直前の関節系による位置ｄα、と上
記変化１ｄαｉｆｌ　とを加粋して、次の点の関節系に
よる位置α１＋１　を１ｑる。このようにして、直角座
標系から関節座標系への座標変換が１７コビアンを用い
て行なわれる。

第２３図はシフトタイミング発生回路の別の例を示ず回
路図である。この実施例では、シフトタイミング発生回
路１０２は７どツｌ〜のカウンタ１４１を含む、このカ
ウンタ１４１のカウント人ノ〕にはクロックφが与えら
れる。そして、最上位ビットＡから最下位ビットＧまで
７ビツトＡ、Ｂ。

Ｃ，Ｄ、Ｅ、、ＦおよびＧを有するバイナリカウンタと
して構成される。そして、シフトタイミング信号Ｕ、■
およびＷは、それぞれ（２２）式に従つＣ導出される。

Ｗ＝Ｃ−ＤＥＦＧ ・・・（２２） なお、クロックφが、それぞれのシフトタイミング信号
とクロックとの同期をとる１＝めに、適当なゲートに入
れられている。この第２３図のようなシフトタイミング
発生回路を用いれば、第１７図に示づものに比べて非常
に簡単な回路構成となる。

以上にうに、この実施例によれば、直角座標系と関節座
標系との間の座標変換のために、マトリクスの各々の要
素は別のバードウコアで構成した演算回路で演算し、Ｃ
Ｉ）　Ｕではその演算結果を取込んで適当な変換式に代
入するだ【ノてよいので、ＣＰＵが座標変換のＩＣめに
演算に利用される時間が非常に短くなる。したがって、
ＣＰＵたとえばマイクロプロｔ？すやマイクロコンピュ
ータとして高速のものを用いなくても、極めて正確な座
標変換が行なわれる。したがって従来の〜しののように
相当大きなエラーが出たり、同時制御可能な自由度が制
限されたりすることはなく、追随の正確性が期待できる
。また、ＣＰＩＪが座標変換の演算のために利用される
時間が非常にλ（Ｉくなるのぐ、ＣＰ　Ｕに対して別の
機能をイ１加することが簡９１に（・きる。しかも、演
算回路は、はと／Ｖど類似し／ｃ演算を何回も繰返すだ
けであり、制御すべき自由度の増減に対しても簡単に対
応することができる。

なぎならば、演算回路で利用可能な演算機能を単に増減
するだけで、ＣＰＵのソフトウェア（プログラム）につ
いてはほとｌυど変更が不要であるからである。このこ
とは、特に、関節を有するロボットにおいて、エンドエ
フェクタの種類を変更１′る際に凹求される自由度が異
なる場合、そのような自由度の変化に容易に追随できる
という極めて大ぎな効果である。

以上に説明したヤコビアンＪ（α）のめ方を、理解をよ
り完全なものとするためここに整理してみる。

現できないので、ｘ　ｃｗｔｏｎ−１＜　ａｐＭｏｎ等
のＩｊ法で解く例が過去にあった。まだ、それがｅＸｐ
ｌｉｃｉｔに表現できたとしても、複雑な非線形な方程
式となり、それの演算には多大の時間を要しＩＣｏそこ
で、ｄ　Ｘ−＋ｄαの変換を位置フィードバックループ
の中で行なうことが提案されている。これをＲｅ５ｏｌ
ｖｅｄ　Ｍ　ｏｔｉｏｎ　Ｒａｔｅ　Ｃｏｎｔｒｏｌと
いう。このとき、ｄＸ→ｄαの関係は線型となり演算時
間は短縮されるが、フィードバックのサンプル周波数を
実用に耐えるだけに上げるために、変換は高速で行なう
必要がある。このプロレスは次の３段階に分割できる。

（ｉ）　ヤコビアンＪ（α）の係数をめる。

（ｉｉ）　ヤコビアンのインバースＣｄ（α）］−１を
める。

（山）　ｄ　ニー［Ｊ　（’；）　］−’・ｄ又をめる
。　・・・（１８） 特に（ｉ）のプロレスはソフ１〜つ「アを用いる従来の
方法では非常に時間がかかっていた。そこで、＼７］ビ
アンＪ（α）を専用ハードでめたのが上記実施例である
ａＬ記実施例−（−は、髪７ニ１ピノＩンインバース［
Ｊ（α）］−１をめることおよび（１８）式のｊ１算は
マイク］］：１ンピ］−タのソフトウェアで行なってい
る。

次に、Ａ７コビアンＪ（α）を専用ハードでめる他の実
施例を説明づる。この実施例は、要約Ｊれば、各自由度
の動きを座標変換マトリクスとして保持するマトリクス
レジスタを自由度の数（たとえばＮ）だ（プシフトレジ
スタとして連結し、同様に前記各自由度の座標変換マト
リクスのその軸の動きに関する微分を保持するマトリク
スレジスタを自由度の数だけシフトレジスタとして連結
し、マ［−リクス乗算器を備え、各マトリクスシフトレ
ジスタにおいてマトリクスのシフトを行なう過程で適宜
マトリクス乗算を行ない、ＮＩＩ！ｉｌのマトリクスの
うち１つのマトリクスだけは微分のマトリクスを用いた
積マトリクスをＮ種類求めることにより、ＮＸＮのヤコ
ビアンマトリクスの係数をめるものである。以下、これ
を図面を用いて説明する。

第２４Δ図および２４Ｂ図は、ヤコビアンＪ（α）をめ
る装置を説明するためのブロック図である。この図は、
第２Ａ図および２Ｂ図に相当し、点線で囲んだ部分がヤ
コビアン演算部９０である。マトリクスシフトレジスタ
２５ａ５よび３２がマトリクスマルチプレクサ４５の入
力部に接続されでＪ５す、マトリクスマルチブレクリ４
５の出力部はマトリクス乗算器４７の入力部に接続され
ている。マトリクス東算器４°７の入力部には、さらに
、マトリクスレジスタ４６が接続されている５、マトリ
クス乗算器４７の出力部には、７１へリクスレジスタ４
８が接続されてＪ３す、７１〜リクスレジスタ４８の出
力部には、マトリクスレジスタ６７が接続されている。

マトリクスシフトレジスタ２５は、６つのマトリクスレ
ジスタ２５１ないし２５６を備える。同様に、７１ヘリ
クスシフトレジスタ３２は、６つのマ［〜リクスレシス
タ３２１ないし３２６を備える。

マトリクスシフトレジスタ２５は、各自由度の座標変換
マｌ−リクスＭ１ないしＭＧを（５）持する。

これらの座標変換マトリクスは、マイク［１コンビコー
タ（図示せず）から与えられる。？１−リクスシフトレ
ジスタ３２は、各自由度の座標変換マトリクスのでの軸
の動きに関ηる微分のンＩヘリクスＭ　１１　ないしＭ
６’　を保持覆る。これらの微分のマトリクスもまた、
マイクロコンピュータから与えられる。マトリクスマル
チブレクリ″４５は、一種のレレクタであり、マトリク
スシフトレジスタ２５あるいは３２のいずれか一方を切
換えて７１−リクス乗粋器４７に接続する。マトリクス
乗算器４７、マトリクスレジスタ４８および４６は、そ
れぞれ、第６図で説明したマ［〜リクス乗算器４１およ
びマトリクスレジスタ４２および４３と同様のものであ
る。マトリクスレジスタ６７は、６×６のマトリクスレ
ジスタである。

次に動作につき説明する。第２５Ａないし第２５Ｃ図は
、ヤコビアン演算部９０の動作を説明するための図であ
る。まず第２５Ａ図を参照して、マトリクスシフトレジ
スタ２５内のマトリクスレジスタ２５１ないし２５６に
は、それぞれ、マトリクスＭ１ないしＭＧが保持されて
いる。マトリクスシフトレジスタ３２内のマトリクスレ
ジスタ３２１ないし３２６には、それぞれ、７１−リク
スＭｌ’ないしＭ　％が保持されている。マトリクスマ
ルチプレクサ４５は、マトリクスシフトレジスタ３２側
に切換えられている。マトリクスレジスタ４６は、単位
マトリクスＥにイニシャライズされている。したがって
、７１−リクス乗算器４７においては、マトリクスＭ、
′　と１１１位マトリクスＥとの積がめられ、その結果
ｒあるマトリクスＭ、′がマトリクスレジスタ４８中に
ロードされる。

次に第２５Ｂ図を参照して、ＩＩｉ＋述したタロツク信
号φＡ　Ｊ５よびφＢに応答して、７１−リクスシフト
レジスタ２５および３２の中身は、１ブロツクだ（プ図
面上で右にローデートされる。マトリクスマルチプレク
＋Ｊ４５は、マトリクスシフ１〜レジスタ２５側にり換
えられる（これ以険もこのままである）。７１−リクス
レジスタ４（５中には、マトリクスレジスタ４８中の中
身であるマトリクスＭ１′がロードされる。マトリクス
乗算器４７において、マトリクスＭ２とマトリクスＭ＋
’　との積がめられ、その結果である７１ヘリクスＭ　
＋　’　・Ｍ２がマトリクスレジスタ４８中に［１−ド
される。

以降同様にして、マトリクスＭ１′　・Ｍ２　・Ｍ、、
Ｍ、’　・Ｍ２−Ｍ、・Ｍ、、Ｍ、’　−ＩＶ＋２　−
Ｍ、・Ｍ４　・Ｍ５がマトリクスレジスタ４８中に順次
ロードされ、最後に Ｍ、′　・Ｍ２・Ｍ３・Ｍ４　・Ｍ５　・Ｍ６・・・（
２３） がマトリクスレジスタ４８中にロードされる。この時貞
′ｃ第１の演算過程が終了する。この（２３）式は、前
記（１９）式のマトリクス積のうちの１つぐある。（２
３）式により、ＶコピアンＪ（α）の６つの要素”Ｐ　
ｘ　／　ａ（Ｘ＋　、ａＰ　ｙ　／　ａα、　。

３Ｐｚ／ａα、、’ａｌｌｘ／ａα＋　、ａｎ、／ａｃ
ｘ＋　６３よびａｎ　ｚ　／　”α１がめられるので、
これらをマトリクスレジスタ６７の適当な場所、たとえ
ば、第６列に［］−ドしてＪ５　＜。

次に、第２の演算過程につき第２５Ｃ図を参照して説明
する。まずマトリクスレジスタ２５１ないし２５６には
、それぞれ、マトリクスＭ１ないしＭｌｉが保持されて
おり、マトリクスレジスタ３２１ないし３２６には、そ
れぞれ、７１〜リクスＭ、′ないしＭ６’が保持されて
いる。７１ヘリクスマルヂプレクサ４５は、マトリクス
シフトレジスタ２５側に切換えられており、マトリクス
レジスタ４６中には単位マトリクス［で初期化されてい
る。マトリクス乗算器４７での演算にｊ、す、マトリク
スレジスタ４８中に（よマトリクスＭ、がロードされる
。

次に、前述と同様に、タロツク信号φ△ｄ３よびφＢに
応答して、マトリクスシフトレジスタ２５Ｊ３よび３２
の中身が１ブロツクＩどけ図面上で右にローデートされ
、７１〜リクスレジスタ４６には７１〜リクスＭ１かロ
ードされ、７１−リクスマルチプレクザ４５（１７１〜
リクスシフトレジスタ３２側に切換えられる。マトリク
ス乗算器４７での演算により、７１−リクスＭ、・Ｍ２
１　がマトリクスレジスタ４８中にロードさねる。

さらにマトリクスマルチブレクザ／１５５は７１へリク
スシフ１〜レジスタ２５側に切換えられ、前述と同様の
動作が行なわれ、Ｒ終的には７１ヘリクスレジスタ４８
中に Ｍ、・Ｍ２’　・Ｍ３　・Ｍ、・Ｍ５　・Ｍ６・・・（
２４） がロードされる。このく２４）式もまた、前記（１９）
式のマトリクス積のうちの１つである。

したがって、（，２４＞式により、ヤコどアンＪ９ （α）のα２微分に関する６つの要素がまる。

そこで、マトリクスレジスタ６７の前記第６列の中身を
第５列にシフ１−づ−るとともに、上記６つの要素を第
６列にロードでる。

以後同様に前述と同様の演算過程を第６まで行なうこと
により、マトリクスレジスタ６７にはレコビアンＪ（α
）のすべての要素がロードされ、これによりＡ７コピア
ンＪ（α）がめられたことになる。

この実施例によっても、上記実施例と同様に、演算時間
の高速化が図られるとともに、装置の汎用化すが図られ
る。

［Ｊ（１）］−１のめ方 次に、ヤコビアンインバース［Ｊ（α）］−１をめる方
法およびそのための装置につき説明する。

これは、第１Ａ図おにび第１Ｂ図の座標変換部６の一部
分に相当する。

Ｖコどアンインバースをめることは、元連立１次方程式
を解くことに他ならない。そのためには、Ｇ　ａｕ／３
−　Ｊ　０ｒｄａ１１の方法のように、ある種の掃き出
しを行なって一気に解をめる方法が一般的であるが、繰
返し近似法による方法しある。この実施例においては、
Ｖコピアンインバースをめることがサーボレート、つま
りＲｅ５ｏｌｖｅｄ　ＭＯｌｉＯｎ　Ｒａｔｅで行なわ
れるため、アームの状態は微小量ずつしか変化しない。

したがって本実施例は、ヤコビアンａ５よび１７ピコア
ンイバースが微小変化しかしないことを最大限に利用し
、繰返し近似法を１回またはＵい田い数回性なえば充分
−（あるとの見地に立って、ヤコビアンインバースをめ
る手段をハードウェア化りる−しのＣある。これを以下
に詳しく説明りる。

まず、Ｖ７コビアンＪから（７コヒアンインバース（Ａ
７」ヒアンの逆マトリクス）Ｊｏｌを近似的にめる方法
について説明する。

ここでは最急降下法と呼ばれる近似法を使用するが、こ
の近似法の数学的な意味を明確にするために、任意のＭ
ＸＮマトリクス［△］（ｉｊ要素はΔ時）から、その逆
マトリクス［Ｂ］（ｉｊ要素はＢυ、、）をめる場合に
ついで説明する。

７１〜リクス［Ｂ］がマトリクス［Ａ］の逆７１〜リク
スであるとは、この２つのマトリクスの間に［△］Ｘ［
Ｂ］＝［Ｅ］　・・・（２５）なる関係式が成立するこ
とを意味する。ここで［Ｅ］（ｉｊ要素はＥｐ、）は単
位マトリクスで゛ある。

この関係式を要素の間の関係式として表わせば、周知の
７１〜リクスの乗算の定義に従って、たＡＱｋ−防御Ｅ
、。

・・・（２６） となる。この〈２６）式を変形すれば、・・・（２７） ｉミＡ　、ｋ　−８１＜ｉ　−Ｅ　υｋ　−〇が得られ
る。〈２７）式は、すべての１．ｊの組に対して成立し
てａ３す、したがって（２７）式は、ＭＸＮ個の方程式
よりなる連立方程式である。

ところで、今考えにうとしでいるのは、実空間の座標変
換に関係した（フコビアンであるから、方程式（２７）
の各係数およびその解は実数である。

したがって、方程式（２７）から Ｖ−ΣΣ（ΣＡ、Ｊｋ−Ｂｋ；−Ｅ等）２・・・（２８
）υｊ−に なる量Ｖを定義すれば、このＶは方程式（２７）の解に
対しては０１他の実数値に対しては正の値どなる。そし
て、Ｖ＞Ｏの場合においては、■がよりＯに近いほど良
い近似解となっていることになる。

最急降下法では請求めるべき１３□つに初期値をＪうえ
７ご後、Ｓ番目の近似解から（Ｓ＋１）ｆｆｉ目の近似
解をめる式 ％式％） によって繰返し行ない、より高いｉｉ’＋度を持つ近似
解をめる。（２８）式の右辺の第２　ｓ＋：＋　４よ、
ｖ”ｔｖＢｍ侭についての偏微分を含んでいることから
れかるように、Ｓ番目の近似解をＶの勾配の方向に移動
させて（ｓ＋１）番目の近似解を１Ｕようとづるもので
ある。（２９）式の２６５）は、１３’Ｈ’ｒ＋にａ３
いてψ′が極小となるように定める。りなわら方程式６
式％（３０） から請求めればよい。（３０）式を２０について解くこ
とは一般に複雑となることから、Ｖ’％λ１柴ついてテ
ーラ−展開し、最低次項のみを残すと０が得られる。

ど買（プば、 λ（Ｓ〕　−　Ｖ”　／　Ｕ（Ｓン　・・・　（３３）
どなる。ソｓ〜よ、（３３）式で定義されるから具体的
には ・・・（３４） どなる。

以上の方法をまとめると、 （ｉ）Ｓ番目の近似解である逆？１〜リクス［Ｂ　”　
］を用いて ［Δ０すＸ　［［３”］　＝　［Ａ（５）Ｂ”］　・　
（３５）をめ、これからマトリクス［、Ｅ　−ｋ’Ｂ（
ｓ）１をめる。このマトリクスの各成分の自乗和を計算
してこれをｖ５をする。

（ｊｌ）式 ％式％）］ （３６） によって右辺のマトリクスを作る。具体的な計紳かられ
かるように、右辺のマトリクスのｍ１１成分要素はＢ　
ｖ＋Ｓ）　１つＢＡｇ）となっている。この７１−リク
スの各成分の自乗和をバ１算するどＵＣトなっている。

但し王は、転置マトリクスを表わず。

（１■）　以上の諸量を（２つ）式に対応する式［Ｂ　
（Ｓ刊〕　コ ＝　ｉＢ”　］　−／（”　［Ａ”’（Ｌ−Ａ”　Ｂ”
）　］・・・　〈３７） の右辺に代入すれば、（Ｓ＋１）番目の近似逆マｌ〜リ
クス［Ｂ”１）１が得られる。

以上は任意のマトリクス［△］の逆マトリクス［Ｂ］を
める方法につき説明した。したがって、以上の方法にＪ
：すＡ１］ビアンＪのインバースＪ″′をめるには、マ
トリクス「△１をヤ」ビアンＪと、マトリクス［Ｂ］を
ヤ」ピノノンインバースＪ４と考えればよい。

次に、以上説明した方法を用いてヤコビアンインバース
Ｊ−’をめる装置につき説明する。

第２６図は、ヤコビアンインバースＪ’−１をめる装置
を説明するためのブロック図である。この図は第２Ｂ図
に相当し、点線で囲んだ部分がヤコビアンインバース演
算部４００である。第２７Ａ図ないし第２７Ｄ図は、ヤ
コビアンインバース演算部４００の動作を説明するだめ
の図である。

まず第２６図を参照して、バッファレジスタ６９の入力
部はマイクロコンピュータ（図示せず）に接続されてお
り、出ノｊ部はマトリクス乗算器７０の一方の入ノｊ部
に接続されている。マトリクス乗算器７０の他方の入ツ
ノ部は、後述づるマトリクスレジスタ７５の出力部に接
続されている；７１〜リクス乗算器７０の出力部は、７
１−リクス加惇器７１の一方の入力部に接続されており
、７１ヘリクス加算器７１の他方の入力部はマトリクス
レジスタ７２の出力部に接続されている。マトリクス加
算器７１の出力部は、マトリクスレジスタ７２の入力部
に接続されており、マトリクスレジスタ７２の出力部は
マトリクスマルチブレク１す７３の一方の入力部に接続
されている。７１〜リクスフルチブレクサ７３の他方の
入力部は、後述Ｊるン１〜リクスレジスタ７７の出方部
に接続されている。マトリクスマルチプレクサ７３の出
力部は、マトリクス乗算器７４の一方の入力部に接続さ
れており、マトリクス乗算器７４の他方の入力部はマト
リクスレジスタ６８の出力部に接続されている。マトリ
クスレジスタ６８の入力部は、前Ａしたマトリクスレジ
スタ６７の出力部に接続されている。マトリクス乗算器
７４の出力部は７１ヘリクスレジスタ７５の入力部に接
続されており、７１−リクスレジスタ７５の出力部はマ
トリクス［ｆｉＬ器７Ｇの一方の人力部に接続されてい
る。マトリクス減算器７６の他方の入力部にはマトリク
スレジスタ８Ｂが接続されている。このマトリクスレジ
スタ８８は、マイクロコンピュータから与えられるＯま
たは単位マトリクス１三を保持する。このマトリクスレ
ジスタ８８は、このように、定数７１〜リクスを保持す
るためのレジスタであるので、ポストシステムからの制
御信号を利用することにより、マトリクスレジスタ８８
を用いないでこれと同等の動作を実現することも可能で
ある。マトリクス減算器７６の出力部は、乗算・加算器
７８の２つの入力部およびマトリクスレジスタ７７の人
力部に接続されている。乗算・加算器７８の出力部はバ
ッファレジスタ７つを介してマイクロコンピュータ（図
示せず）に接続されている。これらの各機器の個々の機
能は先に説明したものと同様である。

ＩＣとえば、マトリクス乗算器７０および７４のＩｊ３
１能は、先に説明したマトリクス乗算器４１の機能と同
様である。ずなわち、後述づるように、バッファレジス
タ６９からはスカラー量λが、マトリクスレジスタ７５
からはマ［・リクス［Ａ（Ｅ−ＡＢ）］がそれぞれ出力
され、それらの積がマトリクス乗算器７０においてめら
れるのであるが、スカラー邑λとマトリクス［Ａ　（Ｅ
−ＡＢ）１との積λ［Ａ　（Ｅ−ＡＢ）］は、 λ［Ａ１（ＩＥ−Ａｓ＞　］　＝／ＩＥＸ　［Ａ”（Ｅ
−ＡＢ）］と書くことができる。ここで、Ｅは単位マト
リクスである。したがって、バッファレジスタ６９に保
持されているスカラー量λを次の７１−・リクスλしＩ
Ｃがって、７１〜リクス乗粋器７０の機能は７１〜９９
７乗算器４１の機能と同様Ｃ・あると言っても差支えは
ない。マトリクスレジスタ６８，７２゜７５Ｊ３よび７
７は、それぞれ６×６の７１〜リクスレジスタであり、
その殿能は先に説明した７１〜リクスレジスタ６７のそ
れと同様である。？トリクスマルヂプレクサ７３の機能
は、先に説明したマトリクスマルチプレクサ４５の機能
と同様である。

乗算・加算器７８の機能は、先に説明しＩこ乗算・加算
器８２０機能と同様である。マトリクス加算器７１は、
２つのマトリクスの加算を行なう。なお、このマトリク
ス加算器７１に、２つのマトリクス間の減算を行なわせ
ることは容易である。マトリクス減棹器７６は、２つの
７１−リクス間の減筒を行なう。

次に１７コビアンインバ一ス演算部４００の動作につき
説明する。なＪ５、以下の動作は、前述したクロック信
号φΔおよびφＢに応答して行なわれる。

まず第２７Δ図を参照して、マトリクス乗筒プロセスど
その結果につぎ説明する。７１〜リクスレジスタ６８は
、たとえばマトリクスレジスタ６７から先に説明した（
３５）式のマトリクス［ＡＳ′］を受取る。７１〜リク
スレジスタ７２には（３５）式の７１−リクス［ＢＬｓ
ｌｌが保持されているｂのどする。マトリクスマルチプ
レクリ°７３はマトリクスレジスタ７２側に切換えられ
【おり、マトリクス乗詐器７４において（３５）式の乗
算が行なわれ、その結果がマトリクスレジスタ７５中に
保持される。

次に第２７Ｂ図を参照して、シフトプロセスとその結果
につき説明する。マトリクスレジスタ８８中には単位マ
トリクスＥが保持されており、マトリクス減算器７６に
おいて［Ｅ　−Ａ”Ｂ”］をめる演算が行なわれ、その
結果がマｉ・リクスレジスタ７７中に保持される。さら
に、乗算・加算器７８においてこのマトリクスの各成分
の自乗用が計算され、その結果であるＶ−バッファ・レ
ジスタ７９中に保持され、そこからそれがマイク［］コ
ンピュータに出力される。

ざらに第２７Ｃ図を参照して、他の７１〜リクス乗算プ
ロセスとその結果につき説明する。マトリクス＠律プ［
」レスにおいて、７１−リクスレシスタ６８からデータ
を供給する順序を制御りることにより、マトリクスレジ
スタ７２中にはみかりし転置マトリクスΔ　があったに
うにすることができる。マトリクスマルチプレクリ−７
３は７１ヘリクスレジスタ７７側に切換えられる。７１
〜リクス乗筒器７４において前述した（３６）式の演算
が行なわれ、その結果がマトリクスレジスタ７５中に保
持される。

最後に第２７Ｄ図を参照して、他のシフ１−プロセスと
その結果につき説明する。マトリクスレジスタ８８から
出力は与えられず、それゆえ乗算・加算器７８において
は上）ホした（３６）式の右辺の各成分の自乗用が計算
され、その結果であるＵｏがバッファレジスタ７９中に
保持される。このヒ０はマイクロコンピュータに与えら
れ、マイクロコンピュータにおいて先の演算過程でめた
ｒとこ（豹 のしとを用いて（３３）式のλがめられる。一方、バッ
ファレジスタ６９中にはマイクロコンピュータから与え
られたλ　が保持されでいる。

（ｓ−１＞は前回の演算結果を意味している。これは、
今回の演算結果が間に合わないために前回の演算結果を
用いるものであるが、大ぎな問題はない。マトリクス乗
算器７０において先に説明した（３７）式の第２項の演
算が行なわれ、さらにマトリクス加算器７１にＪ３いて
（３７）式の演算が行なわれその結果がマトリクスレジ
スタ７２中に保持される。

したがって、以上に説明したすべての演算を行なうこと
により、マトリクスレジスタ７２の出力部に、前回の演
算によるマトリクス［Ｂ］を初１１１１値とした、マト
リクス［Δ］の逆マトリクスの新しい近似マトリクス［
Ｂ１を得ることができる。

それゆえ、マトリクス［Ａ］をレコピアンＪとりれば、
すなわちマトリクスレジスタ６８がマトリクスレジスタ
６７からＡ７コビアンＪを受取るようにずれば、マトリ
クスレジスタ７２の出力部に１７コビアンインバースＪ
−’を得ることがてきる。

以上のようなりコピアンインバース、ノー１をもしもコ
ンピュータのソフトウェアを用いてめるとずれば、その
演算時間は非常に良いしの、たとえば３ＱｍｓｃｃＰｉ
！度となるが、この実施例のように専用のハードウェア
によってめる場合には演算時間が非常に短く、たとえば
１　ｍ　ｓｅｃあるいは−ぞれ以下になることが確認さ
れている。

次に、ｄ、Ｘからｄαへの座標変換の方法おＪ、びその
ための装置につき説明する。これは、第１Δ図および第
１Ｂ図の座標変換部６に相当する。

明するためのブロック図である１、この図は、第２１３
図に相当づる。座標変換部６は、前述した１ノコヒアン
演算部９０．ヤコビアンインバース演ｔ）部４００に加
えてさらにバッファレジスタ６０．ン１ヘ９９２乗算器
６１ｄ３よびバッファレジスタ６２を備える。バッファ
レジスタ６０の入力部はマイクロコンピュータ（図示せ
ず）に接続されており、その出力部は７１〜リクス乗算
器６１の一方の入力部に接続されている。マトリクス乗
算器６１の他方のパノノ部は前述したマトリクスレジス
タ７２の出力部に接続されている。マトリクス乗算器６
１の出力部はバッファレジスタ６２の入力部に接続され
てｄ３す、バラノアレジスタ６２の出力部はマイクロコ
ンピュータに接続されでいる。

マトリクス乗算器６１は、前述したマトリクス乗算器４
１と同様の機能を有しており、２つの７１へりクスの乗
算を行なう。ずなわら、後述するように、バッフフッレ
ジスタ６０からはベクトルｆｆ１ｄ→ Ｘが、マトリクスレジスタ７２がらはマトリクス［Ｊ（
α）］−１がそれぞれ出力され、それらの積がマトリク
ス乗算器６１においてめられるのであるが、ベクトルｆ
ｆ１ｄＸど？トリクス「Ｊ（α）１→　−→→→ −［Ｊ（α）］−’Ｘ　［ｄ　Ｘ　０００］と書くこと
ができる１、シたがって、バッファレジスタ６０に保持
されているベクトル徂ｄＸを次のマトリクスとみなずこ
とがてきる。

したがって、マトリクス乗算器６１の（幾重はマトリク
ス乗算器４１の機能ど同様であると言っても差支えない
。このことは、後述りるマトリクス乗樟器５８ｄ３よび
６５について６同様である。前ｊホしたように、ヤコビ
アン演算部９０内の７１−リクスレジスタ６７の出力部
にヤコビノｌンＪ（α）が与えられ、かつ、レコピアン
インバース演算部４００内のマトリクスレジスタ７２の
出力部にＡ７］じアンインバース［Ｊ（σ）］−１が与
えられる。

一方、ホストシステム４内のマイクロコンピュータから
バッファレジスタ６０に対してｄＸがｉｊえられる。マ
トリクス乗算器６１において、前）ホした（１８）式の
演算が行なわれその結果がバッフスフレジスタ６２中に
保持される。以上に、よって６以上の座標変換装置によ
れば、座標変換に要する時間は１　ｎｌ　ｓｅｃ以下に
なる。従来にｄ３いては、油筒速度があまりにも遅いた
め、５次元あるいは６次元にＪ３いてこのＪ：うな座標
変換を行なうことができなかった。またソフトウェアを
用い゛ζ座標変換を行なう場合には演算時間が２０ｍ５
ｅｃ程度要していたけれども、この実施例によれば前）
ホしたように演算時間が１１１１ｓｅｃ以下となり、ラ
フ１〜ウエアよりｂ　ｉｌｉ’ｌ　ＩＩＩ性能がはるか
に向上する。これは、＼７コビアンＪ（α）をめるプロ
セス、Ｖコピアンインバース［Ｊ（α）］−１をめるプ
ロセスａ３よびＰｌｉ標変換のプロレスの各ブＬＩＬ７
スでの演算速度が非常に速いからである。

次に、ｄ２Ｘから（１’αへの座標変換の方法おにびそ
のための装置につき説明する。これは、第１Ａ図および
第１Ｂ図の座標変換部７に相当する。

ｄ２αは、後で説明づる運動方程式の逆問題を解いてフ
ィートフォワード制御を行／、にうために必要である。

まず、ｄ２Ｘからｄ２ｑに変換づるツノ法につき説明す
る。フィードフォワード制御とは、各関節のｉＩ！ｌＩ
す、したがってエンド１フエクタの動きを指示し、その
動きを達成するためにどのような力を与えればよいかを
演算し、そのツノを発と１さけるようにするものである
。したがって、ｄ２α、ｄαおよびα、すなわち、’ｃ
＋’、ｃｙ、α　、というα系Ｃの運動を記述するＭを
与えてそのＥ）＋さを実現するためのトルクτを演算し
なりればならない。

ところで、ロボットの応用側の要求としては、エンドエ
フェクタの動きは絶対座標系て゛与えることが便宜であ
る。このためｄ　２　Ｘ、　（Ｉ　ＸηなわちＸ、Ｘの
形でエンドエフェクタの先端の仙さ゛を定数とする。こ
のうち文からみへの変換は既に述べたので、ここではＸ
からαへの変換について説明する。

まず、マトリクス［Ｍ］を時間で微分するととなる。こ
れの右辺の第２項を左辺に移行すればとなる。ところで
、（４，０）式の左辺の第１項は［Ｍ］の２階の時間微
分であって、これはＸに関コ 係ηる吊である。左辺の第２項のうちαについて八　二 はＸ→αからめられる。ところで（４０）式の右辺は 二 ［Ｊ］×α　・・・（４１） と占けるから、（４０）式の両辺に左からヤコビアンイ
ンバース［Ｊ１］を１卦ければ ＝ｄ　・・・（４２） が得られる。したがって、（７Ｉ２＞式の左辺の各基。

量を演算すれば右辺のαが得られる。（４２）式はベク
トル形式で書かれており であるから、（４３）式は連立り程式である。

（４３）式の左辺の諸量のうち、ΣΣすなわら、を新た
にめることが必要である。ここて（４４）式の各項を１
−１．とどＵ）りば、（４４）式は、モジ比よ　・・・
（４６） ただし、 う゛ １−Ｉ　Ｌよ−（？、よｉ［Ｍ］）α、αＪ　・・・（
４７）Ｈ，、＝　ＨｊＬ　・・・（４８） となる。この１」、ｄを用いて（４４）式、ずなわち（
４６）式を具体的に書き下せば、 Σ　Σ　−２Ｘ［−ルー　Ｈｕ＋　ｌ］、よ＋　Ｈ＋３
＋　ＨＩ３十　ト１．．＋　ｆ：４Ｉ６十丁＋−１−−
　１−ら３＋ｌ　’ｘｉ’＝　ｊ％＋”ｘｂ＋−ｙ　＋
−１，十　Ｈ，、＋　１−一目、６十丁Ｈ，，４−１−
１−１４，−１−Ｈ−＋ＴＨ，すＨヶ。

十丁Ｆ１６ｃ　］ ・・・　（４９） となる。ここでは、（４８）式の関係を利用している。

（４９）式の各項の一例を具体的に示Ｉば、Ｈ，、＝　
（Ｍ　、αｌ　’）Ｍ２　Ｍ３Ｍ４　Ｍｓ　ＭＧｌ−１
，□−＝（Ｍ’、ル、）（Ｍ′２ル２　）Ｍ−Ｍ２Ｍ５
Ｍ６（゛ある。（４６）式は３６個の項を含み、これが
（４９）式にｄ３いては、（４８）式の関係を用いて２
１個の項へど四替えられている。ところが、これらの項
の間には一定の規則性があり、Ｍ、ないしＭＧ　、Ｍ＝
　’　ないしＭ　Ｉｉ　’　、　Ｍ　＋　″ないしＭ６
″を規則的に組合わせることによって比較的簡単に（４
９）式の値を得ることができる。

次に、以上に説明した方法を実現してｄ２Ｘがらｄ２α
に変換する装置につき説明する。

第２９Ａ図は、（４４）式に示したΣΣの演算を行なう
装置を説明するためのブロック図である。

この図は、第２Ａ図に相当する。点線で囲んだ部分がΣ
Σ演算部５００である。第２９１３図は、ｄ→ ２αの演算を行なう装置を説明するためのブロック図で
ある。この図は、第２Ｂ図に相当リ−る。点線で囲んだ
部分がｄ２α演算部６００である。ΣＬ演算部５００お
よびｄ２α演粋演算００で第１Ａ図Ｊ３よび第１Ｂ図の
座標変換部７が構成される。

第３０Ａ図および第３０８図は、ΣΣ演算部５００の動
作を説明するための図である。。

まず第２９Ａ図を参照しＣ１バツフノ・レジスタ１つの
入力部はマイクし］コンビコーク（図示せず）に接続さ
れており、さらにその人力部と出力部は結合されている
。バッファレジスタ１９の出ツノ部は、マトリクス乗算
器２０の２つの人力部およびマトリクスマルチプレクザ
２２の一方の入力部に接続されている。マトリクス乗終
器２０の出力部はマトリクス乗算器３０の一方の人力部
に接続されており、マトリクス乗算器３０の４ｍ　７’
ｊの入力部にはマトリクスシフトレジスタ３５の出力部
が接続されている。マトリクスマルチプレクサ２２の出
力部はマトリクス乗算器２３の一方の入力部に接続され
ており、マトリクス乗算器２３の他方の入力部はマトリ
クスシフトレジスタ３２の出力部に接続されている。マ
トリクス乗算器２３の出力部は、マトリクス乗算器２４
の一方の入力部およびマトリクス乗算器３４の一方の入
力部に接続されている。マトリクス乗算器２４の他方の
パフ３部は、マトリクスレジスタ４３の出力部に接続さ
れている。マトリクス乗算器２７の一方の入力部はマト
リクスシフトレジスタ２５の出力部に接続されており、
他方の入力部はマトリクスレジスタ２６の出力部に接続
されている。マトリクス加算器２８の２つの入力部はそ
れぞれマトリクスレジスタ２４の出力部およびマトリク
ス乗算器２７の出力部に接続されており、マトリクス加
算器２Ｂの出力部はマトリクスレジスタ２９の入力部に
接続されており、マトリクスレジスタ２９、の出力部は
マトリクスレジスタ２６の入力部に接続されている。

マトリクス乗算器３１．３４および３６．マトリクス加
算器３３および３７およびマトリクスレジスタ３８．１
−ｉよび３９の接続については前述とほぼ同様である。

マトリクスレジスタ３８の出力部にはバッファレジスタ
４０の入力部が接続されている。

マトリクスシフトレジスタ２５，７１−リクス乗算器４
１．マトリクスレジスク４２および４３については、第
６図において座標変換部５としＣ既に説明した。さらに
、７１−リクスシフトレジスタ３２についても、第２４
Ａ図においてヤ」ビアン演算部９０の一部分として既に
説明した。マトリクスシフトレジスタ３５の機能は、既
に説明したマトリクスシフ１〜レジスタ２５および３２
の機能と同様である。マトリクス乗算器２０，２３．２
４．２７．３０，３１，３４ゼよび３６は、既に説明し
たマトリクス乗算器４１の機能と同様である。マトリク
スマルチプレクサ２２の機能は、第２４Ａ図において説
明したマトリクスマルチプレクサ４５の機能と同様であ
る。マトリクス加算器２８．３３および３７の機能は、
第２６図において説明したマトリクス加算器７１の機能
と同様である。マトリクスレジスタ２９および３８の機
能は、既に説明したマトリクスレジスタ４２の機能と同
様である。７１−リクスレジスタ２６おにび３９の機能
は、既に説明した７１〜リクスレジスタ４３の機能と同
様である。バッファレジスタ１９および４００機能もま
た、第６図において既に説明したバッファレジスタ４４
の機能と同様である。

次に第２９Ｂ図を参照して、バッフルジスタロ３の入力
部はマイクロコンピュータ（図示せず）に接続されてお
り、その出力部はマトリクス減算器６４の一方の入力部
に接続されている。マトリクス減界器６４の他方の入力
部は前述したパン７戸レジスタ４０の出力部に接続され
ている。マトリクス減算器６４の出力部はマトリクス乗
算器６５の一方の入力部に接続されてＪ３す、マトリク
ス乗算器６５の他方の入力部は前述したマトリクスレジ
スタ７２の出力部に接続されている。マトリクス乗算器
６５の出力部ｔまバッファレジスタ６６の入力部に接続
されており、バララフ・レジスタ６６の出力部はマイク
ロコンピュータに接続されている。

バッファレジスタ１９は、マイクロゴ１ンピユータから
！ｊえられたα、ないしα６を保ＪＪＩシ、かつそれら
をローチー１〜させている。７１〜リクス減（？Ｉ器６
１１の機能は、第２６図において説明したマトリクス減
算器７６の機能と同様である。パラフン・レジスタ６３
．マトリクス乗算器６５およびバッフ７レジスタ６６の
機能についても、第２８図において説明したバッファレ
ジスタ６０．マトリクス乗算器６１およびバッファレジ
スタ６２の槻ＯＬとそれぞれ同様である。

次に、第３０Ａ図および第３０１３図を参照して、マト
リクスレジスタ３８内に前）本したΣΣかロードされる
動作につき説明する。なお、以下の動作は、前述したク
ロック信号φΔＪ５Ｊ、びφＢに応答して行なわれる。

ま°ｆ３０Ａ図を参照して、第１のユニットタイム間の
動作につき説明する。バッファレジスタ１９に保持され
ていたα、がマトリクス乗算器２０に与えられ、マトリ
クス乗算器２０において工α、′がめられる。マトリク
スマルチプチレクサ２２はバッファレジスタ１９側に切
換えられており、これ以降も同様である。７１〜リクス
シノトレジスタ２５内のマトリクスレジスタ２５１ない
し２５６には、マイクロコンピュータからそれぞれ７１
〜リクスＭ１ないしＭ６がＯ−ドされる。マトリクスシ
フトレジスタ３２内のマトリクスレジスタ３２１ないし
３２６には、マイクロコンピュータからマトリクスＭの
１階の時間微分であるマトリクスＭ、′ないしＭ　［ｉ
′　がロードされる。同様に、マトリクスシフトレジス
タ３５内の７１〜リクスレジスタ３５１ないし３５６に
は、マイクロコンピュータからマトリクスＭの２階の時
間微分であるマトリクスＭ　、　ｎないしＭ６″がロー
ドされる。マトリクス乗算器２３においてマトリクスマ
ルヂプレクサ２２からのα１とマトリクスシフトレジス
タ３２からのＭ＋’　との積がめられる。

マトリクスレジスタ４３には単位マトリクスＥがロード
されている。したがってマトリクス乗算器２４において
はＭ＋’　α１がめられる。マトリクスレジスタ２６に
はＯがロードされており、したがってマトリクス乗算器
２７での演算結果はＯどなる。マトリクス加綿器２８に
おいＣマｔ・リクス乗算器２４からのＭ、′α、と７１
・り久ス東算器２７からのＯとが加算されその結果がマ
トリクスレジスタ２９にロードされる。

マトリクス乗算器３０において、７１〜リクス乗算器２
０からの４．、．２とマトリクスシフトレジスタ３５か
らのＭ１″との乗粋が行なわれその結１　＋ 果Ｔ　Ｍ　、　ＩＩαＩ２がめられる。マトリクス乗算
器３１においては７１〜リクス乗綽器３０からのｉＭＩ
″α１′とマトリクスレジスタ４３からの単位７１〜リ
クスＦとの積がめられる。７１〜リクス乗算器３４の一
方の人力はマトリクスレジスタ２６からのＯであるので
、そこでの演算結果はＯである。同様に、マトリクス乗
算器３６の一方の入りはマトリクスレジスタ３９からの
ＯＣ゛あるのｃｌそこでの演算結果は０である。したが
ってマトリ１　１．　。

クスレジスタ３８にはＴＭ、α、２がロー１：される。

次に第３０１３図を参照して、第２のユニットタイム間
の動作につき説明する。バッファレジスタ１９は、その
保持していた内容を１ブロツクｌどＩｆＯ−テートさせ
、れを出力する。マトリクスシフ１〜レジスタ２５．３
２６よび３５にロードされているマトリクスは右方向へ
１ブロツクだけシフｉ〜される。しｌ〔がって、７１〜
リクスレジスタ２５１．３２１Ｊ５よび３５１には、そ
れぞれ、マド１ノクス１ｙ１２．Ｍｚ’　およびＭ２″
がロードされる。

マトリクスレジスタ２６には、第１のユニットタイム間
の演算結果であるＭ、′α１がマトリクスレジスタ２９
からロードされる。同様に、マドｌ）ｒ　ｒｒ　＋ クスレジスタ３９にはｚ　Ｍ　＋　α、′がロードされ
、７１−リクスレジスタ４３にはＭ、がロードされる。

そして前述と同様の演算により、マトリクスレジスタ２
９にはＭ　＋　Ｍ　２　’　α２＋Ｍ、’　α。

Ｍ２がロードされる。マトリクスレジスタ３８には、　
ＭＩ　Ｍ２　″（Ｘ２　’　＋Ｍ＋　’　α１Ｍ２′α
２λ ＋’Ｍ、″Ｑ’Ｈ２Ｍ２がロードされる。マトリクスレ
ジスタ４２には、ＭＩＭ２がロードされる。

以降同様にして第６のユニットタイム間］ツブ前述した
ΣΣがマ］〜リクスレジスタ３８中【二ロードされる。

次に第２９Ｂ図を参照して、その次のにツ１〜タイム間
の動作につき説明Ｊる。マイクＥ−Ｊ」ンビュータから
はバッファレジスタ６３に対してｄ２Ｘが与えられ、こ
れがさらにマトリクスｉ＋ｉｌｉ　Ｏ器６４の一方の入
力部に与えられる。一方、マトリクス乗算器６４の他方
の人力部には、バッファ・レジスタ４０を介して、先の
ユニットタイム間にめられたΣΣが与えられる。７１〜
リクス減紳器６４において、先に説明した（４０）式の
左辺の演算が行なわれその結果がマトリクス乗瞳器６５
の一方の入力部に与えられる。マトリクス乗算器６５の
他方の入力部には、７１へリクスレジスタ７２ｈＩらψ
コピアンインバースＪ−’がｈえられる。こσ）ヤコビ
アンインハースＪ−’のめ方につ（＼て（よ既に説明し
た。マトリクス乗算器６５において先に説明した（４２
）式の演算が行なわれ、その結果であるｄ２αがバッフ
ァレジスタ６６に［Ｊ−ドされる。このｄ２αはさらに
マイクロコンビコータの変換が行なわれたことになる。

運動方程式 動方程式の逆（ｄ題を解く方法およびそのための装置に
つＪ説明する。これは、第１Ａ図および第１Ｂ図の運動
方程式演算部９に相当する。

運動方程式の逆問題とは、対象としている力学系の運動
を条１′１どして与え、その運動を実現づるためにはい
かなる力をその力学系を加えればよいかをめる手続をい
う。ここでは、運動方程式を導くためにラグランジェ形
式を採用覆ることにする。

ラグランジェ形式においては、まず、考えている力学系
の運動エネルギをに、ポテンシャルエネルギをＰどして
、ラグランジアンＬを Ｌ＝に−Ｐ　・・・（５０） によってめる。ラグランジェの方程式には一般化座標が
現われるが、ここで考えている一般化座標はα、Ｃある
。このα１に対する一般化力をＦｌとづると、ラグラン
ジェの）■動方稈式は（ｉ　＝１．２．・・・、６） で与えられる。運動方程式の逆問題を解くということは
、（５１）式の右辺を計算してＦ、をめることに他なら
ない。

次に、ＫおよびＰを具体的にめ、（５１）式の具体的な
形を導くこととする。

＜ａ　＞　運動エネルギ１く 絶対座標系におけるロボットの各部分の座標をとし、ロ
ボッ１〜の各部分の微小部分の貿ｍをｄｍとすると、全
運動エネルギには に一÷Ｊｄｍ（ｘ　２＋ｙ　２＋ｚ　２）　＋Ｒ−Ｎｉ
３）で与えられる。但しＲは、ロボットの腕の位置状態
に依存しない項であり、たとえばローターのイナーシ１
７等である。ここで、 どなることに注意すると、 となる。但し、］”ｒはトレース（対角用）を意味ηる
。この積分にａ３いて、絶対座標系から児た１番目の腕
の各部分の座標を×１とすれば、（５５）式は、６腕に
ついての積分の和として書換えることができ、 Ｋ　、、、−４ΣＪｄｍＴ　ｒ　（：、”；、”）　十
Ｒ−（５６）となる。ところで、Ｘ、は、 Ｘ　Ｌ　＝Ｍ＋　ＸＭ２　Ｘ・・・ＸＭ、Ｘｏ　・・・
（５７）とａ目ノる。但し、Ｘｏは、［番目の関節に固
定された座標系から見たＬ番目の腕の各部分の位置ベタ
１−ルである。したがって、 Ｍ’　−Ｍ＋　ＸＭ２　Ｘ・・・ＸＭ、　・・・　（５
８）なる１いを定ＩＪれば、ｘＬ　＝　Ｍ　ＸＣＩであ
るから、 肖　ａ　、→・ ｘＬ−π（ＭＸＯ） と書くことができる。また、Ｒは（１、／　２　’）Σ
Ｉａ、αｊ　（ａｉ　は定数）の形に町１ノるから、・
・・　（６０） となる。但し、Ｑｌは慣性テンソルであり、で定義され
る。

（ｂ　）　ボテンシＶルエネルギＰ ポテンシャルエネルギとしては、重力エネルギが考えら
れる。したがって、重力の加速度を表わす゛ベタ１ヘル
をＧとし、重力の方向に絶対軸の第３成分をとれば、 （１−Ａ下金白） となる。重力マトリクスＹ１を ・・・（６３） によって定義すれば、Ｐは １丁 Ｐ＝ＩＴｒ　ＬＹ、Ｍ　］　・・・（６４）と古くこと
ができる。

（Ｃ）　ラグランジェの運動方程式 （ａ　）　＜ｂ　）で用いたに、Ｐを（５０）式に代入
してＬをめ、これを（５］）式に代入することによって ＋ＩＱ−＊伏− ・・・　（６５） が得られる。これを整理すれば 十Ｉｏ−メ・ し ・・・　（６６） てあって、（６７）武力よび（６８〉式の各項をそれぞ
れＲ：、、、ｗ：）、＝ｓけば、これらの式は具体的に
は次の形となる。

Ａ　＋　−２×［２Ｒ，、］ １　（２）　ｔり） Ａ２　＝２Ｘ　［ＩＲＩｔ　十Ｒ１：Ｌ十工１ち、〕 Ａ３　＝　２×［−！−Ｒ′？〜−Ｒ′？ゝ＋Ｒ″′λ
　ｌｌ　ｌｌ　１３ ’　（７１ｆｉｌ −ｈ　Ｔ　ＩでＪＡ　＋Ｒ１３ Ｂ　ｌ＝　Ｗ　＋　” Ｂ２　＝Ｗ＋”＋Ｗ２” 〕こ　Ｉど　じ　、 であり、＜−Ｒ謂が成立している。

（６Ｇ）式のうち第１項は角度変数の時間微分の２次形
式を含む項であって、遠心力Ｊ３よびコリオリのノｊを
表わし、第２項は角度変数の２階の時間微分を含むため
に慣性項である。第３項は重力項そして第４項は先に述
べたようにロボットの腕の位置状態に依存しない項であ
る。

ところで、（６６）式に従って演算を行なえば、一般化
力Ｆ１を得ることができるわけであるが、この演算をハ
ードウェアで行なうことを考慮して、漸化式による方法
を採用することにする。すなわち、 ａＬ、−ａＪ−１×ＭＪ、ａｏ＝Ｆ　・・・（７ｏ）と
して、繰返しを行なえば ａｊ　＝Ｍ、×Ｍ２×・・・×ＭＪ・・・（７１）がｉ
ｔｌられる。まＩこ、 ・・・（７２） としてｊについて繰返しを行なえば １）、＝Ｍ、ＸＭ２Ｘ・・・ｘＭ、’ｘ・・・×Ｍ〜よ
　ｄ ・・・（７３） が得られる。さらに、 　− ＋　ａｊ−’　ｘ　２　α、ｉ　Ｍ；’　・ｄ　ｏ　＝
　０・・・（７４〉 によって繰返しを行なえば、同様に（６６）式の各項に
現われる量をめることができ、結局、Ｆ＋　＝Ｆ＋　’
　＋ｌ”、　’十Ｆ、３＋ｌａ、α。

のようになり、これによってＦ、をめることができる。

このうちＩａ＋　α１の項はロボットの腕の位置状態に
依存しない項であるためその演算はソフトウェアで行な
い、ハードウェアではこれを無視することとする。

すなわち、Ｆ、をめるにあたっては、（７５）式の「＝
’、Ｆ１２．’Ｆ１３の（）の中にあるマトリクスの積
を計算し、そのトレース（対角和）をめて和をとればよ
い。ここで対角和をとるべき（）の中がＡ−Ｂの形をし
ていることに注目すると、対角和の演算はマトリクスＡ
およびＢの要素を抜き出してそれらの積の和をとればよ
いことになる。これは次式より明らかである。

次に、前述した（７５）式のＦ＋’＋Ｆ＋’＋Ｆ　、　
ｓをめる装置につき説明する。そのような演算を行なう
ためには先に示した第２Ａ図内のすべての装置を用いる
。第２Ａ図内の装置のほとんどは既に説明したので、こ
こではまだ説明していない部分についてのみ説明する。

マトリクスシフ１〜レジスタ５０の出力部がマトリクス
乗算器５１の一方の入力部に接続されている。マトリク
スシフトレジスタ５０は、先に説明したマトリクスシフ
トレジスタ２５．３２および３５と同様であり、マトリ
クスレジスタ５０１ないし５０６を備える。マトリクス
乗算器５１の他方の入力部にはマトリクスマルチプレク
サ４９の出力部が接続されてＪ５す、マトリクスマルチ
プレクサ４９の２つの入力部は先に説明したマトリクス
レジスタ２６および３９の出力部に接続されている。マ
トリクス＠枠器５１の出力部は、マトリクスレジスタ５
２を介してマトリクスマルチプレクサ５４の一方の入力
部に接続されている。マトリクスマルチプレクサ５４の
他方の入力部には、マトリクスシフトレジスタ５３の出
力部が接続されている。このマトリクスシフ１〜レジス
タ５３もマトリクスシフトレジスタ５０と同様に、マト
リクスレジスタ５３１ないし５３６を備える。マトリク
スマルチプレクサ−５４の出力部はトレース演算回路５
５の一方の入力部に接続されＣおり、それの他方の入力
部には先に説明したマトリクスレジスタ４８の出ツノ部
が接続されている。トレース演算回路５５の出力部はバ
ッフ７レジスタ５６を介してマイクロコンピュータ（図
示ぼず）に接続されている。

７１〜リクスシフトレジスタ５０および５３の機能は、
既に説明したマトリクスシフトレジスタ２５等の機能と
同様である。マトリクスマルチプレクリ“４９および５
４の機能は、既に説明した７１〜リクスマルヂプレクリ
４５等の機能と同様′ｃ′ある。

マトリクス乗算器５１の機能は、既に説明したマトリク
ス乗算器４１等の機能と同様である。マトリクスレジス
タ５２の機能は、既に説明した７１ヘリクスレジスタ４
２等の機能と同様である。バッフルレジスタ５６の機能
は、既に説明したバッファレジスタ４４等の機能と同様
である。トレース演算回路５５の機能ば、（７５）式に
示したようなトレース７ｒをめるものである。

次に、運動方程式を解く動作につき説明づる。

第３１Ａ図ないし３１０図は、運動方程式演粋部９の動
作を説明りるための図である。運動方程式を解く動作は
、３つのフェースから／、ｆる。

まず、第３１Ａ図を参照して、第１のフ」、−ズにつき
説明する。このフェースにおいては、Ｆ１３の演算の他
に、同時に、先に説明したαからＸへの変１．ｄ２αを
めるためのΣとの計算およびＡ７コビアンＪの係数をめ
る動作も行なわれている。これは、演算の時間を最短に
するためである。マトリクスジノ１〜レジスタ２５内に
は先に説明したようにマトリクスＭ、ハロードされ（（
′Ｘる。

７１〜リクスシフ１〜レジスタ３２内には、先に説明し
たマトリクスＭ・′がロードされている。７１・リクス
マルチブレクリ・４５は１，１＝−ｉのときはマトリク
スシフトレジスタ３２側に切換えられ、ｊ≠１のとさ−
はマトリクスシフトレジスタ２５側に切換えられる。マ
トリクス乗算器４７においてマトリクスＭ−とマトリク
スＭｊ′との乗粋が（７δ し ２）式に従って行なわれ、その結果であるｂ♂　がマト
リクスレジスタ４８にロードされる。一方、マトリクス
シフトレジスタ５３にはマイクロコンピュータ（図示せ
ｆ）から先に説明したマトリクスＹ、がロードされてい
る。マトリクスマルチプレクサす５４はマトリクスシフ
トレジスタ５３側に切換えら　でおり、したがってトレ
ース演算回路５５の　・部にはｂ″：　とＹ・　が入力
され、そこに／　、ｄ おいて（７５）式のＦ　、　３の演算が行なわれる。

この場合、Ｆ　、　３は３６のりＯツク期間（１−１な
いし６．ｊ＝１ないし６）でめられる。この場合、最初
のクロック期間ではまだマトリクスレジスタ４８にはい
かなるｂも［１−ドされていないので、マトリクスＹ２
の出力は１クロツタ期間だけと 遅延さＵ゛る必要がある。それには、マトリクスシフｌ
〜レジスタ５３とマトリクスマルチプレクサ５４との間
にそのような遅延回路を段【プてもよいし、あるいはそ
のようにしなくてもマトリクスシフトレジスタ５３の動
作を１クロック期間だけ止めてもよい。

なお、以上の演算と同時に次の演算が１１なわれる。す
なわち、バッファレジスター９にはｄがロードされてお
り、この７１−−スにあい（（，１７１ヘリクスマルヂ
ブレクサ２２はバッファレジスター９側に切換えられて
いるので、ルは７１−リクス乗尊器２３に与えられる。

したがって、マトリクス乗算器２３においては、ａ、　
Ｍｚ　’　の演算が行なわれそれがマトリクス乗算器２
４の一方の入力部に与えられる。マトリクス乗紳器２７
′ｌの他方の入力部には、前のクロック期間中にめら１
′１だ後述するａ４−１がマトリクスレジスタ４３から
与えられる。

７１〜リクス乗粋器２７の一方の入力部には、７１〜リ
クスシフトレシスタ２５から７１−リクスＭ、２が与え
られ、他方の入力部にはマトリクスレジスタ２６中に保
持されている前回のクロックル１間中にめられたＣ、−
１が与えられる。したが・〕で７１〜リクス乗算器２７
およびマトリクス加紳器２８にｄ３いて（７４）式に示
したＣ１の演算が行なわれそこ の結果がマトリクスレジスタ２９にロードされる。

一方、マトリクス乗ｎ器４１の一方の人力部にはマトリ
クスシフトレジスタ２５からのマトリクスＭ　が与えら
れてあり、他方の入力部にはマトリクスレジスタ４３中
に保持されている前回のクロック期間中にめられたａＪ
−、が与えられる。したがって、マトリクス東棹器４１
にａ３いては（７０）式に示した演算が行なわれ、その
結果がマトリクスレジスタ４２に［１−ドされる。この
マトリクスレジスタ／４２にロードされるａ２　は、言
い換えれこともできる。さらに、マトリクス乗算器２ｏ
においては（１／２）ｃｒ２がめられこれが７１へりク
ス乗粋器３０の一方の人力部に与えられる。マトリクス
乗算器３０の他方の入力部には７１〜リクスシフトレジ
スタ３５からのＭ、″が与えられる。

前述と同様の考えで、マトリクス乗算器３ｏおよび３１
．マトリクス加算器３３．７１−リクス乗錦器３４ａ３
よび３６．マトリクス加算器３７において（７４）式に
示したｄ、をめる演算が行な４つれ、その結果がマトリ
クスレジスタ３８にロードされる。このｄオ　は、言い
換えれば先に説明したＩΣということもできる。

次に、第３１Ｂ図を参照し−Ｃ，第２のフｒ−ズにつき
説明する。７１〜リクスシフｊ〜レジスタ５（ンには、
マイクロコンビコータ（図示せり゛）から先に説明した
慣性テンソルＱＪ力和−ドされている。

マトリクスシフトレジスタ５０ど７１ヘリクス乗算器５
１との間には、第３１△図で説明したＪ、うな遅延手段
が段りられてし良い。そのＪＩＰ山しまた先に示した通
りである。マトリクスフルヂブレクリ４９は、このフェ
ーズにおいＣはマトリクスレジスタ３９（則に１刀１灸
えられてＡ３つ、したがって７１−リクス乗幹器５１に
おいては、−／ｌ−リクスシ７１へレジスタ５０からの
Ｑよとマトリクスレジスタ３９からのｄＪ　どの積がめ
られる３、ン１〜リクスマルチブレクサ５４は、このフ
ェースにおいては７１ヘリクスレジスタ５２側に切換え
られ−Ｃおり、したかつてトレース演算回路５５にＪ５
いては、７１−リクスマルチブレクリ−５４からのｄ、
Ｑオ　と７１〜リクスレジスタ４８からのす、、とを用
いて（７５）式に示したＦ、′をめる演算が行なわれる
。このＦ、２をめる演算もまた、３６のクロック期間（
ｉ−１ないし６．ｊ　＝１ないし６）において行４にね
れる。

さらに、第３１Ｃ図を参照して、第３のフェーズにつき
説明する。バッファレジスタ２１にはδ、がロードされ
ている。このフェーズにｄＪいては７１へりクスマルチ
ブレクサ２２はバッファレジスタ２１側に切換えられ“
ＣｄＪ３す、したがってん・はげ ７１゛リクス乗算器２３の一方の入力部に与えられる。

７１゛リクス乗算器２３の他方の入力部にはマトリクス
シフトレジスタ３２からのＭｚ　’　ｈ鴨えられる。前
述と同様の考え方にＪ二り、マトリクスレジスタ２９に
は（７４）式に示したＣよ　がロートされる。このフェ
ーズにおいては、第２のフェーズの場合と反対に、マｌ
〜リクスマルチプレクサ４９はマトリクスレジスタ２６
側に切換えられている。マトリクスマルヂブレクサ５４
は、このフＬ−ズにａ３いても７１〜リクスレジスタ５
２側に切換えられＣいる。したがって、マトリクス乗算
器５１にａ３いて、マトリクスシフトレジスタ５ｏがら
の慣性テンソルＱ−と７１〜リクスレシスタ２６−δ からのＣ，３との積がめられ、その結果（゛あるＣよＱ
ｊがトレース演算回路５５の一方の人力部に！ｊえられ
る。トレース演算回路５５の他方の入力部にはマトリク
スレジスタ４８からの１亡　が！うえらよ れており、１−レース演算回路５５に６−３いて（７５
〉式に示したｌ−％をめる演算が行なわれる。こ０）Ｆ
ｉ’　をめる演算もまた、３６のクロック期間（１−１
ないし６．ｊ−１ないし６）において行なわれる。

以上のような動作によってめた「、３．Ｆ。

’Ｊ５Ｊ：び１：１′　は、それぞれバッファレジスタ
５６からマイクロコンピュータに与えらね、マイクロコ
ンピュータはそれらのものを用いて（７５）式に示した
１：１　をめる演算を行なう。

従来は、運動方程式を５１算づ−るのには非常に時間が
かかつていた。そのｆｆｔ　Ｉ”ｌは実時間（゛は不可
能だった。これは、関節が複雑に干渉し合い、運動方程
式の中に座標変換が入ってきてａ３す、運動方程式の計
算には非常に良い時間を要１）ていたからである。した
がって、運動方程式を解くことは、制御）■として実用
に供し１ｑながった。しかしながら、Ｆに説明したよう
なこの実施例による方法ａ５よび装置によれば、運動方
程式を解くことが実時間で可能になった。

ｄＦからｄτへの変換 次に、ｄ「がらｄτへの変換の方法およびそのための装
置につき説明分る。これは、第１Ａ図おする。仮想仕事
の原理を用いると、Ｘ系においてエンドエフェクタに加
える力Ｆとそれによる微小変１１／ｌＪｘがなぜる仕事
δｗｘと、α系にＪ３いて各軸に加えるカ［とそれにに
る微小変位ｄαがなせる仕事δｗ、Ａ　どは等しい。り
なわら、→Ｔ　→ δＷｘ　＝ＦＴ６Ｘ＝δＷ＝ｒ　−ｄ　ａ−＜７６）が
成立する。ここでｄＸは先に（１７）式で示されでいる
がこれを改めて表わせは →　→　→ ｄＸ＝Ｊ−ｄα　・・・（７７） となる。したがって上のく７６）式および（７７）この
（７８）式において、α系にｉＪ５けるカ［はトルクτ
ど表わしてもＪ、く、また（７８）式は微小吊について
も成立するので、（７８）　ｊｔ：は次の（７９）式と
表ゎづ−ことがてさる。

→　→Ｔ ｄ　τ　−Ｊ−ｄ　「　・・・　く　７９　ンしたがっ
て、（１４３）式の演算を行なえば、ｄ［からｄτへの
変換が行なわれることになる。

次に、上に述べたような変換を１’ｉ　イｔう装置につ
き説明する。第３２図は、ｄＴ！がら＋Ｊ？への座標変
換部８を説明するためのブロック図Ｃある。この図は、
第２１３図に相当する。座標変換部８は、バッフ７レジ
スタ５７．マトリクス乗剪’　？ｅに８　＋ｊＪＪ、び
バッファレジスタ５９を備える。バッファレジスタ５７
の入力部は、マイクロコンビ−１−タ（図示せず）に接
続されており、出力部はマトリクス乗算器５８の一方の
入力源）に接続されている。

マトリクス乗算器５８の他方の人力部は、先に説明した
ヤコビアンインバース演算部４００内のマトリクスレジ
スタ６８の出力部に接続されている。

７１−リクス乗算器５８の出力部はバッファレジスタ５
９の入力部に接続されており、バッファレジスタ５つの
出力部はマイクロコンピュータに接続されている。

前述したように、ヤコビアン演算部９０内のマトリクス
レジスタ６７の出力部にはヤコビアンＪが出ツノされ、
ヤコビアンインバース演算部４００内のマトリクスレジ
スタ６８の出力部には、第２７Ｃ図において説明したよ
うに、児か（プ上ヤコビアンＪの転置７１〜リクスであ
るＪＴが出力される。

一方マイクロコンピュータからバッファレジスタ５７に
対してｄＦが与えられる。マトリクス乗算器５８におい
て、前述した（７９）式の演算が行なわれ、その結果で
あるｄτがバッファレジスタ５９にロードされ、かつマ
イクロコンピュータに与えられる。以上によりｄＦから
ｄτへの変換が行なわれたことになる。

以上に述べｌ〔座標変換も非常に高速度で行なうことが
できる。

発明の効果 この発明の１つの特徴は、座標変換は簡単な構成のハー
ドウェアで行なわれるので、座標変換を非常に高速で行
なうことができることである。この演算時間の短縮は、
制御の質的変化を生ずるほどである。

この発明の他の特徴は、座標変換は簡単な構成のハード
ウェアで行なわれるので、マイク［１コンビコータが座
標変換に利用される時間が非常に７．ｊｊくなることで
ある。したがって、マイクロコンピュータに高速のもの
を用いなくても極めて正確な座標変換が行なわれる。さ
らに、そのマイクロコンピュータを用いて別の機能を行
なわけることが簡単にできる。

この発明のさらに他の特徴は、座標変換のための演算回
路はばとん′ど類似した演算を何回も繰返すだ番）であ
り、従って演算回路の構成が簡単になり、かつ、制御す
べき自由度の変更や増減に対しても簡単に対応すること
がＣぎることＣ（ｂる。つまり、汎用性が大きいことで
ある。なぎならば、その演算回路で利用可能な演算機能
を単に増減するだ【ノで、マイクロコンピュータのソフ
トウェア（プログラム）についてはほんどん変更が不要
だからである。このことは、自由度の種類や数の変更に
容易に追随できることになる。

この発明のざらに他の特徴は、運動方程式の逆問題を解
くことによりフィードフォワード制御を行なうことがで
きることである。したがって、エンド１フエクタが所望
のパスを移動するような制御を行なうことができる。

この発明のさらに仙の特徴は、全体シスラームにおいて
は、位置のフィードバックが直角座標系に８３いてか【
プられていることである。一般に直角座標系での位置デ
ータＸを関節座標系での位置データαに変換することは
非常に長い演算時間を要し、実時間ぐそれを行なうこと
は不ｉり能に近い（〕れども、このシステムにおいては
そのような座標変換を用いる必要がない。

この発明のさらに他の特徴は、全体システムにおいては
、力制御と位置制御とを直角座標系で切換えることがで
きることである。したがって、ＲＣＣのようなものをロ
ボット自身ひ実現づることができる。

この発明のさらに他の特徴は、この発明に係る制御ＩＩ
装置を、関節にダイレクトドライブモータを用いｌご多
関節ロボットに用いると、予測制御の効果が顕著になる
ことである。というのは、従来のハーモニックドライブ
に４５いてはてこに抵抗やガタが非常に多くあり、これ
らは運動方程式に組み入れることはでさないけれども、
ダイレクトドライブモータにＪ３いてはそのような１氏
抗や刀夕が非常に少なくなるからである。

【図面の簡単な説明】

第１Ａ図および第１Ｂ図は、それぞれ、多関節ロボッ１
への制御装置の全体システムの構成を概略的に示づ７１
３７９図である。 第２Ａ図および第２Ｂ図は、全体システムの構成を詳細
に示すブロック図である。ここで、第２Ａ図の右端は、
第２Ｂ図の左端につながっている。 第３図ないし第５図は、多関節ロボットの座標変換を説
明するだめの模式図である。 →　→ 第６図は、αからＸへの座標変換装置を説明１′るため
のブロック図である。 第７図は、マトリクス乗算器を説明するためのブロック
図である。 第８図は、マトリクスレジスタの一例を示Ｊブロック図
である。 第９図は、制御装置とマトリクス乗算器との接続を示す
ブロック図である。 第１０図は、７１〜リクスレジスタの運用方法を説明す
るための図である。 第１１図は、演算回路の基本的な考え方を示す概念図で
ある。 第１２図は、一実施例の基礎となる演算回路の考え方を
説明する概念図である。 第１３図は、この発明の一実施例に利用される集積回路
を示すブロック図である。 第１４図は、第１３図の集積回路のより詳細な内部構成
を示す回路図である。 第１５図は、第１４図に示した集積回路を利用した演算
回路のブロック図である。 第１６図は、演算回路を制ｔｉｌｌ−ＪるためのＣＰＵ
関連回路を示すブロック図である。 第１７図は、シフトタイミング発生回路の一例を示ず回
路図である。 第１８図ないし第２０図は、９トリクスの乗算過程を説
明し、シフトタイミング発生回路の考え方を説明する図
であり、特に第１ε３図はマトリクスの乗算の原理を示
し、第１９図は演紳過程に８３けるシフト動作を示し、
第２０図はマトリクスの乗算に必要なシフトタイミング
信号のタイミングチャートを示す。 第２１図は、この発明の一実施例の動作を説明するため
のフローチ１７−トである。 第２２図は、Ｒｅ５ｏｌｖｅｄ　Ｍｏｔｉｏｎ　Ｒａｔ
ｅＱＯｎｔｒＯｌにおける処理演算ループを示づ一６第
２３図は、シフトタイミング発生回路の別の例を示す回
路図である。 第２４Ａ図および第２４Ｂ図は、Ａ７コビアンＪ（α）
をめる装置を説明するためのブｌ」ツク図である。 第２５Ａ図ないし第２５Ｃ図は、Ａ７コビアン演算部の
動作を説明するための図である。 第２６図は、ヤコビアンインバースＪ−１をめる装置を
説明するためのブロック図である。 第２７八図ないし第２７Ｄ図は、ヤコビアンインバース
演算部の動作を説明するための図である。 → 第２８図は、ｄＸからｄαへの座標変換部を説明するだ
めのブロック図である。 第２９Ａ図は、（４４）式に示したΣΣの演算を行なう
装置を説明するためのブロック図である。 第２９Ｂ図は、ｄ２αの演算を行なう装置を説明するた
めのブロック図である。 第３０Ａ図および第３０８図は、ΣΣ演算部のり１作を
説明するための図である。 第３１Ａ図ないし第３１Ｃ図は、運動方程式演算部の動
作を説明するだめの図である。 第３２図は、ｄＦからｄτへの座標変換部を説明するた
めのブロック図である。 図において、４はホストシステム、５〜８および１７〜
１８は座標変換部、１０は関節部、１１はエンコーダ、
１２は夕］メータ、１３は力検出器、１４〜１６はゲイ
ン調整部、Ｂ、〜Ｂ＋ｚおよびｂ１〜ｂ＋＋はバッファ
レジスタ、Ｍ　Ｐ　＋〜Ｍ　Ｐ　４は乗算回路、９０は
ヤコビアン演算部、４００はヤコビアンインバース演算
部、５００はΣΣ演算部、６００はｄ２α演算部である
。 第１３図 ＢＬ Ｏ 第２２図 第２１図 第２３図 ｖ　Ｕ　Ｗ 第１頁の続き ０発　明　者　近　藤　隆　彦　西宮市田近野町ター内 ６番１０７号　新明和工業株式会社開発セン手続補正占
（方式〉 昭和５９年２月９日 ：２．′ハ 特許庁長官殿 １、事件の表示 昭和５８年特許願第　１８３２９７　号２、発明の名称 多関節ロボツ１−の制御方法およびそのための装置３、
補正をする者 事件との関係　特許出願人 住所　兵庫県西富市小曽根町１丁目５番２５号名称　（
２’３５）新明和工業株式会社代表者　玉　河　習　次 ４、代理人 住　所　大阪市北区天神橋２丁目３番９号　八千代第一
ビル１〜、◇、ｉＱ ６、補正の対象 図面の第２１図 ７、補正の内容 別紙添付の図面に朱書したように「第２１図」を「第２
１図その１、その２」に訂正する。 以上

Claims (1)

1. 【特許請求の範囲】 （１）　複数の自由度と前記自由度の１つに結合された
   エンドエフェクタとを有する多関節ロボットのためのも
   のであり、前記自由度の各々を制御することにより前記
   エンドエフェクタの動きを制り（１する制御装置であっ
   て、 前記エンドエフェクタの位置を指令するための位置指令
   データおよび前記エンドエフェクタの動く速度を指令す
   るための速度指令データを直角座標系で発生させる指令
   データ発生手段と、前記各自由度の位置を検出し、関節
   座標系での位置フィードバックデータを出力する位置検
   出手段と、 前記各自由度の動く速度を検出し、関節座標系での速度
   フィードバックデータを出力する複数の速度検出手段と
   、 前記関節座標系での位置フィードバックデータを直角座
   標系での位置フィードバックデータに変換する第１の座
   標変換手段と、 前記直角座標系での位置指令データと前記直角座標系で
   の位置フィードバックデータとの（ｆＩ差をめる第１の
   偏差演算手段と、 前記ｖＲ１の偏差演算手段からの直角座標系での偏差を
   、関節座標系での偏差に変換して前記自由度に６える第
   ２の座標変換手段と、 前記直角座標系での速度指令デ゛−夕を関節座標系での
   速度指令データに変換する第３３の座標変換手段と、 前記関節座標系での速度指令データと前記関節座標系で
   の速度フィードバックデータとの偏差をめ、その偏差を
   前記自由度に勺える第２の偏差演算手段とを備える、多
   関節［Ｊホットの制御装置。 （２）　前記指令データ発ｑ゛手段は、さらに、前記エ
   ンドエフェクタの動く加速度を指令するための加速度指
   令データを直角座標系で発生させ、前記直角座標系での
   加速度指令データを関節座襟元での加速度指令データに
   変換する第４の座標変換手段と、 前記関節座標系での速度指令データおよび加速度指令デ
   ータを用いて運動方程式の逆問題を解きその結果に基づ
   いて前記自由度に動作指令データを与える運動方程式演
   算手段とをさらに備える、特許請求の範囲第１項記載の
   多関節ロボットの制御装置。 （３）　前記指令データ発生手段は、さらに、前記エン
   ドエフェクタに発生ザる力を指令するための力指令デー
   タを直角座標系で発生させ、前記各自由度に発生する力
   を検出し、関節座標系でのカフィードバックデータを出
   力ザる複数の力検出手段と、 前記直角座標系での力指令データと前記直角座標系での
   カフィードバックデータとの偏差をめる第３の偏差ＦＡ
   算手段と、 前記第３の偏差演算手段からの直角座標系での偏差を関
   節座標系での偏差に変換して前記自由１庭に与える第５
   の座標変換手段とをさらに備える。 特許請求の範囲第１項記載の多関節ロボットの制御方法
   。 （４）　前記指令データ発生手段は、さらに、前記エン
   ドエフェクタの動く加速度を指令するための加速度指令
   データＪ３よび前記−Ｌノド１フＪ−フタに発生する力
   を指令Ｊるための力指令データを直角座標系で発生させ
   、 前記自由度に発生する力を検出し、関節座標系でのカフ
   ィードバックデータを出力する複数の力検出手段と、 前記直角座標系での加速度指令データを関節座標系での
   加速度指令データに変換する第４の座標変換手段と、 前記関節座標系での速度指令−ｊ゛′−夕および加速度
   指令データを用いて運動方程式の逆問題を解き、その結
   果に基づいて前記自由度に動作指令データを与える運動
   方程式演算手段と、 前記直角座標系での力指令データと前記直角座標系での
   カフィードバックデータとの偏差をめる第３の偏差演算
   手段と、 前記第３の偏差演算手段からの直角座標系での偏差を関
   節座標系での偏差に変換して前記自由度に与える第５の
   座標変換手段とをさらに備える、特許請求の範囲第１項
   記載の多関節ロボットの制御装置。 （５）　複数の自由度と前記自由度の１つに結合された
   エンドエフェクタとを有Ｊる多関節ロボッ］−のための
   ものであり、前記自由度の各々を制御することにより前
   記エンドエフェクタの動きを制御する制御装置であって
   、 前記エンドエフェクタの位置を指令するための位置指令
   データおよび前記エンドエフェクタの動く速度を指令す
   るための速度指令データを直角座標系で発生させる指令
   データ発生手段と、前記各自由度の位置を検出し、関節
   座標系での位置フィードバックデータを出力する少数の
   位置検出手段と、 前記各自由度の動く速度を検出し、関節座標系での速度
   フィードバックデータを出力する複数の速度検出手段と
   、 前記関節座標系での位置フィードバックデータを直角座
   標系での位置フィードバックデータに変換する第１の座
   標変換手段と、 前記直角座標系での位置指令データと前記直角座標系で
   の位置フィードバックデータとの偏差をめる第１の偏差
   演算手段と、 前記直角座標系での速度指令データに前記第１の偏差演
   算手段からの直角座標系での偏差を加算し、修正された
   速度指令データを出力する加算手段と、 前記修正された速度指令データを関節座標系での速度指
   令データに変換する第２の座標変換手段と、 前記関節座標系での速度指令データと前記ｆＪＩ］節座
   標系での速度フィードバックデータとの偏差をめ、その
   偏差を前記自由度に与える第２の偏差演算手段とを備え
   る、多関節ロボットの制御装置。 （６）　前記指令データ発１手段は、さらに、前記エン
   ドエフェクタの動く加速度を指令するための加速度指令
   データを直角座標系で発生さＵ、前記直角座標系−での
   加速度指令データを関節座標系での加速度指令データに
   変換する第３の座標変換手段ど、 前記００節Ｂを襟元での速度指令データおよび加速度指
   令データを用いて運動方程式の逆問題を解き、その結果
   に基づいて前記自由度に動作指令データを与える運動方
   程式演算手段とをさらに備える、竹れり請求の範囲第５
   項記載の条間ｆｌ（５０ボツトの制御装置。 （７）　前記指令データ発生手段は、さらに、前記エン
   ドエフェクタに発生する力を指令するための力指令デー
   タを直角座標系で発生させ、前記各自由度に発生覆る力
   を検出し、関節座標系でのカフィードバックデータを出
   力づる複数の力検出手段と、 前記直角座標系での力指令データと前記直角座標系での
   ノコフィードバックデータとの偏差をめる第３の偏差演
   算手段と、 前記第３の偏差演算手段からの直−色座標系での偏差を
   関節座標系での偏差に変換して前記自由度に与える第４
   の偏差演算手段とをさらに備える、特許請求の範囲第５
   項記載の条間ｉ１ｉ　１：：Ｉボットの制御装置。 （８）　前記指令データ発生手段（ま、さらに、前記エ
   ンドエフェクタの動く加速度を指令？ｌ−るための加速
   度指令）′−夕および前記エンドエフェクタに発生ずる
   力を指令づるための力指令データを直角座標系で発生さ
   ｕｌ 前記自由度に発生ずる力を検出し、関節座標系でのノノ
   ノイードハツクデータを出力りる複数の力検出手段と、 前記直角座標系での加速度指令ｊ゛−夕を関節座標系で
   の加速度指令データに変換する第３の座標変換手段と、 前記関節座標系での速度指令データおよび加速度指令デ
   ータを用いて運動方程式の逆問題を解き、ぞの結果に基
   づいて前記自由度に動作指令データを与える運動方程式
   演算手段と、 前記直角座標系での力指令データと前記直角座標系での
   カフィードバックデータとの偏差をめる第３の偏差演算
   手段と、 前記第３の偏差演算手段からの直角座標系での偏差を関
   節座標系での偏差に変換して前記自由度に与える第４の
   座標変換手段とをさらに備える、特許請求の範囲第５項
   記載の多関節ロボットの制御装置。 （９）　複数Ｎの自由度を右づ゛る多関節ロボットの関
   ＷＪ座４５！ｉ系での位置データを直角座標系での位置
   データに変換りるための座標変換方法であって、 前記各自由度の動きをそれぞれ関節座標系におりる外様
   変換７１ヘリクスＭ、として表現づる第１のステップと
   、 前記座標変換マトリクスＭ、Ｈを用いて次式で表わされ
   る積マトリクスａ・　をめる第２のスデツケ ブどを備える、座標変換方法。 ａ、＝ａ　ＸＭ−、ａｏ＝Ｅ オ　ｒ−ｒ　ｒ ここで、ｊはＯないしＮの整数、Ｅ、　４よ３３１位マ
   トリクスである。 （１０）　複数Ｎの自由度を有する多関節ロボットの関
   節座標系での位置データを直角座標系での位置データに
   変換づ−るための座標変換装置であって、 前記各自由度の動きに対応する関節座拐：系にＪ３（プ
   る座標変換マトリクスＭ７を自由度の数だけめる座標変
   換７１〜リクス３ｔｉ算手゛段と、前記座標変換マトリ
   クス演算手段に接続されていて、前記自由度の数だ（プ
   の座標変換マトリクスを保持し、かつそれらのマトリク
   スを１つずつシフトして順次出力するマトリクスジノ１
   〜レジスタと、 ２つのマトリクスを人力とし、それのそれぞれに入力さ
   れる２つのマトリクスの積をめて偵マトリクスを出力す
   るマトリクス乗算手段と、前記マトリクス乗算手段に接
   続されていて、前記偵マトリクスを保持しかつ出力づる
   ？トリクスレジスタとを備え、ここで、前記マｌ−リク
   スレジスタは単位マｉ〜リクスＥに初期化されることが
   でき、そして、前記マトリクス乗痒手段の−りの人力部
   は前記マトリクスシフトレジスタの出力部に接続されて
   おり、他方の入力部は前記マトリクスレジスタの出力部
   に接続されてＪ５す、さらに前記マトリクスシフトレジ
   スタ、ントリクス乗ｎ手段およびマトリクスレジスタに
   接続されていて、それらをｆｆ１ｌＪ御することにより
   前記マトリクス乗算手段において次式で表わされる積マ
   トリクスａ、がめられるようにする制御手段とを備える
   、座標変換装置。 ａ　ヨーａ、’　−／　×Ｍ　、’　−ａｏ　−りここ
   で、ｊはＯないしＮの整数であり、Ｅは単位マトリクス
   である。 （１１）　前記マトリクスシフトレジスタは、前記自由
   度の数だけ直列に接続されｌζ７１−リクス１ノジスタ
   を協える、特許請求の範囲第１０項記載の座標変換装置
   。 （１２）　複数Ｎの自由度を右づ−る多関節「１ポツト
   の直角座標系での速度データを関節座標系での速度デー
   タに変換するのに用いるヤコビアンマトリクスをめる演
   輝方法であって、 前記各自由度の動きをそれぞれ関節座標系にお（プる座
   標変換７１〜リクスＭ、として変Ｊ条する第１のステッ
   プと、 前記各自由度の座標変換ｖ／ｌ〜リクスのその自由度の
   動きに関づる微分マトリクスＭ、−をめる第２のステッ
   プと、 前記蔑称変換マトリクスＭ、および前記微分マトリクス
   Ｍ、４−を用いて次式で表わされる積マ］〜リクスｂ５
   　をめる第３のステップとを備える、Ａ７ケ コピアンマトリクス演算方法。 ここ・て、ＪはＯないしＮの整数であり、１は０ないし
   Ｎの整数であり、１三は単位マトリクスで゛ある。 （１３）　複数Ｎの自由度を４−ｊづる多関節ロボッ１
   −の直角座標系での速度データを関節座標系て゛の速度
   データに変換するのに用いるＸ７二１ビアンマトリクス
   をめる演韓装置であつ℃、 前記各自由度の動きに対応覆る関節座標系にＪ３（プる
   座標変換マトリクスＭとを自由度の数だけめる座標変換
   マトリクス演算手段と、 前記自由度の座標変１％マトリクスのその自由度の動き
   に関する微分マトリクスＭよ′をめる微分マトリクス演
   算手段と、 前記座標変換マトリクスをそれぞれ保持する、相互に接
   続された複数の第１の群のマトリクスレジスタとを備え
   、ここで前記第１の群のマトリクスレジスタのうちの１
   つは前記座標変換マトリクス演算手段に接続されてＪ３
   す、さらに前記微分マトリクスをそれぞれ保持する、相
   互に接続された複数の第２の群のマトリクス１ノジスタ
   を備え、ここで前記第２の群のマトリクスレジスタのう
   らの１つは前記微分７１−リクス演梓手段に接続されて
   おり、さらに 前記第１　；Ｊ３　Ｊ：び第２の群の７１〜リクスレジ
   スタ内の特定のマトリクスレジスタに接続されていて、
   ２つの７１〜リクス間の乗算を行なう複数のマトリクス
   乗算手段と、 前記第１および第２の群のマトリクス１ジスタおよび前
   記マトリクス乗算手段に接続されていて、それらを制御
   することにより前記マトリクス乗算１段において次式で
   表わされる偵マトリクス１」Ｊがめられるようにする制
   御手段とを備える、ヤコビアンマトリクス演梓装置。 ここで、ｊはＯないしＮの整数であり、ｉは０ないしＮ
   の整数であり、［三は中位ノーへりクスである。 （１４）　前記第１の群の７１ヘリクスレジスタは、双
   方向にシフト可能な第１，１３よび第２のントリクスシ
   フトレジスタを形成してＪ５す、前記第２の群のマトリ
   クスレジスタは、双方向にシフ１〜可能な第３　Ｌｌ’
   ｉよび第４の１７（〜リクスシフトレジスタを形成して
   おり、 前記第１のマトリクスシフトレジスタ内の一方端のマト
   リクスレジスタは、前記座標変換７１〜リクス演算手段
   に接続されて４３す、 前記第９のマトリクスシフ１〜レジスタ内の一方端のマ
   トリクスレジスタは、前記微分マ（ヘリクス演算手段に
   接続されており、 前記複数のマトリクス乗算手段は、第１ないし第４のマ
   トリクス乗算手段を備え、 前記第１および第２のマトリクス乗算手段は、それぞれ
   、マトリクスレジスタと組合わされて、既に保持してい
   たマトリクスと新たに入力されたマ（〜リクスとの積を
   めその結果を改めて保持する第１および第２のマトリク
   ス乗算・保持手段を形成し、 前記第１　Ｊ５よび第２のマトリクス乗算・保持手段は
   、ともに、前記第１のマ（・リクスレジスタ内の他方端
   の７１ヘリクスレジスタＪ３よび前記第２のマトリクス
   シフトレジスタ内の一方端のマトリクスレジスタに接続
   されており、 前記第３のマトリクス乗算手段は、２つの入力部がそれ
   ぞれ前記第３のマトリクスシフトレジスタ内の他方端の
   マトリクスレジスタと前記第１のマトリクス乗算・保持
   手段に接続されてｄ３す、出力部が前記第４のマトリク
   スジットレジスタ内の一方端のマトリクスレジスタに接
   続されてＪ３す、前記第４のマトリクス乗算手段は、２
   つの入力部がそれぞれ前記第４のマトリクスシフトレジ
   スタ内の前記−万端の７ドリクスレジスタと前記第２の
   ７１−リクス乗粋・保持手段に接続されてＪ３す、出力
   部が前記第３の７１〜リクスシノトレジスタ内の前記他
   方端に接続されており、 前記制御手段は、前記ＷＳ　１ないし第４のマトリクス
   シフトレジスタを往復ジノ１へざＵる、特許請求の範囲
   第１３項記載のせコピアンマトリクス演算装置。 （１５）　前記第１の［Ｙの７１〜リクスレジスタは第
   １から第２Ｎの７１ヘリクスレジスタを備え、ここで第
   １から第Ｎのマトリクスレジスタは相Ｕに直列に接続さ
   れており、第Ｎ　（−１ｈｌ　ｒら第２Ｎのマトリクス
   レジスタも相互に直列に接続されてｄ３す、さらに第Ｎ
   −１から第Ｎ　４−２のマトリクスレジスタは相互にル
   ープ状に接続されており、かつ第１のマトリクスレジス
   タは前記座標変換マトリクス演算手段に接続されており
   、 前記第２の群のマトリクスレジスタは第１から第２Ｎ−
   １の互いに直列に接続されＩこ一、／１〜リクスレジス
   タを備え、ここで第１のマトリクス乗算スＩ）ｌ（マ前
   記微分マトリクス演算手段に接続されており、 前記複数のマトリクス乗算手段は、 ２つの入力部がそれぞれ前記第１の群の第Ｎ−１と第Ｎ
   のマトリクスレジスタに接続され、出力部が前記第１の
   群の第Ｎの７１〜リクスレジスタに接続された第１のマ
   トリクス乗算１段と、２つの入力部がそれぞれ前記第１
   の群の第Ｎと第Ｎ＋１のマトリクスレジスタに接続され
   、出ツノ部が前記第１のＢＹの第Ｎ　＋１のマトリクス
   レジスタに接続された第２のマトリクス乗算手段と、２
   つの入力部がそれぞれ前記第１の群の第Ｎのマトリクス
   レジスタと前記第２の群の第Ｎ−１のマトリクスレジス
   タに接続され、出ツノ部が前記第２の群の第Ｎのマトリ
   クスレジスタに接続された第３のマトリクス乗算手段と
   、 ２つの入力部がそれぞれ前記第１の群の第Ｎ＋１のマト
   リクスレジスタと前記第２の群の第Ｎ＋１の７１〜リク
   ス１ノジスタに接続され、出力部が前記第２の群の第Ｎ
   のマトリクスレジスタに接続された第４の７１ヘリクス
   乗算手段とを備える、特許請求の範囲第１３項記載のヤ
   ニ１ヒ′ア装マトリクス演緯Ｂ置。 （１６）　前記第１ｄ３よび第２の群のマトリクスレジ
   スタは、それぞれ、双１ノ向（ご循環可能な第１および
   第２の７１〜リクス循環レジスタを形成しており、 前記複数のマトリクスレジスタは、第１および第２のマ
   トリクス＠尊手段を備え、 前記第１の′？１〜リクス乗紳手段（ま、ン１〜リクス
   レジスタと組合わされて、既に保持していｌこマトリク
   スと新た１こ入力されたマトリクスとの伯をめその結果
   を改めて保持するマ（・リクス東ｑ・保持手段を形成し
   、 前記マトリクス乗算・保持手段（、、Ｌ、前記第１のマ
   トリクス循環レジスタ内の１つの７１−リクスレジスタ
   の出力を入力とし、 前記第２のマトリクス循環レジスタで形成される循環ル
   ープは、ぞのループ内の１つのマトリクスレジスタの出
   力と前記マ［−リクス東算・保持手段の出力とを乗算す
   る前記第２のマトリクス乗算手段を経て閉じられてあり
   、 前記制御手段は、前記第１および第２のマトリクス循環
   レジスタを方向を変えて各々１循環ざＵる、特許請求の
   範囲第１３項記載のヤコビアンマ］・リクス演紳装置。 （１７）　前記第１の群のマトリクスレジスタは第１か
   ら第Ｎ　＋１のマトリクスレジスタを偵１え、ここで第
   １から第Ｎのマトリクスレジスタは相方にループ状に接
   続されており、さらに第１から第Ｎ−１および第Ｎ＋１
   のマトリクスレジスタも相互にループ状に接続されてお
   り、かつ第１のマトリクスレジスタは前記座標変換７１
   〜リクス演算手段に接続されてＪ３す、 前記第２の群のマトリクス１ノジスタは第１から第Ｎの
   互いにループ状に接続された７１ヘリクスレジスタを備
   え、ここで第１の７１〜リクスレジスタは前記微分マト
   リクス演算手段に接続されており、前記複数のマトリク
   ス乗算手段は、 ２つの入力部がそれぞれ前記第１の群の第Ｎ＝１と第Ｎ
   のマトリクスレジスタに接続され、出力部が前記第１の
   群の第Ｎの７１〜リクスレジスタに接続された第１のマ
   トリクス乗算手段と、２つの入力部がそれぞ杓前記第１
   の１１Ｙの第１と第Ｎ　−）−１のマトリクスレジスタ
   に接続され、出力部が前記第１の群の第Ｎ　＋１のマト
   リクスレジスタに接続された第２のマトリクス乗算手段
   と、２つの入力部がそれぞれ前記第１の群の第Ｎのマト
   リクスレジスタと前記第２の群の第Ｎ−１のマトリクス
   １ジスタに、出ツノ部が前記第２の群の第Ｎのマトリク
   スレジスタに接続された第３のマトリクス乗算手段と、 ２つの入力部がそれぞれ前記第１の群の第Ｎ−ト１のマ
   トリクスレジスタと前記第２の群の第１のマトリクスレ
   ジスタに接続され、出力部が前記、第２の群の第Ｎのマ
   ［〜リクスレジスタに接続された第４のマトリクス乗算
   手段とを備える、特に’ｆ’　請求の範囲第１３項記載
   のヤコビアンマトリクス演算装置。 （１８）　複数Ｎの自由度をイＪ′づる多関節ロボット
   の直角座標系での速度データを関節座標系での速度デー
   タに変換するのに用いるＡ７コビアンマトリクスをめる
   演算装置であって、 前記各自由度の動きに対応する関節座標系にＪ３（プる
   座標変換７１〜リクスＭ、２を自由度の数だ（請求める
   座標変換マトリクス演算手段と、 前記各自由度の座標変換マトリクスのその自由度の動き
   に関する微分マトリクスＭ、−をめる微分７１〜リクス
   演算手段と、 前記座標変換マトリクス演算手段に接続されていて、前
   記自由度の数だ（プの座標変換マ］〜リクスを保持し、
   かつそれらのマトリクスを１つずつシフトして順次出力
   する第１のマトリクスシフトレジスタと、 前記微分７１〜リクス演算手段に接続されていて、前記
   自由度の数だけの微分マトリクスを保持し、かつそれら
   のマトリクスを１つずつシフ１−シて順次出力づる第２
   のマトリクスシフトレジスタと、前記第１および第２の
   マトリクスシフトレジスタに接続されていて、そのいず
   れか一方からのマトリクスを切換えて出力するスイッチ
   手段と、２つの入力部を有し、それのそれぞれに人力さ
   れる７１−リクスの積をめＣ槓マトリクスを出力するマ
   トリクス乗算手段と、 前記７１−リクス乗算手段に接続されていて、前記積マ
   トリクスを保持しかつ出力Ｊるマトリクス１ジスタとを
   備え、ここで前記７１−リクスレジスタは単位マトリク
   スに初１９Ｊ化されることができ、そして前記マトリク
   ス乗算手段の一方の人力部は前記スイッチ手段の出ツノ
   部に接続され−Ｃおり、使方の入力部は前記マトリクス
   レジスタの出力部に接続されてＪ３す、さらに 前記第１Ｊ３よび第２のマトリクスシフ１〜レジスタ、
   前記スイッチ手段、前記マトリクス乗算手段および前記
   ７１〜リクスレジスタに接続されていて、それらを制御
   することにより前記マトリクス乗Ｑ手段において次式で
   表わされる積ントリクス１）う♂ がめられるようにする制御手段を備える、ヤコビアンマ
   トリクス演算装置。 ここで、ｊはＯないしＮの整数であり、ＩはＯないしＮ
   の整数であり、Ｅは単位マトリクスである。 （１９）　複数の自由度を有する多関節ロボットの直角
   座標系での速度データを関節座標系での速度データに変
   換することを含む用途に用いるＡ７コビアンマトリクス
   △の逆マトリクスＢをめる演算方法であって、 Ｓ番目のＶコピアンマトリクスＩＳ独、その近似解であ
   る逆マトリクス、ｆとを用いて、７１〜リクス［Ａ（Ｓ
   １ｅｆＳ）１をめる第１のステップを備え、ここでＳは
   任意の整数であり、さらに ６ｕ記７１〜リクス［〆’Ｆ３　’Ｊど単位マトリクス
   １三どを用いてマトリクス［Ｅ−Ｒ１ｓｉ８）］をめる
   第２のステップと、 前記マトリクス［Ｅ　−Ｎ’Ｂ〜の各要素の自乗用を計
   算して請求める第３のステップ と、 前記マトリクスＰの転置ントリクスΔ痢と、前記マトリ
   クス［Ｅ　−Ａ！５）Ｂ”ｌとを用いてマトリクスＥ　
   ＡＴ（５＞（Ｅ　−Ａ”Ｂ”う］をめる第４のステップ
   と、前記マトリクス［△１５（ビー△Ｃ’）Ｉ３＜１）
   、　］の各要素の自乗用を計算してスカラー吊ヒゝ牙求
   める第５のステップと、 前記スカｐ　ｍＶｓよびＵｌ−用いてスカラー化λｃ７
   　請求める第６のスノーツブと、前ｉｉ［！諸ｎ１を用
   いて次式【表わされる（Ｓｉ２）番目の近似マトリクス
   ［Ｂ　１をめる第７のステップとを備える、逆マトリク
   ス演粋ツノ法。 ［１３”１）］　＝　［Ｂ　”　］　−２（ｓ）　［Ａ
   　”’　＜　１　−　Ａ”＋３’う　］（］２）　複数
   の自由度を右づ−る条間ｐｉｔ　Ｄホットの直角座標系
   での速度データを関節座標系での速度データに変換づる
   のに用いるヤコビアンマトリクスへの逆マトリクスＢを
   める演ｆＪｌ装置であって、 Ｓ番目のＶ二Ｉビアンマトリクス入ｓ）を保持づる第１
   のマトリクスレジスタを備え、ここで・Ｓは任意の整数
   であり、さらに 前記マトリクスＲ１の近似解である逆７１〜ソクス１３
   〜保持づる第２のマトリクスレジスタと、前記第１およ
   び第２のマトリクスレジタに接続されていて、前記ヤコ
   ビアンマトリクスＡと前記逆７１−リクスＢとを用いて
   、マトリクス［１’ｓ”＋をめる第１のマトリクス乗算
   手段と、 単位マトリクスＥを与える初期化手段と、前記第１のマ
   トリクス乗算手段と前記初期化手段に接続されていて、
   前記マトリクス［ｌＸ５）ｓ（９ｉと００記単位マトリ
   クスＥとを用いてマトリクス［Ｅ−Ａ”Ｂ”Ｊをめる第
   １のマトリクス減算手段と、前記第１のマトリクス減算
   手段に接続されていて、前記マトリクス［［−Δ（Ｓ）
   ＢＣ５）、の各要素の自乗用を５１算して請求める第１
   の乗算・ 加算手段と、 前記第１のマトリクスレジスタと前記第１のマトリクス
   減算手段に接続されていて、前記マトリクス〆５）の転
   置マトリクス八〇勺と、前記マトリクス［Ｅ　−、ｙ（
   ５Ｊ４Ｓ勺とを用いてマトリクス［ＡＴ（Ｓ）　（Ｅ−
   バ１１Ｂ町］をめる第２のマトリクス乗算手段と、前記
   第２の７１−リクス乗算手段に接続されていて、前記マ
   トリクス１八Ｔ（Ｊ）（「−Ｋｓ）Ｂ（ｓう」の各要素
   の自乗用を計算して請求める第２ の乗絆・加算手段と、 前記第１および第２の乗算・加算手段に接続されていて
   、前記スカラーｍ　Ｘｌ５）Ｊ−；　、Ｊ：びＬＪ’、
   ｉ用いてスカラーｆｆｉλ（５％　請求める演算手段と
   。 前記第２のマトリクス東９手段、１３よび前記演鋒手段
   に接続され−Ｃいて、前記７１〜リクス［ＡＴ（Ｓ）（
   Ｅ　−Ａ”ｔ’う］　６３　、Ｊ：、　’Ｃｆ　前記）
   、　カラー５１　Ａ’Ｘ　用イーＣマトリクスλ（５）
   ［Ａ　７“’　＜　Ｅ　−Ｚ’＋−ｉｏ〉］をめる第３
   ３の７１へりクス乗算手段と、 前記第２の７１〜リクスレシスタおよび前記第３の７１
   −リクス乗算手段に接続されていて、前記マトリクスＢ
   と前記マトリクスλ町ＡＴ（Ｓ）（Ｅ　−／＜”Ｂｏ）
   ］を用いて次式で表わされる（ｓ　ト１）番Ｈの近似マ
   トリクス［Ｂ”’］をめる第２の７１へりクス減算手段
   とを備える、逆マトリクス演ｐ装肢。 ［Ｂ＝”］　＝　［１３”　］　−、；＋”　［Δ１ｃ
   ゛）＜　＋ニー＃’ｓ’＞　Ｊ（２１＞−複数の自由度
   を有づる多関節ｆコボ・ントの直角座標系での速度デー
   タを関節座上：系での速度データに変換することを含む
   用途に用いるヤ］ピノノンマトリクスの逆マトリクスを
   める演算装置であって、 ７１〜リクスを保持ザる第１のマトリクスレジスタと、 ７１〜リクスを保持する第２の７１〜リクスレジスタと
   、 ２つの入力部を有し、その一方の入力部が前記第２のマ
   トリクスレジスタの出力部に接続されていて、人力の切
   換を行なうスイッチ手段と、２つの入力部を有し、それ
   らは前記スイッチ手段および前記第１のマトリクスレジ
   スタの出ツノ部にそれぞれ接続されていて、前記２つの
   人力部に入力される２つのマトリクスの乗算を行なう第
   １のマトリクス乗算手段と、 前記第１のマトリクス乗算手段の出力部に入力部が接続
   されていて、マＩ−リクスを保持する第３のマトリクス
   レジスタと、 １４１位マトリクスを与える初期化手段と、２つの人力
   部をイテし、それらは前記第３のマトリクスレジスタお
   よび前記初期化手段の出力部にそれぞれ接続されていて
   、前記２つの人力部に人力される２つの７１〜リクス間
   の減算を行なう７１〜リクス減算手段と、 前記マトリクス減算手段の出力部【こ人力部が接続され
   ていて、マトリクスを保持りる第４の７１〜リクスレジ
   スタと、 ２つの人力部を有し、それらか共に前記マトリクス減算
   手段の出力部に接続されていて、前記２つの入力部に人
   力されるマトリクスの各豊水の自乗用をめるマトリクス
   乗算・加粋手段と、２つの入ノＪ部を有し、その一方の
   入力部が前記第３のマトリクスレジスタの出力部に接続
   されていて、前記２つの入力部に入力される２つの７１
   〜リクスの乗ｎを行なう第２の７１へりクス乗算手段と
   、 ２つの入力部を有し、それらか前記第２のマトリクスレ
   ジスタＪ３よび前記第２のマトリクスレジスタの出力部
   にそれぞれ接続されてい−く、出力部が前記第２のマト
   リクスレジスタの人力部に接続されていて、前記２つの
   入力部に人力される２つの７１〜９９２間の減算を行な
   うマトリクス減算手段とを備える、逆マトリクス演算装
   置。 （２２）　複数Ｎの自由度を右する多関節ロボットの直
   角座標系での速度データｄｘを関節座標系での速度デー
   タｄαに変換する座標変換方法であって、 各自由度の動きをそれぞれ関節座標系におりる座標変換
   ７１〜リクスＭ、として表現する第１のスσ ブツプと、 前記各自由度の座標変換マトリクスのその自由度の動き
   に関する微分マトリクスＭ、−をめる第ケ ２のステップと、 前記座枠変換マトリクスＭ、Ｊ５よび前記微分マ？ １−リクスＭ、−を用いて次式で表わされる積マトリげ シス１寸　をめる第３のステップとを備え、ここで、ｊ
   はＯないしＮの整数であり、１はＯないしＮの整数であ
   り、Ｅは単位マトリクスであり、ざらに 前記偵ントリクスｂＹ　からＦ−’１ビフ７ンマ１へリ
   クス△をめる第１のステップ′と、 Ｓ番目の前記ヤコビアンマトリクスΔｃｓ匁、その近似
   解である逆マトリクス８０乞を用いて、マトリクス［Ａ
   ”Ｂ”］をめる第５のスーｉツブとを備え、ここでＳは
   任意の整数であり、さら（ご前記マトリクスｃ　ｘｔｓ
   ）Ｂ＜ｓｉｒと単位マトリクスＥとを用いてマＩ−リク
   ス［Ｅ　−ｅｓｔｓ’＋をめる第６のステップと、 前記マトリクス［Ｅ　−ｓ’Ｂ”］の各各車の自乗用を
   泪粋してスカラー量ツ斐求める第７のステップ′と、 前記マトリクスＡＣ１，７）転置ンｉ〜リクス△７（Ｓ
   ）と、前記マトリクス［Ｆ　４”ｓ町とを用いて、／１
   −リクス［Ａ　ＴＬｓ’　（Ｅ　Ａ”Ｂ”）　］　ヲＩ
   Ｃメロ　第８　ノス−７−ツブと、前記マトリクス［Ａ
   Ｔ（Ｓ）　（）Ｅ−だ］３６う］の各些素の自乗用を計
   算してスカラー吊１づ（Ｓ）をめる第９のステップと、 前記スカラーｍ　ｖ’％よび（）３％用い−Ｃスカラー
   Φλｔｓｐ　請求める第１０のステップと、前記諸量を
   用いて次式で表わされる（ｓ＋１＞番目の近似マトリク
   ス［Ｂ　””］をめる第１１のステップと、 ［Ｂ（Ｓｔｌ）］−［ｓ　Ｃ５）　］　７　ｃＳ）　［
   Ａ　ＴＣｓ）（Ｅ　ｐ、６）ｅ（’）＞　］前記マトリ
   クスＢと前記直角座標系での速度データｄＸとの積をめ
   、それによって前記速度データｄＸを前記関節座標系で
   の速度データｄαに変換する第１２のステップとを備え
   る、座標変換方法。 （２３）　複数Ｎの自由度を有する多関節ロボットの直
   角座標系での速度データｄｘを関節座標系での速度デー
   タｄ−７に変換する座標変換装置であって、 前記各自由度の動きに対応する関節座標系における座標
   変換マトリクスＭｊ、を自由度の数だけめる座標変換マ
   トリクス演算手段と、 前記各自由度の座標変換マトリクスのその自由度の動き
   に関する微分マトリクスＭ、−をめる微分マトリクス演
   算手段と、 前記座標変換マトリクスをそれぞれ保持する、相互に接
   続された複数の第１の群の７１〜リクスレジスタとを備
   え、ここで前記第１の群のマトリクスレジスタのうちの
   １つは前記座標変換マトリクス乗算手段に接続されてお
   り、ざらに、前記微分マトリクスをそれぞれ保持づ−る
   、相互に接続された複数の第２の群のマトリクスレジス
   タを備え、ここで前記第２のＩ（Ｙの７１〜リクスレジ
   スタのうちの１つは前記微分７１−リクス演紳手段に接
   続されてＪ５す、さらに、 前記第１および第２の群のマトリクスレジスタ内の特定
   のマトリクスレジスタに接続されていて、２つのマトリ
   クス間の乗算を１１すう複数のマトリクス乗算手段と、 前記第１および第２の群の７１へりクスレジスタおよび
   前記７１〜リクス乗粋手段に接続されていて、それらを
   制御することにより前記ｖ′１〜リクス乗粋手段にＪ５
   いて次式で表わされる積マトリクスｂφ（ がめられるようにづる制９１１手段とを備え、ｂｉ　づ
   す、ｆ、　ｘＭｊ−１Ｊ　≠ｉ　、　ｂ　”、　＝　Ｅ
   同一１×Ｍ！・ｊ＝ｉ ここで、ｊはＯないしＮの整数であり、１は０ないしＮ
   の整数であり、Ｅは単位マトリクスであり。 さらに、 前記第１の群のマトリクスレジスタのうちの１つおよび
   前記第２の群のマトリクスレジスタのうらの１つに接続
   されていて、前記梢ントリクスｂ）から得られるヤコビ
   アンマトリクスの逆マトリクスをめる逆マトリクス演梓
   手段と、 前記逆マ］〜リクス演樟手段に接続されていて、−前記
   逆マトリクスと前記直角座標系での速度デー→ り（Ｉ　Ｘとの積をめ、それによって前記速度デー→ りｄＸを前記関節座標系での速度データｄαに変換する
   乗算手段とを備える、外枠変換装置。 （２４）　複数Ｎの自由度を有する多関節ロボットの直
   角座標系での速度データｄｘを関節座標系での速度デー
   タｄαに変換ザる座標変換装置であって、 前記各自由度の動きに対応する関節座標系にお（プる座
   標変換マトリクスＭ・を自由度の数だけ「 める座標変換マトリクス演算手段と、 前記各自由度の座標変換マトリクスのモの自由度の動き
   に関づる微分マトリクスＭ、−をめる微分マトリクス演
   算手段と、 前記座標変換マトリクス演梓手段に接続されていて、前
   記自由度の数だ（プのＰＩＩｍ変換ントリクスを保持し
   、かつそれらの７１−リクスを１つずつシフトして順次
   出力する第１のン１〜リクスシノトレジスタと、 前記微分マトリクス乗算手段に接続されていて、前記自
   由度の数だけの微分マトリクスを保持し、かつそれらの
   マトリクスを１っす″ウシフトして順次出力づる第２の
   マトリクスシフトレジスタと、前記第１　ｉＪ３よび第
   ２のマトリクスシフトレジスタに接続されていて、その
   いずれか一方からのマトリクスを切換えて出力りるスイ
   ッヂ″Ｊ′段と、２つの入力部を有し、それのそれぞれ
   に入力されるマトリクスの積をめて槓ｖ′ｆ〜リクスを
   出力する第１のマトリクス乗算手段と、 前記第１のマトリクス乗算手段に接続されてぃて、前記
   積マトリクスを保持しかつ出力する第１のマトリクスシ
   フ１〜レジスタとを備え、ここで前記第１のマトリクス
   レジスタは単位マトリクスに初期化されることができ、
   そして前記第１のマトリクス乗算手段の一方の入力部は
   前記スイッヂ手段の出力部に接続されており、他方の入
   力部は前記第１のマトリクスレジスタの出力部に接続さ
   れており、さらに、 前記第１および第２のマトリクスシフトレシスタ、前記
   スイッヂ手段、前記第１のマトリクス乗偉手段ｔｉ　Ｊ
   ：び前記第１のマトリクスレジスタに接続されていて、
   それらを制御づることにより前記第１のマトリクス減算
   手段にａ３いて次式で表わされる梢マトリクスｂヒ　が
   められるにうにする制と 御手段を備え、 ここで、ｊはＯないしＮの整数であり、ｉは０ないしＮ
   の整数であり、Ｅは単位マトリクスであり、さらに、 前記第１のマトリクスレジスタに接続されていて、前記
   積７１へリクスピ　から１７られるＳ番目のと ヤコビフ７ンマ１〜リクス入Ｓ髪保持づる第２のマトリ
   クスレジスタを備え、ここ’（”ＳＩよ４’ｆ　Ｎ、の
   整数（゛あり、さらに 前記マトリクスＡ６ｂ近似解である逆７１〜リクスＢ’
   ％保持する第３のマｊ・リクスレジスタと、前記ヤコビ
   アン？ｉ〜リクスΔ＜ｆ）、前記逆マトリクスＢ（５２
   とを用いて、マトリクス［ｆ　１了ｓ）、をめる第２の
   ７１へりクス乗算手段と、 単位マトリクスＥを与える初１（ｌＪ化手段と、前記マ
   トリクス［、、ｕ）Ｂ（ｓｌ、と前記単位７１〜リクス
   ［とを用いてントリクスし１−△！’Ｌ３”Ｊをめる第
   １のマトリクス減算手段と、 前記７１−リクス［Ｅ−に’Ｂ　”＋の各要素の自乗用
   をも１粋してスカラー吊イ俗求める第１の乗算・加算手
   段と、 前記マトリクスＩ）の転置マトリクスΔ丁（５）と前記
   マトリクスｃ　Ｅ　−Ａ”８０肖とを用いてマトリクス
   ［Ａ　ＴＬ’）　（Ｆ　ｐ、ｌ５）Ｂ悔］をめる第３の
   マトリクス乗算手段と、 １ｊＢＱマド！、Ｊ　’）　スＣＡＴ（ｓ’（Ｉ三Ａ”
   Ｂ”）　］　Ｕ）各’ｆＸＹｉの自乗用を３１痺して請
   求める第２の 乗算・加算手段と、 前記スフＪラーｆｆ１Ｖ（ｓ）ＬＪ３よびｇ（５用いて
   スカラー量λ’＝　ｖ”）　請求める演算手段と、前記
   ？　ト１，１　ｔｙス［ＡＴ（’（Ｅ−Ａ”Ｂ”）　］
   　ｔ３ヨヒ前記スカラー吊ノ〜用いてマトリクスλ（５
   ）［ΔＴｔＳ）　（’Ｅ、５ｃ５）ｏｃｓ）＞　］をめ
   る第４のマトリクス乗算手段と、前記７１ヘリクスＢ（
   ｓを前記マトリクスλ’Ｑ［ＡＴ（Ｓ）（１三−△（ｓ
   ′Ｂ（５））］を用いて次式ひ表わサレル（ｓ　＋１）
   番Ｈの近似マトリクスｃ　［３’Ｓ”）Ｉをめる第２の
   ７１〜リクス減埠手段と、 ［ｒ３”’）］　＝　［１３”］　−、’Ｉ　Ｃｓ）　
   ［ＡＴ”（Ｅ−Ａ”２　Ｂ”）　−前記第２のマトリク
   ス減算手段の出力部に接続されていて、前記逆マトリク
   ス１３ど前記直角座標系での速度データｄＸとの積をめ
   、それによって前記速度データｄＸを前記関節座標系で
   の速度データｄαに変換する第５の７１〜リクス乗算手
   段とを備える、座標変換装置。 く２５）　複数Ｎの自由度を有づる多関節１コ小ツトに
   おいて、一般化力Ｆ１で表わされたラグランジェの運動
   方程式の逆問題を解く演算方法であって、 前記各自由度の動きをでれぞれ関節座標系における座標
   変換マトリクスＭ、とｌノで表現する第１釦 のステップと。 前記各自由度の座標変換ン１ヘリクスのその自由度の動
   きに関づる１階の微分マトリクスＭ、−をめる第２のス
   テップと、 前記各自由度の座標変換７１〜リクスのその自由度の動
   きに関する２階の微分７１−リクスＭ、″をめる第３の
   ステップと、 前記各自由度の次式で表わされる慣性テンソルだ座標系
   から見た位置ベクトルＣ゛あり、ｄｍは前記ロボットの
   各部分の微小部分の質量であり、「は転置マトリクスを
   意味しており、さらに前記各自由度の次式で表わされる
   重力マトリクリ ここで、ｇは重力加速度であり、さらに前記マトリクス
   Ｍよを用いて、次式で表わされる７１ヘリクスａ１をめ
   る第６のステップを備え、σ ａ、−ａ、Ｘ１ｖｌ　、ａ　ｏ　＝［Ｔ６　Ｌｆ−１ｄ
   ” ここて、ｊはＯないしＮの整数であり、ビは中位マトリ
   クスであり、さらに 前記マ［−リクスＭ、およびＭ、′　を用いて、次式４
   式％ で表わされる７１〜リクスｂ？　をめる第７のステケ ツブを備え、 ここで、ｉはＯないしＮの整数であり、さらに、前記マ
   トリクスｂＱ　を用いてＡ７コビアンの逆マ１〜リクス
   Ｂをめる第８のステップと、前記逆マトリクスＢを用い
   て、前記各自由度の関節座標系での速度マトリクスルを
   める第９のステップと、 前記マトリクスＭ、、ｌｖ１．’、ａおよびんを用いｅ
   ｒｒ　ｔｒ て、次式で表わされる７１−リクスＣ・をめる第１Ｏの
   ステップと、 ・　、λ Ｃと一０ζ−１刈町＋８．−１ｘα、、請求める第１１
   のステップと、 ｄ・＝ｄ；−（ＸＭ、、　十〇ｒ、ｘαｉ　Ｍｉ’ケ ＋ａｉ−１弓すＭｉ、ｄ・−〇 前記マトリクス〔１トを用いて、ｆＴｊ記各白山１旦の
   関節座標系での加速度マ］〜リクスδをめる第１２のス
   テップと、 前記マトリクスＭ、、　Ｍ、’　、　ａＪｊＪ、びｈ・
   を用いｒ　ｃ　ｒ　ｒ て、次式で表わされるマトリクスＣ・をめる第１３のス
   テップと、 ｃ、　＝ｃ、−，ｘＭ、　＋ａ、−７Ｘ（Ｚ；　Ｍｉ’
   　、　ｃ　ｏ　−０前記マトリクスＹ、および計　を用
   いて、次式でＩ？を 表わされる一般化力Ｆ、３をめる第１４のステップとを
   備え、 ここで、Ｔｒはトレースを意味し、さらに前記マトリク
   スＣ・ａ３よび１）′：　および前記慣性デ１　、ｌ− ンソルＱ、を用いて、次式で表わされる一般化力Ｆ１′
   をめる第１５のステップと、 前記−膜化力Ｆ＋＃３よび［１３を加算することにより
   、前記−膜化力Ｆ１をめる第１６のステップどを備える
   、運動方程式演梓方法。 〈２６）　前記マトリクスｄ・Ｊ５　Ｊ：びＩＳ’？　
   およびｒ　ヶ 前記慣性テンソルＱ・を用いて、次式で表わされると 一般化力［、′をめる第１７のステップをざらにイイら
   え、 前記第１６のステップにＪ３いて、前記−膜化力Ｆ、’
   　、Ｆ、’おにび「１３を用いて次式で表ねされる演算
   を行なうことにより、前記−膜化ノノ［、がめられる、
   特許請求の範囲第２５項記載の運動方稈式演算方法。 Ｆｌ・、Ｆ、Ｉ　＋、Ｆ、２→−Ｆ　％く２７）　複数
   Ｎの自由度を右する１１１関節ロボットにおいて、−膜
   化力Ｆ、で表わされたラグランジ１の運動方程式の逆問
   題を解く演算装置であって、 前記各自由度の動きをそれぞれ座標変操マトリクスＭ（
   ｒとして表現する第１の演算手段と、前記各自由度の座
   標変換７１〜リクスのその自由度の動きに関づる１階の
   微分マトリクスＭ、′　をめる第２の演偉手段と、 前記各自由度のＰＪＦ！標変換マトリクスのその自由度
   の動きに関づる２　１１’＃の微分７１−リクスＭ・″
   をめる第３の演算手段と、 前記マトリクスＭ、を用いて、次式（・表わされ（ る７１−リクスａ・をめる第４の演算手段とを備え、ａ
   、＝ａ、ＸＭ、、ａｏ＝＝１ ｒ　ナーｔａ− ここで、Ｊは０ないしＮの整数であり、Ｆは単位マトリ
   クスであり、さらに 前記７１−リクスＭｃ　Ｊ５よびＭ、１　を用いて、次
   式ここで、ｉは０ないしＮの整数であり、さらに、前記
   マトリクスｂｉ″　を用いてヤコビアンの逆マトリクス
   Ｂをめる第６の演算手段と、 前記逆７１ヘリクスＢを用いて、前記各自由度の関節座
   標系での速度マトリクス五をめる第７の演舞手段と、 前記マトリクスＭ５．　Ｍ、’　、　ａ−ｄヅよびｂ・
   を用いｒ　ケ　ｒｄ′ て、次式で表わされるマトリクスｃテ　をめる第計 ８の演算手段と、 ｃ、＝Ｃ，ＸＭ・＋ａｘａＭ・、ｃｏ−Ｏｄ　、ｒ−１
   と　ｉ−Ｉ　Ｆ　？ 前記マトリクスＭＪ１Ｍ７．′１Ｍ−″、ａｉ、ルｉお
   よ享 びＣ１を用いて、次式で表わされるマトリクスｄトをめ
   る第９の演算手段と、 ｄ、；　＝ｄＨ−、ｘ　町　−ト　ｃ、’−，ｘ　ｔｌ
   、　Ｍ；＋ａ、−，ｘ−Ｈａ、　Ｍ、”　、　ｄ　ａ　
   ＝　０前記マトリクスｄ・を用いて、前記各自由度の関
   節座標系での加速度マトリクスｔ、をめる第１０の演算
   手段と、 前記マトリクスＭ、、　Ｍ、’　、　ａ、およびδ、を
   用いｆｒ　（ｔ　ｙ て、次式で表わされるマトリクスＣ１をめる第１ζ １の演算手段と、 ｃ、＝ｃ、−、ｘ町＋ａ、−、ｘ６．Ｈ町’　、　ＣＯ
   ＝０δ 前記１コボット固有の各自由度の次式で表わされる７１
   −リクスＹおよび前記ｂＱ　を用いて、次式でσ　ヶ 表わされる一般化力Ｆ、３をめる第１２の演算ここで、
   ９は重ツノ加速度であり、さらに前記マトリクスＣ１お
   よびｂｖ　および前記ロボッＣと ト固有の各自由度の次式で表わされる慣性−ｊンソルＱ
   、を用いて、次式で表わされる一般化カー：１′をめる
   １１３の演梓丁段を１１１１λ、−＞に こで、×ｏは、１番目の前記自由度に固定された座標系
   から見た位置ベクトルであり、ｄｍは前記ロボットの各
   部分の微小部分の貿Ｒ１であり、Ｔは転置マトリクスを
   意味し、さらに 前記一般化力Ｆ、ＩびＦ１″を加粋することににす、前
   記一般化力Ｆ１をめる第１４の演算手段を佑える、運動
   方程式演算装置。 （２８）　前記マトリクスｄおよび１）９　および♂　
   ｒ 前記を６性テンソルＱ、を用いて、次式で表わされる「 一般化力１：：、２をめる第１５の演算手段をざらにイ
   に６え、 Ｆ＋’＋Ｆｉ′および［二、３を用いて次式で表ねされ
   る演算を行なうことにより、前記一般化力Ｆ１がめられ
   る、特許請求の範囲第２７項記載の運動方程式演算装置
   。 Ｆｌ　＝Ｆｉ　’　十Ｆ＋　’　−＋　Ｆｌ　”（２９
   〉　前記各自由度の慣性−アンソルＱ、Ｊ３よび重力マ
   トリクスＹ、をそれぞれ保持し、かつそれぐ らを１つずつシフトして出力するｎ５１　；Ｊ３よび第
   ２のマトリクスシフトレジスタをさらに備え、前記第１
   ないし第３の演算手段は、それぞれ、Ｎ個のマトリクス
   を保持し、かつそれＩうのマトリクスを１つずつシフ１
   〜して順次出力りる第３ないし第５のマトリクスシフト
   レジスタを備え、前記第４の演算手段は、 ２つのマトリクスの積をめる第１のマトリクス乗算１段
   と、 前記第１のマトリクス東樟手段に接続されてイテ、マト
   リクスを保持する第１の７１−リクスレジスタとを備え
   、ここで前記第１のン１〜リクスレジスタは単位マトリ
   クスに初期化されることができ、 前記第５の演算手段は、 ２つの入〕Ｊを切替えて出力りる第１のスイッチ手段と
   、 前記第１のスイッチ手段に接続されていて、２つのマ［
   ・リクスの積をめる第２の７１ヘリクス乗算手段と、 前記第２のマトリクス乗算子段に接続されていて、マト
   リクスを保持する第２のマトリクスレジスタとを備え、
   ここで前記第２のマトリクスレジスタは単位マトリクス
   にρ）期化されることができ、 前記第６の演算手段は、 マトリクスを保持する第３のマトリクスレジスタと、 マトリクスを保持する第４の７１〜リクスレシスタと、 ２つの入力部を有し、その一方の入力部が前記第４の７
   １〜リクスレジスタの出力部に接続されていて、入力の
   切換を行なう第２のスイッチ手段と、 ２つの入ツノ部を有し、それらは前記第２のスイッチ手
   段Ｊ５よび前記第３のマトリクスレジスタの出力部にそ
   れぞれ接続されていて、前記２つの入力部に入力される
   ２つのマトリクスの乗算を行なう第３の７１へりクス乗
   樟手段と、 前記第３のマトリクス乗停丁段の出力部にパノノ部が接
   続されていて、マトリクスを保持づる第５の７１ヘリク
   スレジスタと、 単位７１ヘリクスを与える初期化手段と、２つの入力部
   を有し、それらは前記第５のマトリクスレジスタおよび
   １）う記初＋１１Ｊ化手段の出ツノ部にそれぞれ接続さ
   れていて、前記２つの入力部に入ツノされる２つのマト
   リクス間の減算を行なう第１のマトリクス減算手段と、 前記第１のマトリクス減ｆ３手段の出力部に人ツノ部が
   接続されていて、マトリクスを保持する第６の７１ヘリ
   クスレジスタと、 ２つの入力部を有し、それらがハに前記第１４７）　？
   　＋−１）　’／ス減算手段の出力部に接続されていて
   、前記２つの入力部に入力されるマトリクスの各要素の
   自東和をめるマトリクス東Ｑ・加の手段と、２つの入力
   部を有し、その−力の人力部が前記第５の７１−リクス
   レジスタの出力部に接続されていて、前記２つの人力部
   に人力される２つのマトリクスの乗算を行なう第４のマ
   トリクス乗算手段と、 ２つの入力部を有し、それらが前記第４の７１ヘリクス
   乗算手段および前記第１のマトリクスレジスタの出力部
   にそれぞれ接続され、出力部が前記第４のマトリクスレ
   ジスタの人力部に接続されテイテ、前記２つの入力部に
   入力される２つの７１ヘリクス間の減算を行なう第２の
   マトリクス減尊手段とを備え、 前記第７の演算手段は、第５のマトリクス乗算手段を備
   え、 前記第８の演算手段は、 それぞれが２つのマトリクスの積をめる第６ないし第８
   の７１へりクス乗算手段を備え、ここで前記第６のマト
   リクス乗算手段の出力部は前記第７のマトリクス乗算手
   段の入力部に接続きれており、さらに 前記第７および第８のマトリクス乗算手段に接続されて
   いて、２つのマトリクスの和をめる第１のマトリクス加
   算手段と、 前記第１のマトリクス加算手段に接続されていて、マト
   リクスを保持する第７のマトリクスレジスタとを備え、
   ここで前記第７のマトリクスレジスタは単位マトリクス
   に初期化されることができ、 前記第９の演算手段は、 それぞれか２つのマトリクスのｆｆｉをめる第９ないし
   第１２の７１へりクス乗算手段を（ｌＩｈえ、ここで前
   記第９のマトリクス乗算手段の出ツノ部は前記第１０の
   ７１−リクス乗算ｆ段の入力部に接続されており、さら
   に それぞれが２つのマトリクスの和をめる第２および第３
   のマトリクス加算手段を備え、ここで前記第２のマトリ
   クス加算手段の２つの人力部は、前記第１０および第１
   １のマ（〜リクス乗算手段の出ツノ部にそれぞれ接続さ
   れており、前記第３のマトリクス加算手段の入力部は前
   記第２のマトリクス加算手段および前記第１２のマトリ
   クス乗算手段の出゛力部に接続されており、ざらに前記
   第３のマトリクス加算・１段に接続されていて、マトリ
   クス間保持する第８のマトリクスレジスタを備え、ここ
   で前記第８のマトリクスレジスタは単位マトリクスに初
   期化されることができ、前記第１０の演算手段は、 ２つのマトリクス間の減算を行なう第３のマトリクス減
   算手段と、 ２つの７１〜リクスの積をめる第１３のマトリクス乗算
   手段とを備え、 前記第１１の演算手段は、 それぞれが２つのマトリクスの積をめる第１４ないし第
   １６の７１〜リクス乗算手段を備え、ここで前記第１４
   のマトリクス乗算手段の出力部は前記第１５のマトリク
   ス乗樟丁段の人力部に接続されており、さらに 前記第１５Ｊ３よび第１６のマトリクス乗算手段に接続
   されていて、２つのマトリクスの和をめる第４のマトリ
   クス加算手段と、 前記第４のマトリクス加算手段に接続されていて、マト
   リクスを保持づる第９のマトリクスレジスタとを備え、
   ここで前記第９のマトリクスレジスタは単位マトリクス
   にｗ期化されることができ、 前記第１２の演算手段は、次のような演算を行なう第１
   のトレース演算手段を備え、 ΣＴｒ　［Ｃより、Ｔコ ケ ここで、ＣおよびＤは任意の７１〜リクスＣあり、Ｔ　
   ｒはトレースを示し、■−は転置を示し、ｊＧＪ。 ないしＮの整数であり、 前記第１３の演算手段は、 ２つのマトリクスの積をめる第１７のントリクス乗算手
   段と、 前記第１のトレース演算手段と同一の機能を有する第２
   の１へレース演算手段とを備え、前記第１４の演算手段
   は、 ２つの７１−リクスの積をめる第１８のマトリクス乗算
   手段ど、 前記第１の１〜レ一ス演算手段ど同一の機能を右づる第
   ３の１へレース演算手段とを備える、特許請求の範囲第
   ２８項記載の運動り稈式演算装置。 （３０）　複数の自由度を有づる３関節ロボットの直角
   座標系での力データｄ　Ｆ′を関節座標系でのトルクデ
   ータｄτに変換する座標変換装置であって、 ヤコビアンマトリクスＡの転置マトリクスＡＴをめる演
   算手段と、 前記力データｄＦおよび前記転置マトリクス八〇を入力
   とし、それらの入ノ〕の積をめるマトリクス乗算手段と
   を備える、座標変換装置。