特許 6228079 移動ロボットの動作目標生成装置

(19)【発行国】日本国特許庁(JP)

(12)【公報種別】特許公報(B2)

(11)【特許番号】6228079

(24)【登録日】2017年10月20日

(45)【発行日】2017年11月8日

(54)【発明の名称】移動ロボットの動作目標生成装置

(51)【国際特許分類】

   B25J 5/00 20060101AFI20171030BHJP

【ＦＩ】

   B25J5/00 E

【請求項の数】4

【全頁数】30

(21)【出願番号】特願2014-146322(P2014-146322)

(22)【出願日】2014年7月16日

(65)【公開番号】特開2016-22540(P2016-22540A)

(43)【公開日】2016年2月8日

【審査請求日】2016年11月29日

(73)【特許権者】

【識別番号】000005326

【氏名又は名称】本田技研工業株式会社

(74)【代理人】

【識別番号】110000800

【氏名又は名称】特許業務法人創成国際特許事務所

(72)【発明者】

【氏名】金澤 雅夫

(72)【発明者】

【氏名】渡部 智樹

(72)【発明者】

【氏名】上岡 拓未

(72)【発明者】

【氏名】淺谷 南己

【審査官】 永冨 宏之

(56)【参考文献】

【文献】 特開２０１５−１５０６７８（ＪＰ，Ａ）

【文献】 特開２０１３−１１６５２９（ＪＰ，Ａ）

【文献】 特開２０１３−１８４２４８（ＪＰ，Ａ）

【文献】 特開２０１１−１８３５２７（ＪＰ，Ａ）

【文献】 特開２００７−１８５７５６（ＪＰ，Ａ）

(58)【調査した分野】（Int.Cl.，ＤＢ名）

Ｂ２５Ｊ ５／００

(57)【特許請求の範囲】

【請求項1】

基体から延設された脚リンクを含む複数の可動リンクを有し、各可動リンクの先端部が、該先端部と前記基体との間で該可動リンクに設けられた関節の作動によって前記基体に対して動くように構成された移動ロボットの各関節の動作制御のための制御用目標値を所定の制御処理周期で逐次生成する動作目標生成装置であって、
各制御処理周期において、各可動リンクの先端部の運動状態の現在時刻以後の目標値の予測軌道である可動リンク先端部予測軌道を設定する可動リンク先端部予測軌道設定手段と、
各制御処理周期において、前記移動ロボットの全体の運動量の現在時刻以後の目標値の予測軌道である全体運動量予測軌道を設定する全体運動量予測軌道設定手段と、
各制御処理周期において、前記設定された可動リンク先端部予測軌道及び全体運動量予測軌道を実現し得るように、現在時刻以後の前記移動ロボットの全体の運動状態の目標値の予測軌道である全体運動状態予測軌道を暫定的に決定する全体運動状態予測軌道決定手段と、
各制御処理周期において、前記各可動リンクの各関節の運動状態に関する所定の制約条件であって、前記全体運動状態予測軌道決定手段の処理で使用されていない関節運動制約条件と、現在時刻以後の複数の時刻における前記移動ロボットの所定種類の状態量に関する所定の評価関数とを用いて２次計画問題の解法手法の演算処理を実行することにより、前記関節運動制約条件を満たす範囲内で、前記所定の評価関数の値を最小化するように、前記複数の時刻における前記基体及び各可動リンクの先端部のそれぞれの運動加速度を算出し、前記複数の時刻のうちの初期時刻における前記基体及び各可動リンクの先端部のそれぞれの運動加速度の算出値に基づいて、前記基体及び各可動リンクの先端部のそれぞれの新たな目標運動加速度を決定する運動加速度決定手段と、
前記決定された目標運動加速度を実現するように各制御処理周期における移動ロボットの各関節の制御用目標値を決定する関節制御用目標値決定手段とを備え、
前記所定の評価関数は、前記複数の時刻における前記運動加速度の算出値により規定される前記基体及び各可動リンクの先端部のそれぞれの運動速度と前記全体運動状態予測軌道により規定されるヤコビアン行列とを用いて算出される前記複数の時刻における前記所定種類の状態量の時系列と前記全体運動状態予測軌道により規定される前記複数の時刻における前記所定種類の状態量の時系列との一致度合に応じて値が変化するように構成された評価関数であり、
前記関節運動制約条件は、前記複数の時刻における前記運動加速度の算出値により規定される前記基体及び各可動リンクの先端部のそれぞれの運動速度と前記全体運動状態予測軌道により規定されるヤコビアン行列とを用いて算出される前記各可動リンクの各関節の運動状態の可変範囲を規定する不等式により表される制約条件であることを特徴とする移動ロボットの動作目標生成装置。

【請求項2】

請求項１記載の移動ロボットの動作目標生成装置において、
前記所定種類の状態量は、少なくとも前記移動ロボットの全体の運動量を含み、前記所定の評価関数は、前記複数の時刻における前記運動加速度の算出値により規定される前記基体及び各可動リンクの先端部のそれぞれの運動速度と前記全体運動状態予測軌道により規定されるヤコビアン行列とを用いて算出される前記複数の時刻における前記移動ロボットの全体の運動量の時系列と、前記全体運動量予測軌道により規定される前記複数の時刻における当該運動量の目標値の時系列との一致度合いを示す運動量評価指標値に応じて値が変化するように構成されていることを特徴とする移動ロボットの動作目標生成装置。

【請求項3】

請求項２記載の移動ロボットの動作目標生成装置において、
前記所定種類の状態量は、前記基体及び各可動リンクの先端部のそれぞれの運動加速度をさらに含み、
前記所定の評価関数は、前記複数の時刻における前記基体及び各可動リンクの先端部のそれぞれの運動加速度の算出値の時系列と前記全体運動状態予測軌道により規定される、前記複数の時刻における当該運動加速度の時系列との一致度合を示す運動加速度評価指標値と、前記運動量評価指標値とに応じて値が変化するように構成されていることを特徴とする移動ロボットの動作目標生成装置。

【請求項4】

請求項２又は３記載の移動ロボットの動作目標生成装置において、
前記全体運動量予測軌道設定手段は、前記移動ロボットの現在時刻以後の目標ＺＭＰの予測軌道を設定する手段を含んでおり、前記所定種類の状態量を構成する前記移動ロボットの全体の運動量は、前記移動ロボットの目標ＺＭＰの周りの角運動量を含むことを特徴とする移動ロボットの動作目標生成装置。

【発明の詳細な説明】

【技術分野】

【0001】

本発明は、脚リンクを有する移動ロボットの動作目標生成装置に関する。

【背景技術】

【0002】

２足歩行ロボット等の移動ロボットの動作目標を生成する装置は、従来より種々様々な装置が提案されている。例えば特許文献１には、移動ロボットの各関節の運動状態の制約条件（各関節の変位量及び変位速度の可変範囲の条件）を満たす範囲内で、移動ロボットの所要の運動（移動動作）を行い得るように、該移動ロボットの各関節の変位量を規定する目標歩容を所定の制御処理周期で逐次生成する技術が提案されている。

【先行技術文献】

【特許文献】

【0003】

【特許文献1】特開２０１３−１１６５２９号公報

【発明の概要】

【発明が解決しようとする課題】

【0004】

特許文献１に見られる如き従来の技術では、ロボットの動作目標を生成する各時刻毎（所定の制御処理周期毎）に、その時刻での動作目標が、各関節の運動状態の制約条件（以降、関節制約条件ということがある）を満たすものとなるように、各時刻での動作目標を生成するようにしている。

【0005】

このため、現状のパターンで継続的に動作目標を生成した場合に、近い将来に関節制約条件を満たすことができなくなる可能性が高い状況であっても、動作目標の生成パターンを事前に調整しておくことが困難である。

【0006】

その結果、動作目標の生成パターンの変動が生じやすくなって、該生成パターンの滑らかさ、ひいては、移動ロボットの動作の滑らかさが損なわれる虞があった。

【0007】

本発明はかかる背景に鑑みてなされたものであり、各関節の運動状態の制約条件を満たしつつ、移動ロボットを滑らかに動作させ得る動作目標を適切に生成することができる装置を提供することを目的とする。

【課題を解決するための手段】

【0008】

本発明の移動ロボットの動作目標生成装置は、かかる目的を達成するために、基体から延設された脚リンクを含む複数の可動リンクを有し、各可動リンクの先端部が、該先端部と前記基体との間で該可動リンクに設けられた関節の作動によって前記基体に対して動くように構成された移動ロボットの各関節の動作制御のための制御用目標値を所定の制御処理周期で逐次生成する動作目標生成装置であって、
各制御処理周期において、各可動リンクの先端部の運動状態の現在時刻以後の目標値の予測軌道である可動リンク先端部予測軌道を設定する可動リンク先端部予測軌道設定手段と、
各制御処理周期において、前記移動ロボットの全体の運動量の現在時刻以後の目標値の予測軌道である全体運動量予測軌道を設定する全体運動量予測軌道設定手段と、
各制御処理周期において、前記設定された可動リンク先端部予測軌道及び全体運動量予測軌道を実現し得るように、現在時刻以後の前記移動ロボットの全体の運動状態の目標値の予測軌道である全体運動状態予測軌道を暫定的に決定する全体運動状態予測軌道決定手段と、
各制御処理周期において、前記各可動リンクの各関節の運動状態に関する所定の制約条件であって、前記全体運動状態予測軌道決定手段の処理で使用されていない関節運動制約条件と、現在時刻以後の複数の時刻における前記移動ロボットの所定種類の状態量に関する所定の評価関数とを用いて２次計画問題の解法手法の演算処理を実行することにより、前記関節運動制約条件を満たす範囲内で、前記所定の評価関数の値を最小化するように、前記複数の時刻における前記基体及び各可動リンクの先端部のそれぞれの運動加速度を算出し、前記複数の時刻のうちの初期時刻における前記基体及び各可動リンクの先端部のそれぞれの運動加速度の算出値に基づいて、前記基体及び各可動リンクの先端部のそれぞれの新たな目標運動加速度を決定する運動加速度決定手段と、
前記決定された目標運動加速度を実現するように各制御処理周期における移動ロボットの各関節の制御用目標値を決定する関節制御用目標値決定手段とを備え、
前記所定の評価関数は、前記複数の時刻における前記運動加速度の算出値により規定される前記基体及び各可動リンクの先端部のそれぞれの運動速度と前記全体運動状態予測軌道により規定されるヤコビアン行列とを用いて算出される前記複数の時刻における前記所定種類の状態量の時系列と前記全体運動状態予測軌道により規定される前記複数の時刻における前記所定種類の状態量の時系列との一致度合に応じて値が変化するように構成された評価関数であり、
前記関節運動制約条件は、前記複数の時刻における前記運動加速度の算出値により規定される前記基体及び各可動リンクの先端部のそれぞれの運動速度と前記全体運動状態予測軌道により規定されるヤコビアン行列とを用いて算出される前記各可動リンクの各関節の運動状態の可変範囲を規定する不等式により表される制約条件であることを特徴とする
（第１発明）。

【0009】

なお、本発明において、前記基体の運動状態は、該基体の位置及び姿勢と、該位置及び姿勢の時間的変化率としての該基体の運動速度と、該基体の運動速度の時間的変化率としての運動加速度とのうちのいずれか１つ又は複数の状態量を意味する。このことは、各可動リンクの先端部の運動状態についても同様である。

【0010】

また、各関節の運動状態は、該関節の変位量と、該変位量の時間的変化率としての変位速度と、該変位速度の時間的変化率としての変位加速度とのうちのいずれか１つ又は複数の状態量を意味する。

