JP5392125B2 - ２脚歩行ロボット及びそのための歩容データ生成方法 - Google Patents

Description

本発明は、２脚歩行ロボット（Ｂｉｐｅｄ Ｒｏｂｏｔ）に関する。特に、ロボット動作中のリアルタイム処理に適した歩容データを効率的に生成する技術に関する。

歩行のための脚関節の目標角の時系列データは歩容データと呼ばれる。２脚歩行ロボットは、適切な歩容データを用いないと転倒してしまう。転倒するか否かは、ロボットが現実に歩行して初めて確認できる事項であるので、本明細書では、理論上転倒せずに歩行できることを便宜上、「安定に歩行することができる」と表現する。さらに、理論上、安定な歩行を実現することができる歩容データを便宜上「安定な歩容データ」と称する。また、以下では簡単のため「２脚歩行ロボット」を単に「ロボット」と称する。

安定な歩容データを生成する一つの手法は、ＺＭＰ方程式に基づいてロボットの重心軌道を生成し、重心軌道から脚各関節の目標角を求めることである。転倒するか否かは、ロボットの重心軌道が支配的な要素であるからである。なお、重心軌道は、重心位置の時系列データの別称である。以下、本明細書では、「軌道」という用語を「時系列データ」と等価の意味で用いる。

「重心ヤコビアン」と呼ばれるに変換式を用いることによって、各時刻における重心位置から脚各関節の目標角を求めることができる。このことは良く知られているので説明は省略する。脚各関節の目標角の時系列データは重心軌道、重心周りのロボットの角運動量、および、足先の軌道から算出される。即ち、「重心軌道」は、「歩容データ」の主要な要素である。特に、安定に歩行するためには重心軌道が重要である。そのため、本明細書では、「歩容データ」との用語を、重心軌道と同等のものとして扱う。ＺＭＰ方程式に基づく歩容データ生成方法は、例えば特許文献１〜３に開示されている。なお、本明細書の最後にも、ＺＭＰ方程式に基づく歩容データ生成方法に関する説明を掲載するので参照されたい。

歩容データは、安定して歩行するための条件を満たすことが求められるため、その算出には高い計算負荷が必要とされる。そのため、歩容データを効率よく求める技術が望まれている。歩容データを効率よく求める一つの手法が特許文献１〜３に開示されている。その方法は、ＺＭＰ方程式に現れる重心加速度を連続する３つの時刻の重心位置の変数によって近似した３項方程式を用いる。ＺＭＰ方程式の３項方程式近似による重心位置の求解法も本明細書の最後に掲載するので参照されたい。

特開２００９−０６６６９８号公報 特開２００９−１９５９９９号公報 特開２００９−２１４２５５号公報

本明細書は、特許文献１〜３とは別のアプローチで歩容データを効率よく求める技術を提供する。さらに、本明細書が開示する技術は、特許文献１〜３に開示される３項方程式による歩容データ生成方法に適用することで、さらなる計算効率化を図ることができる。

前述したように、歩容データの計算負荷を増大させる主要な一因は、安定した歩行のための条件を満たす歩容データを求めることである。ここで、「安定した歩行のための条件」について考察する。例えば、現在時刻における遊脚が着地できることを（理論上）保証する歩容データであっても、その時点で体幹が大きく傾いていては、歩行を継続することができない。逆に、１０分後まで歩行を継続できることを（理論上）保証した上で現在時刻から１分後までの歩容データを生成するのは無駄である。そこで、「安定した歩行のための条件」の一つの目安として、２歩進めることを（理論上）保証して歩容データを作成することが妥当であるという考え方がある。ここで、「２歩」とは次の事象を意味する。即ち、現在時刻における遊脚が着地するタイミングが「１歩目」に相当し、現在時刻における立脚が遊脚に移行し次いで着地するタイミングが「２歩目」に相当する。即ち、「安定な歩容データ」とは、現在時刻における立脚が離床し、次いで遊脚から立脚に切り換わる予定の時刻まで歩行が継続できることを（理論上）保証する歩容データに相当する。より詳しくは、前記した予定時刻においてロボット重心が適切な範囲内に存在すること、及び予定時刻における重心速度が適切な大きさ以下であることを（理論上）保証する歩容データが「安定な歩容データ」に相当する。以下では、安定な歩容データであることを保証する予定時刻におけるロボット重心位置と重心速度の範囲を予定時刻における境界条件と称することがある。

なお、「安定した歩行のための条件」の一つの目安は、「２歩」でなく、「３歩」であることが明らかに好ましい。従って別言すれば、「安定な歩容データ」とは、少なくとも現在時刻における立脚が遊脚に移行し次に遊脚から立脚に切り換わる予定時刻において重心位置が予め定められた範囲に存在することが保証できる重心軌道に相当する。以下では、「現在時刻における立脚が遊脚に移行し次に遊脚から立脚に切り換わる予定時刻」を「２歩目着地予定時刻」と称する。

一歩行周期ＴＷよりも短い軌道計算周期で、２歩目着地予定時刻までの「安定な歩容データ」を繰り返し算出すれば、常に２歩目まで歩行できることが（理論上）保証され、結果的に、２歩以上歩行を継続できることになる（理論的には無限に歩行を継続できる）。なお、２歩目着地予定時刻は、一歩行周期ＴＷを予め定めておくことによって、現在時刻における歩行周期内のポジションと一歩行周期ＴＷから決定することができる。以下では、現在時刻から２歩目着地予定時刻までの期間を、軌道算出期間と称する。

