JP2020011361A - 制御装置、水平多関節ロボットおよびロボットシステム - Google Patents

Abstract

【課題】シャフトの先端部におけるシャフトの軸方向の振動を低減する制御装置、水平多関節ロボットおよびロボットシステムを提供すること。【解決手段】基台と、前記基台に設けられ前記基台に対して第１軸を中心に回動する第１アーム、前記第１アームに設けられ、前記第１アームに対して第２軸を中心に回動する第２アームと、前記第２アームに設けられ、第３軸の方向に直動するシャフトと、前記シャフトの直動を駆動するモーターと、前記モーターの位置を検出する位置検出器と、前記第２アームに設けられた慣性センサーと、を備える水平多関節ロボットを制御する制御装置であって、前記慣性センサーの出力を、前記モーターの制御にフィードバックして前記モーターを駆動する制御部を備える、制御装置。【選択図】図２

Description

本発明は、制御装置、水平多関節ロボットおよびロボットシステムに関するものである。

水平面内の位置と姿勢および、鉛直方向の位置を制御できる水平多関節ロボットに、慣性センサーを取り付け、振動を低減する制御装置が提案されている。

例えば、特許文献１に記載の水平多関節ロボットは、基台と、基台に対して第１軸を回転中心として回動可能に設けた第１アームと、第１アームに対して第２軸を回転中心として回動可能に設けた第２アームと、第２アームに対して直動するシャフトと、第２アームに設けられた慣性センサーと、を備え、慣性センサーの出力を、第１アームを駆動するモーターにフィードバックすることにより、アームの回動方向の振動を低減する技術が記載されている。

特開２０１２−１７１０５２号公報

しかし、特許文献１に記載の技術では、シャフトの先端部におけるシャフトの軸方向の振動を低減することは考慮されていない。この場合、振動が自然減衰するまで時間を要するため、振動収束を待つことによる作業能力の低下や、振動が収束する前に作業することによる作業品質の低下という課題があった。

上記課題を解決するために本発明の一態様は、基台と、前記基台に設けられ前記基台に対して第１軸を中心に回動する第１アームと、前記第１アームに設けられ、前記第１アームに対して第２軸を中心に回動する第２アームと、前記第２アームに設けられ、第３軸の方向に直動するシャフトと、前記シャフトの直動を駆動するモーターと、前記モーターの位置を検出する位置検出器と、前記第２アームに設けられた慣性センサーと、を備える水平多関節ロボットを制御する制御装置であって、前記慣性センサーの出力を、前記モーターの制御にフィードバックして前記モーターを駆動する制御部を備える、制御装置である。

また、本発明の他の態様は、前記制御装置によって制御される水平多関節ロボットである。

また、本発明の他の態様は、前記制御装置と、前記制御装置によって制御される水平多関節ロボットと、を備えるロボットシステムである。

第１実施形態に係るロボットシステムの全体構成を示す図である。 第１実施形態に係るロボットの構成を示す図である。 第１実施形態に係る慣性センサー情報を慣性センサー情報処理部Ａで処理後に電流指令にフィードバックする制御ブロック図である。 第１実施形態に係る慣性センサー情報を慣性センサー情報処理部Ｂで処理後に電流指令にフィードバックする制御ブロック図である。 第２実施形態に係る慣性センサー情報を慣性センサー情報処理部Ａで処理後に速度制御部の比例制御にフィードバックする制御ブロック図である。 第２実施形態に係る慣性センサー情報を慣性センサー情報処理部Ｂで処理後に速度制御部の比例制御にフィードバックする制御ブロック図である。 第３実施形態に係る慣性センサー情報を慣性センサー情報処理部Ｂで処理後に速度指令にフィードバックする制御ブロック図である。 第１実施形態に係る制御装置の振動低減効果を示すグラフである。 第４実施形態に係る第２アームの角度θ２と係数Ｒｇｊの関係を示すグラフである。 第５実施形態に係る、ワークの質量Ｗと係数Ｒｇｗの関係を示すグラフである。 第６実施形態に係る、シャフト位置Ｚと係数Ｒｇｚの関係を示すグラフである。 第７実施形態に係る、第２アームの角度θ２と係数Ｒｆｊの関係を示すグラフである。 第８実施形態に係る、ワークの質量Ｗと係数Ｒｆｗの関係を示すグラフである。 第９実施形態に係る、シャフト位置Ｚと係数Ｒｆｚの関係を示すグラフである。 コントローラがロボット、コンピューターおよびティーチングペンダントと接続された状態を示すブロック図である。

本発明の制御装置、水平多関節ロボットおよびロボットシステムを実施形態に基づいて詳細に説明する。

＜第１実施形態＞
図１は、本発明の第１実施形態に係るロボットシステムの全体構成を示す図である。図２は、図１に示すロボットの構成を示す図である。図３および、図４は、図１に示す制御装置の、慣性センサー情報を電流指令にフィードバックする制御ブロック図である。図８は、図１に示す制御装置の振動を低減する効果を示すグラフである。

図１に示すロボットシステム１は、ロボット２と、ロボット２を制御する制御装置３で構成される。ロボットシステム１の用途は限定されず、例えば電子部品の搬送、組立および、検査等の作業に用いることができる。

ロボット２は、水平多関節ロボットであり、基台２１と、基台２１に対して第１軸Ａ１を回動中心として回動可能に設けられた第１アーム２３と、前記第１アーム２３に対して第２軸Ａ２を回動中心として回動可能に設けられた第２アーム２４と、前記第２アーム２４に対して第３軸Ａ３の方向への直動および、第４軸Ａ４を回動中心として回動可能に設けられたシャフト３１と、前記第２アーム２４に設けられ、前記第２軸Ａ２と前記第３軸Ａ３で構成される平面に直交する角速度検出軸Ａｓを回転軸とする角速度を検出する慣性センサー１１と、を備えている。

前記基台２１は、ボルト等により図示しないロボットの設置面に固定される。前記第１アーム２３は、減速機７１を介して第１モーター５１により回動駆動され、第１軸Ａ１を回動中心として回動する。前記第１モーター５１には第１位置検出器６１が設けられ、第１アーム２３の回動の位置を検出する。

前記第２アーム２４は、減速機７２を介して第２モーター５２により回動駆動され、第２軸Ａ２を回動中心として回動する。前記第２モーター５２には第２位置検出器６２が設けられ、第２アーム２４の回動の位置を検出する。

本実施形態では、シャフト３１は直動と回動が可能なボールねじスプラインを使用し、ボールねじナット９３の外輪および、スプライン外筒９４の外輪が第２アーム２４に固定され、シャフト支持部３２が構成される。前記ボールねじナット９３は、第１タイミングベルト８３を介して第３モーター５３により回動駆動され、シャフト３１が第３軸Ａ３の方向に直動する。前記第３モーター５３には第３位置検出器６３が設けられ、シャフト３１の直動の位置を検出する。

前記スプライン外筒９４は、第２タイミングベルト８４を介して第４モーター５４により回動駆動され、シャフト３１が第４軸Ａ４を回動中心として回動する。前記第４モーター５４には第４位置検出器６４が設けられ、シャフト３１の回動の角度を検出する。

なお、本実施形態では、第３軸Ａ３と、第４軸Ａ４が同一軸上に配置されるボールねじスプラインを使用しているが、第３軸Ａ３と第４軸Ａ４が異なる配置の機構でもよい。また、本実施形態では、第２アーム２４にシャフト３１が設けられているが、第１アーム２３と第２アーム２４を含むアーム２２にシャフト３１が設けられていればよい。例えば、アーム２２が第２アーム２４に設けられた第３アームを備える場合は、第３アームにシャフト３１が設けられてもよい。

