JP6450279B2 - 移動ロボットの目標ｚｍｐ軌道の生成装置 - Google Patents

Description

本発明は、移動ロボットの目標ＺＭＰ軌道を生成する装置に関する。

移動ロボットの移動制御を行う場合、ＺＭＰ（Zero Moment Point）の目標位置の軌道（時間軸上での軌道）である目標ＺＭＰ軌道を生成し、この目標ＺＭＰ軌道を満足するように、ロボットの各関節の動作目標を規定する目標歩容を生成することが従来より一般に行われている（例えば特許文献１を参照）。

また、例えば非特許文献１には、多項式関数の形態で表した目標ＺＭＰ軌道を用いて、ロボットの重心の軌道を生成する技術が提案されている。

特許３７２６０８１号

「ヒューマノイドロボットの実時間歩容計画における解析的アプローチ」／原田研介、梶田秀司、森澤光晴、金広文男、藤原清司、金子健二、比留川博久／日本ロボット学会誌，Vol.23 No.6, pp.752-760, 2005

移動ロボットの適切な移動を行い得るように目標ＺＭＰ軌道を生成するためには、該目標ＺＭＰ軌道は種々の制約を受ける。

その制約条件としては、目標ＺＭＰ軌道の各時刻におけるＺＭＰの位置が、その時刻での支持多角形内に存在しなければならないというＺＭＰの存在許容領域に関する制約条件、あるいは、移動ロボットを継続的に安定状態に保ち得る目標歩容を生成できるという制約条件等がある。

例えば、前記特許文献１には、支持多角形の中央付近を通る折れ線状に設定した基準のＺＭＰ軌道を、台形状の軌道パターンで修正することにより、移動ロボットを継続的に安定状態に保ち得る目標歩容（具体的には、定常歩容に収束させ得る目標歩容）を生成できる目標ＺＭＰ軌道を生成する技術が提案されている。

しかしながら、特許文献１に見られる如き従来の目標ＺＭＰ軌道の生成手法では、目標ＺＭＰ軌道に関する複数の制約条件を同時に満たし得る目標ＺＭＰ軌道を効率よく生成することが困難であった。例えば、特許文献１に見られる技術では、基準のＺＭＰ軌道の修正により、移動ロボットを継続的に安定状態に保ち得る目標歩容を生成できる目標ＺＭＰ軌道を生成しても、該目標ＺＭＰ軌道がＺＭＰ存在許容領域に関する制約条件を満たさなくなる場合があり得る。そして、その場合には、目標ＺＭＰ軌道を生成し直す必要がある。

また、特許文献１に見られる技術では、基準のＺＭＰ軌道を台形状の軌道パターンで修正するために、支持多角形のうち、ＺＭＰの目標位置として設定し得る領域が支持多角形よりも狭い領域に制限されやすい。このため、ＺＭＰの目標位置の設定の自由度が抑制され、ひいては、生成し得る目標歩容の自由度が抑制されやすい。

また、特許文献１に見られる技術では、目標ＺＭＰ軌道が折れ線形状であるために、該目標ＺＭＰ軌道の折れ点の時刻付近での移動ロボットの運動の滑らかさが損なわれる虞がある。

なお、前記非特許文献１には、目標ＺＭＰ軌道を多項式関数により表すことが記載されているものの、該非特許文献１には、多項式関数により表される目標ＺＭＰ軌道を、種々の制約条件を満たすように生成する技術については記載されていない。

本発明はかかる背景に鑑みてなされたものであり、目標ＺＭＰの存在許容領域に関する制約条件を含む種々様々な制約条件を満たし得る目標ＺＭＰ軌道を効率よく適切に生成することができる移動ロボットの目標歩容生成装置を提供することを目的とする。

本発明の移動ロボットの目標ＺＭＰ軌道の生成装置は、かかる目的を達成するために、移動ロボットのＺＭＰ（Zero Moment Point）の目標位置の軌道である目標ＺＭＰ軌道を生成する装置であって、
前記目標ＺＭＰ軌道を、時間を変数とする複数次数の多項式関数により表現される軌道であるとして、該多項式関数の各項の係数の目標値から成る目標係数群を決定する多項式関数係数群決定手段を備えており、
該多項式関数係数群決定手段は、前記目標係数群に含まれる係数の二乗値を変数として含むように構成される二次形式の評価関数と、該係数についての線形等式又は線形不等式により各々構成される複数の制約条件とを用い、二次計画問題の解法手法により前記制約条件を満たしつつ、前記評価関数の値を最小化するように前記目標係数群を決定するように構成されており、
前記制約条件は、前記目標ＺＭＰ軌道における１つ以上のサンプリング時刻でのＺＭＰの目標位置が、該サンプリング時刻での移動ロボットの接地部位の目標位置及び目標姿勢に応じて設定した所定の多角形領域内に存在するという条件を表す線形不等式を含むように構成されていることを特徴とする（第１発明）。

ここで、本願発明者は、各種検討によって次のことを知見した。すなわち、目標ＺＭＰ軌道を、時間を変数とする複数次数の多項式関数により表現した場合、目標ＺＭＰ軌道を決定するということは、該目標ＺＭＰ軌道を表す多項式関数の各項の係数の目標値から成る目標係数群を決定することと等価である。

そして、目標ＺＭＰの存在許容領域等、目標ＺＭＰ軌道に関する種々様々な制約条件は、上記多項式関数の各項の係数ついての線形不等式又は線形等式の形態で表すことができる。

なお、線形等式は、上記多項式関数の係数の線形結合値がある定数値に等しいことを示す等式である。また、線形不等式は、上記多項式関数の係数の線形結合値がある定数値よりも大きい、又は小さいということを示す不等式である。

そして、上記多項式関数の目標係数群に含まれる係数の二乗値を変数として含むように構成される二次形式の適当な評価関数を設定することで、上記線形不等式又は線形等式により表される制約条件を満たしつつ、評価関数の値を最小化するように目標係数群を決定することを、二次計画問題の解法手法を適用して行うことができる。

この場合、当該二次計画問題の解法手法では、上記線形不等式又は線形等式として複数の式を用いることができるので、複数の制約条件を同時に満たし得るように、目標係数群を決定できる。

また、特に、ＺＭＰの存在許容領域に関する制約条件は、前記目標ＺＭＰ軌道における１つ以上のサンプリング時刻でのＺＭＰの目標位置が、該サンプリング時刻での移動ロボットの接地部位の目標位置及び目標姿勢に応じて設定した所定の多角形領域内に存在するという条件として表現することができる。そして、この場合、当該制約条件は、多項式関数の係数についての線形不等式の形態で表すことができる。

なお、各サンプリング時刻での上記多角形領域は、該サンプリング時刻での移動ロボットの接地面を包含する最小の凸領域としての支持多角形に一致し、又は該支持多角形の内側に設定される領域である。

そこで、本発明では、移動ロボットの目標ＺＭＰ軌道生成装置を上記第１発明の構成とした。

かかる第１発明によれば、二次計画問題の解法手法を用いることで、ＺＭＰの存在許容領域に関する制約条件（線形不等式により表される制約条件）を含む複数の制約条件を満たす目標係数群を決定できる。これにより多項式関数により表される目標ＺＭＰ軌道を、複数の制約条件を満たし得るように生成できる。

この場合、上記多項式関数は、複数次数の多項式関数であるから、目標ＺＭＰ軌道を直線あるいは曲線のいずれの形態でも生成できる。このため、支持多角形の全体または大部分の領域が、目標ＺＭＰ軌道上の点が設定される領域となり得るように目標ＺＭＰ軌道を生成できる。

従って、第１発明によれば、目標ＺＭＰの存在許容領域に関する制約条件を含む種々様々な制約条件を満たし得る目標ＺＭＰ軌道を効率よく適切に生成することができる。

かかる第１発明では、前記制約条件は、前記目標係数群に含まれる係数についての線形不等式又は線形等式により表される制約条件であれば、ＺＭＰの存在許容領域に関する前記第１制約条件以外の種々様々な制約条件を含み得る。

具体的には、上記第１発明では、前記制約条件は、前記移動ロボットの重心の運動を、該重心に質点を有し、且つ前記目標ＺＭＰ軌道上のＺＭＰを支点として有する倒立振子質点の運動により表現するモデルにおける該倒立振子質点を安定状態に保つための条件であって、将来の所定時刻における該倒立振子質点の運動状態が所定の運動状態に一致するという第２制約条件を表す線形等式をさらに含むように構成され得る（第２発明）。

すなわち、上記第２制約条件は、前記目標係数群に含まれる係数についての線形等式により表すことが可能である。このため、前記制約条件は、上記第２制約条件をさらに含みことができる。これにより、多項式関数により表される目標ＺＭＰ軌道が前記第１制約条件に加えて、さらに第２制約条件をも満たし得るように、二次計画問題の解法手法により、多項式関数の目標係数群を決定できる。ひいては、移動ロボットの重心の安定性が高い目標歩容を生成する上で好適な目標ＺＭＰ軌道を生成できる。

上記第２発明では、前記将来の所定時刻における倒立振子質点の運動状態としては、例えば該倒立振子質点の位置と移動速度との線形結合により表される運動状態を採用できる（第３発明）。

これにより、第２制約条件を、線形等式で表現することができる。

また、上記第１〜第３発明では、前記制約条件は、前記目標ＺＭＰ軌道のうちの所定の時刻でのＺＭＰの目標位置が、所定位置に一致するという第３制約条件を表す線形等式をさらに含むように構成され得る（第４発明）。

すなわち、上記第３制約条件は、前記目標係数群に含まれる係数についての線形等式により表すことが可能である。このため、前記制約条件は、上記第３制約条件をさらに含みことができる。これにより、多項式関数により表される目標ＺＭＰ軌道が前記第１制約条件に加えて、さらに第３制約条件をも満たし得るように、二次計画問題の解法手法により、多項式関数の目標係数群を決定できる。ひいては、目標ＺＭＰ軌道が、所定の時刻において所定の位置を通る軌道となるように目標ＺＭＰ軌道を生成できる。

