JP3331100B2 - マニピュレータシミュレート装置 - Google Patents

マニピュレータシミュレート装置

Info

Publication number
JP3331100B2
JP3331100B2 JP20418695A JP20418695A JP3331100B2 JP 3331100 B2 JP3331100 B2 JP 3331100B2 JP 20418695 A JP20418695 A JP 20418695A JP 20418695 A JP20418695 A JP 20418695A JP 3331100 B2 JP3331100 B2 JP 3331100B2
Authority
JP
Japan
Prior art keywords
equation
matrix
hand
component
homogeneous
Prior art date
Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
Expired - Fee Related
Application number
JP20418695A
Other languages
English (en)
Other versions
JPH0947987A (ja
Inventor
裕一 佐藤
光徳 平田
次人 丸山
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Fujitsu Ltd
Original Assignee
Fujitsu Ltd
Priority date (The priority date is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the date listed.)
Filing date
Publication date
Application filed by Fujitsu Ltd filed Critical Fujitsu Ltd
Priority to JP20418695A priority Critical patent/JP3331100B2/ja
Priority to US08/618,808 priority patent/US5867631A/en
Publication of JPH0947987A publication Critical patent/JPH0947987A/ja
Application granted granted Critical
Publication of JP3331100B2 publication Critical patent/JP3331100B2/ja
Anticipated expiration legal-status Critical
Expired - Fee Related legal-status Critical Current

Links

Classifications

    • B—PERFORMING OPERATIONS; TRANSPORTING
    • B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
    • B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
    • B25J9/00—Programme-controlled manipulators
    • B25J9/16—Programme controls
    • B25J9/1602—Programme controls characterised by the control system, structure, architecture
    • B25J9/1607—Calculation of inertia, jacobian matrixes and inverses
    • B—PERFORMING OPERATIONS; TRANSPORTING
    • B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
    • B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
    • B25J9/00—Programme-controlled manipulators
    • B25J9/16—Programme controls
    • B25J9/1602—Programme controls characterised by the control system, structure, architecture
    • B25J9/1605—Simulation of manipulator lay-out, design, modelling of manipulator
    • G—PHYSICS
    • G05—CONTROLLING; REGULATING
    • G05B—CONTROL OR REGULATING SYSTEMS IN GENERAL; FUNCTIONAL ELEMENTS OF SUCH SYSTEMS; MONITORING OR TESTING ARRANGEMENTS FOR SUCH SYSTEMS OR ELEMENTS
    • G05B2219/00—Program-control systems
    • G05B2219/30—Nc systems
    • G05B2219/39—Robotics, robotics to robotics hand
    • G05B2219/39062—Calculate, jacobian matrix estimator

Landscapes

  • Engineering & Computer Science (AREA)
  • Automation & Control Theory (AREA)
  • Robotics (AREA)
  • Mechanical Engineering (AREA)
  • Physics & Mathematics (AREA)
  • Mathematical Physics (AREA)
  • Manipulator (AREA)
  • Numerical Control (AREA)

Description

【発明の詳細な説明】
【０００１】
【発明の属する技術分野】本発明は、順次連結された６
本以内のアームを有するマニピュレータの先端の位置と
姿勢を表わす情報に基づいて、その位置と姿勢を実現す
るための、アームどうしを連結する関節の回転角を求め
るマニピュレータシミュレート装置に関する。
【０００２】
【従来の技術】図９は、複数のアームとそれらのアーム
どうしを連結する関節とから構成されたマニピュレータ
の一例を模式的に示した図である。基体１上に複数のア
ーム２＿１，２＿２，……，２＿５が各関節３＿１，３
＿２，……，３＿５で順次連結されている。また先端の
アーム２＿５にはハンド４が連結されている。ハンド４
は、そのハンドを構成する指４ａが自在に開閉して作業
対象物５を把持したり離したりすることができ、また各
関節３＿１，３＿２，……，３＿５は、各関節３＿１，
３＿２，……，３＿５で連結されたアーム２＿１，２＿
２，……，２＿５を自在に回動することができるように
構成されている。このようなマニピュレータを用いる
と、そのアーム２＿１，２＿２，……，２＿５の先端の
ハンド４が所定の位置、所定の姿勢となるようにアーム
を回動させて、例えば作業対象物５を把持して他の位置
まで運ぶ等の作業を行なうことができる。
【０００３】例えばこの図のように構成されたマニピュ
レータにおいて、そのマニピュレータの先端の位置と姿
勢の情報を与え、マニピュレータの先端をその与えられ
た位置と姿勢に移動させるための各関節の角度を求める
ことを、逆運動学（逆キネマテクス）と称する。この逆
キネマテクスは、以下のような分野における応用が可能
である。
【０００４】（１）ロボット 任意のリンク形状を持ったロボットに対し、逆キネマテ
クスが自動的に解かれ、ロボットの作業計画等への適用
が容易になる。 （２）機構設計用ＣＡＤシステム 機構設計では、リンク機構を有した部品の設計を行う場
合が多い、機構設計用ＣＡＤシステムに逆キネマテクス
を組み込むことにより、リンク機構を有する製品が、希
望通りに動くか否かを事前にシミュレーションすること
ができる。 （３）マルチメディアにおけるアニメーション作成 マルチメディアでは、コンピュータグラフィックス、実
画像、音声などをミックスさせた様々な応用が期待され
ているが、コマーシャルや映画の分野ではコンピュータ
グラフィックスを使ったリアリティの高いアニメーショ
ン作成が望まれている。従来のコンピュータグラフィッ
クスの世界では、アニメーションはもっぱらオフライン
的に作成されており、特にアンドロイドモデルに対する
動きは、各間接毎に角度を指定する非常に面倒な方法
や、人間の体に特殊なセンサを付け、コンピュータ上に
動きを取り込むような大掛かりな方法（モーションキャ
プチャリング法）が取られていた。アニメーション作成
ツールに逆キネマテクスを組み込むことにより、アンド
ロイド等のアニメーションモデルの動きを非常に効率よ
く作り出すことができる。 （４）エルゴノミクス（人間工学） 従来、人間との何らかの接触をもつ製品（例えば、椅
子、机、キーボード、……）の設計では、エルゴノミク
ス的な使い易さや快適性の議論が欠けているが、コンピ
ュータグラフィックスにより作成したアンドロイドモデ
ルに逆キネマテクスを組み込むことにより、シミュレー
ション段階でそのアンドロイドモデルに様々な動きをさ
せて製品チェック行うことができる。
【０００５】
【発明が解決しようとする課題】従来、逆キネマテクス
問題において解析解が存在する場合には、一つ一つ対象
毎に代数演算を行い、解析解を求めていた。この場合に
は、解析解が存在する場合とそうでない場合の判断が難
しく、方法全体に対して一般性を欠いていた。また、設
計上、解析解を有するマニピュレータであってもそのマ
ニピュレータを実際に製作すると、キャリブレーション
の結果微小なオフセットを有してしまい、解析解による
逆キネマテクスが役に立たない場合がある。
【０００６】一方、解析解が存在しない場合には、近似
解から出発して数値解法を繰り返すことによって精度を
上げていく、ｈｏｍｏｔｏｐｙ法またはｃｏｎｔｉｎｕ
ａｔｉｏｎ法と呼ばれる方法（具体的には、Ｎｅｗｔｏ
ｎ−Ｒａｐｈｓｏｎ的にｐｒｅｄｉｃｔｏｒ−ｃｏｒｒ
ｅｃｔｏｒを構成して精度を上げていく）が開発されて
いる。この方法は、 （１）数値的に不安定な場合がある。 （２）マニピュレータの物理定数（Ｄ−Ｈパラメータ）
に対する依存性が見えない。 （３）計算負荷が大きい。 といった欠点を有し、実用性を欠いていた。本発明は、
上記事情に鑑み、従来よりも高速に逆キネマテクスを解
くことができるマニピュレータシミュレート方法および
装置を提供することを目的とする。
【０００７】
【課題を解決するための手段】上記目的を達成する本発
明の第１のマニピュレータシミュレート装置は、順次連
結された６本以内のアームを有するマニピュレータの先
端の位置と姿勢を表わす情報に基づいて、該位置と姿勢
を実現するための、アームどうしを連結する関節の回転
角を求めるマニピュレータシミュレート装置において、
絶対座標系からみた前記先端の位置と姿勢をあらわす同
次座標変換行列Ａhandを、
【０００８】
【数２１】
【０００９】但し、Ｃhandは姿勢、ｔhandは位置をあら
わすとし、前記マニピュレータの後端から先端に向かっ
て順次に付された座標系のうち、ｉ番目の座標系からみ
たｉ＋１番目の座標系の位置と姿勢を表わす同次座標変
換行列Ａi を、
【００１０】
【数２２】
【００１１】としたとき、前記同次座標変換行列Ａhand
および前記同次座標変換行列Ａi に基づいて求められる
式（１）の行列方程式
【００１２】
【数２３】
【００１３】但し、 Ｕは２０×１６の実数行列 Ｖは２０×１８の実数行列 ξ＝［ｘ3 ｓ4 ｓ5 ，ｘ3 ｓ4 ｃ5 ，ｘ3 ｃ4 ｓ5 ，ｘ3 ｃ4 ｃ5 ， ｘ3 ｓ4 ，ｘ3 ｃ4 ，ｘ3 ｓ5 ，ｘ3 ｃ5 ，ｓ4 ｓ5 ，ｓ4 ｃ5 ， ｃ4 ｓ5 ，ｃ4 ｃ5 ，ｓ4 ，ｃ4 ，ｓ5 ，ｃ5 ］T ρ＝［ｘ1 2ｘ2 2，ｘ1 2ｘ2 ，ｘ1 2，ｘ1 ｘ2 2，ｘ1 ｘ2 ，ｘ1 ，ｘ2 2， ｘ2 ，１］T ｘi ＝ｔａｎ（θi ／２） を記憶する記憶手段と、前記パラメータｔi の各成分を
数値入力する第１の入力手段と、前記第１の入力手段に
より入力された前記パラメータｔi の各成分の数値を前
記式（１）の行列方程式に代入することにより、前記先
端の位置と姿勢をあらわす同次変換行列Ａhandの各成分
を変数とする式（２）の行列方程式
【００１４】
【数２４】
【００１５】但し、Ｕ（Ａhand），Ｖ（Ａhand）は、そ
れぞれ、前記実数行列Ｕ，Ｖに前記パラメータｔi の各
成分を数値代入することにより生成される、前記同次変
換行列Ａhandの各成分を変数とする行列であることを表
わすを求めるパラメータ数値代入手段と、前記同次変換
行列Ａhandの各成分を数値入力する第２の入力手段と、
前記第２の入力手段から入力された前記同次変換行列Ａ
handの各成分の数値を前記式（２）の行列方程式に代入
することにより、式（３）の数値行列方程式
【００１６】
【数２５】
【００１７】但し、Ｕnum ，Ｖnum は、それぞれ、前記
行列Ｕ（Ａhand），Ｖ（Ａhand）に前記同次変換行列Ａ
handの各成分を数値代入することにより生成される
数値行列であることを表わすを求める数値行列方程式導
出手段と、前記式（３）の数値行列方程式に基づいて、
前記関節の回転角θi (k)（ｋはθi の複数解を相互に識
別する最大１６までの正の整数をあらわす）を求める関
節回転角算出手段とを備えたことを特徴とする。また、
上記目的を達成する本発明の第２のマニピュレータシミ
ュレート装置は、順次連結された６本以内のアームを有
するマニピュレータの先端の位置と姿勢を表わす情報に
基づいて、該位置と姿勢を実現するための、アームどう
しを連結する関節の回転角を求めるマニピュレータシミ
ュレート装置において、絶対座標系からみた前記先端の
位置と姿勢をあらわす同次座標変換行列Ａhandを、
【００１８】
【数２６】
【００１９】但し、Ｃhandは姿勢、ｔhandは位置をあら
わすとし、前記マニピュレータの後端から先端に向かっ
て順次に付された座標系のうち、ｉ番目の座標系からみ
たｉ＋１番目の座標系の位置と姿勢を表わす同次座標変
換行列Ａi を、
【００２０】
【数２７】
【００２１】としたとき、前記同次座標変換行列Ａhand
および前記同次座標変換行列Ａi に基づいて求められる
式（１）の行列方程式
【００２２】
【数２８】
【００２３】但し、 Ｕは２０×１６の実数行列 Ｖは２０×１８の実数行列 ξ＝［ｘ3 ｓ4 ｓ5 ，ｘ3 ｓ4 ｃ5 ，ｘ3 ｃ4 ｓ5 ，ｘ3 ｃ4 ｃ5 ， ｘ3 ｓ4 ，ｘ3 ｃ4 ，ｘ3 ｓ5 ，ｘ3 ｃ5 ，ｓ4 ｓ5 ，ｓ4 ｃ5， ｃ4 ｓ5 ，ｃ4 ｃ5 ，ｓ4 ，ｃ4 ，ｓ5 ，ｃ5 ］T ρ＝［ｘ1 2ｘ2 2，ｘ1 2ｘ2 ，ｘ1 2，ｘ1 ｘ2 2，ｘ1 ｘ2 ，ｘ1 ，ｘ2 2， ｘ2 ，１］T ｘi ＝ｔａｎ（θi ／２） に前記パラメータｔi の各成分を数値代入することによ
り求められる、前記先端の位置と姿勢をあらわす同次変
換行列Ａhandの各成分を変数とする式（２）の行列方程
式
【００２４】
【数２９】
【００２５】但し、Ｕ（Ａhand），Ｖ（Ａhand）は、そ
れぞれ、前記実数行列Ｕ，Ｖに前記パラメータｔi の各
成分を数値代入することにより生成される、前記同次変
換行列Ａhandの各成分を変数とする行列であることを表
わすを記憶する記憶手段と、前記同次変換行列Ａhandの
各成分を数値入力する入力手段と、前記入力手段から入
力された前記同次変換行列Ａhandの各成分の数値を前記
式 （２）の行列方程式に代入することにより、式（３）の
数値行列方程式
【００２６】
【数３０】
【００２７】但し、Ｕnum ，Ｖnum は、それぞれ、前記
行列Ｕ（Ａhand），Ｖ（Ａhand）に前記同次変換行列Ａ
handの各成分を数値代入することにより生成される数値
行列であることを表わすを求める数値行列方程式導出手
段と前記式（３）の数値行列方程式に基づいて、前記関
節の回転角θi (k)（ｋはθi の複数解を相互に識別する
最大１６までの正の整数をあらわす）を求める関節回転
角算出手段とを備えたことを特徴とする。ここで、上記
本発明の第１のマニピュレータシミュレート装置ないし
第２のマニピュレータシミュレート装置において、前記
関節回転角算出手段が、前記式（３）の数値行列方程式
に基づいて、θ4 およびθ5 が消去された式を求める手
段と、該θ4 およびθ5 が消去された式に基づいて、固
有値方程式もしくは一般化固有値方程式を求める手段
と、該固有値方程式もしくは一般化固有値方程式に基づ
いて前記関節の回転角θi (k)を求める手段とを備えたも
のであることが好ましい。また、上記目的を達成する本
発明の第３のマニピュレータシミュレート装置は、順次
連結された６本以内のアームを有するマニピュレータの
先端の位置と姿勢を表わす情報に基づいて、該位置と姿
勢を実現するための、アームどうしを連結する関節の回
転角を求めるマニピュレータシミュレート装置におい
て、絶対座標系からみた前記先端の位置と姿勢をあらわ
す同次座標変換行列Ａhandを、
【００２８】
【数３１】
【００２９】但し、Ｃhandは姿勢、ｔhandは位置をあら
わすとし、前記マニピュレータの後端から先端に向かっ
て順次に付された座標系のうち、ｉ番目の座標系からみ
たｉ＋１番目の座標系の位置と姿勢を表わす同次座標変
換行列Ａi を、
【００３０】
【数３２】
【００３１】としたとき、前記同次座標変換行列Ａhand
および前記同次座標変換行列Ａi に基づいて求められる
式（４）の行列方程式
【００３２】
【数３３】
【００３３】但し、 Ｕは２０×１６の実数行列 Ｖは２０×１８の実数行列 ξ＝［ｘ3 ｓ1 ｓ2 ，ｘ3 ｓ1 ｃ2 ，ｘ3 ｃ1 ｓ2 ，ｘ3 ｃ1 ｃ2 ， ｘ3 ｓ1 ，ｘ3 ｃ1 ，ｘ3 ｓ2 ，ｘ3 ｃ2 ，ｓ1 ｓ2 ，ｓ1 ｃ2 ， ｃ1 ｓ2 ，ｃ1 ｃ2 ，ｓ1 ，ｃ1 ，ｓ2 ，ｃ2 ］T ρ＝［ｘ4 2ｘ5 2，ｘ4 2ｘ5 ，ｘ4 2，ｘ4 ｘ5 2，ｘ4 ｘ5 ，ｘ4 ，ｘ5 2， ｘ5 ，１］T ｘi ＝ｔａｎ（θi ／２） を記憶する記憶手段と、前記パラメータｔi の各成分を
数値入力する第１の入力手段と、前記第１の入力手段に
より入力された前記パラメータｔi の各成分の数値を前
記式（４）の行列方程式に代入することにより、前記先
端の位置と姿勢をあらわす同次変換行列Ａhandの各成分
を変数とする式（５）の行列方程式
【００３４】
【数３４】
【００３５】但し、Ｕ（Ａhand），Ｖ（Ａhand）は、そ
れぞれ、前記行列Ｕ，Ｖに前記パラメータｔi の各成分
を数値代入することにより生成される、前記同次変換行
列Ａhandの各成分を変数とする行列であることを表わす
を求めるパラメータ数値代入手段と、前記同次変換行列
Ａhandの各成分を数値入力する第２の入力手段と、前記
第２の入力手段から入力された前記同次変換行列Ａhand
の各成分の数値を前記式（５）の行列方程式に代入する
ことにより、式（６）の数値行列方程式
【００３６】
【数３５】
【００３７】但し、Ｕnum ，Ｖnum は、それぞれ、前記
行列Ｕ（Ａhand），Ｖ（Ａhand）に前記同次変換行列Ａ
handの各成分を数値代入することにより生成される数値
行列であることを表わすを求める数値行列導出手段と、
前記式（６）の数値行列方程式に基づいて、前記関節の
回転角θi (k)（ｋはθi の複数解を相互に識別する最大
１６までの正の整数をあらわす）を求める関節回転角算
出手段とを備えたことをを特徴とする。さらに、上記目
的を達成する本発明の第４のマニピュレータシミュレー
ト装置は、順次連結された６本以内のアームを有するマ
ニピュレータの先端の位置と姿勢を表わす情報に基づい
て、該位置と姿勢を実現するための、アームどうしを連
結する関節の回転角を求めるマニピュレータシミュレー
ト装置において、絶対座標系からみた前記先端の位置と
姿勢をあらわす同次座標変換行列Ａhandを、
【００３８】
【数３６】
【００３９】但し、Ｃhandは姿勢、ｔhandは位置をあら
わすとし、前記マニピュレータの後端から先端に向かっ
て順次に付された座標系のうち、ｉ番目の座標系からみ
たｉ＋１番目の座標系の位置と姿勢を表わす同次座標変
換行列Ａi を、
【００４０】
【数３７】
【００４１】としたとき、前記同次座標変換行列Ａhand
および前記同次座標変換行列Ａi に基づいて求められる
式（４）の行列方程式
【００４２】
【数３８】
【００４３】但し、 Ｕは２０×１６の実数行列 Ｖは２０×１８の実数行列 ξ＝［ｘ3 ｓ1 ｓ2 ，ｘ3 ｓ1 ｃ2 ，ｘ3 ｃ1 ｓ2 ，ｘ3 ｃ1 ｃ2 ， ｘ3 ｓ1 ，ｘ3 ｃ1 ，ｘ3 ｓ2 ，ｘ3 ｃ2 ，ｓ1 ｓ2 ，ｓ1 ｃ2 ， ｃ1 ｓ2 ，ｃ1 ｃ2 ，ｓ1 ，ｃ1 ，ｓ2 ，ｃ2 ］T ρ＝［ｘ4 2ｘ5 2，ｘ4 2ｘ5 ，ｘ4 2，ｘ4 ｘ5 2，ｘ4 ｘ5 ，ｘ4 ，ｘ5 2， ｘ5 ，１］T ｘi ＝ｔａｎ（θi ／２） に前記パラメータｔi の各成分を数値代入することによ
り求められる、前記先端の位置と姿勢をあらわす同次変
換行列Ａhandの各成分を変数とする式（５）の行列方程
式
【００４４】
【数３９】
【００４５】但し、Ｕ（Ａhand），Ｖ（Ａhand）は、そ
れぞれ、前記行列Ｕ，Ｖに前記パラメータｔi の各成分
を数値代入することにより生成される、前記同次変換行
列Ａhandの各成分を変数とする行列であることを表わす
を記憶する記憶手段と、前記同次変換行列Ａhandの各成
分を数値入力する入力手段と、前記入力手段から入力さ
れた前記同次変換行列Ａhandの各成分の数値を前記式
（５）の行列方程式に代入することにより、式（６）の
数値行列方程式
【００４６】
【数４０】
【００４７】但し、Ｕnum ，Ｖnum は、それぞれ、前記
行列Ｕ（Ａhand），Ｖ（Ａhand）に前記同次変換行列Ａ
handの各成分を数値代入することにより生成される数値
行列であることを表わすを求める数値行列導出手段と、
前記式（６）の数値行列方程式に基づいて、前記関節の
回転角θi (k)（ｋはθi の複数解を相互に識別する最大
１６までの正の整数をあらわす）を求める関節回転角算
出手段と備えたことを特徴とする。ここで、上記本発明
の第３のマニピュレータシミュレート装置ないし第４の
マニピュレータシミュレート装置において、前記関節回
転角算出手段が、前記式（６）の数値行列方程式に基づ
いて、θ1 およびθ2 が消去された式を求める手段と、
該θ1 およびθ2 が消去された式に基づいて、固有値方
程式もしくは一般化固有値方程式を求める手段と、該固
有値方程式もしくは一般化固有値方程式に基づいて前記
関節の回転角θi (k)を求める手段とを備えたものである
ことが好ましい。
【００４８】
【実施の形態】以下、本発明の実施の形態について説明
する。以下では、全て、回転関節の組合わせによる６自
由度マニピュレータの逆キネマテクスを取り上げる。図
１は、本発明の第１のマニピュレータシミュレート方法
の一実施形態を示したフローチャートである。以下、こ
の実施形態を「実施形態１」と称する。ここでは、先
ず、ステップ（ａ）において、数式による、前述した式
（１）の行列方程式を求める。絶対座標系、例えば図９
に示す基体１に付された座標系からみたマニピュレータ
先端の位置と姿勢をあらわす同次座標変換行列Ａ
handを、
【００４９】
【数４１】
【００５０】但し、Ｃhandは姿勢、ｔhandは位置をあら
わすとし、前記マニピュレータの後端から先端に向かっ
て順次に付された座標系のうち、ｉ番目の座標系からみ
たｉ＋１番目の座標系の位置と姿勢を表わす同次座標変
換行列Ａi 、およびその逆行列Ａi -1を、
【００５１】
【数４２】
【００５２】とする。ただし、∈Ｒ（４×４）は、この
記号の左側に記載した行列が４列（４×４）の実数行列
（Ｒ）であることを表わしている。ここで、回転部分Ｃ
i ，Ｃz,i ，Ｃx,i ∈Ｒ（３×３）は以下で定義され
る。
【００５３】
【数４３】
【００５４】尚、ここでは自由度６のマニピュレータを
取り扱っているが、マニピュレータの自由度の数が６よ
りも少ない場合は、その少ない分だけ、上記Ａi （ｉ＝
１，２，…，６）のいずれか１つもしくは複数が単位行
列であると見なすことにより、自由度の少ないマニピュ
レータについても、ここでの議論をそのまま適用するこ
とができる。ここで、Ｄ−Ｈパラメータについて、「機
械系のためのロボティクス」遠山茂樹著 総合電子出版
社 に記載された定義に沿って説明する。このＤ−Ｈパ
ラメータは既に広く知られたパラメータであるため、こ
こでは、その定義を説明するにとどめる。
【００５５】図２は、Ｄ−Ｈパラメータを説明するため
の、リンクおよび関節の一部分を示した図である。先
ず、ベース座標系（右手系）を設定する。ｚ0 軸は関節
１の回転方向、アーム肩部分方向を指すようにとる。ｘ
0 軸，ｙ0 軸は適当にとる。次に、各リンクｉ（ｉ＝
１，２，…，６）について、次の（１）から（４）に従
って座標系を設定する。 （１）関節ｉ＋１の運動方向にｚi 軸を設定する。 （２）ｚi 軸とｚi-1 軸の交点、またはｚi 軸とｚi-1
軸の共通法線とｚi 軸との交点を第ｉ座標系の原点とす
る。ｚi 軸とｚi-1 軸が平行のときは、後述するリンク
パラメータｄi が０となるように第ｉ座標系の原点を定
める。 （３）ｚi-1 ，ｚi をそれぞれｚi-1 軸、ｚi 軸の方向
を向いたベクトルとしたとき、 ｘi ＝±（ｚi-1 ×ｚi ）／‖ｚi-1 ×ｚi ‖または、
ｚi 軸とｚi-1 軸の共通法線方向にｘi 軸を設定する。 （４）右手系をなすようにｙi 軸を設定する。上記のよ
うに座標系を設定したとき、第ｉ−１座標系原点と、ｚ
i-1 軸とｘi軸との交点の距離であって、ｚi-1 方向に
測った距離をリンクパラメータｄi とする。またｚi-1
軸とｘi 軸との交点と第ｉ座標系原点との距離であっ
て、ｘi軸負の方向を正とした距離をリンクパラメータ
ａi とする。ここではパラメータｔi として、上述した
式（１０）、すなわち、 ｔi ＝［ａi ０ ｄi ］ ……（１０） ｉ＝１，２，……，６ が採用される。ちなみに、上述したθi ，αi もＤ−Ｈ
パラメータを構成するリンクパラメータであって、リン
クパラメータθi はｘi-1 軸からｘi 軸までのｚi-1 軸
まわりの回転角、リンクパラメータαi はｚi-1 軸から
ｚi 軸までのｘi 軸まわりの回転角である。
【００５６】図１に戻り、ステップ（ａ）における、前
述の式（１）の行列方程式の導出方法についての説明を
続行する。マニピュレータの順キネマテクス方程式は式
（１１）で与えられる。 Ａhand ＝ Ａ1 ・Ａ2 ・Ａ3 ・Ａ4 ・Ａ5 ・Ａ6 ……（１１） ここで、順キネマテクス（順運動学）とは、マニピュレ
ータを構成する各リンクどうしを連結する各関節の回転
角情報を与えたときに、そのマニピュレータの先端の位
置と姿勢がどうなるかを求めることをいう。ここでは、
上記式（１１）に基づいて、１４本の基本方程式（２本
のスカラ方程式、４本のベクトル方程式）を導出し、次
いでそれら１４本の基本方程式を使ってさらに６本の基
本方程式を導出する。それら、合計２０本の基本方程式
をまとめると、前述の式（１）の行列方程式が生成され
る。（１４本の基本方程式の導出）
【００５７】上記式（１１）を以下のように書き換え
る。 Ａ3 ・Ａ4 ・Ａ5 ＝ Ａ2 -1 ・Ａ1 -1 ・Ａhand・Ａ6 -1 ……（１２） 式（１２）を基に、先ず、以下の基本ベクトル方程式が
導かれる。尚、式（１２）の左辺から導出される量を添
字Ａで表わし、式（１２）の右辺から導出される量を添
字Ｂで表わす。 ｚA ≡Ａ3 Ａ4 Ａ5 ｅ3 ＝Ａ2 -1 Ａ1 -1 ＡhandＡ6 -1 ｅ3 ≡ｚB ……（１３） ｐA ≡Ａ3 Ａ4 Ａ5 ｅ4 ＝Ａ2 -1 Ａ1 -1 ＡhandＡ6 -1 ｅ4 ≡ｐB ……（１４） ここで、ｅi （ｉ＝１……４）は単位ベクトルであり、
それぞれ、 ｅ1 ＝［１ ０ ０ ０］T ｅ2 ＝［０ １ ０ ０］T ｅ3 ＝［０ ０ １ ０］T ｅ4 ＝［０ ０ ０ １］T をあらわす。
【００５８】式（１３），式（１４）において第４成分
は、１＝１，０＝０といった自明の方程式となるため、
以後第４成分は無視し、式（１３），式（１４）は第
１、第２、第３成分からなる３次元ベクトルを表すもの
とする。式（１３），式（１４）のｐ，ｚを基に、内
積、外積、ベクトル和をとることにより、以下の方程式
が得られる。 （ｐT ・ｐ）A ＝（ｐT ・ｐ）B ……（１５） （ｐT ・ｚ）A ＝（ｐT ・ｚ）B ……（１６） （ｐT ×ｚ）A ＝（ｐT ×ｚ）B ……（１７） ［ｐ×（ｐ×ｚ）＋（ｐT ・ｚ）ｐ］A ＝［ｐ×（ｐ×ｚ）＋（ｐT ・ｚ）ｐ］B ……（１８） 式（１８）は、以下のように書き換えることもできる。 ［２（ｐT ・ｚ）ｐ−（ｐT ・ｐ）ｚ］A ＝［２（ｐT ・ｚ）ｐ−（ｐT ・ｐ）ｚ］B ……（１９） 数式処理を使った精密な解析の結果、式（１３）から式
（１８）ないし式（１９）の方程式は、ｃｏｓθi ，ｓ
ｉｎθi に対して線形であることがわかる。ｐ，ｚから
なる他のコンビネーションは、線形性を欠く。
【００５９】数式処理を適用するに当たり、式（１３）
から式（１８）ないし式（１９）の方程式は式（８），
式（９）を使って以下のように分解しておく。 （１）ｚA ＝ｚB ｚA ＝Ｃz,3 Ｃx,3 Ｃ4 Ｃ5 ｅ3 ＝Ｃ2 T Ｃ1 T ＣhandＣ6 T ｅ3 ＝ｚB ……（２０） （２）ｐA ＝ｐB ｐA ＝Ｃz,3(Ｃx,3 Ｃ4 Ｃz,5 ・ｔ5 ＋Ｃx,3 Ｃz,4 ・ｔ4 ＋ｔ3) ＝−Ｃ2 T （Ｃ1 ＣhandＣ6 T ・ｔ6 −Ｃ1 T ・ｔhand ＋Ｃx,1 T ・ｔ1 ）−Ｃx,2 T ・ｔ2 ＝ｐB ……（２１） （３）（ｐT ・ｐ）A ＝（ｐT ・ｐ）B （ｐT ・ｐ）A ＝ｔ3 T ・ｔ3 ＋２ｔ3 T Ｃx,3 Ｃz,4 ・ｔ4 ＋２ｔ3 T Ｃx,3 Ｃ4 Ｃz,5 ・ｔ5 ＋ｔ4 T ・ｔ4 ＋２ｔ4 T Ｃx,4 Ｃz,5 ・ｔ5 ＋ｔ5 T ・ｔ5 ＝ｔ2 T ・ｔ2 ＋２ｔ2 T Ｃz,2 T Ｃx,1 T ・ｔ1 −２ｔ2 T Ｃz,2 T Ｃ1 T ・ｔhand ＋２ｔ2 T Ｃz,2 T Ｃ1 T ・ＣhandＣx,6 T ・ｔ6 ＋ｔ1 T ・ｔ1 −２ｔ1 T Ｃz,1 T ・ｔhand ＋２ｔ1 T Ｃz,1 T ＣhandＣx,6 T ・ｔ6 ＋ｔhand T ・ｔhand −２ｔhand T ＣhandＣx,6 T ・ｔ6 ＋ｔ6 T ・ｔ6 ＝（ｐT ・ｐ）B ……（２２） （４）（ｐT ・ｚ）A ＝（ｐT ・ｚ）B （ｐT ・ｚ）A ＝（ｚT ・ｐ）A ＝ｅ3 T ・Ｃx,5 T・ｔ5 ＋ｅ3 T Ｃ5 T Ｃx,4 T・ｔ4 ＋ｅ3 T ・Ｃ5 T Ｃ4 T Ｃx,3 T・ｔ3 ＝−ｅ3 T ｔ6 ＋ｅ3 T Ｃ6 Ｃhand T ・ｔhand −ｅ3 T Ｃ6 Ｃhand T Ｃz,1 ・ｔ1 −ｅ3 T Ｃ6 Ｃhand T Ｃ1 Ｃz,2 ・ｔ2 ＝（ｚT ・ｐ）B ＝（ｐT ・ｚ）B ……（２３） （５）（ｐT ×ｚ）A ＝（ｐT ×ｚ）B （ｐT ×ｚ）A ＝Ｃz,3 Ｃx,3 Ｃ4 Ｃz,5 （ｔ5 ×Ｃx,5 ｅ3 ） ＋Ｃz,3 Ｃx,3 Ｃz,4 （ｔ4 ×Ｃx,4 Ｃ5 ｅ3 ） ＋Ｃz,3 （ｔ3 ×Ｃx,3 Ｃ4 Ｃ5 ｅ3 ） ＝−Ｃ2 T Ｃ1 T Ｃhand T Ｃx,6 T(ｔ6 ×Ｃz,6 Tｅ3) ＋Ｃ2 T Ｃ1 T （ｔhand×ＣhandＣ6 T ｅ3 ） −Ｃ2 T Ｃx,1 T（ｔ1 ×Ｃz,1 TＣhandＣ6 T ｅ3 ） −Ｃx,2 T（ｔ2 ×Ｃz,2 TＣ1 T ＣhandＣ6 T ｅ3 ） ＝（ｐT ×ｚ）B ……（２４） （６）［２（ｐT ・ｚ）ｐ−（ｐT ・ｐ）ｚ］A ＝［２（ｐT ・ｚ）ｐ−（ｐT ・ｐ）ｚ］B ［２（ｐT ・ｚ）ｐ−（ｐT ・ｐ）ｚ］A ＝２（ｐT ・ｚ）A Ｃz,3 （Ｃx,3 Ｃ4 Ｃz,5 ・ｔ5 ＋Ｃx,3 Ｃz,4 ・ｔ4 ＋ｔ3 ）−（ｐT ・ｐ）A Ｃz,3 Ｃx,3 Ｃ4 Ｃ5 ｅ3 ＝２（ｐT ・ｚ）B Ｃx,2 T （Ｃz,2 T Ｃ1 T ＣhandＣ6 T ・ｔ6 −Ｃz,2 TＣ1 T ・ｔhand＋Ｃz,2 T Ｃx,1 T ・ｔ1 ＋ｔ2 ） −（ｐT ・ｐ）B Ｃ2 T Ｃ1 T ＣhandＣ6 T ｅ3 ＝［２（ｐT ・ｚ）ｐ−（ｐT ・ｐ）ｚ］B ……（２５）
【００６０】（基本方程式の性質） 上記の式（２０）から式（２５）の基本方程式は以下の
性質を有する。 （ａ）マニピュレータ先端の位置・姿勢を表すマトリク
スＡhandの各要素は、右辺のみに表れる。 （ｂ）全ての方程式はθ6 に依存しない。 なぜならば、式（２０）から式（２５）の方程式におい
て、θ6 は、Ｃ6 T ｅ3 ＝Ｃx,6 T Ｃz,6 T ｅ3 の形で
表れるが、Ｃz,6 T ｅ3 ＝［０ ０ １］T である。 （ｃ）ｚ3 ，ｐ3 ，ｐT ・ｚ，ｐT ・ｐ，（ｐT ×ｚ）
3 ，［２（ｐT ・ｚ）ｐ−（ｐT ・ｐ）ｚ］3 は、θ3
に依存しない。ここで、例えばｚ3 ，ｐ3 はそれぞれ
ｚ，ｐの３番目の要素をあらわす。他も同様である。な
ぜならば、ｐT ・ｚ，ｐT ・ｐはＣz,3 依存性をもたな
い。ｚ3 ，ｐ3 ，（ｐT ×ｚ）3 ，［２（ｐT ・ｚ）ｐ
−（ｐT ・ｐ）ｚ］3 に関しては、θ3 は、常にｅ3 T
Ｃz,3 の形で表れるが、ｅ3 T Ｃz,3 ＝ｅ3 T である。 （ｄ）ｚ1 ，ｚ2 ，ｐ1 ，ｐ2 ，（ｐT ×ｚ）1 ，（ｐ
T ×ｚ）2 ，［２（ｐT ・ｚ）ｐ−（ｐT ・ｐ）ｚ］
1 ，［２（ｐT ・ｚ）ｐ−（ｐT ・ｐ）ｚ］2 は、ｔａ
ｎ（θ3 ／２）に対して線形である。なぜならば、ｘi
＝ｔａｎ（θi ／２）とする時、 ｃｏｓθi ＝（１−ｘi 2 ）／（１＋ｘi 2 ） ｓｉｎθi ＝２ｘi ／（１＋ｘi 2 ） Ｘ3 - ・Ｃz,3 ＝Ｘ3 +
【００６１】
【数４４】
【００６２】が成立する。ｚ，ｐ，ｐT ×ｚ，２（ｐT
・ｚ）ｐ−（ｐT・ｐ）ｚにおいて、左からＸ3 -を掛け
ることにより、上記の結果を得る。 （ｅ）θ1 ，θ2 ，θ4 ，θ5 は、ｃｏｓθi ，ｓｉｎ
θi （ｉ＝１，２，４，５）に対して常に線形で表れ、
（ｃｏｓθi ）2 ，（ｓｉｎθi ）2 ，ｃｏｓθi ・ｓ
ｉｎθi の項は表れない。なぜならば、ｚ，ｐ，ｐT ・
ｐ，ｐT ・ｚ，ｐT ×ｚにおいて、Ｃz,i は各々一回ず
つしか表れない。また、２（ｐT ・ｚ）ｐ−（ｐT ・
ｐ）ｚに関しては、ｐ，ｚ，ｐT ・ｚ，ｐT ・ｐの値を
代入し、数式処理を使い、（ｃｏｓθi ）2＋（ｓｉｎ
θi ）2 ＝１，（ｃｏｓαi ）2 ＋（ｓｉｎαi ）2 ＝
１に注意し整理すると、線形であることが判明する。 （基本方程式の行列形式） 式（２０）から式（２５）の基本方程式は、以下のよう
な行列形式にまとめることができる。
【００６３】
【数４５】
【００６４】 Ｕ’，Ｖ’∈Ｒ（１４×１８） ξ’，ρ∈Ｃ9 （∈Ｃ9 は、その左の記号が９次元の複素ベクトルであ
ることを表わしている） ξ’＝［ｓ4 ・ｓ5 ，ｓ4 ・ｃ5 ，ｃ4 ・ｓ5 ，ｃ4 ・ｃ5 ，ｓ4 ， ｃ4 ，ｓ5 ，ｃ5 ，１］T ρ＝［ｘ1 2ｘ2 2，ｘ1 2ｘ2 ，ｘ1 2，ｘ1 ｘ2 2，ｘ1 ｘ2 ，ｘ1 ，ｘ2 2， ｘ2 ，１］T Ｕ’，Ｖ’の成分は、式（２０）から式（２５）を丹念
に数式処理し、（ｃｏｓθi ）2 ＋（ｓｉｎθi ）2 ＝
１（ｉ＝４，５），（ｃｏｓαi ）2 ＋（ｓｉｎαi ）
2 ＝１（ｉ＝１，……，６），ｃｏｓθi ＝（１−ｘi
2 ）／（１＋ｘi 2 ），ｓｉｎθi ＝２ｘi ／（１＋ｘ
i 2 ）（ｉ＝１，２，３）を代入し、得られた代数方程
式においてξ’，ρの各項に対する係数を求めることに
より得られる。Ｕ’，Ｖ’の各成分は、パラメータｔ
i 、Ａhandの成分を変数とする多変数多項式である。
【００６５】（基本方程式の追加） ｚ3 ，ｐ3 ，ｐT ・ｚ，ｐT ・ｐ，（ｐT ×ｚ）3 ，
［２（ｐT ・ｚ）ｐ−（ｐT ・ｐ）ｚ］3 はθ3 に依存
しないので、ｘ3 ＝ｔａｎ（θ3 ／２）を掛け、線形和
を取ることにより以下の６本の方程式を得ることができ
る。 ［ｘ3 ｚ3 ＋ｐ3 ］A ＝［ｘ3 ｚ3 ＋ｐ3 ］B ……（２８） ［ｘ3 ｐ3 ＋ｚ3 ］A ＝［ｘ3 ｐ3 ＋ｚ3 ］B ……（２９） ［ｘ3 ｐT ・ｐ＋ｐT ・ｚ］A ＝［ｘ3 ｐT ・ｐ＋ｐT ・ｚ］B ……（３０） ［ｘ3 ｐT ・ｚ＋ｐT ・ｐ］A ＝［ｘ3 ｐT ・ｚ＋ｐT ・ｐ］B ……（３１） ［ｘ3 （ｐT ×ｚ）3 ＋ｚ3 ］A ＝［ｘ3 （ｐT ×ｚ）3 ＋ｚ3 ］B ……（３２） ［ｘ3 ｛２（ｐT ・ｚ）ｐ−（ｐT ・ｐ）ｚ］3 ｝＋ｐ3 ］A ＝［ｘ3 ｛２（ｐT ・ｚ）ｐ−（ｐT ・ｐ）ｚ］3 ｝＋ｐ3 ］B ……（３３） 式（２８）から式（３３）を式（２７）と同様にまと
め、左辺の一部を右辺に移項し、式（２７）と一緒に整
理すると、前述した式（１）の行列方程式が得られる。
【００６６】
【数４６】
【００６７】 Ｕ∈Ｒ（２０×１６），Ｖ∈Ｒ（２０×１８），ξ∈Ｃ16，ρ∈Ｃ9 ξ＝［ｘ3 ｓ4 ｓ5 ，ｘ3 ｓ4 ｃ5 ，ｘ3 ｃ4 ｓ5 ，ｘ3 ｃ4 ｃ5 ， ｘ3 ｓ4 ，ｘ3 ｃ4 ，ｘ3 ｓ5 ，ｘ3 ｃ5 ，ｓ4 ｓ5 ，ｓ4 ｃ5 ， ｃ4 ｓ5 ，ｃ4 ｃ5 ，ｓ4 ，ｃ4 ，ｓ5 ，ｃ5 ］T ρ＝［ｘ1 2ｘ2 2，ｘ1 2ｘ2 ，ｘ1 2，ｘ1 ｘ2 2，ｘ1 ｘ2 ，ｘ1 ，ｘ2 2， ｘ2 ，１］T 尚、上記では、パラメータｔi として前述の式（１
０）、すなわち、ＤＨ−パラメータ ｔi ＝［ａi ０ ｄi ］T ……（１０） を採用したが、この式（１０）に代わり、パラメータｔ
i としていわゆる修正Ｄ−Ｈパラメータ、すなわち、 ｔi ＝［ξi ηi ζi ＋γi ］T ……（２８） を採用し、通常のＤ−Ｈパラメータの制限を緩めた方式
を採ることもできる。
【００６８】Ｄ−Ｈパラメータでは、前述したように、
隣り合った２関節軸の共通法線の長さをリンクパラメー
タａi としたが、このリンクパラメータａi の代わり
に、一方の軸のみに垂直なベクトルＤi ＝（ξi ，η
i ，ζi ）と、リンク間距離を表わすリンクパラメータ
ｄi の代わりに回転軸ｚi-1 沿いの任意の長さのパラメ
ータγi を導入する。このようにして導入されたパラメ
ータが修正Ｄ−Ｈパラメータと称されるパラメータであ
る（前掲の、「機械系のためのロボティクス」遠山茂樹
著 総合電子出版社 参照）。Ｄ−Ｈパラメータでは２
関節軸の共通法線を使っていたが、今度はγi が任意に
設定できるため、座標系を希望の場所に設定できること
になる。
【００６９】式（２８）のパラメータを採用すると、機
構系を表現する座標系のパラメータが増える分、パラメ
ータ変動に対する感度がスムーズになり、機構パラメー
タのキャリブレーションが行いやすいくなる。以上で、
本実施形態１における、図１のステップ（ａ）の説明を
終え、次いで、図１のステップ（ｂ）以下の各ステップ
の説明に移る。ステップ（ａ）は前もって一度だけ行な
って式（１）を求めておけばよく、以下に説明するステ
ップ（ｂ）により、個々のマニピュレータに適合した行
列方程式が生成される。
【００７０】前述のパラメータｔi として、例えばコン
ピュータグラフィックスのような、実際のハードウェア
としてのマニピュレータの存在しないシステムの場合
は、Ｄ−Ｈパラメータ（式（１０））を採用してもよ
く、修正Ｄ−Ｈパラメータ（式（２８））を採用しても
よいが、実際のハードウェアとしてのマニピュレータが
存在する場合、あるいは、未だ存在していなくても、実
際のハードウェアとしてのマニピュレータを構成するこ
とが想定されている場合は、パラメータｔi として修正
Ｄ−Ｈパラメータ（式（２８））を採用することが好ま
しい。パラメータｔi としてＤ−Ｈパラメータ（式（１
０））を採用する場合、そのパラメータｔi の各成分ａ
i ，ｄi （ｉ＝１，２，…，６）の具体的な数値を前述
の式（１）に代入する。
【００７１】パラメータｔi として修正Ｄ−Ｈパラメー
タ（式（２８））を採用する場合も同様ではあるが、こ
の場合、先ず、修正Ｄ−Ｈパラメータのキャリブレーシ
ョンを行ない、そのキャリブレーションの結果得られ
る、パラメータｔi の各成分ξi ，ηi ，ζi ＋γi の
数値を式（１）に代入する。ハードウェアとしてのマニ
ピュレータを構成すると、ガタや寸法誤差等により、モ
デル的なマニピュレータから外れるため、実際に構成さ
れたマニピュレータに適合した数値を求めて代入するも
のである。キャリブレーション自体についてはここでの
主題ではないため、説明は省略する。詳細は、前掲の
「機械系のためのロボティクス」を参照されたい。この
ように、パラメータｔi の各成分を式（１）の行列方程
式に数値代入することにより、Ａhandの各成分を変数と
する、前述の式（２）の行列方程式、すなわち、
【００７２】
【数４７】
【００７３】 Ｕ∈Ｒ（２０×１６），Ｖ∈Ｒ（２０×１８），ξ∈Ｃ16，ρ∈Ｃ9 ξ＝［ｘ3 ｓ4 ｓ5 ，ｘ3 ｓ4 ｃ5 ，ｘ3 ｃ4 ｓ5 ，ｘ3 ｃ4 ｃ5 ， ｘ3 ｓ4 ，ｘ3 ｃ4 ，ｘ3 ｓ5 ，ｘ3 ｃ5 ，ｓ4 ｓ5 ，ｓ4 ｃ5 ， ｃ4 ｓ5 ，ｃ4 ｃ5 ，ｓ4 ，ｃ4 ，ｓ5 ，ｃ5 ］T ρ＝［ｘ1 2ｘ2 2，ｘ1 2ｘ2 ，ｘ1 2，ｘ1 ｘ2 2，ｘ1 ｘ2 ，ｘ1 ，ｘ2 2， ｘ2 ，１］T が導出される（図１ステップ（ｃ））。 数式処理を用いた行列方程式は、上記式（２）まで、す
なわち、図１のステップ（ｃ）までである。以後の過程
は、全て数値行列を取り扱う。すなわち、マニピュレー
タ先端の位置と姿勢を表わすＡhandの各成分を式（２）
に数値代入することにより（図１ステップ（ｄ））、数
値行列Ｕnum ，Ｖnumによる数値行列方程式を求め（図
１ステップ（ｅ））、その数値行列方程式に基づいて各
関節ｉの回転角θi (k)（ｋは、θi の複数角を相互に識
別する最大１６までの正の整数をあらわす）を求める
（図１ステップ（ｆ））。この数値行列方程式は前述の
式（３）、すなわち、
【００７４】
【数４８】
【００７５】 Ｕ∈Ｒ（２０×１６），Ｖ∈Ｒ（２０×１８），ξ∈Ｃ16，ρ∈Ｃ9 ξ＝［ｘ3 ｓ4 ｓ5 ，ｘ3 ｓ4 ｃ5 ，ｘ3 ｃ4 ｓ5 ，ｘ3 ｃ4 ｃ5 ， ｘ3 ｓ4 ，ｘ3 ｃ4 ，ｘ3 ｓ5 ，ｘ3 ｃ5 ，ｓ4 ｓ5 ，ｓ4 ｃ5 ， ｃ4 ｓ5 ，ｃ4 ｃ5 ，ｓ4 ，ｃ4 ，ｓ5 ，ｃ5 ］T ρ＝［ｘ1 2ｘ2 2，ｘ1 2ｘ2 ，ｘ1 2，ｘ1 ｘ2 2，ｘ1 ｘ2 ，ｘ1 ，ｘ2 2， ｘ2 ，１］T であらわされる。
【００７６】図１のステップ（ｄ）〜ステップ（ｆ）を
繰り返すことによりそのマニピュレータモデルについて
種々のシミュレーションを行なうことができる。次に、
式（３）の数値行列方程式に基づく、θi (k)の数値解
法、すなわち図１のステップ（ｆ）の詳細について説明
する。図３は、図１のステップ（ｆ）の詳細フローを示
した図である。以下では、いくつかの数値解法について
説明するが、いずれも、図３のステップ（ｇ）〜ステッ
プ（ｊ）に従う。すなわち、以下に説明するいずれの解
法においても、θi (k)を求めるにあたり、先ず、式
（３）の数値行列方程式に基づいて、θ4 ，θ5 が消去
された式を求める（ステップ（８））。次いで、そのθ
4 ，θ5 が消去された式に基づいて、固有値方程式もし
くは一般化固有値方程式を求める。（ステップ
（ｈ））。その後、その固有値方程式もしくは一般化固
有値方程式に基づいてθ3 (k)，θ2 (k)，θ1 (k)を求め
（ステップ（ｉ））、さらにその後θ4 (k)，θ5 (k)，θ
6 (k)を求める（ステップ（ｊ））。
【００７７】以下、この図３のフローに従ったいくつか
の数値解法について説明する。 （数値解法１） 式（３）の行列方程式からθ4 ，θ5 を消去するに当た
り、式（３）の左側から２０×２０の実行列Ｗを掛け
る。Ｗの各成分は、ランダムに生成する。ＷＵnum ≡Ｕ
ran ＝［Ｕ16ran ，Ｕ4ran］T ，Ｕ16ran ∈Ｒ（１６×
１６），Ｕ4ran∈Ｒ（４×１６），ＷＶnum ≡Ｖran ＝
［Ｖ16ran ，Ｖ4ran］T ，Ｖ16ran ∈Ｒ（１６×１
８），Ｖ4ran∈Ｒ（４×１８）とする時、｜ｄｅｔ（Ｕ
16ran ）｜＞ε（ε：微小定数）となるようにする。ｄ
ｅｔ（Ｕ16ran ）≒０の時は、Ｗの成分をとり直す。こ
の時、式（３）は以下のようになる。
【００７８】
【数４９】
【００７９】したがって
【数５０】
【００８０】これを、式（２９）の残り４本の方程式に
代入することにより、θ4 ，θ5 が消去され、
【００８１】
【数５１】
【００８２】が得られる。 （固有値方程式（一般化固有値方程式）の導出） Ｖ16ran＝［xＶ16ran，1Ｖ16ran ］，Ｖ4ran＝［xＶ4ran，1Ｖ4ran］， x Ｖ16ran ，1 Ｖ16ran ∈Ｒ（１６×９）， x Ｖ4ran，1 Ｖ4ran∈Ｒ（４×９） とする時、上記式（３１）は、 ［Ｕ4ranＵ16ran -1 1 Ｖ16ran −1 Ｖ4ran］ρ ＋ｘ3 ［Ｕ4ranＵ16ran -1 x Ｖ16ran −x Ｖ4ran］ρ＝０ ……（３２） と表される。この式（３２）の両辺にそれぞれｘ1 ｘ
2 ，ｘ1 ，ｘ2 を掛けることにより、さらに１２本の方
程式が得られ、合計１６本の方程式が揃い、以下のよう
な一般固有値方程式に纏められる。 Ｇ・ρ＋ｘ3 Ｈ・ρ＝０ ……（３３） Ｇ，Ｈ∈Ｒ（１６×１６） ρ∈Ｃ16 ｘ3 ∈Ｃ
【００８３】
【数５２】
【００８４】 ρ＝［ｘ1 3ｘ2 3，ｘ1 3ｘ2 2，ｘ1 3ｘ2 ，ｘ1 3，ｘ1 2ｘ2 3，ｘ1 2ｘ2 2， ｘ1 2ｘ2 ，ｘ1 2，ｘ1 ｘ2 3，ｘ1 ｘ2 2，ｘ1 ｘ2 ，ｘ1 ，ｘ2 3，ｘ2 2， ｘ2 ，１］T ここで、ａ，……，ｔは、 Ｕ4ranＵ16ran -1 1 Ｖ16ran − 1Ｖ4ran＝［ａｂｃｄｅｆｇｈｊ］ Ｕ4ranＵ16ran -1 x Ｖ16ran − xＶ4ran＝［ｋｌｍｎｐｑｒｓｔ］ ａ，ｂ，ｃ，ｄ，ｅ，ｆ，ｇ，ｈ，ｊ，ｋ，ｌ，ｍ，
ｎ，ｐ，ｑ，ｒ，ｓ，ｔ∈Ｒ4 （∈Ｒ4 は、その左側の
記号が４次元の実数ベクトルであることをあらわす） で与えられる。｜ｄｅｔ（Ｈ）｜＞ε（ε：微小定数）
の場合には、以下のような固有値方程式となる。 Ｈ-1Ｇρ＋ｘ3 ρ＝０ ……（３６） （固有値方程式（一般化固有値方程式）の数値解法） ｜ｄｅｔ（Ｈ）｜＞εの場合には、式（３６）の固有値
方程式をＱＲ法で解き、ｄｅｔ（Ｈ）≒０の場合には、
式（３３）の一般化固有値方程式をＱＺ法（Ｑで解き、
固有値と固有ベクトルを求める。ＱＲ法、ＱＺ法に関し
ては、「Ｇｅｎｅ Ｈ． Ｇｏｌｕｂ， Ｃｈａｒｌｅ
ｓ Ｆ． ｖａｎ Ｌｏａｎ， “Ｍａｔｒｉｘ Ｃｏ
ｍｐｕｔａｔｉｏｎｓ”，Ｔｈｅ Ｊｏｈｎ Ｈｏｐｋ
ｉｎｓＵｎｉｖｅｒｓｉｔｙ Ｐｒｅｓｓ」を参照され
たい。固有値、固有ベクトルの組は、複素解も含め、一
般に１６通り存在する。固有ベクトルに関しては、１６
番目の成分が１になるように規格化する。
【００８５】この時、固有値の値からｘ3 、固有ベクト
ルの第１２成分、第１５成分の値からｘ1 ，ｘ2 の値が
求まる。従って、 θi (k) ＝ ２ａｒｃｔａｎ（ｘi (k) ） ……（３７） ｉ＝１，２，３；ｋ＝１，……，１６ により、第１、第２、第３関節角の値が求まる。 （θ4 (k) ，θ5 (k) ，θ6 (k) の算出） θ4 (k) ，θ5 (k) に関しては、式（３０）からξを求
め、ベクトルξの第１３〜第１６成分の値を使ってｃi
(k) ，ｓi (k) を求め、これらｃi (k) ，ｓi (k) に合
致する角度θi (k) （ｉ＝４，５；ｋ＝１，……，１
６）を求める。θ6 (k) に関しては、 Ｃz,6 ＝Ａ5 -1Ａ4 -1Ａ3 -1Ａ2 -1Ａ1 -1ＡhandＣx,6 T ……（３８） より、ｃ6 (k) ，ｓ6 (k) の値を求め、これらｃ6
(k) ，ｓ6 (k) に合致する角度θ6 (k) を求める。
【００８６】（数値解法２） 上述の数値解法１では、Ｕ16ran を正則化させるために
正方行列Ｗをランダムに生成させたが、ここでは、Ｕ
num を特異値分解し、以下のようにＷを決定する。 ＱT Ｕnum Ｐ ＝ ｄｉａｇ（σ1 …，σp ）∈Ｒ（２０×１６） ……（３９） Ｑ∈Ｒ（２０×２０）：直交行列 Ｐ∈Ｒ（２０×１６）：直交行列 Ｗ＝ＱT この時、Ｕ16ran は正則化する。以後の処理は、数値解
法１と同様である。
【００８７】（数値解法３） 一般化固有値方程式の解法は、通常の固有値方程式の解
法に比べ、２．５倍から３倍の計算時間を要する。従っ
て、ここでは、一般化固有値方程式の直接的な解法をで
きるだけ避ける方法を採る。正方行列Ｗとして、数値解
法１と同様に各成分をランダムに生成させる。式（３
３）において、ｄｅｔ（Ｈ）≒０の場合に、一般化固有
値方程式をＱＺ法で解く代わりに、次のような有理変換
を行なう。 ｘ3 ＝（ａｘ3 ’＋ｂ）／（ｃｘ3 ’＋ｄ） ……（４０） この時、式（３３）の一般化固有値方程式は以下のよう
になる。 Ｇ’・ρ＋ｘ3 ’Ｈ’・ρ＝０ ……（４１） Ｇ’＝ｄＧ＋ｂＨ Ｈ’＝ｃＧ＋ａＨ Ｇ’，Ｈ’∈Ｒ（１６×１６） ρ∈Ｃ16 ｘ3 ’∈Ｃ 多くの場合、｜ｄｅｔ（Ｈ’）｜＞εとなるため、次の
固有値方程式を解く。 Ｈ’-1Ｇ’・ρ＋ｘ3 ’ρ＝０ ……（４２） ｄｅｔ（Ｈ’）≒０の場合には、（４１）の一般化固有
値方程式を直接解く。以後の処理は、数値解法１と同様
である。
【００８８】（数値解法４） 正方行列Ｗとして、数値解法２においてＵnum を特異値
分解した時のＱT を使う。以後の処理は、数値解法３と
同様である。以上の方法によりマニピュレータのシミュ
レートを高速に行なうことができる。このシミュレート
結果を例えば表示画面上にリアルタイムに表示してコン
ピュータグラフィックスの用に供したり、あるいは、こ
のシミュレート結果をハードウェアとしてのマニピュレ
ータに送り、そのマニピュレータを、リアルタイムに行
なったシミュレートどおりに動作させることができる。
次に、本発明の第２のマニピュレータシミュレート方法
の一実施形態について説明する。以下、この実施形態を
「実施形態２」と称する。実施形態２においても、図１
のフローがそのまま適合するため、ここでは再度、図１
のフローを参照して実施形態２について説明する。
【００８９】ここでは、先ず、ステップ（ａ）におい
て、前述した式（４）の行列方程式を求める。マニピュ
レータの順キネマテクス方程式は、前述した式（１
１）、すなわち Ａhand ＝ Ａ1 ・Ａ2 ・Ａ3 ・Ａ4 ・Ａ5 ・Ａ6 ……（１１） で与えられる。ここでは、実施形態１と同様、上記式
（１１）に基づいて、１４本の基本方程式（２本のスカ
ラ方程式、４本のベクトル方程式）を導出し、次いでそ
れら１４本の基本方程式を使ってさらに６本の基本方程
式を導出する。それら、合計２０本の基本方程式をまと
めると、前述の式（４）の行列方程式が生成される。 （１４本の基本方程式の導出） 式（１１）を以下のように書き換える。 Ａ3 ・Ａ4 ・Ａ5 ＝ Ａ2 -1 ・Ａ1 -1 ・Ａhand・Ａ6 -1 ……（４３） 式（４３）を基に、先ず、以下の基本ベクトル方程式が
導かれる。 ｚA ≡Ａ3 Ａ4 Ａ5 ｅ3 ＝Ａ2 -1 Ａ1 -1 ＡhandＡ6 -1 ｅ3 ≡ｚB ……（４４） ｐA ≡Ａ3 Ａ4 Ａ5 ｅ4 ＝Ａ2 -1 Ａ1 -1 ＡhandＡ6 -1 ｅ4 ≡ｐB ……（４５）
【００９０】ここで、ｅi （ｉ＝１，……，４）は単位
ベクトルである。式（４４），式（４５）において、第
４成分は、１＝１，０＝０といった自明の方程式となる
ため、以後は無視し、式（４４），式（４５）は第１、
第２、第３成分からなる３次元ベクトルを表すものとす
る。式（４４），式（４５）のｐ，ｚを基に、内積、外
積、ベクトル和をとることにより、以下の方程式が得ら
れる。 （ｐT ・ｐ）A ＝（ｐT ・ｐ）B ……（４６） （ｐT ・ｚ）A ＝（ｐT ・ｚ）B ……（４７） （ｐT ×ｚ）A ＝（ｐT ×ｚ）B ……（４８） ［ｐ×（ｐ×ｚ）＋（ｐT ・ｚ）ｐ］A ＝［ｐ×（ｐ×ｚ）＋（ｐT ・ｚ）ｐ］B ……（４９） 式（４９）は、以下のように書き換えることもできる。 ［２（ｐT ・ｚ）ｐ−（ｐT ・ｐ）ｚ］A ＝［２（ｐT ・ｚ）ｐ−（ｐT ・ｐ）ｚ］B ……（５０）
【００９１】数式処理を使った精密な解析の結果、式
（４４）から式（４９）ないし式（５０）の方程式は、
ｃｏｓθi ，ｓｉｎθi に対して線形であることがわか
る。ｐ，ｚからなる他のコンビネーションは、線形性を
欠く。数式処理を適用するに当たり、式（４４）〜式
（４９）ないし式（５０）の方程式は式（８），式
（９）を使って以下のように分解しておく。 （１）ｚA ＝ｚB ｚA ＝Ｃz,3 Ｃx,3 Ｃ4 Ｃ5 ｅ3 ＝Ｃ2 T Ｃ1 T ＣhandＣ6 T ｅ3 ＝ｚB ……（５１） （２）ｐA ＝ｐB ｐA ＝Ｃz,3(Ｃx,3 Ｃ4 Ｃz,5・ｔ5 ＋Ｃx,3 Ｃz,4 ・ｔ4＋ｔ3 ） ＝−Ｃ2 T （Ｃ1 ＣhandＣ6 T ・ｔ6 −Ｃ1 T ・ｔhand ＋Ｃx,1 T ・ｔ1 ）−Ｃx,2 T ・ｔ2 ＝ｐB ……（５２） （３）（ｐT ・ｐ）A ＝（ｐT ・ｐ）B （ｐT ・ｐ）A ＝ｔ3 T ・ｔ3 ＋２ｔ3 T Ｃx,3 Ｃz,4 ・ｔ4 ＋２ｔ3 T Ｃx,3 Ｃ4 Ｃz,5 ・ｔ5 ＋ｔ4 T ・ｔ4 ＋２ｔ4 T Ｃx,4 Ｃz,5 ・ｔ5 ＋ｔ5 T ・ｔ5 ＝ｔ2 T ・ｔ2 ＋２ｔ2 T Ｃz,2 T Ｃx,1 T ・ｔ1 −２ｔ2 T Ｃz,2 T Ｃ1 T ・ｔhand ＋２ｔ2 T Ｃz,2 T Ｃ1 T ・ＣhandＣx,6 T ・ｔ6 ＋ｔ1 T ・ｔ1 −２ｔ1 T Ｃz,1 T ・ｔhand ＋２ｔ1 T Ｃz,1 T ＣhandＣx,6 T ・ｔ6 ＋ｔhand T ・ｔhand −２ｔhand T ＣhandＣx,6 T ・ｔ6 ＋ｔ6 T ・ｔ6 ＝（ｐT ・ｐ）B ……（５３） （４）（ｐT ・ｚ）A ＝（ｐT ・ｚ）B ＝（ｚT ・ｐ）A ＝ｅ3 T ・Ｃx,5 T・ｔ5 ＋ｅ3 TＣ5 T Ｃx,4 T・ｔ4 ＋ｅ3 T ・Ｃ5 T Ｃ4 T Ｃx3 T ・ｔ3 ＝−ｅ3 T ｔ6 ＋ｅ3 T Ｃ6 Ｃhand T ・ｔhand −ｅ3 T Ｃ6 Ｃhand T Ｃz,1 ・ｔ1 −ｅ3 T Ｃ6 Ｃhand T Ｃ1 Ｃz,2 ・ｔ2 ＝（ｚT ・ｐ）B ＝（ｐT ・ｚ）B ……（５４） （５）（ｐT ×ｚ）A ＝（ｐT ×ｚ）B （ｐT ×ｚ）A ＝Ｃz,3 Ｃx,3 Ｃ4 Ｃz,5 （ｔ5 ×Ｃx,5 ｅ3 ） ＋Ｃz,3 Ｃx,3 Ｃz,4 （ｔ4 ×Ｃx,4 Ｃ5 ｅ3 ） ＋Ｃz,3 （ｔ3 ×Ｃx,3 Ｃ4 Ｃ5 ｅ3 ） ＝−Ｃ2 TＣ1 TＣhand T Ｃx,6 T（ｔ6 ×Ｃz,6 Tｅ3 ） ＋Ｃ2 T Ｃ1 T （ｔhand×ＣhandＣ6 T ｅ3 ） −Ｃ2 T Ｃx,1 T（ｔ1×Ｃz,1 TＣhandＣ6 Tｅ3 ） −Ｃx,2 T（ｔ2 ×Ｃz,2 TＣ1 T ＣhandＣ6 Tｅ3 ） ＝（ｐT ×ｚ）B ……（５５） （６）［２（ｐT ・ｚ）ｐ−（ｐT ・ｐ）ｚ］A ＝［２（ｐT ・ｚ）ｐ−（ｐT ・ｐ）ｚ］B ［２（ｐT ・ｚ）ｐ−（ｐT ・ｐ）ｚ］A ＝２（ｐT ・ｚ）A Ｃz,3 （Ｃx,3 Ｃ4 Ｃz,5 ・ｔ5 ＋Ｃx,3 Ｃz,4 ・ｔ4 ＋ｔ3 ）−（ｐT ・ｐ）A Ｃz3Ｃx3Ｃ4 Ｃ5 ｅ3 ＝２（ｐT ・ｚ）B Ｃx,2 T （Ｃz,2 T Ｃ1 T ＣhandＣ6 T ・ｔ6 −Ｃz,2 TＣ1 T ・ｔhand＋Ｃz,2 T Ｃx,1 T ・ｔ1 ＋ｔ2 ） −（ｐT ・ｐ）B Ｃ2 T Ｃ1 T ＣhandＣ6 T ｅ3 ＝［２（ｐT ・ｚ）ｐ−（ｐT ・ｐ）ｚ］B ……（５６）
【００９２】（基本方程式の性質） 式（５１）から式（５６）の基本方程式は以下の性質を
有する。 （ａ）マニピュレータ先端の位置・姿勢を表すマトリク
スＡhandの各要素は、右辺のみに表れる。 （ｂ）全ての方程式はθ6 に依存しない。 なぜならば、式（５１）から式（５６）の方程式におい
てθ6 は、Ｃ6 T ｅ3＝Ｃx,6 T Ｃz,6 T ｅ3 の形で表
れるが、Ｃz,6 T ｅ3 ＝［０ ０ １］Tである。 （ｃ）ｚ3 ，ｐ3 ，ｐT ・ｚ，ｐT ・ｐ，（ｐT ×ｚ）
3 ，［２（ｐT ・ｚ）ｐ−（ｐT ・ｐ）ｚ］3 は、θ3
に依存しない。 なぜならば、ｐT ・ｚ，ｐT ・ｐはＣz,3 依存性をもた
ない。ｚ3 ，ｐ3 ，（ｐT ×ｚ）3 ，［２（ｐT ・ｚ）
ｐ−（ｐT ・ｐ）ｚ］3 に関しては、θ3 は、常にｅ3
T Ｃz,3 の形で表れるが、ｅ3 T Ｃz,3 ＝ｅ3 T であ
る。 （ｄ）ｚ1 ，ｚ2 ，ｐ1 ，ｐ2 ，（ｐT ×ｚ）1 ，（ｐ
T ×ｚ）2 ，［２（ｐT ・ｚ）ｐ−（ｐT ・ｐ）ｚ］
1 ，［２（ｐT ・ｚ）ｐ−（ｐT ・ｐ）ｚ］2 は、ｔａ
ｎ（θ3 ／２）に対して線形である。 なぜならば、ｘi ＝ｔａｎ（θi ／２）とする時、 ｃｏｓθi ＝（１−ｘi 2 ）／（１＋ｘi 2 ）， ｓｉｎθi ＝２ｘi ／（１＋ｘi 2 ） Ｘ3 - ・Ｃz,3 ＝Ｘ3 +
【００９３】
【数５３】
【００９４】が成立する。ｚ，ｐ，ｐT ×ｚ，２（ｐT
・ｚ）ｐ−（ｐT ・ｐ）ｚにおいて、左からＸ3 -を掛け
ることにより、上記の結果を得る。 （ｅ）θ1 ，θ2 ，θ4 ，θ5 は、ｃｏｓθi ，ｓｉｎ
θi （ｉ＝１，２，４，５）に対して常に線形で表れ、
（ｃｏｓθi ）2 ，（ｓｉｎθi ）2 ，ｃｏｓθi ・ｓ
ｉｎθi の項は表れない。なぜならば、ｚ，ｐ，ｐT ・
ｐ，ｐT ・ｚ，ｐT ×ｚにおいて、Ｃz,i は各々一回ず
つしか表れない。また、２（ｐT ・ｚ）ｐ−（ｐT ・
ｐ）ｚに関しては、ｐ，ｚ，ｐT ・ｚ，ｐT ・ｐの値を
代入し、数式処理を使い、（ｃｏｓθi ）2＋（ｓｉｎ
θi ）2 ＝１，（ｃｏｓαi ）2 ＋（ｓｉｎαi ）2 ＝
１に注意し整理すると、線形であることが判明する。 （基本方程式の行列形式） 式（５１）から式（５６）の基本方程式は、以下のよう
な行列形式にまとめることができる。
【００９５】
【数５４】
【００９６】 Ｕ’，Ｖ’∈Ｒ（１４×１８），ξ’，ρ∈Ｃ9 ξ’＝［ｓ1 ・ｓ2 ｓ1 ・ｃ2 ，ｃ1 ・ｓ2 ，ｃ1 ・ｃ2 ，ｓ1 ，ｃ1 ， ｓ2 ，ｃ2 ，１］T ρ＝［ｘ4 2ｘ5 2，ｘ4 2ｘ5 ，ｘ4 2，ｘ4 ｘ5 2，ｘ4 ｘ5 ，ｘ4 ，ｘ5 2， ｘ5 ，１］T Ｕ’，Ｖ’の成分は、式（５１）から式（５６）を丹念
に数式処理し、（ｃｏｓθi ）2 ＋（ｓｉｎθi ）2 ＝
１（ｉ＝１，２），（ｃｏｓαi ）2 ＋ｓｉｎαi ）2
＝１（ｉ＝１，……，６），ｃｏｓθi ＝（１−ｘi
2 ）／（１＋ｘi 2 ），ｓｉｎθi ＝２ｘi ／（１＋ｘi
2 ）（ｉ＝３，４，５）を代入し、得られた代数方程
式においてξ’，ρの各項に対する係数を求めることに
より得られる。Ｕ’，Ｖ’の各成分は、ｔi およびＡ
handの成分を変数とする多変数多項式である。尚、本実
施形態２では、前述した実施形態１と比べｉ＝１，２と
ｉ＝４，５とが入れ代わっている。
【００９７】（基本方程式の追加） ｚ3 ，ｐ3 ，ｐT ・ｚ，ｐT ・ｐ，（ｐT ×ｚ）3 ，
［２（ｐT ・ｚ）ｐ−（ｐT ・ｐ）ｚ］3 はθ3 に依存
しないので、ｘ3 ＝ｔａｎ（θ3 ／２）を掛け、線形和
を取ることにより以下の６本の方程式を得ることができ
る。 ［ｘ3 ｚ3 ＋ｐ3 ］A ＝［ｘ3 ｚ3 ＋ｐ3 ］B ……（５９） ［ｘ3 ｐ3 ＋ｚ3 ］A ＝［ｘ3 ｐ3 ＋ｚ3 ］B ……（６０） ［ｘ3 ｐT ・ｐ＋ｐT ・ｚ］A ＝［ｘ3 ｐT ・ｐ＋ｐT ・ｚ］B ……（６１） ［ｘ3 ｐT ・ｚ＋ｐT ・ｐ］A ＝［ｘ3 ｐT ・ｚ＋ｐT ・ｐ］B ……（６２） ［ｘ3 （ｐT ×ｚ）3 ＋ｚ3 ］A ＝［ｘ3 （ｐT ×ｚ）3 ＋ｚ3 ］B ……（６３） ［ｘ3 ｛２（ｐT ・ｚ）ｐ−（ｐT ・ｐ）ｚ］3 ｝＋ｐ3 ］A ＝［ｘ3 ｛２（ｐT ・ｚ）ｐ−（ｐT ・ｐ）ｚ］3 ｝＋ｐ3 ］B ……（６４） 式（５９）から式（６４）を式（５８）と同様にまと
め、左辺の一部を右辺に移項し、式（５８）と一緒に整
理すると、前述した式（４）の行列方程式が得られる。
【００９８】
【数５５】
【００９９】 Ｕ∈Ｒ（２０×１６），Ｖ∈Ｒ（２０×１８），ξ∈Ｃ16，ρ∈Ｃ9 ξ＝［ｘ3 ｓ1 ｓ2 ，ｘ3 ｓ1 ｃ2 ，ｘ3 ｃ1 ｓ2 ，ｘ3 ｃ1 ｃ2 ， ｘ3 ｓ1 ，ｘ3 ｃ1 ，ｘ3 ｓ2 ，ｘ3 ｃ2 ，ｓ1 ｓ2 ，ｓ1 ｃ2 ， ｃ1 ｓ2 ，ｃ1 ｃ2 ，ｓ1 ，ｃ1 ，ｓ2 ，ｃ2 ］T ρ＝［ｘ4 2ｘ5 2，ｘ4 2ｘ5 ，ｘ4 2，ｘ4 ｘ5 2，ｘ4 ｘ5 ，ｘ4 ，ｘ5 2， ｘ5 ，１］T 尚、この実施形態２においても、前述した実施形態１と
同様、パラメータｔiとして前述の式（１０）、すなわ
ち、Ｄ−Ｈパラメータ ｔi ＝［ａi ０ ｄi ］T ……（１０） を採用してもよく、この式（１０）に代わり、パラメー
タｔi としていわゆる修正Ｄ−Ｈパラメータ、すなわ
ち、 ｔi ＝［ξi ηi ζi ＋γi ］T ……（２８） を採用してもよい。
【０１００】以上が、本実施形態２における、図１のス
テップ（ａ）の説明である。本実施形態２における、ス
テップ（ｂ）の過程は、前述した実施形態１においてパ
ラメータｔi の各成分を式（１）に数値代入したことに
代わり、パラメータｔi の各成分を式（４）に数値代入
することが異なるだけであり、詳細説明は省略する。パ
ラメータｔi の各成分を式（４）に数値代入することに
より、Ａhandの成分を変数とする、前述の式（５）の行
列方程式、すなわち、
【０１０１】
【数５６】
【０１０２】 Ｕ∈Ｒ（２０×１６），Ｖ∈Ｒ（２０×１８），ξ∈Ｃ16，ρ∈Ｃ9 ξ＝［ｘ3 ｓ1 ｓ2 ，ｘ3 ｓ1 ｃ2 ，ｘ3 ｃ1 ｓ2 ，ｘ3 ｃ1 ｃ2 ， ｘ3 ｓ1 ，ｘ3 ｃ1 ，ｘ3 ｓ2 ，ｘ3 ｃ2 ，ｓ1 ｓ2 ，ｓ1 ｃ2 ， ｃ1 ｓ2 ，ｃ1 ｃ2 ，ｓ1 ，ｃ1 ，ｓ2 ，ｃ2 ］T ρ＝［ｘ4 2ｘ5 2，ｘ4 2ｘ5 ，ｘ4 2，ｘ4 ｘ5 2，ｘ4 ｘ5 ，ｘ4 ，ｘ5 2， ｘ5 ，１］T が導出される。数式処理を用いた行列方程式は、上記式
（５）まで、すなわち、図１のステップ（ｃ）までであ
る。以後の過程は、全て数値行列を取り扱う。すなわ
ち、マニピュレータ先端の位置と姿勢を表わすＡhandの
各成分を式（５）に数値代入することにより（図１ステ
ップ（ｄ））、数値行列Ｕnum ，Ｖnumによる数値行列
方程式を求め（図１ステップ（ｅ））、その数値行列方
程式に基づいて各関節ｉの回転角θi (k)（ｋは、θi の
複数角を相互に識別する最大１６までの正の整数をあら
わす）を求める（図１ステップ（ｆ））。この数値行列
方程式は前述の式（６）、すなわち、
【０１０３】
【数５７】
【０１０４】 Ｕ∈Ｒ（２０×１６），Ｖ∈Ｒ（２０×１８），ξ∈Ｃ16，ρ∈Ｃ9 ξ＝［ｘ3 ｓ1 ｓ2 ，ｘ3 ｓ1 ｃ2 ，ｘ3 ｃ1 ｓ2 ，ｘ3 ｃ1 ｃ2 ， ｘ3 ｓ1 ，ｘ3 ｃ1 ，ｘ3 ｓ2 ，ｘ3 ｃ2 ，ｓ1 ｓ2 ，ｓ1 ｃ2 ， ｃ1 ｓ2 ，ｃ1 ｃ2 ，ｓ1 ，ｃ1 ，ｓ2 ，ｃ2 ］T ρ＝［ｘ4 2ｘ5 2，ｘ4 2ｘ5 ，ｘ4 2，ｘ4 ｘ5 2，ｘ4 ｘ5 ，ｘ4 ，ｘ5 2， ｘ5 ，１］T であらわされる。
【０１０５】図１のステップ（ｄ）〜ステップ（ｆ）を
繰り返すことによりそのマニピュレータモデルについて
種々のシミュレーションを行なうことができる。次に、
式（６）の数値行列方程式に基づく、θi (k)の数値解
法、すなわち図１のステップ（ｆ）の詳細について説明
する。図４は、実施形態２に関する図１のステップ
（ｆ）の詳細フローを示した図である。以下では、いく
つかの数値解法について説明するが、いずれも、図４の
ステップ（ｋ）〜ステップ（ｎ）に従う。すなわち、以
下に説明するいずれの解法においても、θi (k)を求める
にあたり、先ず、式（６）の数値行列方程式に基づい
て、θ1 ，θ2 が消去された式を求める（ステップ
（ｋ））。次いで、そのθ1 ，θ2 が消去された式に基
づいて、固有値方程式もしくは一般化固有値方程式を求
める（ステップ（ｌ））。その後、その固有値方程式も
しくは一般化固有値方程式に基づいてθ3 (k)，θ4 (k)，
θ5 (k)を求め（ステップ（ｍ））、さらにその後
θ1 (k)，θ2 (k)，θ6 (k)を求める（ステップ（ｎ））。 この実施形態２が、前述した実施形態１と大きく異なる
点は、θ3 (k)，θ4 (k)，θ5 (k)を先に求め、次に
θ1 (k)，θ2 (k)，θ6 (k)を求めるところにある。両者の
違いは、端的には、Ｕ，Ｖマトリクスの成分の違いとし
て表れる。一般に、この実施形態２では、前述した実施
形態１に比べ、行列Ｕ，Ｖの階数が高く、数値計算を行
う場合に安定性がある。
【０１０６】以下、この図４のフローに従ったいくつか
の数値解法について説明する。 （数値解法５） （θ1 ，θ2 の消去） 式（６）の行列方程式からθ1 ，θ2 を消去するに当た
り、式（６）の左側から２０×２０の実行列Ｗを掛け
る。Ｗの各成分は、ランダムに生成する。ＷＵnum ≡Ｕ
ran ＝［Ｕ16ran ，Ｕ4ran］T ，Ｕ16ran ∈Ｒ（１６×
１６），Ｕ4ran∈Ｒ（４×１６），ＷＶnum ≡Ｖran ＝
［Ｖ16ran ，Ｖ4ran］T ，Ｖ16ran ∈Ｒ（１６×１
８），Ｖ4ran∈Ｒ（４×１８）とする時、｜ｄｅｔ（Ｕ
16ran ）｜＞ε（ε：微小定数）となるようにする。ｄ
ｅｔ（Ｕ16ran ）≒０の時は、Ｗの成分をとり直す。こ
の時、式（６）は以下のようになる。
【０１０７】
【数５８】
【０１０８】したがって
【数５９】
【０１０９】これを、式（６５）の残り４本の方程式に
代入することにより、θ1 ，θ2 が消去され、
【０１１０】
【数６０】
【０１１１】が得られる。 （固有値方程式（一般化固有値方程式）の導出） Ｖ16ran＝［x Ｖ16ran 1Ｖ16ran ］，Ｖ4ran＝［xＶ4ran，1Ｖ4ran］， x Ｖ16ran ，1 Ｖ16ran ∈Ｒ（１６×９）， x Ｖ4ran，1 Ｖ4ran∈Ｒ（４×９） とする時、上記式（６７）は、 ［Ｕ4ranＵ16ran -1 1 Ｖ16ran −1 Ｖ4ran］ρ ＋ｘ3 ［Ｕ4ranＵ16ran -1 x Ｖ16ran −x Ｖ4ran］ρ＝０ ……（６８） と表される。この式（６８）の両辺にそれぞれｘ4 ｘ
5 ，ｘ4 ，ｘ5 を掛けることにより、さらに１２本の方
程式が得られ、合計１６本の方程式が揃い、以下のよう
な一般固有値方程式に纏められる。 Ｇ・ρ＋ｘ3 Ｈ・ρ＝０ ……（６９） Ｇ，Ｈ∈Ｒ（１６×１６） ρ∈Ｃ16 ｘ3 ∈Ｃ
【０１１２】
【数６１】
【０１１３】 ρ＝［ｘ4 3ｘ5 3，ｘ4 3ｘ5 2，ｘ4 3ｘ5 ，ｘ4 3，ｘ4 2ｘ5 3，ｘ4 2ｘ5 2， ｘ4 2ｘ5 ，ｘ4 2，ｘ4 ｘ5 3，ｘ4 ｘ5 2，ｘ4 ｘ5 ，ｘ4 ｘ5 3，ｘ5 2， ｘ5 ，１］T ここで、ａ，……，ｔは、 Ｕ4ranＵ16ran -1 1 Ｖ16ran−1Ｖ4ran＝［ａｂｃｄｅｆｇｈｊ］ Ｕ4ranＵ16ran -1 x Ｖ16ran− xＶ4ran＝［ｋｌｍｎｐｑｒｓｔ］ ａ，ｂ，ｃ，ｄ，ｅ，ｆ，ｇ，ｈ，ｊ，ｋ，ｌ，ｍ，ｎ，ｐ，ｑ，ｒ，ｓ，ｔ ∈Ｒ4 で与えられる。｜ｄｅｔ（Ｈ）｜＞ε（ε：微小定数）
の場合には、以下のような固有値方程式となる。 Ｈ-1Ｇρ＋Ｘ3 ρ＝０ ……（７２） （固有値方程式（一般化固有値方程式）の数値解法） ｜ｄｅｔ（Ｈ）｜＞εの場合には、式（７２）の固有値
方程式をＱＲ法で解き、ｄｅｔ（Ｈ）≒０の場合には、
式（６９）の一般化固有値方程式をＱＺ法で解き、固有
値と固有ベクトルを求める。ＱＲ法、ＱＺ法に関して
は、前掲の、「Ｇｅｎｅ Ｈ． Ｇｏｌｕｂ， Ｃｈａ
ｒｌｅｓ Ｆ． ｖａｎ Ｌｏａｎ， “Ｍａｔｒｉｘ
Ｃｏｍｐｕｔａｔｉｏｎｓ”，Ｔｈｅ Ｊｏｈｎ Ｈ
ｏｐｋｉｎｓ Ｕｎｉｖｅｒｓｉｔｙ Ｐｒｅｓｓ」を
参照されたい。固有値、固有ベクトルの組は、複素解も
含め、一般に１６通り存在する。固有ベクトルに関して
は、１６番目の成分が１になるように規格化する。この
時、固有値の値からｘ3 、固有ベクトルの第１２成分、
第１５成分の値からｘ4 ，ｘ5 の値が求まる。従って、 θi (k) ＝ ２ａｒｃｔａｎ（ｘi (k) ） ……（７３） ｉ＝３，４，５；ｋ＝１，……，１６ により、第３、第４、第５関節角の値が求まる。 （θ1 (k) ，θ2 (k) ，θ6 (k) の算出） θ1 (k) ，θ2 (k) に関しては、式（６６）からξを求
め、ベクトルξの第１３〜第１６成分の値を使ってｃi
(k) ，ｓi (k) を求め、これらｃi (k) ，ｓi (k) に合
致する角度θi (k)（ｉ＝１，２；ｋ＝１，……，１６） を求める。θ6 (k) に関しては、 Ｃz,6 ＝Ａ5 -1Ａ4 -1Ａ3 -1Ａ2 -1Ａ1 -1ＡhandＣx,6 T ……（７４） より、ｃ6 (k) ，ｓ6 (k) の値を求め、これらｃ6
(k) ，ｓ6 (k) に合致する角度θ6 (k)を求める。
【０１１４】（数値解法６） 数値解法５ではＵ16ran を正則化させるために正方行列
Ｗをランダムに生成させたが、ここでは、Ｕnum を特異
値分解し、以下のようにＷを決定する。 ＱT Ｕnum Ｐ ＝ ｄｉａｇ（σ1 …，σp ）∈Ｒ（２０×１６） ……（７５） Ｑ∈Ｒ（２０×２０）：直交行列 Ｐ∈Ｒ（２０×１６）：直交行列 Ｗ＝ＱT この時、Ｕ16ran は正則化する。以後の処理は、数値解
法５と同様である。
【０１１５】（数値解法７） 一般化固有値方程式の解法は、通常の固有値方程式の解
法に比べ、２．５倍から３倍の計算時間を要する。従っ
て、ここでは、一般化固有値方程式の直接的な解法をで
きるだけ避ける方法を採る。正方行列Ｗとして、数値解
法５と同様に各成分をランダムに生成させる。式（６
９）において、ｄｅｔ（Ｈ）≒０の場合に、一般化固有
値方程式をＱＺ法で解く代わりに、次のような有理変換
を行なう。 ｘ3 ＝（ａｘ3 ’＋ｂ）／（ｃｘ3 ’７＋ｄ） ……（７６） この時、式（６９）の一般化固有値方程式は以下のよう
になる。 Ｇ’・ρ＋ｘ3 ’Ｈ’・ρ＝０ ……（７７） Ｇ’＝ｄＧ＋ｂＨ Ｈ’＝ｃＧ＋ａＨ Ｇ’，Ｈ’∈Ｒ（１６×１６） ρ∈Ｃ16 ｘ3 ’∈Ｃ 多くの場合、｜ｄｅｔ（Ｈ’）｜＞εとなるため、次の
固有値方程式を解く、 Ｈ’-1Ｇ’・ρ＋ｘ3 ’ρ＝０ ……（７８） ｄｅｔ（Ｈ’）≒０の場合には、式（７７）の一般化固
有値方程式を直接解く。以後の処理は、数値解法５と同
様である。
【０１１６】（数値解法８） 正方行列Ｗとして、数値解法６においてＵnum を特異値
分解した時のＱT を使う。以後の処理は、数値解法７と
同様である。 （数値解法９） ここでは、前述した式（６）から、図３のステップ
（ｋ）でθ1 ，θ2 を消去する過程において、式（６
６）を経由する代わりに式（６）からダイレクトにξを
求める。式（６）において、Ｕnum は正方行列ではない
が、その階数は１６である。従って、Ｕnum にガウスの
消去法を適用することにより、行列Ｕnum は、以下のよ
うに変換できる。
【０１１７】
【数６２】
【０１１８】式（７９）の消去法を式（６）の両辺に適
用することにより、式（６）は以下のように変換でき
る。
【０１１９】
【数６３】
【０１２０】式（８０）から下式を導くことができる。
【０１２１】
【数６４】
【０１２２】このＶ’4numを Ｖ’4num＝［ xＶ’4ran， 1Ｖ’4ran］x Ｖ’4ran， 1Ｖ’4ran∈Ｒ（４×９） と分解し、1 Ｖ’4ran＝［ａｂｃｄｅｆｇｈｊ］x Ｖ’4ran＝［ｋｌｍｎｐｑｒｓｔ］ とする時、式（６９）〜式（７１）と同様の固有値方程
式が得られる。以後の処理は、数値解法５と同様であ
る。上記の数値解法９を採用すると、数値解法５から数
値解法８の方法に比べ、行列Ｈの正則性（ｄｅｔ（Ｈ）
≠０）が極めて良く保たれ、固有値方程式の解が精度よ
く求まる。
【０１２３】（数値解法１０） 数値解法９で求めたＨの正則性が保たれない時に、式
（７６）の有理変換を行なう。以後の処理は、数値解法
７と同等である。以上の実施形態２においても、前述し
た実施形態１と同様、マニピュレータのシミュレートが
高速化される。したがって、コンピュータグラフィック
スにおいてリアルタイムでシミュレートを行いながら表
示画面上にそのシミュレート結果を表示したり、リアル
タイムにシミュレートを行ないながらハードウェアとし
てのマニピュレータを制御することが可能となる。
【０１２４】図５は、本発明の第１のマニピュレータシ
ミュレート装置の一実施形態を表わすブロック図であ
る。以下、この実施形態を「実施形態３」と称する。演
算アルゴリズムについては前述した実施形態１，実施形
態２の説明中で詳述したため、この実施形態３及び後述
する各実施形態においては、演算アルゴリズムの詳細説
明は省略し、必要に応じて前述した数式の番号を引用す
るにとどめる。この図５に示すマニピュレータシミュレ
ート装置には、前述した（１）の行列方程式を記憶して
おく記憶手段１１が備えられている。この記憶手段１１
は、式（１）の行列方程式を固定的に記憶しておくもの
であってもよいが、そうである必要はなく、例えばフロ
ッピィディスク等の外部記憶媒体から式（１）の行列方
程式をロードして記憶するものであってもよい。
【０１２５】この記憶手段１１に格納された式（１）の
行列方程式は、パラメータ数値代入手段１２に読み出さ
れ、パラメータ数値代入手段１２では、読み出された式
（１）の行列方程式に、入力手段１３から入力されたパ
ラメータｔi の各成分の数値を代入し、これにより、前
述した式（２）の行列方程式を生成する。入力手段１３
は、例えばキーボード等、パラメータｔi の各成分をマ
ニュアルで入力するものであってもよいが、それに限ら
れず、例えば予め記憶されたパラメータｔi の各成分の
数値情報を読み出してパラメータ数値代入手段１２にロ
ードするもの、あるいは通信回線等を経由して外部から
送信されてきたパラメータｔi の各成分の数値情報を受
信してパラメータ数値代入手段１２に渡すものであって
もよい。パラメータ数値代入手段１２で生成された式
（２）の行列方程式は、数値行列方程式導出手段１４に
入力される。この数値行列方程式導出手段１４には、入
力手段１５から入力されたＡhandの各成分（各要素）の
数値を代入することにより、前述した式（３）の数値行
列方程式を生成する。この入力手段１５も、前述した入
力手段１３と同様、種々の構成をとることができる。ま
た、この入力手段１５は、物理的には、前述した入力手
段１３と同一のものであってもよい。数値行列方程式導
出手段１４で導出された式（３）の数値行列方程式は、
関節回転角算出手段１６に入力される。この関節回転角
算出手段１６では、前述した数値解法１〜数値解法４の
いずれか、もしくは、それらを組合せた解法により関節
の回転角θi (k) を求める。この求められた関節角θi
(k) は、表示手段１７に送られてコンピュータグラフィ
ックス表示されたり、制御手段１８に送られて実際のハ
ードウェアとしてマニピュレータの関節回転角の制御に
用いられる。このマニピュレータシミュレート装置で
は、最初の準備を除き、実際のシミュレート時にはＡ
handの数値入力と、前述した数値解法１〜数値解法４の
いずれかを採用して数値行列方程式を解くこととを繰り
返せばよく、高速なシミュレーションが可能である。
【０１２６】図６は、図５に示す実施形態３における関
節回転角算出手段１６の詳細ブロック図である。この関
節回転角算出手段１６は、式（３）の数値行列方程式に
基づいてθ4 ，θ5 を消去する手段１６１と、そのθ
4 ，θ5 が消去された式に基づいて固有値方程式もしく
は一般化固有値方程式を求める手段１６２と、その固有
値方程式もしくは一般化固有値方程式を解くことにより
θ3 (k) ，θ2 (k) ，θ1 (k) を算出する手段１６３
と、さらに、θ4 (k)，θ5 (k) ，θ6 (k) を算出する
手段１６４とから構成されている。各手段１６１〜１６
４で実行されるアルゴリズムは、前述した数値解法〜数
値解法４の欄で説明したため、ここでは重複説明は省略
する。
【０１２７】図７は、本発明の第２のマニピュレータシ
ミュレート装置の一実施形態を表わすブロック図であ
る。以下、この実施形態を「実施形態４」と称する。こ
の実施形態４では、前述した式（２）の行列方程式、す
なわち、パラメータｔi の各成分について既に数値入力
した後の行列方程式を記憶する記憶手段１９が備えられ
ている。他の構成は、図５に示す実施形態３と同様であ
り、重複説明は省略する。この記憶手段１９は、図５の
実施形態３における記憶手段１１と同様、式（２）の行
列方程式を固定的に記憶しておくものであってもよい
が、そうである必要はなく、例えばフロッピィディスク
等の記憶媒体からロードされた式（２）の行列方程式
や、図示しない演算手段等において生成された式（２）
の行列方程式を、シミュレーション間、一時的に記憶す
るものであってもよい。種々の構造のマニピュレータの
シミュレーションを行う必要がある場合には、図５に示
す実施形態３のように、シミュレーションを行なおうと
するマニピュレータに適合したパラメータｔi の各成分
を数値入力する必要があるが、既にある定まったマニピ
ュレータのシミュレーションを行う装置、例えば既に完
成されたある特定のハードウェアとしてのマニピュレー
タの制御用として固定的に用いられる装置等の場合、パ
ラメータｔi の各成分を数値入力できるように構成され
ている必要はなく、図７の実施形態４に示すように、あ
らかじめ、特定されたマニピュレータに関するパラメー
タｔi の各成分が数値入力された式（２）の行列方程式
を記憶しておいてもよい。
【０１２８】次に、本発明の第３のマニピュレータシミ
ュレート装置の一実施形態について説明する。以下、こ
の実施形態を「実施形態５」と称する。この実施形態５
のブロック図は、図５に示す実施形態３のブロック図と
同一であるため、以下、図５を再度参照しながら実施形
態５について説明する。この図５に示す、実施形態５と
してのマニピュレータシミュレート装置には、前述した
式（４）の行列方程式を記憶しておく記憶手段１１が備
えられている。この記憶手段１１は、式（４）の行列方
程式を固定的に記憶しておくものであってもよいが、そ
うである必要はなく、例えばフロッピィディスク等の外
部記憶媒体から式（４）の行列方程式をロードして記憶
するものであってもよい。この記憶手段１１に格納され
た式（４）の行列方程式は、パラメータ数値代入手段１
２に読み出され、パラメータ数値代入手段１２では、読
み出された式（４）の行列方程式に、入力手段１３から
入力されたパラメータｔi の各成分の数値を代入し、こ
れにより、前述した式（５）の行列方程式を生成する。
入力手段１３は、例えばキーボード等、パラメータｔi
の各成分をマニュアルで入力するものであってもよい
が、それに限られず、例えば、あらかじめ記憶されたパ
ラメータｔi の各成分の数値情報を読み出してパラメー
タ数値代入手段１２にロードするもの、あるいは、通信
回線等を経由して外部から送信されてきたパラメータｔ
i の各成分の数値情報を受信してパラメータ数値代入手
段１２に渡すものであってもよい。
【０１２９】パラメータ数値代入手段１２で生成された
式（５）の行列方程式は、数値行列方程式導出手段１４
に入力される。この数値行列方程式導出手段１４には、
入力手段１５から入力されたＡhandの各成分（各要素）
の数値を代入することにより、前述した式（６）の数値
行列方程式を生成する。この入力手段１５も、前述した
入力手段１３と同様、種々の構成をとることができる。
また、この入力手段１５は、物理的には、前述した入力
手段１３と同一のものであってもよい。数値行列方程式
導出手段１４で導出された式（６）の数値行列方程式
は、関節回転角算出手段１６に入力される。この関節回
転角算出手段１６では、前述した（６）式の数値解法５
〜数値解法１０のいずれか、もしくはそれらを組合せた
解法により関節の回転角θi (k) を求める。この求めら
れた関節回転角θi (k) は、表示手段１７に送られてコ
ンピュータグラフィックス表示されたり、制御手段１８
に送られて実際のハードウェアとしてマニピュレータの
関節回転角の制御に用いられる。このマニピュレータシ
ミュレート装置では、最初の準備を除き、実際のシミュ
レート時にはＡhandの数値入力と、前述した数値解法５
〜数値解法１０のいずれかを採用して数値行列方程式を
解くこととを繰り返せばよく、高速なシミュレーション
が可能である。
【０１３０】図８は、実施形態５における、図５に示す
関節回転角算出手段１６の詳細ブロック図である。この
関節回転角算出手段１６は、式（６）の数値行列方程式
に基づいてθ1 ，θ2 を消去する手段１６５と、そのθ
1 ，θ2 が消去された式に基づいて固有値方程式もしく
は一般化固有値方程式を求める手段１６６と、その固有
値方程式もしくは一般化固有値方程式を解くことによ
り、θ3 (k) ，θ4 (k) ，θ5 (k) を算出する手段１６
７と、さらに、θ1 (k) ，θ2 (k) ，θ6 (k) を算出す
る手段１６８とから構成されている。各手段１６５〜１
６８で実行されるアルゴリズムは、前述した数値解法５
〜数値解法１０の欄で説明済のため、ここでは重複説明
は省略する。
【０１３１】次に、本発明の第４のマニピュレータシミ
ュレート装置の一実施形態について説明する。以下、こ
の実施形態を「実施形態６」と称する。この実施形態６
のブロック図は、図７に示す実施形態４のブロック図と
同一であるため、以下図７を再度参照しながら実施形態
６について説明する。この実施形態６では、前述した式
（５）の行列方程式、すなわち、パラメータｔi の各成
分について既に数値入力した後の行列方程式を記憶して
おく記憶手段１９が備えられている。他の構成は、図５
に示す実施形態５と同様であり、重複説明は省略する。
この記憶手段１９は、図５の行列方程式を固定的に記憶
しておくものであってもよいが、そうである必要はな
く、例えばフロッピィディスク等の記憶媒体からロード
された式（５）の行列方程式や、図示しない演算手段等
において生成された式（５）の行列方程式を、シミュレ
ーションの間、一時的に記憶するものであってもよい。
【０１３２】種々の構造のマニピュレータのシミュレー
ションを行なう必要がある場合には、図５に示す実施形
態５のように、シミュレーションを行おうとするマニピ
ュレータに適合したパラメータｔi の各成分を数値入力
する必要があるが、既に定まったマニピュレータのシミ
ュレーションを行なう装置、例えば既に完成されたある
特定のハードウェアとしてのマニピュレータの制御用と
して固定的に用いられる装置等の場合、パラメータｔi
の各成分を数値入力できるように構成されている必要は
なく、図７の実施形態６に示すように、あらかじめ、特
定されたマニピュレータに関するパラメータｔi の各成
分が数値入力された式（５）の行列方程式を記憶してお
いてもよい。
【０１３３】
【発明の効果】以上説明したように、本発明によれば、
いわば数式処理できるところはできるだけ数式処理で追
い込んでおいて、最後、どうしても数値解法に頼らなけ
ればならない部分のみ、Ａhandを入力して数値処理を行
なうようにしたため、演算時間が短くて済み、リアルタ
イム処理に適する。
【図面の簡単な説明】
【図１】本発明の第１のマニピュレータシミュレート方
法の一実施形態を示したフローチャートである。
【図２】Ｄ−Ｈパラメータを説明するための、リンクお
よび関節の一部分を示した図である。
【図３】図１のステップ（ｆ）の詳細フローを示した図
である。
【図４】実施形態２に関する図１のステップ（ｆ）の詳
細フローを示した図である。
【図５】本発明の第１のマニピュレータシミュレート装
置の一実施形態を表わすブロック図である。
【図６】図５に示す関節回転角算出手段の詳細ブロック
図である。
【図７】本発明の第２のマニピュレータシミュレート装
置の一実施形態を表わすブロック図である。
【図８】実施形態５における、図５に示す関節回転角算
出手段の詳細ブロック図である。
【図９】複数のアームとそれらのアームどうしを連結す
る関節とから構成されたマニピュレータの一例を模式的
に示した図である。
【符号の説明】
１１ 記憶手段 １２ パラメータ数値代入手段 １３ 入力手段 １４ 数値行列方程式導出手段 １５ 入力手段 １６ 関節回転角算出手段 １７ 表示手段 １８ 制御手段 １９ 記憶手段
フロントページの続き (56)参考文献 特開 平７−146703（ＪＰ，Ａ) 特開 平４−313105（ＪＰ，Ａ) 特開 平７−132474（ＪＰ，Ａ) 特開 平５−111897（ＪＰ，Ａ) 特開 平６−19528（ＪＰ，Ａ) (58)調査した分野(Int.Cl.7，ＤＢ名) B25J 9/10 G05B 19/18

