JP2009521334A - 多関節システムの動作の再構築、リターゲット、軌道追従及び推定 - Google Patents

Abstract

本発明の制御システムと方法は、ソースシステムの観測を基に、ターゲットシステムの動作または姿勢に関する関節変数を生成する。本制御システムと方法は、ターゲットシステムが複製する動作がより的確なものとなるように、ターゲットシステムの冗長性を有効に利用した、拘束制御とバランス制御を有する。

Description

本願は、米国特許法第１１９条第ｅ項の下、米国特許仮出願番号第６０/７５３，９２２（出願日：２００５年１２月２２日）発明の名称：「Controlling Robots From Observed Human Motion: A Resolved Acceleration Approach」、米国特許仮出願番号第６０/７５３，９２４号（出願日：２００５年１２月２２日）発明の名称：「General Framework For Controlling Robots from Observed human Motion」、米国特許出願番号第１１/６１４，９３３（出願日：２００６年１２月２１日）発明の名称：「Reconstruction, Retargetting, Tracking, and Estimation of Motion for Articulated Systems」に基づいて優先権を主張するものであり、前記出願のすべての記載をここに引用するものである。

本願は、米国特許出願番号第１１/６１４，９３０号（弁護士事件整理番号２３０８５−１２１３８）(出願日：２００６年１２月２１日）発明の名称：「Reconstruction, Retargetting, Tracking, and Estimation of Pose of Articulated Systems」に関連するものであり、この出願のすべての記載をここに引用するものである。

本発明は人間や動物の動作の解析技術に関するものであり、より具体的には、低次元のタスクディスクリプタから人間や動物の動作を再構築、リターゲット、軌道追従及び推定する技術に関するものである。

半世紀以上前に、神経科学者ベルンシュタインは、人間の動力システムにおける冗長な自由度の制御に関する一連の問題を提起した(N. Bernstein著 「The coordination and regulation of movements」 Pergamon London １９６７年 なお、この文献に記載される全ての内容をここに引用する)。ベルンシュタインは、「途方もない自由度を有する筋神経系は、どういった仕組みにより、低い自由度しか持たないように振舞うのだろうか。」という問題を提起し、この問いは「多自由度」の問題と呼ばれている。ベルンシュタインは、筋神経系における操作の制御が、自由度を効率的に減らして、機械的な冗長性を減らしていると推測した。ベルンシュタインは、人間の動作に備わっている「デクステリティ（巧みさ）」は、余剰の自由度がある多くの関節の動きが積み重なることから現れることを観測している。

ベルンシュタインによって提起された問題は、人体の動力制御の分析と理解に大きな影響を与えた。その結果、人間の運動発達理論において確立されていた多くの考え方が大きく変わり、我々の知識も大きく増えた。「多自由度の問題」に関する多くの疑問は未解決のままだが、「多自由度の問題」は、人体やロボットの動作の解析や制御を取り扱う応用科学や工学技術の課題に、直接的または間接的に、哲学的、物理学的な洞察を与えた。

ベルンシュタイン問題に最も直接的に関連する課題は、タスクを行う際に冗長性を有するロボットの制御の研究において検証されてきたといえるだろう。ロボット研究者は長い間、タスクに関する冗長な自由度は、デクステリティ（巧みさ）や多様性を向上させるものと考えてきた。こうした視点は、運動学的レベルにおいても、動力学的レベルにおいても、タスク指向の制御ストラテジの公式化の着想の元となった。ところが、冗長性は、タスク空間から関節空間へ運動学を逆に解く際に、非適切性の問題を生じる。ほぼ全ての刊行物において、タスク空間から関節空間への逆変換の非適切性は、人工的なパフォーマンス指標を特定し、そのパフォーマンス指標を最小化するような逆運動学および逆動力学の解を求めることで対処されてきた。この非適切性に対する適切な対処方法は、低次元のモーション・プリミティブから人間の動作を解析する多くの研究分野に応用されている。

ベルンシュタインの推測を制御構造に定式化することで、動作タスクに関する多大な機械的自由度を、低次元のモーション・ディスクリプタで表すことが可能になった。これらのモーション・ディスクリプタは、より高いレベルのタスク変数によって動作を記述するために使用されるため、タスクディスクリプタとも呼ばれる。ロボット工学において、タスクディスクリプタを使用した制御法則は通常、関節空間ではなく、タスク空間で適用される。タスク指向の制御は、動作の実行時や運動学習時において、中枢神経系が自由度を統合的に管理し単純化しているとするベルンシュタインの仮説や運動学習に関する現在の考え方に適合するものである。

上述の通り、機械装置において連接された関節が、タスクを実行するのに必要な自由度よりも高い自由度を有している場合、タスクの制御は一般的に冗長性を備える。多くの内的関節動作が、同一の所望のタスク動作を生成することが出来るのである。多様な内的自己運動は、内的タスクの軌道追従の実行以外の追加的なタスクの要求を満たすために利用することもでき、この多様な内的自己運動が冗長性分解を提供する。冗長性は、関節制限を守るため（C. A. KleinおよびCH. Huang著 「Review of pseudoinverse control for use with kinematically redundant manipulators」、IEEE Transactions on Systems, Man, and Cybernetics １３（３） P２４５−２５０、１９８３年 なお、この文献に記載される全ての内容をここに引用する)、特異姿勢を避けるため（T. Yoshikawa著 「Manipulability of robot mechanisms」Int. J. Robotics Research ４（３）P３−９、1１９８５年 なお、この文献に記載される全ての内容をここに引用する)、そして障害を避けるためや、その他各種のパフォーマンス基準を最適化するためなどに効果的に利用することが出来る。

本発明の方法とシステムは、観測によって対象の動作を再構築、リターゲット、軌道追従、推定する技術を提供する。観測には、低次元のタスクディスクリプタが含まれていてもよい。ロボット等の多関節システムであるターゲットシステムの各部位の動作は、観測対象の動作を軌道追従することで制御することが出来る。多関節システムの各部位の動作に対しては、拘束が課される。観測対象の動作の軌道追従および課された拘束に応じて、多関節システムの各部位の動きを制御するコマンドが生成される。

本発明の他の側面においては、多関節システムの各部位の動きを制御するコマンドに応じて、バランス制御コマンドが生成される。観測対象の軌道追従は、一次もしくは二次閉ループ逆運動学、逆動力学の使用を含んでいてもよく、逆ヤコビアン行列の正則化を含んでいてもよい。

タスクディスクリプタは、人間等のソースシステムの動作やロボットの動作を定義するために使用することができる。タスク変数はソースシステムの各部位に与えられ、そのタスク変数によって表された人間の各部位の動作が検出される。多関節システムを制御するコマンドは、ソースシステムの動作を複製するために生成され、多関節システムが動作を行う際の物理的な制限に応じて、その動作複製のためのコマンドは調節される。

本明細書に記載された特徴及び利点は、全てを包括したものではなく、詳細には、図面、明細書及び特許請求の範囲を見た当業者にとって、多くの追加的な特徴及び利点が自明である。さらに、本明細書で用いられた言語は原則として読みやすさ及び説明目的で選択されたものであり、本発明の構成要件を限定したり線引きしたりするために選択されたものではないことに留意すべきである。

適宜図面を参照しながら、本発明の好ましい実施形態を以下に説明する。図面内の同一の参照番号は、同一の要素もしくは機能的に類似した要素を表すものとする。また、それぞれの参照番号の左端の数字は、その参照番号が最初に使われた図面の番号に対応している。

明細書中の「一実施形態において」という言葉は、実施形態と関連して説明されるある特定の機能、構造、特徴が本発明の実施形態の少なくとも一つに含まれる、という意味であり、常に同一の実施形態を表すものではない。

以下の詳細な説明には、コンピュータ・メモリ内のデータビット操作のアルゴリズム及び記号表現によって表された部分がある。このようなアルゴリズム記述及び表現は、データ処理技術分野において、当業者が研究内容を他の当業者に最も効率的に伝えるために用いる手段である。あるアルゴリズムがあり、それが概して期待した結果に至る筋の通ったステップの系列だと理解されるとする。それらのステップは、物理量の物理的処理を要するステップである。必ずという訳ではないが、これらの物理量は記憶、転送、結合、比較、その他処理が可能な電気的または磁気的信号の形をとるのが普通である。これらの信号をビット、値、要 素、記号、文字、術語、番号などで表わすのが、主に慣用上の理由から便利な場合があることが分かっている。さらに、一般性を損なうわけではないが、物理量の物理的処理を要するステップのある特定の配列を、システムもしくはコードデバイスなどと表すのが便利な場合があることが分かっている。

