JPWO2011080949A1 - ロボットの制御装置 - Google Patents
ロボットの制御装置 Download PDFInfo
- Publication number
- JPWO2011080949A1 JPWO2011080949A1 JP2011547361A JP2011547361A JPWO2011080949A1 JP WO2011080949 A1 JPWO2011080949 A1 JP WO2011080949A1 JP 2011547361 A JP2011547361 A JP 2011547361A JP 2011547361 A JP2011547361 A JP 2011547361A JP WO2011080949 A1 JPWO2011080949 A1 JP WO2011080949A1
- Authority
- JP
- Japan
- Prior art keywords
- state quantity
- control input
- target
- robot
- vector
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Granted
Links
Images
Classifications
-
- B—PERFORMING OPERATIONS; TRANSPORTING
- B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
- B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
- B25J9/00—Programme-controlled manipulators
- B25J9/16—Programme controls
- B25J9/1628—Programme controls characterised by the control loop
- B25J9/1648—Programme controls characterised by the control loop non-linear control combined or not with linear control
-
- B—PERFORMING OPERATIONS; TRANSPORTING
- B62—LAND VEHICLES FOR TRAVELLING OTHERWISE THAN ON RAILS
- B62D—MOTOR VEHICLES; TRAILERS
- B62D57/00—Vehicles characterised by having other propulsion or other ground- engaging means than wheels or endless track, alone or in addition to wheels or endless track
-
- Y—GENERAL TAGGING OF NEW TECHNOLOGICAL DEVELOPMENTS; GENERAL TAGGING OF CROSS-SECTIONAL TECHNOLOGIES SPANNING OVER SEVERAL SECTIONS OF THE IPC; TECHNICAL SUBJECTS COVERED BY FORMER USPC CROSS-REFERENCE ART COLLECTIONS [XRACs] AND DIGESTS
- Y10—TECHNICAL SUBJECTS COVERED BY FORMER USPC
- Y10S—TECHNICAL SUBJECTS COVERED BY FORMER USPC CROSS-REFERENCE ART COLLECTIONS [XRACs] AND DIGESTS
- Y10S901/00—Robots
- Y10S901/01—Mobile robot
Landscapes
- Engineering & Computer Science (AREA)
- Mechanical Engineering (AREA)
- Physics & Mathematics (AREA)
- Nonlinear Science (AREA)
- Robotics (AREA)
- Chemical & Material Sciences (AREA)
- Combustion & Propulsion (AREA)
- Transportation (AREA)
- Feedback Control In General (AREA)
- Manipulator (AREA)
Abstract
ロボット(1)の複数種類の状態量のそれぞれに関する目標値を実現するための状態量別制御入力を決定する手段(85)と、複数の周波数領域のそれぞれにおける周波数領域別制御入力を状態量別制御入力に応じて決定しつつ、周波数領域別制御入力を合成することによって合成制御入力を決定する手段(86)とを備える。手段(86)は、いずれか1つの周波数領域に対応する周波数領域別制御入力を、複数の状態量別制御入力を相互に非干渉に合成することによって決定する。ロボットの種々様々な動作状況で、複数種類の状態量をそれぞれに対応する目標値に効率よく制御するようにロボットの動作制御を行なう。
Description
本発明は、脚式移動ロボット等のロボットの制御装置に関する。
2足歩行ロボット等の脚式移動ロボットの制御技術としては、例えば特許文献1等にて本願出願人が提案した技術の如く、ロボットの目標とする運動(ロボットの脚体先端部の目標位置の軌道等)に応じてロボットの各関節の目標変位量を逐次決定し、その目標変位量に実際の変位量を一致させるように各関節のアクチュエータの駆動制御を行う、所謂、位置制御が一般的に知られている。
このような位置制御は、関節の目標変位量が一定である限り、該関節の実際の変位量を一定に保持するものであるから、ロボットに作用する外力の変化に対する柔軟性が乏しい。
このため、例えば、特許文献2に見られる如く、関節とアクチュエータとの間にばね部材を介装することで関節に柔軟性を持たせると共に、ロボットの目標とする運動を実現するための各関節の目標駆動力を逆動力学演算により決定し、所謂、力制御によって、各関節の実際の駆動力を目標駆動力に制御する技術も知られている。
ところで、ロボットの運動を制御する場合、その運動の安定性などを確保するために、ロボットの1つ以上の関節の変位量に応じて値が変化する複数種類の状態量(例えば、ロボットの互いに異なる複数の部位の位置や姿勢、あるいは、互いに異なる部位又は代表点の運動速度、あるいは、ロボットの所定の部位の互いに異なる種類の運動状態量等)をそれぞれに対応する適切な目標値に制御することが望ましい状況が多々ある。特に、力制御方式のロボットでは、ロボットに作用する外力の変化に応じて各関節の変位量が変動し易いことから、上記のように、複数種類の状態量をそれぞれに対応する適切な目標値に制御することの必要性が高い。
この場合、一般には、各種類の状態量の値を調整し得る関節が、該状態量の種類毎に常に互いに異なる関節となるとは限らず、ロボットの瞬時瞬時の運動状態等に依存して、各種類の状態量の値を調整し得る関節が互いに重複する場合もある。また、各種類の状態量を目標値に制御することの優先度や該制御に要求される速応性が、状態量の種類毎に相違したり、あるいは、ロボットの動作状況等に応じて変化する場合もある。
このため、ロボットの種々様々な動作状況で、複数種類の状態量をそれぞれに対応する適切な目標値に効率よく制御するようにロボットの動作制御を行なうことができる汎用性の高い制御技術が望まれていた。
しかるに、従来のロボットの制御技術では、このような要望を十分に満足し得る技術が確立されていないのが実状である。
本発明は、かかる背景に鑑みてなされてものであり、ロボットの種々様々な動作状況で、複数種類の状態量をそれぞれに対応する目標値に効率よく制御するようにロボットの動作制御を行なうことができる汎用性の高い制御装置を提供することを目的とする。
本発明のロボットの制御装置は、かかる目的を達成するために、関節を介して相互に連結された複数のリンクと、各関節を駆動するアクチュエータとを備えるロボットの制御装置であって、
前記ロボットの各関節の変位量を成分として少なくとも含むベクトルである該ロボットの一般化変数ベクトルの1つ以上の成分値に依存してそれぞれの値が規定される複数種類の状態量のそれぞれに関する目標値が入力され、各種類の状態量毎に、該状態量に対応する目標値を実現するために必要な前記アクチュエータの動作を規定する制御入力である状態量別制御入力を、少なくとも該状態量に対応する目標値に基づいて決定する状態量別制御入力決定手段と、
あらかじめ設定された複数の周波数領域のそれぞれにおける前記アクチュエータの動作を規定する制御入力である周波数領域別制御入力を、前記複数種類の状態量のそれぞれに対応して決定された前記状態量別制御入力のうちの1つ以上の状態量別制御入力に応じて決定しつつ、前記複数の周波数領域のそれぞれに対応する該周波数領域別制御入力を合成する処理を実行することによって、合成制御入力を決定する合成制御入力決定手段と、
少なくとも前記合成制御入力に応じて、前記アクチュエータの動作を制御するアクチュエータ制御手段とを備え、
前記合成制御入力決定手段が実行する処理は、前記複数の周波数領域のうちの少なくともいずれか1つの周波数領域に対応する前記周波数領域別制御入力を、前記複数種類の状態量のそれぞれに対応する状態量別制御入力のうちの複数の状態量別制御入力を相互に非干渉に合成することによって決定する処理を含むことを特徴とする(第1発明)。
前記ロボットの各関節の変位量を成分として少なくとも含むベクトルである該ロボットの一般化変数ベクトルの1つ以上の成分値に依存してそれぞれの値が規定される複数種類の状態量のそれぞれに関する目標値が入力され、各種類の状態量毎に、該状態量に対応する目標値を実現するために必要な前記アクチュエータの動作を規定する制御入力である状態量別制御入力を、少なくとも該状態量に対応する目標値に基づいて決定する状態量別制御入力決定手段と、
あらかじめ設定された複数の周波数領域のそれぞれにおける前記アクチュエータの動作を規定する制御入力である周波数領域別制御入力を、前記複数種類の状態量のそれぞれに対応して決定された前記状態量別制御入力のうちの1つ以上の状態量別制御入力に応じて決定しつつ、前記複数の周波数領域のそれぞれに対応する該周波数領域別制御入力を合成する処理を実行することによって、合成制御入力を決定する合成制御入力決定手段と、
少なくとも前記合成制御入力に応じて、前記アクチュエータの動作を制御するアクチュエータ制御手段とを備え、
前記合成制御入力決定手段が実行する処理は、前記複数の周波数領域のうちの少なくともいずれか1つの周波数領域に対応する前記周波数領域別制御入力を、前記複数種類の状態量のそれぞれに対応する状態量別制御入力のうちの複数の状態量別制御入力を相互に非干渉に合成することによって決定する処理を含むことを特徴とする(第1発明)。
補足すると、本発明においては、前記「ロボット」は、少なくとも1つのリンクがロボットの動作環境におけるグローバル座標系に対して固定された(該動作環境の床等に固定された)設置型とのロボットと、該グローバル座標系に対してロボットの全体が移動可能な移動ロボットとのいずれのロボットであってもよい。なお、グローバル座標系は、慣性座標系又は近似的に慣性系と見なせる座標系を意味する。
また、前記一般化変数ベクトルは、前記ロボットが設置型のロボットである場合には、該ロボットの全ての関節の変位量(回転角度又はストローク量)を成分値として構成されるベクトルである。一方、前記ロボットが移動ロボットである場合には、前記一般化変数ベクトルは、該ロボットの任意の1つの代表リンクのグローバル座標系での位置及び姿勢と、該ロボットの全ての関節の変位量とを成分値として構成されるベクトルである。
なお、ロボットのリンクの「位置」は、該リンクに対して固定された代表点の空間的な位置を意味し、リンクの「姿勢」は、リンクの空間的な向きを意味する。
前記第1発明によれば、前記ロボットの複数種類の状態量を所要の目標値に制御するために、各種類の状態量毎に、該状態量に対応する目標値を実現する(該状態量の実際の値を目標値に制御する)ために必要な前記アクチュエータの動作を規定する状態量別制御入力が、前記状態量別制御入力決定手段によって、少なくとも該状態量に対応する目標値に基づいて決定される。
なお、各種類の状態量は、該ロボットの一般化変数ベクトルの1つ以上の成分値に依存してそれぞれの値が規定される状態量、すなわち、一般化変数ベクトルの1つ又は複数の成分値の関数値となる状態量である。
そして、第1発明では、前記合成制御入力決定手段が、前記複数の周波数領域のそれぞれにおける周波数領域別制御入力を前記状態量別制御入力に応じて決定しつつ、前記複数の周波数領域のそれぞれに対応する該周波数領域別制御入力を合成する処理を実行することによって、前記合成制御入力を決定する。
さらに、前記アクチュエータ制御手段が、少なくとも前記合成制御入力に応じて前記アクチュエータの動作を制御する。これにより、各周波数領域毎の周波数領域別制御入力を反映させて、ロボットの動作制御が行なわれる。
ここで、各周波数領域毎の周波数領域別制御入力は、前記複数種類の状態量のそれぞれに対応する状態量別制御入力のうちの1つ又は複数の状態量別制御入力(すなわち、1つ以上の状態量別制御入力)に応じて決定される。なお、各周波数領域毎の周波数領域別制御入力の基となる状態量別制御入力は、より詳しくは、該当する周波数領域内の周波数成分である。
そして、少なくともいずれか1つの周波数領域(1つ以上の周波数領域)に関しては、該周波数領域(以降、特定周波数領域ということがある)に対応する周波数別制御入力は、前記複数種類の状態量のそれぞれに対応する状態量別制御入力のうちの2つ以上(複数)の状態量別制御入力(該状態量別制御入力の個数は、前記状態量の種類数よりも少なくてもよい)に応じて決定される。さらに、この場合、該特定周波数領域に対応する周波数別制御入力は、当該2つ以上の状態量別制御入力を相互に非干渉に合成することによって決定される。
なお、前記複数の周波数領域は、部分的な重複を有する周波数領域であってもよい。
ここで、本発明において、「当該2つ以上の状態量別制御入力を相互に非干渉に合成する」ということは、その合成後の周波数別制御入力を制御入力Aと定義すると共に、該制御入力Aの基となる当該2つ以上の状態量別制御入力のうちの任意の1つの状態量別制御入力を制御入力Bと定義し、制御入力Aによってロボットのアクチュエータの動作制御を行なった場合と、制御入力Bの前記特定周波数領域の成分によってロボットのアクチュエータの動作制御を行った場合とを想定したとき、その両方の場合において、前記複数種類の状態量のうちの制御入力Bに対応する種類の状態量とを同じ状態に制御し得るように、当該2つ以上の状態量別制御入力を合成することを意味する。
換言すれば、「当該2つ以上の状態量別制御入力を相互に非干渉に合成する」ということは、前記一般化変数ベクトルのうち、制御入力Bに対応する状態量の値に影響を及ぼす成分に対する操作が、制御入力Aと制御入力Bとで同じ操作となり、その他の成分のうちの1つ又は複数の成分に対する操作が、制御入力Aと制御入力Bとで異なる操作となるように、当該2つ以上の状態量別制御入力を合成することを意味する。
かかる第1発明によれば、各周波数領域毎に、それに対応する周波数別制御入力の基となる状態量別制御入力を選定できる。換言すれば、各周波数領域毎に、制御対象(制御しようとする対象)としての状態量を選定できる。そのため、各周波数領域毎に、前記複数の状態量の制御に関する要求(該制御の優先度合いや速応性の要求度合い、該制御の必要性等)に適した周波数領域別制御入力を決定できる。
また、前記特定周波数領域に関しては、複数の状態量別制御入力を相互に非干渉に合成することによって、それぞれの状態量別制御入力に対応する複数種類の状態量をそれぞれ目標値に制御することを、相互に干渉することがないように(該複数種類の状態量のそれぞれの制御が他の状態量の制御に影響を及ぼすことが無いように)行なうことができる。
この結果、第1発明によれば、ロボットの種々様々な動作状況で、複数種類の状態量をそれぞれに対応する目標値に効率よく制御するようにロボットの動作制御を行なうことができる。また、ロボットの構成や、制御対象の状態量として、種々様々な態様を採用できるので、上記の如き動作制御を行う制御装置の汎用性を高めることができる。
補足すると、本発明において、前記状態量別制御入力決定手段が、前記各種類の状態量に対応させて決定する状態量別制御入力は、各種類の状態量に対して単一の制御入力である必要はなく、いずれかの種類の状態量に対して、該状態量に関する前記目標値を実現するための複数の状態量別制御入力を、それぞれ各別の制御則によって決定するようにしてもよい。例えば、該状態量に関する前記目標値を実現するための2つの状態量別制御入力を、フィードフォワード制御則と、フィードバック制御則とによって各別に決定するようにしてもよい。そして、この場合、互いに異なる周波数領域での制御対象に、当該状態量を含めると共に、そのそれぞれの周波数領域に対応する周波数領域別制御入力を、当該状態量に対応する各別の状態量別制御入力(互いに異なる制御則によって決定した各別の状態量別制御入力)を用いて決定するようにしてもよい。
前記第1発明においては、前記合成制御入力決定手段の処理は、より具体的には、例えば次のような処理によって実現される。
すなわち、前記合成制御入力をτtotal、前記複数の周波数領域の総個数をn、n個の該周波数領域のそれぞれを第i周波数領域(i=1,2,…,n)、第i周波数領域が通過可能な周波数帯となるように周波数通過特性が設定された周波数重みをW(i)としたとき、前記合成制御入力決定手段は、例えば、次式02aにより表される処理によって算出されるn個のτf(i)(i=1,2,…,n)と前記周波数重みW(i)(i=1,2,…,n)とから次式01により表される処理によって、前記合成制御入力τtotalを決定する(第2発明)。
なお、本明細書では、「*」は乗算記号を意味する。
但し、
W(i)*τf(i):第i周波数領域に対応する前記周波数領域別制御入力である第i周波数領域別制御入力
m(i):前記複数種類の状態量のそれぞれに対応する前記状態量別制御入力のうち、第i周波数領域別制御入力W(i)*τf(i)の組成成分として決定される状態量別制御入力の個数であり、所定の制約条件を満たす範囲内で、1≦m(i)≦M(M:前記状態量の種類数)、且つ、いずれかのiに対してm(i)>1という条件とを満たすように設定された整数
τ(i,j):前記所定の制約条件を満たす範囲内で、第i周波数別制御入力W(i)*τf(i)の組成成分として決定されたm(i)個の状態量別制御入力τ(i,j)(j=1,2,…,m(i))のうちの第j番目の状態量別制御入力である第(i,j)状態量別制御入力(ベクトル)
前記所定の制約条件:全てのi(i=1,2,…,n)に対してm(i)=Mが成立し、且つ、互いに異なる任意のiの値i1、i2(1≦i1≦n、1≦i2≦n)に対してτ(i1,j)=τ(i2,j)が1からMまでのすべてのj(j=1,2,…,M)に対して成立する、ということがないという条件
Nu(i,j):Nu(i,j)≡I−Js(i,j)T*(Js#(i,j))Tにより定義される行列
Js#(i,j):Js#(i,j)≡WatT*Js(i,j)T*(Js(i,j)*Wat*Js(i,j)T)-1により定義される行列
I:単位行列
Js(i,j):S(i,j)=Js(i,j)・q'となるヤコビアン
Wat:Wat≡Wd*M-1により定義される行列
Wd:あらかじめ設定された重み行列(対角行列)
M:q''を一般化力ベクトルに変換するイナーシャ行列
(M*q''=一般化力ベクトル)
S(i,j):第(i,j)状態量別制御入力に対応する状態量(ベクトル又はスカラー)
q:一般化変数ベクトル
q':qの1階微分値(=dq/dt)
q'':qの2階微分値(=d2q/dt2)
なお、前記式01におけるW(i)*τf(i)という演算処理は、τf(i)を、周波数重みW(i)と同じ周波数通過特性のフィルタに通す処理(フィルタリング処理)と等価である。
m(i):前記複数種類の状態量のそれぞれに対応する前記状態量別制御入力のうち、第i周波数領域別制御入力W(i)*τf(i)の組成成分として決定される状態量別制御入力の個数であり、所定の制約条件を満たす範囲内で、1≦m(i)≦M(M:前記状態量の種類数)、且つ、いずれかのiに対してm(i)>1という条件とを満たすように設定された整数
τ(i,j):前記所定の制約条件を満たす範囲内で、第i周波数別制御入力W(i)*τf(i)の組成成分として決定されたm(i)個の状態量別制御入力τ(i,j)(j=1,2,…,m(i))のうちの第j番目の状態量別制御入力である第(i,j)状態量別制御入力(ベクトル)
前記所定の制約条件:全てのi(i=1,2,…,n)に対してm(i)=Mが成立し、且つ、互いに異なる任意のiの値i1、i2(1≦i1≦n、1≦i2≦n)に対してτ(i1,j)=τ(i2,j)が1からMまでのすべてのj(j=1,2,…,M)に対して成立する、ということがないという条件
Nu(i,j):Nu(i,j)≡I−Js(i,j)T*(Js#(i,j))Tにより定義される行列
Js#(i,j):Js#(i,j)≡WatT*Js(i,j)T*(Js(i,j)*Wat*Js(i,j)T)-1により定義される行列
I:単位行列
Js(i,j):S(i,j)=Js(i,j)・q'となるヤコビアン
Wat:Wat≡Wd*M-1により定義される行列
Wd:あらかじめ設定された重み行列(対角行列)
M:q''を一般化力ベクトルに変換するイナーシャ行列
(M*q''=一般化力ベクトル)
S(i,j):第(i,j)状態量別制御入力に対応する状態量(ベクトル又はスカラー)
q:一般化変数ベクトル
q':qの1階微分値(=dq/dt)
q'':qの2階微分値(=d2q/dt2)
なお、前記式01におけるW(i)*τf(i)という演算処理は、τf(i)を、周波数重みW(i)と同じ周波数通過特性のフィルタに通す処理(フィルタリング処理)と等価である。
また、前記「一般化力ベクトル」は、その各成分が、一般化変数ベクトルqの各成分に対応しているベクトル(qと同数の成分からなるベクトル)である。この一般化力ベクトルは、より詳しくは、その各成分が、それに対応する一般化変数ベクトルqの成分値の時間的変化を発生させる力(並進力又はモーメント)となるベクトルである。
かかる第2発明によれば、m(i)>1となる第i周波数領域が前記特定周波数領域に相当する。
この場合、行列Nu(i,j)が前記した通り定義される行列であるので、前記式02において、例えば、Nu(i,1)*τ(i,2)は、状態量別制御入力τ(i,2)のうち、状態量別制御入力τ(i,1)に対応する種類の状態量の値に影響を及ぼすことのない(該状態量の値を変化させない)成分から構成されるベクトル(一般化力ベクトル)を意味する。
より一般的に言えば、前記式02における、Nu(i,1)*Nu(i,2)*……*Nu(i,m(i)-1)*τ(i,m(i))は、状態量別制御入力τ(i,m(i))のうち、(m(i)−1)個の状態量別制御入力τ(i,1)〜τ(i,m(i)-1)にそれぞれ対応する状態量の値に影響を及ぼすことのない成分から構成されるベクトル(一般化ベクトル)を意味する。
そして、前記第i周波数領域別制御入力W(i)*τf(i)の組成成分として決定される状態量別制御入力の個数m(i)(換言すれば、第i周波数領域における制御対象として選定される状態量の個数)が前記の条件を満たすように設定される。
このため、第2発明によれば、m(i)>1となる第i周波数領域(特定周波数領域)に対応する第i周波数領域別制御入力W(i)*τf(i)は、m(i)個の状態量別制御入力τ(i,1)〜τ(i,m(i))を相互に非干渉に合成したものとなる。
従って、第2発明によれば、前記第1発明の効果を実現し得る前記合成制御入力を適切に決定することができる。
補足すると、前記所定の制約条件は、換言すれば、いずれかのi(1≦i≦n)に対してm(i)≠Mとなるか、又は、いずれかのj(1≦i≦M)に対して、τ(i3,j)≠τ(i4,j)となるようなiの値のi3,i4の組が存在する、という条件と等価である。
また、前記重み行列Wdは単位行列であってもよい。すなわち、Wat=M-1であってもよい。
ところで、各周波数領域において、制御対象として選定すべき状態量の種類や個数は、ロボットの動作中に常時、一定であることが要求されるとは限らず、ロボットの動作状況等に応じて、制御対象とする状態量の種類や個数を変更することが必要もしくは好適となる場合もある。
例えば、ロボットの一連の動作の中での特定の動作を行なっている期間でのみ、ある特定種類の状態量を、特定の周波数領域における制御対象として選定し、それ以外の期間では、当該特定種類の状態量を、当該特定の周波数領域における制御対象から除外したり、あるいは別の種類の状態量に置き換えることが必要となる場合もある。
このような場合に、ロボットの動作中のあるタイミング(制御対象の状態量の種類や個数を変更しようとするタイミング)で、任意の周波数領域における制御対象の状態量の種類や個数を直ちに変更するようにすると、その変更の前後で該周波数領域に対応する周波数領域別制御入力の不連続的な変化が生じやすい。ひいては、前記合成制御入力の不連続的な変化が生じやすい。
そして、前記合成制御入力の不連続的な変化が生じると、ロボットのアクチュエータの動作の急激な変化が要求されることとなる。従って、前記複数の周波数領域のうちのいずれかの周波数領域において、制御対象とする状態量の種類や個数を変更しようとする場合には、前記合成制御入力の不連続的な変化を防止し得るように当該変更を行なうことが望まれる。
このような要望を達成するためには、前記第1発明において、前記合成制御入力決定手段の処理を、例えば次のような処理によって実現することが好ましい。
すなわち、前記合成制御入力決定手段は、前記式02aの代わりに、次式02bにより表される処理によって算出されるn個のτf(i)(i=1,2,…,n)と前記周波数重みW(i)(i=1,2,…,n)とから前記式01により表される処理によって、前記合成制御入力τtotalを決定する(第3発明)。
但し、
W(i)*τf(i):第i周波数領域に対応する前記周波数領域別制御入力である第i周波数領域別制御入力
m(i):前記複数種類の状態量のそれぞれに対応する前記状態量別制御入力のうち、第i周波数領域別制御入力W(i)*τf(i)の組成成分として決定される状態量別制御入力の個数であり、所定の制約条件を満たす範囲内で、1≦m(i)≦M(M:前記状態量の種類数)、且つ、いずれかのiに対してm(i)>1という条件とを満たすように設定された整数
τ(i,j):前記所定の制約条件を満たす範囲内で、第i周波数別制御入力W(i)*τf(i)の組成成分として決定されたm(i)個の状態量別制御入力τ(i,j)(j=1,2,…,m(i))のうちの第j番目の状態量別制御入力である第(i,j)状態量別制御入力(ベクトル)
前記所定の制約条件:全てのi(i=1,2,…,n)に対してm(i)=Mが成立し、且つ、互いに異なる任意のiの値i1、i2(1≦i1≦n、1≦i2≦n)に対してτ(i1,j)=τ(i2,j)が1からMまでのすべてのj(j=1,2,…,M)に対して成立する、ということがないという条件
wc(i,j):第(i,j)状態量別制御入力τ(i,j)に対応する重み係数であり、0≦wc(i,j)≦1の範囲内で一定値に維持されるか、又は連続的に増加もしくは減少するように設定された重み係数
Nuw(i,j):Nuw(i,j)≡I−wc(i,j)*Js(i,j)T*(Js#(i,j))Tにより定義される行列
Js#(i,j):Js#(i,j)≡(Wat)T*Js(i,j)T*(Js(i,j)*Wat*Js(i,j)T)-1により定義される行列
I:単位行列
Js(i,j):S(i,j)=Js(i,j)・q'となるヤコビアン
Wat:Wat≡Wd*M-1により定義される行列
Wd:あらかじめ設定された重み行列(対角行列)
M:q''を一般化力ベクトルに変換するイナーシャ行列
(M*q''=一般化力ベクトル)
S(i,j):第(i,j)状態量別制御入力に対応する状態量(ベクトル又はスカラー)
q:一般化変数ベクトル
q':qの1階微分値(=dq/dt)
q'':qの2階微分値(=d2q/dt2)
上記第3発明における式02bは、前記第2発明における式02aの右辺の各項に対して、τ(i,j)(j=1,2,…,m(i))に乗じる重み係数wc(i,j)を付加し、且つ、前記式02aにおける行列Nu(i,j)(≡I−*Js(i,j)T*(Js#(i,j))T)を、該重み係数wc(i,j)を付加して定義される上記行列Nuw(i,j)(≡I−wc(i,j)*Js(i,j)T*(Js#(i,j))T)に置き換えたものである。
m(i):前記複数種類の状態量のそれぞれに対応する前記状態量別制御入力のうち、第i周波数領域別制御入力W(i)*τf(i)の組成成分として決定される状態量別制御入力の個数であり、所定の制約条件を満たす範囲内で、1≦m(i)≦M(M:前記状態量の種類数)、且つ、いずれかのiに対してm(i)>1という条件とを満たすように設定された整数
τ(i,j):前記所定の制約条件を満たす範囲内で、第i周波数別制御入力W(i)*τf(i)の組成成分として決定されたm(i)個の状態量別制御入力τ(i,j)(j=1,2,…,m(i))のうちの第j番目の状態量別制御入力である第(i,j)状態量別制御入力(ベクトル)
前記所定の制約条件:全てのi(i=1,2,…,n)に対してm(i)=Mが成立し、且つ、互いに異なる任意のiの値i1、i2(1≦i1≦n、1≦i2≦n)に対してτ(i1,j)=τ(i2,j)が1からMまでのすべてのj(j=1,2,…,M)に対して成立する、ということがないという条件
wc(i,j):第(i,j)状態量別制御入力τ(i,j)に対応する重み係数であり、0≦wc(i,j)≦1の範囲内で一定値に維持されるか、又は連続的に増加もしくは減少するように設定された重み係数
Nuw(i,j):Nuw(i,j)≡I−wc(i,j)*Js(i,j)T*(Js#(i,j))Tにより定義される行列
Js#(i,j):Js#(i,j)≡(Wat)T*Js(i,j)T*(Js(i,j)*Wat*Js(i,j)T)-1により定義される行列
I:単位行列
Js(i,j):S(i,j)=Js(i,j)・q'となるヤコビアン
Wat:Wat≡Wd*M-1により定義される行列
Wd:あらかじめ設定された重み行列(対角行列)
M:q''を一般化力ベクトルに変換するイナーシャ行列
(M*q''=一般化力ベクトル)
S(i,j):第(i,j)状態量別制御入力に対応する状態量(ベクトル又はスカラー)
q:一般化変数ベクトル
q':qの1階微分値(=dq/dt)
q'':qの2階微分値(=d2q/dt2)
上記第3発明における式02bは、前記第2発明における式02aの右辺の各項に対して、τ(i,j)(j=1,2,…,m(i))に乗じる重み係数wc(i,j)を付加し、且つ、前記式02aにおける行列Nu(i,j)(≡I−*Js(i,j)T*(Js#(i,j))T)を、該重み係数wc(i,j)を付加して定義される上記行列Nuw(i,j)(≡I−wc(i,j)*Js(i,j)T*(Js#(i,j))T)に置き換えたものである。
かかる第3発明によれば、第i周波数領域別制御入力W(i)*τf(i)の組成成分となる全ての状態量別制御入力τ(i,j)(j=1,2,…,m(i))に対応する前記重み係数wc(i,j)の値を“1”に設定した場合には、式02bは前記式02aに一致するものとなる。従って、m(i)>1となる第i周波数領域(特定周波数領域)においては、前記第2発明と同様に、m(i)個の状態量別制御入力τ(i,1)〜τ(i,m(i))を相互に非干渉に合成したものととして、第i周波数領域別制御入力を決定できることとなる。
また、いずれかの第i周波数領域において、いずれかの第(i,j)状態量別制御入力τ(i,j)に乗じる重み係数wc(i,j)を“0”に設定した場合には、該第i周波数領域に対応する第i周波数領域別制御入力W(i)*τf(i)は、該第(i,j)状態量別制御入力τ(i,j)や、第(i,j)状態量別制御入力τ(i,j)に対応する前記ヤコビアンJs(i,j)に対する依存性が無くなる。このため、該第(i,j)状態量別制御入力τ(i,j)に対応する状態量は、実質的に、該第i周波数領域における制御対象から除外されることとなる。
さらに、いずれかの第i周波数領域において、いずれかの第(i,j)状態量別制御入力τ(i,j)に対応する重み係数wc(i,j)を“0”と“1”との間の範囲内で連続的に増加もしくは減少させるように設定した場合には、該第i周波数領域に対応する第i周波数領域別制御入力W(i)*τf(i)の、該第(i,j)状態量別制御入力τ(i,j)に対する依存度が連続的に変化することとなる。例えば、該重み係数wc(i,j)の値を“1”から“0”に向かって連続的に減少させていくと、第i周波数領域別制御入力W(i)*τf(i)は、第(i,j)状態量別制御入力τ(i,j)に対する依存度が連続的に低下していく。
従って、各周波数領域における制御対象の状態量の種類及び個数の両方又は一方を、重み係数wc(i,j)の値の設定変更によって適宜変更することができることとなる。さらに、その変更の際に、該重み係数wc(i,j)の値を連続的に増加もしくは減少させることによって、第i周波数領域別制御入力W(i)*τf(i)を連続的に変化させることができることとなる。ひいては、前記合成制御入力τtotalを、不連続的に変化することがないように(連続的に変化するように)決定できることとなる。
この場合、例えば、各周波数領域において、ある状態量を制御対象から除外しようとする場合には、その状態量に対応する状態量別制御入力τ(i,j)に乗じる重み係数wc(i,j)の値を“1”から“0”に連続的に減少させるように設定すればよい。逆に、各周波数領域において、制御対象から除外した状態量を、制御対象に復帰させようとする場合には、その状態量に対応する状態量別制御入力τ(i,j)に乗じる重み係数wc(i,j)の値を“0”から“1”に連続的に増加させるように設定すればよい。
さらに、各周波数領域において、ある状態量を制御対象から除外すると共に、別の種類の状態量を制御対象に加えようとする場合、換言すれば、制御対象とする状態量の種類を変更しようとする場合には、制御対象から除外しようとする状態量に対応する状態量別制御入力τ(i,j)に乗じる重み係数wc(i,j)の値を“1”から“0”に連続的に減少させると共に、制御対象に加えようとする別の種類の状態量に対応する状態量別制御入力τ(i,j)に乗じる重み係数wc(i,j)の値を“0”から“1”に連続的に増加させるように設定すればよい。
以上のことから、第3発明によれば、前記第1発明の効果を実現できる前記合成制御入力を連続的に変化させるようにしつつ、適切に決定することが可能となる。また、各周波数領域毎に、ロボットの動作状況等に応じて重み係数wc(i,j)の値を可変的に設定することで、制御対象とする状態量の種類や個数を、ロボットの動作状況等に応じて適宜変更することができる。そして、この場合、制御対象とする状態量の種類や個数を変更する周波数領域に対応する周波数領域別制御入力W(i)*τf(i)を連続的に変化させることができるので、前記合成制御入力を不連続的に変化することがないように決定できる。ひいては、ロボットの動作をスムーズに行なうことができる。
かかる第3発明では、例えば、前記ロボットがその所定の部位(例えば脚体やアームの先端部)を該ロボットの外界の接触対象部に接触させる動作と該接触対象部から離脱させる動作とを含む運動を行なうロボットであると共に、前記複数種類の状態量が、前記ロボットの所定の部位を前記接触対象部から離脱させた状態での該所定の部位の運動状態量を、前記複数の周波数領域のうちの少なくとも1つの周波数領域における制御対象として含んでいる場合に、次のような態様を採用することができる。
すなわち、前記所定の部位の運動状態量を制御対象として含む周波数領域を第ia周波数領域とし、該第ia周波数領域に対応する第ia周波数領域別制御入力W(ia)*τf(i)の組成成分となる前記状態量別制御入力のうちの前記所定の部位の運動状態量に対応する状態量別制御入力を第(ia,ja)状態量別制御入力τ(ia,ja)としたとき、該τ(ia,ja)に対応する前記重み係数wc(ia,ja)は、前記所定の部位が前記接触対象部に接触する期間では、該重み係数wc(ia,ja)の値が“0”に維持され、且つ、前記所定の部位が前記接触対象部から離脱する期間では、該重み係数wc(ia,ja)の値が“0”から“1”まで連続的に増加した後、“1”から“0”まで連続的に減少するように設定される(第4発明)。
この第4発明によれば、前記所定の部位が前記接触対象部に接触する期間では、前記重み係数wc(ia,ja)の値が“0”に維持される。
ここで、該期間では、前記所定の部位が前記接触対象部に接触するので、その接触によって、該所定の部位の運動が拘束を受けることとなる。そして、このような状況で、該所定の部位の運動状態量(例えば、該所定の部位の位置、姿勢、もしくはその時間的変化率、加速度等)を強制的に所望の目標値に制御しようとすると、該運動状態量の実際の値と目標値とのずれが生じた場合に、前記アクチュエータの過大な駆動力が要求される恐れがある。
そこで、第4発明では、前記所定の部位が前記接触対象部に接触する期間では、該重み係数wc(ia,ja)の値が“0”に維持する。これにより、該期間では、前記所定の部位の運動状態量が、前記第ia周波数領域での制御対象から実質的に除外されることとなる。
一方、前記所定の部位が前記接触対象部から離脱する期間では、前記重み係数wc(ia,ja)の値が“0”から“1”まで連続的に増加した後、“1”から“0”まで連続的に減少するように設定される。このため、該期間では、前記所定の部位の運動状態量が、前記第ia周波数領域での制御対象に含まれるようになる。この場合、前記所定の部位は前記接触対象部から離脱しており、外界との接触に伴う拘束を受けない状態であるので、当該期間での第ia周波数領域における前記所定の部位の運動状態量を、円滑に所望の目標値に制御することができる。
さらに、この場合、前記重み係数wc(ia,ja)の値が“0”から“1”まで連続的に増加した後、“1”から“0”まで連続的に減少するように設定されるので、前記所定の部位が前記接触対象部から離脱する期間内で、第ia周波数領域別制御入力W(ia)*τf(ia)を連続的に滑らかに変化させることができる。ひいては、前記合成制御入力が不連続的に変化しないようにして、ロボットの動作を円滑に行うことができる。
前記第2〜第4発明では、前記各周波数重みW(i)(i=1,2,…,n)としては、例えば、次式03の伝達関数により表される周波数重みを採用することができる(第5発明)。
但し、
s:ラプラス変換の演算子
T(i) (i =1, 2,…, n):T(1)<T(2)<……<T(n)となるように設定された時定数
W(n+1)≡0
この第5発明によれば、n個の周波数重みW(1)〜W(n)のうち、W(n)がローパス特性の周波数重み、その他の周波数重みW(1)〜W(n-1)がバンドパス特性の周波数重みとなる。この場合、W(1)〜W(n)は、その番数i(i=1,2,…,n)が小さいほど(“1”に近いほど)、通過可能な周波数領域がより高周波域の領域となる。
s:ラプラス変換の演算子
T(i) (i =1, 2,…, n):T(1)<T(2)<……<T(n)となるように設定された時定数
W(n+1)≡0
この第5発明によれば、n個の周波数重みW(1)〜W(n)のうち、W(n)がローパス特性の周波数重み、その他の周波数重みW(1)〜W(n-1)がバンドパス特性の周波数重みとなる。この場合、W(1)〜W(n)は、その番数i(i=1,2,…,n)が小さいほど(“1”に近いほど)、通過可能な周波数領域がより高周波域の領域となる。
そして、これらの周波数重みW(1)〜W(n)によって、時定数T(1)により規定されるカットオフ周波数以下の周波数域を間断なく分離できる。このため、時定数T(1)により規定されるカットオフ周波数以下の周波数域でのロボットの所望の制御特性を容易に実現できる。
また、前記第2発明においては、前記個数m(i)がm(i)>1である前記第i周波数領域別制御入力W(i)*τf(i)の組成成分として決定される状態量別制御入力τ(i,j)(j=1,2,…,m(i))は、前記式02の演算に用いるm(i)個の前記行列Nu(i,j)(j=1,2,…,m(i))が、rank(Nu(i,1))≧rank(Nu(i,2))≧……≧rank(Nu(i,m(i)))という関係を満たすように前記複数種類の状態量に対応するM個の状態量別制御入力の中から選定された状態量別制御入力であることが好ましい(第6発明)。
同様に、前記第3発明においては、前記個数m(i)がm(i)>1である前記第i周波数領域別制御入力W(i)*τf(i)の組成成分として決定される状態量別制御入力τ(i,j)(j=1,2,…,m(i))は、前記式02aの演算に用いるm(i)個の前記行列Nuw(i,j)(j=1,2,…,m(i))が、rank(Nuw(i,1))≧rank(Nuw(i,2))≧……≧rank(Nuw(i,m(i)))という関係を満たすように前記複数種類の状態量に対応するM個の状態量別制御入力の中から選定された状態量別制御入力であることが好ましい(第7発明)。
なお、rank(Nu(i,j))は行列Nu(i,j)の階数であり、rank(Nuw(i,j))は行列Nuw(i,j)の階数である。
このようにした場合には、前記第6発明においては、m(i)>1である前記第i周波数領域別制御入力W(i)*τf(i)の組成成分として決定される状態量別制御入力τ(i,j)(j=1,2,…,m(i))が、rank(Nu(i,1))≧rank(Nu(i,2))≧……≧rank(Nu(i,m(i)))という関係を満たすように前記複数種類の状態量に対応するM個の状態量別制御入力の中から選定された状態量別制御入力である。
また、前記第7発明においては、m(i)>1である前記第i周波数領域別制御入力W(i)*τf(i)の組成成分として決定される状態量別制御入力τ(i,j)(j=1,2,…,m(i))が、rank(Nuw(i,1))≧rank(Nuw(i,2))≧……≧rank(Nuw(i,m(i)))という関係を満たすように前記複数種類の状態量に対応するM個の状態量別制御入力の中から選定された状態量別制御入力である。
このため、第6発明及び第7発明にいずれにおいても、第i周波数領域別制御入力W(i)*τf(i)を組成する状態量別制御入力の個数m(i)に関して、前記式02a又は式02bに従って相互に非干渉に合成することが可能な状態量別制御入力の個数m(i)をできるだけ多くするようにすることができる。
この結果、第i周波数領域で制御し得る状態量の種類数(第i周波数領域における制御対象として選定し得る状態量の種類数)を多くすることができる。ひいては、その第i周波数領域でのロボットの動作を、種々様々な要求を満足し得るように制御することが可能となる。
なお、前記第6発明は、前記第5発明と組み合わせてもよい。また、前記第7発明は、前記第4発明又は第5発明と組み合わせてもよい。
[第1実施形態]
以下、本発明の第1実施形態を図1〜図5を参照して説明する。
以下、本発明の第1実施形態を図1〜図5を参照して説明する。
図1は、本実施形態で例示するロボットの概略構成を模式的に示している。このロボット1は、脚式移動ロボットとしての2足移動ロボットである。このロボット1を構成するリンクは、大別すると、上体リンク2と、左右一対の脚リンク3R,3Lと、左右一対の腕リンク4R,4Lと、頭部5とを有する。
なお、本明細書では、ロボット1の前方に向かって右側の部材又はその部材に関連する量を示す変数に符号“R”を付加し、ロボット1の前方に向かって左側の部材又はその部材に関連する量を示す変数に符号“L”を付加する。ただし、右側、左側を特に区別する必要が無いときは、符号“R”,“L”を省略することがある。
上体リンク2は、ロボット1の基体としての下側上体10と、この下側上体10の上方に配置された上側上体11とから構成されている。上側上体11は、ヨー軸周り(Z軸周り)の回転自由度を有する上体回旋用関節12を介して下側上体10に連結されている。
左右一対の脚リンク3R,3Lは、互いに同じ構造のものである。具体的には、各脚リンク3は、下側上体10に股関節部13を介して連結された大腿部14と、この大腿部14に膝関節部15を介して連結された下腿部16と、この下腿部16に足首関節部17を介して連結された足平部18とを、該脚リンク3を構成する複数のリンクとして備える。
この場合、股関節部13は、ヨー軸周り、ピッチ軸周り(Y軸周り)及びロール軸周り(X軸周り)の回転自由度をそれぞれ有する3つの関節19,20,21により構成されている。また、膝関節部15は、ピッチ軸まわりの回転自由度を有する関節22により構成されている。また、足首関節部17は、ピッチ軸周り及びロール軸周りの回転自由度をそれぞれ有する2つの関節23,24により構成されている。従って、各脚リンク3は、本実施形態では、下側上体10に対して、6自由度の運動自由度を有する。なお、上記の説明における各脚リンク3の関節19〜24の回転軸(ロール軸、ピッチ軸、ヨー軸)は、脚リンク3を上下方向に延在させた状態での回転軸を示している。
左右一対の腕リンク4R,4Lは、互いに同じ構造のものである。具体的には、各腕リンク4は、上側上体11に肩関節部25を介して連結された上腕部26と、この上腕部26に肘関節部27を介して連結された前腕部28と、この前腕部28に手首関節部29を介して連結されたハンド30とを、該腕リンク4を構成する複数のリンクとして備える。
この場合、肩関節部25は、ピッチ軸周り、ロール軸周り、及びヨー軸周りの回転自由度をそれぞれ有する3つの関節31,32,33により構成されている。また、肘関節部27は、ピッチ軸周りの回転自由度を有する関節34により構成されている。また、手首関節部29は、ヨー軸周り、ピッチ軸周り及びロール軸周りの回転自由度をそれぞれ有する3つの関節35,36,37により構成されている。従って、各腕リンク4は、本実施形態では、上側上体11に対して、7自由度の運動自由度を有する。なお、上記の説明における各腕リンク4の関節31〜37の回転軸(ロール軸、ピッチ軸、ヨー軸)は、腕リンク4を上下方向に延在させた状態での回転軸を示している。
頭部5は、上側上体6の上方に配置されており、頸関節部38を介して上側上体6に連結されている。この場合、頸関節部38は、ピッチ軸周り及びヨー軸周りの回転自由度をそれぞれ有する2つの関節39,40により構成されている。従って、頭部5は、上側上体6に対して、2自由度の運動自由度を有する。なお、関節31,37の上記の回転軸(ピッチ軸、ヨー軸)は、頸関節部38を上下方向に伸ばした状態での回転軸を示している。
以上の構成を有するロボット1は、各脚リンク3の6個の関節19〜24を駆動することによって、各脚リンク3の空間的な運動が行なわれ、この運動によって床上を移動することが可能となっている。例えば、人の歩行動作あるいは走行動作と同様の形態(歩容)で脚リンク3R,3Lを運動させることによって、ロボット1の歩行動作あるいは走行動作が行なうことが可能である。この場合、ロボット1の各リンクは、その空間的な位置及び姿勢(空間的な向き)に関して6自由度を有する。
また、各腕リンク4の7個の関節31〜37を駆動することによって、各腕リンク4の空間的な運動が行なわれ、この運動によって各腕リンク4のハンド30を、適宜の物体に接触させる等の作業を行なうことが可能となっている。
また、本実施形態では、上体リンク2を構成する下側上体10と、上側上体11とが関節12を介して連結されているので、この関節12を駆動することによって、上体リンク2を捻ることが可能となっている。
また、頸関節部38の関節39,40を駆動することによって、頭部5のチルト動作(ピッチ軸周りの回動動作)あるいはパン動作(ヨー軸周りの回動動作)を行なうことが可能となっている。
補足すると、本実施形態のロボット1は、腕リンク4R,4L及び頭部5を有する人型のロボットであるが、これらの腕リンク4R,4L、あるいは、頭部5を備えないロボットであってもよい。また、上体リンク2の関節12を省略し、下側上体10及び上側上体11を一体に構成してもよい。また、ロボット1は、回転型の関節に限らず、直動型の関節を備えていてもよい。
図1での具体的な図示は省略するが、ロボット1には、上記の各関節を回転駆動するための動力を出力するアクチュエータとしての電動モータを含む関節駆動系(関節の駆動機構)が備えられている。そして、本実施形態では、各関節に柔軟性を持たせるために、各関節と電動モータとの間の動力伝達系には、以下に説明する如く、電動モータの動力を弾性力に変換して関節の被動部材に付与するばね部材が介装されている。
図2は、各関節毎の上記関節駆動系の概略構成を模式的に示している。この関節駆動系は、電動モータ50と、関節の被動部材51(以降、関節被動部51という)との間の動力伝達要素として、波動歯車装置等から構成された減速機52とばね部材であるトーションバー53とを備える。そして、電動モータ50の出力軸50aが減速機52とトーションバー53とを順に介して関節被動部51に連結されている。従って、この関節駆動系は、電動モータ50が出力する動力(トルク)を、減速機52とトーションバー53とを順に介して関節被動部51に伝達するように構成されている。この場合、電動モータ50が出力する動力(トルク)は、トーションバー53で弾性力(捩り力)に変換され、この弾性力が関節被動部51に駆動トルクとして付与される。
補足すると、ここで言うトーションバー53(ばね部材)は、減速機52と関節被動部51との間に付加された弾性体、又は電動モータ50と関節被動部51との間の動力伝達系に元々存在する弾性体を総称的に表現するものである。従って、減速機52が、ハーモニックドライブ(登録商標)等の波動歯車装置のように、その出力段に元々、弾性体を有するものである場合には、その弾性体もトーションバー53(ばね部材)に含まれる。
なお、関節被動部51は、これを有する関節によって互いに相対回転自在に連結された一対の部材のうちの一方である。例えば、前記膝関節部15の関節22を駆動する関節駆動系にあっては、大腿部14及び下腿部16のうちの一方のリンクが関節被動部51となり、他方のリンクに電動モータ50及び減速機52のハウジングが固定される。
以上のように本実施形態では、関節駆動系にばね部材としてのトーションバー53を備えることによって、各関節の柔軟な動作が可能となっている。
本実施形態では、以上の構造を有するロボット1の動作制御を行なうために、CPU、RAM、ROM等を含む電子回路ユニットにより構成された制御装置60と、各種のセンサとが備えられている。
この場合、センサとしては、図1に示すように、ロボット1の基体である下側上体11の角速度や姿勢角(鉛直方向に対する傾斜角度やヨー軸周りの方位角)等を計測するために該下側上体11に搭載された姿勢センサ61と、各足平部18がその接地時に受ける外力(床反力)を計測するために各脚リンク3の足首関節部24と足平部18との間に介装された力センサ62(62R,62L)と、各ハンド30がその接触対象物体から受ける外力を計測するために各腕リンク4の手首関節部29とハンド30との間に介装された力センサ63(63R,63L)とが備えられている。
上記姿勢センサ61は、例えば3軸周りの角速度を検出するジャイロセンサと3軸方向の加速度を検出する加速度センサとから構成される。また、各力センサ62,63は、それぞれ、例えば3軸方向の並進力及び3軸周りのモーメントを検出する6軸力センサにより構成される。なお、姿勢センサ61を構成する加速度センサは、下側上体11の移動速度を計測するためにも使用される。
また、本実施形態では、図2に示すように、各関節毎の関節駆動系には、電動モータ50の出力軸50aの回転角度(ロータの回転角度)を検出する角度センサとしてのロータリーエンコーダ64と、トーションバー53の両端部間の回転角度差を検出する角度センサとしての差分エンコーダ65とが備えられている。
なお、差分エンコーダ65は、本実施形態では、トーションバー53の両端部に各々固定された一対の円板65a,65bを有し、これらの円板65a,65bの一方に対する他方の相対的な回転角度に応じた信号をトーションバー53の両端部間の回転角度差に応じた信号として出力するセンサである。
これらのロータリエンコーダ64及び差分エンコーダ65は、電動モータ50の出力軸50aの回転角度(又はトーションバー53の減速機52側の一端部の回転角度(=減速機52の出力軸の回転角度))とその時間的変化率(回転角速度)、関節被動部51の回転角度とその時間的変化率(回転角速度)、トーションバー53の両端部間の回転角度差(=トーションバー53の捩れ角)等を計測するために使用されるセンサである。
なお、差分エンコーダ65の代わりに、関節被動部51の回転角度を検出するロータリーエンコーダ等の角度センサを備えるようにしてもよい。また、ロータリエンコーダ64の代わりに、減速機52の出力軸の回転角度を検出するロータリエンコーダ等の角度センサを備えるようにしてもよい。
また、以降の説明では、ロボット1の各関節の関節被動部51の回転角度を、関節の回転角度又は関節角ということがある。
制御装置60には、上記の各センサの検出出力(あるいは、該検出出力を適宜のフィルタに通してなるフィルタリング値)が入力される。そして、制御装置60は、これらの入力値から認識される所要の計測値(下側上体10の姿勢角やその時間的変化率(角速度)の計測値、下側上体10の移動速度の計測値、各足平部18やハンド30に作用する外力の計測値、各関節の回転角度やその時間的変化率(角速度)の計測値など)を使用して、ロボット1に目標とする運動を行なわせるために各関節の関節被動部51に付与すべき駆動トルクの目標値(以降、目標関節駆動トルクという)を決定すると共に、この目標関節駆動トルクに応じて各関節駆動系の電動モータ50の出力トルクを制御する処理を実行する。
以下に制御装置60の制御処理の詳細を説明する。なお、本実施形態では、ロボット1の目標とする運動が、床上を歩行動作により移動する運動である場合の制御処理を例にとって説明する。
図3に示すように、制御装置60は、実装されたプログラム等により実現される主要な機能部として、歩容生成部71、目標関節駆動トルク決定部72及び関節駆動制御部73を備える。
歩容生成部71は、ロボット1の目標運動を少なくとも含む目標歩容を生成する処理を実行する機能部である。この歩容生成部71が生成する目標運動は、ロボット1の各関節の変位量(回転角度)の目標値である目標関節角の時系列と、ロボット1の全体重心点の目標位置(以降、目標全体重心点ということがある)の時系列とを含む。
上記目標運動は、例えば、本願出願人が前記特許文献1等で提案した歩容生成手法を用いて生成される。
この歩容生成手法を概略的に説明すると、ロボット1の動作環境の床形状情報やロボット1の移動方向の要求等に基づいて、ロボット1の両足平部18R,18Lのそれぞれの目標とする着地予定位置及び着地予定姿勢と着地予定時刻とが決定される。
そして、これらの着地予定位置及び姿勢と、着地予定時刻とに応じて、各足平部18の目標位置及び目標姿勢の軌道(時系列パターン)である足平位置姿勢軌道が決定される。さらに、この足平位置姿勢軌道に基づいて、ZMP(Zero Moment Point)の目標位置(以降、目標ZMPということがある)の軌道である目標ZMP軌道とが決定される。
さらに、ロボット1の全体の運動によって発生する慣性力と、ロボット1に作用する重力との合力によって目標ZMP周りに発生するモーメントの水平成分が“0”になるという動力学的な平衡条件を満たしつつ、ロボット1の将来的な安定性を確保し得るように、ロボット1の両脚リンク3R,3L以外の各リンク(上体リンク2等)の目標位置及び目標姿勢の軌道が決定される。
そして、これらの目標位置及び目標姿勢から、ロボット1の幾何学モデル(剛体リンクモデル)を用いて、各関節の目標関節角が決定される。
なお、目標歩容は、ロボット1の実際の動作時にリアルタイムで生成されるものでなくてもよく、事前に(オフラインで)作成した目標歩容を、制御装置60に外部から無線通信等により入力したり、あるいは、ロボット1の図示しない記憶装置に事前に記憶保持しておくようにしてもよい。この場合には、歩容生成部71は不要である。
また、目標歩容には、ロボット1の目標運動以外に、足平部18R,18Lの一方又は両方に作用する全床反力(トータルの床反力)の特定成分(例えば並進力の鉛直方向成分)の目標値や、全床反力の作用点の目標位置(すなわち目標ZMP)等が含まれていてもよい。本実施形態では、歩容生成部71が出力する目標歩容には、目標ZMPが含まれる。
目標関節駆動トルク決定部72は、歩容生成部71が生成した上記目標運動を実現する(ロボット1の実際の運動を目標運動に追従させる)ための各関節の目標関節駆動トルクを決定する処理を逐次実行する機能部である。この目標関節駆動トルク決定部72は、その詳細は後述するが、本発明に係わる中核的な処理を実行する機能部である。
関節駆動制御部73は、目標関節駆動トルク決定部72が逐次決定した各関節の目標関節駆動トルク(詳しくは、後述の補正後目標関節駆動トルク↑τtc_cmdにより規定される各関節の目標関節駆動トルク)を実現するように(各関節の関節被動部51に実際に付与される駆動トルクを目標関節駆動トルクに追従させるように)、各関節駆動系の電動モータ50の出力トルクを制御する処理を逐次実行する機能部である。
この関節駆動制御部73は、例えば、次のように電動モータ50の出力トルクを制御する。すなわち、関節駆動制御部73は、前記トーションバー53の両端部間の回転角度差(トーションバー53の捩れ角)の計測値から、トーションバー53が実際に発生している弾性力トルク(トーションバー53から関節被動部51に実際に付与される駆動トルク)を逐次推定する。さらに、関節駆動制御部73は、この弾性力トルクの推定値と、目標関節駆動トルクとの偏差を“0”に収束させるように、電動モータ50の出力軸50aの目標回転角速度を逐次設定する。
次いで、関節駆動制御部73は、この目標回転角速度と、電動モータ50の出力軸50aの回転角速度の計測値との偏差を“0”に収束させるように、電動モータ50の目標出力トルクを決定する。そして、関節駆動制御部73は、この目標出力トルクを電動モータ50の通電電流の目標値に変換し、この目標値に、実際の通電電流をフィードバック制御することで、電動モータ50の実際の出力トルクを目標出力トルクに制御する。
なお、関節駆動制御部73の制御処理においては、トーションバー53の振動の影響を補償したり、関節被動部51に作用する外力の影響を補償しつつ、電動モータ50の出力トルクを制御するようにしてもよい。
次に、説明を後回しにした、前記目標関節駆動トルク決定部72の処理の詳細を説明する。
なお、以降の説明では、ベクトル(複数の成分の列)を表記する場合に、例えば“↑A”というように、変数名の先頭に“↑”を付する。また、ベクトルは、特にことわらない限り、成分を縦に並べた縦ベクトル(列ベクトル)であるとする。また、本明細書では、乗算記号として「*」を用いる。
図4に示すように、目標関節駆動トルク決定部72は、その主要な機能として、基本パラメータ群決定部81、接触部位ヤコビアン決定部82、状態量ヤコビアン決定部83、状態量目標値決定部84、状態量別制御入力決定部85、合成制御入力決定部86、コンプライアンス制御部87、及び接触部弾性変形補償部88を備える。なお、図4では、各機能部の主要な出力と、その入力先とを図示している。
目標関節駆動トルク決定部72は、これらの機能部の処理を所定の演算処理周期で逐次実行することによって、各関節の目標関節駆動トルクを逐次決定する。
以下説明すると、目標関節駆動トルク決定部72は、まず、基本パラメータ群決定部81の処理と、接触部位ヤコビアン決定部82の処理と、状態量ヤコビアン決定部83の処理と、状態量目標値決定部84の処理とを実行する。
基本パラメータ群決定部81は、ロボット1の運動と力との間の関係(運動方程式)を表現するモデルであるロボット運動モデルのパラメータを算出する処理を逐次実行する機能部である。
ここで、ロボット運動モデルは、ロボット1に外界から作用する外力(床反力等)が“0”であるとした場合、次式1−1により表現される。
M*↑q''+↑N+↑G=↑τ ……式1−1
この式1−1における↑q''は、ロボット1の一般化変数ベクトル↑qの2階微分値(=d2↑q/dt2)、Mはイナーシャ行列、↑Nは遠心力・コリオリ力依存一般化力ベクトル、↑Gは重力依存一般化力ベクトル、↑τは駆動力依存一般化力ベクトルである。
上記一般化変数ベクトル↑qは、本実施形態では、ロボット1のあらかじめ定めた1つの代表リンクの位置及び姿勢(グローバル座標系で見た位置及び姿勢)と、各関節の変位量(回転角度)とを成分として、これらの成分を並べて構成されるベクトルである。上記代表リンクは、本実施形態では、ロボット1の基体としての下側上体10である。以降、下側上体10を代表リンク10ということもある。
ここで、本実施形態の説明では、ロボット1の代表リンク10を含めて、各リンクの「位置」は、より詳しくは、該リンクに対して固定的に設定された代表点のグローバル座標系での位置を意味する。また、各リンクの「姿勢」は、より詳しくは、グローバル座標系で見た該リンクの空間的な向き(角度の次元で表現される向き)を意味する(以降の説明では、「姿勢」を「姿勢角」ということもある)。
なお、上記グローバル座標系は、ロボット1の移動環境の床(地面を含む)に対して固定された座標系等、慣性座標系(近似的に慣性系と見なせる座標系を含む)である。
そして、各リンクの「位置」及び「姿勢」(姿勢角)は、本実施形態ではそれぞれ3自由度を有するので、それぞれ3つの座標成分により表現される。このため、本実施形態での一般化変数ベクトル↑qの成分個数(すなわち↑qの自由度数)は、代表リンク10の位置及び姿勢のトータルの自由度数(=6)と、ロボット1の全ての関節のトータルの自由度数との総和の個数である。なお、図1に示した本実施形態のロボット1では、全ての関節のトータルの自由度数は“29”である。従って、↑qの自由度数は、“35”である。
上記イナーシャ行列Mは、一般化変数ベクトル↑qの2階微分値↑q''を、一般化力ベクトルに変換する行列(対称行列)、すなわち、↑q''に乗じることによって得られるベクトル(=M*↑q'')の各成分値が、↑q''の各成分に対応する力(並進力又はモーメント)の次元を有する値となる行列である。
ここで、↑qの各成分は、位置又は姿勢角又は回転角度であるから、↑q''の各成分は、並進加速度又は角加速度の次元を有する。従って、M*↑q''の符号を反転させてなるベクトル(−M*↑q'')の各成分値が、ロボット1の運動によって発生する慣性力を意味する。この場合、ベクトル(−M*↑q'')のうち、代表リンク10の「位置」の2階微分値(並進加速度)に対応する成分の慣性力は並進慣性力(並進力となる慣性力)であり、代表リンク10の「姿勢」の2階微分値(角加速度)に対応する成分の慣性力と、各関節の回転角度の2階微分値(角加速度)に対応する成分の慣性力とは、それぞれ慣性力モーメント(モーメントとなる慣性力)である。
なお、イナーシャ行列Mの逆行列M-1は対角行列となる。
一般化力ベクトルは、その各成分が、一般化変数ベクトル↑qの各成分に対応するベクトル(↑qと同数の成分からなるベクトル)である。この一般化力ベクトルは、より詳しくは、その各成分が、それに対応する↑qの成分値の変化(加速度)を発生させる力(並進力又はモーメント)となるベクトルである。この場合、一般化力ベクトルの各成分は、それに対応する↑qの成分の2階微分値に応じた慣性力と同じ種類の力である。
遠心力・コリオリ力依存一般化力ベクトル↑Nは、ロボット1の各リンクに作用する遠心力及びコリオリ力により発生する一般化力ベクトルである。また、重力依存一般化ベクトル↑Gは、ロボット1の各リンクに作用する重力により発生する一般化力ベクトルである。なお、以降の説明では、↑Nと↑Gとの和(=↑N+↑G)を非線形項一般化力ベクトルということがある。
駆動力依存一般化力ベクトル↑τは、ロボット1の各関節を駆動するアクチュエータ(本実施形態では電動モータ50)の駆動力によって発生する一般化力ベクトルである。本実施形態のロボット1では、この駆動力依存一般化力ベクトル↑τのうち、代表リンク10の位置及び姿勢に対応する成分の値は常に“0”である。また、↑τのうち、各関節の回転角度に対応する成分の値が該関節に付与する駆動トルク(以下、関節駆動トルクということがある)を意味するものとなる。
基本パラメータ群決定部81が算出するパラメータは、式1−1におけるイナーシャ行列Mと、遠心力・コリオリ力依存一般化力ベクトル↑N及び重力依存一般化力ベクトル↑G(又は非線形項一般化力ベクトル(↑N+↑G))とから成る。
補足すると、ロボット1に床反力等の外力(≠0)が作用する場合には、ロボット運動モデルは、その外力に依存する一般化力ベクトルの項(外力項)を式1−1に付加した式(後述の式5−1)によって表現される。ただし、基本パラメータ群決定部81が算出するパラメータである上記M、↑N、↑Gは、該外力には依存せずに、ロボット1の各関節の回転角度やその時間的変化率(角速度)等のロボット1の運動状態に依存してその値が定まるパラメータである。このため、基本パラメータ群決定部81を説明するための上記式1−1では外力項を省略している。
上記のパラメータM、↑N、↑G(あるいは、M及び(↑N+↑G))を算出するために、基本パラメータ群決定部81には、代表リンク(下側上体)10の移動速度の計測値と、代表リンク10の姿勢角の計測値及びその時間的変化率(角速度)の計測値と、各関節の回転角度の計測値及びその時間的変化率(角速度)の計測値とが逐次入力される。これらの計測値は、前記姿勢センサ61、ロータリーエンコーダ64、差分エンコーダ65の検出出力を基に、制御装置60において認識された計測値である。
そして、基本パラメータ群決定部81は、これらの入力された計測値を用いて、RNEA(Recursive Newton Euler Algorithm)、単位ベクトル法等の公知の手法によりM、↑N、↑G(あるいは、M及び(↑N+↑G))を逐次算出する。
補足すると、↑Nの各成分値の大きさが、それに対応する重力依存一般化力ベクトル↑Gの成分値の大きさに比して十分に微小なものとなるか、もしくは“0”となり、ひいては、↑N+↑G≒↑Gとなる場合もある。このような場合には、↑N、↑Gのうちの↑Nを無視し(非線形項一般化ベクトル(↑N+↑G)が↑Gに一致するものとして)、↑Gだけを算出するようにしてもよい。
前記接触部位ヤコビアン決定部82は、ロボット1の外界との接触部位の代表点(以下、接触部位代表点という)の運動速度と、一般化変数ベクトル↑qの1階微分値(時間的変化率)との間の関係を表現するヤコビアン(以降、接触部位ヤコビアンという)を逐次算出する処理を実行する機能部である。接触部位ヤコビアンは、換言すれば、一般化変数ベクトル↑qの各成分値の変化(微小変化)に対する接触部位代表点の空間的な位置の変化の割合い(感度)を表現する行列である。
ここで、本実施形態で説明するロボット1の目標運動は、前記したようにロボット1の歩行動作を行なう運動である。この場合、ロボット1の外界との接触部位(接地部位)は、経時的に変化する。すなわち、歩行動作におけるロボット1の接地状態は、足平部18R,18Lの一方だけが接地部位となる片脚接地状態と、両方が接地部位となる両脚接地状態と、他方だけが接触部位となる片脚接地状態とが順番に繰り返される。このため、接触部位代表点の位置は、足平部18R,18Lの接地状態に応じて変化していくべきものである。
そこで、接触部位ヤコビアン決定部82は、ロボット1の接触部位代表点の位置(より正確には、該位置を規定するパラメータ(後述の比率値rat)の値)を足平部18R,18Lの接地状態に応じて逐次決定しつつ、接触部位ヤコビアンを逐次算出する。この場合、接触部位代表点の位置は、足平部18R,18Lの接地状態の切り替わりの際等に不連続に変化することが無いように(常時、連続的に変化していくように)規定される。
以下、接触部位ヤコビアン決定部82の処理を具体的に説明する。
本実施形態では、接触部位代表点は、複数の点から構成されると共に、その個数は、足平部18R,18Lのいずれの接地状態においても一定に維持される。具体的には、接触部位代表点は、図5(a)〜(c)に黒丸点で示すように、足平部18R,18Lのいずれの接地状態においても、4つの点a1,a2,a3,a4から構成される。なお、図5(a)〜(c)では、足平部18R,18Lのうちの接地している足平部を実線で示し、離床している(空中を移動している)足平部を2点鎖線で示している。
この場合、4つの接触部位代表点a1〜a4の位置は、足平部18R,18Lの接地状態に応じた位置に規定される。
具体的には、左側足平部18Lが接地し、且つ右側足平部18Rが離床している片脚接地状態(左脚接地状態)においては、図5(a)に示す如く、左側足平部18Lの底面の所定の位置(左側足平部18Lに対して固定された位置)にあらかじめ設定された4つの所定点a1L,a2L,a3L,a4Lが、それぞれ接触部位代表点a1,a2,a3,a4として決定される。
その4つの所定点a1L〜a4Lの位置は、本実施形態では、左側足平部18Lの底面の4隅の位置である。より詳しくは、所定点a1L〜a4Lの位置は、それぞれ順番に、左側足平部18Lの底面の左前側の隅部、右前側の隅部、左後側の隅部、右後側の隅部の位置である。
また、右側足平部18Rが接地し、且つ左側脚リンク3Lの足平部18Lが離床している片脚接地状態(右脚接地状態)においては、図5(c)に示す如く、右側足平部18Rの底面の所定の位置(右側足平部18Rに対して固定された位置)にあらかじめ設定された4つの所定点a1R,a2R,a3R,a4Rが、それぞれ接触部位代表点a1,a2,a3,a4として決定される。
その4つの所定点a1R〜a4Rの位置は、左側足平部18Lの4つの所定点a1L〜a4Lと同様に、右側足平部18Rの底面の4隅の位置(左前側の隅部、右前側の隅部、左後側の隅部、右後側の隅部の位置)である。
なお、右側足平部18Rの4つの所定点a1R〜a4Rの相互の位置関係と、左側足平部18Lの4つの所定点a1L〜a4Lの相互の位置関係とは同じである。
また、両足平部18R,18Lが接地する両脚接地状態においては、図5(b)に示す如く、接触部位代表点a1,a2,a3,a4の位置は、それぞれ順番に、両足平部18R,18Lのそれぞれの上記所定点a1R,a1Lを結ぶ線分上の位置、上記所定点a2R,a2Lを結ぶ線分上の位置、上記所定点a3R,a3Lを結ぶ線分上の位置、上記所定点a4R,a4Lを結ぶ線分上の位置に決定される。
ここで、以降の説明では、任意の2つの点A,Bを結ぶ線分を線分A_Bというように表記し、その線分A_Bの長さ(線分長)をL(A_B)と表記する。
この場合、両脚接地状態での接触部位代表点a1〜a4の位置は、より詳しくは、次のように規定された位置である。
すなわち、接触部位代表点a1,a2,a3,a4の位置は、次式2−1aにより示される如く、線分長L(a1R_a1L)に対する線分長L(a1_a1L)の比率L(a1_a1L)/L(a1R_a1L)と、線分長L(a2R_a2L)に対する線分長L(a2_a2L)の比率L(a2_a2L)/L(a2R_a2L)と、線分長L(a3R_a3L)に対する線分長L(a3_a3L)の比率L(a3_a3L)/L(a3R_a3L)と、線分長L(a4R_a4L)に対する線分長L(a4_a4L)の比率L(a4_a4L)/L(a4R_a4L)とが互いに等しい比率値rat(0≦rat≦1)となるように決定される。
換言すれば、接触部位代表点a1,a2,a3,a4の位置は、次式2−1bにより示される如く、線分長L(a1R_a1L)に対する線分長L(a1_a1R)の比率L(a1_a1R)/L(a1R_a1L)と、線分長L(a2R_a2L)に対する線分長L(a2_a2R)の比率L(a2_a2R)/L(a2R_a2L)と、線分長L(a3R_a3L)に対する線分長L(a3_a3R)の比率L(a3_a3R)/L(a3R_a3L)と、線分長L(a4R_a4L)に対する線分長L(a4_a4R)の比率L(a4_a4R)/L(a4R_a4L)とが互いに等しい比率値(1−rat)となるように決定される。
L(a1_a1L)/L(a1R_a1L)=L(a2_a2L)/L(a2R_a2L)
=L(a3_a3L)/L(a3R_a3L)=L(a4_a4L)/L(a4R_a4L)=rat
……式2−1a
L(a1_a1R)/L(a1R_a1L)=L(a2_a2R)/L(a2R_a2L)
=L(a3_a3R)/L(a3R_a3L)=L(a4_a4R)/L(a4R_a4L)=1−rat
……式2−1b
そして、両脚接地状態での接触部位代表点a1〜a4の位置を規定する上記比率値ratは、本実施形態では、右側足平部18Rに実際に作用する並進床反力(床反力のうちの並進力)の鉛直方向成分Fz_act_Rと、左側足平部18Lに実際に作用する並進床反力の鉛直方向成分Fz_act_Lとの比率に応じて決定される。詳しくは、接触部位ヤコビアン決定部82は、次式2−2a又は2−2bにより、上記比率値ratを決定する。
rat=Fz_act_R/(Fz_act_R+Fz_act_L) ……式2−2a
rat=1−Fz_act_L/(Fz_act_R+Fz_act_L) ……式2−2b
なお、式2−2a又は2−2bにおいて、Fz_act_R=0である場合には、rat=0となるので、この場合の接触部位代表点a1,a2,a3,a4は、それぞれ、左側足平部18Lの前記所定点a1L,a2L,a3L,a4Lに一致することとなる。また、式2−2a又は2−2bにおいて、Fz_act_L=0である場合には、rat=1となるので、この場合の接触部位代表点a1,a2,a3,a4は、それぞれ、右側足平部18Rの前記所定点a1R,a2R,a3R,a4Rに一致することとなる。そして、左脚接地状態では、Fz_act_R=0となり、右脚接地状態では、Fz_act_L=0となる。
従って、両脚接地状態だけでなく、左脚接地状態及び右脚接地状態においても、式2−2a又は式2−2bにより比率値ratを決定し、この比率値ratに応じて、前記式2−1a又は2−1bを満たすように接触部位代表点a1,a2,a3,a4の位置を規定すれば、結果的に、左脚接地状態及び右脚接地状態においても、接触部位代表点a1,a2,a3,a4の位置が前記した位置に規定されることとなる。
そこで、本実施形態では、接触部位ヤコビアン決定部82は、上記比率値ratを接触部位代表点a1,a2,a3,a4の位置を規定するパラメータとして決定する処理を実行する。この処理は、具体的には、次のように実行される。
すなわち、上記比率値rat(以降、接触部位代表点規定比率ratという)を決定するために、接触部位ヤコビアン決定部82には、各足平部18R,18Lに作用する実際の並進床反力鉛直成分Fz_act_R,Fz_act_Lの計測値が入力される。なお、これらのFz_act_R,Fz_act_Lの計測値は、それぞれ、前記力センサ62R,62Lの検出出力等に基づき、制御装置60において認識された計測値である。
そして、接触部位ヤコビアン決定部82は、入力された並進床反力鉛直成分Fz_act_R,Fz_act_Lの計測値から、前記式2−2a又は式2−2bにより接触部位代表点規定比率ratを算出する。
このように接触部位代表点規定比率ratを決定することで、結果的に、4つの接触部位代表点a1〜a4のそれぞれの位置が連続的に変化していくように決定されることとなる。
さらに、接触部位ヤコビン決定部82は、上記のように決定した接触部位代表点規定比率ratを用いて接触部位ヤコビアンを決定する。
この場合、本実施形態では、接触部位代表点a1〜a4が4つの点から構成されるので、接触部位ヤコビアンは、接触部位代表点a1〜a4のそれぞれの位置を成分として並べて構成されるベクトル↑Xc(以降、接触部位代表点位置ベクトル↑Xcという)の時間的変化率(1階微分値)↑Xc'(=d↑Xc'/dt)、すなわち、接触部位代表点a1〜a4のそれぞれの運動速度を成分として構成されるベクトル↑Xc'と、前記一般化変数ベクトル↑qの1階微分値↑q'との間の関係を次式2−3により表現する行列Jcである。
↑Xc'=Jc*↑q' ……式2−3
なお、接触部位代表点a1〜a4のそれぞれの運動速度は3自由度を有するので、↑Xc'は、3*4=12個の成分から成るベクトルである。
ここで、接触部位代表点a1〜a4をそれぞれ右側足平部18Rの前記所定点a1R〜a4Rに一致させた場合の接触部位代表点位置ベクトル↑Xcを↑Xc_R、接触部位代表点a1〜a4をそれぞれ左側足平部18Lの前記所定点a1L〜a4Lに一致させた場合の接触部位代表点位置ベクトル↑Xcを↑Xc_Lとおく。また、↑Xc_Rの1階微分値↑Xc_R'(=d↑Xc_R/dt)と、一般化変数ベクトル↑qの一階微分値↑q'との間の関係を表すヤコビアンをJc_R、↑Xc_Lの1階微分値↑Xc_L'(=d↑Xc_L/dt)と、一般化変数ベクトル↑qの一階微分値↑q'との間の関係を表すヤコビアンをJc_Lとおく。すなわち、↑Xc_L'=Jc_L*↑q'、↑Xc_R'=Jc_R*↑q'とおく。
このとき、上記接触部位ヤコビアンJcは、次式2−4により与えられる。
Jc=rat*Jc_R+(1−rat)*Jc_L ……式2−4
そこで、本実施形態では、接触部位ヤコビアン決定部82は、式2−4の右辺のヤコビアンJc_R、Jc_Lを算出し、これらのヤコビアンJc_R、Jc_Lの算出値と、前記の如く算出した接触部位代表点規定比率ratとから式2−4の右辺の演算を行なうことで、接触部位ヤコビアンJcを算出する。
この場合、上記ヤコビアンJc_R、Jc_Lを算出するために、接触部位ヤコビアン決定部82には、下側上体10の移動速度、並びに姿勢角及びその時間的変化率(角速度)の計測値と、各脚リンク3R,3Lの各関節の回転角度及びその時間的変化率(角速度)の計測値とが入力される。そして、接触部位ヤコビアン決定部82は、これらの入力された計測値から、上記ヤコビアンJc_R、Jc_Lを算出する。
以上のように接触部位ヤコビアン決定部82の処理では、ロボット1に作用する並進床反力鉛直成分Fz_act_R,Fz_act_Lの計測値に応じて(ひいては両足平部18R,18Lの接地状態に応じて)、前記接触部位代表点規定比率ratが決定される。また、一般化変数ベクトル↑q及びその一階微分値↑q'の所要の成分の計測値に基づき、ヤコビアンJc_R、Jc_Lが算出される。そして、これらの接触部位代表点規定比率rat、ヤコビアンJc_R、Jc_Lの算出値から前記式2−4により接触部位ヤコビアンJcが算出される。
補足すると、前記式2−2a又は2−2bの右辺の分母がほぼ一定に維持されるとみなせるような場合には、式2−2a又は2−2bの右辺の分母を、ロボット1の全体に作用する重力(=ロボット1の全体質量*重力加速度定数)に置き換えた式によって、前記接触部位代表点規定比率ratを決定するようにしてもよい。
また、前記歩容生成部71が生成・出力する前記目標歩容(あるいは制御装置60に外部から与えられる目標歩容)が、足平部18R,18Lのそれぞれの並進床反力鉛直成分の目標値Fz_cmd_R,Fz_cmd_Lを含んでいる場合には、前記式2−2a又は2−2bのFz_act_R,Fz_act_Lを、それぞれ、Fz_cmd_R,Fz_cmd_Lに置き換えた式によって、前記接触部位代表点規定比率ratを決定するようにしてもよい。
あるいは、例えば、式2−2a又は2−2bのFz_act_R,Fz_act_Lを、それぞれ、Fz_cmd_R,Fz_cmd_Lに置き換えた式によって算出されるratの値にローパス特性のフィルタリング処理を施してなるフィルタリング値と、式2−2a又は2−2bの演算により算出されるratの値にローカット特性(ハイパス特性)のフィルタリング処理を施してなるフィルタリング値とを合成する(加え合わせる)ことによって、前記接触部位代表点規定比率ratを決定するようにしてもよい。
換言すれば、目標値Fz_cmd_R,Fz_cmd_Lの比率に応じて接触部位代表点規定比率ratの低周波側の成分を決定すると共に、Fz_act_R,Fz_act_Lの計測値の比率に応じて接触部位代表点規定比率ratの高周波側の成分を決定し、それらの成分を合成する(加え合わせる)ことによって、接触部位代表点規定比率ratを決定するようにしてもよい。
この場合、上記ローパス特性のフィルタリング処理及びローカット特性のフィルタリング処理としては、例えば、それぞれの伝達関数が、1/(Tc*s+1)、Tc*s/(Tc*s+1)(Tc:所定の時定数)により表されるフィルタを用いることができる。
また、本実施形態では、接触部位代表点として4つの点a1〜a4を用いたが、接触部位代表点として、3つ、もしくは5個以上の点を用いるようにすることも可能である。
前記状態量ヤコビアン決定部83は、ロボット1の歩行動作を安定に行なう上で適切な目標値に制御することが望ましい制御対象としてあらかじめ定められた複数種類の状態量に関して、その各種類の状態量と、一般化変数ベクトル↑qの1階微分値↑q'との間の関係を表現するヤコビアン(以降、状態量ヤコビアンという)を算出する処理を逐次実行する機能部である。
各種類の状態量は、その値が、一般化変数ベクトル↑qのいずれかの成分の値の変化に応じて変化する状態量である。より詳しくは、各種類の状態量は、その値(瞬時値)が、その状態量に対応する、ある状態量ヤコビアンを、一般変数ベクトル↑qの1階微分値↑q'に乗じてなる値となるような状態量である。
本実施形態では、制御対象(制御しようとする対象)とする状態量の種類数は例えば2つである。そして、その1つの種類の状態量(以降、第1種類状態量という)として、グローバル座標系で見たロボット1の全並進運動量↑Ptotalと全角運動量↑Ltotal(詳しくはグローバル座標系の所定の基準点周りの全角運動量↑Ltotal)との組を使用する。なお、グローバル座標系としては、本実施形態では、例えば、ロボット1の足平部18R,18Lのうちの支持脚側の足平部の接地位置に応じた位置(例えば接地面内の位置)に原点が設定される座標系(所謂、支持脚座標系)が使用される。
上記全並進運動量↑Ptotalは、より詳しくは、ロボット1の各リンクの重心点の運動量(=リンクの質量*該リンクの重心点の移動速度)をロボット1の全てのリンクについて加え合わせてなる並進運動量である。換言すれば、↑Ptotalは、ロボット1の全体重心の並進運動量である。この↑Ptotalは、本実施形態では、3自由度のベクトル(ロール軸方向、ピッチ軸方向及びヨー軸方向の3軸方向の成分から成るベクトル)である。
なお、以降の説明では、ロール軸は、ロボット1の前後方向での水平軸、ピッチ軸はロボット1の左右方向での水平軸、ヨー軸は鉛直方向(重力方向)の軸を意味する。
また、上記全角運動量↑Ltotalは、ロボット1の各リンクの重心点の並進運動による上記基準点周りの角運動量(=基準点に対するリンクの重心点の位置ベクトルと、該重心点の並進運動量とのベクトル積)と、各リンクの重心点周りの回転運動による角運動量(=リンクの慣性モーメント*該リンクの重心点周りの角速度)とを、全てのリンクについて加え合わせてなる角運動量である。この全角運動量↑Ltotalは、本実施形態では、ヨー軸周りの成分を除く2自由度のベクトル(ロール軸及びピッチ軸の2軸周りの成分から成るベクトル)である。なお、上記基準点は、前記グローバル座標系にあらかじめ設定された点(例えば前記支持脚座標系の原点)である。
また、他の1つの種類の状態量(以降、第2種類状態量という)として、本実施形態では、ロボット1の各関節の変位速度(回転角速度)を使用する。
状態量ヤコビアン決定部83は、上記の各種類の状態量毎に、状態量ヤコビアンを決定する。この場合、上記第1種類状態量に対応する状態量ヤコビアン(以降、第1状態量ヤコビアンという)は、全並進運動量↑Ptotalと全角運動量↑Ltotalとを成分として並べて構成されるベクトル↑S1(=[↑P,↑L]T。以降、第1種類状態量ベクトル↑S1という)と、前記一般化変数ベクトル↑qの1階微分値↑q'との間の関係を次式3−1により表現する行列Js1である。
↑S1=Js1*↑q' ……式3−1
なお、本実施形態では、全並進運動量↑Ptotal及び全角運動量↑Ltotalは、それぞれ、3自由度、2自由度のベクトルであるので、↑S1は、3+2=5個の成分から成るベクトルである。
この第1状態量ヤコビアンJs1を算出するために、状態量ヤコビアン決定部83には、代表リンク(下側上体)10の移動速度、並びに姿勢角及びその時間的変化率(角速度)の計測値と、各関節の回転角度及びその時間的変化率(角速度)の計測値とが入力される。そして、状態量ヤコビアン決定部83は、これらの入力された計測値から、第1状態量ヤコビンJs1を算出する。
また、上記第2種類状態量に対応する状態量ヤコビアン(以降、第2状態量ヤコビアンという)は、ロボット1の各関節の回転角速度を成分として並べて構成されるベクトル↑S2(以降、第2種類状態量ベクトル↑S2という)と、前記一般化変数ベクトル↑qの1階微分値↑q'との間の関係を次式3−2により表現する行列Js2である。
↑S2=Js2*↑q' ……式3−2
この場合、↑q'は、各関節の回転角度の一階微分値、すなわち、回転角速度を成分として含むベクトルであるので、上記第2種類状態量ベクトル↑S2の各成分は、それぞれに対応する↑q'の成分に一致する。従って、本実施形態では、第2状態量ヤコビアンJs2は、その各成分値が固定値(一定値)となるヤコビアンである。
このため、状態量ヤコビアン決定部83は、第2状態量ヤコビアンJs2を算出する処理は実質的には行わず、制御装置60であらかじめ記憶保持された第2状態量ヤコビアンJs2を出力する。
前記状態量目標値決定部84は、前記第1種類状態量及び第2種類状態量に関する目標値を決定する処理を実行する機能部である。
本実施形態では、第1種類状態量に関する目標値は、前記第1種類状態量ベクトル↑S1の時間的変化率(1階微分値)↑S1'(=d↑S1/dt)の目標値↑S1'_cmdである。また、第2種類状態量に関する目標値は、前記第2種類状態量ベクトル↑S2の時間的変化率(1階微分値)↑S2'(=d↑S2/dt)の目標値↑S2'_cmdである。
この場合、↑S1'は、ロボット1の全並進運動量↑Ptotalの1階微分値↑Ptotal'(=d↑Ptotal/dt)と、全角運動量↑Ltotalの1階微分値↑Ltotal'(=d↑Ltotal/dt)とを成分とするベクトル(=[↑Ptotal',↑Ltotal']T)となるので、↑S1'の目標値↑S1'_cmd(以下、第1種類状態量目標値ベクトル↑S1'_cmdという)は、ロボット1の全体の運動によって発生するトータルの慣性力(ただし、ヨー軸周りのモーメント成分を除く)の目標値に相当するものとなる。
より詳しくは、第1種類状態量目標値ベクトル↑S1'_cmdは、↑Ptotal'の目標値と↑Ltotal'の目標値とを成分とするベクトルとなる。そして、↑S1'_cmdのうちの↑Ptotal'の目標値は、ロボット1の全体の運動によって発生するトータルの並進慣性力の目標値(ロボット1の全体重心の並進慣性力の目標値)に相当し、↑Ltotal'の目標値は、ロボット1の全体の運動によって発生するトータルのモーメント慣性力(ただし、ヨー軸周りの成分を除く)の目標値に相当するものとなる。
また、↑S2'の目標値↑S2'_cmd(以下、第2種類状態量目標値ベクトル↑S2'_cmdという)は、ロボット1の各関節の回転角加速度の目標値である。
そして、本実施形態では、状態量目標値決定部84は、上記第1種類状態量目標値ベクトル↑S1'_cmdと第2種類状態量目標値ベクトル↑S2'_cmdとを以下に説明する如く決定する。
すなわち、状態量目標値決定部84は、第1種類状態量目標値ベクトル↑S1'_cmdのうちの、全並進運動量↑Ptotalの時間的変化率↑Ptotal'の目標値↑Ptotal'_cmdと、全角運動量↑Ltotalの時間的変化率↑Ltotal'の目標値↑Ltotal'_cmdとを各別の演算処理により算出する。
さらに詳細には、↑Ptotal'_cmdの算出処理に関しては、状態量目標値決定部84は、グローバル座標系でのロボット1の全体重心点の目標位置↑rgc_cmdと実際の位置↑rgc_actの計測値との偏差↑Δrgc(=↑rgc_cmd−↑rgc_act)と、その偏差↑Δrgcの1階微分値↑Δrgc'(=d↑Δrgc/dt)とを算出する。
この場合、ロボット1の全体重心点の目標位置↑rgc_cmdは、前記歩容生成部71から状態量目標値決定部84に入力される。また、該全体重心点の実際の位置↑rgc_actの計測値は、各関節の回転角度の計測値等から公知の手法(例えばカルマンフィルタを使用した手法)によって、制御装置60が逐次算出し、その算出した計測値が、状態量目標値決定部84に入力される。
なお、↑rgc_cmd及び↑rgc_actは、ロール軸方向、ピッチ軸方向及びヨー軸方向の3軸方向の位置から成るベクトル(3自由度の位置ベクトル)である。
そして、状態量目標値決定部84は、上記偏差↑Δrgcを“0”に収束させるためのフィードバック操作量を所定のフィードバック制御則により算出し、その算出したフィードバック操作量を↑Ptotal'の↑目標値Ptotal'_cmdとして決定する。
この場合、具体的には、状態量目標値決定部84は、上記偏差↑ΔrgcをPD則(比例・微分則)によって“0”に収束させるように↑Ptotal'_cmdを決定する。すなわち、状態量目標値決定部84は、偏差↑Δrgcとその1階微分値↑Δrgc'とから、次式4−1より↑Ptotal'_cmdを決定する。
↑Ptotal'_cmd=Kp1*↑Δrgc+Kv1*↑Δrgc' ……式4−1
なお、式4−1におけるKp1,Kv1は、それぞれあらかじめ設定された所定値の比例ゲイン、微分ゲインである。これらのゲインKp1,Kv1は対角行列(本実施形態では3次の対角行列)によって表現される。
次に、Ltotal'_cmdの算出処理に関しては、状態量目標値決定部84は、グローバル座標系でのロボット1の目標ZMPの水平位置(ロール軸方向の位置及びピッチ軸方向の位置)と、ロボット1の全体重心点の実際の水平位置↑rgc_act_xy(=前記↑rgc_actのロール軸方向成分及びピッチ軸方向成分)の計測値との偏差↑Δrg_xyを算出する。この場合、ロボット1の目標ZMPの水平位置は、前記歩容生成部71から状態量目標値決定部84に入力される。
さらに、状態量目標値決定部84は、ロボット1の全体重心点が、目標ZMPを支点とする倒立振子の質点であると見なした場合における該倒立振子の鉛直方向に対する傾斜角度↑θpend(ロール軸及びピッチ軸の2軸周りの傾斜角度)と、その1階微分値↑θpend'(=d↑θpend/dt)を上記偏差↑Δrg_xyから算出する。具体的には、↑θpendのロール軸周り成分及びピッチ軸周り成分をそれぞれθpend_x、θpend_yとしたとき、θpend_x,θpend_yは、それぞれ、上記偏差↑Δrg_xyのピッチ軸方向成分Δrg_y、ロール軸方向成分Δrg_xから次式4−2a,4−2bにより算出される。
θpend_x=tan-1(Δrg_y/h)≒Δrg_y/h ……式4−2a
θpend_y=tan-1(Δrg_x/h)≒Δrg_x/h ……式4−2b
そして、これらのθpend_x,θpend_yの1階微分値(時間的変化率)を算出することで、↑θpend_xy'の各成分が算出される。
なお、式4−2a,4−2bにおける“h”は、ロボット1の全体重心点の平均的な高さとしてあらかじめ設定された所定値である。
次いで、状態量目標値決定部84は、上記の如く算出した倒立振子の傾斜角度↑θpend(=[θpend_x,θpend_y]T)を“0”に収束させるためのフィードバック操作量(制御入力)を所定のフィードバック制御則により算出し、その算出したフィードバック操作量を↑Ltotal'の目標値↑Ltotal'_cmdとして決定する。
この場合、具体的には、状態量目標値決定部84は、上記傾斜角度↑ΔθpendをPD則(比例・微分則)によって“0”に収束させるように↑Ltotal'_cmdを決定する。すなわち、状態量目標値決定部84は、傾斜角度↑θpendとその1階微分値↑θpend'とから、次式4−3により↑Ltotal'_cmdを決定する。
↑Ltotal'_cmd=Kp2*↑θpend+Kv2*↑θpend' ……式4−3
なお、式4−3におけるKp2,Kv2は、それぞれあらかじめ設定された所定値の比例ゲイン、微分ゲインである。これらのゲインKp2,Kv2は対角行列(本実施形態では2次の対角行列)によって表現される。
以上、説明した処理によって、↑Ptotal'の目標値↑Ptotal'_cmdと、↑Ltotal'の目標値↑Ltotal'_cmdとが決定され、これにより、第1種類状態量目標値ベクトル↑S1'_cmd(=[↑Ptotal'_cmd,↑Ltotal'cmd]T)が決定されることとなる。
この場合、本実施形態では、↑S1'_cmdのうちの↑Ptotal'_cmdは、ロボット1の全体重心点の位置↑rgc_actを目標位置↑rgc_cmdに収束させるように決定される。
また、↑S1'_cmdのうちの↑Ltotal'_cmdは、ロボット1の実際の全体重心点を倒立振子の質点と見なした場合の該倒立振子の傾斜角度↑θpendを“0”に収束させる(換言すれば、ロボット1の実際の全体重心点を目標ZMPの鉛直方向上方の位置に収束させる)ように決定される。
補足すると、θpend_x,θpend_yは、それぞれ、Δrg_y,Δrg_xにほぼ比例する。このため、θpend_x,θpend_yの算出処理を省略し、Δrg_y,Δrg_xをフィードバック制御則(例えばPD則)により“0”に収束させるように、↑Ltotal'_cmd_xyを決定するようにしてもよい。例えば、式4−3の右辺の↑θpendをベクトル[Δrg_y,Δrg_x]Tに置き換えた形の式によって、↑Ltotal'_cmd_xyを算出するようにしてもよい。
次に、第2種類状態量目標値ベクトル↑S2'cmdの算出処理に関しては、状態量目標値決定部84は、ロボット1の各関節の回転角度(以降、関節角ということがある)の所要の目標値θcmdと、該関節の実際の回転角度θactの計測値との偏差Δθ(=θcmd−θact)と、その1階微分値Δθ'(=dΔθ/dt)とを算出する。この場合、各関節角の目標値θcmdは、制御装置60の前回の演算処理周期において、歩容生成部71で生成された目標関節角を、詳細を後述する接触部弾性変形補償部88によって補正してなる値(前回値)である。
さらに、状態量目標値決定部84は、各関節の上記偏差Δθを“0”に収束させるためのフィードバック操作量を所定のフィードバック制御則により算出し、その算出したフィードバック操作量を成分とするベクトル(各関節毎のフィードバック操作量を並べたベクトル)を第2種類状態量目標値ベクトル↑S2'_cmdとして決定する。
この場合、具体的には、状態量目標値決定部84は、上記偏差ΔθをPD則(比例・微分則)によって“0”に収束させるように↑S2'_cmdを決定する。すなわち、状態量目標値決定部84は、各関節毎の上記偏差Δθを並べたベクトル↑ΔΘと、上記偏差Δθの1階微分値Δθ'を並べたベクトル↑ΔΘ'(すなわち↑ΔΘの1階微分値)とから、次式4−4により↑S2'_cmdを決定する。
↑S2'cmd=Kp3*↑ΔΘ+Kv3*↑ΔΘ' ……式4−4
なお、式4−4におけるKp3,Kv3は、それぞれあらかじめ設定された所定値の比例ゲイン、微分ゲインである。これらのゲインKp3,Kv3は対角行列(↑S2'_cmdと同じ次数の対角行列)によって表現される。
以上、説明した処理によって、第2種類状態量目標値ベクトル↑S2'_cmdが決定されることとなる。この場合、本実施形態では、↑S2'_cmdは、ロボット1の各関節の実際の関節角θactを所要の目標値θcmdに収束させるように決定される。
以上のように、基本パラメータ群決定部81、接触部位ヤコビアン決定部82、状態量ヤコビアン決定部83及び状態量目標値決定部84の処理を実行した後、目標関節駆動トルク決定部72は、次に、状態量別制御入力決定部85の処理を実行する。
この状態量別制御入力決定部85は、各種類の状態量に関する目標値(前記第1種類状態量目標値ベクトル↑S1'_cmd及び第2種類状態量目標値ベクトル↑S2'_cmd)を実現するために必要なロボット1の各関節の目標関節駆動トルクを各種類の状態量毎に決定する処理を逐次実行する機能部である。以降、前記第1種類状態量に対応する各関節の目標関節駆動トルクを成分として並べたベクトルを第1目標関節駆動トルク↑τ1_cmd、前記第2種類状態量に対応する各関節の目標関節駆動トルクを成分として並べたベクトルを第2目標関節駆動トルク↑τ2_cmdと表記する。
なお、本実施形態の説明では、↑τ1_cmd,↑τ2_cmdは、前記駆動力依存一般化力ベクトル↑τと同じ次数(一般化変数ベクトル↑qと同じ次数)のベクトルであるとする。この場合、↑τ1_cmd,↑τ2_cmdのそれぞれの成分のうち、ロボット1の代表リンク(下側上体10)の位置及び姿勢に対応する成分は、いずれも“0”である。
ここで、状態量別制御入力決定部85の処理に使用する基本式を以下に説明しておく。
ロボット1が外界との接触部位に外力を受ける場合、ロボット1の運動と力との関係(運動方程式)を表現するロボット運動モデルは、外力が“0”である場合のロボット運動ロボットモデルを表す前記式1−1に、外力の影響分を付加してなる次式5−1により表される。
M*↑q''+↑N+↑G+JcT*↑Fc=↑τ ……式5−1
この式5−1における↑Fcは、ロボット1の外界との接触部位(本実施形態では足平部18R,18L)に作用する外力であり、JcTは前記接触部位ヤコビアンJcの転置行列である。
この式5−1は、以下に説明する如く、ロボット1が外力を受ける接触部位の運動を規定する(該運動を既定の運動に一致させるものとする)ことによって、後述の式5−11に変形することができる。
以下説明すると、式5−1の両辺に、行列Jc*M-1を乗じることによって、次式5−2が得られる。
Jc*↑q''+Jc*M-1*(↑N+↑G)+Jc*M-1*JcT*↑Fc
=Jc*M-1*↑τ
……式5−2
一方、前記接触部位代表点a1〜a4の運動速度(=接触部位代表点位置ベクトル↑Xcの1階微分値↑Xc')と、一般化変数ベクトル↑qの1階微分値↑q'との間の関係を表現する前記式2−3の両辺を1階微分した後、式変形を行なうことで、次式5−3が得られる。
Jc*↑q''=↑Xc''−Jc'*↑q' ……式5−3
この式5−3におけるJc'は前記接触部位ヤコビアンJcの1階微分値(=dJc/dt)、↑q''は↑qの2階微分値(=d2↑q/dt2)である。
さらに、この式5−3を式5−2に代入することで、次式5−4が得られる。
↑Xc''−Jc'*↑q'+Jc*M-1*(↑N+↑G)
+Jc*M-1*JcT*↑Fc=Jc*M-1*↑τ
……式5−4
ここで、ある重み行列Wat(対角行列)を用いて次式5−5,5−6によりそれぞれ定義される行列Jc#と行列Rcとを導入する。
Jc#≡WatT*JcT*RcT ……式5−5
Rc≡((Jc*Wat*JcT)-1)T ……式5−6
なお、式5−5により定義される行列Jc#は、接触部位ヤコビアンJcの一般化重み付き擬似逆行列としての意味を持つものである。
この場合、上記重み行列Watとして、例えば、前記イナーシャ行列Mの逆行列M-1を採用することができる。そして、このようにWat=M-1とした場合には、(Jc#)T=Rc*Jc*Wat=Rc*Jc*M-1となる。このため、前記式5−4の両辺に行列Rc(但し、Wat=M-1)を乗じることで、次式5−7が得られる。
すなわち、式5−5,5−6でWat=M-1として得られる次式5−8,5−9によりそれぞれ定義される行列Jc#、Rcを用いることによって、前記式5−1は、次式5−7に変形される。
Rc*↑Xc''−Rc*Jc'*↑q'+(Jc#)T*(↑N+↑G)+↑Fc
=(Jc#)T*↑τ
……式5−7
但し、
Jc#≡(M-1)T*JcT*RcT ……式5−8
Rc≡((Jc*M-1*JcT)-1)T ……式5−9
Jc#≡(M-1)T*JcT*RcT ……式5−8
Rc≡((Jc*M-1*JcT)-1)T ……式5−9
なお、M-1は対角行列であるので、(M-1)T=M-1である。
ここで、前記接触部位代表点a1〜a4の運動を規定し、前記接触部位代表点位置ベクトル↑Xcの2階微分値↑Xc''(=d2↑Xc/dt2)をある既定の目標値↑Cに一致させる場合(接触部位代表点a1〜a4の運動加速度を、それぞれ、ある既定の目標値に一致させる場合)を想定する。すなわち、↑Xc''=↑Cとなるように、接触部位である足平部18R,18Lの運動が規定される場合を想定する。
この場合、上記式5−7から、次式5−10が得られる。
↑Fc=(Jc#)T*(↑τ−↑N−↑G)−Rc*↑C−Rc*Jc'*↑q'
……式5−10
この式5−10を前記式5−1に代入し、さらに、次式5−12,5−13,5−14,5−15によりそれぞれ定義される行列Tc,ベクトル↑Cc,行列Pc及び一般化力ベクトル↑τcmpnとを用いて整理することによって、次式5−11が得られる。
M*↑q''+Tc*↑q'=Pc*(↑τ−↑τcmpn) ……式5−11
但し、
Tc≡−JcT*Rc*Jc' ……式5−12
↑Cc≡−JcT*Rc*↑C …式51−13
Pc≡I−JcT*(Jc#)T ……式5−14
↑τcmpn≡(↑N+↑G)−Pc-1*Cc ……式5−15
上記式5−11は、接触部位代表点a1〜a4の運動を規定した場合のロボット1の運動方程式を、外力↑Fcを含まない形態で表現する式である。
次に、ロボット1の前記一般化変数ベクトル↑qの1つ以上の成分に依存する値を有する任意の状態量↑Sを想定し、この↑Sと、一般化変数ベクトル↑qの1階微分値↑q'との間の関係が、あるヤコビアンJsを用いて、次式5−16により表現されるものとする。
↑S=Js*↑q' ……式5−16
この式5−16は、前記式3−1,3−2を一般化して表現したものである。なお、この式5−16での↑Sは、ベクトルである必要はない(スカラーでもよい)が、ここでの説明では、前記式3−1,3−2との整合を採るために、便宜上、ベクトルとして表記している。
ここで、上記式5−16から得られる次式5−17の関係式を用いることによって、前記式5−11は、次式5−20に変形される。
Js*↑q''=↑S'−Js'*↑q' ……式5−17
↑S'+(Js*M-1*Tc−Js')*↑q'
=(Js*M-1*Pc)*(↑τ−↑τcmpn)
……式5−20
従って、この式5−20における行列Js,M,Tc,Js',Pcの値と、↑S',↑q',↑τcmpnの値とを決定すれば、この式5−20に基づいて、駆動力依存一般化力ベクトル↑τ(ひいては、各関節の関節駆動トルク)を決定することができることとなる。
ここで、式5−20の右辺の行列(Js*M-1*Pc)(以降、As≡Js*M-1*Pcと定義する)は、正則行列になるとは限らない(Asの逆行列As-1が存在するとは限らない)。
そこで、本実施形態では、行列Asから次式5−21a又は5−21bにより決定される行列Bsを使用することによって、次式5−22により↑τを算出することとする。
As(≡Js*M-1*Pc)が正則行列である場合
Bs=As-1 ……式5−21a
As(≡Js*M-1*Pc)が正則行列でない場合
Bs=AsT*(k*I+As*AsT)-1 ……式5−21b
但し、
k:(k*I+As*AsT)が正則行列になるように決定される正の実数
↑τ=Bs*(↑S'+(Js*M-1*Tc−Js')*↑q')+↑τcmpn
……式5−22
この式5−22が、本実施形態において、第1種類状態量に対応する前記第1目標関節駆動トルク↑τ1_cmdと第2種類状態量に対応する第2目標関節駆動トルク↑τ2_cmdとを算出するために使用する基本式である。この場合、前記式5−21bにより決定される行列Bsは、行列Asの擬似逆行列としての意味を持つものである。
なお、以降の説明において、任意の行列Aに対して、“A-1”は、Aが正則行列でない場合には、次式5−23により決定される行列Bに一致する行列(擬似逆行列)を意味するものとする。また、行列Aが正則行列である場合には、A-1は、本来の意味でのAの逆行列を意味するものとする。そして、Aが正則行列である場合及び正則行列でない場合のいずれの場合でも、便宜上、A-1をAの逆行列と称する。
B=AT*(k*I+A*AT)-1 ……式5−23
但し、
k:(k*I+A*AT)が正則行列になるように決定される正の実数
前記式5−22における行列Bsは、上記の意味でのAsの逆行列(=As-1)である。
補足すると、前記式5−20の両辺に適当な行列を乗算した上で、↑τを算出する式を構築してもよい。例えば、Rsc≡(Js*PcT*M-1*JsT)-1)Tにより定義される行列Rscを式5−20の両辺に左側から乗じてなる式を基に、上記と同様に、↑τを算出する式を構築してもよい。この場合、行列Rscを式5−20の両辺に左側から乗じることによって、式5−20の両辺は、一般化力ベクトルの次元に変換されることとなる。また、↑τを算出する式は、式5−22の行列Bsを、行列((Rsc*Js*M-1*Pc)-1*Rsc)に置き換えた式となる。
以上説明したことを基礎として、以下に、状態量別制御入力決定部85の処理を具体的に説明する。
状態量別制御入力決定部85には、各種類の状態量に対応する目標関節駆動トルク(第1目標関節駆動トルク↑τ1_cmd及び第2目標関節駆動トルク↑τ2cmd)を算出するために、基本パラメータ群決定部81で決定されたイナーシャ行列M、遠心力・コリオリ力依存一般化力ベクトル↑N及び重力依存一般化力ベクトル↑G(又は非線形項一般化力ベクトル(↑N+↑G))と、接触部位ヤコビアン決定部82で決定された前記接触部位ヤコビアンJcと、状態量ヤコビアン決定部83で決定された各種類の状態量に対応する状態量ヤコビアン(前記第1状態量ヤコビアンJs1及び第2状態量ヤコビアンJs2)と、状態量目標値決定部84で決定された各種類の状態量に関する目標値(前記第1種類状態量目標値ベクトル↑S1'_cmd及び第2種類状態量目標値ベクトル↑S2'_cmd)とが逐次入力される。
この他、状態量別制御入力決定部85には、一般化変数ベクトル↑qの時間的変化率(1階微分値)↑q'の計測値(下側上体10の移動速度、並びに姿勢角の時間的変化率(角速度)の計測値、各関節の回転角度の時間的変化率(角速度)の計測値)も逐次入力されると共に、後述の接触部位代表点目標加速度↑Cも入力される。
そして、状態量別制御入力決定部85は、式5−22の右辺の演算に使用する各パラメータの値を所要の入力値を用いて決定し、その決定したパラメータを用いて式5−22の右辺の演算を行なうことで、第1目標関節駆動トルク↑τ1_cmd及び第2目標関節駆動トルク↑τ2_cmdをそれぞれ算出する。この場合、式5−22の右辺のパラメータのうちの一部は、↑τ1_cmd及び↑τ2_cmdの両方に対して共用され、残りのパラメータは、↑τ1_cmd及び↑τ2_cmdのそれぞれに対して各別に決定される。
さらに詳細には、状態量別制御入力決定部85は、式5−22の演算に必要な行列Tc,Pcと、一般化力ベクトル↑τcmpnとを所定の入力値を用いて算出する処理を実行する。これらのTc,Pc,↑τcmpnは、↑τ1_cmd及び↑τ2_cmdの両方に対して共用されるパラメータである。
この場合、状態量別制御入力決定部85は、入力されたイナーシャ行列M、遠心力・コリオリ力依存一般化力ベクトル↑N及び重力依存一般化力ベクトル↑G(又は非線形項一般化力ベクトル(↑N+↑G))と、接触部位ヤコビアンJcとを用いて、前記式5−8,5−9,5−12,5−13,5−14,5−15の定義式に基づいて、Tc、Pc、↑τcmpnを算出する。
具体的には、状態量別制御入力決定部85は、入力されたイナーシャ行列M及び接触部位ヤコビアンJcから、前記定義式5−8,5−9に基づいて、行列Jc#,Rcを算出する。さらに、これらの行列Jc#,Rcと、入力されたJcとから、前記定義式5−12,5−14に基づいて、行列Tc,Pcを算出する。なお、この場合、Jc'、すなわち、Jcの時間的変化率(1階微分値)は、入力されたJcの時系列から算出される。
また、状態量別制御入力決定部85は、入力されたJc及び算出したRcと、前記接触部位代表点位置ベクトル↑Xcの2階微分値↑Xc''(接触部位代表点a1〜a4の運動加速度)の目標値↑Cとから、前記式5−13の定義式によりベクトルCcを算出する。以降、目標値↑Cを接触部位代表点目標加速度↑Cという。
ここで、本実施形態では、上記接触部位代表点目標加速度↑Cの成分である4つの接触部位代表点a1〜a4のそれぞれの目標加速度は、いずれの接触部位代表点a1〜a4についても、あらかじめ“0”に設定されている。すなわち、本実施形態では、各接触部位代表点a1〜a4が床に対して静止するか、もしくは一定の速度で動くことを、接触部位としての足平部18R,18Lの運動に関する目標とされている。
そして、状態量別制御入力決定部85は、上記接触部位代表点目標加速度↑Cを用いて前記式5−13の定義式によりベクトル↑Ccを算出する。なお、本実施形態では、目標加速度↑Cは“0”ベクトルであるから、ベクトル↑Ccの各成分は、いずれも“0”となる。従って、実際上は、↑Ccを算出する処理は不要であり、単に↑Ccの各成分を“0”に設定しておけばよい。
さらに状態量別制御入力決定部85は、上記↑Ccと、算出したPcと、入力された↑N及び↑Gとから前記式5−15の定義式に基づいて↑τcmpnを算出する。
↑τ1_cmd及び↑τ2_cmdの両方に対して共用されるTc、Pc、↑τcmpnの算出処理は以上説明した如く実行される。
補足すると、↑N+↑G≒↑Gとなる場合には、前記式5−15の↑Nを省略した式によって、↑τcmpnを算出するようにしてもよい。
また、状態量別制御入力決定部85は、第1種類状態量に対応する第1目標関節駆動トルク↑τ1_cmdを算出するために、式5−22の演算に用いる行列Bsを算出する処理を実行する。なお、以降の説明では、第1種類状態量に対応する行列BsをBs1と表記する。
この場合、状態量別制御入力決定部85は、入力されたイナーシャ行列M及び第1状態量ヤコビアンJs1と、前記Pcの算出値とから、行列Bs1を算出する。より詳しくは、行列(Js1*M-1*Pc)(≡As)の逆行列As-1がBs1として決定される。なお、前記した如く、Asが正則行列でない場合には、As-1は、前記式5−23に基づいて算出される擬似逆行列である。
そして、状態量別制御入力決定部85は、Bs1,Tc,↑τcmpnの算出値と、入力された第1種類状態量目標値ベクトル↑S1'_cmd、第1状態量ヤコビアンJs1及びイナーシャ行列Mと、↑q'の計測値とを用いて、前記式5−22の右辺の演算(詳しくは、式5−22の右辺のBs、↑S'、Js、Js'、↑qをそれぞれ、Bs1、Rs1、↑S1'cmd、Js1、Js1'、↑q'の計測値に置き換えた式の演算)を行なうことによって、↑τを算出し、この↑τの算出値を第1目標関節駆動トルク↑τ1_cmdとして決定する。なお、この場合、Js1'(=Js1の1階微分値)は、第1状態量ヤコビアンJs1の時系列から算出される。
また、状態量別制御入力決定部85は、第2種類状態量に対応する第2目標関節駆動トルク↑τ2_cmdを算出するために、式5−22の演算に用いる行列Bsを算出する処理を実行する。なお、以降の説明では、第2種類状態量に対応する行列BsをBs2と表記する。
この場合、状態量別制御入力決定部85は、入力されたイナーシャ行列M及び第2状態量ヤコビアンJs2と、前記Pcの算出値とから、前記式5−21に基づいて、行列Bs2を算出する。より詳しくは、行列(Js2*M-1*Pc)(≡As)の逆行列As-1がBs2として決定される。なお、Bs1を算出する場合と同様、Asが正則行列でない場合には、As-1は、前記式5−23に基づいて算出される擬似逆行列である。
そして、状態量別制御入力決定部85は、Bs2,Tc,↑τcmpnの算出値と、入力された第2種類状態量目標値ベクトル↑S2'_cmd、第2状態量ヤコビアンJs2及びイナーシャ行列Mと、↑q'の計測値とを用いて、前記式5−22の右辺の演算(詳しくは、式5−22の右辺のBs、Rs、↑S'、Js、Js'、↑q'をそれぞれ、Bs2、Rs2、↑S2'_cmd、Js2、Js2'、↑q'の計測値に置き換えた式の演算)を行なうことによって、↑τを算出し、この↑τの算出値を第2目標関節駆動トルク↑τ2_cmdとして決定する。なお、この場合、Js2'(=Js2の1階微分値)は、第2状態量ヤコビアンJs2の時系列から算出される。
以上説明した状態量別制御入力決定部85の処理によって、本実施形態では、第1目標関節駆動トルク↑τ1_cmdは、M、↑N、↑G、Jc、Js1の算出値(又はM、↑G、Jc、Js1の算出値)と、第1種類状態量に関する目標値である第1種類状態量目標値ベクトル↑S1'_cmdと、↑q'の計測値と、接触部位代表点目標加速度↑Cとから決定される。
この場合、↑τ1_cmdは、接触部位代表点a1〜a4の運動加速度を接触部位代表点目標加速度↑Cにより規定される加速度に一致させつつ、第1種類状態量ベクトル↑S1の時間的変化率↑S1'を、第1種類状態量目標値ベクトル↑S1'_cmdに一致させるように決定される。
このとき、前記したように、↑S1'_cmdのうちの↑Ptotal'_cmdは、ロボット1の全体重心点の位置↑rgc_actを目標位置↑rgc_cmdに収束させるように決定され、また、↑S1'_cmdのうちの↑Ltotal'_cmdは、ロボット1の実際の全体重心点を倒立振子の質点と見なした場合の該倒立振子の傾斜角度↑θpendを“0”に収束させるように決定されている。このため、↑τ1_cmdは、結果的に、ロボット1の全体重心点を適切な位置に保つことができるように決定されることとなる。
また、第2目標関節駆動トルク↑τ2_cmdは、M、↑N、↑G、Jc、Js2の算出値(又はM、↑G、Jc、Js2の算出値)と、第2種類状態量に関する目標値である第2種類状態量目標値ベクトル↑S2'_cmdと、↑q'の計測値と、接触部位代表点目標加速度Cとから決定される。
この場合、↑τ2_cmdは、接触部位代表点a1〜a4の運動加速度を接触部位代表点目標加速度↑Cにより規定される加速度に一致させつつ、第2種類状態量ベクトル↑S2の時間的変化率↑S2'を、第2種類状態量目標値ベクトル↑S2'_cmdに一致させるように決定される。
このとき、前記したように、↑S2'_cmdは、ロボット1の各関節の実際の回転角度θactを所要の目標値θcmdに収束させるように決定されている。このため、↑τ2_cmdは、結果的に、ロボット1の各関節の実際の回転角度を、ロボット1の目標とする運動を実現するための所要の目標値θcmdに一致させるように決定される。
以上の如く状態量別制御入力決定部85の処理を実行した後、目標関節駆動トルク決定部72は、次に、合成制御入力決定部86の処理を実行する。
この合成制御入力決定部86は、状態量別制御入力決定部85で決定された、各種類の状態量に対応する目標関節駆動トルク(第1目標関節駆動トルク↑τ1_cmd及び第2目標関節駆動トルク↑τ2_cmd)を逐次合成することによって、各関節の合成目標関節駆動トルクを逐次決定する処理を実行する機能部である。以降、各関節の合成目標関節駆動トルクを成分として並べたベクトルを合成目標関節駆動トルク↑τt_cmdと表記する。
なお、本実施形態の説明では、↑τ1_cmd,↑τ2_cmdと同様、↑τt_cmdは、前記駆動力依存一般化力ベクトル↑τと同じ次数(一般化変数ベクトル↑qと同じ次数)のベクトルであるとする。この場合、↑τt_cmdの成分のうち、ロボット1の代表リンク(下側上体10)の位置及び姿勢に対応する成分は、いずれも“0”である。
本実施形態では、合成制御入力決定部86は、次式6−2,6−3の演算処理によりそれぞれ算出されるベクトル↑τf1,↑τf2を、式6−1の演算処理により合成することによって、合成目標関節駆動トルク↑τt_cmdを決定する。
↑τt_cmd=W1*↑τf1+W2*↑τf2 ……式6−1
ただし、
↑τf1=↑τ1_cmd+Nu1*↑τ2_cmd ……式6−2
↑τf2=↑τ2_cmd ……式6−3
ここで、式6−1のNu1は、前記第1状態量ヤコビアンJs1と、次式6−4−1により定義される行列Js1#とに応じて、次式6−4により定義される行列である。
Nu1≡I−Js1T*Js1#T ……式6−4
ただし、Js1#≡WatT*Js1T*(Js1*Wat*Js1T)-1
……式6−4−1
なお、式6−4−1におけるWatは、重み行列(対角行列)であり、本実施形態では、前記イナーシャ行列Mの逆行列M-1を重み行列Watとして使用する。
このように定義される行列Nu1に対して、式6−2の右辺の第2項の一般化力ベクトル(Nu1*↑τ2_cmd)は、その成分のうち、第1種類状態量ベクトル↑S1に影響を及ぼす成分(詳しくは、↑S1'を変化させる成分)が“0”となり、その他の成分が↑τ2_cmdと一致するようなベクトルである。
従って、(Nu1*↑τ2_cmd)は、ロボット1の各関節の関節駆動トルクを、↑τ1_cmdに制御した場合と、↑τ1_cmd+Nu1*↑τ2_cmd(=↑τf1)に制御した場合とで、第1種類状態量ベクトル↑S1の制御状態を同じにすることができるような、一般化力ベクトルである。
このため、↑τ1_cmd+Nu1*↑τ2_cmd(=↑τf1)は、第1種類状態量目標値ベクトル↑S1'_cmdを実現するための第1目標関節駆動トルク↑τ1_cmdに、第1種類状態量ベクトル↑S1に影響を及ぼさない範囲で、第2種類状態量目標値ベクトル↑S2'_cmdを実現するための第2目標関節駆動トルク↑τ2_cmdの成分を重畳させた(加え合わせた)目標関節駆動トルクとしての意味を持つものである。
また、W1,W2は、互いに異なる周波数通過特性を有する周波数重みである。従って、W1*↑τf1は、↑τf1にW1の周波数通過特性のフィルタリング処理を施したものとしての意味を持つ。また、W2*↑τf2は、↑τf2にW2の周波数通過特性のフィルタリング処理を施したものとしての意味を持つ。
この場合、本実施系形態では、W1は、W2よりも相対的に高周波域の周波数成分を通過可能な周波数通過特性を有する。
具体的には、本実施形態では、W1,W2は、それぞれ、その伝達関数が次式6−5,6−6により表されるフィルタとして機能する周波数重みである。
W1=(1/(T1*s+1))*(1−W2) ……式6−5
W2=1/(T2*s+1) ……式6−6
式6−5、6−6におけるT1,T2は、T1<T2となるようにあかじめ設定された所定値の時定数である。
従って、周波数重みW1は、低周波側のカットオフ周波数と高周波側のカットオフ周波数とがそれぞれ、時定数T2、T1により規定されるバンドパスフィルタとして機能する周波数重みである。また、周波数重みW2は、高周波側のカットオフ周波数が、時定数T2により規定されるローパスフィルタとして機能する周波数重みである。
以上説明した合成制御入力決定部86の処理によって、合成目標関節駆動トルク↑τt_cmdは、第1目標関節駆動トルク↑τ1_cmdに、第2目標関節駆動トルク↑τ2_cmdのうちの第1種類状態量ベクトル↑S1'に影響を及ぼさない成分を付加してなる目標関節駆動トルク↑τf1の高周波側の成分(周波数重みW1の通過周波数帯の成分)と、第2目標関節駆動トルク↑τ2_cmd(=↑τf2)の低周波側の成分(周波数重みW2の通過周波数帯の成分)とを合成したもの(加え合わせたもの)として決定されることとなる。
以上の如く合成制御入力決定部86の処理を実行した後、目標関節駆動トルク決定部72は、次に、コンプライアンス制御部87の処理を実行する。
このコンプライアンス制御部87は、合成制御入力決定部86が決定した合成目標関節駆動トルク↑τt_cmdを基に、ロボット1に作用するトータルの床反力(全床反力)の目標値を決定し、この目標値と実際の全床反力の計測値との偏差を“0”に近づけるように、合成目標関節駆動トルク↑τt_cmdを補正する処理を逐次実行する機能部 である。以降、このコンプライアンス制御部87により合成目標関節駆動トルク↑τt_cmdを補正してなるトルクを補正後合成目標関節駆動トルク↑τtc_cmdという。本実施形態では、この補正後合成目標関節駆動トルク↑τtc_cmdが、制御装置60の各演算処理周期において、最終的に電動モータ50を制御するための目標関節駆動トルク(前記目標関節駆動制御部73に入力するも目標関節駆動トルク)として使用されるものである。
コンプライアンス制御部87には、補正後合成目標関節駆動トルク↑τtc_cmdを算出するために、基本パラメータ群決定部81で決定された遠心力・コリオリ力依存一般化力ベクトル↑N及び重力依存一般化力ベクトル↑G(又は非線形項一般化力ベクトル(↑N+↑G))と、合成制御入力決定部86で決定された合成目標関節駆動トルク↑τt_cmdと、前記接触部位ヤコビアン決定部82で決定された接触部位代表点規定比率rat(又はそれにより規定される接触部位代表点a1〜a4の位置)とが逐次入力される。
この他、コンプライアンス制御部87には、状態量別制御入力決定部85で算出された行列Rc、Jc'、Jc#、前記接触部位代表点目標加速度↑C(本実施形態では、“0”ベクトル)、各足平部18R,18Lにそれぞれ作用する実際の床反力↑FM_act_R,↑FM_act_Lの計測値、代表リンク10の移動速度の計測値、ロボット1の代表リンク(下側上体)10の姿勢角の計測値及びその時間的変化率(角速度)の計測値、並びに、各関節の回転角度の計測値及びその時間的変化率(角速度)の計測値も逐次入力される。
なお、床反力↑FM_act_R,↑FM_act_Lの計測値は、それぞれ、前記力センサ62R,62Lの検出出力に基づき、制御装置60において認識された計測値である。
そして、コンプラインアンス制御部87は、まず、入力された↑N,↑G(又は↑N+↑G)と、Rc、Jc'、Jc#と、↑τt_cmdと、一般化変数ベクトル↑qの1階微分値↑q'の計測値とから、前記式5−10の右辺の演算を行なうことによって、接触部位代表点a1〜a4にそれぞれ作用する力(並進力)の目標値を並べたベクトルとしての接触部位代表点目標作用力↑Fc_cmdを算出する。
すなわち、次式7−1により、↑Fc_cmdが算出される。この接触部位代表点目標作用力↑Fc_cmdは、ロボット1の各関節の実際の関節駆動トルクを↑τt_cmdにより規定される目標関節駆動トルクに一致させた場合に、接触部位代表点a1〜a4でロボット1に作用すべき力(並進力)としての意味を持つものである。
↑Fc_cmd=(Jc#)T*(↑τt_cmd−↑N−↑G)
−Rc*↑C−Rc*Jc'*↑q' ……式7−1
なお、↑N+↑G≒↑Gとなる場合には、式7−1の↑Nを省略してもよい。
次いで、コンプライアンス制御部87は、上記接触部位代表点目標作用力↑Fc_cmdを、ロボット1に対する全床反力の作用点としての所定の全床反力作用点(例えば目標ZMP)に作用する力の目標値としての目標全床反力↑FMt_cmdに変換する。
この目標全床反力↑FMt_cmdは、本実施形態では、3軸方向の並進力成分と3軸周りのモーメント成分とから成るベクトル(6自由度のベクトル)である。この場合、↑FMcmdの並進力成分は、各接触部位代表点a1〜a4に作用する目標並進力を加え合わせたものとなる。また、↑FMt_cmdのモーメント成分は、各接触部位代表点a1〜a4に作用する目標並進力によって、上記全床反力作用点の周りに発生するモーメントを加え合わせたものとなる。
なお、↑Fc_cmdの↑FMt_cmdへの変換は、各接触部位代表点a1〜a4と上記全床反力作用点との間の位置関係に応じて決定される成分を有する変換行列を↑Fc_cmdに乗じることによって、行なわれる。
また、コンプライアンス制御部87は、各足平部18R,18Lにそれぞれ作用する実際の床反力↑FM_act_R,↑FM_act_Lの計測値から、上記全床反力作用点に実際に作用している全床反力↑FMt_actの計測値を算出する。
この場合、本実施形態では、↑FM_act_Rの計測値は、右側足平部18Rに対応する所定の床反力作用点に作用する3軸方向の並進力成分と3軸周りのモーメント成分とから成るベクトルである。同様に、↑FM_act_Lの計測値は、左側足平部18Lに対応する所定の床反力作用点に作用する3軸方向の並進力成分と3軸周りのモーメント成分とから成るベクトルである。
そして、コンプライアンス制御部87は、前記全床反力作用点と右側足平部18Rの床反力作用点との位置関係に応じて決定される成分を有する変換行列によって、↑FMact_Rの計測値を全床反力作用点に作用する力(並進力成分及びモーメント成分の組)に変換したものと、前記全床反力作用点と左側足平部18Lの床反力作用点との位置関係に応じて決定される成分を有する変換行列によって、↑FMact_Lの計測値を全床反力作用点に作用する力(並進力成分及びモーメント成分の組)に変換したものとを加え合わせることによって、全床反力↑FMt_actの計測値を算出する。
この↑FMt_actの計測値の並進力成分は、↑FMact_Rの並進力成分と↑FMact_Lの並進力成分との総和(ベクトル和)の並進力となる。また、↑FMt_actのモーメント成分は、↑FMact_Rのモーメント成分と↑FMact_Lのモーメント成分と↑FMact_Rの並進力成分が全床反力作用点の周りに発生するモーメントと↑FMact_Lの並進力成分が全床反力作用点の周りに発生するモーメントとの総和(ベクトル和)のモーメントとなる。
コンプライアンス制御部87は、上記のように目標全床反力↑FMt_cmdと、全床反力↑FMt_actの計測値とを算出した後、これらの偏差↑ΔFMt(=↑FMt_cmd−↑FMt_act)を算出する。そして、コンプラインアス制御部87は、この偏差↑ΔFMtを“0”に収束させる操作量としての合成目標関節駆動トルク↑τt_cmdの補正量↑Δτtを所定のフィードバック制御則により算出する。
具体的には、本実施形態では、コンプライアンス制御部87は、偏差↑ΔFMtに所定値の比例ゲインGkを乗じてなるベクトルGk*↑ΔFMtを、接触部位代表点a1〜a4にそれぞれ作用する実際の力(並進力)の目標値(接触部位代表点目標作用力↑Fc_cmd)に対する誤差分↑ΔFcに変換する。
この変換は、前記↑Fc_cmdを↑FMt_cmdに変換する変換行列の逆行列を、Gk*↑ΔFMtに乗じることによって行なわれる。なお、本実施形態では、↑Fc_cmdを↑FMt_cmdに変換する変換行列は正則行列とはならないので、その変換行列の逆行列は、前記式5−23に基づいて決定される擬似逆行列である。
さらにコンプライアンス制御部87は、誤差分↑ΔFcに次式7−2の如く、行列Jc#Tの逆行列(Jc#T)-1を乗じることによって、合成目標関節駆動トルク↑τt_cmdの補正量↑Δτtを算出する。
↑Δτt=(Jc#T)-1*↑ΔFc ……式7−2
これにより、補正量↑Δτtは、フィードバック制御則としての比例則により、偏差↑ΔFMtを“0”に収束させるように決定されることとなる。
そして、コンプラインアンス制御部87は、次式7−3の通り、この補正量↑Δτtを合成目標関節駆動トルク↑τt_cmdに加えることによって、補正後合成目標関節駆動トルク↑τtc_cmdを決定する。
↑τtc_cmd=↑τt_cmd+↑Δτt ……式7−3
以上のコンプライアンス制御部87の処理によって、合成目標関節駆動トルク↑τt_cmdに対応する全床反力である目標全床反力↑FMt_cmdと実際の全床反力↑FMt_actの計測値との偏差を“0”に近づけるように、合成目標関節駆動トルク↑τt_cmdが補正されることとなる。
本実施形態では、このように決定された補正後合成目標関節駆動トルク↑τtc_cmd(詳しくは、↑τtc_cmdのうちの各関節の回転角度に対応する成分)が前記関節駆動制御部73に入力される。
以上の如くコンプライアンス制御部87の処理を実行した後、目標関節駆動トルク決定部72は、次に、接触部弾性変形補償部88の処理を実行する。
この接触部弾性変形補償部88は、前記状態量目標値決定部84で、ロボット1の接地している足平部18の弾性変形や床面の弾性変形を考慮し、前記第2種類状態量目標値ベクトル↑S2'cmdを決定するために使用するロボット1の各関節の目標関節角θcmdとして、前記歩容生成部71で生成した目標関節角(以下、基本目標関節角θcmd_baseという)から修正してなる値を設定する処理を逐次実行する機能部である。
この接触部弾性変形補償部88には、コンプライアンス制御部87で算出された前記接触部位代表点目標作用力↑Fc_cmdと、接触部位ヤコビアン決定部82で算出された前記接触部位ヤコビアンJcとが入力されると共に、歩容生成部71で生成された基本目標関節角θcmd_baseが入力される。
そして、接触部弾性変形補償部88は、入力された↑Fc_cmd及びJcを用いて、次式8−1により、各関節の基本目標関節角θcmd_baseを補正するための関節角補正量Δθcmdを成分として含むベクトル↑Δqcmd(以降、関節角補正量ベクトル↑Δqcmdという)を算出する。なお、この関節角補正量ベクトル↑Δqcmdの各成分は、一般化変数ベクトル↑qの各成分にそれぞれ対応しており、↑Δqcmdの成分のうちの各関節の回転角度に対応する成分が、該関節に対応する上記関節角補正量Δθcmdである。
↑Δqcmd=(Jc)-1*(Kstif*↑Fc_cmd) ……式8−1
この式8−1における“Kstif”は、接地している足平部18の弾性特性や床面の弾性特性に依存するばね定数の逆数値に相当するものとしたあらかじめ定められた係数行列(対角行列)である。換言すれば、前記接触部位代表点a1〜a4に作用する力(並進力)を、その力に起因する足平部18(接地している足平部18)や床面の弾性変形によって生じる接触部位代表点a1〜a4の変位量に変換する係数行列である。
従って、式8−1の(Kstif*↑Fc_cmd)は、前記接触部位代表点a1〜a4に作用する力(並進力)を↑Fc_cmdに一致させた場合に生じる接触部位代表点a1〜a4の位置の変位量の推定値に相当するものとなる。
そして、(Kstif*↑Fc_cmd)に、接触部位ヤコビアンJcの逆行列(Jc)-1を乗じることによって、接触部位代表点a1〜a4の位置の上記変位量が、ロボット1の各関節の回転角度を含む一般化変数ベクトル↑qの各成分の変化量に変換され、この変換された変化量を成分として構成されるベクトルが、上記関節角補正量ベクトル↑Δqとして算出される。なお、(Jc)-1は、前記式5−23に基づいて算出される擬似逆行列である。
次いで、接触部弾性変形補償部88は、次式8−2の如く、上記関節角補正量ベクトル↑Δqのうちの関節角補正量Δθcmdを、それに対応する関節の基本目標関節角θcmd_baseに加算することによって、前記状態量目標値決定部88に入力する各関節の目標関節角θcmdを決定する。
θcmd=θcmd_base+Δθcmd ……式8−2
以上の接触部弾性変形補償部88の処理によって、接触部位として接地している足平部18の弾性変形や床面の弾性変形を加味した適切な目標関節角θcmd(実際の関節角の収束目標として適切な目標関節角θcmd)を決定できる。
以上が、目標関節駆動トルク決定部72の処理の詳細である。
以上説明した本実施形態によれば、前記合成目標関節駆動トルク↑τt_cmdの高周波側の成分(W1*↑τf1)は、主に、第1目標関節駆動トルク↑τ1_cmdの高周波側の成分に依存する成分となり、↑τt_cmdの低周波側の成分(W2*↑τf2)は、第2目標関節駆動トルク↑τ2_cmdの低周波側の成分に依存する成分となる。
このため、前記第1種類状態量目標値ベクトル↑S1'_cmdを達成すること(ひいては、ロボット1の全体重心点の位置を目標とする位置に制御すること)を高い速応性で行いつつ、長期的には、前記第2種類状態量目標値ベクトル↑S2'_cmdを達成すること(ひいては、各関節の実際の回転角度を目標関節角に制御すること)を満足し得るように、ロボット1の各関節の駆動トルクを制御することができる。
さらに、合成目標関節駆動トルク↑τt_cmdの高周波側の成分(W1*↑τf1)は、第2目標関節駆動トルク↑τ2_cmdの高周波側の成分のうちの、第1種類状態量ベクトル↑S1に影響を及ぼさない成分をも含んでいる。このため、第1種類状態量ベクトル↑S1の時間的変化率↑S1'の制御性を損なうことなく、高周波域での第2種類状態量ベクトル↑S2の時間的変化率↑S2'の制御性を高めることができる。
また、本実施形態では、第1種類状態量目標値ベクトル↑S1'_cmdのうちの全並進運動量の1階微分値↑Ptotal'の目標値は、鉛直方向の位置を含めたロボット1の全体重心点の位置を歩容生成部71で生成された目標歩容の全体重心点の目標位置に収束させるように決定されている。換言すれば、ロボット1の運動によって発生する並進慣性力の目標値に相当する↑Ptotal'の目標値が、ロボット1の実際の全体重心点の位置を適切な目標位置に収束させるように決定されている。
さらに、↑S1'_cmdのうちの全角運動量の1階微分値↑Ltotal'の目標値は、ロボット1の全体重心点の水平位置を、該全体重心点に質点を有する仮想的な倒立振子の支点としての目標ZMPの鉛直方向上方の位置に収束させるように決定されている。換言すれば、ロボット1の運動によって発生するモーメント慣性力の目標値に相当する↑Ltotal'の目標値が、ロボット1の実際の全体重心点の水平位置を、上記仮想的な倒立振子の傾倒を防止し得る位置に収束させるように決定されている。
そして、本実施形態では、前記周波数重みW1に対応する高周波域において、第1種類状態量目標値ベクトル↑S1'_cmdの↑Ptotal'の目標値及び↑Ltotal'の目標値を実現し得るように、前記合成目標関節駆動トルク↑τt_cmdが決定される。
ここで、本実施形態のロボット1では、各関節の関節駆動系にばね部材であるトーションバー53を備えるため、各関節が柔軟性を有する。このため、ロボット1に作用する外力の影響で、各関節の回転角度の変動が生じやすく、ひいては、全体重心点の位置の変動(特に鉛直方向の位置の変動)も生じやすい。
しかるに、上記の如く第1種類状態量目標値ベクトル↑S1'_cmdを決定しているために、ロボット1の全体重心点の実際の位置が目標位置からのずれると、それを素早い応答性で解消するように合成目標関節駆動トルク↑τt_cmdが決定される。
そのため、ロボット1の実際の全体重心点の位置が適切な目標位置からずれるの防止することができる。その結果、ロボット1の各関節が柔軟性を有していても、ロボット1の姿勢のふらつき等が生じるのを防止し、該ロボット1の挙動の安定性を高めることができる。
また、本実施形態では、前記式5−20に基づいて、各種類の状態量毎の目標関節駆動トルク↑τ1_cmd,↑τ2_cmdが決定される。従って、接触部位代表点a1〜a4の運動を前記目標値↑Cにより規定した上で、ロボット1の制御すべき状態量(第1種類状態量及び第2種類状態量)に関する目標値としての第1種類状態量目標値ベクトル↑S1'_cmdと第2種類状態量目標値ベクトル↑S2'_cmdとに基づいて、それぞれ、↑τ1_cmd、↑τ2_cmdが決定される。
このため、ロボット1に作用する外力の値(これは外乱等の影響を受けやすい)を特定することなく、↑S1'_cmd、↑S2'_cmdを実現し得る適切な↑τ1_cmd、↑τ2_cmdを決定できる。また、特に、第1目標関節駆動トルク↑τ1_cmdは、ロボット1の全ての関節の回転動作を特定することを必要とせずに決定できる。このため、ロボット1に作用する外力の変動等に伴うロボット1の関節の動きの柔軟性を高めることができる。
ここで、以上説明した実施形態と本発明との対応関係について補足しておく。
本実施形態では、前記状態量別制御入力決定部85により、本発明における状態量別制御入力決定手段が実現される。この場合、前記第1種類状態量及び第2種類状態量が本発明における複数種類の状態量に相当し、前記第1目標関節駆動トルク↑τ1_cmd及び第2目標関節駆動トルク↑τ2_cmdがそれぞれ、本発明における状態量別制御入力に相当する。また、第1種類状態量目標値ベクトル↑S1'_cmdと第2種類状態量目標値ベクトル↑S2'_cmdとが、各種類の状態量に関する目標値に相当する。なお、本実施形態では、状態量の種類数Mは“2”である。
また、前記合成制御入力決定部85により、本発明における合成制御入力決定手段が実現される。この場合、前記合成目標関節駆動トルク↑τt_cmdが、本発明における合成制御入力τtotalに相当し、前記式6−1のW1*↑τf1と、W2*↑τf2とがそれぞれ、本発明における周波数領域別制御入力(W(i)*τf(i))に相当し、周波数重みW1,W2のそれぞれの通過可能な周波数帯が、本発明における周波数領域に相当する。なお、本実施形態では、周波数領域の個数nは2つである。
また、本実施形態では、↑τf1、↑τf2がそれぞれ、本発明における(前記式01、02aにおける)τf(i)に相当する。この場合、↑τ1_cmd,↑τ2_cmdがそれぞれ、↑τf1(τf(1))の組成成分τ(1,1)、τ(1,2)に相当し、行列Nu1が本発明におけるNu(1,1)に相当する。また、↑τ2_cmdが、↑τf2(τf(2))の組成成分τ(2,1)に相当する。なお、本実施形態では、↑τf1(τf(1))の組成成分の個数m(1)は“2”、↑τf2(τf(2))の組成成分の個数m(2)は“1”となる。
また、本実施形態では、前記行列Js1#、Js2#が、それぞれ本発明におけるJs#(i,j)に相当し、Js1、Js2がそれぞれ、本発明におけるJs(i,j)に相当し、↑S1,↑S2がそれぞれ、本発明におけるS(i,j)に相当する。また、本実施形態におけるM、↑qがそれぞれ、本発明におけるイナーシャ行列M、一般化変数ベクトルqに相当する。なお、本実施形態では、本発明における重み行列Wdを単位行列とし、Wat=M-1としている。
また、前記コンプライアンス制御部87及び関節駆動制御部73により、本発明におけるアクチュエータ制御手段が実現される。
[第2実施形態]
次に、本発明の第2実施形態を図6〜図8を参照して説明する。なお、本実施形態の説明では、第1実施形態と同一の構成部分については、第1実施形態と同一の参照符号を用い詳細な説明を省略する。
本実施形態におけるロボットは、前記第1実施形態で例示したロボット1と同じである。そして、本実施形態では、制御装置60は、その機能のうちの目標関節駆動トルク決定部の一部の処理が第1実施形態と相違する。すなわち、制御装置60は、第1実施形態で説明した目標関節駆動トルク決定部72の代わりに、図6のブロック図に示す目標関節駆動トルク決定部90を備える。
この目標関節駆動トルク決定部90は、その主要な機能として、基本パラメータ群決定部91、接触部位ヤコビアン決定部92、状態量ヤコビアン決定部93、状態量目標値決定部94、状態量別制御入力決定部95、合成制御入力決定部96、コンプライアンス制御部97、及び接触部弾性変形補償部98を備える。
この場合、これらの機能のうち、基本パラメータ群決定部91、接触部位ヤコビアン決定部92、コンプライアンス制御部97、及び接触部弾性変形補償部98は、それぞれ、第1実施形態で説明した基本パラメータ群決定部81、接触部位ヤコビアン決定部82、コンプライアンス制御部87、及び接触部弾性変形補償部88と同じである。そこで、これらの機能部の処理については説明を省略する。
一方、本実施形態では、状態量ヤコビアン決定部93、状態量目標値決定部94、状態量別制御入力決定部95、及び合成制御入力決定部96の処理が第1実施形態と相違しており、以下にこれらの処理を説明する。
まず、本実施形態において制御対象とする複数種類の状態量について説明する。本実施形態では、ロボット1の歩行動作を安定に行なう上で適切な目標値に制御することが望ましい制御対象として3種類の状態量があらかじめ選定されている。その3種類の状態量のうちの2種類の状態量は、第1実施形態で説明した第1種類状態量(全並進運動量↑Ptotalと全角運動量↑Ltotaとの組)及び第2種類状態量(ロボット1の各関節の回転角速度)である。
一方、残りの1種類の状態量(以降、第3種類状態量という)は、脚体2R,2Lのうち、ロボット1の歩行動作中に先端部(足平部18)を離床させた側の脚体2、すなわち遊脚となる脚体2の先端部(足平部18)の運動状態量である。その運動状態量は、本実施形態の例では、遊脚となる脚体2(以下、遊脚側脚体2ということがある)の足平部18の位置の時間的変化率と姿勢の時間的変化率との組、すなわち、該遊脚側脚体2の足平部18の移動速度と姿勢の角速度との組である。従って、本実施形態では、遊脚側脚体2の足平部18が空中を移動するときの該足平部18の移動速度及び姿勢の角速度を制御対象の1つの状態量とする。
そして、本実施形態における状態量ヤコビアン決定部93は、上記の3種類の状態量のそれぞれと、一般化変数ベクトル↑qの1階微分値↑q'との間の関係を表現するヤコビアン(状態量ヤコビアン)を算出する処理を逐次実行する。
この場合、前記第1種類状態量及び第2種類状態量にそれぞれ対応する状態量ヤコビアンJs1,Js2の算出処理は、前記第1実施形態と同じである。
一方、前記第3種類状態量に対応する状態量ヤコビアンJs3(以降、第3状態量ヤコビアンJs3という)は、遊脚側脚体2の足平部18の並進移動速度↑Vfootと該足平部18の姿勢の時間的変化率(角速度)↑ωfootとを成分として並べて構成されるベクトル↑S3(=[↑Vfoot,↑ωfoot]T。以降、第3種類状態量ベクトル↑S3という)と、前記一般化変数ベクトル↑qの1階微分値↑q'との間の関係を次式3−3より表現する行列Js3である。
↑S3=Js3*↑q' ……式3−3
なお、本実施形態では、↑Vfoot,↑ωfootは、それぞれ、3自由度のベクトルであるので、↑S3は、6自由度のベクトルである。
この第3状態量ヤコビアンJs3を算出するために、状態量ヤコビアン決定部93には、代表リンク(下側上体)10の移動速度、並びに姿勢角及びその時間的変化率(角速度)の計測値と、各関節の回転角度及びその時間的変化率(角速度)の計測値とが入力される。
そして、状態量ヤコビアン決定部93は、右側脚体2Rが遊脚となる期間(片脚支持期)では、第3種類状態量ベクトル↑S3が、右側脚体2Rの足平部18Rの並進移動速度↑Vfootと該足平部18Rの姿勢の角速度↑ωfootとを成分として構成される状態量ベクトルであるとして、この↑S3と、↑q'との間の関係を上記式3−3により表現する第3状態量ヤコビアンJs3を入力された計測値から算出する。
また、状態量ヤコビアン決定部93は、左側脚体2Lが遊脚となる期間(片脚支持期)では、第3種類状態量ベクトル↑S3が、左側脚体2Lの足平部18Lの並進移動速度↑Vfootと該足平部18Lの姿勢の角速度↑ωfootとを成分として構成される状態量ベクトルであるとして、この↑S3と、↑q'との間の関係を上記式3−3により表現する第3状態量ヤコビアンJs3を入力された計測値から算出する。
なお、右側脚体2Rが遊脚となる期間であるか、左側脚体2Lが遊脚となる期間であるかの判断は、例えば、前記力センサ62R,62Lの検出出力、あるいは、前記歩容生成部71から目標関節駆動トルク決定部90に与えられる目標歩容に基づいて行なわれる。
また、ロボット1の歩行動作中に両脚体2R,2Lの足平部18R,18Lが着床状態となる両脚支持期においては、状態量ヤコビアン決定部93は、第3状態量ヤコビアンJs3の算出処理を中断する。
以上が、本実施形態における状態量ヤコビアン決定部93の処理の詳細である。
また、本実施形態における前記状態量目標値決定部94は、前記した3種類の状態量に関する目標値を決定する処理を実行する。
この場合、本実施形態では、前記第1種類状態量に関する目標値は、前記第1実施形態と同じである。すなわち、該目標値は、前記第1種類状態量ベクトル↑S1の時間的変化率(1階微分値)↑S1'(=d↑S1/dt)の目標値↑S1'_cmd(前記第1種類状態量目標値ベクトル↑S1'_cmd)である。そして、状態量目標値決定部94は、この第1種類状態量目標値ベクトル↑S1'_cmdを、第1実施形態と同じ処理によって決定する。
また、前記第2種類状態量に関する目標値は、第1実施形態と同様に前記第2種類状態量ベクトル↑S2の時間的変化率(1階微分値)↑S2'(=d↑S2/dt)の目標値である。但し、本実施形態では、前記状態量目標値決定部94は、↑S2'の目標値として、複数種類、例えば2種類の目標値↑S2'_cmd1,↑S2'_cmd2をそれぞれ各別の処理によって決定する。
さらに詳細には、本実施形態では、状態量目標値決定部94は、上記2種類の目標値↑S2'_cmd1,↑S2'_cmd2をそれぞれ、ロボット1の各関節の実際の回転角度(関節角)を所要の目標値に追従させるための制御入力として決定する。
但し、この場合、2種類の目標値↑S2'_cmd1,↑S2'_cmd2のうちの目標値↑S2'_cmd1は、各関節の実際の関節角の計測値をフィードバックすることなく、該関節角の目標値の時系列から直接的に(フィードフォワード則により)決定される。また、目標値↑S2'_cmd2は、各関節の実際の関節角の計測値のフィードバックを行なうフィードバック制御則により決定される。以降、↑S2'_cmd1,↑S2'_cmd2をそれぞれ、第2種類状態量FF目標値ベクトル↑S2'_cmd1、第2種類状態量FB目標値ベクトル↑S2'_cmd2という。
これらの第2種類状態量FF目標値ベクトル↑S2'_cmd1、第2種類状態量FB目標値ベクトル↑S2'_cmd2は、具体的には、次のように決定される。
すなわち、第2種類状態量FF目標値ベクトル↑S2'_cmd1の決定処理においては、状態量目標値決定部94は、接触部弾性変形補償部98で第1実施形態と同じ処理によって算出されるロボット1の各関節角の目標値θcmdの時系列に基づいて、該目標値θcmdの2階微分値(θcmdの時系列により規定される各関節の目標角加速度)を逐次算出し、その算出した各関節の目標角加速度を成分として構成されるベクトルを、第2種類状態量FF目標値ベクトル↑S2'_cmd1として決定する。
また、第2種類状態量FB目標値ベクトル↑S2'_cmd2の決定処理においては、状態量目標値決定部94は、第1実施形態における第2種類状態量目標値ベクトル↑S2'_cmdの算出処理と同じ処理によって、第2種類状態量FB目標値ベクトル↑S2'_cmd2を決定する。すなわち、状態量目標値決定部94は、接触部弾性変形補償部98から逐次入力される各関節の関節角の目標値θcmdと、該関節の実際の回転角度θactの計測値との偏差Δθ(=θcmd−θact)を並べたベクトル↑ΔΘと、その1階微分値↑Θ’とから、フィードバック制御則としてのPD則によって(前記式4−4の右辺の演算によって)、第2種類状態量FB目標値ベクトル↑S2'_cmd2を決定する。従って、↑S2'_cmd2は、各関節におけるθactをフィードバック制御則にθcmdに収束させるように決定される目標値ベクトルである。
また、前記第3種類状態量に関する目標値は、前記第3種類状態量ベクトル↑S3の時間的変化率(1階微分値)↑S3'(=d↑S3/dt)の目標値である。そして、本実施形態では、前記状態量目標値決定部94は、↑S2'の目標値と同様に、↑S3'の目標値として、複数種類、例えば2種類の目標値↑S3'_cmd1,↑S3'_cmd2をそれぞれ各別の処理によって決定する。
さらに詳細には、本実施形態では、状態量目標値決定部94は、上記2種類の目標値↑S3'_cmd1,↑S3'_cmd2をそれぞれ、ロボット1の遊脚側脚体2の足平部18の実際の位置及び姿勢を所要の目標値に追従させるための制御入力として決定する。
但し、この場合、2種類の目標値↑S3'_cmd1,↑S3'_cmd2のうちの目標値↑S3'_cmd1は、遊脚側脚体2の足平部18の実際の位置及び姿勢の計測値をフィードバックすることなく、該足平部18の目標位置及び目標姿勢の時系列から直接的に(フィードフォワード則により)決定される。また、目標値↑S3'_cmd2は、遊脚側脚体2の足平部18の実際の位置及び姿勢の計測値のフィードバックを行なうフィードバック制御則により決定される。以降、↑S3'_cmd1,↑S3'_cmd2をそれぞれ、第3種類状態量FF目標値ベクトル↑S3'_cmd1、第3種類状態量FB目標値ベクトル↑S3'_cmd2という。
これらの第3種類状態量FF目標値ベクトル↑S3'_cmd1、第3種類状態量FB目標値ベクトル↑S3'_cmd2は、具体的には、次のように決定される。
すなわち、状態量目標値決定部94は、接触部弾性変形補償部98から逐次入力される各関節の関節角の目標値θcmdの時系列に基づいて(又は歩容生成部71から入力される目標歩容のうちの目標関節角の時系列に基づいて)、遊脚側脚体2の足平部18の目標位置及び目標姿勢を逐次算出する。図7は、このように算出される遊脚側足平部18の目標位置(鉛直方向の目標位置)の軌道の例を代表的に示している。
また、状態量目標値決定部94は、ロボット1の各関節の実際の関節角の計測値の時系列に基づいて、遊脚側脚体2の足平部18の実際の位置及び姿勢の計測値を逐次算出する。
そして、第3種類状態量FF目標値ベクトル↑S3'_cmd1の決定処理においては、状態量目標値決定部94は、遊脚側脚体2の足平部18の算出した目標位置及び目標姿勢の時系列に基づいて、該目標位置及び目標姿勢のそれぞれの2階微分値(該目標位置の時系列により規定される該足平部18の目標並進加速度と、該目標姿勢の時系列により規定される該足平部18の姿勢の目標角加速度)を逐次算出し、その算出した目標並進加速度及び目標角加速度を成分として構成されるベクトルを、第3種類状態量FF目標値ベクトル↑S3'_cmd1として決定する。
例えば、図7に示す目標位置の軌道の例では、第3種類状態量FF目標値ベクトル↑S3'_cmd1の目標並進加速度の鉛直方向成分が、図示の軌道を2階微分してなる値として決定される。
また、第3種類状態量FB目標値ベクトル↑S3'_cmd2の決定処理においては、状態量目標値決定部94は、遊脚側脚体2の足平部18の算出した目標位置及び目標姿勢のそれぞれと、該足平部18の実際の位置の計測値及び姿勢の計測値のそれぞれとの偏差↑ΔXfootと、該偏差↑ΔXfootの1階微分値↑ΔXfoot’とを算出する。
なお、上記偏差↑ΔXは、遊脚側脚体2の足平部18の目標位置と実際の位置の計測値との偏差(3自由度の偏差)と、該足平部18の目標姿勢と実際の姿勢の計測値との偏差(3自由度偏差)とを並べて構成されるベクトルである。
さらに、状態量目標値決定部94は、各関節の上記偏差↑ΔXを“0”に収束させるためのフィードバック操作量を所定のフィードバック制御則により算出し、その算出したフィードバック操作量を第3種類状態量FB目標値ベクトル↑S3'_cmd2として決定する。
この場合、具体的には、状態量目標値決定部94は、上記偏差↑ΔXをPD則(比例・微分則)によって“0”に収束させるように↑S3'_cmd2を決定する。すなわち、状態量目標値決定部94は、上記偏差↑ΔXと、その1階微分値↑ΔX’とから、次式4−5により↑S3'_cmd2を決定する。
↑S3'cmd2=Kp4*↑ΔXfoot+Kv4*↑ΔXfoot' ……式4−5
なお、式4−5におけるKp4,Kv4は、それぞれあらかじめ設定された所定値の比例ゲイン、微分ゲインである。これらのゲインKp4,Kv4は対角行列(↑S3'_cmd2と同じ次数の対角行列)によって表現される。
補足すると、第3種類状態量FF目標値ベクトル↑S3'_cmd1及び第3種類状態量FB目標値ベクトル↑S3'_cmd2の決定処理において、右側脚体2R及び左側脚体2Lのいずれが遊脚であるかの判断は、例えば、前記力センサ62R,62Lの検出出力、あるいは、前記歩容生成部71から目標関節駆動トルク決定部90に与えられる目標歩容に基づいて行なわれる。
また、ロボット1の歩行動作中に両脚体2R,2Lの足平部18R,18Lが着床状態となる両脚支持期においては、状態量目標値決定部94は、↑S3'_cmd1及び↑S3'_cmd2の決定処理を中断する。
以上が本実施形態における状態量目標値決定部94の処理の詳細である。
次に、本実施形態における状態量別制御入力決定部95は、前記3種類の状態量に関する目標値、すなわち前記第1種類状態量目標値ベクトル↑S1'_cmd、第2種類状態量FF目標値ベクトル↑S2'_cmd1、第2種類状態量FB目標値ベクトル↑S2'_cmd2、第3種類状態量FF目標値ベクトル↑S3'_cmd1、及び第3種類状態量FB目標値ベクトル↑S3'_cmd2を実現するために必要な制御入力(アクチュエータ(電動モータ50)の動作制御用の制御入力)として、ロボット1の各関節の目標関節駆動トルクを各目標値ベクトル毎に決定する処理を逐次実行する。
以降、前記↑S1'_cmd、↑S2'_cmd1、↑S2'_cmd2、↑S3'_cmd1、↑S3'_cmd2のそれぞれに対応する各関節の目標関節駆動トルクを成分として並べたベクトルを、それぞれ第1目標関節駆動トルク↑τ1_cmd、第2FF目標関節駆動トルク↑τ2_cmd1、第2FB目標関節駆動トルク↑τ2_cmd2、第3FF目標関節駆動トルク↑τ3_cmd1、第3FB目標関節駆動トルク↑τ3_cmd2と表記する。
なお、本実施形態の説明では、↑τ1_cmd,↑τ2_cmd1,↑τ2_cmd2,↑τ3_cmd1,↑τ3_cmd2は、第1実施形態における↑τ1_cmd、↑τ2cmdと同様に、前記駆動力依存一般化力ベクトル↑τと同じ次数(一般化変数ベクトル↑qと同じ次数)のベクトルであるとする。
状態量別制御入力決定部95は、前記第1実施形態と同様に、前記式5−22を用いて↑τ1_cmd,↑τ2_cmd1,↑τ2_cmd2,↑τ3_cmd1,↑τ3_cmd2を算出する。その算出は、第1実施形態と同様の仕方で行なわれる。
さらに詳細には、状態量別制御入力決定部95には、各目標関節駆動トルク↑τ1_cmd,↑τ2_cmd1,↑τ2_cmd2,↑τ3_cmd1,↑τ3_cmd2を算出するために、基本パラメータ群決定部91で決定されたイナーシャ行列M、遠心力・コリオリ力依存一般化力ベクトル↑N及び重力依存一般化力ベクトル↑G(又は非線形項一般化力ベクトル(↑N+↑G))と、接触部位ヤコビアン決定部92で決定された接触部位ヤコビアンJcと、状態量ヤコビアン決定部93で決定された各種類の状態量に対応する状態量ヤコビアンJs1,Js2,Js3と、状態量目標値決定部94で決定された各目標値ベクトル↑S1'_cmd、↑S2'_cmd1、↑S2'_cmd2、↑S3'_cmd1、↑S3'_cmd2とが逐次入力される。
この他、状態量別制御入力決定部95には、一般化変数ベクトル↑qの時間的変化率(1階微分値)↑q'の計測値(下側上体10の移動速度、並びに姿勢角の時間的変化率(角速度)の計測値、各関節の回転角度の時間的変化率(角速度)の計測値)も逐次入力されると共に、接触部位代表点目標加速度↑Cも入力される。なお、本実施形態における接触部位代表点目標加速度↑Cは、第1実施形態のものと同一(“0”ベクトル)である。
そして、状態量別制御入力決定部95は、入力されたイナーシャ行列M、遠心力・コリオリ力依存一般化力ベクトル↑N及び重力依存一般化力ベクトル↑G(又は非線形項一般化力ベクトル(↑N+↑G))と、接触部位ヤコビアンJcと、接触部位代表点目標加速度↑Cとを用いて、行列Tc、Pcと、一般化力ベクトル↑τcmpnとを算出する。この算出の仕方は第1実施形態と同じである。
また、状態量別制御入力決定部95は、第1種類状態量、第2種類状態量及び第3種類状態量にそれぞれ対応する前記式5−22の右辺の行列Bs(Bs1,Bs2,Bs3)を第1実施形態と同様の仕方で算出する。
すなわち、第1種類状態量に対応する行列Bs1は、入力されたイナーシャ行列M及び第1状態量ヤコビアンJs1と、前記行列Pcの算出値とから、前記式5−21a又は5−21bに基づいて算出される。
また、第2種類状態量に対応する行列Bs2は、入力されたイナーシャ行列M及び第2状態量ヤコビアンJs2と、前記行列Pcの算出値とから、前記式5−21a又は5−21bに基づいて算出される。
また、第3種類状態量に対応する行列Bs3は、入力されたイナーシャ行列M及び第3状態量ヤコビアンJs3と、前記行列Pcの算出値とから、前記式5−21a又は5−21bに基づいて算出される。
そして、状態量別制御入力決定部95は、Bs1,Tc,↑τcmpnの算出値と、入力された第1種類状態量目標値ベクトル↑S1'_cmd、第1状態量ヤコビアンJs1及びイナーシャ行列Mと、↑q'の計測値とを用いて、前記式5−22の右辺の演算(詳しくは、式5−22の右辺のBs、↑S'、Js、Js'、↑q'をそれぞれ、Bs1、↑S1'_cmd、Js1、Js1'、↑q'の計測値に置き換えた式の演算)を行なうことによって、↑τを算出し、この↑τの算出値を第1目標関節駆動トルク↑τ1_cmdとして決定する。
この場合、↑τ1_cmdは、接触部位代表点a1〜a4の運動加速度を接触部位代表点目標加速度↑Cにより規定される加速度に一致させつつ、第1種類状態量ベクトル↑S1の時間的変化率↑S1'を、ロボット1の全体重心点を適切な位置に保つように決定された第1種類状態量目標値ベクトル↑S1'_cmdに一致させるように決定される。
また、状態量別制御入力決定部95は、Bs2,Tc,↑τcmpnの算出値と、入力された第2種類状態量FF目標値ベクトル↑S2'_cmd1、第2状態量ヤコビアンJs2及びイナーシャ行列Mと、↑q'の計測値とを用いて、前記式5−22の右辺の演算(詳しくは、式5−22の右辺のBs、↑S'、Js、Js'、↑q'をそれぞれ、Bs2、↑S2'_cmd1、Js2、Js2'、↑q'の計測値に置き換えた式の演算)を行なうことによって、↑τを算出し、この↑τの算出値を第2FF目標関節駆動トルク↑τ2_cmd1として決定する。
この場合、↑τ2_cmd1は、接触部位代表点a1〜a4の運動加速度を接触部位代表点目標加速度↑Cにより規定される加速度に一致させつつ、第2種類状態量ベクトル↑S2の時間的変化率↑S2'を、ロボットの1の各関節の実際の関節角を目標値に追従させるべくフィードフォワード則により決定した第2種類状態量FF目標値ベクトル↑S2'_cmd2に一致させるように決定される。
また、状態量別制御入力決定部95は、Bs2,Tc,↑τcmpnの算出値と、入力された第2種類状態量FB目標値ベクトル↑S2'_cmd2、第2状態量ヤコビアンJs2及びイナーシャ行列Mと、↑q'の計測値とを用いて、前記式5−22の右辺の演算(詳しくは、式5−22の右辺のBs、↑S'、Js、Js'、↑q'をそれぞれ、Bs2、↑S2'_cmd2、Js2、Js2'、↑q'の計測値に置き換えた式の演算)を行なうことによって、↑τを算出し、この↑τの算出値を第2FB目標関節駆動トルク↑τ2_cmd2として決定する。
この場合、↑τ2_cmd2は、接触部位代表点a1〜a4の運動加速度を接触部位代表点目標加速度↑Cにより規定される加速度に一致させつつ、第2種類状態量ベクトル↑S2の時間的変化率↑S2'を、ロボットの1の各関節の実際の関節角を目標値に追従させるべくフィードバック制御則(PD則)により決定した第2種類状態量FB目標値ベクトル↑S2'_cmd2に一致させるように決定される。
また、状態量別制御入力決定部95は、Bs3,Tc,↑τcmpnの算出値と、入力された第3種類状態量FF目標値ベクトル↑S3'_cmd1、第3状態量ヤコビアンJs3及びイナーシャ行列Mと、↑q'の計測値とを用いて、前記式5−22の右辺の演算(詳しくは、式5−22の右辺のBs、↑S'、Js、Js'、↑q'をそれぞれ、Bs3、↑S3'_cmd1、Js3、Js3'、↑q'の計測値に置き換えた式の演算)を行なうことによって、↑τを算出し、この↑τの算出値を第3FF目標関節駆動トルク↑τ3_cmd1として決定する。
この場合、↑τ3_cmd1は、接触部位代表点a1〜a4の運動加速度を接触部位代表点目標加速度↑Cにより規定される加速度に一致させつつ、第3種類状態量ベクトル↑S3の時間的変化率↑S3'を、遊脚側脚体2の足平部18の実際の位置及び姿勢をそれぞれ目標位置及び目標姿勢に追従させるべくフィードフォワード則により決定した第3種類状態量FF目標値ベクトル↑S3'_cmd1に一致させるように決定される。
また、状態量別制御入力決定部95は、Bs3,Tc,↑τcmpnの算出値と、入力された第3種類状態量FB目標値ベクトル↑S3'_cmd2、第3状態量ヤコビアンJs3及びイナーシャ行列Mと、↑q'の計測値とを用いて、前記式5−22の右辺の演算(詳しくは、式5−22の右辺のBs、↑S'、Js、Js'、↑q'をそれぞれ、Bs3、↑S3'_cmd2、Js3、Js3'、↑q'の計測値に置き換えた式の演算)を行なうことによって、↑τを算出し、この↑τの算出値を第3FB目標関節駆動トルク↑τ3_cmd2として決定する。
この場合、↑τ3_cmd2は、接触部位代表点a1〜a4の運動加速度を接触部位代表点目標加速度↑Cにより規定される加速度に一致させつつ、第3種類状態量ベクトル↑S3の時間的変化率↑S3'を、遊脚側脚体2の足平部18の実際の位置及び姿勢をそれぞれ目標位置及び目標姿勢に追従させるべくフィードバック制御則により決定した第3種類状態量FB目標値ベクトル↑S3'_cmd2に一致させるように決定される。
なお、上記Js1'(=Js1の1階微分値)、Js2'(=Js2の1階微分値)、及びJs3'(=Js3の1階微分値)は、それぞれ、第1状態量ヤコビアンJs1の時系列、第2状態量ヤコビアンJs2の時系列、第3状態量ヤコビアンJs3の時系列から算出される。
以上が本実施形態における状態量別制御入力決定部95の処理の詳細である。
また、本実施形態における合成制御入力決定部96は、状態量別制御入力決定部95で決定された、第1目標関節駆動トルク↑τ1_cmd、第2FF目標関節駆動トルク↑τ2_cmd1、第2FB目標関節駆動トルク↑τ2_cmd2、第3FF目標関節駆動トルク↑τ3_cmd1及び第3FB目標関節駆動トルク↑τ3_cmd2を逐次合成することによって、各関節の合成目標関節駆動トルク(合成目標関節駆動トルク↑τt_cmd)を逐次決定する。
なお、本実施形態における↑τt_cmdは、第1実施形態のものと同様に、前記駆動力依存一般化力ベクトル↑τと同じ次数(一般化変数ベクトル↑qと同じ次数)のベクトルであるとする。
本実施形態では、合成制御入力決定部96は、次式6−8,6−9の演算処理によりそれぞれ算出されるベクトル↑τf1a,↑τf2aを、式6−7の演算処理により合成することによって、合成目標関節駆動トルク↑τt_cmdを決定する。
↑τt_cmd=W1*↑τf1a+W2*↑τf2a ……式6−7
ただし、
↑τf1a=wc3*↑τ3_cmd1+Nuw3*wc1*↑τ1_cmd
+Nuw3*Nuw1*wc2*↑τ2_cmd1
……式6−8
↑τf2a=wc3*↑τ3_cmd2+Nuw3*wc1*↑τ1_cmd
+Nuw3*Nuw1*wc2*↑τ2_cmd2
……式6−9
ここで、式6−7におけるW1,W2は、互いに異なる周波数通過特性を有する周波数重みである。この場合、本実施形態では、これらの周波数重みW1,W2の特性は、それぞれ第1実施形態のものと同一とされている。すなわち、W1,W2は、それぞれ、その伝達関数が前記式6−5,式6−6により表される周波数重みである。
また、式6−8,式6−9におけるwc1,wc2,wc3は、それぞれ“0”から“1”の間の範囲で設定された重み係数である。この場合、重み係数wc1は、第1種類状態量に係わる第1目標関節駆動トルク↑τ1_cmdに対応する重み係数であり、本実施形態では、固定的に“1”に設定されている。
また、重み係数wc2は、第2種類状態量に係わる第2FF目標関節駆動トルク↑τ2_cmd1及び第2FB目標関節駆動トルク↑τ2_cmd2に対応する重み係数であり、本実施形態では、固定的に“1”に設定されている。
また、重み係数wc3は、第3種類状態量に係わる第3FF目標関節駆動トルク↑τ3_cmd1及び第3FB目標関節駆動トルク↑τ3_cmd2に対応する重み係数であり、本実施形態では、例えば図8に示す如く可変的に設定される。
具体的には、本実施系形態では、合成制御入力決定部96は、図8に示す如く、遊脚側脚体2の足平部18が空中を移動する期間(片脚支持期)において、wc3の値が、“0”から“1”まで連続的に増加し、続いて、“1”から“0”まで連続的に減少するように、重み係数wc3の値を設定する。そして、両脚体2R,2Lの足平部18R,18Lが着床する両脚支持期では、合成制御入力決定部96は、重み係数wc3の値を定常的に“0”に維持する。
なお、図8に示す例では、重み係数wc3の値は、片脚支持期において“1”まで連続的に増加した後、直ちに、連続的に減少するように設定されているが、wc3の値が片脚支持期の中間で“1”に保持される期間が設定されていてもよい。また、wc3の値の連続的な増加又は減少は、図8に示した如く直線的な形態で行なうことに限らず、曲線状の形態で(wc3の値の時間的変化率が徐々に変化するように)行なうようにしてもよい。
また、前記式6−8,式6−9のNuw1は、前記第1状態量ヤコビアンJs1と、次式6−10−1により定義される行列Js1#と、重み係数wc1とに応じて、次式6−10により定義される行列である。さらに、式6−8,式6−9のNuw3は、前記第3状態量ヤコビアンJs3と、次式6−11−1により定義される行列Js3#と、重み係数wc3とに応じて、次式6−11により定義される行列である。なお、式6−10−1は、前記式6−4−1と同じ式である。
Nuw1≡I−wc1*Js1T*Js1#T ……式6−10
ただし、Js1#≡WatT*Js1T*(Js1*Wat*Js1T)-1
……式6−10−1
Nuw3≡I−wc3*Js3T*Js3#T
……式6−11
ただし、Js3#≡WatT*Js3T*(Js3*Wat*Js3T)-1
……式6−11−1
なお、式6−10−1及び式6−11−1におけるWatは、重み行列(対角行列)である。本実施形態では、第1実施形態と同様に、前記イナーシャ行列Mの逆行列M-1を重み行列Watとして使用する。
ここで、Nuw1、Nuw3が上記の如く定義される行列であるので、式6−8における第2項の一般化力ベクトル(Nuw3*wc1*↑τ1_cmd)は、ロボット1の各関節の関節駆動トルクを、wc31*↑τ3_cmdに制御した場合と、wc31*↑τ3_cmd1+Nuw3*wc1*↑τ1_cmdに制御した場合とで、第3種類状態量ベクトル↑S3の制御状態を同じにすることができるような、一般化力ベクトルである。
また、式6−8における第3項の一般化力ベクトル(Nuw3*Nuw1*wc21*↑τ2_cmd1)は、ロボット1の各関節の関節駆動トルクを、(wc31*↑τ3_cmd1+Nuw3*wc1*↑τ1_cmd)に制御した場合と、上記式6−8により決定される↑τf1aに制御した場合とで、第3種類状態量ベクトル↑S3及び第1種類状態量ベクトル↑S1の制御状態を同じにすることができるような、一般化力ベクトルである。
このため、式6−8により決定される↑τf1aは、第3種類状態量目標値ベクトル↑S3'_cmdを実現するための第3FF目標関節駆動トルク↑τ3_cmd1に、第3種類状態量ベクトル↑S3に影響を及ぼさない範囲で、第1種類状態量目標値ベクトル↑S1'_cmdを実現するための第1目標関節駆動トルク↑τ1_cmdの成分を重畳し(加え合わせ)、さらに、第3種類状態量ベクトル↑S3及び第1種類状態量ベクトル↑S1に影響を及ぼさない範囲で、第2種類状態量目標値ベクトル↑S2'_cmdを実現するための第2FF目標関節駆動トルク↑τ2_cmd1の成分を重畳し(加え合わせ)た目標関節駆動トルクとしての意味を持つものである。
同様に、式6−9により決定される↑τf2aは、第3種類状態量目標値ベクトル↑S3'_cmdを実現するための第3FB目標関節駆動トルク↑τ3_cmd2に、第3種類状態量ベクトル↑S3に影響を及ぼさない範囲で、第1種類状態量目標値ベクトル↑S1'_cmdを実現するための第1目標関節駆動トルク↑τ1_cmdの成分を重畳し(加え合わせ)、さらに、第3種類状態量ベクトル↑S3及び第1種類状態量ベクトル↑S1に影響を及ぼさない範囲で、第2種類状態量目標値ベクトル↑S2'_cmdを実現するための第2FB目標関節駆動トルク↑τ2_cmd2の成分を重畳し(加え合わせ)た目標関節駆動トルクとしての意味を持つものである。
以上の如く、本実施形態では、合成制御入力決定部96は、重み係数wc1,wc2,wc3を含む前記6−8、式6−9により算出される↑τf1aと、↑τf2aとを前記式6−7により合成することによって、合成目標関節駆動トルク↑τt_cmdが決定される。
この場合、合成目標関節駆動トルク↑τt_cmdは、↑τf1aの高周波側の成分(周波数重みW1の通過周波数帯の成分)と、↑τf2aの低周波側の成分(周波数重みW2の通過周波数帯の成分)とを合成したもの(加え合わせたもの)として決定される。そして、上記高周波側成分を組成する↑τf1aは、第3FF目標関節駆動トルク↑τ3_cmd1に、第1目標関節駆動トルク↑τ1_cmdのうちの↑S3'に影響を及ぼさない成分と、第2FF目標関節駆動トルク↑τ2_cmd1のうちの↑S3'及び↑S1'に影響を及ぼさない成分とを付加してなる目標関節駆動トルクとして決定される。また、上記低周波側成分を組成する↑τf2aは、第3FB目標関節駆動トルク↑τ3_cmd2に、第1目標関節駆動トルク↑τ1_cmdのうちの↑S3'に影響を及ぼさない成分と、第2FB目標関節駆動トルク↑τ2_cmd2のうちの↑S3'及び↑S1'に影響を及ぼさない成分とを付加してなる目標関節駆動トルクとして決定される。
補足すると、本実施形態では、ロボット1の両脚支持期においては、重み係数wc3が“0”に設定されるので、Nuw3=単位行列Iとなる。また、本実施形態では、wc1=wc2=1である。従って、両脚支持期においては、式6−8と式6−9とは、それぞれ、第3種類状態量に係わる目標関節駆動トルク↑τ3_cmd1,↑τ3_cmd2を除外してなる次式6−8a、次式6−9aに一致することとなる。
↑τf1a=↑τ1_cmd+Nuw1*↑τ2_cmd1 ……式6−8a
↑τf2a=↑τ1_cmd+Nuw1*↑τ2_cmd2 ……式6−9a
従って、両脚支持期においては、第3種類状態量が実質的に制御対象から除外されることとなる。
以上が本実施形態における合成制御入力決定部96の処理の詳細である。そして、本実施形態は、以上説明した以外の事項は、前記第1実施形態と同じである。
以上説明した本実施形態によれば、ロボット1の片脚支持期においては、前記合成目標関節駆動トルク↑τt_cmdの高周波側の成分(W1*↑τf1a)は、第3FF目標関節駆動トルク↑τ3_cmd2の高周波側の成分主要成分として含み、↑τt_cmdの低周波側の成分(W2*↑τf2a)は、第3FB目標関節駆動トルク↑τ3_cmd2の低周波側の成分を主要成分として含む。
このため、前記第3種類状態量FF目標値ベクトル↑S3'_cmd1を達成すること(ロボット1の遊脚側脚体2の足平部18の位置及び姿勢を目標とする位置及び姿勢にフィードフォワード則により制御すること)を高い速応性で行いつつ、長期的には、前記第3種類状態量FB目標値ベクトル↑S3'_cmd2を達成すること(ひいては、ロボット1の遊脚側脚体2の足平部18の位置及び姿勢を目標とする位置及び姿勢にフィードバック制御すること)を満足し得るように、ロボット1の各関節の駆動トルクを制御することができる。
これにより、遊脚側脚体2の足平部18の運動を適切に行ない、該足平部18の着地位置及び着地姿勢が目標とする位置及び姿勢からずれるのを効果的に防止することができる。
さらに、片脚支持期において、合成目標関節駆動トルク↑τt_cmdの高周波側の成分(W1*↑τf1a)は、第1目標関節駆動トルク↑τ1_cmdの高周波側の成分のうちの、第3種類状態量ベクトル↑S3に影響を及ぼさない成分と、第2FF目標関節駆動トルク↑τ2_cmd1の高周波側の成分のうちの、第3種類状態量ベクトル↑S3及び第1種類状態量ベクトル↑S1に影響を及ぼさない成分とを含んでいる。
同様に、合成目標関節駆動トルク↑τt_cmdの低周波側の成分(W2*↑τf2a)は、第1目標関節駆動トルク↑τ1_cmdの低周波側の成分のうちの、第3種類状態量ベクトル↑S3に影響を及ぼさない成分と、第2FB目標関節駆動トルク↑τ2_cmd2の高周波側の成分のうちの、第3種類状態量ベクトル↑S3及び第1種類状態量ベクトル↑S1に影響を及ぼさない成分とを含んでいる。
このため、第3種類状態量ベクトル↑S3の時間的変化率↑S3'の制御性を損なうことなく、第1種類状態量ベクトル↑S1の時間的変化率↑S1'の制御性を高めることができると共に、第3種類状態量ベクトル↑S3の時間的変化率↑S3'の制御性と第1種類状態量ベクトル↑S1の時間的変化率↑S1'の制御性とを損なうことなく、第2種類状態量ベクトル↑S2の時間的変化率↑S2'の制御性を高めることができる。
この場合、第1種類状態量ベクトル↑S1の時間的変化率↑S1'の制御性については、高周波側と低周波側と両方において、前記第1種類状態量目標値ベクトル↑S1'_cmdを達成すること(ひいては、ロボット1の全体重心の位置を目標位置に制御すること)を適切に実現できる。
また、第2種類状態量ベクトル↑S2の時間的変化率↑S2'の制御性については、前記第2種類状態量FF目標値ベクトル↑S2'_cmd1を達成すること(ロボット1の各関節の関節角を目標とする関節角にフィードフォワード則により制御すること)を高い速応性で行いつつ、長期的には、前記第2種類状態量FB目標値ベクトル↑S2'_cmd2を達成すること(ロボット1の各関節の関節角を目標とする関節角にフィードバック制御すること)を実現できる。
一方、ロボット1の両脚支持期においては、前記合成目標関節駆動トルク↑τt_cmdの高周波側の成分(W1*↑τf1a)と、低周波側の成分(W2*↑τf2a)とは、それぞれ、第1目標関節駆動トルク↑τ1_cmdの高周波側の成分、低周波側の成分を主要成分として含む。
このため、高周波側と低周波側と両方において、前記第1種類状態量目標値ベクトル↑S1'_cmdを達成すること(ひいては、ロボット1の全体重心の位置を目標位置に制御すること)を適切に実現できる。
さらに、両脚支持期において、合成目標関節駆動トルク↑τt_cmdの高周波側の成分(W1*↑τf1a)は、第2FF目標関節駆動トルク↑τ2_cmd1の高周波側の成分のうちの、第1種類状態量ベクトル↑S1に影響を及ぼさない成分を含んでいる。同様に、合成目標関節駆動トルク↑τt_cmdの低周波側の成分(W2*↑τf2a)は、第2FB目標関節駆動トルク↑τ2_cmd2の低周波側の成分のうちの、第1種類状態量ベクトル↑S1に影響を及ぼさない成分を含んでいる。
このため、第1種類状態量ベクトル↑S1の時間的変化率↑S1'の制御性を損なうことなく、前記第2種類状態量FF目標値ベクトル↑S2'_cmd1を達成すること(ロボット1の各関節の関節角を目標とする関節角にフィードフォワード則により制御すること)を高い速応性で行いつつ、長期的には、前記第2種類状態量FB目標値ベクトル↑S2'_cmd2を達成すること(ロボット1の各関節の関節角を目標とする関節角にフィードバック制御すること)を実現できる。
また、両脚支持期において、第3種類状態量が実質的に制御対象から除外され、合成目標関節駆動トルク↑τt_cmdは、第3FF目標関節駆動トルク↑τ3_cmd1及び第3FB目標関節駆動トルク↑τ3_cmd2に依存しないものとなる。このため、両脚支持期における各足平部18、すなわち、床面と接触して拘束を受けた状態の足平部18の位置及び姿勢を強制的に目標とする位置及び姿勢に修正するような制御が行なわれないようにすることができる。このため、電動モータ50に過大な動力(トルク)が要求されることとなるような事態が発生するのを防止することができる。
さらに、前記重み係数wc3を、その値が連続的に変化するように設定しているので、第3種類状態量を制御対象として含む片脚支持期と、該第3種類状態量が制御対象から除外される両脚支持期との切替わりの前後で、合成目標関節駆動トルク↑τt_cmdが不連続的に変化するようなことを防止し、該↑τt_cmdを連続的に変化させるようにすることができる。その結果、ロボット1の動作をスムーズに行なうことができる。
また、本実施形態では、前記式5−20に基づいて、目標関節駆動トルク↑τ1_cmd,↑τ2_cmd1,↑τ2_cmd2、↑τ3_cmd1,↑τ3_cmd2が決定される。このため、第1実施形態と同様に、ロボット1に作用する外力の値(これは外乱等の影響を受けやすい)を特定することなく、↑S1'_cmd、↑S2'_cmd1、↑S2'_cmd2,↑S3'_cmd1,↑S3'_cmd2をそれぞれ実現し得る適切な↑τ1_cmd,↑τ2_cmd1,↑τ2_cmd2、↑τ3_cmd1,↑τ3_cmd2を決定できる。
ここで、以上説明した実施形態と本発明との対応関係について補足しておく。
本実施形態では、前記状態量別制御入力決定部95により、本発明における状態量別制御入力決定手段が実現される。この場合、前記第1種類状態量、第2種類状態量及び第3種類状態量が本発明における複数種類の状態量に相当する。従って、本実施形態では、制御対象とする状態量の種類数はMは“3”である。
そして、第1種類状態量目標値ベクトル↑S1'_cmdが第1種類状態量に関する2つの目標値に相当し、前記第1目標関節駆動トルク↑τ1_cmdが↑S1'_cmdを実現するための状態量別制御入力に相当する。
また、第2種類状態量FF目標値ベクトル↑S2'_cmd1及び第2種類状態量FB目標値ベクトル↑S2'_cmd2が第2種類状態量に関する2つの目標値に相当し、第2FF目標関節駆動トルク↑τ2_cmd1及び第2FB目標関節駆動トルク↑τ2_cmd2がそれぞれ、↑S2'_cmd1、↑S2'_cmd2を実現するための状態量別制御入力に相当する。
また、第3種類状態量FF目標値ベクトル↑S3'_cmd1及び第3種類状態量FB目標値ベクトル↑S3'_cmd2が第3種類状態量に関する2つの目標値に相当し、第3FF目標関節駆動トルク↑τ3_cmd1及び第3FB目標関節駆動トルク↑τ3_cmd2がそれぞれ、↑S3'_cmd1、↑S3'_cmd2を実現するための状態量別制御入力に相当する。
また、前記合成制御入力決定部95により、本発明における合成制御入力決定手段が実現される。この場合、前記式6−7により表される処理により算出される合成目標関節駆動トルク↑τt_cmdが、本発明における合成制御入力τtotalに相当し、式6−7のW1*↑τf1aと、W2*↑τf2aとがそれぞれ、本発明における周波数領域別制御入力(W(i)*τf(i))に相当し、周波数重みW1,W2のそれぞれの通過可能な周波数帯が、本発明における周波数領域に相当する。なお、本実施形態では、周波数領域の個数nは2つである。
また、本実施形態では、↑τf1a、↑τf2aがそれぞれ、本発明における(前記式01、02bにおける)τf(i)に相当する。この場合、↑τ3_cmd1,↑τ1_cmd,↑τ2_cmd1がそれぞれ、↑τf1a(τf(1))の組成成分τ(1,1)、τ(1,2)、τ(1,3)に相当し、行列Nuw3、Nuw1が本発明におけるNuw(1,1)、Nuw(1,2)に相当し、重み係数wc3、wc1、wc2がそれぞれ、本発明におけるwc(1,1)、wc(1,2)、wc(1,3)に相当する。
また、↑τ3_cmd2,↑τ1_cmd,↑τ2_cmd2がそれぞれ↑τf2a(τf(2))の組成成分τ(2,1)、τ(2,2)、τ(2,3)に相当する。この場合、↑τ3_cmd2,↑τ1_cmd,↑τ2_cmd2がそれぞれ、↑τf2a(τf(2))の組成成分τ(2,1)、τ(2,2)、τ(2,3)に相当し、行列Nuw3、Nuw1が本発明におけるNuw(2,1)、Nuw(2,2)に相当し、重み係数wc3、wc1、wc2がそれぞれ、本発明におけるwc(2,1)、wc(2,2)、wc(2,3)に相当する。
さらに、本実施形態では、行列Nuw3、Nuw1のそれぞれの階数rank(Nuw3)、rank(Nuw1)のうちのrank(Nuw3)は、ロボット1のほとんどの姿勢の場合、5又は6であり、rank(Nuw1)は6である。このため、rank(Nuw(1,1))≧rank(Nuw(1,2))という関係、並びに、rank(Nuw(2,1))≧rank(Nuw(2,2))という関係が実現される。
なお、本実施形態では、↑τf1a(τf(1))の組成成分の個数m(1)と、↑τf2a(τf(2))の組成成分の個数m(2)とはいずれも“1”である。
また、本実施形態では、前記行列Js1#、Js2#が、それぞれ本発明におけるJs#(i,j)に相当し、Js1、Js2がそれぞれ、本発明におけるJs(i,j)に相当し、↑S1,↑S2,↑S3がそれぞれ、本発明におけるS(i,j)に相当する。また、本実施形態におけるM、↑qがそれぞれ、本発明におけるイナーシャ行列M、一般化変数ベクトルqに相当する。なお、本実施形態では、本発明における重み行列Wdを単位行列とし、Wat=M-1としている。
また、本実施形態における前記コンプライアンス制御部97及び関節駆動制御部73により、本発明におけるアクチュエータ制御手段が実現される。
また、本実施形態では、脚体2R,2Lが本発明における所定の部位に相当し、ロボット1の歩行動作時の床が本発明における接触対象部に相当する。そして、前記第3種類状態量(↑S3)が本発明における所定の部位の運動状態量に相当し、第3FF目標関節駆動トルク↑τ3_cmd1及び第3FB目標関節駆動トルク↑τ3_cmd2が本発明における第(ia,ja)状態量別制御入力τ(ia,ja)に相当する。また、wc3が、τ(ia,ja)に対応する重み係数に相当する。
[変形態様について]
次に、以上説明した実施形態の変形態様をいくつか説明しておく。
次に、以上説明した実施形態の変形態様をいくつか説明しておく。
本実施形態では、ロボット1を、脚式移動ロボットである2足歩行ロボットとしたが、本発明におけるロボットは、脚式移動ロボット以外の移動ロボット(例えば、車輪により移動する形態のロボット)でもよい。あるいは、本発明におけるロボットは、いずれかのリンクが床等に固定された設置型のロボットであってもよい。
また、ロボット1のアクチュエータは、電動モータ50に限らず、油圧式のアクチュエータであってもよい。
また、ロボット1の歩行動作を行う場合に限らず、走行動作を行なったり、あるいは、ハンド30R,30Lによる作業と移動動作と並行して行なうような場合にも本発明を適用できる。
本発明における前記式01及び01aを適用した前記第1実施形態では、制御対象の状態量を第1種類状態量及び第2種類状態量の2種類としたが、より多くの種類の状態量を制御対象してもよい。より具体的な例を挙げると、例えば、前記第1種類状態量、第2種類状態量に加えて、ロボット1の頭部5の姿勢(ピッチ軸周りとヨー軸周りとの2軸周りの姿勢)の時間的変化率(角速度)を制御対象の状態量に加えてもよい。この場合の合成目標関節駆動トルク↑τt_cmd(合成制御入力)の算出例を以下に示す。
ロボット1の頭部5のロボット1の頭部5の姿勢の角速度(ここではピッチ軸周りとヨー軸周りとの2軸周りの角速度)を制御対象の第4種類状態量とし、第4種類状態量の2つの成分を並べたベクトルを第4種類状態量ベクトル↑S4、この↑S4と一般化変数ベクトル↑qの1階微分値↑q'との間の関係を、次式3−4により表現するヤコビアンを第4状態量ヤコビアンJs4とする。
↑S4=Js4*↑q' ……式3−4
そして、↑S4の1階微分値↑S4'の目標値↑S4'_cmdを第4種類状態量に関する目標値とし、この↑S4'_cmdを、頭部4の実際の姿勢の計測値Xhead_actと該姿勢に対する目標値Xhead_cmdとの偏差ΔXhead(=Xhead_act−Xhead_cmd)と、該偏差ΔXheadの1階微分値ΔXhead'とから、フィードバック制御則、例えばPD則により逐次決定する。すなわち、次式4−6により↑S4'_cmdを逐次決定する。
↑S4'_cmd=Kp5*↑ΔXhead+Kv5*↑ΔXhead' ……式4−6
さらに、この式4−6により算出される目標値↑S4'_cmd(以降、第4種類状態量目標値ベクトル↑S4'_cmdという)を実現するための目標関節駆動トルク↑τ4_cmd(以降、第4目標関節駆動トルク↑τ4_cmdという)を、第1実施形態で説明した↑τ1_cmd及び↑τ2_cmdの算出手法を同じ手法によって(前記式5−22の右辺の演算によって)、決定する。
そして、例えば、次式6−13,6−14の演算処理によりそれぞれ算出されるベクトル↑τf1b、↑τf2bを、式6−12の演算処理により合成することによって、合成目標関節駆動トルク↑τt_cmdを決定する。
↑τt_cmd=W1*↑τf1b+W2*↑τf2b ……式6−12
ただし、
↑τf1b=↑τ1_cmd+Nu1*↑τ2_cmd1 ……式6−13
↑τf2b=↑τ1_cmd+Nu1*↑τ4_cmd
+Nu1*Nu4*↑τ2_cmd2 ……式6−14
なお、この例では、↑τ1_cmdは、第1実施形態のものと同じ第1目標関関節駆動トルクであり、↑τ2_cmd1,↑τ2_cmd2は、それぞれ第2実施形態のものと同じ第2FF目標関節駆動トルク、第2FB目標関節駆動トルクである。また、W1,W2は、第1実施形態と同じ周波数重みである。
また、行列Nu1は、第1実施形態における前記式6−4により定義される行列であり、行列Nu4は、前記第4状態量ヤコビアンJs4と、次式6−15−1により定義される行列Js4#とに応じて、次式6−15により定義される行列である。
Nu4≡I−Js4T*Js4#T ……式6−15
ただし、Js4#≡WatT*Js4T*(Js4*Wat*Js4T)-1
……式6−15−1
……式6−15−1
なお、式6−15−1における重み行列Watとしては、前記イナーシャ行列Mの逆行列M-1を使用する。
以上のようにして、第1種類状態量に関する第1種類状態量目標値ベクトル↑S1'_cmdを実現するための↑τ1_cmdと、第4種類状態量に関する第4種類状態量目標値ベクトル↑S4'_cmdを実現するための↑τ4_cmdと、第2種類状態量に関する第2種類状態量FF目標値ベクトル↑S2'_cmd1,↑S2'_cmd2をそれぞれ実現するための↑τ2_cmd1,↑τ2_cmd2とを合成した合成目標関節駆動トルク↑τt_cmdを決定できる。
なお、この場合、式6−14において、行列Nu1(これは本発明の式02aにおけるτ(2,1)に相当する)の階数は、ロボットのほとんどの姿勢の場合、5又は6、行列Nu4(これは本発明の式02aにおけるτ(2,2)に相当する)の階数は2であるから、rank(Nu1)≧rank(Nu4)という関係が成立することとなる。
また、本発明における前記式01及び01bを適用した前記第2実施形態では、制御対象の状態量を第1種類状態量、第2種類状態量、第3種類状態量の3種類としたが、例えば、制御対象の状態量の種類数は、2種類であってもよく、あるいは、さらに多くの状態量(例えば、前記第4種類状態量)を制御対象に含めるようにしてもよい。
また、前記各実施形態では、接触部位代表点目標加速度↑Cを“0”としたが、接触部位代表点a1〜a4の他の運動形態の目標を↑Cとして設定するようにしてもよい。例えば、接触部位代表点a1〜a4の運動パターンを正弦波状のパターンに設定してもよい。また、例えば、ロボット1がエレベータ等、加速度が発生する系に存在するような場合には、その系の加速度(慣性座標系から見た加速度)を接触部位代表点目標加速度↑Cに付加するようにしてもよい。
また、前記コンプライアンス制御部87,97を省略してもよい。すなわち、合成制御入力決定部87,97が決定した目標関節駆動トルク↑τt_cmdをそのまま使用して、ロボット1の各関節の駆動トルクを制御するようにしてもよい。
また、足平部18R,18Lの接地時における足平部18R,18Lや床面の弾性変形が十分に微小なものとなる場合には、前記接触部弾性変形補償部88,98を省略し、歩容生成部71で生成された目標関節角を状態量目標値決定部84,94に入力するようにしてもよい。
脚式移動ロボット等のロボットの種々様々な動作状況で、複数種類の状態量をそれぞれに対応する目標値に効率よく制御するようにロボットの動作制御を行なうことができる。
1…ロボット、2…上体リンク、3…脚リンク、4…腕リンク、5…頭部(リンク)、10…下側上体(リンク)、11…上側上体(リンク)、12,19〜24,31〜37,39,40…関節、14…大腿部(リンク)、16…下腿部(リンク)、18…足平部(リンク)、26…上腕部(リンク)、28…前腕部(リンク)、30…ハンド(リンク)、60…制御装置、73…関節駆動制御部(アクチュエータ制御手段)、85,95…状態量別制御入力決定部(状態量別制御入力決定手段)、86,96…合成制御入力決定部(合成制御入力決定手段)、87,97…コンプライアンス制御手段(アクチュエータ制御手段)。
Claims (8)
- 関節を介して相互に連結された複数のリンクと、各関節を駆動するアクチュエータとを備えるロボットの制御装置であって、
前記ロボットの各関節の変位量を成分として少なくとも含むベクトルである該ロボットの一般化変数ベクトルの1つ以上の成分値に依存してそれぞれの値が規定される複数種類の状態量のそれぞれに関する目標値が入力され、各種類の状態量毎に、該状態量に対応する目標値を実現するために必要な前記アクチュエータの動作を規定する制御入力である状態量別制御入力を、少なくとも該状態量に対応する目標値に基づいて決定する状態量別制御入力決定手段と、
あらかじめ設定された複数の周波数領域のそれぞれにおける前記アクチュエータの動作を規定する制御入力である周波数領域別制御入力を、前記複数種類の状態量のそれぞれに対応して決定された前記状態量別制御入力のうちの1つ以上の状態量別制御入力に応じて決定しつつ、前記複数の周波数領域のそれぞれに対応する該周波数領域別制御入力を合成する処理を実行することによって、合成制御入力を決定する合成制御入力決定手段と、
少なくとも前記合成制御入力に応じて、前記アクチュエータの動作を制御するアクチュエータ制御手段とを備え、
前記合成制御入力決定手段が実行する処理は、前記複数の周波数領域のうちの少なくともいずれか1つの周波数領域に対応する前記周波数領域別制御入力を、前記複数種類の状態量のそれぞれに対応する状態量別制御入力のうちの複数の状態量別制御入力を相互に非干渉に合成することによって決定する処理を含むことを特徴とするロボットの制御装置。 - 請求項1記載のロボットの制御装置において、
前記合成制御入力をτtotal、前記複数の周波数領域の総個数をn、n個の該周波数領域のそれぞれを第i周波数領域(i=1,2,…,n)、第i周波数領域が通過可能な周波数帯となるように周波数通過特性が設定された周波数重みをW(i)としたとき、前記合成制御入力決定手段は、次式02aにより表される処理によって算出されるn個のτf(i)(i=1,2,…,n)と前記周波数重みW(i)(i=1,2,…,n)とから次式01により表される処理によって、前記合成制御入力τtotalを決定することを特徴とするロボットの制御装置。
但し、
W(i)*τf(i):第i周波数領域に対応する前記周波数領域別制御入力である第i周波数領域別制御入力
m(i):前記複数種類の状態量のそれぞれに対応する前記状態量別制御入力のうち、第i周波数領域別制御入力W(i)*τf(i)の組成成分として決定される状態量別制御入力の個数であり、所定の制約条件を満たす範囲内で、1≦m(i)≦M(M:前記状態量の種類数)、且つ、いずれかのiに対してm(i)>1という条件とを満たすように設定された整数
τ(i,j):前記所定の制約条件を満たす範囲内で、第i周波数別制御入力W(i)*τf(i)の組成成分として決定されたm(i)個の状態量別制御入力τ(i,j)(j=1,2,…,m(i))のうちの第j番目の状態量別制御入力である第(i,j)状態量別制御入力(ベクトル)
前記所定の制約条件:全てのi(i=1,2,…,n)に対してm(i)=Mが成立し、且つ、互いに異なる任意のiの値i1、i2(1≦i1≦n、1≦i2≦n)に対してτ(i1,j)=τ(i2,j)が1からMまでのすべてのj(j=1,2,…,M)に対して成立する、ということがないという条件
Nu(i,j):Nu(i,j)≡I−Js(i,j)T*(Js#(i,j))Tにより定義される行列
Js#(i,j):Js#(i,j)≡(Wat)T*Js(i,j)T*(Js(i,j)*Wat*Js(i,j)T)-1により定義される行列
I:単位行列
Js(i,j):S(i,j)=Js(i,j)・q'となるヤコビアン
Wat:Wat≡Wd*M-1により定義される行列
Wd:あらかじめ設定された重み行列(対角行列)
M:q''を一般化力ベクトルに変換するイナーシャ行列
(M*q''=一般化力ベクトル)
S(i,j):第(i,j)状態量別制御入力に対応する状態量(ベクトル又はスカラー)
q:一般化変数ベクトル
q':qの1階微分値(=dq/dt)
q'':qの2階微分値(=d2q/dt2) - 請求項1記載のロボットの制御装置において、
前記合成制御入力をτtotal、前記複数の周波数領域の総個数をn、n個の該周波数領域のそれぞれを第i周波数領域(i=1,2,…,n)、第i周波数領域が通過可能な周波数帯となるように周波数通過特性が設定された周波数重みをW(i)としたとき、前記合成制御入力決定手段は、次式02bにより表される処理によって算出されるn個のτf(i)(i=1,2,…,n)と前記周波数重みW(i)(i=1,2,…,n)とから次式01により表される処理によって、前記合成制御入力τtotalを決定することを特徴とするロボットの制御装置。
但し、
W(i)*τf(i):第i周波数領域に対応する前記周波数領域別制御入力である第i周波数領域別制御入力
m(i):前記複数種類の状態量のそれぞれに対応する前記状態量別制御入力のうち、第i周波数領域別制御入力W(i)*τf(i)の組成成分として決定される状態量別制御入力の個数であり、所定の制約条件を満たす範囲内で、1≦m(i)≦M(M:前記状態量の種類数)、且つ、いずれかのiに対してm(i)>1という条件とを満たすように設定された整数
τ(i,j):前記所定の制約条件を満たす範囲内で、第i周波数別制御入力W(i)*τf(i)の組成成分として決定されたm(i)個の状態量別制御入力τ(i,j)(j=1,2,…,m(i))のうちの第j番目の状態量別制御入力である第(i,j)状態量別制御入力(ベクトル)
前記所定の制約条件:全てのi(i=1,2,…,n)に対してm(i)=Mが成立し、且つ、互いに異なる任意のiの値i1、i2(1≦i1≦n、1≦i2≦n)に対してτ(i1,j)=τ(i2,j)が1からMまでのすべてのj(j=1,2,…,M)に対して成立する、ということがないという条件
wc(i,j):第(i,j)状態量別制御入力τ(i,j)に対応する重み係数であり、0≦wc(i,j)≦1の範囲内で一定値に維持されるか、又は連続的に変化するように設定された重み係数範囲内で固定的又は可変的に設定された重み係数
Nuw(i,j):Nuw(i,j)≡I−wc(i,j)*Js(i,j)T*(Js#(i,j))Tにより定義される行列
Js#(i,j):Js#(i,j)≡(Wat)T*Js(i,j)T*(Js(i,j)*Wat*Js(i,j)T)-1により定義される行列
I:単位行列
Js(i,j):S(i,j)=Js(i,j)・q'となるヤコビアン
Wat:Wat≡Wd*M-1により定義される行列
Wd:あらかじめ設定された重み行列(対角行列)
M:q''を一般化力ベクトルに変換するイナーシャ行列
(M*q''=一般化力ベクトル)
S(i,j):第(i,j)状態量別制御入力に対応する状態量(ベクトル又はスカラー)
q:一般化変数ベクトル
q':qの1階微分値(=dq/dt)
q'':qの2階微分値(=d2q/dt2) - 請求項3記載のロボットの制御装置において、
前記ロボットはその所定の部位を該ロボットの外界の接触対象部に接触させる動作と該接触対象部から離脱させる動作とを含む運動を行なうロボットであると共に、前記複数種類の状態量は、前記ロボットの所定の部位を前記接触対象部から離脱させた状態での該所定の部位の運動状態量を、前記複数の周波数領域のうちの少なくとも1つの周波数領域における制御対象として含んでおり、
前記所定の部位の運動状態量を制御対象として含む周波数領域を第ia周波数領域とし、該第ia周波数領域に対応する第ia周波数領域別制御入力(W(ia)*τf(i))の組成成分となる前記状態量別制御入力のうちの前記所定の部位の運動状態量に対応する状態量別制御入力を第(ia,ja)状態量別制御入力τ(ia,ja)としたとき、該τ(ia,ja)に対応する前記重み係数wc(ia,ja)は、前記所定の部位が前記接触対象部に接触する期間では、該重み係数wc(ia,ja)の値が“0”に維持され、且つ、前記所定の部位が前記接触対象部から離脱する期間では、該重み係数wc(ia,ja)の値が“0”から“1”まで連続的に増加した後、“1”から“0”まで連続的に減少するように設定されることを特徴とするロボットの制御装置。 - 請求項2記載のロボットの制御装置において、
前記個数m(i)がm(i)>1である前記第i周波数領域別制御入力W(i)*τf(i)の組成成分として決定される状態量別制御入力τ(i,j)(j=1,2,…,m(i))は、前記式02aの演算に用いるm(i)個の前記行列Nu(i,j)(j=1,2,…,m(i))が、rank(Nu(i,1))≧rank(Nu(i,2))≧……≧rank(Nu(i,m(i)))という関係を満たすように前記複数種類の状態量に対応するM個の状態量別制御入力の中から選定された状態量別制御入力であることを特徴とするロボットの制御装置。 - 請求項3記載のロボットの制御装置において、
前記個数m(i)がm(i)>1である前記第i周波数領域別制御入力W(i)*τf(i)の組成成分として決定される状態量別制御入力τ(i,j)(j=1,2,…,m(i))は、前記式02bの演算に用いるm(i)個の前記行列Nuw(i,j)(j=1,2,…,m(i))が、rank(Nuw(i,1))≧rank(Nuw(i,2))≧……≧rank(Nuw(i,m(i)))という関係を満たすように前記複数種類の状態量に対応するM個の状態量別制御入力の中から選定された状態量別制御入力であることを特徴とするロボットの制御装置。
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
JP2011547361A JP5506823B2 (ja) | 2009-12-28 | 2010-09-13 | ロボットの制御装置 |
Applications Claiming Priority (4)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
JP2009297040 | 2009-12-28 | ||
JP2009297040 | 2009-12-28 | ||
PCT/JP2010/065753 WO2011080949A1 (ja) | 2009-12-28 | 2010-09-13 | ロボットの制御装置 |
JP2011547361A JP5506823B2 (ja) | 2009-12-28 | 2010-09-13 | ロボットの制御装置 |
Publications (2)
Publication Number | Publication Date |
---|---|
JPWO2011080949A1 true JPWO2011080949A1 (ja) | 2013-05-09 |
JP5506823B2 JP5506823B2 (ja) | 2014-05-28 |
Family
ID=44226370
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
JP2011547361A Active JP5506823B2 (ja) | 2009-12-28 | 2010-09-13 | ロボットの制御装置 |
Country Status (4)
Country | Link |
---|---|
US (1) | US8818553B2 (ja) |
JP (1) | JP5506823B2 (ja) |
DE (1) | DE112010005024B4 (ja) |
WO (1) | WO2011080949A1 (ja) |
Families Citing this family (25)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
US8504208B2 (en) * | 2011-05-25 | 2013-08-06 | Honda Motor Co., Ltd. | Mobile object controller and floor surface estimator |
FR3002047B1 (fr) * | 2013-02-08 | 2015-02-27 | Inst Nat Rech Inf Automat | Procede de commande d'un robot deformable, module et programme d'ordinateur associes |
JP5642214B2 (ja) * | 2013-02-15 | 2014-12-17 | 株式会社神戸製鋼所 | 多関節ロボットの弾性変形補償制御装置 |
DE102013102403B4 (de) * | 2013-03-11 | 2022-02-17 | Minebea Mitsumi Inc. | Verfahren zum Betreiben einer Steuerschaltung, Steuerschaltung, elektromechanische Betriebseinheit und Fahrzeugsitz |
JP5758976B2 (ja) * | 2013-12-20 | 2015-08-05 | ファナック株式会社 | 数値制御装置付き工作機械 |
JP5897644B2 (ja) * | 2014-06-02 | 2016-03-30 | 株式会社神戸製鋼所 | ロボットの制御装置 |
US9718187B2 (en) * | 2014-06-11 | 2017-08-01 | Canon Kabushiki Kaisha | Robot controlling method, robot apparatus, program, recording medium, and method for manufacturing assembly component |
KR101571313B1 (ko) * | 2014-06-20 | 2015-11-26 | 한국과학기술연구원 | 무게중심 제어를 통한 로봇의 자세 제어 시스템 |
JP5972941B2 (ja) * | 2014-08-06 | 2016-08-17 | ファナック株式会社 | 数値制御装置付き工作機械 |
US9618937B1 (en) | 2014-08-25 | 2017-04-11 | Google Inc. | Slip detection using robotic limbs |
US10081098B1 (en) | 2014-08-25 | 2018-09-25 | Boston Dynamics, Inc. | Generalized coordinate surrogates for integrated estimation and control |
US9387588B1 (en) | 2014-08-25 | 2016-07-12 | Google Inc. | Handling gait disturbances with asynchronous timing |
JP6228097B2 (ja) * | 2014-10-06 | 2017-11-08 | 本田技研工業株式会社 | 移動ロボット |
US9446518B1 (en) * | 2014-11-11 | 2016-09-20 | Google Inc. | Leg collision avoidance in a robotic device |
US9499218B1 (en) | 2014-12-30 | 2016-11-22 | Google Inc. | Mechanically-timed footsteps for a robotic device |
US9594377B1 (en) * | 2015-05-12 | 2017-03-14 | Google Inc. | Auto-height swing adjustment |
US9586316B1 (en) | 2015-09-15 | 2017-03-07 | Google Inc. | Determination of robotic step path |
JP7058929B2 (ja) * | 2015-10-27 | 2022-04-25 | キヤノン株式会社 | 駆動装置、ロボット装置、制御方法、物品の製造方法、制御プログラム、および記録媒体 |
JP6498597B2 (ja) * | 2015-12-14 | 2019-04-10 | 本田技研工業株式会社 | 移動ロボットの制御装置 |
JP6483014B2 (ja) * | 2015-12-25 | 2019-03-13 | 本田技研工業株式会社 | 移動ロボットの制御装置 |
US9789919B1 (en) | 2016-03-22 | 2017-10-17 | Google Inc. | Mitigating sensor noise in legged robots |
US10286556B2 (en) * | 2016-10-16 | 2019-05-14 | The Boeing Company | Method and apparatus for compliant robotic end-effector |
US11453128B2 (en) * | 2017-06-29 | 2022-09-27 | Sony Interactive Entertainment Inc. | Robot control apparatus, control method and control program |
CN107390685B (zh) * | 2017-07-14 | 2020-10-16 | 深圳市优必选科技有限公司 | 一种机器人的回充控制方法、机器人及机器人系统 |
US11084445B2 (en) * | 2019-02-06 | 2021-08-10 | Ford Global Technologies, Llc | Energy-absorbing knee bolster |
Citations (7)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
JPH05305579A (ja) * | 1992-04-30 | 1993-11-19 | Honda Motor Co Ltd | 脚式移動ロボットの歩行制御装置 |
JPH0778030A (ja) * | 1993-09-07 | 1995-03-20 | Citizen Watch Co Ltd | コンプライアンス制御装置 |
JPH10277969A (ja) * | 1997-01-31 | 1998-10-20 | Honda Motor Co Ltd | 脚式移動ロボットの制御装置 |
WO2005109139A1 (ja) * | 2003-07-24 | 2005-11-17 | Keio University | 位置・力制御装置 |
JP2006105054A (ja) * | 2004-10-07 | 2006-04-20 | Toyota Motor Corp | 内燃機関の空燃比制御装置 |
JP2007160446A (ja) * | 2005-12-12 | 2007-06-28 | Honda Motor Co Ltd | 脚式移動ロボット制御装置および脚式移動ロボット、並びに、脚式移動ロボット制御方法 |
JP2008211137A (ja) * | 2007-02-28 | 2008-09-11 | Anritsu Corp | 波長安定化光源 |
Family Cites Families (7)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN1067001C (zh) * | 1995-09-11 | 2001-06-13 | 株式会社安川电机 | 机器人控制方法 |
EP1514777B1 (en) * | 1997-01-31 | 2009-03-11 | Honda Giken Kogyo Kabushiki Kaisha | Control system of legged mobile robot |
JP2002301674A (ja) * | 2001-04-03 | 2002-10-15 | Sony Corp | 脚式移動ロボット及びその運動教示方法、並びに記憶媒体 |
DE60237999D1 (de) * | 2001-12-28 | 2010-11-25 | Honda Motor Co Ltd | Vorrichtung zur steuerung des ganges für mobilen roboter mit beinen |
US7054718B2 (en) * | 2002-10-11 | 2006-05-30 | Sony Corporation | Motion editing apparatus and method for legged mobile robot and computer program |
JP4501601B2 (ja) * | 2004-09-03 | 2010-07-14 | トヨタ自動車株式会社 | 脚式ロボットとその制御方法ならびに歩容データの作成装置と作成方法 |
JP4951732B2 (ja) * | 2006-05-02 | 2012-06-13 | ソニー株式会社 | 駆動方法、駆動制御装置及びロボット |
-
2010
- 2010-09-13 JP JP2011547361A patent/JP5506823B2/ja active Active
- 2010-09-13 DE DE112010005024.4T patent/DE112010005024B4/de not_active Expired - Fee Related
- 2010-09-13 US US13/516,921 patent/US8818553B2/en active Active
- 2010-09-13 WO PCT/JP2010/065753 patent/WO2011080949A1/ja active Application Filing
Patent Citations (7)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
JPH05305579A (ja) * | 1992-04-30 | 1993-11-19 | Honda Motor Co Ltd | 脚式移動ロボットの歩行制御装置 |
JPH0778030A (ja) * | 1993-09-07 | 1995-03-20 | Citizen Watch Co Ltd | コンプライアンス制御装置 |
JPH10277969A (ja) * | 1997-01-31 | 1998-10-20 | Honda Motor Co Ltd | 脚式移動ロボットの制御装置 |
WO2005109139A1 (ja) * | 2003-07-24 | 2005-11-17 | Keio University | 位置・力制御装置 |
JP2006105054A (ja) * | 2004-10-07 | 2006-04-20 | Toyota Motor Corp | 内燃機関の空燃比制御装置 |
JP2007160446A (ja) * | 2005-12-12 | 2007-06-28 | Honda Motor Co Ltd | 脚式移動ロボット制御装置および脚式移動ロボット、並びに、脚式移動ロボット制御方法 |
JP2008211137A (ja) * | 2007-02-28 | 2008-09-11 | Anritsu Corp | 波長安定化光源 |
Also Published As
Publication number | Publication date |
---|---|
US20120259463A1 (en) | 2012-10-11 |
WO2011080949A1 (ja) | 2011-07-07 |
US8818553B2 (en) | 2014-08-26 |
JP5506823B2 (ja) | 2014-05-28 |
DE112010005024T5 (de) | 2012-10-31 |
DE112010005024B4 (de) | 2016-05-25 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
JP5506823B2 (ja) | ロボットの制御装置 | |
JP5506617B2 (ja) | ロボットの制御装置 | |
JP5506618B2 (ja) | ロボットの制御装置 | |
US7603234B2 (en) | Controller of legged mobile robot | |
EP2347867B1 (en) | Walking control apparatus of robot and method of controlling the same | |
CN102139714B (zh) | 仿人机器人及其步行控制方法 | |
US8428780B2 (en) | External force target generating device of legged mobile robot | |
US6505096B2 (en) | Posture control system of legged mobile robot | |
US7715944B2 (en) | Gait generating device of mobile robot | |
US8442680B2 (en) | Motion state evaluation apparatus of legged mobile robot | |
US8825213B2 (en) | Gait generating device for legged mobile robot and operational target generating device for robot | |
US8868240B2 (en) | Walking robot and pose control method thereof | |
US8868239B2 (en) | Walking control apparatus of robot and method of controlling the same | |
US20050104548A1 (en) | Attitude control device of mobile robot | |
US20090321150A1 (en) | Walking robot and method of controlling the same | |
US8396593B2 (en) | Gait generating device of legged mobile robot | |
US8442681B2 (en) | Desired motion evaluation apparatus of legged mobile robot | |
JP2013094935A (ja) | ロボットアーム装置 | |
JP5616289B2 (ja) | 床面推定装置 | |
US8670869B2 (en) | Robot controller | |
CN113467246B (zh) | 一种双足机器人偏摆力矩补偿方法 | |
JP5616288B2 (ja) | 移動体の制御装置 | |
Ranjan et al. | Kinematics and Control of Biped Robot |
Legal Events
Date | Code | Title | Description |
---|---|---|---|
A621 | Written request for application examination |
Free format text: JAPANESE INTERMEDIATE CODE: A621 Effective date: 20130530 |
|
TRDD | Decision of grant or rejection written | ||
A01 | Written decision to grant a patent or to grant a registration (utility model) |
Free format text: JAPANESE INTERMEDIATE CODE: A01 Effective date: 20140225 |
|
A61 | First payment of annual fees (during grant procedure) |
Free format text: JAPANESE INTERMEDIATE CODE: A61 Effective date: 20140318 |
|
R150 | Certificate of patent or registration of utility model |
Ref document number: 5506823 Country of ref document: JP Free format text: JAPANESE INTERMEDIATE CODE: R150 |