JP5349478B2 - 逆運動学 - Google Patents

Description

本発明は、ヤコビ行列式の逆行列に関する。逆運動学（Inverse Kinematics）の問題（ＩＫ問題）が知られている。ＩＫ問題は、ゲームやアニメーションにおけるコンピュータによって生成された関節のあるキャラクタの操作、キーフレーミング、ロボットマニピュレータの制御、コントロールモーメントジャイロ（Control Moment Gyro、以下ＣＭＧとする）機構の制御における特異点回避法、及び、その他の応用例と関連している。

ロボット工学、コンピュータアニメーション、ＣＭＧを用いた宇宙船の制御などにおいて、変数行列は、変数パラメータの関数であり、入力空間と出力空間とを関連づけている。ここで、変数パラメータは、例えば、（ｑ）及び時間、即ち、時間を含む変数である。

（ｘ）は、直交座標系におけるベクトルを表し、先端部の位置と姿勢とを示す。例えば、図１における参照番号１４が先端部を示している。３次元直交座標系において、（ｘ）は、１以上の成分をもち、位置と角回転とを表すことになる。先端部空間のサイズ（次元数）は（ｍ）によって示される。例えば、先端部が３次元における移動のみ可能な場合にはｍ＝３である。

（ｑ）は、関節座標系における姿勢を示す変数である。例えば、図１において、関節１１，１２，１３は（ｑ）によって表される。各関節は１以上の自由度をもつ。（ｑ）における全自由度は、（ｎ）によって示される。例えば、図１における、各関節（１１，１２，１３）は、それぞれ自由度２を有し、全自由度は６（ｎ＝６）である。

従って、マニピュレータ、コンピュータアニメーションにおける物体といった構造物のモデルは、以下のように記述される。
x = f(q) (数1a)

式(数1a)において、f(q)は、関節係数（ｑ）の非線形な関数である。

多くの応用において、一連の所定の先端部の位置（ｘ）に対する関節変数（ｑ）を計算する必要がある。このために、以下のように式(数1a)の逆関数を求めることが必要である。
q = [f(x)]^-1 (数1b)

式(数1b)において、（[f(x)]^-1）は、(q)から（x）への逆写像を表している。

式(数1b)を解析的に求めることは負荷が大きい。式(数1a)を解く数値的な方法では、式(数1a)の両辺を微分する。これにより、運動学的な以下の関係が得られる。
dx = J(q)dq (数1c)

式(数1c)において、（dq）は出力空間を定義しており、(dx)は入力空間を定義している。(J(q))は、パラメータに依存したヤコビ行列であり、（f(q)）を(q)で微分して得られる。
J(q) = diff(f(q))/diff(q) (数1d)

式(数1d)は、(q)による(f(q))の微分を表している。

ロボット工学や、多関節形状のアニメーションにおいて、(x)は、直交座標空間における角変位と直線変位とから構成されるベクトルを定義している。(q)は、関節座標系における配置を示している。(dx)は、直交座標系における速度を示している。(dq)は、関節速度を示している。

コントロールモーメントジャイロ機構を用いる姿勢制御において、(x)は、当業者に知られたＣＭＧクラスタの全角運動量を示している。(q)はジンバル角(gimbal angle)により構成され、(dx)はトルクの成分を示す。(dq)は、当業者に知られたジンバル比（gimbal rate）を示している。

(x)、及び(dx)の次元（次元の数）は、(m)である。

(q)、(dq)の次元は(n)である。

一般的には、(m)は(n)以下である。

冗長構成により、(m)は(n)未満となる。

いくつかの実施の形態においては、(m)と(n)とは任意の大きさを取りうる。

図１に示す構成例は、例えば、３つの関節１１、１２、１３と１つの先端部とを有するロボットマニピュレータに対応する。図１の参照番号１５は、先端部の目標とする軌道を示す。当該軌道は、３次元直交座標空間において定義されている。図１において、参照番号１１，１２，１３は関節構造を示す。関節構造１１，１２，１３は、角度の変化、または、直線的な変化が可能であり、(n)個の自由度を与える。関節構造１１，１２，１３は、参照番号１４が目標軌道１５をたどるようにしている。各関節１１，１２，１３は、それぞれ異なった１以上の自由度を持つことが可能である。

図２は、コンピュータアニメーションの分野などで用いられるグラフィックの構造を示している。図２の構造では、５つの先端部２１１,２１４，２１５，２１３，２１２と、１０個の関節２１，２２，２３，２４，２５，２６，２７，２８，２９，２１０とが例として示されている。図２の構造は説明のためのものであり、本発明に記載のＩＫアルゴリズムは、先端部に(m)個の自由度があり、関節に(n)個の自由度がある構造に対して有効である。ここで、(m)及び(n)は任意に大きな値を取りうる。参照番号２１６，２１７，２１８，２１９，２２０は、先端部がたどるべき軌道を示し、３次元直交座標空間において定義されている。

ロボット工学では、モーター、又は、その他の装置が、線運動量、及び、角運動量を与えることで、(q)の移動及び変化が生じる。

コンピュータグラフィックスでは、アニメーションソフトウェアが図形を再描画することで(q)の移動が生じる。

式(数1c)ではヤコビ行列は(q)に依存するため、センサを用いて(q)を測定したり、あるいは、数学モデルを用いて(q)を導出、または、推測する。

本発明においては、(dxd)は、(dx)の望ましいベクトルを示す。(dxd)と(q)は、数学モデル、あるいは、センサにより与えられるものと仮定する。

一連の軌道(dxd)が与えられたときに、逆運動学の問題は、式(数1c)の逆変換を解くというものになる。即ち、与えられたベクトル(dxd)に対して(dq)をどのように導出するかという問題である。
dq = iJ(q)dxd (数2)

式(数2)において、(iJ(q))は、式(数1)のヤコビ行列の逆行列である。(m)が厳密に(n)より小さい場合、即ち、(m)が(n)と等しくない場合には、(iJ(q))を計算するために擬似逆行列が用いられる。

コンピュータグラフィックス、ロボット工学では、(dxd)は、先端部の所望の速度を表す。

ＣＭＧを用いた姿勢制御では、(dxd)は、所望のトルクの成分を表す。

本発明の課題である逆運動学の問題は、(dx)が所望の目標とする軌道(dxd)を辿るようにどのように関節変数(dq)を選択するかとういこと、即ち、モーターをどのように操作するかを決めること、または、アニメーションアプリケーションにおいてどのように構造物を再描画するかを決めることである。

図３は、逆運動学のブロック、即ち、処理システムである。当該処理システムは、目標とする軌道(dxd)を生成するという条件の下に関節変数(dq)を決める。当該ブロックに (dxd)が入力され、(dq)が当該ブロックより出力される。逆運動学ブロックは、各反復動作において(dx)が(dxd)と同じになるように、与えられた(dxd)に対して(dq)を与える。

