JP2011073127A - 多関節ロボット - Google Patents

Abstract

【課題】フィードバック制御の異常を判断する多関節ロボットを提供する。
【解決手段】多関節ロボットは、各関節角を検出するセンサと、センサの検出値に基づいて関節をフィードバック制御するコントローラを備えている。コントローラは、予め用意されたロボットの運動方程式に、一つの特定関節を除いて他の関節角にセンサの検出関節角を代入して特定関節の角度を算出し、特定関節の算出関節角と検出関節角の角度差が予め定められた許容範囲から外れている場合に、特定関節のフィードバック制御が異常であると判断する。
【選択図】図３

Description

本発明は、異常を検知する機能を備えた多関節ロボットに関する。

ロボットを具現化するに際して、ロボット自らが異常を検知できる機能を用意しておくことは重要である。例えば、特許文献１には、床反力の推定値と検出値の差に基づいて異常を検知する脚式ロボットが開示されている。

特開２００３−２０５４８４号公報

特許文献１に例示されているように、センサの検知対象の物理量を推定し、推定値と実際のセンサ出力の差異に基づいて異常を検知する手法は従来から知られていた。しかしながら、そのような異常検知手法を多関節ロボットの関節制御の異常検知に適用することは従来考えられていなかった。本発明は、多関節ロボットの関節制御の異常を自ら検知する機能を備えたロボットを提供する。なお、本明細書における「多関節ロボット」とは、複数の関節を備えており、夫々の関節が２つの隣接するリンクを揺動可能に連結しているロボットを意味する。アーム型ロボットや脚式ロボットは多関節ロボットの典型である。

上記した異常検知手法がこれまで多関節ロボットの関節制御に適用されていなかった理由は次の通りである。そのような異常検知手法は、センサの検知対象の物理量を推定する計算式が得られていることが前提である。上記の異常検知手法を多関節ロボットの関節制御に適用しようとする場合、関節角を推定するための基礎となる計算式は、ロボットの運動方程式である。なお、制御の技術分野においては、「運動方程式」の代わりに「状態方程式」が採用されることもあるが、「状態方程式」も運動方程式が基礎となる。多関節ロボットの運動方程式は、一般的に、変数として複数の関節の角加速度、角速度、角度、及び、関節に加わるトルクが混在する非線形方程式となる。従って、そのような非線形方程式から関節角を推定することは困難であると考えられていた。

本願の発明者は、非線形方程式であっても、一つの関節制御で異常が発生しており、他の関節制御では異常が発生していないとの仮定を置くことによって、その一つの関節の角度を推定できることに着目した。複数の関節制御で同時に異常が発生する確率は、一つの関節制御についてのみ異常が発生する確率よりも顕著に低いので、そのような仮定は実用上妥当である。本明細書は、上記の妥当な仮定の下、自ら関節制御の異常を検知する機能を備えた多関節ロボットを開示する。以下、異常検知の対象となる関節を「特定関節」と称する。

本明細書が開示する技術的思想は次の通りである。多関節ロボットの非線形運動方程式に含まれる変数（関節の角速度や角加速度などの物理量）に対して、特定関節以外の関節に関する変数にはセンサ値を代入する。そのような代入によって、特定関節の角度が推定できる。多関節ロボットは、特定関節角の推定角度と、センサによって検出された特定関節の検出関節角の相違が顕著である場合に、異常発生を検知する。

本明細書が開示する新規な多関節ロボットを具体的に説明する。この多関節ロボットは、各関節角を検出するセンサと、センサの検出値に基づいて各関節角をフィードバック制御するコントローラを備える。コントローラは、予め用意された多関節ロボットの運動方程式に、一つの特定関節の角度を除いて他の関節の角度にセンサの検出関節角を代入して特定関節の推定関節角を算出する。さらにコントローラは、推定関節角と、センサによって検出された特定関節の検出関節角の差分と比の一方を異常検知指標として算出する。次いでコントローラは、異常検知指標が予め定められた許容範囲から外れている場合に、特定関節のフィードバック制御の異常として検知する。許容範囲は、想定される異常に応じて予め定められる。なお、運動方程式は、予め特定関節の関節角を他の関節の物理量で表した形式で用意しておく。また、関節群の各々について、その関節の推定関節角を求めるための運動方程式を個別に準備しておくことによって、全ての関節について異常検知が可能となる。

本明細書は、得られた推定関節角を有効に用いて制御を継続する新規な手法も開示する。その手法は次の通りである。コントローラは、特定関節のフィードバック制御の異常を検知した場合、特定関節の検出関節角の代わりに推定関節角を用いて擬似フィードバック制御を行うことが好適である。或いは、コントローラは、特定関節のフィードバック制御の異常を検知した場合、フィードバック制御のための特定関節の目標関節角軌道に代えて予め定められた異常時用関節角軌道を用いて擬似フィードバック制御を実行することが好適である。本明細書では、センサ出力の代わりにセンサ出力の推定値を用いたフィードバック制御を擬似フィードバック制御と称している。この手法は、異常発生の要因がセンサの異常である場合に好適である。センサフィードバック制御ほどの精度は得られないが、異常検知後も継続して多関節ロボットを動作させることができる。なお、擬似フィードバック制御は、異常検知前の制御とは異なるので、異常時制御モードと換言することができる。