ここで、一歩行周期ＴＷは概ね１秒である。従って、軌道算出期間は概ね２秒となる。他方、関節のサーボ制御周期は、概ね５msec以下であり好ましくは２msec以下である。これは以下の理由による。サーボ制御周期をシステム全体のサンプリング周期と仮定すると、シャノンのサンプリング定理より、サーボ制御周期が５msecの場合はシステムのダイナミックレンジが理論上１００Ｈｚであり、サーボ制御周期が２msecの場合はシステムのダイナミックレンジが理論上２５０Ｈｚとなる。人間の応答速度が概ね０．１秒（即ち１００Ｈｚ）であるから、人間と同等のダイナミックレンジを有する２脚歩行ロボットを実現するためには、サーボ制御周期は５msec以下が必要ということになる。

サーボ制御周期に合わせて関節目標角を順次変更していく必要がある。即ち、サーボ制御周期に相当する第１時間間隔で関節目標角の時系列データが必要となる。そのためには、第１時間間隔で重心位置の時系列データが必要となる。前述したように軌道算出期間は概ね２秒であるから、サーボ制御周期を仮に２msecとすると１回の軌道計算で１０００点の重心位置のデータを算出しなければならず、高い計算能力が要求される。

発明者は、サーボ制御周期（５msec以下）、２歩目までの軌道算出期間（約２秒）、及び、軌道計算周期の差に着目した。ここで、軌道計算周期は、一歩行周期ＴＷ（約１秒）よりも小さいことが必要であるが、サーボ制御周期ほどには小さくなくてよい。現在時刻から少なくとも軌道計算周期に相当する期間（この期間を初期期間と称する）の間は、サーボ制御周期に相当する第１時間間隔で重心軌道（重心位置の時系列データ）を求める必要があるが、初期期間以降は、安定であることが保証できればよいので第１時間間隔で重心軌道を求める必要はない。なぜならば、軌道計算周期毎に重心軌道を再計算するのであれば、初期期間以降の重心軌道はサーボ制御には利用されないからである。

即ち、初期期間以降の重心軌道は第１時間間隔より長い第２時間間隔で求めればよい。そうすることによって、サーボ制御に影響を与えることなく、２歩目着地予定時刻までの重心軌道を従来よりも少ない点数の時系列データで表すことができる。求める時系列データの点数が少なくなれば計算負荷を低減できる。例えば、サーボ制御周期（第１時間間隔）を２msec、軌道算出期間を２秒とすると、従来は１０００点の時系列データを計算する必要があった。初期期間（軌道計算周期）を５００msec、第２時間間隔を１０msecとすると、２秒の重心算出期間は、５００／２＋１５００／１０＝４００、即ち、４００点の時系列データに短縮することができる。この場合、データ点数が半分以下となるので、計算負荷も概算で半分以下となることが期待できる。即ち、初期期間以降の重心軌道をサーボ制御周期よりも長い時間間隔で求めることによって、安定な歩容データを算出する際の効率を格段に向上することができる。なお、実際には計算オーバーヘッド等を要するために計算時間は単純にデータ点数の比とはならないことに留意されたい。

上記した手法の一実施形態は、２脚歩行ロボットの歩容データ生成方法として具現化できる。その方法は、軌道算出ステップと目標角算出ステップを含む。軌道算出ステップは、予め定められた軌道計算周期毎にＺＭＰ方程式に基づいて重心位置の時系列データを算出する。目標角算出ステップは、時系列データの各時刻における重心位置に基づいて（重心ヤコビアンを使って）脚関節の目標角を算出する。ここで、軌道算出ステップは、現在時刻から少なくとも軌道計算周期に相当する初期期間の間は関節のサーボ制御周期に相当する第１時間間隔で重心位置の時系列データを算出する。さらに軌道算出ステップは、初期期間以降は第１時間間隔よりも長い第２時間間隔で重心位置の時系列データを算出する。なお、軌道計算周期は、一歩行周期ＴＷよりも短い。また、目標角算出ステップは、初期期間における各時刻の重心位置から脚関節の目標角を算出する。

通常、重心軌道の上下方向成分は予め与えられることが多い。従って軌道算出ステップにおいて算出する重心軌道は、歩行面に平行な面における重心軌道でよい。ここで「歩行面に平行な面」は、ロボットの進行方向を表すｘ軸と体側方向を表すｙ軸が張る平面を意味する。以下、「歩行面に平行な面」を、本明細書では床平行面と称する。なお、「サジタル面」は、ロボットの進行方向を表すｘ軸を含み、床平行面と直交する面を意味する。安定か否かは、床平行面内における重心軌道が重要である。安定か否かは、重心軌道が歩行面上に描かれる歩行経路から発散しないことが重要だからである。通常、安定した歩行では、床平行面内の重心軌道は、歩行経路を中心に体側方向に振れながら進んでいく。なお、重心軌道の鉛直方向成分は体幹の上下動に相当し、通常は既定の軌道が予め準備される。

前述したように、安定した歩行を実現するためには、軌道算出ステップは、軌道計算周期毎に、少なくとも現在時刻における立脚が遊脚に移り次いで遊脚から立脚に切り換わる予定時刻（２歩目着地予定時刻）までの期間（軌道算出期間）について、重心位置の時系列データを算出する。前述したように、２歩目着地予定時刻は、予め与えられる一歩行周期ＴＷによって定められる。

