JP2009285816A - 脚式ロボット及びその制御方法 - Google Patents

Abstract

【課題】ロボットが転倒する危険性を迅速かつ正確に評価し、さらに、転倒時のロボットに対する衝撃を低減すること。
【解決手段】本発明に係るロボット２は、ＺＭＰ規範に基づいてロボット２の安定化制御を行うロボットであって、ロボット２をモデル化した方程式であって、ロボット２の状態を示す状態変数を含む状態方程式を用いて、現時点から所定の時間先の物理量を示す予測物理量を計算する予測物理量計算手段２１と、予測物理量計算手段で計算した予測物理量に基づいてロボット２の転倒危険性を判断する転倒判断手段２２と、を備え、予測物理量計算手段２１は、状態方程式に所定の制御入力を与えて該制御入力に対する状態方程式の出力を取得し、該取得した状態方程式の出力と、観測するロボット２の観測物理量とに基づいて状態変数を推定し、該推定した状態変数に基づいて予測物理量を計算する。
【選択図】図２

Description

本発明は脚式ロボット及びその制御方法に関し、特に脚式ロボットの転倒危険性を評価する技術に関する。

脚式ロボットの実世界の運用では、予期せぬ大きな外乱や路面の不整地の影響を受けて、ロボットは常に転倒する危険性をはらんでいる。従って、ロボットの制御においては、歩行中の安定性のみならず、ロボットが転倒しそうな時に転倒を回避するための対策や、転倒が回避できない時の対策が必要となる。

転倒しやすい脚式ロボットについては、転倒を回避し、安定して移動することを目的として、安定化制御技術について、多くの研究がなされている。一般的に、ＺＭＰ（Zero Moment Point）を用いた脚式ロボットの安定化制御技術は、ＺＭＰが両足の支持多角形内に余裕をもって収まっている場合には、有効である。しかし、大きな外乱等によってＺＭＰが両足の支持多角形の境界まで動いてしまったときには、安定化のために必要な床反力を得ることができず、そのまま転倒してしまうことがある。また、これらの研究には、歩行中の外乱に対してどれだけ安定性を保持して歩行を続けることができるかに主眼が置かれたものが多く、転倒回避や、転倒が回避できないときにどうするか等の実用的な手法についてはあまり考慮がなされていない。

従来、姿勢角や、姿勢角速度や、床反力などの目標値と実測値との偏差から、脚式ロボットの転倒を判別する手法が良く知られている。これらの手法では、一般的に、目標値と実測値との偏差が所定の閾値を超えた場合に、脚式ロボットが転倒しそうな状態であるものと判断することが多い。また、例えば特許文献１には、ロボットに搭載されたバッテリの残容量に応じて脚式移動ロボットの転倒可能性を判別し、転倒しそうな状態であると判断した場合には、転倒回避動作を実行する脚式移動ロボットの制御装置が開示されている。特許文献１記載の脚式移動ロボットの制御装置では、ロボットに搭載されたバッテリの残容量が所定量以下に低下した場合に、ロボットが転倒しそうな状態であるものと判断する。そして、ロボットが転倒しそうな状態である場合には、ロボットの重心を下げる（腰を下げる）ように制御することで、転倒を回避するものである。
特開平１１−４８１０号公報

しかしながら、従来の脚式移動ロボットでは、外乱等の影響を受けた後に、実測値を検出した時点を基準とする偏差に応じて転倒を判断するものであるため、ロボットが実際に転倒を開始した後にしか転倒を判断することができないものと考えられる。

例えば、実測ＺＭＰが足裏の許容範囲内に存在するか否かによって転倒判断を行う場合について説明する。外乱などにより、実測ＺＭＰは目標値から外れて移動する。移動の結果、実測ＺＭＰが許容範囲内を超えた場合には、ロボットは転倒する可能性があるものと判断することができる。即ち、許容範囲の境界付近に到達した時点での実測ＺＭＰを基準として、転倒を判別することができる。しかし、既に転倒は始まっているため、許容範囲の境界付近に到達した時点で転倒を判別するものとしては、転倒判断のタイミングが遅くなり、以後の回避動作が間に合わないおそれがある。即ち、外乱により、ロボットが既に大きく傾いている場合には、既に転倒回避動作をとることができない状態になっているものと考えられる。一方で、許容範囲を狭めることで、転倒判断のタイミングを早めることができるものの、この場合には、僅かな外乱に対しても転倒回避動作を実行させてしまい、動作が不安定になるとも考えられ、また、ＺＭＰはノイズが多く含まれるため、頻繁に安定範囲から出てしまい、停止せざるを得ないとも考えられる。これは、転倒判別のために計測する観測情報がＺＭＰである場合に限られず、観測情報がロボットの姿勢角等である場合にも同様に、実際に転倒が開始してからしか転倒の判別をすることができないものと考えられる。

本発明の発明者らは、これらの課題を解決するため、まず、転倒の危険性をより迅速かつ正確に評価することが重要であることに着目した。さらには、転倒時のロボットに対する衝撃を低減するための動作生成を行うことが重要であることに着目して、本発明を創作するに至った。

従って、本発明は、転倒の危険性を迅速かつ正確に評価し、さらに、転倒時のロボットに対する衝撃を低減することが可能な脚式ロボット及びその制御方法を提供することを目的とする。

本発明に係る脚式ロボットは、関節角を変更して動作し、ＺＭＰ規範に基づいて前記脚式ロボットの安定化制御を行う脚式ロボットであって、前記脚式ロボットをモデル化した方程式であって、前記脚式ロボットの状態を示す状態変数を含む状態方程式を用いて、現時点から所定の時間先の物理量を示す予測物理量を計算する予測物理量計算手段と、前記予測物理量計算手段で計算した予測物理量に基づいて前記脚式ロボットの転倒危険性を判断する転倒判断手段と、を備え、前記予測物理量計算手段は、前記状態方程式に所定の制御入力を与えて該制御入力に対する前記状態方程式の出力を取得し、該取得した状態方程式の出力と、観測する前記脚式ロボットの観測物理量とに基づいて前記状態変数を推定し、該推定した状態変数に基づいて前記予測物理量を計算するものである。

これにより、ＺＭＰ規範に基づいて安定化制御を行う脚式ロボットにおいて、その脚式ロボットをモデル化した状態方程式の出力と、観測する観測物理量とから状態変数を推定することで、外乱を受けた直後に、将来の物理量を予測することができる。予測物理量は現時点から所定の時間先に実際に観測されるであろう物理量を示しているため、予測物理量に基づいて転倒危険性を判断することで、従来技術と比較してより迅速かつ正確に転倒判断を行うことができる。

