JP2007185756A - 制御システム及び制御方法、並びにコンピュータ・プログラム - Google Patents

Abstract

【課題】複数の剛体が連なって構成される多リンク構造物が、単一平面以外の複数の面に接触しながら移動する場合や、宙に浮く場合など色々な外界との接触状態にあっても、該リンク構造物の運動を安定化する。
【解決手段】系全体の運動量が長期にわたって発散しないことが安定化規範として有効であるということに着眼し、並進運動量並びに角運動量の目標値との偏差を評価する安定化評価関数を定義し、この安定化評価関数を最小化するような力やモーメントなどの最適制御入力を決定し、この最適制御入力が発生する最適運動量を算出し、この最適運動量を実現するためのリンク構造物の関節駆動方法を決定する。
【選択図】 図６

Description

本発明は、ロボットやキャラクタなどの多リンク構造体のバランスが損なわれないように安定制御する制御システム及び制御方法、並びにコンピュータ・プログラムに係り、特に、ＺＭＰ安定度判別規範に依存せず、平面上の運動以外に普く適用することができる制御システム及び制御方法、並びにコンピュータ・プログラムに関する。

さらに詳しくは、本発明は、特定の前提条件に拘束されない一般化されたケースで安定化規範を扱う制御システム及び制御方法、並びにコンピュータ・プログラムに係り、特に、外界との接触状態に依存しない一般化された安定化規範とその実時間数値解法を実現する制御システム及び制御方法、並びにコンピュータ・プログラムに関する。

昨今、ヒューマノイド・ロボットやペット型ロボットなど、脚式ロボットのアプリケーションに注目が集まっている。これら脚式ロボットは、クローラ式ロボットに比し、階段の昇降や障害物の乗り越えなど、柔軟な歩行・走行動作を実現できるという点で優れている。

また、ＣＰＵ（Ｃｅｎｔｒａｌ Ｐｒｏｃｅｓｓｉｎｇ Ｕｎｉｔ）の演算能力の向上や、３Ｄレンダリング技術や力学シミュレーション技術の成熟などと相俟って、人間などのゲーム・キャラクタを扱う仮想世界中で力学現象をより忠実にシミュレートしたリアリティの高いキャラクタ制御への要求が高まってきている。

この種のアプリケーションでは、ロボットやキャラクタといった多リンク構造体のバランスが損なわれないように安定制御するコントローラ（以下では、「スタビライザ」とも呼ぶ）が必要である。

例えば、脚式ロボットでは、ＺＭＰ（Ｚｅｒｏ Ｍｏｍｅｎｔ Ｐｏｉｎｔ）が歩行の安定度判別の規範とする姿勢安定制御システムが当業界において広く知られている。ＺＭＰ理論は、歩行系から路面には重力と慣性力、並びにこれらのモーメントが路面から歩行系への反作用としての床反力並びに床反力モーメントとバランスするという「ダランベールの原理」に基づく。その力学的推論の帰結として、足底接地点と路面の形成する支持多角形の内側にピッチ軸及びロール軸モーメントがゼロとなる点、すなわちＺＭＰが存在する（例えば、非特許文献１を参照のこと）。

目標ＺＭＰ制御では、すべての瞬間において、動的釣り合いを取るように運動を計画する。ＺＭＰ規範に基づく２足歩行パターン生成は、足底着地点をあらかじめ設定することができ、路面形状に応じた足先の運動学的拘束条件を考慮し易いなどの利点がある。

例えば、ＺＭＰを目標値に一致させるように脚部関節を駆動するとともに、上体の傾斜を検出して指令値に一致させるように同一脚部関節を駆動する２足歩行式脚式移動ロボットの歩行制御装置について提案がなされている（例えば、特許文献１を参照のこと）。

また、センサなどから計測されたロボットの運動状態量、外力及び外力モーメント、環境形状などの実世界の物量を入力するように歩行制御系を構成して、ロボットの歩行パターン生成演算を、運動状態量に関する境界条件、外力、外力モーメント、足底軌道などのパラメータが設定可能な形式で、実機上で実時間により実行可能にするロボット装置について提案がなされている（例えば、特許文献２を参照のこと）。同装置によれば、機体のバランス維持に関するすべての計算を単一の歩行パターン演算に集約させることにより、歩容生成機能と適応制御機能を効率的に両立させるとともに、力学モデルの一貫性を保証し、各力学モデル間の干渉を取り除くことができる。

また、ＺＭＰを安定度判別規範として、接地状態と非接地状態をともに考慮に入れて安定運動パターンを実時間で算出するロボットの運動制御装置について提案がなされている（例えば、特許文献３を参照のこと）。同装置によれば、接地時の支持多角形内の点回りの水平軸回りモーメントが０又は無接地時の水平方向並進力が０で、現在の重心水平位置・速度に連続的に接続するように将来のロボットの重心水平軌道に関する境界条件付運動方程式を求解し、無接地時に重力以外に作用する垂直方向並進力が０となり、現在の重心垂直位置・速度に連続的に接続するように将来のロボットの重心垂直軌道に関する境界条件付運動方程式を求解し、無接地時に重心モーメント回りモーメントが０となり、上記決定した重心位置を実現するように次時刻の運動状態を決定する。すなわち、重心水平位置軌道及び重心垂直位置軌道に関する境界条件付き運動方程式の解を制御周期毎に算出し、さらにロボット装置の重心回りのモーメントを調整しながら、制御周期毎にロボット装置の運動状態を決定するので、ロボット装置の動作を実時間で制御することができる。

また、ＺＭＰ安定度判別規範をより簡略化した線型倒立振子モードを規範とする手法も知られている（例えば、特許文献４、非特許文献２を参照のこと）。

しかしながら、ＺＭＰ安定度判別規範に基づくスタビライザは、適用範囲が基本的には平面上での（歩行）運動に限定され、ロボットの左右それぞれの足が法線の異なる平面群に同時に接しないなどの前提条件が敷かれている。言い換えれば、法線ベクトルが異なる複数の面に同時に接地しながら移動する場合や、鉄棒にぶら下がりながら宙に浮くといった、上記の前提条件の枠外となる一般的なケースをＺＭＰ安定度判別規範で扱うことはできない。

