JP3637387B2 - 脚を有する機械の姿勢制御装置及び制御方法 - Google Patents

Description

【０００１】
【発明の属する技術分野】
本発明は、脚を有する機械の姿勢制御装置及び制御方法に関するものである。
【０００２】
【従来の技術】
脚を有する機械等の脚を有する機械では、直立姿勢を維持する場合、位置のフィードバック制御が主に用いられている。その方法では、平衡が保たれる姿勢を予め計算しておき、その姿勢からのずれを補償することにより、直立状態を維持する。この制御方法は、脚を有する機械がおかれる環境が既知であれば有効な方法である。
【０００３】
なお、本明細書で、環境とは外力が定常的に加えられている状況と定義する。
しかし、既知である環境は現実的には稀であり、例えば傾斜角が不明な斜面で直立しなければならない状況がある。人間であれば、直立した状態において、そのような環境になると、平面に直立していた場合よりも足関節を伸展或いは屈曲させ、上体が鉛直方向に向くような姿勢をとる。又、横方向からの力（例えば、風力、水力）を受け続ける場合に、上体をその力が付与される上流の方向に倒した状態で姿勢を維持する。
【０００４】
【発明が解決しようとする課題】
しかし、脚を有する機械では、前記のような環境に応じて直立姿勢を変化させる場合、その姿勢は環境が与えられなければ決まらない。例えば、坂の傾斜角度が分からないと、足関節の取るべき角度が決まらない問題がある。このことは、直立姿勢の維持を位置のフィードバック制御で行おうとするときに問題となる。
【０００５】
直立姿勢を維持する最も簡単な方法は、足関節角度を検出して、その角度に対する位置のフィードバックをかけるものである。しかし、フィードバックする足関節角度の目標値が、環境が与えられた後、すなわち、事後的にしか決まらない。この理由から、未知環境下での平衡制御において、従来提案されている制御方法がうまく利用できない問題がある。
【０００６】
脚を有する機械における二足歩行系の平衡制御では、歩行運動がダイナミクスを持つため、慣性力と重力が作るモーメントがゼロとなるＺＭＰ（zero moment point）に着目した制御法が提案されている。しかし、ＺＭＰが用いられるのは、主に歩行運動の計画時である。すなわち、ＺＭＰが足底（足底は、一般に多角形をなす支持面であることから、支持多角形ということがある）内部に常に止まるように各関節起動を事前に設計し、実際の歩行運動の制御はその目標関節軌道に関するフィードバックで達成されている。
【０００７】
従って、この制御方法では、目標値が環境により事後的に決定する場合においては、そのままでは利用できない問題がある。
本発明は上記の課題を解消するためになされたものであり、未知環境下においても、その外力に応じて姿勢を変化させることができ適正な姿勢制御を実現することができる脚を有する機械の姿勢制御装置及び制御方法を提供することを目的としている。
【０００８】
【課題を解決するための手段】
上記の課題を解決するために、請求項１に記載の発明は、接地する接地部材と、同接地部材に対して足関節を介して回動自在に設けられた脚と、前記脚を駆動する駆動手段と、前記駆動手段を駆動制御する制御手段とを備えた脚を有する機械の姿勢制御装置において、接地部材が床に接地した複数部分に働く床反力をそれぞれ検出する床反力検出手段と、前記脚の傾き角度を検出する角度検出手段又は前記脚の傾き角度の角速度を検出する角速度検出手段とを備え、前記制御手段は、外力が付与された際に検出された前記角度又は角速度に基づいて得られた角度を予め定めた目標角度に制御するＰＤ制御を行うとともに、外力が付与された際に検出された複数の接地部分に働く床反力の差が所定値になるように収束制御する力フィードバック制御とを行い、さらに、制御手段は両制御により足関節トルクの演算をし、演算された足関節トルクに基づいて前記駆動手段を駆動制御することを特徴とする脚を有する機械の姿勢制御装置を要旨とするものである。
【０００９】
請求項２の発明は、請求項１において、前記所定値は０であることを特徴とする。
請求項３の発明は、請求項１において、前記所定値は０以外であることを特徴とする。
【００１０】
請求項４の発明は、請求項１乃至請求項３のうちいずれか１項において、前記ＰＤ制御における収束性は、前記力フィードバック制御における収束性よりも速いことを特徴とする。
【００１１】
請求項５の発明は、接地する接地部材と、同接地部材に対して足関節を介して回動自在に設けられた脚と、前記脚を駆動する駆動手段とを備えた脚を有する機械の姿勢制御方法において、外力が付与された際に接地部材が床に接地した複数部分に働く床反力をそれぞれ検出するとともに前記脚の傾き角度又は脚の傾きの角速度を検出する工程と、検出された前記角度又は角速度に基づいて得られた角度を予め定めた目標角度に制御するＰＤ制御を行う工程と、外力が付与された際に検出された複数の接地部分に働く床反力の差が所定値になるように収束制御する力フィードバック制御を行う工程と、前記両制御により足関節トルクの演算をし、演算された足関節トルクに基づいて前記駆動手段を駆動制御する工程を含むことを特徴とする脚を有する機械の姿勢制御方法を要旨とするものである。
【００１２】
請求項６の発明は、請求項５において、前記所定値は０であることを特徴とする。
請求項７の発明は、請求項５において、前記所定値は０以外であることを特徴とする。
【００１３】
請求項８の発明は、接地する接地部材と、同接地部材に対して足関節を介して回動自在に設けられた脚と、前記脚を駆動する駆動手段と、前記駆動手段を駆動制御する制御手段とを備えた脚を有する機械の姿勢制御装置であって、前記脚と接地部材をダンパ手段及びダンパ手段と並設したスプリング手段にて連係し、接地部材が床に接地した複数部分に働く床反力をそれぞれ検出する床反力検出手段を備え、前記制御手段は、外力が付与された際に検出された複数の接地部分に働く床反力の差が所定値になるように収束制御する力フィードバック制御を行い、さらに、制御手段は、前記制御に基づいて足関節トルクの演算をし、演算された足関節トルクに基づいて前記駆動手段を駆動制御することを特徴とする脚を有する機械の姿勢制御装置を要旨とするものである。
【００１４】
請求項９の発明は、接地する接地部材と、同接地部材に対して足関節を介して回動自在に設けられた脚と、前記脚を駆動する駆動手段とを備え、前記脚と接地部材をダンパ手段及びダンパ手段と並設したスプリング手段にて連係した脚を有する機械の姿勢制御方法であって、外力が付与された際に接地部材が床に接地した複数部分に働く床反力をそれぞれ検出する検出する工程と、外力が付与された際に検出された複数の接地部分に働く床反力の差が所定値になるように収束制御する力フィードバック制御を行う工程と、前記制御により足関節トルクの演算をし、演算された足関節トルクに基づいて前記駆動手段を駆動制御する工程を含むことを特徴とする脚を有する機械の姿勢制御方法を要旨とするものである。
【００１５】
【発明の実施の形態】
（第１実施形態）
以下、本発明を具体化した第１実施形態を図１〜図８を参照して説明する。
【００１６】
図１は、脚を有する機械１０及びその姿勢制御装置１の電気的構成を示す説明図、図２は脚を有する機械１０の姿勢制御装置１の電気的構成を示すブロック回路図を示している。図３は脚を有する機械の要部側面図、図４は脚を有する機械の平面図を示している。なお、図３において、左が後、右を前とする。
【００１７】
機械１０は足部材１１、脚１２、モータ１３等とを備えている。足部材１１は、四角板状をなし、その上面に脚１２が軸受１４を介して回動自在に軸支されている。なお、脚１２が回動自在に設けられた軸支点Ｏを足関節ということがある。すなわち、脚１２は図３に示すように前後方向に回転自在とされている。なお、脚１２は、図示しない胴体部材を支持するが、省略されている。従って、以下では、脚１２及び胴体部分を含むものを胴部という。足部材１１は接地部材に相当する。モータ１３は足部材１１に対して固定され、その出力軸は脚１２を軸支する軸に連結されている。駆動手段としてのモータ１３はエンコーダ１５（ロータリエンコーダ）を備えたＤＣモータから構成されている。
【００１８】
エンコーダ１５はモータ回転角を検出するようにされており、脚１２の傾き角度（床面に対する垂線であって、足関節Ｏを通る垂線ａからの傾き角度θであり、足関節の傾き角度ともいうことがある）を検出する角度検出手段に相当する。エンコーダ１５の検出信号はパルスカウンタ３１を介してコンピュータ１８に出力される。
【００１９】
足部材１１の接地面（下面）には４隅に床反力検出手段としてのロードセル１６ａ〜１６ｄが設けられている。各ロードセル１６ａ〜１６ｄは足部材１１が床面に接地された際、床面に垂直な床反力をそれぞれ検出することが可能とされている。各ロードセル１６ａ〜１６ｄにて検出された検出信号は動ひずみ測定器１７、Ａ／Ｄ変換器３０、パルスカウンタ３１を介して制御手段としてのコンピュータ１８に出力される。
【００２０】
従って、各ロードセル１６ａ〜１６ｄのうち、脚１２の軸支点を挟んで足部材１１の前側に位置するロードセル１６ａ，１６ｂは足先の床反力を検出する。又、ロードセル１６ａ〜１６ｄのうち、脚１２の軸支点を挟んで足部材１１の後側に位置するロードセル１６ｃ，１６ｄは踵の床反力を検出する。
【００２１】
図２に示すようにコンピュータ１８は中央処理装置１９（ＣＰＵ）、ＲＯＭ２０、ＲＡＭ２１を備えている。
ＣＰＵ１９は、エンコーダ１５及び各ロードセル１６ａ〜１６ｄの検出値を動ひずみ測定器１７、Ａ／Ｄ変換器３０、パルスカウンタ３１を介して入力し、ＲＡＭ２１内に書き込む。又、エンコーダ１５から入力した傾き角度θをＣＰＵ１９は微分し、角速度としてＲＡＭ２１内に書き込む。ＣＰＵ１９は、ＲＯＭ２０内に格納された制御プログラムに基づいて機械１０の姿勢制御を好適に行うようにされている。すなわち、前記入力した検出値に基づいて足関節トルクとしてのモータ１３のトルクの演算を行い、算出されたトルクをＤ／Ａ変換器３２、ＤＣアンプ２２を介してモータ１３に送出する。モータ１３はこのトルク値に基づいて駆動され、脚１２の姿勢を制御する。
【００２２】
次に、姿勢制御装置１の動作を説明する。
まず、機械１０は、本実施形態においては図６（ａ）に示す歩行運動を行う歩行系のロボットを想定し、これを簡略して図６（ｂ）に示すような足部（足部材１１）とそれ以外の胴部（脚１２を含む）の２つのリンク系としてモデル化したものである。
【００２３】
そして、モデルの運動は図６（ｂ）に示す矢印方向の仮想面内（図３において、前後方向に延びる面内）に限定し、以下の条件を仮定している。
１）足関節（脚１２の軸支点）Ｏにはアクチュエータ（モータ１３）があり、適当な足関節トルクτが出力できる。
【００２４】
２）足関節Ｏの傾き角度θ（モータ角度）及び角速度が計測できる。
３）足部（足部材１１）は前後方向の両先端（踵と足先）の両方で接地し、それぞれの接地点では床反力の垂直成分（それぞれＦ_H，Ｆ_Tとする。）が計測できる。
【００２５】
４）水平方向にはＦｘ，鉛直方向にはＦｙの未知な一定の外力が加わる。
５）床は水平で滑らないものとする。
上記のような仮定の下では胴部のみがダイナミクスをもち、その運動は下記（１）式で記述できる。
【００２６】
【数１】