異常発生には様々な要因が想定し得る。異常の要因の相違は、前述した異常判定指標の振舞い（指標の挙動）に現れる。そこで、コントローラは、異常判定指標が第１異常判定範囲に存する場合と第２異常判定範囲内に存する場合とで異なる異常時制御モードを実行することが好ましい。或いは、コントローラは、異常判定指標が既定時間以上にわたって許容範囲を外れている場合と、既定時間内に許容範囲内に戻った場合とで異なる異常時制御モードを実行することが好ましい。第１異常判定範囲と第２異常判定範囲も、想定される異常の種類に応じて予め定められる。

上記したように、コントローラは、推定関節角と検出関節角の差分と比の一方を異常判定指標として採用すればよい。推定関節角と検出関節角の比を異常判定指標として採用することによって、さらなる利点が得られる。推定関節角と検出関節角の比で表される異常判定指標の経時的変化は、異常発生の要因によって顕著に異なる。即ち、異常判定指標の経時的変化の特徴によって、異常発生の要因を推定し得る。発明者は、時系列データの特徴は、そのデータを複素フーリエ変換して得られる複素フーリエ係数に顕著に現れるという知見を、異常発生の要因推定に適用することを案出した。

本明細書は、異常発生の検知とともに、異常の要因を自ら推定することのできる新規な多関節ロボットも提供する。その多関節ロボットのコントローラは、特定関節の推定関節角と検出関節角の比で表される異常判定指標を予め定められた時間に亘って算出する。ここで、異常判定指標は、特定関節の検出関節角を推定関節角で除した値、即ち、検出関節角／推定関節角で表される。次いでコントローラ、異常判定指標の時系列データを複素フーリエ変換して複素フーリエ係数セットを算出する。本明細書では、複素フーリエ変換によって得られる一組の複素フーリエ係数を複素フーリエ係数セットと称する。コントローラは、複素フーリエ係数セットの直流項係数Ｃｄｃと交流項係数Ｃａｃの値に基づいて、異常発生の推定要因を示す信号を出力する。コントローラの信号によって異常の推定要因が直ちに判明するので、速やかな対処が可能となる。例えば、推定要因に基づいてロボットの修復を速やかに開始することがきる。

発明者は、複素フーリエ係数セットの振舞いと異常要因の具体的な関係を幾つか見出した。その関係を以下に説明する。
（１）直流項係数Ｃｄｃ＝１、交流項係数Ｃａｃ＝０の場合は、異常が発生していない。この場合、コントローラは、異常非発生を示す信号を出力するとよい。なおこの場合のＣｄｃとＣａｃの値は、推定関節角と検出関節角が一致している場合に相当する。
（２）０＜直流項係数Ｃｄｃ＜１、交流項係数Ｃａｃ≠０の場合は、電気系統の接触不良が発生している可能性が高い。そのような場合、コントローラは、電計系統の接触不良発生を示す信号を出力するとよい。
（３）０＜直流項係数Ｃｄｃ＜１、交流項係数Ｃａｃ＝０の場合は、電気系統の接触抵抗が大きい可能性が高い。そのような場合、コントローラは、電気系統の接触抵抗が大きくなったことが推定される旨を示す信号を出力するとよい。
（４）直流項係数Ｃｄｃ＝０、交流項係数Ｃａｃ＝０の場合は、電気系統の切断が発生している可能性が高い。そのような場合、コントローラは、電気系統の切断発生を示す信号を出力するとよい。

本明細書は、関節制御の異常を自ら検知する機能を備えた新規な多関節ロボットの技術を提供する。

第１実施例のロボットの模式的側面図を示す。 シミュレーション結果を示す。 第１実施例の異常検知処理のフローチャートを示す。 異常時の制御モード切替処理のフローチャートを示す。 図４の処理の代替処理のフローチャートを示す。 第２実施例のロボットが実行する異常検知処理のフローチャートを示す。 第２実施例のロボットのブロック図を示す。 センサケーブルまたはコネクタが接触不良の場合の足首関節角度の検出値の経時変化を示す。 センサケーブルおよびコネクタが正常な場合の異常判定指標の経時変化を示す。 コネクタの接触抵抗が大きい場合の異常判定指標の経時変化を示す。 センサケーブルが断線した場合の異常判定指標の経時変化を示す。 センサケーブルおよびコネクタが正常な場合の異常判定指標フーリエ係数の値を示す。 センサケーブルまたはコネクタが接触不良の場合の異常判定指標フーリエ係数の値を示す。 コネクタの接触抵抗が大きい場合の異常判定指標フーリエ係数の値を示す。 センサケーブルが断線した場合の異常判定指標フーリエ係数の値を示す。

