JP5282852B2 - 二足歩行ロボット及び二足歩行ロボットの着地タイミング決定方法 - Google Patents

Description

本発明は、二足歩行ロボットに関する。

二足歩行ロボット（Biped Walking Robot）は、１本の足で立つ単脚接地期間と両方の足で立つ両足接地期間を繰り返しながら歩行する。単脚接地期間では二足歩行ロボットは倒れ易い。二足歩行ロボットは、足の着地位置と着地タイミングを適切に決めないと転倒してしまう。以下、本明細書では、「転倒せずに歩行する」ことを「安定に歩行する」と表現する。また、簡単のため、以下では二足歩行ロボットを単に「ロボット」と称する。

安定歩行のための足の着地位置を決定するのにはＺＭＰ方程式がよく用いられる。ＺＭＰ及びＺＭＰ方程式は、二足歩行ロボットの技術分野では良く知られているように、１９７２年にブコブラトビッチ（Miomir Vukoburatoric）によって提唱された。ＺＭＰは床反力の圧力中心を意味する。ＺＭＰ方程式はロボットの重心の運動方程式に相当する。ＺＭＰ方程式では、ロボットの重心の加速度がＺＭＰの関数として表される。

ＺＭＰ方程式を所定の条件の下で解くと、重心の位置（及び／又は速度）を所定の範囲に維持できるＺＭＰ位置の時系列（ＺＭＰ軌道）が算出される。重心の位置（及び／又は速度）を所定の範囲に維持することが即ち安定歩行を意味する。他方、ＺＭＰは床反力の圧力中心であるから、実際のＺＭＰは必ず足の接地領域内に存在する（正確には、ＺＭＰは接地している足によって形成される支持多角形内に存在する。）。即ち、実際のＺＭＰは足の着地位置と着地タイミングで定まる。算出されたＺＭＰ軌道を実現するように定められる足の着地位置と着地タイミングが、安定歩行を原則的に保証する。なお、外乱、計算誤差、或いは床の起伏によって安定歩行が損なわれることがあるので、ＺＭＰ方程式から得られた足の着地位置と着地タイミングは、安定歩行を厳密に保証するものではないことに留意されたい。

日本国特許公開公報特開平０５−３３７８４９号（特許文献１）や日本国特許公開公報２００４−１６７６７６号公報（特許文献２）には、ＺＭＰ方程式に基づいた着地位置の決定方法の一例が開示されている。特に、特許文献２には、始端と終端の重心の速度を境界条件としてＺＭＰ方程式を解く手法が開示されている。

従来は、着地タイミングを予め定めておいてからＺＭＰ方程式を解いて着地位置を算出していた。着地タイミングは歩行速度を表すので、歩行速度を予め定めることが着地タイミングを定めることに相当する。通常は、一定速度で歩行するという条件の下で着地位置を算出していた。具体的には、注目する区間の始端と終端における重心の位置及び／又は速度を境界条件として与えるとともに、着地タイミング（歩行速度）を与えてＺＭＰ方程式を満足するＺＭＰ位置を算出していた。安定歩行を実現する着地位置は、算出されたＺＭＰ位置が足と地面との接地面内に存在するように定められる。着地タイミング、即ち歩行速度は、足の動作速度、関節回転速度などの機構的な制約を超えないように予め調整されていた。

着地タイミングを柔軟に変化させることができれば、歩行の安定性が増す。例えば、外乱等によって重心の速度（前方へ向かう速度）が増大した場合、足の着地位置を前方へシフトするだけでなく、着地タイミングを早めると転倒を回避できる可能性が高まる。あるいは、予期しない障害物を避けるために着地位置をリアルタイムで変更する場合、着地位置だけなく着地タイミングも変更することによって歩行安定性が増す。しかしながら発明者が知る限りでは、着地位置を決めた後に、歩行安定性を保証する制約の下で着地タイミング調整する技術は確立されていない。本明細書は、二足歩行ロボットの着地位置と着地タイミングを適切に求める技術を提供する。

本明細書が開示する技術の一つは、二足歩行ロボットの足の着地位置と着地タイミングを決定する方法を提供する。その方法は、ＺＭＰ算出ステップと、着地位置決定ステップを含む。ＺＭＰ算出ステップは、予め定められた着地タイミング（予定着地タイミング）を与えてＺＭＰ方程式を解き、次の単脚接地期間における第２足ＺＭＰ位置を算出する。より詳しくは、このステップでは、重心軌道の境界条件と第２足の着地タイミングを与えてＺＭＰ方程式を解き、第２足ＺＭＰ位置を算出する。境界条件は、二足歩行ロボットが第１足のみで立つ第１単脚接地期間と第１単脚接地期間の次に第２足のみで立つ第２単脚接地期間を含む重心軌道の始端と終端の境界条件である。境界条件は、具体的には例えば始端の重心位置と速度、及び、終端の重心速度である。始端には現在時刻が選ばれてよい。即ち、始端の重心位置と速度はセンサによって計測された現在時刻の重心位置と速度でよい。その場合、第１単脚接地期間は現在の単脚接地期間に相当する。着地位置決定ステップは、算出された第２足ＺＭＰ位置が、第２足と地面の接触領域内に含まれるように第２足の着地位置を決定する。なお、「第１足」は二足歩行ロボットの一方の脚に相当し、「第２足」は他方の脚に相当する。また、「第１足」は典型的には現在の単脚接地期間における立脚に相当し、「第２足」は現在の単脚接地期間の遊脚に相当する。また、「第２足」は、次の単脚接地期間における立脚に相当する。

ＺＭＰ算出ステップはさらに、算出された第２足ＺＭＰ位置が第２足の着地可能領域に対応して定められる第２足ＺＭＰ許容領域から外れている場合、第２足ＺＭＰ位置が第２足ＺＭＰ許容領域内となるように着地タイミングを修正する。

重心軌道の始端と終端の境界条件を与えてＺＭＰ方程式を解く技術の基本は、前述したように、例えば特許文献２に開示されているので参照されたい。着地可能領域は、平坦な地面であれば、足の可動範囲に基づいて定められる。地面に障害物が存在する場合、着地可能領域は足の可動範囲であって障害物以外の領域に設定される。

上記の方法は、典型的には次の一歩の着地位置と着地タイミングを決定する。着地タイミングの下でＺＭＰ方程式に基づいて算出された第２足ＺＭＰ位置が第２足ＺＭＰ許容領域から外れる場合とは、歩行安定性を確保する次の着地位置が決定できない場合に相当する。上記の方法は、そのような場合に、即ち、着地タイミングでは着地可能領域に着地位置を決定できない場合に着地タイミングを調整する。例えば、着地タイミングでは終端の重心速度を抑制できない場合、着地タイミングを早めることによって、重心が前に移動するよりも早く前方に足を着地するように歩行計画が修正され、重心速度を抑制することができる。

上記の方法では、着地タイミングの下で算出された第２足ＺＭＰ位置が、設定された第２足ＺＭＰ許容領域内の場合は、そのまま着地タイミングが採用される。即ち上記の方法は、着地タイミングで安定歩行が達成できるか否かを判断し、達成できないと判断された場合に着地タイミングを修正する。

