JPH0785203B2

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

Info

Publication number: JPH0785203B2
Application number: JP58183297A
Authority: JP
Inventors: 英夫小山; 文雄野口; 茂樹藤長; 裕敏山本; 隆彦近藤
Original assignee: Shinmaywa Industries Ltd
Current assignee: Shinmaywa Industries Ltd
Priority date: 1983-09-30
Filing date: 1983-09-30
Publication date: 1995-09-13
Anticipated expiration: 2010-09-13
Also published as: JPS6075904A

Description

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

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

まず座標系につき説明する。多関節ロボットの制御に用
いる座標系には、表１に示すように、直角座標系（互い
に直角に交わった３本の軸に関する成分で座標が定ま
る）とそれに対する関節座標系（各関節の角度によって
座標が定まる）とがあり、さらに、絶対座標系（静止し
た座標系）とそれに対するハンド座標系（各ハンドに固
定されていてハンドの動きに従って動く座標系）とがあ
る。この明細書においては、直角座標系はXYZ座標系あ
るいはＸ座標系とも呼ばれている。また、関節座標系
は、角度座標系あるいはα座標系とも呼ばれている。さ
らに、ハンド座標系は、局所座標系とも呼ばれている。

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

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

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

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

さらに、位置のフィードバックによってのみ制御される
ロボットが何らかの障害物に対したときにロボットのア
ームに発生する力は、その力の方向の位置偏差によって
変わってしまうが、組立や面仕上げに類する応用におい
ては、或る方向には一定の力で押し付けそれに垂直な方
向には位置制御する要求があった。また、その方向はロ
ボットの機構の設計上の都合で決まる方向ではなく、被
加工物によって決まる方向である。したがって、任意の
被加工物で決まる座標系で位置制御と力制御の切換また
は荷重の選択を行なう必要がある。それゆえに、第３
に、そのための装置と被加工物で決まる座標系での力か
らロボット機構で決まる座標系での力（トルク）への変
換の手段が望まれていた。さらに、特に組立作業におい
て、挿入する部品によって決まる座標系での回転を含め
たいくつかの方向のうちの或る方向について、柔かい位
置決め（偏差が生じても反力が小さいこと）をすること
により、部品の挿入作業が容易になる現象があり、これ
を特別なエンドエフェクタ上のメカニズムで実現したも
のがRCC（Remote Center of Compliance）である。３つ
の異なる形のRCC装置が、米国特許第4,098,001号、第4,
155,169号および1980年４月16日に出願された米国特許
出願第140,768号中に開示されている。これらの米国特
許および特許出願は、ここに参考として組入れられてい
る。しかし、RCCを特別なエンドエフェクタとしてでは
なく、アーム自身で実現する要求があった。

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

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

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

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

全体システムの構成上記処理のすべてを実施する全体システムの構成につき
概略説明する。第1A図および第1B図は、それぞれ、多関
節ロボットの制御装置の全体システムの構成を概略的に
示すブロック図である。図において、一点鎖線より左側
は直角座標系で表わされる量を取扱う部分であり、右側
は関節座標系で表わされる量を取扱う部分である。

まず、第1A図に示すシステムにつき説明する。ホストシ
ステム４からは、時々刻々の直角座標系における指令値
がマトリクスの形で出力される。ホストシステム４は、
図示しないけれども、マイクロコンピュータを備える。
ホストシステム４から出力される指令値には、大きく分
けて、パスの指令値と押付け力の指令値とがある。パス
の指令値は、位置指令データ，速度指令データｄお
よび加速度指令データd²を含み、押付け力の指令値
は、力指令データを含む。ホストシステム４からの位
置指令データは、後述の位置フィードバックデータが
差引かれた後、ゲイン調整部14でゲイン調整され、後述
の速度指令データに補正分として加算される。ホストシ
ステム４からの速度指令データｄは、ゲイン調整部14
からの補正分と加算され、座標変換部６において関節座
標系に変換され、運動方程式演算部９に与えられる。座
標変換部６からの速度指令データは、さらに、後述の速
度フィードバック信号が差引かれた後、ゲイン調整部15
でゲイン調整され、その結果が補正分として関節部10に
与えられる。ホストシステム４からの加速度指令データ
d²は、座標変換部７において関節座標系に変換されて
運動方程式演算部９に与えられる。運動方程式演算部９
においては、与えられた速度データおよび加速度データ
に基づいて運動方程式の逆問題が解かれ、各関節に与え
られるべきトルクデータが出力され、このデータが関節
部10に与えられる。関節部10は、アンプおよびモータを
含む。関節部10の位置は、エンコーダ11によって検出さ
れ、エンコーダ11からは位置フィードバックデータが出
力されこれが座標変換部５において直角座標系に変換さ
れ、前述したとおりホストシステム４からの位置指令デ
ータから差引かれる。同様に、関節部10の速度はタコ
メータ12によって検出され、タコメータ12は速度フィー
ドバックデータを出力し、これが前述したように座標変
換部６からの速度指令データに加算される。一方、ホス
トシステム４からは、力指令データが出力され、これ
に後述する力フィードバックデータが差引かれて両者の
偏差が求められ、ゲイン調整部16においてゲイン調整さ
れる。力の偏差ｄ_Hはハンド座標系で表現されている
ため、これが座標変換部17において絶対座標系に変換さ
れてｄ_Xとなる。このｄ_Xがさらに座標変換部８にお
いて関節座標系に変換されてｄとなり、補正分として
関節部10に与えられる。関節部10に生じる力は力検出器
13によって検出され、力検出器13は前述の力フィードバ
ックデータを出力する。

次に、第1B図に示すシステムにつき、第1A図のシステム
との相違点を主に説明する。第1A図のシステムにおいて
は、位置フィードバックは、運動方程式演算部９を介し
て調節部10にかけられている。ところが、運動方程式演
算部９には複雑な要素が含まれているため、この方式で
は、フィードバックの効き具合が明瞭でないという欠点
がある。これに対して、第1B図のシステムにおいては、
位置フィードバックは、運動方程式演算部９を介さずに
直接関節部10にかけられている。この方式では、余分の
座標変換部18が必要になるものの、フィードバックの効
き具合が明瞭になるという長所がある。

第1A図および第1B図に示したシステムの特徴は、簡単に
言えば次のとおりである。第１に、直角座標系と関節座
標系との間の座標変換を行なう座標変換部５ないし８を
備えていることである。第２に、ホストシステム４から
座標変換部６を経由して運動方程式演算部９に至るフィ
ードフォワードラインおよびホストシステム４から座標
変換部７を経由して運動方程式演算部９に至るフィード
フォワードラインを備えていることである。第３に、位
置のフィードバックが直角座標系においてかけられてい
ることである。従って、演算を行なうことが不可能に近
い、直角座標系での位置データから関節座標系での位
置データへの座標変換を行なう必要がない。さらに、
速度のフィードバックもかけられている。第４に、ホス
トシステム４から座標変換部８を経由して関節部10に至
る力のフィードバック制御ラインを備えていることであ
る。

以下、第1A図および第1B図のうちで特徴のある各部分を
詳細に説明する。第2A図および第2B図は、全体システム
の構成を詳細に示すブロック図である。第2A図の右端
は、第2B図の左端につながっている。このブロック図
は、第1A図および第1B図に示した座標変換部５ないし８
および運動方程式演算部９に相当する。この第2A図およ
び第2B図の説明をこれ以降において詳細に行なう。

座標変換の一般論まず、座標変換の一般論につき説明する。第３図は、多
関節ロボットの模式図である。この第３図の例では、多
関節ロボットは６つは自由度あるいは関節角α_１，
α_２，α_３，α_４，α_５，α_６を有する。絶対座標系が
XYZで規定され、エンドエフェクタの局所座標系（ハン
ド座標系）がxyzで規定されている。

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

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

但し、C_i＝cosα_ｉ、S_i＝sinα_１であり、l_i，m_iおよび
n_iは、各関節の座標系の原点間の関係を示す。ここで、
ｉ＝1,…,6である。

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

＝M₁×M₂×M₃×M₄×M₅×M₆× …（３）但し、M₁ないしM₆は、各腕についての変換マトリクスを
示す。

一方、ハンド座標系は、第４図に示すベクトル（P_X，
P_Y，P_Z）の点に位置している。そして、絶対座標系の各
軸の単位ベクトルをi,j,kとし、ハンド座標系の各軸の
単位ベクトルをｉ′,j′,k′とすると、第４図における
ベクトル，およびは次式（４）で与えられる。

＝＋とするとき、絶対座標系（）とハンド座標
系（）の相互関係は、次式（５）で与えられる。

但し、ｉ・ｊ′は、ベクトルｉとべクトルｊ′との内積
を意味する。

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

（ｉ）Ｍは、それぞれの関節角α_１，α_２，α_３，
α_４，α_５およびα_６の関数であるが、それぞれの腕に
ついての変換マトリクスM_i（ｉ＝1,…,6）は、その軸の
関節角α_ｉだけの関数である。

（ii）P_X，P_YおよびP_Zは、ハンド座標系の原点位置を示
す。

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

エンドエフェクタの位置・姿勢を実現するのは各自由度
の回転角であるため、絶対座標系から各軸のα座標系へ
の変換が必要である。ロボットのエンドエフェクタの姿
勢・位置ベクトル（）は、位置情報をX,Y,Zとし、姿
勢情報をn_X，n_Y，n_Zとすると、で表わすことができる。また、各軸の回転角度はまたで表わされる。したがって、前記位置情報および姿勢情
報は、それぞれ（７）式で表わされる。

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

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

からへの変換次に、ベクトルからベクトルへの座標変換の方法お
よびそのための装置につき説明する。これは、第1A図お
よび第1B図の座標変換部５に相当する。

（２）式からわかるように、M₁は、_１，l₁，m₁および
n₁で表現されたマトリクスである。また、M₂は、α_２，
l₂，m₂およびn₂で表現されており、M₃ないしM₆も同様で
ある。したがって、それらの積M₁×M₂×M₃×M₄×M₅×
M₆、すなわちＭは、α_ｉ，l_i，m_i，n_i（ｉ＝１ないし
６）を含んだマトリクスとなっている。このうち、l_i，
m_iおよびn_iは、各関節間の距離、すなわち、各腕の長さ
に関係した量であり、ここでは、定数である。このた
め、Ｍの各要素は、変数としてα_ｉ（ｉ＝１ないし６）
を含んだ量となっている。このことから、Ｍは、Ｍ＝［a_jk（α_ｉ）］ …（９）のように書くことができる。Ｍのjk要素a_jk（α_ｉ）
は、具体的には（２）式のマトリクス乗算を実行し、そ
の結果として得られる第ｊ行第ｋ列の要素である。とこ
ろで、このようにして得られるa_jk（α_ｉ）がどのよう
な意味を持っているかを考える。

Ｍは、その定義（１）式から、空間中の任意の点のハン
ド座標系での位置ベクトルと、同じ点の絶対座標系で
の位置ベクトルとの関係を示すものである。そこで、
ハンド座標系での原点すなわちエンドエフェクタの先端
が絶対座標系でどのように表わされるかを考える。この
点は、ハンド座標系ではｘ＝0,y＝0,z＝０であるから、
絶対座標系でのこの点の座標X₀，Y₀，Z₀とすると、
（１）式よりなる式が得られる。この式の右辺の乗算を実行すれば、となる。したがって、なる式が得られる。すなわち、マトクスＭのa
₁₄（α_ｉ），a₂₄（α_ｉ），a₃₄（α_ｉ）はそれぞれエン
ドエフェクタの先端の絶対座標系における座標成分に対
応していることになる。そして、（12）式の左辺は絶対
座標系における量であり、右辺はα_ｉ（ｉ＝１ないし
６）によって書かれていることから、α_ｉを与えれば
X₀，Y₀，Z₀がわかることになる。エンドエフェクタの姿
勢情報を表わす量n_X0，n_Y0，n_Z0についても同様にＭの
中のブロックマトリクスｍに含まれる要素との関係が得
られる。多関節ロボットの制御において重要な役割を演
ずるのは、エンドエフェクタの先端の位置およびその姿
勢であるから、（12）式でX₀，Y₀，Z₀と沿え字０を付け
て書いたものを改めてX,Y,Zと書いて使用することとす
ば（n_X0，n_Y0，n_Z0についても同様）、（12）式等によ
りなる関係があることになる。すなわち、（ｉ）α_ｉで書かれたM_iの乗算を行なってＭを求め（こ
のＭはα_ｉで書かれている）、（ii）Ｍの各要素のうちa₁₂（α_ｉ），a₁₃（α_ｉ）、a
₁₄（α_ｉ），a₂₃（α_ｉ），…，a₃₄（α_ｉ）を取出せ
ば、これらはそれぞれ、n_X，n_Y，n_Z，…,Zに対応してい
るため、（12）式等の関係式が得られ、（iii）（12）式等の関係式は、の関係を示す式
であるため、との相互の変換式が得られたことにな
る。すなわち、からへの変換を行なうためには、前
記（２）式の演算、すなわち、 M₁×M₂×M₃×M₄×M₅×M₆＝Ｍ …（14）なる演算を行なえばよい。さらに換言すれば、αから
への変換は、ロボットの各自由度の動きを座標変換マト
リクスM_iとして表現し、そのマトリクスの自由度の数
（たとえば６自由度の場合、ｉ＝１ないし６）だけの積
マトリクスＭを求めればよい。

そこで、前記（14）式の演算を行なう装置につき説明す
る。第６図は、からへの座標交換装置を説明するた
めのブロック図である。この図は、第2A図に相当し、点
線で囲んだ部分が第1A図および第1B図に示した座標変換
部５である。

座標変換部５は、マトリクスシストレジスタ25と、それ
に接続されたマトリクス乗算器41と、それに接続された
マトリクスレジスタ42と、それに接続されたマトリクス
レジスタ43およびバッファレジスタ44とを備える。マト
リクスシフト25は、自由度の数だけの、たとえば６自由
度の場合６個のマトリクスレジスタ251ないし256から構
成されている。マトリクスレジスタ251ないし256は、そ
れぞれ、各自由度の動きを座標変換マトリクスとして保
持する。マトリクス乗算器41は、２つのマトリクスを入
力とし、その２つのマトリクスの積のマトリクスを出力
する。このマトリクス乗算器41については後でさらに詳
しく説明する。マトリクスレジスタ42は、マトリクス乗
算器41での乗算結果を保持する。

ここで、クロック信号につき説明する。座標変換部５に
タイミング回路（図示せず）から２種類のクロック信号
φＡおよびφＢが与えられる。すなわち、マトリクスレ
ジスタ42にはクロック信号φＡが与えられ、マトリクス
シフトレジスタ25,マトリクス乗算器41,マトリクスレジ
スタ43およびバッファレジスタ44にはクロック信号φＢ
が与えられる。クロック信号φＡは、マトリクスの積を
とるためのものであり、細かく見れば64個のパルスから
なる。クロック信号φＢは、レジスタをシフトさせるた
めのものであり、細かく見れば16個のパルスからなる。
なお、これらのクロックφＡおよびφＢは、以上と同様
の考え方で、第2A図および第2B図に示したすべてのブロ
ックに入力される。すなわち、マトリクスの積を受ける
レジスタにはクロック信号φＡが入力され、それ以外の
ブロックにはクロック信号φＢが入力される。

次に、座標変換部５の動作につき説明する。マイクロコ
ンピュータ（図示せず）から、各マトリクスレジスタ25
1ないし256に変換マトリクスM₁ないしM₆がそれぞれ与え
られる。図において、▽は、マイクロコンピュータから
の入力を示している。つまり、マイクロコンピュータ内
のリードオンメモリ内には、各自由度の正弦および余弦
を求めるためのテーブルがあり、そのテーブルに基づい
てソフトウェアによる補間が行なわれ、これによって上
述のようにマイクロコンピュータからマトリクスレジス
タ251ないし256に変換マトリクスM₁ないしM₆が与えられ
る。まず始めに、マトリクスレジスタ251内にはマトリ
クスM₁が保持されており、マトリクスレジスタ43は単位
マトリクスＥにイニシャライズされている。したがっ
て、マトリクス乗算器41は、マトリクスM₁と単位マトリ
クスＥとの乗算を行ない、その結果であるマトリクスM₁
がマトリクスレジスタ42内に保持される。第６図は、こ
の状態を示している。次に、前記クロック信号によりマ
トリクスシフトレジスタ25内のマトリクスは図において
右の方向に１ブロックだけシフトされ、すなわち、マト
リクスレジスタ251内にはマトリクスM₂がロードされ、
さらにマトリクスレジスタ43にはマトリクスレジスタ42
からのマトリクスM₁がロードされ、そしてマトリクス乗
算器41においてマトリクスレジスタ251からのマトリク
スM₂とマトリクスレジスタ43からのマトリクスM₁との乗
算が行なわれ、その結果マトリクスM₁×M₂がマトリクス
レジスタ42中にロードされる。以降同様の動作が繰返さ
れ、所定の６クロックタイムの後には、マトリクスレジ
スタ42およびバッファレジスタ44中にはマトリクスM₁×
M₂×M₃×M₄×M₅×M₆が、すなわちマトリクスＭがロード
される。これにより、からへの変換が行なわれたこ
とになる。なお、バッファレジスタ44は必ずしも必要な
ものではなく、さらにマトリクスレジスタ43の代りに最
小限１行（または１列）分のバッファ機能をマトリクス
レジスタ42中に含めることあるいはマトリクス乗算器41
に付加することにより省略することができる。この場
合、マトリクスレジスタ42は、単位マトリクスＥにイニ
シャライズされるものとする。

以上のようなからへの変換をマイクロコンピュータ
のソフトウェアのみで行なうとすれば、その演算時間は
たとえば50〜100m sec程度必要とするが、同様の演算を
この実施例ように専用のハードウェアで実施すると非常
に高速度で、たとえば1m sec以下で行なうことができる
という効果がある。

次に、前述したマトリクス乗算器41の詳細につき説明す
る。第７図は、マトリクス乗算器を説明するためのブロ
ック図である。このブロックの全体の運用でマトリクス
乗算器の機能が達成される。第６図のマトリクス乗算器
41は、そのような機能を達成するものを適宜上１つのマ
トリクス乗算器41として表わしたものである。第2A図お
よび第2B図に示し、後で説明する他のマトリクス乗算器
についても同様である。

４×４のマトリクスレジスタ80および81が乗算・加算器
82の入力部に接続されており、乗算・加算器82の出力部
には４×４のマトリクスレジスタ83が接続されている。
このマトリクスレジスタ80,81および83は、それぞれ、
第６図のマトリクスレジスタ43,マトリクスシフトレジ
スタ25およびマトリクスレジスタ42に対応している。乗
算・加算器82は、乗算器821と、それに接続された加算
器822と、それに接続されたバッファレジスタ823とを備
える。マトリクスレジスタ80からの乗算・加算器82への
入力をx_iとし、マトリクスレジスタ81からの乗算・加算
器82への入力をy_iとし、乗算・加算器82からのマトリク
スレジスタ83への出力をz_iとした場合、乗算・加算器82
においては次の式に示す演算が行なわれる。