(dxd)が与えられ、(J(q))が時間的に変化するパラメータに依存するヤコビ行列であるときに、(dq)について式(数2)を解くことは、コンピュータアニメーションやロボットマニピュレータ、宇宙船の制御、及び、その他の応用において本質的な要素、あるいは、本質的なステップである。

リアルタイムに式(数2)を解くことは、計算するデータの量が多く、負荷のかかるタスクである。

本発明のいくつかの実施の形態における目的は、式(数2)で定義される逆運動学の問題を解くための方法であって、コンピュータの計算上有効な方法を得ることである。

(q)の自由度(n)が多いことにより、式(数2)におけるＩＫ問題をリアルタイムに解くことは、数値的にデータ量の多いタスクである。

関節を有する物体がアニメーションする場合、例えば、細部、及び、自由度が増加することで、運動における視覚的な表現は向上する。しかしながら、かかる向上によって、リアルタイムに計算機を用いて、大きい(n)の値に対する式(数2)におけるＩＫ問題を解く必要が生じる。

式(数1c)、(数2)におけるヤコビ行列(J(q))は、パラメータ(q)に依存することから、時間的に変化する関数である。このため、特異状態と呼ばれる特定の状態において、(J(q))のランクが下がる。これにより、与えられた軌道(dq)に対して、式(数2)の逆変換は任意の大きい値(dq)を導くことになる。かかる点は、逆運動学の問題を計算する上で、もう１つの複雑な状況を生み出している。

従来、行列の操作に基づいてヤコビ行列の逆行列である(iJ(q))を求めるＩＫ方法が知られている。かかる行列の操作は、計算におけるデータ量が多くなる処理を有し、並列処理のコンピュータアーキテクチャ上で走らせることが難しい。

従来、ＤＭＳ（減衰最小二乗法）アルゴリズム（damped least squares algorithm）が、特異性を解消するＳＲアルゴリズム（singularity robust algorithm）として知られており、式(数2)の問題を解くために用いられている（非特許文献１、２参照）。
iJ(q) = Jt(q) [J(q)Jt(q) + kI]^-1 (数3a)

ここで、(Jt(q))は、ヤコビ行列の転置である。式(数3a)によって(iJ(q))を計算することで、与えられたベクトル(dxd)に対して式(数2)から(dq)を導くことが出来る。すなわち、
dq = Jt(q) [J(q)Jt(q) + kI]^-1 dxd (数3b)

式(3b)において、(Jt(q))は、式(数1d)におけるヤコビ行列の転置であり、(I)は恒等行列である。また、(k)は、いわゆる減衰係数であり、値は適宜調整される。(^-1)は、逆作用素である。(k=0)の場合には、式(数3)は擬似逆行列法に単純化される。
iJ(q) = Jt(q) [J(q)Jt(q)]^-1 (数4a)

式(数4a)で定義される(iJ(q))を計算し、式(数2)を用いることにより、与えられたベクトル(dxd)に対して(dq)が以下のように得られる。
dq = Jt(q) [J(q)Jt(q)]^-1 dxd (数4b)

特異状態では、ヤコビ行列のランクが低下し、結果として、式(数4a)の逆行列が存在しなくなり、式(数4a)を用いて逆行列を生成することができなくなる。

さらに、(J(q))が特異状態近傍のときには、過剰に大きな値(dq)が式(数4b)に基づいた解として得られる。解の厳密さと、解を求めることの実行可能性とは二律背反であり、減衰係数(k)は、そのいずれかを決めている。(k=0)の場合には、式(3a)は、式(数4a)に単純化される。特異状態から離れている状態では、(k=0)に設定される。特異状態近傍では、(k>0)に設定する。このように、式(数3a)、(数3b)のＩＫ法は、(k)を導入することが必要なため複雑になる。減衰係数(k)を導入することにより、さらなる計算と処理能力が必要になる。

非特許文献３、４には、フィードバックループを有するロボットマニピュレータの制御のためのアルゴリズムが提案されている。当該フィードバックループでは、ヤコビ行列の転置が用いられている。

しかしながら、本発明で提案される方法とは異なり、これらのアルゴリズムは特異状態を回避するものではなく、特異状態の場合の解を与えることができない。

ワイ． ナカムラ、エイチ． ハナフサ、「ロボットマニピュレータの制御における特異性に耐性のある逆運動学の解」、ジャーナルオブダイナミックシステムズ、１０８巻、１９８６年９月（Y. Nakamura and H. Hanafusa, "Inverse kinematic solutions with singularity robustness for robot manipulator control", Journal of Dynamic systems, Measurements and Control, Vol. 108, Sep. 1986） シー． ダブリュ． ワンプラー、エル． ジェイ． レイファー、「マニピュレータの分解速度制御、分解加速度制御への減衰最小二乗法の適用」、ジャーナルオブダイナミックシステムズメジャーメントアンドコントロール、１１０（１９８８）、３１−３８ページ（C. W. Wampler and L. J. Leifer, "Applications of damped least-squares methods to resolved-rate and resolved-acceleration control of manipulators", Journal of Dynamic Systems, Measurement, and Control, 110 (1988), pp. 31-38） ダブリュ． エー． ウォロビッチ、エイチ． エリオット、「逆運動学の計算上の技術」、第２３回デシジョンアンドコントロール会議会報、１９８４年（W.A. Wolovich and H. Elliott in "A computational technique for inverse kinematics", Proceedings of the 23rd Conference on Decision and Control, 1984） エー． バレストリノ、ジー． デ マリア、エル．シアビンコ、「ロボットの耐性の制御」、第９回ＩＦＡＣワールドコングレス（A. Balestrino, G. De Maria and L. Sciavicco in "Robust control of robotic manipulators", 9th IFAC World Congress, 1984）

本発明は、複数の制御手段を有するシステムをリアルタイムに制御する方法を提供している。各制御手段は、少なくとも１つの変数パラメータ(dq)と、前記制御手段に制御される軌道を示す被制御要素とを有し、前記軌道は、変数行列を介して前記変数パラメータと関連しており、前記方法は、前記変数パラメータ(dq)と、前記軌道(dx)とを関連づける制御転送行列(K)を定義し、所望の軌道(dxd)と現在の軌道(dx)との差からなる誤差(e)に依存したフィードバック項を計算するフィードバックループを用いることを特徴とする。

前記フィードバック項は、前記現在の軌道が前記所望の軌道に近づくように、複数回繰返して計算されることが好ましい。

前記行列(K)の出力値である前記出力(dq)は、次元(n)を有し、前記所望の軌道dxdは、次元(m)を有し、前記次元(m)は前記次元(n)以下であることが好ましい。

(m)×(n)次元の前記制御転送行列(K)を選択し、前記(K)の形態と、前記(K)の数値とを前記システムの特性に応じて決定することが好ましい。