また、前記転倒判断手段は、前記予測物理量としての予測ＺＭＰが、前記脚式ロボットの足裏支持多角形の安定領域内に収まらない場合に、前記脚式ロボットが転倒しそうな状態であるものと判断するようにしてもよい。これにより、予測ＺＭＰが脚式ロボットの足裏支持多角形の安定領域内に収まるか否かにより転倒危険性を判断することができ、容易に転倒危険性を判断することができる。

さらにまた、前記物理量としてのＺＭＰを計測するＺＭＰ計測手段を更に備え、前記予測物理量計算手段は、前記状態方程式に所定の制御入力を与えて該制御入力に対する前記状態方程式の出力を取得し、該取得した状態方程式の出力と、前記ＺＭＰ計測手段により計測したＺＭＰとに基づいて前記状態変数としての前記脚式ロボットの重心位置と重心加速度を推定し、該推定した重心位置と重心加速度に基づいて前記予測物理量としての予測ＺＭＰを計算するようにしてもよい。

また、前記ＺＭＰ計測手段を、前記脚式ロボットの足首に設けた力センサとすると好適である。

さらにまた、前記物理量は、前記脚式ロボットの重心体幹の加速度偏差と、姿勢角偏差と、姿勢角速度偏差とを含み、該物理量を計測するセンサを更に備え、前記物理量計算手段は、前記状態方程式に所定の制御入力を与えて該制御入力に対する前記状態方程式の出力を取得し、該取得した状態方程式の出力と、前記センサにより計測した物理量とに基づいて前記状態変数としての前記脚式ロボットの重心位置と重心加速度を推定し、該推定した重心位置と重心加速度に基づいて前記予測物理量としての予測ＺＭＰを計算するようにしてもよい。これにより、力センサを使用せずに状態変数を推定することができる。このため、足首に力センサを設ける必要が無く、脚部の軽量化を図ることができる。

また、前記センサは、前記脚式ロボットの重心体幹の加速度を検出する加速度センサと、前記脚式ロボットの姿勢角及び姿勢角速度を検出する角速度センサとを含み、前記加速度センサにより前記脚式ロボットの重心体幹の加速度を検出し、該検出した重心体幹の加速度と目標加速度とから前記加速度偏差を計測し、前記角速度センサにより前記脚式ロボットの姿勢角及び姿勢角速度を検出し、該検出した姿勢角及び姿勢角速度と、目標姿勢角及び目標姿勢角速度とから、前記姿勢角偏差及び前記姿勢角速度偏差を計測するようにしてもよい。

さらにまた、前記転倒判断手段が、前記脚式ロボットが転倒しそうな状態であり、かつ、転倒が回避できないものと判断した場合に、前記脚式ロボットが転倒する際の重心軌道を生成する転倒動作生成手段を更に備え、前記転倒動作生成手段で生成した重心軌道から目標関節角を計算し、該計算した目標関節角に従って転倒動作を行うようにしてもよい。これにより、外乱を受けた直後に、脚式ロボットが転倒回避動作を実行するか否かを判断し、転倒動作を実行するものと判断した場合には、転倒動作時の重心軌道を生成することで、より適切な転倒動作を実行することができる。

また、前記転倒動作生成手段で生成した重心軌道に基づいて、転倒動作中に前記脚式ロボットに発生する姿勢角速度を補償するための上体補正角を計算する上体補正角計算手段を更に備え、前記上体補正角により前記計算した目標関節角を補正し、該補正後の関節角に従って転倒動作を行うようにしてもよい。このように、転倒動作時に、姿勢角速度を補償することで、転倒時の脚式ロボットに対する衝撃をより低減することができる。

本発明に係る脚式ロボットの制御方法は、関節角を変更して動作し、ＺＭＰ規範に基づいて安定化制御を行う脚式ロボットの制御方法であって、前記脚式ロボットをモデル化した方程式であって、前記脚式ロボットの状態を示す状態変数を含む状態方程式を用いて、現時点から所定の時間先の物理量を示す予測物理量を計算する予測物理量計算ステップと、前記計算した予測物理量に基づいて前記脚式ロボットの転倒危険性を判断する転倒判断ステップと、を備え、前記予測物理量計算ステップでは、前記状態方程式に所定の制御入力を与えて該制御入力に対する前記状態方程式の出力を取得し、該取得した状態方程式の出力と、観測する前記脚式ロボットの観測物理量とに基づいて前記状態変数を推定し、該推定した状態変数に基づいて前記予測物理量を計算するものである。

また、前記転倒判断ステップでは、前記予測物理量としての予測ＺＭＰが、前記脚式ロボットの足裏支持多角形の安定領域内に収まらない場合に、前記脚式ロボットが転倒しそうな状態であるものと判断するようにしてもよい。

さらにまた、前記物理量としてのＺＭＰを計測するＺＭＰ計測ステップを更に備え、前記予測物理量計算ステップでは、前記状態方程式に所定の制御入力を与えて該制御入力に対する前記状態方程式の出力を取得し、該取得した状態方程式の出力と、前記計測したＺＭＰとに基づいて前記状態変数としての前記脚式ロボットの重心位置と重心加速度を推定し、該推定した重心位置と重心加速度に基づいて前記予測物理量としての予測ＺＭＰを計算するようにしてもよい。

また、前記物理量は、前記脚式ロボットの重心体幹の加速度偏差と、姿勢角偏差と、姿勢角速度偏差とを含み、該物理量をセンサにより計測する物理量計測ステップを更に備え、前記予測物理量計算ステップでは、前記状態方程式に所定の制御入力を与えて該制御入力に対する前記状態方程式の出力を取得し、該取得した状態方程式の出力と、前記計測した物理量とに基づいて前記状態変数としての前記脚式ロボットの重心位置と重心加速度を推定し、該推定した重心位置と重心加速度に基づいて前記予測物理量としての予測ＺＭＰを計算するようにしてもよい。

また、前記転倒判断ステップで、前記脚式ロボットが転倒しそうな状態であり、かつ、転倒が回避できないものと判断した場合に、前記脚式ロボットが転倒する際の重心軌道を生成する転倒動作生成ステップを更に備え、前記生成した重心軌道から目標関節角を計算し、該計算した目標関節角に従って転倒動作を行うようにしてもよい。

さらにまた、前記生成した重心軌道に基づいて、転倒動作中に前記脚式ロボットに発生する姿勢角速度を補償するための上体補正角を計算する上体補正角計算ステップを更に備え、前記上体補正角により前記計算した目標関節角を補正し、該補正後の関節角に従って転倒動作を行うようにしてもよい。