z_i＝z_i-1＋x_i×y_i …（15）ここでは、ｉは１乗算時間単位のタイミングを示し、ｉ
＝１ないし４である。このブロック図の装置は、２つの
機能を有している。第１は、次の式に示すようなマトリ
クス乗算の機能である。

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

第８図は、マトリクスレジスタの一例を示すブロック図
である。この図は４×４のマトリクスレジスタについて
示しているが、６×６のマトリクスレジスタについても
同様である。ここでは、マトリクスレジスタ80を例にし
て説明する。４×４のシフトレジスタ802の入力部およ
び出力部に、それぞれ、ゲート801および803が接続され
ている。さらに、ゲート801および803ならびにシフトレ
ジスタ802には、制御装置804が接続されている。このマ
トリクスレジスタ80は、次に示す４つの機能を有する。

（ｉ）２つのモードを選択することが可能である。１つ
は、シフトレジスタ802の各要素をシフトさせるシフト
モードであり、このシフトは指定された１つの行または
列についてのみ行なわれる。他の行または列は巡回（ロ
ーテート）する。他の１つは、シフトレジスタ802の各
要素を巡回させるローテーションモードである。このロ
ーテーションは、全部の行または全部の列について行な
われる。

（ii）データを横方向にシフトまたはローテーションさ
せる横シフトモードと、データを縦方向にシフトまたは
ローテーションさせる縦シフトモードとを選択すること
ができる。

（iii）シフトレジスタ802が外部からデータを受取る要
素を指定することができる。すなわち、シフトレジスタ
802において、シフトする行または列を指定することが
できる（残りの部分はローテートする）。

（iv）シフトレジスタ802において外部へデータを出力
する要素を指定することができる。

上記の４つの機能は、制御装置804によって制御され
る。

第９図は、制御装置とマトリクス乗算器との接続を示す
ブロック図である。マトリクスレジスタ80,81および83
は、データラッチ回路84に接続されており、データラッ
チ回路84は制御用リードオンリメモリ（ROM）85に接続
されている。制御用ROM85には、アドレスカウンタ86が
接続されており、アドレスカウンタ86にはナンドゲート
87が接続されている。

各マトリクスレジスタ80,81および83において、データ
ラッチ回路84からの信号により、前記４つの機能を選択
が行なわれる。制御用ROM85内には、各マトリクスレジ
スタ80,81および83を制御するための所定のデータが記
憶されている。そのデータは、アドレスカウンタ86によ
り指定されたアドレスから順次読出される。アドレスカ
ウンタ86の内容は、ホストシステム（図示せず）からの
クロック信号CLKにより更新される。

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

マトリクスレジスタ80,81および83のモードを選択す
る。すなわち、マトリクスレジスタ80のモードとして、
横モードおよびローテーションモードを選択する。マト
リクスレジスタ81のモードとして、縦モードおよびロー
テーションモードを選択する。マトリクスレジスタ83の
モードとして、横モードおよびシフトモードを選択す
る。

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

次に、マトリクスレジスタ80の第３行の各要素とマトリ
クスレジスタ81の各列の要素との演算が前述と同様に行
なわれ、その演算結果がマトリクスレジスタ83の第３行
にロードされる。以後同様にして、マトリクスレジスタ
80の第２行あるいは第１行の各要素とマトリクスレジス
タ81の各列との演算が行なわれ、その演算結果がマトリ
クスレジスタ83の第２行あるいは第１行にロードされ
る。以上により次式のマトリクス乗算がなされたことに
なる。

［Ｘ］×［Ｙ］＝［Ｚ］Ｊ（）の求め方次に、ヤコビアンＪ（）を専用ハードで求める方法お
よびそのための装置を説明する。これは、第1A図および
第1B図の座標変換部６の一部分に相当する。

前記（８）式は、についての方程式であり、絶対座標
系の姿勢・位置ベクトルと各関節角ベクトルとの間
には、非線形の関係が存在する。したがって、（８）式
から→の直接変換を求めることは一般に複雑であ
る。

そこで、この実施例では上記（８）式について微小変化
ｄおよびｄに限定すれば、線形で取扱い得ることに
着目し、ヤコビアンＪ（）を利用する。なお、このヤ
コビアン（Jacobian）は、たとえば、IEEE TRANSACTION
S ON MAN−MACHINE SYSTEMS,VOL.MMS−10,NO.2,JUNE196
9のDaniel E.Whitneyによる“Resolved Motion Rate Co
ntrol of Manipulators and Human Prosthesis"という
記事でよく知られている。ｄとｄとの間の変換のヤ
コビアンは、（７）式の右辺を微小なｄについてテー
ラー展開し、その最低次の項のみを残すことによって、
次式（16）で表わされることがわかる。

ここで、ｄ＝Ｊ（）・ｄ …（17）であるから、連立方程式を解けば、ｄ＝［Ｊ（）］^-1・ｄ …（18）が成立する。この発明の実施例では、（17）および（1
8）式を利用して、直角座標系からα系への座標変換を
行なう。上記２式は微小変化についてのものであり、し
たがってそのための演算は、少なくともサーボレートで
行なう必要がある。

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

前述の（３）式に戻って、をα_１ないしα_６で偏微分
すると、（19）式が得られる。

変換マトリクスを微分するということはその中の各要素
を微分することであり、したがって上記（19）式の∂
／∂α_１は、（５）式を用い次式（20）式で表わされ
る。

これを前述の（16）式と対応させると、また、∂f₄／∂α_１，∂f₅／∂α_１および∂f₆／∂α_１
は、上記ブロックマトリクス∂m/∂α_１の中から適当な
３つを選択する。（19）式の残りのものについても（2
0）および（21）式と同様に処理できる。このようにし
て∂M/∂αｉ（ｉ＝1,2,…,6）を計算することによって
カコビアンＪ（α）の各々の要素が求まる。このように
して求めた要素を、前述の（17）式に代入することによ
って、微小区間での座標変換が達成される。

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

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

第11図はこの実施例の概念を説明するためのブロック図
である。マイクロプロセサ１は演算回路２に接続され
る。マイクロプロセサ１から、演算回路２に、前述の
（19）式の各腕ごとの変換マトリクスM₁〜M₆およびM₁′
〜M₆′が与えられ、演算回路２では、この（19）式に基
づいて演算を行なう。演算回路２は、バッファレジスタ
B₁〜B₁₂およびb₁〜b₁₁を含み、乗算器MP₁〜MP₄によっ
て、これらバッファレジスタにストアされたデータが乗
算される。乗算器MP₁はバッファレジスタB₅の内容とバ
ッファレジスタB₆の内容とを乗算し、その結果をバッフ
ァレジスタB₆に再びロードする。乗算器MP₂はバッファ
レジスタB₆の内容とバッファレジスタb₅の内容とを乗算
し、その結果をバッファレジスタb₆にロードする。乗算
器MP₃はバッファレジスタB₇の内容とバッファレジスタB
₈の内容とを乗算し、その結果を再びバッファレジスタB
₇にロードする。そして乗算器MP₄はバッファレジスタB₇
の内容とバッファレジスタb₇の内容とを乗算し、その結
果をバッファレジスタb₆にロードする。この演算回路２
においては、２つの演算過程を含み、その１つはデータ
の流れが第11図における右方向であり、他の過程は逆方
向のデータの流れを有する。そして、それぞれの演算動
作および或るバッファレジスタから他のバッファレジス
タへのデータのシフトは、所定の１ユニットタイムごと
に行なわれる。バッファレジスタB₆およびB₇は、バッフ
ァレジスタB₅からのデータを受け、バッファレジスタB₆
はさらにバッファレジスタB₈からのデータを受ける。バ
ッファレジスタB₁〜B₁₂とそれらに関連する乗算器MP₁お
よびMP₃とが（20）式の変換マトリクスM₁〜M₆の演算の
ために用いられ、バッファレジスタb₁〜b₁₁と乗算器MP₂
およびMP₄とが（20）式における微分された変換マトリ
クスM₁′〜M₆′の演算のために利用される。

マイクロプロセサ１からのデータの入力が完了した状態
では、バッファレジスタB₁，B₂，B₃，B₄，B₅，B₆および
B₇に、それぞれ、変換マトリクスM₁，M₂，M₃，M₄，M₅，
M₆およびM₆がロードされ、バッファレジスタb₁，b₂，
b₃，b₄，b₅，b₆に、それぞれ、微分された変換マトリク
スM₁′，M₂′，M₃′，M₄′，M₅′，M₆′がロードされて
いる。次のユニットタイム間にバッファレジスタB₅にロ
ードされた変換マトリクスM₅とバッファレジスタB₆にロ
ードされた変換マトリクスM₆とが乗算器MP₁で乗算さ
れ、同時にバッファレジスタb₅にロードされている微分
された変換マトリクスM₅′とバッファレジスタB₆にロー
ドされている変換マトリクスM₆とが乗算器MP₂によって
乗算され、その結果M₅×M₆およびM₅′×M₆′が、それぞ
れ、バッファレジスタB₆およびb₆にロードされる。ま
た、この間に、各バッファレジスタにロードされている
変換マトリクス（微分されたものも含む）は、次の乗算
に備えて、第11図の右方向へ１ブロックシフトされてい
る。そして、バッファレジスタB₇〜B₁₂にそれぞれ変換
マトリクスM₁〜M₆がロードされ、バッファレジスタB₆に
はすべての変換マトリクスの乗算結果すなわちM₁×M₂×
M₃×M₄×M₅×M₆がロードされ、バッファレジスタb₁₁，b
₁₀，b₉，b₈，b₇およびb₆には、それぞれ、M₆′，M₅′×
M₆，M₄′×M₅×M₆，M₃′×M₄×M₅×M₆，M₂′×M₃×M₄×
M₅×M₆およびM₁′×M₂×M₃×M₄×M₅×M₆がロードされて
いる。そして、この時点で第１の演算過程が終了する。

次の演算過程の流れは第11図の左方向であり、乗算器MP
₃およびMP₄によって処理される。先の演算過程で第11図
の右側のバッファレジスタに移ったそれぞれのデータ
は、次のユニットタイム間にすべて、左側へ再びシフト
される。その結果、バッファレジスタB₁，b₁，b₂，…b₆
には、それぞれ、各マトリクスの積すなわちM₁×M₂×M₃
×M₄×M₅×M₆，M₁′×M₂×M₃×M₄×M₅×M₆，M₁×M₂′×
M₃×M₄×M₅×M₆，…M₁×M₂×M₃×M₄×M₅×M₆′がロード
されることになる。

このようにして演算された（19）式の要素はバッファレ
ジスタb₁からマイクロプロセサ１に順次転送される。こ
れらのマトリクスが各々∂M/∂α_１，∂M/∂α_２，…，
∂M/∂α_６であることは前述した通りである。これらの
うち、例えば∂M/∂α_１について考えてみると、このマ
トリクスは（20）式において［］で示したマトリクス
である。このマトリクスの∂P_X／∂α_１，∂P_Y／∂
α_１，∂P_Z／∂α_１等の要素は、（21）式によって（1
6）式のヤコビアンＪ（α）の一部の要素に対応してい
る。∂M/∂α_２，…，∂M/∂α_６についても同様であ
る。従って上記ロードされた６つのマトリクス積のそれ
ぞれから必要な要素を取り出すことにより、（16）式の
ヤコビアンＪ（）のすべての要素が得られる。ヤコビ
アンＪ（）が得られれば、（18）式を用いて各関節角
の変化量が得られる。これらの処理がマイクロプロセッ
サ１によって行なわれる。

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

なお、バッファレジスタB₁から得られるデータM₁×M₂×
M₃×M₄×M₅×M₆は、前記（１）および（３）式の変換マ
トリクスであり、CPUはこのデータを用いて、ハンド座
標系から絶対座標系への変換も行なうことができる。

第12図はこの実施例の概念を説明するための他のブロッ
ク図である。この第12図の回路は、第11図の回路よりも
演算時間を短縮することができる。たとえばこの第12図
の回路によれば、同じようにヤコビアンマトリクスの各
要素を演算するについて、第11図の回路に比べて、回路
規模を減少することができる。なぜならば、データ転送
数を大幅に減らすことができるからである。この第12図
に示す演算回路２はバッファレジスタB₁〜B₇およびb₁〜
b₆を含み、マイクロプロセサ１（第11図）はバッファレ
ジスタB₁とb₁とに結合される。乗算回路MP₁は、バッフ
ァレジスタB₅の内容とバッファレジスタB₆の内容とを乗
算しその結果をバッファレジスタB₆に再びロードすると
ともに、バッファレジスタB₁の内容とバッファレジスタ
B₇の内容とを乗算しその結果を再びバッファレジスタB₇
にロードすることができる。同じように、乗算回路MP₂
は、バッファレジスタB₆の内容とバッファレジスタb₅の
内容とを乗算してその結果をバッファレジスタb₆にロー
ドするとともに、バッファレジスタB₇の内容とバッファ
レジスタb₁の内容とを乗算し、その結果を同じくバッフ
ァレジスタb₆にロードすることができる。

マイクロプロセサ（図示せず）からのデータ入力が完了
した状態では、バッファレジスタB₁，B₂，B₃，B₄，B₅，
B₆およびB₇には、それぞれ、（19）式の変換マトリクス
M₁，M₂，M₃，M₄，M₅，M₆およびM₆がロードされ、バッフ
ァレジスタb₁〜b₆には、ぞれぞれ、微分された変換マト
リクスM₁′〜M₆′がロードされる。次のユニットタイム
では、乗算回路MP₁によってバッファレジスタB₅にロー
ドされていたM₅とバッファレジスタB₆にロードされてい
たM₆とが乗算され、バッファレジスタB₆にはM₅×M₆がロ
ードされる。同じように、乗算回路MP₂によって、バッ
ファレジスタb₆には、M₅′×M₆がロードされる。またこ
の間に、各ブロックのマトリクス（微分されたものも含
む）は次の乗算に備えて１ブロックずつシフトされる。
次のユニットタイムでは、バッファレジスタB₆にM₄×M₅
×M₆がロードされ、バッファレジスタb₆にはM₄′×M₅×
M₆がロードされる。このとき、バッファレジスタb₁に
は、先にバッファレジスタb₆にロードされていたM₅′×
M₆がロードされている。また、バッファレジスタB₇には
最初にバッファレジスタB₄にロードされていたM₄がロー
ドされているであろう。以後このことが繰返されて、最
終的にはバッファレジスタB₁，B₂，B₃，B₄，B₅，B₆およ
びB₇には、それぞれ、M₁×M₂×M₃×M₄×M₅×M₆，M₂，
M₃，M₄，M₅，M₆およびM₁×M₂×M₃×M₄×M₅×M₆がロード
される。一方、バッファレジスタb₁，b₂，b₃，b₄，b₅お
よびb₆には、それぞれ、M₁′×M₂×M₃×M₄×M₅×M₆，M₁
×M₂′×M₃×M₄×M₅×M₆，M₁×M₂×M₃′×M₄×M₅×M₆，
M₁×M₂×M₃×M₄′×M₅×M₆，M₁×M₂×M₃×M₄×M₅′×M₆
およびM₁×M₂×M₃×M₄×M₅×M₆′がロードされる。そし
て、マイクロプロセサへはバッファレジスタb₁からデー
タが転送される。

マイクロプロセサはこのようにして求められたヤコビア
ンＪ（）のインバース［Ｊ（）］^-1を求め、さらに
（18）式を用いて各関節角の変化量を求める。

上記バッファレジスタB₇に得られたデータM₁×M₂×M₃×
M₄×M₅×M₆を用いてハンド座標系から絶対座標系への変
換を行ない得ることは前述のとおりである。

好ましい実施例では、第12図の回路に従ったハードウェ
ア回路をCPUとは別の集積回路で構成する。

第13図はこの発明の実施例に用いられる集積回路の一例
を示すブロック図である。この集積回路３は、後に詳細
に説明するがその中には８ビットレジスタが16個配置さ
れたシフトレジスタである。そして、この集積回路３は
マイクロプロセサないしCPUに接続され、入力バスBiに
接続された入力ポートI₀〜I₇を有し、出力バスBoに接続
された出力ポートO₀〜O₇を有する。マイクロプロセサな
いしCPUからのデータは入力バスBiの入力ポートI₀〜I₇
によって与えられ、CPUへは出力バスBoの出力ポートO₀
〜O₇を通してデータが与えられる。他の集積回路からの
データはデータバスD1またはD2によってそれぞれに対応
する入力ポートD1₀〜D1₇またはD2₀〜D2₇に与えられる。
他の集積回路へのデータ出力はDout（D₀〜D₇）から得ら
れる。データの選択はCPUから与えられる信号D₁／D₂に
よって行なわれる。集積回路３は、さらに、その中に含
まれるシフトレジスタのためのシフトクロックCKおよび
その他の信号▲▼，▲▼,R/W,▲▼お
よび▲▼を受ける。

第14図はこの集積回路の詳細を示すブロック図である。
集積回路３は16の８ビットレジスタ300〜315を含み、各
々のレジスタは８つのフリップフロップからなる。たと
えばレジスタ300はフリップフロップ300a〜300hからな
り、レジスタ315はフリップフロップ315a〜315hからな
る。レジスタ300の各々のフリップフロップ300a〜300h
は、各々のデータセレクタ316a〜316hからのデータ出力
を受け、これらフリップフロップ300a〜300hの出力は、
ナンドゲート317a〜317hのそれぞれの１入力に与えられ
るとともに、次段のレジスタ301を構成するそれぞれの
フリップフロップ301a〜301hに与えられる。そして、レ
ジスタ301の内容は順次レジスタ302,…314,315に送られ
る。このようにしてレジスタ300〜315によってシフトレ
ジスタが構成される。レジスタ315の各々のフリップフ
ロップ315a〜315hの出力は、対応のデータセレクタ316a
〜316hに与えられるとともに、データ出力Doutの各ビッ
トD₀〜D₇となる。データセレクタ316a〜316hには、それ
ぞれ、データ入力D1およびD2すなわちD1₀〜D1₇およびD2
₀〜D2₇が与えられ、さらに、入力バスBiすなわちI₀〜I₇
が与えられる。データセレクタ316a〜316hには、さら
に、コントローラ318を経た信号D1/D2および信号R/Wの
反転と信号▲▼の反転との論理積▲／▼・CSが
与えられ、さらに信号▲▼が与えられる。ここ
で、信号D1/D2は、データ入力D1₀〜D1₇およびD2₀〜D2₇
のいずれかを選択的に取込むための指令信号であり、た
とえばこの信号「１」でデータ入力D1₀〜D1₇が選択さ
れ、「０」でD2₀〜D2₇が選択される。信号R/Wは、レジ
スタへのデータ書込みまたはレジスタからのデータ読出
しの選択を行ない、「１」で読出し「０」で書込みとな
る。信号▲▼はチップセレクト信号であり、「０」
でこの素子が選択される。信号▲▼は、ローテー
ション信号であり、レジスタ300〜315の内容をそれぞれ
次段のレジスタにシフトさせるためのものであり、
「０」でそのシフト動作が指令される。このようにシフ
トさせる必要があるのは、データ書込みおよび読出しの
両方においてであり、レジスタ300からしかデータ書込
みやデータ読出しができない構成であるからである。

