(57)【要約】【課題】複数の制約条件を満たすように目標ＺＭＰ軌道を生成する処理を、脚式移動ロボットの目標着地位置の修正と併せて効率よく行うことができる装置を提供する。【解決手段】目標ＺＭＰ軌道を表す多項式関数の各項の係数と、脚の目標着地位置を基準の目標着地位置から修正する着地位置修正量とから成る複数の未知変数の目標値を決定する変数目標値決定部５２ａを備える。変数目標値決定部５２ａは、多項式関数の係数と着地位置修正量とを未知変数として構成される評価関数と、当該未知変数に関する線形等式又は線形不等式により構成される複数の制約条件とを用い、二次計画問題の解法手法により、評価関数の値を最小化するように未知変数の目標値を決定する。【選択図】図２
2019042879.tif 000002
【特許請求の範囲】
【請求項１】
脚式移動ロボットのＺＭＰ（Zero Moment Point）の目標位置の軌道である目標ＺＭＰ軌道を生成する装置であって、
生成しようとする目標ＺＭＰ軌道の期間である軌道生成対象期間における前記脚式移動ロボットの移動時の各脚の基準の目標着地位置及び目標着地姿勢を設定する着地目標設定部と、
前記軌道生成対象期間における前記目標ＺＭＰ軌道を時間の関数として表現するように構成された複数次数の多項式関数の各項の係数と、前記軌道生成対象期間における少なくとも一つ以上の目標着地位置を、前記基準の目標着地位置から修正する修正量である着地位置修正量、又は該着地位置修正量と一定の関係を有するパラメータとから成る複数の未知変数の目標値を決定する変数目標値決定部とを備えており、
該変数目標値決定部は、前記多項式関数の各項の係数と前記着地位置修正量又は前記パラメータとを未知変数として構成される評価関数と、前記多項式関数の各項の係数と前記着地位置修正量とに関する線形等式又は線形不等式により各々構成される複数の制約条件とを用い、二次計画問題の解法手法により前記制約条件を満たしつつ、前記評価関数の値を最小化するように前記複数の未知変数の目標値を決定するように構成されていることを特徴とする目標ＺＭＰ軌道の生成装置。
【請求項２】
請求項１記載の目標ＺＭＰ軌道の生成装置において、
前記制約条件は、前記軌道生成対象期間における１つ以上のサンプリング時刻でのＺＭＰの目標位置が、該軌道生成対象期間における前記基準の目標着地位置及び目標着地姿勢に基づいて決定した基準の支持多角形を、前記着地位置修正量又は前記パラメータに応じて規定される変位量だけ変位させた支持多角形の領域内に存在するという第１制約条件を表す線形不等式を含むことを特徴とする目標ＺＭＰ軌道の生成装置。
【請求項３】
請求項２記載の目標ＺＭＰ軌道の生成装置において、
前記脚式移動ロボットは、２つの脚を有すると共に、該２つの脚が接地した状態となる両脚支持期と該２つの脚の一方だけが接地した状態となる片脚支持期とを交互に繰り返す態様で移動するロボットであり、
前記サンプリング時刻は、前記軌道生成対象期間に含まれる前記両脚支持期の時刻を含んでおり、
前記変数目標値決定部は、前記両脚支持期の直前の片脚支持期で接地状態となる第１の脚の目標着地位置及び目標着地姿勢と、該直前の片脚支持期の終了時に着地する第２の脚の前記基準の目標着地位置及び目標着地姿勢とに基づいて決定した基準の支持多角形を該両脚支持期の開始時の支持多角形として設定すると共に、該基準の支持多角形を前記第２の脚の着地位置修正量だけ平行移動させた支持多角形を該両脚支持期の終了時の支持多角形として設定し、さらに、該両脚支持期の前記サンプリング時刻での目標ＺＭＰに対応する支持多角形を、該両脚支持期の開始時から該サンプリング時刻までの時間差の、該両脚支持期の時間幅に対する比率に応じて前記第２の脚の着地位置修正量を分割してなる変位量だけ、該両脚支持期の開始時の前記基準の支持多角形を平行移動させた支持多角形として設定するように構成されていることを特徴とする目標ＺＭＰ軌道の生成装置。
【請求項４】
請求項１〜３のいずれか１項に記載の目標ＺＭＰ軌道の生成装置において、
前記制約条件は、前記脚式移動ロボットの重心の運動を、該重心に質点を有し、且つ前記目標ＺＭＰ軌道上のＺＭＰを支点として有する倒立振子質点の運動により表現するモデルにおける該倒立振子質点を安定状態に保つための条件であって、前記軌道生成対象期間の所定時刻における該倒立振子質点の運動状態が所定の運動状態に一致するという第２制約条件を表す線形等式を含み、
前記所定時刻における前記倒立振子質点の運動状態は、該所定時刻以前の基準の目標着地位置及び目標着地姿勢に応じて規定される成分と、該所定時刻以前の基準の目標着地位置に関する前記着地位置修正量又は前記パラメータに応じて規定される成分との総和として表される運動状態であることを特徴とする目標ＺＭＰ軌道の生成装置。
【請求項５】
請求項１〜４のいずれか１項に記載の目標ＺＭＰ軌道の生成装置において、
前記制約条件は、前記軌道生成対象期間の所定の時刻でのＺＭＰの目標位置が、該所定の時刻において接地状態となる脚の接地面内の所定位置に一致するという第３制約条件を表す線形等式を含み、
前記所定位置は、前記所定の時刻において接地状態となる脚の前記基準の目標着地位置及び目標着地姿勢に応じて決定した位置から、該基準の目標着地位置に対する前記着地位置修正量だけ変位させた位置として設定される位置であることを特徴とする目標ＺＭＰ軌道の生成装置。
【請求項６】
請求項１〜５のいずれか１項に記載の目標ＺＭＰ軌道の生成装置において、
前記評価関数は、前記目標ＺＭＰ軌道におけるＺＭＰの移動加速度の二乗値の積分値を前記多項式関数の各項の係数を用いて表す関数と、前記着地位置修正量のＬ２ノルムを該着地位置修正量又は前記パラメータを用いて表す関数と、前記着地位置修正量のＬ１ノルムを該着地位置修正量又は前記パラメータを用いて表す関数とを加え合わせた関数として設定された評価関数であることを特徴とする目標ＺＭＰ軌道の生成装置。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は、脚式移動ロボットの目標ＺＭＰ軌道を生成する装置に関する。
【背景技術】
【０００２】
移動ロボットの移動制御を行う場合、ＺＭＰ（Zero Moment Point）の目標位置の軌道（時間軸上での軌道）である目標ＺＭＰ軌道を生成し、この目標ＺＭＰ軌道を満足するように、ロボットの各関節の動作目標を規定する目標歩容を生成することが従来より一般に行われている（例えば特許文献１を参照）。
【０００３】
そして、目標ＺＭＰ軌道を生成する技術として、例えば特許文献２に見られる技術が提案されている。この特許文献２に見られる技術では、目標ＺＭＰ軌道が多項式関数により表現されると共に、目標ＺＭＰ軌道に関する複数の制約条件が、当該多項式関数の係数に関する線形等式もしくは線形不等式により表現される。
【０００４】
そして、上記複数の制約条件の基で、当該多項式関数の係数に関する評価関数（コスト関数）を最小化するように当該多項式関数の各係数の値を決定する処理を、２次計画問題の解法手法のアルゴリズムにより実行することで目標ＺＭＰ軌道が決定される。
【先行技術文献】
【特許文献】
【０００５】
【特許文献１】特許第３７２６０８１号公報
【特許文献２】特開２０１７−３０１２３号公報
【発明の概要】
【発明が解決しようとする課題】
【０００６】
特許文献２に見られる従来の技術では、移動ロボットの各脚の目標着地位置をあらかじめ設定した上で、目標ＺＭＰ軌道を生成するようにしている。しかしながら、この場合、目標着地位置が不適切であると、前記複数の制約条件を満たす目標ＺＭＰ軌道を生成できない場合もある。
【０００７】
このような場合には、従来は、移動ロボットの各脚の目標着地位置を設定し直した上で、改めて目標ＺＭＰ軌道を生成する必要があった。また、この場合、各脚の目標着地位置をどのように修正すればよいのかが明確に判らずに、各脚の目標着地位置の修正を試行錯誤的に繰り返すこととなる場合も多い。このため、従来は、目標ＺＭＰ軌道の生成処理に時間がかかる場合も多々ある。
【０００８】
本発明はかかる背景に鑑みてなされたものであり、複数の制約条件を満たすように目標ＺＭＰ軌道を生成する処理を、脚式移動ロボットの目標着地位置の修正と併せて効率よく行うことができる装置を提供することを目的とする。
【課題を解決するための手段】
【０００９】
本発明の目標ＺＭＰ軌道の生成装置は、上記の目的を達成するために、脚式移動ロボットのＺＭＰ（Zero Moment Point）の目標位置の軌道である目標ＺＭＰ軌道を生成する装置であって、
生成しようとする目標ＺＭＰ軌道の期間である軌道生成対象期間における前記脚式移動ロボットの移動時の各脚の基準の目標着地位置及び目標着地姿勢を設定する着地目標設定部と、
前記軌道生成対象期間における前記目標ＺＭＰ軌道を時間の関数として表現するように構成された複数次数の多項式関数の各項の係数と、前記軌道生成対象期間における少なくとも一つ以上の目標着地位置を、前記基準の目標着地位置から修正する修正量である着地位置修正量、又は該着地位置修正量と一定の関係を有するパラメータとから成る複数の未知変数の目標値を決定する変数目標値決定部とを備えており、
該変数目標値決定部は、前記多項式関数の各項の係数と前記着地位置修正量又は前記パラメータとを未知変数として構成される評価関数と、前記多項式関数の各項の係数と前記着地位置修正量とに関する線形等式又は線形不等式により各々構成される複数の制約条件とを用い、二次計画問題の解法手法により前記制約条件を満たしつつ、前記評価関数の値を最小化するように前記複数の未知変数の目標値を決定するように構成されていることを特徴とする（第１発明）。
【００１０】
ここで、脚式移動ロボット（以下、単にロボットということがある）における目標ＺＭＰ軌道は、該ロボットの移動時の各脚の着地位置の影響を受ける。そして、本願発明者の種々の検討によれば、目標ＺＭＰ軌道を複数次数の多項式関数により表現するようにした場合、目標ＺＭＰ軌道に関する種々様々な制約条件（目標ＺＭＰ軌道が満たすべき制約条件）は、当該多項式関数の各項の係数に加えて、各脚の着地位置修正量（任意の基準の着地位置からの修正量）、あるいは、該着地位置修正量と一定の関係を有するパラメータを含む形態の線形等式、あるいは、線形不等式で表現し得ることが判明した。
【００１１】
なお、上記パラメータとしては、着地位置修正量と線形関係を有するパラメータを採用し得る。
【００１２】
このため、当該多項式関数の各項の係数と、各脚の着地位置修正量又は上記パラメータを複数の未知変数として取り扱い、該未知変数を一括して決定し得る手法として、二次計画問題の解決手法を適用することが可能である。そこで、本発明では、目標ＺＭＰ軌道の生成装置を上記第１発明の構成とした。
【００１３】
かかる第１発明によれば、二次計画問題の解法手法を用いることで、前記軌道生成対象期間における目標ＺＭＰ軌道を表す多項式関数の各項の係数と、前記軌道生成対象期間における少なくとも一つ以上の目標着地位置を、基準の目標着地位置から修正する着地位置修正量、又は該着地位置修正量と一定の関係を有するパラメータとから成る複数の未知変数の目標値の全体を、一括的に前記複数の制約条件を満たすように決定することができる。
【００１４】
これにより、前記軌道生成対象期間において、複数の制約条件を満たし得る目標ＺＭＰ軌道を生成することと、一つ以上の目標着地位置を、基準の目標着地位置から修正する着地位置修正量を決定することとを、一括して行うことができる。
【００１５】
よって、第１発明によれば、複数の制約条件を満たすように目標ＺＭＰ軌道を生成する処理を、脚式移動ロボットの目標着地位置の修正と併せて効率よく行うことができる。
【００１６】
上記第１発明では、前記制約条件は、前記軌道生成対象期間における１つ以上のサンプリング時刻でのＺＭＰの目標位置が、該軌道生成対象期間における前記基準の目標着地位置及び目標着地姿勢に基づいて決定した基準の支持多角形を、前記着地位置修正量又は前記パラメータに応じて規定される変位量だけ変位させた支持多角形の領域内に存在するという第１制約条件を表す線形不等式を含むという態様を採用し得る（第２発明）。
【００１７】
これによれば、目標ＺＭＰと支持多角形との位置関係に関する第１制約条件が、前記着地位置修正量を反映させた態様の線形不等式で表される。ひいては、２次計画問題の解決手法を用いて、第１制約条件を満たす目標ＺＭＰ軌道を生成することを、目標着地位置の修正と併せて効率よく行うことができる。
【００１８】
上記第２発明では、前記脚式移動ロボットが、２つの脚を有すると共に、該２つの脚が接地した状態となる両脚支持期と該２つの脚の一方だけが接地した状態となる片脚支持期とを交互に繰り返す態様で移動するロボットであり、前記サンプリング時刻が、前記軌道生成対象期間に含まれる前記両脚支持期の時刻を含んでいる場合には、前記変数目標値決定部は、前記両脚支持期の直前の片脚支持期で接地状態となる第１の脚の目標着地位置及び目標着地姿勢と、該直前の片脚支持期の終了時に着地する第２の脚の前記基準の目標着地位置及び目標着地姿勢とに基づいて決定した基準の支持多角形を該両脚支持期の開始時の支持多角形として設定すると共に、該基準の支持多角形を前記第２の脚の着地位置修正量だけ平行移動させた支持多角形を該両脚支持期の終了時の支持多角形として設定し、さらに、該両脚支持期の前記サンプリング時刻での目標ＺＭＰに対応する支持多角形を、該両脚支持期の開始時から該サンプリング時刻までの時間差の、該両脚支持期の時間幅に対する比率に応じて前記第２の脚の着地位置修正量を分割してなる変位量だけ、該両脚支持期の開始時の前記基準の支持多角形を平行移動させた支持多角形として設定するように構成されていることが好ましい（第３発明）。
【００１９】
なお、第３発明において、前記両脚支持期の直前の片脚支持期で接地状態となる第１の脚の目標着地位置及び目標着地姿勢というのは、該第１の脚について決定済の目標着地位置及び目標着地姿勢を意味する。この場合、決定済の目標着地位置は、該第１の脚についての基準の目標着地位置、又は該第１の脚についての決定済の着地位置修正量により基準の目標着地位置を修正してなる目標着地位置を意味する。また、決定済の目標着地姿勢は、該第１の脚についての基準の目標着地姿勢である。
【００２０】
これによれば、両脚支持期の任意のサンプリング時刻での、第１制約条件を表す線形不等式を容易且つ適切に設定することが可能となる。
【００２１】
上記第１〜第３発明では、前記制約条件は、前記脚式移動ロボットの重心の運動を、該重心に質点を有し、且つ前記目標ＺＭＰ軌道上のＺＭＰを支点として有する倒立振子質点の運動により表現するモデルにおける該倒立振子質点を安定状態に保つための条件であって、前記軌道生成対象期間の所定時刻における該倒立振子質点の運動状態が所定の運動状態に一致するという第２制約条件を表す線形等式を含み得る。この場合、前記所定時刻における前記倒立振子質点の運動状態は、該所定時刻以前の基準の目標着地位置及び目標着地姿勢に応じて規定される成分と、該所定時刻以前の基準の目標着地位置に関する前記着地位置修正量又は前記パラメータに応じて規定される成分との総和として表される運動状態であることを好ましい（第４発明）。
【００２２】
これによれば、第２制約条件を表す線形等式を適切に設定できる。ひいては、二次計画問題の解決手法を用いて、第２制約条件を満たす目標ＺＭＰ軌道を生成することを、目標着地位置の修正と併せて効率よく行うことができる。
【００２３】
なお、第４発明における上記倒立振子質点の運動状態としては、例えば該倒立振子質点の位置と移動速度との線形結合により表される運動状態を採用できる。
【００２４】
上記第１〜第４発明では、前記制約条件は、前記軌道生成対象期間の所定の時刻でのＺＭＰの目標位置が、該所定の時刻において接地状態となる脚の接地面内の所定位置に一致するという第３制約条件を表す線形等式を含み得る。この場合、前記所定位置は、前記所定の時刻において接地状態となる脚の前記基準の目標着地位置及び目標着地姿勢に応じて決定した位置から、該基準の目標着地位置に対する前記着地位置修正量だけ変位させた位置として設定される位置であることが好ましい（第５発明）。
【００２５】
これによれば、第３制約条件を表す線形等式を適切に設定できる。ひいては、二次計画問題の解決手法を用いて、第３制約条件を満たす目標ＺＭＰ軌道を生成することを、目標着地位置の修正と併せて効率よく行うことができる。
【００２６】
上記第１〜第５発明では、前記評価関数は、前記目標ＺＭＰ軌道におけるＺＭＰの移動加速度の二乗値の積分値を前記多項式関数の各項の係数を用いて表す関数と、前記着地位置修正量のＬ２ノルムを該着地位置修正量又は前記パラメータを用いて表す関数と、前記着地位置修正量のＬ１ノルムを該着地位置修正量又は前記パラメータを用いて表す関数とを加え合わせた関数として設定された評価関数であることが好ましい（第６発明）。
【００２７】
これによれば、二次計画問題の解決手法によって、ＺＭＰの移動加速度の二乗値の積分値を極力小さくし、且つ、着地位置修正量のＬ２ノルム及びＬ１ノルムを極力小さくするように、最小化するように、目標ＺＭＰ軌道を表す多項式関数の各項の係数と、前記着地位置修正量又はパラメータとから成る複数の未知変数の目標値が決定されることとなる。
【００２８】
このため、目標ＺＭＰ軌道におけるＺＭＰの移動速度が急激に変化するような形態で目標ＺＭＰ軌道が生成されるのを防止し、且つ、着地位置修正量が極力小さくなるように（各脚の着地位置が極力、基準の着地位置から乖離しないように）、目標ＺＭＰ軌道を生成できる。ひいては、ロボットの滑らかな移動を行い得る目標歩容を生成できる。
【図面の簡単な説明】
【００２９】
【図１】本発明の一実施形態における脚式移動ロボットの構成を示す図。
【図２】脚式移動ロボットの制御装置の機能を示すブロック図。
【図３】脚式移動ロボットの移動時の足部の目標着地位置と、これに対応する目標ＺＭＰ軌道（Ｙ軸方向での軌道）とを例示する図。
【図４】脚式移動ロボットの移動時の足部の目標着地位置と、これに対応する目標ＺＭＰ軌道（Ｘ軸方向での軌道）とを例示する図。
【図５】制約条件としての経由点条件に関する線形等式の設定に関する説明図。
【図６】図６Ａは片脚支持期での支持多角形を例示する図、図６Ｂは両脚支持期での支持多角形を例示する図。
【図７】制約条件としてのＺＭＰ存在許容領域条件に関する線形不等式の設定に関する説明図。
【図８】制約条件としての幾何学条件に関する説明図。
【図９】実施例における外力と、目標着地位置姿勢の基準からのずれ量と、目標ＺＭＰのずれ量とを例示するグラフ。
【発明を実施するための形態】
【００３０】
本発明の一実施形態を図１〜図９を参照して以下に説明する。図１は本実施形態で例示する脚式移動ロボットの概略構成を模式的に示している。この脚式移動ロボット１（以降、単にロボット１という）は、基体２と、基体２から延設された複数（本実施形態では４つ）の可動リンク機構３Ｌ，３Ｒ，４Ｌ，４Ｒと、頭部５とを有する人型の脚式移動ロボットである。
【００３１】
４つの可動リンク機構３Ｌ，３Ｒ，４Ｌ，４Ｒのうちの可動リンク機構３Ｌ，３Ｒは、左右一対の脚リンク機構に相当するリンク機構、可動リンク機構４Ｌ，４Ｒは、左右一対の腕リンク機構に相当するリンク機構である。
【００３２】
なお、図示の参照符号中の「Ｌ」、「Ｒ」は、それぞれロボット１の左側の構成要素、右側の構成要素を示す符号である。ただし、以降の説明では、左右を区別する必要が無いときは、符号「Ｌ」，「Ｒ」を省略する。また、可動リンク機構３Ｌ，３Ｒを脚リンク機構３Ｌ，３Ｒ（又は脚リンク機構３）と称し、可動リンク機構４Ｌ，４Ｒを腕リンク機構４Ｌ，４Ｒ（又は腕リンク機構４）と称する場合がある。
【００３３】
また、本実施形態の以下の説明では、ヨー方向、ピッチ方向、ロール方向は、それぞれ図１に示すようにロボット１が直立姿勢で起立した状態（基体２、各可動リンク機構３Ｒ，３Ｌ，４Ｒ，４Ｌをほぼ上下方向に伸ばした状態）での該ロボット１の上下方向の軸（図１に示すＺ軸）周りの方向、左右方向の軸（図１に示すＹ軸）周りの方向、前後方向の軸（図１ではＸ軸）周りの方向を意味する。なお、図１に示すＸ軸、Ｙ軸、Ｚ軸は、３軸直交座標系の各座標軸である。
【００３４】
基体２は、ロボット１の上体に相当するリンク機構である。この基体２は、本実施形態では、下側基体１０と、この下側基体１０の上方に配置された上側基体１１との２つの要素リンクと、該下側基体１０及び上側基体１１を連結する関節機構１２とから構成されている。図示例のロボット１では、関節機構１２は、例えばピッチ方向（Ｙ軸周り方向）の回転自由度を有する関節１２ａにより構成されている。
【００３５】
可動リンク機構３Ｌ，３Ｒ，４Ｌ，４Ｒのうちの脚リンク機構３Ｌ，３Ｒは、互いに同じ構造のものである。具体的には、各脚リンク機構３は、下側基体１０に股関節機構１３を介して連結された大腿部１４と、この大腿部１４に膝関節機構１５を介して連結された下腿部１６と、この下腿部１６に足首関節機構１７を介して連結された足部１８とを、該脚リンク機構３を構成する要素リンクとして備える。足部１８は、脚リンク機構３の先端部を構成する要素リンクである。
【００３６】
図示例のロボット１では、股関節機構１３は、ヨー方向（Ｚ軸周り方向）、ピッチ方向（Ｙ軸周り方向）及びロール方向（Ｘ軸周り方向）の回転自由度をそれぞれ有する３つの関節１９，２０，２１により構成されている。また、膝関節機構１５は、ピッチ方向の回転自由度を有する関節２２により構成されている。また、足首関節機構１７は、ピッチ方向及びロール方向の回転自由度をそれぞれ有する２つの関節２３，２４により構成されている。
【００３７】
従って、本実施形態では、各脚リンク機構３は、その先端部（足部１８）が下側基体１０に対して、６自由度の運動自由度を有するように構成されている。
【００３８】
可動リンク機構３Ｌ，３Ｒ，４Ｌ，４Ｒのうちの腕リンク機構４Ｒ，４Ｌは、互いに同じ構造のものである。具体的には、各腕リンク機構４は、上側基体１１に肩関節機構２５を介して連結された上腕部２６と、この上腕部２６に肘関節機構２７を介して連結された前腕部２８と、この前腕部２８に手首関節機構２９を介して連結されたハンド部３０とを、該腕リンク機構４を構成する要素リンクとして備える。ハンド部３０は、腕リンク機構４の先端部を構成する要素リンクである。
【００３９】
図示例のロボット１では、肩関節機構２５は、ピッチ方向、ロール方向、及びヨー方向の回転自由度をそれぞれ有する３つの関節３１，３２，３３により構成されている。また、肘関節機構２７は、ピッチ方向（又はロール方向）の回転自由度を有する関節３４により構成されている。また、手首関節機構２９は、ヨー方向、ピッチ方向及びロール方向の回転自由度をそれぞれ有する３つの関節３５，３６，３７により構成されている。
【００４０】
従って、本実施形態では、各腕リンク機構４は、その先端部（ハンド部３０）が上側基体１１に対して、７自由度の運動自由度を有するように構成されている。
【００４１】
なお、ハンド部３０は、該ハンド部３０による作業用の開閉機構、あるいは、複数の屈伸可能な指機構等を備えていてもよい。
【００４２】
頭部５は、上側基体１１の上方に配置され、該上側基体１１に固定されている。ただし、頭部５は、例えばチルト動作あるいはパン動作等を行い得るように、上側基体１１に対して関節機構を介して連結されていてもよい。
【００４３】
以上が本実施形態のロボット１の機構的な構造の概要である。かかる構造のロボット１は、基本的には、２つの脚としての左右の脚リンク機構３Ｌ，３Ｒの動作（各足部１８の空中移動及びそれに続く接地を繰り返す２脚歩容の動作）によって移動できる。
【００４４】
あるいは、腕リンク機構４Ｌ，４Ｒを脚リンク機構３Ｌ，３Ｒとは別の脚リンク機構として動作させてロボット１の移動を行うことも可能である。例えば、脚リンク機構３Ｌ，３Ｒ及び腕リンク機構４Ｌ，４Ｒの４つの可動リンク機構のそれぞれの先端部（足部１８、ハンド部３０）の空中移動及びそれに続く接地を繰り返す４脚歩容の動作によってロボット１の移動を行うことも可能である。
【００４５】
補足すると、各脚リンク機構３は、例えば６自由度よりも大きい運動自由度を有するように構成されていてもよい。また、各腕リンク機構４は、例えば７自由度よりも小さい運動自由度、もしくは７自由度よりも大きい運動自由度を有するように構成されていてもよい。
【００４６】
また、基体２の関節機構１２は、ロール方向又はヨー方向の回転自由度を有する関節により構成されていてもよい。あるいは、該関節機構１２は、１自由度よりも大きい運動自由度を有するように構成されていてもよい。あるいは、基体２は、関節機構１２を備えない一体構成のもの（下側基体１０と上側基体１１とを一体化した構成のもの）であってもよい。
【００４７】
また、各脚リンク機構３及び各腕リンク機構４のそれぞれは、回転型の関節に限らず、直動型の関節を含んでいてもよい。
【００４８】
また、ロボット１は、頭部５を持たない構造のもの、あるいは腕リンク機構４Ｒ，４Ｌの一方又は両方を備えないものであってもよい。
【００４９】
また、ロボット１は、３つ以上の脚リンク機構を備えるものであってもよい。なお、脚リンク機構３Ｌ，３Ｒに加えて、腕リンク機構４Ｌ，４Ｒを脚リンク機構として動作させてロボット１の移動を行う場合には、ロボット１は、実質的に４つの脚リンク機構を有するロボットとみなすこともできる。
【００５０】
図１での図示は省略したが、ロボット１には、図２に示すように、上記の各関節をそれぞれ回転駆動する関節アクチュエータ４０と、ロボット１の動作制御を行なう制御装置４１とが搭載されている。
【００５１】
関節アクチュエータ４０は、例えば電動モータあるいは油圧アクチュエータにより構成されたものであり、各関節毎に備えられている。この場合、各関節アクチュエータ４０による各関節の駆動機構は公知の構造のものでよい。また、関節アクチュエータ４０は、回転型のアクチュエータに限らず、直動型のアクチュエータであってもよい。
【００５２】
制御装置４１は、ＣＰＵ、ＲＡＭ、ＲＯＭ、インターフェース回路等を含む電子回路ユニットにより構成されている。この制御装置４１は、実装されたプログラムとハードウェア構成とにより実現される主要な機能として、ロボット１の各関節の動作制御のための制御用目標値としての目標歩容（ロボット１の動作目標）を生成する目標歩容生成部４２と、ロボット１の実際の姿勢を安定化するように該目標歩容を適宜修正する安定化制御部４３と、該安定化制御部４３による修正後の目標歩容に応じてロボット１の各関節アクチュエータ４０を制御する関節アクチュエータ制御部４４とを備える。
【００５３】
目標歩容生成部４２は、ロボット１の移動時に空中移動及びそれに続く着地（接地）を行わせる部位（足部１８又はハンド部３０）である移動接地部のそれぞれの着地予定箇所での位置及び姿勢の目標（以降、目標着地位置姿勢という）と該着地予定箇所での着地時刻の目標（以降、目標着地時刻という）とを設定する着地目標設定部５１と、ＺＭＰ（Zero Moment Point）の目標位置（以降、目標ＺＭＰという）の軌道である目標ＺＭＰ軌道を生成すると共に、前記目標着地位置姿勢のうちの目標着地位置を修正するための着地位置修正量を決定する目標ＺＭＰ軌道生成部５２と、ロボット１全体の重心の目標位置の軌道である目標重心軌道を生成する目標重心軌道生成部５３と、ロボット１の各関節の変位量（本実施形態では回転角）の目標値である目標関節変位量の軌道を生成する目標関節変位量軌道生成部５４とを含む。
【００５４】
ここで本明細書での用語に関して補足すると、ロボット１の足部１８等の任意の部位の「位置」は、該部位の代表点（該部位に対して固定された点）の空間的な位置を意味し、任意の部位の「姿勢」は、該部位の空間的な向きを意味する。
【００５５】
また、「位置姿勢」は、「位置」及び「姿勢」の組、「目標位置姿勢」は、「目標位置」及び「目標姿勢」の組を意味する。また、「軌道」は、瞬時値の時系列、又は時間の関数として表現されるものを意味する。
【００５６】
なお、ロボット１の任意の部位の「位置」及び「姿勢」のそれぞれは、ロボット１の動作環境に対して任意に設計的に設定されるグローバル座標系（ワールド座標系）で見た位置及び姿勢として記述される。
【００５７】
また、ＺＭＰは、ロボット１の全体の運動によって発生する慣性力とロボット１に作用する重力との合力によって、該ＺＭＰの周りに発生するモーメントの水平方向成分（水平軸周りの成分）がゼロとなる床面上の点である。なお、「床面」は、通常の意味での床面（屋内の床面）に限らず、地面、路面等の接地面（移動接地部を接地させ得る面）も含む。
【００５８】
以降、脚リンク機構３Ｒ，３Ｌの各足部１８の空中移動及びこれに続く着地を繰り返す２脚歩容の動作によってロボット１の移動を行う場合を一例として、制御装置４１の具体的な処理を説明する。
【００５９】
上記２脚歩容は、人の歩行動作と同様の形態で２つの脚としての脚リンク機構３Ｒ，３Ｌの足部１８Ｒ，１８Ｌを動かすことでロボット１の移動を行う歩容である。より詳しくは、該２脚歩容は、脚リンク機構３Ｒ，３Ｌの一方の脚リンク機構（支持脚としての脚リンク機構）の足部１８を接地状態（床面から接触反力を受けた状態）として、他方の脚リンク機構（遊脚としての脚リンク機構）の足部１８の空中移動及びそれに続く着地を行う片脚支持期と、両方の脚リンク機構３Ｒ，３Ｌの足部１８Ｒ，１８Ｌが接地状態となる両脚支持期とが交互に繰り返される歩容である。この場合、片脚支持期での支持脚たる脚リンク機構及び遊脚たる脚リンク機構は、片脚支持期の繰り返し毎に、右側の脚リンク機構３Ｒと左側の脚リンク機構３Ｌとで交互に切替えられる。また、足部１８Ｒ，１８Ｌのそれぞれが前記移動接地部に相当する。
【００６０】
なお、以降の説明では、ロボット１の足部１８等の任意の部位の位置及び姿勢、あるいは、ＺＭＰもしくはロボット１の重心の位置を記述するグローバル座標系として、Ｘ軸及びＹ軸を２つの水平軸、Ｚ軸を鉛直軸とする３軸直交座標系を用いる。この場合、図１に示すように、Ｘ軸は、ロボット１の前後方向の水平軸、Ｙ軸は、ロボット１の左右方向の水平軸とする。
【００６１】
制御装置４１の目標歩容生成部４２は、ロボット１の移動開始前、あるいは、移動途中の適宜のタイミング（例えば、所定歩数の移動毎のタイミング）で、着地目標設定部５１の処理を実行する。該着地目標設定部５１は、Ｎ歩目（Ｎ：２以上の整数）までの１歩毎の遊脚側の脚リンク機構３の足部１８の目標着地位置姿勢及び目標着地時刻を設定する。
【００６２】
この着地目標設定部５１が設定する目標着地位置姿勢は、本発明における基準の目標着地位置及び目標着地姿勢に相当するものである。
【００６３】
この場合、着地目標設定部５１は、Ｎ歩目までの両脚支持期及び片脚支持期のそれぞれの期間（又は両脚支持期及び片脚支持期の間の切り替わり時刻）も併せて設定する。
【００６４】
なお、「１歩」は、いずれか一方の脚リンク機構３（３Ｒ又は３Ｌ）の足部１８の空中移動とそれに続く着地とを行う動作を意味する。
【００６５】
上記目標着地位置姿勢及び目標着地時刻、並びに、両脚支持期及び片脚支持期のそれぞれの期間は、基本的には、ロボット１の移動計画もしくは移動指令により規定される該ロボット１の移動経路及び移動速度等の移動パターンの要求に応じて設定される。
【００６６】
なお、本実施形態では、着地目標設定部５１が設定する目標着地位置姿勢のうちの目標着地位置は、目標ＺＭＰ軌道生成部５２により後述する如く決定される着地位置修正量により適宜修正される。
【００６７】
図４中の左寄りの図は、例えば１歩目の遊脚及び支持脚をそれぞれ左側の脚リンク機構３Ｌ、右側の脚リンク機構３Ｒとし、２歩目の遊脚及び支持脚をそれぞれ右側の脚リンク機構３Ｒ、左側の脚リンク機構３Ｌとして、２歩目までの足部１８の目標着地位置姿勢（上方から見た目標着地位置姿勢）を設定した状態を例示している。この場合、実線で示す足部１８Ｌ(1)が１歩目の遊脚側の足部１８Ｌの目標着地位置姿勢、実線で示す足部１８Ｒ(2)が２歩目の遊脚側の足部１８Ｒの目標着地位置姿勢を例示している。
【００６８】
なお、ロボット１の動作環境の床面形状が事前に判明している場合には、着地目標設定部５１で設定する目標着地位置姿勢及び目標着地時刻、並びに両脚支持期及び片脚支持期のそれぞれの期間は、ロボット１の移動開始前に、あらかじめ作成されたものであってもよい。
【００６９】
また、ロボット１に搭載された外界センサ等によりロボット１の周辺の床面形状を計測しながらロボット１の移動を行う場合には、着地目標設定部５１で設定する目標着地位置姿勢及び目標着地時刻、並びに両脚支持期及び片脚支持期のそれぞれの期間は、随時（例えばロボット１の１歩もしくは複数歩の移動毎に、もしくは所定時間毎に）、更新するようにしてもよい。
【００７０】
次に、制御装置４１の目標歩容生成部４２は、着地目標設定部５１で設定された目標着地位置姿勢及び目標着地時刻、並びに、両脚支持期及び片脚支持期のそれぞれの期間を基に、目標ＺＭＰ軌道を生成する処理を目標ＺＭＰ軌道生成部５２により実行する。
【００７１】
ここで、本実施形態では、制御装置４１の目標歩容生成部４２は、例えばロボット１の１歩の移動毎に、将来の所定期間（例えば２歩分の期間を含む所定期間）におけるロボット１の目標歩容（各足部１８の目標位置姿勢の軌道等）を生成する。
【００７２】
このため、目標ＺＭＰ軌道生成部５２は、ロボット１の１歩の移動毎に、上記所定期間における目標ＺＭＰ軌道を生成する。
【００７３】
この処理の詳細は後述するが、目標ＺＭＰ軌道生成部５２は、目標ＺＭＰ軌道を、所定の制約条件下で、複数次数の多項式関数（時間の関数）により表される軌道として生成する。この場合、目標ＺＭＰ軌道生成部５２の処理では、目標ＺＭＰ軌道の生成と併せて、着地目標設定部５１で設定された目標着地位置姿勢のうちの目標着地位置を適宜修正する着地位置修正量を決定することも行われる。
【００７４】
次に制御装置４１の目標歩容生成部４２は、ロボット１の動力学モデルを用いて、目標ＺＭＰ軌道を満足するように、将来の所定期間におけるロボット１の目標重心軌道を生成する処理を目標重心軌道生成部５３により実行する。この場合、ロボット１の動力学モデルとしては、例えば、ロボット１の重心の運動を、目標ＺＭＰを支点として一定の高さで揺動する倒立振子質点の運動によって表現する倒立振子モデルが用いられる。
【００７５】
そして、目標重心軌道生成部５３は、倒立振子質点の水平方向の運動加速度を、目標ＺＭＰを満足するように（詳しくは、倒立振子質点に作用する重力と、倒立振子質点の運動によって発生する慣性力との合力によって目標ＺＭＰ周りに発生するモーメントの水平軸周り方向の成分がゼロになるように）算出し、さらに該運動加速度を２階積分することで算出される倒立振子質点の水平位置（Ｘ軸方向位置及びＹ軸方向位置）の軌道を、ロボット１の目標重心軌道として算出する。
【００７６】
次に制御装置４１の目標歩容生成部４２は、将来の所定期間におけるロボット１の各関節の目標関節変位量の軌道を生成する処理を目標関節変位量軌道生成部５４により実行する。この場合、目標関節変位量軌道生成部５４は、例えば、分解運動量制御の手法を用いて目標関節変位量の軌道（瞬時値の時系列）を生成する。
【００７７】
この分解運動量制御の手法では、目標関節変位量軌道生成部５４は、ロボット１の目標重心軌道に応じてロボット１の全体の目標並進運動量（＝重心の目標並進運動量）の軌道を設定する。さらに、目標関節変位量軌道生成部５４は、ロボット１の重心の周りの（又は目標ＺＭＰ周りの）全体の目標角運動量の軌道を適宜設定する。
【００７８】
また、目標関節変位量軌道生成部５４は、前記着地目標設定部５１で設定された目標着地位置姿勢を目標ＺＭＰ軌道生成部５２で決定された着地位置修正量で修正してなる目標着地位置姿勢と、前記着地目標設定部５１で設定した目標着地時刻、両脚支持期の期間、及び片脚支持期間の期間を基に、移動接地部たる各足部１８の目標位置姿勢軌道を生成する。
【００７９】
また、目標関節変位量軌道生成部５４は、腕リンク機構４Ｒ，４Ｌのそれぞれの先端部たる各ハンド部３０Ｒ，３０Ｌの目標位置姿勢の軌道を適宜設定する。
【００８０】
なお、目標角運動量と、各ハンド部３０Ｒ，３０Ｌの目標位置姿勢の軌道とは、任意に設計的に設定し得る。
【００８１】
そして、目標関節変位量軌道生成部５４は、上記目標並進運動量の軌道と、目標角運動量の軌道と、各足部１８Ｒ，１８Ｌの目標位置姿勢の軌道と、各ハンド部３０Ｒ，３０Ｌの目標位置姿勢の軌道とから、目標並進運動量及び目標角運動量を実現し得る各関節の目標関節変位量の軌道（所定の制御処理周期毎の瞬時値の時系列）を分解運動量制御の手法により算出する。
【００８２】
なお、目標ＺＭＰ軌道を用いて目標関節変位量の軌道を生成する手法は、上記の手法に限らず、種々様々な公知の手法を採用し得る。
【００８３】
以上の如く、本実施形態では、ロボット１の目標歩容生成部４２は、将来の各関節の目標関節変位量の軌道を生成する。
【００８４】
さらに、制御装置４１は、安定化制御部４３の処理を実行することで、目標歩容生成部４２で生成された目標関節変位量の軌道を適宜修正する。この場合、安定化制御部４３は、例えば、ロボット１の下側基体１０に搭載されている傾斜センサ（図示省略）により検出された該下側基体１０の実際の傾斜量（鉛直方向に対する傾斜角）の検出値と、目標歩容生成部４２で生成された下側基体１０の目標姿勢より規定される傾斜量との偏差に応じて、該偏差をゼロに近づけるように、ロボット１の支持脚としての脚リンク機構３の足部１８（詳しくは、片脚支持期では、支持脚側の片方の足部１８Ｌ又は１８Ｒ、両脚支持期では、両方の足部１８Ｌ，１８Ｒ）の目標位置姿勢の修正量を決定する。そして、安定化制御部４３は、当該修正量に応じて、ロボット１の支持脚としての脚リンク機構３の目標関節変位量を修正する。
【００８５】
そして、制御装置４１は、かかる安定化制御部４３による修正処理を施した目標関節変位量の軌道に応じてロボット１の各関節の変位量を制御する処理を関節アクチュエータ制御部４４により実行する。
【００８６】
この場合、関節アクチュエータ制御部４４は、ロータリエンコーダ等により構成される各関節の変位量センサ（図示省略）で検出される各関節の実際の変位量（回転角）を、該関節の目標関節変位量（安定化制御部４３により修正された目標関節変位量については、その修正後の目標関節変位量）に追従させるように、関節アクチュエータ４０をフィードバック制御する。
【００８７】
なお、この場合、所定の制御処理周期毎の目標関節変位量を、目標関節変位量軌道生成部５４で生成された目標値（又は安定化制御部４３による修正後の目標値）からコンプライアンス制御等により適宜修正するようにしてもよい。
【００８８】
補足すると、ロボット１の目標歩容（目標関節変位量の軌道等）の生成処理は、ロボット１の１歩、又は複数歩の移動毎に行ったり、あるいは、１歩分の期間よりも短い時間間隔で行うようにしてもよい。また、ロボット１の目標歩容（目標関節変位量の軌道等）の生成処理を実行する各タイミングにおいて、２歩目までの期間よりも短い期間（例えば１歩目までの期間）における目標歩容を生成したり、あるいは、２歩目までの期間よりも長い期間（例えば３歩目までの期間）における目標歩容を生成するようにしてもよい。
【００８９】
次に、説明を後回しにした前記目標ＺＭＰ軌道生成部５２の処理を詳説する。目標ＺＭＰ軌道生成部５２は、本発明の目標ＺＭＰ軌道の生成装置の一例としての機能を制御装置４１に付与するものである。該目標ＺＭＰ軌道生成部５２は、目標ＺＭＰ軌道を表わす多項式関数の各項の係数と、目標着地位置を修正するための着地位置修正量を表す変数とから成る複数の未知変数の値を決定する変数目標値決定部５２ａとしての機能を含む。
【００９０】
本実施形態では、目標ＺＭＰ軌道生成部５２は、目標歩容生成部４２が目標歩容（目標関節変位量の軌道等）を生成する処理を実行する各タイミングにおいて目標ＺＭＰ軌道を新たに生成しようとする所定期間としての軌道生成対象期間を、図６に例示するように複数の区間ｉ（ｉ＝０，１，…，ｅ）に分割する。
【００９１】
なお、図３での目標ＺＭＰ軌道は、一例として、ロボット１の直進歩行を行う場合における目標ＺＭＰのＹ軸方向位置の軌道を例示している。また、図中の足部１８Ｒは、右側の脚リンク機構３Ｒを遊脚として足部１８Ｒを動かす場合における該足部１８ＲのＹ軸方向の目標着地位置を示し、図中の足部１８Ｌは、脚リンク機構３Ｌを遊脚として足部１８Ｌを動かす場合における該足部１８ＬのＹ軸方向の目標着地位置を示している。
【００９２】
図示例では、例えば、各区間ｉが、片脚支持期および両脚支持期のいずれか一方の期間になるように、軌道生成対象期間が分割されている。ただし、軌道生成対象期間の分割の仕方は、上記と異なっていてもよい。例えば、軌道生成対象期間をさらに多くの区間に分割してもよい。また、例えば各区間ｉの時間幅Ｔ_iが一定となるように、軌道生成対象期間を分割してもよい。
【００９３】
目標ＺＭＰ軌道生成部５２の処理では、軌道生成対象期間を分割してなる各区間ｉの目標ＺＭＰ軌道（詳しくは、グローバル座標系における目標ＺＭＰのＸ軸方向位置Xzmp及びＹ軸方向位置Yzmpから成るベクトル↑Pzmp＝[Xzmp, Yzmp]Tの各成分の軌道）が、各区間ｉ毎に個別の多項式関数により表現される。なお、目標ＺＭＰのＺ軸方向位置は、Ｘ軸方向位置及びＹ軸方向位置がそれぞれXzmp、Yzmpとなる床面上の点のＺ軸方向位置として規定される。
【００９４】
なお、本明細書では、任意の変数の先頭の「↑」は、当該変数がベクトル（縦ベクトル）であることを示す符号、上付きの添え字「Ｔ」はベクトル又は行列の転置を表す符号である。
【００９５】
各区間ｉ（ｉ＝０，１，…，ｅ）における目標ＺＭＰ軌道（↑Pzmpの軌道）は、時間ｔの関数として、次式（１）の形の多項式関数で定義される。
【００９６】
【数１】
2019042879.tif 000003
【００９７】
式（１）の左辺の↑Pzmp_i(t)（＝[Xzmp_i(t), Yzmp_i(t)]T）は、区間ｉにおける時刻ｔでの目標ＺＭＰの位置（Ｘ軸方向位置Xzmp_i(t)及びＹ軸方向位置Yzmp_i(t)）を示す２成分のベクトル、式（１）の右辺のｎは、↑Pzmp_i(t)（以降、目標ＺＭＰベクトル↑Pzmp_i(t)ということがある）の各成分Xzmp_i(t)，Yzmp_i(t)を表す多項式関数の次数、↑Ｗ_i,j（＝[Wx_i,j , Wy_i,j]T）（ｊ＝０，１，…，ｎ）は、Xzmp_i(t)を表す多項式関数の各項のうちの次数ｊの項の係数Wx_i,jと、Yzmp_i(t)を表す多項式関数の各項のうちの次数ｊの項の係数Wy_i,jとから成るベクトルである。
【００９８】
なお、本実施形態では、目標ＺＭＰベクトル↑Pzmp_i(t)の各成分Xzmp_i(t)，Yzmp_i(t)を表す多項式関数の次数ｎは、例えば“９”である。
【００９９】
また、式（１）の右辺のtc_i(t)は、区間ｉの正規化した時刻を表すものとして、式（１ａ），（１ｂ）により定義されるものである。これらの式（１ａ），（１ｂ）におけるt_iは、区間ｉの始端時刻、t_i+1は、区間ｉの終端時刻（＝区間ｉ＋１の始端時刻）、T_iは、区間ｉの時間幅である。
【０１００】
従って、式（１ａ），（１ｂ）により定義される正規化時刻tc_i(t)は、各区間ｉの始端時刻t_iを“０”、時間幅T_iを“１”とみなして正規化した時刻である。
【０１０１】
軌道生成対象期間の各区間ｉにおける目標ＺＭＰベクトル↑Pzmp_i(t)の各成分Xzmp_i(t)，Yzmp_i(t)を上記の如く多項式関数で表した場合、軌道生成対象期間における目標ＺＭＰ軌道（↑Pzmp_0(t)〜↑Pzmp_e(t)の全体の軌道）は、軌道対象生成期間の全ての区間ｉ（ｉ＝０，１，…，ｅ）のそれぞれにおける多項式関数の各項の係数Wx_i,j、Wy_i,j（ｊ＝０，１，…，ｎ）の値により規定される。
【０１０２】
そこで、目標ＺＭＰ軌道生成部５２は、軌道生成対象期間を分割してなる各区間ｉにおける目標ＺＭＰベクトル↑Pzmp_i(t)の各成分Xzmp_i(t)，Yzmp_i(t)を表す多項式関数の各項の係数Wx_i,j、Wy_i,jを、目標ＺＭＰ軌道の生成処理における決定対象の変数（未知変数）として取り扱う。以降、多項式関数の係数Wx_i,j、Wy_i,j（ｉ＝０，１，…，ｅ、ｊ＝０，１，…，ｎ）をＺＭＰ軌道変数と称する。
【０１０３】
ところで、ＺＭＰは、所謂、支持多角形内に存在すべき点であるので、２脚歩容での目標ＺＭＰの位置は、例えば図４の右側のグラフａ１，ａ２で例示する如く、各足部１８の目標着地位置姿勢の影響を受ける。
【０１０４】
ここで、図４の実線のグラフａ１は、着地目標設定部５１で設定された１歩目及び２歩目の目標着地位置姿勢が、それぞれ、図４の左側に実線で示す足部１８Ｌ(1)，１８Ｒ(2)の位置姿勢であるとした場合（第１の場合）の適切な目標ＺＭＰ軌道（目標ＺＭＰのＸ軸方向位置の軌道）を例示するグラフ、図４の二点鎖線のグラフａ２は、１歩目及び２歩目の目標着地位置姿勢のうち、例えば１歩目の目標着地位置だけを、第１の場合から二点鎖線で示す足部１８Ｌ(1)の位置に変化させた場合（第２の場合）の適切な目標ＺＭＰ軌道（目標ＺＭＰのＸ軸方向位置の軌道）を例示するグラフである。なお、グラフａ１，ａ２では、便宜上、各区間ｉ（ｉ＝０，１，２，…）における目標ＺＭＰ軌道を、一次関数により表される軌道としている。
【０１０５】
図４のグラフａ１，ａ２で例示する如く、例えば１歩目での遊脚としての脚リンク機構３Ｌの足部１８Ｌ(1)の目標着地位置を変化させると、適切な目標ＺＭＰ軌道は、該足部１８Ｌ(1)が接地状態となる片脚支持期の期間（図４では区間３）とその前後の両脚支持期の期間（図４では区間２，４）とで変化する。この場合、該足部１８Ｌ(1)が接地状態となる片脚支持期の期間での目標ＺＭＰ軌道の変化量は、該足部１８Ｌ(1)の目標着地位置の変化量と同程度になるとみなし得る。
【０１０６】
なお、例えば２歩目での遊脚としての脚リンク機構３Ｒの足部１８Ｒ(2)の目標着地位置を変化させた場合には、適切な目標ＺＭＰ軌道は、該足部１８Ｒ(2)が接地状態となる片脚支持期の期間（図４では区間５）とその前後の両脚支持期の期間とで変化する。
【０１０７】
このように目標ＺＭＰ軌道は、各足部１８の目標着地位置姿勢の影響を受ける。そこで、本実施形態では、前記ＺＭＰ軌道変数Wx_i,j、Wy_i,jに加えて、前記軌道生成対象期間での所定の歩数目の目標着地位置を、着地目標設定部５１で設定される目標着地位置から適宜修正し得る修正対象の目標着地位置とする。
【０１０８】
そして、本実施形態では、前記ＺＭＰ軌道変数Wx_i,j、Wy_i,jに加えて、当該修正対象の歩数目の目標着地位置の修正量（以降、着地位置修正量という）、又は該着地位置修正量と一定の関係を有するパラメータをも、目標ＺＭＰ軌道の生成処理における決定対象の変数（未知変数）として取り扱う。なお、上記着地位置修正量は、着地目標設定部５１で設定される目標着地位置からの修正量である。
【０１０９】
以降、ｋ歩目（ｋ＝０，１，２，…）の着地位置修正量を↑ΔＰft(k)＝[ΔＰftx(k), ΔＰfty(k)]T（ただし、ΔＰftx(k)：Ｘ軸方向の着地位置修正量、ΔＰfty(k)：ｙ軸方向の着地位置修正量）と表記する。そして、本実施形態では、着地位置修正量↑ΔＰft_kは、適当な行列Ｓと変数ベクトル↑Ｗft(k)とを用いて、次式（２）の形で表されるものと定義する。
【０１１０】
【数２】
2019042879.tif 000004
【０１１１】
ここで、本実施形態では、後述する評価関数（コスト関数）にＬ１ノルムを導入するために、上記行列Ｓｍとして、上記式（２ａ）に示す２行４列の行列を用いると共に、上記変数ベクトル↑Ｗft(k)として、上記式（２ｂ）により示される４成分のベクトルを用いる。
【０１１２】
この場合、着地位置修正量↑ΔＰft(k)の成分と変数ベクトル↑Ｗft(k)の成分との関係は、上記式（２ｃ）により表される。以降、変数ベクトル↑Ｗft(k)を着地位置修正変数ベクトル、↑Ｗft(k)の成分Ｗftxp(k)，Ｗftxn(k)，Ｗftyp(k)，Ｗftyn(k)を着地位置修正変数と称する。該着地位置修正変数Ｗftxp(k)，Ｗftxn(k)，Ｗftyp(k)，Ｗftyn(k)は、本発明におけるパラメータに相当するものであり、着地位置修正量↑ΔＰft_kと、式（２）より表される線形関係を有する。
【０１１３】
本実施形態では、前記ＺＭＰ軌道変数Wx_i,j、Wy_i,j（ｉ＝０，１，…，ｅ、ｊ＝０，１，…，ｎ）と、ｋ歩目の着地位置修正量↑ΔＰft(k)に対応する上記着地位置修正変数Ｗftxp(k)，Ｗftxn(k)，Ｗftyp(k)，Ｗftyn(k)とを、目標ＺＭＰ軌道の生成処理における決定対象の変数（未知変数）とする。
【０１１４】
そして、目標ＺＭＰ軌道生成部５２は、ＺＭＰ軌道変数Wx_i,j、Wy_i,j及び着地位置修正変数Ｗftxp(k)，Ｗftxn(k)，Ｗftyp(k)，Ｗftyn(k)から成る決定対象の複数の未知変数を、当該未知変数に関する評価関数（コスト関数）と、当該未知変数に関する線形等式又は線形不等式により表される複数の線形制約条件とを用いて、二次計画問題の公知の解法手法のアルゴリズムにより決定する。
【０１１５】
すなわち、目標ＺＭＰ軌道生成部５２は、上記複数の線形制約条件の基で、上記評価関数の値を最小化するようにＺＭＰ軌道変数Wx_i,j、Wy_i,j及び着地位置修正変数Ｗftxp(k)，Ｗftxn(k)，Ｗftyp(k)，Ｗftyn(k)の全体を決定する処理を、二次計画問題の公知の解法手法のアルゴリズムにより実行する。
【０１１６】
この場合、ＺＭＰ軌道変数Wx_i,j、Wy_i,j及び着地位置修正変数Ｗftxp(k)，Ｗftxn(k)，Ｗftyp(k)，Ｗftyn(k)に関する線形制約条件として、本実施形態では、前記倒立振子モデルにおける倒立振子質点としてのロボット１の重心を継続的に安定状態に保つための制約条件（以降、安定化条件）と、目標ＺＭＰ軌道の経由点を規定する制約条件（以降、経由点条件という）と、軌道生成対象期間のうちの隣り合う区間ｉ−１，ｉ（ｉ＝１，２，…，ｅ）の境界での目標ＺＭＰ軌道の接続に関する制約条件（以降、軌道接続条件）と、目標ＺＭＰの位置を支持多角形内の位置に制限するための制約条件（以降、ＺＭＰ存在許容領域条件という）と、ロボット１の支持脚の足部１８に対する遊脚の足部１８の着地位置を幾何学的に制限するための制約条件（以降、幾何学条件という）とが用いられる。
【０１１７】
上記安定化条件、経由点条件及び軌道接続条件は、線形等式により表される制約条件、ＺＭＰ存在許容領域条件及び幾何学条件は線形不等式により表される制約条件である。線形等式は、ＺＭＰ軌道変数Wx_i,j、Wy_i,j及び着地位置修正変数Ｗftxp(k)，Ｗftxn(k)，Ｗftyp(k)，Ｗftyn(k)のうちの複数の変数の線形結合値がある定数値に等しいという関係を表す等式であり、線形不等式は、当該複数の変数の線形結合値がある定数値よりも大きい又は小さいという関係を表す不等式である。
【０１１８】
なお、安定化条件、経由点条件、及びＺＭＰ存在許容領域条件は、それぞれ本発明における第２制約条件、第３制約条件、第１制約条件に相当する。
【０１１９】
以下にこれらの制約条件を表す線形等式又は線形不等式を具体的に説明する。なお、本実施形態の以下の説明では、目着地位置修正量↑ΔＰft(k)による修正対象の目標着地位置は、例えば１歩目の目標着地位置である。
【０１２０】
まず、前記安定化条件は、本実施形態では、軌道生成対象期間（２歩分の期間）の終端時刻における倒立振子モデルの発散成分（以降、終端発散成分という）の値が、軌道生成対象期間でのロボット１の歩容に続く仮想的な歩容としての定常歩容の始端時刻（＝軌道生成対象期間の終端時刻）における倒立振子モデルの発散成分（以降、定常歩容初期発散成分という）の値に一致するという条件（以降、発散成分条件という）として表現される制約条件である。
【０１２１】
なお、上記定常歩容及び発散成分については、例えば前記特許文献１等に詳細に説明されているものである。このため、本実施形態では、簡略的な説明に留める。
【０１２２】
定常歩容は、軌道生成対象期間でのロボット１の歩容（本実施形態では２歩目までの歩容）に続く歩容として仮想的に想定される歩容であって、各移動接地部及び基体２を含むロボット１の各部の運動が一定のパターンで周期的に繰り返されるようにしてロボット１の移動を継続的に行い得る歩容である。
【０１２３】
本実施形態において、２脚歩容の動作でロボット１の移動を行う場合における当該定常歩容は、例えば図４に示す如く、軌道生成対象期間におけるロボット１の２歩目の動作に続く周期的な歩容（２歩分の期間を１周期とする周期的な歩容）である。なお、図４では、定常歩容の各歩における足部１８の着地位置姿勢を破線で示している。
【０１２４】
また、任意の時刻ｔでの倒立振子モデルの発散成分は、次式（３）で示すように、ロボット１の重心としての倒立振子質点の位置↑Ppendと、該倒立振子質点の移動速度d↑Ppend(t)/dt（↑Ppendの時間的変化率）との線形結合値として定義される値↑ｑ(t)（Ｘ軸方向成分ｑx(t)とＹ軸方向成分ｑy(t)とからなるベクトル）である。
【０１２５】
↑ｑ(t)＝[ｑx(t)，ｑy(t)]T
＝↑Ppend(t)＋(1／ω)・d↑Ppend(t)/dt    ……（３）
ただし、ω＝√(ｇ／ｈ)
【０１２６】
なお、ｇは、重力加速度定数、ｈは倒立振子質点の高さ（一定値）、√(ｇ／ｈ)は、(ｇ／ｈ)の平方根である。
【０１２７】
このように定義される発散成分に関して、前記発散成分条件を満たすように目標ＺＭＰ軌道を設定することによって、軌道生成対象期間の後に続く将来において、定常歩容でのロボット１の運動状態に収束させることができるように（ひいては、ロボット１の重心の安定性を保ち得るように）、軌道生成対象期間でのロボット１の目標歩容を決定することができる。
【０１２８】
そこで、本実施形態では、上記発散成分条件を、目標ＺＭＰ軌道の生成処理における１つの制約条件（安定化条件）として用いる。なお、上記発散成分は、本発明における第２制約条件に係る倒立振子質点の運動状態に相当するものである。
【０１２９】
ここで、１歩目の目標着地位置を着地位置修正量ΔＰft(1)だけ変化させることに応じて生じる発散成分の変化量（詳しくは、軌道生成対象期間の終端時刻t_e+1での発散成分の値の変化量）を↑Δｑft(1)、軌道生成対象期間の終端時刻t_e+1での発散成分の目標値（定常歩容の始端時刻での発散成分の値）を↑ｑtarget、軌道生成対象期間の始端時刻ｔ_0での発散成分の値を↑ｑ(t_0)とおくと、上記安定化条件は、次式（４）により表される。
【０１３０】
↑ｑzmp(t_e+1)＋↑Δｑft(1)
＝exp(−ω・(t_e+1))・↑ｑtarget−↑ｑ(t_0)  ……（４）
【０１３１】
この場合、上記exp(  )は、自然対数の底ｅの指数関数である。また、式（４）の左辺の↑ｑzmp(t_e+1)は、次式（５）により与えられ、↑Δｑft(1)は、次式（６）により与えられる。
【０１３２】
【数３】
2019042879.tif 000005
【０１３３】
【数４】
2019042879.tif 000006
【０１３４】
なお、ｊ!、ｋ!、ｍ!は、それぞれｊの階乗、ｋの階乗、ｍの階乗である。
【０１３５】
上記式（５），（６）を式（４）に代入することで、発散成分条件（安定化条件）は、次式（７）により表される。
【０１３６】
【数５】
2019042879.tif 000007
【０１３７】
この式（７）の左辺のベクトル（２成分のベクトル）の第１成分はＺＭＰ軌道変数Ｗx_i,j（ｉ＝０，１，…，ｅ、ｊ＝０，１，…，ｎ）と、着地位置修正変数Ｗftxp(1)，Ｗftxn(1)との線形結合、式（７）の左辺のベクトルの第２成分はＺＭＰ軌道変数Ｗy_i,j（ｉ＝０，１，…，ｅ、ｊ＝０，１，…，ｎ）と、着地位置修正変数Ｗftyp(1)，Ｗftyn(1)との線形結合である。
【０１３８】
そして、式（７）の右辺のベクトル[C1x, C1y]Tの第１成分C1x及び第２成分C1yは、定数である。従って、安定化条件（発散成分条件）は、式（７）の線形等式より表される。
【０１３９】
そこで、本実施形態では、目標ＺＭＰ軌道生成部５２は、上記式（７）を安定化条件を表す線形等式として設定する。この場合、定数C1x，C1yは、前記特許文献２に記載されたものと同様に算出される。
【０１４０】
なお、↑ｑzmp(t_e+1)は、発散成分のうちの、着地目標設定部５１で設定された基準の目標着地位置及び目標着地姿勢に応じた成分であり、↑Δｑft(1)は、着地位置修正変数Ｗftxp(1)，Ｗftxn(1)，Ｗftyp(1)，Ｗftyn(1)に応じた成分である。
【０１４１】
次に、前記経由点条件は、より詳しく言えば、軌道生成対象期間での目標ＺＭＰ軌道が、あらかじめ定められた所定の時刻tfにおいて、所定位置の経由点を通るという条件である。
【０１４２】
本実施形態では、経由点の位置は、図５に例示する如く、接地状態の足部１８の接地面内の位置である。そして、該足部１８に対する経由点の相対位置は、グローバル座標系での該足部１８の位置によらずに一定である。
【０１４３】
このため、図５に示すように、ある時刻での経由点が目標着地位置の修正対象とする１歩目の足部１８（図５では足部１８Ｌ）の着地後の接地面内の点（図５では、例えば点Ｐ１，Ｐ２）である場合、該足部１８Ｌの目標着地位置を↑ΔＰft(1)（＝Ｓm・↑Ｗft(1)）だけ修正すると、経由点も↑ΔＰft(1)だけ動く。
【０１４４】
従って、軌道生成対象期間のうち、１歩目の足部１８が接地状態となる区間（前記区間２，３，４のいずれかの区間）内の時刻ｔfでの経由点の位置（↑ΔＰft(1)をゼロベクトルとした場合の当該１歩目の足部１８の接地面内の位置）を↑Pvia(tf)（＝[Xvia(tf), Yvia(tf)]T）とおき、時刻ｔfが属する区間を区間ｉfとおくと、該経由点に関する経由点条件は、次式（８）により表される。
【０１４５】
【数６】
2019042879.tif 000008
【０１４６】
そして、この式（８）を整理すると、次式（９）の線形等式が得られる。
【０１４７】
【数７】
2019042879.tif 000009
【０１４８】
そこで、目標ＺＭＰ軌道生成部５２は、各経由点に対応する位置↑Pvia(tf)の値（定数値）と、時刻tfとを用いて、上記式（９）を、経由点条件を表す線形等式として設定する。
【０１４９】
なお、経由点が目標着地位置の修正対象でない足部１８（本実施形態では２歩目で着地させる足部１８、あるいは、１歩目で支持脚となる脚リンク機構３の足部（図５では足部１８Ｒ）の接地面内の点である場合には、該経由点に関する経由点条件は、式（９）の左辺の第２項（↑Ｗft(1)を含む項）を除いた線形等式により表される。
【０１５０】
次に、ＺＭＰ存在許容領域条件は、より詳しくは、軌道生成対象期間の任意の時刻での目標ＺＭＰが、当該時刻でのロボット１の接地面を包含する最小の凸領域に一致もしくは近似する支持多角形内に存在するという条件である。
【０１５１】
なお、上記凸領域は、片脚支持期では、支持脚としての脚リンク機構３の足部１８の接地面により形成される領域であり、両脚支持期では、両方の脚リンク機構３Ｒ，３Ｌのそれぞれの足部１８Ｒ，１８Ｌの接地面の両方を含む領域である。
【０１５２】
本実施形態では、脚リンク機構３Ｒ，３Ｌのうちの一方の脚リンク機構３の足部１８だけが接地する片脚支持期での支持多角形は、例えば図６Ａで示すように、四角形で表し、両方の脚リンク機構３Ｒ，３Ｌの足部１８Ｒ，１８Ｌが接地する両脚支持期での支持多角形は、例えば図６Ｂで示すように六角形で表す。
【０１５３】
ここで、図６Ａの点描領域は、片脚支持期での足部１８（図示例では１８Ｒ）の接地面の一例を表し、図７Ｂの点描領域は、両脚支持期での足部１８Ｒ，１８Ｌのそれぞれの接地面の一例を表している。これらの図示例では、支持多角形は、上記最小の凸領域に一致もしくはほぼ一致している。ただし、該支持多角形は、該凸領域の内側の多角形領域であってもよい。
【０１５４】
かかる支持多角形の各辺は、グローバル座標系のＸＹ座標平面に投影して見たとき、次式（１０）により表される。
【０１５５】
Cx_m・ｘ＋Cy_m・ｙ＝d_m  ……（１０）
ただし、ｍ＝１，２，…，Ｍ
【０１５６】
なお、添え字ｍは、支持多角形の各辺の番数、Ｍは支持多角形の辺の総数である。また、Cx_m，Cy_m，d_mは、番数ｍの辺に対応して定まる定数である。これらの定数Cx_m，Cy_m，d_mの値は、各辺を含む直線上の２つの点（例えば、支持多角形の角の点）の位置から決定できる。
【０１５７】
そして、軌道生成対象期間のうち、支持多角形が、目標着地位置の修正対象の足部１８の接地面を含まない期間（本実施形態では、前記区間０，１の期間）では、上記ＺＭＰ存在許容領域条件は、支持多角形の各辺にそれぞれ対応するＭ個の不等式（１１−ｍ）（ｍ＝１，２，…，Ｍ）の組により表される。
【０１５８】
Cx_m・Xzmp＋Cy_m・Yzmp＜ｄ_m  ……（１１−m）
ただし、ｍ＝１，２，…，Ｍ
【０１５９】
ここで、軌道生成対象期間の各区間ｉにおける目標ＺＭＰベクトル↑Pzmp_i＝[Xzmp_i，Yzmp_i]Tは、前記式（２）により表される。従って、軌道生成対象期間のうち、支持多角形が、目標着地位置の修正対象の足部１８の接地面を含まない任意の区間ｉの任意の時刻ｔ（正規化時刻では、時刻tc_i(t)）おけるＺＭＰ存在許容領域条件は、次のＭ個の線形不等式（１２−ｍ）（ｍ＝１，２，…，Ｍ）により表現される。
【０１６０】
【数８】
2019042879.tif 000010
【０１６１】
一方、軌道生成対象期間のうち、支持多角形が、目標着地位置の修正対象の足部１８（１歩目の足部１８）の接地面を含む期間（本実施形態では、前記区間２〜５の期間）では、上記ＺＭＰ存在許容領域条件は、次のように設定される。
【０１６２】
本実施形態では、目標着地位置の修正対象の足部１８（１歩目の足部１８）の着地直後の両脚支持期の期間（前記区間２の期間）では、該両脚支持期の開始時刻での支持多角形が、１歩目の足部１８（図７では１８Ｌ）の目標着地位置の修正が無い状態（↑ΔＰft(1)＝↑０である場合）の支持多角形（図７に実線で示す支持多角形）に近似的に一致すると共に、該両脚支持期の終了時刻での支持多角形が、該両脚支持期の開始時刻での支持多角形を、１歩目の足部１８（１８Ｌ）の着地位置修正量↑ΔＰft(1)だけ平行移動した支持多角形（図７に二点鎖線で示す支持多角形）に近似的に一致するとみなす。なお、図７に破線で示す支持多角形は、１歩目の足部１８（１８Ｌ）の目標着地位置の修正が有る状態（↑ΔＰft(1)≠↑０である場合）の実際の支持多角形を示している。
【０１６３】
そして、該両脚支持期の途中の支持多角形は、該両脚支持期の開始時刻での支持多角形から終了時刻での支持多角形まで、一定の移動速度で連続的に平行移動していくものとみなす。なお、図７に破線で示す支持多角形は、１歩目の足部１８の目標着地位置を↑ΔＰft(1)だけ修正した場合の実際の支持多角形（当該両脚支持期での支持多角形）である。
【０１６４】
従って、当該両脚支持期の任意のサンプリング時刻での支持多角形は、当該両脚支持期の開始時から該サンプリング時刻までの時間差の、当該両脚支持期の時間幅に対する比率に応じて、１歩目の足部１８（目標着地位置の修正対象の足部１８）の着地位置修正量を分割してなる変位量だけ、当該両脚支持期の開始時の支持多角形（基準の支持多角形）を平行移動させた支持多角形に近似するものとして設定される。
【０１６５】
このようにした場合、１歩目の足部１８の着地直後の両脚支持期の期間（前記区間２の期間）の開始時における支持多角形の各辺が、前記式（１１）により表されるとすると、該両脚支持期の期間の任意の時刻ｔ（正規化時刻では、時刻tc_i(t)）おけるＺＭＰ存在許容領域条件は、次のＭ個の線形不等式（１３−ｍ）（ｍ＝１，２，…，Ｍ）により表現される。
【０１６６】
【数９】
2019042879.tif 000011
【０１６７】
この場合、式（１３−ｍ）における区間ｉは、１歩目の足部１８の着地直後の両脚支持期の区間（前記区間３）である。
【０１６８】
また、軌道生成対象期間（区間０〜４）のうち、当該両脚支持期（区間３）の次の片脚支持期（区間４）でのＺＭＰ存在許容領域条件は、上記（１３−ｍ）の左辺のｔc_i(t)を“１”とした次式（１４−ｍ）線形不等式により表される。
【０１６９】
【数１０】
2019042879.tif 000012
【０１７０】
本実施形態では、ＺＭＰ軌道生成部５２は、軌道生成対象期間の各区間ｉ毎に、上記式（１２−ｍ）又は式（１３−ｍ）又は式（１４−ｍ）により表される不等式を、ＺＭＰ存在許容領域条件を表す線形不等式として設定する。
【０１７１】
より具体的には、ＺＭＰ軌道生成部５２は、まず、軌道生成対象期間に複数のサンプリング時刻（例えば所定の刻み時間毎の時刻）を設定し、その各サンプリング時刻での支持多角形の各辺の式（式（１１−ｍ））を、該サンプリング時刻が属する区間で接地状態となる足部１８Ｒ，１８Ｌの目標着地位置姿勢（着地目標設定部５１で設定された目標着地位置姿勢）に基づいて設定する。
【０１７２】
そして、ＺＭＰ軌道生成部５２は、前記区間０，１では、各サンプリング時刻毎に、その時刻ｔの値と、該時刻に対応して設定した支持多角形の各辺の式を表す定数Cx_m，Cy_m，d_mの設定値とを用いて、上記式（１２−ｍ）により表されるＭ個の線形不等式を、ＺＭＰ存在許容領域条件を表す線形不等式として設定する。
【０１７３】
また、ＺＭＰ軌道生成部５２は、前記区間２（１歩目の足部１８の着地直後の両脚支持期の期間）では、各サンプリング時刻毎に、その時刻ｔの値と、該時刻に対応して設定した支持多角形の各辺の式を表す定数Cx_m，Cy_m，d_mの設定値とを用いて、上記式（１３−ｍ）により表されるＭ個の線形不等式を、ＺＭＰ存在許容領域条件を表す線形不等式として設定する。
【０１７４】
また、ＺＭＰ軌道生成部５２は、前記区間３（１歩目の足部１８の着地後の片脚支持期）では、各サンプリング時刻毎に、その時刻ｔの値と、該時刻に対応して設定した支持多角形の各辺の式を表す定数Cx_m，Cy_m，d_mの設定値とを用いて、上記式（１４−ｍ）により表されるＭ個の線形不等式を、ＺＭＰ存在許容領域条件を表す線形不等式として設定する。
【０１７５】
なお、本実施形態では、片脚支持期での支持多角形を四角形、両脚支持期での支持多角形を六角形とした。ただし、片脚支持期及び両脚支持期の両方又は一方の支持多角形として、上記と異なる角数の多角形を用いてもよい。
【０１７６】
次に、軌道接続条件は、本実施形態では、より詳しくは、区間ｉ−１（ｉ＝１，２，…，ｅ）での目標ＺＭＰ軌道と区間ｉでの目標ＺＭＰ軌道とが滑らかにつながるという条件である。このような軌道接続条件は、区間ｉ−１での目標ＺＭＰ軌道のうちの該区間ｉ−１の終端時刻（時刻t_i）での目標ＺＭＰの位置と該目標ＺＭＰの位置の所定階数の微分値とのそれぞれが、区間ｉの始端時刻（時刻t_i）での目標ＺＭＰの位置と該目標ＺＭＰの位置の所定階数の微分値とのそれぞれに一致するという条件として表現し得る。
【０１７７】
本実施形態では、上記所定階数の微分値は、一例として、ＺＭＰの移動速度（目標ＺＭＰの位置の１階微分値）と、移動加速度（目標ＺＭＰの位置の２階微分値）との２つである。
【０１７８】
この場合、隣り合う区間ｉ−１，ｉのうちの先行側の区間ｉ−１と後行側の区間ｉとの境界（ｉ＝１，２，…，ｅ）のそれぞれでの目標ＺＭＰの位置、移動速度及び移動加速度についての軌道接続条件は、それぞれ、次式（１５ａ）、（１５ｂ）、（１５ｃ）により表される。
【０１７９】
【数１１】
2019042879.tif 000013
【０１８０】
さらに、区間ｉ−１の終端時刻ｔiに対応する正規化時刻がｔc_i-1(t_i)＝１、区間ｉの始端時刻ｔiに対応する正規化時刻ｔc_i(t_i)＝０であることを考慮すると、式（１５ａ）〜（１５ｃ）は、それぞれ、次式（１６ａ）〜（１６ｃ）に書き換えられる。
【０１８１】
【数１２】
2019042879.tif 000014
【０１８２】
そこで、目標ＺＭＰ軌道生成部５２は、区間ｉ−１と区間ｉとの境界（ｉ＝１，２，…，ｅ）のそれぞれ毎に、各区間ｉのあらかじめ定められた時間幅Ｔ_i（ｉ＝０，１，…，ｅ）の設定値を用いて、上記式（１６ａ），（１６ｂ），（１６ｃ）を軌道接続条件を表す線形等式として設定する。
【０１８３】
なお、軌道接続条件の要素として使用するＺＭＰの位置の所定階数の微分値は、ＺＭＰの移動速度及び移動加速度の両方に限られるものではない。例えば、移動加速度を軌道接続条件の要素から除外したり、あるいは、ＺＭＰの位置の３階微分値を移動接続条件の要素として用いることも可能である。
【０１８４】
次に、幾何学条件は、ロボット１の各脚リンク機構３の膝関節機構１５の伸び切りや、関節アクチュエータ４０のトルク限界など、各脚リンク機構３の関節の動作限界により決まる条件である。本実施形態では、この幾何学条件は、遊脚の足部１８の着地可能な領域（以降、着地許容領域という）を制限する条件として近似される。
【０１８５】
本実施形態では、上記着地許容領域は、図８に示す如く、例えば三角形の領域とされ、支持脚の足部１８（図８では、足部１８Ｒ）に対して所定の位置関係をなるように該着地許容領域の位置（グローバル座標系での位置）が設定される。
【０１８６】
この場合、当該着地許容領域の各辺（３つの辺）は、前記式（１０）と同様の形の次式（１７−ｒ）（ｒ＝１，２，３）により表される。
【０１８７】
C1x_r・ｘ＋C1y_r・ｙ＝d1_r  ……（１７−ｒ）
ただし、ｒ＝１，２，３
【０１８８】
よって、着地位置が該着地許容領域に含まれる条件は次の３つの線形不等式（１８−ｒ）（ｒ＝１，２，３）によって表される。
【０１８９】
C1x_r・(Pftx(k)＋ΔPftx(k))＋C1y_r・(Pfty(k)＋ΔPfty(k))＜d1_r  ……（１８−ｒ）
ただし、
ｒ＝１，２，３
Pftx(k)：修正前の（基準の）目標着地位置（ｋ歩目）のＸ軸方向位置
Pfty(k)：修正前の（基準の）目標着地位置（ｋ歩目）のＹ軸方向位置
【０１９０】
そして、前記式（２ｃ）によりΔPftx(k)＝Ｗftxp(k)−Ｗftxn(k)、ΔPfty＝Ｗftyp(k)−Ｗftyn(k)であるから、式（１８−ｒ）は、次式（１８−ｒ）’に書き換えられる。
【０１９１】
C1x_r・(Ｗftxp(k)−Ｗftxn(k))＋C1y_r・(Ｗftyp(k)−Ｗftyn(k))
＜d1_r−C1x_r・Pftx(k)−C1y_r・Pfty(k)    ……（１８−ｒ）’
【０１９２】
この式（１８−ｒ）’は、着地位置修正変数Ｗftxp(k)，Ｗftxn(k)，Ｗftyp(k)，Ｗftyn(k)に関する線形不等式である。そこで、本実施形態では、ＺＭＰ軌道生成部５２は、軌道生成対象期間の片脚支持期の各区間ｉ（区間１，３）において、上記式（１８−ｒ）’により表される不等式を、幾何学条件を表す線形不等式として設定する。
【０１９３】
より具体的には、ＺＭＰ軌道生成部５２は、片脚支持期の区間１，３のそれぞれにおいて、該片脚支持期に対応して設定した着地許容領域の各辺の式を表す定数C1x_r，C1y_r，d1_r（ｒ＝１，２，３）の設定値と、該片脚支持期に対応して着地目標設定部５１により設定された基準の目標着地位置とを用いて、上記（１８−ｒ）’により表される３個の線形不等式を、幾何学条件を表す線形不等式として設定する。
【０１９４】
なお、本実施形態では、前記着地許容領域を三角形としたが、該着地許容領域は、他の多角形状であってもよい。
【０１９５】
次に、ＺＭＰ軌道変数Wx_i,j、Wy_i,j及び着地位置修正変数Ｗftxp(k)，Ｗftxn(k)，Ｗftyp(k)，Ｗftyn(k)に関する評価関数（コスト関数）を説明する。
【０１９６】
本実施形態では、ＺＭＰ軌道変数Wx_i,j、Wy_i,j及び着地位置修正変数Ｗftxp(k)，Ｗftxn(k)，Ｗftyp(k)，Ｗftyn(k)（本実施形態では、ｋ＝１）から成る変数群を二次計画問題の解法手法のアルゴリズムにより決定するための評価関数（コスト関数）は、その値を最小化するように当該変数群を決定することによって、目標ＺＭＰの急激な変化が生じるのを極力防止することができ、且つ、着地位置修正量↑ΔＰft(k)の大きさを極力小さくすることができるように設定される。
【０１９７】
このため、本実施形態では、目標ＺＭＰベクトル↑Pzmp＝[Xzmp，Yzmp]Tの変化加速度↑Azmp（＝d2↑Pzmp/dt2＝[d2Xzmp/dt2，d2Yzmp/dt2]T）の二乗値(↑Azmp)2（＝↑AzmpT・↑Azmp）を積分してなる関数Ｆzmpと、着地位置修正変数Ｗftxp(k)，Ｗftxn(k)，Ｗftyp(k)，Ｗftyn(k)のＬ２ノルム及びＬ１ノルムをそれぞれ表す関数Ｆft2，Ｆft1とを、次式（１９）で示すように合成してなる関数を評価関数Ｆとして用いる。
【０１９８】
Ｆ＝Ｆzmp＋Ｆft2＋Ｆft1    ……（１９）
【０１９９】
上記関数Ｆzmp，Ｆft2，Ｆft1は、次のように構築されている。なお、以降、↑AzmpをＺＭＰ加速度と称し、その二乗値(↑Azmp)2（＝↑AzmpT・↑Azmp）をＺＭＰ二乗加速度と称する。
【０２００】
まず、ＺＭＰ軌道変数Wx_i,j、Wy_i,jに係る関数Ｆzmpを説明する。軌道生成対象期間の各区間ｉ（ｉ＝０，１，…，ｅ）の任意の時刻ｔでのＺＭＰ加速度↑Azmp_i(t)（＝[Axzmp_i(t)，Ayzmp_i(t)]T）は、前記式（１）を用いて、次式（２０）により表される。
【０２０１】
【数１３】
2019042879.tif 000015
【０２０２】
そして、ＺＭＰ加速度↑Azmp_i(t)の各成分Axzmp_i(t)，Ayzmp_i(t)の二乗値(Axzmp_i(t))2，(Ayzmp_i(t))2は、次式（２１ａ），（２１ｂ）により表される。
【０２０３】
(Axzmp_i(t))2＝↑Wx_iT・↑h(tc_i(t))・h(tc_i(t))T・↑Wx_i
＝↑Wx_iT・Ｈd_m(tc_i(t))・↑Wx_i    ……（２１ａ）
(Ayzmp_i(t))2＝↑Wy_iT・↑h(tc_i(t))・h(tc_i(t))T・↑Wy_i
＝↑Wy_iT・Ｈd_m(tc_i(t))・↑Wy_i    ……（２１ｂ）
ただし、
Ｈd_m(tc_i(t))＝↑h(tc_i(t))・h(tc_i(t))T（：(n+1)次の正方行列）
【０２０４】
従って、区間ｉにおける二乗値(Axzmp_i(t))2，(Ayzmp_i(t))2のそれぞれの積分値(時刻t_iから時刻t_i+1までの積分値)は、次式（２２ａ），（２２ｂ）により表される。
【０２０５】
【数１４】
2019042879.tif 000016
【０２０６】
ここで、式（２０）のただし書き、式（２１ａ），（２１ｂ）のただし書き、及び式（１２２ａ），（２２ｂ）のただし書きから明らかなように、H_m(T_i)は、その各成分が区間ｉの時間幅Ｔ_iに応じた定数値又はゼロとなる行列である。
【０２０７】
また、任意の時刻ｔでのＺＭＰ二乗加速度(↑Azmp_i(t))2は、ＺＭＰ加速度のＸ軸方向成分(Axzmp_i(t))2とＹ軸方向成分(Ayzmp_i(t))2との総和（＝(Axzmp_i(t))2＋(Ayzmp_i(t))2）である。
【０２０８】
そして、本実施形態では、このＺＭＰ二乗加速度(↑Azmp(t))2を軌道生成対象期間の全体で積分してなる積分値（時刻t_0から時刻t_e+1までの積分値）を、前記評価関数Ｆの構成要素の関数Ｆzmpとして用いる。
【０２０９】
この場合、ＺＭＰ二乗加速度(↑Azmp(t))2の積分値としての関数Ｆzmpは、次式（２３）により表される。
【０２１０】
【数１５】
2019042879.tif 000017
【０２１１】
なお、↑Wは２・(ｅ＋１)・(ｎ＋１）個の成分から成るベクトル、Hh_mは、２・(ｅ＋１)・(ｎ＋１）次の正方行列、Hs_mは、(ｅ＋１)・(ｎ＋１）次の正方行列である。
【０２１２】
このように式（２３）により定義される関数Ｆzmpは、２・(ｅ＋１)・(ｎ＋１）個のＺＭＰ軌道変数Wx_i,j，Wy_i,j（ｉ＝０，１，…，ｅ、ｊ＝０，１，…，ｎ）の二乗値を変数として含む二次形式（Wx_i,j，Wy_i,jについての二次形式）の関数となる。
【０２１３】
なお、関数Ｆzmpにおける行列Hs_mの各成分の値は、軌道生成対象期間の各区間ｉ（ｉ＝０，１，…，ｅ）の時間幅Ｔ_iの設定値を用いて算出される。 
【０２１４】
次に、着地位置修正変数Ｗftxp(k)，Ｗftxn(k)，Ｗftyp(k)，Ｗftyn(k)に係る関数Ｆft2及びＦft1は、それぞれ、次式（２４），（２５）により設定される。
【０２１５】
Ｆft2＝μ・↑Ｗft(k)T・↑Ｗft(k)
＝μ・(Ｗftxp(k)2＋Ｗftxn(k)2＋Ｗftyp(k)2＋Ｗftyn(k)2)    ……（２４）
Ｆft1＝ε・↑１T・↑Ｗft(k)
＝ε・(Ｗftxp(k)＋Ｗftxn(k)＋Ｗftyp(k)＋Ｗftyn(k))    ……（２５）
【０２１６】
なお、上記μ，εは、それぞれ所定の定数値、↑１は、全ての要素が“１”の定数ベクトルである。
【０２１７】
上記式（２４）により設定される関数Ｆft2は、着地位置修正変数Ｗftxp(k)，Ｗftxn(k)，Ｗftyp(k)，Ｗftyn(k)のＬ２ノルムを表す関数、関数Ｆft1は着地位置修正変数Ｗftxp(k)，Ｗftxn(k)，Ｗftyp(k)，Ｗftyn(k)のＬ１ノルムを表す関数である。
【０２１８】
ＺＭＰ軌道生成部５２は、以上の如く、発散成分条件（安定化条件）、経由点条件及び軌道接続条件をそれぞれ表す線形等式と、ＺＭＰ存在許容領域条件及び幾何学条件を表す線形不等式と、評価関数Ｆ（＝Ｆzmp＋Ｆft2＋Ｆft1）とを設定する。
【０２１９】
そして、ＺＭＰ軌道生成部５２は、これらの評価関数Ｆと、線形等式及び線形不等式と用いて、変数目標値決定部５２ａの処理を実行することで、目標ＺＭＰベクトルの軌道を表す多項式関数の係数としてのＺＭＰ軌道変数Wx_i,j，Wy_i,j（ｉ＝０，１，…，ｅ、ｊ＝０，１，…，ｎ）の目標値と、着地位置修正量↑Ｐft(k)（本実施形態では、ｋ＝１）を規定する着地位置修正変数Ｗftxp(k)，Ｗftxn(k)，Ｗftyp(k)，Ｗftyn(k)の目標値とにより構成される目標変数群を決定する。
【０２２０】
この場合、変数目標値決定部５２ａは、上記線形等式及び線形不等式をＺＭＰ軌道変数Wx_i,j、Wy_i,j及び着地位置修正変数Ｗftxp(k)，Ｗftxn(k)，Ｗftyp(k)，Ｗftyn(k)の値に関する制約条件として用いて、評価関数Ｆの値を最小化するように目標変数群を決定する処理を、２次計画問題の解法問題の公知のアルゴリズムにより実行する。このアルゴリズムとしては、例えば内点法、有効制約法等を使用することができる。
【０２２１】
これにより、軌道生成対象期間の全ての区間ｉ（ｉ＝０，１，…，ｅ）のそれぞれの目標ＺＭＰベクトル↑Pzmpの軌道（目標ＺＭＰのＸ軸方向位置XzmpとＹ軸方向位置Yzmpのそれぞれの軌道）を時間の関数として表す多項式関数が決定されると共に、着地位置修正量↑Ｐft(k)（１歩目の着地位置修正量↑Ｐft(1)）が決定されることとなる。
【０２２２】
以上が、本実施形態におけるＺＭＰ軌道生成部５２の処理の詳細である。
【０２２３】
以上説明した本実施形態によれば、目標ＺＭＰ軌道を多項式関数により表すことで、目標ＺＭＰ軌道に関する複数の制約条件のうち、前記安定化条件、経由点条件、及びＺＭＰ存在許容領域条件を、多項式関数の係数であるＺＭＰ軌道変数Wx_i,j，Wy_i,j（ｉ＝０，１，…，ｅ、ｊ＝０，１，…，ｎ）と、着地位置修正変数Ｗftxp(k)，Ｗftxn(k)，Ｗftyp(k)，Ｗftyn(k)とについての線形等式又は線形不等式により表すことができる。また、軌道接続条件は、ＺＭＰ軌道変数Wx_i,j，Wy_i,j（ｉ＝０，１，…，ｅ、ｊ＝０，１，…，ｎ）についての線形等式で表すことができ、幾何学条件は、着地位置修正変数Ｗftxp(k)，Ｗftxn(k)，Ｗftyp(k)，Ｗftyn(k)についての線形不等式で表すことができる。
【０２２４】
そして、評価関数Ｆ（コスト関数）は、目標ＺＭＰベクトル↑Pzmpの変化加速度↑Azmpの二乗値(↑Azmp)2を積分してなる関数Ｆzmpと、着地位置修正変数Ｗftxp(k)，Ｗftxn(k)，Ｗftyp(k)，Ｗftyn(k)のＬ２ノルム及びＬ１ノルムをそれぞれ表す関数Ｆft2，Ｆft1との総和の関数として設定される。
【０２２５】
このため、二次計画問題の解法手法のアルゴリズムによって、線形等式及び線形不等式により表される複数の制約条件を満たしつつ、上記評価関数Ｆの値を最小化するように、ＺＭＰ軌道変数Wx_i,j，Wy_i,jと、着地位置修正変数Ｗftxp(k)，Ｗftxn(k)，Ｗftyp(k)，Ｗftyn(k)との目標値を一括して決定することができる。ひいては、当該複数の制約条件の全てを満足し得る目標ＺＭＰ軌道を生成することと、着地位置修正変数Ｗftxp(k)，Ｗftxn(k)，Ｗftyp(k)，Ｗftyn(k)を決定する（ひいては、着地位置修正量↑ΔＰftを決定する）こととを、一括して効率よく実行することができる。
【０２２６】
また、評価関数Ｆ（コスト関数）が上記の如く設定されているので、目標ＺＭＰの急激な変化を生じ難い滑らかな目標ＺＭＰ軌道を生成できると共に、着地位置修正量↑ΔＰftを極力小さくすることができる。特に、評価関数Ｆは、着地位置修正量↑ΔＰftに関するＬ１コストを表わす関数Ｆft1を含むため、着地位置修正量↑ΔＰftを極力、ゼロもしくはそれに近い値に保つことの効果を高めることができる。ひいては、ロボット１の移動動作を滑らかに行い得る目標歩容を生成できる。
【０２２７】
次に、本実施形態により、目標着地位置の修正と併せて生成される目標ＺＭＰ軌道の実施例を図９を参照して説明する。
【０２２８】
図９の第１段目（最上段）のグラフは、ロボット１に付与された外力を表し、第２段目のグラフは、左右の足部１８Ｌ，１８Ｒのそれぞれの目標着地位置の、基準の目標着地位置からの修正量を表し、第３段目（最下段）のグラフは、目標ＺＭＰの変化量（ロボット１の定常的な通常歩行時の目標ＺＭＰ軌道からのずれ量）を表している。
【０２２９】
時刻ｔａにおいて、ロボット１に外力が付与されている。このため、期間Ｔｐにおいて、右側の足部１８Ｒの目標着地位置が基準の目標着地位置からずれたものとなっている。そして、これに応じて期間Ｔｑにおいて、目標ＺＭＰ軌道が、定常的な通常歩行時の目標ＺＭＰ軌道からずれた軌道として生成されている。
【０２３０】
このように、本実施形態によれば、目標着地位置の修正と併せて、目標ＺＭＰ軌道を生成することができる。
【０２３１】
なお、本発明は、以上説明した実施形態に限定されるものではなく、上記実施形態以外の態様も採用し得る。以下に、いくつかの変形態様を説明する。
【０２３２】
前記実施形態では、目標着地位置の修正を、１歩目の目標着地位置についてだけ行った。ただし、目標着地位置の修正を１歩目だけでなく、２歩目についても行うようにしてもよい。
【０２３３】
また、前記実施形態では、ロボット１の２脚歩容の形態で移動させる場合を一例として説明した。ただし、例えば、ロボット１の脚リンク機構３Ｒ，３Ｌの足部１８Ｒ，１８Ｌに加えて、腕リンク機構４Ｌ，４Ｒのハンド部３０Ｒ，３０Ｌを移動接地部として用いることで、４脚歩容の形態でロボット１の移動を行う場合でも、本発明を適用できる。
【０２３４】
また、本発明における脚式移動ロボットは、２つの脚リンク機構３Ｒ，３Ｌを有するロボット１に限らず、３つ以上の脚リンク機構を有するロボットであってもよい。
【０２３５】
また、前記実施形態では、目標ＺＭＰ軌道に関する制約条件として、前記安定化条件、経由点条件、軌道接続条件、ＺＭＰ存在許容領域条件、及び幾何学条件を用いたが、いずれかの条件を省略したり、あるいは、さらに多くの制約条件を追加してもよい。
【０２３６】
例えば経由点条件を省略してもよい。また、軌道生成対象期間を複数の区間ｉに分割しない場合には、軌道接続条件は不要である。
【０２３７】
また、前記実施形態では、評価関数Ｆの構成要素のうち、ＺＭＰ軌道変数Wx_i,j，Wy_i,jに係る関数Ｆzmpとして、ＺＭＰ二乗加速度の積分値を表す関数を使用した。ただし、関数Ｆzmpは、例えば、目標ＺＭＰの移動速度の積分値などの他の関数を用いることも可能である。また、評価関数Ｆの構成要素の関数Ｆft1，Ｆft2の一方を省略してもよい。
【符号の説明】
【０２３８】
１…脚式移動ロボット、５１…着地目標設定部、５２…目標ＺＭＰ軌道生成部（目標ＺＭＰ軌道の生成装置）、５２ａ…変数目標値決定部。
【図１】
2019042879.tif 000018
【図２】
2019042879.tif 000019
【図３】
2019042879.tif 000020
【図４】
2019042879.tif 000021
【図５】
2019042879.tif 000022
【図６】
2019042879.tif 000023
【図７】
2019042879.tif 000024
【図８】
2019042879.tif 000025
【図９】
2019042879.tif 000026