JP2019141970A - ロボットのアーム回転軸速度検出装置 - Google Patents

Abstract

【課題】アームの軸側に配置する回転センサの分解能が低いものであっても、回転速度の検出精度を高める。【解決手段】ロボットアームの回転軸２５側に、ホールセンサ２８（１），２８（２）を互いに９０度位相差のセンサ信号を出力するように配置する。速度用マイコン１４の速度演算部３１は、ホールセンサ２８（１）より出力されるセンサ信号より得られる角度を微分して角速度（１）を求め、ホールセンサ２８（２）より出力されるセンサ信号より得られる角度を微分して角速度（２）を求める。そして、角速度（１）に重み係数（１）を乗じた結果と、角速度（２）に重み係数（２）を乗じた結果とを加算してアームの回転速度を求める。【選択図】図１

Description

本発明は、ロボットのアーム回転軸速度を検出する装置に関する。

ロボットアームの各軸を駆動するモータには、ロータリエンコーダのような回転センサが取り付けられている。そして、ロボットのアームは、モータの回転軸に連結された減速機構を介して駆動される。このようなロボットが動作する際に安全を確保するため、例えば図９に示すように、減速機構を経たアームの回転軸，つまり出力側にも回転センサを設けてアームの動作速度を直接検出し、動作速度が上限値を超えた際にモータの駆動を停止させることが考えられる。

出力側に配置する回転センサとして、例えば同図に示すようにホールセンサを用いることができる。アームの回転軸２５に永久磁石２７を配置し、その磁石の回転に伴う磁気の変化をホールセンサ２８により検出する。２つのホールセンサ（１），２８（２）を用いて９０度位相差のセンサ信号を得るようにすれば、アームの回転方向も検出できる。

特開２０１５−１４２９４８号公報

２相のセンサ信号から回転速度を得ることを想定すると、一方の信号がｓｉｎθ，他方の信号がｃｏｓθに対応するので、回転位置θを次式で求め、
θ＝ａｒｃｔａｎ（ｓｉｎθ／ｃｏｓθ） …（０）
回転位置θを微分すれば速度が得られる。

しかしながら、一般に、ホールセンサにより得られる回転位置情報は分解能が低い。そのため、上記の式に基づいて回転速度を求めると、誤差が非常に大きくなってしまう。例えば、図１０に示すような２相のホールセンサ信号をＡ／Ｄ変換すると、図１１に示すように、量子化誤差を含んだ値となる。そして、これらの値から（０）式により角度θを求めると図１２に示すようになり、その角度θを微分して得られた速度は、図１３に示すように極めてばらつきが大きい値となってしまう。

本発明は、上記実情に鑑みてなされたものであり、その目的は、アームの軸側に配置する回転センサの分解能が低くいものであっても、回転速度の検出精度を高めることができるロボットのアーム回転軸速度検出装置を提供することにある。

請求項１記載のロボットのアーム回転軸速度検出装置によれば、減速機構を介して駆動されるロボットアームの駆動機構に適用され、第１及び第２回転センサは、アームの回転軸側に配置されて、互いに９０度位相差の回転位置信号を出力する。速度演算部は、第１回転センサより出力される第１回転位置信号より得られる角度を微分して第１速度を求め、第２回転センサより出力される第２回転位置信号より得られる角度を微分して第２速度を求める。そして、第１速度に第１重み係数を乗じた結果と、第２速度に第２重み係数を乗じた結果とを加算してアームの回転速度を求める。

２つの回転センサより得られる回転位置信号の分解能が低く、それらによって得られる軸の回転位置が粗ければ、その回転位置より得られる角度を微分して求めた回転速度の精度も低い。しかし、それぞれの回転位置信号は周期的に出力されるので、得られた第１速度，第２速度の値のばらつきにも周期性が現れる。そこで、その周期性を反映させた第１，第２重み係数を用い、各速度について精度良く得られる期間を採用するようにし、重み係数を乗じた結果を加算して速度を求めれば、精度を向上させることができる。したがって、分解能が低い回転センサを用いても、それらが出力する回転位置信号から回転位置を求め、速度を求める場合に比較して、より精度が高い回転速度を得ることができる。