上記した方法は、ＺＭＰ方程式を、床平行面内の重心加速度を連続する３つの時刻の重心位置の変数によって近似した３項方程式で表し、その３項方程式を解いて重心軌道を算出する手法と組み合わせるとさらに計算効率が向上する。３項方程式近似によって、ＺＭＰ方程式から床平行面内の重心加速度項を消去することができ、それによってＺＭＰ方程式を重心位置とＺＭＰとの関係式として表すことができる。詳細は実施例の後に付した「歩容データ算出方法についての説明」を参照されたい。ここでは、要点だけを説明する。３項方程式近似によって、離散時間系で順次に並ぶｎ時刻分のＺＭＰ方程式（時刻毎のＺＭＰ方程式を、微小時刻Δｔごとにｎ時刻分並べた方程式群）は、［Ａ］［Ｘ］＝［Ｑ］のマトリクス形式で表現できる。なお、重心軌道のｘ軸方向（ロボット進行方向）成分とｙ軸方向（ロボット体側方向）成分の夫々についてマトリクス表現ができる。両者は基本的に同じであるので、以下ではｘ軸方向成分に関して説明する。ここで、［Ａ］はｎ次のマトリクスであり、［Ｘ］は重心位置のｘ座標のｎ次列ベクトルであり、［Ｑ］はＺＭＰのｘ座標のｎ次列ベクトルである。従って、重心位置のｘ座標ベクトル［Ｘ］は、［Ａ］^−１［Ｑ］で求められる。次数ｎは、重心位置の時系列データの時間間隔が短いほど大きくなる。従って、上記方法により初期期間以降の時間間隔（第２時間間隔）を長くすることによって、次数ｎを顕著に減らすことができ、行列演算に要する計算負荷を格段に低減することができる。上記したマトリクス方程式は、初期時刻と終端時刻の境界条件を与えることによって、重心軌道［Ｘ］について解くことができる（詳しくは「歩容データ算出方法についての説明」を参照されたい）。

３項方程式近似を採用する場合、第２時間間隔は、一歩行周期ＴＷを時分割するための予め定められた次数ｎで一歩行周期ＴＷを除した値で定めるとよい。予め定められた次数ｎは、上記した行列の次数に相当する。即ち、そのように定めると、行列の次数ｎを一定にすることができる。重心軌道を求めるための行列演算に要する計算時間を一定にすることができる。

重心軌道を求める手法は、上記したマトリクス法に限られない。ＺＭＰ方程式は、重心の運動方程式に相当する。即ち、ＺＭＰ方程式では、各時刻における重心の加速度が重心の位置とＺＭＰで表される。従ってＺＭＰ方程式で表される重心加速度を数値積分していくことによっても重心軌道を求めることができる。数値積分の際の時間ステップが時系列データの時間間隔に相当する。従って、数値積分によってＺＭＰ方程式から重心軌道を求める場合であっても、上記した手法は計算負荷を低減することができる。数値積分法は、ルンゲクッタ法や予測子修正子法など、良く知られた様々な手法を採用してよい。なお、上記した３項方程式近似は、オイラー法による近似に基本的に対応する。

上記した手法の他の一実施形態は、２脚歩行ロボットに具現化することも好適である。その２脚歩行ロボットは、軌道算出部、目標角算出部、サーボ制御部を備える。軌道算出部は、予め定められた軌道計算周期毎にＺＭＰ方程式に基づいて床平行面における重心位置の時系列データを算出する。目標角算出部は、時系列データの各時刻における重心位置から脚関節の目標角を算出する。サーボ制御部は、サーボ制御周期毎に脚の計測関節角を目標角に追従するように制御する。軌道算出部は、現在時刻から少なくとも軌道計算周期に相当する初期期間の間はサーボ制御周期に相当する第１時間間隔で重心位置の時系列データを算出する。さらに軌道算出部は、初期期間以降は第１時間間隔よりも長い第２時間間隔で重心位置の時系列データを算出する。目標角算出部は、初期期間における各時刻の重心位置から脚関節の目標角を算出する。サーボ制御部は、サーボ制御周期毎に脚の実際の関節角（計測関節角）を目標角に追従するように制御する。なお、軌道算出部は、軌道計算周期毎に、少なくとも２歩目着地予定時刻までの重心位置の時系列データを算出する。そのような２脚歩行ロボットは、上記した歩容データ生成方法をリアルタイムに実行しながら安定に歩行することができる。

実施例の２脚歩行ロボットを示す。 図１の一部拡大図を示す。 コントローラのブロック図を示す。 コントローラが実行する処理のフローチャートを示す。 ＺＭＰ方程式を説明する模式図である。

図１に、歩行中の２脚歩行ロボット１００の側面図を示す。以下、２脚歩行ロボット１００を単にロボット１００と称する。夫々の脚（右脚１２Ｒと左脚１２Ｌ）は、股関節がロール軸、ピッチ軸、ヨー軸周り回転の３自由度を有し、膝関節がピッチ軸周り回転の１自由度を有し、足首関節がロール軸周り回転とピッチ軸周り回転の２自由度を有する。ロボット１００はさらに、足平にピッチ軸回りの関節を備えている。この関節は足指付け根の関節に相当する。足指付け根の関節を有すると、人間の歩行動作と同じ様に、足指の底面（及び母趾球）で接地しつつ踵を上げることができ、円滑な歩行動作を達成することができる。各関節はモータ１４とセンサ１６（エンコーダ）を搭載している。センサ１６は関節角を検出する。体幹１０にはコントローラ２０が内蔵されている。コントローラ２０が、センサ１６のセンサデータに基づいてモータ１４を制御する。コントローラ２０が各モータを適切に制御することによって、ロボット１００は歩行する。本明細書が開示する技術は、脚の幾何学的な構造、各関節のサーボ制御ルールに依存しないことに留意されたい。従って、脚の構造、及び、関節サーボ制御ルールの詳しい説明は省略する。上記した７自由度の脚は一例であり、本明細書が開示する技術は、当然、他の自由度を有する脚、或いは他の幾何学的構造を有する脚を備えた２脚歩行ロボットに適用することができる。また、２脚歩行ロボットは腕や頭を備えていてもよい。

座標系について説明する。図１において、ｘ軸はロボット進行方向に相当し、ｙ軸はロボット体側方向に相当する。ｚ軸はｘ軸及びｙ軸に直交する。本明細書では、ｘ軸とｙ軸が張る平面を「床平行面」と称する。ｘ軸とｚ軸が張る平面はサジタル面と呼ばれている。図１の上側の側面図がサジタル面に相当する。図１の下側は、床平行面を示している。床平行面は歩行面に相当する。図１下側の床平行面は、足の着地位置を示している。なお、図１下側の床平行面の図は、後述の「歩容データ算出についての補足事項」の項にて説明する際に用いる。

