特許 6174654 センサの位置と向きを算出する機能を備えたロボットシステム

(19)【発行国】日本国特許庁(JP)

(12)【公報種別】特許公報(B2)

(11)【特許番号】6174654

(24)【登録日】2017年7月14日

(45)【発行日】2017年8月2日

(54)【発明の名称】センサの位置と向きを算出する機能を備えたロボットシステム

(51)【国際特許分類】

   B25J 19/02 20060101AFI20170724BHJP

【ＦＩ】

   B25J19/02

【請求項の数】6

【全頁数】17

(21)【出願番号】特願2015-203902(P2015-203902)

(22)【出願日】2015年10月15日

(65)【公開番号】特開2017-74647(P2017-74647A)

(43)【公開日】2017年4月20日

【審査請求日】2016年9月14日

【早期審査対象出願】

(73)【特許権者】

【識別番号】390008235

【氏名又は名称】ファナック株式会社

(74)【代理人】

【識別番号】100099759

【弁理士】

【氏名又は名称】青木 篤

(74)【代理人】

【識別番号】100102819

【弁理士】

【氏名又は名称】島田 哲郎

(74)【代理人】

【識別番号】100123582

【弁理士】

【氏名又は名称】三橋 真二

(74)【代理人】

【識別番号】100112357

【弁理士】

【氏名又は名称】廣瀬 繁樹

(74)【代理人】

【識別番号】100130133

【弁理士】

【氏名又は名称】曽根 太樹

(74)【代理人】

【識別番号】100159684

【弁理士】

【氏名又は名称】田原 正宏

(72)【発明者】

【氏名】山岡 宣章

(72)【発明者】

【氏名】陳 文杰

(72)【発明者】

【氏名】王 凱濛

【審査官】 貞光 大樹

(56)【参考文献】

【文献】 特開２０１３−０４１４７８（ＪＰ，Ａ）

【文献】 特開２０１１−２３８０９７（ＪＰ，Ａ）

【文献】 特開２０１１−１６７８１７（ＪＰ，Ａ）

【文献】 特開２０１１−２３０２４５（ＪＰ，Ａ）

(58)【調査した分野】（Int.Cl.，ＤＢ名）

Ｂ２５Ｊ １／００−２１／０２

Ｇ０５Ｂ １９／１８−１９／４１６；１９／４２−１９／４６

(57)【特許請求の範囲】

【請求項1】

少なくとも振動を監視するセンサ（１３）が取付けられた可動部を有するロボット（１１）と、前記ロボット（１１）の動作を制御する制御装置（１２）とを具備するロボットシステム（１０）であって、
前記制御装置（１２）は、所定の動作を前記ロボット（１１）に実行させることにより、前記センサ（１３）の位置と向きを算出するセンサ座標系計算部（１７）を備え、
前記センサ座標系計算部（１７）は、所定の種類の動作パラメータの値をそれぞれ変更した複数の組合せの各々を順次用いて前記所定の動作を前記ロボット（１１）に実行させることにより、前記複数の組合せの中から、前記センサ（１３）の位置と向きを算出するのに最適な組合せを求める動作パラメータ最適化部（１８）を備え、
前記動作パラメータ最適化部（１８）は、
各々の前記組合せによって前記所定の動作を実行させたときに前記センサ（１３）から出力された、前記振動を表す物理量のデータに基づいて、前記センサ（１３）の位置と向きを算出し、算出された前記センサ（１３）の位置および向きと前記所定の動作中の前記ロボット（１１）の位置および姿勢とに基づき、前記センサ（１３）上に働く前記物理量を計算し、
前記センサ（１３）から出力された前記データと、前記センサ（１３）上に働く前記物理量の計算値とに基づいて、前記複数の組合せのうちのどれが、前記センサ（１３）の位置と向きの算出結果の精度が最も高くなる組合せであるかを判定するようにした、ロボットシステム。

【請求項2】

少なくとも振動を監視するセンサ（１３）が取付けられた可動部を有するロボット（１１）と、前記ロボット（１１）の動作を制御する制御装置（１２）とを具備するロボットシステム（１０）であって、
前記制御装置（１２）は、所定の動作を前記ロボット（１１）に実行させることにより、前記センサ（１３）の位置と向きを算出するセンサ座標系計算部（１７）を備え、
前記センサ座標系計算部（１７）は、所定の種類の動作パラメータの値をそれぞれ変更した複数の組合せの各々を順次用いて前記所定の動作を前記ロボット（１１）に実行させることにより、前記複数の組合せの中から、前記センサ（１３）の位置と向きを算出するのに最適な組合せを求める動作パラメータ最適化部（１８）を備え、
前記動作パラメータ最適化部（１８）は、
各々の前記組合せによって前記所定の動作を実行させたときに前記センサ（１３）から出力された、前記振動を表す物理量のデータに基づいて、前記センサ（１３）の位置と向きを算出し、算出された前記センサ（１３）の位置および向きと前記所定の動作中の前記ロボット（１１）の位置および姿勢とに基づき、前記センサ（１３）上に働く前記物理量を計算し、
前記センサ（１３）から出力された前記データと、前記センサ（１３）上に働く前記物理量の計算値とに基づいて、前記複数の組合せのうちのどれが、前記センサ（１３）の位置と向きの算出結果の精度が最も高くなる組合せであるかを判定し、
前記センサ（１３）の位置と向きの算出精度が十分であるか否かを判定し、前記算出精度が不十分であると判定した場合にはその旨を外部に通知するようにした、ロボットシステム。

