JP6015474B2 - 脚式ロボットの制御方法および脚式ロボット - Google Patents

Description

本発明は脚式ロボットの制御方法および脚式ロボットに関する。

床反力中心点であるＺＭＰ（ゼロモーメントポイント）に関して、目標ＺＭＰ軌道に追従して歩行する脚式ロボットが提案されている。従来、このような脚式ロボットの制御方法では、外乱や目標位置の修正などに起因して目標ＺＭＰ軌道への追従が不可能となった場合には、着地位置を修正して安定性を確保することが行われている（例えば特許文献１や特許文献２を参照。）。

特許第３２６９８５２号公報 特開２００９−１０７０３０号公報

しかしながら、従来技術に係る脚式ロボットの制御方法では、着地位置を修正することにより安定性を保つことができるが、目標ＺＭＰへの追従性が悪くなってしまうという問題があった。すなわち、目標ＺＭＰへの追従性の悪化によって、元々計画していた着地位置から大きく離れてしまい、ユーザーが与えた目標の着地予定位置と異なってしまう。

本発明は、このような問題を解決するためになされたものであり、高い安定性と目標ＺＭＰへの追従性を両立することができる脚式ロボットの制御方法および脚式ロボットを提供することを目的とするものである。

一実施の形態に係る脚式ロボットの制御方法は、目標ＺＭＰ軌道に追従して歩行する脚式ロボットの制御方法であって、モデル予測制御における最適化の評価関数を、重み付き目標ＺＭＰ修正量と重み付き着地位置修正量の和を含む値を最小化する構成とし、遊脚着地までの残り時間に応じて前記着地位置修正量の重みを変化させ、前記モデル予測制御を実行して、前記目標ＺＭＰ修正量と前記着地位置修正量の最適解を算出する、ものである。これにより、高い安定性と目標ＺＭＰへの追従性を両立することができる。

また、前記遊脚着地までの残り時間が短くなるほど、前記着地位置修正量の重みを線形に小さくするものとしてもよい。これによって、着地位置を修正できる時間的な余裕がある間に、必要な分だけ早めに着地位置を修正することができ、より安定性を高めることができる。

また、前記遊脚着地までの残り時間が短くなるほど、前記着地位置修正量の重みを線形に大きくするものとしてもよい。これによって、着地位置を修正できる時間的な余裕がある間に、必要な分だけ早めに着地位置を修正することができ、より安定性を高めることができる。

また、前記脚式ロボットが、２脚歩行ロボットであるようにしてもよい。

他の一実施の形態に係る脚式ロボットは、目標ＺＭＰ軌道に追従して歩行する脚式ロボットであって、モデル予測制御における最適化の評価関数を、重み付き目標ＺＭＰ修正量と重み付き着地位置修正量の和を含む値を最小化する構成とし、遊脚着地までの残り時間に応じて前記着地位置修正量の重みを小さくし、前記モデル予測制御を実行して、前記目標ＺＭＰ修正量と前記着地位置修正量の最適解を算出する、ものである。これにより、高い安定性と目標ＺＭＰへの追従性を両立することができる。

他の一実施の形態に係る脚式ロボットは、目標ＺＭＰ軌道に追従して歩行する脚式ロボットであって、モデル予測制御における最適化の評価関数を、重み付き目標ＺＭＰ修正量と重み付き着地位置修正量の和を含む値を最小化する構成とし、遊脚着地までの残り時間に応じて前記着地位置修正量の重みを大きくし、前記モデル予測制御を実行して、前記目標ＺＭＰ修正量と前記着地位置修正量の最適解を算出する、ものである。これにより、高い安定性と目標ＺＭＰへの追従性を両立することができる。

本発明が解決しようとする課題を説明するための図である。 本発明に関する歩行軌道生成の流れを説明するための図である。 本発明に関する歩行軌道生成の流れを説明するためのフローチャートである。 本発明に関する脚制御のプロセスを説明するための図である。 本発明に関する重心軌道生成コントローラの入出力を示す図である。 実施の形態１に係るモデル予測制御に関する重心モデルおよび高さ拘束を説明するための図である。 実施の形態１に係るモデル予測制御に関する各サンプリング点における入出力および状態変数を説明するための図である。 実施の形態１に係るモデル予測制御に関するReceding Horizon制御の考え方を説明するための図である。 実施の形態１に係るＺＭＰ制約範囲を説明するための図である。 実施の形態１に係る着地位置の修正を説明するための図である。 実施の形態１に係る目標ＺＭＰ修正パターンを示すグラフである。 実施の形態１に係る着地位置修正による目標ＺＭＰ軌道の修正を示すグラフである。 実施の形態１に係る修正後の目標ＺＭＰ軌道を説明するための図である。 実施の形態１に係る残り着地時間の違いに応じた着地位置修正手法を説明するための図である。 実施の形態１に係る残り着地時間の違いに応じた着地位置修正手法を説明するための図である。 実施の形態１に係るＺＭＰ軌道と重心軌道（外乱なし）の関係を示すグラフである。 実施の形態１に係る重心位置・速度・加速度（着地位置修正なし、外乱あり）の関係を示すグラフである。 実施の形態１に係る重心位置・速度・加速度（着地位置修正あり、外乱あり）の関係を示すグラフである。 実施の形態１に係るｘｙ方向の重心およびＺＭＰ（外乱なし）の関係を示すグラフである。 実施の形態１に係るｘｙ方向の重心速度・加速度（外乱なし）の関係を示すグラフである。 実施の形態１に係るｘｙ方向の重心およびＺＭＰ（外乱あり）の関係を示すグラフである。 実施の形態１に係るｘｙ方向の重心速度・加速度（外乱あり）の関係を示すグラフである。 実施の形態１に係る初期化処理を示すフローチャートである。 実施の形態１に係る周期実行処理を示すフローチャートである。 実施の形態１に係る離散化された目標ＺＭＰ修正パターンを説明するための図である。 実施の形態１に係る目標ＺＭＰ修正パターンを用いた目標ＺＭＰ軌道の修正手法を説明するための図である。 実施の形態１に係る目標ＺＭＰ修正パターンを用いた目標ＺＭＰ軌道の修正手法を説明するための図である。 実施の形態１に係る目標ＺＭＰ修正パターンを用いた目標ＺＭＰ軌道の修正手法を説明するための図である。 実施の形態１に係る評価関数の各部分の役割を説明するための図である。