ロボット１００は、安定した歩行を実現するための歩容データを高速に求めることができる点に特徴がある。なお、「安定した歩行」とは、前述したように、少なくとも２歩進むことが理論上保証される歩容データを意味する。まず、歩容データについて概説する。歩容データは、実質的には重心軌道（重心位置の時系列データ）で決まる。歩容データは、ロボットが安定に歩行できなれば意味が無く、安定に歩行できるか否かは重心軌道に依存するからである。また、最終的には歩容データは各関節の目標角の時系列データで表現されるが、重心ヤコビアンによって重心軌道から目標角の時系列データが算出できるので、「歩容データ」は実質的には「重心軌道」に相当するとみなしてよい。「重心ヤコビアン」については脚式ロボットの技術分野では良く知られているので詳しい説明は省略する。

前述したように安定した歩容データは、実質的には、床平行面における重心軌道で定まる。なぜならば、床平行面における重心軌道が歩行経路から大きく離れないことが即ち「安定した歩行」に相当するからである。本実施例のロボット１００は、現在時刻Ｔ０から２歩目着地予定時刻Ｔ２までの重心軌道が歩行経路から大きくずれないことを理論上保証する歩容データを算出する。そのような安定な歩容データの算出方法は、ＺＭＰ方程式に基づいて算出できることが知られている。２歩目着地予定時刻Ｔ２までの重心軌道の具体的な算出方法は、後の「歩容データ算出方法についての説明」の項にて説明するのでそちらを参照されたい。なお、本明細書が開示する新規な歩容データ算出技術は、物理現象的な観点からは従来の算出技術と同じ技術を用いるが、計算の仕方に工夫があり、従来よりも効率よく歩容データを算出することを可能にする。

図１の各符号について説明する。図１の（Ａ）は、現在時刻Ｔ０におけるロボット１００の姿勢を示している。現在時刻Ｔ０では、ロボット１００の左脚１２Ｌが立脚となっており、右脚１２Ｒが遊脚となっている。（Ｂ）は１歩目着地予定時刻Ｔ１における姿勢（予定値）を示し、（Ｃ）は２歩目着地予定時刻Ｔ２における姿勢（予定値）を示している。「１歩目着地予定時刻Ｔ１」とは、現在時刻Ｔ０における遊脚（右脚１２Ｒ）が着地する予定の時刻に相当する。２歩目着地予定時刻Ｔ２は、現在時刻Ｔ０における立脚（左脚１２Ｌ）が離床し、次いで遊脚から立脚に切り換わる予定（着地予定）の時刻に相当する。なお、一歩行周期ＴＷは予め与えられる。現在の歩行周期の開始時刻は既に特定されているので、与えられた一歩行周期ＴＷから時刻Ｔ１とＴ２が定まる。

ｘ_ｇ（ｉ）、ｙ_ｇ（ｉ）、及び、ｚ_ｇ（ｉ）は、それぞれ、時刻ｉにおける重心Ｇのｘ座標、ｙ座標、及び、ｚ座標を表している。ここで、「座標」は、「位置」に相当する。ｘ_ｇ（Ｔ０）は、現在時刻Ｔ０における重心Ｇのｘ座標（ｘ軸方向の重心位置）を表している。ｘ_ｇ（Ｔ１）、ｘ_ｇ（Ｔ２）、は夫々、１歩目着地予定時刻Ｔ１、２歩目着地予定時刻Ｔ２における重心Ｇのｘ座標（予定値）を表している。ｖ_ｇ（Ｔ０）は、現在時刻Ｔ０における重心Ｇの速度のｘ軸方向成分を表している。ｖ_ｇ（Ｔ０）は、脚の各関節が備えるセンサ１６のセンサデータから求められる。ｖ_ｇ（Ｔ１）、ｖ_ｇ（Ｔ２）、は夫々、１歩目着地予定時刻Ｔ１、２歩目着地予定時刻Ｔ２における重心Ｇの速度のｘ軸方向成分を表している。ｖ_ｇ（Ｔ１）、ｖ_ｇ（Ｔ２）、は予め与えられる値である。例えば、ロボット１００が一定速度Ｖ０で直線歩行する場合、「ｖ_ｇ（Ｔ１）＝ｖ_ｇ（Ｔ２）＝Ｖ０」となる。

ｘ_ｐ（ｉ）は、現在時刻における遊脚（右脚１２Ｒ）の足の軌道を表している。本実施例では、足の軌道は、足のつま先に固定された点の軌道で現される。ｘ_ｐ（Ｔ０）は、現在時刻Ｔ０における遊脚足のｘ座標（左足位置）を表している。ｘ_ｐ（Ｔ１）は、着地予定時刻Ｔ１における遊脚足のｘ座標、即ち、遊脚の着地予定位置を示している。

図１の下側の床平行面について簡単に説明する。詳しくは、後の「歩容データ算出方法についての説明」の項で述べる。Ｆ０は、現在時刻における立脚（左脚）足の位置を示している。Ｆ１、Ｆ２は、右脚足の着地予定位置、左脚足の次の着地予定位置を表している。符号Ｌ_Ｐａｔｈが示す直線は歩行経路を表している。符号Ｌ_ＺＭＰが示す破線はＺＭＰ軌道を表しており、符号Ｌ_ｇが示す実線は床平行面における重心軌道を表している。本明細書においてＺＭＰとは、接地している足の裏面に分布している床圧力の中心（圧力中心）を意味する。この定義は、ＺＭＰの概念を始めて提唱したユーゴスラビアのブコブラトビッチの定義に相当する。なお、図１の一点鎖線は、時刻Ｔ１において左足が位置Ｆ０で接地しており右足が位置Ｆ１で接地している両脚支持期間における支持多角形を示している。「支持多角形」もブコブラトビッチの定義に準ずる。支持多角形は、片脚立脚期間では接地している足の足裏面に等しい。両脚接地期間では上記したように接地している両足位置によって支持多角形が画定される。