【請求項3】

前記所定の種類の動作パラメータは、前記ロボット（１１）に対して前記制御装置（１２）が指令する速度および加速度である、請求項１または２に記載のロボットシステム。

【請求項4】

前記所定の動作は、前記可動部に定義された３次元直交座標系におけるＸ軸、Ｙ軸、Ｚ軸のうちの少なくとも２つの軸方向の並進動作と、Ｘ軸回り、Ｙ軸回り、Ｚ軸回りのうちの少なくとも２つの軸回りの回転動作とからなる、請求項１から３のいずれか一項に記載のロボットシステム。

【請求項5】

前記センサ（１３）は、加速度センサ、ジャイロセンサ、慣性センサ、力センサ、レーザトラッカ、またはモーションキャプチャ装置である、請求項１から４のいずれか一項に記載のロボットシステム。

【請求項6】

前記物理量は、力、変位、速度、または加速度である、請求項１から５のいずれか一項に記載のロボットシステム。

【発明の詳細な説明】

【技術分野】

【0001】

本発明は、学習制御機能を備えたロボットシステムに関し、特にロボットの可動部に取付けられたセンサの位置と向きを算出するロボットシステムに関する。

【背景技術】

【0002】

ロボットにおいては、サーボモータにより駆動されるアーム部の位置や速度を指令通りに制御するために、通常、位置フィードバック制御、速度フィードバック制御、さらには電流フィードバック制御が行われている。しかし、そのようなフィードバック制御を行っても、動作中のアーム部に振動が発生する場合がある。

【0003】

そこで、従前より、加速度センサをロボットの可動部、例えばアーム部の先端に取付け、アーム部の動作中の振動を加速度センサにより計測し、学習制御を行うことにより当該振動を低減させることが行われている（特許文献１参照）。

【0004】

具体的には、外部制御装置から指令されたアーム部の先端の位置指令データと、当該アーム部を駆動するサーボモータに設置された位置検出器からの実位置データとの差（すなわち位置偏差）に基づいて、アーム部の先端を移動させる。このような移動の間、アーム部の先端に取付けられた加速度センサの計測結果に基づいてアーム部の先端の位置データを取得し、この位置データと前述の位置指令データとから、振動誤差に相当する補正量を求めている。そして、この補正量を考慮して前述の位置偏差を補正している。なお、前述の学習制御は、このような一連のプロセスを繰返し実行することにより前述の振動を低減させる制御のことである。

【0005】

上述のような学習制御を行うためには、加速度センサをロボットのアーム部の先端に取付けた後、取付けられた加速度センサの位置と向きを求めるキャリブレーションを実施しておく必要がある。そこで、特許文献２は、所定の動作をロボットに実行させて、アーム部の先端に在る加速度センサの位置と向きを自動的に算出する方法を開示している。

【0006】

具体的には、特許文献２に開示されたロボットにおいては、加速度センサをアーム部の先端に取付け、当該アーム部の先端を特定の座標系のＸ軸方向とＹ軸方向の各々に並進動作させて加速度センサの向きを推定する。さらに、アーム部の先端側に在る２つの関節軸をそれぞれ回転動作させて加速度センサの位置を推定している。

【先行技術文献】

【特許文献】

【0007】

【特許文献1】特許第４２８３２１４号公報

【特許文献2】特開２０１３−０４１４７８号公報

【発明の概要】

【発明が解決しようとする課題】

【0008】

しかしながら、特許文献２に開示されたロボットにおいては、アーム部の先端に取付けられているツールの重量が大きいほど、さらにはアーム部の剛性が低いほど、ロボットの所定の動作中に加速度センサに加わる外乱が大きくなる。その結果、加速度センサの位置と向きを精度よく算出できないことがある。

【0009】

算出誤差が大きくなると、前述した学習制御による振動の低減効果が低くなり、場合によっては振動が発散してしまう危険性もある。さらに、特許文献２に開示された発明においては、加速度センサの位置と向きを自動的に算出した後に、算出結果に十分な精度があるかどうかを人の作業によって確認する必要がある。