【0011】

また、移動ロボットの全体の運動量は、該移動ロボットの全体の並進運動量と角運動量とのうちの一方又は両方の運動量を意味する。

【0012】

また、移動ロボットの全体の運動状態は、該移動ロボットの全ての部位の位置及び姿勢を規定し得る状態量（例えば、基体の位置及び姿勢と、移動ロボットの全ての関節の変位量との組等）を意味する。

【0013】

上記第１発明によれば、前記移動ロボットの各関節の制御用目標値（例えば各関節の目標変位量、目標変位速度等）を生成する各制御処理周期において、前記設定された可動リンク先端部予測軌道及び全体運動量予測軌道を実現し得るように、前記全体運動状態予測軌道が暫定的に決定される。

【0014】

この全体運動状態予測軌道は、現在時刻以後の可動リンク先端部予測軌道と全体運動量予測軌道とを実現し得るものである一方、各可動リンクの関節の運動状態に関する前記所定の制約条件（関節運動制約条件）を使用せずに（考慮せずに）決定されるものである。従って、該全体運動状態予測軌道は、前記所定の制約条件を満たすとは限らない。

【0015】

なお、このような全体運動状態予測軌道の決定手法としては、例えば分解運動量制御の手法等を採用できる。

【0016】

そして、前記２次計画問題の解法手法の演算処理によって、前記関節運動制約条件を満たす範囲内で、現在時刻以後の複数の時刻における移動ロボットの所定種類の状態量に関する所定の評価関数の値を最小化するように、現在時刻以後の複数の時刻における前記基体及び各可動リンクの先端部のそれぞれの運動加速度が算出される。

【0017】

上記所定の評価関数は、２次計画問題の解法手法の演算処理によって求められる前記複数の時刻における前記運動加速度の算出値（２次計画問題の解）の時系列に応じて規定される前記所定種類の状態量の値の時系列と、前記全体運動状態予測軌道により規定される前記所定種類の状態量の値の時系列との一致度合を評価するための関数であり、当該一致度合が高いほど、該評価関数の値が小さくなるものである。

【0018】

なお、前記所定種類の状態量は、移動ロボットの目的の動作を行う上で好適な制御用目標値を決定し得るように、あらかじめ実験、シミュレーション等に基づいて選定しておけばよい。

【0019】

ここで、本発明では、前記所定の評価関数は、より詳しくは、前記複数の時刻における前記運動加速度の算出値により規定される前記基体及び各可動リンクの先端部のそれぞれの運動速度と前記全体運動状態予測軌道により規定されるヤコビアン行列とを用いて算出される前記複数の時刻における前記所定種類の状態量の時系列と前記全体運動状態予測軌道により規定される前記複数の時刻における前記所定種類の状態量の時系列との一致度合に応じて値が変化するように構成された評価関数とされている。

【0020】

また、前記関節運動制約条件は、前記複数の時刻における前記運動加速度の算出値により規定される前記基体及び各可動リンクの先端部のそれぞれの運動速度と前記全体運動状態予測軌道により規定されるヤコビアン行列とを用いて算出される前記各可動リンクの各関節の運動状態の可変範囲を規定する不等式により表される制約条件とされている。

【0021】

このため、各制御処理周期における前記２次計画問題の解法手法の演算処理では、前記複数の時刻のそれぞれにおける前記所定種類の状態量と、前記各可動リンクの各関節の運動状態とを、前記全体運動状態予測軌道により規定される固定的なヤコビアン行列を用いて算出できる。従って、前記２次計画問題の解法手法の演算処理による前記運動加速度の算出処理を短時間で効率よく完了することができる。

【0022】

この場合、２次計画問題の解法手法の演算処理によって求められる前記複数の時刻における前記運動加速度の算出値により規定されるロボットの全体の運動状態の軌道と、前記全体運動状態予測軌道とは前記関節運動制約条件に起因して一致しない場合があるものの、前記複数の時刻のうちの現在時刻により近い側の時刻では、一般に両軌道で大きな相違は生じ難い。

【0023】

従って、前記複数の時刻のうちの現在時刻により近い側の時刻では、前記全体運動状態予測軌道により規定される固定的なヤコビアン行列を用いても、前記評価関数による評価対象の前記所定種類の状態量と、前記関節運動制約条件を適用する前記各可動リンクの各関節の運動状態とを、前記全体運動状態予測軌道により規定されるヤコビアン行列を用いて高い信頼性で算出することができる。

【0024】

また、前記複数の時刻での前記関節運動制約条件を満たすように、該複数の時刻での運動加速度（基体及び各可動リンクの先端部のそれぞれの運動加速度）が算出されるので、現在時刻以後の近い将来においても前記関節運動制約条件を満たすように当該運動加速度を算出できる。

【0025】

そして、各制御処理周期では、現在時刻以後の前記複数の時刻のうちの初期時刻（現在時刻寄りの時刻）における前記基体及び各可動リンクの先端部のそれぞれの運動加速度の算出値に基づいて、前記基体及び各可動リンクの先端部のそれぞれの新たな目標運動加速度が決定される。

【0026】

これにより、制御処理周期毎に、現在時刻以後の近い将来に、いずれかの可動リンクの関節の運動状態が前記関節運動制約条件を満たさなくなるような状況が発生しないようにしつつ、前記基体及び各可動リンクの先端部のそれぞれの新たな目標運動加速度を決定できる。また、その決定処理で、前記全体運動状態予測軌道により規定されるヤコビアン行列を使用することによって、当該決定処理を短時間で効率よく行うことができる。

【0027】

そして、第１発明では、上記の如く決定された目標運動加速度を実現するように各制御処理周期における移動ロボットの各関節の制御目標値が決定される。

【0028】

従って、第１発明によれば、制御処理周期毎に、現在時刻以後の近い将来に、いずれかの可動リンクの関節の運動状態が関節運動制約条件を満たさなくなるような状況が発生しないように各関節の制御目標値を決定できると共に、その決定処理を効率よく短時間で実行できる。

【0029】

よって、第１発明によれば、各関節の運動状態の制約条件（関節運動制約条件）を満たしつつ、移動ロボットを滑らかに動作させ得る動作目標を適切に生成することができる。

【0030】

本願発明者の各種実験、検討によれば、前記所定種類の状態量は、少なくとも前記移動ロボットの全体の運動量を含み、前記所定の評価関数は、前記複数の時刻における前記運動加速度の算出値により規定される前記基体及び各可動リンクの先端部のそれぞれの運動速度と前記全体運動状態予測軌道により規定されるヤコビアン行列とを用いて算出される前記複数の時刻における前記移動ロボットの全体の運動量の時系列と、前記全体運動量予測軌道により規定される前記複数の時刻における当該運動量の目標値の時系列との一致度合いを示す運動量評価指標値に応じて値が変化するように構成されていることが好ましい（第２発明）。

【0031】

なお、前記全体運動状態予測軌道は、前記全体運動量予測軌道を実現するものであるから、前記全体運動量予測軌道により規定される前記複数の時刻における前記移動ロボットの全体の運動量の目標値の時系列は、換言すれば、前記全体運動状態予測軌道により規定される前記複数の時刻における当該運動量の時系列に相当するものである。

【0032】

かかる第２発明によれば、前記各可動リンクの各関節の制約条件を満たす範囲内で、現在時刻以後の複数の時刻における前記運動量の時系列に関する一致度合ができるだけ高くなる（ひいては、前記評価関数の値が小さくなる）ように、前記基体及び各可動リンクの先端部の運動加速度を算出することができる。

【0033】

上記第２発明では、前記所定種類の状態量は、前記基体及び各可動リンクの先端部のそれぞれの運動加速度をさらに含み、前記所定の評価関数は、前記複数の時刻における前記基体及び各可動リンクの先端部のそれぞれの運動加速度の算出値の時系列と前記全体運動状態予測軌道により規定される、前記複数の時刻における当該運動加速度の時系列との一致度合を示す運動加速度評価指標値と、前記運動量評価指標値とに応じて値が変化するように構成されていることがより好ましい（第３発明）。

【0034】

この第３発明によれば、前記各可動リンクの各関節の制約条件を満たす範囲内で、現在時刻以後の複数の時刻における前記運動量の時系列に関する一致度合と、前記基体及び各可動リンクの先端部の運動加速度に関する一致度合との両方ができるだけ高くなる（ひいては、前記評価関数の値が小さくなる）ように、前記基体及び各可動リンクの先端部の運動加速度を算出することができる。

【0035】

また、上記第２発明又は第３発明では、前記全体運動量予測軌道設定手段は、前記移動ロボットの現在時刻以後の目標ＺＭＰの予測軌道を設定する手段を含んでおり、前記所定種類の状態量を構成する前記移動ロボットの全体の運動量は、前記移動ロボットの目標ＺＭＰの周りの角運動量を含むことが好ましい（第４発明）。

【0036】

なお、上記目標ＺＭＰは、ＺＭＰ（Zero Moment Point）の目標位置を意味する。

【0037】

上記第４発明によれば、動力学的に適切な動作を行うことができるように、移動ロボットの動作目標（各関節の動作制御のための関節制御用目標値）を決定できる。

【図面の簡単な説明】

【0038】

【図1】本発明の一実施形態における移動ロボットの概略構成を示す図。

【図2】実施形態の移動ロボットの制御に関する構成を示すブロック図。

【図3】図２に示す動作目標生成部の機能を示すブロック図。

【図4】図３に示す可動リンク先端部予測軌道設定部で設定する脚リンクの先端部の運動状態の予測軌道の例を示す図。

【図5】図３に示す全体運動量予測軌道設定部の処理で使用する動力学モデルを模式的に示す図。

【図6】（ａ），（ｂ）はそれぞれ実施例、比較例における可動リンク（脚リンク）の関節の変位量の経時変化を示すグラフ。

【発明を実施するための形態】

【0039】

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

【0040】

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

【0041】

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

【0042】

さらに、可動リンク３Ｌ，３Ｒ，４Ｌ，４Ｒを、それぞれ順番に、第１可動リンク，第２可動リンク、第３可動リンク、第４可動リンクと称し、任意の１つの可動リンクを第ｉ可動リンク（ｉ＝１，２，３，４）と称する場合がある。

【0043】

基体２は、ロボット１の上体に相当するリンク部分である。この基体２は、本実施形態では、下側基体１０と、この下側基体１０の上方に配置された上側基体１１との２つの要素リンクと、該下側基体１０及び上側基体１１を連結する関節１２とから構成されている。関節１２は、ヨー軸周り（Ｚ軸周り）の回転自由度を有する。以降、関節１２を基体関節１２ということがある。

【0044】

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

【0045】

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

【0046】

従って、各脚リンク３の先端部（足平部１８）は、本実施形態では、下側基体１０に対して、６自由度の運動自由度を有する。なお、上記の説明における各脚リンク３の関節１９〜２４の回転軸（ロール軸、ピッチ軸、ヨー軸）は、脚リンク３を上下方向に延在させた状態での回転軸を示している。また、Ｘ軸、Ｙ軸、Ｚ軸は、図１に示した３軸直交座標系の各座標軸である。

【0047】

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

【0048】

この場合、肩関節部２５は、ピッチ軸周り、ロール軸周り、及びヨー軸周りの回転自由度をそれぞれ有する３つの関節３１，３２，３３により構成されている。また、肘関節部２７は、ピッチ軸周りの回転自由度を有する関節３４により構成されている。また、手首関節部２９は、ピッチ軸周り及びロール軸周りの回転自由度をそれぞれ有する２つの関節３５，３６により構成されている。

