特許 6836544 制御システムおよび被駆動体の制御方法

(19)【発行国】日本国特許庁(JP)

(12)【公報種別】特許公報(B2)

(11)【特許番号】6836544

(24)【登録日】2021年2月9日

(45)【発行日】2021年3月3日

(54)【発明の名称】制御システムおよび被駆動体の制御方法

(51)【国際特許分類】

   B25J 13/08 20060101AFI20210222BHJP

【ＦＩ】

   B25J13/08 Z

【請求項の数】7

【全頁数】15

(21)【出願番号】特願2018-90496(P2018-90496)

(22)【出願日】2018年5月9日

(65)【公開番号】特開2019-195871(P2019-195871A)

(43)【公開日】2019年11月14日

【審査請求日】2019年10月8日

【早期審査対象出願】

(73)【特許権者】

【識別番号】390008235

【氏名又は名称】ファナック株式会社

(74)【代理人】

【識別番号】100118913

【弁理士】

【氏名又は名称】上田 邦生

(74)【代理人】

【識別番号】100142789

【弁理士】

【氏名又は名称】柳 順一郎

(74)【代理人】

【識別番号】100163050

【弁理士】

【氏名又は名称】小栗 眞由美

(74)【代理人】

【識別番号】100201466

【弁理士】

【氏名又は名称】竹内 邦彦

(72)【発明者】

【氏名】山岡 宣章

(72)【発明者】

【氏名】鈴木 元

【審査官】 石川 薫

(56)【参考文献】

【文献】 特開２０１５−１６３４１６（ＪＰ，Ａ）

(58)【調査した分野】（Int.Cl.，ＤＢ名）

Ｂ２５Ｊ １／００−２１／０２

(57)【特許請求の範囲】

【請求項1】

モータにより駆動される被駆動体と、
前記モータを制御して前記被駆動体を動作させる制御装置と、
前記被駆動体に取り付けられ取付位置における該被駆動体の状態量を検出するセンサとを備え、
前記制御装置が、
前記取付位置に生じる振動を補正するための振動補正量を記憶する記憶部と、
動作プログラムに基づいて制御周期毎に前記モータへの指令値を生成し、前記記憶部に記憶されている前記振動補正量および前記指令値に基づいて前記被駆動体の動作制御を行う動作制御部と、
該動作制御部による前記動作制御が行われたときに前記振動補正量を算出する補正量算出部と、
前記動作制御が行われる毎に前記振動補正量を更新する補正量更新部と、
前記動作制御が行われる毎に、前記指令値と前記状態量とに基づいて、前記センサの内部パラメータ、前記状態量、前記取付位置および姿勢の少なくとも１つの妥当性を判定する判定部とを備え、
該判定部が、前記センサによって検出された前記状態量と前記センサの前記内部パラメータとに基づいて算出された第１の時系列データと、前記指令値と前記センサの前記取付位置および姿勢の設定値とに基づいて算出された第２の時系列データとの積に基づいて相関値を算出し、該相関値と予め設定された閾値に基づいて前記妥当性を判定する制御システム。

【請求項2】

前記第１の時系列データが、前記動作制御が実行される毎に、前記センサによって検出された前記状態量の時系列データと前記センサの前記内部パラメータとに基づいて算出された前記センサの前記取付位置における物理量であり、
前記第２の時系列データが、前記指令値の時系列データと前記センサの前記取付位置および姿勢の設定値とに基づいて算出された前記センサの前記取付位置における物理量である請求項１に記載の制御システム。

【請求項3】

前記判定部が、前記妥当性がないと判定した場合に、前記センサの前記内部パラメータ、前記状態量、前記取付位置および姿勢の内、前記妥当性がないと判定した値を補正する請求項１または請求項２に記載の制御システム。

【請求項4】

前記判定部によって前記妥当性がないと判定された場合に、前記妥当性がない旨を報知する報知部を備える請求項１または請求項２に記載の制御システム。

【請求項5】

前記報知部が、前記センサの前記内部パラメータ、前記状態量、前記取付位置および姿勢の内、前記判定部によって前記妥当性がないと判定された値を報知する請求項４に記載の制御システム。

【請求項6】

前記センサが、加速度センサ、ジャイロセンサ、力センサ、レーザトラッカ、カメラおよびモーションキャプチャ装置のいずれかである請求項１から請求項５のいずれかに記載の制御システム。

