JP4457617B2 - モーション生成システム及びロボットのモーション評価方法 - Google Patents

Description

本発明は、少なくとも１以上の可動部を備えたロボット装置に係り、特に、複数の可動脚を備え、所定の運動パターン（モーション）に従って姿勢の安定性を自律的に維持しながら動作するロボット装置及びロボットのモーション評価方法に関する。

さらに詳しくは、本発明は、ＺＭＰ安定度判別規範を満たす運動パターンを評価して計画通りに実行するロボット装置及びロボットのモーション評価方法に係り、特に、ＺＭＰ安定度判別規範を満たす運動パターンの実行時において実機上で関節アクチュエータに印加されるトルクや角速度を適切に算出して実行可能性をチェックするロボット装置及びロボットのモーション評価方法に関する。

電気的若しくは磁気的な作用を用いて人間の動作に似せた運動を行う機械装置のことを「ロボット」という。ロボットの語源は、スラブ語の"ＲＯＢＯＴＡ（奴隷機械）"に由来すると言われている。わが国では、ロボットが普及し始めたのは１９６０年代末からであるが、その多くは、工場における生産作業の自動化・無人化などを目的としたマニピュレータや搬送ロボットなどの産業用ロボット（ｉｎｄｕｓｔｒｉａｌ ｒｏｂｏｔ）であった。最近では、脚式移動ロボットに関する研究開発が進展し、実用化への期待も高まってきている。ヒトの動作をモデルにした脚式移動ロボットのことを、特に、「人間形」、若しくは「人間型」のロボット（ｈｕｍａｎｏｉｄ ｒｏｂｏｔ）と呼ぶ。

２足直立による脚式移動は、クローラ式や、４足又は６足式などに比し不安定で姿勢制御や歩行制御が難しくなるが、不整地や障害物など作業経路上に凹凸のある歩行面や、階段や梯子の昇降など不連続な歩行面に対応することができるなど、柔軟な移動作業を実現できるという点で優れている。

ここで、ロボット上で実現される動作パターン、あるいは、複数の基本的な動作パターンの組合せによって構成される高度且つ複雑な一連の動作シーケンスは、コンピュータ・プログラミングと同様の作業によって構築される。実機を動作するモーション・データが数多く用意されることが、ロボット本体が普及していくためには必須である。したがって、ロボット用のモーション編集を行なうための開発環境の構築が強く望まれている。

また、今後、産業界のみならず一般家庭や日常生活にもロボットが深く浸透していくことが予想される。とりわけ、エンターティンメント性を追求する製品に関しては、コンピュータやコンピュータ・プログラミングに関する高度な知識を持たない一般消費者層がロボットを購入して使用するケースが多いと予想される。このような一般ユーザにとっても、ロボットの動作シーケンスを対話的な処理により比較的容易且つ効率的に作成・編集することを支援するためのツール、すなわちモーション編集システムを提供することが好ましい。

ロボットは、関節アクチュエータなどの複数の制御点で構成されており、したがって、各制御点における位置や速度（関節角やその角速度）を入力していくことにより、機体動作を編集することができる。この点では、コンピュータ・グラフィックスにおけるキャラクタのアニメーション生成に類似する。しかしながら、仮想空間上での動作と実機動作とでは自ずと相違がある。脚式移動ロボットの場合には、単に関節角を駆動させただけでは所望の動作を実行することはできず、転倒することなく運動を安定化させながら脚式作業を継続することができるモーションである必要がある。

多くの場合、脚式移動ロボットの姿勢安定制御には、足底接地点と路面の形成する支持多角形の辺上あるいはその内側にモーメントがゼロとなる点を探索するというＺＭＰ安定度判別規範を用いる。

ＺＭＰによる安定度判別規範は、歩行系から路面には重力と慣性力、並びにこれらのモーメントが路面から歩行系への反作用としての床反力並びに床反力モーメントとバランスするという「ダランベールの原理」に基づく。力学的推論の帰結として、足底接地点と路面の形成する支持多角形（すなわちＺＭＰ安定領域）の辺上あるいはその内側にピッチ軸及びロール軸モーメントがゼロとなる点、すなわちＺＭＰが存在する（例えば、非特許文献１を参照のこと）。２足の脚式移動ロボットの場合、この支持多角形が極めて高いことから、姿勢安定制御はとりわけ困難とされている。

機体の各制御点における指示値を画面入力してロボットのモーションを組むというモーション編集システムは既に存在する。しかしながら、編集されたモーションで実機動作させる場合の姿勢安定性をチェックしたり、姿勢が安定化したりするように所望のモーションを修正するようなシステムは未だない。せっかく組まれたモーションでは機体の姿勢安定性を維持できず、モーション自体を実機上で実行することができないのでは、実質的にモーションを編集したことにはならない。したがって、コンピュータなどのモーション編集装置上で編集したモーションを、実機上で実演する前に、コンピュータ上で検証を行なう必要がある。すなわち、ＺＭＰ規範に基づいてモーションの実行可能性をコンピュータ上で事前に判定を行なえることが好ましい。

脚式ロボットの安定判別規範として、ＺＭＰ方程式の境界問題を適用したものは多いが、そのほとんどはトルク（又は力）の上限値や角速度（又は速度）の上限値などのアクチュエータの特性は考慮されていない。このため、ＺＭＰ方程式の解が実機の関節トルク及び関節角速度の上限値を超えていると、安定判別規範に基づく理論上は実行可能なはずの運動パターンであっても、実機上では計画通りに実行することができない。たとえ転倒することなく実行できたとしても、安定余裕が計画値より減少する結果となる。