図面を参照して第１実施例の多関節ロボット１０を説明する。なお、以下では、多関節ロボット１０を単にロボット１０と称する。本実施例では、足首関節のフィードバック制御の異常を自ら検知することのできるロボットを説明する。即ち、この実施例における足首関節が前述の「特定関節」に相当する。

図１に、ロボット１０の模式的側面図を示す。ロボット１０は、２脚を有する脚式ロボットである。ロボットの脚２０は、人間の脚を模して作られており、大腿リンク１４、下腿リンク１６、及び足リンク１８で構成されている。なお、図１では、一方の脚２０にのみ符号を付しているが、他方の脚も脚２０と同じ構造を有している。以下では、一方の脚について説明する。

大腿リンク１４の一端が、股関節２２によってボディ１２に連結されている。下腿リンク１６の一端が、膝関節２４によって大腿リンク１４の他端に連結されている。足リンク１８の一端が、足首関節２６によって下腿リンク１６の他端に連結されている。即ち、脚２０は、複数の関節２２、２４、２６を有しており、各関節がリンクを揺動可能に直列に接続している多関節構造を有している。ロボット１０は、コントローラ３０を内蔵している。

股関節２２の角度θｈ、膝関節２４の角度θｋ、及び、足首関節２６の角度θａを図１の通り定義する。なお、破線Ｌ１は、ボディ１２の中心を通りその長手方向に沿って伸びる直線である。破線Ｌ２は、大腿リンク１４の中心を通りその長手方向に沿って伸びる直線である。破線Ｌ３は、下腿リンク１６の中心を通りその長手方向に沿って伸びる直線である。破線Ｌ４は、足リンク１８の中心を通りその長手方向に沿って伸びる直線である。これらの破線Ｌ１、Ｌ２、Ｌ３、及び、Ｌ４は、関節角度の向きを説明するための仮想的な線である。

図示を省略しているが、各関節にはモータが内蔵されており、関節を駆動することができる。各関節はエンコーダ２８を備えており、関節角を検出することができる。ロボット１０は、所望の関節角軌道に追従するように各関節の角度をフィードバック制御することによって歩行することができる。エンコーダ２８は、各関節の角度を検出するセンサに相当する。

ロボット１０の運動方程式について説明する。運動方程式は、関節毎に立てることができる。即ち、脚２０について、股関節２２に関する運動方程式、膝関節２４に関する運動方程式、及び、足首関節２６に関する運動法方程式が立てられる。運動方程式の各々は、関節の角度、角速度、角加速度、及び、関節に加えられる制御トルクを変数として含む非線形方程式となる。３つの非線形運動方程式から股関節２２に加えられる制御トルクと足首関節２６に加えられる制御トルクを表す変数を消去するとともに、膝関節角θｋの２回微分のみが現れるように変形すると、次の運動方程式（数１）が得られる。

（数１）で用いた記号の意味は次の通りである。
θｈ：股関節角（大腿リンク角度）
θｋ：膝関節角（下腿リンク角度）
θａ：足首関節角（足リンク角度）
ｍｎ：下腿リンク質量
Ｊｎ：下腿リンクの慣性モーメント
ｌｎ：下腿リンク長
ｍｆ：足リンク質量
Ｊｆ：足リンクの慣性モーメント
ｌｆ：足リンク長
Ｔｋ：膝関節に加えられる制御トルク（図１において反時計回り方向を正としている）
ｇ：重力加速度

（数１）を変形して整理すると次の（数２）を得る。

さらに（数２）を足首関節の角度θａについて解くと次の（数３）を得る。

（数３）においてφは、位相である。数３におけるａ１、ａ２、及びａ３は、（数２）で示したように、質量など既知の機械定数と、膝関節２４の関節角θｋとその角加速度、股関節２２の関節角θｈ、及び、膝関節２４へ加えられる制御トルクＴｋの関数である。膝関節２４の関節角θｋとその角加速度、股関節２２の関節角θｈは、エンコーダ２８から得られる。なお、角加速度は、エンコーダの検出値を２回時間差分して得られる。制御トルクＴｋも得ることができる。具体的には、制御トルクは、関節のサーボ系内で算出されるモータへの指令電流の値にトルク定数を乗じることによって得られる。サーボ系において制御トルクを知ることができることは良く知られているので詳しい説明は省略する。

なお、脚２０が接地している場合は、上記数式で用いた制御トルクＴｋを次の（数４）で表されるトルクＴｋｌに変更する。これは、接地の影響を脚の運動方程式に反映するためである。