また、上記第１〜第４発明では、前記目標ＺＭＰ軌道は、複数の区間の軌道に分割されていると共に各区間毎に個別の多項式関数により表される軌道であり、前記多項式関数係数群決定手段が決定する目標係数群は、前記複数の区間の全ての多項式関数の各項の係数の目標値からなることが好ましい（第５発明）。

これによれば、目標ＺＭＰ軌道を、複数の区間のそれぞれ毎の個別の多項式関数で表すことができるので、種々様々な軌道パターンの目標ＺＭＰ軌道を生成できる。また、例えば、移動ロボットの接地部位（足部等）の個数の切替えタイミング、あるいは、移動ロボットの接地部位の着地タイミング等、移動ロボットの移動時の動作パターンの切替わり毎に、区間を分けて目標ＺＭＰ軌道を生成することも可能である。

上記第５発明では、前記制約条件は、前記複数の区間で互いに隣合う任意の２つの区間のうち、先行側の区間の多項式関数により規定される該先行側の区間の終端時刻におけるＺＭＰの位置と該ＺＭＰの位置の所定階数の微分値とのそれぞれと、後行側の区間の多項式関数により規定される該後行側の区間の始端時刻におけるＺＭＰの位置と該ＺＭＰの位置の前記所定階数の微分値とのそれぞれとが互いに一致するという第４制約条件を表す線形等式をさらに含むように構成され得る（第６発明）。

なお、「ＺＭＰの位置の所定階数の微分値」というのは、ＺＭＰの移動速度（１階微分値）、移動加速度（２階微分値）、３階微分値等を意味する。当該所定階数の微分値は、互いに異なる階数の複数の微分値を含んでいてもよい。

上記第４制約条件のうちのＺＭＰの位置と該ＺＭＰの位置の所定階数の微分値とのそれぞれの関する条件は、いずれも、前記目標係数群に含まれる係数についての線形等式により表すことが可能である。このため、前記制約条件は、上記第４制約条件をさらに含みことができる。これにより各区間の目標ＺＭＰ軌道が、ＺＭＰの位置と該ＺＭＰの位置の所定階数の微分値との全てについて滑らかに連続するように、各区間の目標ＺＭＰ軌道の全体から成る目標ＺＭＰ軌道を生成できる。ひいては、移動ロボットの滑らかな移動を行う目標歩容を生成する上で好適な目標ＺＭＰ軌道を生成できる。

上記第５発明又は第６発明では、前記複数の区間のそれぞれにおける多項式関数は、該区間の時間幅をあらかじめ定めた一定値とみなし、且つ、該区間の始端時刻をゼロとみなして該区間での時間を正規化してなる時間の関数として表現してなる多項式関数であることが好ましい（第７発明）。

これによれば、前記複数の区間のそれぞれにおける時刻が、該複数の区間で共通の形式で正規化された時刻として表現できるため、前記制約条件を表す線形等式又は線形不等式、あるいは、前記評価関数を簡略化して表現できる。

上記第１〜第７発明では、前記評価関数としては、前記目標係数群に含まれる係数の二乗値を変数として含むように構成される二次形式の関数であれば、種々様々な関数を採用し得る。

例えば、前記評価関数として、前記目標ＺＭＰ軌道におけるＺＭＰの移動加速度の二乗値の積分値を表す関数を採用し得る（第８発明）。

ここで、ＺＭＰの移動加速度は、ＺＭＰの位置の２階微分値に相当するものであるから、目標ＺＭＰ軌道が多項式関数により表される場合、前記目標ＺＭＰ軌道におけるＺＭＰの移動加速度も多項式関数により表される。このため、該移動加速度の二乗値の積分値は、前記目標係数群に含まれる係数の二乗値を変数として含むように構成される二次形式の関数となる。従って、ＺＭＰの移動加速度の二乗値の積分値を表す関数を前記評価関数として採用できる。

そして、このようにＺＭＰの移動加速度の二乗値の積分値を表す関数を前記評価関数として採用した場合、二次計画問題の解法手法によって、ＺＭＰの移動加速度の二乗値の積分値を最小化するように、前記目標係数群が決定されることとなる。

このため、目標ＺＭＰ軌道におけるＺＭＰの移動速度が急激に変化するような形態で目標ＺＭＰ軌道が生成されるのを防止できる。ひいては、移動ロボットの滑らかな移動を行い得る目標歩容を生成できるように、滑らかな目標ＺＭＰ軌道を生成できる。

本発明の一実施形態における移動ロボットの構成を示す図。 移動ロボットの制御装置の機能を示すブロック図。 移動ロボットの移動時の足部の目標着地位置及び目標着地姿勢の例を示す図。 足部の目標位置及び目標姿勢の軌道の一例を示す図。 移動ロボットの倒立振子モデルを説明するための図。 目標ＺＭＰ軌道の軌道生成対象期間の分割に関する説明図。 図７Ａは片脚支持期における支持多角形の一例、図７Ｂは両脚支持期における支持多角形の一例を示す図。 実施例における目標ＺＭＰ軌道及び目標重心軌道を示すグラフ。 実施例及び比較例のそれぞれにおける目標ＺＭＰのＹ軸方向位置の軌道を示すグラフ。

本発明の一実施形態を図１〜図９を参照して以下に説明する。図１は本実施形態で例示する移動ロボットの概略構成を模式的に示している。この移動ロボット１（以降、単にロボット１という）は、基体２と、基体２から延設された複数（本実施形態では４つ）の可動リンク機構３Ｌ，３Ｒ，４Ｌ，４Ｒと、頭部５とを有する人型の脚式移動ロボットである。

４つの可動リンク機構３Ｌ，３Ｒ，４Ｌ，４Ｒのうちの可動リンク機構３Ｌ，３Ｒは、左右一対の脚リンク機構に相当するリンク機構、可動リンク機構４Ｌ，４Ｒは、左右一対の腕リンク機構に相当するリンク機構である。

なお、図示の参照符号中の「Ｌ」、「Ｒ」は、それぞれロボット１の左側の構成要素、右側の構成要素を示す符号である。ただし、以降の説明では、左右を区別する必要が無いときは、符号「Ｌ」，「Ｒ」を省略する。また、可動リンク機構３Ｌ，３Ｒを脚リンク機構３Ｌ，３Ｒ（又は脚リンク機構３）と称し、可動リンク機構４Ｌ，４Ｒを腕リンク機構４Ｌ，４Ｒ（又は腕リンク機構４）と称する場合がある。

また、本実施形態の以下の説明では、ヨー方向、ピッチ方向、ロール方向は、それぞれ図１に示すようにロボット１が直立姿勢で起立した状態（基体２、各可動リンク３Ｒ，３Ｌ，４Ｒ，４Ｌをほぼ上下方向に伸ばした状態）での該ロボット１の上下方向の軸（図１に示すＺ軸）周りの方向、左右方向の軸（図１に示すＹ軸）周りの方向、前後方向の軸（図１ではＸ軸）周りの方向を意味する。なお、図１に示すＸ軸、Ｙ軸、Ｚ軸は、３軸直交座標系の各座標軸である。

基体２は、ロボット１の上体に相当するリンク機構である。この基体２は、本実施形態では、下側基体１０と、この下側基体１０の上方に配置された上側基体１１との２つの要素リンクと、該下側基体１０及び上側基体１１を連結する関節機構１２とから構成されている。図示例のロボット１では、関節機構１２は、例えばピッチ方向（Ｙ軸周り方向）の回転自由度を有する関節１２ａにより構成されている。

可動リンク機構３Ｌ，３Ｒ，４Ｌ，４Ｒのうちの脚リンク機構３Ｌ，３Ｒは、互いに同じ構造のものである。具体的には、各脚リンク機構３は、下側基体１０に股関節機構１３を介して連結された大腿部１４と、この大腿部１４に膝関節機構１５を介して連結された下腿部１６と、この下腿部１６に足首関節機構１７を介して連結された足部１８とを、該脚リンク３機構を構成する要素リンクとして備える。足部１８は、脚リンク機構３の先端部を構成する要素リンクである。

図示例のロボット１では、股関節機構１３は、ヨー方向（Ｚ軸周り方向）、ピッチ方向（Ｙ軸周り方向）及びロール方向（Ｘ軸周り方向）の回転自由度をそれぞれ有する３つの関節１９，２０，２１により構成されている。また、膝関節機構１５は、ピッチ方向の回転自由度を有する関節２２により構成されている。また、足首関節機構１７は、ピッチ方向及びロール方向の回転自由度をそれぞれ有する２つの関節２３，２４により構成されている。

従って、本実施形態では、各脚リンク３機構は、その先端部（足部１８）が下側基体１０に対して、６自由度の運動自由度を有するように構成されている。

可動リンク機構３Ｌ，３Ｒ，４Ｌ，４Ｒのうちの腕リンク機構４Ｒ，４Ｌは、互いに同じ構造のものである。具体的には、各腕リンク機構４は、上側基体１１に肩関節機構２５を介して連結された上腕部２６と、この上腕部２６に肘関節機構２７を介して連結された前腕部２８と、この前腕部２８に手首関節機構２９を介して連結されたハンド部３０とを、該腕リンク機構４を構成する要素リンクとして備える。ハンド部３０は、腕リンク機構４の先端部を構成する要素リンクである。

図示例のロボット１では、肩関節機構２５は、ピッチ方向、ロール方向、及びヨー方向の回転自由度をそれぞれ有する３つの関節３１，３２，３３により構成されている。また、肘関節機構２７は、ピッチ方向（又はロール方向）の回転自由度を有する関節３４により構成されている。また、手首関節機構２９は、ヨー方向、ピッチ方向及びロール方向の回転自由度をそれぞれ有する２つの関節３５，３６，３７により構成されている。

従って、本実施形態では、各腕リンク機構４は、その先端部（ハンド部３０）が上側基体１１に対して、７自由度の運動自由度を有するように構成されている。

なお、ハンド部３０は、該ハンド部３０による作業用の開閉機構、あるいは、複数の屈伸可能な指機構等を備えていてもよい。

頭部５は、上側基体１１の上方に配置され、該上側基体１１に固定されている。ただし、頭部５は、例えばチルト動作あるいはパン動作等を行い得るように、上側基体１１に対して関節機構を介して連結されていてもよい。