このような問題に対し、周波数解析を採り入れて、運動パターンの実行可能性を検証する方法について提案がなされている（例えば、非特許文献２を参照のこと）。しかしながら、この方法では、アクチュエータに印加される負荷が考慮されないため、実機上での実行可能性を確実にチェックすることができない。また、この方法では、片脚は接地していることを前提としていることから（すなわち、２足歩行ロボットの単脚支持期のみをマニピュレータと扱ってトルク算出している）、マニピュレータと同じ扱いで問題を解決するようなものであり、両脚支持期の実行可能性を正確に判別することはできない。本発明者らは、実用的な実行可能性チェックを行なうためには、少なくとも、アクチュエータに印加されるトルクと角速度の両成分の相関関係を考慮する必要があると思料する。

ここで、アクチュエータに印加される負荷（トルク）を算出する方法は、順動力学演算を用いるものと、逆動力学演算を用いるものに大別することができる。

順動力学演算を用いる方法は、いわゆるシミュレーションを実行するものである。作成したモーションを実機に適用する際、事前に順動力学演算を用いて検証するシステムについて幾つか提案されている（例えば、非特許文献３を参照のこと）。

しかしながら、順動力学演算を用いる方法は、床反力パラメータの値によって結果が異なったり、転倒後のモーションの検証を行なうことができなかったり、計算量が多いなどの点で実用的でない。

一方、逆動力学演算を用いる方法は、順動力学演算に比べて計算量が少ない点でトルク算出手段として優れている。マニュピュレータのようにベースが固定されたリンク構造ならば周知の方法を適用することができる。しかし、脚式ロボットのように、機体の特定部位が常に地面に固定されず、多様な接地状態をとり得る系の場合、一般には、これをそのまま用いることはできない。

今日、脚式ロボットの制御手段としてＺＭＰ安定判別規範に基づくものが多い故、特に、運動目標値にＺＭＰを含む運動パターンから、逆動力学演算に基づいて脚式ロボットの要求トルクを適切に算出する手段が必要となっている。

ヴコブラトビッチ（Ｍｉｏｍｉｒ Ｖｕｋｏｂｒａｔｏｖｉｃ）著「脚式移動ロボット（ＬＥＧＧＥＤ ＬＯＣＯＭＯＴＩＯＮ ＲＯＢＯＴＳ）」（加藤一郎外著『歩行ロボットと人工の足』（日刊工業新聞社）） 若原、高西、加藤共著「歩行パターン作成支援システム「ＷＡＬＫ ＭＡＳＴＥＲ−２」の開発」（第３回日本ロボット学会学術講演会予稿集，２６３−２６６，１９８５） 金広他著「ＨＲＰ仮想ロボットプラットフォームを用いた制御ソフトウェア開発環境」（日本機械学会ロボティクスメカトロニクス講演会２００１講演論文集， ２Ｐ２−Ｈ２）

本発明の目的は、所定の運動パターン（モーション）に従って姿勢の安定性を自律的に維持しながら動作することができる、優れたロボット装置及びロボットのモーション評価方法を提供することにある。

本発明のさらなる目的は、ＺＭＰ方程式の境界問題を安定度判別規範として実行可能な運動パターンを評価して計画通りに実行することができる、優れたロボット装置及びロボットのモーション評価方法を提供することにある。

本発明のさらなる目的は、ＺＭＰ方程式の境界問題を安定度判別規範とした運動パターンの実行時において実機上で関節アクチュエータに印加されるトルクや角速度を適切に算出して実行可能性をチェックすることができる、優れたロボット装置及びロボットのモーション評価方法を提供することにある。

本発明は、上記課題を参酌してなされたものであり、その第１の側面は、少なくとも複数の可動部を備えたロボットであって、
該ロボットの運動設計値として与えられたＺＭＰ目標軌道を満足するように、機体の複数の点に印加される外力分布を決定する外力分布決定手段と、
該決定された外力が作用する条件下で逆動力学演算を実行することにより、前記可動部におけるトルクを算出する逆動力学演算手段と、
を具備することを特徴とするロボットである。

ロボットは、例えば前記可動部における目標とする運動パターンを記述したモーション・データを再現するという形態で駆動する。ロボットは、モーション実行時のＺＭＰ目標軌道を求める手段を備えていてもよい。また、前記可動部の実動作制御系と同一の構成をなし、モーション・データを入力してＺＭＰ目標軌道に関する情報を求める仮想動作制御手段をさらに備えていてもよい。

また、前記外力分布決定手段は、機体の複数の点に作用する垂直反力の２次形式を最小化するように、前記の各点に作用する垂直反力の値を決定するようにしてもよい。

また、前記外力分布決定手段は、機体の複数の点に作用する摩擦力が均一になるように、前記の各点に作用する摩擦力の値を決定するようにしてもよい。

そして、前記逆動力学演算手段により算出されたトルクに基づいて、モーション・データが前記可動部の動作性能の範囲内で実行可能か否かを判定することができる。

したがって、本発明によれば、ＺＭＰ方程式に基づいて制御される脚式ロボットのモーション・パターンから、逆動力学演算に基づいて、接地状態に依存することなく統一的に関節トルク又は力を算出することができる。