請求項２記載のロボットのアーム回転軸速度検出装置によれば、回転位置をθとし、第１回転位置信号がｓｉｎθで表され，第２回転位置信号がｃｏｓθで表されるとすると、
第１重み係数をＳ１／（Ｓ１＋Ｃ１）,
第２重み係数をＣ１／（Ｓ１＋Ｃ１）,
Ｓ１＝ ｓｉｎθ ／√（ｓｉｎ²θ＋ｃｏｓ²θ）
Ｃ１＝ ｃｏｓθ ／√（ｓｉｎ²θ＋ｃｏｓ²θ）
とする。
このような重み係数を用いることで、各回転信号の周期性を考慮して回転速度が精度良く得られる期間をそれぞれ抽出できる。

請求項３記載のロボットのアーム回転軸速度検出装置によれば、第１及び第２回転センサを、アームの回転軸に配置される永久磁石の磁気変化を検出する磁気センサとする。このように構成すれば、安価な磁気センサを２つ用いてロボットのアーム回転軸速度を検出できる。

一実施形態であり、ロボットの各軸周りの構成と、それらに係るコントローラの制御部の構成とをモデル的に示す図 コントローラについて、要部に係る部分を示す機能ブロック図 ロボットシステムの構成を示す図 速度用マイコンにより実行される回転軸の速度を求める処理を示すフローチャート ２つのホールセンサより出力される各信号にそれぞれ逆三角関数を適用して求めた角度を示す波形図 図５に示す角度から求めた角速度を示す波形図 図５に示す角度に適用する重み係数を示す波形図 重み係数を適用して求めた角速度を示す波形図 ロボットの各軸周りの構成をモデル的に示す図 ２相のホールセンサ信号を示す図 ホールセンサ信号のＡＤ変換値を示す図 ホールセンサ信号から求めた角度を示す図 角度から求めた回転軸の速度を示す図

以下、一実施形態について図１から図８を参照して説明する。図３に示すように、ロボットシステム１は、垂直多関節型のロボット２、ロボット２を制御するコントローラ３を備えている。このロボットシステム１は、一般的な産業用に用いられている。ロボット２は、いわゆる６軸の垂直多関節型ロボットとして周知の構成を備えており、ベース５上に、Ｚ方向の軸心を持つ第１軸（Ｊ１）を介してショルダ６が水平方向に回転可能に連結されている。ショルダ６には、Ｙ方向の軸心を持つ第２軸（Ｊ２）を介して上方に延びる下アーム７の下端部が垂直方向に回転可能に連結されている。下アーム７の先端部には、Ｙ方向の軸心を持つ第３軸（Ｊ３）を介して第一上アーム８が垂直方向に回転可能に連結されている。

第一上アーム８の先端部には、Ｘ方向の軸心を持つ第４軸（Ｊ４）を介して第二上アーム９が捻り回転可能に連結されている。第二上アーム９の先端部には、Ｙ方向の軸心を持つ第５軸（Ｊ５）を介して手首１０が垂直方向に回転可能に連結されている。手首１０には、Ｘ方向の軸心を持つ第６軸（Ｊ６）を介してフランジ１１が捻り回転可能に連結されている。

ベース５、ショルダ６、下アーム７、第一上アーム８、第二上アーム９、手首１０およびフランジ１１は、ロボット２のアームとして機能し、アームの先端となるフランジ１１には、ワークを保持するための図示しないツールが取り付けられる。ロボット２に設けられている各軸（Ｊ１〜Ｊ６）には、それぞれに対応して駆動源となる図示しないモータが設けられている。

コントローラ３は、ロボット２の制御装置であり、図示しないＣＰＵ、ＲＯＭおよびＲＡＭ等で構成されたコンピュータからなる制御手段においてコンピュータプログラムを実行することで、ロボット２を制御する。具体的には、コントローラ３は、インバータ回路等から構成された駆動部を備えており、各モータに対応して設けられているエンコーダで検知したモータの回転位置に基づいて例えばフィードバック制御によりそれぞれのモータを駆動する。

