JP2012223851A - ロボット制御装置、その制御方法及び制御プログラム - Google Patents

Abstract

【課題】ロボットのセンサに故障が生じた場合でも最適に制御を行うこと。
【解決手段】ロボット制御装置は、先端側の第１関節から第ｉ関節（ｉ≧２）までの関節角度を含む関数に基づいて、第ｉ関節から先端部分までの第ｉ関節周りの慣性モーメントを算出する慣性モーメント演算手段と、第ｉ関節の関節角度を含む関数に基づいて、第ｉ関節の回転軸が鉛直方向となるときの第ｉ関節周りの慣性モーメント推定値を算出する慣性モーメント推定手段と、第ｉ関節周りの慣性モーメントと第ｉ関節周りの慣性モーメント推定値と、の差を第ｉ関節周りの慣性モーメント誤差として算出する推定誤差演算手段と、第ｉ関節周りの慣性モーメント誤差の絶対値と判定閾値とを比較して、関節角度検出手段の故障を判定する故障判定手段と、故障の判定結果と、ロボットの作業パターンと、に基づいて、関節角度指令を生成する関節角度指令生成手段と、を備える。
【選択図】図１

Description

本発明は、ロボットのセンサに故障が生じた場合でも最適に制御を行うことができるロボット制御装置、その制御方法及び制御プログラムに関するものである。

近年、複数のセンサを用いて車両や航空機などの自動制御を行い、センサなどに故障が生じてもその制御を継続できるように構成された自動制御システムの研究開発が行われている。例えば、フィードバック線形化及び状態推定器を用いて、航空機の故障した構成部材を識別し、故障した構成部材を使用することなく、動作を継続させる自動制御システムが知られている（特許文献１参照）。

特表平１１−５０７４５４号公報

上記特許文献１に示す自動制御システムをロボットに適用した場合、例えば、ロボットに設けられたセンサに故障が生じ、正常なセンサが既知でない場合、フィードバック線形化及び状態推定の両方が実施不能となる虞がある。また、上記自動制御システムは、航空機を制御するため、多数のセンサを冗長的に備えている。したがって、ロボットにそのセンサ構成を適用すると、質量及びサイズが大きくなるという問題も生じる。

本発明は、このような問題点を解決するためになされたものであり、ロボットのセンサに故障が生じた場合でも最適に制御を行うことができるロボット制御装置、その制御方法及び制御プログラムを提供することを主たる目的とする。

上記目的を達成するための本発明の一態様は、複数のリンクと、該各リンクを回転可能に連結する複数の関節と、該各関節を駆動する複数の駆動手段と、前記各関節角度を検出する複数の関節角度検出手段と、を備えたロボット制御装置であって、前記各関節角度検出手段により検出された先端側の第１関節から第ｉ関節（ｉ≧２）までの関節角度を含む関数に基づいて、前記第ｉ関節から先端部分までの第ｉ関節周りの慣性モーメントを算出する慣性モーメント演算手段と、前記関節角度検出手段により検出された前記第ｉ関節の関節角度を含む関数に基づいて、前記第ｉ関節の回転軸が鉛直方向となるときの前記第ｉ関節周りの慣性モーメント推定値を算出する慣性モーメント推定手段と、前記慣性モーメント演算手段により算出された前記第ｉ関節周りの慣性モーメントと、前記慣性モーメント推定手段により算出された前記第ｉ関節周りの慣性モーメント推定値と、の差を前記第ｉ関節周りの慣性モーメント誤差として算出する推定誤差演算手段と、前記推定誤差演算手段により算出された前記第ｉ関節周りの慣性モーメント誤差の絶対値と判定閾値とを比較して、前記関節角度検出手段の故障を判定する故障判定手段と、前記故障判定手段により判定された故障の判定結果と、ロボットが実施しようとしている作業パターンと、に基づいて、前記各関節に対する関節角度指令を生成する関節角度指令生成手段と、を備える、ことを特徴とするロボット制御装置である。この一態様によれば、ロボットのセンサに故障が生じた場合でも最適に制御を行うことができる。

この一態様において、前記故障判定手段は、前記推定誤差演算手段により算出された前記第ｉ関節周りの慣性モーメント誤差の絶対値が判定閾値を超えるとき、前記複数の関節角度検出手段の関節角度のうちの一つ関節角度を０度に保持して、前記第ｉ関節周りの慣性モーメント誤差の絶対値と判定閾値との比較を繰返し、前記関節角度検出手段の故障を判定してもよい。これにより、関節角度検出手段を冗長的に設けることなく、各関節の回転軸が鉛直方向を向いた瞬間に、各関節角度検出手段が故障しているか否かを瞬時に判定できる。

この一態様において、前記故障判定手段は、前記複数の関節角度のうちの一つ関節角度を０度に保持して、前記第ｉ関節周りの慣性モーメント誤差の絶対値が判定閾値以下となるとき、前記関節角度を０度に保持した前記関節角度検出手段を故障、それ以外の前記関節角度検出手段を正常、と判定し、前記第ｉ関節周りの慣性モーメント誤差の絶対値が判定閾値を超えるとき、前記関節角度を０度に保持した前記関節角度検出手段を不明、それ以外の前記関節角度検出手段を故障、と判定し、該故障と判定した関節角度検出手段に対応する関節角度を０度に保持し、前記第ｉ関節周りの慣性モーメント誤差の絶対値と判定閾値との比較を繰り返してもよい。これにより、各関節角度検出手段が故障しているか否か不明の場合でも、その故障を瞬時に判定できる。

この一態様において、前記関節角度指令生成手段は、前記故障判定手段により判定された故障の判定結果と、ロボットが実施しようとしている作業パターンと、に基づいて、前記作業パターンが実施可能か否かを判断し、実施不能と判断した場合、前記作業パターンの作業を中止するための関節角度指令を生成し、実施可能と判断した場合、故障した関節角度検出手段が設置された関節以外に対する関節角度指令を生成してもよい。これにより、故障した関節角度検出手段が存在する場合でも、実施可否を適切に判断し、作業を継続できる。

この一態様において、前記故障判定手段からの判定結果に基づいて、故障した前記関節角度検出手段が無いと判断した場合、全関節における前記関節角度検出手段の関節角度が前記関節角度指令生成手段により生成された関節角度指令に追従するような前記駆動手段に対するトルク指令を生成し、前記故障判定手段からの故障判定結果に基づいて、故障した前記関節角度検出手段が存在すると判断した場合、故障していない前記関節角度検出手段の関節角度が前記関節角度指令生成手段により生成された関節角度指令に追従するように前記駆動手段に対するトルク指令を生成し、かつ、故障した前記関節角度検出手段が設けられた関節の駆動手段に対して前記作業内容毎に予め設定されたトルク指令を生成する、トルク指令生成手段を更に備えていてもよい。これにより、故障した関節角度検出手段が存在する場合でも、トルク指令を適切に生成できる。

この一態様において、ユーザの入力情報又は予め設定されたプログラムに基づいて、ロボットが作業を実施する作業パターンを生成する作業パターン生成手段と、前記作業パターン生成手段により生成された前記作業パターンを分類し、予め設定された前記分類に対応する値を作業分類パラメータに設定するパラメータ生成手段と、前記関節角度指令生成手段からの各関節角度指令と、前記パラメータ生成手段からの前記作業分類パラメータと、前記故障判定手段からの故障の判定結果と、に基づいて、前記駆動手段に対するトルク指令を生成するトルク指令生成手段と、を更に備えていてもよい。

この一態様において、前記トルク指令生成手段は、（ａ）前記故障判定手段からの判定結果に基づいて、前記関節角度検出器が故障していると判断し、前記パラメータ生成手段からの作業分類パラメータに第１所定値が設定されていると判断したとき、所定トルクを生じさせるトルク指令を生成し、（ｂ）前記故障判定手段からの判定結果に基づいて、前記関節角度検出器が故障していると判断し、前記パラメータ生成手段からの作業分類パラメータに第２所定値が設定されていると判断したとき、トルク指令に０を設定し、（ｃ）前記故障判定手段からの判定結果に基づいて、前記関節角度検出器が故障していない、或いは不明と判断したとき、前記作業パターン毎に予め設定されたトルク指令を生成してもよい。これにより、故障した関節角度検出手段を使用せずに安全に作業を継続した場合に、その作業の種類に応じて作業方法の変更、中止などを実施できる。

この一態様において、前記パラメータ生成手段は、前記作業パターン生成手段により生成された作業パターンに基づいて、ロボットが実施しようとしている作業がロボットハンドの指を伸ばすように外側に向かって反力を発生させると判断したとき、第１所定値を前記作業分類パラメータに設定し、ロボットが実施しようとしている作業がロボットハンドの指を外側に向かって反力を発生させないと判断したとき、第２所定値を前記作業分類パラメータに設定してもよい。