以上が本実施形態のロボット１の機構的な構造の概要である。かかる構造のロボット１は、基本的には、左右の脚リンク機構３Ｌ，３Ｒの動作（各足部１８の空中移動及びそれに続く接地を繰り返す２脚歩容の動作）によって移動できる。

あるいは、腕リンク機構４Ｌ，４Ｒを脚リンク機構３Ｌ，３Ｒとは別の脚リンク機構として動作させてロボット１の移動を行うことも可能である。例えば、脚リンク機構３Ｌ，３Ｒ及び腕リンク機構４Ｌ，４Ｒの４つの可動リンク機構のそれぞれの先端部（足部１８、ハンド部３０）の空中移動及びそれに続く接地を繰り返す４脚歩容の動作によってロボット１の移動を行うことも可能である。

補足すると、各脚リンク機構３は、例えば６自由度よりも大きい運動自由度を有するように構成されていてもよい。また、各腕リンク機構４は、例えば７自由度よりも小さい運動自由度、もしくは７自由度よりも大きい運動自由度を有するように構成されていてもよい。

また、基体２の関節機構１２は、ロール方向又はヨー方向の回転自由度を有する関節により構成されていてもよい。あるいは、該関節機構１２は、１自由度よりも大きい運動自由度を有するように構成されていてもよい。あるいは、基体２は、関節機構１２を備えない一体構成のもの（下側基体１０と上側基体１１とを一体化した構成のもの）であってもよい。

また、各脚リンク機構３及び各腕リンク機構４のそれぞれは、回転型の関節に限らず、直動型の関節を含んでいてもよい。

また、ロボット１は、頭部５を持たない構造のもの、あるいは腕リンク機構４Ｒ，４Ｌの一方又は両方を備えないものであってもよい。

また、ロボット１は、３つ以上の脚リンク機構を備えるものであってもよい。なお、脚リンク機構３Ｌ，３Ｒに加えて、腕リンク機構４Ｌ，４Ｒを脚リンク機構として動作させてロボット１の移動を行う場合には、ロボット１は、実質的に４つの脚リンク機構を有するロボットとみなすこともできる。

図１での図示は省略したが、ロボット１には、図２に示すように、上記の各関節をそれぞれ回転駆動する関節アクチュエータ４０と、ロボット１の動作制御を行なう制御装置４１とが搭載されている。

関節アクチュエータ４０は、例えば電動モータあるいは油圧アクチュエータにより構成されたものであり、各関節毎に備えられている。この場合、各関節アクチュエータ４０による各関節の駆動機構は公知の構造のものでよい。また、関節アクチュエータ４０は、回転型のアクチュエータに限らず、直動型のアクチュエータであってもよい。

制御装置４１は、ＣＰＵ、ＲＡＭ、ＲＯＭ、インターフェース回路等を含む電子回路ユニットにより構成されている。この制御装置４１は、実装されるプログラムとハードウェア構成とにより実現される主要な機能として、ロボット１の各関節の動作制御のための制御用目標値としての目標歩容（ロボット１の動作目標）を生成する目標歩容生成部４２と、該目標歩容に応じてロボット１の各関節アクチュエータ４０を制御する関節アクチュエータ制御部４３とを備える。

目標歩容生成部４２は、ロボット１の移動時に空中移動及びそれに続く着地（接地）を行わせる部位（足部１８又はハンド部３０）である移動接地部のそれぞれの着地予定箇所での位置及び姿勢の目標（以降、目標着地位置姿勢という）と該着地予定箇所での着地時刻の目標（以降、目標着地時刻という）を設定する着地目標設定部５１と、各移動接地部の移動動作における目標位置及び目標姿勢の軌道を生成する移動接地部軌道生成部５２と、ＺＭＰ（Zero Moment Point）の目標位置（以降、目標ＺＭＰということがある）の軌道である目標ＺＭＰ軌道を生成する目標ＺＭＰ軌道生成部５３と、ロボット１全体の重心の目標位置の軌道である目標重心軌道を生成する目標重心軌道生成部５４と、ロボット１の各関節の変位量の目標値である目標関節変位量の軌道を生成する目標関節変位量軌道生成部５５とを含む。

ここで本明細書での用語に関して補足すると、ロボット１の足部１８等の任意の部位の「位置」は、該部位の代表点（該部位に対して固定された点）の空間的な位置を意味し、任意の部位の「姿勢」は、該部位の空間的な向きを意味する。

また、「位置姿勢」は、「位置」及び「姿勢」の組、「目標位置姿勢」は、「目標位置」及び「目標姿勢」の組を意味する。また、「軌道」は、瞬時値の時系列、又は時間の関数として表現されるものを意味する。

なお、ロボット１の任意の部位の「位置」及び「姿勢」のそれぞれは、ロボット１の動作環境に対して任意に設計的に設定されるグローバル座標系（ワールド座標系）で見た位置及び姿勢として記述される。

また、ＺＭＰは、ロボット１の全体の運動によって発生する慣性力とロボット１に作用する重力との合力によって、該ＺＭＰの周りに発生するモーメントの水平方向成分（水平軸周りの成分）がゼロとなる床面上の点である。なお、「床面」は、通常の意味での床面（屋内の床面）に限らず、地面、路面等の接地面（移動接地部を接地させ得る面）も含む。

以降、脚リンク機構３Ｒ，３Ｌの各足部１８の空中移動及びこれに続く着地を繰り返す２脚歩容の動作によってロボット１の移動を行う場合を一例として、制御装置４１の具体的な処理を説明する。

上記２脚歩容は、人の歩行動作と同様の形態で脚リンク機構３Ｒ，３Ｌの足部１８Ｒ，１８Ｌを動かすことでロボット１の移動を行う歩容である。より詳しくは、該２脚歩容は、脚リンク機構３Ｒ，３Ｌの一方の脚リンク機構（支持脚としての脚リンク機構）の足部１８を接地状態（床面から接触反力を受けた状態）として、他方の脚リンク機構（遊脚としての脚リンク機構）の足部１８の空中移動及びそれに続く着地を行う片脚支持期と、両方の脚リンク機構３Ｒ，３Ｌの足部１８Ｒ，１８Ｌが接地状態となる両脚支持期とが交互に繰り返される歩容である。この場合、片脚支持期での支持脚たる脚リンク機構及び遊脚たる脚リンク機構は、片脚支持期の繰り返し毎に、右側の脚リンク機構３Ｒと左側の脚リンク機構３Ｌとで交互に切替えられる。また、足部１８Ｒ，１８Ｌのそれぞれが前記移動接地部に相当する。

なお、以降の説明では、ロボット１の足部１８等の任意の部位の位置及び姿勢、あるいは、ＺＭＰもしくはロボット１の重心の位置を記述するグローバル座標系として、Ｘ軸及びＹ軸を２つの水平軸、Ｚ軸を鉛直軸とする３軸直交座標系を用いる。この場合、図１に示すように、Ｘ軸は、ロボット１の前後方向の水平軸、Ｙ軸は、ロボット１の左右方向の水平軸とする。

制御装置４１の目標歩容生成部４２は、ロボット１の移動開始前、あるいは、移動途中の適宜のタイミング（例えば、所定歩数の移動毎のタイミング）で、着地目標設定部５１の処理を実行する。該着地目標設定部５１は、Ｎ歩目（Ｎ：２以上の整数）までの１歩毎の遊脚側の脚リンク機構３の足部１８の目標着地位置姿勢及び目標着地時刻を設定する。

なお、「１歩」は、いずれか一方の脚リンク機構３（３Ｒ又は３Ｌ）の足部１８の空中移動とそれに続く着地とを行う動作を意味する。

図３は、例えば１歩目の遊脚及び支持脚をそれぞれ左側の脚リンク機構３Ｌ、右側の脚リンク機構３Ｒとし、２歩目の遊脚及び支持脚をそれぞれ右側の脚リンク機構３Ｒ、左側の脚リンク機構３Ｌとして、２歩目までの足部１８の目標着地位置姿勢（上方から見た目標着地位置姿勢）を設定した状態を例示している。この場合、実線で示す足部１８Ｌ(1)が１歩目の遊脚側の足部１８Ｌの目標着地位置姿勢、実線で示す足部１８Ｒ(2)が２歩目の遊脚側の足部１８Ｒの目標着地位置姿勢を例示している。

なお、ロボット１の動作環境の床面形状が事前に判明している場合には、着地目標設定部５１で設定する目標着地位置姿勢及び目標着地時刻は、ロボット１の移動開始前に、あらかじめ作成されたものであってもよい。また、ロボット１に搭載された外界センサ等によりロボット１の周辺の床面形状を計測しながらロボット１の移動を行う場合には、着地目標設定部５１で設定する目標着地位置姿勢及び目標着地時刻は、随時（例えばロボット１の１歩の移動毎に）、更新するようにしてもよい。

次に制御装置４１の目標歩容生成部４２は、着地目標設定部５１で設定した足部１８の目標着地位置姿勢及び目標着地時刻を用いて、各足部１８の目標位置姿勢軌道を生成する処理を移動接地部軌道生成部５２により実行する。

ここで、本実施形態では、制御装置４１の目標歩容生成部４２は、例えばロボット１の１歩の移動毎に、将来の所定歩数目までの期間におけるロボット１の目標歩容（各足部１８の目標位置姿勢軌道等）を生成する。

このため、移動接地部軌道生成部５２は、ロボット１の１歩の移動毎に、着地目標設定部５１で設定した足部１８の目標着地位置姿勢及び目標着地時刻を用いて、将来の所定歩数目、例えば２歩目までの期間における各足部１８の目標位置姿勢軌道を生成する。

この場合、２歩目までの各歩における遊脚側の脚リンク機構３の足部１８については、例えば、図４に例示する如く該足部１８の位置姿勢が変化していくように該足部１８の目標位置姿勢軌道が生成される。

図示例では、遊脚側の脚リンク機構３の足部１８の位置及び姿勢の両方を経時的に変化させている。ただし、遊脚側の脚リンク機構３の足部１８の姿勢は、該足部１８の空中での移動中の全期間又は一部の期間で一定であってもよい。

