(57)【特許請求の範囲】
【請求項１】
基体から延設されると共に複数の関節がそれぞれ備えられた複数の脚リンクの運動によって移動する脚式移動ロボットの各関節の変位量を規定する目標歩容を逐次生成する歩容生成装置であって、
前記目標歩容の構成要素としての前記移動ロボットの各脚リンクの先端部の位置及び姿勢の目標値である目標脚位置姿勢を逐次決定する目標脚位置姿勢決定手段と、
前記移動ロボットの前記基体と前記複数の脚リンク以外で該基体に連結されている部位とのうちのいずれか一方の部位である特定部位の位置及び姿勢の単位時間当たりの変化量を成分として構成される特定部位運動速度ベクトルとの間の関係が線形写像により表されるベクトルとしての制御対象ベクトルの目標値である目標制御対象ベクトルを逐次決定する目標制御対象ベクトル決定手段と、
前記線形写像を表すヤコビアン行列を、既に決定された前記移動ロボットの目標歩容のうちの少なくとも最新値の目標歩容を用いて逐次決定するヤコビアン行列決定手段と、
前記特定部位運動速度ベクトルを変数とする２次形式の所定の評価関数であって、少なくとも前記決定された目標制御対象ベクトルと前記決定されたヤコビアン行列を該特定部位運動速度ベクトルに乗じてなるベクトルとの差分に応じて値が変化するように構成された評価関数と、前記特定部位運動速度ベクトルを変数として構成された１つ以上の線形行列不等式とを用い、該線形行列不等式が成立するという制約条件を満たす範囲内で、前記評価関数の値を最小化し得る前記特定部位運動速度ベクトルの値を２次計画問題の解法手法の演算処理により逐次算出し、その算出した前記特定部位運動速度ベクトルの値を該特定部位運動速度ベクトルの目標値として逐次決定する特定部位運動速度目標値決定手段と、
前記決定された特定部位運動速度ベクトルの目標値を逐次積分することにより、前記目標歩容の構成要素としての前記特定部位の位置及び姿勢の目標値である目標特定部位位置姿勢を逐次決定する目標特定部位位置姿勢決定手段とを備え、
前記特定部位運動速度目標値決定手段が前記２次計画問題の解法手法の演算処理で用いる前記線形行列不等式は、前記特定部位と前記各脚リンクの先端部との間の各関節の変位量とその時間的変化率である関節速度とのうちの少なくともいずれか一方の値を制限する条件である関節動作制約条件を満たすように設定された線形行列不等式であることを特徴とする脚式移動ロボットの歩容生成装置。
【請求項２】
請求項１記載の脚式移動ロボットの歩容生成装置において、
前記関節動作制約条件は、前記特定部位と前記各脚リンクの先端部との間の各関節の関節速度の値があらかじめ定められた可動範囲内に収まるという条件を少なくとも含んでおり、
前記特定部位運動速度目標値決定手段が前記２次計画問題の解法手法の演算処理で用いる前記線形行列不等式は、前記各脚リンク毎に、次式（１００ａ）及び（１００ｂ）により表される２つの線形行列不等式を含んでおり、
次式（１００ａ）における↑ω_i_minの各成分の値が、前記特定部位と前記各脚リンクの先端部との間の各関節のうちの該成分に対応する関節の関節速度の可動範囲の下限値以上の値となるように該↑ω_i_minを設定し、次式（１００ｂ）における↑ω_i_maxの各成分の値が、前記特定部位と前記各脚リンクの先端部との間の各関節のうちの該成分に対応する関節の関節速度の可動範囲の上限値以下の値となるように該↑ω_i_maxを設定したことを特徴とする脚式移動ロボットの歩容生成装置。
Ｃ_i・↑Ｖｓ＋（↑ｃ_i−↑ω_i_min）≧↑０    ……（１００ａ）
−Ｃ_i・↑Ｖｓ＋（−↑ｃ_i＋↑ω_i_max）≧↑０    ……（１００ｂ）
ただし、
添え字ｉ：脚リンクの識別番数
Ｃ_i：Ｃ_i≡−(Ｊ_i)-1・Ｂ_iにより定義される行列
↑ｃ_i：↑ｃ_i≡(Ｊ_i)-1・↑Ｖft_iにより定義されるベクトル
(Ｊ_i)-1：Ｊ_iの逆行列又は擬似逆行列
Ｊ_i：↑ω_iから↑Ｖft_iへの線形写像を表すヤコビアン行列
↑ω_i：前記特定部位と第ｉ番目の脚リンクの先端部との間に存在する各関節の関節速度を成分として構成されるベクトル
↑Ｖft_i：第ｉ番目の脚リンクの先端部の位置の時間的変化率と該先端部の姿勢の時間的変化率とを成分として構成されるベクトル
Ｂ_i：↑Ｖｓから↑Ｖft_iへの線形写像を表す行列
↑Ｖｓ：前記特定部位運動速度ベクトル
↑ω_i_min：↑ω_iの各成分の許容範囲の下限値により構成されるベクトル
↑ω_i_max：↑ω_iの各成分の許容範囲の上限値により構成されるベクトル
↑０：零ベクトル
【請求項３】
請求項２記載の脚式移動ロボットにおいて、
前記関節動作制約条件は、前記特定部位と前記各脚リンクの先端部との間の各関節の変位量の値があらかじめ定められた可動範囲内に収まるという条件をさらに含んでおり、
前記特定部位運動速度目標値決定手段は、
前記式（１００ａ）における↑ω_i_minの各成分の値が、前記特定部位と前記各脚リンクの先端部との間の各関節のうちの該成分に対応する関節の関節速度の可動範囲の下限値以上の値となり、且つ、前記最新値の目標歩容により規定される該関節の変位量である最新目標関節変位量が該関節の変位量の可動範囲の下限値に近づくに伴い該関節に対応する↑ω_i_minの成分の値がゼロまで変化していくように該最新目標関節変位量に応じて↑ω_i_minを設定する手段と、
前記式（１００ｂ）における↑ω_i_maxの各成分の値が、前記特定部位と前記各脚リンクの先端部との間の各関節のうちの該成分に対応する関節の関節速度の可動範囲の上限値以下の値となり、且つ、該関節の前記最新目標関節変位量が該関節の変位量の可動範囲の上限値に近づくに伴い該関節に対応する↑ω_i_maxの成分の値がゼロまで変化していくように該最新目標関節変位量に応じて↑ω_i_maxを設定する手段とを備えることを特徴とする脚式移動ロボットの歩容生成装置。
【請求項４】
請求項１〜３のいずれか１項に記載の脚式移動ロボットにおいて、
前記移動ロボットの運動を行なうための動力学的な制約条件が該移動ロボットの動力学を表す動力学モデル上で満たされるように、該動力学モデルと前記決定された目標脚位置姿勢とを用いて、該移動ロボットの基体の位置及び姿勢の基本目標値である基本目標基体位置姿勢と、該移動ロボットの全体重心の位置の基本目標値である基本目標全体重心位置と、該移動ロボットの全体重心の並進運動量ベクトルの基本目標値である基本目標並進運動量ベクトルと、該移動ロボットの全体重心周りの角運動量ベクトルの基本目標値である基本目標角運動量ベクトルとを逐次決定する基本動作目標値決定手段と、
前記決定された目標脚位置姿勢と目標基体位置姿勢とを構成要素とする前記目標歩容に対応する前記移動ロボットの全体重心の位置である目標歩容上全体重心位置を、該目標歩容から前記移動ロボットの幾何学モデルに基づいて逐次算出する目標歩容上重心位置算出手段と、
前記算出された目標歩容上全体重心位置を前記決定された基本目標全体重心位置に追従させるために該目標歩容上全体重心位置と基本目標全体重心位置との偏差に応じて決定したフィードバック操作量により前記決定された基本目標並進運動量ベクトルを修正してなる修正後基本目標並進運動量ベクトルを逐次決定する基本目標並進運動量修正手段と、
前記特定部位としての前記基体の位置及び姿勢の単位時間当たりの変化量を成分として構成されるベクトルを前記特定部位運動速度ベクトルとして用い、前記目標基体位置姿勢を前記基本目標基体位置姿勢に追従させるように、前記決定された目標基体位置姿勢と前記決定された基本目標基体位置姿勢との偏差に応じて前記特定部位運動速度ベクトルの基本目標値である基本目標基体運動速度ベクトルを逐次決定する基本目標基体運動速度決定手段とをさらに備え、
前記制御対象ベクトルは、前記特定部位運動速度ベクトルの各成分と前記移動ロボットの全体重心の並進運動量ベクトルの各成分と該移動ロボットの全体重心の周りの角運動量ベクトルの各成分とにより構成されるベクトルであり、
前記目標制御対象ベクトル決定手段は、前記決定された基本目標基体運動速度ベクトルの各成分と、前記決定された修正後基本目標並進運動量ベクトルの各成分と、前記決定された基本目標角運動量ベクトルの各成分とにより構成されるベクトルを前記目標制御対象ベクトルとして決定することを特徴とする脚式移動ロボットの歩容生成装置。
【請求項５】
請求項１〜４のいずれか１項に記載の脚式移動ロボットの歩容生成装置において、
前記ヤコビアン行列決定手段は、前記特定部位運動速度ベクトルから前記制御対象ベクトルへの線形写像を表す新たなな前記ヤコビアン行列を決定するとき、既に決定された前記移動ロボットの目標歩容のうちの最新値の目標歩容に対応する時刻と前記特定部位運動速度ベクトルの新たな目標値に応じて前記目標特定部位位置姿勢決定手段により決定される目標特定部位位置姿勢を構成要素とする新たな目標歩容に対応する時刻との間の期間内での前記移動ロボットの将来の歩容の瞬時値を、前記最新値の目標歩容とその直前の１つ以上の過去値の目標歩容とに基づいて推定し、その推定した将来の歩容の瞬時値を、前記特定部位運動速度ベクトルによる前記特定部位の位置及び姿勢の変化の起点となる前記移動ロボットの動作状態として用いて、該移動ロボットの当該起点の動作状態に対応する前記ヤコビアン行列を決定することを特徴とする脚式移動ロボットの歩容生成装置。
【請求項６】
請求項５記載の脚式移動ロボットの歩容生成装置において、
前記特定部位運動速度目標値決定手段は、前記最新値の目標歩容における目標特定部位位置姿勢と該最新値の目標歩容の１つ前の過去値の目標歩容における目標特定部位位置姿勢との差分に“０”と“１”との間の範囲内であらかじめ定めた係数Ｋｐを乗じてなる補正量を、前記最新値の目標歩容における目標特定部位位置姿勢に加えることにより該目標特定部位位置姿勢の将来の瞬時値を推定する手段と、各脚リンク毎に前記最新値の目標歩容における目標脚位置姿勢と該最新値の１つ前の過去値の目標歩容における目標脚位置姿勢との差分に前記係数Ｋｐを乗じてなる補正量を、前記最新値の目標歩容における目標脚位置姿勢に加えることにより各脚リンクの目標脚位置姿勢の将来の瞬時値を推定する手段とを備え、前記推定された目標特定部位位置姿勢の将来の瞬時値と各脚リンクの目標脚位置姿勢の将来の瞬時値とを構成要素とする歩容を、前記移動ロボットの当該起点の動作状態として用いることを特徴とする脚式移動ロボットの歩容生成装置。
【請求項７】
空間的な位置及び姿勢が可変である特定部位と、該特定部位から延設されると共に複数の関節が備えられたリンク機構とを備えるロボットの各関節の変位量を規定する動作目標を逐次生成する動作目標生成装置であって、
前記動作目標の構成要素としての前記リンク機構の先端部の位置及び姿勢の目標値であるリンク機構目標先端位置姿勢を逐次決定するリンク機構目標先端位置姿勢決定手段と、
前記ロボットの前記特定部位の位置及び姿勢の単位時間当たりの変化量を成分として構成される特定部位運動速度ベクトルとの間の関係が線形写像により表されるベクトルとしての制御対象ベクトルの目標値である目標制御対象ベクトルを逐次決定する目標制御対象ベクトル決定手段と、
前記線形写像を表すヤコビアン行列を、既に決定された前記ロボットの動作目標のうちの少なくとも最新値の動作目標を用いて逐次決定するヤコビアン行列決定手段と、
前記特定部位運動速度ベクトルを変数とする２次形式の所定の評価関数であって、少なくとも前記決定された目標制御対象ベクトルと前記決定されたヤコビアン行列を該特定部位運動速度ベクトルに乗じてなるベクトルとの差分に応じて値が変化するように構成された評価関数と、前記特定部位運動速度ベクトルを変数として構成された１つ以上の線形行列不等式とを用い、該線形行列不等式が成立するという制約条件を満たす範囲内で、前記評価関数の値を最小化し得る前記特定部位運動速度ベクトルの値を２次計画問題の解法手法の演算処理により逐次算出し、その算出した前記特定部位運動速度ベクトルの値を該特定部位運動速度ベクトルの目標値として逐次決定する特定部位運動速度目標値決定手段と、
前記決定された特定部位運動速度ベクトルの目標値を逐次積分することにより、前記動作目標の構成要素としての前記特定部位の位置及び姿勢の目標値である目標特定部位位置姿勢を逐次決定する目標特定部位位置姿勢決定手段とを備え、
前記特定部位運動速度目標値決定手段が前記２次計画問題の解法手法の演算処理で用いる前記線形行列不等式は、前記特定部位と前記リンク機構の先端部との間の各関節の変位量とその時間的変化率である関節速度とのうちの少なくともいずれか一方の値を制限する条件である関節動作制約条件を満たすように設定された線形行列不等式であることを特徴とするロボットの動作目標生成装置。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は脚式移動ロボットの歩容生成装置及びロボットの動作目標生成装置に関する。
【背景技術】
【０００２】
従来、脚式移動ロボットの目標歩容を生成する技術として、例えば非特許文献１に見られるよう、移動ロボットの全体の並進運動量ベクトル（全体重心の並進運動量ベクトル）と、該移動ロボットの全体の角運動量ベクトルとの目標を設定し、その目標を満足するように移動ロボットの各関節の変位量を規定する目標歩容を生成する技術が提案されている。
【０００３】
また、この技術を利用して、二足歩行ロボットの歩容生成を行う技術が、例えば特許文献１に記載されている。
【先行技術文献】
【特許文献】
【０００４】
【特許文献１】ＷＯ２００８／１０５５０６
【非特許文献】
【０００５】
【非特許文献１】「分解運動量制御：運動量と角運動量に基づくヒューマノイドロボットの全身運動生成」／梶田、金広、金子、藤原、原田、横井、比留川／日本ロボット学会誌、Vol.22  No.6，pp.772-779, 2004
【発明の概要】
【発明が解決しようとする課題】
【０００６】
脚式移動ロボット等の種々様々なロボットの各関節の変位量や、その変位量の時間的変化率（関節の変位速度）は、一般に、該関節もしくは該関節の駆動機構の機構的な構造、あるいは、該関節を駆動するアクチュエータの能力等に起因する制約を受け、その制約により規定されるある可動範囲内で変化させることが可能となっている。
【０００７】
従って、ロボットの目標歩容等の動作目標を決定する場合、該動作目標により規定される各関節の変位量又は、その時間的変化率は、それぞれの可動範囲を逸脱しないように設定する必要がある。
【０００８】
しかるに、前記特許文献１及び非特許文献１に見られる如き従来の技術では、ロボットの各関節の変位量やその時間的変化率の可動範囲に関する制約条件が考慮されていない。このため、ロボットの目標歩容における歩幅や、目標とする並進運動量ベクトルや角運動量ベクトルによっては、脚リンクのいずれかの関節の変位量又はその時間的変化率が、可動範囲から逸脱するような目標歩容が生成されてしまう恐れがある。
【０００９】
そして、そのような場合には、該関節の目標とする変位量やその時間的変化率を強制的に可動範囲の上限値又は下限値に制限せざるを得なくなる。その結果、ロボットの動作の滑らかさが損なわれるような動作目標や、ロボットの動作を適切に行い得る本来の動作目標から乖離した動作目標が決定されてしまうという不都合がある。
【００１０】
また、このような不都合を回避するために、生成可能なロボットの動作目標の形態が、ロボットの各関節の変位量やその時間的変化率の可動範囲に関する制約条件を確実に満たし得るような限定的な形態に制限されてしまうというような不都合がある。
【００１１】
本発明はかかる背景に鑑みてなされたものであり、ロボットの一例としての脚式移動ロボットの移動を行なわせる目標歩容を生成する場合に、各脚リンクの関節の動作を制限する制約条件を適切に反映させた手法で、ロボットの目標歩容を生成することができる歩容生成装置を提供することを目的とする。
【００１２】
また、本発明は、より一般的な態様として、種々様々なロボットにおいて、ロボットの特定部位から延設されたリンク機構の関節の動作を制限する制約条件を適切に反映させた手法で、該リンク機構を動かすための動作目標を生成することができる動作目標生成装置を提供することを目的とする。
【課題を解決するための手段】
【００１３】
本発明の脚式移動ロボットの歩容生成装置は、かかる目的を達成するために、基体から延設されると共に複数の関節がそれぞれ備えられた複数の脚リンクの運動によって移動する脚式移動ロボットの各関節の変位量を規定する目標歩容を逐次生成する歩容生成装置であって、
前記目標歩容の構成要素としての前記移動ロボットの各脚リンクの先端部の位置及び姿勢の目標値である目標脚位置姿勢を逐次決定する目標脚位置姿勢決定手段と、
前記移動ロボットの前記基体と前記複数の脚リンク以外で該基体に連結されている部位とのうちのいずれか一方の部位である特定部位の位置及び姿勢の単位時間当たりの変化量を成分として構成される特定部位運動速度ベクトルとの間の関係が線形写像により表されるベクトルとしての制御対象ベクトルの目標値である目標制御対象ベクトルを逐次決定する目標制御対象ベクトル決定手段と、
前記線形写像を表すヤコビアン行列を、既に決定された前記移動ロボットの目標歩容のうちの少なくとも最新値の目標歩容を用いて逐次決定するヤコビアン行列決定手段と、
前記特定部位運動速度ベクトルを変数とする２次形式の所定の評価関数であって、少なくとも前記決定された目標制御対象ベクトルと前記決定されたヤコビアン行列を該特定部位運動速度ベクトルに乗じてなるベクトルとの差分に応じて値が変化するように構成された評価関数と、前記特定部位運動速度ベクトルを変数として構成された１つ以上の線形行列不等式とを用い、該線形行列不等式が成立するという制約条件を満たす範囲内で、前記評価関数の値を最小化し得る前記特定部位運動速度ベクトルの値を２次計画問題の解法手法の演算処理により逐次算出し、その算出した前記特定部位運動速度ベクトルの値を該特定部位運動速度ベクトルの目標値として逐次決定する特定部位運動速度目標値決定手段と、
前記決定された特定部位運動速度ベクトルの目標値を逐次積分することにより、前記目標歩容の構成要素としての前記特定部位の位置及び姿勢の目標値である目標特定部位位置姿勢を逐次決定する目標特定部位位置姿勢決定手段とを備え、
前記特定部位運動速度目標値決定手段が前記２次計画問題の解法手法の演算処理で用いる前記線形行列不等式は、前記特定部位と前記各脚リンクの先端部との間の各関節の変位量とその時間的変化率である関節速度とのうちの少なくともいずれか一方の値を制限する条件である関節動作制約条件を満たすように設定された線形行列不等式であることを特徴とする（第１発明）。
【００１４】
かかる第１発明によれば、前記目標脚位置姿勢決定手段により決定された各脚リンクの目標脚位置姿勢（各脚リンクの先端の位置及び姿勢の目標値）と、前記特定部位運動速度目標値決定手段により逐次決定される特定部位運動速度ベクトル（特定部位の位置及び姿勢の単位時間当たりの変化量を成分として構成されるベクトル）の目標値を前記目標特定部位位置姿勢決定手段により逐次積分することにより決定された目標特定部位位置姿勢（特定部位の位置及び姿勢の目標値）とが前記目標歩容の構成要素として逐次決定されることとなる。
【００１５】
この場合、特定部位運動速度ベクトルの目標値を決定する特定部位運動速度目標値決定手段は、前記線形行列不等式が成立するという制約条件を満たす範囲内で、前記評価関数の値を最小化し得る前記特定部位運動速度ベクトルの値を２次計画問題の解法手法の演算処理により逐次算出し、その算出した前記特定部位運動速度ベクトルの値を該特定部位運動速度ベクトルの目標値として逐次決定する。
【００１６】
ここで、上記評価関数は、前記特定部位運動速度ベクトルを変数とする２次形式の所定の評価関数であって、少なくとも前記決定された目標制御対象ベクトルと前記決定されたヤコビアン行列を該特定部位運動速度ベクトルに乗じてなるベクトルとの差分に応じて値が変化するように構成された評価関数である。
【００１７】
また、前記線形行列不等式は、前記特定部位運動速度ベクトルを変数として構成された線形行列不等式である。
【００１８】
従って、前記線形行列不等式が成立するという制約条件を満たす範囲内で、前記評価関数の値を最小化し得る前記特定部位運動速度ベクトルの値を求めるという処理は、所謂、２次計画問題の解法手法の演算処理により実現できることとなる。また、２次計画問題の解法手法の演算処理は、解の値を比較的高速に決定し得る種々の手法が公知となっている。
【００１９】
そこで、第１発明では、特定部位運動速度目標値決定手段は、上記の如く２次計画問題の解決手法の演算処理によって、特定部位運動速度ベクトルの目標値を逐次決定する。
【００２０】
なお、２次計画問題の解決手法の演算処理の手法は、公知の手法でよく、例えば、内点凸法、あるいは、信頼領域Reflective法等の手法を用いればよい。
【００２１】
また、移動ロボットの各脚リンクの先端部の運動速度ベクトル（該先端部の位置及び姿勢の時間的変化率）は、前記特定部位運動速度ベクトル（前記特定部位の位置及び姿勢の時間的変率）と、該特定部位と該脚リンクの先端部との間の各関節の関節速度（変位量の時間的変化率）とに依存したものとなる。そして、移動ロボットの各脚リンクの先端部の運動速度は、前記目標脚位置姿勢の軌道（時系列）により規定される。
【００２２】
従って、前記特定部位運動速度ベクトルに関する制約条件としての前記線形行列不等式を、前記特定部位と該脚リンクの先端部との間の各関節の変位量あるいはその時間的変化率を制限する制約条件（前記関節動作制約条件）を反映させて設定することができる。
【００２３】
より詳しくは、前記線形行列不等式が成立する場合に、前記関節動作制約条件を満たすように該線形行列不等式を設定することができる。換言すれば、前記線形行列不等式が成立するということが、前記関節動作制約条件を満たすための十分条件になるようにすることができる。
【００２４】
そこで、第１発明においては、前記特定部位運動速度目標値決定手段が前記２次計画問題の解法手法の演算処理で用いる前記線形行列不等式を、前記特定部位と前記各脚リンクの先端部との間の各関節の変位量とその時間的変化率である関節速度とのうちの少なくともいずれか一方の値を制限する条件である関節動作制約条件を満たすように設定した。
【００２５】
このようにすることで、前記関節動作制約条件は、前記線形行列不等式を満たすという条件に変換されることとなる。このため、前記特定部位運動速度目標値決定手段によって、上記の如く前記線形行列不等式が成立するという制約条件を満たす範囲内で、前記評価関数の値を最小化し得る前記特定部位運動速度ベクトルの値としての前記特定部位運動速度ベクトルの目標値を決定することによって、結果的に、前記関節動作制約条件を満たす範囲内で、前記評価関数の値を最小化し得るような特定部位運動速度ベクトルの目標値を決定できることなる。
【００２６】
そして、この場合、前記評価関数は、前記決定された目標制御対象ベクトルと前記決定されたヤコビアン行列を該特定部位運動速度ベクトルに乗じてなるベクトルとの差分に応じて値が変化するように構成された２次形式の評価関数であるから、前記関節動作制約条件を満たす範囲内で、前記決定されたヤコビアン行列を該特定部位運動速度ベクトルの目標値に乗じてなるベクトルが、前記決定された目標制御対象ベクトルに極力一致もしくはほぼ一致するように、該特定部位運動速度ベクトルの目標値が逐次決定されることとなる。
【００２７】
なお、この場合、目標制御対象ベクトルは、移動ロボットの所要の移動動作を適切に行うために要求される制御対象ベクトルの値としての意味を持つ。また、前記決定されたヤコビアン行列を該特定部位運動速度ベクトルの目標値に乗じてなるベクトルは最終的に決定される目標歩容に対応する制御対象ベクトルの値に相当する。
【００２８】
従って、特定部位運動速度ベクトルの目標値、ひいては、目標歩容は、前記関節動作制約条件による関節（特定部位と各脚リンクの先端部との間の関節）の動作の制約を満たしつつ、制御対象ベクトルに関する目標が極力達成させるように決定されることとなる。
【００２９】
特に、特定部位と各脚リンクの先端部との間の関節の動作（変位量や関節速度）が関節動作制約条件により規定される限界付近の動作となる状況でも、前記決定されたヤコビアン行列を該特定部位運動速度ベクトルの目標値に乗じてなるベクトルが、前記目標制御対象ベクトルから乖離するのが極力抑制されるように特定部位運動速度ベクトルの目標値が決定されることとなる。
【００３０】
よって、第１発明によれば、脚式移動ロボットの移動を行なわせる目標歩容を生成する場合に、各脚リンクの関節の動作を制限する制約条件を適切に反映させた手法で、該ロボットの目標歩容を生成することができる。
【００３１】
かかる第１発明のより具体的な態様としては、例えば次のような態様が好適である。すなわち、その好適な態様では、前記関節動作制約条件は、前記特定部位と前記各脚リンクの先端部との間の各関節の関節速度の値があらかじめ定められた可動範囲内に収まるという条件を少なくとも含んでおり、前記特定部位運動速度目標値決定手段が前記２次計画問題の解法手法の演算処理で用いる前記線形行列不等式は、前記各脚リンク毎に、次式（１００ａ）及び（１００ｂ）により表される２つの線形行列不等式を含んでいる。そして、次式（１００ａ）における↑ω_i_minの各成分の値が、前記特定部位と前記各脚リンクの先端部との間の各関節のうちの該成分に対応する関節の関節速度の可動範囲の下限値以上の値となるように該↑ω_i_minが設定され、次式（１００ｂ）における↑ω_i_maxの各成分の値が、前記特定部位と前記各脚リンクの先端部との間の各関節のうちの該成分に対応する関節の関節速度の可動範囲の上限値以下の値となるように該↑ω_i_maxが設定される（第２発明）。
【００３２】
Ｃ_i・↑Ｖｓ＋（↑ｃ_i−↑ω_i_min）≧↑０    ……（１００ａ）
−Ｃ_i・↑Ｖｓ＋（−↑ｃ_i＋↑ω_i_max）≧↑０    ……（１００ｂ）
ただし、
添え字ｉ：脚リンクの識別番数
Ｃ_i：Ｃ_i≡−(Ｊ_i)-1・Ｂ_iにより定義される行列
↑ｃ_i：↑ｃ_i≡(Ｊ_i)-1・↑Ｖft_iにより定義されるベクトル
(Ｊ_i)-1：Ｊ_iの逆行列又は擬似逆行列
Ｊ_i：↑ω_iから↑Ｖft_iへの線形写像を表すヤコビアン行列
↑ω_i：前記特定部位と第ｉ番目の脚リンクの先端部との間に存在する各関節の関節速度を成分として構成されるベクトル
↑Ｖft_i：第ｉ番目の脚リンクの先端部の位置の時間的変化率と該先端部の姿勢の時間的変化率とを成分として構成されるベクトル
Ｂ_i：↑Ｖｓから↑Ｖft_iへの線形写像を表す行列
↑Ｖｓ：前記特定部位運動速度ベクトル
↑ω_i_min：↑ω_iの各成分の許容範囲の下限値により構成されるベクトル
↑ω_i_max：↑ω_iの各成分の許容範囲の上限値により構成されるベクトル
↑０：零ベクトル
なお、↑ω_iの各成分の許容範囲というのは、↑ω_iの各成分に対応する関節の関節速度の可動範囲（関節アクチュエータの能力等により規定される可動範囲）に包含される範囲（該可動範囲に一致する範囲を含む）を意味する。
【００３３】
この第２発明によれば、前記式（１００ａ）における↑ω_i_minの各成分の値と、前記式（１００ｂ）における↑ω_i_maxの各成分の値とを上記の如く設定することによって、各脚リンク毎の前記式（１００ａ）、（１００ｂ）の線形行列不等式が成立する場合に、前記特定部位と各脚リンクの先端部との間の各関節の関節速度がその可動範囲の上限値及び下限値の間の範囲に収まるという制約条件を満たすように、当該線形行列不等式が設定されることとなる。
【００３４】
従って、前記特定部材と各脚リンクの先端部との間の各関節の関節速度がその可動範囲の上限値及び下限値の間の範囲に収まるようにしつつ、前記評価関数の値を最小化するように特定部位運動速度ベクトルの目標値を決定できることとなる。
【００３５】
かかる第２発明では、前記関節動作制約条件は、前記特定部位と前記各脚リンクの先端部との間の各関節の変位量の値があらかじめ定められた可動範囲内に収まるという条件をさらに含んでおり、前記特定部位運動速度目標値決定手段は、前記式（１００ａ）における↑ω_i_minの各成分の値が、前記特定部位と前記各脚リンクの先端部との間の各関節のうちの該成分に対応する関節の関節速度の可動範囲の下限値以上の値となり、且つ、前記最新値の目標歩容により規定される該関節の変位量である最新目標関節変位量が該関節の変位量の可動範囲の下限値に近づくに伴い該関節に対応する↑ω_i_minの成分の値がゼロまで変化していくように該最新目標関節変位量に応じて↑ω_i_minを設定する手段と、前記式（１００ｂ）における↑ω_i_maxの各成分の値が、前記特定部位と前記各脚リンクの先端部との間の各関節のうちの該成分に対応する関節の関節速度の可動範囲の上限値以下の値となり、且つ、該関節の前記最新目標関節変位量が該関節の変位量の可動範囲の上限値に近づくに伴い該関節に対応する↑ω_i_maxの成分の値がゼロまで変化していくように該最新目標関節変位量に応じて↑ω_i_maxを設定する手段とを備えることをがより好ましい（第３発明）。
【００３６】
この第３発明によれば、前記式（１００ａ）における↑ω_i_minの各成分の値を上記の如く設定することによって、前記特定部位と前記各脚リンクの先端部との間の各関節の最新目標関節変位量が該関節の変位量の可動範囲の下限値に近づくと、最終的に該関節に対応する↑ω_i_minの成分の値がゼロとされるので、該関節の変位量は、その可動範囲の下限値よりもさらに減少する方向に変化することができなくなるように、式（１００ａ）の線形行列不等式が設定されることとなる。
【００３７】
同様に、前記式（１００ｂ）における↑ω_i_maxの各成分の値を上記の如く設定することによって、前記特定部位と前記各脚リンクの先端部との間の各関節の最新目標関節変位量が該関節の変位量の可動範囲の上限値に近づくと、最終的に該関節に対応する↑ω_i_maxの値がゼロとされるので、該関節の変位量は、その可動範囲の上限値よりもさらに増加する方向に変化することができなくなるように、式（１００ｂ）の線形行列不等式が設定されることとなる。
【００３８】
このため、各脚リンク毎の前記式（１００ａ）、（１００ｂ）の線形行列不等式が成立する場合に、前記特定部材と各脚リンクの先端部との間の各関節の関節速度がその可動範囲の上限値及び下限値の間の範囲に収まるという制約条件を満たすことに加えて、さらに、前記特定部材と各脚リンクの先端部との間の各関節の変位量がその可動範囲の上限値及び下限値の間に範囲に収まるという制約条件をも満たすように、当該線形行列不等式が設定されることとなる。
【００３９】
従って、第３発明によれば、前記特定部材と各脚リンクの先端部との間の各関節の変位量と関節速度とがそれぞれの可動範囲の上限値及び下限値の間の範囲に収まるようにしつつ、前記評価関数の値を最小化するように特定部位運動速度ベクトルの目標値を決定できることとなる。
【００４０】
前記第１〜第３発明では、前記制御対象ベクトルとしては、例えば、前記移動ロボットの全体重心の並進運動量ベクトルの各成分と該移動ロボットの全体重心の周りの角運動量ベクトルの各成分とにより構成されるベクトルを採用することができる。ただし、この場合、移動ロボットの基体の位置あるいは姿勢が、移動ロボットの移動を安定に行なう上で適切な位置あるいは姿勢からずれていく恐れがある。
【００４１】
そこで、前記第１〜第３発明では、次のような態様を採用することが望ましい。
【００４２】
すなわち、前記移動ロボットの運動を行なうための動力学的な制約条件が該移動ロボットの動力学を表す動力学モデル上で満たされるように、該動力学モデルと前記決定された目標脚位置姿勢とを用いて、該移動ロボットの基体の位置及び姿勢の基本目標値である基本目標基体位置姿勢と、該移動ロボットの全体重心の位置の基本目標値である基本目標全体重心位置と、該移動ロボットの全体重心の並進運動量ベクトルの基本目標値である基本目標並進運動量ベクトルと、該移動ロボットの全体重心周りの角運動量ベクトルの基本目標値である基本目標角運動量ベクトルとを逐次決定する基本動作目標値決定手段と、
前記決定された目標脚位置姿勢と目標基体位置姿勢とを構成要素とする前記目標歩容に対応する前記移動ロボットの全体重心の位置である目標歩容上全体重心位置を、該目標歩容から前記移動ロボットの幾何学モデルに基づいて逐次算出する目標歩容上重心位置算出手段と、
前記算出された目標歩容上全体重心位置を前記決定された基本目標全体重心位置に追従させるために該目標歩容上全体重心位置と基本目標全体重心位置との偏差に応じて決定したフィードバック操作量により前記決定された基本目標並進運動量ベクトルを修正してなる修正後基本目標並進運動量ベクトルを逐次決定する基本目標並進運動量修正手段と、
前記特定部位としての前記基体の位置及び姿勢の単位時間当たりの変化量を成分として構成されるベクトルを前記特定部位運動速度ベクトルとして用い、前記目標基体位置姿勢を前記基本目標基体位置姿勢に追従させるように、前記決定された目標基体位置姿勢と前記決定された基本目標基体位置姿勢との偏差に応じて前記特定部位運動速度ベクトルの基本目標値である基本目標基体運動速度ベクトルを逐次決定する基本目標基体運動速度決定手段とをさらに備え、
前記制御対象ベクトルは、前記特定部位運動速度ベクトルの各成分と前記移動ロボットの全体重心の並進運動量ベクトルの各成分と該移動ロボットの全体重心の周りの角運動量ベクトルの各成分とにより構成されるベクトルであり、
前記目標制御対象ベクトル決定手段は、前記決定された基本目標基体運動速度ベクトルの各成分と、前記決定された修正後基本目標並進運動量ベクトルの各成分と、前記決定された基本目標角運動量ベクトルの各成分とにより構成されるベクトルを前記目標制御対象ベクトルとして決定する（第４発明）。
【００４３】
かかる第４発明によれば、前記特定部位運動速度ベクトル（ここでは前記特定部位としての前記基体の位置及び姿勢の単位時間当たりの変化量を成分として構成されるベクトル）の目標値は、目標制御対象ベクトルを構成する成分のうちの、前記移動ロボットの全体重心の並進運動量ベクトル（前記修正後基本目標並進運動量ベクトル）の値と該全体重心周りの角運動量ベクトル（前記基本目標角運動量ベクトル）の値とに加えて、目標制御対象ベクトルのうちの特定部位運動速度ベクトル（前記基本目標基体運動速度ベクトル）の値をも目標として決定されることとなる。
【００４４】
すなわち、特定部位運動速度ベクトルの目標値は、基本的には、前記線形行列不等式が成立するという制約条件を満たす範囲内で、該特定部位運動速度ベクトルの目標値に対応して生じる基体の位置及び姿勢の変化（目標歩容の現在の状態からの変化）に伴い発生する移動ロボットの全体重心の並進運動量ベクトルと、該全体重心周りの角運動量ベクトルとが、それぞれ目標制御対象ベクトルにより規定される値に追従するようにしつつ、該基体の位置及び姿勢が、目標制御対象ベクトルにより規定される値から乖離しないように決定されることとなる。
【００４５】
この場合、前記目標制御対象ベクトルのうち、移動ロボットの全体重心の並進運動量ベクトルとしての前記修正後基本目標並進運動量ベクトルは、前記基本動作目標値決定手段により決定された基本目標並進運動量ベクトルを、既に決定された目標歩容に対応する目標歩容上全体重心位置と前記基本動作目標値決定手段により決定した基本全体重心位置との偏差に応じて決定したフィードバック操作量によって修正してなるベクトルである。
【００４６】
また、前記目標制御対象ベクトルのうち、移動ロボットの全体重心周りの角運動量ベクトルは、前記基本動作目標値決定手段により決定された基本目標角運動量ベクトルである。
【００４７】
従って、前記目標制御対象ベクトルのうち、移動ロボットの全体重心の並進運動量ベクトルと、該全体重心周りの角運動量ベクトルとは、決定しようとする目標歩容により規定される移動ロボットの全体重心の並進運動量ベクトルと、該全体重心の位置と、該全体重心の周りの角運動量とを、それぞれ、前記移動ロボットの運動を行なうための動力学的な制約条件が前記動力学モデル上で満たされるように前記基本動作目標値決定手段が決定した基本目標並進運動量ベクトル、基本目標全体重心位置、基本目標角運動量ベクトルとに追従させるべく決定されたものとなる。
【００４８】
なお、上記動力学的な制約条件は、移動ロボットの所要の運動を動力学的に行い得るようにするために、移動ロボットに作用する外力、あるいは、移動ロボットが運動によって発生する慣性力を制約する条件である。その制約条件の例としては、例えば、ＺＭＰ（Zero Moment Point）が支持多角形内に収まるという条件や、移動ロボットに作用する並進床反力がある許容範囲に収まるという条件等が挙げられる。
【００４９】
また、前記目標制御対象ベクトルのうち、前記特定部位運動速度ベクトルの基本目標値である基本目標基体運動速度ベクトルは、前記目標基体位置姿勢を前記基本目標基体位置姿勢に追従させるように、既に決定された目標基体位置姿勢と基本目標基体位置姿勢との偏差に応じて決定されたものである。
【００５０】
このため、かかる目標制御対象ベクトルを目標として、前記特定部位運動速度ベクトルの目標値を決定することにより、該特定部位運動速度ベクトルの目標値を積分してなる目標特定部位位置姿勢（基体の位置及び姿勢の目標値）を構成要素として最終的に逐次決定される目標歩容は、それにより規定される移動ロボットの全体重心の並進運動量ベクトルと該全体重心周りの角運動量ベクトルとを、それぞれ上記修正後基本目標並進運動量ベクトルと基本目標角運動量ベクトルとに追従させつつ、該目標歩容における目標特定部位位置姿勢（基体の位置及び姿勢の目標値）が、上記基本目標基体位置姿勢から乖離しないように決定されることなる。
【００５１】
よって、第４発明によれば、前記基体と各脚リンクの先端部との間の各関節の関節動作制約条件を満たし得る範囲内で、脚式移動ロボットの基体の位置や姿勢が適切な状態からずれるの防止しつつ、該脚式移動ロボットの全体の並進運動量ベクトルと角運動量ベクトルとを目標に追従させるように目標歩容を生成することができる。
【００５２】
また、この場合、目標制御対象ベクトルを構成する修正後基本目標並進運動量ベクトルと基本目標角運動量ベクトルと基本目標基体位置姿勢とが上記の如く決定されたものであるから、最終的に逐次決定される目標歩容は、換言すれば、前記動力学モデル上での動力学的な制約条件から逸脱するようなことがなく、しかも、該目標歩容における目標特定部位位置姿勢（基体の位置及び姿勢の目標値）が、該動力学モデル上で適切な運動を行ない得るように決定された基本目標上体位置姿勢から乖離することがないように決定されることとなる。
【００５３】
従って、第４発明によれば、前記基体と各脚リンクの先端部との間の各関節の関節動作制約条件と動力学的な要件を満たしつつ、移動ロボットが姿勢を崩すことが無いような目標歩容を生成できる。
【００５４】
前記第１〜第４発明において、特定部位運動速度ベクトルの積分値としての移動ロボットの特定部位の位置及び姿勢と、前記制御対象ベクトルの積分値に相当する状態量（例えば移動ロボットの全体重心の並進運動量ベクトルの積分値に相当する状態量や、移動ロボットの全体重心周りの角運動量ベクトルの積分値に相当する状態量等）との間の関係は、非線形な関係となる場合が多い。
【００５５】
このため、前記ヤコビアン行列決定手段の処理において、前記特定部位運動速度ベクトルから制御対象ベクトルへの線形写像を表す前記ヤコビアン行列を、既に決定された前記移動ロボットの目標歩容のうちの最新値の目標歩容を用いて逐次決定した場合、すなわち、当該最新値の目標歩容を、前記特定部位運動速度ベクトルの新たな目標値による前記特定部位の位置及び姿勢の変化の起点となる前記移動ロボットの動作状態であるとして前記ヤコビアン行列を決定した場合には、前記評価関数の値を最小化するように決定される特定部位運動速度ベクトルの目標値は、目標制御対象ベクトルを満たす上で、過不足を生じることがある。
【００５６】
そこで、第１〜第４発明では、前記ヤコビアン行列決定手段は、前記特定部位運動速度ベクトルから前記制御対象ベクトルへの線形写像を表す新たなな前記ヤコビアン行列を決定するとき、既に決定された前記移動ロボットの目標歩容のうちの最新値の目標歩容に対応する時刻と前記特定部位運動速度ベクトルの新たな目標値に応じて前記目標特定部位位置姿勢決定手段により決定される目標特定部位位置姿勢を構成要素とする新たな目標歩容に対応する時刻との間の期間内での前記移動ロボットの将来の歩容の瞬時値を、前記最新値の目標歩容とその直前の１つ以上の過去値の目標歩容とに基づいて推定し、その推定した将来の歩容の瞬時値を、前記特定部位運動速度ベクトルによる前記特定部位の位置及び姿勢の変化の起点となる前記移動ロボットの動作状態として用いて、該移動ロボットの当該起点の動作状態に対応する前記ヤコビアン行列を決定することが好ましい（第５発明）。
【００５７】
この第５発明によれば、前記最新値の目標歩容により規定される前記移動ロボットの動作状態よりも、将来の予測される動作状態寄りにずらした動作状態を、前記特定部位運動速度ベクトルによる前記特定部位の位置及び姿勢の変化（換言すれば特定部位運動速度ベクトルの積分値の変化）の起点として、前記ヤコビアン行列が決定される。
【００５８】
このようにすることで、上記非線形性の影響を補償し、目標制御対象ベクトルを実現する上で最適な特定部位運動速度ベクトルの値に対してより一致度合いの高い特定部位運動速度ベクトルの目標値を決定できる。ひいては、目標制御対象ベクトルを極力みたし得る目標歩容を高い信頼性で生成することができる。
【００５９】
この第５発明では、より具体的な形態として、次のような構成を採用できる。すなわち、前記特定部位運動速度目標値決定手段は、前記最新値の目標歩容における目標特定部位位置姿勢と該最新値の目標歩容の１つ前の過去値の目標歩容における目標特定部位位置姿勢との差分に“０”と“１”との間の範囲内であらかじめ定めた係数Ｋｐを乗じてなる補正量を、前記最新値の目標歩容における目標特定部位位置姿勢に加えることにより該目標特定部位位置姿勢の将来の瞬時値を推定する手段と、各脚リンク毎に前記最新値の目標歩容における目標脚位置姿勢と該最新値の１つ前の過去値の目標歩容における目標脚位置姿勢との差分に前記係数Ｋｐを乗じてなる補正量を、前記最新値の目標歩容における目標脚位置姿勢に加えることにより各脚リンクの目標脚位置姿勢の将来の瞬時値を推定する手段とを備え、前記推定された目標特定部位位置姿勢の将来の瞬時値と各脚リンクの目標脚位置姿勢の将来の瞬時値とを構成要素とする歩容を、前記移動ロボットの当該起点の動作状態として用いる（第６発明）。
【００６０】
この第６発明によれば、上記係数Ｋｐを用いることで、既に決定された目標歩容のうちの前記最新値の目標歩容と、その１つ前の過去値の目標歩容とから、最新値の目標歩容に対応する時刻と前記新たな目標歩容に対応する時刻との間の期間内での前記移動ロボットの将来の歩容の瞬時値を、簡易な手法で適切に推定することができる。
【００６１】
上記第１〜第６発明では、ロボットが脚式移動ロボットである場合についての発明であるが、上記第１発明の如く線形行列不等式と評価関数とを用いて、２次計画問題の解法手法の演算処理により特定部位運動速度ベクトルの目標値を逐次決定する技術は、脚式移動ロボットに限らず、種々様々の形態のロボットに対して適用することができる。
【００６２】
そこで、本発明では、上記第１発明をより一般化した発明として、本発明のロボットの動作目標生成装置を構築した。この動作目標生成装置は、空間的な位置及び姿勢が可変である特定部位と、該特定部位から延設されると共に複数の関節が備えられたリンク機構とを備えるロボットの各関節の変位量を規定する動作目標を逐次生成する動作目標生成装置であって、
前記動作目標の構成要素としての前記リンク機構の先端部の位置及び姿勢の目標値であるリンク機構目標先端位置姿勢を逐次決定するリンク機構目標先端位置姿勢決定手段と、
前記ロボットの前記特定部位の位置及び姿勢の単位時間当たりの変化量を成分として構成される特定部位運動速度ベクトルとの間の関係が線形写像により表されるベクトルとしての制御対象ベクトルの目標値である目標制御対象ベクトルを逐次決定する目標制御対象ベクトル決定手段と、
前記線形写像を表すヤコビアン行列を、既に決定された前記ロボットの動作目標のうち
の少なくとも最新値の動作目標を用いて逐次決定するヤコビアン行列決定手段と、
前記特定部位運動速度ベクトルを変数とする２次形式の所定の評価関数であって、少なくとも前記決定された目標制御対象ベクトルと前記決定されたヤコビアン行列を該特定部位運動速度ベクトルに乗じてなるベクトルとの差分に応じて値が変化するように構成された評価関数と、前記特定部位運動速度ベクトルを変数として構成された１つ以上の線形行列不等式とを用い、該線形行列不等式が成立するという制約条件を満たす範囲内で、前記評価関数の値を最小化し得る前記特定部位運動速度ベクトルの値を２次計画問題の解法手法の演算処理により逐次算出し、その算出した前記特定部位運動速度ベクトルの値を該特定部位運動速度ベクトルの目標値として逐次決定する特定部位運動速度目標値決定手段と、
前記決定された特定部位運動速度ベクトルの目標値を逐次積分することにより、前記動作目標の構成要素としての前記特定部位の位置及び姿勢の目標値である目標特定部位位置姿勢を逐次決定する目標特定部位位置姿勢決定手段とを備え、
前記特定部位運動速度目標値決定手段が前記２次計画問題の解法手法の演算処理で用いる前記線形行列不等式は、前記特定部位と前記リンク機構の先端部との間の各関節の変位量とその時間的変化率である関節速度とのうちの少なくともいずれか一方の値を制限する条件である関節動作制約条件を満たすように設定された線形行列不等式であることを特徴とする（第７発明）。
【００６３】
かかる第７発明によれば、第１発明における目標脚位置姿勢決定手段の代わりに、前記リンク機構目標先端位置姿勢決定手段が備えられる。そして、このリンク機構目標先端位置姿勢決定手段により決定された前記リンク機構目標先端位置姿勢（リンク機構の先端部の位置及び姿勢の目標値）と、前記特定部位運動速度目標値決定手段に逐次決定される特定部位運動速度ベクトル（特定部位の位置及び姿勢の単位時間当たりの変化量を成分として構成されるベクトル）の目標値を前記目標特定部位位置姿勢決定手段により逐次積分することにより決定された目標特定部位位置姿勢（特定部位の位置及び姿勢の目標値）とが前記動作目標の構成要素として逐次決定されることとなる。
【００６４】
この場合、第７発明における前記目標制御対象ベクトル決定手段、ヤコビアン行列決定手段、特定部位運動速度目標値決定手段、目標特定部位位置姿勢決定手段の処理は、第１発明と同様に行なわれる。
【００６５】
また、第７発明において、前記特定部位運動速度目標値決定手段が前記２次計画問題の解法手法の演算処理で用いる前記線形行列不等式は、前記特定部位と前記リンク機構の先端部との間の各関節の変位量とその時間的変化率である関節速度とのうちの少なくともいずれか一方の値を制限する条件である関節動作制約条件を満たすように設定された線形行列不等式である。
【００６６】
従って、第７発明によれば、前記関節動作制約条件を満たす範囲内で、前記決定されたヤコビアン行列を該特定部位運動速度ベクトルに乗じてなるベクトルが、前記決定された目標制御対象ベクトルに極力一致もしくはほぼ一致するように、特定部位運動速度ベクトルの目標値が逐次決定されることとなる。
【００６７】
なお、この場合、目標制御対象ベクトルは、ロボットの所要の動作を適切に行うために要求される制御対象ベクトルの値としての意味を持つ。また、前記決定されたヤコビアン行列を該特定部位運動速度ベクトルの目標値に乗じてなるベクトルは最終的に決定される動作目標に対応する制御対象ベクトルの値に相当する。
【００６８】
このため、特定部位運動速度ベクトルの目標値、ひいては、動作目標は、前記関節動作制約条件による関節（特定部位とリンク機構の先端部との間の関節）の動作の制約を満たしつつ、制御対象ベクトルの目標が極力達成させるように決定されることとなる。
【００６９】
よって、第７発明によれば、種々様々なロボットにおいて、ロボットの特定部位から延設されたリンク機構の関節の動作を制限する制約条件を適切に反映させた手法で、該ロボットの動作目標（前記リンク機構を動かす動作目標）を生成することができる。
【図面の簡単な説明】
【００７０】
【図１】本発明の実施形態における脚式移動ロボットの概略構成を示す図。
【図２】図１に示す脚式移動ロボットの制御に関する構成を示す図。
【図３】図２に示す歩容生成部の機能を示すブロック図。
【図４】図３に示す目標脚位置姿勢決定部で決定する目標脚位置姿勢の軌道の例を示す図。
【図５】図３に示す基本動作目標値決定部の処理で用いる動力学モデルを模式的に示す図。
【図６】図３に示す基体運動速度目標値決定部の処理のうちのヤコビアン行列の決定処理を説明するためのグラフ。
【図７】図３に示す基体運動速度目標値決定部の処理のうち、第１実施形態における関節速度の許容範囲の上限値及び下限値を設定する処理を説明するためのグラフ。
【図８】図８（ａ），（ｂ）はそれぞれ実施例、比較例におけるロボットの特定の関節の変位量の経時変化の例を示すグラフ。
【図９】図９（ａ），（ｂ）はそれぞれ実施例、比較例におけるロボットの基体の姿勢（傾斜角度）の経時変化の例を示すグラフ。
【図１０】図１０（ａ），（ｂ）はそれぞれ実施例、比較例におけるロボットの全体重心の並進運動量の誤差の経時変化の例を示すグラフ。
【図１１】図１１（ａ），（ｂ）はそれぞれ実施例、比較例におけるロボットの全体重心周りの角運動量の誤差の経時変化の例を示すグラフ。
【図１２】図３に示す基体運動速度目標値決定部の処理のうち、第２実施形態における関節速度の許容範囲の上限値及び下限値を設定する処理を説明するためのグラフ。
【発明を実施するための形態】
【００７１】
［第１実施形態］
以下、本発明の一実施形態を図１〜図６を参照して説明する。
【００７２】
図１は、本実施形態で例示する脚式移動ロボットの概略構成を模式的に示している。この脚式移動ロボット１（以下、単にロボット１という）は、基体２と、基体２から延設された左右一対（２つ）の脚リンク３Ｒ，３Ｌとを有する２足移動ロボットである。
【００７３】
なお、本明細書では、ロボット１の前方に向かって右側の部材又はその部材に関連する量を示す変数に符号“Ｒ”を付加し、ロボット１の前方に向かって左側の部材又はその部材に関連する量を示す変数に符号“Ｌ”を付加する。ただし、右側、左側を特に区別する必要が無いときは、符号“Ｒ”，“Ｌ”を省略することがある。
【００７４】
基体２は、ロボット１の上体に相当するリンク部分である。この基体２は、本実施形態では、下側基体１０と、この下側基体１０の上方に配置された上側基体１１との２つの要素リンクから構成されている。下側基体１０は、ロボット１の腰部に相当する部分、上側基体１１は、ロボット１胸部に相当する部分である。そして、上側基体１１は、ヨー軸周り（Ｚ軸周り）の回転自由度を有する基体回旋用関節１２を介して下側基体１０に連結されている。
【００７５】
左右一対の脚リンク３Ｒ，３Ｌは、互いに同じ構造のものである。具体的には、各脚リンク３は、下側基体１０に股関節部１３を介して連結された大腿部１４と、この大腿部１４に膝関節部１５を介して連結された下腿部１６と、この下腿部１６に足首関節部１７を介して連結された足平部１８とを、該脚リンク３を構成する複数の要素リンクとして備える。この場合、各脚リンク３の先端部は、足平部１８により構成される。
【００７６】
そして、各脚リンク３の股関節部１３は、ヨー軸周り、ピッチ軸周り（Ｙ軸周り）及びロール軸周り（Ｘ軸周り）の回転自由度をそれぞれ有する３つの関節１９，２０，２１により構成されている。また、膝関節部１５は、ピッチ軸まわりの回転自由度を有する関節２２により構成されている。また、足首関節部１７は、ピッチ軸周り及びロール軸周りの回転自由度をそれぞれ有する２つの関節２３，２４により構成されている。
【００７７】
従って、各脚リンク３の先端部である足平部１８は、本実施形態では、下側基体１０に対して、６自由度の運動自由度を有する。なお、上記の説明における各脚リンク３の関節１９〜２４の回転軸（ロール軸、ピッチ軸、ヨー軸）は、脚リンク３を上下方向に延在させた状態での回転軸を示している。
【００７８】
以上が、本実施形態のロボット１の基本構造である。かかる構成のロボット１は、各脚リンク３の６個の関節１９〜２４を駆動することによって、各脚リンク３の空間的な運動が行なわれ、この運動によって床上を移動することが可能となっている。例えば、人の歩行動作あるいは走行動作と同様の形態（歩容）で脚リンク３Ｒ，３Ｌを運動させることによって、ロボット１の歩行動作あるいは走行動作を行なうことが可能である。
【００７９】
また、本実施形態では、基体２を構成する下側基体１０と、上側基体１１とが関節１２を介して連結されているので、この関節１２を駆動することによって、基体２をその体幹軸に相当する関節１２の回転軸周りに捩じることが可能となっている。
【００８０】
補足すると、ロボット１は、上記基体２、及び脚リンク３Ｒ，３Ｌだけでなく、例えば上側基体１１の両側部から延設された腕リンクや、該上側基体１１の上端部に搭載した頭部等を備えていてもよい。また、基体２の関節１２を省略し、下側基体１０及び上側基体１１を一体に構成してもよい。
【００８１】
図１での図示は省略したが、ロボット１には、図２に示すように、上記の各関節をそれぞれ回転駆動する関節アクチュエータ３０と、ロボット１の動作制御を行なう制御処理ユニット３１とが搭載されている。
【００８２】
関節アクチュエータ３０は、例えば電動モータあるいは油圧アクチュエータにより構成されたものであり、各関節毎に備えられている。この場合、各関節アクチュエータ３０による各関節の駆動機構は公知の構造のものでよい。また、関節アクチュエータ３０は、回転型のアクチュエータに限らず、直動型のアクチュエータであってもよい。
【００８３】
制御処理ユニット３１は、ＣＰＵ、ＲＡＭ、ＲＯＭ、インターフェース回路等を含む電子回路ユニットである。この制御処理ユニット３１は、実装されるプログラム等により実現される主要な機能として、ロボット１の目標歩容を逐次生成する歩容生成部３２と、該目標歩容に応じてロボット１の各関節の変位量（回転角）を関節アクチュエータ３０を介して制御する関節アクチュエータ制御部３３とを備える。
【００８４】
歩容生成部３２は、本発明の歩容生成装置又は動作目標生成装置としての機能を制御処理ユニット３１に付与するものである。この歩容生成部３２は、ロボット１の目標歩容の構成要素として、各脚リンク３の先端部を構成する足平部１８の空間的な位置及び姿勢の目標値である目標脚位置姿勢と、基体２の空間的な位置及び姿勢の目標値である目標基体位置姿勢とを逐次決定する処理を後述する如く実行する。
【００８５】
この場合、本実施形態では、基体２は関節１２を備えるため、上側基体１１を下側基体１０に対して関節１２の回転軸周りに捩じることが可能となっている。このため、本実施形態における目標基体位置姿勢は、より詳しくは、下側基体１０の空間的な位置及び姿勢の目標値と、上側基体１１の下側基体１０に対する捩じり角（関節１２の変位量）の目標値とから構成される。
【００８６】
また、各脚リンク３の足平部１８の位置は、より詳しくは、該足平部１８のあらかじめ定めた代表点の位置であり、該足平部１８の姿勢は、該足平部１８の空間的な向きを意味する。このことは、下側基体１０の位置及び姿勢についても同様である。
【００８７】
これらの位置及び姿勢は、ロボット１の動作環境の床面に対して固定された慣性座標系であるグローバル座標系で見た位置及び姿勢として記述される。このグローバル座標系は、任意に設計的に定めたものでよい。例えばロボット１の両脚リンク３Ｒ，３Ｌのうちの支持脚（遊脚としての一方の脚リンク３の足平部１８を空中で動かすときにロボット１の自重を支えることとなる他方の脚リンク３）の足平部１８の接地時における該足平部１８の直下（もしくはその近辺）の床面上に原点を有し、且つ、該足平部１８の前後方向の水平軸及び左右方向の水平軸をそれぞれＸ軸、Ｙ軸とし、鉛直軸をＺ軸とする３軸直交座標系（所謂、支持脚座標系）をグローバル座標系として用いることができる。
【００８８】
本実施形態では、便宜上、グローバル座標系として上記支持脚座標系が用いられるものとする。そして、特にことわらない限り、Ｘ軸、Ｙ軸、Ｚ軸は、上記支持脚座標系の座標軸を意味するものとする。
【００８９】
補足すると、基体２を構成する下側基体１０及び上側基体１１の一方の基体１０又は１１の空間的な位置及び姿勢と、関節１２の変位量とが決まれば、他方の基体１１又は１０の空間的な位置及び姿勢も一義的に決まる。従って、目標基体位置姿勢は、上側基体１１の空間的な位置及び姿勢の目標値と、関節１２の変位量の目標値との組により構成されていてもよい。
【００９０】
関節アクチュエータ制御部３３は、各関節の実際の変位量を、歩容生成部３２により決定された目標歩容により規定されるロボット１の各関節の変位量の目標値に追従させるように、各関節に対応する関節アクチュエータ３０を制御する。
【００９１】
この場合、各関節の実際の変位量（回転角）が、図示しないロータリエンコーダ等の変位量センサにより検出される。そして、関節アクチュエータ制御部３３は、各関節の実際の変位量の検出値と目標値との偏差に応じて、フィードバック制御により関節アクチュエータ３０の駆動力を制御する。
【００９２】
以降、制御処理ユニット３１の歩容生成部３２の処理を詳細に説明する。
【００９３】
図３に示すように、歩容生成部３２は、目標歩容を生成するための主要な処理部として、目標脚位置姿勢を逐次決定する目標脚位置姿勢決定部４１と、各脚リンク３の足平部１８の位置及び姿勢以外のロボット１の所定種類の状態量に関する基本の動作目標を逐次決定する基本動作目標値決定部４２とを備える。
【００９４】
この場合、基本動作目標値決定部４２が決定する動作目標は、ロボット１の全体重心の位置の基本の目標値である基本目標重心位置↑Ｘg_cmd1と、ロボット１の全体の並進運動量ベクトル（＝全体重心の並進運動量ベクトル）の基本の目標値である基本目標並進運動量ベクトル↑Ｐ_cmd1と、ロボット１の全体重心周りの全体の角運動量ベクトルの基本の目標値である基本目標角運動量ベクトル↑Ｌ_cmd1と、基体２の位置及び姿勢の基本の目標値である基本目標基体位置姿勢とから構成される。
【００９５】
なお、基本目標重心位置↑Ｘg_cmd1、基本目標並進運動量ベクトル↑Ｐ_cmd1、基本目標角運動量ベクトル↑Ｌ_cmd1は、それぞれ、グローバル座標系の各座標成分を構成要素とする３成分のベクトル（縦ベクトル）である。本実施形態の説明では、“↑”は、ベクトル（縦ベクトル）を表記する記号として用いる。
【００９６】
また、歩容生成部３２は、基本動作目標値決定部４２により決定する基本の動作目標のうちの基本目標並進運動量ベクトル↑Ｐ_cmd1を修正してなる修正後基本目標並進運動量ベクトル↑Ｐ_cmd2を逐次決定する基本目標並進運動量修正部４３と、基体２の位置及び姿勢の単位時間当たりの変化量（すなわち変化速度）の基本の目標値である基本目標基体運動速度ベクトル↑Ｖb_cmd1を逐次決定する基本目標基体運動速度決定部４４と、修正後基本目標並進運動量ベクトル↑Ｐ_cmd2、基本目標角運動量ベクトル↑Ｌ_cmd1及び基本目標基体運動速度ベクトル↑Ｖb_cmd1を目標として、基体２の位置及び姿勢の単位時間当たりの変化量の目標値である目標基体運動速度ベクトル↑Ｖb_cmd2を逐次決定する基体運動速度目標値決定部４５とを備える。
【００９７】
上記基本目標基体運動速度ベクトル↑Ｖb_cmd1及び目標基体運動速度ベクトル↑Ｖb_cmd2は、それぞれ、グローバル座標系で見た基体２の代表点（本実施形態では下側基体１０の代表点）の並進移動速度の各座標軸成分と、グローバル座標系で見た基体２の姿勢（本実施形態では下側基体１０の姿勢）の各座標軸周りの角速度成分と、基体２の関節１２の変位量の時間的変化率（角速度）とを並べた７成分のベクトル（縦ベクトル）である。
【００９８】
また、目標基体運動速度ベクトル↑Ｖb_cmd2は、換言すれば、歩容生成部３２が逐次決定する目標歩容における目標基体位置姿勢の時間的変化率である。
【００９９】
歩容生成部３２はさらに、基体運動速度目標値決定部４５により逐次決定される目標基体運動速度ベクトル↑Ｖb_cmd2を積分する（↑Ｖb_cmd2の各成分を積分する）ことにより目標基体位置姿勢を逐次決定する目標基体位置姿勢決定部４６と、目標基体位置姿勢及び両脚リンク３Ｒ，３Ｌの目標脚位置姿勢を構成要素とする目標歩容により規定されるロボット１の全体重心の位置である目標歩容上重心位置↑Ｘg_cmd2を逐次算出する目標歩容上重心位置算出部４７とを備える。
【０１００】
上記目標歩容上重心位置↑Ｘg_cmd2は、基本目標重心位置↑Ｘg_cmd1と同様に、グローバル座標系の各座標成分を構成要素とする３成分のベクトル（縦ベクトル）である。
【０１０１】
本実施形態では、歩容生成部３２はさらに、目標歩容を構成する目標基体位置姿勢及び両脚リンク３Ｒ，３Ｌの目標脚位置姿勢を用いて、ロボット１の各関節の変位量の目標値である目標関節変位量を逐次決定する目標関節変位量決定部４８を備えている。ただし、この目標関節変位量決定部４８は、前記関節アクチュエータ制御部３３に備えるようにしてもよい。
【０１０２】
歩容生成部３２は、以上の各処理部４１〜４８の処理を所定の演算処理周期で逐次実行することにより、目標歩容の構成要素である目標基体位置姿勢と、両脚リンク３Ｒ，３Ｌの目標脚位置姿勢とを逐次決定すると共に、該目標歩容に対応する各関節の目標関節変位量を逐次決定する。
【０１０３】
以下に、歩容生成部３２の各処理部４１〜４８の処理の詳細を含めて、歩容生成部３２の処理の全体を説明する。なお、本実施形態では、主に、ロボット１の歩行動作又は走行動作による移動を行なうための目標歩容を生成する場合を一例として歩容生成部３２の処理を説明する。
【０１０４】
歩容生成部３２は、各演算処理周期において、まず、目標脚位置姿勢決定部４１及び基本動作目標値決定部４２の処理を実行する。
【０１０５】
本実施形態では、目標脚位置姿勢決定部４１及び基本動作目標値決定部４２には、脚リンク３Ｒ，３Ｌのうちの遊脚の着地が行なわれる毎に、次回の遊脚を含めて複数歩分の遊脚の足平部１８の目標とする着地位置及び着地姿勢である目標着地位置姿勢と、目標とする着地時刻である目標着地時刻とが与えられるようになっている。なお、目標着地時刻の代わりに、ロボット１の１歩分の期間（歩容周期）の目標時間を用いててもよい。
【０１０６】
上記目標着地位置姿勢及び目標着地時刻は、ロボット１の移動計画、あるいは、操縦装置による移動指令に基づいて決定される。その決定処理は、制御処理ユニット３１で行なうようにしてもよいことはもちろんであるが、ロボット１の外部のサーバで行うようにしてもよい。
【０１０７】
そして、目標脚位置姿勢決定部４１は、与えられた目標着地位置姿勢及び目標着地時刻に応じて、各脚リンク３の足平部１８の離床直前の接地状態での位置及び姿勢から次の目標着地位置姿勢に至る位置及び姿勢の目標軌道（足平部１８の位置及び姿勢の目標値の時系列パターン）を決定し、その目標軌道の瞬時値として各脚リンク３の目標脚位置姿勢を逐次決定する。
【０１０８】
この場合、各脚リンク３の足平部１８の位置及び姿勢の目標軌道（目標脚位置姿勢の軌道）は、該脚リンク３が遊脚となる期間で、例えば図４に例示するようなパターンで該足平部１８の目標位置及び目標姿勢が連続的に変化しつつ、該足平部１８が空中を移動するように決定される。この目標軌道の始端の足平部１８の位置及び姿勢は、離床前の着地動作時の目標着地位置姿勢に相当し、該目標軌道の終端の足平部１８の位置及び姿勢は、次の目標着地位置姿勢に相当する。
【０１０９】
なお、各脚リンク３の足平部１８の接地中においては、該脚リンク３の位置及び姿勢の目標軌道は、該脚リンク３の足平部１８の位置及び姿勢を一定もしくはほぼ一定に保つように決定してもよいことはもちろんであるが、該足平部１８の接地箇所が、該足平部１８の踵寄りの箇所から爪先寄りの箇所に移行していくように決定してもよい。
【０１１０】
また、各脚リンク３の足平部１８の空中での姿勢の軌道は、該足平部１８の姿勢を一定もしくはほぼ一定に保つように決定してもよい。
【０１１１】
基本動作目標値決定部４２は、与えられた目標着地位置姿勢及び目標着地時刻に応じて（あるいは、各脚リンク３の足平部１８の接地中の目標脚位置姿勢の軌道に応じて）、ＺＭＰ（Zero Moment Point）の目標位置である目標ＺＭＰの軌道を決定する。
【０１１２】
この場合、目標ＺＭＰの軌道は、目標ＺＭＰが所謂、支持多角形内でその境界に近づき過ぎずないような領域（ＺＭＰの存在許容領域）に位置し、且つ、不連続的な変化を生じないように設定される。このような目標ＺＭＰの軌道の設定手法としては、例えば特許第３６７４７８９号にて本願出願人が説明した手法等、公知の手法を用いることができる。
【０１１３】
そして、基本動作目標値決定部４２は、目標脚位置姿勢決定部４１により決定された目標脚位置姿勢の時系列と、ロボット１の動力学（ロボット１に作用する外力（ここでは床反力）とロボット１の運動との関係）を表すものとしてあらかじめ定められた動力学モデルとを用いて、該動力学モデル上で目標ＺＭＰを満足し得る基体２の位置及び姿勢が前記基本目標基体位置姿勢として逐次決定される。
【０１１４】
ここで、目標ＺＭＰを満足し得る基体２の位置及び姿勢というのは、上記動力学モデルにおいて、該基体２の位置及び姿勢の時系列と、両脚リンク３Ｒ，３Ｌの目標脚位置姿勢の時系列とにより規定されるロボット１の全体の運動によって発生する慣性力と該ロボット１に作用する重力との合力によって、目標ＺＭＰ周りに発生するモーメントの水平軸周り成分（Ｘ軸周り成分及びＹ軸周り成分）がゼロになるような、基体２の位置及び姿勢を意味する。
【０１１５】
上記動力学モデルとしては、目標ＺＭＰを満足し得る基体２の位置及び姿勢を比較的容易に算出し得るように、線形性の高い動力学モデルが使用される。例えば、特許第３６７４７８９号にて本願出願人が説明した動力学モデル（特許第３６７４７８９号の図１０に示す動力学モデル）が、基本動作目標値決定部４２の処理における動力学モデルとして採用することができる。
【０１１６】
該動力学モデルは、図５に示すように、各脚リンク３Ｒ，３Ｌと基体２（ロボット１の上体）とにそれぞれ対応する３つの質点３ｍＲ，３ｍＬ，２ｍと、基体２の姿勢（詳しくは、下側基体１０のＸ軸周り及びＹ軸周りの姿勢）の変化に伴う慣性力モーメントを発生するフライホイールＦＨ（質量を持たずにイナーシャを有するフライホイール）とを備える動力学モデルである。そして、この場合、基本目標基体位置姿勢は、特許第３６７４７８９号にて本願出願人が説明した目標上体位置姿勢の決定手法と同様の手法によって、決定することができる。
【０１１７】
このようにすることにより、上記動力学モデル上において、ＺＭＰが支持多角形内に収まり、また、ロボット１に作用する床反力のうちの摩擦力成分が適切な許容範囲内に収まる、というような所要の動力学的な制約条件が満たされるように、基本目標基体位置姿勢を決定できることとなる。
【０１１８】
なお、本実施形態では、基本目標基体位置姿勢のうち、基体２の関節１２の変位量の目標値は、例えば、上側基体１１が関節１２の回転軸周りに回転することで目標ＺＭＰ周りに発生する鉛直軸周りのモーメントが、各質点３ｍＲ，３ｍＬ，２ｍの並進運動によって目標ＺＭＰ周りに発生する鉛直軸周りのモーメントを打ち消すように決定される。
【０１１９】
ただし、関節１２の変位量の目標値は、例えば一定値であってもよく、あるいは、一定のパターンで周期的に（脚リンク３Ｒ，３Ｌの運動に同期した周期で）変化するように設定されていてもよい。
【０１２０】
本実施形態では、基本動作目標値決定部４２は、さらに、上記の如く決定した基本目標基体位置姿勢の時系列と、目標脚位置姿勢決定部４１により決定された目標脚位置姿勢の時系列とから、上記動力学モデルを用いて、該動力学モデル上でのロボット１の全体重心の位置と、該全体重心の並進運動量ベクトルと、該全体重心周りの角運動量ベクトルとを、それぞれ、前記基本目標重心位置↑Ｘg_cmd1、基本目標並進運動量ベクトル↑Ｐ_cmd1、基本目標角運動量ベクトル↑Ｌ_cmd1として算出する。
【０１２１】
この場合、基本目標重心位置↑Ｘg_cmd1は、各演算処理周期における脚リンク３Ｒ，３Ｌのそれぞれの目標脚位置姿勢と基本目標基体位置姿勢とにより規定される質点３ｍＲ，３ｍＬ，２ｍの全体の重心の位置（位置ベクトル）として算出される。すなわち、脚リンク３Ｒ，３Ｌのそれぞれの目標脚位置姿勢と基本目標基体位置姿勢とにより規定される各質点３ｍＲ，３ｍＬ，２ｍの位置ベクトルに、各質点３ｍＲ，３ｍＬ，２ｍの質量を乗じてなるベクトルの総和のベクトルを、ロボット１の全体の質量（＝質点３ｍＲ，３ｍＬ，２ｍの総和の質量）により除算してなるベクトルが、基本目標重心位置↑Ｘg_cmd1として算出される。
【０１２２】
また、基本目標並進運動量ベクトル↑Ｐ_cmd1は、脚リンク３Ｒ，３Ｌのそれぞれの目標脚位置姿勢の時系列と基本目標基体位置姿勢の時系列とにより規定される質点３ｍＲ，３ｍＬ，２ｍの全体の重心の移動速度ベクトル（＝↑Ｘg_cmd1の時間的変化率）にロボット１の全体の質量を乗じることにより算出される。すなわち、質点３ｍＲ，３ｍＬ，２ｍの全体の重心の並進運動量ベクトルが、基本目標並進運動量ベクトル↑Ｐ_cmd1として算出される。
【０１２３】
また、基本目標角運動量ベクトル↑Ｌ_cmd1は、脚リンク３Ｒ，３Ｌのそれぞれの目標脚位置姿勢の時系列により規定される質点３ｍＲ，３ｍＬのそれぞれの並進運動量ベクトルが、質点３ｍＲ，３ｍＬ，２ｍの全体の重心の周りに発生する角運動量ベクトルと、基本目標基体位置姿勢の時系列により規定される質点２ｍの並進運動量ベクトルが、質点３ｍＲ，３ｍＬ，２ｍの全体の重心の周りに発生する角運動量ベクトルと、基本目標基体位置姿勢のうちの基体２の姿勢の時系列により規定されるフライホイールＦＨの回転運動によって発生する角運動量ベクトルと、下側基体１０に対する上側基体１１の回転運動（関節１２の回転軸周りの回転運動）によって発生する角運動量ベクトルとの総和の角運動量ベクトルとして算出される。
【０１２４】
補足すると、ロボット１が腕リンク等、基体２に対して可動な他のリンクを備える場合には、基本動作目標値決定部４２は、当該他のリンクの各関節の変位量を規定する目標値（例えば、当該他のリンクの先端部の目標位置及び目標姿勢、あるいは、当該他のリンクの先端部の各関節の変位量の目標値等）も決定する。
【０１２５】
この場合、ロボット１の歩行動作又は走行動作を行なう目標歩容の生成処理では、当該他のリンク（例えば腕リンク）の各関節の変位量を規定する目標値は、例えば、基体２のうちの上側基体１１に対する当該他のリンクの各部の相対的な位置及び姿勢を定常的に一定に保持するような目標値でよい。このようにした場合には、上記動力学モデルにおける質点２ｍの質量や、フライホイールＦＨのイナーシャ（慣性モーメント）は、当該他のリンクの影響分を含めて設定しておけばよい。
【０１２６】
以上のように目標脚位置姿勢決定部４１及び基本動作目標値決定部４２の処理を実行した後、次に、歩容生成部３２は、目標歩容上重心位置算出部４７の処理と、前記基本目標並進運動量修正部４３及び基本目標基体運動速度決定部４４の処理とを実行する。
【０１２７】
目標歩容上重心位置算出部４７には、歩容生成部３２が前回の演算処理周期で最終的に決定した目標歩容の構成要素（目標脚位置姿勢及び目標基体位置姿勢）、換言すれば、歩容生成部３２が既に決定した目標歩容のうちの最新の（現在の）目標歩容の構成要素が与えられる。
【０１２８】
そして、目標歩容上重心位置算出部４７は、与えられた目標歩容の構成要素により規定されるロボット１の全体重心の位置（グローバル座標系で見た位置ベクトル）を、ロボット１の幾何学モデル（剛体リンクモデル）に基づく幾何学演算によって算出し、この算出した位置を目標歩容上重心位置↑Ｘg_cmd2として決定する。
【０１２９】
さらに詳細には、上記幾何学演算に用いるロボット１の幾何学モデルは、例えばロボット１の各要素リンクの重心位置に質点（該要素リンクの質量を有する質点）が設定されたモデルである。
【０１３０】
そして、目標歩容上重心位置算出部４７は、まず、与えられた最新の目標歩容から、該目標歩容により規定されるロボット１の各関節の変位量を逆運動学演算の処理により算出する。
【０１３１】
次いで、目標歩容上重心位置算出部４７は、この算出した各関節の変位量と、与えられた最新の目標歩容の目標脚位置姿勢又は目標基体位置姿勢とにより規定される上記幾何学モデルの各質点の位置を算出する。
【０１３２】
そして、目標歩容上重心位置算出部４７は、このように算出した各質点の位置と、該質点の質量とから、該幾何学モデルの全ての質点の質量中心の位置としてのロボット１の全体重心の位置を算出する。このように算出されたロボット１の全体重心の位置が、上記目標歩容上重心位置↑Ｘg_cmd2として決定される。
【０１３３】
なお、歩容生成部３２の前回の演算処理周期で最終的に決定された最新の目標歩容に対応する目標歩容上重心位置↑Ｘg_cmd2を目標歩容上重心位置算出部４７により上記の如く決定する処理は、前回の演算処理周期で行なうようにしてもよい。
【０１３４】
補足すると、ロボット１が腕リンク等、基体２に対して可動な他のリンクを備える場合には、当該他のリンクの質量を考慮した幾何学モデルを用いて上記目標歩容上重心位置↑Ｘg_cmd2を決定すればよい。
【０１３５】
この場合には、目標歩容上重心位置算出部４７には、目標脚位置姿勢及び目標基体位置姿勢に加えて、当該他のリンクの各関節の変位量を規定する目標値（例えば、当該他のリンクの先端部の目標位置及び目標姿勢、あるいは、当該他のリンクの先端部の各関節の変位量の目標値等）も与えられる。
【０１３６】
そして、ロボット１の全体重心の位置としての目標歩容上重心位置↑Ｘg_cmd2は、当該他のリンクに対応する質点を含めた幾何学モデルに基づく幾何学演算によって、算出するようにすればよい。
【０１３７】
ただし、上記他のリンクの質量が基体２の質量に比して十分に微小である場合には、当該他のリンクの質量を無視して構成した幾何学モデル、あるいは、当該他のリンクの質量を基体２の質量に含めて構成した幾何学モデルを用いて目標歩容上重心位置↑Ｘg_cmd2を決定するようにしてもよい。
【０１３８】
また、例えば、基体２の上側基体１１から腕リンクが延設されている場合において、移動ロボット１の歩行動作又は走行動作における上側基体１１に対する腕リンクの姿勢を一定もしくはほぼ一定に保持するような場合には、該腕リンクが実質的に上側基体１１と一体の部分となる。このため、この場合には、当該他のリンクの質量を上側基体１１の質量に含めて構成した幾何学モデルを用いて目標歩容上重心位置↑Ｘg_cmd2を決定するようにすればよい。
【０１３９】
基本目標並進運動量修正部４３には、基本動作目標値決定部４２から基本目標重心位置↑Ｘg_cmd1と基本目標並進運動量ベクトル↑Ｐ_cmd1とが逐次与えられると共に、目標歩容上重心位置算出部４７から目標歩容上重心位置↑Ｘg_cmd2が逐次与えられる。
【０１４０】
なお、以降の説明では、歩容生成部３２の処理で決定される↑Ｘg_cmd1、↑Ｐ_cmd1、↑Ｘg_cmd2等の各変数の値に関し、歩容生成部３２の現在の（今回の）演算処理周期で決定される値と、前回の演算処理周期で決定された値とを区別する必要がある場合に、前者の値を今回値、後者の値を前回値ということがある。
【０１４１】
基本目標並進運動量修正部４３は、まず、与えられた基本目標重心位置↑Ｘg_cmd1と、目標歩容上重心位置↑Ｘg_cmd2との偏差（＝↑Ｘg_cmd1−↑Ｘg_cmd2）を演算部４３ａで算出する。
【０１４２】
この場合、基本目標重心位置↑Ｘg_cmd1の今回値に対応する目標歩容の今回値は未だ決定されていない。そして、本実施形態では、目標歩容上重心位置↑Ｘg_cmd2の今回値は、目標歩容の前回値に対応するロボット１の全体重心の位置である。このため、演算部４３ａの処理では、基本目標重心位置↑Ｘg_cmd1の前回値と、目標歩容上重心位置↑Ｘg_cmd2の今回値と用いて上記偏差（以降、重心位置偏差という）が算出される。
【０１４３】
さらに、基本目標並進運動量修正部４３は、演算部４３ａで算出した重心位置偏差（＝↑Ｘg_cmd1−↑Ｘg_cmd2）にあらかじめ定めた所定値のゲインＫｇを乗じる処理を演算部４３ｂで実行する。これにより、上記偏差をゼロに近づけるように基本目標並進運動量ベクトル↑Ｐ_cmd1を補正するための操作量（フィードバック操作量）が算出される。
【０１４４】
次いで、基本目標並進運動量修正部４３は、基本目標並進運動量ベクトル↑Ｐ_cmd1の今回値を、演算部４３ｂで算出した操作量によって補正する処理を演算部４３ｃで実行する。この補正処理は、↑Ｐ_cmd1の今回値に演算部４３ｂで算出した操作量を加算することにより行なわれる。これにより、演算部４３ｃの出力として、修正後基本目標並進運動量ベクトル↑Ｐ_cmd2（今回値）が算出される。
【０１４５】
従って、各演算処理周期において、修正後基本目標並進運動量ベクトル↑Ｐ_cmd2（今回値）は、次式（１）により基本目標並進運動量ベクトル↑Ｐ_cmd1の今回値を修正することで算出される。
【０１４６】
↑Ｐ_cmd2＝↑Ｐ_cmd1＋Ｋｇ・（↑Ｘg_cmd1−↑Ｘg_cmd2）    ……（１）
すなわち、↑Ｐ_cmd2は、重心位置偏差（↑Ｘg_cmd1−↑Ｘg_cmd2）をフィードバック制御則としての比例則によりゼロに近づけるように決定したフィードバック操作量によって↑Ｐ_cmd1を補正することにより決定される。
【０１４７】
なお、上記ゲインＫｇはスカラー及び対角行列（ゲイン行列）のいずれであってもよい。ゲインＫｇを対角行列とした場合には、重心位置偏差の各成分（Ｘ軸、Ｙ軸、Ｚ軸の各座標軸成分）毎にフィードバックゲインを設定することが可能である。
【０１４８】
基本目標基体運動速度決定部４４には、基本動作目標値決定部４２から基本目標基体位置姿勢が逐次与えられると共に、目標基体位置姿勢決定部４６（詳細は後述する）から目標基体位置姿勢が逐次与えられる。
【０１４９】
そして、基本目標基体運動速度決定部４４は、まず、与えられた基本目標基体位置姿勢と、目標基体位置姿勢との偏差（＝基本目標基体位置姿勢−目標基体位置姿勢）を演算部４４ａで算出する。
【０１５０】
この場合、基本目標基体位置姿勢の今回値に対応する目標基体位置姿勢は未だ決定されていない。このため、演算部４４ａの処理では、基本目標基体位置姿勢の前回値と、目標基体位置姿勢の前回値と用いて上記偏差（以降、基体位置姿勢偏差という）が算出される。
【０１５１】
さらに、基本目標基体運動速度決定部４４は、演算部４４ａで算出した基体位置姿勢偏差にあらかじめ定めた所定値のゲインＫｂを乗じる処理を演算部４４ｂで実行する。これにより、上記基体位置姿勢偏差をゼロに近づけて、目標基体位置姿勢が基本目標基体位置姿勢から乖離しないようにするための操作量（フィードバック操作量）として、基体２の位置及び姿勢の単位時間当たりの変化量の目標値により構成される基本目標基体運動速度ベクトル↑Ｖb_cmd1が算出される。
【０１５２】
従って、各演算処理周期において、基本目標基体運動速度ベクトル↑Ｖb_cmd1は、次式（２）により算出される。
【０１５３】
↑Ｖb_cmd1＝Ｋｂ・（基本目標基体位置姿勢−目標基体位置姿勢）    ……（２）
すなわち、↑Ｖb_cmd1は、基体位置姿勢偏差（＝基本目標基体位置姿勢−目標基体位置姿勢）をフィードバック制御則としての比例則によりゼロに近づけるように決定したフィードバック操作量として決定される。
【０１５４】
なお、上記ゲインＫｂはスカラー及び対角行列（ゲイン行列）のいずれであってもよい。ゲインＫｂを対角行列とした場合には、基体位置姿勢偏差の各成分（基体２の各座標軸方向の位置の偏差、基体２の各座標軸周りの姿勢の偏差（角度偏差）、及び基体２の関節１２の変位量の偏差（角度偏差））毎にフィードバックゲインを設定することが可能である。
【０１５５】
以上のように目標歩容上重心位置算出部４７、基本目標並進運動量修正部４３及び基本目標基体運動速度決定部４４の処理を実行した後、歩容生成部３２は、次に基体運動速度目標値決定部４５の処理を実行する。
【０１５６】
この基体運動速度目標値決定部４５は、基体２の位置及び姿勢の単位時間当たりの変化量を成分として構成される基体運動速度ベクトル↑Ｖｂから、ロボット１の全体重心の並進運動量ベクトル↑Ｐの各成分と、ロボット１の全体重心の周りの角運動量ベクトル↑Ｌの各成分と、基体運動速度ベクトル↑Ｖｂの各成分とにより構成されるベクトルである制御対象ベクトル↑Ｓへの線形写像を表すヤコビアン行列を用いて基体運動速度ベクトル↑Ｖｂの目標値である目標基体運動速度ベクトル↑Ｖb_cmd2を決定する。
【０１５７】
上記制御対象ベクトル↑Ｓは、↑Ｐ、↑Ｌ、↑Ｖｂの各成分を並べた縦ベクトルである。より具体的には、↑Ｐ、↑Ｌ、↑Ｖｂをそれぞれの成分を用いて表記したものを、↑Ｐ≡[Ｐx，Ｐy，Ｐz]T、↑Ｌ≡[Ｌx，Ｌy，Ｌz]T、↑Ｖｂ≡[Ｖbx，Ｖby，Ｖbz，ωbx，ωby，ωbz，ωbt]Tとおくと（上付き添え字Ｔは転置を表す）、↑Ｓは、例えば次式（３）により表記される縦ベクトルである。
【０１５８】
↑Ｓ≡[↑Ｐ，↑Ｌ，↑Ｖｂ]T
≡[Ｐx，Ｐy，Ｐz，Ｌx，Ｌy，Ｌz，Ｖbx，Ｖby，Ｖbz，ωbx，ωby，ωbz，ωbt]T
……（３）
なお、Ｐx，Ｐy，Ｐzは、それぞれ↑ＰのＸ軸方向成分、Ｙ軸方向成分、Ｚ軸方向成分であり、Ｌx，Ｌy，Ｌzは、それぞれ↑ＬのＸ軸周り成分、Ｙ軸周り成分、Ｚ軸周り成分である。また、Ｖbx、Ｖby、Ｖbzは、それぞれ、基体２の代表点の移動速度のうちのＸ軸方向成分、Ｙ軸方向成分、Ｚ軸方向成分、ωbx、ωby、ωbzは、それぞれ、基体２の姿勢の時間的変化率のうちのＸ軸周りの角速度成分、Ｙ軸周りの角速度成分、Ｚ軸周りの角速度成分、ωbtは、基体２の関節１２の変位量の時間的変化率（角速度）である。
【０１５９】
また、↑Ｖｂから↑Ｓへの線形写像を表すヤコビアン行列をＡとおくと、その線形写像は、次式（４）により表される。
【０１６０】
↑Ｓ＝Ａ・↑Ｖｂ    ……（４）
この場合、ヤコビアン行列Ａは、次式（５）で示すように、↑Ｖｂから↑Ｐ、↑Ｌのそれぞれへの線形写像を表す慣性行列Ｍ，Ｈと、↑Ｖｂから↑Ｖｂへの線形写像を表す単位行列Ｅ（↑Ｖｂの成分数と同次数の単位行列）とを縦に並べた行列となる。なお、本実施形態では、↑Ｖｂは７成分のベクトルであるので、Ｍ，Ｈはそれぞれ３行７列の慣性行列、Ｅは７行７列の単位行列である。従って、本実施形態では、ヤコビアン行列Ａは、１３行７列の行列である。
【０１６１】
【数１】
0005661023.tif 000002
【０１６２】
かかるヤコビアン行列Ａを用いる基体運動速度目標値決定部４５の処理の全体は、次のように行なわれる。
【０１６３】
すなわち、基体運動速度目標値決定部４５は、各演算処理周期において、まず、式（４）の線形写像を表すヤコビアン行列Ａを決定する。
【０１６４】
さらに、基体運動速度目標値決定部４５は、各脚リンク３の各関節の変位量（回転角）と、その時間的変化率（角速度）とに関する制約条件を満たすことができるようにするための条件として、↑Ｖｂに関する制約条件（↑Ｖｂの各成分又はその線形結合値の許容範囲を規定する条件）を設定する。
【０１６５】
そして、基体運動速度目標値決定部４５は、基体運動速度ベクトル↑Ｖｂを変数として構成される２次形式の所定の評価関数の値を、↑Ｖｂに関する上記制約条件が満たされる範囲内で最小化し得るような↑Ｖｂの値を、２次計画問題の公知の解法手法によって算出し、その算出した↑Ｖｂの値を目標基体運動速度ベクトル↑Ｖb_cmd2として決定する。
【０１６６】
このように目標基体運動速度ベクトル↑Ｖb_cmd2を決定する処理において、基体運動速度目標値決定部４５は、本実施形態ではヤコビアン行列Ａを次のように決定する。
【０１６７】
まず、本実施形態におけるヤコビアン行列Ａの決定手法に関する前提事項を説明しておく。前記式（４）の線形写像を表すヤコビアン行列Ａは、その構成要素である慣性行列Ｍ、Ｈと単位行列ＥとのうちのＭ、Ｈを変数とするヤコビアン行列である。
【０１６８】
かかるヤコビアン行列Ａは、基本的には、ロボット１の現在の目標歩容（前回の演算処理周期で決定した目標歩容）により規定されるロボット１の動作状態を起点として、当該起点の動作状態から基体２の位置及び姿勢の各成分を微小変化させた場合にそれに起因して発生するロボット１の全体重心の並進運動量ベクトル↑Ｐ及び該全体重心周りの角運動量ベクトル↑Ｌの発生感度を表す行列として算出すればよい。
【０１６９】
この場合、上記起点の動作状態から基体２の位置及び姿勢の各成分を微小変化させた場合にそれに起因して発生するロボット１の全体重心の並進運動量ベクトル↑Ｐ及び該全体重心周りの角運動量ベクトル↑Ｌは、例えばロボット１の各リンク毎にその重心位置の質点と該重心位置周りのイナーシャ（慣性モーメント）とを設定した動力学モデル（以降、このような動力学モデルをフルモデルという）を用いて算出することができる。その算出手法は、公知の手法を採用することができる。
【０１７０】
ただし、本実施形態では、目標制御対象ベクトル↑Ｓ_cmdに応じて決定する目標基体運動速度ベクトル↑Ｖb_cmd2の信頼性をより一層高めるために、現在の時刻（今回の演算処理周期の時刻）から次回の演算処理周期の時刻との間の期間内（１演算処理周期の時間内）での将来の目標歩容（目標脚位置姿勢及び目標基体位置姿勢）の瞬時値を推定し、その推定した将来の目標歩容の瞬時値により規定されるロボット１の動作状態を起点としてヤコビアン行列Ａを決定することとした。
【０１７１】
換言すれば、各演算処理周期において、ヤコビアン行列Ａを算出するための起点とするロボット１の動作状態を、現在の目標歩容により規定される動作状態から、将来の予想される動作状態寄りにずらした上で、ヤコビアン行列Ａを算出する。
【０１７２】
これは次の理由による。すなわち、基体２の位置及び姿勢の微小変化と、それに起因して発生する↑Ｐ及び↑Ｌとの間の関係は一般には非線形性を有するため、ロボット１の現在の目標歩容の状態を起点として決定したヤコビアン行列Ａを用いた場合には、決定される↑Ｖb_cmd2が、↑Ｓ_cmdを実現するための最適な値からずれを生じやすい。
【０１７３】
例えば、脚リンク３Ｒ，３Ｌの足平部１８Ｒ，１８Ｌの位置及び姿勢を一定に保持した場合における基体２の位置Ｘｂと、ロボット１の全体重心の位置Ｘｇにロボット１の全体質量Ｍallを乗算したもの（＝Ｍall・Ｘｇ）との間の関係に着目すると、該関係は、一般には、図６に実線の曲線ａ１で例示する如く、非線形な関係となる。
【０１７４】
なお、ここでは、Ｘｂ、Ｘｇは、理解の便宜上、グローバル座標系のＸ軸方向又はＹ軸方向又はＺ軸方向のいずれかの一軸方向の位置、例えばＸ軸方向の位置とする。この場合、Ｍall・Ｘｇは、ロボット１の全体重心のＸ軸方向の並進運動量の積分値に相当する状態量である。
【０１７５】
そして、現在の目標歩容における基体２の現在の位置が、例えば図６中に示すＸb(k)であるとした場合、現在の目標歩容の状態を起点として決定されるヤコビアン行列Ａ（以降、これを基本ヤコビアン行列Ａ１という）では、Ｘｂの微小変化と、これに対応するＭall・Ｘｇの変化との間の関係は、Ｘb(k)の位置での曲線ａ１の接線ａ２により示される線形関係として表されることとなる。
【０１７６】
従って、目標制御対象ベクトル↑Ｓ_cmdに対応する目標基体運動速度ベクトル↑Ｖb_cmd2を、上記基本ヤコビアン行列Ａ１を用いて決定した場合には、歩容生成部３２の演算処理の１周期（１演算処理周期）Δｔの時間における基体２の位置Ｘｂの目標とする変化量は、図６に示すΔＸb_aとなる。
【０１７７】
このΔＸb_aは、上記の如く基本ヤコビアン行列Ａ１の擬似逆行列を用いて決定した↑Ｖb_cmd2のうちの、位置Ｘｂの時間的変化率に相当する成分Ｖbxの値にΔｔを乗じてなる値である。換言すれば、ΔＸb_aは、基体２の位置Ｘｂと、Ｍall・Ｘｇとの間の関係が、上記接線ａ２で表される関係であると仮定した場合に、ΔＸb_aに対応するＭall・Ｘｇの変化量が、目標制御対象ベクトル↑Ｓ_cmdの構成要素である↑Ｐ_cmd2のうちのＸ軸方向の並進運動量の目標値Ｐx_cmd2にΔｔを乗じてなる値（＝Ｐx_cmd2・Δｔ）となるようなＸｂの変化量である。
【０１７８】
ただし、基体２の位置Ｘｂと、Ｍall・Ｘｇとの間の実際の関係が、曲線ａ１で表される関係である場合には、ΔＸb_aに対応するＭall・Ｘｇの実際の変化量は、図６中のＭall・ΔＸg_aとなる。この値Ｍall・ΔＸg_aは、一般には、目標制御対象ベクトル↑Ｓ_cmdにより規定されるＭall・Ｘｇの変化量の目標値としての上記の値Ｐx_cmd2・Δｔに対して誤差を生じるものとなる。
【０１７９】
そして、基体２の位置Ｘｂと、Ｍall・Ｘｇとの間の実際の関係が、曲線ａ１で表される関係である場合には、Ｍall・Ｘｇの変化量が、目標制御対象ベクトル↑Ｓ_cmdにより規定されるＭall・Ｘｇの変化量の目標値Ｐx_cmd2・Δｔになるようにするための、基体２の位置Ｘｂの適切な変化量は、図６に示すΔＸb_b（≠ΔＸb_a）である。
【０１８０】
一方、現在の目標歩容における基体２の位置Ｘb(k)と、この位置Ｘb(k)から上記ΔＸb_bだけ変化させた位置Ｘb(k)＋ΔＸb_bとの間の中間の位置Ｘb(k)＋ΔＸb_c（ただし、０＜ΔＸb_c＜ΔＸb_b）を想定した場合、微分積分学における平均値の定理に基づいて、当該中間の位置Ｘb(k)＋ΔＸb_cでの曲線ａ１の接線ａ３の傾きを、位置Ｘb(k)での曲線ａ１上の点と、位置Ｘb(k)＋ΔＸb_bでの曲線ａ１上の点とを結ぶ直線ａ４の傾きに一致もしくは近似させることができる。
【０１８１】
従って、基体２の位置Ｘｂが当該中間の位置Ｘb(k)＋ΔＸb_cとなるような将来の目標歩容を起点として、ヤコビアン行列Ａを決定し、このヤコビアン行列Ａを用いて↑Ｖb_cmd2を決定するようにすれば、この↑Ｖb_cmd2により規定される基体２の位置Ｘｂの変化量を、上記の適切な変化量ΔＸb_bに一致させ、もしくは、該変化量ΔＸb_bに対して近似精度の高い値にすることができる。すなわち、↑Ｖb_cmd2の信頼性を高めることができる。
【０１８２】
かかる事項は、↑Ｓ_cmdのうちの↑Ｐ_cmd2に影響を及ぼす↑Ｖb_cmd2の成分に限らず、↑Ｌ_cmd1に影響を及ぼす↑Ｖb_cmd2の成分についても同様に言える事項である。
【０１８３】
そこで、本実施形態では、上記の如く、現在の時刻（今回の演算処理周期の時刻）から次回の演算処理周期の時刻との間の期間内（１演算処理周期の時間内）での将来の目標歩容（目標脚位置姿勢及び目標基体位置姿勢）の瞬時値を推定し、その推定した将来の目標歩容の瞬時値により規定されるロボット１の動作状態を起点として、ヤコビアン行列Ａを決定することとした。
【０１８４】
この場合、本実施形態では、ヤコビアン行列Ａを決定するための起点とする動作状態を規定する将来の目標歩容（以降、これをヤコビアン行列決定用目標歩容という）の構成要素である目標脚位置姿勢及び目標基体位置姿勢（以降、これらをヤコビアン行列決定用目標脚位置姿勢、ヤコビアン行列決定用目標基体位置姿勢という）の瞬時値は、次のように決定される。
【０１８５】
すなわち、基体運動速度目標値決定部４５は、次式（７ａ）で示す如く、現在の目標基体位置姿勢（前回の演算処理周期で決定された目標基体位置姿勢。以下、これを目標基体位置姿勢(k)と表記する）と、１演算処理周期前の目標基体位置姿勢（前々回の演算処理周期で決定された目標基体位置姿勢。以下、これを目標基体位置姿勢(k-1)と表記する）との偏差、すなわち、前回の演算処理周期で決定された目標基体位置姿勢の変化量（＝目標基体位置姿勢(k)−目標基体位置姿勢(k-1)）に所定値の係数Ｋｐを乗じてなる補正項を、目標基体位置姿勢(k)に加え合わせることによって、ヤコビンアン行列決定用目標基体位置姿勢を決定する。
【０１８６】
同様に、基体運動速度目標値決定部４５は、次式（７ｂ）で示す如く、各脚リンク３毎の現在の目標脚位置姿勢（前回の演算処理周期で決定された目標脚位置姿勢。以下、これを目標脚位置姿勢(k)と表記する）と、１演算処理周期前の目標脚位置姿勢（前々回の演算処理周期で決定された目標脚位置姿勢。以下、これを目標脚位置姿勢(k-1)と表記する）との偏差、すなわち、前回の演算処理周期で決定された目標脚位置姿勢の変化量（＝目標脚位置姿勢(k)−目標脚位置姿勢(k-1)）に上記係数Ｋｐを乗じてなる補正項を、目標脚位置姿勢(k)に加え合わせることによって、ヤコビンアン行列決定用目標脚位置姿勢を決定する。
【０１８７】
なお、上記係数Ｋｐは、０≦Ｋｐ≦１の範囲であらかじめ決定された値である。本実施形態では、例えばＫｐ＝０．５である。
【０１８８】
ヤコビンアン行列決定用目標基体位置姿勢
＝目標基体位置姿勢(k)＋Ｋｐ・（目標基体位置姿勢(k)−目標基体位置姿勢(k-1)）
……（７ａ）
ヤコビンアン行列決定用目標脚位置姿勢
＝目標脚位置姿勢(k)＋Ｋｐ・（目標脚位置姿勢(k)−目標脚位置姿勢(k-1)）
……（７ｂ）
従って、本実施形態では、ヤコビアン行列決定用目標歩容は、歩容生成部３２の演算処理周期毎の目標歩容の変化量の時系列のうちの最新値に応じて、現在の目標歩容を補正してなる歩容として決定される。
【０１８９】
なお、ロボット１が腕リンク等、基体２に対して可動な他のリンクを備える場合には、当該他のリンクの先端部の将来の目標位置及び目標姿勢、あるいは、当該他のリンクの各関節の将来の目標変位量を、ヤコビンアン行列決定用目標基体位置姿勢やヤコビアン行列決定用目標脚位置姿勢と同じ手法で求めるようにすればよい。
【０１９０】
また、ヤコビアン行列決定用目標歩容は、現在の目標歩容と、１演算処理周期前の目標歩容とを含めて、３個以上の演算処理周期分の目標歩容から決定するようにしてもよい。
【０１９１】
そして、基体運動速度目標値決定部４５は、上記の如く決定したヤコビアン行列決定用目標歩容の状態を起点として、当該起点の状態から基体２の位置及び姿勢の各成分を微小変化させた場合にそれに起因して発生するロボット１の全体重心の並進運動量ベクトル↑Ｐ及び該全体重心周りの角運動量ベクトル↑Ｌの発生感度を表す行列としてのヤコビアン行列Ａを前記フルモデルを用いて算出する。この算出の仕方は、公知の手法と同様でよい。
【０１９２】
なお、ヤコビアン行列Ａを算出するために用いる動力学モデルは、前記フルモデルと異なる動力学モデル（例えば一部のリンクのイナーシャを無視した動力学モデル等）であってもよいが、できるだけ動力学的精度の高い動力学モデル（前記基本動作目標値決定部４２の処理で用いる動力学モデルよりも動力学的精度の高い動力学モデル）を使用することが望ましい。ここで、動力学的精度が高いというのは、ロボット１の運動によって動力学モデル上で発生するトータルの慣性力の、実際のロボット１で発生するトータルの慣性力に対する近似精度が高いことを意味する。
【０１９３】
以上が基体運動速度目標値決定部４５において、ヤコビアン行列Ａを決定する処理の詳細である。
【０１９４】
次に、基体運動速度ベクトル↑Ｖｂに関する制約条件（以降、基体運動速度制約条件という）は、次のように設定される。
【０１９５】
まず、本実施形態における基体運動速度制約条件の設定手法に関する前提事項を説明しておく。ロボット１の実現可能な各関節の変位量（回転角）やその時間的変化率（角速度）の値は、一般に、該関節もしくはこれを駆動する駆動機構（関節アクチュエータ３０及び動力伝達機構）の構造、あるいは、関節アクチュエータ３０の能力等に起因する制約を受ける。このため、ロボット１の各関節の変位量又はその時間的変化率（以降、関節速度という）はそれぞれ、ある可動範囲から逸脱した値をとることができないか、もしくは許容されない。
【０１９６】
そして、基体２の位置及び姿勢（基体２の関節１２の変位量を除く）の変化は、各脚リンク３の関節の変位量の変化に応じて発生するものであるから、基体運動速度ベクトル↑Ｖｂの目標値（目標基体運動速度ベクトル↑Ｖb_cmd2）は、歩容生成部３２の各演算処理周期で最終的に決定される目標歩容により規定される各脚リンク３の関節の変位量や関節速度を、それぞれの可動範囲から逸脱させないように設定する必要がある。
【０１９７】
上記基体運動速度制約条件は、このように各脚リンク３の関節の変位量や関節速度を、それぞれの可動範囲から逸脱することのないようにするために、基体運動速度ベクトル↑Ｖｂの各成分の値を制限する制約条件である。
【０１９８】
一方、本実施形態では、基体運動速度目標値決定部４５は、上記基体運動速度制約条件を満たし得る範囲内で、所定の評価関数（詳細は後述する）の値を最小化するような↑Ｖｂの値を、目標基体運動速度ベクトル↑Ｖb_cmd2として決定する。
【０１９９】
この場合、基体運動速度制約条件が、↑Ｖｂに関する１つもしくは複数の線形行列不等式、すなわち、Ｃ・↑Ｖｂ＋↑ｃ≧↑０、又はＣ・↑Ｖｂ＋↑ｃ≦↑０という形の１つもしくは複数の線形行列不等式により表される制約条件であれば、所謂、２次計画問題の公知の解法手法を適用した演算処理によって、目標基体運動速度ベクトル↑Ｖb_cmd2を決定することができる。
【０２００】
なお、上記線形行列不等式におけるＣは↑Ｖｂの係数に相当する行列、↑ｃは定数項に相当するベクトル、↑０は零ベクトルである。また、上記線形行列不等式における不等号（≧又は≦）は、左辺のベクトルと、右辺のベクトルとの各成分毎の大小関係を表す。
【０２０１】
そこで、本実施形態では、↑Ｖｂの各成分の値を制限する基体運動速度制約条件を、↑Ｖｂを変数とする線形行列不等式の形で設定することとした。
【０２０２】
ここで、ロボット１の脚リンク３Ｒ，３Ｌのうちの任意の１つの脚リンク３Ｒ又は３Ｌを総称的に脚リンク３_iと表記し、該脚リンク３_iの先端部の要素リンクである足平部１８の位置及び姿勢の時間的変化率を成分として構成されるベクトルである脚先端部運動速度ベクトルを↑Ｖft_iとおく。
【０２０３】
なお、↑Ｖft_iは、より詳しくは、グローバル座標系で見た脚リンク３_iの足平部１８の位置の時間的変化率（すなわち該足平部１８の代表点の並進移動速度ベクトル）の各成分と、グローバル座標系で見た脚リンク３_iの足平部１８の姿勢の時間的変化率（すなわち脚リンク３_iの足平部１８の姿勢の回転角速度ベクトル）の各成分とを並べた６成分の縦ベクトルである。
【０２０４】
また、脚リンク３_iの各関節の関節速度を成分として構成されるベクトルである脚関節速度ベクトルを↑ωleg_iとおき、この↑ωleg_iから、脚先端部運動速度ベクトル↑Ｖft_iへの線形写像を表すヤコビアン行列をＪleg_iとおく。
【０２０５】
なお、本実施形態では、脚リンク３_iは６個の関節を有するので、↑ωleg_iは６成分のベクトルである。そして、ヤコビアン行列Ｊleg_iは６行６列の正則行列となる。
【０２０６】
また、基体２（詳しくは下側基体１０）の代表点に対する脚リンク３_iの足平部１８の代表点の相対的な位置ベクトル（グローバル座標系における３成分のベクトル）を↑ｒ_bf_iとおく。
【０２０７】
このとき、上記↑Ｖft_i及び↑ωleg_iと、基体運動速度ベクトル↑Ｖｂとの間の関係は、次式（８）により表される。
【０２０８】
【数２】
0005661023.tif 000003
【０２０９】
なお、“×”は外積（ベクトル積）を表す算術符合である。また、式（８）のただし書きにて示した行列Ｂ_iの第１列の“０”（Ｅの下の“０”）は３行３列の零行列、第３列の１行目及び２行目の“０”は３行１列の零行列（零ベクトル）である。
【０２１０】
式（８）の右辺第１項は、脚リンク３_iの各関節の変位量を一定として基体２を↑Ｖｂの運動速度で動かした場合に発生する脚リンク３_iの足平部１８の並進移動速度及び回転角速度を意味する。また、式（８）の右辺第２項は、基体２の位置及び姿勢を一定として、脚リンク３_iの各関節を↑ωleg_iの各成分の関節速度で動かした場合に発生する脚リンク３_iの足平部１８の並進移動速度及び回転角速度を意味する。
【０２１１】
上記式（８）から次式（９）が得られる。
【０２１２】
↑ωleg_i＝Ｃ_i・↑Ｖｂ＋↑ｃ_i    ……（９）
ただし、Ｃ_i≡−(Ｊleg_i)-1・Ｂ_i    ……（９ａ）
↑ｃ_i≡(Ｊleg_i)-1・↑Ｖft_i    ……（９ｂ）
従って、脚関節速度ベクトル↑ωleg_iの値を制限する制約条件は、式（９）によって、基体運動速度ベクトル↑Ｖｂに関する制約条件に変換できることとなる。
【０２１３】
より具体的には、脚リンク３_iの脚関節速度ベクトル↑ωleg_iの各成分の許容範囲の下限値を成分として構成されるベクトルを↑ωleg_i_min、↑ωleg_iの各成分の許容範囲の上限値を成分として構成されるベクトルを↑ωleg_i_maxとおく。このとき、脚関節速度ベクトル↑ωleg_iの各成分の値をそれぞれの許容範囲内に収めるための制約条件は、式（９）に基づいて、次式（１０ａ），（１０ｂ）の２つの線形行列不等式により表されることとなる。
【０２１４】
Ｃ_i・↑Ｖｂ＋（↑ｃ_i−↑ωleg_i_min）≧↑０……（１０ａ）
−Ｃ_i・↑Ｖｂ＋（−↑ｃ_i＋↑ωleg_i_max）≧↑０……（１０ｂ）
そこで、本実施形態では、脚リンク３Ｒ，３Ｌのそれぞれに対応する上記線形行列不等式（１０ａ），（１０ｂ）を基体運動速度制約条件を表すものとして設定する。そして、この場合、各脚リンク３毎の上記線形行列不等式（１０ａ），（１０ｂ）により表される基体運動速度制約条件を満たす（上記線形行列不等式（１０ａ），（１０ｂ）が成立する）ように↑Ｖｂを決定すれば、各脚リンク３の関節の変位量及び関節速度がそれぞれの可動範囲から逸脱することのないように線形行列不等式（１０ａ），（１０ｂ）のパラメータのうちの↑ωleg_i_min、↑ωleg_i_maxを設定する。
【０２１５】
なお、本実施形態では、脚リンク３Ｒ，３Ｌの個数は２つであるので、基体運動速度制約条件は、トータル４個の線形行列不等式（各脚リンク３毎に２個ずつの線形行列不等式）により表されることとなる。
【０２１６】
補足すると、上記式（１０ａ），（１０ｂ）はそれぞれ、前記式（１００ａ），（１００ｂ）に対応している。この場合、式（１０ａ），（１０ｂ）におけるＣ_i、↑Ｖｂ、↑ｃ_i、↑ωleg_i_min、↑ωleg_i_maxがそれぞれ、前記式（１００ａ），（１００ｂ）におけるＣ_i、↑Ｖｓ、↑ｃ_i、↑ω_i_min、↑ω_i_maxに相当する。また、前記式（８）における↑Ｖft_i、Ｂ_i、Ｊleg_i、↑ωleg_iがそれぞれ、前記式（１００ａ），（１００ｂ）のただし書きにて定義した↑Ｖft_i、Ｂ_i、Ｊ_i、↑ω_iに相当する。
【０２１７】
以上を前提として、基体運動速度目標値決定部４５において、基体運動速度制約条件を設定する処理を説明する。
【０２１８】
本実施形態における基体運動速度制約条件を表す上記線形行列不等式（１０ａ），（１０ｂ）は、↑Ｖｂを変数とする線形行列不等式であるので、基体運動速度制約条件を設定するということは、これらの線形行列不等式（１０ａ），（１０ｂ）における↑Ｖｂ以外のパラメータの値、すなわち、前記式（９ａ）により定義した行列Ｃ_iと、前記式（９ｂ）により定義したベクトル↑ｃ_iと、↑ωleg_i_min及び↑ωleg_i_maxとを決定することを意味する。
【０２１９】
そして、基体運動速度目標値決定部４５は、各脚リンク３Ｒ，３Ｌのそれぞれに対応するＣ_i、↑ｃ_i、↑ωleg_i_min、↑ωleg_i_maxの値を、歩容生成部３２の各演算処理周期において次のように決定する。なお、Ｃ_i、↑ｃ_i、↑ωleg_i_min、↑ωleg_i_maxの値の決定の仕方は、脚リンク３Ｒ，３Ｌのいずれについても同様であるので、以降の説明では、代表的に、右側の脚リンク３ＲについてのＣ_i、↑ｃ_i、↑ωleg_i_min、↑ωleg_i_maxを決定する処理について説明する。
【０２２０】
まず、Ｃ_i、↑ｃ_iをそれぞれ前記式（９ａ），（９ｂ）の定義式に従って決定するために、(Ｊleg_i)-1、Ｂ_i、↑Ｖft_iが決定される。
【０２２１】
(Ｊleg_i)-1に関しては、本実施形態では、基体運動速度目標値決定部４５は、前記式（４）の線形写像を表すヤコビアン行列Ａを決定した場合と同様に、まず、将来の予測される目標歩容としてのヤコビアン行列決定用目標歩容に規定されるロボット１の動作状態を起点として、脚リンク３Ｒのヤコビアン行列Ｊleg_iを決定する。
【０２２２】
この場合、ヤコビアン行列決定用目標歩容は、前記ヤコビアン行列Ａを算出するために求めたものと同じである。そして、基体運動速度目標値決定部４５は、このヤコビアン行列決定用目標歩容により規定されるロボット１の動作状態を起点として、当該起点の動作状態から、脚リンク３Ｒの各関節の変位量を微小変化させた場合にそれに起因して発生する脚リンク３Ｒの足平部１８の位置及び姿勢の変化の感度を表す行列としてのヤコビアン行列Ｊleg_iをロボット１の幾何学モデルを用いて算出する。この算出の仕方は、公知の手法と同様でよい。
【０２２３】
さらに、基体運動速度目標値決定部４５は、算出したヤコビアン行列Ｊleg_iの逆行列を算出する。これにより、脚リンク３Ｒについての(Ｊleg_i)-1が決定される。
【０２２４】
また、Ｂ_iに関しては、前記式（９）のただし書きから明らかなように、行列Ｂ_iは、下側基体１０に対する脚リンク３Ｒの足平部１８の相対的な位置ベクトル↑ｒ_bf_iにより規定されることとなる。この場合、下側基体１０に対する脚リンク３Ｒの足平部１８の相対的な位置ベクトル↑ｒ_bf_iは、前回の演算処理周期で決定された目標歩容における脚リンク３Ｒの足平部１８の位置ベクトルから、該目標歩容における基体２の位置ベクトル（これは本実施形態では下側基体１０の位置ベクトルに一致する）を差し引いたベクトルとして算出される。
【０２２５】
そして、基体運動速度目標値決定部４５は、このように算出した↑ｒ_bf_iと、基体２（下側基体１０）の姿勢の角速度ベクトル[ωbx，ωby，ωbz]Tとの外積（＝↑ｒ_bf_i×[ωbx，ωby，ωbz]T）を表す行列を、Ｂ_iとして算出する。
【０２２６】
また、↑Ｖft_iに関しては、基体運動速度目標値決定部４５は、今回の演算処理周期で目標脚位置姿勢決定部４１が決定した脚リンク３Ｒの目標脚位置姿勢（今回値）と、前回の演算処理周期で目標脚位置姿勢決定部４１が決定した脚リンク３Ｒの目標脚位置姿勢（前回値）との差分のベクトルを、演算処理周期の時間で除算してなるベクトルを、↑Ｖft_iとして算出する。
【０２２７】
そして、基体運動速度目標値決定部４５は、上記の如く算出した(Ｊleg_i)-1、Ｂ_i、↑Ｖft_iから、前記式（９ａ），（９ｂ）の定義式に従って、脚リンク３Ｒに関するＣ_iと↑ｃ_iとを決定する。
【０２２８】
また、↑ωleg_i_min、↑ωleg_i_maxに関しては、基体運動速度目標値決定部４５は、前回の演算処理周期で最終的に決定された目標歩容（最新値の目標歩容）により規定される脚リンク３Ｒの各関節の変位量に応じて、脚関節速度ベクトル↑ωleg_iの各成分であ
る脚リンク３Ｒの各関節の関節速度の上限値及び下限値を決定することで、その下限値を並べたベクトルとしての↑ωleg_i_minと上限値を並べたベクトルとしての↑ωleg_i_maxとを決定する。
【０２２９】
この場合、脚リンク３Ｒの各関節の関節速度の上限値及び下限値は、該関節速度が関節アクチュエータ３０の能力等に応じて規定される可動範囲に収まることに加えて、該関節の変位量を、構造的な制約等により規定される可動範囲から逸脱させないように決定される。
【０２３０】
具体的には、本実施形態では、基体運動速度目標値決定部４５は、脚リンク３Ｒの各関節毎に、その関節速度の上限値及び下限値をそれぞれ、図７のｂ１，ｂ２のグラフで示す如く、最新値の目標歩容により規定される該関節の変位量θ（あるいは目標関節変位量決定部４８が決定した該関節の目標変位量の前回値）に応じて決定する。
【０２３１】
この場合、各関節の関節速度の上限値は、次式（１１ａ），（１１ｂ）で示す如く、該関節の変位量θに応じて決定される。同様に、各関節速度の下限値は、次式（１２ａ），（１２ｂ）で示す如く、該関節の変位量θに応じて決定される。
【０２３２】
θ＜θmax−（ωmax／Ｋth）である場合（Ｋth・（θmax−θ）＞ωmaxである場合）、
関節速度の上限値＝ωmax      ……（１１ａ）
θ≧θmax−（ωmax／Ｋth）である場合（Ｋth・（θmax−θ）≦ωmaxである場合）
関節速度の上限値＝Ｋth・（θmax−θ）      ……（１１ｂ）
θ＞θmin−（ωmin／Ｋth）である場合（Ｋth・（θmin−θ）＜ωminである場合）、
関節速度の下限値＝ωmin      ……（１２ａ）
θ≦θmin−（ωmin／Ｋth）である場合（Ｋth・（θmin−θ）≧ωminである場合）
関節速度の下限値＝Ｋth・（θmin−θ）      ……（１２ｂ）
上記式（１１ａ），（１１ｂ），（１２ａ），（１２ｂ）において、θmax、θminは、それぞれ該関節の変位量θの可動範囲（該関節の構造的な制約に基づく可動範囲）の上限値、下限値である。
【０２３３】
また、ωmax、ωminは、それぞれ該関節の関節速度の可動範囲（関節アクチュエータ３０の能力等の制約に基づく可動範囲）、換言すれば、該関節の変位量θの制約が無いと仮定した場合の該関節の関節速度の可動範囲の上限値、下限値である。なお、ωmaxは、関節の変位量θをθmin側からθmax側に増加させる向きの速度（本実施形態ではωmax＞０）であり、ωminは、関節の変位量θをθmax側からθmin側に減少させる向きの速度（本実施形態ではωmin＜０）である。
【０２３４】
また、Ｋthは、該関節の変位量θが上限値θmax又は下限値θminに近づいた場合における関節速度の大きさの減速度合いを規定する係数（本実施形態では、Ｋth＞０）である。
【０２３５】
上記θmax、θmin、ωmax、ωmin、Ｋthは、脚リンク３Ｒの各関節毎にあらかじめ設定された所定値である。なお、式（１１ｂ）におけるＫthと、式（１２ｂ）におけるＫthとは、本実施形態では互いに同一の値であるが、互いに異なる値に設定されていてもよい。
【０２３６】
上記の如く脚リンク３Ｒの各関節の関節速度の上限値及び下限値を決定することにより、最新値の目標歩容により規定される該関節の変位量θが、上限値θmaxに近い変位量になると、該関節の関節速度の上限値が本来の上限値ωmaxよりも低い値に制限される。そして、該関節の変位量θが、上限値θmaxまで達すると、該関節の関節速度の上限値がゼロとされるので、該関節の変位量θはθmaxよりも増加することはできなくなる。
【０２３７】
また、同様に、該関節の変位量θが、下限値θminに近い変位量になると、該関節の関節速度の下限値の大きさが本来の下限値ωminよりも小さい大きさの値に制限される。そして、該関節の変位量θが、下限値θminまで達すると、該関節の関節速度の下限値がゼロとされるので、該関節の変位量θはθminよりも減少することはできなくなる。
【０２３８】
従って、上記の如く脚リンク３Ｒの各関節の関節速度の上限値及び下限値を決定することにより、結果的に、脚リンク３Ｒの各関節の変位量が上限値θmaxと下限値θminとの間の可動範囲に収まり、且つ、該関節の関節速度が本来の上限値ωmaxと下限値ωminとの間の可動範囲に収まるように該関節の上限値及び下限値が決定されることとなる。
【０２３９】
以上が基体運動速度目標値決定部４５において、脚リンク３Ｒに関する線形行列不等式を規定するパラメータＣ_i、↑ｃ_i、↑ωleg_i_min、↑ωleg_i_maxを決定する処理の詳細である。脚リンク３Ｌに関する線形行列不等式を規定するパラメータＣ_i、↑ｃ_i、↑ωleg_i_min、↑ωleg_i_maxを決定する処理も同様に行なわれる。これにより、各脚リンク３Ｒ，３Ｌについての前記式（１０ａ），（１０ｂ）の線形行列不等式が、基体運動速度制約条件を表すものとして設定されることとなる。
【０２４０】
以上が、基体運動速度目標値決定部４５において、基体運動速度制約条件を設定する処理（詳しくは、各脚リンク３Ｒ，３Ｌに関する線形行列不等式を規定するパラメータＣ_i、↑ｃ_i、↑ωleg_i_min、↑ωleg_i_maxを決定する処理）の詳細である。
【０２４１】
次に、基体運動速度目標値決定部４５は、上記の如くヤコビアン行列Ａを決定する処理と、基体運動速度制約条件を設定する処理とを実行した上で、目標基体運動速度ベクトル↑Ｖb_cmd2を決定する処理を次のように実行する。
【０２４２】
前記したように、基体運動速度目標値決定部４５は、基体運動速度ベクトル↑Ｖｂを変数として構成される２次形式の所定の評価関数の値を、上記基体運動速度制約条件を満たし得る範囲内で最小化し得るような↑Ｖｂの値を、２次計画問題の公知の解法手法によって算出し、その算出した↑Ｖｂの値を目標基体運動速度ベクトル↑Ｖb_cmd2として決定する。
【０２４３】
この場合、↑Ｖｂを変数として構成される２次形式の上記評価関数は、Ａ・↑Ｖｂの値（前記式（４）の右辺の値）と目標制御対象ベクトル↑Ｓ_cmdの値とが一致する場合に値が最小となる項を含むようにあらかじめ設定された関数である。このような評価関数として、例えば次式（１３ａ）又は（１３ｂ）により表される関数を用いることができる。なお、Ｗ、Ｖは、重み行列（対角行列）である。
【０２４４】
ｆ１＝(↑Ｓ_cmd - Ａ・↑Ｖb)T・Ｖ・(↑Ｓ_cmd - Ａ・↑Ｖb)    ……（１３ａ）
ｆ２＝(↑Ｖb)T・Ｗ・↑Ｖb
＋(↑Ｓ_cmd - Ａ・↑Ｖb)T・Ｖ・(↑Ｓ_cmd - Ａ・↑Ｖb)
……（１３ｂ）
式（１３ａ）により表される評価関数ｆ１は、↑Ｓ_cmd及びＡ・↑Ｖbの差分のベクトルの各成分の二乗値を、重み行列Ｖの各対角成分を重み係数として線形結合してなる項により構成される関数である。この評価関数ｆ１は、換言すれば、↑Ｓ_cmd及びＡ・↑Ｖbの差分のベクトルの各成分の二乗値の重み付き平均の大きさを表す関数であり、↑Ｓ_cmd＝Ａ・↑Ｖbである場合に最小値（＝ゼロ）をとる。
【０２４５】
従って、この評価関数ｆ１の値を最小化するように↑Ｖｂを決定するようにした場合には、↑Ｓ_cmdの各成分にＡ・↑Ｖbの各成分を一致もしくはほぼ一致させるように↑Ｖｂを決定できることとなる。
【０２４６】
また、式（１３ｂ）により表される評価関数ｆ２は、前記式（１３ａ）により表される評価関数ｆ１を構成する項と同じ項（式（１３ｂ）の右辺の第２項）に、さらに↑Ｖｂの各成分の二乗値を、重み行列Ｗの各対角成分を重み係数として線形結合してなる項（式（１３ｂ）の右辺の第１項）を加え合わせて構成される関数である。
【０２４７】
この評価関数ｆ２の構成要素としての式（１３ｂ）の右辺の第１項は、換言すれば、↑Ｖｂの各成分の二乗値の重み付き平均の大きさを表す関数であり、↑Ｖb＝↑０である場合に最小値（＝ゼロ）をとる。
【０２４８】
従って、この評価関数ｆ２の値を最小化するように↑Ｖｂを決定するようにした場合には、↑Ｖｂの各成分の大きさが過大にならないようにしつつ（すなわち基体２の位置や姿勢の単位時間当たりの変化量の大きさが過大にならないようにしつつ）、極力、↑Ｓ_cmdの各成分にＡ・↑Ｖbの各成分を一致もしくはほぼ一致させ得るように↑Ｖｂを決定できることとなる。
【０２４９】
なお、評価関数ｆ２における重み行列Ｗを零行列としてもよく、その場合には、評価関数ｆ２は、式（１３ａ）の評価関数ｆ１と一致することとなる。
【０２５０】
目標基体運動速度ベクトル↑Ｖb_cmd2を決定するために用いる評価関数は、上記ｆ１、ｆ２のどちらを用いることも可能である。ただし、本実施形態では、前記基本目標基体運動速度ベクトル↑Ｖb_cmd1を目標基体運動速度ベクトル↑Ｖb_cmd2の基準目標とすることから、式（１３ａ）の評価関数ｆ１を↑Ｖb_cmd2を決定するために用いる評価関数として用いることとした。
【０２５１】
この場合、評価関数ｆ１におけるヤコビアン行列Ａとしては、前記した如くヤコビアン行列決定用目標歩容により規定されるロボット１の動作状態を起点として算出したヤコビアン行列が用いられる。
【０２５２】
また、重み行列Ｖは、目標制御対象ベクトル↑Ｓ_cmdと同じ次数（本実施形態では、１３次数）で、その各対角成分の値があらかじめ定められた行列である。この重み行列Ｖの各対角成分の値（重み係数）は、基本的には、Ａ・↑Ｖb_cmd2により算出される制御対象ベクトル↑Ｓの各成分の値の、目標制御対象ベクトル↑Ｓ_cmdにより規定される目標値に対する誤差の許容度合いに応じて設定すればよい。
【０２５３】
本実施形態では、例えば、目標制御対象ベクトル↑Ｓ_cmdのうちの並進運動量ベクトル↑Ｐの目標値（＝↑Ｐ_cmd2）と角運動量ベクトル↑Ｌの目標値（＝↑Ｌ_cmd1）とに対応する対角成分（６個の対角成分）と、↑Ｓ_cmdのうちの基体運動速度ベクトル↑Ｖｂの目標値（＝↑Ｖb_cmd）のうち、関節１２の変位量の目標値を除く成分（すなわち下側基体１０の位置及び姿勢の目標値）に対応する対角成分（６個の対角成分）と、関節１２の変位量の目標値に対応する対角成分（１個の対角成分）との３種類の対角成分に分類して、重み行列Ｖの各対角成分の値を設定した。
【０２５４】
一例として、重み行列Ｖの対角成分のうち、並進運動量ベクトル↑Ｐの目標値と角運動量ベクトル↑Ｌの目標値とに対応する６個の対角成分の値を１．０、下側基体１０の位置及び姿勢の目標値に対応する６個の対角成分の値を０．５、基体２の関節１２の変位量の目標値に対応する１個の対角成分の値を０．１というように、重み行列Ｖを設定した。
【０２５５】
そして、本実施形態では、基体運動速度目標値決定部４５は、基体運動速度制約条件を満たし得る範囲内で（各脚リンク３Ｒ，３Ｌについての前記式（１０ａ），（１０ｂ）の線形行列不等式が成立する範囲内で）、式（１３ａ）の評価関数ｆ１の値を最小化し得るような↑Ｖｂの値を、２次計画問題の公知の解法手法の演算処理によって算出する。そして、基体運動速度目標値決定部４５は、算出した↑Ｖｂの値を目標基体運動速度ベクトル↑Ｖb_cmd2として決定する。
【０２５６】
この場合、２次計画問題の解法手法としては、例えば、内点凸法、あるいは、信頼領域Reflective法が用いられる。
【０２５７】
これにより、歩容生成部３２の各演算処理周期において、各脚リンク３Ｒ，３Ｌの関節の変位量及び関節速度がそれぞれの可動範囲から逸脱しないようにするための基体運動速度制約条件を満たし得る範囲内で、Ａ・↑Ｖb_cmd2の値が極力、目標制御対象ベクトル↑Ｓ_cmdに一致もしくはほぼ一致するように、目標基体運動速度ベクトル↑Ｖb_cmd2が決定されることとなる。
【０２５８】
また、この場合、↑Ｓ_cmdは、修正後基本目標並進運動量ベクトル↑Ｐ_cmd2及び基本目標角運動量ベクトル↑Ｌ_cmd1に加えて、前記基体位置姿勢偏差をゼロに近づけるための操作量たる基本目標基体運動速度ベクトル↑Ｖb_cmd1を含んでいるため、↑Ｖb_cmd2は、↑Ｖb_cmd1から乖離しないようにしつつ、Ａ・↑Ｖb_cmd2のうちの並進運動量ベクトル↑Ｐと、角運動量ベクトル↑Ｌとがそれぞれ極力、修正後基本目標並進運動量ベクトル↑Ｐ_cmd2、基本目標角運動量ベクトル↑Ｌ_cmd1に一致するように決定されることとなる。
【０２５９】
以上が本実施形態における基体運動速度目標値決定部４５の処理の詳細である。
【０２６０】
歩容生成部３２は、以上の如く基体運動速度目標値決定部４５の処理を実行した後、次に、目標基体位置姿勢決定部４６の処理を実行する。
【０２６１】
この目標基体位置姿勢決定部４６は、基体運動速度目標値決定部４５で決定された目標基体運動速度ベクトル↑Ｖb_cmd2を逐次積分することによって、目標基体位置姿勢を決定する。
【０２６２】
具体的には、目標基体位置姿勢決定部４６は、各演算処理周期において、目標基体運動速度ベクトル↑Ｖb_cmd2の今回値に歩容生成部３２の演算処理周期の時間Δｔを乗じてなる値（＝↑Ｖb_cmd2・Δｔ）を、現在の目標基体位置姿勢の値（前回の演算処理周期において決定した値）に加えることにより、目標基体位置姿勢の今回値を決定する。
【０２６３】
本実施形態では、歩容生成部３２の各演算処理周期において、以上の如く目標基体位置姿勢決定部４６により決定される目標基体位置姿勢（今回値）と、前記目標脚位置姿勢決定部４１により決定される各脚リンク３の目標脚位置姿勢（今回値）とが、目標歩容（今回値）の構成要素とされる。
【０２６４】
そして、本実施形態では、歩容生成部３２は、さらに、目標関節変位量決定部４８の処理を実行する。この目標関節変位量決定部４８には、各演算処理周期において、決定された目標歩容の構成要素（目標基体位置姿勢及び目標脚位置姿勢）が与えられる。
【０２６５】
そして、目標関節変位量決定部４８は、与えられた目標歩容により規定されるロボット１の各関節の目標関節変位量を、ロボット１の幾何学モデル（剛体リンクモデル）に基づく逆運動学の演算処理によって算出する。この場合、ロボット１の幾何学モデルは、前記目標歩容上重心位置算出部４７で用いるものと同じである。
【０２６６】
なお、前記目標歩容上重心位置算出部４７では、目標歩容（目標基体位置姿勢及び目標脚位置姿勢）の代わりに、目標関節変位量決定部４８で決定された各関節の目標関節変位量（前回値）を用いて、目標歩容上重心位置↑Ｘg_cmd2を決定するようにしてもよい。このようにした場合には、目標歩容上重心位置算出部４７で、目標歩容により規定される各関節の変位量を算出する処理は不要である。
【０２６７】
補足すると、ロボット１が腕リンク等、基体２に対して可動な他のリンクを備える場合には、ロボット１の目標歩容のうちの当該他のリンクに関する構成要素は、例えば基本動作目標値決定部４２で適宜、決定しておくようにすればよい。一例として、ロボット１が上側基体１１から延設された腕リンクを備える場合には、該ロボット１の歩行動作又は走行動作を行なう目標歩容の生成処理では、該目標歩容における上側基体１１に対する該腕リンクの全体の姿勢（ひいては、該腕リンクの各関節の変位量）を、一定の姿勢としたり、あるいは、基体２の関節１２の回転軸周りに周期的に腕リンクの全体を動かすような該腕リンクの目標歩容を採用すればよい。
【０２６８】
以上が、本実施形態における歩容生成部３２の処理の詳細である。
【０２６９】
ここで、以上説明した実施形態と本願発明との対応関係について補足しておく。本実施形態では、制御処理ユニット３１は、歩容生成部３２を備えることによって、本発明の歩容生成装置又は動作目標生成装置としての機能を有する。
【０２７０】
そして、ロボット１の基体２は、本発明の歩容生成装置又は動作目標生成装置における特定部位に相当する。また、ロボット１の各脚リンク３は、本発明の歩容生成装置における脚リンクに相当すると共に、本発明の動作目標生成装置におけるリンク機構に相当する。
【０２７１】
また、歩容生成部３２の目標脚位置姿勢決定部４１によって、本発明の歩容生成装置における目標脚位置姿勢決定手段又は本発明の動作目標生成装置におけるリンク機構目標先端位置姿勢決定手段が実現される。
【０２７２】
また、歩容生成部３２の基体運動速度目標値決定部４５、目標基体位置姿勢決定部４６の処理によってそれぞれ、本発明の歩容生成装置又は動作目標生成装置における特定部位運動速度目標値決定手段、目標特定部位位置姿勢決定手段が実現される。
【０２７３】
この場合、本実施形態では、基体運動速度目標値決定部４５は、ヤコビアン行列Ａを決定する処理も実行するので、本発明の歩容生成装置又は動作目標生成装置におけるヤコビアン行列決定手段としての機能を含んでいる。
【０２７４】
また、歩容生成部３２の基本動作目標値決定部４２、基本目標並進運動量修正部４３、基本目標基体運動速度決定部４４、目標歩容上重心位置算出部４７の処理によってそれぞれ、本発明の歩容生成装置における基本動作目標値決定手段、基本目標並進運動量修正手段、基本目標基体運動速度決定手段、目標歩容上重心位置算出手段が実現される。
【０２７５】
さらに、基本動作目標値決定部４２、基本目標並進運動量修正部４３、及び基本目標基体運動速度決定部４４の処理の全体によって、本発明の歩容生成装置又は動作目標生成装置における目標制御対象ベクトル決定手段が実現される。
【０２７６】
以上説明した本実施形態によれば、前記基体運動速度目標値決定部４５は、各脚リンク３Ｒ，３Ｌについての前記式（１０ａ），（１０ｂ）の線形行列不等式により表される基体運動速度制約条件を満たし得る範囲内で、式（１３ａ）の評価関数ｆ１の値を最小化し得るような基体運動速度ベクトル↑Ｖｂの値を、目標基体運動速度ベクトル↑Ｖb_cmd2として決定する。そして、この↑Ｖb_cmd2を積分することにより、目標歩容における目標基体位置姿勢が決定される。
【０２７７】
この場合、前記式（１０ａ），（１０ｂ）の線形行列不等式は、前記したように、各脚リンク３Ｒ，３Ｌの各関節の変位量と関節速度とを制限する関節動作制約条件に応じて前記した如く設定されるので、該線形行列不等式により表される基体運動速度制約条件を満たし得る範囲内で、↑Ｖｂの値を決定することで、結果的に、各脚リンク３Ｒ，３Ｌの各関節の関節動作制約条件が満たされるように（各関節の変位量と関節速度とがそれぞれの可動範囲を逸脱しないように）、↑Ｖｂひいては、目標基体位置姿勢が逐次決定されることとなる。
【０２７８】
従って、決定した目標基体位置姿勢の時系列に対して、各脚リンク３Ｒ，３Ｌの各関節の関節動作制約条件が満たされているか否かを逐次確認したりすることなく、該関節動作制約条件を満たしつつ、目標基体運動速度ベクトル↑Ｖb_cmd2が、目標制御対象ベクトル↑Ｓ_cmdを達成するために適切な値（Ａ・↑Ｖb_cmd2が↑Ｓ_cmdに一致もしくはほぼ一致する値）に極力追従するように、↑Ｖb_cmd2を逐次決定することができる。
【０２７９】
また、↑Ｖb_cmd2を決定するための主目標たる目標制御対象ベクトル↑Ｓ_cmdの構成要素のうちの並進運動量ベクトル↑Ｐの目標値として設定される修正後基本目標並進運動量ベクトル↑Ｐ_cmd2は、基本動作目標値決定部４２で決定した基本目標並進運動量ベクトル↑Ｐ_cmd1を基準の目標としつつ、目標歩容上重心位置↑Ｘg_cmd2が、基本動作目標値決定部４２で決定した基本目標重心位置↑Ｘg_cmd1からずれるのを抑制するためのフィードバック操作量により該基本目標並進運動量ベクトル↑Ｐ_cmd1を修正することにより決定される。
【０２８０】
また、目標制御対象ベクトル↑Ｓ_cmdの構成要素のうちの角運動量ベクトル↑Ｌの目標値として、基本動作目標値決定部４２で決定された基本角並進運動量ベクトル↑Ｐ_cmd2が設定される。
【０２８１】
また、目標制御対象ベクトル↑Ｓ_cmdの構成要素のうちの基体運動速度ベクトル↑Ｖｂの目標値して設定される基本目標基体運動速度ベクトル↑Ｖb_cmd1は、目標歩容における目標基体位置姿勢が、基本動作目標値決定部４２で決定した基本目標基体位置姿勢からずれるのを抑制するためのフィードバック操作量として決定される。
【０２８２】
従って、基体運動速度目標値決定部４５で逐次決定される目標基体運動速度ベクトル↑Ｖb_cmd2は、基本動作目標値決定部４２の処理における動力学モデル上で満たすようにした動力学を極力崩さないようにしつつ、目標基体運動速度ベクトル↑Ｖb_cmd2を積分することにより決定される目標基体位置姿勢が、基本動作目標値決定部４２で決定した基本目標基体位置姿勢から乖離するとことがないように、決定されることとなる。
【０２８３】
このため、本実施形態によれば、各脚リンク３の関節動作制約条件と、動力学的な要件とを満たしつつ、ロボット１が姿勢を崩すことが無いような目標歩容を生成できる。
【０２８４】
また、基体運動速度目標値決定部４５の処理では、ロボット１の基体２の位置及び姿勢と、ロボット１の全体重心の並進運動量ベクトルの積分値に相当する状態量や全体重心周りの角運動量ベクトルの積分値に相当する状態量との間の非線形性を考慮して、将来の目標歩容の推定値としての前記ヤコビアン行列決定用目標歩容を起点として、ヤコビアン行列Ａを決定するようにしたので、前記評価関数ｆ１の値を最小化し得る基体運動速度ベクトル↑Ｖｂの値として決定される目標基体運動速度ベクトル↑Ｖb_cmd2の信頼性を高めることができる。
【０２８５】
換言すれば、↑Ｖb_cmd2に、決定したヤコビアン行列Ａを乗じることによって得られる制御対象ベクトル↑Ｓの値の、目標制御対象ベクトル↑Ｓ_cmdに対する一致度合いを高めることができる。
【０２８６】
従って、本実施形態によれば、各脚リンク３の関節動作制約条件を適切に反映させつつ、ロボット１の移動を安定して継続的に行い得る目標歩容を生成することができる。
【０２８７】
ここで、本実施形態の上記の効果に関する検証結果について図８〜図１１を参照して以下に説明する。
【０２８８】
図８（ａ）のグラフa11，b11は、本実施形態の実施例で作成した目標歩容により規定されるロボット１の脚リンク３Ｒ，３Ｌのうちの一方の脚リンク３の膝関節部１５の関節２２の変位量（回転角）の経時変化と、該脚リンク３の股関節部１３の関節１９の変位量（回転角）の経時変化とをそれぞれ示すグラフである。この場合、ロボット１の目標歩容は、通常の歩行よりも大きな歩幅（例えば６００ｍｍ）の歩幅で歩行を行なう歩容である。
【０２８９】
また、図８（ｂ）のグラフa12，b12は、比較例で作成した目標歩容により規定されるロボット１のロボット１の脚リンク３Ｒ，３Ｌのうちの一方の脚リンク３（図８（ａ）に係わる脚リンク３と同じ側の脚リンク）の膝関節部１５の関節２２の変位量（回転角）の経時変化と、該脚リンク３の股関節部１３の関節１９の変位量（回転角）の経時変化とをそれぞれ示すグラフである。
【０２９０】
この場合、比較例におけるロボット１の目標歩容は、図８（ａ）の実施例の場合と同じ歩幅（６００ｍｍ）で歩行を行なう歩容である。
【０２９１】
ただし、この比較例においては、前記線形行列不等式（１０ａ），（１０ｂ）による制約は行なわないものとして、前記評価関数ｆ１の値を最小化し得るようにヤコビアン行列Ａの擬似逆行列Ａ+を算出し、この擬似逆行列Ａ+を目標制御対象ベクトル↑Ｓ_cmdに乗じることにより算出される↑Ｖｂの値を、目標基体運動速度ベクトル↑Ｖb_cmd2として決定するようにした。なお、上記擬似逆行列Ａ+は、例えばヤコビアン行列Ａと前記重み行列Ｖとから、Ａ+＝(ＡT・Ｖ-1・Ａ)-1・ＡT・Ｖ-1という演算により算出される行列である。
【０２９２】
前記線形行列不等式（１０ａ），（１０ｂ）による制約は行なわない図８（ｂ）の比較例では、図中の破線の丸枠部c12で示すように、目標歩容により規定される脚リンク３の膝関節部１５の関節２２の変位量が、可動範囲の下限値まで達してしまい、該脚リンク３の膝関節部１５が伸びきってしまうような歩容が作成された。
【０２９３】
これに対して、図８（ａ）に示す実施例では、図中の破線の丸枠部c11で示すように、目標歩容により規定される脚リンク３の股関節部１３の関節１９の変位量（ヨー軸周り方向の変位量）が比較例よりも大きめ変化することで、該脚リンク３の膝関節部１５の関節２２の変位量が可動範囲を逸脱することないような歩容、すなわち、該脚リンク３の膝関節部１５が伸びきってしまうことのないような歩容を作成することができた。
【０２９４】
このように、前記実施形態によれば、前記線形行列不等式（１０ａ），（１０ｂ）による制約を加味して、目標基体運動速度ベクトル↑Ｖb_cmd2を決定するようにしたことによって、各脚リンク３の関節の変位量等が、可動範囲から逸脱することのないような歩容を生成することができる。
【０２９５】
次に、図９（ａ）のグラフa13は、本実施形態の実施例における目標基体位置姿勢のうちの、下側基体１０のＹ軸周り（ピッチ軸周り）の傾斜角の経時変化を示している。この場合、ロボット１の目標歩容は、例えば４００ｍｍの最大歩幅で歩行を行なう歩容である。また、この実施例における基本目標基体位置姿勢のうちの、下側基体１０のＹ軸周り（ピッチ軸周り）の姿勢は０degである。
【０２９６】
また、図９（ｂ）のグラフa14は、比較例における目標基体位置姿勢のうちの、下側基体１０のＹ軸周り（ピッチ軸周り）の傾斜角の経時変化を示している。
【０２９７】
この場合、比較例におけるロボット１の目標歩容は、図９（ａ）の実施例と同様に、４００ｍｍの最大歩幅で歩行を行なう歩容である。また、この比較例における基本目標基体位置姿勢のうちの、下側基体１０のＹ軸周り（ピッチ軸周り）の姿勢は、図９（ａ）の実施例と同様に０degである。
【０２９８】
ただし、この比較例においては、目標制御対象ベクトルから基本基体運動速度ベクトル↑Ｖb_cmd1を除外し、修正後基本目標並進運動量ベクトル↑Ｐ_cmd2と、基本目標角運動量ベクトル↑Ｌ_cmd1とにより目標制御対象ベクトルを構成するものとした。
【０２９９】
図９（ｂ）を参照して判るように、基体運動速度ベクトル↑Ｖb_cmd1を目標制御対象ベクトル↑Ｓ_cmdの構成要素として含まない比較例では目標基体位置姿勢が、最終的に基本目標基体位置姿勢（＝０deg）に対して１３deg程度の定常的なずれを生じた。
【０３００】
これに対して、基体運動速度ベクトル↑Ｖb_cmd1を目標制御対象ベクトル↑Ｓ_cmdの構成要素として含む実施例では、図９（ａ）に示すように目標基体位置姿勢の、基本目標基体位置姿勢に対する定常的なずれを解消することができた。
【０３０１】
従って、基体運動速度ベクトル↑Ｖb_cmd1を目標制御対象ベクトル↑Ｓ_cmdの構成要素として含む本実施形態によれば、基体運動速度ベクトル↑Ｖb_cmd1を目標制御対象ベクトル↑Ｓ_cmdに含めない場合に比して、目標基体位置姿勢を、基本目標基体位置姿勢から大きく乖離させることなく該基本目標基体位置姿勢に追従させることができることが判る。
【０３０２】
次に、図１０（ａ）及び図１１（ａ）のそれぞれのグラフa15、a17は、それぞれ、本実施形態の実施例で作成した目標歩容により規定されるロボット１の全体重心のＸ軸方向の並進運動量の誤差（基本目標並進運動量ベクトル↑Ｐ_cmd1のＸ軸方向成分に対する誤差）、該目標歩容により規定されるロボット１の全体重心でのＹ軸周り（ピッチ軸周り）の角運動量の誤差（基本目標角運動量ベクトル↑Ｌ_cmd1のＹ軸周り成分に対する誤差）の経時変化を示すグラフである。この場合、ロボット１の目標歩容は、例えば３５５ｍｍの最大歩幅で歩行を行なう歩容である。
【０３０３】
また、図１０（ｂ）及び図１１（ｂ）のそれぞれのグラフa16、a18は、それぞれ、比較例で作成した目標歩容により規定されるロボット１の全体重心のＸ軸方向の並進運動量の誤差（基本目標並進運動量ベクトル↑Ｐ_cmd1のＸ軸方向成分に対する誤差）、該目標歩容により規定されるロボット１の全体重心でのＹ軸周り（ピッチ軸周り）の角運動量の誤差（基本目標角運動量ベクトル↑Ｌ_cmd1のＹ軸周り成分に対する誤差）の経時変化を示すグラフである。
【０３０４】
ただし、図１０（ｂ）及び図１１（ｂ）の比較例においては、各演算処理周期においてヤコビアン行列Ａを算出するときの起点とするロボット１の動作状態は、図１０（ａ）及び図１１（ａ）の実施例と異なり、前回の演算処理周期で決定した目標歩容（既に決定された目標歩容のうちの最新値の目標歩容）により規定される動作状態とした。
【０３０５】
なお、図１０（ａ）及び図１１（ａ）の実施例と、図１０（ｂ）及び図１１（ｂ）の比較例とでは、ヤコビアン行列Ａの決定処理の相違による効果を明確にするために、目標制御対象ベクトル↑Ｓ_cmdの構成要素のうち、前記修正後基本並進運動量ベクトル↑Ｐ_cmd2を、基本目標並進運動量ベクトル↑Ｐ_cmd1に一致させるものとした。
【０３０６】
図１０（ｂ）のグラフa16で見られるように、比較例では、ロボット１の全体重心の並進運動量（Ｘ軸方向の並進運動量）の誤差の絶対値は、最大で２．３kg・m/s程度であった。これに対して、図１０（ａ）のグラフa15で見られるように、実施例では、ロボット１の全体重心の並進運動量（Ｘ軸方向の並進運動量）の誤差の絶対値は、最大でも０．００８kg・m/s以下に収まっている。
【０３０７】
また、図１１（ｂ）のグラフa18で見られるように、比較例では、ロボット１の全体重心周りの角運動量（Ｙ軸周りの角運動量）の誤差の絶対値は、最大で０．８kg・m2/s程度であった。これに対して、図１１（ａ）のグラフa17で見られるように、実施例では、ロボット１の全体重心周りの角運動量（Ｙ軸周りの角運動量）の誤差の絶対値は、最大でも０．００３kg・m2/s以下に収まっている。
【０３０８】
従って、ヤコビアン行列Ａを算出するためのロボット１の起点の動作状態として、現在の目標歩容により規定される動作状態よりも将来の予測される動作状態寄りにずらした動作状態を用いる本実施形態によれば、ヤコビアン行列Ａを算出するためのロボット１の起点の動作状態として、現在の目標歩容により規定される動作状態を用いる場合に比して、目標制御対象ベクトル↑Ｓ_cmdにより精度よく追従させ得る目標歩容を生成できることがわかる。
【０３０９】
［第２実施形態］
次に、本発明の第２実施形態を以下に図１２を参照して説明する。なお、本実施形態は、基体運動速度目標値決定部４５の処理のうちの一部の処理だけが前記第１実施形態と相違するものである。従って、本実施形態の説明では、第１実施形態と相違する事項を中心に説明し、第１実施形態の同一の事項については説明を省略する。
【０３１０】
本実施形態は、前記線形行列不等式（１０ａ），（１０ｂ）における↑ωleg_i_min及び↑ωleg_i_maxの決定処理だけが第１実施形態と相違するものである。
【０３１１】
具体的には、本実施形態では、基体運動速度目標値決定部４５は、脚リンク３_i（３Ｒ又は３Ｌ）についての↑ωleg_i_min及び↑ωleg_i_maxのそれぞれの成分、すなわち、脚リンク３_iの各関節の関節速度の上限値及び下限値をそれぞれ、図１２のグラフｃ１，ｃ２で示す如く、最新値の目標歩容により規定される該関節の変位量θ（あるいは目標関節変位量決定部４８が決定した該関節の目標変位量の前回値）に応じて決定する。
【０３１２】
この場合、脚リンク３_iの各関節の関節速度の上限値及び下限値は、それぞれ次式（１５ａ），（１５ｂ）によって、該関節の変位量θに応じて決定される。
【０３１３】
関節速度の上限値＝√（２・ａdec・（θmax−θ））    ……（１５ａ）
関節速度の下限値＝−√（２・ａdec・（θ−θmin））    ……（１５ｂ）
上記式（１５ａ），（１５ｂ）において、θmax、θminは、それぞれ該関節の変位量θの可動範囲（該関節の構造的な制約に基づく可動範囲）の上限値、下限値である。
【０３１４】
また、ａdecは、該関節を駆動する関節アクチュエータ３０の能力に基づく該関節の変
位量の変化加速度（回転角加速度）の大きさの最大値以下の範囲で、該関節の変位量θをθminに一致させた場合の式（１５ａ）の右辺の値が、該関節の関節速度の可動範囲（関節アクチュエータ３０の能力等の制約に基づく可動範囲）の上限値ωmax以下となり、且つ、関節の変位量θをθmaxに一致させた場合の式（１５ｂ）の右辺の値が、該関節の関節速度の可動範囲の下限値ωmin以上となるようにあらかじめ設定された所定値の加速度値（本実施形態では正の値）である。
【０３１５】
例えば、この加速度値ａdecの大きさは、該関節の変位量の変化加速度（回転角加速度）の大きさの最大値の１／２程度の大きさに設定される。なお、加速度値ａdecは、各脚リンク３の全ての関節について同一の値である必要はない。
【０３１６】
本実施形態は、以上説明した事項以外は、前記第１実施形態と同じである。
【０３１７】
本実施形態によれば、上記の如く脚リンク３_iの各関節の関節速度の上限値及び下限値を決定することにより、第１実施形態の場合と同様に、各脚リンク３の各関節の変位量が上限値θmaxと下限値θminとの間の可動範囲に収まり、且つ、該関節の関節速度が上限値ωmaxと下限値ωminとの間の可動範囲に収まるように該関節の上限値及び下限値が決定されることとなる。
【０３１８】
加えて、本実施形態では、各脚リンク３の各関節の変位量及び関節速度が、例えば図１２の軌跡線ｃ３で示す如く、前記式（１５ａ）により規定されるグラフｃ１に沿って変化していくような状況で、一定の加速度値ａdecで、該関節の関節速度の大きさを変化させていくことができる。
【０３１９】
このことは、前記式（１５ｂ）により規定されるグラフｃ２に沿って脚リンク３の関節の変位量及び関節速度が変化していく場合でも同様である。
【０３２０】
従って、前記式（１５ａ）により規定されるグラフｃ１又は式（１５ｂ）により規定されるグラフｃ２に沿って変化していくような状況で、各関節の関節アクチュエータの駆動力をほぼ一定に保つようにすることができる目標歩容を生成することができる。ひいては、ロボット１の動作が滑らかなものとなるような目標歩容を生成することができる。
【０３２１】
次に、前記第１及び第２実施形態以外の本発明の変形態様についていくつか説明しておく。
【０３２２】
前記各実施形態では、２つの脚リンクを有する２足移動ロボット１を例にとって説明したが、本発明の歩容生成装置は、３つの以上の脚リンクを有する脚式移動ロボットについても適用できる。
【０３２３】
また、前記各実施形態では、目標基体運動速度ベクトル↑Ｖd_cmd2を２次計画問題の解法手法の演算処理により決定するための評価関数として、前記式（１３ａ）の評価関数ｆ１を用いたが、前記式（１３ｂ）の評価関数ｆ２を用いるようにしてもよい。
【０３２４】
また、前記各実施形態では、基体運動速度制約条件を表す線形行列不等式を、各脚リンク３の各関節の変位量及び関節速度をそれぞれの可動範囲内に収めるように設定したが、例えば、各脚リンク３の各関節の変位量が、その可動範囲から逸脱する恐れのない形態の目標歩容を生成するような場合には、各脚リンク３の各関節の関節速度の可動範囲だけを考慮して、線形行列不等式を設定するようにしてもよい。
【０３２５】
この場合には、前記式（１０ａ）の線形行列不等式における↑ωleg_i_minの各成分の値を、該成分に対応する関節の関節速度の可動範囲の下限値に一致させると共に、前記式（１０ｂ）の線形行列不等式における↑ωleg_i_maxの各成分の値を、該成分に対応する関節の関節速度の可動範囲の上限値に一致させるように式（１０ａ），（１０ｂ）の線形行列不等式を設定すればよい。
【０３２６】
さらに、ロボット１の各脚リンク３が、例えばジンバル機構を備えるような構造のリンク機構である場合には、各脚リンク３の個々の関節毎の変位量や関節速度だけでなく、該脚リンク３の特定の複数の関節の変位量の線形結合値をある範囲内の値に制限することが必要となる場合がある。そして、このような場合には、脚リンク３の特定の複数の関節の変位量の線形結合値をある範囲内の値に制限するための線形行列不等式を、基体運動速度制約条件を表す線形行列不等式に含めるようにしてもよい。
【０３２７】
このように、脚リンク３の複数の関節の変位量の線形結合値をある範囲内の値に制限するための線形行列不等式は、次のように構成することができる。
【０３２８】
ある脚リンク３_iの各関節の変位量の線形結合値（以降これをΦとおく）は、該脚リンク３_iの各関節の変位量を成分とするベクトル↑θleg_iと、各関節の変位量に掛かる係数を成分とするベクトル↑ａとを用いて、次式（１６）により表される。なお、↑ａのうち、上記特定の複数の関節以外の関節に対応する成分は、ゼロでよい。
【０３２９】
Φ＝↑ａT・↑θleg_i    ……（１６）
従って、線形結合値Φの時間的変化率をΦｄ（≡dΦ/dt）とおくと、このΦｄと、脚リンク３_iの各関節の関節速度を成分とするベクトル↑ωleg_iとの間の関係は、次式（１７）により与えられる。
【０３３０】
Φd（≡dΦ/dt）
＝↑ａT・↑ωleg_i    ……（１７）
そして、この式（１７）と、前記式（９）とから次式（１８）が得られる。
【０３３１】
Φｄ＝（↑ａT・Ｃ_i）・↑Ｖｂ＋（↑ａT・↑ｃ_i）    ……（１８）
従って、前記線形結合値Φの値の許容範囲に応じてΦｄの上限値や下限値を設定すれば、基体運動速度ベクトル↑Ｖｂの値を制限する線形行列不等式が得られることとなる。
【０３３２】
この場合、例えば、Φの値の許容範囲が、Φ≦Φmaxという範囲であるとした場合（Φmaxは、Φの上限値）、Φの値がΦmaxに近づくに伴い、Φｄの上限値がゼロまで変化するように、該Φｄの上限値Φd_maxを、最新値の目標歩容におけるΦの値に応じて設定すれば、Φの値がΦmaxを超えることがないように、↑Ｖｂの値を制限する次式（１９）の線形行列不等式が得られることとなる。
【０３３３】
（↑ａT・Ｃ_i）・↑Ｖｂ＋（（↑ａT・↑ｃ_i）−Φd_max）≧↑０    ……（１９）
なお、Φの値をある下限値以上の値に制限する場合でも、上記と同様に↑Ｖｂの値を制限する線形行列不等式を得ることができる。
【０３３４】
また、前記実施形態では、ヤコビアン行列Ａを算出するためのロボット１の起点の動作状態として、現在の目標歩容により規定される動作状態よりも将来の予測される動作状態寄りにずらした動作状態を用いるようにしたが、ヤコビアン行列Ａを算出するためのロボット１の起点の動作状態として、現在の目標歩容により規定される動作状態を用いるようにしてもよい。
【０３３５】
このようにしても、目標基体運動速度ベクトル↑Ｖb_cmd2を前記線形行列不等式により制限する実施形態によれば、各脚リンク３の各関節の関節動作制約条件を満たしつつ、ロボット１の目標歩容を生成することができる。
【０３３６】
なお、特に歩容生成部３２の演算処理周期を十分に短い時間に設定できる場合には、基体２の位置及び姿勢と、制御対象ベクトル↑Ｓの積分値との間の関係が非線形な関係であっても、現在の目標歩容により規定される動作状態を起点として算出したヤコビアン行列Ａを用いて、前記各実施形態と同様の処理を行うことで、前記目標制御対象ベクトル↑Ｓ_cmdに対応する適切な目標基体運動速度ベクトル↑Ｖb_cmd2を決定できる。
【０３３７】
また、前記実施形態では、擬似逆行列Ａ+を前記式（８ｂ）により算出するようにしたが、前記式（８ａ）により算出するようにしてもよい。
【０３３８】
また、前記実施形態におけるロボット１は、基体２に関節１２を備えるロボットであるが、基体２の下側基体１０と上側基体１１とが一体に構成されたものであってもよい。この場合には、基本目標基体運動速度ベクトル↑Ｖb_cmd1や、目標基体運動速度ベクトル↑Ｖb_cmd2は、関節１２の変位量の成分を除いた６成分のベクトルにより構成すればよい。なお、この場合には、前記式（８）の行列Ｂ_iの第３列は不要である。
【０３３９】
あるいは、下側基体１０と上側基体１１との間に２自由度以上の自由度を有する関節（２軸以上の軸周りの回転可能な関節）を備えるようにしてもよい。そして、この場合、基本目標基体運動速度ベクトル↑Ｖb_cmd1や、目標基体運動速度ベクトル↑Ｖb_cmd2を、下側基体１０の位置及び姿勢の時間的変化率の成分と、下側基体１０及び上側基体１１の間の関節の各軸周りの方向の変位量の時間的変化率の成分とにより構成してもよい。
【０３４０】
また、前記実施形態におけるロボット１の各脚リンク３は、６自由度のリンク機構であるが、７自由度以上の自由度を有するリンク機構であってもよい。この場合においては、前記線形行列不等式（１０ａ），（１０ｂ）又は（１９）の行列Ｃ_iを算出する際には、前記式（９ａ）の右辺の(Ｊleg_i)-1を、ヤコビアン行列Ｊleg_iの擬似逆行列（例えば特異点低感度運動分解行列（SR-Inverse）等の擬似逆行列）で置き換えた式によって、行列Ｃ_iを算出するようにすればよい。
【０３４１】
また、本発明の動作目標生成装置に関して言えば、対象とするロボットは前記ロボット１の如き脚式移動ロボットに限らず、例えば設置型の作業ロボットであってもよく、あるいは、移動のための手段として車輪もしくはクローラ等を備える移動型の作業ロボットであってもよい。
【０３４２】
さらに、例えば、ロボットの基体から延設された腕リンクの動作目標を生成するような場合にも本発明の動作目標生成装置を適用することができる。
【符号の説明】
【０３４３】
１…移動ロボット、２…基体（特定部位）、３…脚リンク（リンク機構）、１２，１９〜２４…関節、４１…目標脚位置姿勢決定部（目標脚位置姿勢決定手段）、４２…基本動作目標値決定部（基本動作目標値決定手段、目標制御対象ベクトル決定手段）、４３…基本目標並進運動量修正部（基本目標並進運動量修正手段、目標制御対象ベクトル決定手段）、４４…基本目標基体運動速度決定部（基本目標基体運動速度決定手段、目標制御対象ベクトル決定手段）、４５…基体運動速度目標値決定部（基体運動速度目標値決定手段、ヤコビアン行列決定手段）、４６…目標基体位置姿勢決定部（目標基体位置姿勢決定手段）、４７…目標歩容上重心位置算出部（目標歩容上重心位置算出手段）。
【図１】
0005661023.tif 000004
【図２】
0005661023.tif 000005
【図３】
0005661023.tif 000006
【図５】
0005661023.tif 000007
【図６】
0005661023.tif 000008
【図７】
0005661023.tif 000009
【図８】
0005661023.tif 000010
【図９】
0005661023.tif 000011
【図１０】
0005661023.tif 000012
【図１１】
0005661023.tif 000013
【図１２】
0005661023.tif 000014
【図４】
0005661023.tif 000015