（数４）において、Ｍはボディ１２の質量を表す。また、脚２０が立脚であることの判定は、股関節角度θｈに基づいて行うことができる。

次に、（数３）を用いた関節角推定の効果を確かめるシミュレーションを説明する。シミュレーションに用いた数値は以下のとおりである。
Ｍ＝３５[ｋｇ]
ｍｎ＝３[ｋｇ]
ｍｆ＝１[ｋｇ]
Ｊｎ＝４００×１０＾−４[ｋｇ・ｍ＾２]
Ｊｆ＝３０×１０＾−４[ｋｇ・ｍ＾２]
ｌｎ＝０．４[ｍ]
ｌｆ＝０．２[ｍ]
ｇ＝９．８[ｍ／ｓ＾２]

本シミュレーションに用いた数値は、R.F. Chandler, C.E. Clauser, J.T. McConville, H.M. Reynolds and J.W. Young, "Investigation of Inertial Properties of the Human Body", U.S. Department of Transportation, March 1975に紹介されている実測値を採用した。

シミュレーションでは、図１に示すロボット１０の足首関節角θａがセンサの異常または断線により検出不能であり、膝関節角θｋは検出可能であるケースを想定した。そのようなケースにおいて、（数３）を用いて足首関節角θａを推定した結果を図２に示す。

図２の破線は、股関節角θｈ（Hip angle）、膝関節角θｋ（Knee angle）、足首関節角θａ（Ankle angle）のセンサ検出値を示している。図３の実線は、足首関節角θａの推定値を示している。足首関節角の推定値のグラフはセンサ検出値のグラフと完全に重なっている。このシミュレーションから、足首関節角を、他の関節角（股関節角θｈと膝関節角θｋ）の検出値を用いて高精度に推定することができることがわかる。

次に、ロボット１０のコントローラ３０が実行する制御を図３と図４を参照して説明する。図３は、異常検知までの制御フローチャートを示している。図４は、異常の推定要因に基づいて制御モードを切り替える処理（異常時処理）のフローチャートである。なお、ロボット１０の動作中、コントローラ３０は、既定の目標関節角軌道に追従するように各関節をフィードバック制御する。具体的には、コントローラ３０、目標関節角軌道に記述された各制御周期の目標関節角とエンコーダ２８によって検出された検出関節角の差を小さくする制御トルクを各関節のモータに指令する。ロボットのフィードバック制御自体は良く知られているので詳しい説明は省略する。

図３のフローチャートに沿って説明する。コントローラ３０は、エンコーダ２８から各関節の角度を取得する（Ｓ２）。コントローラ３０は、検出した関節角を（数３）に代入して足首関節の関節角を推定する（Ｓ４）。以下、ステップＳ４で推定された関節角を推定関節角θａｅと称する。また、足首関節が備えるエンコーダ２８によって検出された関節角を検出関節角θａと称する。なお、前述したように、コントローラ３０は、膝関節２４へ加える制御トルクＴｋも計算によって求めることができ、その値を（数３）へ代入する。

コントローラ３０は、「異常判定指標」＝θａｅ−θａを算出する（Ｓ６）。異常判定指標は、足首関節の推定関節角θａｅと検出関節角θａの差分である。異常判定指標は、足首関節の検出関節角θａが推定関節角θａｅからどの位離れているかを示す指標である。コントローラ３０は、異常判定指標が予め定められた許容範囲内であるか否かを判定する（Ｓ８）。許容範囲は、運動方程式の正確さと想定される異常の種類に応じて事前に定められる。なお、異常が発生していない場合、異常判定指標は概ねゼロに等しい。

ステップＳ８の分岐がＹＥＳの場合、異常が発生していないと推定されるので、図３の処理を終了する。ステップＳ８の分岐がＮＯの場合、即ち、異常判定指標が許容範囲外の場合は、異常が発生していることを示す。この場合、コントローラ３０は、足首関節角の検出値θａに代えてステップＳ４で算出した推定関節角θａｍを使ってフィードバック制御を継続する（Ｓ１０）。ステップＳ１０の処理によって実行されるフィードバック制御が、擬似フィードバック制御に相当する。ここで、「擬似」は、センサによって検出される検出関節角の代わりに推定された関節角を用いることを意味する。コントローラ３０は、次に説明する異常時処理を起動するための信号を出力する（Ｓ１２）。この信号は、異常発生を通知する信号であり、以下では異常発生信号と称する。

図４に示した異常時処理を説明する。コントローラ３０には、予め第１異常判定範囲と第２異常判定範囲が記憶されている。これらの値は実験等によって予め決められている。ここでは、第２異常判定範囲は第１異常判定範囲の外側に設定されている。即ち、異常判定指標が第２異常判定範囲内のときは、より深刻な異常が発生していると推定される。