しかしながら、このような術語や同様の用語はすべて適切な物理量に関連付けられるべきであり、また、それら物理量に付けた便宜上のラベルに過ぎないという ことに留意すべきである。以下の説明より明らかなように、特に断わらない限り、”処理”、”演算”、”計算”、”判定”、”表示”などの術語によって論じられることは、コンピュータシステムのレジスタ及びメモリの内部の物理的（電子的）な量として表現されたデータを処理して、コンピュータシステムのメモリやレジスタ、その他同様の情報記憶装置、情報伝送装置又は表示装置の内部の同様に物理量として表現された他のデータへ変換する、コンピュータシステムや同様の電子的演算装置の作用及びプロセスを意味する。

本発明のある側面は、アルゴリズムの形で記述されたプロセス・ステップ及び命令を含んでいる。本発明におけるそれらのプロセス・ステップ及び命令は、ソフトウェア、ファームウェア、もしくはハードウェアのいずれでも実現することが可能であり、ソフトウェアによって実現する場合、ダウンロードして様々なオペレーティングシステム上の各種プラットフォームに駐在させて動作させることも出来る。

本発明は、本明細書に述べた処理を実行するための装置にも関係するものである。このような装置は、所要目的のために専用に作られてもよいし、汎用コンピュータを内蔵のコンピュータ・プログラムによって選択駆動もしくは再構成したものでもよい。そのようなコンピュータ・プログラムは、コンピュータが読み 取り可能な記憶媒体、限定するわけではないが例えば、フロッピー（登録商標）ディスク、光ディスク、ＣＤ−ＲＯＭ、光磁気ディスクなどの任意の種類のディスク、リードオンリーメモリ（ＲＯＭ）やランダムアクセスメモリ（ＲＡＭ）、ＥＰＲＯＭ、ＥＥＰＲＯＭ、磁気カード又は光カードなど、コンピュータのシステムバスに接続された電子的命令の記憶に適した任意種類の媒体に格納することができる。さらに、明細書中でコンピュータと呼ばれるものは、１つのプロセッサのみを含むものでもよいし、演算能力の向上のために設計されたマルチプロセッサ構成を実装したものでもよい。

本明細書で提示したアルゴリズム及び表示は、本質的に、いかなる特定のコンピュータ、その他の装置とも関わりがない。様々な汎用マシンを、本明細書に述べる内容に従ったプログラムで使用し得るが、所要の手順のステップの実行のために、より特化した装置を作るほうが好都合であるかもしれない。そのような多様なシステムに必要とされる構造は以下の説明から明らかになろう。さらに、どのような特定のプログラミング言語とも関連付けることなく本発明を説明する。 本明細書に述べる本発明の内容を実現するために様々なプログラミング言語を使用し得ることを理解されよう。本明細書における、特定のプログラミング言語に関する言及は、本発明の実施可能性及び最良の実施形態を開示するためである。

さらに、本明細書における用語は原則的に可読性及び説明の便宜上選択されたものであり、発明の主題を特定、制限するために選択されたものではない。したがって、本発明の開示は説明のためのものであり、請求の範囲に記載された発明の範囲を制限するものではない。

（本発明の）システムと方法は、タスク空間に表された低次元のモーション・プリミティブから人体動作を解析、再構築、リターゲット、軌道追従するための統合的なタスク空間制御手法を提供する。（本発明の）システムと方法は、その制御構造をデカルト空間、つまりタスク空間におけるタスクディスクリプタの軌道追従制御に分解することもでき、タスクの零空間における内的動作、つまり「セルフモーション」を制御することもできる。 (O. Khatib著「A unified approach for motion and force control of robot manipulators: The operational space Formulation」、IEEE Journal of Robotics and Automation, RA-３（１）：P４３−５３、１９８７年、P. Hsu, J. HauserおよびS. Sastry著「Dynamic control of redundant manipulators」、J. Robotic Systems ６（２）： P１３３−１４８、１９８９年、J.Y.S. Luh、M.W. Walker及びR.P.C. Paul著「Resolved-acceleration control of mechanical manipulators」IEEE Transactions on Automatic Control ２５：P４６８−４７４、１９８０年 なお、これらの文献に含まれる全ての記載をここに引用する)。所望のタスクディスクリプタは、キャプチャされた人間の動作から観測または推測することができる。タスク指向の（動作の）公式化は、人体動作をキャプチャするために使われるセンサや計装に柔軟性をもたらす。制御変数（タスク変数）は、タスクを記述する位置と方向の情報によって表してもよい。観測によって得られたタスク変数は、測定によって簡単に観測することが出来るボディの重要な位置（critical points）を含む。多くのタスクにおいて、末端のエフェクタの位置と方向は、タスクを記述するのに十分な情報を提供する。

図１は、ロボットシステムや生体ロボットシステムなどのターゲットシステム１０４を制御する、本発明の一実施形態における動作制御システム１００を示すブロック図である。 この動作制御システム１００は、ソースシステム１０２のタスクディスクリプタ１０８を検出する。ソースシステム１０２は、例えば人間や動物などであってもよい。動作制御システム１００は、タスクディスクリプタ１０８に応じて、ターゲットシステム１０４の動作を制御するための関節変数１１０を生成する。ターゲットシステムは、例えばロボットや多関節機構などの、多関節システム（例えば、人間モデル）などであってもよい。

動作制御システム１００は、例えば、バイオメカニクスにおいて、リターゲット、人体姿勢の推定、軌道の追従と推定、関節トルクの推定などのために使用することも出来る。

<リターゲット>
動作制御システム１００は、人間の動作のデータを範例として取り込んで、複雑なロボットの動作をプログラミングもしくは学習する過程を単純化するように構成されている。 (A. Nakazawa, S. Nakaoka, K. Ikeuchi, and K. Yokoi著 「Imitating human dance motions through motion structure analysis」 Intl. Conference on Intelligent Robots and Systems (IROS)、P２５３９−２５４４、Lausanne，Switzerland、２００２年、なお、この文献に記載されるすべての内容をここに引用する)。動作制御システム１００は、ロボットが人間が行うタスクを観測することで、そのタスクを学習する「見まね学習」のアルゴリズムを構築するために使用することも出来る(S. Schaal著 「Learning from demonstration」 M.C. Mozer, M. Jordan、T. Petsche編集、Advances in Neural Information Processing Systems 第９章 P１０４０−１０４６ MIT Press，１９９７年、なお、この文献に記載されるすべての内容をここに引用する)。動作制御システム１００は、「見まね学習」 を使用して、ロボットに手動でプログラミングをしてくという労力の必要な作業を、専門家がロボットにタスクを観測させるだけで起動する自動プログラミング処理に置き換えることができるのである。動作制御システム１００は、キャプチャした人間の動作をコンピュータアニメーションで使用して、ある多関節形状の動きを、類似した構造を有する他の形状の動きにリターゲットすることが出来る(S. Tak及びH. Ko著 「Motion balance filtering」Comput. Graph. Forum. (Eurographics ２０００), １９（３）：４３７−４４６，２０００、S. Tak及びH. Ko著、「A physically-based motion regargetting filter」、ACM Trans. On Graphics，２４（１）：P９８−１１７，２００５年 なお、これらの文献に記載されるすべての内容をここに引用する)。

<再構築 ― 人体姿勢の推定>
動作制御システム１００は、低次元のモーション・プリミティブから人体姿勢を再構築して、人体動作の解析を行うことが出来る。動作制御システム１００は、タスク変数として表された低次元のモーション・ディスクリプタから人体の姿勢を再構築するタスク指向の制御手法を有する。このタスク指向の制御手法は、人体姿勢の特徴の観測（もしくは測定）の数が不十分であったり、測定が阻害されている場合に、動作制御システム１００が人間の全自由度の再現を伴う処理を行うことを可能にしている。動作コントローラの物理的拘束は、物理的に不可能な姿勢を排除するために使用することも出来る。

<軌道追従と軌道推定>
動作制御システム１００は、拡張カルマンフィルタに代わるものとして、ノイズを含んだ一連の画像から対象物の動作パラメータを再帰的に推定するためにコンピュータの画像認識で使用されるツールを使用することもできる。拡張カルマンフィルタ及び前記ツールは共に追跡制御方法に使用されるが、その機能と設計において異なっている(Ted J Broida及びRama Chellappa著 「Estimation of object motion parameters from noisy images」 IEEE Transactions on Pattern Analysis and Machine Intelligence, PAMI-８（１）：P９０−９９、１９８９年、Larry Mathies、Takeo Kanade及びRichard Szeliski著 「Kalman filter-based algorithms for estimating depth from image sequences」、International Journal of Computer Vision, ３：P２０９−２３６、1989年 なお、この文献に記載されるすべての内容をここに引用する)。カルマンフィルタと同様に、動作制御システム１００は、予測推定を可能にする物理的力学に基づいている。

