JPH10187657A - リンク機構座標変換方式 - Google Patents

Abstract

(57)【要約】 【課題】分岐を含む複数のリンク機構から構成される機
械系において、リンクの分岐を許容し効率的に運動学解
析可能なパラメータ設定方式を提供する。 【解決手段】ｉ番目のリンク基準原点からi+1番目のリ
ンク基準原点までの相対位置関係をａ、α、ｄ、θ及び
ｂ、βの計６つのパラメータで記述し、ｂとβが零の
時、ｉ番目のリンク基準原点座標系でリンクのＸ軸並進
方向ａ、Ｘ軸回りの回転方向にαの変位を与え、変位後
座標系でＺ軸方向並進ｄ、Ｚ軸回りの回転θを与えてリ
ンク座標系間変位を求め、ｂ又はβの一方でも非零の
時、ｉ番目のリンク基準原点座標系でＸ軸の並進方向に
ａ、Ｙ軸の並進方向にｂ、Ｚ軸の並進方向にｄの位置の
変位を与えＺ軸回りの回転方向にα、回転後の座標系で
Ｙ軸回りの回転方向にβ、回転後座標系でＺ軸回りの回
転θを与え姿勢変位を計算しリンク座標系間の変位を求
める。

Description

【発明の詳細な説明】

【０００１】

【発明の属する技術分野】本発明は、リンク機構座標変
換方式に関し、特に、産業用ロボット、輸送機械など分
岐を含む複数のリンク機構から構成される機械系におい
て、運動シミュレーションによる解析、設計あるいは実
時間処理によるアニメーション作成、又はこれら機械の
制御に用いるコントローラ等の構成上必要となるリンク
間の相対位置姿勢を記述するパラメータの設定方式に関
する。

【０００２】

【従来の技術】一般的に、２つの座標系間の相対位置姿
勢を与えるパラメータとして、並進のｘ、ｙ、ｚ、回転
のロール角、ピッチ角、ヨー角の６つのパラメータや、
回転部分をオイラー角の３つのパラメータとしたもの等
がある。これらのパラメータを用いれば任意の６自由度
の相対位置姿勢が記述できるが、相対位置姿勢を示す行
列（「Ａマトリックス」という）を求めるための演算処
理は、三角関数の呼び出しや乗算回数が多く、通常の関
節部によく見られる１自由度（回転、並進）駆動の場合
効率的でないという問題がある。

【０００３】そこで、従来、シリアルに接続されたロボ
ットアーム等のリンク構造の機構については、「ＤＨパ
ラメータ」と呼ばれるリンク間の接続関係の記述方法が
一般に用いられてきた。このＤＨ記法では、シリアルに
接続された第ｎリンクに貼り付いた第ｎリンクの基準座
標原点を第ｎ＋１リンクの回転軸原点と一致させ、関節
の駆動をＺ軸方向の回転、並進で表現できるよう４つの
パラメータで基準点間の相対変位を設定するものであ
る。例えば（吉川恒夫訳、“ロボットマニピュレー
タ”、コロナ社、昭和５９年９月１０日刊）等の文献が
参照される。

【０００４】この設定方法は、ｉ番目のリンク基準原点
からｉ＋１番目のリンク基準原点まで相対位置接続関係
を記述するパラメータとして、ｉ番目のリンク基準原点
座標系においてリンクのＸ軸方向の並進方向ａ、Ｘ軸回
りの回転方向にα、変位後の座標系において、Ｚ軸方向
の並進ｄ、Ｚ軸回りの回転θの４つのパラメータを与
え、この順に並進及び回転の変位を与えて、ｉ＋１番目
のリンク基準原点の相対位置姿勢を記述するものであ
る。

【０００５】この設定方法は、前述した６つのパラメー
タを与える方法に比べ、三角関数の呼び出し回数や乗算
回数、記述パラメータの数を少なくできる点が特徴であ
る。

【０００６】さらに以上のＤＨパラメータの派生とし
て、ＭＤＨパラメータと呼ばれるパラメータ設定方法が
存在する。