異常判定指標が第１異常判定範囲内の場合（Ｓ２０：ＹＥＳ）、コントローラ３０は、それまで用いていた脚の各関節の目標関節角軌道を、足リンクを最短距離で接地させる異常時用の特別な第１目標関節角軌道に変更して擬似フィードバック制御を継続する（Ｓ２２）。

異常判定指標が第１異常判定範囲外の場合（Ｓ２０：ＮＯ）、コントローラ３０は、異常判定指標が第２異常判定範囲内であるか否かを判定する（Ｓ２４）。異常判定指標が第２異常判定範囲内の場合（Ｓ２４：ＹＥＳ）、コントローラ３０は、それまで用いていた脚の各関節の目標関節角軌道を、足リンクを最速最短で接地させる異常時用の特別な第２目標関節角軌道に変更して擬似フィードバック制御を継続する（Ｓ２６）。

異常判定指標が第２異常判定範囲外の場合（Ｓ２４：ＮＯ）、コントローラ３０は、ロボット１０を緊急停止する（Ｓ２８）。

上記の分岐判断とその後の処理は、総じて言えば、制御モードの切換に相当する。ステップＳ２４の判断結果がＮＯの場合が最も深刻な異常発生が推定された場合に相当する。ステップＳ２４の判断結果がＹＥＳの場合は、中程度の異常発生が推定された場合に相当する。ステップＳ２０の判断結果がＹＥＳの場合は、軽微な異常発生が推定された場合に相当する。なお、ステップＳ２０の判断結果がＮＯの場合は、異常が発生していないことを示す。このロボット１０は、異常発生の推定要因に応じて、制御モードを切り替えることができる。なお、ステップＳ２２とＳ２６の処理の開始時に既に足リンクが着地している場合は、コントローラ３０はロボット１０の制御を直ちに停止する。

図４のフローチャートに示した異常時の制御モード切替処理の代替処理を図５に示す。図５の処理を採用する場合、コントローラ３０は、図３のステップＳ１２の処理と同時に、異常発生からの経過時間を計測するためのタイマを起動する。

コントローラ３０は、異常発生からの経過時間が予め決められた時間内か否かを判定する（Ｓ３０）。経過時間が既定時間内の場合（Ｓ３０：ＹＥＳ）、コントローラ３０は、それまで用いていた通常の目標関節角軌道に代えて、足リンクを最短距離で接地させることができる異常時用の第１目標関節角軌道に変更して擬似フィードバック制御を継続する（Ｓ３４）。経過時間が既定時間を超していた場合（Ｓ３０：ＮＯ）、コントローラ３０は、ロボット１０を緊急停止させる（Ｓ３２）。

上記した第１実施例の留意点について述べる。コントローラ３０は、運動方程式（数１）に基づいて足首関節の推定関節角θａｅを得る。（数１）には、足首関節や股関節に加わるトルクを示す変数や、各関節の角速度を示す変数が明に現れていない。即ち、一見、足首関節や股関節に加わるトルクや各関節の角速度の影響は運動方程式に反映されていないように見える。しかし、各関節の角度は、物理的には、足首関節や股関節に加わるトルクや各関節の角速度の影響を受けて変化するので、それらの影響は、各関節の検出角度に内在している。特定関節（実施例の場合は足首関節）以外の関節の検出角を用いることの利点は、トルクや角速度の影響を内在した検出角を運動方程式に代入することによって、特定関節の角度を推定することにある。このことによって、特定関節の角度を正確に推定することができる。

次に、第２実施例を説明する。この実施例のロボットのメカニカルな構造は図１に示した第１実施例のロボットと同じであるので説明は省略する。また、本実施例のロボットは、第１実施例のロボットと同様に、各関節をフィードバック制御することによって歩行することができる。なお、第２実施例のロボットのコントローラをコントローラ１３０と表す。

図６に、本実施例のロボットが実行する異常発生検知の処理のフローチャートを示す。コントローラ１３０は、エンコーダ２８から各関節の角度を取得する（Ｓ４０）。コントローラ１３０は、検出した関節角を（数３）に代入して足首関節の関節角を推定する（Ｓ４２）。ここまでの処理は、第１実施例のロボットが実行するステップＳ２とＳ４と同じである。なお、第１実施例と同様に、ステップＳ４２で推定された関節角を推定関節角θａｍと称し、エンコーダ２８によって検出された足首関節の関節角を検出関節角θａと称する。

コントローラ１３０は、異常判定指標＝θａ／θａｅを算出する（Ｓ４４）。本実施例の異常判定指標は、足首関節の推定関節角θａｅに対する検出関節角θａの比θａ／θａｅで表される。なお、本実施例においても、足首関節が「特定関節」に相当する。コントローラ１３０は、予め定められた時間に亘って異常判定指標の算出を継続する（Ｓ４６）。即ち、コントローラ１３０は、既定時間に亘る異常判定指標の時系列データを取得する。