Claims (6)

    (57)【特許請求の範囲】
  1. 【請求項１】 順次連結された６本以内のアームを有す
    るマニピュレータの先端の位置と姿勢を表わす情報に基
    づいて、該位置と姿勢を実現するための、アームどうし
    を連結する関節の回転角を求めるマニピュレータシミュ
    レート装置において、 絶対座標系からみた前記先端の位置と姿勢をあらわす同
    次座標変換行列Ａhandを、 【数１】 但し、Ｃhandは姿勢、ｔhandは位置をあらわすとし、前
    記マニピュレータの後端から先端に向かって順次に付さ
    れた座標系のうち、ｉ番目の座標系からみたｉ＋１番目
    の座標系の位置と姿勢を表わす同次座標変換行列Ａi
    を、 【数２】 としたとき、 前記同次座標変換行列Ａhandおよび前記同次座標変換行
    列Ａi に基づいて求められる式（１）の行列方程式 【数３】 但し、 Ｕは２０×１６の実数行列 Ｖは２０×１８の実数行列 ξ＝［ｘ3 ｓ4 ｓ5 ，ｘ3 ｓ4 ｃ5 ，ｘ3 ｃ4 ｓ5 ，ｘ3 ｃ4 ｃ5 ， ｘ3 ｓ4 ，ｘ3 ｃ4 ，ｘ3 ｓ5 ，ｘ3 ｃ5 ，ｓ4 ｓ5 ，ｓ4 ｃ5 ， ｃ4 ｓ5 ，ｃ4 ｃ5 ，ｓ4 ，ｃ4 ，ｓ5 ，ｃ5 ］T ρ＝［ｘ1 2ｘ2 2，ｘ1 2ｘ2 ，ｘ1 2，ｘ1 ｘ2 2，ｘ1 ｘ2 ，ｘ1 ，ｘ2 2， ｘ2 ，１］T ｘi ＝ｔａｎ（θi ／２） を記憶する記憶手段と、 前記パラメータｔi の各成分を数値入力する第１の入力
    手段と、 前記第１の入力手段により入力された前記パラメータｔ
    i の各成分の数値を前記式（１）の行列方程式に代入す
    ることにより、前記先端の位置と姿勢をあらわす同次変
    換行列Ａhandの各成分を変数とする式（２）の行列方程
    式 【数４】 但し、Ｕ（Ａhand），Ｖ（Ａhand）は、それぞれ、前記
    実数行列Ｕ，Ｖに前記パラメータｔi の各成分を数値代
    入することにより生成される、前記同次変換行列Ａhand
    の各成分を変数とする行列であることを表わすを求める
    パラメータ数値代入手段と、 前記同次変換行列Ａhandの各成分を数値入力する第２の
    入力手段と、 前記第２の入力手段から入力された前記同次変換行列Ａ
    handの各成分の数値を前記式（２）の行列方程式に代入
    することにより、式（３）の数値行列方程式 【数５】 但し、Ｕnum ，Ｖnum は、それぞれ、前記行列Ｕ（Ａ
    hand），Ｖ（Ａhand）に前記同次変換行列Ａhandの各成
    分を数値代入することにより生成される数値行列である
    ことを表わすを求める数値行列方程式導出手段と、 前記式（３）の数値行列方程式に基づいて、前記関節の
    回転角θi (k)（ｋはθi の複数解を相互に識別する最大
    １６までの正の整数をあらわす）を求める関節回転角算
    出手段とを備えたことを特徴とするマニピュレータシミ
    ュレート装置。
  2. 【請求項２】 順次連結された６本以内のアームを有す
    るマニピュレータの先端の位置と姿勢を表わす情報に基
    づいて、該位置と姿勢を実現するための、アームどうし
    を連結する関節の回転角を求めるマニピュレータシミュ
    レート装置において、 絶対座標系からみた前記先端の位置と姿勢をあらわす同
    次座標変換行列Ａhandを、 【数６】 但し、Ｃhandは姿勢、ｔhandは位置をあらわすとし、前
    記マニピュレータの後端から先端に向かって順次に付さ
    れた座標系のうち、ｉ番目の座標系からみたｉ＋１番目
    の座標系の位置と姿勢を表わす同次座標変換行列Ａi
    を、 【数７】 としたとき、 前記同次座標変換行列Ａhandおよび前記同次座標変換行
    列Ａi に基づいて求められる式（１）の行列方程式 【数８】 但し、 Ｕは２０×１６の実数行列 Ｖは２０×１８の実数行列 ξ＝［ｘ3 ｓ4 ｓ5 ，ｘ3 ｓ4 ｃ5 ，ｘ3 ｃ4 ｓ5 ，ｘ3 ｃ4 ｃ5 ， ｘ3 ｓ4 ，ｘ3 ｃ4 ，ｘ3 ｓ5 ，ｘ3 ｃ5 ，ｓ4 ｓ5 ，ｓ4 ｃ5 ， ｃ4 ｓ5 ，ｃ4 ｃ5 ，ｓ4 ，ｃ4 ，ｓ5 ，ｃ5 ］T ρ＝［ｘ1 2ｘ2 2，ｘ1 2ｘ2 ，ｘ1 2，ｘ1 ｘ2 2，ｘ1 ｘ2 ，ｘ1 ，ｘ2 2， ｘ2 ，１］T ｘi ＝ｔａｎ（θi ／２） に前記パラメータｔi の各成分を数値代入することによ
    り求められる、前記先端の位置と姿勢をあらわす同次変
    換行列Ａhandの各成分を変数とする式（２）の行列方程
    式 【数９】 但し、Ｕ（Ａhand），Ｖ（Ａhand）は、それぞれ、前記
    実数行列Ｕ，Ｖに前記パラメータｔi の各成分を数値代
    入することにより生成される、前記同次変換行列Ａhand
    の各成分を変数とする行列であることを表わすを記憶す
    る記憶手段と、 前記同次変換行列Ａhandの各成分を数値入力する入力手
    段と、 前記入力手段から入力された前記同次変換行列Ａhandの
    各成分の数値を前記式（２）の行列方程式に代入するこ
    とにより、式（３）の数値行列方程式 【数１０】 但し、Ｕnum ，Ｖnum は、それぞれ、前記行列Ｕ（Ａ
    hand），Ｖ（Ａhand）に前記同次変換行列Ａhandの各成
    分を数値代入することにより生成される数値行列である
    ことを表わすを求める数値行列方程式導出手段と前記式
    （３）の数値行列方程式に基づいて、前記関節の回転角
    θi (k)（ｋはθi の複数解を相互に識別する最大１６ま
    での正の整数をあらわす）を求める関節回転角算出手段
    とを備えたことを特徴とするマニピュレータシミュレー
    ト装置。
  3. 【請求項３】 前記関節回転角算出手段が、 前記式（３）の数値行列方程式に基づいて、θ4 および
    θ5 が消去された式を求める手段と、 該θ4 およびθ5 が消去された式に基づいて、固有値方
    程式もしくは一般化固有値方程式を求める手段と、 該固有値方程式もしくは一般化固有値方程式に基づいて
    前記関節の回転角θi (k)を求める手段とを備えたことを
    特徴とする請求項１又は２記載のマニピュレータシミュ
    レート装置。
  4. 【請求項４】 順次連結された６本以内のアームを有す
    るマニピュレータの先端の位置と姿勢を表わす情報に基
    づいて、該位置と姿勢を実現するための、アームどうし
    を連結する関節の回転角を求めるマニピュレータシミュ
    レート装置において、 絶対座標系からみた前記先端の位置と姿勢をあらわす同
    次座標変換行列Ａhandを、 【数１１】 但し、Ｃhandは姿勢、ｔhandは位置をあらわすとし、前
    記マニピュレータの後端から先端に向かって順次に付さ
    れた座標系のうち、ｉ番目の座標系からみたｉ＋１番目
    の座標系の位置と姿勢を表わす同次座標変換行列Ａi
    を、 【数１２】 としたとき、前記同次座標変換行列Ａhandおよび前記同
    次座標変換行列Ａi に基づいて求められる式（４）の行
    列方程式 【数１３】 但し、 Ｕは２０×１６の実数行列 Ｖは２０×１８の実数行列 ξ＝［ｘ3 ｓ1 ｓ2 ，ｘ3 ｓ1 ｃ2 ，ｘ3 ｃ1 ｓ2 ，ｘ3 ｃ1 ｃ2 ， ｘ3 ｓ1 ，ｘ3 ｃ1 ，ｘ3 ｓ2 ，ｘ3 ｃ2 ，ｓ1 ｓ2 ，ｓ1 ｃ2 ， ｃ1 ｓ2 ，ｃ1 ｃ2 ，ｓ1 ，ｃ1 ，ｓ2 ，ｃ2 ］T ρ＝［ｘ4 2ｘ5 2，ｘ4 2ｘ5 ，ｘ4 2，ｘ4 ｘ5 2，ｘ4 ｘ5 ，ｘ4 ，ｘ5 2， ｘ5 ，１］T ｘi ＝ｔａｎ（θi ／２） を記憶する記憶手段と、 前記パラメータｔi の各成分を数値入力する第１の入力
    手段と、 前記第１の入力手段により入力された前記パラメータｔ
    i の各成分の数値を前記式（４）の行列方程式に代入す
    ることにより、前記先端の位置と姿勢をあらわす同次変
    換行列Ａhandの各成分を変数とする式（５）の行列方程
    式 【数１４】 但し、Ｕ（Ａhand），Ｖ（Ａhand）は、それぞれ、前記
    行列Ｕ，Ｖに前記パラメータｔi の各成分を数値代入す
    ることにより生成される、前記同次変換行列Ａhandの各
    成分を変数とする行列であることを表わすを求めるパラ
    メータ数値代入手段と、 前記同次変換行列Ａhandの各成分を数値入力する第２の
    入力手段と、 前記第２の入力手段から入力された前記同次変換行列Ａ
    handの各成分の数値を前記式（５）の行列方程式に代入
    することにより、式（６）の数値行列方程式 【数１５】 但し、Ｕnum ，Ｖnum は、それぞれ、前記行列Ｕ（Ａ
    hand），Ｖ（Ａhand）に前記同次変換行列Ａhandの各成
    分を数値代入することにより生成される数値行列である
    ことを表わすを求める数値行列導出手段と、 前記式（６）の数値行列方程式に基づいて、前記関節の
    回転角θi (k)（ｋはθi の複数解を相互に識別する最大
    １６までの正の整数をあらわす）を求める関節回転角算
    出手段とを備えたことをを特徴とするマニピュレータシ
    ミュレート装置。
  5. 【請求項５】 順次連結された６本以内のアームを有す
    るマニピュレータの先端の位置と姿勢を表わす情報に基
    づいて、該位置と姿勢を実現するための、アームどうし
    を連結する関節の回転角を求めるマニピュレータシミュ
    レート装置において、 絶対座標系からみた前記先端の位置と姿勢をあらわす同
    次座標変換行列Ａhandを、 【数１６】 但し、Ｃhandは姿勢、ｔhandは位置をあらわすとし、前
    記マニピュレータの後端から先端に向かって順次に付さ
    れた座標系のうち、ｉ番目の座標系からみたｉ＋１番目
    の座標系の位置と姿勢を表わす同次座標変換行列Ａi
    を、 【数１７】 としたとき、 前記同次座標変換行列Ａhandおよび前記同次座標変換行
    列Ａi に基づいて求められる式（４）の行列方程式 【数１８】 但し、 Ｕは２０×１６の実数行列 Ｖは２０×１８の実数行列 ξ＝［ｘ3 ｓ1 ｓ2 ，ｘ3 ｓ1 ｃ2 ，ｘ3 ｃ1 ｓ2 ，ｘ3 ｃ1 ｃ2 ， ｘ3 ｓ1 ，ｘ3 ｃ1 ，ｘ3 ｓ2 ，ｘ3 ｃ2 ，ｓ1 ｓ2 ，ｓ1 ｃ2 ， ｃ1 ｓ2 ，ｃ1 ｃ2 ，ｓ1 ，ｃ1 ，ｓ2 ，ｃ2 ］T ρ＝［ｘ4 2ｘ5 2，ｘ4 2ｘ5 ，ｘ4 2，ｘ4 ｘ5 2，ｘ4 ｘ5 ，ｘ4 ，ｘ5 2， ｘ5 ，１］T ｘi ＝ｔａｎ（θi ／２） に前記パラメータｔi の各成分を数値代入することによ
    り求められる、前記先端の位置と姿勢をあらわす同次変
    換行列Ａhandの各成分を変数とする式（５）の行列方程
    式 【数１９】 但し、Ｕ（Ａhand），Ｖ（Ａhand）は、それぞれ、前記
    行列Ｕ，Ｖに前記パラメータｔi の各成分を数値代入す
    ることにより生成される、前記同次変換行列Ａhandの各
    成分を変数とする行列であることを表わすを記憶する記
    憶手段と、 前記同次変換行列Ａhandの各成分を数値入力する入力手
    段と、 前記入力手段から入力された前記同次変換行列Ａhandの
    各成分の数値を前記式（５）の行列方程式に代入するこ
    とにより、式（６）の数値行列方程式 【数２０】 但し、Ｕnum ，Ｖnum は、それぞれ、前記行列Ｕ（Ａ
    hand），Ｖ（Ａhand）に前記同次変換行列Ａhandの各成
    分を数値代入することにより生成される数値行列である
    ことを表わすを求める数値行列導出手段と、 前記式（６）の数値行列方程式に基づいて、前記関節の
    回転角θi (k)（ｋはθi の複数解を相互に識別する最大
    １６までの正の整数をあらわす）を求める関節回転角算
    出手段と備えたことをを特徴とするマニピュレータシミ
    ュレート装置。
  6. 【請求項６】 前記関節回転角算出手段が、 前記式（６）の数値行列方程式に基づいて、θ1 および
    θ2 が消去された式を求める手段と、 該θ1 およびθ2 が消去された式に基づいて、固有値方
    程式もしくは一般化固有値方程式を求める手段と、 該固有値方程式もしくは一般化固有値方程式に基づいて
    前記関節の回転角θi (k)を求める手段とを備えたことを
    特徴とする請求項４又は５記載のマニピュレータシミュ
    レート装置。