【０００７】これは、ｉ−１番目のリンクとｉ番目のリ
ンクの関節部を、ｉ番目の基準座標系とし、この座標系
から次のｉ＋１リンクと構成する関節部におけるｉ＋１
リンクに張り付いたｉ＋１リンクの座標原点までの変換
を指定するパラメータである。座標変換の計算は、ｉ番
目のリンク基準原点座標系において、リンクのＸ軸方向
の並進方向ａ、Ｘ軸回りの回転方向にα、変位後の座標
系において、変位後の座標系において、Ｚ軸方向の並進
ｄ、Ｚ軸回りの回転θの４つのパラメータを与え、この
順に並進及び回転の変位を与えてｉ＋１番目のリンク基
準原点の相対位置姿勢を求める。

【０００８】

【発明が解決しようとする課題】運動シミュレーション
をアニメーションや運動制御コントローラ等に用いるた
めには、処理の高速化や、メモリ節約するため演算量の
削減は重要である。

【０００９】しかしながら、上記した従来の方法におい
て、ロール角、ピッチ角、ヨー角やオイラー角等、合計
６つのパラメータを用いる方法では、汎用性は高いが演
算処理の負荷が大きく、リンクの数が増えていくに伴い
ネックになっていくという問題点を有している。

【００１０】またＤＨパラメータ、ＭＤＨパラメータで
は、シリアルに接続されたリンク構造の機械について
は、効率的な計算がされるが、この分岐リンクを持つ機
構の構造によっては、これらのパラメータのみでは、相
対位置姿勢を指定できない場合が存在するという問題を
抱えていた。確かに、仮想的なリンクを考えれば指定で
きなくはないが、リンク数が増えるため演算効率が良い
とはいえず、仮想リンクの扱いが煩雑になるなど問題が
多い。

【００１１】したがって、本発明は、上記問題点に鑑み
てなされたものであって、その目的は、リンクの分岐を
許容し、効率的に運動学的に解析可能なリンク機構座標
変換方式を提供することにある。

【００１２】

【課題を解決するための手段】前記目的を達成するた
め、本願第１発明（請求項１）は、複数のリンクから構
成され、リンクの分岐を含むマルチリンク機構におい
て、ｉ番目のリンク基準原点からｉ＋１番目のリンク基
準原点までの相対位置関係をリンクのＸ軸方向並進ａ、
Ｘ軸回りの回転α、Ｚ軸方向並進ｄ、Ｚ軸回りの回転θ
の４つのパラメータと、更にＹ軸方向並進変位ｂ、及び
Ｙ軸回転変位βの計６つのパラメータを用いて記述し、
ｂ及びβが共にゼロのときは、ｉ番目のリンク基準原点
座標系においてリンクのＸ軸方向の並進方向にａ、Ｘ軸
回りの回転方向にαの変位を与え、変位後の座標系にお
いて、Ｚ軸方向の並進ｄ、Ｚ軸回りの回転θを与えるこ
とによりリンク座標系間の変位を求め、ｂまたはβのい
ずれか一方でもゼロでない場合には、ｉ番目のリンク基
準原点座標系において、Ｘ軸の並進方向にａ、Ｙ軸の並
進方向にｂ、Ｚ軸の並進方向にｄの位置の変位を与え、
その後、Ｚ軸回りの回転方向にα、回転後の座標系にお
いて、Ｙ軸回りの回転方向にβ、さらに回転後の座標系
においてＺ軸回りの回転θを与えて姿勢変位を計算する
ことで、リンク座標系間の変位を求める、ことを特徴と
する。

【００１３】また、本願第２発明（請求項２）は、上述
したリンクの分岐を含むマルチリンク機構において、リ
ンク同士が接合する関節部が位置または姿勢方向の１自
由度のみあるいは、同一の座標軸に関して位置方向及び
姿勢方向の２自由度の駆動が許容される場合において、
上述の６つのパラメータに加えてＺ軸方向の並進方向の
初期オフセットｄｐ、Ｚ軸回りの回転の初期オフセット
θ０の２つのパラメータを追加し、請求項２で述べた座
標変換処理手順の中でｂ及びβが共にゼロの場合は、上
述のパラメータの一部についてＺ軸方向の並進をｄ＋ｄ
０、Ｚ軸回りの回転をθ＋θ０としてｉ＋１番目の座標
原点の相対位置姿勢を求め、ｂとβが共にゼロでない場
合には、Ｘ軸方向の並進をａ＋ｄｐ・ｃｏｓα・ｓｉｎ
β、Ｙ軸方向の並進をｂ＋ｄｐ・ｓｉｎα・ｓｉｎβ、
Ｚ軸方向の並進をｄ＋ｄｐ・ｃｏｓβとし、Ｚ軸回りの
回転をθ＋θ０としてｉ＋１番目の座標原点の相対位置
姿勢を求めることを特徴とする。