図１下側の床平行面の図は、安定した歩容データ、即ち安定した重心軌道Ｌ_ｇを示している。重心軌道Ｌ_ｇは歩行経路Ｌ_Ｐａｔｈに対して左右に振れながらも歩行経路Ｌ_Ｐａｔｈに沿っているからである。安定な重心軌道の算出方法については「歩容データ算出方法についての説明」を参照されたい。

ロボット１００は、現在時刻Ｔ０において、２歩目着地予定時刻Ｔ２までの重心軌道を算出する。なお、以下では、重心位置のｘ成分ｘ_ｇ（ｉ）について説明し、ｙ成分ｙ_ｇ（ｉ）についてはｘ成分と同様であるので説明は省略する。ロボット１００は、現在時刻Ｔ０において、重心位置のｘ成分ｘ_ｇ（ｉ）、（ただしｉ＝０からＴ２）を算出する。

図２を用いてロボット１００の歩容データ生成処理をさらに説明する。図２は、図１の一部を拡大した図である。図２は、現在時刻Ｔ０から１歩目着地予定時刻Ｔ１までの期間を拡大した図である。重心軌道Ｌ_ｇは、重心位置の時系列データであり、現在時刻Ｔ０から２歩目着地予定時刻Ｔ２までの重心位置の点列として求められる。ロボット１００は、予め定められた軌道計算周期（例えば２０msec）毎に、２歩目着地予定時刻Ｔ２までの重心軌道を算出する。このとき、ロボット１００は、現在時刻Ｔ０から、軌道計算周期に相当する初期期間ＴＳ１の間は、関節サーボ制御周期に相当する第１時間間隔ΔＴ１で重心位置の時系列データを算出する。初期期間ＴＳ１以降、２歩目着地予定時刻Ｔ２までの期間ＴＳ２は、第１時間間隔ΔＴ１よりも長い第２時間間隔ΔＴ２で重心軌道を算出する。図２では、初期期間ＴＳ１の間の重心軌道Ｌ_ｇ−１が第１時間間隔ΔＴ１で算出され、初期期間ＴＳ１以降の重心軌道Ｌ_ｇ−２が第２時間間隔ΔＴ２で算出される。

ロボット１００は、初期期間ＴＳ１の各時刻における重心位置（重心位置はΔＴ１間隔で求められている）から、各関節の目標角Ｌ_ｐ−１を算出する。各時刻の関節目標角は、対応する時刻の重心位置を重心ヤコビアンに代入することによって得られる。こうして、ロボット１００は、現在時刻から初期期間ＴＳ１までの間、サーボ制御周期毎の関節目標角を得る。なお、初期期間ＴＳ１以降に相当する足軌道Ｌ_ｐ−２は、現在時刻では求める必要がない。Ｌ_ｐ−２は、現在時刻Ｔ０から軌道計算周期に相当する時間後に改めて、第１時間間隔ΔＴ１ごとの時系列データとして算出される。

上記説明したアルゴリズムを実行するコントローラ２０のブロック図を図３に示し、制御フローチャートを図４に示す。コントローラ２０は、軌道算出部２１、目標角算出部２２、メモリ２３、タスク管理部２４、サーボ制御部２５、サーボアンプ２６、及びリアルタイムクロック（ＲＴＣ）２７を備える。軌道算出部２１と目標角算出部２２によって、軌道算出タスクが実行され、サーボ制御部２５によってサーボ制御タスクが実行される（図４参照）。タスク管理部２４は、リアルタイムクロック２７の信号に基づいて、軌道計算周期毎に軌道算出タスクを起動し、サーボ制御周期毎にサーボ制御タスクを起動する。なお、前述したように、軌道計算周期は例えば２０msecでありサーボ制御周期は２msecである。軌道算出タスクとサーボ制御タスクは並列に実行される。

歩行経路は上位コントローラ９０からロボットのコントローラ２０に与えられる。上位コントローラ９０は、ロボット１００と接続されたコンピュータでよい。

軌道算出タスクを説明する。軌道算出部２１は、センサ１６のデータを読み込むとともに、２歩目着地予定時刻Ｔ２までの重心軌道を算出する（Ｓ１２）。次に目標角算出部２２が目標角を算出する（Ｓ１４）。ここで、重心軌道は２歩目着地予定時刻Ｔ２まで算出されるが、目標角は現在時刻から初期期間ＴＳ１までの間の各時刻の重心軌道に対応する分だけが算出される。算出された目標角の時系列データはメモリ２３に格納される。ステップＳ１２の処理が軌道算出ステップに相当し、ステップＳ１４の処理が目標角算出ステップに相当する。後に「歩容データ算出方法についての説明」で述べる、軌道算出ステップでは、現在時刻Ｔ０から２歩目着地予定時刻Ｔ２まで（即ち軌道算出期間の間）の重心軌道が、２歩目着地予定時刻Ｔ２（終端時刻）における既定の重心速度と重心位置範囲を境界条件として算出される。

サーボ制御タスクについて説明する、サーボ制御タスクは、軌道算出タスクと並列に、サーボ制御周期毎に起動される。サーボ制御部２５は、まずセンサデータを取得する（Ｓ２２）。次に、メモリ２３から、記憶されている目標角の時系列データの中から、現在のサーボ制御時刻に対応する目標角を読み出す（Ｓ２４）。コントローラ２０は、センサデータと読み出した目標角との偏差に基づいて、各関節のモータ１４への指令値を算出する（Ｓ２６）。ステップＳ２６では、例えば、ＰＩＤ制御ルールに基づいて指令値が算出される。算出された指令値はサーボアンプ２６へ出力される（Ｓ２８）。サーボアンプ２６は、入力された指令値に基づいて各モータ１４を制御する。

