以下、本発明の一実施形態を図1〜図6を参照して説明する。
図1は、本実施形態で例示する脚式移動ロボットの概略構成を模式的に示している。この脚式移動ロボット1(以下、単にロボット1という)は、基体2と、基体2から延設された左右一対(2つ)の脚リンク3R,3Lとを有する2足移動ロボットである。
なお、本明細書では、ロボット1の前方に向かって右側の部材又はその部材に関連する量を示す変数に符号“R”を付加し、ロボット1の前方に向かって左側の部材又はその部材に関連する量を示す変数に符号“L”を付加する。ただし、右側、左側を特に区別する必要が無いときは、符号“R”,“L”を省略することがある。
基体2は、ロボット1の上体に相当するリンク部分である。この基体2は、本実施形態では、下側基体10と、この下側基体10の上方に配置された上側基体11との2つの要素リンクから構成されている。下側基体10は、ロボット1の腰部に相当する部分、上側基体11は、ロボット1胸部に相当する部分である。そして、上側基体11は、ヨー軸周り(Z軸周り)の回転自由度を有する基体回旋用関節12を介して下側基体10に連結されている。
左右一対の脚リンク3R,3Lは、互いに同じ構造のものである。具体的には、各脚リンク3は、下側基体10に股関節部13を介して連結された大腿部14と、この大腿部14に膝関節部15を介して連結された下腿部16と、この下腿部16に足首関節部17を介して連結された足平部18とを、該脚リンク3を構成する複数の要素リンクとして備える。この場合、各脚リンク3の先端部は、足平部18により構成される。
そして、各脚リンク3の股関節部13は、ヨー軸周り、ピッチ軸周り(Y軸周り)及びロール軸周り(X軸周り)の回転自由度をそれぞれ有する3つの関節19,20,21により構成されている。また、膝関節部15は、ピッチ軸まわりの回転自由度を有する関節22により構成されている。また、足首関節部17は、ピッチ軸周り及びロール軸周りの回転自由度をそれぞれ有する2つの関節23,24により構成されている。
従って、各脚リンク3の先端部である足平部18は、本実施形態では、下側基体10に対して、6自由度の運動自由度を有する。なお、上記の説明における各脚リンク3の関節19〜24の回転軸(ロール軸、ピッチ軸、ヨー軸)は、脚リンク3を上下方向に延在させた状態での回転軸を示している。
以上が、本実施形態のロボット1の基本構造である。かかる構成のロボット1は、各脚リンク3の6個の関節19〜24を駆動することによって、各脚リンク3の空間的な運動が行なわれ、この運動によって床上を移動することが可能となっている。例えば、人の歩行動作あるいは走行動作と同様の形態(歩容)で脚リンク3R,3Lを運動させることによって、ロボット1の歩行動作あるいは走行動作を行なうことが可能である。
また、本実施形態では、基体2を構成する下側基体10と、上側基体11とが関節12を介して連結されているので、この関節12を駆動することによって、基体2をその体幹軸に相当する関節12の回転軸周りに捩じることが可能となっている。
補足すると、ロボット1は、上記基体2、及び脚リンク3R,3Lだけでなく、例えば上側基体11の両側部から延設された腕リンクや、該上側基体11の上端部に搭載した頭部等を備えていてもよい。また、基体2の関節12を省略し、下側基体10及び上側基体11を一体に構成してもよい。
図1での図示は省略したが、ロボット1には、図2に示すように、上記の各関節をそれぞれ回転駆動する関節アクチュエータ30と、ロボット1の動作制御を行なう制御処理ユニット31とが搭載されている。
関節アクチュエータ30は、例えば電動モータあるいは油圧アクチュエータにより構成されたものであり、各関節毎に備えられている。この場合、各関節アクチュエータ30による各関節の駆動機構は公知の構造のものでよい。また、関節アクチュエータ30は、回転型のアクチュエータに限らず、直動型のアクチュエータであってもよい。
制御処理ユニット31は、CPU、RAM、ROM、インターフェース回路等を含む電子回路ユニットである。この制御処理ユニット31は、実装されるプログラム等により実現される主要な機能として、ロボット1の目標歩容を逐次生成する歩容生成部32と、該目標歩容に応じてロボット1の各関節の変位量(回転角)を関節アクチュエータ30を介して制御する関節アクチュエータ制御部33とを備える。
歩容生成部32は、本発明の装置としての機能を制御処理ユニット31に付与するものである。この歩容生成部32は、ロボット1の目標歩容の構成要素として、各脚リンク3の先端部を構成する足平部18の空間的な位置及び姿勢の目標値である目標脚位置姿勢と、基体2の空間的な位置及び姿勢の目標値である目標基***置姿勢とを逐次決定する処理を後述する如く実行する。
この場合、本実施形態では、基体2は関節12を備えるため、上側基体11を下側基体10に対して関節12の回転軸周りに捩じることが可能となっている。このため、本実施形態における目標基***置姿勢は、より詳しくは、下側基体10の空間的な位置及び姿勢の目標値と、上側基体11の下側基体10に対する捩じり角(関節12の変位量)の目標値とから構成される。
また、各脚リンク3の足平部18の位置は、より詳しくは、該足平部18のあらかじめ定めた代表点の位置であり、該足平部18の姿勢は、該足平部18の空間的な向きを意味する。このことは、下側基体10の位置及び姿勢についても同様である。
これらの位置及び姿勢は、ロボット1の動作環境の床面に対して固定された慣性座標系であるグローバル座標系で見た位置及び姿勢として記述される。このグローバル座標系は、任意に設計的に定めたものでよい。例えばロボット1の両脚リンク3R,3Lのうちの支持脚(遊脚としての一方の脚リンク3の足平部18を空中で動かすときにロボット1の自重を支えることとなる他方の脚リンク3)の足平部18の接地時における該足平部18の直下(もしくはその近辺)の床面上に原点を有し、且つ、該足平部18の前後方向の水平軸及び左右方向の水平軸をそれぞれX軸、Y軸とし、鉛直軸をZ軸とする3軸直交座標系(所謂、支持脚座標系)をグローバル座標系として用いることができる。
本実施形態では、便宜上、グローバル座標系として上記支持脚座標系が用いられるものとする。そして、特にことわらない限り、X軸、Y軸、Z軸は、上記支持脚座標系の座標軸を意味するものとする。
補足すると、基体2を構成する下側基体10及び上側基体11の一方の基体10又は11の空間的な位置及び姿勢と、関節12の変位量とが決まれば、他方の基体11又は10の空間的な位置及び姿勢も一義的に決まる。従って、目標基***置姿勢は、上側基体11の空間的な位置及び姿勢の目標値と、関節12の変位量の目標値との組により構成されていてもよい。
関節アクチュエータ制御部33は、各関節の実際の変位量を、歩容生成部32により決定された目標歩容により規定されるロボット1の各関節の変位量の目標値に追従させるように、各関節に対応する関節アクチュエータ30を制御する。
この場合、各関節の実際の変位量(回転角)が、図示しないロータリエンコーダ等の変位量センサにより検出される。そして、関節アクチュエータ制御部33は、各関節の実際の変位量の検出値と目標値との偏差に応じて、フィードバック制御により関節アクチュエータ30の駆動力を制御する。
以降、制御処理ユニット31の歩容生成部32の処理を詳細に説明する。
図3に示すように、歩容生成部32は、目標歩容を生成するための主要な処理部として、目標脚位置姿勢を逐次決定する目標脚位置姿勢決定部41と、各脚リンク3の足平部18の位置及び姿勢以外のロボット1の所定種類の状態量に関する基本の動作目標を逐次決定する基本動作目標値決定部42とを備える。
この場合、基本動作目標値決定部42が決定する動作目標は、ロボット1の全体重心の位置の基本の目標値である基本目標重心位置↑Xg_cmd1と、ロボット1の全体の並進運動量ベクトル(=全体重心の並進運動量ベクトル)の基本の目標値である基本目標並進運動量ベクトル↑P_cmd1と、ロボット1の全体重心周りの全体の角運動量ベクトルの基本の目標値である基本目標角運動量ベクトル↑L_cmd1と、基体2の位置及び姿勢の基本の目標値である基本目標基***置姿勢とから構成される。
なお、基本目標重心位置↑Xg_cmd1、基本目標並進運動量ベクトル↑P_cmd1、基本目標角運動量ベクトル↑L_cmd1は、それぞれ、グローバル座標系の各座標成分を構成要素とする3成分のベクトル(縦ベクトル)である。本実施形態の説明では、“↑”は、ベクトル(縦ベクトル)を表記する記号として用いる。
また、歩容生成部32は、基本動作目標値決定部42により決定する基本の動作目標のうちの基本目標並進運動量ベクトル↑P_cmd1を修正してなる修正後基本目標並進運動量ベクトル↑P_cmd2を逐次決定する基本目標並進運動量修正部43と、基体2の位置及び姿勢の単位時間当たりの変化量(すなわち変化速度)の基本の目標値である基本目標基体運動速度ベクトル↑Vb_cmd1を逐次決定する基本目標基体運動速度決定部44と、修正後基本目標並進運動量ベクトル↑P_cmd2、基本目標角運動量ベクトル↑L_cmd1及び基本目標基体運動速度ベクトル↑Vb_cmd1を目標として、基体2の位置及び姿勢の単位時間当たりの変化量の目標値である目標基体運動速度ベクトル↑Vb_cmd2を逐次決定する基体運動速度目標値決定部45とを備える。
上記基本目標基体運動速度ベクトル↑Vb_cmd1及び目標基体運動速度ベクトル↑Vb_cmd2は、それぞれ、グローバル座標系で見た基体2の代表点(本実施形態では下側基体10の代表点)の並進移動速度の各座標軸成分と、グローバル座標系で見た基体2の姿勢(本実施形態では下側基体10の姿勢)の各座標軸周りの角速度成分と、基体2の関節12の変位量の時間的変化率(角速度)とを並べた7成分のベクトル(縦ベクトル)である。
また、目標基体運動速度ベクトル↑Vb_cmd2は、換言すれば、歩容生成部32が逐次決定する目標歩容における目標基***置姿勢の時間的変化率である。
歩容生成部32はさらに、基体運動速度目標値決定部45により逐次決定される目標基体運動速度ベクトル↑Vb_cmd2を積分する(↑Vb_cmd2の各成分を積分する)ことにより目標基***置姿勢を逐次決定する目標基***置姿勢決定部46と、目標基***置姿勢及び両脚リンク3R,3Lの目標脚位置姿勢を構成要素とする目標歩容により規定されるロボット1の全体重心の位置である目標歩容上重心位置↑Xg_cmd2を逐次算出する目標歩容上重心位置算出部47とを備える。
上記目標歩容上重心位置↑Xg_cmd2は、基本目標重心位置↑Xg_cmd1と同様に、グローバル座標系の各座標成分を構成要素とする3成分のベクトル(縦ベクトル)である。
本実施形態では、歩容生成部32はさらに、目標歩容を構成する目標基***置姿勢及び両脚リンク3R,3Lの目標脚位置姿勢を用いて、ロボット1の各関節の変位量の目標値である目標関節変位量を逐次決定する目標関節変位量決定部48を備えている。ただし、この目標関節変位量決定部48は、前記関節アクチュエータ制御部33に備えるようにしてもよい。
歩容生成部32は、以上の各処理部41〜48の処理を所定の演算処理周期で逐次実行することにより、目標歩容の構成要素である目標基***置姿勢と、両脚リンク3R,3Lの目標脚位置姿勢とを逐次決定すると共に、該目標歩容に対応する各関節の目標関節変位量を逐次決定する。
以下に、歩容生成部32の各処理部41〜48の処理の詳細を含めて、歩容生成部32の処理の全体を説明する。なお、本実施形態では、主に、ロボット1の歩行動作又は走行動作による移動を行なうための目標歩容を生成する場合を一例として歩容生成部32の処理を説明する。
歩容生成部32は、各演算処理周期において、まず、目標脚位置姿勢決定部41及び基本動作目標値決定部42の処理を実行する。
本実施形態では、目標脚位置姿勢決定部41及び基本動作目標値決定部42には、脚リンク3R,3Lのうちの遊脚の着地が行なわれる毎に、次回の遊脚を含めて複数歩分の遊脚の足平部18の目標とする着地位置及び着地姿勢である目標着地位置姿勢と、目標とする着地時刻である目標着地時刻とが与えられるようになっている。なお、目標着地時刻の代わりに、ロボット1の1歩分の期間(歩容周期)の目標時間を用いててもよい。
上記目標着地位置姿勢及び目標着地時刻は、ロボット1の移動計画、あるいは、操縦装置による移動指令に基づいて決定される。その決定処理は、制御処理ユニット31で行なうようにしてもよいことはもちろんであるが、ロボット1の外部のサーバで行うようにしてもよい。
そして、目標脚位置姿勢決定部41は、与えられた目標着地位置姿勢及び目標着地時刻に応じて、各脚リンク3の足平部18の離床直前の接地状態での位置及び姿勢から次の目標着地位置姿勢に至る位置及び姿勢の目標軌道(足平部18の位置及び姿勢の目標値の時系列パターン)を決定し、その目標軌道の瞬時値として各脚リンク3の目標脚位置姿勢を逐次決定する。
この場合、各脚リンク3の足平部18の位置及び姿勢の目標軌道(目標脚位置姿勢の軌道)は、該脚リンク3が遊脚となる期間で、例えば図4に例示するようなパターンで該足平部18の目標位置及び目標姿勢が連続的に変化しつつ、該足平部18が空中を移動するように決定される。この目標軌道の始端の足平部18の位置及び姿勢は、離床前の着地動作時の目標着地位置姿勢に相当し、該目標軌道の終端の足平部18の位置及び姿勢は、次の目標着地位置姿勢に相当する。
なお、各脚リンク3の足平部18の接地中においては、該脚リンク3の位置及び姿勢の目標軌道は、該脚リンク3の足平部18の位置及び姿勢を一定もしくはほぼ一定に保つように決定してもよいことはもちろんであるが、該足平部18の接地箇所が、該足平部18の踵寄りの箇所から爪先寄りの箇所に移行していくように決定してもよい。
また、各脚リンク3の足平部18の空中での姿勢の軌道は、該足平部18の姿勢を一定もしくはほぼ一定に保つように決定してもよい。
基本動作目標値決定部42は、与えられた目標着地位置姿勢及び目標着地時刻に応じて(あるいは、各脚リンク3の足平部18の接地中の目標脚位置姿勢の軌道に応じて)、ZMP(Zero Moment Point)の目標位置である目標ZMPの軌道を決定する。
この場合、目標ZMPの軌道は、目標ZMPが所謂、支持多角形内でその境界に近づき過ぎずないような領域(ZMPの存在許容領域)に位置し、且つ、不連続的な変化を生じないように設定される。このような目標ZMPの軌道の設定手法としては、例えば特許第3674789号にて本願出願人が説明した手法等、公知の手法を用いることができる。
そして、基本動作目標値決定部42は、目標脚位置姿勢決定部41により決定された目標脚位置姿勢の時系列と、ロボット1の動力学(ロボット1に作用する外力(ここでは床反力)とロボット1の運動との関係)を表すものとしてあらかじめ定められた動力学モデルとを用いて、該動力学モデル上で目標ZMPを満足し得る基体2の位置及び姿勢が前記基本目標基***置姿勢として逐次決定される。
ここで、目標ZMPを満足し得る基体2の位置及び姿勢というのは、上記動力学モデルにおいて、該基体2の位置及び姿勢の時系列と、両脚リンク3R,3Lの目標脚位置姿勢の時系列とにより規定されるロボット1の全体の運動によって発生する慣性力と該ロボット1に作用する重力との合力によって、目標ZMP周りに発生するモーメントの水平軸周り成分(X軸周り成分及びY軸周り成分)がゼロになるような、基体2の位置及び姿勢を意味する。
上記動力学モデルとしては、目標ZMPを満足し得る基体2の位置及び姿勢を比較的容易に算出し得るように、線形性の高い動力学モデルが使用される。例えば、特許第3674789号にて本願出願人が説明した動力学モデル(特許第3674789号の図10に示す動力学モデル)が、基本動作目標値決定部42の処理における動力学モデルとして採用することができる。
該動力学モデルは、図5に示すように、各脚リンク3R,3Lと基体2(ロボット1の上体)とにそれぞれ対応する3つの質点3mR,3mL,2mと、基体2の姿勢(詳しくは、下側基体10のX軸周り及びY軸周りの姿勢)の変化に伴う慣性力モーメントを発生するフライホイールFH(質量を持たずにイナーシャを有するフライホイール)とを備える動力学モデルである。そして、この場合、基本目標基***置姿勢は、特許第3674789号にて本願出願人が説明した目標上***置姿勢の決定手法と同様の手法によって、決定することができる。
このようにすることにより、上記動力学モデル上において、ZMPが支持多角形内に収まり、また、ロボット1に作用する床反力のうちの摩擦力成分が適切な許容範囲内に収まる、というような所要の動力学的な制約条件が満たされるように、基本目標基***置姿勢を決定できることとなる。
なお、本実施形態では、基本目標基***置姿勢のうち、基体2の関節12の変位量の目標値は、例えば、上側基体11が関節12の回転軸周りに回転することで目標ZMP周りに発生する鉛直軸周りのモーメントが、各質点3mR,3mL,2mの並進運動によって目標ZMP周りに発生する鉛直軸周りのモーメントを打ち消すように決定される。
ただし、関節12の変位量の目標値は、例えば一定値であってもよく、あるいは、一定のパターンで周期的に(脚リンク3R,3Lの運動に同期した周期で)変化するように設定されていてもよい。
本実施形態では、基本動作目標値決定部42は、さらに、上記の如く決定した基本目標基***置姿勢の時系列と、目標脚位置姿勢決定部41により決定された目標脚位置姿勢の時系列とから、上記動力学モデルを用いて、該動力学モデル上でのロボット1の全体重心の位置と、該全体重心の並進運動量ベクトルと、該全体重心周りの角運動量ベクトルとを、それぞれ、前記基本目標重心位置↑Xg_cmd1、基本目標並進運動量ベクトル↑P_cmd1、基本目標角運動量ベクトル↑L_cmd1として算出する。
この場合、基本目標重心位置↑Xg_cmd1は、各演算処理周期における脚リンク3R,3Lのそれぞれの目標脚位置姿勢と基本目標基***置姿勢とにより規定される質点3mR,3mL,2mの全体の重心の位置(位置ベクトル)として算出される。すなわち、脚リンク3R,3Lのそれぞれの目標脚位置姿勢と基本目標基***置姿勢とにより規定される各質点3mR,3mL,2mの位置ベクトルに、各質点3mR,3mL,2mの質量を乗じてなるベクトルの総和のベクトルを、ロボット1の全体の質量(=質点3mR,3mL,2mの総和の質量)により除算してなるベクトルが、基本目標重心位置↑Xg_cmd1として算出される。
また、基本目標並進運動量ベクトル↑P_cmd1は、脚リンク3R,3Lのそれぞれの目標脚位置姿勢の時系列と基本目標基***置姿勢の時系列とにより規定される質点3mR,3mL,2mの全体の重心の移動速度ベクトル(=↑Xg_cmd1の時間的変化率)にロボット1の全体の質量を乗じることにより算出される。すなわち、質点3mR,3mL,2mの全体の重心の並進運動量ベクトルが、基本目標並進運動量ベクトル↑P_cmd1として算出される。
また、基本目標角運動量ベクトル↑L_cmd1は、脚リンク3R,3Lのそれぞれの目標脚位置姿勢の時系列により規定される質点3mR,3mLのそれぞれの並進運動量ベクトルが、質点3mR,3mL,2mの全体の重心の周りに発生する角運動量ベクトルと、基本目標基***置姿勢の時系列により規定される質点2mの並進運動量ベクトルが、質点3mR,3mL,2mの全体の重心の周りに発生する角運動量ベクトルと、基本目標基***置姿勢のうちの基体2の姿勢の時系列により規定されるフライホイールFHの回転運動によって発生する角運動量ベクトルと、下側基体10に対する上側基体11の回転運動(関節12の回転軸周りの回転運動)によって発生する角運動量ベクトルとの総和の角運動量ベクトルとして算出される。
補足すると、ロボット1が腕リンク等、基体2に対して可動な他のリンクを備える場合には、基本動作目標値決定部42は、当該他のリンクの各関節の変位量を規定する目標値(例えば、当該他のリンクの先端部の目標位置及び目標姿勢、あるいは、当該他のリンクの先端部の各関節の変位量の目標値等)も決定する。
この場合、ロボット1の歩行動作又は走行動作を行なう目標歩容の生成処理では、当該他のリンク(例えば腕リンク)の各関節の変位量を規定する目標値は、例えば、基体2のうちの上側基体11に対する当該他のリンクの各部の相対的な位置及び姿勢を定常的に一定に保持するような目標値でよい。このようにした場合には、上記動力学モデルにおける質点2mの質量や、フライホイールFHのイナーシャ(慣性モーメント)は、当該他のリンクの影響分を含めて設定しておけばよい。
以上のように目標脚位置姿勢決定部41及び基本動作目標値決定部42の処理を実行した後、次に、歩容生成部32は、目標歩容上重心位置算出部47の処理と、前記基本目標並進運動量修正部43及び基本目標基体運動速度決定部44の処理とを実行する。
目標歩容上重心位置算出部47には、歩容生成部32が前回の演算処理周期で最終的に決定した目標歩容の構成要素(目標脚位置姿勢及び目標基***置姿勢)、換言すれば、歩容生成部32が既に決定した目標歩容のうちの最新の(現在の)目標歩容の構成要素が与えられる。
そして、目標歩容上重心位置算出部47は、与えられた目標歩容の構成要素により規定されるロボット1の全体重心の位置(グローバル座標系で見た位置ベクトル)を、ロボット1の幾何学モデル(剛体リンクモデル)に基づく幾何学演算によって算出し、この算出した位置を目標歩容上重心位置↑Xg_cmd2として決定する。
さらに詳細には、上記幾何学演算に用いるロボット1の幾何学モデルは、例えばロボット1の各要素リンクの重心位置に質点(該要素リンクの質量を有する質点)が設定されたモデルである。
そして、目標歩容上重心位置算出部47は、まず、与えられた最新の目標歩容から、該目標歩容により規定されるロボット1の各関節の変位量を逆運動学演算の処理により算出する。
次いで、目標歩容上重心位置算出部47は、この算出した各関節の変位量と、与えられた最新の目標歩容の目標脚位置姿勢又は目標基***置姿勢とにより規定される上記幾何学モデルの各質点の位置を算出する。
そして、目標歩容上重心位置算出部47は、このように算出した各質点の位置と、該質点の質量とから、該幾何学モデルの全ての質点の質量中心の位置としてのロボット1の全体重心の位置を算出する。このように算出されたロボット1の全体重心の位置が、上記目標歩容上重心位置↑Xg_cmd2として決定される。
なお、歩容生成部32の前回の演算処理周期で最終的に決定された最新の目標歩容に対応する目標歩容上重心位置↑Xg_cmd2を目標歩容上重心位置算出部47により上記の如く決定する処理は、前回の演算処理周期で行なうようにしてもよい。
補足すると、ロボット1が腕リンク等、基体2に対して可動な他のリンクを備える場合には、当該他のリンクの質量を考慮した幾何学モデルを用いて上記目標歩容上重心位置↑Xg_cmd2を決定すればよい。
この場合には、目標歩容上重心位置算出部47には、目標脚位置姿勢及び目標基***置姿勢に加えて、当該他のリンクの各関節の変位量を規定する目標値(例えば、当該他のリンクの先端部の目標位置及び目標姿勢、あるいは、当該他のリンクの先端部の各関節の変位量の目標値等)も与えられる。
そして、ロボット1の全体重心の位置としての目標歩容上重心位置↑Xg_cmd2は、当該他のリンクに対応する質点を含めた幾何学モデルに基づく幾何学演算によって、算出するようにすればよい。
ただし、上記他のリンクの質量が基体2の質量に比して十分に微小である場合には、当該他のリンクの質量を無視して構成した幾何学モデル、あるいは、当該他のリンクの質量を基体2の質量に含めて構成した幾何学モデルを用いて目標歩容上重心位置↑Xg_cmd2を決定するようにしてもよい。
また、例えば、基体2の上側基体11から腕リンクが延設されている場合において、移動ロボット1の歩行動作又は走行動作における上側基体11に対する腕リンクの姿勢を一定もしくはほぼ一定に保持するような場合には、該腕リンクが実質的に上側基体11と一体の部分となる。このため、この場合には、当該他のリンクの質量を上側基体11の質量に含めて構成した幾何学モデルを用いて目標歩容上重心位置↑Xg_cmd2を決定するようにすればよい。
基本目標並進運動量修正部43には、基本動作目標値決定部42から基本目標重心位置↑Xg_cmd1と基本目標並進運動量ベクトル↑P_cmd1とが逐次与えられると共に、目標歩容上重心位置算出部47から目標歩容上重心位置↑Xg_cmd2が逐次与えられる。
なお、以降の説明では、歩容生成部32の処理で決定される↑Xg_cmd1、↑P_cmd1、↑Xg_cmd2等の各変数の値に関し、歩容生成部32の現在の(今回の)演算処理周期で決定される値と、前回の演算処理周期で決定された値とを区別する必要がある場合に、前者の値を今回値、後者の値を前回値ということがある。
基本目標並進運動量修正部43は、まず、与えられた基本目標重心位置↑Xg_cmd1と、目標歩容上重心位置↑Xg_cmd2との偏差(=↑Xg_cmd1−↑Xg_cmd2)を演算部43aで算出する。
この場合、基本目標重心位置↑Xg_cmd1の今回値に対応する目標歩容の今回値は未だ決定されていない。そして、本実施形態では、目標歩容上重心位置↑Xg_cmd2の今回値は、目標歩容の前回値に対応するロボット1の全体重心の位置である。このため、演算部43aの処理では、基本目標重心位置↑Xg_cmd1の前回値と、目標歩容上重心位置↑Xg_cmd2の今回値と用いて上記偏差(以降、重心位置偏差という)が算出される。
さらに、基本目標並進運動量修正部43は、演算部43aで算出した重心位置偏差偏差(=↑Xg_cmd1−↑Xg_cmd2)にあらかじめ定めた所定値のゲインKgを乗じる処理を演算部43bで実行する。これにより、上記偏差をゼロに近づけるように基本目標並進運動量ベクトル↑P_cmd1を補正するための操作量(フィードバック操作量)が算出される。
次いで、基本目標並進運動量修正部43は、基本目標並進運動量ベクトル↑P_cmd1の今回値を、演算部43bで算出した操作量によって補正する処理を演算部43cで実行する。この補正処理は、↑P_cmd1の今回値に演算部43bで算出した操作量を加算することにより行なわれる。これにより、演算部43cの出力として、修正後基本並進運動量ベクトル↑P_cmd2(今回値)が算出される。
従って、各演算処理周期において、修正後基本並進運動量ベクトル↑P_cmd2(今回値)は、次式(1)により基本目標並進運動量ベクトル↑P_cmd1の今回値を修正することで算出される。
↑P_cmd2=↑P_cmd1+Kg・(↑Xg_cmd1−↑Xg_cmd2) ……(1)
すなわち、↑P_cmd2は、重心位置偏差(↑Xg_cmd1−↑Xg_cmd2)をフィードバック制御則としての比例則によりゼロに近づけるように決定したフィードバック操作量によって↑P_cmd1を補正することにより決定される。
なお、上記ゲインKgはスカラー及び対角行列(ゲイン行列)のいずれであってもよい。ゲインKgを対角行列とした場合には、重心位置偏差の各成分(X軸、Y軸、Z軸の各座標軸成分)毎にフィードバックゲインを設定することが可能である。
基本目標基体運動速度決定部44には、基本動作目標値決定部42から基本目標基***置姿勢が逐次与えられると共に、目標基***置姿勢決定部46(詳細は後述する)から目標基***置姿勢が逐次与えられる。
そして、基本目標基体運動速度決定部44は、まず、与えられた基本目標基***置姿勢と、目標基***置姿勢との偏差(=基本目標基***置姿勢−目標基***置姿勢)を演算部44aで算出する。
この場合、基本目標基***置姿勢の今回値に対応する目標基***置姿勢は未だ決定されていない。このため、演算部44aの処理では、基本目標基***置姿勢の前回値と、目標基***置姿勢の前回値と用いて上記偏差(以降、基***置姿勢偏差という)が算出される。
さらに、基本目標基体運動速度決定部44は、演算部44aで算出した基***置姿勢偏差にあらかじめ定めた所定値のゲインKbを乗じる処理を演算部44bで実行する。これにより、上記基***置姿勢偏差をゼロに近づけて、目標基***置姿勢が基本目標基***置姿勢から乖離しないようにするための操作量(フィードバック操作量)として、基体2の位置及び姿勢の単位時間当たりの変化量の目標値により構成される基本目標基体運動速度ベクトル↑Vb_cmd1が算出される。
従って、各演算処理周期において、基本目標基体運動速度ベクトル↑Vb_cmd1は、次式(2)により算出される。
↑Vb_cmd1=Kb・(基本目標基***置姿勢−目標基***置姿勢) ……(2)
すなわち、↑Vb_cmd1は、基***置姿勢偏差(=基本目標基***置姿勢−目標基***置姿勢)をフィードバック制御則としての比例則によりゼロに近づけるように決定したフィードバック操作量として決定される。
なお、上記ゲインKbはスカラー及び対角行列(ゲイン行列)のいずれであってもよい。ゲインKbを対角行列とした場合には、基***置姿勢偏差の各成分(基体2の各座標軸方向の位置の偏差、基体2の各座標軸周りの姿勢の偏差(角度偏差)、及び基体2の関節12の変位量の偏差(角度偏差))毎にフィードバックゲインを設定することが可能である。
以上のように目標歩容上重心位置算出部47、基本目標並進運動量修正部43及び基本目標基体運動速度決定部44の処理を実行した後、歩容生成部32は、次に基体運動速度目標値決定部45の処理を実行する。
この基体運動速度目標値決定部45は、基体2の位置及び姿勢の単位時間当たりの変化量を成分として構成される基体運動速度ベクトル↑Vbから、ロボット1の全体重心の並進運動量ベクトル↑Pの各成分と、ロボット1の全体重心の周りの角運動量ベクトル↑Lの各成分と、基体運動速度ベクトル↑Vbの各成分とにより構成されるベクトルである制御対象ベクトル↑Sへの線形写像を表すヤコビアン行列を用いて基体運動速度ベクトル↑Vbの目標値である目標基体運動速度ベクトル↑Vb_cmd2を決定する。
上記制御対象ベクトル↑Sは、↑P、↑L、↑Vbの各成分を並べた縦ベクトルである。より具体的には、↑P、↑L、↑Vbをそれぞれの成分を用いて表記したものを、↑P≡[Px,Py,Pz]T、↑L≡[Lx,Ly,Lz]T、↑Vb≡[Vbx,Vby,Vbz,ωbx,ωby,ωbz,ωbt]Tとおくと(上付き添え字Tは転置を表す)、↑Sは、例えば次式(3)により表記される縦ベクトルである。
↑S≡[↑P,↑L,↑Vb]T
≡[Px,Py,Pz,Lx,Ly,Lz,Vbx,Vby,Vbz,ωbx,ωby,ωbz,ωbt]T
……(3)
なお、Px,Py,Pzは、それぞれ↑PのX軸方向成分、Y軸方向成分、Z軸方向成分であり、Lx,Ly,Lzは、それぞれ↑LのX軸周り成分、Y軸周り成分、Z軸周り成分である。また、Vbx、Vby、Vbzは、それぞれ、基体2の代表点の移動速度のうちのX軸方向成分、Y軸方向成分、Z軸方向成分、ωbx、ωby、ωbzは、それぞれ、基体2の姿勢の時間的変化率のうちのX軸周りの角速度成分、Y軸周りの角速度成分、Z軸周りの角速度成分、ωbtは、基体2の関節12の変位量の時間的変化率(角速度)である。
また、↑Vbから↑Sへの線形写像を表すヤコビアン行列をAとおくと、その線形写像は、次式(4)により表される。
↑S=A・↑Vb ……(4)
この場合、ヤコビアン行列Aは、次式(5)で示すように、↑Vbから↑P、↑Lのそれぞれへの線形写像を表す慣性行列M,Hと、↑Vbから↑Vbへの線形写像を表す単位行列E(↑Vbの成分数と同次数の単位行列)とを縦に並べた行列となる。なお、本実施形態では、↑Vbは7成分のベクトルであるので、M,Hはそれぞれ3行7列の慣性行列、Eは7行7列の単位行列である。従って、本実施形態では、ヤコビアン行列Aは、13行7列の行列である。
かかるヤコビアン行列Aを用いる基体運動速度目標値決定部45の処理の全体は、次のように行なわれる。
すなわち、基体運動速度目標値決定部45は、各演算処理周期において、まず、式(4)の線形写像を表すヤコビアン行列Aを決定する。さらに、基体運動速度目標値決定部45は、算出したヤコビアン行列Aの擬似逆行列A+を決定する。
そして、基体運動速度目標値決定部45は、基本目標並進運動量修正部43により決定された修正後基本並進運動量ベクトル↑P_cmd2(今回値)と、基本動作目標値決定部42により決定された基本目標角運動量ベクトル↑L_cmd1(今回値)と、基本目標基体運動速度決定部44により決定された基本目標基体運動速度ベクトル↑Vb_cmd1(今回値)とをそれぞれ、前記式(3)の↑P、↑L、↑Vbの目標値とすることで構成される制御対象ベクトル↑Sの目標値↑S_cmd(以下、目標制御対象ベクトル↑S_cmdという)と、上記擬似逆行列A+とから次式(6)の右辺の演算(↑S_cmdにA+を乗じる演算処理)を行なうことで、↑S_cmdに対応する↑Vbの目標値としての目標基体運動速度ベクトル↑Vb_cmd2を決定する。
↑Vb_cmd2=A+・↑S_cmd ……(6)
このように目標基体運動速度ベクトル↑Vb_cmd2を決定する処理において、基体運動速度目標値決定部45は、本実施形態ではヤコビアン行列Aを次のように決定する。
まず、本実施形態におけるヤコビアン行列Aの決定手法に関する前提事項を説明しておく。前記式(4)の線形写像を表すヤコビアン行列Aは、その構成要素である慣性行列M、Hと単位行列EとのうちのM、Hを変数とするヤコビアン行列である。
かかるヤコビアン行列Aは、基本的には、ロボット1の現在の目標歩容(前回の演算処理周期で決定した目標歩容)により規定されるロボット1の動作状態を起点として、当該起点の動作状態から基体2の位置及び姿勢の各成分を微小変化させた場合にそれに起因して発生するロボット1の全体重心の並進運動量ベクトル↑P及び該全体重心周りの角運動量ベクトル↑Lの発生感度を表す行列として算出すればよい。
この場合、上記起点の動作状態から基体2の位置及び姿勢の各成分を微小変化させた場合にそれに起因して発生するロボット1の全体重心の並進運動量ベクトル↑P及び該全体重心周りの角運動量ベクトル↑Lは、例えばロボット1の各リンク毎にその重心位置の質点と該重心位置周りのイナーシャ(慣性モーメント)とを設定した動力学モデル(以降、このような動力学モデルをフルモデルという)を用いて算出することができる。その算出手法は、公知の手法を採用することができる。
ただし、本実施形態では、目標制御対象ベクトル↑S_cmdに応じて決定する目標基体運動速度ベクトル↑Vb_cmd2の信頼性をより一層高めるために、現在の時刻(今回の演算処理周期の時刻)から次回の演算処理周期の時刻との間の期間内(1演算処理周期の時間内)での将来の目標歩容(目標脚位置姿勢及び目標基***置姿勢)の瞬時値を推定し、その推定した将来の目標歩容の瞬時値により規定されるロボット1の動作状態を起点としてヤコビアン行列Aを決定することとした。
換言すれば、各演算処理周期において、ヤコビアン行列Aを算出するための起点とするロボット1の動作状態を、現在の目標歩容により規定される動作状態から、将来の予想される動作状態寄りにずらした上で、ヤコビアン行列Aを算出する。
これは次の理由による。すなわち、基体2の位置及び姿勢の微小変化と、それに起因して発生する↑P及び↑Lとの間の関係は一般には非線形性を有するため、ロボット1の現在の目標歩容の状態を起点として決定したヤコビアン行列Aを用いた場合には、上記式(6)により決定される↑Vb_cmd2が、↑S_cmdを実現するための最適な値からずれを生じやすい。
例えば、脚リンク3R,3Lの足平部18R,18Lの位置及び姿勢を一定に保持した場合における基体2の位置Xbと、ロボット1の全体重心の位置Xgにロボット1の全体質量Mallを乗算したもの(=Mall・Xg)との間の関係に着目すると、該関係は、一般には、図6に実線の曲線a1で例示する如く、非線形な関係となる。
なお、ここでは、Xb、Xgは、理解の便宜上、グローバル座標系のX軸方向又はY軸方向又はZ軸方向のいずれかの一軸方向の位置、例えばX軸方向の位置とする。この場合、Mall・Xgは、ロボット1の全体重心のX軸方向の並進運動量の積分値に相当する状態量である。
そして、現在の目標歩容における基体2の現在の位置が、例えば図6中に示すXb(k)であるとした場合、現在の目標歩容の状態を起点として決定されるヤコビアン行列A(以降、これを基本ヤコビアン行列A1という)では、Xbの微小変化と、これに対応するMall・Xgの変化との間の関係は、Xb(k)の位置での曲線a1の接線a2により示される線形関係として表されることとなる。
従って、目標制御対象ベクトル↑S_cmdに対応する目標基体運動速度ベクトル↑Vb_cmd2を、上記基本ヤコビアン行列A1の擬似逆行列を用いて前記式(6)により決定した場合には、歩容生成部32の演算処理の1周期(1演算処理周期)Δtに時間における基体2の位置Xbの目標とする変化量は、図6に示すΔXb_aとなる。
このΔXb_aは、上記の如く基本ヤコビアン行列A1の擬似逆行列を用いて決定した↑Vb_cmd2のうちの、位置Xbの時間的変化率に相当する成分Vbxの値にΔtを乗じてなる値である。換言すれば、ΔXb_aは、基体2の位置Xbと、Mall・Xgとの間の関係が、上記接線a2で表される関係であると仮定した場合に、ΔXb_aに対応するMall・Xgの変化量が、目標制御対象ベクトル↑S_cmdの構成要素である↑P_cmd2のうちのX軸方向の並進運動量の目標値Px_cmd2にΔtを乗じてなる値(=Px_cmd2・Δt)となるようなXbの変化量である。
ただし、基体2の位置Xbと、Mall・Xgとの間の実際の関係が、曲線a1で表される関係である場合には、ΔXb_aに対応するMall・Xgの実際の変化量は、図6中のMall・ΔXg_aとなる。この値Mall・ΔXg_aは、一般には、目標制御対象ベクトル↑S_cmdにより規定されるMall・Xgの変化量の目標値としての上記の値Px_cmd2・Δtに対して誤差を生じるものとなる。
そして、基体2の位置Xbと、Mall・Xgとの間の実際の関係が、曲線a1で表される関係である場合には、Mall・Xgの変化量が、目標制御対象ベクトル↑S_cmdにより規定されるMall・Xgの変化量の目標値Px_cmd2・Δtになるようにするための、基体2の位置Xbの適切な変化量は、図6に示すΔXb_b(≠ΔXb_a)である。
一方、現在の目標歩容における基体2の位置Xb(k)と、この位置Xb(k)から上記ΔXb_bだけ変化させた位置Xb(k)+ΔXb_bとの間の中間の位置Xb(k)+ΔXb_c(ただし、0<ΔXb_c<ΔXb_b)を想定した場合、微分積分学における平均値の定理に基づいて、当該中間の位置Xb(k)+ΔXb_cでの曲線a1の接線a3の傾きを、位置Xb(k)での曲線a1上の点と、位置Xb(k)+ΔXb_bでの曲線a1上の点とを結ぶ直線a4の傾きに一致もしくは近似させることができる。
従って、基体2の位置Xbが当該中間の位置Xb(k)+ΔXb_cとなるような将来の目標歩容を起点として、ヤコビアン行列Aを決定し、このヤコビアン行列Aの擬似逆行列A+を用いて↑Vb_cmd2を決定するようにすれば、この↑Vb_cmd2により規定される基体2の位置Xbの変化量を、上記の適切な変化量ΔXb_bに一致させ、もしくは、該変化量ΔXb_bに対して近似精度の高い値にすることができる。すなわち、↑Vb_cmd2の信頼性を高めることができる。
かかる事項は、↑S_cmdのうちの↑P_cmd2に影響を及ぼす↑Vb_cmd2の成分に限らず、↑L_cmd1に影響を及ぼす↑Vb_cmd2の成分についても同様に言える事項である。
そこで、本実施形態では、上記の如く、現在の時刻(今回の演算処理周期の時刻)から次回の演算処理周期の時刻との間の期間内(1演算処理周期の時間内)での将来の目標歩容(目標脚位置姿勢及び目標基***置姿勢)の瞬時値を推定し、その推定した将来の目標歩容の瞬時値により規定されるロボット1の動作状態を起点として、ヤコビアン行列Aを決定することとした。
この場合、本実施形態では、ヤコビアン行列Aを決定するための起点とする動作状態を規定する将来の目標歩容(以降、これをヤコビアン行列決定用目標歩容という)の構成要素である目標脚位置姿勢及び目標基***置姿勢(以降、これらをヤコビアン行列決定用目標脚位置姿勢、ヤコビアン行列決定用目標基***置姿勢という)の瞬時値は、次のように決定される。
すなわち、基体運動速度目標値決定部45は、次式(7a)で示す如く、現在の目標基***置姿勢(前回の演算処理周期で決定された目標基***置姿勢。以下、これを目標基***置姿勢(k)と表記する)と、1演算処理周期前の目標基***置姿勢(前々回の演算処理周期で決定された目標基***置姿勢。以下、これを目標基***置姿勢(k-1)と表記する)との偏差、すなわち、前回の演算処理周期で決定された目標基***置姿勢の変化量(=目標基***置姿勢(k)−目標基***置姿勢(k-1))に所定値の係数Kpを乗じてなる補正項を、目標基***置姿勢(k)に加え合わせることによって、ヤコビンアン行列決定用目標基***置姿勢を決定する。
同様に、基体運動速度目標値決定部45は、次式(7b)で示す如く、各脚リンク3毎の現在の目標脚位置姿勢(前回の演算処理周期で決定された目標脚位置姿勢。以下、これを目標脚位置姿勢(k)と表記する)と、1演算処理周期前の目標脚位置姿勢(前々回の演算処理周期で決定された目標脚位置姿勢。以下、これを目標脚位置姿勢(k-1)と表記する)との偏差、すなわち、前回の演算処理周期で決定された目標脚位置姿勢の変化量(=目標脚位置姿勢(k)−目標脚位置姿勢(k-1))に上記係数Kpを乗じてなる補正項を、目標脚位置姿勢(k)に加え合わせることによって、ヤコビンアン行列決定用目標基***置姿勢を決定する。
なお、上記係数Kpは、0≦Kp≦1の範囲であらかじめ決定された値である。本実施形態では、例えばKp=0.5である。
ヤコビンアン行列決定用目標基***置姿勢
=目標基***置姿勢(k)+Kp・(目標基***置姿勢(k)−目標基***置姿勢(k-1))
……(7a)
ヤコビンアン行列決定用目標脚位置姿勢
=目標脚位置姿勢(k)+Kp・(目標脚位置姿勢(k)−目標脚位置姿勢(k-1))
……(7b)
従って、本実施形態では、ヤコビアン行列決定用目標歩容は、歩容生成部32の演算処理周期毎の目標歩容の変化量の時系列のうちの最新値に応じて、現在の目標歩容を補正してなる歩容として決定される。
なお、ロボット1が腕リンク等、基体2に対して可動な他のリンクを備える場合には、当該他のリンクの先端部の将来の目標位置及び目標姿勢、あるいは、当該他のリンクの各関節の将来の目標変位量を、ヤコビンアン行列決定用目標基***置姿勢やヤコビアン行列決定用目標脚位置姿勢と同じ手法で求めるようにすればよい。
また、ヤコビアン行列決定用目標歩容は、現在の目標歩容と、1演算処理周期前の目標歩容とを含めて、3個以上の演算処理周期分の目標歩容から決定するようにしてもよい。
そして、基体運動速度目標値決定部45は、上記の如く決定したヤコビアン行列決定用目標歩容の状態を起点として、当該起点の状態から基体2の位置及び姿勢の各成分を微小変化させた場合にそれに起因して発生するロボット1の全体重心の並進運動量ベクトル↑P及び該全体重心周りの角運動量ベクトル↑Lの発生感度を表す行列としてのヤコビアン行列Aを前記フルモデルを用いて算出する。この算出の仕方は、公知の手法と同様でよい。
なお、ヤコビアン行列Aを算出するために用いる動力学モデルは、前記フルモデルと異なる動力学モデル(例えば一部のリンクのイナーシャを無視した動力学モデル等)であってもよいが、できるだけ動力学的精度の高い動力学モデル(前記基本動作目標値決定部42の処理で用いる動力学モデルよりも動力学的精度の高い動力学モデル)を使用することが望ましい。ここで、動力学的精度が高いというのは、ロボット1の運動によって動力学モデル上で発生するトータルの慣性力の、実際のロボット1で発生するトータルの慣性力に対する近似精度が高いことを意味する。
次に、上記の如く決定したヤコビアン行列Aの擬似逆行列A+は、本実施形態では、次のように決定される。
本実施形態では、所謂、特異点低感度運動分解行列(SR-Inverse)を擬似逆行列A+として決定する。この場合、ヤコビアン行列Aの擬似逆行列A+は、W、Vを任意の重み行列(対角行列)として、次式(8a)又は(8b)により算出することができる。
A+=W-1・AT・(A・W-1・AT+V)-1 ……(8a)
A+=(AT・V-1・A+W)-1・AT・V-1 ……(8b)
これらの式(8a)又は(8b)によりに算出される擬似逆行列A+は、次式(9)に与えられる評価関数fの値が最小となるような目標基体運動速度ベクトル↑Vb_cmd2を、前記式(6)により算出することができる擬似逆行列である。
f=(↑Vb_cmd2)T・W・↑Vb_cmd2
+(↑S_cmd - A・↑Vb_cmd2)T・V・(↑S_cmd - A・↑Vb_cmd2)
……(9)
なお、評価関数fの値が最小になるということは、↑Vb_cmd2の各成分の二乗値を、Wの各対角成分を重み係数として線形結合してなる値と、↑S_cmd及びA・↑Vb_cmd2の差分のベクトルの各成分の二乗値を、Vの各対角成分を重み係数として線形結合してなる値との総和が最小になることを意味する。
前記重み行列Wは、目標基体運動速度ベクトル↑Vb_cmd2の成分数と同じ次数(本実施形態では、7次数)の対角行列である。この重み行列Wの各対角成分の値(重み係数)は、例えば↑Vb_cmd2の各成分の値の許容範囲等を考慮して設定すればよい。
また、重み行列Vは、目標制御対象ベクトル↑S_cmdと同じ次数(本実施形態では、13次数)の対角行列である。この重み行列Vの各対角成分の値(重み係数)は、例えばA・↑Vb_cmd2により算出される制御対象ベクトル↑Sの各成分の値の、目標制御対象ベクトル↑S_cmdにより規定される目標値に対する誤差の許容度合いに応じて設定すればよい。
なお、重み行列W,Vは、それぞれ単位行列であってもよい。また、式(8a)におけるV、式(8b)におけるWは、それぞれ零行列であってもよい。
ヤコビアン行列Aの擬似逆行列A+は、前記式(8a),(8b)のいずれの演算式により算出してもよいが、本実施形態では、基体運動速度目標値決定部45は、例えば前記式(8b)を用いて擬似逆行列A+を算出する。この場合、本実施形態では、前記基本目標基体運動速度ベクトル↑Vb_cmd1を目標基体運動速度ベクトル↑Vb_cmd2の基準目標とすることから、重み行列Wを例えば零行列に設定した。
また、重み行列Vに関しては、例えば、目標制御対象ベクトル↑S_cmdのうちの並進運動量ベクトル↑Pの目標値(=↑P_cmd2)と角運動量ベクトル↑Lの目標値(=↑L_cmd1)とに対応する対角成分(6個の対角成分)と、↑S_cmdのうちの基体運動速度ベクトル↑Vbの目標値(=↑Vb_cmd)のうち、関節12の変位量の目標値を除く成分(すなわち下側基体10の位置及び姿勢の目標値)に対応する対角成分(6個の対角成分)と、関節12の変位量の目標値に対応する対角成分(1個の対角成分)との3種類の対角成分に分類して、各種類の対角成分の値を設定した。
一例として、重み行列Vの対角成分のうち、並進運動量ベクトル↑Pの目標値と角運動量ベクトル↑Lの目標値とに対応する6個の対角成分の値を1.0、下側基体10の位置及び姿勢の目標値に対応する6個の対角成分の値を0.5、基体2の関節12の変位量の目標値に対応する1個の対角成分の値を0.1というように、重み行列Vを設定した。
本実施形態では、基体運動速度目標値決定部45は、各演算処理周期において、以上の如くあらかじめ設定された重み行列W,Vを用いて前記式(8b)によりヤコビアン行列Aの擬似逆行列A+を決定する。
そして、この擬似逆行列A+と、前記目標制御対象ベクトル↑S_cmd(今回値)とから前記式(6)により目標基体運動速度ベクトル↑Vb_cmd2(今回値)を決定する。
この場合、本実施形態では重み行列Wを零行列としたので、式(9)の右辺の第2項を最小化するように、すなわち、↑S_cmd及びA・↑Vb_cmd2の差分のベクトルの各成分の二乗値を、重み行列Vの各対角成分を重み係数として線形結合してなる値を最小化するように、↑Vb_cmd2が決定されることとなる。
また、この場合、↑S_cmdは、修正後基本並進運動量ベクトル↑P_cmd2及び基本目標角運動量ベクトル↑L_cmd1に加えて、前記基***置偏差をゼロに近づけるための操作量たる基本目標基体運動速度ベクトル↑Vb_cmd1を含んでいるため、↑Vb_cmd2は、↑Vb_cmd1から乖離しないようにしつつ、前記式(6)の右辺により算出される制御対象ベクトル(=A・↑Vb_cmd2)のうちの並進運動量ベクトル↑Pと、角運動量ベクトル↑Lとがそれぞれ極力、修正後目標並進運動量ベクトル↑P_cmd2、目標角運動量ベクトル↑L_cmd1に一致するように決定されることとなる。
以上が本実施形態における基体運動速度目標値決定部45の処理の詳細である。
歩容生成部32は、以上の如く基体運動速度目標値決定部45の処理を実行した後、次に、目標基***置姿勢決定部46の処理を実行する。
この目標基***置姿勢決定部46は、基体運動速度目標値決定部45で決定された目標基体運動速度ベクトル↑Vb_cmd2を逐次積分することによって、目標基***置姿勢を決定する。
具体的には、目標基***置姿勢決定部46は、各演算処理周期において、目標基体運動速度ベクトル↑Vb_cmd2の今回値に歩容生成部32の演算処理周期の時間Δtを乗じてなる値(=↑Vb_cmd2・Δt)を、現在の目標基***置姿勢の値(前回の演算処理周期において決定した値)に加えることにより、目標基***置姿勢の今回値を決定する。
本実施形態では、歩容生成部32の各演算処理周期において、以上の如く目標基***置姿勢決定部46により決定される目標基***置姿勢(今回値)と、前記目標脚位置姿勢決定部41により決定される各脚リンク3の目標脚位置姿勢(今回値)とが、目標歩容(今回値)の構成要素とされる。
そして、本実施形態では、歩容生成部32は、さらに、目標関節変位量決定部48の処理を実行する。この目標関節変位量決定部48には、各演算処理周期において、決定された目標歩容の構成要素(目標基***置姿勢及び目標脚位置姿勢)が与えられる。
そして、目標関節変位量決定部48は、与えられた目標歩容により規定されるロボット1の各関節の目標関節変位量を、ロボット1の幾何学モデル(剛体リンクモデル)に基づく逆運動学の演算処理によって算出する。この場合、ロボット1の幾何学モデルは、前記目標歩容上重心位置算出部47で用いるものと同じである。
なお、前記目標歩容上重心位置算出部47では、目標歩容(目標基***置姿勢及び目標脚位置姿勢)の代わりに、目標関節変位量決定部48で決定された各関節の目標関節変位量(前回値)を用いて、目標歩容上重心位置↑Xg_cmd2を決定するようにしてもよい。このようにした場合には、目標歩容上重心位置算出部47で、目標歩容により規定される各関節の変位量を算出する処理は不要である。
補足すると、ロボット1が腕リンク等、基体2に対して可動な他のリンクを備える場合には、ロボット1の目標歩容のうちの当該他のリンクに関する構成要素は、例えば基本動作目標値決定部42で適宜、決定しておくようにすればよい。一例として、ロボット1が上側基体11から延設された腕リンクを備える場合には、該ロボット1の歩行動作又は走行動作を行なう目標歩容の生成処理では、該目標歩容における上側基体11に対する該腕リンクの全体の姿勢(ひいては、該腕リンクの各関節の変位量)を、一定の姿勢としたり、あるいは、基体2の関節12の回転軸周りに周期的に腕リンクの全体を動かすような該腕リンクの目標歩容を採用すればよい。
以上が、本実施形態における歩容生成部32の処理の詳細である。
ここで、以上説明した実施形態と本願発明との対応関係について補足しておく。本実施形態では、制御処理ユニット31は、歩容生成部32を備えることによって、本発明における歩容生成装置としての機能を有する。
そして、歩容生成部32の目標脚位置姿勢決定部41、基本動作目標値決定部42、基本目標並進運動量修正部43、基本目標基体運動速度決定部44、基体運動速度目標値決定部45、目標基***置姿勢決定部46、目標歩容上重心位置算出部47の処理によってそれぞれ、本発明における目標脚位置姿勢決定手段、基本動作目標値決定手段、基本目標並進運動量修正手段、基本基体運動速度決定手段、基体運動速度目標値決定手段、目標基***置姿勢決定手段、目標歩容上重心位置算出手段が実現される。
以上説明した本実施形態によれば、前記基体運動速度目標値決定部45は、修正後基本並進運動量ベクトル↑P_cmd2、基本目標角運動量ベクトル↑L_cmd1及び基本目標基体運動速度ベクトル↑Vb_cmd1により構成される目標制御対象ベクトル↑S_cmdを目標として、この↑S_cmdにヤコビアン行列Aの擬似逆行列A+に乗じることにより目標基体運動速度ベクトル↑Vb_cmd2が逐次決定される。そして、この↑Vb_cmd2を積分することにより、目標歩容における目標基***置姿勢が決定される。
この場合、目標制御対象ベクトル↑S_cmdの構成要素のうちの並進運動量ベクトル↑Pの目標値として設定される修正後基本並進運動量ベクトル↑P_cmd2は、基本動作目標値決定部42で決定した基本目標並進運動量ベクトル↑P_cmd1を基準の目標としつつ、目標歩容上重心位置↑Xg_cmd2が、基本動作目標値決定部42で決定した基本目標重心位置↑Xg_cmd1からずれるのを抑制するためのフィードバック操作量により該基本目標並進運動量ベクトル↑P_cmd1を修正することにより決定される。
また、目標制御対象ベクトル↑S_cmdの構成要素のうちの角運動量ベクトル↑Lの目標値として、基本動作目標値決定部42で決定された基本角並進運動量ベクトル↑P_cmd2が設定される。
また、目標制御対象ベクトル↑S_cmdの構成要素のうちの基体運動速度ベクトル↑Vbの目標値して設定される基本目標基体運動速度ベクトル↑Vb_cmd1は、目標歩容における目標基***置姿勢が、基本動作目標値決定部42で決定した基本目標基***置姿勢からずれるのを抑制するためのフィードバック操作量として決定される。
従って、基体運動速度目標値決定部45で逐次決定される目標基体運動速度ベクトル↑Vb_cmd2は、基本動作目標値決定部42の処理における動力学モデル上で満たすようにした動力学を極力崩さないようにしつつ、目標基体運動速度ベクトル↑Vb_cmd2を積分することにより決定される目標基***置姿勢が、基本動作目標値決定部42で決定した基本目標基***置姿勢から乖離することがないように、決定されることとなる。
このため、本実施形態によれば、動力学的な要件を満たしつつ、ロボット1が姿勢を崩すことが無いような目標歩容を生成できる。
また、基体運動速度目標値決定部45の処理では、ロボット1の基体2の位置及び姿勢と、ロボット1の全体重心の並進運動量ベクトルの積分値に相当する状態量や全体重心周りの角運動量ベクトルの積分値に相当する状態量との間の非線形性を考慮して、将来の目標歩容の推定値としての前記ヤコビアン行列決定用目標歩容を起点として、ヤコビアン行列Aを決定するようにしたので、目標制御対象ベクトル↑S_cmdを実現するための基体運動速度ベクトル↑Vbの値として、このヤコビアン行列Aの擬似逆行列A+を目標制御対象ベクトル↑S_cmdに乗じることによって決定される目標基体運動速度ベクトル↑Vb_cmd2の信頼性を高めることができる。
換言すれば、↑Vb_cmd2に、決定したヤコビアン行列Aを乗じることによって得られる制御対象ベクトル↑Sの値の、目標制御対象ベクトル↑S_cmdに対する一致度合いを高めることができる。
加えて、本実施形態では、ヤコビアン行列Aの擬似逆行列A+を、特異点低感度運動分解行列(SR-Inverse)としたので、を該ヤコビアン行列の擬似逆行列として用いて前記基体運動速度ベクトルの目標値を決定する↑S_cmdとA・↑Vb_cmd2との差分のベクトルの各成分の二乗値を、対角行列Vの各対角成分である重み係数により線形結合してなる値を最小化することができる。
従って、目標制御対象ベクトル↑S_cmdを実現する上で適切な目標歩容を逐次決定することができる。
ここで、本実施形態の上記の効果に関する検証結果について図7〜図9を参照して以下に説明する。
図7(a)のグラフa1は、本実施形態の実施例における目標基***置姿勢のうちの、下側基体10のY軸周り(ピッチ軸周り)の傾斜角の経時変化を示している。この場合、ロボット1の目標歩容は、例えば400mmの最大歩幅で歩行を行なう歩容である。また、この実施例における基本目標基***置姿勢のうちの、下側基体10のY軸周り(ピッチ軸周り)の姿勢は0degである。
また、図7(b)のグラフa2は、比較例における目標基***置姿勢のうちの、下側基体10のY軸周り(ピッチ軸周り)の傾斜角の経時変化を示している。
この場合、比較例におけるロボット1の目標歩容は、図7(a)の実施例と同様に、400mmの最大歩幅で歩行を行なう歩容である。また、この比較例における基本目標基***置姿勢のうちの、下側基体10のY軸周り(ピッチ軸周り)の姿勢は、図7(a)の実施例と同様に0degである。
ただし、この比較例においては、目標制御対象ベクトルから基本基体運動速度ベクトル↑Vb_cmd1を除外し、修正後基本目標並進運動量ベクトル↑P_cmd2と、基本目標角運動量ベクトル↑L_cmd1とにより目標制御対象ベクトルを構成するものとした。
図7(b)を参照して判るように、基体運動速度ベクトル↑Vb_cmd1を目標制御対象ベクトル↑S_cmdの構成要素として含まない比較例では目標基***置姿勢が、最終的に基本目標基***置姿勢(=0deg)に対して13deg程度の定常的なずれを生じた。
これに対して、基体運動速度ベクトル↑Vb_cmd1を目標制御対象ベクトル↑S_cmdの構成要素として含む実施例では、図7(a)に示すように目標基***置姿勢の、基本目標基***置姿勢に対する定常的なずれを解消することができた。
従って、基体運動速度ベクトル↑Vb_cmd1を目標制御対象ベクトル↑S_cmdの構成要素として含む本実施形態によれば、基体運動速度ベクトル↑Vb_cmd1を目標制御対象ベクトル↑S_cmdに含めない場合に比して、目標基***置姿勢を、基本目標基***置姿勢から大きく乖離させることなく該基本目標基***置姿勢に追従させることができることが判る。
次に、図8(a)及び図9(a)のそれぞれのグラフb1、c1は、それぞれ、本実施形態の実施例で作成した目標歩容により規定されるロボット1の全体重心のX軸方向の並進運動量の誤差(基本目標並進運動量ベクトル↑P_cmd1のX軸方向成分に対する誤差)、該目標歩容により規定されるロボット1の全体重心でのY軸周り(ピッチ軸周り)の角運動量の誤差(基本目標角運動量ベクトル↑L_cmd1のY軸周り成分に対する誤差)の経時変化を示すグラフである。この場合、ロボット1の目標歩容は、例えば355mmの最大歩幅で歩行を行なう歩容である。
また、図8(b)及び図9(b)のそれぞれのグラフb2、c3は、それぞれ、比較例で作成した目標歩容により規定されるロボット1の全体重心のX軸方向の並進運動量の誤差(基本目標並進運動量ベクトル↑P_cmd1のX軸方向成分に対する誤差)、該目標歩容により規定されるロボット1の全体重心でのY軸周り(ピッチ軸周り)の角運動量の誤差(基本目標角運動量ベクトル↑L_cmd1のY軸周り成分に対する誤差)の経時変化を示すグラフである。
ただし、図8(b)及び図9(b)の比較例においては、各演算処理周期においてヤコビアン行列Aを算出するときの起点とするロボット1の動作状態は、図8(a)及び図9(a)の実施例と異なり、前回の演算処理周期で決定した目標歩容(既に決定された目標歩容のうちの最新値の目標歩容)により規定される動作状態とした。
なお、図8(a)及び図9(a)の実施例と、図8(b)及び図9(b)の比較例とでは、ヤコビアン行列Aの決定処理の相違による効果を明確にするために、目標制御対象ベクトル↑S_cmdの構成要素のうち、前記修正後基本並進運動量ベクトル↑P_cmd2を、基本目標並進運動量ベクトル↑P_cmd1に一致させるものとした。
図8(b)のグラフb2で見られるように、比較例では、ロボット1の全体重心の並進運動量(X軸方向の並進運動量)の誤差の絶対値は、最大で2.3kg・m/s程度であった。これに対して、図8(a)のグラフb1で見られるように、実施例では、ロボット1の全体重心の並進運動量(X軸方向の並進運動量)の誤差の絶対値は、最大でも0.008kg・m/s以下に収まっている。
また、図9(b)のグラフc2で見られるように、比較例では、ロボット1の全体重心周りの角運動量(Y軸周りの角運動量)の誤差の絶対値は、最大で0.8kg・m/s程度であった。これに対して、図9(a)のグラフc1で見られるように、実施例では、ロボット1の全体重心周りの角運動量(Y軸周りの角運動量)の誤差の絶対値は、最大でも0.003kg・m/s以下に収まっている。
従って、ヤコビアン行列Aを算出するためのロボット1の起点の動作状態として、現在の目標歩容により規定される動作状態よりも将来の予測される動作状態寄りにずらした動作状態を用いる本実施形態によれば、ヤコビアン行列Aを算出するためのロボット1の起点の動作状態として、現在の目標歩容により規定される動作状態を用いる場合に比して、目標制御対象ベクトル↑S_cmdにより精度よく追従させ得る目標歩容を生成できることがわかる。
なお、前記実施形態では、2つの脚リンクを有する2足移動ロボット1を例にとって説明したが、本発明は、3つの以上の脚リンクを有する脚式移動ロボットについても適用できる。
また、前記実施形態では、ヤコビアン行列Aを算出するためのロボット1の起点の動作状態として、現在の目標歩容により規定される動作状態よりも将来の予測される動作状態寄りにずらした動作状態を用いるようにしたが、ヤコビアン行列Aを算出するためのロボット1の起点の動作状態として、現在の目標歩容により規定される動作状態を用いるようにしてもよい。
このようにしても、基本目標基体運動速度ベクトル↑Vb_cmd1を目標制御対象ベクトル↑S_cmdの構成要素として含む実施形態によれば、目標歩容における目標基***置姿勢が、基本目標基***置姿勢から乖離するのを防止することができる。
なお、特に歩容生成部32の演算処理周期を十分に短い時間に設定できる場合には、基体2の位置及び姿勢と、制御対象ベクトル↑Sの積分値との間の関係が非線形な関係であっても、現在の目標歩容により規定される動作状態を起点として算出したヤコビアン行列Aの擬似逆行列A+と、目標制御対象ベクトル↑S_cmdとから前記式(6)により算出される目標基体運動速度ベクトル↑Vb_cmd2の、目標制御対象ベクトル↑S_cmdに対応する最適な値に対する誤差を十分に微小なものに留めることができる。
また、前記実施形態では、擬似逆行列A+を前記式(8b)により算出するようにしたが、前記式(8a)により算出するようにしてもよい。
また、前記実施形態におけるロボット1は、基体2に関節12を備えるロボットであるが、基体2の下側基体10と上側基体11とが一体に構成されたものであってもよい。この場合には、基本目標基体運動速度ベクトル↑Vb_cmd1や、目標基体運動速度ベクトル↑Vb_cmd2は、関節12の変位量の時間的変化率の成分を除いた6成分のベクトルにより構成すればよい。
あるいは、下側基体10と上側基体11との間に2自由度以上の自由度を有する関節(2軸以上の軸周りの回転可能な関節)を備えるようにしてもよい。そして、この場合、基本目標基体運動速度ベクトル↑Vb_cmd1や、目標基体運動速度ベクトル↑Vb_cmd2を、下側基体11の位置及び姿勢の時間的変化率の成分と、下側基体10及び上側基体11の間の関節の各軸周りの方向の変位量の時間的変化率の成分とにより構成してもよい。
また、前記実施形態におけるロボット1の各脚リンク3は、6自由度のリンク機構であるが、7自由度以上の自由度を有するリンク機構であってもよい。