コントローラ３は、ＣＰＵ、ＲＯＭ、ＲＡＭ、上記駆動部に相当するモータドライバ等を備えている。ＲＯＭは、ロボット２のシステムプログラムや動作プログラム等を記憶している。ＲＡＭは、これらのプログラムを実行する際にパラメータの値等を記憶する。位置検出回路には、コントローラ３には、ロボット２の各関節に設けられた図示しない各エンコーダの検出信号がそれぞれ入力される。

図２は、コントローラ３について、本実施形態の要部に係る部分を示す機能ブロック図である。コントローラ３は、メインＣＰＵ１２と、各軸Ｊ１〜Ｊ６のそれぞれに対応して設けられる制御用マイコン１３及び速度用マイコン１４とを備えている。コントローラ３には、例えばティーチングペンダントのようなユーザ操作部１５が接続されており、ユーザは、ユーザ操作部１５によりロボット２を制御するための各種パラメータを設定する。メインＣＰＵ１２は、各軸の制御用マイコン１３，速度用マイコン１４に対し、それぞれ角度指令，監視速度を与える。監視速度は、各軸の回転速度を監視するための上限値である。

図１は、ロボット２の各軸の軸周りの構成と、それらに係るコントローラ３の制御部の構成とをモデル的に示している。尚、軸周りの構成は、基本的に図８に示すイメージと同じであり、図８にも同じ符号を付している。モータ２１には、モータ側エンコーダ２２が取り付けられている。エンコーダ２２は、モータ２１の回転角度を制御用マイコン１３に入力する。モータ２１の回転軸２３には、複数段のギアからなるモジュールである減速機構２４の入力側に連結されている。減速機構２４の出力側は、対応するロボットアームの回転軸２５に連結されている。

回転軸２５には、アームの動きを制するための機械式ブレーキ２６が配置されている。また、回転軸２５には永久磁石２７が取り付けられており、回転軸２５が回転することに伴う永久磁石２７の磁界の変化は、２つのホールセンサ２８（１），２８（２）により検出される。ホールセンサ２８（１），２８（２）は、位相が９０度異なる回転位置信号，センサ信号を出力する。

ホールセンサ２８（１），２８（２）より出力されるセンサ信号は、それぞれアンプ２９（１），２９（２）により増幅されて、速度用マイコン１４が備えるＡ／Ｄ変換器３０（１），３０（２）に入力される。Ａ／Ｄ変換器３０（１），３０（２）によりＡ／Ｄ変換されたデータは、角度・速度演算部３１に入力される。

演算部３１は、入力される２相センサ信号のデータに基づいて、回転軸２５の回転角度及び回転速度を演算する。演算された回転角度，アーム角度は、制御用マイコン１３に入力される。また、演算された回転速度は、演算部３１において、メインＣＰＵ１２より与えられる監視速度と比較される。そして、アームの回転速度が監視速度を超えると、速度マイコン１４は、制御用マイコン１３に停止指令を出力する。

制御用マイコン１３は、制御対象のアーム角度が、メインＣＰＵ１２より与えられる角度指令に一致するようにモータ２１を制御する。モータ２１の制御信号は、アンプ３２及びモータドライバを介してモータ２１に与えられる。また、制御用マイコン１３は、ブレーキ２６のＯＮ／ＯＦＦも制御する。速度用マイコン１４より停止指令が入力されると、制御用マイコン１３は、ブレーキ２６をＯＮにすると共に、モータ２１の駆動制御を停止する。また、制御用マイコン１３は、アーム角度を参照することで、特許文献１と同様にモータ２１が何回転しているかを検出する多回転数検出処理を行う。

次に、本実施形態の作用について図４から図６を参照して説明する。図４は、速度用マイコン１４により実行される回転軸２５の回転速度を求める処理を示す。尚、ステップＳ１１〜Ｓ１６の処理は、ホールセンサ２８（１），２８（２）について並行的に実行される。速度演算部３１は、ＡＤ変換器３０が出力する変換値が変化したか否かを判断する（Ｓ１１）。変換値が変化していなければ（ＮＯ）、変化時間をカウントするためのカウンタをインクリメントして（Ｓ１６）ステップＳ１１に戻る。

ＡＤ変換値が変化すると（Ｓ１１；ＹＥＳ）、その変換値に対し逆三角関数を適用して角度を求める（Ｓ１２）。更に、ＡＤ変換値の前回値と今回値との差分を算出し（Ｓ１３）、前記カウンタのカウント値から変換値が変化するまでの時間を求める（Ｓ１４）。そして、前記差分値を前記変化時間で除すことで角速度を算出する（Ｓ１５）。