この方法では、第１足ＺＭＰ位置と第２足ＺＭＰ位置を変数として有するように変形されたＺＭＰ方程式が用いられる。第１足ＺＭＰ位置は、第１単脚接地期間におけるＺＭＰ位置を表す。ＺＭＰ方程式は、重心軌道の運動方程式であり、元々は重心の加速度をＺＭＰ位置の関数で表している。重心の位置を、ＺＭＰ位置を変数に有する数式で表すとともにその数式を積分することによって、ＺＭＰ方程式はＺＭＰの初期位置と終端位置（即ち第１足ＺＭＰ位置と第２足ＺＭＰ位置）の関数に変換される。この方程式は実施例にて詳しく説明する。本明細書が開示する方法のＺＭＰ算出ステップは、さらに詳しくは、上記したＺＭＰ方程式を使った次の第１サブステップと第２サブステップを備える。

第１サブステップでは、前記した境界条件と予め定められたイニシャル拘束条件の下でＺＭＰ方程式を満足する第１足ＺＭＰ位置と第２足ＺＭＰ位置を算出する。第２サブステップでは、算出された第２足ＺＭＰ位置が第２足ＺＭＰ許容領域から外れている場合、第２足ＺＭＰ位置を第２足ＺＭＰ許容領域に拘束する第２足ＺＭＰ拘束条件の下でＺＭＰ方程式が成立するように着地タイミングを修正する。

変換されたＺＭＰ方程式は、第１足ＺＭＰ位置のｘ座標とｙ座標、第２足ＺＭＰ位置のｘ座標とｙ座標の４個の未知数（変数）を有している。他方、ＺＭＰ方程式はｘ座標に関する等式とｙ座標に関する等式の連立方程式である。従って４つの変数の解を求めるには２つの拘束条件が必要である。その拘束条件がイニシャル拘束条件に相当する。イニシャル拘束条件は例えば第１足ＺＭＰ位置を指定する条件でよい。第１足は現在の立脚に相当するから、イニシャル拘束条件は現在の立脚の足の接地面内に第１足ＺＭＰ位置を定める条件でよい。

前記した第２足ＺＭＰ拘束条件は、好ましくは、第２足ＺＭＰ位置を第２足ＺＭＰ許容領域の周囲の境界に拘束する。第２足ＺＭＰ許容領域の境界は、歩行安定性を確保するＺＭＰ位置の限界に相当する。即ち、第２足ＺＭＰ位置を第２足ＺＭＰ許容領域の境界に拘束することは、安定が保証される限界に第２足ＺＭＰ位置を定めることに相当する。

第１サブステップと第２サブステップは、さらに、次の処理を含むことが好ましい。第１サブステップは、算出された第１足ＺＭＰ位置が、第１足の接地領域内に定められる第１足ＺＭＰ許容領域を外れている場合、第１足ＺＭＰ位置を第１足ＺＭＰ許容領域に拘束する第１足ＺＭＰ拘束条件の下で前記ＺＭＰ方程式を満足する第１足ＺＭＰ位置と第２足ＺＭＰ位置を算出し直す。第２サブステップは、第２足ＺＭＰ拘束条件と第１足ＺＭＰ拘束条件の下でＺＭＰ方程式が成立するように着地タイミングを修正する。

本明細書が開示する他の技術は、従来よりも安定に歩行することのできる二足歩行ロボットを提供する。その二足歩行ロボットは、上記した着地位置／着地タイミング決定アルゴリズムが実装されたコントローラと、障害物を検知するセンサを備える。コントローラは、障害物の検知結果に基づいて第２足の着地可能領域を決定する。そのような二足歩行ロボットは、障害物の存在に応じてリアルタイムに着地位置と着地タイミングを変更することができる。この二足歩行ロボットは、着地可能領域に応じて着地位置のみならず着地タイミングも変更するので、安定歩行を維持できる可能性を拡げることができる。

本明細書が開示する技術は、安定に歩行することのできる着地位置と着地タイミングを効率よく決定する技術を提供する。

二足歩行ロボットの模式図である。 ＺＭＰと重心軌道を歩行面に射影した図である。 ＺＭＰ軌道と重心軌道の一例を示す図である。 コントローラが実行する処理のフローチャートである。 コントローラが実行する処理のフローチャートである（続き）。 ＺＭＰ位置とＺＭＰ許容領域の関係の一例を示す。 ＺＭＰ位置とＺＭＰ許容領域の関係の他の例を示す。 新たな拘束条件の数を減じることができる一例を示す。 新たな拘束条件の数を減じることができる他の例を示す。 コントローラが実行する処理のアウトラインを示すフローチャートである。 立脚の足位置とＺＭＰ許容領域の関係の一例を示す図である。 遊脚が着地できる領域とＺＭＰ許容領域との関係の一例を示す図である。

図面を参照しながら本発明の好適な実施形態を説明する。図１は、本実施例の二足歩行ロボット１０の模式図である。以下、二足歩行ロボット１０を単純にロボット１０と称する。

ロボット１０は、２本の脚１６Ｒ、１６Ｌと、障害物センサ１２と、コントローラ１４を備える。夫々の脚は、股関節、膝関節、及び、足首関節を有する多リンク多関節構造を有している。障害物センサ１２は、レーザレンジセンサであり、ロボット１０の前方の障害物を検知する。コントローラ１４は、障害物センサ１２のセンサデータに基づき、障害物を避けるように足の着地位置と着地タイミングを決定する。

図１に示した符号の意味を説明する。まず座標系について説明する。Ｘ軸はロボット１０の進行方向に相当する。Ｙ軸はロボットの横方向に相当する。Ｚ軸は鉛直方向に相当する。Ｇ（ｘ_ｇ，ｙ_ｇ，ｚ_ｇ）はロボット１０の重心を表し、Ｌ_ｇは重心の軌道を表す。ＺＭＰ（ｐ_ｘ，ｐ_ｙ，ｐ_ｚ）はＺＭＰ位置を表す。図１において、ロボット１０は右脚１６Ｒのみで立っているので、ＺＭＰ（ｐ_ｘ，ｐ_ｙ，ｐ_ｚ）は右脚１６Ｒの足の接地領域内に位置する。なお、以下では、説明を簡単にするため、歩行面は水平であると仮定する。即ち、ＺＭＰ位置は２次元座標（ｐ_ｘ，ｐ_ｙ）で表される。

符号Ａ２は遊脚１６Ｌの着地可能領域を示している。着地可能領域Ａ２は、脚の機械的構造と、障害物Ｗで定まる。コントローラ１４は、障害物センサ１２のセンサデータに基づき、前方の障害物Ｗの位置と大きさを認識する。また、コントローラ１４は、脚の機械的構造で定まる、遊脚の足が到達可能な領域を算出する。コントローラ１４は、到達可能な領域から障害物Ｗが占める領域を除外して着地可能領域Ａ２を決定する。例えば図１に示すように、着地可能領域Ａ２は、障害物Ｗを避けるように定められる。

次に、ＺＭＰ方程式について説明する。ＺＭＰ方程式とは、重心Ｇの運動方程式であり、重心Ｇの加速度をＺＭＰ位置の関数で表した式である。いま、重心の高さ（即ちｚ_ｇ）は一定の値ｈであると仮定する。そうすると、ＺＭＰ方程式は次の（数１）で表すことができる。

（数１）において、「ｇ」は重力加速度を表す。「ｄｄｘ_ｇ」は重心のｘ方向の加速度を表す。「ｄｄｙ_ｇ」は重心のｙ方向の加速度を表す。（数１）は歩行ロボットの技術分野においては良く知られている方程式である。（数１）においてＺＭＰ位置の座標ｐ_ｘ，ｐ_ｙを、次の（数２）で表す。