このような一般的なケースを対象とした安定度判別規範として、転倒安定性という概念が導入されているが（例えば、非特許文献３を参照のこと）、実時間解法の構成には至っていない。

法線ベクトルが異なる複数の面に同時に接地しながら移動する場合や、鉄棒にぶら下がりながら宙に浮く場合など、前提条件毎に特化した複数のスタビライザを構成し、直面した条件に応じて各スタビライザを使い分けて実時間で解決するという解決方法も考えられる。この場合、シームレスな運動制御を行なうことはできず、アプリケーション開発上の見通しがよくない。

また、計算量が多くてスタビライザが実時間処理を行なうことができなければ、ロボットやゲーム中のキャラクタのインタラクティビティを前提とするアプリケーションに適用することはできない。

特開平５−３０５５７９号公報 特開２００４−１４２０９５ 特許第３５８８２４４号明細書 特願６１−２９８４０６号明細書 ヴコブラトビッチ（Ｍｉｏｍｉｒ Ｖｕｋｏｂｒａｔｏｖｉｃ）著「脚式移動ロボット（ＬＥＧＧＥＤ ＬＯＣＯＭＯＴＩＯＮＲＯＢＯＴＳ）」（加藤一郎外著『歩行ロボットと人工の足』（日刊工業新聞社）） 梶田著「線形倒立振子モードを規範とする動的二足歩行ロボットの実時間制御」（機械技術研究所報告 第１７１号 ｐｐ．１−６８，１９９６）

本発明の目的は、ロボットやキャラクタなどの多リンク構造体のバランスが損なわれないように安定制御することができる、優れた制御システム及び制御方法、並びにコンピュータ・プログラムを提供することにある。

本発明のさらなる目的は、特定の前提条件に拘束されない一般化されたケースで安定化規範を扱うことができる、優れた制御システム及び制御方法、並びにコンピュータ・プログラムを提供することにある。

本発明は、上記課題を参酌してなされたものであり、その第１の側面は、複数の剛体が連なって構成される多リンク構造物の制御システムであって、
前記多リンク構造物に関する系全体の運動量と制御入力との関係を記述する状態方程式を設定する状態方程式設定手段と、
現在時刻から将来時刻に至る制御目標値を設定する予測制御目標値設定手段と、
該制御目標値に基づく運動量の目標値と現実の運動量との差分に基づいて前記多リンク構造物の安定性を評価する安定性評価関数を設定する安定性評価関数設定手段と、
前記状態方程式で構成される拘束条件下で、前記安定性評価関数の極小値を与える最適制御入力を決定する最適制御入力決定手段と、
該最適制御入力が発生する最適運動量を算出する最適運動量算出手段と、
該最適運動量を実現するための前記多リンク構造物の関節駆動方法を決定する関節駆動方法決定手段と、
を具備することを特徴とする制御システムである。

但し、ここで言う「システム」とは、複数の装置（又は特定の機能を実現する機能モジュール）が論理的に集合した物のことを言い、各装置や機能モジュールが単一の筐体内にあるか否かは特に問わない（以下、同様）。

脚式ロボットやゲーム・キャラクタといった多リンク構造体を扱うアプリケーションでは、構造体のバランスが損なわれないように安定制御するスタビライザが必要である。

ＺＭＰを安定度判別規範としたロボットの姿勢安定制御システムが広く知られているが、適用範囲が基本的には平面上での（歩行）運動に限定され、ロボットの左右それぞれの足が法線の異なる平面群に同時に接しないなどの前提条件の枠外となる一般的なケースをＺＭＰ安定度判別規範で扱うことはできない。

これに対し、本発明に係る制御システムは、系全体の運動量が長期にわたって発散しないことが安定化規範として有効であるということに着眼し、並進運動量並びに角運動量の目標値との偏差を評価する安定性評価関数を定義した。そして、この安定性評価関数を最小化するような制御入力を決定することによって、系全体が長期的に安定化することができる。運動量が発散する最たる例はロボットが転倒する場合である。逆に言えば、運動量が発散しないようにすれば自ずと転倒しない解が選ばれることになる。

すなわち、本発明に係る制御システムによれば、外界との接触状態など、目標運動に関する概略的な将来イメージを入力するだけで、系全体の運動量が長期的に発散しないよう、接触力や運動量の詳細を自動決定し、特定の前提条件に拘束されない一般化されたケースで安定化規範に関する実時間解法を提供することができる。

ここで、前記予測制御目標値設定手段は、制御目標値として前記多リンク構造物の外力の作用点に関する情報、具体的には外力又は外モーメントに関する情報を設定する。

また、前記予測制御目標値設定手段は、制御目標値として前記多リンク構造体の重心、重心速度、あるいは運動量に関する情報、さらには角運動量に関する情報を設定する。

また、前記安定性評価関数設定手段並びに前記最適運動量算出手段は、運動量として前記多リンク構造体の並進運動量、重心又はその他の所定の点回りの角運動量を扱う。

また、本発明に関する制御システムへの制御入力は、前記多リンク構造体に作用する力又はモーメント、前記多リンク構造体全体に作用する力の作用点などからなる。

また、前記安定性評価関数設定手段が設定する安定性評価関数は、制御目標値の偏差、制御入力などを含んでいる。

また、本発明に係る制御システムは、外力に関する不等式拘束条件を設定する不等式拘束条件設定手段をさらに備えていてもよい。この場合、前記最適運動量算出手段は、該不等式拘束条件の下で最適運動量を算出する。

前記関節駆動方法決定手段は、運動量若しくは重心に関する操作空間慣性逆行列を用いて関節駆動方法を決定することができる。

あるいは、前記関節駆動方法決定手段は、運動量若しくは重心に関するヤコビアンを用いて関節駆動方法を決定することができる。