本発明によれば、転倒の危険性を迅速かつ正確に評価し、さらに、転倒時のロボットに対する衝撃を低減することが可能な脚式ロボット及びその制御方法を提供することができる。

発明の実施の形態１．
以下、図面を参照しながら本実施の形態１に係る脚式ロボットの制御動作について説明する。図１は、本実施の形態１に係る脚式ロボットの概要を示す図である。ロボット２は、体幹４と、左脚リンク６と、右脚リンク８と、コントローラ１６とを備えている。

体幹４は、ロボット２の動作（各関節の動作）を制御する制御部１０と、体幹４の加速度を検出する加速度センサ１２と、体幹４の鉛直方向に対する傾斜角（姿勢角）と角速度を検出する角速度センサ１４とを備えている。角速度センサ１４として、例えばジャイロセンサを使用することができる。

左脚リンク６は一方の端部は股関節を介して体幹４に揺動可能に接続されている。左脚リンク６はさらに膝関節と足首関節を備え、先端には足平を備えている。右脚リンク８は一方の端部は股関節を介して体幹４に揺動可能に接続されている。右脚リンク８はさらに膝関節と足首関節を備え、先端には足平を備えている。足首関節には、ＺＭＰ検出手段としての力センサ（不図示）を備えている。

ロボット２の各関節はアクチュエータ（不図示）を備えており、それらのアクチュエータは制御部１０からの指示によって回転駆動する。即ち、制御部１０が２本の各関節（詳細には関節角）を適宜制御することにより、ロボット２を歩行又は走行させることができる。左脚リンク６と右脚リンク８の足平の中心には、それぞれ基準点Ｌ_０、Ｒ_０が設けられている。基準点Ｌ_０、Ｒ_０は、ロボット２の動作パターンを生成する際の基準となる点である。図中Ｇは、ロボット２の重心位置を示す。

制御部１０は、ＣＰＵ、ＲＯＭ、ＲＡＭ、ハードディスクなどを有するコンピュータ装置である。制御部１０はコントローラ１６と通信可能であり、ユーザーが操作するコントローラ１６から指令値を入力する。制御部１０はユーザーから入力される指令値に基づいて、ロボット２の動作パターンを生成ないし計算する。制御部１０は生成した動作パターンを記憶し、記憶された動作パターンを実現するように各関節を駆動する。

図２は、制御部１０の主な構成を示す機能ブロック図である。制御部１０は、予測ＺＭＰ計算手段２１と、転倒判断手段２２と、転倒動作生成手段２３と、上体補正角計算手段２４とを含む。

予測物理量計算手段としての予測ＺＭＰ計算手段２１は、ロボット２をモデル化した方程式であって、ロボット２の状態を示す状態変数を含む状態方程式を用いて、現時点から所定の時間先のＺＭＰを示す予測ＺＭＰを計算する。具体的には、予測ＺＭＰ計算手段２１は、まず、状態方程式に所定の制御入力を与えて、その制御入力に対する状態方程式の出力を取得する。そして、予測ＺＭＰ計算手段２１は、取得した状態方程式の出力と、観測するロボット２の観測物理量とに基づいて状態変数を推定する。予測ＺＭＰ計算手段２１は、推定した状態変数に基づいて予測物理量としての予測ＺＭＰを計算することができる。尚、予測ＺＭＰ計算手段２１による予測ＺＭＰの計算方法の詳細については後述する。

転倒判断手段２２は、予測ＺＭＰ計算手段２１で計算した予測ＺＭＰに基づいてロボット２の転倒危険性を判断する。具体的には、転倒判断手段２２は、予測ＺＭＰ計算手段２１で計算した予測ＺＭＰが、ロボット２の足裏支持多角形の安定領域内に収まるか否かを判断する。これにより、転倒判断手段２２は、予測ＺＭＰが、ロボット２の足裏支持多角形の安定領域内に収まるらない場合には、ロボット２が転倒しそうな状態であるものと判断することができる。尚、転倒判断手段２２による判断の結果、ロボット２が転倒しそうな状態であっても、転倒を回避可能であるものと判断した場合には、ロボット２は転倒回避動作を行うことができる。転倒回避動作は公知の手法により実現することができ、転倒回避動作として、例えばロボット２の脚を予測ZMPを囲い込むように前方へと踏み出させるなどの動作を行えばよい。

転倒動作生成手段２３は、転倒判断手段２２によりロボット２が転倒しそうな状態であり、かつ、転倒が回避できないものと判断した場合に、ロボット２が転倒する際の重心軌道を生成する。ロボット２は、転倒動作生成手段２３で生成した重心軌道から目標関節角を計算し、その計算した目標関節角に従って転倒動作を行うことができる。尚、転倒動作生成手段２３による重心軌道生成方法の詳細については後述する。

上体補正角計算手段２４は、転倒動作生成手段２３で生成した重心軌道に基づいて、転倒動作中にロボット２に発生する姿勢角速度を補償するための上体補正角を計算する。ロボット２は、その上体補正角により目標関節角を補正し、補正後の関節角に従って転倒動作を行うことができる。尚、上体補正角計算手段２４による上体補正角計算方法の詳細については後述する。

さらに、制御部１０は、図示しない重心軌道生成手段と、足先軌道生成手段と、関節角目標値計算手段と、関節駆動手段とを含む。ここでは重心の軌道を計算して生成することから、計算、生成、演算の語を区別なく用いる。

重心軌道生成手段は、目標ＺＭＰ軌道に基づいて、ロボット２の重心の軌道を生成する。ここで生成される重心軌道は、ロボット２が移動する空間において固定する座標系で記述したものである。尚、重心軌道は、ロボット２の支持脚の足先に設けられた基準点Ｌ_０、Ｒ_０を原点とする座標系で記述してもよい。重心軌道は、シミュレーション等によってロボット２を安定して歩行させることができるように作成されている。即ち、重心軌道は、ロボット２のＺＭＰが接地面に接地した足裏で囲まれた凸包内となる関係を満足するように設定されている。言い換えると、ＺＭＰが、ロボット２の足裏支持多角形の安定領域内に収まるように重心軌道を生成する。

足先軌道生成手段は、ロボット２の遊脚の足先の軌道を計算する。ここで計算される遊脚の足先の軌道は、ロボット２が移動する空間において固定する座標系で記述したものである。尚、足先の軌道は、ロボット２の支持脚の足先に設けられた基準点Ｌ_０、Ｒ_０を原点とする座標系で記述してもよい。