【請求項7】

被駆動体に取り付けられたセンサにより検出された取付位置における状態量に基づいて、前記取付位置に生じる振動を補正するための振動補正量を算出するステップと、
算出された前記振動補正量を記憶するステップと、
動作プログラムに基づいて制御周期毎に前記被駆動体への指令値を生成し、記憶されている前記振動補正量および前記指令値に基づいて前記被駆動体の動作制御を行うステップと、
前記動作制御が行われる毎に前記振動補正量を更新するステップと、
前記動作制御が行われる毎に、前記指令値と前記状態量とに基づいて、前記センサの内部パラメータ、前記状態量、前記取付位置および姿勢の少なくとも１つの妥当性を判定するステップとを含み、
該妥当性を判定するステップが、前記センサによって検出された前記状態量と前記センサの前記内部パラメータとに基づいて算出された第１の時系列データと、前記指令値と前記センサの前記取付位置および姿勢の設定値とに基づいて算出された第２の時系列データとの積に基づいて相関値を算出し、該相関値と予め設定された閾値に基づいて前記妥当性を判定する被駆動体の制御方法。

【発明の詳細な説明】

【技術分野】

【0001】

本発明は、制御システムおよび被駆動体の制御方法に関する。

【背景技術】

【0002】

ロボットを動作させるときのロボットのアームに発生する振動およびねじれを補正する制御方法が知られている（例えば、特許文献１参照。）。特許文献１に記載された制御方法では、アームを駆動させるモータの回転角度を検出するエンコーダの検出値と、アームに配置されたジャイロセンサの検出値との差分が算出される。算出された差分を考慮した制御が行われることによってねじれが補正される。補正される値は、ロボットが動作するごとに学習制御が行われるため、その都度更新されていく。

【先行技術文献】

【特許文献】

【0003】

【特許文献1】特開２０１３−１４６８２７号公報

【発明の概要】

【発明が解決しようとする課題】

【0004】

しかし、特許文献１に記載された技術を用いて振動やねじれを補正するためには、ジャイロセンサの検出値と、ジャイロセンサのキャリブレーションデータ等の内部パラメータと、アームに対するジャイロセンサの取付位置および向きに関連する外部パラメータとが、適切に設定されている必要がある。これらが適切に設定されていない場合には、算出された補正値を用いても、アームに発生する振動やねじれを補正できない場合がある。その結果、補正値を用いたロボットの制御を行う前に、上記の値が適切に設定されているかを判定する必要があった。

【0005】

本発明は、振動防止のために用いられるセンサが適切に設定されていない状態のままで被駆動体の動作制御が行われることを防止することができる制御システムおよび被駆動体の制御方法を提供することを目的とする。

【課題を解決するための手段】

【0006】

本発明の一態様は、モータにより駆動される被駆動体と、前記モータを制御して前記被駆動体を動作させる制御装置と、前記被駆動体に取り付けられ取付位置における該被駆動体の状態量を検出するセンサとを備え、前記制御装置が、前記取付位置に生じる振動を補正するための振動補正量を記憶する記憶部と、動作プログラムに基づいて制御周期毎に前記モータへの指令値を生成し、前記記憶部に記憶されている前記振動補正量および前記指令値に基づいて前記被駆動体の動作制御を行う動作制御部と、該動作制御部による前記動作制御が行われたときに前記振動補正量を算出する補正量算出部と、前記動作制御が行われる毎に前記振動補正量を更新する補正量更新部と、前記動作制御が行われる毎に、前記指令値と前記状態量とに基づいて、前記センサの内部パラメータ、前記状態量、前記取付位置および姿勢の少なくとも１つの妥当性を判定する判定部とを備え、該判定部が、前記センサによって検出された前記状態量と前記センサの前記内部パラメータとに基づいて算出された第１の時系列データと、前記指令値と前記センサの前記取付位置および姿勢の設定値とに基づいて算出された第２の時系列データとの積に基づいて相関値を算出し、該相関値と予め設定された閾値に基づいて前記妥当性を判定する制御システムを提供する。

【0007】