まず、図１を参照して、本発明が解決しようとする問題を簡単に説明する。図１は、本発明に係る脚式ロボットの一例としての２脚歩行ロボット１０の模式的な構成例を示している。以下では、２脚歩行ロボット１０を単にロボット１０と称する。

ロボット１０は、関節角を制御するアクチュエータと、関節角を検出するエンコーダと、が脚の各関節に内蔵されている。また、ロボット１０は、制御コントローラ２０を備えている。制御コントローラ２０が、歩容データに基づいて脚の各関節を制御する。すなわち、制御コントローラ２０は、歩容データから得られる各関節の目標角度に追従するように、アクチュエータを制御する。より具体的には、制御コントローラ２０は、歩容データ（足先の目標軌道と重心の目標軌道）を用いて各関節の目標角の時系列データを算出し、算出した目標角に追従するように、各関節を制御する。歩容データを用いて関節目標角を生成する手法については、従来から知られている手法を用いればよいため、ここではその詳細な説明を省略する。

なお、制御コントローラ２０は、主要なハードウェア構成として、制御処理、演算処理等と行うＣＰＵ（Central Processing Unit）と、ＣＰＵによって実行される制御プログラムと、演算プログラム等が記憶されたＲＯＭ（Read Only Memory）と、処理データ等を一時的に記憶するＲＡＭ（Random Access Memory）と、を有するマイクロコンピュータを用いて構成されている。また、これらＣＰＵ、ＲＯＭ、及びＲＡＭは、データバスによって相互に接続されている。

図１において、ロボット１０に対して外力が印加された状況を想定する。この状況において、ロボット１０は、（ｉ）「どれ位まで、支持脚（図１においてロボット１０の右脚）により踏ん張るのか？」、および、（ｉｉ）「どれ位まで、遊脚（図１においてロボット１０の左脚）の着地位置を修正して対応するのか？」を決定する必要がある。

上記（ｉ）に関して、支持脚における目標ＺＭＰからＺＭＰが乖離する程度をできる限り小さくしようと制御した場合にはロボット１０が転倒する可能性があり、このために、上記（ｉｉ）の遊脚の着地位置を大きく修正する必要がある。一方で、上記（ｉ）に関してＺＭＰが乖離する程度を大きくするように制御できれば、ロボット１０は支持脚により踏ん張って外乱を抑えることになり、この結果、上記（ｉｉ）の着地位置をそれほど大きく修正せずに済む。

しかし、上記（ｉ）に関してＺＭＰが乖離する程度を大きくするよう制御した場合には、ロボット１０は足裏の範囲内においてしか踏ん張ることができないため、ロボット１０が踏ん張れる余裕は小さくなる。このため、できる限り安定余裕を確保するためには、目標ＺＭＰからＺＭＰが乖離する程度を小さく抑制したい。一方で、上記（ｉｉ）の遊脚の着地位置を大きく修正するよう制御した場合、元々計画していた着地位置から大きく離れてしまう。着地予定位置はユーザーが与えた目標であるため、着地位置修正量はできる限り小さく抑制したい。本発明は、このようなトレードオフを最適化しようとする技術を提供する。

次に、本発明の理解を容易とするために、詳細な実施の形態の説明に入る前に、歩行軌道生成処理全体における本発明の位置付けを簡単に説明する。以下、図２および図３を参照しながら、歩行軌道生成処理の流れを説明する。

まず、図２の１番目（図の最上部）に示すように、足位置（指令Ｆｏｏｔ Ｐｒｉｎｔ）や歩行周期をユーザーがロボット１０に対して指令する。これは、図３において、指令値（指令Ｆｏｏｔ Ｐｒｉｎｔ・歩行周期）の設定に対応する（Ｓ１０１）。次に、図２の２番目に示すように、ロボット１０の制御コントローラ２０は、指令足位置を繋ぐように目標ＺＭＰ軌道を生成する。これは、図３において、目標ＺＭＰ軌道の生成に対応する（Ｓ２０１）。次に、図２の３番目に示すように、制御コントローラ２０は、指令足位置により構成される支持多角形からＺＭＰ制約条件を計算する。これは、図３において、制約条件の設定に対応する（Ｓ２０２）。

次に、図２の４番目に示すように、制御コントローラ２０は、ＺＭＰ方程式を解いて重心軌道を生成する。これは、図３において、ＺＭＰ方程式の求解に対応する（Ｓ２０３）。本発明に係る技術は、この処理における重心軌道生成・着地位置修正に関する技術である。

次に、図２の５番目に示すように、制御コントローラ２０は、着地位置を繋ぐように足先軌道を生成する。これは、図３において、足先軌道の生成に対応する（Ｓ２０４）。次に、図２の６番目に示すように、制御コントローラ２０は、多質点モデルを用いて重心位置から腰位置への変換を行う。これは、図３において、重心収束計算に対応する（Ｓ２０５）。次に、図２の７番目に示すように、制御コントローラ２０は、ロボット１０に搭載された各種センサ情報を用いて足・腰位置を修正する。これは、図３において、ローカルＦＢ（フィードバック）に対応する（Ｓ２０６）。次に、図２の８番目に示すように、制御コントローラ２０は、ＩＫ（逆運動学）を解いて関節角を決定する。これは、図３において、逆運動学に対応する（Ｓ２０７）。