また、前記関節駆動方法決定手段は、運動量以外の拘束条件をさらに設定し、運動量に関する拘束とそれ以外の運動拘束が同時に成立するように、関節駆動方法を決定することができる。

また、本発明の第２の側面は、複数の剛体が連なって構成される多リンク構造物の制御に関する処理をコンピュータ・システム上で実行するようにコンピュータ可読形式で記述されたコンピュータ・プログラムであって、前記コンピュータ・システムに対し、
前記多リンク構造物に関する系全体の運動量と制御入力との関係を記述する状態方程式を設定する状態方程式設定手順と、
現在時刻から将来時刻に至る制御目標値を設定する予測制御目標値設定手順と、
該制御目標値に基づく運動量の目標値と現実の運動量との差分に基づいて前記多リンク構造物の安定性を評価する安定性評価関数を設定する安定性評価関数設定手順と、
前記状態方程式で構成される拘束条件下で、前記安定性評価関数の極小値を与える最適制御入力を決定する最適制御入力決定手順と、
該最適制御入力が発生する最適運動量を算出する最適運動量算出手順と、
該最適運動量を実現するための前記多リンク構造物の関節駆動方法を決定する関節駆動方法決定手順と、
を実行させることを特徴とするコンピュータ・プログラムである。

本発明の第２の側面に係るコンピュータ・プログラムは、コンピュータ・システム上で所定の処理を実現するようにコンピュータ可読形式で記述されたコンピュータ・プログラムを定義したものである。換言すれば、本発明の第２の側面に係るコンピュータ・プログラムをコンピュータ・システムにインストールすることによって、コンピュータ・システム上では協働的作用が発揮され、本発明の第１の側面に係る制御システムと同様の作用効果を得ることができる。

本発明によれば、ロボットやキャラクタなどの多リンク構造体のバランスが損なわれないように好適に安定制御することができる、優れた制御システム及び制御方法、並びにコンピュータ・プログラムを提供することができる。

また、本発明によれば、外界との接触状態に依存しない一般化された安定化規範とその実時間数値解法を好適に実現することができる、優れた制御システム及び制御方法、並びにコンピュータ・プログラムを提供することができる。

本発明に係る制御システムによれば、外界との接触状態に依存せず、非常に広範な種類の運動を安定化することができる。

本発明に係る制御システムは、系全体の運動量が長期にわたって発散しないことが安定化規範として有効であるということに基づいて構成され、外界との接触状態など、目標運動に関する概略的な将来イメージを入力するだけで、系全体の運動量が長期的に発散しないよう、接触力や運動量の詳細を自動決定し、特定の前提条件に拘束されない一般化されたケースで安定化規範に関する実時間解法を提供することができる。

本発明のさらに他の目的、特徴や利点は、後述する本発明の実施形態や添付する図面に基づくより詳細な説明によって明らかになるであろう。

以下、図面を参照しながら本発明の実施形態について詳解する。

図１には、本発明に係る安定判別規範の適用対象となるロボット装置の外観構成を示している。図示のロボット装置は、いわゆる人間型ロボットであり、骨盤部には、移動手段としての２肢の脚体と、腰関節を介して上体が接続されている。上体には、２肢の腕部と、首関節を介して頭部が接続されている。

左右の脚体は、それぞれ股関節３自由度と、膝関節１自由度と、足首関節２自由度の、計６自由度を備えている。また、左右の腕部は、それぞれ肩関節３自由度と、肘関節１自由度と、手首関節２自由度の、計６自由度を備えている。首関節及び腰関節は、ともにＸ、Ｙ、Ｚ軸回りに３自由度を有するとする。

各関節軸を駆動するアクチュエータは、例えばＤＣブラシレス・モータと減速機、並びに減速機の出力軸の回転位置を検出する位置センサで構成される。これら関節駆動アクチュエータは、ロボット装置全体の動作を統括的にコントロールするホスト・コンピュータと接続され、その位置又は力制御目標値が与えられるとともに、現在の出力トルクや、関節角度、関節角速度をホスト・コンピュータに送信することができるものとする。

図２には、図１に示したロボット装置における結線トポロジの構成例を示している。また、図３には、図１に示したロボット装置の関節自由度モデルを示している。

ロボット装置は、胴体部に、３軸の腰関節アクチュエータａ１、ａ２、ａ３、及び３軸の首関節アクチュエータａ１６、ａ１７、ａ１８を持ち、これらはホスト・コンピュータに対しシリアル接続されている。各関節アクチュエータは、シリアル・ケーブルを通じて、その位置又は力制御目標値を受け取るとともに、現在の出力トルクや、関節角度、関節角速度をホスト・コンピュータに送信する。

また、ロボット装置は、左腕部に、３軸の肩関節アクチュエータａ４、ａ５、ａ６、１軸の肘関節アクチュエータａ７、及び２軸の手首関節アクチュエータａ８、ａ９を持ち、これらはホスト・コンピュータに対しシリアル接続されている。同様に、右腕部には、３軸の肩関節アクチュエータａ１０、ａ１１、ａ１２、１軸の肘関節アクチュエータａ１３、及び２軸の手首関節アクチュエータａ１４、ａ１５を持ち、さらに左右の手先にはヨー軸駆動用アクチュエータａ３１、ａ３２が備えられ、これらはホスト・コンピュータに対しシリアル接続されている。

また、ロボット装置は、左脚部に、３軸の股関節アクチュエータａ１９、ａ２０、ａ２１、１軸の膝関節アクチュエータａ２２、及び２軸の足首関節アクチュエータａ２３、ａ２４を持ち、これらはホスト・コンピュータに対しシリアル接続されている。同様に、右脚部には、３軸の肩関節アクチュエータａ２５、ａ２６、ａ２７、１軸の肘関節アクチュエータａ２８、及び２軸の足首関節アクチュエータａ２９、ａ３０を持ち、これらはホスト・コンピュータに対しシリアル接続されている。