本態様によれば、動作制御部により動作プログラムに基づいて制御周期毎に指令値が生成され、生成された指令値および記憶部に記憶されている振動補正量に基づいて被駆動体の動作制御が行われる。動作制御部による動作制御が行われる毎に、被駆動体に取り付けられているセンサにより取付位置における被駆動体の状態量が検出され、補正量算出部により振動補正量が算出され、補正量更新部により更新される。そして、指令値と状態量とに基づいて、判定部によりセンサの内部パラメータ、状態量、取付位置および姿勢の少なくとも１つの妥当性が判定される。

【0008】

すなわち、センサの内部パラメータ、状態量、取付位置および姿勢が妥当である場合には、被駆動体の振動を抑制する方向に振動補正量が更新されていく一方、妥当でない場合には、振動の抑制が効果的に行われないが、センサにより検出される状態量と動作プログラムに基づいて生成される指令値とに基づいて妥当性が判定されるので、センサの内部パラメータ、状態量、取付位置および姿勢の少なくとも１つが妥当でないことを迅速に認識することができる。これにより、センサの内部パラメータ、状態量、取付位置および姿勢のいずれかが妥当ではない状態で被駆動体が制御し続けられることを防止することができる。

【0009】

上記態様においては、前記判定部が、前記動作制御が実行される毎に、前記センサによって検出された前記状態量の時系列データと前記センサの前記内部パラメータとに基づいて算出された前記センサの前記取付位置における物理量の第１の時系列データと、前記指令値の時系列データと前記センサの前記取付位置および姿勢の設定値とに基づいて算出された前記センサの前記取付位置における物理量の第２の時系列データとを用いて相関値を算出し、算出した該相関値に基づいて前記妥当性を判定してもよい。

【0010】

この構成により、センサの取付位置における物理量が、２つの異なるパラメータに基づいて第１の時系列データと第２の時系列データとして算出される。この２つのパラメータに基づく相関値を比較することにより、センサの内部パラメータ、状態量、取付位置および姿勢の少なくとも１つが妥当でないことを簡単に認識できる。

【0011】

上記態様においては、前記判定部が、前記妥当性がないと判定した場合に、前記センサの前記内部パラメータ、前記状態量、前記取付位置および姿勢の内、前記妥当性がないと判定した値を補正してもよい。
この構成により、正しく設定されていないパラメータ等が特定されて補正されるため、速やかにセンサの内部パラメータ、状態量、取付位置および姿勢を適切に設定できる。

【0012】

上記態様においては、前記判定部によって前記妥当性がないと判定された場合に、前記妥当性がない旨を報知する報知部を備えていてもよい。
この構成により、報知を受けたユーザは、センサの内部パラメータ、状態量、取付位置および姿勢のいずれかが妥当ではない状態で被駆動体が制御し続けられることを防止することができる。

【0013】

上記態様においては、前記報知部が、前記センサの前記内部パラメータ、前記状態量、前記取付位置および姿勢の内、前記判定部によって前記妥当性がないと判定された値を報知してもよい。
この構成により、ユーザは、適切な値のみから算出したセンサの内部パラメータ、状態量、取付位置および姿勢を用いることにより、適切な振動補正量を得ることができる。

【0014】

上記態様においては、前記センサが、加速度センサ、ジャイロセンサ、力センサ、レーザトラッカ、カメラおよびモーションキャプチャ装置のいずれかであってもよい。

【0015】

本発明の他の一態様は、被駆動体に取り付けられたセンサにより検出された取付位置における状態量に基づいて、前記取付位置に生じる振動を補正するための振動補正量を算出するステップと、算出された前記振動補正量を記憶するステップと、動作プログラムに基づいて制御周期毎に前記被駆動体への指令値を生成し、記憶されている前記振動補正量および前記指令値に基づいて前記被駆動体の動作制御を行うステップと、前記動作制御が行われる毎に前記振動補正量を更新するステップと、前記動作制御が行われる毎に、前記指令値と前記状態量とに基づいて、前記センサの内部パラメータ、前記状態量、前記取付位置および姿勢の少なくとも１つの妥当性を判定するステップとを含み、該妥当性を判定するステップが、前記センサによって検出された前記状態量と前記センサの前記内部パラメータとに基づいて算出された第１の時系列データと、前記指令値と前記センサの前記取付位置および姿勢の設定値とに基づいて算出された第２の時系列データとの積に基づいて相関値を算出し、該相関値と予め設定された閾値に基づいて前記妥当性を判定する被駆動体の制御方法を提供する。