【0049】

従って、各腕リンク４の先端部（ハンド３０）は、本実施形態では、上側基体１１に対して、６自由度の運動自由度を有する。なお、上記の説明における各腕リンク４の関節３１〜３７の回転軸（ロール軸、ピッチ軸、ヨー軸）は、腕リンク４を上下方向に延在させた状態での回転軸を示している。

【0050】

頭部５は、上側基体１１の上方に配置され、該上側基体１１に固定されている。ただし、頭部５は、チルト動作あるいはパン動作等を行い得るように、上側基体１１に対して関節を介して連結されていてもよい。その場合には、頭部５を、基体２から延設された１つの可動リンクと見なすこともできる。

【0051】

以上の構成を有するロボット１は、各脚リンク３の６個の関節１９〜２４を駆動することによって、各脚リンク３の空間的な運動が行なわれ、この運動によって床上を移動することが可能となっている。例えば、人の歩行動作あるいは走行動作と同様の形態（歩容）で脚リンク３Ｒ，３Ｌを運動させることによって、ロボット１の歩行動作あるいは走行動作が行なうことが可能である。

【0052】

また、各腕リンク４の６個の関節３１〜３６を駆動することによって、各腕リンク４の空間的な運動が行なわれ、この運動によって各腕リンク４のハンド３０を、適宜の物体に接触させる等の作業を行なうことが可能となっている。

【0053】

さらに、各腕リンク４の先端部を接地（着床）させることで、該腕リンク４を脚リンクとして機能させることも可能である。

【0054】

また、本実施形態では、基体２を構成する下側基体１０と、上側基体１１とが基体関節１２を介して連結されているので、この基体関節１２を駆動することによって、基体２をその体幹軸に相当する基体関節１２の回転軸周りに捩じることが可能となっている。

【0055】

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

【0056】

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

【0057】

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

【0058】

動作目標生成部４２は、本発明の動作目標生成装置としての機能を制御処理ユニット４１に付与するものである。この動作目標生成部４２は、ロボット１の各関節の動作制御のための制御用目標値（ロボット１の動作目標）として、各関節の変位量の目標値（以降、目標変位量又は目標関節変位量という）を所定の制御処理周期で逐次決定する処理を実行する。

【0059】

なお、本実施形態のロボット１の各関節は回転型の関節であるので、各関節の変位量は、該関節の回転角である。

【0060】

関節アクチュエータ制御部４３は、各関節の実際の変位量を、動作目標生成部４２により決定された目標変位量に追従させるように、各関節に対応する関節アクチュエータ４０を制御する。

【0061】

この場合、各関節の実際の変位量（回転角）が、ロボット１に搭載された図示しないロータリエンコーダ等の変位量センサにより検出される。そして、関節アクチュエータ制御部４３は、各関節の実際の変位量の検出値と目標関節変位量との偏差に応じて、フィードバック制御（サーボ制御）により関節アクチュエータ４０の駆動力を制御する。

【0062】

なお、動作目標生成部４２及び関節アクチュエータ制御部４３の処理は並行して行われる。すなわち、ロボット１の動作目標の生成処理は、ロボット１の実際の動作制御を行いつつ、リアルタイムで実行される。

【0063】

以降、制御処理ユニット４１の動作目標生成部４２の処理を具体的に説明する。

【0064】

ここで、本実施形態で使用する用語等について補足説明をしておく。各可動リンク３，４の先端部（足平部１８又はハンド３０）、下側基体１０等、ロボット１の各部位（一体構造の部位）の「位置」は、該部位のあらかじめ定めた代表点の空間的な位置を意味し、該部位の「姿勢」は、該部位の空間的な向きを意味する。

【0065】

各部位の「位置」及び「姿勢」は、それぞれ、ロボット１の動作環境に任意に設定されたグローバル座標系（慣性座標系）で見た位置及び姿勢として表現される。グローバル座標系としては、例えば、図１に示すように、互いに直交する２つの水平軸をＸ軸、Ｙ軸とし、これらに直交する鉛直軸をＺ軸とするＸＹＺ直交座標系が使用される。

【0066】

この場合、各部位の「位置」は、グローバル座標系の各軸方向の位置成分からなる３成分のベクトルとして表現される。同様に、各部位の「姿勢」は、グローバル座標系の各軸周りの角度成分（例えばオイラー角で表現される角度成分）からなる３成分のベクトルにより表現される。

【0067】

また、各部位の「位置」及び「姿勢」のそれぞれの時間的変化率を該部位の運動速度、各部位の運動速度の時間的変化率を該部位の運動加速度という。「位置」に関する運動速度及び運動加速度は、それぞれ並進速度、並進加速度を意味し、「姿勢」に関する運動速度及び運動加速度は、それぞれ角速度、角加速度を意味する。並進速度、並進加速度、角速度、角加速度は、それぞれ、グローバル座標系で見た３成分のベクトルにより表現される。このことは、運動量（並進運動量及び角運動量）についても同様である。

【0068】

また、ロボット１の各関節の変位量の時間的変化率を、該関節の変位速度、各関節の変位速度の時間的変化率を該関節の変位加速度という。

【0069】

なお、本実施形態では、基体２は、下側基体１０及び上側基体１１とこれらを連結する基体関節１２とから構成されるので、基体２の位置及び姿勢は、下側基体１０及び上側基体１１のうちのいずれか一方である基体基準部位（例えば下側基体１０）の位置及び姿勢と、基体関節１２の変位量とから構成されるものとする。

【0070】

同様に、基体２の運動速度は、基体基準部位の運動速度と基体関節１２の変位速度とから構成され、基体２の運動加速度は、基体基準部位の運動加速度と基体関節１２の変位加速度とから構成されるものとする。

【0071】

また、各部位の位置、姿勢、運動速度、運動加速度、あるいは、各関節の変位量、変位速度、変位加速度を総称的に該部位又は該関節の「運動状態」ということがある。

【0072】

また、位置、姿勢、運動速度、運動加速度、運動量、あるいは、各関節の変位量、変位速度、変位加速度等、任意の状態量の「軌道」は、該状態量の瞬時値の時系列を意味する。

【0073】

また、符号「↑」はベクトル（縦ベクトル）を表記する記号として使用する。

【0074】

また、任意のベクトル又は行列を示す変数に付する上付きの添え字「Ｔ」は、該ベクトル又は行列の転置を意味する。

【0075】

次に、動作目標生成部４２の処理を概略的に説明しておく。動作目標生成部４２は、所定の制御処理周期毎に、現在時刻以後の各可動リンク３，４の先端部の運動状態の目標値の予測軌道と、現在時刻以後のロボット１の全体の運動量（並進運動量及び角運動量）の目標値の予測軌道とを設定する。

【0076】

そして、動作目標生成部４２は、これらの予測軌道を実現し得るように、現在時刻以後のロボット１の全体の運動状態の目標値（詳しくは、ロボット１の各部位の位置及び姿勢、並びに各関節の変位量を規定する目標値）の予測軌道である全体運動状態予測軌道を生成する。

【0077】

この全体運動状態予測軌道は、ロボット１の各関節の変位量の可変範囲等、各関節の運動状態に関する制約条件（以降、関節運動制約条件という）を考慮せずに（該関節運動制約条件が無いものとみなして）生成される暫定的な基準軌道である。従って、全体運動状態予測軌道は、関節運動制約条件を満たしていない場合もある。

【0078】

そして、動作目標生成部４２は、この全体運動状態予測軌道により規定されるヤコビアン行列と、ロボット１の所定種類の状態量に関する所定の評価関数と、関節運動制約条件を表す不等式とを用いて２次計画問題の解法手法の演算処理を実行することで、ロボット１の基体２と各可動リンク３，４（第１〜第４可動リンク）の先端部とのそれぞれの現在時刻以後の運動加速度の時系列（所定の刻み時間ΔＴ毎の複数の時刻における当該運動加速度の瞬時値の時系列）を算出する。

【0079】

これらの運動加速度の時系列（軌道）は、それにより規定される運動速度（基体２及び各可動リンク３，４の先端部の運動速度）と前記全体運動状態予測軌道により規定されるヤコビアン行列とから算出されるロボット１の所定種類の状態量の値の時系列と、全体運動状態予測軌道により規定される当該所定種類の状態量の値の時系列との一致度合いが、関節運動制約条件を満たし得る範囲内でできるだけ高くなるように算出される。

【0080】

そして、上記運動加速度の算出値の時系列のうちの初期時刻（本実施形態では現在時刻）での算出値に基づいて、現在の制御処理周期で求めるべきロボット１の基体２及び各可動リンク３，４の先端部のそれぞれの目標運動加速度（ロボット１の実際の制御用の目標運動加速度）が決定される。

【0081】

これにより、制御処理周期毎に、基体２と各可動リンク３，４の先端部とのそれぞれの目標運動加速度が決定される。

【0082】

そして、動作目標生成部４２は、これらの目標運動加速度から、各制御処理周期における基体２の位置及び姿勢の制御用目標値と、各可動リンク３，４の先端部の位置及び姿勢の制御用目標値とを決定する。

【0083】

さらに、動作目標生成部４２は、これらの制御用目標値から逆キネマティクス演算によって、各制御処理周期におけるロボット１の各関節の実際の制御用の目標変位量（前記関節アクチュエータ制御部４３で使用する目標変位量）を決定する。

【0084】

かかる処理を実行する動作目標生成部４２は、主要な処理部として図３のブロック図に示す如き処理部を備える。

【0085】

すなわち、動作目標生成部４２は、可動リンク３Ｌ，３Ｒ，４Ｌ，４Ｒのそれぞれの先端部（足平部１８又はハンド３０）の現在時刻以後の運動状態↑ＸL1，↑ＸL2，↑ＸL3，↑ＸL4の目標値↑ＸL1_ref，↑ＸL2_ref，↑ＸL3_ref，↑ＸL4_refの予測軌道である可動リンク先端部予測軌道を所定の制御処理周期毎に設定する可動リンク先端部予測軌道設定部５１と、現在時刻以後のロボット１の全体の運動量↑ＰＬ（並進運動量及び角運動量）の目標値↑ＰＬ_refの予想軌道である全体運動量予測軌道を所定の制御処理周期毎に設定する全体運動量予測軌道設定部５２とを備える。

【0086】

上記可動リンク先端部予測軌道及び全体運動量予測軌道は、現在時刻から所定の刻み時間ΔＴ毎の時刻における瞬時値の時系列である。このことは、後述する他の予測軌道についても同様である。上記刻み時間ΔＴは、例えば動作目標生成部４２の制御処理周期の時間Δtと同じである。ただし、ΔＴはΔｔと異なっていてもよく、例えばΔＴ＞Δｔであってもよい。

【0087】

上記可動リンク先端部予測軌道における各可動リンク３，４（第ｉ可動リンク）の先端部の運動状態↑ＸLi（瞬時値）の目標値↑ＸLi_refは、本実施形態では、該第ｉ可動リンクの先端部の位置及び姿勢の目標値である。以降、各可動リンク３，４（第ｉ可動リンク）の先端部の位置及び姿勢の目標値↑ＸLi_ref（ｉ＝１〜４）のそれぞれを目標位置姿勢↑ＸLi_refという。

【0088】

各可動リンク３，４の先端部の目標位置姿勢↑ＸLi_refは、位置に関する３成分（グローバル座標系の各座標軸方向の位置成分）の目標値と、姿勢に関する３成分（グローバル座標系の各座標軸周り方向の角度成分）の目標値とを並べた６成分のベクトルとして表現される。

【0089】