そして、算出されたトルクと、モーション・パターンから得られた関節角速度をＮ−Ｔ線図上にマッピングし、両者の領域判定を行なうことで、より広範なモーションに対して、確実で高速なモーション実行可能性チェックが可能となる。

また、前記逆動力学演算手段により制御周期毎に算出されたトルクを最小にするような実時間歩容の自動生成を行なうことができる。

また、前記逆動力学演算手段により制御周期毎に算出されたトルクを基に、前記可動部における消費電力を算出することができる。

また、前記逆動力学演算手段により制御周期毎に算出されたトルクを基に、前記可動部における温度上昇を管理することができる。

本発明によれば、ＺＭＰ安定度判別規範を満たす運動パターンの実行時において実機上で関節アクチュエータに印加されるトルクや角速度を適切に算出して実行可能性をチェックすることができる、優れたロボット装置及びロボットのモーション評価方法を提供することができる。

本発明によれば、目標値にＺＭＰ軌道を有するロボットの運動パターンから、全関節の必要トルクを算出することができる。この情報を基に、モーション作成システムで作成した運動パターンを実際のアクチュエータの出力特性の範囲内で実行可能かどうかをより精密に判定することが可能となり、アクチュエータに過度の負荷がかかるモーションを実機で実行する以前に排斥することができる。この結果、作成したモーションの機体上での再現性を向上させることができ、効率的なモーション作成に寄与する。また、機体の破損率も低減することができる。

本発明では、逆動力学演算を基本としていることから、いわゆるダイナミクス・シミュレーションに比較して高速なトルク算出が可能である。また、シミュレーションでは転倒に至るモーションであっても、本発明によればモーションの最後までトルク算出ができることから、一度にモーションのすべての問題箇所を検出することができる。したがって、モーション修正の作業効率を上げることができる。

また、本発明では、与えられたＺＭＰを実現する外力分布を求め、空に浮いた分岐マニュピュレータ・モデルに外力を作用させるという演算方式を採っていることにより、任意の接地状態を扱うことができる一般性を備えている。したがって、多脚が同時に接地するなど、路面とロボットの間に閉ループが構成される場合も統一的に扱うことができ、汎用性の高いトルク算出が可能である。

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

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

図１には、本発明の実施に供される２脚２腕を有する人間型ロボットの自由度構成を示している。

本実施形態に係るロボットは、基底（又は基体）Ｂから放射状に回転ジョイントを介して開リンク鎖が連なる構造をなし、７自由度を持つ腕部と、６自由度を持つ足部と、３自由度を持つ腰部と、２自由度を持つ頭部とで構成される。

基底Ｂは、左右の股関節位置を結ぶ線と体幹ヨー軸との交点で定義される。脚部は、基底Ｂに接続され、股関節３自由度（ヨー・ロール・ピッチ）、膝関節１自由度（ピッチ）、足首関節２自由度（ピッチ・ロール）で構成される。腰部は、３自由度（ピッチ・ロール・ヨー）で構成され、基底Ｂと胸部Ｃを接続する．腕部は、胸部Ｃに接続され、肩関節３自由度（ピッチ・ロール・ヨー）、肘関節２自由度（ピッチ・ヨー）、手首関節２自由度（ロール・ピッチ）で構成される。頭部は、胸部Ｃに接続され、首関節２自由度（パン・チルト）で構成される。

図示のロボットは、腰部位置に重心が設定されており、姿勢安定制御の重要な制御対象点であるとともに、装置の「基体」を構成する。

基体には、加速度センサＡ１及びジャイロＧ１が取り付けられている。また、左右の足底の四隅には足底面垂直方向の床反力を検出する１軸ロードセル（Ｆ１〜Ｆ８）と、床面までの距離を測定する赤外線測距センサ（Ｄ１〜Ｄ８）がそれぞれ４つ取り付けられている。また、左右の足底中央部には、それぞれ加速度センサ（Ａ２、Ａ３）及びジャイロ（Ｇ２、Ｇ３）が取り付けられている。

図２には、本実施形態に係るロボットの歩行動作などを司る動作制御システムの機能構成を模式的に示している。同システムは、大別して、ロボット状態観測部Ｂ２１と、環境形状観測部Ｂ２２と、外力・外モーメント観測部Ｂ２３と、実時間歩行パターン生成部Ｂ２５と、上体・足部要求運動入力部Ｂ２４と、全身関節駆動部Ｂ２６で構成される。

ロボット状態観測部Ｂ２１は、上述した種々のセンサ情報を基に、実際のロボットの運動状態を計測し、その計測結果を実時間歩行パターン生成部Ｂ２５へ出力する。

環境形状観測部Ｂ２２は、上述した種々のセンサ情報を基に、路面の傾斜や段差、高さなどの環境形状を算出し、その計測結果を実時間歩行パターン生成部Ｂ２５へ出力する。

外力・外モーメント観測部Ｂ２３は、主にロボット上に搭載された力センサや加速度センサからのセンサ情報を基に、ロボットに対して外界から作用する外力や外モーメントを算出し、その算出結果を実時間歩行パターン生成部Ｂ２５へ出力する。

上体・足部要求運動入力部Ｂ２４は、ユーザ・プログラムにより時々刻々と決定される上体への運動要求と、歩幅や歩行周期、機体の旋回角などの足部の運動に関る歩容パラメータを入力し、実時間歩行パターン生成部Ｂ２５へ出力する。