この一態様において、前記ロボットは、前記複数のリンクと、該各リンクを回転可能に連結する複数の関節と、を有する指機構と、前記各関節を駆動する複数のモータと、前記各関節角度を検出する複数の関節角度検出部と、を備えるロボットハンドであってもよい。

この一態様において、前記慣性モーメント演算手段は、前記関数として下記（２１）式を用いて前記第ｉ関節周りの慣性モーメントＪ_ｉを算出し、前記慣性モーメント推定手段は、前記関数として下記（２７）式を用いて前記第ｉ関節周りの慣性モーメント推定値Ｊ_ｉ（ハット）を算出し、前記推定誤差演算手段は、下記（２９）式を用いて前記第ｉ関節周りの慣性モーメント誤差ｅ_Ｊ，ｉを算出してもよい。

他方、上記目的を達成するための本発明の一態様は、複数のリンクと、該各リンクを回転可能に連結する複数の関節と、該各関節を駆動する複数の駆動手段と、前記各関節角度を検出する複数の関節角度検出手段と、を備えたロボット制御装置の制御方法であって、前記検出された先端側の第１関節から第ｉ関節（ｉ≧２）までの関節角度を含む関数に基づいて、前記第ｉ関節から先端部分までの第ｉ関節周りの慣性モーメントを算出するステップと、前記検出された前記第ｉ関節の関節角度を含む関数に基づいて、前記第ｉ関節の回転軸が鉛直方向となるときの前記第ｉ関節周りの慣性モーメント推定値を算出するステップと、前記算出された第ｉ関節周りの慣性モーメントと、前記算出された第ｉ関節周りの慣性モーメント推定値と、の差を前記第ｉ関節周りの慣性モーメント誤差として算出するステップと、前記算出された第ｉ関節周りの慣性モーメント誤差の絶対値と判定閾値とを比較して、前記関節角度検出手段の故障を判定するステップと、前記判定された故障の判定結果と、ロボットが実施しようとしている作業パターンと、に基づいて、前記各関節に対する関節角度指令を生成するステップと、を含む、ことを特徴とするロボット制御装置の制御方法であってもよい。

また、上記目的を達成するための本発明の一態様は、複数のリンクと、該各リンクを回転可能に連結する複数の関節と、該各関節を駆動する複数の駆動手段と、前記各関節角度を検出する複数の関節角度検出手段と、を備えたロボット制御装置の制御プログラムであって、前記検出された先端側の第１関節から第ｉ関節（ｉ≧２）までの関節角度を含む関数に基づいて、前記第ｉ関節から先端部分までの第ｉ関節周りの慣性モーメントを算出する処理と、前記検出された前記第ｉ関節の関節角度を含む関数に基づいて、前記第ｉ関節の回転軸が鉛直方向となるときの前記第ｉ関節周りの慣性モーメント推定値を算出する処理と、前記算出された第ｉ関節周りの慣性モーメントと、前記算出された第ｉ関節周りの慣性モーメント推定値と、の差を前記第ｉ関節周りの慣性モーメント誤差として算出する処理と、前記算出された第ｉ関節周りの慣性モーメント誤差の絶対値と判定閾値とを比較して、前記関節角度検出手段の故障を判定する処理と、前記判定された故障の判定結果と、ロボットが実施しようとしている作業パターンと、に基づいて、前記各関節に対する関節角度指令を生成する処理と、をコンピュータに実行させる、ことを特徴とするロボット制御装置の制御プログラムであってもよい。

本発明によれば、ロボットのセンサに故障が生じた場合でも最適に制御を行うことができるロボット制御装置、その制御方法及び制御プログラムを提供することができる。

本発明の実施の形態に係るロボット制御装置の概略的なシステム構成を示すブロック図である。 本発明の実施の形態に係るロボット制御装置の故障判定部によるｉ＝２における関節角度検出器の故障を判定するための処理フローを示すフローチャートである。 第１関節角度検出器及び第２関節角度検出器のうちいずれが故障しているか否かが分かっている場合の関節角度検出器の故障を判定するための処理フローを示すフローチャートである。 （ａ）本発明の実施の形態に係るロボットハンドの指機構を示す摸式図である。（ｂ）本発明の実施の形態に係るロボットハンドの指機構を示す摸式図である。 （ａ）本発明の実施の形態に係るロボット制御装置のシミュレーションにおける第２関節の関節角度の時間変化を示す図である。（ｂ）従来のロボット制御装置のシミュレーションにおける第２関節の関節角度の時間変化を示す図である。 （ａ）本発明の実施の形態に係るロボット制御装置のシミュレーションにおける第１関節の関節角度の０．０１５[ｓ]までの時間変化を示す図である。（ｂ）本発明の実施の形態に係るロボット制御装置のシミュレーションにおける第１関節の関節角度の０．５[ｓ]までの時間変化を示す図である。 （ａ）本発明の実施の形態に係るロボット制御装置のシミュレーションにおける第３関節の関節角度の０．０１５[ｓ]までの時間変化を示す図である。（ｂ）本発明の実施の形態に係るロボット制御装置のシミュレーションにおける第３関節の関節角度の０．５[ｓ]までの時間変化を示す図である。 本発明の実施の形態に係るロボット制御装置のシミュレーションにおける第２関節周り慣性モーメント推定誤差の時間変化を示す図である。 故障した関節角度検出器が設けられている第２関節を制御するための第２関節のモータトルク指令Ｔ_２の時間変化を示す図である。

以下、図面を参照して本発明の実施の形態について説明する。本発明の一実施の形態に係るロボット制御装置１は、例えば、図４に示すようなロボットハンド１３０の指機構１３１を制御するものである。

ロボットハンド１３０の指機構１３１は、図４（ａ）及び（ｂ）に示す如く、第１リンク４０１と、第１リンク４０１に連結された第１関節４０２と、・・・・、第ｉ−２関節４０３と、第ｉ−２関節４０３に連結された第ｉ−１リンク４０４と、第ｉ−１リンク４０４に連結された第ｉ−１関節４０５と、第ｉ−１関節４０５に連結された第ｉリンク４０６と、第ｉリンク４０６に連結された第ｉ関節４０７と、第ｉ−３リンク４０８と、第ｉ−３リンク４０８に連結された第ｉ−３関節４０９と、第ｉ−３関節４０９に連結された第ｉ−２リンク４１０と、を有している。以下、指機構１３１の先端に一番近い第１関節から数えてｉ番目の関節を第ｉ関節とし、同様に、指機構１３１の先端に一番近い第１リンクから数えてｉ番目のリンクを第ｉリンクとする。

第１関節４０２、・・・・、第ｉ−２関節４０３、第ｉ−１関節４０５、第ｉ関節４０７、及び第ｉ−３関節４０９には、駆動手段の一具体例であり、夫々、各関節を駆動するモータ１３２が夫々設けられており、各関節はこの各モータ１３２の発生するモータトルクにより回転駆動される。また、各関節には、関節角度検出手段の一具体例であり、各モータ１３２により駆動された各関節の回転角度を検出し、関節角度検出値として出力する関節角度検出器１３３が夫々設けられている。

ここで、ｉは正の整数であり、例えば、人差し指の指先から第３関節までをモデル化する場合はｉ＝３となる。なお、図４（ａ）及び（ｂ）において各記号を以下のように定義する。

ａ_ｍ、ｎ：第ｍ関節と第ｎ関節を結ぶ線分の長さ（以下、「第ｍ第ｎ関節間距離」と称す）
φ_ｍ、ｎ：第ｍ関節において、第ｍ＋１リンクと第ｍ関節と第ｎ関節を結ぶ線分が成す角度（以下、「第ｍ第ｎ関節間線分第ｍ＋１リンク間角度」と称す）
θ_ｍ：第ｍ関節角度
まず、第ｍ第ｎ関節間距離ａ_ｍ，ｎと、第ｍ第ｎ関節間線分第ｍ＋１リンク間角度φ_ｍ、ｎを導出する。図４（ａ）において、下記（１）式及び（２）式が成立する。

ただし、上記（１）式及び（２）式において、ｌ_ｉは第ｉリンク４０６の長さを表す。
上記（１）式を第ｉ−２第ｉ関節間距離ａ_{ｉ−２、ｉ}について解くと、下記（３）式を得ることができる。

上記（２）式を第ｉ−２第ｉ関節間線分第ｉ−１リンク間角度φ_{ｉ−２、ｉ}について解くと、下記（４）式を得ることができる。

図４（ｂ）において、下記（５）式及び（６）式が成立する。

上記（５）式を第ｉ−３第ｉ関節間距離ａ_{ｉ−３、ｉ}について解くと、下記（７）式を得ることができる。

上記（６）式を第ｉ−３第ｉ関節間線分第ｉ−２リンク間角度φ_{ｉ−３、ｉ}について解くと、下記（８）式を得ることができる。

同様に、ｊ＝３、…、ｉ−１に対して、下記（９）式及び（１０）式が成立する。

上記（９）式を第ｉ−ｊ第ｉ関節間距離ａ_{ｉ−ｊ、ｉ}について解くと、下記（１１）式を得ることができる。