関節角目標値計算手段は、重心軌道生成手段で計算されるロボット２の重心軌道と、足先軌道生成手段で計算されるロボット２の遊脚の足先の基準点の足先軌道とに基づいて、ロボット２の各関節の関節角目標値の経時的データを演算する。また、関節角目標値計算手段は、ロボット２が転倒回避動作、或いは転倒動作を行う場合には、転倒回避動作、或いは転倒動作の際の重心軌道と足先軌道とに基づいて、ロボット２の各関節の関節角目標値の経時的データを演算する。さらに、転倒動作中にロボット２に発生する姿勢角速度を補償するため、関節角目標値計算手段は、計算した関節角目標値の経時的データに対して、上体補正角計算手段２４により計算した上体補正角を加算する。関節角目標値経時的データは、ロボット２の各関節の目標関節角の経時的データを含む。関節駆動手段は、関節角目標値の経時的データに基づいて、ロボット２の各関節を駆動する。

続いて、予測ＺＭＰ計算手段２１による予測ＺＭＰの計算方法の詳細について説明する。図３は、本実施の形態１に係るロボット２を概念的に示すモデル図である。図３は、脚式ロボットを単質点モデルにより示すものである。以下、図３を参照しながら、予測ＺＭＰの算出方法について説明する。

まず、図３について、ＺＭＰ規範に基づく安定化制御を想定する。図３に示す単質点モデルより、以下の数１に示すＺＭＰ方程式を得る。ここで、ｐはＺＭＰ位置を示す。ｘ、ｚは、それぞれｘ軸方向とｚ軸方向におけるロボット２の重心位置を示す。ロボット２の重心高さｚ_ｃは一定であるものとする。ｇは重力加速度を示す。数１において、記号「ｘ」の上部に「・」が二個付いたものは、時間ｔに関するｘの二階微分を示すものとし、以下、ｘ^（・・）として記述する。

ロボット２をモデル化した状態方程式を構成するため、制御入力ｕを以下の数２により定義する。数２において、記号「ｘ」の上部に「・」が三個付いたものはｘの三階微分を示すものとし、以下、ｘ^{（・・・）}として記述する。

数１と数２より、上述した単質点モデルを示すシステムを、以下の状態方程式により表現する。

ここで、ベクトルｘを、以下の数４に示すように定義する。即ち、ベクトルｘは、ロボットの重心の位置ｘと、その速度ｘ^（・）と、その加速度ｘ^（・・）とを要素に含む。

また、係数行列Ａ、Ｂ、Ｃを、以下の数５に示すように定義する。

数４と数５を用いると、数３を以下の数６により表すことができる。

次に、数６に示す状態方程式を離散化することで、以下の数７を得ることができる。即ち、数７は離散化した状態方程式を示す。A_ｄとB_ｄは、離散化後の状態方程式の各係数を示す。

ここで、ベクトルｘ_ｋはステップｋにおけるロボット２の状態を示す状態変数ベクトルである。ｕ_ｋはステップｋにおける制御入力である。数７における第二式は、観測情報（観測物理量）をＺＭＰとした場合の、出力方程式である。

数７に示す状態方程式について、状態フィードバックにより安定化を実現するため、まず、制御入力ｕ_ｋを以下に示す数８により定義する。ここで、Ｋは、フィードバックゲインの行ベクトルであり、要素ｋ_１と、ｋ_２と、ｋ_３とを含む。

数８を用いて、数７の第一式を以下の数９に示すように変形する。ここで、数９において、記号「Ａ」の上部に「〜」が付いたものを、以下、係数行列Ａ^〜として記述する。

従って、数９を用いることで、数７を以下の数１０に示す離散化した状態方程式として示すことができる。

数１０において、係数行列Ａ^〜が安定な行列であれば、システムの安定性を保証することができる。そして、数１０に示すように、現在のステップ（ステップｋ）における状態変数ベクトルｘ_ｋと係数行列Ａ^〜とから、次のステップ（ステップｋ＋１）における状態変数ベクトルｘ_ｋ＋１を計算することができる。また、現在のステップ（ステップｋ）における状態変数ベクトルｘ_ｋと係数行列Ｃとから、現在のステップ（ステップｋ）におけるＺＭＰを示すｐ_ｋを計算することができる。尚、上述したベクトルＫの要素ｋ_１、ｋ_２、ｋ_３の各値については、係数行列Ａ^〜が安定するような値を、公知の手法により計算することができる。

さらに、数１０を変形することで、現在のステップ（ステップｋ）からｍステップ先（ステップｋ＋ｍ）の状態変数ベクトルｘ_ｋ＋ｍと、ｐ_ｋ＋ｍとについて、以下の数１１を得ることができる。

従って、オブザーバー、或いは、カルマンフィルタによって、状態変数ベクトルｘ_ｋを推定し、推定した状態変数ベクトルｘ_ｋを数１１の第一式に代入することで、ｍステップ先の状態変数ベクトルｘ_ｋ＋ｍを計算することができる。そして、計算した状態変数ベクトルｘ_ｋ＋ｍを数１１の第二式に代入することで、ｍステップ先のＺＭＰを示すｐ_ｋ＋ｍを予測することができる。

図４は、オブザーバー２０による状態変数ベクトルｘ_ｋの推定処理を説明するためのブロック線図である。図４において一点鎖線により示す範囲は、数７に示す状態方程式に相当する部分であり、ロボット２のモデルを示す。ここで、Ａ_ｄ、Ｂ_ｄ、Ｃは、上述した係数行列を示す。また、Ｌは、オブザーバーゲイン行列を示す。ロボット２は、現在の状態変数ベクトルｘ_ｋを直接観測することはできないため、オブザーバー２０により状態変数ベクトルｘ_ｋを推定する。具体的には、オブザーバー２０は、まず、実際に観測される観測物理量としてのｚｍｐ（ｐ）を、ロボット２のＺＭＰ計測手段により取得する。また、オブザーバー２０は、制御入力ｕ_ｋに対する出力ｐ_ｋを、数１０の第二式から計算する。そして、オブザーバー２０は、取得したｚｍｐ（ｐ）と、計算したｐ_ｋとから、状態変数ベクトルｘ_ｋを推定する。