JP20418695A 1995-08-10 1995-08-10 マニピュレータシミュレート装置 Expired - Fee Related JP3331100B2 (ja)

Priority Applications (2)

Application Number Priority Date Filing Date Title
JP20418695A JP3331100B2 (ja) 1995-08-10 1995-08-10 マニピュレータシミュレート装置
US08/618,808 US5867631A (en) 1995-08-10 1996-03-20 Manipulator simulation method and apparatus

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
JP20418695A JP3331100B2 (ja) 1995-08-10 1995-08-10 マニピュレータシミュレート装置

Publications (2)

Publication Number Publication Date
JPH0947987A JPH0947987A (ja) 1997-02-18
JP3331100B2 true JP3331100B2 (ja) 2002-10-07

Family

ID=16486266

Family Applications (1)

Application Number Title Priority Date Filing Date
JP20418695A Expired - Fee Related JP3331100B2 (ja) 1995-08-10 1995-08-10 マニピュレータシミュレート装置

Country Status (2)

Country Link
US (1) US5867631A (ja)
JP (1) JP3331100B2 (ja)

Families Citing this family (12)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US6023645A (en) * 1997-05-15 2000-02-08 Mitsubishi Denki Kabushiki Kaisha Trajectory control apparatus and trajectory control method for intra-planar multifreedom SCARA type of robot, and computer-readable recording medium with trajectory control program for intra-planar multifreedom SCARA type of robot stored therein
US6014150A (en) * 1997-08-01 2000-01-11 Avid Technology, Inc. System and method of defining and employing behaviors for articulated chains
US8260593B2 (en) * 2002-09-18 2012-09-04 Siemens Product Lifecycle Management Software Inc. System and method for simulating human movement
JP4735795B2 (ja) * 2003-12-26 2011-07-27 独立行政法人 宇宙航空研究開発機構 冗長マニピュレータの制御方法
US20050278157A1 (en) * 2004-06-15 2005-12-15 Electronic Data Systems Corporation System and method for simulating human movement using profile paths
US9129077B2 (en) * 2004-09-03 2015-09-08 Siemen Product Lifecycle Management Software Inc. System and method for predicting human posture using a rules-based sequential approach
WO2008109801A1 (en) * 2007-03-07 2008-09-12 Kmt Robotic Solutions, Inc. System and method of locating relative positions of objects
US7990398B2 (en) * 2007-04-13 2011-08-02 Apple Inc. Matching movement behavior in motion graphics
IT1400536B1 (it) 2010-05-26 2013-06-11 Oto Melara Spa Braccio robotizzato per un veicolo.
JP6305673B2 (ja) * 2011-11-07 2018-04-04 セイコーエプソン株式会社 ロボット制御システム、ロボットシステム及びロボット
CN105014666B (zh) * 2015-07-13 2016-10-05 广州霞光技研有限公司 一种多自由度机械手自主抓取逆解工程算法
CN115741672B (zh) * 2022-10-21 2024-04-19 杭州邦杰星医疗科技有限公司 一种基于刚体变换的dh推导方法