前記慣性センサー１１は、第２アーム２４の、シャフト支持部３２の近傍に設置することが好ましい。また、本実施形態では、第２アーム２４に慣性センサー１１が設けられているが、第１アーム２３と第２アーム２４を含むアーム２２に、慣性センサー１１が設けられていればよい。例えば、アーム２２が第２アーム２４に設けられた第３アームを備える場合は、第３アームに慣性センサー１１が設けられてもよい。

本実施形態では、慣性センサー１１は、角速度センサーであり、図２に示すように、第２軸Ａ２と第３軸Ａ３を含む平面に直交する角速度検出軸Ａｓの周りの角速度Ｖｓを検出するように配置する。

第１アーム２３は、基台２１に対して第１関節Ｊ１により片持ち支持され、第２アーム２４は、前記第１アーム２３に対して第２関節Ｊ２により片持ち支持されているため、前記シャフト支持部３２は、図２に示すように、第１関節Ｊ１および第２関節Ｊ２のたわみ変形と、第１アーム２３および第２アーム２４の曲げと捻じり変形により、回転の成分を含むＳの方向に変位する。前記Ｓの方向は、回転運動の成分をもつため、慣性センサー１１は、シャフト支持部３２の変位量Ｄの変化を、角速度検出軸Ａｓの周りの角速度Ｖｓとして検出することができる。

制御装置３は、ロボット２と電気的に接続され、ロボット２を制御する制御部３Ａを有する。ロボット２と制御装置は、ケーブルで電気的に接続されている。また、制御装置３は、ロボット２にその一部または、全部が内蔵されていてもよい。

制御装置３は、例えば、ロボットを制御するための演算を処理するマイクロプロセッサーなどのプロセッサー、メモリ、記憶装置、で構成される演算装置と、モーターを駆動する電流を制御する電流アンプと、周辺機器と情報をやり取りするインターフェイス（Ｉ／Ｆ）とで構成されているコントローラ２００を備える。また、制御装置３には、マン―マシーンインターフェイスとして用いるコンピューター２０１と、ロボットを教示するティーチングペンダント２０２を接続可能である（図１５参照）。

なお、プロセッサーは、複数のプロセッサーによって構成されていてもよい。例えば、プロセッサーがコントローラ２００だけでなく、コントローラ２００とは異なるところ（例えば、コンピューター２０１や、ティーチングペンダント２０２や、ＬＡＮなどのネットワーク環境を介して提供されるクラウドサービスで利用されるサーバーなど）にあり、これらのプロセッサーの一部又は全部を利用して、制御装置３を実現することが可能である。

制御装置３の制御部３Ａは、プロセッサーを用いて、第１位置検出器６１、第２位置検出器６２、第３位置検出器６３および、第４位置検出器６４の位置情報に基づいて、第１モーター５１、第２モーター５２、第３モーター５３および、第４モーター５４を制御する。また、前記制御部３Ａは、慣性センサー１１が検出する角速度を、第３モーター５３の制御にフィードバックして、シャフト支持部３２の鉛直方向の振動を低減する。

図２に示すように、ハンド取り付けフランジ３３がシャフト３１の下端に設けられている。前記ハンド取り付けフランジ３３には、ハンド４１が取り付けられる。前記ハンド４１は、ワーク４２を把持し、ロボット２は、搬送や組立の作業を行う。

このような構成のロボット２において、シャフト３１の直動の加減速や、シャフト３１に鉛直方向の外力が作用すると、シャフト支持部３２が鉛直方向に振動する。前記シャフト支持部３２の鉛直方向の振動は、減衰時間が長い。

次に、図３と図４の制御ブロック図を用いて、慣性センサー１１で検出した角速度情報を電流指令にフィードバックして、シャフト支持部３２の振動を低減する制御方法を説明する。角速度情報のフィルタリング処理方法は、図３の慣性センサー情報処理部Ａと、図４の慣性センサー情報処理部Ｂを使用する２種類がある。

図３の制御部３Ａは、シャフト３１を直動する第３モーター５３を制御するための、位置指令生成部１０１と、位置制御部１０２と、速度制御部１０３と、電流制御部１０４および、慣性センサー１１で検出した角速度情報をフィルタリング処理して電流指令にフィードバックする慣性センサー情報処理部Ａ１０５で構成される。

前記位置指令生成部１０１は、シャフト３１を直動する第３モーター５３の位置指令を、制御周期間隔毎に生成する。前記位置制御部１０２は、前記位置指令生成部１０１が生成した位置指令と、第３位置検出器６３が検出した第３モーター５３の位置を一致させるように速度指令を生成する。前記速度制御部１０３は、その一例として比例積分制御で構成され、第３位置検出器で検出した位置から求めた速度を速度指令に一致させるように電流指令を生成する。前記電流制御部１０４は、第３モーター５３を駆動する電流を電流指令に一致させるように電流を制御する。

前記慣性センサー情報処理部Ａ１０５は、慣性センサー１１と、ＬＰＦ（ローパスフィルター）１１１と、ＤＣ除去部１１２および、フィードバックゲインＫｇｐ乗算処理部で構成される。

前記慣性センサー１１は、本実施形態では角速度センサーを使用し、シャフト支持部３２の鉛直方向の振動を角速度の変化として検出する。

前記ＬＰＦ１１１は、慣性センサー１１で検出した角速度情報に含まれる高周波域のセンサーノイズと、機械共振および、慣性センサー１１が出力する離調ノイズを除去する。ＬＰＦ１１１のカットオフ周波数は、シャフト支持部３２の振動を検出するために必要な低周波域の角速度情報の品質を落とさず、制御を不安定化するノイズを高減衰率で除去するように、２０Ｈｚ以上２００Ｈｚ以下に設定することが好ましい。

前記ＤＣ除去部１１２は、角速度情報に含まれるオフセット成分を除去する。本実施形態では、オフセット成分は、ロボット２が停止時に、角速度情報の移動平均として検出する。前記角速度情報の移動平均によりオフセット成分を除去する方法は、角速度情報に含まれる直流成分のみを除去して、振動を低減するために利用する低周波域の角速度情報の品質が低下しないため、ＨＰＦ（ハイパスフィルター）を用いる方法と比べると、鉛直方向の振動を低減する能力が高い。また、この処理は比較的簡単であるため、制御部３Ａの演算処理の増加を防止することができる。

前記フィードバックゲインＫｇｐ乗算処理部は、慣性センサー１１で検出した角速度情報をフィルタリング処理した後に、角速度フィードバックゲインＫｇｐを乗じて、電流指令への入力を作成する。

このように構成された制御方法を、第３モーターを駆動する制御に適用すれば、シャフト支持部３２の振動を低減することができる。

次に、図３に記載の制御方法を用いて、シャフト支持部３２の振動を低減する働きを説明する。シャフト支持部３２には、シャフト３１が直動する反力が作用する。本発明は、前記反力を利用して、シャフト支持部３２の振動を低減する。図２に記載のように、シャフト３１の位置Ｚと、慣性センサー１１が検出する角速度Ｖｓおよび、シャフト支持部３２の変位量Ｄの方向を定義する。図３に記載のように、慣性センサー情報処理部Ａ１０５の出力を電流指令に加算すると、シャフト支持部３２の角速度を減衰するようにシャフト３１の加速度が調整されるため、シャフト支持部３２の振動が低減する。本制御方法によれば、シャフト３１を駆動するための電流指令を調整してシャフト支持部３２の振動を低減するため、シャフト３１を動作させながら、シャフト支持部３２の振動を低減することができる。

図３に記載の制御方法によれば、角速度情報を電流指令にフィードバックして、シャフト支持部３２の鉛直方向の振動を抑制する反力を直接制御するため、応答性が速く、振動の抑制能力が高い制御を実現することができる。また、シャフト３１を動作させながら、シャフト支持部３２の振動を低減することができる。さらに、ＤＣ除去部１１２は、角速度情報に含まれる直流成分のみを除去して低周波域の角速度情報に歪が発生しないため、位置決めの遅れが発生しない。また、ＤＣ除去部１１２の出力に直流成分が残留しても、速度制御部の積分制御により外乱としてキャンセルされるため、シャフト３１の位置ずれは発生しない。