上記（１０）式を第ｉ−ｊ第ｉ関節間線分第ｉ−ｊ＋１リンク間角度φ_{ｉ−ｊ、ｉ}について解くと、下記（１２）式を得ることができる。

第２リンクと第３リンクに対して下記（１３）式及び（１４）式が成立する。

上記（１３）式を第１第ｉ関節間距離ａ_１、ｉについて解くと、下記（１５）式を得ることができる。

上記（１４）式を第１第ｉ関節間線分第２リンク間角度φ_１、ｉについて解くと、下記（１６）式を得ることができる。

上記（３）式、（４）式、（７）式、（８）式、（１１）式、（１２）式、（１５）式、及び（１６）式において算出された第ｍ第ｎ関節間距離ａ_ｍ，ｎと、第ｍ第ｎ関節間線分第ｍ＋１リンク間角度φ_ｍ、ｎと、を用いて、第ｍリンクの重心と第ｎ関節の間の距離（以下、「第ｍリンク重心第ｎ関節間距離」と称す）ｂ_ｍ、ｎを導出する。
図４（ａ）において第ｉリンク重心第ｉ関節間距離ｂ_ｉ、ｉの２乗は、下記（１７）式として導出できる。

図４（ａ）において、第ｉ−１リンク重心第ｉ関節間距離ｂ_{ｉ−１、ｉ}の２乗は、下記（１８）式として導出できる。

ｊ＝２、…、ｉ−１に対して第ｉ−ｊリンク重心第ｉ関節間距離ｂ_{ｉ−ｊ、ｉ}の２乗は、下記（１９）式として導出できる。

上記（１７）式乃至（１９）式に基づいて導出された第ｍリンク重心第ｎ関節間距離の２乗を用いて、ｊ＝１、…、ｉに対する第ｊリンクの第ｉ関節に関する慣性モーメントＪ_ｊ、ｉは、平行軸の定理より下記（２０）式として導出できる。

ただし、上記（２０）式において、Ｊ_ｊ０は、第ｊリンクの重心周り慣性モーメントを表している。第ｉ関節から先端部分までの慣性モーメント（以下、「第ｉ関節周り慣性モーメント」と称す）Ｊ_ｉは、上記（２０）式を用いて下記（２１）式として表される。

上記（２１）式を用いて算出される第ｉ関節周り慣性モーメントＪ_ｉは、第１関節４０２から第ｉ関節４０７までのｉ個全ての関節角度検出値に基づいている。後述の故障判定部１４０の慣性モーメント演算器１４１は、第１関節４０２から第ｉ関節４０７までの関節角度検出値を含む上記（２１）式（関数）を用いて、第ｉ関節周りの慣性モーメントＪ_ｉ（ｉ＝１、２、…）を演算する。

次に、第ｉ関節角度検出値θ_ｉのみに基づいて第ｉ関節周り慣性モーメントＪ_ｉを推定する方法について説明する。第ｉ関節４０７周りの運動方程式は、下記（２２）式として求められる。

ただし、上記（２２）式において各記号は以下のように定義されている。
Ｔ_ｉ：第ｉ関節モータトルク
ｄ_ｉ：第ｉ関節トルク外乱
Ｄ_ｉ：第ｉ関節粘性摩擦

上記（２２）式における第ｉ関節角度θ_ｉの１階時間微分値である第ｉ関節角速度と、２階時間微分である第ｉ関節角加速度とは、夫々、下記（２３）式及び（２４）式のように差分近似できる。

ただし、上記（２３）式及び（２４）式における各記号は、以下のように定義されている。
Ｔ：制御周期
ｋ：各制御周期の値を示すインデックス（ｋ番目の制御周期における値を表す）

上記（２３）式及び（２４）式を上記（２２）式に代入すると、下記（２５）式を得ることができる。

上記（２５）式を第ｉ関節周り慣性モーメントＪ_ｉについて解くと、第ｉ関節周り慣性モーメント推定値Ｊ_ｉ（ハット）は下記（２６）式として表される。

ただし、第ｉ関節粘性摩擦Ｄ_ｉは設計により既知であるとする。また、第ｉ関節トルクの外乱ｄ_ｉは、各関節軸が鉛直方向を向く瞬間にはｄ_ｉ＝０となるため、各関節軸が鉛直方向を向く瞬間において、上記（２６）式は下記（２７）式と書き換えることができる。

後述の故障判定部１４０の慣性モーメント推定器１４２は、第ｉ関節角度検出器１３３からの第ｉ関節角度検出値θ_ｉを含む上記（２７）式（関数）を用いて、第ｉ関節周りの慣性モーメント推定値Ｊ_ｉ（ハット）（ｉ＝１、２、…）を演算する。
また、上記（２７）式と上記（２１）式との差を、第ｉ関節周り慣性モーメント推定誤差ｅ_Ｊ、ｉとし、下記（２８）式を用いて算出することができる。

上記（２８）式に、上記（１７）式乃至（２０）式を代入すると、下記（２９）式を得ることができる。

ここで、上述より明らかなように、上記（２９）式の第１項は関節角度検出値θ_ｉのみの関数であり、第２項以降は関節角度検出値θ_１からθ_ｉ−１にも依存する関数となっている。後述の故障判定部１４０の推定誤差演算器１４３は、上記（２９）式を用いて、第ｉ関節周りの慣性モーメント推定誤差ｅ_Ｊ、ｉ（ｉ＝１、２、…）を演算する。

本実施の形態に係るロボット制御装置１の故障判定部１４０は、例えば、上述したロボットハンド１３０の指機構１３１の各関節に設けられた各関節角度検出器１３３の故障を、この慣性モーメント推定誤差ｅ_Ｊ、ｉを用いて判定することができる。

図２は、本発明の実施の形態に係るロボット制御装置の故障判定部によるｉ＝２における関節角度検出器の故障を判定するための処理フローを示すフローチャートである。まず、ロボット制御装置１の故障判定部１４０は、ｉ＝２（第２関節までモデル化）において下記（３０）式を満足するか否かを判定する（ステップＳ２０１）。

ただし、上記（３０）式において、δは関節角度検出器１３３の故障を判定するための故障判定閾値（判定閾値）であり、例えば、予め故障判定部１４０に設定されている。

故障判定部１４０は、上記（３０）式を満足すると判断した場合（ステップＳ２０１のＹＥＳ）、関節角度検出値θ_１及びθ_２は正常（第１関節角度検出器１３３と第２関節角度検出器１３３が故障していない）と判断し（ステップＳ２０２）、図３の（ステップＳ３０１）の処理に移行する。

一方、故障判定部１４０は、上記（３０）式を満足しないと判断した場合（ステップＳ２０１のＮＯ）、関節角度検出値θ_１及びθ_２のうち少なくとも一方が異常と判断し（ステップＳ２０３）、例えば、第１関節のモータ１３２に負のトルクを発生させ、関節角度検出値θ_１＝０に保持し、関節角度検出値θ_１＝０の状態で上記（３０）式を満足するか否かを判定する（ステップＳ２０４）。

故障判定部１４０は、上記（３０）式を満足すると判断した場合（ステップＳ２０４のＹＥＳ）、関節角度検出値θ_１が異常（第１関節角度検出器１３３は故障している）、関節角度検出値θ_２が正常（第２関節角度検出器１３３は故障していない）と判断し（ステップＳ２０５）、図３の（ステップＳ３０１）処理に移行する。

故障判定部１４０は、上記（３０）式を満足しないと判断した場合（ステップＳ２０４のＮＯ）、関節角度検出値θ_１が正常か否か（第１関節角度検出器１３３が故障しているか否か）が不明であり、関節角度検出値θ_２が異常（第２関節角度検出器１３３が故障している）と判断し（ステップＳ２０６）、第２関節のモータ１３２に負のトルクを発生させ、関節角度検出値θ_２＝０に保持し、関節角度検出値θ_２＝０の状態で上記（３０）式を満足するか否かを判定する（ステップＳ２０７）。

故障判定部１４０は、上記（３０）式を満足すると判断した場合（ステップＳ２０７のＹＥＳ）、関節角度検出値θ_１が正常（第１関節角度検出器１３３は故障していない）、関節角度検出値θ_２が異常（第２関節角度検出器１３３は故障している）と判断し（ステップＳ２０８）、図３の（ステップＳ３０１）の処理に移行する。

故障判定部１４０は、上記（３０）式を満足しないと判断した場合（ステップＳ２０７のＮＯ）、関節角度検出値θ_１及びθ_２が異常（第１関節角度検出器１３３及び第２関節角度検出器１３３が故障している）と判断し（ステップＳ２０９）、図３の（ステップＳ３０１）の処理に移行する。

図３は、第１関節角度検出器及び第２関節角度検出器のうちいずれが故障しているか否かが分かっている場合の関節角度検出器の故障を判定するための処理フローを示すフローチャートである。