レジスタ300のフリップフロップ300a〜300hの出力を受
けるナンドゲート317a〜317bのそれぞれの残余の２入力
には、コントローラ318を通して、信号▲▼の反転
と信号R/Wとの論理積CS・R/Wおよび信号▲▼の反
転STBが与えられる。信号▲▼はストローブ信号
である。また、レジスタ300〜315を構成するフリップフ
ロップ300a〜315hには、信号▲▼が与えられ、こ
の信号▲▼はクリア信号である。レジスタ301〜3
15のフリップフロップ301a〜315hには、クロックCKが与
えられる。レジスタ300のフリップフロップ300a〜300h
には、クロックCKに代えて、コントローラ318を通し
て、クロックCKとストローブ信号▲▼の反転とチ
ップセレクト信号▲▼の反転との論理積すなわちCK
・STB・CSが与えられる。

ナンドゲート317a〜317hの出力は出力バスBoの各ビット
B₀〜B₇となる。

第14図に示す集積回路３では、レジスタ300〜315間の16
回のシフトすなわちローテーション動作が、先の第12図
に示すバッファレジスタB₀〜B₇およびb₀〜b₆間の１シフ
トに相当する。そして、信号▲▼の「０」で当該チ
ップが能動化され、信号R/Wが「０」のときデータセレ
クタ316a〜316hを通して、信号D1/D2に依って、データ
入力D1（D1₀〜D1₇）またはD2（D2₀〜D2₇）が、レジスタ
300に与えられる。また、信号R/Wが「１」のとき、ナン
ドゲート317a〜317hが有効化され、レジスタ300のフリ
ップフロップ300a〜300hから、出力バスB₀〜B₇へデータ
が読出される。なお、信号D1/D2は第12図に示す概念図
におけるデータシフト方向を表わす。

レジスタ300〜315間のデータのローテーションは信号▲
▼に応答して行なわれ、信号▲▼に応答し
てすべてのレジスタ300〜315がクリアされる。

第15図は第13図および第14図に示す集積回路３を用いて
第12図の回路を実現するための構成の一例を示すブロッ
ク図である。第12図の回路と機能的に対応する部分には
同じ参照符号を付している。第15図において、バッファ
レジスタB₁〜B₇およびb₁〜b₆が、それぞれ、第12図の対
応の参照符号のものに相当する。それぞれのバッファレ
ジスタは第14図に示す集積回路３を２個ずつ並列に含
み、したがって16ビットの16段シフトレジスタとして構
成されている。乗算回路MP₁は、データセレクタ41およ
び51ならびに乗算器61を含み、乗算回路MP₂はデータセ
レクタ42および52ならびに乗算器62を含む。データセレ
クタ41には、バッファレジスタB₅の出力がX_Rとしてまた
レジスタB₇の出力がX_Lとして与えられる。データセレク
タ51には、レジスタB₆の出力がY_RとしてまたレジスタB₁
の出力がY_Lとして与えられる。乗算器61は、指令信号CM
に応答して、データセレクタ41および51からの入力を乗
算し、その乗算結果は一時記憶レジスタR₁にロードされ
る。一方、乗算回路MP₂に含まれるデータセレクタ42に
はバッファレジスタb₅の出力がX_Rとしてまたバッファレ
ジスタB₇の出力がX_Lとして与えられる。データセレクタ
52にはバッファレジスタB₆の出力がY_Rとしてまたバッフ
ァレジスタb₁の出力がY_Lとして与えられる。乗算器62
は、指令信号CMに応答して、データセレクタ42および52
からのデータを乗算し、その結果を一時記憶レジスタR₂
にロードする。データセレクタ41,51,42および52には、
それぞれ、データ選択信号D1/D2が与えられ、この信号
によって信号X_R，Y_RまたはX_L，Y_Lが選択される。なお、
一時記憶レジスタR₁およびR₂もまた他のバッファレジス
タと同じように第14図に示す集積回路の組合わせからな
り16ビットのシフトレジスタである。

シフトレジスタB₁〜B₇，b₁〜b₆，R₁およびR₂には、入力
バスBiおよび出力バスBoが連結される。そして、レジス
タB₁およびb₁には後述の第16図回路からのシフトタイミ
ング信号Ｖが与えられ、レジスタB₅およびb₅には、シフ
トタイミング信号Ｕが与えられ、一時記憶レジスタすな
わちバッファレジスタR₁およびR₂にはシフトタイミング
信号Ｗが与えられ、他のレジスタB₂〜B₄およびb₂〜b₄に
は、クロック信号φが与えられる。

第16図は第15図に示す演算回路に接続されるべきマイク
ロプロセサに関連する回路を示すブロック図である。第
16図において、発振器101は、クォーツ101aを有し、２
相クロックφ１およびφ２を出力する。一方のクロック
φ１はシフトタイミング発生回路102（後述）に与えら
れ、クロックφ２は乗算指令発生回路103に与えられ
る。シフトタイミング発生回路102からは、シフトタイ
ミング信号U,VおよびＷおよびクロックφが出力される
とともに各レジスタB₁〜B₇，b₁〜b₆，R₁およびR₂内にお
いて、一連のシフトが行なわれたことを表わす信号Ｓが
出力される。シフトタイミング発生回路102からの信号
Ｓはシフト回数検出回路104,シフト方向検出回路105お
よびフリプフロップ107に与えられる。シフト回数検出
回路104は16進カウンタとフリップフロップとを含み、
シフトタイミング発生回路102からの信号Ｓを16カウン
トするごとに出力を出す。すなわち、この回路104は第1
4図に示すレジスタ300〜315間でデータが１循環したこ
とを検出する。シフト方向検出回路105は、５進カウン
タを含み、第12図，第15図に示すバッァレジスタ間にお
いて左方向または右方向に５回シフト動作が行なわれる
ごとに出力を出す。このシフト方向検出回路105の出力
はフリップフロップ106に与えられ、このフリップフロ
ップ106はたとえばトグルフリップフロップからなり、
回路105からの信号が与えられるごとにその出力Ｑを反
転する。このフリップフロップ106の出力はデータ選択
信号D1/D2となる。フリップフロップ107は、たとえばＪ
−Ｋフリップフロップからなり、シフト動作をさせるか
または乗算動作をさせるかを区別するもので、出力が
「０」のときバッファレジスタ間のデータシフト動作
を、出力が「１」のとき乗算動作を行なわせる。

シフト回数検出回路104の出力は、フリップフロップ106
の出力とともにアンドゲート108の入力に与えられる。
アンドゲート108のの出力はフリップフロップ109のリセ
ット入力に与えられる。フリップフロップ109はCPU1か
らのスタート信号STARTによってセットされ、その出力
がエンド信号END（または割込信号）としてCPU1に与
えられる。

フリップフロップ107の出力は４つのアンドゲート11
0,111,112および113のそれぞれの一方入力として与えら
れ、これらアンドゲートの他方入力にはそれぞれシフト
タイミング発生回路102からの信号U,V,Wおよびφが与え
られる。したがってフリップフロップ107の出力が
「１」のとき各バッファレジスタ内のシフトレジスタが
シフトされてそのために乗算動作されることになるのは
前述のとおりである。

CPU1はレジスタ呼出タイミング発生回路114にアドレス
デコーダ（図示せず）からの信号を与える。このレジス
タ呼出タイミング発生回路114は第15図の各レジスタ内
のアドレスを指定するためのものであり、アドレス入力
に対応して初期値をロードすべきレジスタを指定し、第
14図に示す各レジスタ間のデータのシフト動作すなわち
ローテーションを行なわせるための信号▲▼を出
力する。この信号▲▼が「０」のとき各レジスタ
がシフト動作され、「１」のときはシフト動作が行なわ
れない。

CPU1に含まれるアドレスデコーダからの指定信号は、こ
のレジスタ呼出タイミング発生回路114を通してオアゲ
ート115の一方入力に与えられ、このオアゲート115の他
方入力にはアンドゲート113の出力が与えられる。オア
ゲート115の出力はまたオアゲート116,117および118の
一方入力として与えられ、これらオアゲートにはそれぞ
れアンドゲート110,111および112の出力が与えられる。
このようにして、オアゲート115,116,117および118の出
力が、シフトタイミング信号φ,U,VおよびＷとなる。

また、CPU1からは、信号▲▼，▲▼，▲
▼およびR/Wが出力される。

ここで、第17図を参照してシフトタイミング発生回路10
2について詳細に説明する。まず初期状態としてフリッ
プフロップ130,133および136はすべてリセットされてい
る。したがってそれぞれの出力は［１］である。応じ
て、アンドゲート121,122および123の入力が「１」であ
り、アンドゲート124の入力が「０」であり、アンドゲ
ート125の入力が「１」であり、アンドゲート126および
127の入力が「０」である。

アンドゲート121にクロックφ１が与えられると、アン
ドゲート122および125を通して４進カウンタ128にクロ
ックφが与えられる。なお、カウンタ128は、他のカウ
ンタ129,131,132および134と同じように、与えられるカ
ウント入力がそのまま出力される端子と所定のカウント
した後信号を出力するカウントアップ出力端子とを含
む。クロックφがカウンタ128に１個入ると、したがっ
てそのときオアゲート138に出力が出る。このオアゲー
ト138の出力がシフトタイミング信号Ｖとなる。そし
て、カウンタ128に４つのクロックφが与えられると、
このカウンタ128からカウントアップ信号が出力されて
それが４進カウンタ129に入力される。したがって、４
進カウンタ129からオアゲート137を通して、シフトタイ
ミング信号Ｕが出力される。したがって、或る場合に
は、このシフトタイミング信号Ｕは４つのシフトタイミ
ング信号Ｖごとに１つ出力されることになる。カウンタ
129がカウントアップするとすなわちアッゲート125から
16個のクロックφが出力されると、フリップフロップ13
0がセットされ、その出力Ｑが「１」となる。このフリ
ップフロップ130の出力ＱがレジスタR₁，R₂（第15図）
へのデータロード信号すなわちシフトタイミング信号Ｗ
である。

フリップフロップ130の出力Ｑが「１」となり、出力
が「０」となることによって、アンドゲート125および1
27が開かれる。しかしながら、アンドゲート127には、
フリップフロップ133の出力が「１」であるので、こ
のときはアンドゲート124からの出力φは与えられな
い。他方アンドゲート125はこのフリップフロップ130の
出力の「０」によって閉じられる。したがって、この
状態ではアンドゲート121からのクロックφはアンドゲ
ート123および126を介してカウンタ131に与えられる。
そして、ゲート126からクロックφが与えられるごとに
カウンタ131からオアゲート137を通してシフトタイミン
グ信号Ｕが出力される。カウンタ131は12進カウンタで
あり、12個のクロックφがシフトタイミング信号Ｕとし
て出力され、このカウンタ131が12クロックをカウント
すると、カウントアップ出力をカウンタ132およびフリ
ップフロップ130のリセット入力に与える。

したがって、フリップフロップ130がリセットされ、再
びカウンタ128および129が能動化される。そして、前述
の16のクロックφのカウントを繰返し、再びフリップフ
ロップ130がセットされ、カウンタ131が能動化される。
カウンタ131がカウントアップするごとにこのことが繰
返され、その都度カウンタ132に信号が与えられる。こ
の信号を３進カウンタ132によってカウントすると、こ
のカウンタ132がフリップフロップ133をセットする。な
お、カウンタ132にカウンタ131から信号が与えられるご
とに、このカウタン132からオアゲート138を通してシフ
トタイミング信号Ｖが出力される。

フリップフロップ133がリセットされることによって、
その出力が「０」となり、アンドゲート122および123
が閉じられ、アンドゲート124のみが開く。このとき、
フリップフロップ130がリセットされているので、アン
ドゲート127が開いている。したがって、アンドゲート1
21からのクロックφがカウンタ134に与えられる。カウ
ンタ134にクロックが与えられるごとに、オアゲート138
を通してシフトタイミング信号Ｖが出力される。カウン
タ134は13進カウンタであり、クロックφを13カウント
するごとにカウントアップ出力を導出する。このカウン
タ134からのカウントアップ出力がカウンタ135に与えら
れるとともに、フリップフロップ133のリセット入力に
与えられる。カウンタ135は４進カウンタであり、その
カウントアップ出力はフリップフロップ136のセット入
力に与えられる。このフリップフロップ136のリセット
入力にはCPUからのリセット信号が与えられる。フリッ
プフロップ136の出力Ｑが、第16図の信号Ｓである。フ
リップフロップ136がリセットされると、その出力が
「１」となり、アンドゲート121が再び開かれて、先の
動作が繰返される。

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

ところで、第15図に示すような集積回路では、第18図お
よび前式に示すマトリクスの乗算を一斉にすることはで
きないので、たとえばまずU₄₄・V₄₄を行ない、次にU₄₃
→U₄₄，U₄₂→U₄₃，…V₃₄→V₄₄，V₂₄→V₃₄，…のように
データをシフトして行なう。すなわち、この実施例で
は、第18図に示すU₄₄の位置とV₄₄の位置との乗算を行な
いこのそれぞれの位置に必要なデータをシフトないしロ
ーテートさせるのである。

このようなマトリクスにおけるU,VおよびＷのデータの
ローテートないしシフト動作は、シフトタイミング発生
回路（第16図）からのそれぞれ対応のシフトタイミング
信号ごとに１回行なわれる。そして、上記１つの演算が
終わると、データは順次シフトされるが、その順序は第
19図に示す。Ｕについては、１つのシフトタイミング信
号Ｕごとに一列ずつシフトする。したがって、たとえば
第18図の例で、U₄₃の位置にあるデータをU₄₄の位置にシ
フトするためには、１つのシフトタイミング信号Ｕを与
えるだけでよい。このとき、U₄₂は同時にU₄₃の位置に移
されるので、次にＵのデータをU₄₄の位置に移すために
は、再度１つのタイミング信号Ｕを加えるだけでよい。
すなわち、Ｕについては、それぞれのデータをU₄₄の位
置にもたらすためには、それぞれ１つのシフトタイミン
グ信号Ｕを加えるだけでよい。

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

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

簡単に言うと、上記式のW₄₄については、シフトタイミ
ング信号Ｗが１回出力され、続いてシフトタイミング信
号Ｕが１回そしてＶが４回与えられると１つ項たとえば
U₄₄・V₄₄が行なわれるので、このW₄₄のすべての項の演
算のためにはこのことを４回繰返せばよい。そして、次
のW₄₃の演算のために、シフトタイミング信号Ｕを12回
出力するとともにＶを１回シフトしてそのW₄₃の演算の
ための初期状態を設定する。続いてW₄₃の演算のためにW
₄₄の演算と同じようにＷ＝1,U＝１およびＶ＝４を４回
繰返せばよい。以下同様である。

このことが第20図のタイミングチャートに表わされてい
る。

そして、第17図のシフトタイミング発生回路は、第20図
に従って、それぞれのタイミング信号U,VおよびＷを出
力されるように構成されている。すなわち、カウンタ12
8によってタイミング信号Ｖを、カウンタ129によってタ
イミング信号Ｕを、そしてフリップフロップ130によっ
てタイミング信号Ｗを出力する。その後の12個のタイミ
ング信号Ｕはカウンタ131から出力される。

第21図は第15図および第16図に示す実施例の動作を説明
するためのフロー図である。ここで、第21図を参照し
て、この実施例の動作について説明する。まず、CPU1
（第16図）は、動作の最初において、初期設定を行な
う。このとき、必要に応じて信号▲▼を出力し、
第15図に示す各レジスタをクリアする。そして、ステッ
プ201において、このCPU1に含まれるカウンタ（図示せ
ず）の値ｎを１にセットする。なお、この値ｎは０〜15
をとり得る。続くステップ202において、CPU1は第14図
に示すレジスタ300〜315を、カウンタの値ｎに従って指
定する。これはこのCPU1からレジスタ呼出タイミング発
生回路114にレジスタ指定アドレスを出力することによ
って行なわれ得る。したがって、このステップ202で
は、レジスタ呼出タイミング発生回路114（第16図）か
ら、信号▲▼が出力され、該当のレジスタが選択
される。すなわち、第15図に示す各レジスタB₁〜B₇およ
びb₁〜b₆のそれぞれｎ番目のレジスタが指定される。続
くステップ203において、前記（19）式の変換マトリク
スM₁〜M₆およびM₁′〜M₆′をそれぞれ対応のレジスタB₁
〜B₇およびb₁〜b₆（第15図）にストアする。すなわち、
まずそれぞれのレジスタのｎ番目のレジスタ（第14図）
に、Mk,Mk′（ｋ＝１〜６）の１つの要素をストアす
る。したがって、このときは、チップセレクト信号▲
▼によって第15図のそれぞれのレジスタB₁〜B₆および
b₁〜b₆が順次指定される。このようにして、それぞれの
レジスタB₁〜B₇およびb₁〜b₆のそれぞれのｎ番目のレジ
スタにそれぞれ対応の変換マトリクスの１つの要素がス
トアされる。そして、続くステップ204において、カウ
ンタの値が16（これは第14図に示すレジスタの数に対応
する）であるかどうかを判断する。そして、ｎ＝16でな
ければ、ステップ205においてそのカウンタをインクリ
メントし、再び同じことを繰返す。したがって、ステッ
プ204において、ｎ＝16であるということは、すべての
レジスタB₁〜B₇およびb₁〜b₆のすべてのレジスタ300〜3
15（第14図）にそれぞれの変換マトリクスM₁〜M₆および
M₁′〜M₆′のすべての要素がストアされたことを意味す
る。

続いて、CPU1は演算スタート指令STARTを出力する。応
じて、フリップフロップ109がセットされるとともにフ
リップフロップ107がリセットされ、このフリップフロ
ップ107の出力が「１」となる。したがって、アンド
ゲート110〜113が開き、シフトタイミング発生回路102
からのシフトタイミング信号U,VおよびＷおよびクロッ
クφが出力可能となる。すなわち、この状態で乗算が行
なわれる。

乗算は、シフトタイミング発生回路102から、第20図で
示すようなタイミングで各々シフトタイミング信号が発
生し第19図に従ってシフト動作が行なわれることによっ
て、第18図に従ってマトリクスの乗算が行なわれる。こ
の間に、シフト方向検出回路105およびデータ選択フリ
ップフロップ106によって、第15図に示す回路における
データ転送（シフト）方向が制御される。すなわち、第
12図の回路に示すデータシフト方向がこのシフト方向検
出回路105およびフリップフロップ106で制御される。デ
ータシフトを５回繰返すことに応じてフリップフロップ
106の出力がデータ選択信号D1/D2として導出されるが、
このときシフト回数検出回路104からはまだ信号が出さ
れていないので、アンドゲート108は出力を出さない。

前式のW₄₄，W₄₃，…のそれぞれの演算が終わるごとにシ
フトタイミング信号発生回路102からシフト回数検出回
路104に信号が与えられる。シフト回数検出回路104は、
前述のように16進カウンタを含むので、シフトタイミン
グ発生回路102から16個の信号が得られたとき、すなわ
ち前（19）式の演算がすべて行なわれたとき、「１」の
信号を出す。このときフリップフロップ106の出力もま
た「１」であり、アンドゲート108から「１」の信号が
フリップフロップ109のリセット信号として与えられ
る。したがって、このフリップフロップ109からの出力
が「１」となり、それがCPU1に与えられる。CPU1で
は、このフリップフロップ109の出力の状態に応じ
て、ステップ207において、演算終了がどうかを判断す
る。