図８に記載のグラフは、制振制御ありＡと、制振制御なしＢの場合において、ワーク４２を位置決めした時の、鉛直方向の振動の変化を記録したものである。制御装置３によれば、鉛直振動を低減する制御が無い場合Ｂに対して、制御がある場合Ａは、振動が低減している。

次に、図４に記載の、慣性センサー情報処理部Ｂを用いた制御方法について説明する。図４に記載の制御部３Ａは、図３と同様に、位置指令生成部１０１と、位置制御部１０２と、速度制御部１０３と、電流制御部１０４と、図３と異なる慣性センサー情報処理部Ｂ１０６で構成する。前記慣性センサー情報処理部Ｂ１０６は、慣性センサー１１と、ＬＰＦ（ローパスフィルター）１１１と、ＢＥＦ（バンドエリミネーションフィルター）１１３（と、ＨＰＦ（ハイパスフィルター）１１４および、フィードバックゲインＫｇｐ乗算処理部で構成される。

前記慣性センサー１１は、本実施形態では角速度センサーを使用し、シャフト支持部３２の鉛直方向の振動を角速度の変化として検出し、角速度情報を出力する。

前記ＬＰＦ１１１は、慣性センサー１１で検出した角速度情報に含まれる高周波域のセンサーノイズおよび、機械共振を除去する。ＬＰＦ１１１のカットオフ周波数は、シャフト支持部３２の鉛直方向の振動を検出するために必要な低周波域の角速度情報の品質を落とさず、発音や制御を不安定化する高周波域のセンサーノイズと機械共振を高減衰率で除去するように、２０Ｈｚ以上２００Ｈｚ以下に設定することが好ましい。

前記ＢＥＦ１１３は、慣性センサー１１が出力する離調ノイズを除去する。前記離調ノイズは、慣性センサー１１の種類により周波数が異なるため、使用する慣性センサー１１の特性に合わせて離調ノイズを除去する帯域を設定することができる。

前記ＨＰＦ１１４は、角速度情報に含まれるオフセット成分を除去する。前記ＨＰＦは、適切なカットオフ周波数を設定すると、経時変化するオフセットを完全に除去することができる。前記ＨＰＦをカットオフ周波数は、２０Ｈｚ以下に設定することが好ましい。

前記フィードバックゲインＫｇｐ乗算処理部は、慣性センサー１１で検出した角速度情報から低周波域のノイズと、ＤＣ成分を除去した角速度情報に、角速度フィードバックゲインＫｇｐを乗算して、電流指令への入力を作成する。

図４に記載の、慣性センサー情報処理部Ｂ１０６によれば、ＢＥＦ１１３を用いて慣性センサー１１の特性に合わせて離調ノイズを除去することができるため、特性の異なる慣性センサーを使用することができる。また、ＨＰＦ１１４は角速度情報に含まれる直流成分を完全に除去することができるため、オフセットの変化が大きい慣性センサー１１を使用することができる。

なお、本実施形態では、慣性センサー１１は、角速度センサーを使用する一例を示したが、加速度センサーで検出した加速度を積分した速度情報を用いることもできる。

以上、ロボットシステム１において、制御部３Ａが、慣性センサー１１で検出した角速度情報を、電流指令にフィードバックする制御方法であることを特徴とする制御装置３の実施形態について説明した。本実施形態によれば、次の効果を得ることができる。

本発明の実施形態に係る制御装置は、基台と、前記基台に設けられ前記基台に対して第１軸を中心に回動する第１アーム、前記第１アームに設けられ、前記第１アームに対して第２軸を中心に回動する第２アームと、前記第２アームに設けられ、第３軸の方向に直動するシャフトと、前記シャフトの直動を駆動するモーターと、前記モーターの位置を検出する位置検出器と、前記第２アームに設けられた慣性センサーと、を備える水平多関節ロボットを制御する制御装置であって、前記慣性センサーの出力を、前記モーターの制御にフィードバックして前記モーターを駆動する制御部を備える。

従来の技術では、シャフト支持部の鉛直方向の振動が発生しないように、アームを支持する軸受のモーメント剛性を高め、アームの曲げおよび、ねじり剛性を高めることが必要であった。近年、ロボットの作業能力を向上するため、ロボットの可搬重量の増加や、ロボットの作業速度の向上が図られる中で、前述のようなロボットの機械要素の剛性を高める対応は、ロボットの形状の大型化や重量増加や、コストアップの課題が存在した。

本実施形態によれば、水平多関節ロボットの、第１アームおよび第２アームの支持部の倒れと、第１アームおよび第２アームの曲げと捻じりにより発生する、シャフト支持部の振動を低減することができる。アームの支持部およびアームの変形により発生するシャフト下端部の鉛直方向の振動は、自然減衰するまで時間を要するため、振動収束を待つことによる作業能力の低下や、振動が収束する前に作業することで作業対象にダメージを与え、作業位置のばらつきで作業品質が低下すること、ロボットの形状の大型化や重量増加、コストアップなどの、従来の水平多関節ロボットの課題を解決することができる。

本実施形態によれば、ロボットを高速化するためのシャフトを直動させる加速度の増加や、対応作業を拡大するためのワークの質量増加や、作業範囲を拡大するためのアーム長の延長による、シャフト支持部の振動の拡大を抑制し、ロボットの作業能力を向上することができる。

本発明の実施形態に係る制御部は、位置指令を生成する位置指令生成部と、前記位置指令に基づいて速度指令を生成する位置制御部と、前記速度指令に基づいて電流指令を生成する速度制御部と、前記モーターを駆動する電流を制御する電流制御部と、を有し、前記慣性センサーの出力を、前記電流指令にフィードバックする。

本実施形態によれば、慣性センサーの出力を、シャフトを直動するモーターを駆動する制御の電流指令にフィードバックするため、応答が速く、振動を抑制する能力が高く、慣性センサーの出力にオフセットが含まれていても位置ズレの発生がなく、シャフト支持部の鉛直方向の振動を低減することができる。

本発明の実施形態に係る制御部は、前記慣性センサーの出力に含まれるノイズを、ローパスフィルターで除去する。

本実施形態によれば、制御を不安定化する高周波域の機械共振や、慣性センサーの出力に含まれる離調ノイズを除去できるため、個体差や環境変化の影響を受けにくく、シャフト支持部の鉛直方向の振動を低減する制御が実現できる。

本発明の実施形態に係るローパスフィルターは、カットオフ周波数が２０Ｈｚ以上２００Ｈｚ以下である。

本実施形態によれば、シャフト支持部の鉛直方向の振動の抑制に必要な低周波域の情報の品質を低下することなく、制御を不安定化するノイズを除去することにより、シャフト支持部の鉛直方向の振動を抑制する効果が高い制御を実現することができる。

本発明の実施形態に係る制御部は、前記慣性センサーの出力に含まれる離調ノイズを、バンドエリミネーションフィルターで除去する。

本実施形態によれば、慣性センサーの出力に含まれるローパスフィルターでは除去できない離調ノイズを、慣性センサーの特性に合わせて除去することにより、異なる離調ノイズの特性をもつ慣性センサーを使用して、シャフト支持部の鉛直方向の振動を抑制することができる。

本発明の実施形態に係る制御部は、前記慣性センサーの出力に含まれるオフセット成分を移動平均により求め、前記オフセット成分を前記慣性センサーの出力から除去する。