拡張カルマンフィルタは、直接測定することが出来ない状態を推定するために使用されることが多い。さらに、拡張カルマンフィルタは予測機能を有している。拡張カルマンフィルタにおいては、測定時刻は等間隔で並んでいる必要はない。時刻更新は、データが得られない時間間隔に実行することもでき、データが得られるようになると、測定の更新が実行される。このことは、拡張カルマンフィルタを使うと、断続的またはデータ抜けのある測定や、データがない場合の純粋予測にも簡単に対処できることを意味している。したがって、拡張カルマンフィルタは、所定のカメラの視界から断続的に隠されたランドマークを追従するためには有用な方法であるといえる。

一実施形態においては、動作制御システム１００は、軌道追従制御方法として分解加速度制御を用いる。分解加速度制御は、カルマンフィルタの予測、推定機能に加え、その動力学が、関節トルクに対応する入力によって駆動する物理システムの動力学を高い精度で表すことが出来るという利点を有している。

<生体力学における関節トルク推定>
筋神経系をコンピュータで分析する研究は、主に人体動作の合成もしくは解析に関連している(S. DeIp及びP. Loan著、「A computational framework for simulating and analyzing human and. animal movement」 IEEE Computing in Science and Engineering、２（５）：P４６−５５、２０００年、D. Thelen、F.C. Anderson及びS. DeIp著 「Generating dynamic simulations of movement using computed muscle control」 Journal of Biomechanics、３６：P３２１−３２８、２００３年、なお、これらの文献に記載されるすべての内容をここに引用する)。合成についての課題、つまり順動力学的解析は、与えられた初期条件と付与された力の結果としてのバイオメカニカル・システムの動作を提供するものである。システムの視点から、動作制御システム１００が処理する動作の方程式を表すシステム方程式をＳとする。Ｓは正確には明らかでない場合もあるため、順動力学の方程式を表すＳ’を、Ｓの推定式もしくはモデルとして表してもよい。順動力学の課題に対する解法を使用することで、動作制御システム１００はボディセグメントの動きをシミュレートしたり、予測したりすることが出来る(F.C. Anderson及びM. G. Pandy著 「Dynamic optimization of human walking」 Journal of Biomechanical Engineering, １２３：Ｐ３８１−３９０、２００１年 なお、この文献に記載される全ての内容をここに引用する)。順動力学的シミュレーションによって生成された動作の数値計算は、様々な用途に応用出来る。例えば、手術における外科的処理の変更に関わる実施例においては、対象となる人の動作パターンを基に、シミュレーションは予期される手術の結果を予測することが出来る(S. Piazza及びS. DeIp著「Three-dimensional dynamic simulation of total knew replacement motion during a step-up task」、Journal of Biomechanical Engineering、１２３：Ｐ５８９−６０６ ２００２年 なお、この文献に記載される全ての内容をここに引用する)。

解析、つまり逆動力学の課題は、合成の課題とは逆のものと考えられる。逆動力学解析によって、各関節のトルクと筋肉活動の総和について推定することが出来る。逆動力学モデルは、測定によって算出された入力セットにより得られた量を推定関節負荷に変換するものである。マーカー位置のモーション・キャプチャから得られた十分な（full）運動学的記述は、正確な逆運動学の解もしくはその近似値を得るのに十分なものである。しかしながら、モーション・キャプチャのデータは多く場合、関節負荷の推定精度を上げるために、フォースプレートを含む他のセンサからの出力と合わせて処理される。利用可能なセンサによって、動作の動力学方程式の最適表現が異なるため、逆動力学は、通常、多様なセンサを使うことに関する問題を有すると考えられている(B. Dariush、H. Hemami、M. Parnianpour著「Multi-modal analysis of human movement from external measurements」Journal of Dynamic Systems, Measurement, and Control, １２３（２）：Ｐ２７２−２７８、２００２年 なお、この文献に記載される全ての内容をここに引用する)。

センサの様式の問題に関わらず、逆動力学には問題がある。第一に、逆動力学の方程式は、ボディセグメントの角加速度と直線加速度の関数であり、ノイズを含んだ実験データの高次導関数の演算を使用しており、しばしば誤差を生じさせやすい作業としてよく知られている(Cullum著、「Numerical Differentiation and Regularization」 SIAM J. Numer. Anal. ８（２）：Ｐ２５６−２６５、１９７１年 なお、この文献に記載されるすべての内容をここに引用する)。 第二に、逆動力学は「解析」にしか適用できないことが挙げられる。つまり、逆動力学では、「仮定的な問い（つまり、「合成における課題」）」に直接的に答えることが出来ないということである。これは、臨床応用において遭遇する典型的な問題であり、順動力学によって解決することが出来るものである。

一実施形態において、動作制御システム１００は、タスク指向の分解加速度制御スキームを使用して、人体動作の解析及び合成の手法を制御する。この手法により、内力および内モーメントの推定が、タスク空間追従制御に関する課題として公式化される。動作制御システム１００は、例えば、マーカーの位置などを表すことが可能なタスク変数を追従する。また、関節制限の違反や、筋トルク制限の違反、自己衝突などを避けるために、拘束を課すこともできる。

<ソースシステム１０２とターゲットシステム１０４>
図２は、ソースモデル２０１と多関節システムに相当するターゲットモデル２０２間における、一つのタスクディスクリプタの対応関係を説明する図である。ソースモデル２０１及びターゲットモデル２０２はそれぞれ、ソースシステム１０２のモデル及びターゲットシステム１０４のモデルである。一実施形態において、ソースシステム１０２及びターゲットシステム１０４は同等のものである。他の実施形態においては、ソースシステム１０２及びターゲットシステム１０４は、２つの異なる多関節システムであり、それぞれ異なる次元、物理パラメータ、自由度を有していてもよい。ターゲットシステム１０４は、その特定の実施形態が運動学的解析または動力学的解析のどちらに関わるものかによって、運動学的パラメータによって記述してもよいし、運動学的パラメータ及び動力学的パラメータの両方によって記述してもよい。

一実施形態においては、ターゲットシステム１０４は、物理的システム、もしくは物理的システムのモデルであり、ソースシステム１０２は、物理的システムもしくはモデルを抽象したものである。ソースシステム１０２は、ターゲットシステム１０４に提供されるデカルト空間（つまりタスク空間）内に表される、一つ以上の所望のモーション・プリミティブを使用することができる。これらのモーション・プリミティブは、所望のタスクディスクリプタとも呼ばれ、例えば、観測や測定もしくは人為的な生成によって得ることが出来る。動作制御システム１００は、ソースシステム１０２の運動学的もしくは動力学的構造に関する知識を利用して、ターゲットシステム１０４用の所望のモーション・プリミティブを得ることが出来る。

一実施形態においては、ソースシステム１０２及びターゲットシステム１０４は、マスタシステムがスレーブシステムの動作を駆動する、「マスタースレーブ」システムであってもよい。一実施形態においては、ソースシステム１０２（マスタ）は、ターゲットシステム１０４（スレーブ）を駆動するモーション・プリミティブのセットを供給する。この実施形態においては、ソース動作は人体動作を観測することによって抽出されたものである。一般性を損なうものではないが、説明の便宜上、ソースシステム１０２は人体モデルを表すものとし、ソース動作は、人体のモーション・プリミティブ、つまり、典型的には観測されたり、測定から推定される「タスクディスクリプタ」であるものとする。観測や測定による推定に代わる手法として、タスクディスクリプタは人為的に生成してもよいし、理論的算出によって取得してもよい。ターゲットシステム１０４は、人体モデルや人間型ロボットなど、任意の多関節体モデルが用いられる。

一実施形態においては、全ての観測されたタスクディスクリプタを記述する空間の次元が、ソースシステム１０２を定義する自由度の総数とは異なっている。一実施形態においては、タスクディスクリプタは、ソースシステム１０２を規定する自由度の総数よりも低い次元しか占めないベクトル空間によって特徴付けられる。