又、ここで、Ｍは胴部の質量、Ｉはその足関節Ｏ周り（軸支点周り）の慣性モーメント、Ｌは胴部の重心Ｊ１と足関節Ｏ間の長さ、ｇは重力加速度である。
【００２７】
一方、リンク間に働く水平方向、鉛直方向の内力ｆｘ及びｆｙは下記（２）式、（３）式で与えられる。
【００２８】
【数２】

【００２９】
【数３】

これらを用いると、床反力Ｆ_T及びＦ_Hは、踵及び足先周りのモーメントの釣り合いの条件より、下記（４）式及び（５）式となる。
【００３０】
【数４】

【００３１】
【数５】

ここで、ｌ_T，ｌ_H，ｌ_Gは、足関節Ｏからそれぞれ足先、踵、足部（足部材１１）の重心Ｊ２までの長さである。又、ｍ_Tとｍ_Hはそれぞれ足先、踵にかかる足部（足部材１１）の重量であり、下記（６）式で与えられる。
【００３２】
【数６】

このモデルにおいて、環境、すなわち、一定外力Ｆｘ，Ｆｙに対して胴部の姿勢が適切に変化させ得るような足関節トルクτを、足関節Ｏの傾き角度θと
【００３３】
【数７】

及び床反力Ｆ_H，Ｆ_Tを基に決定する。すなわち、本実施形態では、ＲＯＭ２０内に格納した制御プログラムは、
【００３４】
【数８】