故障判定部１４０は、ｉ＝ｊ、ｊ≧３において、異常と判断した関節角度検出値を上記（ステップＳ２０４）と同様に、その関節角度検出値を０に保持し、上記（３０）式を満足するか否かを判定する（ステップＳ３０１）。

故障判定部１４０は、上記（３０）式を満足すると判断した場合（ステップＳ３０１のＹＥＳ）、関節角度検出値θ_ｊは正常（第ｊ関節角度検出器１３３は故障していない）と判断し（ステップＳ３０２）、上記（ステップＳ３０１）の処理に戻る。

一方、故障判定部１４０は、上記（３０）式を満足しないと判断した場合（ステップＳ３０１のＮＯ）、関節角度検出値θ_ｊは異常（第ｊ関節角度検出器１３３が故障している）と判断し（ステップＳ３０３）、上記（ステップＳ３０１）の処理に戻る。

このようにして、動作に必要最小限のセンサのみを搭載した人と共生できる質量及びサイズのロボットにおいても、故障した関節角度検出器１３３を容易に特定することができる。
故障判定部１４０が、上述した図２及び図３に示す処理フローを用いて、全ての関節角度検出器１３３の故障判定を行い完了すると、安全制御部１２０は、その判定結果とロボットハンド１３０が実施しようとしている作業の種類に応じて、下記（３１）式に示す第ｊ関節のモータトルクＴ_ｊを生成する。

ただし、上記（３１）式において、Ｔ_ｊｒは、第ｊ関節角度検出器１３３が故障していない場合に、第ｊ関節のモータ１３２が生成するモータトルクの目標値（以下、「第ｊ関節モータトルク指令」と称す）である。また、ｓ_ｊは、ロボットハンド１３０が実施しようとしている作業の種類を分類するためのパラメータ（以下、「作業分類パラメータ」と称す）であり、例えば、上記（３２）式のように設定されている。

作業分類パラメータｓ_ｊが１（第１所定値）に設定されている場合、指機構１３１は外側に向かって反力を発生させ、一方、作業分類パラメータｓ_ｊが０（第２所定値）に設定されている場合、指機構１３１は外側に向かって反力を発生させない。

上記（３１）式における３つのモータトルク指令Ｔ_ｊは、例えば、以下のように定義できる。
（ａ）Ｔ_ｊ＜０：第ｊ関節角度検出器１３３が故障しており、かつロボットハンド１３０が実施しようとしている作業において、指機構１３１が外側（指が伸びる方向）に向かって反力を発生する必要がある場合（ｓ_ｊ＝１）。

この場合、第ｊ関節モータトルク指令Ｔ_ｊを負トルク（所定トルク）に設定することにより、第ｊ関節が伸びた状態になり、指機構１３１が物体に接触した時に物体に対して反力を与えることができるようになる。
（ｂ）Ｔ_ｊ＝０：第ｊ関節角度検出器１３３が故障しており、かつロボットハンド１３０が実施しようとしている作業において、指機構１３１が外側に向かって反力を発生する必要がない場合（ｓ_ｊ＝０）。

この場合、第ｊ関節モータトルク指令Ｔ_ｊを０に設定することにより、第ｊ関節は指機構１３１が物体により内側に押された場合は正の方向に（曲がる方向に）回転し、指機構１３１が物体から外側に押された場合は指機構１３１が伸びた状態になるまで負の方向に（伸びる方向に）回転する。いずれの場合も、第ｊ関節の可動範囲の端に到達したら物体からの力が同じ方向に加わる限りその角度のまま保持される。
（ｃ）Ｔ_ｊ＝Ｔ_ｊｒ：第ｊ関節角度検出器１３３が故障していない、又は故障しているか否か分からない場合。

この場合、ロボットハンド１３０の実施しようとしている作業に必要な第ｊ関節モータトルク指令Ｔ_ｊ（作業パターン毎に予め設定された安全なモータトルク指令）を生成し、この第ｊ関節モータトルク指令Ｔ_ｊに従って第ｊ関節を駆動する。

本実施の形態において、上記（３１）式により第ｊ関節モータトルクＴ_ｊを生成することにより、第ｊ関節角度検出器１３３が故障している場合でも、第ｊ関節のモータ１３２の暴走を回避し、かつロボットハンド１３０が実施しようとしている作業を安全に継続できる。以下上記（３１）式に示すＴ_ｊを「安全関節モータトルク指令」と称す。

図１は、本発明の実施の形態に係るロボット制御装置の概略的なシステム構成を示すブロック図である。本実施の形態に係るロボット制御装置１は、関節角度指令生成部１１０と、安全制御部１２０と、ロボットハンド１３０と、故障判定部１４０と、を備えている。

関節角度指令生成部１１０は、故障判定部１４０より出力された各関節角度検出器１３３の故障判定結果に基づいて、各関節を駆動するための関節角度指令を生成し、生成した各関節角度指令を安全制御部１２０に対して出力する。

関節角度指令生成部１１０は、作業パターン生成器１１１と、関節角度指令生成器１１２と、を有している。

作業パターン生成器１１１は、作業パターン生成手段の一具体例であり、例えば、ロボットハンド１３０のユーザによる入力情報、又は、当該ロボット制御装置１などに予め設定されたプログラムに従って、ロボットハンド１３０が次に実施する作業に必要な作業パターン（動作データ）を生成し、関節角度指令生成器１１２及び安全制御部１２０に対して出力する。

関節角度指令生成器１１２は、関節角度指令生成手段の一具体例であり、故障判定部１４０からの各関節角度検出器１３３の故障判定結果に基づいて、故障した関節角度検出器１３３が無いと判断した場合、作業パターン生成器１１１からの作業パターンに基づいて、その作業パターンが表す動作を実施するのに必要な関節角度指令を生成し、安全制御部１２０に対して出力する。

関節角度指令生成器１１２は、故障判定部１４０からの各関節角度検出器１３３の故障判定結果に基づいて、故障した関節角度検出器１３３が存在すると判断した場合、故障している関節角度検出器１３３の位置と、ロボットハンド１３０が実施しようとしている作業パターンと、に基づいて、その作業パターンが実施可能か否かを判定する。

関節角度指令生成器１１２は、その作業内容が実施不能と判定した場合、作業パターンの作業を中止するために必要な関節角度指令を生成し、安全制御部１２０に対して出力する。ここで、上記作業内容を中止するために必要な関節角度指令とは、例えば、故障していない関節角度検出器１３３のみを使ってロボットハンド１３０が安全な姿勢をとることができる関節角度指令である。

一方、関節角度指令生成部１１０は、その作業パターンが実施可能と判定した場合、故障した関節角度検出器１３３が配置された関節以外の関節に対する各関節角度指令を生成し、安全制御部１２０に対して出力する。

安全制御部１２０は、関節角度指令生成部１１０から関節角度指令と、故障判定部１４０からの各関節角度検出器１３３の故障判定結果と、ロボットハンド１３０の各関節角度検出器１３３からの関節角度検出値と、に基づいて、ロボットハンド１３０の各関節のモータ１３２を安全に駆動させるための安全関節モータトルク指令を生成し、ロボットハンド１３０のモータ１３２に対して出力する。ここで、安全関節モータトルク指令とは、例えば、ロボットハンド１３０の指機構１３１の全関節における各関節角度検出値が各関節角度指令に追従するような各関節のモータトルク指令である。

安全制御部１２０は、パラメータ生成器１２１と、モータトルク指令生成器１２２と、を有している。

パラメータ生成器１２１は、パラメータ生成手段の一具体例であり、関節角度指令生成部１１０の作業パターン生成器１１１からの作業パターンに基づいて、ロボットハンド１３０が実施しようとしている作業を各関節に求められる動作に分類して、作業分類パラメータｓ_ｊ（ｊ＝１、２、…）を設定し、設定した作業パラメータｓ_ｊをモータトルク指令生成器１２２に対して出力する。

モータトルク指令生成器１２２は、トルク指令生成手段の一具体例であり、関節角度指令生成部１１０からの各関節角度指令と、パラメータ生成器１２１からの作業分類パラメータｓ_ｊと、故障判定部１４０からの各関節角度検出器１３３の故障判定結果と、に基づいて、上記（３１）式で表される安全関節モータトルク指令Ｔ_ｊ（ｊ＝１、２、…）を生成し、ロボットハンド１３０の各モータ１３２に対して出力する。

例えば、モータトルク指令生成器１２２は、故障判定部１４０からの各関節角度検出器１３３の故障判定結果に基づいて、故障した関節角度検出器１３３が無いと判断した場合、ロボットハンド１３０の指機構１３１の全関節における関節角度検出器１３３の関節角度検出値が関節角度指令生成器１１２により生成された各関節角度指令に追従するような安全関節モータトルク指令Ｔ_ｊを生成し、ロボットハンド１３０の各モータ１３２に対して出力する。