（数２）において、「ｑ_ｘ１，ｑ_ｘ２，ｑ_ｙ１，ｑ_ｙ２」は予め定められた定数である。また、「ｔ」は時間を表す。ＺＭＰ位置（ｐ_ｘ，ｐ_ｙ）を（数２）で表した場合、（数１）の一般解（即ち重心Ｇの軌道）は次の（数３）で表されることが知られている。

（数３）において、「ｃ_ｘ１，ｃ_ｘ２，ｃ_ｙ１，ｃ_ｙ２」は予め与えられる定数である。また、α^２＝ｇ／ｈである。（数３）より、重心Ｇの速度（ｄｘ_ｇ，ｄｙ_ｇ）は次の（数４）で表される。

（数３）と（数４）は、重心Ｇの軌道を表す。ただし重心Ｇの高さｈは一定である。「安定歩行」は、重心Ｇの速度を予め定められた所定の閾値以下に抑えるとともに、重心Ｇの座標が足の着地位置から大きくずれないように着地位置と着地タイミングを決定することで実現される。そのような着地位置と着地タイミングは、ＺＭＰ方程式に基づいて算出される。

コントローラ１４は、遊脚１６Ｌの着地位置と着地タイミングを決定する。コントローラ１４は、（数２）〜（数４）を用いて、歩行安定性を維持する着地位置と着地タイミングを決定する。重心の軌道と着地位置の関係を図２に示す。なお、現在時刻ｔ_０において、ロボット１０は右脚１６Ｒのみで立っており、左脚１６Ｌは遊脚に相当する。今、現在時刻ｔ_０から終端時刻ｔ_３までの期間の重心軌道に着目する。Ｇ_０（ｘ_ｇ０，ｙ_ｇ０）は、現在時刻ｔ_０における重心位置を表す。符号（ｄｘ_ｇ０，ｄｙ_ｇ０）は、現在時刻ｔ_０における重心の速度を表す。現在時刻ｔ_０における重心の速度はセンサによって計測される。符号Ｇ_３（ｘ_ｇ３，ｙ_ｇ３）は終端時刻ｔ_３における重心位置を示している。符号（ｄｘ_ｇ３，ｄｙ_ｇ３）は、終端時刻ｔ_３における重心の速度を表している。ここで、終端時刻ｔ_３は、現在時刻ｔ_０からの経過時間として予め設定されている。現在時刻ｔ_０は、一方の脚（図２では右脚）のみで立っている第１単脚期間内に設定され、終端時刻ｔ_３は、他方の脚（図２では左脚）が接地し、接地した脚のみで立っている第２単脚期間内に設定される。終端時刻ｔ_３における重心速度（ｄｘ_ｇ３，ｄｙ_ｇ３）は、安定歩行を保証するための境界条件として予め与えられる。ここで、終端時刻ｔ_３における重心速度（境界条件）を（ｖ_ｘｄ，ｖ_ｙｄ）で表す。なお、現在時刻ｔ_０は、今注目している重心軌道の始端の時刻に相当する。

図２においてＳＴＥＰ１が示す矩形は、現在時刻ｔ_０における立脚の足の位置（接地面）を示す。符号Ｂ１は、現在時刻ｔ_０においてＺＭＰ（ｐ_ｘ１，ｐ_ｙ１）が取り得る領域（第１足ＺＭＰ許容領域）を示す。以下の説明では、現在時刻ｔ_０におけるＺＭＰ（ｐ_ｘ１，ｐ_ｙ１）が第１足ＺＭＰ位置に相当する。第１足ＺＭＰ許容領域Ｂ１は、第１足（右脚）の接地面ＳＴＥＰ１の内側に制限される。

符号Ａ２が示す領域は、前述したように、現在の遊脚（第２足）が着地できる領域、即ち着地可能領域を表す。図２では、着地可能領域Ａ２は、障害物Ｗを避けるように設定される。符号Ｂ２は、次の単脚接地期間、即ち、第２足のみで立つ第２単脚接地期間においてＺＭＰが取るべき位置の範囲（第２足ＺＭＰ許容領域）を表す。第２足ＺＭＰ許容領域Ｂ２は、着地可能領域Ａ２と基本的に重なる。なお、図２では、マージンを見込んでいるので、第２足ＺＭＰ許容領域Ｂ２は着地可能領域Ａ２よりも小さい。符号ＳＴＥＰ２が示す矩形は、第２足の着地位置（着地予定位置）を示している。そして符号ＺＭＰ（ｐ_ｘ２，ｐ_ｙ２）は、第２単脚接地期間におけるＺＭＰ位置を表している。

現在時刻ｔ_０においてロボット１０は右脚のみで立っている。コントローラ１４は、終端時刻ｔ_３における重心速度（ｄｘ_ｇ３，ｄｙ_ｇ３）が（ｖ_ｘｄ，ｖ_ｙｄ）となるように、左脚の次の着地位置ＳＴＥＰ２と着地タイミングを決定する。以下、着地タイミングを符号ｔ_１で表す。着地位置ＳＴＥＰ２は、そのときの足の接地領域内に第２足ＺＭＰ位置（ｐ_ｘ２，ｐ_ｙ２）が含まれるように定めなければならない。また、明らかに、重心軌道は常に連続でなければならない。従って、コントローラ１４の目的は、着地可能領域Ａ２に対応して定められる第２足ＺＭＰ許容領域Ｂ２内に第２足ＺＭＰ位置を設定しながら、予め与えられた終端の重心速度（ｖ_ｘｄ，ｖ_ｙｄ）を満たし、なおかつ、現在の重心の状態から重心位置・速度・加速度が連続となる重心軌道を生成することにある。このことは別言すると、（数１）〜（数４）を使って、重心軌道が連続であり予め与えられた終端の重心速度（ｖ_ｘｄ，ｖ_ｙｄ）を満たすための、第１足ＺＭＰ位置（ｐ_ｘ１，ｐ_ｙ１）、第２足ＺＭＰ位置（ｐ_ｘ２，ｐ_ｙ２）、及び着地タイミングｔ１を決定することである。そのような重心軌道、及び、着地位置と着地タイミングは、安定な歩行を保証する。なお、現在の立脚である第１足のみで立っている第１単脚接地期間におけるＺＭＰ位置（第１足ＺＭＰ位置）と、次の一歩を踏み出したのちに第２足のみで立っている第２単脚接地期間におけるＺＭＰ位置（第２足ＺＭＰ位置）を求めるのであるから、重心軌道の注目する期間は、第１単脚接地期間と第２単脚接地期間を含む。即ち、注目する重心軌道の始端は第１単脚接地期間内に設定され、終端は第２単脚接地期間に設定される。また、予期しない外乱や路面の起伏によって歩行安定性が損なわれることがあることに留意されたい。

図３に、重心軌道のｘ座標（Ｌ_ｘ）、ｘ方向速度（Ｌ_ｄｘ）、及び、ＺＭＰ位置のｘ座標（Ｐ_ｘ）のグラフを示す。ロボット１０は、一方の脚のみで立つ単脚接地期間と両方の脚で立つ両脚接地期間を繰り返しながら歩行する。