<3. 1 部分的に観測、制御可能なタスク>
ソースシステム１０２から観測されたタスクディスクリプタは、連続する観測の間の対応関係を有する。すなわち、タスクディスクリプタが観測可能な場合、連続する時間インスタンスにおけるタスクディスクリプタの位置及び/または方向の間には対応関係がある。さらに、観測された「ソース」タスクディスクリプタとそれに関連付けられた「ターゲット」タスクディスクリプタの間には空間的な対応関係があるものとされている。ある「ソース」タスクディスクリプタの全ての観測値に対して、同じ次元と同様の自由度を有する、対応する「ターゲット」 タスクディスクリプタを定義することが可能である。また、「ソース」タスクディスクリプタと「ターゲット」タスクディスクリプタは、タスクの位置とタスクフレームの方向を特定するために要求される６パラメータで常に表現しなくてはならないものではない。

タスクディスクリプタが６未満のパラメータを有する場合は以下の通りである。
・「ソース」タスクディスクリプタの空間的位置及び/または空間的方向が部分的にしか観測出来ない場合がある。つまり、６度の自由度全ては測定できない場合である。これは、例えば、タスクに関連付けられた位置変数は測定により求めることが出来るが、タスクフレームの方向は求めることが出来ない場合に起こりうる。
・「ソース」タスクディスクリプタの位置及び方向は完全に観測可能であるが、ターゲットシステム１０４が、６度の自由度未満のタスクを実行する場合がある。つまり、利用可能な測定値がタスクの実行に必要な数を超えているため、「ターゲット」タスクディスクリプタと同じパラメータで「ソース」タスクディスクリプタを表せば十分な場合である。
・「ターゲット」システムが、あるタスクを実行するために十分な自由度を有していない時、そのタスクの実行が制限される、もしくは不可能な場合がある。

図３は、観測したタスクディスクリプタから動作を生成する動作制御システム３００を示すブロック図である。動作制御システム３００は、軌道追従制御システム３０２、拘束システム３０４及びバランス制御システム３０６により構成されている。軌道追従制御システム３０２は、観測されたタスクディスクリプタ１０８から関節変数ｑを生成し、拘束システム３０４から拘束タスクディスクリプタを生成し、バランス制御システム３０６からバランスタスクディスクリプタを生成する。

軌道追従制御システム３０２は、位置/方向誤差検出システム３１０を実装している。位置/方向誤差検出システム３１０は、観測されたタスクディスクリプタ、拘束タスクディスクリプタ、バランスタスクディスクリプタ、軌道追従制御システム３０２の順運動学システム３１２により算出されたタスクディスクリプタに応じて誤差を生成する。順運動学システム３１２は、関節変数ｑに応じてタスクディスクリプタを算出する。制御法則システム３１４は、位置/方向誤差検出システム３１０からの位置/方向誤差、拘束システム３０４からの拘束タスクディスクリプタ、関節変数 ｑに応じて制御信号を生成する。予測システム３１６は、関節変数ｑを生成するためにその制御信号を利用する。

拘束システム３０４は、衝突回避システム３２２、特異姿勢ロバストシステム３２４、関節速度制限システム３２６、関節制限システム３２８、トルク制限システム３２３を含む。以下、動作制御システム３００の詳細について説明する。

<運動学的構造>
運動学的構造を含む一実施形態に対応する動作制御システム３００について説明する。一実施形態において、ターゲットシステム１０４はツリー構造を有する一般的な多関節メカニズムを表すものとする。図４は、動作制御システム１００の１つの関節に対応する座標系及び座標変換を示す図である。この多関節メカニズムは、i＝１...ＮまでのＮ個のセグメントと、ｄ＝１...ｎまでのｎ_ｉを合計した自由度を持つ。なお、各関節は自由度ｎ_ｉを有する。このシステムは、ルートノードもしくはセグメント０とされる固定または可動の土台(ベース)がある。Ｎ個の関節はそれぞれのセグメントを接合しており、関節iはセグメントｐ（ｉ）とセグメントｉを接合している。ここで、ｐ（ｉ）はツリー構造におけるリンクｉの親リンク番号を表している。この番号はｐ（ｉ）＜ｉとなるようにとられる。特に、未分枝運動鎖においては、ｐ（ｉ）＝ｉ−１とし、セグメントと関節は土台から先端へと連続的に番号付けられる。セグメントｉは、次の式(１)に表される関係ｃ（ｉ）＝ ｛ｊ｝により定義される子(children)のセットを有している。

<リンク・関節構造>
座標系｛Ａ｝から座標系｛Ｂ｝への空間ベクトル量の変換は式（２）により与えられる。

波形符号（^〜）は、ベクトル外積を示す簡略記号である。この簡略記号により、任意のベクトルａとｂの外積を式（４）で表すことが出来る。

したがって、親リンクｐ（ｉ）の座標系からリンクｉの座標系への空間ベクトル量の変換は式（５）により与えられる。

ここで、ｒは座標系｛ｐ（ｉ）｝の原点から座標系｛ｉ｝の原点への3×1ベクトルであり、そのベクトル成分は座標系｛ｐ（ｉ）｝を基準座標系としている。式（５）が表すリンク間変換式は、一つのリンクに固定された２つの関節の相対的な位置関係である定数部分と、一つの関節によって接続された二つのリンクの相対的な位置を表す変数部分を含んでいる。変数部分は、適当な関節変数の関数、すなわち関節自由度であり、二つのリンクの相対的な位置として再算出される。図４は、リンク変換および関節変換の二つの変換を示している。従って、式６が示す複合変換は以下のように関節変換とリンク変換の積に分解することが出来る。

ここで、Ｘ_Ｊ＝^ｉＸ_Ｊｉは変数部分である関節変換を表し、Ｘ_Ｌ＝^ＪｉＸ_ｐ（ｉ）は定数部分であるリンク変換を表す。

<4.2 ヤコビアン（ヤコビ行列）>
一実施形態において、ターゲットシステム１０４は6次元タスク空間（ｎ≧ｍ、ｍ ＝ ６）でタスクを行う。タスクディスクリプタの位置及び方向は、それぞれベクトル^０ｐと回転行列^０Ｒ によって表される。左上付き文字の表記は基準とする座標系を表すものである。以後ベース座標系を基準とする数量の場合はこの左上付き文字は省略する。もう一つの実施形態では、ターゲットシステム１０４は完全には特定されていない（ｍ＜６）タスクを行う。

ベクトルｑ＝ [ｑ_１ ... ｑ_ｎ]^Ｔが、ターゲットシステム１０４の配置を完全に特徴付ける自由度を表すとする。全ての可能なターゲットシステム１０４の配置のセットは、コンフィギュレーション空間もしくは関節空間と呼ばれる。関節空間速度とタスク空間速度間の写像は、その二つの空間に関連する微分運動学を考慮することによって得られ、次式(７)で表される。

^ｉｖ_ｉ を、座標系ｉを基準座標系とするリンクｉの速度であるとすると、基準となるリンク座標系におけるリンク速度を算出する標準的な反復式は、式(１０)により与えられる。

ここで、ｈ_ｉは６×ｎ_ｉ次元を有するものであり、ボディiの座標系を基準座標系として、ある特定の関節の動作部分空間を表すものである(Roy FeatherstoneおよびDavid Orin著 「Robot dynamics: Equations and algorithms」 International Conference on Robotics and Automation, San Francisco CA, 2000年、なお、この文献に記載される全ての内容をここに引用する)。その動作部分空間は、関節iの自由度を表し、その列はベクトル空間の基底を形成する。例えば、リンクiとリンクｐ（ｉ）をつなぐ回転関節のｚ軸を中心とする回転の動作部分空間は、式（１１）により与えられる。

式（１０）は、式（１２）で表す総和式に変換することも出来る。

さらに詳細には、^ｉＸ_ｋは、次式（１３）で表されるように定義される。

ここで、ｋは ルートからリンクｉへの経路上のリンクの番号を表す。式（７）で表したように、デカルト速度は関節速度と関連付けられており、その関係は次式(１４)で表される。

式（１４）において、ヤコビアン行列はリンク座標系に表され、式（１５）で表すことができる。

ここで、Ｊ_ｋは次の式（１６）によって定義される。

したがって、リンクiのヤコビアン行列は、以下の関係式（１７）に表されるように、親リンクのヤコビアン行列に関係付けられている。

ただし、第一リンクのヤコビアンは、^１Ｊ_１＝ｈ_１によって与えられる。ベース（もしくはルート）座標系を基準座標系とするヤコビアンは、以下の変換式（１８）を使って求めることも出来る。