次いで、図４を参照して、本発明に関する脚制御のプロセスを簡単に説明する。脚制御のプロセスは、図４に示すＰＤＣＡサイクルを回すようにして実行する。まず、Ｐにおいて、制御コントローラ２０は、将来の安定性を考慮した軌道生成を行う。次いで、Ｄにおいて、制御コントローラ２０は、生成した軌道を実現するような床反力を発生させる。次いで、Ｃにおいて、制御コントローラ２０は、ロボット実機とモデル挙動のズレを推定する。次いで、Ａにおいて、制御コントローラ２０は、モデルをロボット実機に一致させる。再びＰに戻り、制御コントローラ２０は、ＰＤＣＡサイクルを繰り返す。

制御コントローラ２０は、重心軌道生成処理を実行する重心軌道生成コントローラを含んでいる。重心軌道生成コントローラは、例えば、２０〜３０［ｍｓ］の周期に従って軌道生成計算を周期的に実行する。重心軌道生成コントローラの役割は、目標ＺＭＰ軌道や目標着地位置に基づいてＺＭＰおよび遊脚着地位置を操作し、これによって安定な重心軌道を生成することである。重心軌道生成コントローラは、現在の重心を始端として、位置・速度・加速度が連続となる重心軌道を生成する。

図５を参照して、重心軌道生成コントローラの入出力を説明する。図に示すように、目標ＺＭＰ軌道、目標遊脚着地位置、および現在の重心位置・速度・加速度が重心軌道生成コントローラに入力される。重心軌道生成コントローラは、入力されたこれらのデータに基づいて重心軌道生成処理を行い、ＺＭＰ位置、重心軌道、および遊脚着地位置を出力する。

次に、本発明の概要を簡単に説明する。
上述したように、ロボット１０は目標ＺＭＰ軌道に追従するように各関節を駆動することによって安定な歩行を実現することができる。ロボット１０の制御系には、目標ＺＭＰ軌道に追従する重心軌道を出力する機能が要求される一方で、ロボット１０に大きな外乱が印加された場合などには、目標ＺＭＰに追従しているだけではロボット１０が転倒してしまうために、着地位置を修正（すなわち、目標ＺＭＰ軌道を修正）することにより安定性を確保する機能も必要とされる。

しかしながら、「目標ＺＭＰ軌道への追従性を高めること（すなわち、目標ＺＭＰからのＺＭＰの乖離を抑制すること）」と「着地位置修正量を抑制すること（すなわち、目標ＺＭＰ軌道の修正量を抑制すること）」はトレードオフの関係にあり、従来技術では、このトレードオフに着眼した発明はあまりなされていない。

本発明は、このトレードオフ関係にある両者の重み付き和を最小化するようにモデル予測制御を構築し、着地までの残り時間に応じて連続的に重みを変化させることによって、高い安定性とＺＭＰ追従性を両立することができる新しい制御手法を提供する。

以下では、本発明を適用した具体的な実施の形態について、図面を参照しながら詳細に説明する。説明の明確化のため、必要に応じて重複説明は省略される。

＜実施の形態１．＞

本実施の形態の説明においては、発明の理解を容易にするため、まず始めに、着地位置修正を含まないモデル予測制御型重心軌道生成を簡単に説明する。その後、着地位置修正を含めた定式化への拡張を行い、目標ＺＭＰへの追従と着地位置修正のトレードオフを最適化するモデル予測制御手法を構築する。

１：モデル予測制御型重心軌道生成法について
以下、ロボット１０の重心位置をｘ_ｇ、ｙ_ｇ、ｚ_ｇと定義する。
ロボット歩行制御の分野においてよく知られた線形倒立振子モデルでは、図６に示すように重心高さを一定にすることによって（すなわち、ｚ_ｇを一定値ｈと設定する。）、式（１）に示す線形式を用いてＺＭＰ方程式を記述することができる。なお、以下の式において、変数の上部に配置される"・"は変数の一階微分を示し、"・・"は変数の二階微分を示す。

システムへの入力をｕ^ｘ（ｔ）＝ｘ^{（・・・）} _ｇ（ｘ_ｇの三階微分）とし、システムからの出力をｐ^ｘ（ｔ）＝ｘ_ｚｍｐと定義した場合、線形の連続システムを用いて式（１）を記述することにより、以下の式（２）を得る。

次いで、式（２）をΔｔ（離散時間間隔）により離散化する。この離散化の様子を、図７に示す。これによって、式（２）を以下の式（３）に示す離散システムとして表すことができる。なお、式（２）では、現在時点のタイミングを、ｋサンプル目としている。

本実施の形態に係るモデル予測制御は、Ｒｅｃｅｄｉｎｇ Ｈｏｒｉｚｏｎ Ｃｏｎｔｒｏｌとも呼ばれる。図８に、このＲｅｃｅｄｉｎｇ Ｈｏｒｉｚｏｎ Ｃｏｎｔｒｏｌの考え方を示す。図８に示すように、まず、現在時点において、ＮΔｔ未来までの期間のモデル挙動を予測して最適化計算を行い、現在の入力値ｕ^ｋを決定する。Δｔ秒経過後にはまた同様の計算を行い、Δｔ秒後時点における入力値を決定する。Ｒｅｃｅｄｉｎｇ Ｈｏｒｉｚｏｎ Ｃｏｎｔｒｏｌとは、このような計算過程を一定周期毎に実行することにより、結果として、最適な軌道を計算する手法である。

ＮΔｔ未来までの期間の予測に関しては、以下の式（４）に示すように、離散システムを未来に向かって積算していくことにより得ることができる。

式（４）において、ｕ^ｘ _ｋ，ｕ^ｘ _ｋ＋１，ｕ^ｘ _ｋ＋２，...，ｕ^ｘ _ｋ＋Ｎは、現在から未来までの各サンプリング点における入力値を示し、ｘ_ｋ，ｘ_ｋ＋１，ｘ_ｋ＋２，...，ｘ_ｋ＋Ｎは、現在から未来までの各サンプリング点における状態変数値を示す。また、同様に、ｐ^ｘ _ｋ，ｐ^ｘ _ｋ＋１，ｐ^ｘ _ｋ＋２，...，ｐ^ｘ _ｋ＋Ｎは、現在から未来までの各サンプリング点における出力値を示す。これらの変数を図７に示している。なお、式（４）においては、現在時点がちょうどｋサンプル目であるとする。