図３において、時刻ｔ_０は現在時刻（始端時刻）を示している。現在時刻ｔ_０は、第１単脚接地期間内である。即ち、現在時刻ｔ_０においては第２足（左脚）は遊脚である。時刻ｔ_１は、第２足（左脚）の着地タイミングを示している。時刻ｔ_２は第１足（右脚）の離地タイミングを示している。時刻ｔ_３は終端時刻を示している。なお終端時刻ｔ_３は、第１足（右脚）が再び着地して両脚立脚期間が開始するタイミングに相当する。ｙ軸方向にも同様のグラフを描くことができるが、ｙ軸方向の説明は省略する。以下ではｘ軸についてのみ説明する。また、図３のＴ_ｂｄは、両脚接地期間の時間を示している。Ｔ_ｓｄは、単脚接地期間の時間を示している。即ち、単脚接地期間の開始時刻から時間Ｔ_ｓｄ経過したときが、着地タイミングに相当する。

符号ｔ_０は現在時刻（始端時刻）を表し、符号ｔ_１は着地タイミングを表し、ｔ_２は他方の脚の離地タイミングを表し、符号ｔ_３はさらに次の着地タイミングを表す。ＺＭＰ位置のｘ座標（Ｐ_ｘ）は、第１足のみで立っている第１単脚接地期間では一定の値ｐ_ｘ１（第１足ＺＭＰ位置）であり、第２足の着地タイミングｔ_１にて、第２足の着地位置に向かって移動し始め、第１足が離地する時刻ｔ_２で第２足ＺＭＰ位置ｐ_ｘ２となる。その後、第１足が着地する時刻ｔ_３まで一定の値（第２足ＺＭＰ位置ｐ_ｘ２）を維持する。前述したように、コントローラ１４の目的は、着地タイミングｔ_１、第１足ＺＭＰ位置ｐ_ｘ１（ｐ_ｙ１）、第２足ＺＭＰ位置ｐ_ｘ２（ｐ_ｙ２）を求めることにある。重心軌道の境界条件は、始端の位置（ｘ_ｇ０，ｙ_ｇ０）と速度（ｄｘ_ｇ０，ｄｙ_ｇ０）、及び、終端の速度（ｄｘ_ｇ３，ｄｙ_ｇ３）＝（ｖ_ｘｄ，ｖ_ｙｄ）である。なお、始端の境界条件はセンサにより計測される。

ＺＭＰ方程式の一般解（数３、数４）を用いると、終端の重心速度ｄｘ_ｇ３は次の（数５）で表すことができる。

なお、ｙ方向にも（数５）と同様の式が得られる。（数５）、及び、（数５）に対応するｙ方向の数式において、ｄｘ_ｇ３＝ｖ_ｘｄ，ｄｙ_ｇ３＝ｖ_ｙｄを代入してｐ_ｘ１，ｐ_ｘ２，ｐ_ｙ１，ｐ_ｙ２について整理すると次の（数６）を得る。なお、（数６）では、着地タイミングｔ_１を一定として扱っている。なお、着地タイミングｔ_１は予め与えられるが、後述するように修正されることがある。

（数６）において、ｒ_ｘ１やＳ_ｘは、（数５）の定数をまとめて表した定数である。ｒ_ｘ１やＳ_ｘには着地タイミングｔ_１が含まれる。着地タイミングｔ_１を定数として扱うことによって、ｒ_ｘ１やＳ_ｘを定数で表すこと可能となる。（数６）は、変数（ｐ_ｘ１，ｐ_ｘ２，ｐ_ｙ１，ｐ_ｙ２）に関する連立方程式である。（数６）は、ＺＭＰ方程式（数１）の変形であり、現在時刻ｔ_０におけるＺＭＰ位置を表す第１足ＺＭＰ位置（ｐ_ｘ１，ｐ_ｙ１）と、第２単脚接地期間におけるＺＭＰ位置を表す第２足ＺＭＰ位置（ｐ_ｘ２，ｐ_ｙ２）を変数として有している。

（数６）を解いて変数（ｐ_ｘ１，ｐ_ｘ２，ｐ_ｙ１，ｐ_ｙ２を決定するには拘束条件が足りない。そこで、（数６）を解くための拘束条件（この拘束条件をイニシャル拘束条件と称する）を与える。イニシャル拘束条件は予め与えられる。例えばイニシャル拘束条件は次の（数７）で与えられる。

（数７）において、ρ、ａ、ｂ、ｃは予め与えられる定数である。（数７）は、第１足ＺＭＰ位置のｘ座標をρに設定し、第２足ＺＭＰ位置（ｐ_ｘ２，ｐ_ｙ２）を直線ａｐ_ｘ２＋ｂｐ_ｙ２＝ｃに拘束することを表している。（数７）の拘束条件の式と（数６）を合わせて次の（数８）が得られる。

（数８）はＺＭＰ方程式の別表現に相当する、また（数８）は、第１足ＺＭＰ位置（ｐ_ｘ１，ｐ_ｙ１）と第２足ＺＭＰ位置（ｐ_ｘ２，ｐ_ｙ２）を変数に有する連立方程式である。（数８）はマトリクス表現すると、次の（数９）で表され、その解｛ｘ｝は（数１０）で得られる。ただし［Ａ］はマトリクスであり、｛ｘ｝、｛ｓ｝はベクトルである。

（数１０）によって、第１足ＺＭＰ位置（ｐ_ｘ１，ｐ_ｙ１）と、着地タイミングｔ_１におけるＺＭＰ位置を表す第２足ＺＭＰ位置（ｐ_ｘ２，ｐ_ｙ２）を定めることができる。ここまでの処理は、まとめて表現すると、始端と終端における重心の境界条件とイニシャル拘束条件の下でＺＭＰ方程式（数６）を満足する第１足ＺＭＰ位置（ｐ_ｘ１，ｐ_ｙ１）と第２足ＺＭＰ位置（ｐ_ｘ２，ｐ_ｙ２）を算出する処理である。この処理を第１サブステップと称する。

第１サブステップは、着地タイミングｔ_１を一定として第１足ＺＭＰ位置と第２足ＺＭＰ位置を算出する。イニシャル拘束条件の数式の数を３個以上に増やした場合には、着地タイミングｔ_１を変更することでＺＭＰ方程式（数６）を満足する第１足ＺＭＰ位置（ｐ_ｘ１，ｐ_ｙ１）と第２足ＺＭＰ位置（ｐ_ｘ２，ｐ_ｙ２）を決めることができる。また、２個以上の拘束条件の式が平行でありマトリクス［Ａ］の逆行列が存在しない場合にも、着地タイミングｔ_１を変更することでＺＭＰ方程式（数６）を満足する第１足ＺＭＰ位置（ｐ_ｘ１，ｐ_ｙ１）と第２足ＺＭＰ位置（ｐ_ｘ２，ｐ_ｙ２）を決めることができる。逆行列が存在するか否かは、その行列のランクを調べればよい。その行列がフルランクを有する場合、逆行列が存在する。逆行列が存在しない場合、新しい拘束条件を（数６）に与える。新しい拘束条件は例えば次の（数１１）で表される。

（数１１）は、３個の拘束条件の数式を表している。（数１１）の拘束条件からｐ_ｘ２，ｐ_ｙ２は一意に決まる（数１２）。

（数１２）を（数５）に代入し、（数１３）の条件の下でｔ_１，ｐ_ｘ１，ｐ_ｙ１について整理すると（数１４）が得られる。