【0010】

そこで本発明は、上述したような問題点に鑑み、ロボットの可動部に取付けられたセンサの位置と向きの算出結果の精度を向上させることができ、算出精度を確認する人の作業が不要となるロボットシステムを提供することを目的とする。

【課題を解決するための手段】

【0011】

本発明の第一態様によれば、少なくとも振動を監視するセンサが取付けられた可動部を有するロボットと、ロボットの動作を制御する制御装置とを具備するロボットシステムであって、
制御装置は、所定の動作をロボットに実行させることにより、センサの位置と向きを算出するセンサ座標系計算部を備え、
センサ座標系計算部は、所定の種類の動作パラメータの値をそれぞれ変更した複数の組合せの各々を順次用いて前記所定の動作をロボットに実行させることにより、複数の組合せの中から、センサの位置と向きを算出するのに最適な組合せを求める動作パラメータ最適化部を備える、ロボットシステムが提供される。

【0012】

本発明の第二態様によれば、上記の第一態様のロボットシステムであって、
上記の所定の種類の動作パラメータは、ロボットに対して制御装置が指令する速度および加速度である、ロボットシステムが提供される。

【0013】

本発明の第三態様によれば、上記の第一態様または第二態様のロボットシステムであって、上記の動作パラメータ最適化部は、
各々の組合せによって所定の動作を実行させたときにセンサから出力されたデータに基づいてセンサの位置と向きを算出し、算出されたセンサの位置および向きと所定の動作中のロボットの位置および姿勢とに基づき、センサ上に働く物理量を計算し、
センサから出力されたデータと、センサ上に働く物理量の計算値とに基づいて、複数の組合せのうちのどれが、センサの位置と向きの算出結果の精度が最も高くなる組合せであるかを判定するようにした、ロボットシステムが提供される。

【0014】

本発明の第四態様によれば、上記の第一態様から第三態様のいずれかのロボットシステムであって、
上記の動作パラメータ最適化部は、
各々の組合せによって所定の動作を実行させたときにセンサから出力されたデータに基づいてセンサの位置と向きを算出し、算出されたセンサの位置および向きと所定の動作中のロボットの位置および姿勢とに基づき、センサ上に働く物理量を計算し、
センサから出力されたデータと、センサ上に働く物理量の計算値とに基づいて、複数の組合せのうちのどれが、センサの位置と向きの算出結果の精度が最も高くなる組合せであるかを判定し、
センサの位置と向きの算出精度が十分であるか否かを判定し、その算出精度が不十分であると判定した場合にはその旨を外部に通知するようにした、ロボットシステムが提供される。

【0015】

本発明の第五態様によれば、上記の第一態様から第四態様のいずれかのロボットシステムであって、
上記の所定の動作は、可動部に定義された３次元直交座標系におけるＸ軸、Ｙ軸、Ｚ軸のうちの少なくとも２つの軸方向の並進動作と、Ｘ軸回り、Ｙ軸回り、Ｚ軸回りのうちの少なくとも２つの軸回りの回転動作とからなる、ロボットシステムが提供される。

【0016】

本発明の第六態様によれば、上記の第一態様から第五態様のいずれかのロボットシステムであって、
上記のセンサは、加速度センサ、ジャイロセンサ、慣性センサ、力センサ、レーザトラッカ、カメラ、またはモーションキャプチャ装置である、ロボットシステムが提供される。

【発明の効果】

【0017】

本発明によれば、所定の種類の動作パラメータの値をそれぞれ変更した複数の組合せを順次用いて、所定の動作をロボットに実行させることにより、センサの位置と向きを前述の組合せ毎に算出するようにしている。このことにより、上記の複数の組合せの中から、センサに加わる外乱の影響が最小になる動作パラメータの組合せを求めることができるため、センサの位置と向きの算出結果の精度が向上することとなる。その結果、前述した学習制御による振動の低減効果を良好に得ることができる。

【0018】

また、本発明によれば、センサの位置と向きの算出結果の精度が十分であるかどうかを自動的に判定することができるため、算出精度を確認する人の作業が不要になる。

【0019】

添付図面に示される本発明の典型的な実施形態の詳細な説明から、本発明のこれらの目的、特徴および利点ならびに他の目的、特徴および利点がさらに明確になるであろう。

【図面の簡単な説明】

【0020】

【図1】本発明の一実施形態のロボットシステムの構成を示したブロック図である。

【図2】図１に示された多関節ロボットの構成要素と座標系とを説明するための図である。

【図3】図１に示されたセンサ座標系計算部と動作パラメータ最適化部によって行われる処理を示すフローチャートである。

【図4】加速度センサがメカニカルインタフェイス座標系のＸ軸回りに１５度回転したときの態様を模式的に示した図である。