【００１４】さらに、本願第３発明（請求項３）は、本
願第１発明の６つのパラメータ、又は本願第２発明の８
つのパラメータに加え、リンクの接続関係を示すパラメ
ータとして、該当リンクに接続する元の親リンク名と、
親リンクから分岐する他のリンクである兄弟リンク名の
１つと、該当リンクより接続する子リンク名を有し、順
運動学の計算方法として、リンクが分岐する親リンクの
ワールド座標系での位置姿勢を用いて該当リンクのワー
ルド座標系での位置姿勢を計算し、次に計算対象とする
リンクの決定方法として、まず子リンクよりも兄弟リン
クの計算を優先し、兄弟リンクの中で子リンクを有する
ものについては、子リンクについて計算し、この子リン
ク以下にも同様の階層が存在する場合には、子リンクよ
りも兄弟リンクの計算を優先した順に計算を行い、下の
階層まで含めた全ての兄弟リンク座標計算が完了した
後、該当リンクの子リンクついての計算を行い、以下該
当リンクの場合と同様な順番で全ての階層の子リンクの
計算をすることを特徴とする。

【００１５】

【発明の実施の形態】本発明の実施の形態について以下
に説明する。本発明は、その好ましい実施の形態におい
て、複数のリンクから構成され、リンクの分岐を含むマ
ルチリンク機構において、ｉ番目のリンク基準原点から
ｉ＋１番目のリンク基準原点までの相対位置関係をＸ軸
方向並進変位ａ、Ｘ軸回りの回転変位α、変位後の座標
率において、Ｚ軸方向並進変位ｄ、Ｚ軸回りの回転変位
θの４つのパラメータ、及びＹ軸方向並進変位ｂ、Ｙ軸
回転変位βの計６つのパラメータを用いて記述し、座標
変換手順において、ｂ及びβが共にゼロのときは、ｉ番
目のリンク基準原点座標系においてリンクのＸ軸方向の
並進方向にａ、Ｘ軸回りの回転方向にαの変位を与え、
変位後の座標系において、Ｚ軸方向の並進ｄ、Ｚ軸回り
の回転θを与えることによりリンク座標系間の変位を求
める。

【００１６】ｂまたはβのいずれか一方でもゼロでない
場合には、ｉ番目のリンク基準原点座標系において、Ｘ
軸の並進方向にａ、Ｙ軸の並進方向にｂ、Ｚ軸の並進方
向にｄの位置の変位を与え、その後、Ｚ軸回りの回転方
向にα、回転後の座標系において、Ｙ軸回りの回転方向
にβ、さらに回転後の座標系においてＺ軸回りの回転θ
を与えて姿勢変位を計算することで、リンク座標系間の
変位を求める。

【００１７】リンク同士が接合する関節部が、位置また
は姿勢方向の１自由度のみ、あるいは、同一の座標軸に
関して位置方向及び姿勢方向の２自由度の駆動が許容さ
れる場合、前記６つのパラメータに加えて、Ｚ軸方向の
並進方向の初期オフセットｄｐ、Ｚ軸回りの回転の初期
オフセットθ０の２つのパラメータを追加し、ｂ及びβ
が共にゼロの場合は、前記パラメータの一部についてＺ
軸方向の並進をｄ＋ｄ０、Ｚ軸回りの回転をθ＋θ０と
して、ｉ＋１番目の座標原点の相対位置姿勢を求め、ｂ
とβが共にゼロでない場合には、Ｘ軸方向の並進を、ａ
＋ｄｐ・ｃｏｓα・ｓｉｎβ、Ｙ軸方向の並進を、ｂ＋
ｄｐ・ｓｉｎα・ｓｉｎβ、Ｚ軸方向の並進を、ｄ＋ｄ
ｐ・ｃｏｓβとし、Ｚ軸回りの回転をθ＋θ０として、
（Ｔｒａｎｓ（ｚ，ｄ＋ｄ０）＊（Ｒｏｔ（ｚ，θ＋θ
０））、ｉ＋１番目の座標原点の相対位置姿勢を求め
る。