これらの入力値・出力値列を、以下の式（５）に示すように定義する。

式（３）、（４）、（５）を用いて未来の出力値列を計算すると、以下の式（６）を得る。

ここで、ＮΔｔ未来までの目標ＺＭＰ列をｖ^ｘ _ｋ＋１のハットとし、以下の式（７）により定める。なお、目標ＺＭＰ軌道（ｐ^ｘ _ｋ＋１のハットの列）は、予め適切な値が設定されるものとする。

モデル予測制御型軌道生成法では、最適化の評価関数は、以下の式（８）に示すように、予測区間内における目標ＺＭＰとＺＭＰの誤差の２乗と、入力値の２乗とを足し合わせた値を最小化する形により与えられる。

これはモデル予測制御の一般的な評価関数の与え方である。本例で言うと、第１項は、ＺＭＰを目標ＺＭＰに近づける役割を担い、第２項は、入力値が過剰に働くことを抑止する（すなわち、重心が発散することを抑制する）役割を担っているものと考えられる。

式（８）におけるＱ、Ｒは、上記２項をバランスするための重み行列である。重み行列Ｑ、Ｒを以下の式（９）に示す。この重み行列Ｑ、Ｒの各重み値は、制御対象に応じて予め適切な値が設定される。

式（６）を式（８）に示した評価関数に代入することによって、以下の式（１０）を得る。

ただし、式（１０）における行列Ｈ^ｃ _ｋ、ベクトルｆ^ｃ _ｋを、以下の式（１１）に示す。

上式より、式（８）に示した評価関数を定式化し、以下の式（１２）として示す。

上述した式展開は、ｙ方向にいても同様に成り立つ。このため、ｙ方向の評価関数を以下の式（１３）に示す。

式（１３）におけるベクトルｕ_ｋ、行列Ｈ_ｋ、ベクトルｆ_ｋを以下の式（１４）に示すように定める。

式（１２）および式（１３）をまとめると、以下の式（１５）に示す評価関数を得る。

次に、評価関数に対する制約条件を説明する。脚軌道生成では、足裏の範囲内（より正確には、足裏外縁から一定のマージンを設けて設定される多角形内）にＺＭＰが留まるような軌道を生成する。これにより、安定な歩行を実現できる。本実施の形態では、目標ＺＭＰを中心とする足裏範囲内にＺＭＰが収まるようにＺＭＰ制約範囲を設定する。本実施の形態に係るＺＭＰ制約範囲の一例を図９に示す。ＺＭＰ制約範囲は凸多角形により示される。このため、「制約範囲内にＺＭＰが留まる」との条件は、任意のベクトルλ^ｘ、λ^ｙ、ｄ^ｚを用いて、以下の式（１６）により示すことができる。なお、これらのベクトルλ^ｘ、λ^ｙ、ｄ^ｚの値は、予め適切な値が設定されるものとする。

従って、未来の各サンプリング点ｋ＋ｊ（ｊ＝１，・・・，Ｎ）において上述した式（１６）に示したＺＭＰ制約を仮定することにより、以下の式（１７）を得る。

ただし、式（１７）におけるベクトルηおよび行列Γを、以下の式（１８）により示す。

式（１７）に式（６）を代入することにより、以下の式（１９）に示す線形不等式を導くことができる。式（１９）は、ＺＭＰを支持多角形内に留めるという条件が、入力値列ベクトルｕ_ｋに関する線形不等式を用いて最終的に記述できることを示している。

ただし、式（１９）における行列Ｐ_ｋおよびベクトルｑ_ｋを、以下の式（２０）に示すように定める。

以上をまとめると、式（１５）および式（１９）より、最適化問題を以下の式（２１）に示す凸二次計画問題として定式化することができる。この凸二次計画問題は、高速かつ安定に求解することが可能である。モデル予測制御は、現在状態を入力して最適化問題を求解するまでのこの計算プロセスを、時間を進めながら毎周期実行し、入力値を決定していくものである。

２：着地位置修正を含んだモデル予測制御型重心軌道生成法への拡張について
次に、モデル予測制御型重心軌道生成を、着地位置修正を含めた定式化へと拡張する。上記１に説明したモデル予測制御型重心軌道生成は、例えば、ＩＣＨＲ２００６（"Trajectory Free Linear Model Predictive Control for Stable Walking in the Presence of Strong Perturbations"，Pierre-Brice Wieber，IEEE-RAS International Conference on Humanoid Robots，pp. 137 - 142 (2006)）においても提案されており、これ自体は公知技術である。本実施の形態では、このモデル予測制御型重心軌道生成法を、目標ＺＭＰへの追従誤差と着地位置修正量との重み付き和を最小化するものへと拡張し、着地までの残り時間に応じてこの重みを連続的に変化させる。これによって、最適な着地位置修正量および重心軌道を生成する新たな手法を提供する。

まず、着地位置修正量について説明する。本実施の形態では、未来の予測区間内に、最大で２歩分の着地位置が入っている場合を想定する（図１１を参照）。そして、ｘ方向におけるこの未来の２歩分のそれぞれの着地位置修正量を、Δｘ_ｆ１、Δｘ_ｆ２とする（図１０を参照）。すなわち、Δｘ_ｆ１は、遊脚の１歩目の着地位置を修正する着地位置修正量であり、Δｘ_ｆ２は、遊脚の２歩目の着地位置を修正する着地位置修正量である。なお、ｙ方向の説明はｘ方向の説明と基本的に同一であるため、以下、ｘ方向について説明する。