補足すると、可動リンク先端部予測軌道設定部５１は、可動リンク先端部予測軌道として、各可動リンク３，４の先端部の目標位置姿勢↑ＸLi_refの予測軌道の代わりに、該先端部の運動速度（位置及び姿勢の時間的変化率）の目標値の予測軌道、あるいは、該先端部の運動加速度（運動速度の時間的変化率）の目標値の予測軌道を設定するようにしてもよい。

【0090】

各可動リンク３，４の先端部の運動速度あるいは運動加速度の目標値の予測軌道を設定した場合であっても、それらの目標値を積分することで、各可動リンク３，４の先端部の目標位置姿勢↑ＸLi_refを随時、算出することができる。逆に、各可動リンク３，４の先端部の目標位置姿勢↑ＸLi_refを微分する（時間的変化率を求める）ことで、該先端部の運動速度あるいは運動加速度の目標値を算出することもできる。

【0091】

前記全体運動量予測軌道におけるロボット１の全体の運動量↑ＰＬの目標値↑ＰＬ_ref（以降、目標全体運動量↑ＰＬという）は、詳しくは、ロボット１の全体の並進運動量↑Ｐ（換言すれば、ロボット１の全体重心の並進運動量↑Ｐ）の目標値である目標並進運動量↑Ｐ_refと、ロボット１の全体の角運動量↑Ｌの目標値である目標角運動量↑Ｌ_refとから構成される。

【0092】

ここで、本実施形態では、目標角運動量↑Ｌ_refは、詳しくは、ロボット１のＺＭＰ（Zero Moment Point）の目標値である目標ＺＭＰ↑Zmp_refの周りに発生する角運動量の目標値である。

【0093】

また、本実施形態では、目標全体運動量↑ＰＬは、目標ＺＭＰ↑Zmp_refとロボット１の動力学モデルとを用いて設定される。

【0094】

このため、本実施形態では、全体運動量予測軌道設定部５２は、目標全体運動量↑ＰＬの予測軌道に加えて、目標ＺＭＰ↑Zmp_refの予測軌道も設定する。

【0095】

さらに、本実施形態では、全体運動量予測軌道設定部５２は、基体関節１２の変位量を含めた基体２の位置及び姿勢の基本目標値↑Ｘb_ref_sの予測軌道も設定する機能も有する。この基本目標値↑Ｘb_ref_sは、目標全体運動量↑ＰＬを設定するために用いる動力学モデル上で目標ＺＭＰ↑Zmp_refを達成し得るように設定される参考的な目標値である。

【0096】

目標並進運動量↑Ｐ_ref、目標角運動量↑Ｌ_ref、及び目標ＺＭＰ↑Zmp_refは、それぞれグローバル座標系で見た３成分のベクトルとして表現される。そして、↑Ｐ_ref及び↑Ｌ_refを合わせた目標全体運動量↑ＰＬ_refは６成分のベクトル（＝[↑Ｐ_ref^T，↑Ｌ_ref^T]^T）として表現される。

【0097】

また、基体２の位置及び姿勢↑Ｘbの基本目標値↑Ｘb_ref_s（以降、基本目標位置姿勢↑Ｘb_ref_sという）は、前記基体基準部位（下側基体１０又は上側基体１１）の位置及び姿勢（グローバル座標系で見た６成分のベクトル↑Ｘbb）の基本目標値↑Ｘbb_ref_sと、基体関節１２の変位量（回転角）θfの基本目標値θf_ref_sとにより構成される７成分のベクトル（＝[↑Ｘbb_ref_s^T，θf_ref_s]^T）として表現される。

【0098】

動作目標生成部４２は、さらに、↑ＸLi_ref（ｉ＝１〜４）、↑ＰＬ_ref、↑Zmp_ref、↑Ｘb_ref_sの予測軌道を用いて、↑ＸLi_ref（ｉ＝１〜４）の予測軌道により規定される各可動リンク３，４（第ｉ可動リンク）の先端部の運動速度↑ＸLdotiの目標値↑ＸLdoti_ref（ｉ＝１〜４）と、目標全体運動量↑ＰＬ_refとを実現し得るように、現在時刻以後のロボット１の全体の運動状態の目標値の予測軌道である全体運動状態予測軌道を決定する全体運動状態予測軌道決定部５３を備える。

【0099】

上記全体運動状態予測軌道におけるロボット１の全体の運動状態の目標値は、基体２の位置及び姿勢↑Ｘbの目標値↑Ｘb_refと、可動リンク３，４（第１〜第４可動リンク）のそれぞれの関節の変位量↑θLi（ｉ＝１〜４）の目標値↑θLi_refとから構成される。

【0100】

この場合、基体２の位置及び姿勢↑Ｘbの目標値↑Ｘb_ref（以降、目標位置姿勢↑Ｘb_refという）は、前記基本目標位置姿勢↑Ｘb_ref_sと同様に、基体基準部位の位置及び姿勢の目標値↑Ｘbb_ref（６成分のベクトル）と、基体関節１２の変位量（回転角）の目標値θf_refとにより構成される７成分のベクトル（＝[↑Ｘbb_ref^T，θf_ref]^T）として表現される。

【0101】

また、可動リンク３，４（第１〜第４可動リンク）のそれぞれの関節の変位量↑θLi（ｉ＝１〜４）の目標値↑θLi_ref（以降、目標関節変位量↑θLi_ref）は、各可動リンク３，４（第ｉ可動リンク）の全ての関節のそれぞれの変位量（回転角）の目標値の組を示すベクトル（本実施形態では６成分のベクトル）として表現される。

【0102】

動作目標生成部４２は、さらに、前記全体運動状態予測軌道の各時刻でのロボット１の全体の運動状態により規定されるヤコビアン行列と、ロボット１の所定種類の状態量に関する所定の評価関数と、ロボット１の関節運動制約条件を表す不等式とを用いて、２次計画問題の解法手法の演算処理によって、ロボット１の基体２の運動加速度↑Ｘbdot2と各可動リンク３，４（第１〜第４可動リンク）の先端部の運動加速度↑ＸLidot2（ｉ＝１〜４）との組として構成される運動加速度↑ｕ（＝[↑Ｘbdot2^T，↑ＸL1dot2^T，↑ＸL2dot2^T，↑ＸL3dot2^T，↑ＸL4dot2^T]^T）の現在時刻以後の複数の時刻での値を算出し、その算出結果から各制御処理周期での目標運動加速度↑ｕ_cmd（＝[↑Ｘbdot2_cmd^T，↑ＸL1dot2_cmd^T，↑ＸL2dot2_cmd^T，↑ＸL3dot2_cmd^T，↑ＸL4dot2_cmd^T]^T）を決定する運動加速度決定部５４と、各制御処理周期で決定した目標運動加速度↑ｕ_cmdの積分演算によって各制御処理周期でのロボット１の基体２の位置及び姿勢の制御用目標値↑Ｘb_cmdと各可動リンク３，４の先端部のそれぞれの位置及び姿勢の制御用目標値↑ＸLi_cmd（ｉ＝１〜４）とを決定する制御用目標位置姿勢決定部５５と、これらの制御用目標値↑Ｘb_cmd，↑ＸLi_cmd（ｉ＝１〜４）から逆キネマティクスの演算によってロボット１の各関節の目標変位量を決定する関節変位量決定部５６とを備える。

【0103】

補足すると、前記可動リンク先端部予測軌道、全体運動状態予測軌道等の上記の各予測軌道（添え字「_ref」が付された状態量の予測軌道）は、各制御処理周期の時刻（現在時刻）でのロボット１の各関節の目標変位量（関節アクチュエータ制御部４３で実際に使用する制御用の目標変位量）を最終的に決定するための参照用の基準軌道（暫定的な軌道）である。このため、これらの予測軌道は、現在時刻以後のロボット１の実際の制御用目標値の軌道に一致するとは限らない。本実施形態において、ロボット１の各関節の実際の動作制御のために使用される目標値は、添え字「_cmd」を付した目標値である。

【0104】

なお、可動リンク先端部予測軌道設定部５１、全体運動量予測軌道設定部５２、全体運動状態予測軌道決定部５３、運動加速度決定部５４はそれぞれ本発明における可動リンク先端部予測軌道設定手段、全体運動量予測軌道設定手段、全体運動状態予測軌道決定手段、運動加速度決定手段に相当する。

【0105】

また、制御用目標位置姿勢決定部５５と関節変位量決定部５６とにより本発明における関節制御用目標値決定手段が構成される。

【0106】

次に、動作目標生成部４２の各処理部の処理の詳細を含めて、動作目標生成部４２の処理の全体を説明する。なお、本実施形態では、主に、ロボット１を歩行動作又は走行動作により移動させるための動作目標を生成する場合を一例として動作目標生成部４２の処理を説明する。

【0107】

動作目標生成部４２は、所定の制御処理周期で上記の各処理部の処理を実行する。具体的には、動作目標生成部４２は、各制御処理周期において、まず、可動リンク先端部予測軌道設定部５１及び全体運動量予測軌道設定部５２の処理を実行する。

【0108】

本実施形態では、可動リンク先端部予測軌道設定部５１及び全体運動量予測軌道設定部５２には、脚リンク３Ｒ，３Ｌのうちの遊脚の着地が行なわれる毎に、あるいは、制御処理周期毎に、現在時刻以後の複数歩分の遊脚の先端部（足平部１８）の目標とする着地位置及び着地姿勢である目標着地位置姿勢と、目標とする着地時刻である目標着地時刻とが与えられるようになっている。

【0109】

上記目標着地位置姿勢及び目標着地時刻は、例えばロボット１の移動計画、操縦装置による移動指令、ロボット１の周囲の環境情報等に基づいて決定されたものである。その決定処理は、制御処理ユニット４１で行われ、あるいは、ロボット１の外部のサーバで行われる。なお、目標着地時刻の代わりに、ロボット１の１歩分の期間（歩容周期）の目標時間を用いてもよい。

【0110】

そして、可動リンク先端部予測軌道設定部５１は、与えられた複数歩分の目標着地位置姿勢及び目標着地時刻に応じて、脚リンク３Ｌ，３Ｒのそれぞれの先端部（足平部１８）の現在時刻以後の目標位置姿勢↑ＸL1_ref，↑ＸL2_refの予測軌道（現在時刻からあらかじめ定められた所定時間後の時刻までの期間における予測軌道）を設定する。

【0111】

該予測軌道の期間の時間間隔（所定時間）は、ロボット１の複数歩分の時間でもよいが、１歩分の時間よりも短い時間間隔（例えば、１制御処理周期の数十倍の時間間隔等）であってもよい。

【0112】

可動リンク先端部予測軌道設定部５１の処理では、脚リンク３Ｌ，３Ｒ（第１可動リンク及び第２可動リンク）のうち、遊脚として動作させる脚リンク３の先端部（足平部１８）の目標位置姿勢↑ＸL1_ref又↑ＸL2_refの予測軌道は、該足平部１８が離床直前の接地箇所から空中を移動して、目標着地位置姿勢及び目標着地時刻に従って着地するように設定される。

【0113】

例えば、図４に例示する如く、遊脚として動作させる脚リンク３の先端部（足平部１８）の位置及び姿勢が変化していくように該先端部の目標位置姿勢↑ＸL1_ref又↑ＸL2_refの予測軌道が設定される。

【0114】

なお、図４に示す例では、脚リンク３の足平部１８の姿勢を経時的に変化させているが、該姿勢は、足平部１８の空中での移動中の全期間又は一部の期間で一定であってもよい。

【0115】