【００１８】本発明の実施の形態によれば、従来のＭＤ
Ｈパラメータである４つのパラメータに、ｂ、βの２つ
のパラメータを導入することにより、あらゆるリンクの
分岐の記述に対応し、かつ効率的な計算を可能とする。

【００１９】シリアル接続されたリンク機構に適用され
る従来のＭＤＨパラメータ等では、リンクの分岐に対応
できず、また位置姿勢の全てを指定する従来の方式で
は、シリアルに接続されたリンクの部分が多い機構にと
って冗長な計算が多くなるという問題があったが、本発
明の実施の形態は、従来のＭＤＨパラメータである４つ
のパラメータに、ｂ、βの２つのパラメータを導入する
ことにより、あらゆるリンクの分岐の記述に対応し、か
つ効率的な計算を可能としている。

【００２０】また、本発明の実施の形態においては、前
記６つのパラメータ、又は８つのパラメータに加え、リ
ンクの接続関係を示すパラメータとして、該当リンクに
接続する元の親リンク名（図３、図４、図５参照）と、
親リンクから分岐する他のリンクである兄弟リンク名の
１つと、該当リンクより接続する子リンク名（図３、図
４、図５参照）を有し 運動学の計算方法として、リン
クが分岐する親リンクのワールド座標系での位置姿勢を
用いて該当リンクのワールド座標系での位置姿勢を計算
し、次に計算対象とするリンクの決定方法として、まず
子リンクよりも兄弟リンクの計算を優先し、兄弟リンク
の中で子リンクを有するものについては、子リンクにつ
いて計算し、この子リンク以下にも同様の階層が存在す
る場合には、子リンクよりも兄弟リンクの計算を優先し
た順に計算を行い、下の階層まで含めた全ての兄弟リン
ク座標計算が完了した後、該当リンクの子リンクついて
の計算を行い、以下該当リンクの場合と同様な順番で全
ての階層の子リンクの計算をする。

【００２１】また本発明の実施の形態においては、リン
クが分岐する場合、ｂ、βに値を設定することで、あら
ゆる分岐に対応可能となり、親リンク名、兄弟リンク
名、子リンク名の追加により、任意のトリー構造の全リ
ンクの運動学計算を可能としている。

【００２２】本発明の実施の形態においては、シリアル
に接続するリンク部では、ｂ、βの２つのパラメータが
ともにゼロとなるため、従来のＭＤＨパラメータの４つ
のみ用いて計算を行うことで分岐リンクで６つのパラメ
ータを用いる場合に較べ、不必要な三角関数の呼び出し
や演算を抑える、ことができる。

【００２３】１自由度の回転及び直動の場合には、オフ
セットに関するパラメータｄｐＩ及びθ０ｉの負荷によ
って、関節変位原点の変更が容易にしている。以上述べ
た処理は、リアルタイム性が問題となるコントローラや
シミュレータ等への応用に有効であり、リンク構造が複
雑化する程計算時間が節約できるという効果が大きく発
揮される。

【００２４】また、本発明の実施の形態に係るパラメー
タの設定方法は、１自由度の回転及び直動の場合のみな
らず、無拘束から６自由度拘束まで広くリンクの接続状
態に応じて適用できるものであり、拘束状態に応じた計
算負荷削減が可能となる汎用性がある。

【００２５】

【実施例】上記した本発明の実施の形態について更に詳
細に説明するために、本発明の実施例について図面を参
照して以下に説明する。

【００２６】図１は、本発明の一実施例におけるリンク
パラメータの設定方法について説明するための図であ
る。図１には、リンク機構の一例として、シリアルにリ
ンクがつながった機構を有する作業機械の構成例が示さ
れている。