実時間歩行パターン生成部Ｂ２５は、外力・外モーメント観測部Ｂ２３により検出された外力下にて力学的なバランス平衡を維持しつつ、現在のロボットの運動状態から滑らかに遷移可能で、環境形状に係る運動学的拘束と、ユーザ・プログラムからの上肢・下肢運動変更要求をともに満足することができるような、次時刻のロボットの運動状態を決定し、全身の関節角参照値として出力する。

全身関節駆動部Ｂ２６は、実時間歩行パターン生成部Ｂ２５の出力した関節角参照値を実現するよう、各関節アクチュエータをサーボ・コントローラにより駆動する。

図３には、実時間歩行パターン生成部Ｂ２５の機能構成を詳解している。同図に示すように、実時間歩行パターン生成部Ｂ２５は、大別して、足底位置・姿勢軌道生成部Ｂ４１と、ＺＭＰ方程式求解部Ｂ４２と、質点分布調整部Ｂ４３と、関節角算出部Ｂ４４と、現在状態決定部Ｂ４５で構成される。

足底位置・姿勢軌道生成部Ｂ４１には、上体・足部要求運動入力部Ｂ２４から歩幅、歩行周期、旋回角、足上げ高さなどの足部要求運動に関る歩容パラメータが入力されるとともに、環境形状観測部Ｂ２２より得られた路面高さ、路面傾斜などの環境形状が入力される。足底位置・姿勢軌道生成部Ｂ４１は、これらの入力値を用いて、一歩前の着床状態から数歩先までの左右両足底の位置及び姿勢軌道を算出する。

ＺＭＰ方程式求解部Ｂ４２では、足底位置・姿勢軌道生成部Ｂ４１で生成された足底位置姿勢軌道、歩容パラメータ、ロボットの現在の状態、外力印加状態に基づいて、機体の力学的バランス平衡を維持しつつ、これらの条件を満足するための重心位置・速度すなわち次時刻のロボットの状態ベクトルを算出する。

質点分布調整部Ｂ４３では、ＺＭＰ方程式求解部Ｂ４２で得られた次時刻の状態ベクトルを実現し、且つ、ユーザ・プログラムより指定された上肢参照軌道（例えば関節角軌道）と足底軌道（歩容パラメータにより与えられる）を満足するよう、全身の質点分布状態を調整する。

関節角算出部Ｂ４４では、質点分布調整部Ｂ４３で得られた次時刻の基体位置と、足底位置・姿勢軌道生成部Ｂ４１より得られる次時刻の足底位置姿勢を実現するよう、脚部の関節角を決定する。これは、両者の相対位置姿勢を求めた後、例えば逆キネマティクス計算（周知）によって実行することができる。ここで得られた脚関節角と、ユーザ・プログラムより与えられた上肢の参照関節角とを併せて、次時刻における全身関節角参照値として出力する。全身関節角参照値は全身関節駆動部Ｂ２６の関節角参照値として用いられる。

現在状態決定部Ｂ４５は、各種センサ値を基に算出されたロボットの現在状態ベクトルと、前回の制御サイクルでＺＭＰ方程式求解部Ｂ４２において算出され、参照値として用いられた状態ベクトルより、次回の制御サイクルでＺＭＰ方程式求解部Ｂ４２の始端条件として用いるべき現在状態ベクトルを決定・更新する。すなわち、各種センサ値を基に算出されたロボットの現在状態ベクトルと、ＺＭＰ方程式求解部Ｂ４２において算出された状態ベクトルは一般に異なるため、その調整演算を行なう。ロボットの実状態を反映する必要の無い場合は、ＺＭＰ方程式求解部Ｂ４２の出力を現在状態ベクトルとして出力するのみでよい。

モーション作成システムでは、上述した歩行制御系のようなロボット制御系のためのモーションを作成するが、モーション実行時に初めて確定するような実環境下特有の不確定要素を取り除いて扱うのが一般的である。上述した制御系で言えば、ロボット状態観測部Ｂ２１と、環境形状観測部Ｂ２２と、外力・外モーメント観測部Ｂ２３からの出力は無いとした、一種の理想環境内でのモーション設計が行なわれる。すなわち、図２及び図３でハッチングが施された部分の制御のみが仮定されてモーション作成が行なわれる。

よって、モーション作成システムは、実時間歩行パターン生成部Ｂ２５に入力される、上体及び足部に対する要求運動をデザインすることが主な機能となる。

ここで、上体運動は、例えば図４に示すように、上体関節角時系列として与えられる。但し、図４では、図面の簡素化のため１関節分のみを示している。各関節角時系列をスプライン曲線などのパラメータ曲線として表現することで、比較的少数のパラメータで多様な軌道を表現することができる。モーション作成システムは、このような上体運動表記が行なわれる制御系に対して、スプライン曲線のノード情報、すなわち、時刻と値のリスト｛（ｔ_i，ｑ_i）｜ｉ＝１，２，…｝を作成・編集する機能を提供する。

一方、足部の運動は、例えば図５に示すように、実時間歩行パターン生成部Ｂ２５に入力される歩容パラメータ付歩行コマンドと、その発行時刻のリストとして与えられる。歩行コマンドは、「歩行」及び「歩行停止」からなり、各コマンドはパラメータ引数として、ステップ毎の歩行周期、歩幅、旋回角などの歩容パラメータを有する。モーション作成システムは、コマンドの種別、その発行時刻と歩容パラメータを作成・編集する機能を提供する。