本実施形態によれば、慣性センサーに含まれるオフセットを、簡単な演算で実現することができる移動平均で求めて、オフセットをキャンセルするため、振動抑制に必要な慣性センサーの出力の低周波域の情報が歪むことがないことにより位置決めが遅れず、制御処理の演算量の増加を抑えて、シャフト支持部の鉛直方向の振動を抑制することができる。

本発明の適用例に係る制御部は、前記慣性センサーの出力に含まれるオフセット成分をハイパスフィルターで除去する。

本実施形態によれば、慣性センサーに含まれるオフセットを、ハイパスフィルターで完全に除去できるため、オフセットの変化が大きい慣性センサー１１を使用して、シャフト支持部の鉛直方向の振動を抑制することができる。

本発明の実施形態に係る慣性センサーは、前記第２軸と前記第３軸を含む平面に対して直交する角速度検出軸周りの角速度を検出する角速度センサーである。

本実施形態によれば、角速度センサーを第２軸と第３軸を含む平面に対して直交する角速度検出軸の周りの角速度を検出するように配置することにより、シャフト支持部の鉛直方向の振動と等価な角速度を、高感度に検出することができる。

本発明の実施形態に係る水平多関節ロボットは、上記制御装置によって制御される水平多関節ロボットである。

本実施形態によれば、シャフト支持部の鉛直方向の振動を抑制する水平多関節ロボットを実現することができる。

本発明の実施形態に係るロボットシステムは、上記制御装置と、上記制御装置によって制御される水平多関節ロボットと、を備える。

本実施形態によれば、シャフト支持部の鉛直方向の振動を抑制する水平多関節ロボットと制御装置で構成されるロボットシステムを実現することができる。

＜第２実施形態＞
図５および、図６は、本発明の第２実施形態に係る図１に示す制御装置の、慣性センサー情報を速度制御部の比例制御にフィードバックする制御ブロック図である。

本実施形態に係るロボットシステム１は、制御装置３の制御部３Ａの制御方法が異なること以外は、前述した第１実施形態のロボットシステム１と同様である。なお、以下の説明では、第２実施形態のロボットシステム１に関し、前述した第１実施形態との相違点を中心に説明し、同様の事項に関しては、その説明を省略する。

次に、図５と図６の制御ブロック図を用いて、慣性センサー１１で検出した角速度情報を速度制御部の比例制御にフィードバックして、シャフト支持部３２の振動を低減する制御方法を説明する。角速度情報のフィルタリング処理方法は、図５の慣性センサー情報処理部Ａと、図６の慣性センサー情報処理部Ｂを使用する２種類がある。

図５の制御部３Ａは、シャフト３１を直動する第３モーター５３を制御するための、位置指令生成部１０１と、位置制御部１０２と、速度制御部１０３と、電流制御部１０４および、慣性センサー１１で検出した角速度情報をフィルタリング処理して、速度制御部の比例制御にフィードバックする慣性センサー情報処理部Ａ１０５で構成する。

前記慣性センサー情報処理部Ａ１０５は、慣性センサー１１と、ＬＰＦ（ローパスフィルター）１１１と、ＤＣ除去部１１２および、フィードバックゲインＫｇｐ乗算処理部で構成する。

前記慣性センサー１１は、本実施形態では角速度センサーを使用し、シャフト支持部３２の鉛直方向の振動を角速度の変化として検出する。

このように構成された制御方法を、第３モーターを駆動する制御に適用すれば、シャフト支持部３２の鉛直方向の振動を低減することができる。

次に、図５に記載の制御方法により、シャフト支持部３２の鉛直方向の振動を低減する働きを、図２を用いて説明する。

シャフト支持部３２が、変位量Ｄの座標に対して＋方向の速度で、図２の上方向に変位する場合、慣性センサー１１は、＋方向の角速度Ｖｓを検出する。図５に記載の制御方法において、前記角速度Ｖｓは、ＬＰＦとＤＣ除去部でのフィルタリング処理後、角速度フィードバックゲインＫｇｐを乗算し、速度制御部の比例制御に加算され、第３モーター５３の速度を＋方向に増やす速度指令が生成される。このため、シャフト３１が、図２に記載のＺ座標＋方向に加速すると、シャフト支持部３２にはＤ−方向の反力が作用し、シャフト支持部３２のＤ＋方向の変位を抑制する。

前記シャフト３１を駆動する反力がシャフト支持部３２に作用することにより、シャフト支持部３２の角速度が減衰されるため、シャフト支持部３２の振動を低減することができる。

また、前記反力は、シャフト３１を位置指令と一致させて駆動する制御と合わせて処理されるため、シャフト３１の位置制御と同時に、シャフト支持部３２の振動を低減することができる。

図５に記載の制御方法によれば、角速度情報を速度制御部の比例制御部にフィードバックして、シャフト支持部３２の鉛直方向の振動を抑制する反力を、速度指令を変化させて得るため、制御の安定性が高く、環境変化や外乱に対してロバストな制御を実現することができる。また、シャフト３１の位置を制御しながら、シャフト支持部３２の振動を低減することができる。さらに、ＤＣ除去部１１２は、角速度情報に含まれるオフセット成分のみを除去して、シャフト支持部３２の振動を低減するために必要な低周波域の角速度情報の品質が低下しないため、振動抑制効果が高い制御を実現することができる。また、ＤＣ除去部１１２から出力される角速度情報にオフセット成分が残留しても、速度制御部の積分制御により外乱としてキャンセルされるため、シャフト３１の位置ずれは発生しない。

次に、本発明の第２実施形態において、慣性センサー情報処理部Ｂを用いた実施形態について説明する。

図６は、慣性センサー情報を、ＬＰＦ１１１、ＢＥＦ１１３、ＨＰＦ１１４および、フィードバックゲインＫｇｐ乗算処理部で構成される慣性センサー情報処理部Ｂで、フィルタリング処理することを特徴とする。

前記慣性センサー１１は、本実施形態では角速度センサーを使用し、シャフト支持部３２の鉛直方向の振動を角速度の変化として検出し、角速度情報を出力する。

このように構成された制御方法を、第３モーターを駆動する制御に適用すれば、シャフト支持部３２の鉛直方向の振動を低減することができる。

図６に記載の制御方法による、シャフト支持部３２の鉛直方向の振動を低減する働きは、慣性センサー情報処理部Ａを用いる制御方法と同様であるため、省略する。

図６に記載の、慣性センサー情報処理部Ｂ１０６によれば、ＢＥＦ１１３を用いて慣性センサー１１の特性に合わせて離調ノイズを除去することができるため、特性の異なる慣性センサーを使用することができる。また、ＨＰＦ１１４を用いて角速度情報に含まれる時変動するオフセット成分を除去することができるため、オフセットの変化が大きい慣性センサー１１を使用することができる。

なお、本実施形態では、慣性センサー１１は、角速度センサーを使用する一例を示したが、加速度センサーで検出した加速度を積分した速度情報を用いることもできる。

以上、ロボットシステム１において、制御部３Ａが、慣性センサー１１で検出した角速度情報を、速度制御部の比例制御にフィードバックする制御方法であることを特徴とする制御装置３の実施形態について説明した。本実施形態によれば、次の効果を得ることができる。

本発明の実施形態に係る制御部は、位置指令を生成する位置指令生成部と、前記位置指令に基づいて速度指令を生成する位置制御部と、前記速度指令に基づいて電流指令を生成する速度制御部と、前記モーターを駆動する電流を制御する電流制御部と、を有し、前記慣性センサーの出力を、前記速度制御部の比例制御にフィードバックする。

本実施形態によれば、慣性センサーの出力を、シャフトを直動するモーターを駆動する制御の速度制御部の比例制御にフィードバックするため、環境変化の影響を受けにくく、慣性センサーの出力にオフセットが含まれていても位置ズレの発生がなく、シャフト支持部の鉛直方向の振動を低減することができる。