【図5】図４に示された態様をメカニカルインタフェイス座標系のＹＺ面で表した図である。

【図6】所定の動作中における加速度の時間変化の一例を示すグラフである。

【図7】加速度センサ上に働く加速度の算出値と加速度センサの出力値との差の時間変化の一例を示すグラフである。

【発明を実施するための形態】

【0021】

次に、本発明の実施形態について図面を参照して説明する。以下の図面において、同じ部材には同じ参照符号が付けられている。そして、異なる図面において同じ参照符号が付されたものは同じ機能を有する構成要素であることを意味するものとする。また、理解を容易にするために、これらの図面は縮尺を適宜変更している。

【0022】

図１は、本発明の一実施形態のロボットシステムの構成を示したブロック図である。図２は、図１に示された多関節ロボットの構成要素と座標系とを説明するための図である。

【0023】

図１に示されるように、本実施形態のロボットシステム１０は、ロボット１１と、ロボット１１を制御するロボット制御装置１２とを備える。

【0024】

ロボット１１は例えば垂直多関節ロボットであり、多関節ロボットの位置制御の対象となる可動部の振動を計測するために当該可動部に加速度センサ１３が取付けられている。本実施形態においては、図１に示されるようにロボット１１のアーム部１１ａの先端部にツール１４がブラケット１５を介して装着されていて、そのツール１４に加速度センサ１３が取付けられている。

【0025】

また、ツール１４としてはハンド部、ボルト締め具、溶接ガンなどを使用することができる。さらに、加速度センサ１３としては、３軸加速度センサを使用することができる。

【0026】

さらに、ロボット１１とロボット制御装置１２とがケーブル１６を介して互いに接続されている。加速度センサ１３もまた、ケーブル（図示せず）によってロボット制御装置１２と接続されている。

【0027】

また、前述したロボット１１は、図２に示されているように、六つの関節軸１１ｂ１〜１１ｂ６と、関節軸１１ｂ１〜１１ｂ６の各々を回転駆動するサーボモータ（図示せず）とを具備している。各サーボモータは、ロボット制御装置１２からの動作指令により制御される。

【0028】

さらに、ロボット１１は、空間上に固定されたワールド座標系２１と、ツール１４が取付けられるアーム部１１ａの先端部に設定されたメカニカルインタフェイス座標系２２とを備える典型的なロボットである。ワールド座標系２１とメカニカルインタフェイス座標系２２とはそれぞれ３次元直交座標系である。

【0029】

なお、ワールド座標系２１におけるＸＹＺ軸の方向、およびメカニカルインタフェイス座標系２２におけるＸＹＺ軸の方向は、図２中に拡大図によって示されている。ワールド座標系２１においては、図２の右方向に＋Ｘ軸、図２の上方向に＋Ｚ軸、図２の紙面に対して奥行方向に＋Ｙ軸をそれぞれ定義している。一方、メカニカルインタフェイス座標系２２においては、図２の右方向に＋Ｘ軸、図２の下方向に＋Ｚ軸、図２の紙面に対して手前方向に＋Ｙ軸をそれぞれ定義している。

【0030】

さらに、本実施形態のロボットシステム１０においても、図１に示されるようなアーム部１１ａの先端のツール１４に取付けられた加速度センサ１３によってロボット１１の動作中の振動を計測し、学習制御を行うことにより当該振動を低減させるようにする。そして、そのような学習制御を行うためには、ツール１４に取付けられた加速度センサ１３の位置と向きを求めるキャリブレーションを実施しておく必要がある。

【0031】

そこで、本実施形態のロボット制御装置１２は、所定の動作をロボット１１に実行させることにより、前述のツール１４に取付けられた加速度センサ１３の位置と向きを自動的に算出する機能を備える。

【0032】

具体的には、ロボット制御装置１２は、図１に示されるようにセンサ座標系計算部１７を備える。センサ座標系計算部１７は、所定の動作をロボット１１に実行させて、ツール１４に取付けられた加速度センサ１３の位置と向きを算出する。

【0033】

より具体的には、前述のセンサ座標系計算部１７は、図１に示されるように動作パラメータ最適化部１８を備える。動作パラメータ最適化部１８は、所定の種類の動作パラメータの値をそれぞれ変更した複数の組合せ（以下、動作パラメータセットと呼ぶ。）の各々を順次用いて、前述の所定の動作をロボット１１に実行させる。それにより、動作パラメータ最適化部１８は、加速度センサ１３の位置と向きを算出するのに最適な動作パラメータセットを求める。なお、前述の動作パラメータセットの具体例については後述する。

【0034】

前述の動作パラメータ最適化部１８によりロボット１１に実行させる所定の動作は、次のような動作とする。

【0035】