このようにして作成された上体及び足部に対する要求運動を実ロボットの制御系に入力することで基本的にはモーション作成システム上で計画されたモーションを実ロボット上で再現することができる。しかし、前述したように、モーション作成システムは、実ロボットのアクチュエータ性能を考慮しないと、過剰なトルクや関節角速度を要求するモーションの作成を許容してしまう可能性がある。図２及び図３に示したロボットの動作制御系は外乱やモデル化誤差などに対する適応制御機能が備わっているため、ある程度の不安定化要因を自律的に解決する能力を有している。しかしながら、一般には、このような過剰スペックのモーションを実ロボットに適用した場合、計画通りのモーションを実機上で再現できる保証はない。

そこで、本実施形態に係るモーション作成システムでは、さらに実機上での高い再現率を保証するために、モーション作成システム内に実機制御系の図２並びに図３においてハッチングで示した機能モジュール部分を包含し、実機上でのモーション再現結果を同システム上で数値的・視覚的に確認できるようにするとともに、モーション再現の結果が実ロボットのアクチュエータ性能にて実現可能かどうか自動判定するように構成した。

図６には、本発明の一実施形態に係るモーション作成システムの機能構成例を示している。同図に示すように、モーション作成システムは、モーション再現の結果が実ロボットのアクチュエータ性能にて実現可能かどうか自動判定する実行可否判定部Ｂ６４を有している。

上体運動軌道及び歩行コマンド時系列は、ユーザがモーション作成システムを用いて作成・編集したデータである。これらのデータは仮想動作再生部Ｂ６１により制御サイクル毎に抽出され、仮想歩行制御系に入力される。

仮想歩行制御系は、実機上の歩行制御系と同一の構成をなし、上体・足部要求運動入力部Ｂ６２と実時間歩行パターン生成部Ｂ６３で構成される。この仮想歩行制御系に上体運動及び歩行コマンドを入力することで、解として全身関節角が制御周期毎に出力される。同時に、求解の副産物として、制御周期毎に、ＺＭＰ位置、重心加速度、基底位置・姿勢、足底位置・姿勢及び接地状態（左足支持、右足支持、両足支持）に関する軌道情報が容易に得られる。上記副産物は図中では破線枠で示されている。

実行可否判定部Ｂ６４は、これらの軌道情報に基づいて、作成されたモーションが実ロボットのアクチュエータ性能にて実現可能かどうかを自動判定する。

図７には、上体運動軌道と歩行コマンド時系列からモーションの実行可否判定を得るための処理手順をフローチャートの形式で示している。

時刻を初期化した後（ステップＳ７０）、ユーザの設計した上体運動軌道から時刻ｔの値を抽出し，仮想歩行制御系に入力する（ステップＳ７１）。

次いで、時刻ｔに発行する歩行コマンドがユーザによって設定されているかどうかをチェックする（ステップＳ７２）。歩行コマンドが設定されている場合は 、仮想歩行制御系にこれを入力する（ステップＳ７３）。また、設定されていない場合は、仮想歩行制御系に歩行コマンドは入力しない。

次いで、仮想歩行制御系を１制御周期（ｔｉｃｋ）分だけ実行し（ステップＳ７４）、時刻ｔにおける全身関節角、ＺＭＰ位置・重心加速度、基底位置・姿勢、接地状態に関するデータを得る。そして、得られたデータを基に、時刻ｔの運動状態が実ロボットのアクチュエータ性能にて実現可能かどうかの判定処理（実行可否判定）を行なう（ステップＳ７５）。

ここで、実行不能の判定結果が得られた場合は、「モーション実行不能」との結果を提示し、処理を終了する。一方、実行可能の判定結果が得られた場合は、時刻を制御周期（ｄｔ）分だけインクリメントする（ステップＳ７６）。そして、全時刻分の処理が完了したか判定し（ステップＳ７７）、完了していない場合にはステップＳ７１に戻り、次の制御周期における実行可否判定の処理を繰り返す。

モーションの最後に至るまで実行不能の判定が発生しなかった場合には（ステップＳ７５）、「モーション実行可能」の判定結果を提示し、本処理ルーチン全体を終了する。

本発明の主要部分は、モーション作成システム（図６を参照のこと）における実行可否判定部Ｂ６４、並びに、モーション実行可否判定処理手順（図７を参照のこと）のステップＳ７５における制御周期毎の実行判定処理ステップの構成法にある．以下では，これらの実行可否判定処理の詳細について述べる。

図８には、実行可否判定部Ｂ６４の機能構成を示している。実行可否判定部Ｂ６４は、接地点検出部Ｂ８１と、荷重（外力）分布決定部Ｂ８２と、微分器Ｂ８３と、逆動力学演算部Ｂ８４と、ＮＴ領域判定部Ｂ８５で構成される。

接地点検出部Ｂ８１は、実時間歩行パターン生成部Ｂ６３の副産物として得られた、時刻ｔにおける足底接地状態及び足底位置姿勢情報より、接地点情報を出力する。ここで、足底リンクには、接地点候補があらかじめ設けられているものとする。接地点候補には，各足底の凸包の頂点などを用いると良い。例えば，図１に示した各ロードセル（Ｆ１〜Ｆ８）の位置に接地点候補を設定する。接地点検出部は、以下のような接地点情報を作成・出力する。