なお、（数１４）では、ｄｘ_ｇ３＝ｖ_ｘｄ，ｄｙ_ｇ３＝ｖ_ｙｄを用いている。（数１４）の第１式にａ_１を乗じ、第２式にｂ_１を乗じて両者を加え合わせると次の（数１５）が得られる。

（数１５）より次の（数１６）が得られる。

（数１６）を次のβとｕを使って簡略化して表すと（数１７）が得られる。なお、（数１７）のβとｕは（数１８）で与えられる。

（数１７）はｕの２次式であるからｕは次の（数１９）で決まる。

（数１９）から、着地タイミングｔ_１は次の（数２０）によって修正される。なお、前述したように、元の着地タイミングｔ_１の値はＴ_ｓｄであった。

なお、ｕ_１＞０、ｕ_２＞０の場合はどちらかを選択する。基本的には、値の大きい方を選択すればよい。（数１１）から（数２０）の処理は、まとめると次のように表現することができる。コントローラ１４は、第１サブステップで算出された第２足ＺＭＰ位置（ｐ_ｘ２，ｐ_ｙ２）が第２足の着地可能領域Ａ２に対応して定められる第２足ＺＭＰ許容領域Ｂ２から外れている場合、第２足ＺＭＰ位置（ｐ_ｘ２，ｐ_ｙ２）を第２足ＺＭＰ許容領域Ｂ２に拘束する第２足ＺＭＰ拘束条件（数１２）と前述したイニシャル拘束条件の下でＺＭＰ方程式（数６）が成立するように着地タイミングｔ_１を（数２０）によって修正する。これらの処理を第２サブステップと称する。

なお、（数１２）が示しているとおり、第２足ＺＭＰ位置（ｐ_ｘ２，ｐ_ｙ２）は、拘束条件によって（ρ_１，ρ_２）に指定することができる。この（ρ_１，ρ_２）は任意に与えることができるので、第２足ＺＭＰ許容領域Ｂ２の境界（図２で符号Ｂ２が示す多角形の周囲線上）を指定することが好ましい。

さて、（数２０）によって着地タイミングｔ_１が修正された。このとき、第１足ＺＭＰ位置（ｐ_ｘ１，ｐ_ｙ１）は、次の処理によって算出し直される。即ち、（数２０）によって修正された着地タイミングｔ_１を使って（数６）のｒ_ｘ１，ｒ_ｘ２，ｒ_ｙ１，ｒ_ｙ２を再計算すると次の（数２１）を得る。

即ち、（数２１）によれば、（数１１）の拘束条件の下で重心速度の終端境界条件ｄｘ_ｇ３＝ｖ_ｘｄ，ｄｙ_ｇ３＝ｖ_ｙｄを満たすｔ_１，ｐ_ｘ１，ｐ_ｙ１，ｐ_ｘ２，ｐ_ｙ２が算出される。このように、３個の拘束条件が与えられる場合であっても、着地タイミングｔ_１を修正することによって、第１足ＺＭＰ許容領域内となる第１足ＺＭＰ位置、及び、第２足ＺＭＰ許容領域内となる第２足ＺＭＰ位置を決定することができる。

これらの処理は、次のとおり表現することができる。即ち、コントローラ１４は第１サブステップにおいて、算出された第１足ＺＭＰ位置（ｐ_ｘ１，ｐ_ｙ１）が、第１足の接地領域ＳＴＥＰ１内に定められる第１足ＺＭＰ許容領域（図２のＢ１）を外れている場合、第１足ＺＭＰ位置（ｐ_ｘ１，ｐ_ｙ１）を第１足ＺＭＰ許容領域Ｂ１内に拘束する第１足ＺＭＰ拘束条件とイニシャル拘束条件の下でＺＭＰ方程式を満足する第１足ＺＭＰ位置と第２足ＺＭＰ位置を算出し直す。このとき、コントローラ１４は、第２サブステップにおいて、第２足ＺＭＰ拘束条件と第１足ＺＭＰ拘束条件とイニシャル拘束条件の下でＺＭＰ方程式が成立するように着地タイミングｔ_１を修正する。

なお、上記の例では拘束条件の式の数が３個の場合（数１１）を例示した。拘束条件の式の数は２個だがその２個の拘束条件の式が互いに平行な直線である場合、（数９）のマトリクス［Ａ］は逆行列を持たない。例えば拘束条件が次の（数２２）で与えられる場合は、（数８）は（数２３）のとおり表される。

（数２３）の左辺のマトリクスは逆行列を持たない。この場合は、（数５）にｐ_ｘ１＝ρ_１，ｐ_ｘ２＝ρ_２を代入して着地タイミングｔ_１について整理し、（数２４）を得る。

（数２４）からやはり（数１７）が得られ、最終的に（数２０）が得られる。このとき、コントローラ１４は、次の（数２５）を解いてｐ_ｙ１，ｐ_ｙ２を決定することができる。

最終的に第２足ＺＭＰ位置（ｐ_ｘ２，ｐ_ｙ２）が算出されたら、着地する足と地面の接触領域内に第２足ＺＭＰ位置が含まれるように着地位置を決定する。図２の例では、足の接触領域の中央に第２足ＺＭＰ位置（ｐ_ｘ２，ｐ_ｙ２）が位置するように、着地位置ＳＴＥＰ２を決定する。こうして、遊脚の着地位置と着地タイミングが決定される。最後に着地位置を決定する処理が、着地位置決定ステップに相当する。

以上の処理は、図４Ａ、図４Ｂのフローチャートで表される。次に、着地タイミングと第２足ＺＭＰ位置の決定プロセスを、図４Ａと図４Ｂのフローチャートに沿って説明する。コントローラ１４はまず、第１足ＺＭＰ許容領域Ｂ１と第２足ＺＭＰ許容領域Ｂ２を設定する（Ｓ２）。第１足ＺＭＰ許容領域Ｂ１の具体例は図２に示されている。図２の例では、第１足ＺＭＰ許容領域Ｂ１は矩形である。コントローラ１４は、第１足の接地面の周囲に沿って４本の直線を定める。コントローラ１４は、直線で囲まれた領域を第１足ＺＭＰ許容領域Ｂ１と定める。各直線はａ_ｉｘ＋ｂ_ｉｙ＝ｃ_ｉの形式で表される。次にコントローラ１４は、第２足ＺＭＰ許容領域を設定する。第２足ＺＭＰ許容領域は、図２の符号Ｂ２で示される。第２足ＺＭＰ許容領域Ｂ２は、着地可能領域Ａ２内に設定される。

次にコントローラ１４は、第２足ＺＭＰ位置（ｐ_ｘ２，ｐ_ｙ２）を設定する（Ｓ４）。第２足ＺＭＰ位置（ｐ_ｘ２，ｐ_ｙ２）は、第２足ＺＭＰ許容領域Ｂ１の中心、もしくは、予め定められた位置に設定する。図２では、第２足ＺＭＰ位置（ｐ_ｘ２，ｐ_ｙ２）は、遊脚の着地位置が着地可能領域Ａ２の先端に位置するように決められる。第２足ＺＭＰ位置（ｐ_ｘ２，ｐ_ｙ２）は、例えば（数１２）で設定される。