次に、推定する状態変数ベクトルｘ_ｋ＋ｍについて、その値ｍの算出方法を説明する。上述したように、本実施の形態１に係るロボット２は、状態変数ベクトルｘ_ｋを推定することにより、ｍステップ先の状態変数ベクトルｘ_ｋ＋ｍを推定することができる。これにより、推定した状態変数ベクトルｘ_ｋ＋ｍを用いて、ｍステップ先のＺＭＰであるｐ_ｋ＋ｍを予測することができる。ここで、ロボット２による安定化制御周期をｄｔとし、ロボット２は
サンプル周期ｄｔでＺＭＰを計測する。即ち、サンプリングを行う各ステップ間の間隔をｄｔとする。現在のステップｋを時点ｔとすると、ステップｋからｍステップ後の時点は、ｔ＋ｍ・ｄｔにより示すことができる。値のｍの決定方法としては、ｍの値を大きくしすぎた場合には、現時点よりもかなり先の将来時点のｐ_ｋ＋ｍを予測することになり、ｍの値を小さくしすぎた場合には、現時点に対してかなり直近の将来時点のｐ_ｋ＋ｍを予測することになる。このため、外乱によってＺＭＰのピークが発生する時点が大きく変動する場合には、現時点からｍステップ先のＺＭＰを示すｐ_ｋ＋ｍを、そのピークに応じて適切に検出することができない。即ち、外乱によって変動するＺＭＰのピーク発生時点に応じて、そのピーク発生時点を適切に検出可能となるように値ｍを決定する必要がある。

図５は、ＺＭＰ計測手段により実際に観測される実ＺＭＰと、予測ＺＭＰ（数１１に示すｐ_ｋ＋ｍ。）との関係を示す図である。図において、実ＺＭＰを破線により示し、予測ＺＭＰを実線により示す。横軸は時間ｔを示し、縦軸はＺＭＰの位置（ｘ軸方向、又は、ｙ軸方向のｚｍｐ（ｐ）。）を示す。実ＺＭＰ及び予測ＺＭＰが値Ｌ_１から値Ｌ_２の範囲内にある場合には、ＺＭＰは足裏支持多角形の安定領域内に存在しているものと判断することができる。足裏支持多角形に対して所定の余裕範囲を持たせた範囲を安定領域として設定する。図において、時間Ｔ_１において外乱などを受け、ロボットの足裏のＺＭＰが移動を開始する。実ＺＭＰは、外乱の影響により、時間Ｔ_１から時間Ｔ_ｐにかけて、目標ＺＭＰから大きく乖離するように移動し、時間Ｔ_ｐにおいてそのピークとなる極値をとる。以下、実ＺＭＰが極値をとる際のピーク時間Ｔ_ｐを計算することで、予測ＺＭＰ（ｐ_ｋ＋ｍ）のステップ数ｍを算出する方法を説明する。

まず、数１０の第一式を伝達関数Ｇ（ｓ）により表現することで、以下に示す数１２を得る。ここで、ｚ_１、ｚ_２は、伝達関数Ｇ（ｓ）の零点を示す。ｐ_１は伝達関数Ｇ（ｓ）の極を示す。ζは減衰率を示す。ω_ｎは自然角振動数を示す。Ｋ、α、βは任意の定数である。ζとω_ｎに対して、予め所定の値を設定する。

数１２に対して逆ラプラス変換を施すことにより、以下の数１３に示す時間応答ｐ_ｋを得ることができる。

ここで、ｐ_１が負の値で、且つ、十分大きな値である場合には、数１３を以下の数１４により近似して示すことができる。

数１２において、右辺第二項の極で、虚部が応答の振動周波数を定めている。従って、数１４のｓｉｎ（ω_ｄｔ）が極値をとる際のピーク時間Ｔ_ｄを、以下の数１５に示すようにして求めることができる。

これにより、予測ＺＭＰのステップ数ｍを、以下の数１６により算出することができる。即ち、ｍは、自然角振動数ω_ｎと、減衰率ζと、サンプル周期ｄｔとに基づいて算出することができる。尚、ｍの値は、予め計算しておき、ロボット２に設定しておく。

図５に示すように、時間Ｔ_１において外乱などを受け、足裏のＺＭＰが移動を開始する。その直後、実線により示す予測ＺＭＰは、ｍステップ先のＺＭＰを予測する（即ち、時間Ｔ_ｐにおける実ＺＭＰに相当する。）。このため、外乱などを受けた時間Ｔ_１の直後に、予測ＺＭＰは、実ＺＭＰよりも先行して変化する。即ち、予測ＺＭＰは、実ＺＭＰがその時点からｍステップ先に到達するであろうと予測されるＺＭＰの位置を示す。予測ＺＭＰは、実ＺＭＰよりも先行して変化し、時間Ｔ_ｐよりも早くそのピークとなる極値をとる。予測ＺＭＰが、値Ｌ_１から値Ｌ_２の範囲を超えた場合には、ロボット２は転倒しそうな状態であるものと判断することができる。従って、予測ＺＭＰに基づいて転倒を判断することで、実ＺＭＰに基づいて転倒を判断する場合に比べて、より迅速に転倒を判断することができる。また、予測ＺＭＰに基づいて転倒を判断することで、値Ｌ_１と値Ｌ_２とを小さくするなどの変更をすることなく転倒状態をより高速に判断することができる。このため、微小な外乱などに対して不要な転倒回避動作を実行させずに済み、より正確に転倒判断を実行することができる。

次に、転倒動作生成手段２３による処理の詳細について説明する。転倒動作生成手段２３は、転倒時の受身動作をリアルタイムに生成することができる。以下、転倒時の転倒動作生成方法について説明する。

図６は、ロボット２を単質点モデルで示した場合の、ロボット２が転倒する際の様子を示す図である。図６において、ロボット２のＺＭＰを以下の数１７により示すことができる。ここで、ｐ_ｘ、ｐ_ｙ、ｐ_ｚはＺＭＰを示す。ｇは重力加速度を示す。ｘ、ｙ、ｚはロボットの重心位置を示す。尚、ここでは、ロボット２の回転による項についてはその影響を考慮しないものとして省略する。また、ロボット転倒時には、その足平の端部のみが接地しているものと考えられるため、ＺＭＰ位置を足平の端部に固定し、不変であるものとする。以下では、転倒時におけるロボット２の重心位置ｘ、ｙ、ｚの時系列データを示す重心軌道を計算する。

数１７を解析的に解くことで、ロボット転倒時の重心位置軌道を生成することができる。まず、鉛直方向（ｚ軸方向）における重心軌道ｚ（ｔ）を生成する。このため、重心軌道ｚ（ｔ）を以下の数１８に示す微分方程式により拘束する。ここで、ｆ（ｔ）は拘束関数である。