＜第３実施形態＞
図７は、本発明の第３実施形態に係る図１に示す制御装置３において、慣性センサー情報を速度指令にフィードバックする制御ブロック図である。

本実施形態に係るロボットシステム１は、制御装置３の制御部３Ａの制御方法が異なること以外は、前述した第１実施形態のロボットシステム１と同様である。なお、以下の説明にでは、第３実施形態のロボットシステム１に関し、前述した第１実施形態との相違点を中心に説明し、同様の事項に関しては、その説明を省略する。

慣性センサー１１で検出した慣性センサー情報を速度指令にフィードバックして、シャフト支持部３２の鉛直方向の振動を低減する制御部３Ａの制御方法を、図７の制御ブロック図に基づいて説明する。

図７の制御部３Ａは、シャフト３１を直動する第３モーター５３を制御するための、位置指令生成部１０１と、位置制御部１０２と、速度制御部１０３と、電流制御部１０４と、慣性センサー１１で検出した角速度情報をフィルタリング処理して、速度指令にフィードバックする慣性センサー情報処理部Ｂ１０６で構成されている。

本発明は、慣性センサー情報を、ＬＰＦ１１１、ＢＥＦ１１３、ＨＰＦ１１４および、フィードバックゲインＫｇｐ乗算処理部で構成される慣性センサー情報処理部Ｂで、フィルタリング処理した角速度情報を速度指令にフィードバックすることを特徴とする。

前記慣性センサー１１は、本実施形態では角速度センサーを使用し、シャフト支持部３２の鉛直方向の振動を角速度の変化として検出し、角速度情報を出力する。

このように構成された制御方法を、第３モーターを駆動する制御に適用すれば、シャフト支持部３２の鉛直方向の振動を低減することができる。

図７に記載の制御方法による、シャフト支持部３２の鉛直方向の振動を低減する働きは、第２実施形態の制御方法と同様であるため、省略する。

図７に記載の、慣性センサー情報処理部Ｂ１０６によれば、ＢＥＦ１１３を用いて慣性センサー１１の特性に合わせて離調ノイズを除去することができるため、制御の安定性を高めることができる。また、ＨＰＦ１１４を用いて角速度情報に含まれる時変動するオフセット成分を完全に除去することができるため、オフセットの変化が大きい慣性センサー１１を使用することができる。

なお、本実施形態では、慣性センサー１１は、角速度センサーを使用する一例を示したが、加速度センサーで検出した加速度を積分した速度情報を用いることもできる。

以上、ロボットシステム１において、制御部３Ａが、慣性センサー１１で検出した角速度情報を、速度指令にフィードバックする制御方法であることを特徴とする制御装置３の実施形態について説明した。本実施形態によれば、次の効果を得ることができる。

本発明の実施形態に係る制御部は、位置指令を生成する位置指令生成部と、前記位置指令に基づいて速度指令を生成する位置制御部と、前記速度指令に基づいて電流指令を生成する速度制御部と、前記モーターを駆動する電流を制御する電流制御部と、を有し、前記慣性センサーの出力を、前記速度指令にフィードバックする。

本実施形態によれば、慣性センサーの出力を、シャフトを直動するモーターを駆動する制御の速度指令にフィードバックするため、条件変化の影響を受けにくく、シャフト支持部の鉛直方向の振動を低減することができる。

＜第４実施形態＞
本発明の第４実施形態に係る制御装置３は、角速度フィードバックゲインＫｇｐを、第２アーム２４の角度θ２に基づいて調整する。

本実施形態に係る図１に記載のロボットシステム１の制御装置３は、シャフト支持部３２の鉛直方向の振動を低減する効果を向上し、制御を安定化する機能を有する。なお、以下の説明では、第１実施形態から第３実施形態との相違点を中心に説明し、同様の事項に関しては、その説明を省略する。

本実施形態に係るロボットシステム１は、図３から図７に示すように、制御部３Ａは、角速度フィードバックゲインＫｇｐを、第１アーム２３に対する第２アーム２４の角度θ２に基づいて変更するＫｇｐ調整部１２２を有する。角度θ２が変化すると、シャフト支持部３２の基台２１に対する鉛直方向の剛性および、第１アーム２３と第２アーム２４を合わせたシャフト支持部３２周りの慣性が変化する。具体的には、第２アームを曲げて角度θ２が増加すると、前記剛性が低下し、前記慣性が減少する。このため、角度θ２に関係なく、角速度フィードバックゲインＫｇｐを固定してしまうと、制御の不安定化や、十分な制振効果が得られない場合がある。そこで、本実施形態の制御装置３は、角度θ２に応じて角速度フィードバックゲインＫｇｐを調整するように構成され、上述した問題を解決している。

このような制御方法の一例として、図９に、第２アームの角度θ２と係数Ｒｇｊとの関係のグラフを示す。図９のグラフの縦軸は、第２アームの角度θ２によりＫｇｐを調整する係数Ｒｇｊである。また、Ｒｇｊの下限値をＲｇｊｍｉｎとしている。また、第２アームの角度θ２に対するＲｇｊの傾きを、θ２＝０°を対称にして、±Ｋｇｊで定義している。図９に基づくＲｇｊの調整方法によれば、傾きＫｇｊおよび下限値Ｒｇｊｍｉｎの２種類のパラメーターで、角度θ２に対する角速度フィードバックゲインＫｇｐを調整することができる。

角速度フィードバックゲインＫｇｐは、Ｋｇｐの基準値Ｋｇｐｂに、図９から求めた係数Ｒｇｊを乗じる（１）式で求める。

前述したように、第２アーム２４を第１アーム２３に対して伸ばした姿勢（θ２＝０）の付近では、第１アーム２３と第２アーム２４で構成されるシャフト支持部３２周りの慣性は大きく、基台２１に対するシャフト支持部３２の鉛直方向の剛性は高いため、制御系が安定化する傾向がある。この場合には、Ｒｇｊを大きくして、鉛直方向の振動を低減する効果を高めることができる。

一方、第２アーム２４を第１アーム２３に対して曲げた姿勢では、第１アーム２３と第２アーム２４で構成される慣性が減少し、基台２１に対するシャフト支持部３２の剛性が下がり、制御が不安定化する傾向がある。この場合には、Ｒｇｊを小さくして、制御の安定性を高めることができる。

本発明の実施形態に係る制御部は、前記慣性センサーの出力を、前記モーターの制御にフィードバックするゲインを、前記第２アームの角度に基づいて調整する。

本実施形態によれば、第２アームの角度の変化による、シャフト支持部における鉛直方向の剛性および、シャフト支持部周りの慣性の変化に合わせて、慣性センサーの出力のフィードバックゲインを調整することにより、シャフト支持部の鉛直方向の振動を抑制する制御の不安定化を防止することができる。

＜第５実施形態＞
本発明の第５実施形態に係る制御装置３は、角速度フィードバックゲインＫｇｐを、ワークの質量Ｗに基づいて調整する。

本実施形態に係る図１に記載のロボットシステム１の制御装置３は、シャフト支持部３２の鉛直方向の振動を低減する効果を向上し、制御を安定化する機能を有する。なお、以下の説明では、第１実施形態から第３実施形態との相違点を中心に説明し、同様の事項に関しては、その説明を省略する。

本実施形態のロボットシステム１は、図３から図７に示すように、制御部３Ａは、角速度フィードバックゲインＫｇｐを、ワークの質量Ｗに基づいて変更するＫｇｐ調整部１２２を有する。ワークの質量Ｗが変化すると、シャフト３１の駆動によってシャフト支持部３２に作用する反力の大きさが変化する。具体的には、ワークの質量Ｗが大きい場合、シャフト３１の駆動による反力が大きくなるため、角速度フィードバックゲインＫｇｐを上げるのと同様の影響がある。このため、ワークの質量Ｗに関係なく、角速度フィードバックゲインＫｇｐを固定してしまうと、制御の不安定化や、十分な制振効果が得られない場合がある。そこで、本実施形態の制御装置３は、ワークの質量Ｗに応じて角速度フィードバックゲインＫｇｐを調整するように構成され、上述した問題を解決している。