次に、目標ＺＭＰ修正量について説明する。目標ＺＭＰ修正量は、所定の目標ＺＭＰ修正パターンを用いて、遊脚の着地位置修正量を反映させることによって求める。所定の目標ＺＭＰ修正パターンは、遊脚が着地してからその遊脚と異なる他の脚（支持脚）が離地するまでの間における目標ＺＭＰ軌道のうちの対応する部分を修正するための所定のパターンである。

本実施の形態では、図１１に例示する目標ＺＭＰ軌道修正パターンを設定する。図１１に例示する目標ＺＭＰ軌道修正パターンは、１歩目の目標ＺＭＰ修正パターンと、２歩目の目標ＺＭＰ修正パターンを含んでいる。１歩目の目標ＺＭＰ修正パターンは、遊脚の１歩目が着地してからその遊脚と異なる他の脚が離地するまでの間（図１１において時刻ｔ_１からｔ_４の間）における目標ＺＭＰ軌道のうちの対応する部分を修正するために用いる。２歩目の目標ＺＭＰ修正パターンは、遊脚の２歩目が着地してからその遊脚と異なる他の脚が離地するまでの間（図１１において時刻ｔ_３からｔ_６の間）における目標ＺＭＰ軌道のうちの対応する部分を修正するために用いる。

図１１における１歩目の目標ＺＭＰ修正パターンを、以下の式（２２）を用いて示す。目標ＺＭＰ修正パターンは、１歩目の単脚期の間のみ１であり、それ以外の単脚期間では０、そして両脚期はその間を滑らかに接続する３次関数を用いて構成される。

両脚期間の３次関数は、具体的には以下の式（２３）を満たす関数である。

式（２３）において、ｆ_ｕｐ１（ｔ）、ｆ_ｄｎ１（ｔ）を以下の式（２４）に示す。なお、目標ＺＭＰ修正パターンを構成する関数はこれに限定されず、連続な関数であればよい。例えば、式（２４）に示す関数に代えて、線形な関数を使用するものとしてもよい。

式（２２）に示す目標ＺＭＰ修正パターンのパターン関数を、ＮΔｔ未来までのサンプリング点にわたって離散化すると、以下の式（２５）に示す時系列のベクトルｗ^ｆ１ _ｋを得る。

ただし、式（２５）において、ｓ_１，ｓ_２，ｓ_３，ｓ_４は、以下の式（２６）を満たす整数である。

同様にして、２歩目の目標ＺＭＰ修正パターンに関しても、時系列のベクトルｗ^ｆ２ _ｋを得ることができる。

この目標ＺＭＰ修正パターンｗ^ｆ１ _ｋ，ｗ^ｆ２ _ｋに対して着地位置修正量Δｘ_ｆ１，Δｘ_ｆ２を掛け合わせることで、目標ＺＭＰ修正量ｗ^ｆ１ _ｋΔｘ_ｆ１，ｗ^ｆ２ _ｋΔｘ_ｆ２を得ることができる。この目標ＺＭＰ修正量ｗ^ｆ１ _ｋΔｘ_ｆ１，ｗ^ｆ２ _ｋΔｘ_ｆ２を、元々の目標ＺＭＰ軌道（ｖ^ｘ _ｋ＋１のハット）に加え合わせる（このようすを図１２に示す。）。これによって、図１３に示すように、足裏内の中央に目標ＺＭＰ軌道を設定したまま、着地位置修正を反映させることができる。修正後の目標ＺＭＰ軌道を以下の式（２７）に示す。

そして、式（８）の目標ＺＭＰ軌道について式（２７）を用いて修正した上で、着地位置修正量についても最小化されるように２乗和の項を付け加える。これによって、以下の式（２８）に示す評価関数を得る。

また、以下の式（２９）に示すように、着地位置修正量Δｘ_ｆ１，Δｘ_ｆ２を入力ベクトルに加える。

さらに、式（２８）を展開することによって、以下の式（３０）を得る。

ただし、式（３０）における行列Ｈ^ｃ _ｋ、ベクトルｆ^ｃ _ｋを、以下の式（３１）に示す。

以後の式の展開については、式（１２）〜（１５）と同様の展開を行えばよいため、説明を省略する。

上式より、式（２８）の評価関数を、以下の式（３２）として定式化することができる。

また、ｙ方向についても式（２９）と同じようにして、以下の式（３３）に示すように入力ベクトルを拡張する。

これによって、以下の式（３４）を得ることができる。

式（３４）におけるベクトルｕ_ｋ、行列Ｈ_ｋ、ベクトルｆ_ｋを、以下の式（３５）に示すように定める。

式（３２）および式（３４）をまとめることによって、以下の式（３６）を得る。

次に、着地位置修正量の制約条件を説明する。着地位置修正量の制約条件については、例えば、環境や機体に応じて遊脚の着地可能領域（着地位置設定可能範囲）を凸多角形として抽出することができる（この技術は、例えば、先に出願された特開２０１１−２０６９０３号に開示されている。）。このため、着地位置修正量の制約条件は、ＺＭＰ制約と同様にして、任意のベクトルλ^ｘ _ｆ１、λ^ｙ _ｆ１、ｄ_ｆ１、λ^ｘ _ｆ２、λ^ｙ _ｆ２、ｄ_ｆ２を用いて、以下の式（３７）により示すことができる。これらのベクトルλ^ｘ _ｆ１、λ^ｙ _ｆ１、ｄ_ｆ１、λ^ｘ _ｆ２、λ^ｙ _ｆ２、ｄ_ｆ２の値は、遊脚の着地可能領域に応じて予め適切な値が設定されるものとする。

従って、式（１９）の目標ＺＭＰを式（２７）に置き換えた上で、着地位置の制約条件を含むように拡張することによって、以下の式（３８）を得る。

さらに、式（２９）および式（３３）を用いて式（３８）を整理することによって、以下の式（３９）を得る。

式（３９）における行列Ｐ_ｋおよびベクトルｑ_ｋを、以下の式（４０）に示すように定めた。