ロボット１００の各関節は、サーボ制御周期（即ち第１時間間隔ΔＴ１）毎の目標角に追従する。その結果、ロボット１００はその重心が算出された重心軌道に追従するように動作する。すなわちロボット１００は安定に歩行する。軌道計算周期毎に軌道算出タスクが軌道され、その都度、現在時刻から初期期間分の目標角データが算出される。毎回の目標角データは常に２歩目まで安定に歩行することが（理論上）保証されている。そのため、ロボット１００は、（理論上）無限に安定に歩行し続けることができる。

後の「歩容データ算出方法についての説明」の項にて述べるように、ロボット１００は、各時刻におけるＺＭＰ方程式を、床平行面内の重心加速度を連続する３つの時刻の重心位置の変数によって近似した３項方程式で表し、現在時刻と終端時刻の境界条件を与えることによって各時刻の３項方程式を解いて重心位置の時系列データを算出する。現在時刻の境界条件は、現在時刻の重心速度ｖ_ｇ（Ｔ０）と位置ｘ_ｇ（Ｔ０）である。終端時刻の境界条件は、２歩目着地予定時刻Ｔ２における重心速度ｖ_ｇ（Ｔ２）と予め決められた２歩目安定保証領域である。２歩目安定保証領域は、後の「歩容データ算出方法についての説明」の項にて説明する。さらにロボット１００は、第２時間間隔ΔＴ２を次のルールによって決定する。コントローラ２０は、一歩行周期ＴＷを、予め定められた次数ｎで除した値でΔＴ２を定める。次数ｎは、後述する行列［Ａ］の次数である。

実施例のロボット１００の利点を説明する。ロボット１００は、リアルタイムに２歩目着地予定時刻Ｔ２までの重心軌道を算出する。別言すれば、ロボット１００は、２歩目まで安定して歩行できることが理論上保証される重心軌道を算出する。ロボット１００は、２歩目までの重心軌道のうち、関節目標角を算出するための初期期間ＴＳ１の重心軌道を、サーボ制御周期と同じ第１時間間隔ΔＴ１で求める。初期期間ＴＳ１以降、２歩目着地予定時間Ｔ２までの後期間ＴＳ２は、第１時間間隔ΔＴ１より長い第２時間間隔ΔＴ２で重心軌道を求める。第２時間間隔ΔＴ２を長くすることによって、２歩目着地予定時刻Ｔ２まで重心軌道を求めるための計算負荷を軽減できる。

後期間ＴＳ２の重心軌道を求める際、第２時間間隔ΔＴ２は、３項方程式で近似したＺＭＰ方程式の係数行列［Ａ］の次数ｎが一定となるように決定される。従って、２歩目着地予定時刻Ｔ２までの重心軌道算出に要する演算時間を一定にすることができる。

（歩容データ算出方法についての説明）ＺＭＰ方程式に基づく重心軌道の算出方法について説明する。ここでは説明のため、ロボット全体を一つの剛体で表す。また、ここでは、重心軌道のｘ軸成分ｘ_ｇ（ｉ）について説明する。以下の議論はｙ軸成分についても成立する。図５に、時刻（ｉ）における、ロボットを模式化した図（サジタル面における側面図）を示す。図５において、ｑ_ｘ（ｉ）は、ＺＭＰのｘ座標を表す。図５のＦｒは、足裏に加わる圧力分布（反力分布）に等価な集中反力を示す。ＺＭＰは、足裏に分布する圧力（反力）に等価な集中反力が床面と交差する点で定義されるので、Ｆｒの起点がＺＭＰの位置を示す。ｇは重力加速度を表す。ｒ_ｙ（ｉ）は重心Ｇに作用するｙ軸周りのモーメントを示す。モーメントｒ_ｙ（ｉ）は、ロボットｙ軸周りの角運動量の時間微分に相当する。時刻（ｉ）における重心Ｇを中心とするｙ軸回りの釣り合い方程式は次の（数１）で与えられる。

ここで、ｍはロボット全体の質量を表す。ｚ_ｇ（ｉ）は重心Ｇのｚ座標を表す。上付き文字の（２）は時間２回微分を表す。即ち、ｘ_ｇ ^（２）（ｉ）は、重心Ｇの加速度のｘ軸成分を表す。

重心Ｇの加速度のｚ軸成分、即ち、ｚ_ｇ ^（２）（ｉ）は予め与えられる。ｚ_ｇ ^（２）（ｉ）は、歩行時の腰の上下動にほぼ相当する。重心Ｇの加速度のｘ軸成分、即ち、ｘ_ｇ ^（２）（ｉ）、及び、重心Ｇの速度のｘ軸成分ｘ_ｇ ^（１）（ｉ）は、微小時間間隔Δｔを用いたオイラー近似によると次の（数２）で表される。

（数２）より、重心Ｇの加速度のｘ軸成分ｘ_ｇ ^（２）（ｉ）は重心Ｇの連続する３つの時刻の位置、ｘ_ｇ（ｉ）、ｘ_ｇ（ｉ＋１）、ｘ_ｇ（ｉ＋２）によって次の（数３）で近似できる。

（数３）を（数１）に代入し、ｒ_ｙ（ｉ）＝０とおくと次の（数４）が得られる。

（数４）は、（数１）のＺＭＰ方程式において、床平行面内の重心加速度ｘ_ｇ ^（２）（ｉ）を連続する３つの時刻の重心位置の変数、即ち、ｘ_ｇ（ｉ＋１）、ｘ_ｇ（ｉ）及び、ｘ_ｇ（ｉ−１）によって近似した３項方程式である。ｉ＝１からｎまでについて、（数４）をマトリクス表現すると次の（数５）が得られる。

