(57)【特許請求の範囲】
【請求項１】
複数のリンクと、該各リンクを回転可能に連結する複数の関節と、該各関節を駆動する複数の駆動手段と、前記各関節角度を検出する複数の関節角度検出手段と、を備えたロボット制御装置であって、
前記各関節角度検出手段により検出された先端側の第１関節から第ｉ関節（ｉ≧２）までの関節角度を含む計算式に基づいて、該関節角度が実現された場合の前記第ｉ関節から先端部分までの第ｉ関節周りの慣性モーメントを運動学的に算出する慣性モーメント演算手段と、
前記関節角度検出手段により検出された前記第ｉ関節の関節角度のみを含む運動方程式に基づいて、前記第ｉ関節の回転軸が鉛直方向となるときの前記第ｉ関節周りの慣性モーメント推定値を動力学的に算出する慣性モーメント推定手段と、
前記慣性モーメント演算手段により算出された前記第ｉ関節周りの慣性モーメントと、前記慣性モーメント推定手段により算出された前記第ｉ関節周りの慣性モーメント推定値と、の差を前記第ｉ関節周りの慣性モーメント誤差として算出する推定誤差演算手段と、
前記推定誤差演算手段により算出された前記第ｉ関節周りの慣性モーメント誤差の絶対値と判定閾値とを比較して、前記関節角度検出手段の故障を判定する故障判定手段と、
前記故障判定手段により判定された故障の判定結果と、ロボットが実施しようとしている作業パターンと、に基づいて、前記各関節に対する関節角度指令を生成する関節角度指令生成手段と、
を備える、ことを特徴とするロボット制御装置。
【請求項２】
請求項１記載のロボット制御装置であって、
前記故障判定手段は、前記推定誤差演算手段により算出された前記第ｉ関節周りの慣性モーメント誤差の絶対値が判定閾値を超えるとき、前記複数の関節角度検出手段の関節角度のうちの一つ関節角度を０度に保持して、前記第ｉ関節周りの慣性モーメント誤差の絶対値と判定閾値との比較を繰返し、前記関節角度検出手段の故障を判定する、ことを特徴とするロボット制御装置。
【請求項３】
請求項２記載のロボット制御装置であって、
前記故障判定手段は、前記複数の関節角度のうちの一つ関節角度を０度に保持して、
前記第ｉ関節周りの慣性モーメント誤差の絶対値が判定閾値以下となるとき、前記関節角度を０度に保持した前記関節角度検出手段を故障、それ以外の前記関節角度検出手段を正常、と判定し、
前記第ｉ関節周りの慣性モーメント誤差の絶対値が判定閾値を超えるとき、前記関節角度を０度に保持した前記関節角度検出手段を不明、それ以外の前記関節角度検出手段を故障、と判定し、該故障と判定した関節角度検出手段に対応する関節角度を０度に保持し、前記第ｉ関節周りの慣性モーメント誤差の絶対値と判定閾値との比較を繰り返す、ことを特徴とするロボット制御装置。
【請求項４】
請求項１乃至３のうちいずれか１項記載のロボット制御装置であって、
前記関節角度指令生成手段は、前記故障判定手段により判定された故障の判定結果と、ロボットが実施しようとしている作業パターンと、に基づいて、前記作業パターンが実施可能か否かを判断し、
実施不能と判断した場合、前記作業パターンの作業を中止するための関節角度指令を生成し、
実施可能と判断した場合、故障した関節角度検出手段が設置された関節以外に対する関節角度指令を生成する、ことを特徴とするロボット制御装置。
【請求項５】
請求項１乃至４のうちいずれか１項記載のロボット制御装置であって、
前記故障判定手段からの判定結果に基づいて、故障した前記関節角度検出手段が無いと判断した場合、全関節における前記関節角度検出手段の関節角度が前記関節角度指令生成手段により生成された関節角度指令に追従するような前記駆動手段に対するトルク指令を生成し、
前記故障判定手段からの故障判定結果に基づいて、故障した前記関節角度検出手段が存在すると判断した場合、故障していない前記関節角度検出手段の関節角度が前記関節角度指令生成手段により生成された関節角度指令に追従するように前記駆動手段に対するトルク指令を生成し、かつ、故障した前記関節角度検出手段が設けられた関節の駆動手段に対して作業内容毎に予め設定されたトルク指令を生成する、
トルク指令生成手段を更に備える、ことを特徴とするロボット制御装置。
【請求項６】
請求項１乃至４のうちいずれか１項記載のロボット制御装置であって、
ユーザの入力情報又は予め設定されたプログラムに基づいて、ロボットが作業を実施する作業パターンを生成する作業パターン生成手段と、
前記作業パターン生成手段により生成された前記作業パターンを予め設定されたパターンに分類し、予め設定された前記パターンの分類に対応する値を作業分類パラメータに設定するパラメータ生成手段と、
前記関節角度指令生成手段からの各関節角度指令と、前記パラメータ生成手段からの前記作業分類パラメータと、前記故障判定手段からの故障の判定結果と、に基づいて、前記駆動手段に対するトルク指令を生成するトルク指令生成手段と、
を更に備える、ことを特徴とするロボット制御装置。
【請求項７】
請求項６記載のロボット制御装置であって、
前記トルク指令生成手段は、
（ａ）前記故障判定手段からの判定結果に基づいて、前記関節角度検出器が故障していると判断し、前記パラメータ生成手段からの作業分類パラメータに第１所定値が設定されていると判断したとき、所定トルクを生じさせるトルク指令を生成し、
（ｂ）前記故障判定手段からの判定結果に基づいて、前記関節角度検出器が故障していると判断し、前記パラメータ生成手段からの作業分類パラメータに第２所定値が設定されていると判断したとき、トルク指令に０を設定し、
（ｃ）前記故障判定手段からの判定結果に基づいて、前記関節角度検出器が故障していない、或いは不明と判断したとき、前記作業パターン毎に予め設定されたトルク指令を生成する、ことを特徴とするロボット制御装置。
【請求項８】
請求項６又は７記載のロボット制御装置であって、
前記パラメータ生成手段は、前記作業パターン生成手段により生成された作業パターンに基づいて、
ロボットが実施しようとしている作業がロボットハンドの指を伸ばすように外側に向かって反力を発生させると判断したとき、第１所定値を前記作業分類パラメータに設定し、
ロボットが実施しようとしている作業がロボットハンドの指を外側に向かって反力を発生させないと判断したとき、第２所定値を前記作業分類パラメータに設定する、
ことを特徴とするロボット制御装置。
【請求項９】
請求項１乃至８のうちいずれか１項記載のロボット制御装置であって、
前記ロボットは、
前記複数のリンクと、該各リンクを回転可能に連結する複数の関節と、を有する指機構と、
前記各関節を駆動する複数のモータと、
前記各関節角度を検出する複数の関節角度検出部と、
を備えるロボットハンドである、ことを特徴とするロボット制御装置。
【請求項１０】
請求項１乃至９のうちいずれか１項記載のロボット制御装置であって、
前記慣性モーメント演算手段は、（２１）式を用いて前記第ｉ関節周りの慣性モーメントＪｉを算出し、
前記慣性モーメント推定手段は、（２７）式を用いて前記第ｉ関節周りの慣性モーメント推定値Ｊｉ（ハット）を算出し、
前記推定誤差演算手段は、（２９）式を用いて前記第ｉ関節周りの慣性モーメント誤差ｅＪ，ｉを算出する、ことを特徴とするロボット装置。
【数１】
0005732993.tif 000044
但し、上記（２１）式、（２７）式、及び（２９）式において、下記式が成立し、ｌｍは第ｍリンク長さ、ｍｉは第ｉリンク質量、ａｍ、ｎは第ｍ関節と第ｎ関節を結ぶ線分の長さ、ｂｍ、ｎは第ｍリンク重心第ｎ関節間距離、φｍ、ｎは第ｍ関節において第ｍ＋１リンクと第ｍ関節と第ｎ関節を結ぶ線分が成す角度、θｍは第ｍ関節角度、Ｊｊ、ｉは第ｊリンクの第ｉ関節に関する慣性モーメント、Ｊｊ０は第ｊリンクの重心周り慣性モーメント、Ｔｉは第ｉ関節モータトルク、Ｄｉは第ｉ関節粘性摩擦、Ｔは制御周期、ｋは各制御周期の値を示すインデックス、とする。
【数２】
0005732993.tif 000045
【請求項１１】
複数のリンクと、該各リンクを回転可能に連結する複数の関節と、該各関節を駆動する複数の駆動手段と、前記各関節角度を検出する複数の関節角度検出手段と、を備えたロボット制御装置の制御方法であって、
前記検出された先端側の第１関節から第ｉ関節（ｉ≧２）までの関節角度を含む計算式に基づいて、該関節角度が実現された場合の前記第ｉ関節から先端部分までの第ｉ関節周りの慣性モーメントを運動学的に算出するステップと、
前記検出された前記第ｉ関節の関節角度のみを含む運動方程式に基づいて、前記第ｉ関節の回転軸が鉛直方向となるときの前記第ｉ関節周りの慣性モーメント推定値を動力学的に算出するステップと、
前記算出された第ｉ関節周りの慣性モーメントと、前記算出された第ｉ関節周りの慣性モーメント推定値と、の差を前記第ｉ関節周りの慣性モーメント誤差として算出するステップと、
前記算出された第ｉ関節周りの慣性モーメント誤差の絶対値と判定閾値とを比較して、前記関節角度検出手段の故障を判定するステップと、
前記判定された故障の判定結果と、ロボットが実施しようとしている作業パターンと、に基づいて、前記各関節に対する関節角度指令を生成するステップと、
を含む、ことを特徴とするロボット制御装置の制御方法。
【請求項１２】
複数のリンクと、該各リンクを回転可能に連結する複数の関節と、該各関節を駆動する複数の駆動手段と、前記各関節角度を検出する複数の関節角度検出手段と、を備えたロボット制御装置の制御プログラムであって、
前記検出された先端側の第１関節から第ｉ関節（ｉ≧２）までの関節角度を含む計算式に基づいて、該関節角度が実現された場合の前記第ｉ関節から先端部分までの第ｉ関節周りの慣性モーメントを運動学的に算出する処理と、
前記検出された前記第ｉ関節の関節角度のみを含む運動方程式に基づいて、前記第ｉ関節の回転軸が鉛直方向となるときの前記第ｉ関節周りの慣性モーメント推定値を動力学的に算出する処理と、
前記算出された第ｉ関節周りの慣性モーメントと、前記算出された第ｉ関節周りの慣性モーメント推定値と、の差を前記第ｉ関節周りの慣性モーメント誤差として算出する処理と、
前記算出された第ｉ関節周りの慣性モーメント誤差の絶対値と判定閾値とを比較して、前記関節角度検出手段の故障を判定する処理と、
前記判定された故障の判定結果と、ロボットが実施しようとしている作業パターンと、に基づいて、前記各関節に対する関節角度指令を生成する処理と、
をコンピュータに実行させる、ことを特徴とするロボット制御装置の制御プログラム。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は、ロボットのセンサに故障が生じた場合でも最適に制御を行うことができるロボット制御装置、その制御方法及び制御プログラムに関するものである。
【背景技術】
【０００２】
近年、複数のセンサを用いて車両や航空機などの自動制御を行い、センサなどに故障が生じてもその制御を継続できるように構成された自動制御システムの研究開発が行われている。例えば、フィードバック線形化及び状態推定器を用いて、航空機の故障した構成部材を識別し、故障した構成部材を使用することなく、動作を継続させる自動制御システムが知られている（特許文献１参照）。
【先行技術文献】
【特許文献】
【０００３】
【特許文献１】特表平１１−５０７４５４号公報
【発明の概要】
【発明が解決しようとする課題】
【０００４】
上記特許文献１に示す自動制御システムをロボットに適用した場合、例えば、ロボットに設けられたセンサに故障が生じ、正常なセンサが既知でない場合、フィードバック線形化及び状態推定の両方が実施不能となる虞がある。また、上記自動制御システムは、航空機を制御するため、多数のセンサを冗長的に備えている。したがって、ロボットにそのセンサ構成を適用すると、質量及びサイズが大きくなるという問題も生じる。
【０００５】
本発明は、このような問題点を解決するためになされたものであり、ロボットのセンサに故障が生じた場合でも最適に制御を行うことができるロボット制御装置、その制御方法及び制御プログラムを提供することを主たる目的とする。
【課題を解決するための手段】
【０００６】
上記目的を達成するための本発明の一態様は、複数のリンクと、該各リンクを回転可能に連結する複数の関節と、該各関節を駆動する複数の駆動手段と、前記各関節角度を検出する複数の関節角度検出手段と、を備えたロボット制御装置であって、前記各関節角度検出手段により検出された先端側の第１関節から第ｉ関節（ｉ≧２）までの関節角度を含む関数に基づいて、前記第ｉ関節から先端部分までの第ｉ関節周りの慣性モーメントを算出する慣性モーメント演算手段と、前記関節角度検出手段により検出された前記第ｉ関節の関節角度を含む関数に基づいて、前記第ｉ関節の回転軸が鉛直方向となるときの前記第ｉ関節周りの慣性モーメント推定値を算出する慣性モーメント推定手段と、前記慣性モーメント演算手段により算出された前記第ｉ関節周りの慣性モーメントと、前記慣性モーメント推定手段により算出された前記第ｉ関節周りの慣性モーメント推定値と、の差を前記第ｉ関節周りの慣性モーメント誤差として算出する推定誤差演算手段と、前記推定誤差演算手段により算出された前記第ｉ関節周りの慣性モーメント誤差の絶対値と判定閾値とを比較して、前記関節角度検出手段の故障を判定する故障判定手段と、前記故障判定手段により判定された故障の判定結果と、ロボットが実施しようとしている作業パターンと、に基づいて、前記各関節に対する関節角度指令を生成する関節角度指令生成手段と、を備える、ことを特徴とするロボット制御装置である。この一態様によれば、ロボットのセンサに故障が生じた場合でも最適に制御を行うことができる。
【０００７】
この一態様において、前記故障判定手段は、前記推定誤差演算手段により算出された前記第ｉ関節周りの慣性モーメント誤差の絶対値が判定閾値を超えるとき、前記複数の関節角度検出手段の関節角度のうちの一つ関節角度を０度に保持して、前記第ｉ関節周りの慣性モーメント誤差の絶対値と判定閾値との比較を繰返し、前記関節角度検出手段の故障を判定してもよい。これにより、関節角度検出手段を冗長的に設けることなく、各関節の回転軸が鉛直方向を向いた瞬間に、各関節角度検出手段が故障しているか否かを瞬時に判定できる。
【０００８】
この一態様において、前記故障判定手段は、前記複数の関節角度のうちの一つ関節角度を０度に保持して、前記第ｉ関節周りの慣性モーメント誤差の絶対値が判定閾値以下となるとき、前記関節角度を０度に保持した前記関節角度検出手段を故障、それ以外の前記関節角度検出手段を正常、と判定し、前記第ｉ関節周りの慣性モーメント誤差の絶対値が判定閾値を超えるとき、前記関節角度を０度に保持した前記関節角度検出手段を不明、それ以外の前記関節角度検出手段を故障、と判定し、該故障と判定した関節角度検出手段に対応する関節角度を０度に保持し、前記第ｉ関節周りの慣性モーメント誤差の絶対値と判定閾値との比較を繰り返してもよい。これにより、各関節角度検出手段が故障しているか否か不明の場合でも、その故障を瞬時に判定できる。
【０００９】
この一態様において、前記関節角度指令生成手段は、前記故障判定手段により判定された故障の判定結果と、ロボットが実施しようとしている作業パターンと、に基づいて、前記作業パターンが実施可能か否かを判断し、実施不能と判断した場合、前記作業パターンの作業を中止するための関節角度指令を生成し、実施可能と判断した場合、故障した関節角度検出手段が設置された関節以外に対する関節角度指令を生成してもよい。これにより、故障した関節角度検出手段が存在する場合でも、実施可否を適切に判断し、作業を継続できる。
【００１０】
この一態様において、前記故障判定手段からの判定結果に基づいて、故障した前記関節角度検出手段が無いと判断した場合、全関節における前記関節角度検出手段の関節角度が前記関節角度指令生成手段により生成された関節角度指令に追従するような前記駆動手段に対するトルク指令を生成し、前記故障判定手段からの故障判定結果に基づいて、故障した前記関節角度検出手段が存在すると判断した場合、故障していない前記関節角度検出手段の関節角度が前記関節角度指令生成手段により生成された関節角度指令に追従するように前記駆動手段に対するトルク指令を生成し、かつ、故障した前記関節角度検出手段が設けられた関節の駆動手段に対して前記作業内容毎に予め設定されたトルク指令を生成する、トルク指令生成手段を更に備えていてもよい。これにより、故障した関節角度検出手段が存在する場合でも、トルク指令を適切に生成できる。
【００１１】
この一態様において、ユーザの入力情報又は予め設定されたプログラムに基づいて、ロボットが作業を実施する作業パターンを生成する作業パターン生成手段と、前記作業パターン生成手段により生成された前記作業パターンを分類し、予め設定された前記分類に対応する値を作業分類パラメータに設定するパラメータ生成手段と、前記関節角度指令生成手段からの各関節角度指令と、前記パラメータ生成手段からの前記作業分類パラメータと、前記故障判定手段からの故障の判定結果と、に基づいて、前記駆動手段に対するトルク指令を生成するトルク指令生成手段と、を更に備えていてもよい。
【００１２】
この一態様において、前記トルク指令生成手段は、（ａ）前記故障判定手段からの判定結果に基づいて、前記関節角度検出器が故障していると判断し、前記パラメータ生成手段からの作業分類パラメータに第１所定値が設定されていると判断したとき、所定トルクを生じさせるトルク指令を生成し、（ｂ）前記故障判定手段からの判定結果に基づいて、前記関節角度検出器が故障していると判断し、前記パラメータ生成手段からの作業分類パラメータに第２所定値が設定されていると判断したとき、トルク指令に０を設定し、（ｃ）前記故障判定手段からの判定結果に基づいて、前記関節角度検出器が故障していない、或いは不明と判断したとき、前記作業パターン毎に予め設定されたトルク指令を生成してもよい。これにより、故障した関節角度検出手段を使用せずに安全に作業を継続した場合に、その作業の種類に応じて作業方法の変更、中止などを実施できる。
【００１３】
この一態様において、前記パラメータ生成手段は、前記作業パターン生成手段により生成された作業パターンに基づいて、ロボットが実施しようとしている作業がロボットハンドの指を伸ばすように外側に向かって反力を発生させると判断したとき、第１所定値を前記作業分類パラメータに設定し、ロボットが実施しようとしている作業がロボットハンドの指を外側に向かって反力を発生させないと判断したとき、第２所定値を前記作業分類パラメータに設定してもよい。
【００１４】
この一態様において、前記ロボットは、前記複数のリンクと、該各リンクを回転可能に連結する複数の関節と、を有する指機構と、前記各関節を駆動する複数のモータと、前記各関節角度を検出する複数の関節角度検出部と、を備えるロボットハンドであってもよい。
【００１５】
この一態様において、前記慣性モーメント演算手段は、前記関数として下記（２１）式を用いて前記第ｉ関節周りの慣性モーメントＪｉを算出し、前記慣性モーメント推定手段は、前記関数として下記（２７）式を用いて前記第ｉ関節周りの慣性モーメント推定値Ｊｉ（ハット）を算出し、前記推定誤差演算手段は、下記（２９）式を用いて前記第ｉ関節周りの慣性モーメント誤差ｅＪ，ｉを算出してもよい。
【００１６】
他方、上記目的を達成するための本発明の一態様は、複数のリンクと、該各リンクを回転可能に連結する複数の関節と、該各関節を駆動する複数の駆動手段と、前記各関節角度を検出する複数の関節角度検出手段と、を備えたロボット制御装置の制御方法であって、前記検出された先端側の第１関節から第ｉ関節（ｉ≧２）までの関節角度を含む関数に基づいて、前記第ｉ関節から先端部分までの第ｉ関節周りの慣性モーメントを算出するステップと、前記検出された前記第ｉ関節の関節角度を含む関数に基づいて、前記第ｉ関節の回転軸が鉛直方向となるときの前記第ｉ関節周りの慣性モーメント推定値を算出するステップと、前記算出された第ｉ関節周りの慣性モーメントと、前記算出された第ｉ関節周りの慣性モーメント推定値と、の差を前記第ｉ関節周りの慣性モーメント誤差として算出するステップと、前記算出された第ｉ関節周りの慣性モーメント誤差の絶対値と判定閾値とを比較して、前記関節角度検出手段の故障を判定するステップと、前記判定された故障の判定結果と、ロボットが実施しようとしている作業パターンと、に基づいて、前記各関節に対する関節角度指令を生成するステップと、を含む、ことを特徴とするロボット制御装置の制御方法であってもよい。
【００１７】
また、上記目的を達成するための本発明の一態様は、複数のリンクと、該各リンクを回転可能に連結する複数の関節と、該各関節を駆動する複数の駆動手段と、前記各関節角度を検出する複数の関節角度検出手段と、を備えたロボット制御装置の制御プログラムであって、前記検出された先端側の第１関節から第ｉ関節（ｉ≧２）までの関節角度を含む関数に基づいて、前記第ｉ関節から先端部分までの第ｉ関節周りの慣性モーメントを算出する処理と、前記検出された前記第ｉ関節の関節角度を含む関数に基づいて、前記第ｉ関節の回転軸が鉛直方向となるときの前記第ｉ関節周りの慣性モーメント推定値を算出する処理と、前記算出された第ｉ関節周りの慣性モーメントと、前記算出された第ｉ関節周りの慣性モーメント推定値と、の差を前記第ｉ関節周りの慣性モーメント誤差として算出する処理と、前記算出された第ｉ関節周りの慣性モーメント誤差の絶対値と判定閾値とを比較して、前記関節角度検出手段の故障を判定する処理と、前記判定された故障の判定結果と、ロボットが実施しようとしている作業パターンと、に基づいて、前記各関節に対する関節角度指令を生成する処理と、をコンピュータに実行させる、ことを特徴とするロボット制御装置の制御プログラムであってもよい。
【発明の効果】
【００１８】
本発明によれば、ロボットのセンサに故障が生じた場合でも最適に制御を行うことができるロボット制御装置、その制御方法及び制御プログラムを提供することができる。
【図面の簡単な説明】
【００１９】
【図１】本発明の実施の形態に係るロボット制御装置の概略的なシステム構成を示すブロック図である。
【図２】本発明の実施の形態に係るロボット制御装置の故障判定部によるｉ＝２における関節角度検出器の故障を判定するための処理フローを示すフローチャートである。
【図３】第１関節角度検出器及び第２関節角度検出器のうちいずれが故障しているか否かが分かっている場合の関節角度検出器の故障を判定するための処理フローを示すフローチャートである。
【図４】（ａ）本発明の実施の形態に係るロボットハンドの指機構を示す摸式図である。（ｂ）本発明の実施の形態に係るロボットハンドの指機構を示す摸式図である。
【図５】（ａ）本発明の実施の形態に係るロボット制御装置のシミュレーションにおける第２関節の関節角度の時間変化を示す図である。（ｂ）従来のロボット制御装置のシミュレーションにおける第２関節の関節角度の時間変化を示す図である。
【図６】（ａ）本発明の実施の形態に係るロボット制御装置のシミュレーションにおける第１関節の関節角度の０．０１５[ｓ]までの時間変化を示す図である。（ｂ）本発明の実施の形態に係るロボット制御装置のシミュレーションにおける第１関節の関節角度の０．５[ｓ]までの時間変化を示す図である。
【図７】（ａ）本発明の実施の形態に係るロボット制御装置のシミュレーションにおける第３関節の関節角度の０．０１５[ｓ]までの時間変化を示す図である。（ｂ）本発明の実施の形態に係るロボット制御装置のシミュレーションにおける第３関節の関節角度の０．５[ｓ]までの時間変化を示す図である。
【図８】本発明の実施の形態に係るロボット制御装置のシミュレーションにおける第２関節周り慣性モーメント推定誤差の時間変化を示す図である。
【図９】故障した関節角度検出器が設けられている第２関節を制御するための第２関節のモータトルク指令Ｔ２の時間変化を示す図である。
【発明を実施するための形態】
【００２０】
以下、図面を参照して本発明の実施の形態について説明する。本発明の一実施の形態に係るロボット制御装置１は、例えば、図４に示すようなロボットハンド１３０の指機構１３１を制御するものである。
【００２１】
ロボットハンド１３０の指機構１３１は、図４（ａ）及び（ｂ）に示す如く、第１リンク４０１と、第１リンク４０１に連結された第１関節４０２と、・・・・、第ｉ−２関節４０３と、第ｉ−２関節４０３に連結された第ｉ−１リンク４０４と、第ｉ−１リンク４０４に連結された第ｉ−１関節４０５と、第ｉ−１関節４０５に連結された第ｉリンク４０６と、第ｉリンク４０６に連結された第ｉ関節４０７と、第ｉ−３リンク４０８と、第ｉ−３リンク４０８に連結された第ｉ−３関節４０９と、第ｉ−３関節４０９に連結された第ｉ−２リンク４１０と、を有している。以下、指機構１３１の先端に一番近い第１関節から数えてｉ番目の関節を第ｉ関節とし、同様に、指機構１３１の先端に一番近い第１リンクから数えてｉ番目のリンクを第ｉリンクとする。
【００２２】
第１関節４０２、・・・・、第ｉ−２関節４０３、第ｉ−１関節４０５、第ｉ関節４０７、及び第ｉ−３関節４０９には、駆動手段の一具体例であり、夫々、各関節を駆動するモータ１３２が夫々設けられており、各関節はこの各モータ１３２の発生するモータトルクにより回転駆動される。また、各関節には、関節角度検出手段の一具体例であり、各モータ１３２により駆動された各関節の回転角度を検出し、関節角度検出値として出力する関節角度検出器１３３が夫々設けられている。
【００２３】
ここで、ｉは正の整数であり、例えば、人差し指の指先から第３関節までをモデル化する場合はｉ＝３となる。なお、図４（ａ）及び（ｂ）において各記号を以下のように定義する。
【００２４】
ａｍ、ｎ：第ｍ関節と第ｎ関節を結ぶ線分の長さ（以下、「第ｍ第ｎ関節間距離」と称す）
φｍ、ｎ：第ｍ関節において、第ｍ＋１リンクと第ｍ関節と第ｎ関節を結ぶ線分が成す角度（以下、「第ｍ第ｎ関節間線分第ｍ＋１リンク間角度」と称す）
θｍ：第ｍ関節角度
まず、第ｍ第ｎ関節間距離ａｍ，ｎと、第ｍ第ｎ関節間線分第ｍ＋１リンク間角度φｍ、ｎを導出する。図４（ａ）において、下記（１）式及び（２）式が成立する。
【００２５】
【数１】
0005732993.tif 000002
【数２】
0005732993.tif 000003
【００２６】
ただし、上記（１）式及び（２）式において、ｌｉは第ｉリンク４０６の長さを表す。
上記（１）式を第ｉ−２第ｉ関節間距離ａｉ−２、ｉについて解くと、下記（３）式を得ることができる。
【数３】
0005732993.tif 000004
【００２７】
上記（２）式を第ｉ−２第ｉ関節間線分第ｉ−１リンク間角度φｉ−２、ｉについて解くと、下記（４）式を得ることができる。
【数４】
0005732993.tif 000005
【００２８】
図４（ｂ）において、下記（５）式及び（６）式が成立する。
【数５】
0005732993.tif 000006
【数６】
0005732993.tif 000007
【００２９】
上記（５）式を第ｉ−３第ｉ関節間距離ａｉ−３、ｉについて解くと、下記（７）式を得ることができる。
【数７】
0005732993.tif 000008
【００３０】
上記（６）式を第ｉ−３第ｉ関節間線分第ｉ−２リンク間角度φｉ−３、ｉについて解くと、下記（８）式を得ることができる。
【数８】
0005732993.tif 000009
【００３１】
同様に、ｊ＝３、…、ｉ−１に対して、下記（９）式及び（１０）式が成立する。
【数９】
0005732993.tif 000010
【数１０】
0005732993.tif 000011
【００３２】
上記（９）式を第ｉ−ｊ第ｉ関節間距離ａｉ−ｊ、ｉについて解くと、下記（１１）式を得ることができる。
【数１１】
0005732993.tif 000012
【００３３】
上記（１０）式を第ｉ−ｊ第ｉ関節間線分第ｉ−ｊ＋１リンク間角度φｉ−ｊ、ｉについて解くと、下記（１２）式を得ることができる。
【数１２】
0005732993.tif 000013
【００３４】
第２リンクと第３リンクに対して下記（１３）式及び（１４）式が成立する。
【数１３】
0005732993.tif 000014
【数１４】
0005732993.tif 000015
【００３５】
上記（１３）式を第１第ｉ関節間距離ａ１、ｉについて解くと、下記（１５）式を得ることができる。
【数１５】
0005732993.tif 000016
【００３６】
上記（１４）式を第１第ｉ関節間線分第２リンク間角度φ１、ｉについて解くと、下記（１６）式を得ることができる。
【数１６】
0005732993.tif 000017
【００３７】
上記（３）式、（４）式、（７）式、（８）式、（１１）式、（１２）式、（１５）式、及び（１６）式において算出された第ｍ第ｎ関節間距離ａｍ，ｎと、第ｍ第ｎ関節間線分第ｍ＋１リンク間角度φｍ、ｎと、を用いて、第ｍリンクの重心と第ｎ関節の間の距離（以下、「第ｍリンク重心第ｎ関節間距離」と称す）ｂｍ、ｎを導出する。
図４（ａ）において第ｉリンク重心第ｉ関節間距離ｂｉ、ｉの２乗は、下記（１７）式として導出できる。
【数１７】
0005732993.tif 000018
【００３８】
図４（ａ）において、第ｉ−１リンク重心第ｉ関節間距離ｂｉ−１、ｉの２乗は、下記（１８）式として導出できる。
【数１８】
0005732993.tif 000019
【００３９】
ｊ＝２、…、ｉ−１に対して第ｉ−ｊリンク重心第ｉ関節間距離ｂｉ−ｊ、ｉの２乗は、下記（１９）式として導出できる。
【数１９】
0005732993.tif 000020
【００４０】
上記（１７）式乃至（１９）式に基づいて導出された第ｍリンク重心第ｎ関節間距離の２乗を用いて、ｊ＝１、…、ｉに対する第ｊリンクの第ｉ関節に関する慣性モーメントＪｊ、ｉは、平行軸の定理より下記（２０）式として導出できる。
【数２０】
0005732993.tif 000021
【００４１】
ただし、上記（２０）式において、Ｊｊ０は、第ｊリンクの重心周り慣性モーメントを表している。第ｉ関節から先端部分までの慣性モーメント（以下、「第ｉ関節周り慣性モーメント」と称す）Ｊｉは、上記（２０）式を用いて下記（２１）式として表される。
【数２１】
0005732993.tif 000022
【００４２】
上記（２１）式を用いて算出される第ｉ関節周り慣性モーメントＪｉは、第１関節４０２から第ｉ−１関節４０７までのｉ−１個全ての関節角度検出値に基づいている。後述の故障判定部１４０の慣性モーメント演算器１４１は、第１関節４０２から第ｉ関節４０７までの関節角度検出値を含む上記（２１）式（関数）を用いて、第ｉ関節周りの慣性モーメントＪｉ（ｉ＝１、２、…）を演算する。
【００４３】
次に、第ｉ関節角度検出値θｉのみに基づいて第ｉ関節周り慣性モーメントＪｉを推定する方法について説明する。第ｉ関節４０７周りの運動方程式は、下記（２２）式として求められる。
【数２２】
0005732993.tif 000023
ただし、上記（２２）式において各記号は以下のように定義されている。
Ｔｉ：第ｉ関節モータトルク
ｄｉ：第ｉ関節トルク外乱
Ｄｉ：第ｉ関節粘性摩擦
【００４４】
上記（２２）式における第ｉ関節角度θｉの１階時間微分値である第ｉ関節角速度と、２階時間微分である第ｉ関節角加速度とは、夫々、下記（２３）式及び（２４）式のように差分近似できる。
【数２３】
0005732993.tif 000024
【数２４】
0005732993.tif 000025
【００４５】
ただし、上記（２３）式及び（２４）式における各記号は、以下のように定義されている。
Ｔ：制御周期
ｋ：各制御周期の値を示すインデックス（ｋ番目の制御周期における値を表す）
【００４６】
上記（２３）式及び（２４）式を上記（２２）式に代入すると、下記（２５）式を得ることができる。
【数２５】
0005732993.tif 000026
【００４７】
上記（２５）式を第ｉ関節周り慣性モーメントＪｉについて解くと、第ｉ関節周り慣性モーメント推定値Ｊｉ（ハット）は下記（２６）式として表される。
【数２６】
0005732993.tif 000027
【００４８】
ただし、第ｉ関節粘性摩擦Ｄｉは設計により既知であるとする。また、第ｉ関節トルクの外乱ｄｉは、各関節軸が鉛直方向を向く瞬間にはｄｉ＝０となるため、各関節軸が鉛直方向を向く瞬間において、上記（２６）式は下記（２７）式と書き換えることができる。
【数２７】
0005732993.tif 000028
【００４９】
後述の故障判定部１４０の慣性モーメント推定器１４２は、第ｉ関節角度検出器１３３からの第ｉ関節角度検出値θｉを含む上記（２７）式（関数）を用いて、第ｉ関節周りの慣性モーメント推定値Ｊｉ（ハット）（ｉ＝１、２、…）を演算する。
また、上記（２７）式と上記（２１）式との差を、第ｉ関節周り慣性モーメント推定誤差ｅＪ、ｉとし、下記（２８）式を用いて算出することができる。
【数２８】
0005732993.tif 000029
【００５０】
上記（２８）式に、上記（１７）式乃至（２０）式を代入すると、下記（２９）式を得ることができる。
【数２９】
0005732993.tif 000030
【００５１】
ここで、上述より明らかなように、上記（２９）式の第１項は関節角度検出値θｉのみの関数であり、第２項以降は関節角度検出値θ１からθｉ−１にも依存する関数となっている。後述の故障判定部１４０の推定誤差演算器１４３は、上記（２９）式を用いて、第ｉ関節周りの慣性モーメント推定誤差ｅＪ、ｉ（ｉ＝１、２、…）を演算する。
【００５２】
本実施の形態に係るロボット制御装置１の故障判定部１４０は、例えば、上述したロボットハンド１３０の指機構１３１の各関節に設けられた各関節角度検出器１３３の故障を、この慣性モーメント推定誤差ｅＪ、ｉを用いて判定することができる。
【００５３】
図２は、本発明の実施の形態に係るロボット制御装置の故障判定部によるｉ＝２における関節角度検出器の故障を判定するための処理フローを示すフローチャートである。まず、ロボット制御装置１の故障判定部１４０は、ｉ＝２（第２関節までモデル化）において下記（３０）式を満足するか否かを判定する（ステップＳ２０１）。
【数３０】
0005732993.tif 000031
【００５４】
ただし、上記（３０）式において、δは関節角度検出器１３３の故障を判定するための故障判定閾値（判定閾値）であり、例えば、予め故障判定部１４０に設定されている。
【００５５】
故障判定部１４０は、上記（３０）式を満足すると判断した場合（ステップＳ２０１のＹＥＳ）、関節角度検出値θ１及びθ２は正常（第１関節角度検出器１３３と第２関節角度検出器１３３が故障していない）と判断し（ステップＳ２０２）、図３の（ステップＳ３０１）の処理に移行する。
【００５６】
一方、故障判定部１４０は、上記（３０）式を満足しないと判断した場合（ステップＳ２０１のＮＯ）、関節角度検出値θ１及びθ２のうち少なくとも一方が異常と判断し（ステップＳ２０３）、例えば、第１関節のモータ１３２にトルクを発生させ、関節角度検出値θ１＝０に保持し、関節角度検出値θ１＝０の状態で上記（３０）式を満足するか否かを判定する（ステップＳ２０４）。
【００５７】
故障判定部１４０は、上記（３０）式を満足すると判断した場合（ステップＳ２０４のＹＥＳ）、関節角度検出値θ１が異常（第１関節角度検出器１３３は故障している）、関節角度検出値θ２が正常（第２関節角度検出器１３３は故障していない）と判断し（ステップＳ２０５）、図３の（ステップＳ３０１）処理に移行する。
【００５８】
故障判定部１４０は、上記（３０）式を満足しないと判断した場合（ステップＳ２０４のＮＯ）、関節角度検出値θ１が正常か否か（第１関節角度検出器１３３が故障しているか否か）が不明であり、関節角度検出値θ２が異常（第２関節角度検出器１３３が故障している）と判断し（ステップＳ２０６）、第２関節のモータ１３２にトルクを発生させ、関節角度検出値θ２＝０に保持し、関節角度検出値θ２＝０の状態で上記（３０）式を満足するか否かを判定する（ステップＳ２０７）。
【００５９】
故障判定部１４０は、上記（３０）式を満足すると判断した場合（ステップＳ２０７のＹＥＳ）、関節角度検出値θ１が正常（第１関節角度検出器１３３は故障していない）、関節角度検出値θ２が異常（第２関節角度検出器１３３は故障している）と判断し（ステップＳ２０８）、図３の（ステップＳ３０１）の処理に移行する。
【００６０】
故障判定部１４０は、上記（３０）式を満足しないと判断した場合（ステップＳ２０７のＮＯ）、関節角度検出値θ１及びθ２が異常（第１関節角度検出器１３３及び第２関節角度検出器１３３が故障している）と判断し（ステップＳ２０９）、図３の（ステップＳ３０１）の処理に移行する。
【００６１】
図３は、第１関節角度検出器及び第２関節角度検出器のうちいずれが故障しているか否かが分かっている場合の関節角度検出器の故障を判定するための処理フローを示すフローチャートである。
【００６２】
故障判定部１４０は、ｉ＝ｊ、ｊ≧３において、異常と判断した関節角度検出値を上記（ステップＳ２０４）と同様に、その関節角度検出値を０に保持し、上記（３０）式を満足するか否かを判定する（ステップＳ３０１）。
【００６３】
故障判定部１４０は、上記（３０）式を満足すると判断した場合（ステップＳ３０１のＹＥＳ）、関節角度検出値θｊは正常（第ｊ関節角度検出器１３３は故障していない）と判断し（ステップＳ３０２）、上記（ステップＳ３０１）の処理に戻る。
【００６４】
一方、故障判定部１４０は、上記（３０）式を満足しないと判断した場合（ステップＳ３０１のＮＯ）、関節角度検出値θｊは異常（第ｊ関節角度検出器１３３が故障している）と判断し（ステップＳ３０３）、上記（ステップＳ３０１）の処理に戻る。
【００６５】
このようにして、動作に必要最小限のセンサのみを搭載した人と共生できる質量及びサイズのロボットにおいても、故障した関節角度検出器１３３を容易に特定することができる。
故障判定部１４０が、上述した図２及び図３に示す処理フローを用いて、全ての関節角度検出器１３３の故障判定を行い完了すると、安全制御部１２０は、その判定結果とロボットハンド１３０が実施しようとしている作業の種類に応じて、下記（３１）式に示す第ｊ関節のモータトルクＴｊを生成する。
【００６６】
【数３１】
0005732993.tif 000032
【数３２】
0005732993.tif 000033
【００６７】
ただし、上記（３１）式において、Ｔｊｒは、第ｊ関節角度検出器１３３が故障していない場合に、第ｊ関節のモータ１３２が生成するモータトルクの目標値（以下、「第ｊ関節モータトルク指令」と称す）である。また、ｓｊは、ロボットハンド１３０が実施しようとしている作業の種類を分類するためのパラメータ（以下、「作業分類パラメータ」と称す）であり、例えば、上記（３２）式のように設定されている。
【００６８】
作業分類パラメータｓｊが１（第１所定値）に設定されている場合、指機構１３１は外側に向かって反力を発生させ、一方、作業分類パラメータｓｊが０（第２所定値）に設定されている場合、指機構１３１は外側に向かって反力を発生させない。
【００６９】
上記（３１）式における３つのモータトルク指令Ｔｊは、例えば、以下のように定義できる。
（ａ）Ｔｊ＜０：第ｊ関節角度検出器１３３が故障しており、かつロボットハンド１３０が実施しようとしている作業において、指機構１３１が外側（指が伸びる方向）に向かって反力を発生する必要がある場合（ｓｊ＝１）。
【００７０】
この場合、第ｊ関節モータトルク指令Ｔｊを負トルク（所定トルク）に設定することにより、第ｊ関節が伸びた状態になり、指機構１３１が物体に接触した時に物体に対して反力を与えることができるようになる。
（ｂ）Ｔｊ＝０：第ｊ関節角度検出器１３３が故障しており、かつロボットハンド１３０が実施しようとしている作業において、指機構１３１が外側に向かって反力を発生する必要がない場合（ｓｊ＝０）。
【００７１】
この場合、第ｊ関節モータトルク指令Ｔｊを０に設定することにより、第ｊ関節は指機構１３１が物体により内側に押された場合は正の方向に（曲がる方向に）回転し、指機構１３１が物体から外側に押された場合は指機構１３１が伸びた状態になるまで負の方向に（伸びる方向に）回転する。いずれの場合も、第ｊ関節の可動範囲の端に到達したら物体からの力が同じ方向に加わる限りその角度のまま保持される。
（ｃ）Ｔｊ＝Ｔｊｒ：第ｊ関節角度検出器１３３が故障していない、又は故障しているか否か分からない場合。
【００７２】
この場合、ロボットハンド１３０の実施しようとしている作業に必要な第ｊ関節モータトルク指令Ｔｊ（作業パターン毎に予め設定された安全なモータトルク指令）を生成し、この第ｊ関節モータトルク指令Ｔｊに従って第ｊ関節を駆動する。
【００７３】
本実施の形態において、上記（３１）式により第ｊ関節モータトルクＴｊを生成することにより、第ｊ関節角度検出器１３３が故障している場合でも、第ｊ関節のモータ１３２の暴走を回避し、かつロボットハンド１３０が実施しようとしている作業を安全に継続できる。以下上記（３１）式に示すＴｊを「安全関節モータトルク指令」と称す。
【００７４】
図１は、本発明の実施の形態に係るロボット制御装置の概略的なシステム構成を示すブロック図である。本実施の形態に係るロボット制御装置１は、関節角度指令生成部１１０と、安全制御部１２０と、ロボットハンド１３０と、故障判定部１４０と、を備えている。
【００７５】
関節角度指令生成部１１０は、故障判定部１４０より出力された各関節角度検出器１３３の故障判定結果に基づいて、各関節を駆動するための関節角度指令を生成し、生成した各関節角度指令を安全制御部１２０に対して出力する。
【００７６】
関節角度指令生成部１１０は、作業パターン生成器１１１と、関節角度指令生成器１１２と、を有している。
【００７７】
作業パターン生成器１１１は、作業パターン生成手段の一具体例であり、例えば、ロボットハンド１３０のユーザによる入力情報、又は、当該ロボット制御装置１などに予め設定されたプログラムに従って、ロボットハンド１３０が次に実施する作業に必要な作業パターン（動作データ）を生成し、関節角度指令生成器１１２及び安全制御部１２０に対して出力する。
【００７８】
関節角度指令生成器１１２は、関節角度指令生成手段の一具体例であり、故障判定部１４０からの各関節角度検出器１３３の故障判定結果に基づいて、故障した関節角度検出器１３３が無いと判断した場合、作業パターン生成器１１１からの作業パターンに基づいて、その作業パターンが表す動作を実施するのに必要な関節角度指令を生成し、安全制御部１２０に対して出力する。
【００７９】
関節角度指令生成器１１２は、故障判定部１４０からの各関節角度検出器１３３の故障判定結果に基づいて、故障した関節角度検出器１３３が存在すると判断した場合、故障している関節角度検出器１３３の位置と、ロボットハンド１３０が実施しようとしている作業パターンと、に基づいて、その作業パターンが実施可能か否かを判定する。
【００８０】
関節角度指令生成器１１２は、その作業内容が実施不能と判定した場合、作業パターンの作業を中止するために必要な関節角度指令を生成し、安全制御部１２０に対して出力する。ここで、上記作業内容を中止するために必要な関節角度指令とは、例えば、故障していない関節角度検出器１３３のみを使ってロボットハンド１３０が安全な姿勢をとることができる関節角度指令である。
【００８１】
一方、関節角度指令生成部１１０は、その作業パターンが実施可能と判定した場合、故障した関節角度検出器１３３が配置された関節以外の関節に対する各関節角度指令を生成し、安全制御部１２０に対して出力する。
【００８２】
安全制御部１２０は、関節角度指令生成部１１０から関節角度指令と、故障判定部１４０からの各関節角度検出器１３３の故障判定結果と、ロボットハンド１３０の各関節角度検出器１３３からの関節角度検出値と、に基づいて、ロボットハンド１３０の各関節のモータ１３２を安全に駆動させるための安全関節モータトルク指令を生成し、ロボットハンド１３０のモータ１３２に対して出力する。ここで、安全関節モータトルク指令とは、例えば、ロボットハンド１３０の指機構１３１の全関節における各関節角度検出値が各関節角度指令に追従するような各関節のモータトルク指令である。
【００８３】
安全制御部１２０は、パラメータ生成器１２１と、モータトルク指令生成器１２２と、を有している。
【００８４】
パラメータ生成器１２１は、パラメータ生成手段の一具体例であり、関節角度指令生成部１１０の作業パターン生成器１１１からの作業パターンに基づいて、ロボットハンド１３０が実施しようとしている作業を各関節に求められる動作に分類して、作業分類パラメータｓｊ（ｊ＝１、２、…）を設定し、設定した作業パラメータｓｊをモータトルク指令生成器１２２に対して出力する。
【００８５】
モータトルク指令生成器１２２は、トルク指令生成手段の一具体例であり、関節角度指令生成部１１０からの各関節角度指令と、パラメータ生成器１２１からの作業分類パラメータｓｊと、故障判定部１４０からの各関節角度検出器１３３の故障判定結果と、に基づいて、上記（３１）式で表される安全関節モータトルク指令Ｔｊ（ｊ＝１、２、…）を生成し、ロボットハンド１３０の各モータ１３２に対して出力する。
【００８６】
例えば、モータトルク指令生成器１２２は、故障判定部１４０からの各関節角度検出器１３３の故障判定結果に基づいて、故障した関節角度検出器１３３が無いと判断した場合、ロボットハンド１３０の指機構１３１の全関節における関節角度検出器１３３の関節角度検出値が関節角度指令生成器１１２により生成された各関節角度指令に追従するような安全関節モータトルク指令Ｔｊを生成し、ロボットハンド１３０の各モータ１３２に対して出力する。
【００８７】
一方、モータトルク指令生成器１２２は、故障判定部１４０からの各関節角度検出器１３３の故障判定結果に基づいて、故障した関節角度検出器１３３が存在すると判断した場合、故障していない関節角度検出器１３３の関節角度検出値が関節角度指令生成器１１２により生成された関節角度指令に追従し、かつ故障した関節角度検出器１３３を備える関節のモータ１３２に対するモータトルクが作業パターン毎に設定された安全なモータトルクとなるような安全関節モータトルク指令Ｔｊを生成し、ロボットハンド１３０の各モータ１３２に対して出力する。ここで、モータトルク指令生成器１２２は、安全関節モータトルク指令Ｔｊをモータ電流に変換してロボットハンド１３０の各関節のモータ１３２に対して出力する。
【００８８】
ロボットハンド１３０は、指機構１３１と、安全制御部１２０のモータトルク指令生成器１２２からの安全関節モータトルク指令Ｔｊに基づいて各関節を駆動する複数のモータ１３２と、各モータ１３２により駆動された各関節の回転角度を検出し、関節角度検出値として出力する複数の関節角度検出器１３３と、を有している。指機構１３１は、複数のリンクと、隣合うリンクを回転可能に連結する複数の関節と、から構成される機構である。
【００８９】
故障判定部１４０は、ロボットハンド１３０の各関節角度検出器１３３からの関節角度検出値に基づいて、ロボットハンド１３０が物体に接触していない状態で、各関節の回転軸が鉛直方向を向いた瞬間に、故障した関節角度検出器１３３の有無およびその位置を判定し、その各関節角度検出器１３３の故障判定結果を、関節角度指令生成部１１０及び安全制御部１２０に対して出力する。
【００９０】
故障判定部１４０は、慣性モーメント演算器１４１と、慣性モーメント推定器１４２と、慣性モーメント推定誤差演算器１４３と、故障判定器１４４と、を有している。
【００９１】
慣性モーメント演算器１４１は、慣性モーメント演算手段の一具体例であり、各関節角度検出器１３３からの第１乃至第ｉ関節角度検出値と、上記（２１）式と、に基づいて、第ｉ関節周りの慣性モーメントＪｉ（ｉ＝１、２、…）を演算し、推定誤差演算器１４３に対して出力する。
【００９２】
慣性モーメント推定器１４２は、慣性モーメント推定手段の一具体例であり、各関節角度検出器１３３からの第ｉ関節角度検出値と、上記（２７）式と、に基づいて、第ｉ関節の回転軸が鉛直方向となるときの第ｉ関節周りの慣性モーメント推定値Ｊｉ（ハット）（ｉ＝１、２、…）を演算し、慣性モーメント推定誤差演算器１４３に対して出力する。
【００９３】
慣性モーメント推定誤差演算器１４３は、推定誤差演算手段の一具体例であり、慣性モーメント演算器１４１からの慣性モーメントＪｉと、慣性モーメント推定器１４２からの慣性モーメント推定値Ｊｉ（ハット）と、の差を、上記（２９）式を用いて、第ｉ関節周りの慣性モーメント推定誤差ｅＪ、ｉ（ｉ＝１、２、…）として演算し、故障判定器１４４に対して出力する。
【００９４】
故障判定器１４４は、故障判定手段の一具体例であり、慣性モーメント推定誤差演算器１４３からの慣性モーメント推定誤差ｅＪ、ｉと、上記（３０）式と、に基づいて、図２及び図３に示す処理フローに従って各関節角度検出器１３３が故障しているか否かを判定し、その各関節角度検出器１３３の故障判定結果を関節角度指令生成部１１０及び安全制御部１２０に対して出力する。
【００９５】
故障判定部１４０は、上述の各関節角度検出器１３３の判定演算を、ロボットハンド１３０の作業と作業の間、物体に接触する前に各関節の回転軸が鉛直方向を向いた瞬間に実施する。
【００９６】
このように、本実施の形態によれば、関節角度検出器１３３を冗長的に設けることなく、ロボットハンド１３０の作業と作業の間、物体に接触していない状態で各関節の回転軸が鉛直方向を向いた瞬間に、各関節角度検出器１３３が故障しているか否かを瞬時に判定できる。また、その判定結果に基づいて、故障した関節角度検出器１３３を使用することなく、ロボットハンド１３０に安全に次の作業を実施させるか、或いは中止させることができる。
【００９７】
次に、本実施の形態に係るロボット制御装置１を用いて、シミュレーションを行った結果について説明する。なお、本シミュレーションは、３つの関節と３つのリンクから構成されるロボットハンド１３０の人差し指において、第２関節角度検出器１３３のみが故障している場合に、人差し指と親指を使って複数のはがきを輪ゴムで止める作業を模擬している。また、シミュレーションに用いた数値は以下のように設定されている。
【００９８】
ｌ１＝２０×１０−３[ｍ]、ｌ２＝２０×１０−３[ｍ]、ｌ３＝２０×１０−３[ｍ]、ｌ１０＝１０×１０−３[ｍ]、ｌ２０＝１０×１０−３[ｍ]、ｌ３０＝１０×１０−３[ｍ]、ｍ１＝３０×１０−３[ｋｇ]、ｍ２＝３０×１０−３[ｋｇ]、ｍ３＝３０×１０−３[ｋｇ]、Ｊ１０＝１．４２×１０−６[ｋｇ・ｍ２]、Ｊ２０＝１．４２×１０−６[ｋｇ・ｍ２]、Ｊ３０＝１．４２×１０−６[ｋｇ・ｍ２]、Ｄ１＝１×１０−４[Ｎ・ｍ・ｓ／ｒａｄ]、Ｄ２＝１×１０−４[Ｎ・ｍ・ｓ／ｒａｄ]、Ｄ３＝１×１０−４[Ｎ・ｍ・ｓ／ｒａｄ]、θ１ｍｉｎ＝０［ｒａｄ］、θ１ｍａｘ＝π［ｒａｄ］、θ２ｍｉｎ＝０［ｒａｄ］、θ２ｍａｘ＝π［ｒａｄ］、θ３ｍｉｎ＝０［ｒａｄ］、θ３ｍａｘ＝π［ｒａｄ］、ｃｖ２＝−１×１０−１［ｒａｄ／ｓ］、Ｔ＝１×１０−３[ｓ]、Ｋｐ１＝４０[ｓ−１]、Ｋｖ１＝４０×２×π[ｓ−１]、Ｋｖｊ１＝Ｋｖ１×（Ｊ１０＋ｍ１×ｌ１０２）[Ｎ・ｍ２・ｓ−１]、Ｋｐ２＝４０[ｓ−１]、Ｋｖ２＝４０×２×π[ｓ−１]Ｋｖｊ２＝Ｋｖ２×（Ｊ２０＋ｍ２×ｌ２０２）[Ｎ・ｍ２・ｓ１]、Ｋｐ３＝４０[ｓ−１]、Ｋｖ３＝４０×２×π[ｓ−１]、Ｋｖｊ３＝Ｋｖ３×（Ｊ３０＋ｍ３×ｌ３０２）[Ｎ・ｍ２・ｓ−１]、Ｔ１ｃ＝−１×１０−１[Ｎ・ｍ]、Ｔ２ｃ＝−１×１０−１[Ｎ・ｍ]、Ｔ３ｃ＝−１×１０−１[Ｎ・ｍ]、θ１ｆ＝１×１０−１［ｒａｄ］、θ２ｆ＝１×１０−１［ｒａｄ］、θ３ｆ＝１×１０−１［ｒａｄ］、ｖ１ｃ＝５×１０−１［ｒａｄ／ｓ］、ｖ２ｃ＝５×１０−１［ｒａｄ／ｓ］、ｖ３ｃ＝５×１０−１［ｒａｄ／ｓ］、δ＝５×１０−３[ｋｇ・ｍ２]、
ｇ＝９．８［ｍ／ｓ−２］
ただし、上記各記号は、以下のように定義されている。
ｌｉ０：第ｉリンク重心と第ｉ関節の間の距離（以下、「第ｉリンク重心第ｉ関節間距離」と称す）
θｉｍｉｎ：第ｉ関節角度下限値、
θｉｍａｘ：第ｉ関節角度上限値、
ｃｖ２：第２関節速度検出値ドリフト量、
Ｋｐｉ：第ｉ関節角度比例制御ゲイン、
Ｋｖｉ：正規化第ｉ関節速度比例制御ゲイン、
Ｋｖｊｉ：第ｉ関節速度比例制御ゲイン、
Ｔｉｃ：第ｉ関節角度を第ｉ関節角度下限値に保持するための第ｉ関節のモータトルク（以下、「第ｉ関節角度下限値保持モータトルク」と称す）
θｉｆ：第ｉ関節角度検出器１３３が故障していない場合の第ｉ関節角度最終目標値（以下、「第ｉ関節角度最終目標値」と称す）
ｖｉｃ：第ｉ関節角度検出器１３３が故障していない場合の各制御周期における第ｉ関節速度指令変化量（以下、「第ｉ関節速度指令変化量」と称す）
ｇ：重力加速度
【００９９】
ロボットハンド１３０の指機構１３１の人差し指の運動方程式は、下記（３３）式乃至（３５）式として導出できる。
【数３３】
0005732993.tif 000034
【数３４】
0005732993.tif 000035
【数３５】
0005732993.tif 000036
【０１００】
ここで、上記（３３）乃至（３５）で表される指機構１３１の人差し指を、輪ゴムに通すために、各関節の回転軸は鉛直方向を向いている状態で各関節を少し曲げる動作を想定する。輪ゴムを広げる際に、人差し指の各リンクは外向きの反力を輪ゴムに発生させる必要があるため、作業分類パラメータは、ｓ１＝１、ｓ２＝１、ｓ３＝１となる。
【０１０１】
初期状態において、人差し指は伸びている状態（θ１＝０、θ２＝０、θ３＝０）であるとする。まず、第１関節角度検出器１３３乃至第３関節角度検出器１３３が故障しているか否か不明である。このため、上記（３６）式乃至（３８）式で表される第１関節のモータトルク指令Ｔ１ｒ、第２関節のモータトルク指令Ｔ２ｒ、及び第３関節のモータトルク指令Ｔ３ｒにより各関節角度を制御する。
【数３６】
0005732993.tif 000037
【数３７】
0005732993.tif 000038
【数３８】
0005732993.tif 000039
【０１０２】
上記（３６）式乃至（３８）式において、関節角度指令生成器１１２は、第１関節角度指令θ１ｒ、第２関節角度指令θ２ｒ、及び第３関節角度指令θ３ｒを、下記（３９）式乃至（４１）式の１階時間積分として演算する。
【数３９】
0005732993.tif 000040
【数４０】
0005732993.tif 000041
【数４１】
0005732993.tif 000042
【０１０３】
ただし、上記（３９）式乃至（４１）式において、ｃθｉは、故障した第ｉ関節角度検出器１３３の故障判定結果（１：故障しているか否か不明、０：故障していない、−１：故障している）とする。
【０１０４】
上記（３９）式乃至（４１）式により、第ｉ関節角度検出値θｉが第ｉ関節角度最終目標値θｉｆに到達するまで、第ｉ関節角度指令θｉｒは、制御周期ごとに第ｉ関節速度指令変化量ｖｉｃずつその１階時間微分値を増加させ、第ｉ関節角度最終目標値θｉｆに到達した時点でその値を保持する。
【０１０５】
以下、上述のように制御した人差し指の各関節角度検出器１３３の故障判定を、図５乃至図８を用いて説明する。
【０１０６】
図５（ａ）は、本実施の形態に係るロボット制御装置のシミュレーションにおける第２関節の関節角度の時間変化を示しており、図５（ｂ）は、従来のロボット制御装置のシミュレーションにおける第２関節の関節角度の時間変化を示している。図５（ａ）及び（ｂ）において、実線は実際の第２関節角度θ２、一点鎖線は第２関節角度検出値θ２ｍ、破線は第２関節角度指令θ２ｒを示している。
【０１０７】
図５（ａ）において、時間０．００３[ｓ]まで図２の（ステップＳ２０１）から（ステップＳ２０３）の演算が実施され、実際の第２関節角度θ１及びθ２のうち少なくとも一方が異常であることのみが分かっている。この間、故障している関節角度検出器１３３が特定できていないため、上記（４０）式の１番目の式に従って破線の第２関節角度指令θ２ｒは増加している。ここで、第２関節角度指令θ２ｒが階段状に変化するのは、第２関節角度指令θ２ｒが制御周期Ｔ＝１×１０−３[ｓ]の離散時間に従って生成されるためである。本実施の形態において第２関節角度検出器１３３は故障しており、下記（４２）式で表される誤った第２関節速度検出値θ２ｍの１回時間積分値を出力するものとする。
【数４２】
0005732993.tif 000043
【０１０８】
上記（４２）式で表される誤った第２関節角度検出値θ２ｍは破線が示すように変化する。すなわち、０．００３[ｓ]までは第２関節角度検出値θ２ｍを第２関節角度指令θ２ｒに収束させようとして実際の第２関節角度θ２を制御し、その結果、実際の第２関節角度θ２は実線のように増加する。
【０１０９】
図５（ａ）の０．００３[ｓ]において、図２の（ステップＳ２０４）から（ステップＳ２０６）の演算が実施され、第２関節角度検出器１３３の故障が検出され、（ステップＳ２０７）において上記（３１）式の１番目の式に基づいて安全関節モータトルクが出力され、実際の第２関節角度θ２＝０［ｒａｄ］に保持される。実際、０．００３[ｓ]以降において、実際の第２関節角度θ２は０［ｒａｄ］に保持されており、第２関節が伸びた状態で保持されていることが分かる。この間、第２関節角度検出器１３３は故障したままであるため、誤った第２関節角度検出値θ２ｍを出力し続けている。
【０１１０】
一方で、従来のロボット制御装置においては、図５（ｂ）に示す如く、実線で示す実際の第２関節角度θ２は発散しており、人差し指の第２関節が完全に折れるまで曲がり続けることが分かる。
【０１１１】
図６（ａ）は、本実施の形態に係るロボット制御装置のシミュレーションにおける第１関節の関節角度の０．０１５[ｓ]までの時間変化を示しており、図６（ｂ）は、本実施の形態に係るロボット制御装置のシミュレーションにおける第１関節の関節角度の０．５[ｓ]までの時間変化を示している。図６（ａ）及び（ｂ）において、実線は実際の第１関節角度θ１、破線は第１関節角度指令θ１ｒを示している。
【０１１２】
図６（ａ）の０．００６[ｓ]において、図２の（ステップＳ２０７）の演算が完了し、その後、（ステップＳ２０８）の演算が行われ、第１関節角度検出器１３３が故障していないと判定される。よって、０．００６[ｓ]以降も関節角度指令生成器１１２は、上記（３９）式の２番目の式に従って、図６（ａ）の破線で示す第１関節角度指令θ１ｒを出力しており、実際の第１関節角度θ１も第１関節角度指令θ１ｒに追従するように増加している。
【０１１３】
図６（ｂ）に示すように、破線で示す第１関節角度指令θ１ｒは０．２[ｓ]において第１関節角度最終目標値θ１ｆ＝０．１［ｒａｄ］に到達し、それ以降はθ１ｒ＝θ１ｆに保持されている。また、実線で示す実際の第１関節角度θ１は、上記（３１）式の３番目の式および上記（３６）式に基づいてモータトルク指令生成器１２２が出力する安全関節モータトルク指令により制御され、第１関節角度最終目標値θ１ｆに漸近的に収束している。なお、図６（ａ）及び（ｂ）において、第１関節角度検出値は実際の第１関節角度に一致している。
【０１１４】
図７（ａ）は、本実施の形態に係るロボット制御装置のシミュレーションにおける第３関節の関節角度の０．０１５[ｓ]までの時間変化を示しており、図７（ｂ）は、本実施の形態に係るロボット制御装置のシミュレーションにおける第３関節の関節角度の０．５[ｓ]までの時間変化を示している。図７（ａ）及び（ｂ）において、実線は実際の第３関節角度θ３、破線は第３関節角度指令θ３ｒを示している。
【０１１５】
図７（ａ）において、０．００９[ｓ]までは図２の（ステップＳ２０１）乃至（ステップＳ２０８）、及び図３の（ステップＳ３０１）（ｊ＝３とした場合）が実施される。その結果、０．００９[ｓ]で（ステップＳ３０２）において、故障判定器１４４は、第３関節角度検出器１３３が故障していないと判定する。したがって、０．００９[ｓ]以降も、関節角度指令生成器１１２は、上記（４１）式の２番目の式に基づいて、図７（ａ）の破線で示す第３関節角度指令θ３ｒを、モータトルク指令生成器１２２に対して出力する。これにより、実線で示す実際の第３関節角度θ３も第３関節角度指令θ３ｒに追従するように増加している。
【０１１６】
図７（ｂ）に示すように、破線で示す第３関節角度指令θ３ｒは、０．２[ｓ]において第３関節角度最終目標値θ３ｆ＝０．１［ｒａｄ］に到達し、それ以降は第３関節角度指令θ３ｒ＝第３関節角度最終目標値θ３ｆに保持されている。また、実線で示す実際の第３関節角度θ３は、上記（３１）式の３番目の式、及び上記（３８）式に基づいてモータトルク指令生成器１２２が出力する安全関節モータトルク指令により制御され、第３関節角度最終目標値θ３ｆに漸近的に収束している。
【０１１７】
図８は、本実施の形態に係るロボット制御装置のシミュレーションにおける第２関節周り慣性モーメント推定誤差の時間変化を示している。実線は、推定誤差演算器１４３により上記（２９）式に基づいて算出された第２関節周りの慣性モーメント推定誤差ｅＪ、２を示している。慣性モーメント推定誤差ｅＪ、２の絶対値は、０[ｓ]において実際の第２関節角度θ２＝０であるため、０[ｋｇ・ｍ２]から増加し、０．００３[ｓ]において関節角度検出器１３３の故障判定閾値δ＝５×１０−３[ｋｇ・ｍ２]を超える。このため、上述のように故障判定器１４４、図２の（ステップＳ２０６）の処理において、第２関節角度検出器１３３が故障していると判定する。
【０１１８】
次に、故障判定器１４４は、（ステップＳ２０７）の処理に移行し、このとき、図８に示す如く、慣性モーメント推定誤差ｅＪ、２は減少している。そして、０．００６[ｓ]において、故障判定器１４４は、上記（３０）式を満足したと判断するため（ステップＳ２０７のＹＥＳ）、（ステップＳ２０８）の処理に移行し、第１関節角度検出器１３３が故障していないと判定する。
【０１１９】
図９は、故障した関節角度検出器が設けられている第２関節を制御するための第２関節のモータトルク指令Ｔ２の時間変化を示す図である。
【０１２０】
図９に示す如く、０．００３[ｓ]までの間、故障判定器１４４は、図２の（ステップＳ２０４）の処理まで実施し、第１関節角度検出器１３３または第２関節角度検出器１３３の少なくとも１つが故障していることだけが判明している。このため、モータトルク指令生成器１２２は、上記（３１）式の３番目の式に基づいて、第２関節モータトルク指令Ｔ２＝Ｔ２ｒを第２関節のモータ１３２に対して出力している。
【０１２１】
また、０．００３[ｓ]以降において、故障判定器１４４は図２の（ステップＳ２０７）の処理を実行し、モータトルク指令生成器１２２は実際の第２関節角度θ２＝０に保持するように、上記（３１）式の１番目の式で表される安全関節モータトルク指令Ｔ２＝Ｔ２ｃを第２関節のモータ１３２に対して出力している。
【０１２２】
以上、本実施の形態に係るロボット制御装置１によれば、関節角度検出器１３３の冗長設計に頼ることなく、例えば、ロボットハンド１３０の作業間、物体に接触していない状態で各関節の回転軸が鉛直方向を向いた時に、各関節角度検出器１３３が故障しているか否かを瞬時に判定でき、その判定結果に基づいて故障した関節角度検出器１３３を使用することなく、ロボットハンド１３０に安全に次の作業を実施させ、或いは、作業を中止させることができる。すなわち、ロボットハンド１３０のセンサに故障が生じた場合でも最適に制御を行うことができる。
【０１２３】
また、特許文献１に示す従来技術においては、どの関節角度検出器が故障していないか不明な場合、一般的に、オブザーバーが使用する正しい関節角度検出値を出力し、可観測となるのに十分な関節角度検出器が特定できない。そのため故障した関節角度検出器を特定することができない。一方で、本実施の形態に係るロボット制御装置１によれば、故障している関節角度検出器１３３の位置や数によらず短時間かつ確実に故障した関節角度検出器１３３を特定できるメリットを有している。
【０１２４】
さらに、上述の理由から、特許文献１に示す従来技術によると、例えば図５（ａ）に示すように、故障したロボットハンドの指機構の関節が制御不能となり、次の作業を実施できなくなり、また、実施する作業の種類によっては周囲の物体や人に対して十分に安全でない可能性もある。これに対し、本実施の形態に係るロボット制御装置１によれば、図５（ｂ）に示すように、故障した関節角度検出器１３３の影響を受けないように制御を行うため、作業を継続することが可能となり、また実施する作業の種類によっては安全に作業を中止することもできる。
【０１２５】
なお、本発明は上記実施の形態に限られたものではなく、趣旨を逸脱しない範囲で適宜変更することが可能である。
【０１２６】
例えば、上記実施の形態において、上記（３９）式乃至（４１）式における第ｉ関節速度指令変化量ｖｉｃを一定値としているが、これに限らず、第ｉ関節角度が第ｉ関節角度指令に収束するような任意の可変量を適用することも可能である。
【０１２７】
上記実施の形態において、上記（３６）式乃至（３８）式における第ｉ関節モータトルク指令をフィードバック線形化制御によって算出しているが、これに限らず、その他の任意の制御則にしたがって算出してもよい。
【０１２８】
さらに、上記実施の形態において、ロボットハンド１３０の指機構１３１は３つのリンクと３つの関節を有する構成であるが、これに限らず、任意の構成が適用可能である。
【０１２９】
上記実施の形態において、輪ゴムを複数のはがきに掛ける作業の例を示したが、これに限らず、ロボットハンド１３０を使った任意の作業に適用可能である。
【０１３０】
上記実施の形態において、関節角度指令生成部１１０、安全制御部１２０、及び、故障判定部１４０は、マイコンに実装したソフトウエアとして実現することができるが、ＡＳＩＣなどハードウェアとして実現してもよい。本発明は、例えば、ＣＰＵにコンピュータプログラムを実行させることにより実現できる。
【０１３１】
プログラムは、様々なタイプの非一時的なコンピュータ可読媒体（non-transitory computer readable medium）を用いて格納され、コンピュータに供給することができる。非一時的なコンピュータ可読媒体は、様々なタイプの実体のある記録媒体（tangible storage medium）を含む。非一時的なコンピュータ可読媒体の例は、磁気記録媒体（例えばフレキシブルディスク、磁気テープ、ハードディスクドライブ）、光磁気記録媒体（例えば光磁気ディスク）、ＣＤ−ＲＯＭ、ＣＤ−Ｒ、ＣＤ−Ｒ／Ｗ、半導体メモリ（例えば、マスクＲＯＭ、ＰＲＯＭ（Programmable ROM）、ＥＰＲＯＭ（Erasable PROM）、フラッシュＲＯＭ、ＲＡＭ）を含む。
【０１３２】
また、プログラムは、様々なタイプの一時的なコンピュータ可読媒体（transitory computer readable medium）によってコンピュータに供給されてもよい。一時的なコンピュータ可読媒体の例は、電気信号、光信号、及び電磁波を含む。一時的なコンピュータ可読媒体は、電線及び光ファイバ等の有線通信路、又は無線通信路を介して、プログラムをコンピュータに供給できる。
【産業上の利用可能性】
【０１３３】
本発明は、各関節に設けられた関節角度検出器１３３の故障を考慮して、ロボットを安全かつ最適に制御できるため、例えば、小型軽量低価格でありかつ周囲の人や物に対して安全であることが要求される、家事手伝いロボット、介護ロボット、楽器演奏ロボット、事務手伝いロボットなど、ロボットハンドを備え、人と共生するサービスロボット全般に適用できる。
【符号の説明】
【０１３４】
１１０  関節角度指令生成部
１１１  作業パターン生成器
１１２  関節角度指令生成器
１２０  安全制御部
１２１  パラメータ生成器
１２２  モータトルク指令生成器
１３０  ロボットハンド
１３１  指機構
１３２  モータ
１３３  関節角度検出器
１４０  故障判定部
１４１  慣性モーメント演算器
１４２  慣性モーメント推定器
１４３  慣性モーメント推定誤差演算器
１４４  故障判定器
４０１  第１リンク
４０２  第１関節
４０３  第ｉ−２関節
４０４  第ｉ−１リンク
４０５  第ｉ−１関節
４０６  第ｉリンク
４０７  第ｉ関節
４０８  第ｉ−３リンク
４０９  第ｉ−３関節
４１０  第ｉ−２リンク
【図１】
0005732993.tif 000046
【図２】
0005732993.tif 000047
【図３】
0005732993.tif 000048
【図４】
0005732993.tif 000049
【図５】
0005732993.tif 000050
【図６】
0005732993.tif 000051
【図７】
0005732993.tif 000052
【図８】
0005732993.tif 000053
【図９】
0005732993.tif 000054