また、脚リンク３Ｌ，３Ｒのうち、支持脚として動作させる脚リンク３の足平部１８の目標位置姿勢↑ＸL1_ref又↑ＸL2_refの予測軌道は、該足平部１８が滑らずに接地した状態に保たれるように設定される。この場合、該足平部１８の目標姿勢は、接地中の全期間で一定になるように設定してもよいが、該接地中に目標姿勢が経時的に変化する（ひいては、該足平部１８の接地部分が変化する）ようにしてもよい。

【0116】

なお、ロボット１の歩行動作又は走行動作では、遊脚として動作させる脚リンク３と支持脚として動作させる脚リンク３とは交互に切替えられる。

【0117】

また、本実施形態では、可動リンク先端部予測軌道設定部５１は、ロボット１の歩行動作又は走行動作における腕リンク４Ｌ，４Ｒ（第３可動リンク及び第４可動リンク）のそれぞれの先端部（ハンド３０）の現在時刻以後の目標位置姿勢↑ＸL3_ref，↑ＸL4_refの予測軌道を、例えば上側基体１１に対するハンド３０の位置及び姿勢が一定に維持されるように設定する。このため、↑ＸL3_ref，↑ＸL4_refの予測軌道は、全体運動量予測軌道設定部５２により後述する如く設定される基体２の基本目標位置姿勢↑Ｘb_ref_sの予測軌道に応じて設定される。

【0118】

ただし、各腕リンク４Ｌ，４Ｒのハンド３０の位置又は姿勢を上側基体１１に対して相対的に動かすように↑ＸL3_ref，↑ＸL4_refの予測軌道を設定してもよい。

【0119】

可動リンク先端部予測軌道設定部５１の処理は以上の如く実行される。

【0120】

全体運動量予測軌道設定部５２は、与えられた目標着地位置姿勢及び目標着地時刻に応じて（あるいは、各脚リンク３の先端部の目標位置姿勢↑ＸL1_ref，↑ＸL2_refの予測軌道のうち、接地状態となるに脚リンク３の先端部の目標位置姿勢の予測軌道に応じて）、現在時刻以後の目標ＺＭＰ↑Zmp_refの予測軌道を設定する。

【0121】

この場合、↑Zmp_refの予測軌道は、↑Zmp_refが所謂、支持多角形内でその境界に近づき過ぎないような領域（ＺＭＰの存在許容領域）に位置し、且つ、不連続的な変化を生じないように設定される。このような目標ＺＭＰの軌道の設定手法としては、例えば特許第３６７４７８９号にて本願出願人が説明した手法等、公知の手法を用いることができる。

【0122】

そして、全体運動量予測軌道設定部５２は、可動リンク先端部予測軌道設定部５１により設定された各脚リンク３の先端部の目標位置姿勢↑ＸL1_ref，↑ＸL2_refの予測軌道と、目標ＺＭＰ↑Zmp_refの予測軌道と、ロボット１の動力学（ロボット１に作用する外力（ここでは床反力）とロボット１の運動との関係）を表すものとしてあらかじめ定められた動力学モデルとを用いて、該動力学モデル上で目標ＺＭＰ↑Zmp_refを達成し得る基体２の運動加速度の基本目標値↑Ｘbdot2_ref_s（以降、基本目標運動加速度↑Ｘbdot2_ref_sという）の予測軌道が決定される。

【0123】

基体２の基本目標運動加速度↑Ｘbdot2_ref_sは、基体基準部位（下側基体１０又は上側基体１１）の運動加速度↑Ｘbbdot2の基本目標値↑Ｘbbdot2_ref_s（６成分のベクトル）と、基体関節１２の変位加速度θfdot2（回転角加速度）の基本目標値θfdot2_ref_sとから構成される７成分のベクトルとして表現される。

【0124】

ここで、目標ＺＭＰ↑Zmp_refを達成し得る基体２の基本目標運動加速度↑Ｘbdot2_ref_sというのは、上記動力学モデルにおいて、ロボット１の全体の運動によって発生する慣性力と該ロボット１に作用する重力との合力によって、目標ＺＭＰ周りに発生するモーメントの水平軸周り成分（Ｘ軸周り成分及びＹ軸周り成分）がゼロになるような、基体２の運動加速度を意味する。

【0125】

なお、このような基体２の基本目標運動加速度↑Ｘbdot2_ref_sは、ロボット１の各関節の変位量の可変範囲等の関節運動制約条件を考慮せずに（該関節運動制約条件が無いものとみなして）算出されるものである。

【0126】

上記動力学モデルとしては、目標ＺＭＰ↑Zmp_refを達成し得る基体２の基本目標位置姿勢↑Ｘbdot2_ref_sを比較的容易に算出し得るように、線形性の高い動力学モデルが使用される。例えば、特許第３６７４７８９号にて本願出願人が説明した動力学モデル（特許第３６７４７８９号の図１０に示す動力学モデル）と同様の動力学モデルが、全体運動量予測軌道設定部５２の処理における動力学モデルとして採用することができる。

【0127】

該動力学モデルは、図５に示すように、各脚リンク３Ｌ，３Ｒと基体２とにそれぞれ対応する３つの質点３ｍＬ，３ｍＲ，２ｍと、基体２の姿勢の変化に伴う慣性力モーメントを発生するフライホイールＦＨ（質量を持たずにイナーシャを有するフライホイール）とを備える動力学モデルである。

【0128】

なお、フライホイールＦＨが発生する慣性力モーメントは、より詳しくは、グローバル座標系の３つの各軸周りの慣性力モーメントである。また、質点３ｍＲ，３ｍＬ，２ｍのの質量は、その総和の質量がロボット１の全体質量に一致するようにあらかじめ設定される。

【0129】

この動力学モデルでは、各脚リンク３Ｒ，３Ｌの先端部の運動が、質点３ｍＬ，３ｍＲの運動に相当し、また、基体関節１２を含めた基体２の運動が、質点２ｍ及びフライホイールＦＨの運動に相当するものとされる。

【0130】

このような動力学モデルを使用することで、特許第３６７４７８９号にて本願出願人が説明した目標上体位置姿勢の決定手法と同様の手法によって、現在時刻以後の基体２の基本目標運動加速度↑Ｘbdot2_ref_sの予測軌道を設定できる。

【0131】

すなわち、上記動力学モデル上において、ＺＭＰが支持多角形内に収まり、また、ロボット１に作用する床反力のうちの摩擦力成分が適切な許容範囲内に収まる、というような所要の動力学的な制約条件が満たされるように、基体２の基本目標運動加速度↑Ｘbdot2_ref_sの予測軌道（ひいては、質点２ｍの並進加速度及びフライホイールＦＨの回転角加速度）を設定できる。

【0132】

全体運動量予測軌道設定部５２は、上記の如く設定した基体２の基本目標運動加速度↑Ｘbdot2_ref_sの予測軌道における↑Ｘbdot2_ref_sの値を積分（２階積分）することで、基体２の基本目標位置姿勢↑Ｘb_ref_sの予測軌道を設定する。

【0133】

さらに、全体運動量予測軌道設定部５２は、上記の如く設定した基体２の目標運動加速度↑Ｘbdot2_ref_s（又は目標位置姿勢↑Ｘb_ref_s）の予測軌道と、可動リンク先端部予測軌道設定部５１により設定された脚リンク３Ｌ，３Ｒのそれぞれの先端部の目標位置姿勢↑ＸL1_ref，↑ＸL2_refの予測軌道と、目標ＺＭＰ↑Zmp_refの予測軌道とから、上記動力学モデルを用いて、現在時刻以後の目標全体運動量↑ＰＬ_refの予測軌道を設定する。

【0134】

具体的には、全体運動量予測軌道設定部５２は、脚リンク３Ｌ，３Ｒのそれぞれの先端部の目標位置姿勢↑ＸL1_ref，↑ＸL2_refの予測軌道から算出される脚リンク３Ｌ，３Ｒのそれぞれの先端部の運動速度↑ＸL1dotの目標値↑ＸL1dot_refの予測軌道に基づいて、質点３ｍＬ，３ｍＲのそれぞれの並進運動量の予測軌道を算出する。

【0135】

また、全体運動量予測軌道設定部５２は、基体２の目標運動加速度↑Ｘbdot2_ref_s（又は目標位置姿勢↑Ｘb_ref_s）の予測軌道から算出される基体２の運動速度↑Ｘbdot_ref_sの予測軌道に基づいて、質点２ｍの並進運動量の予測軌道とフライホイールＦＨの角運動量の予測軌道とを算出する。

【0136】

そして、全体運動量予測軌道設定部５２は、質点３ｍＬ，３ｍＲ，２ｍのそれぞれの並進運動量の予測軌道とフライホイールＦＨの角運動量の予測軌道と目標ＺＭＰ↑Zmp_refの予測軌道とから、目標全体運動量↑ＰＬ_refの予測軌道を算出する。

【0137】

この場合、目標全体運動量↑ＰＬ_refの予測軌道の各時刻（所定の刻み時間ΔＴ毎の時刻）での目標並進運動量↑Ｐ_refは、質点３ｍＬ，３ｍＲ，２ｍのそれぞれの並進運動量の総和の並進運動量（これは質点３ｍＬ，３ｍＲ，２ｍの全体重心の並進運動量に相当する）として算出される。

【0138】

また、目標全体運動量↑ＰＬ_refの予測軌道の各時刻での目標角運動量↑Ｌ_refは、質点３ｍＬ，３ｍＲ，２ｍのそれぞれの並進運動量によって、目標ＺＭＰ↑Zmp_refの周りに発生する角運動量と、フライホイールＦＨの角運動量との総和の角運動量として算出される。

【0139】

補足すると、↑ＰＬ_refの予測軌道を設定するために使用する動力学モデルは、前記したように３つの質点３ｍＬ，３ｍＲ，２ｍと、フライホイールＦＨとを備える動力学モデルであるが、他の形態の動力学モデルを使用してもよい。

【0140】

例えば、腕リンク４Ｌ，４Ｒにそれぞれ対応する２つの質点をさらに備える動力学モデル、あるいは、下側基体１０と上側基体１１とで各別の質点を備える動力学モデル等を使用することもできる。また、例えば、基体２の質量に比して、各可動リンク３，４の質量が十分に小さいような場合には、各可動リンク３，４に対応する質点を省略した動力学モデルを使用することも可能である。

【0141】

↑ＰＬ_refの予測軌道を設定するための動力学モデルとしては、ロボット１で実現し得る目標全体運動量↑ＰＬの予測軌道を生成し得るものであれば、種々様々な動力学モデルを使用することができる。該動力学モデルは、比較的低負荷の演算処理によって目標全体運動量↑ＰＬの予測軌道を生成し得るものであることが望ましい。

【0142】

全体運動量予測軌道設定部５２の処理は、以上の如く実行される。

【0143】

以降の説明では、↑ＰＬ_refの予測軌道の各時刻（所定の刻み時間ΔＴ毎の各時刻）での瞬時値を区別する場合に、現在時刻側から順番に↑ＰＬ_ref(1)、↑ＰＬ_ref(2)、……、↑ＰＬ_ref(N)というように、離散時間系での時刻を表す番数ｋ（ｋ＝１，２，…，Ｎ）を付して表記する。この場合、ｋ＝１の時刻は、現在時刻（現在の制御処理周期）を意味し、ｋ＝Ｎの時刻は、現在時刻から所定時間後の時刻を意味する。このことは、↑ＸLi_refの予測軌道等、他の予測軌道についても同様とする。

【0144】

動作目標生成部４２は、以上の如く可動リンク先端部予測軌道設定部５１及び全体運動量予測軌道設定部５２の処理を実行した後、全体運動状態予測軌道決定部５３の処理を実行する。