次にコントローラ１４は、（数２３）のマトリクス［Ａ］を設定する（Ｓ６）。なお、このとき、重心の高さｈ、着地タイミングｔ_１、単脚接地時間Ｔ_ｓｄ、両脚接地時間Ｔ_ｂｄ、終端における重心速度ｖ_ｘｄ、ｖ_ｙｄは、予め与えられる。（数２３）のマトリクス［Ａ］に逆行列が存在する場合（Ｓ８：ＹＥＳ）、与えられた着地タイミングｔ_１で連立方程式を解き、第１足ＺＭＰ位置（ｐ_ｘ１，ｐ_ｙ１）を求める（Ｓ１０）。なお、第２足ＺＭＰ位置（ｐ_ｘ２，ｐ_ｙ２）はステップＳ４で設定した。ここで、連立方程式は、ＺＭＰ方程式（数６）と拘束条件の式で与えられる。（数２３）が連立方程式の一例に相当する。算出された第１足ＺＭＰ位置（ｐ_ｘ１，ｐ_ｙ１）が第１足ＺＭＰ許容領域Ｂ１内であり、設定された第２足ＺＭＰ位置（ｐ_ｘ２，ｐ_ｙ２）が第２足ＺＭＰ許容領域Ｂ２内であるならば、コントローラ１４は、算出された第１足ＺＭＰ位置（ｐ_ｘ１，ｐ_ｙ１）と第２足ＺＭＰ位置（ｐ_ｘ２，ｐ_ｙ２）を求めるべきＺＭＰ位置として採用する（Ｓ１２：ＹＥＳ）。このとき、コントローラ１４は、第２足ＺＭＰ位置が含まれるように遊脚の足の着地位置ＳＴＥＰ２（図２参照）を決定する。最後にコントローラ１４は、遊脚の足が着地位置ＳＴＥＰ２へ向かうように遊脚の軌道を決定する（Ｓ１６）。コントローラ１４は、遊脚の足が決定された軌道に沿って動くように、脚の関節を制御する。

算出された第１足ＺＭＰ位置（ｐ_ｘ１，ｐ_ｙ１）が第１足ＺＭＰ許容領域Ｂ１を外れている場合、或いは、設定された第２足ＺＭＰ位置（ｐ_ｘ２，ｐ_ｙ２）が第２足ＺＭＰ許容領域Ｂ２を外れている場合、コントローラ１４は、拘束条件を追加する（Ｓ１４）。

算出された第１足ＺＭＰ位置（ｐ_ｘ１，ｐ_ｙ１）が第１足ＺＭＰ許容領域Ｂ１を外れている場合の拘束条件の例を、図５、図６を参照して説明する。第１足ＺＭＰ許容領域Ｂ１は、足の４辺を表す４本の直線（ａ_ｉｘ＋ｂ_ｉｙ＝ｃ_ｉ、ｉ＝１〜４）で囲まれる領域に相当する。コントローラ１４は、算出された第１足ＺＭＰ位置（ｐ_ｘ１，ｐ_ｙ１）が第１足ＺＭＰ許容領域Ｂ１から外れていることを表す直線を抽出する。図５の場合、直線ａ_１ｘ＋ｂ_１ｙ＝ｃ_１が、算出された第１足ＺＭＰ位置（ｐ_ｘ１，ｐ_ｙ１）が第１足ＺＭＰ許容領域Ｂ１から外れていることを示している。図６の場合、直線ａ_１ｘ＋ｂ_１ｙ＝ｃ_１とａ_２ｘ＋ｂ_２ｙ＝ｃ_２が、算出された第１足ＺＭＰ位置（ｐ_ｘ１，ｐ_ｙ１）が第１足ＺＭＰ許容領域Ｂ１から外れていることを示している。コントローラ１４は、抽出された直線を新たな拘束条件として採用する。図５の場合、新たな第１足ＺＭＰ位置（ｐ_ｘ１，ｐ_ｙ１）が直線ａ_１ｘ＋ｂ_１ｙ＝ｃ_１に拘束される。図６の場合、新たな第１足ＺＭＰ位置（ｐ_ｘ１，ｐ_ｙ１）が、直線ａ_１ｘ＋ｂ_１ｙ＝ｃ_１と直線ａ_２ｘ＋ｂ_２ｙ＝ｃ_２の交点に拘束される。

次にコントローラ１４は、追加された拘束条件の式の数を調べる（Ｓ２２、Ｓ２４）。拘束条件の式の数が２個以下の場合（Ｓ２２：ＹＥＳ）、ステップＳ６に戻ってＺＭＰ位置を求め直す。ここで、（数２３）の行列［Ａ］は、新たな拘束条件によって変化する。例えば図５の場合、新たな拘束条件によって、行列［Ａ］は（数８）で表される。図５と図６の例は、いずれも新たな拘束条件の式の数が２個以下の場合に相当する。

追加された拘束条件の式の数が３個の場合（Ｓ２４：ＹＥＳ）、前述した連立方程式を時間（着地タイミングｔ_１）に関して解く（Ｓ２６）。別言すれば、ステップ２６にて、着地タイミングｔ_１を変数として扱って上記連立方程式を解く。（数８）或いは（数２３）が連立方程式の例に相当する。連立方程式を解くことによって、着地タイミングｔ_１が改めて算出される。別言すれば、予め与えられた着地タイミングが修正される。修正された着地タイミングｔ_１は、第１足ＺＭＰ位置が第１足ＺＭＰ許容領域Ｂ１内であり、第２足ＺＭＰ位置が第２足ＺＭＰ許容領域Ｂ２内であることを保証する。修正された着地タイミングｔ_１は、たとえば（数２０）で算出される。最後にコントローラ１４は、修正された着地タイミングが設定範囲内であるか否かを検証する（Ｓ３０）。設定範囲は予め与えられる。

拘束条件の式の数が４個以上の場合（Ｓ２４：ＮＯ）、或いは、時間（着地タイミング）を変数に含む連立方程式の解が存在しない場合（Ｓ２８：ＮＯ）、この場合は安定歩行を実現する着地位置と着地タイミングが存在しないことに相当するのでコントローラ１４は歩行動作を停止する（Ｓ３２）。

なお、ステップＳ８において逆行列が存在しない場合（Ｓ８：ＮＯ）、ステップＳ２４に処理が移り、コントローラ１４は拘束条件の式の数が３個であるか否かを確認する（Ｓ２４）。ステップＳ２４以降の処理は既に説明したとおりである。

拘束条件を追加する処理の留意点を説明する。新たに追加する拘束条件の数は減じることができる場合がある。その例を図７、図８を参照して説明する。図７の場合、第１足ＺＭＰ位置（ｐ_ｘ１，ｐ_ｙ１）が第１足ＺＭＰ許容領域Ｂ１から外れていることを示す直線は３本である（ａ_１ｘ＋ｂ_１ｙ＝ｃ_１、ａ_２ｘ＋ｂ_２ｙ＝ｃ_２、及び、ａ_３ｘ＋ｂ_３ｙ＝ｃ_３）。第１足ＺＭＰ位置（ｐ_ｘ１，ｐ_ｙ１）が第１足ＺＭＰ許容領域Ｂ１から外れていることを示す直線の数が奇数の場合、中央の直線（この場合はａ_２ｘ＋ｂ_２ｙ＝ｃ_２）だけを拘束条件として採用すれば十分である。即ち、この場合、拘束条件の式の数を３から１に減じることができる。