一方、モータトルク指令生成器１２２は、故障判定部１４０からの各関節角度検出器１３３の故障判定結果に基づいて、故障した関節角度検出器１３３が存在すると判断した場合、故障していない関節角度検出器１３３の関節角度検出値が関節角度指令生成器１１２により生成された関節角度指令に追従し、かつ故障した関節角度検出器１３３を備える関節のモータ１３２に対するモータトルクが作業パターン毎に設定された安全なモータトルクとなるような安全関節モータトルク指令Ｔ_ｊを生成し、ロボットハンド１３０の各モータ１３２に対して出力する。ここで、モータトルク指令生成器１２２は、安全関節モータトルク指令Ｔ_ｊをモータ電流に変換してロボットハンド１３０の各関節のモータ１３２に対して出力する。

ロボットハンド１３０は、指機構１３１と、安全制御部１２０のモータトルク指令生成器１２２からの安全関節モータトルク指令Ｔ_ｊに基づいて各関節を駆動する複数のモータ１３２と、各モータ１３２により駆動された各関節の回転角度を検出し、関節角度検出値として出力する複数の関節角度検出器１３３と、を有している。指機構１３１は、複数のリンクと、隣合うリンクを回転可能に連結する複数の関節と、から構成される機構である。

故障判定部１４０は、ロボットハンド１３０の各関節角度検出器１３３からの関節角度検出値に基づいて、ロボットハンド１３０が物体に接触していない状態で、各関節の回転軸が鉛直方向を向いた瞬間に、故障した関節角度検出器１３３の有無およびその位置を判定し、その各関節角度検出器１３３の故障判定結果を、関節角度指令生成部１１０及び安全制御部１２０に対して出力する。

故障判定部１４０は、慣性モーメント演算器１４１と、慣性モーメント推定器１４２と、慣性モーメント推定誤差演算器１４３と、故障判定器１４４と、を有している。

慣性モーメント演算器１４１は、慣性モーメント演算手段の一具体例であり、各関節角度検出器１３３からの第１乃至第ｉ関節角度検出値と、上記（２１）式と、に基づいて、第ｉ関節周りの慣性モーメントＪ_ｉ（ｉ＝１、２、…）を演算し、推定誤差演算器１４３に対して出力する。

慣性モーメント推定器１４２は、慣性モーメント推定手段の一具体例であり、各関節角度検出器１３３からの第ｉ関節角度検出値と、上記（２７）式と、に基づいて、第ｉ関節の回転軸が鉛直方向となるときの第ｉ関節周りの慣性モーメント推定値Ｊ_ｉ（ハット）（ｉ＝１、２、…）を演算し、慣性モーメント推定誤差演算器１４３に対して出力する。

慣性モーメント推定誤差演算器１４３は、推定誤差演算手段の一具体例であり、慣性モーメント演算器１４１からの慣性モーメントＪ_ｉと、慣性モーメント推定器１４２からの慣性モーメント推定値Ｊ_ｉ（ハット）と、の差を、上記（２９）式を用いて、第ｉ関節周りの慣性モーメント推定誤差ｅ_Ｊ、ｉ（ｉ＝１、２、…）として演算し、故障判定器１４４に対して出力する。

故障判定器１４４は、故障判定手段の一具体例であり、慣性モーメント推定誤差演算器１４３からの慣性モーメント推定誤差ｅ_Ｊ、ｉと、上記（３０）式と、に基づいて、図２及び図３に示す処理フローに従って各関節角度検出器１３３が故障しているか否かを判定し、その各関節角度検出器１３３の故障判定結果を関節角度指令生成部１１０及び安全制御部１２０に対して出力する。

故障判定部１４０は、上述の各関節角度検出器１３３の判定演算を、ロボットハンド１３０の作業と作業の間、物体に接触する前に各関節の回転軸が鉛直方向を向いた瞬間に実施する。

このように、本実施の形態によれば、関節角度検出器１３３を冗長的に設けることなく、ロボットハンド１３０の作業と作業の間、物体に接触していない状態で各関節の回転軸が鉛直方向を向いた瞬間に、各関節角度検出器１３３が故障しているか否かを瞬時に判定できる。また、その判定結果に基づいて、故障した関節角度検出器１３３を使用することなく、ロボットハンド１３０に安全に次の作業を実施させるか、或いは中止させることができる。

次に、本実施の形態に係るロボット制御装置１を用いて、シミュレーションを行った結果について説明する。なお、本シミュレーションは、３つの関節と３つのリンクから構成されるロボットハンド１３０の人差し指において、第２関節角度検出器１３３のみが故障している場合に、人差し指と親指を使って複数のはがきを輪ゴムで止める作業を模擬している。また、シミュレーションに用いた数値は以下のように設定されている。

ｌ_１＝２０×１０^−３[ｍ]、ｌ_２＝２０×１０^−３[ｍ]、ｌ_３＝２０×１０^−３[ｍ]、ｌ_１０＝１０×１０^−３[ｍ]、ｌ_２０＝１０×１０^−３[ｍ]、ｌ_３０＝１０×１０^−３[ｍ]、ｍ_１＝３０×１０^−３[ｋｇ]、ｍ_２＝３０×１０^−３[ｋｇ]、ｍ_３＝３０×１０^−３[ｋｇ]、Ｊ_１０＝１．４２×１０^−６[ｋｇ・ｍ^２]、Ｊ_２０＝１．４２×１０^−６[ｋｇ・ｍ^２]、Ｊ_３０＝１．４２×１０^−６[ｋｇ・ｍ^２]、Ｄ_１＝１×１０^−４[Ｎ・ｍ・ｓ／ｒａｄ]、Ｄ_２＝１×１０^−４[Ｎ・ｍ・ｓ／ｒａｄ]、Ｄ_３＝１×１０^−４[Ｎ・ｍ・ｓ／ｒａｄ]、θ_１ｍｉｎ＝０［ｒａｄ］、θ_１ｍａｘ＝π［ｒａｄ］、θ_２ｍｉｎ＝０［ｒａｄ］、θ_２ｍａｘ＝π［ｒａｄ］、θ_３ｍｉｎ＝０［ｒａｄ］、θ_３ｍａｘ＝π［ｒａｄ］、ｃ_ｖ２＝−１×１０^−１［ｒａｄ／ｓ］、Ｔ＝１×１０^−３[ｓ]、Ｋ_ｐ１＝４０[ｓ^−１]、Ｋ_ｖ１＝４０×２×π[ｓ^−１]、Ｋ_ｖｊ１＝Ｋ_ｖ１×（Ｊ_１０＋ｍ_１×ｌ_１０ ^２）[Ｎ・ｍ^２・ｓ^−１]、Ｋ_ｐ２＝４０[ｓ^−１]、Ｋ_ｖ２＝４０×２×π[ｓ^−１]Ｋ_ｖｊ２＝Ｋ_ｖ２×（Ｊ_２０＋ｍ_２×ｌ_２０ ^２）[Ｎ・ｍ^２・ｓ^１]、Ｋ_ｐ３＝４０[ｓ^−１]、Ｋ_ｖ３＝４０×２×π[ｓ^−１]、Ｋ_ｖｊ３＝Ｋ_ｖ３×（Ｊ_３０＋ｍ_３×ｌ_３０ ^２）[Ｎ・ｍ^２・ｓ^−１]、Ｔ_１ｃ＝−１×１０^−１[Ｎ・ｍ]、Ｔ_２ｃ＝−１×１０^−１[Ｎ・ｍ]、Ｔ_３ｃ＝−１×１０^−１[Ｎ・ｍ]、θ_１ｆ＝１×１０^−１［ｒａｄ］、θ_２ｆ＝１×１０^−１［ｒａｄ］、θ_３ｆ＝１×１０^−１［ｒａｄ］、ｖ_１ｃ＝５×１０^−１［ｒａｄ／ｓ］、ｖ_２ｃ＝５×１０^−１［ｒａｄ／ｓ］、ｖ_３ｃ＝５×１０^−１［ｒａｄ／ｓ］、δ＝５×１０^−３[ｋｇ・ｍ^２]、
ｇ＝９．８［ｍ／ｓ^−２］
ただし、上記各記号は、以下のように定義されている。
ｌ_ｉ０：第ｉリンク重心と第ｉ関節の間の距離（以下、「第ｉリンク重心第ｉ関節間距離」と称す）
θ_ｉｍｉｎ：第ｉ関節角度下限値、
θ_ｉｍａｘ：第ｉ関節角度上限値、
ｃ_ｖ２：第２関節速度検出値ドリフト量、
Ｋ_ｐｉ：第ｉ関節角度比例制御ゲイン、
Ｋ_ｖｉ：正規化第ｉ関節速度比例制御ゲイン、
Ｋ_ｖｊｉ：第ｉ関節速度比例制御ゲイン、
Ｔ_ｉｃ：第ｉ関節角度を第ｉ関節角度下限値に保持するための第ｉ関節のモータトルク（以下、「第ｉ関節角度下限値保持モータトルク」と称す）
θ_ｉｆ：第ｉ関節角度検出器１３３が故障していない場合の第ｉ関節角度最終目標値（以下、「第ｉ関節角度最終目標値」と称す）
ｖ_ｉｃ：第ｉ関節角度検出器１３３が故障していない場合の各制御周期における第ｉ関節速度指令変化量（以下、「第ｉ関節速度指令変化量」と称す）
ｇ：重力加速度