与えられた(dxd)に対する(dq)を生成する数値アルゴリズムは、フィルタ形式であることが好ましい。前記アルゴリズムは、乗算に関する命令と、加算に関する命令と含むことが好ましい。

前記方法は、単一のプロセッサ、または、並列プロセッサによって実行されることが好ましい。

（J(q)）のランクが低下したときにも、行列(K)は、(dq)に対する解を与えるように構成されていることが好ましい。

特異状態において、誤差(e)は、特異方向に増大し、(K)の全体構成は、軌道を特異状態から離れさせるような動きを生み出すゼロでない解(dq)を生成することが好ましい。

前記システムは、移動可能な対象物の画像を表示するよう構成されたディスプレイを有し、制御手段は、前記移動可能な前記対象物の関節を制御し、制御される前記対象物は、移動可能な要素を有することが好ましい。

前記システムは、ロボットと制御システムとを有するロボットシステムであり、前記被制御要素は、ロボットの一部であり、前記制御手段はロボットの関節を有することが好ましい。

前記制御手段は、コントロールモーメントジャイロシステムにおけるジャイロを有することが好ましい。

本発明は、さらに、前記方法を操作するように構成された移動可能なシステムの制御システムを提供する。

本発明は、さらに、ロボットと、本発明の前記制御システムとを有する、ロボットの動きを制御するロボットシステムを提供する。

本発明は、さらに、複数のジャイロと、本発明の前記制御システムとを有するコントロールモーメントジャイロシステムを提供する。

本発明のいくつかの実施の形態の目的は、計算機上有効であって、リアルタイムに実行される数値的な方法であって、式(数2)で規定される逆運動学の問題を解くための方法を導出することである。

さらに、本発明のいくつかの実施の形態は、ヤコビ行列(J(q))のランクが低下したとき、または、特異なときに、解が存在するため、特異性に対して強固である。

さらに、本発明のいくつかの実施の形態におけるアルゴリズムは、減衰係数の計算を必要としない。

従って、本発明は、数値的に変数行列の逆行列をリアルタイムに計算する方法を提供している。前記方法は、任意の次元(m)をとりうる所望の軌道(dxd)と、現在の軌道(dx)とを比較して誤差(e)を生成するフィードバックループを用いている。かかる比較は、逆運動額の問題が計算される全サイクルにおいて行われることが好ましい。誤差(e)は、制御転送行列の入力として用いられ、前記制御転送行列は出力(dq)を生成することが好ましい。(dq)は、任意の次元(n)をとり、(m)は(n)より小さい。

行列は時間と共に変化することが好ましい。例えば、行列は、時間に依存するか、または、時間と共に変化する別のパラメータに依存する。

前記方法は、計算機によって実行されることが好ましい。

逆運動学の問題は、時間変数とパラメータとに依存する行列の逆行列を計算するものである。逆運動学の問題は、例えば、ロボットの制御、コンピュータで生成され、任意の数の関節と先端部とを有するキャラクターの制御と操作、キーフレーミング（keyframing）アプリケーション、及び、コントロールジャイロ機構を有する宇宙船の制御などに応用可能である。

前記方法は、(m)×(n)次元のフィードバック補正器を選択する方法、及び、(K)の形態と数値とを逆運動学の問題が解かれる対象の特性や構造に応じて決定する方法とを有することが好ましい。

前記方法は、アルゴリズムの数値的な実行を用いている。当該アルゴリズムは、与えられた(dxd)に対して(dq)を生成し、フィルタとして実行される。前記フィルタは、単一のプロセッサ、又は、並列のプラットフォームで実行可能な加算命令と積算命令のみを用いている。

いくつかの実施の形態では、(J(q))のランクが低下しても、(K)は、(dq)に対する解を与える。特異状態では、誤差(e)は、特異方向に増大し、(K)の全体構造が(dq)に対するゼロでない解を生成し、特異状態から離れる軌道を生み出す。結果として、特異性を解消することを、前記アルゴリズムに組み込むことができる。

本発明は、計算機上有効で、特異性に対して強固な逆運動学の問題を解く方法を提供することができる。

本発明は、システムの移動を制御する方法を提供している。当該方法は、本発明の前記方法を有している。前記システムは、ロボットのアームのように関節のあるシステム、又は、宇宙船のような移動可能なシステムである。

本発明は、さらに、移動可能なシステムの制御システムを提供している。当該制御システムは、本発明の前記方法によって操作されるように構成されている。

本発明は、さらに、移動可能な対象物のグラフィック画像を生成する方法を提供している。当該方法は、上記記載のいずれか一の方法を有している。

本発明は、ディスプレイシステムであって、上記記載のいずれか一の方法を実行し、画像データを生成しする処理手段と、前記画像を表示する表示手段とを有することを特徴とする。例えば、当該システムは、ゲーム機や、その他のコンピュータシステムを有する。

３つの関節と、１つの先端部を有するロボットマニピュレータなどに対応した構成例である。 コンピュータアニメーションなどに用いられるグラフィックの構成を示している。 逆運動学のブロックを示している。 本発明の第１の実施の形態における、逆運動学の解のフィードバック形式を表している。 図４に記載されたフィードバック逆運動学則の拡張版を表している。 本発明に記載の逆運動学の問題のアルゴリズムを実行するＣ言語におけるコードの例である。 本発明の実施の形態における、ロボット、及び、制御システムの概略図である。 本発明の実施の形態における、グラフィック表示装置の概略図である。

本発明の実施の形態を図面を参照しながら説明する。

本発明の実施の形態は以下の構成を有する。
１）乗算手段
２）加算手段
３）減算手段
４）(ｍ)×(n)次元のメモリブロックのようにヤコビ行列を記憶可能なヤコビ行列記憶手段
５）(dxd)を供給する手段
６）数学モデル、または、センサのなどの(q)を供給する手段
７）パラメータ(z(t-1))を記憶可能な(m)次元のメモリブロックなどのパラメータ記憶手段
８）パラメータ(dq(t-1))を記憶可能な(m)次元のメモリブロックなどのパラメータ記憶手段
９）パラメータ(tmp)を記憶可能な(m)次元のメモリブロックなどのパラメータ記憶手段
１０）(A)を記憶可能な(m)次元のメモリブロックなどのパラメータ記憶手段。もし、Aが恒等行列にスカラーを掛けたものなら、メモリーブロックは、当該スカラーのみを記憶するだけでよく、次元数は(1)になる。
１１）(P)を記憶可能な(m)×(m)次元のメモリブロックなどのパラメータ記憶手段

上記の手段は汎用プロセッサによって与えられる。汎用プロセッサとしては、例えば、パーソナルコンピュータ、マイクロプロセッサ、マイクロコンピュータ、デジタルシグナルプロセッサ、又は、特定目的用に構成された演算器、及び、メモリブロックである。