次にコントローラ１３０は、取得した異常判定指標の時系列データを複素フーリエ変換し、複素フーリエ係数を得る（Ｓ４８）。次いでコントローラ１３０は、複素フーリエ係数の値に応じて、異常発生の有無を通知する信号、或いは、異常の推定要因を通知する信号（異常通知信号）を出力する（Ｓ５０）。コントローラ１３０が出力する異常通知信号について以下、説明する。

ステップＳ４８で得られる複素フーリエ係数には、周波数ゼロの項の係数、とゼロ以外の周波数の項の係数が存在する。周波数ゼロの項の係数を直流項係数Ｃｄｃと称し、ゼロ以外の周波数の項の係数を交流項係数Ｃａｃと称する。

コントローラ１３０は、直流項係数Ｃｄｃと交流項係数Ｃａｃの値に応じて、異常発生の要因を推定し、推定結果を出力する。具体的には、コントローラ１３０は、次の通り異常通知信号を出力する。
（１）直流項係数Ｃｄｃ＝１、交流項係数Ｃａｃ＝０の場合、異常非発生を示す信号を出力する。
（２）０＜直流項係数Ｃｄｃ＜１、交流項係数Ｃａｃ≠０の場合、電気系統の接触不良発生を示す信号を出力する。
（３）０＜直流項係数Ｃｄｃ＜１、交流項係数Ｃａｃ＝０の場合、電気系統の接触抵抗が高くなったことを示す信号を出力する。
（４）直流項係数Ｃｄｃ＝０、交流項係数Ｃａｃ＝０の場合、電気系統の切断発生を示す信号を出力する。

第２実施例のロボットのコントローラの詳細な構造と、上記（１）〜（４）で示した複素フーリエ係数と推定異常要因との関係について説明する。図７に、本実施例のロボット１００のブロック図を示す。なお、ロボット機構１０３は、図１に示した第１実施例の多関節ロボット（脚式ロボット）１０と同じ構造を有する。即ち、ロボット機構１０３は、複数のリンクが関節によって直列に連結された多リンク機構を有しており、アクチュエータ（モータ）によって各リンクが揺動する。状態検出器１０５は、第１実施例のエンコーダ２８に相当する。状態検出器１０５は、各関節の角度などを検出し状態検出値として出力する。

ロボット１００のコントローラ１３０は、指令発生器１０１と、制御器１０２と、異常検知モジュール１０６を備える。指令発生器１０１は、ロボット機構１０３の所望の動作を示す指令を発生する。ここで、「指令」とは、前述した第１実施例のロボット１０と同様に、歩行動作を実現するための各関節目標角の軌道に相当する。制御器１０２は、指令発生器１０１が出力する指令と、状態検出器が出力する状態検出値と、制御調整信号に基づいてロボット機構１０３を制御する。ここでいう制御は、前述した第１実施例のロボット１０と同様に、各関節目標角の軌道に追従するように状態検出値（検出関節角）に基づいて各関節角をフィードバック制御することを意味する。異常検知モジュール１０６は、状態検出器１０５の一部が故障した場合に状態検出値に基づいてロボット機構１０３を安全に動作させる制御調整信号を演算して出力する。

異常検知モジュール１０６は、機械定数記憶器１０７、推定状態方程式係数演算器１０８、推定状態方程式位相演算器１０９、推定状態演算器１１０、状態検出値推定値比演算器１１１、故障判別器１１２、及び、制御調整器１１３を備える。

機械定数記憶器１０７は、ロボット機構１０３の機械定数を記憶し機械定数記憶値として出力する。推定状態方程式係数演算器１０８は、状態検出値と機械状態記憶値に基づいて、検出できない状態の方程式係数である推定状態方程式係数を演算し、推定状態方程式係数演算値として出力する。推定状態方程式位相演算器１０９は、推定状態方程式係数演算値に基づいて検出できない状態の方程式位相である推定状態方程式位相を演算し、推定状態方程式位相演算値として出力する。推定状態演算器１１０は、推定状態方程式係数演算値と推定状態方程式位相演算値に基づいて、検出できない状態の推定値である推定状態演算値を算出する。状態検出値推定値比演算器１１１は、状態検出値と推定状態演算値を入力し、その入力信号比である状態検出値推定値比を出力する。故障判別器１１２は、状態検出値推定値比の複素フーリエ係数に基づいて状態検出器１０５の故障種類（接触不良、汚れ、断線など）を判別し、故障判別値として出力する。制御調整器１１３は、故障判別値に基づいて制御調整信号を出力する。

制御器１０２は、ロボット機構１０３の状態の一部が検出器の故障などによって検出不能となった場合にも、推定状態演算値に基づいてロボット機構１０３を安全に動作させることができる。