【００２７】図１を参照すると、この機械は、７つのリ
ンクから構成され、各リンクの基準座標系を矢印で示し
ている。これらのシリアルに接続されたリンクをここで
は「シリアルリンク」と呼ぶことにする。各シリアルリ
ンク（Ｌｉｎｋ１〜Ｌｉｎｋ７）は、各基準座標系のＺ
軸回りの回転自由度を有する。

【００２８】図２は、この機械についてパラメータを用
いてリンクパラメータを設定した例を示す。図２に示す
ように、図１に示す機械は、シリアルに接続されたリン
ク機構であるため、すべてのリンクにおいて、ｂｉ＝０
かつβｉ＝０となっており、従来のＭＤＨパラメータ４
つのみで接続関係を記述できることも示している。な
お、各リンクごとに同種のパラメータが存在するため、
本実施例では、添え字ｉを用いてリンクごとのパラメー
タを区別することにする。

【００２９】次に、分岐リンクのある場合について考え
る。分岐リンクの構造として、側面図を図３に、正面図
を図４に示す。

【００３０】この分岐リンクは、第７リンク（Ｌink
７）７を分岐元の親リンクとして、７つのリンクに分岐
している。分岐リンクもシリアルリンクと同様に各基準
座標系のＺ軸回りに回転駆動できるようになっている。
第１フィンガー（Ｆinger１）８から第４フィンガー
（Ｆinger４）１１までの４つのリンクは、回転駆動に
より物体を把持する機能を持つ。

【００３１】第１ロッド（Ｒod１）１２、第１ローラー
（Ｒoller１）１３、第２ロッド（Ｒod２）１４、第２
ローラー（Ｒoller２）１５の２組の分岐リンクは、第
１ロッド１２と第２ロッド１４にてリンク間にある物体
を把持、第１ローラー１３、第２ローラー１５によって
把持物体を平行移動する機能を持つ。カッター（Ｃutte
r）１６は、把持された物体を切断することができる。

【００３２】このようなシリアルリンクの先端に物体把
持、切断機能を持った手先効果器がついた機構を分岐型
リンク機構の例とし、本発明の一実施例に係るパラメー
タを用いてリンク接続関係を記述することができる。

【００３３】図５に、パラメータの設定の一例を示す。
図５に示す分岐リンクの中で、例えば、第１フィンガー
（Ｆinger１）８のように、基準となる第７リンク（Ｌi
nk７）７の原点に対してｘ、ｙ、ｚの３つの自由度のオ
フセットを持つ。

【００３４】シリアルリンクの場合では、第７リンク
（Ｌink７）７の基準座標系の設定を工夫することによ
り２つの自由度のオフセットに減らすことができる。

【００３５】しかし、分岐リンクであるため、第１フィ
ンガー（Ｆinger１）８のオフセット指定を減らすよう
に、ｂｓ／２だけＺ軸方向に、第７リンク（Ｌink７）
７の基準座標系を移動させると、第２フィンガー（Ｆin
ger２）９については、やはり３つの自由度のオフセッ
トを指定しなければならない。

【００３６】すなわち従来のＤＨパラメータの設定方法
ではこのような関節配置は指定できない。

【００３７】本発明の一実施例に係るパラメータ設定方
法を用いれば、従来ＤＨパラメータで設定できる部分は
これを活用し、従来のＤＨパラメータでは設定できない
場合についてのみ拡張することができる。

【００３８】次に本発明の一実施例で説明したパラメー
タ設定方法を用いて座標変換（運動学計算）を行う処理
手順について説明する。図６は、その処理フローを説明
するための流れ図である。

【００３９】まず、ステップ２１において、ｉは計算対
象のインデックスを表し、ｊは対象リンクの親リンクの
インデックスを表す。図５に示した表のパラメータを読
み、計算対象のリンクのｉを設定する。

【００４０】ステップ２２で、ｂ_i、β_iがゼロであるか
を判定し、ｂ_i、及びβ_iが共にゼロの場合にはステップ
２３、ｂ_i又はβ_iがゼロでない場合にはステップ２４に
おいて、リンク基準座標系間の相対位置姿勢を表す４×
４のＡｉマトリックスを算出する。ステップ２３、２４
の処理で用いる計算式の内容は以下の通りである。