数１８を数１７に代入することで、以下の数１９を得る。

数１７を解くことができるように拘束関数ｆ（ｔ）を選択することで、数１７を解析的に解くことが可能となる。ここでは、例えば拘束関数ｆ（ｔ）を以下の数２０に示すように設定する。ここで、ａ₀とａ₁は拘束関数ｆのパラメータである。

数２０より、数１８の解析解は以下の数２１となる。数２１は、ｚ（ｔ）の解析解を示す。ここで、Ｃ_ｚ０とＣ_ｚ１は積分定数を示す。Ｉ_１（τ）は第１変形ベッセル関数を示す。Ｋ_１（τ）は第２次変形ベッセル関数を示す。

数２１におけるτとｚ_ｓ（ｔ）の定義を、以下の数２２に示す。

同様に、数２０より、数１９の解析解は以下の数２３となる。数２３は、ｘ（ｔ）と、ｙ（ｔ）の解析解を示す。ここで、Ｃ_ｘ０、Ｃ_ｘ１、Ｃ_ｙ０、Ｃ_ｙ１は積分定数を示す。ｘ_ｓ（ｔ）とｙ_ｓ（ｔ）は特殊解を示す。転倒時のロボットはその足平端部のみが地面と接触しているものと考えられる。このため、特殊解ｘ_ｓ（ｔ）とｙ_ｓ（ｔ）は０とする。

転倒時の解析解を得るためには、重心位置ｚについて、以下の数２４を満足する必要がある。即ち、重心位置ｚの加速度の絶対値が、重力加速度ｇ以下である必要がある。

従って、ｚ（ｔ）を３次関数で近似することにより、ｚ（ｔ）について、以下の数２４に示す解の存在条件を得る。ｔ_０とｔ_１は、それぞれ転倒開始時刻と転倒終端時刻を示す。

ここで、転倒開始時刻ｔ_０を、予測ＺＭＰが足裏支持多角形の範囲から外れた時間（若しくは、許容範囲を超えた時間）とする。そして、ｚ_０とｚ_１を以下の数２４に示すように定義する。

従って、数２５に示す不等式を満足するように、転倒終端時刻における重心位置ｚ_１と転倒終端時刻ｔ_１を決定する。そして、転倒開始時刻ｔ_０と、転倒終端時刻ｔ_１と、決定した重心位置ｚ_１と、転倒終端時刻ｔ_１とから、Ｃ_ｚ０と、Ｃ_ｚ１と、ａ₀と、ａ₁とを求めて数２１に代入することで、重心軌道ｚ（ｔ）を得ることができる。

次に、重心軌道ｘ（ｔ）を生成する。尚、重心軌道ｙ（ｔ）についても同様にして生成することができるため、ここでは説明を省略する。適切なｘ（ｔ）を算出するため、ｚ（ｔ）と同様に、転倒開始時刻と転倒終端時刻での位置・速度の境界条件を満足する必要がある。しかし、上述したように、数２３においては、パラメータを２つ（Ｃ_ｘ０、Ｃ_ｘ１）のみしか含んでいないため過拘束となってしまう。このため、パラメータ（Ｃ_ｘ０、Ｃ_ｘ１）は、以下の数２７に示すように、初期条件のみで決定されることとなる。

ここで、数２７におけるτ_０を、以下の数２８に示すように定義する。

従って、重心軌道ｘ（ｔ）と、重心軌道ｙ（ｔ）を得ることができる。

転倒動作生成手段２３は、転倒判断手段２２が、ロボット２が転倒しそうな状態であり、かつ、転倒が回避できないものと判断した場合に、ロボット２が転倒する際の重心軌道を生成する。そして、ロボット２は、生成した重心軌道から目標関節角を計算し、その計算した目標関節角に従って転倒動作を行う。即ち、外乱を受けた直後に、ロボット２は、転倒回避動作を実行するか否かを判断する。そして、転倒動作を実行するものと判断した場合には、転倒動作時の重心軌道を生成することで、より適切な転倒動作を実行することができる。

次に、上体補正角計算手段２４による処理の詳細について説明する。上体補正角計算手段２４により、ロボット転倒時に、ロボット２の上体姿勢による角速度補償を実現することができる。上述した重心軌道生成方法においては、ロボット２の回転による影響を考慮しないものとして説明したが、ロボット２が大きな外乱などを受けた場合には、ロボット２の回転による影響を無視することができないものとなる。このため、本実施の形態１では、外乱などによりロボット２に発生した角速度を、上体姿勢を用いて補償する。これにより、転倒時のロボットに対する衝撃をより低減することができる。

上述した図６に示すモデルは、単質点モデルである。このため、ロボット２のｙ軸周り（ピッチ方向）における理想的な角速度^ｒｅｆω_ｙを、以下の数２９により示すことができる。

従って、転倒時にロボット２に発生した角速度を補償するための上体補正角θ_{ｕｐｐｅｒ}を、以下の数３０により得ることができる。上体補正角θ_{ｕｐｐｅｒ}により上体を回転することで、ロボット２に発生した角速度を補償することができる。ここで、Δω_{ｕｐｐｅｒ}は補正角速度を示す。実角速度ω_ｙは、ロボット２の角速度センサ１４により計測することができる。Ｋは角速度偏差に関するゲインであり、予め所定の値を設定する。例えば、上体補正角θ_{ｕｐｐｅｒ}を股関節の角度に加えることで、角速度補償を実現することができる。

上体補正角計算手段２４は、転倒動作生成手段２３で生成した重心軌道に基づいて、転倒動作中にロボット２に発生する姿勢角速度を補償するための上体補正角を計算する。ロボット２は、その上体補正角により目標関節角を補正し、補正後の関節角に従って転倒動作を行うことができる。このように、転倒動作時に姿勢角速度を補償することで、転倒時のロボット２に対する衝撃をより低減することができる。

発明の実施の形態２．
次に、本実施の形態２に係る脚式ロボットの制御動作について説明する。本実施の形態２にかかるロボットは、実施の形態１において図１に示すロボットと略同一の構成を有する。実施の形態１では、観測物理量（観測情報）としてＺＭＰを使用するものとしたが、本実施の形態２では、観測物理量として重心体幹の加速度と、姿勢角度と、姿勢角速度とを使用する。従って、本実施の形態２では、実施の形態１のロボット２に必要としたＺＭＰ計測手段は不要であり、例えば力センサなどのＺＭＰ計測手段を搭載する必要が無い。