【0016】

本態様によれば、センサの内部パラメータ、状態量、取付位置および姿勢の妥当性があると判定された場合には、被駆動体の振動を抑制する方向に振動補正量が更新されていく。一方で、妥当性がないと判定された場合には、状態量と、動作プログラムに基づく指令値とに基づいて妥当性が判定されているので、センサの内部パラメータ、状態量、取付位置および姿勢の少なくとも１つが妥当でないことを認識できる。これにより、センサの内部パラメータ、状態量、取付位置および姿勢のいずれかが妥当ではない状態で被駆動体が制御し続けられることを防止することができる。

【発明の効果】

【0017】

本発明によれば、振動防止のために用いられるセンサが適切に設定されていない状態のままで被駆動体の動作制御が行われることを防止することができるという効果を奏する。

【図面の簡単な説明】

【0018】

【図1】本発明の制御システムの概略図である。

【図2】第１加速度データ、第２加速度データおよび相関値の算出方法のフローチャートである。

【図3】加速度センサの検出値および第２加速度データの一例を示すグラフである。

【図4】図３のデータから算出された修正加速度の一例を示すグラフである。

【図5】図４のデータから算出された第１加速度データの一例を示すグラフである。

【図6】図５のデータから算出された振動補正量の一例を示すグラフである。

【図7】第１加速度データと第２加速度データとから算出された相関値と、予め設定された閾値との関係の一例を示すグラフである。

【図8】比較例における加速度センサの検出値および第２加速度データの一例を示すグラフである。

【図9】図８に示された比較例における第１加速度データと第２加速度データとから算出された相関値と、予め設定された閾値との関係の一例を示すグラフである。

【図10】比較例における加速度センサの検出値および第２加速度データの一例を示すグラフである。

【図11】図１０に示された比較例における第１加速度データと第２加速度データとから算出された相関値と、予め設定された閾値との関係の一例を示すグラフである。

【図12】比較例における加速度センサの検出値および第２加速度データの一例を示すグラフである。

【図13】図１２に示された比較例における第１加速度データと第２加速度データとから算出された相関値と、予め設定された閾値との関係の一例を示すグラフである。

【図14】ロボットの制御方法のフローチャートである。

【図15】変形例における第１加速度データ、第２加速度データおよび相関値の算出方法のフローチャートである。

【図16】変形例における第１加速度データ、第２加速度データおよび相関値の算出方法のフローチャートである。

【発明を実施するための形態】

【0019】

本発明の実施形態に係る制御システム１００について、図面を参照しながら以下に説明する。
図１は、制御システム１００を説明するための概略図である。制御システム１００は、６つの関節軸を有する垂直多関節型ロボット（被駆動体：以降、単に「ロボット」と呼ぶ）１０と、ロボット１０の動作を制御する制御装置２０と、ロボット１０の先端に取り付けられた加速度センサ（センサ）ＳＮとを備えている。

【0020】

ロボット１０は、先端位置に作業対象のワークに作業を行うためのエンドエフェクタＥＦを有している。加速度センサＳＮは、エンドエフェクタＥＦに取り付けられ、取付位置であるエンドエフェクタＥＦの加速度（状態量）を検出する。ロボット１０の各関節軸は、制御装置２０によって制御されているモータの駆動によって、各回転軸回りに駆動する。各モータには、モータの回転角度を検出するエンコーダが配置されている。ロボット１０には、ワールド座標系Ｃｗが設定されており、エンドエフェクタＥＦには、インタフェイス座標系Ｃｉ（以降、「センサ座標系Ｃｉ」とも呼ぶ）が設定されている。

【0021】

制御装置２０は、各種制御を実行する制御部２９と、ユーザの操作を受け付ける操作部２６と、ロボット１０の制御に関連する情報を画像として表示可能なモニタ（報知部）２７と、警告通知などの音を出力するスピーカ（報知部）２８とを備えている。制御部２９は、図１に図示されていないＣＰＵ（Central Processing Unit）と、ＲＯＭ（Read Only
Memory）と、ＲＡＭ（Random Access Memory）と、メモリとで構成されている。ＣＰＵが、ＲＯＭに格納されたプログラムを、ＲＡＭとメモリとに展開することにより、制御部２９は、各プログラム機能を実行できる。操作部２６は、キーボードおよびマウスで構成されている。