【００４１】

【数１】

【００４２】本発明の一実施例で示したリンク機構は、
核リンクの接合する関節部は１自由度の回転自由度を持
ち、このパラメータの設定によりＺ軸回りに回転するよ
うに設定される。

【００４３】従って、θｉ＋θ０ｉとすることで、各関
節の駆動による回転基準の位置（関節駆動変位ゼロの位
置）を容易に変更できるようにしている。

【００４４】直動関節の場合には、リンク座標系のＺ軸
方向に直動するように設定するため、リンクＸ軸方向の
並進を、 ａｉ＋ｄｐｉ・ｃｏｓα・ｓｉｎβ、 Ｙ軸方向の並進を、 ｂｉ＋ｄｐｉ・ｓｉｎα・ｓｉｎβ、 Ｚ軸方向の並進を、 ｄｉ＋ｄｐｉ・ｃｏｓβ、 とすれば、Ｚ軸方向の位置のオフセットｄｐｉが計算に
反映できる。

【００４５】ステップ２５では、各リンク基準座標のワ
ールド座標系で示した位置はＴｉマトリックス（４×４
マトリックス）として算出する。なお、Ａｉ、Ｔｉの初
期値は４×４単位行列であるものとする。

【００４６】ステップ２６では、リンクの分岐があるか
どうかを、図５のパラメータ設定の兄弟リンク名によっ
て調べる。

【００４７】兄弟リンク名がＮＵＬＬ（ヌル）であれば
分岐なし、名前が設定されていれば分岐があることを示
している。

【００４８】ステップ２６の判定で、分岐がある場合、
ステップ３０にて、現在のリンクのインデックスｉを親
のインデックスｊとして保存する。

【００４９】もし、すでにゼロでない値が設定されてい
れば、その値はスタックバッファに格納する。

【００５０】ステップ３１で、次に計算すべきリンクの
インデックスをｉに設定し、ステップ２２に戻る。

【００５１】一方、ステップ２６の判定で、リンクの分
岐がない場合、ステップ２７において、シリアルに接続
されたリンクであるかを子リンク名より判断する。

【００５２】子リンク名がＮＵＬＬであればシリアル接
続リンクがないものとし、その他の名前が設定されてい
ればリンクの接続があることを示している。

【００５３】子リンクがある場合、ステップ３０へ分岐
する。

【００５４】シリアル接続リンクがない場合は、スタッ
クより親リンクのインデックスを取り出し、ｊに設定す
る。ｉ＝ｊとして、ステップ２６に戻る。但し、スタッ
クに親リンクのインデックスがないとき、演算を中止
し、処理を終了する。以上の処理により、全てのリンク
についてＴｉ、Ａｉが算出されたことになる。このＴ
ｉ、Ａｉを用いれば、任意のリンク原点の位置姿勢が明
らかとなる。

【００５５】次に本発明の応用例として、運動シミュレ
ータへの適用について説明する。図７に処理の概要を流
れ図で示す。

【００５６】図７を参照すると、ステップ４１では、各
リンク関節部の一般化変位Δｑｉを入力する。

【００５７】ステップ４２では、もし該当関節が回転型
の場合は、 θｉ＝θｉ＋Δｑｉ …(9) 直動型の場合は、ｂ＝β＝０のとき、 ｄｉ＝ｄｉ＋Δｑｉ …(10) それ以外のとき、 ａｉ＝ａｉ＋Δｑｉ・ｃｏｓα・ｓｉｎβ …(11) ｂｉ＝ｂｉ＋Δｑｉ・ｓｉｎα・ｓｉｎβ …(12) ｄｉ＝ｄｉ＋Δｑｉ・ｃｏｓβ …(13) と計算し、パラメータを設定しなおす。

【００５８】ステップ４３は、図６を参照して説明した
運動学計算処理である。ステップ４３の処理の結果、Ｔ
ｉ、Ａｉが算出されるので、これと各リンクの幾何形状
モデルを用いて３次元物体の形状の特徴点の座標が求め
ることができる。