左足接地時：
接地点情報＝（（接地リンク＝左足底リンク，接地点＝（Ｆ１〜Ｆ４）））
右足接地時：
接地点情報＝（（接地リンク＝右足底リンク，接地点＝（Ｆ５〜Ｆ８）））
両足接地時：
接地点情報＝（（接地リンク＝左足底リンク，接地点＝（Ｆ１〜Ｆ４））
（接地リンク＝右足底リンク，接地点＝（Ｆ５〜Ｆ８）））

荷重分布決定部Ｂ８２は、実時間歩行パターン生成部Ｂ６３の副産物として得られた、時刻ｔにおけるＺＭＰ位置及び重心加速度と、接地点検出部Ｂ８１の出力した接地点情報を用いて、各接地点へ作用する反力分布を求める。一般に、ＺＭＰ位置が与えられたとき、そのＺＭＰ位置を実現する荷重分布は一意には定まらないため、例えば、以下の最適化問題を解くことで決定する。

ここで、ｆ_ext,iZはｉ番目の接地点における垂直反力、Ｆ_zは総垂直反力、ｃ_xi及びｃ_yiはそれぞれｉ番目の接地点のＸ，Ｙ座標、ｐ^ZMP _x、ｐ^ZMP _yはそれぞれＺＭＰのＸ，Ｙ座標を表している。上式は、全接地点の荷重合計が総床反力に等しく、ＺＭＰ回りのトルクが０になるという制約条件下で、各接地点の荷重の２乗和が最小となるような荷重分布を求めるものである。

このような最適化問題は、例えばＬａｇｒａｎｇｅ乗数法や種々の２次計画法を用いて容易に解くことができる。総床反力Ｆ_zは、実ロボットの質量と、重心の垂直方向加速度と、重力加速度に基づいて、以下の式で容易に求められる。

上式（１）に示した最適化問題を解くことで、各接地点（ｃ _xi ，ｃ _yi ）に床面から作用する垂直反力ｆ_Zi、すなわち床反力分布が決定される。摩擦力に関しても、与えられた重心加速度を満足する摩擦力の分布は一意には定まらないが、例えば、下式に示すようにロボットに作用する水平方向の力すなわち摩擦力が接地点に平均的に作用するとして求めることができる。

ここで、Ｍは接地点総数、ｆ_ext,iX、ｆ_ext,iYはそれぞれ接地点ｉに作用するＸ及びＹ各方向の摩擦力である。Ｆ_x、Ｆ_yはロボットに作用する水平方向の力の総和であり、以下の式で求められる。

荷重分布決定部Ｂ８２は、以上の算出結果を基に、以下のような情報のリストを作成し、荷重分布情報として出力する。

（接地リンク，接地点ｐ₁，荷重値ｆ_ext,1）
（接地リンク，接地点ｐ₂，荷重値ｆ_ext,2）
...（接地リンク，接地点ｐ_N，荷重値ｆ_ext,3）

微分器Ｂ８３は、仮想歩行制御系で算出された全身関節角及び基底位置・姿勢を数値微分し、全身関節角速度・角加速度、基底速度・加速度を算出する。ここでは、基底の姿勢は図９に示すように長さ０の３つの仮想リンクの関節角によって表現し、全身関節角をベクトルθ＝（θ_１．．．θ_N）^Tで表すものとする。このように仮想関節の概念を導入することにより、逆動力学演算の中で基底の姿勢を通常の関節として扱うことができる。但し、Ｎは仮想関節を含めた総関節数であり、図９の例ではＮ＝３４である。また、制御サイクル（離散時刻）ｋにおけるθの値をθ（ｋ）とすると、その速度及び加速度は以下の式で再帰的に求められる。但し、ｄｔは制御周期を表している

また、基底の位置を図９のｐ０によって表現すると、その１階微分、２階微分の値も同様にして数値微分によって得ることができる。

微分器Ｂ８３は、関節角度・角速度・角加速度、基底位置・速度・加速度、基底姿勢角・基底角速度・基底角加速度の値として、θ（ｋ）、ｄθ（ｋ）、ｄｄθ（ｋ）、ｐ０（ｋ）、ｄｐ０（ｋ）、ｄｄｐ０（ｋ）の値を出力する。

逆動力学演算部Ｂ８４は、以上のようにして求められた荷重分布情報、関節角度・角速度・角加速度、基底位置・速度・加速度、基底姿勢角・基底角速度・基底角加速度を基に、各関節に要求されるトルクを算出する。より具体的には、ロボットの状態を図９に示したように宙に浮いた枝分かれのあるマニピュレータに、接地点に作用する外力ｆ_ext,i（ｉ＝１…Ｍ）が印加されているとしてモデル化し、以下の手順（１）〜（４）により逆動力学演算を実行する。

（１）基底の角速度ω₀、角加速度ｄω₀、並進加速度ｄｄｐ０を求める。並進加速度は、微分器Ｂ８３からの出力に重力加速度ｇ分を加算する。

（２）基底から末端リンクに向かって再帰的にリンクｉの角速度ω_i、角加速度ｄω_i、並進角速度ｄｄｐｉ、重心加速度ｄｄｒｉを求める。

但し、ｐはリンクｉの親リンクのインデクスを表し、ｑ_iはリンクｉのジョイントの回転軸方向ベクトル、ｌ_iはリンクｐからリンクｉへの相対位置ベクトル、ａ_iはリンクｉの原点からリンクｉの重心への相対位置ベクトルを表している。

（３）末端リンクについてリンクｉのジョイントに作用する力ｆ_i、モーメントｎ_i、トルクτ_iを求める。