図７は、本実施の形態２に係るロボットを概念的に示すモデル図である。図７は、脚式ロボットを単質点モデルにより示すものである。以下、図７を参照しながら、本実施の形態２に係る予測ＺＭＰの算出方法について説明する。

まず、離散化した状態変数ベクトルｘ_ｋと制御入力ｕ_ｋを、以下の数３２に示すように定義する。即ち、離散化した状態変数ベクトルｘ_ｋは、ロボットの重心の位置偏差と、その速度偏差と、その加速度偏差とを要素に含む。ここで、記号の右上部に「＊」がついたものはその目標値を示すものとし、予め計算して設定する。また、例えば、記号「ｘ」の右上部に「＊」がついたものを、以下、ｘ^（＊）として記述する。

すると、実施の形態１と同様にして、以下の数３２に示す離散化した状態方程式を得ることができる。

ここで、観測情報を、重心体幹の加速度偏差（即ち、ｘ_ｋ ^（・・）−ｘ_ｋ ^{（・・）（＊）}。）と、姿勢角偏差（即ち、θ_ｋ−θ_ｋ ^（＊）。）と、姿勢角速度偏差（即ち、ω_ｋ−ω_ｋ ^（＊）。）とすると、数３２に示す状態方程式の出力方程式を、以下の数３３により示すことができる。尚、姿勢角偏差θ_ｋ−θ_ｋ ^（＊）をθ_ｋ ^ｅｒとして記述する。姿勢角偏差θ_ｋ ^ｅｒにより、重心の位置偏差ｘ_ｋ−ｘ_ｋ ^（＊）を近似することができる。

従って、上述した実施の形態１と同様にして、オブザーバー、或いは、カルマンフィルタによって、状態変数ベクトルｘ_ｋを推定し、推定した状態変数ベクトルｘ_ｋを数１１の第一式に代入することで、ｍステップ先の状態変数ベクトルｘ_ｋ＋ｍを計算することができる。具体的には、オブザーバーは、加速度センサ１２によりロボット２の重心体幹の加速度を検出し、その検出した重心体幹の加速度と目標加速度とから加速度偏差を計測する。オブザーバーは、角速度センサ１４によりロボット２の姿勢角及び姿勢角速度を検出し、その検出した姿勢角及び姿勢角速度と、目標姿勢角及び目標姿勢角速度とから、姿勢角偏差及び姿勢角速度偏差を計測する。また、オブザーバーは、制御入力ｕ_ｋに対する出力を、数３３に示す出力方程式から計算する。そして、オブザーバー２０は、取得した重心体幹の加速度偏差、姿勢角偏差、姿勢角速度偏差と、計算した出力とから、状態変数ベクトルｘ_ｋを推定する。そして、計算した状態変数ベクトルｘ_ｋ＋ｍを数１１の第二式に代入することで、ｍステップ先のＺＭＰ偏差（ｐ_ｋ＋ｍ−ｐ_ｋ＋ｍ ^（＊））を求めることができる。

本実施の形態２では、例えば力センサなどのＺＭＰ計測手段を用いることなく予測ＺＭＰを計算することができる。従って、予測ＺＭＰに基づいて転倒を判断することで、実ＺＭＰに基づいて転倒を判断する場合に比べて、より迅速かつ正確に転倒を判断することができる。

以上説明したように、本発明によれば、ＺＭＰ規範に基づいて安定化制御を行うロボットにおいて、そのロボットをモデル化した状態方程式の出力と、観測する観測物理量とから状態変数を推定することで、外乱を受けた直後に、将来のＺＭＰを予測することができる。予測ＺＭＰは現時点から所定の時間先に実際に観測されるであろうＺＭＰを示しており、予測ＺＭＰに基づいて転倒危険性を判断することで、従来技術と比較してより迅速かつ正確に転倒判断を行うことができる。従って、外乱などを受けた直後に、ロボットが転倒回避動作をとるべきか否かを判断することができ、さらに、転倒を回避できないものと判断した場合には、適切な転倒動作を生成した上で転倒させることができる。

尚、上述した実施の形態においては、推定した状態変数に基づいて計算する予測物理量を予測ＺＭＰとして説明したが本発明はこれに限定されない。即ち、転倒判定に使用する指標はＺＭＰに限定されず、角度や、角速度や、或いは、これらの非線形関数を転倒判定指標としてもよい。従って、予測物理量は予測ＺＭＰに限定されず、予測角度や、予測角速度などを計算するものとしてもよい。

尚、本発明は上述した実施の形態のみに限定されるものではなく、既に述べた本発明の要旨を逸脱しない範囲において種々の変更が可能であることは勿論である。

本発明の実施の形態１に係る脚式ロボットの構成を示す図である。 本発明の実施の形態１に係る脚式ロボットの機能構成を示すブロック図である。 本発明の実施の形態１に係る脚式ロボットの単質点モデルを示す図である。 本発明の実施の形態１に係るオブザーバーによる状態変数ベクトルｘ_ｋの推定処理を説明するための図である。 本発明の実施の形態１に係る実ＺＭＰと予測ＺＭＰとの関係を示す図である。 本発明の実施の形態１に係る脚式ロボットが転倒する際の様子を示す図である。 本発明の実施の形態２に係る脚式ロボットの単質点モデルを示す図である。

符号の説明

２ ロボット、
４ 体幹、
６ 左脚リンク、
８ 右脚リンク、
１０ 制御部、
１２ 加速度センサ、
１４ 角速度センサ、
１６ コントローラ、
２１ 予測ＺＭＰ計算手段、
２２ 転倒判断手段、
２３ 転倒動作生成手段、
２４ 上体補正角計算手段

Claims (14)

1. 関節角を変更して動作し、ＺＭＰ規範に基づいて安定化制御を行う脚式ロボットであって、
   前記脚式ロボットをモデル化した方程式であって、前記脚式ロボットの状態を示す状態変数を含む状態方程式を用いて、現時点から所定の時間先の物理量を示す予測物理量を計算する予測物理量計算手段と、
   前記予測物理量計算手段で計算した予測物理量に基づいて前記脚式ロボットの転倒危険性を判断する転倒判断手段と、を備え、
   前記予測物理量計算手段は、
   前記状態方程式に所定の制御入力を与えて該制御入力に対する前記状態方程式の出力を取得し、該取得した状態方程式の出力と、観測する前記脚式ロボットの観測物理量とに基づいて前記状態変数を推定し、該推定した状態変数に基づいて前記予測物理量を計算する
   ことを特徴とする脚式ロボット。