上述のようにして演算が終了すると、ステップ208,209,
210,211および212において、先のステップ201,202,203,
204および205と同じようにして、第15図に示す各々のレ
ジスタB₁〜B₇およびb₁〜b₆のそれぞれのレジスタ300〜3
15（第14図）からデータを読込む。このとき、データの
書込みと異なり、CPU1からの信号R/Wは「０」とされる
のはもちろんである。このようにして、CPUは、ヤコビ
アンの各々の要素を演算回路２から取込み、前記（18）
式に基づいて各関節角の変化量を求める。このとき、CP
Uは第22図に示すような演算を行なう。

第22図は、Resolved Motion Rate Controlにおける処理
演算ループを示す。すなわち、CPUは第22図に示すよう
に、直角座標系での変化量ｄ_i+1＝_i+1−_ｉを求
め、次いで前述のようにして求めたヤコビアンＪ
（_ｉ）の逆マトリクス［Ｊ（_ｉ）］^-1とｄ_i+1と
を用いて関節座標系の変化量ｄ_i+1を演算する。最後
に、直前の関節系による位置ｄα_ｉと上記変化量ｄ
_i+1とを加算して、次の点の関節系による位置_i+1を得
る。このようにして、直角座標系から関節座標系への座
標変換がヤコビアンを用いて行なわれる。

第23図はシフトタイミング発生回路の別の例を示す回路
図である。この実施例では、シフトタイミング発生回路
102は７ビットのカウンタ141を含む、このカウンタ141
のカウント入力にはクロックφが与えられる。そして、
最上位ビットＡから最下位ビットＧまで７ビットA,B,C,
D,E,FおよびＧを有するバイナリカウンタとして構成さ
れる。そして、シフトタイミング信号U,VおよびＷは、
それぞれ（22）式に従って導出される。

なお、クロックφが、それぞれのシフトタイミング信号
とクロックとの同期をとるために、適当なゲートに入れ
られている。この第23図のようなシフトタイミング発生
回路を用いれば、第17図に示すものに比べて非常に簡単
な回路構成となる。

以上ように、この実施例によれば、直角座標系と関節座
標系との間の座標変換のために、マトリクスの各々の要
素は別のハードウェアで構成した演算回路で演算し、CP
Uではその演算結果を取込んで適当な変換式に代入する
だけでよいので、CPUが座標変換のために演算に利用さ
れる時間が非常に短くなる。したがって、CPUたとえば
マイクロプロセサやマイクロコンピュータとして高速の
ものを用いなくても、極めて正確な座標変換が行なわれ
る。したがって従来のもののように相当大きなエラーが
出たり、同時制御可能な自由度が制限されたりすること
はなく、追随の正確性が期待できる。また、CPUが座標
変換の演算のために利用される時間が非常に短くなるの
で、CPUに対して別の機能を付加することが簡単にでき
る。しかも、演算回路は、ほとんど類似した演算を何回
も繰返すだけであり、制御すべき自由度の増減に対して
も簡単に対応することができる。なぜならば、演算回路
で利用可能な演算機能を単に増減するだけで、CPUのソ
フトウェア（プログラム）についてはほとんど変更が不
要であるからである。このことは、特に、関節を有する
ロボットにおいて、エンドエフェクタの種類を変更する
際に要求される自由度が異なる場合、そのような自由度
の変化に容易に追随できるという極めて大きな効果であ
る。

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

→の変換は、一般には、implicitにしか表現できな
いので、Newton−Raphson等の方法で解く例が過去にあ
った。まだ、それがexplicitに表現できたとしても、複
雑な非線形な方程式となり、それの演算には多大の時間
を要した。そこで、ｄ→ｄの変換を位置フィードバ
ックループの中で行なうことが提案されている。これを
Resolved Motion Rate Controlという。このとき、ｄ
→ｄの関係は線型となり演算時間は短縮されるが、フ
ィードバックのサンプル周波数を実用に耐えるだけに上
げるために、変換は高速で行なう必要がある。このプロ
セスは次の３段階に分割できる。

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

（ii）ヤコビアンのインンバース［Ｊ（）］^-1を求め
る。

（iii）ｄ＝［Ｊ（）］^-1・ｄを求める。 …（1
8）特に（ｉ）のプロセスはソフトウェアを用いる従来の方
法では非常に時間がかかっていた。そこで、ヤコビアン
Ｊ（）を専用ハードで求めたのが上記実施例である。
上記実施例では、ヤコビアンインバース［Ｊ（）］^-1
を求めることおよび（18）式の計算はマイクロコンピュ
ータのソフトウェアで行なっている。

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

第24A図および24B図は、ヤコビアンＪ（）を求める装
置を説明するためのブロック図である。この図は、第2A
図および2B図に相当し、点線で囲んだ部分がヤコビアン
演算部90である。マトリクスシフトレジスタ25および32
がマトリクスマルチプレクサ45の入力部に接続されてお
り、マトリクスマルチプレクサ45の出力部はマトリクス
乗算器47の入力部に接続されている。マトリクス乗算器
47の入力部には、さらに、マトリクスレジスタ46が接続
されている。マトリクス乗算器47の出力部には、マトリ
クスレジスタ48が接続されており、マトリクスレジスタ
48の出力部には、マトリクスレジスタ67が接続されてい
る。マトリクスシフトレジスタ25は、６つのマトリクレ
ジスタ251ないし256を備える。同様に、マトリクスシフ
トレジスタ32は、６つのマトリクレジスタ321ないし326
を備える。

マトリクスシフトレジスタ25は、各自由度の座標変換マ
トリクスM₁ないしM₆を保持する。これらの座標変換マト
リクスは、マイクロコンピュータ（図示せず）から与え
られる。マトリクスシフトレジスタ32は、各自由度の座
標変換マトリクスのその軸の動きに関する微分のマトリ
クスM₁′ないしM₆′を保持する。これらの微分のマトリ
クスもまた、マイクロコンピュータから与えられる。マ
トリクスマルチプレクサ45は、一種のセレクタであり、
マトリクスシフトレジスタ25あるいは32のいずれか一方
を切換えてマトリクス乗算器47に接続する。マトリクス
乗算器47,マトリクスレジスタ48および46は、それぞ
れ、第６図で説明したマトリクス乗算器41およびマトリ
クスレジスタ42および43と同様のものである。マトリク
スレジスタ67は、６×６のマトリクスレジスタである。

次に動作につき説明する。第25Aないし第25C図は、ヤコ
ビアン演算部90の動作を説明するための図である。まず
第25A図を参照して、マトリクスシフトレジスタ25内の
マトリクスレジスタ251ないし256には、それぞれ、マト
リクスM₁ないしM₆が保持されている。マトリクスシフト
レジスタ32内のマトリクスレジスタ321ないし326には、
それぞれ、マトリクスM₁′ないしM₆′が保持されてい
る。マトリクスマルチプレクサ45は、マトリクスシフト
レジスタ32側に切換えられている。マトリクスレジスタ
46は、単位マトリクスＥにイニシャライズされている。
したがって、マトリクス乗算器47においては、マトリク
スM₁′と単位マトリクスＥとの積が求められ、その結果
であるマトリクスM₁′がマトリクスレジスタ48中にロー
ドされる。

次に第25B図を参照して、前述したクロック信号φＡお
よびφＢに応答して、マトリクスシフトレジスタ25およ
び32の中身は、１ブロックだけ図面上で右にローテート
される。マトリクスマルチプレクサ45は、マトリクスシ
フトレジスタ25側に切換えられる（これ以降もこのまま
である）。マトリクスレジスタ46中には、マトリクスレ
ジスタ48中の中身であるマトリクスM₁′がロードされ
る。マトリクス乗算器47においては、マトリクスM₂とマ
トリクスM₁′との積が求められ、その結果であるマトリ
クスM₁′・M₂がマトリクスレジスタ48中にロードされ
る。

以降同様にして、マトリクスM₁′・M₂・M₃，M₁′・M₂・
M₃・M₄，M₁′・M₂・M₃・M₄・M₅がマトリクスレジスタ48
中に順次ロードされ、最後に M₁′・M₂・M₃・M₄・M₅・M₆ …（23）がマトリクスレジスタ48中にロードされる。この時点で
第１の演算過程が終了する。この（23）式は、前記（1
9）式のマトリクス積のうちの１つである。（23）式に
より、ヤコビアンＪ（）の６つの要素∂P_X／∂α_１，
∂P_Y／∂α_１，∂P_Z／∂α_１，∂n_X／∂α_１，∂n_Y／∂
α_１および∂n_Z／∂α_１が求められるので、これらをマ
トリクスレジスタ67の適当な場所、たとえば、第６列に
ロードしておく。

次に、第２の演算過程につき第25C図を参照して説明す
る。まずマトリクスレジスタ251ないし256には、それぞ
れ、マトリクスM₁ないしM₆が保持されており、マトリク
スレジスタ321ないし326には、それぞれ、マトリクス
M₁′ないしM₆′が保持されている。マトリクスマルチプ
レクサ45は、マトリクスシフトレジスタ25側に切換えら
れており、マトリクスレジスタ46中には単位マトリクス
Ｅで初期化されている。マトリクス乗算器47での演算に
より、マトリクスレジスタ48中にはマトリクスM₁がロー
ドされる。

次に、前述と同様に、クロック信号φＡおよびφＢに応
答して、マトリクスシフトレジスタ25および32の中身が
１ブロックだけ図面上で右にローテートされ、マトリク
スレジスタ46にはマトリクスM₁がロードされ、マトリク
スマルチプレクサ45はマトリクスシフトレジスタ32側に
切換えられる。マトリクス乗算器47での演算により、マ
トリクスM₁・M₂′がマトリクスレジスタ48中にロードさ
れる。

さらにマトリクスマルチプレクサ45はマトリクスシフト
レジスタ25側に切換えられ、前述と同様の動作が行なわ
れ、最終的にはマトリクスレジスタ48中に M₁・M₂′・M₃・M₄・M₅・M₆ …（24）がロードされる。この（24）式もまた、前記（19）式の
マトリクス積のうちの１つである。したがって、（24）
式により、ヤコビアンＪ（）のα_２微分に関する６つ
の要素が求まる。そこで、マトリクスレジスタ67の前記
第６列の中身を第５列にシフトするとともに、上記６つ
の要素を第６列にロードする。

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

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

［Ｊ（）］^-1の求め方次に、ヤコビアンインバース［Ｊ（）］^-1を求める方
法およびそのための装置につき説明する。これは、第1A
図および第1B図の座標変換部６の一部分に相当する。

ヤコビアンインバースを求めることは、元連立１次方程
式を解くことに他ならない。そのためには、Gauβ−Jor
danの方法のように、ある種の掃き出しを行なって一気
に解を求める方法が一般的であるが、繰返し近似法によ
る方法もある。この実施例においては、ヤコビアンイン
バースを求めることがサーボレート、つまりResolved M
otion Rateで行なわれるため、アームの状態は微小量ず
つしか変化しない。したがって本実施例は、ヤコビアン
およびヤコビアンインバースが微小変化しかしないこと
を最大限に利用し、繰返し近似法を１回またはせいぜい
数回行なえば充分であるとの見地に立って、ヤコビアン
インバースを求める手段をハードウェア化するものであ
る。これを以下に詳しく説明する。

まず、ヤコビアンＪからヤコビアンインバース（ヤコビ
アンの逆マトリクス）J^-1を近似的に求める方法につい
て説明する。

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

マトリクス［Ｂ］がマトリクス［Ａ］の逆マトリクスで
あるとは、この２つのマトリクスの間に［Ａ］×［Ｂ］
＝［Ｅ］ …（25）なる関係式が成立することを意味する。ここで、［Ｅ］
（ij要素はE_ij）は単位マトリクスである。この関係式
を要素の間の関係式として表わせば、周知のマトリクス
の乗算の定義に従って、となる。この（26）式を変形すれば、が得られる。（27）式は、すべてのi,jの組に対して成
立しており、したがって（27）式は,M×Ｎ個の方程式よ
りなる連立方程式である。

ところで、今考えようとしているのは、実空間の座標変
換に関係したヤコビアンであるから、方程式（27）の各
係数およびその解は実数である。したがって、方程式
（27）からなる量Ｖを定義すれば、このＶは方程式（27）の解に対
して０、他の実数値に対しては正の値となる。そして、
Ｖ＞０の場合においては、Ｖがより０に近いほど良い近
似解となっていることになる。

最急降下法では、求めるべきB_mnに初期値を与えた後、
ｓ番目の近似解から（ｓ＋１）番目の近似解を求める式によって繰返し行ない、より高い精度を持つ近似解を求
める。（28）式の右辺の第２項は、V^(S)の▲Ｂ^(S) _mn▼
についての偏微分を含んでいることからわかるように、
ｓ番目の近似解をＶの勾配の方向に移動させて（ｓ＋
１）番目の近似解を得ようとするものである。（29）式
のλ^(S)は、▲Ｂ^(S) _mn▼においてV^(S)が微小となるよう
に定める。すなわち方程式からλ^(S)を求めればよい。（30）式をλ^(S)について解
くことは一般に複雑となることから、V^(S)をλ^(S)につ
いてテーラー展開し、最低次項のみを残すという近似を
行なえばが得られる。

と置けば、 λ^(S)＝V^(S)／U^(S) …（33）となる。V^(S)は、（33）式で定義されるから具体的にはとなる。

以上の方法をまとめると、（ｉ）ｓ番目の近似解である逆マトリクス［B^(S)］を用
いて［A^(S)］×［B^(S)］＝［A^(S)B^(S)］ …（35）を求め、これからマトリクス［Ｅ−A^(S)B^(S)］を求め
る。このマトリクスの各成分の自乗和を計算してこれを
V^(S)とする。

（ii）式［A^T］×［Ｅ−A^(S)B^(S)］＝［A^T（Ｅ−A^(S)B^(S))］ …
（36）によって右辺のマトリクスを作る。具体的な計算からわ
かるように、右辺のマトリクスのmn成分要素は∂V^(S)／
∂▲Ｂ^(S) _mn▼となっている。このマトリクスの各成分
の自乗和を計算するとU^(S)となっている。但しＴは、転
置マトリクスを表わす。

（iii）V^(S)とU^(S)からλ^(S)＝V^(S)／U^(S)を作る。

（iv）以上の諸量を（29）式に対応する式［B^(S+1)］＝［B^(S)］−λ⁽³⁾［A^T(S)(E-A^(S)B^(S))］…
（37）の右辺に代入すれば、（ｓ＋１）番目の近似逆マトリク
ス［B^(S+1)］が得られる。

以上は任意のマトリクス［Ａ］の逆マトリクス［Ｂ］を
求める方法につき説明した。したがって、以上の方法に
よりヤコビアンＪのインバースJ^-1を求めるには、マト
リクス［Ａ］をヤコビアンＪと、マトリクス［Ｂ］をヤ
コビアンインバースJ^-1と考えればよい。

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

第26図は、ヤコビアンインバースJ^-1を求める装置を説
明するためのブロック図である。この図は第2B図に相当
し、点線で囲んだ部分がヤコビアンインバース演算部40
0である。第27A図ないし第27D図は、ヤコビアンインバ
ース演算部400の動作を説明するための図である。

まず第26図を参照して、バッファレジスタ69の入力部は
マイクロコンピュータ（図示せず）に接続されており、
出力部はマトリクス乗算器70の一方の入力部に接続され
ている。マトリクス乗算器70の他方の入力部は、後述す
るマトリクスレジスタ75の出力部に接続されている。マ
トリクス乗算器70の出力部は、マトリクス加算器71の一
方の入力部に接続されており、マトリクス加算器71の他
方の入力部はマトリクスレジスタ72の出力部に接続され
ている。マトリクス加算器71の出力部は、マトリクスレ
ジスタ72の入力部に接続されており、マトリクスレジス
タ72の出力部はマトリクスマルチプレクサ73の一方の入
力部に接続されている。マトリクスマルチプレクサ73の
他方の入力部は、後述するマトリクスレジスタ77の出力
部に接続されている。マトリクスマルチプレクサ73の出
力部は、マトリクス乗算器74の一方の入力部に接続され
ており、マトリクス乗算器74の他方の入力部はマトリク
スレジスタ68の出力部に接続されている。マトリクスレ
ジスタ68の入力部は、前述したマトリクスレジスタ67の
出力部に接続されている。マトリクス乗算器74の出力部
はマトリクスレジスタ75の入力部に接続されており、マ
トリクスレジスタ75の出力部はマトリクス減算器76の一
方の入力部に接続されている。マトリクス減算器76の他
方の入力部にはマトリクスレジスタ88が接続されてい
る。このマトリクスレジスタ88は、マイクロコンピュー
タから与えられる０または単位マトリクスＥを保持す
る。このマトリクスレジスタ88は、このように、定数マ
トリクスを保持するためのレジスタであるので、ホスト
システムからの制御信号を利用することにより、マトリ
クスレジスタ88を用いないでこれと同等の動作を実現す
ることも可能である。マトリクス減算器76の出力部は、
乗算・加算器78の２つの入力部およびマトリクスレジス
タ77の入力部に接続されている。乗算・加算器78の出力
部はバッファレジスタ79を介してマイクロコンピュータ
（図示せず）に接続されている。これらの各機器の個々
の機能は先に説明したものと同様である。たとえば、マ
トリクス乗算器70および74の機能は、先に説明したマト
リクス乗算器41の機能と同様である。すなわち、後述す
るように、バッファレジスタ69からはスカラー量λが、
マトリクスレジスタ75からはマトリクス［A^T（Ｅ−A
B）］がそれぞれ出力され、それらの積がマトリクス乗
算器70において求められるのであるが、スカラー量λと
マトリクス［A^T（Ｅ−AB）］との積λ［A^T（Ｅ−AB）］
は、 λ［A^T（Ｅ−AB）］＝λＥ×［A^T（Ｅ−AB）］と書くこ
とができる。ここで、Ｅは単位マトリクスである。した
がって、バッファレジスタ69に保持されているスカラー
量λを次のマトリクスλＥとみなすことができる。

したがって、マトリクス乗算器70の機能はマトリクス乗
算器41の機能と同様であると言っても差支えはない。マ
トリクスレジスタ68,72,75および77は、それぞれ６×６
のマトリクスレジスタであり、その機能は先に説明した
マトリクスレジスタ67のそれと同様である。マトリクス
マルチプレクサ73の機能は、先に説明したマトリクスマ
ルチプレクサ45の機能と同様である。乗算・加算器78の
機能は、先に説明した乗算・加算器82の機能と同様であ
る。マトリクス加算器71は、２つのマトリクスの加算を
行なう。なお、このマトリクス加算器71に、２つのマト
リクス間の減算を行なわせることは容易である。マトリ
クス減算器76は、２つのマトリクス間の減算を行なう。

次にヤコビアンインバース演算部400の動作につき説明
する。なお、以下の動作は、前述したクロック信号φＡ
およびφＢに応答して行なわれる。

まず第27A図を参照して、マトリクス乗算プロセスとそ
の結果につき説明する。マトリクスレジスタ68は、たと
えばマトリクスレジスタ67から先に説明した（35）式の
マトリクス［A^(S)］を受取る。マトリクスレジスタ72に
は（35）式のマトリクス［B^(S)］が保持されているもの
とする。マトリクスマルチプレクサ73はマトリクスレジ
スタ72側に切換えられており、マトリクス乗算器74にお
いて（35）式の乗算が行なわれ、その結果がマトリクス
レジスタ75中に保持される。