このような制御方法の一例として、図１０に、正規化したワークの質量Ｗと係数Ｒｇｗとの関係を示すグラフを示す。図１０のグラフの縦軸は、ワークの質量ＷによりＫｇｐを調整する係数Ｒｇｗである。また、Ｒｇｗの下限値をＲｇｗｍｉｎ、Ｒｇｗの上限値をＲｇｗｍａｘとしている。また、グラフの横軸は、正規化したワークの質量Ｗとしている。また、ワークの質量Ｗが１より小さいときのグラフの傾きをＫｇｗ１に、ワークの質量Ｗが１より大きいときのグラフの傾きをＫｇｗ２で定義している。図１０に基づくＲｇｗの調整方法によれば、傾きＫｇｗ１と、Ｋｇｗ２と、ＲｇｗｍａｘとＲｇｗｍｉｎの４種類のパラメーターで、ワークの質量Ｗに対する角速度フィードバックゲインＫｇｐを調整することができる。

前述したように、ロボットシステム１では、ワークの質量Ｗが大きいほど、シャフト３１を駆動する反力が増加するため、角速度フィードバックゲインＫｇｐを上げたのと同等の影響があるため、図１０に示すように、ワークの質量Ｗが１より大きい領域では、傾きＫｇｗ２でＲｇｗを減少させて、制御の安定性を一定に保つことができる。また、ワークの質量Ｗが１より小さい領域では、傾きＫｇｗ１でＲｇｗを増加させて、制振効果を高めることができる。

角速度フィードバックゲインＫｇｐは、Ｋｇｐの基準値Ｋｇｐｂに、図１０から求めた係数Ｒｇｗを乗じる（２）式で求める。

前述したように、ロボットシステム１では、ワークの質量Ｗが大きくなるほど、反力が増加するため、制御が不安定化する傾向がある。この場合には、図１０に示すように、Ｒｇｗを下げて、制御の安定性を高めることができる。

以上のように、本発明の実施形態に係る制御部は、前記慣性センサーの出力を、前記モーターの制御にフィードバックするゲインを、前記シャフトに付加されるワークの質量に基づいて調整する。

本実施形態によれば、ワークの質量の変化に合わせて、慣性センサーの出力のフィードバックゲインを調整することにより、シャフト支持部の鉛直方向の振動を抑制する制御の不安定化を防止することができる。

＜第６実施形態＞
本発明の第６実施形態に係る制御装置３は、角速度フィードバックゲインＫｇｐを、シャフトの位置Ｚに基づいて調整する。

本実施形態に係る図１に記載のロボットシステム１の制御装置３は、シャフト支持部３２の鉛直方向の振動を低減する効果を向上し、制御を安定化する機能を有する。なお、以下の説明では、第１実施形態から第３実施形態との相違点を中心に説明し、同様の事項に関しては、その説明を省略する。

本実施形態のロボットシステム１は、図３から図７に示すように、制御部３Ａは、角速度フィードバックゲインＫｇｐを、シャフト位置Ｚに基づいて変更するＫｇｐ調整部１２２を有する。シャフト位置Ｚが変化すると、シャフト３１の曲げモードの共振周波数が変化する。具体的には、シャフト３１を下げると、シャフト３１の曲げモードの共振周波数が下がり、鉛直方向の振動を低減する制御と干渉して制御が不安定化する場合がある。そこで、本実施形態の制御装置３は、シャフト位置Ｚに応じて角速度フィードバックゲインＫｇｐを変更するように構成され、上述した問題を解決している。

このような制御方法の一例として、図１１に、シャフト位置Ｚと係数Ｒｇｚとの関係のグラフを示す。図１１のグラフの縦軸は、係数Ｒｇｚであり、最大値が１で、最小値をＲｇｚｍｉｎとしている。また、グラフの横軸は、シャフト位置Ｚを示している。また、Ｚ位置に対する係数Ｒｇｚの傾きは、Ｋｇｚで示している。図１１に基づくＲｇｚの調整方法によれば、最小値Ｒｇｚｍｉｎと、係数Ｒｇｚの２種類のパラメーターで、シャフト位置Ｚに対する角速度フィードバックゲインＫｇｐを調整することができる。

角速度フィードバックゲインＫｇｐは、Ｋｇｐの基準値Ｋｇｐｂに、図１１から求めた係数Ｒｇｚを乗じる（３）式で求める。

前述したように、ロボットシステム１では、シャフト位置Ｚが下がるほど、シャフトの曲げモードの共振周波数が下がり、制御が不安定化する傾向がある。この場合には、図１１に示すように、Ｒｇｚを下げて、制御の安定性を高めることができる。

以上のように、本発明の実施形態に係る制御部は、前記慣性センサーの出力を、前記モーターの制御にフィードバックするゲインを、前記直動の位置に基づいて調整する。
本実施形態によれば、シャフトの直動の位置に合わせて、慣性センサーの出力のフィードバックゲインを調整し、シャフト支持部の鉛直方向の振動を抑制する制御の不安定化を防止することができる。

＜第７実施形態＞
本発明の第７実施形態に係る制御装置３は、ＬＰＦカットオフ周波数Ｆｌｐｆを、第２アーム２４の角度θ２に基づいて調整する。

本実施形態に係る図１に記載のロボットシステム１の制御装置３において、シャフト支持部３２の鉛直方向の振動を低減する効果を向上し、制御を安定化する機能を有する。なお、以下の説明では、第１実施形態から第３実施形態との相違点を中心に説明し、同様の事項に関しては、その説明を省略する。

本実施形態のロボットシステム１は、図３から図７に示すように、制御部３Ａは、ＬＰＦカットオフ周波数Ｆｌｐｆを、第１アーム２３に対する第２アーム２４の角度θ２に基づいて変更するＦｌｐｆ調整部１２１を有する。角度θ２が変化すると、シャフト支持部３２の基台２１に対する鉛直方向の剛性および、第１アーム２３と第２アーム２４を合わせたシャフト支持部３２周りの慣性が変化する。具体的には、第２アームを曲げて角度θ２が増加すると、前記剛性が低下し、前記慣性が減少する。このため、角度θ２に関係なく、ＬＰＦカットオフ周波数Ｆｌｐｆを固定してしまうと、制御の不安定化や、十分な制振効果が得られない場合がある。そこで、本実施形態の制御装置３は、角度θ２に応じてＬＰＦカットオフ周波数Ｆｌｐｆを調整するように構成され、上述した問題を解決している。

このような制御方法の一例として、図１２に、第２アームの角度θ２と係数Ｒｆｊとの関係のグラフを示す。図９のグラフの縦軸は、θ２によりＦｌｐｆを調整する係数Ｒｆｊである。また、Ｒｆｊの下限値をＲｆｊｍｉｎとしている。また、第２アームの角度θ２に対するＲｆｊの傾きを、θ２＝０°を対称にして、±Ｋｆｊで定義している。図１２に基づくＲｆｊの調整方法によれば、傾きＫｆｊおよび下限値Ｒｆｊｍｉｎの２種類のパラメーターで、角度θ２に対してＬＰＦカットオフ周波数Ｆｌｐｆを調整することができる。

ＬＰＦカットオフ周波数Ｆｌｐｆは、Ｆｌｐｆの基準値Ｆｌｐｆｂに、図１２から求めた係数Ｒｆｊを乗じる（４）式で求める。