以上をまとめると、式（３６）および式（３９）より、最適化問題を以下の式（４１）に示す凸二次計画問題として定式化することができる。

この凸二次計画問題は、足裏範囲内にＺＭＰが留まるという「ＺＭＰ制約」と、足位置設定可能な領域内に着地位置を制限する「着地位置制約」という制約条件を設定し、目標ＺＭＰ追従と着地位置修正量の抑制とをバランスするように最適解を算出する構成である。

さらに、本実施の形態では、遊脚着地までの残り時間に応じて着地位置修正量の重みを小さくするように連続的に変化させる。これによって、着地位置を修正できる時間的な余裕がある間に、必要な分だけ早めに着地位置を修正することができ、より安定性を高めることができる。

さらに、本実施の形態では、遊脚着地までの残り時間に応じて着地位置修正量の重みを大きくするように連続的に変化させる。これによって、着地位置を修正できる時間的な余裕がある間に、必要な分だけ早めに着地位置を修正することができ、より安定性を高めることができる。

具体的には、遊脚着地までの残り時間が短くなるほど、着地位置修正量の重みを線形に大きくする。図１４（Ａ）に例示するように、着地までの残り時間が長いときには、足を踏み出す時間的な余裕があるため、１歩目の着地位置を大きく修正することが可能である。しかし、図１４（Ｂ）に例示するように、着地までの残り時間が短いときには、着地位置を修正することはほぼ不可能であるため、１歩目の着地位置はほとんど修正することができない。

３：実験結果
最後に、上記の重心軌道生成法を用いた場合の実験結果について述べる。実験では、ｘ、ｙ方向における外乱の有無を設定し、本手法を適用した場合と適用しない場合に分けて実験を行った。以下に実験結果を示す。
（１）ｘ方向のみ、外乱なしという条件において、本手法を適用した場合（図１５）
（２）ｘ方向のみ、外乱ありという条件において、着地位置修正をなしにした場合（図１６）
（３）ｘ方向のみ、外乱ありという条件において、本手法を適用した場合（図１７）
（４）ｘｙ方向において、外乱なしという条件において本手法を適用した場合（図１８、図１９）
（５）ｘｙ方向において、外乱ありという条件において本手法を適用した場合（図２０、図２１）

図１５に示すように、外乱なしの場合には、目標ＺＭＰ軌道にＺＭＰが追従するような重心軌道を生成できていることが分かる。また、図１６、図１７に示すように、２．３［ｓ］のタイミングにおいて突発的に大きな外乱が作用したときに、着地位置修正を行わない場合には、図１６に示すように重心軌道が発散してしまい、歩行を持続することができなくなる。これに対して、本手法を適用して着地位置修正を行った場合には、図１７に示すように着地位置を修正することによってより高い安定性を確保することができる。注目すべき点は、図１７から分かるように、外乱が作用した直後はＺＭＰ制約の限界までＺＭＰを動かして支持脚により踏ん張っており（つまり、目標ＺＭＰへの追従を一時的に犠牲にして安定化を図っている）、着地位置修正によって転倒を回避した後は、ＺＭＰが目標ＺＭＰに直ぐに漸近して、目標ＺＭＰへの追従を達成していることである。すなわち、本実施の形態によって、高い安定性とＺＭＰ追従性を両立していることを示している。

図１８と図１９、および図２０と図２１は、ｘｙ両方向について本手法を適用した結果を示す。図２０、図２１に関しては、２．３［ｓ］および６［ｓ］のタイミングにおいて突発的に大きな外乱が作用した場合を示す。図２０では、着地位置制約も図示したが、ＺＭＰ軌道がＺＭＰ制約内、着地位置が着地位置制約内に収まっていることも確認できる。これらの結果から、ｘｙ方向に関しても、本手法により、制約の範囲内にＺＭＰおよび着地位置を制限しながら、高い安定性とＺＭＰ追従性を両立できていることを確認できる。

４：重心軌道生成処理の初期化ルーチン処理および周期実行ルーチン処理の説明
以下、本実施の形態に係る重心軌道生成処理の初期化ルーチン処理および周期実行ルーチン処理の詳細を図２２及び図２３を用いて説明する。

４−１：重心軌道生成処理の初期化ルーチン処理
図２２は、制御コントローラ２０が実行する重心軌道生成処理の初期化ルーチン処理のフローチャートを示す。初期化ルーチンは、後述する図２３に示す周期実行ルーチン開始前の初期化時に実行される。

制御コントローラ２０は、サンプリング時間間隔である周期Δｔ、基準重心高さｈ、およびサンプリング数Ｎを設定する（Ｓ１００１）。制御コントローラ２０は、システム行列およびベクトルを計算する（Ｓ１００２）。より具体的には、上述した式（３）に示した行列Ａ、ｂ、ベクトルｃを計算する。制御コントローラ２０は、上述した式（９）に示した重み行列Ｑ、Ｒと、式（２８）に示した重みＳ_２を設定する（Ｓ１００３）。制御コントローラ２０は、上述した式（６）に示した出力値系列の係数（Ｇ_１、Ｇ_２）を計算する（Ｓ１００４）。

４−２：重心軌道生成処理の周期実行ルーチン処理の説明
図２３は、制御コントローラ２０が実行する重心軌道生成処理の周期実行ルーチン処理のフローチャートを示す。制御コントローラ２０は、ロボット１０の現在状態ｘ_ｋ、ｙ_ｋ（重心位置、速度、加速度）を設定する（Ｓ１１０１）。ロボット１０の現在状態は、ロボット１０自らが現在の重心位置・速度・加速度を取得して設定することができる。また、現在状態は、ユーザー、あるいは制御コントローラ２０に指令を与える上位コントローラによって設定するものとしてもよい。