但し、ｍ_iはリンクｉの質量、Ｌ_iはリンクｉに作用する外力のインデクス集合、Ｉ_iはリンクｉの慣性テンソル、ｓ_jはリンクｉの原点から外力ｆ_ext,jの作用点への相対位置ベクトル、Ｊ_i及びＤ_iはアクチュエータ内の慣性、粘性抵抗係数を表す。

（４）末端リンクから基底に向かって、再帰的にリンクｉのジョイントに作用する力、モーメント、トルクを求める。

但し、Ｍ_iはリンクｉの子リンクのインデクス集合を表すものとする。

以上のような手順（１）〜（４）からなる逆動力学演算に従って、すべての関節トルクが算出される。本演算はＯ（Ｎ）の計算複雑度のため、順動力学計算に比べて高速なトルク算出が可能となる。なお、逆動力学演算の詳細については、例えば、長阪憲一郎著「動力学フィルタによる人間型ロボットの全身運動生成」（東京大学工学部情報工学専攻博士論文，１９９９）を参照されたい。

最後に、Ｎ−Ｔ領域判定部Ｂ８５は、作成されたモーションが実機のアクチュエータで実行可能なパターンかどうかを、逆動力学演算部Ｂ８４で得られた各関節のトルク情報と微分器Ｂ８３で得られた各関節の角速度情報に基づいて、判定する。アクチュエータの静特性は、一般にＮ−Ｔ線図で与えられる。図１０には、Ｎ−Ｔ線図を例示している。同図のハッチ領域内であれば、アクチュエータの出力が得られるが、それ以外の領域の回転数とトルクの組み合わせでは出力を得ることができない。よって、すべての関節について、回転数とトルクをこのようなＮ−Ｔ線図上にマッピングし、すべての点が所定のハッチ領域内に存在するかどうか判定を行なうことにより、比較的容易に実行可否を判定することができる。関節ｉの回転数Ｎ_iは関節角速度ｄθ_iにより、以下の式で求められる。

よって、関節ｉの発生トルクをτ_i、起動トルクをＴ_0i、無負荷回転数をＮ_0iとすると、下式に示す判定を実行すればよい。

すべての実関節（図９中の関節θ₄からθ₃₄）についてＮ−Ｔ線図に基づく判定を行なう。そして、実行不能な関節が存在しない場合にはＮ−Ｔ領域判定部Ｂ８５は「実行可能」を判定結果として出力するが、それ以外の場合は「実行不能」を判定結果として出力する。

図１１には、実行可否判定部Ｂ６４における上述した処理の流れをフローチャートの形式で示している。

足底接地状態及び足底姿勢情報より、接地点情報を作成した後（ステップＳ１０１）、式（１）の２次計画問題を解いて、各接地点に作用する垂直反力分布を求める（ステップＳ１０２）。

次いで、荷重等分配則に基づいて摩擦力分布を求める（ステップＳ１０３）。続いて、式（５）及び式（６）を用いて全身関節角及び基底位置・姿勢を数値微分し、全身関節角速度・角加速度，基底速度・加速度を算出する（ステップＳ１０４）。

次いで、基底から末端に向かって式（式）及び式（８）の漸化式を用いて逆動力学演算の正順計算を実行する（ステップＳ１０５）。また、これとは逆に、末端から基底に向かって式（９）及び式（１０）の漸化式を用いて逆動力学演算の逆順計算を実行し（ステップＳ１０６）、すべてのジョイントにおけるトルクの算出を完了する。なお、この際、ステップＳ１０２並びにＳ１０３で算出した荷重分布を外力として作用させる。

最後に、すべての関節に関して関節角速度並びにトルクを式（１２）に示した判定にかける。そして、実行不能な関節が存在しない場合は「実行可能」を判定結果とし、それ以外の場合は「実行不能」として処理を完了する。

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

本明細書中では、ＺＭＰ目標軌道を実現するときの外力分布に基づいて逆動力学演算により求められた全関節のトルクを、モーション作成システムで編集されたモーションなど運動パターンの実行可否の評価に用いているが、本発明の要旨はこれに限定されるものではない。例えば、本発明に係る方式により算出されたトルクを最小にするような実時間歩容の自動生成に利用することができる。あるいは、制御周期毎のトルク算出を行ない、アクチュエータの消費電力を算出したり、さらには機体駆動用バッテリの残存容量やアクチュエータの温度上昇を算出したりすることができる。

本発明の要旨は、必ずしも「ロボット」と称される製品には限定されない。すなわち、電気的若しくは磁気的な作用を用いて人間の動作に似せた運動を行なう機械装置あるいはその他一般的な移動体装置であるならば、例えば玩具などのような他の産業分野に属する製品であっても、同様に本発明を適用することができる。

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

図１は、本発明の実施に供される２脚２腕を有する人間型ロボットの自由度構成を示した図である。 図２は、本発明に係るロボットの動作制御システムの機能構成を模式的に示した図である。 図３は、実時間歩行パターン生成部Ｂ２５の機能構成を詳解した図である。 図４は、上体関節角時系列の一例を示した図である。 図５は、足部の運動の記述例を示した図である。 図６は、本発明の一実施形態に係るモーション作成システムの機能構成を模式的に示した図である。 図７は、上体運動軌道と歩行コマンド時系列からモーションの実行可否判定を得るための処理手順を示したフローチャートである。 図８は、実行可否判定部Ｂ６４の機能構成を示した図である。 図９は、基底の姿勢を長さ０の３つの仮想関節によって表現した様子を示した図である。 図１０は、Ｎ−Ｔ線図を示した図である。 図１１は、実行可否判定部Ｂ６４における処理の流れを示したフローチャートである。

