ロボットの制御装置及び制御方法、並びにコンピューター・プログラム
【課題】非駆動関節を含むロボットの加速度オーダーの精密な制御を実現する。
【解決手段】複数の剛体が連なったリンク構造のロボットにおいて、関節の全部を可動とした力学モデル（主モデル）を用いて既知の力から未知の加速度を求める順動力学演算部と、駆動関節を不動とし非駆動関節を可動とした力学モデル（補助モデル）を用いて既知の加速度及び力から未知の力及び加速度を求めるハイブリッド動力学演算部を有し，順動力学演算部とハイブリッド動力学演算部の演算結果を用いて、ロボットの任意部位に所望の加速度を発生させるための関節力を決定する。
【解決手段】複数の剛体が連なったリンク構造のロボットにおいて、関節の全部を可動とした力学モデル（主モデル）を用いて既知の力から未知の加速度を求める順動力学演算部と、駆動関節を不動とし非駆動関節を可動とした力学モデル（補助モデル）を用いて既知の加速度及び力から未知の力及び加速度を求めるハイブリッド動力学演算部を有し，順動力学演算部とハイブリッド動力学演算部の演算結果を用いて、ロボットの任意部位に所望の加速度を発生させるための関節力を決定する。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は、リンク構造物からなるロボットの制御装置及び制御方法、並びにコンピューター・プログラムに係り、特に、関節の一部が力を発することのない非駆動関節であるロボットの制御装置及び制御方法、並びにコンピューター・プログラムに関する。
【背景技術】
【０００２】
倒立振子型のロボットや２足歩行型のロボットは、接地面積が小さいため、人と共存する際に邪魔になり難く、人にさまざまなサービスを提供することができる、という優れた側面を持っている。逆に、この種のロボットは、接地面積が小さいゆえに、タスク遂行のために環境から得られる外力に関する制約が厳しいという問題がある。倒立振子ロボットでは、支持点回りのモーメントを得ることができないし、２足歩行型のロボットでは、足底形状によって路面から得られるモーメントに不等式制約が課される。より極端な例として、宇宙ロボットが挙げられる。宇宙ロボットは、環境からの反力を一切得ることができない。これらのロボットは、関節の一部が力を発することのできない、非駆動関節としてモデル化できることを特徴としている。
【０００３】
図１１〜図１３には、非駆動関節を有するロボットの関節構成を例示している。図１１は、倒立振子型のロボットの関節構成例を示している。車輪の並進移動成分は、直動関節で表現され、車輪によるロボット全体の姿勢変化は、次に連結される回転関節で表される。ロボット全体は、この点回りに自由に回転し得ることから、この関節が非駆動関節として表現される。残りの関節は腕部の関節であり、いずれも駆動関節である。また、図１２は、２足歩行型のロボットの関節構成例を示している。脚部の回転関節はいずれも駆動関節であり、その他、ロボット全体が空間内を自由に移動できることを表現するための、３自由度並進と３自由度回転からなる６自由度を有する。この６自由度は仮想的若しくは論理的な関節であり、実在しないため、力を発することができない。ゆえに、非駆動関節として表現されている。２足歩行型ロボットは、６自由度の非駆動関節を有する系としてモデル化されるが、足先から得られる力及びモーメント成分を利用して、全身の制御をすることができる。また、図１３は、宇宙ロボットの関節構成例を示している。宇宙ロボットに搭載されるロボット・アームの関節は駆動関節として表現されるが、ロボット全体の３自由度並進と３自由度回転自由度については、６自由度の非駆動関節として表現される。宇宙ロボットは、２足歩行ロボットと異なり、外力を得ることができない。
【０００４】
ロボットなどのリンク構造物において、例えば手先の位置に所望の速度を発生させるためには関節速度を得る必要がある。あるいは、所望の加速度を発生させるためには関節力を得る必要がある。駆動関節のみから構成される系では、関節速度のすべての成分、あるいは、関節加速度のすべての成分を制御することができる。ところが、非駆動関節が含まれる系では、非駆動関節の成分を制御することはできない、という問題がある。
【０００５】
力制御系のロボット制御において重要な概念として、ロボットに作用する力と発生する加速度の関係を記述するための空間、すなわち操作空間（Ｏｐｅｒａｔｉｏｎａｌ Ｓｐａｃｅ）が挙げられる。例えば、ロボットの手先の位置を操作空間として定義して、手先に所望の加速度を発生させるための関節力を決定するのに操作空間が用いられる。操作空間の加速度を精密に制御するための方法が幾つか提案されているが（例えば、特許文献１、２を参照のこと）、いずれの方法もすべての関節が駆動関節である場合に対象が限定されている。
【先行技術文献】
【特許文献】
【０００６】
【特許文献１】特開２００９−９５９５９号公報
【特許文献２】特開２０１０−１８８４７１号公報
【発明の概要】
【発明が解決しようとする課題】
【０００７】
本発明の目的は、非駆動関節を含むロボットの加速度オーダーの精密な制御を実現することができる、優れたロボットの制御装置及び制御方法、並びにコンピューター・プログラムを提供することにある。
【課題を解決するための手段】
【０００８】
本願は、上記課題を参酌してなされたものであり、請求項１に記載の発明は、
駆動関節及び非駆動関節を有するロボットの前記駆動関節を不動とした補助モデルを用いて、逆動力学と順動力学からなるハイブリッド動力学演算を実行して、関節加速度が既知である不動関節に作用する関節力と、関節力が既知である可動関節に発生する関節加速度を算出するハイブリッド動力学演算部と、
前記ロボットのすべての関節を可動とした主モデルを用いて、前記ロボットに作用する既知の力によって発生する加速度を算出する順動力学演算部と、
前記ハイブリッド動力学演算部及び前記順動力学演算部による演算結果に基づいて、前記ロボットの任意の部位に所望の目標加速度を発生させるための関節力を決定する関節力決定部と、
前記決定した関節力を制御目標値として、前記ロボットの各関節の関節力を制御する関節力制御部と、
を具備するロボットの制御装置である。
【０００９】
本願の請求項２に記載の発明によれば、請求項１に記載のロボットの制御装置のハイブリッド動力学演算部によって得られた前記ロボットに作用する力を、一般化ヤコビアンを用いて関節力に変換するように構成されている。
【００１０】
本願の請求項３に記載の発明によれば、請求項１に記載のロボットの制御装置の関節力決定部は、
関節値及び外力以外をすべてゼロとし、ｉ番目の操作空間にのみ第ｉ成分が１の単位ベクトルｅiからなる外力を作用させる条件下で、前記順動力学演算部に前記順動力学演算をすべての操作空間にわたり繰り返し実行させることによって、一般化操作空間慣性逆行列を求める一般化操作空間慣性逆行列演算部と、
関節空間に発生する速度と重力のみが作用するという制約条件下で、前記順動力学演算部に前記順動力学演算を１回だけ実行させて、一般化操作空間バイアス加速度を求める一般化操作空間バイアス加速度演算部と、
前記一般化操作空間慣性逆行列及び前記一般化操作空間バイアス加速度を用いて表される操作空間に作用する力と加速度の関係式において、前記操作空間に発生する目標加速度を既知とし、それを達成するために前記操作空間に発する仮想的な外力ｆを解く仮想外力算出部と、
前記仮想外力算出部が求めた外力ｆを、一般化逆力運動学演算を用いて駆動関節の駆動関節力に変換する関節算出部と、
を備えている。
【００１１】
本願の請求項４に記載の発明によれば、請求項３に記載のロボットの制御装置の仮想外力算出部は、前記関係式と前記ロボットと環境との間で形成される拘束条件からなる線形相補性問題を満足する前記仮想的な外力を、前記一般化操作空間慣性逆行列及び前記一般化操作空間バイアス加速度演算部を用いて求め、得られた力に基づいてリンクの関節力を決定するように構成されている。
【００１２】
本願の請求項５に記載の発明によれば、請求項１に記載のロボットの制御装置のハイブリッド動力学演算部に、関節空間及び外力以外をすべてゼロとし、ｉ番目の操作空間にのみ第ｉ成分が１の単位ベクトルｅiからなる外力を作用させる条件下で、駆動関節空間の関節力を算出するハイブリッド動力学演算を繰り返し実行させることによって、一般化ヤコビアンを求めるように構成されている。
【００１３】
また、本願の請求項６に記載の発明は、
駆動関節及び非駆動関節を有するロボットの前記駆動関節を不動とした補助モデルを用いて、逆動力学と順動力学からなるハイブリッド動力学演算を実行して、関節加速度が既知である不動関節に作用する関節力と、関節力が既知である可動関節に発生する関節加速度を算出するハイブリッド動力学演算ステップと、
前記ロボットのすべての関節を可動とした主モデルを用いて、前記ロボットに作用する既知の力によって発生する加速度を算出する順動力学演算ステップと、
前記ハイブリッド動力学演算ステップ及び前記順動力学演算ステップによる演算結果に基づいて、前記ロボットの任意の部位に所望の目標加速度を発生させるための関節力を決定する関節力決定ステップと、
前記決定した関節力を制御目標値として、前記ロボットの各関節の関節力を制御する関節力制御ステップと、
を有するロボットの制御方法である。
【００１４】
また、本願の請求項７に記載の発明は、駆動関節及び非駆動関節を有するロボットを制御するための処理をコンピューター上で実行するようにコンピューター可読形式で記述されたコンピューター・プログラムであって、前記コンピューターを、
前記ロボットの前記駆動関節を不動とした補助モデルを用いて、逆動力学と順動力学からなるハイブリッド動力学演算を実行して、関節加速度が既知である不動関節に作用する関節力と、関節力が既知である可動関節に発生する関節加速度を算出するハイブリッド動力学演算部、
前記ロボットのすべての関節を可動とした主モデルを用いて、前記ロボットに作用する既知の力によって発生する加速度を算出する順動力学演算部、
前記ハイブリッド動力学演算部及び前記順動力学演算部による演算結果に基づいて、前記ロボットの任意の部位に所望の目標加速度を発生させるための関節力を決定する関節力決定部、
前記決定した関節力を制御目標値として、前記ロボットの各関節の関節力を制御する関節力制御部、
として機能させるためのコンピューター・プログラムである。
【００１５】
本願の請求項７に係るコンピューター・プログラムは、コンピューター上で所定の処理を実現するようにコンピューター可読形式で記述されたコンピューター・プログラムを定義したものである。換言すれば、本願の請求項７に係るコンピューター・プログラムをコンピューターにインストールすることによって、コンピューター上では協働的作用が発揮され、本願の請求項１に係るロボットの制御装置と同様の作用効果を得ることができる。
【発明の効果】
【００１６】
本発明によれば、非駆動関節を含むロボットの加速度オーダーの精密な制御を、関節数Ｎに対してＯ（Ｎ）という少ない計算量で達成することができる、優れたロボットの制御装置及び制御方法、並びにコンピューター・プログラムを提供することができる。
【００１７】
本発明のさらに他の目的、特徴や利点は、後述する本発明の実施形態や添付する図面に基づくより詳細な説明によって明らかになるであろう。
【図面の簡単な説明】
【００１８】
【図１】図１は、非駆動関節を有したロボットを、全軸が可動なロボットのモデルと、駆動関節を不動とし、非駆動関節を可動とした補助モデルという２つのモデルで表現した図である。
【図２】図２は、主モデル及び補助モデルを用いた、非駆動関節を有するロボットの制御システム構成例を模式的に示した図である。
【図３】図３は、図２に示したロボットの制御システムにおいて実行される処理手順を示したフローチャートである。
【図４】図４は、一般化操作空間慣性逆行列演算部１０６により一般化操作空間慣性逆行列ΛG-1を算出するための処理手順を示したフローチャートである。
【図５】図５は、一般化操作空間慣性逆行列演算部１０６により一般化操作空間バイアス加速度ｃGを算出するための処理手順を示したフローチャートである。
【図６Ａ】図６Ａは、図１〜図３に示した制御系を適用する倒立振子型ロボットの構成例を示した図である。
【図６Ｂ】図６Ｂは、図１〜図３に示した制御系を適用する倒立振子型ロボットの構成例を示した図である。
【図６Ｃ】図６Ｃは、図６Ｂに示した倒立振子型ロボットのすべての関節を可動とした主モデルを示した図である。
【図６Ｄ】図６Ｆは、図６Ｂに示した倒立振子型ロボットの駆動関節を不動とするとともに非駆動関節を可動とした補助モデルを示した図である。
【図７】図７は、図６に示したロボットのバランスを保ちながら、機体の所定部位の位置や姿勢（方位）を一定に保つための制御系の構成例を示した図である。
【図８Ａ】図８Ａは、ワインが注がれたグラスをグリッパーで把持したロボットが、外力が印加されても、グラスのワインをこぼさずに保持している様子を示した図である。
【図８Ｂ】図８Ｂは、ワインが注がれたグラスをグリッパーで把持したロボットが、外力が印加されても、グラスのワインをこぼさずに保持している様子を示した図である。
【図８Ｃ】図８Ｃは、ワインが注がれたグラスをグリッパーで把持したロボットが、外力が印加されても、グラスのワインをこぼさずに保持している様子を示した図である。
【図９】図９は、ロボットが後方に押された際の車輪の前後位置と左手先位置をそれぞれ示した図である。
【図１０】図１０は、ロボットが後方に押された際のロボットの重心の位相平面上での変化（位置及び速度の変化）を示した図である。
【図１１】図１１は、倒立振子型のロボットの関節構成を示した図である。
【図１２】図１２は、２足歩行型のロボットの関節構成例を示した図である。
【図１３】図１３は、宇宙ロボットの関節構成例を示した図である。
【図１４】図１４は、床面に設置された多リンク構成の装置において、末端から基底の順序で情報を算出する通常の経路を示した図である。
【図１５】図１５は、床面に設置された多リンク構成の装置において、基底から末端に向かって速度情報を算出する通常の経路を示した図である。
【発明を実施するための形態】
【００１９】
以下、図面を参照しながら本発明の実施形態について詳細に説明する。
【００２０】
ロボットは、一般に、複数の剛体が連なって構成されるリンク構造物である。このうち倒立振子型のロボットや２足歩行型のロボット、宇宙ロボットなどは、関節の一部が力を発することのできない、非駆動関節としてモデル化することができる（前述並びに図１１〜図１３を参照のこと）。ロボットなどのリンク構造物において、例えば手先の位置に所望の速度を発生させるためには関節速度を得る必要がある。あるいは、所望の加速度を発生させるためには関節力を得る必要がある。駆動関節のみから構成される系では、関節速度のすべての成分、あるいは、関節加速度のすべての成分を直接に制御することができる。ところが、非駆動関節が含まれる系では、非駆動関節の成分を直接には制御できない、という問題がある。
【００２１】
ここで、ロボットの運動方程式は、一般に、下式（１）のように表現される。
【００２２】
【数１】
【００２３】
上式（１）において、ｑは関節空間、τは関節空間ｑに発生する力、ｂは重力又はコリオリ力、Ｈはロボット（リンク構造全体）の関節空間に対する慣性行列、ｆは外力、Ｊは外力ｆの作用する空間を表すヤコビアンである。以下では、ロボットの第ｉリンクとその親リンクを接合している関節を第ｉ関節とする。
【００２４】
上記の運動方程式（１）を、駆動関節成分と非駆動関節成分に分けて表現すると、下式（２）に示す通りとなる。
【００２５】
【数２】
【００２６】
上式（２）において、インデックスＡは駆動関節成分、インデックスＵは非駆動関節成分をそれぞれ表す。非駆動関節を有するロボットとは、式（２）において、非駆動関節ｑUの関節力が常にゼロ、すなわちτU＝０となるロボットを意味する。
【００２７】
非駆動関節を有するロボットを制御する場合の重要な概念として、一般化ヤコビアンがある（例えば、Ｙ．Ｕｍｅｔａｍｉ ａｎｄ Ｋ．Ｙｏｓｈｉｄａ，“Ｒｅｓｏｌｖｅｄ ｍｏｔｉｏｎ ｒａｔｅ ｃｏｎｔｒｏｌ ｏｆ ｓｐａｃｅ ｍａｎｉｐｕｌａｔｏｒｓ ｗｉｔｈ ｇｅｎｅｒａｌｉｚｅｄ Ｊａｃｏｂｉａｎｍａｔｒｉｘ”（Ｉｎ ＩＥＥＥ Ｔｒａｎｓａｃｔｉｏｎｓ ｏｎ Ｒｏｂｏｔｉｃｓ ａｎｄ Ａｕｔｏｍａｔｉｏｎ，５（３），Ｊｕｎｅ １９８９）を参照のこと）。一般化ヤコビアンＪGは、下式（３）で与えられる。
【００２８】
【数３】
【００２９】
力制御系のロボット制御において重要な概念として、ロボットに作用する力と発生する加速度の関係を記述する操作空間がある（前述）。例えば、ロボットの手先の位置を操作空間として定義して、手先に所望の加速度を発生させるための関節力を決定するのに操作空間が用いられる。通常、操作空間ｘの速度は、関節ｑの関節速度とヤコビアンＪを用いて、下式（４）のように表わされる。
【００３０】
【数４】
【００３１】
ゆえに、操作空間ｘに所望の速度を発生するための関節速度を、下式（５）から得ることができる。但し、同式において、Ｊ#は、ヤコビアンＪの擬似逆行列である。
【００３２】
【数５】
【００３３】
上式（５）に示す関係をＮｅｗｔｏｎ−Ｒａｐｈｓｏｎ法などの反復法とともに用いて、いわゆる逆運動学（Ｉｎｖｅｒｓｅ Ｋｉｎｅｍａｔｉｃｓ）演算も構成することができる。駆動関節のみから構成される系では、関節空間ｑのすべての速度成分を制御することができるので、上記の関係式（５）が有効である。これに対し、非駆動関節が含まれる系では、関節空間ｑの速度成分のうち非駆動関節成分は制御できないため、関係式（５）を適用することができない。しかしながら、系の全運動量がゼロに保存されているという前提では、以下の関係式（６）が成立する。
【００３４】
【数６】
【００３５】
上式（６）を用いれば、運動量がゼロに保たれている宇宙ロボットなどの逆運動学を構成することができる。しかし、この関係式（６）は、一般化ヤコビアンＪGの定義式（３）をそのまま用いると、慣性行列Ｈを求める必要がある。関節数がＮの系において慣性逆行列ＨのサイズはＮ×Ｎであり、慣性行列Ｈを求めるには計算量が膨大である。また、関係式（６）は、全運動量が保存されないケースでは成立しないうえ、速度レベルの制御手段しか提供しない。すなわち、この関係式（６）に基づいて、運動量が保存されないケースや、非駆動関節を有する系の加速度オーダーの精密な制御を実現するのは難しい。
【００３６】
また、非駆動関節を含むＦｌｏａｔｉｎｇＢａｓｅ の系の操作空間に発生する力を制御する方法も提案されている（例えば、Ｌｕｉｓ Ｓｅｎｔｉｓ ａｎｄ Ｏｕｓｓａｍａ Ｋｈａｔｉｂ，“Ｃｏｎｔｒｏｌ ｏｆ Ｆｒｅｅ−Ｆｌｏａｔｉｎｇ Ｈｕｍａｎｏｉｄ Ｒｏｂｏｔｓ Ｔｈｒｏｕｇｈ Ｔａｓｋ Ｐｒｉｏｒｉｔｉｚａｔｉｏｎ”（Ｐｒｏｃｅｅｄｉｎｇｓ ｏｆ ｔｈｅ ＩＥＥＥ Ｉｎｔｅｒｎａｔｉｏｎａｌ Ｃｏｎｆｅｒｅｎｃｅ ｉｎ Ｒｏｂｏｔｉｃｓ ａｎｄ Ａｕｔｏｍａｔｉｏｎ，Ｂａｒｃｅｌｏｎａ，Ｓｐａｉｎ，Ａｐｒｉｌ ２００５）を参照のこと）。しかしながら、上記の一般化ヤコビアンＪGやそのＮｕｌｌＳｐａｃｅ を求める必要があり、計算量が多い。
【００３７】
また、操作空間の加速度を精密に制御するための方法が幾つか提案されているが（前述、並びに、特許文献１、２を参照のこと）、いずれの方法もすべての関節が駆動関節である場合に対象が限定されている。
【００３８】
そこで、以下では、非駆動関節を含むロボットの加速度オーダーの精密な制御を、関節数Ｎに対してＯ（Ｎ）という少ない計算量で達成する方法について説明する。
【００３９】
内力による運動を考えるため、上記で示した運動方程式（２）から外力項を除いた系を考える。上式（２）の上段及び下段のそれぞれより、以下の２式（７−１）、（７−２）を得ることができる。
【００４０】
【数７】
【００４１】
上式（７−１）を上式（２）の下段に代入すると、以下の駆動関節空間ｑAに関する運動方程式（８）を得ることができる。
【００４２】
【数８】
【００４３】
同様に、上式（７−２）を上式（２）の上段に代入すると、以下の非駆動関節空間ｑUに関する運動方程式（９）も得ることができる。
【００４４】
【数９】
【００４５】
一方、上記の操作空間ｘの速度と関節空間ｑの関節速度の関係を表した式（４）を微分して、下式（１０）に示すように、操作空間ｘに発生する加速度を得ることができる。
【００４６】
【数１０】
【００４７】
上式（１０）に上式（７−１）を代入すると、下式（１１）に示すように、操作空間ｘに発生する加速度を駆動関節空間ｑAの関節加速度で表わすことができる。
【００４８】
【数１１】
【００４９】
上記の駆動関節空間ｑAに関する運動方程式（８）を用いて上式（１１）から駆動関節空間ｑAの加速度を消去すると、下式（１２）に示すように、操作空間ｘに発生する加速度を、駆動関節空間ｑA及び非駆動関節空間ｑUにそれぞれ発生させる関節力τA、τUで表現することができる。
【００５０】
【数１２】
【００５１】
但し、ＨGは、一般化慣性行列（Ｇｅｎｅｒａｌｉｚｅｄ Ｉｎｅｒｔｉａ Ｍａｔｒｉｘ）であり、下式（１３）のように表わされる。一般化慣性行列については、例えばＹ．Ｘｕ ａｎｄ Ｔ．Ｋａｎａｄｅ，“Ｓｐａｃｅ Ｒｏｂｏｔｉｃｓ：Ｄｙｎａｍｉｃｓ ａｎｄ Ｃｏｎｔｒｏｌ”（Ｐｒｅｎｔｉｃｅ Ｈａｌｌ，１９９２）を参照されたい。
【００５２】
【数１３】
【００５３】
操作空間ｘに対し、仮想的な外力ｆを考え、下式（１４）に示すように駆動関節空間ｑAの関節力τA及び非駆動関節空間ｑUの関節力τUからなる関節力τを発生するようにする（このようにｆからτを得る演算は、「逆力運動学（Ｉｎｖｅｒｓｅ Ｆｏｒｃｅ Ｋｉｎｅｍａｔｉｃｓ）」と呼ばれる）。
【００５４】
【数１４】
【００５５】
このとき、上式（１２）で示した操作空間ｘに発生する加速度は、下式（１５）に示すように、外力ｆで表わすことができる。
【００５６】
【数１５】
【００５７】
ここで、外力ｆの係数行列を、下式（１６）に示すように、Λ-1とおく。この係数行列Λ-1は、操作空間慣性逆行列（Ｏｐｅｒａｔｉｏｎａｌ Ｉｎｖｅｒｓｅ Ｉｎｅｒｔｉａ Ｍａｔｒｉｘ）として知られている。操作空間慣性逆行列Λ-1については、例えばＯ．Ｋｈａｔｉｂ，“Ａ Ｕｎｉｆｉｅｄ Ａｐｐｒｏａｃｈ ｔｏ Ｍｏｔｉｏｎ ａｎｄ Ｆｏｒｃｅ Ｃｏｎｔｒｏｌ ｏｆ Ｒｏｂｏｔ Ｍａｎｉｐｕｌａｔｏｒｓ：Ｔｈｅ Ｏｐｅｒａｔｉｏｎａｌ Ｓｐａｃｅ Ｆｏｒｍｕｌａｔｉｏｎ”（Ｉｎ ＩＥＥＥ Ｊ．ｏｎ Ｒｏｂｏｔｉｃｓ ａｎｄ Ａｕｔｏｍａｔｉｏｎ，ｖｏｌ．３，ｎｏ．１，１９８７，ｐｐ．４３−５３）を参照されたい。
【００５８】
【数１６】
【００５９】
上式（１６）中で、慣性行列Ｈの逆行列Ｈ-1は正定値対称行列であるから、ＪＨ-1ＪTすなわち操作空間慣性逆行列Λ-1も正定値対称行列である。正定値対称行列である操作空間慣性逆行列Λ-1は、数値計算上、線形相補正問題（Ｌｉｎｅａｒ Ｃｏｍｐｒｅｍｅｎｔａｒｙ Ｐｒｏｂｌｅｍ：ＬＣＰ）として解くことができる。ゆえに、操作空間ｘに発生する加速度の目標値を実現するための外力ｆを安定に求めることができる。
【００６０】
一方、制御対象のロボットに非駆動関節ｑUが存在すると、上式（１４）中で、非駆動関節ｑUの関節力τUを発生することができない。且つ、上式（１４）でτU＝０（非駆動関節ｑUの関節力が常にゼロ）としたのでは、操作空間ｘに発生する加速度は下式（１７）に示す通りとなり、外力ｆの係数行列ＪGＨG-1ＪATは正定値対称行列とはならない。すなわち、外力ｆの係数行列を数値計算で解くことが困難になり、操作空間ｘに発生する加速度の目標値を実現するための外力ｆを安定に求めることができなくなる。
【００６１】
【数１７】
【００６２】
ところが、仮想的な外力ｆにより発生する関節力τを、上式（１４）に代えて、下式（１８）のように表現すると、操作空間ｘに発生する加速度は下式（１９）のように表わされる。下式（１８）に示すように、一般化ヤコビアンＪGを用いて、外力ｆから駆動関節空間ｑAの関節力τAを求める演算を、以下では「一般化逆力運動学（Ｇｅｎｅｒａｌｉｚｅｄ Ｉｎｖｅｒｓｅ Ｆｏｒｃｅ Ｋｉｎｅｍａｔｉｃｓ）」と呼ぶことにする。
【００６３】
【数１８】
【００６４】
【数１９】
【００６５】
上式（１９）において、外力ｆの係数行列ＪGＨG-1ＪGTは半正定値対称行列となる。一般化ヤコビアンＪGがランク落ちすると、すべての固有値がゼロになり得るが、それ以外のケースでは正定値対称行列としての解法を用いることができる。
【００６６】
以上をまとめると、上式（１）で示したロボットの運動方程式は、操作空間ｘに関するダイナミクスを表現する下式（２０）に示すように変形することができる。
【００６７】
【数２０】
【００６８】
上式（２０）は、操作空間ｘに上式（１８）で定義される力ｆを作用したときの、ｆと操作空間ｘに発生する加速度との関係を表している。但し、上式（２０）で外力ｆの係数行列ΛG-1は、「一般化操作空間慣性逆行列（Ｇｅｎｅｒａｌｉｚｅｄ Ｏｐｅｒａｔｉｏｎａｌ Ｉｎｖｅｒｓｅ Ｉｎｅｒｔｉａ Ｍａｔｒｉｘ）」と呼ぶ。また、上式（２０）の右辺の定数項ｃGは、「一般化操作空間バイアス加速度（Ｇｅｎｅｒａｌｉｌｚｅｄ Ｏｐｅｒａｔｉｏｎａｌ Ｂｉａｓ Ａｃｃｅｌａｒａｔｉｏｎ）」と呼ぶ。一般化操作空間慣性逆行列ΛG-1並びに一般化操作空間バイアス加速度ｃGは、それぞれ下式（２１）、（２２）のように表わされる。
【００６９】
【数２１】
【００７０】
【数２２】
【００７１】
一般化操作空間慣性逆行列ΛG-1及び一般化操作空間バイアス加速度ｃGが求められれば、上式（２０）より、操作空間ｘに目標加速度を発生するための制御入力となる力ｆ（言い換えれば、上式（２０）を用いて力ｆから変換される、駆動関節空間ｑAの関節力τA）を決定することができる。
【００７２】
例えば、本出願人に既に譲渡されている特開２００７−１０８９５５号公報には、線形相補正問題（ＬＣＰ）ソルバーなどを用いて操作空間慣性逆行列及び操作空間バイアス加速度を高速で且つ低い計算負荷で算出する方法について開示されている。
【００７３】
しかしながら、一般化操作空間慣性逆行列ΛG-1及び一般化操作空間バイアス加速度ｃGをそれぞれ上記の定義式（２１）及び（２２）に従って求めると、計算量が多い。一般化慣性行列ＨGを得るには、まず慣性行列Ｈを得る必要がある。関節数がＮの系において慣性行列ＨのサイズはＮ×Ｎであり、関節数Ｎに対しＯ（Ｎ2）の計算コストを要する。よって、インバージョン・コストを考えると、一般化慣性行列ＨGを得るには、関節数Ｎに対しＯ（Ｎ3）の計算コストがかかることになり、関節数が増加するにつれ急速に計算量が増大する。
【００７４】
より少ない計算量で一般化操作空間慣性逆行列ΛG-1及び一般化操作空間バイアス加速度ｃGを求める方法について、以下で考えてみる。上式（２０）で、右辺から左辺を得る計算は、外力や重力、速度積に関連する力（コリオリ力など）が作用したときの操作空間ｘにおける加速度を求める問題と見ることができる。操作空間ｘにおける加速度を求める点が通常の順動力学計算と異なるが、一種の順動力学演算と捉えることができる。順動力学演算ＦＤGは、リンク構造物の力学モデルとしての関節空間ｑ並びにその関節速度、重力ｇ、並びに外力ｆをパラメータに持ち、下式（２３）のように表現することができる。但し、下式（２３）において、ｇは重力加速度である。
【００７５】
【数２３】
【００７６】
この順動力学演算ＦＤGによれば、関節空間ｑと、重力ｇ、外力ｆといったリンク構造物に作用する力情報から、リンク構造物の各点に発生する加速度を得ることができる。
【００７７】
ここで、上式（２３）において、順動力学演算ＦＤGの入力パラメータのうち関節空間ｑ及び外力ｆ以外をすべてゼロとする制約条件下では、重力、関節力、速度積に関連する力（コリオリ力など）が発生しない状況において操作空間ｘに発生する加速度を求めることができる。すなわち、式（２２）において一般化操作空間バイアス加速度ｃG＝０とすることができる。さらに、ｆ＝ｅi、すなわち、ｉ番目の操作空間にのみ第ｉ成分が１の単位ベクトルｅiを作用させる条件下で上式（２３）の計算を実行すると、一般化操作空間慣性逆行列ΛG-1の第ｉ列を求めることができることが分る。よって、下式（２４）の演算をすべての行ｉについて実行すれば、操作空間慣性逆行ΛG-1全体を得ることができる。
【００７８】
【数２４】
【００７９】
また、外力ｆ＝０とし、順動力学演算ＦＤGの入力パラメータのうち関節空間ｑに発生する速度と重力ｇのみが作用するという制約条件下で上式（２４）の順動力学演算ＦＤGを実行することで、下式（２５）に示すように一般化操作空間バイアス加速度ｃGを算出することができる。
【００８０】
【数２５】
【００８１】
順動力学演算は、当初は逆動力学演算（加速度から力を求める演算）を利用して構成する方法が一般的であったが、Ｏ（Ｎ3）の計算量を有し、自由度数が増すと演算量が嵩むという問題がある。これに対し、特開２００７−１０８９５５号公報（前述）やＲ．Ｆｅａｔｈｅｒｓｔｏｎｅ，“Ｒｏｂｏｔ Ｄｙｎａｍｉｃｓ Ａｌｇｏｒｉｔｈｍｓ”（Ｋｌｕｗｅｒ Ａｃａｄｅｍｉｃ Ｐｕｂｌｉｓｈｅｒｓ，１９８７）には、ＡｒｔｉｃｕｌａｔｅｄＢｏｄｙ法（以下、「ＡＢ法」とする）を用いてＯ（Ｎ）の計算複雑度で順動力学演算を構成する方法について記載されている。なお、ＡＢ法自体については、例えば、“Ｔｈｅ
ｃａｌｃｕｌａｔｉｏｎ ｏｆ ｒｏｂｏｔ ｄｙｎａｍｉｃｓｕｓｉｎｇ ａｒｔｉｃｕｌａｔｅｄ−ｂｏｄｙ ｉｎｅｒｔｉａｓ”（Ｉｎｔ．Ｊ．ＲｏｂｏｔｉｃｓＲｅｓｅａｒｃｈ，ｖｏｌ．２，ｎｏ．１，ｐｐ．１３−３０，１９８３）に記載されている。特開２００７−１０８９５５号公報に記載の順動力学演算の構成方法では、ＡＢ法をイナーシャ情報算出、速度情報算出、力情報算出、加速度情報算出の４つの過程に分解する。
【００８２】
上記の順動力学演算式（２３）を、ＡＢ法と同様にＯ（Ｎ）の計算複雑度で実現することができれば、上式（２４）及び（２５）より、一般化操作空間慣性逆行列ΛG-1及び一般化操作空間バイアス加速度ｃGについても同様の計算複雑度で算出できることになる。
【００８３】
上記の順動力学演算式（２３）が通常のＡＢ法のプロセスと異なる点は、外力ｆによって発生する関節力τが、上式（１４）ではなく上式（１８）によって発生する点である。上記の定義式（３）に従って一般化ヤコビアンＪGを算出すると、慣性行列Ｈの算出を経由しなければならず、Ｏ（Ｎ2）の計算量があるため効率がよくない。
【００８４】
上記の運動方程式（２）を別な形式で書き直すと、下式（２６）に示す通りとなる。
【００８５】
【数２６】
【００８６】
すなわち、上式（２６）の左辺から右辺を得る演算を、下式（２７）のように表記することができる。
【００８７】
【数２７】
【００８８】
上式（２７）に示す演算は、加速度から作用する力を求める逆動力学と、力が作用したときに発生する加速度を求める順動力学の混合演算であり、「ハイブリッド動力学（Ｈｙｂｒｉｄ Ｄｙｎａｍｉｃｓ：ＨＤ）」と呼ばれる。ハイブリッド動力学については、例えばＲ．Ｆｅａｔｈｅｒｓｔｏｎｅ，“Ｒｏｂｏｔ Ｄｙｎａｍｉｃｓ Ａｌｇｏｒｉｔｈｍｓ”（Ｋｌｕｗｅｒ Ａｃａｄｅｍｉｃ Ｐｕｂｌｉｓｈｅｒｓ，１９８７）（前述）を参照されたい。特に、「１．非駆動関節ｑUには関節力が発生しない（非駆動関節ｑUの関節力τUは既知である）」、「２．駆動関節ｑAは動かない（駆動関節ｑAの加速度は既知である）」、「３．関節ｑの速度はすべてゼロ、重力も作用しない」、すなわち下式（２８−１）で表わされる補助モデルを考えると、下式（２８−２）が成立することが分かる。
【００８９】
【数２８】
【００９０】
上式（２８−２）は、上式（２８−１）で表わされる補助モデルのハイブリッド動力学演算の結果として得られる駆動関節力τAを符号反転したものは、上式（１８）すなわち一般化逆力運動学の結果得られる駆動関節力τAに一致することを意味している。外力ｆに対して、このようにして得られた駆動関節力τAを作用してから、特開２００７−１０８９５５号公報（前述）に記載の順動力学演算を実行することにより、一般化ヤコビアンＪGで定義される操作空間ｘに関する上記の順動力学演算式（２３）を実現することができる。特に、上記のハイブリッド動力学演算は、ＡＢ法を変形した、イナーシャ情報算出、速度情報算出、力情報算出、加速度情報算出の４つの過程に分解して、それぞれ以下に述べる演算によって、Ｏ（Ｎ）の計算量で実行することができる。したがって、一般化ヤコビアンＪGで定義される操作空間ｘに関する順動力学演算式（２３）全体も、Ｏ（Ｎ）の計算量で達成することが可能である。
【００９１】
イナーシャ情報算出：
イナーシャ情報は、すべてのリンクのＩAi（Ａｒｔｉｃｕｌａｔｅｄ Ｂｏｄｙ Ｉｎｅｒｔｉａ）を、下式（２９）をロボット装置の末端から基底に向かって実行することで求める。図１４には、床面に設置された多リンク構成の装置において、末端から基底の順序で情報を算出する通常の経路（Ｂａｃｋｗｏｒｄ Ｐｒｏｐａｇａｔｉｏｎ）を示している。
【００９２】
【数２９】
【００９３】
上式（２９）において、Ｉiはリンクｉのイナーシャ、ＣF（ｉ）はリンクｉの関節力既知の子リンクのインデックス集合、ＣA（ｉ）はリンクｉの加速度既知の子リンクのインデックス集合とする。また、Ｓiはリンクｉの関節の運動自由度を表す行列である。
【００９４】
速度情報算出：
速度情報は、すべてのリンクの原点の速度を、下式（３０）をロボット装置の基底から末端に向かって実行することで求める。図１５には、床面に設置された多リンク構成の装置において、基底から末端に向かって速度情報を算出する通常の経路（Ｆｏｒｗａｒｄ Ｐｒｏｐａｇａｔｉｏｎ）を示している。
【００９５】
【数３０】
【００９６】
上式（３０）において、ｖiはリンクｉの速度、ｐはその親リンクのインデックスとする。また、ｑiはリンクｉの関節空間である。
【００９７】
力情報算出：
力情報は、すべてのリンクのバイアス力ｐAiを、下式（３１）〜（３２）をロボット装置末端から基底に向かって実行することで求める（図１４を参照のこと）。
【００９８】
【数３１】
【００９９】
【数３２】
【０１００】
上式（３１）において、ｆkはリンクｉの中でｋ番目の外力、Ｆ（ｉ）はリンクｉに作用する外力のインデックス集合、τiはリンクｉの関節力である。
【０１０１】
加速度情報算出：
加速度情報は、すべてのリンクの加速度ａiを、下式（３３）〜（３５）を基底から末端に向かって実行することで求める（図１５を参照のこと）。リンクｉの関節が非駆動関節のときは下式（３３）、（３４）を実行し、リンクｉの関節が駆動関節のときは下式（３５）を実行する。
【０１０２】
【数３３】
【０１０３】
【数３４】
【０１０４】
【数３５】
【０１０５】
なお、上記の計算で、速度情報算出の起点となる基底の速度はｖ0＝０である。また、重力の効果は、加速度情報算出において、基底が重力加速度で上昇している、として考慮する。
【０１０６】
【数３６】
【０１０７】
以上の計算を、イナーシャ情報算出⇒速度情報算出⇒力情報算出⇒加速度情報算出の順で実行することにより、ハイブリッド動力学演算を効率的に実行することができる。ひいては、一般化操作空間慣性逆行列ΛG-1及び一般化操作空間バイアス加速度ｃGを効率的に算出することが可能となり、非駆動関節を有したロボットであっても、リンク構造の任意の箇所において所望の加速度を発生させるための関節力を決定することができ、リンク構造物の任意の部位において精密な加速度操作が可能となる。
【０１０８】
操作空間をすべてまとめた変数としてｘ を考えると、すべての拘束は以下のような不等式を含む線形拘束条件式（３７）〜（３８）として記述することができる。
【０１０９】
【数３７】
【０１１０】
【数３８】
【０１１１】
ここで、ｗは一種のスラック変数である．また，上式（３８）中でそれぞれ存在するＬi及びＵiは、ｉ番目の操作空間に作用する仮想的な力ｆiの下限と上限をそれぞれ表す。
【０１１２】
上式（３７）〜（３８）は、線形相補性問題である。そして、一般化操作空間慣性逆行列ΛG-1及び一般化操作空間バイアス加速度ｃGが既知であれば、この線形相補性問題を解くことによって、線形拘束条件を満足する操作空間ｘに目標加速度を発生させるための仮想的な外力ｆを決定することができる。線形相補性問題の数学的な解法自体については、例えば“Ｆａｓｔ Ｃｏｎｔａｃｔ Ｆｏｒｃｅ Ｃｏｍｐｕｔａｔｉｏｎ ｆｏｒ Ｎｏｎｐｅｎｅｔｒａｔｉｎｇ Ｒｉｇｉｄ Ｂｏｄｉｅｓ”（ＳＩＧＧＲＡＰＨ９４，ｐｐ．２３−３４，１９９４）に記載されているので、本明細書では説明を省略する。
【０１１３】
線形相補性問題として操作空間ｘの目標加速度を発生させるための力ｆを求める場合の多くは、一般化操作空間慣性逆行列ΛG-1を得る部分において最も計算を要する。上述したように、本実施形態によれば、一般化操作空間慣性逆行列ΛG-1の算出を高速に行なうことができるので、ロボットの実機上でも実時間の処理が可能である。こうして力ｆが得られれば、拘束条件式（３７）〜（３８）を満足する関節力τ
を上式（１８）に基づいて決定することができる。
【０１１４】
ここまでをまとめると、非駆動関節を有したロボット（Ｕｎｄｅｒ Ａｃｔｕａｔｅｄ Ｓｙｓｔｅｍ）を、図１に示すように、（１）すべての関節が可動関節（Ｍｏｖａｂｌｅ Ｊｏｉｎｔ）となるロボットの主モデル（Ｆｕｌｌｙ Ｍｏｖａｂｌｅ Ｓｙｓｔｅｍ）と、（２）駆動関節（Ａｃｔｕａｔｅｄ Ｊｏｉｎｔ）を不動関節（Ｉｍｍｏｖａｂｌｅ Ｊｏｉｎｔ）とし、非駆動関節（Ｕｎａｃｔｕａｔｅｄ Ｊｏｉｎｔ）を可動関節とした補助モデル（Ｐａｒｔｉａｌｌｙ Ｒｉｇｉｄ Ｓｙｓｔｅｍ）という２つのモデルの組み合わせによって表現することができる。そして、前者の主モデルでは通常の順動力学演算（上式（２３）を参照のこと）を実行し、後者の補助モデルではハイブリッド動力学演算（上式（２８−２）を参照のこと）を実行することで、系全体の操作空間物理量を効率的に求めて制御する。
【０１１５】
図２には、主モデル及び補助モデルを用いた、非駆動関節を有するロボットの制御システム構成例を模式的に示している。
【０１１６】
図示の制御システム１００は、主モデル（Ｍａｉｎ Ｍｏｄｅｌ：Ｆｕｌｌｙ Ａｃｔｕａｔｅｄ Ｓｙｓｔｅｍ Ｍｏｄｅｌ）１０１と、補助モデル（Ａｕｘｉｌｉａｒｙ Ｍｏｄｅｌ：Ｐａｒｔｉａｌｌｙ Ｒｉｇｉｄ Ｍｏｄｅｌ）１０２と、順動力学演算部（Ｆｏｒｗａｒｄ Ｄｙｎａｍｉｃｓ Ｅｘｅｃｕｔｏｒ）１０３と、ハイブリッド動力学演算部（Ｈｙｂｒｉｄ Ｄｙｎａｍｉｃｓ Ｅｘｅｃｕｔｏｒ）１０４と、一般化逆力運動学演算部（Ｇｅｎｅｒａｌｉｚｅｄ Ｉｎｖｅｒｓｅ Ｆｏｒｃｅ Ｋｉｎｅｍａｔｉｃｓ Ｅｘｅｃｕｔｏｒ）１０５と、一般化操作空間慣性逆行列演算部（Ｇｅｎｅｒａｌｉｚｅｄ Ｏｐｅｒａｔｉｏｎａｌ Ｉｎｖｅｒｓｅ Ｉｎｅｒｔｉａ Ｃａｌｃｕｌａｔｏｒ）１０６と、一般化操作空間バイアス加速度演算部（Ｇｅｎｅｒａｌｉｚｅｄ Ｏｐｅｒａｔｉｏｎａｌ Ｂｉａｓ Ａｃｃｅｌｅｒａｔｉｏｎ Ｃａｌｃｕｌａｔｏｒ）１０７と、仮想外力算出部（Ｌｉｎｅａｒ Ｃｏｍｐｌｅｍｅｎｔａｒｙ Ｐｒｏｂｌｅｍ Ｓｏｌｖｅｒ）１０８と、全身協調力制御部（Ｗｈｏｌｅ−ｂｏｄｙ Ｃｏｏｅｐｅｒａｔｅｄ Ｆｏｒｃｅ Ｃｏｎｔｒｏｌｌｅｒ）１０９と、関節力制御部（Ｊｏｉｎｔ Ｆｏｒｃｅ Ｃｏｎｔｒｏｌｌｅｒ）１１０で構成される。
【０１１７】
主モデル１０１は、すべての関節を可動関節（Ｍｏｖａｂｌｅ Ｊｏｉｎｔ）としたロボットの動力学モデルである。一方、補助モデル１０２は、駆動関節を不動関節（Ｉｍｍｏｖａｂｌｅ Ｊｏｉｎｔ）とし、非駆動関節を可動関節としたロボットの動力学モデルである。補助モデル１０２では、「１．非駆動関節ｑUには関節力が発生しない（非駆動関節ｑUの関節力τUは既知である）」、「２．駆動関節ｑAは動かない（駆動関節ｑAの加速度は既知である）」、「３．関節ｑの速度はすべてゼロ、重力も作用しない」（前述）。図１中の中央に示したモデルが主モデル１０１に相当し、図１中の右に示したモデルが補助モデル１０２に相当する。
【０１１８】
ハイブリッド動力学演算部１０４は、補助モデル１０２を用いて、上記の逆動力学と順動力学の混合演算式（２７）を実行して、関節加速度が既知の関節に作用する関節力と、関節力が既知の関節に発生する関節加速度をそれぞれ算出することができる。
【０１１９】
補助モデル１０２は、駆動関節に相当する不動関節と、非駆動関節に相当する可動関節が混在する系である。上式（２８−１）で示したように、不動関節として扱われる駆動関節は、動かないという意味で関節加速度が既知であり、可動関節として扱われる非駆動関節は、関節力が発生しないという意味で関節力が既知である。補助モデル１０２を、関節加速度が既知である不動関節と、関節力が既知である可動関節が混在する系、と言い換えることができる。
【０１２０】
一般化逆力運動学演算部１０５は、上式（２８−１）に示した条件をハイブリッド動力学演算部１０４に与えて上記の混合演算式（２７）を実行させることで、補助モデル１０２上の不動関節すなわち駆動関節ｑAに作用する関節力τAと、可動関節すなわち非駆動関節ｑUに発生する関節加速度をそれぞれ算出する。また、一般化逆力運動学演算部１０５は、一般化逆力運動学演算を実行して（式（１８）を参照のこと）、関節力τAからｆを求める。
【０１２１】
順動力学演算部１０３は、主モデル１０１を用いて、上記の操作空間ｘに関する順動力学演算式（２３）を実行して、操作空間ｘに発生する加速度を求める。ここで、外力ｆは、上式（１４）により関節力に変換するのではなく、上式（１８）によって関節力τAに変換する点で、特開２００７−１０８９５５号公報（前述）に開示された操作空間物理量算出装置とは異なる点に留意されたい。このため、順動力学演算部１０３は、一般化逆力運動学演算部１０５に関節空間ｑ及び外力ｆを与えて（ＩＦＫG（ｑ，ｆ））、一般化逆力運動学演算部１０５が求めた駆動関節の関節力τA（＝ＪGTｆ）を用いて順動力学演算式（２３）を実行する。
【０１２２】
一般化操作空間慣性逆行列演算部１０６は、上式（２４）の条件に基づいて順動力学演算ＦＤGを繰り返し実行して、上式（２１）に示した一般化操作空間慣性逆行列ΛG-1を算出する。具体的には、一般化操作空間慣性逆行列演算部１０６は、順動力学演算部１０３に対する入力パラメータのうち関節空間ｑ及び外力ｆ以外をすべてゼロとする制約条件下で、さらに、ｆ＝ｅi、すなわち、ｉ番目の操作空間にのみ単位力を作用させる条件下で、順動力学演算部１０３に上式（２３）の計算を繰り返し実行させることによって、一般化操作空間慣性逆行列ΛG-1のすべての列の成分を求める。
【０１２３】
一般化操作空間バイアス加速度演算部１０７は、上式（２５）の条件に基づいて順動力学演算ＦＤGを１回だけ実行して、一般化操作空間バイアス加速度ｃGを算出する。具体的には、一般化操作空間バイアス加速度演算部１０７は、順動力学演算部１０３に対する入力パラメータのうち関節空間ｑに発生する速度と重力ｇのみが作用するという制約条件下で、順動力学演算部１０３に上式（２３）の計算を１回だけ実行させることによって、一般化操作空間バイアス加速度ｃGを算出させる。
【０１２４】
仮想外力算出部１０８は、一般化操作空間慣性逆行列ΛG-1と一般化操作空間バイアス加速度ｃGを用いて表される操作空間ｘに作用する力と加速度の関係式（２０）において、操作空間ｘに発生する目標加速度を既知とし、それを達成するために操作空間ｘに発する仮想的な外力ｆを解く。ここで、既知の目標加速度は、外部（インピーダンス・コントローラーなど）から仮想外力算出部１０８に入力される制御指令値である。例えば、仮想外力算出部１０８は、上記関係式（２０）と、ロボットのリンク同士及びリンクと環境との間で形成される不等式拘束条件からなる線形相補性問題（ＬＣＰ）として捉え、ＬＣＰソルバーを用いて、操作空間ｘに発生する加速度の目標値を実現するための外力ｆを安定に求めることができる（前述）。
【０１２５】
全身協調力制御部１０９は、仮想外力算出部１０８から得られた仮想的な外力ｆを、上式（１８）で示した一般化逆力運動学演算を用いて駆動関節の駆動関節力τAに変換し、これを関節力制御部１１０の制御目標値として出力する。
【０１２６】
関節力制御部１１０は、全身協調力制御部１０９が作成した各関節の制御目標値を実現するように、ロボットの各駆動関節の関節力を制御する。精密に関節力を制御するには、例えば、本出願人に既に譲渡されている特開２００９−２６９１０２号公報に開示されている、力制御を行なうアクチュエータ制御装置を適用することができる。また、同アクチュエータ制御装置には、アクチュエータに印加されるトルクを正確に計測するために、本出願人に既に譲渡されている特開２００９−２８８１９８号公報に開示されているトルク計測装置を適用してもよい。
【０１２７】
図３には、図２に示したロボットの制御システムにおいて実行される処理手順をフローチャートの形式で示している。
【０１２８】
まず、上式（３）に示した一般化ヤコビアンＪGに対応する操作空間ｘを定義する（ステップＳ３１）。すなわち、ロボットの任意の部位に発生する加速度を制御する運動空間を定義する。例えば、ロボットの手先の位置を制御する場合、手先リンクと手先リンク座標系から見た手先の位置を指定する。
【０１２９】
次いで、操作空間ｘに発生させる目標加速度を設定する（ステップＳ３２）。すなわち。操作空間ｘのダイナミクスを示した上式（２０）の左辺を与える。操作空間ｘの加速度目標値が指定できれば、速度及び位置も指定できる。例えば、操作空間ｘの速度を制御したい場合は下式（３９−１）に示すように速度目標値から加速度目標値を指定すればよいし、操作空間ｘの位置を制御したい場合は下式（３９−２）に示すように位置目標値から加速度目標値を指定すればよい。但し、Ｋv、Ｋpはそれぞれ速度ゲイン、位置ゲインである。
【０１３０】
【数３９】
【０１３１】
次いで、一般化操作空間慣性逆行列演算部１０６により、一般化操作空間慣性逆行列ΛG-1（式（２１）を参照のこと）を算出する（ステップＳ３３）。
【０１３２】
次いで、一般化操作空間バイアス加速度演算部１０７により、一般化操作空間バイアス加速度ｃGを算出する（ステップＳ３４）。
【０１３３】
次いで、仮想外力算出部１０８は、先行するステップＳ３３、Ｓ３４で得られた一般化操作空間慣性逆行列ΛG-1と一般化操作空間バイアス加速度ｃGを用いて、上式（２０）から、ステップＳ３２で設定した目標加速度を達成するために操作空間ｘに発する仮想的な外力ｆを解く（ステップＳ３５）。本実施形態では、仮想外力算出部１０８は、上記関係式（３７）と、ロボットのリンク同士及びリンクと環境との間で形成される不等式拘束条件式（３８）からなる線形相補性問題を、ＬＣＰソルバーを用いて解いて、拘束条件式（３７）〜（３８）を満足する満足する仮想外力ｆを求める。
【０１３４】
次いで、全身協調力制御部１０９は、先行するステップＳ３５で得られた仮想外力ｆを、上式（１８）で示した一般化逆力運動学演算を用いて駆動関節の駆動関節力τAに変換する（ステップＳ３６）。
【０１３５】
そして、関節力制御部１１０は、先行するステップＳ３６で得られた関節力τAを各関節の制御目標値とし、これを実現するように、ロボットの各駆動関節の関節力を制御する（ステップＳ３７）。
【０１３６】
図２に示したロボットの制御システム１００では、図３に示した処理手順を、例えば１ＫＨｚの制御周波数で実行する。
【０１３７】
図４には、図３に示したフローチャート中のステップＳ３３において、一般化操作空間慣性逆行列演算部１０６により一般化操作空間慣性逆行列ΛG-1を算出するための処理手順をフローチャートの形式で示している。
【０１３８】
まず、一般化操作空間慣性逆行列演算部１０６は、列インデクスｉを初期値０に設定する（ステップＳ４１）。
【０１３９】
次いで、一般化逆力運動学演算部１０５は、関節空間ｑ及び外力ｆ以外をすべてゼロとする制約条件下で、さらに、ｆ＝ｅi、すなわち、ｉ番目の操作空間にのみ単位力を作用させる条件下で、駆動関節空間ｑAに発生する等価関節力τAを求める（ステップＳ４２）。
【０１４０】
次いで、順動力学演算部１０３は、一般化逆力運動学演算部１０５が求めた駆動関節の関節力τA（＝ＪGTｆ）を用いて順動力学演算式（２３）を実行して、すべての操作空間に発生する加速度を求める（ステップＳ４３）。すなわち、順動力学演算部１０３は、上式（２４）を実行する。
【０１４１】
次いで、一般化操作空間慣性逆行列演算部１０６は、このようにして得られた加速度を、一般化操作空間慣性逆行列ΛG-1の第ｉ列に代入する（ステップＳ４４）。
【０１４２】
そして、一般化操作空間慣性逆行列演算部１０６は、列インデクスｉを１だけ増分し（ステップＳ４５）、ｉが操作空間数に到達するまで（ステップＳ４６のＮｏ）、ステップＳ４２〜Ｓ４５の処理ルーチンを繰り返し実行する。このようにして、一般化操作空間慣性逆行列演算部１０６は、一般化操作空間慣性逆行列ΛG-1全体を得ることができる。
【０１４３】
図５には、図３に示したフローチャート中のステップＳ３４において、一般化操作空間バイアス加速度演算部１０７により一般化操作空間バイアス加速度ｃGを算出するための処理手順をフローチャートの形式で示している。
【０１４４】
一般化操作空間バイアス加速度演算部１０７は、関節空間ｑに発生する速度と重力ｇのみが作用するという制約条件下で上式（２４）の順動力学演算ＦＤGを１回だけ実行して、上式（２２）に示した一般化操作空間バイアス加速度ｃGを上式（２５）に示すように算出する（ステップＳ５１）。
【０１４５】
そして、一般化操作空間バイアス加速度演算部１０７は、求めた一般化操作空間バイアス加速度ｃGを、操作空間ｘのダイナミクスを示した上式（２０）に代入する（ステップＳ５２）。
【０１４６】
以下では、図１〜図３に示した制御系を、図６Ａに示す倒立振子型ロボットに適用した例について説明する。図示のロボットは、肩３自由度、肘２自由度、手首２自由度、及びグリッパー１自由度を有する左右の腕部を備えている。また、同ロボットは、頭部には２自由度、腰部に１自由度、対向２輪の車輪を有している。同ロボットは、車輪以外の接地点を持たない。
【０１４７】
図６Ａに示したロボットの各関節に、例えば特開２００９−２８８１９８号公報に開示されているトルク計測装置を搭載して、精密にトルク制御することができる。ロボットの体中に分散して配置された複数のサテライトＣＰＵ（Ｃｅｎｔｒａｌ Ｐｒｏｃｅｓｓｉｎｇ Ｕｎｉｔ）でそれぞれトルク制御が実行され、各サテライトＣＰＵはリアルタイムＬＡＮ（Ｌｏｃａｌ Ａｒｅａ Ｎｅｔｗｏｒｋ）を介して中央のＣＰＵと通信するものとする。また、ロボットの機体にはＩＭＵ（Ｉｎｅｒｔｉａｌ Ｍｅａｓｕｒｅｍｅｎｔ Ｕｎｉｔ）が搭載され、カルマン（Ｋａｌｍａｎ）フィルタを用いた姿勢演算が行なわれるものとする。この演算値も、リアルタイムＬＡＮを介して中央のＣＰＵに転送される。
【０１４８】
本出願人に既に譲渡されている特開２０１０−１８８４７１号公報には、図６Ａに示したものと同様の対向２輪型移動ロボットを、枝分かれのあるマニピュレーターとして表現した対向２輪等価モデルについて開示されている。図６Ｂに示すように、対向２輪等価関節に続いて非駆動関節を１つ挿入して表現することで、同ロボット全体を倒立振子型の２輪ロボットとしてモデル化する。図６Ｃには、図６Ｂに示した倒立振子型ロボットのすべての関節を可動とした主モデルを示している。また、図６Ｄには、図６Ｂに示した倒立振子型ロボットの駆動関節を不動とするとともに非駆動関節を可動とした補助モデルを示している。
【０１４９】
図７には、図６に示したロボットのバランスを保ちながら、機体の所定部位の位置や姿勢（方位）を一定に保つための制御系の構成例を示している。図示のように、制御ユニット７００には、さまざまな部位に対する加速度目標値が入力される。制御ユニット７００は、図３〜図５に示した処理手順を実行して、各部位に対する加速度目標値を実現するための、駆動関節空間ｑAの関節力τAを得る。そして、制御ユニット７００は、この関節力τAを用いてロボットの各駆動関節をトルク制御することで、すべての部位の運動目的を同時に満たすことができる。
【０１５０】
図６に示したロボットのバランス維持のためには、Ｓｌｉｄｉｎｇ Ｍｏｄｅ制御系（Ｓｌｉｄｉｎｇ Ｍｏｄｅ Ｃｏｎｔｒｏｌｌｅｒ）７０１が別途構成される。制御ユニット７０は、Ｓｌｉｄｉｎｇ Ｍｏｄｅ制御系の出力として、前後方向ｘ1の加速度目標値を得る。
【０１５１】
また、左右のグリッパーの位置及び姿勢をグローバル座標内で一定に保つ制御系（単純なＰＤ制御）として、Ｌｅｆｔ Ｈａｎｄ Ｐｏｓｉｔｉｏｎ Ｃｏｎｔｒｏｌｌｅｒ７０２、Ｌｅｆｔ Ｈａｎｄ Ｐｏｓｔｕｒｅ Ｃｏｎｔｒｏｌｌｅｒ７０３、Ｒｉｇｈｔ Ｈａｎｄ Ｐｏｓｉｔｉｏｎ Ｃｏｎｔｒｏｌｌｅｒ７０４、Ｒｉｇｈｔ Ｈａｎｄ Ｐｏｓｔｕｒｅ Ｃｏｎｔｒｏｌｌｅｒ７０５が構成され、制御ユニット７０は、各制御系からそれぞれ並進ｘｙｚ方向ｘ2,3,4、ｘ8,9,10の加速度目標値と、回転ｘｙｚ方向ｘ5,6,7、ｘ11,12,13の加速度目標値を得る。頭部の姿勢とベースの方位についても、Ｈｅａｄ Ｐｏｓｔｕｒｅ Ｃｏｎｔｒｏｌｌｅｒ７０６とＢａｓｅ Ｏｒｉｅｎｔａｔｉｏｎ Ｃｏｎｔｒｏｌｌｅｒ７０７が構成され、制御ユニット７００は、各制御系から頭部の回転ｘｙｚ方向ｘ14,15,16及びベースの方位ｘ17の加速度目標値を得る。
【０１５２】
制御ユニット７００は、各制御系からそれぞれの加速度目標値を入力すると、図３〜図５に示した処理手順を実行して、各部位に対する加速度目標値を実現するための、駆動関節空間ｑAの関節力τAを得る。そして、制御ユニット７００は、この関節力τAを用いてロボットの各駆動関節をトルク制御することで、すべての部位の運動目的を同時に満たすことができる。
【０１５３】
図８Ａ〜図８Ｃには、ワインが注がれたグラスを左手のグリッパーで把持したロボットが、前後に押されたり左右にねじられたりして外力が印加されても、グラスのワインをこぼさずに保持している様子を示している。
【０１５４】
図９には、ロボットが後方に押された際の車輪の前後位置と左手先位置をそれぞれ示している。また、図１０には、ロボットが後方に押された際のロボットの重心の位相平面上での変化（位置及び速度の変化）を示している。これらの図から、ロボットが、外力が印加されても、車輪位置を前後させながらも左手先位置を一定に保っている、すなわちバランスを保ちながら手先の位置及び姿勢を維持していることが分かる。
【０１５５】
なお、上式（１８）に示した一般化逆力運動学によって、一般化ヤコビアンＪGを用いて力ｆから関節力τAが求められることは、既に述べた通りである。一般化逆力運動学の別の応用として、上式（６）を用いて逆運動学演算を実行する際に、一般化ヤコビアンＪGを高速算出するのにも用いることができる。具体的には、上記の一般化逆力運動学演算式（１８）を下式（４０）のように表記する。
【０１５６】
【数４０】
【０１５７】
そして、ハイブリッド動力学演算部１０４は、関節空間ｑ及び外力ｆ以外をすべてゼロとし、ｉ番目の操作空間にのみ第ｉ成分が１の単位ベクトルｅiからなる外力を作用させる。
【０１５８】
【数４１】
【０１５９】
そして、このような外力ｆの作用条件下で、駆動関節空間の関節力を算出するハイブリッド動力学演算を操作空間数にわたり繰り返し実行させることによって、一般化ヤコビアンＪG全体を求めることができる。上式（４０）をＯ（Ｎ）の計算量で求めることができることから、一般化ヤコビアンＪGもＯ（Ｎ）の計算量で求めることができる。
【産業上の利用可能性】
【０１６０】
以上、特定の実施形態を参照しながら、本発明について詳細に説明してきた。しかしながら、本発明の要旨を逸脱しない範囲で当業者が該実施形態の修正や代用を成し得ることは自明である。
【０１６１】
本発明によれば、倒立振子型ロボットや歩行ロボット、宇宙ロボットなど、力を発することのできない非駆動関節を有するロボットにおいて、全身の加速度オーダーの精密な制御を実時間で実行することが可能である。また、一般化ヤコビアンを用いた、従来の宇宙ロボット用インバースキネマティクスも、ロボットのリンク数Ｎに対してＯ（Ｎ）の計算複雑度で求めることが可能となり、多自由度なロボットほど処理負荷を低減させることができる。ゆえに、これらのロボットの普及を容易にすることができる。
【０１６２】
要するに、例示という形態で本発明を開示してきたのであり、本明細書の記載内容を限定的に解釈するべきではない。本発明の要旨を判断するためには、特許請求の範囲を参酌すべきである。
【符号の説明】
【０１６３】
１００…制御システム
１０１…主モデル
１０２…補助モデル
１０３…順動力学演算部
１０４…ハイブリッド動力学演算部
１０５…一般化逆力運動学演算部
１０６…一般化操作空間慣性逆行列演算部
１０７…一般化操作空間バイアス加速度演算部
１０８…仮想外力算出部
１０９…全身協調力制御部
１１０…関節力制御部
【技術分野】
【０００１】
本発明は、リンク構造物からなるロボットの制御装置及び制御方法、並びにコンピューター・プログラムに係り、特に、関節の一部が力を発することのない非駆動関節であるロボットの制御装置及び制御方法、並びにコンピューター・プログラムに関する。
【背景技術】
【０００２】
倒立振子型のロボットや２足歩行型のロボットは、接地面積が小さいため、人と共存する際に邪魔になり難く、人にさまざまなサービスを提供することができる、という優れた側面を持っている。逆に、この種のロボットは、接地面積が小さいゆえに、タスク遂行のために環境から得られる外力に関する制約が厳しいという問題がある。倒立振子ロボットでは、支持点回りのモーメントを得ることができないし、２足歩行型のロボットでは、足底形状によって路面から得られるモーメントに不等式制約が課される。より極端な例として、宇宙ロボットが挙げられる。宇宙ロボットは、環境からの反力を一切得ることができない。これらのロボットは、関節の一部が力を発することのできない、非駆動関節としてモデル化できることを特徴としている。
【０００３】
図１１〜図１３には、非駆動関節を有するロボットの関節構成を例示している。図１１は、倒立振子型のロボットの関節構成例を示している。車輪の並進移動成分は、直動関節で表現され、車輪によるロボット全体の姿勢変化は、次に連結される回転関節で表される。ロボット全体は、この点回りに自由に回転し得ることから、この関節が非駆動関節として表現される。残りの関節は腕部の関節であり、いずれも駆動関節である。また、図１２は、２足歩行型のロボットの関節構成例を示している。脚部の回転関節はいずれも駆動関節であり、その他、ロボット全体が空間内を自由に移動できることを表現するための、３自由度並進と３自由度回転からなる６自由度を有する。この６自由度は仮想的若しくは論理的な関節であり、実在しないため、力を発することができない。ゆえに、非駆動関節として表現されている。２足歩行型ロボットは、６自由度の非駆動関節を有する系としてモデル化されるが、足先から得られる力及びモーメント成分を利用して、全身の制御をすることができる。また、図１３は、宇宙ロボットの関節構成例を示している。宇宙ロボットに搭載されるロボット・アームの関節は駆動関節として表現されるが、ロボット全体の３自由度並進と３自由度回転自由度については、６自由度の非駆動関節として表現される。宇宙ロボットは、２足歩行ロボットと異なり、外力を得ることができない。
【０００４】
ロボットなどのリンク構造物において、例えば手先の位置に所望の速度を発生させるためには関節速度を得る必要がある。あるいは、所望の加速度を発生させるためには関節力を得る必要がある。駆動関節のみから構成される系では、関節速度のすべての成分、あるいは、関節加速度のすべての成分を制御することができる。ところが、非駆動関節が含まれる系では、非駆動関節の成分を制御することはできない、という問題がある。
【０００５】
力制御系のロボット制御において重要な概念として、ロボットに作用する力と発生する加速度の関係を記述するための空間、すなわち操作空間（Ｏｐｅｒａｔｉｏｎａｌ Ｓｐａｃｅ）が挙げられる。例えば、ロボットの手先の位置を操作空間として定義して、手先に所望の加速度を発生させるための関節力を決定するのに操作空間が用いられる。操作空間の加速度を精密に制御するための方法が幾つか提案されているが（例えば、特許文献１、２を参照のこと）、いずれの方法もすべての関節が駆動関節である場合に対象が限定されている。
【先行技術文献】
【特許文献】
【０００６】
【特許文献１】特開２００９−９５９５９号公報
【特許文献２】特開２０１０−１８８４７１号公報
【発明の概要】
【発明が解決しようとする課題】
【０００７】
本発明の目的は、非駆動関節を含むロボットの加速度オーダーの精密な制御を実現することができる、優れたロボットの制御装置及び制御方法、並びにコンピューター・プログラムを提供することにある。
【課題を解決するための手段】
【０００８】
本願は、上記課題を参酌してなされたものであり、請求項１に記載の発明は、
駆動関節及び非駆動関節を有するロボットの前記駆動関節を不動とした補助モデルを用いて、逆動力学と順動力学からなるハイブリッド動力学演算を実行して、関節加速度が既知である不動関節に作用する関節力と、関節力が既知である可動関節に発生する関節加速度を算出するハイブリッド動力学演算部と、
前記ロボットのすべての関節を可動とした主モデルを用いて、前記ロボットに作用する既知の力によって発生する加速度を算出する順動力学演算部と、
前記ハイブリッド動力学演算部及び前記順動力学演算部による演算結果に基づいて、前記ロボットの任意の部位に所望の目標加速度を発生させるための関節力を決定する関節力決定部と、
前記決定した関節力を制御目標値として、前記ロボットの各関節の関節力を制御する関節力制御部と、
を具備するロボットの制御装置である。
【０００９】
本願の請求項２に記載の発明によれば、請求項１に記載のロボットの制御装置のハイブリッド動力学演算部によって得られた前記ロボットに作用する力を、一般化ヤコビアンを用いて関節力に変換するように構成されている。
【００１０】
本願の請求項３に記載の発明によれば、請求項１に記載のロボットの制御装置の関節力決定部は、
関節値及び外力以外をすべてゼロとし、ｉ番目の操作空間にのみ第ｉ成分が１の単位ベクトルｅiからなる外力を作用させる条件下で、前記順動力学演算部に前記順動力学演算をすべての操作空間にわたり繰り返し実行させることによって、一般化操作空間慣性逆行列を求める一般化操作空間慣性逆行列演算部と、
関節空間に発生する速度と重力のみが作用するという制約条件下で、前記順動力学演算部に前記順動力学演算を１回だけ実行させて、一般化操作空間バイアス加速度を求める一般化操作空間バイアス加速度演算部と、
前記一般化操作空間慣性逆行列及び前記一般化操作空間バイアス加速度を用いて表される操作空間に作用する力と加速度の関係式において、前記操作空間に発生する目標加速度を既知とし、それを達成するために前記操作空間に発する仮想的な外力ｆを解く仮想外力算出部と、
前記仮想外力算出部が求めた外力ｆを、一般化逆力運動学演算を用いて駆動関節の駆動関節力に変換する関節算出部と、
を備えている。
【００１１】
本願の請求項４に記載の発明によれば、請求項３に記載のロボットの制御装置の仮想外力算出部は、前記関係式と前記ロボットと環境との間で形成される拘束条件からなる線形相補性問題を満足する前記仮想的な外力を、前記一般化操作空間慣性逆行列及び前記一般化操作空間バイアス加速度演算部を用いて求め、得られた力に基づいてリンクの関節力を決定するように構成されている。
【００１２】
本願の請求項５に記載の発明によれば、請求項１に記載のロボットの制御装置のハイブリッド動力学演算部に、関節空間及び外力以外をすべてゼロとし、ｉ番目の操作空間にのみ第ｉ成分が１の単位ベクトルｅiからなる外力を作用させる条件下で、駆動関節空間の関節力を算出するハイブリッド動力学演算を繰り返し実行させることによって、一般化ヤコビアンを求めるように構成されている。
【００１３】
また、本願の請求項６に記載の発明は、
駆動関節及び非駆動関節を有するロボットの前記駆動関節を不動とした補助モデルを用いて、逆動力学と順動力学からなるハイブリッド動力学演算を実行して、関節加速度が既知である不動関節に作用する関節力と、関節力が既知である可動関節に発生する関節加速度を算出するハイブリッド動力学演算ステップと、
前記ロボットのすべての関節を可動とした主モデルを用いて、前記ロボットに作用する既知の力によって発生する加速度を算出する順動力学演算ステップと、
前記ハイブリッド動力学演算ステップ及び前記順動力学演算ステップによる演算結果に基づいて、前記ロボットの任意の部位に所望の目標加速度を発生させるための関節力を決定する関節力決定ステップと、
前記決定した関節力を制御目標値として、前記ロボットの各関節の関節力を制御する関節力制御ステップと、
を有するロボットの制御方法である。
【００１４】
また、本願の請求項７に記載の発明は、駆動関節及び非駆動関節を有するロボットを制御するための処理をコンピューター上で実行するようにコンピューター可読形式で記述されたコンピューター・プログラムであって、前記コンピューターを、
前記ロボットの前記駆動関節を不動とした補助モデルを用いて、逆動力学と順動力学からなるハイブリッド動力学演算を実行して、関節加速度が既知である不動関節に作用する関節力と、関節力が既知である可動関節に発生する関節加速度を算出するハイブリッド動力学演算部、
前記ロボットのすべての関節を可動とした主モデルを用いて、前記ロボットに作用する既知の力によって発生する加速度を算出する順動力学演算部、
前記ハイブリッド動力学演算部及び前記順動力学演算部による演算結果に基づいて、前記ロボットの任意の部位に所望の目標加速度を発生させるための関節力を決定する関節力決定部、
前記決定した関節力を制御目標値として、前記ロボットの各関節の関節力を制御する関節力制御部、
として機能させるためのコンピューター・プログラムである。
【００１５】
本願の請求項７に係るコンピューター・プログラムは、コンピューター上で所定の処理を実現するようにコンピューター可読形式で記述されたコンピューター・プログラムを定義したものである。換言すれば、本願の請求項７に係るコンピューター・プログラムをコンピューターにインストールすることによって、コンピューター上では協働的作用が発揮され、本願の請求項１に係るロボットの制御装置と同様の作用効果を得ることができる。
【発明の効果】
【００１６】
本発明によれば、非駆動関節を含むロボットの加速度オーダーの精密な制御を、関節数Ｎに対してＯ（Ｎ）という少ない計算量で達成することができる、優れたロボットの制御装置及び制御方法、並びにコンピューター・プログラムを提供することができる。
【００１７】
本発明のさらに他の目的、特徴や利点は、後述する本発明の実施形態や添付する図面に基づくより詳細な説明によって明らかになるであろう。
【図面の簡単な説明】
【００１８】
【図１】図１は、非駆動関節を有したロボットを、全軸が可動なロボットのモデルと、駆動関節を不動とし、非駆動関節を可動とした補助モデルという２つのモデルで表現した図である。
【図２】図２は、主モデル及び補助モデルを用いた、非駆動関節を有するロボットの制御システム構成例を模式的に示した図である。
【図３】図３は、図２に示したロボットの制御システムにおいて実行される処理手順を示したフローチャートである。
【図４】図４は、一般化操作空間慣性逆行列演算部１０６により一般化操作空間慣性逆行列ΛG-1を算出するための処理手順を示したフローチャートである。
【図５】図５は、一般化操作空間慣性逆行列演算部１０６により一般化操作空間バイアス加速度ｃGを算出するための処理手順を示したフローチャートである。
【図６Ａ】図６Ａは、図１〜図３に示した制御系を適用する倒立振子型ロボットの構成例を示した図である。
【図６Ｂ】図６Ｂは、図１〜図３に示した制御系を適用する倒立振子型ロボットの構成例を示した図である。
【図６Ｃ】図６Ｃは、図６Ｂに示した倒立振子型ロボットのすべての関節を可動とした主モデルを示した図である。
【図６Ｄ】図６Ｆは、図６Ｂに示した倒立振子型ロボットの駆動関節を不動とするとともに非駆動関節を可動とした補助モデルを示した図である。
【図７】図７は、図６に示したロボットのバランスを保ちながら、機体の所定部位の位置や姿勢（方位）を一定に保つための制御系の構成例を示した図である。
【図８Ａ】図８Ａは、ワインが注がれたグラスをグリッパーで把持したロボットが、外力が印加されても、グラスのワインをこぼさずに保持している様子を示した図である。
【図８Ｂ】図８Ｂは、ワインが注がれたグラスをグリッパーで把持したロボットが、外力が印加されても、グラスのワインをこぼさずに保持している様子を示した図である。
【図８Ｃ】図８Ｃは、ワインが注がれたグラスをグリッパーで把持したロボットが、外力が印加されても、グラスのワインをこぼさずに保持している様子を示した図である。
【図９】図９は、ロボットが後方に押された際の車輪の前後位置と左手先位置をそれぞれ示した図である。
【図１０】図１０は、ロボットが後方に押された際のロボットの重心の位相平面上での変化（位置及び速度の変化）を示した図である。
【図１１】図１１は、倒立振子型のロボットの関節構成を示した図である。
【図１２】図１２は、２足歩行型のロボットの関節構成例を示した図である。
【図１３】図１３は、宇宙ロボットの関節構成例を示した図である。
【図１４】図１４は、床面に設置された多リンク構成の装置において、末端から基底の順序で情報を算出する通常の経路を示した図である。
【図１５】図１５は、床面に設置された多リンク構成の装置において、基底から末端に向かって速度情報を算出する通常の経路を示した図である。
【発明を実施するための形態】
【００１９】
以下、図面を参照しながら本発明の実施形態について詳細に説明する。
【００２０】
ロボットは、一般に、複数の剛体が連なって構成されるリンク構造物である。このうち倒立振子型のロボットや２足歩行型のロボット、宇宙ロボットなどは、関節の一部が力を発することのできない、非駆動関節としてモデル化することができる（前述並びに図１１〜図１３を参照のこと）。ロボットなどのリンク構造物において、例えば手先の位置に所望の速度を発生させるためには関節速度を得る必要がある。あるいは、所望の加速度を発生させるためには関節力を得る必要がある。駆動関節のみから構成される系では、関節速度のすべての成分、あるいは、関節加速度のすべての成分を直接に制御することができる。ところが、非駆動関節が含まれる系では、非駆動関節の成分を直接には制御できない、という問題がある。
【００２１】
ここで、ロボットの運動方程式は、一般に、下式（１）のように表現される。
【００２２】
【数１】
【００２３】
上式（１）において、ｑは関節空間、τは関節空間ｑに発生する力、ｂは重力又はコリオリ力、Ｈはロボット（リンク構造全体）の関節空間に対する慣性行列、ｆは外力、Ｊは外力ｆの作用する空間を表すヤコビアンである。以下では、ロボットの第ｉリンクとその親リンクを接合している関節を第ｉ関節とする。
【００２４】
上記の運動方程式（１）を、駆動関節成分と非駆動関節成分に分けて表現すると、下式（２）に示す通りとなる。
【００２５】
【数２】
【００２６】
上式（２）において、インデックスＡは駆動関節成分、インデックスＵは非駆動関節成分をそれぞれ表す。非駆動関節を有するロボットとは、式（２）において、非駆動関節ｑUの関節力が常にゼロ、すなわちτU＝０となるロボットを意味する。
【００２７】
非駆動関節を有するロボットを制御する場合の重要な概念として、一般化ヤコビアンがある（例えば、Ｙ．Ｕｍｅｔａｍｉ ａｎｄ Ｋ．Ｙｏｓｈｉｄａ，“Ｒｅｓｏｌｖｅｄ ｍｏｔｉｏｎ ｒａｔｅ ｃｏｎｔｒｏｌ ｏｆ ｓｐａｃｅ ｍａｎｉｐｕｌａｔｏｒｓ ｗｉｔｈ ｇｅｎｅｒａｌｉｚｅｄ Ｊａｃｏｂｉａｎｍａｔｒｉｘ”（Ｉｎ ＩＥＥＥ Ｔｒａｎｓａｃｔｉｏｎｓ ｏｎ Ｒｏｂｏｔｉｃｓ ａｎｄ Ａｕｔｏｍａｔｉｏｎ，５（３），Ｊｕｎｅ １９８９）を参照のこと）。一般化ヤコビアンＪGは、下式（３）で与えられる。
【００２８】
【数３】
【００２９】
力制御系のロボット制御において重要な概念として、ロボットに作用する力と発生する加速度の関係を記述する操作空間がある（前述）。例えば、ロボットの手先の位置を操作空間として定義して、手先に所望の加速度を発生させるための関節力を決定するのに操作空間が用いられる。通常、操作空間ｘの速度は、関節ｑの関節速度とヤコビアンＪを用いて、下式（４）のように表わされる。
【００３０】
【数４】
【００３１】
ゆえに、操作空間ｘに所望の速度を発生するための関節速度を、下式（５）から得ることができる。但し、同式において、Ｊ#は、ヤコビアンＪの擬似逆行列である。
【００３２】
【数５】
【００３３】
上式（５）に示す関係をＮｅｗｔｏｎ−Ｒａｐｈｓｏｎ法などの反復法とともに用いて、いわゆる逆運動学（Ｉｎｖｅｒｓｅ Ｋｉｎｅｍａｔｉｃｓ）演算も構成することができる。駆動関節のみから構成される系では、関節空間ｑのすべての速度成分を制御することができるので、上記の関係式（５）が有効である。これに対し、非駆動関節が含まれる系では、関節空間ｑの速度成分のうち非駆動関節成分は制御できないため、関係式（５）を適用することができない。しかしながら、系の全運動量がゼロに保存されているという前提では、以下の関係式（６）が成立する。
【００３４】
【数６】
【００３５】
上式（６）を用いれば、運動量がゼロに保たれている宇宙ロボットなどの逆運動学を構成することができる。しかし、この関係式（６）は、一般化ヤコビアンＪGの定義式（３）をそのまま用いると、慣性行列Ｈを求める必要がある。関節数がＮの系において慣性逆行列ＨのサイズはＮ×Ｎであり、慣性行列Ｈを求めるには計算量が膨大である。また、関係式（６）は、全運動量が保存されないケースでは成立しないうえ、速度レベルの制御手段しか提供しない。すなわち、この関係式（６）に基づいて、運動量が保存されないケースや、非駆動関節を有する系の加速度オーダーの精密な制御を実現するのは難しい。
【００３６】
また、非駆動関節を含むＦｌｏａｔｉｎｇＢａｓｅ の系の操作空間に発生する力を制御する方法も提案されている（例えば、Ｌｕｉｓ Ｓｅｎｔｉｓ ａｎｄ Ｏｕｓｓａｍａ Ｋｈａｔｉｂ，“Ｃｏｎｔｒｏｌ ｏｆ Ｆｒｅｅ−Ｆｌｏａｔｉｎｇ Ｈｕｍａｎｏｉｄ Ｒｏｂｏｔｓ Ｔｈｒｏｕｇｈ Ｔａｓｋ Ｐｒｉｏｒｉｔｉｚａｔｉｏｎ”（Ｐｒｏｃｅｅｄｉｎｇｓ ｏｆ ｔｈｅ ＩＥＥＥ Ｉｎｔｅｒｎａｔｉｏｎａｌ Ｃｏｎｆｅｒｅｎｃｅ ｉｎ Ｒｏｂｏｔｉｃｓ ａｎｄ Ａｕｔｏｍａｔｉｏｎ，Ｂａｒｃｅｌｏｎａ，Ｓｐａｉｎ，Ａｐｒｉｌ ２００５）を参照のこと）。しかしながら、上記の一般化ヤコビアンＪGやそのＮｕｌｌＳｐａｃｅ を求める必要があり、計算量が多い。
【００３７】
また、操作空間の加速度を精密に制御するための方法が幾つか提案されているが（前述、並びに、特許文献１、２を参照のこと）、いずれの方法もすべての関節が駆動関節である場合に対象が限定されている。
【００３８】
そこで、以下では、非駆動関節を含むロボットの加速度オーダーの精密な制御を、関節数Ｎに対してＯ（Ｎ）という少ない計算量で達成する方法について説明する。
【００３９】
内力による運動を考えるため、上記で示した運動方程式（２）から外力項を除いた系を考える。上式（２）の上段及び下段のそれぞれより、以下の２式（７−１）、（７−２）を得ることができる。
【００４０】
【数７】
【００４１】
上式（７−１）を上式（２）の下段に代入すると、以下の駆動関節空間ｑAに関する運動方程式（８）を得ることができる。
【００４２】
【数８】
【００４３】
同様に、上式（７−２）を上式（２）の上段に代入すると、以下の非駆動関節空間ｑUに関する運動方程式（９）も得ることができる。
【００４４】
【数９】
【００４５】
一方、上記の操作空間ｘの速度と関節空間ｑの関節速度の関係を表した式（４）を微分して、下式（１０）に示すように、操作空間ｘに発生する加速度を得ることができる。
【００４６】
【数１０】
【００４７】
上式（１０）に上式（７−１）を代入すると、下式（１１）に示すように、操作空間ｘに発生する加速度を駆動関節空間ｑAの関節加速度で表わすことができる。
【００４８】
【数１１】
【００４９】
上記の駆動関節空間ｑAに関する運動方程式（８）を用いて上式（１１）から駆動関節空間ｑAの加速度を消去すると、下式（１２）に示すように、操作空間ｘに発生する加速度を、駆動関節空間ｑA及び非駆動関節空間ｑUにそれぞれ発生させる関節力τA、τUで表現することができる。
【００５０】
【数１２】
【００５１】
但し、ＨGは、一般化慣性行列（Ｇｅｎｅｒａｌｉｚｅｄ Ｉｎｅｒｔｉａ Ｍａｔｒｉｘ）であり、下式（１３）のように表わされる。一般化慣性行列については、例えばＹ．Ｘｕ ａｎｄ Ｔ．Ｋａｎａｄｅ，“Ｓｐａｃｅ Ｒｏｂｏｔｉｃｓ：Ｄｙｎａｍｉｃｓ ａｎｄ Ｃｏｎｔｒｏｌ”（Ｐｒｅｎｔｉｃｅ Ｈａｌｌ，１９９２）を参照されたい。
【００５２】
【数１３】
【００５３】
操作空間ｘに対し、仮想的な外力ｆを考え、下式（１４）に示すように駆動関節空間ｑAの関節力τA及び非駆動関節空間ｑUの関節力τUからなる関節力τを発生するようにする（このようにｆからτを得る演算は、「逆力運動学（Ｉｎｖｅｒｓｅ Ｆｏｒｃｅ Ｋｉｎｅｍａｔｉｃｓ）」と呼ばれる）。
【００５４】
【数１４】
【００５５】
このとき、上式（１２）で示した操作空間ｘに発生する加速度は、下式（１５）に示すように、外力ｆで表わすことができる。
【００５６】
【数１５】
【００５７】
ここで、外力ｆの係数行列を、下式（１６）に示すように、Λ-1とおく。この係数行列Λ-1は、操作空間慣性逆行列（Ｏｐｅｒａｔｉｏｎａｌ Ｉｎｖｅｒｓｅ Ｉｎｅｒｔｉａ Ｍａｔｒｉｘ）として知られている。操作空間慣性逆行列Λ-1については、例えばＯ．Ｋｈａｔｉｂ，“Ａ Ｕｎｉｆｉｅｄ Ａｐｐｒｏａｃｈ ｔｏ Ｍｏｔｉｏｎ ａｎｄ Ｆｏｒｃｅ Ｃｏｎｔｒｏｌ ｏｆ Ｒｏｂｏｔ Ｍａｎｉｐｕｌａｔｏｒｓ：Ｔｈｅ Ｏｐｅｒａｔｉｏｎａｌ Ｓｐａｃｅ Ｆｏｒｍｕｌａｔｉｏｎ”（Ｉｎ ＩＥＥＥ Ｊ．ｏｎ Ｒｏｂｏｔｉｃｓ ａｎｄ Ａｕｔｏｍａｔｉｏｎ，ｖｏｌ．３，ｎｏ．１，１９８７，ｐｐ．４３−５３）を参照されたい。
【００５８】
【数１６】
【００５９】
上式（１６）中で、慣性行列Ｈの逆行列Ｈ-1は正定値対称行列であるから、ＪＨ-1ＪTすなわち操作空間慣性逆行列Λ-1も正定値対称行列である。正定値対称行列である操作空間慣性逆行列Λ-1は、数値計算上、線形相補正問題（Ｌｉｎｅａｒ Ｃｏｍｐｒｅｍｅｎｔａｒｙ Ｐｒｏｂｌｅｍ：ＬＣＰ）として解くことができる。ゆえに、操作空間ｘに発生する加速度の目標値を実現するための外力ｆを安定に求めることができる。
【００６０】
一方、制御対象のロボットに非駆動関節ｑUが存在すると、上式（１４）中で、非駆動関節ｑUの関節力τUを発生することができない。且つ、上式（１４）でτU＝０（非駆動関節ｑUの関節力が常にゼロ）としたのでは、操作空間ｘに発生する加速度は下式（１７）に示す通りとなり、外力ｆの係数行列ＪGＨG-1ＪATは正定値対称行列とはならない。すなわち、外力ｆの係数行列を数値計算で解くことが困難になり、操作空間ｘに発生する加速度の目標値を実現するための外力ｆを安定に求めることができなくなる。
【００６１】
【数１７】
【００６２】
ところが、仮想的な外力ｆにより発生する関節力τを、上式（１４）に代えて、下式（１８）のように表現すると、操作空間ｘに発生する加速度は下式（１９）のように表わされる。下式（１８）に示すように、一般化ヤコビアンＪGを用いて、外力ｆから駆動関節空間ｑAの関節力τAを求める演算を、以下では「一般化逆力運動学（Ｇｅｎｅｒａｌｉｚｅｄ Ｉｎｖｅｒｓｅ Ｆｏｒｃｅ Ｋｉｎｅｍａｔｉｃｓ）」と呼ぶことにする。
【００６３】
【数１８】
【００６４】
【数１９】
【００６５】
上式（１９）において、外力ｆの係数行列ＪGＨG-1ＪGTは半正定値対称行列となる。一般化ヤコビアンＪGがランク落ちすると、すべての固有値がゼロになり得るが、それ以外のケースでは正定値対称行列としての解法を用いることができる。
【００６６】
以上をまとめると、上式（１）で示したロボットの運動方程式は、操作空間ｘに関するダイナミクスを表現する下式（２０）に示すように変形することができる。
【００６７】
【数２０】
【００６８】
上式（２０）は、操作空間ｘに上式（１８）で定義される力ｆを作用したときの、ｆと操作空間ｘに発生する加速度との関係を表している。但し、上式（２０）で外力ｆの係数行列ΛG-1は、「一般化操作空間慣性逆行列（Ｇｅｎｅｒａｌｉｚｅｄ Ｏｐｅｒａｔｉｏｎａｌ Ｉｎｖｅｒｓｅ Ｉｎｅｒｔｉａ Ｍａｔｒｉｘ）」と呼ぶ。また、上式（２０）の右辺の定数項ｃGは、「一般化操作空間バイアス加速度（Ｇｅｎｅｒａｌｉｌｚｅｄ Ｏｐｅｒａｔｉｏｎａｌ Ｂｉａｓ Ａｃｃｅｌａｒａｔｉｏｎ）」と呼ぶ。一般化操作空間慣性逆行列ΛG-1並びに一般化操作空間バイアス加速度ｃGは、それぞれ下式（２１）、（２２）のように表わされる。
【００６９】
【数２１】
【００７０】
【数２２】
【００７１】
一般化操作空間慣性逆行列ΛG-1及び一般化操作空間バイアス加速度ｃGが求められれば、上式（２０）より、操作空間ｘに目標加速度を発生するための制御入力となる力ｆ（言い換えれば、上式（２０）を用いて力ｆから変換される、駆動関節空間ｑAの関節力τA）を決定することができる。
【００７２】
例えば、本出願人に既に譲渡されている特開２００７−１０８９５５号公報には、線形相補正問題（ＬＣＰ）ソルバーなどを用いて操作空間慣性逆行列及び操作空間バイアス加速度を高速で且つ低い計算負荷で算出する方法について開示されている。
【００７３】
しかしながら、一般化操作空間慣性逆行列ΛG-1及び一般化操作空間バイアス加速度ｃGをそれぞれ上記の定義式（２１）及び（２２）に従って求めると、計算量が多い。一般化慣性行列ＨGを得るには、まず慣性行列Ｈを得る必要がある。関節数がＮの系において慣性行列ＨのサイズはＮ×Ｎであり、関節数Ｎに対しＯ（Ｎ2）の計算コストを要する。よって、インバージョン・コストを考えると、一般化慣性行列ＨGを得るには、関節数Ｎに対しＯ（Ｎ3）の計算コストがかかることになり、関節数が増加するにつれ急速に計算量が増大する。
【００７４】
より少ない計算量で一般化操作空間慣性逆行列ΛG-1及び一般化操作空間バイアス加速度ｃGを求める方法について、以下で考えてみる。上式（２０）で、右辺から左辺を得る計算は、外力や重力、速度積に関連する力（コリオリ力など）が作用したときの操作空間ｘにおける加速度を求める問題と見ることができる。操作空間ｘにおける加速度を求める点が通常の順動力学計算と異なるが、一種の順動力学演算と捉えることができる。順動力学演算ＦＤGは、リンク構造物の力学モデルとしての関節空間ｑ並びにその関節速度、重力ｇ、並びに外力ｆをパラメータに持ち、下式（２３）のように表現することができる。但し、下式（２３）において、ｇは重力加速度である。
【００７５】
【数２３】
【００７６】
この順動力学演算ＦＤGによれば、関節空間ｑと、重力ｇ、外力ｆといったリンク構造物に作用する力情報から、リンク構造物の各点に発生する加速度を得ることができる。
【００７７】
ここで、上式（２３）において、順動力学演算ＦＤGの入力パラメータのうち関節空間ｑ及び外力ｆ以外をすべてゼロとする制約条件下では、重力、関節力、速度積に関連する力（コリオリ力など）が発生しない状況において操作空間ｘに発生する加速度を求めることができる。すなわち、式（２２）において一般化操作空間バイアス加速度ｃG＝０とすることができる。さらに、ｆ＝ｅi、すなわち、ｉ番目の操作空間にのみ第ｉ成分が１の単位ベクトルｅiを作用させる条件下で上式（２３）の計算を実行すると、一般化操作空間慣性逆行列ΛG-1の第ｉ列を求めることができることが分る。よって、下式（２４）の演算をすべての行ｉについて実行すれば、操作空間慣性逆行ΛG-1全体を得ることができる。
【００７８】
【数２４】
【００７９】
また、外力ｆ＝０とし、順動力学演算ＦＤGの入力パラメータのうち関節空間ｑに発生する速度と重力ｇのみが作用するという制約条件下で上式（２４）の順動力学演算ＦＤGを実行することで、下式（２５）に示すように一般化操作空間バイアス加速度ｃGを算出することができる。
【００８０】
【数２５】
【００８１】
順動力学演算は、当初は逆動力学演算（加速度から力を求める演算）を利用して構成する方法が一般的であったが、Ｏ（Ｎ3）の計算量を有し、自由度数が増すと演算量が嵩むという問題がある。これに対し、特開２００７−１０８９５５号公報（前述）やＲ．Ｆｅａｔｈｅｒｓｔｏｎｅ，“Ｒｏｂｏｔ Ｄｙｎａｍｉｃｓ Ａｌｇｏｒｉｔｈｍｓ”（Ｋｌｕｗｅｒ Ａｃａｄｅｍｉｃ Ｐｕｂｌｉｓｈｅｒｓ，１９８７）には、ＡｒｔｉｃｕｌａｔｅｄＢｏｄｙ法（以下、「ＡＢ法」とする）を用いてＯ（Ｎ）の計算複雑度で順動力学演算を構成する方法について記載されている。なお、ＡＢ法自体については、例えば、“Ｔｈｅ
ｃａｌｃｕｌａｔｉｏｎ ｏｆ ｒｏｂｏｔ ｄｙｎａｍｉｃｓｕｓｉｎｇ ａｒｔｉｃｕｌａｔｅｄ−ｂｏｄｙ ｉｎｅｒｔｉａｓ”（Ｉｎｔ．Ｊ．ＲｏｂｏｔｉｃｓＲｅｓｅａｒｃｈ，ｖｏｌ．２，ｎｏ．１，ｐｐ．１３−３０，１９８３）に記載されている。特開２００７−１０８９５５号公報に記載の順動力学演算の構成方法では、ＡＢ法をイナーシャ情報算出、速度情報算出、力情報算出、加速度情報算出の４つの過程に分解する。
【００８２】
上記の順動力学演算式（２３）を、ＡＢ法と同様にＯ（Ｎ）の計算複雑度で実現することができれば、上式（２４）及び（２５）より、一般化操作空間慣性逆行列ΛG-1及び一般化操作空間バイアス加速度ｃGについても同様の計算複雑度で算出できることになる。
【００８３】
上記の順動力学演算式（２３）が通常のＡＢ法のプロセスと異なる点は、外力ｆによって発生する関節力τが、上式（１４）ではなく上式（１８）によって発生する点である。上記の定義式（３）に従って一般化ヤコビアンＪGを算出すると、慣性行列Ｈの算出を経由しなければならず、Ｏ（Ｎ2）の計算量があるため効率がよくない。
【００８４】
上記の運動方程式（２）を別な形式で書き直すと、下式（２６）に示す通りとなる。
【００８５】
【数２６】
【００８６】
すなわち、上式（２６）の左辺から右辺を得る演算を、下式（２７）のように表記することができる。
【００８７】
【数２７】
【００８８】
上式（２７）に示す演算は、加速度から作用する力を求める逆動力学と、力が作用したときに発生する加速度を求める順動力学の混合演算であり、「ハイブリッド動力学（Ｈｙｂｒｉｄ Ｄｙｎａｍｉｃｓ：ＨＤ）」と呼ばれる。ハイブリッド動力学については、例えばＲ．Ｆｅａｔｈｅｒｓｔｏｎｅ，“Ｒｏｂｏｔ Ｄｙｎａｍｉｃｓ Ａｌｇｏｒｉｔｈｍｓ”（Ｋｌｕｗｅｒ Ａｃａｄｅｍｉｃ Ｐｕｂｌｉｓｈｅｒｓ，１９８７）（前述）を参照されたい。特に、「１．非駆動関節ｑUには関節力が発生しない（非駆動関節ｑUの関節力τUは既知である）」、「２．駆動関節ｑAは動かない（駆動関節ｑAの加速度は既知である）」、「３．関節ｑの速度はすべてゼロ、重力も作用しない」、すなわち下式（２８−１）で表わされる補助モデルを考えると、下式（２８−２）が成立することが分かる。
【００８９】
【数２８】
【００９０】
上式（２８−２）は、上式（２８−１）で表わされる補助モデルのハイブリッド動力学演算の結果として得られる駆動関節力τAを符号反転したものは、上式（１８）すなわち一般化逆力運動学の結果得られる駆動関節力τAに一致することを意味している。外力ｆに対して、このようにして得られた駆動関節力τAを作用してから、特開２００７−１０８９５５号公報（前述）に記載の順動力学演算を実行することにより、一般化ヤコビアンＪGで定義される操作空間ｘに関する上記の順動力学演算式（２３）を実現することができる。特に、上記のハイブリッド動力学演算は、ＡＢ法を変形した、イナーシャ情報算出、速度情報算出、力情報算出、加速度情報算出の４つの過程に分解して、それぞれ以下に述べる演算によって、Ｏ（Ｎ）の計算量で実行することができる。したがって、一般化ヤコビアンＪGで定義される操作空間ｘに関する順動力学演算式（２３）全体も、Ｏ（Ｎ）の計算量で達成することが可能である。
【００９１】
イナーシャ情報算出：
イナーシャ情報は、すべてのリンクのＩAi（Ａｒｔｉｃｕｌａｔｅｄ Ｂｏｄｙ Ｉｎｅｒｔｉａ）を、下式（２９）をロボット装置の末端から基底に向かって実行することで求める。図１４には、床面に設置された多リンク構成の装置において、末端から基底の順序で情報を算出する通常の経路（Ｂａｃｋｗｏｒｄ Ｐｒｏｐａｇａｔｉｏｎ）を示している。
【００９２】
【数２９】
【００９３】
上式（２９）において、Ｉiはリンクｉのイナーシャ、ＣF（ｉ）はリンクｉの関節力既知の子リンクのインデックス集合、ＣA（ｉ）はリンクｉの加速度既知の子リンクのインデックス集合とする。また、Ｓiはリンクｉの関節の運動自由度を表す行列である。
【００９４】
速度情報算出：
速度情報は、すべてのリンクの原点の速度を、下式（３０）をロボット装置の基底から末端に向かって実行することで求める。図１５には、床面に設置された多リンク構成の装置において、基底から末端に向かって速度情報を算出する通常の経路（Ｆｏｒｗａｒｄ Ｐｒｏｐａｇａｔｉｏｎ）を示している。
【００９５】
【数３０】
【００９６】
上式（３０）において、ｖiはリンクｉの速度、ｐはその親リンクのインデックスとする。また、ｑiはリンクｉの関節空間である。
【００９７】
力情報算出：
力情報は、すべてのリンクのバイアス力ｐAiを、下式（３１）〜（３２）をロボット装置末端から基底に向かって実行することで求める（図１４を参照のこと）。
【００９８】
【数３１】
【００９９】
【数３２】
【０１００】
上式（３１）において、ｆkはリンクｉの中でｋ番目の外力、Ｆ（ｉ）はリンクｉに作用する外力のインデックス集合、τiはリンクｉの関節力である。
【０１０１】
加速度情報算出：
加速度情報は、すべてのリンクの加速度ａiを、下式（３３）〜（３５）を基底から末端に向かって実行することで求める（図１５を参照のこと）。リンクｉの関節が非駆動関節のときは下式（３３）、（３４）を実行し、リンクｉの関節が駆動関節のときは下式（３５）を実行する。
【０１０２】
【数３３】
【０１０３】
【数３４】
【０１０４】
【数３５】
【０１０５】
なお、上記の計算で、速度情報算出の起点となる基底の速度はｖ0＝０である。また、重力の効果は、加速度情報算出において、基底が重力加速度で上昇している、として考慮する。
【０１０６】
【数３６】
【０１０７】
以上の計算を、イナーシャ情報算出⇒速度情報算出⇒力情報算出⇒加速度情報算出の順で実行することにより、ハイブリッド動力学演算を効率的に実行することができる。ひいては、一般化操作空間慣性逆行列ΛG-1及び一般化操作空間バイアス加速度ｃGを効率的に算出することが可能となり、非駆動関節を有したロボットであっても、リンク構造の任意の箇所において所望の加速度を発生させるための関節力を決定することができ、リンク構造物の任意の部位において精密な加速度操作が可能となる。
【０１０８】
操作空間をすべてまとめた変数としてｘ を考えると、すべての拘束は以下のような不等式を含む線形拘束条件式（３７）〜（３８）として記述することができる。
【０１０９】
【数３７】
【０１１０】
【数３８】
【０１１１】
ここで、ｗは一種のスラック変数である．また，上式（３８）中でそれぞれ存在するＬi及びＵiは、ｉ番目の操作空間に作用する仮想的な力ｆiの下限と上限をそれぞれ表す。
【０１１２】
上式（３７）〜（３８）は、線形相補性問題である。そして、一般化操作空間慣性逆行列ΛG-1及び一般化操作空間バイアス加速度ｃGが既知であれば、この線形相補性問題を解くことによって、線形拘束条件を満足する操作空間ｘに目標加速度を発生させるための仮想的な外力ｆを決定することができる。線形相補性問題の数学的な解法自体については、例えば“Ｆａｓｔ Ｃｏｎｔａｃｔ Ｆｏｒｃｅ Ｃｏｍｐｕｔａｔｉｏｎ ｆｏｒ Ｎｏｎｐｅｎｅｔｒａｔｉｎｇ Ｒｉｇｉｄ Ｂｏｄｉｅｓ”（ＳＩＧＧＲＡＰＨ９４，ｐｐ．２３−３４，１９９４）に記載されているので、本明細書では説明を省略する。
【０１１３】
線形相補性問題として操作空間ｘの目標加速度を発生させるための力ｆを求める場合の多くは、一般化操作空間慣性逆行列ΛG-1を得る部分において最も計算を要する。上述したように、本実施形態によれば、一般化操作空間慣性逆行列ΛG-1の算出を高速に行なうことができるので、ロボットの実機上でも実時間の処理が可能である。こうして力ｆが得られれば、拘束条件式（３７）〜（３８）を満足する関節力τ
を上式（１８）に基づいて決定することができる。
【０１１４】
ここまでをまとめると、非駆動関節を有したロボット（Ｕｎｄｅｒ Ａｃｔｕａｔｅｄ Ｓｙｓｔｅｍ）を、図１に示すように、（１）すべての関節が可動関節（Ｍｏｖａｂｌｅ Ｊｏｉｎｔ）となるロボットの主モデル（Ｆｕｌｌｙ Ｍｏｖａｂｌｅ Ｓｙｓｔｅｍ）と、（２）駆動関節（Ａｃｔｕａｔｅｄ Ｊｏｉｎｔ）を不動関節（Ｉｍｍｏｖａｂｌｅ Ｊｏｉｎｔ）とし、非駆動関節（Ｕｎａｃｔｕａｔｅｄ Ｊｏｉｎｔ）を可動関節とした補助モデル（Ｐａｒｔｉａｌｌｙ Ｒｉｇｉｄ Ｓｙｓｔｅｍ）という２つのモデルの組み合わせによって表現することができる。そして、前者の主モデルでは通常の順動力学演算（上式（２３）を参照のこと）を実行し、後者の補助モデルではハイブリッド動力学演算（上式（２８−２）を参照のこと）を実行することで、系全体の操作空間物理量を効率的に求めて制御する。
【０１１５】
図２には、主モデル及び補助モデルを用いた、非駆動関節を有するロボットの制御システム構成例を模式的に示している。
【０１１６】
図示の制御システム１００は、主モデル（Ｍａｉｎ Ｍｏｄｅｌ：Ｆｕｌｌｙ Ａｃｔｕａｔｅｄ Ｓｙｓｔｅｍ Ｍｏｄｅｌ）１０１と、補助モデル（Ａｕｘｉｌｉａｒｙ Ｍｏｄｅｌ：Ｐａｒｔｉａｌｌｙ Ｒｉｇｉｄ Ｍｏｄｅｌ）１０２と、順動力学演算部（Ｆｏｒｗａｒｄ Ｄｙｎａｍｉｃｓ Ｅｘｅｃｕｔｏｒ）１０３と、ハイブリッド動力学演算部（Ｈｙｂｒｉｄ Ｄｙｎａｍｉｃｓ Ｅｘｅｃｕｔｏｒ）１０４と、一般化逆力運動学演算部（Ｇｅｎｅｒａｌｉｚｅｄ Ｉｎｖｅｒｓｅ Ｆｏｒｃｅ Ｋｉｎｅｍａｔｉｃｓ Ｅｘｅｃｕｔｏｒ）１０５と、一般化操作空間慣性逆行列演算部（Ｇｅｎｅｒａｌｉｚｅｄ Ｏｐｅｒａｔｉｏｎａｌ Ｉｎｖｅｒｓｅ Ｉｎｅｒｔｉａ Ｃａｌｃｕｌａｔｏｒ）１０６と、一般化操作空間バイアス加速度演算部（Ｇｅｎｅｒａｌｉｚｅｄ Ｏｐｅｒａｔｉｏｎａｌ Ｂｉａｓ Ａｃｃｅｌｅｒａｔｉｏｎ Ｃａｌｃｕｌａｔｏｒ）１０７と、仮想外力算出部（Ｌｉｎｅａｒ Ｃｏｍｐｌｅｍｅｎｔａｒｙ Ｐｒｏｂｌｅｍ Ｓｏｌｖｅｒ）１０８と、全身協調力制御部（Ｗｈｏｌｅ−ｂｏｄｙ Ｃｏｏｅｐｅｒａｔｅｄ Ｆｏｒｃｅ Ｃｏｎｔｒｏｌｌｅｒ）１０９と、関節力制御部（Ｊｏｉｎｔ Ｆｏｒｃｅ Ｃｏｎｔｒｏｌｌｅｒ）１１０で構成される。
【０１１７】
主モデル１０１は、すべての関節を可動関節（Ｍｏｖａｂｌｅ Ｊｏｉｎｔ）としたロボットの動力学モデルである。一方、補助モデル１０２は、駆動関節を不動関節（Ｉｍｍｏｖａｂｌｅ Ｊｏｉｎｔ）とし、非駆動関節を可動関節としたロボットの動力学モデルである。補助モデル１０２では、「１．非駆動関節ｑUには関節力が発生しない（非駆動関節ｑUの関節力τUは既知である）」、「２．駆動関節ｑAは動かない（駆動関節ｑAの加速度は既知である）」、「３．関節ｑの速度はすべてゼロ、重力も作用しない」（前述）。図１中の中央に示したモデルが主モデル１０１に相当し、図１中の右に示したモデルが補助モデル１０２に相当する。
【０１１８】
ハイブリッド動力学演算部１０４は、補助モデル１０２を用いて、上記の逆動力学と順動力学の混合演算式（２７）を実行して、関節加速度が既知の関節に作用する関節力と、関節力が既知の関節に発生する関節加速度をそれぞれ算出することができる。
【０１１９】
補助モデル１０２は、駆動関節に相当する不動関節と、非駆動関節に相当する可動関節が混在する系である。上式（２８−１）で示したように、不動関節として扱われる駆動関節は、動かないという意味で関節加速度が既知であり、可動関節として扱われる非駆動関節は、関節力が発生しないという意味で関節力が既知である。補助モデル１０２を、関節加速度が既知である不動関節と、関節力が既知である可動関節が混在する系、と言い換えることができる。
【０１２０】
一般化逆力運動学演算部１０５は、上式（２８−１）に示した条件をハイブリッド動力学演算部１０４に与えて上記の混合演算式（２７）を実行させることで、補助モデル１０２上の不動関節すなわち駆動関節ｑAに作用する関節力τAと、可動関節すなわち非駆動関節ｑUに発生する関節加速度をそれぞれ算出する。また、一般化逆力運動学演算部１０５は、一般化逆力運動学演算を実行して（式（１８）を参照のこと）、関節力τAからｆを求める。
【０１２１】
順動力学演算部１０３は、主モデル１０１を用いて、上記の操作空間ｘに関する順動力学演算式（２３）を実行して、操作空間ｘに発生する加速度を求める。ここで、外力ｆは、上式（１４）により関節力に変換するのではなく、上式（１８）によって関節力τAに変換する点で、特開２００７−１０８９５５号公報（前述）に開示された操作空間物理量算出装置とは異なる点に留意されたい。このため、順動力学演算部１０３は、一般化逆力運動学演算部１０５に関節空間ｑ及び外力ｆを与えて（ＩＦＫG（ｑ，ｆ））、一般化逆力運動学演算部１０５が求めた駆動関節の関節力τA（＝ＪGTｆ）を用いて順動力学演算式（２３）を実行する。
【０１２２】
一般化操作空間慣性逆行列演算部１０６は、上式（２４）の条件に基づいて順動力学演算ＦＤGを繰り返し実行して、上式（２１）に示した一般化操作空間慣性逆行列ΛG-1を算出する。具体的には、一般化操作空間慣性逆行列演算部１０６は、順動力学演算部１０３に対する入力パラメータのうち関節空間ｑ及び外力ｆ以外をすべてゼロとする制約条件下で、さらに、ｆ＝ｅi、すなわち、ｉ番目の操作空間にのみ単位力を作用させる条件下で、順動力学演算部１０３に上式（２３）の計算を繰り返し実行させることによって、一般化操作空間慣性逆行列ΛG-1のすべての列の成分を求める。
【０１２３】
一般化操作空間バイアス加速度演算部１０７は、上式（２５）の条件に基づいて順動力学演算ＦＤGを１回だけ実行して、一般化操作空間バイアス加速度ｃGを算出する。具体的には、一般化操作空間バイアス加速度演算部１０７は、順動力学演算部１０３に対する入力パラメータのうち関節空間ｑに発生する速度と重力ｇのみが作用するという制約条件下で、順動力学演算部１０３に上式（２３）の計算を１回だけ実行させることによって、一般化操作空間バイアス加速度ｃGを算出させる。
【０１２４】
仮想外力算出部１０８は、一般化操作空間慣性逆行列ΛG-1と一般化操作空間バイアス加速度ｃGを用いて表される操作空間ｘに作用する力と加速度の関係式（２０）において、操作空間ｘに発生する目標加速度を既知とし、それを達成するために操作空間ｘに発する仮想的な外力ｆを解く。ここで、既知の目標加速度は、外部（インピーダンス・コントローラーなど）から仮想外力算出部１０８に入力される制御指令値である。例えば、仮想外力算出部１０８は、上記関係式（２０）と、ロボットのリンク同士及びリンクと環境との間で形成される不等式拘束条件からなる線形相補性問題（ＬＣＰ）として捉え、ＬＣＰソルバーを用いて、操作空間ｘに発生する加速度の目標値を実現するための外力ｆを安定に求めることができる（前述）。
【０１２５】
全身協調力制御部１０９は、仮想外力算出部１０８から得られた仮想的な外力ｆを、上式（１８）で示した一般化逆力運動学演算を用いて駆動関節の駆動関節力τAに変換し、これを関節力制御部１１０の制御目標値として出力する。
【０１２６】
関節力制御部１１０は、全身協調力制御部１０９が作成した各関節の制御目標値を実現するように、ロボットの各駆動関節の関節力を制御する。精密に関節力を制御するには、例えば、本出願人に既に譲渡されている特開２００９−２６９１０２号公報に開示されている、力制御を行なうアクチュエータ制御装置を適用することができる。また、同アクチュエータ制御装置には、アクチュエータに印加されるトルクを正確に計測するために、本出願人に既に譲渡されている特開２００９−２８８１９８号公報に開示されているトルク計測装置を適用してもよい。
【０１２７】
図３には、図２に示したロボットの制御システムにおいて実行される処理手順をフローチャートの形式で示している。
【０１２８】
まず、上式（３）に示した一般化ヤコビアンＪGに対応する操作空間ｘを定義する（ステップＳ３１）。すなわち、ロボットの任意の部位に発生する加速度を制御する運動空間を定義する。例えば、ロボットの手先の位置を制御する場合、手先リンクと手先リンク座標系から見た手先の位置を指定する。
【０１２９】
次いで、操作空間ｘに発生させる目標加速度を設定する（ステップＳ３２）。すなわち。操作空間ｘのダイナミクスを示した上式（２０）の左辺を与える。操作空間ｘの加速度目標値が指定できれば、速度及び位置も指定できる。例えば、操作空間ｘの速度を制御したい場合は下式（３９−１）に示すように速度目標値から加速度目標値を指定すればよいし、操作空間ｘの位置を制御したい場合は下式（３９−２）に示すように位置目標値から加速度目標値を指定すればよい。但し、Ｋv、Ｋpはそれぞれ速度ゲイン、位置ゲインである。
【０１３０】
【数３９】
【０１３１】
次いで、一般化操作空間慣性逆行列演算部１０６により、一般化操作空間慣性逆行列ΛG-1（式（２１）を参照のこと）を算出する（ステップＳ３３）。
【０１３２】
次いで、一般化操作空間バイアス加速度演算部１０７により、一般化操作空間バイアス加速度ｃGを算出する（ステップＳ３４）。
【０１３３】
次いで、仮想外力算出部１０８は、先行するステップＳ３３、Ｓ３４で得られた一般化操作空間慣性逆行列ΛG-1と一般化操作空間バイアス加速度ｃGを用いて、上式（２０）から、ステップＳ３２で設定した目標加速度を達成するために操作空間ｘに発する仮想的な外力ｆを解く（ステップＳ３５）。本実施形態では、仮想外力算出部１０８は、上記関係式（３７）と、ロボットのリンク同士及びリンクと環境との間で形成される不等式拘束条件式（３８）からなる線形相補性問題を、ＬＣＰソルバーを用いて解いて、拘束条件式（３７）〜（３８）を満足する満足する仮想外力ｆを求める。
【０１３４】
次いで、全身協調力制御部１０９は、先行するステップＳ３５で得られた仮想外力ｆを、上式（１８）で示した一般化逆力運動学演算を用いて駆動関節の駆動関節力τAに変換する（ステップＳ３６）。
【０１３５】
そして、関節力制御部１１０は、先行するステップＳ３６で得られた関節力τAを各関節の制御目標値とし、これを実現するように、ロボットの各駆動関節の関節力を制御する（ステップＳ３７）。
【０１３６】
図２に示したロボットの制御システム１００では、図３に示した処理手順を、例えば１ＫＨｚの制御周波数で実行する。
【０１３７】
図４には、図３に示したフローチャート中のステップＳ３３において、一般化操作空間慣性逆行列演算部１０６により一般化操作空間慣性逆行列ΛG-1を算出するための処理手順をフローチャートの形式で示している。
【０１３８】
まず、一般化操作空間慣性逆行列演算部１０６は、列インデクスｉを初期値０に設定する（ステップＳ４１）。
【０１３９】
次いで、一般化逆力運動学演算部１０５は、関節空間ｑ及び外力ｆ以外をすべてゼロとする制約条件下で、さらに、ｆ＝ｅi、すなわち、ｉ番目の操作空間にのみ単位力を作用させる条件下で、駆動関節空間ｑAに発生する等価関節力τAを求める（ステップＳ４２）。
【０１４０】
次いで、順動力学演算部１０３は、一般化逆力運動学演算部１０５が求めた駆動関節の関節力τA（＝ＪGTｆ）を用いて順動力学演算式（２３）を実行して、すべての操作空間に発生する加速度を求める（ステップＳ４３）。すなわち、順動力学演算部１０３は、上式（２４）を実行する。
【０１４１】
次いで、一般化操作空間慣性逆行列演算部１０６は、このようにして得られた加速度を、一般化操作空間慣性逆行列ΛG-1の第ｉ列に代入する（ステップＳ４４）。
【０１４２】
そして、一般化操作空間慣性逆行列演算部１０６は、列インデクスｉを１だけ増分し（ステップＳ４５）、ｉが操作空間数に到達するまで（ステップＳ４６のＮｏ）、ステップＳ４２〜Ｓ４５の処理ルーチンを繰り返し実行する。このようにして、一般化操作空間慣性逆行列演算部１０６は、一般化操作空間慣性逆行列ΛG-1全体を得ることができる。
【０１４３】
図５には、図３に示したフローチャート中のステップＳ３４において、一般化操作空間バイアス加速度演算部１０７により一般化操作空間バイアス加速度ｃGを算出するための処理手順をフローチャートの形式で示している。
【０１４４】
一般化操作空間バイアス加速度演算部１０７は、関節空間ｑに発生する速度と重力ｇのみが作用するという制約条件下で上式（２４）の順動力学演算ＦＤGを１回だけ実行して、上式（２２）に示した一般化操作空間バイアス加速度ｃGを上式（２５）に示すように算出する（ステップＳ５１）。
【０１４５】
そして、一般化操作空間バイアス加速度演算部１０７は、求めた一般化操作空間バイアス加速度ｃGを、操作空間ｘのダイナミクスを示した上式（２０）に代入する（ステップＳ５２）。
【０１４６】
以下では、図１〜図３に示した制御系を、図６Ａに示す倒立振子型ロボットに適用した例について説明する。図示のロボットは、肩３自由度、肘２自由度、手首２自由度、及びグリッパー１自由度を有する左右の腕部を備えている。また、同ロボットは、頭部には２自由度、腰部に１自由度、対向２輪の車輪を有している。同ロボットは、車輪以外の接地点を持たない。
【０１４７】
図６Ａに示したロボットの各関節に、例えば特開２００９−２８８１９８号公報に開示されているトルク計測装置を搭載して、精密にトルク制御することができる。ロボットの体中に分散して配置された複数のサテライトＣＰＵ（Ｃｅｎｔｒａｌ Ｐｒｏｃｅｓｓｉｎｇ Ｕｎｉｔ）でそれぞれトルク制御が実行され、各サテライトＣＰＵはリアルタイムＬＡＮ（Ｌｏｃａｌ Ａｒｅａ Ｎｅｔｗｏｒｋ）を介して中央のＣＰＵと通信するものとする。また、ロボットの機体にはＩＭＵ（Ｉｎｅｒｔｉａｌ Ｍｅａｓｕｒｅｍｅｎｔ Ｕｎｉｔ）が搭載され、カルマン（Ｋａｌｍａｎ）フィルタを用いた姿勢演算が行なわれるものとする。この演算値も、リアルタイムＬＡＮを介して中央のＣＰＵに転送される。
【０１４８】
本出願人に既に譲渡されている特開２０１０−１８８４７１号公報には、図６Ａに示したものと同様の対向２輪型移動ロボットを、枝分かれのあるマニピュレーターとして表現した対向２輪等価モデルについて開示されている。図６Ｂに示すように、対向２輪等価関節に続いて非駆動関節を１つ挿入して表現することで、同ロボット全体を倒立振子型の２輪ロボットとしてモデル化する。図６Ｃには、図６Ｂに示した倒立振子型ロボットのすべての関節を可動とした主モデルを示している。また、図６Ｄには、図６Ｂに示した倒立振子型ロボットの駆動関節を不動とするとともに非駆動関節を可動とした補助モデルを示している。
【０１４９】
図７には、図６に示したロボットのバランスを保ちながら、機体の所定部位の位置や姿勢（方位）を一定に保つための制御系の構成例を示している。図示のように、制御ユニット７００には、さまざまな部位に対する加速度目標値が入力される。制御ユニット７００は、図３〜図５に示した処理手順を実行して、各部位に対する加速度目標値を実現するための、駆動関節空間ｑAの関節力τAを得る。そして、制御ユニット７００は、この関節力τAを用いてロボットの各駆動関節をトルク制御することで、すべての部位の運動目的を同時に満たすことができる。
【０１５０】
図６に示したロボットのバランス維持のためには、Ｓｌｉｄｉｎｇ Ｍｏｄｅ制御系（Ｓｌｉｄｉｎｇ Ｍｏｄｅ Ｃｏｎｔｒｏｌｌｅｒ）７０１が別途構成される。制御ユニット７０は、Ｓｌｉｄｉｎｇ Ｍｏｄｅ制御系の出力として、前後方向ｘ1の加速度目標値を得る。
【０１５１】
また、左右のグリッパーの位置及び姿勢をグローバル座標内で一定に保つ制御系（単純なＰＤ制御）として、Ｌｅｆｔ Ｈａｎｄ Ｐｏｓｉｔｉｏｎ Ｃｏｎｔｒｏｌｌｅｒ７０２、Ｌｅｆｔ Ｈａｎｄ Ｐｏｓｔｕｒｅ Ｃｏｎｔｒｏｌｌｅｒ７０３、Ｒｉｇｈｔ Ｈａｎｄ Ｐｏｓｉｔｉｏｎ Ｃｏｎｔｒｏｌｌｅｒ７０４、Ｒｉｇｈｔ Ｈａｎｄ Ｐｏｓｔｕｒｅ Ｃｏｎｔｒｏｌｌｅｒ７０５が構成され、制御ユニット７０は、各制御系からそれぞれ並進ｘｙｚ方向ｘ2,3,4、ｘ8,9,10の加速度目標値と、回転ｘｙｚ方向ｘ5,6,7、ｘ11,12,13の加速度目標値を得る。頭部の姿勢とベースの方位についても、Ｈｅａｄ Ｐｏｓｔｕｒｅ Ｃｏｎｔｒｏｌｌｅｒ７０６とＢａｓｅ Ｏｒｉｅｎｔａｔｉｏｎ Ｃｏｎｔｒｏｌｌｅｒ７０７が構成され、制御ユニット７００は、各制御系から頭部の回転ｘｙｚ方向ｘ14,15,16及びベースの方位ｘ17の加速度目標値を得る。
【０１５２】
制御ユニット７００は、各制御系からそれぞれの加速度目標値を入力すると、図３〜図５に示した処理手順を実行して、各部位に対する加速度目標値を実現するための、駆動関節空間ｑAの関節力τAを得る。そして、制御ユニット７００は、この関節力τAを用いてロボットの各駆動関節をトルク制御することで、すべての部位の運動目的を同時に満たすことができる。
【０１５３】
図８Ａ〜図８Ｃには、ワインが注がれたグラスを左手のグリッパーで把持したロボットが、前後に押されたり左右にねじられたりして外力が印加されても、グラスのワインをこぼさずに保持している様子を示している。
【０１５４】
図９には、ロボットが後方に押された際の車輪の前後位置と左手先位置をそれぞれ示している。また、図１０には、ロボットが後方に押された際のロボットの重心の位相平面上での変化（位置及び速度の変化）を示している。これらの図から、ロボットが、外力が印加されても、車輪位置を前後させながらも左手先位置を一定に保っている、すなわちバランスを保ちながら手先の位置及び姿勢を維持していることが分かる。
【０１５５】
なお、上式（１８）に示した一般化逆力運動学によって、一般化ヤコビアンＪGを用いて力ｆから関節力τAが求められることは、既に述べた通りである。一般化逆力運動学の別の応用として、上式（６）を用いて逆運動学演算を実行する際に、一般化ヤコビアンＪGを高速算出するのにも用いることができる。具体的には、上記の一般化逆力運動学演算式（１８）を下式（４０）のように表記する。
【０１５６】
【数４０】
【０１５７】
そして、ハイブリッド動力学演算部１０４は、関節空間ｑ及び外力ｆ以外をすべてゼロとし、ｉ番目の操作空間にのみ第ｉ成分が１の単位ベクトルｅiからなる外力を作用させる。
【０１５８】
【数４１】
【０１５９】
そして、このような外力ｆの作用条件下で、駆動関節空間の関節力を算出するハイブリッド動力学演算を操作空間数にわたり繰り返し実行させることによって、一般化ヤコビアンＪG全体を求めることができる。上式（４０）をＯ（Ｎ）の計算量で求めることができることから、一般化ヤコビアンＪGもＯ（Ｎ）の計算量で求めることができる。
【産業上の利用可能性】
【０１６０】
以上、特定の実施形態を参照しながら、本発明について詳細に説明してきた。しかしながら、本発明の要旨を逸脱しない範囲で当業者が該実施形態の修正や代用を成し得ることは自明である。
【０１６１】
本発明によれば、倒立振子型ロボットや歩行ロボット、宇宙ロボットなど、力を発することのできない非駆動関節を有するロボットにおいて、全身の加速度オーダーの精密な制御を実時間で実行することが可能である。また、一般化ヤコビアンを用いた、従来の宇宙ロボット用インバースキネマティクスも、ロボットのリンク数Ｎに対してＯ（Ｎ）の計算複雑度で求めることが可能となり、多自由度なロボットほど処理負荷を低減させることができる。ゆえに、これらのロボットの普及を容易にすることができる。
【０１６２】
要するに、例示という形態で本発明を開示してきたのであり、本明細書の記載内容を限定的に解釈するべきではない。本発明の要旨を判断するためには、特許請求の範囲を参酌すべきである。
【符号の説明】
【０１６３】
１００…制御システム
１０１…主モデル
１０２…補助モデル
１０３…順動力学演算部
１０４…ハイブリッド動力学演算部
１０５…一般化逆力運動学演算部
１０６…一般化操作空間慣性逆行列演算部
１０７…一般化操作空間バイアス加速度演算部
１０８…仮想外力算出部
１０９…全身協調力制御部
１１０…関節力制御部
【特許請求の範囲】
【請求項１】
駆動関節及び非駆動関節を有するロボットの前記駆動関節を不動とした補助モデルを用いて、逆動力学と順動力学からなるハイブリッド動力学演算を実行して、関節加速度が既知である不動関節に作用する関節力と、関節力が既知である可動関節に発生する関節加速度を算出するハイブリッド動力学演算部と、
前記ロボットのすべての関節を可動とした主モデルを用いて、前記ロボットに作用する既知の力によって発生する加速度を算出する順動力学演算部と、
前記ハイブリッド動力学演算部及び前記順動力学演算部による演算結果に基づいて、前記ロボットの任意の部位に所望の目標加速度を発生させるための関節力を決定する関節力決定部と、
前記決定した関節力を制御目標値として、前記ロボットの各関節の関節力を制御する関節力制御部と、
を具備するロボットの制御装置。
【請求項２】
前記ハイブリッド動力学演算部によって得られた前記ロボットに作用する力を、一般化ヤコビアンを用いて関節力に変換する、
請求項１に記載のロボットの制御装置。
【請求項３】
前記関節力決定部は、
関節空間及び外力以外をすべてゼロとし、ｉ番目の操作空間にのみ第ｉ成分が１の単位ベクトルｅiからなる外力を作用させる条件下で、前記順動力学演算部に前記順動力学演算をすべての操作空間にわたり繰り返し実行させることによって、一般化操作空間慣性逆行列を求める一般化操作空間慣性逆行列演算部と、
関節空間に発生する速度と重力のみが作用するという制約条件下で、前記順動力学演算部に前記順動力学演算を１回だけ実行させて、一般化操作空間バイアス加速度を求める一般化操作空間バイアス加速度演算部と、
前記一般化操作空間慣性逆行列及び前記一般化操作空間バイアス加速度を用いて表される操作空間に作用する力と加速度の関係式において、前記操作空間に発生する目標加速度を既知とし、それを達成するために前記操作空間に発する仮想的な外力ｆを解く仮想外力算出部と、
前記仮想外力算出部が求めた外力ｆを、一般化逆力運動学演算を用いて駆動関節の駆動関節力に変換する関節算出部と、
を備える請求項１に記載のロボットの制御装置。
【請求項４】
前記仮想外力算出部は、前記関係式と前記ロボットと環境との間で形成される拘束条件からなる線形相補性問題を満足する前記仮想的な外力を、前記一般化操作空間慣性逆行列及び前記一般化操作空間バイアス加速度演算部を用いて求め、得られた力に基づいてリンクの関節力を決定する、
請求項３に記載のロボットの制御装置。
【請求項５】
前記ハイブリッド動力学演算部に、関節空間及び外力以外をすべてゼロとし、ｉ番目の操作空間にのみ第ｉ成分が１の単位ベクトルｅiからなる外力を作用させる条件下で、駆動関節空間の関節力を算出するハイブリッド動力学演算を繰り返し実行させることによって、一般化ヤコビアンを求める、
請求項１に記載のロボットの制御装置。
【請求項６】
駆動関節及び非駆動関節を有するロボットの前記駆動関節を不動とした補助モデルを用いて、逆動力学と順動力学からなるハイブリッド動力学演算を実行して、関節加速度が既知である不動関節に作用する関節力と、関節力が既知である可動関節に発生する関節加速度を算出するハイブリッド動力学演算ステップと、
前記ロボットのすべての関節を可動とした主モデルを用いて、前記ロボットに作用する既知の力によって発生する加速度を算出する順動力学演算ステップと、
前記ハイブリッド動力学演算ステップ及び前記順動力学演算ステップによる演算結果に基づいて、前記ロボットの任意の部位に所望の目標加速度を発生させるための関節力を決定する関節力決定ステップと、
前記決定した関節力を制御目標値として、前記ロボットの各関節の関節力を制御する関節力制御ステップと、
を有するロボットの制御方法。
【請求項７】
駆動関節及び非駆動関節を有するロボットを制御するための処理をコンピューター上で実行するようにコンピューター可読形式で記述されたコンピューター・プログラムであって、前記コンピューターを、
前記ロボットの前記駆動関節を不動とした補助モデルを用いて、逆動力学と順動力学からなるハイブリッド動力学演算を実行して、関節加速度が既知である不動関節に作用する関節力と、関節力が既知である可動関節に発生する関節加速度を算出するハイブリッド動力学演算部、
前記ロボットのすべての関節を可動とした主モデルを用いて、前記ロボットに作用する既知の力によって発生する加速度を算出する順動力学演算部、
前記ハイブリッド動力学演算部及び前記順動力学演算部による演算結果に基づいて、前記ロボットの任意の部位に所望の目標加速度を発生させるための関節力を決定する関節力決定部、
前記決定した関節力を制御目標値として、前記ロボットの各関節の関節力を制御する関節力制御部、
として機能させるためのコンピューター・プログラム。
【請求項１】
駆動関節及び非駆動関節を有するロボットの前記駆動関節を不動とした補助モデルを用いて、逆動力学と順動力学からなるハイブリッド動力学演算を実行して、関節加速度が既知である不動関節に作用する関節力と、関節力が既知である可動関節に発生する関節加速度を算出するハイブリッド動力学演算部と、
前記ロボットのすべての関節を可動とした主モデルを用いて、前記ロボットに作用する既知の力によって発生する加速度を算出する順動力学演算部と、
前記ハイブリッド動力学演算部及び前記順動力学演算部による演算結果に基づいて、前記ロボットの任意の部位に所望の目標加速度を発生させるための関節力を決定する関節力決定部と、
前記決定した関節力を制御目標値として、前記ロボットの各関節の関節力を制御する関節力制御部と、
を具備するロボットの制御装置。
【請求項２】
前記ハイブリッド動力学演算部によって得られた前記ロボットに作用する力を、一般化ヤコビアンを用いて関節力に変換する、
請求項１に記載のロボットの制御装置。
【請求項３】
前記関節力決定部は、
関節空間及び外力以外をすべてゼロとし、ｉ番目の操作空間にのみ第ｉ成分が１の単位ベクトルｅiからなる外力を作用させる条件下で、前記順動力学演算部に前記順動力学演算をすべての操作空間にわたり繰り返し実行させることによって、一般化操作空間慣性逆行列を求める一般化操作空間慣性逆行列演算部と、
関節空間に発生する速度と重力のみが作用するという制約条件下で、前記順動力学演算部に前記順動力学演算を１回だけ実行させて、一般化操作空間バイアス加速度を求める一般化操作空間バイアス加速度演算部と、
前記一般化操作空間慣性逆行列及び前記一般化操作空間バイアス加速度を用いて表される操作空間に作用する力と加速度の関係式において、前記操作空間に発生する目標加速度を既知とし、それを達成するために前記操作空間に発する仮想的な外力ｆを解く仮想外力算出部と、
前記仮想外力算出部が求めた外力ｆを、一般化逆力運動学演算を用いて駆動関節の駆動関節力に変換する関節算出部と、
を備える請求項１に記載のロボットの制御装置。
【請求項４】
前記仮想外力算出部は、前記関係式と前記ロボットと環境との間で形成される拘束条件からなる線形相補性問題を満足する前記仮想的な外力を、前記一般化操作空間慣性逆行列及び前記一般化操作空間バイアス加速度演算部を用いて求め、得られた力に基づいてリンクの関節力を決定する、
請求項３に記載のロボットの制御装置。
【請求項５】
前記ハイブリッド動力学演算部に、関節空間及び外力以外をすべてゼロとし、ｉ番目の操作空間にのみ第ｉ成分が１の単位ベクトルｅiからなる外力を作用させる条件下で、駆動関節空間の関節力を算出するハイブリッド動力学演算を繰り返し実行させることによって、一般化ヤコビアンを求める、
請求項１に記載のロボットの制御装置。
【請求項６】
駆動関節及び非駆動関節を有するロボットの前記駆動関節を不動とした補助モデルを用いて、逆動力学と順動力学からなるハイブリッド動力学演算を実行して、関節加速度が既知である不動関節に作用する関節力と、関節力が既知である可動関節に発生する関節加速度を算出するハイブリッド動力学演算ステップと、
前記ロボットのすべての関節を可動とした主モデルを用いて、前記ロボットに作用する既知の力によって発生する加速度を算出する順動力学演算ステップと、
前記ハイブリッド動力学演算ステップ及び前記順動力学演算ステップによる演算結果に基づいて、前記ロボットの任意の部位に所望の目標加速度を発生させるための関節力を決定する関節力決定ステップと、
前記決定した関節力を制御目標値として、前記ロボットの各関節の関節力を制御する関節力制御ステップと、
を有するロボットの制御方法。
【請求項７】
駆動関節及び非駆動関節を有するロボットを制御するための処理をコンピューター上で実行するようにコンピューター可読形式で記述されたコンピューター・プログラムであって、前記コンピューターを、
前記ロボットの前記駆動関節を不動とした補助モデルを用いて、逆動力学と順動力学からなるハイブリッド動力学演算を実行して、関節加速度が既知である不動関節に作用する関節力と、関節力が既知である可動関節に発生する関節加速度を算出するハイブリッド動力学演算部、
前記ロボットのすべての関節を可動とした主モデルを用いて、前記ロボットに作用する既知の力によって発生する加速度を算出する順動力学演算部、
前記ハイブリッド動力学演算部及び前記順動力学演算部による演算結果に基づいて、前記ロボットの任意の部位に所望の目標加速度を発生させるための関節力を決定する関節力決定部、
前記決定した関節力を制御目標値として、前記ロボットの各関節の関節力を制御する関節力制御部、
として機能させるためのコンピューター・プログラム。
【図１】
【図２】
【図３】
【図４】
【図５】
【図６Ａ】
【図６Ｂ】
【図６Ｃ】
【図６Ｄ】
【図７】
【図８Ａ】
【図８Ｂ】
【図８Ｃ】
【図９】
【図１０】
【図１１】
【図１２】
【図１３】
【図１４】
【図１５】
【図２】
【図３】
【図４】
【図５】
【図６Ａ】
【図６Ｂ】
【図６Ｃ】
【図６Ｄ】
【図７】
【図８Ａ】
【図８Ｂ】
【図８Ｃ】
【図９】
【図１０】
【図１１】
【図１２】
【図１３】
【図１４】
【図１５】
【公開番号】特開２０１２−８１５６８（Ｐ２０１２−８１５６８Ａ）
【公開日】平成２４年４月２６日（２０１２．４．２６）
【国際特許分類】
【出願番号】特願２０１０−２３１６４０（Ｐ２０１０−２３１６４０）
【出願日】平成２２年１０月１４日（２０１０．１０．１４）
【出願人】（０００００２１８５）ソニー株式会社 (34,172)
【Ｆターム（参考）】
【公開日】平成２４年４月２６日（２０１２．４．２６）
【国際特許分類】
【出願日】平成２２年１０月１４日（２０１０．１０．１４）
【出願人】（０００００２１８５）ソニー株式会社 (34,172)
【Ｆターム（参考）】
[ Back to top ]