Family Cites Families (14)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US4884216A (en) * 1987-11-09 1989-11-28 Michael Kuperstein Neural network system for adaptive sensory-motor coordination of multijoint robots for single postures
US5177563A (en) * 1989-02-01 1993-01-05 Texas A&M University System Method and apparatus for locating physical objects
US5049796A (en) * 1989-05-17 1991-09-17 The United States Of America As Represented By The Administrator Of The National Aeronautics And Space Administration Robust high-performance control for robotic manipulators
US4999553A (en) * 1989-12-28 1991-03-12 The United States Of America As Represented By The Administrator Of The National Aeronautics And Space Administration Method and apparatus for configuration control of redundant robots
US4967126A (en) * 1990-01-30 1990-10-30 Ford Aerospace Corporation Method of controlling a seven degree of freedom manipulator arm
US5347616A (en) * 1991-01-28 1994-09-13 Tsubakimoto Chain Co. Method of controlling position and attitude of working robot and its manipulator and apparatus thereof
US5297238A (en) * 1991-08-30 1994-03-22 Cimetrix Incorporated Robot end-effector terminal control frame (TCF) calibration method and device
US5542028A (en) * 1991-10-29 1996-07-30 Tsubakimoto Chain Co. Method of controlling position and attitude of working robot and its manipulator and apparatus thereof
KR950010972B1 (ko) * 1991-12-07 1995-09-26 포항종합제철주식회사 업셋(offset)이 있는 로보트손목의 기구변수 결정법
US5550953A (en) * 1994-04-20 1996-08-27 The United States Of America As Represented By The Administrator Of The National Aeronautics And Space Administration On-line method and apparatus for coordinated mobility and manipulation of mobile robots
US5430643A (en) * 1992-03-11 1995-07-04 The United States Of America As Represented By The Administrator Of The National Aeronautics And Space Administration Configuration control of seven degree of freedom arms
US5294873A (en) * 1992-10-27 1994-03-15 The United States Of America As Represented By The Administrator Of The National Aeronautics And Space Administration Kinematic functions for redundancy resolution using configuration control
US5602968A (en) * 1994-05-02 1997-02-11 The United States Of America As Represented By The Administrator Of The National Aeronautics And Space Administration Task space angular velocity blending for real-time trajectory generation
US5528955A (en) * 1994-09-08 1996-06-25 Hannaford; Blake Five axis direct-drive mini-robot having fifth actuator located at non-adjacent joint

