JP2014180751A - ロボット及びその制御方法 - Google Patents

Abstract

【課題】 従来の制御方式では不可能であった作業を行うことができるロボットを提供する。
【解決手段】 ガイドチューブと、ガイドチューブと連動して動作し、ガイドチューブから分岐する複数のツールとを有するマルチツールモジュールであって、冗長自由度を有するマルチツールモジュールと；複数のツールの末端部に対する作業空間でのモーション命令情報に基づいてマルチツールモジュールに対する関節空間でのモーションに関する制御信号を生成し、且つ冗長自由度を活用して制御信号を生成する制御部と；を含み、ツールの作業領域を拡張させ、通常的なツール制御方式では不可能な作業を行うことができ、ツールの関節別必要剛性を最小化し、ツールと周辺障害物との間の衝突可能性を最小化することができ、ツールの必要自由度を最小化し、従来不可能であった複合的な作業を行うことができる。
【選択図】 図１２

Description

複数のツールを有するロボット及びその制御方法に関する。

一般的に、電気的または磁気的な作用を利用して人間の動作と類似な運動を行う機械装置をロボットと言う。最近になって、ロボットは、制御技術の発達に伴い、多様な分野で活用されており、その例としては、手術ロボット、家事コンパニオンロボット、公共場所用サービスロボット、宇宙航空遠隔ロボット、危険物処理ロボットなどを挙げることができる。このようなロボットは、電気的・機械的メカニズムによって腕や手の動作に近く運動することができるように作られたマニピュレータを利用して作業を行う。

前述した多様なロボットのうち手術ロボットは、操作者（主として医者）の操作によって必要な信号を生成して伝送するマスター装置と、マスター装置から信号を受けて直接患者に対して手術に必要な操作を行うスレーブロボットとで構成され、マスター装置とスレーブロボットを有線、無線方式で連結し、遠隔で手術を進行するようになる。

従来の手術ロボットを利用した最小侵襲手術（Ｍｉｎｉｍａｌｌｙ Ｉｎｖａｓｉｖｅ Ｓｕｒｇｅｒｙ）は、末端部にエンドエフェクタが付着された複数の手術器具を複数の切開孔を通じて患者の体内に進入させる方式のマルチポート手術（Ｍｕｌｔｉ−Ｐｏｒｔ Ｓｕｒｇｅｒｙ）が主であったが、最近になって、複数の手術器具を１つの切開孔を通じて患者の体内に進入させる方式のシングルポート手術（Ｓｉｎｇｌｅ−Ｐｏｒｔ Ｓｕｒｇｅｒｙ）を施行する事例が増加している。シングルポート手術は、マルチポート手術に比べて治癒期間が短く、外観上、手術跡が少なく残るなどいろいろ有利な点があるが、手術器具間の干渉が発生する可能性があり、手術器具の作業領域（ｗｏｒｋｓｐａｃｅ）に制限が多く、適用することができる手術の範囲が限定されるという問題点がある。

冗長自由度（ｒｅｄｕｎｄａｎｃｙ）を活用することによって従来の制御方式では不可能であった作業を行うことができるロボットを提案する。

ロボットは、ガイドチューブと、ガイドチューブと連動して動作し、ガイドチューブから分岐する複数のツールとを有するマルチツールモジュールであって、冗長自由度を有するマルチツールモジュールと；複数のツールの末端部に対する作業空間でのモーション命令情報に基づいてマルチツールモジュールに対する関節空間でのモーションに関する制御信号を生成し、且つ冗長自由度を活用して制御信号を生成する制御部と；を含む。

本発明のロボット及びその制御方法によれば、マウンティングアームと、マウンティングアームに装着されるガイドチューブ、及びそれぞれエンドエフェクタを有し、ガイドチューブから分岐して出る複数のツールを含み、マウンティングアーム、ガイドチューブ及び複数のツールが連動して動作するように制御するロボットにおいて、近位部、すなわちマウンティングアーム及びガイドチューブの自由度によって発生する冗長自由度を活用することによって、従来のツール制御方式では不可能な作業を行うことができる。

手術ロボットの全体構造を示す斜視図である。 図１に示す“Ａ”部分の内部様子を示す図である。 手術ロボットを成すスレーブロボットの外観斜視図である。 ケーシング内に内蔵されるマルチツールモジュール及び駆動部の構造を示す図である。 図３Ｂに示す“Ｂ”部分（ガイドチューブ２１２の末端部）で２つのツールと１つの内視鏡が展開された様子を示す図である。 手術ロボットの制御ブロック図である。 本発明によって達成されるツールの作業領域の拡張を説明するための概念図である。 本発明によって達成されるツールの関節別必要剛性の最小化を説明するための概念図である。 本発明によって達成されるツールと周辺障害物との間の衝突可能性の最小化を説明するための概念図である。 本発明によって達成されるツールの必要自由度の最小化を説明するための概念図である。 本発明によって達成される複合的な作業実行を説明するための概念図である。 手術ロボットの制御方法を示す流れ図である。 手術ロボットの制御ブロック図である。 手術ロボットの制御方法を示す流れ図である。 ヒューマノイドロボットの外観構成図である。 ヒューマノイドロボットの制御ブロック図である。 ヒューマノイドロボットの制御方法を示す流れ図である。 ヒューマノイドロボットの制御ブロック図である。 ヒューマノイドロボットの制御方法を示す流れ図である。

＜実施の形態の概要＞
マウンティングアームと、マウンティングアームに装着されるガイドチューブ、及びそれぞれエンドエフェクタを有し、ガイドチューブから分岐して出る複数のツールを含み、マウンティングアーム、ガイドチューブ及び複数のツールが連動して動作するように制御するロボットにおいて、近位部（身中心部、ｐｒｏｘｉｍａｌ ｅｎｄ）、すなわちマウンティングアーム及びガイドチューブの自由度によって発生する冗長自由度（ｒｅｄｕｎｄａｎｃｙ）を活用することによって、ツールの活動領域（ｗｏｒｋｓｐａｃｅ）を拡張させ、通常的なツール制御方式では不可能な作業を行うことができるロボット及びその制御方法を提案する。

また、マウンティングアームと、マウンティングアームに装着されるガイドチューブ、及びそれぞれエンドエフェクタを有し、ガイドチューブから分岐して出る複数のツールを含み、マウンティングアーム、ガイドチューブ及び複数のツールが連動して動作するように制御するロボットにおいて、近位部、すなわちマウンティングアーム及びガイドチューブの自由度によって発生する冗長自由度を活用することによって、ツールの関節別必要剛性を最小化し、ツールと周辺障害物間の衝突可能性を最小化することができるロボット及びその制御方法を提案する。

また、マウンティングアームと、マウンティングアームに装着されるガイドチューブ、及びそれぞれエンドエフェクタを有し、ガイドチューブから分岐して出る複数のツールを含み、マウンティングアーム、ガイドチューブ及び複数のツールが連動して動作するように制御するロボットにおいて、近位部、すなわちマウンティングアーム及びガイドチューブの自由度によって発生する冗長自由度を活用することによって、ツールの必要自由度を最小化し、従来に不可能であった複合的な作業（ｃｏｍｐｌｅｘ ｔａｓｋ）を行うことができるロボット及びその制御方法を提案する。

ロボットは、ガイドチューブと、ガイドチューブと連動して動作し、ガイドチューブから分岐する複数のツールとを有するマルチツールモジュールであって、冗長自由度を有するマルチツールモジュールと；複数のツールの末端部に対する作業空間でのモーション命令情報に基づいてマルチツールモジュールに対する関節空間でのモーションに関する制御信号を生成し、且つ冗長自由度を活用して制御信号を生成する制御部と；を含み、冗長自由度は、関節空間でのマルチツールモジュールの自由度の数が作業空間での自由度の数より大きい場合を表す。

また、ガイドチューブ及び複数のツールそれぞれは、複数のリンク及び複数の関節を含み、複数のツールの各末端部には、エンドエフェクタが装着される。

また、制御部は、複数のツールの末端部に対する作業空間でのモーション命令情報に対応するヤコビアン行列を算出し、複数のツールの末端部に対する作業空間でのモーション命令情報及び算出されたヤコビアン行列に基づいて冗長自由度を活用してマルチツールモジュールに対する関節空間でのモーションに関する制御信号を生成する。

また、制御部は、制御信号の生成過程でロボットの目的関数を算出し、且つ目的関数は、複数の個別目的関数の加重和で表現される。

また、個別目的関数は、それぞれのツールと器具的限界（ｊｏｉｎｔ ｌｉｍｉｔ）との間の距離の逆数、それぞれのツールと特異姿勢（ｓｉｎｇｕｌａｒ ｐｏｓｅ）との間の距離の逆数、各関節のトルク二乗の和及びそれぞれのツールと周辺障害物との間の距離の逆数を含む。

また、制御部は、算出されたロボットの目的関数を最適化又は次善化する(optimize or suboptimize)関節空間でのモーションに関する制御信号を生成する。

また、ヤコビアン行列算出時に必要とするアルゴリズム及びロボットの運動学的構造に関する情報、目的関数算出時に必要とする複数の個別目的それぞれを達成するための複数の個別目的関数、ロボットが行う作業に応じて各個別目的関数に乗算される加重値を格納する格納部をさらに含む。

また、制御部は、前記複数のツールの末端部に対する作業空間でのモーション命令情報に優先順位が設定されているとき、優先順位が高い作業空間でのモーション命令情報及び優先順位が低い作業空間でのモーション命令情報に対応するヤコビアン行列を算出し、優先順位が設定された作業空間でのモーション命令情報及び算出されたヤコビアン行列に基づいて冗長自由度を活用し、マルチツールモジュールに対する関節空間でのモーションに関する前記制御信号を生成する。

また、制御部は、制御信号の生成過程でロボットの目的関数を算出し、且つ目的関数は、複数の個別目的関数の加重和で表現される。

また、個別目的関数は、それぞれのツールと器具的限界（ｊｏｉｎｔ ｌｉｍｉｔ）との間の距離の逆数、それぞれのツールと特異姿勢（ｓｉｎｇｕｌａｒ ｐｏｓｅ）との間の距離の逆数、各関節のトルク二乗の和及びそれぞれのツールと周辺障害物との間の距離の逆数を含む。

また、制御部は、算出されたロボットの目的関数を最小化する関節空間でのモーションに関する制御信号を生成する。

また、複数のツールの末端部に対する作業空間でのモーション命令情報に設定された優先順位、ヤコビアン行列算出時に必要とするアルゴリズム及びロボットの運動学的構造に関する情報、目的関数算出時に必要とする複数の個別目的それぞれを達成するための複数の個別目的関数、ロボットが行う作業に応じて各個別目的関数に乗算される加重値を格納する格納部をさらに含む。

ロボットの制御方法は、ガイドチューブと、ガイドチューブと連動して動作し、ガイドチューブから分岐する複数のツールとよりなるマルチツールモジュールを含むロボットの制御方法において、マルチツールモジュールは、冗長自由度を有し、複数のツールの末端部に対する作業空間でのモーション命令情報を生成し；生成された作業空間でのモーション命令情報に基づいてマルチツールモジュールに対する関節空間でのモーションに関する制御信号を生成し、且つ冗長自由度を活用して制御信号を生成し、冗長自由度は、関節空間でのマルチツールモジュールの自由度の数が作業空間での自由度の数より大きい場合を表す。

また、ガイドチューブ及び複数のツールそれぞれは、複数のリンク及び複数の関節を含んでなり、複数のツールの各末端部には、エンドエフェクタが装着される。

また、冗長自由度を活用してマルチツールモジュールに対する関節空間でのモーションに関する制御信号を生成することは、複数のツールの末端部に対する作業空間でのモーション命令情報に対応するヤコビアン行列を算出し、複数のツールの末端部に対する作業空間でのモーション命令情報及び算出されたヤコビアン行列に基づいて制御信号を生成することである。

また、冗長自由度を活用して前記マルチツールモジュールに対する関節空間でのモーションに関する前記制御信号を生成することは、制御信号の生成過程で前記ロボットの目的関数を算出することであって、目的関数は、複数の個別目的関数の加重和で表現される。

また、冗長自由度を活用してマルチツールモジュールに対する関節空間でのモーションに関する制御信号を生成することは、ロボットの目的関数を算出し、算出されたロボットの目的関数を最適化又は次善化する(optimize or suboptimize)関節空間でのモーションに関する制御信号を生成することである。

また、個別目的関数は、それぞれのツールと器具的限界（ｊｏｉｎｔ ｌｉｍｉｔ）との間の距離の逆数、それぞれのツールと特異姿勢（ｓｉｎｇｕｌａｒ ｐｏｓｅ）との間の距離の逆数、各関節のトルク二乗の和及びそれぞれのツールと周辺障害物との間の距離の逆数を含む。

また、冗長自由度を活用してマルチツールモジュールに対する関節空間でのモーションに関する制御信号を生成することは、複数のツールの末端部に対する作業空間でのモーション命令情報に優先順位が設定されているとき、優先順位が高い作業空間でのモーション命令情報及び優先順位が低い作業空間でのモーション命令情報に対応するヤコビアン行列を算出し、優先順位が設定された作業空間でのモーション命令情報及び算出されたヤコビアン行列に基づいて冗長自由度を活用してマルチツールモジュールに対する関節空間でのモーションに関する制御信号を生成する。

また、冗長自由度を活用して前記マルチツールモジュールに対する関節空間でのモーションに関する前記制御信号を生成することは、制御信号の生成過程で前記ロボットの目的関数を算出することであり、目的関数は、複数の個別目的関数の加重和で表現される。

また、冗長自由度を活用してマルチツールモジュールに対する関節空間でのモーションに関する制御信号を生成することは、ロボットの目的関数を算出し、算出されたロボットの目的関数を最適化又は次善化する(optimize or suboptimize)関節空間でのモーションに関する制御信号を生成することである。

また、個別目的関数は、それぞれのツールと器具的限界（ｊｏｉｎｔ ｌｉｍｉｔ）との間の距離の逆数、それぞれのツールと特異姿勢（ｓｉｎｇｕｌａｒ ｐｏｓｅ）との間の距離の逆数、各関節のトルク二乗の和及びそれぞれのツールと周辺障害物との間の距離の逆数を含む。

本実施形態のロボット及びその制御方法によれば、マウンティングアームと、マウンティングアームに装着されるガイドチューブ、及びそれぞれエンドエフェクタを有し、ガイドチューブから分岐して出る複数のツールを含み、マウンティングアーム、ガイドチューブ及び複数のツールが連動して動作するように制御するロボットにおいて、近位部、すなわちマウンティングアーム及びガイドチューブの自由度によって発生する冗長自由度を活用することによって、ツールの活動領域（ｗｏｒｋｓｐａｃｅ）を増加させ、通常的なツール制御方式では不可能な作業を行うことができる。

また、本実施形態のロボット及びその制御方法によれば、マウンティングアーム、マウンティングアームに装着されるガイドチューブ、及びそれぞれエンドエフェクタを有し、ガイドチューブから分岐して出る複数のツールを含み、マウンティングアーム、ガイドチューブ及び複数のツールが連動して動作するように制御するロボットにおいて、近位部、すなわちマウンティングアーム及びガイドチューブの自由度によって発生する冗長自由度を活用することによって、ツールの関節別必要剛性を最小化し、ツールと周辺障害物との間の衝突可能性を最小化することができる。

また、本実施形態のロボット及びその制御方法によれば、マウンティングアーム、マウンティングアームに装着されるガイドチューブ、及びそれぞれエンドエフェクタを有し、ガイドチューブから分岐して出る複数のツールを含み、マウンティングアーム、ガイドチューブ及び複数のツールが連動して動作するように制御するロボットにおいて、近位部、すなわちマウンティングアーム及びガイドチューブの自由度によって発生する冗長自由度を活用することによって、ツールの必要自由度を最小化し、従来不可能であった複合的な作業を行うことができる。

＜実施の形態の詳細な説明＞
以下では、添付の図面を参照して実施例を詳細に説明する。