本発明の実施の形態の目的は、計算機上有効であって、式(数2)を解くための特異性に強固な方法であって、リアルタイムに実行され、逆行列を計算することを要せず、かつ、減衰係数を要しない方法を提供することである。式(数2)のヤコビ行列は、解を求めるために用いられ、ロボットマニピュレータの運動学、コンピュータアニメーションのキャラクターを表している。あるいは、式(数2)のヤコビ行列は、宇宙船の操作と関連している。あるいは、式(数2)のヤコビ行列は、逆行列、あるいは(q)のような変数パラメータに依存したヤコビ行列の逆行列が必要な応用例に関連している。

フィードバックループは図４に示すように構成されている。当該フィードバックループは、各サイクルで計算を行っており、リアルタイムで実行される。誤差４７は、生成されたベクトル(dx)４５と、要求されたベクトル(dxd)４４との差であり、制御法則（Ｋ）４１の入力値として用いられる。かかる制御法則は、誤差４７が無視できるくらい小さい値になるようにして、要求された(dq)４３を生成する。参照番号４６は加算器を示す。参照番号４８は正の符号を示し、参照番号４９は負の符号を示す。

図４のフィードバックループは、与えられたヤコビ行列(J(q))にたいして、(dxd)から(dq)を導く機構を備える。図４におけるフィードバックループは、図３のＩＫブロックを置き換えるものである。図４のフィードバックループを用いることで、以下の関係が導出される。
dq = K[JK+I]^-1 dxd (数5)

式(数5)において、(K)は制御転送行列(control transfer matrix)、または、制御法則であり、本発明における本実施の形態の一部として導かれるものである。(J)は、式(数1d)から導かれるヤコビ行列であり、(^-1)は、逆作用素を表す。本発明における本実施の形態、または、他の実施の形態において、重要な要素は、(K)を導出し、特異性を回避する性質をもつ(K)の形態を選択することである。さらに、(K)は、J(q)の可変性を示している。

ループにおける特異性回避の性質を提供する当業者に知られているように、(K)は、伝達関数の完全転送行列（full transfer matrix）であり、ゼロでない非対角成分を有している。(K)は、(n)×(m)の次元を持つ。

(K)は(q)の関数である。即ち、当業者に知られているように、(K)は、ヤコビ行列の時間可変性、及び、(q)依存性を示していおり、その値は調整される。

ヤコビ行列のランクが低下すると、誤差(e)が一方向に増大する。これは、(J(q))が、その固有の方向において、ゼロを出力するためである。(K)のゼロでない成分は、必要な出力(dq)を生成する。ここで、必要な出力(dq)は、特異方向から軌道を誘導し、誤差(e)を無視できるほど小さくし、目標起動を良く辿るものである。

図４のフィードバックループにおいて、誤差４７は、所望の変数(dxd)と、式(数1c)から計算された実際の変数との差異を示している。
e=dxd - J(q)dq (数6)

式(数6)の誤差は(K)を適切に選択することで任意に小さくすることができる。(K)が決められたときには、式(数5)を用いて(dq)を計算することができる。

＜固定抽出間隔によるリアルタイムな処理の実現＞
もし、（ロボット工学の制御、宇宙船の制御を含む典型的な応用例において）パラメータ依存性のあるヤコビ行列（以下、単にヤコビ行列とする）の逆行列を求めることが、抽出間隔として当業者に知られた所定の間隔で実行される場合、以下のアルゴリズムが使われる。

本実施の形態では、(K)は以下の分離形式で与えられる。
K: z(t) = Az(t-1) + e (数7a)
dq = Jt(q)Pz(t-1) (数7b)

式(数7a)と式(数7b)とで、図４および式(数5)のフィードバックループとブロック(K)とを表している。

式(数7a)と式(数7b)とで、式(数6)における誤差(e)と(dq)とを結びつける。

式(数7a)と式(数7b)とは、各サイクルでリアルタイムに同時に実行される。ここで、各サイクルでは、逆運動学の問題が所望の(dxd)を辿るための(dq)を求めるために解かれる。２つの連続するサンプルの時間差、即ち、連続して２回実行されるＩＫアルゴリズムの時間間隔は、当業者にサンプリング時間(dt)として知られたものである。

式(数7a)において、(A)は、 (m)×(m)次元の負の対角行列である。

式(数7a)において、(P)は、(m)×(m)次元の正の完全行列(full matrix)である。

式(数7b)において、(Jt(q))は、式(1d)から求められる、(n)×(m)次元のヤコビ行列の転置である。

式(数7b)において、ヤコビ行列(Jt(q))の転置は、各サンプルにおいて、現在の(q)を用いて再計算される。(q)は、センサ、または、数学モデルに基づいて与えられる。

式(数7a)、(数7b)において、(z(t))は、次元(m)の状態変数を示すベクトルである。

式(数7a)において、(z(t-1))は、次元(m)の状態変数を示すベクトルである。

式(数7a)、(数7b)において(z(t-1))は、(z(t))において、１回のサンプル間隔分に相当する(dt)だけ遅れたものである。かかる遅延した値を求めるためには、(m)次元のメモリ要素が必要である。ここで、一回のサンプルに相当する(dt)の遅延は、D(dt)と表される。
z(t-1) = D(dt) z(t) (数8)

式(数7a)、(数7b)において、(z(t-1))は、前回の繰り返しにおいて式(数7a)より計算された (z(t))の値を表している。

式(数7a)、(数7b)は、与えられた誤差(e)に対して、(dq)を求めるためのフィルタである。

式(数7a)、(数7b)と、式(数6)とは、本実施の形態の結論を示し、逆問題を解くためのアルゴリズム全体を与えている。このアルゴリズムは以下のようにまとめられる。
e = dxd - J(q)dq(t-1) (数9a)
z(t) = Az(t-1) + e (数9b)
dq = Jt(q) P z(t-1) (数9c)

式(数9a)、(数9b)、(数9c)とをまとめて用いることで、与えられた所望の軌道(dxd)に対する(dq)を計算することができる。

図５は、式(数9a)、(数9b)、(数9c)を図形的に構成したものであり、図４の構成を拡張したものである。図５は、リアルタイムな実行を可能としたものであり、アルゴリズムのフィルタ特性を示している。D(dt)５１、D(dt)５１１は、遅延ブロックであり、(z(t))５１３の値と、(dq)５３の値とを記憶するメモリを表している。(A)５１２は、式(数16)によって定義される。ブロック５１０内のPは、式(数20)により定義される。ブロック５１０内の(Jt(q))は、ヤコビ行列の転置であり、現在のサンプルにおける(q)を用いて計算される。(z(t))５１３は、式(数9b)を用いて計算される。(J(q))５２は、現在のサンプルにおける(q)を用いて計算されるヤコビ行列である。(q)は、センサや、数学モデルに基づいて与えられる。(e)は、要求された軌道(dxd)５４と、実際の軌道(dx)５５との誤差を示す。参照番号５６、５１５は加算ブロックを示している。参照番号５８、５１６、５１７は、正の符号を示し、参照番号５９は負の符号を示している。(z(t-1))５１４は、前回のサンプル、または、前回の繰り返し処理において計算された(z(t))５１３の値を示している。(dq(t-1))５１８は、前回のサンプル、または、前回の繰り返し処理で計算された(dq)５３を示している。