【0022】

図１に示されるように、制御部２９は、ロボット１０の動作プログラムおよびエンドエフェクタＥＦが動作した際にエンドエフェクタＥＦに発生する振動を補正するための振動補正量を記憶する記憶部２５と、記憶部２５から読み出した動作プログラムに応じて生成した指令値に基づいてモータを駆動させる動作制御部２１と、振動補正量を算出する補正量算出部２３と、動作プログラムが実行される毎に算出される振動補正量を更新する補正量更新部２４と、動作プログラムが実行される毎に加速度センサＳＮのキャリブレーションデータ等の内部パラメータを用いて内部パラメータ等が適切であるか否かを判定する判定部２２とを備えている。

【0023】

補正量算出部２３は、動作プログラムが実行される毎に振動補正量を算出するために、加速度センサＳＮの検出値と、加速度センサＳＮの内部パラメータとを用いて、センサ加速度Ａｓ１を算出する。

【0024】

補正量算出部２３は、図２に示されるように、動作プログラムに基づく指令値を変換することによって、エンドエフェクタＥＦの位置データを算出する。補正量算出部２３は、算出したエンドエフェクタＥＦの位置データに、センサ座標系Ｃｉを使用することによって、加速度センサＳＮの位置データである第２位置データを算出する。補正量算出部２３は、算出した第２位置データを２回微分することによって、第２加速度データ（第２の時系列データ）Ａｃを算出する。

【0025】

補正量算出部２３は、算出した第２加速度データＡｃと、加速度センサＳＮによって検出された図３に示されるセンサ加速度Ａｓ１とを順に加工することによって、振動補正量を算出する。図３に示されるように、センサ加速度Ａｓ１は、第２加速度データＡｃと比較して、重力加速度の分およびエンドエフェクタＥＦにおける振動の分に対応する差分を有している。

【0026】

補正量算出部２３は、センサ加速度Ａｓ１から重力加速度を差し引いて、図４に示される修正加速度Ａｓ２の時系列データを算出する。その後、補正量算出部２３は、修正加速度Ａｓ２に対してローパスフィルタをかけて電気的なノイズを除去する。補正量算出部２３は、ノイズが除去された加速度に対して、図２に示されるように、加速度センサＳＮのキャリブレーションデータを使用することによって、第１加速度データ（第１の時系列データ）Ａｓ３を算出する。補正量算出部２３は、図５に示される第１加速度データＡｓ３から、第２加速度データＡｃを差し引いた差分を、図６に示される振動補正量Ａｄ１として算出し、振動補正量Ａｄ１を記憶部２５に記憶させる。

【0027】

補正量更新部２４は、動作プログラムが実行される毎に補正量算出部２３によって算出された振動補正量Ａｄ１を、記憶部２５に記憶された新たな振動補正量として更新する。

【0028】

図１に示される判定部２２は、記憶部２５に記憶された振動補正量が下記３点について適切であるか否かの妥当性を判定する。
１ 加速度センサＳＮの内部パラメータの設定値
２ エンドエフェクタＥＦに対する加速度センサＳＮの取付位置および姿勢の設定値
３ 加速度センサＳＮの検出値
なお、以降では、上記３点の値の総称として、単に「上記内部パラメータ等」を用いる。

【0029】

判定部２２は、動作プログラムに基づいて各モータが駆動すると、補正量算出部２３により算出された第１加速度データＡｓ３および第２加速度データＡｃを用いて、下記式（１）に示される相関値ｃを算出する。判定部２２は、算出した相関値ｃに基づいて上記内部パラメータ等についての妥当性を判定する。

【0030】

【数1】

ａ_ｃｋ：各時刻における第２加速度データＡｃ
ａ_ｓｋ：各時刻における第１加速度データＡｓ３

【0031】

上記式（１）に示されるように、第１加速度データＡｓ３と、第２加速度データＡｃとの差分が小さいほど、相関値ｃは１に近づく。判定部２２は、相関値ｃを含む下記式（２）を用いて、上記内部パラメータ等の妥当性を判定する。

【0032】

【数2】

【0033】