を基に制御入力（足関節トルクτ）を決定する。
【００３５】
なお、説明の便宜上、以降の計算を簡単にするため、（１）式を以下の（７）式のように変形しておく。
【００３６】
【数９】

ここで、Ａは、下記の（８）式で表されており、θ_fは下記の（９）式を満たす定数である。
【００３７】
【数１０】

【００３８】
【数１１】

ここで、（８）式、及び（９）式から分かるように、重要なことは、Ａとθ_fがともに環境を表すＦｘ，Ｆｙに依存していることである。
【００３９】
（力フィードバックによる制御）
次に、力フィードバックによる制御について説明する。
まず、理論が簡単である足部（足部材１１）が対称性をもつ場合、すなわち、ｌ_T＝ｌ_H＝ｌ、ｌ_G＝０の場合について説明する。
【００４０】
転倒が起きないためには、Ｆ_TとＦ_Hがいずれも正の値を保つ必要がある。
そして、足先と踵に体重が均等にかかり、Ｆ_T＝Ｆ_Hとなった場合、最も安定性が高くなる。これは対称な足底において、ＺＭＰがその中央にくることが説明できる。
【００４１】
従って、２つの床反力Ｆ_TとＦ_Hの差を小さくするように、足関節トルクτ（制御入力）を決定する。
前記（４）式と（５）式より、Ｆ_TとＦ_Hとτの関係は下記（１０）式で与えられる。
【００４２】
【数１２】

これより、定数Ｋ_Iを用いて、τを下記（１１）式とすれば、Ｆ_H−Ｆ_Tを０に収束させることができる。
【００４３】
【数１３】

ただし、この制御入力のみでは、直立姿勢を必ずしも安定化することができない。Ｆ_T＝Ｆ_Hを保ったまま胴部（脚１２を含む）が転倒してしまうからである。
【００４４】
（ＰＤ制御と力フィードバックによる制御との組合せ）
胴部の安定化を図るには、足関節の傾き角度θのＰＤ制御が有効である。
ＰＤ制御での制御入力分は、下記（１２）式で決めることができる。
【００４５】
【数１４】

ここで、（１２）式中のτθはＰＤ制御で得られる制御入力分であって、微分項と比例項とからなり、Ｋ_d，Ｋ_pは正の定数、θ_dはＰＤ制御の目標値である。姿勢制御において、直立姿勢を目指す場合は、θ_d＝０とするが、後述する力フィードバック制御との組合せにより、この値に限定するものではない。
【００４６】
なお、外力が存在するため、θは目標値θ_dには収束しない。
そのときの収束値θ０を算出する。
τ＝τθを（７）式に代入すると、下記（１３）式が得られる。
【００４７】
【数１５】

ここで、θ−θ_f〜０を仮定して上式をθをθ_fのまわりで線形化すると、下記（１４）式となる。
【００４８】
【数１６】

ここで、θ０は下記（１５）式となる。
【００４９】
【数１７】

これより、Ｋ_p＞ＡＬが成立すれば、第１次近似ではθはθ０に収束する。
【００５０】
このＰＤ制御を行うことにより、胴部の局所的な安定性が得られることになる。そこで、前記（１１）式のような体重を足先と踵に均等にかけさせる床反力のフィードバックを制御入力に加える。
【００５１】
すなわち、力フィードバックとして、下記（１６）式を定義する。
【００５２】
【数１８】

そして、最終的な足関節トルクτ（制御入力）を下記（１７）式にて定義する。
【００５３】
τ＝τθ＋Ｋ_f・τ_f …（１７）
ここでＫ_fは力のフィードバックゲインである。
以降では、この姿勢制御方法を、この明細書では、ＰＤ制御と力フィードバックの組合せによることから足関節のＰＤＦ制御と定義する。
【００５４】
なお、前記（１２）式、（１６）式、及び（１７式）で与えられる足関節トルクに対し胴部の局所的安定性が保証できる。
上記のように、足関節のＰＤＦ制御において、足部（足部材１１）が対称性をもつ場合、すなわち、ｌ_T＝ｌ_H＝ｌ、ｌ_G＝０の場合の局所的安定性について説明する。
【００５５】
この足関節のＰＤＦ制御（対称性を有する足部材）の局所的安定性に関しての条件を以下、定理１という。
（定理１）
ｌ_T＝ｌ_H＝ｌ、ｌ_G＝０の場合、前記（１）式で与えられる系のダイナミクスに対して、（１２）式、（１６）式及び（１７）式で与えられる足関節トルクを考える。
【００５６】
もし、フィードバックゲイン（定数）Ｋ_d，Ｋ_p，Ｋ_fが下記の（１８）式〜（２０）式の条件を満たせば、θ＝θ_fは局所的漸近安定な平衡点となる。
Ｋ_p＞ＡＬ＞０ …（１８）
（ｌ／Ｉ）Ｋ_d＞Ｋ_f＞０ …（１９）
（Ｋ_dｌ−Ｋ_fｌ）Ｋ_p＞Ｋ_dｌＡＬ …（２０）
（１８）式〜（２０）式の条件は、外力によって定まるパラメータＡを含んでいる。これにより、各フィードバックゲインＫ_d，Ｋ_p，Ｋ_fが与えられたとき、どれくらいの外力に耐えうるかを見積もることが可能である。逆に外力の最大値が既知の場合は、これらの式により各フィードバックゲインが決定できる。
【００５７】
なお、前記（１１）式は（１７）式において、Ｋ_d＝Ｋ_p＝０と設定したものに相当する。この場合には、前記（１８）式〜（２０）式で与えられる条件を満たさない。従って、力フィードバックのみの制御では、胴部が安定せず、不安定となり、転倒する。
【００５８】
なお、（１８）式〜（２０）式の算出は、下記の通りに行うことによって得られる。
（７）式に、（１７）、（１２）式を代入すると、下記（２１）式が得られる。
【００５９】
【数１９】