【0145】

全体運動状態予測軌道決定部５３は、本実施形態では、分解運動量制御の手法によって、各可動リンク３，４の先端部の目標位置姿勢↑ＸLi_ref（ｉ＝１〜４）の予測軌道から算出される各可動リンク３，４の先端部の目標運動速度↑ＸLidot_ref（ｉ＝１〜４）と、ロボット１の目標全体運動量↑ＰＬ_ref（＝[↑Ｐ_ref^T，↑Ｌ_ref^T]^T）を実現するように、現在時刻以後のロボット１の全体の運動状態の目標値↑Ｘb_ref，↑θLi_ref（ｉ＝１〜４）の予測軌道である全体運動状態予測軌道を決定する。

【0146】

上記分解運動量制御の手法は、それを要約して言えば、ロボット１の全体運動量↑ＰＬの目標値（↑ＰＬ_ref）と各可動リンク３，４の先端部の運動速度↑ＸLidotの目標値（↑ＸLidot_ref）とから、これらの目標値を実現し得るようにロボット１の全体の運動状態を決定する手法である。ただし、この手法では、ロボット１の関節運動制約条件を考慮せずに（該関節運動制約条件が無いものとみなして）、ロボット１の全体の運動状態が決定される。

【0147】

具体的には、この分解運動量制御の手法では、まず、ロボット１の全体運動量↑ＰＬと、基体２の運動速度と、各可動リンク３，４の各関節の変位速度との間の関係を表す次式（１）と、基体２の運動速度と、各可動リンク３，４の各関節の変位速度との間の関係を表す次式（２）とに基づく演算処理によって、全体運動状態予測軌道の各時刻（所定の刻み時間ΔＴ毎の時刻）での↑ＸLidot_ref（ｉ＝１〜４）及び↑ＰＬ_refを実現するための基体２の目標運動速度↑Ｘbdot_refが算出される。

【0148】

【数1】

【0149】

ここで、式（１）において、↑Ｐはロボット１の全体の並進運動量（３成分のベクトル）、↑Ｌcはロボット１の全体重心の周りの全体の角運動量（３成分のベクトル）、↑Ｘbdotは基体２の運動速度（基体基準部位の運動速度↑Ｘbbdotの各成分と基体関節１２の変位速度θfdotとから成る７成分のベクトル）、Ａ1は基体２の運動速度↑Ｘbdotを、これに応じて発生する並進運動量及び角運動量（全体重心周り角運動量）の組に変換するヤコビアン行列（６行７列の行列）、ＮＬは可動リンク３，４の個数（＝４）、↑θLidotは第ｉ可動リンクの各関節の変位速度を成分として構成されるベクトル（６成分のベクトル）、ＭLiは↑θLidot（以降、第ｉ可動リンクの関節変位速度↑θLidotという）を、これに応じて発生する並進運動量に変換するヤコビアン行列（３行６列の行列）、ＨLiは第ｉ可動リンクの関節変位速度↑θLidotを、これに応じて発生する角運動量（ロボット１の全体重心の周りの角運動量）に変換するヤコビアン行列（３行６列の行列）である。

【0150】

この場合、上記ヤコビアン行列Ａ1は、次式（１ａ）で示す形の行列である。

【0151】

【数2】

【0152】

この式（１ａ）において、ｍはロボット１の全体質量、Ｅ₃は３行３列の単位行列、Ｏ₃は３行３列の零行列、Ｉはロボット１の全体重心周りの回転運動に関する慣性行列（３行３列の行列）、Ｍfは基体関節１２の変位速度を、これに応じて発生する並進運動量に変換する行列（３行１列の行列）、Ｈfは基体関節１２の変位速度を、これに応じて発生する角運動量（ロボット１の全体重心周りの角運動量）に変換する行列（３行１列の行列）である。

【0153】

さらに、式（１ａ）におけるＤbcは基体基準部位の位置に対するロボット１の全体重心の位置ベクトル↑ｒbb_c（３成分のベクトル）と基体基準部位の姿勢に関する運動速度（角速度）↑ωbb（３成分のベクトル）との外積（＝ｒbb_c×↑ωbb）に対して、Ｄbc・↑ωbb＝−ｒbb_c×↑ωbbという関係が成立するように定義される行列（３行３列の行列）である。

【0154】

また、前記式（２）において、↑ＸLidotは第ｉ可動リンクの先端部の運動速度（６成分のベクトル）、Ｊbは基体２の運動速度↑Ｘbdotを第ｉ可動リンクの先端部の運動速度↑ＸLidotに変換するヤコビアン行列（６行７列の行列）、ＪLiは第ｉ可動リンクの関節変位速度↑θLidotを第ｉ可動リンクの先端部の運動速度↑ＸLidotに変換するヤコビアン行列（６行６列の行列）である。

【0155】

上記式（１）、（２）から次式（３）が得られる。

【0156】

【数3】

【0157】

上記式（３）において、Ａは前記ヤコビアン行列Ａ1，ＭLi（ｉ＝１〜４），ＨLi（ｉ＝１〜４），Ｊbと、前記ヤコビアン行列ＪLi（ｉ＝１〜４）の擬似逆行列ＪLi^#とから上記但し書きの式（３ａ）で定義される行列（６行７列の行列）、Ａ^#は行列Ａの擬似逆行列（７行６列の行列）、Ｅ₇は７行７列の単位行列である。また、↑Ｘbdot_aは冗長自由度を利用した基体２の調整用運動速度（７成分のベクトル）である。

【0158】

本実施形態では、上記式（３）により↑ＸLidot_ref（ｉ＝１〜４）及び↑ＰＬ_refの予測軌道を実現するための基体２の目標運動速度↑Ｘbdot_refの予測軌道が決定される。

【0159】

具体的には、全体運動状態予測軌道の各時刻ｋ（ｋ＝１〜Ｎ）での基体２の目標運動速度↑Ｘbdot_ref(k)が順次、上記式（３）の右辺の演算によって算出される。

【0160】

この場合、式（３）の右辺の演算を行うためのパラメータは次のように決定される。

【0161】

時刻ｋでの↑Ｘbdot_ref(k)を算出するために用いる行列Ａは、式（１），（２）に関して説明したヤコビアン行列Ａ1，ＭLi（ｉ＝１〜４），ＨLi（ｉ＝１〜４）と、式（２）の但し書きで定義されるヤコビアン行列Ｊb，ＪLi（ｉ＝１〜４）とから、式（３）の但し書きの式（３ａ）に従って算出される。

【0162】

この場合、ヤコビアン行列Ａ1，ＭLi（ｉ＝１〜４），ＨLi（ｉ＝１〜４），Ｊb，ＪLiは、ロボット１の全体の運動状態が、全体運動状態予測軌道の時刻ｋ−１でのロボット１の全体の運動状態の目標値↑Ｘb_ref，↑θLi_ref（ｉ＝１〜４）に一致する状態でのヤコビアン行列として算出される。ただし、ｋ＝１の時刻での↑Ｘbdot_ref(k)を算出する場合においては、ロボット１の全体の運動状態は、前回の制御処理周期で最終的に決定されたロボット１の全体の運動状態の目標値↑Ｘb_cmd，↑θLi_cmd（ｉ＝１〜４）に一致するものとして、ヤコビアン行列が算出される。

【0163】

なお、ヤコビアン行列Ａ1，ＭLi（ｉ＝１〜４），ＨLi（ｉ＝１〜４）を算出する場合、ロボット１の各リンク要素に質量とイナーシャとを設定した動力学モデルが使用される。

【0164】

さらに、上記の如く算出したヤコビアン行列のうちのＪLiから、その擬似逆行列ＪLi^#が算出される。この場合、本実施形態では、各可動リンク３，４の関節の個数（↑θLidotの成分数）は６個であるので、ＪLi^#は、ＪLiの逆行列ＪLi^-1として算出される。

【0165】

補足すると、ロボット１の可動リンク３又は４の関節の個数を７個以上にする（換言すれば、該可動リンク３又は４の自由度数を７以上とする）ことも可能である。７個以上の関節を有する可動リンクについての擬似逆行列ＪLi^#は、後述するＡ^#の算出の場合と同様の演算式により算出することが可能である。

【0166】

また、以上の如く算出される行列Ａから、時刻ｋでの↑Ｘbdot_ref(k)を算出するために用いる擬似逆行列Ａ^#が算出される。

【0167】

この場合、Ａ^#は、例えば次式（４）により算出される。

【0168】

Ａ^#＝Ｗ^-1・Ａ^T・(Ａ・Ｗ^-1・Ａ^T)^-1 ……（４）
この式（４）におけるＷは、あらかじめ設定された重み係数行列（対角行列）である。この重み係数行列は、ロボット１の制御特性等を考慮して実験等に基づいてあらかじめ設定される。

【0169】

また、↑Ｘbdot_ref(k)を算出するために用いる↑Ｐの値として、前記全体運動量予測軌道における時刻ｋでの目標並進運動量↑Ｐ_refが決定される。さらに、↑Ｘbdot_ref(k)を算出するために用いる↑Ｌcの値は、前記全体運動量予測軌道における時刻ｋでの目標角運動量↑Ｌ_ref（目標ＺＭＰ↑Zmp_ref(k)の周りの角運動量）を、時刻ｋでのロボット１の全体重心の周りの角運動量に変換してなる値として算出される。

【0170】

なお、この場合、ロボット１の全体重心の位置は、全体運動量予測軌道設定部５２の処理で使用した動力学モデルにおける時刻ｋでの全体重心の位置に一致するものとされる。

【0171】

補足すると、前記式（３）の↑Ｌcの代わりに、目標ＺＭＰ↑Zmp_refの周りの角運動量↑Ｌを採用することもできる。その場合には、前記式（１）におけるヤコビアン行列Ａ1が、式（１）の但し書きに示したものと異なる行列となる。そして、この場合には、↑Ｘbdot_ref(k)を算出するために用いる↑Ｌの値として、前記全体運動量予測軌道における時刻ｋでの目標角運動量↑Ｌ_refをそのまま使用できる。

【0172】

また、↑Ｘbdot_ref(k)を算出するために用いる第ｉ可動リンクの↑ＸLidot（ｉ＝１〜４）の値として、前記可動リンク先端部予測軌道により規定される時刻ｋでの第ｉ可動リンクの先端部の目標運動速度↑ＸLidot_refが決定される。

【0173】

また、↑Ｘbdot_ref(k)を算出するために用いる基体２の調整用運動速度↑Ｘbdot_aの値は、本実施形態では、例えば次のように決定される。

【0174】

すなわち、既に算出した時刻ｋ−１までの全体運動状態予測軌道と、全体運動量予測軌道設定部５２により算出された時刻ｋ−１までの基体２の基本目標位置姿勢↑Ｘb_ref_sの予測軌道とから、時刻ｋ−１での基体２の目標位置姿勢↑Ｘb_refと基本目標位置姿勢↑Ｘb_ref_sとの偏差Δ↑Ｘb、及びその時間的変化率ｄΔ↑Ｘb（換言すれば、↑Ｘb_refの時間的変化率と↑Ｘb_ref_sの時間的変化率との偏差）が算出される。そして、これらの偏差Δ↑Ｘb、及びその時間的変化率ｄΔ↑Ｘbから、次式（５）により、↑Ｘbdot_ref(k)を算出するために用いる基体２の調整用運動速度↑Ｘbdot_aの値が算出される。

【0175】

↑Ｘbdot_a＝Ｋp・Δ↑Ｘb＋Ｋv・ｄΔ↑Ｘb ……（５）