図８の場合、第１足ＺＭＰ位置（ｐ_ｘ１，ｐ_ｙ１）が第１足ＺＭＰ許容領域Ｂ１から外れていることを示す直線は４本である（ａ_１ｘ＋ｂ_１ｙ＝ｃ_１、ａ_２ｘ＋ｂ_２ｙ＝ｃ_２、ａ_３ｘ＋ｂ_３ｙ＝ｃ_３、及び、ａ_４ｘ＋ｂ_４ｙ＝ｃ_４）。第１足ＺＭＰ位置（ｐ_ｘ１，ｐ_ｙ１）が第１足ＺＭＰ許容領域Ｂ１から外れていることを示す直線の数が偶数（４以上）の場合、中央の２本の直線（この場合はａ_２ｘ＋ｂ_２ｙ＝ｃ_２とａ_３ｘ＋ｂ_３ｙ＝ｃ_３）だけを拘束条件として採用すれば十分である。即ち、この場合、拘束条件の式の数を４から２に減じることができる。

図１に示した二足歩行ロボット１０のコントローラ１４には、上記説明したアルゴリズムが実装されている。二足歩行ロボット１０は、障害物センサ１２のセンサデータに基づいて遊脚の着地可能領域（図２のＡ２）を決定する。ロボット１０は、決定された着地可能領域Ａ２を使って上記アルゴリズムを実行する。ロボット１０は、上記アルゴリズムによって遊脚（第２足）の着地位置と着地タイミングを決定する。ロボット１０は、決定した着地位置と着地タイミングを実現するように脚の各関節を制御する。こうして、ロボット１０は安定に歩行することができる。

上記説明した着地タイミング決定の処理のアウトラインを、図９を参照して説明する。現在時刻ｔ_０は、右脚のみで立っている第１単脚接地期間接地であると仮定する（図１）。立脚ロボット１０のコントローラは、まず、遊脚である左脚の着地可能領域Ａ２を決定する（Ｓ４１）。次にコントローラは、第１足ＺＭＰ許容領域と第２足ＺＭＰ許容領域を設定する（Ｓ４２）。ここで、第１足は、立脚（右脚）に相当し、第２足は遊脚（左脚）に相当する。第１足ＺＭＰ許容領域は、右脚の接地面に相当する。右足の接地面は、図２の符号ＳＴＥＰ１が示す矩形で表されている。図２の符号Ｂ１が第１足ＺＭＰ許容領域に相当する。安定のためのマージンを考慮し、第１足ＺＭＰ許容領域Ｂ１は、右足の接地面ＳＴＥＰ１よりわずかに小さく設定されている。

第２足ＺＭＰ許容領域は、遊脚である左足の着地可能領域に対応して定められる。左足の着地可能領域は、図２の符号Ａ２で表されている。図２の符号Ｂ２が第２足ＺＭＰ許容領域に相当する。安定のためのマージンを考慮し、第２足ＺＭＰ許容領域Ｂ２は、左足の着地可能領域Ａ２よりわずかに小さく設定されている。

次にコントローラは、第１単脚接地期間と第１単脚接地期間の次に第２足のみで立つ第２単脚接地期間を含む重心軌道の始端と終端の境界条件を与えるとともに、第２足の着地タイミングを与えてＺＭＰ方程式を解き、第２単脚接地期間におけるＺＭＰ位置を表す第２足ＺＭＰ位置を算出する。この処理がＺＭＰ算出ステップに相当する。図９のステップＳ４３〜Ｓ４６が、ＺＭＰ算出ステップに相当する。なお、前出したように、（数６）で表された連立方程式がＺＭＰ方程式に相当する。（数６）の連立方程式は、（数１）のＺＭＰ方程式に、重心軌道の境界条件と着地タイミングｔ_１一定という条件を加えることによって得られる。

ＺＭＰ算出ステップは、詳しくは、第１サブステップと第２サブステップに分けられる。第１サブステップは、第１サブステップは、重心軌道の境界条件と予め定められたイニシャル拘束条件の下で（数６）の連立方程式（ＺＭＰ方程式）を満足する第１足ＺＭＰ位置と第２足ＺＭＰ位置を算出する（Ｓ４３）。第２サブステップは、Ｓ４３で算出された第２足ＺＭＰ位置が第２足ＺＭＰ許容領域から外れている場合（Ｓ４４：ＮＯ）、着地タイミングｔ_１を変数として扱って再度ＺＭＰ方程式を解く（Ｓ４６）。即ち、ＺＭＰ方程式を満足する着地タイミングｔ_１を求める。このとき、（数６）のＺＭＰ方程式に、第２足ＺＭＰ位置を第２足ＺＭＰ許容領域に拘束する第２足ＺＭＰ拘束条件を新たに加える（Ｓ４５）。別言すれば、第２足ＺＭＰ拘束条件とイニシャル拘束条件の下でＺＭＰ方程式が成立するように着地タイミングを修正する。

コントローラは、ステップＳ４４の判断においてＺＭＰ位置が許容範囲内の場合（Ｓ４４：ＹＥＳ）、或いは、ステップＳ４６で着地タイミングｔ１が求まった場合、算出された第２足ＺＭＰ位置が、第２足（左足）と地面の接触領域内に含まれるように第２足の着地位置を決定する（Ｓ４７）。この処理が着地位置決定ステップに相当する。

なお、ステップＳ４３では、算出された第１足ＺＭＰ位置が、第１足の接地領域ＳＴＥＰ１内に定められる第１足ＺＭＰ許容領域Ｂ１を外れている場合、第１足ＺＭＰ位置を第１足ＺＭＰ許容領域に拘束する第１足ＺＭＰ拘束条件とイニシャル拘束条件の下で前記ＺＭＰ方程式を満足する第１足ＺＭＰ位置と第２足ＺＭＰ位置を算出し直す。この処理も第１サブステップの一部である。またこのとき、ステップＳ４６では、第２足ＺＭＰ拘束条件と第１足ＺＭＰ拘束条件とイニシャル拘束条件の下でＺＭＰ方程式が成立するように着地タイミングを修正する。この処理は第２サブステップの一部である。

次に、着地タイミングと第２足ＺＭＰ位置の決定プロセスの別の例を示す。
（第１ステップ）まず、第１単脚接地期間におけるＺＭＰ位置を表す第１足ＺＭＰ位置の許容可能領域（第１足ＺＭＰ許容領域）を設定する。図１０に現在時刻ｔ_０における第１足（立脚）の足の位置（接地面）ＳＴＥＰ１と、第１足ＺＭＰ許容領域Ｂ１を定める境界線の式の関係を示す。ここでは、接地面（足裏面）の形状は矩形であると仮定する。数式ａ_ｉｘ＋ｂ_ｉｙ＝ｃ_ｉは、足裏面の各辺に平行であり、足裏面の境界から所定の幅だけ内側の直線を表す。なお、ここでは、座標系のｘｙ平面を接地面に一致させている。足裏面は矩形であるから、４本の直線が決定される。４本の直線で囲まれた領域が、第１足ＺＭＰ許容領域Ｂ１に相当する。

（第２ステップ）次に、現在の遊脚（第２足）が着地できる領域、即ち着地可能領域Ａ２から、第２足のＺＭＰ許容領域Ｂ２を設定し、各辺の直線の式を求める（図１１）。

（第３ステップ）次に、ユーザパラメータを設定する。ここで、ユーザパラメータには、重心の高さｈ、初期ＺＭＰ移動時間ｔ_０、基準の単脚時間Ｔ_ｓｄ、基準の両脚時間Ｔ_ｂｄ、及び、現在注目している期間の終端における目標重心速度ｖ_ｘｄ、ｖ_ｙｄがある。