ロボット機構１０３は、図１の通りモデル化できる。本実施例においても、第１実施例と同様に、ロボット１００は、足首関節の検出器（エンコーダ）が故障しており、足首関節の角度θａが検出できないと仮定する。なお、状態検出器１０５は、股関節の角度θｈと、膝関節の角度θｋは検出することができるとする。即ち、第２実施例のロボットも、前述した（数１）〜（数４）を適用することができ、足首関節の推定関節角θａｅを得ることができる。

以下、本実施例に係る異常検知モジュール１０６の仕組みを説明する。ここでは、異常検知モジュール１０６が、状態検出器１０５の一部が故障した場合にロボット機構１０３の動作を安全に継続させるケースを説明する。

推定状態方程式位相演算器１０９は、（数３）により、推定状態方程式位相φを演算する。推定状態演算器１１０は、（数３）により推定状態演算値θａを演算する。

次に、状態検出値推定値比演算器１１１は（数３）に基づいて算出した推定状態演算値θａｅ（足首関節角θａｅ）と、故障した状態検出器１０５から出力される状態検出値θａ（足首関節の推定関節角θａ）とを用いて異常判定指標θａ／θａｅを算出する。

状態検出器１０５のセンサケーブルまたはコネクタが接触不良の場合には、状態検出値θａｅが図８（ａ）のような波形になる。一方、推定状態演算値θａｅは図８（ｂ）となり、異常判定指標θａ／θａｅは図８（ｃ）に示すような０と１の値をとる矩形波となる。なお、図では、「異常判定指標」という表現に代えて「状態検出値推定値比（Ratio of measurement joint angle to estimated joint angle）」という表現を用いた。「状態検出値推定値比（Ratio of measurement joint angle to estimated joint angle）」は「異常判定指標」と同じ意味である。同様に、センサケーブルおよびコネクタが正常な場合、異常判定指標θａ／θａｅは図９のように１となり、コネクタの汚れなどにより接触抵抗が大きくなった場合は図１０に示すように０より大きく１より小さい値をとる、断線した場合は図１１に示すように０となる。

故障判別器１１２は、異常判定指標θａ／θａｅの複素フーリエ係数である異常判定指標フーリエ係数を複素平面上に示した図に基づいて故障種類を判別する。図１２は、センサケーブルおよびコネクタが正常な場合の異常判定指標フーリエ係数を示す図である。センサケーブルおよびコネクタが正常な場合、常に異常判定指標θａ／θａｅは１となるので、異常判定指標フーリエ係数は直流項係数Ｃｄｃ（図中の「ＤＣ ｔｅｒｍ」）の係数が１、交流項係数Ｃａｃ（ｃｏｓ、ｓｉｎの項、図中のＡＣ ｔｅｒｍ）の係数が全て０である。センサケーブルまたはコネクタが接触不良の場合は、図１３に示すように、直流項の係数Ｃｄｃは図８（ｃ）の平均値なので０と１の間の値をとり、交流項は無限に存在するので交流項の係数Ｃａｃは０でない値をとる。

コネクタの接触抵抗が大きい場合は、図１４に示すように、状態検出値を示す電圧が正常値より下がるため、直流項の係数Ｃｄｃは０と１の間の値をとり、交流項の係数Ｃａｃはほぼ０となる。センサケーブルが断線した場合は、図１５に示すように、状態検出値を示す電圧が０となるため、直流項の係数Ｃｄｃおよび交流項の係数Ｃａｃは全て０となる。故障判別器１１２は、以上の特徴を用いて故障の種類を判別し故障判別値として出力する。

制御調整器１１３は、故障判別値に基づいて以下の制御調整信号を制御器１０２に出力する。
（ａ）センサケーブルまたはコネクタが接触不良の場合：
この場合は、（数３）の推定状態演算値を状態検出値に代えて、動作を継続し歩行を終了させる。
（ｂ）コネクタの接触抵抗が大きい場合：
この場合は、状態検出値に異常判定指標（θａ／θａｅ）の逆数を乗算して動作を継続し歩行を終了させる。
（ｃ）センサケーブルが断線した場合：
この場合は、状態検出値の異常が認められた関節である足首関節のモータに対して、半時計回り方向に回転させる指令を生成し、つま先（足リンクの先端）が最も上がった状態にし、歩行を終了させる。

このような構成を備える本実施例によれば、次の効果を奏することができる。
（１）センサケーブルまたはコネクタが接触不良の場合、汚れて接触抵抗が大きくなった場合、断線した場合にも、ロボットを安全に停止させることができる。
（２）ロボット機構１０３が動作していない状態で検出が困難な接触不良を、ロボット機構１０３を動作させながら検出することができる。
（３）コネクタなどの掃除時期を知らせるロボット機構１０３のメンテナンス機能として利用できる。