Claims (9)

1. 少なくとも上体と複数の可動脚を備えたロボットのモーション生成システムであって、前記上体の各関節角の時系列を記述した上体要求運動と前記可動脚の足部のステップ毎の歩容パラメータの時系列を記述した足部要求運動からなるモーション・データの前記ロボット上での実行の可否を判定するために、
   ＺＭＰ安定度判別規範に従って前記モーション・データを前記ロボット上で実行するための制御周期毎の全身の関節角の参照値を求解するとともに、その際の副産物として、制御周期毎のＺＭＰの位置及び重心加速度、基底の位置及び姿勢、前記可動脚の足底の位置、姿勢及び接地状態を含む軌道情報を出力する実時間歩行パターン生成部と、
   前記可動脚の足底の位置、姿勢及び接地状態の情報に基づいて、接地点の位置を判定する接地点検出部と、
   ＺＭＰの位置及び重心加速度の情報と前記接地点検出部が判定する接地点の情報に基づいて、各接地点に作用する外力分布を求める外力分布決定部と、
   全身関節角と基底の位置及び姿勢を数値微分して、全身関節角速度、各加速度、基底の速度及び加速度を算出する微分器と、
   各接地点に作用する外力分布と、全身の関節角、角速度、角加速度と、基底の位置、姿勢、加速度、基底の姿勢角、角速度、角加速度に基づいて、各関節に要求されるトルクを逆動力学演算により算出する逆動力学演算部と、
   前記の算出された各関節のトルク及び角速度を各関節で使用するアクチュエータのトルクをその回転数の組み合わせをＮ−Ｔ線図上で照合して、該当するアクチュエータの性能の範囲内で実行可能か否かを判定するＮＴ領域判定部と、
   を具備することを特徴とするモーション生成システム。
2. 前記外力分布決定部は、複数の前記接地点に作用する垂直反力の２次形式を最小化するように、前記の各接地点に作用する垂直反力の値を決定する、
   ことを特徴とする請求項１に記載のモーション生成システム。
3. 前記外力分布決定部は、複数の前記接地点に作用する摩擦力が均一になるように、前記の各接地点に作用する摩擦力の値を決定する、
   ことを特徴とする請求項１に記載のモーション生成システム。
4. 前記逆動力学演算部により制御周期毎に算出されたトルクを最小にするようなモーション・データの自動生成を行なう運動安定化手段をさらに備える、
   ことを特徴とする請求項１に記載のモーション生成システム。
5. 前記逆動力学演算部により制御周期毎に算出されたトルクを基に、前記アクチュエータにおける消費電力を算出する電力管理手段をさらに備える、
   ことを特徴とする請求項１に記載のモーション生成システム。
6. 前記逆動力学演算部により制御周期毎に算出されたトルクを基に、前記アクチュエータにおける温度上昇を管理する温度管理手段をさらに備える、
   ことを特徴とする請求項１に記載のモーション生成システム。
7. 少なくとも上体と複数の可動脚備えたロボットにおける前記上体の各関節角の時系列を記述した上体要求運動と前記可動脚の足部のステップ毎の歩容パラメータの時系列を記述した足部要求運動からなるモーション・データを評価するロボットのためのモーション評価方法であって、
   ＺＭＰ安定度判別規範に従って前記モーション・データを前記ロボット上で実行するための制御周期毎の全身の関節角の参照値を求解するとともに、その際の副産物として、制御周期毎のＺＭＰの位置及び重心加速度、基底の位置及び姿勢、前記可動脚の足底の位置、姿勢及び接地状態を含む軌道情報を出力する実時間歩行パターン生成ステップと、
   前記可動脚の足底の位置、姿勢及び接地状態の情報に基づいて、接地点の位置を判定する接地点検出ステップと、
   ＺＭＰの位置及び重心加速度の情報と前記接地点検出部が判定する接地点の情報に基づいて、各接地点に作用する外力分布を求める外力分布決定ステップと、
   全身関節角と基底の位置及び姿勢を数値微分して、全身関節角速度、各加速度、基底の速度及び加速度を算出する微分ステップと、
   各接地点に作用する外力分布と、全身の関節角、角速度、角加速度と、基底の位置、姿勢、加速度、基底の姿勢角、角速度、角加速度に基づいて、各関節に要求されるトルクを逆動力学演算により算出する逆動力学演算ステップと、
   前記の算出された各関節のトルク及び角速度を各関節で使用するアクチュエータのトルクをその回転数の組み合わせをＮ−Ｔ線図上で照合して、該当するアクチュエータの性能の範囲内で実行可能か否かを判定するＮＴ領域判定ステップと、
   を有することを特徴とするロボットのためのモーション評価方法。
8. 前記外力分布決定ステップでは、複数の前記接地点に作用する垂直反力の２次形式を最小化するように、前記の各接地点に作用する垂直反力の値を決定する、
   ことを特徴とする請求項７に記載のモーション評価方法。
9. 前記外力分布決定ステップでは、複数の前記接地点に作用する摩擦力が均一になるように、前記の各接地点に作用する摩擦力の値を決定する、
   ことを特徴とする請求項７に記載のモーション評価方法。
   

Images