<動作生成における階層的制御>
次に、所望のモーション・プリミティブのセットから、ターゲットシステム１０４の全自由度に対応する動作を生成するためのタスク空間制御の手法を説明する。これらの所望のモーション・プリミティブは、測定によって観測されたもの、合成されたもの、またはターゲットシステム１０４の現在の配置から算出されたもののいずれかである。動作制御システム３００は、ａ）所望のタスクディスクリプタを軌道追従する算出タスクディスクリプタのセットに動作の結果を反映させる（例えばデカルト空間における追従誤差を最小化する）軌道追従制御システム３０２、ｂ）生成した動作をバランスをとって制御し、ターゲットシステム１０４を不安定な方向へは動かさないようにするバランス制御システム３０６、ｃ）関節制限、速度制限、トルク制限などの物理制限をターゲットシステム１０４が超えないようにするコマンドを提供し、システム３００が障害との衝突や自己衝突を避け、特異姿勢を原因とする計算問題を避ける拘束システム３０４を備える。この三つの制御システム３０２、３０４、３０６は互いに相入れない数多くのタスクを提示することもあるが、それは、階層的タスク管理ストラテジによって解決することが出来る。これは、低優先度（もしくは低重要度）の要素の正確性を犠牲にして、より優先度の高い（もしくはより重要度の高い）要素の正確性を保つということである。

<軌道追従制御>

多様な逆問題の解法が、式(１９)もしくは式（２０）から得ることが出来る。必ずというわけではないが、一般的に一次系の式（１９）に基づく解法は、システム運動学のみを扱うのに対し、式（２０）の二次系に基づく解法は、運動学解析と動力学解析を扱うことが出来る。所望のタスクディスクリプタのセットを所与として、式(１９)と式（２０）に基づき、関節コマンドのセットを生成するための三つの軌道追従制御の公式を以下に説明する。

<一次閉ループ逆運動学 (C L I K)>
図５は、冗長性分解のない一次閉ループ逆運動学システムを実装した動作制御システム３００を示すブロック図である。閉ループ逆運動学 (CLHC)の制御アルゴリズムは、時間に応じて変化する所望のタスクディスクリプタの位置及び方向を追従するための制御コマンドを得るために（例えば、軌道追従の課題のために）使用することが出来る。図５のシステムは、図３の軌道追従制御システム３０２と同様の構造を有していることに注意したい。ここで、式（１９）の一次微分運動学を参照する。一実施形態においては、6次元空間のタスクディスクリプタで所望の動作が与えられる。その所望の動作に関する微分運動学は式（２１）で表される。

ここで、所望の角速度ｗ_ｄは、式(２２)により算出される。

ここで、Ｊ^＊は、正定値行列 Ｗ_１によって重み付けられ、正定値の減衰マトリクスＷ_２によって正則化された、Ｊの正則化右側擬似逆行列を表しており、次式（２４）で表される。

減衰マトリクスは、Ｊが悪条件行列の場合に使用してもよく、その詳細については後述する。Ｗ_２＝０の場合、式（２４）は、単純にＪの加重右側擬似逆行列である。さらに、Ｊが非特異行列である場合、Ｗ_１は単位行列であり、かつＷ_２＝０である場合、行列Ｊ^＊は通常の逆行列Ｊ^−１に置き換えられる。

ここで、Ｋは６×６正定値対角ゲイン行列であり、ｅ＝[ｅ_ｏｅ_ｐ]^Ｔ は、所望のタスクディスクリプタと算出されたタスクディスクリプタとの間の位置誤差（ｅ_ｐ）及び方向誤差（ｅ_ｏ）を表すベクトルである。位置誤差は単純に以下の式（２７）で表される。

ここで、ｐ_ｄとｐはそれぞれ所望のタスク位置と算出されたタスク位置に対応する。

図６は、位置制御システムと方向制御システムを分けて実装した一次閉ループ逆運動学の軌道追従制御システムを示すブロック図である。

方向誤差の算出は、複雑な場合もあり、多様な方向の表現を使用して行われる場合もある。角度誤差と軸誤差に基づいた方法は、後述する式（２８）から（３０）を使用してもよい。

所望のタスク方向フレームは通常最小数の座標で表される。最小数の座標は、典型的には３つのオイラー角であり、ベクトルΘ_ｄで表される。所望のタスク方向はオイラー角以外のパラメータで表してもよいが、オイラー角Θ_ｄは所望の回転行列Ｒ_ｄが既知であれば、常に算出可能であることに留意すべきである。角度誤差と軸誤差の観点から方向誤差を関数で表すと式（２８）のようになる。

ここで、（ｎ_ｄ，ｓ_ｄ，ａ_ｄ）と（ｎ，ｓ，ａ）はそれぞれ、タスクフレームの所望の三つの単位ベクトルと実際の三つの単位ベクトルに対応する(J.Y.S. Luh、M.W. Walker及びR.P.C. Paul著 「Resolved-acceleration control of mechanical manipulators」 IEEE Transactions on Automatic Control, ２５：Ｐ４６８−４７４，年、なお、この文献に記載される全ての内容をここに引用する)。すなわち、次式(２９)、（３０）の関係が成り立つ。

<二次C L l K >
図７は、位置制御システムと方向制御システムを分けて実装した二次閉ループ逆運動学の軌道追従制御システムを示すブロック図である。

一次CLIK軌道追従アルゴリズムは、関節速度の値を求め、その後関節変位の値を求める。多種の制御方法を実現するために、二階微分逆運動学の方程式を使用することで、関節加速度も求めることも出来る。二次微分逆運動学の方程式（２０）に基づいた、関節加速度を求める解法は、式（３１）によって与えられる。

数値的ドリフトを補正するために、位置フィードバック項と速度フィードバック項を式（３１）に加えてもよい。その結果得られる二次逆運動学の制御法則は、式（３３）によって表される。

ただし、式（３３）において、Ｋ_ｐとＫ_ｖは、正定値ゲイン行列であり、次式(３４)で表される関係を使用して臨界減衰反応を生成するように選択することも出来る。

位置及び方向の誤差（ｅ＝[ｅ_ｏ ｅ_ｐ]^Ｔ）は、前述の通り、それぞれ式（２７）と（２８）により定義される。したがって、誤差率は、位置誤差率と方向誤差率に分けられ、次式(３５)によって表される。

<逆動力学制御>
図８は、位置制御システムと方向制御システムを分けて実装した二次閉ループ逆運動学の軌道追従制御システムを示すブロック図である。

式（３３）の二次閉ループ逆運動学の制御式は、逆動力学制御アルゴリズムを構築するために使用することもできる。予測のために力学モデルを使用することと同様に、二次閉ループ逆運動学の制御式に直接的に動力学を組み込むことで、運動学的拘束だけではなく動力学的拘束をも満たす、より現実的で自然な動作を生成することが可能になる。

典型的な多関節体システムにおける動作の動力学方程式は、式(３７)に表される一般形を有する。

式(３７)において、Ｍは、ｎ×ｎ関節空間慣性行列であり、Ｃ はコリオリ力と遠心力トルクのｎ×１ベクトルであり、Ｇは重力トルクのｎ×１ベクトルであり、τは関節トルクのｎ×１ベクトルである。

式(３７)は 関節空間の動力学を表している。一実施形態において、動作制御システム３００は、非線形モデルを基にした補償を使用して、閉ループのシステムを動的に非干渉化して線形化することが出来る。その様な補償ストラテジの一つとして、以下の逆動力学制御法則(３８)を使用することが考えられる。

ここで、カレット記号^Λ は、動力学モデルのパラメータの推定式を表す。式(３８)のモデルパラメータが式(３７)のパラメータと一致するとすると、ベクトルαは、閉ループのシステムを動的に非干渉化して線形化する分解加速度制御の入力を表し、式(３９)により定義される。

次式(４０)で表される通り、関節変数の観点からの分解加速度は、前述したように、式(３３)で表される位置及び方向のフィードバック制御法則を使用して得ることも出来る。

式(３８)により表される逆動力学の制御法則は、関節空間の動力学方程式とタスク空間加速度を利用している。このタイプの制御は、通常分解加速度制御（ＲＡＣ）と呼ばれる。代わりの方法として、制御と同様に、動力学方程式をタスク空間、つまり「操作空間」の視点で表すことが出来る。タスク変数によってロボットの動作を記述するタスク空間（つまり操作空間）動力学は、式(２０)及び式(３７)から導くことが出来る（O. Khatib著 「A unified approach for motion and force control of robot manipulators: The operational space formulation」 IEEE Journal of Robotics and Automation RA- ３（１）：Ｐ４３−５３, １９８７年、 なお、この文献に記載される全ての内容をここに引用する)。操作空間の動力学方程式は以下の構造式（４１）を有している。