【００５９】ステップ４４では、各形状モデルの特徴点
の座標を結び、ステップ４５で、これを透視変換して２
次元平面上へ投影した結果をＣＲＴ等へ表示させること
により、マルチリンク機構の各リンク原点よりΔｑｉだ
け変位した状態のワイヤフレーム画像が生成される。

【００６０】またコンピュータグラフィックスの技法を
用いて、陰影処理やテキスチャを張り付けることを行え
ばよりリアルな画像を得ることの可能である。この処理
を繰り返し行うことにより、リンク機構の運動結果がア
ニメーションとして表現できる。

【００６１】

【発明の効果】以上説明したように、本発明によれば、
従来のＭＤＨパラメータである４つのパラメータに、
ｂ、βの２つのパラメータを導入することにより、あら
ゆるリンクの分岐の記述に対応可能とし、かつ効率的な
計算を可能とするという効果を奏する。

【００６２】すなわち、シリアル接続されたリンク機構
に適用される従来のＭＤＨパラメータ等では、リンクの
分岐に対応できず、また位置姿勢の全てを指定する従来
の方式では、シリアルに接続されたリンクの部分が多い
機構にとって冗長な計算が多くなるという問題があった
が、本発明は、従来のＭＤＨパラメータである４つのパ
ラメータに、ｂ、βの２つのパラメータを導入すること
により、あらゆるリンクの分岐の記述に対応可能とし、
かつ効率的な計算を可能としている。

【００６３】また本発明は、リンクが分岐する場合、
ｂ、βに値を設定することで、あらゆる分岐に対応可能
となり、親リンク名、兄弟リンク名、子リンク名の追加
により、任意のトリー構造の全リンクの運動学計算を可
能としている。

【００６４】そして、本発明においては、シリアルに接
続するリンク部では、ｂ、βの２つのパラメータがとも
にゼロとなるため、従来のＭＤＨパラメータの４つのみ
用いて計算を行うことで分岐リンクで６つのパラメータ
を用いる場合に較べ、不必要な三角関数の呼び出しや演
算を抑える、ことができる。

【００６５】１自由度の回転及び直動の場合には、オフ
セットに関するパラメータｄｐＩ及びθ０ｉの負荷によ
って、関節変位原点の変更が容易にしている。以上説明
した本発明の処理方式は、リアルタイム性が問題となる
コントローラやシミュレータ等への応用に有効であり、
リンク構造が複雑化する程計算時間が節約できるという
効果が大きく発揮される。

【００６６】また、本発明におけるパラメータの設定方
式は、１自由度の回転及び直動の場合のみならず、無拘
束から６自由度拘束まで広くリンクの接続状態に応じて
適用できるものであり、拘束状態に応じた計算負荷削減
を可能とし、汎用性があるという利点を有している。

【図面の簡単な説明】

【図１】本発明の一実施例としてシリアルに接合された
リンク機構の一例を示す図である。

【図２】図１に示した機構についてＭＤＨパラメータの
設定例を示す図である。

【図３】図１に示した機構にとりつける分岐リンクの構
造（側面）を示す図である。

【図４】図１に示した機構にとりつける分岐リンクの構
造（正面）を示す図である。

【図５】本発明の一実施例を説明するための図であり、
図１に示したシリアルリンク機構に、分岐リンクを取り
付けた機構に、本発明を適用した一例を示す図である。

【図６】本発明の一実施例の処理フローを説明するため
の流れ図である。

【図７】本発明の別の実施例としてグラフィックシミュ
レータに適用した場合の処理フローを示す図である。

【符号の説明】

１ 第１リンク（Ｌink１） ２ 第２リンク（Ｌink２） ３ 第３リンク（Ｌink３） ４ 第４リンク（Ｌink４） ５ 第５リンク（Ｌink５） ６ 第６リンク（Ｌink６） ７ 第７リンク（Ｌink７） ８ 第１フィンガー（Ｆinger１） ９ 第２フィンガー（Ｆinger２） １０ 第３フィンガー（Ｆinger３） １１ 第４フィンガー（Ｆinger４） １２ 第１ロッド（Ｒod１） １３ 第１ローラー（Ｒoller１） １４ 第２ロッド（Ｒod２） １５ 第２ローラー（Ｒoller２） １６ カッター（Ｃutter）