すなわち、本実施形態においては、ロボット１１の任意の姿勢を基準として、加速度センサ１３が取付けられたツール１４をメカニカルインタフェイス座標系２２のＸ軸、Ｙ軸およびＺ軸の方向にそれぞれ並進動作させる。各並進動作での移動量は１００ｍｍとする。その後、加速度センサ１３が取付けられたツール１４をメカニカルインタフェイス座標系２２のＸ軸回り、Ｙ軸回りおよびＺ軸回りの方向にそれぞれ回転動作させる。各回転動作での回転角度は１５度とする。

【0036】

前述した所定の動作をロボット１１に実行させるために、Ｋ個（Ｋは自然数）の動作パラメータセットがロボット制御装置１２に予め記憶されている。これらの動作パラメータセットはロボット制御装置１２外から書換えられることが好ましい。

【0037】

本実施形態においては、各動作パラメータセットを構成している所定の種類の動作パラメータは、ロボット１１に対してロボット制御装置１２が指令する速度および加速度である。また、本実施形態においては、各々の動作パラメータセットは、速度と加速度の値をそれぞれ変更した組合せとする。なお、各々の動作パラメータセットにおける速度と加速度の値はそれぞれロボット１１の最大速度と最大加速度に対する割合によって定められている。例えば、速度と加速度の組合せ（速度［％］、加速度［％］）としては、（３０、１００）、（５０、１００）、（７０、１００）、（８０、８０）、（１００、５０）が事前に用意されている。つまり、前述の１００ｍｍの並進動作を行う場合、移動体を始点から所定の割合の加速度で増速移動させてから、所定の割合の速度で定速動作させ、その後所定の割合の加速度で減速動作して終点に到達させるようにする。

【0038】

次に、前述したセンサ座標系計算部１７および動作パラメータ最適化部１８によって行われる処理について具体的に説明する。

【0039】

図３は、前述したセンサ座標系計算部１７および動作パラメータ最適化部１８によって行われる処理を示すフローチャートである。

【0040】

動作パラメータ最適化部１８は、複数の動作パラメータセット、例えば上述のような５個の動作パラメータセットの各々を用いて、前述した所定の動作をロボット１１に実行させる。本実施形態においては、図３に示されるステップＳ１〜ステップＳ５から分かるように、Ｋ個（本例の場合Ｋ＝５）の動作パラメータセットを順番に用いて前述の所定の動作を行う。

【0041】

さらに、動作パラメータ最適化部１８は、各動作パラメータセットを用いて前述の所定の動作を行う度に、その時の動作データから加速度センサ１３の位置と向きを算出する（ステップＳ２およびステップＳ３）。そのような算出方法は以下の通りである。

【0042】

初めに加速度センサ１３の向きを算出し、それから加速度センサ１３の位置を算出する。

【0043】

まず、加速度センサ１３の向きの算出においては、前述の所定の動作を開始する前の加速度データと、前述の所定の動作のうちの並進動作の際に得られる加速度データを使用する。前述の所定の動作を開始する前の１秒間の加速度データを取得して、その平均値を記録する。並進動作については、メカニカルインタフェイス座標系２２のＸ軸、Ｙ軸およびＺ軸のそれぞれの方向へツール１４を並進動作させ、各々の並進動作中の加速度データを一定時間毎に順次記録する。但し、この記録される加速度データは、加速度センサ１３により検出された生の加速度データから前述の所定の動作を開始する前の加速度データの平均値を差引き、さらにローパスフィルタによってノイズを除去した後のデータとする。後述の２回積分によって加速度データ上のノイズが増大してしまうのを防ぐためである。なお、当該データは、ロボット制御装置１２内に設けられた記憶部（図示せず）または外部記憶装置などに記録することが好ましい。

【0044】

そして、前述のように一定時間毎に記録された時系列の加速度データを用いて各時刻の加速度データを２回積分し、それらの積分結果を合計することにより、並進動作したときの加速度センサ１３の３次元の移動量を求める。このような加速度センサ１３の３次元の移動量を、メカニカルインタフェイス座標系２２のＸ軸、Ｙ軸およびＺ軸のそれぞれの方向の並進動作ごとに求める。

【0045】

続いて、前述したＸ軸方向の並進動作について、加速度センサ１３の移動方向を表す単位ベクトル

【数1】

を計算する。その計算式は次式（１）とする。

【数2】

【0046】

なお、式（１）中の（ｘ_xi，ｙ_xi，ｚ_xi）は、前述のように加速度データの２回積分によって求められた加速度センサ１３の３次元の移動量を座標値として表したものである。また、各座標値の添え字のｘはＸ軸方向の並進動作であることを表す。さらに添え字のｉは、Ｋ個の動作パラメータセットのうちのｉ番目（ｉ←１〜Ｋ）の動作パラメータセットを用いた場合であることを表す。なお、上記の式（１）から分かるように（ｘ_xi，ｙ_xi，ｚ_xi）の絶対値が分母にあるため、ベクトル