式（４１）において、Ｍはｎ×ｎ操作空間慣性行列であり、Ｃ、Ｇ、Ｔはそれぞれ、遠心力とコリオリ力のベクトル、重力ベクトル、操作空間に働くトルク力ベクトルを表す。

分解加速度制御と同じように、動作制御システム１００は、動的に非干渉化された制御を公式化して、操作空間内のタスクとサブタスクを実行することが出来る。操作空間におけるモデルを基にした補償は以下の式（４２）によって与えられる。

<複数タスクの管理>
図９は、ソースモデル上のタスクディスクリプタと一般座標系を示す図である。タスクディスクリプタは、一般座標系の関数として表すことができる任意の量値をとることが出来る。タスクディスクリプタの例としては、ボディセグメントのランドマークの位置、ボディセグメントの方向、全身の重心の位置などが考えられる。所望のタスクディスクリプタは測定により取得もしくは推測される。図９は、測定もしくは推測しやすく、かつタスクの記述に組込みやすい好ましいランドマークを図示している。

一実施形態においては、動作制御システム１００は複数のタスクを同時に実行することが出来る。例えば、複数のタスクディスクリプタが、人体構造上の複数のランドマーク位置を表していてもよい。あるタスクディスクリプタの実行が、その他のタスクディスクリプタよりも重要度が高かったり、優先度が高かったりしてもよい。 例えば、ターゲットシステム１０４が掌握タスクを実行する場合を想定してみる。ソースシステム１０２は、所望の手の位置、所望の肩の位置などの複数のタスクディスクリプタを提供することが出来る。ターゲットシステムが複数タスクを実行できない（条件を満たさない）場合、動作制御システム１００は、例えば、所望の肩の位置を得ることよりも所望の手の位置を得ることを優先するなど、ある動作を達成することに重要度もしくは優先度を与えることが出来る。複数のタスクを管理するための動作制御システム１００の２つの実施形態を以下に説明する。それらは、1)重み付けストラテジと、2)優先順位付与ストラテジである。

<タスクの拡張 重み付けストラテジ>

タスクディスクリプタの各要素の軌道追従誤差はフィードバックゲイン行列によって制御することが出来る。式（２６）における一次システムにおいて、軌道追従誤差の収束速度は、フィードバックゲイン行列 Kの固有値に依存する。すなわち、その固有値が大きいと、収束速度も速くなる。一実施形態においては、前述したような一次システムは、連続時間系の離散時間近似を使用することが出来る。したがって、その固有値には、サンプリング時間によって、上界が存在すると予測することが妥当である。ある特定のタスク (もしくは、ある特定のタスクの特定の方向)は、その特定のタスクに関連付けられたKの固有値を増加させることにより、よりタイトに管理することが出来る。

<優先順位付与ストラテジ>

ここで、Ｊ_ｉはｉ番目のタスクディスクリプタのヤコビアン行列であり、Ｊ^＋は通常、Ｊ^＋＝Ｊ^Ｔ（ＪＪ^Ｔ）^−１で表されるＪの右側擬似逆行列として定義される。二つの所望の動作が優先順位を有する場合の逆運動学の解法は、次の式(４６)により表される。

優先順位を課された２つのタスクに関する前述の手順は、Y. Nakamura著 「Advanced Robotics, Redundancy and Optimization. Adisson- Wesley」 1991年に概略が説明されている。なお、この文献に記載される全ての内容をここに引用する。同様の手順に従うことで、優先順位を有するタスクに関する解法を、３つ以上のタスクに適用することもできる。

<5. 3 バランス制御>
図１０は、推定されたタスク変数としてバランスディスクリプタを含む一次閉ループ逆運動学システムを示すブロック図である。

動作制御システム３００は、所望のタスク変数を直接観測もしくは合成することも可能であり、ある特定の動作を生成するための所望のタスクディスクリプタを生成する制御法則により「推定」することも可能である。その様な所望のタスクディスクリプタは、通常ターゲットシステム１０４の現在の状態をモニタし、所望のタスクディスクリプタを生成するシステムによって推定されることから、本明細書においては「推定」タスクディスクリプタとも呼ぶこととする。バランス保持は、推定されたバランスディスクリプタによって制御可能な行為の一例である。

一実施形態において、ターゲットシステム１０４は人間もしくは人型ロボットを表す。ゼロモーメントポイント(ZMP)や全身の重心などのバランス基準が、ｑの関数である所望のバランスディスクリプタを生成するために使われる。体心の位置を指示するコマンドは、部分的にバランスを制御するための有効なタスクディスクリプタの一つである。

ｐ_ｃｍが、全身の重心の位置を表し、式(４８)によって算出されるものとする。

ただし、Ｍは全体重であり、ｍ_ｉとｐ_ｃｍｉ はそれぞれ、セグメントiの質量とセグメント iの重心の位置に対応する。前述したように、重心速度と関節速度の関係は、重心ヤコビアン行列Ｊ_ｃｍを通じて得ることができ、次式(４９)で表される。

ただし、式(５０)、(５１)において、説明の便宜上、観測されたディスクリプタとバランシング・ディスクリプタは垂直の分離線によって区分けしている。一実施形態においては、多くの用途でバランシング・ディスクリプタは第一優先度もしくは最重要度を有する。

<拘束>
一実施形態において、ターゲットシステム１０４は、満たすべき運動学的拘束と動力学的拘束を有する。関節制限、自分自身や周りの環境との衝突を避ける拘束は、運動学的拘束の例である。正則化（regularization）をしない場合、特異姿勢もターゲットシステム１０４が通過することができるワークスペースの許容域に拘束を課す。さらに、ターゲットシステム１０４は、許容可能な関節速度および関節トルクが制限されている場合もある。このことは、ターゲットシステム１０４が、アクチュエータの速度制限やトルク制限が重要なロボットである場合に特に当てはまる。これらの拘束は、動作の（自由度の）冗長性を有効に利用するために、ベクトルｖ_ｎを特定することで、零空間において対処される場合もある。これらの拘束は、最も優先度が高く設定される場合もあり、その場合、拘束は第一のタスクとして使用される。

拘束を零空間で対処する場合、動作制御システム３００は、動作の（自由度の）冗長性を有効に利用するために、目的関数ｗ（ｑ）を構築して、ベクトルｖ_ｎを特定することが出来る。ただし、この目的関数ｗ（ｑ）の勾配は、ベクトルｖ_ｎを次式(５２)で表されるように定義するものとする。

ここで、ｋ_０＞ ０である。解は、その目的関数の勾配の方向に沿って移動するため、第一の目的を満たしながら、目的関数を局所的に最大化しようとする(運動学的拘束)。動作制御システム３００は、例えば、特異姿勢の回避、関節制限の回避、衝突の回避などのために目的関数を使用することが出来る。

<特異姿勢の対処>

特異姿勢の問題を克服するためのいくつかの実施形態がある。一実施形態においては、動作制御システム１００はタスクの可操作性の尺度であるポテンシャル関数を使用することで、特異点から離れた軌道を計算する。一実施形態においては、ターゲットシステム１０４の装置は、特異姿勢ロバスト逆ヤコビアン行列の使用により、特異点やその近傍を通過可能にすることも出来る（Y. Nakamura及び H. Hanafusa著 「Inverse kinematic solution with singularity robustness for robot manipulator control」, ASMEJ. Dyn. Sys. Meas., Contr., １０８（３）：Ｐ１６３−１７１、１９８６年、なお、この文献に記載される全ての内容をここに引用する）。また、この特異姿勢ロバスト逆ヤコビアン行列は、減衰最小二乗法（DLS）としても知られている（C. W. Wampler著「Manipulator inverse kinematic solutions based on vector formulations and damped least squares methods」, IEEE Trans. Sys., Man, Cyber., １６（１）：Ｐ９３−１０１、１９８６年、なお、この文献に記載される全ての内容をここに引用する）。ヤコビアンの転置を行列反転の代わりに使用することも可能であり、ヤコビアンの転置は、顕著に計算量を削減するとともに、特異姿勢の対処に関して、減衰最小二乗法を使った逆問題解決法に代わる手法として使用することもできる（L. Sciavicco及びB. Siciliano著 「A solution algorithm to the inverse kinematic problem for redundant manipulators」IEEE Journal of Robotics and Automation, ４：Ｐ４０３−４１０ １９８８年、なお、この文献に記載される全ての内容をここに引用する）。次に、減衰最小二乗法について詳述する。

