TOP > 国内特許検索 > 脚を有する機械の姿勢制御装置及び制御方法 > 明細書

明細書 :脚を有する機械の姿勢制御装置及び制御方法

発行国 日本国特許庁(JP)
公報種別 特許公報(B2)
特許番号 特許第3637387号 (P3637387)
公開番号 特開2003-080478 (P2003-080478A)
登録日 平成17年1月21日(2005.1.21)
発行日 平成17年4月13日(2005.4.13)
公開日 平成15年3月18日(2003.3.18)
発明の名称または考案の名称 脚を有する機械の姿勢制御装置及び制御方法
国際特許分類 B25J  5/00      
FI B25J 5/00 E
請求項の数または発明の数 9
全頁数 24
出願番号 特願2001-271863 (P2001-271863)
出願日 平成13年9月7日(2001.9.7)
審査請求日 平成13年9月7日(2001.9.7)
特許権者または実用新案権者 【識別番号】304019399
【氏名又は名称】国立大学法人岐阜大学
発明者または考案者 【氏名】伊藤 聡
【氏名】川崎 晴久
個別代理人の代理人 【識別番号】100068755、【弁理士】、【氏名又は名称】恩田 博宣
【識別番号】100105957、【弁理士】、【氏名又は名称】恩田 誠
審査官 【審査官】八木 誠
参考文献・文献 特開平3-32582(JP,A)
特開平3-184781(JP,A)
特開2000-254888(JP,A)
特開平6-31658(JP,A)
調査した分野 B25J5/00
特許請求の範囲 【請求項1】
接地する接地部材と、同接地部材に対して足関節を介して回動自在に設けられた脚と、前記脚を駆動する駆動手段と、前記駆動手段を駆動制御する制御手段とを備えた脚を有する機械の姿勢制御装置において、
接地部材が床に接地した複数部分に働く床反力をそれぞれ検出する床反力検出手段と、
前記脚の傾き角度を検出する角度検出手段又は前記脚の傾き角度の角速度を検出する角速度検出手段とを備え、
前記制御手段は、外力が付与された際に検出された前記角度又は角速度に基づいて得られた角度を予め定めた目標角度に制御するPD制御を行うとともに、外力が付与された際に検出された複数の接地部分に働く床反力の差が所定値になるように収束制御する力フィードバック制御とを行い、さらに、制御手段は両制御により足関節トルク演算をし、演算された足関節トルクに基づいて前記駆動手段を駆動制御することを特徴とする脚を有する機械の姿勢制御装置。
【請求項2】
前記所定値は0であることを特徴とする請求項1に記載の脚を有する機械の姿勢制御装置。
【請求項3】
前記所定値は0以外であることを特徴とする請求項1に記載の脚を有する機械の姿勢制御装置。
【請求項4】
前記PD制御における収束性は、前記力フィードバック制御における収束性よりも速いことを特徴とする請求項1乃至請求項3のうちいずれか1項に記載の脚を有する機械の姿勢制御装置。
【請求項5】
接地する接地部材と、同接地部材に対して足関節を介して回動自在に設けられた脚と、前記脚を駆動する駆動手段とを備えた脚を有する機械の姿勢制御方法において、
外力が付与された際に接地部材が床に接地した複数部分に働く床反力をそれぞれ検出するとともに前記脚の傾き角度又は脚の傾きの角速度を検出する工程と、 検出された前記角度又は角速度に基づいて得られた角度を予め定めた目標角度に制御するPD制御を行う工程と、
外力が付与された際に検出された複数の接地部分に働く床反力の差が所定値になるように収束制御する力フィードバック制御を行う工程と、
前記両制御により足関節トルク演算をし、演算された足関節トルクに基づいて前記駆動手段を駆動制御する工程を含むことを特徴とする脚を有する機械の姿勢制御方法。
【請求項6】
前記所定値は0であることを特徴とする請求項5に記載の脚を有する機械の姿勢制御方法。
【請求項7】
前記所定値は0以外であることを特徴とする請求項5に記載の脚を有する機械の姿勢制御方法。
【請求項8】
接地する接地部材と、同接地部材に対して足関節を介して回動自在に設けられた脚と、前記脚を駆動する駆動手段と、前記駆動手段を駆動制御する制御手段とを備えた脚を有する機械の姿勢制御装置であって、
前記脚と接地部材をダンパ手段及びダンパ手段と並設したスプリング手段にて連係し、
接地部材が床に接地した複数部分に働く床反力をそれぞれ検出する床反力検出手段を備え、
前記制御手段は、外力が付与された際に検出された複数の接地部分に働く床反力の差が所定値になるように収束制御する力フィードバック制御を行い、さらに、制御手段は、前記制御により足関節トルク演算をし、演算された足関節トルクに基づいて前記駆動手段を駆動制御することを特徴とする脚を有する機械の姿勢制御装置。
【請求項9】
接地する接地部材と、同接地部材に対して足関節を介して回動自在に設けられた脚と、前記脚を駆動する駆動手段とを備え、前記脚と接地部材をダンパ手段及びダンパ手段と並設したスプリング手段にて連係した脚を有する機械の姿勢制御方法であって、
外力が付与された際に接地部材が床に接地した複数部分に働く床反力をそれぞれ検出する検出する工程と、
外力が付与された際に検出された複数の接地部分に働く床反力の差が所定値になるように収束制御する力フィードバック制御を行う工程と、
前記制御により足関節トルク演算をし、演算された足関節トルクに基づいて前記駆動手段を駆動制御する工程を含むことを特徴とする脚を有する機械の姿勢制御方法。
発明の詳細な説明 【0001】
【発明の属する技術分野】
本発明は、脚を有する機械の姿勢制御装置及び制御方法に関するものである。
【0002】
【従来の技術】
脚を有する機械等の脚を有する機械では、直立姿勢を維持する場合、位置のフィードバック制御が主に用いられている。その方法では、平衡が保たれる姿勢を予め計算しておき、その姿勢からのずれを補償することにより、直立状態を維持する。この制御方法は、脚を有する機械がおかれる環境が既知であれば有効な方法である。
【0003】
なお、本明細書で、環境とは外力が定常的に加えられている状況と定義する。
しかし、既知である環境は現実的には稀であり、例えば傾斜角が不明な斜面で直立しなければならない状況がある。人間であれば、直立した状態において、そのような環境になると、平面に直立していた場合よりも足関節を伸展或いは屈曲させ、上体が鉛直方向に向くような姿勢をとる。又、横方向からの力(例えば、風力、水力)を受け続ける場合に、上体をその力が付与される上流の方向に倒した状態で姿勢を維持する。
【0004】
【発明が解決しようとする課題】
しかし、脚を有する機械では、前記のような環境に応じて直立姿勢を変化させる場合、その姿勢は環境が与えられなければ決まらない。例えば、坂の傾斜角度が分からないと、足関節の取るべき角度が決まらない問題がある。このことは、直立姿勢の維持を位置のフィードバック制御で行おうとするときに問題となる。
【0005】
直立姿勢を維持する最も簡単な方法は、足関節角度を検出して、その角度に対する位置のフィードバックをかけるものである。しかし、フィードバックする足関節角度の目標値が、環境が与えられた後、すなわち、事後的にしか決まらない。この理由から、未知環境下での平衡制御において、従来提案されている制御方法がうまく利用できない問題がある。
【0006】
脚を有する機械における二足歩行系の平衡制御では、歩行運動がダイナミクスを持つため、慣性力と重力が作るモーメントがゼロとなるZMP(zero moment point)に着目した制御法が提案されている。しかし、ZMPが用いられるのは、主に歩行運動の計画時である。すなわち、ZMPが足底(足底は、一般に多角形をなす支持面であることから、支持多角形ということがある)内部に常に止まるように各関節起動を事前に設計し、実際の歩行運動の制御はその目標関節軌道に関するフィードバックで達成されている。
【0007】
従って、この制御方法では、目標値が環境により事後的に決定する場合においては、そのままでは利用できない問題がある。
本発明は上記の課題を解消するためになされたものであり、未知環境下においても、その外力に応じて姿勢を変化させることができ適正な姿勢制御を実現することができる脚を有する機械の姿勢制御装置及び制御方法を提供することを目的としている。
【0008】
【課題を解決するための手段】
上記の課題を解決するために、請求項1に記載の発明は、接地する接地部材と、同接地部材に対して足関節を介して回動自在に設けられた脚と、前記脚を駆動する駆動手段と、前記駆動手段を駆動制御する制御手段とを備えた脚を有する機械の姿勢制御装置において、接地部材が床に接地した複数部分に働く床反力をそれぞれ検出する床反力検出手段と、前記脚の傾き角度を検出する角度検出手段又は前記脚の傾き角度の角速度を検出する角速度検出手段とを備え、前記制御手段は、外力が付与された際に検出された前記角度又は角速度に基づいて得られた角度を予め定めた目標角度に制御するPD制御を行うとともに、外力が付与された際に検出された複数の接地部分に働く床反力の差が所定値になるように収束制御する力フィードバック制御とを行い、さらに、制御手段は両制御により足関節トルク演算をし、演算された足関節トルクに基づいて前記駆動手段を駆動制御することを特徴とする脚を有する機械の姿勢制御装置を要旨とするものである。
【0009】
請求項2の発明は、請求項1において、前記所定値は0であることを特徴とする。
請求項3の発明は、請求項1において、前記所定値は0以外であることを特徴とする。
【0010】
請求項4の発明は、請求項1乃至請求項3のうちいずれか1項において、前記PD制御における収束性は、前記力フィードバック制御における収束性よりも速いことを特徴とする。
【0011】
請求項5の発明は、接地する接地部材と、同接地部材に対して足関節を介して回動自在に設けられた脚と、前記脚を駆動する駆動手段とを備えた脚を有する機械の姿勢制御方法において、外力が付与された際に接地部材が床に接地した複数部分に働く床反力をそれぞれ検出するとともに前記脚の傾き角度又は脚の傾きの角速度を検出する工程と、検出された前記角度又は角速度に基づいて得られた角度を予め定めた目標角度に制御するPD制御を行う工程と、外力が付与された際に検出された複数の接地部分に働く床反力の差が所定値になるように収束制御する力フィードバック制御を行う工程と、前記両制御により足関節トルク演算をし、演算された足関節トルクに基づいて前記駆動手段を駆動制御する工程を含むことを特徴とする脚を有する機械の姿勢制御方法を要旨とするものである。
【0012】
請求項6の発明は、請求項5において、前記所定値は0であることを特徴とする。
請求項7の発明は、請求項5において、前記所定値は0以外であることを特徴とする。
【0013】
請求項8の発明は、接地する接地部材と、同接地部材に対して足関節を介して回動自在に設けられた脚と、前記脚を駆動する駆動手段と、前記駆動手段を駆動制御する制御手段とを備えた脚を有する機械の姿勢制御装置であって、前記脚と接地部材をダンパ手段及びダンパ手段と並設したスプリング手段にて連係し、接地部材が床に接地した複数部分に働く床反力をそれぞれ検出する床反力検出手段を備え、前記制御手段は、外力が付与された際に検出された複数の接地部分に働く床反力の差が所定値になるように収束制御する力フィードバック制御を行い、さらに、制御手段は、前記制御に基づいて足関節トルク演算をし、演算された足関節トルクに基づいて前記駆動手段を駆動制御することを特徴とする脚を有する機械の姿勢制御装置を要旨とするものである。
【0014】
請求項9の発明は、接地する接地部材と、同接地部材に対して足関節を介して回動自在に設けられた脚と、前記脚を駆動する駆動手段とを備え、前記脚と接地部材をダンパ手段及びダンパ手段と並設したスプリング手段にて連係した脚を有する機械の姿勢制御方法であって、外力が付与された際に接地部材が床に接地した複数部分に働く床反力をそれぞれ検出する検出する工程と、外力が付与された際に検出された複数の接地部分に働く床反力の差が所定値になるように収束制御する力フィードバック制御を行う工程と、前記制御により足関節トルク演算をし、演算された足関節トルクに基づいて前記駆動手段を駆動制御する工程を含むことを特徴とする脚を有する機械の姿勢制御方法を要旨とするものである。
【0015】
【発明の実施の形態】
(第1実施形態)
以下、本発明を具体化した第1実施形態を図1~図8を参照して説明する。
【0016】
図1は、脚を有する機械10及びその姿勢制御装置1の電気的構成を示す説明図、図2は脚を有する機械10の姿勢制御装置1の電気的構成を示すブロック回路図を示している。図3は脚を有する機械の要部側面図、図4は脚を有する機械の平面図を示している。なお、図3において、左が後、右を前とする。
【0017】
機械10は足部材11、脚12、モータ13等とを備えている。足部材11は、四角板状をなし、その上面に脚12が軸受14を介して回動自在に軸支されている。なお、脚12が回動自在に設けられた軸支点Oを足関節ということがある。すなわち、脚12は図3に示すように前後方向に回転自在とされている。なお、脚12は、図示しない胴体部材を支持するが、省略されている。従って、以下では、脚12及び胴体部分を含むものを胴部という。足部材11は接地部材に相当する。モータ13は足部材11に対して固定され、その出力軸は脚12を軸支する軸に連結されている。駆動手段としてのモータ13はエンコーダ15(ロータリエンコーダ)を備えたDCモータから構成されている。
【0018】
エンコーダ15はモータ回転角を検出するようにされており、脚12の傾き角度(床面に対する垂線であって、足関節Oを通る垂線aからの傾き角度θであり、足関節の傾き角度ともいうことがある)を検出する角度検出手段に相当する。エンコーダ15の検出信号はパルスカウンタ31を介してコンピュータ18に出力される。
【0019】
足部材11の接地面(下面)には4隅に床反力検出手段としてのロードセル16a~16dが設けられている。各ロードセル16a~16dは足部材11が床面に接地された際、床面に垂直な床反力をそれぞれ検出することが可能とされている。各ロードセル16a~16dにて検出された検出信号は動ひずみ測定器17、A/D変換器30、パルスカウンタ31を介して制御手段としてのコンピュータ18に出力される。
【0020】
従って、各ロードセル16a~16dのうち、脚12の軸支点を挟んで足部材11の前側に位置するロードセル16a,16bは足先の床反力を検出する。又、ロードセル16a~16dのうち、脚12の軸支点を挟んで足部材11の後側に位置するロードセル16c,16dは踵の床反力を検出する。
【0021】
図2に示すようにコンピュータ18は中央処理装置19(CPU)、ROM20、RAM21を備えている。
CPU19は、エンコーダ15及び各ロードセル16a~16dの検出値を動ひずみ測定器17、A/D変換器30、パルスカウンタ31を介して入力し、RAM21内に書き込む。又、エンコーダ15から入力した傾き角度θをCPU19は微分し、角速度としてRAM21内に書き込む。CPU19は、ROM20内に格納された制御プログラムに基づいて機械10の姿勢制御を好適に行うようにされている。すなわち、前記入力した検出値に基づいて足関節トルクとしてのモータ13のトルク演算を行い、算出されたトルクをD/A変換器32、DCアンプ22を介してモータ13に送出する。モータ13はこのトルク値に基づいて駆動され、脚12の姿勢を制御する。
【0022】
次に、姿勢制御装置1の動作を説明する。
まず、機械10は、本実施形態においては図6(a)に示す歩行運動を行う歩行系のロボットを想定し、これを簡略して図6(b)に示すような足部(足部材11)とそれ以外の胴部(脚12を含む)の2つのリンク系としてモデル化したものである。
【0023】
そして、モデルの運動は図6(b)に示す矢印方向の仮想面内(図3において、前後方向に延びる面内)に限定し、以下の条件を仮定している。
1)足関節(脚12の軸支点)Oにはアクチュエータ(モータ13)があり、適当な足関節トルクτが出力できる。
【0024】
2)足関節Oの傾き角度θ(モータ角度)及び角速度が計測できる。
3)足部(足部材11)は前後方向の両先端(踵と足先)の両方で接地し、それぞれの接地点では床反力の垂直成分(それぞれFH,FTとする。)が計測できる。
【0025】
4)水平方向にはFx,鉛直方向にはFyの未知な一定の外力が加わる。
5)床は水平で滑らないものとする。
上記のような仮定の下では胴部のみがダイナミクスをもち、その運動は下記(1)式で記述できる。
【0026】
【数1】
JP0003637387B2_000002t.gif又、ここで、Mは胴部の質量、Iはその足関節O周り(軸支点周り)の慣性モーメント、Lは胴部の重心J1と足関節O間の長さ、gは重力加速度である。
【0027】
一方、リンク間に働く水平方向、鉛直方向の内力fx及びfyは下記(2)式、(3)式で与えられる。
【0028】
【数2】
JP0003637387B2_000003t.gif【0029】
【数3】
JP0003637387B2_000004t.gifこれらを用いると、床反力FT及びFHは、踵及び足先周りのモーメントの釣り合いの条件より、下記(4)式及び(5)式となる。
【0030】
【数4】
JP0003637387B2_000005t.gif【0031】
【数5】
JP0003637387B2_000006t.gifここで、lT,lH,lGは、足関節Oからそれぞれ足先、踵、足部(足部材11)の重心J2までの長さである。又、mTとmHはそれぞれ足先、踵にかかる足部(足部材11)の重量であり、下記(6)式で与えられる。
【0032】
【数6】
JP0003637387B2_000007t.gifこのモデルにおいて、環境、すなわち、一定外力Fx,Fyに対して胴部の姿勢が適切に変化させ得るような足関節トルクτを、足関節Oの傾き角度θと
【0033】
【数7】
JP0003637387B2_000008t.gif及び床反力FH,FTを基に決定する。すなわち、本実施形態では、ROM20内に格納した制御プログラムは、
【0034】
【数8】
JP0003637387B2_000009t.gifを基に制御入力(足関節トルクτ)を決定する。
【0035】
なお、説明の便宜上、以降の計算を簡単にするため、(1)式を以下の(7)式のように変形しておく。
【0036】
【数9】
JP0003637387B2_000010t.gifここで、Aは、下記の(8)式で表されており、θfは下記の(9)式を満たす定数である。
【0037】
【数10】
JP0003637387B2_000011t.gif【0038】
【数11】
JP0003637387B2_000012t.gifここで、(8)式、及び(9)式から分かるように、重要なことは、Aとθfがともに環境を表すFx,Fyに依存していることである。
【0039】
(力フィードバックによる制御)
次に、力フィードバックによる制御について説明する。
まず、理論が簡単である足部(足部材11)が対称性をもつ場合、すなわち、lT=lH=l、lG=0の場合について説明する。
【0040】
転倒が起きないためには、FTとFHがいずれも正の値を保つ必要がある。
そして、足先と踵に体重が均等にかかり、FT=FHとなった場合、最も安定性が高くなる。これは対称な足底において、ZMPがその中央にくることが説明できる。
【0041】
従って、2つの床反力FTとFHの差を小さくするように、足関節トルクτ(制御入力)を決定する。
前記(4)式と(5)式より、FTとFHとτの関係は下記(10)式で与えられる。
【0042】
【数12】
JP0003637387B2_000013t.gifこれより、定数KIを用いて、τを下記(11)式とすれば、FH-FTを0に収束させることができる。
【0043】
【数13】
JP0003637387B2_000014t.gifただし、この制御入力のみでは、直立姿勢を必ずしも安定化することができない。FT=FHを保ったまま胴部(脚12を含む)が転倒してしまうからである。
【0044】
(PD制御と力フィードバックによる制御との組合せ)
胴部の安定化を図るには、足関節の傾き角度θのPD制御が有効である。
PD制御での制御入力分は、下記(12)式で決めることができる。
【0045】
【数14】
JP0003637387B2_000015t.gifここで、(12)式中のτθはPD制御で得られる制御入力分であって、微分項と比例項とからなり、Kd,Kpは正の定数、θdはPD制御の目標値である。姿勢制御において、直立姿勢を目指す場合は、θd=0とするが、後述する力フィードバック制御との組合せにより、この値に限定するものではない。
【0046】
なお、外力が存在するため、θは目標値θdには収束しない。
そのときの収束値θ0を算出する。
τ=τθを(7)式に代入すると、下記(13)式が得られる。
【0047】
【数15】
JP0003637387B2_000016t.gifここで、θ-θf~0を仮定して上式をθをθfのまわりで線形化すると、下記(14)式となる。
【0048】
【数16】
JP0003637387B2_000017t.gifここで、θ0は下記(15)式となる。
【0049】
【数17】
JP0003637387B2_000018t.gifこれより、Kp>ALが成立すれば、第1次近似ではθはθ0に収束する。
【0050】
このPD制御を行うことにより、胴部の局所的な安定性が得られることになる。そこで、前記(11)式のような体重を足先と踵に均等にかけさせる床反力のフィードバックを制御入力に加える。
【0051】
すなわち、力フィードバックとして、下記(16)式を定義する。
【0052】
【数18】
JP0003637387B2_000019t.gifそして、最終的な足関節トルクτ(制御入力)を下記(17)式にて定義する。
【0053】
τ=τθ+Kf・τf …(17)
ここでKfは力のフィードバックゲインである。
以降では、この姿勢制御方法を、この明細書では、PD制御と力フィードバックの組合せによることから足関節のPDF制御と定義する。
【0054】
なお、前記(12)式、(16)式、及び(17式)で与えられる足関節トルクに対し胴部の局所的安定性が保証できる。
上記のように、足関節のPDF制御において、足部(足部材11)が対称性をもつ場合、すなわち、lT=lH=l、lG=0の場合の局所的安定性について説明する。
【0055】
この足関節のPDF制御(対称性を有する足部材)の局所的安定性に関しての条件を以下、定理1という。
(定理1)
T=lH=l、lG=0の場合、前記(1)式で与えられる系のダイナミクスに対して、(12)式、(16)式及び(17)式で与えられる足関節トルクを考える。
【0056】
もし、フィードバックゲイン(定数)Kd,Kp,Kfが下記の(18)式~(20)式の条件を満たせば、θ=θfは局所的漸近安定な平衡点となる。
p>AL>0 …(18)
(l/I)Kd>Kf>0 …(19)
(Kdl-Kfl)Kp>KdlAL …(20)
(18)式~(20)式の条件は、外力によって定まるパラメータAを含んでいる。これにより、各フィードバックゲインKd,Kp,Kfが与えられたとき、どれくらいの外力に耐えうるかを見積もることが可能である。逆に外力の最大値が既知の場合は、これらの式により各フィードバックゲインが決定できる。
【0057】
なお、前記(11)式は(17)式において、Kd=Kp=0と設定したものに相当する。この場合には、前記(18)式~(20)式で与えられる条件を満たさない。従って、力フィードバックのみの制御では、胴部が安定せず、不安定となり、転倒する。
【0058】
なお、(18)式~(20)式の算出は、下記の通りに行うことによって得られる。
(7)式に、(17)、(12)式を代入すると、下記(21)式が得られる。
【0059】
【数19】
JP0003637387B2_000020t.gif一方、(16)式を微分した式に(10)式、(12)式及び(17)式を代入すると、(22)式が得られる。
【0060】
【数20】
JP0003637387B2_000021t.gifそこで、(21)式と(22)式で与えられるダイナミクスの
【0061】
【数21】
JP0003637387B2_000022t.gif【0062】
【数22】
JP0003637387B2_000023t.gifと、
【0063】
【数23】
JP0003637387B2_000024t.gifを代入すると、(21)式及び(22)式はそれぞれ下記の(23)式及び(24)式となる。この代数方程式を解くと、下記の(25)式が得られる。
【0064】
【数24】
JP0003637387B2_000025t.gif【0065】
【数25】
JP0003637387B2_000026t.gif【0066】
【数26】
JP0003637387B2_000027t.gif次にこの平衡点の安定性を調べる。
【0067】
θ=θ1、
【0068】
【数27】
JP0003637387B2_000028t.gifとおき、方程式を平衡点まわりで線形化すると下記(26)式となる。
【0069】
【数28】
JP0003637387B2_000029t.gifこの線形微分方程式の特性方程式は下記(27)式で与えられる。
【0070】
【数29】
JP0003637387B2_000030t.gifここで、
【0071】
【数30】
JP0003637387B2_000031t.gifである。
【0072】
Routh/Hurwitzの安定判別法を用いると、平衡点が安定となるための必要十分条件は
P0>0,P1>0,P2>0,P1・P2-P0>0 …(29)
である。上式により、(18)式~(20)式が導出される。
【0073】
(制御フローチャートの説明)
さて上記のような前提で、本実施形態における姿勢制御装置1が所定時間周期で実行する制御プログラムを図5のフローチャートを参照して説明する。
【0074】
この制御プログラムがスタートすると、ステップ(以下、ステップをSという)10において、各ロードセル16a~16dの検出値及びエンコーダ15の検出値を読み込む。S20においては、床反力FT及び床反力FHの算出を行う。本実施形態では、床反力FTは足先側のロードセル16a,16bの検出値を加算して求める。床反力FHは踵側のロードセル16c,16dの検出値を加算して求める。
【0075】
次のS30では、PD制御を行い、τθを前記(12)式により求める。
本実施形態では、直立姿勢を安定化するものとして、目標値θdをθd=0としている。
【0076】
次のS40では、力フィードバックを行い、τfを前記(16)式により求める。続く、S50において、S30及びS40で算出したτθ、τfに基づいて最終的な足関節トルクτ(制御入力)を前記(17)式により算出する。
【0077】
そして、S60において、算出した足関節トルクτをDCアンプ22を介してモータ13に出力し、この制御フローチャートを一旦終了する。
さて、上記実施形態の特徴的な利点について述べる。
【0078】
(1) 本実施形態では、足関節トルクτとして(17)式を用いると、定常状態での胴部(脚12を含む)姿勢は、θ=θfとなる。ここで、前述したように、θfは環境、すなわち、一定な外力Fx,Fyに依存していることに注意をする。これは、定常状態の姿勢が、環境に合わせて変化することを意味している。
【0079】
この一定外力が付与される場合とは、例えば、機械10が斜面に載置された場合、機械10に横風が働き、風力が係る場合、或いは、水中に載置された状態で横から水力が係る場合等が考えられる。
【0080】
定常状態での姿勢θ=θfでは、図7に示すように胴部(脚12を含む)が重力と外力との合力の方向に向かうことに注意されたい。
従って、もし外力が働かなければ、鉛直方向が定常状態での姿勢となる。この姿勢をとることの利点は、足関節周りの重力と外力とのモーメントが釣り合っているため、その姿勢の維持がわずかなトルクで可能であることを意味する。
【0081】
計算上では、前記(25)式を(12)式と(17)式に代入すると、わかるように足関節トルクτはτ=0となる。このことから、エネルギー消費の点から見ても有利である。
【0082】
このように、重力と外力のモーメントが釣り合うことから、姿勢の維持にはわずかなトルクしか必要とはせず、エネルギコスト削減の効果がある。
(2) 本実施形態の姿勢制御装置1は、接地する足部材11(接地部材)と、足部材11に対して回動自在に設けられた脚12と、脚12を駆動するモータ13(駆動手段)と、モータ13を制御するコンピュータ18(制御手段)とを備えるようにした。
【0083】
さらに姿勢制御装置1は、足部材11が床に接地した複数部分に働く床反力をそれぞれ検出するロードセル16a~16d(床反力検出手段)と、脚12の傾き角度θを検出するエンコーダ15(角度検出手段)とを備えた。
【0084】
そして、コンピュータ18は、外力が付与された際に検出された足関節の傾き角度θ(脚の傾き角度)を予め定めた目標値θd(目標角度)に制御するPD制御を行うとともに、外力が付与された際に検出された複数の接地部分に働く床反力の差(FH-FT)が0(所定値)になるように収束制御する力フィードバック制御とを行うようにした。さらに、コンピュータ18は両制御に基づいて(17)式にて足関節トルクτの算出(トルク演算)をし、演算された足関節トルクτに基づいてモータ13を駆動制御するようにした。
【0085】
この結果、未知環境下において、適正な姿勢制御を実現することができる。
又、足関節Oの傾き角度θとその角速度、床反力という脚12、足部材11の局所的な情報のみで制御入力(足関節トルク)が計算できる効果がある。
【0086】
(3) 本実施形態では、外力が付与された際に検出された複数の接地部分に働く床反力の差が0(所定値)になるように収束制御するようにした。
こうすると、足部材11に対称性がある場合、すなわち、lT=lH=l、lG=0の場合、上記(2)の作用効果を実現することができる。
【0087】
(4) 本実施形態での姿勢制御方法は、外力が付与された際に足部材11(接地部材)が床に接地した複数部分に働く床反力をそれぞれ検出するとともに脚12の傾き角度θを検出する工程と、検出された傾き角度θを予め定めた目標値θd(目標角度)に制御するPD制御を行う工程とを備えた。
【0088】
そして、外力が付与された際に検出された複数の接地部分に働く床反力の差(FH-FT)が0(所定値)になるように収束制御する力フィードバック制御を行う工程を備えた。さらに、前記両制御に基づいて足関節トルク演算をし、演算された足関節トルクに基づいてモータ13(駆動手段)を駆動制御するようにした。
【0089】
この結果、前記(2)と同じ効果を奏することができる。
(5) 本実施形態の姿勢制御方法では、外力が付与された際に検出された複数の接地部分に働く床反力の差が0(所定値)になるように収束制御するようにした。こうすると、前記(3)と同じ効果を奏することができる。
【0090】
(6) 本実施形態の姿勢制御方法では、PD制御での目標値θd(目標角度)を0とした。この結果、(4)と同じ効果を奏する。
(7) 足関節のPDF制御の基本的な考え方は、一過性の外乱等に対してはとりあえずPD制御にて対処して平衡を保ち、その結果として重さのかけ方に不均等が生じた際に力フィードバックで解消しようとするものである。
【0091】
従って、PD制御の収束性は、力フィードバックの収束性よりも速くする必要がある。大まかにいうと、PDF制御の収束性はフィードバックゲインKpとKd,力フィードバックの収束性はフィードバックゲインKfで決まる。
【0092】
この収束性の速さに関する条件は、(18)式、(19)式において、Kfの大きさに上限があることで表現されている。
従って、PD制御の収束性を、力フィードバックの収束性よりも速くすることにより、適正な姿勢制御を好適に実現することができる。
【0093】
(実験)
さて、上記の実施形態における実験例を説明する。
脚12は長さを0.55mとし、足部材11は前後方向の長さを0.12mとし、対称性のあるものとしている。脚12及び足部材11を含めた全体の質量は1.11Kgとしている。制御入力は、モータ13のエンコーダ15からの角度及びロードセル16a~16dからの床反力の情報を基に1ms毎に算出する。この実験での、フィードバックゲインは、Kp=2.5、Kd=1.5、Kf=0.01とした。
【0094】
この機械10を傾斜角αの傾斜面に載置した場合、傾斜面での環境は図8(a)、(b)に示すように、水平面上でFx=Mgsinα,Fy=Mg(1-cosα)の一定外力が働く場合と等価となる。なお、図8(a)、(b)では、説明の便宜上、モータ13、各ロードセル16a~16d等は省略して図示している。
【0095】
本実験では、下記3つのステージからなる。
初期状態として、機械10を水平な板の上に置く(ステージ1)。
この状態から、板の左端を持ち上げて傾斜面をつくり、しばらくの間一定傾斜角を保つ(ステージ2)。
【0096】
その後、再び板の左端を床に降ろし、水平な状態を維持する(ステージ3)。このときの数値データを図9~図11に示す。図9は、足関節角度(足関節の傾き角度θ)の時間変化を示している。板の傾斜角が一定のとき(ステージ2)、足関節の傾き角度θは0.3radに保たれ、又、板が水平に戻されたとき(ステージ3)、傾き角度θは0rad付近に戻っていることが分かる。すなわち、ステージ1では、脚12は、水平面に対して直立状態となっており、ステージ2では、傾斜面に適応するように、直立状態を保とうとして図8(a)の矢印方向に変化した。さらに、ステージ2からステージ3に移行すると、脚12は、前記矢印方向とは逆方向に傾き、直立状態を保持した。
【0097】
図10は、前記(17)式中の制御入力である、τ,τθ,Kfτfの時間変化を示している。板の傾斜角が一定に保たれているとき(ステージ2)、理論上制御入力τ(足関節トルクτ)は0となるはずである。しかし、足関節周りの摩擦が大きいため、定常偏差が残り、ここではτは0とはなっていない。図11は床反力の時間変化である。機械10の自重が足部材11の前後方向両端に均等にかかるように制御が行われていることが分かる。
【0098】
(第2実施形態)
次に第2実施形態について説明する。なお、第1実施形態の構成と同一構成については説明を省略し、異なるところ説明する。
【0099】
前記第1実施形態では、足部材11が対称性のあるものに具体化したが、本実施形態では、足部材11に対称性がない場合を想定したものである。すなわち、lH≠lG,lG≠0の場合を想定している。
【0100】
この場合、外力なしの状況で鉛直方向に直立しても、FTとFHは等しくならず、FTとFHはそれぞれ下記(30)式,(31)式で表される。
【0101】
【数31】
JP0003637387B2_000032t.gif【0102】
【数32】
JP0003637387B2_000033t.gif従って、FTとFHとの差が、(31)式と(32)式の差、すなわち下記(32)式で表されるF0に収束されるように、床反力のフィードバックτfを(16)式の替わりに下記(33)式で示すように修正している。
【0103】
【数33】
JP0003637387B2_000034t.gif【0104】
【数34】
JP0003637387B2_000035t.gifすなわち、本実施形態では、この修正した(33)式にて、τfを算出するように、前記第1実施形態の構成中、制御フローチャートのS40で行うようにしている。他の構成は第1実施形態と同じ構成である。
【0105】
従って、この修正した姿勢制御装置1及び姿勢制御方法により、対称性のない足部材11に対しても胴部(脚12を含む)の安定性が保証できる。
上記のように、足関節のPDF制御において、足部(足部材11)が対称性をもたない場合(非対称性の場合)の局所的安定性について説明する。
【0106】
この足関節のPDF制御(非対称性を有する足部材)の局所的安定性に関しての条件を、以下、定理2という。
(定理2)
前記(1)式におけるモデルの場合、前記(12)式、(17)式及び(33)式でそれぞれ定義される制御入力を考える。
【0107】
ここで、l=(lT+lH)/2とする。
フィードバックゲインKd,Kp,Kfが(18)式~(20)式を満たせば、θ=θfは局所的漸近安定な平衡点となる。
【0108】
このことは、(21)式及び(22)式と同様の微分方程式が導かれることから言える。
なお、(21)式の導出は、前記第1実施形態で説明した定理1と同様に(1)式に(17)式、(12)式を代入することにより自明である。
【0109】
又、(22)式の導出は、(33)式を微分した下記(34)式に対して、(5)式から(4)式を引いて得られる下記(35)式を代入する。
【0110】
【数35】
JP0003637387B2_000036t.gif【0111】
【数36】
JP0003637387B2_000037t.gifこのとき(32)式に注意すれば、(22)式と同様の式となる。以下の解析は定理1と同様であるので説明を省略する。
【0112】
なお、前記(12)式、(17)式及び(33)式でそれぞれ定義される制御入力を得る場合、(32)式に含まれるfyの値が必要になる。
この値は、(5)式と(4)式を足し合わせて得られる下記(36)式より求める。
【0113】
fy=FT+FH-(mT+mH)g=FT+FH-mg …(36)
足部材11の質量mが既知であれば、床反力フィードバックのFT,FHより算出できる。すなわち、本実施形態では、この質量mのデータはROM20内に予め格納されているものとし、本実施形態では、fyはFT,FHの算出時(S20)に、同時に算出されるものとする。
【0114】
第2実施形態によると、次のような作用効果を奏する。
(1) 第2実施形態の姿勢制御装置1では、外力が付与された際に検出された複数の接地部分に働く床反力の差(FH-FT)が0以外の所定値F0になるように収束制御する力フィードバック制御を行うようにした。
【0115】
この結果、対称性のない足部材11においても、胴部の安定性を保証することができる。
(2) 第2実施形態の姿勢制御方法では、外力が付与された際に検出された複数の接地部分に働く床反力の差(FH-FT)が0以外の所定値F0になるように収束制御する力フィードバック制御を行うようにした。この結果、上記(1)と同じ効果を奏することができる。
【0116】
(3) 本実施形態においても、姿勢制御装置1及び制御方法では、PD制御での目標値θd(目標角度)を0とした。この結果、第1実施形態と同様に胴部(脚12を含む)を直立状態に保持することができる。
【0117】
(第1実施形態と第2実施形態との共通の作用効果)
次に第1実施形態と第2実施形態との共通の作用効果について説明する。
環境から、一定外力Fx,Fyばかりでなく、一定のモーメントτeが同時に胴部にかかる場合においても、局所的漸近安定な平衡点が存在する効果がある。
【0118】
このことについて説明する。
このときの胴部の運動方程式は、下記の(37)式で与えられる。
【0119】
【数37】
JP0003637387B2_000038t.gifこの系に対し足部材11が対称ならば、第1実施形態の定理1で定義される足関節のPDF制御を実行し、足部材11が対称でないならば、第2実施形態の定理2で定義される足関節のPDF制御を実行する。
【0120】
この場合の平衡点について考える。
得られる微分方程式は下記(38)式、(39)式である。
【0121】
【数38】
JP0003637387B2_000039t.gif【0122】
【数39】
JP0003637387B2_000040t.gif従って、AL≧|τe|のとき平衡点(θe,Kp(θe-θd)/Kf)が存在する。ただし、θeは下記(40)式を満たす定数である。
【0123】
【数40】
JP0003637387B2_000041t.gif次に平衡点の安定性については下記のことから理解することができる。
【0124】
θ=θ1,
【0125】
【数41】
JP0003637387B2_000042t.gifとおき、(38)式,(39)式を平衡点周りで線形化すると、状態変数を(θ1,θ2,τf)としたときの線形微分方程式の係数行列は下記(41)式で表される。
【0126】
【数42】
JP0003637387B2_000043t.gifここで改めてAcos(θe-θf)>0をAと定義すると、以下、第1実施形態の定理1で説明したものと同様であるため、説明を省略する。
【0127】
この結果、(37)式が成立するモデル(系)においては、下記の定理3が得られる。
(定理3)
(37)式が成立するモデル(系)においては、外部から外力Fx,Fy及びモーメントτeが下記(42)式の関係を満たすと仮定する。
【0128】
【数43】
JP0003637387B2_000044t.gifこの系に対し、(12)式、(17)式及び(33)式で定義される制御入力を与える。そして、フィードバックゲインKd,Kp,Kfが(18)式~(20)式を満たすθ=θeは局所的漸近安定な平衡点となる。
【0129】
なお、ここでlは下記(数44)で表される。
【0130】
【数44】
JP0003637387B2_000045t.gifなお、前記(40)式より、定常状態での姿勢θ=θeは、足関節周りの外部からの外力及びモーメントが釣り合う姿勢であることが分かる。従って、定常状態では、足関節トルクτは理論上0でよく、姿勢の維持には効率がよい。
【0131】
本発明の実施形態は、上記実施形態以外に次のように変更することも可能である。
(1) 第1実施形態、第2実施形態では、足関節Oは床面と同じ高さにあると仮定しているが、厳密には足関節Oは床面よりも上にある。
【0132】
このため、厳密に制御を行う場合には、下記のようにしてもよい。
水平面からの足関節Oの高さをlAとおくと、床反力は胴部からの水平方向内力fxの影響を受け、下記の(43)式、(44)式で表される。
【0133】
【数45】
JP0003637387B2_000046t.gif【0134】
【数46】
JP0003637387B2_000047t.gif従って、前記(32)式で定義されるF0を下記(45)式で定義し直すと、定理2及び定理3と同じ条件で、同一の結果が得られる。
【0135】
【数47】
JP0003637387B2_000048t.gifなお、この場合、F0の決定には、fxの値が必要であるため、胴部と足部材11との2リンク間の水平方向内力fx(図6(b)参照)を検出するための力センサを設けるものとする。
【0136】
(2) 前記第1実施形態、第2実施形態では胴部(脚12を含む)を1リンクの剛体としてモデル化した。
この代わりに胴部を多リンクとしたものに具体化することも可能である。
【0137】
この多リンク系への応用可能性について説明する。
例えば、図13(a)に示すように、脛部Hを図6(a)の胴部とみなし、膝関節Haより上の部分(以下、上体という)Uは外力Fx,Fyによってその力学的な影響が反映されているものと考える。
【0138】
この場合の外力は、上体Uに働く外力をも含めて扱うことができる。なお、この場合、異なるのは膝関節Haに働くトルクの影響である。これを外部からのモーメントとして扱えば、前記定理3を適用することができる。
【0139】
人間型の多リンク系に応用した場合、お辞儀をするときのように、安定性を確保したまま、導体、腕、頭の位置をゆっくり変化させることができる。
足関節のPDF制御は、足関節Oの傾き角度θ及びその角速度と床反力のフィードバックのみで構成されている。このため、多リンク系の制御に用いた場合でも、上体Uの関節角度という情報は一切用いていないため、局所的なフィードバックで独立的にできるのが利点の1つである。
【0140】
そして、定理3の(42)式により、平衡を保てる姿勢の範囲をおおよそ見当づけることができる。
以下に、平衡を保てる姿勢の範囲の説明をする。
【0141】
簡単のため、外力は働かないものとし、脛部Hの質量M=0、リンク長をLとし、上体Uの質量をMB、膝関節Haから上部の重心と膝関節Ha間の距離をLB,重心と膝関節Haを結ぶ直線が鉛直方向からなす角をθBとする。
【0142】
定常状態では、Fx=0,Fy=MBgよりA=MBg,又、τe=MBgsinθBであるから、(42)式より、
L/LB≧sinθB …(46)
となる。
【0143】
姿勢の変化は、重心が上式を満たすθBの範囲内で可能であることから、Lは大きく、LBが小さい方が広い範囲で上体Uの姿勢変化ができることとなる。
図13(c)には(42)式を満たす限界の姿勢の一つを示している。なお、実際には、関節の可動範囲には限界があり、結局上体Uの重心が足関節Oの真上に持ってこられる範囲で姿勢変化が実現できる。
【0144】
(3)前記各実施形態では、図6(b)に示す矢印方向の仮想面内(図3において、前後方向に延びる面内)に限定した運動を前提としていたがこれに限定されるものではない。
【0145】
足関節のPDF制御は、ZMP、すなわち床反力が作用する中心点を足関節Oの位置に制御することと等価である。従って、足底(足部材11の底面)すなわち、支持多角形の各頂点で床反力を検出することによって、これまで図6(b)に示す矢印方向の仮想面内(図3において、前後方向に延びる面内)に限定した運動を、3次元空間へ拡張することも可能である。
【0146】
(4) 前記各実施形態では、PD制御での目標値θd(目標角度)を0としたが目標値θdを0に限定する必要はない。
(5) 前記各実施形態では、外力は一定と仮定してきた。しかし、第1実施形態の実験例でも示したように、ある程度のダイナミクスをもって傾斜面を作ったり、外力を変化しても脚12は倒れずに安定した姿勢制御ができることが確認されている。これは、環境の変化が力フィードバックの収束性よりも遅ければ、その変化に追従できることを意味する。従って、フィードバックゲインKfが大きくとれるほどの変化の速い環境にも適応でき、足関節のPDF制御の有効性が高いものと言える。
【0147】
従って、外力は必ずしも一定である必要はない。
(6) 前記各実施形態では、PD制御を行うことを前提にしていたが、機械10のハード構成として、下記のように構成した場合には、PD制御を省いた姿勢制御装置1としてもよい。
【0148】
この考え方は、PDF制御では、平衡状態の姿勢は重力と外力のみによって決定され、PD制御の目標値θd((12)式参照)には依存していないことから結論ずけられる。
【0149】
このため、適当な姿勢に対する弾性と粘性とが、足関節Oに与えられていればよいことになる。
従って、本変形例では、足関節OのPDF制御のPD制御部分の代わりに、図12に示すように、脚12の下部と足部材11間に、スプリング手段としてのスプリング25と、ダンパ手段としてのダンパ26とが並設されている。そして、スプリング25とダンパ26とにより、脚12が足部材11と連係されており、脚12の姿勢に対して弾性と粘性とが付与されている。なお、ダンパ26は、脚12に対して、その回動を干渉しないようにその一端が回動自在に取付られている。
【0150】
このようにした場合、一過性の外乱等に対してはとりあえず、スプリング25とダンパ26とにより対処して平衡を保ち、その結果として重さのかけ方に不均等が生じた際に力フィードバックで解消する。
【0151】
この場合、姿勢制御装置1の制御フローチャートにおいては、S30が省略されるとともに、S10,S20,S40~S60の処理を行うが、下記で説明するところが異なっている。
【0152】
S10では、各ロードセル16a~16dの検出値のみを読み込み、S50においては、τ=Kf・τfの演算を行う。
この結果、未知の状況下において、外力が付与された際、その外力に応じて姿勢を変化させることができ、安定した平衡状態にすることができる。
【0153】
又、PD制御を行うことなく、制御に足関節の傾き角度に関する情報は一切用いないにもかかわらず、未知環境下においても、その外力に応じて姿勢を変化させることができ適正な姿勢制御を実現することができる。
(7) 前記各実施形態では、脚の傾き角度を検出する角度検出手段として、
エンコーダ15を設けたが、この代わりに脚12の傾き角度の角速度を検出する角速度検出手段として回転速度センサをモータ13に設けてもよい。回転速度センサとしてはタコジェネレータが使用される。この場合、CPU19は、回転速度センサから入力した角速度を、RAM21に格納し、さらに角速度を積分して得た角度θをRAM21に格納するものとする。
【0154】
【発明の効果】
以上詳述したように、請求項1乃至請求項9の発明によれば、未知環境下においても、その外力に応じて姿勢を変化させることができ適正な姿勢制御を実現することができる効果を奏する。
【0155】
請求項1乃至請求項7の発明によれば、外力に応じて姿勢を変化させ、外力と重力が足関節周りにつくるモーメントの釣り合う姿勢を平衡状態とすることができ、又、その平衡状態の局所的な安定性を保証できる効果がある。
【0156】
請求項8及び請求項9の発明によれば、さらに、制御入力のために足関節の傾き角度の検出を行うことなく、未知環境下においても、その外力に応じて姿勢を変化させることができ適正な姿勢制御を実現することができる。
【図面の簡単な説明】
【図1】姿勢制御装置及び脚を有した機械の説明図。
【図2】姿勢制御装置1の電気ブロック図。
【図3】機械の要部側面図。
【図4】機械の平面図。
【図5】コンピュータ18が実行する制御フローチャート図。
【図6】(a)はロボットの概念図、(b)は2つのリンク系の説明図。
【図7】胴部に働く重力と外力及び合力の説明図。
【図8】(a)は傾斜面上の機械に作用する外力の説明図、(b)は平面上の機械に作用する外力の説明図。
【図9】は実験例の足関節角度(足関節の傾き角度θ)の時間変化を示すグラフ。
【図10】同じくτ,τθ,Kfτfの時間変化を示すグラフ。
【図11】同じく床反力の時間変化を示すグラフ。
【図12】他の実施形態の機械の要部側面図。
【図13】他の実施形態の(a)はロボットの概念図、(b)は多リンク系の説明図、(c)はリンクの姿勢変化の範囲の説明図。
【符号の説明】
1…姿勢制御装置
10…脚を有する機械
11…足部材(接地部材)
12…脚
13…モータ(駆動手段)
15…エンコーダ(角度検出手段)
16a~16d…ロードセル(床反力検出手段)
18…コンピュータ(制御手段)
19…中央処理装置(CPU)
20…ROM
21…RAM
25…スプリング(スプリング手段)
26…ダンパ(ダンパ手段)
図面
【図1】
0
【図2】
1
【図3】
2
【図4】
3
【図5】
4
【図6】
5
【図7】
6
【図8】
7
【図9】
8
【図10】
9
【図11】
10
【図12】
11
【図13】
12