上記した２つの実施例について留意点を述べる。実施例では、３つの関節（股関節２２、膝関節２４、及び、足首関節２６）は全てロボットのピッチ軸方向の回転軸を有している。本発明は、ピッチ軸関節に限らず、どのようなどのような向きの関節を有するロボットにも適用可能である。また、実施例のロボットは脚式ロボットである。本発明はアーム型ロボットにも適用可能である。さらに、本発明は、ユーザが装着してユーザの動作を補助するいわゆるロボットスーツタイプの多関節ロボットに適用することも好適である。ロボットスーツの一例は、ユーザの脚に装着し、ユーザの脚にトルクを加えることによってユーザの歩行動作を補助する。或いは本発明は、ユーザの腕にトルクを加えることによってユーザの腕動作を補助する多関節ロボット型装具に適用することも好適である。

以上、本発明の具体例を詳細に説明したが、これらは例示に過ぎず、特許請求の範囲を限定するものではない。特許請求の範囲に記載の技術には、以上に例示した具体例を様々に変形、変更したものが含まれる。

本明細書または図面に説明した技術要素は、単独であるいは各種の組合せによって技術的有用性を発揮するものであり、出願時請求項記載の組合せに限定されるものではない。また、本明細書または図面に例示した技術は複数目的を同時に達成し得るものであり、そのうちの一つの目的を達成すること自体で技術的有用性を持つものである。

１０：多関節ロボット
１２：ボディ
１４：大腿リンク
１６：下腿リンク
１８：足リンク
２０：脚
２２、２４、２６：関節
２８：エンコーダ
３０、１３０：コントローラ
１００：ロボット
１０１：指令発生器
１０２：制御器
１０３：ロボット機構
１０５：状態検出器
１０６：異常検知モジュール
１０７：機械定数記憶器
１０８：推定状態方程式係数演算器
１０９：推定状態方程式位相演算器
１１０：推定状態演算器
１１１：状態検出値推定値比演算器
１１２：故障判別器
１１３：制御調整器

Claims (10)

1. 複数の関節を備えており、夫々の関節がリンクを揺動可能に連結している多関節ロボットであり、
   各関節角を検出するセンサと、
   センサの検出値に基づいて各関節角をフィードバック制御するコントローラと、
   を備えており、コントローラは、
   予め用意された多関節ロボットの運動方程式に、一つの特定関節の角度を除いて他の関節角にセンサの検出関節角を代入して特定関節の推定関節角を算出し、
   推定関節角と、センサによって検出された特定関節の検出関節角の差分と比の一方を異常判定指標として算出し、異常判定指標が予め定められた許容範囲から外れている場合に、特定関節のフィードバック制御の異常として検知することを特徴とする多関節ロボット。
2. コントローラは、特定関節のフィードバック制御の異常を検知した場合、特定関節の検出関節角の代わりに推定関節角を用いて擬似フィードバック制御を行うことを特徴とする請求項１に記載の多関節ロボット。
3. コントローラは、特定関節のフィードバック制御の異常を検知した場合、フィードバック制御のための特定関節の目標関節角軌道に代えて予め定められた異常時用関節角軌道を用いて擬似フィードバック制御を実行することを特徴とする請求項２に記載の多関節ロボット。
4. コントローラは、異常判定指標が第１異常判定範囲に存する場合と第２異常判定範囲内に存する場合とで異なる異常時制御モードを実行することを特徴とする請求項１から３のいずれか１項に記載の多関節ロボット。
5. コントローラは、異常判定指標が既定時間以上にわたって許容範囲を外れている場合と、既定時間内に許容範囲内に戻った場合とで異なる異常時制御モードを実行することを特徴とする請求項１から４のいずれか１項に記載の多関節ロボット。
6. コントローラは、
   特定関節の推定関節角と検出関節角の比で表される異常判定指標を予め定められた時間に亘って算出し、
   異常判定指標の時系列データを複素フーリエ変換して複素フーリエ係数セットを算出し、
   複素フーリエ係数セットの直流項係数Ｃｄｃと交流項係数Ｃａｃの値に基づいて、異常の推定要因を示す信号を出力することを特徴とする請求項１から５のいずれか１項に記載の多関節ロボット。
7. 直流項係数Ｃｄｃ＝１、交流項係数Ｃａｃ＝０の場合に、異常非発生を示す信号を出力することを特徴とする請求項６に記載の多関節ロボット。
8. ０＜直流項係数Ｃｄｃ＜１、交流項係数Ｃａｃ≠０の場合に、電気系統の接触不良発生を示す信号を出力することを特徴とする請求項６に記載の多関節ロボット。
9. ０＜直流項係数Ｃｄｃ＝１、交流項係数Ｃａｃ＝０の場合に、電気系統の接触抵抗が高くなったことを示す信号を出力することを特徴とする請求項６に記載の多関節ロボット。
10. 直流項係数Ｃｄｃ＝０、交流項係数Ｃａｃ＝０の場合に、電気系統の切断発生を示す信号を出力することを特徴とする請求項６に記載の多関節ロボット。

Images