なお、（数５）のｑ_ｘ（１）とｑ_ｘ（ｎ）は、他のｑ_ｘ（ｉ）すなわち、ＺＭＰの位置とは別の意味を有している。（数５）の第１行は、初期速度が現在時刻の重心速度に一致することを保証し、第ｎ行は、２歩目着地予定時刻Ｔ２における重心速度が予定された速度に一致することを保証する境界条件に相当する。（数２）の定義から、ｑ_ｘ（１）＝ｖ（Ｔ０）Δｔ、ｑ_ｘ（ｎ）＝ｖ（Ｔ２）Δｔである。ｖ（Ｔ０）とｖ（Ｔ２）は、第１図に示した通り、現在時刻Ｔ０と２歩目着地予定時刻Ｔ２における重心Ｇの速度のｘ軸成分である。（数５）より、重心軌道のｘ軸成分［Ｘｇ］（即ち、ｘ_ｇ（ｉ），ｉ＝１〜ｎ）は、次の（数６）で求めることができる。

（数６）は、ＺＭＰ軌道［Ｑｘ］を与えると重心軌道［Ｘｇ］が求まることを表している。再び図１を参照して、（数６）を用いた重心軌道演算を説明する。図１下側の床平行面を参照されたい。今、歩行速度Ｖ０＝ｖ（Ｔ０）＝ｖ（Ｔ１）＝ｖ（Ｔ２）は与えられている。また、一歩行周期ＴＷのＺＭＰ軌道も与えられている。一歩行周期のＺＭＰ軌道が与えられているということは、１歩目と２歩目の足着地位置（図１の足位置Ｆ１とＦ２）が与えられていることを意味する。即ち、１歩目と２歩目の足位置Ｆ１とＦ２によって支持多角形が定まり、ＺＭＰ軌道はその支持多角形内を移動するように予め定められている。なお、与えられたＺＭＰ軌道は、仮の値であり、後述する演算によって、安定性が保証されるＺＭＰ軌道（即ち１歩目と２歩目の足着地位置）が改めて求まる。（数５）、（数６）における［Ｑｘ］がＺＭＰ軌道に相当し、［Ｘｇ］が重心軌道（但しｘ軸方向成分）に相当する。

歩行経路Ｌ_Ｐａｔｈが与えられると、ロボット１００のコントローラ２０は、与えられた歩行経路に沿って、１歩目と２歩目の足着地位置（図１の足位置Ｆ１とＦ２）及び、ＺＭＰ軌道、即ち（数６）の［Ｑｘ］を仮決定する。また、前述したとおり。重心軌道のｚ軸成分（ｚ_ｇ（ｉ）、ｚ_ｇ ^（２）（ｉ））も予め与えられており、行列［Ａ］の各要素（ａ_ｉ、ｂ_ｉ、及びｃ_ｉ）が（数４）から算出される。［Ａ］と［Ｑｘ］が与えられるので、［Ａ］の逆行列を演算し、（数６）から重心軌道（ここでは、重心軌道のｘ軸成分ｘ_ｇ（ｉ））が求められる。このときのｘ_ｇ（ｉ）はまだ仮の値である。次に、ｉ＝ｎ−１における重心位置ｘ_ｇ（ｎ−１）が所定の範囲内であるか否かを確認する。なお、前述したとおり、（数５）の第ｎ行はＺＭＰ位置とは異なる別の意味を有しているので、ｎ−１における重心位置ｘ_ｇ（ｎ−１）を使う。また、所定の範囲とは、１歩目着地予定位置Ｆ１と２歩目着地予定位置Ｆ２の中間やや前方の領域（２歩目安定保証領域と称する）に設定されている。そのような２歩目安定保証領域は、２歩目が着地したときに重心位置が両足のラテラル方向略中央に位置することを保証し、安定歩行を（理論上）保証する。

重心位置ｘ_ｇ（ｎ−１）が２歩目安定保証領域外である場合は、仮決定したＺＭＰ軌道が不適切であることを示している。この場合は、仮決定したＺＭＰ軌道を何らかのルールに基づいて修正して上記演算を繰り返す。そのような繰り返し演算（いわば収束演算）を重心位置ｘ_ｇ（ｎ−１）が２歩目安定保証領域内となるまで行う。上記アルゴリズムは、重心軌道のｙ軸成分についても当てはまる。こうして、安定な重心軌道（即ち歩容データ）が求まる。

ｉ＝０とｎにおける重心速度ｖ_ｇ（Ｔ０）、ｖ_ｇ（Ｔ２）と、ｉ＝ｎ−１における重心位置ｘ_ｇ（ｎ−１）が２歩目安定保証領域内に存在することが、（数６）を解くときの境界条件に相当する。

上記の歩容データ（重心軌道）求解手法では、重心Ｇに作用するｙ軸周りのモーメントｒ_ｙ（ｉ）をゼロと仮定した。モーメントｒ_ｙ（ｉ）を考慮した歩容データ求解手法は、例えば、本願の発明者による特許出願（特願２００９−２９１２５７）で提唱されている。

上記の求解手法では、現在時刻Ｔ０から２歩目着地予定時刻Ｔ２までの重心軌道を一つの行列［Ａ］でまとめて求めた。その場合、重心軌道を求めるためのＺＭＰ軌道［Ｑｘ］の点列（時系列データ）は、現在時刻から少なくとも初期期間の間は第１時間間隔ΔＴ１で構成され、初期期間以降は第２時間間隔ΔＴ２で構成される。そのような手法に変えて、現在時刻Ｔ０から１歩目着地予定時刻Ｔ１までを１歩目着地予定時刻Ｔ１における境界条件を与えて解き、その後、１歩目着地予定時刻Ｔ１から２歩目着地予定時刻Ｔ２までの重心軌道を別途求めることも好適である。その場合、時刻Ｔ０から時刻Ｔ１までのＺＭＰ軌道を第１時間間隔ΔＴ１で準備し、時刻Ｔ１から時刻Ｔ２までのＺＭＰ軌道を第２時間間隔ΔＴ２で準備するとよい。