判定部２２は、式（２）の左辺が予め設定された閾値ε以下の場合に、上記内部パラメータ等の妥当性があると判定する。一方で、判定部２２は、式（２）の左辺が閾値εを超える場合に、上記内部パラメータ等の妥当性がないと判定する。

【0034】

図５に示される第１加速度データＡｓ３および第２加速度データＡｃとを用いて算出された相関値ｃを上記式（２）に代入すると、図７に示される時系列データが得られる。図７に示されるように、この場合の相関値ｃから１を差し引いた値は、−εから＋εまでの範囲に含まれている。よって、判定部２２は、この場合の上記内部パラメータ等の妥当性があると判定する。なお、図７における時間軸である横軸のスケールは、指令値が変動している期間に対応している。すなわち、図７の横軸のスケールは、指令値の変動が完了した後の指令値の加速度がゼロになった後の時系列データを含んでおらず、図３−６の横軸全体のスケールと異なる。

【0035】

一方で、判定部２２が上記内部パラメータ等の妥当性がないと判定する場合には、妥当性がない原因として、例えば、下記の３つの事象が考えられる。
Ａ 加速度センサＳＮのキャリブレーションデータが不適切な場合
Ｂ 加速度センサＳＮの取付方向の設定値が不適切な場合
Ｃ 加速度センサＳＮが異常値を検出している場合

【0036】

図８には、妥当性がないと判定される事象Ａの場合に、加速度センサＳＮによって検出された比較例のセンサ加速度Ａｓ１ａが示されている。なお、図８の縦軸および横軸のスケールは、図３−６のスケールと同じであり、以降で説明する図１０，１２のスケールとも同じである。図８に示されるセンサ加速度Ａｓ１ａと、図３に示されるセンサ加速度Ａｓ１とを比較すると、センサ加速度Ａｓ１ａが、実際よりも重力加速度を小さく検出されている。センサ加速度Ａｓ１ａを用いた場合の上記式（２）の時系列データは、図９に示され、＋εよりも大きい値および−εよりも小さい値を含んでいる。なお、図９の縦軸及び横軸のスケールは、図７のスケールと同じであり、以降で説明する図１１，１３のスケールとも同じである。

【0037】

図１０には、妥当性がないと判定される事象Ｂの場合に、加速度センサＳＮによって検出された別の比較例のセンサ加速度Ａｓ１ｂが示されている。図１０に示されるセンサ加速度Ａｓ１ｂと、図３に示されるセンサ加速度Ａｓ１とを比較すると、センサ加速度Ａｓ１ｂの検出方向が、実際の方向と正反対になっている。センサ加速度Ａｓ１ｂを用いた事象Ｂの場合の上記式（２）の時系列データは、図１１に示されるように、全域において−εよりも小さい値で推移している。

【0038】

図１２には、妥当性がないと判定される事象Ｃの場合に、加速度センサＳＮによって検出された別の比較例のセンサ加速度Ａｓ１ｃが示されている。図１２に示されるセンサ加速度Ａｓ１ｃと、図３に示されるセンサ加速度Ａｓ１とを比較すると、センサ加速度Ａｓ１ｃが、指令値の振幅が最も小さい時点付近で瞬間的な異常値を含んでいる。センサ加速度Ａｓ１ｃを用いた事象Ｃの場合の上記式（２）の時系列データは、図１３に示されるように、センサ加速度Ａｓ１ｃが異常値を含む前後で−εよりも小さい値を含んでいる。

【0039】

ここで、実際に行われるロボット１０の制御方法について、図１４に示されるフローチャートに沿って説明する。初めに、動作制御部２１は、記憶部２５に記憶された動作プログラムと、前回の動作プログラム実行時に記憶部２５に記憶された振動補正量を読み出す（ステップＳ１）。動作制御部２１は、読み出した動作プログラムおよび振動補正量に基づき、各モータを駆動させる動作制御を実行する（ステップＳ２）。

【0040】

補正量算出部２３は、動作制御の実行時に検出された加速度センサＳＮの検出値と、動作プログラムの指令値とに基づいて、振動補正量Ａｄ１を算出する（ステップＳ３）。補正量更新部２４は、補正量算出部２３によって算出された振動補正量Ａｄ１を、新たな振動補正量として記憶部２５に記憶させる（ステップＳ４）。