（第４ステップ）次に、第２足のＺＭＰ位置ｐ_ｘ２、ｐ_ｙ２を着地可能領域Ａ２の中心、もしくは、予め指定された位置に設定し、（数２６）を解く。なお、ここで与えられる第２足のＺＭＰ位置ｐ_ｘ２、ｐ_ｙ２は、テンポラリの拘束条件（イニシャル拘束条件）に相当する。

（数２６）は、先の例における（数６）に相当する。（数２６）を解くと、ｐ_ｘ１、ｐ_ｘ２、ｐ_ｙ１、ｐ_ｙ２が求まる。

（第５ステップ）次に、ＺＭＰ位置ｐ_ｘ１、ｐ_ｘ２、ｐ_ｙ１、ｐ_ｙ２がＺＭＰ許容領域から外れていないかを調べる。ＺＭＰ位置がＺＭＰ許容領域から外れていない場合は、後述する第１０ステップに移行する。ＺＭＰ位置がＺＭＰ許容領域から外れている場合は、外れているＺＭＰ位置に近い直線を新たな拘束条件として追加する。第１足のＺＭＰ位置ｐ_ｘ１、ｐ_ｘ２が第１足ＺＭＰ許容領域Ｂ１を外れた場合の例は、先の図５、図６で例示した。図５の場合、直線ａ_１ｘ＋ｂ_１ｙ＝ｃ_１が追加される拘束条件に相当する。図６の場合は、直線ａ_１ｘ＋ｂ_１ｙ＝ｃ_１と直線ａ_２ｘ＋ｂ_２ｙ＝ｃ_２が追加される拘束条件に相当する。

（第６ステップ）次に、追加される拘束条件の数を調べる。１個の拘束条件が追加される場合（図５の場合）、（数２６）に直線ａ_１ｘ＋ｂ_１ｙ＝ｃ_１を拘束条件として追加し、次の（数２７）を解く。（数２７）の第３行に、拘束条件が追加されている。

２個の拘束条件が追加される場合（図６の場合）、（数２６）に直線ａ_１ｘ＋ｂ_１ｙ＝ｃ_１と直線ａ_２ｘ＋ｂ_２ｙ＝ｃ_２を拘束条件として追加し、次の（数２８）を解く。（数２８）の第３行と第４行に、拘束条件が追加されている。なお、拘束条件が３個の場合は、後述する第８ステップに移行する。また、拘束条件が４個以上の場合は、（数２６）を解くことができない。この場合は、歩行を継続することができないので、脚式ロボットの歩行動作を停止する。

（第７ステップ）次に、行列Ａのランクを調べる。ここで、行列Ａは、（数２６）、（数２７）、又は、（数２８）の、左辺の行列に相当する。行列Ａがフルランクを有する場合、（数２６）、（数２７）、又は、（数２８）は、（数１０）の形式で解くことが出来る。この場合、（数２６）、（数２７）、又は、（数２８）を解いた後、第５ステップに戻る。行列Ａがフルランクを有していない場合、次の第８ステップに移行する。

（第８ステップ）次に、第１足の着地タイミングｔ_１を変数に加えて（自由度に加えて）、（数２６）、（数２７）、又は（数２８）を解く。追加された拘束条件と（数５）から、ｔ_１、ｐ_ｘ１、ｐ_ｙ１、ｐ_ｘ２、及び、ｐ_ｙ２が求められる。なお、解が求められない場合は、歩行動作を停止する。

（第９ステップ）次に、着地タイミングｔ_１が早過ぎないか否かをチェックする。なぜならば、着地タイミングｔ_１が早すぎる場合、遊脚のスイング速度が過大となってしまうからである。着地タイミングｔ_１が適正な範囲内の場合、第５ステップへ戻る。着地タイミングｔ_１が適正な範囲から外れている場合、歩行動作を停止する。着地タイミングｔ_１の適正な範囲は、予め与えられる。

（第１０ステップ）最後に、決定されたｔ_１、ｐ_ｘ１、ｐ_ｙ１、ｐ_ｘ２、及び、ｐ_ｙ２の値と初期条件から、重心軌道と遊脚の足の軌道（第１足の軌道）を生成する。重心の軌道は、（数３）で与えられる。

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

１０：二足歩行ロボット
１２：障害物センサ
１４：コントローラ
１６Ｒ、１６Ｌ：脚

Claims (5)

1. 単脚接地期間と両脚接地期間を繰り返しながら歩行する二足歩行ロボットの足の着地位置と着地タイミングを決定する方法であり、
   第１足のみで立つ第１単脚接地期間と第１単脚接地期間の次に第２足のみで立つ第２単脚接地期間を含む重心軌道の始端と終端の境界条件を与えるとともに、第２足の着地タイミングを与えてＺＭＰ方程式を解き、第２単脚接地期間におけるＺＭＰ位置を表す第２足ＺＭＰ位置を算出するＺＭＰ算出ステップと、
   第２足と地面の接触領域内に第２足ＺＭＰ位置が含まれるように第２足の着地位置を決定する着地位置決定ステップと、
   を備えており、ＺＭＰ算出ステップは、
   算出された第２足ＺＭＰ位置が第２足の着地可能領域に対応して定められる第２足ＺＭＰ許容領域から外れている場合、第２足ＺＭＰ位置が第２足ＺＭＰ許容領域内となるように着地タイミングを修正することを特徴とする方法。
2. 前記ＺＭＰ方程式は、第１単脚接地期間におけるＺＭＰ位置を表す第１足ＺＭＰ位置と、前記第２足ＺＭＰ位置を変数として有しており、
   前記ＺＭＰ算出ステップは、前記境界条件と予め定められたイニシャル拘束条件の下で前記ＺＭＰ方程式を満足する第１足ＺＭＰ位置と第２足ＺＭＰ位置を算出する第１サブステップと、
   算出された第２足ＺＭＰ位置が第２足ＺＭＰ許容領域から外れている場合、第２足ＺＭＰ位置を第２足ＺＭＰ許容領域に拘束する第２足ＺＭＰ拘束条件の下で前記ＺＭＰ方程式が成立するように着地タイミングを修正する第２サブステップと、
   を備えることを特徴とする請求項１に記載の方法。
3. 前記第２足ＺＭＰ拘束条件は、第２足ＺＭＰ位置を第２足ＺＭＰ許容領域の境界に拘束することを特徴とする請求項２に記載の方法。
4. 第１サブステップは、算出された第１足ＺＭＰ位置が、第１足の接地領域内に定められる第１足ＺＭＰ許容領域を外れている場合、第１足ＺＭＰ位置を第１足ＺＭＰ許容領域に拘束する第１足ＺＭＰ拘束条件の下で前記ＺＭＰ方程式を満足する第１足ＺＭＰ位置と第２足ＺＭＰ位置を算出し直し、
   第２サブステップは、前記第２足ＺＭＰ拘束条件と第１足ＺＭＰ拘束条件の下でＺＭＰ方程式が成立するように着地タイミングを修正することを特徴とする請求項２又は３に記載の方法。
5. 請求項１から４のいずれか１項に記載のアルゴリズムが実装されたコントローラと、
   障害物を検知するセンサと、
   を備えており、コントローラは、障害物の検知結果に基づいて前記着地可能領域を決定することを特徴とする二足歩行ロボット。

Images