(57)【要約】  二足歩行ロボットの足の着地タイミングを決定する方法を提供する。この方法は、まず、第１足のみで立つ第１単脚接地期間と、第１単脚接地期間の次に第２足のみで立つ第２単脚接地期間を含む重心軌道を表すＺＭＰ方程式を、予め与えられた着地タイミングを使って解く。そうして第２単脚接地期間におけるＺＭＰ位置を表す第２足ＺＭＰ位置が算出される。算出された第２足ＺＭＰ位置が、第２足の着地可能領域に対応して定められる第２足ＺＭＰ許容領域から外れている場合、第２足ＺＭＰ位置が第２足ＺＭＰ許容領域内となるように着地タイミングを修正する。
2012011182.tif 000002
【特許請求の範囲】
【請求項１】
単脚接地期間と両脚接地期間を繰り返しながら歩行する二足歩行ロボットの足の着地位置と着地タイミングを決定する方法であり、
第１足のみで立つ第１単脚接地期間と第１単脚接地期間の次に第２足のみで立つ第２単脚接地期間を含む重心軌道の始端と終端の境界条件を与えるとともに、第２足の着地タイミングを与えてＺＭＰ方程式を解き、第２単脚接地期間におけるＺＭＰ位置を表す第２足ＺＭＰ位置を算出するＺＭＰ算出ステップと、
第２足と地面の接触領域内に第２足ＺＭＰ位置が含まれるように第２足の着地位置を決定する着地位置決定ステップと、
を備えており、ＺＭＰ算出ステップは、
算出された第２足ＺＭＰ位置が第２足の着地可能領域に対応して定められる第２足ＺＭＰ許容領域から外れている場合、第２足ＺＭＰ位置が第２足ＺＭＰ許容領域内となるように着地タイミングを修正することを特徴とする方法。
【請求項２】
前記ＺＭＰ方程式は、第１単脚接地期間におけるＺＭＰ位置を表す第１足ＺＭＰ位置と、前記第２足ＺＭＰ位置を変数として有しており、
前記ＺＭＰ算出ステップは、前記境界条件と予め定められたイニシャル拘束条件の下で前記ＺＭＰ方程式を満足する第１足ＺＭＰ位置と第２足ＺＭＰ位置を算出する第１サブステップと、
算出された第２足ＺＭＰ位置が第２足ＺＭＰ許容領域から外れている場合、第２足ＺＭＰ位置を第２足ＺＭＰ許容領域に拘束する第２足ＺＭＰ拘束条件の下で前記ＺＭＰ方程式が成立するように着地タイミングを修正する第２サブステップと、
を備えることを特徴とする請求項１に記載の方法。
【請求項３】
前記第２足ＺＭＰ拘束条件は、第２足ＺＭＰ位置を第２足ＺＭＰ許容領域の境界に拘束することを特徴とする請求項２に記載の方法。
【請求項４】
第１サブステップは、算出された第１足ＺＭＰ位置が、第１足の接地領域内に定められる第１足ＺＭＰ許容領域を外れている場合、第１足ＺＭＰ位置を第１足ＺＭＰ許容領域に拘束する第１足ＺＭＰ拘束条件の下で前記ＺＭＰ方程式を満足する第１足ＺＭＰ位置と第２足ＺＭＰ位置を算出し直し、
第２サブステップは、前記第２足ＺＭＰ拘束条件と第１足ＺＭＰ拘束条件の下でＺＭＰ方程式が成立するように着地タイミングを修正することを特徴とする請求項２又は３に記載の方法。
【請求項５】
請求項１から４のいずれか１項に記載のアルゴリズムが実装されたコントローラと、
障害物を検知するセンサと、
を備えており、コントローラは、障害物の検知結果に基づいて前記着地可能領域を決定することを特徴とする二足歩行ロボット。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は、二足歩行ロボットに関する。
【背景技術】
【０００２】
二足歩行ロボット（Biped Walking Robot）は、１本の足で立つ単脚接地期間と両方の足で立つ両足接地期間を繰り返しながら歩行する。単脚接地期間では二足歩行ロボットは倒れ易い。二足歩行ロボットは、足の着地位置と着地タイミングを適切に決めないと転倒してしまう。以下、本明細書では、「転倒せずに歩行する」ことを「安定に歩行する」と表現する。また、簡単のため、以下では二足歩行ロボットを単に「ロボット」と称する。【０００３】
安定歩行のための足の着地位置を決定するのにはＺＭＰ方程式がよく用いられる。ＺＭＰ及びＺＭＰ方程式は、二足歩行ロボットの技術分野では良く知られているように、１９７２年にブコブラトビッチ（Miomir Vukoburatoric）によって提唱された。ＺＭＰは床反力の圧力中心を意味する。ＺＭＰ方程式はロボットの重心の運動方程式に相当する。ＺＭＰ方程式では、ロボットの重心の加速度がＺＭＰの関数として表される。【０００４】
ＺＭＰ方程式を所定の条件の下で解くと、重心の位置（及び／又は速度）を所定の範囲に維持できるＺＭＰ位置の時系列（ＺＭＰ軌道）が算出される。重心の位置（及び／又は速度）を所定の範囲に維持することが即ち安定歩行を意味する。他方、ＺＭＰは床反力の圧力中心であるから、実際のＺＭＰは必ず足の接地領域内に存在する（正確には、ＺＭＰは接地している足によって形成される支持多角形内に存在する。）。即ち、実際のＺＭＰは足の着地位置と着地タイミングで定まる。算出されたＺＭＰ軌道を実現するように定められる足の着地位置と着地タイミングが、安定歩行を原則的に保証する。なお、外乱、計算誤差、或いは床の起伏によって安定歩行が損なわれることがあるので、ＺＭＰ方程式から得られた足の着地位置と着地タイミングは、安定歩行を厳密に保証するものではないことに留意されたい。【０００５】
日本国特許公開公報特開平０５−３３７８４９号（特許文献１）や日本国特許公開公報２００４−１６７６７６号公報（特許文献２）には、ＺＭＰ方程式に基づいた着地位置の決定方法の一例が開示されている。特に、特許文献２には、始端と終端の重心の速度を境界条件としてＺＭＰ方程式を解く手法が開示されている。
【発明の概要】
【発明が解決しようとする課題】
【０００６】
従来は、着地タイミングを予め定めておいてからＺＭＰ方程式を解いて着地位置を算出していた。着地タイミングは歩行速度を表すので、歩行速度を予め定めることが着地タイミングを定めることに相当する。通常は、一定速度で歩行するという条件の下で着地位置を算出していた。具体的には、注目する区間の始端と終端における重心の位置及び／又は速度を境界条件として与えるとともに、着地タイミング（歩行速度）を与えてＺＭＰ方程式を満足するＺＭＰ位置を算出していた。安定歩行を実現する着地位置は、算出されたＺＭＰ位置が足と地面との接地面内に存在するように定められる。着地タイミング、即ち歩行速度は、足の動作速度、関節回転速度などの機構的な制約を超えないように予め調整されていた。【０００７】
着地タイミングを柔軟に変化させることができれば、歩行の安定性が増す。例えば、外乱等によって重心の速度（前方へ向かう速度）が増大した場合、足の着地位置を前方へシフトするだけでなく、着地タイミングを早めると転倒を回避できる可能性が高まる。あるいは、予期しない障害物を避けるために着地位置をリアルタイムで変更する場合、着地位置だけなく着地タイミングも変更することによって歩行安定性が増す。しかしながら発明者が知る限りでは、着地位置を決めた後に、歩行安定性を保証する制約の下で着地タイミング調整する技術は確立されていない。本明細書は、二足歩行ロボットの着地位置と着地タイミングを適切に求める技術を提供する。
【課題を解決するための手段】
【０００８】
本明細書が開示する技術の一つは、二足歩行ロボットの足の着地位置と着地タイミングを決定する方法を提供する。その方法は、ＺＭＰ算出ステップと、着地位置決定ステップを含む。ＺＭＰ算出ステップは、予め定められた着地タイミング（予定着地タイミング）を与えてＺＭＰ方程式を解き、次の単脚接地期間における第２足ＺＭＰ位置を算出する。より詳しくは、このステップでは、重心軌道の境界条件と第２足の着地タイミングを与えてＺＭＰ方程式を解き、第２足ＺＭＰ位置を算出する。境界条件は、二足歩行ロボットが第１足のみで立つ第１単脚接地期間と第１単脚接地期間の次に第２足のみで立つ第２単脚接地期間を含む重心軌道の始端と終端の境界条件である。境界条件は、具体的には例えば始端の重心位置と速度、及び、終端の重心速度である。始端には現在時刻が選ばれてよい。即ち、始端の重心位置と速度はセンサによって計測された現在時刻の重心位置と速度でよい。その場合、第１単脚接地期間は現在の単脚接地期間に相当する。着地位置決定ステップは、算出された第２足ＺＭＰ位置が、第２足と地面の接触領域内に含まれるように第２足の着地位置を決定する。なお、「第１足」は二足歩行ロボットの一方の脚に相当し、「第２足」は他方の脚に相当する。また、「第１足」は典型的には現在の単脚接地期間における立脚に相当し、「第２足」は現在の単脚接地期間の遊脚に相当する。また、「第２足」は、次の単脚接地期間における立脚に相当する。【０００９】
ＺＭＰ算出ステップはさらに、算出された第２足ＺＭＰ位置が第２足の着地可能領域に対応して定められる第２足ＺＭＰ許容領域から外れている場合、第２足ＺＭＰ位置が第２足ＺＭＰ許容領域内となるように着地タイミングを修正する。【００１０】
重心軌道の始端と終端の境界条件を与えてＺＭＰ方程式を解く技術の基本は、前述したように、例えば特許文献２に開示されているので参照されたい。着地可能領域は、平坦な地面であれば、足の可動範囲に基づいて定められる。地面に障害物が存在する場合、着地可能領域は足の可動範囲であって障害物以外の領域に設定される。【００１１】
上記の方法は、典型的には次の一歩の着地位置と着地タイミングを決定する。着地タイミングの下でＺＭＰ方程式に基づいて算出された第２足ＺＭＰ位置が第２足ＺＭＰ許容領域から外れる場合とは、歩行安定性を確保する次の着地位置が決定できない場合に相当する。上記の方法は、そのような場合に、即ち、着地タイミングでは着地可能領域に着地位置を決定できない場合に着地タイミングを調整する。例えば、着地タイミングでは終端の重心速度を抑制できない場合、着地タイミングを早めることによって、重心が前に移動するよりも早く前方に足を着地するように歩行計画が修正され、重心速度を抑制することができる。【００１２】
上記の方法では、着地タイミングの下で算出された第２足ＺＭＰ位置が、設定された第２足ＺＭＰ許容領域内の場合は、そのまま着地タイミングが採用される。即ち上記の方法は、着地タイミングで安定歩行が達成できるか否かを判断し、達成できないと判断された場合に着地タイミングを修正する。【００１３】
この方法では、第１足ＺＭＰ位置と第２足ＺＭＰ位置を変数として有するように変形されたＺＭＰ方程式が用いられる。第１足ＺＭＰ位置は、第１単脚接地期間におけるＺＭＰ位置を表す。ＺＭＰ方程式は、重心軌道の運動方程式であり、元々は重心の加速度をＺＭＰ位置の関数で表している。重心の位置を、ＺＭＰ位置を変数に有する数式で表すとともにその数式を積分することによって、ＺＭＰ方程式はＺＭＰの初期位置と終端位置（即ち第１足ＺＭＰ位置と第２足ＺＭＰ位置）の関数に変換される。この方程式は実施例にて詳しく説明する。本明細書が開示する方法のＺＭＰ算出ステップは、さらに詳しくは、上記したＺＭＰ方程式を使った次の第１サブステップと第２サブステップを備える。【００１４】
第１サブステップでは、前記した境界条件と予め定められたイニシャル拘束条件の下でＺＭＰ方程式を満足する第１足ＺＭＰ位置と第２足ＺＭＰ位置を算出する。第２サブステップでは、算出された第２足ＺＭＰ位置が第２足ＺＭＰ許容領域から外れている場合、第２足ＺＭＰ位置を第２足ＺＭＰ許容領域に拘束する第２足ＺＭＰ拘束条件の下でＺＭＰ方程式が成立するように着地タイミングを修正する。【００１５】
変換されたＺＭＰ方程式は、第１足ＺＭＰ位置のｘ座標とｙ座標、第２足ＺＭＰ位置のｘ座標とｙ座標の４個の未知数（変数）を有している。他方、ＺＭＰ方程式はｘ座標に関する等式とｙ座標に関する等式の連立方程式である。従って４つの変数の解を求めるには２つの拘束条件が必要である。その拘束条件がイニシャル拘束条件に相当する。イニシャル拘束条件は例えば第１足ＺＭＰ位置を指定する条件でよい。第１足は現在の立脚に相当するから、イニシャル拘束条件は現在の立脚の足の接地面内に第１足ＺＭＰ位置を定める条件でよい。【００１６】
前記した第２足ＺＭＰ拘束条件は、好ましくは、第２足ＺＭＰ位置を第２足ＺＭＰ許容領域の周囲の境界に拘束する。第２足ＺＭＰ許容領域の境界は、歩行安定性を確保するＺＭＰ位置の限界に相当する。即ち、第２足ＺＭＰ位置を第２足ＺＭＰ許容領域の境界に拘束することは、安定が保証される限界に第２足ＺＭＰ位置を定めることに相当する。【００１７】
第１サブステップと第２サブステップは、さらに、次の処理を含むことが好ましい。第１サブステップは、算出された第１足ＺＭＰ位置が、第１足の接地領域内に定められる第１足ＺＭＰ許容領域を外れている場合、第１足ＺＭＰ位置を第１足ＺＭＰ許容領域に拘束する第１足ＺＭＰ拘束条件の下で前記ＺＭＰ方程式を満足する第１足ＺＭＰ位置と第２足ＺＭＰ位置を算出し直す。第２サブステップは、第２足ＺＭＰ拘束条件と第１足ＺＭＰ拘束条件の下でＺＭＰ方程式が成立するように着地タイミングを修正する。【００１８】
本明細書が開示する他の技術は、従来よりも安定に歩行することのできる二足歩行ロボットを提供する。その二足歩行ロボットは、上記した着地位置／着地タイミング決定アルゴリズムが実装されたコントローラと、障害物を検知するセンサを備える。コントローラは、障害物の検知結果に基づいて第２足の着地可能領域を決定する。そのような二足歩行ロボットは、障害物の存在に応じてリアルタイムに着地位置と着地タイミングを変更することができる。この二足歩行ロボットは、着地可能領域に応じて着地位置のみならず着地タイミングも変更するので、安定歩行を維持できる可能性を拡げることができる。
【発明の効果】
【００１９】
本明細書が開示する技術は、安定に歩行することのできる着地位置と着地タイミングを効率よく決定する技術を提供する。
【図面の簡単な説明】
【００２０】
【図１】二足歩行ロボットの模式図である。
【図２】ＺＭＰと重心軌道を歩行面に射影した図である。
【図３】ＺＭＰ軌道と重心軌道の一例を示す図である。
【図４Ａ】コントローラが実行する処理のフローチャートである。
【図４Ｂ】コントローラが実行する処理のフローチャートである（続き）。
【図５】ＺＭＰ位置とＺＭＰ許容領域の関係の一例を示す。
【図６】ＺＭＰ位置とＺＭＰ許容領域の関係の他の例を示す。
【図７】新たな拘束条件の数を減じることができる一例を示す。
【図８】新たな拘束条件の数を減じることができる他の例を示す。
【図９】コントローラが実行する処理のアウトラインを示すフローチャートである。
【図１０】立脚の足位置とＺＭＰ許容領域の関係の一例を示す図である。
【図１１】遊脚が着地できる領域とＺＭＰ許容領域との関係の一例を示す図である。
【発明を実施するための形態】
【００２１】
図面を参照しながら本発明の好適な実施形態を説明する。図１は、本実施例の二足歩行ロボット１０の模式図である。以下、二足歩行ロボット１０を単純にロボット１０と称する。
【００２２】
ロボット１０は、２本の脚１６Ｒ、１６Ｌと、障害物センサ１２と、コントローラ１４を備える。夫々の脚は、股関節、膝関節、及び、足首関節を有する多リンク多関節構造を有している。障害物センサ１２は、レーザレンジセンサであり、ロボット１０の前方の障害物を検知する。コントローラ１４は、障害物センサ１２のセンサデータに基づき、障害物を避けるように足の着地位置と着地タイミングを決定する。
【００２３】
図１に示した符号の意味を説明する。まず座標系について説明する。Ｘ軸はロボット１０の進行方向に相当する。Ｙ軸はロボットの横方向に相当する。Ｚ軸は鉛直方向に相当する。Ｇ（ｘｇ，ｙｇ，ｚｇ）はロボット１０の重心を表し、Ｌｇは重心の軌道を表す。ＺＭＰ（ｐｘ，ｐｙ，ｐｚ）はＺＭＰ位置を表す。図１において、ロボット１０は右脚１６Ｒのみで立っているので、ＺＭＰ（ｐｘ，ｐｙ，ｐｚ）は右脚１６Ｒの足の接地領域内に位置する。なお、以下では、説明を簡単にするため、歩行面は水平であると仮定する。即ち、ＺＭＰ位置は２次元座標（ｐｘ，ｐｙ）で表される。
【００２４】
符号Ａ２は遊脚１６Ｌの着地可能領域を示している。着地可能領域Ａ２は、脚の機械的構造と、障害物Ｗで定まる。コントローラ１４は、障害物センサ１２のセンサデータに基づき、前方の障害物Ｗの位置と大きさを認識する。また、コントローラ１４は、脚の機械的構造で定まる、遊脚の足が到達可能な領域を算出する。コントローラ１４は、到達可能な領域から障害物Ｗが占める領域を除外して着地可能領域Ａ２を決定する。例えば図１に示すように、着地可能領域Ａ２は、障害物Ｗを避けるように定められる。
【００２５】
次に、ＺＭＰ方程式について説明する。ＺＭＰ方程式とは、重心Ｇの運動方程式であり、重心Ｇの加速度をＺＭＰ位置の関数で表した式である。いま、重心の高さ（即ちｚｇ）は一定の値ｈであると仮定する。そうすると、ＺＭＰ方程式は次の（数１）で表すことができる。
【００２６】
【数１】
2012011182.tif 000003
【００２７】
（数１）において、「ｇ」は重力加速度を表す。「ｄｄｘｇ」は重心のｘ方向の加速度を表す。「ｄｄｙｇ」は重心のｙ方向の加速度を表す。（数１）は歩行ロボットの技術分野においては良く知られている方程式である。（数１）においてＺＭＰ位置の座標ｐｘ，ｐｙを、次の（数２）で表す。
【００２８】
【数２】
2012011182.tif 000004
【００２９】
（数２）において、「ｑｘ１，ｑｘ２，ｑｙ１，ｑｙ２」は予め定められた定数である。また、「ｔ」は時間を表す。ＺＭＰ位置（ｐｘ，ｐｙ）を（数２）で表した場合、（数１）の一般解（即ち重心Ｇの軌道）は次の（数３）で表されることが知られている。
【００３０】
【数３】
2012011182.tif 000005
【００３１】
（数３）において、「ｃｘ１，ｃｘ２，ｃｙ１，ｃｙ２」は予め与えられる定数である。また、α２＝ｇ／ｈである。（数３）より、重心Ｇの速度（ｄｘｇ，ｄｙｇ）は次の（数４）で表される。
【００３２】
【数４】
2012011182.tif 000006
【００３３】
（数３）と（数４）は、重心Ｇの軌道を表す。ただし重心Ｇの高さｈは一定である。「安定歩行」は、重心Ｇの速度を予め定められた所定の閾値以下に抑えるとともに、重心Ｇの座標が足の着地位置から大きくずれないように着地位置と着地タイミングを決定することで実現される。そのような着地位置と着地タイミングは、ＺＭＰ方程式に基づいて算出される。
【００３４】
コントローラ１４は、遊脚１６Ｌの着地位置と着地タイミングを決定する。コントローラ１４は、（数２）〜（数４）を用いて、歩行安定性を維持する着地位置と着地タイミングを決定する。重心の軌道と着地位置の関係を図２に示す。なお、現在時刻ｔ０において、ロボット１０は右脚１６Ｒのみで立っており、左脚１６Ｌは遊脚に相当する。今、現在時刻ｔ０から終端時刻ｔ３までの期間の重心軌道に着目する。Ｇ０（ｘｇ０，ｙｇ０）は、現在時刻ｔ０における重心位置を表す。符号（ｄｘｇ０，ｄｙｇ０）は、現在時刻ｔ０における重心の速度を表す。現在時刻ｔ０における重心の速度はセンサによって計測される。符号Ｇ３（ｘｇ３，ｙｇ３）は終端時刻ｔ３における重心位置を示している。符号（ｄｘｇ３，ｄｙｇ３）は、終端時刻ｔ３における重心の速度を表している。ここで、終端時刻ｔ３は、現在時刻ｔ０からの経過時間として予め設定されている。現在時刻ｔ０は、一方の脚（図２では右脚）のみで立っている第１単脚期間内に設定され、終端時刻ｔ３は、他方の脚（図２では左脚）が接地し、接地した脚のみで立っている第２単脚期間内に設定される。終端時刻ｔ３における重心速度（ｄｘｇ３，ｄｙｇ３）は、安定歩行を保証するための境界条件として予め与えられる。ここで、終端時刻ｔ３における重心速度（境界条件）を（ｖｘｄ，ｖｙｄ）で表す。なお、現在時刻ｔ０は、今注目している重心軌道の始端の時刻に相当する。
【００３５】
図２においてＳＴＥＰ１が示す矩形は、現在時刻ｔ０における立脚の足の位置（接地面）を示す。符号Ｂ１は、現在時刻ｔ０においてＺＭＰ（ｐｘ１，ｐｙ１）が取り得る領域（第１足ＺＭＰ許容領域）を示す。以下の説明では、現在時刻ｔ０におけるＺＭＰ（ｐｘ１，ｐｙ１）が第１足ＺＭＰ位置に相当する。第１足ＺＭＰ許容領域Ｂ１は、第１足（右脚）の接地面ＳＴＥＰ１の内側に制限される。
【００３６】
符号Ａ２が示す領域は、前述したように、現在の遊脚（第２足）が着地できる領域、即ち着地可能領域を表す。図２では、着地可能領域Ａ２は、障害物Ｗを避けるように設定される。符号Ｂ２は、次の単脚接地期間、即ち、第２足のみで立つ第２単脚接地期間においてＺＭＰが取るべき位置の範囲（第２足ＺＭＰ許容領域）を表す。第２足ＺＭＰ許容領域Ｂ２は、着地可能領域Ａ２と基本的に重なる。なお、図２では、マージンを見込んでいるので、第２足ＺＭＰ許容領域Ｂ２は着地可能領域Ａ２よりも小さい。符号ＳＴＥＰ２が示す矩形は、第２足の着地位置（着地予定位置）を示している。そして符号ＺＭＰ（ｐｘ２，ｐｙ２）は、第２単脚接地期間におけるＺＭＰ位置を表している。
【００３７】
現在時刻ｔ０においてロボット１０は右脚のみで立っている。コントローラ１４は、終端時刻ｔ３における重心速度（ｄｘｇ３，ｄｙｇ３）が（ｖｘｄ，ｖｙｄ）となるように、左脚の次の着地位置ＳＴＥＰ２と着地タイミングを決定する。以下、着地タイミングを符号ｔ１で表す。着地位置ＳＴＥＰ２は、そのときの足の接地領域内に第２足ＺＭＰ位置（ｐｘ２，ｐｙ２）が含まれるように定めなければならない。また、明らかに、重心軌道は常に連続でなければならない。従って、コントローラ１４の目的は、着地可能領域Ａ２に対応して定められる第２足ＺＭＰ許容領域Ｂ２内に第２足ＺＭＰ位置を設定しながら、予め与えられた終端の重心速度（ｖｘｄ，ｖｙｄ）を満たし、なおかつ、現在の重心の状態から重心位置・速度・加速度が連続となる重心軌道を生成することにある。このことは別言すると、（数１）〜（数４）を使って、重心軌道が連続であり予め与えられた終端の重心速度（ｖｘｄ，ｖｙｄ）を満たすための、第１足ＺＭＰ位置（ｐｘ１，ｐｙ１）、第２足ＺＭＰ位置（ｐｘ２，ｐｙ２）、及び着地タイミングｔ１を決定することである。そのような重心軌道、及び、着地位置と着地タイミングは、安定な歩行を保証する。なお、現在の立脚である第１足のみで立っている第１単脚接地期間におけるＺＭＰ位置（第１足ＺＭＰ位置）と、次の一歩を踏み出したのちに第２足のみで立っている第２単脚接地期間におけるＺＭＰ位置（第２足ＺＭＰ位置）を求めるのであるから、重心軌道の注目する期間は、第１単脚接地期間と第２単脚接地期間を含む。即ち、注目する重心軌道の始端は第１単脚接地期間内に設定され、終端は第２単脚接地期間に設定される。また、予期しない外乱や路面の起伏によって歩行安定性が損なわれることがあることに留意されたい。
【００３８】
図３に、重心軌道のｘ座標（Ｌｘ）、ｘ方向速度（Ｌｄｘ）、及び、ＺＭＰ位置のｘ座標（Ｐｘ）のグラフを示す。ロボット１０は、一方の脚のみで立つ単脚接地期間と両方の脚で立つ両脚接地期間を繰り返しながら歩行する。
【００３９】
図３において、時刻ｔ０は現在時刻（始端時刻）を示している。現在時刻ｔ０は、第１単脚接地期間内である。即ち、現在時刻ｔ０においては第２足（左脚）は遊脚である。時刻ｔ１は、第２足（左脚）の着地タイミングを示している。時刻ｔ２は第１足（右脚）の離地タイミングを示している。時刻ｔ３は終端時刻を示している。なお終端時刻ｔ３は、第１足（右脚）が再び着地して両脚立脚期間が開始するタイミングに相当する。ｙ軸方向にも同様のグラフを描くことができるが、ｙ軸方向の説明は省略する。以下ではｘ軸についてのみ説明する。また、図３のＴｂｄは、両脚接地期間の時間を示している。Ｔｓｄは、単脚接地期間の時間を示している。即ち、単脚接地期間の開始時刻から時間Ｔｓｄ経過したときが、着地タイミングに相当する。
【００４０】
符号ｔ０は現在時刻（始端時刻）を表し、符号ｔ１は着地タイミングを表し、ｔ２は他方の脚の離地タイミングを表し、符号ｔ３はさらに次の着地タイミングを表す。ＺＭＰ位置のｘ座標（Ｐｘ）は、第１足のみで立っている第１単脚接地期間では一定の値ｐｘ１（第１足ＺＭＰ位置）であり、第２足の着地タイミングｔ１にて、第２足の着地位置に向かって移動し始め、第１足が離地する時刻ｔ２で第２足ＺＭＰ位置ｐｘ２となる。その後、第１足が着地する時刻ｔ３まで一定の値（第２足ＺＭＰ位置ｐｘ２）を維持する。前述したように、コントローラ１４の目的は、着地タイミングｔ１、第１足ＺＭＰ位置ｐｘ１（ｐｙ１）、第２足ＺＭＰ位置ｐｘ２（ｐｙ２）を求めることにある。重心軌道の境界条件は、始端の位置（ｘｇ０，ｙｇ０）と速度（ｄｘｇ０，ｄｙｇ０）、及び、終端の速度（ｄｘｇ３，ｄｙｇ３）＝（ｖｘｄ，ｖｙｄ）である。なお、始端の境界条件はセンサにより計測される。
【００４１】
ＺＭＰ方程式の一般解（数３、数４）を用いると、終端の重心速度ｄｘｇ３は次の（数５）で表すことができる。
【００４２】
【数５】
2012011182.tif 000007
【００４３】
なお、ｙ方向にも（数５）と同様の式が得られる。（数５）、及び、（数５）に対応するｙ方向の数式において、ｄｘｇ３＝ｖｘｄ，ｄｙｇ３＝ｖｙｄを代入してｐｘ１，ｐｘ２，ｐｙ１，ｐｙ２について整理すると次の（数６）を得る。なお、（数６）では、着地タイミングｔ１を一定として扱っている。なお、着地タイミングｔ１は予め与えられるが、後述するように修正されることがある。
【００４４】
【数６】
2012011182.tif 000008
【００４５】
（数６）において、ｒｘ１やＳｘは、（数５）の定数をまとめて表した定数である。ｒｘ１やＳｘには着地タイミングｔ１が含まれる。着地タイミングｔ１を定数として扱うことによって、ｒｘ１やＳｘを定数で表すこと可能となる。（数６）は、変数（ｐｘ１，ｐｘ２，ｐｙ１，ｐｙ２）に関する連立方程式である。（数６）は、ＺＭＰ方程式（数１）の変形であり、現在時刻ｔ０におけるＺＭＰ位置を表す第１足ＺＭＰ位置（ｐｘ１，ｐｙ１）と、第２単脚接地期間におけるＺＭＰ位置を表す第２足ＺＭＰ位置（ｐｘ２，ｐｙ２）を変数として有している。
【００４６】
（数６）を解いて変数（ｐｘ１，ｐｘ２，ｐｙ１，ｐｙ２を決定するには拘束条件が足りない。そこで、（数６）を解くための拘束条件（この拘束条件をイニシャル拘束条件と称する）を与える。イニシャル拘束条件は予め与えられる。例えばイニシャル拘束条件は次の（数７）で与えられる。
【００４７】
【数７】
2012011182.tif 000009
【００４８】
（数７）において、ρ、ａ、ｂ、ｃは予め与えられる定数である。（数７）は、第１足ＺＭＰ位置のｘ座標をρに設定し、第２足ＺＭＰ位置（ｐｘ２，ｐｙ２）を直線ａｐｘ２＋ｂｐｙ２＝ｃに拘束することを表している。（数７）の拘束条件の式と（数６）を合わせて次の（数８）が得られる。
【００４９】
【数８】
2012011182.tif 000010
【００５０】
（数８）はＺＭＰ方程式の別表現に相当する、また（数８）は、第１足ＺＭＰ位置（ｐｘ１，ｐｙ１）と第２足ＺＭＰ位置（ｐｘ２，ｐｙ２）を変数に有する連立方程式である。（数８）はマトリクス表現すると、次の（数９）で表され、その解｛ｘ｝は（数１０）で得られる。ただし［Ａ］はマトリクスであり、｛ｘ｝、｛ｓ｝はベクトルである。
【００５１】
【数９】
2012011182.tif 000011
【数１０】
2012011182.tif 000012
【００５２】
（数１０）によって、第１足ＺＭＰ位置（ｐｘ１，ｐｙ１）と、着地タイミングｔ１におけるＺＭＰ位置を表す第２足ＺＭＰ位置（ｐｘ２，ｐｙ２）を定めることができる。ここまでの処理は、まとめて表現すると、始端と終端における重心の境界条件とイニシャル拘束条件の下でＺＭＰ方程式（数６）を満足する第１足ＺＭＰ位置（ｐｘ１，ｐｙ１）と第２足ＺＭＰ位置（ｐｘ２，ｐｙ２）を算出する処理である。この処理を第１サブステップと称する。
【００５３】
第１サブステップは、着地タイミングｔ１を一定として第１足ＺＭＰ位置と第２足ＺＭＰ位置を算出する。イニシャル拘束条件の数式の数を３個以上に増やした場合には、着地タイミングｔ１を変更することでＺＭＰ方程式（数６）を満足する第１足ＺＭＰ位置（ｐｘ１，ｐｙ１）と第２足ＺＭＰ位置（ｐｘ２，ｐｙ２）を決めることができる。また、２個以上の拘束条件の式が平行でありマトリクス［Ａ］の逆行列が存在しない場合にも、着地タイミングｔ１を変更することでＺＭＰ方程式（数６）を満足する第１足ＺＭＰ位置（ｐｘ１，ｐｙ１）と第２足ＺＭＰ位置（ｐｘ２，ｐｙ２）を決めることができる。逆行列が存在するか否かは、その行列のランクを調べればよい。その行列がフルランクを有する場合、逆行列が存在する。逆行列が存在しない場合、新しい拘束条件を（数６）に与える。新しい拘束条件は例えば次の（数１１）で表される。
【００５４】
【数１１】
2012011182.tif 000013
【００５５】
（数１１）は、３個の拘束条件の数式を表している。（数１１）の拘束条件からｐｘ２，ｐｙ２は一意に決まる（数１２）。
【００５６】
【数１２】
2012011182.tif 000014
【００５７】
（数１２）を（数５）に代入し、（数１３）の条件の下でｔ１，ｐｘ１，ｐｙ１について整理すると（数１４）が得られる。
【数１３】
2012011182.tif 000015
【数１４】
2012011182.tif 000016
【００５８】
なお、（数１４）では、ｄｘｇ３＝ｖｘｄ，ｄｙｇ３＝ｖｙｄを用いている。（数１４）の第１式にａ１を乗じ、第２式にｂ１を乗じて両者を加え合わせると次の（数１５）が得られる。
【００５９】
【数１５】
2012011182.tif 000017
【００６０】
（数１５）より次の（数１６）が得られる。
【数１６】
2012011182.tif 000018
【００６１】
（数１６）を次のβとｕを使って簡略化して表すと（数１７）が得られる。なお、（数１７）のβとｕは（数１８）で与えられる。
【数１７】
2012011182.tif 000019
【数１８】
2012011182.tif 000020
【００６２】
（数１７）はｕの２次式であるからｕは次の（数１９）で決まる。
【数１９】
2012011182.tif 000021
【００６３】
（数１９）から、着地タイミングｔ１は次の（数２０）によって修正される。なお、前述したように、元の着地タイミングｔ１の値はＴｓｄであった。
【数２０】
2012011182.tif 000022
【００６４】
なお、ｕ１＞０、ｕ２＞０の場合はどちらかを選択する。基本的には、値の大きい方を選択すればよい。（数１１）から（数２０）の処理は、まとめると次のように表現することができる。コントローラ１４は、第１サブステップで算出された第２足ＺＭＰ位置（ｐｘ２，ｐｙ２）が第２足の着地可能領域Ａ２に対応して定められる第２足ＺＭＰ許容領域Ｂ２から外れている場合、第２足ＺＭＰ位置（ｐｘ２，ｐｙ２）を第２足ＺＭＰ許容領域Ｂ２に拘束する第２足ＺＭＰ拘束条件（数１２）と前述したイニシャル拘束条件の下でＺＭＰ方程式（数６）が成立するように着地タイミングｔ１を（数２０）によって修正する。これらの処理を第２サブステップと称する。
【００６５】
なお、（数１２）が示しているとおり、第２足ＺＭＰ位置（ｐｘ２，ｐｙ２）は、拘束条件によって（ρ１，ρ２）に指定することができる。この（ρ１，ρ２）は任意に与えることができるので、第２足ＺＭＰ許容領域Ｂ２の境界（図２で符号Ｂ２が示す多角形の周囲線上）を指定することが好ましい。
【００６６】
さて、（数２０）によって着地タイミングｔ１が修正された。このとき、第１足ＺＭＰ位置（ｐｘ１，ｐｙ１）は、次の処理によって算出し直される。即ち、（数２０）によって修正された着地タイミングｔ１を使って（数６）のｒｘ１，ｒｘ２，ｒｙ１，ｒｙ２を再計算すると次の（数２１）を得る。
【００６７】
【数２１】
2012011182.tif 000023
【００６８】
即ち、（数２１）によれば、（数１１）の拘束条件の下で重心速度の終端境界条件ｄｘｇ３＝ｖｘｄ，ｄｙｇ３＝ｖｙｄを満たすｔ１，ｐｘ１，ｐｙ１，ｐｘ２，ｐｙ２が算出される。このように、３個の拘束条件が与えられる場合であっても、着地タイミングｔ１を修正することによって、第１足ＺＭＰ許容領域内となる第１足ＺＭＰ位置、及び、第２足ＺＭＰ許容領域内となる第２足ＺＭＰ位置を決定することができる。
【００６９】
これらの処理は、次のとおり表現することができる。即ち、コントローラ１４は第１サブステップにおいて、算出された第１足ＺＭＰ位置（ｐｘ１，ｐｙ１）が、第１足の接地領域ＳＴＥＰ１内に定められる第１足ＺＭＰ許容領域（図２のＢ１）を外れている場合、第１足ＺＭＰ位置（ｐｘ１，ｐｙ１）を第１足ＺＭＰ許容領域Ｂ１内に拘束する第１足ＺＭＰ拘束条件とイニシャル拘束条件の下でＺＭＰ方程式を満足する第１足ＺＭＰ位置と第２足ＺＭＰ位置を算出し直す。このとき、コントローラ１４は、第２サブステップにおいて、第２足ＺＭＰ拘束条件と第１足ＺＭＰ拘束条件とイニシャル拘束条件の下でＺＭＰ方程式が成立するように着地タイミングｔ１を修正する。
【００７０】
なお、上記の例では拘束条件の式の数が３個の場合（数１１）を例示した。拘束条件の式の数は２個だがその２個の拘束条件の式が互いに平行な直線である場合、（数９）のマトリクス［Ａ］は逆行列を持たない。例えば拘束条件が次の（数２２）で与えられる場合は、（数８）は（数２３）のとおり表される。
【００７１】
【数２２】
2012011182.tif 000024
【数２３】
2012011182.tif 000025
【００７２】
（数２３）の左辺のマトリクスは逆行列を持たない。この場合は、（数５）にｐｘ１＝ρ１，ｐｘ２＝ρ２を代入して着地タイミングｔ１について整理し、（数２４）を得る。
【００７３】
【数２４】
2012011182.tif 000026
【００７４】
（数２４）からやはり（数１７）が得られ、最終的に（数２０）が得られる。このとき、コントローラ１４は、次の（数２５）を解いてｐｙ１，ｐｙ２を決定することができる。
【数２５】
2012011182.tif 000027
【００７５】
最終的に第２足ＺＭＰ位置（ｐｘ２，ｐｙ２）が算出されたら、着地する足と地面の接触領域内に第２足ＺＭＰ位置が含まれるように着地位置を決定する。図２の例では、足の接触領域の中央に第２足ＺＭＰ位置（ｐｘ２，ｐｙ２）が位置するように、着地位置ＳＴＥＰ２を決定する。こうして、遊脚の着地位置と着地タイミングが決定される。最後に着地位置を決定する処理が、着地位置決定ステップに相当する。
【００７６】
以上の処理は、図４Ａ、図４Ｂのフローチャートで表される。次に、着地タイミングと第２足ＺＭＰ位置の決定プロセスを、図４Ａと図４Ｂのフローチャートに沿って説明する。コントローラ１４はまず、第１足ＺＭＰ許容領域Ｂ１と第２足ＺＭＰ許容領域Ｂ２を設定する（Ｓ２）。第１足ＺＭＰ許容領域Ｂ１の具体例は図２に示されている。図２の例では、第１足ＺＭＰ許容領域Ｂ１は矩形である。コントローラ１４は、第１足の接地面の周囲に沿って４本の直線を定める。コントローラ１４は、直線で囲まれた領域を第１足ＺＭＰ許容領域Ｂ１と定める。各直線はａｉｘ＋ｂｉｙ＝ｃｉの形式で表される。次にコントローラ１４は、第２足ＺＭＰ許容領域を設定する。第２足ＺＭＰ許容領域は、図２の符号Ｂ２で示される。第２足ＺＭＰ許容領域Ｂ２は、着地可能領域Ａ２内に設定される。
【００７７】
次にコントローラ１４は、第２足ＺＭＰ位置（ｐｘ２，ｐｙ２）を設定する（Ｓ４）。第２足ＺＭＰ位置（ｐｘ２，ｐｙ２）は、第２足ＺＭＰ許容領域Ｂ１の中心、もしくは、予め定められた位置に設定する。図２では、第２足ＺＭＰ位置（ｐｘ２，ｐｙ２）は、遊脚の着地位置が着地可能領域Ａ２の先端に位置するように決められる。第２足ＺＭＰ位置（ｐｘ２，ｐｙ２）は、例えば（数１２）で設定される。
【００７８】
次にコントローラ１４は、（数２３）のマトリクス［Ａ］を設定する（Ｓ６）。なお、このとき、重心の高さｈ、着地タイミングｔ１、単脚接地時間Ｔｓｄ、両脚接地時間Ｔｂｄ、終端における重心速度ｖｘｄ、ｖｙｄは、予め与えられる。（数２３）のマトリクス［Ａ］に逆行列が存在する場合（Ｓ８：ＹＥＳ）、与えられた着地タイミングｔ１で連立方程式を解き、第１足ＺＭＰ位置（ｐｘ１，ｐｙ１）を求める（Ｓ１０）。なお、第２足ＺＭＰ位置（ｐｘ２，ｐｙ２）はステップＳ４で設定した。ここで、連立方程式は、ＺＭＰ方程式（数６）と拘束条件の式で与えられる。（数２３）が連立方程式の一例に相当する。算出された第１足ＺＭＰ位置（ｐｘ１，ｐｙ１）が第１足ＺＭＰ許容領域Ｂ１内であり、設定された第２足ＺＭＰ位置（ｐｘ２，ｐｙ２）が第２足ＺＭＰ許容領域Ｂ２内であるならば、コントローラ１４は、算出された第１足ＺＭＰ位置（ｐｘ１，ｐｙ１）と第２足ＺＭＰ位置（ｐｘ２，ｐｙ２）を求めるべきＺＭＰ位置として採用する（Ｓ１２：ＹＥＳ）。このとき、コントローラ１４は、第２足ＺＭＰ位置が含まれるように遊脚の足の着地位置ＳＴＥＰ２（図２参照）を決定する。最後にコントローラ１４は、遊脚の足が着地位置ＳＴＥＰ２へ向かうように遊脚の軌道を決定する（Ｓ１６）。コントローラ１４は、遊脚の足が決定された軌道に沿って動くように、脚の関節を制御する。
【００７９】
算出された第１足ＺＭＰ位置（ｐｘ１，ｐｙ１）が第１足ＺＭＰ許容領域Ｂ１を外れている場合、或いは、設定された第２足ＺＭＰ位置（ｐｘ２，ｐｙ２）が第２足ＺＭＰ許容領域Ｂ２を外れている場合、コントローラ１４は、拘束条件を追加する（Ｓ１４）。
【００８０】
算出された第１足ＺＭＰ位置（ｐｘ１，ｐｙ１）が第１足ＺＭＰ許容領域Ｂ１を外れている場合の拘束条件の例を、図５、図６を参照して説明する。第１足ＺＭＰ許容領域Ｂ１は、足の４辺を表す４本の直線（ａｉｘ＋ｂｉｙ＝ｃｉ、ｉ＝１〜４）で囲まれる領域に相当する。コントローラ１４は、算出された第１足ＺＭＰ位置（ｐｘ１，ｐｙ１）が第１足ＺＭＰ許容領域Ｂ１から外れていることを表す直線を抽出する。図５の場合、直線ａ１ｘ＋ｂ１ｙ＝ｃ１が、算出された第１足ＺＭＰ位置（ｐｘ１，ｐｙ１）が第１足ＺＭＰ許容領域Ｂ１から外れていることを示している。図６の場合、直線ａ１ｘ＋ｂ１ｙ＝ｃ１とａ２ｘ＋ｂ２ｙ＝ｃ２が、算出された第１足ＺＭＰ位置（ｐｘ１，ｐｙ１）が第１足ＺＭＰ許容領域Ｂ１から外れていることを示している。コントローラ１４は、抽出された直線を新たな拘束条件として採用する。図５の場合、新たな第１足ＺＭＰ位置（ｐｘ１，ｐｙ１）が直線ａ１ｘ＋ｂ１ｙ＝ｃ１に拘束される。図６の場合、新たな第１足ＺＭＰ位置（ｐｘ１，ｐｙ１）が、直線ａ１ｘ＋ｂ１ｙ＝ｃ１と直線ａ２ｘ＋ｂ２ｙ＝ｃ２の交点に拘束される。
【００８１】
次にコントローラ１４は、追加された拘束条件の式の数を調べる（Ｓ２２、Ｓ２４）。拘束条件の式の数が２個以下の場合（Ｓ２２：ＹＥＳ）、ステップＳ６に戻ってＺＭＰ位置を求め直す。ここで、（数２３）の行列［Ａ］は、新たな拘束条件によって変化する。例えば図５の場合、新たな拘束条件によって、行列［Ａ］は（数８）で表される。図５と図６の例は、いずれも新たな拘束条件の式の数が２個以下の場合に相当する。
【００８２】
追加された拘束条件の式の数が３個の場合（Ｓ２４：ＹＥＳ）、前述した連立方程式を時間（着地タイミングｔ１）に関して解く（Ｓ２６）。別言すれば、ステップ２６にて、着地タイミングｔ１を変数として扱って上記連立方程式を解く。（数８）或いは（数２３）が連立方程式の例に相当する。連立方程式を解くことによって、着地タイミングｔ１が改めて算出される。別言すれば、予め与えられた着地タイミングが修正される。修正された着地タイミングｔ１は、第１足ＺＭＰ位置が第１足ＺＭＰ許容領域Ｂ１内であり、第２足ＺＭＰ位置が第２足ＺＭＰ許容領域Ｂ２内であることを保証する。修正された着地タイミングｔ１は、たとえば（数２０）で算出される。最後にコントローラ１４は、修正された着地タイミングが設定範囲内であるか否かを検証する（Ｓ３０）。設定範囲は予め与えられる。
【００８３】
拘束条件の式の数が４個以上の場合（Ｓ２４：ＮＯ）、或いは、時間（着地タイミング）を変数に含む連立方程式の解が存在しない場合（Ｓ２８：ＮＯ）、この場合は安定歩行を実現する着地位置と着地タイミングが存在しないことに相当するのでコントローラ１４は歩行動作を停止する（Ｓ３２）。
【００８４】
なお、ステップＳ８において逆行列が存在しない場合（Ｓ８：ＮＯ）、ステップＳ２４に処理が移り、コントローラ１４は拘束条件の式の数が３個であるか否かを確認する（Ｓ２４）。ステップＳ２４以降の処理は既に説明したとおりである。
【００８５】
拘束条件を追加する処理の留意点を説明する。新たに追加する拘束条件の数は減じることができる場合がある。その例を図７、図８を参照して説明する。図７の場合、第１足ＺＭＰ位置（ｐｘ１，ｐｙ１）が第１足ＺＭＰ許容領域Ｂ１から外れていることを示す直線は３本である（ａ１ｘ＋ｂ１ｙ＝ｃ１、ａ２ｘ＋ｂ２ｙ＝ｃ２、及び、ａ３ｘ＋ｂ３ｙ＝ｃ３）。第１足ＺＭＰ位置（ｐｘ１，ｐｙ１）が第１足ＺＭＰ許容領域Ｂ１から外れていることを示す直線の数が奇数の場合、中央の直線（この場合はａ２ｘ＋ｂ２ｙ＝ｃ２）だけを拘束条件として採用すれば十分である。即ち、この場合、拘束条件の式の数を３から１に減じることができる。
【００８６】
図８の場合、第１足ＺＭＰ位置（ｐｘ１，ｐｙ１）が第１足ＺＭＰ許容領域Ｂ１から外れていることを示す直線は４本である（ａ１ｘ＋ｂ１ｙ＝ｃ１、ａ２ｘ＋ｂ２ｙ＝ｃ２、ａ３ｘ＋ｂ３ｙ＝ｃ３、及び、ａ４ｘ＋ｂ４ｙ＝ｃ４）。第１足ＺＭＰ位置（ｐｘ１，ｐｙ１）が第１足ＺＭＰ許容領域Ｂ１から外れていることを示す直線の数が偶数（４以上）の場合、中央の２本の直線（この場合はａ２ｘ＋ｂ２ｙ＝ｃ２とａ３ｘ＋ｂ３ｙ＝ｃ３）だけを拘束条件として採用すれば十分である。即ち、この場合、拘束条件の式の数を４から２に減じることができる。
【００８７】
図１に示した二足歩行ロボット１０のコントローラ１４には、上記説明したアルゴリズムが実装されている。二足歩行ロボット１０は、障害物センサ１２のセンサデータに基づいて遊脚の着地可能領域（図２のＡ２）を決定する。ロボット１０は、決定された着地可能領域Ａ２を使って上記アルゴリズムを実行する。ロボット１０は、上記アルゴリズムによって遊脚（第２足）の着地位置と着地タイミングを決定する。ロボット１０は、決定した着地位置と着地タイミングを実現するように脚の各関節を制御する。こうして、ロボット１０は安定に歩行することができる。
【００８８】
上記説明した着地タイミング決定の処理のアウトラインを、図９を参照して説明する。現在時刻ｔ０は、右脚のみで立っている第１単脚接地期間接地であると仮定する（図１）。立脚ロボット１０のコントローラは、まず、遊脚である左脚の着地可能領域Ａ２を決定する（Ｓ４１）。次にコントローラは、第１足ＺＭＰ許容領域と第２足ＺＭＰ許容領域を設定する（Ｓ４２）。ここで、第１足は、立脚（右脚）に相当し、第２足は遊脚（左脚）に相当する。第１足ＺＭＰ許容領域は、右脚の接地面に相当する。右足の接地面は、図２の符号ＳＴＥＰ１が示す矩形で表されている。図２の符号Ｂ１が第１足ＺＭＰ許容領域に相当する。安定のためのマージンを考慮し、第１足ＺＭＰ許容領域Ｂ１は、右足の接地面ＳＴＥＰ１よりわずかに小さく設定されている。
【００８９】
第２足ＺＭＰ許容領域は、遊脚である左足の着地可能領域に対応して定められる。左足の着地可能領域は、図２の符号Ａ２で表されている。図２の符号Ｂ２が第２足ＺＭＰ許容領域に相当する。安定のためのマージンを考慮し、第２足ＺＭＰ許容領域Ｂ２は、左足の着地可能領域Ａ２よりわずかに小さく設定されている。
【００９０】
次にコントローラは、第１単脚接地期間と第１単脚接地期間の次に第２足のみで立つ第２単脚接地期間を含む重心軌道の始端と終端の境界条件を与えるとともに、第２足の着地タイミングを与えてＺＭＰ方程式を解き、第２単脚接地期間におけるＺＭＰ位置を表す第２足ＺＭＰ位置を算出する。この処理がＺＭＰ算出ステップに相当する。図９のステップＳ４３〜Ｓ４６が、ＺＭＰ算出ステップに相当する。なお、前出したように、（数６）で表された連立方程式がＺＭＰ方程式に相当する。（数６）の連立方程式は、（数１）のＺＭＰ方程式に、重心軌道の境界条件と着地タイミングｔ１一定という条件を加えることによって得られる。
【００９１】
ＺＭＰ算出ステップは、詳しくは、第１サブステップと第２サブステップに分けられる。第１サブステップは、第１サブステップは、重心軌道の境界条件と予め定められたイニシャル拘束条件の下で（数６）の連立方程式（ＺＭＰ方程式）を満足する第１足ＺＭＰ位置と第２足ＺＭＰ位置を算出する（Ｓ４３）。第２サブステップは、Ｓ４３で算出された第２足ＺＭＰ位置が第２足ＺＭＰ許容領域から外れている場合（Ｓ４４：ＮＯ）、着地タイミングｔ１を変数として扱って再度ＺＭＰ方程式を解く（Ｓ４６）。即ち、ＺＭＰ方程式を満足する着地タイミングｔ１を求める。このとき、（数６）のＺＭＰ方程式に、第２足ＺＭＰ位置を第２足ＺＭＰ許容領域に拘束する第２足ＺＭＰ拘束条件を新たに加える（Ｓ４５）。別言すれば、第２足ＺＭＰ拘束条件とイニシャル拘束条件の下でＺＭＰ方程式が成立するように着地タイミングを修正する。
【００９２】
コントローラは、ステップＳ４４の判断においてＺＭＰ位置が許容範囲内の場合（Ｓ４４：ＹＥＳ）、或いは、ステップＳ４６で着地タイミングｔ１が求まった場合、算出された第２足ＺＭＰ位置が、第２足（左足）と地面の接触領域内に含まれるように第２足の着地位置を決定する（Ｓ４７）。この処理が着地位置決定ステップに相当する。
【００９３】
なお、ステップＳ４３では、算出された第１足ＺＭＰ位置が、第１足の接地領域ＳＴＥＰ１内に定められる第１足ＺＭＰ許容領域Ｂ１を外れている場合、第１足ＺＭＰ位置を第１足ＺＭＰ許容領域に拘束する第１足ＺＭＰ拘束条件とイニシャル拘束条件の下で前記ＺＭＰ方程式を満足する第１足ＺＭＰ位置と第２足ＺＭＰ位置を算出し直す。この処理も第１サブステップの一部である。またこのとき、ステップＳ４６では、第２足ＺＭＰ拘束条件と第１足ＺＭＰ拘束条件とイニシャル拘束条件の下でＺＭＰ方程式が成立するように着地タイミングを修正する。この処理は第２サブステップの一部である。
【００９４】
次に、着地タイミングと第２足ＺＭＰ位置の決定プロセスの別の例を示す。
（第１ステップ）まず、第１単脚接地期間におけるＺＭＰ位置を表す第１足ＺＭＰ位置の許容可能領域（第１足ＺＭＰ許容領域）を設定する。図１０に現在時刻ｔ０における第１足（立脚）の足の位置（接地面）ＳＴＥＰ１と、第１足ＺＭＰ許容領域Ｂ１を定める境界線の式の関係を示す。ここでは、接地面（足裏面）の形状は矩形であると仮定する。数式ａｉｘ＋ｂｉｙ＝ｃｉは、足裏面の各辺に平行であり、足裏面の境界から所定の幅だけ内側の直線を表す。なお、ここでは、座標系のｘｙ平面を接地面に一致させている。足裏面は矩形であるから、４本の直線が決定される。４本の直線で囲まれた領域が、第１足ＺＭＰ許容領域Ｂ１に相当する。
【００９５】
（第２ステップ）次に、現在の遊脚（第２足）が着地できる領域、即ち着地可能領域Ａ２から、第２足のＺＭＰ許容領域Ｂ２を設定し、各辺の直線の式を求める（図１１）。
【００９６】
（第３ステップ）次に、ユーザパラメータを設定する。ここで、ユーザパラメータには、重心の高さｈ、初期ＺＭＰ移動時間ｔ０、基準の単脚時間Ｔｓｄ、基準の両脚時間Ｔｂｄ、及び、現在注目している期間の終端における目標重心速度ｖｘｄ、ｖｙｄがある。
【００９７】
（第４ステップ）次に、第２足のＺＭＰ位置ｐｘ２、ｐｙ２を着地可能領域Ａ２の中心、もしくは、予め指定された位置に設定し、（数２６）を解く。なお、ここで与えられる第２足のＺＭＰ位置ｐｘ２、ｐｙ２は、テンポラリの拘束条件（イニシャル拘束条件）に相当する。
【００９８】
【数２６】
2012011182.tif 000028
【００９９】
（数２６）は、先の例における（数６）に相当する。（数２６）を解くと、ｐｘ１、ｐｘ２、ｐｙ１、ｐｙ２が求まる。
【０１００】
（第５ステップ）次に、ＺＭＰ位置ｐｘ１、ｐｘ２、ｐｙ１、ｐｙ２がＺＭＰ許容領域から外れていないかを調べる。ＺＭＰ位置がＺＭＰ許容領域から外れていない場合は、後述する第１０ステップに移行する。ＺＭＰ位置がＺＭＰ許容領域から外れている場合は、外れているＺＭＰ位置に近い直線を新たな拘束条件として追加する。第１足のＺＭＰ位置ｐｘ１、ｐｘ２が第１足ＺＭＰ許容領域Ｂ１を外れた場合の例は、先の図５、図６で例示した。図５の場合、直線ａ１ｘ＋ｂ１ｙ＝ｃ１が追加される拘束条件に相当する。図６の場合は、直線ａ１ｘ＋ｂ１ｙ＝ｃ１と直線ａ２ｘ＋ｂ２ｙ＝ｃ２が追加される拘束条件に相当する。
【０１０１】
（第６ステップ）次に、追加される拘束条件の数を調べる。１個の拘束条件が追加される場合（図５の場合）、（数２６）に直線ａ１ｘ＋ｂ１ｙ＝ｃ１を拘束条件として追加し、次の（数２７）を解く。（数２７）の第３行に、拘束条件が追加されている。
【０１０２】
【数２７】
2012011182.tif 000029
【０１０３】
２個の拘束条件が追加される場合（図６の場合）、（数２６）に直線ａ１ｘ＋ｂ１ｙ＝ｃ１と直線ａ２ｘ＋ｂ２ｙ＝ｃ２を拘束条件として追加し、次の（数２８）を解く。（数２８）の第３行と第４行に、拘束条件が追加されている。なお、拘束条件が３個の場合は、後述する第８ステップに移行する。また、拘束条件が４個以上の場合は、（数２６）を解くことができない。この場合は、歩行を継続することができないので、脚式ロボットの歩行動作を停止する。
【０１０４】
【数２８】
2012011182.tif 000030
【０１０５】
（第７ステップ）次に、行列Ａのランクを調べる。ここで、行列Ａは、（数２６）、（数２７）、又は、（数２８）の、左辺の行列に相当する。行列Ａがフルランクを有する場合、（数２６）、（数２７）、又は、（数２８）は、（数１０）の形式で解くことが出来る。この場合、（数２６）、（数２７）、又は、（数２８）を解いた後、第５ステップに戻る。行列Ａがフルランクを有していない場合、次の第８ステップに移行する。
【０１０６】
（第８ステップ）次に、第１足の着地タイミングｔ１を変数に加えて（自由度に加えて）、（数２６）、（数２７）、又は（数２８）を解く。追加された拘束条件と（数５）から、ｔ１、ｐｘ１、ｐｙ１、ｐｘ２、及び、ｐｙ２が求められる。なお、解が求められない場合は、歩行動作を停止する。
【０１０７】
（第９ステップ）次に、着地タイミングｔ１が早過ぎないか否かをチェックする。なぜならば、着地タイミングｔ１が早すぎる場合、遊脚のスイング速度が過大となってしまうからである。着地タイミングｔ１が適正な範囲内の場合、第５ステップへ戻る。着地タイミングｔ１が適正な範囲から外れている場合、歩行動作を停止する。着地タイミングｔ１の適正な範囲は、予め与えられる。
【０１０８】
（第１０ステップ）最後に、決定されたｔ１、ｐｘ１、ｐｙ１、ｐｘ２、及び、ｐｙ２の値と初期条件から、重心軌道と遊脚の足の軌道（第１足の軌道）を生成する。重心の軌道は、（数３）で与えられる。
【０１０９】
以上、本発明の具体例を詳細に説明したが、これらは例示に過ぎず、特許請求の範囲を限定するものではない。特許請求の範囲に記載の技術には、以上に例示した具体例を様々に変形、変更したものが含まれる。本明細書または図面に説明した技術要素は、単独であるいは各種の組合せによって技術的有用性を発揮するものであり、出願時請求項記載の組合せに限定されるものではない。また、本明細書または図面に例示した技術は複数目的を同時に達成し得るものであり、そのうちの一つの目的を達成すること自体で技術的有用性を持つものである。
【符号の説明】
【０１１０】
１０：二足歩行ロボット
１２：障害物センサ
１４：コントローラ
１６Ｒ、１６Ｌ：脚
【図１】
2012011182.tif 000031
【図２】
2012011182.tif 000032
【図３】
2012011182.tif 000033
【図４Ａ】
2012011182.tif 000034
【図４Ｂ】
2012011182.tif 000035
【図５】
2012011182.tif 000036
【図６】
2012011182.tif 000037
【図７】
2012011182.tif 000038
【図８】
2012011182.tif 000039
【図９】
2012011182.tif 000040
【図１０】
2012011182.tif 000041
【図１１】
2012011182.tif 000042
【国際調査報告】
2012011182.tif 000043
2012011182.tif 000044
2012011182.tif 000045
2012011182.tif 000046