【0041】

次に、判定部２２は、加速度センサＳＮの検出値と、指令値とに基づいて相関値ｃを算出する。判定部２２は、算出した相関値ｃを式（２）に代入することによって、上記内部パラメータ等の妥当性を判定する（ステップＳ５）。

【0042】

判定部２２により妥当性がないと判定された場合には（ステップＳ６：ＮＯ）、モニタ２７およびスピーカ２８によって妥当性がない旨の報知が行われる（ステップＳ７）。具体的な報知として、モニタ２７によって妥当性がない旨の画像が表示され、かつ、スピーカ２８によって警告音が出力される。

【0043】

ステップＳ７の処理が行われる、または、ステップＳ６の処理において判定部２２によって妥当性があると判定されると（ステップＳ６：ＹＥＳ）、動作制御を終了するか否かが判定され（ステップＳ８）、終了しない場合には（ステップＳ８：ＮＯ）、ステップＳ２からの処理が繰り返される。

【0044】

本実施形態に係る制御システム１００の作用について以下に説明する。
本実施形態に係る制御システム１００によれば、動作制御部２１が、動作プログラムの指令値と、記憶部２５に記憶された前回の振動補正量とを用いて、動作制御を実行する。補正量算出部２３は、動作制御により検出された加速度センサＳＮの検出値と、指令値とを用いて振動補正量Ａｄ１を算出する。算出された振動補正量Ａｄ１は、補正量更新部２４によって、新たな振動補正量として毎回記憶部２５に記憶される。振動補正量Ａｄ１が算出されると同時に、判定部２２は、加速度センサＳＮの検出値と、指令値とに基づく相関値ｃを用いて、加速度センサＳＮの検出値、加速度センサＳＮの内部パラメータの設定値、および、エンドエフェクタＥＦに対する加速度センサの取付位置および姿勢の少なくとも１つの妥当性を判定する。

【0045】

すなわち、上記内部パラメータ等の妥当性がある場合には、算出された振動補正量Ａｄ１によってエンドエフェクタＥＦの振動が抑制される。一方で、妥当性がない場合には、上記内部パラメータ等の内のいずれかが適切に設定されていないことがユーザに認識される。ユーザは、適切に設定されていないパラメータ等を修正した後に、エンドエフェクタＥＦの振動を抑制するための適切な振動補正量Ａｄ１を求めることができる。また、妥当性がない場合には、算出された振動補正量Ａｄ１を用いて動作制御を行わないことによって、振動が発散した場合などにエンドエフェクタＥＦに発生し得る事故および損傷を未然に防止することができる。

【0046】

上記実施形態では、判定部２２が、算出した第１加速度データＡｓ３と、第２加速度データＡｃとを用いて相関値ｃを算出したが、変形例では、加速度データの代わりに図１５に示される第１速度データおよび第２速度データ等の他の物理量を用いてもよい。また、図１６に示されるように、実施形態の加速度データの代わりに、第１位置データおよび第２位置データが用いられて相関値ｃが算出されてもよい。

【0047】

上記実施形態では、判定部２２が、相関値ｃを含む式（２）の左辺と閾値εとを比較することによって上記内部パラメータ等の妥当性を判定したが、変形例では、上記実施形態の相関値ｃとは異なる相関値によって妥当性を判定してもよい。例えば、判定部２２は、式（３）に示されているａ_ｃｋおよびａ_ｓｋの積である相関値ｃ２の正負を判定することによって上記内部パラメータ等の妥当性を判定してもよい。また、判定部２２は、相関値ｃ２と、相関値ｃとを組み合わせて妥当性を判定してもよい。

【0048】

【数3】

【0049】

上記実施形態では、判定部２２は、上記内部パラメータ等の妥当性を判定していたが、さらに、妥当性がないと判定した場合に、上記内部パラメータ等の適切な設定値を算出してもよい。適切な設定値の算出方法の一例として、判定部２２は、初めに、図１に示される加速度センサＳＮの検出値が、重力加速度を加味して（Ｘ，Ｙ，Ｚ）＝（−９．８ｍ／ｓ^２，０，０）になるように、キャリブレーションデータを書き換える。その後、判定部２２は、再度、相関値ｃを算出して、妥当性を判定する。