ロボットハンド１３０の指機構１３１の人差し指の運動方程式は、下記（３３）式乃至（３５）式として導出できる。

ここで、上記（３３）乃至（３５）で表される指機構１３１の人差し指を、輪ゴムに通すために、各関節の回転軸は鉛直方向を向いている状態で各関節を少し曲げる動作を想定する。輪ゴムを広げる際に、人差し指の各リンクは外向きの反力を輪ゴムに発生させる必要があるため、作業分類パラメータは、ｓ_１＝１、ｓ_２＝１、ｓ_３＝１となる。

初期状態において、人差し指は伸びている状態（θ_１＝０、θ_２＝０、θ_３＝０）であるとする。まず、第１関節角度検出器１３３乃至第３関節角度検出器１３３が故障しているか否か不明である。このため、上記（３６）式乃至（３８）式で表される第１関節のモータトルク指令Ｔ_１ｒ、第２関節のモータトルク指令Ｔ_２ｒ、及び第３関節のモータトルク指令Ｔ_３ｒにより各関節角度を制御する。

上記（３６）式乃至（３８）式において、関節角度指令生成器１１２は、第１関節角度指令θ_１ｒ、第２関節角度指令θ_２ｒ、及び第３関節角度指令θ_３ｒを、下記（３９）式乃至（４１）式の１階時間積分として演算する。

ただし、上記（３９）式乃至（４１）式において、ｃ_θｉは、故障した第ｉ関節角度検出器１３３の故障判定結果（１：故障しているか否か不明、０：故障していない、−１：故障している）とする。

上記（３９）式乃至（４１）式により、第ｉ関節角度検出値θ_ｉが第ｉ関節角度最終目標値θ_ｉｆに到達するまで、第ｉ関節角度指令θ_ｉｒは、制御周期ごとに第ｉ関節速度指令変化量ｖ_ｉｃずつその１階時間微分値を増加させ、第ｉ関節角度最終目標値θ_ｉｆに到達した時点でその値を保持する。

以下、上述のように制御した人差し指の各関節角度検出器１３３の故障判定を、図５乃至図８を用いて説明する。

図５（ａ）は、本実施の形態に係るロボット制御装置のシミュレーションにおける第２関節の関節角度の時間変化を示しており、図５（ｂ）は、従来のロボット制御装置のシミュレーションにおける第２関節の関節角度の時間変化を示している。図５（ａ）及び（ｂ）において、実線は実際の第２関節角度θ_２、一点鎖線は第２関節角度検出値θ_２ｍ、破線は第２関節角度指令θ_２ｒを示している。

図５（ａ）において、時間０．００３[ｓ]まで図２の（ステップＳ２０１）から（ステップＳ２０３）の演算が実施され、実際の第２関節角度θ_１及びθ_２のうち少なくとも一方が異常であることのみが分かっている。この間、故障している関節角度検出器１３３が特定できていないため、上記（４０）式の１番目の式に従って破線の第２関節角度指令θ_２ｒは増加している。ここで、第２関節角度指令θ_２ｒが階段状に変化するのは、第２関節角度指令θ_２ｒが制御周期Ｔ＝１×１０^−３[ｓ]の離散時間に従って生成されるためである。本実施の形態において第２関節角度検出器１３３は故障しており、下記（４２）式で表される誤った第２関節速度検出値θ_２ｍの１回時間積分値を出力するものとする。

上記（４２）式で表される誤った第２関節角度検出値θ_２ｍは破線が示すように変化する。すなわち、０．００３[ｓ]までは第２関節角度検出値θ_２ｍを第２関節角度指令θ_２ｒに収束させようとして実際の第２関節角度θ_２を制御し、その結果、実際の第２関節角度θ_２は実線のように増加する。

図５（ａ）の０．００３[ｓ]において、図２の（ステップＳ２０４）から（ステップＳ２０６）の演算が実施され、第２関節角度検出器１３３の故障が検出され、（ステップＳ２０７）において上記（３１）式の１番目の式に基づいて安全関節モータトルクが出力され、実際の第２関節角度θ_２＝０［ｒａｄ］に保持される。実際、０．００３[ｓ]以降において、実際の第２関節角度θ_２は０［ｒａｄ］に保持されており、第２関節が伸びた状態で保持されていることが分かる。この間、第２関節角度検出器１３３は故障したままであるため、誤った第２関節角度検出値θ_２ｍを出力し続けている。

一方で、従来のロボット制御装置においては、図５（ｂ）に示す如く、実線で示す実際の第２関節角度θ_２は発散しており、人差し指の第２関節が完全に折れるまで曲がり続けることが分かる。

図６（ａ）は、本実施の形態に係るロボット制御装置のシミュレーションにおける第１関節の関節角度の０．０１５[ｓ]までの時間変化を示しており、図６（ｂ）は、本実施の形態に係るロボット制御装置のシミュレーションにおける第１関節の関節角度の０．５[ｓ]までの時間変化を示している。図６（ａ）及び（ｂ）において、実線は実際の第１関節角度θ_１、破線は第１関節角度指令θ_１ｒを示している。

図６（ａ）の０．００６[ｓ]において、図２の（ステップＳ２０７）の演算が完了し、その後、（ステップＳ２０８）の演算が行われ、第１関節角度検出器１３３が故障していないと判定される。よって、０．００６[ｓ]以降も関節角度指令生成器１１２は、上記（３９）式の２番目の式に従って、図６（ａ）の破線で示す第１関節角度指令θ_１ｒを出力しており、実際の第１関節角度θ_１も第１関節角度指令θ_１ｒに追従するように増加している。

図６（ｂ）に示すように、破線で示す第１関節角度指令θ_１ｒは０．２[ｓ]において第１関節角度最終目標値θ_１ｆ＝０．１［ｒａｄ］に到達し、それ以降はθ_１ｒ＝θ_１ｆに保持されている。また、実線で示す実際の第１関節角度θ_１は、上記（３１）式の３番目の式および上記（３６）式に基づいてモータトルク指令生成器１２２が出力する安全関節モータトルク指令により制御され、第１関節角度最終目標値θ_１ｆに漸近的に収束している。なお、図６（ａ）及び（ｂ）において、第１関節角度検出値は実際の第１関節角度に一致している。

図７（ａ）は、本実施の形態に係るロボット制御装置のシミュレーションにおける第３関節の関節角度の０．０１５[ｓ]までの時間変化を示しており、図７（ｂ）は、本実施の形態に係るロボット制御装置のシミュレーションにおける第３関節の関節角度の０．５[ｓ]までの時間変化を示している。図７（ａ）及び（ｂ）において、実線は実際の第３関節角度θ_３、破線は第３関節角度指令θ_３ｒを示している。

図７（ａ）において、０．００９[ｓ]までは図２の（ステップＳ２０１）乃至（ステップＳ２０８）、及び図３の（ステップＳ３０１）（ｊ＝３とした場合）が実施される。その結果、０．００９[ｓ]で（ステップＳ３０２）において、故障判定器１４４は、第３関節角度検出器１３３が故障していないと判定する。したがって、０．００９[ｓ]以降も、関節角度指令生成器１１２は、上記（４１）式の２番目の式に基づいて、図７（ａ）の破線で示す第３関節角度指令θ_３ｒを、モータトルク指令生成器１２２に対して出力する。これにより、実線で示す実際の第３関節角度θ_３も第３関節角度指令θ_３ｒに追従するように増加している。

図７（ｂ）に示すように、破線で示す第３関節角度指令θ_３ｒは、０．２[ｓ]において第３関節角度最終目標値θ_３ｆ＝０．１［ｒａｄ］に到達し、それ以降は第３関節角度指令θ_３ｒ＝第３関節角度最終目標値θ_３ｆに保持されている。また、実線で示す実際の第３関節角度θ_３は、上記（３１）式の３番目の式、及び上記（３８）式に基づいてモータトルク指令生成器１２２が出力する安全関節モータトルク指令により制御され、第３関節角度最終目標値θ_３ｆに漸近的に収束している。

図８は、本実施の形態に係るロボット制御装置のシミュレーションにおける第２関節周り慣性モーメント推定誤差の時間変化を示している。実線は、推定誤差演算器１４３により上記（２９）式に基づいて算出された第２関節周りの慣性モーメント推定誤差ｅ_Ｊ、２を示している。慣性モーメント推定誤差ｅ_Ｊ、２の絶対値は、０[ｓ]において実際の第２関節角度θ_２＝０であるため、０[ｋｇ・ｍ^２]から増加し、０．００３[ｓ]において関節角度検出器１３３の故障判定閾値δ＝５×１０^−３[ｋｇ・ｍ^２]を超える。このため、上述のように故障判定器１４４、図２の（ステップＳ２０６）の処理において、第２関節角度検出器１３３が故障していると判定する。