制御コントローラ２０は、目標ＺＭＰ軌道（ｖ^ｘ _ｋ＋１のハット、ｖ^ｙ _ｋ＋１のハット）を設定する（Ｓ１１０２）。制御コントローラ２０は、上述した式（４２）に示したように、１歩目の着地位置修正量の重みを遊脚着地までの残り時間に応じて計算して設定する（Ｓ１１０３）。なお、制御コントローラ２０は、設定された目標ＺＭＰ軌道に基づいて、遊脚の着地および支持脚の離地時間を求めることができる。制御コントローラ２０は、上述した式（２４）に示した目標ＺＭＰ修正パターンｗ^ｆ１ _ｋ，ｗ^ｆ２ _ｋのパターン関数を計算する（Ｓ１１０４）。

制御コントローラ２０は、上述した式（３５）に示したＨ_ｋ、ｆ_ｋを用いて、上述した式（３６）に示した評価関数を設定する（Ｓ１１０５）。制御コントローラ２０は、遊脚の着地可能領域を計算する（Ｓ１１０６）。制御コントローラ２０は、例えば、環境や機体に応じて遊脚の着地可能領域を凸多角形として抽出する。制御コントローラ２０は、上述した式（３７）〜（４０）に示した不等式制約条件（Ｐ_ｋ、ｑ_ｋ）を計算する（Ｓ１１０７）。制御コントローラ２０は、上述した式（４１）に示した凸二次計画問題を求解する（Ｓ１１０８）。

制御コントローラ２０は、凸二次計画問題の求解結果ｕ_ｋを用いて、重心軌道を修正する（Ｓ１１０９）。すなわち、現在の時刻から未来のＮ個のサンプリング点における各重心位置を計算する。ｊは０〜Ｎ−１までの値を取る整数である。制御コントローラ２０は、凸二次計画問題の求解結果ｕ_ｋに含まれる着地位置修正量Δｘ_ｆ１，Δｘ_ｆ２，Δｙ_ｆ１，Δｙ_ｆ２を用いて、遊脚の着地位置を修正する（Ｓ１１１０）。

制御コントローラ２０は、時間Δｔ後から開始する次の周期での軌道生成処理を行うために、周期更新を行う（Ｓ１１１１）。すなわち、ｋを１増加させ、ｔをΔｔ増加させる。

５：最適化指標に関する補足説明
上述した式（２８）の最適化指標について、以下に補足して説明する。式（２８）は、ＺＭＰ軌道と修正後の目標ＺＭＰ軌道の差を２乗した値を含む部分（以下、第１部分と称する場合がある。）と、入力値ｕ^ｘ _ｋを２乗した部分（以下、第２部分と称する場合がある。）と、着地位置修正量を２乗した値を含む部分（以下、第３部分と称する場合がある。）と、を備えて構成されている。

第１部分は、ＺＭＰ軌道と修正後の目標ＺＭＰ軌道の誤差の２乗に重みを付けた構成であり、目標ＺＭＰ軌道を修正するための目標ＺＭＰ修正量を含んでいる。式（２８）に示す評価関数において、第１部分は、ＺＭＰ軌道を修正後の目標ＺＭＰ軌道に近づける働きを担っている（すなわち、踏ん張り量をできる限り抑制する働きを担っている。）。

修正後の目標ＺＭＰ軌道は、元々の目標ＺＭＰ軌道を、目標ＺＭＰ修正量を用いて修正することにより得られる。目標ＺＭＰ修正量は、着地位置修正量および目標ＺＭＰ修正パターンを用いて得られる。具体的には、例えば、離散化された１歩目の目標ＺＭＰ修正パターンｗ^ｆ１ _ｋ（図２４に示す。）に対して着地位置修正量Δｘ_ｆ１を係数として掛け合わせることで１歩目の目標ＺＭＰ修正量を求める。この目標ＺＭＰ修正量を元々の目標ＺＭＰ軌道（図２５に示す。）に対して加えることによって、修正後の目標ＺＭＰ軌道を得ることができる（図２６に示す。）。さらに、２歩目の目標ＺＭＰ修正パターンｗ^ｆ２ _ｋに対して着地位置修正量Δｘ_ｆ２を係数として掛け合わせることで２歩目の目標ＺＭＰ修正量を求め、この目標ＺＭＰ修正量を元々の目標ＺＭＰ軌道に対して加えることによって、２歩分の目標ＺＭＰ修正量を反映させた、修正後の目標ＺＭＰ軌道を得ることができる（図２７に示す。）。

第２部分は、重心が発散するのを防ぐ働きを担っている。重心が発散すると入力値ｕ^ｘ（ｔ）＝ｘ^{（・・・）} _ｇ（ｘ_ｇの三階微分）が過大な値になるため、この項は発散抑制の効果を有している。これは、通常のモデル予測制御と同様であり、詳細な説明を省略する。

第３部分は、着地位置修正量に重みを付けた構成であり、着地位置修正量を抑制する働きを担っている。図５に例示した着地位置修正量Δｘ_ｆ１，Δｘ_ｆ２に関して、重み付き着地位置修正量の２乗を最小化することにより、着地位置修正量をできる限り０に抑制し、これにより、着地位置の修正をできる限り抑制することができる。

以上をまとめて、各部分の役割を図２８に示す。３つの部分は全てトレードオフの関係にあるため、それぞれに重み付けをして足し合わせた評価関数になっている。すなわち、本実施の形態に係るモデル予測制御では、最適化の評価関数を、重み付き目標ＺＭＰ修正量と重み付き着地位置修正量の和を含む値を最小化する構成とし、制御コントローラ２０が、モデル予測制御を実行して、目標ＺＭＰ修正量と着地位置修正量の最適解を算出する。そして、制御コントローラ２０は、モデル予測制御において得た結果ｕ_ｋを用いて、重心軌道を修正し、また、算出した着地位置修正量を用いて元々の着地予定位置を修正する。これによって、目標ＺＭＰ軌道に追従して歩行する脚式ロボットの制御に関して、高い安定性と目標ＺＭＰへの追従性を両立することができる。