【数3】

は規格化されている。

【0047】

さらに、前述したＹ軸方向の並進動作およびＺ軸方向の並進動作についても、それぞれ、加速度センサ１３の移動方向を表す単位ベクトル

【数4】

を計算する。それらの計算式は、次の式（２）および式（３）とする。

【0048】

【数5】

【0049】

そして、上記の式（１）〜式（３）によって得られる３つの単位ベクトルから、加速度センサ１３の向きを表す回転行列Ｒは以下の式（４）のように求められる。

【数6】

【0050】

なお、前述のように回転行列ＲをＸＹＺ軸の３つの軸方向の並進動作から求めているが、３つの軸方向のうちの少なくとも２つの軸方向の並進動作から求めることもできる。

【0051】

【数7】

【0052】

つまり、上記の式（５）のように、

【数8】

と

【数9】

の外積から

【数10】

を求めることができる。このため、本実施形態においては、前述の回転行列Ｒを２つの軸方向の並進動作から求めてもよい。

【0053】

以上のようにして加速度センサ１３の向きが算出される。勿論、このようなセンサ向きの算出は前述したＫ個の動作パラメータセットの全てに対して実施される。

【0054】

次に、加速度センサ１３の位置を算出する。
加速度センサ１３の位置の算出においては、前述の所定の動作のうちの回転動作の際に得られる加速度データを使用する。そのため、メカニカルインタフェイス座標系２２のＸ軸回り、Ｙ軸回りおよびＺ軸回りのそれぞれの方向へのツール１４の回転動作について、各々の回転動作中の加速度データを一定時間毎に順次記録する。但し、この記録される加速度データは、加速度センサ１３により検出された生の加速度データから重力加速度を、上記の式（４）により求められた加速度センサ１３の向きを考慮して差引いた後のデータとする。なお、当該データは、ロボット制御装置１２に設けられた記憶部（図示せず）または外部記憶装置などに記録することが好ましい。

【0055】

そして、前述のように一定時間毎に記録された時系列の加速度データを用いて各時刻の加速度データを２回積分し、それらの積分結果を合計することにより、回転動作したときの加速度センサ１３の回転変位量を求める。

【0056】

このような加速度センサ１３の回転変位量を、メカニカルインタフェイス座標系２２のＸ軸回り、Ｙ軸回りおよびＺ軸回りのそれぞれの方向の回転動作ごとに求める。

【0057】

そして、Ｘ軸回りの加速度センサ１３の回転変位量に基づいて加速度センサ１３の位置のＹ成分とＺ成分を求める。また、Ｙ軸回りの加速度センサ１３の回転変位量に基づいて加速度センサ１３の位置のＸ成分とＺ成分を求める。さらに、Ｚ軸回りの加速度センサ１３の回転変位量に基づいて加速度センサ１３の位置のＸ成分とＹ成分を求める。

【0058】

ここで、代表して、前述したＸ軸回りの回転動作の場合について詳述する。図４は、加速度センサ１３がメカニカルインタフェイス座標系２２のＸ軸回りに１５度回転したときの態様を模式的に示した図である。図４において、符号Ａが回転前の加速度センサ１３の位置を指しており、符号Ｂが回転後の加速度センサ１３の位置を指している。また、図５は、図４に示された態様をメカニカルインタフェイス座標系２２のＹＺ面によって示した図である。

【0059】

図５に示されるように、メカニカルインタフェイス座標系での加速度センサ１３の位置Ａを（Ｙ₀，Ｚ₀）として、加速度センサ１３をＸ軸回りに１５度［ｄｅｇ］回転させたとき、加速度センサ１３が（ｄＹ，ｄＺ）の変位で位置Ｂに移動したとする。この場合、位置Ａを表した座標値（Ｙ₀，Ｚ₀）と、変位を表した座標値（ｄＹ，ｄＺ）との間には、以下の関係式が成り立つ。

【0060】

【数11】

【0061】

上記の式（６）中の（ｄＹ，ｄＺ）は、前述したように加速度データの２回積分によって求められたＸ軸回りの加速度センサ１３の回転変位量であり、座標値として表したものである。したがって、既に求められているｄＹとｄＺのそれぞれの値を上記の式（６）に代入することにより、（Ｙ₀，Ｚ₀）の座標値が算出される。以上のように、前述したＸ軸回りの回転動作により加速度センサ１３の位置のＹ成分とＺ成分を求めることができる。

【0062】