【0176】

なお、Ｋp、Ｋvはあらかじめ定められた所定値のゲインである。また、ｋ＝１の時刻での↑Ｘbdot_ref(k)を算出する場合においては、↑Ｘbdot_aはゼロベクトルとされる。

【0177】

以上の如く、全体運動状態予測軌道の各時刻ｋ（ｋ＝１，２，…，Ｎ）での↑Ｘbdot_ref(k)を算出するために用いるパラメータ（Ａ，Ａ^#，↑Ｐ，↑Ｌc，ＭLi（ｉ＝１〜４），ＨLi（ｉ＝１〜４），ＪLi^#，↑XLidot，↑Ｘbdot_a）の値が決定される。そして、これらのパラメータの値を用いて、前記式（３）の右辺の演算を行うことで、↑Ｘbdot_ref(k)が算出される。

【0178】

これにより、↑ＰＬ_refの予測軌道と、↑ＸLi_ref（ｉ＝１〜４）の予測軌道とを実現し得るように基体２の目標運動速度↑Ｘbdot_refの予測軌道が分解運動量制御の手法によって決定される。

【0179】

全体運動状態予測軌道決定部５３は、上記の如く↑Ｘbdot_refの予測軌道を算出した後、その↑Ｘbdot_refを積分することで、全体運動状態予測軌道の各時刻ｋでの基体２の目標位置姿勢↑Ｘb_ref(k)を算出する。これにより、全体運動状態予測軌道のうちの基体２の目標位置姿勢↑Ｘb_refの予測軌道が決定される。

【0180】

なお、この場合、↑Ｘbdot_refの積分演算におけるを↑Ｘb_refの初期値は、前回の制御処理周期で最終的に決定されたロボット１の基体２の目標位置姿勢↑Ｘb_cmdに一致するものとされる。

【0181】

そして、全体運動状態予測軌道決定部５３は、↑Ｘb_refの予測軌道と各可動リンク３，４の先端部の目標位置姿勢↑ＸLi_refの予測軌道とから、逆キネマティクスの演算によって、全体運動状態予測軌道の各時刻ｋでの各可動リンク３，４のそれぞれの目標関節変位量↑θLi_ref（ｉ＝１〜４）を算出する。これにより、全体運動状態予測軌道のうちの、各可動リンク３，４のそれぞれの目標関節変位量↑θLi_ref（ｉ＝１〜４）の予測軌道が決定される。

【0182】

全体運動状態予測軌道決定部５３の処理は、以上の如く実行される。かかる処理により決定される全体運動状態予測軌道は、ロボット１の各関節の関節運動制約条件を考慮せずに、ロボット１の目標全体運動量↑ＰＬ_refの予測軌道と、可動リンク３，４（第１〜第４可動リンク）のそれぞれの先端部の目標位置姿勢↑ＸLi_refの予測軌道とを実現し得るように決定されるものである。

【0183】

このため、全体運動状態予測軌道は、ロボット１のいずれかの可動リンク３，４の関節の変位量の可変範囲等の関節運動制約条件を満たしていない軌道となっている場合もある。

【0184】

動作目標生成部４２は、上記の如く全体運動状態予測軌道を決定した後、次に、運動加速度決定部５４の処理を実行する。

【0185】

運動加速度決定部５４は、ロボット１の関節運動制約条件の基で、現在時刻以後の複数の時刻ｋ（ｋ＝１〜Ｎ）におけるロボット１の所定種類の状態量に関する所定の評価関数を最小化し得るように、２次計画問題の解法手法の演算処理によって、ロボット１の基体２及び各可動リンク３，４の先端部のそれぞれの運動加速度↑Ｘbdot2，↑ＸLidot2（ｉ＝１〜４）の組として構成される運動加速度↑ｕの現在時刻以後の時系列↑ｕ_sol(k)（ｋ＝１〜Ｎ）を算出する。以降、当該算出される運動加速度↑ｕ_sol(k)を制約条件下運動加速度↑ｕ_sol(k)という。

【0186】

この場合、上記所定種類の状態量は、本実施形態では、ロボット１の基体２及び各可動リンク３，４の先端部のそれぞれの運動加速度↑Ｘbdot2，↑ＸLidot2（ｉ＝１〜４）と、ロボット１の全体運動量↑ＰＬとの２種類の状態量である。

【0187】

そして、運動加速度決定部５４は、制約条件下運動加速度↑ｕ_sol(k)のうち、初期時刻である現在時刻（ｋ＝１の時刻）での制約条件下運動加速度↑ｕ_sol(1)を、現在の制御処理周期で決定すべきロボット１の実際の制御用の目標運動加速度↑ｕ_cmdとして決定する。

【0188】

各制御処理周期で算出する現在時刻以後の上記制約条件下運動加速度↑ｕ_sol(1)〜↑ｕ_sol(N)は、より詳しく言えば、制約条件下運動加速度↑ｕ_solの軌道と前記全体運動状態予測軌道により規定される目標運動加速度↑ｕ_refの軌道との一致度合、並びに、制約条件下運動加速度↑ｕ_solの軌道に応じて発生する該ロボット１の全体運動量↑ＰＬ_solの軌道と前記全体運動量予測軌道（↑ＰＬ_refの予測軌道）との一致度合が、ロボット１の関節運動制約条件を満たす範囲内で、できるだけ高くなるような運動加速度↑ｕの時系列（現在時刻以後の所定の刻み時間ΔＴ毎の↑ｕの値の時系列）である。

【0189】

そして、ロボット１の関節運動制約条件は、本実施形態では、各関節の変位量と変位速度とのそれぞれの可変範囲の制約条件（各関節の変位量がそれぞれ所定の可変範囲内に収まるという制約条件、及び、各関節の変位速度がそれぞれ所定の可変範囲内に収まるという制約条件）である。

【0190】

このような制約条件下運動加速度↑ｕ_sol(1)〜↑ｕ_sol(N)を算出するための所定の評価関数は、本実施形態では次式（６）により表される評価関数Ｅfである。

【0191】

【数4】

【0192】

上記式（６）及び（６ａ）〜（６ｃ）において、↑ＰＬ_sol(k)は制約条件下運動加速度↑ｕ_sol(1)〜↑ｕ_sol(N)により規定されるロボット１の運動によって発生する全体運動量の推定値（時刻ｋでの値）である。

【0193】

この↑ＰＬ_sol(k)は、制約条件下運動加速度↑ｕ_solの積分によって算出される時刻ｋでの運動速度↑ｖ_sol(k)（詳しくは、↑ｕ_solの各成分↑Ｘbdot2_sol，↑ＸLidot2_sol（ｉ＝１〜４）のそれぞれを積分することで算出される時刻ｋでの運動速度↑Ｘbdot_sol，↑ＸLidot_sol（ｉ＝１〜４）を並べたベクトル）から、式（６ｃ）により算出される全体運動量である。

【0194】

そして、この式（６ｃ）におけるＢ(k)は、時刻ｋでのロボット１の運動状態において、運動速度↑ｖ（基体２及び各可動リンクの先端部のそれぞれの運動速度を成分として構成されるベクトル）を、全体運動量↑ＰＬに変換するヤコビアン行列である。この場合、式（６ｃ）の演算に用いるヤコビアン行列Ｂ(k)は、時刻ｋでのロボット１の運動状態が、前記全体運動状態予測軌道における時刻ｋでのロボット１の運動状態に一致すると見なして、当該運動状態に応じて決定される。

【0195】

また、↑ＰＬ_ref(k)は前記全体運動量予測軌道における時刻ｋでの目標全体運動量、↑ｕ_refは前記全体運動状態予測軌道により規定される時刻ｋでの目標運動加速度（基体２及び各可動リンクの先端部のそれぞれの運動加速度の目標値を成分として構成されるベクトル）、ＱPL，Ｒuはそれぞれロボット１の動作特性の調整用の重み係数行列（対角行列）である。

【0196】

かかる式（６）のｅf(k)（ｋ＝１〜Ｎ）は、式（６）の但し書きの式（６ａ）により定義される運動量偏差↑ＤPL(k)の各成分の二乗値を線形結合してなる値と、但し書きの式（６ｂ）により定義される運動加速度偏差↑Ｄu(k)の各成分の二乗値を線形結合してなる値とを加え合わせたものである。

【0197】

なお、上記重み係数行列ＱPL及びＲuは、それぞれ、ロボット１の所要の動作特性を実現し得るように、実験等に基づいてあらかじめ設定されるものである。ただし、ＱPL又はＲuとして、単位行列を採用することもできる。

【0198】

上記評価関数Ｅfの値は、↑ＰＬ_solの軌道と↑ＰＬ_refの軌道との一致度合を示す運動量評価指標値と、↑ｕ_solの軌道と↑ｕ_refの軌道との一致度合を示す運動加速度評価指標値とを合成した指標値に相当するものである。従って、Ｅfの値が小さいほど（ゼロに近いほど）、↑ｕ_solの軌道と↑ｕ_refの軌道との一致度合、及び↑ＰＬ_solの軌道と↑ＰＬ_refの軌道との一致度合が高いことを意味する。

【0199】

また、ロボット１の各関節の変位量と変位速度とに関する関節運動制約条件は、本実施形態では、次式（７）〜（１０）の不等式が用いられる。

【0200】

【数5】

【0201】

これらの式（７）〜（１０）における↑θLidot_min（ｉ＝１〜４）は、第ｉ可動リンクの各関節の変位速度の可変範囲の下限値を並べたベクトル（６成分のベクトル）、↑θLidot_max（ｉ＝１〜４）は、第ｉ可動リンクの各関節の変位速度の可変範囲の上限値を並べたベクトル（６成分のベクトル）、↑θLi_min（ｉ＝１〜４）は、第ｉ可動リンクの各関節の変位量の可変範囲の下限値を並べたベクトル（６成分のベクトル）、↑θLi_max（ｉ＝１〜４）は、第ｉ可動リンクの各関節の変位量の可変範囲の上限値を並べたベクトル（６成分のベクトル）、θfdot_minは基体関節１２の変位速度の可変範囲の下限値、θfdot_maxは基体関節１２の変位速度の可変範囲の上限値、θf_minは基体関節１２の変位量の可変範囲の下限値、θf_maxは基体関節１２の変位量の可変範囲の上限値である。

【0202】

また、↑θLidot_sol(k)（ｉ＝１〜４）は、↑ｕ_sol(1)〜↑ｕ_sol(N)により規定されるロボット１の運動状態における時刻ｋでの第ｉ可動リンクの各関節の変位速度を並べたベクトルである。そして、この↑θLidot_sol(k)は、次式（７ａ）により算出されるベクトルである。

【0203】

↑θLidot_sol(k)＝ＪLi(k)^#・(↑ＸLidot_sol(k)−Ｊbi(k)・↑Ｘbdot_sol(k))
……（７ａ）

【0204】

この式（７ａ）におけるＪLi(k)^#（ｉ＝１〜４）は、前記全体運動状態予測軌道における時刻ｋでのロボット１の運動状態において、第ｉ可動リンクの各関節の変位速度を並べたベクトル↑θLidotを該第ｉ可動リンクの先端部の運動速度↑ＸLidotに変換するヤコビアン行列ＪLi(k)の擬似逆行列、↑ＸLidot_sol(k)（ｉ＝１〜４）は、制約条件下運動加速度↑ｕ_solのうちの↑ＸLidot2_solを積分することで算出される時刻ｋでの第ｉ可動リンクの先端部の運動速度、↑Ｘbdot_sol(k)は、制約条件下運動加速度↑ｕ_solのうちの↑Ｘbdot2_solを積分することで算出される時刻ｋでの基体２の運動速度、Ｊbi(k)（ｉ＝１〜４）は、前記全体運動状態予測軌道における時刻ｋでのロボット１の運動状態において、基体２の運動速度↑Ｘbdotを第ｉ可動リンクの先端部の運動速度↑ＸLidotに変換するヤコビアン行列である。