次に第27B図を参照して、シフトプロセスとその結果に
つき説明する。マトリクスレジスタ88中には単位マトリ
クスＥが保持されており、マトリクス減算器76において
［Ｅ−A^(S)B^(S)］を求める演算が行なわれ、その結果が
マトリクスレジスタ77中に保持される。さらに、乗算・
加算器78においてこのマトリクスの各成分の自乗和が計
算され、その結果であるV^(S)がバッファレジスタ79中に
保持され、そこからそれがマイクロコンピュータに出力
される。

さらに第27C図を参照して、他のマトリクス乗算プロセ
スとその結果につき説明する。マトリクス乗算プロセス
において、マトリクスレジスタ68からデータを供給する
順序を制御することにより、マトリクスレジスタ68中に
はみかけ上転置マトリクスA^T(S)があったようにするこ
とができる。マトリクスマルチプレクサ73はマトリクス
レジスタ77側に切換えられる。マトリクス乗算器74にお
いて前述した（36）式の演算が行なわれ、その結果がマ
トリクスレジスタ75中に保持される。

最後に第27D図を参照して、他のシフトプロセスとその
結果につき説明する。マトリクスレジスタ88から出力は
与えられず、それゆえ乗算・加算器78においては上述し
た（36）式の右辺の各成分の自乗和が計算され、その結
果であるU^(S)がバッファレジスタ79中に保持される。こ
のU^(S)はマイクロコンピュータに与えられ、マイクロコ
ンピュータにおいて先の演算過程で求めたV^(S)とこのU
^(S)とを用いて（33）式のλ^(S)が求められる。一方、バ
ッファレジスタ69中にはマイクロコンピュータから与え
られたλ^(s-1)が保持されている。（ｓ−１）は前回の
演算結果を意味している。これは、今回の演算結果が間
に合わないために前回の演算結果を用いるものである
が、大きな問題はない。マトリクス乗算器70において先
に説明した（37）式の第２項の演算が行なわれ、さらに
マトリクス加算器71において（37）式の演算が行なわれ
その結果がマトリクスレジスタ72中に保持される。

したがって、以上に説明したすべての演算を行なうこと
により、マトリクスレジスタ72の出力部に、前回の演算
によるマトリクス［Ｂ］を初期値とした、マトリクス
［Ａ］の逆マトリクスの新しい近似マトリクス［Ｂ］を
得ることができる。それゆえ、マトリクス［Ａ］をヤコ
ビアンＪとすれば、すなわちマトリクスレジスタ68がマ
トリクスレジスタ67からヤコビアンＪを受取るようにす
れば、マトリクスレジスタ72の出力部にヤコビアンイン
バースJ^-1を得ることができる。

以上のようなヤコビアンインバースJ^-1をもしもコンピ
ュータのソフトウェアを用いて求めるとすれば、その演
算時間は非常に長いもの、たとえば30m sec程度となる
が、この実施例のように専用のハードウェアによって求
める場合には演算時間が非常に短く、たとえば1m secあ
るいはそれ以下になることが確認されている。

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

第28図は、ｄからｄへの座標変換部を説明するため
のブロック図である。この図は、第2B図に相当する。座
標変換部６は、前述したヤコビアン演算部90,ヤコビア
ンインバース演算部400に加えてさらにバッファレジス
タ60,マトリクス乗算器61およびバッファレジスタ62を
備える。バッファレジスタ60の入力部はマイクロコンピ
ュータ（図示せず）に接続されており、その出力部はマ
トリクス乗算器61の一方の入力部に接続されている。マ
トリクス乗算器61の他方の入力部は前述したマトリクス
レジスタ72の出力部に接続されている。マトリクス乗算
器61の出力部はバッファレジスタ62の入力部に接続され
ており、バッファレジスタ62の出力部はマイクロコンピ
ュータに接続されている。

マトリクス乗算器61は、前述したマトリクス乗算器41と
同様の機能を有しており、２つのマトリクスの乗算を行
なう。すなわち、後述するように、バッファレジスタ60
からはベクトル量ｄが、マトリクスレジスタ72からは
マトリクス［Ｊ（）］^-1がそれぞれ出力され、それら
の積がマトリクス乗算器61において求められるのである
が、ベクトル量ｄとマトリクス［Ｊ（）］^-1との積
［Ｊ（）］^-1ｄは、［Ｊ（）］^-1ｄ＝［Ｊ（）］^-1×［ｄ］と書くことができる。したがって、バッファレジスタ60
に保持されているベクトル量ｄを次のマトリクスとみ
なすことができる。

［ｄ］したがって、マトリクス乗算器61の機能はマトリクス乗
算器41の機能と同様であると言っても差支えない。この
ことは、後述するマトリクス乗算器58および65について
も同様である。前述したように、ヤコビアン演算部90内
のマトリクスレジスタ67の出力部にヤコビアンＪ（）
が与えられ、かつ、ヤコビアンインバース演算部400内
のマトリクスレジスタ72の出力部にヤコビアンインバー
ス［Ｊ（）］^-1が与えられる。一方、ホストシステム
４内のマイクロコンピュータからバッファレジスタ60に
対してｄが与えられる。マトリクス乗算器61におい
て、前述した（18）式の演算が行なわれその結果がバッ
ファレジスタ62中に保持される。以上によってｄから
ｄへの座標変換が行なわれる。

以上の座標変換装置によれば、座標変換に要する時間は
1m sec以下になる。従来においては、演算速度があまり
にも遅いため、５次元あるいは６次元においてこのよう
な座標変換を行なうことができなかった。またソフトウ
ェアを用いて座標変換を行なう場合には演算時間が20m
sec程度要していたけれども、この実施例によれば前述
したように演算時間が1m sec以下となり、ソフトウェア
よりも制御性能がはるかに向上する。これは、ヤコビア
ンＪ（）を求めるプロセス，ヤコビアンインバース
［Ｊ（）］^-1を求めるプロセスおよび座標変換のプロ
セスの各プロセスでの演算速度が非常に速いからであ
る。

d²からd²への変換次に、d²からd²への座標変換の方法およびそのため
の装置につき説明する。これは、第1A図および第1B図の
座標変換部７に相当する。d²は、後で説明する運動方
程式の逆問題を解いてフィードフォワード制御を行なう
ために必要である。

まず、d²からd²に変換する方法につき説明する。フ
ィードフォワード制御とは、各関節の動き、したがって
エンドエフェクタの動きを指示し、その動きを達成する
ためにどのような力を与えればよいかを演算し、その力
を発生させるようにするものである。したがって、d
²α,dαおよびα，すなわち、，，α，というα系
での運動を記述する量を与えてその動きを実現するため
のトルクτを演算しなければならない。

ところで、ロボットの応用側の要求としては、エンドエ
フェクタの動きは絶対座標系で与えることが便宜であ
る。このためd²X,dXすなち，の形でエンドエフェク
タの先端の動きを定めることとなり、，から，
への変換を必要とする。このうちからへの変換が既
に述べたので、ここではからへの変換について説明
する。

まず、マトリクス［Ｍ］を時間で微分するととなり、さらにこれを時間で微分すればとなる。これの右辺の第２項を左辺に移行すればとなる。ところで、（40）式の左辺の第１項は［Ｍ］の
２階の時間微分であって、これはに関係する量であ
る。左辺の第２項のうちについてはから求められる。ところで（40）式の右辺はと書けるから、（40）式の両辺に左からヤコビアンイン
バース［J¹］を掛ければが得られる。したがって、（42）式の左辺の各量を演算
すれば右辺のが得られる。（42）式はベクトル形式で書かれておりであるから、（43）式は連立方程式である。

（43）式の左辺の諸量のうち、ΣΣすなわち、を新たに求めることが必要である。ここで（44）式の各
項をHijと書けば、（44）式は、ただし、 H_ij＝H_ji …（48）となる。このH_ijを用いて（44）式、すなわち（46）式
を具体的に書き下せば、となる。ここでは、（48）式の関係を利用している。
（49）式の各項の一例を具体的に示せば、H₁₁＝（Ｍ″₁
▲² ₁▼）M₂M₃M₄M₅M₆ H₁₂＝（Ｍ′₁ _１）（Ｍ′₂ _２）M₃M₄M₅M₆である。（4
6）式は36個の項を含み、これが（49）式においては、
（48）式の関係を用いて21個の項へと書替えられてい
る。ところが、これらの項の間には一定の規則性があ
り、M₁ないしM₆，M₁′ないしM₆′，M₁″ないしM₆″を規
則的に組合わせることによって比較的簡単に（49）式の
値を得ることができる。

次に、以上に説明した方法を実現してd²からd₂に変
換する装置につき説明する。

第29A図は、（44）式に示したΣΣの演算を行なう装置
を説明するためのブロック図である。この図は、第2A図
に相当する。点線で囲んだ部分がΣΣ演算部500であ
る。第29B図は、d²の演算を行なう装置を説明するた
めのブロック図である。この図は、第2B図に相当する。
点線で囲んだ部分がd²演算部600である。ΣΣ演算部5
00およびd²演算部600で第1A図および第1B図の座標変
換部７が構成される。

第30A図および第30B図は、ΣΣ演算部500の動作を説明
するための図である。

まず第29A図を参照して、バッファレジスタ19の入力部
はマイクロコンピュータ（図示せず）に接続されてお
り、さらにその入力部と出力部は結合されている。バッ
ファレジスタ19の出力部は、マトリクス乗算器20の２つ
の入力部およびマトリクスマルチプレクサ22の一方の入
力部に接続されている。マトリクス乗算器20の出力部は
マトリクス乗算器30の一方の入力部に接続されており、
マトリクス乗算器30の他方の入力部にはマトリクスシフ
トレジスタ35の出力部が接続されている。マトリクスマ
ルチプレクサ22の出力部はマトリクス乗算器23の一方の
入力部に接続されており、マトリクス乗算器23の他方の
入力部はマトリクスシフトレジスタ32の出力部に接続さ
れている。マトリクス乗算器23の出力部は、マトリクス
乗算器24の一方の入力部およびマトリクス乗算器34の一
方の入力部に接続されている。マトリクス乗算器24の他
方の入力部は、マトリクスレジスタ43の出力部に接続さ
れている。マトリクス乗算器27の一方の入力部はマトリ
クスシフトレジスタ25の出力部に接続されており、他方
の入力部はマトリクスレジスタ26の出力部に接続されて
いる。マトリクス加算器28の２つの入力部はそれぞれマ
トリクス乗算器24の出力部およびマトリクス乗算器27の
出力部に接続されており、マトリクス加算器28の出力部
はマトリクスレジスタ29の入力部に接続されており、マ
トリクスレジスタ29の出力部はマトリクスレジスタ26の
入力部に接続されている。マトリクス乗算器31,34およ
び36,マトリクス加算器33および37およびマトリクスレ
ジスタ38および39の接続については前述とほぼ同様であ
る。マトリクスレジスタ38の出力部にはバッファレジス
タ40の入力部が接続されている。

マトリクスシフトレジスタ25,マトリクス乗算器41,マト
リクスレジスタ42および43については、第６図において
座標変換部５として既に説明した。さらに、マトリクス
シフトレジスタ32についても、第24A図においてヤコビ
アン演算部90の一部分として既に説明した。マトリクス
シフトレジスタ35の機能は、既に説明したマトリクスシ
フトレジスタ25および32の機能と同様である。マトリク
ス乗算器20,23,24,27,30,31,34および36は、既に説明し
たマトリクス乗算器41の機能と同様である。マトリクス
マルチプレクサ22の機能は、第24A図において説明した
マトリクスマルチプレクサ45の機能と同様である。マト
リクス加算器28,33および37の機能は、第26図において
説明したマトリクス加算器71の機能と同様である。マト
リクスレジスタ29および38の機能は、既に説明したマト
リクスレジスタ42の機能と同様である。マトリクスレジ
スタ26および39の機能は、既に説明したマトリクスレジ
スタ43の機能と同様である。バッファレジスタ19および
40の機能もまた、第６図において既に説明したバッファ
レジスタ44の機能と同様である。

次に第29B図を参照して、バッファレジスタ63の入力部
はマイクロコンピュータ（図示せず）に接続されてお
り、その出力部はマトリクス減算器64の一方の入力部に
接続されている。マトリクス減算器64の他方の入力部は
前述したバッファレジスタ40の出力部に接続されてい
る。マトリクス減算器64の出力部はマトリクス乗算器65
の一方の入力部に接続されており、マトリクス乗算器65
の他方の入力部は前述したマトリクスレジスタ72の出力
部に接続されている。マトリクス乗算器65の出力部はバ
ッファレジスタ66の入力部に接続されており、バッファ
レジスタ66の出力部はマイクロコンピュータに接続され
ている。

バッファレジスタ19は、マイクロコンピュータから与え
られた₁ないし₆を保持し、かつそれらをローテート
させている。マトリクス減算器64の機能は、第26図にお
いて説明したマトリクス減算器76の機能と同様である。
バッファレジスタ63,マトリクス乗算器65およびバッフ
ァレジスタ66の機能についても、第28図において説明し
たバッファレジスタ60,マトリクス乗算器61およびバッ
ファレジスタ62の機能とそれぞれ同様である。

次に、第30A図および第30B図を参照して、マトリクスレ
ジスタ38内に前述したΣΣがロードされる動作につき説
明する。なお、以下の動作は、前述したクロック信号φ
ＡおよびφＢに応答して行なわれる。

まず30A図を参照して、第１のユニットタイム間の動作
につき説明する。バッファレジスタ19に保持されていた
₁がマトリクス乗算器20に与えられ、マトリクス乗算
器20において1/2₁ ²が求められる。マトリクスマルチ
プチレクサ22はバッファレジスタ19側に切換えられてお
り、これ以降も同様である。マトリクスシフトレジスタ
25内のマトリクスレジスタ251ないし256には、マイクロ
コンピュータからそれぞれマトリクスM₁ないしM₆がロー
ドされる。マトリクスシフトレジスタ32内のマトリクス
レジスタ321ないし326には、マイクロコンピュータから
マトリクスＭの１階の時間微分であるマトリクスM₁′な
いしM₆′がロードされる。同様に、マトリクスシフトレ
ジスタ35内のマトリクスレジスタ351ないし356には、マ
イクロコンピュータからマトリクスＭの２階の時間微分
であるマトリクスM₁″ないしM₆″がロードされる。マト
リクス乗算器23においてマトリクスマルチプレクサ22か
らの₁とマトリクスシフトレジスタ32からのM₁′との
積が求められる。マトリクスレジスタ43には単位マトリ
クスＥがロードされている。したがってマトリクス乗算
器24においてはM₁′₁が求められる。マトリクスレジ
スタ26には０がロードされており、したがってマトリク
ス乗算器27での演算結果は０となる。マトリクス加算器
28においてマトリクス乗算器24からのM₁′₁とマトリ
クス乗算器27からの０とが加算されその結果がマトリク
スレジスタ29にロードされる。

マトリクス乗算器30において、マトリクス乗算器20から
のとマトリクスシフトレジスタ35からのM₁″との乗算が行
なわれその結果が求められる。マトリクス乗算器31においてはマトリク
ス乗算器30からのとマトリクスレジスタ43からの単位マトリクスＥとの積
が求められる。マトリクス乗算器34の一方の入力はマト
リクスレジスタ26からの０であるので、そこでの演算結
果は０である。同様に、マトリクス乗算器36の一方の入
力はマトリクスレジスタ39からの０であるので、そこで
の演算結果は０である。したがってマトリクスレジスタ
38にはがロードされる。

次に第30B図を参照して、第２のユニットタイム間の動
作につき説明する。バッファレジスタ19は、その保持し
ていた内容を１ブロックだけローテートさせ、₂を出
力する。マトリクスシフトレジスタ25,32および35にロ
ードされているマトリクスは右方向へ１ブロックだけシ
フトされる。したがって、マトリクスレジスタ251,321
および351には、それぞれ、マトリクスM₂，M₂′およびM
₂″がロードされる。マトリクスレジスタ26には、第１
のユニットタイム間の演算結果であるM₁′₁がマトリ
クスレジスタ29からロードされる。同様に、マトリクス
レジスタ39にはがロードされ、マトリクスレジスタ43にはM₁がロードさ
れる。そして前述と同様の演算により、マトリクスレジ
スタ29にはM₁M₂′₂＋M₁′₁M₂がロードされる。マト
リクスレジスタ38には、がロードされる。マトリクスレジスタ42には、M₁M₂がロ
ードされる。

以降同様にして第６のユニットタイムの後には、前述し
たΣΣがマトリクスレジスタ38中にロードされる。

次に第29B図を参照して、その次のユニットタイム間の
動作につき説明する。マイクロコンピュータからはバッ
ファレジスタ63に対してd²が与えられ、これがさらに
マトリクス減算器64の一方の入力部に与えられる。一
方、マトリクス減算器64の他方の入力部には、バッファ
レジスタ40を介して、先のユニットタイム間に求められ
たΣΣが与えられる。マトリクス減算器64において、先
に説明した（40）式の左辺の演算が行なわれその結果が
マトリクス乗算器65の一方の入力部に与えられる。マト
リクス乗算器65の他方の入力部には、マトリクスレジス
タ72からヤコビアンインバースJ^-1が与えられる。この
ヤコビアンインバースJ^-1の求め方については既に説明
した。マトリクス乗算器65において先に説明した（42）
式の演算が行なわれ、その結果であるd²がバッファレ
ジスタ66にロードされる。このd²はさらにマイクロコ
ンピュータに与えられる。これによってd²からd²へ
の変換が行なわれたことになる。

運動方程式次に、前述したｄおよびd²を使用して運動方程式の
逆問題を解く方法およびそのための装置につき説明す
る。これは、第1A図および第1B図の運動方程式演算部９
に相当する。

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

ラグランジェ形式においては、まず、考えている力学系
の運動エネルギをK,ポテンシャルエネルギをＰとして、
ラグランジアンＬをＬ＝Ｋ−Ｐ …（50）によって求める。ラグランジェの方程式には一般化座標
が現われるが、ここで考えている一般化座標はα_iであ
る。このα_iに対する一般化力をF_iとすると、ラグラン
ジェの運動方程式はで与えられる。運動方程式の逆問題を解くということ
は、（51）式の右辺を計算してF_iを求めることに他なら
ない。

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

（ａ）運動エネルギＫ絶対座標系におけるロボットの各部分の座標をとし、ロボットの各部分の微小部分の質量をdmとする
と、全運動エネルギＫはで与えられる。但しＲは、ロボットの腕の位置状態に依
存しない項であり、たとえばローターのイナーシャ等で
ある。ここで、となることに注意すると、となる。但し、Trはトレース（対角和）を意味する。こ
の積分において、絶対座標系から見たｉ番目の腕の各部
分の座標を_iとすれば、（55）式は、各腕についての
積分の和として書換えることができ、となる。ところで、 _i＝M₁×M₂×…×M_i ⁱ ₀ …（57）と書ける。但し、ⁱ ₀は、ｉ番目の関節に固定された座
標系から見たｉ番目の腕の各部分の位置ベクトルであ
る。したがって、 Mⁱ＝M₁×M₂×…×M_i …（58）なる量Mⁱを定義すれば、X_i＝Mⁱ▲ⁱ ₀▼であるから、と書くことができる。また、Ｒは（a_iは定数）の形に書けるから、となる。但し、Q_iは慣性テンソルであり、で定義される。