式(数9a)、(数9b)、(数9c)は、図３のＩＫブロックを置き換えるものである。

式(数9a)、(数9b)、(数9c)は、式(数2)を置き換える新しいアルゴリズムを表している。

式(数9a)、(数9b)、(数9c)は、は、逆運動（ＩＫ）の問題を解くためのアルゴリズムを表している。

式(数9a)、(数9b)、(数9c)は、リアルタイムに各サイクルで実行される。

式(数9a)、(数9b)、(数9c)は、フィルタ処理と同様な処理を実行し、加算命令と、乗算命令とを用いる。この計算は、図５においてフィルタ形式で示されている。

式(数9a)において、(dq(t-1))は、一回分のサンプル期間(dt)だけ遅らせた(dq(t))の値を示している。
dq(t-1) = D(dt) dq (数10)

＜変動抽出間隔によるリアルタイムな処理の実現＞
逆運動学の別の種類の応用例では、ヤコビ行列の逆行列、あるいは、パラメータ依存性のある行列の逆行列は、リアルタイムな方法で、かつ、固定した抽出率を用いずに行われる。例えば、コンピュータグラフィックスや関節のあるキャラクターアニメーションに動きを統合する場合において、端点（あるいは、図１の先端部１４、図２の先端部２１１，２１２，２１３，２１４，２１５）の新たな位置は、所定の割合（フレームレート）で規定される。端点、或いは、先端部が要求された位置に到達するまで、フレーム（あるいは、時間間隔）内に、逆運動のタスクは、連続して、数回解かれる。１フレーム内において連続して解を求めることを反復法と呼ぶ。端点、または、先端部が要求されるレベルに到達するまで、１フレーム内で、逆運動学の反復が実行される。各反復は、不規則な間隔でコンピュータプログラムによって実行される。ここで、不規則な間隔とは、ソフトウェアの負荷に依存して変化する固定されていない間隔である。これらの応用例において、以下の変更されたアルゴリズムが、逆運動学、即ち、dqの値、を計算するために各反復で用いられる。
dq = Jt(q) P z(t-1) (数9d)
z(t) = z(t-1) + h(Az(t-1) - J(q)dq) + h dxd (数9e)

式(数9d)、(数9e)は、本発明のさらなる実施の形態における、Kの形態と、図４におけるフィードバックループを表している。

この実施の形態は、前述の実施の形態の構成に加えて、以下の構成を有する。
１２）hを記憶可能な次元(1)のメモリブロックなどのパラメータ記憶手段
１３）bdqを記憶可能な次元(n)のメモリブロックなどのパラメータ記憶手段
１４）dq0を記憶可能な次元(n)のメモリブロックなどのパラメータ記憶手段
１５）dq_fullを記憶可能な次元(n)のメモリブロックなどのパラメータ記憶手段
１５）Vを記憶可能な次元(n)のメモリブロックなどのパラメータ記憶手段

図８に示す構成は、z(t)からz(t-1)を計算するために用いられる。hは、応用例に応じて調整される定数である。hは、正の値であって、設備（又は、システム）の最小時間定数の５０％以下となるように選択される。ここで、設備（又はシステム）は、既知のシステム行列(A-J(q)Jt(q)P)で示されている。hは、A、P、現在のヤコビ行列値J(q)に依存する。例えば、hは、h=2e-2のように設定し、A、Pも選択することが出来る。dqが非常に小さくなるか、又は、端部（先端部）が現在のフレームにおいて要求された所望の位置に達するまで、式(数9d)、(数9e)は各反復で実行される。式(数9d)、(数9e)は、図４に示されるフィードバック運動学法則の一次の積分解である。高次の積分器を用いることも可能である。式(数9d)において、各自由度毎に異なる増幅率をdqに乗じて、各自由度毎の増幅率の差異を調整している。即ち、dqは、dq = V Jt(q) P z(t-1)となる。ここで、V=[v1,v2, ... , vn]である。

次に(P)と(A)との導出を説明する。

＜式(数9a)-(数9c)における(A)、及び、(P)の導出＞
式(数7a)、(数9b)における(A)は、(m)×(m)次元負定値対角行列である。(A)は、全部で(m)個の要素を有する。

(a1)、(a2)、(a3)、．．．(am)は、正の実数のスカラーであり、下限の周波数、及び、(dx)と(dxd)との誤差を規定している。ロボットマニピュレータ、グラフィックの構成、コントロールモーメントジャイロ操縦法則といったシステムにおける力学的な振る舞いに応じて、(a1)から(am)までが適切に調整される。もし、 (m)個の自由度全てが同じ重みであるときには、(a1) = (a2) = ... = (am) = (a)と設定することができる。すなわち、全ての値を同じ値(a)に設定することができる。第１ジンバル慣性(first gimbal inertia)が働くＣＭＧ制御システムにおいて、(a)を例えば以下のように設定することができる。
a1 = a2 = ... am = a = exp(0.05dt) (数12a)

(dt)は、上記のように定義されたサンプリング期間であり、(exp)は、指数演算子である。

グラフィックの構成においては、以下のような値を用いても良い。
a1 = a2 = ... am = a = exp(5dt) (数12b)

