(57)【要約】  パラレルリンク機構の制御装置は、パラレルリンク機構の駆動を制御する駆動制御部（５）及びアクチュエータ（６ａ）を制御するための指令を駆動制御部（５）へ付与する指令部（１）を備える。指令部（１）は、パラレルリンク機構のベースからリンク結合部までの機械系を並進ばねで模擬し、リンク結合部からエンドエフェクタまでの機械系を１つの剛体で模擬した力学モデルによって、エンドエフェクタの位置に対応して変化する固有振動数の予測値をエンドエフェクタの補間位置毎に算出する固有振動数予測部（４）を備える。駆動制御部（５）は、固有振動数予測部（４）が算出したエンドエフェクタの補間位置毎の予測値列に従って抑制する振動数成分を補間位置毎に変化させるフィルタを備える。
2018029910.tif 000002
【特許請求の範囲】
【請求項１】
パラレルリンク機構を駆動するアクチュエータを制御する駆動制御部と、前記駆動制御部へアクチュエータの制御指令を付与する指令部と、を備えたパラレルリンク機構の制御装置において、
前記指令部は、前記パラレルリンク機構のベースからリンク結合部までの間の機械系を並進ばねで模擬し、前記リンク結合部からエンドエフェクタまでの機械系を１つの剛体で模擬した力学モデルによって、前記エンドエフェクタの補間位置毎に算出される前記パラレルリンク機構の固有振動数の予測値列を算出する固有振動数予測部を備え、
前記駆動制御部は、前記固有振動数予測部が算出した前記エンドエフェクタの補間位置毎の予測値列に従って、抑制する振動数成分を前記補間位置毎に変化させるフィルタを備えたことを特徴とするパラレルリンク機構の制御装置。
【請求項２】
前記フィルタは、前記エンドエフェクタを目標位置に制御する制御器から出力された信号を入力とし、前記駆動制御部の電力増幅器へ出力することを特徴とする請求項１に記載のパラレルリンク機構の制御装置。
【請求項３】
前記フィルタは、前記駆動制御部への指令信号を入力とし、前記エンドエフェクタを目標位置に制御する制御器へ出力することを特徴とする請求項１に記載のパラレルリンク機構の制御装置。
【請求項４】
前記指令部は、外部測定器で測定されたパラレルリンク機構の実測振動数からバネ定数を推定するバネ定数推定部を備えたことを特徴とする請求項１から３の何れか１項に記載のパラレルリンク機構の制御装置。
【請求項５】
前記指令部は、前記バネ定数推定部が推定したバネ定数の推定値を監視し、前記推定値の変化量によってアラーム信号を出力する特性変化検出部を備えたことを特徴とする請求項４に記載のパラレルリンク機構の制御装置。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は、複数のリンクが並列にジョイントによって連結されているパラレルリンク機構の制御装置に関する。
【背景技術】
【０００２】
パラレルリンク機構では、ベースからエンドエフェクタまでの間で、複数のリンクが並列にジョイントによって連結されている。一部または全てのリンクは、アクチュエータによって駆動され、エンドエフェクタを自由な位置、又は自由な姿勢に駆動制御する。【０００３】
下記特許文献１には、エンドエフェクタが原点位置から離れた姿勢の状態にあるときに、パラレルリンク機構の剛性の低下により、駆動時に振動が発生することが知見されている。【０００４】
上記の知見に対し、特許文献１に記載の制御装置では、機械剛性が低いほど、かつ、アクチュエータの送り速度が大きいほど、アクチュエータの応答性が低くなるように、制御系のパラメータ（以下「制御パラメータ」と称する）の値を変化させることで振動を抑制している。
【先行技術文献】
【特許文献】
【０００５】
【特許文献１】特開２００５−１９６４８８号公報
【発明の概要】
【発明が解決しようとする課題】
【０００６】
しかしながら、特許文献１の手法は、比例ゲイン、積分ゲイン、微分ゲイン、積分時間、微分時間、及び時定数などの応答性に関する制御パラメータの上限値及び下限値、並びに、当該制御パラメータの変化条件の調整が必要となる。応答性に関する制御パラメータの調整は、装置の立ち上げ時だけでなく、エンドエフェクタが変わる度に必要となるため、特許文献１の手法では、応答性に関する制御パラメータの調整工数が増大してしまうという課題があった。【０００７】
本発明は、上記に鑑みてなされたものであって、応答性に関する制御パラメータの調整工数を低減しつつ、パラレルリンク機構に発生しうる振動を抑制することができるパラレルリンク機構の制御装置を得ることを目的とする。
【課題を解決するための手段】
【０００８】
上述した課題を解決し、目的を達成するため、本発明に係るパラレルリンク機構の制御装置は、パラレルリンク機構を駆動するアクチュエータを制御する駆動制御部、及び前記駆動制御部へアクチュエータの制御指令を付与する指令部を備える。前記指令部は、前記パラレルリンク機構のベースからリンク結合部までの間の機械系を並進ばねで模擬し、前記リンク結合部からエンドエフェクタまでの機械系を１つの剛体で模擬した力学モデルによって、前記エンドエフェクタの補間位置毎に算出される前記パラレルリンク機構の固有振動数の予測値列を算出する固有振動数予測部を備える。また前記駆動制御部は、前記固有振動数予測部が算出した前記エンドエフェクタの補間位置毎の予測値列に従って、抑制する振動数成分を前記補間位置毎に変化させるフィルタを備える。
【発明の効果】
【０００９】
本発明によれば、応答性に関する制御パラメータの調整工数を低減しつつ、パラレルリンク機構に発生しうる振動を抑制することができるという効果を奏する。
【図面の簡単な説明】
【００１０】
【図１】実施の形態１に係るパラレルリンク機構の制御装置の機能構成を示すブロック図
【図２】実施の形態１の駆動制御部におけるサーボ系の機能構成を示すブロック図
【図３】実施の形態１の駆動制御部における図２とは異なるサーボ系の機能構成を示すブロック図
【図４】２リンクの回転型パラレルリンク機構の模式図
【図５】２リンクの回転型パラレルリンク機構の力学モデルを示す図
【図６】２リンクの回転型パラレルリンク機構においてエンドエフェクタが原点から離れた場合の力学モデルの説明図
【図７】２リンクの直動型パラレルリンク機構の模式図
【図８】２リンクの直動型パラレルリンク機構の力学モデルを示す図
【図９】実施の形態２に係るパラレルリンク機構の制御装置の機能構成を示すブロック図
【図１０】実施の形態３に係るパラレルリンク機構の制御装置の機能構成を示すブロック図
【図１１】実施の形態１から３における指令部の機能を実現するためのハードウェア構成の一例を示すブロック図
【発明を実施するための形態】
【００１１】
以下に、本発明の実施の形態に係るパラレルリンク機構の制御装置を図面に基づいて詳細に説明する。なお、以下の実施の形態により、本発明が限定されるものではない。
【００１２】
実施の形態１．
図１は、実施の形態１に係るパラレルリンク機構の制御装置（以下、適宜「制御装置」と略す）の機能構成を示すブロック図である。図１において、実施の形態１に係る制御装置は、パラレルリンク機構の駆動を制御する駆動制御部５と、アクチュエータ６ａを制御するための指令を駆動制御部５へ付与する指令部１と、を備える。
【００１３】
図１に示すように、指令部１は、補間位置座標計算部２及び逆機構変換部３を備える。補間位置座標計算部２は、エンドエフェクタの目標位置ＴＰに基づいて、エンドエフェクタの目標位置までの軌跡の補間位置座標ＣＰを算出する。逆機構変換部３は、エンドエフェクタの各補間位置座標ＣＰに対応するアクチュエータの目標位置指令ＬＰを算出して駆動制御部５に出力する。なお、アクチュエータとは、入力されたエネルギーを回転運動又は並進運動に変換する駆動装置の総称である。アクチュエータは、例えば、サーボモータ、リニアモータ、ステッピングモータなどを含む。
【００１４】
指令部１は、さらにエンドエフェクタの位置に対応して変化する固有振動数を算出する固有振動数予測部４を備える。固有振動数予測部４は、補間位置座標ＣＰ、及び補間位置座標ＣＰに対応する目標位置指令ＬＰを用いて、補間位置座標ＣＰに対応する固有振動数ｆｎを計算して駆動制御部５に出力する。固有振動数予測部４によって、エンドエフェクタの補間位置毎に固有振動数の予測値が算出される。なお、予測値は一つとは限らず、複数の予測値を算出してもよい。この一つまたは複数の予測値データを集めた集合体を「予測値データ列」もしくは単に「予測値列」と呼ぶ。
【００１５】
駆動制御部５は、目標位置指令ＬＰに従ってアクチュエータ６ａを駆動する。アクチュエータ６ａを駆動する際の駆動量は、アクチュエータ６ａに搭載された位置検出器である検出器６ｂからの出力によってフィードバック制御される。検出器６ｂは、アクチュエータ６ａにおける磁極位置を検出する。アクチュエータ６ａにおける磁極位置の情報を「モータ位置情報」と呼び、また、適宜「モータ位置情報ＥＰ」と表記する。駆動制御部５は、アクチュエータ６ａを目標位置指令ＬＰに制御する。すなわち、駆動制御部５は、目標位置指令ＬＰと、モータ位置情報ＥＰとに基づいてフィードバック制御を行う。駆動制御部５の制御によって、エンドエフェクタは所望の位置及び姿勢に制御される。
【００１６】
次に、実施の形態１の駆動制御部５におけるサーボ系の構成について図２を参照して説明する。駆動制御部５は、図２に示すように、フィルタ７、位置及び速度制御器８、並びに電力増幅器９を備えている。
【００１７】
フィルタ７には、逆機構変換部３が算出した目標位置指令ＬＰと、固有振動数予測部４が予測した固有振動数ｆｎとが入力される。フィルタ７は、任意の補間位置座標ＣＰに対応する目標位置指令ＬＰから当該補間位置座標ＣＰに対応する固有振動数ｆｎの成分を抑制する。この処理により、目標位置指令ＬＰは、目標位置指令ＬＰ’へと変換される。なお、目標位置指令ＬＰと目標位置指令ＬＰ’とを符号無しで区別する場合には、目標位置指令ＬＰを「第１の目標位置指令」と呼び、目標位置指令ＬＰ’を「第２の目標位置指令」と呼ぶ。
【００１８】
位置及び速度制御器８は、エンドエフェクタを目標位置に制御する制御器である。位置及び速度制御器８は、フィルタ７が生成した目標位置指令ＬＰ’と、検出器６ｂが検出したモータ位置情報ＥＰとに基づいて、電流指令値Ｉｃを演算する。位置及び速度制御器８が演算した電流指令値Ｉｃは、電力増幅器９に対して出力される。電力増幅器９は、電流指令値Ｉｃに基づいて、アクチュエータ６ａを駆動する。
【００１９】
図２に示すサーボ系のように、固有振動数ｆｎの成分を抑制するフィルタ機能がフィードバックループの外部にある場合は、目標位置指令ＬＰが目標位置指令ＬＰ’へと変換されると、目標位置へ到達するまでの時間が長くなる場合がある。
【００２０】
図３に示すサーボ系では、図２の構成において、フィルタ７と位置及び速度制御器８とを入れ替えて構成している。これにより、フィルタ７’は、フィードバックループ内に配置される。図３に示すサーボ系の動作を説明する。位置及び速度制御器８は、目標位置指令ＬＰと、検出器６ｂが検出したモータ位置情報ＥＰとに基づいて電流指令値Ｉｃを演算する。位置及び速度制御器８が演算した電流指令値Ｉｃは、フィルタ７’に出力する。フィルタ７’には、固有振動数予測部４が予測した固有振動数ｆｎも入力される。フィルタ７’は、電流指令値Ｉｃから固有振動数ｆｎの成分を抑制する。この処理により、電流指令値Ｉｃは電流指令値Ｉｃ’へと変換される。なお、電流指令値Ｉｃと電流指令値Ｉｃ’とを符号無しで区別する場合には、電流指令値Ｉｃを「第１の電流指令値」と呼び、電流指令値Ｉｃ’を「第２の電流指令値」と呼ぶ。フィルタ７’が生成した電流指令値Ｉｃ’は、電力増幅器９に対して出力される。電力増幅器９は、電流指令値Ｉｃ’に基づいて、アクチュエータ６ａを駆動する。
【００２１】
図３に示すサーボ系のように、固有振動数ｆｎの成分を抑制するフィルタ機能がフィードバックループの内部にある場合は、目標位置指令ＬＰの変換が無いため、図２の構成のように目標位置へ到達するまでの時間が長くなることは無い。しかし、固有振動数ｆｎによってサーボ系を構成する制御系が不安定になる場合があるため、固有振動数ｆｎによって図２の構成と図３の構成を選択するのがよい。
【００２２】
以上の説明のように、実施の形態１に係る制御装置によれば、フィルタ７もしくはフィルタ７’が、固有振動数予測部４によって算出された固有振動数ｆｎに対応する振動成分を抑制した信号を出力する。フィルタ７もしくはフィルタ７’は、補間位置毎に求められる固有振動数の予測値もしくは予測値列に応じて抑制する振動数成分を変化させるので、パラレルリンク機構に発生しうる振動を抑制することが可能となる。
【００２３】
実施の形態１に係る制御装置によれば、フィルタ７もしくはフィルタ７’の作用によって、パラレルリンク機構に発生しうる振動を抑制することができるので、位置及び速度制御器８における応答性に関する制御パラメータの調整工数を大幅に低減することができる。なお、制御系の用途によっては、応答性に関する制御パラメータを、エンドエフェクタ位置に応じて変化させることなく、一定としてもよい。
【００２４】
次に、固有振動数予測部４における固有振動数の予測方法について説明する。ここではまず、図４に示す２リンクの回転型パラレルリンク機構を例として説明する。
【００２５】
図４において、ベース１０には、アクチュエータ６ａ及び検出器６ｂが設置されている。アクチュエータ６ａは、アクチュエータシャフト１１を回転駆動する。アクチュエータシャフト１１は、ベルト１２を介して駆動軸１３を回転駆動する。駆動軸１３の回転に伴って、駆動軸１３に直結された上部リンク１４が回転する。上部リンク１４が駆動されると、上部リンク１４及びリンク結合部１５を介して結合される下部リンク１６が駆動される。下部リンク１６が駆動されると、最終的に下部リンク１６及びエンドエフェクタ結合部１７を介して結合されるエンドエフェクタ１８が駆動される。
【００２６】
一般的なパラレルリンク機構の振動問題について、支配的な振動モードは、剛性の低いリンク結合部からエンドエフェクタまでの系が一体となって振り子のように振動する現象である。この現象を、本願発明者は、実験により知見している。
【００２７】
そこで、２リンクの回転型パラレルリンク機構について、図５に示す力学モデルを導入する。以下、図５に示す力学モデルを説明する。
【００２８】
まず、一方のリンク結合部１５ともう一方のリンク結合部１５とを結ぶ直線状に伸びる仮想ベース１９を想定する。次に、リンク結合部１５を仮想ベース１９の長手方向に対して直交する並進ばね２０として表す。そして、リンク結合部１５からエンドエフェクタ１８までの系を１つの剛体２１として模擬する。
【００２９】
上記力学モデルによれば、ベース１０と、ベース１０に最も近いジョイント部であるリンク結合部１５との間の機構部が仮想ベース１９及び並進ばね２０で模擬され、リンク結合部１５とエンドエフェクタ１８との間の機構部が１つの剛体２１で模擬されている。別言すれば、パラレルリンク機構のベース１０と、パラレルリンク機構におけるベース１０との結合部であるリンク結合部１５との間の系が並進ばね２０で模擬され、この結合部とエンドエフェクタ１８との間の系が１つの剛体２１で模擬されている。
【００３０】
次に、図５の力学モデルによる固有振動数の算出方法について説明する。なお、図５において、座標原点は、２つのリンク結合部すなわち左方のリンク結合部１５と右方のリンク結合部１５との中点とする。また、Ｘ軸は当該座標原点から右方のリンク結合部１５へ向かう方向とし、Ｙ軸はＸ軸とは垂直でエンドエフェクタ１８とは逆方向とし、Ｚ軸はＸ軸及びＹ軸のそれぞれと右手系を成すように定義する。
【００３１】
図５のモデルでは、２つの並進ばね２０が同位相で振動して剛体２１が図中Ｙ方向に並進する振動モードと、２つの並進ばね２０が逆位相で振動して剛体２１が図中Ｚ軸周りに回転振動する振動モードとが存在する。本願において問題となっている振動現象は、剛体２１が振り子のように振動する現象である。よって、ここでは、並進運動は無視して回転振動モードに着目する。
【００３２】
まず、回転振動モードの固有振動数ｆｎは、共振周波数を表す次の式（１）にて計算することができる。
【００３３】
ｆｎ＝（１／２π）×√（Ｋ／Ｊ）……（１）
【００３４】
上記式（１）において、Ｋは２つの並進ばね２０によるＺ軸周りの合成回転ばね定数であり、Ｊは剛体２１の原点におけるＺ軸周りの慣性モーメントである。
【００３５】
合成回転ばね定数Ｋと並進ばね定数ｋの関係式は、次の式（２）にて表される。
【００３６】
Ｋ＝（１／２）ｋ×Ｌ＾２……（２）
【００３７】
式（２）において、Ｌはリンク結合部１５間の距離である。
【００３８】
剛体２１の慣性モーメントＪについては、剛体質量ＭＡＬＬ、剛体重心Ｚ軸周りの慣性モーメントＪＺＧ、及び座標原点から剛体重心までの距離ＬＧを用いて次の式（３）にて計算可能である。
【００３９】
Ｊ＝ＪＺＧ＋ＭＡＬＬ×ＬＧ＾２……（３）
【００４０】
剛体質量ＭＡＬＬは、２つの下部リンク１６及びエンドエフェクタ１８の各単体質量ＭＡＲＭ１、ＭＡＲＭ２、ＭＥＥの総和として次の式（４）にて計算可能である。
【００４１】
ＭＡＬＬ＝ＭＡＲＭ１＋ＭＡＲＭ２＋ＭＥＥ……（４）
【００４２】
また、剛体重心Ｚ軸周りの慣性モーメントＪＺＧは、２つの下部リンク１６及びエンドエフェクタ１８の各質量ＭＡＲＭ１、ＭＡＲＭ２、ＭＥＥ、各剛体重心Ｚ軸周りの慣性モーメントＪＡＲＭ１ＺＧ、ＪＡＲＭ２ＺＧ、ＪＥＥＺＧ、剛体重心から２つの下部リンク１６の単体重心までの距離ＬＡＲＭ１Ｇ、ＬＡＲＭ２Ｇ、並びに、剛体重心からエンドエフェクタ単体重心までの距離ＬＥＥＧを用いて、次の式（５）にて計算可能である。
【００４３】
ＪＺＧ＝ＪＥＥＺＧ＋ＭＥＥ×ＬＥＥＧ＾２
＋ＪＡＲＭ１ＺＧ＋ＭＡＲＭ１×ＬＡＲＭ１Ｇ＾２
＋ＪＡＲＭ２ＺＧ＋ＭＡＲＭ２×ＬＡＲＭ２Ｇ＾２……（５）
【００４４】
なお、座標原点から剛体重心までの距離ＬＧ及び剛体重心から２つの下部リンク１６の単体重心位置までの距離は、エンドエフェクタの補間位置座標ＣＰ、各アクチュエータの目標位置指令ＬＰ及び各単体の質量値から算出可能である。
【００４５】
以上で述べた力学モデルについて、エンドエフェクタ位置に対する予測振動数の変化の傾向について、図６を参照して説明する。
【００４６】
エンドエフェクタ位置が原点から離れた位置にある場合では、図６に示すように、剛体２１の重心位置が仮想ベース１９から遠くなり、座標原点から剛体重心までの距離ＬＧの値が大きくなるため、式（３）の右辺第２項の値が大きくなる。また右辺第１項にある剛体重心Ｚ軸周りの慣性モーメントＪＺＧについても、剛体重心とエンドエフェクタ重心との間の距離が離れて、距離ＬＥＥＧの値が大きくなる。さらに、パラレルリンク機構では、一般的に、エンドエフェクタ質量ＭＥＥが下部リンク質量ＭＡＲＭ１、ＭＡＲＭ２より非常に大きいため、式（５）で示される通り、剛体重心Ｚ軸周りの慣性モーメントＪＺＧが大きくなる。これらの結果、剛体２１の慣性モーメントＪの値が大きくなる。さらに、エンドエフェクタ位置が原点から離れた位置にある場合にはリンク結合部間の距離Ｌの値が小さくなるため、式（２）にて計算される合成回転ばね定数Ｋの値も小さくなり、式（１）にて計算される固有振動数が低下する。したがって、エンドエフェクタ位置が原点から離れると固有振動数が低下する傾向が表れる。
【００４７】
次に、異なる構成のパラレルリンク機構への適用について、図７を参照して説明する。図７には、直動型モータを採用した２リンクのパラレルリンク機構を示している。
【００４８】
図７において、ベース１０には、アクチュエータ６ａ及び検出器６ｂが設置されている。アクチュエータ６ａは、ボールねじ２２を駆動する。ボールねじ２２には、ナット２３が挿通されている。下部リンク１６は、ナット２３及びリンク結合部１５を介して結合されている。ボールねじ２２の回転に伴ってナット２３が直動する。ナット２３が直動すると下部リンク１６が駆動され、最終的に下部リンク１６及びエンドエフェクタ結合部１７を介して結合されるエンドエフェクタ１８が駆動される。
【００４９】
図７に示す２リンクの直動型パラレルリンク機構についても、図８に示す力学モデルを導入する。以下、図８に示す力学モデルを説明する。まず、リンク結合部１５を結ぶ直線状に伸びる仮想ベース１９を想定する。次に、リンク結合部１５を仮想ベース１９の長手方向に対して直交する並進ばね２０として表す。そして、リンク結合部１５からエンドエフェクタ１８までの系を１つの剛体２１として模擬する。
【００５０】
エンドエフェクタの位置が原点から離れた位置にある場合には、図４の機構と同様に、慣性モーメントの増大と、リンク結合部間の距離が接近することに起因する固有振動数ｆｎの低下が発生する。
【００５１】
３本以上リンクがある機構については、合成回転ばね定数及び慣性モーメントの算出方法は、図４の機構の場合と異なるが、基礎的な力学計算によって容易に算出可能で、エンドエフェクタ位置が原点から離れた位置にある場合に固有振動数ｆｎの低下が発生する傾向についても、２リンクの場合と同様である。
【００５２】
なお、モデル化パラメータについて、ベース、リンク結合部、リンク、エンドエフェクタ、ベルト、アクチュエータ及び検出器の各質量値及び重心位置は、設計値、実測値又は推定値でもよい。また、並進ばね定数ｋについては、例えば最も振動数予測精度が重要となるエンドエフェクタ位置での振動数予測値が実測値と一致するように調整するか、もしくは、複数のエンドエフェクタ位置での振動数予測値と実測値との誤差が最小となるように調整してもよい。
【００５３】
以上説明したように、実施の形態１に係る制御装置によれば、パラレルリンク機構のベースからリンク結合部までの間の機械系を並進ばねで模擬し、リンク結合部からエンドエフェクタまでの機械系を１つの剛体で模擬した力学モデルを構築し、エンドエフェクタの位置に対応して変化する固有振動数を予測し、予測した予測値列に従って抑制する振動数成分を補間位置毎に変化させているので、応答性に関する制御系のパラメータをエンドエフェクタ位置に応じて変化させることなく、固有振動数予測部によって算出される振動数を抑制するフィルタによって振動を抑制することができる。これにより、調整が必要な制御パラメータ数を低減でき、パラメータ調整工数を低減できる、といった従来にはない顕著な効果を得ることができる。
【００５４】
実施の形態２．
以下に、実施の形態２に係る制御装置について説明する。図９は、実施の形態２に係る制御装置の機能構成を示すブロック図である。実施の形態２では、図１に示す実施の形態１の構成において、指令部１にバネ定数推定部１００が追加され、指令部１の外部に外部測定器１０１が追加されている。外部測定器１０１にて計測された実測振動数ｆｎ_ｒｅａｌはバネ定数推定部１００に入力され、バネ定数推定部１００にて推定された推定バネ定数ｋｅｓｔが固有振動数予測部４に入力されるように構成されている。なお、その他の構成については、図１に示す実施の形態１の構成と同一又は同等であり、同一又は同等の構成部には同一の符号を付して重複する説明は省略する。なお、外部測定器１０１が計測する実測振動数ｆｎ＿ｒｅａｌは、機構に取付けた加速度センサから得られる加速度信号を周波数変換して得られる信号でよいし、また、検出器６ｂから得られる位置信号を周波数変換して得られる信号でもよい。
【００５５】
次に、実施の形態２に係る制御装置の要部動作について説明する。まず、バネ定数推定部１００では、上述した式（１）及び式（２）から得られる、次の式（６）を用いて、力学モデルにおけるバネ定数を推定する。
【００５６】
ｋｅｓｔ＝Ｊ×（２πｆｎ＿ｒｅａｌ）＾２／｛（Ｌ＾２）／２｝
……（６）
【００５７】
上記の式（６）にて推定されたバネ定数の推定値をここでは、推定バネ定数と呼ぶ。バネ定数推定部１００にて推定された推定バネ定数ｋｅｓｔを固有振動数予測部４にて用いるようにすれば、力学モデルの自動推定が可能となり、パラメータ調整工数の更なる低減が可能となる。
【００５８】
実施の形態３．
以下に、実施の形態３に係る制御装置について説明する。図１０は、実施の形態３に係る制御装置の機能構成を示すブロック図である。実施の形態３では、図９に示す実施の形態２の構成において、指令部１に特性変化検出部１０２が追加されている。特性変化検出部１０２は、バネ定数推定部１００にて推定された推定バネ定数ｋｅｓｔに基づいて、アラーム信号を外部に出力するように構成されている。なお、その他の構成については、図９に示す実施の形態２の構成と同一又は同等であり、同一又は同等の構成部には同一の符号を付して重複する説明は省略する。
【００５９】
次に、実施の形態３に係る制御装置の要部動作について説明する。特性変化検出部１０２は、推定バネ定数ｋｅｓｔの値を監視及び記憶し、推定バネ定数ｋｅｓｔが大きく変化した場合には、装置特性の変化が発生したと判定し、アラーム信号を出力する。これにより、装置の劣化に対して装置破損が発生する前に保全作業を行うことが可能になる。
【００６０】
最後に、実施の形態１から３に係る指令部１の機能を実現するためのハードウェア構成について説明する。実施の形態１から３に係る指令部１の機能をソフトウェアで実現する場合には、図１１に示すように、演算を行うＣＰＵ（Ｃｅｎｔｒａｌ  Ｐｒｏｃｅｓｓｉｎｇ  Ｕｎｉｔ：中央処理装置）２００、ＣＰＵ２００によって読みとられるプログラムが保存されるメモリ２０２、及び、信号の入出力を行うインタフェース２０４を含む構成とすることができる。図１０に示す実施の形態３の構成では、装置特性の変化が発生したと判定した場合にはアラーム信号を出力するようにしているが、図１１のように指令部１が表示器２０６を備えていれば、アラーム信号を、ユーザ、管理者又は設計者に通知することができる。
【００６１】
なお、ＣＰＵ２００は、演算装置、マイクロプロセッサ、マイクロコンピュータ、プロセッサ、又はＤＳＰ（Ｄｉｇｉｔａｌ  Ｓｉｇｎａｌ  Ｐｒｏｃｅｓｓｏｒ）などと称されるものであってもよい。また、メモリ２０２とは、例えば、ＲＡＭ（Ｒａｎｄｏｍ  Ａｃｃｅｓｓ  Ｍｅｍｏｒｙ）、ＲＯＭ（Ｒｅａｄ  Ｏｎｌｙ  Ｍｅｍｏｒｙ）、フラッシュメモリ、ＥＰＲＯＭ（Ｅｒａｓａｂｌｅ  Ｐｒｏｇｒａｍｍａｂｌｅ  ＲＯＭ）、ＥＥＰＲＯＭ（Ｅｌｅｃｔｒｉｃａｌｌｙ  ＥＰＲＯＭ）などの、不揮発性又は揮発性の半導体メモリ、磁気ディスク、フレキシブルディスク、光ディスク、コンパクトディスク、ミニディスク、ＤＶＤ（Ｄｉｇｉｔａｌ  Ｖｅｒｓａｔｉｌｅ  Ｄｉｓｃ）などが該当する。
【００６２】
メモリ２０２には、指令部１の機能を実現するためのプログラムが格納されている。ＣＰＵ２００は、インタフェース２０４を介して、種々の情報を受領する。メモリ２０２に格納されたプログラムをＣＰＵ２００が実行することにより、指令部１の機能が実現される。ＣＰＵ２００の処理結果を表示器２０６に表示することもできる。
【００６３】
なお、以上の実施の形態に示した構成は、本発明の内容の一例を示すものであり、別の公知の技術と組み合わせることも可能であるし、本発明の要旨を逸脱しない範囲で、構成の一部を省略、変更することも可能である。
【符号の説明】
【００６４】
１  指令部、２  補間位置座標計算部、３  逆機構変換部、４  固有振動数予測部、５  駆動制御部、６ａ  アクチュエータ、６ｂ  検出器、７  フィルタ、８  位置及び速度制御器、９  電力増幅器、１０  ベース、１１  アクチュエータシャフト、１２  ベルト、１３  駆動軸、１４  上部リンク、１５  リンク結合部、１６  下部リンク、１７  エンドエフェクタ結合部、１８  エンドエフェクタ、１９  力学モデルの仮想ベース、２０  力学モデルの並進ばね、２１  力学モデルの剛体、２２  ボールねじ、２３  ナット、１００  バネ定数推定部、１０１  外部測定器、１０２  特性変化検出部、２００  ＣＰＵ、２０２  メモリ、２０４  インタフェース、２０６  表示器。
【図１】
2018029910.tif 000003
【図２】
2018029910.tif 000004
【図３】
2018029910.tif 000005
【図４】
2018029910.tif 000006
【図５】
2018029910.tif 000007
【図６】
2018029910.tif 000008
【図７】
2018029910.tif 000009
【図８】
2018029910.tif 000010
【図９】
2018029910.tif 000011
【図１０】
2018029910.tif 000012
【図１１】
2018029910.tif 000013
【国際調査報告】
2018029910.tif 000014
2018029910.tif 000015
2018029910.tif 000016
2018029910.tif 000017