Also Published As

Publication number Publication date
JPH0947987A (ja) 1997-02-18
US5867631A (en) 1999-02-02

Similar Documents

Publication Publication Date Title
Wu et al. Finding the kinematic base frame of a robot by hand-eye calibration using 3D position data
Driels et al. Full-pose calibration of a robot manipulator using a coordinate-measuring machine
US8140189B2 (en) Apparatus and method for computing operational-space physical quantity
Rocha et al. A comparison between the Denavit–Hartenberg and the screw-based methods used in kinematic modeling of robot manipulators
JP3331100B2 (ja) マニピュレータシミュレート装置
Bajd et al. Introduction to robotics
Qassem et al. Modeling and Simulation of 5 DOF educational robot arm
US20120303318A1 (en) Kinematic Predictor for Articulated Mechanisms
Lin et al. Analytical solution for inverse kinematics using dual quaternions
Gabiccini et al. Grasp analysis tools for synergistic underactuated robotic hands
Singh et al. Kinematic modeling of robotic manipulators
Lambert et al. Design, modeling, and implementation of a 7-dof cable-driven haptic device with a configurable cable platform
Gu A journey from robot to digital human: mathematical principles and applications with MATLAB programming
Nabavi et al. A homogeneous payload specific performance index for robot manipulators based on the kinetic energy
Wiese et al. Kinematic modeling of a soft pneumatic actuator using cubic hermite splines
Martı´ nez et al. Forward and inverse acceleration analyses of in-parallel manipulators
Duindam et al. Singularity-free dynamic equations of open-chain mechanisms with general holonomic and nonholonomic joints
González-Palacios et al. SnAM: a simulation software on serial manipulators
Ahmed et al. Inverse kinematic solution of 6-DOF robot-arm based on dual quaternions and axis invariant methods
Pozna et al. The inverse kinematics problem, a heuristical approach
Stigger et al. Analysis of a 3-RUU parallel manipulator using algebraic constraints
Lin et al. Virtual simulation and experimental verification for 3D-printed robot manipulators
Shi et al. Formal analysis of the kinematic Jacobian in screw theory
Moulianitis et al. A comparative study of three manipulator performance measures
Song et al. Efficient formulation approach for the forward kinematics of 3-6 parallel mechanisms

Legal Events

Date Code Title Description
A01 Written decision to grant a patent or to grant a registration (utility model)

Free format text: JAPANESE INTERMEDIATE CODE: A01

Effective date: 20020709

LAPS Cancellation because of no payment of annual fees