各関節で使用されるアクチュエータａ１〜ａ３０は、例えばＤＣブラシレス・モータと減速機、並びにモータの回転位置、速度、トルクを検出する位置センサ、速度センサ、トルクセンサで構成され、外部から与えられた位置又は力制御目標値に従って回転駆動するとともに、現在の出力トルクや、関節角度、関節角速度を出力する。この種の関節アクチュエータについては、例えば本出願人に既に譲渡されている特開２００４−１８１６１３号公報に記載されている。

ジョイントによって接続される各リンクは剛体リンクとしてモデル化され、幾何形状の他、質量や重心位置、慣性テンソルといった物理属性を保持する。

リンクの接続関係は、骨盤Ｂを基底とする開リンク木構造として表現する。上記のロボット装置の場合、ワールド空間中を自由に移動し、姿勢を変化させることができることから、ロボット装置全体の姿勢は、すべての関節角θ＝（θ₁θ₂…θ_N）^T（但し、Ｎは関節数）だけでなく、骨盤Ｂのオイラー角α＝（α，β，γ）^Tとその位置ｐ₀（ｐ_0x，ｐ_0y，ｐ_0z）^Tを加えた、下式（１）に示す一般化変数で定義される。

すると、ロボット装置の運動方程式は、下式（２）のような形で表現することができる。

ここで、τは一般化変数ｑに対応した一般化力であり、αとｐ₀に対応する成分は（力を発生するアクチュエータが存在しないため）０となり、θに対応する成分は各関節部を駆動するアクチュエータの発生力に等しい。Ｈは、ロボット全体の慣性行列、ｂは重力やコリオリ力などを表す。ｆ_extは外力である。Ｊ_extは外力作用方向に発生する速度と一般化変数ｑの時間微分である一般化速度とを対応付けるヤコビアンである。

本発明者らは、系全体の運動量が長期にわたって発散しないことが安定化規範として有効であるということに着眼した。したがって、上述したような系を安定化するのに、上式（２）のような細部のダイナミクスは一旦隠蔽し、式（２）の系全体が有する運動量を安定化することについて考察する。

系全体の運動量は、並進運動量Ｐと重心ｒ回りの角運動量Ｌによって表現することができる。環境から受ける外モーメントをｎ_j、ｉ番目の作用点ｌ_iが外力をｆ_iとすると（図４を参照のこと）、運動方程式は下式（３）のように表現することができる。但し、Ｍはロボット装置の総重量、ｇは重力加速度を表す。

ここで、下式（４）の関係を加えると、上式（３）は次々式（５）のようにまとめられる。

ここで、状態変数として、

制御入力として、

出力変数として（出力変数は、状態変数の一部の成分で構成される）、

をそれぞれ考える。すると、上式（５）で示した状態方程式、並びに上式（８）で示した出力方程式は、それぞれ下式のように表記することができる。

但し、行列Ｃ及び関数φ（ｘ，ｕ，ｔ）は以下の通りとする。

系全体の運動量が長期にわたって発散しないことを安定化規範とする場合、上式（９）及び（１０）で表される系の状態方程式を拘束条件として、下式（１３）に示す安定性評価関数Ｅを最小化するように制御入力が決定されれば、系全体が長期的に安定とみなすことができる。但し、ｙ_refはｙの目標値を表すとする。

上式（１３）の右辺第１項は、現在時刻からＴ秒後の並進運動量と角運動量が目標値に収束するかどうかを評価する。また、同式の右辺第２項は、積分区間［０，Ｔ］全体にわたって目標運動量に追従するかどうかを評価する。また、同式の右辺第３項は、大きい外力が作用しないかどうかを評価する。すなわち、上記の安定性評価関数Ｅは、現在時刻ｔ＝０から将来時刻ｔ＝Ｔの区間を考え、この間、系全体の運動量（出力変数）偏差、及び制御入力が発散しないことを安定条件として規定し、一般化された安定判別規範を提供するものである。運動量が発散する最たる例はロボットが転倒する場合である。逆に言えば、運動量が発散し内容にすればおのずと転倒しない解が選ばれることになる。

上式（９）及び（１０）で示したような非線形時変な拘束条件の下で、上式（１３）のような評価関数を最小化する問題は、一般には、非線形Ｒｅｃｅｄｉｎｇ Ｈｏｒｉｚｏｎ制御と呼ばれる問題に分類される。例えば、Ｔ．Ｏｔｓｕｋａ著“Ｔｉｍｅ Ｖａｒｉｅｎｔ Ｒｅｃｅｄｉｎｇ Ｈｏｒｉｚｏｎ Ｃｏｎｔｒｏｌ ｏｆ Ｎｏｎｌｉｎｅａｒ Ｓｙｓｔｅｍｓ”（Ｊｏｕｒｎａｌ ｏｆ Ｇｕｉｄａｎｃｅ，Ｃｏｎｔｒｏｌ ａｎｄ Ｄｙｎａｍｉｃｓ，Ｖｏｌ．２１，Ｎｏ．１，ｐｐ．１７４−１７６，１９９８）には、収束演算を含まずに非線形Ｒｅｃｅｄｉｎｇ Ｈｏｒｉｚｏｎ制御問題を解く方法が記載されており、計算量は嵩むが、原理的には実時間で制御入力を得ることができる。但し、本明細書では、一般的な非線形問題ではなく、時変な線形系として扱えるように、簡略化した問題を例に、本問題の一数値解法を示す。

上式（５）において、外モーメントｎ_jは作用せず（ｎ_j＝０）、外力ｆ_iは既知とする。但し、ｆ_iの値は、正確なものではなく、概略値とする。その誤差分を調整するために、系全体に並進力ｆとモーメントｎを入力することで、運動量を最小化する問題を考える（図５を参照のこと）。ｆ及びｎは、ｆ_i及びその作用点ｌ_iに加えられる微調整分の総和と言い換えることもできる。このとき、拘束式（５）は、以下のように改められる。

上式を整理すると、下式（１５）の通りとなる。

ここで、制御入力を下式（１６）のように取り直す。

すると、拘束式（１５）は以下のように表される。