図１は、手術ロボットの全体構造を示す斜視図であり、図２は、図１に示す“Ａ”部分の内部構造を示す図である。特に、図１は、末端部に手術道具（ｓｕｒｇｉｃａｌ ｔｏｏｌ）が装着された複数の手術器具を１つの切開孔を通じて患者の体内に進入させて、人体内多様な場所で手術を可能にするシングルポート手術ロボットを示している。シングルポート手術ロボットを利用した手術では、以下で説明するさまざまな要件が必要である。第一に、複数の手術器具が１つの切開孔を通じて体内に進入した後、手術作業のために任意の位置に移動可能ではなければならないので、手術器具の作業領域が広くなければならない。第二に、手術器具が高い自由度を有し、多様な作業が可能ながらも、腹腔壁などで人体組職との衝突を最小化しなければならない。第三に、手術器具が体内に進入するときには、柔軟ではなければならないし、手術作業を行うときには、高い剛性を有しなければならない。すなわち、シングルポート手術ロボットを利用した手術時には、手術器具が自由に移動することができる広い作業領域と高自由度(ｈｉｇｈ ｄｅｇｒｅｅｏｆ ｆｒｅｅｄｏｍ）、高剛性（ｈｉｇｈ ｒｉｇｉｄｉｔｙ）及び柔軟性を同時に満たすことができる手術器具が確保されなければならない。

ここで、自由度（Ｄｅｇｒｅｅ ｏｆ Ｆｒｅｅｄｏｍ、ＤＯＦ）とは、運動学（Ｋｉｎｅｍａｔｉｃｓ）または逆運動学（Ｉｎｖｅｒｓｅ Ｋｉｎｅｍａｔｉｃｓ）での自由度を言う。器具の自由度とは、器具の独立的な運動の数または各リンク間の相対位置の独立した運動を決定する変数の数を言う。例えば、ｘ軸、ｙ軸、ｚ軸よりなる３次元空間上の物体は、物体の空間的な位置（ｐｏｓｉｔｉｏｎ）を決定するための３自由度（各軸での位置）と、物体の空間的な姿勢（ｏｒｉｅｎｔａｔｉｏｎ）を決定するための３自由度（各軸に対する回転角度）のうち１つ以上の自由度を有する。もし物体がそれぞれの軸に沿って移動可能であり、それぞれの軸を基準として回転可能であるとすれば、この物体は、６自由度を有するものと理解することができる。

図１に示されたように、手術ロボットは、手術台に横になっている患者に手術を行うスレーブロボット２００と、スレーブロボット２００を操作者（主として医者）が遠隔操縦するマスター装置１００とを含んでなる。マスター装置１００は、操作者（主として医者）の操作による制御信号を生成し、スレーブロボット２００に伝送する。一方、スレーブロボット２００は、マスター装置１００から制御信号を受信し、受信した制御信号によって移動し、手術に必要な操作を患者に加える。ここで、マスター装置１００とスレーブロボット２００が必ず物理的に独立した別途の装置に分離されなければならないものではなく、１つに統合されて一体型で構成されてもよい。

図１に示されたように、スレーブロボット２００は、マウンティングアーム２０２及び円筒状のケーシング２０８を含むことができる。

スレーブロボット２００のマウンティングアーム２０２は、多自由度を持って駆動するように具現することができる。マウンティングアーム２０２は、複数のリンク（図３Ａの２０６ａ、２０６ｂ、２０６ｃ参照）及び複数の関節（図３Ａの２０４ａ、２０４ｂ、２０４ｃ参照）よりなる。

また、マウンティングアーム２０２の上部には、円筒状のケーシング２０８が連結される。ケーシング２０８には、内部に複数のツール２１４ａ、２１４ｂ及び／または内視鏡２１６を含むガイドチューブ２１２と、複数のツール２１４ａ、２１４ｂ及び／または内視鏡２１６とガイドチューブ２１２の駆動のための駆動部（図３Ｂの２７０参照）が内蔵されることができる。ここで、ガイドチューブ２１２は、マウンティングアーム２０２に連結して設置される。スレーブロボット２００が手術を行わないときには、ガイドチューブ２１２がケーシング２０８内に内蔵されていて、スレーブロボット２００が手術を行うときには、図１及び図２に示されたようにケーシング２０８内に内蔵されているガイドチューブ２１２が外に出て、患者の体内に挿入される。ガイドチューブ２１２が患者の体内に挿入され、手術作業を行う様子、すなわち図１に示す“Ａ”部分の内部様子をさらに詳細に説明すれば、図２に示されたように、ガイドチューブ２１２が患者の皮膚に形成された切開孔Ｉを通じて体内に進入した後、手術しようとする部位（手術部位）に接近するようになれば、ガイドチューブ２１２から複数のツール２１４ａ、２１４ｂと内視鏡２１６が分岐して出て、手術作業を行う。ここで、ガイドチューブ２１２、複数のツール２１４ａ、２１４ｂ及び内視鏡２１６は、マウンティングアーム２０２と同様に、複数のリンク及び複数の関節よりなり、多自由度を持って駆動するように具現することができる。複数のツール２１４ａ、２１４ｂそれぞれの末端部（ｄｉｓｔａｌ ｅｎｄ）には、腹腔内で臓器と接触し、切って縫う直接的な施術作業を行うフォーセップス（ｆｏｒｃｅｐｓ）、ジョー（ｊａｗ）、グラスパー（ｇｒａｓｐｅｒ）、はさみ（ｓｃｉｓｓｏｒｓ）、ステープラー（ｓｔａｐｌｅｒ）、焼灼器（ｃａｕｔｅｒｙ）、ニードル（ｎｅｅｄｌｅ）などの手術道具、すなわちエンドエフェクタ２１８ａ、２１８ｂが装着される。

内視鏡２１６は、広い意味でガイドチューブ２１２から分岐して出る複数のツール２１４ａ、２１４ｂのうち１つとして見られるが、以下では、手術ロボットの特性を考慮して末端部に手術部位に対する切開、縫合などの直接的な手術動作を行うエンドエフェクタ２１８ａ、２１８ｂが設けられる複数のツール２１４ａ、２１４ｂと、手術部位に対して直接的な動作を行うことなく、エンドエフェクタ２１８ａ、２１８ｂの動作を補助するための多関節を有する内視鏡２１６を別に分けて説明する。

ここで、内視鏡２１６としては、ロボット手術で主に使用される腹腔鏡（ｌａｐａｒｏｓｃｏｐｅ）以外に、胸腔鏡（ｔｈｏｒａｃｏｓｃｏｐｙ）、関節鏡（ａｒｔｈｒｏｓｃｏｐｅ）及び鼻鏡（ｒｈｉｎｏｓｃｏｐｅ）など多様な手術用内視鏡が使用されることができる。

一方、マスター装置１００は、マスター操作機１１２Ｌ、１１２Ｒ、クラッチペダル１１４Ｌ、１１４Ｒ及び表示部１１６を含むことができる。マスター装置１００は、操作者が両手にそれぞれ把持して操作することができるようにマスター操作機１１２Ｌ、１１２Ｒを具備する。操作者は、マスター操作機１１２Ｌ、１１２Ｒを通じてスレーブロボット２００のマウンティングアーム２０２、ガイドチューブ２１２、複数のツール２１４ａ、２１４ｂ及び内視鏡２１６の位置及び機能を操作する。マスター操作機１１２Ｌ、１１２Ｒは、３次元空間上でマウンティングアーム２０２などのｘ軸、ｙ軸、ｚ軸の並進運動及びロール（ｒｏｌｌ）方向、ピッチ（ｐｉｔｃｈ）方向、ヨー（ｙａｗ）方向の回転運動を制御するために６自由度を有するように具現されることができる。マスター操作機１１２Ｌ、１１２Ｒは、図１に示されたように、２つのハンドルで具現されることができ、操作者のハンドル操作による制御信号がスレーブロボット２００に伝送され、マウンティングアーム２０２などを含むスレーブロボット２００の動作が制御される。操作者のハンドル操作によってマウンティングアーム２０２、ガイドチューブ２１２、複数のツール２１４ａ、２１４ｂ及び内視鏡２１６の並進運動及び回転運動、このような動きを用いた実質的な手術作業（例：縫合、配管挿入など）が行われることができる。

また、マスター装置１００は、マスター操作機１１２Ｌ、１１２Ｒの操作機能を拡張するために操作者が両足でそれぞれ踏んだり押して操作することができるように、２つのペダルセンサー１１４Ｌ、１１４Ｒを含む。

図１に示す２つのハンドルよりなるマスター操作機１１２Ｌ、１１２Ｒと２つのペダルセンサー１１４Ｌ、１１４Ｒを利用してマウンティングアーム２０２などの動作を制御する方式を具体的な例を取って説明すれば、まず、左側ハンドル１１２Ｌを利用してマウンティングアーム２０２の位置及び動作を制御し、右側ハンドル１１２Ｒを利用してガイドチューブ２１２の位置及び動作を制御することができる。また、マスター装置１００に設けられたモードスイッチ(図示せず)またはボタン(図示せず)が操作された状態では、左側ハンドル１１２Ｌを利用して第１ツール２１４ａの位置及び動作を制御し、右側ハンドル１１２Ｒを利用して第２ツール２１４ｂの位置及び動作を制御することができる。これに加えて（モードスイッチまたはボタンが操作された後）、左側ペダルセンサー１１４Ｌが操作された状態では、左側ハンドル１１２Ｌを利用して内視鏡２１６の位置及び動作を制御することができる。また、モードスイッチまたはボタンが操作された後、右側ペダルセンサー１１４Ｒが操作された状態で、右側ハンドル１１２Ｒを利用して内視鏡２１６の位置及び動作を制御することができるように具現することも可能である。

図１では、マスター装置１００に２つのマスター操作機（ハンドル）が装着された場合を例に取って説明したが、ハンドルをさらに追加し、同時に複数の手術装備（例えば、ガイドチューブ、複数のツール）をリアルタイムで操作することができるように具現することも可能である。ここで、ハンドル１１２Ｌ、１１２Ｒは、その操作方式に従って多様な器具的構成を有することができ、例えば、ジョイスティックなど３次元的な動きを有することができ、スレーブロボット２００のマウンティングアーム２０２、ガイドチューブ２１２及び複数のツール２１４ａ、２１４ｂなどを作動させるための多様な入力手段が利用されることができる。マスター操作機１１２Ｌ、１１２Ｒには、複数のリンク及び複数の関節（リンクとリンクの連結部位）が連結される。マスター操作機１１２Ｌ、１１２Ｒに連結される複数の関節それぞれには、マスター操作機１１２Ｌ、１１２Ｒに連結される各関節の回転角度を検出する回転角センサー（例えば、エンコーダー）が装着されることができる。

マスター装置１００の表示部１１６には、内視鏡(図２の２１６参照)及び／または超音波プローブ(図示せず)によって入力される映像が画像イメージとして表示される。表示部１１６は、１つ以上のモニターで構成されることができ、各モニターに手術時に必要な情報が個別的に表示されるようにすることができる。図１には、表示部１１６が３つのモニターを含む場合が例示されたが、モニターの数は、表示を必要とする情報の類型や種類などによって多様に決定されることができる。

マスター装置１００とスレーブロボット２００は、有線通信網または無線通信網を通じて相互結合され、制御信号、内視鏡２１６を通じて入力された内視鏡映像及び超音波プローブ(図示せず)を通じて入力された超音波映像などが相手（スレーブロボットまたはマスター装置）に伝送されることができる。もし、マスター装置１００に設けられた２つのマスター操作機（ハンドル）による２つの制御信号、例えば、ガイドチューブ２１２から分岐した第１ツール２１４ａの位置調整のための制御信号とガイドチューブ２１２から分岐した第２ツール２１４ｂの位置調整のための制御信号が同時にまたは近接した時点で伝送される必要がある場合、各制御信号は、相互独立的にスレーブロボット２００に伝送されることができる。ここで、各制御信号が“独立的に”伝送されるということは、制御信号の間に互いに干渉を与えず、いずれか１つの制御信号が他の１つの信号に影響を及ぼさないことを意味する。このように、複数の制御信号が互いに独立的に伝送されるようにするためには、各制御信号の生成段階で各制御信号に対するヘッダー情報を付加して伝送させるか、または各制御信号がその生成手順によって伝送されるようにするか、または各制御信号の伝送手順に関してあらかじめ優先順位を定め、それによって伝送されるようにするなど多様な方式が利用されることができる。この場合、各制御信号が伝送される伝送経路が独立的に具備されるようにすることによって、各制御信号の間に干渉が根本的に防止されるようにすることもできる。

図３Ａは、手術ロボットを成すスレーブロボットの外観斜視図であり、図３Ｂは、ケーシング内に内蔵されるマルチツールモジュール及び駆動部の構造を示す図であり、図３Ｃは、図３Ｂに示す“Ｂ”部分(ガイドチューブ２１２の末端部)で複数のツールと１つの内視鏡が展開された様子を示す図である。

図３Ａに示されたように、スレーブロボット２００は、胴体２０１、複数のリンク及び関節よりなるマウンティングアーム２０２及び円筒状のケーシング２０８を含むことができる。

スレーブロボット２００の胴体２０１は、手術作業を行うマウンティングアーム２０２を設置及び支持するための構成部であって、胴体２０１の下端には、スレーブロボット２００の位置移動のための複数のキャスター（ｃａｓｔｅｒ）２０１ａが装着される。それぞれのキャスター２０１ａには、キャスター２０１ａの動作状態を変更するためのレバー(図示せず)が設けられることができる。操作者は、このレバーの位置を調節し、キャスター２０１ａの動作状態を変更させることができる。キャスター２０１ａの動作状態としては、制動（ｂｒａｋｅ）、自由旋回（ｆｒｅｅ ｓｗｉｖｅｌ）及び方向固定、旋回固定（ｄｉｒｅｃｔｉｏｎａｌ ｌｏｃｋ、ｓｗｉｖｅｌ ｌｏｃｋ）などを挙げることができる。

スレーブロボット２００のマウンティングアーム２０２は、３つのリンク第１〜第３リンク、２０６ａ〜２０６ｃ及び３つの関節第１〜第３関節、２０４ａ〜２０４ｃを含んでなる。
マウンティングアーム２０２を成す第１リンク２０６ａは、直線型の柱形状を有し、胴体２０１と垂直する方向に設けられる。

マウンティングアーム２０２を成す第１関節２０４ａは、胴体２０１と第１リンク２０６ａの連結部位に設けられる。第１関節２０４ａは、ｘ、ｙ、ｚ軸のうち指定された軸に沿って移動する直動関節（ｐｒｉｓｍａｔｉｃ ｊｏｉｎｔ）で具現することができる。第１関節２０４ａは、患者の体内に挿入されるガイドチューブ２１２に対する制限的な動作制御のためのＲＣＭ（Ｒｅｍｏｔｅ Ｃｅｎｔｅｒ ｏｆ Ｍｏｔｉｏｎ）のｘ、ｙ、ｚ座標調節のためのものなので、３自由度を有することができる。具体的に、第１関節２０４ａは、ｘ軸並進運動、ｙ軸並進運動及びｚ軸並進運動を含む３自由度を有する。このために、第１関節２０４ａには、ｘ軸駆動部(図示せず)、ｙ軸駆動部(図示せず)及びｚ軸駆動部(図示せず)が設けられることができる。

第１リンク２０６ａの先端には、第２リンク２０６ｂが機械的に連結される。第２リンク２０６ｂは、図３Ａに示されたように、曲線形状を有する。具体的に、第２リンク２０６ｂは、円弧の一部のような形状を有する。

第２関節２０４ｂは、第１リンク２０６ａと第２リンク２０６ｂの連結部位に設けられる。第２関節２０４ｂは、ｘ、ｙ、ｚ軸のうち指定された軸を基準として回転する回転関節（ｒｅｖｏｌｕｔｅ ｊｏｉｎｔ）で具現することができる。このような第２関節２０４ｂは、ガイドチューブ２１２などが内蔵されるケーシング２０８の回転運動のための部分であって、２自由度を有することができる。具体的に、第２関節２０４ｂは、ケーシング２０８のロール（ｒｏｌｌ）方向回転及びピッチ（ｐｉｔｃｈ）方向回転を含む２自由度を有する。このために、第２関節２０４ｂには、ロール駆動部(図示せず)及びピッチ駆動部(図示せず)が設けられることができる。

第２リンク２０６ｂの先端には、第３リンク２０６ｃが機械的に連結される。第３リンク２０６ｃは、図３Ａに示されたように、リング（ｒｉｎｇ）形状で形成される。第３リンク２０６ｃの上部には、円筒状のケーシング２０８が設けられる。

円筒状のケーシング２０８内には、マウンティングアーム２０２に連結されるガイドチューブ２１２と、その内部に設けられる複数のツール２１４ａ、２１４ｂ及び内視鏡２１６よりなるマルチツールモジュール２１０、及びマルチツールモジュール２１０を成す構成要素（ガイドチューブ、複数のツール及び内視鏡）を駆動させる駆動部２７０が内蔵されることができる(図３Ｂ参照)。スレーブロボット２００が手術を行わないときには、図３Ａに示されたように、マルチツールモジュール２１０、すなわち内部に複数のツール２１４ａ、２１４ｂと内視鏡２１６を含むガイドチューブ２１２が円筒状のケーシング２０８内に内蔵されているので、ガイドチューブ２１２が外部に露出しない。このようなガイドチューブ２１２などが内蔵されたケーシング２０８は、第３リンク２０６ｃと機械的に分離することができるように具現することができる。このようにガイドチューブ２１２などが内蔵されるケーシング２０８が第３リンク２０６ｃから分離される場合、手術に使用されたガイドチューブ２１２を交替するか、または消毒することが容易になる。

第３関節２０４ｃは、第２リンク２０６ｂと第３リンク２０６ｃの連結部位に設けられる。第３関節２０４ｃは、ｘ、ｙ、ｚ軸のうち指定された軸を基準として回転する回転関節（ｒｅｖｏｌｕｔｅ ｊｏｉｎｔ）で具現することができる。このような第３関節２０４ｃは、ガイドチューブ２１２などが内蔵されるケーシング２０８の回転運動のための部分であって、１自由度を有することができる。具体的に、第３関節２０４ｃは、ケーシング２０８のヨー（ｙａｗ）方向回転を含む１自由度を有する。このために、第３関節２０４ｃには、ヨー駆動部(図示せず)が設けられることができる。

前述したように、スレーブロボット２００が手術を行わないときには、円筒状のケーシング２０８内に、図３Ｂに示されたようなガイドチューブ２１２及びその内部に設けられる複数のツール２１４ａ、２１４ｂと内視鏡２１６を含んでなるマルチツールモジュール２１０、そしてマルチツールモジュール２１０に連結され、マルチツールモジュール２１０を成す構成要素それぞれを駆動させるためのモーターなどのアクチュエータを含む駆動部２７０が内蔵される。

一方、ガイドチューブ２１２とその内部に設けられる複数のツール２１４ａ、２１４ｂ及び内視鏡２１６よりなるマルチツールモジュール２１０を利用したロボット手術過程は、大きく、マルチツールモジュール２１０の挿入（ｉｎｓｅｒｔｉｏｎ）段階、マルチツールモジュール２１０の位置選定（ｐｏｓｉｔｉｏｎｉｎｇ）段階及びマルチツールモジュール２１０を利用した手術作業（ｏｐｅｒａｔｉｏｎ）段階を含む。

スレーブロボット２００が手術を行うときには、ケーシング２０８内に内蔵されているマルチツールモジュール２１０、さらに厳密には、ガイドチューブ２１２がケーシング２０８の外に出て、患者の皮膚に形成された切開孔Ｉを通じて体内に挿入される。ガイドチューブ２１２が体内に挿入される段階（ｉｎｓｅｒｔｉｏｎ）では、図３Ｂに示された“Ｂ”部分のように、複数のツール２１４ａ、２１４ｂと内視鏡２１６がガイドチューブ２１２から分岐されて出る前の状態、すなわち複数のツール２１４ａ、２１４ｂと内視鏡２１６がガイドチューブ２１２の内部空間に折り畳まれている（ｆｏｌｄｅｄ）状態のマルチツールモジュール２１０が患者の体内に進入するようになる。

体内に挿入されたガイドチューブ２１２を手術しようとする部位(手術部位)に位置させる位置選定（ｐｏｓｉｔｉｏｎｉｎｇ）段階を経てガイドチューブ２１２が手術部位に接近すれば、図３Ｃに示されたように、ガイドチューブ２１２からそれぞれ末端部に手術作業のためのエンドエフェクタ２１８ａ、２１８ｂが装着された複数のツール２１４ａ、２１４ｂと１つの内視鏡２１６が分岐して出て、手術作業を行うようになる。図３Ｃは、ガイドチューブ２１２の末端部（図３Ｂに示す“Ｂ”部分）から２つのツール（第１及び第２ツール、２１４ａ、２１４ｂ）と１つの内視鏡２１６が展開された様子を例示している。

本発明は、図３Ａ〜図３Ｃに示されたように、マウンティングアーム２０２、マウンティングアーム２０２に連結設置されるガイドチューブ２１２、及びガイドチューブ２１２内に設けられ、手術作業時にガイドチューブ２１２から分岐して出る複数のツール２１４ａ、２１４ｂ及び１つの内視鏡２１６を含むスレーブロボット２００を備えた手術ロボットにおいて、各構成要素、すなわちマウンティングアーム２０２、ガイドチューブ２１２、複数のツール２１４ａ、２１４ｂ及び１つの内視鏡２１６の関節空間（ｊｏｉｎｔ ｓｐａｃｅ）での自由度の和が作業空間（ｔａｓｋ ｓｐａｃｅ）での自由度(作業に必要な自由度)の和より大きい場合、すなわち手術ロボット(特に、スレーブロボット)が冗長自由度（冗長性）を有する場合、この冗長自由度を活用してスレーブロボット２００を成す各構成要素の動作を統合的に制御する方法に関する発明である。

前述した冗長自由度の概念をさらに詳しく説明するために、まず、図３Ａ〜図３Ｃで例示したマウンティングアーム２０２、マウンティングアーム２０２に連結設置されるガイドチューブ２１２、手術作業時にガイドチューブ２１２から分岐して出る２つのツール２１４ａ、２１４ｂ及び１つの内視鏡２１６を含むスレーブロボット２００において、マウンティングアーム２０２、ガイドチューブ２１２、２つのツール２１４ａ、２１４ｂ及び１つの内視鏡２１６それぞれは、複数のリンク及び複数の関節を含んでなるという点を前提する。また、マウンティングアーム２０２とガイドチューブ２１２が互いに連動して動作し、ガイドチューブ２１２とそれぞれのツール２１４ａ、２１４ｂが互いに連動して動作し、ガイドチューブ２１２と１つの内視鏡２１６が互いに連動して動作するものと前提する。ここで、マウンティングアーム２０２の関節空間での自由度ａを６（ｘ、ｙ、ｚ軸方向の並進自由度３とロール、ピッチ、ヨー方向の回転自由度３の和）で、ガイドチューブ２１２の関節空間での自由度ｂを６(２個の曲げ（ｂｅｎｄｉｎｇ）自由度２と挿入方向の自由度１とロール方向の回転自由度１の和)で、第１及び第２ツール２１４ａ、２１４ｂそれぞれの関節空間での自由度ｃ、ｄを６で、内視鏡２１６の関節空間での自由度ｅを３（ロール、ピッチ、ヨー方向の回転自由度３）で具現してスレーブロボット２００を製作する場合、前述した構成要素をすべて含むスレーブロボット２００の関節空間での自由度Ｎは、２７になる（Ｎ＝ａ＋ｂ＋ｃ＋ｄ＋ｅ＝６＋６＋６＋６＋３＝２７）。

一方、ｘ軸、ｙ軸、ｚ軸よりなる３次元空間で複数の関節よりなる器具部（例えば、ツールまたは内視鏡）が任意の作業を行うのに必要な作業空間での自由度は、ｘ、ｙ、ｚ軸方向の並進自由度３とロール、ピッチ、ヨー方向の回転自由度３の和である６になる。

図３Ａ〜図３Ｃで例示した２つのツール２１４ａ、２１４ｂと１つの内視鏡２１６を含むスレーブロボット２００において、第１及び第２ツール２１４ａ、２１４ｂそれぞれの作業空間での自由度ｆ、ｇ、すなわち手術作業のために作業空間で必要とする自由度を６とし、内視鏡２１６の作業空間での自由度ｈを３（ロール、ピッチ、ヨー方向の回転自由度だけが必要）というとき、２つのツール２１４ａ、２１４ｂと１つの内視鏡２１６を含むスレーブロボット２００の作業空間での自由度Ｍは、１５になる（Ｍ＝ｆ＋ｇ＋ｈ＝６＋６＋３＝１５）。

多関節ロボットの関節空間での自由度Ｎが作業空間での自由度Ｍより大きいとき、冗長自由度が発生したと言い、先立って例示したスレーブロボット２００の関節空間での自由度（Ｎ＝２７）が作業空間での自由度（Ｍ＝１５）より大きいので、例示したスレーブロボット２００は、冗長自由度を有するシステムと見なすことができる。本発明は、この冗長自由度を活用してさまざまな目的を達成することができるようにするスレーブロボット２００を成す各構成要素、すなわちマウンティングアーム２０２、ガイドチューブ２１２、ガイドチューブ２１２から分岐して出る複数のツール２１４ａ、２１４ｂ及び内視鏡２１６それぞれの関節空間での解（ｓｏｌｕｔｉｏｎ）を求める過程に関するものである。ここで、スレーブロボット２００を成すマウンティングアーム２０２、ガイドチューブ２１２、複数のツール２１４ａ、２１４ｂ及び内視鏡２１６それぞれの関節空間での解というのは、手術作業を行うために各制御周期ごとに算出されるマウンティングアーム２０２を成す複数の関節それぞれの目標回転角度、ガイドチューブ２１２を成す複数の関節それぞれの目標回転角度、複数のツール２１４ａ、２１４ｂを成す複数の関節それぞれの目標回転角度及び内視鏡２１６を成す複数の関節それぞれの目標回転角度を意味する。

図４は、手術ロボットの制御ブロック図である。図４に示されたように、手術ロボットは、マスター装置１００Ａ及びスレーブロボット２００Ａを含んでなる。

本実施例では、スレーブロボット２００Ａを成すマウンティングアーム２０２、ガイドチューブ２１２、複数のツール２１４ａ、２１４ｂ及び内視鏡２１６それぞれが複数のリンク及び複数の関節を含んでなるという点を前提する。また、本実施例では、マウンティングアーム２０２とガイドチューブ２１２が互いに連動して動作し、ガイドチューブ２１２とそれぞれのツール２１４ａ、２１４ｂが互いに連動して動作し、ガイドチューブ２１２と１つの内視鏡２１６が互いに連動して動作するものと前提する。しかも、本実施例では、スレーブロボット２００Ａが冗長自由度(ｒｅｄｕｎｄａｎｃｙ)を有する場合、すなわちスレーブロボット２００Ａの関節空間での自由度Ｎが作業空間での自由度Ｍより大きい場合（Ｎ＞Ｍ）を前提する。

マスター装置１００Ａは、位置／姿勢検出部１２０Ａ、速度検出部１２５Ａ、格納部１３０Ａ、メイン制御部１４０Ａ、通信部１５０Ａ及び表示部１１６Ａを含むことができる。

位置／姿勢検出部１２０Ａは、操作者によって操作されるマスター操作機１１２Ｌ、１１２Ｒの位置及び姿勢（方向）を検出する。マスター操作機１１２Ｌ、１１２Ｒが６自由度を有するように具現された場合、マスター操作機１１２Ｌ、１１２Ｒの位置情報は、ｘ’、ｙ’、ｚ’で表現することができ、マスター操作機１１２Ｌ、１１２Ｒの姿勢情報は、α’、β’、γ’で表現することができる。位置／姿勢検出部１２０Ａは、マスター操作機１１２Ｌ、１１２Ｒに連結される複数の関節それぞれに装着され、各関節の回転角度を検出する回転角センサー(図示せず)と、回転角センサーを通じて検出された各関節の回転角度をマスター操作機１１２Ｌ、１１２Ｒの順運動学式に代入して、マスター操作機１１２Ｌ、１１２Ｒの３次元空間上での位置及び姿勢情報を算出する演算モジュールよりなる。回転角センサーとしては、エンコーダー（ｅｎｃｏｄｅｒ）、ポテンショメータ（ｐｏｔｅｎｔｉｏｍｅｔｅｒ）などを利用することができる。ここでは、回転角センサーと演算モジュールよりなる位置／姿勢検出部１２０Ａを例に取って説明したが、その他、マスター操作機１１２Ｌ、１１２Ｒの位置及び姿勢に対する情報を検出することができる装置なら、どんな装置でも位置／姿勢検出部１２０Ａとして使用可能であることは勿論である。

速度検出部１２５Ａは、マスター操作機１１２Ｌ、１１２Ｒに連結される複数の関節それぞれに装着され、マスター操作機１１２Ｌ、１１２Ｒの速度、より詳細には、マスター操作機１１２Ｌ、１１２Ｒに連結される各関節の回転速度を検出する。速度検出部１２５Ａとしては、タコメーター（ｔａｃｈｏｍｅｔｅｒ）などを利用することができる。本実施例では、マスター操作機１１２Ｌ、１１２Ｒの速度を速度検出部１２５Ａを利用して検出する場合を例に取って説明したが、別途の速度検出部（速度センサー）を利用すことなく、位置／姿勢検出部１２０Ａを成す回転角センサー（例えば、エンコーダー）の出力値を微分し、マスター操作機１１２Ｌ、１１２Ｒの速度を算出し、算出された速度情報を利用することも可能である。

格納部１３０Ａは、スレーブロボット２００Ａの冗長自由度を活用して関節空間での解、すなわちマウンティングアーム２０２、ガイドチューブ２１２、複数のツール２１４ａ、２１４ｂ及び内視鏡２１６を成す各関節の目標回転角度を算出するのに必要な情報及びアルゴリズムなどが格納されるメモリであって、格納部１３０Ａ内には、マスター装置１００Ａのマスター操作機１１２Ｌ、１１２Ｒの動作とスレーブロボット２００Ａの末端部（複数のツール及び内視鏡）の動作との間の動作スケーリング（ｍｏｔｉｏｎ ｓｃａｌｉｎｇ）時に適用されるスケーリングファクター、ヤコビアン行列算出時に必要とするアルゴリズム及びスレーブロボット２００Ａの運動学的構造に関する情報、目的関数算出時に必要とする複数の個別目的それぞれを達成するための複数の個別目的関数、手術作業を成す複数の動作に対する学習結果及び手術作業がどんなものであるかによって各個別目的関数に乗算される加重値などが格納される。

また、格納部１３０Ａは、手術以前に撮影されたＸ線映像、コンピューター断層撮映（ＣＴ）映像及び磁気共鳴映像（ＭＲＩ）などの多様な参照映像などを格納することができる。

メイン制御部１４０Ａは、手術ロボットの全般的な動作を制御するためのプロセッサであって、メイン制御部１４０Ａは、さらに、変換部１４１Ａ、冗長自由度逆運動学解析部１４２Ａ及び映像処理部１４７Ａを含む。

変換部１４１Ａは、位置／姿勢検出部１２０Ａを通じて検出されたマスター操作機１１２Ｌ、１１２Ｒの位置及び姿勢情報ｘ’、ｙ’、ｚ’、α’、β’、γ’をスレーブロボット２００Ａの作業空間でのモーション命令情報、すなわち操作者が所望のスレーブロボット２００Ａの末端部（複数のツールの各末端部及び内視鏡の末端部）の位置及び姿勢情報ｘ、ｙ、ｚ、α、β、γに変換する。変換部１４１Ａは、マスター装置１００Ａのマスター操作機１１２Ｌ、１１２Ｒの動作とスレーブロボット２００Ａの末端部（複数のツール及び内視鏡）の動作との間の動作スケーリング(ｍｏｔｉｏｎ ｓｃａｌｉｎｇ)を通じてマスター操作機１１２Ｌ、１１２Ｒの位置及び姿勢情報ｘ’、ｙ’、ｚ’、α’、β’、γ’をスレーブロボット２００Ａの作業空間でのモーション命令情報ｘ、ｙ、ｚ、α、β、γに変換することができる。すなわち変換部１４１Ａは、マスター操作機１１２Ｌ、１１２Ｒの位置及び姿勢情報ｘ’、ｙ’、ｚ’、α’、β’、γ’に動作スケーリング時に適用されるスケーリングファクター（ｓｃａｉｎｇｆａｃｔｏｒ）を乗算し、スレーブロボット２００Ａの作業空間でのモーション命令情報ｘ、ｙ、ｚ、α、β、γを算出することができる。

冗長自由度逆運動学解析部１４２Ａは、スレーブロボット２００Ａの冗長自由度を活用してスレーブロボット２００Ａを成す各構成要素（マウンティングアーム、ガイドチューブ、複数のツール及び内視鏡）の動作を統合的に制御するための制御信号（各構成要素を成す複数の関節それぞれの目標回転角度）を生成する構成部であって、冗長自由度逆運動学解析部１４２Ａは、さらに、ヤコビアン行列算出部１４４Ａ、目的関数算出部１４５Ａ及び冗長自由度活用部１４６Ａを含んでなる。

冗長自由度逆運動学解析部１４２Ａを成すそれぞれの構成部に対する詳細な説明に先立って、以下では、まず、順運動学及び逆運動学の概念と冗長自由度を有するスレーブロボット２００Ａでの逆運動学解析について説明する。

一般的に、ロボットシステムでの順運動学（Ｆｏｒｗａｒｄ Ｋｉｎｅｍａｔｉｃｓ）は、一連の関節角（関節変数、ｑ）が与えられたとき、ロボット末端部の直交座標系上の位置及び姿勢（作業変数、ｐ）を求める問題であって、逆運動学に比べて相対的に簡単であり、均質変換（Ｈｏｍｏｇｅｎｅｏｕｓ Ｔｒａｎｓｆｏｒｍ）を利用して順運動学の解を求めることができる。一方、逆運動学（Ｉｎｖｅｒｓｅ ｋｉｎｅｍａｔｉｃｓ）は、順運動学と反対に、ロボット末端部の直交座標系上の位置及び姿勢（作業変数、ｐ）が与えられたとき、関節角（関節変数、ｑ）を求める問題、すなわち作業空間（ｔａｓｋ ｓｐａｃｅ）で定義された作業を関節空間（ｊｏｉｎｔ ｓｐａｃｅ）の動きで表現するものであって、運動学的方程式が超越関数よりなる非線形方程式なので、順運動学に比べて解を求めることが相対的に難しいか、または可能でないことがある。また、解が存在しないことがあり、または多数の解が存在することがある。

先立って前提したように、冗長自由度を有するスレーブロボット２００Ａの場合、ヤコビアンの疑似逆行列を利用して数値解析的に逆運動学解析をすることができる。以下では、冗長自由度の逆運動学解析についてさらに詳細に説明する。

逆運動学は、マニピュレーター末端部の位置及び姿勢（ここで、姿勢は、オイラー角（Ｅｕｌｅｒａｎｇｌｅｓ）で表現される）が与えられる場合、これに対応する関節の回転角を決定する過程である。位置水準では、逆運動学的解を求めるための特別な方法が存在せず、与えられたロボットの関節構造によって幾何的関係式を利用するか、または直観に依存して解を求めなければならない。関節の数が多い手術ロボットやヒューマノイドロボットのような場合には、位置水準で逆運動学的解を捜すことが不可能なことがある。このような場合には、速度水準でのヤコビアン関係式を利用してリアルタイムで逆行列を算出し、各関節の速度を求めた後、これを数値積分する方式で逆運動学的解を決定するようになる。

前述したスレーブロボット２００Ａのように、冗長自由度を有するシステムにおいて、ロボット末端部（複数のツールの各末端部及び内視鏡の末端部）の直交座標系上の位置及び姿勢、すなわち作業変数（ｔａｓｋ ｖａｒｉａｂｌｅ）ｐと関節空間での一連の関節角、すなわち関節変数（ｊｏｉｎｔ ｖａｒｉａｂｌｅ）ｑとの間の関係は、下記の数式（１）のように示すことができる。

ｐ＝ｆ（ｑ） （１）
ここで、ｐ∈Ｒ^Ｍ、ｑ∈Ｒ^Ｎ、Ｍ＜Ｎであり、Ｎは、冗長自由度を有するスレーブロボット２００Ａの関節空間での自由度を示し、Ｍは、冗長自由度を有するスレーブロボット２００Ａの作業空間での自由度を示す。

この際、ｐ∈Ｒ^Ｍは、作業変数ｐをＭ×１行列で示すことができるということを意味する。例えば、作業空間で６自由度を有する第１ツール２１４ａの末端部の位置情報ｘ_１、ｙ_１、ｚ_１及び姿勢情報α_１、β_１、γ_１と、作業空間で６自由度を有する第２ツール２１４ｂの末端部の位置情報ｘ_２、ｙ_２、ｚ_２及び姿勢情報α_２、β_２、γ_２と、作業空間で３自由度を有する内視鏡２１６の末端部の姿勢情報α_３、β_３、γ_３を作業変数ｐとするとき（作業空間での自由度Ｍ＝６＋６＋３＝１５）、作業変数ｐは、以下のような１５×１行列で示すことができる。

また、ｑ∈Ｒ^Ｎは、関節変数ｑをＮ×１行列で示すことができるということを意味する。例えば、関節空間で６自由度を有するマウンティングアーム２０２を成す６個の関節の回転角度情報ｑ_１、ｑ_２、ｑ_３、ｑ_４、ｑ_５、ｑ_６と、関節空間で６自由度を有するガイドチューブ２１２を成す６個の関節の回転角度情報ｑ_７、ｑ_８、ｑ_９、ｑ_１０、ｑ_１１、ｑ_１２と、関節空間で６自由度を有する第１ツール２１４ａを成す６個の関節の回転角度情報ｑ_１３、ｑ_１４、ｑ_１５、ｑ_１６、ｑ_１７、ｑ_１８と、関節空間で６自由度を有する第２ツール２１４ｂを成す６個の関節の回転角度情報ｑ_１９、ｑ_２０、ｑ_２１、ｑ_２２、ｑ_２３、ｑ_２４と、関節空間で３自由度を有する内視鏡２１６を成す３個の関節の回転角度情報ｑ_２５、ｑ_２６、ｑ_２７を関節変数ｑとするとき（関節空間での自由度Ｎ＝６＋６＋６＋６＋３＝２７）、関節変数ｑは、下記のような２７×１行列で示すことができる。

前述した数式（１）を時間について微分すれば、下記の数式（２）のような微分運動学方程式（ｄｉｆｆｅｒｅｎｔｉａｌ ｋｉｎｅｍａｔｉｃ ｓｅｑｕａｔｉｏｎ）を算出することができる。

ここで、Ｊ（ｑ）は、ｆ（ｑ）のヤコビアン行列（Ｊａｃｏｂｉａｎ Ｍａｔｒｉｘ）である。

逆運動学解析を通じて求めようとするものは、関節変数ｑなので、前述した数式（２）を関節の速度ベクトル

に関する式で整理すれば、下記の数式（３）のように示すことができる。

関節空間での自由度Ｎが作業空間での自由度Ｍと同一である場合(Ｎ＝Ｍ)、すなわちシステムが冗長自由度を有しない場合には、ヤコビアン行列Ｊ（ｑ）の逆行列Ｊ^−１（ｑ）を算出し、これを数式（３）に代入し、各関節の速度(関節の速度ベクトル

の元素、すなわち、

を算出した後、これを積分し、各関節の回転角（関節変数ｑの元素、すなわちｑ_１、ｑ_２、…）を算出することができる。

一方、関節空間での自由度Ｎが作業空間での自由度Ｍより大きい場合（Ｎ＞Ｍ）、すなわち前述したスレーブロボット２００Ａのようにシステムが冗長自由度を有する場合には、ヤコビアン行列Ｊ（ｑ）の列の数が行の数より多くて、ヤコビアン行列の逆行列Ｊ^−１（ｑ）を算出することができないので、無限な逆運動学的解が存在するようになる。

したがって、システムが冗長自由度を有する場合には、ヤコビアン行列Ｊ（ｑ）の疑似逆行列（Ｐｓｅｕｄｏ−ｉｎｖｅｒｓｅ Ｍａｔｒｉｘ）Ｊ^＃（ｑ）を利用して逆運動学的解を算出する。ヤコビアン行列Ｊ（ｑ）の疑似逆行列Ｊ^＃（ｑ）を利用して関節空間での各関節の速度

を算出する式は、下記の数式（４）のように示すことができ、ヤコビアン行列Ｊ（ｑ）の疑似逆行列Ｊ^＃（ｑ）は、下記の数式（５）のように示すことができる。

Ｊ^＃＝Ｊ^Ｔ（ＪＪＪ^Ｔ）^−１ （５）
ここで、Ｉ_ｎは、ｎ×ｎ単位行列（ｕｎｉｔ ｍａｔｒｉｘ、ｉｄｅｎｔｉｔｙ ｍａｔｒｉｘ）であり、ｒは、任意のベクトルであり、（Ｉ_ｎ−Ｊ^＃Ｊ）は、ベクトルｒの零空間（ｎｕｌｌ ｓｐａｃｅ）である。

冗長自由度を活用するために任意のベクトルｒ（ｑ）を定義する多様な方法があり、例えば、下記の数式（６）のように定義することができる。

ここで、ｋは、定数であり、ｗは、目的関数(ｐｅｒｆｏｒｍａｎｃｅ ｉｎｄｅｘ)または評価関数(ｅｖａｌｕａｔｉｏｎ ｆｕｎｃｔｉｏｎ)である。

目的関数ｗは、システムの目的（本実施例では、ツールの作業領域増加、ツールと周辺障害物との間の衝突可能性の最小化など）をすべての条件を考慮して定量的に設定したものであって、システムの最適化問題や目的達成状況の定量的な評価において重要な基準になる。すなわち目的関数ｗは、制御系と工業システムの設計・操作・運転で、そのシステムの適否を定量的に評価するために設定される関数(システムを評価する関数)であり、システムの最適設計や最適制御を定量的に行うためには、システムモデルを満足する解（本実施例では関節空間での解、マウンティングアーム２０２、ガイドチューブ２１２、複数のツール２１４ａ、２１４ｂ及び内視鏡２１６を成す複数の関節それぞれの目標回転角度ベクトルｑ）のうち設定された目的関数値を最大または最小にする解（複数の関節それぞれの目標回転角度）を捜し出さなければならない。

本実施例では、目的関数ｗを最小化する関節空間での解を捜し出せば、スレーブロボット２００Ａの冗長自由度を活用したシステム（手術ロボット）の最適制御を行うことができるようになる。このような目的関数ｗ(以下ではｗをシステム全体の目的関数と定義する)は、複数の目的関数ｗ_１〜ｗ_ｎ（以下ではｗ_１〜ｗ_ｎを個別目的関数と定義する）の加重和（ｗｅｉｇｈｔｅｄ ｓｕｍ）で示すことができる。個別目的関数の例としては、それぞれのツール２１４ａ、２１４ｂと器具的関節限界（ｊｏｉｎｔ ｌｉｍｉｔ）との間の距離の逆数（ｉｎｖｅｒｓｅ ｎｕｍｂｅｒ）、それぞれのツール２１４ａ、２１４ｂと特異姿勢（ｓｉｎｇｕｌａｒ ｐｏｓｅ）との間の距離の逆数、各関節のトルク二乗の和（ｊｏｉｎｔ ｔｏｒｑｕｅ ｓｑｕａｒｅ ｓｕｍ）、それぞれのツール２１４ａ、２１４ｂと周辺障害物との間の距離の逆数などを挙げることができる。

以下では、図５〜図９を参照して本発明によって達成しようとする多様な目的（または本発明によって達成される多様な効果）についてさらに詳細に説明する。

図５は、本発明によって達成されるツール（内視鏡を含む）の作業領域拡張を説明するための概念図である。図５の左側に示す図は、従来ツール制御方式に従って手術作業を行う様子を示しており、図５の右側に示す図は、本発明のツール制御方式に従って手術作業を行う様子を示している。

従来、柔軟性がない（ｒｉｇｉｄ、ｉｎｆｌｅｘｉｂｌｅ）固定式ガイドチューブまたは棒(ｒｏｄ)型ガイドチューブを使用して手術作業を行うか、または図５の左側に示すように、柔軟性があり（ｆｌｅｘｉｂｌｅ）、多自由度を有するガイドチューブ２２を使用して手術作業を行う場合にも、ガイドチューブ２２とガイドチューブ２２から展開され、その末端部にエンドエフェクタ２８ａ２８ｂが装着されるそれぞれのツール（第１ツール２４ａ、第２ツール２４ｂ）が独立的に動作し、ガイドチューブ２２とガイドチューブ２２から展開される内視鏡２６が独立的に動作するので、手術作業の実行途中にそれぞれのツール２４ａ、２４ｂ及び／または内視鏡２１６が関節限界（ｊｏｉｎｔ ｌｉｍｉｔ）に到逹するか、または特異姿勢（ｓｉｎｇｕｌａｒ ｐｏｓｅ）に該当し、複数のツール２４ａ、２４ｂを所望の位置や姿勢に動くことができない状態（ロック）が発生する問題点がある。すなわち、それぞれのツール２４ａ、２４ｂが及び／または内視鏡２１６が関節限界に到逹するか、特異姿勢に該当すれば、ツールの作業領域の限界によって具現が不可能な動作が発生するようになる。図５の左側で、矢印で表示した地点が第１ツール２４ａ及び第２ツール２４ｂの目標姿勢(ｄｅｓｉｒｅｄ ｐｏｓｅ)であり、点線で表示したものが第２ツール２４ｂをその目標姿勢に移動させた場合を示すものとするとき、第２ツール２４ｂが関節限界に到逹するか、または特異姿勢に該当する場合には、第２ツール２４ｂを目標姿勢に動くことができない（第２ツールを点線で表示した領域に動くことができない）。

一方、本発明では、図５の右側に示すように、柔軟性があり（ｆｌｅｘｉｂｌｅ）、多自由度を有するガイドチューブ２１２を使用して手術作業を行い、ガイドチューブ２１２と、ガイドチューブ２１２から展開され、その末端部にエンドエフェクタ２１８ａ、２１８ｂが装着されるそれぞれのツール（第１ツール２１４ａ、第２ツール、２１４ｂ）が互いに連動して動作し、ガイドチューブ２１２と、ガイドチューブ２１２から展開される内視鏡２１６が互いに連動して動作し、スレーブロボット２００Ａの冗長自由度を活用してスレーブロボット２００Ａを成す各構成要素の動作を統合的に制御するので、従来、ガイドチューブとツールの制御方式(独立的な動作制御)を適用するとき、ツール（内視鏡を含む）の作業領域の限界によって具現が不可能な動作も行うことができるようになる。図５の右側で矢印で表示した地点を第１ツール２１４ａ及び第２ツール２１４ｂの目標姿勢とするとき、本発明では、ガイドチューブ２１２と第２ツール２１４ｂが互いに連動して動作するので、第２ツール２１４ｂを目標姿勢に移動させるために、第２ツール２１４ｂが目標姿勢に該当する方向（右側方向）に動くように制御すると同時に、ガイドチューブ２１２も第２ツール２１４ｂが目標姿勢に容易に到逹することができるようにする方向に動くように制御する（ガイドチューブが第２ツールの目標姿勢に該当する方向である右側方向に曲がる）。

図５を参照して説明したツールの作業領域増加という個別目的を達成するための個別目的関数をｗ_１とするとき、個別目的関数ｗ_１は、それぞれのツール２１４ａ、２１４ｂと器具的関節限界との間の距離または内視鏡２１６と器具的関節限界との間の距離ｄ１の逆数１／ｄ_１で定義するか（ｗ_１＝１／ｄ_１）、またはそれぞれのツール２１４ａ、２１４ｂと特異姿勢との間の距離または内視鏡２１６と特異姿勢との間の距離ｄ_２の逆数１／ｄ_２で定義することができる(ｗ_１＝１／ ｄ_２)。また、個別目的関数ｗ_１をそれぞれのツール２１４ａ、２１４ｂと器具的関節限界との間の距離の逆数１／ｄ_１とそれぞれのツール２１４ａ、２１４ｂと特異姿勢との間の距離の逆数１／ｄ_２の単純和や加重和で定義することも可能である。この際、それぞれのツール２１４ａ、２１４ｂと器具的関節限界との間の距離ｄ_１またはそれぞれのツール２１４ａ、２１４ｂと特異姿勢との間の距離ｄ_２を最大化するとき、ツールの作業領域を最大に拡張させることができる。これを個別目的関数ｗ_１の観点から見れば、個別目的関数ｗ_１を最小化するとき、ツールの作業領域を最大に拡張させることができる。

図６は、本発明によって達成されるツール（内視鏡を含む）の関節別必要剛性の最小化を説明するための概念図である。図６の上側に示す絵は、従来のツール制御方式に従って手術作業を行う様子を示しており、図６の下側に示す絵は、本発明のツール制御方式に従って手術作業を行う様子を示している。

従来、柔軟性がない（ｒｉｇｉｄ、ｉｎｆｌｅｘｉｂｌｅ）固定式ガイドチューブまたは棒（ｒｏｄ）型ガイドチューブを使用して手術作業を行うか、または図６の上側に示すように、柔軟性があり（ｆｌｅｘｉｂｌｅ）、多自由度を有するガイドチューブ２２を使用して手術作業を行う場合にも、ガイドチューブ２２と、ガイドチューブ２２から展開され、その末端部にエンドエフェクタ２８ａ、２８ｂが装着されるそれぞれのツール（第１ツール２４ａ、第２ツール２４ｂ）が独立的に動作し、ガイドチューブ２２と、ガイドチューブ２２から展開される内視鏡２６が独立的に動作するので、手術作業の実行途中にそれぞれのツール２４ａ、２４ｂ及び内視鏡２６を成す複数の関節のうち一部の関節に過度なペイロード（ｐａｙｌｏａｄ）が印加され、複数のツール２４ａ、２４ｂ及び内視鏡２６を成す各関節が必要とする剛性関（節別必要剛性）が増加する問題点がある。図６の上側に示す円筒形状の物体を複数のツール２４ａ、２４ｂ及び内視鏡２６に印加されるペイロードとするとき、図６の上側では、このペイロードが第２ツール２４ｂの末端部に印加されているので、複数のツール２４ａ、２４ｂ及び内視鏡２６を成す各関節別必要剛性が増加するようになる。

一方、本発明では、図６の下側に示すように、柔軟性があり（ｆｌｅｘｉｂｌｅ）、多自由度を有するガイドチューブ２１２を使用して手術作業を行い、ガイドチューブ２１２と、ガイドチューブ２１２から展開され、その末端部にエンドエフェクタ２１８ａ、２１８ｂが装着されるそれぞれのツール（第１ツール２１４ａ、第２ツール、２１４ｂ）が互いに連動して動作し、ガイドチューブ２１２と、ガイドチューブ２１２から展開される内視鏡２１６が互いに連動して動作し、スレーブロボット２００Ａの冗長自由度を活用してスレーブロボット２００Ａを成す各構成要素の動作を統合的に制御するので、ペイロードの分配を通じて複数のツール２１４ａ、２１４ｂ及び内視鏡２１６を成す各関節別必要剛性を減少させることができる。

図６を参照して説明したツールの関節別必要剛性の最小化という個別目的を達成するための個別目的関数をｗ_２とするとき、個別目的関数ｗ_２は、複数のツール２１４ａ、２１４ｂ及び内視鏡２１６を成す各関節のトルク二乗τ_１ ^２、τ_２ ^２、τ_３ ^２、…の和（ｊｏｉｎｔ ｔｏｒ（ｑ）ｕｅ ｓｑｕａｒｅ ｓｕｍ）で定義することができる（ｗ_２＝τ_１ ^２＋τ_２ ^２＋τ_３ ^２＋…）。この際、複数のツール２１４ａ、２１４ｂ及び内視鏡２１６を成す各関節のトルク二乗τ_１２、τ_２ ^２、τ_３ ^２、…の和、すなわち個別目的関数ｗ_２を最小化するとき、複数のツール２１４ａ、２１４ｂ及び内視鏡２１６を成す各関節別必要剛性を最小化させることができる。

図７は、本発明によって達成されるツール（内視鏡を含む）と周辺障害物との間の衝突可能性の最小化を説明するための概念図である。図７の左側に示す絵は、従来のツール制御方式に従って手術作業を行う様子を示しており、図７の右側に示す絵は、本発明のツール制御方式に従って手術作業を行う様子を示している。

従来、柔軟性がない（ｒｉｇｉｄ、ｉｎｆｌｅｘｉｂｌｅ）固定式ガイドチューブまたは棒（ｒｏｄ）型ガイドチューブを使用して手術作業を行うか、または図７の左側に示すように、柔軟性があり（ｆｌｅｘｉｂｌｅ）、多自由度を有するガイドチューブ２２を使用して手術作業を行う場合にも、ガイドチューブ２２と、ガイドチューブ２２から展開され、その末端部にエンドエフェクタ２８ａ、２８ｂが装着されるそれぞれのツール（第１ツール２４ａ、第２ツール２４ｂ）が独立的に動作し、ガイドチューブ２２と、ガイドチューブ２２から展開される内視鏡２６が独立的に動作するので、手術作業の実行途中にそれぞれのツール２４ａ、２４ｂ及び／または内視鏡２６が目標姿勢に動くために、周辺障害物（人体組職や他のツール）との衝突が不可避になる問題点がある。図７の左側で、第２ツール２４ｂに装着されたエンドエフェクタ２８ｂが向かう方向を目標姿勢とするとき、第２ツール２４ｂが目標姿勢に移動するためには、周辺障害物との衝突を伴うようになる。

一方、本発明では、図７の右側に示すように、柔軟性があり（ｆｌｅｘｉｂｌｅ）、多自由度を有するガイドチューブ２１２を使用して手術作業を行い、ガイドチューブ２１２と、ガイドチューブ２１２から展開され、その末端部にエンドエフェクタ２１８ａ、２１８ｂが装着されるそれぞれのツール（第１ツール２１４ａ、第２ツール２１４ｂ）が互いに連動して動作し、ガイドチューブ２１２と、ガイドチューブ２１２から展開される内視鏡２１６が互いに連動して動作し、スレーブロボット２００Ａの冗長自由度を活用してスレーブロボット２００Ａを成す各構成要素の動作を統合的に制御するので、それぞれのツール２１４ａ、２１４ｂと周辺障害物との間の衝突及び／または内視鏡２１６と周辺障害物との間の衝突を回避するか、または周辺障害物との衝突可能性を減少させることができるようになる。図７の右側で第２ツール２１４ｂに装着されたエンドエフェクタ２１８ｂが向かう方向を目標姿勢と言うとき、本発明では、ガイドチューブ２１２と第２ツール２１４ｂが互いに連動して動作するので、第２ツール２１４ｂと周辺障害物との間の衝突を回避するために、第２ツール２１４ｂが周辺障害物から遠くなる方向（左側方向）に動くように制御すると同時に、ガイドチューブ２１２は、第２ツール２１４ｂが周辺障害物から容易に遠くなることができるようにする方向に動くように制御する（ガイドチューブが第２ツールと周辺障害物との間の衝突を防止することができるようにＳ字形状に曲がる）。

図７を参照して説明したツールと周辺障害物との間の衝突可能性の最小化という個別目的を達成するための個別目的関数をｗ_３とするとき、個別目的関数ｗ_３は、それぞれのツール２１４ａ、２１４ｂと周辺障害物との間の距離または内視鏡２１６と周辺障害物との間の距離ｄ_３の逆数１／ｄ_３で定義することができる（ｗ_３＝１／ｄ_３）。この際、それぞれのツール２１４ａ、２１４ｂと周辺障害物との間の距離または内視鏡２１６と周辺障害物との間の距離ｄ_３を最大化するとき、ツールと周辺障害物との間の衝突可能性を最小化させることができる。これを個別目的関数ｗ_３の観点から見れば、個別目的関数ｗ_３を最小化するとき、ツールと周辺障害物との間の衝突可能性を最小化させることができる。

図８は、本発明によって達成されるツールの必要自由度の最小化を説明するための概念図である。図８の上側に示す絵は、従来のツール制御方式に従って手術作業を行う様子を示しており、図８の下側に示す絵は、本発明のツール制御方式に従って手術作業を行う様子を示している。

従来、柔軟性がない（ｒｉｇｉｄ、ｉｎｆｌｅｘｉｂｌｅ）固定式ガイドチューブまたは棒（ｒｏｄ）型ガイドチューブを使用して手術作業を行うか、または図８の上側に示すように、柔軟性があり（ｆｌｅｘｉｂｌｅ）、多自由度を有するガイドチューブ２２を使用して手術作業を行う場合にも、ガイドチューブ２２と、ガイドチューブ２２から展開され、その末端部にエンドエフェクタ２８ａ、２８ｂが装着されるそれぞれのツール（第１ツール２４ａ、第２ツール２４ｂ）が独立的に動作し、ガイドチューブ２２と、ガイドチューブ２２から展開される内視鏡２６が独立的に動作するので、手術作業を行うためには、それぞれのツール２４ａ、２４ｂ及び内視鏡２６がすべて６自由度を有するように具現される必要がある。

一方、本発明では、図８の下側に示されたように、柔軟性があり（ｆｌｅｘｉｂｌｅ）、多自由度を有するガイドチューブ２１２を使用して手術作業を行い、ガイドチューブ２１２と、ガイドチューブ２１２から展開され、その末端部にエンドエフェクタが装着されるそれぞれのツール（第１ツール２１４ａ、第２ツール２１４ｂ）が互いに連動して動作し、ガイドチューブ２１２と、ガイドチューブ２１２から展開される内視鏡２１６が互いに連動して動作し、スレーブロボット２００の近位部、すなわちガイドチューブ２１２及びガイドチューブ２１２が連結設置されるマウンティングアーム２０２の自由度によって発生する冗長自由度を活用してスレーブロボット２００を成す各構成要素の動作を統合的に制御するので、６自由度未満の自由度(例えば、４自由度)を有するように具現されたそれぞれのツール２１４ａ、２１４ｂ及び内視鏡２１６を通じて６自由度の動作を行うことができるようになる。

図９は、本発明によって達成される複合的な作業実行を説明するための概念図である。図９の左側に示す絵は、従来ツール制御方式に従って手術作業を行う様子を示しており、図９の右側に示す絵は、本発明のツール制御方式に従って手術作業を行う様子を示している。

従来、柔軟性がない（ｒｉｇｉｄ、ｉｎｆｌｅｘｉｂｌｅ）固定式ガイドチューブまたは棒（ｒｏｄ）型ガイドチューブを使用して手術作業を行うか、または図９の左側に示すように、柔軟性があり（ｆｌｅｘｉｂｌｅ）、多自由度を有するガイドチューブ２２を使用して手術作業を行う場合にも、ガイドチューブ２２と、ガイドチューブ２２から展開され、その末端部にエンドエフェクタ２８ａ、２８ｂが装着されるそれぞれのツール（第１ツール２４ａ、第２ツール２４ｂ）が独立的に動作し、ガイドチューブ２２と、ガイドチューブ２２から展開される内視鏡２６が独立的に動作するので、それぞれのツール２４ａ、２４ｂを通じて行われる手術作業は単純な作業（例：パス（ｐａｓｓｉｎｇ）及びランニング（ｒｕｎｎｉｎｇ））に限定される問題点がある。

一方、本発明では、図９の右側に示すように、柔軟性があり（ｆｌｅｘｉｂｌｅ）、多自由度を有するガイドチューブ２１２を使用して手術作業を行い、ガイドチューブ２１２と、ガイドチューブ２１２から展開され、その末端部にエンドエフェクタ２１８ａ、２１８ｂが装着されるそれぞれのツール（第１ツール２１４ａ、第２ツール２１４ｂ）が互いに連動して動作し、ガイドチューブ２１２と、ガイドチューブ２１２から展開される内視鏡２１６が互いに連動して動作し、スレーブロボット２００Ａの冗長自由度を活用してスレーブロボット２００Ａを成す各構成要素の動作を統合的に制御するので、ツールの作業領域の拡張（図５参照）、ツールの関節別必要剛性の最小化（図６参照）及びツールと周辺障害物との間の衝突可能性の最小化（図７参照）を達成することができ、このような多様な個別目的をすべて組み合わせることによって、従来のツール制御方式では不可能な複合的な作業（例えば、ａｕｔｏ−ｓｕｔｕｒｉｎｇ）を行うことができるようになる。

図９を参照して説明した複合的な作業実行というシステム全体の目的を達成するためのシステム全体の目的関数をｗとするとき、システム全体の目的関数ｗは、先立って図５〜図７と関連した説明で前述した複数の個別目的関数ｗ_１、ｗ_２、ｗ_３の加重和（ｗｅｉｇｈｔｅｄ ｓｕｍ）として定義することができる（ｗ＝ａｗ_１＋ｂｗ_２＋ｃｗ_３）。すなわち、システム全体の目的関数ｗは、それぞれのツール２１４ａ、２１４ｂと器具的関節限界との間の距離または内視鏡２１６と器具的関節限界との間の距離ｄ_１の逆数１／ｄ_１として定義される個別目的関数ｗ_１と、複数のツール２１４ａ、２１４ｂ及び内視鏡２１６を成す各関節のトルク二乗τ_１ ^２、τ_２ ^２、τ_３ ^２、…の和τ_１ ^２＋τ_２ ^２＋τ_３ ^２＋…として定義される個別目的関数ｗ_２と、それぞれのツール２１４ａ、２１４ｂと周辺障害物との間の距離または内視鏡２１６と周辺障害物との間の距離ｄ３の逆数１／ｄ_３として定義される個別目的関数ｗ_３の加重和で示すことができる。この際、システム全体の目的関数ｗ（ｗ＝ａｗ_１＋ｂｗ_２＋ｃｗ_３）は、時間の流れによって変化するようになる。すなわち、スレーブロボット２００が行う手術作業がどんなものであるかによって各個別目的関数に乗算される加重値の値が変わる。例えば、スレーブロボット２００が行う手術作業が配管挿入（ｃａｎｎｕｌａｔｉｏｎ）の場合、ツールの作業領域の拡張やツールの関節別必要剛性の最小化という個別目的よりは、ツールと周辺障害物との間の衝突可能性の最小化という個別目的の重要性がさらに大きいので、ツールと周辺障害物との間の衝突可能性の最小化という個別目的を達成するための個別目的関数ｗ_３に乗算される加重値ｃが他の個別目的関数ｗ_１及びｗ_２にそれぞれ乗算される加重値ａ及びｂよりさらに大きい値として決定される。

さらに、図４に対する説明に戻って、冗長自由度逆運動学解析部１４２Ａを成すそれぞれの構成部について詳細に説明する。

ヤコビアン行列算出部１４４Ａは、作業変数ｐと関節変数ｑとの間の関係を示す数式（１）、すなわちｐ＝ｆ（ｑ）を時間に関して微分して獲得する数式（２）、すなわち微分運動学方程式

で関節の速度ベクトル

に乗算されるヤコビアン行列Ｊ（ｑ）を算出する。この際、ヤコビアン行列算出部１４４Ａは、ヤコビアン行列を算出するためのアルゴリズムにスレーブロボット２００Ａの運動学的構造に関する情報（例えば、関節と関節との間を連結するリンクの長さ情報など）を入力してｆ（ｑ）のヤコビアン行列Ｊ（ｑ）を算出する。

目的関数算出部１４５Ａは、冗長自由度を有するスレーブロボット２００Ａでヤコビアン行列Ｊ（ｑ）の疑似逆行列Ｊ^＃（ｑ）を利用して関節空間での各関節の速度

を算出する数式（４）、すなわち

に記載した任意のベクトルｒ（ｑ）を表現する数式（６）、すなわち

に記載した目的関数ｗを算出する。このような目的関数（ｗ、システム全体の目的関数）は、複数の個別目的関数ｗ_１〜ｗ_ｎの加重和（ｗｅｉｇｈｔｅｄ ｓｕｍ）で示すことができる（ｗ＝ａｗ_１＋ｂｗ_２＋ｃｗ_３＋…）。個別目的関数の例としては、それぞれのツール２１４ａ、２１４ｂと器具的関節限界との間の距離の逆数、複数のツール２１４ａ、２１４ｂ及び内視鏡２１６を成す各関節のトルク二乗の和、それぞれのツール２１４ａ、２１４ｂと周辺障害物との間の距離の逆数などを挙げることができる。この際、システム全体の目的関数ｗは、時間の流れによって変化するようになる。すなわち、スレーブロボット２００が行う手術作業がどんなものであるかによって各個別目的関数に乗算される加重値ａ、ｂ、ｃ、…が変わる。目的関数算出部１４５Ａは、スレーブロボット２００が行う手術作業がどんなものであるかによって各個別目的関数に乗算される加重値ａ、ｂ、ｃ、…を決定する。目的関数算出部１４５Ａは、位置検出部１２０Ａ及び速度検出部１２５Ａを通じて検出されたマスター操作機１１２Ｌ、１１２Ｒの動作に関する情報（マスター操作機の位置情報及び速度情報）及び格納部１３０Ａに既格納された手術作業を成す複数の動作に対する学習結果を利用して操作者が行おうとする手術作業を予測する。ここで、手術作業は、縫合（ｓｕｔｕｒｉｎｇ）、パス（ｐａｓｓｉｎｇ）、ランニング（ｒｕｎｎｉｎｇ）及び配管挿入（ｃａｎｎｕｌａｔｉｏｎ）のうちいずれか１つであり、手術作業を成す複数の動作は、オリエント（ｏｒｉｅｎｔ）、プッシ（ｐｕｓｈ）及びプール（ｐｕｌｌ）のうち少なくとも１つを含む。これを通じて、目的関数算出部１４５Ａは、スレーブロボット２００が行う手術作業がどんなものであるかを判断し、当該判断結果に基づいて各個別目的関数に乗算される加重値ａ、ｂ、ｃ、…を決定した後、決定された各加重値を各個別目的関数に乗算し、システム全体の目的関数ｗを算出する。

冗長自由度活用部１４６Ａは、システム全体の目的関数ｗを最小化する関節空間での解、すなわちマウンティングアーム２０２、ガイドチューブ２１２、複数のツール２１４ａ、２１４ｂ及び内視鏡２１６を成す各関節の目標回転角度ｑを算出する。冗長自由度活用部１４６Ａは、目的関数算出部１４５Ａで算出された目的関数ｗを前述した数式（６）に代入し、目的関数ｗが代入された数式（６）を前述した数式（４）に代入した後、目的関数ｗを最小化する関節空間での各関節の速度

を算出する。冗長自由度活用部１４６Ａは、算出された関節空間での各関節の速度

を積分し、最終的な関節空間での解であるマウンティングアーム２０２、ガイドチューブ２１２、複数のツール２１４ａ、２１４ｂ及び内視鏡２１６を成す各関節の目標回転角度ｑを算出するようになる。

映像処理部１４７Ａは、スレーブロボット２００Ａの映像情報獲得部２２０Ａ、例えば内視鏡２１６及び／または超音波プローブ(図示せず)から入力された映像を画像イメージに出力するために入力された映像に対する処理を行う。ここで、映像処理の例としては、撮影された映像の拡大、縮小、回転、移動、編集及びフィルタリングなどを挙げることができる。

通信部１５０Ａは、有線通信網または無線通信網を通じてメイン制御部１４０Ａ及びスレーブロボット２００Ａの通信部２５０Ａに連結され、データを送受信する通信回路であって、冗長自由度活用部１４６Ａを通じて生成された制御信号（関節の目標回転角度、ｑ）をスレーブロボット２００Ａに送信するか、または映像情報獲得部２２０Ａを通じて獲得された映像情報（内視鏡映像情報及び／または超音波映像情報）などをスレーブロボット２００Ａから受信することができる。

表示部１１６Ａは、スレーブロボット２００の内視鏡２１６から伝達された内視鏡映像に相当する画像イメージ、スレーブロボット２００の超音波プローブ(図示せず)から伝達された超音波映像に相当する画像イメージまたは格納部１３０Ａに格納されている手術以前に撮影されたＸ線映像、コンピューター断層撮映（ＣＴ）映像及び磁気共鳴映像（ＭＲＩ）などの多様な参照映像などを視覚的情報として出力する。

スレーブロボット２００Ａは、マスター装置１００Ａから受信した制御信号によってマウンティングアーム２０２、ガイドチューブ２１２及び複数のツール２１４ａ、２１４ｂ、２１６を動作させて、直接患者に対して手術に必要な操作を行う。このようなスレーブロボット２００Ａは、図４に示されたように、映像情報獲得部２２０Ａ、格納部２３０Ａ、スレーブ制御部２４０Ａ、通信部２５０Ａ、サーボ制御部２６０Ａ及び駆動部２７０Ａを含むことができる。

映像情報獲得部２２０Ａは、患者の体内に挿入されて移動しながら内臓臓器や体腔内部を撮影し、手術部位の映像情報を獲得する。映像情報獲得部２２０Ａは、内視鏡２１６及び超音波プローブ(図示せず)などで具現されることができる。映像情報獲得部２２０Ａを通じて獲得された映像情報は、スレーブ制御部２４０Ａ内の映像処理部２４７Ａに伝達され、映像処理過程を経るか、または映像処理過程なしに通信部２５０Ａを通じてマスター装置１００Ａに伝送されることができる。

格納部２３０Ａは、スレーブロボット２００Ａの動作を制御するのに必要な情報及びアルゴリズム、スレーブロボット２００Ａ側で獲得した情報などを格納する。例えば、格納部２３０Ａ内には、映像情報獲得部２２０Ａを通じて獲得された手術部位に対する映像情報が格納される。

スレーブ制御部２４０Ａは、スレーブロボット２００を成す各種構成要素を連結し、スレーブロボット２００の動作を制御するためのプロセッサであって、映像情報獲得部２２０Ａで獲得された手術部位の映像情報を通信部２５０Ａを通じてマスター装置１００Ａに伝送するか、またはメイン制御部１４０Ａ内の冗長自由度活用部１４６Ａで生成された制御信号（各関節の目標回転角度）を通信部２５０Ａを通じて受信し、サーボ制御部２６０Ａに伝達する役目を行う。また、スレーブ制御部２４０Ａは、映像情報獲得部２２０Ａを通じて獲得された手術部位の映像に対する処理を行う映像処理部２４７Ａを含むことができる。ここで、映像処理の例としては、撮影された映像の拡大、縮小、回転、移動、編集及びフィルタリングなどを挙げることができる。スレーブ制御部２４０Ａ内で行われる映像処理過程は、場合によって省略されることもできる。

通信部２５０Ａは、有線通信網または無線通信網を通じてスレーブ制御部２４０Ａ及びマスター装置１００Ａの通信部１５０Ａに連結され、データを送受信する通信回路であって、マスター装置１００Ａから冗長自由度を活用して生成された制御信号（各関節の目標回転角度）を受信するか、または映像情報獲得部２２０Ａを通じて獲得された映像情報（内視鏡映像情報及び／または超音波映像情報）などをマスター装置１００Ａに送信することができる。

サーボ制御部２６０Ａは、メイン制御部１４０Ａ内の冗長自由度活用部１４６Ａから伝達された各関節の目標回転角度ｑを追従するための関節トルクτを算出し、算出された関節トルクτに対応するトルク制御信号を生成し、マウンティングアーム２０２、ガイドチューブ２１２、それぞれのツール２１４ａ、２１４ｂ及び内視鏡２１６を成す各関節を回転駆動する駆動部２７０Ａに出力する。

駆動部２７０Ａは、マウンティングアーム２０２、ガイドチューブ２１２、それぞれのツール２１４ａ、２１４ｂ及び内視鏡２１６を成す複数の関節それぞれに電気または油圧による動力を伝達するためのモーターなどのアクチュエータであって、サーボ制御部２６０Ａから伝達されたトルク制御信号によってマウンティングアーム２０２、ガイドチューブ２１２、それぞれのツール２１４ａ、２１４ｂ及び内視鏡２１６を成す各関節を回転駆動する。先立って図３Ａ〜図３Ｃで例示したマウンティングアーム２０２、マウンティングアーム２０２に連結設置されるガイドチューブ２１２、手術作業時にガイドチューブ２１２から分岐して出る２つのツール２１４ａ、２１４ｂ及び１つの内視鏡２１６を含むスレーブロボット２００において、マウンティングアーム２０２、ガイドチューブ２１２、各ツール２１４ａ、２１４ｂの関節空間での自由度は、６であり、内視鏡２１６の関節空間での自由度は３であるから、スレーブロボット２００側には、全体２７個の回転関節が設けられる。したがって、この回転関節を駆動するために、全体２７個のアクチュエータを必要とするようになる。

図１０は、手術ロボットの制御方法を示す流れ図である。

本実施例の動作説明のための初期条件として、スレーブロボット２００Ａは、冗長自由度を有し、マウンティングアーム２０２、ガイドチューブ２１２、２つのツール２１４ａ、２１４ｂ及び１つの内視鏡２１６それぞれは、複数のリンク及び複数の関節を含んでなるという点を前提する。また、マウンティングアーム２０２とガイドチューブ２１２が互いに連動して動作し、ガイドチューブ２１２とそれぞれのツール２１４ａ、２１４ｂが互いに連動して動作し、ガイドチューブ２１２と１つの内視鏡２１６が互いに連動して動作するものと前提する。また、格納部１３０Ａには、マスター装置１００Ａのマスター操作機１１２Ｌ、１１２Ｒの動作とスレーブロボット２００Ａの末端部(複数のツール及び内視鏡)の動作との間の動作スケーリング（ｍｏｔｉｏｎ ｓｃａｌｉｎｇ）時に適用されるスケーリングファクター、ヤコビアン行列算出時に必要とするアルゴリズム及びスレーブロボット２００Ａの運動学的構造に関する情報、目的関数算出時に必要とする複数の個別目的それぞれを達成するための複数の個別目的関数、手術作業を成す複数の動作に対する学習結果及び手術作業がどんなものであるかによって各個別目的関数に乗算される加重値などがあらかじめ格納されていることを前提する。

まず、手術が始まって、マスター操作機１１２Ｌ、１１２Ｒの操作者（手術ロボットを使用する医者）が手術作業のためにマスター操作機１１２Ｌ、１１２Ｒを通じて一定の動作を取れば、マスター装置１００Ａの位置／姿勢検出部１２０Ａは、マスター操作機１１２Ｌ、１１２Ｒの位置情報ｘ’、ｙ’、ｚ’及び姿勢情報α’、β’、γ’を検出し、検出されたマスター操作機１１２Ｌ、１１２Ｒの位置情報ｘ’、ｙ’、ｚ’及び姿勢情報α’、β’、γ’をメイン制御部１４０Ａに伝送する（３１０）。

次に、メイン制御部１４０Ａ内の変換部１４１Ａは、位置／姿勢検出部１２０Ａから獲得したマスター操作機１１２Ｌ、１１２Ｒの位置及び姿勢情報ｘ’、ｙ’、ｚ’、α’、β’、γ’をスレーブロボット２００Ａの作業空間でのモーション命令情報すなわち、操作者が所望のスレーブロボット２００Ａの末端部（複数のツールの各末端部及び内視鏡の末端部）の位置及び姿勢情報ｘ、ｙ、ｚ、α、β、γに変換する（３２０）。この際、変換部１４１Ａは、マスター操作機１１２Ｌ、１１２Ｒの位置及び姿勢情報ｘ’、ｙ’、ｚ’、α’、β’、γ’にマスター装置１００Ａのマスター操作機１１２Ｌ、１１２Ｒの動作とスレーブロボット２００Ａの末端部（複数のツール及び内視鏡）の動作との間の動作スケーリング時にスケーリングファクター（ｓｃａｉｎｇ ｆａｃｔｏｒ）を乗算し、スレーブロボット２００Ａの作業空間でのモーション命令情報ｘ、ｙ、ｚ、α、β、γを算出することができる。

その後、メイン制御部１４０Ａの冗長自由度逆運動学解析部１４２Ａ内のヤコビアン行列算出部１４４Ａは、格納部１３０Ａに既格納されているヤコビアン行列を算出するためのアルゴリズム及びスレーブロボット２００Ａの運動学的構造に関する情報（例えば、関節と関節の間を連結するリンクの長さ情報など）を利用して前述した数式（２）、すなわち微分運動学方程式

で関節の速度ベクトル

に乗算されるヤコビアン行列Ｊ（ｑ）を算出する(３３０)。

次に、メイン制御部１４０Ａの冗長自由度逆運動学解析部１４２Ａ内の目的関数算出部１４５Ａは、スレーブロボット２００Ａが行う手術作業がどんなものであるかを判断し、当該判断結果に基づいて各個別目的関数に乗算される加重値ａ、ｂ、ｃ、…を決定した後、決定された各加重値を各個別目的関数に乗算し、システム全体の目的関数ｗを算出する（３４０）。この際、目的関数算出部１４５Ａは、位置／姿勢検出部１２０Ａ及び速度検出部１２５Ａを通じて検出されたマスター操作機１１２Ｌ、１１２Ｒの動作に関する情報（マスター操作機の位置情報及び速度情報）及び格納部１３０Ａに既格納されている手術作業を成す複数の動作に対する学習結果を利用して操作者が行おうとする手術作業、すなわちスレーブロボット２００Ａが行う手術作業がどんなものであるかを予測し、予測された手術作業（例えば、配管挿入）によって各個別目的関数に乗算される加重値ａ、ｂ、ｃ、…を格納部１３０Ａから検索した後、検索された各加重値ａ、ｂ、ｃ、…を各個別目的関数に乗算し、システム全体の目的関数ｗを算出することができる。

その後、メイン制御部１４０Ａの冗長自由度逆運動学解析部１４２Ａ内の冗長自由度活用部１４６Ａは、スレーブロボット２００Ａの冗長自由度を活用してマウンティングアーム２０２、ガイドチューブ２１２、複数のツール２１４Ａ、２１４ｂ及び内視鏡２１６を成す各関節の目標回転角度ｑを算出する(３５０)。この際、冗長自由度活用部１４６Ａは、目的関数算出部１４５Ａを通じて算出されたシステム全体の目的関数ｗを最小化する関節空間での各関節の速度

を算出し、算出された関節空間での各関節の速度

を積分し、最終的な関節空間での解であるマウンティングアーム２０２、ガイドチューブ２１２、複数のツール２１４ａ、２１４ｂ及び内視鏡２１６を成す各関節の目標回転角度ｑを算出するようになる。

次に、メイン制御部１４０Ａは、冗長自由度活用部１４６Ａで算出した各関節の目標回転角度ｑを通信部１５０Ａを通じてスレーブロボット２００Ａ側のスレーブ制御部２４０Ａに伝送し、スレーブ制御部２４０Ａは、メイン制御部１４０Ａから伝送された各関節の目標回転角度ｑをサーボ制御部２６０Ａに伝送する（３６０）。

その後、スレーブロボット２００Ａ側サーボ制御部２６０Ａは、メイン制御部１４０Ａ内の冗長自由度活用部１４６Ａから伝達された各関節の目標回転角度ｑを追従するための関節トルクτを算出し、算出された関節トルクτに対応するトルク制御信号を生成する（３７０）。

次に、サーボ制御部２６０Ａは、生成されたトルク制御信号をマウンティングアーム２０２、ガイドチューブ２１２、それぞれのツール２１４ａ、２１４ｂ及び内視鏡２１６を成す各関節を回転駆動する駆動部２７０Ａに伝送する（３８０）。

このような過程を通じてスレーブロボット２００Ａの冗長自由度を活用して多様な個別目的（ツールの作業領域の拡張、ツールの関節別必要剛性の最小化、ツールと周辺障害物との間の衝突可能性の最小化、ツールの必要自由度の最小化及び複合的な作業実行）を達成しながらスレーブロボット２００Ａを成す各構成要素（マウンティングアーム、ガイドチューブ、複数のツール及び内視鏡）の動作を統合的に制御することができるようになる。

図１１は、手術ロボットの制御ブロック図である。図１１に示す手術ロボットは、図４に示す手術ロボットと比較して見れば、マスター装置１００Ｂのメイン制御部１４０Ｂの冗長自由度逆運動学解析部１４２Ｂ内に優先順位決定部１４３Ｂが追加された点で図４に示す手術ロボットと異なる。

ここでは、同一の名称と同一の参照符号を使用している構成要素については、説明を省略し（但し、参照符号において符号の後方に併記されたＡ、Ｂは、各実施例を区別するためのものである）、図１１に追加的に示す優先順位決定部１４３Ｂの構成及びこの優先順位決定部１４３Ｂによってその機能が変化される格納部１３０Ｂ及びメイン制御部１４０Ｂ内の冗長自由度逆運動学解析部１４２Ｂの構成のみについて説明する。

図１１に示す格納部１３０Ｂは、スレーブロボット２００Ｂの冗長自由度を活用して関節空間での解、すなわちマウンティングアーム２０２、ガイドチューブ２１２、複数のツール２１４ａ、２１４ｂ及び内視鏡２１６を成す各関節の目標回転角度を算出するのに必要な情報及びアルゴリズムなどが格納されるメモリであって、格納部１３０Ｂ内には、マスター装置１００Ｂのマスター操作機１１２Ｌ、１１２Ｒの動作とスレーブロボット２００Ｂの末端部（複数のツール及び内視鏡）の動作との間の動作スケーリング（ｍｏｔｉｏｎ ｓｃａｌｉｎｇ）時に適用されるスケーリングファクター、作業変数ｐ、すなわち複数のツール２１４ａ、２１４ｂの位置／姿勢情報及び内視鏡２１６の姿勢情報に設定された優先順位（例えば、複数のツール２１４ａ、２１４ｂの位置及び姿勢情報と内視鏡２１６の姿勢情報のうち複数のツール２１４ａ、２１４ｂの位置及び姿勢情報にさらに高い優先順位が設定されるか、または第１ツール２１４ａの末端部の位置情報ｘ_１、ｙ_１、ｚ_１と姿勢情報α_１、β_１、γ_１のうち位置情報ｘ_１、ｙ_１、ｚ_１にさらに高い優先順位が設定される）、ヤコビアン行列算出時に必要とするアルゴリズム及びスレーブロボット２００Ｂの運動学的構造に関する情報、目的関数算出時に必要とする複数の個別目的それぞれを達成するための複数の個別目的関数、手術作業を成す複数の動作に対する学習結果及び手術作業がどんなものであるかによって各個別目的関数に乗算される加重値などが格納される。

また、格納部１３０Ｂは、手術以前に撮影されたＸ線映像、コンピュータ断層撮映（ＣＴ）映像及び磁気共鳴映像(ＭＲＩ)などの多様な参照映像などを格納することができる。

図１１に示されたように、メイン制御部１４０Ｂ内の冗長自由度逆運動学解析部１４２Ｂは、スレーブロボット２００Ｂの冗長自由度を活用してスレーブロボット２００Ｂを成す各構成要素（マウンティングアーム、ガイドチューブ、複数のツール及び内視鏡）の動作を統合的に制御するための制御信号（各構成要素を成す複数の関節それぞれの目標回転角度）を生成する構成部で、冗長自由度逆運動学解析部１４２Ｂは、さらに、優先順位決定部１４３Ｂ、ヤコビアン行列算出部１４４Ｂ、目的関数算出部１４５Ｂ及び冗長自由度活用部１４６Ｂを含んでなる。

冗長自由度逆運動学解析部１４２Ｂを成すそれぞれの構成部に対する詳細な説明に先立って、以下では、まず、冗長自由度を有するスレーブロボット２００Ｂで作業変数ｐに優先順位が設定されたときの逆運動学解析について説明する。

システムが冗長自由度を有する場合には、ヤコビアン行列Ｊ（ｑ）の疑似逆行列（Ｐｓｅｕｄｏ−ｉｎｖｅｒｓｅ Ｍａｔｒｉｘ）Ｊ^＃（ｑ）を利用して逆運動学的解を算出するという点は、先立って言及したことがある。

この際、作業変数ｐに優先順位を設定する場合、例えば、作業変数ｐを優先順位によってｐ_１、ｐ_２に分けて、ｐ_１にさらに高い優先順位を設定するとき、ヤコビアン行列の疑似逆行列を利用して関節空間での各関節の速度

を算出する式は、下記の数式（７）のように示すことができ、数式（７）に記載された

は、下記の数式（８）のように定義することができる。

ここで、ｐ_１とｐ_２は、作業変数ｐを優先順位によって分類したサブベクトル（ｓｕｂ ｖｅｃｔｏｒ）であり、Ｊ_１とＪ_２は、それぞれｐ_１とｐ_２に対応するヤコビアン行列であり、Ｉ_ｎは、ｎ×ｎ単位行列（ｕｎｉｔ ｍａｔｒｉｘ、ｉｄｅｎｔｉｔｙ ｍａｔｒｉｘ）であり、ｒは、任意のベクトルである。

作業変数ｐに優先順位を付与する方式について例を取って説明すれば、複数のツール２１４ａ、２１４ｂの位置及び姿勢情報と内視鏡２１６の姿勢情報（内視鏡がｒｏｌｌ、ｐｉｔｃｈ、ｙａｗ方向の回転３自由度を有する場合）のうち複数のツール２１４ａ、２１４ｂの位置及び姿勢情報にさらに高い優先順位を設定するか（互いに異なるツールの位置／姿勢情報の間に優先順位が異なる場合）、または第１ツール２１４ａの末端部の位置情報ｘ_１、ｙ_１、ｚ_１と姿勢情報α_１、β_１、γ_１のうち位置情報ｘ_１、ｙ_１、ｚ_１にさらに高い優先順位を設定することもできる（１つのツール内で位置情報と姿勢情報の優先順位が異なる場合）。

作業変数ｐを優先順位によって分類した場合にも、冗長自由度を活用するために任意のベクトルｒ（ｑ）を定義し、個別目的関数ｗ_１〜ｗ_ｎ及びシステム全体の目的関数ｗを定義する内容は、数式（６）及び図５〜図９に関連した説明部分に記載した内容と同一なので、ここでは詳細な説明を省略する。

さらに図１１に対する説明に戻って、冗長自由度逆運動学解析部１４２Ｂを成すそれぞれの構成部について詳細に説明する。

優先順位決定部１４３Ｂは、変換部１４１Ｂを通じて変換されたスレーブロボット２００Ｂの末端部（複数のツールの各末端部及び内視鏡の末端部）の位置及び姿勢情報ｘ、ｙ、ｚ、α、β、γをあらかじめ設定された優先順位によって分類する。ここで、スレーブロボット２００Ｂの末端部の位置及び姿勢情報、すなわち作業変数ｐを優先順位によってｐ_１、ｐ_２に分類し、且つｐ_１に設定された優先順位がｐ_２に設定された優先順位より高いと前提する。例えば、複数のツール２１４ａ、２１４ｂの位置及び姿勢情報と内視鏡２１６の姿勢情報のうち複数のツール２１４ａ、２１４ｂの位置及び姿勢情報にさらに高い優先順位が設定されている場合（互いに異なるツールの位置／姿勢情報の間に優先順位が異なる場合）、優先順位決定部１４３Ｂは、複数のツール２１４ａ、２１４ｂの位置及び姿勢情報をｐ_１に決定し、内視鏡２１６の姿勢情報をｐ２に決定する。また、例えば、第１ツール２１４ａの末端部の位置情報ｘ_１、ｙ_１、ｚ_１と姿勢情報α_１、β_１、γ_１のうち位置情報ｘ_１、ｙ_１、ｚ_１にさらに高い優先順位が設定されている場合（１つのツール内で位置と姿勢の優先順位が異なる）場合、優先順位決定部１４３Ｂは、第１ツール２１４ａの末端部の位置情報ｘ_１、ｙ_１、ｚ_１をｐ_１に決定し、第１ツール２１４ａの末端部の姿勢情報α_１、β_１、γ_１をｐ_２に決定する。

ヤコビアン行列算出部１４４Ｂは、ヤコビアン行列を算出するためのアルゴリズムにスレーブロボット２００Ｂの運動学的構造に関する情報（例えば、関節と関節の間を連結するリンクの長さ情報など）を入力し、作業変数ｐのうち優先順位が高いｐ_１に対応するヤコビアン行列Ｊ_１(ｑ)と作業変数のうち相対的に優先順位が低いｐ_２に対応するヤコビアン行列Ｊ_２(ｑ)を算出する。

目的関数算出部１４５Ｂは、冗長自由度を有するスレーブロボット２００Ｂから作業変数ｐのうち優先順位が高いｐ_１に対応するヤコビアン行列Ｊ_１(ｑ)の疑似逆行列Ｊ_１ ^＃(ｑ)及び作業変数ｐのうち優先順位が低いｐ_２に対応するヤコビアン行列Ｊ_２(ｑ)の疑似逆行列Ｊ_２ ^＃(ｑ)を利用して関節空間での各関節の速度

を算出する数式（７）、すなわち

に記載した任意のベクトルｒ（ｑ）を表現する数式（６）、すなわち

に記載した目的関数ｗを算出する。このような目的関数(ｗ、システム全体の目的関数)は、複数の個別目的関数ｗ_１〜ｗ_ｎの加重和（ｗｅｉｇｈｔｅｄ ｓｕｍ）で示すことができる（ｗ＝ａｗ_１＋ｂｗ_２＋ｃｗ_３＋…）。個別目的関数の例としては、それぞれのツール２１４ａ、２１４ｂと器具的関節限界との間の距離の逆数、複数のツール２１４ａ、２１４ｂ及び内視鏡２１６を成す各関節のトルク二乗の和、それぞれのツール２１４ａ、２１４ｂと周辺障害物との間の距離の逆数などを挙げることができる。この際、システム全体の目的関数ｗは、時間の流れによって変化するようになる。すなわち、スレーブロボット２００が行う手術作業がどんなものであるかによって各個別目的関数に乗算される加重値ａ、ｂ、ｃ、…が変わる。目的関数算出部１４５Ｂは、スレーブロボット２００Ｂが行う手術作業がどんなものであるかによって各個別目的関数に乗算される加重値ａ、ｂ、ｃ、…を決定する。目的関数算出部１４５Ｂは、位置／姿勢検出部１２０Ｂ及び速度検出部１２５Ｂを通じて検出されたマスター操作機１１２Ｌ、１１２Ｒの動作に関する情報（マスター操作機の位置情報及び速度情報）及び格納部１３０Ｂに既格納された手術作業を成す複数の動作に対する学習結果を利用して操作者が行おうとする手術作業を予測する。ここで、手術作業は、縫合（ｓｕｔｕｒｉｎｇ）、パス（ｐａｓｓｉｎｇ）、ランニング（ｒｕｎｎｉｎｇ）及び配管挿入（ｃａｎｎｕｌａｔｉｏｎ）のうちいずれか１つであり、手術作業を成す複数の動作は、オリエント（ｏｒｉｅｎｔ）、プッシュ（ｐｕｓｈ）及びプル（ｐｕｌｌ）のうち少なくとも１つを含む。これを通じて、目的関数算出部１４５Ｂは、スレーブロボット２００が行う手術作業がどんなものであるかを判断し、当該判断結果に基づいて各個別目的関数に乗算される加重値ａ、ｂ、ｃ、…を決定した後、決定された各加重値を各個別目的関数に乗算し、システム全体の目的関数ｗを算出する。

冗長自由度活用部１４６Ｂは、システム全体の目的関数ｗを最小化する関節空間での解、すなわちマウンティングアーム２０２、ガイドチューブ２１２、複数のツール２１４ａ、２１４ｂ及び内視鏡２１６を成す各関節の目標回転角度ｑを算出する。冗長自由度活用部１４６Ｂは、目的関数算出部１４５Ｂで算出された目的関数ｗを前述した数式（６）に代入し、目的関数ｗが代入された数式（６）を前述した数式（７）に代入した後、目的関数ｗを最小化する関節空間での各関節の速度

を算出する。冗長自由度活用部１４６Ｂは、算出された関節空間での各関節の速度

を積分し、最終的な関節空間での解であるマウンティングアーム２０２、ガイドチューブ２１２、複数のツール２１４ａ、２１４ｂ及び内視鏡２１６を成す各関節の目標回転角度ｑを算出するようになる。

図１２は、手術ロボットの制御方法を示す流れ図である。

本実施例の動作説明のための初期条件として、スレーブロボット２００Ｂは、冗長自由度を有し、マウンティングアーム２０２、ガイドチューブ２１２、２つのツール２１４ａ、２１４ｂ及び１つの内視鏡２１６それぞれは、複数のリンク及び複数の関節を含んでなるという点を前提する。また、マウンティングアーム２０２とガイドチューブ２１２が互いに連動して動作し、ガイドチューブ２１２とそれぞれのツール２１４ａ、２１４ｂが互いに連動して動作し、ガイドチューブ２１２と１つの内視鏡２１６が互いに連動して動作するものと前提する。また、格納部１３０Ｂには、マスター装置１００Ｂのマスター操作機１１２Ｌ、１１２Ｒの動作とスレーブロボット２００Ｂの末端部（複数のツール及び内視鏡）の動作との間の動作スケーリング（ｍｏｔｉｏｎ ｓｃａｌｉｎｇ）時に適用されるスケーリングファクター、作業変数ｐすなわち複数のツール２１４ａ、２１４ｂの位置／姿勢情報及び内視鏡２１６の姿勢情報に設定された優先順位、ヤコビアン行列算出時い必要とするアルゴリズム及びスレーブロボット２００Ｂの運動学的構造に関する情報、目的関数算出時に必要とする複数の個別目的それぞれを達成するための複数の個別目的関数、手術作業を成す複数の動作に対する学習結果及び手術作業がどんなものであるかによって各個別目的関数に乗算される加重値などがあらかじめ格納されていることを前提する。

まず、手術が始まって、マスター操作機１１２Ｌ、１１２Ｒの操作者（手術ロボットを使用する医者）が手術作業のためにマスター操作機１１２Ｌ、１１２Ｒを通じて一定の動作を取れば、マスター装置１００Ｂの位置／姿勢検出部１２０Ａは、マスター操作機１１２Ｌ、１１２Ｒの位置情報ｘ’、ｙ’、ｚ’及び姿勢情報α’、β’、γ’を検出し、検出されたマスター操作機１１２Ｌ、１１２Ｒの位置情報ｘ’、ｙ’、ｚ’及び姿勢情報α’、β’、γ’をメイン制御部１４０Ｂに伝送する（４１０）。

次に、メイン制御部１４０Ｂ内の変換部１４１Ｂは、位置／姿勢検出部１２０Ｂから獲得したマスター操作機１１２Ｌ、１１２Ｒの位置及び姿勢情報ｘ’、ｙ’、ｚ’、α’、β’、γ’をスレーブロボット２００Ｂの作業空間でのモーション命令情報、すなわち操作者が所望のスレーブロボット２００Ｂの末端部(複数のツールの各末端部及び内視鏡の末端部)の位置及び姿勢情報ｘ、ｙ、ｚ、α、β、γに変換する（４２０）。この際、変換部１４１Ｂは、マスター操作機１１２Ｌ、１１２Ｒの位置及び姿勢情報ｘ’、ｙ’、ｚ’、α’、β’、γ’にマスター装置１００Ｂのマスター操作機１１２Ｌ、１１２Ｒの動作とスレーブロボット２００Ｂの末端部（複数のツール及び内視鏡）の動作との間の動作スケーリング時にスケーリングファクター（ｓｃａｉｎｇ ｆａｃｔｏｒ）を乗算し、スレーブロボット２００Ｂの作業空間でのモーション命令情報ｘ、ｙ、ｚ、α、β、γを算出することができる。

その後、メイン制御部１４０Ｂの冗長自由度逆運動学解析部１４２Ｂ内の優先順位決定部１４３Ｂは、変換部１４１Ｂを通じて変換されたスレーブロボット２００Ｂの末端部(複数のツールの各末端部及び内視鏡の末端部)の位置及び姿勢情報ｘ、ｙ、ｚ、α、β、γ、すなわち作業変数ｐをあらかじめ設定された優先順位によって分類する(４３０)。作業変数ｐを優先順位によってｐ_１、ｐ_２に分類し、且つｐ_１に設定された優先順位がｐ_２に設定された優先順位より高いと前提するとき、例えば、複数のツール２１４ａ、２１４ｂの位置及び姿勢情報と内視鏡２１６の姿勢情報のうち複数のツール２１４ａ、２１４ｂの位置及び姿勢情報にさらに高い優先順位が設定されている場合、優先順位決定部１４３Ｂは、複数のツール２１４ａ、２１４ｂの位置及び姿勢情報をｐ_１に決定し、内視鏡２１６の姿勢情報をｐ_２に決定する。

次に、メイン制御部１４０Ｂの冗長自由度逆運動学解析部１４２Ｂ内のヤコビアン行列算出部１４４Ａは、格納部１３０Ｂに既格納されているヤコビアン行列を算出するためのアルゴリズム及びスレーブロボット２００Ｂの運動学的構造に関する情報（例えば、関節と関節の間を連結するリンクの長さ情報など）を利用して作業変数ｐのうち優先順位が高いｐ_１に対応するヤコビアン行列Ｊ_１(ｑ)と作業変数のうち相対的に優先順位が低いｐ_２に対応するヤコビアン行列Ｊ_２(ｑ)を算出する(４４０)。

その後、メイン制御部１４０Ｂの冗長自由度逆運動学解析部１４２Ｂ内の目的関数算出部１４５Ｂは、スレーブロボット２００Ｂが行う手術作業がどんなものであるかを判断し、当該判断結果に基づいて各個別目的関数に乗算される加重値ａ、ｂ、ｃ、…を決定した後、決定された各加重値を各個別目的関数に乗算し、システム全体の目的関数ｗを算出する（４５０）。この際、目的関数算出部１４５Ｂは、位置／姿勢検出部１２０Ｂ及び速度検出部１２５Ｂを通じて検出されたマスター操作機１１２Ｌ、１１２Ｒの動作に関する情報（マスター操作機の位置情報及び速度情報）及び格納部１３０Ｂに既格納されている手術作業を成す複数の動作に対する学習結果を利用して操作者が行おうとする手術作業、すなわちスレーブロボット２００Ｂが行う手術作業がどんなものであるかを予測し、予測された手術作業（例えば、配管挿入）によって各個別目的関数に乗算される加重値ａ、ｂ、ｃ、…を格納部１３０Ａで検索した後、検索された各加重値ａ、ｂ、ｃ、…を各個別目的関数に乗算し、システム全体の目的関数ｗを算出することができる。

次に、メイン制御部１４０Ｂの冗長自由度逆運動学解析部１４２Ａ内の冗長自由度活用部１４６Ｂは、スレーブロボット２００Ｂの冗長自由度を活用してマウンティングアーム２０２、ガイドチューブ２１２、複数のツール２１４Ａ、２１４ｂ及び内視鏡２１６を成す各関節の目標回転角度ｑを算出する（４６０）。この際、冗長自由度活用部１４６Ｂは、目的関数算出部１４５Ｂを通じて算出されたシステム全体の目的関数ｗを最小化する関節空間での各関節の速度

を算出し、算出された関節空間での各関節の速度

を積分し、最終的な関節空間での解であるマウンティングアーム２０２、ガイドチューブ２１２、複数のツール２１４ａ、２１４ｂ及び内視鏡２１６を成す各関節の目標回転角度ｑを算出するようになる。

その後、メイン制御部１４０Ｂは、冗長自由度活用部１４６Ｂで算出した各関節の目標回転角度ｑを通信部１５０Ｂを通じてスレーブロボット２００Ｂ側スレーブ制御部２４０Ｂに伝送し、スレーブ制御部２４０Ｂは、メイン制御部１４０Ｂから伝送された各関節の目標回転角度ｑをサーボ制御部２６０Ｂに伝送する(４７０)。

次に、スレーブロボット２００Ｂ側のサーボ制御部２６０Ｂは、メイン制御部１４０Ｂ内の冗長自由度活用部１４６Ｂから伝達された各関節の目標回転角度ｑを追従するための関節トルクτを算出し、算出された関節トルクτに対応するトルク制御信号を生成する(４８０)。

その後、サーボ制御部２６０Ｂは、生成されたトルク制御信号をマウンティングアーム２０２、ガイドチューブ２１２、それぞれのツール２１４ａ、２１４ｂ及び内視鏡２１６を成す各関節を回転駆動する駆動部２７０Ｂに伝送する（４９０）。

このような過程を通じてスレーブロボット２００Ｂの冗長自由度を活用して多様な個別目的（ツールの作業領域の拡張、ツールの関節別必要剛性の最小化、ツールと周辺障害物との間の衝突可能性の最小化、ツールの必要自由度の最小化及び複合的な作業実行）を達成しながらスレーブロボット２００Ｂを成す各構成要素（マウンティングアーム、ガイドチューブ、複数のツール及び内視鏡）の動作を統合的に制御することができるようになる。

図１３は、ヒューマノイドロボットの外観構成図である。図１３に示されたように、ヒューマノイドロボット５００は、人間と同様に、２つの脚５１８Ｌ、５１８Ｒによって直立移動する二足歩行ロボットであって、頭５０２、胴体５０３、腕５０４Ｌ、５０４Ｒよりなる上部ボディー５０１と、２つの脚５１８Ｌ、５１８Ｒよりなる下部ボディー５１７を備える。

ヒューマノイドロボット５００の上部ボディー５０１は、胴体５０３と、胴体５０３の上部に首５０５を通じて連結された頭５０２と、胴体５０３の上部両側に肩５０６Ｌ、５０６Ｒを通じて連結された２つの腕５０４Ｌ、５０４Ｒと、この２つの腕５０４Ｌ、５０４Ｒの末端にそれぞれ連結された手５０７Ｌ、５０７Ｒよりなる。頭５０２で人間の目の形状を有する器具部には、移動空間の周囲を撮影するためのカメラ５１１が装着される。

２つの腕５０４Ｌ、５０４Ｒは、多自由度を持って駆動するように具現することができる。２つの腕５０４Ｌ、５０４Ｒは、複数のリンク及び複数の関節よりなる。特に、図１３に示されたように、左側腕５０４Ｌの末端部、すなわち左側手５０７Ｌには、ガイドチューブ５１２及びそれぞれエンドエフェクタ５１６ａ、５１６ｂ、５１６ｃを有し、ガイドチューブ５１２から分岐して出る複数のツール５１４ａ、５１４ｂ、５１４ｃよりなるマルチツールモジュール５１０が連結される。ヒューマノイドロボット５００が作業を行わないときには、複数のツール５１６ａ、５１６ｂ、５１６ｃがガイドチューブ５１２内に内蔵されていて、ヒューマノイドロボット５００が作業を行うときには、図１３に示されたように、ガイドチューブ５１２内に内蔵されている複数のツール５１４ａ、５１４ｂ、５１４ｃが外に出て、ユーザの命令による作業（例えば、掃除作業）を行う。以下では、ガイドチューブ５１２及び複数のツール５１４ａ、５１４ｂ、５１４ｃよりなるマルチツールモジュール５１０が連結される左側腕５０４Ｌを特にマウンティングアーム５０４Ｌとして定義する。

ここで、ガイドチューブ５１２、複数のツール５１４ａ、５１４ｂ、５１４ｃは、マウンティングアーム５０４Ｌと同様に、複数のリンク及び複数の関節よりなり、多自由度を持って駆動するように具現することができる。図１３に示す例で、複数のツール５１４ａ、５１４ｂ、５１４ｃそれぞれの末端部（ｄｉｓｔａｌ ｅｎｄ）には、直接的な掃除作業を行うごみを取り上げるためのやっとこ、空気中のほこりを集める集塵機、汚い所を拭き上げる雑巾などの掃除道具、すなわちエンドエフェクタ５１６ａ、５１６ｂ、５１６ｃが装着される。

ヒューマノイドロボット５００の下部ボディー５１７は、上部ボディー５０１の胴体５０３の下部両側に連結された２つの脚５１８Ｌ、５１８Ｒと、２つの脚５１８Ｌ、５１８Ｒの末端にそれぞれ連結された足５１９Ｌ、５１９Ｒよりなる。

参照符号において、“Ｒ”と“Ｌ”は、それぞれヒューマノイドロボット５００の左側（ｌｅｆｔ）と右側（ｒｉｇｈｔ）を示す。

図１４は、ヒューマノイドロボットの制御ブロック図である。

まず、本実施例では、ヒューマノイドロボット５００の左側の腕部分を成すマウンティングアーム５０４Ｌ、ガイドチューブ５１２、複数のツール５１４ａ、５１４ｂ、５１４ｃそれぞれが複数のリンク及び複数の関節を含んでなるという点を前提する。また、本実施例では、マウンティングアーム５０４Ｌとガイドチューブ５１２が互いに連動して動作し、ガイドチューブ５１２とそれぞれのツール５１４ａ、５１４ｂ、５１４ｃが互いに連動して動作するものと前提する。同時に、本実施例では、ヒューマノイドロボット５００Ａが冗長自由度（ｒｅｄｕｎｄａｎｃｙ）を有する場合、すなわちヒューマノイドロボット５００Ａの関節空間での自由度Ｎが作業空間での自由度Ｍより大きい場合（Ｎ＞Ｍ）を前提する。

図１４に示されたように、ヒューマノイドロボット５００Ａは、入力部５２０Ａ、映像情報獲得部５２５Ａ、格納部５３０Ａ、制御部５４０Ａ、サーボ制御部５５０Ａ、駆動部５６０Ａ及び位置／姿勢検出部５７０Ａを含むことができる。

入力部５２０Ａは、ユーザがヒューマノイドロボット５００Ａの動作命令（例えば、歩行命令、作業命令など）を入力するためのものであって、ユーザインターフェースＵＩや遠隔操作機などで構成することができる。

映像情報獲得部５２５Ａは、被写体から反射する光を検出し、これをデジタル信号に変換及び処理し、移動空間周辺の映像を獲得する。このために、映像情報獲得部５２５Ａは、移動空間の周囲を撮影するカメラ５１１と、カメラ５１１の出力を受信し、２次元イメージ映像及び３次元距離情報を生成する映像処理モジュールよりなる。カメラ５１１としては、ＣＣＤ（ｃｈａｒｇｅ−ｃｏｕｐｌｅｄ ｄｅｖｉｃｅ）カメラ、ＣＭＯＳ（ｃｏｍｐｌｅｍｅｎｔａｒｙ ｍｅｔａｌ ｏｘｉｄｅ）カメラ、ＴＯＦ（Ｔｉｍｅ ｏｆ Ｆｌｉｇｈｔ）カメラなどが利用されることができ、その他、ヒューマノイドロボット５００が歩行する経路上に位置する物体に対する映像情報を獲得することができる装置なら、どんな装置でも使用可能であることは勿論である。

格納部５３０Ａは、ヒューマノイドロボット５００Ａの冗長自由度を活用して関節空間での解、すなわちマウンティングアーム５０４Ｌ、ガイドチューブ５１２及び複数のツール５１４ａ、５１４ｂ、５１４ｃを成す各関節の目標回転角度を算出するのに必要な情報及びアルゴリズムなどが格納されるメモリであって、格納部５３０Ａ内には、ヤコビアン行列算出時に必要とするアルゴリズム及びヒューマノイドロボット５００Ａの運動学的構造に関する情報、目的関数算出時に必要とする複数の個別目的それぞれを達成するための複数の個別目的関数、行う作業がどんなものであるかによって各個別目的関数に乗算される加重値などが格納される。

また、格納部５３０Ａは、ヒューマノイドロボット５００Ａの位置認識結果及びＳＬＡＭアルゴリズムを利用して作成される移動空間に対する地図などを格納することができる。

制御部５４０Ａは、ヒューマノイドロボット５００Ａの全般的な動作を制御するためのプロセッサであって、制御部５４０Ａは、さらに、モーション命令生成部５４１Ａ、冗長自由度逆運動学解析部５４２Ａ及び位置推定部５４７Ａを含む。

モーション命令生成部５４１Ａは、入力部５２０Ａから伝送されたユーザ命令信号、映像情報獲得部５２５Ａから伝送された移動空間周辺の映像信号及び位置／姿勢検出部５７０Ａから伝送された複数のツール５１４ａ、５１４ｂ、５１４ｃの各末端部の３次元空間上での位置及び姿勢情報に基づいて複数のツール５１４ａ、５１４ｂ、５１４ｃに対する作業空間でのモーション命令ｘ、ｙ、ｚ、α、β、γを生成する。ここでは、モーション命令生成部５４１Ａを通じて生成された作業空間でのモーション命令ｘ、ｙ、ｚ、α、β、γが前述した作業変数ｐになる。

本実施例では、位置／姿勢フィードバック（ｆｅｅｄｂａｃｋ）制御のために作業空間でのモーション命令生成時に位置／姿勢検出部５７０Ａから伝送された複数のツール５１４ａ、５１４ｂ、５１４ｃの各末端部の３次元空間上での位置及び姿勢情報を反映する場合を例に取って説明したが、オープンループ(ｏｐｅｎ−ｌｏｏｐ)制御時には、複数のツール５１４ａ、５１４ｂ、５１４ｃの各末端部の３次元空間上での位置及び姿勢情報を反映せず、入力部５２０Ａから伝送されたユーザ命令信号及び映像情報獲得部５２５Ａから伝送された移動空間周辺の映像信号に基づいて複数のツール５１４ａ、５１４ｂ、５１４ｃに対する作業空間でのモーション命令を生成するようになる。

冗長自由度逆運動学解析部５４２Ａは、ヒューマノイドロボット５００の冗長自由度を活用してヒューマノイドロボット５００Ａを成す各構成要素(マウンティングアーム、ガイドチューブ、複数のツール)の動作を統合的に制御するための制御信号（各構成要素を成す複数の関節それぞれの目標回転角度）を生成する構成部であって、冗長自由度逆運動学解析部５４２Ａは、さらに、ヤコビアン行列算出部５４４Ａ、目的関数算出部５４５Ａ及び冗長自由度活用部５４６Ａを含んでなる。

ヤコビアン行列算出部５４４Ａは、作業変数ｐと関節変数ｑとの間の関係を示す数式（１）、すなわちｐ＝ｆ（ｑ）を時間に関して微分して獲得する数式（２）、すなわち微分運動学方程式

で関節の速度ベクトル

に乗算されるヤコビアン行列Ｊ（ｑ）を算出する。この際、ヤコビアン行列算出部５４４Ａは、ヤコビアン行列を算出するためのアルゴリズムにヒューマノイドロボット５００の運動学的構造に関する情報(例えば、関節と関節の間を連結するリンクの長さ情報など)を入力し、ｆ（ｑ）のヤコビアン行列Ｊ（ｑ）を算出する。

目的関数算出部５４５Ａは、冗長自由度を有するヒューマノイドロボット５００でヤコビアン行列Ｊ（ｑ）の疑似逆行列Ｊ^＃（ｑ）を利用して関節空間での各関節の速度

を算出する数式（４）、すなわち

に記載した任意のベクトルｒ（ｑ）を表現する数式（６）、すなわち

に記載した目的関数ｗを算出する。このような目的関数ｗ、システム全体の目的関数は、複数の個別目的関数ｗ_１〜ｗ_ｎの加重和(ｗｅｉｇｈｔｅｄ ｓｕｍ)で示すことができる(ｗ＝ａｗ_１＋ｂｗ_２＋ｃｗ_３＋…)。個別目的関数の例としては、それぞれのツール５１４ａ、５１４ｂ、５１４ｃと器具的関節限界との間の距離の逆数、複数のツール５１４ａ、５１４ｂ、５１４ｃを成す各関節のトルク二乗の和、それぞれのツール５１４ａ、５１４ｂ、５１４ｃと周辺障害物との間の距離の逆数などを挙げることができる。この際、システム全体の目的関数ｗは、時間の流れによって変化するようになる。すなわち、ヒューマノイドロボット５００Ａが行う作業がどんなものであるかによって各個別目的関数に乗算される加重値ａ、ｂ、ｃ、…が変わる。目的関数算出部５４５Ｂは、ヒューマノイドロボット５００Ａが行う作業がどんなものであるかを判断し、当該判断結果に基づいて各個別目的関数に乗算される加重値ａ、ｂ、ｃ、…を決定した後、決定された各加重値を各個別目的関数に乗算し、システム全体の目的関数ｗを算出する。

冗長自由度活用部５４６Ａは、システム全体の目的関数ｗを最小化する関節空間での解、すなわちマウンティングアーム５０４Ｌ、ガイドチューブ５１２及び複数のツール５１４ａ、５１４ｂ、５１４ｃを成す各関節の目標回転角度ｑを算出する。冗長自由度活用部５４６Ａは、目的関数算出部５４５Ａで算出された目的関数ｗを前述した数式（６）に代入し、目的関数ｗが代入された数式（６）を前述した数式（４）に代入した後、目的関数ｗを最小化する関節空間での各関節の速度

を算出する。冗長自由度活用部５４６Ａは、算出された関節空間での各関節の速度

を積分し、最終的な関節空間での解であるマウンティングアーム５０４Ｌ、ガイドチューブ５１２及び複数のツール５１４ａ、５１４ｂ、５１４ｃを成す各関節の目標回転角度ｑを算出するようになる。

位置推定部５４７Ａは、映像情報獲得部５２５Ａを通じて獲得された映像情報及びヒューマノイドロボット５００Ａを成す各リンクの運動学情報（長さ情報）と各回転関節の回転角度情報に基づいて算出されたオドメトリ情報にＳＬＡＭ（Ｓｉｍｕｌｔａｎｅｏｕｓ Ｌｏｃａｌｉｚａｔｉｏｎ Ａｎｄ Ｍａｐｐｉｎｇ）アルゴリズムを適用してヒューマノイドロボット５００自分の位置を推定すると同時に、移動空間に対応する地図を作成する。ＳＬＡＭアルゴリズムは、特徴点（ｆｅａｔｕｒｅ）の位置とヒューマノイドロボット５００の位置情報及び姿勢情報を１つの状態変数（ｓｔａｔｅ）に設定し、確率的なフィルタ技法を通じて同時に推定するようになり、その過程は、予測（Ｐｒｅｄｉｃｔｉｏｎ）段階、情報連合｛れんごう｝（Ｄａｔａ Ａｓｓｏｃｉａｔｉｏｎ）段階及び更新（Ｕｐｄａｔｅ）段階で成り立って繰り返し的に行われる。この時、確率フィルタとしては、拡張カルマンフィルタ（Ｅｘｔｅｎｄｅｄ ＫａｌｍａｎＦｉｌｔｅｒ）、粒子フィルタ（Ｐａｒｔｉｃｌｅ Ｆｉｌｔｅｒ）などが使用されることができる。

サーボ制御部５５０Ａは、制御部５４０Ａ内の冗長自由度活用部５４６Ａから伝達された各関節の目標回転角度ｑを追従するための関節トルクτを算出し、算出された関節トルクτに対応するトルク制御信号を生成し、マウンティングアーム５０４Ｌ、ガイドチューブ５１２及びそれぞれのツール５１４ａ、５１４ｂ、５１４ｃを成す各関節を回転駆動する駆動部５６０Ａに出力する。

駆動部５６０Ａは、マウンティングアーム５０４Ｌ、ガイドチューブ５１２及びそれぞれのツール５１４ａ、５１４ｂ、５１４ｃを成す複数の関節それぞれに電気または油圧による動力を伝達するためのモーターなどのアクチュエータであって、サーボ制御部５５０Ａから伝達されたトルク制御信号によってマウンティングアーム５０４Ｌ、ガイドチューブ５１２及びそれぞれのツール５１４ａ、５１４ｂ、５１４ｃを成す各関節を回転駆動する。ここで、マウンティングアーム５０４Ｌ、ガイドチューブ５１２及び各ツール５１４ａ、５１４ｂ、５１４ｃの関節空間での自由度がそれぞれ６である場合、ヒューマノイドロボット５００の左側の腕部分には、全体３０個の回転関節が設けられる。したがって、この回転関節を駆動するために、全体３０個のアクチュエータを必要とする。

位置／姿勢検出部５７０Ａは、ヒューマノイドロボット５００Ａの末端部（複数のツールの各末端部）の位置（ｐｏｓｉｔｉｏｎ）及び姿勢（方向、ｏｒｉｅｎｔａｔｉｏｎ）を検出する。位置／姿勢検出部５７０Ａは、複数のツール５１４ａ、５１４ｂ、５１４ｃを成す複数の関節それぞれに装着されて各関節の回転角度を検出する回転角センサー(図示せず)と、回転角センサーを通じて検出された各関節の回転角度を複数のツール５１４ａ、５１４ｂ、５１４ｃの順運動学式に代入して、複数のツール５１４ａ、５１４ｂ、５１４ｃの各末端部の３次元空間上での位置及び姿勢情報を算出する演算モジュールよりなる。回転角センサーとしては、エンコーダー（ｅｎｃｏｄｅｒ）、ポテンショメータ（ｐｏｔｅｎｔｉｏｍｅｔｅｒ）などを利用することができる。ここでは、回転角センサーと演算モジュールよりなる位置／姿勢検出部５７０Ａを例に取って説明したが、その他、複数のツール５１４ａ、５１４ｂ、５１４ｃの各末端部の位置及び姿勢に対する情報を検出することができる装置なら、どんな装置でも位置／姿勢検出部５７０Ａとして使用可能であることは勿論である。

図１５は、ヒューマノイドロボットの制御方法を示す流れ図である。

本実施例の動作説明のための初期条件として、ヒューマノイドロボット５００Ａ、特に図１３に示す左側の腕部分は、冗長自由度を有し、マウンティングアーム５０４Ｌ、ガイドチューブ５１２、３つのツール５１４ａ、５１４ｂ、５１４ｃそれぞれは、複数のリンク及び複数の関節を含んでなるという点を前提する。また、マウンティングアーム５０４Ｌとガイドチューブ５１２が互いに連動して動作し、ガイドチューブ５１２とそれぞれのツール５１４ａ、５１４ｂ、５１４ｃが互いに連動して動作するものと前提する。また、格納部５３０Ａには、ヤコビアン行列算出時に必要とするアルゴリズム及びヒューマノイドロボット５００の運動学的構造に関する情報、目的関数算出時に必要とする複数の個別目的それぞれを達成するための複数の個別目的関数、行う作業がどんなものであるかによって各個別目的関数に乗算される加重値などがあらかじめ格納されていることを前提する。

入力部５２０Ａを通じてユーザからヒューマノイドロボット５００Ａの作業命令（例えば、掃除命令）が入力されれば、ヒューマノイドロボット５００Ａの作業が始まる。

まず、ヒューマノイドロボット５００Ａの作業が始まれば、制御部５４０Ａは、位置／姿勢検出部５７０Ａから複数のツール５１４ａ、５１４ｂ、５１４ｃの各末端部の３次元空間上での位置及び姿勢情報を周期的に入力されながら作業を行う(６１０)。

次に、制御部５４０Ａ内のモーション命令生成部５４１Ａは、入力部５２０Ａから伝送されたユーザ命令信号、映像情報獲得部５２５Ａから伝送された移動空間周辺の映像信号及び位置／姿勢検出部５７０Ａから伝送された複数のツール５１４ａ、５１４ｂ、５１４ｃの各末端部の３次元空間上での位置及び姿勢情報に基づいて複数のツール５１４ａ、５１４ｂ、５１４ｃに対する作業空間でのモーション命令ｘ、ｙ、ｚ、α、β、γを生成する６２０。この際、モーション命令生成部５４１Ａを通じて生成された作業空間でのモーション命令ｘ、ｙ、ｚ、α、β、γが作業変数ｐになる。

その後、制御部５４０Ａの冗長自由度逆運動学解析部５４２Ａ内のヤコビアン行列算出部５４４Ａは、格納部５３０Ａに既格納されているヤコビアン行列を算出するためのアルゴリズム及びヒューマノイドロボット５００Ａの運動学的構造に関する情報（例えば、関節と関節の間を連結するリンクの長さ情報など）を利用して前述した数式（２）、すなわち微分運動学方程式

で関節の速度ベクトル

に乗算されるヤコビアン行列Ｊ（ｑ）を算出する(６３０)。

次に、制御部５４０Ａの冗長自由度逆運動学解析部５４２Ａ内の目的関数算出部５４５Ａは、ヒューマノイドロボット５００Ａが行う作業がどんなものであるか判断し、当該判断結果に基づいて各個別目的関数に乗算される加重値ａ、ｂ、ｃ、…を決定した後、決定された各加重値を各個別目的関数に乗算し、システム全体の目的関数ｗを算出する（６４０）。

その後、制御部５４０Ａの冗長自由度逆運動学解析部５４２Ａ内の冗長自由度活用部５４６Ａは、ヒューマノイドロボット５００Ａの冗長自由度を活用してマウンティングアーム５０４Ｌ、ガイドチューブ５１２及び複数のツール５１４Ａ、５１４ｂ、５１４ｃを成す各関節の目標回転角度ｑを算出する（６５０）。この際、冗長自由度活用部５４６Ａは、目的関数算出部５４５Ａを通じて算出されたシステム全体の目的関数ｗを最小化する関節空間での各関節の速度

を算出し、算出された関節空間での各関節の速度

を積分し、最終的な関節空間での解であるマウンティングアーム５０４Ｌ、ガイドチューブ５１２及び複数のツール５１４Ａ、５１４ｂ、５１４ｃを成す各関節の目標回転角度ｑを算出するようになる。

次に、制御部５４０Ａは、冗長自由度活用部５４６Ａで算出した各関節の目標回転角度ｑをサーボ制御部５５０Ａに伝送する（６６０）。

その後、サーボ制御部５５０Ａは、制御部５４０Ａ内の冗長自由度活用部５４６Ａから伝達された各関節の目標回転角度ｑを追従するための関節トルクτを算出し、算出された関節トルクτに対応するトルク制御信号を生成する(６７０)。

次に、サーボ制御部５５０Ａは、生成されたトルク制御信号をマウンティングアーム５０４Ｌ、ガイドチューブ５１２及び複数のツール５１４ａ、５１４ｂ、５１４ｃを成す各関節を回転駆動する駆動部５６０Ａに伝送する（６８０）。

このような過程を通じてヒューマノイドロボット５００の冗長自由度を活用して多様な個別目的（ツールの作業領域の拡張、ツールの関節別必要剛性の最小化、ツールと周辺障害物との間の衝突可能性の最小化、ツールの必要自由度の最小化及び複合的な作業実行）を達成しながらヒューマノイドロボット５００を成す各構成要素（マウンティングアーム、ガイドチューブ、複数のツール）の動作を統合的に制御することができるようになる。

図１６は、ヒューマノイドロボットの制御ブロック図である。図１６に示すヒューマノイドロボット５００Ｂは、図１４に示すヒューマノイドロボット５００Ａと比較してみるとき、制御部５４０Ｂの冗長自由度逆運動学解析部５４２Ｂ内に優先順位決定部５４３Ｂが追加された点で、図１４に示すヒューマノイドロボット５００Ａと異なる。

ここでは、同一の名称と同一の参照符号を使用している構成要素については説明を省略し（、但し、参照符号において番号の後方に併記されたＡ、Ｂは、各実施例を区別するためのものである）、図１６に追加的に示す優先順位決定部５４３Ｂの構成及びこの優先順位決定部５４３Ｂによってその機能が変化する格納部５３０Ｂ及び制御部５４０Ｂ内の冗長自由度逆運動学解析部５４２Ｂの構成のみについて説明する。

図１６に示す格納部５３０Ｂは、ヒューマノイドロボット５００Ｂの冗長自由度を活用して関節空間での解、すなわちマウンティングアーム５０４Ｌ、ガイドチューブ５１２及び複数のツール５１４Ａ、５１４ｂ、５１４ｃを成す各関節の目標回転角度を算出するのに必要な情報及びアルゴリズムなどが格納されるメモリであって、格納部５３０Ｂ内には、作業変数ｐ、すなわち複数のツール５１４Ａ、５１４ｂ、５１４ｃの位置／姿勢情報に設定された優先順位（例えば、複数のツール５１４Ａ、５１４ｂ、５１４ｃの位置及び姿勢情報のうち第１ツール５１４ａの位置及び姿勢情報にさらに高い優先順位が設定されるか、または第１ツール５１４ａの末端部の位置情報ｘ_１、ｙ_１、ｚ_１と姿勢情報α_１、β_１、γ_１のうち位置情報ｘ_１、ｙ_１、ｚ_１にさらに高い優先順位が設定される）、ヤコビアン行列算出時に必要とするアルゴリズム及びヒューマノイドロボット５００Ｂの運動学的構造に関する情報、目的関数算出時に必要とする複数の個別目的それぞれを達成するための複数の個別目的関数、行う作業がどんなものであるかによって各個別目的関数に乗算される加重値などが格納される。

また、格納部５３０Ｂは、ヒューマノイドロボット５００Ｂの位置認識結果及びＳＬＡＭアルゴリズムを利用して作成される移動空間に対する地図などを格納することができる。

図１６に示されたように、制御部５４０Ｂ内の冗長自由度逆運動学解析部５４２Ｂは、ヒューマノイドロボット５００Ｂの冗長自由度を活用してヒューマノイドロボット５００Ｂを成す各構成要素（マウンティングアーム、ガイドチューブ、複数のツール）の動作を統合的に制御するための制御信号（各構成要素を成す複数の関節それぞれの目標回転角度）を生成する構成部であって、冗長自由度逆運動学解析部５４２Ｂは、さらに、優先順位決定部５４３Ｂ、ヤコビアン行列算出部５４４Ｂ、目的関数算出部５４５Ｂ及び冗長自由度活用部５４６Ｂを含んでなる。

冗長自由度逆運動学解析部５４２Ｂを成すそれぞれの構成部に対する詳細な説明に先立って、以下では、まず、冗長自由度を有するヒューマノイドロボット５００Ｂで作業変数ｐに優先順位が設定されたときの逆運動学解析について説明する。

システムが冗長自由度を有する場合には、ヤコビアン行列Ｊ（ｑ）の疑似逆行列(Ｐｓｅｕｄｏ−ｉｎｖｅｒｓｅ Ｍａｔｒｉｘ)Ｊ^＃（ｑ）を利用して逆運動学的解を算出するという点は先立って言及した事がある。

この際、作業変数ｐに優先順位を設定する場合、例えば作業変数ｐを優先順位によってｐ_１、ｐ_２に分け、ｐ_１にさらに高い優先順位を設定するとき、ヤコビアン行列の疑似逆行列を利用して関節空間での各関節の速度

を算出する式は、下記の数式（７）のように示すことができ、数式（７）に記載した

は、下記の数式（８）のように定義することができる。

ここで、ｐ_１とｐ_２は、作業変数ｐを優先順位によって分類したサブベクトル（ｓｕｂｖｅｃｔｏｒ）であり、Ｊ_１とＪ_２は、それぞれｐ_１とｐ_２に対応するヤコビアン行列であり、Ｉｎは、ｎ×ｎ単位行列（ｕｎｉｔ ｍａｔｒｉｘ、ｉｄｅｎｔｉｔｙ ｍａｔｒｉｘ）であり、ｒは、任意のベクトルである。

作業変数ｐに優先順位を付与する方式について例を取って説明すれば、複数のツール５１４Ａ、５１４ｂ、５１４ｃの位置及び姿勢情報のうち第１ツール５１４ａの位置及び姿勢情報にさらに高い優先順位を設定するか（互いに異なるツールの位置／姿勢情報の間に優先順位が異なる場合）、第１ツール５１４ａの末端部の位置情報ｘ_１、ｙ_１、ｚ_１と姿勢情報α_１、β_１、γ_１のうち位置情報ｘ_１、ｙ_１、ｚ_１にさらに高い優先順位を設定することもできる(１つのツール内で位置情報と姿勢情報の優先順位が異なる場合)。

作業変数ｐを優先順位によって分類した場合にも、冗長自由度を活用するために、任意のベクトルｒ（ｑ）を定義し、個別目的関数ｗ_１〜ｗ_ｎ及びシステム全体の目的関数ｗを定義する内容は、数式（６）及び図５〜図９に関連した説明部分に記載した内容と同一なので、ここでは、詳細な説明を省略する。

さらに図１６に対する説明に戻って、冗長自由度逆運動学解析部５４２Ｂを成すそれぞれの構成部について詳細に説明する。

優先順位決定部５４３Ｂは、モーション命令生成部５４１Ｂを通じて生成されたヒューマノイドロボット５００Ｂの末端部（複数のツールの各末端部）の位置及び姿勢情報ｘ、ｙ、ｚ、α、β、γをあらかじめ設定された優先順位によって分類する。ここで、ヒューマノイドロボット５００Ｂの末端部の位置及び姿勢情報、すなわち作業変数ｐを優先順位によってｐ_１、ｐ_２に分類し、且つｐ_１に設定された優先順位がｐ_２に設定された優先順位より高いと前提する。例えば、複数のツール５１４Ａ、５１４ｂ、５１４ｃの位置及び姿勢情報のうち第１ツール５１４ａの位置及び姿勢情報にさらに高い優先順位が設定されている場合（互いに異なるツールの位置／姿勢情報の間に優先順位が異なる）場合に、優先順位決定部５４３Ｂは、第１ツール５１４ａの位置及び姿勢情報をｐ_１に決定し、第２及び第３ツール５１４ｂ、５１４ｃの姿勢情報をｐ_２に決定する。また、例えば、第１ツール５１４ａの末端部の位置情報ｘ_１、ｙ_１、ｚ_１と姿勢情報α_１、β_１、γ_１のうち位置情報ｘ_１、ｙ_１、ｚ_１にさらに高い優先順位が設定されている場合（１つのツール内で位置と姿勢の優先順位が異なる場合）、優先順位決定部５４３Ｂは、第１ツール５１４ａの末端部の位置情報ｘ_１、ｙ_１、ｚ_１をｐ_１に決定し、第１ツール５１４ａの末端部の姿勢情報α_１、β_１、γ_１をｐ_２に決定する。

ヤコビアン行列算出部５４４Ｂは、ヤコビアン行列を算出するためのアルゴリズムにヒューマノイドロボット５００Ｂの運動学的構造に関する情報（例えば、関節と関節の間を連結するリンクの長さ情報など）を入力し、作業変数ｐのうち優先順位が高いｐ_１に対応するヤコビアン行列Ｊ_１(ｑ)と作業変数のうち相対的に優先順位が低いｐ_２に対応するヤコビアン行列Ｊ_２(ｑ)を算出する。

目的関数算出部５４５Ｂは、冗長自由度を有するヒューマノイドロボット５００Ｂで作業変数ｐのうち優先順位が高いｐ_１に対応するヤコビアン行列Ｊ_１(ｑ)の疑似逆行列Ｊ_１ ^＃(ｑ)及び作業変数ｐのうち優先順位が低いｐ_２に対応するヤコビアン行列Ｊ_２(ｑ)の疑似逆行列Ｊ_２ ^＃(ｑ)を利用して関節空間での各関節の速度

を算出する数式（７）、すなわち

に記載した任意のベクトルｒ（ｑ）を表現する数式（６）、すなわち

に記載した目的関数ｗを算出する。このような目的関数ｗ、システム全体の目的関数は、複数の個別目的関数ｗ_１〜ｗ_ｎの加重和（ｗｅｉｇｈｔｅｄ ｓｕｍ）で示すことができる（ｗ＝ａｗ_１＋ｂｗ_２＋ｃｗ_３＋…）。個別目的関数の例としては、それぞれのツール５１４ａ、５１４ｂ、５１４ｃと器具的関節限界との間の距離の逆数、複数のツール５１４Ａ、５１４ｂ、５１４ｃを成す各関節のトルク二乗の和、それぞれのツール５１４ａ、５１４ｂ、５１４ｃと周辺障害物との間の距離の逆数などを挙げることができる。この際、システム全体の目的関数ｗは、時間の流れによって変化する。すなわち、ヒューマノイドロボット５００Ｂが行う作業がどんなものであるかによって各個別目的関数に乗算される加重値ａ、ｂ、ｃ、…が変わる。目的関数算出部５４５Ｂは、ヒューマノイドロボット５００Ｂが行う作業がどんなものであるかを判断し、当該判断結果に基づいて各個別目的関数に乗算される加重値ａ、ｂ、ｃ、…を決定した後、決定された各加重値を各個別目的関数に乗算し、システム全体の目的関数ｗを算出する。

冗長自由度活用部５４６Ｂは、システム全体の目的関数ｗを最小化する関節空間での解、すなわちマウンティングアーム５０４Ｌ、ガイドチューブ５１２及び複数のツール５１４Ａ、５１４ｂ、５１４ｃを成す各関節の目標回転角度ｑを算出する。冗長自由度活用部５４６Ｂは、目的関数算出部５４５Ｂで算出された目的関数ｗを前述した数式（６）に代入し、目的関数ｗが代入された数式（６）を前述した数式（７）に代入した後、目的関数ｗを最小化する関節空間での各関節の速度

を算出する。冗長自由度活用部５４６Ｂは、算出された関節空間での各関節の速度

を積分し、最終的な関節空間での解であるマウンティングアーム５０４Ｌ、ガイドチューブ５１２及び複数のツール５１４Ａ、５１４ｂ、５１４ｃを成す各関節の目標回転角度ｑを算出するようになる。

図１７は、ヒューマノイドロボットの制御方法を示す流れ図である。

本実施例の動作説明のための初期条件として、ヒューマノイドロボット５００Ｂ、特に図１３に示す左側の腕部分は、冗長自由度を有し、マウンティングアーム５０４Ｌ、ガイドチューブ５１２、３つのツール５１４ａ、５１４ｂ、５１４ｃそれぞれは、複数のリンク及び複数の関節を含んでなるという点を前提する。またマウンティングアーム５０４Ｌとガイドチューブ５１２が互いに連動して動作し、ガイドチューブ５１２とそれぞれのツール５１４ａ、５１４ｂ、５１４ｃが互いに連動して動作するものと前提する。また、格納部５３０Ｂには、作業変数ｐ、すなわち複数のツール５１４Ａ、５１４ｂ、５１４ｃの位置／姿勢情報に設定された優先順位、ヤコビアン行列算出時に必要とするアルゴリズム及びヒューマノイドロボット５００Ｂの運動学的構造に関する情報、目的関数算出時に必要とする複数の個別目的それぞれを達成するための複数の個別目的関数、行う作業がどんなものであるかによって各個別目的関数に乗算される加重値などがあらかじめ格納されていることを前提する。

入力部５２０Ｂを通じてユーザからヒューマノイドロボット５００Ｂの作業命令（例えば、掃除命令）が入力されれば、ヒューマノイドロボット５００Ｂの作業が始まる。

まず、ヒューマノイドロボット５００Ｂの作業が始まれば、制御部５４０Ｂは、位置／姿勢検出部５７０Ａから複数のツール５１４ａ、５１４ｂ、５１４ｃの各末端部の３次元空間上での位置及び姿勢情報を周期的に入力されながら作業を行う（７１０）。

次に、制御部５４０Ｂ内のモーション命令生成部５４１Ｂは、入力部５２０Ｂから伝送されたユーザ命令信号、映像情報獲得部５２５Ｂから伝送された移動空間周辺の映像信号及び位置／姿勢検出部５７０Ｂから伝送された複数のツール５１４ａ、５１４ｂ、５１４ｃの各末端部の３次元空間上での位置及び姿勢情報に基づいて複数のツール５１４ａ、５１４ｂ、５１４ｃに対する作業空間でのモーション命令ｘ、ｙ、ｚ、α、β、γを生成する(７２０)。この際、モーション命令生成部５４１Ｂを通じて生成された作業空間でのモーション命令ｘ、ｙ、ｚ、α、β、γが作業変数ｐになる。

その後、制御部５４０Ｂの冗長自由度逆運動学解析部５４２Ｂ内の優先順位決定部５４３Ｂは、モーション命令生成部５４１Ｂを通じて生成されたヒューマノイドロボット５００Ｂの末端部複数のツールの各末端部の位置及び姿勢情報ｘ、ｙ、ｚ、α、β、γ、すなわち作業変数ｐをあらかじめ設定された優先順位によって分類する（７３０）。作業変数ｐを優先順位によってｐ_１、ｐ_２に分類し、且つｐ_１に設定された優先順位がｐ_２に設定された優先順位より高いと前提するとき、例えば、複数のツール５１４ａ、５１４ｂ、５１４ｃの位置及び姿勢情報のうち第１ツール５１４ａの位置及び姿勢情報にさらに高い優先順位が設定されている場合に、優先順位決定部５４３Ｂは、第１ツール２１４ａの位置及び姿勢情報をｐ_１に決定し、第２及び第３ツール２１４ｂ、２１４ｃの姿勢情報をｐ_２に決定する。

次に、制御部５４０Ｂの冗長自由度逆運動学解析部５４２Ｂ内のヤコビアン行列算出部５４４Ｂは、格納部５３０Ｂに既格納されているヤコビアン行列を算出するためのアルゴリズム及びヒューマノイドロボット５００Ｂの運動学的構造に関する情報（例えば、関節と関節の間を連結するリンクの長さ情報など）を利用して作業変数ｐのうち優先順位が高いｐ_１に対応するヤコビアン行列Ｊ_１(ｑ)と作業変数のうち相対的に優先順位が低いｐ_２に対応するヤコビアン行列Ｊ_２(ｑ)を算出する(７４０)。

その後、制御部５４０Ｂの冗長自由度逆運動学解析部５４２Ｂ内の目的関数算出部５４５Ｂは、ヒューマノイドロボット５００Ｂが行う作業がどんなものであるかを判断し、当該判断結果に基づいて各個別目的関数に乗算される加重値ａ、ｂ、ｃ、…を決定した後、決定された各加重値を各個別目的関数に乗算し、システム全体の目的関数ｗを算出する（７５０）。

次に、制御部５４０Ｂの冗長自由度逆運動学解析部５４２Ｂ内の冗長自由度活用部５４６Ｂは、ヒューマノイドロボット５００Ｂの冗長自由度を活用してマウンティングアーム５０４Ｌ、ガイドチューブ５１２及び複数のツール５１４Ａ、５１４ｂ、５１４ｃを成す各関節の目標回転角度ｑを算出する（７６０）。この際、冗長自由度活用部５４６Ｂは、目的関数算出部５４５Ｂを通じて算出されたシステム全体の目的関数ｗを最小化する関節空間での各関節の速度

を算出し、算出された関節空間での各関節の速度

を積分し、最終的な関節空間での解であるマウンティングアーム５０４Ｌ、ガイドチューブ５１２及び複数のツール５１４Ａ、５１４ｂ、５１４ｃを成す各関節の目標回転角度ｑを算出するようになる。

その後、制御部５４０Ｂは、冗長自由度活用部５４６Ｂで算出した各関節の目標回転角度ｑをサーボ制御部５５０Ｂに伝送する（７７０）。

次に、サーボ制御部５５０Ｂは、制御部５４０Ｂ内の冗長自由度活用部５４６Ｂから伝達された各関節の目標回転角度ｑを追従するための関節トルクτを算出し、算出された関節トルクτに対応するトルク制御信号を生成する（７８０）。

その後、サーボ制御部５５０Ｂは、生成されたトルク制御信号をマウンティングアーム５０４Ｌ、ガイドチューブ５１２及び複数のツール５１４ａ、５１４ｂ、５１４ｃを成す各関節を回転駆動する駆動部５６０Ｂに伝送する（７９０）。

このような過程を通じてヒューマノイドロボット５００Ｂの冗長自由度を活用して多様な個別目的（ツールの作業領域の拡張、ツールの関節別必要剛性の最小化、ツールと周辺障害物との間の衝突可能性の最小化、ツールの必要自由度の最小化及び複合的な作業実行）を達成しながらヒューマノイドロボット５００Ｂを成す各構成要素（マウンティングアーム、ガイドチューブ、複数のツール）の動作を統合的に制御することができるようになる。

前述した実施例では、複数のツールを有するロボットとして手術ロボット及びヒューマノイドロボットを例に取って説明したが、その他、ガイドチューブから複数のツールが分岐して出る形態の多様なロボットシステムに本発明が適用されることができる。

Claims (18)

1. ガイドチューブと、前記ガイドチューブと連動して動作して前記ガイドチューブから分岐する複数のツールとを有するマルチツールモジュールであって、冗長自由度を有するマルチツールモジュールと；
   前記複数のツールの末端部に対する作業空間でのモーション命令情報に基づいて前記マルチツールモジュールに対する関節空間でのモーションに関する制御信号を生成し、且つ前記冗長自由度を活用して前記制御信号を生成する制御部と；を含み、
   前記冗長自由度は、前記関節空間での前記マルチツールモジュールの自由度の数が前記作業空間での自由度の数より大きい場合を表す、ロボット。
2. 前記ガイドチューブ及び前記複数のツールそれぞれは、複数のリンク及び複数の関節を含み、前記複数のツールの各末端部には、エンドエフェクタが装着される、請求項１に記載のロボット。
3. 前記制御部は、前記複数のツールの末端部に対する作業空間でのモーション命令情報に対応するヤコビアン行列を算出し、前記複数のツールの末端部に対する作業空間でのモーション命令情報及び前記算出されたヤコビアン行列に基づいて前記冗長自由度を活用して前記マルチツールモジュールに対する関節空間でのモーションに関する前記制御信号を生成する、請求項２に記載のロボット。
4. 前記制御部は、前記複数のツールの末端部に対する作業空間でのモーション命令情報に優先順位が設定されているとき、前記優先順位が高い前記作業空間でのモーション命令情報及び前記優先順位が低い前記作業空間でのモーション命令情報に対応するヤコビアン行列を算出し、前記優先順位が設定された前記作業空間でのモーション命令情報及び前記算出されたヤコビアン行列に基づいて前記冗長自由度を活用して前記マルチツールモジュールに対する関節空間でのモーションに関する前記制御信号を生成する、請求項２に記載のロボット。
5. 前記制御部は、前記制御信号の生成過程で前記ロボットの目的関数を算出し、且つ前記目的関数は、複数の個別目的関数の加重和で表現される、請求項３に記載のロボット。
6. 前記制御部は、前記制御信号の生成過程で前記ロボットの目的関数を算出し、且つ前記目的関数は、複数の個別目的関数の加重和で表現される、請求項４に記載のロボット。
7. 前記個別目的関数は、それぞれの前記ツールと器具的限界との間の距離の逆数を含む、請求項５又は６に記載のロボット。
8. 前記個別目的関数は、それぞれのツールと特異姿勢との間の距離の逆数を含む、請求項５又は６に記載のロボット。
9. 前記個別目的関数は、各前記関節のトルク二乗の和を含む、請求項５又は６に記載のロボット。
10. 前記個別目的関数は、それぞれのツールと周辺障害物との間の距離の逆数 を含む、請求項５又は６に記載のロボット。
11. 前記制御部は、前記算出された前記ロボットの目的関数を最適化又は次善化する(optimize or suboptimize)前記関節空間でのモーションに関する前記制御信号を生成する、請求項５乃至１０のいずれか一項に記載のロボット。
12. 前記ヤコビアン行列算出時に必要とするアルゴリズム及び前記ロボットの運動学的構造に関する情報、前記目的関数算出時に必要とする複数の個別目的それぞれを達成するための複数の個別目的関数、前記ロボットが行う作業に応じて各個別目的関数に乗算される加重値を格納する格納部をさらに含む、請求項５乃至１１のいずれか一項に記載のロボット。
13. ガイドチューブと、前記ガイドチューブと連動して動作し、前記ガイドチューブから分岐する複数のツールとを有するマルチツールモジュールを含むロボットの制御方法において、
    前記マルチツールモジュールは、冗長自由度を有し、
    前記複数のツールの末端部に対する作業空間でのモーション命令情報を生成し；
    前記生成された作業空間でのモーション命令情報に基づいて前記マルチツールモジュールに対する関節空間でのモーションに関する制御信号を生成し、且つ前記冗長自由度を活用して前記制御信号を生成し、
    前記冗長自由度は、前記関節空間での前記マルチツールモジュールの自由度の数が前記作業空間での自由度の数より大きい場合を表す、ロボットの制御方法。
14. 前記ガイドチューブ及び前記複数のツールそれぞれは、複数のリンク及び複数の関節を含み、前記複数のツールの各末端部には、エンドエフェクタが装着される、請求項１３に記載のロボットの制御方法。
15. 前記冗長自由度を活用して前記マルチツールモジュールに対する関節空間でのモーションに関する前記制御信号を生成することは、前記複数のツールの末端部に対する作業空間でのモーション命令情報に対応するヤコビアン行列を算出し、前記複数のツールの末端部に対する作業空間でのモーション命令情報及び前記算出されたヤコビアン行列に基づいて前記制御信号を生成することである、請求項１４に記載のロボットの制御方法。
16. 前記冗長自由度を活用して前記マルチツールモジュールに対する関節空間でのモーションに関する前記制御信号を生成することは、前記複数のツールの末端部に対する作業空間でのモーション命令情報に優先順位が設定されているとき、前記優先順位が高い前記作業空間でのモーション命令情報及び前記優先順位が低い前記作業空間でのモーション命令情報に対応するヤコビアン行列を算出し、前記優先順位が設定された前記作業空間でのモーション命令情報及び前記算出されたヤコビアン行列に基づいて前記冗長自由度を活用して前記マルチツールモジュールに対する関節空間でのモーションに関する前記制御信号を生成する、請求項１４に記載のロボットの制御方法。
17. 前記冗長自由度を活用して前記マルチツールモジュールに対する関節空間でのモーションに関する前記制御信号を生成することは、前記ロボットの目的関数を算出し、前記算出された前記ロボットの目的関数を最適化又は次善化する(optimize or suboptimize)前記関節空間でのモーションに関する前記制御信号を生成することである、請求項１５又は１６に記載のロボットの制御方法。
18. 前記目的関数は、複数の個別目的関数の加重和で表現される、請求項１７に記載のロボットの制御方法。

Images