以上のようにして、ホールセンサ２８（１），２８（２）が出力するセンサ信号から角速度（１），角速度（２）を求めると、前記センサ信号の特性に応じて、角速度（１），（２）にそれぞれ適用する重み係数（１），（２）を求める（Ｓ１７）。ここで、重み係数（１），（２）は、例えば以下のように決定する。回転位置をθとし、ホールセンサ２８（１）のセンサ信号をｓｉｎθで表し、ホールセンサ２８（２）のセンサ信号をｃｏｓθで表す。そして、
重み係数（１）＝Ｓ１／（Ｓ１＋Ｃ１） …（１）
重み係数（２）＝Ｃ１／（Ｓ１＋Ｃ１） …（２）
Ｓ１＝ ｓｉｎθ ／√（ｓｉｎ²θ＋ｃｏｓ²θ）
Ｃ１＝ ｃｏｓθ ／√（ｓｉｎ²θ＋ｃｏｓ²θ）
とする。

尚、係数Ｓ１，Ｃ１の分母は、各センサ信号が理想的な正弦波，余弦波であれば「１」になるが、実際に得られるセンサ信号は、振幅や位相に理想値からのずれがあるため、必ずしも「１」にならない。

それから、角速度（１）に重み係数（１）を乗じて重み付け角速度（１）を算出し（Ｓ１８）、角速度（２）に重み係数（２）を乗じて重み付け角速度（２）を算出すると（Ｓ１９）、重み付け角速度（１）に重み付け角速度（２）を加算して回転軸２５の速度Ｖを求める（Ｓ２０）。尚、角速度（１）は第１速度に相当し、角速度（２）は第２速度に相当する。また、重み係数（１）は第１重み係数に相当し、重み係数（２）は第２重み係数に相当する。

速度マイコン１４は、上式で求めた速度を監視速度と比較して、監視速度を超えると前述したように制御用マイコン１３に停止指令を出力する。すると、制御用マイコン１３はブレーキ２６をＯＮにすると共に、モータ２１の駆動制御を停止する。

ホールセンサ２８（１），２８（２）により出力される各信号について、それぞれ逆三角関数を適用して角度［ｄｅｇ］を求めると、図５に示すようになる。そして、この角度から角速度［ｄｅｇ／ｓ］を求めると、図６に示すようになる。図１３に示したように、センサ信号から角度θを求め、角度θを微分して求めた速度のばらつきの最大，最小間の振れ幅は１０７．３［ｄｅｇ／ｓ］である。これに対して、図６に示す最大，最小間の振れ幅は３６．５［ｄｅｇ／ｓ］であり、かなり改善されているが誤差はまだ大きい。

これに対して、ステップＳ１７〜Ｓ１９において、図７に示す波形の重み係数（１），（２）を適用して角速度［ｄｅｇ／ｓ］を求めると、各センサ信号より得られる角速度が精度良く得られる区間をそれぞれ抽出できる。尚、図中の「Ａｓｉｎ」は重み係数（１）を示し、「Ａｃｏｓ」は重み係数（２）を示す。その結果、最大，最小間の振れ幅は６．２［ｄｅｇ／ｓ］となり、図１３に示すケースに比較して約１７倍，図６に示すケースに比較して約５．９倍に精度が改善されている。

以上のように本実施形態によれば、ロボットアームの回転軸２５側に、ホールセンサ２８（１），２８（２）を互いに９０度位相差のセンサ信号を出力するように配置する。速度用マイコン１４の速度演算部３１は、ホールセンサ２８（１）より出力されるセンサ信号より得られる角度を微分して角速度（１）を求め、ホールセンサ２８（２）より出力されるセンサ信号より得られる角度を微分して角速度（２）を求める。

そして、角速度（１）に重み係数（１）を乗じた結果と、角速度（２）に重み係数（２）を乗じた結果とを加算してアームの回転速度を求める。これにより、分解能が低い安価なホールセンサ２８を用いても、（０）式から角度を求めて速度を求める場合に比較して、より精度が高い回転速度Ｖを得ることができる。この場合、（１）式，（２）式より得られる重み係数（１），（２）を用いることで、各センサ信号の周期性を考慮して、回転速度が精度良く得られる期間をそれぞれ抽出できる。