一方、（１６）式を微分した式に（１０）式、（１２）式及び（１７）式を代入すると、（２２）式が得られる。
【００６０】
【数２０】

そこで、（２１）式と（２２）式で与えられるダイナミクスの
【００６１】
【数２１】

【００６２】
【数２２】

と、
【００６３】
【数２３】

を代入すると、（２１）式及び（２２）式はそれぞれ下記の（２３）式及び（２４）式となる。この代数方程式を解くと、下記の（２５）式が得られる。
【００６４】
【数２４】

【００６５】
【数２５】

【００６６】
【数２６】

次にこの平衡点の安定性を調べる。
【００６７】
θ＝θ１、
【００６８】
【数２７】

とおき、方程式を平衡点まわりで線形化すると下記（２６）式となる。
【００６９】
【数２８】

この線形微分方程式の特性方程式は下記（２７）式で与えられる。
【００７０】
【数２９】

ここで、
【００７１】
【数３０】

である。
【００７２】
Routh/Hurwitzの安定判別法を用いると、平衡点が安定となるための必要十分条件は
Ｐ０＞０，Ｐ１＞０，Ｐ２＞０，Ｐ１・Ｐ２−Ｐ０＞０ …（２９）
である。上式により、（１８）式〜（２０）式が導出される。
【００７３】
（制御フローチャートの説明）
さて上記のような前提で、本実施形態における姿勢制御装置１が所定時間周期で実行する制御プログラムを図５のフローチャートを参照して説明する。
【００７４】
この制御プログラムがスタートすると、ステップ（以下、ステップをＳという）１０において、各ロードセル１６ａ〜１６ｄの検出値及びエンコーダ１５の検出値を読み込む。Ｓ２０においては、床反力Ｆ_T及び床反力Ｆ_Hの算出を行う。本実施形態では、床反力Ｆ_Tは足先側のロードセル１６ａ，１６ｂの検出値を加算して求める。床反力Ｆ_Hは踵側のロードセル１６ｃ，１６ｄの検出値を加算して求める。
【００７５】
次のＳ３０では、ＰＤ制御を行い、τθを前記（１２）式により求める。
本実施形態では、直立姿勢を安定化するものとして、目標値θ_dをθ_d＝０としている。
【００７６】
次のＳ４０では、力フィードバックを行い、τ_fを前記（１６）式により求める。続く、Ｓ５０において、Ｓ３０及びＳ４０で算出したτθ、τ_fに基づいて最終的な足関節トルクτ（制御入力）を前記（１７）式により算出する。
【００７７】
そして、Ｓ６０において、算出した足関節トルクτをＤＣアンプ２２を介してモータ１３に出力し、この制御フローチャートを一旦終了する。
さて、上記実施形態の特徴的な利点について述べる。
【００７８】
（１） 本実施形態では、足関節トルクτとして（１７）式を用いると、定常状態での胴部（脚１２を含む）姿勢は、θ＝θ_fとなる。ここで、前述したように、θ_fは環境、すなわち、一定な外力Ｆｘ，Ｆｙに依存していることに注意をする。これは、定常状態の姿勢が、環境に合わせて変化することを意味している。
【００７９】
この一定外力が付与される場合とは、例えば、機械１０が斜面に載置された場合、機械１０に横風が働き、風力が係る場合、或いは、水中に載置された状態で横から水力が係る場合等が考えられる。
【００８０】
定常状態での姿勢θ＝θ_fでは、図７に示すように胴部（脚１２を含む）が重力と外力との合力の方向に向かうことに注意されたい。
従って、もし外力が働かなければ、鉛直方向が定常状態での姿勢となる。この姿勢をとることの利点は、足関節周りの重力と外力とのモーメントが釣り合っているため、その姿勢の維持がわずかなトルクで可能であることを意味する。
【００８１】
計算上では、前記（２５）式を（１２）式と（１７）式に代入すると、わかるように足関節トルクτはτ＝０となる。このことから、エネルギー消費の点から見ても有利である。
【００８２】
このように、重力と外力のモーメントが釣り合うことから、姿勢の維持にはわずかなトルクしか必要とはせず、エネルギコスト削減の効果がある。
（２） 本実施形態の姿勢制御装置１は、接地する足部材１１（接地部材）と、足部材１１に対して回動自在に設けられた脚１２と、脚１２を駆動するモータ１３（駆動手段）と、モータ１３を制御するコンピュータ１８（制御手段）とを備えるようにした。
【００８３】
さらに姿勢制御装置１は、足部材１１が床に接地した複数部分に働く床反力をそれぞれ検出するロードセル１６ａ〜１６ｄ（床反力検出手段）と、脚１２の傾き角度θを検出するエンコーダ１５（角度検出手段）とを備えた。
【００８４】
そして、コンピュータ１８は、外力が付与された際に検出された足関節の傾き角度θ（脚の傾き角度）を予め定めた目標値θ_d（目標角度）に制御するＰＤ制御を行うとともに、外力が付与された際に検出された複数の接地部分に働く床反力の差（Ｆ_H−Ｆ_T）が０（所定値）になるように収束制御する力フィードバック制御とを行うようにした。さらに、コンピュータ１８は両制御に基づいて（１７）式にて足関節トルクτの算出（トルク演算）をし、演算された足関節トルクτに基づいてモータ１３を駆動制御するようにした。
【００８５】
この結果、未知環境下において、適正な姿勢制御を実現することができる。
又、足関節Ｏの傾き角度θとその角速度、床反力という脚１２、足部材１１の局所的な情報のみで制御入力（足関節トルク）が計算できる効果がある。
【００８６】
（３） 本実施形態では、外力が付与された際に検出された複数の接地部分に働く床反力の差が０（所定値）になるように収束制御するようにした。
こうすると、足部材１１に対称性がある場合、すなわち、ｌ_T＝ｌ_H＝ｌ、ｌ_G＝０の場合、上記（２）の作用効果を実現することができる。
【００８７】
（４） 本実施形態での姿勢制御方法は、外力が付与された際に足部材１１（接地部材）が床に接地した複数部分に働く床反力をそれぞれ検出するとともに脚１２の傾き角度θを検出する工程と、検出された傾き角度θを予め定めた目標値θ_d（目標角度）に制御するＰＤ制御を行う工程とを備えた。
【００８８】
そして、外力が付与された際に検出された複数の接地部分に働く床反力の差（ＦＨ−ＦＴ）が０（所定値）になるように収束制御する力フィードバック制御を行う工程を備えた。さらに、前記両制御に基づいて足関節トルクの演算をし、演算された足関節トルクに基づいてモータ１３（駆動手段）を駆動制御するようにした。
【００８９】
この結果、前記（２）と同じ効果を奏することができる。
（５） 本実施形態の姿勢制御方法では、外力が付与された際に検出された複数の接地部分に働く床反力の差が０（所定値）になるように収束制御するようにした。こうすると、前記（３）と同じ効果を奏することができる。
【００９０】
（６） 本実施形態の姿勢制御方法では、ＰＤ制御での目標値θ_d（目標角度）を０とした。この結果、（４）と同じ効果を奏する。
（７） 足関節のＰＤＦ制御の基本的な考え方は、一過性の外乱等に対してはとりあえずＰＤ制御にて対処して平衡を保ち、その結果として重さのかけ方に不均等が生じた際に力フィードバックで解消しようとするものである。
【００９１】
従って、ＰＤ制御の収束性は、力フィードバックの収束性よりも速くする必要がある。大まかにいうと、ＰＤＦ制御の収束性はフィードバックゲインＫ_pとＫ_d，力フィードバックの収束性はフィードバックゲインＫ_fで決まる。
【００９２】
この収束性の速さに関する条件は、（１８）式、（１９）式において、Ｋ_fの大きさに上限があることで表現されている。
従って、ＰＤ制御の収束性を、力フィードバックの収束性よりも速くすることにより、適正な姿勢制御を好適に実現することができる。
【００９３】
（実験）
さて、上記の実施形態における実験例を説明する。
脚１２は長さを０．５５ｍとし、足部材１１は前後方向の長さを０．１２ｍとし、対称性のあるものとしている。脚１２及び足部材１１を含めた全体の質量は１．１１Ｋｇとしている。制御入力は、モータ１３のエンコーダ１５からの角度及びロードセル１６ａ〜１６ｄからの床反力の情報を基に１ｍｓ毎に算出する。この実験での、フィードバックゲインは、Ｋ_p＝２．５、Ｋ_d＝１．５、Ｋ_f＝０．０１とした。
【００９４】
この機械１０を傾斜角αの傾斜面に載置した場合、傾斜面での環境は図８（ａ）、（ｂ）に示すように、水平面上でＦｘ＝Ｍｇsinα，Ｆｙ＝Ｍｇ（１−cosα）の一定外力が働く場合と等価となる。なお、図８（ａ）、（ｂ）では、説明の便宜上、モータ１３、各ロードセル１６ａ〜１６ｄ等は省略して図示している。
【００９５】
本実験では、下記３つのステージからなる。
初期状態として、機械１０を水平な板の上に置く（ステージ１）。
この状態から、板の左端を持ち上げて傾斜面をつくり、しばらくの間一定傾斜角を保つ（ステージ２）。
【００９６】
その後、再び板の左端を床に降ろし、水平な状態を維持する（ステージ３）。このときの数値データを図９〜図１１に示す。図９は、足関節角度（足関節の傾き角度θ）の時間変化を示している。板の傾斜角が一定のとき（ステージ２）、足関節の傾き角度θは０．３radに保たれ、又、板が水平に戻されたとき（ステージ３）、傾き角度θは０rad付近に戻っていることが分かる。すなわち、ステージ１では、脚１２は、水平面に対して直立状態となっており、ステージ２では、傾斜面に適応するように、直立状態を保とうとして図８（ａ）の矢印方向に変化した。さらに、ステージ２からステージ３に移行すると、脚１２は、前記矢印方向とは逆方向に傾き、直立状態を保持した。
【００９７】
図１０は、前記（１７）式中の制御入力である、τ，τθ，Ｋ_fτ_fの時間変化を示している。板の傾斜角が一定に保たれているとき（ステージ２）、理論上制御入力τ（足関節トルクτ）は０となるはずである。しかし、足関節周りの摩擦が大きいため、定常偏差が残り、ここではτは０とはなっていない。図１１は床反力の時間変化である。機械１０の自重が足部材１１の前後方向両端に均等にかかるように制御が行われていることが分かる。
【００９８】
（第２実施形態）
次に第２実施形態について説明する。なお、第１実施形態の構成と同一構成については説明を省略し、異なるところ説明する。
【００９９】
前記第１実施形態では、足部材１１が対称性のあるものに具体化したが、本実施形態では、足部材１１に対称性がない場合を想定したものである。すなわち、ｌ_H≠ｌ_G，ｌ_G≠０の場合を想定している。
【０１００】
この場合、外力なしの状況で鉛直方向に直立しても、Ｆ_TとＦ_Hは等しくならず、Ｆ_TとＦ_Hはそれぞれ下記（３０）式，（３１）式で表される。
【０１０１】
【数３１】