（ｂ）ポテンシャルエネルギＰポテンシャルエネルギとしては、重力エネルギが考えら
れる。したがって、重力の加速度を表わすベクトルを
とし、重力の方向に絶対軸の第３成分をとれば、となる。重力マトリクスY_iをによって定義すれが、Ｐはと書くことができる。

（ｃ）ラグランジェの運動方程式（ａ）（ｂ）で用いたK,Pを（50）式に代入してＬを求
め、これを（51）式に代入することによってが得られる。これを整理すればとなる。但し、であって、（67）式および（68）式の各項をそれぞれ▲
Ｒ^(j) _km▼，▲Ｗ^(j) _k▼と書けば、これらの式は具体的
には次の形となる。

ただし、であり、▲Ｗ^(j) _km▼＝▲Ｒ^(j) _mk▼が成立している。

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

ところで、（66）式に従って演算を行なえば、一般化力
F_iを得ることができるわけであるが、この演算をハード
ウェアで行なうことを考慮して、漸化式による方法を採
用することにする。すなわち、 a_j＝a_j-1×M_j，a₀＝Ｅ…（70）として、繰返しを行なえ
ば a_j＝M₁×M₂×…×M_j …（71）が得られる。また、としてｊについて繰返しを行なえば ▲ｂⁱ _j▼＝M₁×M₂×…×M_i′×…×M_j …（73）が得られる。さらに、によって繰返しを行なえば、同様に（66）式の各項に現
われる量を求めることができ、結局、のようになり、これによってF_iを求めることができる。
このうちIa_i _iの項はロボットの腕の位置状態に依存し
ない項であるためその演算はソフトウェアで行ない、ハ
ードウェアではこれを無視することとする。

すなわち、F_iを求めるにあたっては、（75）式のF_i ¹，F
_i ²，F_i ³の（）の中にあるマトリクスの積を計算し、
そのトレース（対角和）を求めて和をとればよい。ここ
で対角和をとるべき（）の中がＡ・B^Tの形をしている
ことに注目すると、対角和の演算はマトリクスＡおよび
Ｂの要素を抜き出してそれらの積の和をとればよいこと
になる。これは次式より明らかである。

次に、前述した（75）式のF_i ¹，F_i ²，F_i ³を求める装置
につき説明する。そのような演算を行なうためには先に
示した第2A図内のすべての装置を用いる。第2A図内の装
置のほとんどは既に説明したので、ここではまだ説明し
ていない部分についてのみ説明する。

マトリクスシフトレジスタ50の出力部がマトリクス乗算
器51の一方の入力部に接続されている。マトリクスシフ
トレジスタ50は、先に説明したマトリクスシフトレジス
タ25,32および35と同様であり、マトリクスレジスタ501
ないし506を備える。マトリクス乗算器51の他方の入力
部にはマトリクスマルチプレクサ49の出力部が接続され
ており、マトリクスマルチプレクサ49の２つの入力部は
先に説明したマトリクスレジスタ26および39の出力部に
接続されている。マトリクス乗算器51の出力部は、マト
リクスレジスタ52を介してマトリクスマルチプレクサ54
の一方の入力部に接続されている。マトリクスマルチプ
レクサ54の他方の入力部には、マトリクスシフトレジス
タ53の出力部が接続されている。このマトリクスシフト
レジスタ53もマトリクスシフトレジスタ50と同様に、マ
トリクスレジスタ531ないし536を備える。マトリクスマ
ルチプレクサ54の出力部はトレース演算回路55の一方の
入力部に接続されており、それの他方の入力部には先に
説明したマトリクスレジスタ48の出力部が接続されてい
る。トレース演算回路55の出力部はバッファレジスタ56
を介してマイクロコンピュータ（図示せず）に接続され
ている。

マトリクスシフトレジスタ50および53の機能は、既に説
明したマトリクスシフトレジスタ25等の機能と同様であ
る。マトリクスマルチプレクサ49および54の機能は、既
に説明したマトリクスマルチプレクサ45等の機能と同様
である。マトリクス乗算器51の機能は、既に説明したマ
トリクス乗算器41等の機能と同様である。マトリクスレ
ジスタ52の機能は、既に説明したマトリクスレジスタ42
等の機能と同様である。バッファレジスタ56の機能は、
既に説明したバッファレジスタ44等の機能と同様であ
る。トレース演算回路55の機能は、（75）式に示したよ
うなトレースTrを求めるものである。

次に、運動方程式を解く動作につき説明する。第31A図
ないし31C図は、運動方程式演算部９の動作を説明する
ための図である。運動方程式を解く動作は、３つのフェ
ーズからなる。

まず、第31A図を参照して、第１のフェーズにつき説明
する。このフェーズにおいては、F_i ³の演算の他に、同
時に、先に説明したからへの変換，d²を求めるた
めのΣΣの計算およびヤコビアンＪの係数を求める動作
も行なわれている。これは、演算の時間を最短にするた
めである。マトリクスシフトレジスタ25内には先には説
明したようにマトリクスM_jがロードされている。マトリ
クスシフトレジスタ32内には、先に説明したマトリクス
M_j′がロードされている。マトリクスマルチプレクサ45
は、ｊ＝ｉのときはマトリクスシフトレジスタ32側に切
換えられ、ｊ≠ｉのときはマトリクスシフトレジスタ25
側に切換えられる。マトリクス乗算器47においてマトリ
クスM_jとマトリクスM_j′との乗算が（72）式に従って行
なわれ、その結果である▲ｂⁱ _j▼がマトリクスレジスタ
48にロードされる。一方、マトリクスシフトレジスタ53
にはマイクロコンピュータ（図示せず）から先に説明し
たマトリクスY_jがロードされている。マトリクスマルチ
プレクサ54はマトリクスシフトレジスタ53側に切換えら
れており、したがってトレース演算回路55の入力部には
▲ｂⁱ _j▼とY_jが入力され、そこにおいて（75）式のF_i ³
の演算が行なわれる。この場合、F_i ³は36のクロック期
間（ｉ＝１ないし6,j＝１ないし６）で求められる。こ
の場合、最初のクロック期間ではまだマトリクスレジス
タ48にはいかなるｂもロードされていないので、マトリ
クスY_jの出力は１クロック期間だけ遅延させる必要があ
る。それには、マトリクスシフトレジスタ53とマトリク
スマルチプレクサ54との間にそのような遅延回路を設け
てもよいし、あるいはそのようにしなくてもマトリクス
シフトレジスタ53の動作を１クロック期間だけ止めても
よい。

なお、以上の演算と同時に次の演算が行なわれる。すな
わち、バッファレジスタ19にはがロードされており、
このフェーズにおいてはマトリクスマルチプレクサ22は
バッファレジスタ19側に切換えられているので、はマ
トリクス乗算器23に与えられる。したがって、マトリク
ス乗算器23においては、_jM_j′の演算が行なわれそれ
がマトリクス乗算器24の一方の入力部に与えられる。マ
トリクス乗算器24の他方の入力部には、前のクロック期
間中に求められた後述するa_j-1がマトリクスレジスタ43
から与えられる。マトリクス乗算器27の一方の入力部に
は、マトリクスシフトレジスタ25からマトリクスM_jが与
えられ、他方の入力部にはマトリクスレジスタ26中に保
持されている前回のクロック期間中に求められた▲ｃ^*
_j-1▼が与えられる。したがってマトリクス乗算器27お
よびマトリクス加算器28において（74）式に示した▲ｃ
^* _j▼の演算が行なわれその結果がマトリクスレジスタ29
にロードされる。一方、マトリクス乗算器41の一方の入
力部にはマトリクスシフトレジスタ25からのマトリクス
Ｍが与えられており、他方の入力部にはマトリクスレジ
スタ43中に保持されている前回のクロック期間中に求め
られたa_j-1が与えられる。したがって、マトリクス乗算
器41においては（70）式に示した演算が行なわれ、その
結果がマトリクスレジスタ42にロードされる。このマト
リクスレジスタ42にロードされるa_jは、言い換えれば、
先に説明したからへ変換した結果ということもでき
る。さらに、マトリクス乗算器20においては（1/2）²
が求められこれがマトリクス乗算器30の一方の入力部に
与えられる。マトリクス乗算器30の他方の入力部にはマ
トリクスシフトレジスタ35からのM_j″が与えられる。前
述と同様の考えで、マトリクス乗算器30および31,マト
リクス加算器33,マトリクス乗算器34および36,マトリク
ス加算器37において（74）式に示したd_jを求める演算が
行なわれ、その結果がマトリクスレジスタ38にロードさ
れる。このd_jは、言い換えれば先に説明したΣΣという
こともできる。

次に、第31B図を参照して、第２のフェーズにつき説明
する。マトリクスシフトレジスタ50には、マイクロコン
ピュータ（図示せず）から先に説明した慣性テンソルQ_j
がロードされている。マトリクスシフトレジスタ50とマ
トリクス乗算器51との間には、第31A図で説明したよう
な遅延手段が設けられても良い。その理由もまた先に示
した通りである。マトリクスマルチプレクサ49は、この
フェーズにおいてはマトリクスレジスタ39側に切換えら
れており、したがってマトリクス乗算器51においては、
マトリクスシフトレジスタ50からのQ_jとマトリクスレジ
スタ39からのd_jとの積が求められる。マトリクスマルチ
プレクサ54は、このフェーズにおいてはマトリクスレジ
スタ52側に切換えられており、したがってトレース演算
回路55においては、マトリクスマルチプレクサ54からの
d_jQ_jとマトリクスレジスタ48からの▲ｂⁱ _j▼とを用いて
（75）式に示したF_i ²を求める演算が行なわれる。このF
_i ²を求める演算もまた、36のクロック期間（ｉ＝１ない
し6,j＝１ないし６）において行なわれる。

さらに、第31C図を参照して、第３のフェーズにつき説
明する。バッファレジスタ21には_jがロードされてい
る。このフェーズにおいてはマトリクスマルチプレクサ
22はバッファレジスタ21側に切換えられており、したが
って_jはマトリクス乗算器23の一方の入力部に与えら
れる。マトリクス乗算器23の他方の入力部にはマトリク
スシフトレジスタ32からのM_j′が与えられる。前述と同
様の考え方により、マトリクスレジスタ29には（74）式
に示したc_jがロードされる。このフェーズにおいては、
第２のフェーズの場合と反対に、マトリクスマルチプレ
クサ49はマトリクスレジスタ26側に切換えられている。
マトリクスマルチプレクサ54は、このフェーズにおいて
もマトリクスレジスタ52側に切換えられている。したが
って、マトリクス乗算器51において、マトリクスシフト
レジスタ50からの慣性テンソルQ_jとマトリクスレジスタ
26からのc_jとの積が求められ、その結果であるc_jQ_jがト
レース演算回路55の一方の入力部に与えられる。トレー
ス演算回路55の他方の入力部にはマトリクスレジスタ48
からの▲ｂⁱ _j▼が与えられており、トレース演算回路55
において（75）式に示したF_i ¹を求める演算が行なわれ
る。このF_i ¹を求める演算もまた、36のクロック期間
（ｉ＝１ないし6,j＝１ないし６）において行なわれ
る。

以上のような動作によって求めたF_i ³，F_i ²およびF
_i ¹は、それぞれバッファレジスタ56からマイクロコンピ
ュータに与えられ、マイクロコンピュータはそれらのも
のを用いて（75）式に示したF_iを求める演算を行なう。

従来は、運動方程式を計算するのには非常に時間がかか
っていた。その計算は実時間では不可能だった。これ
は、関節が複雑に干渉し合い、運動方程式の中に座標変
換が入ってきており、運動方程式の計算には非常に長い
時間を要していたからである。したがって、運動方程式
を解くことは、制御として実用に供し得なかった。しか
しながら、上に説明したようなこの実施例による方法お
よび装置によれば、運動方程式を解くことが実時間で可
能になった。

ｄからｄへの変換次に、ｄからｄへの変換の方法およびそのための装
置につき説明する。これは、第1A図および第1B図の座標
変換部８に相当する。

まず、ｄからｄに変換する方法につき説明する。仮
想仕事の原理を用いると、Ｘ系においてエンドエフェク
タに加える力とそれによる微小変位ｄがなせる仕事
δW_xと、α系において各軸に加える力とそれによる微
小変位ｄがなせる仕事δＷ_αとは等しい。すなわち、 δW_x＝^Tδ＝δＷ＝^T・ｄ …（76）が成立する。ここでｄは先に（17）式で示されている
がこれを改めて表わせばｄ＝・ｄ …（77）となる。したがって上の（76）式および（77）式から次
の（78）式の関係が得られる。

＝^T・ …（78）この（78）式において、α系における力はトルクと
表わしてもよく、また（78）式は微小量についても成立
するので、（78）式は次の（79）式と表わすことができ
る。

ｄ＝^T・ｄ …（79）したがって、（143）式の演算を行なえば、ｄからｄ
への変換が行なわれることになる。

次に、上に述べたような変換を行なう装置につき説明す
る。第32図は、ｄからｄへの座標変換部８を説明す
るためのブロック図である。この図は、第2B図に相当す
る。座標変換部８は、バッファレジスタ57,マトリクス
乗算器58およびバッファレジスタ59を備える。バッファ
レジスタ57の入力部は、マイクロコンピュータ（図示せ
ず）に接続されており、出力部はマトリクス乗算器58の
一方の入力部に接続されている。マトリクス乗算器58の
他方の入力部は、先に説明したヤコビアンインバース演
算部400内のマトリクスレジスタ68の出力部に接続され
ている。マトリクス乗算器58の出力部はバッファレジス
タ59の入力部に接続されており、バッファレジスタ59の
出力部はマイクロコンピュータに接続されている。

前述したように、ヤコビアン演算部90内のマトリクスレ
ジスタ67の出力部にはヤコビアンが出力され、ヤコビ
アンインバース演算部400内のマトリクスレジスタ68の
出力部には、第27C図において説明したように、見かけ
上ヤコビアンの転置マトリクスである^Tが出力され
る。一方マイクロコンピュータからバッファレジスタ57
に対してｄから与えられる。マトリクス乗算器58にお
いて、前述した（79）式の演算が行なわれ、その結果で
あるｄがバッファレジスタ59にロードされ、かつマイ
クロコンピュータに与えられる。以上によりｄからｄ
への変換が行なわれたことになる。

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

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

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

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

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

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

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

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

【図面の簡単な説明】

第1A図および第1B図は、それぞれ、多関節ロボットの制
御装置の全体システムの構成を概略的に示すブロック図
である。第2A図および第2B図は、全体システムの構成を詳細に示
すブロック図である。ここで、第2A図の右端は、第2B図
の左端につながっている。第３図ないし第５図は、多関節ロボットの座標変換を説
明するための模式図である。第６図は、からへの座標変換装置を説明するための
ブロック図である。第７図は、マトリクス乗算器を説明するためのブロック
図である。第８図は、マトリクスレジスタの一例を示すブロック図
である。第９図は、制御装置とマトリクス乗算器との接続を示す
ブロック図である。第10図は、マトリクスレジスタの運用方法を説明するた
めの図である。第11図は、演算回路の基本的な考え方を示す概念図であ
る。第12図は、一実施例の基礎となる演算回路の考え方を説
明する概念図である。第13図は、この発明の一実施例に利用される集積回路を
示すブロック図である。第14図は、第13図の集積回路のより詳細な内部構成を示
す回路図である。第15図は、第14図に示した集積回路を利用した演算回路
のブロック図である。第16図は、演算回路を制御するためのCPU関連回路を示
すブロック図である。第17図は、シフトタイミング発生回路の一例を示す回路
図である。第18図ないし第20図は、マトリクスの乗算過程を説明
し、シフトタイミング発生回路の考え方を説明する図で
あり、特に第18図はマトリクスの乗算の原理を示し、第
19図は演算過程におけるシフト動作を示し、第20図はマ
トリクスの乗算に必要なシフトタイミング信号のタイミ
ングチャートを示す。第21図は、この発明の一実施例の動作を説明するための
フローチャートである。第22図は、Resolved Motion Rate Controlにおける処理
演算ループを示す。第23図は、シフトタイミング発生回路の別の例を示す回
路図である。第24A図および第24B図は、ヤコビアンＪ（α）を求める
装置を説明するためのブロック図である。第25A図ないし第25C図は、ヤコビアン演算部の動作を説
明するための図である。第26図は、ヤコビアンインバースJ^-1を求める装置を説
明するためのブロック図である。第27A図ないし第27D図は、ヤコビアンインバース演算部
の動作を説明するための図である。第28図は、ｄからｄへの座標変換部を説明するため
のブロック図である。第29A図は、（44）式に示したΣΣの演算を行なう装置
を説明するためのブロック図である。第29B図は、d²の演算を行なう装置を説明するための
ブロック図である。第30A図および第30B図は、ΣΣ演算部の動作を説明する
ための図である。第31A図ないし第31C図は、運動方程式演算部の動作を説
明するための図である。第32図は、ｄからｄへの座標変換部を説明するため
のブロック図である。図において、４はホストシステム、５〜８および17〜18
は座標変換部、10は関節部、11はエンコーダ、12はタコ
メータ、13は力検出器、14〜16はゲイン調整部、B₁〜B
₁₂およびb₁〜b₁₁はバッファレジスタ、MP₁〜MP₄は乗算
回路、90はヤコビアン演算部、400はヤコビアンインバ
ース演算部、500はΣΣ演算部、600はd²演算部であ
る。

───────────────────────────────────────────────────── フロントページの続き (51)Int.Cl.⁶ 識別記号庁内整理番号ＦＩ技術表示箇所Ｂ２５Ｊ 13/00 Ｇ０５Ｂ 19/18 Ｇ０５Ｄ 3/12 ３０６Ｚ 7740−3Ｈ (72)発明者山本裕敏兵庫県西宮市田近野町６番107号新明和工業株式会社開発センター内 (72)発明者近藤隆彦兵庫県西宮市田近野町６番107号新明和工業株式会社開発センター内 (56)参考文献特開昭57−120112（ＪＰ，Ａ)

Claims

【特許請求の範囲】