減衰最小二乗法の最も一般的な形は式(２４)によって定義されるものであり、以下に式(５３)として繰り返して記載する。

ここで、Ｗ_２＝λ^２Ｉが減衰項、λ＞０ は減衰係数、Ｉは単位行列である。Ｗ_１が単位行列である場合、式(５３)は次式(５４)の最適化条件を満たす。

λが小さい場合、正確な解が求められるが、特異姿勢や特異姿勢に近い姿勢の発生に対してロバスト性が低くなる。λを大きい値にすると、実現可能で正確な解がある場合でも、軌道追従の精度が低くなってしまう。減衰係数は上記の２つの目的の相対的な重要度を決定している。λに定数値を選ぶと、「ターゲット」システムのワークスペース全体に渡って高いパフォーマンスを発揮するには不十分である場合がある。現在の「ターゲット」システムの配置の特異姿勢への近さの度合いに応じて、適応的に減衰係数を選択する手法もある（S. Buss 及び J. S. Kim著「Selectively damped least squares for inverse kinematics」 Journal of Graphics Tools １０：３, Ｐ３７−４９、2005年 なお、この文献に記載される全ての内容をここに引用する) (A. A. Maciejewski 及び C. A. Klein著 「Obstacle avoidance for kinematically redundant manipulators in dynamically varying environments」International Journal of Robotics Research, ４：Ｐ１０９−１１７, １９８５年 なお、この文献に記載される全ての内容をここに引用する)。

<関節制限の回避>
動作制御システム１００は、関節制限を回避するためにいくつかの方法を使用することが出来る。その一つは勾配投影法である。勾配投影法においては、パフォーマンス基準が関節制限の関数として定義され、その勾配がヤコビアンの零空間投射行列に投射され、パフォーマンス基準を最適化するのに必要なセルフモーションを得る。第二の方法は、関節制限を避けるための重み付き最小ノルム解(WLN)である。重み付き最小ノルム解は、T. R Chan とR. V. Dubeyによって、「A weighted least-norm solution based scheme for avoiding joint limit for redundant joint manipulators」IEEE Transactions on Robotics and Automation, １１（２）, １９９５年 で最初に提案されている。なお、この文献に記載される全ての内容をここに引用する。重み付き最小ノルム解は、減衰最小二乗法に関連して公式化される。

ここで、式(２４)の重み付き減衰最小二乗法のヤコビアンを再度考慮する。Ｗ_１は、関節制限を回避するために使われる対角行列であり、次式(５５)により定義されるものとする。

ここで、ｗ_i(スカラー)は、Ｗ_１の対角成分に対応し、次式(５６)により定義される。

<衝突回避>
ターゲットシステム１０４は、自身や他の障害との衝突回避により、動作を安全に実行することが出来る。衝突回避は、二組の衝突点のペアが衝突に向けて近づいている各時刻において、その衝突点を求めることによって行うことが出来る。衝突点のペアにおけるそれぞれの衝突点は、「反応」衝突点または「受動」衝突点のどちらかに定義される。反応衝突点は、衝突回避のためのタスクディスクリプタとしてみなし、他のタスクディスクリプタと同じ方法で制御することができる。そのため、反応衝突点は衝突回避タスクディスクリプタを表すともいえ、その所望の動作は衝突を回避するための制御法則に基づいて求められる。受動衝突点は、衝突回避において、明確には制御されない点である。受動衝突点は、外部システム（例えば、環境や障害）もしくはターゲットシステム１０４上の異なった剛体に与えられる。一実施形態において、動作制御システム１００は外部システムに与えられた衝突点を受動的なものとして処理する。この実施形態は、例えば、衝突点がターゲットシステム１０４の内部制御システムには制御不可能な場合に用いることが出来る。

衝突点のペアにおける衝突点が両方ともターゲットシステム１０４に与えられた場合(自己衝突)、「反応」衝突点と「受動」衝突点の指定はより複雑になる。この衝突点の指定の現実的な方法としては、衝突点を衝突から回避するように動かすために必要なエネルギーの最小化に関する評価指標を考慮することが考えられる。考えられる評価指標の一つは、式(２４)において、関節空間の慣性行列であるＭをＷ_１に代入して、逆運動学的もしくは逆動力学的解法に関する慣性重み付け逆ヤコビアン行列を考慮することである。この解法では、タスクを実行するのに必要な運動学的エネルギーを最小化する解が求められるが、計算量が多い関節空間の慣性行列の計算を必要とするため、閉ループ逆運動学の解法には現実的ではない場合がある。コンピュータ的により実現性が高く、アルゴリズム的に単純な手法は、衝突点のペアに関連付けられた剛体の相対的な質量を使うものである。すなわち、より大きな質量の剛体に与えられる衝突点を受動衝突点とし、より小さな質量の剛体に与えられる衝突点を反応衝突点とするものである。例えば、手と胴体が衝突する場合を考えてみる。この場合、胴体の衝突点には受動衝突点が指定され、手の衝突点には反応衝突点が指定される。

反応衝突点と受動衝突点が指定されると、算出される衝突回避タスクディスクリプタ速度は次式(５９)により定義される。

衝突回避システムは、衝突までの距離ｄをモニタし、その衝突までの距離ｄが閾値ｄ_{ｔｈｒｅｓｈ}よりも低い場合ｄ＜ ｄ_{ｔｈｒｅｓｈ}、衝突点の衝突方向への移動を停止するように、所望の衝突回避タスクディスクリプタを生成する制御法則を定めることもできる。衝突回避システムは、次式(６２)で表すように、所望の速度を、衝突点の現在の方向と同じ大きさで、反対方向となるように指示してもよい。

<関節速度の制限>
通常、ターゲットシステム１０４の各自由度の許容可能な関節速度には制限があり、その制限は関節を駆動するアクチュエータの物理的限界によって決められる場合が多い。ロボット工学においては、関節速度が上限に達した時、その関節速度を固定することによって、関節速度の拘束に対処することもできる。それに代わる手法としては、減衰係数を調節することによって、上述した減衰最小二乗法と同様の手法で速度を制御することもできる。この手法は、動作全体の実行に要する時間を維持することができるが、元の軌道を維持することができず、結果的に生じる動作の軌道は元の動作の軌道から大きく変わってしまう場合がある。ここでは、元の関節動作を保ちつつ、２つの連続するサンプルの間の時間を適応的に調節することで、関節速度を制限する方法を説明する。

ここで、Δｔ_ｋ はサンプル時間である。関節制限を回避するための時間調節スキームは、式(６３)の時間系列を次式(６４)に置き換える。

時間導関数の一次有限差分近似は、次の式(６７)で表される関係を有する。

上記の条件１から３を満たす時間調整スキーマは、次式（７０）にまとめることができる。

上記の例は、元の動作の軌跡を維持しているが、当然、動作の実行に要する時間が延びることがある。時間調整に関する他の実施形態においては、全体の動作実行時間と動作の軌跡の両方を保つために、関節速度が限界値を超える場合には延長され、限界値未満の場合は短縮される Δt_k を使用してもよい。これにより、動作の全体の実行時間を変えることなく、動作を行うことが出来る。

特定の実施形態および用途を例示して本発明を説明したが、本発明は、ここに開示された厳密な構成および構成要素のみに限定されるのではなく、本発明の付属の請求項に定義される技術的な思想と範囲を逸脱しない限りにおいて、各種の修正、変更あるいは変形を、構成、運用、本発明の方法および装置の詳細について行うことが可能である。

本発明の１つの実施形態による、ロボットやバイオロボットシステムなどのターゲットシステムを制御するための動作制御システムを示すブロック図である。 ソースモデルとターゲットモデル間における、１つのタスクディスクリプタの関連付けを示す図である。 観測したタスクディスクリプタから動作を生成する動作制御システムを示すブロック図である。 図１のターゲットシステムのある関節に関連付けられた座標系及び座標変換を示す図である。 冗長性分解のない一次閉ループ逆運動学システムを示すブロック図である。 位置制御システムと方向制御システムを分けて実装した一次閉ループ逆運動学の軌道追従制御システムを示すブロック図である。 位置制御システムと方向制御システムを分けて実装した二次閉ループ逆運動学の軌道追従制御を含むシステム示すブロック図である。 位置制御システムと方向制御システムを分けて実装した二次閉ループ逆運動学の軌道追従制御システムを示すブロック図である。 ソースモデル上のタスクディスクリプタと一般座標系を示す図である。 推測されたタスク変数としてバランスディスクリプタを含む一次閉ループ逆運動学システムを示すブロック図である。

