無線伝送装置とそれを用いたロボットの振動抑制制御装置およびロボット制御装置
【課題】無線化した加速度センサをロボットに取り付ける場合、センサを用いた振動抑制の効果を向上できるとともにセンサ基板における消費電力が低減できるようにする。
【解決手段】センサ信号の主成分の振動パラメータ（周波数、振幅、位相）を得る信号解析装置と、該パラメータからセンサ信号の近似波形を生成する信号発生器８１を備えて、該パラメータが変更された場合のみパラメータの無線伝送を行う。得られる近似波形から各軸モータの速度を求め、シミュレーションによりロボット動作に基づくモータ速度を得て、モータ速度の差をモータ速度の振動成分とする。モータ速度の振動成分をモータの位置−速度制御ループにフィードバックしてロボットアームに発生する振動を抑制する。
【解決手段】センサ信号の主成分の振動パラメータ（周波数、振幅、位相）を得る信号解析装置と、該パラメータからセンサ信号の近似波形を生成する信号発生器８１を備えて、該パラメータが変更された場合のみパラメータの無線伝送を行う。得られる近似波形から各軸モータの速度を求め、シミュレーションによりロボット動作に基づくモータ速度を得て、モータ速度の差をモータ速度の振動成分とする。モータ速度の振動成分をモータの位置−速度制御ループにフィードバックしてロボットアームに発生する振動を抑制する。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は、例えばロボット制御装置に関し、詳しくはロボットアームの先端に加速度センサを取り付けて発生する振動の信号をロボットコントローラに伝送する無線伝送装置とそれを応用してアーム先端の振動抑制を行うロボットの振動抑制制御装置に関する。
【背景技術】
【０００２】
半導体分野で使用されるクリーンロボットでは生産性を向上させるためロボットの高速動作が必要とされている。高速動作を行う場合アームが持つ機構の振動が励起され、ロボットが動作停止した後も動作方向と垂直な方向にアームが振動する残留振動が発生するという問題がある。
クリーンロボットがウェハを搬送してカセットに収納する場合、残留振動が発生すると
ウェハとカセットが衝突してウェハの破損の原因となるため、残留振動が収まるのを待たなければならず、生産性の向上を阻害する原因の１つとなっている。
ロボットアームの振動抑制方法の一つとして加速度センサをアーム先端に取り付け、振動によって発生する加速度信号といくつかのロボットパラメータを用いて補償信号を生成して、各軸モータのトルク指令に得られる補償信号をフィードバックして振動を抑制する方法が開示されている（特開平１０−１０００８５）。
そのため、従来の方法では加速度センサ信号は時系列データとしてロボットコントローラに伝送し、ロボットコントローラ内部で各種のロボットパラメータを参照しながら一括処理されている。
【０００３】
一方、ロボットに加速度センサを取り付ける場合、配線レスにして使用することが望まれる（特許文献５、特許文献６）。なぜなら配線があると、
（１）旋回動作を行う場合、その動作範囲が制限される。
（２）センサに取り付けた配線にはロボットの動作によってねじれや引っ張りの力が加わり断線または配線の接触不良が発生する。
（３）加速度センサの取り付け位置に制限を受ける。
（４）配線をロボット内部にはわせる手間がかかる。
などの理由があるためである。
【０００４】
図１４に特許文献５のセンサ装置のブロック図を示す。この発明では上記のようにロボットアーム先端の加速度情報を時系列データとして伝送するセンサ装置が示され電源ＯＮ／ＯＦＦを行うことによる省電力化の方法が開示されている。
図１５に特許文献６の無線出力センサのブロック図を示す。図に示されるように従来の発明では検出対象を検出する加速度センサ部と、上記加速度センサ部の検出に係るセンサデータを無線で送信処理をする無線送信部と、上記無線送信部の処理に係るセンサデータを送信する送信アンテナと、上記加速度センサ部と無線送信部それぞれの電源となる電池を搭載する電池搭載部と、を備えたことを特徴とする無線出力センサの技術が開示されている。
【０００５】
図１５の無線出力センサでは３軸加速度センサ１８３の出力信号の大きさに応じてＡＣアンプ部１８４において振動波形が生成され、その信号を復調器１８５と出力部１８６において検出対象の可動の有無を検出する信号に変換されている。この可動の有無の信号を発振部１３３においてＡＣ信号とし、アンプ部１３４とフィルタ１３５において処理した後アンテナ２４を介して無線出力する技術が開示されている。
【０００６】
図１４及び図１５に示される様に、加速度センサを無線化する場合、現在のところ測定データは無線を用いて伝送し、センサ基板で消費される電力は基板内部に設けるバッテリによって供給することが現実的である。また、特許文献５及び６ではソーラー充電式または振動充電式の補助電源を使用してバッテリ寿命を延ばすことが提案されている。
センサ基板に使用される電子部品の消費電力の一例を図１６に示す。センサ基板で消費される電力の大半は無線で消費されていることが分かる。
【０００７】
また、非特許文献１に示されるように建築構造物に多数の加速度センサを配置して、各センサの信号を中央の親局に無線伝送して、親局に伝送された多数の信号をもとにして地震などの災害の発生を検出する構造ヘルスモニタリングシステムが開発されている。
このシステムにおいては、各センサの信号はセンサ基板に設けられた信号処理回路において転送するデータ量が最小となるように加工して、センサ基板から親局に無線伝送される伝送量を低減させて、センサ基板に設けられたバッテリの消費を低減させる技術が開示されている。
【０００８】
特許文献１には、タイヤ側に設けられた加速度センサ信号から振動レベル演算値を計算して、車体側に無線を用いて伝送する路面状態推定装置の技術が開示されている。図１２に路面状態推定システムの構成を示すブロック図を示す。タイヤに装着された加速度センサの信号は周波数分析手段において周波数分析され、振動レベル演算値が計算されて車体側に設けられた路面状態推定装置に無線伝送される。路面状態推定装置では伝送された振動レベル演算値を用いて車両の走行している路面状態を判定する。
【０００９】
特許文献２、特許文献３、特許文献４にはベアリングの固定部分に加速度センサを設置して、ベアリングが回転するときに発生する振動加速度をFFT処理し、回転速度信号の周波数成分と比較照合して、回転部品の異常の有無と異常部位を検出する機械設備の異常診断装置が開示されている。
図１３に異常診断装置の信号処理部のブロック図を示す。検出部に設けられた加速度センサの信号は信号処理部の診断用スペクトル算出部によって周波数分析され、その結果が回転情報を回転分析部で周波数分析した結果と比較されてベアリングの異常の有無が判定されている。
【００１０】
このように、従来の加速度センサを用いた無線伝送装置は、加速度センサから得られるデータを時系列で伝送してコントローラで測定対象物の状態推定や異常検出の処理のすべてを行うか、センサ側でデータを周波数分析などの手段を用いて加工することにより伝送情報量を低減させた後、コントローラで測定対象物の状態推定や異常検出を行うとともに無線伝送で消費される電力の低減をはかることが行われている。
【先行技術文献】
【特許文献】
【００１１】
【特許文献１】特開２００７−５５２８４（ブリヂストン）
【特許文献２】特開２００７−２６３６０９（日本精工）
【特許文献３】特開２００７−２８５８７５（日本精工）
【特許文献４】特開２００９−２００９０（日本精工）
【特許文献５】特開２００８−１８６３３６（安川電機）
【特許文献６】特開２００８−１７１４０３（光洋電子工業）
【非特許文献】
【００１２】
【非特許文献１】「スマートセンサと無線ネットワークを用いた構造ヘルスモニタリングシステムの開発」 日本地震工学会論文集 第7 巻、第6 号、2007 p.17-30
【発明の概要】
【発明が解決しようとする課題】
【００１３】
特許文献６の発明では対象物の可動の有無の情報のみを検出している。無線通信を0.5秒間だけワンショット送信する構成（段落番号（００４５）参照）としてもロボット動作の状態が検出でき、2[sample/s]より少ないサンプルレートとなっていても問題なく、電池の寿命もある程度の期間が得られると考えられる。しかし、アームの振動抑制制御の場合、加速度信号の時系列の信号が必要である。アームの振動周波数が１０[Ｈｚ]程度であり、その１０〜３０倍程度の300[sample/s]程度の時系列データが必要であるため伝送情報量が大きくなり、図１６の無線センサモジュールの電子部品の場合、電池寿命は数日程度となる。
特許文献５のセンサ装置の発明においても同様で電池寿命の短さが問題となり、無線化した加速度センサを振動抑制制御などに応用できない理由の１つである。
【００１４】
従来の構造ヘルスモニタリングシステムの場合、センサデータの加工は、各センサで独立に行なわれ、センサ基板上で加速度のゼロクロス点数と最大値および振幅絶対値の累積値である損傷指標に変換されて無線通信によって親局に伝送される。
また従来の路面状態推定装置の場合、センサ信号の処理のほとんどをタイヤ側で行なっている。加速度センサはタイヤの回転とともに重力の影響を受けるが、車両の走行に伴い発生する振動の加速度が４０[g]程度であるので回転による加速度変化（±１[ｇ]）を補正する必要はない。
また従来の機械設備の異常診断装置においてもセンサ信号の処理は独立して行われ、
工作機械が動作して発生する加速度の考慮はなされていない。一般に工作機械では機械の動作による加速度と異常振動の加速度の周波数は異なるものとなり、機械の動作による加速度はノイズとして取り扱われて周波数解析などの手段を用いて除去することがおこなわれている。すなわち、制御器のパラメータと組み合わせてセンサ信号の処理は行われていない。
【００１５】
加速度センサを用いたロボットの振動抑制制御では、ロボットの姿勢や動作状態によって変化するロボットの各種のパラメータと組み合わせて加速度信号を処理してモータのトルク指令に加える補償信号の算出を行なわなければならない。そのため各センサにおけるデータの加工は、ロボットコントローラ側でロボットのパラメータと組み合わせて補正処理ができる範囲でなければならない。
【００１６】
また、ロボットにおいては振動と動作の加速度の周波数帯域が重なり、振動と動作の加速度の大きさも似たような大きさになるため、ロボットが動作して発生する加速度をノイズとして処理することができず、姿勢や動作状態から計算される動作の加速度をセンサの信号から補正する方法（特開2005−316937）が有効になり、従来の無線伝送装置の技術の組み合わせでは振動抑制の効果を上げることができない。
このような信号処理をロボットコントローラでおこなうためには特許文献５に示すように時系列でセンサ信号を伝送してロボットコントローラ側ですべての処理を行う必要がある。
【００１７】
ロボットの振動抑制制御のようにセンサ信号の高いサンプリング速度が要求される場合、センサ信号を常時伝送する必要があり、無線回路が常時送信動作を行っている状態になるため、市販の乾電池を用いたとすると数日程度でバッテリの電力を消費してしまい、バッテリの頻繁な交換または充電が必要になる。
特許文献５及び６には、補助電源によるバッテリ寿命を延ばすことが述べられているが図１６に示した消費電力の場合、ソーラー充電の場合は太陽電池パネルの面積を大きくとる必要があり取り付けが難しくなる。振動充電の場合も現在得られている圧電素子の体積あたりの発生電力が小さいため電源として補助的な効果を得るためには体積を大きくとる必要があり、ロボットアーム先端に取り付けることがむつかしくなる。
加速度センサを配線レスで使用して振動抑制制御などのリアルタイム性が要求される用途に応用する場合、ロボットの制御性能の向上が達成されるだけではなく、センサ基板の低消費電力化を同時に達成できる工夫をしなければ実用化することができないという問題がある。
【００１８】
無線センサモジュールの構成部品別の消費電力は図１６で分かるように無線回路が常時送信動作を行う場合、他の構成部品に比べてかなり大きくなる。また、センサを含めた信号処理回路の部品の消費電力は、この数年で大きく低減されている。このため測定データの伝送情報量を低減させて無線回路の消費電力が低減できればセンサ基板の低消費電力化に有効な方法のひとつになる。
【００１９】
また、振動抑制制御などのリアルタイム性が要求される用途では無線伝送の伝送遅れ時間によって加速度信号に位相遅れが生じ、制御性能の向上が制約される問題がある。
無線回路としては高性能・低価格化が進むブルートゥースなどのデジタル無線装置の応用が考えられるが、市販の無線回路を用いる場合、サンプリングによる遅れなどのために5[ms]程度の伝送遅れが発生するため、10[Hz]の振動波形の場合18°程度の位相遅れとなり、振動抑制制御の制御性能の劣化の原因となる。
【００２０】
本発明はこのような問題点に鑑みてなされたものであり、無線化される加速度センサを使用してロボットの振動抑制制御を行う場合、振動抑制の効果を向上するとともに無線センサモジュールの消費電力を低減するロボット制御装置を提供することを目的とする。
【課題を解決するための手段】
【００２１】
上記問題を解決するため、本発明は、次のように構成したのである。
本発明は、一対の無線回路を用いてセンサ信号をコントローラに伝送する無線伝送装置において、前記センサ信号を周波数解析して主成分である振動の周波数、振幅、位相からなるパラメータを得て、前記パラメータが変化したとき前記無線回路から前記パラメータを送出する信号解析装置と、受信した振動の前記パラメータを用いて前記センサ信号の近似波形を生成する信号発生器を有することを特徴とする無線伝送装置とするものである。
【００２２】
また、本発明は、請求項１記載の無線伝送装置と、送信される２軸のセンサ信号からロボットの各軸モータ速度を求めるヤコビ逆変換手段と、ロボットの位置指令から各軸モータが動作したときに発生する速度を求める手段を有し、センサから得られるモータ速度から動作したときに発生する速度を差し引いてモータ速度の振動成分を求め、モータの位置−速度制御ループの速度指令にモータ速度の振動成分を補償してアームに発生する振動を抑制することを特徴とするロボット制御装置とするものである。
【００２３】
また、本発明は、請求項１記載の無線伝送装置と、モーション部で生成される位置指令を周波数解析した結果からロボットの動作したときに発生するモータ速度の周波数成分を求める手段を有し、
センサ速度信号の周波数成分から動作したときに発生する速度の周波数成分を差し引き、モータ速度の振動成分の周波数成分を求め、得られる振動の周波数、振幅、位相のパラメータからモータ速度が持つ振動の近似波形を生成して、モータの位置−速度制御ループに振動成分として補償してアームに発生する振動を抑制することを特徴とするロボット制御装置とするものである。
【００２４】
また、本発明は、請求項１記載の信号解析装置と伝送されるセンサ信号の主成分の振動の周波数と振幅によって中心周波数とＱ値が変更できるノッチフィルタを用いてモータのトルク指令からセンサ信号の主成分の振動周波数成分を除去することを特徴とするロボット制御装置とするものである。
【００２５】
また、本発明は、ロボットコントローラにおいてアームを構成する各軸モータの振動周波数を計算して無線センサモジュールに伝送して、無線センサモジュールにおいてはＹ軸センサ信号の２つの振動周波数の周波数成分の振幅および位相を計算して前記ロボットコントローラに伝送し、前記ロボットコントローラでは伝送された振幅、位相及び計算によって得られる振動周波数、ロボットの姿勢情報を用いてＳ軸振動振幅計算器、Ｌ軸振動振幅計算器でＳ軸モータの振動振幅及びＬ軸モータの振動振幅を計算し、Ｓ軸モータの振動振幅とＳ軸振動周波数におけるＹ軸信号の位相とＳ軸振動周波数から信号発生器によってＳ軸振動波形を合成し、Ｌ軸モータの振動振幅とＬ軸振動周波数におけるＹ軸信号の位相とＬ軸振動周波数から信号発生器によってＬ振動波形を合成し、前記２つの合成波形を用いて振動抑制制御を行うことを特徴とするロボット制御装置とするものである。
【００２６】
また、本発明は、センサ信号の周波数解析により得られる振幅と位相の情報に基づいて振幅の情報の伝送を位相の情報に基づいたタイミングで行い、ロボットコントローラでは伝送される振幅の情報の到着時刻に基づいて周波数解析で得られる位相を計算して、前記振幅と計算された位相とコントローラ内部の振動周波数から振動波形を合成する信号発生器を構成したことを特徴とする請求項５記載のロボット制御装置とするものである。
【００２７】
また、本発明は、各周波数に対応する伝送遅れ時間のオフラインでの測定結果を内蔵する位相補償器を有して、振動周波数計算器で得られるＳ軸振動周波数ωＳ、Ｌ軸振動周波数ωＬにおける伝送遅れ時間を信号発生器に入力し、無線センサモジュール及び２つの無線回路で発生する伝送遅れ時間を補償する信号発生器を構成したことを特徴とする請求項５または６記載のロボット制御装置とするものである。
【発明の効果】
【００２８】
本発明によると、振動パラメータが変化する場合のみパラメータの伝送を行えばよく、無線伝送の回数を低減できるため、センサ装置の低消費電力化を図ることができる。
また、本発明によると、信号発生器のセンサ信号の近似波形を用いて各軸モータの補償信号の算出ができ、補償信号を制御ループにフィードバックして振動抑制の効果を上げることができる。また、振動パラメータが変化する場合のみパラメータの伝送を行うのでセンサ装置の低消費電力化を図ることができる。
また、本発明によると、伝送された振動パラメータと位置指令の周波数解析の結果を用いてモータ速度の振動の周波数成分が求まる。それを用いてモータ速度の振動の時間波形が得られる。これを補償信号として制御ループにフィードバックして振動抑制の効果を上げることができる。また、振動パラメータが変化する場合のみパラメータの伝送を行うのでセンサ装置の低消費電力化を図ることができる。
また、本発明によると、伝送される振動パラメータの周波数と振幅に従って中心周波数とＱ値を変化できるノッチフィルタをトルク指令に挿入して、トルク指令からロボットアームの振動周波数成分を除去することができ、ロボットアームの振動励起を防止することができる。また、機械の振動パラメータが変化する場合のみパラメータの伝送を行うのでセンサ装置の低消費電力化を図ることができる。
また、本発明によると、各モータ軸で発生する振動波形がＹ軸センサ信号のＳ軸共振周波数成分、Ｌ軸共振周波数成分及びコントローラ内の信号情報から生成することができるので、無線センサモジュールとコントローラの間の伝送情報量を減らすことができ無線センサモジュールの消費電力を低減することができる。
また、本発明によると、振動周波数成分の解析によって得られる位相情報を振動周波数成分から得られる振幅の伝送を行う時刻の情報として伝送できるので位相データの伝送が省略でき、伝送情報量を減らすことが可能となる。無線センサモジュールの低消費電力化を図ることができる。
また、本発明によると、無線センサモジュールとコントローラの間の無線伝送で発生する伝送遅れ時間を補償できるので、ロボットの動作で発生する振動加速度に位相が合った補償信号が生成でき、振動抑制制御の制御性能の向上を図ることができる。
【図面の簡単な説明】
【００２９】
【図１】本発明の第１実施例を示す無線伝送装置のブロック図
【図２】本発明の信号発生器の内部構成を示すブロック図
【図３】本発明の第２実施例を示す振動抑制制御装置のブロック図
【図４】第２実施例のヤコビ逆変換を説明するためのヤコビ行列の要素の式
【図５】本発明の第３実施例を示す振動抑制制御装置のブロック図
【図６】第３実施例の伝達関数を説明するためのブロック図
【図７】本発明の第４実施例を示す振動抑制制御装置のブロック図
【図８】ロボットアームの動作を説明する構成図
【図９】ロボットコントローラの内部を説明する構成図
【図１０】本発明の第５実施例を示す振動抑制装置のブロック図
【図１１】第５実施例を説明するための構成図
【図１２】従来の路面状態推定装置を説明するための構成図
【図１３】従来の機械設備の異常診断装置を説明するための構成図
【図１４】従来のセンサ装置を説明するためのブロック図
【図１５】従来の無線出力センサを説明するためのブロック図
【図１６】無線センサモジュールの構成部品の消費電力を示す図
【発明を実施するための形態】
【００３０】
以下、本発明の実施の形態について図を参照して説明する。
図８及び図９は本発明の実施形態に用いられるロボットを説明するための図である。図８はロボットを含むロボットシステムの概要を模式的に示す図である。図８に示すようにロボットシステム１は水平多関節型のロボット２及びロボットコントローラ２０により構成されている。そして、ロボット２は、基台３，第１アーム４，第２アーム５，フォーク６，S軸モータ７、L軸モータ８及び無線センサモジュール１５を備えている。基台３は、円柱状に形成されており上下方向に昇降可能に構成されている。第１アーム４は、水平面内で旋回可能に基台３にベアリング等を介して取り付けられて、第１アーム根元側に取り付けられたＳ軸モータ７の回転運動によって旋回動作を行う。また、第１アーム４の先端側には水平面内で旋回可能に第２アーム５が取り付けられて、第２アームの根元側に取り付けられたＬ軸モータ８の回転運動によって旋回動作を行う。ここで説明しているクリーンロボットの多くの場合、各軸のモータがアームの回転中心を直接駆動する構成とはならず、モータには減速機が挿入され、減速された出力をベルト等を介してアームの回転方向に印加してアームを旋回動作させる構成となっている。ここでは減速機とベルト等の機構を含めてＳ軸モータ７・Ｌ軸モータ８として、減速比１のモータが直接アームの回転軸を駆動するとして実施例の説明を行っている。
【００３１】
第２アーム５の先端側には、フォーク６が水平面内で旋回自在に取り付けられており、フォーク６は搬送するワークＷが載置可能に形成されている。第２アーム５およびフォーク６は、L軸モータ８の駆動によって予め定められた速度比（回転比）を保って旋回するようになっており、これによりフォーク６を基台３の半径方向に移動できるように構成されている。
無線センサモジュール１５は２軸加速度センサ１５１と信号解析装置１５２と無線回路71とバッテリ１５４から構成され、２軸加速度センサ１５１で水平面内の加速度が測定され、そのデータを信号解析装置１５２で周波数解析して測定パラメータを無線回路71からロボットコントローラ２０に無線伝送するようになっている。バッテリ１５４は無線センサモジュール１５の各部品の電源となる。
【００３２】
ロボットアーム２はロボットコントローラ２０によって動作が制御される。図９にロボットコントローラ２０の機能の概略を示すブロック図を示す。ロボットコントローラ２０はロボット用多軸制御演算装置２０１とロボット用多軸アンプ２０２により構成される。
ロボットコントローラ２０の構成例としてロボット用多軸制御演算装置２０１はパーソナルコンピュータを用い、ロボット用多軸アンプ２０２はロボットの各軸モータをトルク制御できる複数のモータ制御器を用いて構成することができる。この場合、ロボット用多軸制御演算装置２０１の主要機能であるモーション部とサーボ部は、パーソナルコンピュータ内部で実行されるソフトウェアによって構成され、実行周期の異なる２つのタスクとなっている。
【００３３】
ロボット動作プログラムはあらかじめユーザによって作成され、ロボット用多軸制御
演算装置２０１上の不揮発性メモリ内部に記憶されている。ロボットを動作させるときは、ロボット動作プログラムを読み出しモーション部で解読して各軸モータの位置指令を計算する。サーボ部では、モーション部で計算された位置指令とモータに取り付けられたエンコーダから得られる各軸モータ位置を用いて位置−速度制御系を構成して各軸モータのトルク指令を計算している。
ロボット用多軸アンプ２０２では、サーボ部で計算された各軸モータのトルク指令に
相当する電流を各軸モータに供給してモータ位置とモータ速度を制御して動作プログラムによって生成されるロボットの姿勢と動作を得ている。
ここでは、アーム先端の振動検出手段としてロボット２のフォーク６の付け根に取り付けられた２軸加速度センサ（X軸センサ、Y軸センサ）を用いて振動を検出して無線センサモジュール１５とロボットコントローラ２０内部の無線回路１５３と無線回路７０の間で測定信号を無線伝送してロボットコントローラ２０内部に測定信号を取り込むことを考える。
【００３４】
以下、本発明の実施の形態について図を参照して説明する。
【実施例１】
【００３５】
図１は、本実施形態の信号伝送装置のブロック図を示す。図において、ロボットアーム２にはその水平方向の加速度のＸ軸方向成分とＹ軸方向成分を測定する２軸加速度センサ１５１とその周波数解析を行う信号解析装置１５２と解析結果をロボットコントローラ２０に無線を用いて伝送する無線回路71とこれらの電源であるバッテリ１５４を有する無線センサモジュール１５が搭載されている。ロボットコントローラ２０には無線回路７０、信号発生器８１から構成される無線モジュール７００が搭載されている。
信号発生器８１は図２に示すブロックで構成されている。信号発生器８１には無線回路で受信された振動のパラメータである周波数ω、振幅Ｖ、位相θが入力される。これらのパラメータは受信が完了すると信号発生器８１のレジスタにラッチされる。また、信号発生器８１の内部にはタイマがあり、データのラッチされた時刻をt0をラッチする。正弦波計算器ではタイマが示す時刻tを用いて
A=V・SIN(ω・（t−t0）＋θ） ・・・（１式）
に従い計算が行われ、加速度近似波形Ａが出力される。
【００３６】
ロボットアーム２が水平面内で動作して２軸加速度センサ１５１が発生する信号は信号解析装置１５２の内部のCPU（図示されず）によって周波数解析が行われ振動の主成分の振動パラメータ（周波数ω、振幅Ｖ、位相θ）が求められる。
周波数解析の手段としてはロボットアームの振動抑制制御を行う場合、リアルタイム性が要求されるため、ウェーブレット変換を行うことを想定している。求められた各軸の主成分の振動パラメータは無線回路71と無線回路７０の間で無線伝送されロボットコントローラ２０内部に取り込まれる。
コントローラに取り込まれた振動パラメータは信号発生器８１の内部にセットされ加速度センサ信号の主成分のみからなる加速度近似波形Ａがロボットコントローラ２０内部で発生される。
実施例１では、無線伝送は信号解析装置１５２で計算される振動パラメータの要素が一定値以上の変化をする場合のみに行うものと考える。
主成分の信号が周波数、振幅、位相が一定である正弦波である場合は無線回路71と無線回路７０の間の振動パラメータの伝送は1回行われるとそれ以後の加速度近似波形Ａの計算は信号発生器８１内部で行われ、無線伝送の回数を大幅に減少させることが可能となる。
図１では伝送信号が各軸センサの主成分のみの場合を考えたが、成分が複数となるように構成することも可能であり、信号発生器８１が複数の正弦波計算器を持ち、その和によって加速度近似波形Ａを求めるように構成する。この構成によって加速度近似波形Ａを加速度センサ１５１の信号に近づけることができる。
【実施例２】
【００３７】
図３は実施例２の構成を示す図である。実施例１で示した無線センサモジュール１５と無線モジュール７００をロボットシステム１内部に設けてロボットアーム２に発生する振動を抑えるために振動抑制制御装置を構成した場合を示す。
無線モジュール７００はロボットコントローラ２０内部のサーボ部に実装される。信号解析装置１５２ではＸ軸加速度センサとＹ軸加速度センサの振動加速度が周波数解析されて、それぞれの軸の主成分の振動パラメータが求められる。ここではＸ軸方向の主成分の周波数ωｘとＹ軸方向の主成分の振動周波数ωｙが異なり、求められる振動パラメータは信号解析装置１５２内部で加速度から速度に変換されて信号処理されているとして説明する。
ここではＳ軸モータの制御ブロック図のみを示している。Ｌ軸モータの制御ブロックも同様に構成される。
信号発生器８２は第１の実施例で説明した信号発生器８１が２軸分内蔵されている。２つの信号発生器８１には無線回路７０で受信されるＸ軸方向とＹ軸方向の振動パラメータがセットされてそれぞれの速度近似波形がロボットコントローラ２０内部で生成される。
２つの速度近似波形はヤコビ逆変換によってＳ軸モータ及びＬ軸モータの速度に変換される。ヤコビ逆変換はヤコビ逆行列９０をかけて行われる。ヤコビ逆行列は図８の水平多関節ロボットの場合図４の（２式）で示される２ｘ２行列となる。
各要素は２つのモータの関節角θsとθlに依存して、ロボットの姿勢が変わると変化する。
モータの制御系はモーション部から与えられる位置指令とモータに取り付けられたエンコーダ位置を用いて位置ループがＰ制御、速度ループがＰ−Ｉ制御で構成される制御ループと制御ループと同じ位置指令と制御パラメータを用いてロボットの剛体モデルの動作を逐次計算するシミュレーション部から構成される。シミュレーション部ではロボットの姿勢で変化するイナーシャを逐次代入して剛体モデルの速度を求めている。
ここではＸ軸速度とＹ軸速度の主成分ωｘとωｙのみを考えているのでシミュレーションで得られる速度も２つの周波数成分のみを持つように、主成分の周波数ωｘとωｙの変化にともなって中心周波数がωｘ、ωｙに一致するように変化するバンドパスフィルタ３００、３０１を一対入れている。
センサから得られるＳ軸モータ速度からシミュレーションで得られるＳ軸モータ速度を引いた結果を振動成分として、メインループの速度指令に補償を加えている。
ロボットと等価なイナーシャを持つ剛体モデルにロボットが動作するときのＳ軸位置指令を与えてシミュレーションを行っているため、シミュレーションで得られる動作速度は振動成分を含まないロボットの動作を近似していて、ロボットの動作によって発生する速度を表している。センサで測定する速度には振動で発生する成分とともに動作で発生する速度成分が含まれるため、シミュレーションで求まるロボットの動作で発生する成分を除去することによって振動で発生する速度成分を求めることができる。この振動の速度成分をメインループの速度指令に補償を加えることによってロボットアームの振動抑制を行っている。
【００３８】
このように、センサ信号を周波数解析して主成分の振動パラメータ（振幅、周波数、位相）から速度の近似波形を求めてそれを基に補償信号を計算する構成をしているので、振動抑制の効果を向上できるとともに、振動パラメータが変化する場合のみパラメータの無線伝送を行っているのでセンサ基板における消費電力を低減することができる。
【実施例３】
【００３９】
図５は実施例３の構成を示す図である。無線モジュール７００はロボットコントローラ２０をモーション部に実装すると考える。無線伝送される振動のパラメータ、ロボットの剛体モデルとその制御系から得られる伝達関数と位置指令の周波数解析結果からモータ速度の振動成分を計算する場合を示す。
本実施例ではモーション部で計算された位置指令を周波数解析するウェーブレット変換器９００、周波数解析の結果をロボットの動作時間と周波数の２つのパラメータに対して信号強度と位相を与えるように作成された変換表９０１、変換表の出力と振動パラメータの周波数から位置指令に対する動作速度の伝達関数９０２、伝送された振動のパラメータからＳ軸モータの速度の周波数成分を与える周波数変換されたヤコビ逆変換９０３、振動成分の周波数成分から時間波形を生成する信号発生器８１を構成要素としている。
【００４０】
モーション部においてＳ軸モータとＬ軸モータの位置指令が生成される。それをウェーブレット変換９００において周波数解析してロボットの動作時間に対する位置指令の周波数成分の振幅と位相が計算できる。これをもとにしてロボットの動作時間と振動パラメータの周波数ωｘ（またはωｙ）に対して、位置指令の振幅と位相を与える変換表９０１が作成される。
剛体モデルに対して、振動パラメータの周波数ωｘ（またはωｙ）が与えられると伝達関数が計算される。図６に位置指令が与えられて剛体モデルが動作する場合のS軸モータのブロック図を示す。位置指令θrefに対するＳ軸モータ速度Ｖｓの伝達関数は次式に示すようになる。
【００４１】
Vs={(Kp・(Kv・S２+Ki・S))/(Jｓ・S３+Kv・S２+(Kp・Kv+Ki)・S+Kp・Ki)}・θref
・・・（３式）
【００４２】
無線回路７０に信号が受信されるとX軸方向及びY軸方向の主成分の振動の周波数ωｘ（ωｙ）が受信される。また受信された時刻からロボットの動作時間ｔ１が求まる。このωｘとｔ１の組から上で求めた変換表９０１によって位置指令が持つ振幅成分と位相成分が得られる。
求めた振幅と位相から得られる複素振幅に伝達関数をかけて位置指令が与えられたときにＳ軸モータの速度が持つ周波数成分を計算できる。これは位置指令が与えられるときのロボットの振動を含まない速度成分を得ている。
【００４３】
一方、伝送された振動パラメータにヤコビ逆行列をFFT変換した行列をかけてセンサのS軸モータ速度の周波数成分が得られる。
センサのＳ軸モータ速度の周波数成分に指令から得られるＳ軸モータ速度の周波数成分を補償してＳ軸モータ速度の振動の周波数成分を得る。
振動の周波数成分のパラメータ（周波数、振幅、位相）から信号発生器８１よってモータ速度の振動成分の時間波形を得ている。
この振動の時間波形を位置−速度制御系１００の速度指令に補償してロボットアーム２で発生する振動を抑制する効果を得ている。
Jinv（ω）を計算する位置指令はＳ軸エンコーダ及びＬ軸エンコーダから得られるそれぞれのモータ位置とすることができる。また、位置指令とエンコーダ位置を組み合わせて得られる計算値としてもよい。また、トルク指令を含む計算式から得られる計算値としてもよい。
【実施例４】
【００４４】
図７は第４実施例の構成を示す図である。位置−速度制御系１００とサーボドライバ２の間のトルク指令にノッチフィルタ５０を入れている。ノッチフィルタ５０はそれぞれ独立した２つのノッチフィルタ５１と５２を直列に接続して構成されている。
本実施例ではノッチフィルタ５１にはＸ軸センサの振動パラメータである周波数ωｘと振幅Ｖｘが接続され、ノッチフィルタ５２にはＹ軸センサの振動パラメータωｙと振幅Ｖｙが接続される。
ここでは２つのフィルタの次数はそれぞれ２次であると考え、２つのフィルタの伝達関数は
【００４５】
H(s)=(s２+ω0２)/(s２+(ω0/Q)*s+ω0２) ・・・ （４式）
であるとする。
ノッチフィルタ５１及び５２は中心周波数ω0とノッチフィルタの幅を決めるＱ値が外部信号を用いて変更できる構成とする。
無線センサモジュール１５では２軸センサの主成分の振動パラメータ（ωｘ、Ｖｘ、ωｙ、Ｖｙ）を計算してロボットコントローラ２０に伝送する。
ノッチフィルタの中心周波数ω0は入力される振動パラメータの周波数ωｘ（またはωｙ）に一致するように変更できる構成とする。また、Ｑ値は振動パラメータの振幅Ｖｘ（またはＶｙ）が大きいときはＱ値が高くなり、振幅Ｖｘが小さいときはＱ値が低くなるように構成される。
【００４６】
ωｘとωｙはロボットのＸ方向及びＹ方向の共振周波数と考えられ、各軸モータのトルク指令からこれらの周波数を持つ信号を除去することによってロボットアーム２で発生しやすい振動の励起を防止することができる。振動パラメータの変化に伴いノッチフィルタの中心周波数ω0とQ値を変化できるので、振動の励起を防止する効果を上げることができる。
【００４７】
このように、無線センサモジュール１５の信号解析装置１５２において周波数解析した振動パラメータ（周波数、振幅）をロボットコントローラ２０に伝送して、ロボットコントローラ２０に搭載されたノッチフィルタにより振動成分を除去する構成としているので振動抑制制御の効果を上げることができるとともに、センサ信号の無線伝送回数を減らして無線センサモジュール１５に搭載されたバッテリ１５４の消費電力を低減することができる。
【実施例５】
【００４８】
このタイプのロボットは第１アーム４と第２アーム５の長さを同じにする場合が多く、実施例５ではその長さをｄとした場合を図１１に説明している。
図１０は第５実施例の構成を示す図である。コントローラには指令生成器で与えられるＳ軸位置指令θＳとＬ軸位置指令θＬから各軸モータの振動周波数を計算する振動周波数計算器が内蔵されている。振動周波数計算器は、θＳとθＬからロボットの姿勢を計算して、各軸モータのイナーシャを計算する。振動周波数計算器は、モータと減速機のパラメータとロボットの姿勢に伴って変化するパラメータを計算することができる。これから得られるイナーシャ、バネ定数、減衰定数を用いて各モータ軸の減速機で発生する振動周波数ωＳ、ωＬが計算されて無線回路７０に入力される。無線回路７０は振動周波数ωＳとωＬを無線センサモジュールの無線回路７１に伝送する。
無線センサモジュールは、２軸加速度センサ、バンドパスフィルタ、無線回路７１から構成される。バンドパスフィルタには無線回路７１から得られる振動周波数ωＳ、ωＬが入力される。バンドパスフィルタはＹ軸センサ信号のωＳ周波数成分とωＬ周波数成分を求めるように２つパラレルに構成されていて、それぞれの中心周波数がωＳ、ωＬに変更される構成をしている。加速度センサのＹ軸成分ＡｙのωＳ周波数成分Ａｙ（ωＳ）、位相θｙ（ωＳ）、ωＬ周波数成分Ａｙ（ωＬ）、位相θｙ（ωＬ）を算出する機能がある。
バンドパスフィルタで得られた４つの信号は無線回路を介してコントローラに伝送される。
Ｓ軸振動振幅計算器ではＡｙ（ωＳ）、θＬを入力してθＬから計算されるＸを用いて（図１１を参照）Ｓ軸モータの振動加速度Ａｓ（ωＳ）が
Ａｓ（ωＳ）＝−Ａｙ（ωＳ）／Ｘ ・・・（５式）
と算出される。ここで、Ｘ＝√（２・ｄ２＋２・ｄ２＊Ｃｏｓ（θＬ））である。
Ｌ軸振動振幅計算器ではＡｙ（ωＬ）、θＬを入力して
Ａｌ（ωＬ）＝２・Ａｙ（ωＬ）／√（２・ｄ２＋２・ｄ２・Ｃｏｓ（θＬ）
・・・（６式）
と算出される。
２つの位相補償器にはオフラインの状態で求めた各振動周波数に対する２軸加速度センサから無線回路７０までの遅れ時間を求める手段が内蔵されていて計算されたωＳとωＬの周波数における遅れ時間を算出する機能がある。
Ｓ軸の信号発生器にはＡｓ（ωＳ）、θｓ（ωＳ）、ωｓ、位相補償器の出力が入力されＳ軸振動波形の近似波形が計算される。
Ｌ軸についても同様に近似波形が計算される。
Ｓ軸、Ｌ軸の振動波形の近似波形をモータの速度指令に入力して各軸モータの振動成分を補償してロボットアーム先端の振動を抑制するように構成される。
本実施例では２つの振動周波数成分の位相を求めてコントローラに伝送しているが、振幅を伝送するタイミングを位相情報で決まる時刻で行い、受信時にこの時刻を測定することによって位相情報を得るように構成することもできる。これによって伝送情報量を減らすことができ、無線センサモジュールの消費電力を低減することができる。
【符号の説明】
【００４９】
１ ロボットシステム
２ ロボット
３ 基台
４ Ｓ軸アーム
５ Ｌ軸アーム
６ フォーク
７ Ｓ軸モータ
１７ Ｓ軸エンコーダ
８ Ｌ軸モータ
１８ Ｌ軸エンコーダ
１５ 無線センサモジュール
１５１ ２軸加速度センサ
１５２ 信号解析装置
２０ ロボット制御装置
７０、７１ 無線回路
８１、８２ 信号発生器
９０ ヤコビ逆変換
５０ ノッチフィルタ
５１、５２ ２次ノッチフィルタ
W ワーク
１００ 位置−速度制御系
２０１ ロボット用多軸制御演算装置
２０２ ロボット用多軸アンプ
３００，３０１ バンドパスフィルタ
【技術分野】
【０００１】
本発明は、例えばロボット制御装置に関し、詳しくはロボットアームの先端に加速度センサを取り付けて発生する振動の信号をロボットコントローラに伝送する無線伝送装置とそれを応用してアーム先端の振動抑制を行うロボットの振動抑制制御装置に関する。
【背景技術】
【０００２】
半導体分野で使用されるクリーンロボットでは生産性を向上させるためロボットの高速動作が必要とされている。高速動作を行う場合アームが持つ機構の振動が励起され、ロボットが動作停止した後も動作方向と垂直な方向にアームが振動する残留振動が発生するという問題がある。
クリーンロボットがウェハを搬送してカセットに収納する場合、残留振動が発生すると
ウェハとカセットが衝突してウェハの破損の原因となるため、残留振動が収まるのを待たなければならず、生産性の向上を阻害する原因の１つとなっている。
ロボットアームの振動抑制方法の一つとして加速度センサをアーム先端に取り付け、振動によって発生する加速度信号といくつかのロボットパラメータを用いて補償信号を生成して、各軸モータのトルク指令に得られる補償信号をフィードバックして振動を抑制する方法が開示されている（特開平１０−１０００８５）。
そのため、従来の方法では加速度センサ信号は時系列データとしてロボットコントローラに伝送し、ロボットコントローラ内部で各種のロボットパラメータを参照しながら一括処理されている。
【０００３】
一方、ロボットに加速度センサを取り付ける場合、配線レスにして使用することが望まれる（特許文献５、特許文献６）。なぜなら配線があると、
（１）旋回動作を行う場合、その動作範囲が制限される。
（２）センサに取り付けた配線にはロボットの動作によってねじれや引っ張りの力が加わり断線または配線の接触不良が発生する。
（３）加速度センサの取り付け位置に制限を受ける。
（４）配線をロボット内部にはわせる手間がかかる。
などの理由があるためである。
【０００４】
図１４に特許文献５のセンサ装置のブロック図を示す。この発明では上記のようにロボットアーム先端の加速度情報を時系列データとして伝送するセンサ装置が示され電源ＯＮ／ＯＦＦを行うことによる省電力化の方法が開示されている。
図１５に特許文献６の無線出力センサのブロック図を示す。図に示されるように従来の発明では検出対象を検出する加速度センサ部と、上記加速度センサ部の検出に係るセンサデータを無線で送信処理をする無線送信部と、上記無線送信部の処理に係るセンサデータを送信する送信アンテナと、上記加速度センサ部と無線送信部それぞれの電源となる電池を搭載する電池搭載部と、を備えたことを特徴とする無線出力センサの技術が開示されている。
【０００５】
図１５の無線出力センサでは３軸加速度センサ１８３の出力信号の大きさに応じてＡＣアンプ部１８４において振動波形が生成され、その信号を復調器１８５と出力部１８６において検出対象の可動の有無を検出する信号に変換されている。この可動の有無の信号を発振部１３３においてＡＣ信号とし、アンプ部１３４とフィルタ１３５において処理した後アンテナ２４を介して無線出力する技術が開示されている。
【０００６】
図１４及び図１５に示される様に、加速度センサを無線化する場合、現在のところ測定データは無線を用いて伝送し、センサ基板で消費される電力は基板内部に設けるバッテリによって供給することが現実的である。また、特許文献５及び６ではソーラー充電式または振動充電式の補助電源を使用してバッテリ寿命を延ばすことが提案されている。
センサ基板に使用される電子部品の消費電力の一例を図１６に示す。センサ基板で消費される電力の大半は無線で消費されていることが分かる。
【０００７】
また、非特許文献１に示されるように建築構造物に多数の加速度センサを配置して、各センサの信号を中央の親局に無線伝送して、親局に伝送された多数の信号をもとにして地震などの災害の発生を検出する構造ヘルスモニタリングシステムが開発されている。
このシステムにおいては、各センサの信号はセンサ基板に設けられた信号処理回路において転送するデータ量が最小となるように加工して、センサ基板から親局に無線伝送される伝送量を低減させて、センサ基板に設けられたバッテリの消費を低減させる技術が開示されている。
【０００８】
特許文献１には、タイヤ側に設けられた加速度センサ信号から振動レベル演算値を計算して、車体側に無線を用いて伝送する路面状態推定装置の技術が開示されている。図１２に路面状態推定システムの構成を示すブロック図を示す。タイヤに装着された加速度センサの信号は周波数分析手段において周波数分析され、振動レベル演算値が計算されて車体側に設けられた路面状態推定装置に無線伝送される。路面状態推定装置では伝送された振動レベル演算値を用いて車両の走行している路面状態を判定する。
【０００９】
特許文献２、特許文献３、特許文献４にはベアリングの固定部分に加速度センサを設置して、ベアリングが回転するときに発生する振動加速度をFFT処理し、回転速度信号の周波数成分と比較照合して、回転部品の異常の有無と異常部位を検出する機械設備の異常診断装置が開示されている。
図１３に異常診断装置の信号処理部のブロック図を示す。検出部に設けられた加速度センサの信号は信号処理部の診断用スペクトル算出部によって周波数分析され、その結果が回転情報を回転分析部で周波数分析した結果と比較されてベアリングの異常の有無が判定されている。
【００１０】
このように、従来の加速度センサを用いた無線伝送装置は、加速度センサから得られるデータを時系列で伝送してコントローラで測定対象物の状態推定や異常検出の処理のすべてを行うか、センサ側でデータを周波数分析などの手段を用いて加工することにより伝送情報量を低減させた後、コントローラで測定対象物の状態推定や異常検出を行うとともに無線伝送で消費される電力の低減をはかることが行われている。
【先行技術文献】
【特許文献】
【００１１】
【特許文献１】特開２００７−５５２８４（ブリヂストン）
【特許文献２】特開２００７−２６３６０９（日本精工）
【特許文献３】特開２００７−２８５８７５（日本精工）
【特許文献４】特開２００９−２００９０（日本精工）
【特許文献５】特開２００８−１８６３３６（安川電機）
【特許文献６】特開２００８−１７１４０３（光洋電子工業）
【非特許文献】
【００１２】
【非特許文献１】「スマートセンサと無線ネットワークを用いた構造ヘルスモニタリングシステムの開発」 日本地震工学会論文集 第7 巻、第6 号、2007 p.17-30
【発明の概要】
【発明が解決しようとする課題】
【００１３】
特許文献６の発明では対象物の可動の有無の情報のみを検出している。無線通信を0.5秒間だけワンショット送信する構成（段落番号（００４５）参照）としてもロボット動作の状態が検出でき、2[sample/s]より少ないサンプルレートとなっていても問題なく、電池の寿命もある程度の期間が得られると考えられる。しかし、アームの振動抑制制御の場合、加速度信号の時系列の信号が必要である。アームの振動周波数が１０[Ｈｚ]程度であり、その１０〜３０倍程度の300[sample/s]程度の時系列データが必要であるため伝送情報量が大きくなり、図１６の無線センサモジュールの電子部品の場合、電池寿命は数日程度となる。
特許文献５のセンサ装置の発明においても同様で電池寿命の短さが問題となり、無線化した加速度センサを振動抑制制御などに応用できない理由の１つである。
【００１４】
従来の構造ヘルスモニタリングシステムの場合、センサデータの加工は、各センサで独立に行なわれ、センサ基板上で加速度のゼロクロス点数と最大値および振幅絶対値の累積値である損傷指標に変換されて無線通信によって親局に伝送される。
また従来の路面状態推定装置の場合、センサ信号の処理のほとんどをタイヤ側で行なっている。加速度センサはタイヤの回転とともに重力の影響を受けるが、車両の走行に伴い発生する振動の加速度が４０[g]程度であるので回転による加速度変化（±１[ｇ]）を補正する必要はない。
また従来の機械設備の異常診断装置においてもセンサ信号の処理は独立して行われ、
工作機械が動作して発生する加速度の考慮はなされていない。一般に工作機械では機械の動作による加速度と異常振動の加速度の周波数は異なるものとなり、機械の動作による加速度はノイズとして取り扱われて周波数解析などの手段を用いて除去することがおこなわれている。すなわち、制御器のパラメータと組み合わせてセンサ信号の処理は行われていない。
【００１５】
加速度センサを用いたロボットの振動抑制制御では、ロボットの姿勢や動作状態によって変化するロボットの各種のパラメータと組み合わせて加速度信号を処理してモータのトルク指令に加える補償信号の算出を行なわなければならない。そのため各センサにおけるデータの加工は、ロボットコントローラ側でロボットのパラメータと組み合わせて補正処理ができる範囲でなければならない。
【００１６】
また、ロボットにおいては振動と動作の加速度の周波数帯域が重なり、振動と動作の加速度の大きさも似たような大きさになるため、ロボットが動作して発生する加速度をノイズとして処理することができず、姿勢や動作状態から計算される動作の加速度をセンサの信号から補正する方法（特開2005−316937）が有効になり、従来の無線伝送装置の技術の組み合わせでは振動抑制の効果を上げることができない。
このような信号処理をロボットコントローラでおこなうためには特許文献５に示すように時系列でセンサ信号を伝送してロボットコントローラ側ですべての処理を行う必要がある。
【００１７】
ロボットの振動抑制制御のようにセンサ信号の高いサンプリング速度が要求される場合、センサ信号を常時伝送する必要があり、無線回路が常時送信動作を行っている状態になるため、市販の乾電池を用いたとすると数日程度でバッテリの電力を消費してしまい、バッテリの頻繁な交換または充電が必要になる。
特許文献５及び６には、補助電源によるバッテリ寿命を延ばすことが述べられているが図１６に示した消費電力の場合、ソーラー充電の場合は太陽電池パネルの面積を大きくとる必要があり取り付けが難しくなる。振動充電の場合も現在得られている圧電素子の体積あたりの発生電力が小さいため電源として補助的な効果を得るためには体積を大きくとる必要があり、ロボットアーム先端に取り付けることがむつかしくなる。
加速度センサを配線レスで使用して振動抑制制御などのリアルタイム性が要求される用途に応用する場合、ロボットの制御性能の向上が達成されるだけではなく、センサ基板の低消費電力化を同時に達成できる工夫をしなければ実用化することができないという問題がある。
【００１８】
無線センサモジュールの構成部品別の消費電力は図１６で分かるように無線回路が常時送信動作を行う場合、他の構成部品に比べてかなり大きくなる。また、センサを含めた信号処理回路の部品の消費電力は、この数年で大きく低減されている。このため測定データの伝送情報量を低減させて無線回路の消費電力が低減できればセンサ基板の低消費電力化に有効な方法のひとつになる。
【００１９】
また、振動抑制制御などのリアルタイム性が要求される用途では無線伝送の伝送遅れ時間によって加速度信号に位相遅れが生じ、制御性能の向上が制約される問題がある。
無線回路としては高性能・低価格化が進むブルートゥースなどのデジタル無線装置の応用が考えられるが、市販の無線回路を用いる場合、サンプリングによる遅れなどのために5[ms]程度の伝送遅れが発生するため、10[Hz]の振動波形の場合18°程度の位相遅れとなり、振動抑制制御の制御性能の劣化の原因となる。
【００２０】
本発明はこのような問題点に鑑みてなされたものであり、無線化される加速度センサを使用してロボットの振動抑制制御を行う場合、振動抑制の効果を向上するとともに無線センサモジュールの消費電力を低減するロボット制御装置を提供することを目的とする。
【課題を解決するための手段】
【００２１】
上記問題を解決するため、本発明は、次のように構成したのである。
本発明は、一対の無線回路を用いてセンサ信号をコントローラに伝送する無線伝送装置において、前記センサ信号を周波数解析して主成分である振動の周波数、振幅、位相からなるパラメータを得て、前記パラメータが変化したとき前記無線回路から前記パラメータを送出する信号解析装置と、受信した振動の前記パラメータを用いて前記センサ信号の近似波形を生成する信号発生器を有することを特徴とする無線伝送装置とするものである。
【００２２】
また、本発明は、請求項１記載の無線伝送装置と、送信される２軸のセンサ信号からロボットの各軸モータ速度を求めるヤコビ逆変換手段と、ロボットの位置指令から各軸モータが動作したときに発生する速度を求める手段を有し、センサから得られるモータ速度から動作したときに発生する速度を差し引いてモータ速度の振動成分を求め、モータの位置−速度制御ループの速度指令にモータ速度の振動成分を補償してアームに発生する振動を抑制することを特徴とするロボット制御装置とするものである。
【００２３】
また、本発明は、請求項１記載の無線伝送装置と、モーション部で生成される位置指令を周波数解析した結果からロボットの動作したときに発生するモータ速度の周波数成分を求める手段を有し、
センサ速度信号の周波数成分から動作したときに発生する速度の周波数成分を差し引き、モータ速度の振動成分の周波数成分を求め、得られる振動の周波数、振幅、位相のパラメータからモータ速度が持つ振動の近似波形を生成して、モータの位置−速度制御ループに振動成分として補償してアームに発生する振動を抑制することを特徴とするロボット制御装置とするものである。
【００２４】
また、本発明は、請求項１記載の信号解析装置と伝送されるセンサ信号の主成分の振動の周波数と振幅によって中心周波数とＱ値が変更できるノッチフィルタを用いてモータのトルク指令からセンサ信号の主成分の振動周波数成分を除去することを特徴とするロボット制御装置とするものである。
【００２５】
また、本発明は、ロボットコントローラにおいてアームを構成する各軸モータの振動周波数を計算して無線センサモジュールに伝送して、無線センサモジュールにおいてはＹ軸センサ信号の２つの振動周波数の周波数成分の振幅および位相を計算して前記ロボットコントローラに伝送し、前記ロボットコントローラでは伝送された振幅、位相及び計算によって得られる振動周波数、ロボットの姿勢情報を用いてＳ軸振動振幅計算器、Ｌ軸振動振幅計算器でＳ軸モータの振動振幅及びＬ軸モータの振動振幅を計算し、Ｓ軸モータの振動振幅とＳ軸振動周波数におけるＹ軸信号の位相とＳ軸振動周波数から信号発生器によってＳ軸振動波形を合成し、Ｌ軸モータの振動振幅とＬ軸振動周波数におけるＹ軸信号の位相とＬ軸振動周波数から信号発生器によってＬ振動波形を合成し、前記２つの合成波形を用いて振動抑制制御を行うことを特徴とするロボット制御装置とするものである。
【００２６】
また、本発明は、センサ信号の周波数解析により得られる振幅と位相の情報に基づいて振幅の情報の伝送を位相の情報に基づいたタイミングで行い、ロボットコントローラでは伝送される振幅の情報の到着時刻に基づいて周波数解析で得られる位相を計算して、前記振幅と計算された位相とコントローラ内部の振動周波数から振動波形を合成する信号発生器を構成したことを特徴とする請求項５記載のロボット制御装置とするものである。
【００２７】
また、本発明は、各周波数に対応する伝送遅れ時間のオフラインでの測定結果を内蔵する位相補償器を有して、振動周波数計算器で得られるＳ軸振動周波数ωＳ、Ｌ軸振動周波数ωＬにおける伝送遅れ時間を信号発生器に入力し、無線センサモジュール及び２つの無線回路で発生する伝送遅れ時間を補償する信号発生器を構成したことを特徴とする請求項５または６記載のロボット制御装置とするものである。
【発明の効果】
【００２８】
本発明によると、振動パラメータが変化する場合のみパラメータの伝送を行えばよく、無線伝送の回数を低減できるため、センサ装置の低消費電力化を図ることができる。
また、本発明によると、信号発生器のセンサ信号の近似波形を用いて各軸モータの補償信号の算出ができ、補償信号を制御ループにフィードバックして振動抑制の効果を上げることができる。また、振動パラメータが変化する場合のみパラメータの伝送を行うのでセンサ装置の低消費電力化を図ることができる。
また、本発明によると、伝送された振動パラメータと位置指令の周波数解析の結果を用いてモータ速度の振動の周波数成分が求まる。それを用いてモータ速度の振動の時間波形が得られる。これを補償信号として制御ループにフィードバックして振動抑制の効果を上げることができる。また、振動パラメータが変化する場合のみパラメータの伝送を行うのでセンサ装置の低消費電力化を図ることができる。
また、本発明によると、伝送される振動パラメータの周波数と振幅に従って中心周波数とＱ値を変化できるノッチフィルタをトルク指令に挿入して、トルク指令からロボットアームの振動周波数成分を除去することができ、ロボットアームの振動励起を防止することができる。また、機械の振動パラメータが変化する場合のみパラメータの伝送を行うのでセンサ装置の低消費電力化を図ることができる。
また、本発明によると、各モータ軸で発生する振動波形がＹ軸センサ信号のＳ軸共振周波数成分、Ｌ軸共振周波数成分及びコントローラ内の信号情報から生成することができるので、無線センサモジュールとコントローラの間の伝送情報量を減らすことができ無線センサモジュールの消費電力を低減することができる。
また、本発明によると、振動周波数成分の解析によって得られる位相情報を振動周波数成分から得られる振幅の伝送を行う時刻の情報として伝送できるので位相データの伝送が省略でき、伝送情報量を減らすことが可能となる。無線センサモジュールの低消費電力化を図ることができる。
また、本発明によると、無線センサモジュールとコントローラの間の無線伝送で発生する伝送遅れ時間を補償できるので、ロボットの動作で発生する振動加速度に位相が合った補償信号が生成でき、振動抑制制御の制御性能の向上を図ることができる。
【図面の簡単な説明】
【００２９】
【図１】本発明の第１実施例を示す無線伝送装置のブロック図
【図２】本発明の信号発生器の内部構成を示すブロック図
【図３】本発明の第２実施例を示す振動抑制制御装置のブロック図
【図４】第２実施例のヤコビ逆変換を説明するためのヤコビ行列の要素の式
【図５】本発明の第３実施例を示す振動抑制制御装置のブロック図
【図６】第３実施例の伝達関数を説明するためのブロック図
【図７】本発明の第４実施例を示す振動抑制制御装置のブロック図
【図８】ロボットアームの動作を説明する構成図
【図９】ロボットコントローラの内部を説明する構成図
【図１０】本発明の第５実施例を示す振動抑制装置のブロック図
【図１１】第５実施例を説明するための構成図
【図１２】従来の路面状態推定装置を説明するための構成図
【図１３】従来の機械設備の異常診断装置を説明するための構成図
【図１４】従来のセンサ装置を説明するためのブロック図
【図１５】従来の無線出力センサを説明するためのブロック図
【図１６】無線センサモジュールの構成部品の消費電力を示す図
【発明を実施するための形態】
【００３０】
以下、本発明の実施の形態について図を参照して説明する。
図８及び図９は本発明の実施形態に用いられるロボットを説明するための図である。図８はロボットを含むロボットシステムの概要を模式的に示す図である。図８に示すようにロボットシステム１は水平多関節型のロボット２及びロボットコントローラ２０により構成されている。そして、ロボット２は、基台３，第１アーム４，第２アーム５，フォーク６，S軸モータ７、L軸モータ８及び無線センサモジュール１５を備えている。基台３は、円柱状に形成されており上下方向に昇降可能に構成されている。第１アーム４は、水平面内で旋回可能に基台３にベアリング等を介して取り付けられて、第１アーム根元側に取り付けられたＳ軸モータ７の回転運動によって旋回動作を行う。また、第１アーム４の先端側には水平面内で旋回可能に第２アーム５が取り付けられて、第２アームの根元側に取り付けられたＬ軸モータ８の回転運動によって旋回動作を行う。ここで説明しているクリーンロボットの多くの場合、各軸のモータがアームの回転中心を直接駆動する構成とはならず、モータには減速機が挿入され、減速された出力をベルト等を介してアームの回転方向に印加してアームを旋回動作させる構成となっている。ここでは減速機とベルト等の機構を含めてＳ軸モータ７・Ｌ軸モータ８として、減速比１のモータが直接アームの回転軸を駆動するとして実施例の説明を行っている。
【００３１】
第２アーム５の先端側には、フォーク６が水平面内で旋回自在に取り付けられており、フォーク６は搬送するワークＷが載置可能に形成されている。第２アーム５およびフォーク６は、L軸モータ８の駆動によって予め定められた速度比（回転比）を保って旋回するようになっており、これによりフォーク６を基台３の半径方向に移動できるように構成されている。
無線センサモジュール１５は２軸加速度センサ１５１と信号解析装置１５２と無線回路71とバッテリ１５４から構成され、２軸加速度センサ１５１で水平面内の加速度が測定され、そのデータを信号解析装置１５２で周波数解析して測定パラメータを無線回路71からロボットコントローラ２０に無線伝送するようになっている。バッテリ１５４は無線センサモジュール１５の各部品の電源となる。
【００３２】
ロボットアーム２はロボットコントローラ２０によって動作が制御される。図９にロボットコントローラ２０の機能の概略を示すブロック図を示す。ロボットコントローラ２０はロボット用多軸制御演算装置２０１とロボット用多軸アンプ２０２により構成される。
ロボットコントローラ２０の構成例としてロボット用多軸制御演算装置２０１はパーソナルコンピュータを用い、ロボット用多軸アンプ２０２はロボットの各軸モータをトルク制御できる複数のモータ制御器を用いて構成することができる。この場合、ロボット用多軸制御演算装置２０１の主要機能であるモーション部とサーボ部は、パーソナルコンピュータ内部で実行されるソフトウェアによって構成され、実行周期の異なる２つのタスクとなっている。
【００３３】
ロボット動作プログラムはあらかじめユーザによって作成され、ロボット用多軸制御
演算装置２０１上の不揮発性メモリ内部に記憶されている。ロボットを動作させるときは、ロボット動作プログラムを読み出しモーション部で解読して各軸モータの位置指令を計算する。サーボ部では、モーション部で計算された位置指令とモータに取り付けられたエンコーダから得られる各軸モータ位置を用いて位置−速度制御系を構成して各軸モータのトルク指令を計算している。
ロボット用多軸アンプ２０２では、サーボ部で計算された各軸モータのトルク指令に
相当する電流を各軸モータに供給してモータ位置とモータ速度を制御して動作プログラムによって生成されるロボットの姿勢と動作を得ている。
ここでは、アーム先端の振動検出手段としてロボット２のフォーク６の付け根に取り付けられた２軸加速度センサ（X軸センサ、Y軸センサ）を用いて振動を検出して無線センサモジュール１５とロボットコントローラ２０内部の無線回路１５３と無線回路７０の間で測定信号を無線伝送してロボットコントローラ２０内部に測定信号を取り込むことを考える。
【００３４】
以下、本発明の実施の形態について図を参照して説明する。
【実施例１】
【００３５】
図１は、本実施形態の信号伝送装置のブロック図を示す。図において、ロボットアーム２にはその水平方向の加速度のＸ軸方向成分とＹ軸方向成分を測定する２軸加速度センサ１５１とその周波数解析を行う信号解析装置１５２と解析結果をロボットコントローラ２０に無線を用いて伝送する無線回路71とこれらの電源であるバッテリ１５４を有する無線センサモジュール１５が搭載されている。ロボットコントローラ２０には無線回路７０、信号発生器８１から構成される無線モジュール７００が搭載されている。
信号発生器８１は図２に示すブロックで構成されている。信号発生器８１には無線回路で受信された振動のパラメータである周波数ω、振幅Ｖ、位相θが入力される。これらのパラメータは受信が完了すると信号発生器８１のレジスタにラッチされる。また、信号発生器８１の内部にはタイマがあり、データのラッチされた時刻をt0をラッチする。正弦波計算器ではタイマが示す時刻tを用いて
A=V・SIN(ω・（t−t0）＋θ） ・・・（１式）
に従い計算が行われ、加速度近似波形Ａが出力される。
【００３６】
ロボットアーム２が水平面内で動作して２軸加速度センサ１５１が発生する信号は信号解析装置１５２の内部のCPU（図示されず）によって周波数解析が行われ振動の主成分の振動パラメータ（周波数ω、振幅Ｖ、位相θ）が求められる。
周波数解析の手段としてはロボットアームの振動抑制制御を行う場合、リアルタイム性が要求されるため、ウェーブレット変換を行うことを想定している。求められた各軸の主成分の振動パラメータは無線回路71と無線回路７０の間で無線伝送されロボットコントローラ２０内部に取り込まれる。
コントローラに取り込まれた振動パラメータは信号発生器８１の内部にセットされ加速度センサ信号の主成分のみからなる加速度近似波形Ａがロボットコントローラ２０内部で発生される。
実施例１では、無線伝送は信号解析装置１５２で計算される振動パラメータの要素が一定値以上の変化をする場合のみに行うものと考える。
主成分の信号が周波数、振幅、位相が一定である正弦波である場合は無線回路71と無線回路７０の間の振動パラメータの伝送は1回行われるとそれ以後の加速度近似波形Ａの計算は信号発生器８１内部で行われ、無線伝送の回数を大幅に減少させることが可能となる。
図１では伝送信号が各軸センサの主成分のみの場合を考えたが、成分が複数となるように構成することも可能であり、信号発生器８１が複数の正弦波計算器を持ち、その和によって加速度近似波形Ａを求めるように構成する。この構成によって加速度近似波形Ａを加速度センサ１５１の信号に近づけることができる。
【実施例２】
【００３７】
図３は実施例２の構成を示す図である。実施例１で示した無線センサモジュール１５と無線モジュール７００をロボットシステム１内部に設けてロボットアーム２に発生する振動を抑えるために振動抑制制御装置を構成した場合を示す。
無線モジュール７００はロボットコントローラ２０内部のサーボ部に実装される。信号解析装置１５２ではＸ軸加速度センサとＹ軸加速度センサの振動加速度が周波数解析されて、それぞれの軸の主成分の振動パラメータが求められる。ここではＸ軸方向の主成分の周波数ωｘとＹ軸方向の主成分の振動周波数ωｙが異なり、求められる振動パラメータは信号解析装置１５２内部で加速度から速度に変換されて信号処理されているとして説明する。
ここではＳ軸モータの制御ブロック図のみを示している。Ｌ軸モータの制御ブロックも同様に構成される。
信号発生器８２は第１の実施例で説明した信号発生器８１が２軸分内蔵されている。２つの信号発生器８１には無線回路７０で受信されるＸ軸方向とＹ軸方向の振動パラメータがセットされてそれぞれの速度近似波形がロボットコントローラ２０内部で生成される。
２つの速度近似波形はヤコビ逆変換によってＳ軸モータ及びＬ軸モータの速度に変換される。ヤコビ逆変換はヤコビ逆行列９０をかけて行われる。ヤコビ逆行列は図８の水平多関節ロボットの場合図４の（２式）で示される２ｘ２行列となる。
各要素は２つのモータの関節角θsとθlに依存して、ロボットの姿勢が変わると変化する。
モータの制御系はモーション部から与えられる位置指令とモータに取り付けられたエンコーダ位置を用いて位置ループがＰ制御、速度ループがＰ−Ｉ制御で構成される制御ループと制御ループと同じ位置指令と制御パラメータを用いてロボットの剛体モデルの動作を逐次計算するシミュレーション部から構成される。シミュレーション部ではロボットの姿勢で変化するイナーシャを逐次代入して剛体モデルの速度を求めている。
ここではＸ軸速度とＹ軸速度の主成分ωｘとωｙのみを考えているのでシミュレーションで得られる速度も２つの周波数成分のみを持つように、主成分の周波数ωｘとωｙの変化にともなって中心周波数がωｘ、ωｙに一致するように変化するバンドパスフィルタ３００、３０１を一対入れている。
センサから得られるＳ軸モータ速度からシミュレーションで得られるＳ軸モータ速度を引いた結果を振動成分として、メインループの速度指令に補償を加えている。
ロボットと等価なイナーシャを持つ剛体モデルにロボットが動作するときのＳ軸位置指令を与えてシミュレーションを行っているため、シミュレーションで得られる動作速度は振動成分を含まないロボットの動作を近似していて、ロボットの動作によって発生する速度を表している。センサで測定する速度には振動で発生する成分とともに動作で発生する速度成分が含まれるため、シミュレーションで求まるロボットの動作で発生する成分を除去することによって振動で発生する速度成分を求めることができる。この振動の速度成分をメインループの速度指令に補償を加えることによってロボットアームの振動抑制を行っている。
【００３８】
このように、センサ信号を周波数解析して主成分の振動パラメータ（振幅、周波数、位相）から速度の近似波形を求めてそれを基に補償信号を計算する構成をしているので、振動抑制の効果を向上できるとともに、振動パラメータが変化する場合のみパラメータの無線伝送を行っているのでセンサ基板における消費電力を低減することができる。
【実施例３】
【００３９】
図５は実施例３の構成を示す図である。無線モジュール７００はロボットコントローラ２０をモーション部に実装すると考える。無線伝送される振動のパラメータ、ロボットの剛体モデルとその制御系から得られる伝達関数と位置指令の周波数解析結果からモータ速度の振動成分を計算する場合を示す。
本実施例ではモーション部で計算された位置指令を周波数解析するウェーブレット変換器９００、周波数解析の結果をロボットの動作時間と周波数の２つのパラメータに対して信号強度と位相を与えるように作成された変換表９０１、変換表の出力と振動パラメータの周波数から位置指令に対する動作速度の伝達関数９０２、伝送された振動のパラメータからＳ軸モータの速度の周波数成分を与える周波数変換されたヤコビ逆変換９０３、振動成分の周波数成分から時間波形を生成する信号発生器８１を構成要素としている。
【００４０】
モーション部においてＳ軸モータとＬ軸モータの位置指令が生成される。それをウェーブレット変換９００において周波数解析してロボットの動作時間に対する位置指令の周波数成分の振幅と位相が計算できる。これをもとにしてロボットの動作時間と振動パラメータの周波数ωｘ（またはωｙ）に対して、位置指令の振幅と位相を与える変換表９０１が作成される。
剛体モデルに対して、振動パラメータの周波数ωｘ（またはωｙ）が与えられると伝達関数が計算される。図６に位置指令が与えられて剛体モデルが動作する場合のS軸モータのブロック図を示す。位置指令θrefに対するＳ軸モータ速度Ｖｓの伝達関数は次式に示すようになる。
【００４１】
Vs={(Kp・(Kv・S２+Ki・S))/(Jｓ・S３+Kv・S２+(Kp・Kv+Ki)・S+Kp・Ki)}・θref
・・・（３式）
【００４２】
無線回路７０に信号が受信されるとX軸方向及びY軸方向の主成分の振動の周波数ωｘ（ωｙ）が受信される。また受信された時刻からロボットの動作時間ｔ１が求まる。このωｘとｔ１の組から上で求めた変換表９０１によって位置指令が持つ振幅成分と位相成分が得られる。
求めた振幅と位相から得られる複素振幅に伝達関数をかけて位置指令が与えられたときにＳ軸モータの速度が持つ周波数成分を計算できる。これは位置指令が与えられるときのロボットの振動を含まない速度成分を得ている。
【００４３】
一方、伝送された振動パラメータにヤコビ逆行列をFFT変換した行列をかけてセンサのS軸モータ速度の周波数成分が得られる。
センサのＳ軸モータ速度の周波数成分に指令から得られるＳ軸モータ速度の周波数成分を補償してＳ軸モータ速度の振動の周波数成分を得る。
振動の周波数成分のパラメータ（周波数、振幅、位相）から信号発生器８１よってモータ速度の振動成分の時間波形を得ている。
この振動の時間波形を位置−速度制御系１００の速度指令に補償してロボットアーム２で発生する振動を抑制する効果を得ている。
Jinv（ω）を計算する位置指令はＳ軸エンコーダ及びＬ軸エンコーダから得られるそれぞれのモータ位置とすることができる。また、位置指令とエンコーダ位置を組み合わせて得られる計算値としてもよい。また、トルク指令を含む計算式から得られる計算値としてもよい。
【実施例４】
【００４４】
図７は第４実施例の構成を示す図である。位置−速度制御系１００とサーボドライバ２の間のトルク指令にノッチフィルタ５０を入れている。ノッチフィルタ５０はそれぞれ独立した２つのノッチフィルタ５１と５２を直列に接続して構成されている。
本実施例ではノッチフィルタ５１にはＸ軸センサの振動パラメータである周波数ωｘと振幅Ｖｘが接続され、ノッチフィルタ５２にはＹ軸センサの振動パラメータωｙと振幅Ｖｙが接続される。
ここでは２つのフィルタの次数はそれぞれ２次であると考え、２つのフィルタの伝達関数は
【００４５】
H(s)=(s２+ω0２)/(s２+(ω0/Q)*s+ω0２) ・・・ （４式）
であるとする。
ノッチフィルタ５１及び５２は中心周波数ω0とノッチフィルタの幅を決めるＱ値が外部信号を用いて変更できる構成とする。
無線センサモジュール１５では２軸センサの主成分の振動パラメータ（ωｘ、Ｖｘ、ωｙ、Ｖｙ）を計算してロボットコントローラ２０に伝送する。
ノッチフィルタの中心周波数ω0は入力される振動パラメータの周波数ωｘ（またはωｙ）に一致するように変更できる構成とする。また、Ｑ値は振動パラメータの振幅Ｖｘ（またはＶｙ）が大きいときはＱ値が高くなり、振幅Ｖｘが小さいときはＱ値が低くなるように構成される。
【００４６】
ωｘとωｙはロボットのＸ方向及びＹ方向の共振周波数と考えられ、各軸モータのトルク指令からこれらの周波数を持つ信号を除去することによってロボットアーム２で発生しやすい振動の励起を防止することができる。振動パラメータの変化に伴いノッチフィルタの中心周波数ω0とQ値を変化できるので、振動の励起を防止する効果を上げることができる。
【００４７】
このように、無線センサモジュール１５の信号解析装置１５２において周波数解析した振動パラメータ（周波数、振幅）をロボットコントローラ２０に伝送して、ロボットコントローラ２０に搭載されたノッチフィルタにより振動成分を除去する構成としているので振動抑制制御の効果を上げることができるとともに、センサ信号の無線伝送回数を減らして無線センサモジュール１５に搭載されたバッテリ１５４の消費電力を低減することができる。
【実施例５】
【００４８】
このタイプのロボットは第１アーム４と第２アーム５の長さを同じにする場合が多く、実施例５ではその長さをｄとした場合を図１１に説明している。
図１０は第５実施例の構成を示す図である。コントローラには指令生成器で与えられるＳ軸位置指令θＳとＬ軸位置指令θＬから各軸モータの振動周波数を計算する振動周波数計算器が内蔵されている。振動周波数計算器は、θＳとθＬからロボットの姿勢を計算して、各軸モータのイナーシャを計算する。振動周波数計算器は、モータと減速機のパラメータとロボットの姿勢に伴って変化するパラメータを計算することができる。これから得られるイナーシャ、バネ定数、減衰定数を用いて各モータ軸の減速機で発生する振動周波数ωＳ、ωＬが計算されて無線回路７０に入力される。無線回路７０は振動周波数ωＳとωＬを無線センサモジュールの無線回路７１に伝送する。
無線センサモジュールは、２軸加速度センサ、バンドパスフィルタ、無線回路７１から構成される。バンドパスフィルタには無線回路７１から得られる振動周波数ωＳ、ωＬが入力される。バンドパスフィルタはＹ軸センサ信号のωＳ周波数成分とωＬ周波数成分を求めるように２つパラレルに構成されていて、それぞれの中心周波数がωＳ、ωＬに変更される構成をしている。加速度センサのＹ軸成分ＡｙのωＳ周波数成分Ａｙ（ωＳ）、位相θｙ（ωＳ）、ωＬ周波数成分Ａｙ（ωＬ）、位相θｙ（ωＬ）を算出する機能がある。
バンドパスフィルタで得られた４つの信号は無線回路を介してコントローラに伝送される。
Ｓ軸振動振幅計算器ではＡｙ（ωＳ）、θＬを入力してθＬから計算されるＸを用いて（図１１を参照）Ｓ軸モータの振動加速度Ａｓ（ωＳ）が
Ａｓ（ωＳ）＝−Ａｙ（ωＳ）／Ｘ ・・・（５式）
と算出される。ここで、Ｘ＝√（２・ｄ２＋２・ｄ２＊Ｃｏｓ（θＬ））である。
Ｌ軸振動振幅計算器ではＡｙ（ωＬ）、θＬを入力して
Ａｌ（ωＬ）＝２・Ａｙ（ωＬ）／√（２・ｄ２＋２・ｄ２・Ｃｏｓ（θＬ）
・・・（６式）
と算出される。
２つの位相補償器にはオフラインの状態で求めた各振動周波数に対する２軸加速度センサから無線回路７０までの遅れ時間を求める手段が内蔵されていて計算されたωＳとωＬの周波数における遅れ時間を算出する機能がある。
Ｓ軸の信号発生器にはＡｓ（ωＳ）、θｓ（ωＳ）、ωｓ、位相補償器の出力が入力されＳ軸振動波形の近似波形が計算される。
Ｌ軸についても同様に近似波形が計算される。
Ｓ軸、Ｌ軸の振動波形の近似波形をモータの速度指令に入力して各軸モータの振動成分を補償してロボットアーム先端の振動を抑制するように構成される。
本実施例では２つの振動周波数成分の位相を求めてコントローラに伝送しているが、振幅を伝送するタイミングを位相情報で決まる時刻で行い、受信時にこの時刻を測定することによって位相情報を得るように構成することもできる。これによって伝送情報量を減らすことができ、無線センサモジュールの消費電力を低減することができる。
【符号の説明】
【００４９】
１ ロボットシステム
２ ロボット
３ 基台
４ Ｓ軸アーム
５ Ｌ軸アーム
６ フォーク
７ Ｓ軸モータ
１７ Ｓ軸エンコーダ
８ Ｌ軸モータ
１８ Ｌ軸エンコーダ
１５ 無線センサモジュール
１５１ ２軸加速度センサ
１５２ 信号解析装置
２０ ロボット制御装置
７０、７１ 無線回路
８１、８２ 信号発生器
９０ ヤコビ逆変換
５０ ノッチフィルタ
５１、５２ ２次ノッチフィルタ
W ワーク
１００ 位置−速度制御系
２０１ ロボット用多軸制御演算装置
２０２ ロボット用多軸アンプ
３００，３０１ バンドパスフィルタ
【特許請求の範囲】
【請求項１】
一対の無線回路を用いてセンサ信号をコントローラに伝送する無線伝送装置において、
前記センサ信号を周波数解析して主成分である振動の周波数、振幅、位相からなるパラメータを得て、前記パラメータが変化したとき前記無線回路から前記パラメータを送出する信号解析装置と、受信した振動の前記パラメータを用いて前記センサ信号の近似波形を生成する信号発生器を有することを特徴とする無線伝送装置。
【請求項２】
請求項１記載の無線伝送装置と、送信される２軸のセンサ信号からロボットの各軸モータ速度を求めるヤコビ逆変換手段と、前記ロボットの位置指令から各軸モータが動作したときに発生する速度を求める手段を有し、センサから得られるモータ速度から動作したときに発生する速度を差し引いてモータ速度の振動成分を求め、モータの位置−速度制御ループの速度指令にモータ速度の振動成分を補償してアームに発生する振動を抑制することを特徴とするロボットの振動抑制制御装置。
【請求項３】
請求項１記載の無線伝送装置と、モーション部で生成される位置指令を周波数解析した結果からロボットの動作したときに発生するモータ速度の周波数成分を求める手段を有し、
センサ速度信号の周波数成分から動作したときに発生する速度の周波数成分を差し引き、モータ速度の振動成分の周波数成分を求め、得られる振動の周波数、振幅、位相のパラメータからモータ速度が持つ振動の近似波形を生成して、モータの位置−速度制御ループに振動成分として補償してアームに発生する振動を抑制することを特徴とするロボットの振動抑制制御装置。
【請求項４】
請求項１記載の信号解析装置と伝送されるセンサ信号の主成分の振動の周波数と振幅
によって中心周波数とＱ値が変更できるノッチフィルタを用いてモータのトルク指令からセンサ信号の主成分の振動周波数成分を除去することを特徴とするロボットの振動抑制制御装置。
【請求項５】
ロボットコントローラにおいてアームを構成する各軸モータの振動周波数を計算して無線センサモジュールに伝送して、無線センサモジュールにおいてはＹ軸センサ信号の２つの振動周波数の周波数成分の振幅および位相を計算して前記ロボットコントローラに伝送し、前記ロボットコントローラでは伝送された振幅、位相及び計算によって得られる振動周波数、ロボットの姿勢情報を用いてＳ軸振動振幅計算器、Ｌ軸振動振幅計算器でＳ軸モータの振動振幅及びＬ軸モータの振動振幅を計算し、Ｓ軸モータの振動振幅とＳ軸振動周波数におけるＹ軸信号の位相とＳ軸振動周波数から信号発生器によってＳ軸振動波形を合成し、Ｌ軸モータの振動振幅とＬ軸振動周波数におけるＹ軸信号の位相とＬ軸振動周波数から信号発生器によってＬ軸振動波形を合成し、前記２つの合成波形を用いて振動抑制制御を行うことを特徴とするロボット制御装置。
【請求項６】
センサ信号の周波数解析により得られる振幅と位相の情報に基づいて振幅の情報の伝送を位相の情報に基づいたタイミングで行い、ロボットコントローラでは伝送される振幅の情報の到着時刻に基づいて周波数解析で得られる位相を計算して、前記振幅と計算された位相とコントローラ内部の振動周波数から振動波形を合成する信号発生器を構成したことを特徴とする請求項５記載のロボット制御装置。
【請求項７】
各周波数に対応する伝送遅れ時間のオフラインでの測定結果を内蔵する位相補償器を有して、振動周波数計算器で得られるＳ軸振動周波数ωＳ、Ｌ軸振動周波数ωＬにおける伝送遅れ時間を信号発生器に入力し、無線センサモジュール及び２つの無線回路で発生する伝送遅れ時間を補償する信号発生器を構成したことを特徴とする請求項５または６記載のロボット制御装置。
【請求項１】
一対の無線回路を用いてセンサ信号をコントローラに伝送する無線伝送装置において、
前記センサ信号を周波数解析して主成分である振動の周波数、振幅、位相からなるパラメータを得て、前記パラメータが変化したとき前記無線回路から前記パラメータを送出する信号解析装置と、受信した振動の前記パラメータを用いて前記センサ信号の近似波形を生成する信号発生器を有することを特徴とする無線伝送装置。
【請求項２】
請求項１記載の無線伝送装置と、送信される２軸のセンサ信号からロボットの各軸モータ速度を求めるヤコビ逆変換手段と、前記ロボットの位置指令から各軸モータが動作したときに発生する速度を求める手段を有し、センサから得られるモータ速度から動作したときに発生する速度を差し引いてモータ速度の振動成分を求め、モータの位置−速度制御ループの速度指令にモータ速度の振動成分を補償してアームに発生する振動を抑制することを特徴とするロボットの振動抑制制御装置。
【請求項３】
請求項１記載の無線伝送装置と、モーション部で生成される位置指令を周波数解析した結果からロボットの動作したときに発生するモータ速度の周波数成分を求める手段を有し、
センサ速度信号の周波数成分から動作したときに発生する速度の周波数成分を差し引き、モータ速度の振動成分の周波数成分を求め、得られる振動の周波数、振幅、位相のパラメータからモータ速度が持つ振動の近似波形を生成して、モータの位置−速度制御ループに振動成分として補償してアームに発生する振動を抑制することを特徴とするロボットの振動抑制制御装置。
【請求項４】
請求項１記載の信号解析装置と伝送されるセンサ信号の主成分の振動の周波数と振幅
によって中心周波数とＱ値が変更できるノッチフィルタを用いてモータのトルク指令からセンサ信号の主成分の振動周波数成分を除去することを特徴とするロボットの振動抑制制御装置。
【請求項５】
ロボットコントローラにおいてアームを構成する各軸モータの振動周波数を計算して無線センサモジュールに伝送して、無線センサモジュールにおいてはＹ軸センサ信号の２つの振動周波数の周波数成分の振幅および位相を計算して前記ロボットコントローラに伝送し、前記ロボットコントローラでは伝送された振幅、位相及び計算によって得られる振動周波数、ロボットの姿勢情報を用いてＳ軸振動振幅計算器、Ｌ軸振動振幅計算器でＳ軸モータの振動振幅及びＬ軸モータの振動振幅を計算し、Ｓ軸モータの振動振幅とＳ軸振動周波数におけるＹ軸信号の位相とＳ軸振動周波数から信号発生器によってＳ軸振動波形を合成し、Ｌ軸モータの振動振幅とＬ軸振動周波数におけるＹ軸信号の位相とＬ軸振動周波数から信号発生器によってＬ軸振動波形を合成し、前記２つの合成波形を用いて振動抑制制御を行うことを特徴とするロボット制御装置。
【請求項６】
センサ信号の周波数解析により得られる振幅と位相の情報に基づいて振幅の情報の伝送を位相の情報に基づいたタイミングで行い、ロボットコントローラでは伝送される振幅の情報の到着時刻に基づいて周波数解析で得られる位相を計算して、前記振幅と計算された位相とコントローラ内部の振動周波数から振動波形を合成する信号発生器を構成したことを特徴とする請求項５記載のロボット制御装置。
【請求項７】
各周波数に対応する伝送遅れ時間のオフラインでの測定結果を内蔵する位相補償器を有して、振動周波数計算器で得られるＳ軸振動周波数ωＳ、Ｌ軸振動周波数ωＬにおける伝送遅れ時間を信号発生器に入力し、無線センサモジュール及び２つの無線回路で発生する伝送遅れ時間を補償する信号発生器を構成したことを特徴とする請求項５または６記載のロボット制御装置。
【図１】
【図２】
【図３】
【図４】
【図５】
【図６】
【図７】
【図８】
【図９】
【図１０】
【図１１】
【図１２】
【図１３】
【図１４】
【図１５】
【図１６】
【図２】
【図３】
【図４】
【図５】
【図６】
【図７】
【図８】
【図９】
【図１０】
【図１１】
【図１２】
【図１３】
【図１４】
【図１５】
【図１６】
【公開番号】特開２０１１−１６１５６２（Ｐ２０１１−１６１５６２Ａ）
【公開日】平成２３年８月２５日（２０１１．８．２５）
【国際特許分類】
【出願番号】特願２０１０−２６４３６（Ｐ２０１０−２６４３６）
【出願日】平成２２年２月９日（２０１０．２．９）
【出願人】（０００００６６２２）株式会社安川電機 (2,482)
【Ｆターム（参考）】
【公開日】平成２３年８月２５日（２０１１．８．２５）
【国際特許分類】
【出願日】平成２２年２月９日（２０１０．２．９）
【出願人】（０００００６６２２）株式会社安川電機 (2,482)
【Ｆターム（参考）】
[ Back to top ]