また、このような算出方法により、前述したＹ軸回りの回転動作およびＺ軸回りの回転動作についても、それぞれ、回転前の加速度センサ１３の２次元の位置を求めることができる。つまり、前述したＹ軸回りの回転動作により加速度センサ１３の位置のＸ成分とＺ成分を求めることができる。さらに、前述したＺ軸回りの加速度センサ１３の回転動作により加速度センサ１３の位置のＸ成分とＹ成分を求めることができる。

【0063】

以上のように、３つの軸回りの回転動作により加速度センサ１３の位置のＸ成分、Ｙ成分、およびＺ成分がそれぞれ２個ずつ求められる。本実施形態においては、２個のＸ成分、２個のＹ成分、および２個のＺ成分をそれぞれ平均化することにより、加速度センサ１３の３次元の位置を算出することとする。

【0064】

なお、上述したとおり、加速度センサ１３の３次元の位置をＸＹＺ軸の３つの軸回りの回転動作から求めているが、３つの軸回りのうちの少なくとも２つの軸回りの回転動作から求めることもできる。

【0065】

つまり、前述のＸ軸回りの回転動作により加速度センサ１３の位置のＹ成分とＺ成分が求まり、前述のＹ軸回りの回転動作により加速度センサ１３の位置のＸ成分とＺ成分が求まる。したがって、加速度センサ１３のＺ成分の算出位置については、Ｘ軸回りとＹ軸回りの回転動作から求まる２個のＺ成分を平均化して求めることができる。そして、加速度センサ１３のＸ成分の算出位置については前述のＹ軸回りの回転動作から求まるＸ成分とし、加速度センサ１３のＹ成分の算出位置については前述のＸ軸回りの回転動作から求まるＹ成分とすることができる。

【0066】

再び図３を参照すると、前述のように加速度センサ１３の位置と向きを算出したステップＳ３の後、ステップＳ４において、番号ｉを一つ繰上げる。なお、本実施形態においては、Ｋ個（本例の場合は５個）の動作パラメータセットには連続番号が順次付けられており、前述の番号ｉに対応する連続番号のパラメータセットが使用されることとなる。

【0067】

さらに、ステップＳ５において、前述したＫ個の動作パラメータセットの全てについて加速度センサ１３の向きと位置の算出が終了したと判断した場合、ステップＳ６に移行する。ステップＳ６においては、前述の５個の動作パラメータセットのうちのどれが、加速度センサ１３の位置と向きを求めるのに最適な動作パラメータセットであるのかを判定する。

【0068】

具体的には、動作パラメータ最適化部１８は、以下のように最適な動作パラメータセットの判定を行う。

【0069】

まず、所定の動作パラメータセットを用いて算出された加速度センサ１３の位置と向きと、前述した所定の動作中のロボット１１の位置および姿勢とに基づき、一定時間毎に加速度センサ１３上に働く物理量、すなわち加速度

【数12】

の絶対値を順次計算する。ここで、絶対値とはＸＹＺ３成分の二乗和の平方根である。

【0070】

具体的には、上記の加速度は、算出された加速度センサ１３の位置と向きを基準として、前述した所定の動作における一定時間毎のロボット１１のワールド座標系での位置および姿勢から、各時刻の加速度センサ１３の位置を順次求め、２回微分することにより算出される。なお、各加速度を算出する時刻については、前述の所定の動作を実行した際に加速度センサ１３より取得した加速度データの記録時刻と同期させている。

【0071】

このような加速度の算出は、前述した５個の動作パラメータセットの各々の場合について行われる。

【0072】

続いて、各々の動作パラメータセットについて、前述のように算出された加速度

【数13】

の標準偏差Ｓを順次求める。

【数14】

【0073】

また、前述した加速度の標準偏差Ｓは、以下のように、前述の所定の動作中における加速度センサ１３の出力値

【数15】

から求めても良い。

【数16】

【0074】

あるいは、前述した加速度の標準偏差Ｓは、以下のように、最大値と最小値の差としても良い。

【数17】

【0075】

上式のｍａｘの項は各時刻の加速度のうちの最大値、ｍｉｎの項は各時刻の加速度のうちの最小値を表している。

【0076】

なお、図６は、前述した加速度

【数18】

のそれぞれの時間変化の一例を表している。

【0077】

次に、前述のように算出された加速度

【数19】

の絶対値、例えば図６の曲線Ｐにより示される加速度データと、前述の所定の動作中における加速度センサ１３の出力値

【数20】

の絶対値、例えば図６の曲線Ｑにより示される加速度データとの差のノルムＮを以下のように求める。

【数21】

上式のｎは時系列のデータ数、すなわち一定時間毎に記録および算出された加速度データの数を表している。

【0078】

なお、図７は、前述した加速度

【数22】

の絶対値と加速度

【数23】

の絶対値との差の時間変化の一例を表している。

【0079】