ここで、行列Ａ、Ｂ、並びにベクトルｄは以下の通りとなる。

上式（１５）に示した拘束条件は時変な線形系であり、数値解法の構成が容易である。ポントリヤーギンの最大原理を用いて、拘束条件（１５）及び（１０）の下で、上式（１３）に示した評価関数を最小化する条件を求めると、以下の通りとなる。

ξは随伴状態ベクトルである。ξとして次式（２５）のように状態変数ｘに対して線形となる形を仮定すると、Ｋ及びｈは式（２６）〜（２９）に示すような微分方程式境界問題を構成する。

上式（２６）はＲｉｃｃａｔｉ方程式として知られている。これらの微分方程式は、ｔ＝Ｔからｔ＝０に向けて逆向きの数値積分（Ｂａｃｋｗａｒｄ ｓｗｅｅｐ）を実行することで、数値解を得ることができる。これによって、Ｋ（０）及びｈ（０）を得ることができる。続いて、式（２５）にこれらを代入することでξ（０）を算出し、さらにこれを式（２３）に代入することで、現在時刻ｔ＝０における制御入力ｕ（０）を得ることができる。次時刻ｄｔにおける状態ベクトルｘは、式（２１）にｕ（０）を代入し、数値積分することで得られる。

以上で、運動量の最小化を実現するための現在時刻の制御入力としての微調整分の並進力ｆ及びモーメントｎを得ることができ、その結果得られる次時刻の運動量Ｐ（ｄｔ）、Ｌ（ｄｔ）が得られたことになる。ここで、ｄｔは制御周期を表す。

続いて、上述のようにして、制御周期毎に得られる系全体の運動量Ｐ及びＬを満足するように、全身のキネマティクス、マスプロパティを考慮して関節を駆動する。

例えば、操作空間制御に基づいて全身強調制御系を構成することができる。操作空間（Ｏｐｅｒａｔｉｏｎａｌ Ｓｐａｃｅ）とは、オブジェクトに作用する力と発生する加速度の関係を記述するための空間のことである。操作空間は、基本的には操作空間慣性逆行列と呼ばれる行列によって与えられ、操作空間慣性逆行列及び操作空間バイアス加速度といった操作空間物理量を用いて、操作空間の加速度と力との関係式を記述することができる。

前述の式（２）で示した通り、脚式ロボットやＣＧキャラクタなど複数の剛体が連なって構成されるリンク構造物全体の運動方程式は下式（３０）のような形で表現できることが知られている。

ここで、τは一般化変数ｑに対応した一般化力、ｂは重力・コリオリ力、ｆ₀は操作空間に作用する外力、Ｈは構造全体の関節空間に対する慣性行列、Ｊ₀は操作空間を定義するヤコビアンである。剛体が関節を介して連なったリンク構造物において、関節の値をすべて連ねてできるベクトルは一般化変数ｑで表される。

操作空間は、基本的には操作空間慣性逆行列と呼ばれる行列によって与えられる。すなわち、操作空間慣性逆行列は、力と加速度を紐付けする表現方法であり、下式（３１）に示す行列Λ^-1として定義される。但し、Ｈは構造全体の関節空間に対する慣性行列である。

操作空間慣性逆行列Λ^-1を用いて、上式（３０）を以下のように変形することができる。

上式の左辺は、操作空間に発生する加速度を表す。但し、ｃ₀は操作空間バイアス加速度（外力が作用しない場合に操作空間に発生する加速度）であり、下式のように表される。

操作空間慣性逆行列Λ^-1は、任意部位における力と加速度を関連付ける行列であり、力制御、力学シミュレーション、コンピュータ・アニメーション、姿勢編集、外力推定など、多くの分野への応用を可能にする重要な物理量である。

操作空間慣性逆行列Λ^-1を定義通りに計算すると、構造全体の関節空間に対する慣性行列Ｈが介在して計算の無駄が発生して膨大な計算量となるため、実時間処理に向かないという問題がある。これに対して、リンク構造物の一般化力（関節力）から一般化加速度（関節加速度）を得る順動力学演算を応用することで、操作空間慣性逆行列Λ^-1を高速に算出するとともに計算負荷の軽減を図ることができる。操作空間慣性逆行列Λ^-1の高速計算方法については、例えば本出願人に既に譲渡されている特願２００５−２９８０９０号明細書を参照されたい。

リンク構造体の全身協調制御を、操作空間制御を用いた力制御により実現する場合、拘束の次元は、基本的の加速度となるので、運動量に関する拘束は、その差分表現で与える必要がある。すなわち、現在時刻の運動量をＰ及びＬとおくと、以下の連立方程式を解く必要がある。

操作空間としては、運動量だけでなく、環境との接触に関する拘束や関節可動範囲の拘束なども同時に課される。これらは不等式拘束を含む場合もあり、一般に線型相補問題として上式（３４）を解く（例えば、床からの力は引っ張る方向に作用しない、などが不等式拘束に相当する）。並進運動量を操作空間とする場合の操作空間慣性逆行列Λ^-1及びバイアス加速度の算出方法には、特願２００５−２９８０９０号明細書（前述）に記載の重心操作空間に関する計算方法を適用することができる（並進運動量は重心速度をＭ倍するのみのため）。角運動量を操作空間とする場合の操作空間慣性逆行列Λ^-1及びバイアス加速度の算出方法も、特願２００５−２９８０９０号明細書に記載されているので、参照されたい。本問題を解くことで、安定規範の課す運動量拘束を、脚式ロボットやＣＧキャラクタなど、リンク構造体の身体性に関わる詳細な拘束を考慮した解を得ることができる。

最後に、得られた操作空間力ｆ₀を以下の式によって、アクチュエータ発生力に変換すればよい。

ここで、Ｓはセレクション・マトリクスであり、力の発生できない一般化変数の要素のみを１とする対角行列とする。仮想関節ｐ₀、αのみ、力が発生できない場合、このセレクション・マトリクスは以下の通りとなる。