次に、故障判定器１４４は、（ステップＳ２０７）の処理に移行し、このとき、図８に示す如く、慣性モーメント推定誤差ｅ_Ｊ、２は減少している。そして、０．００６[ｓ]において、故障判定器１４４は、上記（３０）式を満足したと判断するため（ステップＳ２０７のＹＥＳ）、（ステップＳ２０８）の処理に移行し、第１関節角度検出器１３３が故障していないと判定する。

図９は、故障した関節角度検出器が設けられている第２関節を制御するための第２関節のモータトルク指令Ｔ_２の時間変化を示す図である。

図９に示す如く、０．００３[ｓ]までの間、故障判定器１４４は、図２の（ステップＳ２０４）の処理まで実施し、第１関節角度検出器１３３または第２関節角度検出器１３３の少なくとも１つが故障していることだけが判明している。このため、モータトルク指令生成器１２２は、上記（３１）式の３番目の式に基づいて、第２関節モータトルク指令Ｔ_２＝Ｔ_２ｒを第２関節のモータ１３２に対して出力している。

また、０．００３[ｓ]以降において、故障判定器１４４は図２の（ステップＳ２０７）の処理を実行し、モータトルク指令生成器１２２は実際の第２関節角度θ_２＝０に保持するように、上記（３１）式の１番目の式で表される安全関節モータトルク指令Ｔ_２＝Ｔ_２ｃを第２関節のモータ１３２に対して出力している。

以上、本実施の形態に係るロボット制御装置１によれば、関節角度検出器１３３の冗長設計に頼ることなく、例えば、ロボットハンド１３０の作業間、物体に接触していない状態で各関節の回転軸が鉛直方向を向いた時に、各関節角度検出器１３３が故障しているか否かを瞬時に判定でき、その判定結果に基づいて故障した関節角度検出器１３３を使用することなく、ロボットハンド１３０に安全に次の作業を実施させ、或いは、作業を中止させることができる。すなわち、ロボットハンド１３０のセンサに故障が生じた場合でも最適に制御を行うことができる。

また、特許文献１に示す従来技術においては、どの関節角度検出器が故障していないか不明な場合、一般的に、オブザーバーが使用する正しい関節角度検出値を出力し、可観測となるのに十分な関節角度検出器が特定できない。そのため故障した関節角度検出器を特定することができない。一方で、本実施の形態に係るロボット制御装置１によれば、故障している関節角度検出器１３３の位置や数によらず短時間かつ確実に故障した関節角度検出器１３３を特定できるメリットを有している。

さらに、上述の理由から、特許文献１に示す従来技術によると、例えば図５（ａ）に示すように、故障したロボットハンドの指機構の関節が制御不能となり、次の作業を実施できなくなり、また、実施する作業の種類によっては周囲の物体や人に対して十分に安全でない可能性もある。これに対し、本実施の形態に係るロボット制御装置１によれば、図５（ｂ）に示すように、故障した関節角度検出器１３３の影響を受けないように制御を行うため、作業を継続することが可能となり、また実施する作業の種類によっては安全に作業を中止することもできる。

なお、本発明は上記実施の形態に限られたものではなく、趣旨を逸脱しない範囲で適宜変更することが可能である。

例えば、上記実施の形態において、上記（３９）式乃至（４１）式における第ｉ関節速度指令変化量ｖ_ｉｃを一定値としているが、これに限らず、第ｉ関節角度が第ｉ関節角度指令に収束するような任意の可変量を適用することも可能である。

上記実施の形態において、上記（３６）式乃至（３８）式における第ｉ関節モータトルク指令をフィードバック線形化制御によって算出しているが、これに限らず、その他の任意の制御則にしたがって算出してもよい。

さらに、上記実施の形態において、ロボットハンド１３０の指機構１３１は３つのリンクと３つの関節を有する構成であるが、これに限らず、任意の構成が適用可能である。

上記実施の形態において、輪ゴムを複数のはがきに掛ける作業の例を示したが、これに限らず、ロボットハンド１３０を使った任意の作業に適用可能である。

上記実施の形態において、関節角度指令生成部１１０、安全制御部１２０、及び、故障判定部１４０は、マイコンに実装したソフトウエアとして実現することができるが、ＡＳＩＣなどハードウェアとして実現してもよい。本発明は、例えば、ＣＰＵにコンピュータプログラムを実行させることにより実現できる。

プログラムは、様々なタイプの非一時的なコンピュータ可読媒体（non-transitory computer readable medium）を用いて格納され、コンピュータに供給することができる。非一時的なコンピュータ可読媒体は、様々なタイプの実体のある記録媒体（tangible storage medium）を含む。非一時的なコンピュータ可読媒体の例は、磁気記録媒体（例えばフレキシブルディスク、磁気テープ、ハードディスクドライブ）、光磁気記録媒体（例えば光磁気ディスク）、ＣＤ−ＲＯＭ、ＣＤ−Ｒ、ＣＤ−Ｒ／Ｗ、半導体メモリ（例えば、マスクＲＯＭ、ＰＲＯＭ（Programmable ROM）、ＥＰＲＯＭ（Erasable PROM）、フラッシュＲＯＭ、ＲＡＭ）を含む。

また、プログラムは、様々なタイプの一時的なコンピュータ可読媒体（transitory computer readable medium）によってコンピュータに供給されてもよい。一時的なコンピュータ可読媒体の例は、電気信号、光信号、及び電磁波を含む。一時的なコンピュータ可読媒体は、電線及び光ファイバ等の有線通信路、又は無線通信路を介して、プログラムをコンピュータに供給できる。

本発明は、各関節に設けられた関節角度検出器１３３の故障を考慮して、ロボットを安全かつ最適に制御できるため、例えば、小型軽量低価格でありかつ周囲の人や物に対して安全であることが要求される、家事手伝いロボット、介護ロボット、楽器演奏ロボット、事務手伝いロボットなど、ロボットハンドを備え、人と共生するサービスロボット全般に適用できる。

１１０ 関節角度指令生成部
１１１ 作業パターン生成器
１１２ 関節角度指令生成器
１２０ 安全制御部
１２１ パラメータ生成器
１２２ モータトルク指令生成器
１３０ ロボットハンド
１３１ 指機構
１３２ モータ
１３３ 関節角度検出器
１４０ 故障判定部
１４１ 慣性モーメント演算器
１４２ 慣性モーメント推定器
１４３ 慣性モーメント推定誤差演算器
１４４ 故障判定器
４０１ 第１リンク
４０２ 第１関節
４０３ 第ｉ−２関節
４０４ 第ｉ−１リンク
４０５ 第ｉ−１関節
４０６ 第ｉリンク
４０７ 第ｉ関節
４０８ 第ｉ−３リンク
４０９ 第ｉ−３関節
４１０ 第ｉ−２リンク

Claims (12)

1. 複数のリンクと、該各リンクを回転可能に連結する複数の関節と、該各関節を駆動する複数の駆動手段と、前記各関節角度を検出する複数の関節角度検出手段と、を備えたロボット制御装置であって、
   前記各関節角度検出手段により検出された先端側の第１関節から第ｉ関節（ｉ≧２）までの関節角度を含む関数に基づいて、前記第ｉ関節から先端部分までの第ｉ関節周りの慣性モーメントを算出する慣性モーメント演算手段と、
   前記関節角度検出手段により検出された前記第ｉ関節の関節角度を含む関数に基づいて、前記第ｉ関節の回転軸が鉛直方向となるときの前記第ｉ関節周りの慣性モーメント推定値を算出する慣性モーメント推定手段と、
   前記慣性モーメント演算手段により算出された前記第ｉ関節周りの慣性モーメントと、前記慣性モーメント推定手段により算出された前記第ｉ関節周りの慣性モーメント推定値と、の差を前記第ｉ関節周りの慣性モーメント誤差として算出する推定誤差演算手段と、
   前記推定誤差演算手段により算出された前記第ｉ関節周りの慣性モーメント誤差の絶対値と判定閾値とを比較して、前記関節角度検出手段の故障を判定する故障判定手段と、
   前記故障判定手段により判定された故障の判定結果と、ロボットが実施しようとしている作業パターンと、に基づいて、前記各関節に対する関節角度指令を生成する関節角度指令生成手段と、
   を備える、ことを特徴とするロボット制御装置。
2. 請求項１記載のロボット制御装置であって、
   前記故障判定手段は、前記推定誤差演算手段により算出された前記第ｉ関節周りの慣性モーメント誤差の絶対値が判定閾値を超えるとき、前記複数の関節角度検出手段の関節角度のうちの一つ関節角度を０度に保持して、前記第ｉ関節周りの慣性モーメント誤差の絶対値と判定閾値との比較を繰返し、前記関節角度検出手段の故障を判定する、ことを特徴とするロボット制御装置。