ここで、前述した加速度の標準偏差Ｓが大きいほど、加速度センサ１３の位置と向きの算出に使用される加速度データ（図６参照）のばらつきが大きくなる。このことは、加速度センサ１３の位置と向きを求めるための所定の動作が高速であることを意味するため、加速度センサ１３の位置と向きを精度良く求められる条件の一つとなる。

【0080】

また、前述したノルムＮが小さいほど、前述のように算出された加速度

【数24】

の絶対値と、前述の所定の動作中の加速度センサ１３の出力値

【数25】

の絶対値との差（図７参照）が小さくなる。つまり、前述の所定の動作中に加速度センサ１３に加わる外乱の影響が小さくなる。このため、前述のノルムＮが小さいことも、加速度センサ１３の位置と向きを精度良く求められる条件の一つとなる。

【0081】

以上の事より、前述の標準偏差Ｓが大きく且つ前述のノルムＮが小さい条件が、加速度センサ１３の位置と向きを精度良く算出できる条件である。このため、本実施形態においては、上記のＳとＮの比であるＳ／Ｎの値を、前述した５個の動作パラメータセットの各々の場合について順次算出することとする。

【0082】

そして、Ｓ／Ｎ値が大きいほど加速度センサ１３の位置と向きの算出精度が高いことを意味するため、動作パラメータ最適化部１８は、Ｓ／Ｎ値の最も大きい動作パラメータセットを最適な動作パラメータセットと判定する（図３のステップＳ６）。

【0083】

次に、図３に示されるように、ステップＳ７において、動作パラメータ最適化部１８は、加速度センサ１３の位置と向きの算出結果の精度が十分であるかどうかを判定する。

【0084】

具体的には、前述のステップＳ６の判定結果を示したＳ／Ｎ値が予め定められた閾値よりも大きい場合には、加速度センサ１３の位置と向きの算出精度が十分であると判定する。この場合には、ステップＳ８のように正常終了となり、動作パラメータ最適化部１８は加速度センサ１３の位置と向きの算出結果を外部に出力する。なお、動作パラメータ最適化部１８は前述の閾値を設定する設定部（図示せず）を備え、前述の閾値を外部から書換えられることが好ましい。

【0085】

一方、前述のステップＳ６の判定結果を示したＳ／Ｎ値が前述の閾値よりも小さい場合には、加速度センサ１３の位置と向きの算出精度が不十分であると判定する。この場合には、ステップＳ９のように異常終了となり、動作パラメータ最適化部１８は、算出精度が不十分であることを警告表示や警報音などによって外部に通知する。

【0086】

以上に説明したように、本実施形態のロボットシステム１０によれば、前述した複数の動作パラメータセットを順次用いて、所定の動作をロボットに実行させることにより、センサの位置と向きを前述の動作パラメータセット毎に算出するようにしている。このことにより、複数の動作パラメータセットの中から、加速度センサ１３に加わる外乱の影響が最小になる最適な動作パラメータセットを求めることができるので、加速度センサ１３の位置と向きの算出結果の精度が向上することとなる。その結果、前述した学習制御による振動の低減効果を良好に得ることができる。

【0087】

また、本実施形態のロボットシステム１０によれば、加速度センサ１３の位置と向きの算出結果の精度が十分であるかどうかを自動的に判定することもできるため、算出精度を確認する人の作業が不要になる。

【0088】

なお、本発明においては、ロボット１１の位置制御の対象となる部位に取付けられるセンサは加速度センサ１３に限定されない。つまり、上述の実施形態においては、加速度センサ１３がロボット１１のアーム部１１ａの先端のツール１４に取付けられている。しかし、本発明においては、取付けられるセンサは、ロボット１１の位置制御の対象となる部位の振動を監視できるセンサであればよい。そのようなセンサとしては、例えば、加速度センサ、ジャイロセンサ、慣性センサ、力センサ、レーザトラッカ、カメラ、またはモーションキャプチャ装置などの、位置や変位を計測できる装置を適用しうる。また、これらの装置を適用した場合、計測されたデータを基に計算機により加速度を算出すればよい。

【0089】

以上では典型的な実施形態を用いて本発明を説明したが、当業者であれば、本発明の範囲から逸脱することなしに、上述の実施形態の形状、構造や配置などに変更および種々の他の変更、省略、追加を行うことができるのを理解できるであろう。

【符号の説明】

【0090】

１０ ロボットシステム
１１ ロボット
１１ａ アーム部
１１ｂ１〜１１ｂ６ 関節軸
１２ ロボット制御装置
１３ 加速度センサ
１４ ツール
１５ ブラケット
１６ ケーブル
１７ センサ座標系計算部
１８ 動作パラメータ最適化部
２１ ワールド座標系
２２ メカニカルインタフェイス座標系

【図1】

【図2】

【図3】

【図4】

【図5】

【図6】

【図7】