【０１０２】
【数３２】

従って、Ｆ_TとＦ_Hとの差が、（３１）式と（３２）式の差、すなわち下記（３２）式で表されるＦ₀に収束されるように、床反力のフィードバックτ_fを（１６）式の替わりに下記（３３）式で示すように修正している。
【０１０３】
【数３３】

【０１０４】
【数３４】

すなわち、本実施形態では、この修正した（３３）式にて、τ_fを算出するように、前記第１実施形態の構成中、制御フローチャートのＳ４０で行うようにしている。他の構成は第１実施形態と同じ構成である。
【０１０５】
従って、この修正した姿勢制御装置１及び姿勢制御方法により、対称性のない足部材１１に対しても胴部（脚１２を含む）の安定性が保証できる。
上記のように、足関節のＰＤＦ制御において、足部（足部材１１）が対称性をもたない場合（非対称性の場合）の局所的安定性について説明する。
【０１０６】
この足関節のＰＤＦ制御（非対称性を有する足部材）の局所的安定性に関しての条件を、以下、定理２という。
（定理２）
前記（１）式におけるモデルの場合、前記（１２）式、（１７）式及び（３３）式でそれぞれ定義される制御入力を考える。
【０１０７】
ここで、ｌ＝（ｌ_T＋ｌ_H）／２とする。
フィードバックゲインＫ_d，Ｋ_p，Ｋ_fが（１８）式〜（２０）式を満たせば、θ＝θ_fは局所的漸近安定な平衡点となる。
【０１０８】
このことは、（２１）式及び（２２）式と同様の微分方程式が導かれることから言える。
なお、（２１）式の導出は、前記第１実施形態で説明した定理１と同様に（１）式に（１７）式、（１２）式を代入することにより自明である。
【０１０９】
又、（２２）式の導出は、（３３）式を微分した下記（３４）式に対して、（５）式から（４）式を引いて得られる下記（３５）式を代入する。
【０１１０】
【数３５】