アクチュエータが位置制御型である場合でも、幾何拘束を用いることで全身協調運動を実現することができる。例えば、本出願人に既に譲渡されている特開２００４−３０６２３１号公報には、運動実行時に発生する拘束要求に応じて、ロボット全体に適当な拘束条件を課して、拘束条件をすべて満足するような各可動部の位置駆動量を決定することによって、全身協調運動を実現する運動制御装置について開示されている。すなわち、運動量に関する拘束は、下式のように反映される。

Ｊ_tは各拘束に関するヤコビアンを行方向に並べたものである。並進運動量ヤコビアン、角運動量ヤコビアンの算出方法は、特開２００４−３０６２３１号公報に記載されているので参照されたい。式（３７）に示した方程式を一般化変数ｑの時間微分について解いて、アクチュエータの位置指令値に反映すればよい。

図６には、以上のスタビライズ処理を実現するシステムの機能的構成を模式的に示している。図示のシステムは、運動量安定化部１０と、運動拘束解決部２０で構成される。これまで詳解してきた一般化された安定化処理の本質を担うのは運動量安定化部１０であり、運動拘束解決部２０は、算出された運動量に関する指令値が運動拘束解決部２０を入力して、その他の拘束条件と連立するように、全身を協調駆動するための各関節への指令値が決定される。

運動量安定化部１０は、運動量目標値時系列設定部１１と、作用点目標値時系列設定部１２と、状態方程式設定部１３と、評価関数設定部１４と、随伴状態ベクトル算出部１５と、制御入力算出部１６と、運動量算出部１７を備えている。

運動量目標値時系列設定部１１は、現在時刻（ｔ＝０）から将来時刻（ｔ＝Ｔ）に至るまでの、運動量の目標値（上式（１３）のｙ_ref（ｔ））を設定する。運動量の目標値は、運動の目的に応じて設定されるが、ほとんどの場合は０である。また、鉄棒にぶら下がるといった目的の場合は、各運動量にも目標値が設定される。

作用点目標値時系列設定部１２は、現在時刻（ｔ＝０）から将来時刻（ｔ＝Ｔ）に至るまでの、作用点の目標値（上式（１４）中のｌ_i（ｔ））を設定する。具体的には、ロボットやキャラクタなどの多リンク構造体のモーション・データを基に作用点の目標値の時系列が設定される。

状態方程式設定部１３は、上式（２１）に示した状態方程式に関わる、式（１８）、式（１９）、式（２０）の算出に必要な情報（変数やマトリクス）を設定する。

評価関数設定部１４は、上式（１３）中の重み行列Ｆ、Ｑ（ｔ）、Ｒ（ｔ）を設定し、評価関数を定義する。一般には、Ｑ（ｔ）とＲ（ｔ）も、現在時刻（ｔ＝０）から将来時刻（ｔ＝Ｔ）に至るまでの時変な関数となる。

随伴状態ベクトル算出部１５は、Ｋ、ｈに関する微分方程式（２６）〜（２９）を解き、現在時刻（ｔ＝０）から将来時刻（ｔ＝Ｔ）に至るＫ及びｈの数値解を得た後、上式（２５）を用いて現在時刻ｔ＝０における随伴状態ベクトルξ（０）を算出する。

制御入力算出部１６は、上式（２３）を用いて、現在時刻ｔ＝０における制御入力ｕ（０）を算出する。

運動量算出部１７は、上式（２１）を用いて、現在時刻ｔ＝０における状態変数速度を求めた後、これを数値積分して、ｘ（ｄｔ）すなわち次時刻の運動量の目標値Ｐ（ｄｔ）並びにＬ（ｄｔ）を得る。

運動拘束解決部２０は、上述のようにして運動量安定化部１０で得られた次時刻の運動量目標値Ｐ（ｄｔ）及びＬ（ｄｔ）を、他の運動拘束と同時に成立するような、関節の駆動方法すなわち全身協調方法を決定して、関節指令値として出力する。ここで言う他の運動拘束として、例えば、身体の一部に発生する力や位置、速度、加速度を指定したり、外界との接触拘束を指定したり、関節の発生力、関節値、関節速度、関節加速度、可動範囲拘束を指定したりする。力制御型の関節アクチュエータであれば、全身協調方法として操作空間制御（前述）を用いることができる（例えば、特願２００５−２９８０９０号明細書を参照のこと）。また、位置制御型の関節アクチュエータであれば、一般化された逆キネマティクス（前述）を用いることができる（例えば、特開２００４−３０６２３１号公報を参照のこと）。

図７には、図６に示したシステムによる一般化スタビライズ処理の手順をフローチャートの形式で示している。

まず、運動量目標値時系列設定部１１にて、現在時刻（ｔ＝０）から将来時刻（ｔ＝Ｔ）に至るまでの、運動量の目標値（上式（１３）のｙ_ref（ｔ））を設定する（ステップＳ１）。

次いで、作用点目標値時系列設定部１２にて、現在時刻（ｔ＝０）から将来時刻（ｔ＝Ｔ）に至るまでの、作用点の目標値（上式（１４）中のｌ_i（ｔ））を設定する（ステップＳ２）。

次いで、状態方程式設定部１３にて、上式（２１）に示した状態方程式に関わる、式（１８）、式（１９）、式（２０）の算出に必要な情報を設定する（ステップＳ３）。

次いで、評価関数設定部１４にて、上式（１３）中の重み行列Ｆ、Ｑ（ｔ）、Ｒ（ｔ）を設定し、評価関数を定義する（ステップＳ４）。

次いで、随伴状態ベクトル算出部１５にて、Ｋ、ｈに関する微分方程式（２６）〜（２９）を解き、現在時刻（ｔ＝０）から将来時刻（ｔ＝Ｔ）に至るＫ及びｈの数値解を得た後、上式（２５）を用いて現在時刻ｔ＝０における随伴状態ベクトルξ（０）を算出する（ステップＳ５）。

次いで、制御入力算出部１６にて、上式（２３）を用いて、現在時刻ｔ＝０における制御入力ｕ（０）を算出する（ステップＳ６）。

