(57)【特許請求の範囲】
【請求項１】
アームと、
前記アームの回転角度を検出する角度センサーと、
前記アームに取り付けられた慣性センサーと、を備えたロボットであって、
前記慣性センサーによって検出される角速度と、前記角度センサーによって検出される情報から得られる慣性センサー座標における角速度と、の差を用いて算出された前記慣性センサーの感度補正量に基づいて、前記ロボットは動作されることを特徴とするロボット。
【請求項２】
前記差は、前記ロボットの動作中の所与の時間間隔で算出され、
前記感度補正量は、前記ロボットの動作終了後、前記時間間隔で算出された前記差の総和から算出されることを特徴とする請求項１に記載のロボット。
【請求項３】
前記感度補正量は、前記ロボットの動作速度の絶対値に対応する前記慣性センサーの感度の補正定数を用いて算出されることを特徴とする請求項１または請求項２に記載のロボット。
【請求項４】
移動装置と、
前記移動装置の速度を検出する速度センサーと、
前記移動装置に取り付けられた加速度センサーと、を備えたロボットであって、
前記速度センサーによって検出される速度と、前記加速度センサーによって検出されるセンサー座標系における速度と、の差を用いて算出された前記加速度センサーの感度補正量に基づいて、前記ロボットは動作されることを特徴とするロボット。
【請求項５】
前記差は、前記ロボットの動作中の所与の時間間隔で算出され、
前記感度補正量は、前記ロボットの動作終了後、前記時間間隔で算出された前記差の総和から算出されることを特徴とする請求項４に記載のロボット。
【請求項６】
前記感度補正量は、前記ロボットの動作速度の絶対値に対応する前記加速度センサーの感度の補正定数を用いて算出されることを特徴とする請求項４または請求項５に記載のロボット。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は、ロボットに関する。
【背景技術】
【０００２】
ロボットを動作させる場合、角度センサーによってモーターの回転角度を検出し回転角度から角速度を算出して動作を制御している。しかし、モーターによって駆動される伝達機構やアームは剛体ではなく、動作時に振動や捩れが発生し、正確な制御ができないことがあった。
そこで、アームの先端部に配置した慣性センサーによって検出した加速度を用いてアームの撓み量を算出し、アーム動作の角度目標値に加算して、アームの振動を抑制するロボットが提案されている（例えば、特許文献１参照）。
【０００３】
また、アーム先端部の動作を慣性センサーで検出し、この慣性センサーの検出信号を用いてアームを駆動することによって、アームに振動があっても高精度の位置決めを可能にしようとしたものもある（例えば、特許文献２参照）。
【先行技術文献】
【特許文献】
【０００４】
【特許文献１】特開平１−１７３１１６号公報
【特許文献２】特開平７−９３７４号公報
【発明の概要】
【発明が解決しようとする課題】
【０００５】
前述した特許文献１や特許文献２では、アーム先端部に慣性センサーを配置して、慣性センサーの検出値を用いてアームの動作（位置を含む）を制御するものである。しかしながら、慣性センサーの感度は、慣性センサーの個体差、温度変化による影響や経年変化などで精度の低下が予想される。このような慣性センサーの精度低下によって、目標どおりの制御が行われなくなることから、ロボットの動作精度が低下してしまうという課題がある。
【課題を解決するための手段】
【０００６】
本発明は、上述の課題の少なくとも一部を解決するためになされたものであり、以下の形態または適用例として実現することが可能である。
【０００７】
［適用例１］本適用例に係るロボットの制御方法は、モーターと、前記モーターの回転角度を検出する角度センサーと、前記モーターに連結されるアームと、前記アームに取り付けられる慣性センサーと、を備えるロボットの制御方法であって、前記慣性センサーによって検出される角速度と、前記角度センサーによって検出される角度情報から得られる慣性センサー座標における角速度と、の差を用いて前記アームの捩れ角速度を算出する工程と、前記捩れ角速度を用いて前記慣性センサーの感度の補正量を算出する工程と、前記補正量を用いて前記慣性センサーの感度を補正する工程と、を含むことを特徴とする。
ここで、慣性センサーとしてはジャイロセンサー、角度センサーとしてはエンコーダー、レゾルバー等がある。
また、慣性センサー座標は、慣性センサーが取付けられた位置を基準として表す座標系である。なお、以降、慣性センサー座標を単にセンサー座標と表すことがある。慣性センサーの出力値はセンサー座標系の出力値となるため、角度センサーの角速度情報もセンサー座標系に変換して捩れ角速度を算出する。
【０００８】
本適用例によれば、慣性センサーが検出する角速度と、角度センサーが検出する情報から得られる慣性センサー座標における角速度と、の差を用いて慣性センサーの感度補正量を決定する。この角速度差が捩れ角速度である。こうして算出した感度補正量を用いて慣性センサーの感度を補正し、ロボットを動作させる。このことによって、慣性センサーの個体差、温度変化による精度への影響、経年変化による精度低下への影響を低減し、ロボット動作の精度を向上させることができる。
【０００９】
［適用例２］上記適用例に係るロボットの制御方法において、前記補正量を算出する工程は、前記ロボットの動作中に実行すること、が好ましい。
【００１０】
ロボットが静止時に感度誤差の算出を実行すると、感度検出時のノイズ成分が累積されて感度誤差が実際と異なることが考えられる。よって、動作しているときのみに、感度誤差の補正量を算出することにより、ノイズの影響を抑制し、高精度の感度補正量を得ることができる。
なお、慣性センサーが角速度を持たない動作、角速度が一定の大きさを持たない動作の場合には静止時にあると判定する。
【００１１】
［適用例３］上記適用例に係るロボットの制御方法において、前記補正量を算出する工程は、前記ロボットの動作中の所与の時間間隔で捩れ角速度を算出し、動作終了後に前記時間間隔で算出した前記捩れ角速度の総和から前記感度誤差の補正量を算出すること、が好ましい。
【００１２】
例えば、６００ｍＳで１動作を終了する場合に、慣性センサーでは２ｍｓ毎に角速度を検出し、１動作終了後に、２ｍｓ毎に検出された捩れ角速度の和を算出して感度誤差の補正量とする。このようにすれば、捩れ角速度の変化をほぼリアルタイムで取得し、補正量を１動作毎に算出すれば演算工程の計算負荷を低減することができる。
また、ロボットを動作させながら慣性センサーの感度を更新していくので、繰り返し動作を実行する場合に、１動作毎に精度を向上させていくことができるという効果がある。
【００１３】
［適用例４］上記適用例に係るロボットの制御方法において、前記補正量を算出する工程は、前記モーターの回転方向を検出した後、前記回転方向に応じて前記捩れ角速度の値の正負を切り替えること、が好ましい。
【００１４】
捩れ角速度の算出では回転方向の情報がない。そこで、モーターの回転方向の正逆によって算出した捩れ角速度の値を正または負に切り替える。そのことによって、慣性センサーが配置されたアームの正逆回転両方の感度補正を行うことができる。
【００１５】
［適用例５］上記適用例に係るロボットの制御方法において、前記慣性センサーのオフセット除去工程をさらに有し、前記捩れ角速度を算出する工程は、前記オフセット除去工程後に実行すること、が好ましい。
【００１６】
このようにすれば、慣性センサーのオフセットの影響を除去し、高精度の感度誤差の補正を行うことができる。
【００１７】
［適用例６］上記適用例に係るロボットの制御方法において、前記補正量は、前記ロボットの動作速度の大小に対応する前記慣性センサーの感度の補正定数を用いて算出すること、が好ましい。
【００１８】
このようにすれば、動作速度が大きい場合には補正定数を大きくして感度補正量を大きくし、動作速度が小さい場合には補正定数を小さくすることで感度補正量を小さくすることができる。なお、補正定数は、例えば慣性センサーの感度補正ゲインを用いて算出する。
【００１９】
［適用例７］本適用例に係るロボットは、モーターと、前記モーターに連結されて駆動されるアームと、前記モーターの回転角度を検出する角度センサーと、前記アームに取り付けられた慣性センサーと、前記慣性センサーによって検出される角速度と、前記角度センサーによって検出される情報から得られる慣性センサー座標における角速度と、の差を用いて前記アームの捩れ角速度を算出する捩れ角速度演算部と、前記慣性センサーの感度補正量を算出する感度補正量演算部と、前記感度補正量演算部が算出する感度補正量を用いて感度を補正する感度補正部と、前記アームの動作を判断する動作判断部と、を備えることを特徴とする。
【００２０】
本適用例によれば、慣性センサーが検出する角速度と、角度センサーが検出する角度情報から得られる慣性センサー座標における角速度と、の差を用いて慣性センサーの感度補正量を決定する。この角速度差が捩れ角速度である。こうして算出した感度補正量を用いて慣性センサーの感度を補正し、ロボットを動作させる。このことによって、慣性センサーの個体差、温度変化による精度への影響、経年変化による精度低下への影響を排除し、ロボット動作の精度を向上させることができる。
【００２１】
［適用例８］本適用例に係るロボットの制御方法は、加速度センサーによって検出される加速度を用いて、センサー座標系の速度を演算する工程と、速度センサーによって検出されるセンサー座標系の速度と、前記加速度を用いて算出されるセンサー座標系の速度と、の差から歪み速度を算出する工程と、前記歪み速度を用いて前記加速度センサーの感度補正量を算出する工程と、前記感度補正量を用いて前記加速度センサーの感度を補正する工程と、を含むことを特徴とする。
【００２２】
本適用例によるロボットの制御方法は、上述した捩れ角速度を用いて慣性センサー（例えば、ジャイロセンサー）の感度補正を行うことに対して、歪み速度を用いて加速度センサーの感度補正を行いロボットを動作させる。このことによって、加速度センサーの個体差、温度変化による精度への影響、経年変化による精度低下への影響を低減し、ロボット動作の精度を向上させることができる。
【００２３】
［適用例９］上記適用例に係るロボットの制御方法において、前記補正量を算出する工程は、前記ロボットの動作中に実行すること、が好ましい。
【００２４】
ロボットが静止時に感度誤差の算出を実行すると、感度検出時のノイズ成分が累積されて感度誤差が実際と異なることが考えられる。よって、動作しているときのみに、感度誤差の補正量を算出することにより、ノイズの影響を抑制し、高精度の感度補正量を得ることができる。
なお、加速度センサーが速度を持たない動作、速度が一定の大きさを持たない動作の場合には静止時にあると判定する。
【００２５】
［適用例１０］上記適用例に係るロボットの制御方法において、前記補正量を算出する工程は、前記ロボットの動作中の所与の時間間隔で歪み速度を算出し、動作終了後に前記時間間隔で算出した歪み速度の総和から前記感度誤差の補正量を算出すること、が好ましい。
【００２６】
例えば、６００ｍＳで１動作を終了する場合に、加速度センサーでは２ｍｓ毎に加速度を検出し、１動作終了後に、２ｍｓ毎に検出された歪み速度の総和を算出して感度誤差の補正量とする。このようにすれば、歪み速度の変化をほぼリアルタイムで取得し、補正量を１動作毎に算出すれば演算工程の計算負荷を低減することができる。
また、ロボットを動作させながら加速度センサーの感度を更新していくので、繰り返し動作を実行する場合に、１動作毎に精度を向上させていくことができるという効果がある。
【００２７】
［適用例１１］上記適用例に係るロボットの制御方法において、前記補正量を算出する工程は、前記加速度センサーの取付け位置の移動方向を検出した後、前記移動方向に応じて前記歪み速度の値の正負を切り替えること、が好ましい。
【００２８】
歪み速度の算出では移動方向の情報がない。そこで、加速度センサー取付け位置の移動方向の正逆によって算出した歪み速度の値を正または負に切り替える。そのことによって、加速度センサーが配置された位置における正逆移動両方の感度補正を行うことができる。
【００２９】
［適用例１２］上記適用例に係るロボットの制御方法において、前記加速度センサーのオフセット除去工程をさらに有し、前記歪み速度を算出する工程は、前記オフセット除去工程後に実行すること、が好ましい。
【００３０】
加速度センサーは初期状態ではオフセット誤差が存在する。オフセット誤差は、静止時に加速度センサーの検出値が「０」以外の値を検出する場合の誤差であって、オフセット誤差を除去しておくことで、高精度の感度誤差の補正を行うことができる。
【００３１】
［適用例１３］上記適用例に係るロボットの制御方法において、前記補正量は、前記ロボットの動作速度の絶対値に対応する前記加速度センサーの感度の補正定数を用いて算出すること、が好ましい。
【００３２】
このようにすれば、動作速度が大きい場合には補正定数を大きくして感度補正量を大きくし、動作速度が小さい場合には補正定数を小さくすることで感度補正量を小さくすることができる。
なお、補正定数は、例えば慣性センサーの感度補正ゲインを用いて算出する。
【００３３】
［適用例１４］本適用例に係るロボットは、加速度センサーの検出値を取得し、センサー座標系における速度を算出するセンサー座標速度演算部と、速度センサーにより検出した速度と、前記センサー座標系における速度と、の差から歪み速度を算出する歪み速度演算部と、前記加速度センサーの感度補正量を算出する感度補正量演算部と、前記感度補正量に基づき前記加速度センサーの感度を補正する感度演算部と、動作中であるか、静止中であるかを判断する動作判断部と、を備えていることを特徴とする。
【００３４】
本適用例によれば、歪み速度を用いて加速度センサーの感度補正を行いロボットを動作させる。このことによって、加速度センサーの個体差、温度変化による精度への影響、経年変化による精度低下への影響を低減し、ロボット動作の精度を向上させることができる。
なお、本適用例のロボットは、直動ロボットや３軸・６軸ロボットにも適合可能である。
【図面の簡単な説明】
【００３５】
【図１】実施形態１に係るロボットの概略構成を例示する構成説明図。
【図２】実施形態１に係る制御装置の主たる構成を示す構成説明図。
【図３】事例１の感度誤差と捩れ角速度の関係を表すデータ。
【図４】事例２の感度誤差と捩れ角速度の関係を表すデータ。
【図５】事例３の感度誤差と捩れ角速度の関係を表すデータ。
【図６】事例４の感度誤差と捩れ角速度の関係を表すデータ。
【図７】実施形態１に係るロボットの制御方法の主要工程を示すフローチャート。
【図８】実施形態１に係る６軸ロボットを模式的に示す構成説明図。
【図９】実施形態２に係る直動ロボットの概略構成を例示する構成説明図。
【図１０】実施形態２に係る制御装置の主たる構成を示す構成説明図。
【図１１】実施形態２に係るセンサー座標速度演算部の構成を示す構成説明図。
【図１２】実施形態２に係る６軸ロボットを模式的に示す構成説明図。
【発明を実施するための形態】
【００３６】
以下、本発明の実施形態を図面を参照して説明する。
（実施形態１）
（ロボットの構成）
図１は、実施形態１に係るロボットの概略構成を例示する構成説明図である。なお、図１は、各部材を認識可能な大きさとするため、各部材ないし部分の縦横の縮尺は実際のものとは異なる模式図である。また、本実施形態では３軸ロボット（スカラーロボット）を例示している。図１において、ロボット１０は、平板状の基台３８をベースとして、複数のモーターとアームとが連結されて構成されている。基台３８上には、空間が形成された支持枠３９が配置されている。
【００３７】
支持枠３９の下方空間にはモーター４１が配置され、モーター４１の軸方向端部には角度センサーとしての第１エンコーダー４２が配置されている。第１エンコーダー４２は、モーター４１の回転角度を検出する。
【００３８】
支持枠３９の上方空間には第１減速機４３が配置されており、モーター４１の回転軸４１ａが第１減速機４３と連結されている。第１減速機４３の上側には出力軸４３ａが突設されており、モーター４１の回転速度を所定の減速比で減速した回転速度にて出力軸４３ａが回転される。第１減速機４３は各種の減速機構を採用することができるが、本実施形態では、波動歯車を採用している。出力軸４３ａは、支持枠３９の孔部３９ａを貫通して支持枠３９の上方に突設されている。
【００３９】
突設された出力軸４３ａには、アーム４４の一方の端部が連結されて、アーム４４は出力軸４３ａを回転軸としてＸＹ平面内で回動する。アーム４４の他方の端部には、第２減速機４６、モーター４７が重ねて連結されており、モーター４７の回転軸方向端部には角度センサーとしての第２エンコーダー４８が配置されている。第２エンコーダー４８は、モーター４７の回転角度を検出する。そして、第２減速機４６の出力軸４６ａは、アーム４４の孔部４４ａを貫通して、アーム４４から突設されている。第２減速機４６の出力軸４６ａは、モーター４７の回転速度を所定の減速比で減速した回転速度にて回転する。
【００４０】
モーター４１及びモーター４７は電気信号によって回転方向が制御可能であれば各種のモーターを用いることができる。本実施形態では、例えば、直流モーターを採用する。第１エンコーダー４２はモーター４１の回転角度を検出可能であればよく、第２エンコーダー４８はモーター４７の回転角度を検出可能であればよいので、各種のロータリーエンコーダーを用いることができる。本実施形態では、例えば、光学式のロータリーエンコーダーを採用する。出力軸４６ａには、アーム４９の一方の端部が連結されており、出力軸４６ａを回転軸としてＸＹ平面内で回動する。
【００４１】
アーム４９の他方の端部には、慣性センサーとしてのジャイロセンサー５０が配置されている。ジャイロセンサー５０は、種別は特に限定されないが、本実施形態では、振動型ジャイロセンサーを採用している。なお、ジャイロセンサー５０は、本実施例では３軸ジャイロセンサーを採用することが好ましい。
【００４２】
アーム４９のジャイロセンサー５０側には、モーター５１が配置され、モーター５１の回転軸５１ａが、アーム４９の孔部４９ａを貫通して基台３８側に突設されている。回転軸５１ａには昇降装置５２が連結されており、回転軸５１ａによって回転可能な構成としている。
【００４３】
昇降装置５２の下先端部にはハンド部５３が配置されている。昇降装置５２は上下（Ｚ軸方向）に移動する直動機構を有し、ハンド部５３を昇降することができる。ハンド部５３は複数の指部５３ａと直動機構（図示せず）とを有し、直動機構は複数の指部５３ａの間隔を変更することができる。そして、ハンド部５３は指部５３ａの間に作業対象のワーク（図示せず）を挟んで保持することが可能になっている。
ロボット１０には、上述した各駆動系を制御する制御装置２０が備えられている。制御装置２０は、モーター４１，４７，５１の駆動制御、第１エンコーダー４２、第２エンコーダー４８、及びジャイロセンサー５０の検出インターフェイス、第１エンコーダー４２、第２エンコーダー４８、及びジャイロセンサー５０の検出値に基づき捩れ角速度、感度補正量を算出する演算部等を有している。
【００４４】
続いて、制御装置２０の構成について説明する。
図２は、制御装置２０の主たる構成を示す構成説明図である。制御装置２０は、ジャイロセンサー５０の角速度の検出値と、第１エンコーダー４２によるモーター４１の回転角度検出値、及び第２エンコーダー４８によるモーター４７の回転角度検出値をインターフェイス（図示せず）を介して入手し、捩れ角速度を算出する捩れ角速度演算部２１と、ジャイロセンサー５０の感度補正量を算出する感度補正量演算部２２と、算出された感度補正量に基づきジャイロセンサー５０の感度を補正する感度補正部２４と、ロボット１０が動作中であるか、静止中であるかを判断する動作判断部２３と、を備えている。
なお、図２では、第１エンコーダー４２と第２エンコーダー４８とを総称してエンコーダー４０と表している。
（ロボットの制御方法）
【００４５】
次に、ロボット１０の制御方法について説明する。まず、ロボット１０の制御方法の考え方について計算式を例示して説明する。
【００４６】
ここで、ジャイロセンサー５０が検出した角速度をジャイロ出力と表し、第１エンコーダー４２及び第２エンコーダー４８が検出した角度から算出したジャイロセンサー５０の配置位置における角速度をエンコーダー出力と表す。ジャイロ出力とエンコーダー出力の差が捩れ角速度である。なお、「ジャイロ出力−エンコーダー出力」には、動作の捩れ角速度とジャイロセンサー５０のオフセット誤差とジャイロセンサー５０自身の感度誤差とが含まれている。捩れ角速度は振動成分であるため動作によって差がでるが、１動作を平均するとおよそ「０」になる。
【００４７】
従って、オフセットの影響を除去した「ジャイロ出力−エンコーダー出力」の和をとっていくと感度誤差の影響が分かる。この感度誤差を補正すればジャイロセンサー５０の感度補正を行うことができる。なお、第１エンコーダー４２の情報から算出したモーター４１の角速度をＪ１、第２エンコーダー４８の情報から算出したモーター４７の角速度をＪ２とすると、エンコーダー出力は、Ｊ１とＪ２和であって、「Ｊ１＋Ｊ２」をモーター角速度と表し説明する。
このことを数式を用いて説明する。
【００４８】
ジャイロ角速度＝モーター角速度（Ｊ１＋Ｊ２）＋捩れ角速度……（１）
（１）式を変形すると、
捩れ角速度＝ジャイロ角速度−モーター角速度（Ｊ１＋Ｊ２）……（２）
となり、（２）式は次のように変形させることができる。
捩れ角速度＝捩れ角速度＋モーター角速度（Ｊ１＋Ｊ２）−モーター角速度（Ｊ１＋Ｊ２）…（３）
ここで、算出された捩れ角速度は、次のように表すことができる。
但し、ジャイロ感度は実際の値を１に正規化した値であり、（ジャイロ感度−１）が感度誤差を表す。
算出された捩れ角速度＝ジャイロ角速度−モーター角速度（Ｊ１＋Ｊ２）
＝（捩れ角速度＋モーター角速度（Ｊ１＋Ｊ２））×ジャイロ感度
−モーター角速度（Ｊ１＋Ｊ２）
＝捩れ角速度×ジャイロ感度＋モーター角速度（Ｊ１＋Ｊ２）×
（ジャイロ感度−１）……（４）
【００４９】
捩れ角速度は振動成分であるため、捩れ成分がなければ１動作中の平均値はおよそ「０」になる。従って（３）式の第１項は「０」になり第２項のみが残る。従って、（ジャイロ感度−１）が「０」に近づくように補正すれば、正しいジャイロ感度の値を得ることができる。
【００５０】
続いて、感度誤差と捩れ角速度の関係について、感度誤差がない場合の事例と、感度誤差がある場合の事例をあげ、図３〜図６を参照して説明する。なお、各図の（ａ）の実線はエンコーダー４０からの情報から算出した角速度（モーター角速度（Ｊ１＋Ｊ２））、破線はジャイロセンサー５０が検出した角速度を表し、各図の（ｂ）は算出した捩れ角速度を表し、縦軸が角速度（ｄｅｇ／ｓ）、横軸は時間（ｓｅｃ）である。また、算出された捩れ角速度は、所与の時間間隔（本実例では各事例共に、２ｍｓ間隔とした）で検出した値をグラフで表している。
【００５１】
また、各図共に、動作開始からおよそ０．１３秒までは加速動作し、その後およそ０．１３秒〜０．２秒まで速度一定で動作し、およそ０．２秒〜０．３秒まで減速し静止させる一連の動作を１動作としている。例えば、ワークをＡ地点からＢ地点に移動させる動作を１動作としている。
（事例１）
【００５２】
図３は、事例１の感度誤差と捩れ角速度の関係を表すデータであり、感度誤差がおよそ「０」の場合を表している。つまり、図３（ａ）に示すようにエンコーダー４０から算出したジャイロセンサー座標（ジャイロ座標と表すことがある）における角速度と、ジャイロセンサー５９が検出した角速度の値はほぼ一致している。このとき、図３（ｂ）に表すように、上記（３）式を用いて算出された捩れ角速度の総和は、およそ「０」となる。
（事例２）
【００５３】
図４は、事例２の感度誤差と捩れ角速度の関係を表すデータであり、図４（ａ）に表すように、ジャイロセンサー５０により検出された角速度は、エンコーダー４０の検出値から算出した角速度に対しておよそ＋１０％の感度誤差があることを示している。この感度誤差が捩れ角速度であり、図４（ｂ）に表すように、上記（４）式を用いて算出された捩れ角速度の総和は正の値となり、この値が＋１０％の感度誤差に相当する。
（事例３）
【００５４】
図５は、事例３の感度誤差と捩れ角速度の関係を表すデータであり、感度誤差（捩れ成分と感度誤差成分の和）が−１０％ある場合を表している。図５（ｂ）に表すように、上記（４）式を用いて算出された捩れ角速度の総和は負の値となり、図５（ａ）に表すように、ジャイロセンサー５０により検出された角速度は、エンコーダー４０の検出値から算出した角速度に対しておよそ−１０％の感度誤差があることを示している。
（事例４）
【００５５】
図６は、事例４の感度誤差と捩れ角速度の関係を表すデータである。事例４は、上記事例２及び事例３が正回転させた事例であることに対して逆回転させた場合の事例であり、感度誤差（捩れ成分と感度誤差成分の和）が＋１０％ある場合を表している。図６（ｂ）に表すように、上記（４）式を用いて算出された捩れ角速度の総和は負の値となり、図６（ａ）に表すように、ジャイロセンサー５０により検出された角速度は、エンコーダー４０の検出値から算出した角速度に対しておよそ＋１０％の感度誤差があることを示している。
つまり、以上説明した感度誤差を補正すれば、ジャイロセンサー５０のアーム捩れに起因する感度誤差を排除することができる。以下に、このような考え方を用いたロボットの制御方法をフローチャートを参照して説明する。
【００５６】
図７は、ロボットの制御方法の主要工程を示すフローチャートである。まず、ジャイロセンサー５０のオフセット除去を行う（Ｓ１０）。ジャイロセンサー５０は初期状態ではオフセット誤差が存在する。オフセット誤差は、静止時にジャイロセンサー５０の検出値が「０」以外の値を検出する場合の誤差であって、オフセット誤差を除去しておくことで後工程の演算処理の正確性を確保する。次に、捩れ角速度を算出する（Ｓ２０）。捩れ角速度は、ジャイロセンサー５０によって検出した角速度とモーター角速度（Ｊ１＋Ｊ２）を用いて前述した（２）式によって算出する。
【００５７】
続いて、ロボット１０が動作中であるか、静止中であるかを判定する（Ｓ３０）。ここで、ジャイロセンサー５０、及び第１エンコーダー４２と第２エンコーダー４８の両方が角速度を持たない状態、及びモーター４１の角速度Ｊ１と第２エンコーダー４８で検出したモーター４７の角速度Ｊ２とが逆方向に同じ角度だけ回転するというような動作の場合にロボット１０が静止中であると判断する。ロボット１０が静止中であると判定した場合には、感度補正量＝０とし（Ｓ３５）、補正定数算出の工程（Ｓ６０）に移行する。この際、感度補正量＝０なので、感度補正は行わない。
【００５８】
工程Ｓ３０において、ロボット１０が動作中であると判断した場合には、ジャイロセンサー座標に対してジャイロセンサー５０の回転が正（正回転）であるか、負（逆回転）であるか判断する（Ｓ４０）。逆回転の場合には、捩れ角速度に（−１）を乗ずる（Ｓ４５）。よって、逆回転の場合は、感度補正量＝捩れ角速度×（−１）として次の補正定数算出の工程（Ｓ６０）に移行する。
【００５９】
工程Ｓ４０においてジャイロセンサー５０が正回転と判定した場合には、感度補正量＝捩れ角速度として次の補正定数算出の工程（Ｓ６０）に移行する。つまり、動作指令があって、ジャイロセンサー５０とエンコーダー４０によって取得した角速度が一定の値以上になった場合に動作中であると判定する。補正定数は、工程Ｓ４５、または工程Ｓ５０で算出した感度補正量に、ジャイロセンサー５０の感度補正ゲインを乗じて算出した値であって、回転角速度が大きい場合には大きくなり、回転角速度が小さい場合には小さくなる。
【００６０】
このようにして算出した感度補正量及び補正定数に基づきジャイロセンサー５０の感度補正を実行する（Ｓ７０）。そして、動作指令に基づく動作が終了したと判定した場合（Ｙｅｓ）には、上述した各種演算工程を停止する。この際、この一連の制御工程で算出したデータは、制御装置２０にフィードバックし、次の動作命令はこれらの算出し補正されたデータによる感度で出力される。そして、このような１動作が終了する毎に、ロボット１０に動作をさせながらジャイロセンサー５０の感度を更新していく。
【００６１】
工程Ｓ８０で動作指令による動作が終了していない（Ｎｏ）場合には、工程Ｓ２０から以降の工程を動作終了になるまで継続する。
【００６２】
なお、オフセット除去工程（Ｓ１０）は、感度補正量算出工程（Ｓ５０またはＳ４５）で実行してもよい。
【００６３】
前述したロボットの制御方法によれば、ジャイロセンサー５０が検出する角速度と、エンコーダー４０が検出する情報（第１エンコーダー４２の検出値と第２エンコーダー４８の検出値とを統合した情報）から得られるジャイロセンサー座標における角速度と、の差を用いてジャイロセンサー５０の感度補正量を決定する。この角速度差が捩れ角速度である。こうして算出した感度補正量を用いてジャイロセンサー５０の感度を補正し、ロボット１０を動作させる。このことによって、ジャイロセンサー５０の個体差、温度変化による精度への影響、経年変化による精度低下への影響を排除し、ロボット動作の精度を向上させることができる。
【００６４】
また、ロボット１０を動作させながらジャイロセンサー５０の感度を更新していくので、繰り返し動作を実行する場合に、１動作毎に精度を向上させていくことができるという効果がある。
【００６５】
また、実施形態１のロボット制御方法では、ジャイロセンサー５０の感度誤差の補正量を算出する工程（Ｓ５０またはＳ４５）は、ロボット１０の動作中に実行する。ロボット１０が静止時に感度誤差の算出を実行すると、ノイズ成分が累積されて感度誤差が実際と異なる値になることが考えられる。よって、動作しているときのみに、感度誤差の補正量を算出することにより、ノイズの影響を抑制し、高精度の感度補正量を得ることができる。
【００６６】
ジャイロセンサー５０の感度誤差の補正量を算出する工程（Ｓ５０またはＳ４５）は、ロボット１０の動作中の所与の時間間隔で捩れ角速度を算出し、動作終了後に前述の時間間隔で算出した捩れ角速度を統合して感度誤差の補正量を算出する。このようにすれば、捩れ角速度の変化をほぼリアルタイムで取得することができること、補正量は１動作終了毎に算出すれば演算工程の計算負荷を低減することができる。
【００６７】
また、ジャイロセンサー５０の感度誤差の補正量を算出する工程（Ｓ５０またはＳ４５）は、ジャイロセンサー５０の回転方向を検出した後、回転方向に合わせて捩れ角速度の値の正負を切り替えている。捩れ角速度の算出では直接的な回転方向の情報がない。そこで、ジャイロセンサー５０の回転方向によって算出した捩れ角速度の値を正または負に切り替える。そのことによって、ジャイロセンサー５０の正逆回転両方の感度補正を行うことができる。
【００６８】
また、捩れ角速度を算出する工程（Ｓ２０）は、ジャイロセンサー５０のオフセット除去後に実行することによって、オフセットの影響を除去し、高精度の感度誤差の補正を行うことができる。
【００６９】
ジャイロセンサー５０の感度誤差の補正量は、ロボット１０の動作速度の大小に対応する補正定数を用いて算出する。補正定数は感度補正量に感度補正ゲインを乗じた値である。従って、動作速度が大きい場合には補正定数を大きくして感度補正量を大きくし、動作速度が小さい場合には補正定数を小さくすることで感度補正量を小さくすることができる。
【００７０】
以上説明したロボットの制御方法を用いたロボット１０は、ジャイロセンサー５０の感度を補正しながら動作させることから、ジャイロセンサー５０の個体差、温度変化による精度への影響、経年変化による精度低下、捩れによる動作精度への影響を排除し、動作精度を向上させることができる。
【００７１】
なお、前述したロボット１０は、３軸ロボット（スカラーロボット）を用いた場合を例示して説明したが、６軸ロボットにおいても前述したロボットの制御方法を適合させることができる。このことについて図面を参照して説明する。
（６軸ロボット）
【００７２】
図８は、６軸ロボット１００を模式的に示す構成説明図である。６軸ロボット１００は、Ｘ・Ｙ平面を有する支持台１１上に、Ｘ・Ｙ平面に垂直に配置される基部１２を有し、基部１２の中心軸（Ｚ軸）を基本座標軸として、複数の関節と、関節間を連結するアームとから構成されている。ここで、図８に表した円形と四角形は関節を表し、関節に付す矢印は回転方向を表している。基部１２の端部は支持台１１に固定され、関節Ｊ1と関節Ｊ2の間はアーム１３で連結されている。
【００７３】
また、関節Ｊ2と関節Ｊ3とはアーム１４で連結され、関節Ｊ3とＪ4とはアーム１５で連結され、関節Ｊ4と関節Ｊ5とはアーム１６で連結され、関節Ｊ5と関節Ｊ6とはアーム１７で連結されている。さらに、関節Ｊ6にはアーム１８が連結されており、アーム１８の終端部（Ｅｎｄｐｏｉｎｔ）には指部（図示せず）を有する。
【００７４】
図示は省略するが、関節Ｊ1〜Ｊ6にはそれぞれ、モーターと、減速機と、モーターの回転角度（つまり、関節の回転角度）を検出する角度センサーとしてのエンコーダーと、が備えられている。また、アーム１６には慣性センサーとしてのジャイロセンサー５０が取り付けられており、ジャイロセンサー５０の取り付け位置における角速度を検出する。
【００７５】
なお、ジャイロセンサー５０は、本実施例では、アーム１６に配設しているが、この位置には限定されない。しかし、取り付け可能であれば、Ｅｎｄｐｏｉｎｔに近い位置に配設されることが望ましい。これは、先端部のＥｎｄｐｏｉｎｔでの振動が基部１２付近よりも大きくなるためである。
【００７６】
６軸ロボット１００は、各モーターの駆動制御、エンコーダーによる角速度の算出、ジャイロセンサー５０による角速度検出、前述した各種演算処理、等を行う制御装置２０を備えている。そして、６軸ロボット１００は、アーム１３〜１８と、関節Ｊ1〜Ｊ6のモーター及び減速機と、によって相対的な運動をすることで、Ｅｎｄｐｏｉｎｔ（たとえば指部）において、所定の作業をさせる。
【００７７】
ここで、６軸ロボット１００におけるジャイロセンサー５０の角速度の算出方法について説明する。まず、ジャイロセンサー５０の取り付け位置における姿勢情報と、エンコーダー角速度（関節角度）を用いて、ジャイロセンサーの取り付け位置姿勢間ヤコビ行列を算出する。
【００７８】
【数１】
0005929224.tif 000002
【００７９】
続いて、ジャイロセンサー５０の取り付け位置姿勢間ヤコビ行列Ｊｓと、関節角度から算出した関節角速度を用いて、ジャイロセンサーの取り付け位置における座標系角速度を算出する。
【００８０】
【数２】
0005929224.tif 000003
【００８１】
数式１及び数式２による演算によって、モーター角速度（エンコーダーの角速度）から算出したジャイロセンサー５０の取り付け位置における座標系角速度と、ジャイロセンサー５０の出力角速度の差分から捩れ角速度を算出し、前述した３軸のロボット１０（スカラーロボット）の制御方法（図７、参照）と同様な感度補正を実行する。つまり、図７の捩れ角速度の算出（Ｓ２０）を上記数式１及び数式２を用いて行えばよい。
【００８２】
このようにすれば、６軸ロボット１００の場合にも、３軸のロボット１０と同様に、ジャイロセンサー５０の感度誤差を補正しながら動作させることから、ジャイロセンサー５０の個体差、温度変化による精度への影響、経年変化による精度低下、捩れによる動作精度への影響を排除し、動作精度を向上させることができる。
なお、上述した３軸ロボット、６軸ロボットでも同様に制御することができる。
（実施形態２）
【００８３】
次に、実施形態２に係るロボット、及びロボットの制御方法について説明する。前述した実施形態１では慣性センサーとしてジャイロセンサーを用いた場合について説明したが、実施形態２は、加速度センサーを用いていることを特徴とする。直動ロボットと６軸ロボットを例にあげ説明する。
【００８４】
まず、直動ロボットについて説明する。
図９は、実施形態２に係る直動ロボット２００の概略構成を例示する構成説明図である。ロボット２００は、Ｘ方向（図示Ｖｘ方向）に延在される固定レール２１０と、固定レール２１０に取付けられＸ方向（固定レール２１０に沿うＶｘ方向）に移動可能なＸ方向移動装置２２０と、Ｘ方向移動装置２２０に取付けられＹ方向（図示Ｖｘ方向）に移動可能なＹ方向移動装置２３０と、Ｙ方向移動装置２３０に取付けられＺ方向（図示Ｖｚ方向）に移動可能なＺ方向移動装置２４０とを有して構成されている。Ｚ方向移動装置２４０の先端部はワークを掴む把持部である。このＺ方向移動装置２４０には、慣性センサーとしての加速度センサー６０が配置されている。加速度センサー６０は、Ｘ方向、Ｙ方向、Ｚ方向それぞれの加速度・姿勢を検出することが可能な３軸加速度センサーである。
【００８５】
Ｘ方向移動装置２２０、Ｙ方向移動装置２３０、及びＺ方向移動装置２４０には、リニアアクチュエーターを有し、それぞれＸ方向、Ｙ方向、Ｚ方向の移動速度を検出する検出器を備えている。検出器としては直動系または回転系検出用のエンコーダー４０が備えられている（共に、図示は省略）。
【００８６】
続いて、実施形態２に係る制御装置１２０の構成について説明する。
図１０は、実施形態２に係る制御装置１２０の主たる構成を示す構成説明図である。制御装置１２０は、加速度センサー６０の検出値を取得し、速度とセンサー座標とを算出するセンサー座標速度演算部１２１と、エンコーダー４０により検出した速度とセンサー座標速度演算部１２１により算出した速度との差（歪み速度という）を算出する歪み速度演算部１２２と、加速度センサー６０の感度補正量を算出する感度補正量演算部１２３と、算出された感度補正量に基づき加速度センサー６０の感度を補正する感度演算部１２５と、ロボット２００が動作中であるか、静止中であるかを判断する動作判断部１２４と、を備えている。
【００８７】
次に、センサー座標速度演算部１２１の構成について説明する。
図１１は、センサー座標速度演算部１２１の構成を示す構成説明図である。センサー座標速度演算部１２１は、加速度センサー６０の検出値を用いて、ロボット２００の基準座標へ変換する基準座標への座標変換部１３０と、加速度センサー６０の検出値（加速度と移動方向）から重力加速度の影響を除去する重力加速度除去演算部１３１と、重力加速度を除去した加速度を速度に変換する積分演算部１３２と、センサー座標への座標変換部１３３と、から構成されている。ここで、センサー座標は、前述した実施形態１に記載のジャイロセンサー座標に相当する。
【００８８】
次に、歪み速度について説明する。エンコーダー４０の角度情報から算出したＸ方向、Ｙ方向、ｚ方向のそれぞれの直動速度をＶｍｘ、Ｖｍｙ、Ｖｍｚとし、加速度センサー６０によって検出したＸ方向加速度をａｘ、Ｙ方向加速度をａｙ、Ｚ方向加速度をａｚとする。この加速度センサー６０が検出した加速度から重力加速度成分を除去した値を積分すると、Ｘ方向速度Ｖｓｘ、Ｙ方向速度Ｖｓｙ、Ｚ方向速度Ｖｓｚが得られる。そこで、エンコーダー４０によって得られた速度（Ｖｍｘ、Ｖｍｙ、Ｖｍｚ）と、加速度センサー６０によって得られた速度（Ｖｓｘ、Ｖｓｙ、Ｖｓｚ）の差から歪み速度を算出する。
【００８９】
次に、実施形態２のロボット２００の制御方法について説明する。
ロボット２００の制御方法は、加速度センサー６０によって検出される加速度を用いて、センサー座標系の速度を演算する工程と、エンコーダー４０によって検出されるセンサー座標系の速度と、加速度を用いて算出されるセンサー座標系の速度と、の差から歪み速度を算出する工程と、歪み速度を用いて加速度センサー６０の感度補正量を算出する工程と、感度補正量を用いて加速度センサー６０の感度を補正する工程と、有する。図７に示す制御方法の主要工程を示すフローチャートの各工程において、回転系の「捩れ角速度算出（Ｓ２０）」を上述した算出方法で算出した直動系の「歪み速度算出」に置き換えて説明することができる。また、「回転方向の判定（Ｓ４０）」を「移動方向の判定」に置き換えればよい。
【００９０】
続いて、６軸ロボットについて説明する。
図１２は、実施形態２に係る６軸ロボット３００を模式的に示す構成説明図である。６軸ロボットの構成は、実施形態１における６軸ロボット１００（図８、参照）に対して、ジャイロセンサー５０と加速度センサー６０とを置き換えたこと以外は同じ構成であり、制御装置及びセンサー座標速度演算部の構成は上述した直動ロボットと同じなので詳細な説明は省略する。
【００９１】
６軸ロボット３００における速度の算出方法について説明する。まず、加速度センサー６０の取り付け位置における姿勢情報と、関節角度（エンコーダー４０による角度情報）を用いて、関節角度・取り付け位置姿勢間ヤコビ行列を算出する。
【００９２】
【数３】
0005929224.tif 000004
【００９３】
続いて、関節角度・センサー取り付け位置姿勢間ヤコビ行列ＪTと、関節角度から算出した関節角速度を用いて、加速度センサー６０の取り付け位置における座標系加速度を算出する。
【００９４】
【数４】
0005929224.tif 000005
【００９５】
数式３及び数式４による演算によって算出したセンサー取り付け位置における３軸のセンサー座標系加速度をそれぞれａｘ、ａｙ、ａｚで表す。そして加速度を積分することによって、センサー座標系の速度を算出する。
【００９６】
なお、加速度センサー６０の出力値はセンサー座標系の出力値となるため、一度ロボット基部を基準とする基準座標系（ロボット座標系）の加速度に変換する。その後、重力加速度成分を除去し、積分を行い基準座標系での速度を演算した後、センサー座標系での速度に変換する。
その後、エンコーダーから算出したセンサー座標系での速度と、加速度センサーから算出したセンサー座標系での速度との差分から歪み速度を算出する。
【００９７】
以降の感度補正の手順は、実施形態１（図７、参照）と同様に行えばよい。つまり、各工程のなかで、回転系の「捩れ角速度」を上述した算出方法で算出した直動系の「歪み速度」に置き換えて説明することができる。
【００９８】
以上説明した実施形態２のロボット制御方法によれば、加速度センサー６０が検出する速度と、エンコーダー４０が検出する情報から得られる加速度センサー座標における速度と、の差を用いて加速度センサー６０の感度補正量を決定する。この速度差が歪み速度である。こうして算出した感度補正量を用いて加速度センサー６０の感度を補正し、直動ロボット２００、または６軸ロボット３００を動作させる。このことによって、加速度センサー６０の個体差、温度変化による精度への影響、経年変化による精度低下への影響を排除し、ロボット動作の精度を向上させることができる。
【００９９】
また、直動ロボット２００、または６軸ロボット３００を動作させながら加速度センサー６０の感度を更新していくので、繰り返し動作を実行する場合に、１動作毎に精度を向上させていくことができるという効果がある。
【０１００】
また、実施形態２のロボット制御方法では、加速度センサー６０の感度誤差の補正量を算出する工程は、ロボット２００，３００の動作中に実行する。直動ロボット２００または６軸ロボット３００が静止時に感度誤差の算出を実行すると、ノイズ成分が累積されて感度誤差が実際と異なる値になることが考えられる。よって、動作しているときのみに、感度誤差の補正量を算出することにより、ノイズの影響を抑制し、高精度の感度補正量を得ることができる。
【０１０１】
加速度センサー６０の感度誤差の補正量を算出する工程は、直動ロボット２００または６軸ロボット３００の動作中の所与の時間間隔で捩れ角速度を算出し、動作終了後に前述の時間間隔で算出した歪み速度を統合して感度誤差の補正量を算出する。このようにすれば、歪み速度の変化をほぼリアルタイムで取得することができること、補正量は１動作終了毎に算出すれば演算工程の計算負荷を低減することができる。
【０１０２】
また、加速度センサー６０の感度誤差の補正量を算出する工程は、加速度センサー６０の取付け位置の移動方向を検出した後、移動方向に合わせて歪速度の値の正負を切り替えている。歪み速度の算出では直接的な移動方向の情報がない。そこで、加速度センサー６０の取付け位置の移動方向によって算出した歪み速度の値を正または負に切り替える。そのことによって、加速度センサー６０の正逆移動方向両方の感度補正を行うことができる。
【０１０３】
また、捩れ角速度を算出する工程（Ｓ２０）は、加速度センサー６０のオフセット除去後に実行することによって、オフセット誤差の影響を除去し、高精度の感度誤差の補正を行うことができる。
【０１０４】
加速度センサー６０の感度誤差の補正量は、直動ロボット２００または６軸ロボット３００の動作速度の大小に対応する補正定数を用いて算出する。補正定数は感度補正量に感度補正ゲインを乗じた値である。従って、動作速度が大きい場合には補正定数を大きくして感度補正量を大きくし、動作速度が小さい場合には補正定数を小さくすることで感度補正量を小さくすることができる。
【０１０５】
また、以上説明した実施形態２によれば、歪み速度を算出した感度補正量を用いて加速度センサー６０の感度を補正し、直動ロボット２００または６軸ロボット３００を動作させる。このことによって、加速度センサー６０の個体差、温度変化による精度への影響、経年変化による精度低下への影響を低減し、ロボット動作の精度を向上させることができる。
【符号の説明】
【０１０６】
１０…ロボット（３軸）、２０…制御装置、４１，５１…モーター、４２…第１エンコーダー、４４，４９…アーム、４８…第２エンコーダー、５０…ジャイロセンサー。
【図１】
0005929224.tif 000006
【図２】
0005929224.tif 000007
【図３】
0005929224.tif 000008
【図４】
0005929224.tif 000009
【図５】
0005929224.tif 000010
【図６】
0005929224.tif 000011
【図７】
0005929224.tif 000012
【図８】
0005929224.tif 000013
【図９】
0005929224.tif 000014
【図１０】
0005929224.tif 000015
【図１１】
0005929224.tif 000016
【図１２】
0005929224.tif 000017