【０１１１】
【数３６】

このとき（３２）式に注意すれば、（２２）式と同様の式となる。以下の解析は定理１と同様であるので説明を省略する。
【０１１２】
なお、前記（１２）式、（１７）式及び（３３）式でそれぞれ定義される制御入力を得る場合、（３２）式に含まれるｆｙの値が必要になる。
この値は、（５）式と（４）式を足し合わせて得られる下記（３６）式より求める。
【０１１３】
ｆｙ＝Ｆ_T＋Ｆ_H−（ｍ_T＋ｍ_H）ｇ＝Ｆ_T＋Ｆ_H−ｍｇ …（３６）
足部材１１の質量ｍが既知であれば、床反力フィードバックのＦ_T，Ｆ_Hより算出できる。すなわち、本実施形態では、この質量ｍのデータはＲＯＭ２０内に予め格納されているものとし、本実施形態では、ｆｙはＦ_T，Ｆ_Hの算出時（Ｓ２０）に、同時に算出されるものとする。
【０１１４】
第２実施形態によると、次のような作用効果を奏する。
（１） 第２実施形態の姿勢制御装置１では、外力が付与された際に検出された複数の接地部分に働く床反力の差（Ｆ_H−Ｆ_T）が０以外の所定値Ｆ₀になるように収束制御する力フィードバック制御を行うようにした。
【０１１５】
この結果、対称性のない足部材１１においても、胴部の安定性を保証することができる。
（２） 第２実施形態の姿勢制御方法では、外力が付与された際に検出された複数の接地部分に働く床反力の差（Ｆ_H−Ｆ_T）が０以外の所定値Ｆ₀になるように収束制御する力フィードバック制御を行うようにした。この結果、上記（１）と同じ効果を奏することができる。
【０１１６】
（３） 本実施形態においても、姿勢制御装置１及び制御方法では、ＰＤ制御での目標値θ_d（目標角度）を０とした。この結果、第１実施形態と同様に胴部（脚１２を含む）を直立状態に保持することができる。
【０１１７】
（第１実施形態と第２実施形態との共通の作用効果）
次に第１実施形態と第２実施形態との共通の作用効果について説明する。
環境から、一定外力Ｆｘ，Ｆｙばかりでなく、一定のモーメントτ_eが同時に胴部にかかる場合においても、局所的漸近安定な平衡点が存在する効果がある。
【０１１８】
このことについて説明する。
このときの胴部の運動方程式は、下記の（３７）式で与えられる。
【０１１９】
【数３７】

この系に対し足部材１１が対称ならば、第１実施形態の定理１で定義される足関節のＰＤＦ制御を実行し、足部材１１が対称でないならば、第２実施形態の定理２で定義される足関節のＰＤＦ制御を実行する。
【０１２０】
この場合の平衡点について考える。
得られる微分方程式は下記（３８）式、（３９）式である。
【０１２１】
【数３８】

【０１２２】
【数３９】

従って、ＡＬ≧｜τ_e｜のとき平衡点（θ_e，Ｋ_p（θ_e−θ_d）／Ｋ_f）が存在する。ただし、θ_eは下記（４０）式を満たす定数である。
【０１２３】
【数４０】

次に平衡点の安定性については下記のことから理解することができる。
【０１２４】
θ＝θ１，
【０１２５】
【数４１】

とおき、（３８）式，（３９）式を平衡点周りで線形化すると、状態変数を（θ１，θ２，τ_f）としたときの線形微分方程式の係数行列は下記（４１）式で表される。
【０１２６】
【数４２】

ここで改めてＡcos（θ_e−θ_f）＞０をＡと定義すると、以下、第１実施形態の定理１で説明したものと同様であるため、説明を省略する。
【０１２７】
この結果、（３７）式が成立するモデル（系）においては、下記の定理３が得られる。
（定理３）
（３７）式が成立するモデル（系）においては、外部から外力Ｆｘ，Ｆｙ及びモーメントτ_eが下記（４２）式の関係を満たすと仮定する。
【０１２８】
【数４３】

この系に対し、（１２）式、（１７）式及び（３３）式で定義される制御入力を与える。そして、フィードバックゲインＫ_d，Ｋ_p，Ｋ_fが（１８）式〜（２０）式を満たすθ＝θ_eは局所的漸近安定な平衡点となる。
【０１２９】
なお、ここでｌは下記（数４４）で表される。
【０１３０】
【数４４】

なお、前記（４０）式より、定常状態での姿勢θ＝θ_eは、足関節周りの外部からの外力及びモーメントが釣り合う姿勢であることが分かる。従って、定常状態では、足関節トルクτは理論上０でよく、姿勢の維持には効率がよい。
【０１３１】
本発明の実施形態は、上記実施形態以外に次のように変更することも可能である。
（１） 第１実施形態、第２実施形態では、足関節Ｏは床面と同じ高さにあると仮定しているが、厳密には足関節Ｏは床面よりも上にある。
【０１３２】
このため、厳密に制御を行う場合には、下記のようにしてもよい。
水平面からの足関節Ｏの高さをｌ_Aとおくと、床反力は胴部からの水平方向内力ｆｘの影響を受け、下記の（４３）式、（４４）式で表される。
【０１３３】
【数４５】

【０１３４】
【数４６】

従って、前記（３２）式で定義されるＦ₀を下記（４５）式で定義し直すと、定理２及び定理３と同じ条件で、同一の結果が得られる。
【０１３５】
【数４７】