次いで、運動量算出部１７にて、上式（２１）を用いて、現在時刻ｔ＝０における状態変数速度を求めた後、これを数値積分して、ｘ（ｄｔ）すなわち次時刻の運動量の目標値Ｐ（ｄｔ）並びにＬ（ｄｔ）を得る（ステップＳ７）。

運動拘束解決部２０では、次時刻の運動量目標値Ｐ（ｄｔ）及びＬ（ｄｔ）、及び他の運動拘束を設定する（ステップＳ８）。そして、設定した運動拘束が同時に成立するような、関節指令値を決定する（ステップＳ９）。算出された関節指令値は、各関節アクチュエータに出力される（ステップＳ１０）。

図６に示したシステムは、以上のようなスタビライズ処理を、制御周期ｄｔ毎に繰り返し実行する。

上述したスタビライズ方法は、法線ベクトルが異なる複数の面に同時に接地しながら移動するといった、ＺＭＰ安定度判別規範では扱うことができない一般的な前提条件下において多リンク構造体の安定化処理を実現することができる。勿論、外力の作用点を１点とし、この作用点の目標値時系列をＺＭＰ計画軌道として用いることで、運動量目標軌道として安定歩行パターンを算出することもできる。

また、上述した実施形態では、角運動量として、重心回りの角運動量を用いたが、実用上は、重心以外の点ｐ回りの角運動量を扱っても、同様に安定化処理を行なうことができる。その場合、システムの状態方程式は、上式（５）に代わって下式（３８）のように簡略化される。

この問題はシステムの状態方程式が変更されただけであり、上述と同様に解くことができる。すなわち、上式（３８）及び式（１０）の拘束条件下で、式（１３）の評価関数を最小化する問題を上述した方法に従って解けばよい。

また、外力は、一般には、「引く力」、「押す力」、「摩擦円錐内の力」など、不等式の拘束が課せられる。このような、より一般化された場合においても、本質的に上述と同様の問題として定式化して解くことができる。すなわち、システムの状態方程式に基づく拘束条件（９）、（１０）、及び以下の不等式拘束の条件下で、式（１３）の評価関数を最小化する問題を解けばよい。

計算量は多いが、２次計画法や等式拘束を扱えるようにしたＲｅｃｅｄｉｎｇ Ｈｏｒｉｚｏｎ制御にスラック変数を導入するなどの方法を、具体的な数値解法として用いることができる。

図８には、本実施形態に係るスタビライズ方法を脚式ロボットの壁へのよりかかり動作に適用した結果を示している。この場合、床からの反力ｆ₁及び壁からの反力ｆ₂の概略値を目標値として与え、制御周期毎に２秒先までの未来予測値を与えている。そのｘ成分のみを同図Ｂのｆ₁及びｆ₂のグラフに示した。また、同図Ｂの最下段には、安定化の結果として得られた重心軌道のｘ成分を示した。安定化を行なうことによって、重心軌道が発散することなく、前によりかかった後、再度元の位置に戻っている様子が分かる。

また、図９には、本実施形態に係るスタビライズ方法を脚式ロボットの跳躍運動の安定化に適用した結果を示している。この場合、床からの反力ｆの概略値を目標値として与え、制御周期毎に２秒先までの未来予測値を与えている。なお、非接地期間に相当する期間では、Ｂ＝０（すなわち制御入力が０）となるようにＢについても時変として与えた。これは、非接地期間では、外界から外力作用が許されず、外力ｆ並びに外モーメントｎともに作用が許されないためである。よって、安定化は設置期間のｆ及びｎのみを入力として行なわれる。床反力目標値のｚ成分を、同図Ｂのｆ_zのグラフに示した。また、同図Ｂの最下段には、安定化の結果として得られた重心軌道のｚ成分を示した。安定化の結果、重心軌道が発散することなく、離床、着床している様子が分かる。

このように、本発明に係るスタビライズ方法によれば、外界との接触状態に依存せず、非常に広範な種類の運動を安定化することができる。

なお、本明細書中では言及しなかったが、特許文献２と同様に、センサから計測された運動状態を、上式（５）の強制項あるいは境界条件として与えることで、実運動状態を反映した、よりロバストな制御系を効率的に構成することもできる。

以上で明らかなように、本発明に係るスタビライザによれば、ロボットの作業内容や活動領域が拡大したり、力学シミュレータ内のキャラクタの安定化を統一的に行なったりすることが可能になる、という波及効果が望める。

以上、特定の実施形態を参照しながら、本発明について詳解してきた。しかしながら、本発明の要旨を逸脱しない範囲で当業者が該実施形態の修正や代用を成し得ることは自明である。

本明細書では、脚式ロボットに関する全身協調制御を行なう実施形態を中心に説明してきたが、本発明の要旨はこれに限定されるものではない。本発明に係る一般化スタビライズ方法は、コンピュータ・アニメーションにおけるキャラクタやその他の多リンク構造体の安定化処理に幅広く適用することができる。

要するに、例示という形態で本発明を開示してきたのであり、本明細書の記載内容を限定的に解釈するべきではない。本発明の要旨を判断するためには、特許請求の範囲を参酌すべきである。

図１は、本発明に係る安定判別規範の適用対象となるロボット装置の外観構成を示した図である。 図２は、図１に示したロボット装置における結線トポロジの構成例を示した図である。 図３は、図１に示したロボット装置の関節自由度モデルを示した図である。 図４は、系の各作用点ｌ_iが受ける外力ｆ_iを示した図である。 図５は、外モーメントｎ_jは作用せず、外力ｆ_iは既知として、系全体に並進力ｆとモーメントｎを入力した様子を示した図である。 図６は、本発明に係るスタビライズ処理を実現するシステムの機能的構成を模式的に示した図である。 図７は、図６に示したシステムによるスタビライズ処理の手順を示したフローチャートである。 本発明に係るスタビライズ方法を脚式ロボットの壁へのよりかかり動作に適用した様子を示した図である。 本発明に係るスタビライズ方法を脚式ロボットの壁へのよりかかり動作に適用した結果を示した図である。 本発明に係るスタビライズ方法を脚式ロボットの跳躍運動の安定化に適用した様子を示した図である。 本発明に係るスタビライズ方法を脚式ロボットの跳躍運動の安定化に適用した結果を示した図である。