2. 前記転倒判断手段は、
   前記予測物理量としての予測ＺＭＰが、前記脚式ロボットの足裏支持多角形の安定領域内に収まらない場合に、前記脚式ロボットが転倒しそうな状態であるものと判断する
   ことを特徴とする請求項１記載の脚式ロボット。
3. 前記物理量としてのＺＭＰを計測するＺＭＰ計測手段を更に備え、
   前記予測物理量計算手段は、
   前記状態方程式に所定の制御入力を与えて該制御入力に対する前記状態方程式の出力を取得し、該取得した状態方程式の出力と、前記ＺＭＰ計測手段により計測したＺＭＰとに基づいて前記状態変数としての前記脚式ロボットの重心位置と重心加速度を推定し、該推定した重心位置と重心加速度に基づいて前記予測物理量としての予測ＺＭＰを計算する
   ことを特徴とする請求項１又は２記載の脚式ロボット。
4. 前記ＺＭＰ計測手段は、前記脚式ロボットの足首に設けた力センサである
   ことを特徴とする請求項３記載の脚式ロボット。
5. 前記物理量は、前記脚式ロボットの重心体幹の加速度偏差と、姿勢角偏差と、姿勢角速度偏差とを含み、該物理量を計測するセンサを更に備え、
   前記予測物理量計算手段は、
   前記状態方程式に所定の制御入力を与えて該制御入力に対する前記状態方程式の出力を取得し、該取得した状態方程式の出力と、前記センサにより計測した物理量とに基づいて前記状態変数としての前記脚式ロボットの重心位置と重心加速度を推定し、該推定した重心位置と重心加速度に基づいて前記予測物理量としての予測ＺＭＰを計算する
   ことを特徴とする請求項１又は２記載の脚式ロボット。
6. 前記センサは、前記脚式ロボットの重心体幹の加速度を検出する加速度センサと、前記脚式ロボットの姿勢角及び姿勢角速度を検出する角速度センサとを含み、
   前記加速度センサにより前記脚式ロボットの重心体幹の加速度を検出し、該検出した重心体幹の加速度と目標加速度とから前記加速度偏差を計測し、
   前記角速度センサにより前記脚式ロボットの姿勢角及び姿勢角速度を検出し、該検出した姿勢角及び姿勢角速度と、目標姿勢角及び目標姿勢角速度とから、前記姿勢角偏差及び前記姿勢角速度偏差を計測する
   ことを特徴とする請求項５記載の脚式ロボット。
7. 前記転倒判断手段が、前記脚式ロボットが転倒しそうな状態であり、かつ、転倒が回避できないものと判断した場合に、前記脚式ロボットが転倒する際の重心軌道を生成する転倒動作生成手段を更に備え、
   前記転倒動作生成手段で生成した重心軌道から目標関節角を計算し、該計算した目標関節角に従って転倒動作を行う
   ことを特徴とする請求項１乃至６いずれか１項記載の脚式ロボット。
8. 前記転倒動作生成手段で生成した重心軌道に基づいて、転倒動作中に前記脚式ロボットに発生する姿勢角速度を補償するための上体補正角を計算する上体補正角計算手段を更に備え、
   前記上体補正角により前記計算した目標関節角を補正し、該補正後の関節角に従って転倒動作を行う
   ことを特徴とする請求項７記載の脚式ロボット。
9. 関節角を変更して動作し、ＺＭＰ規範に基づいて安定化制御を行う脚式ロボットの制御方法であって、
   前記脚式ロボットをモデル化した方程式であって、前記脚式ロボットの状態を示す状態変数を含む状態方程式を用いて、現時点から所定の時間先の物理量を示す予測物理量を計算する予測物理量計算ステップと、
   前記計算した予測物理量に基づいて前記脚式ロボットの転倒危険性を判断する転倒判断ステップと、を備え、
   前記予測物理量計算ステップでは、
   前記状態方程式に所定の制御入力を与えて該制御入力に対する前記状態方程式の出力を取得し、該取得した状態方程式の出力と、観測する前記脚式ロボットの観測物理量とに基づいて前記状態変数を推定し、該推定した状態変数に基づいて前記予測物理量を計算する
   ことを特徴とする脚式ロボットの制御方法。
10. 前記転倒判断ステップでは、
    前記予測物理量としての予測ＺＭＰが、前記脚式ロボットの足裏支持多角形の安定領域内に収まらない場合に、前記脚式ロボットが転倒しそうな状態であるものと判断する
    ことを特徴とする請求項９記載の脚式ロボットの制御方法。
11. 前記物理量としてのＺＭＰを計測するＺＭＰ計測ステップを更に備え、
    前記予測物理量計算ステップでは、
    前記状態方程式に所定の制御入力を与えて該制御入力に対する前記状態方程式の出力を取得し、該取得した状態方程式の出力と、前記計測したＺＭＰとに基づいて前記状態変数としての前記脚式ロボットの重心位置と重心加速度を推定し、該推定した重心位置と重心加速度に基づいて前記予測物理量としての予測ＺＭＰを計算する
    ことを特徴とする請求項９又は１０記載の脚式ロボットの制御方法。
12. 前記物理量は、前記脚式ロボットの重心体幹の加速度偏差と、姿勢角偏差と、姿勢角速度偏差とを含み、該物理量をセンサにより計測する物理量計測ステップを更に備え、
    前記予測物理量計算ステップでは、
    前記状態方程式に所定の制御入力を与えて該制御入力に対する前記状態方程式の出力を取得し、該取得した状態方程式の出力と、前記計測した物理量とに基づいて前記状態変数としての前記脚式ロボットの重心位置と重心加速度を推定し、該推定した重心位置と重心加速度に基づいて前記予測物理量としての予測ＺＭＰを計算する
    ことを特徴とする請求項９又は１０記載の脚式ロボットの制御方法。
13. 前記転倒判断ステップで、前記脚式ロボットが転倒しそうな状態であり、かつ、転倒が回避できないものと判断した場合に、前記脚式ロボットが転倒する際の重心軌道を生成する転倒動作生成ステップを更に備え、
    前記生成した重心軌道から目標関節角を計算し、該計算した目標関節角に従って転倒動作を行う
    ことを特徴とする請求項９乃至１２いずれか１項記載の脚式ロボットの制御方法。
14. 前記生成した重心軌道に基づいて、転倒動作中に前記脚式ロボットに発生する姿勢角速度を補償するための上体補正角を計算する上体補正角計算ステップを更に備え、
    前記上体補正角により前記計算した目標関節角を補正し、該補正後の関節角に従って転倒動作を行う
    ことを特徴とする請求項１３記載の脚式ロボットの制御方法。

Images