なお、この場合、Ｆ₀の決定には、ｆｘの値が必要であるため、胴部と足部材１１との２リンク間の水平方向内力ｆｘ（図６（ｂ）参照）を検出するための力センサを設けるものとする。
【０１３６】
（２） 前記第１実施形態、第２実施形態では胴部（脚１２を含む）を１リンクの剛体としてモデル化した。
この代わりに胴部を多リンクとしたものに具体化することも可能である。
【０１３７】
この多リンク系への応用可能性について説明する。
例えば、図１３（ａ）に示すように、脛部Ｈを図６（ａ）の胴部とみなし、膝関節Ｈａより上の部分（以下、上体という）Ｕは外力Ｆｘ，Ｆｙによってその力学的な影響が反映されているものと考える。
【０１３８】
この場合の外力は、上体Ｕに働く外力をも含めて扱うことができる。なお、この場合、異なるのは膝関節Ｈａに働くトルクの影響である。これを外部からのモーメントとして扱えば、前記定理３を適用することができる。
【０１３９】
人間型の多リンク系に応用した場合、お辞儀をするときのように、安定性を確保したまま、導体、腕、頭の位置をゆっくり変化させることができる。
足関節のＰＤＦ制御は、足関節Ｏの傾き角度θ及びその角速度と床反力のフィードバックのみで構成されている。このため、多リンク系の制御に用いた場合でも、上体Ｕの関節角度という情報は一切用いていないため、局所的なフィードバックで独立的にできるのが利点の１つである。
【０１４０】
そして、定理３の（４２）式により、平衡を保てる姿勢の範囲をおおよそ見当づけることができる。
以下に、平衡を保てる姿勢の範囲の説明をする。
【０１４１】
簡単のため、外力は働かないものとし、脛部Ｈの質量Ｍ＝０、リンク長をＬとし、上体Ｕの質量をＭ_B、膝関節Ｈａから上部の重心と膝関節Ｈａ間の距離をＬ_B，重心と膝関節Ｈａを結ぶ直線が鉛直方向からなす角をθ_Bとする。
【０１４２】
定常状態では、Ｆｘ＝０，Ｆｙ＝Ｍ_BｇよりＡ＝Ｍ_Bｇ，又、τ_e＝Ｍ_Bｇsinθ_Bであるから、（４２）式より、
Ｌ／Ｌ_B≧sinθ_B …（４６）
となる。
【０１４３】
姿勢の変化は、重心が上式を満たすθ_Bの範囲内で可能であることから、Ｌは大きく、Ｌ_Bが小さい方が広い範囲で上体Ｕの姿勢変化ができることとなる。
図１３（ｃ）には（４２）式を満たす限界の姿勢の一つを示している。なお、実際には、関節の可動範囲には限界があり、結局上体Ｕの重心が足関節Ｏの真上に持ってこられる範囲で姿勢変化が実現できる。
【０１４４】
（３）前記各実施形態では、図６（ｂ）に示す矢印方向の仮想面内（図３において、前後方向に延びる面内）に限定した運動を前提としていたがこれに限定されるものではない。
【０１４５】
足関節のＰＤＦ制御は、ＺＭＰ、すなわち床反力が作用する中心点を足関節Ｏの位置に制御することと等価である。従って、足底（足部材１１の底面）すなわち、支持多角形の各頂点で床反力を検出することによって、これまで図６（ｂ）に示す矢印方向の仮想面内（図３において、前後方向に延びる面内）に限定した運動を、３次元空間へ拡張することも可能である。
【０１４６】
（４） 前記各実施形態では、ＰＤ制御での目標値θ_d（目標角度）を０としたが目標値θ_dを０に限定する必要はない。
（５） 前記各実施形態では、外力は一定と仮定してきた。しかし、第１実施形態の実験例でも示したように、ある程度のダイナミクスをもって傾斜面を作ったり、外力を変化しても脚１２は倒れずに安定した姿勢制御ができることが確認されている。これは、環境の変化が力フィードバックの収束性よりも遅ければ、その変化に追従できることを意味する。従って、フィードバックゲインＫ_fが大きくとれるほどの変化の速い環境にも適応でき、足関節のＰＤＦ制御の有効性が高いものと言える。
【０１４７】
従って、外力は必ずしも一定である必要はない。
（６） 前記各実施形態では、ＰＤ制御を行うことを前提にしていたが、機械１０のハード構成として、下記のように構成した場合には、ＰＤ制御を省いた姿勢制御装置１としてもよい。
【０１４８】
この考え方は、ＰＤＦ制御では、平衡状態の姿勢は重力と外力のみによって決定され、ＰＤ制御の目標値θ_d（（１２）式参照）には依存していないことから結論ずけられる。
【０１４９】
このため、適当な姿勢に対する弾性と粘性とが、足関節Ｏに与えられていればよいことになる。
従って、本変形例では、足関節ＯのＰＤＦ制御のＰＤ制御部分の代わりに、図１２に示すように、脚１２の下部と足部材１１間に、スプリング手段としてのスプリング２５と、ダンパ手段としてのダンパ２６とが並設されている。そして、スプリング２５とダンパ２６とにより、脚１２が足部材１１と連係されており、脚１２の姿勢に対して弾性と粘性とが付与されている。なお、ダンパ２６は、脚１２に対して、その回動を干渉しないようにその一端が回動自在に取付られている。
【０１５０】
このようにした場合、一過性の外乱等に対してはとりあえず、スプリング２５とダンパ２６とにより対処して平衡を保ち、その結果として重さのかけ方に不均等が生じた際に力フィードバックで解消する。
【０１５１】
この場合、姿勢制御装置１の制御フローチャートにおいては、Ｓ３０が省略されるとともに、Ｓ１０，Ｓ２０，Ｓ４０〜Ｓ６０の処理を行うが、下記で説明するところが異なっている。
【０１５２】
Ｓ１０では、各ロードセル１６ａ〜１６ｄの検出値のみを読み込み、Ｓ５０においては、τ＝Ｋ_f・τ_fの演算を行う。
この結果、未知の状況下において、外力が付与された際、その外力に応じて姿勢を変化させることができ、安定した平衡状態にすることができる。
【０１５３】
又、ＰＤ制御を行うことなく、制御に足関節の傾き角度に関する情報は一切用いないにもかかわらず、未知環境下においても、その外力に応じて姿勢を変化させることができ適正な姿勢制御を実現することができる。
（７） 前記各実施形態では、脚の傾き角度を検出する角度検出手段として、
エンコーダ１５を設けたが、この代わりに脚１２の傾き角度の角速度を検出する角速度検出手段として回転速度センサをモータ１３に設けてもよい。回転速度センサとしてはタコジェネレータが使用される。この場合、ＣＰＵ１９は、回転速度センサから入力した角速度を、ＲＡＭ２１に格納し、さらに角速度を積分して得た角度θをＲＡＭ２１に格納するものとする。
【０１５４】
【発明の効果】
以上詳述したように、請求項１乃至請求項９の発明によれば、未知環境下においても、その外力に応じて姿勢を変化させることができ適正な姿勢制御を実現することができる効果を奏する。
【０１５５】
請求項１乃至請求項７の発明によれば、外力に応じて姿勢を変化させ、外力と重力が足関節周りにつくるモーメントの釣り合う姿勢を平衡状態とすることができ、又、その平衡状態の局所的な安定性を保証できる効果がある。
【０１５６】
請求項８及び請求項９の発明によれば、さらに、制御入力のために足関節の傾き角度の検出を行うことなく、未知環境下においても、その外力に応じて姿勢を変化させることができ適正な姿勢制御を実現することができる。
【図面の簡単な説明】
【図１】姿勢制御装置及び脚を有した機械の説明図。
【図２】姿勢制御装置１の電気ブロック図。
【図３】機械の要部側面図。
【図４】機械の平面図。
【図５】コンピュータ１８が実行する制御フローチャート図。
【図６】（ａ）はロボットの概念図、（ｂ）は２つのリンク系の説明図。
【図７】胴部に働く重力と外力及び合力の説明図。
【図８】（ａ）は傾斜面上の機械に作用する外力の説明図、（ｂ）は平面上の機械に作用する外力の説明図。
【図９】は実験例の足関節角度（足関節の傾き角度θ）の時間変化を示すグラフ。
【図１０】同じくτ，τθ，Ｋ_fτ_fの時間変化を示すグラフ。
【図１１】同じく床反力の時間変化を示すグラフ。
【図１２】他の実施形態の機械の要部側面図。
【図１３】他の実施形態の（ａ）はロボットの概念図、（ｂ）は多リンク系の説明図、（ｃ）はリンクの姿勢変化の範囲の説明図。
【符号の説明】
１…姿勢制御装置
１０…脚を有する機械
１１…足部材（接地部材）
１２…脚
１３…モータ（駆動手段）
１５…エンコーダ（角度検出手段）
１６ａ〜１６ｄ…ロードセル（床反力検出手段）
１８…コンピュータ（制御手段）
１９…中央処理装置（ＣＰＵ）
２０…ＲＯＭ
２１…ＲＡＭ
２５…スプリング（スプリング手段）
２６…ダンパ（ダンパ手段）