符号の説明

１０…運動量安定化部
１１…運動量目標値時系列設定部
１２…作用点目標値時系列設定部
１３…状態方程式設定部
１４…評価関数設定部
１５…随伴状態ベクトル算出部
１６…制御入力算出部
１７…運動量算出部
２０…運動拘束解決部

Claims (16)

1. 複数の剛体が連なって構成される多リンク構造物の制御システムであって、
   前記多リンク構造物に関する系全体の運動量と制御入力との関係を記述する状態方程式を設定する状態方程式設定手段と、
   現在時刻から将来時刻に至る制御目標値を設定する予測制御目標値設定手段と、
   該制御目標値に基づく運動量の目標値と現実の運動量との差分に基づいて前記多リンク構造物の安定性を評価する安定性評価関数を設定する安定性評価関数設定手段と、
   前記状態方程式で構成される拘束条件下で、前記安定性評価関数の極小値を与える最適制御入力を決定する最適制御入力決定手段と、
   該最適制御入力が発生する最適運動量を算出する最適運動量算出手段と、
   該最適運動量を実現するための前記多リンク構造物の関節駆動方法を決定する関節駆動方法決定手段と、
   を具備することを特徴とする制御システム。
2. 前記予測制御目標値設定手段は、制御目標値として前記多リンク構造物の外力の作用点に関する情報を設定する、
   ことを特徴とする請求項１に記載の制御システム。
3. 前記予測制御目標値設定手段は、制御目標値として前記多リンク構造物に加わる外力又は外モーメントに関する情報を設定する、
   ことを特徴とする請求項１に記載の制御システム。
4. 前記予測制御目標値設定手段は、制御目標値として前記多リンク構造体の重心、重心速度、あるいは運動量に関する情報を設定する、
   ことを特徴とする請求項１に記載の制御システム。
5. 前記予測制御目標値設定手段は、制御目標値として前記多リンク構造体の角運動量に関する情報を設定する、
   ことを特徴とする請求項１に記載の制御システム。
6. 前記安定性評価関数設定手段並びに前記最適運動量算出手段は、運動量として前記多リンク構造体の並進運動量、重心又はその他の所定の点回りの角運動量を扱う、
   ことを特徴とする請求項１に記載の制御システム。
7. 制御入力として、前記多リンク構造体に作用する力又はモーメントを含む、
   ことを特徴とする請求項１に記載の制御システム。
8. 制御入力として、前記多リンク構造体全体に作用する力の作用点を含む、
   ことを特徴とする請求項１に記載の制御システム。
9. 前記安定性評価関数設定手段は、制御目標値の偏差を含む安定性評価関数を設定する、
   ことを特徴とする請求項１に記載の制御システム。
10. 前記安定性評価関数設定手段は、制御入力を含む安定性評価関数を設定する、
    ことを特徴とする請求項１に記載の制御システム。
11. 制御入力に関する不等式拘束条件を設定する不等式拘束条件設定手段をさらに備え、
    前記最適運動量算出手段は、該不等式拘束条件の下で最適運動量を算出する、
    ことを特徴とする請求項１に記載の制御システム。
12. 前記関節駆動方法決定手段は、運動量若しくは重心に関する操作空間慣性逆行列を用いて関節駆動方法を決定する、
    ことを特徴とする請求項１に記載の制御システム。
13. 前記関節駆動方法決定手段は、運動量若しくは重心に関するヤコビアンを用いて関節駆動方法を決定する、
    ことを特徴とする請求項１に記載の制御システム。
14. 前記関節駆動方法決定手段は、運動量以外の拘束条件をさらに設定し、運動量に関する拘束とそれ以外の運動拘束が同時に成立するように、関節駆動方法を決定する、
    ことを特徴とする請求項１に記載の制御システム。
15. 複数の剛体が連なって構成される多リンク構造物の制御方法であって、
    前記多リンク構造物に関する系全体の運動量と制御入力との関係を記述する状態方程式を設定する状態方程式設定ステップと、
    現在時刻から将来時刻に至る制御目標値を設定する予測制御目標値設定ステップと、
    該制御目標値に基づく運動量の目標値と現実の運動量との差分に基づいて前記多リンク構造物の安定性を評価する安定性評価関数を設定する安定性評価関数設定ステップと、
    前記状態方程式で構成される拘束条件下で、前記安定性評価関数の極小値を与える最適制御入力を決定する最適制御入力決定ステップと、
    該最適制御入力が発生する最適運動量を算出する最適運動量算出ステップと、
    該最適運動量を実現するための前記多リンク構造物の関節駆動方法を決定する関節駆動方法決定ステップと、
    を具備することを特徴とする制御方法。
16. 複数の剛体が連なって構成される多リンク構造物の制御に関する処理をコンピュータ・システム上で実行するようにコンピュータ可読形式で記述されたコンピュータ・プログラムであって、前記コンピュータ・システムに対し、
    前記多リンク構造物に関する系全体の運動量と制御入力との関係を記述する状態方程式を設定する状態方程式設定手順と、
    現在時刻から将来時刻に至る制御目標値を設定する予測制御目標値設定手順と、
    該制御目標値に基づく運動量の目標値と現実の運動量との差分に基づいて前記多リンク構造物の安定性を評価する安定性評価関数を設定する安定性評価関数設定手順と、
    前記状態方程式で構成される拘束条件下で、前記安定性評価関数の極小値を与える最適制御入力を決定する最適制御入力決定手順と、
    該最適制御入力が発生する最適運動量を算出する最適運動量算出手順と、
    該最適運動量を実現するための前記多リンク構造物の関節駆動方法を決定する関節駆動方法決定手順と、
    を実行させることを特徴とするコンピュータ・プログラム。
    

Images