符号の説明

１００ 動作制御システム
１０２ ソースシステム
１０４ ターゲットシステム
１０８ タスクディスクリプタ
１１０ 関節変数
２０１ ソースモデル
２０２ ターゲットモデル
３００ 動作制御システム
３０４ 拘束システム
３０６ バランス制御システム
３１０ 位置/方向誤差検出システム
３１２ 順運動学システム
３１４ 制御法則システム
３１６ 予測システム
３２２ 衝突回避システム
３２３ トルク制限システム３２３
３２４ 特異姿勢ロバストシステム
３２６ 関節速度制限システム
３２８ 関節制限システム

Claims (33)

1. 多関節システムの部位の動作を制御する方法であって、
   ソースシステムの動作を追従するステップと、
   前記多関節システムの前記部位の動作に拘束を課すステップと、
   前記追従したソースシステムの動作と前記拘束に応じて、前記多関節システムの前記部位の動作を制御するコマンドを生成するステップとを含む
   ことを特徴とする方法。
2. 前記コマンドに応じて、バランス制御コマンドを生成するステップをさらに含むことを特徴とする請求項１に記載の方法。
3. 前記ソースシステムの動作を追従するステップは、一次閉ループ逆運動学の使用を含むことを特徴とする請求項１に記載の方法。
4. 前記ソースシステムの動作を追従するステップは、前記多関節システムにおける変数の微分運動学を記述するために使われる逆ヤコビアン行列を正則化することをさらに含むことを特徴とする請求項３に記載の方法。
5. 前記ソースシステムの動作を追従するステップは、二次閉ループ逆運動学の使用を含むことを特徴とする請求項１に記載の方法。
6. 前記ソースシステムの動作を追従するステップは、前記多関節システムにおける変数の微分運動学を記述するために使われる逆ヤコビアン行列を正則化することをさらに含むことを特徴とする請求項５に記載の方法。
7. 前記コマンドを生成するステップは、
   前記コマンドの結果と追従した前記ソースシステムの前記動作の間の誤差を生成し、
   前記コマンドと前記拘束に応じて、制御法則に従って制御コマンドを生成し、
   前記制御コマンドに応じて前記多関節システムの動作の予測を生成し、
   前記予測に応じて前記コマンドを生成することを含む
   ことを特徴とする請求項１に記載の方法。
8. 前記コマンドの前記結果を生成するために、前記コマンドの順運動学を計算するステップをさらに含むことを特徴とする請求項７に記載の方法。
9. 前記拘束は、タスクを実行するための自由度が不十分であることによるタスク実行の制限から生じる特異姿勢を含むことを特徴とする請求項１に記載の方法。
10. 前記拘束は、制御コマンドを生成する際の数学的原因により生じる特異姿勢を含むことを特徴とする請求項１に記載の方法。
11. 前記多関節システムが、人間モデルであることを特徴とする請求項１に記載の方法。
12. 前記多関節システムがロボットであることを特徴とする請求項１に記載の方法。
13. 前記拘束を課すステップは、
    前記多関節システムの部位の動作の速度が閾値を超える場合、コマンドを生成する際のサンプル時間の間隔を調整するステップを含むことを特徴とする請求項１に記載の方法。
14. 前記追従したソースシステムの動作は、画像やデータから得られる人間の特徴を含むことを特徴とする請求項１に記載の方法。
15. 多関節システムの部位の動作を制御するシステムであって、
    ソースシステムの動作を追従する追従システムと、
    前記多関節システムの前記部位の動作に拘束を課す拘束システムと、
    前記追従したソースシステムの動作と前記拘束に応じて、前記多関節システムの前記部位の動作を制御するコマンドを生成する制御システムとを含む
    ことを特徴とするシステム。
16. 多関節システムが人間の動作を複製するための、動作の追従方法であって、
    人間の部位にタスク変数を与えるステップと、
    前記タスク変数によって表される前記人間の部位の動作を検出するステップと、
    前記多関節システムが前記人間の動作を複製するためのコマンドを生成するステップと、
    前記多関節システムが動作を実行する際の物理的制限に応じて、前記動作を複製するための前記コマンドを調整するステップとを含む
    ことを特徴とする方法。
17. 前記タスク変数は、前記人間の動作の自由度とは異なる次元を有することを特徴とする請求項１６に記載の方法。
18. 前記物理的制限は、関節制限、トルク制限、速度制限、衝突回避を含むことを特徴とする請求項１６に記載の方法。
19. 前記動作の実行中における前記多関節システムのバランス保持のために、前記人間の動作を複製するための前記コマンドを調整するステップをさらに含むことを特徴とする請求項１６に記載の方法。
20. 前記物理的制限が特異姿勢を含むことを特徴とする請求項１６に記載の方法。
21. 前記人間の前記部位に与えられた前記タスク変数と前記コマンド間の位置および方向の誤差に応じて前記コマンドを調節するステップをさらに含むことを特徴とする請求項１６に記載の方法。
22. 前記多関節システムが人間モデルであることを特徴とする請求項１６に記載の方法。
23. 前記多関節システムがロボットであることを特徴とする請求項１６に記載の方法。
24. 前記多関節システムの衝突点を決定するステップと、
    前記衝突点に応じて前記動作を複製するコマンドを調節するステップとをさらに含む
    ことを特徴とする請求項１６に記載の方法。
25. 前記衝突点が受動衝突点と反応衝突点であることを特徴とする請求項２４に記載の動作の追従方法。
26. 多関節システムが人間の動作を複製するための、動作の追従システムであって、
    人間の部位にタスク変数を与える第一の制御システムと、
    前記タスク変数によって表される前記人間の前記部位の動作を検出する第二の制御システムと、
    前記多関節システムが前記人間の動作を複製するためのコマンドを生成する第三の制御システムと、
    前記多関節システムが動作を実行する際の物理的制限に応じて、前記動作を複製するための前記コマンドを調整する第四の制御システムとを含む
    ことを特徴とするシステム。
27. 多関節システムが人間の動作を複製するための、動作の追従方法であって、
    人間の部位にタスク変数を与えるステップと、
    前記タスク変数によって表される前記人間の前記部位の動作を検出するステップと、
    多関節システムが前記人間の動作を複製するためのコマンドを生成するステップと、
    前記多関節システムが動作を実行するための前記タスク変数の優先傾向に応じて、前記動作を複製するための前記コマンドを調整するステップとを含む
    ことを特徴とする動作の追従方法。
28. 前記コマンドを調節するステップは、前記タスクの優先度を基に前記タスク変数に優先傾向を与えることを含むことを特徴とする請求項２７に記載の動作の追従方法。
29. 前記コマンドを調節するステップは、前記タスクの重み付けを基に前記タスク変数に優先傾向を与えることを含むことを特徴とする請求項２７に記載の動作の追従方法。
30. 多関節システムが人間の動作を複製するための、動作の追従システムであって、
    人間の部位にタスク変数を与える第一の制御システムと、
    前記タスク変数によって表される前記人間の前記部位の動作を検出する第二の制御システムと、
    前記多関節システムが前記人間の動作を複製するためのコマンドを生成する第三の制御システムと、
    前記多関節システムが動作を実行する際の前記タスク変数の優先傾向に応じて、前記動作を複製するための前記コマンドを調整する第四の制御システムとを含む
    ことを特徴とするシステム。
31. 多関節システムの部位の動きを制御する方法であって、
    ソースシステムの動作を追従するステップと、
    前記多関節システムの前記部位の動きに拘束を生成するステップと、
    前記追従したソースシステムの動作と前記拘束に応じて、前記多関節システムの前記部位の動きを制御するための力を推定するステップと、
    前記追従したソースシステムの動作と前記拘束と前記力に応じて、前記多関節システムの前記部位の動きを制御するためのトルクを推定するステップとを含む
    ことを特徴とする方法。
32. 対象の動作を多関節システムにリターゲットする方法であって、
    前記対象の動作を表す複数のタスクディスクリプタを生成するステップと、
    前記タスクディスクリプタに応じて、前記多関節システムの部位の動作のための関節変数を生成するステップとを含み、
    前記タスクディスクリプタの次元は、前記対象の動作の自由度とは異なる場合もあることを特徴とする方法。
33. 前記関節変数を生成するステップは、タスクを階層的に管理することを含むことを特徴とする請求項３２に記載の方法。
    

Images