Claims (3)

【特許請求の範囲】
1. 【請求項１】複数のリンクから構成され、リンクの分岐
   を含むマルチリンク機構において、 ｉ番目のリンク基準原点からｉ＋１番目のリンク基準原
   点までの相対位置関係をリンクのＸ軸方向並進ａ、Ｘ軸
   回りの回転α、Ｚ軸方向並進ｄ、Ｚ軸回りの回転θの４
   つのパラメータと、更にＹ軸方向並進変位ｂ、及びＹ軸
   回転変位βの計６つのパラメータを用いて記述し、 ｂ及びβが共にゼロのときは、ｉ番目のリンク基準原点
   座標系においてリンクのＸ軸方向の並進方向にａ、Ｘ軸
   回りの回転方向にαの変位を与え、変位後の座標系にお
   いて、Ｚ軸方向の並進ｄ、Ｚ軸回りの回転θを与えるこ
   とによりリンク座標系間の変位を求め、 ｂまたはβのいずれか一方でもゼロでない場合には、ｉ
   番目のリンク基準原点座標系において、Ｘ軸の並進方向
   にａ、Ｙ軸の並進方向にｂ、Ｚ軸の並進方向にｄの位置
   の変位を与え、その後、Ｚ軸回りの回転方向にα、回転
   後の座標系において、Ｙ軸回りの回転方向にβ、さらに
   回転後の座標系においてＺ軸回りの回転θを与えて姿勢
   変位を計算することで、リンク座標系間の変位を求め
   る、ことを特徴とするリンク機構座標変換方式。
2. 【請求項２】リンク同士が接合する関節部が、位置また
   は姿勢方向の１自由度のみ、あるいは、同一の座標軸に
   関して位置方向及び姿勢方向の２自由度の駆動が許容さ
   れる場合、 前記６つのパラメータに加えて、Ｚ軸方向の並進方向の
   初期オフセットｄｐ、Ｚ軸回りの回転の初期オフセット
   θ０の２つのパラメータを追加し、 座標変換処理において、ｂ及びβが共にゼロの場合は、
   前記パラメータの一部についてＺ軸方向の並進をｄ＋ｄ
   ０、Ｚ軸回りの回転をθ＋θ０として、ｉ＋１番目の座
   標原点の相対位置姿勢を求め、 ｂとβが共にゼロでない場合には、 Ｘ軸方向の並進を、ａ＋ｄｐ・ｃｏｓα・ｓｉｎβ、 Ｙ軸方向の並進を、ｂ＋ｄｐ・ｓｉｎα・ｓｉｎβ、 Ｚ軸方向の並進を、ｄ＋ｄｐ・ｃｏｓβとし、 Ｚ軸回りの回転をθ＋θ０としてｉ＋１番目の座標原点
   の相対位置姿勢を求めることを特徴とする、請求項１記
   載のリンク機構座標変換方式。
3. 【請求項３】請求項１記載の前記６つのパラメータ、又
   は請求項２記載の８つのパラメータに加え、リンクの接
   続関係を示すパラメータとして、該当リンクに接続する
   元の親リンク名と、親リンクから分岐する他のリンクで
   ある兄弟リンク名の１つと、該当リンクより接続する子
   リンク名を有し、 順運動学の計算方法として、 リンクが分岐する親リンクのワールド座標系での位置姿
   勢を用いて該当リンクのワールド座標系での位置姿勢を
   計算し、 次に計算対象とするリンクの決定方法として、まず子リ
   ンクよりも兄弟リンクの計算を優先し、兄弟リンクの中
   で子リンクを有するものについては、子リンクについて
   計算し、 この子リンク以下にも同様の階層が存在する場合には、
   子リンクよりも兄弟リンクの計算を優先した順に計算を
   行い、 下の階層まで含めた全ての兄弟リンク座標計算が完了し
   た後、該当リンクの子リンクついての計算を行い、以下
   該当リンクの場合と同様な順番で全ての階層の子リンク
   の計算をする、ことを特徴とするリンク機構座標変換方
   式。