式(数7b)、(数9c)における(P)は、当業者に知られた正定値対象行列である。(P)は、完全行列(full matrix、又は、対角行列であってもよい。

(P)を見つけるために、以下の文献のように当業者に知られているリッカチ方程式の解である行列(Pa)を計算する(ドイル ジェイ．、グロバー ケー．、カーゴネカー ピー．、フランシス ビー．、「標準Ｈ２とＨ無限大制御問題における状態空間の解」（Doyle, J., Glover, K., Khargonekar, P., and Francis, B., “State-space solutions to standard H2 and H-infinity control problems" IEEE Transactions on Automatic Control , Vol. 34, 1989, pp. 831-847.）)。
Mt*Pa + Pa*M - Pa*( B2*(V)^-1*B2t - s^-2*B1*B1t )*Pa + Ct*C = 0 (数13)

(Pa)を導出するために、１つのゼロでないスカラー、及び、６つの行列 (B1)、 (B2)、 (M)、 (V)、(C)が定義される。

(B1)は、(m)×(m)次元の恒等行列である。(B1t)は、(B1)の転置である。

(B2)は、(m)×(n)次元の行列であり、ゼロでない(q)=(q0)におけるヤコビ行列(J(q))に(-1)を掛けたものである。
B2 = - J(q0) (数14)

(q0)は、変数(q)に代入される値である。(q0)は、(n)次元であり、ゼロでない変数(q)の組合せとして構成することができる。(B2t)は(B2)の転置である。

ここで、(J(q0))は、ゼロでない特異値をもたなければならない。

(C)は、(n+m+m)×(m)次元である。(C)は以下のように区画される。

第１のグループは、(n)×(m)の要素であり、値は全て０である。

第２のグループは、(m)×(m)次元であり、 (m)×(m)の要素の恒等行列(I)に正のスカラー(w)を掛けたものである。ここで、(w)は、図４のループのカットオフ周波数であり、例えば、w = 4 rad/s. (w)である。(w)は、応用例に応じて適切に調整される必要がある。

第３のグループは、(m)×(m)の要素であり値は全て０である。

式(数13)の(M)は、(m)×(m)次元の負定値対角行列である。(M)は、全部で(m)個の要素を持つ。

(m1)、(m2)、(m3)、．．．(mm)は正の実数のスカラーであり、(dx)と(dxd)との誤差における低い周波数の減衰を規定している。ロボットマニピュレータ、グラフィックの構成、コントロールモーメントジャイロの操縦法則といったシステムにおける力学的な振る舞いに応じて、(m1)から(mm)までが適切に調整される。もし、 (m)個の自由度全てが同じ重みであるときには、(m1) = (m2) = ... = (mm)と設定することができる。すなわち、全ての値を同じ値に設定することができる。第１ジンバル慣性(first gimbal inertia)が働くＣＭＧ制御システムにおいて、例えば、以下のように設定することができる。
m1 = m2 = ... mm = 0.05 (数17a)

グラフィックの構成においては、例えば、以下のように設定することができる。
m1 = m2 = ... mm = 5 (数17b)

(Mt)は(M)の転置である。

式(数13)の(V)は、(n)×(n)次元の正定値対角行列である。(V)は全(n)個の要素を持つ。

ここで、(v1)、(v2)、(v3)、．．．、 (vn)は正の実数のスカラーであり、システムが達成しうる各自由度(q)の最大比率（例えば、rad/s、または、m/s）の二乗を規定する。ロボットマニピュレータ、グラフィックの構成、コントロールモーメントジャイロの操縦法則といったシステムにおける力学的な振る舞いに応じて、(v1)から(vn)までが適切に調整される。もし、 (n)個の自由度全てが同じ重みであるときには、(v1) = (v2) = ... = (vn)と設定することができる。すなわち、全ての値を同じ値に設定することができる。これらは、第１ジンバル慣性(first gimbal inertia)が働くＣＭＧ制御システムにおいて、例えば以下のように設定することができる。
v1 = v2 = ... = vn = 1.8 * 1.8 (数19a)

グラフィックの構成では、例えば、以下のように設定することができる。
v1 = v2 = ... = vn = 1 * 1 (数19b)

式(13)における(s)は、スカラーである。以下のアルゴリズムが(s)を決定するのに用いられる。
ステップ１：(s)を大きな数、例えばs=100に設定する。
ステップ２：(s)の値に対して、式(数13)を解いて(Pa)を決定する。
ステップ３：もし、解が存在し、(Pa)が正定値であれば、(s)を、例えば(s)/2に減らす。そして、ステップ２からの処理を繰り返す。
ステップ４：もし、新たな(s)の値に対して、式(13)が正定値の解を生成しなければ、最後に正定値として得られた解(Pa)を式(数13)のリッカチ方程式の解とする。

(Pa)が、式(13)より計算されると、式(数7b)、(数9c)における(P)は、式(数19a)、(数19b)により定義される自由度のうち１つの自由度の最大率を(Pa)に掛けることにより得られる。即ち、(P)は以下のようになる。
P = (v1)*Pa (数20)

コンピュータアニメーションなどの多くの応用例では、式(数9d)、(数9e)における(P)と(A)とに定数を選択できる。これにより、計算資源をかなり減らすことができる。(P)と(A)とを計算するために、以下のアルゴリズムを上記のアルゴリズムの代わりに用いてもよい。
ステップ１：Aを選択する。Aは、一般的には、-0.0001から-1000の負の変数である。95
自由度をもつスケルトンの典型的な値はA=-20である。
ステップ２：現在の結合部（関節）の組合せにおけるヤコビ行列J(q)を取得し、以下の式を計算する。
JJnorm = sqrt ( trace(J(q)Jt(q)) )

ここで、trace(J(q)Jt(q))は、行列のトレース、即ち、J(q)Jt(q) 主対角成分の和である。また、sqrtはルートを示している。
ステップ３：Pを以下の条件を満たすように選択する。
P <= (pi/h - A) / Jjnorm
A、h、Pの適合：ステップ３より明らかなように、PとAとhとは、相互に関係している。例えば、典型的なアルゴリズムにおいて、運動学的な構造が定義されると（または、コンピュータアニメーションにおいてスケルトンフレームが定義されると）、以下のような処理が行われる。
ステップ１：Aは固定された、例えば、以下のような値である。
A = -20
ステップ２：hは固定された、例えば、以下のような値である。
h = 0.02
ステップ３：Pは以下のように計算される。
P <= (pi/h - A) / Jjnorm
ステップ４：上記の値に基づいて、先端部のダミー軌道が定義され、実行される。式(数9d)と(数9e)とを反復して計算して、所望の値に落ち着かせる。かかる反復処理の回数を監視し、反復数を減らすために、Pの値が増加するように調整される。もし、反応が不安定であった場合（dqが均一に大きくなる場合）、hの値を減少させる。
ステップ５：安定した反応が達成されたら、Aは、反復回数を改良するように調整される。Aは、典型的には、反復回数を減らすように増加させられる。

上記の適合アルゴリズムは、１つの与えられたスケルトンに対して、一回のみ実行される。

＜フィードバック逆運動学の実行（固定サンプリング間隔）＞
式(数9a)-(数9c)で定義されるフィードバック逆運動学アルゴリズムは、フィルタとして働き、乗算命令、加算命令のみを用いている。アルゴリズムのフィルタ形式は図５に示される。

フィードバック逆運動学は以下のように実行される。
ステップ０（初期化）：
(z(k-1))を次元(m)のゼロベクトルと設定する。
(dq(t-1))を次元(n)のゼロベクトルと設定する。
ステップ１：現在の(q)におけるヤコビ行列(J(q))を計算する。ここで、(q)は、センサ（カメラ、モーター光学エンコーダー、モータータコメータ、など）の測定、あるいは、数学モデルなどから得られる。ヤコビ行列は、逆運動学が解かれるシステムの構造に依存している。一般的には、ヤコビ行列は、(q)に依存するスカラーと、sin(q)、cos(q)といった(q)に依存する三角関数とから計算される。ヤコビ行列は(m)×(n)次元である。数値形式の(q)を代入したヤコビ行列を計算し、 (J(q))の各要素を解き、(m)×(n)の数値形式の行列を生成する。
ステップ２：乗算手段、加算手段を用いて以下の計算を行う。計算結果を(dx)として(dx)記憶手段に記憶する。これにより、(m)次元のベクトル(dx)が得られる。
dx = J(q)dq(t-1)
ステップ３：(dxd)からステップ２の結果を引くことにより、式(数9a)の誤差を計算する。即ち、減算手段を用いて以下の計算を行う。
e = dxd - dx
記憶手段を用いて、この結果を(e)として記憶する。

ステップ４：記憶手段の(P)と(z(t-1))とを用いて、(P)と(z(t-1))との積を計算し、結果を(tmp)に記憶する。
tmp = Pz(t-1)
この演算は、記憶手段と、乗算手段と、加算手段とを用いる。(tmp)を記憶する手段を用いて、(tmp)を記憶する。ここで(tmp)は、(m)次元ベクトルである。

ステップ５：式(数9c)における(dq)を導くために、ヤコビ行列の転置(Jt(q))にステップ４で計算された(tmp)を掛ける。
dq = Jt(q)tmp
この演算は、乗算手段と、加算手段と、(dq)を記憶するための手段を用いる。

ステップ６：(z(t-1))、 (A)、ステップ３で計算された(e)を用いて、式(数9b)からz(t)を計算する。
z(t) = Az(t-1) + e
この演算は、加算手段と、乗算手段と、(z(t))を記憶するための手段とを用いる。

ステップ７：以下のように設定する。
z(t-1) = z(t)
dq(t-1) = dq
この演算は、(z(t-1))と(dq(t-1))とを記憶する手段を用いる。これらの値は、(dt)秒後に行われる次回のIKアルゴリズムの反復において用いられる。

ステップ８：ステップ５で得られた(dq)をIK問題の解として用いる。ステップ１に戻り、(dt)秒後に次の反復処理を行う。

図６は、一般的な逆運動学の問題のための上記のアルゴリズムを実行するＣ言語命令を用いたコンピュータソフトウェアプログラムを示している。全演算数は以下のようになる。
2*m*m + 2*m -1 + n*(2m-1) (数21)

もし、(m=6)ならば、ヤコビ行列の逆行列を計算し、IK問題を解くために、(83 + 11*n)回の演算が必要になる。

もし、(m=12)で(n=25)ならば、ヤコビ行列の逆行列を計算し、IK問題を解くために、(886)回の演算が必要である。

＜フィードバック逆運動学の実行（変動サンプリング間隔）＞
変動サンプリング間隔を用いた応用例の場合、以下のアルゴリズムを用いることができる。
ステップ０、ステップ１：上記のステップ０，１と同じである。
ステップ３：式(数9d)を計算する。
dq = Jt(q) P z(t-1)

ここでは、ヤコビ行列の転置(Jt(q))、及び、P、z(t-1)を用いる。
ステップ４：式(数9e)を計算する。
z(t) = z(t-1) + h(Az(t-1) - J(q)dq) + h dxd

ここでは、ヤコビ行列(J(q))、A、z(t-1)、h、dxdを用いている。z(t)は、次回の反復のステップ３に用いられるz(t-1)の値になる。

ステップ３におけるdqを今回の反復における逆運動学の問題の解とする。dqが閾値より小さい値になるか、または／かつ、先端部が目標に到達するまで、上記のステップを繰り返す。

逆運動学の解を求めるために、2*m*n + 4*m回の乗算と、2*m*n-n+2*m回の加算が一回あたりの反復で行われる。明らかに、計算の複雑さは、mとnとに線形である。

以下の文献では、IK問題を計算するために、本実施の形態と同じ次元数(m=12)、(n=25)において、乗算命令と加算命令とを使用する除算命令を含んだ６０００命令が最低でも必要となることが報告されている。
ワイ． ナカムラ、エイチ． ハナフサ、「ロボットマニピュレータの制御における特異性に耐性のある逆運動学の解」、ジャーナルオブダイナミックシステムズ、１０８巻、１９８６年９月（Y. Nakamura and H. Hanafusa, “Inverse kinematic solutions with singularity robustness for robot manipulator control”, Journal of Dynamic systems, Measurements and Control, Vol. 108, Sep. 1986）

従って、上記のアルゴリズムは、計算の負荷を少なくとも１／７倍に減らしている。また、以下の文献では、従来のIK法における計算の負荷を見積もっている。
エー． エー． マシジェウスキ、ジェイ． エム． リーギン、「運動学的に冗長なマニピュレータの制御における並列アルゴリズムおよび並列アーキテクチャ」、ロボティックスアンドアニメーション、ＩＥＥＥ会議、１０巻、４刷、１９９４年８月、４０５−４１４ページ（A.A. Maciejewski and J. M. Reagin, “Parallel Algorithm and Architecture for the control of kinematically redundant manipulators”, Robotics and Automation, IEEE Transactions on, Volume 10, Issue 4, Aug 1994 Page(s):405 - 414）

この文献と比較しても、本発明のアルゴリズムでは、少なく見積もって約１／１０倍に負荷を減らしている。

式(数21)の数は、説明のためのものであり、ヤコビ行列(J(q))は、全てゼロでない要素を持つ(full structure)ように仮定されている。かかる点に関しては、さらに最適化して、 (J(q))のゼロの要素が存在するように、アルゴリズムをより効率よく実行することが可能
である。

また、(J(q))を適切に調整して、(P)が対角になるようにすることができる。

また、２以上の処理ブロックを使った並列、多重処理によって、処理を向上させることが可能である。本発明のアルゴリズムは、付加的な作業を必要とせずに、並列アーキテクチャに適合する。これには、いくつかの方法がある。例えば以下のような方法である。

図６において、２つの処理ブロックを有するアーキテクチャにおいて、(e(t))と(z(t))との計算は、(tmp)と(dq)との計算と並行に実行することができる。

例えば、(J(q)dq(t-1))の計算において、(m)×(n)次元の(J(q))は、以下のように区画される。

ここで、各列は、(n)個の要素を持つ。並列して各列を(dq(t-1))倍することが可能である。従って、もし、(m)個のプロセッサが利用できるシステムであれば、各プロセッサは、並列して、(J(q))の各列を(dq(t-1))倍することができる。これにより、単一のプロセッサで処理するよりもループを(m)倍速く実行することができる。

同様の処理は、上記のアルゴリズムにおけるステップ４，５においても適用できる。その際に、乗算の結果に対して適切にメモリのポインタを構成すれば、追加の処理を加えることなく実行することが可能である。

＜関節拘束条件、関節制限、軌道、環境拘束条件のため、および、動作再標的化のためのNULL-motion＞
関節拘束条件、関節制限、環境拘束条件、及び、動作再標的化を導入するために、以下のように、式(数2)に同次項(homogeneous term)を追加する。
dq = iJ(q)dxd + [I - iJ(q)J(q)]dq0

新たな同次項[I - iJ(q)J(q)]dq0は、図４のフィードバック形式iJ(q)を置き換えることにより計算することができる。

コンピュータアニメーション、及び、非固定サンプリング間隔を考慮した完全解は、以下のように変更される。
bdq = Jt(q) P bz(t-1) (数9f)
bz(t) = bz(t-1) + h(A bz(t-1) - J(q) bdq) + h J(q)dq0 (数9j)

最終関節の速度は、以下のようになる。
dq_full = dq - bdq + dq0

ここで、dqは、式(数9d)から計算される、bdqは、式(数9f)から計算され、dq0は、与えられた拘束条件、例えば、関節動作の限界、与えられた関節変数(k)に対する動作の再標的化、に応じて以下のように計算される。
dq0(k) = gain * ( bq(k) - q(k) )

ここで、bq(k)は、関節の所望の値であり、定数か、時間に依存した関数である。q(k)
は、関節変数(k)の現在の値であり、gainは、選択可能な正の定数である。

図７を参照して、本発明の実施の形態としてのロボットについて説明する。ロボットは先端部７１４と、３つの関節７１１、７１２、７１３とを有している。従って、このロボットは図１のロボットに対応するものである。各関節は３つの自由度をもち、３つのアクチュエータを有する。各アクチュエータは、１つの自由度に関する動きを制御している。制御システムは、プロセッサ７２２と、メモリ７２４とを有するコンピュータ７２０を有する。プロセッサ７２２は、要求される全ての処理手段を供給するように構成されている。メモリ７２４は、要求される全ての記憶手段を供給するように構成されている。例えばユーザによる入力７３０などによる入力により、先端部７１４の所望の動きを示すコンピュータ７２０への入力を生成するように構成されている。センサ７４０は、関節７１１、７１２、７１３の位置と動きとを測定するように構成され、コンピュータ７２０に当該位置と動きとを示す信号を送信する。コンピュータは、上述した方法の１つを用いて、関節７１１、７１２、７１３と関連付けされたアクチュエータを制御する。

図８を参照して、本発明の別の実施の形態について説明する。ＰＣ８２０は、プロセッサ８２２と、メモリ８２４とを有している。コンピュータは、ディスプレイ８２６と、ユーザ入力装置８３０とに接続されている。メモリ８２４は、ユーザがコンピュータ上でゲームを遊ぶことを可能とする命令を保存している。ここで、ゲームにおいて、グラフィック画像８３２は、ディスプレイ８３４に表示され、ユーザインプット装置８３０の入力値に応じて、動くように制御されている。コンピュータは、ユーザ入力装置８３０の入力信号に応じて、上述の方法の１つを用いて、当該画像の動きを制御するように構成される。当該画像は、図２に示されるようなキャラクターに対応するものであってもよい。

Claims (17)

1. 少なくとも１つの変数パラメータ(dq)と、軌道(dx)とを有する複数の制御対象を有するシステムをリアルタイムに制御する方法であって、前記軌道（dx）は、変数行列を介して前記変数パラメータと関連しており、前記方法は、
   前記変数パラメータ(dq)と、前記軌道(dx)とを関連づける制御転送行列(K)を定義し、
   所望の軌道(dxd)と現在の軌道(dx)との差からなる誤差(e)に依存したフィードバック項を計算するフィードバックループを用いており、
   (J)は行列であり、(Jt)は(J)の転置であり、(A)は負定値対角行列、又は、負の定数であり、(P)は正定値完全行列、正定値対角行列、又は、正の定数であり、(h)は、正の定数であり、(ｚ)は状態変数からなるベクトルであり、(ｔ)はサンプリング時間であり、以下の演算：
   dq = Jt(q) P z(t-1)
   z(t) = z(t-1) + h(Az(t-1) - J(q)dq) + h dxd
   を用いて、前記少なくとも１つの変数パラメータ(dq)、および、前記ベクトル(z)を求めることを特徴とする方法。
2. 前記変数行列はヤコビ行列であることを特徴とする請求項１に記載の方法。
3. 前記フィードバック項は、前記現在の軌道が前記所望の軌道に近づくように、複数回繰返して計算されることを特徴とする請求項１または２に記載の方法。
4. 前記少なくとも１つの変数パラメータ(dq)は、 前記制御転送行列(K)の出力値であり、次元(n)を有し、前記所望の軌道(dxd)は、次元(m)を有し、前記次元(m)は前記次元(n)以下であることを特徴とする請求項１乃至３のいずれか一に記載の方法。
5. (m)×(n)次元の前記制御転送行列(K)を選択し、
   前記制御転送行列(K)の成分は、前記システムの特性に応じて決定することを特徴とする請求項４に記載の方法。
6. 前記所望の軌道(dxd)に対する(dq)を生成する数値アルゴリズムは、フィルタ形式であることを特徴とする請求項１乃至５のいずれか一に記載の方法。
7. 前記数値アルゴリズムは、乗算に関する命令と、加算に関する命令と含むことを特徴とする請求項６に記載の方法。
8. 前記方法は、単一のプロセッサ、または、並列プロセッサによって実行されることを特徴とする請求項１乃至７のいずれか一に記載の方法。
9. 前記行列（J(q)）のランクが低下したときにも、前記制御転送行列(K)は、前記少なくとも１つの変数パラメータ(dq)に対する解を与えるように構成されていることを特徴とする請求項１乃至８のいずれか一に記載の方法。
10. 特異状態において、誤差(e)は、特異方向に増大し、前記制御転送行列(K)の全体構成は、軌道を特異状態から離れさせるような動きを生み出すゼロでない解として前記少なくとも１つの変数パラメータ(dq)を生成することを特徴とする請求項１乃至９のいずれか一に記載の方法。
11. 前記システムは、移動可能な対象物の画像を表示するよう構成されたディスプレイを有し、制御手段は、前記移動可能な前記対象物の関節を制御し、制御される前記対象物は、移動可能な要素を有することを特徴とする請求項１乃至１０のいずれか一に記載の方法。
12. 前記システムは、ロボットと制御システムとを有するロボットシステムであり、前記被制御要素は、ロボットの一部であり、前記制御手段はロボットの関節を有することを特徴とする請求項１乃至１１のいずれか一に記載の方法。
13. 前記制御手段は、コントロールモーメントジャイロシステムにおけるジャイロを有することを特徴とする請求項１乃至１０のいずれか一に記載の方法。
14. 請求項１乃至１３のいずれか一に記載の前記方法に基づいて動作するように構成された制御システム。
15. ロボットと、請求項１４に記載の前記制御システムとを有する、ロボットの動きを制御するロボットシステム。
16. 複数のジャイロと、請求項１４に記載の前記制御システムとを有するコントロールモーメントジャイロシステム。
17. 請求項１乃至１０のいずれか一に前記方法を実行し、前記制御対象を画像化した画像データを生成するよう構成された画像生成手段と、前記画像データに基づいた画像を表示するよう構成された表示手段とを有する表示システム。
    

Images