前述したように、第２アーム２４を第１アーム２３に対して伸ばした姿勢（θ２＝０）の付近では、第１アーム２３と第２アーム２４で構成されるシャフト支持部３２周りの慣性は大きく、基台２１に対するシャフト支持部３２の鉛直方向の剛性は高いため、制御系が安定化する傾向がある。この場合には、Ｆｌｐｆを大きくして、鉛直方向の振動を低減する効果を高めることができる。

一方、第２アーム２４を第１アーム２３に対して曲げた姿勢では、第１アーム２３と第２アーム２４で構成される慣性が減少し、基台２１に対するシャフト支持３２の剛性が下がり、制御が不安定化する傾向がある。この場合には、Ｆｌｐｆを小さくして、制御の安定性を高めることができる。

以上のように、本発明の実施形態に係る制御部は、前記ローパスフィルターのカットオフ周波数を、前記第２アームの角度に基づいて調整する。

本実施形態によれば、第２アームの角度に合わせて、ローパスフィルターのカットオフ周波数を調整することにより、シャフト支持部の鉛直方向の振動を抑制する制御の不安定化を防止することができる。

＜第８実施形態＞
図１３は、本発明の第８実施形態に係る制御装置３は、ＬＰＦカットオフ周波数Ｆｌｐｆを、ワークの質量Ｗに基づいて調整する。

本実施形態に係る図１に記載のロボットシステム１の制御装置３は、シャフト支持部３２の鉛直方向の振動を低減する効果を向上し、制御を安定化する機能を有する。なお、以下の説明では、第１実施形態から第３実施形態との相違点を中心に説明し、同様の事項に関しては、その説明を省略する。

本実施形態のロボットシステム１は、図３から図７に示すように、制御部３Ａは、ＬＰＦカットオフ周波数Ｆｌｐｆを、ワークの質量Ｗに基づいて変更するＦｌｐｆ調整部１２１を有する。ワークの質量Ｗが変化すると、シャフト３１の曲げモードの共振周波数が変化し、シャフト支持部３２の鉛直方向の振動を低減する制御と干渉する場合がある。このため、ワークの質量Ｗに関係なく、ＬＰＦカットオフ周波数Ｆｌｐｆを固定してしまうと、制御の不安定化や、十分な制振効果が得られない場合がある。そこで、本実施形態の制御装置３は、ワークの質量Ｗに応じてＬＰＦカットオフ周波数Ｆｌｐｆを変更するように構成され、上述した問題を解決している。

このような制御方法の一例として、図１３に、正規化したワークの質量Ｗと係数Ｒｆｗとの関係を示すグラフを示す。図１３のグラフの縦軸は、ワークの質量ＷによりＦｌｐｆを調整する係数Ｒｆｗである。また、Ｒｆｗの下限値をＲｆｗｍｉｎ、Ｒｆｗの上限値をＲｆｗｍａｘとしている。また、グラフの横軸は、ワークの質量Ｗとしている。また、ワークの質量Ｗが１より小さいときのグラフの傾きをＫｆｗ１に、ワークの質量Ｗが１より大きいときのグラフの傾きをＫｆｗ２で定義している。図１３に基づくＲｆｗの調整方法によれば、傾きＫｆｗ１と、Ｋｆｗ２と、ＲｆｗｍａｘとＲｆｗｍｉｎの４種類のパラメーターで、ワークの質量Ｗに対すてＬＰＦカットオフ周波数Ｆｌｐｆを調整することができる。

前述したように、ロボットシステム１では、ワークの質量Ｗが大きいほど、シャフト３１の曲げモードの共振周波数が下がり、鉛直方向の振動を低減する制御と干渉して制御が不安定化する場合があるため、図１３に示すように、ワークの質量Ｗが１より大きい領域では、Ｋｆｗ２でＲｆｗを減少させて、制御の安定性を一定に保つことができる。また、ワークの質量Ｗが１より小さい領域では、傾きＫｆｗ１でＦｊｗを増加させて、制振効果を高めることができる。

ＬＰＦカットオフ周波数Ｆｌｐｆは、Ｆｌｐｆの基準値Ｆｌｐｆｂに、図１３から求めた係数Ｒｆｗを乗じる（５）式で求める。

前述したように、ワークの質量Ｗが大きいほど、シャフト３１の曲げモードの共振周波数が下がり、鉛直方向の振動を低減する制御が不安定化する場合がある。この場合には、Ｆｌｐｆを下げて、制御を安定化することができる。

以上のように、本発明の実施形態に係る制御部は、前記ローパスフィルターのカットオフ周波数を、前記シャフトに付加されるワークの質量に基づいて調整する。

本実施形態によれば、ワークの質量に合わせて、ローパスフィルターのカットオフ周波数を調整することにより、シャフト支持部の鉛直方向の振動を抑制する制御の不安定化を防止することができる。

＜第９実施形態＞
本発明の第９実施形態に係る制御装置３は、ＬＰＦカットオフ周波数Ｆｌｐｆを、シャフトの位置Ｚに基づいて調整する。

本実施形態に係る図１に記載のロボットシステム１の制御装置３において、シャフト支持部３２の鉛直方向の振動を低減する効果を向上し、制御を安定化する機能を有する。なお、以下の説明では、第１実施形態から第３実施形態との相違点を中心に説明し、同様の事項に関しては、その説明を省略する。

本実施形態のロボットシステム１は、図３から図７に示すように、制御部３０は、ＬＰＦカットオフ周波数Ｆｌｐｆを、シャフト位置Ｚに基づいて変更するＦｌｐｆ調整部１２１を有する。シャフト位置Ｚが変化すると、シャフト３１の曲げモードの共振周波数が変化する。具体的には、シャフト３１を下げると、前記曲げモードの共振周波数が下がり、鉛直方向の振動を低減する制御と干渉して制御が不安定化する場合がある。そこで、本実施形態の制御装置３は、シャフト位置Ｚに応じてＬＰＦカットオフ周波数Ｆｌｐｆを変更するように構成され、上述した問題を解決している。

このような制御方法の一例として、図１４に、シャフト位置Ｚと、シャフト位置ＺによりＦｌｐｆを調整する係数Ｒｆｚとの関係のグラフを示す。図１４のグラフの縦軸は、係数Ｒｆｚであり、最大値が１で、最小値をＲｆｚｍｉｎとしている。また、グラフの横軸は、シャフト位置Ｚを示している。また、Ｚ位置に対する係数Ｒｆｚの傾きは、Ｋｆｚで示されている。図１４に基づくＲｆｚの調整方法によれば、最小値Ｒｆｚｍｉｎと、傾きＫｆｚの２種類のパラメーターで、シャフト位置Ｚに対するＬＰＦカットオフ周波数Ｆｌｐｆを調整することができる。

ＬＰＦカットオフ周波数Ｆｌｐｆは、Ｆｌｐｆの基準値Ｆｌｐｆｂに、図１４から求めた係数Ｒｆｚを乗じる（６）式で求める。

前述したように、ロボットシステム１では、シャフト位置Ｚが下がるほど、シャフトの曲げモードの共振周波数が下がり、制御が不安定化する傾向がある。この場合には、図１４に示すように、Ｒｆｚを下げて、制御の安定性を高めることができる。

以上のように、本発明の実施形態に係る制御部は、前記ローパスフィルターのカットオフ周波数を、前記直動の位置に基づいて調整する。

本実施形態によれば、直動の位置に合わせて、ローパスフィルターのカットオフ周波数を調整することにより、シャフト支持部の鉛直方向の振動を抑制する制御の不安定化を防止することができる。