【0050】

別の例として、エンドエフェクタＥＦに対する加速度センサＳＮの取付位置および姿勢が適切でない場合に、判定部２２は、下記式（４）を用いて、センサ座標系Ｃｉの向きを複数に変更することによって、適切なエンドエフェクタＥＦに対する加速度センサＳＮの姿勢を算出してもよい。

【0051】

【数4】

Ｗ_０，Ｐ_０，Ｒ_０：初期設定値
ｊ_１，ｊ_２，ｊ_３：−１７〜＋１７までの任意の整数

【0052】

判定部２２は、式（４）におけるｊ_１，ｊ_２，ｊ_３のそれぞれに３５通りの数字を代入することによって、式（４）から算出される３５^３通りのセンサ座標系Ｃｉのすべての相関値ｃを算出する。判定部２２は、算出した３５^３種類の相関値ｃの内、式（２）の左辺の最大値が最小である相関値ｃのｊ_１，ｊ_２，ｊ_３の組み合わせを仮のセンサ座標系Ｃｉの仮値（Ｗ_１，Ｐ_１，Ｒ_１）として設定する。これにより、１０ｄｅｇ刻みで、センサ座標系Ｃｉの大まかな適切な向きが決定する。

【0053】

次に、判定部２２は、センサ座標系Ｃｉの仮値（Ｗ_１，Ｐ_１，Ｒ_１）を基準として、下記式（５）を用いることによって、センサ座標系Ｃｉのより詳細な適切な向きを決定する。

【0054】

【数5】

ｊ_４，ｊ_５，ｊ_６：−９〜＋９までの任意の整数

【0055】

判定部２２は、式（５）におけるｊ_４，ｊ_５，ｊ_６のそれぞれに１９通りの数字を代入することによって、式（５）から算出される１９^３通りのセンサ座標系Ｃｉの全ての相関値ｃを算出する。判定部２２は、算出した１９^３種類の相関値ｃの内、式（２）の左辺の最大値が最小である相関値ｃのｊ_１，ｊ_２，ｊ_３の組み合わせを仮のセンサ座標系Ｃｉの仮値（Ｗ_１，Ｐ_１，Ｒ_１）として設定する。これにより、１ｄｅｇ刻みでセンサ座標系Ｃｉのより詳細な適切な向きが決定する。なお、判定部２２は、加速度センサＳＮの回転動作に対して同様の処理を行うことにより、加速度センサＳＮの取付位置についても適切な設定値を決定できる。

【0056】

上記実施形態および上記変形例では、判定部２２は、相関値ｃを用いて上記内部パラメータ等の妥当性を判定したが、それ以外の指標を用いて妥当性を判定してもよい。例えば、変形例の判定部２２は、第１加速度データＡｓ３および第２加速度データＡｃに、移動平均フィルタおよびメディアンフィルタのような平滑化フィルタを掛け、２つのデータの差分を指標として算出してもよい。判定部２２は、当該指標が予め設定された閾値を超える場合に、妥当性がないと判定してもよい。

【0057】

上記実施形態では、被駆動体として、ロボット１０を例に挙げて説明したが、被駆動体については、指令値に基づくモータ駆動によって動作する物体であればよい。また、被駆動体の状態量を検出するためのセンサとして、上記実施形態では、加速度センサＳＮを用いたが、他の実施形態では、例えば、ジャイロセンサ、力センサ、レーザトラッカ、カメラおよびモーションキャプチャ装置などであってもよい。指令値として、エンコーダの検出値が用いられてもよい。

【符号の説明】

【0058】

１０ ロボット（被駆動体）
２０ 制御装置
２１ 動作制御部
２２ 判定部
２３ 補正量算出部
２４ 補正量更新部
２５ 記憶部
２７ モニタ（報知部）
２８ スピーカ（報知部）
１００ 制御システム
Ａｓ３ 第１加速度データ（第１の時系列データ）
Ａｃ 第２加速度データ（第２の時系列データ）
Ａｄ１ 振動補正量
ＳＮ 加速度センサ（センサ）
ｃ，ｃ２ 相関値

【図1】

【図2】

【図3】

【図4】

【図5】

【図6】

【図7】

【図8】

【図9】

【図10】

【図11】

【図12】

【図13】

【図14】

【図15】

【図16】