Claims (9)

1. 接地する接地部材と、同接地部材に対して足関節を介して回動自在に設けられた脚と、前記脚を駆動する駆動手段と、前記駆動手段を駆動制御する制御手段とを備えた脚を有する機械の姿勢制御装置において、
   接地部材が床に接地した複数部分に働く床反力をそれぞれ検出する床反力検出手段と、
   前記脚の傾き角度を検出する角度検出手段又は前記脚の傾き角度の角速度を検出する角速度検出手段とを備え、
   前記制御手段は、外力が付与された際に検出された前記角度又は角速度に基づいて得られた角度を予め定めた目標角度に制御するＰＤ制御を行うとともに、外力が付与された際に検出された複数の接地部分に働く床反力の差が所定値になるように収束制御する力フィードバック制御とを行い、さらに、制御手段は両制御により足関節トルクの演算をし、演算された足関節トルクに基づいて前記駆動手段を駆動制御することを特徴とする脚を有する機械の姿勢制御装置。
2. 前記所定値は０であることを特徴とする請求項１に記載の脚を有する機械の姿勢制御装置。
3. 前記所定値は０以外であることを特徴とする請求項１に記載の脚を有する機械の姿勢制御装置。
4. 前記ＰＤ制御における収束性は、前記力フィードバック制御における収束性よりも速いことを特徴とする請求項１乃至請求項３のうちいずれか１項に記載の脚を有する機械の姿勢制御装置。
5. 接地する接地部材と、同接地部材に対して足関節を介して回動自在に設けられた脚と、前記脚を駆動する駆動手段とを備えた脚を有する機械の姿勢制御方法において、
   外力が付与された際に接地部材が床に接地した複数部分に働く床反力をそれぞれ検出するとともに前記脚の傾き角度又は脚の傾きの角速度を検出する工程と、 検出された前記角度又は角速度に基づいて得られた角度を予め定めた目標角度に制御するＰＤ制御を行う工程と、
   外力が付与された際に検出された複数の接地部分に働く床反力の差が所定値になるように収束制御する力フィードバック制御を行う工程と、
   前記両制御により足関節トルクの演算をし、演算された足関節トルクに基づいて前記駆動手段を駆動制御する工程を含むことを特徴とする脚を有する機械の姿勢制御方法。
6. 前記所定値は０であることを特徴とする請求項５に記載の脚を有する機械の姿勢制御方法。
7. 前記所定値は０以外であることを特徴とする請求項５に記載の脚を有する機械の姿勢制御方法。
8. 接地する接地部材と、同接地部材に対して足関節を介して回動自在に設けられた脚と、前記脚を駆動する駆動手段と、前記駆動手段を駆動制御する制御手段とを備えた脚を有する機械の姿勢制御装置であって、
   前記脚と接地部材をダンパ手段及びダンパ手段と並設したスプリング手段にて連係し、
   接地部材が床に接地した複数部分に働く床反力をそれぞれ検出する床反力検出手段を備え、
   前記制御手段は、外力が付与された際に検出された複数の接地部分に働く床反力の差が所定値になるように収束制御する力フィードバック制御を行い、さらに、制御手段は、前記制御により足関節トルクの演算をし、演算された足関節トルクに基づいて前記駆動手段を駆動制御することを特徴とする脚を有する機械の姿勢制御装置。
9. 接地する接地部材と、同接地部材に対して足関節を介して回動自在に設けられた脚と、前記脚を駆動する駆動手段とを備え、前記脚と接地部材をダンパ手段及びダンパ手段と並設したスプリング手段にて連係した脚を有する機械の姿勢制御方法であって、
   外力が付与された際に接地部材が床に接地した複数部分に働く床反力をそれぞれ検出する検出する工程と、
   外力が付与された際に検出された複数の接地部分に働く床反力の差が所定値になるように収束制御する力フィードバック制御を行う工程と、
   前記制御により足関節トルクの演算をし、演算された足関節トルクに基づいて前記駆動手段を駆動制御する工程を含むことを特徴とする脚を有する機械の姿勢制御方法。

Images