上記の求解手法は、３項方程式で近似したＺＭＰ方程式を用いた。その手法は、加速度をオイラー法で近似することを含む。オイラー法は数値積分としては簡単であるが精度が低い。上記求解手法に変えて、例えばルンゲクッタ法等の数値積分法によって（数１）から、重心軌道を求めてもよい。具体的には、（数１）を変形し、重心加速度のｘ軸方向成分ｘ_ｇ ^（２）（ｉ）に関する式に変形する（数７）。（数７）を逐次積分することによって重心軌道、即ちｘ_ｇ（ｉ）が求まる。

前述した境界条件を満たすまで、ＺＭＰ軌道を修正しながら数値積分による重心軌道の求解を繰り返す。境界条件を満たすときの重心軌道が、２歩目着地予定時刻Ｔ２まで（理論上）安定な歩容データに相当する。

本明細書が開示する技術について留意点を述べる。本明細書における「歩行」は、「走行」も含む。「歩行」は、いずれかの足が常に接地している移動形態を意味するが、「走行」は両足ともに浮いている期間を有する移動形態を意味する。「走行」の場合であっても、一方の脚が接地している期間においては、本明細書が開示する歩容データ生成手法が適用できる。

上記の実施例では、１回の重心軌道演算において、２歩目着地予定時刻Ｔ２までの軌道を算出した。これに代えて、１回の重心軌道演算において、３歩目着地予定時刻までの重心軌道を算出することも好適である。１回の重心軌道演算において何歩目までの重心軌道を算出するかは、安定歩行の確実性と演算負荷のトレードオフである。発明者らの知見によると、１回の軌道演算において、２歩目まで、或いは３歩目まで安定して歩行することが保証される重心軌道を算出すれば実用上十分である。

以上、本発明の具体例を詳細に説明したが、これらは例示に過ぎず、特許請求の範囲を限定するものではない。特許請求の範囲に記載の技術には、以上に例示した具体例を様々に変形、変更したものが含まれる。本明細書または図面に説明した技術要素は、単独であるいは各種の組合せによって技術的有用性を発揮するものであり、出願時請求項記載の組合せに限定されるものではない。また、本明細書または図面に例示した技術は複数目的を同時に達成し得るものであり、そのうちの一つの目的を達成すること自体で技術的有用性を持つものである。

１０：体幹
１２Ｌ：左脚
１２Ｒ：右脚
１４：モータ
１６：センサ
２０：コントローラ
２１：軌道算出部
２２：目標角算出部
２３：メモリ
２４：タスク管理部
２５：サーボ制御部
２６：サーボアンプ
２７：リアルタイムクロック
１００：脚歩行ロボット

Claims (6)

1. ２脚歩行ロボットの歩容データ生成方法であり、
   予め定められた軌道計算周期毎にＺＭＰ方程式に基づいて重心位置の時系列データを算出する軌道算出ステップと、
   前記時系列データの各時刻における重心位置から脚関節の目標角を算出する目標角算出ステップと、
   を備えており、
   前記軌道算出ステップは、現在時刻から少なくとも前記軌道計算周期に相当する初期期間の間は関節のサーボ制御周期に相当する第１時間間隔で重心位置の時系列データを算出するとともに、前記初期期間以降は第１時間間隔よりも長い第２時間間隔で重心位置の時系列データを算出し、
   前記目標角算出ステップは、前記初期期間における各時刻の重心位置から脚関節の目標角を算出することを特徴とする歩容データ生成方法。
2. 前記軌道算出ステップは、前記軌道計算周期毎に、少なくとも現在時刻における立脚が次に遊脚から立脚に切り換わる予定時刻までの重心位置の時系列データを算出することを特徴とする請求項１に記載の歩容データ生成方法。
3. 軌道算出ステップは、各時刻におけるＺＭＰ方程式を、床平行面内の重心加速度を連続する３つの時刻の重心位置の変数によって近似した３項方程式で表し、初期時刻と終端時刻の境界条件を与えることによって各時刻の３項方程式を解いて重心位置の時系列データを算出することを特徴とする請求項１又は２に記載の歩容データ生成方法。
4. 前記第２時間間隔は、予め与えられた一歩行周期を時分割するための予め定められた次数ｎで前記一歩行周期を除した値であることを特徴とする請求項３に記載の歩容データ生成方法。
5. 予め定められた軌道計算周期毎にＺＭＰ方程式に基づいて重心位置の時系列データを算出する軌道算出部と、
   前記時系列データの各時刻における重心位置から脚関節の目標角を算出する目標角算出部と、
   サーボ制御周期毎に脚の計測関節角を目標角に追従するように制御するサーボ制御部と、を備えており、
   前記軌道算出部は、現在時刻から少なくとも前記軌道計算周期に相当する初期期間の間は前記サーボ制御周期に相当する第１時間間隔で重心位置の時系列データを算出するとともに、前記初期期間以降は第１時間間隔よりも長い第２時間間隔で重心位置の時系列データを算出し、
   前記目標角算出部は、前記初期期間における各時刻の重心位置から脚関節の目標角を算出し、
   前記サーボ制御部は、サーボ制御周期毎に、前記初期期間における各時刻の脚関節の目標角に追従するように前記計測関節角を制御することを特徴とする２脚歩行ロボット。
6. 前記軌道算出部は、前記軌道計算周期毎に、少なくとも現在時刻における立脚が次に遊脚から立脚に切り換わる予定時刻までの重心位置の時系列データを算出することを特徴とする請求項５に記載の２脚歩行ロボット。

Images