また、２歩目までの各歩における支持脚側の脚リンク機構３の足部１８については、該足部１８が滑らずに接地した状態に保たれるように該足部１８の目標足平位置姿勢軌道が決定される。この場合、該足部１８の目標位置姿勢は、例えば接地中の全期間で一定になるように設定され得る。あるいは、該足部１８の接地中の全期間又は一部の期間において、該足部１８の接地部分を変化させるように（接地部分を踵側からつま先側に変化させる等）、該足部１８の目標位置姿勢を経時的に変化させるように設定することも可能である。

さらに制御装置４１の目標歩容生成部４２は、移動接地部軌道生成部５２で生成した各足部１８の目標位置姿勢軌道を用いて、所定歩数目（２歩目）までの期間における将来の目標ＺＭＰ軌道を生成する処理を目標ＺＭＰ軌道生成部５３により実行する。

この処理の詳細は後述するが、目標ＺＭＰ軌道生成部５３は、目標ＺＭＰ軌道を、所定の制約条件下で、複数次数の多項式関数（時間の関数）により表される軌道として生成する。

次に制御装置４１の目標歩容生成部４２は、ロボット１の動力学モデルを用いて、目標ＺＭＰ軌道を満足するように、所定歩数目（２歩目）までの期間における将来のロボット１の目標重心軌道を生成する処理を目標重心軌道生成部５４により実行する。この場合、本実施形態では、ロボット１の動力学モデルとして倒立振子モデルが用いられる。

この倒立振子モデルは、ロボット１の重心の運動を、図５に示す倒立振子質点６０の運動によって表現する動力学モデルである。この倒立振子モデルでは、倒立振子質点６０は、床面からの高さが一定値ｈに維持された状態で、目標ＺＭＰを支点として、該高さｈの水平面上を動く。また、倒立振子質点６０の質量は、ロボット１全体の質量に一致する。

なお、図５は、グローバル座標系のＹ軸方向から見た（ＸＺ座標平面に投影して見た）倒立振子モデルを表している。

このような倒立振子モデルにおける倒立振子質点６０（ロボット１の重心）の動力学的な挙動は、次式（１）により表される。

d²↑Ppend/dt²＝(ｇ／ｈ)・(↑Ppend−↑Pzmp) ……（１）

ここで、↑Ppendは、グローバル座標系における倒立振子質点６０のＸ軸方向の位置XpendとＹ軸方向の位置Ypendとから成る２成分のベクトル（＝[Xpend, Ypend]^T）、↑Pzmpは、グローバル座標系における目標ＺＭＰ（＝倒立振子質点６０の揺動支点）のＸ軸方向の位置XzmpとＹ軸方向の位置Yzmpとから成る２成分のベクトル（＝[Xzmp, Yzmp]^T）、ｇは重力加速度定数である。

なお、本明細書では、任意の変数の先頭の「↑」は、当該変数がベクトル（縦ベクトル）であることを示す符号、上付きの添え字「Ｔ」はベクトル又は行列の転置を表す符号である。

目標重心軌道生成部５４は、目標ＺＭＰ軌道生成部５３で生成された目標ＺＭＰ軌道（２歩目までの期間における目標ＺＭＰ軌道）と、上記式（１）とを用いて、目標ＺＭＰを満足するように（詳しくは、倒立振子質点６０の運動によって発生する慣性力と倒立振子質点６０に作用する重力との合力によって目標ＺＭＰの周りに発生する水平軸周りのモーメント（Ｘ軸周りのモーメント及びＹ軸周りのモーメント）がゼロになるように）、２歩目までの期間における倒立振子質点６０の水平方向の運動加速度（式（１）の左辺の値）の時系列を算出する。

そして、目標重心軌道生成部５４は、算出した運動加速度を積分する（２階積分する）ことで、２歩目までの期間における倒立振子質点６０の位置↑Ｐpendの軌道を、ロボット１の目標重心軌道（詳しくは、重心の水平位置（Ｘ軸方向位置及びＹ軸方向位置）の軌道）として算出する。なお、ロボット１全体の重心のＺ軸方向の目標位置は、高さｈの位置である。

次に制御装置４１の目標歩容生成部４２は、所定歩数目（２歩目）までの将来のロボット１の各関節の目標変関節変位量の軌道を生成する処理を目標関節変位量軌道生成部５５により実行する。この場合、目標関節変位量軌道生成部５５は、例えば、分解運動量制御の手法を用いて目標関節変位量の軌道（瞬時値の時系列）を生成する。

この分解運動量制御の手法では、目標関節変位量軌道生成部５５は、ロボット１の目標重心軌道に応じてロボット１の全体の目標並進運動量（＝重心の目標並進運動量）の軌道を設定する。さらに、目標関節変位量軌道生成部５５は、ロボット１の重心の周りの（又は目標ＺＭＰ周りの）全体の目標角運動量の軌道を適宜設定する。また、目標関節変位量軌道生成部５５は、腕リンク機構４Ｒ，４Ｌのそれぞれの先端部たる各ハンド部３０Ｒ，３０Ｌの目標位置姿勢の軌道を適宜設定する。

なお、目標角運動量と、各ハンド部３０Ｒ，３０Ｌの目標位置姿勢の軌道とは、任意に設計的に設定し得る。

そして、目標関節変位量軌道生成部５５は、これらの目標並進運動量及び目標角運動量と、移動接地部たる各足部１８Ｒ，１８Ｌの目標位置姿勢軌道と、各ハンド部３０Ｒ，３０Ｌの目標位置姿勢軌道とから、目標並進運動量及び目標角運動量を実現し得る各関節の目標関節変位量の軌道（所定の制御処理周期毎の瞬時値の時系列）を分解運動量制御の手法により算出する。

なお、目標関節変位量軌道生成部５５は、分解運動量制御の手法以外の手法により目標関節変位量の軌道を生成してもよい。例えば、ロボット１の基体２及び各脚リンク機構３のそれぞれに１つ以上の質点を設定した多質点モデル（動力学モデル）、あるいは、該多質点モデルにフライホイールを付加した動力学モデルと、目標重心軌道と、目標ＺＭＰ軌道と、各足部１８Ｒ，１８Ｌの目標位置姿勢軌道とを用いて、目標重心軌道と目標ＺＭＰ軌道とを満足し得る基体２の目標位置姿勢の軌道を生成する。そして、基体２の目標位置姿勢の軌道と、足部１８Ｒ，１８Ｌの目標位置姿勢軌道とから逆運動学の演算処理により各脚リンク機構３の各関節の目標関節変位量の軌道を算出する。

この場合、基体２の関節１２ａの目標関節変位量と、各腕リンク機構４の各関節３１〜３７の目標関節変位量とは、例えば、一定に維持され、あるいは、足部１８Ｒ，１８Ｌの動きに同期した一定のパターンで経時的に変化するように設定し得る。

このように、各足部１８Ｒ，１８Ｌの目標位置姿勢軌道と目標ＺＭＰ軌道とを満足し得るロボット１の目標歩容は、種々様々な手法により生成し得る。

以上の如く、本実施形態では、ロボット１の目標歩容生成部４２は、ロボット１の１歩の移動毎に所定歩数目（２歩目）までの期間における将来の各関節の目標関節変位量の軌道を生成する。

そして、制御装置４１は、かかる関節変位量の軌道に応じてロボット１の各関節の変位量を制御する処理を関節アクチュエータ制御部４３により実行する。

この場合、関節アクチュエータ制御部４３は、ロータリエンコーダ等により構成される各関節の変位量センサ（図示省略）で検出される各関節の実際の変位量を、該関節の目標関節変位量に追従させるように、関節アクチュエータ４０をフィードバック制御する。

なお、この場合、所定の制御処理周期毎の目標関節変位量を、目標関節変位量軌道生成部５５で生成された目標値からコンプライアンス制御等により適宜修正するようにしてもよい。

補足すると、ロボット１の目標歩容（目標関節変位量の軌道等）の生成処理は、ロボット１の複数歩の移動毎に行ったり、あるいは、１歩分の期間よりも短い時間間隔で行うようにしてもよい。また、ロボット１の目標歩容（目標関節変位量の軌道等）の生成処理を実行する各タイミングにおいて、２歩目までの期間よりも短い期間（例えば１歩目までの期間）における目標歩容を生成したり、あるいは、２歩目までの期間よりも長い期間（例えば３歩目までの期間）における目標歩容を生成するようにしてもよい。

次に、説明を後回しにした前記目標ＺＭＰ軌道生成部５３の処理を詳説する。

目標ＺＭＰ軌道生成部５３は、本発明の目標ＺＭＰ軌道の生成装置の一例としての機能を制御装置４１に付与するものである。該目標ＺＭＰ軌道生成部５３は、目標ＺＭＰ軌道を表わす多項式関数の各項の係数の目標値から成る目標係数群を決定する多項式関数係数群決定部５３ａとしての機能を含む。該多項式関数係数群決定部５３ａは、本発明における多項式関数係数群決定手段に相当する。

本実施形態では、目標ＺＭＰ軌道生成部５３は、目標歩容生成部４２が目標歩容（目標関節変位量の軌道等）を生成する処理を実行する各タイミングにおいて目標ＺＭＰ軌道生成部５３が生成する所定歩数目（２歩目）までの期間（以降、軌道生成対象期間という）を、図６に例示するように複数の区間ｉ（ｉ＝０，１，…，ｅ）に分割する。

なお、図６での目標ＺＭＰ軌道は、一例として、ロボット１の直進歩行を行う場合における目標ＺＭＰのＹ軸方向位置の軌道を例示している。また、図中の足部１８Ｒは、右側の脚リンク機構３Ｒを遊脚として足部１８Ｒを動かす場合における該足部１８ＲのＹ軸方向の目標着地位置を示し、図中の足部１８Ｌは、脚リンク機構３Ｌを遊脚として足部１８Ｌを動かす場合における該足部１８ＬのＹ軸方向の目標着地位置を示している。

図示例では、例えば、各区間ｉが、片脚支持期および両脚支持期のいずれか一方の区間となるように、軌道生成対象期間が分割されている。この例では、軌道生成対象期間の分割個数ｅ＋１は、７個である。ただし、軌道生成対象期間を上記と異なる仕方で分割してもよい。例えば、軌道生成対象期間をさらに多くの区間に分割してもよい。また、例えば各区間ｉの時間幅が一定となるよように、軌道生成対象期間を分割してもよい。