3. 請求項２記載のロボット制御装置であって、
   前記故障判定手段は、前記複数の関節角度のうちの一つ関節角度を０度に保持して、
   前記第ｉ関節周りの慣性モーメント誤差の絶対値が判定閾値以下となるとき、前記関節角度を０度に保持した前記関節角度検出手段を故障、それ以外の前記関節角度検出手段を正常、と判定し、
   前記第ｉ関節周りの慣性モーメント誤差の絶対値が判定閾値を超えるとき、前記関節角度を０度に保持した前記関節角度検出手段を不明、それ以外の前記関節角度検出手段を故障、と判定し、該故障と判定した関節角度検出手段に対応する関節角度を０度に保持し、前記第ｉ関節周りの慣性モーメント誤差の絶対値と判定閾値との比較を繰り返す、ことを特徴とするロボット制御装置。
4. 請求項１乃至３のうちいずれか１項記載のロボット制御装置であって、
   前記関節角度指令生成手段は、前記故障判定手段により判定された故障の判定結果と、ロボットが実施しようとしている作業パターンと、に基づいて、前記作業パターンが実施可能か否かを判断し、
   実施不能と判断した場合、前記作業パターンの作業を中止するための関節角度指令を生成し、
   実施可能と判断した場合、故障した関節角度検出手段が設置された関節以外に対する関節角度指令を生成する、ことを特徴とするロボット制御装置。
5. 請求項１乃至４のうちいずれか１項記載のロボット制御装置であって、
   前記故障判定手段からの判定結果に基づいて、故障した前記関節角度検出手段が無いと判断した場合、全関節における前記関節角度検出手段の関節角度が前記関節角度指令生成手段により生成された関節角度指令に追従するような前記駆動手段に対するトルク指令を生成し、
   前記故障判定手段からの故障判定結果に基づいて、故障した前記関節角度検出手段が存在すると判断した場合、故障していない前記関節角度検出手段の関節角度が前記関節角度指令生成手段により生成された関節角度指令に追従するように前記駆動手段に対するトルク指令を生成し、かつ、故障した前記関節角度検出手段が設けられた関節の駆動手段に対して前記作業内容毎に予め設定されたトルク指令を生成する、
   トルク指令生成手段を更に備える、ことを特徴とするロボット制御装置。
6. 請求項１乃至４のうちいずれか１項記載のロボット制御装置であって、
   ユーザの入力情報又は予め設定されたプログラムに基づいて、ロボットが作業を実施する作業パターンを生成する作業パターン生成手段と、
   前記作業パターン生成手段により生成された前記作業パターンを分類し、予め設定された前記分類に対応する値を作業分類パラメータに設定するパラメータ生成手段と、
   前記関節角度指令生成手段からの各関節角度指令と、前記パラメータ生成手段からの前記作業分類パラメータと、前記故障判定手段からの故障の判定結果と、に基づいて、前記駆動手段に対するトルク指令を生成するトルク指令生成手段と、
   を更に備える、ことを特徴とするロボット制御装置。
7. 請求項６記載のロボット制御装置であって、
   前記トルク指令生成手段は、
   （ａ）前記故障判定手段からの判定結果に基づいて、前記関節角度検出器が故障していると判断し、前記パラメータ生成手段からの作業分類パラメータに第１所定値が設定されていると判断したとき、所定トルクを生じさせるトルク指令を生成し、
   （ｂ）前記故障判定手段からの判定結果に基づいて、前記関節角度検出器が故障していると判断し、前記パラメータ生成手段からの作業分類パラメータに第２所定値が設定されていると判断したとき、トルク指令に０を設定し、
   （ｃ）前記故障判定手段からの判定結果に基づいて、前記関節角度検出器が故障していない、或いは不明と判断したとき、前記作業パターン毎に予め設定されたトルク指令を生成する、ことを特徴とするロボット制御装置。
8. 請求項６又は７記載のロボット制御装置であって、
   前記パラメータ生成手段は、前記作業パターン生成手段により生成された作業パターンに基づいて、
   ロボットが実施しようとしている作業がロボットハンドの指を伸ばすように外側に向かって反力を発生させると判断したとき、第１所定値を前記作業分類パラメータに設定し、
   ロボットが実施しようとしている作業がロボットハンドの指を外側に向かって反力を発生させないと判断したとき、第２所定値を前記作業分類パラメータに設定する、
   ことを特徴とするロボット制御装置。
9. 請求項１乃至８のうちいずれか１項記載のロボット制御装置であって、
   前記ロボットは、
   前記複数のリンクと、該各リンクを回転可能に連結する複数の関節と、を有する指機構と、
   前記各関節を駆動する複数のモータと、
   前記各関節角度を検出する複数の関節角度検出部と、
   を備えるロボットハンドである、ことを特徴とするロボット制御装置。
10. 請求項１乃至９のうちいずれか１項記載のロボット制御装置であって、
    前記慣性モーメント演算手段は、前記関数として（２１）式を用いて前記第ｉ関節周りの慣性モーメントＪ_ｉを算出し、
    前記慣性モーメント推定手段は、前記関数として（２７）式を用いて前記第ｉ関節周りの慣性モーメント推定値Ｊ_ｉ（ハット）を算出し、
    前記推定誤差演算手段は、（２９）式を用いて前記第ｉ関節周りの慣性モーメント誤差ｅ_Ｊ，ｉを算出する、ことを特徴とするロボット装置。
    

    

    
    但し、上記（２１）式、（２７）式、及び（２９）式において、下記式が成立し、ｌ_ｍは第ｍリンク長さ、ｍ_ｉは第ｉリンク質量、ａ_ｍ、ｎは第ｍ関節と第ｎ関節を結ぶ線分の長さ、ｂ_ｍ、ｎは第ｍリンク重心第ｎ関節間距離、φ_ｍ、ｎは第ｍ関節において第ｍ＋１リンクと第ｍ関節と第ｎ関節を結ぶ線分が成す角度、θ_ｍは第ｍ関節角度、Ｊ_ｊ、ｉは第ｊリンクの第ｉ関節に関する慣性モーメント、Ｊ_ｊ０は第ｊリンクの重心周り慣性モーメント、Ｔ_ｉは第ｉ関節モータトルク、Ｄ_ｉは第ｉ関節粘性摩擦、Ｔは制御周期、ｋは各制御周期の値を示すインデックス、とする。
    

    
11. 複数のリンクと、該各リンクを回転可能に連結する複数の関節と、該各関節を駆動する複数の駆動手段と、前記各関節角度を検出する複数の関節角度検出手段と、を備えたロボット制御装置の制御方法であって、
    前記検出された先端側の第１関節から第ｉ関節（ｉ≧２）までの関節角度を含む関数に基づいて、前記第ｉ関節から先端部分までの第ｉ関節周りの慣性モーメントを算出するステップと、
    前記検出された前記第ｉ関節の関節角度を含む関数に基づいて、前記第ｉ関節の回転軸が鉛直方向となるときの前記第ｉ関節周りの慣性モーメント推定値を算出するステップと、
    前記算出された第ｉ関節周りの慣性モーメントと、前記算出された第ｉ関節周りの慣性モーメント推定値と、の差を前記第ｉ関節周りの慣性モーメント誤差として算出するステップと、
    前記算出された第ｉ関節周りの慣性モーメント誤差の絶対値と判定閾値とを比較して、前記関節角度検出手段の故障を判定するステップと、
    前記判定された故障の判定結果と、ロボットが実施しようとしている作業パターンと、に基づいて、前記各関節に対する関節角度指令を生成するステップと、
    を含む、ことを特徴とするロボット制御装置の制御方法。
12. 複数のリンクと、該各リンクを回転可能に連結する複数の関節と、該各関節を駆動する複数の駆動手段と、前記各関節角度を検出する複数の関節角度検出手段と、を備えたロボット制御装置の制御プログラムであって、
    前記検出された先端側の第１関節から第ｉ関節（ｉ≧２）までの関節角度を含む関数に基づいて、前記第ｉ関節から先端部分までの第ｉ関節周りの慣性モーメントを算出する処理と、
    前記検出された前記第ｉ関節の関節角度を含む関数に基づいて、前記第ｉ関節の回転軸が鉛直方向となるときの前記第ｉ関節周りの慣性モーメント推定値を算出する処理と、
    前記算出された第ｉ関節周りの慣性モーメントと、前記算出された第ｉ関節周りの慣性モーメント推定値と、の差を前記第ｉ関節周りの慣性モーメント誤差として算出する処理と、
    前記算出された第ｉ関節周りの慣性モーメント誤差の絶対値と判定閾値とを比較して、前記関節角度検出手段の故障を判定する処理と、
    前記判定された故障の判定結果と、ロボットが実施しようとしている作業パターンと、に基づいて、前記各関節に対する関節角度指令を生成する処理と、
    をコンピュータに実行させる、ことを特徴とするロボット制御装置の制御プログラム。

Images