【0205】

また、θLi_sol(k)は、↑θLidot_solを積分することにより算出される時刻ｋでの第ｉ可動リンクの各関節の変位量を並べたベクトル、θfdot_sol(k)は、制約条件下運動加速度↑ｕ_solのうちの↑θfdot2_sol（基体関節２の変位加速度）を積分することにより算出される時刻ｋでの基体関節２の変位速度、↑θf_sol(k)は、θfdot_solを積分することにより算出される基体関節１２の変位量である。

【0206】

式（７）〜（１０）の不等式（線形不等式）により示される関節運動制約条件の基で、式（６）により示される評価関数Ｅfの値を最小化するような制約条件下運動加速度↑ｕの時系列↑ｕ_sol(1)〜↑ｕ_sol(N)は、次のように算出される。

【0207】

すなわち、運動加速度決定部５４は、まず、式（６）の右辺の↑ＤPL(k)を算出するために必要な前記ヤコビアン行列Ｂ(k)（ｋ＝１〜Ｎ）と、式（７）のθLidot_sol(k)を算出するために必要なヤコビアン行列ＪLi(k)、Ｊbi(k)（ｉ＝１〜４、ｋ＝１〜Ｎ）とを各々算出する。

【0208】

この場合、ヤコビアン行列Ｂ(k)，ＪLi(k)、Ｊbi(k)は、時刻ｋでのロボット１の全体の運動状態が、全体運動状態予測軌道の時刻ｋでの全体の運動状態の目標値↑Ｘb_ref(k)，↑θLi_ref(k)（ｉ＝１〜４）に一致する状態でのヤコビアン行列として算出される。なお、ヤコビアン行列Ｂ(k)の算出処理では、ロボット１の各リンク要素に質量とイナーシャとを設定した動力学モデルが使用される。

【0209】

さらに、運動加速度決定部５４は、ヤコビアン行列ＪLi(k)の擬似逆行列ＪLi(k)^#を算出する。この場合、本実施形態では、各可動リンク３，４の関節の個数（↑θLidotの成分数）は６個であるので、ＪLi(k)^#は、ＪLi(k)の逆行列ＪLi(k)^-1として算出される。

【0210】

そして、運動加速度決定部５４は、（７）〜（１０）の不等式により示される制約条件の基で、式（６）により示される評価関数Ｅfの値を最小化するような制約条件下運動加速度↑ｕの時系列↑ｕ_sol(1)〜↑ｕ_sol(N)を、ヤコビアン行列Ｂ(k)，Ｊbi(k)（ｉ＝１〜４）と、擬似逆行列ＪLi(k)^#（ｉ＝１〜４、ｋ＝１〜Ｎ）を用いて、２次計画問題の公知の解法手法の演算処理を実行することで、算出する。

【0211】

この場合、２次計画問題の解法手法としては、例えば、内点凸法、あるいは、信頼領域Reflective法を使用することができる。

【0212】

運動加速度決定部５４は、以上の如く制約条件下運動加速度↑ｕの時系列↑ｕ_sol(1)〜↑ｕ_sol(N)を算出する。

【0213】

かかる処理により、制約条件下運動加速度↑ｕ_solの軌道と前記全体運動状態予測軌道により規定される目標運動加速度↑ｕ_refの軌道との一致度合、並びに、制約条件下運動加速度↑ｕ_solの軌道に応じて発生する該ロボット１の全体運動量↑ＰＬ_solの軌道と前記全体運動量予測軌道（↑ＰＬ_refの予測軌道）との一致度合が、ロボット１の各関節の作動に関する制約条件を満たす範囲内で、できるだけ高くなるように、制約条件下運動加速度↑ｕ_solの時系列↑ｕ_sol(1)〜↑ｕ_sol(N)が、２次計画問題の解法手法の演算処理により算出される。

【0214】

この場合、制約条件下運動加速度↑ｕ_solの軌道に応じて発生する該ロボット１の全体運動量↑ＰＬ_solの軌道の各時刻ｋでの値↑ＰＬ_sol(k)は、各関節の作動に関する制約条件を考慮せずに決定された全体運動状態予測軌道に応じて規定されるヤコビアン行列を用いて算出される。このため、２次計画問題の演算処理を短時間で実行することができる。

【0215】

そして、運動加速度決定部５４は、制約条件下運動加速度↑ｕ_solの時系列↑ｕ_sol(1)〜↑ｕ_sol(N)のうちの現在時刻（ｋ＝１の時刻）での制限条件下運動加速度↑ｕ_sol(1)を、現在の制御処理周期で決定すべき目標運動加速度↑ｕ_cmdとして決定する。

【0216】

このようにして、各制御処理周期毎に、目標運動加速度↑ｕ_cmdが決定されることとなる。

【0217】

運動加速度決定部５４の処理は、以上の如く実行される。

【0218】

動作目標生成部４２は、次に、制御用目標位置姿勢決定部５５及び関節変位量決定部５６の処理を順次実行する。

【0219】

制御用目標位置姿勢決定部５５は、運動加速度決定部５４が各制御処理周期で決定した目標運動加速度↑ｕ_cmdの各成分の積分演算（２階積分）を各制御処理周期で実行する。これにより、各制御処理周期でのロボット１の基体２の位置及び姿勢の制御用目標値↑Ｘb_cmdと各可動リンク３，４の先端部のそれぞれの位置及び姿勢の制御用目標値↑ＸLi_cmd（ｉ＝１〜４）とが決定される。

【0220】

次いで、関節変位量決定部５６は、各制御処理周期で制御用目標位置姿勢決定部５５が決定した制御用目標値↑Ｘb_cmd，↑ＸLi_cmd（ｉ＝１〜４）から逆キネマティクスの演算によってロボット１の各関節の目標変位量を決定する。

【0221】

以上説明した動作目標生成部４２の処理によって、制御処理周期毎に、ロボット１の各関節の目標変位量が決定される。

【0222】

かかる本実施形態によれば、各制御処理周期において、現在時刻以後の複数の時刻ｋ（ｋ＝１〜Ｎ）における制約条件下運動加速度↑ｕ_sol(k)の時系列が、ロボット１の各関節の変位量及び変位速度のそれぞれの可変範囲に関する関節運動制約条件を満たす範囲内で、ロボット１の全体運動量↑ＰＬと運動加速度↑ｕとに関する前記評価関数Ｅfの値を最小化するように、２次計画問題の解法手法の演算処理によって算出される。

【0223】

この場合、評価関数Ｅfにおける全体運動量↑ＰＬ_solを算出するためのヤコビアン行列は、前記全体運動状態予測軌道により規定されるヤコビアン行列であるので、２次計画問題の解法手法の演算処理を効率よく短時間で実行できる。

【0224】

そして、２次計画問題の解法手法の演算処理により算出した制約条件下運動加速度↑ｕ_sol(k)の時系列のうちの初期時刻としての現在時刻での制約条件下運動加速度↑ｕ_sol(1)が現在の制御処理周期で求めるべき目標運動加速度↑ｕ_cmdとされる。

【0225】

制御処理周期毎に決定した目標運動加速度↑ｕ_cmdを実現し得るように各制御処理周期における各関節の目標変位量が決定される。

【0226】

このため、近い将来に関節運動制約条件が満たされなくなる虞がある状況で、該関節運動制約条件が満たされなくなる状況が発生するのを事前に防止するようにロボット１の動作目標（各関節の目標変位量）を決定できる。

【0227】

これにより、関節運動制約条件を満たし得る範囲内でロボット１の継続的な動作を滑らかに行い得る動作目標を決定できる。

【0228】

また、本実施形態では、評価関数Ｅfにおける全体運動量↑ＰＬ_solのうちの角運動量↑Ｌ_solが目標ＺＭＰ↑Zmp_refの周りの角運動量であるので、動力学的に適切なロボット１の動作を実現し得る動作目標を生成できる。

【0229】

次に、以上説明した実施形態の効果の検証について図６を参照して説明する。図６（ａ）は、実施例のグラフを示し、図６（ｂ）は比較例のグラフを示している。実施例のグラフは、前記実施形態において、ロボット１の一歩分の歩幅を大きな歩幅（例えば７５０ｍｍ）に設定してロボット１の動作目標の生成処理を行った場合に、遊脚として動作させる脚リンク３の膝関節部１５の関節２２の目標変位量の経時変化を示すグラフである。

【0230】

また、比較例のグラフは、現在時刻以後の将来の全体運動状態予測軌道を生成することなく、上記の歩幅でのロボット１の動作目標の生成処理を行った場合に、遊脚として動作させる脚リンク３の膝関節部１５の関節２２の目標変位量の経時変化を示すグラフである。この比較例での動作目標の生成処理は、具体的には、前記式（６）におけるＮの値を「１」として動作目標を生成する処理である。

【0231】

図６（ｂ）に見られるように、比較例では、図中のＣ部分において、関節２２の目標変位量が、該関節２２の変位量の可変範囲の下限値に保持されてしまうものとなっている。

【0232】

これに対して、図６（ａ）に見られるように、実施例では、関節２２の目標変位量が、下限値に保持されるような状況を生じることなく滑らかに変化している。これは、近い将来に関節２２の目標変位量が可変範囲を逸脱しそうな状況になると、当該状況が発生するのを事前に防止するように各制御処理周期での前記制約条件下運動加速度↑ｕ_solが決定され、ひいては、ロボット１の各関節の変位量が決定されるためであると考えられる。

【0233】

なお、以上説明した実施形態におけるロボット１は、腕リンク４Ｒ，４Ｌ及び頭部５を有する人型の移動ロボットであるが、これらの腕リンク４Ｒ，４Ｌ、あるいは、頭部５を備えない移動ロボットであってもよい。また、基体関節１２を省略し、下側基体１０及び上側基体１１を一体に構成してもよい。

【0234】

また、移動ロボット１は、３つ以上の脚リンクを備えていてもよい。また、腕リンクの個数が１つであってよく、あるいは、３つ以上であってもよい。また、基体関節１２は、２以上の運動自由度を有するものであってもよい。また、移動ロボット１は、回転型の関節に限らず、直動型の関節を含んでいてもよい。また、ロボット１は、回転型の関節以外に直動型の関節を含んでいてもよい。

【0235】

また、移動ロボット１の各可動リンク３，４は、７つ以上の関節を備えていてもよい。

【0236】

また、前記実施形態では、関節運動制約条件として、各関節の変位量の可変範囲の制約条件と各関節の変位速度の可変範囲の制約条件とを用いたが、いずれか一方の制約条件だけを用いてもよい。あるいは、各関節の変位加速度の可変範囲の制約条件を、関節運動制約条件として用いてもよい。

【符号の説明】

【0237】

１…移動ロボット、２…基体、３，４…可動リンク、５１…可動リンク先端部予測軌道設定部（可動リンク先端部予測軌道設定手段）、５２…全体運動量予測軌道設定部（全体運動量予測軌道設定手段）、５４…運動加速度決定部（運動加速度決定手段）、５５…御用目標位置姿勢決定部（関節制御用目標値決定手）、５６…関節変位量決定部（関節制御用目標値決定手）。

【図1】

【図2】

【図3】

【図4】

【図5】

【図6】