＜その他の実施の形態＞
実施の形態１では、遊脚の着地位置修正について２歩分の着地位置を修正する例を示した。しかしながら、本発明はこれに限定されず、遊脚の着地位置修正について１歩分の着地位置を修正するものとしてもよいし、３歩分以上の着地位置を修正するものとしてもよい。遊脚の着地位置修正について１歩分のみの着地位置を修正する場合と比較して、２歩分の着地位置についても修正する場合には、それぞれの着地位置に関する着地位置の修正量に関してより余裕を確保することができ、１歩目の着地位置修正量をより抑制することができる。この結果、目標ＺＭＰへの追従性をより高めることができる。さらに、３歩分以上の着地位置を修正するものとした場合には、計算時間は増加するものの、それぞれの着地位置修正量をより抑制することができる。

また、重みＱ、Ｒ、Ｓ（Ｓ_１、Ｓ_２）の設定手法について、上述した実施の形態１に限定されず、ユーザー、あるいは制御コントローラ２０に指令を与える上位コントローラが、目的に応じて、予め適切な値を設定するようにしてもよい。

例えば、着地位置修正量の重みＳ（Ｓ_１、Ｓ_２）を大きく設定することによって、着地位置修正量をより小さく抑制することができ、この結果、指定した着地位置からできる限り離れないようにロボット１０の歩行を制御することができる。

また例えば、ＺＭＰ軌道と修正後の目標ＺＭＰ軌道の差に関する重みＱを大きく設定することによって、目標ＺＭＰ修正量をより小さく抑制することができ、この結果、外力に対して安全確保を優先させてロボット１９を過度に踏ん張らせないように制御することができる。

また例えば、遊脚着地までの残り時間に応じて、重みＱやＳを大きくあるいは小さくするように適応的に変更するようにしてもよい。

上述した複数の実施の形態は、適宜組み合わせることも可能である。さらに、本発明は上述した実施の形態のみに限定されるものではなく、既に述べた本発明の要旨を逸脱しない範囲において種々の変更が可能であることは勿論である。例えば、脚式ロボットは、２脚以上の脚を有していてもよく、３脚以上のロボットであってもよい。

１０ 脚式ロボット、
２０ 制御コントローラ、

Claims (8)

1. 目標ＺＭＰ軌道に追従して歩行する脚式ロボットの制御方法であって、
   モデル予測制御における最適化の評価関数を、重み付き目標ＺＭＰ修正量と重み付き着地位置修正量の和を含む値を最小化する構成とし、
   遊脚着地までの残り時間が短くなるほど、前記着地位置修正量の重みを大きくするように変化させ、
   前記モデル予測制御を実行して、前記目標ＺＭＰ修正量と前記着地位置修正量の最適解を算出する、
   脚式ロボットの制御方法。
2. 前記遊脚着地までの残り時間が短くなるほど、前記着地位置修正量の重みを線形に大きくする、
   請求項１に記載の脚式ロボットの制御方法。
3. 前記着地位置修正量は、
   遊脚の１歩目の着地位置を修正する第１の着地位置修正量と、
   遊脚の２歩目の着地位置を修正する第２の着地位置修正量と、を含み、
   前記目標ＺＭＰ修正量は、
   前記遊脚の１歩目が着地してから当該遊脚と異なる他の脚が離地するまでの間における前記目標ＺＭＰ軌道のうちの対応する部分を修正するための所定の第１の目標ＺＭＰ修正パターンと、前記１歩目の着地位置修正量と、を用いて求める１歩目の目標ＺＭＰ修正量と、
   前記遊脚の２歩目が着地してから当該遊脚と異なる他の脚が離地するまでの間における前記目標ＺＭＰ軌道のうちの対応する部分を修正するための所定の第２の目標ＺＭＰ修正パターンと、前記２歩目の着地位置修正量と、を用いて求める２歩目の目標ＺＭＰ修正量と、を含む、
   請求項１または２に記載の脚式ロボットの制御方法。
4. 前記脚式ロボットが、２脚歩行ロボットである、
   請求項１〜３いずれか１項に記載の脚式ロボットの制御方法。
5. 目標ＺＭＰ軌道に追従して歩行する脚式ロボットであって、
   モデル予測制御における最適化の評価関数を、重み付き目標ＺＭＰ修正量と重み付き着地位置修正量の和を含む値を最小化する構成とし、
   遊脚着地までの残り時間が短くなるほど、前記着地位置修正量の重みを大きくするように変化させ、
   前記モデル予測制御を実行して、前記目標ＺＭＰ修正量と前記着地位置修正量の最適解を算出する、
   脚式ロボット。
6. 前記遊脚着地までの残り時間が短くなるほど、前記着地位置修正量の重みを線形に大きくする、
   請求項５に記載の脚式ロボット。
7. 前記着地位置修正量は、
   遊脚の１歩目の着地位置を修正する第１の着地位置修正量と、
   遊脚の２歩目の着地位置を修正する第２の着地位置修正量と、を含み、
   前記目標ＺＭＰ修正量は、
   前記遊脚の１歩目が着地してから当該遊脚と異なる他の脚が離地するまでの間における前記目標ＺＭＰ軌道のうちの対応する部分を修正するための所定の第１の目標ＺＭＰ修正パターンと、前記１歩目の着地位置修正量と、を用いて求める１歩目の目標ＺＭＰ修正量と、
   前記遊脚の２歩目が着地してから当該遊脚と異なる他の脚が離地するまでの間における前記目標ＺＭＰ軌道のうちの対応する部分を修正するための所定の第２の目標ＺＭＰ修正パターンと、前記２歩目の着地位置修正量と、を用いて求める２歩目の目標ＺＭＰ修正量と、を含む、
   請求項５または６に記載の脚式ロボット。
8. 前記脚式ロボットは、２脚歩行ロボットである、
   請求項５〜７いずれか１項に記載の脚式ロボット。

Images