目標ＺＭＰ軌道生成部５３の処理では、軌道生成対象期間を分割してなる各区間ｉの目標ＺＭＰ軌道（詳しくは、グローバル座標系における目標ＺＭＰのＸ軸方向位置Xzmp及びＹ軸方向位置Yzmpから成るベクトル↑Pzmp＝[Xzmp, Yzmp]^Tの各成分の軌道）が、各区間ｉ毎に個別の多項式関数により表現される。なお、目標ＺＭＰのＺ軸方向位置は、Ｘ軸方向位置及びＹ軸方向位置がそれぞれXzmp、Yzmpとなる床面上の点のＺ軸方向位置として規定される。

各区間ｉ（ｉ＝０，１，…，ｅ）における目標ＺＭＰ軌道（↑Pzmpの軌道）は、時間ｔの関数として、次式（２）の形の多項式関数で定義される。

式（２）の左辺の↑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_i(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の二乗値を変数として含む二次形式の評価関数（目的関数）と、該係数Wx_i,j、Wy_i,jについての線形等式又は線形不等式により表される複数の線形制約条件とを用いて、二次計画問題の公知の解法手法のアルゴリズムにより目標係数群を決定する。

この場合、本実施形態では、線形制約条件として、前記倒立振子モデルにおける倒立振子質点６０を継続的に安定状態に保つための制約条件（以降、安定化条件）と、目標ＺＭＰ軌道の経由点を規定する制約条件（以降、経由点条件という）と、軌道生成対象期間のうちの隣り合う区間ｉ−１，ｉ（ｉ＝１，２，…，ｅ）の境界での目標ＺＭＰ軌道の接続に関する制約条件（以降、軌道接続条件）と、目標ＺＭＰの位置を後述する支持多角形内の位置に制限するための制約条件（以降、ＺＭＰ存在許容領域条件という）とが用いられる。

上記安定化条件、経由点条件及び軌道接続条件は、線形等式により表される制約条件、ＺＭＰ存在許容領域条件は線形不等式により表される制約条件である。線形等式は、軌道生成対象期間の全ての区間ｉ（ｉ＝０，１，…，ｅ）のそれぞれにおける多項式関数の各項の係数Wx_i,j、Wy_i,j（ｊ＝０，１，…，ｎ）のうちの複数の係数の線形結合値がある定数値に等しいという関係を表す等式であり、線形不等式は、当該複数の係数の線形結合値がある定数値よりも大きい又は小さいという関係を表す不等式である。

なお、安定化条件、経由点条件、軌道接続条件、及びＺＭＰ存在許容領域条件は、それぞれ本発明における第２制約条件、第３制約条件、第４制約条件、第１制約条件に相当する。

以下にこれらの制約条件を表す線形等式又は線形不等式を具体的に説明する。

まず、前記安定化条件は、本実施形態では、軌道生成対象期間（本実施形態では２歩分の期間）の終端時刻における倒立振子モデルの発散成分（以降、終端発散成分という）の値が、軌道生成対象期間でのロボット１の歩容に続く仮想的な歩容としての定常歩容の始端時刻（＝軌道生成対象期間の終端時刻）における倒立振子モデルの発散成分（以降、定常歩容初期発散成分という）の値に一致するという条件（以降、発散成分条件）として表現される制約条件である。

なお、上記定常歩容及び発散成分については、例えば前記特許文献１等に詳細に説明されているものである。このため、本実施形態では、簡略的な説明に留める。

定常歩容は、軌道生成対象期間でのロボット１の歩容に続く歩容として仮想的に想定される歩容であって、各移動接地部及び基体２を含むロボット１の各部の運動が一定のパターンで周期的に繰り返されるようにしてロボット１の移動を継続的に行い得る歩容である。

このような定常歩容は、ロボット１の移動を継続的に行い得る歩容であるから、ロボット１の重心の挙動を表す倒立振子モデルの倒立振子質点６０が安定状態（倒立振子質点６０の水平方向位置が支点（目標ＺＭＰ）の直上位置から大きく乖離することのない状態）に保つことができる歩容である。

本実施形態において、２脚歩容の動作でロボット１の移動を行う場合における当該定常歩容は、例えば図３に示す如く、軌道生成対象期間におけるロボット１の２歩目の動作に続く周期的な歩容（２歩分の期間を１周期とする周期的な歩容）である。なお、図３では、定常歩容の各歩における足部１８の着地位置姿勢を破線で示している。

また、任意の時刻ｔでの倒立振子モデルの発散成分は、次式（４）で示すように、倒立振子質点６０の位置↑Ppendと、該倒立振子質点６０の移動速度d↑Ppend(t)/dt（↑Ppendの時間的変化率）との線形結合値として定義される値↑ｑ(t)（Ｘ軸方向成分ｑx(t)とＹ軸方向成分ｑy(t)とからなるベクトル）である。

↑ｑ(t)＝↑Ppend(t)＋(1／ω)・d↑Ppend(t)/dt ……（４）
ただし、ω＝√(ｇ／ｈ)

なお、√(ｇ／ｈ)は、(ｇ／ｈ)の平方根である。

このように定義される発散成分に関して、前記発散成分条件を満たすように目標ＺＭＰ軌道を設定することによって、軌道生成対象期間の後に続く将来に、倒立振子モデルの倒立振子質点６０を継続的に安定状態に保ち得る定常歩容でのロボット１の運動状態に収束させることができるように軌道生成対象期間でのロボット１の目標歩容を決定することができる。

そこで、本実施形態では、上記発散成分条件を、目標ＺＭＰ軌道に関する１つの制約条件（安定化条件）として用いる。

倒立振子モデルでは、軌道生成対象期間における発散成分↑ｑ(t)と、軌道生成対象期間における目標ＺＭＰベクトル↑Pzmp(t)との関係は、次式（５）により表される。

ここで、exp( )は、自然対数の底ｅの指数関数、↑q(t_0)は、軌道生成対象期間の始端時刻t_0での発散成分の値、↑Pzmp(t)は、軌道生成対象期間の任意の時刻ｔでの目標ＺＭＰベクトルである。軌道生成対象期間の各区間ｉにおける↑Pzmp(t)は、該区間ｉでの目標ＺＭＰベクトル↑Pzmp_i(t)に一致する。

従って、軌道生成対象期間の終端時刻t_e+1での発散成分（＝↑q(t_e+1)）の目標値としての定常歩容初期発散成分を↑q_targetとおくと、安定化条件としての発散成分条件は、次式（６）により表される。

また、この式（６）の左辺に、前記式（２），（３ａ），（３ｂ）を適用することで、式（６）の左辺は、次式（７）に書き換えられる。

なお、ｊ!、ｋ!は、それぞれｊの階乗、ｋの階乗である。

この式（７）の右辺の第１成分は、(ｅ＋１)・(ｎ＋１）個の係数Wx_i,j（ｉ＝０，１，…，ｅ、ｊ＝０、１，…，ｎ）の線形結合、第２成分は、(ｅ＋１)・(ｎ＋１）個の係数Wy_i,j（ｉ＝０，１，…，ｅ、ｊ＝０，１，…，ｎ）の線形結合として表される。

また、式（６）の右辺の第１成分及び第２成分は定数である。従って、発散成分条件は、次式（８）の線形等式として表される。

ここで、ａ_i,jは、Ｗx_i,j及びＷy_i,jの係数、C1x，C1yは定数である。

目標ＺＭＰ軌道生成部５３は、上記式（８）の第１成分及び第２成分の式を発散成分条件（安定化条件）を表す線形等式として設定する。

この場合、係数ａ_i,jは、倒立振子モデルの固有振動数ω（＝√(ｇ／ｈ)）と、軌道生成対象期間の区間ｉの個数（＝ｉ＋１）、各区間ｉにおける多項式関数の次数ｎ、及び各区間ｉの時間幅Ｔ_iのそれぞれのあらかじめ定められた設定値から、前記式（７）の演算式に従って算出される。

また、定数C1x，C1yは、倒立振子モデルの固有振動数ω（＝√(ｇ／ｈ)）及び軌道生成対象期間の終端時刻t_e+1のあらかじめ定められた設定値と、定常歩容初期発散成分↑q_target（軌道生成対象期間の終端時刻での発散成分の目標値）の値と、軌道生成対象期間の始端時刻t_0での発散成分↑q(t_0)の値とから式（６）の右辺の演算式により算出される。

この場合、↑q(t_0)は、始端時刻t_0までのロボット１の作成済の目標歩容により規定される始端時刻t_0でのロボット１の重心位置とその移動速度とから算出される。

また、定常歩容初期発散成分↑q_targetは、例えば次のように決定される。すなわち、目標ＺＭＰ軌道生成部５３は、定常歩容の各歩における足部１８の位置姿勢軌道を、軌道生成対象期間における足部１８の目標位置姿勢軌道に応じて設定する。

例えば、右側の脚リンク機構３Ｒを支持脚、左側の脚リンク機構３Ｌを遊脚として、該遊脚側の足部１８Ｌを移動させる各歩（定常歩容の奇数歩目の各歩）における足部１８Ｌの位置姿勢軌道（支持脚側の足部１８Ｒから見た相対的な位置姿勢軌道）のパターンが軌道生成対象期間における１歩目の足部１８Ｌの目標位置姿勢軌道のパターンと同じになり、且つ、左側の脚リンク機構３Ｌを支持脚、右側の脚リンク機構３Ｒを遊脚として、該遊脚側の足部１８Ｒを移動させる各歩（定常歩容の偶数歩目の各歩）における足部１８Ｒの位置姿勢軌道（支持脚側の足部１８Ｌから見た相対的な位置姿勢軌道）のパターンが軌道生成対象期間における２歩目の足部１８Ｒの目標位置姿勢軌道のパターンと同じになるように、定常歩容における足部１８の位置姿勢軌道が設定される。

そして、目標ＺＭＰ軌道生成部５３は、定常歩容の各周期の終端時刻（＝次の周期の始端時刻）でのロボット１の重心の運動状態（位置、移動速度及び加速度）が該周期の始端時刻での運動状態に一致するという条件を満たし得る重心の位置の軌道（倒立振子質点６０の位置の軌道）を倒立振子モデルにより生成し得るように、定常歩容の各周期における目標ＺＭＰ軌道を決定する。この場合、定常歩容の目標ＺＭＰ軌道は、折れ線状の軌道であってもよい。

なお、定常歩容の各周期の終端時刻（＝次の周期の始端時刻）でのロボット１の重心の運動状態のうちの位置が、該周期の始端時刻での該重心の位置に一致するというのは、より詳しくは、定常歩容の各周期の始端時刻での重心を、該周期での支持脚の足部の位置に設定したグローバル座標系（支持脚座標系）で見た場合における該重心の位置と、定常歩容の各周期の終端時刻での重心を、該周期の次の周期での支持脚の足部の位置に設定したグローバル座標系（支持脚座標系）で見た場合における当該重心の位置とが互いに一致するということを意味する。

そして、上記の如く決定した目標ＺＭＰ軌道に応じて前記式（１）に基づいて規定される倒立振子質点６０の位置の軌道（重心の位置の軌道）から定常歩容初期発散成分↑q_targetが決定される。

なお、定常歩容初期発散成分↑q_targetを求める具体的な手法は、例えば前記特許文献１、あるいは、特許４８０５８１８号等に詳細に説明されている。このため、本明細書での詳細な説明は省略する。

補足すると、目標ＺＭＰのＸ軸方向の成分に関する(ｅ＋１)・(ｎ＋１）個の係数Wx_i,jと、Ｙ軸方向の成分に関する(ｅ＋１)・(ｎ＋１）個の係数Wy_i,jとを合わせた２・(ｅ＋１)・(ｎ＋１）個の係数Wx_i,j，Wy_i,jを並べたベクトルを↑Wと表記した場合、発散成分条件を表す線形等式としての上記式（８）の第１成分（第１行）及び第２成分（第２行）のそれぞれの式は、↑W^T・↑α＝Ｃという形の線形等式で表現することもできる。ここで、↑αは↑W^Tの成分の線形結合における係数ベクトル、Ｃは定数である。例えば、↑αを、Wx_i,jに係る成分がａ_i,j、Wy_i,jに係る成分がゼロである係数ベクトル、Ｃ＝C1xとすれば、式（８）の第１成分を↑W^T・↑α＝Ｃという形の線形等式で表現することができる。

次に、前記経由点条件は、より詳しく言えば、目標ＺＭＰ軌道が、あらかじめ定められた所定の時刻tfにおいて、あらかじめ定められた所定位置の経由点（床面上の点）を通るという条件である。なお、かかる経由点は、任意に設計的に設定し得る。また、経由点は、複数であってもよい。

例えば、遊脚側の足部１８の着地のタイミング、あるいは、離地のタイミングでの経由点を設定することが望ましい。該経由点は、当該着地のタイミング又は離地のタイミングでの足部１８の接地面（床との接触面）の中央位置に設定することが好ましい。

上記経由点条件は、目標ＺＭＰ軌道上で、所定の時刻tfにおける目標ＺＭＰベクトル↑Pzmp(t)の値が、各経由点に対応してあらかじめ定めた所定値↑Pzmp(tf)（＝[Xzmp(tf), Yznp(tf)]^T）となるという条件として表現し得る。従って、時刻ｔfが区間ｉfの時刻であるとすると、経由点条件は、次式（９）の線形等式で表される。

そこで、目標ＺＭＰ軌道生成部５３は、あらかじめ定められた各経由点に対応する↑Pzmp(tf)の値（定数値）と、時刻tfとを用いて、上記式（９）の第１成分及び第２成分の式を、経由点条件を表す線形等式として設定する。

補足すると、発散成分条件を表す線形等式に関して補足説明した場合と同様に、２・(ｅ＋１)・(ｎ＋１）個の係数Wx_i,j，Wy_i,jを並べたベクトル↑Ｗを用いると、経由点条件を表す線形等式としての上記式（９）の第１成分及び第２成分の式は、それぞれ、↑W^T・↑α＝Ｃという形の線形等式で表現することもできる。

次に、軌道接続条件は、本実施形態では、より詳しくは、区間ｉ−１（ｉ＝１，２，…，ｅ）での目標ＺＭＰ軌道と区間ｉでの目標ＺＭＰ軌道とが滑らかにつながるという条件である。このような軌道接続条件は、区間ｉ−１での目標ＺＭＰ軌道のうちの該区間ｉ−１の終端時刻（時刻t_i）での目標ＺＭＰの位置と該目標ＺＭＰの位置の所定階数の微分値とのそれぞれが、区間ｉの始端時刻（時刻t_i）での目標ＺＭＰの位置と該目標ＺＭＰの位置の所定階数の微分値とのそれぞれに一致するという条件として表現し得る。

本実施形態では、上記所定階数の微分値は、一例として、ＺＭＰの移動速度（目標ＺＭＰの位置の１階微分値）と、移動加速度（目標ＺＭＰの位置の２階微分値）との２つである。

この場合、隣り合う区間ｉ−１，ｉのうちの先行側の区間ｉ−１と後行側の区間ｉとの境界（ｉ＝１，２，…，ｅ）のそれぞれでの目標ＺＭＰの位置、移動速度及び移動加速度についての軌道接続条件は、それぞれ、次式（１０ａ）、（１０ｂ）、（１０ｃ）により表される。

さらに、区間ｉ−１の終端時刻ｔiに対応する正規化時刻がｔc_i-1(t_i)＝１、区間ｉの始端時刻ｔiに対応する正規化時刻ｔc_i(t_i)＝０であること、並びに、０⁰（ゼロのゼロ乗）＝１であることを考慮すると、式（１０ａ）〜（１０ｃ）は、それぞれ、次式（１１ａ）〜（１１ｃ）に書き換えられる。

そこで、目標ＺＭＰ軌道生成部５３は、区間ｉ−１と区間ｉとの境界（ｉ＝１，２，…，ｅ）のそれぞれ毎に、各区間ｉのあらかじめ定められた時間幅Ｔ_i（ｉ＝０，１，…，ｅ）の設定値を用いて、上記式（１１ａ），（１１ｂ），（１１ｃ）を軌道接続条件を表す線形等式として設定する。

補足すると、発散成分条件を表す線形等式に関して補足説明した場合と同様に、２・(ｅ＋１)・(ｎ＋１）個の係数Wx_i,j，Wy_i,jを並べたベクトル↑Ｗを用いると、軌道接続条件を表す線形等式としての上記式（１１ａ），（１１ｂ），（１１ｃ）のそれぞれの第１成分及び第２成分の式は、それぞれ、↑W^T・↑α＝Ｃという形の線形等式で表現することもできる。

なお、軌道接続条件の要素として使用するＺＭＰの位置の所定階数の微分値は、ＺＭＰの移動速度及び移動加速度の両方に限られるものではない。例えば、移動加速度を軌道接続条件の要素から除外したり、あるいは、ＺＭＰの位置の３階微分値を移動接続条件の要素として用いることも可能である。

次に、ＺＭＰ存在許容領域条件は、より詳しくは、軌道生成対象期間の任意の時刻での目標ＺＭＰが、当該時刻でのロボット１の接地面を包含する最小の凸領域に一致もしくは近似する支持多角形内に存在するという条件である。

なお、上記凸領域は、片脚支持期では、支持脚としての脚リンク機構３の足部１８の接地面により形成される領域であり、両脚支持期では、両方の脚リンク機構３Ｒ，３Ｌのそれぞれの足部１８Ｒ，１８Ｌの接地面の両方を含む領域である。

本実施形態では、脚リンク機構３Ｒ，３Ｌのうちの一方の脚リンク機構３の足部１８だけが接地する片脚支持期での支持多角形は、例えば図７Ａで示すように、四角形で表し、両方の脚リンク機構３Ｒ，３Ｌの足部１８Ｒ，１８Ｌが接地する両脚支持期での支持多角形は、例えば図７Ｂで示すように六角形で表す。

ここで、図７Ａの点描領域は、片脚支持期での足部１８（図示例では１８Ｒ）の接地面の一例を表し、図７Ｂの点描領域は、両脚支持期での足部１８Ｒ，１８Ｌのそれぞれの接地面の一例を表している。これらの図示例では、支持多角形は、上記最小の凸領域に一致もしくはほぼ一致している。ただし、該支持多角形は、該凸領域の内側の多角形領域であってもよい。

かかる支持多角形の各辺は、グローバル座標系のＸＹ座標平面に投影して見たとき、次式（１２）により表される。

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
又は
Cx_m・Xzmp＋Cy_m・Yzmp＞ｄ_m
……（１３−m）
ただし、ｍ＝１，２，…，Ｍ

ここで、軌道生成対象期間の各区間ｉにおける目標ＺＭＰベクトル↑Pzmp_i＝[Xzmp_i，Yzmp_i]^Tは、前記式（２）により表される。従って、軌道生成対象期間の任意の区間ｉの任意の時刻ｔ（正規化時刻では、時刻tc_i(t)）おけるＺＭＰ存在許容領域条件は、次のＭ個の線形不等式（１４−ｍ）（ｍ＝１，２，…，Ｍ）により表現される。

この線形不等式（１４−ｍ）は、総計２・(ｎ＋１)個の係数Ｗx_i,j，Ｗy_i,j（ｊ＝０，１，…，ｎ）の線形結合についての線形不等式である。

そこで、目標ＺＭＰ軌道生成部５３は、式（１４−ｍ）により表される不等式を、ＺＭＰ存在許容領域条件を表す線形不等式として設定する。。

より具体的には、ＺＭＰ軌道生成部５３は、まず、軌道生成対象期間に複数のサンプリング時刻（例えば所定の刻み時間毎の時刻）を設定し、その各サンプリング時刻での支持多角形の各辺の式（式（１２））を、該サンプリング時刻での足部１８Ｒ，１８Ｌの目標位置姿勢に基づいて設定する。この場合、各サンプリング時刻での支持多角形の各辺は、その時刻での足部１８Ｒ，１８Ｌの接地面を包含する最小の凸領域（これは、該時刻での足部１８Ｒ，１８Ｌの目標位置姿勢により規定される）の境界に一致又は近接するように設定される。

なお、軌道生成対象期間における複数のサンプリング時刻の設定については、例えば各区間ｉに含まれるサンプリング時刻の個数が、いずれの区間ｉでも同数となるようにサンプリング時刻を設定してもよい。

そして、ＺＭＰ軌道生成部５３は、各サンプリング時刻毎に、その時刻ｔの値と、該時刻に対応して設定した支持多角形の各辺の式を表す定数Cx_m，Cy_m，d_mの設定値とを用いて、上記（１４−ｍ）により表されるＭ個の線形不等式を、ＺＭＰ存在許容領域条件を表す線形不等式として設定する。

補足すると、発散成分条件を表す線形等式に関して補足説明した場合と同様に、２・(ｅ＋１)・(ｎ＋１）個の係数Wx_i,j，Wy_i,jを並べたベクトル↑Ｗを用いると、ＺＭＰ存在許容領域条件を表す線形不等式としての上記式（１４−ｍ）（ｍ＝１，２，…，Ｍ）は、それぞれ、↑W^T・↑α＜Ｃ又は↑W^T・↑α＞Ｃという形の線形不等式で表現することもできる。

なお、本実施形態では、片脚支持期での支持多角形を四角形、両脚支持期での支持多角形を六角形とした。ただし、片脚支持期及び両脚支持期の両方又は一方の支持多角形として、上記と異なる角数の多角形を用いてもよい。目標ＺＭＰ軌道の自由度を高める上では、支持多角形は、前記凸領域の面積にできるだけ近い面積を有する多角形が好ましい。

次に、目標係数群に含まれる係数Wx_i,j、Wy_i,jの二乗値を変数として含む二次形式の評価関数を説明する。

本実施形態では、目標係数群を二次計画問題の解法手法のアルゴリズムにより決定するための評価関数（目的関数）として、目標ＺＭＰの急激な変化が生じるのを極力防止するために、目標ＺＭＰベクトル↑Pzmp＝[Xzmp，Yzmp]^Tの変化加速度↑Azmp（＝d²↑Pzmp/dt²＝[d²Xzmp/dt²，d²Yzmp/dt²]^T）の二乗値(↑Azmp)²（＝↑Azmp^T・↑Azmp）を積分してなる関数を用いる。以降、↑AzmpをＺＭＰ加速度と称し、その二乗値(↑Azmp)²（＝↑Azmp^T・↑Azmp）をＺＭＰ二乗加速度と称する。

ここで、軌道生成対象期間の各区間ｉ（ｉ＝０，１，…，ｅ）の任意の時刻ｔでのＺＭＰ加速度↑Azmp_i(t)（＝[Axzmp_i(t)，Ayzmp_i(t)]^T）は、前記式（２）を用いて、次式（１５）により表される。

そして、ＺＭＰ加速度↑Azmp_i(t)の各成分Axzmp_i(t)，Ayzmp_i(t)はスカラーであるので、各成分の二乗値(Axzmp_i(t))²，(Ayzmp_i(t))²は、次式（１６ａ），（１６ｂ）により表される。

(Axzmp_i(t))²＝↑Wx_i^T ・↑h(tc_i(t)) ・ h(tc_i(t))^T ・↑Wx_i
＝↑Wx_i^T ・Ｈd_m(tc_i(t)) ・↑Wx_i ……（１６ａ）
(Ayzmp_i(t))²＝↑Wy_i^T ・↑h(tc_i(t)) ・ h(tc_i(t))^T ・↑Wy_i
＝↑Wy_i^T ・Ｈ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))²，(Ayzmp_i(t))²のそれぞれの積分値(時刻t_iから時刻t_i+1までの積分値)は、次式（１７ａ），（１７ｂ）により表される。

ここで、式（１５）のただし書き、式（１６ａ），（１６ｂ）のただし書き、及び式（１７ａ），（１７ｂ）のただし書きから明らかなように、H_m(T_i)は、その各成分が区間ｉの時間幅Ｔ_iに応じた定数値又はゼロとなる行列である。

また、任意の時刻でのＺＭＰ二乗加速度(↑Azmp_i(t))²は、ＺＭＰ加速度のＸ軸方向成分(Axzmp_i(t))²とＹ軸方向成分(Ayzmp_i(t))²との総和（＝(Axzmp_i(t))²＋(Ayzmp_i(t))²）である。

従って、軌道生成対象期間の全体でのＺＭＰ二乗加速度(↑Azmp(t))²の積分値Ｆ（時刻t_0から時刻t_e+1までの積分値）は、次式（１８）により表される。

なお、↑Wは２・(ｅ＋１)・(ｎ＋１）個の成分から成るベクトル（これは、前記発散成分条件を表す線形等式に関して補足説明したベクトル↑Wに相当する）、Hh_mは、２・(ｅ＋１)・(ｎ＋１）次の正方行列、Hs_mは、(ｅ＋１)・(ｎ＋１）次の正方行列である。

このように式（１８）により定義されるＺＭＰ二乗加速度(↑Azmp(t))²の積分値Ｆは、目標ＺＭＰベクトル↑Pzmpの各成分の軌道を多項式関数で表した場合の目標係数群に含まれる２・(ｅ＋１)・(ｎ＋１）個の係数Wx_i,j，Wy_i,j（ｉ＝０，１，…，ｅ、ｊ＝０，１，…，ｎ）の二乗値を変数として含む二次形式（Wx_i,j，Wy_i,jについての二次形式）の関数となる。

そこで、本実施形態では、ＺＭＰ軌道生成部５３は、上記式（１８）により定義されるＺＭＰ二乗加速度(↑Azmp(t))²の積分値Ｆを、２次計画問題における評価関数Ｆ（目的関数）として設定する。

この場合、評価関数Ｆにおける行列Hs_mの各成分の値は、軌道生成対象期間の各区間ｉ（ｉ＝０，１，…，ｅ）の時間幅Ｔ_iの設定値を用いて算出される。

ＺＭＰ軌道生成部５３は、以上の如く、発散成分条件（安定化条件）、経由点条件及び軌道接続条件をそれぞれ表す線形等式と、ＺＭＰ存在許容領域条件を表す線形不等式と、二次形式の評価関数Ｆとを設定する。

そして、ＺＭＰ軌道生成部５３は、これらの評価関数Ｆと、線形等式及び線形不等式と用いて、多項式関数係数群決定部５３ａの処理を実行することで、目標ＺＭＰベクトルの軌道を表す多項式関数の係数Wx_i,j，Wy_i,j（ｉ＝０，１，…，ｅ、ｊ＝０，１，…，ｎ）の目標値により構成される目標係数群を決定する。

この場合、多項式関数係数群決定部５３ａは、上記線形等式及び線形不等式を係数Wx_i,j，Wy_i,jの値に関する制約条件として用いて、評価関数Ｆの値を最小化するように、目標係数群を決定する処理を、２次計画問題の解法問題の公知のアルゴリズムにより実行する。このアルゴリズムとしては、例えば内点法、有効制約法等を使用することができる。

これにより、軌道生成対象期間の全ての区間ｉ（ｉ＝０，１，…，ｅ）のそれぞれの目標ＺＭＰベクトル↑Pzmpの軌道（目標ＺＭＰのＸ軸方向位置XzmpとＹ軸方向位置Yzmpのそれぞれの軌道）を時間の関数として表す多項式関数が決定されることとなる。

以上が、本実施形態におけるＺＭＰ軌道生成部５３の処理の詳細である。

以上説明した本実施形態によれば、目標ＺＭＰ軌道を多項式関数により表すことで、目標ＺＭＰ軌道に関する複数の制約条件（具体的には、前記安定化条件、経由点条件、軌道接続条件、ＺＭＰ存在許容領域条件）を、多項式関数の係数Wx_i,j，Wy_i,j（ｉ＝０，１，…，ｅ、ｊ＝０，１，…，ｎ）についての線形等式又は線形不等式により表すことができる。

そして、例えば、目標ＺＭＰベクトル↑Azmpの二乗値（ＺＭＰ二乗加速度）の積分値を評価関数Ｆとして用いることで、該評価関数Ｆを目標ＺＭＰ軌道を表す多項式関数の係数Wx_i,j，Wy_i,j（ｉ＝０，１，…，ｅ、ｊ＝０，１，…，ｎ）に関する二次形式の関数として表現することができる。

このため、二次計画問題の解法手法のアルゴリズムによって、線形等式及び線形不等式により表される複数の制約条件を満たしつつ、上記評価関数の値を最小化するように多項式関数の目標係数群を決定することができる。ひいては、当該複数の制約条件の全てを満足し得る目標ＺＭＰ軌道を効率よく生成することができる。

また、当該複数の制約条件を満足し得る目標ＺＭＰ軌道を多項式関数により滑らかな曲線形状に生成できる。さらに、軌道生成対象期間が複数の区間ｉに分割され、各区間ｉの目標ＺＭＰ軌道が各区間ｉ毎に個別の多項式関数により表される。このため、支持多角形の領域を幅広く使用し得るように、高い自由度で目標ＺＭＰ軌道を生成することができる。

ひいては、目標歩容生成部４２で生成し得る目標歩容の自由度が高まり、ロボット１の種々様々な動作要求を満たし得る目標歩容を適切に生成できる。

また、本実施形態では、目標ＺＭＰベクトル↑Azmpの二乗値（ＺＭＰ二乗加速度）の積分値を評価関数Ｆとして用いることで、目標ＺＭＰの急激な変化を生じ難い滑らかな目標ＺＭＰ軌道を生成できる。ひいては、ロボット１の移動動作を滑らかに行い得る目標歩容を生成できる。

また、本実施形態では、各区間ｉの時間を正規化してなる正規化時刻ｔc_iの関数として、各区間ｉにおける多項式関数が表現される。このため、特に目標ＺＭＰ軌道に関する制約条件のうち、特に前記軌道接続条件の導出を簡素化することができる。

本実施形態により生成される目標ＺＭＰ軌道の実施例を図８及び図９を参照して説明する。

図８は、ロボット１が、足部１８Ｒ，１８Ｌを左右方向に並列させて起立した状態（時刻t0）から歩行を開始する目標歩容を生成する場合に目標ＺＭＰ軌道生成部５３により生成される目標ＺＭＰ軌道（Ｘ軸方向位置Xzmp及びＹ軸方向位置Yzmpの軌道）の実施例を実線のグラフで示す図である。

また、図８中の破線のグラフは、実線で示した目標ＺＭＰ軌道を用いて前記目標重心軌道生成部５４により生成されるロボット１の目標重心軌道（Ｘ軸方向位置Xpend及びＹ軸方向位置Ypendの軌道）の実施例を示している。

同図に示されるように、本実施形態によれば、目標ＺＭＰを滑らかな曲線形状で生成することができることが判る。また、ロボット１の重心（倒立振子質点６０）が位置（ＸＹ座標平面上の位置）が目標ＺＭＰの位置（ＸＹ座標平面上の位置）から大きく乖離することないように、滑らかな軌道でロボット１の目標重心軌道を生成することができることが判る。

図９は、ロボット１が足部１８Ｒ，１８Ｌを左右方向（Ｙ軸方向）に並列させて起立した状態（時刻t0）から、すばやく（０．１秒の予備動作で）遊脚側の足部１８Ｌの空中移動を開始して、ロボット１の歩行を行った場合に目標ＺＭＰ軌道生成部５３により生成される目標ＺＭＰのＹ軸方向位置Yzmpの軌道の実施例を実線のグラフで示す図である。

また、図９中の破線のグラフは、比較例における目標ＺＭＰのＹ軸方向位置Yzmpの軌道を示している。この比較例は、特許文献１に記載されている如く、基準の目標ＺＭＰ軌道を台形状の補正パターンで補正することにより生成される折れ線状の目標ＺＭＰ軌道である。この場合、この比較例では、支持多角形による目標ＺＭＰの存在位置の制限は行われていない。

なお、図９中のＹＬ、ＹＲはそれぞれ、片脚支持期での足部１８Ｌ，１８ＲのＹ軸方向位置（足部１８の幅方向の中央位置）を示している。また、図９中の二点鎖線ａ１，ａ２の間の範囲は、支持多角形により規定される目標ＺＭＰのＹ軸方向位置の存在許容範囲の例を示している。

図９に示される比較例では、最初の片脚支持期（足部１８Ｌを接地させる片脚支持期）での目標ＺＭＰのＹ軸方向位置Yzmpの軌道が、足部１８Ｌの位置YLに対してＹ軸方向に比較的大きく変位する。このため、目標ＺＭＰのＹ軸方向位置Yzmpが足部１８Ｌの接地面により規定される支持多角形からはみ出す場合がある。

これに対して、図９に示される実施例では、歩行開始当初の両脚支持期において、目標ＺＭＰのＹ軸方向位置Yzmpが比較的大きく変位するようにYzmpの軌道が生成されることで、最初の片脚支持期での目標ＺＭＰのＹ軸方向位置Yzmpの軌道が、足部１８Ｌの位置YLに対してＹ軸方向に小さめに変位するように生成される。このため、目標ＺＭＰのＹ軸方向位置Yzmpが足部１８Ｌの接地面により規定される支持多角形からはみ出すことがない（前記ＺＭＰ存在許容領域条件が満たされる）。なお、両脚支持期での支持多角形は、Ｙ軸方向に幅広いので、歩行開始当初の両脚支持期においても目標ＺＭＰのＹ軸方向位置Yzmpが支持多角形からはみ出すことがない（前記ＺＭＰ存在許容領域条件が満たされる）。

このように本実施形態によれば、前記ＺＭＰ存在許容領域条件を満たす目標ＺＭＰ軌道を適切に生成することができる。

なお、本発明は、以上説明した実施形態に限定されるものではなく、上記実施形態以外の態様も採用し得る。以下に、いくつかの変形態様を説明する。

前記実施形態では、ロボット１の２脚歩容の形態で移動させる場合を一例として説明した。ただし、例えば、ロボット１の脚リンク機構３Ｒ，３Ｌの足部１８Ｒ，１８Ｌに加えて、腕リンク機構４Ｌ，４Ｒのハンド部３０Ｒ，３０Ｌを移動接地部として用いることで、４脚歩容の形態でロボット１の移動を行う場合でも、本発明を適用できる。

また、本発明における移動ロボットは、２つの脚リンク機構３Ｒ，３Ｌを有するロボット１に限らず、３つ以上の脚リンク機構を有するロボットであってもよい。

また、前記実施形態では、目標ＺＭＰ軌道に関する制約条件として、前記安定化条件、経由点条件、軌道接続条件、ＺＭＰ存在許容領域条件を用いたが、いずれかの条件を省略したり、あるいは、さらに多くの制約条件を追加してもよい。

例えば経由点条件を省略してもよい。また、軌道生成対象期間を複数の区間ｉに分割しない場合には、軌道接続条件は不要である。

また、前記実施形態では、評価関数Ｆとして、ＺＭＰ二乗加速度の積分値を用いた。ただし、評価関数は、目標ＺＭＰ軌道を表す多項式関数の係数についての二次形式の関数として表すことができるものでれば、他の評価関数を用いてもよい。例えば、目標ＺＭＰの移動速度の積分値などを評価関数を用いることも可能である。

また、前記実施形態では、目標ＺＭＰ軌道を表す多項式関数として７次（ｎ＝７）の多項式関数を用いた。ただし、該多項式関数の次数ｎは７次に限られず、１０次など、その他の次数であってもよい。

なお、本実施形態の如く、軌道接続条件として、目標ＺＭＰの位置、移動速度、及び移動加速度に関する制約条件を用いた場合には、多項式関数の次数ｎは、５次以上であることが好ましい。

１…移動ロボット、５３…目標ＺＭＰ軌道生成部（目標ＺＭＰ軌道生成装置）、５３ａ…多項式関数係数群決定部（多項式関数係数群決定手段）。

Claims (8)

1. 移動ロボットのＺＭＰ（Zero Moment Point）の目標位置の軌道である目標ＺＭＰ軌道を生成する装置であって、
   前記目標ＺＭＰ軌道を、時間を変数とする複数次数の多項式関数により表現される軌道であるとして、該多項式関数の各項の係数の目標値から成る目標係数群を決定する多項式関数係数群決定手段を備えており、
   該多項式関数係数群決定手段は、前記目標係数群に含まれる係数の二乗値を変数として含むように構成される二次形式の評価関数と、該係数についての線形等式又は線形不等式により各々構成される複数の制約条件とを用い、二次計画問題の解法手法により前記制約条件を満たしつつ、前記評価関数の値を最小化するように前記目標係数群を決定するように構成されており、
   前記制約条件は、前記目標ＺＭＰ軌道における１つ以上のサンプリング時刻でのＺＭＰの目標位置が、該サンプリング時刻での移動ロボットの接地部位の目標位置及び目標姿勢に応じて設定した所定の多角形領域内に存在するという第１制約条件を表す線形不等式を含むように構成されていることを特徴とする移動ロボットの目標ＺＭＰ軌道の生成装置。
2. 請求項１記載の移動ロボットの目標ＺＭＰ軌道の生成装置において、
   前記制約条件は、前記移動ロボットの重心の運動を、該重心に質点を有し、且つ前記目標ＺＭＰ軌道上のＺＭＰを支点として有する倒立振子質点の運動により表現するモデルにおける該倒立振子質点を安定状態に保つための条件であって、将来の所定時刻における該倒立振子質点の運動状態が所定の運動状態に一致するという第２制約条件を表す線形等式をさらに含むように構成されていることを特徴とする移動ロボットの目標ＺＭＰ軌道の生成装置。
3. 請求項２記載の移動ロボットの目標ＺＭＰ軌道の生成装置において、
   前記将来の所定時刻における倒立振子質点の運動状態は、該倒立振子質点の位置と移動速度との線形結合により表される運動状態であることを特徴とする移動ロボットの目標ＺＭＰ軌道の生成装置。
4. 請求項１〜３のいずれか１項に記載の移動ロボットの目標ＺＭＰ軌道の生成装置において、
   前記制約条件は、前記目標ＺＭＰ軌道のうちの所定の時刻でのＺＭＰの目標位置が、所定位置に一致するという第３制約条件を表す線形等式をさらに含むように構成されていることを特徴とする移動ロボットの目標ＺＭＰ軌道の生成装置。
5. 請求項１〜４のいずれか１項に記載の移動ロボットの目標ＺＭＰ軌道の生成装置において、
   前記目標ＺＭＰ軌道は、複数の区間の軌道に分割されていると共に各区間毎に個別の多項式関数により表される軌道であり、
   前記多項式関数係数群決定手段が決定する目標係数群は、前記複数の区間の全ての多項式関数の各項の係数の目標値からなることを特徴とする移動ロボットの目標ＺＭＰ軌道の生成装置。
6. 請求項５記載の移動ロボットの目標ＺＭＰ軌道の生成装置において、
   前記制約条件は、前記複数の区間で互いに隣合う任意の２つの区間のうち、先行側の区間の多項式関数により規定される該先行側の区間の終端時刻におけるＺＭＰの位置と該ＺＭＰの位置の所定階数の微分値とのそれぞれと、後行側の区間の多項式関数により規定される該後行側の区間の始端時刻におけるＺＭＰの位置と該ＺＭＰの位置の所定階数の微分値とのそれぞれとが互いに一致するという第４制約条件を表す線形等式をさらに含むように構成されていることを特徴とする移動ロボットの目標ＺＭＰ軌道の生成装置。
7. 請求項５又は６記載の移動ロボットの目標ＺＭＰ軌道の生成装置において、
   前記複数の区間のそれぞれにおける多項式関数は、該区間の時間幅をあらかじめ定めた一定値とみなし、且つ、該区間の始端時刻をゼロとみなして該区間での時間を正規化してなる時間の関数として表現してなる多項式関数であることを特徴とする移動ロボットの目標ＺＭＰ軌道の生成装置。
8. 請求項１〜７のいずれか１項に記載の移動ロボットの目標ＺＭＰ軌道の生成装置において、
   前記評価関数は、前記目標ＺＭＰ軌道におけるＺＭＰの移動加速度の二乗値の積分値を表す関数であることを特徴とする目標ＺＭＰ軌道の生成装置。