１、１Ａ…ロボットシステム、２…ロボット、３…制御装置、３Ａ…制御部、１１…慣性センサー、２１…基台、２２…アーム、２３…第１アーム、２４…第２アーム、３１…シャフト、３２…シャフト支持部、３３…ハンド取り付けフランジ、４１…ハンド、４２…ワーク、５１…第１モーター、５２…第２モーター、５３…第３モーター、５４…第４モーター、６１…第１位置検出器、６２…第２位置検出器、６３…第３位置検出器、６４…第４位置検出器、７１…第１減速機、７２…第２減速機、８３…第１タイミングベルト、８４…第２タイミングベルト、９３…ボールねじナット、９４…スプライン外筒、１００…制御ブロック図、１０１…位置指令生成部、１０２…位置制御部、１０３…速度制御部、１０４…電流制御部、１０５…慣性センサー情報処理部Ａ、１０６…慣性センサー情報処理部Ｂ、１１１…ＬＰＦ（ローパスフィルター）、１１２…ＤＣ除去部、１１３…ＢＥＦ（バンドエリミネーションフィルター）、１１４…ＨＰＦ（ハイパスフィルター）、１２１…ＬＰＦカットオフ周波数調整部、１２２…Ｋｇｐ調整部、Ａ１…第１回動軸、Ａ２…第２回動軸、Ａ３…第３直動軸、Ａ４…第４回動軸、Ａｓ…角速度検出軸、Ｖｓ…角速度、Ｊ１…第１関節、Ｊ２…第２関節、θ１…第１アーム角度、θ２…第２アームの角度、Ｚ…シャフト直動位置、Ｕ…シャフト回動角度、Ｄ…変位量、Ｓ…シャフト支持部変位方向、Ｋｖｐ…速度ループ比例ゲイン、Ｋｖｉ…速度ループ積分ゲイン、Ｋｇｐ…角速度フィードバックゲイン、Ｆｌｐｆ…ＬＰＦカットオフ周波数、ｓ…微分演算子、１／ｓ…積分演算子、Ｒｇｊ…係数、Ｋｇｊ…Ｋｇｐ第２アーム角度補正係数傾き、Ｒｇｊｍｉｎ…Ｋｇｐ第２アーム角度補正係数下限値、Ｒｇｗ…係数、Ｋｇｗ１…第１Ｋｇｐワーク質量補正係数傾き、Ｋｇｗ２…第２Ｋｇｐワーク質量補正係数傾き、Ｒｇｗｍｉｎ…Ｋｇｐワーク質量補正係数下限値、Ｒｇｗｍａｘ…Ｋｇｐワーク質量補正係数上限値、Ｒｇｚ…係数、Ｋｇｚ…Ｋｇｐ直動位置補正係数傾き、Ｒｇｚｍｉｎ…Ｋｇｐ直動位置補正係数下限値、Ｒｆｊ…係数、Ｋｆｊ…Ｆｌｐｆ第２アーム角度補正係数傾き、Ｒｆｊｍｉｎ…Ｆｌｐｆ第２アーム角度補正係数下限値、Ｒｆｗ…係数、Ｋｆｗ１…第１Ｆｌｐｆワーク質量補正係数傾き、Ｋｆｗ２…第２Ｆｌｐｆワーク質量補正係数傾き、Ｒｆｗｍｉｎ…Ｆｌｐｆワーク質量補正係数下限値、Ｒｆｗｍａｘ…Ｆｌｐｆワーク質量補正係数上限値、Ｒｆｚ…係数、Ｋｆｚ…Ｆｌｐｆ直動位置補正係数傾き、Ｒｆｚｍｉｎ…Ｆｌｐｆ直動位置補正係数下限値

Claims (18)

1. 基台と、前記基台に設けられ前記基台に対して第１軸を中心に回動する第１アームと、前記第１アームに設けられ、前記第１アームに対して第２軸を中心に回動する第２アームと、前記第２アームに設けられ、第３軸の方向に直動するシャフトと、前記シャフトの直動を駆動するモーターと、前記モーターの位置を検出する位置検出器と、前記第２アームに設けられた慣性センサーと、を備える水平多関節ロボットを制御する制御装置であって、
   前記慣性センサーの出力を、前記モーターの制御にフィードバックして前記モーターを駆動する制御部を備える、
   制御装置。
2. 前記制御部は、位置指令を生成する位置指令生成部と、前記位置指令に基づいて速度指令を生成する位置制御部と、前記速度指令に基づいて電流指令を生成する速度制御部と、前記モーターを駆動する電流を制御する電流制御部と、を有し、前記慣性センサーの出力を、前記電流指令にフィードバックすることを特徴とする請求項１に記載の制御装置。
3. 前記制御部は、位置指令を生成する位置指令生成部と、前記位置指令に基づいて速度指令を生成する位置制御部と、前記速度指令に基づいて電流指令を生成する速度制御部と、前記モーターを駆動する電流を制御する電流制御部と、を有し、前記慣性センサーの出力を、前記速度制御部の比例制御にフィードバックすることを特徴とする請求項１に記載の制御装置。
4. 前記制御部は、位置指令を生成する位置指令生成部と、前記位置指令に基づいて速度指令を生成する位置制御部と、前記速度指令に基づいて電流指令を生成する速度制御部と、前記モーターを駆動する電流を制御する電流制御部と、を有し、前記慣性センサーの出力を、前記速度指令にフィードバックすることを特徴とする請求項１に記載の制御装置。
5. 前記制御部は、前記慣性センサーの出力に含まれるノイズを、ローパスフィルターで除去することを特徴とする請求項１から４のいずれか一項に記載の制御装置。
6. 前記ローパスフィルターは、カットオフ周波数が２０Ｈｚ以上２００Ｈｚ以下である請求項５に記載の制御装置。
7. 前記制御部は、前記慣性センサーの出力に含まれる離調ノイズを、バンドエリミネーションフィルターで除去することを特徴とする請求項１から４のいずれか一項に記載の制御装置。
8. 前記制御部は、前記慣性センサーの出力に含まれるオフセット成分を移動平均により求め、前記オフセット成分を前記慣性センサーの出力から除去することを特徴とする請求項１から３のいずれか一項に記載の制御装置。
9. 前記制御部は、前記慣性センサーの出力に含まれるオフセット成分をハイパスフィルターで除去することを特徴とする請求項１から４のいずれか一項に記載の制御装置。
10. 前記制御部は、前記慣性センサーの出力を、前記モーターの制御にフィードバックするゲインを、前記第２アームの角度に基づいて調整することを特徴とする請求項１から４のいずれか一項に記載の制御装置。
11. 前記制御部は、前記慣性センサーの出力を、前記モーターの制御にフィードバックするゲインを、前記シャフトに付加されるワークの質量に基づいて調整することを特徴とする請求項１から４のいずれか一項に記載の制御装置。
12. 前記制御部は、前記慣性センサーの出力を、前記モーターの制御にフィードバックするゲインを、前記直動の位置に基づいて調整することを特徴とする請求項１から４のいずれか一項に記載の制御装置。
13. 前記制御部は、前記ローパスフィルターのカットオフ周波数を、前記第２アームの角度に基づいて調整することを特徴とする請求項５または６に記載の制御装置。
14. 前記制御部は、前記ローパスフィルターのカットオフ周波数を、前記シャフトに付加されるワークの質量に基づいて調整することを特徴とする請求項５または６に記載の制御装置。
15. 前記制御部は、前記ローパスフィルターのカットオフ周波数を、前記直動の位置に基づいて調整することを特徴とする請求項５または６に記載の制御装置。
16. 前記慣性センサーは、前記第２軸と前記第３軸を含む平面に対して直交する角速度検出軸周りの角速度を検出する角速度センサーである、請求項１から１５のいずれか一項に記載の制御装置。
17. 請求項１から１６のいずれか一項に記載の制御装置によって制御される、
    水平多関節ロボット。
18. 請求項１から１６のいずれか一項に記載の制御装置と、
    前記制御装置によって制御される水平多関節ロボットと、を備える、
    ロボットシステム。

Images