(57)【特許請求の範囲】
【請求項１】
上体部と、上体部に腰関節を介して連結されたＷＡＩＳＴリンクと、ＷＡＩＳＴリンクに脚関節を介して連結された２本の脚と、前記２本の脚に膝関節及び足首関節を備えた歩行ロボットの特性を台車に乗った倒立振子によってモデル化し、ロボットの重心位置、重心速度、ＺＭＰ位置の計測値をもとに計算したＺＭＰ位置の指令値に応じて腰関節をロール軸周りに回転させるフィードバックにより前記上体部の姿勢を保ったまま前記ＷＡＩＳＴリンクを回転させ全体を安定化することを特徴とする歩行ロボットのＺＭＰ制御装置において、
ＺＭＰ位置の制御遅れに対する逆システムを用いてＺＭＰ位置の制御遅れを補償することを特徴とする歩行ロボットのＺＭＰ制御装置。
【請求項２】
前記逆システムの高周波ノイズを低減するためにモデルマッチング制御を用いてＺＭＰ位置の制御遅れを補償することを特徴とする請求項１記載の歩行ロボットのＺＭＰ制御装置。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は、サービスロボット、エンターテインメント分野等に用いられる人間型ロボット、恐竜型ロボット等の２足歩行ロボットに適用するための、歩行ロボットのＺＭＰ制御装置に関する。
【背景技術】
【０００２】
人間型ロボット、恐竜型ロボット等の２足歩行ロボットでは、その運動を制御するためにゼロモーメントポイント（ＺＭＰ）と呼ばれる量（ＺＭＰ位置）を用いることが知られている。ロボットの運動とＺＭＰとの関係を理解するため、まず従来技術（特許文献１、特許文献２参照）について説明する。
【０００３】
図１に両脚を開いて直立している簡単な２足歩行ロボットを示す。床反力の制御を容易にするため、ロボットの足裏にはスプリングないし、それに相当する弾性要素がついている。また、ロボットの足部には力を計測するセンサが取り付けられており、床面からロボットに作用する力を計測することができる。
【０００４】
図１（ａ）では両脚の間に重心が位置しており、静止した状態を示している。床面からロボットに作用する床反力の中心点をＺＭＰと呼び、図１（ａ）では、ＺＭＰは重心の真下に位置している。このとき、前足と後足のスプリングはロボットの体重により与圧された状態にある。
ＺＭＰ位置を後方に移動させたい場合は、図１（ｂ）のように後脚の膝を伸展させて足部を押し下げスプリングを圧縮するとともに、前脚の膝を屈曲させ足部を持ち上げてスプリングの与圧を開放する。その結果、前脚足部からの床反力が減少、後脚足部からの床反力が増大し、ＺＭＰ位置は後方へ移動する。
ＺＭＰ位置を前方に移動させたい場合は、図１（ｃ）のように前脚の膝を伸展させて足部を押し下げスプリングを圧縮するとともに、後脚の膝を屈曲させて足部を持ち上げスプリングの与圧を開放する。その結果、前脚からの床半力が増大、後脚足部からの床反力が減少し、ＺＭＰ位置は前方へ移動する。
【０００５】
図１（ａ）（ｂ）（ｃ）のロボットは、それぞれ図２（ａ）（ｂ）（ｃ）のような台車に乗った倒立振子によってモデル化することができる。図１のＺＭＰ位置と重心の位置が、それぞれ図２の台車と倒立振子重心の位置に対応する。さらに、図２の倒立振子は台車上の
回転軸まわりに自由回転できるものとする。
【０００６】
図２（ａ）では、台車（ＺＭＰ）が重心の真下にあるため重心は静止したままである。図２（ｂ）では台車に対して重心が右にあるため倒立振子は右に倒れようとする。つまり、重心は右に加速する。図２（ｃ）では台車に対して重心が左にあるため倒立振子は左に倒れようとする。つまり、重心は左に加速する。このダイナミクスは近似的に次式であらわすことができる。
【０００７】
【数１】
0005605624.tif 000002
【０００８】
ここで、ｘは重心位置、ｐはＺＭＰ位置、ｇは重力加速度、ｚｈは重心の高さを表す。実際の２足歩行ロボットの運動も近似的に上式で表せることが知られている。
なお、実際のロボットの運動は三次元なので、ロボットの進行方向と左右方向についてそれぞれ式（１）と同じ方程式を立てる必要がある。
【先行技術文献】
【特許文献】
【０００９】
【特許文献１】特許第３１４８８２９号公報（脚式移動ロボットの歩行制御装置）
【特許文献２】特許第４１１３９４８号公報（操作力発生装置）
【特許文献３】特許第３８３４６２９号公報（歩行ロボットの歩行歩容生成装置）
【非特許文献】
【００１０】
【非特許文献１】梶田、森澤他「２足歩行制御のための倒立振子安定化について」第２７回日本ロボット学界学術講演会予稿集  ＲＳＪ２００９ＡＣ２Ｐ１−０１，２００９年
【非特許文献２】野波、西村著「Ｍａｔｌａｂによる制御理論の基礎」東京電機大学出版局  ｐ．２００〜
【発明の概要】
【発明が解決しようとする課題】
【００１１】
図１からわかるように、従来の手法ではＺＭＰ位置を制御するためにロボットはある程度膝を曲げている必要がある。図３のようにロボットの両膝が伸びきった状態では、膝を伸展させて足部を押し下げられないし、膝を屈曲させて足部を持ち上げる場合にもその量は小さい。従って、両脚足部の上下動をうまく操作できないため、ＺＭＰを希望する位置に持ってゆくことが難しい。ゆえに、従来の手法では、ファッションモデルのように膝を伸ばした優美な歩行を制御することは難しい。これが第一の問題点である。
【００１２】
第二の問題点として、両脚の膝が曲がりＺＭＰ位置を制御しやすい図１のような状態であっても、実際のロボットでは目標ＺＭＰ位置が実際のＺＭＰ位置として実現されるまでに遅れが発生する。これは、「目標ＺＭＰ位置→脚の運動（足部の上下動）→足部のバネ要素の変形→ＺＭＰ位置」という段階を経る過程で生ずるものであり、模式的に書けば図４のようになる。以後、これを「ＺＭＰ位置の制御遅れ」と呼ぶ。
従来の２足歩行ロボットではＺＭＰ位置の制御遅れの影響が十分に考慮されていなかったため、十分な精度でＺＭＰと重心の軌道を制御できなかった。例えば、一歩０．５秒の高速な歩行において、０．１秒のＺＭＰ位置の制御遅れが存在すれば、大きな影響が生ずることは自明である。また、ＺＭＰ位置を精度よく制御できないため、ロボットの足部を人間と同程度に小さく製作すると、転倒しやすいという問題があった。
【課題を解決するための手段】
【００１３】
上記第一の問題は、ロボットに腰関節を追加し、これを用いて左右の脚を上下させることで解決できる。
上記第二の問題は、（１）ＺＭＰ位置の制御遅れを、逆システムを用いて補償する、（２）ＺＭＰ位置の制御遅れを、モデルマッチング制御によって補償する、のいずれかを用いることで解決できる。
【００１４】
すなわち、本発明の歩行ロボットのＺＭＰ制御装置は、上体部と、上体部に腰関節を介して連結されたＷＡＩＳＴリンクと、ＷＡＩＳＴリンクに脚関節を介して連結された２本の脚と、前記２本の脚に膝関節及び足首関節を備えた歩行ロボットの特性を台車に乗った倒立振子によってモデル化し、ロボットの重心位置、重心速度、ＺＭＰ位置の計測値をもとに計算したＺＭＰ位置の指令値に応じて腰関節をロール軸周りに回転させるフィードバックにより前記上体部の姿勢を保ったまま前記ＷＡＩＳＴリンクを回転させ全体を安定化することを特徴とする歩行ロボットのＺＭＰ制御装置において、ＺＭＰ位置の制御遅れに対する逆システムを用いてＺＭＰ位置の制御遅れを補償することを特徴とする。
また、本発明の歩行ロボットのＺＭＰ制御装置は、さらに、逆システムの高周波ノイズを低減するためにモデルマッチング制御を用いてＺＭＰ位置の制御遅れを補償することを特徴とする。
【発明の効果】
【００１５】
本発明によれば、ファッションモデルのように膝の伸展した優美な２足歩行を実現することが可能となるとともに、正確なＺＭＰ位置の制御が実現できるため、従来よりも小さな、人間に近いサイズの足部を持ったロボットを安定に歩行させることが可能となる。その結果として、本発明により、従来に比べより人間に近いロボットの歩行を実現することが可能となる。
【図面の簡単な説明】
【００１６】
【図１】２足歩行ロボットと目標重心位置を示す図。
【図２】台車付の倒立振子によるモデルを示す図。
【図３】両膝が伸びきった状態のロボットを示す図。
【図４】ＺＭＰ位置の制御遅れを示す図。
【図５】本発明の腰関節の追加を示す図。
【図６】図５の腰関節を利用して、膝関節が完全に伸展した場合に、右足にＺＭＰ位置を移動させる図。
【図７】図５の腰関節を利用して、膝関節が完全に伸展した場合に、左足にＺＭＰ位置を移動させる図。
【図８】歩行パターンの性質を示す図。
【図９】具体的な歩行パターンの例（四歩足踏み）を示す図。
【図１０】ＺＭＰ位置の制御遅れの存在するシステム構造を示す図。
【図１１】目標ＺＭＰ位置を直接システムに与えた場合を示す図。
【図１２】目標パターンのＺＭＰ位置に修正を加える方法（従来技術）を示す図。
【図１３】従来の方法による歩行の安定化。
【図１４】本発明の逆システムによる報償方式を示す図。
【図１５】本発明の逆システムを用いた制御系による制御シミュレーション結果を示す図。
【図１６】本発明のモデルマッチング制御を用いた安定化制御系を示す図。
【図１７】本発明のモデルマッチング制御による制御例を示す図。
【発明を実施するための形態】
【実施例】
【００１７】
（腰関節の追加）
まず、膝の伸展した歩行動作において、ＺＭＰ位置が制御不能となる問題を解決するため、ロボットの構造を見直し、図５のように腰関節を新たに追加する。
腰関節は、ロール、ピッチ、ヨーの３軸があることが望ましいが、最低限ロール軸（進行方向まわりの回転）があれば良い。説明の都合上、腰関節より下の部分をＷＡＩＳＴリンク、腰関節より上の部分をＣＨＥＳＴリンクと呼ぶ。
【００１８】
図６、図７のようにこの腰関節を利用することで、膝が完全に伸展した場合であってもＺＭＰ位置を制御することが可能となる。
図６では、ＣＨＥＳＴリンクの姿勢を保ったままＷＡＩＳＴリンクを右に回転させることで、右脚を下げ、左脚を持ち上げている。この結果、ＺＭＰ位置を右足部に移動させることができる。
図７では、ＣＨＥＳＴリンクの姿勢を保ったままＷＡＩＳＴリンクを左に回転させることで、右脚を持ち上げ、左脚を下げている。この結果、ＺＭＰ位置を左足部に移動させることができる。
【００１９】
第二の問題の解決策を示すため、まず従来技術とその問題点について改めて説明する。まず、図２のモデルと式（１）に示されたダイナミクスを想定し、理想的な状態での歩行パターンを作成する。具体的な方法の例としては、上記特許文献３がある。生成された歩行パターンは図８の関係を満たしている。
すなわち、目標ＺＭＰ  ｐｐｇをシステムＧ（ｓ）に加えると、目標重心位置ｘｐｇ、目標速度ｖｐｇ、目標ＺＭＰ  ｐｐｇが得られる。ここで、Ｇ（ｓ）は式（１）をラプラス変換したものであり次式で与えられる。
【００２０】
【数２】
0005605624.tif 000003
【００２１】
ここで、ωは、倒立振子の特性を表すパラメータで、ω＝（ｇ／ｚｈ）１／２。
具体的な歩行パターンの例を図９に示す。これはロボットがその場で四歩足踏みを行った後、静止する動きを表している。
実際のロボットでは図４に示したようなＺＭＰ位置の制御遅れがあるため、指令値ｐｄから重心位置、速度、ＺＭＰ位置までのシステムは図１０のように書ける。ここで簡単のためＺＭＰ位置の制御遅れを一次遅れで近似するものとする。ＴはＺＭＰ位置の制御遅れを示す時定数であり典型的には５０ｍｓ程度である。
【００２２】
まず、図１０の入力として歩行パターンのＺＭＰ位置を直接与えた場合を考えてみよう。
【００２３】
【数３】
0005605624.tif 000004
【００２４】
図９の歩行パターンについてシミュレーションを行った結果を図１１に示す。
図１１の上側のグラフから、ＺＭＰ位置の指令値（細破線）に対して、実ＺＭＰ位置（細線）が遅れて応答していること、また、歩行パターンの重心位置（太破線）に対して、実際の重心位置（太線）がしだいにずれてゆき、時刻４ｓのあたりで、重心位置が歩行パターンから完全に離れていること、すなわち、転倒していることがわかる。
図１１の下側のグラフでは、歩行パターンの重心速度（細線）と実際の重心速度（太線）が示されている。ここからも重心が歩行パターンに追従できていないことがわかる。
すなわち、この方法では，実際のシステムの状態［ｘ  ｖ  ｐ］を歩行パターンで指定される状態［ｘｐｇ  ｖｐｇ  ｐｐｇ］に追従させることは出来ない。その理由は、以下の２つである。
（１）ＺＭＰ位置の制御遅れによって正しいＺＭＰ位置が与えられない
（２）は不安定なシステムなので、重心位置と速度の誤差が指数関数的に増大する
【００２５】
この問題を解決するには、歩行パターンのＺＭＰ位置に対して次のように修正量を加えればよい。
【００２６】
【数４】
0005605624.tif 000005
【００２７】
Δｐは歩行パターンのＺＭＰ位置に対する修正量であり、次式のように歩行パターンと実際の状態をもとに計算する。
【００２８】
【数５】
0005605624.tif 000006
【００２９】
ここで、ｋｘ，ｋｖ，ｋｐは状態フィードバックゲインであり、ＺＭＰ位置制御遅れのある倒立振子を安定化するため極配置法等を用いて適切な値に設定される。具体的な計算法については、非特許文献１を参考にされたい。
【００３０】
以上をまとめたシステム全体のブロック図を、図１２に示す。
図を見やすくするため、状態フィードバックゲインを１×３の行列Ｋとしてまとめた。すなわち、
【００３１】
【数６】
0005605624.tif 000007
【００３２】
この制御のシミュレーション結果を図１３に示す。図１３の上側のグラフにおいて、実際のＺＭＰ位置（細線）は図９に示した歩行パターンで指定されたものとは大きく形状が異なっている。同じグラフで重心軌道（太線）は歩行パターンの重心軌道（太破線）に対して追従はしているものの、一歩ごとに大きな誤差が生じていることがわかる。同様に、図１３の下側のグラフで、重心速度（太線）は歩行パターンの重心速度（細線）に対して、大きな制御誤差が生じている。これらの制御誤差を修正するため式（４）（５）で与えられるＺＭＰ位置の指令値（図１３上側のグラフの細破線）も結果として歩行パターンからかけ離れたものとなっている。
以上のように、図１２の方法では、ＺＭＰ位置、重心位置、重心速度について歩行パターンどおりに正確に制御できないことがわかる。
【００３３】
（逆システムによる補償方式）
先のシミュレーションで見られた大きな制御誤差は、ＺＭＰ位置の制御遅れが正しく考慮されていないために生じたものである。そこで、歩行パターンのＺＭＰ位置にフィルタをかけて、事前にＺＭＰ位置の遅れを補償するという発想が生まれる。すなわち、
【００３４】
【数７】
0005605624.tif 000008
【００３５】
とすることで、指令値に対するＺＭＰ位置の遅れを打ち消すわけである。ここで（１＋ｓＴ）は、ＺＭＰ位置の制御遅れに対する「逆システム」である。実際には厳密な逆システムは実現不可能であるが、後述する段落００４７〜００６０に示す方法で近似的に実現できる。
逆システムを利用した制御システムのブロック線図の具体例を図１４に示す。
【００３６】
逆システムによる補償を行った場合のシミュレーション結果を図１５に示す。
図１５の上側のグラフにおいて、逆システムによって作り出されたＺＭＰ位置の目標値（破線）は、一歩ごとに素早く立ち上がり、少し行過ぎてから目標位置に戻っている。これにより、結果的に本来の歩行パターンに近いＺＭＰ位置が実現され（細線）、目標歩行パターンに対する重心（太線）および速度（下のグラフ）の追従性能が大幅に改善されていることがわかる。
【００３７】
（モデルマッチング制御による補償方式）
逆システムを用いたＺＭＰ位置の制御遅れ補償では目標の歩行パターンに高周波のノイズが存在すると、これを増幅してしまい、２足歩行ロボットの挙動が不安定になるケースがある。この問題を解決するため、モデルマッチング制御（非特許文献２参照）を利用することが考えられる。モデルマッチング制御では、目標歩行パターンに、前置フィルタを作用させることで、ノイズを低減させることができる。
まず、ＺＭＰ位置の目標値は次式で与えられる。
【００３８】
【数８】
0005605624.tif 000009
【００３９】
ここでＦｐｒｅ（ｓ）は前置フィルタである。Ｆｐｒｅ（ｓ）の具体例としては次式のように一次遅れ系を設定する。
【００４０】
【数９】
0005605624.tif 000010
Ｔｐｒｅとしては、ＺＭＰ位置の制御遅れ時定数Ｔより短く設定する（等身大のヒューマノイドの場合１０〜２０ｍｓ程度）。これを（８）式に代入すると次式となる。
【００４１】
【数１０】
0005605624.tif 000011
【００４２】
これと別個に、歩行パターンの重心位置、速度、ＺＭＰ位置にもそれぞれ前置フィルタを加える。
【００４３】
【数１１】
0005605624.tif 000012
【００４４】
これをもとに誤差を計算する。
【００４５】
【数１２】
0005605624.tif 000013
【００４６】
前置フィルタを介して歩行パターンを使用するため、歩行パターンに多少のノイズが存在しても安定な歩行動作を行うことができる。
モデルマッチング制御を用いたブロック線図の具体例を図１６に示す。
モデルマッチング制御を用いた場合のシミュレーション結果を図１７に示す。逆システムを用いた場合に比べ、細破線で示されたＺＭＰ位置の指令値が多少なめらかになっている一方で、重心と重心速度に関しては良好に歩行パターンに追従できていることがわかる。
【００４７】
（一次遅れの逆システムの具体的な計算法）
入力ｕから出力ｘを与える時定数Ｔの一次遅れシステムは次式で与えられる。
【００４８】
【数１３】
0005605624.tif 000014
【００４９】
出力ｘから入力ｕを与える「逆システム」は次式となる。
【００５０】
【数１４】
0005605624.tif 000015
【００５１】
（１４）式は因果律を満たさないため、通常の手法では実現できないが、離散化システムとして実装すると、以下のように近似的に（１４）と等価な逆システムを実現することができる。
まず、（１３）式を連続時間系のダイナミクスとして書き直す。
【００５２】
【数１５】
0005605624.tif 000016
【００５３】
（１５）式をサンプリングタイムΔで離散化すると次式を得る。
【００５４】
【数１６】
0005605624.tif 000017
【００５５】
ここで、ｘｋ，ｕｋは現在（第ｋ番目）の状態と入力、ｘｋ＋１は１サンプリングタイム後の状態を表す。
時定数ＴとサンプリングタイムΔは一定なので、次のような定数ａ，ｂを導入する。
ａ：＝ｅｘｐ（−Δ／Ｔ）
ｂ：＝１−ｅｘｐ（−Δ／Ｔ）
これを用いれば、（１６）式は次のように簡単に書ける。
【００５６】
【数１７】
0005605624.tif 000018
【００５７】
上式（１７）をｕｋについて解くと次式を得る。
【００５８】
【数１８】
0005605624.tif 000019
【００５９】
ここで、サンプリング時間が十分短ければ、（１８）式の右辺を１ステップだけ前に戻しても、おおよそ正しいｕが得られると期待できる。すなわち、
【００６０】
【数１９】
0005605624.tif 000020
【００６１】
この式は、現在の状態ｘｋと、一つ前の状態ｘｋ−１を用いて、現在の入力値ｕｋを与える「逆システム」となっている。
【図３】
0005605624.tif 000021
【図４】
0005605624.tif 000022
【図８】
0005605624.tif 000023
【図１０】
0005605624.tif 000024
【図１２】
0005605624.tif 000025
【図１４】
0005605624.tif 000026
【図１６】
0005605624.tif 000027
【図１】
0005605624.tif 000028
【図２】
0005605624.tif 000029
【図５】
0005605624.tif 000030
【図６】
0005605624.tif 000031
【図７】
0005605624.tif 000032
【図９】
0005605624.tif 000033
【図１１】
0005605624.tif 000034
【図１３】
0005605624.tif 000035
【図１５】
0005605624.tif 000036
【図１７】
0005605624.tif 000037