【請求項１】回転や旋回やスライドなどの複数Ｎの自由
度を有する多関節ロボット関節座標系での位置データを
直角座標系での位置データに変換するための座標変換装
置であって、前記各自由度の動きに対応する関節座標系を直角座標系
に変換するための座標変換マトリクスM_jが各自由度ごと
に設定され、かつ各座標変換マトリクスをシフトして順
次出力するマトリクスシフト手段と、２つのマトリクス入力を含み、一方のマトリクス入力に
入力される前記マトリクスシフト手段からの座標変換マ
トリクスM_jと他方のマトリクス入力に入力されるマトリ
クスとを乗算して出力するマトリクス乗算手段と、前記マトリクス乗算手段によって乗算されたマトリクス
をストアして、前記マトリクス乗算手段の出力に与える
マトリクスレジスタ手段と、前記マトリクスレジスタ手段を単位マトリクスＥに初期
化した後、前記マトリクスシフト手段から出力された各
座標変換マトリクスと前記マトリクスレジスタ手段の出
力とを前記マトリクス乗算手段によって乗算させ、関節
座標系を直角座標系に変換し、次式で表わされる各関節
のトルクを計算するために必要な積マトリクスa_jを求め
るように制御する制御手段とを備えた、座標変換装置。 a_j＝a_j-1×M_j，a₀＝Ｅここで、ｊは０ないしＮの整数であり、Ｅは単位マトリ
クスである。
【請求項２】前記マトリクスシフトレジスタ手段は、前
記自由度の数だけ直列に接続されたマトリクスレジスタ
を含む、特許請求の範囲第１項記載の座標変換装置。
【請求項３】複数Ｎの自由度を有する多関節ロボットの
直角座標系での速度データを関節座標系での速度データ
に変換するのに用いるヤコビアンマトリクスを求める演
算方法であって、前記各自由度の動きをそれぞれ関節座標系における座標
変換マトリクスM_jとして変換する第１のステップと、前記各自由度の座標変換マトリクスのその自由度の動き
に関する微分マトリクスＭ′_jを求める第２のステップ
と、前記座標変換マトリクスM_jおよび前記微分マトリクス▲
Ｍ^′ _ｊ▼を用いて次式で表わされる各関節のトルクを計
算するために必要な積マトリクス▲ｂⁱ _j▼を求める第３
のステップとを備える、ヤコビアンマトリクス演算方
法。ここで、ｊは０ないしＮの整数であり、ｉは０ないしＮ
の整数であり、Ｅは単位マトリクスである。
【請求項４】複数Ｎの自由度を有する多関節ロボットの
直角座標系での速度データを関節座標系での速度データ
に変換するのに用いるヤコビアンマトリクスを求める演
算装置であって、前記各自由度の動きに対応する関節座標系における座標
変換マトリクスM_jを自由度の数だけ求める座標変換マト
リクス演算手段と、前記自由度の座標変換マトリクスのその自由度の動きに
関する微分マトリクス▲Ｍ^′ _ｊ▼を求める微分マトリク
ス演算手段と、前記座標変換マトリクスをそれぞれ保持する、相互に接
続された複数の第１の群のマトリクスレジスタとを備
え、ここで前記第１の群のマトリクスレジスタのうちの
１つは前記座標変換マトリクス演算手段に接続されてお
り、さらに前記微分マトリクスをそれぞれ保持する、相互に接続さ
れた複数の第２の群のマトリクスレジスタを備え、ここ
で前記第２の群のマトリクスレジスタのうちの１つは前
記微分マトリクス演算手段に接続されており、さらにそれぞれが前記第１および第２の群のマトリクスレジス
タ内の中央のマトリクスレジスタに接続されていて、前
記第１の群のマトリクスレジスタ内の中央のマトリクス
レジスタに保持されている座標変換マトリクスM_jの乗算
結果と、前記第２の群のマトリクスレジスタからの微分
マトリクス▲Ｍ^′ _ｊ▼とを乗算して、前記第２の群のマ
トリクスレジスタ内の中央のマトリクスレジスタに出力
する複数のマトリクス乗算手段と、前記第１および第２の群のマトリクスレジスタおよび前
記マトリクス乗算手段に接続されていて、それらを制御
することにより前記マトリクス乗算手段において次式で
表わされる各関節のトルクを計算するために必要な積マ
トリクス▲ｂⁱ _j▼が求められるように制御する制御手段
とを備える、ヤコビアンマトリクス演算装置。ここで、ｊは０ないしＮの整数であり、ｉは０ないしＮ
の整数であり、Ｅは単位マトリクスである。
【請求項５】前記第１の群のマトリクスレジスタは、双
方向にシフト可能な第１および第２のマトリクスシフト
レジスタを形成しており、前記第２の群のマトリクスレジスタは、双方向にシフト
可能な第３および第４のマトリクスシフトレジスタを形
成しており、前記第１のマトリクスシフトレジスタ内の一方端のマト
リクスレジスタは、前記座標変換マトリクス演算手段に
接続されており、前記第３のマトリクスシフトレジスタ内の一方端のマト
リクスレジスタは、前記微分マトリクス演算手段に接続
されており、前記複数のマトリクス乗算手段は、第１ないし第４のマ
トリクス乗算手段を備え、前記第１のマトリクス乗算手段は前記第１のマトリクス
シフトレジスタからシフトされてくる演算マトリクスを
保持し、次にシフトされてくる新たな演算マトリクスと
乗算結果とを乗算して前記第２のマトリクスシフトレジ
スタに出力する第１のマトリクス乗算・保持手段を含
み、前記第２のマトリクス乗算手段は、前記第２のマトリク
スシフトレジスタからシフトされてくる演算マトリクス
を乗算して保持し、次にシフトされてくる新たな演算マ
トリクスと乗算結果とを乗算して前記第１のマトリクス
シフトレジスタに出力する第２のマトリクス乗算・保持
手段を含み、前記第３のマトリクス乗算手段は、２つの入力部がそれ
ぞれ前記第３のマトリクスシフトレジスタ内の他方端の
マトリクスレジスタと前記第１のマトリクス乗算・保持
手段に接続されており、出力部が前記第４のマトリクス
シフトレジスタ内の一方端のマトリクスレジスタに接続
されており、前記第４のマトリクス乗算手段は、２つの入力部がそれ
ぞれ前記第４のマトリクスシフトレジスタ内の前記一方
端のマトリクスレジスタと前記第２のマトリクス乗算・
保持手段に接続されており、出力部が前記第３のマトリ
クスシフトレジスタ内の他方端に接続されており、前記制御手段は、前記第１ないし第４のマトリクスシフ
トレジスタを往復シフトさせる、特許請求の範囲第４項
記載のヤコビアンマトリクス演算装置。
【請求項６】前記第１の群のマトリクスレジスタは第１
から第2Nのマトリクスレジスタを備え、ここで第１から
第Ｎのマトリクスレジスタは相互に直列に接続されてお
り、第Ｎ＋１から第2Nのマトリクスの相互に直列に接続
されており、さらに第Ｎ−１から第Ｎ＋２のマトリクス
レジスタは相互にループ状に接続されており、かつ第１
のマトリクスレジスタは前記座標変換マトリクス演算手
段に接続されており、前記第２の群のマトリクスレジスタは第１から第2N−１
の互いに直列に接続されたマトリクスレジスタを備え、
ここで第１のマトリクスレジスタは前記微分マトリクス
演算手段に接続されており、前記複数のマトリクス乗算手段は、２つの入力部がそれぞれ前記第１の群の第Ｎ−１と第Ｎ
のマトリクスレジスタに接続され、出力部が前記第１の
群の第Ｎのマトリクスレジスタに接続された第１のマト
リクス乗算手段と、２つの入力部がそれぞれ前記第１の群の第Ｎと第Ｎ＋１
のマトリクスレジスタに接続され、出力部が前記第１の
群の第Ｎ＋１のマトリクスレジスタに接続された第２の
マトリクス乗算手段と、２つの入力部がそれぞれ前記第１の群の第Ｎのマトリク
スレジスタと前記第２の群の第Ｎ−１のマトリクスレジ
スタに接続され、出力部が前記第２の群の第Ｎのマトリ
クスレジスタに接続された第３のマトリクス乗算手段
と、２つの入力部がそれぞれ前記第１の群の第Ｎ＋１のマト
リクスレジスタと前記第２の群の第Ｎ＋１のマトリクス
レジスタに接続され、出力部が前記第２の群の第Ｎのマ
トリクスレジスタに接続された第４のマトリクス乗算手
段とを備える、特許請求の範囲第４項記載のヤコビアン
マトリクス演算装置。
【請求項７】前記第１および第２の群のマトリクスレジ
スタは、それぞれ、双方向に循環可能な第１および第２
のマトリクス循環レジスタを形成しており、前記複数のマトリクス乗算手段は、第１および第２のマ
トリクス乗算手段を備え、前記第１のマトリクス乗算手段は、マトリクスレジスタ
と組合わされて、既に保持していたマトリクスと新たに
入力されたマトリクスとの積を求めその結果を改めて保
持するマトリクス乗算・保持手段を形成し、前記マトリクス乗算・保持手段は、前記第１のマトリク
ス循環レジスタ内の１つのマトリクスレジスタの出力を
入力とし、前記第２のマトリクス循環レジスタで形成される循環ル
ープは、そのループ内の１つのマトリクスレジスタの出
力と前記マトリクス乗算・保持手段の出力とを乗算する
前記第２のマトリクス乗算手段を経て閉じられており、前記制御手段は、前記第１および第２のマトリクス循環
レジスタを方向を変えて各々１循環させる、特許請求の
範囲第４項記載のヤコビアンマトリクス演算装置。
【請求項８】前記第１の群のマトリクスレジスタは第１
から第Ｎ＋１のマトリクスレジスタを備え、ここで第１
から第Ｎのマトリクスレジスタは相互にループ状に接続
されており、さらに第１から第Ｎ−１および第Ｎ＋１の
マトリクスレジスタも相互にループ状に接続されてお
り、かつ第１のマトリクスレジスタは前記座標変換マト
リクス演算手段に接続されており、前記第２の群のマトリクスレジスタは第１から第Ｎの互
いにループ状に接続されたマトリクスレジスタを備え、
ここで第１のマトリクスレジスタは前記微分マトリクス
演算手段に接続されており、前記複数のマトリクス乗算手段は、２つの入力部がそれぞれ前記第１の群の第Ｎ−１と第Ｎ
のマトリクスレジスタに接続され、出力部が前記第１の
群の第Ｎのマトリクスレジスタに接続された第１のマト
リクス乗算手段と、２つの入力部がそれぞれ前記第１の群の第１と第Ｎ＋１
のマトリクスレジスタに接続され、出力部が前記第１の
群の第Ｎ＋１のマトリクスレジスタに接続された第２の
マトリクス乗算手段と、２つの入力部がそれぞれ前記第１の群の第Ｎのマトリク
スレジスタと前記第２の群の第Ｎ−１のマトリクスレジ
スタに、出力部が前記第２の群の第Ｎのマトリクスレジ
スタに接続された第３のマトリクス乗算手段と、２つの入力部がそれぞれ前記第１の群の第Ｎ＋１のマト
リクスレジスタと前記第２の群の第１のマトリクスレジ
スタに接続され、出力部が前記第２の群の第Ｎのマトリ
クスレジスタに接続された第４のマトリクス乗算手段と
を備える、特許請求の範囲第４項記載のヤコビアンマト
リクス演算装置。
【請求項９】回転や旋回やスライドなどの複数のＮの自
由度を有する多関節ロボットの直角座標系での速度デー
タを関節座標系での速度データに変換するのに用いるヤ
コビアンマトリクスを求める演算装置であって、前記各自由度の動きに対応する関節座標系における座標
変換マトリクスM_jを自由度の数だけ求める座標変換マト
リクス演算手段と、前記各自由度の座標変換マトリクスのその自由度の動き
に関する微分マトリクス▲Ｍ^′ _ｊ▼を求める微分マトリ
クス演算手段と、前記座標変換マトリクス演算手段に接続されていて、前
記自由度の数だけの座標変換マトリクスを保持し、かつ
それらのマトリクスを１つずつシフトして順次出力する
第１のマトリクスシフトレジスタと、前記微分マトリクス演算手段に接続されていて、前記自
由度の数だけの微分マトリクスを保持し、かつそれらの
マトリクスを１つずつシフトして順次出力する第２のマ
トリクスシフトレジスタと、前記第１および第２のマトリクスシフトレジスタに接続
されていて、そのいずれか一方からのマトリクスを切換
えて出力するスイッチ手段と、２つの入力部を有し、それのそれぞれに入力されるマト
リクスの積を求めて積マトリクスを出力するマトリクス
乗算手段と、前記マトリクス乗算手段に接続されていて、前記積マト
リクスを保持しかつ出力するマトリクスレジスタとを備
え、ここで前記マトリクスレジスタは単位マトリクスに
初期化されることができ、前記マトリクス乗算手段の一
方の入力部は前記スイッチ手段の出力部に接続されてお
り、他方の入力部は前記マトリクスレジスタの出力部に
接続されており、さらに前記第１および第２のマトリクスシフトレジスタ，前記
スイッチ手段，前記マトリクス乗算手段および前記マト
リクスレジスタに接続されていて、それらを制御するこ
とにより前記マトリクス乗算手段において次式で表わさ
れる各関節のトルクを計算するために必要な積マトリク
ス▲ｂⁱ _j▼を求めるように制御する制御手段を備える、
ヤコビアンマトリクス演算装置。ここで、ｊは０ないしＮの整数であり、ｉは０ないしＮ
の整数であり、Ｅは単位マトリクスである。
【請求項１０】複数の自由度を有する多関節ロボットの
直角座標系での速度データを関節座標系での速度データ
に変換することを含む用途に用いるヤコビアンマトリク
スＡの逆マトリクスＢを求める演算方法であって、ｓ番目のヤコビアンマトリクスA^(s)と、その近似解であ
る逆マトリクスB^(s)とを用いて、マトリクス〔A
^(s)B^(s)〕を求める第１のステップを備え、ここでｓは
任意の整数であり、さらに前記マトリクス〔A^(s)B^(s)〕と単位マトリクスＥとを用
いてマトリクス〔Ｅ−A^(s)B^(s)〕を求める第２のステッ
プと、前記マトリクス〔Ｅ−A^(s)B^(s)〕の各要素の自乗和を計
算してスカラー量V^(s)を求める第３のステップと、前記マトリクスA^(s)の転置マトリクスA^T(s)と、前記マ
トリクス〔Ｅ−A^(s)B^(s)〕とを用いてマトリクス〔A
^T(s)（Ｅ−A^(s)B^(s)）〕を求める第４のステップと、前記マトリクス〔A^T(s)（Ｅ−A^(s)B^(s)）〕の各要素の
自乗和を計算してスカラー量U^(s)を求める第５のステッ
プと、前記スカラー量V^(s)およびU^(s)を用いてスカラー量λ
^(s)＝V^(s)／U^(s)を求める第６のステップと、前記諸量を用いて次式で表わされる（ｓ＋１）番目の近
似マトリクス〔B^(s+1)〕を求める第７のステップとを備
える、逆マトリクス演算方法。〔B^(s+1)〕＝〔B^(s)〕−λ^(s)〔A^T(s)（Ｅ−A
^(s)B^(s)）〕
【請求項１１】複数の自由度を有する多関節ロボットの
直角座標系での速度データを関節座標系での速度データ
へ変換するのに用いるヤコビアンマトリクスＡの逆マト
リクスＢを求める演算装置であって、ｓ番目のヤコビアンマトリクスA^(s)を保持する第１のマ
トリクスレジスタを備え、ここでｓは任意の整数であ
り、さらに前記マトリクスA^(s)の近似解である逆マトリクスB^(s)を
保持する第２のマトリクスレジスタと、前記第１および第２のマトリクスレジスタに接続されて
いて、前記ヤコビアンマトリクスＡと前記逆マトリクス
Ｂとを用いて、マトリクス〔A^(s)B^(s)〕を求める第１の
マトリクス乗算手段と、単位マトリクスＥを与える初期化手段と、前記第１のマトリクス乗算手段と前記初期化手段に接続
されていて、前記マトリクス〔A^(s)B^(s)〕と前記単位マ
トリクスＥとを用いてマトリクス〔Ｅ−A^(s)B^(s)〕を求
める第１のマトリクス減算手段と、前記第１のマトリクス演算手段に接続されていて、前記
マトリクス〔Ｅ−A^(s)B^(s)〕の各要素の自乗和を計算し
てスカラー量V^(s)を求める第１の乗算・加算手段と、前記第１のマトリクスレジスタと前記第１のマトリクス
減算手段に接続されていて、前記マトリクスA^(s)の転置
マトリクスA^T(s)と、前記マトリクス〔Ｅ−A^(s)B^(s)〕
とを用いてマトリクス〔A^T(s)（Ｅ−A^(s)B^(s)）〕を求
める第２のマトリクス乗算手段と、前記第２のマトリクス乗算手段に接続されていて、前記
マトリクス〔A^T(s)（Ｅ−A^(s)B^(s)）〕の各要素の自乗
和を計算してスカラー量U^(s)を求める第２の乗算・加算
手段と、前記第１および第２の乗算・加算手段に接続されてい
て、前記スカラー量V^(s)およびU^(s)を用いてスカラー量
λ^(s)＝V^(s)／U^(s)を求める演算手段と、前記第２のマトリクス乗算手段および前記演算手段に接
続されていて、前記マトリクス〔A^T(s)（Ｅ−A
^(s)B^(s)）〕および前記スカラー量λ^(s)を用いてマトリ
クスλ^(s)〔A^T(s)（Ｅ−A^(s)B^(s)）〕を求める第３のマ
トリクス乗算手段と、前記第２のマトリクスレジスタおよび前記第３のマトリ
クス乗算手段に接続されていて、前記マトリクスB^(s)と
前記マトリクスλ^(s)〔A^T(s)（Ｅ−A^(s)B^(s)）〕を用い
て次式で表わされる（ｓ＋１）番目の近似マトリクス
〔B^(s+1)〕を求める第２のマトリクス減算手段とを備え
る、逆マトリクス演算装置。〔B^(s+1)〕＝〔B^(s)−λ^(s)（A^T(s)（Ｅ−A^(s)B^(s)）〕
【請求項１２】複数の自由度を有する多関節ロボットの
直角座標系での速度データを関節座標系での速度データ
に変換することを含む用途に用いるヤコビアンマトリク
スの逆マトリクスを求める演算装置であって、マトリクスを保持する第１のマトリクスレジスタと、マトリクスを保持する第２のマトリクスレジスタと、２つの入力部を有し、その一方の入力部が前記第２のマ
トリクスレジスタの出力部に接続されていて、入力の切
換えを行なうスイッチ手段と、２つの入力部を有し、それらは前記スイッチ手段および
前記第１のマトリクスレジスタの出力部にそれぞれ接続
されていて、前記２つの入力部に入力される２つのマト
リクスの乗算を行なう第１のマトリクス乗算手段と、前記第１のマトリクス乗算手段の出力部に入力部が接続
されていて、マトリクスを保持する第３のマトリクスレ
ジスタと、単位マトリクスを与える初期化手段と、２つの入力部を有し、それらは前記第３のマトリクスレ
ジスタおよび前記初期化手段の出力部にそれぞれに接続
されていて、前記２つの入力部に入力される２つのマト
リクス間の減算を行なうマトリクス減算手段と、前記マトリクス減算手段の出力部に入力部が接続されて
いて、マトリクスを保持し、その出力が前記スイッチ手
段の他方の入力部に接続される第４のマトリクスレジス
タと、２つの入力部を有し、それらが共に前記マトリクス減産
手段の出力部に接続されていて、前記２つの入力部に入
力されるマトリクスの各要素の自乗和を求めるマトリク
ス乗算・加算手段と、２つの入力部を有し、その一方の入力部が前記第３のマ
トリクスレジスタの出力部に接続されていて、他方の入
力部にはスカラー量が与えられていて、前記２つの入力
部に入力される２つのマトリクスの乗算を行なう第２の
マトリクス乗算手段と、２つの入力部を有し、それらが前記第２のマトリクス乗
算手段および前記第２のマトリクスレジスタの出力部に
それぞれ接続されていて、出力部が前記第２のマトリク
スレジスタの入力部に接続されていて、前記２つの入力
部に入力される２つのマトリクス間の加算を行なうマト
リクス加算手段とを備える、逆マトリクス演算装置。
【請求項１３】複数の自由度を有する多関節ロボットの
直角座標系での速度データｄを関節座標系での速度デ
ータｄに変換する座標変換方法であって、各自由度の動きをそれぞれ関節座標系における座標変換
マトリクスM_jとして表現する第１のステップと、前記各自由度の座標変換マトリクスのその自由度の動き
に関する微分マトリクス▲Ｍ^′ _ｊ▼を求める第２のステ
ップと、前記座標変換マトリクスM_jおよび前記微分マトリクス▲
Ｍ^′ _ｊ▼を用いて次式で表わされる各関節のトルクを計
算するために必要な積マトリクス▲ｂⁱ _j▼を求める第３
のステップとを備え、ここで、ｊは０ないしＮの整数であり、ｉは０ないしＮ
の整数であり、Ｅは単位マトリクスであり、さらに、前記積マトリクス▲ｂⁱ _j▼からヤコビアンマトリクスＡ
を求める第４のステップと、ｓ番目の前記ヤコビアンマトリクスA^(s)と、その近似解
である逆マトリクスB^(s)とを用いて、マトリクス〔A^(s)
B^(s)〕を求める第５のステップとを備え、ここでｓは任
意の整数であり、さらに前記マトリクス〔A^(s)B^(s)〕と単位マトリクスＥとを用
いて〔Ｅ−A^(s)B^(s)〕を求める第６のステップと、前記マトリクス〔Ｅ−A^(s)B^(s)〕の各要素の自乗和を計
算してスカラー量V^(s)を求める第７のステップと、前記マトリクスA^(s)の転置マトリクスA^T(s)と、前記マ
トリクス〔Ｅ−A^(s)B^(s)〕とを用いてマトリクス〔A
^T(s)（Ｅ−A^(s)B^(s)）〕を求める第８のステップと、前記マトリクス〔A^T(s)（Ｅ−A^(s)B^(s)）〕の各要素の
自乗和を計算してスカラー量U^(s)を求める第９のステッ
プと、前記スカラーV^(s)およびおよびU^(s)を用いてスカラー量
λ^(s)＝V^(s)／U^(s)を求める第10のステップと、前記諸量を用いて次式で表わされる（ｓ＋１）番目の近
似マトリクス〔B^(s+1)〕を求める第11のステップと、〔B^(s+1)〕＝〔B^(s)〕−λ^(s)〔A^T(s)（Ｅ−A
^(s)B^(s)）〕前記マトリクスＢと前記直角座標系での速度データｄ
との積を求め、それによって前記速度データｄを前記
関節座標系での速度データｄに変換する第12のステッ
プとを備える、座標変換方法。
【請求項１４】複数のＮの自由度を有する多関節ロボッ
トの直角座標系での速度データｄを関節座標系での速
度データｄに変換する座標変換装置であって、前記各自由度の動きに対応する関節座標系における座標
変換マトリクスM_jを自由度の数だけ求める座標変換マト
リクス演算手段と、前記各自由度の座標変換マトリクスのその自由度の動き
に関する微分マトリクス▲Ｍ^′ _ｊ▼を求める微分マトリ
クス演算手段と、前記座標変換マトリクスをそれぞれ保持する、相互に接
続された複数の第１の群のマトリクスレジスタとを備
え、ここで前記第１の群のマトリクスレジスタのうちの
１つは前記座標変換マトリクス演算手段に接続されてお
り、さらに、前記微分マトリクスをそれぞれ保持する、相互に接続さ
れた複数の第２の群のマトリクスレジスタを備え、ここ
で前記第２の群のマトリクスレジスタのうちの１つは前
記微分マトリクス演算手段に接続されており、さらに、それぞれが前記第１および第２の群のマトリクスレジス
タ内の中央のマトリクスレジスタに接続されていて、前
記第１の群のマトリクスレジスタ内の中央のマトリクス
レジスタに保持されている座標変換マトリクスM_jの乗算
結果と、前記第２の群のマトリクスレジスタからの微分
マトリクス▲Ｍ^′ _ｊ▼とを乗算して、前記第２の群のマ
トリクスレジスタ内の中央のマトリクスレジスタに出力
する複数のマトリクス乗算手段と、前記第１および第２の群のマトリクスレジスタおよび前
記マトリクス乗算手段に接続されていて、それらを制御
することにより前記マトリクス乗算手段において次式で
表わされる各関節のトルクを計算するために必要な積マ
トリクス▲ｂⁱ _j▼が求められるように制御する制御手段
とを備え、ここで、ｊは０ないしＮの整数であり、ｉは０ないしＮ
の整数であり、Ｅは単位マトリクスであり、さらに、前記第１の群のマトリクスレジスタのうちの１つおよび
前記第２の群のマトリクスレジスタのうちの１つに接続
されていて、前記積マトリクス▲ｂⁱ _j▼に基づいてヤコ
ビアンマトリクスを求め、そのヤコビアンマトリクスか
ら逆マトリクスを求める逆マトリクス演算手段と、前記逆マトリクス演算手段に接続されていて、前記逆マ
トリクスと前記直角座標系での速度データｄとの積を
求め、それによって前記速度データｄを前記関節座標
系での速度データｄに変換する乗算手段とを備える、
座標変換装置。
【請求項１５】複数のＮの自由度を有する多関節ロボッ
トの直角座標系での速度データｄを関節座標系での速
度データｄに変換する座標変換装置であって、前記各自由度の動きに対応する関節座標系における座標
変換マトリクスM_jを自由度の数だけ求める座標変換マト
リクス演算手段と、前記各自由度の座標変換マトリクスのその自由度の動き
に関する微分マトリクス▲Ｍ^′ _ｊ▼を求める微分マトリ
クス演算手段と、前記座標変換マトリクス演算手段に接続されていて、前
記自由度の数だけの座標変換マトリクスを保持し、かつ
それらのマトリクスを１つずつシフトして順次出力する
第１のマトリクスシフトレジスタと、前記微分マトリクス演算手段に接続されていて、前記自
由度の数だけの微分マトリクスを保持し、かつそれらの
マトリクスを１つずつシフトして順次出力する第２のマ
トリクスシフトレジスタと、前記第１および第２のマトリクスシフトレジスタに接続
されていて、そのいずれか一方からのマトリクスを切換
えて出力するスイッチ手段と、２つの入力部を有し、それのそれぞれに入力されるマト
リクスの積を求めて積マトリクスを出力する第１のマト
リクス乗算手段と、前記第１のマトリクス乗算手段に接続されていて、前記
積マトリクスを保持しかつ出力する第１のマトリクスレ
ジスタとを備え、ここで前記第１のマトリクスレジスタ
は単位マトリクスに初期化されることができ、前記第１
のマトリクス乗算手段の一方の入力部は前記スイッチ手
段の出力部に接続されており、他方の入力部は前記第１
のマトリクスレジスタの出力部に接続されており、さら
に、前記第１および第２のマトリクスシフトレジスタ，前記
スイッチ手段，前記第１のマトリクス乗算手段および前
記第１のマトリクスレジスタに接続されていて、それら
を制御することにより前記第１のマトリクス乗算手段に
おいて次式で表わされる各関節のトルクを計算するため
に必要な積マトリクス▲ｂⁱ _j▼が求められるように制御
する制御手段を備え、ここで、ｊは０ないしＮの整数であり、ｉは０ないしＮ
の整数であり、Ｅは単位マトリクスであり、さらに、前記第１のマトリクスレジスタに接続されていて、前記
積マトリクス▲ｂⁱ _j▼から得られるｓ番目のヤコビアン
マトリクスA^(s)を保持する第２のマトリクスレジスタを
備え、ここでｓは任意の整数であり、さらに前記マトリクスA^(s)の近似解である逆マトリクスB^(s)を
保持する第３のマトリクスレジスタと、前記ヤコビアンマトリクスA^(s)とB^(s)とを用いて、マト
リクス〔A^(s)B^(s)〕を求める第２のマトリクス乗算手段
と、単位マトリクスＥを与える初期化手段と、前記マトリクス〔A^(s)B^(s)〕と前記単位マトリクスＥを
用いてマトリクス〔Ｅ−A^(s)B^(s)〕を求める第１のマト
リクス減算手段と、前記マトリクス〔Ｅ−A^(s)B^(s)〕の各要素の自乗和を計
算してスカラー量V^(s)を求める第１の乗算・加算手段
と、前記マトリクスA^(s)の転置マトリクスA^T(s)と前記マト
リクス〔Ｅ−A^(s)B^(s)〕とを用いてマトリクス〔A^T(s)
（Ｅ−A^(s)B^(s)）〕を求める第３のマトリクス乗算手段
と、前記マトリクス〔A^T(s)（Ｅ−A^(s)B^(s)）〕の各要素の
自乗和を計算してスカラー量U^(s)を求める第２の乗算・
加算手段と、前記スカラー量V^(s)およびU^(s)を用いてスカラー量λ
^(s)＝V^(s)／U^(s)を求める演算手段と、前記マトリクス〔A^T(s)（Ｅ−A^(s)B^(s)）〕および前記
スカラー量λ^(s)を用いてマトリクスλ^(s)〔A^T(s)（Ｅ
−A^(s)B^(s)）〕を求める第４のマトリクス乗算手段と、前記マトリクスB^(s)と前記マトリクスλ^(s)〔A^T(s)（Ｅ
−A^(s)B^(s)）〕を用いて次式で表わされる（ｓ＋１）番
目の近似マトリクス〔B^(s+1)〕を求める第２のマトリク
ス減算手段と、〔B^(s+1)〕＝〔B^(s)〕λ^(s)〔A^T(s)（Ｅ−A^(s)B^(s)）〕前記第２のマトリクス減算手段の出力部に接続されてい
て、前記逆マトリクスＢと前記直角座標系での速度デー
タｄとの積を求め、それによって前記速度データｄ
を前記関節座標系での速度データｄに変換する第５の
マトリクス乗算手段とを備える、座標変換装置。
【請求項１６】複数のＮの自由度を有する多関節ロボッ
トにおいて、一般化力F_iで表わされたラグランジェの運
動方程式の逆問題を解く演算方法であって、前記各自由度の動きをそれぞれ関節座標系における座標
変換マトリクスM_jとして表現する第１のステップと、前記各自由度の座標変換マトリクスのその自由度の動き
に関する１階の微分マトリクス▲Ｍ^′ _ｊ▼を求める第２
のステップと、前記各自由度の座標変換マトリクスのその自由度の動き
に関する２階の微分マトリクス▲Ｍ^″ _ｊ▼を求める第３
のステップと、前記各自由度の次式で表わされる慣性テンソルQ_jを求め
る第４のステップとを備え、ここで、_０は、ｉ番目の前記自由度に固定された座標
系から見た位置ベクトルであり、dmは前記ロボットの各
部分の微小部分の質量であり、Ｔは転置マトリクスを意
味しており、さらに前記各自由度の次式で表わされる重力マトリクスY_jを求
める第５のステップを備え、ここで、ｇは重力加速度であり、さらに前記マトリクスM_jを用いて、次式で表わされる各関節の
トルクを計算するために必要なマトリクスa_jを求める第
６のステップを備え、 a_j＝a_j-1×M_j，a₀＝Ｅここで、ｊは０ないしＮの整数であり、Ｅは単位マトリ
クスであり、さらに前記マトリクスM_jおよび▲Ｍ^′ _ｊ▼を用いて、次式で表
わされる各関節のトルクを計算するために必要なマトリ
クス▲ｂⁱ _j▼を求める第７のステップを備え、ここで、ｉは０ないしＮの整数であり、さらに前記マトリクス▲ｂⁱ _j▼を用いてマトリクスを求め、そ
の逆マトリクスと単位マトリクスとを用いてヤコビアン
の逆マトリクスＢを求める第８のステップと、前記逆マトリクスＢを用いて、前記各自由度の関節座標
系での速度マトリクス_ｊを求める第９のステップと、前記マトリクスM_j，▲Ｍ^′ _ｊ▼，a_jおよび_ｊを用い
て、次式で表わされる各関節のトルクを計算するために
必要なマトリクス▲ｃ^* _j▼を求める第10のステップと、 ▲ｃ^* _j▼＝▲ｃ^* _j-1▼×M_j＋a_j-1×_ｊ▲Ｍ^′ _ｊ▼，▲
ｃ^* ₀▼＝０前記マトリクスM_j，▲Ｍ^′ _ｊ▼，▲Ｍ^″ _ｊ▼，a_j，_ｊ
および▲ｃ^* _j▼を用いて、次式で表わされる各関節のト
ルクを計算するために必要なマトリクスd_jを求める第11
のステップと、前記マトリクスd_jを用いて、前記各自由度の関節座標系
での加速度マトリクス_ｊを求める第12のステップと、前記マトリクスM_j，▲Ｍ^′ _ｊ▼，a_jおよび_ｊを用い
て、次式で表わされる各関節のトルクを計算するために
必要なマトリクスc_jを求める第13のステップと、 c_j＝c_j-1×M_j＋a_j-1×_ｊ▲Ｍ^′ _ｊ▼，c₀＝０前記マトリクスY_jおよび▲ｂⁱ _j▼を用いて、次式で表わ
される一般化力▲Ｆ³ _i▼を求める第14のステップとを備
え、ここで、Trはトレースを意味し、さらに前記マトリクスc_jおよび▲ｂⁱ _j▼および前記慣性テンソ
ルQ_jを用いて、次式で表わされる一般化力▲Ｆ¹ _i▼求め
る第15のステップと、前記一般化力▲Ｆ¹ _i▼および▲Ｆ³ _i▼を加算することに
より、前記一般化力F_iを求める第16のステップとを備え
る、運動方程式演算方法。
【請求項１７】前記マトリクスd_jおよび▲ｂⁱ _j▼および
前記慣性テンソルQ_jを用いて、次式で表わされる一般化
力▲F² _i▼を求める第17のステップをさらに備え、前記第16のステップにおいて、前記一般化力▲Ｆ¹ _i▼，
▲Ｆ² _i▼および▲Ｆ³ _i▼を用いて次式で表わされる演算
を行なうことにより、前記一般化力F_iが求められる、特
許請求の範囲第16項記載の運動方程式演算方法。 F_i＝▲Ｆ¹ _i▼＋▲Ｆ² _i▼＋▲Ｆ³ _i▼
【請求項１８】複数Ｎの自由度を有する多関節ロボット
において、一般化力F_iで表わされたラグランジェの運動
方程式の逆問題を解く演算装置であって、前記各自由度の動きをそれぞれ座標変換マトリクスM_jと
して表現する第１の演算手段と、前記各自由度の座標変換マトリクスのその自由度の動き
に関する１階の微分マトリクス▲Ｍ^′ _ｊ▼を求める第２
の演算手段と、前記各自由度の座標変換マトリクスのその自由度の動き
に関する２階の微分マトリクス▲Ｍ^″ _ｊ▼を求める第３
の演算手段と、前記マトリクスM_jを用いて、次式で表わされるマトリク
スa_jを求める第４の演算手段とを備え、 a_j＝a_j-1×M_j，a₀＝Ｅここで、ｊは０ないしＮの整数であり、Ｅは単位マトリ
クスであり、さらに前記マトリクスM_jおよび▲Ｍ^′ _ｊ▼を用いて、次式で表
わされる▲ｂⁱ _j▼を求める第５の演算手段を備え、ここで、ｉは０ないしＮの整数であり、さらに前記マトリクス▲ｂⁱ _j▼を用いてヤコビアンマトリクス
を求め、その逆マトリクスと単位マトリクスとを用いて
ヤコビアンの逆マトリクスＢを求める第６の演算手段
と、前記逆マトリクスＢを用いて、前記各自由度の関節座標
系での速度マトリクス_ｊを求める第７の演算手段と、前記マトリクスM_j，▲Ｍ^′ _ｊ▼，a_jおよび_ｊを用い
て、次式で表わされるマトリクス▲ｃ^* _j▼を求める第８
の演算手段と、 ▲ｃ^* _j▼＝▲ｃ^* _j-1▼M_j＋a_j-1×_ｊ▲Ｍ^′ _ｊ▼，▲ｃ
^* ₀▼＝０前記マトリクスM_j，▲Ｍ^′ _ｊ▼，▲Ｍ^″ _ｊ▼，a_j，_ｊ
および▲ｃい^* _j▼を用いて、次式で表わされるマトリク
スd_jを求める第９の演算手段と、前記マトリクスd_jを用いて、前記各自由度の関節座標系
での加速度マトリクス_ｊを求める第10の演算手段と、前記マトリクスM_j，▲Ｍ^′ _ｊ▼，a_jおよび_ｊを用い
て、次式で表わされる各関節のトルクを計算するために
必要なマトリクスc_jを求める第11の演算手段と、 c_j＝c_j-1×M_j＋a_j-1×_ｊ▲Ｍ^′ _ｊ▼，c₀＝０前記ロボット固有の各自由度の次式で表わされるマトリ
クスY_jおよび前記▲ｂⁱ _j▼を用いて、次式で表わされる
一般化力▲Ｆ³ _i▼を求める第12の演算手段とを備えここで、ｇは重力加速度であり、さらに前記マトリクスc_jおよび▲ｂⁱ _j▼および前記ロボット固
有の各自由度の次式で表わされる慣性テンソルQ_jを用い
て、次式で表わされる一般化力▲Ｆ¹ _i▼を求める第13の
演算手段を備え、ここで、▲ⁱ _j▼は、ｉ番目の前記自由度に固定された
座標系から見た位置ベクトルであり、dmは前記ロボット
の各部分の微小部分の質量であり、Ｔは転置マトリクス
を意味し、さらに前記一般化力▲Ｆ¹ _i▼および▲Ｆ³ _i▼を加算することに
より、前記一般化力F_iを求める第14の演算手段を備え
る、運動方程式演算装置。
【請求項１９】前記マトリクスd_jおよび▲ｂⁱ _j▼および
前記慣性テンソルQ_jを用いて、次式で表わされる一般化
力▲Ｆ² _i▼を求める第15の演算手段をさらに備え、前記第14の演算手段において、前記一般化力▲Ｆ¹ _i▼，
▲Ｆ² _i▼および▲Ｆ³ _i▼を用いて次式で表わされる演算
を行なうことにより、前記一般化力F_iが求められる、特
許請求の範囲第18項記載の運動方程式演算装置。 F_i＝▲Ｆ¹ _i▼＋▲Ｆ² _i▼＋▲Ｆ³ _i▼
【請求項２０】複数の自由度を有する多関節ロボットの
直角座標系での力データｄを関節座標系でのトルクデ
ータｄに変換する座標変換装置であって、ヤコビアンマトリクスＡの転置マトリクスA^Tを求める演
算手段と、前記力データｄおよび前記転置マトリクスA^Tを入力と
し、それらの入力の積を求めるマトリクス乗算手段とを
備える、座標変換装置。