本発明は上記した、又は図面に記載した実施形態にのみ限定されるものではなく、以下のような変形又は拡張が可能である。
ステップＳ２及びＳ３の処理を省略しても良い。
回転センサは、磁気センサに限ることはない。
垂直６軸型以外のロボットに適用しても良い。
重み係数（１），（２）は、（１）式，（２）式より得られるものに限らない。

図面中、１はロボットシステム、２はロボット、３はコントローラ、１３は制御用マイコン、１４は速度用マイコン、２１はモータ、２４は減速機構、２５は回転軸、２７は永久磁石、２８はホールセンサ、３１は角度・速度演算部を示す。

請求項２記載のロボットのアーム回転軸速度検出装置によれば、回転位置をθとし、第１回転位置信号がｓｉｎθで表され，第２回転位置信号がｃｏｓθで表されるとすると、
第１重み係数をＳ１／（Ｓ１＋Ｃ１）,
第２重み係数をＣ１／（Ｓ１＋Ｃ１）,
Ｓ１＝｜ｓｉｎθ｜／√（ｓｉｎ²θ＋ｃｏｓ²θ）
Ｃ１＝｜ｃｏｓθ｜／√（ｓｉｎ²θ＋ｃｏｓ²θ）
とする。
このような重み係数を用いることで、各回転信号の周期性を考慮して回転速度が精度良く得られる期間をそれぞれ抽出できる。

以上のようにして、ホールセンサ２８（１），２８（２）が出力するセンサ信号から角速度（１），角速度（２）を求めると、前記センサ信号の特性に応じて、角速度（１），（２）にそれぞれ適用する重み係数（１），（２）を求める（Ｓ１７）。ここで、重み係数（１），（２）は、例えば以下のように決定する。回転位置をθとし、ホールセンサ２８（１）のセンサ信号をｓｉｎθで表し、ホールセンサ２８（２）のセンサ信号をｃｏｓθで表す。そして、
重み係数（１）＝Ｓ１／（Ｓ１＋Ｃ１） …（１）
重み係数（２）＝Ｃ１／（Ｓ１＋Ｃ１） …（２）
Ｓ１＝｜ｓｉｎθ｜／√（ｓｉｎ²θ＋ｃｏｓ²θ）
Ｃ１＝｜ｃｏｓθ｜／√（ｓｉｎ²θ＋ｃｏｓ²θ）
とする。

Claims (3)

1. モータと、このモータの回転軸に連結される減速機構と、この減速機構を介して駆動されるロボットアームの回転軸とで構成されるロボットアームの駆動機構に適用され、
   前記アームの回転軸側に配置され、９０度位相差の回転位置信号を出力する第１及び第２回転センサと、
   前記第１回転センサより出力される第１回転位置信号より得られる角度を微分して第１速度を求め、
   前記第２回転センサより出力される第２回転位置信号より得られる角度を微分して第２速度を求め、
   前記第１速度に第１重み係数を乗じた結果と、前記第２速度に第２重み係数を乗じた結果とを加算して、前記アームの回転速度を求める速度演算部とを備えるロボットのアーム回転軸速度検出装置。
2. 回転位置をθとした際に、前記第１回転位置信号がｓｉｎθで表され，前記第２回転位置信号がｃｏｓθで表されるとすると、
   前記第１重み係数をＳ１／（Ｓ１＋Ｃ１）,
   前記第２重み係数をＣ１／（Ｓ１＋Ｃ１）,
   Ｓ１＝ ｓｉｎθ ／√（ｓｉｎ²θ＋ｃｏｓ²θ）
   Ｃ１＝ ｃｏｓθ ／√（ｓｉｎ²θ＋ｃｏｓ²θ）
   とする請求項１記載のロボットのアーム回転軸速度検出装置。
3. 前記アームの回転軸には永久磁石が配置されており、
   前記第１及び第２回転センサは、前記永久磁石の磁気変化を検出する磁気センサである請求項１又は２記載のロボットのアーム回転軸速度検出装置。

Images